[visp] 01/06: New upstream version 3.1.0
Olivier Sallou
osallou at debian.org
Thu Dec 28 10:50:59 UTC 2017
This is an automated email from the git hooks/post-receive script.
osallou pushed a commit to branch master
in repository visp.
commit 1f234eb219553c1853b604004a8f510a7c03b5e1
Author: Olivier Sallou <osallou at debian.org>
Date: Wed Dec 27 06:54:17 2017 +0000
New upstream version 3.1.0
---
.clang-format | 6 +
.travis.yml | 14 +-
3rdparty/apriltag/CMakeLists.txt | 73 +
3rdparty/apriltag/LICENSE | 30 +
3rdparty/apriltag/README | 93 +
3rdparty/apriltag/README.ViSP | 25 +
3rdparty/apriltag/apriltag.c | 1546 ++++++
3rdparty/apriltag/apriltag.h | 293 ++
3rdparty/apriltag/apriltag_math.h | 109 +
3rdparty/apriltag/apriltag_quad_thresh.c | 1916 +++++++
3rdparty/apriltag/common/doubles.h | 29 +
3rdparty/apriltag/common/doubles_floats_impl.h | 1018 ++++
3rdparty/apriltag/common/floats.h | 29 +
3rdparty/apriltag/common/g2d.c | 919 ++++
3rdparty/apriltag/common/g2d.h | 132 +
3rdparty/apriltag/common/getopt.c | 553 +++
3rdparty/apriltag/common/getopt.h | 73 +
3rdparty/apriltag/common/homography.c | 485 ++
3rdparty/apriltag/common/homography.h | 191 +
3rdparty/apriltag/common/image_f32.c | 264 +
3rdparty/apriltag/common/image_f32.h | 55 +
3rdparty/apriltag/common/image_types.h | 60 +
3rdparty/apriltag/common/image_u8.c | 753 +++
3rdparty/apriltag/common/image_u8.h | 99 +
3rdparty/apriltag/common/image_u8x3.c | 293 ++
3rdparty/apriltag/common/image_u8x3.h | 61 +
3rdparty/apriltag/common/image_u8x4.c | 227 +
3rdparty/apriltag/common/image_u8x4.h | 62 +
3rdparty/apriltag/common/matd.c | 2067 ++++++++
3rdparty/apriltag/common/matd.h | 458 ++
3rdparty/apriltag/common/math_util.h | 229 +
3rdparty/apriltag/common/pam.c | 250 +
3rdparty/apriltag/common/pam.h | 51 +
3rdparty/apriltag/common/pjpeg-idct.c | 386 ++
3rdparty/apriltag/common/pjpeg.c | 896 ++++
3rdparty/apriltag/common/pjpeg.h | 100 +
3rdparty/apriltag/common/pnm.c | 160 +
3rdparty/apriltag/common/pnm.h | 66 +
3rdparty/apriltag/common/postscript_utils.h | 61 +
3rdparty/apriltag/common/string_util.c | 787 +++
3rdparty/apriltag/common/string_util.h | 471 ++
3rdparty/apriltag/common/svd22.c | 268 +
3rdparty/apriltag/common/svd22.h | 42 +
3rdparty/apriltag/common/sys/times.h | 40 +
3rdparty/apriltag/common/thash_impl.h | 391 ++
3rdparty/apriltag/common/time.h | 7 +
3rdparty/apriltag/common/time_util.c | 192 +
3rdparty/apriltag/common/time_util.h | 91 +
3rdparty/apriltag/common/timeprofile.h | 133 +
3rdparty/apriltag/common/times.c | 21 +
3rdparty/apriltag/common/unionfind.c | 35 +
3rdparty/apriltag/common/unionfind.h | 157 +
3rdparty/apriltag/common/workerpool.c | 292 ++
3rdparty/apriltag/common/workerpool.h | 57 +
3rdparty/apriltag/common/zarray.c | 62 +
3rdparty/apriltag/common/zarray.h | 496 ++
3rdparty/apriltag/common/zhash.c | 577 +++
3rdparty/apriltag/common/zhash.h | 440 ++
3rdparty/apriltag/common/zmaxheap.c | 436 ++
3rdparty/apriltag/common/zmaxheap.h | 84 +
3rdparty/apriltag/tag16h5.c | 90 +
3rdparty/apriltag/tag16h5.h | 50 +
3rdparty/apriltag/tag25h7.c | 302 ++
3rdparty/apriltag/tag25h7.h | 50 +
3rdparty/apriltag/tag25h9.c | 95 +
3rdparty/apriltag/tag25h9.h | 50 +
3rdparty/apriltag/tag36artoolkit.c | 573 +++
3rdparty/apriltag/tag36artoolkit.h | 49 +
3rdparty/apriltag/tag36h10.c | 2380 +++++++++
3rdparty/apriltag/tag36h10.h | 50 +
3rdparty/apriltag/tag36h11.c | 647 +++
3rdparty/apriltag/tag36h11.h | 50 +
3rdparty/atidaq/CMakeLists.txt | 41 +
3rdparty/{libatidaq-c => atidaq}/ascii.h | 0
3rdparty/{libatidaq-c => atidaq}/asciitab.h | 0
3rdparty/{libatidaq-c => atidaq}/dom.c | 0
3rdparty/{libatidaq-c => atidaq}/dom.h | 0
3rdparty/{libatidaq-c => atidaq}/events.c | 0
3rdparty/{libatidaq-c => atidaq}/expatls.c | 0
3rdparty/{libatidaq-c => atidaq}/ftconfig.c | 0
3rdparty/{libatidaq-c => atidaq}/ftconfig.h | 0
3rdparty/{libatidaq-c => atidaq}/ftrt.c | 0
3rdparty/{libatidaq-c => atidaq}/ftrt.h | 0
3rdparty/{libatidaq-c => atidaq}/ftsharedrt.h | 0
3rdparty/{libatidaq-c => atidaq}/iasciitab.h | 0
3rdparty/{libatidaq-c => atidaq}/latin1tab.h | 0
3rdparty/{libatidaq-c => atidaq}/nametab.h | 0
3rdparty/{libatidaq-c => atidaq}/node.c | 0
3rdparty/atidaq/readme.txt | 139 +
3rdparty/{libatidaq-c => atidaq}/stack.c | 0
3rdparty/{libatidaq-c => atidaq}/stack.h | 0
3rdparty/{libatidaq-c => atidaq}/utf8tab.h | 0
3rdparty/{libatidaq-c => atidaq}/xmldef.h | 0
3rdparty/{libatidaq-c => atidaq}/xmlparse.c | 0
3rdparty/{libatidaq-c => atidaq}/xmlparse.h | 0
3rdparty/{libatidaq-c => atidaq}/xmlrole.c | 0
3rdparty/{libatidaq-c => atidaq}/xmlrole.h | 0
3rdparty/{libatidaq-c => atidaq}/xmltok.c | 0
3rdparty/{libatidaq-c => atidaq}/xmltok.h | 0
3rdparty/{libatidaq-c => atidaq}/xmltok_impl.c | 0
3rdparty/{libatidaq-c => atidaq}/xmltok_impl.h | 0
3rdparty/{libatidaq-c => atidaq}/xmltok_ns.c | 0
3rdparty/clapack/CMakeLists.txt | 49 +
3rdparty/clapack/COPYING | 36 +
3rdparty/clapack/blas/dcopy.c | 107 +
3rdparty/clapack/blas/ddot.c | 110 +
3rdparty/clapack/blas/dgemm.c | 389 ++
3rdparty/clapack/blas/dgemv.c | 312 ++
3rdparty/clapack/blas/dger.c | 194 +
3rdparty/clapack/blas/dnrm2.c | 95 +
3rdparty/clapack/blas/drot.c | 86 +
3rdparty/clapack/blas/dscal.c | 96 +
3rdparty/clapack/blas/dswap.c | 114 +
3rdparty/clapack/blas/dsyrk.c | 372 ++
3rdparty/clapack/blas/dtrmm.c | 453 ++
3rdparty/clapack/blas/dtrmv.c | 345 ++
3rdparty/clapack/blas/dtrsm.c | 490 ++
3rdparty/clapack/blas/idamax.c | 93 +
3rdparty/clapack/blas/lsame.c | 117 +
3rdparty/clapack/f2c/Notice | 23 +
3rdparty/clapack/f2c/README | 374 ++
3rdparty/clapack/f2c/close.c | 101 +
3rdparty/clapack/f2c/d_sign.c | 18 +
3rdparty/clapack/f2c/endfile.c | 160 +
3rdparty/clapack/f2c/err.c | 293 ++
3rdparty/clapack/f2c/exit_.c | 43 +
3rdparty/clapack/f2c/f77_aloc.c | 44 +
3rdparty/clapack/f2c/fio.h | 141 +
3rdparty/clapack/f2c/fmt.c | 530 ++
3rdparty/clapack/f2c/fmt.h | 105 +
3rdparty/clapack/f2c/fmtlib.c | 51 +
3rdparty/clapack/f2c/fp.h | 28 +
3rdparty/clapack/f2c/i_nint.c | 19 +
3rdparty/clapack/f2c/lio.h | 74 +
3rdparty/clapack/f2c/lwrite.c | 314 ++
3rdparty/clapack/f2c/open.c | 311 ++
3rdparty/clapack/f2c/pow_dd.c | 19 +
3rdparty/clapack/f2c/pow_di.c | 41 +
3rdparty/clapack/f2c/pow_ii.c | 39 +
3rdparty/clapack/f2c/s_cat.c | 86 +
3rdparty/clapack/f2c/s_cmp.c | 50 +
3rdparty/clapack/f2c/s_copy.c | 57 +
3rdparty/clapack/f2c/s_stop.c | 48 +
3rdparty/clapack/f2c/sfe.c | 47 +
3rdparty/clapack/f2c/sig_die.c | 51 +
3rdparty/clapack/f2c/sysdep1.h | 66 +
3rdparty/clapack/f2c/util.c | 57 +
3rdparty/clapack/f2c/wref.c | 294 ++
3rdparty/clapack/f2c/wrtfmt.c | 377 ++
3rdparty/clapack/f2c/wsfe.c | 78 +
3rdparty/clapack/include/blaswrap.h | 160 +
3rdparty/clapack/include/f2c.h | 223 +
3rdparty/clapack/install/dlamch.c | 1001 ++++
3rdparty/clapack/src/dbdsdc.c | 514 ++
3rdparty/clapack/src/dbdsqr.c | 918 ++++
3rdparty/clapack/src/dgebd2.c | 304 ++
3rdparty/clapack/src/dgebrd.c | 336 ++
3rdparty/clapack/src/dgelq2.c | 157 +
3rdparty/clapack/src/dgelqf.c | 251 +
3rdparty/clapack/src/dgeqr2.c | 161 +
3rdparty/clapack/src/dgeqrf.c | 252 +
3rdparty/clapack/src/dgesdd.c | 1609 ++++++
3rdparty/clapack/src/dgetf2.c | 193 +
3rdparty/clapack/src/dgetrf.c | 219 +
3rdparty/clapack/src/dgetri.c | 264 +
3rdparty/clapack/src/disnan.c | 52 +
3rdparty/clapack/src/dlabrd.c | 434 ++
3rdparty/clapack/src/dlacpy.c | 125 +
3rdparty/clapack/src/dlaed6.c | 374 ++
3rdparty/clapack/src/dlaisnan.c | 58 +
3rdparty/clapack/src/dlamrg.c | 131 +
3rdparty/clapack/src/dlange.c | 199 +
3rdparty/clapack/src/dlanst.c | 166 +
3rdparty/clapack/src/dlapy2.c | 73 +
3rdparty/clapack/src/dlarf.c | 193 +
3rdparty/clapack/src/dlarfb.c | 774 +++
3rdparty/clapack/src/dlarfg.c | 170 +
3rdparty/clapack/src/dlarfp.c | 192 +
3rdparty/clapack/src/dlarft.c | 325 ++
3rdparty/clapack/src/dlartg.c | 190 +
3rdparty/clapack/src/dlas2.c | 144 +
3rdparty/clapack/src/dlascl.c | 354 ++
3rdparty/clapack/src/dlasd0.c | 291 ++
3rdparty/clapack/src/dlasd1.c | 288 ++
3rdparty/clapack/src/dlasd2.c | 609 +++
3rdparty/clapack/src/dlasd3.c | 452 ++
3rdparty/clapack/src/dlasd4.c | 1010 ++++
3rdparty/clapack/src/dlasd5.c | 189 +
3rdparty/clapack/src/dlasd6.c | 367 ++
3rdparty/clapack/src/dlasd7.c | 518 ++
3rdparty/clapack/src/dlasd8.c | 326 ++
3rdparty/clapack/src/dlasda.c | 488 ++
3rdparty/clapack/src/dlasdq.c | 380 ++
3rdparty/clapack/src/dlasdt.c | 136 +
3rdparty/clapack/src/dlaset.c | 152 +
3rdparty/clapack/src/dlasq1.c | 219 +
3rdparty/clapack/src/dlasq2.c | 602 +++
3rdparty/clapack/src/dlasq3.c | 350 ++
3rdparty/clapack/src/dlasq4.c | 403 ++
3rdparty/clapack/src/dlasq5.c | 240 +
3rdparty/clapack/src/dlasq6.c | 212 +
3rdparty/clapack/src/dlasr.c | 453 ++
3rdparty/clapack/src/dlasrt.c | 286 ++
3rdparty/clapack/src/dlassq.c | 116 +
3rdparty/clapack/src/dlasv2.c | 274 +
3rdparty/clapack/src/dlaswp.c | 158 +
3rdparty/clapack/src/dlauu2.c | 183 +
3rdparty/clapack/src/dlauum.c | 217 +
3rdparty/clapack/src/dorg2r.c | 175 +
3rdparty/clapack/src/dorgbr.c | 299 ++
3rdparty/clapack/src/dorgl2.c | 175 +
3rdparty/clapack/src/dorglq.c | 280 ++
3rdparty/clapack/src/dorgqr.c | 281 ++
3rdparty/clapack/src/dorm2r.c | 235 +
3rdparty/clapack/src/dormbr.c | 360 ++
3rdparty/clapack/src/dorml2.c | 231 +
3rdparty/clapack/src/dormlq.c | 334 ++
3rdparty/clapack/src/dormqr.c | 327 ++
3rdparty/clapack/src/dpotf2.c | 224 +
3rdparty/clapack/src/dpotrf.c | 245 +
3rdparty/clapack/src/dpotri.c | 125 +
3rdparty/clapack/src/dtrti2.c | 183 +
3rdparty/clapack/src/dtrtri.c | 242 +
3rdparty/clapack/src/ieeeck.c | 166 +
3rdparty/clapack/src/iladlc.c | 88 +
3rdparty/clapack/src/iladlr.c | 90 +
3rdparty/clapack/src/ilaenv.c | 654 +++
3rdparty/clapack/src/iparmq.c | 282 ++
3rdparty/clapack/src/xerbla.c | 65 +
3rdparty/clipper/CMakeLists.txt | 34 +
3rdparty/clipper/License.txt | 24 +
3rdparty/clipper/README | 413 ++
3rdparty/clipper/clipper.cpp | 4629 +++++++++++++++++
3rdparty/clipper/clipper.hpp | 406 ++
3rdparty/libatidaq-c.orig/CMakeLists.txt | 149 -
3rdparty/libatidaq-c.orig/readme.txt | 138 -
3rdparty/libatidaq-c.orig/samples/calinfo.c | 122 -
3rdparty/libatidaq-c.orig/samples/ftconvert.c | 156 -
3rdparty/libatidaq-c.orig/samples/makefile | 34 -
3rdparty/libatidaq-c/CMakeLists.txt | 39 -
3rdparty/libatidaq-c/readme.txt | 138 -
3rdparty/pthreads-win32/ANNOUNCE | 483 ++
3rdparty/pthreads-win32/BUGS | 141 +
3rdparty/pthreads-win32/Bmakefile | 268 +
3rdparty/pthreads-win32/CMakeLists.txt | 191 +
3rdparty/pthreads-win32/CONTRIBUTORS | 140 +
3rdparty/pthreads-win32/COPYING | 150 +
3rdparty/pthreads-win32/COPYING.LIB | 504 ++
3rdparty/pthreads-win32/ChangeLog | 5211 +++++++++++++++++++
3rdparty/pthreads-win32/FAQ | 451 ++
3rdparty/pthreads-win32/GNUmakefile | 593 +++
3rdparty/pthreads-win32/MAINTAINERS | 4 +
3rdparty/pthreads-win32/Makefile | 514 ++
3rdparty/pthreads-win32/NEWS | 1241 +++++
3rdparty/pthreads-win32/Nmakefile | 24 +
3rdparty/pthreads-win32/Nmakefile.tests | 260 +
3rdparty/pthreads-win32/PROGRESS | 4 +
3rdparty/pthreads-win32/README | 601 +++
3rdparty/pthreads-win32/README.Borland | 57 +
3rdparty/pthreads-win32/README.CV | 3036 ++++++++++++
3rdparty/pthreads-win32/README.NONPORTABLE | 783 +++
3rdparty/pthreads-win32/README.ViSP | 6 +
3rdparty/pthreads-win32/README.Watcom | 62 +
3rdparty/pthreads-win32/README.WinCE | 6 +
3rdparty/pthreads-win32/TODO | 7 +
3rdparty/pthreads-win32/WinCE-PORT | 222 +
3rdparty/pthreads-win32/attr.c | 53 +
3rdparty/pthreads-win32/autostatic.c | 69 +
3rdparty/pthreads-win32/barrier.c | 47 +
3rdparty/pthreads-win32/builddmc.bat | 9 +
3rdparty/pthreads-win32/cancel.c | 44 +
3rdparty/pthreads-win32/cleanup.c | 148 +
3rdparty/pthreads-win32/condvar.c | 50 +
3rdparty/pthreads-win32/config.h | 159 +
3rdparty/pthreads-win32/context.h | 74 +
3rdparty/pthreads-win32/create.c | 308 ++
3rdparty/pthreads-win32/dll.c | 92 +
3rdparty/pthreads-win32/errno.c | 94 +
3rdparty/pthreads-win32/exit.c | 44 +
3rdparty/pthreads-win32/fork.c | 39 +
3rdparty/pthreads-win32/global.c | 107 +
3rdparty/pthreads-win32/implement.h | 943 ++++
3rdparty/pthreads-win32/misc.c | 50 +
3rdparty/pthreads-win32/mutex.c | 62 +
3rdparty/pthreads-win32/need_errno.h | 145 +
3rdparty/pthreads-win32/nonportable.c | 47 +
3rdparty/pthreads-win32/private.c | 54 +
3rdparty/pthreads-win32/pthread.c | 66 +
3rdparty/pthreads-win32/pthread.h | 1368 +++++
3rdparty/pthreads-win32/pthread_attr_destroy.c | 79 +
.../pthreads-win32/pthread_attr_getdetachstate.c | 86 +
.../pthreads-win32/pthread_attr_getinheritsched.c | 51 +
.../pthreads-win32/pthread_attr_getschedparam.c | 52 +
.../pthreads-win32/pthread_attr_getschedpolicy.c | 61 +
3rdparty/pthreads-win32/pthread_attr_getscope.c | 54 +
.../pthreads-win32/pthread_attr_getstackaddr.c | 97 +
.../pthreads-win32/pthread_attr_getstacksize.c | 100 +
3rdparty/pthreads-win32/pthread_attr_init.c | 117 +
.../pthreads-win32/pthread_attr_setdetachstate.c | 91 +
.../pthreads-win32/pthread_attr_setinheritsched.c | 57 +
.../pthreads-win32/pthread_attr_setschedparam.c | 63 +
.../pthreads-win32/pthread_attr_setschedpolicy.c | 55 +
3rdparty/pthreads-win32/pthread_attr_setscope.c | 62 +
.../pthreads-win32/pthread_attr_setstackaddr.c | 97 +
.../pthreads-win32/pthread_attr_setstacksize.c | 110 +
3rdparty/pthreads-win32/pthread_barrier_destroy.c | 103 +
3rdparty/pthreads-win32/pthread_barrier_init.c | 69 +
3rdparty/pthreads-win32/pthread_barrier_wait.c | 104 +
.../pthreads-win32/pthread_barrierattr_destroy.c | 83 +
.../pthread_barrierattr_getpshared.c | 95 +
3rdparty/pthreads-win32/pthread_barrierattr_init.c | 85 +
.../pthread_barrierattr_setpshared.c | 119 +
3rdparty/pthreads-win32/pthread_cancel.c | 197 +
3rdparty/pthreads-win32/pthread_cond_destroy.c | 253 +
3rdparty/pthreads-win32/pthread_cond_init.c | 167 +
3rdparty/pthreads-win32/pthread_cond_signal.c | 231 +
3rdparty/pthreads-win32/pthread_cond_wait.c | 567 +++
3rdparty/pthreads-win32/pthread_condattr_destroy.c | 86 +
.../pthreads-win32/pthread_condattr_getpshared.c | 97 +
3rdparty/pthreads-win32/pthread_condattr_init.c | 87 +
.../pthreads-win32/pthread_condattr_setpshared.c | 117 +
3rdparty/pthreads-win32/pthread_delay_np.c | 172 +
3rdparty/pthreads-win32/pthread_detach.c | 142 +
3rdparty/pthreads-win32/pthread_equal.c | 76 +
3rdparty/pthreads-win32/pthread_exit.c | 106 +
3rdparty/pthreads-win32/pthread_getconcurrency.c | 45 +
3rdparty/pthreads-win32/pthread_getschedparam.c | 75 +
3rdparty/pthreads-win32/pthread_getspecific.c | 87 +
3rdparty/pthreads-win32/pthread_getunique_np.c | 47 +
.../pthreads-win32/pthread_getw32threadhandle_np.c | 65 +
3rdparty/pthreads-win32/pthread_join.c | 157 +
3rdparty/pthreads-win32/pthread_key_create.c | 108 +
3rdparty/pthreads-win32/pthread_key_delete.c | 125 +
3rdparty/pthreads-win32/pthread_kill.c | 105 +
3rdparty/pthreads-win32/pthread_mutex_consistent.c | 190 +
3rdparty/pthreads-win32/pthread_mutex_destroy.c | 148 +
3rdparty/pthreads-win32/pthread_mutex_init.c | 136 +
3rdparty/pthreads-win32/pthread_mutex_lock.c | 293 ++
3rdparty/pthreads-win32/pthread_mutex_timedlock.c | 324 ++
3rdparty/pthreads-win32/pthread_mutex_trylock.c | 154 +
3rdparty/pthreads-win32/pthread_mutex_unlock.c | 175 +
.../pthreads-win32/pthread_mutexattr_destroy.c | 83 +
.../pthreads-win32/pthread_mutexattr_getkind_np.c | 44 +
.../pthreads-win32/pthread_mutexattr_getpshared.c | 95 +
.../pthreads-win32/pthread_mutexattr_getrobust.c | 113 +
.../pthreads-win32/pthread_mutexattr_gettype.c | 56 +
3rdparty/pthreads-win32/pthread_mutexattr_init.c | 86 +
.../pthreads-win32/pthread_mutexattr_setkind_np.c | 44 +
.../pthreads-win32/pthread_mutexattr_setpshared.c | 119 +
.../pthreads-win32/pthread_mutexattr_setrobust.c | 119 +
.../pthreads-win32/pthread_mutexattr_settype.c | 143 +
.../pthreads-win32/pthread_num_processors_np.c | 56 +
3rdparty/pthreads-win32/pthread_once.c | 79 +
3rdparty/pthreads-win32/pthread_rwlock_destroy.c | 143 +
3rdparty/pthreads-win32/pthread_rwlock_init.c | 109 +
3rdparty/pthreads-win32/pthread_rwlock_rdlock.c | 102 +
.../pthreads-win32/pthread_rwlock_timedrdlock.c | 109 +
.../pthreads-win32/pthread_rwlock_timedwrlock.c | 139 +
3rdparty/pthreads-win32/pthread_rwlock_tryrdlock.c | 102 +
3rdparty/pthreads-win32/pthread_rwlock_trywrlock.c | 122 +
3rdparty/pthreads-win32/pthread_rwlock_unlock.c | 93 +
3rdparty/pthreads-win32/pthread_rwlock_wrlock.c | 133 +
.../pthreads-win32/pthread_rwlockattr_destroy.c | 84 +
.../pthreads-win32/pthread_rwlockattr_getpshared.c | 97 +
3rdparty/pthreads-win32/pthread_rwlockattr_init.c | 83 +
.../pthreads-win32/pthread_rwlockattr_setpshared.c | 120 +
3rdparty/pthreads-win32/pthread_self.c | 141 +
3rdparty/pthreads-win32/pthread_setcancelstate.c | 125 +
3rdparty/pthreads-win32/pthread_setcanceltype.c | 126 +
3rdparty/pthreads-win32/pthread_setconcurrency.c | 53 +
3rdparty/pthreads-win32/pthread_setschedparam.c | 123 +
3rdparty/pthreads-win32/pthread_setspecific.c | 167 +
3rdparty/pthreads-win32/pthread_spin_destroy.c | 111 +
3rdparty/pthreads-win32/pthread_spin_init.c | 123 +
3rdparty/pthreads-win32/pthread_spin_lock.c | 80 +
3rdparty/pthreads-win32/pthread_spin_trylock.c | 77 +
3rdparty/pthreads-win32/pthread_spin_unlock.c | 71 +
3rdparty/pthreads-win32/pthread_testcancel.c | 103 +
.../pthreads-win32/pthread_timechange_handler_np.c | 108 +
.../pthread_win32_attach_detach_np.c | 258 +
3rdparty/pthreads-win32/ptw32_MCS_lock.c | 290 ++
.../pthreads-win32/ptw32_callUserDestroyRoutines.c | 232 +
3rdparty/pthreads-win32/ptw32_calloc.c | 56 +
.../pthreads-win32/ptw32_cond_check_need_init.c | 78 +
3rdparty/pthreads-win32/ptw32_getprocessors.c | 91 +
3rdparty/pthreads-win32/ptw32_is_attr.c | 47 +
.../pthreads-win32/ptw32_mutex_check_need_init.c | 92 +
3rdparty/pthreads-win32/ptw32_new.c | 100 +
3rdparty/pthreads-win32/ptw32_processInitialize.c | 92 +
3rdparty/pthreads-win32/ptw32_processTerminate.c | 105 +
3rdparty/pthreads-win32/ptw32_relmillisecs.c | 132 +
3rdparty/pthreads-win32/ptw32_reuse.c | 151 +
.../pthreads-win32/ptw32_rwlock_cancelwrwait.c | 50 +
.../pthreads-win32/ptw32_rwlock_check_need_init.c | 77 +
3rdparty/pthreads-win32/ptw32_semwait.c | 141 +
.../ptw32_spinlock_check_need_init.c | 78 +
3rdparty/pthreads-win32/ptw32_threadDestroy.c | 79 +
3rdparty/pthreads-win32/ptw32_threadStart.c | 357 ++
3rdparty/pthreads-win32/ptw32_throw.c | 189 +
3rdparty/pthreads-win32/ptw32_timespec.c | 83 +
3rdparty/pthreads-win32/ptw32_tkAssocCreate.c | 118 +
3rdparty/pthreads-win32/ptw32_tkAssocDestroy.c | 114 +
3rdparty/pthreads-win32/rwlock.c | 51 +
3rdparty/pthreads-win32/sched.c | 53 +
3rdparty/pthreads-win32/sched.h | 183 +
3rdparty/pthreads-win32/sched_get_priority_max.c | 134 +
3rdparty/pthreads-win32/sched_get_priority_min.c | 135 +
3rdparty/pthreads-win32/sched_getscheduler.c | 71 +
3rdparty/pthreads-win32/sched_setscheduler.c | 83 +
3rdparty/pthreads-win32/sched_yield.c | 71 +
3rdparty/pthreads-win32/sem_close.c | 58 +
3rdparty/pthreads-win32/sem_destroy.c | 144 +
3rdparty/pthreads-win32/sem_getvalue.c | 110 +
3rdparty/pthreads-win32/sem_init.c | 178 +
3rdparty/pthreads-win32/sem_open.c | 58 +
3rdparty/pthreads-win32/sem_post.c | 128 +
3rdparty/pthreads-win32/sem_post_multiple.c | 142 +
3rdparty/pthreads-win32/sem_timedwait.c | 244 +
3rdparty/pthreads-win32/sem_trywait.c | 117 +
3rdparty/pthreads-win32/sem_unlink.c | 58 +
3rdparty/pthreads-win32/sem_wait.c | 193 +
3rdparty/pthreads-win32/semaphore.c | 69 +
3rdparty/pthreads-win32/semaphore.h | 169 +
3rdparty/pthreads-win32/signal.c | 179 +
3rdparty/pthreads-win32/spin.c | 46 +
3rdparty/pthreads-win32/sync.c | 43 +
3rdparty/pthreads-win32/tsd.c | 44 +
3rdparty/pthreads-win32/version.rc | 394 ++
3rdparty/pthreads-win32/w32_CancelableWait.c | 167 +
CMakeLists.txt | 364 +-
CONTRIBUTING.md | 3 +
CTestConfig.cmake | 34 +-
ChangeLog | 78 +
LICENSE.txt | 2 +-
README.md | 8 +-
appveyor.yml | 99 +
cmake/AddExtraCompilationFlags.cmake | 13 +-
cmake/CPackConfig.cmake | 7 +-
cmake/CPackConfigDeb.cmake | 10 +-
cmake/CPackConfigNsis.cmake | 7 +-
cmake/CPackConfigRpm.cmake | 7 +-
cmake/FindARIA.cmake | 7 +-
cmake/FindAfma6_data.cmake | 7 +-
cmake/FindBICLOPS.cmake | 7 +-
cmake/FindC99.cmake | 85 +
cmake/FindCALINUX.cmake | 7 +-
cmake/FindCMU1394.cmake | 7 +-
cmake/FindCPP11.cmake | 7 +-
cmake/FindComedi.cmake | 7 +-
cmake/FindDC1394.cmake | 7 +-
cmake/FindDIRECT3D.cmake | 7 +-
cmake/FindDIRECTSHOW.cmake | 7 +-
cmake/FindDL.cmake | 7 +-
cmake/FindDMTX.cmake | 7 +-
cmake/FindErfc.cmake | 7 +-
cmake/FindFFMPEG.cmake | 311 --
cmake/FindFlyCapture.cmake | 7 +-
cmake/FindGDI.cmake | 15 +-
cmake/FindGSL.cmake | 45 +-
cmake/FindICONV.cmake | 7 +-
cmake/FindIRISA.cmake | 7 +-
cmake/FindIsInf.cmake | 7 +-
cmake/FindIsNaN.cmake | 7 +-
cmake/FindLAPACK_C.cmake | 7 +-
cmake/FindLIBFREENECT.cmake | 7 +-
cmake/FindLIBUSB_1.cmake | 7 +-
cmake/FindLog1p.cmake | 57 +
cmake/FindMyCoin3D.cmake | 7 +-
cmake/FindMyGTK2.cmake | 7 +-
cmake/FindMyJPEG.cmake | 7 +-
cmake/FindMyOpenCV.cmake | 7 +-
cmake/FindMyPNG.cmake | 7 +-
cmake/FindMyZLIB.cmake | 7 +-
cmake/FindPARPORT.cmake | 7 +-
cmake/FindPTHREAD.cmake | 7 +-
cmake/FindPTU46.cmake | 7 +-
cmake/FindPylon.cmake | 163 +
cmake/FindRAW1394.cmake | 7 +-
cmake/FindRT.cmake | 7 +-
cmake/FindRealSense.cmake | 21 +-
cmake/FindRealSense2.cmake | 94 +
cmake/FindRound.cmake | 7 +-
cmake/FindSOQT.cmake | 7 +-
cmake/FindSOWIN.cmake | 7 +-
cmake/FindSOXT.cmake | 7 +-
cmake/FindStrtof.cmake | 7 +-
cmake/FindV4L2.cmake | 7 +-
cmake/FindViper650_data.cmake | 7 +-
cmake/FindViper850_data.cmake | 7 +-
cmake/FindVirtuose.cmake | 7 +-
cmake/FindXML2.cmake | 7 +-
cmake/FindZBAR.cmake | 19 +-
cmake/OgreTools.cmake | 16 +-
cmake/PCLTools.cmake | 7 +-
cmake/VISP3rdParty.cmake | 38 +-
cmake/VISPConfig.cmake | 15 +-
cmake/VISPDetectPlatform.cmake | 40 +-
cmake/VISPExtraTargets.cmake | 17 +-
cmake/VISPGenerateConfig.cmake | 24 +-
cmake/VISPGenerateConfigScript.cmake | 416 ++
cmake/VISPGenerateHeaders.cmake | 71 +-
cmake/VISPGeneratePkgConfigScript.cmake | 383 --
cmake/VISPModule.cmake | 61 +-
cmake/VISPUse.cmake.in | 7 +-
cmake/VISPUtils.cmake | 81 +-
cmake/templates/VISPConfig.cmake.in | 96 +-
cmake/templates/cmake_uninstall.cmake.in | 7 +-
cmake/templates/visp-config.bat.in | 21 +-
cmake/templates/visp-config.in | 33 +-
cmake/templates/visp-config.install.in | 29 +-
cmake/templates/visp.pc.in | 10 +-
cmake/templates/vpConfig.h.in | 69 +-
demo/CMakeLists.txt | 7 +-
demo/wireframe-simulator/CMakeLists.txt | 11 +-
demo/wireframe-simulator/servoSimu4Points.cpp | 278 +-
demo/wireframe-simulator/servoSimuCylinder.cpp | 245 +-
demo/wireframe-simulator/servoSimuSphere.cpp | 291 +-
doc/biblio/references.bib | 160 +-
doc/config-doxygen.in | 89 +-
doc/image/{ => cpp}/image-data-structure.fig | 0
doc/image/{ => cpp}/image-data-structure.gif | Bin
doc/image/{ => cpp}/image-data-structure.ps | 0
doc/image/{ => cpp}/model-viper.png | Bin
doc/image/{ => cpp}/pioneer-pan.fig | 0
doc/image/{ => cpp}/pioneer.fig | 0
doc/image/cpp/soft_archi_v6.png | Bin 0 -> 99245 bytes
doc/image/{ => cpp}/vpFeatureLine.fig | 0
doc/image/{ => cpp}/vpFeatureLine.gif | Bin
doc/image/{ => cpp}/vpFeatureLine.ps | 0
doc/image/{ => cpp}/vpImagePoint.fig | 0
doc/image/{ => cpp}/vpImagePoint.gif | Bin
doc/image/{ => cpp}/vpImagePoint.ps | 0
doc/image/{ => cpp}/vpMeEllipse.fig | 0
doc/image/{ => cpp}/vpMeEllipse.gif | Bin
doc/image/{ => cpp}/vpMeEllipse.ps | 0
doc/image/{ => cpp}/vpMeEllipse2.fig | 0
doc/image/{ => cpp}/vpMeEllipse2.gif | Bin
doc/image/{ => cpp}/vpMeEllipse2.ps | 0
doc/image/{ => cpp}/vpMeLine.fig | 0
doc/image/{ => cpp}/vpMeLine.gif | Bin
doc/image/{ => cpp}/vpMeLine.ps | 0
doc/image/{ => cpp}/vpWireFrameSimulator.fig | 0
doc/image/{ => cpp}/vpWireFrameSimulator.jpeg | Bin
doc/image/{ => cpp}/vpWireFrameSimulator.ps | 0
doc/image/img-ccmake-centos-all.png | Bin 54061 -> 0 bytes
doc/image/img-ccmake-ubuntu-all.jpeg | Bin 79766 -> 0 bytes
doc/image/img-cmake-debug-trace.jpg | Bin 86382 -> 0 bytes
doc/image/img-cmake-win-opencv-advanced.jpg | Bin 89577 -> 0 bytes
doc/image/img-cmake-win-shared.jpg | Bin 67344 -> 0 bytes
doc/image/img-cmake-win10-config-end.png | Bin 52558 -> 0 bytes
doc/image/img-cmake-win10-config.png | Bin 48680 -> 0 bytes
doc/image/img-cmake-win10-create-build-folder.png | Bin 8509 -> 0 bytes
doc/image/img-cmake-win10-msvc-generate.png | Bin 51934 -> 0 bytes
doc/image/img-cmake-win10-msvc-launch.png | Bin 14031 -> 0 bytes
doc/image/img-cmake-win10-msvc-version.png | Bin 8252 -> 0 bytes
doc/image/img-cmake-win8.1-config-end.jpg | Bin 84682 -> 0 bytes
doc/image/img-cmake-win8.1-config.jpg | Bin 88970 -> 0 bytes
doc/image/img-cmake-win8.1-create-build-folder.jpg | Bin 16648 -> 0 bytes
doc/image/img-cmake-win8.1-mingw-configure-end.jpg | Bin 85409 -> 0 bytes
doc/image/img-cmake-win8.1-mingw-configure.jpg | Bin 90009 -> 0 bytes
doc/image/img-cmake-win8.1-mingw-generate.jpg | Bin 85597 -> 0 bytes
doc/image/img-cmake-win8.1-mingw-version.jpg | Bin 20413 -> 0 bytes
doc/image/img-cmake-win8.1-msvc-generate.jpg | Bin 84682 -> 0 bytes
doc/image/img-cmake-win8.1-msvc-launch.jpg | Bin 40333 -> 0 bytes
doc/image/img-cmake-win8.1-msvc-version.jpg | Bin 29365 -> 0 bytes
doc/image/img-configure-generator-iOS.png | Bin 59393 -> 0 bytes
doc/image/img-configure-options-iOS.png | Bin 37333 -> 0 bytes
doc/image/img-configure-path-iOS.png | Bin 14186 -> 0 bytes
doc/image/img-getting-started-iOS-code.jpeg | Bin 74929 -> 0 bytes
doc/image/img-getting-started-iOS-drag-drop.png | Bin 62674 -> 0 bytes
doc/image/img-getting-started-iOS-link-visp.png | Bin 57676 -> 0 bytes
doc/image/img-getting-started-iOS-log.jpeg | Bin 97721 -> 0 bytes
doc/image/img-getting-started-iOS-navigator.png | Bin 66779 -> 0 bytes
doc/image/img-getting-started-iOS-new.png | Bin 73666 -> 0 bytes
doc/image/img-getting-started-iOS-options.png | Bin 29245 -> 0 bytes
doc/image/img-getting-started-iOS-rename.png | Bin 96093 -> 0 bytes
.../img-started-imgproc-ios-output-canny.jpeg | Bin 45190 -> 0 bytes
.../img-started-imgproc-ios-output-color.jpeg | Bin 30710 -> 0 bytes
.../img-started-imgproc-ios-output-gradient.jpeg | Bin 22491 -> 0 bytes
doc/image/img-started-imgproc-ios-output-gray.jpeg | Bin 21734 -> 0 bytes
doc/image/img-started-imgproc-ios.jpeg | Bin 80201 -> 0 bytes
doc/image/img-win10-explorer-install-end.png | Bin 44346 -> 0 bytes
doc/image/img-win10-msvc-build-end.png | Bin 48368 -> 0 bytes
doc/image/img-win10-msvc-build.jpeg | Bin 32270 -> 0 bytes
doc/image/img-win10-msvc-install-end.png | Bin 44659 -> 0 bytes
doc/image/img-win10-msvc-install.jpeg | Bin 54304 -> 0 bytes
doc/image/img-win10-msvc-open.png | Bin 27182 -> 0 bytes
doc/image/img-win10-msvc-release.jpeg | Bin 80806 -> 0 bytes
doc/image/img-win8.1-cmake-ogre.jpg | Bin 81613 -> 0 bytes
doc/image/img-win8.1-cmake-ogredeps.jpg | Bin 91797 -> 0 bytes
doc/image/img-win8.1-explorer-install-end.jpg | Bin 72636 -> 0 bytes
...win8.1-mingw-opencv-cmake-configure-install.jpg | Bin 82285 -> 0 bytes
.../img-win8.1-mingw-opencv-cmake-configure.jpg | Bin 45901 -> 0 bytes
doc/image/img-win8.1-msvc-build-end.jpg | Bin 89639 -> 0 bytes
doc/image/img-win8.1-msvc-build-ogredeps.jpg | Bin 26860 -> 0 bytes
doc/image/img-win8.1-msvc-build.jpg | Bin 99119 -> 0 bytes
doc/image/img-win8.1-msvc-install-end.jpg | Bin 90792 -> 0 bytes
doc/image/img-win8.1-msvc-install.jpg | Bin 86045 -> 0 bytes
doc/image/img-win8.1-msvc-open.jpg | Bin 57429 -> 0 bytes
doc/image/img-win8.1-msvc-release.jpg | Bin 97919 -> 0 bytes
doc/image/img-win8.1-msvc-solution.jpg | Bin 30423 -> 0 bytes
doc/image/img-win8.1-ogre-explorer.jpg | Bin 43758 -> 0 bytes
doc/image/img-win8.1-ogre-tortoisehg.jpg | Bin 48158 -> 0 bytes
doc/image/img-win8.1-ogredeps-explorer.jpg | Bin 20817 -> 0 bytes
doc/image/img-win8.1-ogredeps-tortoisehg.jpg | Bin 30825 -> 0 bytes
doc/image/{ => logo}/img-logo-visp.png | Bin
.../calibration}/img-chessboard-01.png | Bin
.../calibration}/img-chessboard-undistort.png | Bin
.../calibration}/img-circles-grid-02.png | Bin
.../tutorial/detection/img-apriltag-image.png | Bin 0 -> 78060 bytes
doc/image/tutorial/detection/img-apriltag-pose.png | Bin 0 -> 65257 bytes
.../detection}/img-detection-datamatrix.png | Bin
.../detection}/img-detection-qrcode.png | Bin
.../detection}/img-detection-step.jpeg | Bin
.../detection}/img-learning-step.jpeg | Bin
.../image}/img-monkey-blured-default.png | Bin
.../image}/img-monkey-blured-var2.png | Bin
.../{ => tutorial/image}/img-monkey-canny.png | Bin
doc/image/{ => tutorial/image}/img-monkey-dIxy.png | Bin
doc/image/{ => tutorial/image}/img-monkey-gray.png | Bin
doc/image/{ => tutorial/image}/img-monkey-pyr.png | Bin
.../{ => tutorial/image}/img-monkey-sobel.png | Bin
.../image}/img-simu-image-target-square.png | Bin
.../{ => tutorial/image}/img-target-square.png | Bin
.../imgproc/img-auto-threshold-grid36-03-otsu.png | Bin 0 -> 4086 bytes
.../imgproc/img-auto-threshold-grid36-03.png | Bin 0 -> 123937 bytes
.../imgproc/img-tutorial-autothreshold.png | Bin 0 -> 92299 bytes
...mg-tutorial-brighness-adjust-alpha10-beta50.png | Bin 0 -> 301889 bytes
...img-tutorial-brighness-gamma-correction-3.5.png | Bin 0 -> 333541 bytes
...mg-tutorial-brighness-gamma-correction-plot.png | Bin 0 -> 129513 bytes
.../img-tutorial-brighness-hist-eq-cumulative.png | Bin 0 -> 18521 bytes
.../img-tutorial-brighness-hist-eq-cumulative2.png | Bin 0 -> 19606 bytes
.../imgproc/img-tutorial-brighness-hist-eq.png | Bin 0 -> 470777 bytes
.../img-tutorial-brighness-retinex-dynamic-3.png | Bin 0 -> 461117 bytes
.../img-tutorial-connected-components-img.png | Bin 0 -> 1060 bytes
.../img-tutorial-connected-components-labeling.png | Bin 0 -> 2005 bytes
.../imgproc/img-tutorial-contour-binarisation.png | Bin 0 -> 5927 bytes
.../imgproc/img-tutorial-contour-binarisation2.png | Bin 0 -> 1984 bytes
.../imgproc/img-tutorial-contour-draw-contours.png | Bin 0 -> 6515 bytes
.../img-tutorial-contour-draw-contours2.png | Bin 0 -> 2326 bytes
.../img-tutorial-contour-draw-contours3.png | Bin 0 -> 1896 bytes
...img-tutorial-contrast-sharpening-CLAHE-hist.png | Bin 0 -> 24133 bytes
.../img-tutorial-contrast-sharpening-CLAHE.png | Bin 0 -> 315463 bytes
...trast-sharpening-Crayfish-low-contrast-hist.png | Bin 0 -> 20074 bytes
...l-contrast-sharpening-Crayfish-low-contrast.png | Bin 0 -> 261775 bytes
...rast-sharpening-histogram-equalization-hist.png | Bin 0 -> 29912 bytes
...-contrast-sharpening-histogram-equalization.png | Bin 0 -> 546849 bytes
...st-sharpening-histogram-stretching-HSV-hist.png | Bin 0 -> 24281 bytes
...ontrast-sharpening-histogram-stretching-HSV.png | Bin 0 -> 359295 bytes
...ntrast-sharpening-histogram-stretching-hist.png | Bin 0 -> 24967 bytes
...al-contrast-sharpening-histogram-stretching.png | Bin 0 -> 373407 bytes
...utorial-contrast-sharpening-unsharp-masking.png | Bin 0 -> 555924 bytes
.../img-tutorial-count-coins-binarisation1.png | Bin 0 -> 1572 bytes
.../img-tutorial-count-coins-binarisation2.png | Bin 0 -> 10949 bytes
.../imgproc/img-tutorial-count-coins-close1.png | Bin 0 -> 2744 bytes
.../imgproc/img-tutorial-count-coins-coins1.png | Bin 0 -> 36150 bytes
.../imgproc/img-tutorial-count-coins-coins2.png | Bin 0 -> 98011 bytes
.../img-tutorial-count-coins-count-coins1.png | Bin 0 -> 2694 bytes
.../img-tutorial-count-coins-count-coins2.png | Bin 0 -> 5815 bytes
.../img-tutorial-count-coins-fill-holes.png | Bin 0 -> 32470 bytes
.../imgproc/img-tutorial-count-coins-fill1.png | Bin 0 -> 1524 bytes
.../imgproc/img-tutorial-count-coins-mask.png | Bin 0 -> 19004 bytes
.../img-tutorial-count-coins-white-holes.png | Bin 0 -> 25301 bytes
.../img-tutorial-flood-fill-4-connexity.png | Bin 0 -> 1079 bytes
.../img-tutorial-flood-fill-8-connexity.png | Bin 0 -> 1003 bytes
.../img-tutorial-flood-fill-bucket-fill.png | Bin 0 -> 6087 bytes
.../img-tutorial-flood-fill-draw-polygons.png | Bin 0 -> 6014 bytes
.../ios/img-detection-apriltag-ios-drag-drop.png | Bin 0 -> 173299 bytes
.../ios/img-detection-apriltag-ios-output.png | Bin 0 -> 199576 bytes
.../ios/img-detection-apriltag-ios-xcode.png | Bin 0 -> 236841 bytes
.../tutorial/ios/img-getting-started-iOS-code.jpeg | Bin 0 -> 114685 bytes
.../ios}/img-getting-started-iOS-create.png | Bin
.../img-getting-started-iOS-drag-drop-dialog.png | Bin
.../ios/img-getting-started-iOS-drag-drop.png | Bin 0 -> 74670 bytes
.../ios/img-getting-started-iOS-link-visp.png | Bin 0 -> 74915 bytes
.../tutorial/ios/img-getting-started-iOS-log.jpeg | Bin 0 -> 104422 bytes
.../ios/img-getting-started-iOS-navigator.png | Bin 0 -> 66747 bytes
.../tutorial/ios/img-getting-started-iOS-new.png | Bin 0 -> 64966 bytes
.../ios/img-getting-started-iOS-options.png | Bin 0 -> 31347 bytes
.../ios/img-getting-started-iOS-rename.png | Bin 0 -> 92669 bytes
.../img-started-imgproc-ios-drag-drop-dialog.png | Bin
.../ios}/img-started-imgproc-ios-drag-drop.jpeg | Bin
.../ios/img-started-imgproc-ios-output-canny.png | Bin 0 -> 173389 bytes
.../ios/img-started-imgproc-ios-output-color.jpeg | Bin 0 -> 66521 bytes
.../img-started-imgproc-ios-output-gradient.png | Bin 0 -> 162750 bytes
.../ios/img-started-imgproc-ios-output-gray.png | Bin 0 -> 160152 bytes
.../tutorial/ios/img-started-imgproc-ios.jpeg | Bin 0 -> 93307 bytes
doc/image/tutorial/misc/img-cmake-debug-trace.jpg | Bin 0 -> 69890 bytes
.../tutorial/started/img-contrib-module-1.png | Bin 0 -> 14303 bytes
.../tutorial/started/img-contrib-module-2.png | Bin 0 -> 9369 bytes
.../tutorial/started/img-contrib-module-3.png | Bin 0 -> 44405 bytes
.../tutorial/started/img-contrib-module-4.png | Bin 0 -> 40783 bytes
.../tutorial/started/img-contrib-module-5.png | Bin 0 -> 41581 bytes
.../tutorial/started/img-contrib-module-6.png | Bin 0 -> 44743 bytes
.../tutorial/started/img-contrib-module-7.png | Bin 0 -> 44743 bytes
.../tutorial/started/img-contrib-module-tree.jpg | Bin 0 -> 38119 bytes
doc/image/tutorial/started/img-contrib-module.jpg | Bin 0 -> 33892 bytes
.../tutorial/started/img-contrib-visp-module.jpg | Bin 0 -> 24794 bytes
doc/image/tutorial/started/img-contrib.pptx | Bin 0 -> 58848 bytes
.../{ => tutorial/started}/img-monkey-win.jpg | Bin
doc/image/{ => tutorial/started}/img-monkey.png | Bin
.../started}/img-started-win-cmake-1.jpg | Bin
.../started}/img-started-win-cmake-2.jpg | Bin
.../started}/img-started-win-cmake-3.jpg | Bin
.../started}/img-started-win-msvc-1.jpg | Bin
.../started}/img-started-win-msvc-2.jpg | Bin
.../{ => tutorial/started}/img-started-win-run.jpg | Bin
.../tutorial/started/img-visp-into-eclipse-10.png | Bin 0 -> 36981 bytes
.../tutorial/started/img-visp-into-eclipse-11.png | Bin 0 -> 37064 bytes
.../tutorial/started/img-visp-into-eclipse-12.png | Bin 0 -> 39139 bytes
.../tutorial/started/img-visp-into-eclipse-4.png | Bin 0 -> 13388 bytes
.../tutorial/started/img-visp-into-eclipse-5.png | Bin 0 -> 22274 bytes
.../tutorial/started/img-visp-into-eclipse-6.png | Bin 0 -> 21919 bytes
.../tutorial/started/img-visp-into-eclipse-7.png | Bin 0 -> 42724 bytes
.../tutorial/started/img-visp-into-eclipse-8.png | Bin 0 -> 33170 bytes
.../tutorial/started/img-visp-into-eclipse-9.png | Bin 0 -> 31055 bytes
.../started/img-visp-into-eclipse-arg-1.png | Bin 0 -> 31697 bytes
.../started/img-visp-into-eclipse-arg-2.png | Bin 0 -> 33797 bytes
.../started/img-visp-into-eclipse-unr-1.png | Bin 0 -> 27181 bytes
.../started/img-visp-into-eclipse-unr-2.png | Bin 0 -> 32916 bytes
.../started/img-visp-into-eclipse-unr-3.png | Bin 0 -> 40446 bytes
.../started/img-visp-into-eclipse-unr-4.png | Bin 0 -> 25451 bytes
.../tracking}/img-blob-auto-detection.png | Bin
doc/image/{ => tutorial/tracking}/img-circle.png | Bin
doc/image/{ => tutorial/tracking}/img-cylinder.png | Bin
doc/image/{ => tutorial/tracking}/img-fov.png | Bin
.../tracking}/img-initClickTemplateTracker.png | Bin
.../img-mbt-multi-class-diagram-resize.jpeg | Bin
.../tracking}/img-multi-cameras-config.png | Bin
.../tracking}/img-plane-hierarchical-diagram.jpg | Bin
.../tracking}/img-teabox-cao-triangle.jpg | Bin
.../{ => tutorial/tracking}/img-teabox-cao.jpg | Bin
.../{ => tutorial/tracking}/img-teabox-click.jpg | Bin
.../tracking}/img-template-tracker.jpg | Bin
.../img-tracker-mb-visibility-ogre-advanced.png | Bin
.../tracking}/img-tracker-mb-visibility-ogre.png | Bin
.../img-tracker-mb-visibility-scanline.png | Bin
.../tracking}/img-tracker-mb-visibility.jpg | Bin
.../img-win8.1-msvc-mbtracker-ogre-issue.jpg | Bin
.../img-win8.1-msvc-mbtracker-ogre-opengl.jpg | Bin
.../tutorial-tracking-mb-generic-depth-map.png | Bin 0 -> 96973 bytes
doc/image/tutorial/unix/img-ccmake-centos-all.jpg | Bin 0 -> 172087 bytes
doc/image/tutorial/unix/img-ccmake-fedora-all.jpeg | Bin 0 -> 86764 bytes
.../tutorial/unix/img-ccmake-opensuse-all.png | Bin 0 -> 62460 bytes
doc/image/tutorial/unix/img-ccmake-osx-all.jpg | Bin 0 -> 125788 bytes
.../tutorial/unix/img-ccmake-raspberry-all.png | Bin 0 -> 40487 bytes
doc/image/tutorial/unix/img-ccmake-ubuntu-all.jpeg | Bin 0 -> 147619 bytes
doc/image/{ => tutorial/unix}/img-raspberry-pi.jpg | Bin
.../{ => tutorial/vision}/img-pose-qrcode.jpg | Bin
.../{ => tutorial/vision}/img-pose-square.jpg | Bin
.../img-ibvs-control-law-adaptive.png | Bin
.../img-ibvs-control-law-continuous-adaptive.png | Bin
.../img-ibvs-control-law-exponential.png | Bin
.../visual-servo}/img-ibvs-plotter.jpg | Bin
.../{ => tutorial/visual-servo}/pioneer-pan.png | Bin
doc/image/{ => tutorial/visual-servo}/pioneer.png | Bin
.../windows/img-cmake-win10-mingw-makefiles.png | Bin 0 -> 6451 bytes
.../img-cmake-win10-mingw-opencv-install.png | Bin 0 -> 28963 bytes
...in10-mingw-opencv-issue-precompiled-headers.png | Bin 0 -> 28957 bytes
.../img-cmake-win10-mingw-opencv-launch.png | Bin 0 -> 18553 bytes
.../windows/img-cmake-win10-mingw-visp-config.png | Bin 0 -> 53334 bytes
.../windows/img-cmake-win10-mingw-visp-config2.png | Bin 0 -> 55142 bytes
.../img-cmake-win10-mingw-visp-create-folder.png | Bin 0 -> 6845 bytes
.../img-cmake-win10-mingw-visp-generate.png | Bin 0 -> 55212 bytes
.../windows/img-cmake-win10-mingw-visp-launch.png | Bin 0 -> 14413 bytes
.../img-cmake-win10-msvc12-msvc-version.png | Bin 0 -> 7522 bytes
.../windows/img-cmake-win10-msvc12-ogre.png | Bin 0 -> 41646 bytes
.../windows/img-cmake-win10-msvc12-ogredeps.png | Bin 0 -> 44567 bytes
.../img-cmake-win10-msvc12-realsense-config.png | Bin 0 -> 15354 bytes
.../img-cmake-win10-msvc12-realsense-install.png | Bin 0 -> 51398 bytes
.../windows/img-cmake-win10-msvc12-visp-config.png | Bin 0 -> 44912 bytes
.../img-cmake-win10-msvc12-visp-config2.png | Bin 0 -> 41444 bytes
.../img-cmake-win10-msvc12-visp-create-folder.png | Bin 0 -> 6817 bytes
.../img-cmake-win10-msvc12-visp-generate.png | Bin 0 -> 41168 bytes
.../windows/img-cmake-win10-msvc12-visp-launch.png | Bin 0 -> 17708 bytes
.../img-cmake-win10-msvc14-eigen-install.png | Bin 0 -> 29402 bytes
.../img-cmake-win10-msvc14-eigen-launch.png | Bin 0 -> 15172 bytes
.../img-cmake-win10-msvc14-msvc-version.png | Bin 0 -> 7682 bytes
.../img-cmake-win10-msvc14-realsense-config.png | Bin 0 -> 15260 bytes
.../img-cmake-win10-msvc14-realsense-install.png | Bin 0 -> 49182 bytes
.../windows/img-cmake-win10-msvc14-visp-config.png | Bin 0 -> 58790 bytes
.../img-cmake-win10-msvc14-visp-config2.png | Bin 0 -> 60358 bytes
.../img-cmake-win10-msvc14-visp-create-folder.png | Bin 0 -> 6814 bytes
.../img-cmake-win10-msvc14-visp-generate.png | Bin 0 -> 60389 bytes
.../windows/img-cmake-win10-msvc14-visp-launch.png | Bin 0 -> 14724 bytes
.../img-cmake-win10-msvc15-eigen-install.png | Bin 0 -> 40111 bytes
.../img-cmake-win10-msvc15-eigen-launch.png | Bin 0 -> 15113 bytes
.../img-cmake-win10-msvc15-msvc-version.png | Bin 0 -> 9406 bytes
.../img-cmake-win10-msvc15-opencv-folder.png | Bin 0 -> 7250 bytes
.../img-cmake-win10-msvc15-opencv-generate.png | Bin 0 -> 31086 bytes
.../img-cmake-win10-msvc15-opencv-install.png | Bin 0 -> 31325 bytes
.../img-cmake-win10-msvc15-opencv-launch.png | Bin 0 -> 15318 bytes
.../img-cmake-win10-msvc15-realsense-config.png | Bin 0 -> 15372 bytes
.../img-cmake-win10-msvc15-realsense-install.png | Bin 0 -> 45776 bytes
.../windows/img-cmake-win10-msvc15-visp-config.png | Bin 0 -> 57651 bytes
.../img-cmake-win10-msvc15-visp-config2.png | Bin 0 -> 59935 bytes
.../img-cmake-win10-msvc15-visp-create-folder.png | Bin 0 -> 6810 bytes
.../img-cmake-win10-msvc15-visp-generate.png | Bin 0 -> 60309 bytes
.../windows/img-cmake-win10-msvc15-visp-launch.png | Bin 0 -> 59166 bytes
.../img-cmake-win7-create-build-folder.jpg | Bin
.../windows}/img-cmake-win7-generate.jpg | Bin
.../windows}/img-cmake-win7-msvc-config-end.jpg | Bin
.../windows}/img-cmake-win7-msvc-config.jpg | Bin
.../windows}/img-cmake-win7-msvc-launch.jpg | Bin
.../windows}/img-cmake-win7-msvc-version.jpg | Bin
.../windows}/img-cmake-win7-solution.jpg | Bin
.../windows}/img-mingw64-installer-process.jpg | Bin
.../windows}/img-mingw64-installer-started.jpg | Bin
.../windows}/img-opencv-issue-ipp.jpg | Bin
.../windows}/img-opencv-issue-test-big-endian.jpg | Bin
.../windows}/img-opencv-issue-tiff.jpg | Bin
.../{ => tutorial/windows}/img-uwp-msvc-modify.png | Bin
.../windows}/img-uwp-msvc-modify2.png | Bin
.../windows}/img-uwp-msvc-modify3.png | Bin
.../windows}/img-win10-cmake-error-sdk-missing.png | Bin
.../tutorial/windows/img-win10-extract-opencv.png | Bin 0 -> 8334 bytes
.../windows/img-win10-mingw-cmd-displayGDI.png | Bin 0 -> 9066 bytes
.../img-win10-mingw-opencv-issue-cap-dshow.png | Bin 0 -> 48635 bytes
...in10-mingw-opencv-issue-precompiled-headers.png | Bin 0 -> 42847 bytes
...img-win10-msvc12-build-ogre-install-release.png | Bin 0 -> 52593 bytes
...win10-msvc12-build-ogredeps-install-release.png | Bin 0 -> 37473 bytes
.../windows/img-win10-msvc12-build-ogredeps.png | Bin 0 -> 45779 bytes
.../windows/img-win10-msvc12-realsense-build.png | Bin 0 -> 39727 bytes
.../windows/img-win10-msvc12-visp-build-end.png | Bin 0 -> 73646 bytes
.../img-win10-msvc12-visp-explorer-install.png | Bin 0 -> 32037 bytes
.../windows/img-win10-msvc12-visp-install-end.png | Bin 0 -> 68584 bytes
.../windows/img-win10-msvc12-visp-install.png | Bin 0 -> 106347 bytes
.../windows/img-win10-msvc12-visp-open.png | Bin 0 -> 36876 bytes
.../windows/img-win10-msvc12-visp-release.png | Bin 0 -> 92433 bytes
.../windows/img-win10-msvc14-cmd-displayGDI.png | Bin 0 -> 9627 bytes
.../img-win10-msvc14-eigen-build-install.png | Bin 0 -> 69650 bytes
.../windows/img-win10-msvc14-realsense-build.png | Bin 0 -> 52340 bytes
.../windows/img-win10-msvc14-visp-build-end.png | Bin 0 -> 69684 bytes
.../windows/img-win10-msvc14-visp-build.png | Bin 0 -> 50722 bytes
.../img-win10-msvc14-visp-explorer-install.png | Bin 0 -> 28606 bytes
.../windows/img-win10-msvc14-visp-install-end.png | Bin 0 -> 69574 bytes
.../windows/img-win10-msvc14-visp-install.png | Bin 0 -> 96663 bytes
.../windows/img-win10-msvc14-visp-open.png | Bin 0 -> 34184 bytes
.../windows/img-win10-msvc14-visp-release.png | Bin 0 -> 93687 bytes
.../img-win10-msvc15-build-opencv-debug.png | Bin 0 -> 67245 bytes
.../img-win10-msvc15-build-opencv-release.png | Bin 0 -> 64831 bytes
.../windows/img-win10-msvc15-cmd-displayGDI.png | Bin 0 -> 12163 bytes
.../img-win10-msvc15-eigen-build-install.png | Bin 0 -> 54005 bytes
.../windows/img-win10-msvc15-realsense-build.png | Bin 0 -> 51045 bytes
.../windows/img-win10-msvc15-visp-build-end.png | Bin 0 -> 52372 bytes
.../windows/img-win10-msvc15-visp-build.png | Bin 0 -> 51006 bytes
.../img-win10-msvc15-visp-explorer-install.png | Bin 0 -> 29621 bytes
.../windows/img-win10-msvc15-visp-install-end.png | Bin 0 -> 53915 bytes
.../windows/img-win10-msvc15-visp-install.png | Bin 0 -> 69238 bytes
.../windows/img-win10-msvc15-visp-open.png | Bin 0 -> 34587 bytes
.../windows/img-win10-msvc15-visp-release.png | Bin 0 -> 73517 bytes
.../windows/img-win10-msvc15-zbar-build.png | Bin 0 -> 40284 bytes
.../tutorial/windows/img-win10-ogre-explorer.png | Bin 0 -> 59743 bytes
.../tutorial/windows/img-win10-ogre-tortoisehg.png | Bin 0 -> 17034 bytes
.../windows/img-win10-ogredeps-tortoisehg.png | Bin 0 -> 9229 bytes
.../tutorial/windows/img-win10-pylon-developer.png | Bin 0 -> 81555 bytes
.../tutorial/windows/img-win10-visp-images.png | Bin 0 -> 17540 bytes
.../windows}/img-win7-ViSP-images.jpg | Bin
.../windows}/img-win7-cmd-displayGDI.jpg | Bin
.../{ => tutorial/windows}/img-win7-cygwin-git.jpg | Bin
.../windows}/img-win7-msvc-build-succeed.jpg | Bin
.../{ => tutorial/windows}/img-win7-msvc-build.jpg | Bin
.../windows}/img-win7-msvc-install-end.jpg | Bin
.../windows}/img-win7-msvc-install-succeed.jpg | Bin
.../windows}/img-win7-msvc-install.jpg | Bin
.../{ => tutorial/windows}/img-win7-msvc-open.jpg | Bin
.../windows}/img-win7-msvc-release.jpg | Bin
.../windows}/img-win8.1-cmake-ogredeps-issue.jpg | Bin
.../windows}/img-win8.1-cmd-displayGDI.jpg | Bin
.../windows}/img-win8.1-mingw-cmd-displayGDI.jpg | Bin
.../img-win8.1-mingw-opencv-issue-dtor.jpg | Bin
.../windows}/img-win8.1-msvc-ogredeps-issue.jpg | Bin
.../windows}/img-win8.1-visp-images.jpg | Bin
doc/mainpage.doc.in | 87 +-
doc/tutorial-boost-vs.doc | 104 -
doc/tutorial-detection-barcode.doc | 106 -
doc/tutorial-getting-started-iOS.doc | 159 -
doc/tutorial-getting-started-naoqi.doc | 84 -
doc/tutorial-getting-started.doc | 204 -
doc/tutorial-grabber.doc | 155 -
doc/tutorial-image-ios.doc | 79 -
doc/tutorial-install-centos.doc | 353 --
doc/tutorial-install-fedora.doc | 326 --
doc/tutorial-install-iOS.doc | 103 -
doc/tutorial-install-ios-package.doc | 40 -
doc/tutorial-install-opensuse.doc | 307 --
doc/tutorial-install-osx-homebrew.doc | 379 --
doc/tutorial-install-raspberry.doc | 387 --
doc/tutorial-install-ubuntu-package.doc | 36 -
doc/tutorial-install-ubuntu.doc | 326 --
doc/tutorial-install-win10-msvc14-uwp.doc | 147 -
doc/tutorial-install-win10-msvc14.doc | 174 -
doc/tutorial-install-win7.doc | 185 -
doc/tutorial-install-win8.1-mingw-w64.doc | 275 -
doc/tutorial-install-win8.1-msvc.doc | 282 --
doc/tutorial-matching-deprecated.doc | 63 -
doc/tutorial-trace.doc | 138 -
doc/tutorial-tracking-mb-stereo.doc | 224 -
doc/tutorial-tracking-mb.doc | 876 ----
doc/tutorial-tracking-tt.doc | 226 -
doc/tutorial.doc | 150 -
.../bridge}/tutorial-bridge-opencv.doc | 0
.../calibration}/tutorial-calibration.doc | 0
.../tutorial-install-crosscompiling-naoqi.doc | 0
.../tutorial-install-crosscompiling-raspberry.doc | 0
.../detection/tutorial-detection-apriltag.doc | 138 +
.../detection/tutorial-detection-barcode.doc | 113 +
.../detection}/tutorial-detection-face.doc | 0
.../detection}/tutorial-detection-object.doc | 0
.../detection/tutorial-matching-deprecated.doc | 63 +
doc/{ => tutorial/detection}/tutorial-matching.doc | 0
doc/tutorial/image/tutorial-grabber.doc | 153 +
.../image}/tutorial-image-display.doc | 0
.../image}/tutorial-image-filtering.doc | 0
doc/{ => tutorial/image}/tutorial-simu-image.doc | 0
.../imgproc/tutorial-imgproc-autothreshold.doc | 61 +
.../imgproc/tutorial-imgproc-brightness.doc | 84 +
.../tutorial-imgproc-connected-components.doc | 46 +
doc/tutorial/imgproc/tutorial-imgproc-contour.doc | 269 +
.../tutorial-imgproc-contrast-sharpening.doc | 124 +
.../imgproc/tutorial-imgproc-count-coins.doc | 123 +
.../imgproc/tutorial-imgproc-flood-fill.doc | 77 +
.../ios/tutorial-detection-apriltag-ios.doc | 62 +
doc/tutorial/ios/tutorial-getting-started-iOS.doc | 160 +
doc/tutorial/ios/tutorial-image-ios.doc | 85 +
doc/tutorial/ios/tutorial-install-iOS.doc | 100 +
doc/tutorial/ios/tutorial-install-ios-package.doc | 41 +
.../misc}/tutorial-multi-threading.doc | 0
doc/{ => tutorial/misc}/tutorial-plotter.doc | 0
doc/tutorial/misc/tutorial-trace.doc | 142 +
.../tutorial-install-archlinux-package.doc | 0
.../tutorial-install-osx-homebrew-package.doc | 0
.../package}/tutorial-install-ros-package.doc | 0
.../package/tutorial-install-ubuntu-package.doc | 36 +
doc/tutorial/started/tutorial-contrib-module.doc | 209 +
.../started/tutorial-getting-started-naoqi.doc | 88 +
doc/tutorial/started/tutorial-getting-started.doc | 205 +
.../tutorial-import-visp-into-EclipseIDE.doc | 84 +
.../tracking}/tutorial-tracking-blob.doc | 0
.../tracking}/tutorial-tracking-keypoint.doc | 0
.../tracking/tutorial-tracking-mb-CAO-editor.doc | 27 +
.../tracking/tutorial-tracking-mb-generic.doc | 388 ++
.../tracking/tutorial-tracking-mb-stereo.doc | 227 +
doc/tutorial/tracking/tutorial-tracking-mb.doc | 928 ++++
.../tracking}/tutorial-tracking-me.doc | 0
doc/tutorial/tracking/tutorial-tracking-tt.doc | 253 +
doc/tutorial/tutorial.doc | 182 +
doc/tutorial/unix/tutorial-install-centos.doc | 424 ++
doc/tutorial/unix/tutorial-install-fedora.doc | 410 ++
doc/tutorial/unix/tutorial-install-opensuse.doc | 405 ++
.../unix/tutorial-install-osx-homebrew.doc | 444 ++
doc/tutorial/unix/tutorial-install-raspberry.doc | 461 ++
doc/tutorial/unix/tutorial-install-ubuntu.doc | 416 ++
.../vision}/tutorial-homography-deprecated.doc | 0
doc/{ => tutorial/vision}/tutorial-homography.doc | 0
.../vision}/tutorial-pose-estimation-qrcode.doc | 0
.../vision}/tutorial-pose-estimation.doc | 0
doc/tutorial/visual-servo/tutorial-boost-vs.doc | 100 +
doc/{ => tutorial/visual-servo}/tutorial-ibvs.doc | 0
.../visual-servo}/tutorial-simu-robot-pioneer.doc | 0
.../windows/tutorial-install-win10-msvc12.doc | 328 ++
.../windows/tutorial-install-win10-msvc14-uwp.doc | 159 +
.../windows/tutorial-install-win10-msvc14.doc | 372 ++
.../windows/tutorial-install-win10-msvc15-uwp.doc | 79 +
.../windows/tutorial-install-win10-msvc15.doc | 379 ++
.../windows/tutorial-install-win7-msvc11.doc | 170 +
.../windows/tutorial-install-win8.1-mingw-w64.doc | 319 ++
example/CMakeLists.txt | 8 +-
example/calibration/CMakeLists.txt | 7 +-
example/calibration/calibrateTsai.cpp | 121 +-
example/calibration/camera_calibration.cpp | 167 +-
example/coin-simulator/CMakeLists.txt | 7 +-
.../coin-simulator/simulateCircle2DCamVelocity.cpp | 270 +-
.../simulateFourPoints2DCartesianCamVelocity.cpp | 260 +-
.../simulateFourPoints2DPolarCamVelocity.cpp | 275 +-
example/device/display/CMakeLists.txt | 9 +-
example/device/display/displayD3D.cpp | 254 +-
example/device/display/displayGDI.cpp | 246 +-
example/device/display/displayGTK.cpp | 250 +-
example/device/display/displayOpenCV.cpp | 250 +-
example/device/display/displaySequence.cpp | 202 +-
example/device/display/displayX.cpp | 248 +-
example/device/display/displayXMulti.cpp | 227 +-
example/device/framegrabber/CMakeLists.txt | 10 +-
example/device/framegrabber/grab1394CMU.cpp | 95 +-
example/device/framegrabber/grab1394Two.cpp | 325 +-
example/device/framegrabber/grabDirectShow.cpp | 111 +-
.../device/framegrabber/grabDirectShowMulti.cpp | 492 +-
example/device/framegrabber/grabDisk.cpp | 167 +-
example/device/framegrabber/grabFlyCapture.cpp | 81 +-
example/device/framegrabber/grabOpenCV.cpp | 42 +-
example/device/framegrabber/grabRealSense.cpp | 168 +-
example/device/framegrabber/grabRealSense2.cpp | 244 +
example/device/framegrabber/grabV4l2.cpp | 180 +-
.../framegrabber/grabV4l2MultiCpp11Thread.cpp | 270 +-
example/device/kinect/CMakeLists.txt | 7 +-
example/device/kinect/kinectAcquisition.cpp | 89 +-
example/device/laserscanner/CMakeLists.txt | 7 +-
example/device/laserscanner/SickLDMRS-Acq.cpp | 30 +-
example/device/laserscanner/SickLDMRS-Process.cpp | 196 +-
example/device/light/CMakeLists.txt | 7 +-
example/device/light/ringLight.cpp | 79 +-
example/direct-visual-servoing/CMakeLists.txt | 7 +-
.../photometricVisualServoing.cpp | 269 +-
example/homography/CMakeLists.txt | 7 +-
example/homography/homographyHLM2DObject.cpp | 231 +-
example/homography/homographyHLM3DObject.cpp | 214 +-
.../homography/homographyHartleyDLT2DObject.cpp | 191 +-
example/homography/homographyRansac2DObject.cpp | 140 +-
example/image/CMakeLists.txt | 7 +-
example/image/imageDiskRW.cpp | 145 +-
example/key-point/CMakeLists.txt | 7 +-
example/key-point/fernClassifier.cpp | 246 +-
example/key-point/keyPointSurf.cpp | 208 +-
example/key-point/planarObjectDetector.cpp | 233 +-
example/manual/CMakeLists.txt | 11 +-
.../geometric-features/manGeometricFeatures.cpp | 64 +-
.../manual/hello-world/Autotools/HelloWorld.cpp | 5 +-
example/manual/hello-world/Autotools/have_visp.m4 | 7 +-
example/manual/hello-world/CMake/HelloWorld.cpp | 14 +-
example/manual/hello-world/Makefile/HelloWorld.cpp | 5 +-
example/manual/image-manipulation/manDisplay.cpp | 97 +-
.../manual/image-manipulation/manGrab1394-2.cpp | 27 +-
.../image-manipulation/manGrabDirectShow.cpp | 21 +-
example/manual/image-manipulation/manGrabDisk.cpp | 17 +-
.../manual/image-manipulation/manGrabOpenCV.cpp | 30 +-
example/manual/image-manipulation/manGrabV4l2.cpp | 14 +-
example/manual/moments/manServoMomentsSimple.cpp | 152 +-
example/manual/ogre/HelloWorldOgre.cpp | 47 +-
example/manual/ogre/HelloWorldOgreAdvanced.cpp | 62 +-
.../manual/simulation/manServo4PointsDisplay.cpp | 141 +-
example/manual/simulation/manSimu4Dots.cpp | 207 +-
example/manual/simulation/manSimu4Points.cpp | 193 +-
example/math/BSpline.cpp | 151 +-
example/math/CMakeLists.txt | 7 +-
example/math/exponentialMap.cpp | 40 +-
example/moments/image/CMakeLists.txt | 7 +-
example/moments/image/servoMomentImage.cpp | 447 +-
example/moments/points/CMakeLists.txt | 7 +-
example/moments/points/servoMomentPoints.cpp | 318 +-
example/moments/polygon/CMakeLists.txt | 7 +-
example/moments/polygon/servoMomentPolygon.cpp | 316 +-
example/ogre-simulator/AROgre.cpp | 457 +-
example/ogre-simulator/AROgreBasic.cpp | 370 +-
example/ogre-simulator/CMakeLists.txt | 9 +-
example/parse-argv/CMakeLists.txt | 7 +-
example/parse-argv/parse-argv1.cpp | 61 +-
example/parse-argv/parse-argv2.cpp | 59 +-
example/pose-estimation/CMakeLists.txt | 7 +-
example/pose-estimation/poseVirtualVS.cpp | 421 +-
example/robot-simulator/afma6/CMakeLists.txt | 7 +-
.../servoSimuAfma6FourPoints2DCamVelocity.cpp | 372 +-
example/robot-simulator/camera/CMakeLists.txt | 7 +-
.../camera/servoSimu3D_cMcd_CamVelocity.cpp | 146 +-
.../servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp | 120 +-
.../camera/servoSimu3D_cdMc_CamVelocity.cpp | 145 +-
.../servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp | 109 +-
.../camera/servoSimuCircle2DCamVelocity.cpp | 127 +-
.../camera/servoSimuCircle2DCamVelocityDisplay.cpp | 171 +-
.../servoSimuCylinder2DCamVelocityDisplay.cpp | 209 +-
...muCylinder2DCamVelocityDisplaySecondaryTask.cpp | 351 +-
.../camera/servoSimuFourPoints2DCamVelocity.cpp | 173 +-
.../servoSimuFourPoints2DCamVelocityDisplay.cpp | 231 +-
...ervoSimuFourPoints2DPolarCamVelocityDisplay.cpp | 317 +-
.../camera/servoSimuLine2DCamVelocityDisplay.cpp | 181 +-
.../camera/servoSimuPoint2DCamVelocity1.cpp | 82 +-
.../camera/servoSimuPoint2DCamVelocity2.cpp | 123 +-
.../camera/servoSimuPoint2DCamVelocity3.cpp | 115 +-
.../camera/servoSimuPoint2DhalfCamVelocity1.cpp | 137 +-
.../camera/servoSimuPoint2DhalfCamVelocity2.cpp | 154 +-
.../camera/servoSimuPoint2DhalfCamVelocity3.cpp | 150 +-
.../camera/servoSimuPoint3DCamVelocity.cpp | 115 +-
.../camera/servoSimuSphere2DCamVelocity.cpp | 127 +-
.../servoSimuSphere2DCamVelocitySecondaryTask.cpp | 148 +-
.../servoSimuSquareLine2DCamVelocityDisplay.cpp | 201 +-
.../camera/servoSimuThetaUCamVelocity.cpp | 114 +-
example/robot-simulator/viper850/CMakeLists.txt | 7 +-
.../servoSimuViper850FourPoints2DCamVelocity.cpp | 342 +-
example/servo-afma4/CMakeLists.txt | 7 +-
example/servo-afma4/moveAfma4.cpp | 85 +-
.../servo-afma4/servoAfma4Point2DArtVelocity.cpp | 221 +-
.../servo-afma4/servoAfma4Point2DCamVelocity.cpp | 176 +-
.../servoAfma4Point2DCamVelocityKalman.cpp | 273 +-
example/servo-afma6/CMakeLists.txt | 7 +-
.../servo-afma6/servoAfma62DhalfCamVelocity.cpp | 407 +-
.../servoAfma6Cylinder2DCamVelocity.cpp | 281 +-
...ervoAfma6Cylinder2DCamVelocitySecondaryTask.cpp | 383 +-
.../servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp | 247 +-
.../servoAfma6FourPoints2DArtVelocity.cpp | 247 +-
...a6FourPoints2DCamVelocityInteractionCurrent.cpp | 273 +-
...a6FourPoints2DCamVelocityInteractionDesired.cpp | 221 +-
.../servo-afma6/servoAfma6Line2DCamVelocity.cpp | 222 +-
.../servo-afma6/servoAfma6Point2DArtVelocity.cpp | 227 +-
.../servo-afma6/servoAfma6Point2DCamVelocity.cpp | 158 +-
.../servoAfma6Points2DCamVelocityEyeToHand.cpp | 447 +-
.../servo-afma6/servoAfma6Segment2DCamVelocity.cpp | 163 +-
.../servoAfma6SquareLines2DCamVelocity.cpp | 286 +-
.../servoAfma6TwoLines2DCamVelocity.cpp | 265 +-
example/servo-biclops/CMakeLists.txt | 7 +-
example/servo-biclops/moveBiclops.cpp | 189 +-
.../servoBiclopsPoint2DArtVelocity.cpp | 300 +-
example/servo-franka/CMakeLists.txt | 66 +
example/servo-franka/LICENCE.txt | 176 +
example/servo-franka/franka_echo_robot_state.cpp | 50 +
example/servo-franka/franka_execute_trajectory.cpp | 108 +
.../franka_generate_cartesian_pose_motion.cpp | 73 +
.../franka_generate_cartesian_velocity_motion.cpp | 81 +
.../franka_generate_consecutive_motions.cpp | 79 +
.../franka_generate_joint_position_motion.cpp | 70 +
.../franka_generate_joint_velocity_motion.cpp | 69 +
example/servo-franka/franka_grasp_object.cpp | 83 +
.../franka_joint_impedance_control.cpp | 225 +
.../franka_joint_point_to_point_motion.cpp | 216 +
.../servo-franka/franka_motion_with_control.cpp | 183 +
.../servo-franka/franka_print_joint_positions.cpp | 57 +
example/servo-pioneer/CMakeLists.txt | 8 +-
example/servo-pioneer/movePioneer.cpp | 50 +-
example/servo-pioneer/servoPioneerPanSegment3D.cpp | 252 +-
example/servo-pioneer/servoPioneerPoint2DDepth.cpp | 185 +-
.../servoPioneerPoint2DDepthWithoutVpServo.cpp | 170 +-
example/servo-pioneer/sonarPioneerReader.cpp | 120 +-
example/servo-ptu46/CMakeLists.txt | 7 +-
example/servo-ptu46/movePtu46.cpp | 74 +-
.../servo-ptu46/servoPtu46Point2DArtVelocity.cpp | 252 +-
example/servo-viper650/CMakeLists.txt | 17 +-
example/servo-viper650/SR300-eMc.cnf | 11 +
...50FourPoints2DArtVelocityInteractionCurrent.cpp | 342 +-
...Points2DCamVelocityInteractionCurrent-SR300.cpp | 377 ++
...50FourPoints2DCamVelocityInteractionCurrent.cpp | 338 +-
.../servoViper650Point2DCamVelocity.cpp | 213 +-
example/servo-viper850/CMakeLists.txt | 7 +-
...50FourPoints2DArtVelocityInteractionCurrent.cpp | 290 +-
...50FourPoints2DArtVelocityInteractionDesired.cpp | 249 +-
...50FourPoints2DCamVelocityInteractionCurrent.cpp | 265 +-
.../servoViper850FourPointsKinect.cpp | 260 +-
...r850Point2DArtVelocity-jointAvoidance-basic.cpp | 367 +-
...per850Point2DArtVelocity-jointAvoidance-gpa.cpp | 275 +-
...r850Point2DArtVelocity-jointAvoidance-large.cpp | 226 +-
.../servoViper850Point2DArtVelocity.cpp | 227 +-
.../servoViper850Point2DCamVelocity.cpp | 171 +-
.../servoViper850Point2DCamVelocityKalman.cpp | 237 +-
example/tools/CMakeLists.txt | 7 +-
example/tools/histogram.cpp | 161 +-
example/tools/keyboard.cpp | 23 +-
example/tools/parallelPort.cpp | 55 +-
example/tools/plot2d.cpp | 87 +-
example/tools/plot3d.cpp | 65 +-
example/tracking/CMakeLists.txt | 63 +-
example/tracking/mbtEdgeKltMultiTracking.cpp | 279 +-
example/tracking/mbtEdgeKltTracking.cpp | 304 +-
example/tracking/mbtEdgeMultiTracking.cpp | 262 +-
example/tracking/mbtEdgeTracking.cpp | 293 +-
example/tracking/mbtGenericTracking.cpp | 672 +++
example/tracking/mbtGenericTracking2.cpp | 760 +++
example/tracking/mbtGenericTrackingDepth.cpp | 865 ++++
example/tracking/mbtKltMultiTracking.cpp | 253 +-
example/tracking/mbtKltTracking.cpp | 279 +-
example/tracking/templateTracker.cpp | 298 +-
example/tracking/trackDot.cpp | 221 +-
example/tracking/trackDot2.cpp | 245 +-
example/tracking/trackDot2WithAutoDetection.cpp | 273 +-
example/tracking/trackKltOpencv.cpp | 205 +-
example/tracking/trackMeCircle.cpp | 161 +-
example/tracking/trackMeEllipse.cpp | 162 +-
example/tracking/trackMeLine.cpp | 192 +-
example/tracking/trackMeNurbs.cpp | 164 +-
example/video/CMakeLists.txt | 7 +-
example/video/imageSequenceReader.cpp | 184 +-
example/video/videoReader.cpp | 155 +-
example/wireframe-simulator/CMakeLists.txt | 7 +-
example/wireframe-simulator/wireframeSimulator.cpp | 204 +-
macros/have_visp.m4 | 7 +-
modules/CMakeLists.txt | 7 +-
modules/ar/CMakeLists.txt | 21 +-
modules/ar/include/visp3/ar/vpAR.h | 45 +-
modules/ar/include/visp3/ar/vpAROgre.h | 321 +-
modules/ar/include/visp3/ar/vpSimulator.h | 182 +-
modules/ar/include/visp3/ar/vpSimulatorException.h | 65 +-
modules/ar/include/visp3/ar/vpViewer.h | 70 +-
modules/ar/src/coin-simulator/vpAR.cpp | 151 +-
modules/ar/src/coin-simulator/vpSimulator.cpp | 952 ++--
modules/ar/src/coin-simulator/vpViewer.cpp | 221 +-
modules/ar/src/ogre-simulator/vpAROgre.cpp | 633 ++-
modules/ar/src/vpSimulatorException.cpp | 17 +-
modules/core/CMakeLists.txt | 33 +-
modules/core/include/visp3/core/vpArray2D.h | 384 +-
modules/core/include/visp3/core/vpBSpline.h | 168 +-
modules/core/include/visp3/core/vpCPUFeatures.h | 76 +
.../core/include/visp3/core/vpCameraParameters.h | 254 +-
modules/core/include/visp3/core/vpCircle.h | 98 +-
modules/core/include/visp3/core/vpClient.h | 113 +-
modules/core/include/visp3/core/vpColVector.h | 162 +-
modules/core/include/visp3/core/vpColor.h | 232 +-
modules/core/include/visp3/core/vpConvert.h | 33 +-
modules/core/include/visp3/core/vpCylinder.h | 112 +-
modules/core/include/visp3/core/vpDebug.h | 354 +-
modules/core/include/visp3/core/vpDisplay.h | 526 +-
.../core/include/visp3/core/vpDisplayException.h | 92 +-
modules/core/include/visp3/core/vpException.h | 129 +-
modules/core/include/visp3/core/vpExponentialMap.h | 41 +-
modules/core/include/visp3/core/vpFeatureDisplay.h | 76 +-
.../core/include/visp3/core/vpForceTwistMatrix.h | 115 +-
.../core/include/visp3/core/vpForwardProjection.h | 88 +-
modules/core/include/visp3/core/vpFrameGrabber.h | 38 +-
.../include/visp3/core/vpFrameGrabberException.h | 85 +-
modules/core/include/visp3/core/vpGEMM.h | 479 +-
modules/core/include/visp3/core/vpGaussRand.h | 37 +-
modules/core/include/visp3/core/vpHinkley.h | 41 +-
modules/core/include/visp3/core/vpHistogram.h | 106 +-
modules/core/include/visp3/core/vpHistogramPeak.h | 53 +-
modules/core/include/visp3/core/vpHistogramValey.h | 56 +-
.../core/include/visp3/core/vpHomogeneousMatrix.h | 83 +-
modules/core/include/visp3/core/vpImage.h | 1291 +++--
modules/core/include/visp3/core/vpImageConvert.h | 343 +-
modules/core/include/visp3/core/vpImageException.h | 80 +-
modules/core/include/visp3/core/vpImageFilter.h | 353 +-
.../core/include/visp3/core/vpImageMorphology.h | 108 +-
modules/core/include/visp3/core/vpImagePoint.h | 118 +-
modules/core/include/visp3/core/vpImageTools.h | 834 ++--
modules/core/include/visp3/core/vpIoException.h | 76 +-
modules/core/include/visp3/core/vpIoTools.h | 91 +-
modules/core/include/visp3/core/vpKalmanFilter.h | 109 +-
modules/core/include/visp3/core/vpLine.h | 69 +-
.../visp3/core/vpLinearKalmanFilterInstantiation.h | 77 +-
modules/core/include/visp3/core/vpList.h | 555 +--
modules/core/include/visp3/core/vpMath.h | 324 +-
modules/core/include/visp3/core/vpMatrix.h | 445 +-
.../core/include/visp3/core/vpMatrixException.h | 102 +-
.../include/visp3/core/vpMeterPixelConversion.h | 173 +-
modules/core/include/visp3/core/vpMoment.h | 85 +-
modules/core/include/visp3/core/vpMomentAlpha.h | 83 +-
modules/core/include/visp3/core/vpMomentArea.h | 20 +-
.../include/visp3/core/vpMomentAreaNormalized.h | 82 +-
modules/core/include/visp3/core/vpMomentBasic.h | 47 +-
.../core/include/visp3/core/vpMomentCInvariant.h | 360 +-
modules/core/include/visp3/core/vpMomentCentered.h | 74 +-
modules/core/include/visp3/core/vpMomentCommon.h | 94 +-
modules/core/include/visp3/core/vpMomentDatabase.h | 114 +-
.../include/visp3/core/vpMomentGravityCenter.h | 34 +-
.../visp3/core/vpMomentGravityCenterNormalized.h | 36 +-
modules/core/include/visp3/core/vpMomentObject.h | 202 +-
modules/core/include/visp3/core/vpMouseButton.h | 16 +-
modules/core/include/visp3/core/vpMutex.h | 113 +-
modules/core/include/visp3/core/vpNetwork.h | 479 +-
modules/core/include/visp3/core/vpNoise.h | 8 +-
.../include/visp3/core/vpPixelMeterConversion.h | 349 +-
modules/core/include/visp3/core/vpPlane.h | 99 +-
modules/core/include/visp3/core/vpPoint.h | 53 +-
modules/core/include/visp3/core/vpPolygon.h | 212 +-
modules/core/include/visp3/core/vpPolygon3D.h | 192 +-
modules/core/include/visp3/core/vpPoseVector.h | 72 +-
.../core/include/visp3/core/vpQuaternionVector.h | 41 +-
modules/core/include/visp3/core/vpRGBa.h | 83 +-
modules/core/include/visp3/core/vpRansac.h | 145 +-
modules/core/include/visp3/core/vpRect.h | 138 +-
modules/core/include/visp3/core/vpRequest.h | 131 +-
modules/core/include/visp3/core/vpRobust.h | 125 +-
modules/core/include/visp3/core/vpRotationMatrix.h | 54 +-
modules/core/include/visp3/core/vpRotationVector.h | 63 +-
modules/core/include/visp3/core/vpRowVector.h | 123 +-
modules/core/include/visp3/core/vpRxyzVector.h | 50 +-
modules/core/include/visp3/core/vpRzyxVector.h | 46 +-
modules/core/include/visp3/core/vpRzyzVector.h | 45 +-
modules/core/include/visp3/core/vpScale.h | 23 +-
modules/core/include/visp3/core/vpServer.h | 117 +-
modules/core/include/visp3/core/vpSphere.h | 75 +-
modules/core/include/visp3/core/vpSubColVector.h | 19 +-
modules/core/include/visp3/core/vpSubMatrix.h | 80 +-
modules/core/include/visp3/core/vpSubRowVector.h | 28 +-
modules/core/include/visp3/core/vpThetaUVector.h | 80 +-
modules/core/include/visp3/core/vpThread.h | 102 +-
modules/core/include/visp3/core/vpTime.h | 26 +-
modules/core/include/visp3/core/vpTracker.h | 31 +-
.../core/include/visp3/core/vpTrackingException.h | 82 +-
.../core/include/visp3/core/vpTranslationVector.h | 69 +-
modules/core/include/visp3/core/vpTriangle.h | 120 +-
modules/core/include/visp3/core/vpUDPClient.h | 188 +
modules/core/include/visp3/core/vpUDPServer.h | 220 +
modules/core/include/visp3/core/vpUniRand.h | 32 +-
.../include/visp3/core/vpVelocityTwistMatrix.h | 111 +-
modules/core/include/visp3/core/vpXmlParser.h | 209 +-
.../core/include/visp3/core/vpXmlParserCamera.h | 266 +-
.../visp3/core/vpXmlParserHomogeneousMatrix.h | 115 +-
modules/core/src/camera/vpCameraParameters.cpp | 405 +-
modules/core/src/camera/vpMeterPixelConversion.cpp | 61 +-
modules/core/src/camera/vpPixelMeterConversion.cpp | 116 +-
modules/core/src/camera/vpXmlParserCamera.cpp | 770 ++-
modules/core/src/display/vpColor.cpp | 98 +-
modules/core/src/display/vpDisplay.cpp | 113 +-
modules/core/src/display/vpDisplay_impl.h | 693 ++-
modules/core/src/display/vpDisplay_rgba.cpp | 378 +-
modules/core/src/display/vpDisplay_uchar.cpp | 367 +-
modules/core/src/display/vpFeatureDisplay.cpp | 337 +-
modules/core/src/image/vpImageConvert.cpp | 3767 +++++++-------
modules/core/src/image/vpImageFilter.cpp | 657 ++-
modules/core/src/image/vpImageMorphology.cpp | 239 +-
modules/core/src/image/vpImagePoint.cpp | 154 +-
modules/core/src/image/vpImageTools.cpp | 195 +-
modules/core/src/image/vpRGBa.cpp | 85 +-
modules/core/src/math/kalman/vpKalmanFilter.cpp | 152 +-
.../kalman/vpLinearKalmanFilterInstantiation.cpp | 395 +-
modules/core/src/math/matrix/vpColVector.cpp | 930 ++--
modules/core/src/math/matrix/vpMatrix.cpp | 4621 +++++++++++------
modules/core/src/math/matrix/vpMatrix_cholesky.cpp | 201 +-
.../core/src/math/matrix/vpMatrix_covariance.cpp | 224 +-
modules/core/src/math/matrix/vpMatrix_lu.cpp | 696 ++-
modules/core/src/math/matrix/vpMatrix_mul.cpp | 75 +
modules/core/src/math/matrix/vpMatrix_qr.cpp | 268 +-
modules/core/src/math/matrix/vpMatrix_svd.cpp | 909 ++--
modules/core/src/math/matrix/vpRowVector.cpp | 484 +-
modules/core/src/math/matrix/vpSubColVector.cpp | 182 +-
modules/core/src/math/matrix/vpSubMatrix.cpp | 156 +-
modules/core/src/math/matrix/vpSubRowVector.cpp | 145 +-
modules/core/src/math/misc/vpHinkley.cpp | 148 +-
modules/core/src/math/misc/vpMath.cpp | 131 +-
.../core/src/math/random-generator/vpGaussRand.cpp | 28 +-
.../core/src/math/random-generator/vpUniRand.cpp | 62 +-
modules/core/src/math/robust/vpRobust.cpp | 791 ++-
modules/core/src/math/robust/vpScale.cpp | 193 +-
modules/core/src/math/spline/vpBSpline.cpp | 517 +-
.../src/math/transformation/vpExponentialMap.cpp | 286 +-
.../src/math/transformation/vpForceTwistMatrix.cpp | 536 +-
.../math/transformation/vpHomogeneousMatrix.cpp | 455 +-
.../core/src/math/transformation/vpPoseVector.cpp | 262 +-
.../src/math/transformation/vpQuaternionVector.cpp | 155 +-
.../src/math/transformation/vpRotationMatrix.cpp | 507 +-
.../src/math/transformation/vpRotationVector.cpp | 41 +-
.../core/src/math/transformation/vpRxyzVector.cpp | 112 +-
.../core/src/math/transformation/vpRzyxVector.cpp | 112 +-
.../core/src/math/transformation/vpRzyzVector.cpp | 116 +-
.../src/math/transformation/vpThetaUVector.cpp | 227 +-
.../math/transformation/vpTranslationVector.cpp | 272 +-
.../math/transformation/vpVelocityTwistMatrix.cpp | 488 +-
.../vpXmlParserHomogeneousMatrix.cpp | 369 +-
modules/core/src/tools/convert/vpConvert.cpp | 720 +--
.../core/src/tools/cpu-features/vpCPUFeatures.cpp | 58 +
.../core/src/tools/cpu-features/x86/LICENSE.txt | 116 +
.../core/src/tools/cpu-features/x86/cpu_x86.cpp | 264 +
modules/core/src/tools/cpu-features/x86/cpu_x86.h | 98 +
.../src/tools/cpu-features/x86/cpu_x86_Linux.ipp | 41 +
.../src/tools/cpu-features/x86/cpu_x86_Windows.ipp | 112 +
modules/core/src/tools/exceptions/vpException.cpp | 91 +-
modules/core/src/tools/file/vpIoTools.cpp | 1241 ++---
modules/core/src/tools/geometry/vpPlane.cpp | 275 +-
modules/core/src/tools/geometry/vpPolygon.cpp | 398 +-
modules/core/src/tools/geometry/vpPolygon3D.cpp | 591 ++-
modules/core/src/tools/geometry/vpRect.cpp | 141 +-
modules/core/src/tools/geometry/vpTriangle.cpp | 124 +-
modules/core/src/tools/histogram/vpHistogram.cpp | 648 ++-
.../core/src/tools/histogram/vpHistogramPeak.cpp | 30 +-
.../core/src/tools/histogram/vpHistogramValey.cpp | 20 +-
modules/core/src/tools/network/vpClient.cpp | 154 +-
modules/core/src/tools/network/vpNetwork.cpp | 572 ++-
modules/core/src/tools/network/vpRequest.cpp | 41 +-
modules/core/src/tools/network/vpServer.cpp | 189 +-
modules/core/src/tools/network/vpUDPClient.cpp | 226 +
modules/core/src/tools/network/vpUDPServer.cpp | 319 ++
modules/core/src/tools/time/vpTime.cpp | 143 +-
modules/core/src/tools/xml/vpXmlParser.cpp | 363 +-
.../src/tracking/forward-projection/vpCircle.cpp | 465 +-
.../src/tracking/forward-projection/vpCylinder.cpp | 315 +-
.../forward-projection/vpForwardProjection.cpp | 73 +-
.../src/tracking/forward-projection/vpLine.cpp | 288 +-
.../src/tracking/forward-projection/vpPoint.cpp | 360 +-
.../src/tracking/forward-projection/vpSphere.cpp | 204 +-
modules/core/src/tracking/moments/vpMoment.cpp | 74 +-
.../core/src/tracking/moments/vpMomentAlpha.cpp | 188 +-
modules/core/src/tracking/moments/vpMomentArea.cpp | 85 +-
.../tracking/moments/vpMomentAreaNormalized.cpp | 95 +-
.../core/src/tracking/moments/vpMomentBasic.cpp | 55 +-
.../src/tracking/moments/vpMomentCInvariant.cpp | 376 +-
.../core/src/tracking/moments/vpMomentCentered.cpp | 191 +-
.../core/src/tracking/moments/vpMomentCommon.cpp | 179 +-
.../core/src/tracking/moments/vpMomentDatabase.cpp | 87 +-
.../src/tracking/moments/vpMomentGravityCenter.cpp | 45 +-
.../moments/vpMomentGravityCenterNormalized.cpp | 79 +-
.../core/src/tracking/moments/vpMomentObject.cpp | 585 +--
modules/core/src/tracking/vpTracker.cpp | 30 +-
.../test/camera/testCameraParametersConversion.cpp | 54 +-
modules/core/test/image/testConversion.cpp | 540 +-
modules/core/test/image/testCrop.cpp | 86 +-
modules/core/test/image/testCropAdvanced.cpp | 95 +-
modules/core/test/image/testImageAddSub.cpp | 138 +-
modules/core/test/image/testImageBinarise.cpp | 85 +-
modules/core/test/image/testImageComparison.cpp | 93 +-
modules/core/test/image/testImageFilter.cpp | 287 +-
modules/core/test/image/testImageMorphology.cpp | 563 +--
modules/core/test/image/testImagePoint.cpp | 32 +-
modules/core/test/image/testImagePrint.cpp | 22 +-
modules/core/test/image/testImageResize.cpp | 279 +-
modules/core/test/image/testIoPGM.cpp | 102 +-
modules/core/test/image/testIoPPM.cpp | 127 +-
modules/core/test/image/testPerformanceLUT.cpp | 247 +-
modules/core/test/image/testReadImage.cpp | 95 +-
modules/core/test/image/testUndistortImage.cpp | 114 +-
modules/core/test/math/testArray2D.cpp | 65 +-
modules/core/test/math/testColVector.cpp | 152 +-
modules/core/test/math/testKalmanAcceleration.cpp | 47 +-
modules/core/test/math/testKalmanVelocity.cpp | 47 +-
modules/core/test/math/testMath.cpp | 247 +-
modules/core/test/math/testMatrix.cpp | 654 ++-
modules/core/test/math/testMatrixDeterminant.cpp | 464 ++
modules/core/test/math/testMatrixException.cpp | 28 +-
modules/core/test/math/testMatrixInverse.cpp | 557 ++-
modules/core/test/math/testMatrixPseudoInverse.cpp | 720 +++
modules/core/test/math/testPoseVector.cpp | 17 +-
modules/core/test/math/testQuaternion.cpp | 92 +-
modules/core/test/math/testRobust.cpp | 82 +-
modules/core/test/math/testRotation.cpp | 232 +-
modules/core/test/math/testRowVector.cpp | 41 +-
modules/core/test/math/testSvd.cpp | 497 +-
modules/core/test/math/testTranslationVector.cpp | 17 +-
modules/core/test/math/testTwistMatrix.cpp | 39 +-
modules/core/test/network/testClient.cpp | 19 +-
modules/core/test/network/testServer.cpp | 23 +-
modules/core/test/network/testUDPClient.cpp | 98 +
modules/core/test/network/testUDPServer.cpp | 117 +
modules/core/test/tools/convert/testConvert.cpp | 213 +-
.../test/tools/cpu-features/testCPUFeatures.cpp | 76 +
modules/core/test/tools/geometry/testPolygon.cpp | 84 +-
modules/core/test/tools/geometry/testRect.cpp | 10 +-
.../core/test/tools/histogram/testHistogram.cpp | 149 +-
.../core/test/tools/io/testBuildInformation.cpp | 8 +-
modules/core/test/tools/io/testIoTools.cpp | 418 +-
modules/core/test/tools/threading/testMutex.cpp | 35 +-
modules/core/test/tools/threading/testThread.cpp | 29 +-
modules/core/test/tools/threading/testThread2.cpp | 232 +-
modules/core/test/tools/time/testTime.cpp | 40 +-
modules/core/test/tools/xml/testXmlParser.cpp | 212 +-
modules/detection/CMakeLists.txt | 22 +-
.../include/visp3/detection/vpDetectorAprilTag.h | 291 ++
.../include/visp3/detection/vpDetectorBase.h | 73 +-
.../visp3/detection/vpDetectorDataMatrixCode.h | 32 +-
.../include/visp3/detection/vpDetectorFace.h | 37 +-
.../include/visp3/detection/vpDetectorQRCode.h | 32 +-
.../src/barcode/vpDetectorDataMatrixCode.cpp | 51 +-
modules/detection/src/barcode/vpDetectorQRCode.cpp | 26 +-
modules/detection/src/face/vpDetectorFace.cpp | 86 +-
modules/detection/src/tag/vpDetectorAprilTag.cpp | 463 ++
modules/detection/src/vpDetectorBase.cpp | 98 +-
modules/detection/test/testAprilTag.cpp | 404 ++
modules/gui/CMakeLists.txt | 7 +-
modules/gui/include/visp3/gui/vpD3DRenderer.h | 129 +-
modules/gui/include/visp3/gui/vpDisplayD3D.h | 27 +-
modules/gui/include/visp3/gui/vpDisplayGDI.h | 24 +-
modules/gui/include/visp3/gui/vpDisplayGTK.h | 159 +-
modules/gui/include/visp3/gui/vpDisplayOpenCV.h | 213 +-
modules/gui/include/visp3/gui/vpDisplayWin32.h | 129 +-
modules/gui/include/visp3/gui/vpDisplayX.h | 166 +-
modules/gui/include/visp3/gui/vpGDIRenderer.h | 91 +-
modules/gui/include/visp3/gui/vpPlot.h | 186 +-
modules/gui/include/visp3/gui/vpPlotCurve.h | 65 +-
modules/gui/include/visp3/gui/vpPlotGraph.h | 322 +-
.../gui/include/visp3/gui/vpProjectionDisplay.h | 94 +-
modules/gui/include/visp3/gui/vpWin32API.h | 22 +-
modules/gui/include/visp3/gui/vpWin32Renderer.h | 69 +-
modules/gui/include/visp3/gui/vpWin32Window.h | 55 +-
modules/gui/src/display/vpDisplayGTK.cpp | 1314 ++---
modules/gui/src/display/vpDisplayOpenCV.cpp | 1915 +++----
modules/gui/src/display/vpDisplayX.cpp | 3249 ++++++------
modules/gui/src/display/windows/vpD3DRenderer.cpp | 917 ++--
modules/gui/src/display/windows/vpDisplayD3D.cpp | 123 +-
modules/gui/src/display/windows/vpDisplayGDI.cpp | 119 +-
modules/gui/src/display/windows/vpDisplayWin32.cpp | 495 +-
modules/gui/src/display/windows/vpGDIRenderer.cpp | 593 ++-
modules/gui/src/display/windows/vpWin32API.cpp | 98 +-
modules/gui/src/display/windows/vpWin32Window.cpp | 232 +-
.../src/forward-projection/vpProjectionDisplay.cpp | 142 +-
modules/gui/src/plot/vpPlot.cpp | 726 +--
modules/gui/src/plot/vpPlotCurve.cpp | 81 +-
modules/gui/src/plot/vpPlotGraph.cpp | 1599 +++---
modules/gui/test/display/testClick.cpp | 248 +-
.../gui/test/display/testDisplayPolygonLines.cpp | 230 +
modules/gui/test/display/testDisplayRoi.cpp | 53 +-
modules/gui/test/display/testDisplayScaled.cpp | 146 +-
modules/gui/test/display/testDisplays.cpp | 152 +-
modules/gui/test/display/testMouseEvent.cpp | 293 +-
modules/gui/test/display/testVideoDevice.cpp | 254 +-
modules/gui/test/display/testVideoDeviceDual.cpp | 156 +-
modules/imgproc/CMakeLists.txt | 56 +
modules/imgproc/README.md | 9 +
modules/imgproc/doc/module.doc | 69 +
modules/imgproc/include/visp3/imgproc/vpContours.h | 254 +
modules/imgproc/include/visp3/imgproc/vpImgproc.h | 151 +
modules/imgproc/src/vpCLAHE.cpp | 528 ++
modules/imgproc/src/vpConnectedComponents.cpp | 172 +
modules/imgproc/src/vpContours.cpp | 468 ++
modules/imgproc/src/vpFloodFill.cpp | 173 +
modules/imgproc/src/vpImgproc.cpp | 706 +++
modules/imgproc/src/vpMorph.cpp | 180 +
modules/imgproc/src/vpRetinex.cpp | 305 ++
modules/imgproc/src/vpThreshold.cpp | 421 ++
modules/imgproc/test/testAutoThreshold.cpp | 310 ++
modules/imgproc/test/testConnectedComponents.cpp | 403 ++
modules/imgproc/test/testContours.cpp | 439 ++
modules/imgproc/test/testFloodFill.cpp | 383 ++
modules/imgproc/test/testImgproc.cpp | 420 ++
modules/io/CMakeLists.txt | 13 +-
modules/io/include/visp3/io/vpDiskGrabber.h | 78 +-
modules/io/include/visp3/io/vpFFMPEG.h | 268 -
modules/io/include/visp3/io/vpImageIo.h | 109 +-
modules/io/include/visp3/io/vpKeyboard.h | 25 +-
modules/io/include/visp3/io/vpParallelPort.h | 20 +-
.../io/include/visp3/io/vpParallelPortException.h | 81 +-
modules/io/include/visp3/io/vpParseArgv.h | 89 +-
modules/io/include/visp3/io/vpVideoReader.h | 429 +-
modules/io/include/visp3/io/vpVideoWriter.h | 287 +-
modules/io/src/image/vpImageIo.cpp | 1323 +++--
modules/io/src/parallel-port/vpParallelPort.cpp | 54 +-
modules/io/src/tools/vpKeyboard.cpp | 58 +-
modules/io/src/tools/vpParseArgv.cpp | 837 ++--
modules/io/src/video/vpDiskGrabber.cpp | 314 +-
modules/io/src/video/vpFFMPEG.cpp | 928 ----
modules/io/src/video/vpVideoReader.cpp | 986 ++--
modules/io/src/video/vpVideoWriter.cpp | 283 +-
modules/robot/CMakeLists.txt | 38 +-
modules/robot/include/visp3/robot/vpAfma4.h | 54 +-
modules/robot/include/visp3/robot/vpAfma6.h | 112 +-
modules/robot/include/visp3/robot/vpBiclops.h | 92 +-
.../robot/include/visp3/robot/vpImageSimulator.h | 392 +-
modules/robot/include/visp3/robot/vpPioneer.h | 32 +-
modules/robot/include/visp3/robot/vpPioneerPan.h | 78 +-
modules/robot/include/visp3/robot/vpPtu46.h | 30 +-
modules/robot/include/visp3/robot/vpRingLight.h | 19 +-
modules/robot/include/visp3/robot/vpRobot.h | 119 +-
modules/robot/include/visp3/robot/vpRobotAfma4.h | 95 +-
modules/robot/include/visp3/robot/vpRobotAfma6.h | 136 +-
modules/robot/include/visp3/robot/vpRobotBiclops.h | 113 +-
.../include/visp3/robot/vpRobotBiclopsController.h | 76 +-
modules/robot/include/visp3/robot/vpRobotCamera.h | 46 +-
.../robot/include/visp3/robot/vpRobotException.h | 168 +-
modules/robot/include/visp3/robot/vpRobotPioneer.h | 58 +-
modules/robot/include/visp3/robot/vpRobotPtu46.h | 71 +-
.../robot/include/visp3/robot/vpRobotSimulator.h | 36 +-
.../robot/include/visp3/robot/vpRobotTemplate.h | 48 +-
.../robot/include/visp3/robot/vpRobotViper650.h | 156 +-
.../robot/include/visp3/robot/vpRobotViper850.h | 166 +-
.../visp3/robot/vpRobotWireFrameSimulator.h | 873 ++--
modules/robot/include/visp3/robot/vpServolens.h | 53 +-
.../robot/include/visp3/robot/vpSimulatorAfma6.h | 230 +-
.../robot/include/visp3/robot/vpSimulatorCamera.h | 36 +-
.../robot/include/visp3/robot/vpSimulatorPioneer.h | 44 +-
.../include/visp3/robot/vpSimulatorPioneerPan.h | 40 +-
.../include/visp3/robot/vpSimulatorViper850.h | 238 +-
modules/robot/include/visp3/robot/vpUnicycle.h | 72 +-
modules/robot/include/visp3/robot/vpViper.h | 62 +-
modules/robot/include/visp3/robot/vpViper650.h | 64 +-
modules/robot/include/visp3/robot/vpViper850.h | 66 +-
modules/robot/include/visp3/robot/vpVirtuose.h | 119 +-
.../include/visp3/robot/vpWireFrameSimulator.h | 418 +-
.../visp3/robot/vpWireFrameSimulatorTypes.h | 106 +-
.../src/haptic-device/virtuose/vpVirtuose.cpp | 422 +-
.../robot/src/image-simulator/vpImageSimulator.cpp | 1387 +++---
modules/robot/src/light/vpRingLight.cpp | 79 +-
modules/robot/src/real-robot/afma4/vpAfma4.cpp | 270 +-
.../robot/src/real-robot/afma4/vpRobotAfma4.cpp | 795 ++-
modules/robot/src/real-robot/afma4/vpServolens.cpp | 510 +-
modules/robot/src/real-robot/afma6/vpAfma6.cpp | 909 ++--
.../robot/src/real-robot/afma6/vpRobotAfma6.cpp | 993 ++--
modules/robot/src/real-robot/biclops/vpBiclops.cpp | 216 +-
.../src/real-robot/biclops/vpRobotBiclops.cpp | 1058 ++--
.../biclops/vpRobotBiclopsController.cpp | 207 +-
.../src/real-robot/pioneer/vpRobotPioneer.cpp | 162 +-
modules/robot/src/real-robot/ptu46/vpPtu46.cpp | 151 +-
.../robot/src/real-robot/ptu46/vpRobotPtu46.cpp | 752 ++-
.../robot/src/real-robot/viper/vpRobotViper650.cpp | 1380 +++---
.../robot/src/real-robot/viper/vpRobotViper850.cpp | 1369 +++--
modules/robot/src/real-robot/viper/vpViper.cpp | 635 ++-
modules/robot/src/real-robot/viper/vpViper650.cpp | 501 +-
modules/robot/src/real-robot/viper/vpViper850.cpp | 495 +-
.../robot/src/robot-simulator/vpRobotCamera.cpp | 161 +-
.../robot/src/robot-simulator/vpRobotSimulator.cpp | 11 +-
.../robot-simulator/vpRobotWireFrameSimulator.cpp | 347 +-
.../robot/src/robot-simulator/vpSimulatorAfma6.cpp | 2176 ++++----
.../src/robot-simulator/vpSimulatorCamera.cpp | 147 +-
.../src/robot-simulator/vpSimulatorPioneer.cpp | 157 +-
.../src/robot-simulator/vpSimulatorPioneerPan.cpp | 152 +-
.../src/robot-simulator/vpSimulatorViper850.cpp | 2101 ++++----
modules/robot/src/vpRobot.cpp | 172 +-
modules/robot/src/vpRobotTemplate.cpp | 84 +-
modules/robot/src/wireframe-simulator/vpArit.cpp | 785 ++-
modules/robot/src/wireframe-simulator/vpArit.h | 309 +-
modules/robot/src/wireframe-simulator/vpAritio.cpp | 105 +-
modules/robot/src/wireframe-simulator/vpAritio.h | 18 +-
modules/robot/src/wireframe-simulator/vpBound.cpp | 214 +-
modules/robot/src/wireframe-simulator/vpBound.h | 114 +-
.../robot/src/wireframe-simulator/vpBoundio.cpp | 232 +-
modules/robot/src/wireframe-simulator/vpBoundio.h | 13 +-
.../robot/src/wireframe-simulator/vpClipping.cpp | 572 ++-
modules/robot/src/wireframe-simulator/vpClipping.h | 21 +-
.../src/wireframe-simulator/vpCoreDisplay.cpp | 203 +-
.../robot/src/wireframe-simulator/vpCoreDisplay.h | 19 +-
modules/robot/src/wireframe-simulator/vpImstack.h | 19 +-
.../robot/src/wireframe-simulator/vpKeyword.cpp | 227 +-
modules/robot/src/wireframe-simulator/vpKeyword.h | 15 +-
modules/robot/src/wireframe-simulator/vpLex.cpp | 1052 ++--
modules/robot/src/wireframe-simulator/vpLex.h | 29 +-
modules/robot/src/wireframe-simulator/vpMy.h | 58 +-
modules/robot/src/wireframe-simulator/vpMyio.cpp | 63 +-
modules/robot/src/wireframe-simulator/vpMyio.h | 17 +-
modules/robot/src/wireframe-simulator/vpParser.cpp | 90 +-
modules/robot/src/wireframe-simulator/vpParser.h | 9 +-
.../robot/src/wireframe-simulator/vpProjection.cpp | 275 +-
.../robot/src/wireframe-simulator/vpProjection.h | 15 +-
.../robot/src/wireframe-simulator/vpRfstack.cpp | 176 +-
modules/robot/src/wireframe-simulator/vpRfstack.h | 25 +-
modules/robot/src/wireframe-simulator/vpScene.cpp | 228 +-
modules/robot/src/wireframe-simulator/vpScene.h | 53 +-
modules/robot/src/wireframe-simulator/vpSkipio.cpp | 74 +-
modules/robot/src/wireframe-simulator/vpSkipio.h | 11 +-
.../robot/src/wireframe-simulator/vpTmstack.cpp | 145 +-
modules/robot/src/wireframe-simulator/vpTmstack.h | 36 +-
modules/robot/src/wireframe-simulator/vpToken.cpp | 60 +-
modules/robot/src/wireframe-simulator/vpToken.h | 89 +-
modules/robot/src/wireframe-simulator/vpView.h | 174 +-
modules/robot/src/wireframe-simulator/vpViewio.cpp | 189 +-
modules/robot/src/wireframe-simulator/vpViewio.h | 11 +-
.../robot/src/wireframe-simulator/vpVwstack.cpp | 311 +-
modules/robot/src/wireframe-simulator/vpVwstack.h | 21 +-
.../wireframe-simulator/vpWireFrameSimulator.cpp | 1580 +++---
modules/robot/test/servo-afma4/testAfma4.cpp | 17 +-
modules/robot/test/servo-afma4/testRobotAfma4.cpp | 17 +-
modules/robot/test/servo-afma6/testAfma6.cpp | 42 +-
modules/robot/test/servo-afma6/testRobotAfma6.cpp | 42 +-
.../robot/test/servo-afma6/testRobotAfma6Pose.cpp | 134 +-
.../test/servo-viper/testRobotViper650-frames.cpp | 99 +-
.../test/servo-viper/testRobotViper850-frames.cpp | 99 +-
.../robot/test/servo-viper/testRobotViper850.cpp | 41 +-
.../test/servo-viper/testRobotViper850Pose.cpp | 135 +-
modules/robot/test/servo-viper/testViper650.cpp | 38 +-
modules/robot/test/servo-viper/testViper850.cpp | 38 +-
modules/robot/test/virtuose/testVirtuose.cpp | 24 +-
modules/robot/test/virtuose/testVirtuoseAfma6.cpp | 45 +-
.../robot/test/virtuose/testVirtuoseHapticBox.cpp | 160 +-
.../test/virtuose/testVirtuoseJointLimits.cpp | 64 +-
.../test/virtuose/testVirtuosePeriodicFunction.cpp | 28 +-
modules/sensor/CMakeLists.txt | 46 +-
.../sensor/include/visp3/sensor/vp1394CMUGrabber.h | 197 +-
.../sensor/include/visp3/sensor/vp1394TwoGrabber.h | 316 +-
modules/sensor/include/visp3/sensor/vpComedi.h | 113 +-
.../include/visp3/sensor/vpDirectShowDevice.h | 41 +-
.../include/visp3/sensor/vpDirectShowGrabber.h | 80 +-
.../include/visp3/sensor/vpDirectShowGrabberImpl.h | 241 +-
.../visp3/sensor/vpDirectShowSampleGrabberI.h | 78 +-
.../include/visp3/sensor/vpFlyCaptureGrabber.h | 92 +-
.../include/visp3/sensor/vpForceTorqueAtiSensor.h | 36 +-
modules/sensor/include/visp3/sensor/vpKinect.h | 96 +-
modules/sensor/include/visp3/sensor/vpLaserScan.h | 76 +-
.../sensor/include/visp3/sensor/vpLaserScanner.h | 34 +-
.../sensor/include/visp3/sensor/vpOpenCVGrabber.h | 70 +-
.../sensor/include/visp3/sensor/vpPylonFactory.h | 92 +
.../sensor/include/visp3/sensor/vpPylonGrabber.h | 395 ++
modules/sensor/include/visp3/sensor/vpRealSense.h | 199 +-
modules/sensor/include/visp3/sensor/vpRealSense2.h | 361 ++
modules/sensor/include/visp3/sensor/vpScanPoint.h | 206 +-
modules/sensor/include/visp3/sensor/vpSickLDMRS.h | 70 +-
.../sensor/include/visp3/sensor/vpV4l2Grabber.h | 224 +-
modules/sensor/src/force-torque/vpComedi.cpp | 68 +-
.../src/force-torque/vpForceTorqueAtiSensor.cpp | 94 +-
.../src/framegrabber/1394/vp1394CMUGrabber.cpp | 561 +--
.../src/framegrabber/1394/vp1394TwoGrabber.cpp | 1798 +++----
.../src/framegrabber/OpenCV/vpOpenCVGrabber.cpp | 280 +-
.../framegrabber/directshow/vpDirectShowDevice.cpp | 110 +-
.../directshow/vpDirectShowGrabber.cpp | 204 +-
.../directshow/vpDirectShowGrabberImpl.cpp | 1799 ++++---
.../directshow/vpDirectShowSampleGrabberI.cpp | 480 +-
.../flycapture/vpFlyCaptureGrabber.cpp | 369 +-
.../src/framegrabber/pylon/vpPylonFactory.cpp | 89 +
.../src/framegrabber/pylon/vpPylonGrabberGigE.cpp | 810 +++
.../src/framegrabber/pylon/vpPylonGrabberGigE.h | 127 +
.../src/framegrabber/pylon/vpPylonGrabberUsb.cpp | 795 +++
.../src/framegrabber/pylon/vpPylonGrabberUsb.h | 127 +
.../sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp | 800 ++-
.../sensor/src/laserscanner/sick/vpSickLDMRS.cpp | 176 +-
modules/sensor/src/rgb-depth/kinect/vpKinect.cpp | 323 +-
.../sensor/src/rgb-depth/realsense/vpRealSense.cpp | 400 +-
.../src/rgb-depth/realsense/vpRealSense2.cpp | 794 +++
.../src/rgb-depth/realsense/vpRealSense_impl.h | 404 +-
modules/sensor/test/force-torque/testComedi.cpp | 25 +-
.../test/force-torque/testForceTorqueAti.cpp | 56 +-
.../test/framegrabber/test1394TwoGrabber.cpp | 45 +-
.../test/framegrabber/test1394TwoResetBus.cpp | 28 +-
.../sensor/test/framegrabber/testPylonGrabber.cpp | 139 +
modules/sensor/test/rgb-depth/testRealSense.cpp | 574 ---
.../sensor/test/rgb-depth/testRealSense2_SR300.cpp | 537 ++
.../sensor/test/rgb-depth/testRealSense_R200.cpp | 580 +++
.../sensor/test/rgb-depth/testRealSense_SR300.cpp | 515 ++
modules/tracker/blob/CMakeLists.txt | 7 +-
modules/tracker/blob/include/visp3/blob/vpDot.h | 325 +-
modules/tracker/blob/include/visp3/blob/vpDot2.h | 399 +-
modules/tracker/blob/src/dots/vpDot.cpp | 659 ++-
modules/tracker/blob/src/dots/vpDot2.cpp | 1677 +++----
.../tracker/blob/test/tracking/testTrackDot.cpp | 169 +-
modules/tracker/klt/CMakeLists.txt | 7 +-
.../tracker/klt/include/visp3/klt/vpKltOpencv.h | 359 +-
modules/tracker/klt/src/vpKltOpencv.cpp | 786 ++-
modules/tracker/mbt/CMakeLists.txt | 128 +-
.../mbt/include/visp3/mbt/vpMbDepthDenseTracker.h | 168 +
.../mbt/include/visp3/mbt/vpMbDepthNormalTracker.h | 179 +
.../include/visp3/mbt/vpMbEdgeKltMultiTracker.h | 265 +-
.../mbt/include/visp3/mbt/vpMbEdgeKltTracker.h | 248 +-
.../mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h | 265 +-
.../mbt/include/visp3/mbt/vpMbEdgeTracker.h | 471 +-
.../mbt/include/visp3/mbt/vpMbGenericTracker.h | 494 ++
.../mbt/include/visp3/mbt/vpMbHiddenFaces.h | 938 ++--
.../mbt/include/visp3/mbt/vpMbKltMultiTracker.h | 232 +-
.../tracker/mbt/include/visp3/mbt/vpMbKltTracker.h | 385 +-
.../tracker/mbt/include/visp3/mbt/vpMbScanLine.h | 135 +-
.../tracker/mbt/include/visp3/mbt/vpMbTracker.h | 539 +-
.../tracker/mbt/include/visp3/mbt/vpMbXmlParser.h | 108 +-
.../mbt/include/visp3/mbt/vpMbtDistanceCircle.h | 392 +-
.../mbt/include/visp3/mbt/vpMbtDistanceCylinder.h | 431 +-
.../include/visp3/mbt/vpMbtDistanceKltCylinder.h | 115 +-
.../mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h | 115 +-
.../mbt/include/visp3/mbt/vpMbtDistanceLine.h | 394 +-
.../mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h | 47 +-
.../mbt/include/visp3/mbt/vpMbtFaceDepthDense.h | 229 +
.../mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h | 291 ++
.../mbt/include/visp3/mbt/vpMbtKltXmlParser.h | 109 +-
.../tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h | 132 +-
.../tracker/mbt/include/visp3/mbt/vpMbtMeLine.h | 137 +-
.../tracker/mbt/include/visp3/mbt/vpMbtPolygon.h | 112 +-
.../mbt/include/visp3/mbt/vpMbtTukeyEstimator.h | 61 +
.../mbt/include/visp3/mbt/vpMbtXmlGenericParser.h | 563 +++
.../tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h | 48 +-
.../mbt/src/depth/vpMbDepthDenseTracker.cpp | 722 +++
.../mbt/src/depth/vpMbDepthNormalTracker.cpp | 763 +++
.../tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp | 765 +++
.../tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp | 1524 ++++++
.../tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp | 408 ++
.../tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp | 2178 ++++----
modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp | 2679 +++++-----
.../tracker/mbt/src/edge/vpMbtDistanceCircle.cpp | 279 +-
.../tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp | 652 +--
modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp | 823 +--
modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp | 367 +-
modules/tracker/mbt/src/edge/vpMbtMeLine.cpp | 640 ++-
modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp | 509 +-
.../mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp | 1895 ++++---
.../tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp | 1136 ++---
.../mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp | 164 +-
.../tracker/mbt/src/klt/vpMbKltMultiTracker.cpp | 1693 ++++---
modules/tracker/mbt/src/klt/vpMbKltTracker.cpp | 1199 ++---
.../mbt/src/klt/vpMbtDistanceKltCylinder.cpp | 590 ++-
.../tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp | 522 +-
modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp | 251 +-
modules/tracker/mbt/src/vpMbGenericTracker.cpp | 5244 ++++++++++++++++++++
modules/tracker/mbt/src/vpMbScanLine.cpp | 736 ++-
modules/tracker/mbt/src/vpMbTracker.cpp | 2357 +++++----
modules/tracker/mbt/src/vpMbXmlParser.cpp | 326 +-
modules/tracker/mbt/src/vpMbtPolygon.cpp | 164 +-
modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp | 1188 +++++
modules/tracker/mbt/test/testTukeyEstimator.cpp | 180 +
modules/tracker/me/CMakeLists.txt | 7 +-
modules/tracker/me/include/visp3/me/vpMe.h | 177 +-
modules/tracker/me/include/visp3/me/vpMeEllipse.h | 297 +-
modules/tracker/me/include/visp3/me/vpMeLine.h | 142 +-
modules/tracker/me/include/visp3/me/vpMeNurbs.h | 227 +-
modules/tracker/me/include/visp3/me/vpMeSite.h | 172 +-
modules/tracker/me/include/visp3/me/vpMeTracker.h | 115 +-
modules/tracker/me/include/visp3/me/vpNurbs.h | 120 +-
modules/tracker/me/src/moving-edges/vpMe.cpp | 627 ++-
.../tracker/me/src/moving-edges/vpMeEllipse.cpp | 1101 ++--
modules/tracker/me/src/moving-edges/vpMeLine.cpp | 1260 +++--
modules/tracker/me/src/moving-edges/vpMeNurbs.cpp | 1128 ++---
modules/tracker/me/src/moving-edges/vpMeSite.cpp | 579 ++-
.../tracker/me/src/moving-edges/vpMeTracker.cpp | 248 +-
modules/tracker/me/src/moving-edges/vpNurbs.cpp | 1038 ++--
modules/tracker/me/test/testNurbs.cpp | 213 +-
modules/tracker/tt/CMakeLists.txt | 7 +-
.../tt/include/visp3/tt/vpTemplateTracker.h | 428 +-
.../tt/include/visp3/tt/vpTemplateTrackerBSpline.h | 7 +-
.../tt/include/visp3/tt/vpTemplateTrackerHeader.h | 52 +-
.../tt/include/visp3/tt/vpTemplateTrackerSSD.h | 41 +-
.../tt/include/visp3/tt/vpTemplateTrackerSSDESM.h | 39 +-
.../tt/vpTemplateTrackerSSDForwardAdditional.h | 46 +-
.../tt/vpTemplateTrackerSSDForwardCompositional.h | 28 +-
.../tt/vpTemplateTrackerSSDInverseCompositional.h | 61 +-
.../include/visp3/tt/vpTemplateTrackerTriangle.h | 215 +-
.../tt/include/visp3/tt/vpTemplateTrackerWarp.h | 387 +-
.../include/visp3/tt/vpTemplateTrackerWarpAffine.h | 258 +-
.../visp3/tt/vpTemplateTrackerWarpHomography.h | 307 +-
.../visp3/tt/vpTemplateTrackerWarpHomographySL3.h | 339 +-
.../tt/include/visp3/tt/vpTemplateTrackerWarpRT.h | 81 +-
.../tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h | 255 +-
.../visp3/tt/vpTemplateTrackerWarpTranslation.h | 255 +-
.../tt/include/visp3/tt/vpTemplateTrackerZNCC.h | 43 +-
.../tt/vpTemplateTrackerZNCCForwardAdditional.h | 24 +-
.../tt/vpTemplateTrackerZNCCInverseCompositional.h | 49 +-
.../tt/include/visp3/tt/vpTemplateTrackerZone.h | 115 +-
.../tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp | 106 +-
.../tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp | 255 +-
.../ssd/vpTemplateTrackerSSDForwardAdditional.cpp | 222 +-
.../vpTemplateTrackerSSDForwardCompositional.cpp | 177 +-
.../vpTemplateTrackerSSDInverseCompositional.cpp | 229 +-
.../tt/src/tools/vpTemplateTrackerBSpline.cpp | 50 +-
.../tt/src/tools/vpTemplateTrackerTriangle.cpp | 331 +-
.../tracker/tt/src/tools/vpTemplateTrackerZone.cpp | 376 +-
modules/tracker/tt/src/vpTemplateTracker.cpp | 937 ++--
.../tracker/tt/src/warp/vpTemplateTrackerWarp.cpp | 153 +-
.../tt/src/warp/vpTemplateTrackerWarpAffine.cpp | 202 +-
.../src/warp/vpTemplateTrackerWarpHomography.cpp | 245 +-
.../warp/vpTemplateTrackerWarpHomographySL3.cpp | 498 +-
.../tt/src/warp/vpTemplateTrackerWarpRT.cpp | 185 +-
.../tt/src/warp/vpTemplateTrackerWarpSRT.cpp | 213 +-
.../src/warp/vpTemplateTrackerWarpTranslation.cpp | 106 +-
.../tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp | 122 +-
.../vpTemplateTrackerZNCCForwardAdditional.cpp | 386 +-
.../vpTemplateTrackerZNCCInverseCompositional.cpp | 547 +-
modules/tracker/tt_mi/CMakeLists.txt | 7 +-
.../include/visp3/tt_mi/vpTemplateTrackerMI.h | 128 +-
.../visp3/tt_mi/vpTemplateTrackerMIBSpline.h | 69 +-
.../include/visp3/tt_mi/vpTemplateTrackerMIESM.h | 57 +-
.../tt_mi/vpTemplateTrackerMIForwardAdditional.h | 81 +-
.../vpTemplateTrackerMIForwardCompositional.h | 17 +-
.../vpTemplateTrackerMIInverseCompositional.h | 92 +-
.../tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp | 950 ++--
.../tt_mi/src/mi/vpTemplateTrackerMIESM.cpp | 558 ++-
.../mi/vpTemplateTrackerMIForwardAdditional.cpp | 424 +-
.../mi/vpTemplateTrackerMIForwardCompositional.cpp | 332 +-
.../mi/vpTemplateTrackerMIInverseCompositional.cpp | 551 +-
.../tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp | 738 +--
modules/vision/CMakeLists.txt | 11 +-
.../vision/include/visp3/vision/vpBasicKeyPoint.h | 326 +-
.../vision/include/visp3/vision/vpCalibration.h | 177 +-
.../include/visp3/vision/vpCalibrationException.h | 92 +-
.../vision/include/visp3/vision/vpFernClassifier.h | 189 +-
modules/vision/include/visp3/vision/vpHomography.h | 395 +-
modules/vision/include/visp3/vision/vpKeyPoint.h | 889 ++--
.../vision/include/visp3/vision/vpKeyPointSurf.h | 246 +-
.../include/visp3/vision/vpLevenbergMarquartd.h | 63 +-
.../include/visp3/vision/vpPlanarObjectDetector.h | 241 +-
modules/vision/include/visp3/vision/vpPose.h | 335 +-
.../vision/include/visp3/vision/vpPoseException.h | 89 +-
.../vision/include/visp3/vision/vpPoseFeatures.h | 684 ++-
.../visp3/vision/vpXmlConfigParserKeyPoint.h | 162 +-
modules/vision/src/calibration/vpCalibration.cpp | 600 ++-
.../vision/src/calibration/vpCalibrationTools.cpp | 2273 ++++-----
.../src/homography-estimation/vpHomography.cpp | 572 +--
.../src/homography-estimation/vpHomographyDLT.cpp | 366 +-
.../homography-estimation/vpHomographyExtract.cpp | 894 ++--
.../homography-estimation/vpHomographyMalis.cpp | 719 ++-
.../homography-estimation/vpHomographyRansac.cpp | 470 +-
.../src/homography-estimation/vpHomographyVVS.cpp | 994 ++--
modules/vision/src/key-point/vpBasicKeyPoint.cpp | 10 +-
modules/vision/src/key-point/vpFernClassifier.cpp | 411 +-
modules/vision/src/key-point/vpKeyPoint.cpp | 3399 +++++++------
modules/vision/src/key-point/vpKeyPointSurf.cpp | 508 +-
.../src/key-point/vpPlanarObjectDetector.cpp | 404 +-
.../src/key-point/vpXmlConfigParserKeyPoint.cpp | 199 +-
.../src/pose-estimation/vpLevenbergMarquartd.cpp | 815 ++-
modules/vision/src/pose-estimation/vpPose.cpp | 744 ++-
.../vision/src/pose-estimation/vpPoseDementhon.cpp | 722 ++-
.../vision/src/pose-estimation/vpPoseFeatures.cpp | 443 +-
.../vision/src/pose-estimation/vpPoseLagrange.cpp | 652 +--
modules/vision/src/pose-estimation/vpPoseLowe.cpp | 337 +-
.../vision/src/pose-estimation/vpPoseRansac.cpp | 772 +--
.../vpPoseVirtualVisualServoing.cpp | 290 +-
.../vision/test/homography/testDisplacement.cpp | 141 +-
modules/vision/test/key-point/testKeyPoint-2.cpp | 219 +-
modules/vision/test/key-point/testKeyPoint-3.cpp | 102 +-
modules/vision/test/key-point/testKeyPoint-4.cpp | 197 +-
modules/vision/test/key-point/testKeyPoint-5.cpp | 99 +-
modules/vision/test/key-point/testKeyPoint-6.cpp | 104 +-
modules/vision/test/key-point/testKeyPoint-7.cpp | 390 +-
modules/vision/test/key-point/testKeyPoint.cpp | 93 +-
modules/vision/test/key-point/testSurfKeyPoint.cpp | 204 +-
modules/vision/test/pose/testFindMatch.cpp | 54 +-
modules/vision/test/pose/testPose.cpp | 105 +-
modules/vision/test/pose/testPoseFeatures.cpp | 97 +-
modules/vision/test/pose/testPoseRansac.cpp | 88 +-
modules/vision/test/pose/testPoseRansac2.cpp | 3633 +++++++-------
modules/visual_features/CMakeLists.txt | 7 +-
.../include/visp3/visual_features/vpBasicFeature.h | 80 +-
.../visp3/visual_features/vpFeatureBuilder.h | 122 +-
.../include/visp3/visual_features/vpFeatureDepth.h | 83 +-
.../visp3/visual_features/vpFeatureEllipse.h | 75 +-
.../visp3/visual_features/vpFeatureException.h | 85 +-
.../include/visp3/visual_features/vpFeatureLine.h | 80 +-
.../visp3/visual_features/vpFeatureLuminance.h | 95 +-
.../visp3/visual_features/vpFeatureMoment.h | 237 +-
.../visp3/visual_features/vpFeatureMomentAlpha.h | 149 +-
.../visp3/visual_features/vpFeatureMomentArea.h | 63 +-
.../vpFeatureMomentAreaNormalized.h | 153 +-
.../visp3/visual_features/vpFeatureMomentBasic.h | 78 +-
.../visual_features/vpFeatureMomentCInvariant.h | 428 +-
.../visual_features/vpFeatureMomentCentered.h | 100 +-
.../visp3/visual_features/vpFeatureMomentCommon.h | 138 +-
.../visual_features/vpFeatureMomentDatabase.h | 69 +-
.../visual_features/vpFeatureMomentGravityCenter.h | 186 +-
.../vpFeatureMomentGravityCenterNormalized.h | 215 +-
.../include/visp3/visual_features/vpFeaturePoint.h | 82 +-
.../visp3/visual_features/vpFeaturePoint3D.h | 95 +-
.../visp3/visual_features/vpFeaturePointPolar.h | 111 +-
.../visp3/visual_features/vpFeatureSegment.h | 184 +-
.../visp3/visual_features/vpFeatureThetaU.h | 126 +-
.../visp3/visual_features/vpFeatureTranslation.h | 162 +-
.../visual_features/vpFeatureVanishingPoint.h | 68 +-
.../visp3/visual_features/vpGenericFeature.h | 108 +-
.../feature-builder/vpFeatureBuilderEllipse.cpp | 350 +-
.../src/feature-builder/vpFeatureBuilderLine.cpp | 209 +-
.../src/feature-builder/vpFeatureBuilderPoint.cpp | 259 +-
.../feature-builder/vpFeatureBuilderPoint3D.cpp | 41 +-
.../feature-builder/vpFeatureBuilderPointPolar.cpp | 171 +-
.../feature-builder/vpFeatureBuilderSegment.cpp | 169 +-
.../vpFeatureBuilderVanishingPoint.cpp | 91 +-
.../src/visual-feature/vpBasicFeature.cpp | 136 +-
.../src/visual-feature/vpFeatureDepth.cpp | 357 +-
.../src/visual-feature/vpFeatureEllipse.cpp | 487 +-
.../src/visual-feature/vpFeatureLine.cpp | 381 +-
.../src/visual-feature/vpFeatureLuminance.cpp | 339 +-
.../src/visual-feature/vpFeatureMoment.cpp | 298 +-
.../src/visual-feature/vpFeatureMomentAlpha.cpp | 205 +-
.../src/visual-feature/vpFeatureMomentArea.cpp | 106 +-
.../vpFeatureMomentAreaNormalized.cpp | 260 +-
.../src/visual-feature/vpFeatureMomentBasic.cpp | 196 +-
.../visual-feature/vpFeatureMomentCInvariant.cpp | 980 ++--
.../src/visual-feature/vpFeatureMomentCentered.cpp | 359 +-
.../src/visual-feature/vpFeatureMomentCommon.cpp | 77 +-
.../src/visual-feature/vpFeatureMomentDatabase.cpp | 68 +-
.../vpFeatureMomentGravityCenter.cpp | 174 +-
.../vpFeatureMomentGravityCenterNormalized.cpp | 306 +-
.../src/visual-feature/vpFeaturePoint.cpp | 423 +-
.../src/visual-feature/vpFeaturePoint3D.cpp | 441 +-
.../src/visual-feature/vpFeaturePointPolar.cpp | 429 +-
.../src/visual-feature/vpFeatureSegment.cpp | 481 +-
.../src/visual-feature/vpFeatureThetaU.cpp | 445 +-
.../src/visual-feature/vpFeatureTranslation.cpp | 459 +-
.../src/visual-feature/vpFeatureVanishingPoint.cpp | 279 +-
.../src/visual-feature/vpGenericFeature.cpp | 492 +-
modules/visual_features/test/feature/testPoint.cpp | 228 +-
modules/vs/CMakeLists.txt | 7 +-
modules/vs/include/visp3/vs/vpAdaptiveGain.h | 233 +-
modules/vs/include/visp3/vs/vpServo.h | 541 +-
modules/vs/include/visp3/vs/vpServoData.h | 60 +-
modules/vs/include/visp3/vs/vpServoDisplay.h | 30 +-
modules/vs/include/visp3/vs/vpServoException.h | 76 +-
modules/vs/src/vpAdaptiveGain.cpp | 343 +-
modules/vs/src/vpServo.cpp | 1754 ++++---
modules/vs/src/vpServoData.cpp | 98 +-
modules/vs/src/vpServoDisplay.cpp | 46 +-
modules/vs/test/visual-feature/testFeature.cpp | 39 +-
.../vs/test/visual-feature/testFeatureMoment.cpp | 254 +-
.../vs/test/visual-feature/testFeatureSegment.cpp | 141 +-
platforms/ios/build_framework.py | 10 +-
platforms/ios/cmake/Modules/Platform/iOS.cmake | 86 +-
.../Toolchains/Toolchain-iPhoneOS_Xcode.cmake | 29 +-
.../Toolchain-iPhoneSimulator_Xcode.cmake | 29 +-
platforms/naoqi/cmake/extra.cmake | 6 -
script/create_module.py | 220 +
script/format-coding-style.sh | 33 +
tutorial/CMakeLists.txt | 51 +-
tutorial/bridge/opencv/tutorial-bridge-opencv.cpp | 8 +-
tutorial/computer-vision/CMakeLists.txt | 5 +
.../tutorial-homography-from-points.cpp | 29 +-
.../tutorial-pose-from-points-image.cpp | 36 +-
.../tutorial-pose-from-points-tracking.cpp | 72 +-
.../tutorial-pose-from-qrcode-image.cpp | 47 +-
tutorial/detection/barcode/CMakeLists.txt | 5 +
.../barcode/tutorial-barcode-detector-live.cpp | 41 +-
.../barcode/tutorial-barcode-detector.cpp | 37 +-
.../face/tutorial-face-detector-live-threaded.cpp | 85 +-
.../detection/face/tutorial-face-detector-live.cpp | 62 +-
tutorial/detection/face/tutorial-face-detector.cpp | 27 +-
.../matching/tutorial-matching-keypoint-SIFT.cpp | 26 +-
.../tutorial-matching-keypoint-homography.cpp | 55 +-
.../matching/tutorial-matching-keypoint.cpp | 22 +-
.../matching/tutorial-matching-surf-deprecated.cpp | 17 +-
...utorial-matching-surf-homography-deprecated.cpp | 46 +-
tutorial/detection/object/CMakeLists.txt | 3 +-
tutorial/detection/object/teabox.ppm | 5 +
.../object/tutorial-detection-object-mbt.cpp | 74 +-
.../object/tutorial-detection-object-mbt2.cpp | 77 +-
tutorial/detection/tag/AprilTag.pgm | 5 +
tutorial/detection/tag/CMakeLists.txt | 24 +
.../tag/tutorial-apriltag-detector-live.cpp | 175 +
.../detection/tag/tutorial-apriltag-detector.cpp | 159 +
tutorial/grabber/CMakeLists.txt | 10 +
tutorial/grabber/tutorial-grabber-1394-writer.cpp | 15 +-
tutorial/grabber/tutorial-grabber-1394.cpp | 11 +-
tutorial/grabber/tutorial-grabber-CMU1394.cpp | 9 +-
tutorial/grabber/tutorial-grabber-basler-pylon.cpp | 78 +
tutorial/grabber/tutorial-grabber-flycapture.cpp | 15 +-
.../grabber/tutorial-grabber-opencv-threaded.cpp | 43 +-
tutorial/grabber/tutorial-grabber-opencv.cpp | 19 +-
tutorial/grabber/tutorial-grabber-realsense.cpp | 49 +
.../grabber/tutorial-grabber-v4l2-threaded.cpp | 41 +-
tutorial/grabber/tutorial-grabber-v4l2.cpp | 32 +-
tutorial/grabber/tutorial-video-reader.cpp | 20 +-
tutorial/grabber/tutorial-video-recorder.cpp | 55 +-
tutorial/image/tutorial-image-converter.cpp | 9 +-
.../image/tutorial-image-display-scaled-auto.cpp | 7 +-
.../image/tutorial-image-display-scaled-manu.cpp | 7 +-
tutorial/image/tutorial-image-display.cpp | 5 +-
tutorial/image/tutorial-image-filter.cpp | 40 +-
tutorial/image/tutorial-image-manipulation.cpp | 12 +-
tutorial/image/tutorial-image-reader.cpp | 7 +-
tutorial/image/tutorial-image-viewer.cpp | 10 +-
tutorial/image/tutorial-undistort.cpp | 12 +-
tutorial/image/tutorial-viewer.cpp | 16 +-
tutorial/imgproc/autothreshold/CMakeLists.txt | 23 +
tutorial/imgproc/autothreshold/grid36-03.pgm | 184 +
.../autothreshold/tutorial-autothreshold.cpp | 105 +
tutorial/imgproc/brightness/CMakeLists.txt | 23 +
.../imgproc/brightness/Sample_low_brightness.png | Bin 0 -> 202293 bytes
.../brightness/tutorial-brightness-adjustment.cpp | 143 +
.../imgproc/connected-components/CMakeLists.txt | 23 +
tutorial/imgproc/connected-components/img.pgm | Bin 0 -> 307254 bytes
.../tutorial-connected-components.cpp | 88 +
tutorial/imgproc/contour/CMakeLists.txt | 24 +
tutorial/imgproc/contour/Contours_tree.pgm | Bin 0 -> 307254 bytes
tutorial/imgproc/contour/grid36-03.pgm | 184 +
tutorial/imgproc/contour/tutorial-contour.cpp | 142 +
.../imgproc/contrast-sharpening/CMakeLists.txt | 23 +
.../contrast-sharpening/Crayfish-low-contrast.ppm | 5 +
.../tutorial-contrast-sharpening.cpp | 121 +
tutorial/imgproc/count-coins/CMakeLists.txt | 24 +
tutorial/imgproc/count-coins/coins1.pgm | 5 +
tutorial/imgproc/count-coins/coins2.pgm | Bin 0 -> 178674 bytes
.../imgproc/count-coins/tutorial-count-coins.cpp | 154 +
tutorial/imgproc/flood-fill/CMakeLists.txt | 16 +
.../imgproc/flood-fill/tutorial-flood-fill.cpp | 270 +
.../GettingStarted.xcodeproj/project.pbxproj | 318 ++
.../project.xcworkspace/contents.xcworkspacedata | 7 +
.../UserInterfaceState.xcuserstate | Bin 0 -> 10933 bytes
.../xcschemes/GettingStarted.xcscheme | 91 +
.../xcschemes/xcschememanagement.plist | 22 +
.../GettingStarted/GettingStarted/AppDelegate.h | 11 +
.../GettingStarted/GettingStarted/AppDelegate.m | 46 +
.../AppIcon.appiconset/Contents.json | 68 +
.../Base.lproj/LaunchScreen.storyboard | 27 +
.../GettingStarted/Base.lproj/Main.storyboard | 26 +
.../ios/GettingStarted/GettingStarted/Info.plist | 45 +
.../GettingStarted/GettingStarted/ViewController.h | 9 +
.../GettingStarted/ViewController.mm | 116 +
tutorial/ios/GettingStarted/GettingStarted/main.m | 16 +
.../StartedAprilTag.xcodeproj/project.pbxproj | 335 ++
.../project.xcworkspace/contents.xcworkspacedata | 7 +
.../UserInterfaceState.xcuserstate | Bin 0 -> 11638 bytes
.../xcschemes/StartedAprilTag.xcscheme | 91 +
.../xcschemes/xcschememanagement.plist | 22 +
.../StartedAprilTag/StartedAprilTag/AppDelegate.h | 11 +
.../StartedAprilTag/StartedAprilTag/AppDelegate.m | 47 +
.../StartedAprilTag/StartedAprilTag/AprilTag.png | Bin 0 -> 130463 bytes
.../AppIcon.appiconset/Contents.json | 68 +
.../Base.lproj/LaunchScreen.storyboard | 27 +
.../StartedAprilTag/Base.lproj/Main.storyboard | 26 +
.../StartedAprilTag/ImageConversion.h | 52 +
.../StartedAprilTag/ImageConversion.mm | 214 +
.../StartedAprilTag/StartedAprilTag/ImageDisplay.h | 49 +
.../StartedAprilTag/ImageDisplay.mm | 140 +
.../ios/StartedAprilTag/StartedAprilTag/Info.plist | 45 +
.../StartedAprilTag/ViewController.h | 11 +
.../StartedAprilTag/ViewController.mm | 89 +
.../ios/StartedAprilTag/StartedAprilTag/main.m | 16 +
.../StartedImageProc.xcodeproj/project.pbxproj | 6 +
.../UserInterfaceState.xcuserstate | Bin 11976 -> 16256 bytes
.../StartedImageProc/AppDelegate.h | 12 +-
.../StartedImageProc/AppDelegate.m | 12 +-
.../StartedImageProc/ImageConversion.h | 52 +
.../StartedImageProc/ImageConversion.mm | 214 +
.../StartedImageProc/ViewController.h | 5 +-
.../StartedImageProc/ViewController.mm | 355 +-
tutorial/robot/pioneer/tutorial-pioneer-robot.cpp | 3 +-
...orial-simu-pioneer-continuous-gain-adaptive.cpp | 58 +-
...orial-simu-pioneer-continuous-gain-constant.cpp | 58 +-
.../robot/pioneer/tutorial-simu-pioneer-pan.cpp | 67 +-
tutorial/robot/pioneer/tutorial-simu-pioneer.cpp | 56 +-
.../simulator/image/tutorial-image-simulator.cpp | 32 +-
tutorial/trace/tutorial-trace.cpp | 16 +-
.../tracking/blob/tutorial-blob-auto-tracker.cpp | 17 +-
.../blob/tutorial-blob-tracker-live-firewire.cpp | 19 +-
.../blob/tutorial-blob-tracker-live-v4l2.cpp | 19 +-
.../keypoint/tutorial-klt-tracker-live-v4l2.cpp | 20 +-
.../keypoint/tutorial-klt-tracker-with-reinit.cpp | 68 +-
.../tracking/keypoint/tutorial-klt-tracker.cpp | 37 +-
tutorial/tracking/model-based/edges/CMakeLists.txt | 2 +-
.../model-based/edges/tutorial-mb-edge-tracker.cpp | 44 +-
.../tracking/model-based/generic/CMakeLists.txt | 2 +-
.../generic/tutorial-mb-tracker-full.cpp | 87 +-
.../model-based/generic/tutorial-mb-tracker.cpp | 52 +-
.../tracking/model-based/hybrid/CMakeLists.txt | 2 +-
.../hybrid/tutorial-mb-hybrid-tracker.cpp | 41 +-
.../tracking/model-based/keypoint/CMakeLists.txt | 2 +-
.../keypoint/tutorial-mb-klt-tracker.cpp | 38 +-
.../model-based/stereo-generic/CMakeLists.txt | 44 +
.../model-based/stereo-generic/cRightMcLeft.txt | 4 +
.../tracking/model-based/stereo-generic/teabox.cao | 27 +
.../model-based/stereo-generic/teabox.init | 5 +
.../tracking/model-based/stereo-generic/teabox.mpg | Bin 0 -> 239076 bytes
.../tracking/model-based/stereo-generic/teabox.ppm | 5 +
.../tracking/model-based/stereo-generic/teabox.xml | 44 +
.../model-based/stereo-generic/teabox_left.cao | 27 +
.../model-based/stereo-generic/teabox_left.init | 6 +
.../model-based/stereo-generic/teabox_left.mpg | Bin 0 -> 872448 bytes
.../model-based/stereo-generic/teabox_left.ppm | 5 +
.../model-based/stereo-generic/teabox_left.xml | 44 +
.../model-based/stereo-generic/teabox_right.cao | 27 +
.../model-based/stereo-generic/teabox_right.init | 6 +
.../model-based/stereo-generic/teabox_right.mpg | Bin 0 -> 864256 bytes
.../model-based/stereo-generic/teabox_right.ppm | 5 +
.../model-based/stereo-generic/teabox_right.xml | 44 +
.../tutorial-mb-generic-tracker-stereo-mono.cpp | 171 +
.../tutorial-mb-generic-tracker-stereo.cpp | 180 +
.../tracking/model-based/stereo/CMakeLists.txt | 2 +-
.../stereo/tutorial-mb-tracker-stereo-mono.cpp | 45 +-
.../stereo/tutorial-mb-tracker-stereo.cpp | 136 +-
.../moving-edges/tutorial-me-ellipse-tracker.cpp | 10 +-
.../moving-edges/tutorial-me-line-tracker.cpp | 21 +-
.../template-tracker/tutorial-template-tracker.cpp | 12 +-
tutorial/visual-servo/ibvs/CMakeLists.txt | 4 +-
.../ibvs/tutorial-ibvs-4pts-display.cpp | 69 +-
.../ibvs/tutorial-ibvs-4pts-image-tracking.cpp | 70 +-
.../ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp | 106 +-
.../visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp | 44 +-
...-ibvs-4pts-plotter-continuous-gain-adaptive.cpp | 33 +-
.../tutorial-ibvs-4pts-plotter-gain-adaptive.cpp | 31 +-
.../ibvs/tutorial-ibvs-4pts-plotter.cpp | 31 +-
.../ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp | 68 +-
.../tutorial-ibvs-4pts-wireframe-robot-afma6.cpp | 57 +-
.../tutorial-ibvs-4pts-wireframe-robot-viper.cpp | 77 +-
tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp | 27 +-
2122 files changed, 266954 insertions(+), 133445 deletions(-)
diff --git a/.clang-format b/.clang-format
new file mode 100644
index 0000000..67fb821
--- /dev/null
+++ b/.clang-format
@@ -0,0 +1,6 @@
+Language: Cpp
+BasedOnStyle: LLVM
+BreakBeforeBraces: Linux
+ColumnLimit: 120
+ConstructorInitializerIndentWidth: 2
+Standard: Cpp03
diff --git a/.travis.yml b/.travis.yml
index 2bb9c07..0a1f5bf 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -15,12 +15,15 @@ sudo: required
dist: trusty
before_install:
- - cd ${TRAVIS_BUILD_DIR} && { curl -O http://visp-doc.inria.fr/download/dataset/ViSP-images-3.0.0.zip ; cd -; }
- - unzip ${TRAVIS_BUILD_DIR}/ViSP-images-3.0.0.zip -d ${TRAVIS_BUILD_DIR}
+ - git clone https://github.com/lagadic/ViSP-images ${TRAVIS_BUILD_DIR}/ViSP-images
+# - cd ${TRAVIS_BUILD_DIR} && { curl -O http://visp-doc.inria.fr/download/dataset/ViSP-images-3.0.0.zip ; cd -; }
+# - unzip ${TRAVIS_BUILD_DIR}/ViSP-images-3.0.0.zip -d ${TRAVIS_BUILD_DIR}
# Get libs for OSX
- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew update; fi"
- #- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew tap homebrew/science; fi"
- #- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew install opencv3; fi"
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then /usr/bin/yes | pip2 uninstall numpy; fi"
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew tap homebrew/science; fi"
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew install opencv3; fi"
+ - "if [ $TRAVIS_OS_NAME = 'osx' ]; then export OpenCV_DIR=/usr/local/opt/opencv3; fi"
- "if [ $TRAVIS_OS_NAME = 'osx' ]; then brew install libxml2 libdc1394 gsl; fi"
# Get libs for Linux
@@ -37,6 +40,9 @@ before_install:
- "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libogre-1.9-dev libois-dev; fi"
- "if [ $TRAVIS_OS_NAME = 'linux' ]; then sudo apt-get install -qq libzbar-dev libdmtx-dev; fi"
+ # Work arround to found libomp.so when executing tests
+ - "if [ $CC = 'clang' ]; then export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/clang-3.9.0/lib; fi"
+
before_script:
- mkdir build
- cd build
diff --git a/3rdparty/apriltag/CMakeLists.txt b/3rdparty/apriltag/CMakeLists.txt
new file mode 100644
index 0000000..100de69
--- /dev/null
+++ b/3rdparty/apriltag/CMakeLists.txt
@@ -0,0 +1,73 @@
+project(${APRILTAG_LIBRARY})
+
+set(APRILTAG_MAJOR_VERSION 0 PARENT_SCOPE)
+set(APRILTAG_MINOR_VERSION 9 PARENT_SCOPE)
+set(APRILTAG_PATCH_VERSION 8 PARENT_SCOPE)
+
+vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+file(GLOB lib_srcs *.c common/*.c)
+file(GLOB lib_hdrs *.h common/*.h common/sys/*.h)
+
+vp_list_filterout(lib_srcs common/unionfind.c) # has no symbols
+vp_list_filterout(lib_srcs common/getopt.c) # useless and produces errors on uwp
+vp_list_filterout(lib_hdrs common/getopt.h) # useless and produces errors on uwp
+
+add_library(${APRILTAG_LIBRARY} STATIC ${lib_srcs} ${lib_hdrs})
+if(WIN32 AND MSVC)
+ target_link_libraries (${APRILTAG_LIBRARY} ${PTHREADS_LIBRARY})
+endif()
+
+if(C99_FOUND)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${C99_C_FLAGS}")
+endif()
+
+if(UNIX)
+ if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
+ endif()
+endif()
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${APRILTAG_LIBRARY} PROPERTIES FOLDER "3rdparty")
+endif()
+
+set_target_properties(${APRILTAG_LIBRARY}
+ PROPERTIES OUTPUT_NAME ${APRILTAG_LIBRARY}
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ COMPILE_PDB_NAME ${APRILTAG_LIBRARY}
+ COMPILE_PDB_NAME_DEBUG "${APRILTAG_LIBRARY}${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
+ )
+
+if(NOT BUILD_SHARED_LIBS)
+ vp_install_target(${APRILTAG_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
+endif()
+
+if(MSVC)
+ vp_set_source_file_compile_flag(apriltag.c /wd"4018" /wd"4305" /wd"4334" /wd"4244")
+ vp_set_source_file_compile_flag(apriltag_quad_thresh.c /wd"4018" /wd"4244")
+ vp_set_source_file_compile_flag(common/g2d.c /wd"4244")
+ vp_set_source_file_compile_flag(common/getopt.c /wd"4018" /wd"4244" /wd"4996")
+ vp_set_source_file_compile_flag(common/homography.c /wd"4244")
+ vp_set_source_file_compile_flag(common/image_f32.c /wd"4244")
+ vp_set_source_file_compile_flag(common/image_u8.c /wd"4244")
+ vp_set_source_file_compile_flag(common/image_u8x3.c /wd"4244")
+ vp_set_source_file_compile_flag(common/matd.c /wd"4018" /wd"4244")
+ vp_set_source_file_compile_flag(common/pjpeg-idct.c /wd"4244")
+ vp_set_source_file_compile_flag(common/pjpeg.c /wd4018 /wd"4267")
+ vp_set_source_file_compile_flag(common/string_util.c /wd"4996" /wd"4267")
+ vp_set_source_file_compile_flag(common/time_util.c /wd"4244")
+ vp_set_source_file_compile_flag(common/times.c /wd"4244")
+ vp_set_source_file_compile_flag(common/zhash.c /wd"4267")
+ vp_set_source_file_compile_flag(common/zmaxheap.c /wd"4098" /wd"4244")
+ vp_set_source_file_compile_flag(common/workerpool.c /wd"4018" /wd"4244")
+ vp_set_source_file_compile_flag(common/pam.c /wd"4018")
+ vp_set_source_file_compile_flag(common/string_util.c /wd"4018" /wd"4267" /wd"4996")
+ vp_set_source_file_compile_flag(tag16h5.c /wd"4996")
+ vp_set_source_file_compile_flag(tag25h7.c /wd"4996")
+ vp_set_source_file_compile_flag(tag25h9.c /wd"4996")
+ vp_set_source_file_compile_flag(tag36artoolkit.c /wd"4996")
+ vp_set_source_file_compile_flag(tag36h10.c /wd"4996")
+ vp_set_source_file_compile_flag(tag36h11.c /wd"4996")
+endif()
diff --git a/3rdparty/apriltag/LICENSE b/3rdparty/apriltag/LICENSE
new file mode 100644
index 0000000..4488ed5
--- /dev/null
+++ b/3rdparty/apriltag/LICENSE
@@ -0,0 +1,30 @@
+Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
diff --git a/3rdparty/apriltag/README b/3rdparty/apriltag/README
new file mode 100644
index 0000000..806243a
--- /dev/null
+++ b/3rdparty/apriltag/README
@@ -0,0 +1,93 @@
+INSTALL
+=======
+
+The default installation will place headers in /usr/local/include and
+shared library in /usr/local/lib. It also installs a pkg-config script
+into /usr/local/lib/pkgconfig.
+
+ $ make
+ $ sudo make install
+
+To install to a different directory than /usr/local:
+
+ $ PREFIX=/some/path sudo make install
+
+
+USAGE
+=====
+
+A basic AprilTag application can be seen in example/apriltag_demo.c.
+
+
+Initialization: instantiate a detector and at least one tag family.
+
+ apriltag_detector_t *td = apriltag_detector_create();
+ apriltag_family_t *tf = tag36h11_create();
+ apriltag_detector_add_family(td, tf);
+
+Some tag detector parameters can be set at this time.
+The default parameters are the recommended starting point.
+
+ td->quad_decimate = 1.0;
+ td->quad_sigma = 0.0;
+ td->refine_edges = 1;
+ td->refine_decode = 0;
+ td->refine_pose = 0;
+
+Increase the image decimation if faster processing is required; the
+trade-off is a slight decrease in detection range. A factor of 1.0
+means the full-size input image is used.
+
+Some Gaussian blur (quad_sigma) may help with noisy input images.
+
+
+Detection: a single one-line call will process an input image
+and return a list of detections.
+
+ zarray_t *detections = apriltag_detector_detect(td, im);
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ // Do something with det here
+ }
+
+ apriltag_detections_destroy(detections);
+
+zarray is a container class which is included with apriltag.
+To process through the list of detections, use zarray_get,
+as illustrated above.
+
+The caller is responsible for freeing detections by calling
+apriltag_detections_destroy().
+
+
+Cleanup: free the detector and tag family when done.
+
+ apriltag_detector_destroy(td);
+ tag36h11_destroy(tf);
+
+
+OPENCV INTEGRATION
+==================
+
+Note that this library has no external dependencies. Most applications
+will require, at minimum, a method for acquiring images.
+
+See example/opencv_demo.cc for an example of using AprilTag in C++ with OpenCV.
+This example application can be built by executing the following:
+
+ $ cd examples
+ $ make opencv_demo
+
+Image data in a cv::Mat object can be passed to AprilTag without creating
+a deep copy. Simply create an image_u8_t header for the cv::Mat data buffer:
+
+ cv::Mat img;
+
+ image_u8_t img_header = { .width = img.cols,
+ .height = img.rows,
+ .stride = img.cols,
+ .buf = img.data
+ };
diff --git a/3rdparty/apriltag/README.ViSP b/3rdparty/apriltag/README.ViSP
new file mode 100644
index 0000000..74c4577
--- /dev/null
+++ b/3rdparty/apriltag/README.ViSP
@@ -0,0 +1,25 @@
+Porting of the AprilTag library: https://april.eecs.umich.edu/software/apriltag.html
+AprilTag version is 2016-12-01 (0.9.8).
+
+Changes / modifications made:
+ - scale directly the pose computed in homography_to_pose() (homography.c)
+ by the marker size
+ - we observed that the pose returned by homography_to_pose() is left-handed,
+ we add a transformation to get a right-handed pose
+
+Windows porting (MinGW):
+ - see commit 4f5d150 and previous
+ - use C99 flag
+ - replace srandom and random by srand and rand
+ - replacement for <sys/time.h>, see https://www.codefull.org/2015/12/systime-h-replacement-for-windows/
+ - replacement for getline() function, see here https://stackoverflow.com/a/735472
+
+Windows porting (MSVC):
+ - see commit 8bf0b87
+ - see MinGW porting
+ - build AprilTag with pthreads-win32 (https://www.sourceware.org/pthreads-win32/)
+ - replace variable-length array in C99 by malloc as Visual Studio does not conform to C99
+ - replace some missing posix functions or constants
+
+Windows UWP porting (MSVC):
+ - remove functions that are using missing win32 functions not available on UWP (see #ifdef WINRT and #ifndef WINRT)
diff --git a/3rdparty/apriltag/apriltag.c b/3rdparty/apriltag/apriltag.c
new file mode 100644
index 0000000..172604b
--- /dev/null
+++ b/3rdparty/apriltag/apriltag.c
@@ -0,0 +1,1546 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include "apriltag.h"
+
+#include <math.h>
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+#include <stdio.h>
+#include <inttypes.h>
+
+#include "common/image_u8.h"
+#include "common/image_u8x3.h"
+#include "common/zhash.h"
+#include "common/zarray.h"
+#include "common/matd.h"
+#include "common/homography.h"
+#include "common/timeprofile.h"
+#include "common/math_util.h"
+#include "common/g2d.h"
+#include "common/floats.h"
+
+#include "apriltag_math.h"
+
+#include "common/postscript_utils.h"
+
+#ifndef M_PI
+# define M_PI 3.141592653589793238462643383279502884196
+#endif
+
+#if defined(__MINGW32__) || defined(_MSC_VER)
+#define srandom srand
+#define random rand
+#endif
+
+extern zarray_t *apriltag_quad_gradient(apriltag_detector_t *td, image_u8_t *im);
+extern zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im);
+
+// Regresses a model of the form:
+// intensity(x,y) = C0*x + C1*y + CC2
+// The J matrix is the:
+// J = [ x1 y1 1 ]
+// [ x2 y2 1 ]
+// [ ... ]
+// The A matrix is J'J
+
+struct graymodel
+{
+ double A[3][3];
+ double B[3];
+ double C[3];
+};
+
+void graymodel_init(struct graymodel *gm)
+{
+ memset(gm, 0, sizeof(struct graymodel));
+}
+
+void graymodel_add(struct graymodel *gm, double x, double y, double gray)
+{
+ // update upper right entries of A = J'J
+ gm->A[0][0] += x*x;
+ gm->A[0][1] += x*y;
+ gm->A[0][2] += x;
+ gm->A[1][1] += y*y;
+ gm->A[1][2] += y;
+ gm->A[2][2] += 1;
+
+ // update B = J'gray
+ gm->B[0] += x * gray;
+ gm->B[1] += y * gray;
+ gm->B[2] += gray;
+}
+
+void graymodel_solve(struct graymodel *gm)
+{
+ mat33_sym_solve((double*) gm->A, gm->B, gm->C);
+}
+
+double graymodel_interpolate(struct graymodel *gm, double x, double y)
+{
+ return gm->C[0]*x + gm->C[1]*y + gm->C[2];
+}
+
+struct quick_decode_entry
+{
+ uint64_t rcode; // the queried code
+ uint16_t id; // the tag ID (a small integer)
+ uint8_t hamming; // how many errors corrected?
+ uint8_t rotation; // number of rotations [0, 3]
+};
+
+struct quick_decode
+{
+ int nentries;
+ struct quick_decode_entry *entries;
+};
+
+/** if the bits in w were arranged in a d*d grid and that grid was
+ * rotated, what would the new bits in w be?
+ * The bits are organized like this (for d = 3):
+ *
+ * 8 7 6 2 5 8 0 1 2
+ * 5 4 3 ==> 1 4 7 ==> 3 4 5 (rotate90 applied twice)
+ * 2 1 0 0 3 6 6 7 8
+ **/
+static uint64_t rotate90(uint64_t w, uint32_t d)
+{
+ uint64_t wr = 0;
+
+ for (int32_t r = d-1; r >=0; r--) {
+ for (int32_t c = 0; c < d; c++) {
+ int32_t b = r + d*c;
+
+ wr = wr << 1;
+
+ if ((w & (((uint64_t) 1) << b))!=0)
+ wr |= 1;
+ }
+ }
+
+ return wr;
+}
+
+void quad_destroy(struct quad *quad)
+{
+ if (!quad)
+ return;
+
+ matd_destroy(quad->H);
+ matd_destroy(quad->Hinv);
+ free(quad);
+}
+
+struct quad *quad_copy(struct quad *quad)
+{
+ struct quad *q = calloc(1, sizeof(struct quad));
+ memcpy(q, quad, sizeof(struct quad));
+ if (quad->H)
+ q->H = matd_copy(quad->H);
+ if (quad->Hinv)
+ q->Hinv = matd_copy(quad->Hinv);
+ return q;
+}
+
+void quick_decode_add(struct quick_decode *qd, uint64_t code, int id, int hamming)
+{
+ uint32_t bucket = code % qd->nentries;
+
+ while (qd->entries[bucket].rcode != UINT64_MAX) {
+ bucket = (bucket + 1) % qd->nentries;
+ }
+
+ qd->entries[bucket].rcode = code;
+ qd->entries[bucket].id = id;
+ qd->entries[bucket].hamming = hamming;
+}
+
+void quick_decode_uninit(apriltag_family_t *fam)
+{
+ if (!fam->impl)
+ return;
+
+ struct quick_decode *qd = (struct quick_decode*) fam->impl;
+ free(qd->entries);
+ free(qd);
+ fam->impl = NULL;
+}
+
+void quick_decode_init(apriltag_family_t *family, int maxhamming)
+{
+ assert(family->impl == NULL);
+ assert(family->ncodes < 65535);
+
+ struct quick_decode *qd = calloc(1, sizeof(struct quick_decode));
+ int capacity = family->ncodes;
+
+ int nbits = family->d * family->d;
+
+ if (maxhamming >= 1)
+ capacity += family->ncodes * nbits;
+
+ if (maxhamming >= 2)
+ capacity += family->ncodes * nbits * (nbits-1);
+
+ if (maxhamming >= 3)
+ capacity += family->ncodes * nbits * (nbits-1) * (nbits-2);
+
+ qd->nentries = capacity * 3;
+
+// printf("capacity %d, size: %.0f kB\n",
+// capacity, qd->nentries * sizeof(struct quick_decode_entry) / 1024.0);
+
+ qd->entries = calloc(qd->nentries, sizeof(struct quick_decode_entry));
+ if (qd->entries == NULL) {
+ printf("apriltag.c: failed to allocate hamming decode table. Reduce max hamming size.\n");
+ exit(-1);
+ }
+
+ for (int i = 0; i < qd->nentries; i++)
+ qd->entries[i].rcode = UINT64_MAX;
+
+ for (int i = 0; i < family->ncodes; i++) {
+ uint64_t code = family->codes[i];
+
+ // add exact code (hamming = 0)
+ quick_decode_add(qd, code, i, 0);
+
+ if (maxhamming >= 1) {
+ // add hamming 1
+ for (int j = 0; j < nbits; j++)
+ quick_decode_add(qd, code ^ (1L << j), i, 1);
+ }
+
+ if (maxhamming >= 2) {
+ // add hamming 2
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ quick_decode_add(qd, code ^ (1L << j) ^ (1L << k), i, 2);
+ }
+
+ if (maxhamming >= 3) {
+ // add hamming 3
+ for (int j = 0; j < nbits; j++)
+ for (int k = 0; k < j; k++)
+ for (int m = 0; m < k; m++)
+ quick_decode_add(qd, code ^ (1L << j) ^ (1L << k) ^ (1L << m), i, 3);
+ }
+
+ if (maxhamming > 3) {
+ printf("apriltag.c: maxhamming beyond 3 not supported\n");
+ }
+ }
+
+ family->impl = qd;
+
+ if (0) {
+ int longest_run = 0;
+ int run = 0;
+ int run_sum = 0;
+ int run_count = 0;
+
+ // This accounting code doesn't check the last possible run that
+ // occurs at the wrap-around. That's pretty insignificant.
+ for (int i = 0; i < qd->nentries; i++) {
+ if (qd->entries[i].rcode == UINT64_MAX) {
+ if (run > 0) {
+ run_sum += run;
+ run_count ++;
+ }
+ run = 0;
+ } else {
+ run ++;
+ longest_run = imax(longest_run, run);
+ }
+ }
+
+ printf("quick decode: longest run: %d, average run %.3f\n", longest_run, 1.0 * run_sum / run_count);
+ }
+}
+
+// returns an entry with hamming set to 255 if no decode was found.
+static void quick_decode_codeword(apriltag_family_t *tf, uint64_t rcode,
+ struct quick_decode_entry *entry)
+{
+ struct quick_decode *qd = (struct quick_decode*) tf->impl;
+
+ for (int ridx = 0; ridx < 4; ridx++) {
+
+ for (int bucket = rcode % qd->nentries;
+ qd->entries[bucket].rcode != UINT64_MAX;
+ bucket = (bucket + 1) % qd->nentries) {
+
+ if (qd->entries[bucket].rcode == rcode) {
+ *entry = qd->entries[bucket];
+ entry->rotation = ridx;
+ return;
+ }
+ }
+
+ rcode = rotate90(rcode, tf->d);
+ }
+
+ entry->rcode = 0;
+ entry->id = 65535;
+ entry->hamming = 255;
+ entry->rotation = 0;
+}
+
+static inline int detection_compare_function(const void *_a, const void *_b)
+{
+ apriltag_detection_t *a = *(apriltag_detection_t**) _a;
+ apriltag_detection_t *b = *(apriltag_detection_t**) _b;
+
+ return a->id - b->id;
+}
+
+void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam)
+{
+ quick_decode_uninit(fam);
+ zarray_remove_value(td->tag_families, &fam, 0);
+}
+
+void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected)
+{
+ zarray_add(td->tag_families, &fam);
+
+ if (!fam->impl)
+ quick_decode_init(fam, bits_corrected);
+}
+
+void apriltag_detector_clear_families(apriltag_detector_t *td)
+{
+ for (int i = 0; i < zarray_size(td->tag_families); i++) {
+ apriltag_family_t *fam;
+ zarray_get(td->tag_families, i, &fam);
+ quick_decode_uninit(fam);
+ }
+ zarray_clear(td->tag_families);
+}
+
+apriltag_detector_t *apriltag_detector_create()
+{
+ apriltag_detector_t *td = (apriltag_detector_t*) calloc(1, sizeof(apriltag_detector_t));
+
+ td->nthreads = 1;
+ td->quad_decimate = 1.0;
+ td->quad_sigma = 0.0;
+
+ td->qtp.max_nmaxima = 10;
+ td->qtp.min_cluster_pixels = 5;
+
+ td->qtp.max_line_fit_mse = 10.0;
+ td->qtp.critical_rad = 10 * M_PI / 180;
+ td->qtp.deglitch = 0;
+ td->qtp.min_white_black_diff = 5;
+
+ td->tag_families = zarray_create(sizeof(apriltag_family_t*));
+
+ pthread_mutex_init(&td->mutex, NULL);
+
+ td->tp = timeprofile_create();
+
+ td->refine_edges = 1;
+ td->refine_pose = 0;
+ td->refine_decode = 0;
+
+ td->debug = 0;
+
+ // NB: defer initialization of td->wp so that the user can
+ // override td->nthreads.
+
+ return td;
+}
+
+void apriltag_detector_destroy(apriltag_detector_t *td)
+{
+ timeprofile_destroy(td->tp);
+ workerpool_destroy(td->wp);
+
+ apriltag_detector_clear_families(td);
+
+ zarray_destroy(td->tag_families);
+ free(td);
+}
+
+struct quad_decode_task
+{
+ int i0, i1;
+ zarray_t *quads;
+ apriltag_detector_t *td;
+
+ image_u8_t *im;
+ zarray_t *detections;
+
+ image_u8_t *im_samples;
+};
+
+struct evaluate_quad_ret
+{
+ int64_t rcode;
+ double score;
+ matd_t *H, *Hinv;
+
+ int decode_status;
+ struct quick_decode_entry e;
+};
+
+// returns non-zero if an error occurs (i.e., H has no inverse)
+int quad_update_homographies(struct quad *quad)
+{
+ zarray_t *correspondences = zarray_create(sizeof(float[4]));
+
+ for (int i = 0; i < 4; i++) {
+ float corr[4];
+
+ // At this stage of the pipeline, we have not attempted to decode the
+ // quad into an oriented tag. Thus, just act as if the quad is facing
+ // "up" with respect to our desired corners. We'll fix the rotation
+ // later.
+ // [-1, -1], [1, -1], [1, 1], [-1, 1]
+ corr[0] = (i==0 || i==3) ? -1 : 1;
+ corr[1] = (i==0 || i==1) ? -1 : 1;
+
+ corr[2] = quad->p[i][0];
+ corr[3] = quad->p[i][1];
+
+ zarray_add(correspondences, &corr);
+ }
+
+ if (quad->H)
+ matd_destroy(quad->H);
+ if (quad->Hinv)
+ matd_destroy(quad->Hinv);
+
+ // XXX Tunable
+ quad->H = homography_compute(correspondences, HOMOGRAPHY_COMPUTE_FLAG_SVD);
+ quad->Hinv = matd_inverse(quad->H);
+ zarray_destroy(correspondences);
+
+ if (quad->H && quad->Hinv)
+ return 0;
+
+ return -1;
+}
+
+// compute a "score" for a quad that is independent of tag family
+// encoding (but dependent upon the tag geometry) by considering the
+// contrast around the exterior of the tag.
+double quad_goodness(apriltag_family_t *family, image_u8_t *im, struct quad *quad)
+{
+ // when sampling from the white border, how much white border do
+ // we actually consider valid, measured in bit-cell units? (the
+ // outside portions are often intruded upon, so it could be advantageous to use
+ // less than the "nominal" 1.0. (Less than 1.0 not well tested.)
+
+ // XXX Tunable
+ float white_border = 1;
+
+ // in tag coordinates, how big is each bit cell?
+ double bit_size = 2.0 / (2*family->black_border + family->d);
+// double inv_bit_size = 1.0 / bit_size;
+
+ int32_t xmin = INT32_MAX, xmax = 0, ymin = INT32_MAX, ymax = 0;
+
+ for (int i = 0; i < 4; i++) {
+ double tx = (i == 0 || i == 3) ? -1 - bit_size : 1 + bit_size;
+ double ty = (i == 0 || i == 1) ? -1 - bit_size : 1 + bit_size;
+ double x, y;
+
+ homography_project(quad->H, tx, ty, &x, &y);
+ xmin = imin(xmin, x);
+ xmax = imax(xmax, x);
+ ymin = imin(ymin, y);
+ ymax = imax(ymax, y);
+ }
+
+ // clamp bounding box to image dimensions
+ xmin = imax(0, xmin);
+ xmax = imin(im->width-1, xmax);
+ ymin = imax(0, ymin);
+ ymax = imin(im->height-1, ymax);
+
+// int nbits = family->d * family->d;
+
+ int64_t W1 = 0, B1 = 0, Wn = 0, Bn = 0;
+
+ float wsz = bit_size*white_border;
+ float bsz = bit_size*family->black_border;
+
+ matd_t *Hinv = quad->Hinv;
+// matd_t *H = quad->H;
+
+ // iterate over all the pixels in the tag. (Iterating in pixel space)
+ for (int y = ymin; y <= ymax; y++) {
+
+ // we'll incrementally compute the homography
+ // projections. Begin by evaluating the homogeneous position
+ // [(xmin - .5f), y, 1]. Then, we'll update as we stride in
+ // the +x direction.
+ double Hx = MATD_EL(Hinv, 0, 0) * (.5 + (int) xmin) +
+ MATD_EL(Hinv, 0, 1) * (y + .5) + MATD_EL(Hinv, 0, 2);
+ double Hy = MATD_EL(Hinv, 1, 0) * (.5 + (int) xmin) +
+ MATD_EL(Hinv, 1, 1) * (y + .5) + MATD_EL(Hinv, 1, 2);
+ double Hh = MATD_EL(Hinv, 2, 0) * (.5 + (int) xmin) +
+ MATD_EL(Hinv, 2, 1) * (y + .5) + MATD_EL(Hinv, 2, 2);
+
+ for (int x = xmin; x <= xmax; x++) {
+ // project the pixel center.
+ double tx, ty;
+
+ // divide by homogeneous coordinate
+ tx = Hx / Hh;
+ ty = Hy / Hh;
+
+ // if we move x one pixel to the right, here's what
+ // happens to our three pre-normalized coordinates.
+ Hx += MATD_EL(Hinv, 0, 0);
+ Hy += MATD_EL(Hinv, 1, 0);
+ Hh += MATD_EL(Hinv, 2, 0);
+
+ float txa = fabsf((float) tx), tya = fabsf((float) ty);
+ float xymax = fmaxf(txa, tya);
+
+// if (txa >= 1 + wsz || tya >= 1 + wsz)
+ if (xymax >= 1 + wsz)
+ continue;
+
+ uint8_t v = im->buf[y*im->stride + x];
+
+ // it's within the white border?
+// if (txa >= 1 || tya >= 1) {
+ if (xymax >= 1) {
+ W1 += v;
+ Wn ++;
+ continue;
+ }
+
+ // it's within the black border?
+// if (txa >= 1 - bsz || tya >= 1 - bsz) {
+ if (xymax >= 1 - bsz) {
+ B1 += v;
+ Bn ++;
+ continue;
+ }
+
+ // it must be a data bit. We don't do anything with these.
+ continue;
+ }
+ }
+
+
+ // score = average margin between white and black pixels near border.
+ double margin = 1.0 * W1 / Wn - 1.0 * B1 / Bn;
+// printf("margin %f: W1 %f, B1 %f\n", margin, W1, B1);
+
+ return margin;
+}
+
+// returns the decision margin. Return < 0 if the detection should be rejected.
+float quad_decode(apriltag_family_t *family, image_u8_t *im, struct quad *quad, struct quick_decode_entry *entry, image_u8_t *im_samples)
+{
+ // decode the tag binary contents by sampling the pixel
+ // closest to the center of each bit cell.
+
+ int64_t rcode = 0;
+
+ // how wide do we assume the white border is?
+ float white_border = 1.0;
+
+ // We will compute a threshold by sampling known white/black cells around this tag.
+ // This sampling is achieved by considering a set of samples along lines.
+ //
+ // coordinates are given in bit coordinates. ([0, fam->d]).
+ //
+ // { initial x, initial y, delta x, delta y, WHITE=1 }
+ float patterns[] = {
+ // left white column
+ 0 - white_border / 2.0, 0.5,
+ 0, 1,
+ 1,
+
+ // left black column
+ 0 + family->black_border / 2.0, 0.5,
+ 0, 1,
+ 0,
+
+ // right white column
+ 2*family->black_border + family->d + white_border / 2.0, .5,
+ 0, 1,
+ 1,
+
+ // right black column
+ 2*family->black_border + family->d - family->black_border / 2.0, .5,
+ 0, 1,
+ 0,
+
+ // top white row
+ 0.5, -white_border / 2.0,
+ 1, 0,
+ 1,
+
+ // top black row
+ 0.5, family->black_border / 2.0,
+ 1, 0,
+ 0,
+
+ // bottom white row
+ 0.5, 2*family->black_border + family->d + white_border / 2.0,
+ 1, 0,
+ 1,
+
+ // bottom black row
+ 0.5, 2*family->black_border + family->d - family->black_border / 2.0,
+ 1, 0,
+ 0
+
+ // XXX double-counts the corners.
+ };
+
+ struct graymodel whitemodel, blackmodel;
+ graymodel_init(&whitemodel);
+ graymodel_init(&blackmodel);
+
+ for (int pattern_idx = 0; pattern_idx < sizeof(patterns)/(5*sizeof(float)); pattern_idx ++) {
+ float *pattern = &patterns[pattern_idx * 5];
+
+ int is_white = pattern[4];
+
+ for (int i = 0; i < 2*family->black_border + family->d; i++) {
+ double tagx01 = (pattern[0] + i*pattern[2]) / (2*family->black_border + family->d);
+ double tagy01 = (pattern[1] + i*pattern[3]) / (2*family->black_border + family->d);
+
+ double tagx = 2*(tagx01-0.5);
+ double tagy = 2*(tagy01-0.5);
+
+ double px, py;
+ homography_project(quad->H, tagx, tagy, &px, &py);
+
+ // don't round
+ int ix = px;
+ int iy = py;
+ if (ix < 0 || iy < 0 || ix >= im->width || iy >= im->height)
+ continue;
+
+ int v = im->buf[iy*im->stride + ix];
+
+ if (im_samples) {
+ im_samples->buf[iy*im_samples->stride + ix] = (1-is_white)*255;
+ }
+
+ if (is_white)
+ graymodel_add(&whitemodel, tagx, tagy, v);
+ else
+ graymodel_add(&blackmodel, tagx, tagy, v);
+ }
+ }
+
+ graymodel_solve(&whitemodel);
+ graymodel_solve(&blackmodel);
+
+ // XXX Tunable
+ if (graymodel_interpolate(&whitemodel, 0, 0) - graymodel_interpolate(&blackmodel, 0, 0) < 0)
+ return -1;
+
+ // compute the average decision margin (how far was each bit from
+ // the decision boundary?
+ //
+ // we score this separately for white and black pixels and return
+ // the minimum average threshold for black/white pixels. This is
+ // to penalize thresholds that are too close to an extreme.
+ float black_score = 0, white_score = 0;
+ float black_score_count = 1, white_score_count = 1;
+
+ for (int bitidx = 0; bitidx < family->d * family->d; bitidx++) {
+ int bitx = bitidx % family->d;
+ int bity = bitidx / family->d;
+
+ double tagx01 = (family->black_border + bitx + 0.5) / (2*family->black_border + family->d);
+ double tagy01 = (family->black_border + bity + 0.5) / (2*family->black_border + family->d);
+
+ // scale to [-1, 1]
+ double tagx = 2*(tagx01-0.5);
+ double tagy = 2*(tagy01-0.5);
+
+ double px, py;
+ homography_project(quad->H, tagx, tagy, &px, &py);
+
+ rcode = (rcode << 1);
+
+ // don't round.
+ int ix = px;
+ int iy = py;
+
+ if (ix < 0 || iy < 0 || ix >= im->width || iy >= im->height)
+ continue;
+
+ int v = im->buf[iy*im->stride + ix];
+
+ double thresh = (graymodel_interpolate(&blackmodel, tagx, tagy) + graymodel_interpolate(&whitemodel, tagx, tagy)) / 2.0;
+ if (v > thresh) {
+ white_score += (v - thresh);
+ white_score_count ++;
+ rcode |= 1;
+ } else {
+ black_score += (thresh - v);
+ black_score_count ++;
+ }
+
+ if (im_samples)
+ im_samples->buf[iy*im_samples->stride + ix] = (1 - (rcode & 1)) * 255;
+ }
+
+ quick_decode_codeword(family, rcode, entry);
+
+ return fmin(white_score / white_score_count, black_score / black_score_count);
+}
+
+double score_goodness(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user)
+{
+ return quad_goodness(family, im, quad);
+}
+
+double score_decodability(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user)
+{
+ struct quick_decode_entry entry;
+
+ float decision_margin = quad_decode(family, im, quad, &entry, NULL);
+
+ // hamming trumps decision margin; maximum value for decision_margin is 255.
+ return decision_margin - entry.hamming*1000;
+}
+
+// returns score of best quad
+double optimize_quad_generic(apriltag_family_t *family, image_u8_t *im, struct quad *quad0,
+ float *stepsizes, int nstepsizes,
+ double (*score)(apriltag_family_t *family, image_u8_t *im, struct quad *quad, void *user),
+ void *user)
+{
+ struct quad *best_quad = quad_copy(quad0);
+ double best_score = score(family, im, best_quad, user);
+
+ for (int stepsize_idx = 0; stepsize_idx < nstepsizes; stepsize_idx++) {
+
+ int improved = 1;
+
+ // when we make progress with a particular step size, how many
+ // times will we try to perform that same step size again?
+ // (max_repeat = 0 means ("don't repeat--- just move to the
+ // next step size").
+ // XXX Tunable
+ int max_repeat = 1;
+
+ for (int repeat = 0; repeat <= max_repeat && improved; repeat++) {
+
+ improved = 0;
+
+ // wiggle point i
+ for (int i = 0; i < 4; i++) {
+
+ float stepsize = stepsizes[stepsize_idx];
+
+ // XXX Tunable (really 1 makes the best sense since)
+ int nsteps = 1;
+
+ struct quad *this_best_quad = NULL;
+ double this_best_score = best_score;
+
+ for (int sx = -nsteps; sx <= nsteps; sx++) {
+ for (int sy = -nsteps; sy <= nsteps; sy++) {
+ if (sx==0 && sy==0)
+ continue;
+
+ struct quad *this_quad = quad_copy(best_quad);
+ this_quad->p[i][0] = best_quad->p[i][0] + sx*stepsize;
+ this_quad->p[i][1] = best_quad->p[i][1] + sy*stepsize;
+ if (quad_update_homographies(this_quad))
+ continue;
+
+ double this_score = score(family, im, this_quad, user);
+
+ if (this_score > this_best_score) {
+ quad_destroy(this_best_quad);
+
+ this_best_quad = this_quad;
+ this_best_score = this_score;
+ } else {
+ quad_destroy(this_quad);
+ }
+ }
+ }
+
+ if (this_best_score > best_score) {
+ quad_destroy(best_quad);
+ best_quad = this_best_quad;
+ best_score = this_best_score;
+ improved = 1;
+ }
+ }
+ }
+ }
+
+ matd_destroy(quad0->H);
+ matd_destroy(quad0->Hinv);
+ memcpy(quad0, best_quad, sizeof(struct quad)); // copy pointers
+ free(best_quad);
+ return best_score;
+}
+
+static void refine_edges(apriltag_detector_t *td, image_u8_t *im_orig, struct quad *quad)
+{
+ double lines[4][4]; // for each line, [Ex Ey nx ny]
+
+ for (int edge = 0; edge < 4; edge++) {
+ int a = edge, b = (edge + 1) & 3; // indices of the end points.
+
+ // compute the normal to the current line estimate
+ double nx = quad->p[b][1] - quad->p[a][1];
+ double ny = -quad->p[b][0] + quad->p[a][0];
+ double mag = sqrt(nx*nx + ny*ny);
+ nx /= mag;
+ ny /= mag;
+
+ // we will now fit a NEW line by sampling points near
+ // our original line that have large gradients. On really big tags,
+ // we're willing to sample more to get an even better estimate.
+ int nsamples = imax(16, mag / 8); // XXX tunable
+
+ // stats for fitting a line...
+ double Mx = 0, My = 0, Mxx = 0, Mxy = 0, Myy = 0, N = 0;
+
+ for (int s = 0; s < nsamples; s++) {
+ // compute a point along the line... Note, we're avoiding
+ // sampling *right* at the corners, since those points are
+ // the least reliable.
+ double alpha = (1.0 + s) / (nsamples + 1);
+ double x0 = alpha*quad->p[a][0] + (1-alpha)*quad->p[b][0];
+ double y0 = alpha*quad->p[a][1] + (1-alpha)*quad->p[b][1];
+
+ // search along the normal to this line, looking at the
+ // gradients along the way. We're looking for a strong
+ // response.
+ double Mn = 0;
+ double Mcount = 0;
+
+ // XXX tunable: how far to search? We want to search far
+ // enough that we find the best edge, but not so far that
+ // we hit other edges that aren't part of the tag. We
+ // shouldn't ever have to search more than quad_decimate,
+ // since otherwise we would (ideally) have started our
+ // search on another pixel in the first place. Likewise,
+ // for very small tags, we don't want the range to be too
+ // big.
+ double range = td->quad_decimate + 1;
+
+ // XXX tunable step size.
+ for (double n = -range; n <= range; n += 0.25) {
+ // Because of the guaranteed winding order of the
+ // points in the quad, we will start inside the white
+ // portion of the quad and work our way outward.
+ //
+ // sample to points (x1,y1) and (x2,y2) XXX tunable:
+ // how far +/- to look? Small values compute the
+ // gradient more precisely, but are more sensitive to
+ // noise.
+ double grange = 1;
+ int x1 = x0 + (n + grange)*nx;
+ int y1 = y0 + (n + grange)*ny;
+ if (x1 < 0 || x1 >= im_orig->width || y1 < 0 || y1 >= im_orig->height)
+ continue;
+
+ int x2 = x0 + (n - grange)*nx;
+ int y2 = y0 + (n - grange)*ny;
+ if (x2 < 0 || x2 >= im_orig->width || y2 < 0 || y2 >= im_orig->height)
+ continue;
+
+ int g1 = im_orig->buf[y1*im_orig->stride + x1];
+ int g2 = im_orig->buf[y2*im_orig->stride + x2];
+
+ if (g1 < g2) // reject points whose gradient is "backwards". They can only hurt us.
+ continue;
+
+ double weight = (g2 - g1)*(g2 - g1); // XXX tunable. What shape for weight=f(g2-g1)?
+
+ // compute weighted average of the gradient at this point.
+ Mn += weight*n;
+ Mcount += weight;
+ }
+
+ // what was the average point along the line?
+ if (Mcount == 0)
+ continue;
+
+ double n0 = Mn / Mcount;
+
+ // where is the point along the line?
+ double bestx = x0 + n0*nx;
+ double besty = y0 + n0*ny;
+
+ // update our line fit statistics
+ Mx += bestx;
+ My += besty;
+ Mxx += bestx*bestx;
+ Mxy += bestx*besty;
+ Myy += besty*besty;
+ N++;
+ }
+
+ // fit a line
+ double Ex = Mx / N, Ey = My / N;
+ double Cxx = Mxx / N - Ex*Ex;
+ double Cxy = Mxy / N - Ex*Ey;
+ double Cyy = Myy / N - Ey*Ey;
+
+ double normal_theta = .5 * atan2f(-2*Cxy, (Cyy - Cxx));
+ nx = cosf(normal_theta);
+ ny = sinf(normal_theta);
+ lines[edge][0] = Ex;
+ lines[edge][1] = Ey;
+ lines[edge][2] = nx;
+ lines[edge][3] = ny;
+ }
+
+ // now refit the corners of the quad
+ for (int i = 0; i < 4; i++) {
+
+ // solve for the intersection of lines (i) and (i+1)&3.
+ double A00 = lines[i][3], A01 = -lines[(i+1)&3][3];
+ double A10 = -lines[i][2], A11 = lines[(i+1)&3][2];
+ double B0 = -lines[i][0] + lines[(i+1)&3][0];
+ double B1 = -lines[i][1] + lines[(i+1)&3][1];
+
+ double det = A00 * A11 - A10 * A01;
+
+ // inverse.
+ if (fabs(det) > 0.001) {
+ // solve
+ double W00 = A11 / det, W01 = -A01 / det;
+
+ double L0 = W00*B0 + W01*B1;
+
+ // compute intersection
+ quad->p[i][0] = lines[i][0] + L0*A00;
+ quad->p[i][1] = lines[i][1] + L0*A10;
+ } else {
+ // this is a bad sign. We'll just keep the corner we had.
+// printf("bad det: %15f %15f %15f %15f %15f\n", A00, A11, A10, A01, det);
+ }
+ }
+}
+
+static void quad_decode_task(void *_u)
+{
+ struct quad_decode_task *task = (struct quad_decode_task*) _u;
+ apriltag_detector_t *td = task->td;
+ image_u8_t *im = task->im;
+
+ for (int quadidx = task->i0; quadidx < task->i1; quadidx++) {
+ struct quad *quad_original;
+ zarray_get_volatile(task->quads, quadidx, &quad_original);
+
+ // refine edges is not dependent upon the tag family, thus
+ // apply this optimization BEFORE the other work.
+ //if (td->quad_decimate > 1 && td->refine_edges) {
+ if (td->refine_edges) {
+ refine_edges(td, im, quad_original);
+ }
+
+ // make sure the homographies are computed...
+ if (quad_update_homographies(quad_original))
+ continue;
+
+ for (int famidx = 0; famidx < zarray_size(td->tag_families); famidx++) {
+ apriltag_family_t *family;
+ zarray_get(td->tag_families, famidx, &family);
+
+ double goodness = 0;
+
+ // since the geometry of tag families can vary, start any
+ // optimization process over with the original quad.
+ struct quad *quad = quad_copy(quad_original);
+
+ // improve the quad corner positions by minimizing the
+ // variance within each intra-bit area.
+ if (td->refine_pose) {
+ // NB: We potentially step an integer
+ // number of times in each direction. To make each
+ // sample as useful as possible, the step sizes should
+ // not be integer multiples of each other. (I.e.,
+ // probably don't use 1, 0.5, 0.25, etc.)
+
+ // XXX Tunable
+ float stepsizes[] = { 1, .4, .16, .064 };
+ int nstepsizes = sizeof(stepsizes)/sizeof(float);
+
+ goodness = optimize_quad_generic(family, im, quad, stepsizes, nstepsizes, score_goodness, NULL);
+ }
+
+ if (td->refine_decode) {
+ // this optimizes decodability, but we don't report
+ // that value to the user. (so discard return value.)
+ // XXX Tunable
+ float stepsizes[] = { .4 };
+ int nstepsizes = sizeof(stepsizes)/sizeof(float);
+
+ optimize_quad_generic(family, im, quad, stepsizes, nstepsizes, score_decodability, NULL);
+ }
+
+ struct quick_decode_entry entry;
+
+ float decision_margin = quad_decode(family, im, quad, &entry, task->im_samples);
+
+ if (entry.hamming < 255 && decision_margin >= 0) {
+ apriltag_detection_t *det = calloc(1, sizeof(apriltag_detection_t));
+
+ det->family = family;
+ det->id = entry.id;
+ det->hamming = entry.hamming;
+ det->goodness = goodness;
+ det->decision_margin = decision_margin;
+
+ double theta = -entry.rotation * M_PI / 2.0;
+ double c = cos(theta), s = sin(theta);
+
+ // Fix the rotation of our homography to properly orient the tag
+ matd_t *R = matd_create(3,3);
+ MATD_EL(R, 0, 0) = c;
+ MATD_EL(R, 0, 1) = -s;
+ MATD_EL(R, 1, 0) = s;
+ MATD_EL(R, 1, 1) = c;
+ MATD_EL(R, 2, 2) = 1;
+
+ det->H = matd_op("M*M", quad->H, R);
+
+ matd_destroy(R);
+
+ homography_project(det->H, 0, 0, &det->c[0], &det->c[1]);
+
+ // [-1, -1], [1, -1], [1, 1], [-1, 1], Desired points
+ // [-1, 1], [1, 1], [1, -1], [-1, -1], FLIP Y
+ // adjust the points in det->p so that they correspond to
+ // counter-clockwise around the quad, starting at -1,-1.
+ for (int i = 0; i < 4; i++) {
+ int tcx = (i == 1 || i == 2) ? 1 : -1;
+ int tcy = (i < 2) ? 1 : -1;
+
+ double p[2];
+
+ homography_project(det->H, tcx, tcy, &p[0], &p[1]);
+
+ det->p[i][0] = p[0];
+ det->p[i][1] = p[1];
+ }
+
+ pthread_mutex_lock(&td->mutex);
+ zarray_add(task->detections, &det);
+ pthread_mutex_unlock(&td->mutex);
+ }
+
+ quad_destroy(quad);
+ }
+ }
+}
+
+void apriltag_detection_destroy(apriltag_detection_t *det)
+{
+ if (det == NULL)
+ return;
+
+ matd_destroy(det->H);
+ free(det);
+}
+
+int prefer_smaller(int pref, double q0, double q1)
+{
+ if (pref) // already prefer something? exit.
+ return pref;
+
+ if (q0 < q1)
+ return -1; // we now prefer q0
+ if (q1 < q0)
+ return 1; // we now prefer q1
+
+ // no preference
+ return 0;
+}
+
+zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig)
+{
+ if (zarray_size(td->tag_families) == 0) {
+ zarray_t *s = zarray_create(sizeof(apriltag_detection_t*));
+ printf("apriltag.c: No tag families enabled.");
+ return s;
+ }
+
+ if (td->wp == NULL || td->nthreads != workerpool_get_nthreads(td->wp)) {
+ workerpool_destroy(td->wp);
+ td->wp = workerpool_create(td->nthreads);
+ }
+
+ timeprofile_clear(td->tp);
+ timeprofile_stamp(td->tp, "init");
+
+ ///////////////////////////////////////////////////////////
+ // Step 1. Detect quads according to requested image decimation
+ // and blurring parameters.
+ image_u8_t *quad_im = im_orig;
+ if (td->quad_decimate > 1) {
+ quad_im = image_u8_decimate(im_orig, td->quad_decimate);
+
+ timeprofile_stamp(td->tp, "decimate");
+ }
+
+ if (td->quad_sigma != 0) {
+ // compute a reasonable kernel width by figuring that the
+ // kernel should go out 2 std devs.
+ //
+ // max sigma ksz
+ // 0.499 1 (disabled)
+ // 0.999 3
+ // 1.499 5
+ // 1.999 7
+
+ float sigma = fabsf((float) td->quad_sigma);
+
+ int ksz = 4 * sigma; // 2 std devs in each direction
+ if ((ksz & 1) == 0)
+ ksz++;
+
+ if (ksz > 1) {
+
+ if (td->quad_sigma > 0) {
+ // Apply a blur
+ image_u8_gaussian_blur(quad_im, sigma, ksz);
+ } else {
+ // SHARPEN the image by subtracting the low frequency components.
+ image_u8_t *orig = image_u8_copy(quad_im);
+ image_u8_gaussian_blur(quad_im, sigma, ksz);
+
+ for (int y = 0; y < orig->height; y++) {
+ for (int x = 0; x < orig->width; x++) {
+ int vorig = orig->buf[y*orig->stride + x];
+ int vblur = quad_im->buf[y*quad_im->stride + x];
+
+ int v = 2*vorig - vblur;
+ if (v < 0)
+ v = 0;
+ if (v > 255)
+ v = 255;
+
+ quad_im->buf[y*quad_im->stride + x] = (uint8_t) v;
+ }
+ }
+ image_u8_destroy(orig);
+ }
+ }
+ }
+
+ timeprofile_stamp(td->tp, "blur/sharp");
+
+ if (td->debug)
+ image_u8_write_pnm(quad_im, "debug_preprocess.pnm");
+
+// zarray_t *quads = apriltag_quad_gradient(td, im_orig);
+ zarray_t *quads = apriltag_quad_thresh(td, quad_im);
+
+ // adjust centers of pixels so that they correspond to the
+ // original full-resolution image.
+ if (td->quad_decimate > 1) {
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *q;
+ zarray_get_volatile(quads, i, &q);
+
+ for (int i = 0; i < 4; i++) {
+ q->p[i][0] *= td->quad_decimate;
+ q->p[i][1] *= td->quad_decimate;
+ }
+ }
+ }
+
+ if (quad_im != im_orig)
+ image_u8_destroy(quad_im);
+
+ zarray_t *detections = zarray_create(sizeof(apriltag_detection_t*));
+
+ td->nquads = zarray_size(quads);
+
+ timeprofile_stamp(td->tp, "quads");
+
+ if (td->debug) {
+ image_u8_t *im_quads = image_u8_copy(im_orig);
+ image_u8_darken(im_quads);
+ image_u8_darken(im_quads);
+
+ srandom(0);
+
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *quad;
+ zarray_get_volatile(quads, i, &quad);
+
+ const int bias = 100;
+ int color = bias + (random() % (255-bias));
+
+ image_u8_draw_line(im_quads, quad->p[0][0], quad->p[0][1], quad->p[1][0], quad->p[1][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[1][0], quad->p[1][1], quad->p[2][0], quad->p[2][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[2][0], quad->p[2][1], quad->p[3][0], quad->p[3][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[3][0], quad->p[3][1], quad->p[0][0], quad->p[0][1], color, 1);
+ }
+
+ image_u8_write_pnm(im_quads, "debug_quads_raw.pnm");
+ image_u8_destroy(im_quads);
+ }
+
+ ////////////////////////////////////////////////////////////////
+ // Step 2. Decode tags from each quad.
+ if (1) {
+ image_u8_t *im_samples = td->debug ? image_u8_copy(im_orig) : NULL;
+
+ int chunksize = 1 + zarray_size(quads) / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
+
+#ifdef _MSC_VER
+ struct quad_decode_task *tasks = malloc((zarray_size(quads) / chunksize + 1)*sizeof *tasks);
+#else
+ struct quad_decode_task tasks[zarray_size(quads) / chunksize + 1];
+#endif
+
+ int ntasks = 0;
+ for (int i = 0; i < zarray_size(quads); i+= chunksize) {
+ tasks[ntasks].i0 = i;
+ tasks[ntasks].i1 = imin(zarray_size(quads), i + chunksize);
+ tasks[ntasks].quads = quads;
+ tasks[ntasks].td = td;
+ tasks[ntasks].im = im_orig;
+ tasks[ntasks].detections = detections;
+
+ tasks[ntasks].im_samples = im_samples;
+
+ workerpool_add_task(td->wp, quad_decode_task, &tasks[ntasks]);
+ ntasks++;
+ }
+
+ workerpool_run(td->wp);
+
+#ifdef _MSC_VER
+ free(tasks);
+#endif
+
+ if (im_samples != NULL) {
+ image_u8_write_pnm(im_samples, "debug_samples.pnm");
+ image_u8_destroy(im_samples);
+ }
+ }
+
+ if (td->debug) {
+ image_u8_t *im_quads = image_u8_copy(im_orig);
+ image_u8_darken(im_quads);
+ image_u8_darken(im_quads);
+
+ srandom(0);
+
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *quad;
+ zarray_get_volatile(quads, i, &quad);
+
+ const int bias = 100;
+ int color = bias + (random() % (255-bias));
+
+ image_u8_draw_line(im_quads, quad->p[0][0], quad->p[0][1], quad->p[1][0], quad->p[1][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[1][0], quad->p[1][1], quad->p[2][0], quad->p[2][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[2][0], quad->p[2][1], quad->p[3][0], quad->p[3][1], color, 1);
+ image_u8_draw_line(im_quads, quad->p[3][0], quad->p[3][1], quad->p[0][0], quad->p[0][1], color, 1);
+
+ }
+
+ image_u8_write_pnm(im_quads, "debug_quads_fixed.pnm");
+ image_u8_destroy(im_quads);
+ }
+
+ timeprofile_stamp(td->tp, "decode+refinement");
+
+ ////////////////////////////////////////////////////////////////
+ // Step 3. Reconcile detections--- don't report the same tag more
+ // than once. (Allow non-overlapping duplicate detections.)
+ if (1) {
+ zarray_t *poly0 = g2d_polygon_create_zeros(4);
+ zarray_t *poly1 = g2d_polygon_create_zeros(4);
+
+ for (int i0 = 0; i0 < zarray_size(detections); i0++) {
+
+ apriltag_detection_t *det0;
+ zarray_get(detections, i0, &det0);
+
+ for (int k = 0; k < 4; k++)
+ zarray_set(poly0, k, det0->p[k], NULL);
+
+ for (int i1 = i0+1; i1 < zarray_size(detections); i1++) {
+
+ apriltag_detection_t *det1;
+ zarray_get(detections, i1, &det1);
+
+ if (det0->id != det1->id || det0->family != det1->family)
+ continue;
+
+ for (int k = 0; k < 4; k++)
+ zarray_set(poly1, k, det1->p[k], NULL);
+
+ if (g2d_polygon_overlaps_polygon(poly0, poly1)) {
+ // the tags overlap. Delete one, keep the other.
+
+ int pref = 0; // 0 means undecided which one we'll keep.
+ pref = prefer_smaller(pref, det0->hamming, det1->hamming); // want small hamming
+ pref = prefer_smaller(pref, -det0->decision_margin, -det1->decision_margin); // want bigger margins
+ pref = prefer_smaller(pref, -det0->goodness, -det1->goodness); // want bigger goodness
+
+ // if we STILL don't prefer one detection over the other, then pick
+ // any deterministic criterion.
+ for (int i = 0; i < 4; i++) {
+ pref = prefer_smaller(pref, det0->p[i][0], det1->p[i][0]);
+ pref = prefer_smaller(pref, det0->p[i][1], det1->p[i][1]);
+ }
+
+ if (pref == 0) {
+ // at this point, we should only be undecided if the tag detections
+ // are *exactly* the same. How would that happen?
+ printf("uh oh, no preference for overlappingdetection\n");
+ }
+
+ if (pref < 0) {
+ // keep det0, destroy det1
+ apriltag_detection_destroy(det1);
+ zarray_remove_index(detections, i1, 1);
+ i1--; // retry the same index
+ goto retry1;
+ } else {
+ // keep det1, destroy det0
+ apriltag_detection_destroy(det0);
+ zarray_remove_index(detections, i0, 1);
+ i0--; // retry the same index.
+ goto retry0;
+ }
+ }
+
+ retry1: ;
+ }
+
+ retry0: ;
+ }
+
+ zarray_destroy(poly0);
+ zarray_destroy(poly1);
+ }
+
+ timeprofile_stamp(td->tp, "reconcile");
+
+ ////////////////////////////////////////////////////////////////
+ // Produce final debug output
+ if (td->debug) {
+
+ image_u8_t *darker = image_u8_copy(im_orig);
+ image_u8_darken(darker);
+ image_u8_darken(darker);
+
+ // assume letter, which is 612x792 points.
+ FILE *f = fopen("debug_output.ps", "w");
+ fprintf(f, "%%!PS\n\n");
+ double scale = fmin(612.0/darker->width, 792.0/darker->height);
+ fprintf(f, "%f %f scale\n", scale, scale);
+ fprintf(f, "0 %d translate\n", darker->height);
+ fprintf(f, "1 -1 scale\n");
+ postscript_image(f, darker);
+
+ image_u8_destroy(darker);
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ float rgb[3];
+ int bias = 100;
+
+ for (int i = 0; i < 3; i++)
+ rgb[i] = bias + (random() % (255-bias));
+
+ fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
+ fprintf(f, "%f %f moveto %f %f lineto %f %f lineto %f %f lineto %f %f lineto stroke\n",
+ det->p[0][0], det->p[0][1],
+ det->p[1][0], det->p[1][1],
+ det->p[2][0], det->p[2][1],
+ det->p[3][0], det->p[3][1],
+ det->p[0][0], det->p[0][1]);
+ }
+
+ fprintf(f, "showpage\n");
+ fclose(f);
+ }
+
+ if (td->debug) {
+ image_u8_t *darker = image_u8_copy(im_orig);
+ image_u8_darken(darker);
+ image_u8_darken(darker);
+
+ image_u8x3_t *out = image_u8x3_create(darker->width, darker->height);
+ for (int y = 0; y < im_orig->height; y++) {
+ for (int x = 0; x < im_orig->width; x++) {
+ out->buf[y*out->stride + 3*x + 0] = darker->buf[y*darker->stride + x];
+ out->buf[y*out->stride + 3*x + 1] = darker->buf[y*darker->stride + x];
+ out->buf[y*out->stride + 3*x + 2] = darker->buf[y*darker->stride + x];
+ }
+ }
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ float rgb[3];
+ int bias = 100;
+
+ for (int i = 0; i < 3; i++)
+ rgb[i] = bias + (random() % (255-bias));
+
+ for (int j = 0; j < 4; j++) {
+ int k = (j + 1) & 3;
+ image_u8x3_draw_line(out,
+ det->p[j][0], det->p[j][1], det->p[k][0], det->p[k][1],
+ (uint8_t[]) { rgb[0], rgb[1], rgb[2] },
+ 1);
+ }
+ }
+
+ image_u8x3_write_pnm(out, "debug_output.pnm");
+ image_u8x3_destroy(out);
+ }
+
+ // deallocate
+ if (td->debug) {
+ FILE *f = fopen("debug_quads.ps", "w");
+ fprintf(f, "%%!PS\n\n");
+
+ image_u8_t *darker = image_u8_copy(im_orig);
+ image_u8_darken(darker);
+ image_u8_darken(darker);
+
+ // assume letter, which is 612x792 points.
+ double scale = fmin(612.0/darker->width, 792.0/darker->height);
+ fprintf(f, "%f %f scale\n", scale, scale);
+ fprintf(f, "0 %d translate\n", darker->height);
+ fprintf(f, "1 -1 scale\n");
+
+ postscript_image(f, darker);
+
+ image_u8_destroy(darker);
+
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *q;
+ zarray_get_volatile(quads, i, &q);
+
+ float rgb[3];
+ int bias = 100;
+
+ for (int i = 0; i < 3; i++)
+ rgb[i] = bias + (random() % (255-bias));
+
+ fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
+ fprintf(f, "%f %f moveto %f %f lineto %f %f lineto %f %f lineto %f %f lineto stroke\n",
+ q->p[0][0], q->p[0][1],
+ q->p[1][0], q->p[1][1],
+ q->p[2][0], q->p[2][1],
+ q->p[3][0], q->p[3][1],
+ q->p[0][0], q->p[0][1]);
+ }
+
+ fprintf(f, "showpage\n");
+ fclose(f);
+ }
+
+ timeprofile_stamp(td->tp, "debug output");
+
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *quad;
+ zarray_get_volatile(quads, i, &quad);
+ matd_destroy(quad->H);
+ matd_destroy(quad->Hinv);
+ }
+
+ zarray_destroy(quads);
+
+ zarray_sort(detections, detection_compare_function);
+ timeprofile_stamp(td->tp, "cleanup");
+
+ return detections;
+}
+
+
+// Call this method on each of the tags returned by apriltag_detector_detect
+void apriltag_detections_destroy(zarray_t *detections)
+{
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ apriltag_detection_destroy(det);
+ }
+
+ zarray_destroy(detections);
+}
+
+image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx)
+{
+ assert(fam != NULL);
+ assert(idx >= 0 && idx < fam->ncodes);
+
+ uint64_t code = fam->codes[idx];
+ int border = fam->black_border + 1;
+ int dim = fam->d + 2*border;
+ image_u8_t *im = image_u8_create(dim, dim);
+
+ // Make 1px white border
+ for (int i = 0; i < dim; i += 1) {
+ im->buf[i] = 255;
+ im->buf[(dim-1)*im->stride + i] = 255;
+ im->buf[i*im->stride] = 255;
+ im->buf[i*im->stride + (dim-1)] = 255;
+ }
+
+ for (int y = 0; y < fam->d; y += 1) {
+ for (int x = 0; x < fam->d; x += 1) {
+ int pos = (fam->d-1 - y) * fam->d + (fam->d-1 - x);
+ if ((code >> pos) & 0x1) {
+ int i = (y+border)*im->stride + x+border;
+ im->buf[i] = 255;
+ }
+ }
+ }
+
+ return im;
+}
diff --git a/3rdparty/apriltag/apriltag.h b/3rdparty/apriltag/apriltag.h
new file mode 100644
index 0000000..c699b29
--- /dev/null
+++ b/3rdparty/apriltag/apriltag.h
@@ -0,0 +1,293 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _APRILTAG_H
+#define _APRILTAG_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdlib.h>
+
+#include "common/matd.h"
+#include "common/image_u8.h"
+#include "common/zarray.h"
+#include "common/workerpool.h"
+#include "common/timeprofile.h"
+#include <pthread.h>
+
+#define APRILTAG_TASKS_PER_THREAD_TARGET 10
+
+struct quad
+{
+ float p[4][2]; // corners
+
+ // H: tag coordinates ([-1,1] at the black corners) to pixels
+ // Hinv: pixels to tag
+ matd_t *H, *Hinv;
+};
+
+// Represents a tag family. Every tag belongs to a tag family. Tag
+// families are generated by the Java tool
+// april.tag.TagFamilyGenerator and can be converted to C using
+// april.tag.TagToC.
+typedef struct apriltag_family apriltag_family_t;
+struct apriltag_family
+{
+ // How many codes are there in this tag family?
+ uint32_t ncodes;
+
+ // The codes in the family.
+ uint64_t *codes;
+
+ // how wide (in bit-sizes) is the black border? (usually 1)
+ uint32_t black_border;
+
+ // how many bits tall and wide is it? (e.g. 36bit tag ==> 6)
+ uint32_t d;
+
+ // minimum hamming distance between any two codes. (e.g. 36h11 => 11)
+ uint32_t h;
+
+ // a human-readable name, e.g., "tag36h11"
+ char *name;
+
+ // some detector implementations may preprocess codes in order to
+ // accelerate decoding. They put their data here. (Do not use the
+ // same apriltag_family instance in more than one implementation)
+ void *impl;
+};
+
+
+struct apriltag_quad_thresh_params
+{
+ // reject quads containing too few pixels
+ int min_cluster_pixels;
+
+ // how many corner candidates to consider when segmenting a group
+ // of pixels into a quad.
+ int max_nmaxima;
+
+ // Reject quads where pairs of edges have angles that are close to
+ // straight or close to 180 degrees. Zero means that no quads are
+ // rejected. (In radians).
+ float critical_rad;
+
+ // When fitting lines to the contours, what is the maximum mean
+ // squared error allowed? This is useful in rejecting contours
+ // that are far from being quad shaped; rejecting these quads "early"
+ // saves expensive decoding processing.
+ float max_line_fit_mse;
+
+ // When we build our model of black & white pixels, we add an
+ // extra check that the white model must be (overall) brighter
+ // than the black model. How much brighter? (in pixel values,
+ // [0,255]). .
+ int min_white_black_diff;
+
+ // should the thresholded image be deglitched? Only useful for
+ // very noisy images
+ int deglitch;
+};
+
+// Represents a detector object. Upon creating a detector, all fields
+// are set to reasonable values, but can be overridden by accessing
+// these fields.
+typedef struct apriltag_detector apriltag_detector_t;
+struct apriltag_detector
+{
+ ///////////////////////////////////////////////////////////////
+ // User-configurable parameters.
+
+ // How many threads should be used?
+ int nthreads;
+
+ // detection of quads can be done on a lower-resolution image,
+ // improving speed at a cost of pose accuracy and a slight
+ // decrease in detection rate. Decoding the binary payload is
+ // still done at full resolution.
+ float quad_decimate;
+
+ // What Gaussian blur should be applied to the segmented image
+ // (used for quad detection?) Parameter is the standard deviation
+ // in pixels. Very noisy images benefit from non-zero values
+ // (e.g. 0.8).
+ float quad_sigma;
+
+ // When non-zero, the edges of the each quad are adjusted to "snap
+ // to" strong gradients nearby. This is useful when decimation is
+ // employed, as it can increase the quality of the initial quad
+ // estimate substantially. Generally recommended to be on (1).
+ //
+ // Very computationally inexpensive. Option is ignored if
+ // quad_decimate = 1.
+ int refine_edges;
+
+ // when non-zero, detections are refined in a way intended to
+ // increase the number of detected tags. Especially effective for
+ // very small tags near the resolution threshold (e.g. 10px on a
+ // side).
+ int refine_decode;
+
+ // when non-zero, detections are refined in a way intended to
+ // increase the accuracy of the extracted pose. This is done by
+ // maximizing the contrast around the black and white border of
+ // the tag. This generally increases the number of successfully
+ // detected tags, though not as effectively (or quickly) as
+ // refine_decode.
+ //
+ // This option must be enabled in order for "goodness" to be
+ // computed.
+ int refine_pose;
+
+ // When non-zero, write a variety of debugging images to the
+ // current working directory at various stages through the
+ // detection process. (Somewhat slow).
+ int debug;
+
+ struct apriltag_quad_thresh_params qtp;
+
+ ///////////////////////////////////////////////////////////////
+ // Statistics relating to last processed frame
+ timeprofile_t *tp;
+
+ uint32_t nedges;
+ uint32_t nsegments;
+ uint32_t nquads;
+
+ ///////////////////////////////////////////////////////////////
+ // Internal variables below
+
+ // Not freed on apriltag_destroy; a tag family can be shared
+ // between multiple users. The user should ultimately destroy the
+ // tag family passed into the constructor.
+ zarray_t *tag_families;
+
+ // Used to manage multi-threading.
+ workerpool_t *wp;
+
+ // Used for thread safety.
+ pthread_mutex_t mutex;
+};
+
+// Represents the detection of a tag. These are returned to the user
+// and must be individually destroyed by the user.
+typedef struct apriltag_detection apriltag_detection_t;
+struct apriltag_detection
+{
+ // a pointer for convenience. not freed by apriltag_detection_destroy.
+ apriltag_family_t *family;
+
+ // The decoded ID of the tag
+ int id;
+
+ // How many error bits were corrected? Note: accepting large numbers of
+ // corrected errors leads to greatly increased false positive rates.
+ // NOTE: As of this implementation, the detector cannot detect tags with
+ // a hamming distance greater than 2.
+ int hamming;
+
+ // A measure of the quality of tag localization: measures the
+ // average contrast of the pixels around the border of the
+ // tag. refine_pose must be enabled, or this field will be zero.
+ float goodness;
+
+ // A measure of the quality of the binary decoding process: the
+ // average difference between the intensity of a data bit versus
+ // the decision threshold. Higher numbers roughly indicate better
+ // decodes. This is a reasonable measure of detection accuracy
+ // only for very small tags-- not effective for larger tags (where
+ // we could have sampled anywhere within a bit cell and still
+ // gotten a good detection.)
+ float decision_margin;
+
+ // The 3x3 homography matrix describing the projection from an
+ // "ideal" tag (with corners at (-1,-1), (1,-1), (1,1), and (-1,
+ // 1)) to pixels in the image. This matrix will be freed by
+ // apriltag_detection_destroy.
+ matd_t *H;
+
+ // The center of the detection in image pixel coordinates.
+ double c[2];
+
+ // The corners of the tag in image pixel coordinates. These always
+ // wrap counter-clock wise around the tag.
+ double p[4][2];
+};
+
+// don't forget to add a family!
+apriltag_detector_t *apriltag_detector_create();
+
+// add a family to the apriltag detector. caller still "owns" the family.
+// a single instance should only be provided to one apriltag detector instance.
+void apriltag_detector_add_family_bits(apriltag_detector_t *td, apriltag_family_t *fam, int bits_corrected);
+
+// Tunable, but really, 2 is a good choice. Values of >=3
+// consume prohibitively large amounts of memory, and otherwise
+// you want the largest value possible.
+static inline void apriltag_detector_add_family(apriltag_detector_t *td, apriltag_family_t *fam)
+{
+ apriltag_detector_add_family_bits(td, fam, 2);
+}
+
+// does not deallocate the family.
+void apriltag_detector_remove_family(apriltag_detector_t *td, apriltag_family_t *fam);
+
+// unregister all families, but does not deallocate the underlying tag family objects.
+void apriltag_detector_clear_families(apriltag_detector_t *td);
+
+// Destroy the april tag detector (but not the underlying
+// apriltag_family_t used to initialize it.)
+void apriltag_detector_destroy(apriltag_detector_t *td);
+
+// Detect tags from an image and return an array of
+// apriltag_detection_t*. You can use apriltag_detections_destroy to
+// free the array and the detections it contains, or call
+// _detection_destroy and zarray_destroy yourself.
+zarray_t *apriltag_detector_detect(apriltag_detector_t *td, image_u8_t *im_orig);
+
+// Call this method on each of the tags returned by apriltag_detector_detect
+void apriltag_detection_destroy(apriltag_detection_t *det);
+
+// destroys the array AND the detections within it.
+void apriltag_detections_destroy(zarray_t *detections);
+
+// Renders the apriltag with with 1px white border.
+// Caller is responsible for calling image_u8_destroy on the image
+image_u8_t *apriltag_to_image(apriltag_family_t *fam, int idx);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/apriltag_math.h b/3rdparty/apriltag/apriltag_math.h
new file mode 100644
index 0000000..49d8a08
--- /dev/null
+++ b/3rdparty/apriltag/apriltag_math.h
@@ -0,0 +1,109 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _APRILTAG_MATH_H
+#define _APRILTAG_MATH_H
+
+#include <math.h>
+
+// Computes the cholesky factorization of A, putting the lower
+// triangular matrix into R.
+static inline void mat33_chol(const double *A,
+ double *R)
+{
+ // A[0] = R[0]*R[0]
+ R[0] = sqrt(A[0]);
+
+ // A[1] = R[0]*R[3];
+ R[3] = A[1] / R[0];
+
+ // A[2] = R[0]*R[6];
+ R[6] = A[2] / R[0];
+
+ // A[4] = R[3]*R[3] + R[4]*R[4]
+ R[4] = sqrt(A[4] - R[3]*R[3]);
+
+ // A[5] = R[3]*R[6] + R[4]*R[7]
+ R[7] = (A[5] - R[3]*R[6]) / R[4];
+
+ // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8]
+ R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]);
+
+ R[1] = 0;
+ R[2] = 0;
+ R[5] = 0;
+}
+
+static inline void mat33_lower_tri_inv(const double *A,
+ double *R)
+{
+ // A[0]*R[0] = 1
+ R[0] = 1 / A[0];
+
+ // A[3]*R[0] + A[4]*R[3] = 0
+ R[3] = -A[3]*R[0] / A[4];
+
+ // A[4]*R[4] = 1
+ R[4] = 1 / A[4];
+
+ // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0
+ R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8];
+
+ // A[7]*R[4] + A[8]*R[7] = 0
+ R[7] = -A[7]*R[4] / A[8];
+
+ // A[8]*R[8] = 1
+ R[8] = 1 / A[8];
+}
+
+
+static inline void mat33_sym_solve(const double *A,
+ const double *B,
+ double *R)
+{
+ double L[9];
+ mat33_chol(A, L);
+
+ double M[9];
+ mat33_lower_tri_inv(L, M);
+
+ double tmp[3];
+ tmp[0] = M[0]*B[0];
+ tmp[1] = M[3]*B[0] + M[4]*B[1];
+ tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2];
+
+ R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2];
+ R[1] = M[4]*tmp[1] + M[7]*tmp[2];
+ R[2] = M[8]*tmp[2];
+}
+
+#endif
diff --git a/3rdparty/apriltag/apriltag_quad_thresh.c b/3rdparty/apriltag/apriltag_quad_thresh.c
new file mode 100644
index 0000000..2329e32
--- /dev/null
+++ b/3rdparty/apriltag/apriltag_quad_thresh.c
@@ -0,0 +1,1916 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+// limitation: image size must be <32768 in width and height. This is
+// because we use a fixed-point 16 bit integer representation with one
+// fractional bit.
+#include <math.h>
+#include <assert.h>
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+
+#include "apriltag.h"
+#include "common/image_u8x3.h"
+#include "common/zarray.h"
+#include "common/zhash.h"
+#include "common/unionfind.h"
+#include "common/timeprofile.h"
+#include "common/zmaxheap.h"
+#include "common/postscript_utils.h"
+#include "common/math_util.h"
+
+#if defined(__MINGW32__) || defined(_MSC_VER)
+#define random rand
+#endif
+
+static inline uint32_t u64hash_2(uint64_t x) {
+ return (2654435761 * x) >> 32;
+ return (uint32_t) x;
+}
+
+struct uint64_zarray_entry
+{
+ uint64_t id;
+ zarray_t *cluster;
+
+ struct uint64_zarray_entry *next;
+};
+
+#ifndef M_PI
+# define M_PI 3.141592653589793238462643383279502884196
+#endif
+
+struct pt
+{
+ // Note: these represent 2*actual value.
+ uint16_t x, y;
+ float theta;
+ int16_t gx, gy;
+};
+
+struct unionfind_task
+{
+ int y0, y1;
+ int w, h, s;
+ unionfind_t *uf;
+ image_u8_t *im;
+};
+
+struct quad_task
+{
+ zarray_t *clusters;
+ int cidx0, cidx1; // [cidx0, cidx1)
+ zarray_t *quads;
+ apriltag_detector_t *td;
+ int w, h;
+
+ image_u8_t *im;
+};
+
+struct remove_vertex
+{
+ int i; // which vertex to remove?
+ int left, right; // left vertex, right vertex
+
+ double err;
+};
+
+struct segment
+{
+ int is_vertex;
+
+ // always greater than zero, but right can be > size, which denotes
+ // a wrap around back to the beginning of the points. and left < right.
+ int left, right;
+};
+
+struct line_fit_pt
+{
+ double Mx, My;
+ double Mxx, Myy, Mxy;
+ double W; // total weight
+};
+
+static inline void ptsort(struct pt *pts, int sz)
+{
+#define MAYBE_SWAP(arr,apos,bpos) \
+ if (arr[apos].theta > arr[bpos].theta) { \
+ tmp = arr[apos]; arr[apos] = arr[bpos]; arr[bpos] = tmp; \
+ };
+
+ if (sz <= 1)
+ return;
+
+ if (sz == 2) {
+ struct pt tmp;
+ MAYBE_SWAP(pts, 0, 1);
+ return;
+ }
+
+ // NB: Using less-branch-intensive sorting networks here on the
+ // hunch that it's better for performance.
+ if (sz == 3) { // 3 element bubble sort is optimal
+ struct pt tmp;
+ MAYBE_SWAP(pts, 0, 1);
+ MAYBE_SWAP(pts, 1, 2);
+ MAYBE_SWAP(pts, 0, 1);
+ return;
+ }
+
+ if (sz == 4) { // 4 element optimal sorting network.
+ struct pt tmp;
+ MAYBE_SWAP(pts, 0, 1); // sort each half, like a merge sort
+ MAYBE_SWAP(pts, 2, 3);
+ MAYBE_SWAP(pts, 0, 2); // minimum value is now at 0.
+ MAYBE_SWAP(pts, 1, 3); // maximum value is now at end.
+ MAYBE_SWAP(pts, 1, 2); // that only leaves the middle two.
+ return;
+ }
+
+ if (sz == 5) {
+ // this 9-step swap is optimal for a sorting network, but two
+ // steps slower than a generic sort.
+ struct pt tmp;
+ MAYBE_SWAP(pts, 0, 1); // sort each half (3+2), like a merge sort
+ MAYBE_SWAP(pts, 3, 4);
+ MAYBE_SWAP(pts, 1, 2);
+ MAYBE_SWAP(pts, 0, 1);
+ MAYBE_SWAP(pts, 0, 3); // minimum element now at 0
+ MAYBE_SWAP(pts, 2, 4); // maximum element now at end
+ MAYBE_SWAP(pts, 1, 2); // now resort the three elements 1-3.
+ MAYBE_SWAP(pts, 2, 3);
+ MAYBE_SWAP(pts, 1, 2);
+ return;
+ }
+
+#undef MAYBE_SWAP
+
+ // a merge sort with temp storage.
+
+ // Use stack storage if it's not too big.
+ int stacksz = sz;
+ if (stacksz > 1024)
+ stacksz = 0;
+
+#ifdef _MSC_VER
+ struct pt *_tmp_stack = malloc(stacksz*sizeof *_tmp_stack);
+#else
+ struct pt _tmp_stack[stacksz];
+#endif
+ struct pt *tmp = _tmp_stack;
+
+ if (stacksz == 0) {
+ // it was too big, malloc it instead.
+ tmp = malloc(sizeof(struct pt) * sz);
+ }
+
+ memcpy(tmp, pts, sizeof(struct pt) * sz);
+
+ int asz = sz/2;
+ int bsz = sz - asz;
+
+ struct pt *as = &tmp[0];
+ struct pt *bs = &tmp[asz];
+
+ ptsort(as, asz);
+ ptsort(bs, bsz);
+
+#define MERGE(apos,bpos) \
+ if (as[apos].theta < bs[bpos].theta) \
+ pts[outpos++] = as[apos++]; \
+ else \
+ pts[outpos++] = bs[bpos++];
+
+ int apos = 0, bpos = 0, outpos = 0;
+ while (apos + 8 < asz && bpos + 8 < bsz) {
+ MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos);
+ MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos); MERGE(apos,bpos);
+ }
+
+ while (apos < asz && bpos < bsz) {
+ MERGE(apos,bpos);
+ }
+
+ if (apos < asz)
+ memcpy(&pts[outpos], &as[apos], (asz-apos)*sizeof(struct pt));
+ if (bpos < bsz)
+ memcpy(&pts[outpos], &bs[bpos], (bsz-bpos)*sizeof(struct pt));
+
+ if (stacksz == 0)
+ free(tmp);
+
+#undef MERGE
+
+#ifdef _MSC_VER
+ free(_tmp_stack);
+#endif
+}
+
+// lfps contains *cumulative* moments for N points, with
+// index j reflecting points [0,j] (inclusive).
+//
+// fit a line to the points [i0, i1] (inclusive). i0, i1 are both [0,
+// sz) if i1 < i0, we treat this as a wrap around.
+void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm, double *err, double *mse)
+{
+ assert(i0 != i1);
+ assert(i0 >= 0 && i1 >= 0 && i0 < sz && i1 < sz);
+
+ double Mx, My, Mxx, Myy, Mxy, W;
+ int N; // how many points are included in the set?
+
+ if (i0 < i1) {
+ N = i1 - i0 + 1;
+
+ Mx = lfps[i1].Mx;
+ My = lfps[i1].My;
+ Mxx = lfps[i1].Mxx;
+ Mxy = lfps[i1].Mxy;
+ Myy = lfps[i1].Myy;
+ W = lfps[i1].W;
+
+ if (i0 > 0) {
+ Mx -= lfps[i0-1].Mx;
+ My -= lfps[i0-1].My;
+ Mxx -= lfps[i0-1].Mxx;
+ Mxy -= lfps[i0-1].Mxy;
+ Myy -= lfps[i0-1].Myy;
+ W -= lfps[i0-1].W;
+ }
+
+ } else {
+ // i0 > i1, e.g. [15, 2]. Wrap around.
+ assert(i0 > 0);
+
+ Mx = lfps[sz-1].Mx - lfps[i0-1].Mx;
+ My = lfps[sz-1].My - lfps[i0-1].My;
+ Mxx = lfps[sz-1].Mxx - lfps[i0-1].Mxx;
+ Mxy = lfps[sz-1].Mxy - lfps[i0-1].Mxy;
+ Myy = lfps[sz-1].Myy - lfps[i0-1].Myy;
+ W = lfps[sz-1].W - lfps[i0-1].W;
+
+ Mx += lfps[i1].Mx;
+ My += lfps[i1].My;
+ Mxx += lfps[i1].Mxx;
+ Mxy += lfps[i1].Mxy;
+ Myy += lfps[i1].Myy;
+ W += lfps[i1].W;
+
+ N = sz - i0 + i1 + 1;
+ }
+
+ assert(N >= 2);
+
+ double Ex = Mx / W;
+ double Ey = My / W;
+ double Cxx = Mxx / W - Ex*Ex;
+ double Cxy = Mxy / W - Ex*Ey;
+ double Cyy = Myy / W - Ey*Ey;
+
+ double nx, ny;
+
+ if (1) {
+ // on iOS about 5% of total CPU spent in these trig functions.
+ // 85 ms per frame on 5S, example.pnm
+ //
+ // XXX this was using the double-precision atan2. Was there a case where
+ // we needed that precision? Seems doubtful.
+ double normal_theta = .5 * atan2(-2*Cxy, (Cyy - Cxx));
+ //double normal_theta = .5 * atan2f(-2 * Cxy, (Cyy - Cxx));
+ nx = cos(normal_theta);
+ ny = sin(normal_theta);
+ //nx = cosf(normal_theta);
+ //ny = sinf(normal_theta);
+ } else {
+ // 73.5 ms per frame on 5S, example.pnm
+ double ty = -2*Cxy;
+ double tx = (Cyy - Cxx);
+ double mag = ty*ty + tx*tx;
+
+ if (mag == 0) {
+ nx = 1;
+ ny = 0;
+ } else {
+ double norm = sqrtf(ty*ty + tx*tx);
+ tx /= norm;
+
+ // ty is now sin(2theta)
+ // tx is now cos(2theta). We want sin(theta) and cos(theta)
+
+ // due to precision err, tx could still have slightly too large magnitude.
+ if (tx > 1) {
+ ny = 0;
+ nx = 1;
+ } else if (tx < -1) {
+ ny = 1;
+ nx = 0;
+ } else {
+ // half angle formula
+ ny = sqrtf((1 - tx)/2);
+ nx = sqrtf((1 + tx)/2);
+
+ // pick a consistent branch cut
+ if (ty < 0)
+ ny = - ny;
+ }
+ }
+ }
+
+ if (lineparm) {
+ lineparm[0] = Ex;
+ lineparm[1] = Ey;
+ lineparm[2] = nx;
+ lineparm[3] = ny;
+ }
+
+ // sum of squared errors =
+ //
+ // SUM_i ((p_x - ux)*nx + (p_y - uy)*ny)^2
+ // SUM_i nx*nx*(p_x - ux)^2 + 2nx*ny(p_x -ux)(p_y-uy) + ny*ny*(p_y-uy)*(p_y-uy)
+ // nx*nx*SUM_i((p_x -ux)^2) + 2nx*ny*SUM_i((p_x-ux)(p_y-uy)) + ny*ny*SUM_i((p_y-uy)^2)
+ //
+ // nx*nx*N*Cxx + 2nx*ny*N*Cxy + ny*ny*N*Cyy
+
+ // sum of squared errors
+ if (err)
+ *err = nx*nx*N*Cxx + 2*nx*ny*N*Cxy + ny*ny*N*Cyy;
+
+ // mean squared error
+ if (mse)
+ *mse = nx*nx*Cxx + 2*nx*ny*Cxy + ny*ny*Cyy;
+}
+
+int pt_compare_theta(const void *_a, const void *_b)
+{
+ struct pt *a = (struct pt*) _a;
+ struct pt *b = (struct pt*) _b;
+
+ return (a->theta < b->theta) ? -1 : 1;
+}
+
+int err_compare_descending(const void *_a, const void *_b)
+{
+ const double *a = _a;
+ const double *b = _b;
+
+ return ((*a) < (*b)) ? 1 : -1;
+}
+
+/*
+
+ 1. Identify A) white points near a black point and B) black points near a white point.
+
+ 2. Find the connected components within each of the classes above,
+ yielding clusters of "white-near-black" and
+ "black-near-white". (These two classes are kept separate). Each
+ segment has a unique id.
+
+ 3. For every pair of "white-near-black" and "black-near-white"
+ clusters, find the set of points that are in one and adjacent to the
+ other. In other words, a "boundary" layer between the two
+ clusters. (This is actually performed by iterating over the pixels,
+ rather than pairs of clusters.) Critically, this helps keep nearby
+ edges from becoming connected.
+*/
+int quad_segment_maxima(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
+{
+ int sz = zarray_size(cluster);
+
+ // ksz: when fitting points, how many points on either side do we consider?
+ // (actual "kernel" width is 2ksz).
+ //
+ // This value should be about: 0.5 * (points along shortest edge).
+ //
+ // If all edges were equally-sized, that would give a value of
+ // sz/8. We make it somewhat smaller to account for tags at high
+ // aspects.
+
+ // XXX Tunable. Maybe make a multiple of JPEG block size to increase robustness
+ // to JPEG compression artifacts?
+ int ksz = imin(20, sz / 12);
+
+ // can't fit a quad if there are too few points.
+ if (ksz < 2)
+ return 0;
+
+// printf("sz %5d, ksz %3d\n", sz, ksz);
+
+#ifdef _MSC_VER
+ double *errs = malloc(sz*sizeof *errs);
+#else
+ double errs[sz];
+#endif
+
+ for (int i = 0; i < sz; i++) {
+ fit_line(lfps, sz, (i + sz - ksz) % sz, (i + ksz) % sz, NULL, &errs[i], NULL);
+ }
+
+ // apply a low-pass filter to errs
+ if (1) {
+#ifdef _MSC_VER
+ double *y = malloc(sz*sizeof *y);
+#else
+ double y[sz];
+#endif
+
+ // how much filter to apply?
+
+ // XXX Tunable
+ double sigma = 1; // was 3
+
+ // cutoff = exp(-j*j/(2*sigma*sigma));
+ // log(cutoff) = -j*j / (2*sigma*sigma)
+ // log(cutoff)*2*sigma*sigma = -j*j;
+
+ // how big a filter should we use? We make our kernel big
+ // enough such that we represent any values larger than
+ // 'cutoff'.
+
+ // XXX Tunable (though not super useful to change)
+ double cutoff = 0.05;
+ int fsz = sqrt(-log(cutoff)*2*sigma*sigma) + 1;
+ fsz = 2*fsz + 1;
+
+ // For default values of cutoff = 0.05, sigma = 3,
+ // we have fsz = 17.
+#ifdef _MSC_VER
+ float *f = malloc(fsz*sizeof *f);
+#else
+ float f[fsz];
+#endif
+
+ for (int i = 0; i < fsz; i++) {
+ int j = i - fsz / 2;
+ f[i] = exp(-j*j/(2*sigma*sigma));
+ }
+
+ for (int iy = 0; iy < sz; iy++) {
+ double acc = 0;
+
+ for (int i = 0; i < fsz; i++) {
+ acc += errs[(iy + i - fsz / 2 + sz) % sz] * f[i];
+ }
+ y[iy] = acc;
+ }
+
+#ifdef _MSC_VER
+ memcpy(errs, y, sz * sizeof *y);
+#else
+ memcpy(errs, y, sizeof(y));
+#endif
+
+#ifdef _MSC_VER
+ free(y);
+ free(f);
+#endif
+ }
+
+#ifdef _MSC_VER
+ int *maxima = malloc(sz*sizeof *maxima);
+ double *maxima_errs = malloc(sz*sizeof *maxima_errs);
+#else
+ int maxima[sz];
+ double maxima_errs[sz];
+#endif
+ int nmaxima = 0;
+
+ for (int i = 0; i < sz; i++) {
+ if (errs[i] > errs[(i+1)%sz] && errs[i] > errs[(i+sz-1)%sz]) {
+ maxima[nmaxima] = i;
+ maxima_errs[nmaxima] = errs[i];
+ nmaxima++;
+ }
+ }
+
+ // if we didn't get at least 4 maxima, we can't fit a quad.
+ if (nmaxima < 4) {
+#ifdef _MSC_VER
+ free(errs);
+ free(maxima);
+ free(maxima_errs);
+#endif
+ return 0;
+ }
+
+ // select only the best maxima if we have too many
+ int max_nmaxima = td->qtp.max_nmaxima;
+
+ if (nmaxima > max_nmaxima) {
+#ifdef _MSC_VER
+ double *maxima_errs_copy = malloc(nmaxima*sizeof *maxima_errs_copy);
+ memcpy(maxima_errs_copy, maxima_errs, nmaxima * sizeof *maxima_errs_copy);
+#else
+ double maxima_errs_copy[nmaxima];
+ memcpy(maxima_errs_copy, maxima_errs, sizeof(maxima_errs_copy));
+#endif
+
+ // throw out all but the best handful of maxima. Sorts descending.
+ qsort(maxima_errs_copy, nmaxima, sizeof(double), err_compare_descending);
+
+ double maxima_thresh = maxima_errs_copy[max_nmaxima];
+ int out = 0;
+ for (int in = 0; in < nmaxima; in++) {
+ if (maxima_errs[in] <= maxima_thresh)
+ continue;
+ maxima[out++] = maxima[in];
+ }
+ nmaxima = out;
+
+#ifdef _MSC_VER
+ free(maxima_errs_copy);
+#endif
+ }
+
+ int best_indices[4];
+ double best_error = HUGE_VAL; // HUGE_VALF;
+
+ double err01, err12, err23, err30;
+ double mse01, mse12, mse23, mse30;
+ double params01[4], params12[4], params23[4], params30[4];
+
+ // disallow quads where the angle is less than a critical value.
+ double max_dot = cos(td->qtp.critical_rad); //25*M_PI/180);
+
+ for (int m0 = 0; m0 < nmaxima - 3; m0++) {
+ int i0 = maxima[m0];
+
+ for (int m1 = m0+1; m1 < nmaxima - 2; m1++) {
+ int i1 = maxima[m1];
+ fit_line(lfps, sz, i0, i1, params01, &err01, &mse01);
+
+ if (mse01 > td->qtp.max_line_fit_mse)
+ continue;
+
+ for (int m2 = m1+1; m2 < nmaxima - 1; m2++) {
+ int i2 = maxima[m2];
+
+ fit_line(lfps, sz, i1, i2, params12, &err12, &mse12);
+ if (mse12 > td->qtp.max_line_fit_mse)
+ continue;
+
+ double dot = params01[2]*params12[2] + params01[3]*params12[3];
+ if (fabs(dot) > max_dot)
+ continue;
+
+ for (int m3 = m2+1; m3 < nmaxima; m3++) {
+ int i3 = maxima[m3];
+
+ fit_line(lfps, sz, i2, i3, params23, &err23, &mse23);
+ if (mse23 > td->qtp.max_line_fit_mse)
+ continue;
+
+ fit_line(lfps, sz, i3, i0, params30, &err30, &mse30);
+ if (mse30 > td->qtp.max_line_fit_mse)
+ continue;
+
+ double err = err01 + err12 + err23 + err30;
+ if (err < best_error) {
+ best_error = err;
+ best_indices[0] = i0;
+ best_indices[1] = i1;
+ best_indices[2] = i2;
+ best_indices[3] = i3;
+ }
+ }
+ }
+ }
+ }
+
+#ifdef _MSC_VER
+ free(errs);
+ free(maxima);
+ free(maxima_errs);
+#endif
+
+ if (best_error == HUGE_VAL /*HUGE_VALF*/)
+ return 0;
+
+ for (int i = 0; i < 4; i++)
+ indices[i] = best_indices[i];
+
+ if (best_error / sz < td->qtp.max_line_fit_mse)
+ return 1;
+ return 0;
+}
+
+// returns 0 if the cluster looks bad.
+int quad_segment_agg(apriltag_detector_t *td, zarray_t *cluster, struct line_fit_pt *lfps, int indices[4])
+{
+ int sz = zarray_size(cluster);
+
+ zmaxheap_t *heap = zmaxheap_create(sizeof(struct remove_vertex*));
+
+ // We will initially allocate sz rvs. We then have two types of
+ // iterations: some iterations that are no-ops in terms of
+ // allocations, and those that remove a vertex and allocate two
+ // more children. This will happen at most (sz-4) times. Thus we
+ // need: sz + 2*(sz-4) entries.
+
+ int rvalloc_pos = 0;
+ int rvalloc_size = 3*sz;
+ struct remove_vertex *rvalloc = calloc(rvalloc_size, sizeof(struct remove_vertex));
+
+ struct segment *segs = calloc(sz, sizeof(struct segment));
+
+ // populate with initial entries
+ for (int i = 0; i < sz; i++) {
+ struct remove_vertex *rv = &rvalloc[rvalloc_pos++];
+ rv->i = i;
+ if (i == 0) {
+ rv->left = sz-1;
+ rv->right = 1;
+ } else {
+ rv->left = i-1;
+ rv->right = (i+1) % sz;
+ }
+
+ fit_line(lfps, sz, rv->left, rv->right, NULL, NULL, &rv->err);
+
+ zmaxheap_add(heap, &rv, -rv->err);
+
+ segs[i].left = rv->left;
+ segs[i].right = rv->right;
+ segs[i].is_vertex = 1;
+ }
+
+ int nvertices = sz;
+
+ while (nvertices > 4) {
+ assert(rvalloc_pos < rvalloc_size);
+
+ struct remove_vertex *rv;
+ float err;
+
+ int res = zmaxheap_remove_max(heap, &rv, &err);
+ if (!res)
+ return 0;
+ assert(res);
+
+ // is this remove_vertex valid? (Or has one of the left/right
+ // vertices changes since we last looked?)
+ if (!segs[rv->i].is_vertex ||
+ !segs[rv->left].is_vertex ||
+ !segs[rv->right].is_vertex) {
+ continue;
+ }
+
+ // we now merge.
+ assert(segs[rv->i].is_vertex);
+
+ segs[rv->i].is_vertex = 0;
+ segs[rv->left].right = rv->right;
+ segs[rv->right].left = rv->left;
+
+ // create the join to the left
+ if (1) {
+ struct remove_vertex *child = &rvalloc[rvalloc_pos++];
+ child->i = rv->left;
+ child->left = segs[rv->left].left;
+ child->right = rv->right;
+
+ fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err);
+
+ zmaxheap_add(heap, &child, -child->err);
+ }
+
+ // create the join to the right
+ if (1) {
+ struct remove_vertex *child = &rvalloc[rvalloc_pos++];
+ child->i = rv->right;
+ child->left = rv->left;
+ child->right = segs[rv->right].right;
+
+ fit_line(lfps, sz, child->left, child->right, NULL, NULL, &child->err);
+
+ zmaxheap_add(heap, &child, -child->err);
+ }
+
+ // we now have one less vertex
+ nvertices--;
+ }
+
+ free(rvalloc);
+ zmaxheap_destroy(heap);
+
+ int idx = 0;
+ for (int i = 0; i < sz; i++) {
+ if (segs[i].is_vertex) {
+ indices[idx++] = i;
+ }
+ }
+
+ free(segs);
+
+ return 1;
+}
+
+// return 1 if the quad looks okay, 0 if it should be discarded
+int fit_quad(apriltag_detector_t *td, image_u8_t *im, zarray_t *cluster, struct quad *quad)
+{
+ int res = 0;
+
+ int sz = zarray_size(cluster);
+ if (sz < 4) // can't fit a quad to less than 4 points
+ return 0;
+
+ /////////////////////////////////////////////////////////////
+ // Step 1. Sort points so they wrap around the center of the
+ // quad. We will constrain our quad fit to simply partition this
+ // ordered set into 4 groups.
+
+ // compute a bounding box so that we can order the points
+ // according to their angle WRT the center.
+ int32_t xmax = 0, xmin = INT32_MAX, ymax = 0, ymin = INT32_MAX;
+
+ for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, pidx, &p);
+
+ xmax = imax(xmax, p->x);
+ xmin = imin(xmin, p->x);
+
+ ymax = imax(ymax, p->y);
+ ymin = imin(ymin, p->y);
+ }
+
+ // add some noise to (cx,cy) so that pixels get a more diverse set
+ // of theta estimates. This will help us remove more points.
+ // (Only helps a small amount. The actual noise values here don't
+ // matter much at all, but we want them [-1, 1]. (XXX with
+ // fixed-point, should range be bigger?)
+ double cx = (xmin + xmax) * 0.5 + 0.05118;
+ double cy = (ymin + ymax) * 0.5 + -0.028581;
+
+ double dot = 0;
+
+ for (int pidx = 0; pidx < zarray_size(cluster); pidx++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, pidx, &p);
+
+ double dx = p->x - cx;
+ double dy = p->y - cy;
+
+ p->theta = atan2f(dy, dx);
+
+ dot += dx*p->gx + dy*p->gy;
+// p->theta = terrible_atan2(dy, dx);
+ }
+
+ // Ensure that the black border is inside the white border.
+ if (dot < 0)
+ return 0;
+
+ // we now sort the points according to theta. This is a prepatory
+ // step for segmenting them into four lines.
+ if (1) {
+ // zarray_sort(cluster, pt_compare_theta);
+ ptsort((struct pt*) cluster->data, zarray_size(cluster));
+
+ // remove duplicate points. (A byproduct of our segmentation system.)
+ if (1) {
+ int outpos = 1;
+
+ struct pt *last;
+ zarray_get_volatile(cluster, 0, &last);
+
+ for (int i = 1; i < sz; i++) {
+
+ struct pt *p;
+ zarray_get_volatile(cluster, i, &p);
+
+ if (p->x != last->x || p->y != last->y) {
+
+ if (i != outpos) {
+ struct pt *out;
+ zarray_get_volatile(cluster, outpos, &out);
+ memcpy(out, p, sizeof(struct pt));
+ }
+
+ outpos++;
+ }
+
+ last = p;
+ }
+
+ cluster->size = outpos;
+ sz = outpos;
+ }
+
+ } else {
+ // This is a counting sort in which we retain at most one
+ // point for every bucket; the bucket index is computed from
+ // theta. Since a good quad completes a complete revolution,
+ // there's reason to think that we should get a good
+ // distribution of thetas. We might "lose" a few points due
+ // to collisions, but this shouldn't affect quality very much.
+
+ // XXX tunable. Increase to reduce the likelihood of "losing"
+ // points due to collisions.
+ int nbuckets = 4*sz;
+
+#define ASSOC 2
+#ifdef _MSC_VER
+ struct pt **v = malloc(nbuckets * sizeof *v);
+ for (int i = 0; i < nbuckets; i++) {
+ v[i] = malloc(ASSOC*sizeof *v[i]);
+ for (int j = 0; j < ASSOC; j++) {
+ v[i][j].gx = 0;
+ v[i][j].gy = 0;
+ v[i][j].theta = 0;
+ v[i][j].x = 0;
+ v[i][j].y = 0;
+ }
+ }
+#else
+ struct pt v[nbuckets][ASSOC];
+ memset(v, 0, sizeof(v));
+#endif
+
+ // put each point into a bucket.
+ for (int i = 0; i < sz; i++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, i, &p);
+
+ assert(p->theta >= -M_PI && p->theta <= M_PI);
+
+ int bucket = (nbuckets - 1) * (p->theta + M_PI) / (2*M_PI);
+ assert(bucket >= 0 && bucket < nbuckets);
+
+ for (int i = 0; i < ASSOC; i++) {
+ if (v[bucket][i].theta == 0) {
+ v[bucket][i] = *p;
+ break;
+ }
+ }
+ }
+
+ // collect the points from the buckets and put them back into the array.
+ int outsz = 0;
+ for (int i = 0; i < nbuckets; i++) {
+ for (int j = 0; j < ASSOC; j++) {
+ if (v[i][j].theta != 0) {
+ zarray_set(cluster, outsz, &v[i][j], NULL);
+ outsz++;
+ }
+ }
+ }
+
+ zarray_truncate(cluster, outsz);
+ sz = outsz;
+
+#ifdef _MSC_VER
+ for (int i = 0; i < nbuckets; i++) {
+ free(v[i]);
+ }
+ free(v);
+#endif
+ }
+
+ if (sz < 4)
+ return 0;
+
+ /////////////////////////////////////////////////////////////
+ // Step 2. Precompute statistics that allow line fit queries to be
+ // efficiently computed for any contiguous range of indices.
+
+ struct line_fit_pt *lfps = calloc(sz, sizeof(struct line_fit_pt));
+
+ for (int i = 0; i < sz; i++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, i, &p);
+
+ if (i > 0) {
+ memcpy(&lfps[i], &lfps[i-1], sizeof(struct line_fit_pt));
+ }
+
+ if (0) {
+ // we now undo our fixed-point arithmetic.
+ double delta = 0.5;
+ double x = p->x * .5 + delta;
+ double y = p->y * .5 + delta;
+ double W;
+
+ for (int dy = -1; dy <= 1; dy++) {
+ int iy = y + dy;
+
+ if (iy < 0 || iy + 1 >= im->height)
+ continue;
+
+ for (int dx = -1; dx <= 1; dx++) {
+ int ix = x + dx;
+
+ if (ix < 0 || ix + 1 >= im->width)
+ continue;
+
+ int grad_x = im->buf[iy * im->stride + ix + 1] -
+ im->buf[iy * im->stride + ix - 1];
+
+ int grad_y = im->buf[(iy+1) * im->stride + ix] -
+ im->buf[(iy-1) * im->stride + ix];
+
+ W = sqrtf(grad_x*grad_x + grad_y*grad_y) + 1;
+
+// double fx = x + dx, fy = y + dy;
+ double fx = ix + .5, fy = iy + .5;
+ lfps[i].Mx += W * fx;
+ lfps[i].My += W * fy;
+ lfps[i].Mxx += W * fx * fx;
+ lfps[i].Mxy += W * fx * fy;
+ lfps[i].Myy += W * fy * fy;
+ lfps[i].W += W;
+ }
+ }
+ } else {
+ // we now undo our fixed-point arithmetic.
+ double delta = 0.5; // adjust for pixel center bias
+ double x = p->x * .5 + delta;
+ double y = p->y * .5 + delta;
+ int ix = x, iy = y;
+ double W = 1;
+
+ if (ix > 0 && ix+1 < im->width && iy > 0 && iy+1 < im->height) {
+ int grad_x = im->buf[iy * im->stride + ix + 1] -
+ im->buf[iy * im->stride + ix - 1];
+
+ int grad_y = im->buf[(iy+1) * im->stride + ix] -
+ im->buf[(iy-1) * im->stride + ix];
+
+ // XXX Tunable. How to shape the gradient magnitude?
+ W = sqrt(grad_x*grad_x + grad_y*grad_y) + 1;
+ }
+
+ double fx = x, fy = y;
+ lfps[i].Mx += W * fx;
+ lfps[i].My += W * fy;
+ lfps[i].Mxx += W * fx * fx;
+ lfps[i].Mxy += W * fx * fy;
+ lfps[i].Myy += W * fy * fy;
+ lfps[i].W += W;
+ }
+ }
+
+ int indices[4];
+ if (1) {
+ if (!quad_segment_maxima(td, cluster, lfps, indices))
+ goto finish;
+ } else {
+ if (!quad_segment_agg(td, cluster, lfps, indices))
+ goto finish;
+ }
+
+// printf("%d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
+
+ if (0) {
+ // no refitting here; just use those points as the vertices.
+ // Note, this is useful for debugging, but pretty bad in
+ // practice since this code path also omits several
+ // plausibility checks that save us tons of time in quad
+ // decoding.
+ for (int i = 0; i < 4; i++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, indices[i], &p);
+
+ quad->p[i][0] = .5*p->x; // undo fixed-point arith.
+ quad->p[i][1] = .5*p->y;
+ }
+
+ res = 1;
+
+ } else {
+ double lines[4][4];
+
+ for (int i = 0; i < 4; i++) {
+ int i0 = indices[i];
+ int i1 = indices[(i+1)&3];
+
+ if (0) {
+ // if there are enough points, skip the points near the corners
+ // (because those tend not to be very good.)
+ if (i1-i0 > 8) {
+ int t = (i1-i0)/6;
+ if (t < 0)
+ t = -t;
+
+ i0 = (i0 + t) % sz;
+ i1 = (i1 + sz - t) % sz;
+ }
+ }
+
+ double err;
+ fit_line(lfps, sz, i0, i1, lines[i], NULL, &err);
+
+ if (err > td->qtp.max_line_fit_mse) {
+ res = 0;
+ goto finish;
+ }
+ }
+
+ for (int i = 0; i < 4; i++) {
+ // solve for the intersection of lines (i) and (i+1)&3.
+ // p0 + lambda0*u0 = p1 + lambda1*u1, where u0 and u1
+ // are the line directions.
+ //
+ // lambda0*u0 - lambda1*u1 = (p1 - p0)
+ //
+ // rearrange (solve for lambdas)
+ //
+ // [u0_x -u1_x ] [lambda0] = [ p1_x - p0_x ]
+ // [u0_y -u1_y ] [lambda1] [ p1_y - p0_y ]
+ //
+ // remember that lines[i][0,1] = p, lines[i][2,3] = NORMAL vector.
+ // We want the unit vector, so we need the perpendiculars. Thus, below
+ // we have swapped the x and y components and flipped the y components.
+
+ double A00 = lines[i][3], A01 = -lines[(i+1)&3][3];
+ double A10 = -lines[i][2], A11 = lines[(i+1)&3][2];
+ double B0 = -lines[i][0] + lines[(i+1)&3][0];
+ double B1 = -lines[i][1] + lines[(i+1)&3][1];
+
+ double det = A00 * A11 - A10 * A01;
+
+ // inverse.
+ double W00 = A11 / det, W01 = -A01 / det;
+ if (fabs(det) < 0.001) {
+ res = 0;
+ goto finish;
+ }
+
+ // solve
+ double L0 = W00*B0 + W01*B1;
+
+ // compute intersection
+ quad->p[i][0] = lines[i][0] + L0*A00;
+ quad->p[i][1] = lines[i][1] + L0*A10;
+
+ if (0) {
+ // we should get the same intersection starting
+ // from point p1 and moving L1*u1.
+ double W10 = -A10 / det, W11 = A00 / det;
+ double L1 = W10*B0 + W11*B1;
+
+ double x = lines[(i+1)&3][0] - L1*A10;
+ double y = lines[(i+1)&3][1] - L1*A11;
+ assert(fabs(x - quad->p[i][0]) < 0.001 &&
+ fabs(y - quad->p[i][1]) < 0.001);
+ }
+
+ res = 1;
+ }
+ }
+
+ // reject quads that are too small
+ if (1) {
+ double area = 0;
+
+ // get area of triangle formed by points 0, 1, 2, 0
+ double length[3], p;
+ for (int i = 0; i < 3; i++) {
+ int idxa = i; // 0, 1, 2,
+ int idxb = (i+1) % 3; // 1, 2, 0
+ length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) +
+ sq(quad->p[idxb][1] - quad->p[idxa][1]));
+ }
+ p = (length[0] + length[1] + length[2]) / 2;
+
+ area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2]));
+
+ // get area of triangle formed by points 2, 3, 0, 2
+ for (int i = 0; i < 3; i++) {
+ int idxs[] = { 2, 3, 0, 2 };
+ int idxa = idxs[i];
+ int idxb = idxs[i+1];
+ length[i] = sqrt(sq(quad->p[idxb][0] - quad->p[idxa][0]) +
+ sq(quad->p[idxb][1] - quad->p[idxa][1]));
+ }
+ p = (length[0] + length[1] + length[2]) / 2;
+
+ area += sqrt(p*(p-length[0])*(p-length[1])*(p-length[2]));
+
+ // we don't actually know the family yet (quad detection is generic.)
+ // This threshold is based on a 6x6 tag (which is actually 8x8)
+// int d = fam->d + fam->black_border*2;
+ int d = 8;
+ if (area < d*d) {
+ res = 0;
+ goto finish;
+ }
+ }
+
+ // reject quads whose cumulative angle change isn't equal to 2PI
+ if (1) {
+ double total = 0;
+
+ for (int i = 0; i < 4; i++) {
+ int i0 = i, i1 = (i+1)&3, i2 = (i+2)&3;
+
+ double theta0 = atan2f(quad->p[i0][1] - quad->p[i1][1],
+ quad->p[i0][0] - quad->p[i1][0]);
+ double theta1 = atan2f(quad->p[i2][1] - quad->p[i1][1],
+ quad->p[i2][0] - quad->p[i1][0]);
+
+ double dtheta = theta0 - theta1;
+ if (dtheta < 0)
+ dtheta += 2*M_PI;
+
+ if (dtheta < td->qtp.critical_rad || dtheta > (M_PI - td->qtp.critical_rad))
+ res = 0;
+
+ total += dtheta;
+ }
+
+ // looking for 2PI
+ if (total < 6.2 || total > 6.4) {
+ res = 0;
+ goto finish;
+ }
+ }
+
+ // adjust pixel coordinates; all math up 'til now uses pixel
+ // coordinates in which (0,0) is the lower left corner. But each
+ // pixel actually spans from to [x, x+1), [y, y+1) the mean value of which
+ // is +.5 higher than x & y.
+/* double delta = .5;
+ for (int i = 0; i < 4; i++) {
+ quad->p[i][0] += delta;
+ quad->p[i][1] += delta;
+ }
+*/
+ finish:
+
+ free(lfps);
+
+ return res;
+}
+
+#define DO_UNIONFIND(dx, dy) if (im->buf[y*s + dy*s + x + dx] == v) unionfind_connect(uf, y*w + x, y*w + dy*w + x + dx);
+
+static void do_unionfind_line(unionfind_t *uf, image_u8_t *im, int h, int w, int s, int y)
+{
+ assert(y+1 < im->height);
+
+ for (int x = 1; x < w - 1; x++) {
+ uint8_t v = im->buf[y*s + x];
+
+ if (v == 127)
+ continue;
+
+ // (dx,dy) pairs for 8 connectivity:
+ // (REFERENCE) (1, 0)
+ // (-1, 1) (0, 1) (1, 1)
+ //
+ DO_UNIONFIND(1, 0);
+ DO_UNIONFIND(0, 1);
+ if (v == 255) {
+ DO_UNIONFIND(-1, 1);
+ DO_UNIONFIND(1, 1);
+ }
+ }
+}
+#undef DO_UNIONFIND
+
+static void do_unionfind_task(void *p)
+{
+ struct unionfind_task *task = (struct unionfind_task*) p;
+
+ for (int y = task->y0; y < task->y1; y++) {
+ do_unionfind_line(task->uf, task->im, task->h, task->w, task->s, y);
+ }
+}
+
+static void do_quad_task(void *p)
+{
+ struct quad_task *task = (struct quad_task*) p;
+
+ zarray_t *clusters = task->clusters;
+ zarray_t *quads = task->quads;
+ apriltag_detector_t *td = task->td;
+ int w = task->w, h = task->h;
+
+ for (int cidx = task->cidx0; cidx < task->cidx1; cidx++) {
+
+ zarray_t *cluster;
+ zarray_get(clusters, cidx, &cluster);
+
+ if (zarray_size(cluster) < td->qtp.min_cluster_pixels)
+ continue;
+
+ // a cluster should contain only boundary points around the
+ // tag. it cannot be bigger than the whole screen. (Reject
+ // large connected blobs that will be prohibitively slow to
+ // fit quads to.) A typical point along an edge is added three
+ // times (because it has 3 neighbors). The maximum perimeter
+ // is 2w+2h.
+ if (zarray_size(cluster) > 3*(2*w+2*h)) {
+ continue;
+ }
+
+ struct quad quad;
+ memset(&quad, 0, sizeof(struct quad));
+
+ if (fit_quad(td, task->im, cluster, &quad)) {
+ pthread_mutex_lock(&td->mutex);
+
+ zarray_add(quads, &quad);
+ pthread_mutex_unlock(&td->mutex);
+ }
+ }
+}
+
+image_u8_t *threshold(apriltag_detector_t *td, image_u8_t *im)
+{
+ int w = im->width, h = im->height, s = im->stride;
+ assert(w < 32768);
+ assert(h < 32768);
+
+ image_u8_t *threshim = image_u8_create_alignment(w, h, s);
+ assert(threshim->stride == s);
+
+ // The idea is to find the maximum and minimum values in a
+ // window around each pixel. If it's a contrast-free region
+ // (max-min is small), don't try to binarize. Otherwise,
+ // threshold according to (max+min)/2.
+ //
+ // Mark low-contrast regions with value 127 so that we can skip
+ // future work on these areas too.
+
+ // however, computing max/min around every pixel is needlessly
+ // expensive. We compute max/min for tiles. To avoid artifacts
+ // that arise when high-contrast features appear near a tile
+ // edge (and thus moving from one tile to another results in a
+ // large change in max/min value), the max/min values used for
+ // any pixel are computed from all 3x3 surrounding tiles. Thus,
+ // the max/min sampling area for nearby pixels overlap by at least
+ // one tile.
+ //
+ // The important thing is that the windows be large enough to
+ // capture edge transitions; the tag does not need to fit into
+ // a tile.
+
+ // XXX Tunable. Generally, small tile sizes--- so long as they're
+ // large enough to span a single tag edge--- seem to be a winner.
+ const int tilesz = 4;
+
+ // the last (possibly partial) tiles along each row and column will
+ // just use the min/max value from the last full tile.
+ int tw = w / tilesz;
+ int th = h / tilesz;
+
+ uint8_t *im_max = calloc(tw*th, sizeof(uint8_t));
+ uint8_t *im_min = calloc(tw*th, sizeof(uint8_t));
+
+ // first, collect min/max statistics for each tile
+ for (int ty = 0; ty < th; ty++) {
+ for (int tx = 0; tx < tw; tx++) {
+ uint8_t max = 0, min = 255;
+
+ for (int dy = 0; dy < tilesz; dy++) {
+
+ for (int dx = 0; dx < tilesz; dx++) {
+
+ uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx];
+ if (v < min)
+ min = v;
+ if (v > max)
+ max = v;
+ }
+ }
+
+ im_max[ty*tw+tx] = max;
+ im_min[ty*tw+tx] = min;
+ }
+ }
+
+ // second, apply 3x3 max/min convolution to "blur" these values
+ // over larger areas. This reduces artifacts due to abrupt changes
+ // in the threshold value.
+ if (1) {
+ uint8_t *im_max_tmp = calloc(tw*th, sizeof(uint8_t));
+ uint8_t *im_min_tmp = calloc(tw*th, sizeof(uint8_t));
+
+ for (int ty = 0; ty < th; ty++) {
+ for (int tx = 0; tx < tw; tx++) {
+ uint8_t max = 0, min = 255;
+
+ for (int dy = -1; dy <= 1; dy++) {
+ if (ty+dy < 0 || ty+dy >= th)
+ continue;
+ for (int dx = -1; dx <= 1; dx++) {
+ if (tx+dx < 0 || tx+dx >= tw)
+ continue;
+
+ uint8_t m = im_max[(ty+dy)*tw+tx+dx];
+ if (m > max)
+ max = m;
+ m = im_min[(ty+dy)*tw+tx+dx];
+ if (m < min)
+ min = m;
+ }
+ }
+
+ im_max_tmp[ty*tw + tx] = max;
+ im_min_tmp[ty*tw + tx] = min;
+ }
+ }
+ free(im_max);
+ free(im_min);
+ im_max = im_max_tmp;
+ im_min = im_min_tmp;
+ }
+
+ for (int ty = 0; ty < th; ty++) {
+ for (int tx = 0; tx < tw; tx++) {
+
+ int min = im_min[ty*tw + tx];
+ int max = im_max[ty*tw + tx];
+
+ // low contrast region? (no edges)
+ if (max - min < td->qtp.min_white_black_diff) {
+ for (int dy = 0; dy < tilesz; dy++) {
+ int y = ty*tilesz + dy;
+
+ for (int dx = 0; dx < tilesz; dx++) {
+ int x = tx*tilesz + dx;
+
+ threshim->buf[y*s+x] = 127;
+ }
+ }
+ continue;
+ }
+
+ // otherwise, actually threshold this tile.
+
+ // argument for biasing towards dark; specular highlights
+ // can be substantially brighter than white tag parts
+ uint8_t thresh = min + (max - min) / 2;
+
+ for (int dy = 0; dy < tilesz; dy++) {
+ int y = ty*tilesz + dy;
+
+ for (int dx = 0; dx < tilesz; dx++) {
+ int x = tx*tilesz + dx;
+
+ uint8_t v = im->buf[y*s+x];
+ if (v > thresh)
+ threshim->buf[y*s+x] = 255;
+ else
+ threshim->buf[y*s+x] = 0;
+ }
+ }
+ }
+ }
+
+ // we skipped over the non-full-sized tiles above. Fix those now.
+ if (1) {
+ for (int y = 0; y < h; y++) {
+
+ // what is the first x coordinate we need to process in this row?
+
+ int x0;
+
+ if (y >= th*tilesz) {
+ x0 = 0; // we're at the bottom; do the whole row.
+ } else {
+ x0 = tw*tilesz; // we only need to do the right most part.
+ }
+
+ // compute tile coordinates and clamp.
+ int ty = y / tilesz;
+ if (ty >= th)
+ ty = th - 1;
+
+ for (int x = x0; x < w; x++) {
+ int tx = x / tilesz;
+ if (tx >= tw)
+ tx = tw - 1;
+
+ int max = im_max[ty*tw + tx];
+ int min = im_min[ty*tw + tx];
+ int thresh = min + (max - min) / 2;
+
+ uint8_t v = im->buf[y*s+x];
+ if (v > thresh)
+ threshim->buf[y*s+x] = 255;
+ else
+ threshim->buf[y*s+x] = 0;
+ }
+ }
+ }
+
+ free(im_min);
+ free(im_max);
+
+ // this is a dilate/erode deglitching scheme that does not improve
+ // anything as far as I can tell.
+ if (0 || td->qtp.deglitch) {
+ image_u8_t *tmp = image_u8_create(w, h);
+
+ for (int y = 1; y + 1 < h; y++) {
+ for (int x = 1; x + 1 < w; x++) {
+ uint8_t max = 0;
+ for (int dy = -1; dy <= 1; dy++) {
+ for (int dx = -1; dx <= 1; dx++) {
+ uint8_t v = threshim->buf[(y+dy)*s + x + dx];
+ if (v > max)
+ max = v;
+ }
+ }
+ tmp->buf[y*s+x] = max;
+ }
+ }
+
+ for (int y = 1; y + 1 < h; y++) {
+ for (int x = 1; x + 1 < w; x++) {
+ uint8_t min = 255;
+ for (int dy = -1; dy <= 1; dy++) {
+ for (int dx = -1; dx <= 1; dx++) {
+ uint8_t v = tmp->buf[(y+dy)*s + x + dx];
+ if (v < min)
+ min = v;
+ }
+ }
+ threshim->buf[y*s+x] = min;
+ }
+ }
+
+ image_u8_destroy(tmp);
+ }
+
+ timeprofile_stamp(td->tp, "threshold");
+
+ return threshim;
+}
+
+// basically the same as threshold(), but assumes the input image is a
+// bayer image. It collects statistics separately for each 2x2 block
+// of pixels. NOT WELL TESTED.
+image_u8_t *threshold_bayer(apriltag_detector_t *td, image_u8_t *im)
+{
+ int w = im->width, h = im->height, s = im->stride;
+
+ image_u8_t *threshim = image_u8_create_alignment(w, h, s);
+ assert(threshim->stride == s);
+
+ int tilesz = 32;
+ assert((tilesz & 1) == 0); // must be multiple of 2
+
+ int tw = w/tilesz + 1;
+ int th = h/tilesz + 1;
+
+ uint8_t *im_max[4], *im_min[4];
+ for (int i = 0; i < 4; i++) {
+ im_max[i] = calloc(tw*th, sizeof(uint8_t));
+ im_min[i] = calloc(tw*th, sizeof(uint8_t));
+ }
+
+ for (int ty = 0; ty < th; ty++) {
+ for (int tx = 0; tx < tw; tx++) {
+
+ uint8_t max[4] = { 0, 0, 0, 0};
+ uint8_t min[4] = { 255, 255, 255, 255 };
+
+ for (int dy = 0; dy < tilesz; dy++) {
+ if (ty*tilesz+dy >= h)
+ continue;
+
+ for (int dx = 0; dx < tilesz; dx++) {
+ if (tx*tilesz+dx >= w)
+ continue;
+
+ // which bayer element is this pixel?
+ int idx = (2*(dy&1) + (dx&1));
+
+ uint8_t v = im->buf[(ty*tilesz+dy)*s + tx*tilesz + dx];
+ if (v < min[idx])
+ min[idx] = v;
+ if (v > max[idx])
+ max[idx] = v;
+ }
+ }
+
+ for (int i = 0; i < 4; i++) {
+ im_max[i][ty*tw+tx] = max[i];
+ im_min[i][ty*tw+tx] = min[i];
+ }
+ }
+ }
+
+ for (int ty = 0; ty < th; ty++) {
+ for (int tx = 0; tx < tw; tx++) {
+
+ uint8_t max[4] = { 0, 0, 0, 0};
+ uint8_t min[4] = { 255, 255, 255, 255 };
+
+ for (int dy = -1; dy <= 1; dy++) {
+ if (ty+dy < 0 || ty+dy >= th)
+ continue;
+ for (int dx = -1; dx <= 1; dx++) {
+ if (tx+dx < 0 || tx+dx >= tw)
+ continue;
+
+ for (int i = 0; i < 4; i++) {
+ uint8_t m = im_max[i][(ty+dy)*tw+tx+dx];
+ if (m > max[i])
+ max[i] = m;
+ m = im_min[i][(ty+dy)*tw+tx+dx];
+ if (m < min[i])
+ min[i] = m;
+ }
+ }
+ }
+
+ // XXX CONSTANT
+// if (max - min < 30)
+// continue;
+
+ // argument for biasing towards dark: specular highlights
+ // can be substantially brighter than white tag parts
+ uint8_t thresh[4];
+ for (int i = 0; i < 4; i++) {
+ thresh[i] = min[i] + (max[i] - min[i]) / 2;
+ }
+
+ for (int dy = 0; dy < tilesz; dy++) {
+ int y = ty*tilesz + dy;
+ if (y >= h)
+ continue;
+
+ for (int dx = 0; dx < tilesz; dx++) {
+ int x = tx*tilesz + dx;
+ if (x >= w)
+ continue;
+
+ // which bayer element is this pixel?
+ int idx = (2*(y&1) + (x&1));
+
+ uint8_t v = im->buf[y*s+x];
+ threshim->buf[y*s+x] = v > thresh[idx];
+ }
+ }
+ }
+ }
+
+ for (int i = 0; i < 4; i++) {
+ free(im_min[i]);
+ free(im_max[i]);
+ }
+
+ timeprofile_stamp(td->tp, "threshold");
+
+ return threshim;
+}
+
+zarray_t *apriltag_quad_thresh(apriltag_detector_t *td, image_u8_t *im)
+{
+ ////////////////////////////////////////////////////////
+ // step 1. threshold the image, creating the edge image.
+
+ int w = im->width, h = im->height;
+
+ image_u8_t *threshim = threshold(td, im);
+ int ts = threshim->stride;
+
+ if (td->debug)
+ image_u8_write_pnm(threshim, "debug_threshold.pnm");
+
+ ////////////////////////////////////////////////////////
+ // step 2. find connected components.
+
+ unionfind_t *uf = unionfind_create(w * h);
+
+ if (td->nthreads <= 1) {
+ for (int y = 0; y < h - 1; y++) {
+ do_unionfind_line(uf, threshim, h, w, ts, y);
+ }
+ } else {
+ int sz = h - 1;
+ int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
+#ifdef _MSC_VER
+ struct unionfind_task *tasks = malloc((sz / chunksize + 1)*sizeof *tasks);
+#else
+ struct unionfind_task tasks[sz / chunksize + 1];
+#endif
+
+ int ntasks = 0;
+
+ for (int i = 0; i < sz; i += chunksize) {
+ // each task will process [y0, y1). Note that this attaches
+ // each cell to the right and down, so row y1 *is* potentially modified.
+ //
+ // for parallelization, make sure that each task doesn't touch rows
+ // used by another thread.
+ tasks[ntasks].y0 = i;
+ tasks[ntasks].y1 = imin(sz, i + chunksize - 1);
+ tasks[ntasks].h = h;
+ tasks[ntasks].w = w;
+ tasks[ntasks].s = ts;
+ tasks[ntasks].uf = uf;
+ tasks[ntasks].im = threshim;
+
+ workerpool_add_task(td->wp, do_unionfind_task, &tasks[ntasks]);
+ ntasks++;
+ }
+
+ workerpool_run(td->wp);
+
+ // XXX stitch together the different chunks.
+ for (int i = 0; i + 1 < ntasks; i++) {
+ do_unionfind_line(uf, threshim, h, w, ts, tasks[i].y1);
+ }
+ }
+
+ timeprofile_stamp(td->tp, "unionfind");
+
+ // XXX sizing??
+ int nclustermap = 2*w*h - 1;
+
+ struct uint64_zarray_entry **clustermap = calloc(nclustermap, sizeof(struct uint64_zarray_entry*));
+
+ for (int y = 1; y < h-1; y++) {
+ for (int x = 1; x < w-1; x++) {
+
+ uint8_t v0 = threshim->buf[y*ts + x];
+ if (v0 == 127)
+ continue;
+
+ // XXX don't query this until we know we need it?
+ uint64_t rep0 = unionfind_get_representative(uf, y*w + x);
+
+ // whenever we find two adjacent pixels such that one is
+ // white and the other black, we add the point half-way
+ // between them to a cluster associated with the unique
+ // ids of the white and black regions.
+ //
+ // We additionally compute the gradient direction (i.e., which
+ // direction was the white pixel?) Note: if (v1-v0) == 255, then
+ // (dx,dy) points towards the white pixel. if (v1-v0) == -255, then
+ // (dx,dy) points towards the black pixel. p.gx and p.gy will thus
+ // be -255, 0, or 255.
+ //
+ // Note that any given pixel might be added to multiple
+ // different clusters. But in the common case, a given
+ // pixel will be added multiple times to the same cluster,
+ // which increases the size of the cluster and thus the
+ // computational costs.
+ //
+ // A possible optimization would be to combine entries
+ // within the same cluster.
+
+#define DO_CONN(dx, dy) \
+ if (1) { \
+ uint8_t v1 = threshim->buf[y*ts + dy*ts + x + dx]; \
+ \
+ if (v0 + v1 == 255) { \
+ uint64_t rep1 = unionfind_get_representative(uf, y*w + dy*w + x + dx); \
+ uint64_t clusterid; \
+ if (rep0 < rep1) \
+ clusterid = (rep1 << 32) + rep0; \
+ else \
+ clusterid = (rep0 << 32) + rep1; \
+ \
+ /* XXX lousy hash function */ \
+ uint32_t clustermap_bucket = u64hash_2(clusterid) % nclustermap; \
+ struct uint64_zarray_entry *entry = clustermap[clustermap_bucket]; \
+ while (entry && entry->id != clusterid) { \
+ entry = entry->next; \
+ } \
+ \
+ if (!entry) { \
+ entry = calloc(1, sizeof(struct uint64_zarray_entry)); \
+ entry->id = clusterid; \
+ entry->cluster = zarray_create(sizeof(struct pt)); \
+ entry->next = clustermap[clustermap_bucket]; \
+ clustermap[clustermap_bucket] = entry; \
+ } \
+ \
+ struct pt p = { .x = 2*x + dx, .y = 2*y + dy, .gx = dx*((int) v1-v0), .gy = dy*((int) v1-v0)}; \
+ zarray_add(entry->cluster, &p); \
+ } \
+ }
+
+ // do 4 connectivity. NB: Arguments must be [-1, 1] or we'll overflow .gx, .gy
+ DO_CONN(1, 0);
+ DO_CONN(0, 1);
+
+ // do 8 connectivity
+ DO_CONN(-1, 1);
+ DO_CONN(1, 1);
+ }
+ }
+#undef DO_CONN
+
+ image_u8_destroy(threshim);
+
+ // make segmentation image.
+ if (td->debug) {
+ image_u8x3_t *d = image_u8x3_create(w, h);
+
+ uint32_t *colors = (uint32_t*) calloc(w*h, sizeof(*colors));
+
+ for (int y = 0; y < h; y++) {
+ for (int x = 0; x < w; x++) {
+ uint32_t v = unionfind_get_representative(uf, y*w+x);
+
+ if (unionfind_get_set_size(uf, v) < td->qtp.min_cluster_pixels)
+ continue;
+
+ uint32_t color = colors[v];
+ uint8_t r = color >> 16,
+ g = color >> 8,
+ b = color;
+
+ if (color == 0) {
+ const int bias = 50;
+ r = bias + (random() % (200-bias));
+ g = bias + (random() % (200-bias));
+ b = bias + (random() % (200-bias));
+ colors[v] = (r << 16) | (g << 8) | b;
+ }
+
+ d->buf[y*d->stride + 3*x + 0] = r;
+ d->buf[y*d->stride + 3*x + 1] = g;
+ d->buf[y*d->stride + 3*x + 2] = b;
+ }
+ }
+
+ free(colors);
+
+ image_u8x3_write_pnm(d, "debug_segmentation.pnm");
+ image_u8x3_destroy(d);
+ }
+
+ timeprofile_stamp(td->tp, "make clusters");
+
+ ////////////////////////////////////////////////////////
+ // step 3. process each connected component.
+ zarray_t *clusters = zarray_create(sizeof(zarray_t*)); //, uint64_zarray_hash_size(clustermap));
+ if (1) {
+ for (int i = 0; i < nclustermap; i++) {
+
+ for (struct uint64_zarray_entry *entry = clustermap[i]; entry; entry = entry->next) {
+ // XXX reject clusters here?
+ zarray_add(clusters, &entry->cluster);
+ }
+ }
+ }
+
+
+ if (td->debug) {
+ image_u8x3_t *d = image_u8x3_create(w, h);
+
+ for (int i = 0; i < zarray_size(clusters); i++) {
+ zarray_t *cluster;
+ zarray_get(clusters, i, &cluster);
+
+ uint32_t r, g, b;
+
+ if (1) {
+ const int bias = 50;
+ r = bias + (random() % (200-bias));
+ g = bias + (random() % (200-bias));
+ b = bias + (random() % (200-bias));
+ }
+
+ for (int j = 0; j < zarray_size(cluster); j++) {
+ struct pt *p;
+ zarray_get_volatile(cluster, j, &p);
+
+ int x = p->x / 2;
+ int y = p->y / 2;
+ d->buf[y*d->stride + 3*x + 0] = r;
+ d->buf[y*d->stride + 3*x + 1] = g;
+ d->buf[y*d->stride + 3*x + 2] = b;
+ }
+ }
+
+ image_u8x3_write_pnm(d, "debug_clusters.pnm");
+ image_u8x3_destroy(d);
+ }
+
+ if (1) {
+ for (int i = 0; i < nclustermap; i++) {
+ struct uint64_zarray_entry *entry = clustermap[i];
+ while (entry) {
+ struct uint64_zarray_entry *tmp = entry->next;
+ free(entry);
+ entry = tmp;
+ }
+ }
+ free(clustermap);
+ }
+
+ zarray_t *quads = zarray_create(sizeof(struct quad));
+
+ int sz = zarray_size(clusters);
+ int chunksize = 1 + sz / (APRILTAG_TASKS_PER_THREAD_TARGET * td->nthreads);
+#ifdef _MSC_VER
+ struct quad_task *tasks = malloc((sz / chunksize + 1)*sizeof *tasks);
+#else
+ struct quad_task tasks[sz / chunksize + 1];
+#endif
+
+ int ntasks = 0;
+ for (int i = 0; i < sz; i += chunksize) {
+ tasks[ntasks].td = td;
+ tasks[ntasks].cidx0 = i;
+ tasks[ntasks].cidx1 = imin(sz, i + chunksize);
+ tasks[ntasks].h = h;
+ tasks[ntasks].w = w;
+ tasks[ntasks].quads = quads;
+ tasks[ntasks].clusters = clusters;
+ tasks[ntasks].im = im;
+
+ workerpool_add_task(td->wp, do_quad_task, &tasks[ntasks]);
+ ntasks++;
+ }
+
+ workerpool_run(td->wp);
+
+#ifdef _MSC_VER
+ free(tasks);
+#endif
+
+ timeprofile_stamp(td->tp, "fit quads to clusters");
+
+ if (td->debug) {
+ FILE *f = fopen("debug_lines.ps", "w");
+ fprintf(f, "%%!PS\n\n");
+
+ image_u8_t *im2 = image_u8_copy(im);
+ image_u8_darken(im2);
+ image_u8_darken(im2);
+
+ // assume letter, which is 612x792 points.
+ double scale = fmin(612.0/im->width, 792.0/im2->height);
+ fprintf(f, "%.15f %.15f scale\n", scale, scale);
+ fprintf(f, "0 %d translate\n", im2->height);
+ fprintf(f, "1 -1 scale\n");
+
+ postscript_image(f, im);
+
+ for (int i = 0; i < zarray_size(quads); i++) {
+ struct quad *q;
+ zarray_get_volatile(quads, i, &q);
+
+ float rgb[3];
+ int bias = 100;
+
+ for (int i = 0; i < 3; i++)
+ rgb[i] = bias + (random() % (255-bias));
+
+ fprintf(f, "%f %f %f setrgbcolor\n", rgb[0]/255.0f, rgb[1]/255.0f, rgb[2]/255.0f);
+ fprintf(f, "%.15f %.15f moveto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto %.15f %.15f lineto stroke\n",
+ q->p[0][0], q->p[0][1],
+ q->p[1][0], q->p[1][1],
+ q->p[2][0], q->p[2][1],
+ q->p[3][0], q->p[3][1],
+ q->p[0][0], q->p[0][1]);
+ }
+
+ fclose(f);
+ }
+
+ // printf(" %d %d %d %d\n", indices[0], indices[1], indices[2], indices[3]);
+
+ unionfind_destroy(uf);
+
+ for (int i = 0; i < zarray_size(clusters); i++) {
+ zarray_t *cluster;
+ zarray_get(clusters, i, &cluster);
+ zarray_destroy(cluster);
+ }
+
+ zarray_destroy(clusters);
+
+
+ return quads;
+}
diff --git a/3rdparty/apriltag/common/doubles.h b/3rdparty/apriltag/common/doubles.h
new file mode 100644
index 0000000..e75db4b
--- /dev/null
+++ b/3rdparty/apriltag/common/doubles.h
@@ -0,0 +1,29 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _DOUBLES
+#define _DOUBLES
+
+#define TNAME double
+#include "doubles_floats_impl.h"
+#undef TNAME
+
+#endif
diff --git a/3rdparty/apriltag/common/doubles_floats_impl.h b/3rdparty/apriltag/common/doubles_floats_impl.h
new file mode 100644
index 0000000..8ada474
--- /dev/null
+++ b/3rdparty/apriltag/common/doubles_floats_impl.h
@@ -0,0 +1,1018 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+#include <float.h>
+
+#include "matd.h"
+#include "math_util.h"
+
+// XXX Write unit tests for me!
+// XXX Rewrite matd_coords in terms of this.
+
+/*
+ This file provides conversions between the following formats:
+
+ quaternion (TNAME[4], { w, x, y, z})
+
+ xyt (translation in x, y, and rotation in radians.)
+
+ xytcov (xyt as a TNAME[3] followed by covariance TNAME[9])
+
+ xy, xyz (translation in x, y, and z)
+
+ mat44 (4x4 rigid-body transformation matrix, row-major
+ order. Conventions: We assume points are projected via right
+ multiplication. E.g., p' = Mp.) Note: some functions really do rely
+ on it being a RIGID, scale=1 transform.
+
+ angleaxis (TNAME[4], { angle-rads, x, y, z }
+
+ xyzrpy (translation x, y, z, euler angles)
+
+ Roll Pitch Yaw are evaluated in the order: roll, pitch, then yaw. I.e.,
+ rollPitchYawToMatrix(rpy) = rotateZ(rpy[2]) * rotateY(rpy[1]) * rotateX(rpy[0])
+*/
+
+#define TRRFN(root, suffix) root ## suffix
+#define TRFN(root, suffix) TRRFN(root, suffix)
+#define TFN(suffix) TRFN(TNAME, suffix)
+
+// if V is null, returns null.
+static inline TNAME *TFN(s_dup)(const TNAME *v, int len)
+{
+ if (!v)
+ return NULL;
+
+ TNAME *r = malloc(len * sizeof(TNAME));
+ memcpy(r, v, len * sizeof(TNAME));
+ return r;
+}
+
+static inline void TFN(s_print)(const TNAME *a, int len, const char *fmt)
+{
+ for (int i = 0; i < len; i++)
+ printf(fmt, a[i]);
+ printf("\n");
+}
+
+static inline void TFN(s_print_mat)(const TNAME *a, int nrows, int ncols, const char *fmt)
+{
+ for (int i = 0; i < nrows * ncols; i++) {
+ printf(fmt, a[i]);
+ if ((i % ncols) == (ncols - 1))
+ printf("\n");
+ }
+}
+
+static inline void TFN(s_print_mat44)(const TNAME *a, const char *fmt)
+{
+ for (int i = 0; i < 4 * 4; i++) {
+ printf(fmt, a[i]);
+ if ((i % 4) == 3)
+ printf("\n");
+ }
+}
+
+static inline void TFN(s_add)(const TNAME *a, const TNAME *b, int len, TNAME *r)
+{
+ for (int i = 0; i < len; i++)
+ r[i] = a[i] + b[i];
+}
+
+static inline void TFN(s_subtract)(const TNAME *a, const TNAME *b, int len, TNAME *r)
+{
+ for (int i = 0; i < len; i++)
+ r[i] = a[i] - b[i];
+}
+
+static inline void TFN(s_scale)(TNAME s, const TNAME *v, int len, TNAME *r)
+{
+ for (int i = 0; i < len; i++)
+ r[i] = s * v[i];
+}
+
+static inline TNAME TFN(s_dot)(const TNAME *a, const TNAME *b, int len)
+{
+ TNAME acc = 0;
+ for (int i = 0; i < len; i++)
+ acc += a[i] * b[i];
+ return acc;
+}
+
+static inline TNAME TFN(s_distance)(const TNAME *a, const TNAME *b, int len)
+{
+ TNAME acc = 0;
+ for (int i = 0; i < len; i++)
+ acc += (a[i] - b[i])*(a[i] - b[i]);
+ return sqrt(acc);
+}
+
+static inline TNAME TFN(s_squared_distance)(const TNAME *a, const TNAME *b, int len)
+{
+ TNAME acc = 0;
+ for (int i = 0; i < len; i++)
+ acc += (a[i] - b[i])*(a[i] - b[i]);
+ return acc;
+}
+
+static inline TNAME TFN(s_squared_magnitude)(const TNAME *v, int len)
+{
+ TNAME acc = 0;
+ for (int i = 0; i < len; i++)
+ acc += v[i]*v[i];
+ return acc;
+}
+
+static inline TNAME TFN(s_magnitude)(const TNAME *v, int len)
+{
+ TNAME acc = 0;
+ for (int i = 0; i < len; i++)
+ acc += v[i]*v[i];
+ return sqrt(acc);
+}
+
+static inline void TFN(s_normalize)(const TNAME *v, int len, TNAME *r)
+{
+ TNAME mag = TFN(s_magnitude)(v, len);
+ for (int i = 0; i < len; i++)
+ r[i] = v[i] / mag;
+}
+
+static inline void TFN(s_normalize_self)(TNAME *v, int len)
+{
+ TNAME mag = TFN(s_magnitude)(v, len);
+ for (int i = 0; i < len; i++)
+ v[i] /= mag;
+}
+
+static inline void TFN(s_scale_self)(TNAME *v, int len, double scale)
+{
+ for (int i = 0; i < len; i++)
+ v[i] *= scale;
+}
+
+static inline void TFN(s_quat_rotate)(const TNAME q[4], const TNAME v[3], TNAME r[3])
+{
+ TNAME t2, t3, t4, t5, t6, t7, t8, t9, t10;
+
+ t2 = q[0]*q[1];
+ t3 = q[0]*q[2];
+ t4 = q[0]*q[3];
+ t5 = -q[1]*q[1];
+ t6 = q[1]*q[2];
+ t7 = q[1]*q[3];
+ t8 = -q[2]*q[2];
+ t9 = q[2]*q[3];
+ t10 = -q[3]*q[3];
+
+ r[0] = 2*((t8+t10)*v[0] + (t6-t4)*v[1] + (t3+t7)*v[2]) + v[0];
+ r[1] = 2*((t4+t6)*v[0] + (t5+t10)*v[1] + (t9-t2)*v[2]) + v[1];
+ r[2] = 2*((t7-t3)*v[0] + (t2+t9)*v[1] + (t5+t8)*v[2]) + v[2];
+}
+
+static inline void TFN(s_quat_multiply)(const TNAME a[4], const TNAME b[4], TNAME r[4])
+{
+ r[0] = a[0]*b[0] - a[1]*b[1] - a[2]*b[2] - a[3]*b[3];
+ r[1] = a[0]*b[1] + a[1]*b[0] + a[2]*b[3] - a[3]*b[2];
+ r[2] = a[0]*b[2] - a[1]*b[3] + a[2]*b[0] + a[3]*b[1];
+ r[3] = a[0]*b[3] + a[1]*b[2] - a[2]*b[1] + a[3]*b[0];
+}
+
+static inline void TFN(s_quat_inverse)(const TNAME q[4], TNAME r[4])
+{
+ TNAME mag = TFN(s_magnitude)(q, 4);
+ r[0] = q[0]/mag;
+ r[1] = -q[1]/mag;
+ r[2] = -q[2]/mag;
+ r[3] = -q[3]/mag;
+}
+
+static inline void TFN(s_copy)(const TNAME *src, TNAME *dst, int n)
+{
+ memcpy(dst, src, n * sizeof(TNAME));
+}
+
+static inline void TFN(s_xyt_copy)(const TNAME xyt[3], TNAME r[3])
+{
+ TFN(s_copy)(xyt, r, 3);
+}
+
+static inline void TFN(s_xyt_to_mat44)(const TNAME xyt[3], TNAME r[16])
+{
+ TNAME s = sin(xyt[2]), c = cos(xyt[2]);
+ memset(r, 0, sizeof(TNAME)*16);
+ r[0] = c;
+ r[1] = -s;
+ r[3] = xyt[0];
+ r[4] = s;
+ r[5] = c;
+ r[7] = xyt[1];
+ r[10] = 1;
+ r[15] = 1;
+}
+
+static inline void TFN(s_xyt_transform_xy)(const TNAME xyt[3], const TNAME xy[2], TNAME r[2])
+{
+ TNAME s = sin(xyt[2]), c = cos(xyt[2]);
+ r[0] = c*xy[0] - s*xy[1] + xyt[0];
+ r[1] = s*xy[0] + c*xy[1] + xyt[1];
+}
+
+static inline void TFN(s_mat_transform_xyz)(const TNAME M[16], const TNAME xyz[3], TNAME r[3])
+{
+ r[0] = M[0]*xyz[0] + M[1]*xyz[1] + M[2]*xyz[2] + M[3];
+ r[1] = M[4]*xyz[0] + M[5]*xyz[1] + M[6]*xyz[2] + M[7];
+ r[2] = M[8]*xyz[0] + M[9]*xyz[1] + M[10]*xyz[2] + M[11];
+}
+
+static inline void TFN(s_quat_to_angleaxis)(const TNAME _q[4], TNAME r[4])
+{
+ TNAME q[4];
+ TFN(s_normalize)(_q, 4, q);
+
+ // be polite: return an angle from [-pi, pi]
+ // use atan2 to be 4-quadrant safe
+ TNAME mag = TFN(s_magnitude)(&q[1], 3);
+ r[0] = mod2pi(2 * atan2(mag, q[0]));
+ if (mag != 0) {
+ r[1] = q[1] / mag;
+ r[2] = q[2] / mag;
+ r[3] = q[3] / mag;
+ } else {
+ r[1] = 1;
+ r[2] = 0;
+ r[3] = 0;
+ }
+}
+
+static inline void TFN(s_angleaxis_to_quat)(const TNAME aa[4], TNAME q[4])
+{
+ TNAME rad = aa[0];
+ q[0] = cos(rad / 2.0);
+ TNAME s = sin(rad / 2.0);
+
+ TNAME v[3] = { aa[1], aa[2], aa[3] };
+ TFN(s_normalize)(v, 3, v);
+
+ q[1] = s * v[0];
+ q[2] = s * v[1];
+ q[3] = s * v[2];
+}
+
+static inline void TFN(s_quat_to_mat44)(const TNAME q[4], TNAME r[16])
+{
+ TNAME w = q[0], x = q[1], y = q[2], z = q[3];
+
+ r[0] = w*w + x*x - y*y - z*z;
+ r[1] = 2*x*y - 2*w*z;
+ r[2] = 2*x*z + 2*w*y;
+ r[3] = 0;
+
+ r[4] = 2*x*y + 2*w*z;
+ r[5] = w*w - x*x + y*y - z*z;
+ r[6] = 2*y*z - 2*w*x;
+ r[7] = 0;
+
+ r[8] = 2*x*z - 2*w*y;
+ r[9] = 2*y*z + 2*w*x;
+ r[10] = w*w - x*x - y*y + z*z;
+ r[11] = 0;
+
+ r[12] = 0;
+ r[13] = 0;
+ r[14] = 0;
+ r[15] = 1;
+}
+
+static inline void TFN(s_angleaxis_to_mat44)(const TNAME aa[4], TNAME r[4])
+{
+ TNAME q[4];
+
+ TFN(s_angleaxis_to_quat)(aa, q);
+ TFN(s_quat_to_mat44)(q, r);
+}
+
+static inline void TFN(s_quat_xyz_to_mat44)(const TNAME q[4], const TNAME xyz[3], TNAME r[16])
+{
+ TFN(s_quat_to_mat44)(q, r);
+
+ if (xyz != NULL) {
+ r[3] = xyz[0];
+ r[7] = xyz[1];
+ r[11] = xyz[2];
+ }
+}
+
+static inline void TFN(s_rpy_to_quat)(const TNAME rpy[3], TNAME quat[4])
+{
+ TNAME roll = rpy[0], pitch = rpy[1], yaw = rpy[2];
+
+ TNAME halfroll = roll / 2;
+ TNAME halfpitch = pitch / 2;
+ TNAME halfyaw = yaw / 2;
+
+ TNAME sin_r2 = sin(halfroll);
+ TNAME sin_p2 = sin(halfpitch);
+ TNAME sin_y2 = sin(halfyaw);
+
+ TNAME cos_r2 = cos(halfroll);
+ TNAME cos_p2 = cos(halfpitch);
+ TNAME cos_y2 = cos(halfyaw);
+
+ quat[0] = cos_r2 * cos_p2 * cos_y2 + sin_r2 * sin_p2 * sin_y2;
+ quat[1] = sin_r2 * cos_p2 * cos_y2 - cos_r2 * sin_p2 * sin_y2;
+ quat[2] = cos_r2 * sin_p2 * cos_y2 + sin_r2 * cos_p2 * sin_y2;
+ quat[3] = cos_r2 * cos_p2 * sin_y2 - sin_r2 * sin_p2 * cos_y2;
+}
+
+// Reference: "A tutorial on SE(3) transformation parameterizations and
+// on-manifold optimization" by Jose-Luis Blanco
+static inline void TFN(s_quat_to_rpy)(const TNAME q[4], TNAME rpy[3])
+{
+ const TNAME qr = q[0];
+ const TNAME qx = q[1];
+ const TNAME qy = q[2];
+ const TNAME qz = q[3];
+
+ TNAME disc = qr*qy - qx*qz;
+
+ if (fabs(disc+0.5) < DBL_EPSILON) { // near -1/2
+ rpy[0] = 0;
+ rpy[1] = -M_PI/2;
+ rpy[2] = 2 * atan2(qx, qr);
+ }
+ else if (fabs(disc-0.5) < DBL_EPSILON) { // near 1/2
+ rpy[0] = 0;
+ rpy[1] = M_PI/2;
+ rpy[2] = -2 * atan2(qx, qr);
+ }
+ else {
+ // roll
+ TNAME roll_a = 2 * (qr*qx + qy*qz);
+ TNAME roll_b = 1 - 2 * (qx*qx + qy*qy);
+ rpy[0] = atan2(roll_a, roll_b);
+
+ // pitch
+ rpy[1] = asin(2*disc);
+
+ // yaw
+ TNAME yaw_a = 2 * (qr*qz + qx*qy);
+ TNAME yaw_b = 1 - 2 * (qy*qy + qz*qz);
+ rpy[2] = atan2(yaw_a, yaw_b);
+ }
+}
+
+static inline void TFN(s_rpy_to_mat44)(const TNAME rpy[3], TNAME M[16])
+{
+ TNAME q[4];
+ TFN(s_rpy_to_quat)(rpy, q);
+ TFN(s_quat_to_mat44)(q, M);
+}
+
+
+static inline void TFN(s_xyzrpy_to_mat44)(const TNAME xyzrpy[6], TNAME M[16])
+{
+ TFN(s_rpy_to_mat44)(&xyzrpy[3], M);
+ M[3] = xyzrpy[0];
+ M[7] = xyzrpy[1];
+ M[11] = xyzrpy[2];
+}
+
+static inline void TFN(s_mat44_transform_xyz)(const TNAME M[16], const TNAME in[3], TNAME out[3])
+{
+ for (int i = 0; i < 3; i++)
+ out[i] = M[4*i + 0]*in[0] + M[4*i + 1]*in[1] + M[4*i + 2]*in[2] + M[4*i + 3];
+}
+
+// out = (upper 3x3 of M) * in
+static inline void TFN(s_mat44_rotate_vector)(const TNAME M[16], const TNAME in[3], TNAME out[3])
+{
+ for (int i = 0; i < 3; i++)
+ out[i] = M[4*i + 0]*in[0] + M[4*i + 1]*in[1] + M[4*i + 2]*in[2];
+}
+
+static inline void TFN(s_mat44_to_xyt)(const TNAME M[16], TNAME xyt[3])
+{
+ // c -s
+ // s c
+ xyt[0] = M[3];
+ xyt[1] = M[7];
+ xyt[2] = atan2(M[4], M[0]);
+}
+
+static inline void TFN(s_mat_to_xyz)(const TNAME M[16], TNAME xyz[3])
+{
+ xyz[0] = M[3];
+ xyz[1] = M[7];
+ xyz[2] = M[11];
+}
+
+static inline void TFN(s_mat_to_quat)(const TNAME M[16], TNAME q[4])
+{
+ double T = M[0] + M[5] + M[10] + 1.0;
+ double S;
+
+ if (T > 0.0000001) {
+ S = sqrt(T) * 2;
+ q[0] = 0.25 * S;
+ q[1] = (M[9] - M[6]) / S;
+ q[2] = (M[2] - M[8]) / S;
+ q[3] = (M[4] - M[1]) / S;
+ } else if (M[0] > M[5] && M[0] > M[10]) { // Column 0:
+ S = sqrt(1.0 + M[0] - M[5] - M[10]) * 2;
+ q[0] = (M[9] - M[6]) / S;
+ q[1] = 0.25 * S;
+ q[2] = (M[4] + M[1]) / S;
+ q[3] = (M[2] + M[8]) / S;
+ } else if (M[5] > M[10]) { // Column 1:
+ S = sqrt(1.0 + M[5] - M[0] - M[10]) * 2;
+ q[0] = (M[2] - M[8]) / S;
+ q[1] = (M[4] + M[1]) / S;
+ q[2] = 0.25 * S;
+ q[3] = (M[9] + M[6]) / S;
+ } else { // Column 2:
+ S = sqrt(1.0 + M[10] - M[0] - M[5]);
+ q[0] = (M[4] - M[1]) / S;
+ q[1] = (M[2] + M[8]) / S;
+ q[2] = (M[9] + M[6]) / S;
+ q[3] = 0.25 * S;
+ }
+
+ TFN(s_normalize)(q, 4, q);
+}
+
+static inline void TFN(s_quat_xyz_to_xyt)(const TNAME q[4], const TNAME xyz[3], TNAME xyt[3])
+{
+ TNAME M[16];
+ TFN(s_quat_xyz_to_mat44)(q, xyz, M);
+ TFN(s_mat44_to_xyt)(M, xyt);
+}
+
+// xytr = xyta * xytb;
+static inline void TFN(s_xyt_mul)(const TNAME xyta[3], const TNAME xytb[3], TNAME xytr[3])
+{
+ TNAME xa = xyta[0], ya = xyta[1], ta = xyta[2];
+ TNAME s = sin(ta), c = cos(ta);
+
+ xytr[0] = c*xytb[0] - s*xytb[1] + xa;
+ xytr[1] = s*xytb[0] + c*xytb[1] + ya;
+ xytr[2] = ta + xytb[2];
+}
+
+static inline void TFN(s_xytcov_copy)(const TNAME xyta[3], const TNAME Ca[9],
+ TNAME xytr[3], TNAME Cr[9])
+{
+ memcpy(xytr, xyta, 3 * sizeof(TNAME));
+ memcpy(Cr, Ca, 9 * sizeof(TNAME));
+}
+
+static inline void TFN(s_xytcov_mul)(const TNAME xyta[3], const TNAME Ca[9],
+ const TNAME xytb[3], const TNAME Cb[9],
+ TNAME xytr[3], TNAME Cr[9])
+{
+ TNAME xa = xyta[0], ya = xyta[1], ta = xyta[2];
+ TNAME xb = xytb[0], yb = xytb[1];
+
+ TNAME sa = sin(ta), ca = cos(ta);
+
+ TNAME P11 = Ca[0], P12 = Ca[1], P13 = Ca[2];
+ TNAME P22 = Ca[4], P23 = Ca[5];
+ TNAME P33 = Ca[8];
+
+ TNAME Q11 = Cb[0], Q12 = Cb[1], Q13 = Cb[2];
+ TNAME Q22 = Cb[4], Q23 = Cb[5];
+ TNAME Q33 = Cb[8];
+
+ TNAME JA13 = -sa*xb - ca*yb;
+ TNAME JA23 = ca*xb - sa*yb;
+ TNAME JB11 = ca;
+ TNAME JB12 = -sa;
+ TNAME JB21 = sa;
+ TNAME JB22 = ca;
+
+ Cr[0] = P33*JA13*JA13 + 2*P13*JA13 + Q11*JB11*JB11 + 2*Q12*JB11*JB12 + Q22*JB12*JB12 + P11;
+ Cr[1] = P12 + JA23*(P13 + JA13*P33) + JA13*P23 + JB21*(JB11*Q11 + JB12*Q12) + JB22*(JB11*Q12 + JB12*Q22);
+ Cr[2] = P13 + JA13*P33 + JB11*Q13 + JB12*Q23;
+ Cr[3] = Cr[1];
+ Cr[4] = P33*JA23*JA23 + 2*P23*JA23 + Q11*JB21*JB21 + 2*Q12*JB21*JB22 + Q22*JB22*JB22 + P22;
+ Cr[5] = P23 + JA23*P33 + JB21*Q13 + JB22*Q23;
+ Cr[6] = Cr[2];
+ Cr[7] = Cr[5];
+ Cr[8] = P33 + Q33;
+
+ xytr[0] = ca*xb - sa*yb + xa;
+ xytr[1] = sa*xb + ca*yb + ya;
+ xytr[2] = xyta[2] + xytb[2];
+
+/*
+ // the code above is just an unrolling of the following:
+
+ TNAME JA[][] = new TNAME[][] { { 1, 0, -sa*xb - ca*yb },
+ { 0, 1, ca*xb - sa*yb },
+ { 0, 0, 1 } };
+ TNAME JB[][] = new TNAME[][] { { ca, -sa, 0 },
+ { sa, ca, 0 },
+ { 0, 0, 1 } };
+
+ newge.P = LinAlg.add(LinAlg.matrixABCt(JA, P, JA),
+ LinAlg.matrixABCt(JB, ge.P, JB));
+*/
+}
+
+
+static inline void TFN(s_xyt_inv)(const TNAME xyta[3], TNAME xytr[3])
+{
+ TNAME s = sin(xyta[2]), c = cos(xyta[2]);
+ xytr[0] = -s*xyta[1] - c*xyta[0];
+ xytr[1] = -c*xyta[1] + s*xyta[0];
+ xytr[2] = -xyta[2];
+}
+
+static inline void TFN(s_xytcov_inv)(const TNAME xyta[3], const TNAME Ca[9],
+ TNAME xytr[3], TNAME Cr[9])
+{
+ TNAME x = xyta[0], y = xyta[1], theta = xyta[2];
+ TNAME s = sin(theta), c = cos(theta);
+
+ TNAME J11 = -c, J12 = -s, J13 = -c*y + s*x;
+ TNAME J21 = s, J22 = -c, J23 = s*y + c*x;
+
+ TNAME P11 = Ca[0], P12 = Ca[1], P13 = Ca[2];
+ TNAME P22 = Ca[4], P23 = Ca[5];
+ TNAME P33 = Ca[8];
+
+ Cr[0] = P11*J11*J11 + 2*P12*J11*J12 + 2*P13*J11*J13 +
+ P22*J12*J12 + 2*P23*J12*J13 + P33*J13*J13;
+ Cr[1] = J21*(J11*P11 + J12*P12 + J13*P13) +
+ J22*(J11*P12 + J12*P22 + J13*P23) +
+ J23*(J11*P13 + J12*P23 + J13*P33);
+ Cr[2] = - J11*P13 - J12*P23 - J13*P33;
+ Cr[3] = Cr[1];
+ Cr[4] = P11*J21*J21 + 2*P12*J21*J22 + 2*P13*J21*J23 +
+ P22*J22*J22 + 2*P23*J22*J23 + P33*J23*J23;
+ Cr[5] = - J21*P13 - J22*P23 - J23*P33;
+ Cr[6] = Cr[2];
+ Cr[7] = Cr[5];
+ Cr[8] = P33;
+
+ /*
+ // the code above is just an unrolling of the following:
+
+ TNAME J[][] = new TNAME[][] { { -c, -s, -c*y + s*x },
+ { s, -c, s*y + c*x },
+ { 0, 0, -1 } };
+ ge.P = LinAlg.matrixABCt(J, P, J);
+ */
+
+ xytr[0] = -s*y - c*x;
+ xytr[1] = -c*y + s*x;
+ xytr[2] = -xyta[2];
+}
+
+// xytr = inv(xyta) * xytb
+static inline void TFN(s_xyt_inv_mul)(const TNAME xyta[3], const TNAME xytb[3], TNAME xytr[3])
+{
+ TNAME theta = xyta[2];
+ TNAME ca = cos(theta);
+ TNAME sa = sin(theta);
+ TNAME dx = xytb[0] - xyta[0];
+ TNAME dy = xytb[1] - xyta[1];
+
+ xytr[0] = ca*dx + sa*dy;
+ xytr[1] = -sa*dx + ca*dy;
+ xytr[2]= xytb[2] - xyta[2];
+}
+
+static inline void TFN(s_mat_add)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Arows == Brows);
+ assert(Arows == Rrows);
+ assert(Bcols == Bcols);
+ assert(Bcols == Rcols);
+
+ for (int i = 0; i < Arows; i++)
+ for (int j = 0; j < Bcols; j++)
+ R[i*Acols + j] = A[i*Acols + j] + B[i*Acols + j];
+}
+
+// matrix should be in row-major order, allocated in a single packed
+// array. (This is compatible with matd.)
+static inline void TFN(s_mat_AB)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Acols == Brows);
+ assert(Rrows == Arows);
+ assert(Bcols == Rcols);
+
+ for (int Rrow = 0; Rrow < Rrows; Rrow++) {
+ for (int Rcol = 0; Rcol < Rcols; Rcol++) {
+ TNAME acc = 0;
+ for (int i = 0; i < Acols; i++)
+ acc += A[Rrow*Acols + i] * B[i*Bcols + Rcol];
+ R[Rrow*Rcols + Rcol] = acc;
+ }
+ }
+}
+
+// matrix should be in row-major order, allocated in a single packed
+// array. (This is compatible with matd.)
+static inline void TFN(s_mat_ABt)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Acols == Bcols);
+ assert(Rrows == Arows);
+ assert(Brows == Rcols);
+
+ for (int Rrow = 0; Rrow < Rrows; Rrow++) {
+ for (int Rcol = 0; Rcol < Rcols; Rcol++) {
+ TNAME acc = 0;
+ for (int i = 0; i < Acols; i++)
+ acc += A[Rrow*Acols + i] * B[Rcol*Bcols + i];
+ R[Rrow*Rcols + Rcol] = acc;
+ }
+ }
+}
+
+static inline void TFN(s_mat_ABC)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ const TNAME *C, int Crows, int Ccols,
+ TNAME *R, int Rrows, int Rcols)
+{
+#ifdef _MSC_VER
+ TNAME *tmp = malloc(Arows*Bcols * sizeof *tmp);
+#else
+ TNAME tmp[Arows*Bcols];
+#endif
+
+ TFN(s_mat_AB)(A, Arows, Acols, B, Brows, Bcols, tmp, Arows, Bcols);
+ TFN(s_mat_AB)(tmp, Arows, Bcols, C, Crows, Ccols, R, Rrows, Rcols);
+
+#ifdef _MSC_VER
+ free(tmp);
+#endif
+}
+
+static inline void TFN(s_mat_Ab)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Blength,
+ TNAME *R, int Rlength)
+{
+ assert(Acols == Blength);
+ assert(Arows == Rlength);
+
+ for (int Ridx = 0; Ridx < Rlength; Ridx++) {
+ TNAME acc = 0;
+ for (int i = 0; i < Blength; i++)
+ acc += A[Ridx*Acols + i] * B[i];
+ R[Ridx] = acc;
+ }
+}
+
+static inline void TFN(s_mat_AtB)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Arows == Brows);
+ assert(Rrows == Acols);
+ assert(Bcols == Rcols);
+
+ for (int Rrow = 0; Rrow < Rrows; Rrow++) {
+ for (int Rcol = 0; Rcol < Rcols; Rcol++) {
+ TNAME acc = 0;
+ for (int i = 0; i < Acols; i++)
+ acc += A[i*Acols + Rrow] * B[i*Bcols + Rcol];
+ R[Rrow*Rcols + Rcol] = acc;
+ }
+ }
+}
+
+static inline void TFN(s_quat_slerp)(const TNAME q0[4], const TNAME _q1[4], TNAME r[4], TNAME w)
+{
+ TNAME dot = TFN(s_dot)(q0, _q1, 4);
+
+ TNAME q1[4];
+ memcpy(q1, _q1, sizeof(TNAME) * 4);
+
+ if (dot < 0) {
+ // flip sign on one of them so we don't spin the "wrong
+ // way" around. This doesn't change the rotation that the
+ // quaternion represents.
+ dot = -dot;
+ for (int i = 0; i < 4; i++)
+ q1[i] *= -1;
+ }
+
+ // if large dot product (1), slerp will scale both q0 and q1
+ // by 0, and normalization will blow up.
+ if (dot > 0.95) {
+
+ for (int i = 0; i < 4; i++)
+ r[i] = q0[i]*(1-w) + q1[i]*w;
+
+ } else {
+ TNAME angle = acos(dot);
+
+ TNAME w0 = sin(angle*(1-w)), w1 = sin(angle*w);
+
+ for (int i = 0; i < 4; i++)
+ r[i] = q0[i]*w0 + q1[i]*w1;
+
+ TFN(s_normalize)(r, 4, r);
+ }
+}
+
+static inline void TFN(s_cross_product)(const TNAME v1[3], const TNAME v2[3], TNAME r[3])
+{
+ r[0] = v1[1]*v2[2] - v1[2]*v2[1];
+ r[1] = v1[2]*v2[0] - v1[0]*v2[2];
+ r[2] = v1[0]*v2[1] - v1[1]*v2[0];
+}
+
+////////////////////
+static inline void TFN(s_mat44_identity)(TNAME out[16])
+{
+ memset(out, 0, 16 * sizeof(TNAME));
+ out[0] = 1;
+ out[5] = 1;
+ out[10] = 1;
+ out[15] = 1;
+}
+
+static inline void TFN(s_mat44_translate)(const TNAME txyz[3], TNAME out[16])
+{
+ TFN(s_mat44_identity)(out);
+
+ for (int i = 0; i < 3; i++)
+ out[4*i + 3] += txyz[i];
+}
+
+static inline void TFN(s_mat44_scale)(const TNAME sxyz[3], TNAME out[16])
+{
+ TFN(s_mat44_identity)(out);
+
+ for (int i = 0; i < 3; i++)
+ out[4*i + i] = sxyz[i];
+}
+
+static inline void TFN(s_mat44_rotate_z)(TNAME rad, TNAME out[16])
+{
+ TFN(s_mat44_identity)(out);
+ double s = sin(rad), c = cos(rad);
+ out[0*4 + 0] = c;
+ out[0*4 + 1] = -s;
+ out[1*4 + 0] = s;
+ out[1*4 + 1] = c;
+}
+
+static inline void TFN(s_mat44_rotate_y)(TNAME rad, TNAME out[16])
+{
+ TFN(s_mat44_identity)(out);
+ double s = sin(rad), c = cos(rad);
+ out[0*4 + 0] = c;
+ out[0*4 + 2] = s;
+ out[2*4 + 0] = -s;
+ out[2*4 + 2] = c;
+}
+
+static inline void TFN(s_mat44_rotate_x)(TNAME rad, TNAME out[16])
+{
+ TFN(s_mat44_identity)(out);
+ double s = sin(rad), c = cos(rad);
+ out[1*4 + 1] = c;
+ out[1*4 + 2] = -s;
+ out[2*4 + 1] = s;
+ out[2*4 + 2] = c;
+}
+
+// out = out * translate(txyz)
+static inline void TFN(s_mat44_translate_self)(const TNAME txyz[3], TNAME out[16])
+{
+ TNAME tmp[16], prod[16];
+ TFN(s_mat44_translate(txyz, tmp));
+ TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4);
+ memcpy(out, prod, sizeof(TNAME)*16);
+}
+
+static inline void TFN(s_mat44_scale_self)(const TNAME sxyz[3], TNAME out[16])
+{
+ TNAME tmp[16], prod[16];
+ TFN(s_mat44_scale(sxyz, tmp));
+ TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4);
+ memcpy(out, prod, sizeof(TNAME)*16);
+}
+
+static inline void TFN(s_mat44_rotate_z_self)(TNAME rad, TNAME out[16])
+{
+ TNAME tmp[16], prod[16];
+ TFN(s_mat44_rotate_z(rad, tmp));
+ TFN(s_mat_AB)(out, 4, 4, tmp, 4, 4, prod, 4, 4);
+ memcpy(out, prod, sizeof(TNAME)*16);
+}
+
+// out = inv(M)*in. Note: this assumes that mat44 is a rigid-body transformation.
+static inline void TFN(s_mat44_inv)(const TNAME M[16], TNAME out[16])
+{
+// NB: M = T*R, inv(M) = inv(R) * inv(T)
+
+ // transpose of upper-left corner
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ out[4*i + j] = M[4*j + i];
+
+ out[4*0 + 3] = 0;
+ out[4*1 + 3] = 0;
+ out[4*2 + 3] = 0;
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ out[4*i + 3] -= out[4*i + j] * M[4*j + 3];
+
+ out[4*3 + 0] = 0;
+ out[4*3 + 1] = 0;
+ out[4*3 + 2] = 0;
+ out[4*3 + 3] = 1;
+
+/* TNAME tmp[16];
+ TFN(s_mat_AB)(M, 4, 4, out, 4, 4, tmp, 4, 4);
+ printf("identity: ");
+ TFN(s_print_mat)(tmp, 4, 4, "%15f"); */
+}
+
+// out = inv(M)*in
+static inline void TFN(s_mat44_inv_transform_xyz)(const TNAME M[16], const TNAME in[3], TNAME out[3])
+{
+ TNAME T[16];
+ TFN(s_mat44_inv)(M, T);
+
+ TFN(s_mat44_transform_xyz)(T, in, out);
+}
+
+// out = (upper 3x3 of inv(M)) * in
+static inline void TFN(s_mat44_inv_rotate_vector)(const TNAME M[16], const TNAME in[3], TNAME out[3])
+{
+ TNAME T[16];
+ TFN(s_mat44_inv)(M, T);
+
+ TFN(s_mat44_rotate_vector)(T, in, out);
+}
+
+static inline void TFN(s_elu_to_mat44)(const TNAME eye[3], const TNAME lookat[3], const TNAME _up[3],
+ TNAME M[16])
+{
+ TNAME f[3];
+ TFN(s_subtract)(lookat, eye, 3, f);
+ TFN(s_normalize)(f, 3, f);
+
+ TNAME up[3];
+
+ // remove any component of 'up' that isn't perpendicular to the look direction.
+ TFN(s_normalize)(_up, 3, up);
+
+ TNAME up_dot = TFN(s_dot)(f, up, 3);
+ for (int i = 0; i < 3; i++)
+ up[i] -= up_dot*f[i];
+
+ TFN(s_normalize_self)(up, 3);
+
+ TNAME s[3], u[3];
+ TFN(s_cross_product)(f, up, s);
+ TFN(s_cross_product)(s, f, u);
+
+ TNAME R[16] = { s[0], s[1], s[2], 0,
+ u[0], u[1], u[2], 0,
+ -f[0], -f[1], -f[2], 0,
+ 0, 0, 0, 1};
+
+ TNAME T[16] = {1, 0, 0, -eye[0],
+ 0, 1, 0, -eye[1],
+ 0, 0, 1, -eye[2],
+ 0, 0, 0, 1};
+
+ // M is the extrinsics matrix [R | t] where t = -R*c
+ TNAME tmp[16];
+ TFN(s_mat_AB)(R, 4, 4, T, 4, 4, tmp, 4, 4);
+ TFN(s_mat44_inv)(tmp, M);
+}
+
+// Computes the cholesky factorization of A, putting the lower
+// triangular matrix into R.
+static inline void TFN(s_mat33_chol)(const TNAME *A, int Arows, int Acols,
+ TNAME *R, int Brows, int Bcols)
+{
+ assert(Arows == Brows);
+ assert(Bcols == Bcols);
+
+ // A[0] = R[0]*R[0]
+ R[0] = sqrt(A[0]);
+
+ // A[1] = R[0]*R[3];
+ R[3] = A[1] / R[0];
+
+ // A[2] = R[0]*R[6];
+ R[6] = A[2] / R[0];
+
+ // A[4] = R[3]*R[3] + R[4]*R[4]
+ R[4] = sqrt(A[4] - R[3]*R[3]);
+
+ // A[5] = R[3]*R[6] + R[4]*R[7]
+ R[7] = (A[5] - R[3]*R[6]) / R[4];
+
+ // A[8] = R[6]*R[6] + R[7]*R[7] + R[8]*R[8]
+ R[8] = sqrt(A[8] - R[6]*R[6] - R[7]*R[7]);
+
+ R[1] = 0;
+ R[2] = 0;
+ R[5] = 0;
+}
+
+static inline void TFN(s_mat33_lower_tri_inv)(const TNAME *A, int Arows, int Acols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ // A[0]*R[0] = 1
+ R[0] = 1 / A[0];
+
+ // A[3]*R[0] + A[4]*R[3] = 0
+ R[3] = -A[3]*R[0] / A[4];
+
+ // A[4]*R[4] = 1
+ R[4] = 1 / A[4];
+
+ // A[6]*R[0] + A[7]*R[3] + A[8]*R[6] = 0
+ R[6] = (-A[6]*R[0] - A[7]*R[3]) / A[8];
+
+ // A[7]*R[4] + A[8]*R[7] = 0
+ R[7] = -A[7]*R[4] / A[8];
+
+ // A[8]*R[8] = 1
+ R[8] = 1 / A[8];
+}
+
+
+static inline void TFN(s_mat33_sym_solve)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Arows == Acols);
+ assert(Acols == 3);
+ assert(Brows == 3);
+ assert(Bcols == 1);
+ assert(Rrows == 3);
+ assert(Rcols == 1);
+
+ TNAME L[9];
+ TFN(s_mat33_chol)(A, 3, 3, L, 3, 3);
+
+ TNAME M[9];
+ TFN(s_mat33_lower_tri_inv)(L, 3, 3, M, 3, 3);
+
+ double tmp[3];
+ tmp[0] = M[0]*B[0];
+ tmp[1] = M[3]*B[0] + M[4]*B[1];
+ tmp[2] = M[6]*B[0] + M[7]*B[1] + M[8]*B[2];
+
+ R[0] = M[0]*tmp[0] + M[3]*tmp[1] + M[6]*tmp[2];
+ R[1] = M[4]*tmp[1] + M[7]*tmp[2];
+ R[2] = M[8]*tmp[2];
+}
+
+/*
+// solve Ax = B. Assumes A is symmetric; uses cholesky factorization
+static inline void TFN(s_mat_solve_chol)(const TNAME *A, int Arows, int Acols,
+ const TNAME *B, int Brows, int Bcols,
+ TNAME *R, int Rrows, int Rcols)
+{
+ assert(Arows == Acols);
+ assert(Arows == Brows);
+ assert(Acols == Rrows);
+ assert(Bcols == Rcols);
+
+ //
+}
+*/
+#undef TRRFN
+#undef TRFN
+#undef TFN
diff --git a/3rdparty/apriltag/common/floats.h b/3rdparty/apriltag/common/floats.h
new file mode 100644
index 0000000..f5c9f06
--- /dev/null
+++ b/3rdparty/apriltag/common/floats.h
@@ -0,0 +1,29 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _FLOATS_H
+#define _FLOATS_H
+
+#define TNAME float
+#include "doubles_floats_impl.h"
+#undef TNAME
+
+#endif
diff --git a/3rdparty/apriltag/common/g2d.c b/3rdparty/apriltag/common/g2d.c
new file mode 100644
index 0000000..45f67e7
--- /dev/null
+++ b/3rdparty/apriltag/common/g2d.c
@@ -0,0 +1,919 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+
+#include "g2d.h"
+#include "common/math_util.h"
+
+#if defined(__MINGW32__) || defined(_MSC_VER)
+#define random rand
+#endif
+
+double g2d_distance(const double a[2], const double b[2])
+{
+ return sqrtf(sq(a[0]-b[0]) + sq(a[1]-b[1]));
+}
+
+zarray_t *g2d_polygon_create_empty()
+{
+ return zarray_create(sizeof(double[2]));
+}
+
+void g2d_polygon_add(zarray_t *poly, double v[2])
+{
+ zarray_add(poly, v);
+}
+
+zarray_t *g2d_polygon_create_data(double v[][2], int sz)
+{
+ zarray_t *points = g2d_polygon_create_empty();
+
+ for (int i = 0; i < sz; i++)
+ g2d_polygon_add(points, v[i]);
+
+ return points;
+}
+
+zarray_t *g2d_polygon_create_zeros(int sz)
+{
+ zarray_t *points = zarray_create(sizeof(double[2]));
+
+ double z[2] = { 0, 0 };
+
+ for (int i = 0; i < sz; i++)
+ zarray_add(points, z);
+
+ return points;
+}
+
+void g2d_polygon_make_ccw(zarray_t *poly)
+{
+ // Step one: we want the points in counter-clockwise order.
+ // If the points are in clockwise order, we'll reverse them.
+ double total_theta = 0;
+ double last_theta = 0;
+
+ // Count the angle accumulated going around the polygon. If
+ // the sum is +2pi, it's CCW. Otherwise, we'll get -2pi.
+ int sz = zarray_size(poly);
+
+ for (int i = 0; i <= sz; i++) {
+ double p0[2], p1[2];
+ zarray_get(poly, i % sz, &p0);
+ zarray_get(poly, (i+1) % sz, &p1);
+
+ double this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]);
+
+ if (i > 0) {
+ double dtheta = mod2pi(this_theta-last_theta);
+ total_theta += dtheta;
+ }
+
+ last_theta = this_theta;
+ }
+
+ int ccw = (total_theta > 0);
+
+ // reverse order if necessary.
+ if (!ccw) {
+ for (int i = 0; i < sz / 2; i++) {
+ double a[2], b[2];
+
+ zarray_get(poly, i, a);
+ zarray_get(poly, sz-1-i, b);
+ zarray_set(poly, i, b, NULL);
+ zarray_set(poly, sz-1-i, a, NULL);
+ }
+ }
+}
+
+int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2])
+{
+ // use winding. If the point is inside the polygon, we'll wrap
+ // around it (accumulating 6.28 radians). If we're outside the
+ // polygon, we'll accumulate zero.
+ int psz = zarray_size(poly);
+
+ double acc_theta = 0;
+
+ double last_theta;
+
+ for (int i = 0; i <= psz; i++) {
+ double p[2];
+
+ zarray_get(poly, i % psz, &p);
+
+ double this_theta = atan2(q[1]-p[1], q[0]-p[0]);
+
+ if (i != 0)
+ acc_theta += mod2pi(this_theta - last_theta);
+
+ last_theta = this_theta;
+ }
+
+ return acc_theta > M_PI;
+}
+
+/*
+// sort by x coordinate, ascending
+static int g2d_convex_hull_sort(const void *_a, const void *_b)
+{
+ double *a = (double*) _a;
+ double *b = (double*) _b;
+
+ if (a[0] < b[0])
+ return -1;
+ if (a[0] == b[0])
+ return 0;
+ return 1;
+}
+*/
+
+/*
+zarray_t *g2d_convex_hull2(const zarray_t *points)
+{
+ zarray_t *hull = zarray_copy(points);
+
+ zarray_sort(hull, g2d_convex_hull_sort);
+
+ int hsz = zarray_size(hull);
+ int hout = 0;
+
+ for (int hin = 1; hin < hsz; hin++) {
+ double *p;
+ zarray_get_volatile(hull, i, &p);
+
+ // Everything to the right of hin is already convex. We now
+ // add one point, p, which begins "connected" by two
+ // (coincident) edges from the last right-most point to p.
+ double *last;
+ zarray_get_volatile(hull, hout, &last);
+
+ // We now remove points from the convex hull by moving
+ }
+
+ return hull;
+}
+*/
+
+// creates and returns a zarray(double[2]). The resulting polygon is
+// CCW and implicitly closed. Unnecessary colinear points are omitted.
+zarray_t *g2d_convex_hull(const zarray_t *points)
+{
+ zarray_t *hull = zarray_create(sizeof(double[2]));
+
+ // gift-wrap algorithm.
+
+ // step 1: find left most point.
+ int insz = zarray_size(points);
+
+ // must have at least 2 points. (XXX need 3?)
+ assert(insz >= 2);
+
+ double *pleft = NULL;
+ for (int i = 0; i < insz; i++) {
+ double *p;
+ zarray_get_volatile(points, i, &p);
+
+ if (pleft == NULL || p[0] < pleft[0])
+ pleft = p;
+ }
+
+ // cannot be NULL since there must be at least one point.
+ assert(pleft != NULL);
+
+ zarray_add(hull, pleft);
+
+ // step 2. gift wrap. Keep searching for points that make the
+ // smallest-angle left-hand turn. This implementation is carefully
+ // written to use only addition/subtraction/multiply. No division
+ // or sqrts. This guarantees exact results for integer-coordinate
+ // polygons (no rounding/precision problems).
+ double *p = pleft;
+
+ while (1) {
+ assert(p != NULL);
+
+ double *q = NULL;
+ double n0 = 0, n1 = 0; // the normal to the line (p, q) (not
+ // necessarily unit length).
+
+ // Search for the point q for which the line (p,q) is most "to
+ // the right of" the other points. (i.e., every time we find a
+ // point that is to the right of our current line, we change
+ // lines.)
+ for (int i = 0; i < insz; i++) {
+ double *thisq;
+ zarray_get_volatile(points, i, &thisq);
+
+ if (thisq == p)
+ continue;
+
+ // the first time we find another point, we initialize our
+ // value of q, forming the line (p,q)
+ if (q == NULL) {
+ q = thisq;
+ n0 = q[1] - p[1];
+ n1 = -q[0] + p[0];
+ } else {
+ // we already have a line (p,q). is point thisq RIGHT OF line (p, q)?
+ double e0 = thisq[0] - p[0], e1 = thisq[1] - p[1];
+ double dot = e0*n0 + e1*n1;
+
+ if (dot > 0) {
+ // it is. change our line.
+ q = thisq;
+ n0 = q[1] - p[1];
+ n1 = -q[0] + p[0];
+ }
+ }
+ }
+
+ // we must have elected *some* line, so long as there are at
+ // least 2 points in the polygon.
+ assert(q != NULL);
+
+ // loop completed?
+ if (q == pleft)
+ break;
+
+ int colinear = 0;
+
+ // is this new point colinear with the last two?
+ if (zarray_size(hull) > 1) {
+ double *o;
+ zarray_get_volatile(hull, zarray_size(hull) - 2, &o);
+
+ double e0 = o[0] - p[0];
+ double e1 = o[1] - p[1];
+
+ if (n0*e0 + n1*e1 == 0)
+ colinear = 1;
+ }
+
+ // if it is colinear, overwrite the last one.
+ if (colinear)
+ zarray_set(hull, zarray_size(hull)-1, q, NULL);
+ else
+ zarray_add(hull, q);
+
+ p = q;
+ }
+
+ return hull;
+}
+
+// Find point p on the boundary of poly that is closest to q.
+void g2d_polygon_closest_boundary_point(const zarray_t *poly, const double q[2], double *p)
+{
+ int psz = zarray_size(poly);
+ double min_dist = HUGE_VALF;
+
+ for (int i = 0; i < psz; i++) {
+ double *p0, *p1;
+
+ zarray_get_volatile(poly, i, &p0);
+ zarray_get_volatile(poly, (i+1) % psz, &p1);
+
+ g2d_line_segment_t seg;
+ g2d_line_segment_init_from_points(&seg, p0, p1);
+
+ double thisp[2];
+ g2d_line_segment_closest_point(&seg, q, thisp);
+
+ double dist = g2d_distance(q, thisp);
+ if (dist < min_dist) {
+ memcpy(p, thisp, sizeof(double[2]));
+ min_dist = dist;
+ }
+ }
+}
+
+int g2d_polygon_contains_point(const zarray_t *poly, double q[2])
+{
+ // use winding. If the point is inside the polygon, we'll wrap
+ // around it (accumulating 6.28 radians). If we're outside the
+ // polygon, we'll accumulate zero.
+ int psz = zarray_size(poly);
+ assert(psz > 0);
+
+ int last_quadrant;
+ int quad_acc = 0;
+
+ for (int i = 0; i <= psz; i++) {
+ double *p;
+
+ zarray_get_volatile(poly, i % psz, &p);
+
+ // p[0] < q[0] p[1] < q[1] quadrant
+ // 0 0 0
+ // 0 1 3
+ // 1 0 1
+ // 1 1 2
+
+ // p[1] < q[1] p[0] < q[0] quadrant
+ // 0 0 0
+ // 0 1 1
+ // 1 0 3
+ // 1 1 2
+
+ int quadrant;
+ if (p[0] < q[0])
+ quadrant = (p[1] < q[1]) ? 2 : 1;
+ else
+ quadrant = (p[1] < q[1]) ? 3 : 0;
+
+ if (i > 0) {
+ int dquadrant = quadrant - last_quadrant;
+
+ // encourage a jump table by mapping to small positive integers.
+ switch (dquadrant) {
+ case -3:
+ case 1:
+ quad_acc ++;
+ break;
+ case -1:
+ case 3:
+ quad_acc --;
+ break;
+ case 0:
+ break;
+ case -2:
+ case 2:
+ {
+ // get the previous point.
+ double *p0;
+ zarray_get_volatile(poly, i-1, &p0);
+
+ // Consider the points p0 and p (the points around the
+ //polygon that we are tracing) and the query point q.
+ //
+ // If we've moved diagonally across quadrants, we want
+ // to measure whether we have rotated +PI radians or
+ // -PI radians. We can test this by computing the dot
+ // product of vector (p0-q) with the vector
+ // perpendicular to vector (p-q)
+ double nx = p[1] - q[1];
+ double ny = -p[0] + q[0];
+
+ double dot = nx*(p0[0]-q[0]) + ny*(p0[1]-q[1]);
+ if (dot < 0)
+ quad_acc -= 2;
+ else
+ quad_acc += 2;
+
+ break;
+ }
+ }
+ }
+
+ last_quadrant = quadrant;
+ }
+
+ int v = (quad_acc >= 2) || (quad_acc <= -2);
+
+ if (0 && v != g2d_polygon_contains_point_ref(poly, q)) {
+ printf("FAILURE %d %d\n", v, quad_acc);
+ exit(-1);
+ }
+
+ return v;
+}
+
+void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2])
+{
+ line->p[0] = p0[0];
+ line->p[1] = p0[1];
+ line->u[0] = p1[0]-p0[0];
+ line->u[1] = p1[1]-p0[1];
+ double mag = sqrtf(sq(line->u[0]) + sq(line->u[1]));
+
+ line->u[0] /= mag;
+ line->u[1] /= mag;
+}
+
+double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2])
+{
+ return (q[0]-line->p[0])*line->u[0] + (q[1]-line->p[1])*line->u[1];
+}
+
+// Compute intersection of two line segments. If they intersect,
+// result is stored in p and 1 is returned. Otherwise, zero is
+// returned. p may be NULL.
+int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p)
+{
+ // this implementation is many times faster than the original,
+ // mostly due to avoiding a general-purpose LU decomposition in
+ // Matrix.inverse().
+ double m00, m01, m10, m11;
+ double i00, i01;
+ double b00, b10;
+
+ m00 = linea->u[0];
+ m01= -lineb->u[0];
+ m10 = linea->u[1];
+ m11= -lineb->u[1];
+
+ // determinant of m
+ double det = m00*m11-m01*m10;
+
+ // parallel lines?
+ if (fabs(det) < 0.00000001)
+ return 0;
+
+ // inverse of m
+ i00 = m11/det;
+ i01 = -m01/det;
+
+ b00 = lineb->p[0] - linea->p[0];
+ b10 = lineb->p[1] - linea->p[1];
+
+ double x00; //, x10;
+ x00 = i00*b00+i01*b10;
+
+ if (p != NULL) {
+ p[0] = linea->u[0]*x00 + linea->p[0];
+ p[1] = linea->u[1]*x00 + linea->p[1];
+ }
+
+ return 1;
+}
+
+
+void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2])
+{
+ g2d_line_init_from_points(&seg->line, p0, p1);
+ seg->p1[0] = p1[0];
+ seg->p1[1] = p1[1];
+}
+
+// Find the point p on segment seg that is closest to point q.
+void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p)
+{
+ double a = g2d_line_get_coordinate(&seg->line, seg->line.p);
+ double b = g2d_line_get_coordinate(&seg->line, seg->p1);
+ double c = g2d_line_get_coordinate(&seg->line, q);
+
+ if (a < b)
+ c = dclamp(c, a, b);
+ else
+ c = dclamp(c, b, a);
+
+ p[0] = seg->line.p[0] + c * seg->line.u[0];
+ p[1] = seg->line.p[1] + c * seg->line.u[1];
+}
+
+// Compute intersection of two line segments. If they intersect,
+// result is stored in p and 1 is returned. Otherwise, zero is
+// returned. p may be NULL.
+int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p)
+{
+ double tmp[2];
+
+ if (!g2d_line_intersect_line(&sega->line, &segb->line, tmp))
+ return 0;
+
+ double a = g2d_line_get_coordinate(&sega->line, sega->line.p);
+ double b = g2d_line_get_coordinate(&sega->line, sega->p1);
+ double c = g2d_line_get_coordinate(&sega->line, tmp);
+
+ // does intersection lie on the first line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ a = g2d_line_get_coordinate(&segb->line, segb->line.p);
+ b = g2d_line_get_coordinate(&segb->line, segb->p1);
+ c = g2d_line_get_coordinate(&segb->line, tmp);
+
+ // does intersection lie on second line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ if (p != NULL) {
+ p[0] = tmp[0];
+ p[1] = tmp[1];
+ }
+
+ return 1;
+}
+
+// Compute intersection of a line segment and a line. If they
+// intersect, result is stored in p and 1 is returned. Otherwise, zero
+// is returned. p may be NULL.
+int g2d_line_segment_intersect_line(const g2d_line_segment_t *seg, const g2d_line_t *line, double *p)
+{
+ double tmp[2];
+
+ if (!g2d_line_intersect_line(&seg->line, line, tmp))
+ return 0;
+
+ double a = g2d_line_get_coordinate(&seg->line, seg->line.p);
+ double b = g2d_line_get_coordinate(&seg->line, seg->p1);
+ double c = g2d_line_get_coordinate(&seg->line, tmp);
+
+ // does intersection lie on the first line?
+ if ((c<a && c<b) || (c>a && c>b))
+ return 0;
+
+ if (p != NULL) {
+ p[0] = tmp[0];
+ p[1] = tmp[1];
+ }
+
+ return 1;
+}
+
+// do the edges of polya and polyb collide? (Does NOT test for containment).
+int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is no.
+
+ // dumb N^2 method.
+ for (int ia = 0; ia < zarray_size(polya); ia++) {
+ double pa0[2], pa1[2];
+ zarray_get(polya, ia, pa0);
+ zarray_get(polya, (ia+1)%zarray_size(polya), pa1);
+
+ g2d_line_segment_t sega;
+ g2d_line_segment_init_from_points(&sega, pa0, pa1);
+
+ for (int ib = 0; ib < zarray_size(polyb); ib++) {
+ double pb0[2], pb1[2];
+ zarray_get(polyb, ib, pb0);
+ zarray_get(polyb, (ib+1)%zarray_size(polyb), pb1);
+
+ g2d_line_segment_t segb;
+ g2d_line_segment_init_from_points(&segb, pb0, pb1);
+
+ if (g2d_line_segment_intersect_segment(&sega, &segb, NULL))
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+// does polya completely contain polyb?
+int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is no.
+ if (g2d_polygon_intersects_polygon(polya, polyb))
+ return 0;
+
+ // if none of the edges cross, then the polygon is either fully
+ // contained or fully outside.
+ double p[2];
+ zarray_get(polyb, 0, p);
+
+ return g2d_polygon_contains_point(polya, p);
+}
+
+// compute a point that is inside the polygon. (It may not be *far* inside though)
+void g2d_polygon_get_interior_point(const zarray_t *poly, double *p)
+{
+ // take the first three points, which form a triangle. Find the middle point
+ double a[2], b[2], c[2];
+
+ zarray_get(poly, 0, a);
+ zarray_get(poly, 1, b);
+ zarray_get(poly, 2, c);
+
+ p[0] = (a[0]+b[0]+c[0])/3;
+ p[1] = (a[1]+b[1]+c[1])/3;
+}
+
+int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb)
+{
+ // do any of the line segments collide? If so, the answer is yes.
+ if (g2d_polygon_intersects_polygon(polya, polyb))
+ return 1;
+
+ // if none of the edges cross, then the polygon is either fully
+ // contained or fully outside.
+ double p[2];
+ g2d_polygon_get_interior_point(polyb, p);
+
+ if (g2d_polygon_contains_point(polya, p))
+ return 1;
+
+ g2d_polygon_get_interior_point(polya, p);
+
+ if (g2d_polygon_contains_point(polyb, p))
+ return 1;
+
+ return 0;
+}
+
+static int double_sort_up(const void *_a, const void *_b)
+{
+ double a = *((double*) _a);
+ double b = *((double*) _b);
+
+ if (a < b)
+ return -1;
+
+ if (a == b)
+ return 0;
+
+ return 1;
+}
+
+// Compute the crossings of the polygon along line y, storing them in
+// the array x. X must be allocated to be at least as long as
+// zarray_size(poly). X will be sorted, ready for
+// rasterization. Returns the number of intersections (and elements
+// written to x).
+/*
+ To rasterize, do something like this:
+
+ double res = 0.099;
+ for (double y = y0; y < y1; y += res) {
+ double xs[zarray_size(poly)];
+
+ int xsz = g2d_polygon_rasterize(poly, y, xs);
+ int xpos = 0;
+ int inout = 0; // start off "out"
+
+ for (double x = x0; x < x1; x += res) {
+ while (x > xs[xpos] && xpos < xsz) {
+ xpos++;
+ inout ^= 1;
+ }
+
+ if (inout)
+ printf("y");
+ else
+ printf(" ");
+ }
+ printf("\n");
+*/
+
+// returns the number of x intercepts
+int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x)
+{
+ int sz = zarray_size(poly);
+
+ g2d_line_t line;
+ if (1) {
+ double p0[2] = { 0, y };
+ double p1[2] = { 1, y };
+
+ g2d_line_init_from_points(&line, p0, p1);
+ }
+
+ int xpos = 0;
+
+ for (int i = 0; i < sz; i++) {
+ g2d_line_segment_t seg;
+ double *p0, *p1;
+ zarray_get_volatile(poly, i, &p0);
+ zarray_get_volatile(poly, (i+1)%sz, &p1);
+
+ g2d_line_segment_init_from_points(&seg, p0, p1);
+
+ double q[2];
+ if (g2d_line_segment_intersect_line(&seg, &line, q))
+ x[xpos++] = q[0];
+ }
+
+ qsort(x, xpos, sizeof(double), double_sort_up);
+
+ return xpos;
+}
+
+/*
+ /---(1,5)
+ (-2,4)-/ |
+ \ |
+ \ (1,2)--(2,2)\
+ \ \
+ \ \
+ (0,0)------------------(4,0)
+*/
+#if 0
+
+#include "timeprofile.h"
+
+int main(int argc, char *argv[])
+{
+ timeprofile_t *tp = timeprofile_create();
+
+ zarray_t *polya = g2d_polygon_create_data((double[][2]) {
+ { 0, 0},
+ { 4, 0},
+ { 2, 2},
+ { 1, 2},
+ { 1, 5},
+ { -2,4} }, 6);
+
+ zarray_t *polyb = g2d_polygon_create_data((double[][2]) {
+ { .1, .1},
+ { .5, .1},
+ { .1, .5 } }, 3);
+
+ zarray_t *polyc = g2d_polygon_create_data((double[][2]) {
+ { 3, 0},
+ { 5, 0},
+ { 5, 1} }, 3);
+
+ zarray_t *polyd = g2d_polygon_create_data((double[][2]) {
+ { 5, 5},
+ { 6, 6},
+ { 5, 6} }, 3);
+
+/*
+ 5 L---K
+ 4 |I--J
+ 3 |H-G
+ 2 |E-F
+ 1 |D--C
+ 0 A---B
+ 01234
+*/
+ zarray_t *polyE = g2d_polygon_create_data((double[][2]) {
+ {0,0}, {4,0}, {4, 1}, {1,1},
+ {1,2}, {3,2}, {3,3}, {1,3},
+ {1,4}, {4,4}, {4,5}, {0,5}}, 12);
+
+ srand(0);
+
+ timeprofile_stamp(tp, "begin");
+
+ if (1) {
+ int niters = 100000;
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ g2d_polygon_contains_point(polyE, q);
+ }
+
+ timeprofile_stamp(tp, "fast");
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ g2d_polygon_contains_point_ref(polyE, q);
+ }
+
+ timeprofile_stamp(tp, "slow");
+
+ for (int i = 0; i < niters; i++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ int v0 = g2d_polygon_contains_point(polyE, q);
+ int v1 = g2d_polygon_contains_point_ref(polyE, q);
+ assert(v0 == v1);
+ }
+
+ timeprofile_stamp(tp, "both");
+ timeprofile_display(tp);
+ }
+
+ if (1) {
+ zarray_t *poly = polyE;
+
+ double res = 0.399;
+ for (double y = 5.2; y >= -.5; y -= res) {
+ double xs[zarray_size(poly)];
+
+ int xsz = g2d_polygon_rasterize(poly, y, xs);
+ int xpos = 0;
+ int inout = 0; // start off "out"
+ for (double x = -3; x < 6; x += res) {
+ while (x > xs[xpos] && xpos < xsz) {
+ xpos++;
+ inout ^= 1;
+ }
+
+ if (inout)
+ printf("y");
+ else
+ printf(" ");
+ }
+ printf("\n");
+
+ for (double x = -3; x < 6; x += res) {
+ double q[2] = {x, y};
+ if (g2d_polygon_contains_point(poly, q))
+ printf("X");
+ else
+ printf(" ");
+ }
+ printf("\n");
+ }
+ }
+
+
+
+/*
+// CW order
+double p[][2] = { { 0, 0},
+{ -2, 4},
+{1, 5},
+{1, 2},
+{2, 2},
+{4, 0} };
+*/
+
+ double q[2] = { 10, 10 };
+ printf("0==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 1; q[1] = 1;
+ printf("1==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 3; q[1] = .5;
+ printf("1==%d\n", g2d_polygon_contains_point(polya, q));
+
+ q[0] = 1.2; q[1] = 2.1;
+ printf("0==%d\n", g2d_polygon_contains_point(polya, q));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyb));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyc));
+
+ printf("0==%d\n", g2d_polygon_contains_polygon(polya, polyd));
+
+ ////////////////////////////////////////////////////////
+ // Test convex hull
+ if (1) {
+ zarray_t *hull = g2d_convex_hull(polyE);
+
+ for (int k = 0; k < zarray_size(hull); k++) {
+ double *h;
+ zarray_get_volatile(hull, k, &h);
+
+ printf("%15f, %15f\n", h[0], h[1]);
+ }
+ }
+
+ for (int i = 0; i < 100000; i++) {
+ zarray_t *points = zarray_create(sizeof(double[2]));
+
+ for (int j = 0; j < 100; j++) {
+ double q[2];
+ q[0] = 10.0f * random() / RAND_MAX - 2;
+ q[1] = 10.0f * random() / RAND_MAX - 2;
+
+ zarray_add(points, q);
+ }
+
+ zarray_t *hull = g2d_convex_hull(points);
+ for (int j = 0; j < zarray_size(points); j++) {
+ double *q;
+ zarray_get_volatile(points, j, &q);
+
+ int on_edge;
+
+ double p[2];
+ g2d_polygon_closest_boundary_point(hull, q, p);
+ if (g2d_distance(q, p) < .00001)
+ on_edge = 1;
+
+ assert(on_edge || g2d_polygon_contains_point(hull, q));
+ }
+
+ zarray_destroy(hull);
+ zarray_destroy(points);
+ }
+}
+#endif
diff --git a/3rdparty/apriltag/common/g2d.h b/3rdparty/apriltag/common/g2d.h
new file mode 100644
index 0000000..40182be
--- /dev/null
+++ b/3rdparty/apriltag/common/g2d.h
@@ -0,0 +1,132 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _G2D_H
+#define _G2D_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "common/zarray.h"
+
+// This library tries to avoid needless proliferation of types.
+//
+// A point is a double[2]. (Note that when passing a double[2] as an
+// argument, it is passed by pointer, not by value.)
+//
+// A polygon is a zarray_t of double[2]. (Note that in this case, the
+// zarray contains the actual vertex data, and not merely a pointer to
+// some other data. IMPORTANT: A polygon must be specified in CCW
+// order. It is implicitly closed (do not list the same point at the
+// beginning at the end.
+//
+// Where sensible, it is assumed that objects should be allocated
+// sparingly; consequently "init" style methods, rather than "create"
+// methods are used.
+
+////////////////////////////////////////////////////////////////////
+// Lines
+
+typedef struct
+{
+ // Internal representation: a point that the line goes through (p) and
+ // the direction of the line (u).
+ double p[2];
+ double u[2]; // always a unit vector
+} g2d_line_t;
+
+// initialize a line object.
+void g2d_line_init_from_points(g2d_line_t *line, const double p0[2], const double p1[2]);
+
+// The line defines a one-dimensional coordinate system whose origin
+// is p. Where is q? (If q is not on the line, the point nearest q is
+// returned.
+double g2d_line_get_coordinate(const g2d_line_t *line, const double q[2]);
+
+// Intersect two lines. The intersection, if it exists, is written to
+// p (if not NULL), and 1 is returned. Else, zero is returned.
+int g2d_line_intersect_line(const g2d_line_t *linea, const g2d_line_t *lineb, double *p);
+
+////////////////////////////////////////////////////////////////////
+// Line Segments. line.p is always one endpoint; p1 is the other
+// endpoint.
+typedef struct
+{
+ g2d_line_t line;
+ double p1[2];
+} g2d_line_segment_t;
+
+void g2d_line_segment_init_from_points(g2d_line_segment_t *seg, const double p0[2], const double p1[2]);
+
+// Intersect two segments. The intersection, if it exists, is written
+// to p (if not NULL), and 1 is returned. Else, zero is returned.
+int g2d_line_segment_intersect_segment(const g2d_line_segment_t *sega, const g2d_line_segment_t *segb, double *p);
+
+void g2d_line_segment_closest_point(const g2d_line_segment_t *seg, const double *q, double *p);
+double g2d_line_segment_closest_point_distance(const g2d_line_segment_t *seg, const double *q);
+
+////////////////////////////////////////////////////////////////////
+// Polygons
+
+zarray_t *g2d_polygon_create_data(double v[][2], int sz);
+
+zarray_t *g2d_polygon_create_zeros(int sz);
+
+zarray_t *g2d_polygon_create_empty();
+
+void g2d_polygon_add(zarray_t *poly, double v[2]);
+
+// Takes a polygon in either CW or CCW and modifies it (if necessary)
+// to be CCW.
+void g2d_polygon_make_ccw(zarray_t *poly);
+
+// Return 1 if point q lies within poly.
+int g2d_polygon_contains_point(const zarray_t *poly, double q[2]);
+
+// Do the edges of the polygons cross? (Does not test for containment).
+int g2d_polygon_intersects_polygon(const zarray_t *polya, const zarray_t *polyb);
+
+// Does polya completely contain polyb?
+int g2d_polygon_contains_polygon(const zarray_t *polya, const zarray_t *polyb);
+
+// Is there some point which is in both polya and polyb?
+int g2d_polygon_overlaps_polygon(const zarray_t *polya, const zarray_t *polyb);
+
+// returns the number of points written to x. see comments.
+int g2d_polygon_rasterize(const zarray_t *poly, double y, double *x);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/getopt.c b/3rdparty/apriltag/common/getopt.c
new file mode 100644
index 0000000..97bcc33
--- /dev/null
+++ b/3rdparty/apriltag/common/getopt.c
@@ -0,0 +1,553 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include <ctype.h>
+#include <errno.h>
+
+#include "zhash.h"
+#include "zarray.h"
+#include "getopt.h"
+#include "common/math_util.h"
+
+#define GOO_BOOL_TYPE 1
+#define GOO_STRING_TYPE 2
+
+typedef struct getopt_option getopt_option_t;
+
+struct getopt_option
+{
+ char *sname;
+ char *lname;
+ char *svalue;
+
+ char *help;
+ int type;
+
+ int spacer;
+
+ int was_specified;
+};
+
+struct getopt
+{
+ zhash_t *lopts;
+ zhash_t *sopts;
+ zarray_t *extraargs;
+ zarray_t *options;
+};
+
+getopt_t *getopt_create()
+{
+ getopt_t *gopt = (getopt_t*) calloc(1, sizeof(getopt_t));
+
+ gopt->lopts = zhash_create(sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals);
+ gopt->sopts = zhash_create(sizeof(char*), sizeof(getopt_option_t*), zhash_str_hash, zhash_str_equals);
+ gopt->options = zarray_create(sizeof(getopt_option_t*));
+ gopt->extraargs = zarray_create(sizeof(char*));
+
+ return gopt;
+}
+
+void getopt_option_destroy(getopt_option_t *goo)
+{
+ free(goo->sname);
+ free(goo->lname);
+ free(goo->svalue);
+ free(goo->help);
+ memset(goo, 0, sizeof(getopt_option_t));
+ free(goo);
+}
+
+void getopt_destroy(getopt_t *gopt)
+{
+ // free the extra arguments and container
+ zarray_vmap(gopt->extraargs, free);
+ zarray_destroy(gopt->extraargs);
+
+ // deep free of the getopt_option structs. Also frees key/values, so
+ // after this loop, hash tables will no longer work
+ zarray_vmap(gopt->options, getopt_option_destroy);
+ zarray_destroy(gopt->options);
+
+ // free tables
+ zhash_destroy(gopt->lopts);
+ zhash_destroy(gopt->sopts);
+
+ memset(gopt, 0, sizeof(getopt_t));
+ free(gopt);
+}
+
+static void getopt_modify_string(char **str, char *newvalue)
+{
+ char *old = *str;
+ *str = newvalue;
+ if (old != NULL)
+ free(old);
+}
+
+static char *get_arg_assignment(char *arg)
+{
+ // not an arg starting with "--"?
+ if (!str_starts_with(arg, "--")) {
+ return NULL;
+ }
+
+ int eq_index = str_indexof(arg, "=");
+
+ // no assignment?
+ if (eq_index == -1) {
+ return NULL;
+ }
+
+ // no quotes allowed before '=' in "--key=value" option specification.
+ // quotes can be used in value string, or by extra arguments
+ for (int i = 0; i < eq_index; i++) {
+ if (arg[i] == '\'' || arg[i] == '"') {
+ return NULL;
+ }
+ }
+
+ return &arg[eq_index];
+}
+
+// returns 1 if no error
+int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors)
+{
+ int okay = 1;
+ zarray_t *toks = zarray_create(sizeof(char*));
+
+ // take the input stream and chop it up into tokens
+ for (int i = 1; i < argc; i++) {
+
+ char *arg = strdup(argv[i]);
+ char *eq = get_arg_assignment(arg);
+
+ // no equal sign? Push the whole thing.
+ if (eq == NULL) {
+ zarray_add(toks, &arg);
+ } else {
+ // there was an equal sign. Push the part
+ // before and after the equal sign
+ char *val = strdup(&eq[1]);
+ eq[0] = 0;
+ zarray_add(toks, &arg);
+
+ // if the part after the equal sign is
+ // enclosed by quotation marks, strip them.
+ if (val[0]=='\"') {
+ size_t last = strlen(val) - 1;
+ if (val[last]=='\"')
+ val[last] = 0;
+ char *valclean = strdup(&val[1]);
+ zarray_add(toks, &valclean);
+ free(val);
+ } else {
+ zarray_add(toks, &val);
+ }
+ }
+ }
+
+ // now loop over the elements and evaluate the arguments
+ unsigned int i = 0;
+
+ char *tok = NULL;
+
+ while (i < zarray_size(toks)) {
+
+ // rather than free statement throughout this while loop
+ if (tok != NULL)
+ free(tok);
+
+ zarray_get(toks, i, &tok);
+
+ if (!strncmp(tok,"--", 2)) {
+ char *optname = &tok[2];
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->lopts, &optname, &goo);
+ if (goo == NULL) {
+ okay = 0;
+ if (showErrors)
+ printf("Unknown option --%s\n", optname);
+ i++;
+ continue;
+ }
+
+ goo->was_specified = 1;
+
+ if (goo->type == GOO_BOOL_TYPE) {
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+
+ if (!strcmp(val,"true")) {
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+ if (!strcmp(val,"false")) {
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+ }
+ getopt_modify_string(&goo->svalue, strdup("true"));
+ i++;
+ continue;
+ }
+
+ if (goo->type == GOO_STRING_TYPE) {
+ // TODO: check whether next argument is an option, denoting missing argument
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+ i+=2;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+
+ okay = 0;
+ if (showErrors)
+ printf("Option %s requires a string argument.\n",optname);
+ }
+ }
+
+ if (!strncmp(tok,"-",1) && strncmp(tok,"--",2)) {
+ size_t len = strlen(tok);
+ int pos;
+ for (pos = 1; pos < len; pos++) {
+ char sopt[2];
+ sopt[0] = tok[pos];
+ sopt[1] = 0;
+ char *sopt_ptr = (char*) &sopt;
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->sopts, &sopt_ptr, &goo);
+
+ if (goo==NULL) {
+ // is the argument a numerical literal that happens to be negative?
+ if (pos==1 && isdigit(tok[pos])) {
+ zarray_add(gopt->extraargs, &tok);
+ tok = NULL;
+ break;
+ } else {
+ okay = 0;
+ if (showErrors)
+ printf("Unknown option -%c\n", tok[pos]);
+ i++;
+ continue;
+ }
+ }
+
+ goo->was_specified = 1;
+
+ if (goo->type == GOO_BOOL_TYPE) {
+ getopt_modify_string(&goo->svalue, strdup("true"));
+ continue;
+ }
+
+ if (goo->type == GOO_STRING_TYPE) {
+ if ((i+1) < zarray_size(toks)) {
+ char *val = NULL;
+ zarray_get(toks, i+1, &val);
+ // TODO: allow negative numerical values for short-name options ?
+ if (val[0]=='-')
+ {
+ okay = 0;
+ if (showErrors)
+ printf("Ran out of arguments for option block %s\n", tok);
+ }
+ i++;
+ getopt_modify_string(&goo->svalue, val);
+ continue;
+ }
+
+ okay = 0;
+ if (showErrors)
+ printf("Option -%c requires a string argument.\n", tok[pos]);
+ }
+ }
+ i++;
+ continue;
+ }
+
+ // it's not an option-- it's an argument.
+ zarray_add(gopt->extraargs, &tok);
+ tok = NULL;
+ i++;
+ }
+ if (tok != NULL)
+ free(tok);
+
+ zarray_destroy(toks);
+
+ return okay;
+}
+
+void getopt_add_spacer(getopt_t *gopt, const char *s)
+{
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->spacer = 1;
+ goo->help = strdup(s);
+ zarray_add(gopt->options, &goo);
+}
+
+void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help)
+{
+ char sname[2];
+ sname[0] = sopt;
+ sname[1] = 0;
+ char *sname_ptr = (char*) &sname;
+
+ if (strlen(lname) < 1) { // must have long name
+ fprintf (stderr, "getopt_add_bool(): must supply option name\n");
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt == '-') { // short name cannot be '-' (no way to reference)
+ fprintf (stderr, "getopt_add_bool(): invalid option character: '%c'\n", sopt);
+ exit (EXIT_FAILURE);
+ }
+
+ if (zhash_contains(gopt->lopts, &lname)) {
+ fprintf (stderr, "getopt_add_bool(): duplicate option name: --%s\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
+ fprintf (stderr, "getopt_add_bool(): duplicate option: -%s ('%s')\n", sname, lname);
+ exit (EXIT_FAILURE);
+ }
+
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->sname=strdup(sname);
+ goo->lname=strdup(lname);
+ goo->svalue=strdup(def ? "true" : "false");
+ goo->type=GOO_BOOL_TYPE;
+ goo->help=strdup(help);
+
+ zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
+ zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
+ zarray_add(gopt->options, &goo);
+}
+
+void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ getopt_add_string(gopt, sopt, lname, def, help);
+}
+
+void
+getopt_add_double (getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ getopt_add_string (gopt, sopt, lname, def, help);
+}
+
+void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help)
+{
+ char sname[2];
+ sname[0] = sopt;
+ sname[1] = 0;
+ char *sname_ptr = (char*) &sname;
+
+ if (strlen(lname) < 1) { // must have long name
+ fprintf (stderr, "getopt_add_string(): must supply option name\n");
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt == '-') { // short name cannot be '-' (no way to reference)
+ fprintf (stderr, "getopt_add_string(): invalid option character: '%c'\n", sopt);
+ exit (EXIT_FAILURE);
+ }
+
+ if (zhash_contains(gopt->lopts, &lname)) {
+ fprintf (stderr, "getopt_add_string(): duplicate option name: --%s\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ if (sopt != '\0' && zhash_contains(gopt->sopts, &sname_ptr)) {
+ fprintf (stderr, "getopt_add_string(): duplicate option: -%s ('%s')\n", sname, lname);
+ exit (EXIT_FAILURE);
+ }
+
+ getopt_option_t *goo = (getopt_option_t*) calloc(1, sizeof(getopt_option_t));
+ goo->sname=strdup(sname);
+ goo->lname=strdup(lname);
+ goo->svalue=strdup(def);
+ goo->type=GOO_STRING_TYPE;
+ goo->help=strdup(help);
+
+ zhash_put(gopt->lopts, &goo->lname, &goo, NULL, NULL);
+ zhash_put(gopt->sopts, &goo->sname, &goo, NULL, NULL);
+ zarray_add(gopt->options, &goo);
+}
+
+const char *getopt_get_string(getopt_t *gopt, const char *lname)
+{
+ getopt_option_t *goo = NULL;
+ zhash_get(gopt->lopts, &lname, &goo);
+ // could return null, but this would be the only
+ // method that doesn't assert on a missing key
+ assert (goo != NULL);
+ return goo->svalue;
+}
+
+int getopt_get_int(getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string(getopt, lname);
+ assert(v != NULL);
+
+ errno = 0;
+ char *endptr = (char *) v;
+ long val = strtol(v, &endptr, 10);
+
+ if (errno != 0) {
+ fprintf (stderr, "--%s argument: strtol failed: %s\n", lname, strerror(errno));
+ exit (EXIT_FAILURE);
+ }
+
+ if (endptr == v) {
+ fprintf (stderr, "--%s argument cannot be parsed as an int\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ return (int) val;
+}
+
+int getopt_get_bool(getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string(getopt, lname);
+ assert (v!=NULL);
+ int val = !strcmp(v, "true");
+ return val;
+}
+
+double getopt_get_double (getopt_t *getopt, const char *lname)
+{
+ const char *v = getopt_get_string (getopt, lname);
+ assert (v!=NULL);
+
+ errno = 0;
+ char *endptr = (char *) v;
+ double d = strtod (v, &endptr);
+
+ if (errno != 0) {
+ fprintf (stderr, "--%s argument: strtod failed: %s\n", lname, strerror(errno));
+ exit (EXIT_FAILURE);
+ }
+
+ if (endptr == v) {
+ fprintf (stderr, "--%s argument cannot be parsed as a double\n", lname);
+ exit (EXIT_FAILURE);
+ }
+
+ return d;
+}
+
+int getopt_was_specified(getopt_t *getopt, const char *lname)
+{
+ getopt_option_t *goo = NULL;
+ zhash_get(getopt->lopts, &lname, &goo);
+ if (goo == NULL)
+ return 0;
+
+ return goo->was_specified;
+}
+
+const zarray_t *getopt_get_extra_args(getopt_t *gopt)
+{
+ return gopt->extraargs;
+}
+
+void getopt_do_usage(getopt_t * gopt)
+{
+ char * usage = getopt_get_usage(gopt);
+ printf("%s", usage);
+ free(usage);
+}
+
+char * getopt_get_usage(getopt_t *gopt)
+{
+ string_buffer_t * sb = string_buffer_create();
+
+ int leftmargin=2;
+ int longwidth=12;
+ int valuewidth=10;
+
+ for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
+ getopt_option_t *goo = NULL;
+ zarray_get(gopt->options, i, &goo);
+
+ if (goo->spacer)
+ continue;
+
+ longwidth = max(longwidth, (int) strlen(goo->lname));
+
+ if (goo->type == GOO_STRING_TYPE)
+ valuewidth = max(valuewidth, (int) strlen(goo->svalue));
+ }
+
+ for (unsigned int i = 0; i < zarray_size(gopt->options); i++) {
+ getopt_option_t *goo = NULL;
+ zarray_get(gopt->options, i, &goo);
+
+ if (goo->spacer)
+ {
+ if (goo->help==NULL || strlen(goo->help)==0)
+ string_buffer_appendf(sb,"\n");
+ else
+ string_buffer_appendf(sb,"\n%*s%s\n\n", leftmargin, "", goo->help);
+ continue;
+ }
+
+ string_buffer_appendf(sb,"%*s", leftmargin, "");
+
+ if (goo->sname[0]==0)
+ string_buffer_appendf(sb," ");
+ else
+ string_buffer_appendf(sb,"-%c | ", goo->sname[0]);
+
+ string_buffer_appendf(sb,"--%*s ", -longwidth, goo->lname);
+
+ string_buffer_appendf(sb," [ %s ]", goo->svalue); // XXX: displays current value rather than default value
+
+ string_buffer_appendf(sb,"%*s", (int) (valuewidth-strlen(goo->svalue)), "");
+
+ string_buffer_appendf(sb," %s ", goo->help);
+ string_buffer_appendf(sb,"\n");
+ }
+
+ char * usage = string_buffer_to_string(sb);
+ string_buffer_destroy(sb);
+ return usage;
+}
diff --git a/3rdparty/apriltag/common/getopt.h b/3rdparty/apriltag/common/getopt.h
new file mode 100644
index 0000000..3ca77e9
--- /dev/null
+++ b/3rdparty/apriltag/common/getopt.h
@@ -0,0 +1,73 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _GETOPT_H
+#define _GETOPT_H
+
+#include "zarray.h"
+#include "string_util.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct getopt getopt_t;
+
+getopt_t *getopt_create();
+void getopt_destroy(getopt_t *gopt);
+
+// Parse args. Returns 1 on success
+int getopt_parse(getopt_t *gopt, int argc, char *argv[], int showErrors);
+void getopt_do_usage(getopt_t *gopt);
+
+// Returns a string containing the usage. Must be freed by caller
+char * getopt_get_usage(getopt_t *gopt);
+
+void getopt_add_spacer(getopt_t *gopt, const char *s);
+void getopt_add_bool(getopt_t *gopt, char sopt, const char *lname, int def, const char *help);
+void getopt_add_int(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
+void getopt_add_string(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
+void getopt_add_double(getopt_t *gopt, char sopt, const char *lname, const char *def, const char *help);
+
+const char *getopt_get_string(getopt_t *gopt, const char *lname);
+int getopt_get_int(getopt_t *getopt, const char *lname);
+int getopt_get_bool(getopt_t *getopt, const char *lname);
+double getopt_get_double(getopt_t *getopt, const char *lname);
+int getopt_was_specified(getopt_t *gopt, const char *lname);
+const zarray_t *getopt_get_extra_args(getopt_t *gopt);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif
diff --git a/3rdparty/apriltag/common/homography.c b/3rdparty/apriltag/common/homography.c
new file mode 100644
index 0000000..8b89ab7
--- /dev/null
+++ b/3rdparty/apriltag/common/homography.c
@@ -0,0 +1,485 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <math.h>
+#include <stdio.h>
+
+#include "common/matd.h"
+#include "common/zarray.h"
+#include "common/homography.h"
+#include "common/math_util.h"
+
+// correspondences is a list of float[4]s, consisting of the points x
+// and y concatenated. We will compute a homography such that y = Hx
+matd_t *homography_compute(zarray_t *correspondences, int flags)
+{
+ // compute centroids of both sets of points (yields a better
+ // conditioned information matrix)
+ double x_cx = 0, x_cy = 0;
+ double y_cx = 0, y_cy = 0;
+
+ for (int i = 0; i < zarray_size(correspondences); i++) {
+ float *c;
+ zarray_get_volatile(correspondences, i, &c);
+
+ x_cx += c[0];
+ x_cy += c[1];
+ y_cx += c[2];
+ y_cy += c[3];
+ }
+
+ int sz = zarray_size(correspondences);
+ x_cx /= sz;
+ x_cy /= sz;
+ y_cx /= sz;
+ y_cy /= sz;
+
+ // NB We don't normalize scale; it seems implausible that it could
+ // possibly make any difference given the dynamic range of IEEE
+ // doubles.
+
+ matd_t *A = matd_create(9,9);
+ for (int i = 0; i < zarray_size(correspondences); i++) {
+ float *c;
+ zarray_get_volatile(correspondences, i, &c);
+
+ // (below world is "x", and image is "y")
+ double worldx = c[0] - x_cx;
+ double worldy = c[1] - x_cy;
+ double imagex = c[2] - y_cx;
+ double imagey = c[3] - y_cy;
+
+ double a03 = -worldx;
+ double a04 = -worldy;
+ double a05 = -1;
+ double a06 = worldx*imagey;
+ double a07 = worldy*imagey;
+ double a08 = imagey;
+
+ MATD_EL(A, 3, 3) += a03*a03;
+ MATD_EL(A, 3, 4) += a03*a04;
+ MATD_EL(A, 3, 5) += a03*a05;
+ MATD_EL(A, 3, 6) += a03*a06;
+ MATD_EL(A, 3, 7) += a03*a07;
+ MATD_EL(A, 3, 8) += a03*a08;
+ MATD_EL(A, 4, 4) += a04*a04;
+ MATD_EL(A, 4, 5) += a04*a05;
+ MATD_EL(A, 4, 6) += a04*a06;
+ MATD_EL(A, 4, 7) += a04*a07;
+ MATD_EL(A, 4, 8) += a04*a08;
+ MATD_EL(A, 5, 5) += a05*a05;
+ MATD_EL(A, 5, 6) += a05*a06;
+ MATD_EL(A, 5, 7) += a05*a07;
+ MATD_EL(A, 5, 8) += a05*a08;
+ MATD_EL(A, 6, 6) += a06*a06;
+ MATD_EL(A, 6, 7) += a06*a07;
+ MATD_EL(A, 6, 8) += a06*a08;
+ MATD_EL(A, 7, 7) += a07*a07;
+ MATD_EL(A, 7, 8) += a07*a08;
+ MATD_EL(A, 8, 8) += a08*a08;
+
+ double a10 = worldx;
+ double a11 = worldy;
+ double a12 = 1;
+ double a16 = -worldx*imagex;
+ double a17 = -worldy*imagex;
+ double a18 = -imagex;
+
+ MATD_EL(A, 0, 0) += a10*a10;
+ MATD_EL(A, 0, 1) += a10*a11;
+ MATD_EL(A, 0, 2) += a10*a12;
+ MATD_EL(A, 0, 6) += a10*a16;
+ MATD_EL(A, 0, 7) += a10*a17;
+ MATD_EL(A, 0, 8) += a10*a18;
+ MATD_EL(A, 1, 1) += a11*a11;
+ MATD_EL(A, 1, 2) += a11*a12;
+ MATD_EL(A, 1, 6) += a11*a16;
+ MATD_EL(A, 1, 7) += a11*a17;
+ MATD_EL(A, 1, 8) += a11*a18;
+ MATD_EL(A, 2, 2) += a12*a12;
+ MATD_EL(A, 2, 6) += a12*a16;
+ MATD_EL(A, 2, 7) += a12*a17;
+ MATD_EL(A, 2, 8) += a12*a18;
+ MATD_EL(A, 6, 6) += a16*a16;
+ MATD_EL(A, 6, 7) += a16*a17;
+ MATD_EL(A, 6, 8) += a16*a18;
+ MATD_EL(A, 7, 7) += a17*a17;
+ MATD_EL(A, 7, 8) += a17*a18;
+ MATD_EL(A, 8, 8) += a18*a18;
+
+ double a20 = -worldx*imagey;
+ double a21 = -worldy*imagey;
+ double a22 = -imagey;
+ double a23 = worldx*imagex;
+ double a24 = worldy*imagex;
+ double a25 = imagex;
+
+ MATD_EL(A, 0, 0) += a20*a20;
+ MATD_EL(A, 0, 1) += a20*a21;
+ MATD_EL(A, 0, 2) += a20*a22;
+ MATD_EL(A, 0, 3) += a20*a23;
+ MATD_EL(A, 0, 4) += a20*a24;
+ MATD_EL(A, 0, 5) += a20*a25;
+ MATD_EL(A, 1, 1) += a21*a21;
+ MATD_EL(A, 1, 2) += a21*a22;
+ MATD_EL(A, 1, 3) += a21*a23;
+ MATD_EL(A, 1, 4) += a21*a24;
+ MATD_EL(A, 1, 5) += a21*a25;
+ MATD_EL(A, 2, 2) += a22*a22;
+ MATD_EL(A, 2, 3) += a22*a23;
+ MATD_EL(A, 2, 4) += a22*a24;
+ MATD_EL(A, 2, 5) += a22*a25;
+ MATD_EL(A, 3, 3) += a23*a23;
+ MATD_EL(A, 3, 4) += a23*a24;
+ MATD_EL(A, 3, 5) += a23*a25;
+ MATD_EL(A, 4, 4) += a24*a24;
+ MATD_EL(A, 4, 5) += a24*a25;
+ MATD_EL(A, 5, 5) += a25*a25;
+ }
+
+ // make symmetric
+ for (int i = 0; i < 9; i++)
+ for (int j = i+1; j < 9; j++)
+ MATD_EL(A, j, i) = MATD_EL(A, i, j);
+
+ matd_t *H = matd_create(3,3);
+
+ if (flags & HOMOGRAPHY_COMPUTE_FLAG_INVERSE) {
+ // compute singular vector by (carefully) inverting the rank-deficient matrix.
+
+ if (1) {
+ matd_t *Ainv = matd_inverse(A);
+ double scale = 0;
+
+ for (int i = 0; i < 9; i++)
+ scale += sq(MATD_EL(Ainv, i, 0));
+ scale = sqrt(scale);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
+
+ matd_destroy(Ainv);
+ } else {
+
+ matd_t *b = matd_create_data(9, 1, (double[]) { 1, 0, 0, 0, 0, 0, 0, 0, 0 });
+ matd_t *Ainv = NULL;
+
+ if (0) {
+ matd_plu_t *lu = matd_plu(A);
+ Ainv = matd_plu_solve(lu, b);
+ matd_plu_destroy(lu);
+ } else {
+ matd_chol_t *chol = matd_chol(A);
+ Ainv = matd_chol_solve(chol, b);
+ matd_chol_destroy(chol);
+ }
+
+ double scale = 0;
+
+ for (int i = 0; i < 9; i++)
+ scale += sq(MATD_EL(Ainv, i, 0));
+ scale = sqrt(scale);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(Ainv, 3*i+j, 0) / scale;
+
+ matd_destroy(b);
+ matd_destroy(Ainv);
+ }
+
+ } else {
+ // compute singular vector using SVD. A bit slower, but more accurate.
+ matd_svd_t svd = matd_svd_flags(A, MATD_SVD_NO_WARNINGS);
+
+ for (int i = 0; i < 3; i++)
+ for (int j = 0; j < 3; j++)
+ MATD_EL(H, i, j) = MATD_EL(svd.U, 3*i+j, 8);
+
+ matd_destroy(svd.U);
+ matd_destroy(svd.S);
+ matd_destroy(svd.V);
+
+ }
+
+ matd_t *Tx = matd_identity(3);
+ MATD_EL(Tx,0,2) = -x_cx;
+ MATD_EL(Tx,1,2) = -x_cy;
+
+ matd_t *Ty = matd_identity(3);
+ MATD_EL(Ty,0,2) = y_cx;
+ MATD_EL(Ty,1,2) = y_cy;
+
+ matd_t *H2 = matd_op("M*M*M", Ty, H, Tx);
+
+ matd_destroy(A);
+ matd_destroy(Tx);
+ matd_destroy(Ty);
+ matd_destroy(H);
+
+ return H2;
+}
+
+
+// assuming that the projection matrix is:
+// [ fx 0 cx 0 ]
+// [ 0 fy cy 0 ]
+// [ 0 0 1 0 ]
+//
+// And that the homography is equal to the projection matrix times the
+// model matrix, recover the model matrix (which is returned). Note
+// that the third column of the model matrix is missing in the
+// expression below, reflecting the fact that the homography assumes
+// all points are at z=0 (i.e., planar) and that the element of z is
+// thus omitted. (3x1 instead of 4x1).
+//
+// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
+// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
+// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
+// [ 0 0 1 ]
+//
+// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
+// fx*R01 + cx*R21 = H01
+// fx*TX + cx*TZ = H02
+// fy*R10 + cy*R20 = H10
+// fy*R11 + cy*R21 = H11
+// fy*TY + cy*TZ = H12
+// R20 = H20
+// R21 = H21
+// TZ = H22
+
+matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy, double markerScale)
+{
+ // Note that every variable that we compute is proportional to the scale factor of H.
+ double R20 = MATD_EL(H, 2, 0);
+ double R21 = MATD_EL(H, 2, 1);
+ double TZ = MATD_EL(H, 2, 2);
+ double R00 = (MATD_EL(H, 0, 0) - cx*R20) / fx;
+ double R01 = (MATD_EL(H, 0, 1) - cx*R21) / fx;
+ double TX = (MATD_EL(H, 0, 2) - cx*TZ) / fx;
+ double R10 = (MATD_EL(H, 1, 0) - cy*R20) / fy;
+ double R11 = (MATD_EL(H, 1, 1) - cy*R21) / fy;
+ double TY = (MATD_EL(H, 1, 2) - cy*TZ) / fy;
+
+ // compute the scale by requiring that the rotation columns are unit length
+ // (Use geometric average of the two length vectors we have)
+ double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
+ double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
+ double s = 1.0 / sqrtf(length1 * length2);
+
+ // get sign of S by requiring the tag to be in front the camera;
+ // we assume camera looks in the -Z direction.
+ if (TZ > 0)
+ s *= -1;
+
+ R20 *= s;
+ R21 *= s;
+ TZ *= s;
+ R00 *= s;
+ R01 *= s;
+ TX *= s;
+ R10 *= s;
+ R11 *= s;
+ TY *= s;
+
+ // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
+ double R02 = R10*R21 - R20*R11;
+ double R12 = R20*R01 - R00*R21;
+ double R22 = R00*R11 - R10*R01;
+
+ // Improve rotation matrix by applying polar decomposition.
+ if (1) {
+ // do polar decomposition. This makes the rotation matrix
+ // "proper", but probably increases the reprojection error. An
+ // iterative alignment step would be superior.
+
+ matd_t *R = matd_create_data(3, 3, (double[]) { R00, R01, R02,
+ R10, R11, R12,
+ R20, R21, R22 });
+
+ matd_svd_t svd = matd_svd(R);
+ matd_destroy(R);
+
+ R = matd_op("M*M'", svd.U, svd.V);
+
+ matd_destroy(svd.U);
+ matd_destroy(svd.S);
+ matd_destroy(svd.V);
+
+ R00 = MATD_EL(R, 0, 0);
+ R01 = MATD_EL(R, 0, 1);
+ R02 = MATD_EL(R, 0, 2);
+ R10 = MATD_EL(R, 1, 0);
+ R11 = MATD_EL(R, 1, 1);
+ R12 = MATD_EL(R, 1, 2);
+ R20 = MATD_EL(R, 2, 0);
+ R21 = MATD_EL(R, 2, 1);
+ R22 = MATD_EL(R, 2, 2);
+
+ matd_destroy(R);
+ }
+
+ return matd_create_data(4, 4, (double[]) { -R00, R01, -R02, -TX*markerScale,
+ -R10, R11, -R12, -TY*markerScale,
+ -R20, R21, -R22, -TZ*markerScale,
+ 0, 0, 0, 1 });
+}
+
+// Similar to above
+// Recover the model view matrix assuming that the projection matrix is:
+//
+// [ F 0 A 0 ] (see glFrustrum)
+// [ 0 G B 0 ]
+// [ 0 0 C D ]
+// [ 0 0 -1 0 ]
+
+matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D)
+{
+ // Note that every variable that we compute is proportional to the scale factor of H.
+ double R20 = -MATD_EL(H, 2, 0);
+ double R21 = -MATD_EL(H, 2, 1);
+ double TZ = -MATD_EL(H, 2, 2);
+ double R00 = (MATD_EL(H, 0, 0) - A*R20) / F;
+ double R01 = (MATD_EL(H, 0, 1) - A*R21) / F;
+ double TX = (MATD_EL(H, 0, 2) - A*TZ) / F;
+ double R10 = (MATD_EL(H, 1, 0) - B*R20) / G;
+ double R11 = (MATD_EL(H, 1, 1) - B*R21) / G;
+ double TY = (MATD_EL(H, 1, 2) - B*TZ) / G;
+
+ // compute the scale by requiring that the rotation columns are unit length
+ // (Use geometric average of the two length vectors we have)
+ double length1 = sqrtf(R00*R00 + R10*R10 + R20*R20);
+ double length2 = sqrtf(R01*R01 + R11*R11 + R21*R21);
+ double s = 1.0 / sqrtf(length1 * length2);
+
+ // get sign of S by requiring the tag to be in front of the camera
+ // (which is Z < 0) for our conventions.
+ if (TZ > 0)
+ s *= -1;
+
+ R20 *= s;
+ R21 *= s;
+ TZ *= s;
+ R00 *= s;
+ R01 *= s;
+ TX *= s;
+ R10 *= s;
+ R11 *= s;
+ TY *= s;
+
+ // now recover [R02 R12 R22] by noting that it is the cross product of the other two columns.
+ double R02 = R10*R21 - R20*R11;
+ double R12 = R20*R01 - R00*R21;
+ double R22 = R00*R11 - R10*R01;
+
+ // TODO XXX: Improve rotation matrix by applying polar decomposition.
+
+ return matd_create_data(4, 4, (double[]) { R00, R01, R02, TX,
+ R10, R11, R12, TY,
+ R20, R21, R22, TZ,
+ 0, 0, 0, 1 });
+}
+
+// Only uses the upper 3x3 matrix.
+/*
+static void matrix_to_quat(const matd_t *R, double q[4])
+{
+ // see: "from quaternion to matrix and back"
+
+ // trace: get the same result if R is 4x4 or 3x3:
+ double T = MATD_EL(R, 0, 0) + MATD_EL(R, 1, 1) + MATD_EL(R, 2, 2) + 1;
+ double S = 0;
+
+ double m0 = MATD_EL(R, 0, 0);
+ double m1 = MATD_EL(R, 1, 0);
+ double m2 = MATD_EL(R, 2, 0);
+ double m4 = MATD_EL(R, 0, 1);
+ double m5 = MATD_EL(R, 1, 1);
+ double m6 = MATD_EL(R, 2, 1);
+ double m8 = MATD_EL(R, 0, 2);
+ double m9 = MATD_EL(R, 1, 2);
+ double m10 = MATD_EL(R, 2, 2);
+
+ if (T > 0.0000001) {
+ S = sqrtf(T) * 2;
+ q[1] = -( m9 - m6 ) / S;
+ q[2] = -( m2 - m8 ) / S;
+ q[3] = -( m4 - m1 ) / S;
+ q[0] = 0.25 * S;
+ } else if ( m0 > m5 && m0 > m10 ) { // Column 0:
+ S = sqrtf( 1.0 + m0 - m5 - m10 ) * 2;
+ q[1] = -0.25 * S;
+ q[2] = -(m4 + m1 ) / S;
+ q[3] = -(m2 + m8 ) / S;
+ q[0] = (m9 - m6 ) / S;
+ } else if ( m5 > m10 ) { // Column 1:
+ S = sqrtf( 1.0 + m5 - m0 - m10 ) * 2;
+ q[1] = -(m4 + m1 ) / S;
+ q[2] = -0.25 * S;
+ q[3] = -(m9 + m6 ) / S;
+ q[0] = (m2 - m8 ) / S;
+ } else {
+ // Column 2:
+ S = sqrtf( 1.0 + m10 - m0 - m5 ) * 2;
+ q[1] = -(m2 + m8 ) / S;
+ q[2] = -(m9 + m6 ) / S;
+ q[3] = -0.25 * S;
+ q[0] = (m4 - m1 ) / S;
+ }
+
+ double mag2 = 0;
+ for (int i = 0; i < 4; i++)
+ mag2 += q[i]*q[i];
+ double norm = 1.0 / sqrtf(mag2);
+ for (int i = 0; i < 4; i++)
+ q[i] *= norm;
+}
+*/
+
+// overwrites upper 3x3 area of matrix M. Doesn't touch any other elements of M.
+void quat_to_matrix(const double q[4], matd_t *M)
+{
+ double w = q[0], x = q[1], y = q[2], z = q[3];
+
+ MATD_EL(M, 0, 0) = w*w + x*x - y*y - z*z;
+ MATD_EL(M, 0, 1) = 2*x*y - 2*w*z;
+ MATD_EL(M, 0, 2) = 2*x*z + 2*w*y;
+
+ MATD_EL(M, 1, 0) = 2*x*y + 2*w*z;
+ MATD_EL(M, 1, 1) = w*w - x*x + y*y - z*z;
+ MATD_EL(M, 1, 2) = 2*y*z - 2*w*x;
+
+ MATD_EL(M, 2, 0) = 2*x*z - 2*w*y;
+ MATD_EL(M, 2, 1) = 2*y*z + 2*w*x;
+ MATD_EL(M, 2, 2) = w*w - x*x - y*y + z*z;
+}
diff --git a/3rdparty/apriltag/common/homography.h b/3rdparty/apriltag/common/homography.h
new file mode 100644
index 0000000..098b1e8
--- /dev/null
+++ b/3rdparty/apriltag/common/homography.h
@@ -0,0 +1,191 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _HOMOGRAPHY_H
+#define _HOMOGRAPHY_H
+
+#include "matd.h"
+#include "zarray.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ /** Given a 3x3 homography matrix and the focal lengths of the
+ * camera, compute the pose of the tag. The focal lengths should
+ * be given in pixels. For example, if the camera's focal length
+ * is twice the width of the sensor, and the sensor is 600 pixels
+ * across, the focal length in pixels is 2*600. Note that the
+ * focal lengths in the fx and fy direction will be approximately
+ * equal for most lenses, and is not a function of aspect ratio.
+ *
+ * Theory: The homography matrix is the product of the camera
+ * projection matrix and the tag's pose matrix (the matrix that
+ * projects points from the tag's local coordinate system to the
+ * camera's coordinate frame).
+ *
+ * [ h00 h01 h02 h03] = [ fx 0 cx 0 ] [ R00 R01 R02 TX ]
+ * [ h10 h11 h12 h13] = [ 0 fy cy 0 ] [ R10 R11 R12 TY ]
+ * [ h20 h21 h22 h23] = [ 0 0 s 0 ] [ R20 R21 R22 TZ ]
+ * [ 0 0 0 1 ]
+ *
+ * fx is the focal length in the x direction of the camera
+ * (typically measured in pixels), fy is the focal length. cx and
+ * cy give the focal center (usually the middle of the image), and
+ * s is either +1 or -1, depending on the conventions you use. (We
+ * use 1.)
+
+ * When observing a tag, the points we project in world space all
+ * have z=0, so we can form a 3x3 matrix by eliminating the 3rd
+ * column of the pose matrix.
+ *
+ * [ h00 h01 h02 ] = [ fx 0 cx 0 ] [ R00 R01 TX ]
+ * [ h10 h11 h12 ] = [ 0 fy cy 0 ] [ R10 R11 TY ]
+ * [ h20 h21 h22 ] = [ 0 0 s 0 ] [ R20 R21 TZ ]
+ * [ 0 0 1 ]
+ *
+ * (note that these h's are different from the ones above.)
+ *
+ * We can multiply the right-hand side to yield a set of equations
+ * relating the values of h to the values of the pose matrix.
+ *
+ * There are two wrinkles. The first is that the homography matrix
+ * is known only up to scale. We recover the unknown scale by
+ * constraining the magnitude of the first two columns of the pose
+ * matrix to be 1. We use the geometric average scale. The sign of
+ * the scale factor is recovered by constraining the observed tag
+ * to be in front of the camera. Once scaled, we recover the first
+ * two colmuns of the rotation matrix. The third column is the
+ * cross product of these.
+ *
+ * The second wrinkle is that the computed rotation matrix might
+ * not be exactly orthogonal, so we perform a polar decomposition
+ * to find a good pure rotation approximation.
+ *
+ * Tagsize is the size of the tag in your desired units. I.e., if
+ * your tag measures 0.25m along the side, your tag size is
+ * 0.25. (The homography is computed in terms of *half* the tag
+ * size, i.e., that a tag is 2 units wide as it spans from -1 to
+ * +1, but this code makes the appropriate adjustment.)
+ *
+ * A note on signs:
+ *
+ * The code below incorporates no additional negative signs, but
+ * respects the sign of any parameters that you pass in. Flipping
+ * the signs allows you to modify the projection to suit a wide
+ * variety of conditions.
+ *
+ * In the "pure geometry" projection matrix, the image appears
+ * upside down; i.e., the x and y coordinates on the left hand
+ * side are the opposite of those on the right of the camera
+ * projection matrix. This would happen for all parameters
+ * positive: recall that points in front of the camera have
+ * negative Z values, which will cause the sign of all points to
+ * flip.
+ *
+ * However, most cameras flip things so that the image appears
+ * "right side up" as though you were looking through the lens
+ * directly. This means that the projected points should have the
+ * same sign as the points on the right of the camera projection
+ * matrix. To achieve this, flip fx and fy.
+ *
+ * One further complication: cameras typically put y=0 at the top
+ * of the image, instead of the bottom. Thus you generally want to
+ * flip y yet again (so it's now positive again).
+ *
+ * General advice: you probably want fx negative, fy positive, cx
+ * and cy positive, and s=1.
+ **/
+
+// correspondences is a list of float[4]s, consisting of the points x
+// and y concatenated. We will compute a homography such that y = Hx
+// Specifically, float [] { a, b, c, d } where x = [a b], y = [c d].
+
+
+#define HOMOGRAPHY_COMPUTE_FLAG_INVERSE 1
+#define HOMOGRAPHY_COMPUTE_FLAG_SVD 0
+
+matd_t *homography_compute(zarray_t *correspondences, int flags);
+
+//void homography_project(const matd_t *H, double x, double y, double *ox, double *oy);
+static inline void homography_project(const matd_t *H, double x, double y, double *ox, double *oy)
+{
+ double xx = MATD_EL(H, 0, 0)*x + MATD_EL(H, 0, 1)*y + MATD_EL(H, 0, 2);
+ double yy = MATD_EL(H, 1, 0)*x + MATD_EL(H, 1, 1)*y + MATD_EL(H, 1, 2);
+ double zz = MATD_EL(H, 2, 0)*x + MATD_EL(H, 2, 1)*y + MATD_EL(H, 2, 2);
+
+ *ox = xx / zz;
+ *oy = yy / zz;
+}
+
+// assuming that the projection matrix is:
+// [ fx 0 cx 0 ]
+// [ 0 fy cy 0 ]
+// [ 0 0 1 0 ]
+//
+// And that the homography is equal to the projection matrix times the model matrix,
+// recover the model matrix (which is returned). Note that the third column of the model
+// matrix is missing in the expresison below, reflecting the fact that the homography assumes
+// all points are at z=0 (i.e., planar) and that the element of z is thus omitted.
+// (3x1 instead of 4x1).
+//
+// [ fx 0 cx 0 ] [ R00 R01 TX ] [ H00 H01 H02 ]
+// [ 0 fy cy 0 ] [ R10 R11 TY ] = [ H10 H11 H12 ]
+// [ 0 0 1 0 ] [ R20 R21 TZ ] = [ H20 H21 H22 ]
+// [ 0 0 1 ]
+//
+// fx*R00 + cx*R20 = H00 (note, H only known up to scale; some additional adjustments required; see code.)
+// fx*R01 + cx*R21 = H01
+// fx*TX + cx*TZ = H02
+// fy*R10 + cy*R20 = H10
+// fy*R11 + cy*R21 = H11
+// fy*TY + cy*TZ = H12
+// R20 = H20
+// R21 = H21
+// TZ = H22
+matd_t *homography_to_pose(const matd_t *H, double fx, double fy, double cx, double cy, double markerScale);
+
+// Similar to above
+// Recover the model view matrix assuming that the projection matrix is:
+//
+// [ F 0 A 0 ] (see glFrustrum)
+// [ 0 G B 0 ]
+// [ 0 0 C D ]
+// [ 0 0 -1 0 ]
+
+matd_t *homography_to_model_view(const matd_t *H, double F, double G, double A, double B, double C, double D);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/image_f32.c b/3rdparty/apriltag/common/image_f32.c
new file mode 100644
index 0000000..3bad1ce
--- /dev/null
+++ b/3rdparty/apriltag/common/image_f32.c
@@ -0,0 +1,264 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <math.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "math_util.h"
+
+#include "image_f32.h"
+
+#ifndef HUGE
+//# define HUGE 3.40282347e+38F
+#include <float.h>
+#endif
+
+
+static inline float sqf(float v)
+{
+ return v*v;
+}
+
+image_f32_t *image_f32_create_stride(int width, int height, int stride)
+{
+ float *buf = calloc(height * stride, sizeof(float));
+ // const initializer
+ image_f32_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf };
+
+ image_f32_t *fim = (image_f32_t*) calloc(1, sizeof(image_f32_t));
+
+ memcpy(fim, &tmp, sizeof(image_f32_t));
+
+ return fim;
+}
+
+image_f32_t *image_f32_create(int width, int height)
+{
+ int stride = width;
+ while (stride & 7)
+ stride++;
+ return image_f32_create_stride(width, height, stride);
+}
+
+// scales by 1/255u
+image_f32_t *image_f32_create_from_u8(const image_u8_t *im)
+{
+ image_f32_t *fim = image_f32_create(im->width, im->height);
+
+ for (int y = 0; y < fim->height; y++)
+ for (int x = 0; x < fim->width; x++)
+ fim->buf[y*fim->stride + x] = im->buf[y*im->stride + x] / 255.0f;
+
+ return fim;
+}
+
+void image_f32_destroy(image_f32_t *im)
+{
+ free(im->buf);
+ free(im);
+}
+
+static void convolve(const float *x, float *y, int sz, const float *k, int ksz)
+{
+ assert((ksz&1)==1);
+
+ for (int i = 0; i < ksz/2 && i < sz; i++)
+ y[i] = x[i];
+
+ for (int i = 0; i < sz - ksz; i++) {
+ float acc = 0;
+
+ for (int j = 0; j < ksz; j++)
+ acc += k[j]*x[i+j];
+
+ y[ksz/2 + i] = acc;
+ }
+
+ for (int i = sz - ksz + ksz/2; i < sz; i++)
+ y[i] = x[i];
+}
+
+void image_f32_gaussian_blur(image_f32_t *im, double sigma, int ksz)
+{
+ assert((ksz & 1) == 1); // ksz must be odd.
+
+ // build the kernel.
+#ifdef _MSC_VER
+ float *k = malloc(ksz*sizeof *k);
+#else
+ float k[ksz];
+#endif
+
+ // for kernel of length 5:
+ // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2)
+ for (int i = 0; i < ksz; i++) {
+ int x = -ksz/2 + i;
+ float v = exp(-.5*sqf(x / sigma));
+ k[i] = v;
+ }
+
+ // normalize
+ float acc = 0;
+ for (int i = 0; i < ksz; i++)
+ acc += k[i];
+
+ for (int i = 0; i < ksz; i++)
+ k[i] /= acc;
+
+#ifdef _MSC_VER
+ free(k);
+#endif
+
+ for (int y = 0; y < im->height; y++) {
+#ifdef _MSC_VER
+ float *x = malloc(im->stride*sizeof *x);
+#else
+ float x[im->stride];
+#endif
+ memcpy(x, &im->buf[y*im->stride], im->stride * sizeof(float));
+ convolve(x, &im->buf[y*im->stride], im->width, k, ksz);
+
+#ifdef _MSC_VER
+ free(x);
+#endif
+ }
+
+ for (int x = 0; x < im->width; x++) {
+#ifdef _MSC_VER
+ float *xb = malloc(im->height*sizeof *xb);
+ float *yb = malloc(im->height*sizeof *yb);
+#else
+ float xb[im->height];
+ float yb[im->height];
+#endif
+
+ for (int y = 0; y < im->height; y++)
+ xb[y] = im->buf[y*im->stride + x];
+
+ convolve(xb, yb, im->height, k, ksz);
+
+ for (int y = 0; y < im->height; y++)
+ im->buf[y*im->stride + x] = yb[y];
+
+#ifdef _MSC_VER
+ free(xb);
+ free(yb);
+#endif
+ }
+}
+
+// remap all values to [0, 1]
+void image_f32_normalize(image_f32_t *im)
+{
+#ifndef HUGE
+ float min = FLT_MAX, max = -FLT_MAX;
+#else
+ float min = HUGE, max = -HUGE;
+#endif
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ float v = im->buf[y*im->stride + x];
+ if (v < min)
+ min = v;
+ if (v > max)
+ max = v;
+ }
+ }
+
+ if (min == max) {
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ im->buf[y*im->stride + x] = 0.5;
+ }
+ }
+ } else {
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ float v = im->buf[y*im->stride + x];
+
+ im->buf[y*im->stride + x] = (v - min) / (max - min);
+ }
+ }
+ }
+}
+
+// image is assumed to be [0, 1]
+int image_f32_write_pnm(const image_f32_t *im, const char *path)
+{
+ FILE *f = fopen(path, "wb");
+ int res = 0;
+
+ if (f == NULL) {
+ res = -1;
+ goto finish;
+ }
+
+ // Only outputs to grayscale
+ fprintf(f, "P5\n%d %d\n255\n", im->width, im->height);
+
+ for (int y = 0; y < im->height; y++) {
+#ifdef _MSC_VER
+ uint8_t *line = malloc(im->width*sizeof *line);
+#else
+ uint8_t line[im->width];
+#endif
+ for (int x = 0; x < im->width; x++) {
+ float v = im->buf[y*im->stride + x];
+ if (v < 0)
+ v = 0;
+ if (v > 1)
+ v = 1;
+ line[x] = v * 255.0;
+ }
+
+ if (im->width != fwrite(line, 1, im->width, f)) {
+ res = -2;
+ goto finish;
+ }
+
+#ifdef _MSC_VER
+ free(line);
+#endif
+ }
+
+finish:
+ if (f != NULL)
+ fclose(f);
+
+ return res;
+}
diff --git a/3rdparty/apriltag/common/image_f32.h b/3rdparty/apriltag/common/image_f32.h
new file mode 100644
index 0000000..1755088
--- /dev/null
+++ b/3rdparty/apriltag/common/image_f32.h
@@ -0,0 +1,55 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _IMAGE_F32
+#define _IMAGE_F32
+
+#include <stdint.h>
+#include "common/image_types.h"
+
+image_f32_t *image_f32_create(int width, int height);
+
+image_f32_t *image_f32_create_stride(int width, int height, int stride);
+
+image_f32_t *image_f32_create_from_u8(const image_u8_t *im);
+
+void image_f32_destroy(image_f32_t *im);
+
+void image_f32_gaussian_blur(image_f32_t *im, double sigma, int ksz);
+
+// remap all values to [0, 1]
+void image_f32_normalize(image_f32_t *im);
+
+// image is assumed to be [0, 1]
+int image_f32_write_pnm(const image_f32_t *im, const char *path);
+
+#endif
diff --git a/3rdparty/apriltag/common/image_types.h b/3rdparty/apriltag/common/image_types.h
new file mode 100644
index 0000000..4110815
--- /dev/null
+++ b/3rdparty/apriltag/common/image_types.h
@@ -0,0 +1,60 @@
+#ifndef _IMAGE_TYPES_H
+#define _IMAGE_TYPES_H
+
+#include <stdint.h>
+
+// to support conversions between different types, we define all image
+// types at once. Type-specific implementations can then #include this
+// file, assured that the basic types of each image are known.
+
+typedef struct image_u8 image_u8_t;
+struct image_u8
+{
+ const int32_t width;
+ const int32_t height;
+ const int32_t stride;
+
+ uint8_t *buf;
+};
+
+typedef struct image_u8x3 image_u8x3_t;
+struct image_u8x3
+{
+ const int32_t width;
+ const int32_t height;
+ const int32_t stride; // bytes per line
+
+ uint8_t *buf;
+};
+
+typedef struct image_u8x4 image_u8x4_t;
+struct image_u8x4
+{
+ const int32_t width;
+ const int32_t height;
+ const int32_t stride; // bytes per line
+
+ uint8_t *buf;
+};
+
+typedef struct image_f32 image_f32_t;
+struct image_f32
+{
+ const int32_t width;
+ const int32_t height;
+ const int32_t stride; // floats per line
+
+ float *buf; // indexed as buf[y*stride + x]
+};
+
+typedef struct image_u32 image_u32_t;
+struct image_u32
+{
+ const int32_t width;
+ const int32_t height;
+ const int32_t stride; // int32_ts per line
+
+ uint32_t *buf; // indexed as buf[y*stride + x]
+};
+
+#endif
diff --git a/3rdparty/apriltag/common/image_u8.c b/3rdparty/apriltag/common/image_u8.c
new file mode 100644
index 0000000..6e0fe30
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8.c
@@ -0,0 +1,753 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "common/image_u8.h"
+#include "common/pnm.h"
+#include "common/math_util.h"
+
+// least common multiple of 64 (sandy bridge cache line) and 24 (stride
+// needed for RGB in 8-wide vector processing)
+#define DEFAULT_ALIGNMENT_U8 96
+
+image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride)
+{
+ uint8_t *buf = calloc(height*stride, sizeof(uint8_t));
+
+ // const initializer
+ image_u8_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf };
+
+ image_u8_t *im = calloc(1, sizeof(image_u8_t));
+ memcpy(im, &tmp, sizeof(image_u8_t));
+ return im;
+}
+
+image_u8_t *image_u8_create(unsigned int width, unsigned int height)
+{
+ return image_u8_create_alignment(width, height, DEFAULT_ALIGNMENT_U8);
+}
+
+image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment)
+{
+ int stride = width;
+
+ if ((stride % alignment) != 0)
+ stride += alignment - (stride % alignment);
+
+ return image_u8_create_stride(width, height, stride);
+}
+
+image_u8_t *image_u8_copy(const image_u8_t *in)
+{
+ uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t));
+ memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t));
+
+ // const initializer
+ image_u8_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf };
+
+ image_u8_t *copy = calloc(1, sizeof(image_u8_t));
+ memcpy(copy, &tmp, sizeof(image_u8_t));
+ return copy;
+}
+
+void image_u8_destroy(image_u8_t *im)
+{
+ if (!im)
+ return;
+
+ free(im->buf);
+ free(im);
+}
+
+////////////////////////////////////////////////////////////
+// PNM file i/o
+image_u8_t *image_u8_create_from_pnm(const char *path)
+{
+ return image_u8_create_from_pnm_alignment(path, DEFAULT_ALIGNMENT_U8);
+}
+
+image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment)
+{
+ pnm_t *pnm = pnm_create_from_file(path);
+ if (pnm == NULL)
+ return NULL;
+
+ image_u8_t *im = NULL;
+
+ switch (pnm->format) {
+ case PNM_FORMAT_GRAY: {
+ im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
+
+ if (pnm->max == 255) {
+ for (int y = 0; y < im->height; y++)
+ memcpy(&im->buf[y*im->stride], &pnm->buf[y*im->width], im->width);
+ } else if (pnm->max == 65535) {
+ for (int y = 0; y < im->height; y++)
+ for (int x = 0; x < im->width; x++)
+ im->buf[y*im->stride + x] = pnm->buf[2*(y*im->width + x)];
+ } else {
+ assert(0);
+ }
+
+ break;
+ }
+
+ case PNM_FORMAT_RGB: {
+ im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
+
+ if (pnm->max == 255) {
+ // Gray conversion for RGB is gray = (r + g + g + b)/4
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ uint8_t gray = (pnm->buf[y*im->width*3 + 3*x+0] + // r
+ pnm->buf[y*im->width*3 + 3*x+1] + // g
+ pnm->buf[y*im->width*3 + 3*x+1] + // g
+ pnm->buf[y*im->width*3 + 3*x+2]) // b
+ / 4;
+
+ im->buf[y*im->stride + x] = gray;
+ }
+ }
+ } else if (pnm->max == 65535) {
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ int r = pnm->buf[6*(y*im->width + x) + 0];
+ int g = pnm->buf[6*(y*im->width + x) + 2];
+ int b = pnm->buf[6*(y*im->width + x) + 4];
+
+ im->buf[y*im->stride + x] = (r + g + g + b) / 4;
+ }
+ }
+ } else {
+ assert(0);
+ }
+
+ break;
+ }
+
+ case PNM_FORMAT_BINARY: {
+ im = image_u8_create_alignment(pnm->width, pnm->height, alignment);
+
+ // image is padded to be whole bytes on each row.
+
+ // how many bytes per row on the input?
+ int pbmstride = (im->width + 7) / 8;
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ int byteidx = y * pbmstride + x / 8;
+ int bitidx = 7 - (x & 7);
+
+ // ack, black is one according to pbm docs!
+ if ((pnm->buf[byteidx] >> bitidx) & 1)
+ im->buf[y*im->stride + x] = 0;
+ else
+ im->buf[y*im->stride + x] = 255;
+ }
+ }
+ break;
+ }
+ }
+
+ pnm_destroy(pnm);
+ return im;
+}
+
+image_u8_t *image_u8_create_from_f32(image_f32_t *fim)
+{
+ image_u8_t *im = image_u8_create(fim->width, fim->height);
+
+ for (int y = 0; y < fim->height; y++) {
+ for (int x = 0; x < fim->width; x++) {
+ float v = fim->buf[y*fim->stride + x];
+ im->buf[y*im->stride + x] = (int) (255 * v);
+ }
+ }
+
+ return im;
+}
+
+
+int image_u8_write_pnm(const image_u8_t *im, const char *path)
+{
+ FILE *f = fopen(path, "wb");
+ int res = 0;
+
+ if (f == NULL) {
+ res = -1;
+ goto finish;
+ }
+
+ // Only outputs to grayscale
+ fprintf(f, "P5\n%d %d\n255\n", im->width, im->height);
+
+ for (int y = 0; y < im->height; y++) {
+ if (im->width != fwrite(&im->buf[y*im->stride], 1, im->width, f)) {
+ res = -2;
+ goto finish;
+ }
+ }
+
+ finish:
+ if (f != NULL)
+ fclose(f);
+
+ return res;
+}
+
+void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v)
+{
+ r = r*r;
+
+ for (int y = y0-r; y <= y0+r; y++) {
+ for (int x = x0-r; x <= x0+r; x++) {
+ float d = (x-x0)*(x-x0) + (y-y0)*(y-y0);
+ if (d > r)
+ continue;
+
+ if (x >= 0 && x < im->width && y >= 0 && y < im->height) {
+ int idx = y*im->stride + x;
+ im->buf[idx] = v;
+ }
+ }
+ }
+}
+
+void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v)
+{
+ r0 = r0*r0;
+ r1 = r1*r1;
+
+ assert(r0 < r1);
+
+ for (int y = y0-r1; y <= y0+r1; y++) {
+ for (int x = x0-r1; x <= x0+r1; x++) {
+ float d = (x-x0)*(x-x0) + (y-y0)*(y-y0);
+ if (d < r0 || d > r1)
+ continue;
+
+ int idx = y*im->stride + x;
+ im->buf[idx] = v;
+ }
+ }
+}
+
+// only widths 1 and 3 supported (and 3 only badly)
+void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width)
+{
+ double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
+ double delta = 0.5 / dist;
+
+ // terrible line drawing code
+ for (float f = 0; f <= 1; f += delta) {
+ int x = ((int) (x1 + (x0 - x1) * f));
+ int y = ((int) (y1 + (y0 - y1) * f));
+
+ if (x < 0 || y < 0 || x >= im->width || y >= im->height)
+ continue;
+
+ int idx = y*im->stride + x;
+ im->buf[idx] = v;
+ if (width > 1) {
+ im->buf[idx+1] = v;
+ im->buf[idx+im->stride] = v;
+ im->buf[idx+1+im->stride] = v;
+ }
+ }
+}
+
+void image_u8_darken(image_u8_t *im)
+{
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ im->buf[im->stride*y+x] /= 2;
+ }
+ }
+}
+
+static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz)
+{
+ assert((ksz&1)==1);
+
+ for (int i = 0; i < ksz/2 && i < sz; i++)
+ y[i] = x[i];
+
+ for (int i = 0; i < sz - ksz; i++) {
+ uint32_t acc = 0;
+
+ for (int j = 0; j < ksz; j++)
+ acc += k[j]*x[i+j];
+
+ y[ksz/2 + i] = acc >> 8;
+ }
+
+ for (int i = sz - ksz + ksz/2; i < sz; i++)
+ y[i] = x[i];
+}
+
+void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz)
+{
+ assert((ksz & 1) == 1); // ksz must be odd.
+
+ for (int y = 0; y < im->height; y++) {
+#ifdef _MSC_VER
+ uint8_t *x = malloc(im->stride*sizeof *x);
+#else
+ uint8_t x[im->stride];
+#endif
+ memcpy(x, &im->buf[y*im->stride], im->stride);
+
+ convolve(x, &im->buf[y*im->stride], im->width, k, ksz);
+
+#ifdef _MSC_VER
+ free(x);
+#endif
+ }
+
+ for (int x = 0; x < im->width; x++) {
+#ifdef _MSC_VER
+ uint8_t *xb = malloc(im->height*sizeof *xb);
+ uint8_t *yb = malloc(im->height*sizeof *yb);
+#else
+ uint8_t xb[im->height];
+ uint8_t yb[im->height];
+#endif
+
+ for (int y = 0; y < im->height; y++)
+ xb[y] = im->buf[y*im->stride + x];
+
+ convolve(xb, yb, im->height, k, ksz);
+
+ for (int y = 0; y < im->height; y++)
+ im->buf[y*im->stride + x] = yb[y];
+
+#ifdef _MSC_VER
+ free(xb);
+ free(yb);
+#endif
+ }
+}
+
+void image_u8_gaussian_blur(image_u8_t *im, double sigma, int ksz)
+{
+ if (sigma == 0)
+ return;
+
+ assert((ksz & 1) == 1); // ksz must be odd.
+
+ // build the kernel.
+#ifdef _MSC_VER
+ double *dk = malloc(ksz*sizeof *dk);
+#else
+ double dk[ksz];
+#endif
+
+ // for kernel of length 5:
+ // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2)
+ for (int i = 0; i < ksz; i++) {
+ int x = -ksz/2 + i;
+ double v = exp(-.5*sq(x / sigma));
+ dk[i] = v;
+ }
+
+ // normalize
+ double acc = 0;
+ for (int i = 0; i < ksz; i++)
+ acc += dk[i];
+
+ for (int i = 0; i < ksz; i++)
+ dk[i] /= acc;
+
+#ifdef _MSC_VER
+ uint8_t *k = malloc(ksz*sizeof *k);
+#else
+ uint8_t k[ksz];
+#endif
+ for (int i = 0; i < ksz; i++)
+ k[i] = dk[i]*255;
+
+ if (0) {
+ for (int i = 0; i < ksz; i++)
+ printf("%d %15f %5d\n", i, dk[i], k[i]);
+ }
+
+ image_u8_convolve_2D(im, k, ksz);
+
+#ifdef _MSC_VER
+ free(dk);
+ free(k);
+#endif
+}
+
+image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad)
+{
+ int iwidth = in->width, iheight = in->height;
+ rad = -rad; // interpret y as being "down"
+
+ float c = cos(rad), s = sin(rad);
+
+ float p[][2] = { { 0, 0}, { iwidth, 0 }, { iwidth, iheight }, { 0, iheight} };
+
+ float xmin = HUGE_VALF, xmax = -HUGE_VALF, ymin = HUGE_VALF, ymax = -HUGE_VALF;
+ float icx = iwidth / 2.0, icy = iheight / 2.0;
+
+ for (int i = 0; i < 4; i++) {
+ float px = p[i][0] - icx;
+ float py = p[i][1] - icy;
+
+ float nx = px*c - py*s;
+ float ny = px*s + py*c;
+
+ xmin = fmin(xmin, nx);
+ xmax = fmax(xmax, nx);
+ ymin = fmin(ymin, ny);
+ ymax = fmax(ymax, ny);
+ }
+
+ int owidth = ceil(xmax-xmin), oheight = ceil(ymax - ymin);
+ image_u8_t *out = image_u8_create(owidth, oheight);
+
+ // iterate over output pixels.
+ for (int oy = 0; oy < oheight; oy++) {
+ for (int ox = 0; ox < owidth; ox++) {
+ // work backwards from destination coordinates...
+ // sample pixel centers.
+ float sx = ox - owidth / 2.0 + .5;
+ float sy = oy - oheight / 2.0 + .5;
+
+ // project into input-image space
+ int ix = floor(sx*c + sy*s + icx);
+ int iy = floor(-sx*s + sy*c + icy);
+
+ if (ix >= 0 && iy >= 0 && ix < iwidth && iy < iheight)
+ out->buf[oy*out->stride+ox] = in->buf[iy*in->stride + ix];
+ else
+ out->buf[oy*out->stride+ox] = pad;
+ }
+ }
+
+ return out;
+}
+
+#ifdef __ARM_NEON__
+#include <arm_neon.h>
+
+void neon_decimate2(uint8_t * __restrict dest, int destwidth, int destheight, int deststride,
+ uint8_t * __restrict src, int srcwidth, int srcheight, int srcstride)
+{
+ for (int y = 0; y < destheight; y++) {
+ for (int x = 0; x < destwidth; x+=8) {
+ uint8x16x2_t row0 = vld2q_u8(src + 2*x);
+ uint8x16x2_t row1 = vld2q_u8(src + 2*x + srcstride);
+ uint8x16_t sum0 = vhaddq_u8(row0.val[0], row1.val[1]);
+ uint8x16_t sum1 = vhaddq_u8(row1.val[0], row0.val[1]);
+ uint8x16_t sum = vhaddq_u8(sum0, sum1);
+ vst1q_u8(dest + x, sum);
+ }
+ src += 2*srcstride;
+ dest += deststride;
+ }
+}
+
+void neon_decimate3(uint8_t * __restrict dest, int destwidth, int destheight, int deststride,
+ uint8_t * __restrict src, int srcwidth, int srcheight, int srcstride)
+{
+ for (int y = 0; y < destheight; y++) {
+ for (int x = 0; x < destwidth; x+=8) {
+ uint8x16x3_t row0 = vld3q_u8(src + 3*x);
+ uint8x16x3_t row1 = vld3q_u8(src + 3*x + srcstride);
+ uint8x16x3_t row2 = vld3q_u8(src + 3*x + 2*srcstride);
+
+ uint8x16_t sum0 = vhaddq_u8(row0.val[0], row0.val[1]);
+ uint8x16_t sum1 = vhaddq_u8(row0.val[2], row1.val[0]);
+ uint8x16_t sum2 = vhaddq_u8(row1.val[1], row1.val[2]);
+ uint8x16_t sum3 = vhaddq_u8(row2.val[0], row2.val[1]);
+
+ uint8x16_t suma = vhaddq_u8(sum0, sum1);
+ uint8x16_t sumb = vhaddq_u8(sum2, sum3);
+ uint8x16_t sum = vhaddq_u8(suma, sumb);
+
+ vst1q_u8(dest + x, sum);
+ }
+ src += 3*srcstride;
+ dest += deststride;
+ }
+}
+
+void neon_decimate4(uint8_t * __restrict dest, int destwidth, int destheight, int deststride,
+ uint8_t * __restrict src, int srcwidth, int srcheight, int srcstride)
+{
+ for (int y = 0; y < destheight; y++) {
+ for (int x = 0; x < destwidth; x+=8) {
+ uint8x16x4_t row0 = vld4q_u8(src + 4*x);
+ uint8x16x4_t row1 = vld4q_u8(src + 4*x + srcstride);
+ uint8x16x4_t row2 = vld4q_u8(src + 4*x + 2*srcstride);
+ uint8x16x4_t row3 = vld4q_u8(src + 4*x + 3*srcstride);
+
+ uint8x16_t sum0, sum1;
+
+ sum0 = vhaddq_u8(row0.val[0], row0.val[3]);
+ sum1 = vhaddq_u8(row0.val[2], row0.val[1]);
+ uint8x16_t suma = vhaddq_u8(sum0, sum1);
+
+ sum0 = vhaddq_u8(row1.val[0], row1.val[3]);
+ sum1 = vhaddq_u8(row1.val[2], row1.val[1]);
+ uint8x16_t sumb = vhaddq_u8(sum0, sum1);
+
+ sum0 = vhaddq_u8(row2.val[0], row2.val[3]);
+ sum1 = vhaddq_u8(row2.val[2], row2.val[1]);
+ uint8x16_t sumc = vhaddq_u8(sum0, sum1);
+
+ sum0 = vhaddq_u8(row3.val[0], row3.val[3]);
+ sum1 = vhaddq_u8(row3.val[2], row3.val[1]);
+ uint8x16_t sumd = vhaddq_u8(sum0, sum1);
+
+ uint8x16_t sumx = vhaddq_u8(suma, sumd);
+ uint8x16_t sumy = vhaddq_u8(sumc, sumb);
+
+ uint8x16_t sum = vhaddq_u8(sumx, sumy);
+
+ vst1q_u8(dest + x, sum);
+ }
+ src += 4*srcstride;
+ dest += deststride;
+ }
+}
+
+#endif
+
+image_u8_t *image_u8_decimate(image_u8_t *im, float ffactor)
+{
+ int width = im->width, height = im->height;
+
+ if (ffactor == 1.5) {
+ int swidth = width / 3 * 2, sheight = height / 3 * 2;
+
+ image_u8_t *decim = image_u8_create(swidth, sheight);
+
+ int y = 0, sy = 0;
+ while (sy < sheight) {
+ int x = 0, sx = 0;
+ while (sx < swidth) {
+
+ // a b c
+ // d e f
+ // g h i
+ uint8_t a = im->buf[(y+0)*im->stride + (x+0)];
+ uint8_t b = im->buf[(y+0)*im->stride + (x+1)];
+ uint8_t c = im->buf[(y+0)*im->stride + (x+2)];
+
+ uint8_t d = im->buf[(y+1)*im->stride + (x+0)];
+ uint8_t e = im->buf[(y+1)*im->stride + (x+1)];
+ uint8_t f = im->buf[(y+1)*im->stride + (x+2)];
+
+ uint8_t g = im->buf[(y+2)*im->stride + (x+0)];
+ uint8_t h = im->buf[(y+2)*im->stride + (x+1)];
+ uint8_t i = im->buf[(y+2)*im->stride + (x+2)];
+
+ decim->buf[(sy+0)*decim->stride + (sx + 0)] =
+ (4*a+2*b+2*d+e)/9;
+ decim->buf[(sy+0)*decim->stride + (sx + 1)] =
+ (4*c+2*b+2*f+e)/9;
+
+ decim->buf[(sy+1)*decim->stride + (sx + 0)] =
+ (4*g+2*d+2*h+e)/9;
+ decim->buf[(sy+1)*decim->stride + (sx + 1)] =
+ (4*i+2*f+2*h+e)/9;
+
+ x += 3;
+ sx += 2;
+ }
+
+ y += 3;
+ sy += 2;
+ }
+
+ return decim;
+ }
+
+ int factor = (int) ffactor;
+
+ int swidth = width / factor, sheight = height / factor;
+
+ image_u8_t *decim = image_u8_create(swidth, sheight);
+
+#ifdef __ARM_NEON__
+ if (factor == 2) {
+ neon_decimate2(decim->buf, decim->width, decim->height, decim->stride,
+ im->buf, im->width, im->height, im->stride);
+ return decim;
+ } else if (factor == 3) {
+ neon_decimate3(decim->buf, decim->width, decim->height, decim->stride,
+ im->buf, im->width, im->height, im->stride);
+ return decim;
+ } else if (factor == 4) {
+ neon_decimate4(decim->buf, decim->width, decim->height, decim->stride,
+ im->buf, im->width, im->height, im->stride);
+ return decim;
+ }
+#endif
+
+ if (factor == 2) {
+ for (int sy = 0; sy < sheight; sy++) {
+ int sidx = sy * decim->stride;
+ int idx = (sy*2)*im->stride;
+
+ for (int sx = 0; sx < swidth; sx++) {
+ uint32_t v = im->buf[idx] + im->buf[idx+1] +
+ im->buf[idx+im->stride] + im->buf[idx+im->stride + 1];
+ decim->buf[sidx] = (v>>2);
+ idx+=2;
+ sidx++;
+ }
+ }
+ } else if (factor == 3) {
+ for (int sy = 0; sy < sheight; sy++) {
+ int sidx = sy * decim->stride;
+ int idx = (sy*3)*im->stride;
+
+ for (int sx = 0; sx < swidth; sx++) {
+ uint32_t v = im->buf[idx] + im->buf[idx+1] + im->buf[idx+2] +
+ im->buf[idx+im->stride] + im->buf[idx+im->stride + 1] + im->buf[idx+im->stride + 2] +
+ im->buf[idx+2*im->stride] + im->buf[idx+2*im->stride + 1];
+ // + im->buf[idx+2*im->stride + 1];
+ // deliberately omit lower right corner so there are exactly 8 samples...
+ decim->buf[sidx] = (v>>3);
+ idx+=3;
+ sidx++;
+ }
+ }
+ } else if (factor == 4) {
+ for (int sy = 0; sy < sheight; sy++) {
+ int sidx = sy * decim->stride;
+ int idx = (sy*4)*im->stride;
+
+ for (int sx = 0; sx < swidth; sx++) {
+ uint32_t v = im->buf[idx] + im->buf[idx+1] + im->buf[idx+2] + im->buf[idx+3] +
+ im->buf[idx+im->stride] + im->buf[idx+im->stride + 1] + im->buf[idx+im->stride + 1] + im->buf[idx+im->stride + 2] +
+ im->buf[idx+2*im->stride] + im->buf[idx+2*im->stride + 1] + im->buf[idx+2*im->stride + 2] + im->buf[idx+2*im->stride + 3];
+
+ decim->buf[sidx] = (v>>4);
+ idx+=4;
+ sidx++;
+ }
+ }
+ } else {
+ // XXX this isn't a very good decimation code.
+#ifdef _MSC_VER
+ uint32_t *row = malloc(swidth*sizeof *row);
+#else
+ uint32_t row[swidth];
+#endif
+
+ for (int y = 0; y < height; y+= factor) {
+ memset(row, 0, sizeof(row));
+
+ for (int dy = 0; dy < factor; dy++) {
+ for (int x = 0; x < width; x++) {
+ row[x/factor] += im->buf[(y+dy)*im->stride + x];
+ }
+ }
+
+ for (int x = 0; x < swidth; x++)
+ decim->buf[(y/factor)*decim->stride + x] = row[x] / sq(factor);
+
+ }
+
+#ifdef _MSC_VER
+ free(row);
+#endif
+ }
+
+ return decim;
+}
+
+void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1)
+{
+ // what is the maximum distance that will result in drawing into our LUT?
+ float max_dist2 = (lut->nvalues-1)/lut->scale;
+ float max_dist = sqrt(max_dist2);
+
+ // the orientation of the line
+ double theta = atan2(xy1[1]-xy0[1], xy1[0]-xy0[0]);
+ double v = sin(theta), u = cos(theta);
+
+ int ix0 = iclamp(fmin(xy0[0], xy1[0]) - max_dist, 0, im->width-1);
+ int ix1 = iclamp(fmax(xy0[0], xy1[0]) + max_dist, 0, im->width-1);
+
+ int iy0 = iclamp(fmin(xy0[1], xy1[1]) - max_dist, 0, im->height-1);
+ int iy1 = iclamp(fmax(xy0[1], xy1[1]) + max_dist, 0, im->height-1);
+
+ // the line segment xy0---xy1 can be parameterized in terms of line coordinates.
+ // We fix xy0 to be at line coordinate 0.
+ float xy1_line_coord = (xy1[0]-xy0[0])*u + (xy1[1]-xy0[1])*v;
+
+ float min_line_coord = fmin(0, xy1_line_coord);
+ float max_line_coord = fmax(0, xy1_line_coord);
+
+ for (int iy = iy0; iy <= iy1; iy++) {
+ float y = iy+.5;
+
+ for (int ix = ix0; ix <= ix1; ix++) {
+ float x = ix+.5;
+
+ // compute line coordinate of this pixel.
+ float line_coord = (x - xy0[0])*u + (y - xy0[1])*v;
+
+ // find point on line segment closest to our current pixel.
+ if (line_coord < min_line_coord)
+ line_coord = min_line_coord;
+ else if (line_coord > max_line_coord)
+ line_coord = max_line_coord;
+
+ float px = xy0[0] + line_coord*u;
+ float py = xy0[1] + line_coord*v;
+
+ double dist2 = (x-px)*(x-px) + (y-py)*(y-py);
+
+ // not in our LUT?
+ int idx = dist2 * lut->scale;
+ if (idx >= lut->nvalues)
+ continue;
+
+ uint8_t lut_value = lut->values[idx];
+ uint8_t old_value = im->buf[iy*im->stride + ix];
+ if (lut_value > old_value)
+ im->buf[iy*im->stride + ix] = lut_value;
+ }
+ }
+}
diff --git a/3rdparty/apriltag/common/image_u8.h b/3rdparty/apriltag/common/image_u8.h
new file mode 100644
index 0000000..2b04bab
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8.h
@@ -0,0 +1,99 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _IMAGE_U8_H
+#define _IMAGE_U8_H
+
+#include <stdint.h>
+#include "image_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct image_u8_lut image_u8_lut_t;
+struct image_u8_lut
+{
+ // When drawing, we compute the squared distance between a given pixel and a filled region.
+ // int idx = squared_distance * scale;
+ // We then index into values[idx] to obtain the color. (If we must index beyond nvalues,
+ // no drawing is performed.)
+ float scale;
+
+ int nvalues;
+ uint8_t *values;
+};
+
+
+// Create or load an image. returns NULL on failure. Uses default
+// stride alignment.
+image_u8_t *image_u8_create_stride(unsigned int width, unsigned int height, unsigned int stride);
+image_u8_t *image_u8_create(unsigned int width, unsigned int height);
+image_u8_t *image_u8_create_alignment(unsigned int width, unsigned int height, unsigned int alignment);
+image_u8_t *image_u8_create_from_rgb3(int width, int height, uint8_t *rgb, int stride);
+image_u8_t *image_u8_create_from_f32(image_f32_t *fim);
+
+image_u8_t *image_u8_create_from_pnm(const char *path);
+ image_u8_t *image_u8_create_from_pnm_alignment(const char *path, int alignment);
+
+image_u8_t *image_u8_copy(const image_u8_t *in);
+void image_u8_draw_line(image_u8_t *im, float x0, float y0, float x1, float y1, int v, int width);
+void image_u8_draw_circle(image_u8_t *im, float x0, float y0, float r, int v);
+void image_u8_draw_annulus(image_u8_t *im, float x0, float y0, float r0, float r1, int v);
+
+void image_u8_fill_line_max(image_u8_t *im, const image_u8_lut_t *lut, const float *xy0, const float *xy1);
+
+void image_u8_clear(image_u8_t *im);
+void image_u8_darken(image_u8_t *im);
+void image_u8_convolve_2D(image_u8_t *im, const uint8_t *k, int ksz);
+void image_u8_gaussian_blur(image_u8_t *im, double sigma, int k);
+
+// 1.5, 2, 3, 4, ... supported
+image_u8_t *image_u8_decimate(image_u8_t *im, float factor);
+
+void image_u8_destroy(image_u8_t *im);
+
+// Write a pnm. Returns 0 on success
+// Currently only supports GRAY and RGBA. Does not write out alpha for RGBA
+int image_u8_write_pnm(const image_u8_t *im, const char *path);
+
+// rotate the image by 'rad' radians. (Rotated in the "intuitive
+// sense", i.e., if Y were up. When input values are unavailable, the
+// value 'pad' is inserted instead. The geometric center of the output
+// image corresponds to the geometric center of the input image.
+image_u8_t *image_u8_rotate(const image_u8_t *in, double rad, uint8_t pad);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/image_u8x3.c b/3rdparty/apriltag/common/image_u8x3.c
new file mode 100644
index 0000000..ee10be9
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8x3.c
@@ -0,0 +1,293 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include "math_util.h"
+#include "pnm.h"
+
+#include "image_u8x3.h"
+
+// least common multiple of 64 (sandy bridge cache line) and 48 (stride needed
+// for 16byte-wide RGB processing). (It's possible that 48 would be enough).
+#define DEFAULT_ALIGNMENT_U8X3 192
+
+image_u8x3_t *image_u8x3_create(unsigned int width, unsigned int height)
+{
+ return image_u8x3_create_alignment(width, height, DEFAULT_ALIGNMENT_U8X3);
+}
+
+image_u8x3_t *image_u8x3_create_alignment(unsigned int width, unsigned int height, unsigned int alignment)
+{
+ int stride = 3*width;
+
+ if ((stride % alignment) != 0)
+ stride += alignment - (stride % alignment);
+
+ uint8_t *buf = calloc(height*stride, sizeof(uint8_t));
+
+ // const initializer
+ image_u8x3_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf };
+
+ image_u8x3_t *im = calloc(1, sizeof(image_u8x3_t));
+ memcpy(im, &tmp, sizeof(image_u8x3_t));
+ return im;
+}
+
+image_u8x3_t *image_u8x3_copy(const image_u8x3_t *in)
+{
+ uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t));
+ memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t));
+
+ // const initializer
+ image_u8x3_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf };
+
+ image_u8x3_t *copy = calloc(1, sizeof(image_u8x3_t));
+ memcpy(copy, &tmp, sizeof(image_u8x3_t));
+ return copy;
+}
+
+void image_u8x3_destroy(image_u8x3_t *im)
+{
+ if (!im)
+ return;
+
+ free(im->buf);
+ free(im);
+}
+
+////////////////////////////////////////////////////////////
+// PNM file i/o
+
+// Create an RGB image from PNM
+image_u8x3_t *image_u8x3_create_from_pnm(const char *path)
+{
+ pnm_t *pnm = pnm_create_from_file(path);
+ if (pnm == NULL)
+ return NULL;
+
+ image_u8x3_t *im = NULL;
+
+ switch (pnm->format) {
+ case PNM_FORMAT_GRAY: {
+ im = image_u8x3_create(pnm->width, pnm->height);
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ uint8_t gray = pnm->buf[y*im->width + x];
+ im->buf[y*im->stride + x*3 + 0] = gray;
+ im->buf[y*im->stride + x*3 + 1] = gray;
+ im->buf[y*im->stride + x*3 + 2] = gray;
+ }
+ }
+
+ break;
+ }
+
+ case PNM_FORMAT_RGB: {
+ im = image_u8x3_create(pnm->width, pnm->height);
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ uint8_t r = pnm->buf[y*im->width*3 + 3*x];
+ uint8_t g = pnm->buf[y*im->width*3 + 3*x+1];
+ uint8_t b = pnm->buf[y*im->width*3 + 3*x+2];
+
+ im->buf[y*im->stride + x*3 + 0] = r;
+ im->buf[y*im->stride + x*3 + 1] = g;
+ im->buf[y*im->stride + x*3 + 2] = b;
+ }
+ }
+
+ break;
+ }
+ }
+
+ pnm_destroy(pnm);
+ return im;
+}
+
+int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path)
+{
+ FILE *f = fopen(path, "wb");
+ int res = 0;
+
+ if (f == NULL) {
+ res = -1;
+ goto finish;
+ }
+
+ // Only outputs to RGB
+ fprintf(f, "P6\n%d %d\n255\n", im->width, im->height);
+ int linesz = im->width * 3;
+ for (int y = 0; y < im->height; y++) {
+ if (linesz != fwrite(&im->buf[y*im->stride], 1, linesz, f)) {
+ res = -1;
+ goto finish;
+ }
+ }
+
+finish:
+ if (f != NULL)
+ fclose(f);
+
+ return res;
+}
+
+// only width 1 supported
+void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width)
+{
+ double dist = sqrtf((y1-y0)*(y1-y0) + (x1-x0)*(x1-x0));
+ double delta = 0.5 / dist;
+
+ // terrible line drawing code
+ for (float f = 0; f <= 1; f += delta) {
+ int x = ((int) (x1 + (x0 - x1) * f));
+ int y = ((int) (y1 + (y0 - y1) * f));
+
+ if (x < 0 || y < 0 || x >= im->width || y >= im->height)
+ continue;
+
+ int idx = y*im->stride + 3*x;
+ for (int i = 0; i < 3; i++)
+ im->buf[idx + i] = rgb[i];
+ }
+}
+
+static void convolve(const uint8_t *x, uint8_t *y, int sz, const uint8_t *k, int ksz)
+{
+ assert((ksz&1)==1);
+
+ for (int i = 0; i < ksz/2 && i < sz; i++)
+ y[i] = x[i];
+
+ for (int i = 0; i < sz - ksz; i++) {
+ uint32_t acc = 0;
+
+ for (int j = 0; j < ksz; j++)
+ acc += k[j]*x[i+j];
+
+ y[ksz/2 + i] = acc >> 8;
+ }
+
+ for (int i = sz - ksz + ksz/2; i < sz; i++)
+ y[i] = x[i];
+}
+
+void image_u8x3_gaussian_blur(image_u8x3_t *im, double sigma, int ksz)
+{
+ if (sigma == 0)
+ return;
+
+ assert((ksz & 1) == 1); // ksz must be odd.
+
+ // build the kernel.
+#ifdef _MSC_VER
+ double *dk = malloc(ksz*sizeof *dk);
+#else
+ double dk[ksz];
+#endif
+
+ // for kernel of length 5:
+ // dk[0] = f(-2), dk[1] = f(-1), dk[2] = f(0), dk[3] = f(1), dk[4] = f(2)
+ for (int i = 0; i < ksz; i++) {
+ int x = -ksz/2 + i;
+ double v = exp(-.5*sq(x / sigma));
+ dk[i] = v;
+ }
+
+ // normalize
+ double acc = 0;
+ for (int i = 0; i < ksz; i++)
+ acc += dk[i];
+
+ for (int i = 0; i < ksz; i++)
+ dk[i] /= acc;
+
+#ifdef _MSC_VER
+ uint8_t *k = malloc(ksz*sizeof *k);
+#else
+ uint8_t k[ksz];
+#endif
+ for (int i = 0; i < ksz; i++)
+ k[i] = dk[i]*255;
+
+ if (0) {
+ for (int i = 0; i < ksz; i++)
+ printf("%d %15f %5d\n", i, dk[i], k[i]);
+ }
+
+ for (int c = 0; c < 3; c++) {
+ for (int y = 0; y < im->height; y++) {
+#ifdef _MSC_VER
+ uint8_t *in = malloc(im->stride*sizeof *in);
+ uint8_t *out = malloc(im->stride*sizeof *out);
+#else
+ uint8_t in[im->stride];
+ uint8_t out[im->stride];
+#endif
+
+ for (int x = 0; x < im->width; x++)
+ in[x] = im->buf[y*im->stride + 3 * x + c];
+
+ convolve(in, out, im->width, k, ksz);
+
+ for (int x = 0; x < im->width; x++)
+ im->buf[y*im->stride + 3 * x + c] = out[x];
+
+#ifdef _MSC_VER
+ free(in);
+ free(out);
+#endif
+ }
+
+ for (int x = 0; x < im->width; x++) {
+#ifdef _MSC_VER
+ uint8_t *in = malloc(im->height*sizeof *in);
+ uint8_t *out = malloc(im->height*sizeof *out);
+#else
+ uint8_t in[im->height];
+ uint8_t out[im->height];
+#endif
+
+ for (int y = 0; y < im->height; y++)
+ in[y] = im->buf[y*im->stride + 3*x + c];
+
+ convolve(in, out, im->height, k, ksz);
+
+ for (int y = 0; y < im->height; y++)
+ im->buf[y*im->stride + 3*x + c] = out[y];
+
+#ifdef _MSC_VER
+ free(in);
+ free(out);
+#endif
+ }
+ }
+
+#ifdef _MSC_VER
+ free(dk);
+#endif
+}
diff --git a/3rdparty/apriltag/common/image_u8x3.h b/3rdparty/apriltag/common/image_u8x3.h
new file mode 100644
index 0000000..670ebc0
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8x3.h
@@ -0,0 +1,61 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _IMAGE_U8X3_H
+#define _IMAGE_U8X3_H
+
+#include <stdint.h>
+#include "common/image_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/////////////////////////////////////
+// IMPORTANT NOTE ON BYTE ORDER
+//
+// Format conversion routines will (unless otherwise specified) assume
+// R, G, B, ordering of bytes. This is consistent with GTK, PNM, etc.
+//
+/////////////////////////////////////
+
+// Create or load an image. returns NULL on failure
+image_u8x3_t *image_u8x3_create(unsigned int width, unsigned int height);
+image_u8x3_t *image_u8x3_create_alignment(unsigned int width, unsigned int height, unsigned int alignment);
+image_u8x3_t *image_u8x3_create_from_pnm(const char *path);
+
+image_u8x3_t *image_u8x3_copy(const image_u8x3_t *in);
+
+void image_u8x3_gaussian_blur(image_u8x3_t *im, double sigma, int ksz);
+
+void image_u8x3_destroy(image_u8x3_t *im);
+
+int image_u8x3_write_pnm(const image_u8x3_t *im, const char *path);
+
+// only width 1 supported
+void image_u8x3_draw_line(image_u8x3_t *im, float x0, float y0, float x1, float y1, uint8_t rgb[3], int width);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/image_u8x4.c b/3rdparty/apriltag/common/image_u8x4.c
new file mode 100644
index 0000000..53d73f3
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8x4.c
@@ -0,0 +1,227 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include "pam.h"
+#include "pnm.h"
+#include "image_u8x4.h"
+
+// least common multiple of 64 (sandy bridge cache line) and 64 (stride needed
+// for 16byte-wide RGBA processing).
+#define DEFAULT_ALIGNMENT_U8X4 64
+
+image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height)
+{
+ return image_u8x4_create_alignment(width, height, DEFAULT_ALIGNMENT_U8X4);
+}
+
+image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment)
+{
+ int stride = 4*width;
+
+ if ((stride % alignment) != 0)
+ stride += alignment - (stride % alignment);
+
+ uint8_t *buf = calloc(height*stride, sizeof(uint8_t));
+
+ // const initializer
+ image_u8x4_t tmp = { .width = width, .height = height, .stride = stride, .buf = buf };
+
+ image_u8x4_t *im = calloc(1, sizeof(image_u8x4_t));
+ memcpy(im, &tmp, sizeof(image_u8x4_t));
+ return im;
+}
+
+image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in)
+{
+ uint8_t *buf = malloc(in->height*in->stride*sizeof(uint8_t));
+ memcpy(buf, in->buf, in->height*in->stride*sizeof(uint8_t));
+
+ // const initializer
+ image_u8x4_t tmp = { .width = in->width, .height = in->height, .stride = in->stride, .buf = buf };
+
+ image_u8x4_t *copy = calloc(1, sizeof(image_u8x4_t));
+ memcpy(copy, &tmp, sizeof(image_u8x4_t));
+ return copy;
+}
+
+void image_u8x4_destroy(image_u8x4_t *im)
+{
+ if (!im)
+ return;
+
+ free(im->buf);
+ free(im);
+}
+
+////////////////////////////////////////////////////////////
+image_u8x4_t *image_u8x4_create_from_pam(const char *inpath)
+{
+ pam_t *pam = pam_create_from_file(inpath);
+ if (!pam)
+ return NULL;
+
+ image_u8x4_t *im = image_u8x4_create(pam->width, pam->height);
+
+ for (int y = 0; y < pam->height; y++) {
+ if (pam->depth == 1) {
+ for (int x = 0; x < pam->width; x++) {
+ im->buf[y*im->stride + 4*x + 0] = pam->data[pam->width*y + x + 0];
+ im->buf[y*im->stride + 4*x + 1] = pam->data[pam->width*y + x + 0];
+ im->buf[y*im->stride + 4*x + 2] = pam->data[pam->width*y + x + 0];
+ im->buf[y*im->stride + 4*x + 3] = 255;
+ }
+ } else if (pam->depth == 3) {
+ for (int x = 0; x < pam->width; x++) {
+ im->buf[y*im->stride + 4*x + 0] = pam->data[3*pam->width*y + 3*x + 0];
+ im->buf[y*im->stride + 4*x + 1] = pam->data[3*pam->width*y + 3*x + 1];
+ im->buf[y*im->stride + 4*x + 2] = pam->data[3*pam->width*y + 3*x + 2];
+ im->buf[y*im->stride + 4*x + 3] = 255;
+ }
+ } else if (pam->depth == 4) {
+ memcpy(&im->buf[y*im->stride], &pam->data[4*pam->width*y], 4*pam->width);
+ } else {
+ assert(0); // not implemented
+ }
+ }
+
+ pam_destroy(pam);
+ return im;
+}
+////////////////////////////////////////////////////////////
+// PNM file i/o
+
+// Create an RGBA image from PNM
+image_u8x4_t *image_u8x4_create_from_pnm(const char *path)
+{
+ pnm_t *pnmp = pnm_create_from_file(path);
+ if (pnmp == NULL)
+ return NULL;
+
+ pnm_t pnm = *pnmp;
+ image_u8x4_t *imp = NULL;
+
+ switch (pnm.format) {
+ case PNM_FORMAT_GRAY: {
+ imp = image_u8x4_create(pnm.width, pnm.height);
+
+ // copy struct by value for common subexpression elimination
+ const image_u8x4_t im = *imp;
+
+ for (int y = 0; y < im.height; y++) {
+ for (int x = 0; x < im.width; x++) {
+ uint8_t gray = pnm.buf[y*pnm.width + x];
+ im.buf[y*im.stride + 4*x + 0] = gray;
+ im.buf[y*im.stride + 4*x + 1] = gray;
+ im.buf[y*im.stride + 4*x + 2] = gray;
+ im.buf[y*im.stride + 4*x + 3] = 0xff;
+ }
+ }
+
+ break;
+ }
+
+ case PNM_FORMAT_RGB: {
+ imp = image_u8x4_create(pnm.width, pnm.height);
+
+ // copy struct by value for common subexpression elimination
+ const image_u8x4_t im = *imp;
+
+ // Gray conversion for RGB is gray = (r + g + g + b)/4
+ for (int y = 0; y < im.height; y++) {
+ for (int x = 0; x < im.width; x++) {
+
+ uint8_t r = pnm.buf[y*pnm.width*3 + 3*x + 0];
+ uint8_t g = pnm.buf[y*pnm.width*3 + 3*x + 1];
+ uint8_t b = pnm.buf[y*pnm.width*3 + 3*x + 2];
+
+ im.buf[y*im.stride + 4*x + 0] = r;
+ im.buf[y*im.stride + 4*x + 1] = g;
+ im.buf[y*im.stride + 4*x + 2] = b;
+ im.buf[y*im.stride + 4*x + 3] = 0xff;
+ }
+ }
+
+ break;
+ }
+ }
+
+ pnm_destroy(pnmp);
+ return imp;
+}
+
+int image_u8x4_write_pnm(const image_u8x4_t *imp, const char *path)
+{
+ // copy struct by value to ensure common subexpression elimination occurs
+ const image_u8x4_t im = *imp;
+
+ FILE *f = fopen(path, "wb");
+ int res = 0;
+
+ if (f == NULL) {
+ res = -1;
+ goto finish;
+ }
+
+ // Only outputs to RGB
+ fprintf(f, "P6\n%d %d\n255\n", im.width, im.height);
+
+ for (int y = im.height-1; y >= 0; y--) {
+ for (int x = 0; x < im.width; x++) {
+
+ uint8_t r = im.buf[y*im.stride + 4*x + 0];
+ uint8_t g = im.buf[y*im.stride + 4*x + 1];
+ uint8_t b = im.buf[y*im.stride + 4*x + 2];
+
+ fwrite(&r, 1, 1, f);
+ fwrite(&g, 1, 1, f);
+ fwrite(&b, 1, 1, f);
+ }
+ }
+
+ finish:
+ if (f != NULL)
+ fclose(f);
+
+ return res;
+}
+
+void image_u8x4_write_pam(const image_u8x4_t *im, const char *path)
+{
+ FILE *f = fopen(path, "w");
+ fprintf(f, "P7\n");
+ fprintf(f, "WIDTH %d\n", im->width);
+ fprintf(f, "HEIGHT %d\n", im->height);
+ fprintf(f, "DEPTH 4\n");
+ fprintf(f, "MAXVAL 255\n");
+ fprintf(f, "TUPLTYPE RGB_ALPHA\n");
+ fprintf(f, "ENDHDR\n");
+
+ for (int y = 0; y < im->height; y++)
+ fwrite(&im->buf[y*im->stride], 1, 4*im->width, f);
+
+ fclose(f);
+
+}
diff --git a/3rdparty/apriltag/common/image_u8x4.h b/3rdparty/apriltag/common/image_u8x4.h
new file mode 100644
index 0000000..f8746ed
--- /dev/null
+++ b/3rdparty/apriltag/common/image_u8x4.h
@@ -0,0 +1,62 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _IMAGE_U8X4_H
+#define _IMAGE_U8X4_H
+
+#include <stdint.h>
+#include "common/image_types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/////////////////////////////////////
+// IMPORTANT NOTE ON BYTE ORDER
+//
+// Format conversion routines will (unless otherwise specified) assume
+// R, G, B, A ordering of bytes.
+//
+/////////////////////////////////////
+
+// Create or load an image. returns NULL on failure
+image_u8x4_t *image_u8x4_create(unsigned int width, unsigned int height);
+image_u8x4_t *image_u8x4_create_alignment(unsigned int width, unsigned int height, unsigned int alignment);
+image_u8x4_t *image_u8x4_create_from_pnm(const char *path);
+
+image_u8x4_t *image_u8x4_copy(const image_u8x4_t *in);
+
+void image_u8x4_destroy(image_u8x4_t *im);
+
+// Write a pnm. Return 0 on success.
+// Currently supports GRAY and RGB
+int image_u8x4_write_pnm(const image_u8x4_t *im, const char *path);
+
+image_u8x4_t *image_u8x4_create_from_pam(const char *path);
+
+ void image_u8x4_write_pam(const image_u8x4_t *im, const char *path);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/matd.c b/3rdparty/apriltag/common/matd.c
new file mode 100644
index 0000000..d41ddc3
--- /dev/null
+++ b/3rdparty/apriltag/common/matd.c
@@ -0,0 +1,2067 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdarg.h>
+#include <assert.h>
+#include <math.h>
+#include <float.h>
+
+#include "common/math_util.h"
+#include "common/svd22.h"
+#include "common/matd.h"
+
+// a matd_t with rows=0 cols=0 is a SCALAR.
+
+// to ease creating mati, matf, etc. in the future.
+#define TYPE double
+
+matd_t *matd_create(int rows, int cols)
+{
+ assert(rows >= 0);
+ assert(cols >= 0);
+
+ if (rows == 0 || cols == 0)
+ return matd_create_scalar(0);
+
+ matd_t *m = calloc(1, sizeof(matd_t) + (rows*cols*sizeof(double)));
+ m->nrows = rows;
+ m->ncols = cols;
+
+ return m;
+}
+
+matd_t *matd_create_scalar(TYPE v)
+{
+ matd_t *m = calloc(1, sizeof(matd_t) + sizeof(double));
+ m->nrows = 0;
+ m->ncols = 0;
+ m->data[0] = v;
+
+ return m;
+}
+
+matd_t *matd_create_data(int rows, int cols, const TYPE *data)
+{
+ if (rows == 0 || cols == 0)
+ return matd_create_scalar(data[0]);
+
+ matd_t *m = matd_create(rows, cols);
+ for (int i = 0; i < rows * cols; i++)
+ m->data[i] = data[i];
+
+ return m;
+}
+
+matd_t *matd_create_dataf(int rows, int cols, const float *data)
+{
+ if (rows == 0 || cols == 0)
+ return matd_create_scalar(data[0]);
+
+ matd_t *m = matd_create(rows, cols);
+ for (int i = 0; i < rows * cols; i++)
+ m->data[i] = (double)data[i];
+
+ return m;
+}
+
+matd_t *matd_identity(int dim)
+{
+ if (dim == 0)
+ return matd_create_scalar(1);
+
+ matd_t *m = matd_create(dim, dim);
+ for (int i = 0; i < dim; i++)
+ MATD_EL(m, i, i) = 1;
+
+ return m;
+}
+
+// row and col are zero-based
+TYPE matd_get(const matd_t *m, int row, int col)
+{
+ assert(m != NULL);
+ assert(!matd_is_scalar(m));
+ assert(row >= 0);
+ assert(row < m->nrows);
+ assert(col >= 0);
+ assert(col < m->ncols);
+
+ return MATD_EL(m, row, col);
+}
+
+// row and col are zero-based
+void matd_put(matd_t *m, int row, int col, TYPE value)
+{
+ assert(m != NULL);
+
+ if (matd_is_scalar(m)) {
+ matd_put_scalar(m, value);
+ return;
+ }
+
+ assert(row >= 0);
+ assert(row < m->nrows);
+ assert(col >= 0);
+ assert(col < m->ncols);
+
+ MATD_EL(m, row, col) = value;
+}
+
+TYPE matd_get_scalar(const matd_t *m)
+{
+ assert(m != NULL);
+ assert(matd_is_scalar(m));
+
+ return (m->data[0]);
+}
+
+void matd_put_scalar(matd_t *m, TYPE value)
+{
+ assert(m != NULL);
+ assert(matd_is_scalar(m));
+
+ m->data[0] = value;
+}
+
+matd_t *matd_copy(const matd_t *m)
+{
+ assert(m != NULL);
+
+ matd_t *x = matd_create(m->nrows, m->ncols);
+ if (matd_is_scalar(m))
+ x->data[0] = m->data[0];
+ else
+ memcpy(x->data, m->data, sizeof(TYPE)*m->ncols*m->nrows);
+
+ return x;
+}
+
+matd_t *matd_select(const matd_t * a, int r0, int r1, int c0, int c1)
+{
+ assert(a != NULL);
+
+ assert(r0 >= 0 && r0 < a->nrows);
+ assert(c0 >= 0 && c0 < a->ncols);
+
+ int nrows = r1 - r0 + 1;
+ int ncols = c1 - c0 + 1;
+
+ matd_t * r = matd_create(nrows, ncols);
+
+ for (int row = r0; row <= r1; row++)
+ for (int col = c0; col <= c1; col++)
+ MATD_EL(r,row-r0,col-c0) = MATD_EL(a,row,col);
+
+ return r;
+}
+
+void matd_print(const matd_t *m, const char *fmt)
+{
+ assert(m != NULL);
+ assert(fmt != NULL);
+
+ if (matd_is_scalar(m)) {
+ printf(fmt, MATD_EL(m, 0, 0));
+ printf("\n");
+ } else {
+ for (int i = 0; i < m->nrows; i++) {
+ for (int j = 0; j < m->ncols; j++) {
+ printf(fmt, MATD_EL(m, i, j));
+ }
+ printf("\n");
+ }
+ }
+}
+
+void matd_print_transpose(const matd_t *m, const char *fmt)
+{
+ assert(m != NULL);
+ assert(fmt != NULL);
+
+ if (matd_is_scalar(m)) {
+ printf(fmt, MATD_EL(m, 0, 0));
+ printf("\n");
+ } else {
+ for (int j = 0; j < m->ncols; j++) {
+ for (int i = 0; i < m->nrows; i++) {
+ printf(fmt, MATD_EL(m, i, j));
+ }
+ printf("\n");
+ }
+ }
+}
+
+void matd_destroy(matd_t *m)
+{
+ if (!m)
+ return;
+
+ assert(m != NULL);
+ free(m);
+}
+
+matd_t *matd_multiply(const matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+
+ if (matd_is_scalar(a))
+ return matd_scale(b, a->data[0]);
+ if (matd_is_scalar(b))
+ return matd_scale(a, b->data[0]);
+
+ assert(a->ncols == b->nrows);
+ matd_t *m = matd_create(a->nrows, b->ncols);
+
+ for (int i = 0; i < m->nrows; i++) {
+ for (int j = 0; j < m->ncols; j++) {
+ TYPE acc = 0;
+ for (int k = 0; k < a->ncols; k++) {
+ acc += MATD_EL(a, i, k) * MATD_EL(b, k, j);
+ }
+ MATD_EL(m, i, j) = acc;
+ }
+ }
+
+ return m;
+}
+
+matd_t *matd_scale(const matd_t *a, double s)
+{
+ assert(a != NULL);
+
+ if (matd_is_scalar(a))
+ return matd_create_scalar(a->data[0] * s);
+
+ matd_t *m = matd_create(a->nrows, a->ncols);
+
+ for (int i = 0; i < m->nrows; i++) {
+ for (int j = 0; j < m->ncols; j++) {
+ MATD_EL(m, i, j) = s * MATD_EL(a, i, j);
+ }
+ }
+
+ return m;
+}
+
+void matd_scale_inplace(matd_t *a, double s)
+{
+ assert(a != NULL);
+
+ if (matd_is_scalar(a)) {
+ a->data[0] *= s;
+ return;
+ }
+
+ for (int i = 0; i < a->nrows; i++) {
+ for (int j = 0; j < a->ncols; j++) {
+ MATD_EL(a, i, j) *= s;
+ }
+ }
+}
+
+matd_t *matd_add(const matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(a->nrows == b->nrows);
+ assert(a->ncols == b->ncols);
+
+ if (matd_is_scalar(a))
+ return matd_create_scalar(a->data[0] + b->data[0]);
+
+ matd_t *m = matd_create(a->nrows, a->ncols);
+
+ for (int i = 0; i < m->nrows; i++) {
+ for (int j = 0; j < m->ncols; j++) {
+ MATD_EL(m, i, j) = MATD_EL(a, i, j) + MATD_EL(b, i, j);
+ }
+ }
+
+ return m;
+}
+
+void matd_add_inplace(matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(a->nrows == b->nrows);
+ assert(a->ncols == b->ncols);
+
+ if (matd_is_scalar(a)) {
+ a->data[0] += b->data[0];
+ return;
+ }
+
+ for (int i = 0; i < a->nrows; i++) {
+ for (int j = 0; j < a->ncols; j++) {
+ MATD_EL(a, i, j) += MATD_EL(b, i, j);
+ }
+ }
+}
+
+
+matd_t *matd_subtract(const matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(a->nrows == b->nrows);
+ assert(a->ncols == b->ncols);
+
+ if (matd_is_scalar(a))
+ return matd_create_scalar(a->data[0] - b->data[0]);
+
+ matd_t *m = matd_create(a->nrows, a->ncols);
+
+ for (int i = 0; i < m->nrows; i++) {
+ for (int j = 0; j < m->ncols; j++) {
+ MATD_EL(m, i, j) = MATD_EL(a, i, j) - MATD_EL(b, i, j);
+ }
+ }
+
+ return m;
+}
+
+void matd_subtract_inplace(matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(a->nrows == b->nrows);
+ assert(a->ncols == b->ncols);
+
+ if (matd_is_scalar(a)) {
+ a->data[0] -= b->data[0];
+ return;
+ }
+
+ for (int i = 0; i < a->nrows; i++) {
+ for (int j = 0; j < a->ncols; j++) {
+ MATD_EL(a, i, j) -= MATD_EL(b, i, j);
+ }
+ }
+}
+
+
+matd_t *matd_transpose(const matd_t *a)
+{
+ assert(a != NULL);
+
+ if (matd_is_scalar(a))
+ return matd_create_scalar(a->data[0]);
+
+ matd_t *m = matd_create(a->ncols, a->nrows);
+
+ for (int i = 0; i < a->nrows; i++) {
+ for (int j = 0; j < a->ncols; j++) {
+ MATD_EL(m, j, i) = MATD_EL(a, i, j);
+ }
+ }
+ return m;
+}
+
+static
+double matd_det_general(const matd_t *a)
+{
+ // Use LU decompositon to calculate the determinant
+ matd_plu_t *mlu = matd_plu(a);
+ matd_t *L = matd_plu_l(mlu);
+ matd_t *U = matd_plu_u(mlu);
+
+ // The determinants of the L and U matrices are the products of
+ // their respective diagonal elements
+ double detL = 1; double detU = 1;
+ for (int i = 0; i < a->nrows; i++) {
+ detL *= matd_get(L, i, i);
+ detU *= matd_get(U, i, i);
+ }
+
+ // The determinant of a can be calculated as
+ // epsilon*det(L)*det(U),
+ // where epsilon is just the sign of the corresponding permutation
+ // (which is +1 for an even number of permutations and is −1
+ // for an uneven number of permutations).
+ double det = mlu->pivsign * detL * detU;
+
+ // Cleanup
+ matd_plu_destroy(mlu);
+ matd_destroy(L);
+ matd_destroy(U);
+
+ return det;
+}
+
+double matd_det(const matd_t *a)
+{
+ assert(a != NULL);
+ assert(a->nrows == a->ncols);
+
+ switch(a->nrows) {
+ case 0:
+ // scalar: invalid
+ assert(a->nrows > 0);
+ break;
+
+ case 1:
+ // 1x1 matrix
+ return a->data[0];
+
+ case 2:
+ // 2x2 matrix
+ return a->data[0] * a->data[3] - a->data[1] * a->data[2];
+
+ case 3:
+ // 3x3 matrix
+ return a->data[0]*a->data[4]*a->data[8]
+ - a->data[0]*a->data[5]*a->data[7]
+ + a->data[1]*a->data[5]*a->data[6]
+ - a->data[1]*a->data[3]*a->data[8]
+ + a->data[2]*a->data[3]*a->data[7]
+ - a->data[2]*a->data[4]*a->data[6];
+
+ case 4: {
+ // 4x4 matrix
+ double m00 = MATD_EL(a,0,0), m01 = MATD_EL(a,0,1), m02 = MATD_EL(a,0,2), m03 = MATD_EL(a,0,3);
+ double m10 = MATD_EL(a,1,0), m11 = MATD_EL(a,1,1), m12 = MATD_EL(a,1,2), m13 = MATD_EL(a,1,3);
+ double m20 = MATD_EL(a,2,0), m21 = MATD_EL(a,2,1), m22 = MATD_EL(a,2,2), m23 = MATD_EL(a,2,3);
+ double m30 = MATD_EL(a,3,0), m31 = MATD_EL(a,3,1), m32 = MATD_EL(a,3,2), m33 = MATD_EL(a,3,3);
+
+ return m00 * m11 * m22 * m33 - m00 * m11 * m23 * m32 -
+ m00 * m21 * m12 * m33 + m00 * m21 * m13 * m32 + m00 * m31 * m12 * m23 -
+ m00 * m31 * m13 * m22 - m10 * m01 * m22 * m33 +
+ m10 * m01 * m23 * m32 + m10 * m21 * m02 * m33 -
+ m10 * m21 * m03 * m32 - m10 * m31 * m02 * m23 +
+ m10 * m31 * m03 * m22 + m20 * m01 * m12 * m33 -
+ m20 * m01 * m13 * m32 - m20 * m11 * m02 * m33 +
+ m20 * m11 * m03 * m32 + m20 * m31 * m02 * m13 -
+ m20 * m31 * m03 * m12 - m30 * m01 * m12 * m23 +
+ m30 * m01 * m13 * m22 + m30 * m11 * m02 * m23 -
+ m30 * m11 * m03 * m22 - m30 * m21 * m02 * m13 +
+ m30 * m21 * m03 * m12;
+ }
+
+ default:
+ return matd_det_general(a);
+ }
+
+ assert(0);
+ return 0;
+}
+
+// returns NULL if the matrix is (exactly) singular. Caller is
+// otherwise responsible for knowing how to cope with badly
+// conditioned matrices.
+matd_t *matd_inverse(const matd_t *x)
+{
+ matd_t *m = NULL;
+
+ assert(x != NULL);
+ assert(x->nrows == x->ncols);
+
+ if (matd_is_scalar(x)) {
+ if (x->data[0] == 0)
+ return NULL;
+
+ return matd_create_scalar(1.0 / x->data[0]);
+ }
+
+ switch(x->nrows) {
+ case 1: {
+ double det = x->data[0];
+ if (det == 0)
+ return NULL;
+
+ double invdet = 1.0 / det;
+
+ m = matd_create(x->nrows, x->nrows);
+ MATD_EL(m, 0, 0) = 1.0 * invdet;
+ return m;
+ }
+
+ case 2: {
+ double det = x->data[0] * x->data[3] - x->data[1] * x->data[2];
+ if (det == 0)
+ return NULL;
+
+ double invdet = 1.0 / det;
+
+ m = matd_create(x->nrows, x->nrows);
+ MATD_EL(m, 0, 0) = MATD_EL(x, 1, 1) * invdet;
+ MATD_EL(m, 0, 1) = - MATD_EL(x, 0, 1) * invdet;
+ MATD_EL(m, 1, 0) = - MATD_EL(x, 1, 0) * invdet;
+ MATD_EL(m, 1, 1) = MATD_EL(x, 0, 0) * invdet;
+ return m;
+ }
+
+ default: {
+ matd_plu_t *plu = matd_plu(x);
+
+ matd_t *inv = NULL;
+ if (!plu->singular) {
+ matd_t *ident = matd_identity(x->nrows);
+ inv = matd_plu_solve(plu, ident);
+ matd_destroy(ident);
+ }
+
+ matd_plu_destroy(plu);
+
+ return inv;
+ }
+ }
+
+ return NULL; // unreachable
+}
+
+
+
+// TODO Optimization: Some operations we could perform in-place,
+// saving some memory allocation work. E.g., ADD, SUBTRACT. Just need
+// to make sure that we don't do an in-place modification on a matrix
+// that was an input argument!
+
+// handle right-associative operators, greedily consuming them. These
+// include transpose and inverse. This is called by the main recursion
+// method.
+static inline matd_t *matd_op_gobble_right(const char *expr, int *pos, matd_t *acc, matd_t **garb, int *garbpos)
+{
+ while (expr[*pos] != 0) {
+
+ switch (expr[*pos]) {
+
+ case '\'': {
+ assert(acc != NULL); // either a syntax error or a math op failed, producing null
+ matd_t *res = matd_transpose(acc);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+
+ (*pos)++;
+ break;
+ }
+
+ // handle inverse ^-1. No other exponents are allowed.
+ case '^': {
+ assert(acc != NULL);
+ assert(expr[*pos+1] == '-');
+ assert(expr[*pos+2] == '1');
+
+ matd_t *res = matd_inverse(acc);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+
+ (*pos)+=3;
+ break;
+ }
+
+ default:
+ return acc;
+ }
+ }
+
+ return acc;
+}
+
+// @garb, garbpos A list of every matrix allocated during evaluation... used to assist cleanup.
+// @oneterm: we should return at the end of this term (i.e., stop at a PLUS, MINUS, LPAREN).
+static matd_t *matd_op_recurse(const char *expr, int *pos, matd_t *acc, matd_t **args, int *argpos,
+ matd_t **garb, int *garbpos, int oneterm)
+{
+ while (expr[*pos] != 0) {
+
+ switch (expr[*pos]) {
+
+ case '(': {
+ if (oneterm && acc != NULL)
+ return acc;
+ (*pos)++;
+ matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 0);
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ if (acc == NULL) {
+ acc = rhs;
+ } else {
+ matd_t *res = matd_multiply(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+
+ break;
+ }
+
+ case ')': {
+ if (oneterm)
+ return acc;
+
+ (*pos)++;
+ return acc;
+ }
+
+ case '*': {
+ (*pos)++;
+
+ matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ if (acc == NULL) {
+ acc = rhs;
+ } else {
+ matd_t *res = matd_multiply(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+
+ break;
+ }
+
+ case 'F': {
+ matd_t *rhs = args[*argpos];
+ garb[*garbpos] = rhs;
+ (*garbpos)++;
+
+ (*pos)++;
+ (*argpos)++;
+
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ if (acc == NULL) {
+ acc = rhs;
+ } else {
+ matd_t *res = matd_multiply(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+
+ break;
+ }
+
+ case 'M': {
+ matd_t *rhs = args[*argpos];
+
+ (*pos)++;
+ (*argpos)++;
+
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ if (acc == NULL) {
+ acc = rhs;
+ } else {
+ matd_t *res = matd_multiply(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+
+ break;
+ }
+
+/*
+ case 'D': {
+ int rows = expr[*pos+1]-'0';
+ int cols = expr[*pos+2]-'0';
+
+ matd_t *rhs = matd_create(rows, cols);
+
+ break;
+ }
+*/
+ // a constant (SCALAR) defined inline. Treat just like M, creating a matd_t on the fly.
+ case '0':
+ case '1':
+ case '2':
+ case '3':
+ case '4':
+ case '5':
+ case '6':
+ case '7':
+ case '8':
+ case '9':
+ case '.': {
+ const char *start = &expr[*pos];
+ char *end;
+ double s = strtod(start, &end);
+ (*pos) += (end - start);
+ matd_t *rhs = matd_create_scalar(s);
+ garb[*garbpos] = rhs;
+ (*garbpos)++;
+
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ if (acc == NULL) {
+ acc = rhs;
+ } else {
+ matd_t *res = matd_multiply(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+
+ break;
+ }
+
+ case '+': {
+ if (oneterm && acc != NULL)
+ return acc;
+
+ // don't support unary plus
+ assert(acc != NULL);
+ (*pos)++;
+ matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ matd_t *res = matd_add(acc, rhs);
+
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ break;
+ }
+
+ case '-': {
+ if (oneterm && acc != NULL)
+ return acc;
+
+ if (acc == NULL) {
+ // unary minus
+ (*pos)++;
+ matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ matd_t *res = matd_scale(rhs, -1);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ } else {
+ // subtract
+ (*pos)++;
+ matd_t *rhs = matd_op_recurse(expr, pos, NULL, args, argpos, garb, garbpos, 1);
+ rhs = matd_op_gobble_right(expr, pos, rhs, garb, garbpos);
+
+ matd_t *res = matd_subtract(acc, rhs);
+ garb[*garbpos] = res;
+ (*garbpos)++;
+ acc = res;
+ }
+ break;
+ }
+
+ case ' ': {
+ // nothing to do. spaces are meaningless.
+ (*pos)++;
+ break;
+ }
+
+ default: {
+ fprintf(stderr, "matd_op(): Unknown character: '%c'\n", expr[*pos]);
+ assert(expr[*pos] != expr[*pos]);
+ }
+ }
+ }
+ return acc;
+}
+
+// always returns a new matrix.
+matd_t *matd_op(const char *expr, ...)
+{
+ int nargs = 0;
+ int exprlen = 0;
+
+ assert(expr != NULL);
+
+ for (const char *p = expr; *p != 0; p++) {
+ if (*p == 'M' || *p == 'F')
+ nargs++;
+ exprlen++;
+ }
+
+ assert(nargs > 0);
+
+ if (!exprlen) // expr = ""
+ return NULL;
+
+ va_list ap;
+ va_start(ap, expr);
+
+#ifdef _MSC_VER
+ matd_t **args = malloc(nargs*sizeof *args);
+#else
+ matd_t *args[nargs];
+#endif
+ for (int i = 0; i < nargs; i++) {
+ args[i] = va_arg(ap, matd_t*);
+ // XXX: sanity check argument; emit warning/error if args[i]
+ // doesn't look like a matd_t*.
+ }
+
+ va_end(ap);
+
+ int pos = 0;
+ int argpos = 0;
+ int garbpos = 0;
+
+#ifdef _MSC_VER
+ matd_t **garb = malloc(2 * exprlen*sizeof *garb);
+#else
+ matd_t *garb[2*exprlen]; // can't create more than 2 new result per character
+ // one result, and possibly one argument to free
+#endif
+
+ matd_t *res = matd_op_recurse(expr, &pos, NULL, args, &argpos, garb, &garbpos, 0);
+
+ // 'res' may need to be freed as part of garbage collection (i.e. expr = "F")
+ matd_t *res_copy = (res ? matd_copy(res) : NULL);
+
+ for (int i = 0; i < garbpos; i++) {
+ matd_destroy(garb[i]);
+ }
+
+#ifdef _MSC_VER
+ free(args);
+ free(garb);
+#endif
+
+ return res_copy;
+}
+
+double matd_vec_mag(const matd_t *a)
+{
+ assert(a != NULL);
+ assert(matd_is_vector(a));
+
+ double mag = 0.0;
+ int len = a->nrows*a->ncols;
+ for (int i = 0; i < len; i++)
+ mag += sq(a->data[i]);
+ return sqrt(mag);
+}
+
+double matd_vec_dist(const matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(matd_is_vector(a) && matd_is_vector(b));
+ assert(a->nrows*a->ncols == b->nrows*b->ncols);
+
+ int lena = a->nrows*a->ncols;
+ return matd_vec_dist_n(a, b, lena);
+}
+
+double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(matd_is_vector(a) && matd_is_vector(b));
+
+ int lena = a->nrows*a->ncols;
+ int lenb = b->nrows*b->ncols;
+
+ assert(n <= lena && n <= lenb);
+
+ double mag = 0.0;
+ for (int i = 0; i < n; i++)
+ mag += sq(a->data[i] - b->data[i]);
+ return sqrt(mag);
+}
+
+// find the index of the off-diagonal element with the largest mag
+static inline int max_idx(const matd_t *A, int row, int maxcol)
+{
+ int maxi = 0;
+ double maxv = -1;
+
+ for (int i = 0; i < maxcol; i++) {
+ if (i == row)
+ continue;
+ double v = fabs(MATD_EL(A, row, i));
+ if (v > maxv) {
+ maxi = i;
+ maxv = v;
+ }
+ }
+
+ return maxi;
+}
+
+double matd_vec_dot_product(const matd_t *a, const matd_t *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(matd_is_vector(a) && matd_is_vector(b));
+ int adim = a->ncols*a->nrows;
+ int bdim = b->ncols*b->nrows;
+ assert(adim == bdim);
+
+ double acc = 0;
+ for (int i = 0; i < adim; i++) {
+ acc += a->data[i] * b->data[i];
+ }
+ return acc;
+}
+
+
+matd_t *matd_vec_normalize(const matd_t *a)
+{
+ assert(a != NULL);
+ assert(matd_is_vector(a));
+
+ double mag = matd_vec_mag(a);
+ assert(mag > 0);
+
+ matd_t *b = matd_create(a->nrows, a->ncols);
+
+ int len = a->nrows*a->ncols;
+ for(int i = 0; i < len; i++)
+ b->data[i] = a->data[i] / mag;
+
+ return b;
+}
+
+matd_t *matd_crossproduct(const matd_t *a, const matd_t *b)
+{ // only defined for vecs (col or row) of length 3
+ assert(a != NULL);
+ assert(b != NULL);
+ assert(matd_is_vector_len(a, 3) && matd_is_vector_len(b, 3));
+
+ matd_t * r = matd_create(a->nrows, a->ncols);
+
+ r->data[0] = a->data[1] * b->data[2] - a->data[2] * b->data[1];
+ r->data[1] = a->data[2] * b->data[0] - a->data[0] * b->data[2];
+ r->data[2] = a->data[0] * b->data[1] - a->data[1] * b->data[0];
+
+ return r;
+}
+
+TYPE matd_err_inf(const matd_t *a, const matd_t *b)
+{
+ assert(a->nrows == b->nrows);
+ assert(a->ncols == b->ncols);
+
+ TYPE maxf = 0;
+
+ for (int i = 0; i < a->nrows; i++) {
+ for (int j = 0; j < a->ncols; j++) {
+ TYPE av = MATD_EL(a, i, j);
+ TYPE bv = MATD_EL(b, i, j);
+
+ TYPE err = fabs(av - bv);
+ maxf = fmax(maxf, err);
+ }
+ }
+
+ return maxf;
+}
+
+// Computes an SVD for square or tall matrices. This code doesn't work
+// for wide matrices, because the bidiagonalization results in one
+// non-zero element too far to the right for us to rotate away.
+//
+// Caller is responsible for destroying U, S, and V.
+static matd_svd_t matd_svd_tall(matd_t *A, int flags)
+{
+ matd_t *B = matd_copy(A);
+
+ // Apply householder reflections on each side to reduce A to
+ // bidiagonal form. Specifically:
+ //
+ // A = LS*B*RS'
+ //
+ // Where B is bidiagonal, and LS/RS are unitary.
+ //
+ // Why are we doing this? Some sort of transformation is necessary
+ // to reduce the matrix's nz elements to a square region. QR could
+ // work too. We need nzs confined to a square region so that the
+ // subsequent iterative process, which is based on rotations, can
+ // work. (To zero out a term at (i,j), our rotations will also
+ // affect (j,i).
+ //
+ // We prefer bidiagonalization over QR because it gets us "closer"
+ // to the SVD, which should mean fewer iterations.
+
+ // LS: cumulative left-handed transformations
+ matd_t *LS = matd_identity(A->nrows);
+
+ // RS: cumulative right-handed transformations.
+ matd_t *RS = matd_identity(A->ncols);
+
+ for (int hhidx = 0; hhidx < A->nrows; hhidx++) {
+
+ if (hhidx < A->ncols) {
+ // We construct the normal of the reflection plane: let u
+ // be the vector to reflect, x =[ M 0 0 0 ] the target
+ // location for u (u') after reflection (with M = ||u||).
+ //
+ // The normal vector is then n = (u - x), but since we
+ // could equally have the target location be x = [-M 0 0 0
+ // ], we could use n = (u + x).
+ //
+ // We then normalize n. To ensure a reasonable magnitude,
+ // we select the sign of M so as to maximize the magnitude
+ // of the first element of (x +/- M). (Otherwise, we could
+ // end up with a divide-by-zero if u[0] and M cancel.)
+ //
+ // The householder reflection matrix is then H=(I - nn'), and
+ // u' = Hu.
+ //
+ //
+ int vlen = A->nrows - hhidx;
+
+#ifdef _MSC_VER
+ double *v = malloc(vlen*sizeof *v);
+#else
+ double v[vlen];
+#endif
+
+ double mag2 = 0;
+ for (int i = 0; i < vlen; i++) {
+ v[i] = MATD_EL(B, hhidx+i, hhidx);
+ mag2 += v[i]*v[i];
+ }
+
+ double oldv0 = v[0];
+ if (oldv0 < 0)
+ v[0] -= sqrt(mag2);
+ else
+ v[0] += sqrt(mag2);
+
+ mag2 += -oldv0*oldv0 + v[0]*v[0];
+
+ // normalize v
+ double mag = sqrt(mag2);
+
+ // this case arises with matrices of all zeros, for example.
+ if (mag == 0)
+ continue;
+
+ for (int i = 0; i < vlen; i++)
+ v[i] /= mag;
+
+ // Q = I - 2vv'
+ //matd_t *Q = matd_identity(A->nrows);
+ //for (int i = 0; i < vlen; i++)
+ // for (int j = 0; j < vlen; j++)
+ // MATD_EL(Q, i+hhidx, j+hhidx) -= 2*v[i]*v[j];
+
+
+ // LS = matd_op("F*M", LS, Q);
+ // Implementation: take each row of LS, compute dot product with n,
+ // subtract n (scaled by dot product) from it.
+ for (int i = 0; i < LS->nrows; i++) {
+ double dot = 0;
+ for (int j = 0; j < vlen; j++)
+ dot += MATD_EL(LS, i, hhidx+j) * v[j];
+ for (int j = 0; j < vlen; j++)
+ MATD_EL(LS, i, hhidx+j) -= 2*dot*v[j];
+ }
+
+ // B = matd_op("M*F", Q, B); // should be Q', but Q is symmetric.
+ for (int i = 0; i < B->ncols; i++) {
+ double dot = 0;
+ for (int j = 0; j < vlen; j++)
+ dot += MATD_EL(B, hhidx+j, i) * v[j];
+ for (int j = 0; j < vlen; j++)
+ MATD_EL(B, hhidx+j, i) -= 2*dot*v[j];
+ }
+
+#ifdef _MSC_VER
+ free(v);
+#endif
+ }
+
+ if (hhidx+2 < A->ncols) {
+ int vlen = A->ncols - hhidx - 1;
+
+#ifdef _MSC_VER
+ double *v = malloc(vlen*sizeof *v);
+#else
+ double v[vlen];
+#endif
+
+ double mag2 = 0;
+ for (int i = 0; i < vlen; i++) {
+ v[i] = MATD_EL(B, hhidx, hhidx+i+1);
+ mag2 += v[i]*v[i];
+ }
+
+ double oldv0 = v[0];
+ if (oldv0 < 0)
+ v[0] -= sqrt(mag2);
+ else
+ v[0] += sqrt(mag2);
+
+ mag2 += -oldv0*oldv0 + v[0]*v[0];
+
+ // compute magnitude of ([1 0 0..]+v)
+ double mag = sqrt(mag2);
+
+ // this case can occur when the vectors are already perpendicular
+ if (mag == 0)
+ continue;
+
+ for (int i = 0; i < vlen; i++)
+ v[i] /= mag;
+
+ // TODO: optimize these multiplications
+ // matd_t *Q = matd_identity(A->ncols);
+ // for (int i = 0; i < vlen; i++)
+ // for (int j = 0; j < vlen; j++)
+ // MATD_EL(Q, i+1+hhidx, j+1+hhidx) -= 2*v[i]*v[j];
+
+ // RS = matd_op("F*M", RS, Q);
+ for (int i = 0; i < RS->nrows; i++) {
+ double dot = 0;
+ for (int j = 0; j < vlen; j++)
+ dot += MATD_EL(RS, i, hhidx+1+j) * v[j];
+ for (int j = 0; j < vlen; j++)
+ MATD_EL(RS, i, hhidx+1+j) -= 2*dot*v[j];
+ }
+
+ // B = matd_op("F*M", B, Q); // should be Q', but Q is symmetric.
+ for (int i = 0; i < B->nrows; i++) {
+ double dot = 0;
+ for (int j = 0; j < vlen; j++)
+ dot += MATD_EL(B, i, hhidx+1+j) * v[j];
+ for (int j = 0; j < vlen; j++)
+ MATD_EL(B, i, hhidx+1+j) -= 2*dot*v[j];
+ }
+
+#ifdef _MSC_VER
+ free(v);
+#endif
+ }
+ }
+
+ // maxiters used to be smaller to prevent us from looping forever,
+ // but this doesn't seem to happen any more with our more stable
+ // svd22 implementation.
+ int maxiters = 1UL << 30;
+ assert(maxiters > 0); // reassure clang
+ int iter;
+
+ double maxv; // maximum non-zero value being reduced this iteration
+
+ double tol = 1E-10;
+
+ // which method will we use to find the largest off-diagonal
+ // element of B?
+ const int find_max_method = 1; //(B->ncols < 6) ? 2 : 1;
+
+ // for each of the first B->ncols rows, which index has the
+ // maximum absolute value? (used by method 1)
+#ifdef _MSC_VER
+ int *maxrowidx = malloc(B->ncols*sizeof *maxrowidx);
+#else
+ int maxrowidx[B->ncols];
+#endif
+ int lastmaxi, lastmaxj;
+
+ if (find_max_method == 1) {
+ for (int i = 2; i < B->ncols; i++)
+ maxrowidx[i] = max_idx(B, i, B->ncols);
+
+ // note that we started the array at 2. That's because by setting
+ // these values below, we'll recompute first two entries on the
+ // first iteration!
+ lastmaxi = 0, lastmaxj = 1;
+ }
+
+ for (iter = 0; iter < maxiters; iter++) {
+
+ // No diagonalization required for 0x0 and 1x1 matrices.
+ if (B->ncols < 2)
+ break;
+
+ // find the largest off-diagonal element of B, and put its
+ // coordinates in maxi, maxj.
+ int maxi, maxj;
+
+ if (find_max_method == 1) {
+ // method 1 is the "smarter" method which does at least
+ // 4*ncols work. More work might be needed (up to
+ // ncols*ncols), depending on data. Thus, this might be a
+ // bit slower than the default method for very small
+ // matrices.
+ maxi = -1;
+ maxv = -1;
+
+ // every iteration, we must deal with the fact that rows
+ // and columns lastmaxi and lastmaxj have been
+ // modified. Update maxrowidx accordingly.
+
+ // now, EVERY row also had columns lastmaxi and lastmaxj modified.
+ for (int rowi = 0; rowi < B->ncols; rowi++) {
+
+ // the magnitude of the largest off-diagonal element
+ // in this row.
+ double thismaxv;
+
+ // row 'lastmaxi' and 'lastmaxj' have been completely
+ // changed. compute from scratch.
+ if (rowi == lastmaxi || rowi == lastmaxj) {
+ maxrowidx[rowi] = max_idx(B, rowi, B->ncols);
+ thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
+ goto endrowi;
+ }
+
+ // our maximum entry was just modified. We don't know
+ // if it went up or down, and so we don't know if it
+ // is still the maximum. We have to update from
+ // scratch.
+ if (maxrowidx[rowi] == lastmaxi || maxrowidx[rowi] == lastmaxj) {
+ maxrowidx[rowi] = max_idx(B, rowi, B->ncols);
+ thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
+ goto endrowi;
+ }
+
+ // This row is unchanged, except for columns
+ // 'lastmaxi' and 'lastmaxj', and those columns were
+ // not previously the largest entry... just check to
+ // see if they are now the maximum entry in their
+ // row. (Remembering to consider off-diagonal entries
+ // only!)
+ thismaxv = fabs(MATD_EL(B, rowi, maxrowidx[rowi]));
+
+ // check column lastmaxi. Is it now the maximum?
+ if (lastmaxi != rowi) {
+ double v = fabs(MATD_EL(B, rowi, lastmaxi));
+ if (v > thismaxv) {
+ thismaxv = v;
+ maxrowidx[rowi] = lastmaxi;
+ }
+ }
+
+ // check column lastmaxj
+ if (lastmaxj != rowi) {
+ double v = fabs(MATD_EL(B, rowi, lastmaxj));
+ if (v > thismaxv) {
+ thismaxv = v;
+ maxrowidx[rowi] = lastmaxj;
+ }
+ }
+
+ // does this row have the largest value we've seen so far?
+ endrowi:
+ if (thismaxv > maxv) {
+ maxv = thismaxv;
+ maxi = rowi;
+ }
+ }
+
+ assert(maxi >= 0);
+ maxj = maxrowidx[maxi];
+
+ // save these for the next iteration.
+ lastmaxi = maxi;
+ lastmaxj = maxj;
+
+ if (maxv < tol)
+ break;
+
+ } else if (find_max_method == 2) {
+ // brute-force (reference) version.
+ maxv = -1;
+
+ // only search top "square" portion
+ for (int i = 0; i < B->ncols; i++) {
+ for (int j = 0; j < B->ncols; j++) {
+ if (i == j)
+ continue;
+
+ double v = fabs(MATD_EL(B, i, j));
+
+ if (v > maxv) {
+ maxi = i;
+ maxj = j;
+ maxv = v;
+ }
+ }
+ }
+
+ // termination condition.
+ if (maxv < tol)
+ break;
+ } else {
+ assert(0);
+ }
+
+// printf(">>> %5d %3d, %3d %15g\n", maxi, maxj, iter, maxv);
+
+ // Now, solve the 2x2 SVD problem for the matrix
+ // [ A0 A1 ]
+ // [ A2 A3 ]
+ double A0 = MATD_EL(B, maxi, maxi);
+ double A1 = MATD_EL(B, maxi, maxj);
+ double A2 = MATD_EL(B, maxj, maxi);
+ double A3 = MATD_EL(B, maxj, maxj);
+
+ if (1) {
+ double AQ[4];
+ AQ[0] = A0;
+ AQ[1] = A1;
+ AQ[2] = A2;
+ AQ[3] = A3;
+
+ double U[4], S[2], V[4];
+ svd22(AQ, U, S, V);
+
+/* Reference (slow) implementation...
+
+ // LS = LS * ROT(theta) = LS * QL
+ matd_t *QL = matd_identity(A->nrows);
+ MATD_EL(QL, maxi, maxi) = U[0];
+ MATD_EL(QL, maxi, maxj) = U[1];
+ MATD_EL(QL, maxj, maxi) = U[2];
+ MATD_EL(QL, maxj, maxj) = U[3];
+
+ matd_t *QR = matd_identity(A->ncols);
+ MATD_EL(QR, maxi, maxi) = V[0];
+ MATD_EL(QR, maxi, maxj) = V[1];
+ MATD_EL(QR, maxj, maxi) = V[2];
+ MATD_EL(QR, maxj, maxj) = V[3];
+
+ LS = matd_op("F*M", LS, QL);
+ RS = matd_op("F*M", RS, QR); // remember we'll transpose RS.
+ B = matd_op("M'*F*M", QL, B, QR);
+
+ matd_destroy(QL);
+ matd_destroy(QR);
+*/
+
+ // LS = matd_op("F*M", LS, QL);
+ for (int i = 0; i < LS->nrows; i++) {
+ double vi = MATD_EL(LS, i, maxi);
+ double vj = MATD_EL(LS, i, maxj);
+
+ MATD_EL(LS, i, maxi) = U[0]*vi + U[2]*vj;
+ MATD_EL(LS, i, maxj) = U[1]*vi + U[3]*vj;
+ }
+
+ // RS = matd_op("F*M", RS, QR); // remember we'll transpose RS.
+ for (int i = 0; i < RS->nrows; i++) {
+ double vi = MATD_EL(RS, i, maxi);
+ double vj = MATD_EL(RS, i, maxj);
+
+ MATD_EL(RS, i, maxi) = V[0]*vi + V[2]*vj;
+ MATD_EL(RS, i, maxj) = V[1]*vi + V[3]*vj;
+ }
+
+ // B = matd_op("M'*F*M", QL, B, QR);
+ // The QL matrix mixes rows of B.
+ for (int i = 0; i < B->ncols; i++) {
+ double vi = MATD_EL(B, maxi, i);
+ double vj = MATD_EL(B, maxj, i);
+
+ MATD_EL(B, maxi, i) = U[0]*vi + U[2]*vj;
+ MATD_EL(B, maxj, i) = U[1]*vi + U[3]*vj;
+ }
+
+ // The QR matrix mixes columns of B.
+ for (int i = 0; i < B->nrows; i++) {
+ double vi = MATD_EL(B, i, maxi);
+ double vj = MATD_EL(B, i, maxj);
+
+ MATD_EL(B, i, maxi) = V[0]*vi + V[2]*vj;
+ MATD_EL(B, i, maxj) = V[1]*vi + V[3]*vj;
+ }
+ }
+ }
+
+ if (!(flags & MATD_SVD_NO_WARNINGS) && iter == maxiters) {
+ printf("WARNING: maximum iters (maximum = %d, matrix %d x %d, max=%.15f)\n",
+ iter, A->nrows, A->ncols, maxv);
+
+// matd_print(A, "%15f");
+ }
+
+ // them all positive by flipping the corresponding columns of
+ // U/LS.
+#ifdef _MSC_VER
+ int *idxs = malloc(A->ncols*sizeof *idxs);
+ double *vals = malloc(A->ncols*sizeof *vals);
+#else
+ int idxs[A->ncols];
+ double vals[A->ncols];
+#endif
+ for (int i = 0; i < A->ncols; i++) {
+ idxs[i] = i;
+ vals[i] = MATD_EL(B, i, i);
+ }
+
+ // A bubble sort. Seriously.
+ int changed;
+ do {
+ changed = 0;
+
+ for (int i = 0; i + 1 < A->ncols; i++) {
+ if (fabs(vals[i+1]) > fabs(vals[i])) {
+ int tmpi = idxs[i];
+ idxs[i] = idxs[i+1];
+ idxs[i+1] = tmpi;
+
+ double tmpv = vals[i];
+ vals[i] = vals[i+1];
+ vals[i+1] = tmpv;
+
+ changed = 1;
+ }
+ }
+ } while (changed);
+
+ matd_t *LP = matd_identity(A->nrows);
+ matd_t *RP = matd_identity(A->ncols);
+
+ for (int i = 0; i < A->ncols; i++) {
+ MATD_EL(LP, idxs[i], idxs[i]) = 0; // undo the identity above
+ MATD_EL(RP, idxs[i], idxs[i]) = 0;
+
+ MATD_EL(LP, idxs[i], i) = vals[i] < 0 ? -1 : 1;
+ MATD_EL(RP, idxs[i], i) = 1; //vals[i] < 0 ? -1 : 1;
+ }
+
+ // we've factored:
+ // LP*(something)*RP'
+
+ // solve for (something)
+ B = matd_op("M'*F*M", LP, B, RP);
+
+ // update LS and RS, remembering that RS will be transposed.
+ LS = matd_op("F*M", LS, LP);
+ RS = matd_op("F*M", RS, RP);
+
+ matd_destroy(LP);
+ matd_destroy(RP);
+
+ matd_svd_t res;
+ memset(&res, 0, sizeof(res));
+
+ // make B exactly diagonal
+
+ for (int i = 0; i < B->nrows; i++) {
+ for (int j = 0; j < B->ncols; j++) {
+ if (i != j)
+ MATD_EL(B, i, j) = 0;
+ }
+ }
+
+ res.U = LS;
+ res.S = B;
+ res.V = RS;
+
+#ifdef _MSC_VER
+ free(maxrowidx);
+ free(idxs);
+ free(vals);
+#endif
+
+ return res;
+}
+
+matd_svd_t matd_svd(matd_t *A)
+{
+ return matd_svd_flags(A, 0);
+}
+
+matd_svd_t matd_svd_flags(matd_t *A, int flags)
+{
+ matd_svd_t res;
+
+ if (A->ncols <= A->nrows) {
+ res = matd_svd_tall(A, flags);
+ } else {
+ matd_t *At = matd_transpose(A);
+
+ // A =U S V'
+ // A'=V S' U'
+
+ matd_svd_t tmp = matd_svd_tall(At, flags);
+
+ memset(&res, 0, sizeof(res));
+ res.U = tmp.V; //matd_transpose(tmp.V);
+ res.S = matd_transpose(tmp.S);
+ res.V = tmp.U; //matd_transpose(tmp.U);
+
+ matd_destroy(tmp.S);
+ matd_destroy(At);
+ }
+
+/*
+ matd_t *check = matd_op("M*M*M'-M", res.U, res.S, res.V, A);
+ double maxerr = 0;
+
+ for (int i = 0; i < check->nrows; i++)
+ for (int j = 0; j < check->ncols; j++)
+ maxerr = fmax(maxerr, fabs(MATD_EL(check, i, j)));
+
+ matd_destroy(check);
+
+ if (maxerr > 1e-7) {
+ printf("bad maxerr: %15f\n", maxerr);
+ }
+
+ if (maxerr > 1e-5) {
+ printf("bad maxerr: %15f\n", maxerr);
+ matd_print(A, "%15f");
+ assert(0);
+ }
+
+*/
+ return res;
+}
+
+
+matd_plu_t *matd_plu(const matd_t *a)
+{
+ unsigned int *piv = calloc(a->nrows, sizeof(unsigned int));
+ int pivsign = 1;
+ matd_t *lu = matd_copy(a);
+
+ // only for square matrices.
+ assert(a->nrows == a->ncols);
+
+ matd_plu_t *mlu = calloc(1, sizeof(matd_plu_t));
+
+ for (int i = 0; i < a->nrows; i++)
+ piv[i] = i;
+
+ for (int j = 0; j < a->ncols; j++) {
+ for (int i = 0; i < a->nrows; i++) {
+ int kmax = i < j ? i : j; // min(i,j)
+
+ // compute dot product of row i with column j (up through element kmax)
+ double acc = 0;
+ for (int k = 0; k < kmax; k++)
+ acc += MATD_EL(lu, i, k) * MATD_EL(lu, k, j);
+
+ MATD_EL(lu, i, j) -= acc;
+ }
+
+ // find pivot and exchange if necessary.
+ int p = j;
+ if (1) {
+ for (int i = j+1; i < lu->nrows; i++) {
+ if (fabs(MATD_EL(lu,i,j)) > fabs(MATD_EL(lu, p, j))) {
+ p = i;
+ }
+ }
+ }
+
+ // swap rows p and j?
+ if (p != j) {
+#ifdef _MSC_VER
+ TYPE *tmp = malloc(lu->ncols*sizeof *tmp);
+#else
+ TYPE tmp[lu->ncols];
+#endif
+ memcpy(tmp, &MATD_EL(lu, p, 0), sizeof(TYPE) * lu->ncols);
+ memcpy(&MATD_EL(lu, p, 0), &MATD_EL(lu, j, 0), sizeof(TYPE) * lu->ncols);
+ memcpy(&MATD_EL(lu, j, 0), tmp, sizeof(TYPE) * lu->ncols);
+ int k = piv[p];
+ piv[p] = piv[j];
+ piv[j] = k;
+ pivsign = -pivsign;
+
+#ifdef _MSC_VER
+ free(tmp);
+#endif
+ }
+
+ double LUjj = MATD_EL(lu, j, j);
+
+ // If our pivot is very small (which means the matrix is
+ // singular or nearly singular), replace with a new pivot of the
+ // right sign.
+ if (fabs(LUjj) < MATD_EPS) {
+/*
+ if (LUjj < 0)
+ LUjj = -MATD_EPS;
+ else
+ LUjj = MATD_EPS;
+
+ MATD_EL(lu, j, j) = LUjj;
+*/
+ mlu->singular = 1;
+ }
+
+ if (j < lu->ncols && j < lu->nrows && LUjj != 0) {
+ LUjj = 1.0 / LUjj;
+ for (int i = j+1; i < lu->nrows; i++)
+ MATD_EL(lu, i, j) *= LUjj;
+ }
+ }
+
+ mlu->lu = lu;
+ mlu->piv = piv;
+ mlu->pivsign = pivsign;
+
+ return mlu;
+}
+
+void matd_plu_destroy(matd_plu_t *mlu)
+{
+ matd_destroy(mlu->lu);
+ free(mlu->piv);
+ memset(mlu, 0, sizeof(matd_plu_t));
+ free(mlu);
+}
+
+double matd_plu_det(const matd_plu_t *mlu)
+{
+ matd_t *lu = mlu->lu;
+ double det = mlu->pivsign;
+
+ if (lu->nrows == lu->ncols) {
+ for (int i = 0; i < lu->ncols; i++)
+ det *= MATD_EL(lu, i, i);
+ }
+
+ return det;
+}
+
+matd_t *matd_plu_p(const matd_plu_t *mlu)
+{
+ matd_t *lu = mlu->lu;
+ matd_t *P = matd_create(lu->nrows, lu->nrows);
+
+ for (int i = 0; i < lu->nrows; i++) {
+ MATD_EL(P, mlu->piv[i], i) = 1;
+ }
+
+ return P;
+}
+
+matd_t *matd_plu_l(const matd_plu_t *mlu)
+{
+ matd_t *lu = mlu->lu;
+
+ matd_t *L = matd_create(lu->nrows, lu->ncols);
+ for (int i = 0; i < lu->nrows; i++) {
+ MATD_EL(L, i, i) = 1;
+
+ for (int j = 0; j < i; j++) {
+ MATD_EL(L, i, j) = MATD_EL(lu, i, j);
+ }
+ }
+
+ return L;
+}
+
+matd_t *matd_plu_u(const matd_plu_t *mlu)
+{
+ matd_t *lu = mlu->lu;
+
+ matd_t *U = matd_create(lu->ncols, lu->ncols);
+ for (int i = 0; i < lu->ncols; i++) {
+ for (int j = 0; j < lu->ncols; j++) {
+ if (i <= j)
+ MATD_EL(U, i, j) = MATD_EL(lu, i, j);
+ }
+ }
+
+ return U;
+}
+
+// PLU = A
+// Ax = B
+// PLUx = B
+// LUx = P'B
+matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b)
+{
+ matd_t *x = matd_copy(b);
+
+ // permute right hand side
+ for (int i = 0; i < mlu->lu->nrows; i++)
+ memcpy(&MATD_EL(x, i, 0), &MATD_EL(b, mlu->piv[i], 0), sizeof(TYPE) * b->ncols);
+
+ // solve Ly = b
+ for (int k = 0; k < mlu->lu->nrows; k++) {
+ for (int i = k+1; i < mlu->lu->nrows; i++) {
+ double LUik = -MATD_EL(mlu->lu, i, k);
+ for (int t = 0; t < b->ncols; t++)
+ MATD_EL(x, i, t) += MATD_EL(x, k, t) * LUik;
+ }
+ }
+
+ // solve Ux = y
+ for (int k = mlu->lu->ncols-1; k >= 0; k--) {
+ double LUkk = 1.0 / MATD_EL(mlu->lu, k, k);
+ for (int t = 0; t < b->ncols; t++)
+ MATD_EL(x, k, t) *= LUkk;
+
+ for (int i = 0; i < k; i++) {
+ double LUik = -MATD_EL(mlu->lu, i, k);
+ for (int t = 0; t < b->ncols; t++)
+ MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
+ }
+ }
+
+ return x;
+}
+
+matd_t *matd_solve(matd_t *A, matd_t *b)
+{
+ matd_plu_t *mlu = matd_plu(A);
+ matd_t *x = matd_plu_solve(mlu, b);
+
+ matd_plu_destroy(mlu);
+ return x;
+}
+
+#if 0
+
+static int randi()
+{
+ int v = random()&31;
+ v -= 15;
+ return v;
+}
+
+static double randf()
+{
+ double v = 1.0 *random() / RAND_MAX;
+ return 2*v - 1;
+}
+
+int main(int argc, char *argv[])
+{
+ if (1) {
+ int maxdim = 16;
+ matd_t *A = matd_create(maxdim, maxdim);
+
+ for (int iter = 0; 1; iter++) {
+ srand(iter);
+
+ if (iter % 1000 == 0)
+ printf("%d\n", iter);
+
+ int m = 1 + (random()%(maxdim-1));
+ int n = 1 + (random()%(maxdim-1));
+
+ for (int i = 0; i < m*n; i++)
+ A->data[i] = randi();
+
+ A->nrows = m;
+ A->ncols = n;
+
+// printf("%d %d ", m, n);
+ matd_svd_t svd = matd_svd(A);
+ matd_destroy(svd.U);
+ matd_destroy(svd.S);
+ matd_destroy(svd.V);
+
+ }
+
+/* matd_t *A = matd_create_data(2, 5, (double[]) { 1, 5, 2, 6,
+ 3, 3, 0, 7,
+ 1, 1, 0, -2,
+ 4, 0, 9, 9, 2, 6, 1, 3, 2, 5, 5, 4, -1, 2, 5, 9, 8, 2 });
+
+ matd_svd(A);
+*/
+ return 0;
+ }
+
+
+ struct svd22 s;
+
+ srand(0);
+
+ matd_t *A = matd_create(2, 2);
+ MATD_EL(A,0,0) = 4;
+ MATD_EL(A,0,1) = 7;
+ MATD_EL(A,1,0) = 2;
+ MATD_EL(A,1,1) = 6;
+
+ matd_t *U = matd_create(2, 2);
+ matd_t *V = matd_create(2, 2);
+ matd_t *S = matd_create(2, 2);
+
+ for (int iter = 0; 1; iter++) {
+ if (iter % 100000 == 0)
+ printf("%d\n", iter);
+
+ MATD_EL(A,0,0) = randf();
+ MATD_EL(A,0,1) = randf();
+ MATD_EL(A,1,0) = randf();
+ MATD_EL(A,1,1) = randf();
+
+ matd_svd22_impl(A->data, &s);
+
+ memcpy(U->data, s.U, 4*sizeof(double));
+ memcpy(V->data, s.V, 4*sizeof(double));
+ MATD_EL(S,0,0) = s.S[0];
+ MATD_EL(S,1,1) = s.S[1];
+
+ assert(s.S[0] >= s.S[1]);
+ assert(s.S[0] >= 0);
+ assert(s.S[1] >= 0);
+ if (s.S[0] == 0) {
+// printf("*"); fflush(NULL);
+// printf("%15f %15f %15f %15f\n", MATD_EL(A,0,0), MATD_EL(A,0,1), MATD_EL(A,1,0), MATD_EL(A,1,1));
+ }
+ if (s.S[1] == 0) {
+// printf("#"); fflush(NULL);
+ }
+
+ matd_t *USV = matd_op("M*M*M'", U, S, V);
+
+ double maxerr = 0;
+ for (int i = 0; i < 4; i++)
+ maxerr = fmax(maxerr, fabs(USV->data[i] - A->data[i]));
+
+ if (0) {
+ printf("------------------------------------\n");
+ printf("A:\n");
+ matd_print(A, "%15f");
+ printf("\nUSV':\n");
+ matd_print(USV, "%15f");
+ printf("maxerr: %.15f\n", maxerr);
+ printf("\n\n");
+ }
+
+ matd_destroy(USV);
+
+ assert(maxerr < 0.00001);
+ }
+}
+
+#endif
+
+// XXX NGV Cholesky
+/*static double *matd_cholesky_raw(double *A, int n)
+ {
+ double *L = (double*)calloc(n * n, sizeof(double));
+
+ for (int i = 0; i < n; i++) {
+ for (int j = 0; j < (i+1); j++) {
+ double s = 0;
+ for (int k = 0; k < j; k++)
+ s += L[i * n + k] * L[j * n + k];
+ L[i * n + j] = (i == j) ?
+ sqrt(A[i * n + i] - s) :
+ (1.0 / L[j * n + j] * (A[i * n + j] - s));
+ }
+ }
+
+ return L;
+ }
+
+ matd_t *matd_cholesky(const matd_t *A)
+ {
+ assert(A->nrows == A->ncols);
+ double *L_data = matd_cholesky_raw(A->data, A->nrows);
+ matd_t *L = matd_create_data(A->nrows, A->ncols, L_data);
+ free(L_data);
+ return L;
+ }*/
+
+// NOTE: The below implementation of Cholesky is different from the one
+// used in NGV.
+matd_chol_t *matd_chol(matd_t *A)
+{
+ assert(A->nrows == A->ncols);
+ int N = A->nrows;
+
+ // make upper right
+ matd_t *U = matd_copy(A);
+
+ // don't actually need to clear lower-left... we won't touch it.
+/* for (int i = 0; i < U->nrows; i++) {
+ for (int j = 0; j < i; j++) {
+// assert(MATD_EL(U, i, j) == MATD_EL(U, j, i));
+MATD_EL(U, i, j) = 0;
+}
+}
+*/
+ int is_spd = 1; // (A->nrows == A->ncols);
+
+ for (int i = 0; i < N; i++) {
+ double d = MATD_EL(U, i, i);
+ is_spd &= (d > 0);
+
+ if (d < MATD_EPS)
+ d = MATD_EPS;
+ d = 1.0 / sqrt(d);
+
+ for (int j = i; j < N; j++)
+ MATD_EL(U, i, j) *= d;
+
+ for (int j = i+1; j < N; j++) {
+ double s = MATD_EL(U, i, j);
+
+ if (s == 0)
+ continue;
+
+ for (int k = j; k < N; k++) {
+ MATD_EL(U, j, k) -= MATD_EL(U, i, k)*s;
+ }
+ }
+ }
+
+ matd_chol_t *chol = calloc(1, sizeof(matd_chol_t));
+ chol->is_spd = is_spd;
+ chol->u = U;
+ return chol;
+}
+
+void matd_chol_destroy(matd_chol_t *chol)
+{
+ matd_destroy(chol->u);
+ free(chol);
+}
+
+// Solve: (U')x = b, U is upper triangular
+void matd_ltransposetriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
+{
+ int n = u->ncols;
+ memcpy(x, b, n*sizeof(TYPE));
+ for (int i = 0; i < n; i++) {
+ x[i] /= MATD_EL(u, i, i);
+
+ for (int j = i+1; j < u->ncols; j++) {
+ x[j] -= x[i] * MATD_EL(u, i, j);
+ }
+ }
+}
+
+// Solve: Lx = b, L is lower triangular
+void matd_ltriangle_solve(matd_t *L, const TYPE *b, TYPE *x)
+{
+ int n = L->ncols;
+
+ for (int i = 0; i < n; i++) {
+ double acc = b[i];
+
+ for (int j = 0; j < i; j++) {
+ acc -= MATD_EL(L, i, j)*x[j];
+ }
+
+ x[i] = acc / MATD_EL(L, i, i);
+ }
+}
+
+// solve Ux = b, U is upper triangular
+void matd_utriangle_solve(matd_t *u, const TYPE *b, TYPE *x)
+{
+ for (int i = u->ncols-1; i >= 0; i--) {
+ double bi = b[i];
+
+ double diag = MATD_EL(u, i, i);
+
+ for (int j = i+1; j < u->ncols; j++)
+ bi -= MATD_EL(u, i, j)*x[j];
+
+ x[i] = bi / diag;
+ }
+}
+
+matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b)
+{
+ matd_t *u = chol->u;
+
+ matd_t *x = matd_copy(b);
+
+ // LUx = b
+
+ // solve Ly = b ==> (U')y = b
+
+ for (int i = 0; i < u->nrows; i++) {
+ for (int j = 0; j < i; j++) {
+ // b[i] -= L[i,j]*x[j]... replicated across columns of b
+ // ==> i.e., ==>
+ // b[i,k] -= L[i,j]*x[j,k]
+ for (int k = 0; k < b->ncols; k++) {
+ MATD_EL(x, i, k) -= MATD_EL(u, j, i)*MATD_EL(x, j, k);
+ }
+ }
+ // x[i] = b[i] / L[i,i]
+ for (int k = 0; k < b->ncols; k++) {
+ MATD_EL(x, i, k) /= MATD_EL(u, i, i);
+ }
+ }
+
+ // solve Ux = y
+ for (int k = u->ncols-1; k >= 0; k--) {
+ double LUkk = 1.0 / MATD_EL(u, k, k);
+ for (int t = 0; t < b->ncols; t++)
+ MATD_EL(x, k, t) *= LUkk;
+
+ for (int i = 0; i < k; i++) {
+ double LUik = -MATD_EL(u, i, k);
+ for (int t = 0; t < b->ncols; t++)
+ MATD_EL(x, i, t) += MATD_EL(x, k, t) *LUik;
+ }
+ }
+
+ return x;
+}
+
+/*void matd_chol_solve(matd_chol_t *chol, const TYPE *b, TYPE *x)
+ {
+ matd_t *u = chol->u;
+
+ TYPE y[u->ncols];
+ matd_ltransposetriangle_solve(u, b, y);
+ matd_utriangle_solve(u, y, x);
+ }
+*/
+// only sensible on PSD matrices. had expected it to be faster than
+// inverse via LU... for now, doesn't seem to be.
+matd_t *matd_chol_inverse(matd_t *a)
+{
+ assert(a->nrows == a->ncols);
+
+ matd_chol_t *chol = matd_chol(a);
+
+ matd_t *eye = matd_identity(a->nrows);
+ matd_t *inv = matd_chol_solve(chol, eye);
+ matd_destroy(eye);
+ matd_chol_destroy(chol);
+
+ return inv;
+}
+
+double matd_max(matd_t *m)
+{
+ double d = -DBL_MAX;
+ for(int x=0; x<m->nrows; x++) {
+ for(int y=0; y<m->ncols; y++) {
+ if(MATD_EL(m, x, y) > d)
+ d = MATD_EL(m, x, y);
+ }
+ }
+
+ return d;
+}
diff --git a/3rdparty/apriltag/common/matd.h b/3rdparty/apriltag/common/matd.h
new file mode 100644
index 0000000..099f16b
--- /dev/null
+++ b/3rdparty/apriltag/common/matd.h
@@ -0,0 +1,458 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _MATD_H
+#define _MATD_H
+
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+/**
+ * Defines a matrix structure for holding double-precision values with
+ * data in row-major order (i.e. index = row*ncols + col).
+ *
+ * nrows and ncols are 1-based counts with the exception that a scalar (non-matrix)
+ * is represented with nrows=0 and/or ncols=0.
+ */
+typedef struct
+{
+ unsigned int nrows, ncols;
+ double data[];
+// double *data;
+} matd_t;
+
+#define MATD_ALLOC(name, nrows, ncols) double name ## _storage [nrows*ncols]; matd_t name = { .nrows = nrows, .ncols = ncols, .data = &name ## _storage };
+
+/**
+ * Defines a small value which can be used in place of zero for approximating
+ * calculations which are singular at zero values (i.e. inverting a matrix with
+ * a zero or near-zero determinant).
+ */
+#define MATD_EPS 1e-8
+
+/**
+ * A macro to reference a specific matd_t data element given it's zero-based
+ * row and column indexes. Suitable for both retrieval and assignment.
+ */
+#define MATD_EL(m, row, col) (m)->data[((row)*(m)->ncols + (col))]
+
+/**
+ * Creates a double matrix with the given number of rows and columns (or a scalar
+ * in the case where rows=0 and/or cols=0). All data elements will be initialized
+ * to zero. It is the caller's responsibility to call matd_destroy() on the
+ * returned matrix.
+ */
+matd_t *matd_create(int rows, int cols);
+
+/**
+ * Creates a double matrix with the given number of rows and columns (or a scalar
+ * in the case where rows=0 and/or cols=0). All data elements will be initialized
+ * using the supplied array of data, which must contain at least rows*cols elements,
+ * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_create_data(int rows, int cols, const double *data);
+
+/**
+ * Creates a double matrix with the given number of rows and columns (or a scalar
+ * in the case where rows=0 and/or cols=0). All data elements will be initialized
+ * using the supplied array of float data, which must contain at least rows*cols elements,
+ * arranged in row-major order (i.e. index = row*ncols + col). It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_create_dataf(int rows, int cols, const float *data);
+
+/**
+ * Creates a square identity matrix with the given number of rows (and
+ * therefore columns), or a scalar with value 1 in the case where dim=0.
+ * It is the caller's responsibility to call matd_destroy() on the
+ * returned matrix.
+ */
+matd_t *matd_identity(int dim);
+
+/**
+ * Creates a scalar with the supplied value 'v'. It is the caller's responsibility
+ * to call matd_destroy() on the returned matrix.
+ *
+ * NOTE: Scalars are different than 1x1 matrices (implementation note:
+ * they are encoded as 0x0 matrices). For example: for matrices A*B, A
+ * and B must both have specific dimensions. However, if A is a
+ * scalar, there are no restrictions on the size of B.
+ */
+matd_t *matd_create_scalar(double v);
+
+/**
+ * Retrieves the cell value for matrix 'm' at the given zero-based row and column index.
+ * Performs more thorough validation checking than MATD_EL().
+ */
+double matd_get(const matd_t *m, int row, int col);
+
+/**
+ * Assigns the given value to the matrix cell at the given zero-based row and
+ * column index. Performs more thorough validation checking than MATD_EL().
+ */
+void matd_put(matd_t *m, int row, int col, double value);
+
+/**
+ * Retrieves the scalar value of the given element ('m' must be a scalar).
+ * Performs more thorough validation checking than MATD_EL().
+ */
+double matd_get_scalar(const matd_t *m);
+
+/**
+ * Assigns the given value to the supplied scalar element ('m' must be a scalar).
+ * Performs more thorough validation checking than MATD_EL().
+ */
+void matd_put_scalar(matd_t *m, double value);
+
+/**
+ * Creates an exact copy of the supplied matrix 'm'. It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_copy(const matd_t *m);
+
+/**
+ * Creates a copy of a subset of the supplied matrix 'a'. The subset will include
+ * rows 'r0' through 'r1', inclusive ('r1' >= 'r0'), and columns 'c0' through 'c1',
+ * inclusive ('c1' >= 'c0'). All parameters are zero-based (i.e. matd_select(a, 0, 0, 0, 0)
+ * will return only the first cell). Cannot be used on scalars or to extend
+ * beyond the number of rows/columns of 'a'. It is the caller's responsibility to
+ * call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_select(const matd_t *a, int r0, int r1, int c0, int c1);
+
+/**
+ * Prints the supplied matrix 'm' to standard output by applying the supplied
+ * printf format specifier 'fmt' for each individual element. Each row will
+ * be printed on a separate newline.
+ */
+void matd_print(const matd_t *m, const char *fmt);
+
+/**
+ * Prints the transpose of the supplied matrix 'm' to standard output by applying
+ * the supplied printf format specifier 'fmt' for each individual element. Each
+ * row will be printed on a separate newline.
+ */
+void matd_print_transpose(const matd_t *m, const char *fmt);
+
+/**
+ * Adds the two supplied matrices together, cell-by-cell, and returns the results
+ * as a new matrix of the same dimensions. The supplied matrices must have
+ * identical dimensions. It is the caller's responsibility to call matd_destroy()
+ * on the returned matrix.
+ */
+matd_t *matd_add(const matd_t *a, const matd_t *b);
+
+/**
+ * Adds the values of 'b' to matrix 'a', cell-by-cell, and overwrites the
+ * contents of 'a' with the results. The supplied matrices must have
+ * identical dimensions.
+ */
+void matd_add_inplace(matd_t *a, const matd_t *b);
+
+/**
+ * Subtracts matrix 'b' from matrix 'a', cell-by-cell, and returns the results
+ * as a new matrix of the same dimensions. The supplied matrices must have
+ * identical dimensions. It is the caller's responsibility to call matd_destroy()
+ * on the returned matrix.
+ */
+matd_t *matd_subtract(const matd_t *a, const matd_t *b);
+
+/**
+ * Subtracts the values of 'b' from matrix 'a', cell-by-cell, and overwrites the
+ * contents of 'a' with the results. The supplied matrices must have
+ * identical dimensions.
+ */
+void matd_subtract_inplace(matd_t *a, const matd_t *b);
+
+/**
+ * Scales all cell values of matrix 'a' by the given scale factor 's' and
+ * returns the result as a new matrix of the same dimensions. It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_scale(const matd_t *a, double s);
+
+/**
+ * Scales all cell values of matrix 'a' by the given scale factor 's' and
+ * overwrites the contents of 'a' with the results.
+ */
+void matd_scale_inplace(matd_t *a, double s);
+
+/**
+ * Multiplies the two supplied matrices together (matrix product), and returns the
+ * results as a new matrix. The supplied matrices must have dimensions such that
+ * columns(a) = rows(b). The returned matrix will have a row count of rows(a)
+ * and a column count of columns(b). It is the caller's responsibility to call
+ * matd_destroy() on the returned matrix.
+ */
+matd_t *matd_multiply(const matd_t *a, const matd_t *b);
+
+/**
+ * Creates a matrix which is the transpose of the supplied matrix 'a'. It is the
+ * caller's responsibility to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_transpose(const matd_t *a);
+
+/**
+ * Calculates the determinant of the supplied matrix 'a'.
+ */
+double matd_det(const matd_t *a);
+
+/**
+ * Attempts to compute an inverse of the supplied matrix 'a' and return it as
+ * a new matrix. This is strictly only possible if the determinant of 'a' is
+ * non-zero (matd_det(a) != 0).
+ *
+ * If the determinant is zero, NULL is returned. It is otherwise the
+ * caller's responsibility to cope with the results caused by poorly
+ * conditioned matrices. (E.g.., if such a situation is likely to arise, compute
+ * the pseudo-inverse from the SVD.)
+ **/
+matd_t *matd_inverse(const matd_t *a);
+
+static inline void matd_set_data(matd_t *m, const double *data)
+{
+ memcpy(m->data, data, m->nrows * m->ncols * sizeof(double));
+}
+
+/**
+ * Determines whether the supplied matrix 'a' is a scalar (positive return) or
+ * not (zero return, indicating a matrix of dimensions at least 1x1).
+ */
+static inline int matd_is_scalar(const matd_t *a)
+{
+ assert(a != NULL);
+ return a->ncols == 0 || a->nrows == 0;
+}
+
+/**
+ * Determines whether the supplied matrix 'a' is a row or column vector
+ * (positive return) or not (zero return, indicating either 'a' is a scalar or a
+ * matrix with at least one dimension > 1).
+ */
+static inline int matd_is_vector(const matd_t *a)
+{
+ assert(a != NULL);
+ return a->ncols == 1 || a->nrows == 1;
+}
+
+/**
+ * Determines whether the supplied matrix 'a' is a row or column vector
+ * with a dimension of 'len' (positive return) or not (zero return).
+ */
+static inline int matd_is_vector_len(const matd_t *a, int len)
+{
+ assert(a != NULL);
+ return (a->ncols == 1 && a->nrows == (unsigned int) len) || (a->ncols == (unsigned int) len && a->nrows == 1);
+}
+
+/**
+ * Calculates the magnitude of the supplied matrix 'a'.
+ */
+double matd_vec_mag(const matd_t *a);
+
+/**
+ * Calculates the magnitude of the distance between the points represented by
+ * matrices 'a' and 'b'. Both 'a' and 'b' must be vectors and have the same
+ * dimension (although one may be a row vector and one may be a column vector).
+ */
+double matd_vec_dist(const matd_t *a, const matd_t *b);
+
+
+/**
+ * Same as matd_vec_dist, but only uses the first 'n' terms to compute distance
+ */
+double matd_vec_dist_n(const matd_t *a, const matd_t *b, int n);
+
+/**
+ * Calculates the dot product of two vectors. Both 'a' and 'b' must be vectors
+ * and have the same dimension (although one may be a row vector and one may be
+ * a column vector).
+ */
+double matd_vec_dot_product(const matd_t *a, const matd_t *b);
+
+/**
+ * Calculates the normalization of the supplied vector 'a' (i.e. a unit vector
+ * of the same dimension and orientation as 'a' with a magnitude of 1) and returns
+ * it as a new vector. 'a' must be a vector of any dimension and must have a
+ * non-zero magnitude. It is the caller's responsibility to call matd_destroy()
+ * on the returned matrix.
+ */
+matd_t *matd_vec_normalize(const matd_t *a);
+
+/**
+ * Calculates the cross product of supplied matrices 'a' and 'b' (i.e. a x b)
+ * and returns it as a new matrix. Both 'a' and 'b' must be vectors of dimension
+ * 3, but can be either row or column vectors. It is the caller's responsibility
+ * to call matd_destroy() on the returned matrix.
+ */
+matd_t *matd_crossproduct(const matd_t *a, const matd_t *b);
+
+double matd_err_inf(const matd_t *a, const matd_t *b);
+
+/**
+ * Creates a new matrix by applying a series of matrix operations, as expressed
+ * in 'expr', to the supplied list of matrices. Each matrix to be operated upon
+ * must be represented in the expression by a separate matrix placeholder, 'M',
+ * and there must be one matrix supplied as an argument for each matrix
+ * placeholder in the expression. All rules and caveats of the corresponding
+ * matrix operations apply to the operated-on matrices. It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ *
+ * Available operators (in order of increasing precedence):
+ * M+M add two matrices together
+ * M-M subtract one matrix from another
+ * M*M multiply to matrices together (matrix product)
+ * MM multiply to matrices together (matrix product)
+ * -M negate a matrix
+ * M^-1 take the inverse of a matrix
+ * M' take the transpose of a matrix
+ *
+ * Expressions can be combined together and grouped by enclosing them in
+ * parenthesis, i.e.:
+ * -M(M+M+M)-(M*M)^-1
+ *
+ * Scalar values can be generated on-the-fly, i.e.:
+ * M*2.2 scales M by 2.2
+ * -2+M adds -2 to all elements of M
+ *
+ * All whitespace in the expression is ignored.
+ */
+matd_t *matd_op(const char *expr, ...);
+
+/**
+ * Frees the memory associated with matrix 'm', being the result of an earlier
+ * call to a matd_*() function, after which 'm' will no longer be usable.
+ */
+void matd_destroy(matd_t *m);
+
+typedef struct
+{
+ matd_t *U;
+ matd_t *S;
+ matd_t *V;
+} matd_svd_t;
+
+/** Compute a complete SVD of a matrix. The SVD exists for all
+ * matrices. For a matrix MxN, we will have:
+ *
+ * A = U*S*V'
+ *
+ * where A is MxN, U is MxM (and is an orthonormal basis), S is MxN
+ * (and is diagonal up to machine precision), and V is NxN (and is an
+ * orthonormal basis).
+ *
+ * The caller is responsible for destroying U, S, and V.
+ **/
+matd_svd_t matd_svd(matd_t *A);
+
+#define MATD_SVD_NO_WARNINGS 1
+ matd_svd_t matd_svd_flags(matd_t *A, int flags);
+
+////////////////////////////////
+// PLU Decomposition
+
+// All square matrices (even singular ones) have a partially-pivoted
+// LU decomposition such that A = PLU, where P is a permutation
+// matrix, L is a lower triangular matrix, and U is an upper
+// triangular matrix.
+//
+typedef struct
+{
+ // was the input matrix singular? When a zero pivot is found, this
+ // flag is set to indicate that this has happened.
+ int singular;
+
+ unsigned int *piv; // permutation indices
+ int pivsign; // either +1 or -1
+
+ // The matd_plu_t object returned "owns" the enclosed LU matrix. It
+ // is not expected that the returned object is itself useful to
+ // users: it contains the L and U information all smushed
+ // together.
+ matd_t *lu; // combined L and U matrices, permuted so they can be triangular.
+} matd_plu_t;
+
+matd_plu_t *matd_plu(const matd_t *a);
+void matd_plu_destroy(matd_plu_t *mlu);
+double matd_plu_det(const matd_plu_t *lu);
+matd_t *matd_plu_p(const matd_plu_t *lu);
+matd_t *matd_plu_l(const matd_plu_t *lu);
+matd_t *matd_plu_u(const matd_plu_t *lu);
+matd_t *matd_plu_solve(const matd_plu_t *mlu, const matd_t *b);
+
+// uses LU decomposition internally.
+matd_t *matd_solve(matd_t *A, matd_t *b);
+
+////////////////////////////////
+// Cholesky Factorization
+
+/**
+ * Creates a double matrix with the Cholesky lower triangular matrix
+ * of A. A must be symmetric, positive definite. It is the caller's
+ * responsibility to call matd_destroy() on the returned matrix.
+ */
+//matd_t *matd_cholesky(const matd_t *A);
+
+typedef struct
+{
+ int is_spd;
+ matd_t *u;
+} matd_chol_t;
+
+matd_chol_t *matd_chol(matd_t *A);
+matd_t *matd_chol_solve(const matd_chol_t *chol, const matd_t *b);
+void matd_chol_destroy(matd_chol_t *chol);
+// only sensible on PSD matrices
+matd_t *matd_chol_inverse(matd_t *a);
+
+void matd_ltransposetriangle_solve(matd_t *u, const double *b, double *x);
+void matd_ltriangle_solve(matd_t *u, const double *b, double *x);
+void matd_utriangle_solve(matd_t *u, const double *b, double *x);
+
+
+double matd_max(matd_t *m);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/math_util.h b/3rdparty/apriltag/common/math_util.h
new file mode 100644
index 0000000..d026ce1
--- /dev/null
+++ b/3rdparty/apriltag/common/math_util.h
@@ -0,0 +1,229 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _MATHUTIL_H
+#define _MATHUTIL_H
+
+#include <math.h>
+#include <float.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <assert.h>
+#include <string.h> // memcpy
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+#ifndef M_TWOPI
+# define M_TWOPI 6.2831853071795862319959 /* 2*pi */
+#endif
+
+#ifndef M_PI
+# define M_PI 3.141592653589793238462643383279502884196
+#endif
+
+#define to_radians(x) ( (x) * (M_PI / 180.0 ))
+#define to_degrees(x) ( (x) * (180.0 / M_PI ))
+
+#ifndef max
+#define max(A, B) (A < B ? B : A)
+#endif
+
+#ifndef min
+#define min(A, B) (A < B ? A : B)
+#endif
+
+ /* DEPRECATE, threshold meaningless without context.
+static inline int dequals(double a, double b)
+{
+ double thresh = 1e-9;
+ return (fabs(a-b) < thresh);
+}
+ */
+
+static inline int dequals_mag(double a, double b, double thresh)
+{
+ return (fabs(a-b) < thresh);
+}
+
+static inline int isq(int v)
+{
+ return v*v;
+}
+
+static inline float fsq(float v)
+{
+ return v*v;
+}
+
+static inline double sq(double v)
+{
+ return v*v;
+}
+
+static inline double sgn(double v)
+{
+ return (v>=0) ? 1 : -1;
+}
+
+// random number between [0, 1)
+static inline float randf()
+{
+ return ((float) rand()) / (RAND_MAX + 1.0f);
+}
+
+
+static inline float signed_randf()
+{
+ return randf()*2 - 1;
+}
+
+// return a random integer between [0, bound)
+static inline int irand(int bound)
+{
+ int v = (int) (randf()*bound);
+ if (v == bound)
+ return (bound-1);
+ //assert(v >= 0);
+ //assert(v < bound);
+ return v;
+}
+
+/** Map vin to [0, 2*PI) **/
+static inline double mod2pi_positive(double vin)
+{
+ return vin - M_TWOPI * floor(vin / M_TWOPI);
+}
+
+/** Map vin to [-PI, PI) **/
+static inline double mod2pi(double vin)
+{
+ return mod2pi_positive(vin + M_PI) - M_PI;
+}
+
+/** Return vin such that it is within PI degrees of ref **/
+static inline double mod2pi_ref(double ref, double vin)
+{
+ return ref + mod2pi(vin - ref);
+}
+
+/** Map vin to [0, 360) **/
+static inline double mod360_positive(double vin)
+{
+ return vin - 360 * floor(vin / 360);
+}
+
+/** Map vin to [-180, 180) **/
+static inline double mod360(double vin)
+{
+ return mod360_positive(vin + 180) - 180;
+}
+
+static inline int theta_to_int(double theta, int max)
+{
+ theta = mod2pi_ref(M_PI, theta);
+ int v = (int) (theta / M_TWOPI * max);
+
+ if (v == max)
+ v = 0;
+
+ assert (v >= 0 && v < max);
+
+ return v;
+}
+
+static inline int imin(int a, int b)
+{
+ return (a < b) ? a : b;
+}
+
+static inline int imax(int a, int b)
+{
+ return (a > b) ? a : b;
+}
+
+static inline int64_t imin64(int64_t a, int64_t b)
+{
+ return (a < b) ? a : b;
+}
+
+static inline int64_t imax64(int64_t a, int64_t b)
+{
+ return (a > b) ? a : b;
+}
+
+static inline int iclamp(int v, int minv, int maxv)
+{
+ return imax(minv, imin(v, maxv));
+}
+
+static inline double dclamp(double a, double min, double max)
+{
+ if (a < min)
+ return min;
+ if (a > max)
+ return max;
+ return a;
+}
+
+static inline int fltcmp (float f1, float f2)
+{
+ float epsilon = f1-f2;
+ if (epsilon < 0.0)
+ return -1;
+ else if (epsilon > 0.0)
+ return 1;
+ else
+ return 0;
+}
+
+static inline int dblcmp (double d1, double d2)
+{
+ double epsilon = d1-d2;
+ if (epsilon < 0.0)
+ return -1;
+ else if (epsilon > 0.0)
+ return 1;
+ else
+ return 0;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/pam.c b/3rdparty/apriltag/common/pam.c
new file mode 100644
index 0000000..6200209
--- /dev/null
+++ b/3rdparty/apriltag/common/pam.c
@@ -0,0 +1,250 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <assert.h>
+
+#include "pam.h"
+
+pam_t *pam_create_from_file(const char *inpath)
+{
+ FILE *infile = fopen(inpath, "r");
+ if (infile == NULL) {
+ printf("pam.c: couldn't open input file: %s\n", inpath);
+ return NULL;
+ }
+
+ pam_t *pam = calloc(1, sizeof(pam_t));
+ pam->width = -1;
+ pam->height = -1;
+ pam->depth = -1;
+ pam->maxval = -1;
+ pam->type = -1;
+
+ int linenumber = 0;
+
+ while (1) {
+ char line[1024];
+ if (!fgets(line, sizeof(line), infile)) {
+ printf("pam.c: unexpected EOF\n");
+ goto fail;
+ }
+ linenumber++;
+
+ char *tok0 = line;
+ char *tok1 = NULL;
+
+ if (line[0] == '#') // comment
+ continue;
+
+ size_t linelen = strlen(line);
+ for (int idx = 0; idx < linelen; idx++) {
+ if (line[idx] == ' ') {
+ line[idx] = 0;
+ if (tok1) {
+ printf("pam.c: More than two tokens, %s:%d\n", inpath, linenumber);
+ }
+
+ tok1 = &line[idx+1];
+ }
+ if (line[idx] == '\n')
+ line[idx] = 0;
+ }
+
+ if (!strcmp(tok0, "P7"))
+ continue;
+
+ if (!strcmp(tok0, "ENDHDR"))
+ break;
+
+ if (!strcmp(tok0, "WIDTH") && tok1) {
+ pam->width = atoi(tok1);
+ continue;
+ }
+
+ if (!strcmp(tok0, "HEIGHT") && tok1) {
+ pam->height = atoi(tok1);
+ continue;
+ }
+
+ if (!strcmp(tok0, "DEPTH") && tok1) {
+ pam->depth = atoi(tok1);
+ continue;
+ }
+
+ if (!strcmp(tok0, "MAXVAL") && tok1) {
+ pam->maxval = atoi(tok1);
+ continue;
+ }
+
+ if (!strcmp(tok0, "TUPLTYPE") && tok1) {
+ if (!strcmp(tok1, "GRAYSCALE_ALPHA")) {
+ pam->type = PAM_GRAYSCALE_ALPHA;
+ continue;
+ }
+
+ if (!strcmp(tok1, "RGB_ALPHA")) {
+ pam->type = PAM_RGB_ALPHA;
+ continue;
+ }
+
+ if (!strcmp(tok1, "RGB")) {
+ pam->type = PAM_RGB;
+ continue;
+ }
+
+ if (!strcmp(tok1, "GRAYSCALE")) {
+ pam->type = PAM_GRAYSCALE;
+ continue;
+ }
+
+ printf("pam.c: unrecognized tupl type %s\n", tok1);
+ continue;
+ }
+
+ printf("pam.c: unrecognized attribute %s\n", tok0);
+ }
+
+ if (pam->width < 0 || pam->height < 0 || pam->depth < 0 ||
+ pam->maxval < 0 || pam->type < 0) {
+ printf("pam.c: missing required metadata field\n");
+ goto fail;
+ }
+
+ assert(pam->maxval == 255);
+
+ pam->datalen = pam->width * pam->height * pam->depth;
+ pam->data = malloc(pam->datalen);
+ if (pam->datalen != fread(pam->data, 1, pam->datalen, infile)) {
+ printf("pam.c: couldn't read body\n");
+ goto fail;
+ }
+
+ fclose(infile);
+ return pam;
+
+ fail:
+ free(pam);
+ fclose(infile);
+ return NULL;
+}
+
+int pam_write_file(pam_t *pam, const char *outpath)
+{
+ FILE *f = fopen(outpath, "w+");
+ if (!f)
+ return -1;
+
+ const char *tupl = NULL;
+ switch (pam->type) {
+ case PAM_GRAYSCALE_ALPHA:
+ tupl = "GRAYSCALE_ALPHA";
+ break;
+ case PAM_RGB_ALPHA:
+ tupl = "RGB_ALPHA";
+ break;
+ case PAM_RGB:
+ tupl = "RGB";
+ break;
+ case PAM_GRAYSCALE:
+ tupl = "GRAYSCALE";
+ break;
+ default:
+ assert(0);
+ }
+
+ fprintf(f, "P7\nWIDTH %d\nHEIGHT %d\nDEPTH %d\nMAXVAL %d\nTUPLTYPE %s\nENDHDR\n",
+ pam->width, pam->height, pam->depth, pam->maxval, tupl);
+ int len = pam->width * pam->height * pam->depth;
+ if (len != fwrite(pam->data, 1, len, f)) {
+ fclose(f);
+ return -2;
+ }
+
+ fclose(f);
+
+ return 0;
+}
+
+void pam_destroy(pam_t *pam)
+{
+ if (!pam)
+ return;
+
+ free(pam->data);
+ free(pam);
+}
+
+pam_t *pam_copy(pam_t *pam)
+{
+ pam_t *copy = calloc(1, sizeof(pam_t));
+ copy->width = pam->width;
+ copy->height = pam->height;
+ copy->depth = pam->depth;
+ copy->maxval = pam->maxval;
+ copy->type = pam->type;
+
+ copy->datalen = pam->datalen;
+ copy->data = malloc(pam->datalen);
+ memcpy(copy->data, pam->data, pam->datalen);
+
+ return copy;
+}
+
+pam_t *pam_convert(pam_t *in, int type)
+{
+ if (type == in->type)
+ return pam_copy(in);
+
+ assert(type == PAM_RGB_ALPHA); // we don't support a lot yet
+ assert(in->maxval == 255);
+
+ int w = in->width;
+ int h = in->height;
+
+ pam_t *out = calloc(1, sizeof(pam_t));
+ out->type = type;
+ out->width = w;
+ out->height = h;
+ out->maxval = in->maxval;
+ out->depth = 4;
+ out->datalen = 4 * w * h;
+ out->data = malloc(out->datalen);
+
+ if (in->type == PAM_RGB) {
+ assert(in->depth == 3);
+ for (int y = 0; y < h; y++) {
+ for (int x = 0; x < w; x++) {
+ out->data[y*4*w + 4*x + 0] = in->data[y*3*w + 3*x + 0];
+ out->data[y*4*w + 4*x + 1] = in->data[y*3*w + 3*x + 1];
+ out->data[y*4*w + 4*x + 2] = in->data[y*3*w + 3*x + 2];
+ out->data[y*4*w + 4*x + 3] = 255;
+ }
+ }
+ } else {
+ printf("pam.c unsupported type %d\n", in->type);
+ assert(0);
+ }
+
+ return out;
+}
diff --git a/3rdparty/apriltag/common/pam.h b/3rdparty/apriltag/common/pam.h
new file mode 100644
index 0000000..b667bd0
--- /dev/null
+++ b/3rdparty/apriltag/common/pam.h
@@ -0,0 +1,51 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _PAM_H
+#define _PAM_H
+
+#include <stdint.h>
+
+enum { PAM_GRAYSCALE_ALPHA = 5000, PAM_RGB_ALPHA, PAM_RGB, PAM_GRAYSCALE };
+
+typedef struct pam pam_t;
+struct pam
+{
+ int type; // one of PAM_*
+
+ int width, height; // note, stride always width.
+ int depth; // bytes per pixel
+ int maxval; // maximum value per channel, e.g. 255 for 8bpp
+
+ int datalen; // in bytes
+ uint8_t *data;
+};
+
+pam_t *pam_create_from_file(const char *inpath);
+int pam_write_file(pam_t *pam, const char *outpath);
+void pam_destroy(pam_t *pam);
+
+pam_t *pam_copy(pam_t *pam);
+
+// NB doesn't handle many conversions yet.
+pam_t *pam_convert(pam_t *in, int type);
+
+#endif
diff --git a/3rdparty/apriltag/common/pjpeg-idct.c b/3rdparty/apriltag/common/pjpeg-idct.c
new file mode 100644
index 0000000..0095d76
--- /dev/null
+++ b/3rdparty/apriltag/common/pjpeg-idct.c
@@ -0,0 +1,386 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <math.h>
+#include <stdint.h>
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+#ifndef M_PI
+# define M_PI 3.141592653589793238462643383279502884196
+#endif
+
+// 8 bits of fixed-point output
+//
+// This implementation has a worst-case complexity of 22 multiplies
+// and 64 adds. This makes it significantly worse (about 2x) than the
+// best-known fast inverse cosine transform methods. HOWEVER, zero
+// coefficients can be skipped over, and since that's common (often
+// more than half the coefficients are zero).
+//
+// The output is scaled by a factor of 256 (due to our fixed-point
+// integer arithmetic)..
+static inline void idct_1D_u32(int32_t *in, int instride, int32_t *out, int outstride)
+{
+ for (int x = 0; x < 8; x++)
+ out[x*outstride] = 0;
+
+ int32_t c;
+
+ c = in[0*instride];
+ if (c) {
+ // 181 181 181 181 181 181 181 181
+ int32_t c181 = c * 181;
+ out[0*outstride] += c181;
+ out[1*outstride] += c181;
+ out[2*outstride] += c181;
+ out[3*outstride] += c181;
+ out[4*outstride] += c181;
+ out[5*outstride] += c181;
+ out[6*outstride] += c181;
+ out[7*outstride] += c181;
+ }
+
+ c = in[1*instride];
+ if (c) {
+ // 251 212 142 49 -49 -142 -212 -251
+ int32_t c251 = c * 251;
+ int32_t c212 = c * 212;
+ int32_t c142 = c * 142;
+ int32_t c49 = c * 49;
+ out[0*outstride] += c251;
+ out[1*outstride] += c212;
+ out[2*outstride] += c142;
+ out[3*outstride] += c49;
+ out[4*outstride] -= c49;
+ out[5*outstride] -= c142;
+ out[6*outstride] -= c212;
+ out[7*outstride] -= c251;
+ }
+
+ c = in[2*instride];
+ if (c) {
+ // 236 97 -97 -236 -236 -97 97 236
+ int32_t c236 = c*236;
+ int32_t c97 = c*97;
+ out[0*outstride] += c236;
+ out[1*outstride] += c97;
+ out[2*outstride] -= c97;
+ out[3*outstride] -= c236;
+ out[4*outstride] -= c236;
+ out[5*outstride] -= c97;
+ out[6*outstride] += c97;
+ out[7*outstride] += c236;
+ }
+
+ c = in[3*instride];
+ if (c) {
+ // 212 -49 -251 -142 142 251 49 -212
+ int32_t c212 = c*212;
+ int32_t c49 = c*49;
+ int32_t c251 = c*251;
+ int32_t c142 = c*142;
+ out[0*outstride] += c212;
+ out[1*outstride] -= c49;
+ out[2*outstride] -= c251;
+ out[3*outstride] -= c142;
+ out[4*outstride] += c142;
+ out[5*outstride] += c251;
+ out[6*outstride] += c49;
+ out[7*outstride] -= c212;
+ }
+
+ c = in[4*instride];
+ if (c) {
+ // 181 -181 -181 181 181 -181 -181 181
+ int32_t c181 = c*181;
+ out[0*outstride] += c181;
+ out[1*outstride] -= c181;
+ out[2*outstride] -= c181;
+ out[3*outstride] += c181;
+ out[4*outstride] += c181;
+ out[5*outstride] -= c181;
+ out[6*outstride] -= c181;
+ out[7*outstride] += c181;
+ }
+
+ c = in[5*instride];
+ if (c) {
+ // 142 -251 49 212 -212 -49 251 -142
+ int32_t c142 = c*142;
+ int32_t c251 = c*251;
+ int32_t c49 = c*49;
+ int32_t c212 = c*212;
+ out[0*outstride] += c142;
+ out[1*outstride] -= c251;
+ out[2*outstride] += c49;
+ out[3*outstride] += c212;
+ out[4*outstride] -= c212;
+ out[5*outstride] -= c49;
+ out[6*outstride] += c251;
+ out[7*outstride] -= c142;
+ }
+
+ c = in[6*instride];
+ if (c) {
+ // 97 -236 236 -97 -97 236 -236 97
+ int32_t c97 = c*97;
+ int32_t c236 = c*236;
+ out[0*outstride] += c97;
+ out[1*outstride] -= c236;
+ out[2*outstride] += c236;
+ out[3*outstride] -= c97;
+ out[4*outstride] -= c97;
+ out[5*outstride] += c236;
+ out[6*outstride] -= c236;
+ out[7*outstride] += c97;
+ }
+
+ c = in[7*instride];
+ if (c) {
+ // 49 -142 212 -251 251 -212 142 -49
+ int32_t c49 = c*49;
+ int32_t c142 = c*142;
+ int32_t c212 = c*212;
+ int32_t c251 = c*251;
+ out[0*outstride] += c49;
+ out[1*outstride] -= c142;
+ out[2*outstride] += c212;
+ out[3*outstride] -= c251;
+ out[4*outstride] += c251;
+ out[5*outstride] -= c212;
+ out[6*outstride] += c142;
+ out[7*outstride] -= c49;
+ }
+}
+
+void pjpeg_idct_2D_u32(int32_t in[64], uint8_t *out, uint32_t outstride)
+{
+ int32_t tmp[64];
+
+ // idct on rows
+ for (int y = 0; y < 8; y++)
+ idct_1D_u32(&in[8*y], 1, &tmp[8*y], 1);
+
+ int32_t tmp2[64];
+
+ // idct on columns
+ for (int x = 0; x < 8; x++)
+ idct_1D_u32(&tmp[x], 8, &tmp2[x], 8);
+
+ // scale, adjust bias, and clamp
+ for (int y = 0; y < 8; y++) {
+ for (int x = 0; x < 8; x++) {
+ int i = 8*y + x;
+
+ // Shift of 18: the divide by 4 as part of the idct, and a shift by 16
+ // to undo the fixed-point arithmetic. (We accumulated 8 bits of
+ // fractional precision during each of the row and column IDCTs)
+ //
+ // Originally:
+ // int32_t v = (tmp2[i] >> 18) + 128;
+ //
+ // Move the add before the shift and we can do rounding at
+ // the same time.
+ const int32_t offset = (128 << 18) + (1 << 17);
+ int32_t v = (tmp2[i] + offset) >> 18;
+
+ if (v < 0)
+ v = 0;
+ if (v > 255)
+ v = 255;
+
+ out[y*outstride + x] = v;
+ }
+ }
+}
+
+///////////////////////////////////////////////////////
+// Below: a "as straight-forward as I can make" implementation.
+static inline void idct_1D_double(double *in, int instride, double *out, int outstride)
+{
+ for (int x = 0; x < 8; x++)
+ out[x*outstride] = 0;
+
+ // iterate over IDCT coefficients
+ double Cu = 1/sqrt(2);
+
+ for (int u = 0; u < 8; u++, Cu = 1) {
+
+ double coeff = in[u*instride];
+ if (coeff == 0)
+ continue;
+
+ for (int x = 0; x < 8; x++)
+ out[x*outstride] += Cu*cos((2*x+1)*u*M_PI/16) * coeff;
+ }
+}
+
+void pjpeg_idct_2D_double(int32_t in[64], uint8_t *out, uint32_t outstride)
+{
+ double din[64], dout[64];
+ for (int i = 0; i < 64; i++)
+ din[i] = in[i];
+
+ double tmp[64];
+
+ // idct on rows
+ for (int y = 0; y < 8; y++)
+ idct_1D_double(&din[8*y], 1, &tmp[8*y], 1);
+
+ // idct on columns
+ for (int x = 0; x < 8; x++)
+ idct_1D_double(&tmp[x], 8, &dout[x], 8);
+
+ // scale, adjust bias, and clamp
+ for (int y = 0; y < 8; y++) {
+ for (int x = 0; x < 8; x++) {
+ int i = 8*y + x;
+
+ dout[i] = (dout[i] / 4) + 128;
+ if (dout[i] < 0)
+ dout[i] = 0;
+ if (dout[i] > 255)
+ dout[i] = 255;
+
+ // XXX round by adding +.5?
+ out[y*outstride + x] = dout[i];
+ }
+ }
+}
+
+//////////////////////////////////////////////
+static inline unsigned char njClip(const int x) {
+ return (x < 0) ? 0 : ((x > 0xFF) ? 0xFF : (unsigned char) x);
+}
+
+#define W1 2841
+#define W2 2676
+#define W3 2408
+#define W5 1609
+#define W6 1108
+#define W7 565
+
+static inline void njRowIDCT(int* blk) {
+ int x0, x1, x2, x3, x4, x5, x6, x7, x8;
+ if (!((x1 = blk[4] << 11)
+ | (x2 = blk[6])
+ | (x3 = blk[2])
+ | (x4 = blk[1])
+ | (x5 = blk[7])
+ | (x6 = blk[5])
+ | (x7 = blk[3])))
+ {
+ blk[0] = blk[1] = blk[2] = blk[3] = blk[4] = blk[5] = blk[6] = blk[7] = blk[0] << 3;
+ return;
+ }
+ x0 = (blk[0] << 11) + 128;
+ x8 = W7 * (x4 + x5);
+ x4 = x8 + (W1 - W7) * x4;
+ x5 = x8 - (W1 + W7) * x5;
+ x8 = W3 * (x6 + x7);
+ x6 = x8 - (W3 - W5) * x6;
+ x7 = x8 - (W3 + W5) * x7;
+ x8 = x0 + x1;
+ x0 -= x1;
+ x1 = W6 * (x3 + x2);
+ x2 = x1 - (W2 + W6) * x2;
+ x3 = x1 + (W2 - W6) * x3;
+ x1 = x4 + x6;
+ x4 -= x6;
+ x6 = x5 + x7;
+ x5 -= x7;
+ x7 = x8 + x3;
+ x8 -= x3;
+ x3 = x0 + x2;
+ x0 -= x2;
+ x2 = (181 * (x4 + x5) + 128) >> 8;
+ x4 = (181 * (x4 - x5) + 128) >> 8;
+ blk[0] = (x7 + x1) >> 8;
+ blk[1] = (x3 + x2) >> 8;
+ blk[2] = (x0 + x4) >> 8;
+ blk[3] = (x8 + x6) >> 8;
+ blk[4] = (x8 - x6) >> 8;
+ blk[5] = (x0 - x4) >> 8;
+ blk[6] = (x3 - x2) >> 8;
+ blk[7] = (x7 - x1) >> 8;
+}
+
+static inline void njColIDCT(const int* blk, unsigned char *out, int stride) {
+ int x0, x1, x2, x3, x4, x5, x6, x7, x8;
+ if (!((x1 = blk[8*4] << 8)
+ | (x2 = blk[8*6])
+ | (x3 = blk[8*2])
+ | (x4 = blk[8*1])
+ | (x5 = blk[8*7])
+ | (x6 = blk[8*5])
+ | (x7 = blk[8*3])))
+ {
+ x1 = njClip(((blk[0] + 32) >> 6) + 128);
+ for (x0 = 8; x0; --x0) {
+ *out = (unsigned char) x1;
+ out += stride;
+ }
+ return;
+ }
+ x0 = (blk[0] << 8) + 8192;
+ x8 = W7 * (x4 + x5) + 4;
+ x4 = (x8 + (W1 - W7) * x4) >> 3;
+ x5 = (x8 - (W1 + W7) * x5) >> 3;
+ x8 = W3 * (x6 + x7) + 4;
+ x6 = (x8 - (W3 - W5) * x6) >> 3;
+ x7 = (x8 - (W3 + W5) * x7) >> 3;
+ x8 = x0 + x1;
+ x0 -= x1;
+ x1 = W6 * (x3 + x2) + 4;
+ x2 = (x1 - (W2 + W6) * x2) >> 3;
+ x3 = (x1 + (W2 - W6) * x3) >> 3;
+ x1 = x4 + x6;
+ x4 -= x6;
+ x6 = x5 + x7;
+ x5 -= x7;
+ x7 = x8 + x3;
+ x8 -= x3;
+ x3 = x0 + x2;
+ x0 -= x2;
+ x2 = (181 * (x4 + x5) + 128) >> 8;
+ x4 = (181 * (x4 - x5) + 128) >> 8;
+ *out = njClip(((x7 + x1) >> 14) + 128); out += stride;
+ *out = njClip(((x3 + x2) >> 14) + 128); out += stride;
+ *out = njClip(((x0 + x4) >> 14) + 128); out += stride;
+ *out = njClip(((x8 + x6) >> 14) + 128); out += stride;
+ *out = njClip(((x8 - x6) >> 14) + 128); out += stride;
+ *out = njClip(((x0 - x4) >> 14) + 128); out += stride;
+ *out = njClip(((x3 - x2) >> 14) + 128); out += stride;
+ *out = njClip(((x7 - x1) >> 14) + 128);
+}
+
+void pjpeg_idct_2D_nanojpeg(int32_t in[64], uint8_t *out, uint32_t outstride)
+{
+ int coef;
+
+ for (coef = 0; coef < 64; coef += 8)
+ njRowIDCT(&in[coef]);
+ for (coef = 0; coef < 8; ++coef)
+ njColIDCT(&in[coef], &out[coef], outstride);
+}
diff --git a/3rdparty/apriltag/common/pjpeg.c b/3rdparty/apriltag/common/pjpeg.c
new file mode 100644
index 0000000..6341359
--- /dev/null
+++ b/3rdparty/apriltag/common/pjpeg.c
@@ -0,0 +1,896 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+
+#include "pjpeg.h"
+
+#include "image_u8.h"
+#include "image_u8x3.h"
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+// https://www.w3.org/Graphics/JPEG/itu-t81.pdf
+
+void pjpeg_idct_2D_double(int32_t in[64], uint8_t *out, uint32_t outstride);
+void pjpeg_idct_2D_u32(int32_t in[64], uint8_t *out, uint32_t outstride);
+void pjpeg_idct_2D_nanojpeg(int32_t in[64], uint8_t *out, uint32_t outstride);
+
+struct pjpeg_huffman_code
+{
+ uint8_t nbits; // how many bits should we actually consume?
+ uint8_t code; // what is the symbol that was encoded? (not actually a DCT coefficient; see encoding)
+};
+
+struct pjpeg_decode_state
+{
+ int error;
+
+ uint32_t width, height;
+ uint8_t *in;
+ uint32_t inlen;
+
+ uint32_t flags;
+
+ // to decode, we load the next 16 bits of input (generally more
+ // than we need). We then look up in our code book how many bits
+ // we have actually consumed. For example, if there was a code
+ // whose bit sequence was "0", the first 32768 entries would all
+ // be copies of {.bits=1, .value=XX}; no matter what the following
+ // 15 bits are, we would get the correct decode.
+ //
+ // Can be up to 8 tables; computed as (ACDC * 2 + htidx)
+ struct pjpeg_huffman_code huff_codes[4][65536];
+ int huff_codes_present[4];
+
+ uint8_t qtab[4][64];
+
+ int ncomponents;
+ pjpeg_component_t *components;
+
+ int reset_interval;
+ int reset_count;
+ int reset_next; // What reset marker do we expect next? (add 0xd0)
+
+ int debug;
+};
+
+// from K.3.3.1 (page 158)
+static uint8_t mjpeg_dht[] = { // header
+ 0xFF,0xC4,0x01,0xA2,
+
+ /////////////////////////////////////////////////////////////
+ // luminance dc coefficients.
+ // DC table 0
+ 0x00,
+ // code lengths
+ 0x00,0x01,0x05,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
+ // values
+ 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,
+
+ /////////////////////////////////////////////////////////////
+ // chrominance DC coefficents
+ // DC table 1
+ 0x01,
+ // code lengths
+ 0x00,0x03,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00,
+ // values
+ 0x00,0x01,0x02,0x03,0x04,0x05,0x06,0x07,0x08,0x09,0x0A,0x0B,
+
+ /////////////////////////////////////////////////////////////
+ // luminance AC coefficients
+ // AC table 0
+ 0x10,
+ // code lengths
+ 0x00,0x02,0x01,0x03,0x03,0x02,0x04,0x03,0x05,0x05,0x04,0x04,0x00,0x00,0x01,0x7D,
+ // codes
+ 0x01,0x02,0x03,0x00,0x04,0x11,0x05,0x12,0x21,0x31,0x41,0x06,0x13,0x51,0x61,
+ 0x07,0x22,0x71,0x14,0x32,0x81,0x91,0xA1,0x08,0x23,0x42,0xB1,0xC1,0x15,0x52,0xD1,0xF0,0x24,
+ 0x33,0x62,0x72,0x82,0x09,0x0A,0x16,0x17,0x18,0x19,0x1A,0x25,0x26,0x27,0x28,0x29,0x2A,0x34,
+ 0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56,
+ 0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78,
+ 0x79,0x7A,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,0x99,
+ 0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,0xB9,
+ 0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,0xD9,
+ 0xDA,0xE1,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF1,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,
+ 0xF8,0xF9,0xFA,
+
+ /////////////////////////////////////////////////////////////
+ // chrominance DC coefficients
+ // DC table 1
+ 0x11,
+ // code lengths
+ 0x00,0x02,0x01,0x02,0x04,0x04,0x03,0x04,0x07,0x05,0x04,0x04,0x00,0x01,0x02,0x77,
+ // values
+ 0x00,0x01,0x02,0x03,0x11,0x04,0x05,0x21,0x31,0x06,0x12,0x41,0x51,0x07,0x61,0x71,
+ 0x13,0x22,0x32,0x81,0x08,0x14,0x42,0x91,0xA1,0xB1,0xC1,0x09,0x23,0x33,0x52,0xF0,0x15,0x62,
+ 0x72,0xD1,0x0A,0x16,0x24,0x34,0xE1,0x25,0xF1,0x17,0x18,0x19,0x1A,0x26,0x27,0x28,0x29,0x2A,
+ 0x35,0x36,0x37,0x38,0x39,0x3A,0x43,0x44,0x45,0x46,0x47,0x48,0x49,0x4A,0x53,0x54,0x55,0x56,
+ 0x57,0x58,0x59,0x5A,0x63,0x64,0x65,0x66,0x67,0x68,0x69,0x6A,0x73,0x74,0x75,0x76,0x77,0x78,
+ 0x79,0x7A,0x82,0x83,0x84,0x85,0x86,0x87,0x88,0x89,0x8A,0x92,0x93,0x94,0x95,0x96,0x97,0x98,
+ 0x99,0x9A,0xA2,0xA3,0xA4,0xA5,0xA6,0xA7,0xA8,0xA9,0xAA,0xB2,0xB3,0xB4,0xB5,0xB6,0xB7,0xB8,
+ 0xB9,0xBA,0xC2,0xC3,0xC4,0xC5,0xC6,0xC7,0xC8,0xC9,0xCA,0xD2,0xD3,0xD4,0xD5,0xD6,0xD7,0xD8,
+ 0xD9,0xDA,0xE2,0xE3,0xE4,0xE5,0xE6,0xE7,0xE8,0xE9,0xEA,0xF2,0xF3,0xF4,0xF5,0xF6,0xF7,0xF8,
+ 0xF9,0xFA
+};
+
+static inline uint8_t max_u8(uint8_t a, uint8_t b)
+{
+ return a > b ? a : b;
+}
+
+// order of coefficients in each DC block
+static const char ZZ[64] = { 0, 1, 8, 16, 9, 2, 3, 10,
+ 17, 24, 32, 25, 18, 11, 4, 5,
+ 12, 19, 26, 33, 40, 48, 41, 34,
+ 27, 20, 13, 6, 7, 14, 21, 28,
+ 35, 42, 49, 56, 57, 50, 43, 36,
+ 29, 22, 15, 23, 30, 37, 44, 51,
+ 58, 59, 52, 45, 38, 31, 39, 46,
+ 53, 60, 61, 54, 47, 55, 62, 63 };
+
+
+
+struct bit_decoder
+{
+ uint8_t *in;
+ uint32_t inpos;
+ uint32_t inlen;
+
+ uint32_t bits; // the low order bits contain the next nbits_avail bits.
+
+ int nbits_avail; // how many bits in 'bits' (left aligned) are valid?
+
+ int error;
+};
+
+// ensure that at least 'nbits' of data is available in the bit decoder.
+static inline void bd_ensure(struct bit_decoder *bd, int nbits)
+{
+ while (bd->nbits_avail < nbits) {
+
+ if (bd->inpos >= bd->inlen) {
+ printf("hallucinating 1s!\n");
+ // we hit end of stream hallucinate an infinite stream of 1s
+ bd->bits = (bd->bits << 8) | 0xff;
+ bd->nbits_avail += 8;
+ continue;
+ }
+
+ uint8_t nextbyte = bd->in[bd->inpos];
+ bd->inpos++;
+
+ if (nextbyte == 0xff && bd->inpos < bd->inlen && bd->in[bd->inpos] == 0x00) {
+ // a stuffed byte
+ nextbyte = 0xff;
+ bd->inpos++;
+ }
+
+ // it's an ordinary byte
+ bd->bits = (bd->bits << 8) | nextbyte;
+ bd->nbits_avail += 8;
+ }
+}
+
+static inline uint32_t bd_peek_bits(struct bit_decoder *bd, int nbits)
+{
+ bd_ensure(bd, nbits);
+
+ return (bd->bits >> (bd->nbits_avail - nbits)) & ((1 << nbits) - 1);
+}
+
+static inline uint32_t bd_consume_bits(struct bit_decoder *bd, int nbits)
+{
+ assert(nbits < 32);
+
+ bd_ensure(bd, nbits);
+
+ uint32_t v = (bd->bits >> (bd->nbits_avail - nbits)) & ((1 << nbits) - 1);
+
+ bd->nbits_avail -= nbits;
+
+ return v;
+}
+
+// discard without regard for byte stuffing!
+static inline void bd_discard_bytes(struct bit_decoder *bd, int nbytes)
+{
+ assert(bd->nbits_avail == 0);
+ bd->inpos += nbytes;
+}
+
+static inline int bd_has_more(struct bit_decoder *bd)
+{
+ return bd->nbits_avail > 0 || bd->inpos < bd->inlen;
+}
+
+// throw away up to 7 bits of data so that the next data returned
+// began on a byte boundary.
+static inline void bd_discard_to_byte_boundary(struct bit_decoder *bd)
+{
+ bd->nbits_avail -= (bd->nbits_avail & 7);
+}
+
+static inline uint32_t bd_get_offset(struct bit_decoder *bd)
+{
+ return bd->inpos - bd->nbits_avail / 8;
+}
+
+static int pjpeg_decode_buffer(struct pjpeg_decode_state *pjd)
+{
+ // XXX TODO Include sanity check that this is actually a JPG
+
+ struct bit_decoder bd;
+ memset(&bd, 0, sizeof(struct bit_decoder));
+ bd.in = pjd->in;
+ bd.inpos = 0;
+ bd.inlen = pjd->inlen;
+
+ int marker_sync_skipped = 0;
+ int marker_sync_skipped_from_offset = 0;
+
+ while (bd_has_more(&bd)) {
+
+ uint32_t marker_offset = bd_get_offset(&bd);
+
+ // Look for the 0xff that signifies the beginning of a marker
+ bd_discard_to_byte_boundary(&bd);
+
+ while (bd_consume_bits(&bd, 8) != 0xff) {
+ if (marker_sync_skipped == 0)
+ marker_sync_skipped_from_offset = marker_offset;
+ marker_sync_skipped++;
+ continue;
+ }
+
+ if (marker_sync_skipped) {
+ printf("%08x: skipped %04x bytes\n", marker_sync_skipped_from_offset, marker_sync_skipped);
+ marker_sync_skipped = 0;
+ }
+
+ uint8_t marker = bd_consume_bits(&bd, 8);
+
+// printf("marker %08x : %02x\n", marker_offset, marker);
+
+ switch (marker) {
+
+ case 0xd8: // start of image. Great, continue.
+ continue;
+
+ // below are the markers that A) we don't care about
+ // that B) encode length as two bytes.
+ //
+ // Note: Other unknown fields should not be added since
+ // we should be able to skip over them by looking for
+ // the next marker byte.
+ case 0xe0: // JFIF header.
+ case 0xe1: // EXIF header (Yuck: Payload may contain 0xff 0xff!)
+ case 0xe2: // ICC Profile. (Yuck: payload may contain 0xff 0xff!)
+ case 0xe6: // some other common header
+ case 0xfe: // Comment
+ {
+ uint16_t length = bd_consume_bits(&bd, 16);
+ bd_discard_bytes(&bd, length - 2);
+ continue;
+ }
+
+ case 0xdb: { // DQT Define Quantization Table
+ uint16_t length = bd_consume_bits(&bd, 16);
+
+ if (((length-2) % 65) != 0)
+ return PJPEG_ERR_DQT;
+
+ // can contain multiple DQTs
+ for (int offset = 0; offset < length - 2; offset += 65) {
+
+ // pq: quant table element precision. 0=8bit, 1=16bit.
+ // tq: quant table destination id.
+ uint8_t pqtq = bd_consume_bits(&bd, 8);
+
+ if ((pqtq & 0xf0) != 0 || (pqtq & 0x0f) >= 4)
+ return PJPEG_ERR_DQT;
+
+ uint8_t id = pqtq & 3;
+
+ for (int i = 0; i < 64; i++)
+ pjd->qtab[id][i] = bd_consume_bits(&bd, 8);
+ }
+
+ break;
+ }
+
+ case 0xc0: { // SOF, non-differential, huffman, baseline
+ uint16_t length = bd_consume_bits(&bd, 16);
+ (void) length;
+
+ uint8_t p = bd_consume_bits(&bd, 8); // precision
+ if (p != 8)
+ return PJPEG_ERR_SOF;
+
+ pjd->height = bd_consume_bits(&bd, 16);
+ pjd->width = bd_consume_bits(&bd, 16);
+
+// printf("%d x %d\n", pjd->height, pjd->width);
+
+ int nf = bd_consume_bits(&bd, 8); // # image components
+
+ if (nf < 1 || nf > 3)
+ return PJPEG_ERR_SOF;
+
+ pjd->ncomponents = nf;
+ pjd->components = calloc(nf, sizeof(struct pjpeg_component));
+
+ for (int i = 0; i < nf; i++) {
+ // comp. identifier
+ pjd->components[i].id = bd_consume_bits(&bd, 8);
+
+ // horiz/vert sampling
+ pjd->components[i].hv = bd_consume_bits(&bd, 8);
+ pjd->components[i].scaley = pjd->components[i].hv & 0x0f;
+ pjd->components[i].scalex = pjd->components[i].hv >> 4;
+
+ // which quant table?
+ pjd->components[i].tq = bd_consume_bits(&bd, 8);
+ }
+ break;
+ }
+
+ case 0xc1: // SOF, non-differential, huffman, extended DCT
+ case 0xc2: // SOF, non-differential, huffman, progressive DCT
+ case 0xc3: // SOF, non-differential, huffman, lossless
+ case 0xc5: // SOF, differential, huffman, baseline DCT
+ case 0xc6: // SOF, differential, huffman, progressive
+ case 0xc7: // SOF, differential, huffman, lossless
+ case 0xc8: // reserved
+ case 0xc9: // SOF, non-differential, arithmetic, extended
+ case 0xca: // SOF, non-differential, arithmetic, progressive
+ case 0xcb: // SOF, non-differential, arithmetic, lossless
+ case 0xcd: // SOF, differential, arithmetic, sequential
+ case 0xce: // SOF, differential, arithmetic, progressive
+ case 0xcf: // SOF, differential, arithmetic, lossless
+ {
+ printf("pjepg.c: unsupported JPEG type %02x\n", marker);
+ return PJEPG_ERR_UNSUPPORTED;
+ }
+
+ case 0xc4: { // DHT Define Huffman Tables
+ // [ED: the encoding of these tables is really quite
+ // clever!]
+ uint16_t length = bd_consume_bits(&bd, 16);
+ length = length - 2;
+
+ while (length > 0) {
+ uint8_t TcTh = bd_consume_bits(&bd, 8);
+ length--;
+ uint8_t Tc = (TcTh >> 4);
+ int Th = TcTh & 0x0f; // which index are we using?
+
+ if (Tc >= 2 || Th >= 2)
+ // Tc must be either AC=1 or DC=0.
+ // Th must be less than 2
+ return PJPEG_ERR_DHT;
+
+ int htidx = Tc*2 + Th;
+
+ uint8_t L[17]; // how many symbols of each bit length?
+ L[0] = 0; // no 0 bit codes :)
+ for (int nbits = 1; nbits <= 16; nbits++) {
+ L[nbits] = bd_consume_bits(&bd, 8);
+ length -= L[nbits];
+ }
+ length -= 16;
+
+ uint32_t code_pos = 0;
+
+ for (int nbits = 1; nbits <= 16; nbits++) {
+ int nvalues = L[nbits];
+
+ // how many entries will we fill?
+ // (a 1 bit code will fill 32768, a 2 bit code 16384, ...)
+ uint32_t ncodes = (1 << (16 - nbits));
+
+ // consume the values...
+ for (int vi = 0; vi < nvalues; vi++) {
+ uint8_t code = bd_consume_bits(&bd, 8);
+
+ if (code_pos + ncodes > 0xffff)
+ return PJPEG_ERR_DHT;
+
+ for (int ci = 0; ci < ncodes; ci++) {
+ pjd->huff_codes[htidx][code_pos].nbits = nbits;
+ pjd->huff_codes[htidx][code_pos].code = code;
+ code_pos++;
+ }
+ }
+ }
+ pjd->huff_codes_present[htidx] = 1;
+ }
+ break;
+ }
+
+ // a sequentially-encoded JPG has one SOS segment. A
+ // progressive JPG will have multiple SOS segments.
+ case 0xda: { // Start Of Scan (SOS)
+
+ // Note that this marker frame (and its encoded
+ // length) does NOT include the bitstream that
+ // follows.
+
+ uint16_t length = bd_consume_bits(&bd, 16);
+ (void) length;
+
+ // number of components in this scan
+ uint8_t ns = bd_consume_bits(&bd, 8);
+
+ // for each component, what is the index into our pjd->components[] array?
+#ifdef _MSC_VER
+ uint8_t *comp_idx = malloc(ns*sizeof *comp_idx);
+#else
+ uint8_t comp_idx[ns];
+#endif
+ memset(comp_idx, 0, ns*sizeof(uint8_t));
+
+ for (int i = 0; i < ns; i++) {
+ // component name
+ uint8_t cs = bd_consume_bits(&bd, 8);
+
+ int found = 0;
+ for (int j = 0; j < pjd->ncomponents; j++) {
+
+ if (cs == pjd->components[j].id) {
+ // which huff tables will we use for
+ // DC (high 4 bits) and AC (low 4 bits)
+ pjd->components[j].tda = bd_consume_bits(&bd, 8);
+ comp_idx[i] = j;
+ found = 1;
+ break;
+ }
+ }
+
+ if (!found)
+ return PJPEG_ERR_SOS;
+ }
+
+ // start of spectral selection. baseline == 0
+ uint8_t ss = bd_consume_bits(&bd, 8);
+
+ // end of spectral selection. baseline == 0x3f
+ uint8_t se = bd_consume_bits(&bd, 8);
+
+ // successive approximation bits. baseline == 0
+ uint8_t Ahl = bd_consume_bits(&bd, 8);
+
+ if (ss != 0 || se != 0x3f || Ahl != 0x00)
+ return PJPEG_ERR_SOS;
+
+ // compute the dimensions of each MCU in pixels
+ int maxmcux = 0, maxmcuy = 0;
+ for (int i = 0; i < ns; i++) {
+ struct pjpeg_component *comp = &pjd->components[comp_idx[i]];
+
+ maxmcux = max_u8(maxmcux, comp->scalex * 8);
+ maxmcuy = max_u8(maxmcuy, comp->scaley * 8);
+ }
+
+ // how many MCU blocks are required to encode the whole image?
+ int mcus_x = (pjd->width + maxmcux - 1) / maxmcux;
+ int mcus_y = (pjd->height + maxmcuy - 1) / maxmcuy;
+
+ if (0)
+ printf("Image has %d x %d MCU blocks, each %d x %d pixels\n",
+ mcus_x, mcus_y, maxmcux, maxmcuy);
+
+ // allocate output storage
+ for (int i = 0; i < ns; i++) {
+ struct pjpeg_component *comp = &pjd->components[comp_idx[i]];
+ comp->width = mcus_x * comp->scalex * 8;
+ comp->height = mcus_y * comp->scaley * 8;
+ comp->stride = comp->width;
+
+ int alignment = 32;
+ if ((comp->stride % alignment) != 0)
+ comp->stride += alignment - (comp->stride % alignment);
+
+ comp->data = calloc(comp->height * comp->stride, 1);
+ }
+
+
+ // each component has its own DC prediction
+#ifdef _MSC_VER
+ int32_t *dcpred = malloc(ns*sizeof *dcpred);
+#else
+ int32_t dcpred[ns];
+#endif
+ memset(dcpred, 0, sizeof(dcpred));
+
+ pjd->reset_count = 0;
+
+ for (int mcu_y = 0; mcu_y < mcus_y; mcu_y++) {
+ for (int mcu_x = 0; mcu_x < mcus_x; mcu_x++) {
+
+ // the next two bytes in the input stream
+ // should be 0xff 0xdN, where N is the next
+ // reset counter.
+ //
+ // Our bit decoder may have already shifted
+ // these into the buffer. Consequently, we
+ // want to use our bit decoding functions to
+ // check for the marker. But we must first
+ // discard any fractional bits left.
+ if (pjd->reset_interval > 0 && pjd->reset_count == pjd->reset_interval) {
+
+ // RST markers are byte-aligned, so force
+ // the bit-decoder to the next byte
+ // boundary.
+ bd_discard_to_byte_boundary(&bd);
+
+ while (1) {
+ int32_t value = bd_consume_bits(&bd, 8);
+ if (bd.inpos > bd.inlen)
+ return PJPEG_ERR_EOF;
+ if (value == 0xff)
+ break;
+ printf("RST SYNC\n");
+ }
+
+ int32_t marker = bd_consume_bits(&bd, 8);
+
+// printf("%04x: RESET? %02x\n", *bd.inpos, marker);
+ if (marker != (0xd0 + pjd->reset_next))
+ return PJPEG_ERR_RESET;
+
+ pjd->reset_count = 0;
+ pjd->reset_next = (pjd->reset_next + 1) & 0x7;
+
+ memset(dcpred, 0, sizeof(dcpred));
+ }
+
+ for (int nsidx = 0; nsidx < ns; nsidx++) {
+
+ struct pjpeg_component *comp = &pjd->components[comp_idx[nsidx]];
+
+ int32_t block[64];
+
+ int qtabidx = comp->tq; // which quant table?
+
+ for (int sby = 0; sby < comp->scaley; sby++) {
+ for (int sbx = 0; sbx < comp->scalex; sbx++) {
+ // decode block for component nsidx
+ memset(block, 0, sizeof(block));
+
+ int dc_huff_table_idx = comp->tda >> 4;
+ int ac_huff_table_idx = 2 + (comp->tda & 0x0f);
+
+ if (!pjd->huff_codes_present[dc_huff_table_idx] ||
+ !pjd->huff_codes_present[ac_huff_table_idx])
+ return PJPEG_ERR_MISSING_DHT; // probably an MJPEG.
+
+
+ if (1) {
+ // do DC coefficient
+ uint32_t next16 = bd_peek_bits(&bd, 16);
+ struct pjpeg_huffman_code *huff_code = &pjd->huff_codes[dc_huff_table_idx][next16];
+ bd_consume_bits(&bd, huff_code->nbits);
+
+ int ssss = huff_code->code & 0x0f; // ssss == number of additional bits to read
+ int32_t value = bd_consume_bits(&bd, ssss);
+
+ // if high bit is clear, it's negative
+ if ((value & (1 << (ssss-1))) == 0)
+ value += ((-1) << ssss) + 1;
+
+ dcpred[nsidx] += value;
+ block[0] = dcpred[nsidx] * pjd->qtab[qtabidx][0];
+ }
+
+ if (1) {
+ // do AC coefficients
+ for (int coeff = 1; coeff < 64; coeff++) {
+
+ uint32_t next16 = bd_peek_bits(&bd, 16);
+
+ struct pjpeg_huffman_code *huff_code = &pjd->huff_codes[ac_huff_table_idx][next16];
+ bd_consume_bits(&bd, huff_code->nbits);
+
+ if (huff_code->code == 0) {
+ break; // EOB
+ }
+
+ int rrrr = huff_code->code >> 4; // run length of zeros
+ int ssss = huff_code->code & 0x0f;
+
+ int32_t value = bd_consume_bits(&bd, ssss);
+
+ // if high bit is clear, it's negative
+ if ((value & (1 << (ssss-1))) == 0)
+ value += ((-1) << ssss) + 1;
+
+ coeff += rrrr;
+
+ block[(int) ZZ[coeff]] = value * pjd->qtab[qtabidx][coeff];
+ }
+ }
+
+ // do IDCT
+
+ // output block's upper-left
+ // coordinate (in pixels) is
+ // (comp_x, comp_y).
+ uint32_t comp_x = (mcu_x * comp->scalex + sbx) * 8;
+ uint32_t comp_y = (mcu_y * comp->scaley + sby) * 8;
+ uint32_t dataidx = comp_y * comp->stride + comp_x;
+
+// pjpeg_idct_2D_u32(block, &comp->data[dataidx], comp->stride);
+ pjpeg_idct_2D_nanojpeg(block, &comp->data[dataidx], comp->stride);
+ }
+ }
+ }
+
+ pjd->reset_count++;
+// printf("%04x: reset count %d / %d\n", pjd->inpos, pjd->reset_count, pjd->reset_interval);
+
+ }
+ }
+
+#ifdef _MSC_VER
+ free(comp_idx);
+ free(dcpred);
+#endif
+
+ break;
+ }
+
+ case 0xd9: { // EOI End of Image
+ goto got_end_of_image;
+ }
+
+ case 0xdd: { // Define Restart Interval
+ uint16_t length = bd_consume_bits(&bd, 16);
+ if (length != 4)
+ return PJPEG_ERR_DRI;
+
+ // reset interval measured in the number of MCUs
+ pjd->reset_interval = bd_consume_bits(&bd, 16);
+
+ break;
+ }
+
+ default: {
+ printf("pjepg: Unknown marker %02x at offset %04x\n", marker, marker_offset);
+
+ // try to skip it.
+ uint16_t length = bd_consume_bits(&bd, 16);
+ bd_discard_bytes(&bd, length - 2);
+ continue;
+ }
+ } // switch (marker)
+ } // while inpos < inlen
+
+ got_end_of_image:
+
+ return PJPEG_OKAY;
+}
+
+void pjpeg_destroy(pjpeg_t *pj)
+{
+ if (!pj)
+ return;
+
+ for (int i = 0; i < pj->ncomponents; i++)
+ free(pj->components[i].data);
+ free(pj->components);
+
+ free(pj);
+}
+
+
+// just grab the first component.
+image_u8_t *pjpeg_to_u8_baseline(pjpeg_t *pj)
+{
+ assert(pj->ncomponents > 0);
+
+ pjpeg_component_t *comp = &pj->components[0];
+
+ assert(comp->width >= pj->width && comp->height >= pj->height);
+
+ image_u8_t *im = image_u8_create(pj->width, pj->height);
+ for (int y = 0; y < im->height; y++)
+ memcpy(&im->buf[y*im->stride], &comp->data[y*comp->stride], pj->width);
+
+ return im;
+}
+
+static inline uint8_t clampd(double v)
+{
+ if (v < 0)
+ return 0;
+ if (v > 255)
+ return 255;
+
+ return (uint8_t) v;
+}
+
+static inline uint8_t clamp_u8(int32_t v)
+{
+ if (v < 0)
+ return 0;
+ if (v > 255)
+ return 255;
+ return v;
+}
+
+// color conversion formulas taken from JFIF spec v 1.02
+image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj)
+{
+ assert(pj->ncomponents == 3);
+
+ pjpeg_component_t *Y = &pj->components[0];
+ pjpeg_component_t *Cb = &pj->components[1];
+ pjpeg_component_t *Cr = &pj->components[2];
+
+ int Cb_factor_y = Y->height / Cb->height;
+ int Cb_factor_x = Y->width / Cb->width;
+
+ int Cr_factor_y = Y->height / Cr->height;
+ int Cr_factor_x = Y->width / Cr->width;
+
+ image_u8x3_t *im = image_u8x3_create(pj->width, pj->height);
+
+ if (Cr_factor_y == 1 && Cr_factor_x == 1 && Cb_factor_y == 1 && Cb_factor_x == 1) {
+
+ for (int y = 0; y < pj->height; y++) {
+ for (int x = 0; x < pj->width; x++) {
+ int32_t y_val = Y->data[y*Y->stride + x] * 65536;
+ int32_t cb_val = Cb->data[y*Cb->stride + x] - 128;
+ int32_t cr_val = Cr->data[y*Cr->stride + x] - 128;
+
+ int32_t r_val = y_val + 91881 * cr_val;
+ int32_t g_val = y_val + -22554 * cb_val - 46802 * cr_val;
+ int32_t b_val = y_val + 116130 * cb_val;
+
+ im->buf[y*im->stride + 3*x + 0 ] = clamp_u8(r_val >> 16);
+ im->buf[y*im->stride + 3*x + 1 ] = clamp_u8(g_val >> 16);
+ im->buf[y*im->stride + 3*x + 2 ] = clamp_u8(b_val >> 16);
+ }
+ }
+ } else if (Cb_factor_y == Cr_factor_y && Cb_factor_x == Cr_factor_x) {
+ for (int by = 0; by < pj->height / Cb_factor_y; by++) {
+ for (int bx = 0; bx < pj->width / Cb_factor_x; bx++) {
+
+ int32_t cb_val = Cb->data[by*Cb->stride + bx] - 128;
+ int32_t cr_val = Cr->data[by*Cr->stride + bx] - 128;
+
+ int32_t r0 = 91881 * cr_val;
+ int32_t g0 = -22554 * cb_val - 46802 * cr_val;
+ int32_t b0 = 116130 * cb_val;
+
+ for (int dy = 0; dy < Cb_factor_y; dy++) {
+ int y = by*Cb_factor_y + dy;
+
+ for (int dx = 0; dx < Cb_factor_x; dx++) {
+ int x = bx*Cb_factor_x + dx;
+
+ int32_t y_val = Y->data[y*Y->stride + x] * 65536;
+
+ int32_t r_val = r0 + y_val;
+ int32_t g_val = g0 + y_val;
+ int32_t b_val = b0 + y_val;
+
+ im->buf[y*im->stride + 3*x + 0 ] = clamp_u8(r_val >> 16);
+ im->buf[y*im->stride + 3*x + 1 ] = clamp_u8(g_val >> 16);
+ im->buf[y*im->stride + 3*x + 2 ] = clamp_u8(b_val >> 16);
+ }
+ }
+ }
+ }
+ } else {
+
+ for (int y = 0; y < pj->height; y++) {
+ for (int x = 0; x < pj->width; x++) {
+ int32_t y_val = Y->data[y*Y->stride + x];
+ int32_t cb_val = Cb->data[(y / Cb_factor_y)*Cb->stride + (x / Cb_factor_x)] - 128;
+ int32_t cr_val = Cr->data[(y / Cr_factor_y)*Cr->stride + (x / Cr_factor_x)] - 128;
+
+ uint8_t r_val = clampd(y_val + 1.402 * cr_val);
+ uint8_t g_val = clampd(y_val - 0.34414 * cb_val - 0.71414 * cr_val);
+ uint8_t b_val = clampd(y_val + 1.772 * cb_val);
+
+ im->buf[y*im->stride + 3*x + 0 ] = r_val;
+ im->buf[y*im->stride + 3*x + 1 ] = g_val;
+ im->buf[y*im->stride + 3*x + 2 ] = b_val;
+ }
+ }
+ }
+
+ return im;
+}
+
+///////////////////////////////////////////////////////////////////
+// returns NULL if file loading fails.
+pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error)
+{
+ FILE *f = fopen(path, "r");
+ if (f == NULL)
+ return NULL;
+
+ fseek(f, 0, SEEK_END);
+ long buflen = ftell(f);
+
+ uint8_t *buf = malloc(buflen);
+ fseek(f, 0, SEEK_SET);
+ int res = fread(buf, 1, buflen, f);
+ fclose(f);
+ if (res != buflen) {
+ free(buf);
+ if (error)
+ *error = PJPEG_ERR_FILE;
+ return NULL;
+ }
+
+ pjpeg_t *pj = pjpeg_create_from_buffer(buf, buflen, flags, error);
+
+ free(buf);
+ return pj;
+}
+
+pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int *error)
+{
+ struct pjpeg_decode_state pjd;
+ memset(&pjd, 0, sizeof(pjd));
+
+ if (flags & PJPEG_MJPEG) {
+ pjd.in = mjpeg_dht;
+ pjd.inlen = sizeof(mjpeg_dht);
+ int result = pjpeg_decode_buffer(&pjd);
+ assert(result == 0);
+ }
+
+ pjd.in = buf;
+ pjd.inlen = buflen;
+ pjd.flags = flags;
+
+ int result = pjpeg_decode_buffer(&pjd);
+ if (error)
+ *error = result;
+
+ if (result) {
+ for (int i = 0; i < pjd.ncomponents; i++)
+ free(pjd.components[i].data);
+ free(pjd.components);
+
+ return NULL;
+ }
+
+ pjpeg_t *pj = calloc(1, sizeof(pjpeg_t));
+
+ pj->width = pjd.width;
+ pj->height = pjd.height;
+ pj->ncomponents = pjd.ncomponents;
+ pj->components = pjd.components;
+
+ return pj;
+}
diff --git a/3rdparty/apriltag/common/pjpeg.h b/3rdparty/apriltag/common/pjpeg.h
new file mode 100644
index 0000000..fdae443
--- /dev/null
+++ b/3rdparty/apriltag/common/pjpeg.h
@@ -0,0 +1,100 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+This library is free software; you can redistribute it and/or
+modify it under the terms of the GNU Lesser General Public
+License as published by the Free Software Foundation; either
+version 2.1 of the License, or (at your option) any later version.
+
+This library is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+Lesser General Public License for more details.
+
+You should have received a copy of the GNU Lesser General Public
+License along with this library; if not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef _PJPEG_H
+#define _PJPEG_H
+
+#include "image_u8.h"
+#include "image_u8x3.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct pjpeg_component pjpeg_component_t;
+struct pjpeg_component
+{
+ // resolution of this component (which is smaller than the
+ // dimensions of the image if the channel has been sub-sampled.)
+ uint32_t width, height;
+
+ // number of bytes per row. May be larger than width for alignment
+ // reasons.
+ uint32_t stride;
+
+ // data[y*stride + x]
+ uint8_t *data;
+
+ ////////////////////////////////////////////////////////////////
+ // These items probably not of great interest to most
+ // applications.
+ uint8_t id; // the identifier associated with this component
+ uint8_t hv; // horiz scale (high 4 bits) / vert scale (low 4 bits)
+ uint8_t scalex, scaley; // derived from hv above
+ uint8_t tq; // quantization table index
+
+ // this filled in at the last moment by SOS
+ uint8_t tda; // which huff tables will we use for DC (high 4 bits) and AC (low 4 bits)
+};
+
+typedef struct pjpeg pjpeg_t;
+struct pjpeg
+{
+ // status of the decode is put here. Non-zero means error.
+ int error;
+
+ uint32_t width, height; // pixel dimensions
+
+ int ncomponents;
+ pjpeg_component_t *components;
+};
+
+enum PJPEG_FLAGS {
+ PJPEG_STRICT = 1, // Don't try to recover from errors.
+ PJPEG_MJPEG = 2, // Support JPGs with missing DHT segments.
+};
+
+enum PJPEG_ERROR {
+ PJPEG_OKAY = 0,
+ PJPEG_ERR_FILE, // something wrong reading file
+ PJPEG_ERR_DQT, // something wrong with DQT marker
+ PJPEG_ERR_SOF, // something wrong with SOF marker
+ PJPEG_ERR_DHT, // something wrong with DHT marker
+ PJPEG_ERR_SOS, // something wrong with SOS marker
+ PJPEG_ERR_MISSING_DHT, // missing a necessary huffman table
+ PJPEG_ERR_DRI, // something wrong with DRI marker
+ PJPEG_ERR_RESET, // didn't get a reset marker where we expected. Corruption?
+ PJPEG_ERR_EOF, // ran out of bytes while decoding
+ PJEPG_ERR_UNSUPPORTED, // an unsupported format
+};
+
+pjpeg_t *pjpeg_create_from_file(const char *path, uint32_t flags, int *error);
+pjpeg_t *pjpeg_create_from_buffer(uint8_t *buf, int buflen, uint32_t flags, int *error);
+void pjpeg_destroy(pjpeg_t *pj);
+
+image_u8_t *pjpeg_to_u8_baseline(pjpeg_t *pj);
+image_u8x3_t *pjpeg_to_u8x3_baseline(pjpeg_t *pj);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/pnm.c b/3rdparty/apriltag/common/pnm.c
new file mode 100644
index 0000000..267d893
--- /dev/null
+++ b/3rdparty/apriltag/common/pnm.c
@@ -0,0 +1,160 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "pnm.h"
+
+pnm_t *pnm_create_from_file(const char *path)
+{
+ FILE *f = fopen(path, "rb");
+ if (f == NULL)
+ return NULL;
+
+ pnm_t *pnm = calloc(1, sizeof(pnm_t));
+ pnm->format = -1;
+
+ char tmp[1024];
+ int nparams = 0; // will be 3 when we're all done.
+ int params[3];
+
+ while (nparams < 3 && !(pnm->format == PNM_FORMAT_BINARY && nparams == 2)) {
+ if (fgets(tmp, sizeof(tmp), f) == NULL)
+ goto error;
+
+ // skip comments
+ if (tmp[0]=='#')
+ continue;
+
+ char *p = tmp;
+
+ if (pnm->format == -1 && tmp[0]=='P') {
+ pnm->format = tmp[1]-'0';
+ assert(pnm->format == PNM_FORMAT_GRAY || pnm->format == PNM_FORMAT_RGB || pnm->format == PNM_FORMAT_BINARY);
+ p = &tmp[2];
+ }
+
+ // pull integers out of this line until there are no more.
+ while (nparams < 3 && *p!=0) {
+ while (*p==' ')
+ p++;
+
+ // encounter rubbish? (End of line?)
+ if (*p < '0' || *p > '9')
+ break;
+
+ int acc = 0;
+ while (*p >= '0' && *p <= '9') {
+ acc = acc*10 + *p - '0';
+ p++;
+ }
+
+ params[nparams++] = acc;
+ p++;
+ }
+ }
+
+ pnm->width = params[0];
+ pnm->height = params[1];
+ pnm->max = params[2];
+
+ switch (pnm->format) {
+ case PNM_FORMAT_BINARY: {
+ // files in the wild sometimes simply don't set max
+ pnm->max = 1;
+
+ pnm->buflen = pnm->height * ((pnm->width + 7) / 8);
+ pnm->buf = malloc(pnm->buflen);
+ size_t len = fread(pnm->buf, 1, pnm->buflen, f);
+ if (len != pnm->buflen)
+ goto error;
+
+ fclose(f);
+ return pnm;
+ }
+
+ case PNM_FORMAT_GRAY: {
+ if (pnm->max == 255)
+ pnm->buflen = pnm->width * pnm->height;
+ else if (pnm->max == 65535)
+ pnm->buflen = 2 * pnm->width * pnm->height;
+ else
+ assert(0);
+
+ pnm->buf = malloc(pnm->buflen);
+ size_t len = fread(pnm->buf, 1, pnm->buflen, f);
+ if (len != pnm->buflen)
+ goto error;
+
+ fclose(f);
+ return pnm;
+ }
+
+ case PNM_FORMAT_RGB: {
+ if (pnm->max == 255)
+ pnm->buflen = pnm->width * pnm->height * 3;
+ else if (pnm->max == 65535)
+ pnm->buflen = 2 * pnm->width * pnm->height * 3;
+ else
+ assert(0);
+
+ pnm->buf = malloc(pnm->buflen);
+ size_t len = fread(pnm->buf, 1, pnm->buflen, f);
+ if (len != pnm->buflen)
+ goto error;
+ fclose(f);
+ return pnm;
+ }
+ }
+
+error:
+ fclose(f);
+
+ if (pnm != NULL) {
+ free(pnm->buf);
+ free(pnm);
+ }
+
+ return NULL;
+}
+
+void pnm_destroy(pnm_t *pnm)
+{
+ if (pnm == NULL)
+ return;
+
+ free(pnm->buf);
+ free(pnm);
+}
diff --git a/3rdparty/apriltag/common/pnm.h b/3rdparty/apriltag/common/pnm.h
new file mode 100644
index 0000000..952acfb
--- /dev/null
+++ b/3rdparty/apriltag/common/pnm.h
@@ -0,0 +1,66 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _PNM_H
+#define _PNM_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define PNM_FORMAT_BINARY 4
+#define PNM_FORMAT_GRAY 5
+#define PNM_FORMAT_RGB 6
+
+// supports ppm, pnm, pgm
+
+typedef struct pnm pnm_t;
+struct pnm
+{
+ int width, height;
+ int format;
+ int max; // 1 = binary, 255 = one byte, 65535 = two bytes
+
+ uint32_t buflen;
+ uint8_t *buf; // if max=65535, in big endian
+};
+
+pnm_t *pnm_create_from_file(const char *path);
+void pnm_destroy(pnm_t *pnm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/postscript_utils.h b/3rdparty/apriltag/common/postscript_utils.h
new file mode 100644
index 0000000..6faafa5
--- /dev/null
+++ b/3rdparty/apriltag/common/postscript_utils.h
@@ -0,0 +1,61 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _POSTSCRIPT_UTILS_H
+#define _POSTSCRIPT_UTILS_H
+
+// write commands in postscript language to render an image in the current
+// graphics environment. The image will be rendered in one pixel per unit
+// with Y up coordinate axis (e.g. upside down).
+static void postscript_image(FILE *f, image_u8_t *im)
+{
+// fprintf(f, "/readstring {\n currentfile exch readhexstring pop\n} bind def\n");
+ fprintf(f, "/picstr %d string def\n", im->width);
+
+ fprintf(f, "%d %d 8 [1 0 0 1 0 0]\n",
+ im->width, im->height);
+
+ fprintf(f, "{currentfile picstr readhexstring pop}\nimage\n");
+
+ for (int y = 0; y < im->height; y++) {
+ for (int x = 0; x < im->width; x++) {
+ uint8_t v = im->buf[y*im->stride + x];
+ fprintf(f, "%02x", v);
+ if ((x % 32)==31)
+ fprintf(f, "\n");
+ }
+ }
+
+ fprintf(f, "\n");
+}
+
+#endif
diff --git a/3rdparty/apriltag/common/string_util.c b/3rdparty/apriltag/common/string_util.c
new file mode 100644
index 0000000..14d0ce5
--- /dev/null
+++ b/3rdparty/apriltag/common/string_util.c
@@ -0,0 +1,787 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <assert.h>
+#include <ctype.h>
+#include <errno.h>
+#include <string.h>
+#include <stdarg.h>
+#include <stdlib.h>
+#include <stdio.h>
+
+#include "string_util.h"
+#include "zarray.h"
+
+struct string_buffer
+{
+ char *s;
+ int alloc;
+ size_t size; // as if strlen() was called; not counting terminating \0
+};
+
+#define MIN_PRINTF_ALLOC 16
+
+char *sprintf_alloc(const char *fmt, ...)
+{
+ assert(fmt != NULL);
+
+ va_list args;
+
+ va_start(args,fmt);
+ char *buf = vsprintf_alloc(fmt, args);
+ va_end(args);
+
+ return buf;
+}
+
+char *vsprintf_alloc(const char *fmt, va_list orig_args)
+{
+ assert(fmt != NULL);
+
+ int size = MIN_PRINTF_ALLOC;
+ char *buf = malloc(size * sizeof(char));
+
+ int returnsize;
+ va_list args;
+
+ va_copy(args, orig_args);
+ returnsize = vsnprintf(buf, size, fmt, args);
+ va_end(args);
+
+ // it was successful
+ if (returnsize < size) {
+ return buf;
+ }
+
+ // otherwise, we should try again
+ free(buf);
+ size = returnsize + 1;
+ buf = malloc(size * sizeof(char));
+
+ va_copy(args, orig_args);
+ returnsize = vsnprintf(buf, size, fmt, args);
+ va_end(args);
+
+ assert(returnsize <= size);
+ return buf;
+}
+
+char *_str_concat_private(const char *first, ...)
+{
+ size_t len = 0;
+
+ // get the total length (for the allocation)
+ {
+ va_list args;
+ va_start(args, first);
+ const char *arg = first;
+ while(arg != NULL) {
+ len += strlen(arg);
+ arg = va_arg(args, const char *);
+ }
+ va_end(args);
+ }
+
+ // write the string
+ char *str = malloc(len*sizeof(char) + 1);
+ char *ptr = str;
+ {
+ va_list args;
+ va_start(args, first);
+ const char *arg = first;
+ while(arg != NULL) {
+ while(*arg)
+ *ptr++ = *arg++;
+ arg = va_arg(args, const char *);
+ }
+ *ptr = '\0';
+ va_end(args);
+ }
+
+ return str;
+}
+
+// Returns the index of the first character that differs:
+int str_diff_idx(const char * a, const char * b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+
+ int i = 0;
+
+ size_t lena = strlen(a);
+ size_t lenb = strlen(b);
+
+ size_t minlen = lena < lenb ? lena : lenb;
+
+ for (; i < minlen; i++)
+ if (a[i] != b[i])
+ break;
+
+ return i;
+}
+
+
+zarray_t *str_split(const char *str, const char *delim)
+{
+ assert(str != NULL);
+ assert(delim != NULL);
+
+ zarray_t *parts = zarray_create(sizeof(char*));
+ string_buffer_t *sb = string_buffer_create();
+
+ size_t delim_len = strlen(delim);
+ size_t len = strlen(str);
+ size_t pos = 0;
+
+ while (pos < len) {
+ if (str_starts_with(&str[pos], delim) && delim_len > 0) {
+ pos += delim_len;
+ // never add empty strings (repeated tokens)
+ if (string_buffer_size(sb) > 0) {
+ char *part = string_buffer_to_string(sb);
+ zarray_add(parts, &part);
+ }
+ string_buffer_reset(sb);
+ } else {
+ string_buffer_append(sb, str[pos]);
+ pos++;
+ }
+ }
+
+ if (string_buffer_size(sb) > 0) {
+ char *part = string_buffer_to_string(sb);
+ zarray_add(parts, &part);
+ }
+
+ string_buffer_destroy(sb);
+ return parts;
+}
+
+// split on one or more spaces.
+zarray_t *str_split_spaces(const char *str)
+{
+ zarray_t *parts = zarray_create(sizeof(char*));
+ size_t len = strlen(str);
+ size_t pos = 0;
+
+ while (pos < len) {
+
+ while (pos < len && str[pos] == ' ')
+ pos++;
+
+ // produce a token?
+ if (pos < len) {
+ // yes!
+ size_t off0 = pos;
+ while (pos < len && str[pos] != ' ')
+ pos++;
+ size_t off1 = pos;
+
+ size_t len = off1 - off0;
+ char *tok = malloc(len + 1);
+ memcpy(tok, &str[off0], len);
+ tok[len] = 0;
+ zarray_add(parts, &tok);
+ }
+ }
+
+ return parts;
+}
+
+void str_split_destroy(zarray_t *za)
+{
+ if (!za)
+ return;
+
+ zarray_vmap(za, free);
+ zarray_destroy(za);
+}
+
+char *str_trim(char *str)
+{
+ assert(str != NULL);
+
+ return str_lstrip(str_rstrip(str));
+}
+
+char *str_lstrip(char *str)
+{
+ assert(str != NULL);
+
+ char *ptr = str;
+ char *end = str + strlen(str);
+ for(; ptr != end && isspace(*ptr); ptr++);
+ // shift the string to the left so the original pointer still works
+ memmove(str, ptr, strlen(ptr)+1);
+ return str;
+}
+
+char *str_rstrip(char *str)
+{
+ assert(str != NULL);
+
+ char *ptr = str + strlen(str) - 1;
+ for(; ptr+1 != str && isspace(*ptr); ptr--);
+ *(ptr+1) = '\0';
+ return str;
+}
+
+int str_indexof(const char *haystack, const char *needle)
+{
+ assert(haystack != NULL);
+ assert(needle != NULL);
+
+ // use signed types for hlen/nlen because hlen - nlen can be negative.
+ int hlen = (int) strlen(haystack);
+ int nlen = (int) strlen(needle);
+
+ if (nlen > hlen) return -1;
+
+ for (int i = 0; i <= hlen - nlen; i++) {
+ if (!strncmp(&haystack[i], needle, nlen))
+ return i;
+ }
+
+ return -1;
+}
+
+int str_last_indexof(const char *haystack, const char *needle)
+{
+ assert(haystack != NULL);
+ assert(needle != NULL);
+
+ // use signed types for hlen/nlen because hlen - nlen can be negative.
+ int hlen = (int) strlen(haystack);
+ int nlen = (int) strlen(needle);
+
+ int last_index = -1;
+ for (int i = 0; i <= hlen - nlen; i++) {
+ if (!strncmp(&haystack[i], needle, nlen))
+ last_index = i;
+ }
+
+ return last_index;
+}
+
+// in-place modification.
+char *str_tolowercase(char *s)
+{
+ assert(s != NULL);
+
+ size_t slen = strlen(s);
+ for (int i = 0; i < slen; i++) {
+ if (s[i] >= 'A' && s[i] <= 'Z')
+ s[i] = s[i] + 'a' - 'A';
+ }
+
+ return s;
+}
+
+char *str_touppercase(char *s)
+{
+ assert(s != NULL);
+
+ size_t slen = strlen(s);
+ for (int i = 0; i < slen; i++) {
+ if (s[i] >= 'a' && s[i] <= 'z')
+ s[i] = s[i] - ('a' - 'A');
+ }
+
+ return s;
+}
+
+string_buffer_t* string_buffer_create()
+{
+ string_buffer_t *sb = (string_buffer_t*) calloc(1, sizeof(string_buffer_t));
+ assert(sb != NULL);
+ sb->alloc = 32;
+ sb->s = calloc(sb->alloc, 1);
+ return sb;
+}
+
+void string_buffer_destroy(string_buffer_t *sb)
+{
+ if (sb == NULL)
+ return;
+
+ if (sb->s)
+ free(sb->s);
+
+ memset(sb, 0, sizeof(string_buffer_t));
+ free(sb);
+}
+
+void string_buffer_append(string_buffer_t *sb, char c)
+{
+ assert(sb != NULL);
+
+ if (sb->size+2 >= sb->alloc) {
+ sb->alloc *= 2;
+ sb->s = realloc(sb->s, sb->alloc);
+ }
+
+ sb->s[sb->size++] = c;
+ sb->s[sb->size] = 0;
+}
+
+char string_buffer_pop_back(string_buffer_t *sb) {
+ assert(sb != NULL);
+ if (sb->size == 0)
+ return 0;
+
+ char back = sb->s[--sb->size];
+ sb->s[sb->size] = 0;
+ return back;
+}
+
+void string_buffer_appendf(string_buffer_t *sb, const char *fmt, ...)
+{
+ assert(sb != NULL);
+ assert(fmt != NULL);
+
+ int size = MIN_PRINTF_ALLOC;
+ char *buf = malloc(size * sizeof(char));
+
+ int returnsize;
+ va_list args;
+
+ va_start(args,fmt);
+ returnsize = vsnprintf(buf, size, fmt, args);
+ va_end(args);
+
+ if (returnsize >= size) {
+ // otherwise, we should try again
+ free(buf);
+ size = returnsize + 1;
+ buf = malloc(size * sizeof(char));
+
+ va_start(args, fmt);
+ returnsize = vsnprintf(buf, size, fmt, args);
+ va_end(args);
+
+ assert(returnsize <= size);
+ }
+
+ string_buffer_append_string(sb, buf);
+ free(buf);
+}
+
+void string_buffer_append_string(string_buffer_t *sb, const char *str)
+{
+ assert(sb != NULL);
+ assert(str != NULL);
+
+ size_t len = strlen(str);
+
+ while (sb->size+len + 1 >= sb->alloc) {
+ sb->alloc *= 2;
+ sb->s = realloc(sb->s, sb->alloc);
+ }
+
+ memcpy(&sb->s[sb->size], str, len);
+ sb->size += len;
+ sb->s[sb->size] = 0;
+}
+
+bool string_buffer_ends_with(string_buffer_t *sb, const char *str)
+{
+ assert(sb != NULL);
+ assert(str != NULL);
+
+ return str_ends_with(sb->s, str);
+}
+
+char *string_buffer_to_string(string_buffer_t *sb)
+{
+ assert(sb != NULL);
+#ifdef WINRT
+ return _strdup(sb->s);
+#else
+ return strdup(sb->s);
+#endif
+}
+
+// returns length of string (not counting \0)
+size_t string_buffer_size(string_buffer_t *sb)
+{
+ assert(sb != NULL);
+
+ return sb->size;
+}
+
+void string_buffer_reset(string_buffer_t *sb)
+{
+ assert(sb != NULL);
+
+ sb->s[0] = 0;
+ sb->size = 0;
+}
+
+string_feeder_t *string_feeder_create(const char *str)
+{
+ assert(str != NULL);
+
+ string_feeder_t *sf = (string_feeder_t*) calloc(1, sizeof(string_feeder_t));
+#ifdef WINRT
+ sf->s = _strdup(str);
+#else
+ sf->s = strdup(str);
+#endif
+ sf->len = strlen(sf->s);
+ sf->line = 1;
+ sf->col = 0;
+ sf->pos = 0;
+ return sf;
+}
+
+int string_feeder_get_line(string_feeder_t *sf)
+{
+ assert(sf != NULL);
+ return sf->line;
+}
+
+int string_feeder_get_column(string_feeder_t *sf)
+{
+ assert(sf != NULL);
+ return sf->col;
+}
+
+void string_feeder_destroy(string_feeder_t *sf)
+{
+ if (sf == NULL)
+ return;
+
+ free(sf->s);
+ memset(sf, 0, sizeof(string_feeder_t));
+ free(sf);
+}
+
+bool string_feeder_has_next(string_feeder_t *sf)
+{
+ assert(sf != NULL);
+
+ return sf->s[sf->pos] != 0 && sf->pos <= sf->len;
+}
+
+char string_feeder_next(string_feeder_t *sf)
+{
+ assert(sf != NULL);
+ assert(sf->pos <= sf->len);
+
+ char c = sf->s[sf->pos++];
+ if (c == '\n') {
+ sf->line++;
+ sf->col = 0;
+ } else {
+ sf->col++;
+ }
+
+ return c;
+}
+
+char *string_feeder_next_length(string_feeder_t *sf, size_t length)
+{
+ assert(sf != NULL);
+ assert(length >= 0);
+ assert(sf->pos <= sf->len);
+
+ if (sf->pos + length > sf->len)
+ length = sf->len - sf->pos;
+
+ char *substr = calloc(length+1, sizeof(char));
+ for (int i = 0 ; i < length ; i++)
+ substr[i] = string_feeder_next(sf);
+ return substr;
+}
+
+char string_feeder_peek(string_feeder_t *sf)
+{
+ assert(sf != NULL);
+ assert(sf->pos <= sf->len);
+
+ return sf->s[sf->pos];
+}
+
+char *string_feeder_peek_length(string_feeder_t *sf, size_t length)
+{
+ assert(sf != NULL);
+ assert(length >= 0);
+ assert(sf->pos <= sf->len);
+
+ if (sf->pos + length > sf->len)
+ length = sf->len - sf->pos;
+
+ char *substr = calloc(length+1, sizeof(char));
+ memcpy(substr, &sf->s[sf->pos], length*sizeof(char));
+ return substr;
+}
+
+bool string_feeder_starts_with(string_feeder_t *sf, const char *str)
+{
+ assert(sf != NULL);
+ assert(str != NULL);
+ assert(sf->pos <= sf->len);
+
+ return str_starts_with(&sf->s[sf->pos], str);
+}
+
+void string_feeder_require(string_feeder_t *sf, const char *str)
+{
+ assert(sf != NULL);
+ assert(str != NULL);
+ assert(sf->pos <= sf->len);
+
+ size_t len = strlen(str);
+
+ for (int i = 0; i < len; i++) {
+ char c = string_feeder_next(sf);
+ assert(c == str[i]);
+ }
+}
+
+////////////////////////////////////////////
+bool str_ends_with(const char *haystack, const char *needle)
+{
+ assert(haystack != NULL);
+ assert(needle != NULL);
+
+ size_t lens = strlen(haystack);
+ size_t lenneedle = strlen(needle);
+
+ if (lenneedle > lens)
+ return false;
+
+ return !strncmp(&haystack[lens - lenneedle], needle, lenneedle);
+}
+
+inline bool str_starts_with(const char *haystack, const char *needle)
+{
+ assert(haystack != NULL);
+ assert(needle != NULL);
+
+ // haystack[pos] doesn't have to be compared to zero; if it were
+ // zero, it either doesn't match needle (in which case the loop
+ // terminates) or it matches needle[pos] (in which case the loop
+ // terminates).
+ int pos = 0;
+ while (haystack[pos] == needle[pos] && needle[pos] != 0)
+ pos++;
+
+ return (needle[pos] == 0);
+}
+
+bool str_starts_with_any(const char *haystack, const char **needles, int num_needles)
+{
+ assert(haystack != NULL);
+ assert(needles != NULL);
+ assert(num_needles >= 0);
+
+ for (int i = 0; i < num_needles; i++) {
+ assert(needles[i] != NULL);
+ if (str_starts_with(haystack, needles[i]))
+ return true;
+ }
+
+ return false;
+}
+
+bool str_matches_any(const char *haystack, const char **needles, int num_needles)
+{
+ assert(haystack != NULL);
+ assert(needles != NULL);
+ assert(num_needles >= 0);
+
+ for (int i = 0; i < num_needles; i++) {
+ assert(needles[i] != NULL);
+ if (!strcmp(haystack, needles[i]))
+ return true;
+ }
+
+ return false;
+}
+
+char *str_substring(const char *str, size_t startidx, long endidx)
+{
+ assert(str != NULL);
+ assert(startidx >= 0 && startidx <= strlen(str)+1);
+ assert(endidx < 0 || endidx >= startidx);
+ assert(endidx < 0 || endidx <= strlen(str)+1);
+
+ if (endidx < 0)
+ endidx = (long) strlen(str);
+
+ size_t blen = endidx - startidx; // not counting \0
+ char *b = malloc(blen + 1);
+ memcpy(b, &str[startidx], blen);
+ b[blen] = 0;
+ return b;
+}
+
+char *str_replace(const char *haystack, const char *needle, const char *replacement)
+{
+ assert(haystack != NULL);
+ assert(needle != NULL);
+ assert(replacement != NULL);
+
+ string_buffer_t *sb = string_buffer_create();
+ size_t haystack_len = strlen(haystack);
+ size_t needle_len = strlen(needle);
+
+ int pos = 0;
+ while (pos < haystack_len) {
+ if (needle_len > 0 && str_starts_with(&haystack[pos], needle)) {
+ string_buffer_append_string(sb, replacement);
+ pos += needle_len;
+ } else {
+ string_buffer_append(sb, haystack[pos]);
+ pos++;
+ }
+ }
+ if (needle_len == 0 && haystack_len == 0)
+ string_buffer_append_string(sb, replacement);
+
+ char *res = string_buffer_to_string(sb);
+ string_buffer_destroy(sb);
+ return res;
+}
+
+char *str_replace_many(const char *_haystack, ...)
+{
+ va_list ap;
+ va_start(ap, _haystack);
+
+#ifdef WINRT
+ char *haystack = _strdup(_haystack);
+#else
+ char *haystack = strdup(_haystack);
+#endif
+
+ while (true) {
+ char *needle = va_arg(ap, char*);
+ if (!needle)
+ break;
+
+ char *replacement = va_arg(ap, char*);
+ char *tmp = str_replace(haystack, needle, replacement);
+ free(haystack);
+ haystack = tmp;
+ }
+
+ va_end(ap);
+
+ return haystack;
+}
+
+static void buffer_appendf(char **_buf, int *bufpos, void *fmt, ...)
+{
+ char *buf = *_buf;
+ va_list ap;
+
+ int salloc = 128;
+ char *s = malloc(salloc);
+
+ va_start(ap, fmt);
+ int slen = vsnprintf(s, salloc, fmt, ap);
+ va_end(ap);
+
+ if (slen >= salloc) {
+ s = realloc(s, slen + 1);
+ va_start(ap, fmt);
+ vsprintf((char*) s, fmt, ap);
+ va_end(ap);
+ }
+
+ buf = realloc(buf, *bufpos + slen + 1);
+ *_buf = buf;
+
+ memcpy(&buf[*bufpos], s, slen + 1); // get trailing \0
+ (*bufpos) += slen;
+
+ free(s);
+}
+
+static int is_variable_character(char c)
+{
+ if (c >= 'a' && c <= 'z')
+ return 1;
+
+ if (c >= 'A' && c <= 'Z')
+ return 1;
+
+ if (c >= '0' && c <= '9')
+ return 1;
+
+ if (c == '_')
+ return 1;
+
+ return 0;
+}
+
+#ifndef WINRT
+char *str_expand_envs(const char *in)
+{
+ size_t inlen = strlen(in);
+ size_t inpos = 0;
+
+ char *out = NULL;
+ int outpos = 0;
+
+ while (inpos < inlen) {
+
+ if (in[inpos] != '$') {
+ buffer_appendf(&out, &outpos, "%c", in[inpos]);
+ inpos++;
+ continue;
+
+ } else {
+ inpos++; // consume '$'
+
+ char *varname = NULL;
+ int varnamepos = 0;
+
+ while (varnamepos < sizeof(varname) && inpos < inlen && is_variable_character(in[inpos])) {
+ buffer_appendf(&varname, &varnamepos, "%c", in[inpos]);
+ inpos++;
+ }
+
+ char *env = getenv(varname);
+ if (env)
+ buffer_appendf(&out, &outpos, "%s", env);
+
+ free(varname);
+ }
+ }
+
+ return out;
+}
+#endif
\ No newline at end of file
diff --git a/3rdparty/apriltag/common/string_util.h b/3rdparty/apriltag/common/string_util.h
new file mode 100644
index 0000000..2b8c491
--- /dev/null
+++ b/3rdparty/apriltag/common/string_util.h
@@ -0,0 +1,471 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _STRING_UTIL_H
+#define _STRING_UTIL_H
+
+#include <stdio.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <ctype.h>
+
+#include "common/zarray.h"
+
+#ifdef _MSC_VER
+#define __attribute__(A)
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+typedef struct string_buffer string_buffer_t;
+
+typedef struct string_feeder string_feeder_t;
+struct string_feeder
+{
+ char *s;
+ size_t len;
+ size_t pos;
+
+ int line, col;
+};
+
+/**
+ * Similar to sprintf(), except that it will malloc() enough space for the
+ * formatted string which it returns. It is the caller's responsibility to call
+ * free() on the returned string when it is no longer needed.
+ */
+char *sprintf_alloc(const char *fmt, ...) __attribute__ ((format (printf, 1, 2)));
+
+/**
+ * Similar to vsprintf(), except that it will malloc() enough space for the
+ * formatted string which it returns. It is the caller's responsibility to call
+ * free() on the returned string when it is no longer needed.
+ */
+char *vsprintf_alloc(const char *fmt, va_list args);
+
+/**
+ * Concatenates 1 or more strings together and returns the result, which will be a
+ * newly allocated string which it is the caller's responsibility to free.
+ */
+#define str_concat(...) _str_concat_private(__VA_ARGS__, NULL)
+char *_str_concat_private(const char *first, ...);
+
+
+// Returns the index of the first character that differs:
+int str_diff_idx(const char * a, const char * b);
+
+/**
+ * Splits the supplied string into an array of strings by subdividing it at
+ * each occurrence of the supplied delimiter string. The split strings will not
+ * contain the delimiter. The original string will remain unchanged.
+ * If str is composed of all delimiters, an empty array will be returned.
+ *
+ * It is the caller's responsibilty to free the returned zarray, as well as
+ * the strings contained within it, e.g.:
+ *
+ * zarray_t *za = str_split("this is a haystack", " ");
+ * => ["this", "is", "a", "haystack"]
+ * zarray_vmap(za, free);
+ * zarray_destroy(za);
+ */
+zarray_t *str_split(const char *str, const char *delim);
+
+zarray_t *str_split_spaces(const char *str);
+
+void str_split_destroy(zarray_t *s);
+
+/*
+ * Determines if str1 exactly matches str2 (more efficient than strcmp(...) == 0)
+ */
+static inline bool streq(const char *str1, const char* str2)
+{
+ int i;
+ for (i = 0 ; str1[i] != '\0' ; i++) {
+ if (str1[i] != str2[i])
+ return false;
+ }
+
+ return str2[i] == '\0';
+}
+
+/**
+ * Determines if str1 exactly matches str2, ignoring case (more efficient than
+ * strcasecmp(...) == 0)
+ */
+static inline bool strcaseeq(const char *str1, const char* str2)
+{
+ int i;
+ for (i = 0 ; str1[i] != '\0' ; i++) {
+ if (str1[i] == str2[i])
+ continue;
+ else if (islower(str1[i]) && (str1[i] - 32) == str2[i])
+ continue;
+ else if (isupper(str1[i]) && (str1[i] + 32) == str2[i])
+ continue;
+
+ return false;
+ }
+
+ return str2[i] == '\0';
+}
+
+/**
+ * Trims whitespace characters (i.e. matching isspace()) from the beginning and/or
+ * end of the supplied string. This change affects the supplied string in-place.
+ * The supplied/edited string is returned to enable chained reference.
+ *
+ * Note: do not pass a string literal to this function
+ */
+char *str_trim(char *str);
+
+/**
+ * Trims whitespace characters (i.e. matching isspace()) from the beginning
+ * of the supplied string. This change affects the supplied string in-place.
+ * The supplied/edited string is returned to enable chained reference.
+ *
+ * Note: do not pass a string literal to this function
+ */
+char *str_lstrip(char *str);
+
+/**
+ * Trims whitespace characters (i.e. matching isspace()) from the end of the
+ * supplied string. This change affects the supplied string in-place.
+ * The supplied/edited string is returned to enable chained reference.
+ *
+ * Note: do not pass a string literal to this function
+ */
+char *str_rstrip(char *str);
+
+/**
+ * Returns true if the end of string 'haystack' matches 'needle', else false.
+ *
+ * Note: An empty needle ("") will match any source.
+ */
+bool str_ends_with(const char *haystack, const char *needle);
+
+/**
+ * Returns true if the start of string 'haystack' matches 'needle', else false.
+ *
+ * Note: An empty needle ("") will match any source.
+ */
+bool str_starts_with(const char *haystack, const char *needle);
+
+/**
+ * Returns true if the start of string 'haystack' matches any needle, else false.
+ *
+ * Note: An empty needle ("") will match any source.
+ */
+bool str_starts_with_any(const char *haystack, const char **needles, int num_needles);
+
+/**
+ * Returns true if the string 'haystack' matches any needle, else false.
+ */
+bool str_matches_any(const char *haystack, const char **needles, int num_needles);
+
+/**
+ * Retrieves a (newly-allocated) substring of the given string, 'str', starting
+ * from character index 'startidx' through index 'endidx' - 1 (inclusive).
+ * An 'endidx' value -1 is equivalent to strlen(str).
+ *
+ * It is the caller's responsibility to free the returned string.
+ *
+ * Examples:
+ * str_substring("string", 1, 3) = "tr"
+ * str_substring("string", 2, -1) = "ring"
+ * str_substring("string", 3, 3) = ""
+ *
+ * Note: startidx must be >= endidx
+ */
+char *str_substring(const char *str, size_t startidx, long endidx);
+
+/**
+ * Retrieves the zero-based index of the beginning of the supplied substring
+ * (needle) within the search string (haystack) if it exists.
+ *
+ * Returns -1 if the supplied needle is not found within the haystack.
+ */
+int str_indexof(const char *haystack, const char *needle);
+
+ static inline int str_contains(const char *haystack, const char *needle) {
+ return str_indexof(haystack, needle) >= 0;
+ }
+
+// same as above, but returns last match
+int str_last_indexof(const char *haystack, const char *needle);
+
+/**
+ * Replaces all upper-case characters within the supplied string with their
+ * lower-case counterparts, modifying the original string's contents.
+ *
+ * Returns the supplied / modified string.
+ */
+char *str_tolowercase(char *s);
+
+/**
+ * Replaces all lower-case characters within the supplied string with their
+ * upper-case counterparts, modifying the original string's contents.
+ *
+ * Returns the supplied / modified string.
+ */
+char *str_touppercase(char *s);
+
+/**
+ * Replaces all occurrences of 'needle' in the string 'haystack', substituting
+ * for them the value of 'replacement', and returns the result as a newly-allocated
+ * string. The original strings remain unchanged.
+ *
+ * It is the caller's responsibility to free the returned string.
+ *
+ * Examples:
+ * str_replace("string", "ri", "u") = "stung"
+ * str_replace("singing", "ing", "") = "s"
+ * str_replace("string", "foo", "bar") = "string"
+ *
+ * Note: An empty needle will match only an empty haystack
+ */
+char *str_replace(const char *haystack, const char *needle, const char *replacement);
+
+ char *str_replace_many(const char *_haystack, ...);
+//////////////////////////////////////////////////////
+// String Buffer
+
+/**
+ * Creates and initializes a string buffer object which can be used with any of
+ * the string_buffer_*() functions.
+ *
+ * It is the caller's responsibility to free the string buffer resources with
+ * a call to string_buffer_destroy() when it is no longer needed.
+ */
+string_buffer_t *string_buffer_create();
+
+/**
+ * Frees the resources associated with a string buffer object, including space
+ * allocated for any appended characters / strings.
+ */
+void string_buffer_destroy(string_buffer_t *sb);
+
+/**
+ * Appends a single character to the end of the supplied string buffer.
+ */
+void string_buffer_append(string_buffer_t *sb, char c);
+
+/**
+ * Removes a single character from the end of the string and
+ * returns it. Does nothing if string is empty and returns NULL
+ */
+char string_buffer_pop_back(string_buffer_t *sb);
+
+/**
+ * Appends the supplied string to the end of the supplied string buffer.
+ */
+void string_buffer_append_string(string_buffer_t *sb, const char *str);
+
+/**
+ * Formats the supplied string and arguments in a manner akin to printf(), and
+ * appends the resulting string to the end of the supplied string buffer.
+ */
+void string_buffer_appendf(string_buffer_t *sb, const char *fmt, ...) __attribute__ ((format (printf, 2, 3)));
+
+/**
+ * Determines whether the character contents held by the supplied string buffer
+ * ends with the supplied string.
+ *
+ * Returns true if the string buffer's contents ends with 'str', else false.
+ */
+bool string_buffer_ends_with(string_buffer_t *sb, const char *str);
+
+/**
+ * Returns the string-length of the contents of the string buffer (not counting \0).
+ * Equivalent to calling strlen() on the string returned by string_buffer_to_string(sb).
+ */
+size_t string_buffer_size(string_buffer_t *sb);
+
+/**
+ * Returns the contents of the string buffer in a newly-allocated string, which
+ * it is the caller's responsibility to free once it is no longer needed.
+ */
+char *string_buffer_to_string(string_buffer_t *sb);
+
+/**
+ * Clears the contents of the string buffer, setting its length to zero.
+ */
+void string_buffer_reset(string_buffer_t *sb);
+
+//////////////////////////////////////////////////////
+// String Feeder
+
+/**
+ * Creates a string feeder object which can be used to traverse the supplied
+ * string using the string_feeder_*() functions. A local copy of the string's
+ * contents will be stored so that future changes to 'str' will not be
+ * reflected by the string feeder object.
+ *
+ * It is the caller's responsibility to call string_feeder_destroy() on the
+ * returned object when it is no longer needed.
+ */
+string_feeder_t *string_feeder_create(const char *str);
+
+/**
+ * Frees resources associated with the supplied string feeder object, after
+ * which it will no longer be valid for use.
+ */
+void string_feeder_destroy(string_feeder_t *sf);
+
+/**
+ * Determines whether any characters remain to be retrieved from the string
+ * feeder's string (not including the terminating '\0').
+ *
+ * Returns true if at least one more character can be retrieved with calls to
+ * string_feeder_next(), string_feeder_peek(), string_feeder_peek(), or
+ * string_feeder_consume(), else false.
+ */
+bool string_feeder_has_next(string_feeder_t *sf);
+
+/**
+ * Retrieves the next available character from the supplied string feeder
+ * (which may be the terminating '\0' character) and advances the feeder's
+ * position to the next character in the string.
+ *
+ * Note: Attempts to read past the end of the string will throw an assertion.
+ */
+char string_feeder_next(string_feeder_t *sf);
+
+/**
+ * Retrieves a series of characters from the supplied string feeder. The number
+ * of characters returned will be 'length' or the number of characters
+ * remaining in the string, whichever is shorter. The string feeder's position
+ * will be advanced by the number of characters returned.
+ *
+ * It is the caller's responsibility to free the returned string when it is no
+ * longer needed.
+ *
+ * Note: Calling once the end of the string has already been read will throw an assertion.
+ */
+char *string_feeder_next_length(string_feeder_t *sf, size_t length);
+
+/**
+ * Retrieves the next available character from the supplied string feeder
+ * (which may be the terminating '\0' character), but does not advance
+ * the feeder's position so that subsequent calls to _next() or _peek() will
+ * retrieve the same character.
+ *
+ * Note: Attempts to peek past the end of the string will throw an assertion.
+ */
+char string_feeder_peek(string_feeder_t *sf);
+
+/**
+ * Retrieves a series of characters from the supplied string feeder. The number
+ * of characters returned will be 'length' or the number of characters
+ * remaining in the string, whichever is shorter. The string feeder's position
+ * will not be advanced.
+ *
+ * It is the caller's responsibility to free the returned string when it is no
+ * longer needed.
+ *
+ * Note: Calling once the end of the string has already been read will throw an assertion.
+ */
+char *string_feeder_peek_length(string_feeder_t *sf, size_t length);
+
+/**
+ * Retrieves the line number of the current position in the supplied
+ * string feeder, which will be incremented whenever a newline is consumed.
+ *
+ * Examples:
+ * prior to reading 1st character: line = 1, column = 0
+ * after reading 1st non-newline character: line = 1, column = 1
+ * after reading 2nd non-newline character: line = 1, column = 2
+ * after reading 1st newline character: line = 2, column = 0
+ * after reading 1st character after 1st newline: line = 2, column = 1
+ * after reading 2nd newline character: line = 3, column = 0
+ */
+int string_feeder_get_line(string_feeder_t *sf);
+
+/**
+ * Retrieves the column index in the current line for the current position
+ * in the supplied string feeder, which will be incremented with each
+ * non-newline character consumed, and reset to 0 whenever a newline (\n) is
+ * consumed.
+ *
+ * Examples:
+ * prior to reading 1st character: line = 1, column = 0
+ * after reading 1st non-newline character: line = 1, column = 1
+ * after reading 2nd non-newline character: line = 1, column = 2
+ * after reading 1st newline character: line = 2, column = 0
+ * after reading 1st character after 1st newline: line = 2, column = 1
+ * after reading 2nd newline character: line = 3, column = 0
+ */
+int string_feeder_get_column(string_feeder_t *sf);
+
+/**
+ * Determines whether the supplied string feeder's remaining contents starts
+ * with the given string.
+ *
+ * Returns true if the beginning of the string feeder's remaining contents matches
+ * the supplied string exactly, else false.
+ */
+bool string_feeder_starts_with(string_feeder_t *sf, const char *str);
+
+/**
+ * Consumes from the string feeder the number of characters contained in the
+ * given string (not including the terminating '\0').
+ *
+ * Throws an assertion if the consumed characters do not exactly match the
+ * contents of the supplied string.
+ */
+void string_feeder_require(string_feeder_t *sf, const char *str);
+
+/*#ifndef strdup
+ static inline char *strdup(const char *s) {
+ int len = strlen(s);
+ char *out = malloc(len+1);
+ memcpy(out, s, len + 1);
+ return out;
+ }
+#endif
+*/
+
+
+// find everything that looks like an env variable and expand it
+// using getenv. Caller should free the result.
+// e.g. "$HOME/abc" ==> "/home/ebolson/abc"
+#ifndef WINRT
+char *str_expand_envs(const char *in);
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/svd22.c b/3rdparty/apriltag/common/svd22.c
new file mode 100644
index 0000000..5fc709d
--- /dev/null
+++ b/3rdparty/apriltag/common/svd22.c
@@ -0,0 +1,268 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <math.h>
+#include <string.h>
+#include <assert.h>
+#include <stdio.h>
+
+#include "common/doubles.h"
+
+/** SVD 2x2.
+
+ Computes singular values and vectors without squaring the input
+ matrix. With double precision math, results are accurate to about
+ 1E-16.
+
+ U = [ cos(theta) -sin(theta) ]
+ [ sin(theta) cos(theta) ]
+
+ S = [ e 0 ]
+ [ 0 f ]
+
+ V = [ cos(phi) -sin(phi) ]
+ [ sin(phi) cos(phi) ]
+
+
+ Our strategy is basically to analytically multiply everything out
+ and then rearrange so that we can solve for theta, phi, e, and
+ f. (Derivation by ebolson at umich.edu 5/2016)
+
+ V' = [ CP SP ]
+ [ -SP CP ]
+
+USV' = [ CT -ST ][ e*CP e*SP ]
+ [ ST CT ][ -f*SP f*CP ]
+
+ = [e*CT*CP + f*ST*SP e*CT*SP - f*ST*CP ]
+ [e*ST*CP - f*SP*CT e*SP*ST + f*CP*CT ]
+
+A00+A11 = e*CT*CP + f*ST*SP + e*SP*ST + f*CP*CT
+ = e*(CP*CT + SP*ST) + f*(SP*ST + CP*CT)
+ = (e+f)(CP*CT + SP*ST)
+B0 = (e+f)*cos(P-T)
+
+A00-A11 = e*CT*CP + f*ST*SP - e*SP*ST - f*CP*CT
+ = e*(CP*CT - SP*ST) - f*(-ST*SP + CP*CT)
+ = (e-f)(CP*CT - SP*ST)
+B1 = (e-f)*cos(P+T)
+
+A01+A10 = e*CT*SP - f*ST*CP + e*ST*CP - f*SP*CT
+ = e(CT*SP + ST*CP) - f*(ST*CP + SP*CT)
+ = (e-f)*(CT*SP + ST*CP)
+B2 = (e-f)*sin(P+T)
+
+A01-A10 = e*CT*SP - f*ST*CP - e*ST*CP + f*SP*CT
+ = e*(CT*SP - ST*CP) + f(SP*CT - ST*CP)
+ = (e+f)*(CT*SP - ST*CP)
+B3 = (e+f)*sin(P-T)
+
+B0 = (e+f)*cos(P-T)
+B1 = (e-f)*cos(P+T)
+B2 = (e-f)*sin(P+T)
+B3 = (e+f)*sin(P-T)
+
+B3/B0 = tan(P-T)
+
+B2/B1 = tan(P+T)
+ **/
+void svd22(const double A[4], double U[4], double S[2], double V[4])
+{
+ double A00 = A[0];
+ double A01 = A[1];
+ double A10 = A[2];
+ double A11 = A[3];
+
+ double B0 = A00 + A11;
+ double B1 = A00 - A11;
+ double B2 = A01 + A10;
+ double B3 = A01 - A10;
+
+ double PminusT = atan2(B3, B0);
+ double PplusT = atan2(B2, B1);
+
+ double P = (PminusT + PplusT) / 2;
+ double T = (-PminusT + PplusT) / 2;
+
+ double CP = cos(P), SP = sin(P);
+ double CT = cos(T), ST = sin(T);
+
+ U[0] = CT;
+ U[1] = -ST;
+ U[2] = ST;
+ U[3] = CT;
+
+ V[0] = CP;
+ V[1] = -SP;
+ V[2] = SP;
+ V[3] = CP;
+
+ // C0 = e+f. There are two ways to compute C0; we pick the one
+ // that is better conditioned.
+ double CPmT = cos(P-T), SPmT = sin(P-T);
+ double C0 = 0;
+ if (fabs(CPmT) > fabs(SPmT))
+ C0 = B0 / CPmT;
+ else
+ C0 = B3 / SPmT;
+
+ // C1 = e-f. There are two ways to compute C1; we pick the one
+ // that is better conditioned.
+ double CPpT = cos(P+T), SPpT = sin(P+T);
+ double C1 = 0;
+ if (fabs(CPpT) > fabs(SPpT))
+ C1 = B1 / CPpT;
+ else
+ C1 = B2 / SPpT;
+
+ // e and f are the singular values
+ double e = (C0 + C1) / 2;
+ double f = (C0 - C1) / 2;
+
+ if (e < 0) {
+ e = -e;
+ U[0] = -U[0];
+ U[2] = -U[2];
+ }
+
+ if (f < 0) {
+ f = -f;
+ U[1] = -U[1];
+ U[3] = -U[3];
+ }
+
+ // sort singular values.
+ if (e > f) {
+ // already in big-to-small order.
+ S[0] = e;
+ S[1] = f;
+ } else {
+ // Curiously, this code never seems to get invoked. Why is it
+ // that S[0] always ends up the dominant vector? However,
+ // this code has been tested (flipping the logic forces us to
+ // sort the singular values in ascending order).
+ //
+ // P = [ 0 1 ; 1 0 ]
+ // USV' = (UP)(PSP)(PV')
+ // = (UP)(PSP)(VP)'
+ // = (UP)(PSP)(P'V')'
+ S[0] = f;
+ S[1] = e;
+
+ // exchange columns of U and V
+ double tmp[2];
+ tmp[0] = U[0];
+ tmp[1] = U[2];
+ U[0] = U[1];
+ U[2] = U[3];
+ U[1] = tmp[0];
+ U[3] = tmp[1];
+
+ tmp[0] = V[0];
+ tmp[1] = V[2];
+ V[0] = V[1];
+ V[2] = V[3];
+ V[1] = tmp[0];
+ V[3] = tmp[1];
+ }
+
+ /*
+ double SM[4] = { S[0], 0, 0, S[1] };
+
+ doubles_print_mat(U, 2, 2, "%20.10g");
+ doubles_print_mat(SM, 2, 2, "%20.10g");
+ doubles_print_mat(V, 2, 2, "%20.10g");
+ printf("A:\n");
+ doubles_print_mat(A, 2, 2, "%20.10g");
+
+ double SVt[4];
+ doubles_mat_ABt(SM, 2, 2, V, 2, 2, SVt, 2, 2);
+ double USVt[4];
+ doubles_mat_AB(U, 2, 2, SVt, 2, 2, USVt, 2, 2);
+
+ printf("USVt\n");
+ doubles_print_mat(USVt, 2, 2, "%20.10g");
+
+ double diff[4];
+ for (int i = 0; i < 4; i++)
+ diff[i] = A[i] - USVt[i];
+
+ printf("diff\n");
+ doubles_print_mat(diff, 2, 2, "%20.10g");
+
+ */
+
+}
+
+
+// for the matrix [a b; b d]
+void svd_sym_singular_values(double A00, double A01, double A11,
+ double *Lmin, double *Lmax)
+{
+ double A10 = A01;
+
+ double B0 = A00 + A11;
+ double B1 = A00 - A11;
+ double B2 = A01 + A10;
+ double B3 = A01 - A10;
+
+ double PminusT = atan2(B3, B0);
+ double PplusT = atan2(B2, B1);
+
+ double P = (PminusT + PplusT) / 2;
+ double T = (-PminusT + PplusT) / 2;
+
+ // C0 = e+f. There are two ways to compute C0; we pick the one
+ // that is better conditioned.
+ double CPmT = cos(P-T), SPmT = sin(P-T);
+ double C0 = 0;
+ if (fabs(CPmT) > fabs(SPmT))
+ C0 = B0 / CPmT;
+ else
+ C0 = B3 / SPmT;
+
+ // C1 = e-f. There are two ways to compute C1; we pick the one
+ // that is better conditioned.
+ double CPpT = cos(P+T), SPpT = sin(P+T);
+ double C1 = 0;
+ if (fabs(CPpT) > fabs(SPpT))
+ C1 = B1 / CPpT;
+ else
+ C1 = B2 / SPpT;
+
+ // e and f are the singular values
+ double e = (C0 + C1) / 2;
+ double f = (C0 - C1) / 2;
+
+ *Lmin = fmin(e, f);
+ *Lmax = fmax(e, f);
+}
diff --git a/3rdparty/apriltag/common/svd22.h b/3rdparty/apriltag/common/svd22.h
new file mode 100644
index 0000000..c34256c
--- /dev/null
+++ b/3rdparty/apriltag/common/svd22.h
@@ -0,0 +1,42 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _SVD22_H
+#define _SVD22_H
+
+void svd22(const double A[4], double U[4], double S[2], double V[4]);
+
+// for the matrix [a b; b d]
+void svd_sym_singular_values(double A00, double A01, double A11,
+ double *Lmin, double *Lmax);
+
+#endif
diff --git a/3rdparty/apriltag/common/sys/times.h b/3rdparty/apriltag/common/sys/times.h
new file mode 100644
index 0000000..f3bd926
--- /dev/null
+++ b/3rdparty/apriltag/common/sys/times.h
@@ -0,0 +1,40 @@
+#ifndef _TIMES_H
+#define _TIMES_H
+
+#ifdef _WIN32
+#include <sys/timeb.h>
+#include <sys/types.h>
+#include <winsock2.h>
+#include <Windows.h>
+#include <stdint.h> // portable: uint64_t MSVC: __int64
+
+int gettimeofday(struct timeval* t, void* timezone);
+
+// from linux's sys/times.h
+
+//#include <features.h>
+
+#define __need_clock_t
+#include <time.h>
+
+
+/* Structure describing CPU time used by a process and its children. */
+struct tms
+ {
+ clock_t tms_utime; /* User CPU time. */
+ clock_t tms_stime; /* System CPU time. */
+
+ clock_t tms_cutime; /* User CPU time of dead children. */
+ clock_t tms_cstime; /* System CPU time of dead children. */
+ };
+
+/* Store the CPU time used by this process and all its
+ dead children (and their dead children) in BUFFER.
+ Return the elapsed real time, or (clock_t) -1 for errors.
+ All times are in CLK_TCKths of a second. */
+clock_t times (struct tms *__buffer);
+
+typedef long long suseconds_t;
+
+#endif
+#endif
diff --git a/3rdparty/apriltag/common/thash_impl.h b/3rdparty/apriltag/common/thash_impl.h
new file mode 100644
index 0000000..3865840
--- /dev/null
+++ b/3rdparty/apriltag/common/thash_impl.h
@@ -0,0 +1,391 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+/*
+// beware: The combination of:
+// 1) a hash function that uses floating point values
+// 2) that is inlined
+// 3) and compiled with -Ofast
+// can result in inconsistent values being computed. You can force the function
+// NOT to be inlined with __attribute__ ((noinline)).
+//
+// It's also tempting to do:
+// #define TKEYEQUAL(pka, pkb) (!memcmp(pka, pkb, sizeof(my_key_tyep)))
+//
+// But this will not work as expected if the structure contains
+// padding that is not consistently cleared to zero. It appears that
+// in C99, copying a struct by value does NOT necessarily copy
+// padding, and so it may be difficult to guarantee that padding is
+// zero, even when the code otherwise appears sane.
+//
+// You can use the "performance" method to evaluate how well your hash
+// function is doing. Bad hash functions (obviously) are very bad for
+// performance!
+
+#define TNAME sm_points_hash
+#define TVALTYPE struct sm_points_record
+#define TKEYTYPE zarray_t*
+
+// takes a pointer to the key
+#define TKEYHASH(pk) ((uint32_t) (pk->level + (a->rad * 100) + (a->meters_per_pixel*100)))
+
+// takes a pointer to the value
+#define TKEYEQUAL(pka, pkb) (!memcmp(pka, pkb, sizeof(struct sm_points_record)))
+
+*/
+
+// when nentries/size is greater than _CRITICAL, we trigger a rehash.
+#define THASH_FACTOR_CRITICAL 2
+
+// when rehashing (or allocating with a target capacity), we use this ratio of nentries/size
+// should be greater than THASH_FACTOR_CRITICAL
+#define THASH_FACTOR_REALLOC 4
+
+#define TRRFN(root, suffix) root ## _ ## suffix
+#define TRFN(root, suffix) TRRFN(root, suffix)
+#define TFN(suffix) TRFN(TNAME, suffix)
+
+#define TTYPENAME TFN(t)
+
+struct TFN(_entry)
+{
+ // XXX Better to store the hash value and to reserve a special
+ // hash value to mean invalid?
+ uint8_t valid;
+ TKEYTYPE key;
+ TVALTYPE value;
+};
+
+typedef struct TTYPENAME TTYPENAME;
+struct TTYPENAME
+{
+ struct TFN(_entry) *entries;
+ int nentries;
+ int size; // always a power of two.
+};
+
+// will allocate enough room so that size can grow to 'capacity'
+// without rehashing.
+static inline TTYPENAME *TFN(create_capacity)(int capacity)
+{
+ // must be this large to not trigger rehash
+ int _nentries = THASH_FACTOR_REALLOC*capacity;
+ if (_nentries < 8)
+ _nentries = 8;
+
+ // but must also be a power of 2
+ int nentries = _nentries;
+ if ((nentries & (nentries - 1)) != 0) {
+ nentries = 8;
+ while (nentries < _nentries)
+ nentries *= 2;
+ }
+
+ assert((nentries & (nentries-1)) == 0);
+ TTYPENAME *hash = calloc(1, sizeof(TTYPENAME));
+ hash->nentries = nentries;
+ hash->entries = calloc(hash->nentries, sizeof(struct TFN(_entry)));
+ return hash;
+}
+
+static inline TTYPENAME *TFN(create)()
+{
+ return TFN(create_capacity)(8);
+}
+
+static inline void TFN(destroy)(TTYPENAME *hash)
+{
+ if (!hash)
+ return;
+
+ free(hash->entries);
+ free(hash);
+}
+
+static inline int TFN(size)(TTYPENAME *hash)
+{
+ return hash->size;
+}
+
+static inline void TFN(clear)(TTYPENAME *hash)
+{
+ // could just clear the 'valid' flag.
+ memset(hash->entries, 0, hash->nentries * sizeof(struct TFN(_entry)));
+ hash->size = 0;
+}
+
+// examine the performance of the hashing function by looking at the distribution of bucket->size
+static inline void TFN(performance)(TTYPENAME *hash)
+{
+ int runs_sz = 32;
+ int runs[runs_sz];
+ int cnt = 0;
+ int max_run = 0;
+ int min_run = hash->size;
+ int run1 = 0;
+ int run2 = 0;
+
+ memset(runs, 0, sizeof(runs));
+
+ for (int entry_idx = 0; entry_idx < hash->nentries; entry_idx++) {
+ if (!hash->entries[entry_idx].valid)
+ continue;
+
+ int this_run = 0;
+ while (hash->entries[(entry_idx+this_run) & (hash->nentries - 1)].valid)
+ this_run++;
+ if (this_run < runs_sz)
+ runs[this_run]++;
+ if (this_run < min_run)
+ min_run = this_run;
+ if (this_run > max_run)
+ max_run = this_run;
+
+ run1 += this_run;
+ run2 += this_run*this_run;
+ cnt++;
+ }
+
+ double Ex1 = 1.0 * run1 / cnt;
+ double Ex2 = 1.0 * run2 / cnt;
+
+#define strr(s) #s
+#define str(s) strr(s)
+ printf("%s: size %8d, nentries: %8d, min %3d, max %3d, mean %6.3f, stddev %6.3f\n",
+ str(TNAME),
+ hash->size, hash->nentries, min_run, max_run, Ex1, sqrt(Ex2 - Ex1*Ex1));
+}
+
+static inline int TFN(get_volatile)(TTYPENAME *hash, TKEYTYPE *key, TVALTYPE **value)
+{
+ uint32_t code = TKEYHASH(key);
+ uint32_t entry_idx = code & (hash->nentries - 1);
+
+ while (hash->entries[entry_idx].valid) {
+ if (TKEYEQUAL(key, &hash->entries[entry_idx].key)) {
+ *value = &hash->entries[entry_idx].value;
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ return 0;
+}
+
+static inline int TFN(get)(TTYPENAME *hash, TKEYTYPE *key, TVALTYPE *value)
+{
+ // XXX see implementation in zhash.c (implement in terms of
+ // get_volatile)
+
+ uint32_t code = TKEYHASH(key);
+ uint32_t entry_idx = code & (hash->nentries - 1);
+
+ while (hash->entries[entry_idx].valid) {
+ if (TKEYEQUAL(key, &hash->entries[entry_idx].key)) {
+ *value = hash->entries[entry_idx].value;
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ return 0;
+}
+
+static inline int TFN(put)(TTYPENAME *hash, TKEYTYPE *key, TVALTYPE *value, TKEYTYPE *oldkey, TVALTYPE *oldvalue)
+{
+ uint32_t code = TKEYHASH(key);
+ uint32_t entry_idx = code & (hash->nentries - 1);
+
+ while (hash->entries[entry_idx].valid) {
+ if (TKEYEQUAL(key, &hash->entries[entry_idx].key)) {
+ if (oldkey)
+ *oldkey = hash->entries[entry_idx].key;
+ if (oldvalue)
+ *oldvalue = hash->entries[entry_idx].value;
+ hash->entries[entry_idx].key = *key;
+ hash->entries[entry_idx].value = *value;
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ hash->entries[entry_idx].valid = 1;
+ hash->entries[entry_idx].key = *key;
+ hash->entries[entry_idx].value = *value;
+ hash->size++;
+
+ if (hash->nentries < THASH_FACTOR_CRITICAL*hash->size) {
+// printf("rehash: \n before: ");
+// TFN(performance)(hash);
+
+ // rehash!
+ TTYPENAME *newhash = TFN(create_capacity)(hash->size + 1);
+
+ for (int entry_idx = 0; entry_idx < hash->nentries; entry_idx++) {
+ if (hash->entries[entry_idx].valid) {
+
+ if (TFN(put)(newhash, &hash->entries[entry_idx].key, &hash->entries[entry_idx].value, NULL, NULL))
+ assert(0); // shouldn't already be present.
+ }
+ }
+
+ // play switch-a-roo. We become 'newhash' and free the old one.
+ TTYPENAME tmp;
+ memcpy(&tmp, hash, sizeof(TTYPENAME));
+ memcpy(hash, newhash, sizeof(TTYPENAME));
+ memcpy(newhash, &tmp, sizeof(TTYPENAME));
+ TFN(destroy)(newhash);
+
+// printf(" after : ");
+// TFN(performance)(hash);
+ }
+
+ return 0;
+}
+
+static inline int TFN(remove)(TTYPENAME *hash, TKEYTYPE *key, TKEYTYPE *oldkey, TVALTYPE *oldvalue)
+{
+ uint32_t code = TKEYHASH(key);
+ uint32_t entry_idx = code & (hash->nentries - 1);
+
+ while (hash->entries[entry_idx].valid) {
+ if (TKEYEQUAL(key, &hash->entries[entry_idx].key)) {
+
+ if (oldkey)
+ *oldkey = hash->entries[entry_idx].key;
+ if (oldvalue)
+ *oldvalue = hash->entries[entry_idx].value;
+
+ hash->entries[entry_idx].valid = 0;
+ hash->size--;
+
+ // re-put following entries
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ while (hash->entries[entry_idx].valid) {
+ TKEYTYPE key = hash->entries[entry_idx].key;
+ TVALTYPE value = hash->entries[entry_idx].value;
+ hash->entries[entry_idx].valid = 0;
+ hash->size--;
+
+ if (TFN(put)(hash, &key, &value, NULL, NULL)) {
+ assert(0);
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ return 0;
+}
+
+static inline TTYPENAME *TFN(copy)(TTYPENAME *hash)
+{
+ TTYPENAME *newhash = TFN(create_capacity)(hash->size);
+
+ for (int entry_idx = 0; entry_idx < hash->nentries; entry_idx++) {
+ if (hash->entries[entry_idx].valid) {
+ if (TFN(put)(newhash, &hash->entries[entry_idx].key, &hash->entries[entry_idx].value, NULL, NULL))
+ assert(0); // shouldn't already be present.
+ }
+ }
+
+ return newhash;
+}
+
+typedef struct TFN(iterator) TFN(iterator_t);
+struct TFN(iterator)
+{
+ TTYPENAME *hash;
+ int last_entry; // points to last entry returned by _next
+};
+
+static inline void TFN(iterator_init)(TTYPENAME *hash, TFN(iterator_t) *iter)
+{
+ iter->hash = hash;
+ iter->last_entry = -1;
+}
+
+static inline int TFN(iterator_next)(TFN(iterator_t) *iter, TKEYTYPE *outkey, TVALTYPE *outval)
+{
+ TTYPENAME *hash = iter->hash;
+
+ while(1) {
+ if (iter->last_entry+1 >= hash->nentries)
+ return 0;
+
+ iter->last_entry++;
+
+ if (hash->entries[iter->last_entry].valid) {
+ if (outkey)
+ *outkey = hash->entries[iter->last_entry].key;
+ if (outval)
+ *outval = hash->entries[iter->last_entry].value;
+ return 1;
+ }
+ }
+}
+
+static inline void TFN(iterator_remove)(TFN(iterator_t) *iter)
+{
+ TTYPENAME *hash = iter->hash;
+
+ hash->entries[iter->last_entry].valid = 0;
+
+ // have to reinsert any consecutive entries that follow.
+ int entry_idx = (iter->last_entry + 1) & (hash->nentries - 1);
+ while (hash->entries[entry_idx].valid) {
+ TKEYTYPE key = hash->entries[entry_idx].key;
+ TVALTYPE value = hash->entries[entry_idx].value;
+ hash->entries[entry_idx].valid = 0;
+ hash->size--;
+
+ if (TFN(put)(hash, &key, &value, NULL, NULL)) {
+ assert(0);
+ }
+
+ entry_idx = (entry_idx + 1) & (hash->nentries - 1);
+ }
+
+ hash->size--;
+}
diff --git a/3rdparty/apriltag/common/time.h b/3rdparty/apriltag/common/time.h
new file mode 100644
index 0000000..1fc7862
--- /dev/null
+++ b/3rdparty/apriltag/common/time.h
@@ -0,0 +1,7 @@
+#pragma once
+
+#ifndef _TIMES_H
+
+#include "sys/times.h"
+
+#endif
diff --git a/3rdparty/apriltag/common/time_util.c b/3rdparty/apriltag/common/time_util.c
new file mode 100644
index 0000000..c2e666e
--- /dev/null
+++ b/3rdparty/apriltag/common/time_util.c
@@ -0,0 +1,192 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include <math.h>
+#include "time_util.h"
+#if defined(_MSC_VER) || defined(__MINGW32__)
+#include <windows.h>
+#endif
+
+#if defined(_MSC_VER) && (_MSC_VER < 1900) && !defined(_CRT_NO_TIME_T)
+struct timespec {
+ time_t tv_sec;
+ long tv_nsec;
+};
+#endif
+
+struct timeutil_rest
+{
+ int64_t acc_time;
+ int64_t start_time;
+};
+
+timeutil_rest_t *timeutil_rest_create()
+{
+ timeutil_rest_t *rest = calloc(1, sizeof(timeutil_rest_t));
+ return rest;
+}
+
+void timeutil_rest_destroy(timeutil_rest_t *rest)
+{
+ free(rest);
+}
+
+int64_t utime_now() // blacklist-ignore
+{
+ struct timeval tv;
+ gettimeofday (&tv, NULL); // blacklist-ignore
+ return (int64_t) tv.tv_sec * 1000000 + tv.tv_usec;
+}
+
+int64_t utime_get_seconds(int64_t v)
+{
+ return v/1000000;
+}
+
+int64_t utime_get_useconds(int64_t v)
+{
+ return v%1000000;
+}
+
+void utime_to_timeval(int64_t v, struct timeval *tv)
+{
+ tv->tv_sec = (time_t) utime_get_seconds(v);
+ tv->tv_usec = (suseconds_t) utime_get_useconds(v);
+}
+
+void utime_to_timespec(int64_t v, struct timespec *ts)
+{
+ ts->tv_sec = (time_t) utime_get_seconds(v);
+ ts->tv_nsec = (suseconds_t) utime_get_useconds(v)*1000;
+}
+
+#if defined(_MSC_VER) && ! defined(WINRT)
+//https://stackoverflow.com/a/17283549/6055233
+void usleep(__int64 usec)
+{
+ HANDLE timer;
+ LARGE_INTEGER ft;
+
+ ft.QuadPart = -(10 * usec); // Convert to 100 nanosecond interval, negative value indicates relative time
+
+ timer = CreateWaitableTimer(NULL, TRUE, NULL);
+ SetWaitableTimer(timer, &ft, 0, NULL, NULL, 0);
+ WaitForSingleObject(timer, INFINITE);
+ CloseHandle(timer);
+}
+#endif
+
+#ifndef WINRT
+int32_t timeutil_usleep(int64_t useconds)
+{
+ // unistd.h function, but usleep is obsoleted in POSIX.1-2008.
+ // TODO: Eventually, rewrite this to use nanosleep
+#ifdef _MSC_VER
+ usleep(useconds);
+ return 0;
+#else
+ return usleep(useconds);
+#endif
+}
+#endif
+
+uint32_t timeutil_sleep(unsigned int seconds)
+{
+ // unistd.h function
+#if defined(_MSC_VER) || defined(__MINGW32__)
+ Sleep(seconds);
+ return 0;
+#else
+ return sleep(seconds);
+#endif
+}
+
+#ifndef WINRT
+int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz)
+{
+ int64_t max_delay = 1000000L/hz;
+ int64_t curr_time = utime_now();
+ int64_t diff = curr_time - rest->start_time;
+ int64_t delay = max_delay - diff;
+ if (delay < 0) delay = 0;
+
+ int32_t ret = timeutil_usleep(delay);
+ rest->start_time = utime_now();
+
+ return ret;
+}
+#endif
+
+void timeutil_timer_reset(timeutil_rest_t *rest)
+{
+ rest->start_time = utime_now();
+ rest->acc_time = 0;
+}
+
+void timeutil_timer_start(timeutil_rest_t *rest)
+{
+ rest->start_time = utime_now();
+}
+
+void timeutil_timer_stop(timeutil_rest_t *rest)
+{
+ int64_t curr_time = utime_now();
+ int64_t diff = curr_time - rest->start_time;
+
+ rest->acc_time += diff;
+}
+
+bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s)
+{
+ int64_t timeout_us = (int64_t)(1000000L*timeout_s);
+ return rest->acc_time > timeout_us;
+}
+
+int64_t time_util_hhmmss_ss_to_utime(double time)
+{
+ int64_t utime = 0;
+
+ int itime = ((int) time);
+
+ double seconds = fmod(time, 100.0);
+ uint8_t minutes = (itime % 10000) / 100;
+ uint8_t hours = itime / 10000;
+
+ utime += seconds * 100;
+ utime += minutes * 6000;
+ utime += hours *360000;
+
+ utime *= 10000;
+
+ return utime;
+}
diff --git a/3rdparty/apriltag/common/time_util.h b/3rdparty/apriltag/common/time_util.h
new file mode 100644
index 0000000..151ff78
--- /dev/null
+++ b/3rdparty/apriltag/common/time_util.h
@@ -0,0 +1,91 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TIME_UTIL_H
+#define _TIME_UTIL_H
+
+#include <stdbool.h>
+#include <stdint.h>
+#if defined(_MSC_VER) || defined(__MINGW32__)
+#include "sys/times.h"
+#include "time.h"
+#else
+#include <sys/time.h>
+#include <time.h>
+#endif
+
+#if defined(__MINGW32__) // to define struct timespec
+#include <pthread.h>
+#endif
+
+#ifdef _MSC_VER
+#include <io.h>
+#else
+#include <unistd.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct timeutil_rest timeutil_rest_t;
+timeutil_rest_t *timeutil_rest_create();
+void timeutil_rest_destroy(timeutil_rest_t * rest);
+
+int64_t utime_now(); // blacklist-ignore
+int64_t utime_get_seconds(int64_t v);
+int64_t utime_get_useconds(int64_t v);
+void utime_to_timeval(int64_t v, struct timeval *tv);
+void utime_to_timespec(int64_t v, struct timespec *ts);
+
+#ifndef WINRT
+int32_t timeutil_usleep(int64_t useconds);
+#endif
+uint32_t timeutil_sleep(unsigned int seconds);
+#ifndef WINRT
+int32_t timeutil_sleep_hz(timeutil_rest_t *rest, double hz);
+#endif
+
+void timeutil_timer_reset(timeutil_rest_t *rest);
+void timeutil_timer_start(timeutil_rest_t *rest);
+void timeutil_timer_stop(timeutil_rest_t *rest);
+bool timeutil_timer_timeout(timeutil_rest_t *rest, double timeout_s);
+
+int64_t time_util_hhmmss_ss_to_utime(double time);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/timeprofile.h b/3rdparty/apriltag/common/timeprofile.h
new file mode 100644
index 0000000..c80d8ce
--- /dev/null
+++ b/3rdparty/apriltag/common/timeprofile.h
@@ -0,0 +1,133 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TIME_PROFILE_H
+#define _TIME_PROFILE_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#if defined(_MSC_VER) || defined(__MINGW32__)
+//#include "time.h" //included in "time_util.h"
+#else
+#include <sys/time.h>
+#endif
+#include <string.h>
+#include <stdint.h>
+
+#include "time_util.h"
+#include "zarray.h"
+
+struct timeprofile_entry
+{
+ char name[32];
+ int64_t utime;
+};
+
+typedef struct timeprofile timeprofile_t;
+struct timeprofile
+{
+ int64_t utime;
+ zarray_t *stamps;
+};
+
+static inline timeprofile_t *timeprofile_create()
+{
+ timeprofile_t *tp = (timeprofile_t*) calloc(1, sizeof(timeprofile_t));
+ tp->stamps = zarray_create(sizeof(struct timeprofile_entry));
+
+ tp->utime = utime_now();
+
+ return tp;
+}
+
+static inline void timeprofile_destroy(timeprofile_t *tp)
+{
+ zarray_destroy(tp->stamps);
+ free(tp);
+}
+
+static inline void timeprofile_clear(timeprofile_t *tp)
+{
+ zarray_clear(tp->stamps);
+ tp->utime = utime_now();
+}
+
+static inline void timeprofile_stamp(timeprofile_t *tp, const char *name)
+{
+ struct timeprofile_entry tpe;
+
+ strncpy(tpe.name, name, sizeof(tpe.name));
+ tpe.name[sizeof(tpe.name)-1] = 0;
+ tpe.utime = utime_now();
+
+ zarray_add(tp->stamps, &tpe);
+}
+
+static inline void timeprofile_display(timeprofile_t *tp)
+{
+ int64_t lastutime = tp->utime;
+
+ for (int i = 0; i < zarray_size(tp->stamps); i++) {
+ struct timeprofile_entry *stamp;
+
+ zarray_get_volatile(tp->stamps, i, &stamp);
+
+ double cumtime = (stamp->utime - tp->utime)/1000000.0;
+
+ double parttime = (stamp->utime - lastutime)/1000000.0;
+
+ printf("%2d %32s %15f ms %15f ms\n", i, stamp->name, parttime*1000, cumtime*1000);
+
+ lastutime = stamp->utime;
+ }
+}
+
+static inline uint64_t timeprofile_total_utime(timeprofile_t *tp)
+{
+ if (zarray_size(tp->stamps) == 0)
+ return 0;
+
+ struct timeprofile_entry *first, *last;
+ zarray_get_volatile(tp->stamps, 0, &first);
+ zarray_get_volatile(tp->stamps, zarray_size(tp->stamps) - 1, &last);
+
+ return last->utime - first->utime;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/times.c b/3rdparty/apriltag/common/times.c
new file mode 100644
index 0000000..6ba170e
--- /dev/null
+++ b/3rdparty/apriltag/common/times.c
@@ -0,0 +1,21 @@
+#include "sys/times.h"
+
+#ifdef _WIN32
+int gettimeofday(struct timeval* t, void* timezone) {
+ struct _timeb timebuffer;
+ _ftime( &timebuffer );
+ t->tv_sec=timebuffer.time;
+ t->tv_usec=1000*timebuffer.millitm;
+ return 0;
+}
+
+clock_t times (struct tms *__buffer) {
+ __buffer->tms_utime = clock();
+ __buffer->tms_stime = 0;
+ __buffer->tms_cstime = 0;
+ __buffer->tms_cutime = 0;
+ return __buffer->tms_utime;
+}
+#else
+void dummy_times() {}
+#endif
diff --git a/3rdparty/apriltag/common/unionfind.c b/3rdparty/apriltag/common/unionfind.c
new file mode 100644
index 0000000..1fcb2e3
--- /dev/null
+++ b/3rdparty/apriltag/common/unionfind.c
@@ -0,0 +1,35 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include "unionfind.h"
+#include <stdlib.h>
+#include <assert.h>
diff --git a/3rdparty/apriltag/common/unionfind.h b/3rdparty/apriltag/common/unionfind.h
new file mode 100644
index 0000000..230cdf1
--- /dev/null
+++ b/3rdparty/apriltag/common/unionfind.h
@@ -0,0 +1,157 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+ Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _UNIONFIND_H
+#define _UNIONFIND_H
+
+#include <stdint.h>
+#include <stdlib.h>
+
+typedef struct unionfind unionfind_t;
+
+struct unionfind
+{
+ uint32_t maxid;
+ struct ufrec *data;
+};
+
+struct ufrec
+{
+ // the parent of this node. If a node's parent is its own index,
+ // then it is a root.
+ uint32_t parent;
+
+ // for the root of a connected component, the number of components
+ // connected to it. For intermediate values, it's not meaningful.
+ uint32_t size;
+};
+
+static inline unionfind_t *unionfind_create(uint32_t maxid)
+{
+ unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t));
+ uf->maxid = maxid;
+ uf->data = (struct ufrec*) malloc((maxid+1) * sizeof(struct ufrec));
+ for (int i = 0; i <= maxid; i++) {
+ uf->data[i].size = 1;
+ uf->data[i].parent = i;
+ }
+ return uf;
+}
+
+static inline void unionfind_destroy(unionfind_t *uf)
+{
+ free(uf->data);
+ free(uf);
+}
+
+/*
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id)
+{
+ // base case: a node is its own parent
+ if (uf->data[id].parent == id)
+ return id;
+
+ // otherwise, recurse
+ uint32_t root = unionfind_get_representative(uf, uf->data[id].parent);
+
+ // short circuit the path. [XXX This write prevents tail recursion]
+ uf->data[id].parent = root;
+
+ return root;
+}
+*/
+
+// this one seems to be every-so-slightly faster than the recursive
+// version above.
+static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id)
+{
+ uint32_t root = id;
+
+ // chase down the root
+ while (uf->data[root].parent != root) {
+ root = uf->data[root].parent;
+ }
+
+ // go back and collapse the tree.
+ //
+ // XXX: on some of our workloads that have very shallow trees
+ // (e.g. image segmentation), we are actually faster not doing
+ // this...
+ while (uf->data[id].parent != root) {
+ uint32_t tmp = uf->data[id].parent;
+ uf->data[id].parent = root;
+ id = tmp;
+ }
+
+ return root;
+}
+
+static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id)
+{
+ uint32_t repid = unionfind_get_representative(uf, id);
+ return uf->data[repid].size;
+}
+
+static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid)
+{
+ uint32_t aroot = unionfind_get_representative(uf, aid);
+ uint32_t broot = unionfind_get_representative(uf, bid);
+
+ if (aroot == broot)
+ return aroot;
+
+ // we don't perform "union by rank", but we perform a similar
+ // operation (but probably without the same asymptotic guarantee):
+ // We join trees based on the number of *elements* (as opposed to
+ // rank) contained within each tree. I.e., we use size as a proxy
+ // for rank. In my testing, it's often *faster* to use size than
+ // rank, perhaps because the rank of the tree isn't that critical
+ // if there are very few nodes in it.
+ uint32_t asize = uf->data[aroot].size;
+ uint32_t bsize = uf->data[broot].size;
+
+ // optimization idea: We could shortcut some or all of the tree
+ // that is grafted onto the other tree. Pro: those nodes were just
+ // read and so are probably in cache. Con: it might end up being
+ // wasted effort -- the tree might be grafted onto another tree in
+ // a moment!
+ if (asize > bsize) {
+ uf->data[broot].parent = aroot;
+ uf->data[aroot].size += bsize;
+ return aroot;
+ } else {
+ uf->data[aroot].parent = broot;
+ uf->data[broot].size += asize;
+ return broot;
+ }
+}
+#endif
diff --git a/3rdparty/apriltag/common/workerpool.c b/3rdparty/apriltag/common/workerpool.c
new file mode 100644
index 0000000..349f1e9
--- /dev/null
+++ b/3rdparty/apriltag/common/workerpool.c
@@ -0,0 +1,292 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#define __USE_GNU
+#include <pthread.h>
+#include <sched.h>
+#include <assert.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <inttypes.h>
+
+#include "workerpool.h"
+#include "timeprofile.h"
+#include "math_util.h"
+#include "string_util.h"
+
+struct workerpool {
+ int nthreads;
+ zarray_t *tasks;
+ int taskspos;
+
+ pthread_t *threads;
+ int *status;
+
+ pthread_mutex_t mutex;
+ pthread_cond_t startcond; // used to signal the availability of work
+ pthread_cond_t endcond; // used to signal completion of all work
+
+ int end_count; // how many threads are done?
+};
+
+struct task
+{
+ void (*f)(void *p);
+ void *p;
+};
+
+void *worker_thread(void *p)
+{
+ workerpool_t *wp = (workerpool_t*) p;
+
+ int cnt = 0;
+
+ while (1) {
+ struct task *task;
+
+ pthread_mutex_lock(&wp->mutex);
+ while (wp->taskspos == zarray_size(wp->tasks)) {
+ wp->end_count++;
+// printf("%"PRId64" thread %d did %d\n", utime_now(), pthread_self(), cnt);
+ pthread_cond_broadcast(&wp->endcond);
+ pthread_cond_wait(&wp->startcond, &wp->mutex);
+ cnt = 0;
+// printf("%"PRId64" thread %d awake\n", utime_now(), pthread_self());
+ }
+
+ zarray_get_volatile(wp->tasks, wp->taskspos, &task);
+ wp->taskspos++;
+ cnt++;
+ pthread_mutex_unlock(&wp->mutex);
+// pthread_yield();
+ sched_yield();
+
+ // we've been asked to exit.
+ if (task->f == NULL)
+ return NULL;
+
+ task->f(task->p);
+ }
+
+ return NULL;
+}
+
+workerpool_t *workerpool_create(int nthreads)
+{
+ assert(nthreads > 0);
+
+ workerpool_t *wp = calloc(1, sizeof(workerpool_t));
+ wp->nthreads = nthreads;
+ wp->tasks = zarray_create(sizeof(struct task));
+
+ if (nthreads > 1) {
+ wp->threads = calloc(wp->nthreads, sizeof(pthread_t));
+
+ pthread_mutex_init(&wp->mutex, NULL);
+ pthread_cond_init(&wp->startcond, NULL);
+ pthread_cond_init(&wp->endcond, NULL);
+
+ for (int i = 0; i < nthreads; i++) {
+ int res = pthread_create(&wp->threads[i], NULL, worker_thread, wp);
+ if (res != 0) {
+ perror("pthread_create");
+ exit(-1);
+ }
+ }
+ }
+
+ return wp;
+}
+
+void workerpool_destroy(workerpool_t *wp)
+{
+ if (wp == NULL)
+ return;
+
+ // force all worker threads to exit.
+ if (wp->nthreads > 1) {
+ for (int i = 0; i < wp->nthreads; i++)
+ workerpool_add_task(wp, NULL, NULL);
+
+ pthread_mutex_lock(&wp->mutex);
+ pthread_cond_broadcast(&wp->startcond);
+ pthread_mutex_unlock(&wp->mutex);
+
+ for (int i = 0; i < wp->nthreads; i++)
+ pthread_join(wp->threads[i], NULL);
+
+ pthread_mutex_destroy(&wp->mutex);
+ pthread_cond_destroy(&wp->startcond);
+ pthread_cond_destroy(&wp->endcond);
+ free(wp->threads);
+ }
+
+ zarray_destroy(wp->tasks);
+ free(wp);
+}
+
+int workerpool_get_nthreads(workerpool_t *wp)
+{
+ return wp->nthreads;
+}
+
+void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p)
+{
+ struct task t;
+ t.f = f;
+ t.p = p;
+
+ zarray_add(wp->tasks, &t);
+}
+
+void workerpool_run_single(workerpool_t *wp)
+{
+ for (int i = 0; i < zarray_size(wp->tasks); i++) {
+ struct task *task;
+ zarray_get_volatile(wp->tasks, i, &task);
+ task->f(task->p);
+ }
+
+ zarray_clear(wp->tasks);
+}
+
+// runs all added tasks, waits for them to complete.
+void workerpool_run(workerpool_t *wp)
+{
+ if (wp->nthreads > 1) {
+ wp->end_count = 0;
+
+ pthread_mutex_lock(&wp->mutex);
+ pthread_cond_broadcast(&wp->startcond);
+
+ while (wp->end_count < wp->nthreads) {
+// printf("caught %d\n", wp->end_count);
+ pthread_cond_wait(&wp->endcond, &wp->mutex);
+ }
+
+ pthread_mutex_unlock(&wp->mutex);
+
+ wp->taskspos = 0;
+
+ zarray_clear(wp->tasks);
+
+ } else {
+ workerpool_run_single(wp);
+ }
+}
+
+#if _WIN32
+// Source: https://stackoverflow.com/a/735472
+///* This code is public domain -- Will Hartung 4/9/09 */
+size_t getline(char **lineptr, size_t *n, FILE *stream) {
+ char *bufptr = NULL;
+ char *p = bufptr;
+ size_t size;
+ int c;
+
+ if (lineptr == NULL) {
+ return -1;
+ }
+ if (stream == NULL) {
+ return -1;
+ }
+ if (n == NULL) {
+ return -1;
+ }
+ bufptr = *lineptr;
+ size = *n;
+
+ c = fgetc(stream);
+ if (c == EOF) {
+ return -1;
+ }
+ if (bufptr == NULL) {
+ bufptr = malloc(128);
+ if (bufptr == NULL) {
+ return -1;
+ }
+ size = 128;
+ }
+ p = bufptr;
+ while(c != EOF) {
+ int offset = p - bufptr;
+ if ((p - bufptr + 1) > size) {
+ size = size + 128;
+ bufptr = realloc(bufptr, size);
+ if (bufptr == NULL) {
+ return -1;
+ }
+ p = bufptr + offset;
+ }
+ *p++ = c;
+ if (c == '\n') {
+ break;
+ }
+ c = fgetc(stream);
+ }
+
+ *p++ = '\0';
+ *lineptr = bufptr;
+ *n = size;
+
+ return p - bufptr - 1;
+}
+#endif
+
+int workerpool_get_nprocs()
+{
+#ifdef _MSC_VER
+ int nproc = 1;
+#else
+ FILE * f = fopen("/proc/cpuinfo", "r");
+ size_t n = 0;
+ char * buf = NULL;
+
+ int nproc = 0;
+
+ while(getline(&buf, &n, f) != -1)
+ {
+ if(!str_starts_with(buf, "processor"))
+ continue;
+
+ int colon = str_indexof(buf, ":");
+
+ int v = atoi(&buf[colon+1]);
+ if (v > nproc)
+ nproc = v;
+ }
+
+ free(buf);
+#endif
+
+ return nproc;
+}
diff --git a/3rdparty/apriltag/common/workerpool.h b/3rdparty/apriltag/common/workerpool.h
new file mode 100644
index 0000000..ac206aa
--- /dev/null
+++ b/3rdparty/apriltag/common/workerpool.h
@@ -0,0 +1,57 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _WORKERPOOL_H
+#define _WORKERPOOL_H
+
+#include "zarray.h"
+
+typedef struct workerpool workerpool_t;
+
+// as a special case, if nthreads==1, no additional threads are
+// created, and workerpool_run will run synchronously.
+workerpool_t *workerpool_create(int nthreads);
+void workerpool_destroy(workerpool_t *wp);
+
+void workerpool_add_task(workerpool_t *wp, void (*f)(void *p), void *p);
+
+// runs all added tasks, waits for them to complete.
+void workerpool_run(workerpool_t *wp);
+
+// same as workerpool_run, except always single threaded. (mostly for debugging).
+void workerpool_run_single(workerpool_t *wp);
+
+int workerpool_get_nthreads(workerpool_t *wp);
+
+int workerpool_get_nprocs();
+
+#endif
diff --git a/3rdparty/apriltag/common/zarray.c b/3rdparty/apriltag/common/zarray.c
new file mode 100644
index 0000000..cffc9eb
--- /dev/null
+++ b/3rdparty/apriltag/common/zarray.c
@@ -0,0 +1,62 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include "zarray.h"
+
+int zstrcmp(const void * a_pp, const void * b_pp)
+{
+ assert(a_pp != NULL);
+ assert(b_pp != NULL);
+
+ char * a = *(void**)a_pp;
+ char * b = *(void**)b_pp;
+
+ return strcmp(a,b);
+}
+
+void zarray_vmap(zarray_t *za, void (*f)())
+{
+ assert(za != NULL);
+ assert(f != NULL);
+ assert(za->el_sz == sizeof(void*));
+
+ for (int idx = 0; idx < za->size; idx++) {
+ void *pp = &za->data[idx*za->el_sz];
+ void *p = *(void**) pp;
+ f(p);
+ }
+}
diff --git a/3rdparty/apriltag/common/zarray.h b/3rdparty/apriltag/common/zarray.h
new file mode 100644
index 0000000..94a750b
--- /dev/null
+++ b/3rdparty/apriltag/common/zarray.h
@@ -0,0 +1,496 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _ZARRAY_H
+#define _ZARRAY_H
+
+#include <stddef.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+/**
+ * Defines a structure which acts as a resize-able array ala Java's ArrayList.
+ */
+typedef struct zarray zarray_t;
+struct zarray
+{
+ size_t el_sz; // size of each element
+
+ int size; // how many elements?
+ int alloc; // we've allocated storage for how many elements?
+ char *data;
+};
+
+/**
+ * Creates and returns a variable array structure capable of holding elements of
+ * the specified size. It is the caller's responsibility to call zarray_destroy()
+ * on the returned array when it is no longer needed.
+ */
+static inline zarray_t *zarray_create(size_t el_sz)
+{
+ assert(el_sz > 0);
+
+ zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t));
+ za->el_sz = el_sz;
+ return za;
+}
+
+/**
+ * Frees all resources associated with the variable array structure which was
+ * created by zarray_create(). After calling, 'za' will no longer be valid for storage.
+ */
+static inline void zarray_destroy(zarray_t *za)
+{
+ if (za == NULL)
+ return;
+
+ if (za->data != NULL)
+ free(za->data);
+ memset(za, 0, sizeof(zarray_t));
+ free(za);
+}
+
+/** Allocate a new zarray that contains a copy of the data in the argument. **/
+static inline zarray_t *zarray_copy(const zarray_t *za)
+{
+ assert(za != NULL);
+
+ zarray_t *zb = (zarray_t*) calloc(1, sizeof(zarray_t));
+ zb->el_sz = za->el_sz;
+ zb->size = za->size;
+ zb->alloc = za->alloc;
+ zb->data = (char*) malloc(zb->alloc * zb->el_sz);
+ memcpy(zb->data, za->data, za->size * za->el_sz);
+ return zb;
+}
+
+static int iceillog2(int v)
+{
+ v--;
+ v |= v >> 1;
+ v |= v >> 2;
+ v |= v >> 4;
+ v |= v >> 8;
+ v |= v >> 16;
+ v++;
+ return v;
+}
+
+/**
+ * Allocate a new zarray that contains a subset of the original
+ * elements. NOTE: end index is EXCLUSIVE, that is one past the last
+ * element you want.
+ */
+static inline zarray_t *zarray_copy_subset(const zarray_t *za,
+ int start_idx,
+ int end_idx_exclusive)
+{
+ zarray_t *out = (zarray_t*) calloc(1, sizeof(zarray_t));
+ out->el_sz = za->el_sz;
+ out->size = end_idx_exclusive - start_idx;
+ out->alloc = iceillog2(out->size); // round up pow 2
+ out->data = (char*) malloc(out->alloc * out->el_sz);
+ memcpy(out->data, za->data +(start_idx*out->el_sz), out->size*out->el_sz);
+ return out;
+}
+
+/**
+ * Retrieves the number of elements currently being contained by the passed
+ * array, which may be different from its capacity. The index of the last element
+ * in the array will be one less than the returned value.
+ */
+static inline int zarray_size(const zarray_t *za)
+{
+ assert(za != NULL);
+
+ return za->size;
+}
+
+/**
+ * Returns 1 if zarray_size(za) == 0,
+ * returns 0 otherwise.
+ */
+/*
+JUST CALL zarray_size
+int zarray_isempty(const zarray_t *za)
+{
+ assert(za != NULL);
+ if (za->size <= 0)
+ return 1;
+ else
+ return 0;
+}
+*/
+
+
+/**
+ * Allocates enough internal storage in the supplied variable array structure to
+ * guarantee that the supplied number of elements (capacity) can be safely stored.
+ */
+static inline void zarray_ensure_capacity(zarray_t *za, int capacity)
+{
+ assert(za != NULL);
+
+ if (capacity <= za->alloc)
+ return;
+
+ while (za->alloc < capacity) {
+ za->alloc *= 2;
+ if (za->alloc < 8)
+ za->alloc = 8;
+ }
+
+ za->data = (char*) realloc(za->data, za->alloc * za->el_sz);
+}
+
+/**
+ * Adds a new element to the end of the supplied array, and sets its value
+ * (by copying) from the data pointed to by the supplied pointer 'p'.
+ * Automatically ensures that enough storage space is available for the new element.
+ */
+static inline void zarray_add(zarray_t *za, const void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+
+ zarray_ensure_capacity(za, za->size + 1);
+
+ memcpy(&za->data[za->size*za->el_sz], p, za->el_sz);
+ za->size++;
+}
+
+/**
+ * Retrieves the element from the supplied array located at the zero-based
+ * index of 'idx' and copies its value into the variable pointed to by the pointer
+ * 'p'.
+ */
+static inline void zarray_get(const zarray_t *za, int idx, void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+ assert(idx >= 0);
+ assert(idx < za->size);
+
+ memcpy(p, &za->data[idx*za->el_sz], za->el_sz);
+}
+
+/**
+ * Similar to zarray_get(), but returns a "live" pointer to the internal
+ * storage, avoiding a memcpy. This pointer is not valid across
+ * operations which might move memory around (i.e. zarray_remove_value(),
+ * zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()).
+ * 'p' should be a pointer to the pointer which will be set to the internal address.
+ */
+inline static void zarray_get_volatile(const zarray_t *za, int idx, void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+ assert(idx >= 0);
+ assert(idx < za->size);
+
+ *((void**) p) = &za->data[idx*za->el_sz];
+}
+
+inline static void zarray_truncate(zarray_t *za, int sz)
+{
+ assert(za != NULL);
+ assert(sz <= za->size);
+ za->size = sz;
+}
+
+/**
+ * Copies the memory array used internally by zarray to store its owned
+ * elements to the address pointed by 'buffer'. It is the caller's responsibility
+ * to allocate zarray_size()*el_sz bytes for the copy to be stored and
+ * to free the memory when no longer needed. The memory allocated at 'buffer'
+ * and the internal zarray storage must not overlap. 'buffer_bytes' should be
+ * the size of the 'buffer' memory space, in bytes, and must be at least
+ * zarray_size()*el_sz.
+ *
+ * Returns the number of bytes copied into 'buffer'.
+ */
+static inline size_t zarray_copy_data(const zarray_t *za, void *buffer, size_t buffer_bytes)
+{
+ assert(za != NULL);
+ assert(buffer != NULL);
+ assert(buffer_bytes >= za->el_sz * za->size);
+ memcpy(buffer, za->data, za->el_sz * za->size);
+ return za->el_sz * za->size;
+}
+
+/**
+ * Removes the entry at index 'idx'.
+ * If shuffle is true, the last element in the array will be placed in
+ * the newly-open space; if false, the zarray is compacted.
+ */
+static inline void zarray_remove_index(zarray_t *za, int idx, int shuffle)
+{
+ assert(za != NULL);
+ assert(idx >= 0);
+ assert(idx < za->size);
+
+ if (shuffle) {
+ if (idx < za->size-1)
+ memcpy(&za->data[idx*za->el_sz], &za->data[(za->size-1)*za->el_sz], za->el_sz);
+ za->size--;
+ return;
+ } else {
+ // size = 10, idx = 7. Should copy 2 entries (at idx=8 and idx=9).
+ // size = 10, idx = 9. Should copy 0 entries.
+ int ncopy = za->size - idx - 1;
+ if (ncopy > 0)
+ memmove(&za->data[idx*za->el_sz], &za->data[(idx+1)*za->el_sz], ncopy*za->el_sz);
+ za->size--;
+ return;
+ }
+}
+
+/**
+ * Remove the entry whose value is equal to the value pointed to by 'p'.
+ * If shuffle is true, the last element in the array will be placed in
+ * the newly-open space; if false, the zarray is compacted. At most
+ * one element will be removed.
+ *
+ * Note that objects will be compared using memcmp over the full size
+ * of the value. If the value is a struct that contains padding,
+ * differences in the padding bytes can cause comparisons to
+ * fail. Thus, it remains best practice to bzero all structs so that
+ * the padding is set to zero.
+ *
+ * Returns the number of elements removed (0 or 1).
+ */
+// remove the entry whose value is equal to the value pointed to by p.
+// if shuffle is true, the last element in the array will be placed in
+// the newly-open space; if false, the zarray is compacted.
+static inline int zarray_remove_value(zarray_t *za, const void *p, int shuffle)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+
+ for (int idx = 0; idx < za->size; idx++) {
+ if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) {
+ zarray_remove_index(za, idx, shuffle);
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+
+/**
+ * Creates a new entry and inserts it into the array so that it will have the
+ * index 'idx' (i.e. before the item which currently has that index). The value
+ * of the new entry is set to (copied from) the data pointed to by 'p'. 'idx'
+ * can be one larger than the current max index to place the new item at the end
+ * of the array, or zero to add it to an empty array.
+ */
+static inline void zarray_insert(zarray_t *za, int idx, const void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+ assert(idx >= 0);
+ assert(idx <= za->size);
+
+ zarray_ensure_capacity(za, za->size + 1);
+ // size = 10, idx = 7. Should copy three entries (idx=7, idx=8, idx=9)
+ int ncopy = za->size - idx;
+
+ memmove(&za->data[(idx+1)*za->el_sz], &za->data[idx*za->el_sz], ncopy*za->el_sz);
+ memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
+
+ za->size++;
+}
+
+
+/**
+ * Sets the value of the current element at index 'idx' by copying its value from
+ * the data pointed to by 'p'. The previous value of the changed element will be
+ * copied into the data pointed to by 'outp' if it is not null.
+ */
+static inline void zarray_set(zarray_t *za, int idx, const void *p, void *outp)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+ assert(idx >= 0);
+ assert(idx < za->size);
+
+ if (outp != NULL)
+ memcpy(outp, &za->data[idx*za->el_sz], za->el_sz);
+
+ memcpy(&za->data[idx*za->el_sz], p, za->el_sz);
+}
+
+/**
+ * Calls the supplied function for every element in the array in index order.
+ * The map function will be passed a pointer to each element in turn and must
+ * have the following format:
+ *
+ * void map_function(element_type *element)
+ */
+static inline void zarray_map(zarray_t *za, void (*f)(void*))
+{
+ assert(za != NULL);
+ assert(f != NULL);
+
+ for (int idx = 0; idx < za->size; idx++)
+ f(&za->data[idx*za->el_sz]);
+}
+
+/**
+ * Calls the supplied function for every element in the array in index order.
+ * HOWEVER values are passed to the function, not pointers to values. In the
+ * case where the zarray stores object pointers, zarray_vmap allows you to
+ * pass in the object's destroy function (or free) directly. Can only be used
+ * with zarray's which contain pointer data. The map function should have the
+ * following format:
+ *
+ * void map_function(element_type *element)
+ */
+ void zarray_vmap(zarray_t *za, void (*f)());
+
+/**
+ * Removes all elements from the array and sets its size to zero. Pointers to
+ * any data elements obtained i.e. by zarray_get_volatile() will no longer be
+ * valid.
+ */
+static inline void zarray_clear(zarray_t *za)
+{
+ assert(za != NULL);
+ za->size = 0;
+}
+
+/**
+ * Determines whether any element in the array has a value which matches the
+ * data pointed to by 'p'.
+ *
+ * Returns 1 if a match was found anywhere in the array, else 0.
+ */
+static inline int zarray_contains(const zarray_t *za, const void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+
+ for (int idx = 0; idx < za->size; idx++) {
+ if (!memcmp(p, &za->data[idx*za->el_sz], za->el_sz)) {
+ return 1;
+ }
+ }
+
+ return 0;
+}
+
+/**
+ * Uses qsort() to sort the elements contained by the array in ascending order.
+ * Uses the supplied comparison function to determine the appropriate order.
+ *
+ * The comparison function will be passed a pointer to two elements to be compared
+ * and should return a measure of the difference between them (see strcmp()).
+ * I.e. it should return a negative number if the first element is 'less than'
+ * the second, zero if they are equivalent, and a positive number if the first
+ * element is 'greater than' the second. The function should have the following format:
+ *
+ * int comparison_function(const element_type *first, const element_type *second)
+ *
+ * zstrcmp() can be used as the comparison function for string elements, which
+ * will call strcmp() internally.
+ */
+static inline void zarray_sort(zarray_t *za, int (*compar)(const void*, const void*))
+{
+ assert(za != NULL);
+ assert(compar != NULL);
+ if (za->size == 0)
+ return;
+
+ qsort(za->data, za->size, za->el_sz, compar);
+}
+
+/**
+ * A comparison function for comparing strings which can be used by zarray_sort()
+ * to sort arrays with char* elements.
+ */
+ int zstrcmp(const void * a_pp, const void * b_pp);
+
+/**
+ * Find the index of an element, or return -1 if not found. Remember that p is
+ * a pointer to the element.
+ **/
+// returns -1 if not in array. Remember p is a pointer to the item.
+static inline int zarray_index_of(const zarray_t *za, const void *p)
+{
+ assert(za != NULL);
+ assert(p != NULL);
+
+ for (int i = 0; i < za->size; i++) {
+ if (!memcmp(p, &za->data[i*za->el_sz], za->el_sz))
+ return i;
+ }
+
+ return -1;
+}
+
+
+
+/**
+ * Add all elements from 'source' into 'dest'. el_size must be the same
+ * for both lists
+ **/
+static inline void zarray_add_all(zarray_t * dest, const zarray_t * source)
+{
+ assert(dest->el_sz == source->el_sz);
+
+ // Don't allocate on stack because el_sz could be larger than ~8 MB
+ // stack size
+ char *tmp = (char*)calloc(1, dest->el_sz);
+
+ for (int i = 0; i < zarray_size(source); i++) {
+ zarray_get(source, i, tmp);
+ zarray_add(dest, tmp);
+ }
+
+ free(tmp);
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/zhash.c b/3rdparty/apriltag/common/zhash.c
new file mode 100644
index 0000000..693ff01
--- /dev/null
+++ b/3rdparty/apriltag/common/zhash.c
@@ -0,0 +1,577 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include "zhash.h"
+
+// force a rehash when our capacity is less than this many times the size
+#define ZHASH_FACTOR_CRITICAL 2
+
+// When resizing, how much bigger do we want to be? (should be greater than _CRITICAL)
+#define ZHASH_FACTOR_REALLOC 4
+
+struct zhash
+{
+ size_t keysz, valuesz;
+ int entrysz; // valid byte (1) + keysz + values
+
+ uint32_t(*hash)(const void *a);
+
+ // returns 1 if equal
+ int(*equals)(const void *a, const void *b);
+
+ int size; // # of items in hash table
+
+ char *entries; // each entry of size entrysz;
+ int nentries; // how many entries are allocated? Never 0.
+};
+
+zhash_t *zhash_create_capacity(size_t keysz, size_t valuesz,
+ uint32_t(*hash)(const void *a), int(*equals)(const void *a, const void*b),
+ int capacity)
+{
+ assert(hash != NULL);
+ assert(equals != NULL);
+
+ // resize...
+ int _nentries = ZHASH_FACTOR_REALLOC * capacity;
+ if (_nentries < 8)
+ _nentries = 8;
+
+ // to a power of 2.
+ int nentries = _nentries;
+ if ((nentries & (nentries - 1)) != 0) {
+ nentries = 8;
+ while (nentries < _nentries)
+ nentries *= 2;
+ }
+
+ zhash_t *zh = (zhash_t*) calloc(1, sizeof(zhash_t));
+ zh->keysz = keysz;
+ zh->valuesz = valuesz;
+ zh->hash = hash;
+ zh->equals = equals;
+ zh->nentries = nentries;
+
+ zh->entrysz = 1 + zh->keysz + zh->valuesz;
+
+ zh->entries = calloc(zh->nentries, zh->entrysz);
+ zh->nentries = nentries;
+
+ return zh;
+}
+
+zhash_t *zhash_create(size_t keysz, size_t valuesz,
+ uint32_t(*hash)(const void *a), int(*equals)(const void *a, const void *b))
+{
+ return zhash_create_capacity(keysz, valuesz, hash, equals, 8);
+}
+
+void zhash_destroy(zhash_t *zh)
+{
+ if (zh == NULL)
+ return;
+
+ free(zh->entries);
+ free(zh);
+}
+
+int zhash_size(const zhash_t *zh)
+{
+ return zh->size;
+}
+
+void zhash_clear(zhash_t *zh)
+{
+ memset(zh->entries, 0, zh->nentries * zh->entrysz);
+ zh->size = 0;
+}
+
+int zhash_get_volatile(const zhash_t *zh, const void *key, void *out_value)
+{
+ uint32_t code = zh->hash(key);
+ uint32_t entry_idx = code & (zh->nentries - 1);
+
+ while (zh->entries[entry_idx * zh->entrysz]) {
+ void *this_key = &zh->entries[entry_idx * zh->entrysz + 1];
+ if (zh->equals(key, this_key)) {
+ *((void**) out_value) = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz];
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (zh->nentries - 1);
+ }
+
+ return 0;
+}
+
+int zhash_get(const zhash_t *zh, const void *key, void *out_value)
+{
+ void *tmp;
+ if (zhash_get_volatile(zh, key, &tmp)) {
+ memcpy(out_value, tmp, zh->valuesz);
+ return 1;
+ }
+
+ return 0;
+}
+
+int zhash_put(zhash_t *zh, const void *key, const void *value, void *oldkey, void *oldvalue)
+{
+ uint32_t code = zh->hash(key);
+ uint32_t entry_idx = code & (zh->nentries - 1);
+
+ while (zh->entries[entry_idx * zh->entrysz]) {
+ void *this_key = &zh->entries[entry_idx * zh->entrysz + 1];
+ void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz];
+
+ if (zh->equals(key, this_key)) {
+ // replace
+ if (oldkey)
+ memcpy(oldkey, this_key, zh->keysz);
+ if (oldvalue)
+ memcpy(oldvalue, this_value, zh->valuesz);
+ memcpy(this_key, key, zh->keysz);
+ memcpy(this_value, value, zh->valuesz);
+ zh->entries[entry_idx * zh->entrysz] = 1; // mark valid
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (zh->nentries - 1);
+ }
+
+ // add the entry
+ zh->entries[entry_idx * zh->entrysz] = 1;
+ memcpy(&zh->entries[entry_idx * zh->entrysz + 1], key, zh->keysz);
+ memcpy(&zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz], value, zh->valuesz);
+ zh->size++;
+
+ if (zh->nentries < ZHASH_FACTOR_CRITICAL * zh->size) {
+ zhash_t *newhash = zhash_create_capacity(zh->keysz, zh->valuesz,
+ zh->hash, zh->equals,
+ zh->size);
+
+ for (int entry_idx = 0; entry_idx < zh->nentries; entry_idx++) {
+
+ if (zh->entries[entry_idx * zh->entrysz]) {
+ void *this_key = &zh->entries[entry_idx * zh->entrysz + 1];
+ void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz];
+ if (zhash_put(newhash, this_key, this_value, NULL, NULL))
+ assert(0); // shouldn't already be present.
+ }
+ }
+
+ // play switch-a-roo
+ zhash_t tmp;
+ memcpy(&tmp, zh, sizeof(zhash_t));
+ memcpy(zh, newhash, sizeof(zhash_t));
+ memcpy(newhash, &tmp, sizeof(zhash_t));
+ zhash_destroy(newhash);
+ }
+
+ return 0;
+}
+
+int zhash_remove(zhash_t *zh, const void *key, void *old_key, void *old_value)
+{
+ uint32_t code = zh->hash(key);
+ uint32_t entry_idx = code & (zh->nentries - 1);
+
+ while (zh->entries[entry_idx * zh->entrysz]) {
+ void *this_key = &zh->entries[entry_idx * zh->entrysz + 1];
+ void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz];
+
+ if (zh->equals(key, this_key)) {
+ if (old_key)
+ memcpy(old_key, this_key, zh->keysz);
+ if (old_value)
+ memcpy(old_value, this_value, zh->valuesz);
+
+ // mark this entry as available
+ zh->entries[entry_idx * zh->entrysz] = 0;
+ zh->size--;
+
+ // reinsert any consecutive entries that follow
+ while (1) {
+ entry_idx = (entry_idx + 1) & (zh->nentries - 1);
+
+ if (zh->entries[entry_idx * zh->entrysz]) {
+ // completely remove this entry
+#ifdef _MSC_VER
+ char *tmp = malloc(zh->entrysz*sizeof *tmp);
+#else
+ char tmp[zh->entrysz];
+#endif
+ memcpy(tmp, &zh->entries[entry_idx * zh->entrysz], zh->entrysz);
+ zh->entries[entry_idx * zh->entrysz] = 0;
+ zh->size--;
+ // reinsert it
+ if (zhash_put(zh, &tmp[1], &tmp[1+zh->keysz], NULL, NULL))
+ assert(0);
+
+#ifdef _MSC_VER
+ free(tmp);
+#endif
+ } else {
+ break;
+ }
+ }
+ return 1;
+ }
+
+ entry_idx = (entry_idx + 1) & (zh->nentries - 1);
+ }
+
+ return 0;
+}
+
+zhash_t *zhash_copy(const zhash_t *zh)
+{
+ zhash_t *newhash = zhash_create_capacity(zh->keysz, zh->valuesz,
+ zh->hash, zh->equals,
+ zh->size);
+
+ for (int entry_idx = 0; entry_idx < zh->nentries; entry_idx++) {
+ if (zh->entries[entry_idx * zh->entrysz]) {
+ void *this_key = &zh->entries[entry_idx * zh->entrysz + 1];
+ void *this_value = &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz];
+ if (zhash_put(newhash, this_key, this_value, NULL, NULL))
+ assert(0); // shouldn't already be present.
+ }
+ }
+
+ return newhash;
+}
+
+int zhash_contains(const zhash_t *zh, const void *key)
+{
+ void *tmp;
+ return zhash_get_volatile(zh, key, &tmp);
+}
+
+void zhash_iterator_init(zhash_t *zh, zhash_iterator_t *zit)
+{
+ zit->zh = zh;
+ zit->czh = zh;
+ zit->last_entry = -1;
+}
+
+void zhash_iterator_init_const(const zhash_t *zh, zhash_iterator_t *zit)
+{
+ zit->zh = NULL;
+ zit->czh = zh;
+ zit->last_entry = -1;
+}
+
+int zhash_iterator_next_volatile(zhash_iterator_t *zit, void *outkey, void *outvalue)
+{
+ const zhash_t *zh = zit->czh;
+
+ while (1) {
+ if (zit->last_entry + 1 >= zh->nentries)
+ return 0;
+
+ zit->last_entry++;
+
+ if (zh->entries[zit->last_entry * zh->entrysz]) {
+ void *this_key = &zh->entries[zit->last_entry * zh->entrysz + 1];
+ void *this_value = &zh->entries[zit->last_entry * zh->entrysz + 1 + zh->keysz];
+
+ if (outkey != NULL)
+ *((void**) outkey) = this_key;
+ if (outvalue != NULL)
+ *((void**) outvalue) = this_value;
+
+ return 1;
+ }
+ }
+}
+
+int zhash_iterator_next(zhash_iterator_t *zit, void *outkey, void *outvalue)
+{
+ const zhash_t *zh = zit->czh;
+
+ void *outkeyp, *outvaluep;
+
+ if (!zhash_iterator_next_volatile(zit, &outkeyp, &outvaluep))
+ return 0;
+
+ if (outkey != NULL)
+ memcpy(outkey, outkeyp, zh->keysz);
+ if (outvalue != NULL)
+ memcpy(outvalue, outvaluep, zh->valuesz);
+
+ return 1;
+}
+
+void zhash_iterator_remove(zhash_iterator_t *zit)
+{
+ assert(zit->zh); // can't call _remove on a iterator with const zhash
+ zhash_t *zh = zit->zh;
+
+ zh->entries[zit->last_entry * zh->entrysz] = 0;
+ zh->size--;
+
+ // re-insert following entries
+ int entry_idx = (zit->last_entry + 1) & (zh->nentries - 1);
+ while (zh->entries[entry_idx *zh->entrysz]) {
+ // completely remove this entry
+#ifdef _MSC_VER
+ char *tmp = malloc(zh->entrysz*sizeof *tmp);
+#else
+ char tmp[zh->entrysz];
+#endif
+ memcpy(tmp, &zh->entries[entry_idx * zh->entrysz], zh->entrysz);
+ zh->entries[entry_idx * zh->entrysz] = 0;
+ zh->size--;
+
+ // reinsert it
+ if (zhash_put(zh, &tmp[1], &tmp[1+zh->keysz], NULL, NULL))
+ assert(0);
+
+ entry_idx = (entry_idx + 1) & (zh->nentries - 1);
+ }
+
+ zit->last_entry--;
+}
+
+void zhash_map_keys(zhash_t *zh, void (*f)())
+{
+ assert(zh != NULL);
+ if (f == NULL)
+ return;
+
+ zhash_iterator_t itr;
+ zhash_iterator_init(zh, &itr);
+
+ void *key, *value;
+
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ f(key);
+ }
+}
+
+void zhash_vmap_keys(zhash_t * zh, void (*f)())
+{
+ assert(zh != NULL);
+ if (f == NULL)
+ return;
+
+ zhash_iterator_t itr;
+ zhash_iterator_init(zh, &itr);
+
+ void *key, *value;
+
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ void *p = *(void**) key;
+ f(p);
+ }
+}
+
+void zhash_map_values(zhash_t * zh, void (*f)())
+{
+ assert(zh != NULL);
+ if (f == NULL)
+ return;
+
+ zhash_iterator_t itr;
+ zhash_iterator_init(zh, &itr);
+
+ void *key, *value;
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ f(value);
+ }
+}
+
+void zhash_vmap_values(zhash_t * zh, void (*f)())
+{
+ assert(zh != NULL);
+ if (f == NULL)
+ return;
+
+ zhash_iterator_t itr;
+ zhash_iterator_init(zh, &itr);
+
+ void *key, *value;
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ void *p = *(void**) value;
+ f(p);
+ }
+}
+
+zarray_t *zhash_keys(const zhash_t *zh)
+{
+ assert(zh != NULL);
+
+ zarray_t *za = zarray_create(zh->keysz);
+
+ zhash_iterator_t itr;
+ zhash_iterator_init_const(zh, &itr);
+
+ void *key, *value;
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ zarray_add(za, key);
+ }
+
+ return za;
+}
+
+zarray_t *zhash_values(const zhash_t *zh)
+{
+ assert(zh != NULL);
+
+ zarray_t *za = zarray_create(zh->valuesz);
+
+ zhash_iterator_t itr;
+ zhash_iterator_init_const(zh, &itr);
+
+ void *key, *value;
+ while(zhash_iterator_next_volatile(&itr, &key, &value)) {
+ zarray_add(za, value);
+ }
+
+ return za;
+}
+
+
+uint32_t zhash_uint32_hash(const void *_a)
+{
+ assert(_a != NULL);
+
+ uint32_t a = *((uint32_t*) _a);
+ return a;
+}
+
+int zhash_uint32_equals(const void *_a, const void *_b)
+{
+ assert(_a != NULL);
+ assert(_b != NULL);
+
+ uint32_t a = *((uint32_t*) _a);
+ uint32_t b = *((uint32_t*) _b);
+
+ return a==b;
+}
+
+uint32_t zhash_uint64_hash(const void *_a)
+{
+ assert(_a != NULL);
+
+ uint64_t a = *((uint64_t*) _a);
+ return (uint32_t) (a ^ (a >> 32));
+}
+
+int zhash_uint64_equals(const void *_a, const void *_b)
+{
+ assert(_a != NULL);
+ assert(_b != NULL);
+
+ uint64_t a = *((uint64_t*) _a);
+ uint64_t b = *((uint64_t*) _b);
+
+ return a==b;
+}
+
+
+union uintpointer
+{
+ const void *p;
+ uint32_t i;
+};
+
+uint32_t zhash_ptr_hash(const void *a)
+{
+ assert(a != NULL);
+
+ union uintpointer ip;
+ ip.p = * (void**)a;
+
+ // compute a hash from the lower 32 bits of the pointer (on LE systems)
+ uint32_t hash = ip.i;
+ hash ^= (hash >> 7);
+
+ return hash;
+}
+
+
+int zhash_ptr_equals(const void *a, const void *b)
+{
+ assert(a != NULL);
+ assert(b != NULL);
+
+ const void * ptra = * (void**)a;
+ const void * ptrb = * (void**)b;
+ return ptra == ptrb;
+}
+
+
+int zhash_str_equals(const void *_a, const void *_b)
+{
+ assert(_a != NULL);
+ assert(_b != NULL);
+
+ char *a = * (char**)_a;
+ char *b = * (char**)_b;
+
+ return !strcmp(a, b);
+}
+
+uint32_t zhash_str_hash(const void *_a)
+{
+ assert(_a != NULL);
+
+ char *a = * (char**)_a;
+
+ int32_t hash = 0;
+ while (*a != 0) {
+ hash = (hash << 7) + (hash >> 23);
+ hash += *a;
+ a++;
+ }
+
+ return (uint32_t) hash;
+}
+
+
+void zhash_debug(zhash_t *zh)
+{
+ for (int entry_idx = 0; entry_idx < zh->nentries; entry_idx++) {
+ char *k, *v;
+ memcpy(&k, &zh->entries[entry_idx * zh->entrysz + 1], sizeof(char*));
+ memcpy(&v, &zh->entries[entry_idx * zh->entrysz + 1 + zh->keysz], sizeof(char*));
+ printf("%d: %d, %s => %s\n", entry_idx, zh->entries[entry_idx * zh->entrysz], k, v);
+ }
+}
diff --git a/3rdparty/apriltag/common/zhash.h b/3rdparty/apriltag/common/zhash.h
new file mode 100644
index 0000000..170adfc
--- /dev/null
+++ b/3rdparty/apriltag/common/zhash.h
@@ -0,0 +1,440 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _ZHASH_H
+#define _ZHASH_H
+
+#include <stdint.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "zarray.h"
+
+
+/**
+ * A hash table for structs and primitive types that stores entries by value.
+ * - The size of the key/values must be known at instantiation time, and remain fixed.
+ * e.g. for pointers: zhash_create(sizeof(void*), sizeof(void*)....)
+ * for structs: zhash_create(sizeof(struct key_struct), sizeof(struct value_struct)...)
+ * for bytes: zhash_create(sizeof(uint8_t), sizeof(uint8_t)...)
+ * - Entries are copied by value. This means you must always pass a reference to the start
+ * of 'key_size' and 'value_size' bytes, which you have already malloc'd or stack allocated
+ * - This data structure can be used to store types of any size, from bytes & doubles to
+ * user defined structs
+ * Note: if zhash stores pointers, user must be careful to manually manage the lifetime
+ * of the memory they point to.
+ *
+ */
+
+typedef struct zhash zhash_t;
+
+// The contents of the iterator should be considered private. However,
+// since our usage model prefers stack-based allocation of iterators,
+// we must publicly declare them.
+struct zhash_iterator
+{
+ zhash_t *zh;
+ const zhash_t *czh;
+ int last_entry; // points to the last entry returned by _next
+};
+
+typedef struct zhash_iterator zhash_iterator_t;
+
+/**
+ * Create, initializes, and returns an empty hash table structure. It is the
+ * caller's responsibility to call zhash_destroy() on the returned array when it
+ * is no longer needed.
+ *
+ * The size of values used in the hash and equals function must match 'keysz'.
+ * I.e. if keysz = sizeof(uint64_t), then hash() and equals() should accept
+ * parameters as *uint64_t.
+ */
+zhash_t *zhash_create(size_t keysz, size_t valuesz,
+ uint32_t(*hash)(const void *a),
+ int(*equals)(const void *a, const void *b));
+
+/**
+ * Frees all resources associated with the hash table structure which was
+ * created by zhash_create(). After calling, 'zh' will no longer be valid for storage.
+ *
+ * If 'zh' contains pointer data, it is the caller's responsibility to manage
+ * the resources pointed to by those pointers.
+ */
+void zhash_destroy(zhash_t *zh);
+
+/**
+ * Creates and returns a new identical copy of the zhash (i.e. a "shallow" copy).
+ * If you're storing pointers, be sure not to double free their pointees!
+ * It is the caller's responsibility to call zhash_destroy() on the returned array
+ * when it is no longer needed (in addition to the zhash_destroy() call for the
+ * original zhash).
+ */
+zhash_t * zhash_copy(const zhash_t* other);
+
+/**
+ * Determines whether the supplied key value exists as an entry in the zhash
+ * table. If zhash stores pointer types as keys, this function can differentiate
+ * between a non-existent key and a key mapped to NULL.
+ * Returns 1 if the supplied key exists in the zhash table, else 0.
+ */
+int zhash_contains(const zhash_t *zh, const void *key);
+
+/**
+ * Retrieves the value for the given key, if it exists, by copying its contents
+ * into the space pointed to by 'out_value', which must already be allocated.
+ * Returns 1 if the supplied key exists in the table, else 0, in which case
+ * the contents of 'out_value' will be unchanged.
+ */
+int zhash_get(const zhash_t *zh, const void *key, void *out_value);
+
+/**
+ * Similar to zhash_get(), but more dangerous. Provides a pointer to the zhash's
+ * internal storage. This can be used to make simple modifications to
+ * the underlying data while avoiding the memcpys associated with
+ * zhash_get and zhash_put. However, some zhash operations (that
+ * resize the underlying storage, in particular) render this pointer
+ * invalid. For maximum safety, call no other zhash functions for the
+ * period during which you intend to use the pointer.
+ * 'out_p' should be a pointer to the pointer which will be set to the internal
+ * data address.
+ */
+int zhash_get_volatile(const zhash_t *zh, const void *key, void *out_p);
+
+/**
+ * Adds a key/value pair to the hash table, if the supplied key does not already
+ * exist in the table, or overwrites the value for the supplied key if it does
+ * already exist. In the latter case, the previous contents of the key and value
+ * will be copied into the spaces pointed to by 'oldkey' and 'oldvalue', respectively,
+ * if they are not NULL.
+ *
+ * The key/value is added to / updated in the hash table by copying 'keysz' bytes
+ * from the data pointed to by 'key' and 'valuesz' bytes from the data pointed
+ * to by 'value'. It is up to the caller to manage the memory allocation of the
+ * passed-in values, zhash will store and manage a copy.
+ *
+ * NOTE: If the key is a pointer type (such as a string), the contents of the
+ * data that it points to must not be modified after the call to zhash_put(),
+ * or future zhash calls will not successfully locate the key (using either its
+ * previous or new value).
+ *
+ * NOTE: When using array data as a key (such as a string), the array should not
+ * be passed directly or it will cause a segmentation fault when it is dereferenced.
+ * Instead, pass a pointer which points to the array location, i.e.:
+ * char key[strlen];
+ * char *keyptr = key;
+ * zhash_put(zh, &keyptr, ...)
+ *
+ * Example:
+ * char * key = ...;
+ * zarray_t * val = ...;
+ * char * old_key = NULL;
+ * zarray_t * old_val = NULL;
+ * if (zhash_put(zh, &key, &val, &old_key, &old_value))
+ * // manage resources for old_key and old_value
+ *
+ * Returns 1 if the supplied key previously existed in the table, else 0, in
+ * which case the data pointed to by 'oldkey' and 'oldvalue' will be set to zero
+ * if they are not NULL.
+ */
+int zhash_put(zhash_t *zh, const void *key, const void *value, void *oldkey, void *oldvalue);
+
+/**
+ * Removes from the zhash table the key/value pair for the supplied key, if
+ * it exists. If it does, the contents of the key and value will be copied into
+ * the spaces pointed to by 'oldkey' and 'oldvalue', respectively, if they are
+ * not NULL. If the key does not exist, the data pointed to by 'oldkey' and
+ * 'oldvalue' will be set to zero if they are not NULL.
+ *
+ * Returns 1 if the key existed and was removed, else 0, indicating that the
+ * table contents were not changed.
+ */
+int zhash_remove(zhash_t *zh, const void *key, void *oldkey, void *oldvalue);
+
+/**
+ * Removes all entries in the has table to create the equivalent of starting from
+ * a zhash_create(), using the same size parameters. If any elements need to be
+ * freed manually, this will need to occur before calling clear.
+ */
+void zhash_clear(zhash_t *zh);
+
+/**
+ * Retrieves the current number of key/value pairs currently contained in the
+ * zhash table, or 0 if the table is empty.
+ */
+int zhash_size(const zhash_t *zh);
+
+/**
+ * Initializes an iterator which can be used to traverse the key/value pairs of
+ * the supplied zhash table via successive calls to zhash_iterator_next() or
+ * zhash_iterator_next_volatile(). The iterator can also be used to remove elements
+ * from the zhash with zhash_iterator_remove().
+ *
+ * Any modifications to the zhash table structure will invalidate the
+ * iterator, with the exception of zhash_iterator_remove().
+ */
+void zhash_iterator_init(zhash_t *zh, zhash_iterator_t *zit);
+
+/**
+ * Initializes an iterator which can be used to traverse the key/value pairs of
+ * the supplied zhash table via successive calls to zhash_iterator_next() or
+ * zhash_iterator_next_volatile().
+ *
+ * An iterator initialized with this function cannot be used with
+ * zhash_iterator_remove(). For that you must use zhash_iterator_init().
+ *
+ * Any modifications to the zhash table structure will invalidate the
+ * iterator.
+ */
+void zhash_iterator_init_const(const zhash_t *zh, zhash_iterator_t *zit);
+
+/**
+ * Retrieves the next key/value pair from a zhash table via the (previously-
+ * initialized) iterator. Copies the key and value data into the space
+ * pointed to by outkey and outvalue, respectively, if they are not NULL.
+ *
+ * Returns 1 if the call retrieved the next available key/value pair, else 0
+ * indicating that no entries remain, in which case the contents of outkey and
+ * outvalue will remain unchanged.
+ */
+int zhash_iterator_next(zhash_iterator_t *zit, void *outkey, void *outvalue);
+
+/**
+ * Similar to zhash_iterator_next() except that it retrieves a pointer to zhash's
+ * internal storage. This can be used to avoid the memcpys associated with
+ * zhash_iterator_next(). Call no other zhash functions for the
+ * period during which you intend to use the pointer.
+ * 'outkey' and 'outvalue' should be pointers to the pointers which will be set
+ * to the internal data addresses.
+ *
+ * Example:
+ * key_t *outkey;
+ * value_t *outvalue;
+ * if (zhash_iterator_next_volatile(&zit, &outkey, &outvalue))
+ * // access internal key and value storage via outkey and outvalue
+ *
+ * Returns 1 if the call retrieved the next available key/value pair, else 0
+ * indicating that no entries remain, in which case the pointers outkey and
+ * outvalue will remain unchanged.
+ */
+int zhash_iterator_next_volatile(zhash_iterator_t *zit, void *outkey, void *outvalue);
+
+/**
+ * Removes from the zhash table the key/value pair most recently returned via
+ * a call to zhash_iterator_next() or zhash_iterator_next_volatile() for the
+ * supplied iterator.
+ *
+ * Requires that the iterator was initialized with zhash_iterator_init(),
+ * not zhash_iterator_init_const().
+ */
+void zhash_iterator_remove(zhash_iterator_t *zit);
+
+/**
+ * Calls the supplied function with a pointer to every key in the hash table in
+ * turn. The function will be passed a pointer to the table's internal storage
+ * for the key, which the caller should not modify, as the hash table will not be
+ * re-indexed. The function may be NULL, in which case no action is taken.
+ */
+void zhash_map_keys(zhash_t *zh, void (*f)());
+
+/**
+ * Calls the supplied function with a pointer to every value in the hash table in
+ * turn. The function will be passed a pointer to the table's internal storage
+ * for the value, which the caller may safely modify. The function may be NULL,
+ * in which case no action is taken.
+ */
+void zhash_map_values(zhash_t *zh, void (*f)());
+
+/**
+ * Calls the supplied function with a copy of every key in the hash table in
+ * turn. While zhash_map_keys() passes a pointer to internal storage, this function
+ * passes a copy of the actual storage. If the zhash stores pointers to data,
+ * functions like free() can be used directly with zhash_vmap_keys().
+ * The function may be NULL, in which case no action is taken.
+ *
+ * NOTE: zhash_vmap_keys() can only be used with pointer-data keys.
+ * Use with non-pointer keys (i.e. integer, double, etc.) will likely cause a
+ * segmentation fault.
+ */
+void zhash_vmap_keys(zhash_t *vh, void (*f)());
+
+/**
+ * Calls the supplied function with a copy of every value in the hash table in
+ * turn. While zhash_map_values() passes a pointer to internal storage, this function
+ * passes a copy of the actual storage. If the zhash stores pointers to data,
+ * functions like free() can be used directly with zhash_vmap_values().
+ * The function may be NULL, in which case no action is taken.
+ *
+ * NOTE: zhash_vmap_values() can only be used with pointer-data values.
+ * Use with non-pointer values (i.e. integer, double, etc.) will likely cause a
+ * segmentation fault.
+ */
+void zhash_vmap_values(zhash_t *vh, void (*f)());
+
+/**
+ * Returns an array which contains copies of all of the hash table's keys, in no
+ * particular order. It is the caller's responsibility to call zarray_destroy()
+ * on the returned structure when it is no longer needed.
+ */
+zarray_t *zhash_keys(const zhash_t *zh);
+
+/**
+ * Returns an array which contains copies of all of the hash table's values, in no
+ * particular order. It is the caller's responsibility to call zarray_destroy()
+ * on the returned structure when it is no longer needed.
+ */
+zarray_t *zhash_values(const zhash_t *zh);
+
+/**
+ * Defines a hash function which will calculate a zhash value for uint32_t input
+ * data. Can be used with zhash_create() for a key size of sizeof(uint32_t).
+ */
+uint32_t zhash_uint32_hash(const void *a);
+
+/**
+ * Defines a function to compare zhash values for uint32_t input data.
+ * Can be used with zhash_create() for a key size of sizeof(uint32_t).
+ */
+int zhash_uint32_equals(const void *a, const void *b);
+
+/**
+ * Defines a hash function which will calculate a zhash value for uint64_t input
+ * data. Can be used with zhash_create() for a key size of sizeof(uint64_t).
+ */
+uint32_t zhash_uint64_hash(const void *a);
+
+/**
+ * Defines a function to compare zhash values for uint64_t input data.
+ * Can be used with zhash_create() for a key size of sizeof(uint64_t).
+ */
+int zhash_uint64_equals(const void *a, const void *b);
+
+/////////////////////////////////////////////////////
+// functions for keys that can be compared via their pointers.
+/**
+ * Defines a hash function which will calculate a zhash value for pointer input
+ * data. Can be used with zhash_create() for a key size of sizeof(void*). Will
+ * use only the pointer value itself for computing the hash value.
+ */
+uint32_t zhash_ptr_hash(const void *a);
+
+/**
+ * Defines a function to compare zhash values for pointer input data.
+ * Can be used with zhash_create() for a key size of sizeof(void*).
+ */
+int zhash_ptr_equals(const void *a, const void *b);
+
+/////////////////////////////////////////////////////
+// Functions for string-typed keys
+/**
+ * Defines a hash function which will calculate a zhash value for string input
+ * data. Can be used with zhash_create() for a key size of sizeof(char*). Will
+ * use the contents of the string in computing the hash value.
+ */
+uint32_t zhash_str_hash(const void *a);
+
+/**
+ * Defines a function to compare zhash values for string input data.
+ * Can be used with zhash_create() for a key size of sizeof(char*).
+ */
+int zhash_str_equals(const void *a, const void *b);
+
+void zhash_debug(zhash_t *zh);
+
+ static inline zhash_t *zhash_str_str_create(void)
+ {
+ return zhash_create(sizeof(char*), sizeof(char*),
+ zhash_str_hash, zhash_str_equals);
+ }
+
+
+
+// for zhashes that map strings to strings, this is a convenience
+// function that allows easier retrieval of values. NULL is returned
+// if the key is not found.
+static inline char *zhash_str_str_get(zhash_t *zh, const char *key)
+{
+ char *value;
+ if (zhash_get(zh, &key, &value))
+ return value;
+ return NULL;
+}
+
+ static inline void zhash_str_str_put(zhash_t *zh, char *key, char *value)
+ {
+ char *oldkey, *oldval;
+ if (zhash_put(zh, &key, &value, &oldkey, &oldval)) {
+ free(oldkey);
+ free(oldval);
+ }
+ }
+
+ static inline void zhash_str_str_destroy(zhash_t *zh)
+ {
+ zhash_iterator_t zit;
+ zhash_iterator_init(zh, &zit);
+
+ char *key, *value;
+ while (zhash_iterator_next(&zit, &key, &value)) {
+ free(key);
+ free(value);
+ }
+
+ zhash_destroy(zh);
+ }
+
+
+static inline uint32_t zhash_int_hash(const void *_a)
+{
+ assert(_a != NULL);
+
+ uint32_t a = *((int*) _a);
+ return a;
+}
+
+static inline int zhash_int_equals(const void *_a, const void *_b)
+{
+ assert(_a != NULL);
+ assert(_b != NULL);
+
+ int a = *((int*) _a);
+ int b = *((int*) _b);
+
+ return a==b;
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/common/zmaxheap.c b/3rdparty/apriltag/common/zmaxheap.c
new file mode 100644
index 0000000..70b2b1c
--- /dev/null
+++ b/3rdparty/apriltag/common/zmaxheap.c
@@ -0,0 +1,436 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+#include <stdint.h>
+
+#include "zmaxheap.h"
+
+#if defined(_MSC_VER)
+#define inline __inline
+#endif
+
+#if defined(__MINGW32__) || defined(_MSC_VER)
+#define srandom srand
+#define random rand
+#endif
+
+// 0
+// 1 2
+// 3 4 5 6
+// 7 8 9 10 11 12 13 14
+//
+// Children of node i: 2*i+1, 2*i+2
+// Parent of node i: (i-1) / 2
+//
+// Heap property: a parent is greater than (or equal to) its children.
+
+#define MIN_CAPACITY 16
+
+struct zmaxheap
+{
+ size_t el_sz;
+
+ int size;
+ int alloc;
+
+ float *values;
+ char *data;
+
+ void (*swap)(zmaxheap_t *heap, int a, int b);
+};
+
+static inline void swap_default(zmaxheap_t *heap, int a, int b)
+{
+ float t = heap->values[a];
+ heap->values[a] = heap->values[b];
+ heap->values[b] = t;
+
+#ifdef _MSC_VER
+ char *tmp = malloc(heap->el_sz*sizeof *tmp);
+#else
+ char tmp[heap->el_sz];
+#endif
+ memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz);
+ memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz);
+ memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz);
+
+#ifdef _MSC_VER
+ free(tmp);
+#endif
+}
+
+static inline void swap_pointer(zmaxheap_t *heap, int a, int b)
+{
+ float t = heap->values[a];
+ heap->values[a] = heap->values[b];
+ heap->values[b] = t;
+
+ void **pp = (void**) heap->data;
+ void *tmp = pp[a];
+ pp[a] = pp[b];
+ pp[b] = tmp;
+}
+
+
+zmaxheap_t *zmaxheap_create(size_t el_sz)
+{
+ zmaxheap_t *heap = calloc(1, sizeof(zmaxheap_t));
+ heap->el_sz = el_sz;
+
+ heap->swap = swap_default;
+
+ if (el_sz == sizeof(void*))
+ heap->swap = swap_pointer;
+
+ return heap;
+}
+
+void zmaxheap_destroy(zmaxheap_t *heap)
+{
+ free(heap->values);
+ free(heap->data);
+ memset(heap, 0, sizeof(zmaxheap_t));
+ free(heap);
+}
+
+int zmaxheap_size(zmaxheap_t *heap)
+{
+ return heap->size;
+}
+
+void zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity)
+{
+ if (heap->alloc >= capacity)
+ return;
+
+ int newcap = heap->alloc;
+
+ while (newcap < capacity) {
+ if (newcap < MIN_CAPACITY) {
+ newcap = MIN_CAPACITY;
+ continue;
+ }
+
+ newcap *= 2;
+ }
+
+ heap->values = realloc(heap->values, newcap * sizeof(float));
+ heap->data = realloc(heap->data, newcap * heap->el_sz);
+ heap->alloc = newcap;
+}
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v)
+{
+
+ assert (isfinite(v) && "zmaxheap_add: Trying to add non-finite number to heap. NaN's prohibited, could allow INF with testing");
+ zmaxheap_ensure_capacity(heap, heap->size + 1);
+
+ int idx = heap->size;
+
+ heap->values[idx] = v;
+ memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz);
+
+ heap->size++;
+
+ while (idx > 0) {
+
+ int parent = (idx - 1) / 2;
+
+ // we're done!
+ if (heap->values[parent] >= v)
+ break;
+
+ // else, swap and recurse upwards.
+ heap->swap(heap, idx, parent);
+ idx = parent;
+ }
+}
+
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)())
+{
+ assert(heap != NULL);
+ assert(f != NULL);
+ assert(heap->el_sz == sizeof(void*));
+
+ for (int idx = 0; idx < heap->size; idx++) {
+ void *p = NULL;
+ memcpy(&p, &heap->data[idx*heap->el_sz], heap->el_sz);
+ if (p == NULL) {
+ printf("Warning: zmaxheap_vmap item %d is NULL\n", idx);
+ fflush(stdout);
+ }
+ f(p);
+ }
+}
+
+// Removes the item in the heap at the given index. Returns 1 if the
+// item existed. 0 Indicates an invalid idx (heap is smaller than
+// idx). This is mostly intended to be used by zmaxheap_remove_max.
+int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v)
+{
+ if (idx >= heap->size)
+ return 0;
+
+ // copy out the requested element from the heap.
+ if (v != NULL)
+ *v = heap->values[idx];
+ if (p != NULL)
+ memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz);
+
+ heap->size--;
+
+ // If this element is already the last one, then there's nothing
+ // for us to do.
+ if (idx == heap->size)
+ return 1;
+
+ // copy last element to first element. (which probably upsets
+ // the heap property).
+ heap->values[idx] = heap->values[heap->size];
+ memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz);
+
+ // now fix the heap. Note, as we descend, we're "pushing down"
+ // the same node the entire time. Thus, while the index of the
+ // parent might change, the parent_score doesn't.
+ int parent = idx;
+ float parent_score = heap->values[idx];
+
+ // descend, fixing the heap.
+ while (parent < heap->size) {
+
+ int left = 2*parent + 1;
+ int right = left + 1;
+
+// assert(parent_score == heap->values[parent]);
+
+ float left_score = (left < heap->size) ? heap->values[left] : -INFINITY;
+ float right_score = (right < heap->size) ? heap->values[right] : -INFINITY;
+
+ // put the biggest of (parent, left, right) as the parent.
+
+ // already okay?
+ if (parent_score >= left_score && parent_score >= right_score)
+ break;
+
+ // if we got here, then one of the children is bigger than the parent.
+ if (left_score >= right_score) {
+ assert(left < heap->size);
+ heap->swap(heap, parent, left);
+ parent = left;
+ } else {
+ // right_score can't be less than left_score if right_score is -INFINITY.
+ assert(right < heap->size);
+ heap->swap(heap, parent, right);
+ parent = right;
+ }
+ }
+
+ return 1;
+}
+
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v)
+{
+ return zmaxheap_remove_index(heap, 0, p, v);
+}
+
+void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it)
+{
+ memset(it, 0, sizeof(zmaxheap_iterator_t));
+ it->heap = heap;
+ it->in = 0;
+ it->out = 0;
+}
+
+int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v)
+{
+ zmaxheap_t *heap = it->heap;
+
+ if (it->in >= zmaxheap_size(heap))
+ return 0;
+
+ *v = heap->values[it->in];
+ memcpy(p, &heap->data[it->in*heap->el_sz], heap->el_sz);
+
+ if (it->in != it->out) {
+ heap->values[it->out] = heap->values[it->in];
+ memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz);
+ }
+
+ it->in++;
+ it->out++;
+ return 1;
+}
+
+int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v)
+{
+ zmaxheap_t *heap = it->heap;
+
+ if (it->in >= zmaxheap_size(heap))
+ return 0;
+
+ *v = heap->values[it->in];
+ *((void**) p) = &heap->data[it->in*heap->el_sz];
+
+ if (it->in != it->out) {
+ heap->values[it->out] = heap->values[it->in];
+ memcpy(&heap->data[it->out*heap->el_sz], &heap->data[it->in*heap->el_sz], heap->el_sz);
+ }
+
+ it->in++;
+ it->out++;
+ return 1;
+}
+
+void zmaxheap_iterator_remove(zmaxheap_iterator_t *it)
+{
+ it->out--;
+}
+
+static void maxheapify(zmaxheap_t *heap, int parent)
+{
+ int left = 2*parent + 1;
+ int right = 2*parent + 2;
+
+ int betterchild = parent;
+
+ if (left < heap->size && heap->values[left] > heap->values[betterchild])
+ betterchild = left;
+ if (right < heap->size && heap->values[right] > heap->values[betterchild])
+ betterchild = right;
+
+ if (betterchild != parent) {
+ heap->swap(heap, parent, betterchild);
+ return maxheapify(heap, betterchild);
+ }
+}
+
+#if 0 //won't compile if defined but not used
+// test the heap property
+static void validate(zmaxheap_t *heap)
+{
+ for (int parent = 0; parent < heap->size; parent++) {
+ int left = 2*parent + 1;
+ int right = 2*parent + 2;
+
+ if (left < heap->size) {
+ assert(heap->values[parent] > heap->values[left]);
+ }
+
+ if (right < heap->size) {
+ assert(heap->values[parent] > heap->values[right]);
+ }
+ }
+}
+#endif
+void zmaxheap_iterator_finish(zmaxheap_iterator_t *it)
+{
+ // if nothing was removed, no work to do.
+ if (it->in == it->out)
+ return;
+
+ zmaxheap_t *heap = it->heap;
+
+ heap->size = it->out;
+
+ // restore heap property
+ for (int i = heap->size/2 - 1; i >= 0; i--)
+ maxheapify(heap, i);
+}
+
+void zmaxheap_test()
+{
+ int cap = 10000;
+ int sz = 0;
+ int32_t *vals = calloc(sizeof(int32_t), cap);
+
+ zmaxheap_t *heap = zmaxheap_create(sizeof(int32_t));
+
+ int maxsz = 0;
+ int zcnt = 0;
+
+ for (int iter = 0; iter < 5000000; iter++) {
+ assert(sz == heap->size);
+
+ if ((random() & 1) == 0 && sz < cap) {
+ // add a value
+ int32_t v = (int32_t) (random() / 1000);
+ float fv = v;
+ assert(v == fv);
+
+ vals[sz] = v;
+ zmaxheap_add(heap, &v, fv);
+ sz++;
+
+// printf("add %d %f\n", v, fv);
+ } else {
+ // remove a value
+ int maxv = -1, maxi = -1;
+
+ for (int i = 0; i < sz; i++) {
+ if (vals[i] > maxv) {
+ maxv = vals[i];
+ maxi = i;
+ }
+ }
+
+
+ int32_t outv;
+ float outfv;
+ int res = zmaxheap_remove_max(heap, &outv, &outfv);
+ if (sz == 0) {
+ assert(res == 0);
+ } else {
+// printf("%d %d %d %f\n", sz, maxv, outv, outfv);
+ assert(outv == outfv);
+ assert(maxv == outv);
+
+ // shuffle erase the maximum from our list.
+ vals[maxi] = vals[sz - 1];
+ sz--;
+ }
+ }
+
+ if (sz > maxsz)
+ maxsz = sz;
+
+ if (maxsz > 0 && sz == 0)
+ zcnt++;
+ }
+
+ printf("max size: %d, zcount %d\n", maxsz, zcnt);
+ free (vals);
+}
diff --git a/3rdparty/apriltag/common/zmaxheap.h b/3rdparty/apriltag/common/zmaxheap.h
new file mode 100644
index 0000000..7c537ca
--- /dev/null
+++ b/3rdparty/apriltag/common/zmaxheap.h
@@ -0,0 +1,84 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _ZMAXHEAP_H
+#define _ZMAXHEAP_H
+
+#include <stdio.h>
+
+typedef struct zmaxheap zmaxheap_t;
+
+typedef struct zmaxheap_iterator zmaxheap_iterator_t;
+struct zmaxheap_iterator {
+ zmaxheap_t *heap;
+ int in, out;
+};
+
+zmaxheap_t *zmaxheap_create(size_t el_sz);
+
+void zmaxheap_vmap(zmaxheap_t *heap, void (*f)());
+
+void zmaxheap_destroy(zmaxheap_t *heap);
+
+void zmaxheap_add(zmaxheap_t *heap, void *p, float v);
+
+int zmaxheap_size(zmaxheap_t *heap);
+
+// returns 0 if the heap is empty, so you can do
+// while (zmaxheap_remove_max(...)) { }
+int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v);
+
+////////////////////////////////////////////
+// This is a peculiar iterator intended to support very specific (and
+// unusual) applications, and the heap is not necessarily in a valid
+// state until zmaxheap_iterator_finish is called. Consequently, do
+// not call any other methods on the heap while iterating through.
+
+// you must provide your own storage for the iterator, and pass in a
+// pointer.
+void zmaxheap_iterator_init(zmaxheap_t *heap, zmaxheap_iterator_t *it);
+
+// Traverses the heap in top-down/left-right order. makes a copy of
+// the content into memory (p) that you provide.
+int zmaxheap_iterator_next(zmaxheap_iterator_t *it, void *p, float *v);
+
+// will set p to be a pointer to the heap's internal copy of the dfata.
+int zmaxheap_iterator_next_volatile(zmaxheap_iterator_t *it, void *p, float *v);
+
+// remove the current element.
+void zmaxheap_iterator_remove(zmaxheap_iterator_t *it);
+
+// call after all iterator operations are done. After calling this,
+// the iterator should no longer be used, but the heap methods can be.
+void zmaxheap_iterator_finish(zmaxheap_iterator_t *it);
+
+#endif
diff --git a/3rdparty/apriltag/tag16h5.c b/3rdparty/apriltag/tag16h5.c
new file mode 100644
index 0000000..0654fb0
--- /dev/null
+++ b/3rdparty/apriltag/tag16h5.c
@@ -0,0 +1,90 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag16h5_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("tag16h5");
+#else
+ tf->name = strdup("tag16h5");
+#endif
+ tf->black_border = 1;
+ tf->d = 4;
+ tf->h = 5;
+ tf->ncodes = 30;
+ tf->codes = calloc(30, sizeof(uint64_t));
+ tf->codes[0] = 0x000000000000231bUL;
+ tf->codes[1] = 0x0000000000002ea5UL;
+ tf->codes[2] = 0x000000000000346aUL;
+ tf->codes[3] = 0x00000000000045b9UL;
+ tf->codes[4] = 0x00000000000079a6UL;
+ tf->codes[5] = 0x0000000000007f6bUL;
+ tf->codes[6] = 0x000000000000b358UL;
+ tf->codes[7] = 0x000000000000e745UL;
+ tf->codes[8] = 0x000000000000fe59UL;
+ tf->codes[9] = 0x000000000000156dUL;
+ tf->codes[10] = 0x000000000000380bUL;
+ tf->codes[11] = 0x000000000000f0abUL;
+ tf->codes[12] = 0x0000000000000d84UL;
+ tf->codes[13] = 0x0000000000004736UL;
+ tf->codes[14] = 0x0000000000008c72UL;
+ tf->codes[15] = 0x000000000000af10UL;
+ tf->codes[16] = 0x000000000000093cUL;
+ tf->codes[17] = 0x00000000000093b4UL;
+ tf->codes[18] = 0x000000000000a503UL;
+ tf->codes[19] = 0x000000000000468fUL;
+ tf->codes[20] = 0x000000000000e137UL;
+ tf->codes[21] = 0x0000000000005795UL;
+ tf->codes[22] = 0x000000000000df42UL;
+ tf->codes[23] = 0x0000000000001c1dUL;
+ tf->codes[24] = 0x000000000000e9dcUL;
+ tf->codes[25] = 0x00000000000073adUL;
+ tf->codes[26] = 0x000000000000ad5fUL;
+ tf->codes[27] = 0x000000000000d530UL;
+ tf->codes[28] = 0x00000000000007caUL;
+ tf->codes[29] = 0x000000000000af2eUL;
+ return tf;
+}
+
+void tag16h5_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag16h5.h b/3rdparty/apriltag/tag16h5.h
new file mode 100644
index 0000000..4d2065a
--- /dev/null
+++ b/3rdparty/apriltag/tag16h5.h
@@ -0,0 +1,50 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TAG16H5
+#define _TAG16H5
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag16h5_create();
+void tag16h5_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/tag25h7.c b/3rdparty/apriltag/tag25h7.c
new file mode 100644
index 0000000..9f5a383
--- /dev/null
+++ b/3rdparty/apriltag/tag25h7.c
@@ -0,0 +1,302 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag25h7_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("tag25h7");
+#else
+ tf->name = strdup("tag25h7");
+#endif
+ tf->black_border = 1;
+ tf->d = 5;
+ tf->h = 7;
+ tf->ncodes = 242;
+ tf->codes = calloc(242, sizeof(uint64_t));
+ tf->codes[0] = 0x00000000004b770dUL;
+ tf->codes[1] = 0x00000000011693e6UL;
+ tf->codes[2] = 0x0000000001a599abUL;
+ tf->codes[3] = 0x0000000000c3a535UL;
+ tf->codes[4] = 0x000000000152aafaUL;
+ tf->codes[5] = 0x0000000000accd98UL;
+ tf->codes[6] = 0x0000000001cad922UL;
+ tf->codes[7] = 0x00000000002c2fadUL;
+ tf->codes[8] = 0x0000000000bb3572UL;
+ tf->codes[9] = 0x00000000014a3b37UL;
+ tf->codes[10] = 0x000000000186524bUL;
+ tf->codes[11] = 0x0000000000c99d4cUL;
+ tf->codes[12] = 0x000000000023bfeaUL;
+ tf->codes[13] = 0x000000000141cb74UL;
+ tf->codes[14] = 0x0000000001d0d139UL;
+ tf->codes[15] = 0x0000000001670aebUL;
+ tf->codes[16] = 0x0000000000851675UL;
+ tf->codes[17] = 0x000000000150334eUL;
+ tf->codes[18] = 0x00000000006e3ed8UL;
+ tf->codes[19] = 0x0000000000fd449dUL;
+ tf->codes[20] = 0x0000000000aa55ecUL;
+ tf->codes[21] = 0x0000000001c86176UL;
+ tf->codes[22] = 0x00000000015e9b28UL;
+ tf->codes[23] = 0x00000000007ca6b2UL;
+ tf->codes[24] = 0x000000000147c38bUL;
+ tf->codes[25] = 0x0000000001d6c950UL;
+ tf->codes[26] = 0x00000000008b0e8cUL;
+ tf->codes[27] = 0x00000000011a1451UL;
+ tf->codes[28] = 0x0000000001562b65UL;
+ tf->codes[29] = 0x00000000013f53c8UL;
+ tf->codes[30] = 0x0000000000d58d7aUL;
+ tf->codes[31] = 0x0000000000829ec9UL;
+ tf->codes[32] = 0x0000000000faccf1UL;
+ tf->codes[33] = 0x000000000136e405UL;
+ tf->codes[34] = 0x00000000007a2f06UL;
+ tf->codes[35] = 0x00000000010934cbUL;
+ tf->codes[36] = 0x00000000016a8b56UL;
+ tf->codes[37] = 0x0000000001a6a26aUL;
+ tf->codes[38] = 0x0000000000f85545UL;
+ tf->codes[39] = 0x000000000195c2e4UL;
+ tf->codes[40] = 0x000000000024c8a9UL;
+ tf->codes[41] = 0x00000000012bfc96UL;
+ tf->codes[42] = 0x00000000016813aaUL;
+ tf->codes[43] = 0x0000000001a42abeUL;
+ tf->codes[44] = 0x0000000001573424UL;
+ tf->codes[45] = 0x0000000001044573UL;
+ tf->codes[46] = 0x0000000000b156c2UL;
+ tf->codes[47] = 0x00000000005e6811UL;
+ tf->codes[48] = 0x0000000001659bfeUL;
+ tf->codes[49] = 0x0000000001d55a63UL;
+ tf->codes[50] = 0x00000000005bf065UL;
+ tf->codes[51] = 0x0000000000e28667UL;
+ tf->codes[52] = 0x0000000001e9ba54UL;
+ tf->codes[53] = 0x00000000017d7c5aUL;
+ tf->codes[54] = 0x0000000001f5aa82UL;
+ tf->codes[55] = 0x0000000001a2bbd1UL;
+ tf->codes[56] = 0x00000000001ae9f9UL;
+ tf->codes[57] = 0x0000000001259e51UL;
+ tf->codes[58] = 0x000000000134062bUL;
+ tf->codes[59] = 0x0000000000e1177aUL;
+ tf->codes[60] = 0x0000000000ed07a8UL;
+ tf->codes[61] = 0x000000000162be24UL;
+ tf->codes[62] = 0x000000000059128bUL;
+ tf->codes[63] = 0x0000000001663e8fUL;
+ tf->codes[64] = 0x00000000001a83cbUL;
+ tf->codes[65] = 0x000000000045bb59UL;
+ tf->codes[66] = 0x000000000189065aUL;
+ tf->codes[67] = 0x00000000004bb370UL;
+ tf->codes[68] = 0x00000000016fb711UL;
+ tf->codes[69] = 0x000000000122c077UL;
+ tf->codes[70] = 0x0000000000eca17aUL;
+ tf->codes[71] = 0x0000000000dbc1f4UL;
+ tf->codes[72] = 0x000000000088d343UL;
+ tf->codes[73] = 0x000000000058ac5dUL;
+ tf->codes[74] = 0x0000000000ba02e8UL;
+ tf->codes[75] = 0x00000000001a1d9dUL;
+ tf->codes[76] = 0x0000000001c72eecUL;
+ tf->codes[77] = 0x0000000000924bc5UL;
+ tf->codes[78] = 0x0000000000dccab3UL;
+ tf->codes[79] = 0x0000000000886d15UL;
+ tf->codes[80] = 0x000000000178c965UL;
+ tf->codes[81] = 0x00000000005bc69aUL;
+ tf->codes[82] = 0x0000000001716261UL;
+ tf->codes[83] = 0x000000000174e2ccUL;
+ tf->codes[84] = 0x0000000001ed10f4UL;
+ tf->codes[85] = 0x0000000000156aa8UL;
+ tf->codes[86] = 0x00000000003e2a8aUL;
+ tf->codes[87] = 0x00000000002752edUL;
+ tf->codes[88] = 0x000000000153c651UL;
+ tf->codes[89] = 0x0000000001741670UL;
+ tf->codes[90] = 0x0000000000765b05UL;
+ tf->codes[91] = 0x000000000119c0bbUL;
+ tf->codes[92] = 0x000000000172a783UL;
+ tf->codes[93] = 0x00000000004faca1UL;
+ tf->codes[94] = 0x0000000000f31257UL;
+ tf->codes[95] = 0x00000000012441fcUL;
+ tf->codes[96] = 0x00000000000d3748UL;
+ tf->codes[97] = 0x0000000000c21f15UL;
+ tf->codes[98] = 0x0000000000ac5037UL;
+ tf->codes[99] = 0x000000000180e592UL;
+ tf->codes[100] = 0x00000000007d3210UL;
+ tf->codes[101] = 0x0000000000a27187UL;
+ tf->codes[102] = 0x00000000002beeafUL;
+ tf->codes[103] = 0x000000000026ff57UL;
+ tf->codes[104] = 0x0000000000690e82UL;
+ tf->codes[105] = 0x000000000077765cUL;
+ tf->codes[106] = 0x0000000001a9e1d7UL;
+ tf->codes[107] = 0x000000000140be1aUL;
+ tf->codes[108] = 0x0000000001aa1e3aUL;
+ tf->codes[109] = 0x0000000001944f5cUL;
+ tf->codes[110] = 0x00000000019b5032UL;
+ tf->codes[111] = 0x0000000000169897UL;
+ tf->codes[112] = 0x0000000001068eb9UL;
+ tf->codes[113] = 0x0000000000f30dbcUL;
+ tf->codes[114] = 0x000000000106a151UL;
+ tf->codes[115] = 0x0000000001d53e95UL;
+ tf->codes[116] = 0x0000000001348ceeUL;
+ tf->codes[117] = 0x0000000000cf4fcaUL;
+ tf->codes[118] = 0x0000000001728bb5UL;
+ tf->codes[119] = 0x0000000000dc1eecUL;
+ tf->codes[120] = 0x000000000069e8dbUL;
+ tf->codes[121] = 0x00000000016e1523UL;
+ tf->codes[122] = 0x000000000105fa25UL;
+ tf->codes[123] = 0x00000000018abb0cUL;
+ tf->codes[124] = 0x0000000000c4275dUL;
+ tf->codes[125] = 0x00000000006d8e76UL;
+ tf->codes[126] = 0x0000000000e8d6dbUL;
+ tf->codes[127] = 0x0000000000e16fd7UL;
+ tf->codes[128] = 0x0000000001ac2682UL;
+ tf->codes[129] = 0x000000000077435bUL;
+ tf->codes[130] = 0x0000000000a359ddUL;
+ tf->codes[131] = 0x00000000003a9c4eUL;
+ tf->codes[132] = 0x000000000123919aUL;
+ tf->codes[133] = 0x0000000001e25817UL;
+ tf->codes[134] = 0x000000000002a836UL;
+ tf->codes[135] = 0x00000000001545a4UL;
+ tf->codes[136] = 0x0000000001209c8dUL;
+ tf->codes[137] = 0x0000000000bb5f69UL;
+ tf->codes[138] = 0x0000000001dc1f02UL;
+ tf->codes[139] = 0x00000000005d5f7eUL;
+ tf->codes[140] = 0x00000000012d0581UL;
+ tf->codes[141] = 0x00000000013786c2UL;
+ tf->codes[142] = 0x0000000000e15409UL;
+ tf->codes[143] = 0x0000000001aa3599UL;
+ tf->codes[144] = 0x000000000139aad8UL;
+ tf->codes[145] = 0x0000000000b09d2aUL;
+ tf->codes[146] = 0x000000000054488fUL;
+ tf->codes[147] = 0x00000000013c351cUL;
+ tf->codes[148] = 0x0000000000976079UL;
+ tf->codes[149] = 0x0000000000b25b12UL;
+ tf->codes[150] = 0x0000000001addb34UL;
+ tf->codes[151] = 0x0000000001cb23aeUL;
+ tf->codes[152] = 0x0000000001175738UL;
+ tf->codes[153] = 0x0000000001303bb8UL;
+ tf->codes[154] = 0x0000000000d47716UL;
+ tf->codes[155] = 0x000000000188ceeaUL;
+ tf->codes[156] = 0x0000000000baf967UL;
+ tf->codes[157] = 0x0000000001226d39UL;
+ tf->codes[158] = 0x000000000135e99bUL;
+ tf->codes[159] = 0x000000000034adc5UL;
+ tf->codes[160] = 0x00000000002e384dUL;
+ tf->codes[161] = 0x000000000090d3faUL;
+ tf->codes[162] = 0x0000000000232713UL;
+ tf->codes[163] = 0x00000000017d49b1UL;
+ tf->codes[164] = 0x0000000000aa84d6UL;
+ tf->codes[165] = 0x0000000000c2ddf8UL;
+ tf->codes[166] = 0x0000000001665646UL;
+ tf->codes[167] = 0x00000000004f345fUL;
+ tf->codes[168] = 0x00000000002276b1UL;
+ tf->codes[169] = 0x0000000001255dd7UL;
+ tf->codes[170] = 0x00000000016f4cccUL;
+ tf->codes[171] = 0x00000000004aaffcUL;
+ tf->codes[172] = 0x0000000000c46da6UL;
+ tf->codes[173] = 0x000000000085c7b3UL;
+ tf->codes[174] = 0x0000000001311fcbUL;
+ tf->codes[175] = 0x00000000009c6c4fUL;
+ tf->codes[176] = 0x000000000187d947UL;
+ tf->codes[177] = 0x00000000008578e4UL;
+ tf->codes[178] = 0x0000000000e2bf0bUL;
+ tf->codes[179] = 0x0000000000a01b4cUL;
+ tf->codes[180] = 0x0000000000a1493bUL;
+ tf->codes[181] = 0x00000000007ad766UL;
+ tf->codes[182] = 0x0000000000ccfe82UL;
+ tf->codes[183] = 0x0000000001981b5bUL;
+ tf->codes[184] = 0x0000000001cacc85UL;
+ tf->codes[185] = 0x0000000000562cdbUL;
+ tf->codes[186] = 0x00000000015b0e78UL;
+ tf->codes[187] = 0x00000000008f66c5UL;
+ tf->codes[188] = 0x00000000003332bfUL;
+ tf->codes[189] = 0x00000000012ce754UL;
+ tf->codes[190] = 0x0000000000096a76UL;
+ tf->codes[191] = 0x0000000001d5e3baUL;
+ tf->codes[192] = 0x000000000027ea41UL;
+ tf->codes[193] = 0x00000000014412dfUL;
+ tf->codes[194] = 0x000000000067b9b4UL;
+ tf->codes[195] = 0x0000000000daa51aUL;
+ tf->codes[196] = 0x00000000001dcb17UL;
+ tf->codes[197] = 0x00000000004d4afdUL;
+ tf->codes[198] = 0x00000000006335d5UL;
+ tf->codes[199] = 0x0000000000ee2334UL;
+ tf->codes[200] = 0x00000000017d4e55UL;
+ tf->codes[201] = 0x0000000001b8b0f0UL;
+ tf->codes[202] = 0x00000000014999e3UL;
+ tf->codes[203] = 0x0000000001513dfaUL;
+ tf->codes[204] = 0x0000000000765cf2UL;
+ tf->codes[205] = 0x000000000056af90UL;
+ tf->codes[206] = 0x00000000012e16acUL;
+ tf->codes[207] = 0x0000000001d3d86cUL;
+ tf->codes[208] = 0x0000000000ff279bUL;
+ tf->codes[209] = 0x00000000018822ddUL;
+ tf->codes[210] = 0x000000000099d478UL;
+ tf->codes[211] = 0x00000000008dc0d2UL;
+ tf->codes[212] = 0x000000000034b666UL;
+ tf->codes[213] = 0x0000000000cf9526UL;
+ tf->codes[214] = 0x000000000186443dUL;
+ tf->codes[215] = 0x00000000007a8e29UL;
+ tf->codes[216] = 0x00000000019c6aa5UL;
+ tf->codes[217] = 0x0000000001f2a27dUL;
+ tf->codes[218] = 0x00000000012b2136UL;
+ tf->codes[219] = 0x0000000000d0cd0dUL;
+ tf->codes[220] = 0x00000000012cb320UL;
+ tf->codes[221] = 0x00000000017ddb0bUL;
+ tf->codes[222] = 0x000000000005353bUL;
+ tf->codes[223] = 0x00000000015b2cafUL;
+ tf->codes[224] = 0x0000000001e5a507UL;
+ tf->codes[225] = 0x000000000120f1e5UL;
+ tf->codes[226] = 0x000000000114605aUL;
+ tf->codes[227] = 0x00000000014efe4cUL;
+ tf->codes[228] = 0x0000000000568134UL;
+ tf->codes[229] = 0x00000000011b9f92UL;
+ tf->codes[230] = 0x000000000174d2a7UL;
+ tf->codes[231] = 0x0000000000692b1dUL;
+ tf->codes[232] = 0x000000000039e4feUL;
+ tf->codes[233] = 0x0000000000aaff3dUL;
+ tf->codes[234] = 0x000000000096224cUL;
+ tf->codes[235] = 0x00000000013c9f77UL;
+ tf->codes[236] = 0x000000000110ee8fUL;
+ tf->codes[237] = 0x0000000000f17beaUL;
+ tf->codes[238] = 0x000000000099fb5dUL;
+ tf->codes[239] = 0x0000000000337141UL;
+ tf->codes[240] = 0x000000000002b54dUL;
+ tf->codes[241] = 0x0000000001233a70UL;
+ return tf;
+}
+
+void tag25h7_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag25h7.h b/3rdparty/apriltag/tag25h7.h
new file mode 100644
index 0000000..d47217e
--- /dev/null
+++ b/3rdparty/apriltag/tag25h7.h
@@ -0,0 +1,50 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TAG25H7
+#define _TAG25H7
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag25h7_create();
+void tag25h7_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/tag25h9.c b/3rdparty/apriltag/tag25h9.c
new file mode 100644
index 0000000..367cac3
--- /dev/null
+++ b/3rdparty/apriltag/tag25h9.c
@@ -0,0 +1,95 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag25h9_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("tag25h9");
+#else
+ tf->name = strdup("tag25h9");
+#endif
+ tf->black_border = 1;
+ tf->d = 5;
+ tf->h = 9;
+ tf->ncodes = 35;
+ tf->codes = calloc(35, sizeof(uint64_t));
+ tf->codes[0] = 0x000000000155cbf1UL;
+ tf->codes[1] = 0x0000000001e4d1b6UL;
+ tf->codes[2] = 0x00000000017b0b68UL;
+ tf->codes[3] = 0x0000000001eac9cdUL;
+ tf->codes[4] = 0x00000000012e14ceUL;
+ tf->codes[5] = 0x00000000003548bbUL;
+ tf->codes[6] = 0x00000000007757e6UL;
+ tf->codes[7] = 0x0000000001065dabUL;
+ tf->codes[8] = 0x0000000001baa2e7UL;
+ tf->codes[9] = 0x0000000000dea688UL;
+ tf->codes[10] = 0x000000000081d927UL;
+ tf->codes[11] = 0x000000000051b241UL;
+ tf->codes[12] = 0x0000000000dbc8aeUL;
+ tf->codes[13] = 0x0000000001e50e19UL;
+ tf->codes[14] = 0x00000000015819d2UL;
+ tf->codes[15] = 0x00000000016d8282UL;
+ tf->codes[16] = 0x000000000163e035UL;
+ tf->codes[17] = 0x00000000009d9b81UL;
+ tf->codes[18] = 0x000000000173eec4UL;
+ tf->codes[19] = 0x0000000000ae3a09UL;
+ tf->codes[20] = 0x00000000005f7c51UL;
+ tf->codes[21] = 0x0000000001a137fcUL;
+ tf->codes[22] = 0x0000000000dc9562UL;
+ tf->codes[23] = 0x0000000001802e45UL;
+ tf->codes[24] = 0x0000000001c3542cUL;
+ tf->codes[25] = 0x0000000000870fa4UL;
+ tf->codes[26] = 0x0000000000914709UL;
+ tf->codes[27] = 0x00000000016684f0UL;
+ tf->codes[28] = 0x0000000000c8f2a5UL;
+ tf->codes[29] = 0x0000000000833ebbUL;
+ tf->codes[30] = 0x000000000059717fUL;
+ tf->codes[31] = 0x00000000013cd050UL;
+ tf->codes[32] = 0x0000000000fa0ad1UL;
+ tf->codes[33] = 0x0000000001b763b0UL;
+ tf->codes[34] = 0x0000000000b991ceUL;
+ return tf;
+}
+
+void tag25h9_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag25h9.h b/3rdparty/apriltag/tag25h9.h
new file mode 100644
index 0000000..969f621
--- /dev/null
+++ b/3rdparty/apriltag/tag25h9.h
@@ -0,0 +1,50 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TAG25H9
+#define _TAG25H9
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag25h9_create();
+void tag25h9_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/tag36artoolkit.c b/3rdparty/apriltag/tag36artoolkit.c
new file mode 100644
index 0000000..40328d0
--- /dev/null
+++ b/3rdparty/apriltag/tag36artoolkit.c
@@ -0,0 +1,573 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag36artoolkit_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("artoolkit");
+#else
+ tf->name = strdup("artoolkit");
+#endif
+ tf->black_border = 1;
+ tf->d = 6;
+ tf->h = 7; // not sure.
+ tf->ncodes = 512;
+ tf->codes = calloc(512, sizeof(uint64_t));
+
+ tf->codes[0] = 0x0006dc269c27UL;
+ tf->codes[1] = 0x0006d4229e26UL;
+ tf->codes[2] = 0x0006cc2e9825UL;
+ tf->codes[3] = 0x0006c42a9a24UL;
+ tf->codes[4] = 0x0006fc369423UL;
+ tf->codes[5] = 0x0006f4329622UL;
+ tf->codes[6] = 0x0006ec3e9021UL;
+ tf->codes[7] = 0x0006e43a9220UL;
+ tf->codes[8] = 0x00069c068c2fUL;
+ tf->codes[9] = 0x000694028e2eUL;
+ tf->codes[10] = 0x00068c0e882dUL;
+ tf->codes[11] = 0x0006840a8a2cUL;
+ tf->codes[12] = 0x0006bc16842bUL;
+ tf->codes[13] = 0x0006b412862aUL;
+ tf->codes[14] = 0x0006ac1e8029UL;
+ tf->codes[15] = 0x0006a41a8228UL;
+ tf->codes[16] = 0x00065c66bc37UL;
+ tf->codes[17] = 0x00065462be36UL;
+ tf->codes[18] = 0x00064c6eb835UL;
+ tf->codes[19] = 0x0006446aba34UL;
+ tf->codes[20] = 0x00067c76b433UL;
+ tf->codes[21] = 0x00067472b632UL;
+ tf->codes[22] = 0x00066c7eb031UL;
+ tf->codes[23] = 0x0006647ab230UL;
+ tf->codes[24] = 0x00061c46ac3fUL;
+ tf->codes[25] = 0x00061442ae3eUL;
+ tf->codes[26] = 0x00060c4ea83dUL;
+ tf->codes[27] = 0x0006044aaa3cUL;
+ tf->codes[28] = 0x00063c56a43bUL;
+ tf->codes[29] = 0x00063452a63aUL;
+ tf->codes[30] = 0x00062c5ea039UL;
+ tf->codes[31] = 0x0006245aa238UL;
+ tf->codes[32] = 0x0007dca6dc07UL;
+ tf->codes[33] = 0x0007d4a2de06UL;
+ tf->codes[34] = 0x0007ccaed805UL;
+ tf->codes[35] = 0x0007c4aada04UL;
+ tf->codes[36] = 0x0007fcb6d403UL;
+ tf->codes[37] = 0x0007f4b2d602UL;
+ tf->codes[38] = 0x0007ecbed001UL;
+ tf->codes[39] = 0x0007e4bad200UL;
+ tf->codes[40] = 0x00079c86cc0fUL;
+ tf->codes[41] = 0x00079482ce0eUL;
+ tf->codes[42] = 0x00078c8ec80dUL;
+ tf->codes[43] = 0x0007848aca0cUL;
+ tf->codes[44] = 0x0007bc96c40bUL;
+ tf->codes[45] = 0x0007b492c60aUL;
+ tf->codes[46] = 0x0007ac9ec009UL;
+ tf->codes[47] = 0x0007a49ac208UL;
+ tf->codes[48] = 0x00075ce6fc17UL;
+ tf->codes[49] = 0x000754e2fe16UL;
+ tf->codes[50] = 0x00074ceef815UL;
+ tf->codes[51] = 0x000744eafa14UL;
+ tf->codes[52] = 0x00077cf6f413UL;
+ tf->codes[53] = 0x000774f2f612UL;
+ tf->codes[54] = 0x00076cfef011UL;
+ tf->codes[55] = 0x000764faf210UL;
+ tf->codes[56] = 0x00071cc6ec1fUL;
+ tf->codes[57] = 0x000714c2ee1eUL;
+ tf->codes[58] = 0x00070ccee81dUL;
+ tf->codes[59] = 0x000704caea1cUL;
+ tf->codes[60] = 0x00073cd6e41bUL;
+ tf->codes[61] = 0x000734d2e61aUL;
+ tf->codes[62] = 0x00072cdee019UL;
+ tf->codes[63] = 0x000724dae218UL;
+ tf->codes[64] = 0x0004dd261c67UL;
+ tf->codes[65] = 0x0004d5221e66UL;
+ tf->codes[66] = 0x0004cd2e1865UL;
+ tf->codes[67] = 0x0004c52a1a64UL;
+ tf->codes[68] = 0x0004fd361463UL;
+ tf->codes[69] = 0x0004f5321662UL;
+ tf->codes[70] = 0x0004ed3e1061UL;
+ tf->codes[71] = 0x0004e53a1260UL;
+ tf->codes[72] = 0x00049d060c6fUL;
+ tf->codes[73] = 0x000495020e6eUL;
+ tf->codes[74] = 0x00048d0e086dUL;
+ tf->codes[75] = 0x0004850a0a6cUL;
+ tf->codes[76] = 0x0004bd16046bUL;
+ tf->codes[77] = 0x0004b512066aUL;
+ tf->codes[78] = 0x0004ad1e0069UL;
+ tf->codes[79] = 0x0004a51a0268UL;
+ tf->codes[80] = 0x00045d663c77UL;
+ tf->codes[81] = 0x000455623e76UL;
+ tf->codes[82] = 0x00044d6e3875UL;
+ tf->codes[83] = 0x0004456a3a74UL;
+ tf->codes[84] = 0x00047d763473UL;
+ tf->codes[85] = 0x000475723672UL;
+ tf->codes[86] = 0x00046d7e3071UL;
+ tf->codes[87] = 0x0004657a3270UL;
+ tf->codes[88] = 0x00041d462c7fUL;
+ tf->codes[89] = 0x000415422e7eUL;
+ tf->codes[90] = 0x00040d4e287dUL;
+ tf->codes[91] = 0x0004054a2a7cUL;
+ tf->codes[92] = 0x00043d56247bUL;
+ tf->codes[93] = 0x00043552267aUL;
+ tf->codes[94] = 0x00042d5e2079UL;
+ tf->codes[95] = 0x0004255a2278UL;
+ tf->codes[96] = 0x0005dda65c47UL;
+ tf->codes[97] = 0x0005d5a25e46UL;
+ tf->codes[98] = 0x0005cdae5845UL;
+ tf->codes[99] = 0x0005c5aa5a44UL;
+ tf->codes[100] = 0x0005fdb65443UL;
+ tf->codes[101] = 0x0005f5b25642UL;
+ tf->codes[102] = 0x0005edbe5041UL;
+ tf->codes[103] = 0x0005e5ba5240UL;
+ tf->codes[104] = 0x00059d864c4fUL;
+ tf->codes[105] = 0x000595824e4eUL;
+ tf->codes[106] = 0x00058d8e484dUL;
+ tf->codes[107] = 0x0005858a4a4cUL;
+ tf->codes[108] = 0x0005bd96444bUL;
+ tf->codes[109] = 0x0005b592464aUL;
+ tf->codes[110] = 0x0005ad9e4049UL;
+ tf->codes[111] = 0x0005a59a4248UL;
+ tf->codes[112] = 0x00055de67c57UL;
+ tf->codes[113] = 0x000555e27e56UL;
+ tf->codes[114] = 0x00054dee7855UL;
+ tf->codes[115] = 0x000545ea7a54UL;
+ tf->codes[116] = 0x00057df67453UL;
+ tf->codes[117] = 0x000575f27652UL;
+ tf->codes[118] = 0x00056dfe7051UL;
+ tf->codes[119] = 0x000565fa7250UL;
+ tf->codes[120] = 0x00051dc66c5fUL;
+ tf->codes[121] = 0x000515c26e5eUL;
+ tf->codes[122] = 0x00050dce685dUL;
+ tf->codes[123] = 0x000505ca6a5cUL;
+ tf->codes[124] = 0x00053dd6645bUL;
+ tf->codes[125] = 0x000535d2665aUL;
+ tf->codes[126] = 0x00052dde6059UL;
+ tf->codes[127] = 0x000525da6258UL;
+ tf->codes[128] = 0x0002de279ca7UL;
+ tf->codes[129] = 0x0002d6239ea6UL;
+ tf->codes[130] = 0x0002ce2f98a5UL;
+ tf->codes[131] = 0x0002c62b9aa4UL;
+ tf->codes[132] = 0x0002fe3794a3UL;
+ tf->codes[133] = 0x0002f63396a2UL;
+ tf->codes[134] = 0x0002ee3f90a1UL;
+ tf->codes[135] = 0x0002e63b92a0UL;
+ tf->codes[136] = 0x00029e078cafUL;
+ tf->codes[137] = 0x000296038eaeUL;
+ tf->codes[138] = 0x00028e0f88adUL;
+ tf->codes[139] = 0x0002860b8aacUL;
+ tf->codes[140] = 0x0002be1784abUL;
+ tf->codes[141] = 0x0002b61386aaUL;
+ tf->codes[142] = 0x0002ae1f80a9UL;
+ tf->codes[143] = 0x0002a61b82a8UL;
+ tf->codes[144] = 0x00025e67bcb7UL;
+ tf->codes[145] = 0x00025663beb6UL;
+ tf->codes[146] = 0x00024e6fb8b5UL;
+ tf->codes[147] = 0x0002466bbab4UL;
+ tf->codes[148] = 0x00027e77b4b3UL;
+ tf->codes[149] = 0x00027673b6b2UL;
+ tf->codes[150] = 0x00026e7fb0b1UL;
+ tf->codes[151] = 0x0002667bb2b0UL;
+ tf->codes[152] = 0x00021e47acbfUL;
+ tf->codes[153] = 0x00021643aebeUL;
+ tf->codes[154] = 0x00020e4fa8bdUL;
+ tf->codes[155] = 0x0002064baabcUL;
+ tf->codes[156] = 0x00023e57a4bbUL;
+ tf->codes[157] = 0x00023653a6baUL;
+ tf->codes[158] = 0x00022e5fa0b9UL;
+ tf->codes[159] = 0x0002265ba2b8UL;
+ tf->codes[160] = 0x0003dea7dc87UL;
+ tf->codes[161] = 0x0003d6a3de86UL;
+ tf->codes[162] = 0x0003ceafd885UL;
+ tf->codes[163] = 0x0003c6abda84UL;
+ tf->codes[164] = 0x0003feb7d483UL;
+ tf->codes[165] = 0x0003f6b3d682UL;
+ tf->codes[166] = 0x0003eebfd081UL;
+ tf->codes[167] = 0x0003e6bbd280UL;
+ tf->codes[168] = 0x00039e87cc8fUL;
+ tf->codes[169] = 0x00039683ce8eUL;
+ tf->codes[170] = 0x00038e8fc88dUL;
+ tf->codes[171] = 0x0003868bca8cUL;
+ tf->codes[172] = 0x0003be97c48bUL;
+ tf->codes[173] = 0x0003b693c68aUL;
+ tf->codes[174] = 0x0003ae9fc089UL;
+ tf->codes[175] = 0x0003a69bc288UL;
+ tf->codes[176] = 0x00035ee7fc97UL;
+ tf->codes[177] = 0x000356e3fe96UL;
+ tf->codes[178] = 0x00034eeff895UL;
+ tf->codes[179] = 0x000346ebfa94UL;
+ tf->codes[180] = 0x00037ef7f493UL;
+ tf->codes[181] = 0x000376f3f692UL;
+ tf->codes[182] = 0x00036efff091UL;
+ tf->codes[183] = 0x000366fbf290UL;
+ tf->codes[184] = 0x00031ec7ec9fUL;
+ tf->codes[185] = 0x000316c3ee9eUL;
+ tf->codes[186] = 0x00030ecfe89dUL;
+ tf->codes[187] = 0x000306cbea9cUL;
+ tf->codes[188] = 0x00033ed7e49bUL;
+ tf->codes[189] = 0x000336d3e69aUL;
+ tf->codes[190] = 0x00032edfe099UL;
+ tf->codes[191] = 0x000326dbe298UL;
+ tf->codes[192] = 0x0000df271ce7UL;
+ tf->codes[193] = 0x0000d7231ee6UL;
+ tf->codes[194] = 0x0000cf2f18e5UL;
+ tf->codes[195] = 0x0000c72b1ae4UL;
+ tf->codes[196] = 0x0000ff3714e3UL;
+ tf->codes[197] = 0x0000f73316e2UL;
+ tf->codes[198] = 0x0000ef3f10e1UL;
+ tf->codes[199] = 0x0000e73b12e0UL;
+ tf->codes[200] = 0x00009f070cefUL;
+ tf->codes[201] = 0x000097030eeeUL;
+ tf->codes[202] = 0x00008f0f08edUL;
+ tf->codes[203] = 0x0000870b0aecUL;
+ tf->codes[204] = 0x0000bf1704ebUL;
+ tf->codes[205] = 0x0000b71306eaUL;
+ tf->codes[206] = 0x0000af1f00e9UL;
+ tf->codes[207] = 0x0000a71b02e8UL;
+ tf->codes[208] = 0x00005f673cf7UL;
+ tf->codes[209] = 0x000057633ef6UL;
+ tf->codes[210] = 0x00004f6f38f5UL;
+ tf->codes[211] = 0x0000476b3af4UL;
+ tf->codes[212] = 0x00007f7734f3UL;
+ tf->codes[213] = 0x0000777336f2UL;
+ tf->codes[214] = 0x00006f7f30f1UL;
+ tf->codes[215] = 0x0000677b32f0UL;
+ tf->codes[216] = 0x00001f472cffUL;
+ tf->codes[217] = 0x000017432efeUL;
+ tf->codes[218] = 0x00000f4f28fdUL;
+ tf->codes[219] = 0x0000074b2afcUL;
+ tf->codes[220] = 0x00003f5724fbUL;
+ tf->codes[221] = 0x0000375326faUL;
+ tf->codes[222] = 0x00002f5f20f9UL;
+ tf->codes[223] = 0x0000275b22f8UL;
+ tf->codes[224] = 0x0001dfa75cc7UL;
+ tf->codes[225] = 0x0001d7a35ec6UL;
+ tf->codes[226] = 0x0001cfaf58c5UL;
+ tf->codes[227] = 0x0001c7ab5ac4UL;
+ tf->codes[228] = 0x0001ffb754c3UL;
+ tf->codes[229] = 0x0001f7b356c2UL;
+ tf->codes[230] = 0x0001efbf50c1UL;
+ tf->codes[231] = 0x0001e7bb52c0UL;
+ tf->codes[232] = 0x00019f874ccfUL;
+ tf->codes[233] = 0x000197834eceUL;
+ tf->codes[234] = 0x00018f8f48cdUL;
+ tf->codes[235] = 0x0001878b4accUL;
+ tf->codes[236] = 0x0001bf9744cbUL;
+ tf->codes[237] = 0x0001b79346caUL;
+ tf->codes[238] = 0x0001af9f40c9UL;
+ tf->codes[239] = 0x0001a79b42c8UL;
+ tf->codes[240] = 0x00015fe77cd7UL;
+ tf->codes[241] = 0x000157e37ed6UL;
+ tf->codes[242] = 0x00014fef78d5UL;
+ tf->codes[243] = 0x000147eb7ad4UL;
+ tf->codes[244] = 0x00017ff774d3UL;
+ tf->codes[245] = 0x000177f376d2UL;
+ tf->codes[246] = 0x00016fff70d1UL;
+ tf->codes[247] = 0x000167fb72d0UL;
+ tf->codes[248] = 0x00011fc76cdfUL;
+ tf->codes[249] = 0x000117c36edeUL;
+ tf->codes[250] = 0x00010fcf68ddUL;
+ tf->codes[251] = 0x000107cb6adcUL;
+ tf->codes[252] = 0x00013fd764dbUL;
+ tf->codes[253] = 0x000137d366daUL;
+ tf->codes[254] = 0x00012fdf60d9UL;
+ tf->codes[255] = 0x000127db62d8UL;
+ tf->codes[256] = 0x000ed8249d27UL;
+ tf->codes[257] = 0x000ed0209f26UL;
+ tf->codes[258] = 0x000ec82c9925UL;
+ tf->codes[259] = 0x000ec0289b24UL;
+ tf->codes[260] = 0x000ef8349523UL;
+ tf->codes[261] = 0x000ef0309722UL;
+ tf->codes[262] = 0x000ee83c9121UL;
+ tf->codes[263] = 0x000ee0389320UL;
+ tf->codes[264] = 0x000e98048d2fUL;
+ tf->codes[265] = 0x000e90008f2eUL;
+ tf->codes[266] = 0x000e880c892dUL;
+ tf->codes[267] = 0x000e80088b2cUL;
+ tf->codes[268] = 0x000eb814852bUL;
+ tf->codes[269] = 0x000eb010872aUL;
+ tf->codes[270] = 0x000ea81c8129UL;
+ tf->codes[271] = 0x000ea0188328UL;
+ tf->codes[272] = 0x000e5864bd37UL;
+ tf->codes[273] = 0x000e5060bf36UL;
+ tf->codes[274] = 0x000e486cb935UL;
+ tf->codes[275] = 0x000e4068bb34UL;
+ tf->codes[276] = 0x000e7874b533UL;
+ tf->codes[277] = 0x000e7070b732UL;
+ tf->codes[278] = 0x000e687cb131UL;
+ tf->codes[279] = 0x000e6078b330UL;
+ tf->codes[280] = 0x000e1844ad3fUL;
+ tf->codes[281] = 0x000e1040af3eUL;
+ tf->codes[282] = 0x000e084ca93dUL;
+ tf->codes[283] = 0x000e0048ab3cUL;
+ tf->codes[284] = 0x000e3854a53bUL;
+ tf->codes[285] = 0x000e3050a73aUL;
+ tf->codes[286] = 0x000e285ca139UL;
+ tf->codes[287] = 0x000e2058a338UL;
+ tf->codes[288] = 0x000fd8a4dd07UL;
+ tf->codes[289] = 0x000fd0a0df06UL;
+ tf->codes[290] = 0x000fc8acd905UL;
+ tf->codes[291] = 0x000fc0a8db04UL;
+ tf->codes[292] = 0x000ff8b4d503UL;
+ tf->codes[293] = 0x000ff0b0d702UL;
+ tf->codes[294] = 0x000fe8bcd101UL;
+ tf->codes[295] = 0x000fe0b8d300UL;
+ tf->codes[296] = 0x000f9884cd0fUL;
+ tf->codes[297] = 0x000f9080cf0eUL;
+ tf->codes[298] = 0x000f888cc90dUL;
+ tf->codes[299] = 0x000f8088cb0cUL;
+ tf->codes[300] = 0x000fb894c50bUL;
+ tf->codes[301] = 0x000fb090c70aUL;
+ tf->codes[302] = 0x000fa89cc109UL;
+ tf->codes[303] = 0x000fa098c308UL;
+ tf->codes[304] = 0x000f58e4fd17UL;
+ tf->codes[305] = 0x000f50e0ff16UL;
+ tf->codes[306] = 0x000f48ecf915UL;
+ tf->codes[307] = 0x000f40e8fb14UL;
+ tf->codes[308] = 0x000f78f4f513UL;
+ tf->codes[309] = 0x000f70f0f712UL;
+ tf->codes[310] = 0x000f68fcf111UL;
+ tf->codes[311] = 0x000f60f8f310UL;
+ tf->codes[312] = 0x000f18c4ed1fUL;
+ tf->codes[313] = 0x000f10c0ef1eUL;
+ tf->codes[314] = 0x000f08cce91dUL;
+ tf->codes[315] = 0x000f00c8eb1cUL;
+ tf->codes[316] = 0x000f38d4e51bUL;
+ tf->codes[317] = 0x000f30d0e71aUL;
+ tf->codes[318] = 0x000f28dce119UL;
+ tf->codes[319] = 0x000f20d8e318UL;
+ tf->codes[320] = 0x000cd9241d67UL;
+ tf->codes[321] = 0x000cd1201f66UL;
+ tf->codes[322] = 0x000cc92c1965UL;
+ tf->codes[323] = 0x000cc1281b64UL;
+ tf->codes[324] = 0x000cf9341563UL;
+ tf->codes[325] = 0x000cf1301762UL;
+ tf->codes[326] = 0x000ce93c1161UL;
+ tf->codes[327] = 0x000ce1381360UL;
+ tf->codes[328] = 0x000c99040d6fUL;
+ tf->codes[329] = 0x000c91000f6eUL;
+ tf->codes[330] = 0x000c890c096dUL;
+ tf->codes[331] = 0x000c81080b6cUL;
+ tf->codes[332] = 0x000cb914056bUL;
+ tf->codes[333] = 0x000cb110076aUL;
+ tf->codes[334] = 0x000ca91c0169UL;
+ tf->codes[335] = 0x000ca1180368UL;
+ tf->codes[336] = 0x000c59643d77UL;
+ tf->codes[337] = 0x000c51603f76UL;
+ tf->codes[338] = 0x000c496c3975UL;
+ tf->codes[339] = 0x000c41683b74UL;
+ tf->codes[340] = 0x000c79743573UL;
+ tf->codes[341] = 0x000c71703772UL;
+ tf->codes[342] = 0x000c697c3171UL;
+ tf->codes[343] = 0x000c61783370UL;
+ tf->codes[344] = 0x000c19442d7fUL;
+ tf->codes[345] = 0x000c11402f7eUL;
+ tf->codes[346] = 0x000c094c297dUL;
+ tf->codes[347] = 0x000c01482b7cUL;
+ tf->codes[348] = 0x000c3954257bUL;
+ tf->codes[349] = 0x000c3150277aUL;
+ tf->codes[350] = 0x000c295c2179UL;
+ tf->codes[351] = 0x000c21582378UL;
+ tf->codes[352] = 0x000dd9a45d47UL;
+ tf->codes[353] = 0x000dd1a05f46UL;
+ tf->codes[354] = 0x000dc9ac5945UL;
+ tf->codes[355] = 0x000dc1a85b44UL;
+ tf->codes[356] = 0x000df9b45543UL;
+ tf->codes[357] = 0x000df1b05742UL;
+ tf->codes[358] = 0x000de9bc5141UL;
+ tf->codes[359] = 0x000de1b85340UL;
+ tf->codes[360] = 0x000d99844d4fUL;
+ tf->codes[361] = 0x000d91804f4eUL;
+ tf->codes[362] = 0x000d898c494dUL;
+ tf->codes[363] = 0x000d81884b4cUL;
+ tf->codes[364] = 0x000db994454bUL;
+ tf->codes[365] = 0x000db190474aUL;
+ tf->codes[366] = 0x000da99c4149UL;
+ tf->codes[367] = 0x000da1984348UL;
+ tf->codes[368] = 0x000d59e47d57UL;
+ tf->codes[369] = 0x000d51e07f56UL;
+ tf->codes[370] = 0x000d49ec7955UL;
+ tf->codes[371] = 0x000d41e87b54UL;
+ tf->codes[372] = 0x000d79f47553UL;
+ tf->codes[373] = 0x000d71f07752UL;
+ tf->codes[374] = 0x000d69fc7151UL;
+ tf->codes[375] = 0x000d61f87350UL;
+ tf->codes[376] = 0x000d19c46d5fUL;
+ tf->codes[377] = 0x000d11c06f5eUL;
+ tf->codes[378] = 0x000d09cc695dUL;
+ tf->codes[379] = 0x000d01c86b5cUL;
+ tf->codes[380] = 0x000d39d4655bUL;
+ tf->codes[381] = 0x000d31d0675aUL;
+ tf->codes[382] = 0x000d29dc6159UL;
+ tf->codes[383] = 0x000d21d86358UL;
+ tf->codes[384] = 0x000ada259da7UL;
+ tf->codes[385] = 0x000ad2219fa6UL;
+ tf->codes[386] = 0x000aca2d99a5UL;
+ tf->codes[387] = 0x000ac2299ba4UL;
+ tf->codes[388] = 0x000afa3595a3UL;
+ tf->codes[389] = 0x000af23197a2UL;
+ tf->codes[390] = 0x000aea3d91a1UL;
+ tf->codes[391] = 0x000ae23993a0UL;
+ tf->codes[392] = 0x000a9a058dafUL;
+ tf->codes[393] = 0x000a92018faeUL;
+ tf->codes[394] = 0x000a8a0d89adUL;
+ tf->codes[395] = 0x000a82098bacUL;
+ tf->codes[396] = 0x000aba1585abUL;
+ tf->codes[397] = 0x000ab21187aaUL;
+ tf->codes[398] = 0x000aaa1d81a9UL;
+ tf->codes[399] = 0x000aa21983a8UL;
+ tf->codes[400] = 0x000a5a65bdb7UL;
+ tf->codes[401] = 0x000a5261bfb6UL;
+ tf->codes[402] = 0x000a4a6db9b5UL;
+ tf->codes[403] = 0x000a4269bbb4UL;
+ tf->codes[404] = 0x000a7a75b5b3UL;
+ tf->codes[405] = 0x000a7271b7b2UL;
+ tf->codes[406] = 0x000a6a7db1b1UL;
+ tf->codes[407] = 0x000a6279b3b0UL;
+ tf->codes[408] = 0x000a1a45adbfUL;
+ tf->codes[409] = 0x000a1241afbeUL;
+ tf->codes[410] = 0x000a0a4da9bdUL;
+ tf->codes[411] = 0x000a0249abbcUL;
+ tf->codes[412] = 0x000a3a55a5bbUL;
+ tf->codes[413] = 0x000a3251a7baUL;
+ tf->codes[414] = 0x000a2a5da1b9UL;
+ tf->codes[415] = 0x000a2259a3b8UL;
+ tf->codes[416] = 0x000bdaa5dd87UL;
+ tf->codes[417] = 0x000bd2a1df86UL;
+ tf->codes[418] = 0x000bcaadd985UL;
+ tf->codes[419] = 0x000bc2a9db84UL;
+ tf->codes[420] = 0x000bfab5d583UL;
+ tf->codes[421] = 0x000bf2b1d782UL;
+ tf->codes[422] = 0x000beabdd181UL;
+ tf->codes[423] = 0x000be2b9d380UL;
+ tf->codes[424] = 0x000b9a85cd8fUL;
+ tf->codes[425] = 0x000b9281cf8eUL;
+ tf->codes[426] = 0x000b8a8dc98dUL;
+ tf->codes[427] = 0x000b8289cb8cUL;
+ tf->codes[428] = 0x000bba95c58bUL;
+ tf->codes[429] = 0x000bb291c78aUL;
+ tf->codes[430] = 0x000baa9dc189UL;
+ tf->codes[431] = 0x000ba299c388UL;
+ tf->codes[432] = 0x000b5ae5fd97UL;
+ tf->codes[433] = 0x000b52e1ff96UL;
+ tf->codes[434] = 0x000b4aedf995UL;
+ tf->codes[435] = 0x000b42e9fb94UL;
+ tf->codes[436] = 0x000b7af5f593UL;
+ tf->codes[437] = 0x000b72f1f792UL;
+ tf->codes[438] = 0x000b6afdf191UL;
+ tf->codes[439] = 0x000b62f9f390UL;
+ tf->codes[440] = 0x000b1ac5ed9fUL;
+ tf->codes[441] = 0x000b12c1ef9eUL;
+ tf->codes[442] = 0x000b0acde99dUL;
+ tf->codes[443] = 0x000b02c9eb9cUL;
+ tf->codes[444] = 0x000b3ad5e59bUL;
+ tf->codes[445] = 0x000b32d1e79aUL;
+ tf->codes[446] = 0x000b2adde199UL;
+ tf->codes[447] = 0x000b22d9e398UL;
+ tf->codes[448] = 0x0008db251de7UL;
+ tf->codes[449] = 0x0008d3211fe6UL;
+ tf->codes[450] = 0x0008cb2d19e5UL;
+ tf->codes[451] = 0x0008c3291be4UL;
+ tf->codes[452] = 0x0008fb3515e3UL;
+ tf->codes[453] = 0x0008f33117e2UL;
+ tf->codes[454] = 0x0008eb3d11e1UL;
+ tf->codes[455] = 0x0008e33913e0UL;
+ tf->codes[456] = 0x00089b050defUL;
+ tf->codes[457] = 0x000893010feeUL;
+ tf->codes[458] = 0x00088b0d09edUL;
+ tf->codes[459] = 0x000883090becUL;
+ tf->codes[460] = 0x0008bb1505ebUL;
+ tf->codes[461] = 0x0008b31107eaUL;
+ tf->codes[462] = 0x0008ab1d01e9UL;
+ tf->codes[463] = 0x0008a31903e8UL;
+ tf->codes[464] = 0x00085b653df7UL;
+ tf->codes[465] = 0x000853613ff6UL;
+ tf->codes[466] = 0x00084b6d39f5UL;
+ tf->codes[467] = 0x000843693bf4UL;
+ tf->codes[468] = 0x00087b7535f3UL;
+ tf->codes[469] = 0x0008737137f2UL;
+ tf->codes[470] = 0x00086b7d31f1UL;
+ tf->codes[471] = 0x0008637933f0UL;
+ tf->codes[472] = 0x00081b452dffUL;
+ tf->codes[473] = 0x000813412ffeUL;
+ tf->codes[474] = 0x00080b4d29fdUL;
+ tf->codes[475] = 0x000803492bfcUL;
+ tf->codes[476] = 0x00083b5525fbUL;
+ tf->codes[477] = 0x0008335127faUL;
+ tf->codes[478] = 0x00082b5d21f9UL;
+ tf->codes[479] = 0x0008235923f8UL;
+ tf->codes[480] = 0x0009dba55dc7UL;
+ tf->codes[481] = 0x0009d3a15fc6UL;
+ tf->codes[482] = 0x0009cbad59c5UL;
+ tf->codes[483] = 0x0009c3a95bc4UL;
+ tf->codes[484] = 0x0009fbb555c3UL;
+ tf->codes[485] = 0x0009f3b157c2UL;
+ tf->codes[486] = 0x0009ebbd51c1UL;
+ tf->codes[487] = 0x0009e3b953c0UL;
+ tf->codes[488] = 0x00099b854dcfUL;
+ tf->codes[489] = 0x000993814fceUL;
+ tf->codes[490] = 0x00098b8d49cdUL;
+ tf->codes[491] = 0x000983894bccUL;
+ tf->codes[492] = 0x0009bb9545cbUL;
+ tf->codes[493] = 0x0009b39147caUL;
+ tf->codes[494] = 0x0009ab9d41c9UL;
+ tf->codes[495] = 0x0009a39943c8UL;
+ tf->codes[496] = 0x00095be57dd7UL;
+ tf->codes[497] = 0x000953e17fd6UL;
+ tf->codes[498] = 0x00094bed79d5UL;
+ tf->codes[499] = 0x000943e97bd4UL;
+ tf->codes[500] = 0x00097bf575d3UL;
+ tf->codes[501] = 0x000973f177d2UL;
+ tf->codes[502] = 0x00096bfd71d1UL;
+ tf->codes[503] = 0x000963f973d0UL;
+ tf->codes[504] = 0x00091bc56ddfUL;
+ tf->codes[505] = 0x000913c16fdeUL;
+ tf->codes[506] = 0x00090bcd69ddUL;
+ tf->codes[507] = 0x000903c96bdcUL;
+ tf->codes[508] = 0x00093bd565dbUL;
+ tf->codes[509] = 0x000933d167daUL;
+ tf->codes[510] = 0x00092bdd61d9UL;
+ tf->codes[511] = 0x000923d963d8UL;
+
+ return tf;
+}
+
+void tag36artoolkit_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag36artoolkit.h b/3rdparty/apriltag/tag36artoolkit.h
new file mode 100644
index 0000000..384b065
--- /dev/null
+++ b/3rdparty/apriltag/tag36artoolkit.h
@@ -0,0 +1,49 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+#ifndef _TAG36ARTOOLKIT
+#define _TAG36ARTOOLKIT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag36artoolkit_create();
+void tag36artoolkit_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/tag36h10.c b/3rdparty/apriltag/tag36h10.c
new file mode 100644
index 0000000..0be6950
--- /dev/null
+++ b/3rdparty/apriltag/tag36h10.c
@@ -0,0 +1,2380 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag36h10_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("tag36h10");
+#else
+ tf->name = strdup("tag36h10");
+#endif
+ tf->black_border = 1;
+ tf->d = 6;
+ tf->h = 10;
+ tf->ncodes = 2320; //2320;
+ tf->codes = calloc(2320, sizeof(uint64_t));
+ tf->codes[0] = 0x00000001ca92a687UL;
+ tf->codes[1] = 0x000000020521ac4cUL;
+ tf->codes[2] = 0x000000027a3fb7d6UL;
+ tf->codes[3] = 0x00000002b4cebd9bUL;
+ tf->codes[4] = 0x00000003647bceeaUL;
+ tf->codes[5] = 0x000000039f0ad4afUL;
+ tf->codes[6] = 0x00000003d999da74UL;
+ tf->codes[7] = 0x000000044eb7e5feUL;
+ tf->codes[8] = 0x0000000538f3fd12UL;
+ tf->codes[9] = 0x00000005738302d7UL;
+ tf->codes[10] = 0x000000065dbf19ebUL;
+ tf->codes[11] = 0x000000070d6c2b3aUL;
+ tf->codes[12] = 0x00000007f7a8424eUL;
+ tf->codes[13] = 0x0000000832374813UL;
+ tf->codes[14] = 0x000000086cc64dd8UL;
+ tf->codes[15] = 0x00000008a755539dUL;
+ tf->codes[16] = 0x00000009570264ecUL;
+ tf->codes[17] = 0x0000000991916ab1UL;
+ tf->codes[18] = 0x0000000a06af763bUL;
+ tf->codes[19] = 0x0000000ab65c878aUL;
+ tf->codes[20] = 0x0000000b2b7a9314UL;
+ tf->codes[21] = 0x0000000b660998d9UL;
+ tf->codes[22] = 0x0000000bdb27a463UL;
+ tf->codes[23] = 0x0000000cc563bb77UL;
+ tf->codes[24] = 0x0000000e24bdde15UL;
+ tf->codes[25] = 0x0000000ed46aef64UL;
+ tf->codes[26] = 0x0000000f4988faeeUL;
+ tf->codes[27] = 0x000000006e5417c7UL;
+ tf->codes[28] = 0x0000000158902edbUL;
+ tf->codes[29] = 0x00000001cdae3a65UL;
+ tf->codes[30] = 0x0000000242cc45efUL;
+ tf->codes[31] = 0x000000027d5b4bb4UL;
+ tf->codes[32] = 0x00000002b7ea5179UL;
+ tf->codes[33] = 0x000000032d085d03UL;
+ tf->codes[34] = 0x00000003679762c8UL;
+ tf->codes[35] = 0x00000003a226688dUL;
+ tf->codes[36] = 0x00000003dcb56e52UL;
+ tf->codes[37] = 0x000000048c627fa1UL;
+ tf->codes[38] = 0x00000005769e96b5UL;
+ tf->codes[39] = 0x00000006264ba804UL;
+ tf->codes[40] = 0x0000000660daadc9UL;
+ tf->codes[41] = 0x00000006d5f8b953UL;
+ tf->codes[42] = 0x000000074b16c4ddUL;
+ tf->codes[43] = 0x00000007fac3d62cUL;
+ tf->codes[44] = 0x000000091f8ef305UL;
+ tf->codes[45] = 0x000000095a1df8caUL;
+ tf->codes[46] = 0x0000000994acfe8fUL;
+ tf->codes[47] = 0x0000000a09cb0a19UL;
+ tf->codes[48] = 0x0000000a445a0fdeUL;
+ tf->codes[49] = 0x0000000a7ee915a3UL;
+ tf->codes[50] = 0x0000000ab9781b68UL;
+ tf->codes[51] = 0x0000000af407212dUL;
+ tf->codes[52] = 0x0000000b69252cb7UL;
+ tf->codes[53] = 0x0000000c8df04990UL;
+ tf->codes[54] = 0x0000000d3d9d5adfUL;
+ tf->codes[55] = 0x0000000d782c60a4UL;
+ tf->codes[56] = 0x0000000f12158907UL;
+ tf->codes[57] = 0x00000001d0c9ce43UL;
+ tf->codes[58] = 0x000000020b58d408UL;
+ tf->codes[59] = 0x00000002f594eb1cUL;
+ tf->codes[60] = 0x00000003a541fc6bUL;
+ tf->codes[61] = 0x0000000454ef0dbaUL;
+ tf->codes[62] = 0x000000053f2b24ceUL;
+ tf->codes[63] = 0x0000000629673be2UL;
+ tf->codes[64] = 0x000000074e3258bbUL;
+ tf->codes[65] = 0x00000008ad8c7b59UL;
+ tf->codes[66] = 0x00000009d2579832UL;
+ tf->codes[67] = 0x0000000a8204a981UL;
+ tf->codes[68] = 0x0000000af722b50bUL;
+ tf->codes[69] = 0x0000000b6c40c095UL;
+ tf->codes[70] = 0x0000000ba6cfc65aUL;
+ tf->codes[71] = 0x0000000f15311ce5UL;
+ tf->codes[72] = 0x00000000748b3f83UL;
+ tf->codes[73] = 0x00000000af1a4548UL;
+ tf->codes[74] = 0x00000000e9a94b0dUL;
+ tf->codes[75] = 0x00000002be217935UL;
+ tf->codes[76] = 0x00000003e2ec960eUL;
+ tf->codes[77] = 0x00000004cd28ad22UL;
+ tf->codes[78] = 0x0000000507b7b2e7UL;
+ tf->codes[79] = 0x000000054246b8acUL;
+ tf->codes[80] = 0x000000057cd5be71UL;
+ tf->codes[81] = 0x00000006a1a0db4aUL;
+ tf->codes[82] = 0x00000006dc2fe10fUL;
+ tf->codes[83] = 0x0000000876190972UL;
+ tf->codes[84] = 0x000000099ae4264bUL;
+ tf->codes[85] = 0x0000000abfaf4324UL;
+ tf->codes[86] = 0x0000000c9427714cUL;
+ tf->codes[87] = 0x0000000d09457cd6UL;
+ tf->codes[88] = 0x0000000d43d4829bUL;
+ tf->codes[89] = 0x0000000ea32ea539UL;
+ tf->codes[90] = 0x0000000f52dbb688UL;
+ tf->codes[91] = 0x0000000161e2ea75UL;
+ tf->codes[92] = 0x0000000286ae074eUL;
+ tf->codes[93] = 0x000000066a2d6963UL;
+ tf->codes[94] = 0x00000008b3c3a315UL;
+ tf->codes[95] = 0x00000008ee52a8daUL;
+ tf->codes[96] = 0x0000000a131dc5b3UL;
+ tf->codes[97] = 0x0000000e6bbb3352UL;
+ tf->codes[98] = 0x0000000f55f74a66UL;
+ tf->codes[99] = 0x0000000005a45bb5UL;
+ tf->codes[100] = 0x000000007ac2673fUL;
+ tf->codes[101] = 0x00000001da1c89ddUL;
+ tf->codes[102] = 0x0000000289c99b2cUL;
+ tf->codes[103] = 0x00000003ae94b805UL;
+ tf->codes[104] = 0x000000050deedaa3UL;
+ tf->codes[105] = 0x00000005830ce62dUL;
+ tf->codes[106] = 0x00000005bd9bebf2UL;
+ tf->codes[107] = 0x0000000632b9f77cUL;
+ tf->codes[108] = 0x00000006e26708cbUL;
+ tf->codes[109] = 0x0000000841c12b69UL;
+ tf->codes[110] = 0x000000092bfd427dUL;
+ tf->codes[111] = 0x00000009668c4842UL;
+ tf->codes[112] = 0x00000009dbaa53ccUL;
+ tf->codes[113] = 0x0000000b007570a5UL;
+ tf->codes[114] = 0x0000000b3b04766aUL;
+ tf->codes[115] = 0x0000000c25408d7eUL;
+ tf->codes[116] = 0x0000000ea965ccf5UL;
+ tf->codes[117] = 0x0000000f93a1e409UL;
+ tf->codes[118] = 0x00000000434ef558UL;
+ tf->codes[119] = 0x00000001681a1231UL;
+ tf->codes[120] = 0x00000001dd381dbbUL;
+ tf->codes[121] = 0x0000000302033a94UL;
+ tf->codes[122] = 0x000000075aa0a833UL;
+ tf->codes[123] = 0x000000092f18d65bUL;
+ tf->codes[124] = 0x00000009a436e1e5UL;
+ tf->codes[125] = 0x0000000a1954ed6fUL;
+ tf->codes[126] = 0x0000000b78af100dUL;
+ tf->codes[127] = 0x0000000bb33e15d2UL;
+ tf->codes[128] = 0x0000000c62eb2721UL;
+ tf->codes[129] = 0x00000000466a8936UL;
+ tf->codes[130] = 0x00000000f6179a85UL;
+ tf->codes[131] = 0x000000016b35a60fUL;
+ tf->codes[132] = 0x0000000589440de9UL;
+ tf->codes[133] = 0x00000006738024fdUL;
+ tf->codes[134] = 0x0000000847f85325UL;
+ tf->codes[135] = 0x00000009e1e17b88UL;
+ tf->codes[136] = 0x0000000acc1d929cUL;
+ tf->codes[137] = 0x0000000b06ac9861UL;
+ tf->codes[138] = 0x0000000d5042d213UL;
+ tf->codes[139] = 0x0000000fd468118aUL;
+ tf->codes[140] = 0x00000000f9332e63UL;
+ tf->codes[141] = 0x0000000342c96815UL;
+ tf->codes[142] = 0x000000037d586ddaUL;
+ tf->codes[143] = 0x0000000551d09c02UL;
+ tf->codes[144] = 0x00000005c6eea78cUL;
+ tf->codes[145] = 0x00000006017dad51UL;
+ tf->codes[146] = 0x00000009354ffe17UL;
+ tf->codes[147] = 0x00000009aa6e09a1UL;
+ tf->codes[148] = 0x0000000a94aa20b5UL;
+ tf->codes[149] = 0x0000000acf39267aUL;
+ tf->codes[150] = 0x0000000bb9753d8eUL;
+ tf->codes[151] = 0x0000000bf4044353UL;
+ tf->codes[152] = 0x000000008730b6b7UL;
+ tf->codes[153] = 0x00000001716ccdcbUL;
+ tf->codes[154] = 0x000000022119df1aUL;
+ tf->codes[155] = 0x00000003f5920d42UL;
+ tf->codes[156] = 0x000000058f7b35a5UL;
+ tf->codes[157] = 0x00000006b446527eUL;
+ tf->codes[158] = 0x0000000972fa97baUL;
+ tf->codes[159] = 0x00000009e818a344UL;
+ tf->codes[160] = 0x0000000a5d36aeceUL;
+ tf->codes[161] = 0x0000000d1beaf40aUL;
+ tf->codes[162] = 0x0000000dcb980559UL;
+ tf->codes[163] = 0x0000000f65812dbcUL;
+ tf->codes[164] = 0x0000000139f95be4UL;
+ tf->codes[165] = 0x00000006b761e65cUL;
+ tf->codes[166] = 0x00000006f1f0ec21UL;
+ tf->codes[167] = 0x0000000a605242acUL;
+ tf->codes[168] = 0x0000000e43d1a4c1UL;
+ tf->codes[169] = 0x000000029c6f1260UL;
+ tf->codes[170] = 0x0000000386ab2974UL;
+ tf->codes[171] = 0x00000004e6054c12UL;
+ tf->codes[172] = 0x00000008c984ae27UL;
+ tf->codes[173] = 0x000000097931bf76UL;
+ tf->codes[174] = 0x00000009ee4fcb00UL;
+ tf->codes[175] = 0x0000000d22221bc6UL;
+ tf->codes[176] = 0x0000000e46ed389fUL;
+ tf->codes[177] = 0x0000000ebc0b4429UL;
+ tf->codes[178] = 0x00000006f82813ddUL;
+ tf->codes[179] = 0x0000000732b719a2UL;
+ tf->codes[180] = 0x00000009072f47caUL;
+ tf->codes[181] = 0x0000000941be4d8fUL;
+ tf->codes[182] = 0x000000097c4d5354UL;
+ tf->codes[183] = 0x00000009f16b5edeUL;
+ tf->codes[184] = 0x0000000b8b548741UL;
+ tf->codes[185] = 0x0000000d253dafa4UL;
+ tf->codes[186] = 0x0000000d5fccb569UL;
+ tf->codes[187] = 0x0000000d9a5bbb2eUL;
+ tf->codes[188] = 0x0000000e4a08cc7dUL;
+ tf->codes[189] = 0x00000003c77156f5UL;
+ tf->codes[190] = 0x00000007aaf0b90aUL;
+ tf->codes[191] = 0x0000000b53e1155aUL;
+ tf->codes[192] = 0x0000000b8e701b1fUL;
+ tf->codes[193] = 0x000000005c2b9448UL;
+ tf->codes[194] = 0x000000014667ab5cUL;
+ tf->codes[195] = 0x000000047a39fc22UL;
+ tf->codes[196] = 0x00000004ef5807acUL;
+ tf->codes[197] = 0x000000064eb22a4aUL;
+ tf->codes[198] = 0x0000000982847b10UL;
+ tf->codes[199] = 0x0000000aa74f97e9UL;
+ tf->codes[200] = 0x0000000ae1de9daeUL;
+ tf->codes[201] = 0x0000000b56fca938UL;
+ tf->codes[202] = 0x0000000f750b1112UL;
+ tf->codes[203] = 0x00000001bea14ac4UL;
+ tf->codes[204] = 0x00000001f9305089UL;
+ tf->codes[205] = 0x0000000233bf564eUL;
+ tf->codes[206] = 0x000000031dfb6d62UL;
+ tf->codes[207] = 0x00000003931978ecUL;
+ tf->codes[208] = 0x000000052d02a14fUL;
+ tf->codes[209] = 0x00000005a220acd9UL;
+ tf->codes[210] = 0x0000000b1f893751UL;
+ tf->codes[211] = 0x0000000fb2b5aab5UL;
+ tf->codes[212] = 0x000000040b531854UL;
+ tf->codes[213] = 0x00000005a53c40b7UL;
+ tf->codes[214] = 0x00000008d90e917dUL;
+ tf->codes[215] = 0x00000009139d9742UL;
+ tf->codes[216] = 0x000000094e2c9d07UL;
+ tf->codes[217] = 0x0000000d6c3b04e1UL;
+ tf->codes[218] = 0x0000000e910621baUL;
+ tf->codes[219] = 0x0000000f40b33309UL;
+ tf->codes[220] = 0x00000001152b6131UL;
+ tf->codes[221] = 0x000000032432951eUL;
+ tf->codes[222] = 0x00000003d3dfa66dUL;
+ tf->codes[223] = 0x000000065804e5e4UL;
+ tf->codes[224] = 0x0000000ab0a25383UL;
+ tf->codes[225] = 0x0000000b604f64d2UL;
+ tf->codes[226] = 0x0000000de474a449UL;
+ tf->codes[227] = 0x000000011846f50fUL;
+ tf->codes[228] = 0x00000006d03e854cUL;
+ tf->codes[229] = 0x00000007455c90d6UL;
+ tf->codes[230] = 0x0000000ab3bde761UL;
+ tf->codes[231] = 0x0000000dad013262UL;
+ tf->codes[232] = 0x0000000e973d4976UL;
+ tf->codes[233] = 0x00000002b54bb150UL;
+ tf->codes[234] = 0x00000009577f58a1UL;
+ tf->codes[235] = 0x00000009920e5e66UL;
+ tf->codes[236] = 0x0000000b66868c8eUL;
+ tf->codes[237] = 0x0000000f4a05eea3UL;
+ tf->codes[238] = 0x00000003dd326207UL;
+ tf->codes[239] = 0x00000005b1aa902fUL;
+ tf->codes[240] = 0x000000099529f244UL;
+ tf->codes[241] = 0x0000000b2f131aa7UL;
+ tf->codes[242] = 0x0000000d038b48cfUL;
+ tf->codes[243] = 0x0000000d3e1a4e94UL;
+ tf->codes[244] = 0x000000024664cd82UL;
+ tf->codes[245] = 0x000000036b2fea5bUL;
+ tf->codes[246] = 0x000000095db6805dUL;
+ tf->codes[247] = 0x0000000a0d6391acUL;
+ tf->codes[248] = 0x0000000abd10a2fbUL;
+ tf->codes[249] = 0x000000015f444a4cUL;
+ tf->codes[250] = 0x00000002be9e6ceaUL;
+ tf->codes[251] = 0x000000057d52b226UL;
+ tf->codes[252] = 0x00000005f270bdb0UL;
+ tf->codes[253] = 0x0000000b6fd94828UL;
+ tf->codes[254] = 0x0000000879b19105UL;
+ tf->codes[255] = 0x0000000d476d0a2eUL;
+ tf->codes[256] = 0x0000000e6c382707UL;
+ tf->codes[257] = 0x0000000dbfa6a996UL;
+ tf->codes[258] = 0x00000001689705e6UL;
+ tf->codes[259] = 0x00000003b22d3f98UL;
+ tf->codes[260] = 0x0000000636527f0fUL;
+ tf->codes[261] = 0x00000007d03ba772UL;
+ tf->codes[262] = 0x0000000ee78d5a4dUL;
+ tf->codes[263] = 0x0000000bf165a32aUL;
+ tf->codes[264] = 0x0000000c2bf4a8efUL;
+ tf->codes[265] = 0x0000000517be89f2UL;
+ tf->codes[266] = 0x000000067718ac90UL;
+ tf->codes[267] = 0x00000006b1a7b255UL;
+ tf->codes[268] = 0x0000000726c5bddfUL;
+ tf->codes[269] = 0x0000000bb9f23143UL;
+ tf->codes[270] = 0x00000001375abbbbUL;
+ tf->codes[271] = 0x0000000296b4de59UL;
+ tf->codes[272] = 0x00000008893b745bUL;
+ tf->codes[273] = 0x0000000a9842a848UL;
+ tf->codes[274] = 0x0000000b827ebf5cUL;
+ tf->codes[275] = 0x00000003840c894bUL;
+ tf->codes[276] = 0x00000006b7deda11UL;
+ tf->codes[277] = 0x0000000bc02958ffUL;
+ tf->codes[278] = 0x000000055ba04b51UL;
+ tf->codes[279] = 0x000000076aa77f3eUL;
+ tf->codes[280] = 0x00000009b43db8f0UL;
+ tf->codes[281] = 0x00000009eeccbeb5UL;
+ tf->codes[282] = 0x0000000a295bc47aUL;
+ tf->codes[283] = 0x0000000b4e26e153UL;
+ tf->codes[284] = 0x0000000e476a2c54UL;
+ tf->codes[285] = 0x00000006be1601cdUL;
+ tf->codes[286] = 0x00000006f8a50792UL;
+ tf->codes[287] = 0x000000097cca4709UL;
+ tf->codes[288] = 0x0000000bc66080bbUL;
+ tf->codes[289] = 0x00000001093a056eUL;
+ tf->codes[290] = 0x00000006fbc09b70UL;
+ tf->codes[291] = 0x0000000b8eed0ed4UL;
+ tf->codes[292] = 0x0000000cee473172UL;
+ tf->codes[293] = 0x000000023120b625UL;
+ tf->codes[294] = 0x00000005da111275UL;
+ tf->codes[295] = 0x0000000cf162c550UL;
+ tf->codes[296] = 0x0000000ec8f68756UL;
+ tf->codes[297] = 0x0000000b5db0c4a9UL;
+ tf->codes[298] = 0x00000002b2ad1127UL;
+ tf->codes[299] = 0x0000000536d2509eUL;
+ tf->codes[300] = 0x00000009c9fec402UL;
+ tf->codes[301] = 0x0000000c1394fdb4UL;
+ tf->codes[302] = 0x000000006c326b53UL;
+ tf->codes[303] = 0x00000005e99af5cbUL;
+ tf->codes[304] = 0x0000000af1e574b9UL;
+ tf->codes[305] = 0x0000000e6046cb44UL;
+ tf->codes[306] = 0x0000000661d49533UL;
+ tf->codes[307] = 0x00000008e5f9d4aaUL;
+ tf->codes[308] = 0x0000000db3b54dd3UL;
+ tf->codes[309] = 0x0000000e63625f22UL;
+ tf->codes[310] = 0x0000000e9df164e7UL;
+ tf->codes[311] = 0x0000000455e8f524UL;
+ tf->codes[312] = 0x00000005b54317c2UL;
+ tf->codes[313] = 0x0000000be258b389UL;
+ tf->codes[314] = 0x000000054340a016UL;
+ tf->codes[315] = 0x00000005b85eaba0UL;
+ tf->codes[316] = 0x00000001284dcc1aUL;
+ tf->codes[317] = 0x000000024d18e8f3UL;
+ tf->codes[318] = 0x00000004d13e286aUL;
+ tf->codes[319] = 0x00000008b4bd8a7fUL;
+ tf->codes[320] = 0x0000000215a5770cUL;
+ tf->codes[321] = 0x000000046572d87aUL;
+ tf->codes[322] = 0x0000000c2c719ca4UL;
+ tf->codes[323] = 0x00000004ddac77e2UL;
+ tf->codes[324] = 0x0000000d19c94796UL;
+ tf->codes[325] = 0x00000002d1c0d7d3UL;
+ tf->codes[326] = 0x00000009ae8384e9UL;
+ tf->codes[327] = 0x00000009e9128aaeUL;
+ tf->codes[328] = 0x0000000ca7c6cfeaUL;
+ tf->codes[329] = 0x0000000016282675UL;
+ tf->codes[330] = 0x0000000ad985c97eUL;
+ tf->codes[331] = 0x00000004af8bc195UL;
+ tf->codes[332] = 0x00000009f580da26UL;
+ tf->codes[333] = 0x0000000a6a9ee5b0UL;
+ tf->codes[334] = 0x0000000bc9f9084eUL;
+ tf->codes[335] = 0x0000000d63e230b1UL;
+ tf->codes[336] = 0x0000000c4232a7b6UL;
+ tf->codes[337] = 0x0000000d66fdc48fUL;
+ tf->codes[338] = 0x0000000ec657e72dUL;
+ tf->codes[339] = 0x0000000a364707a7UL;
+ tf->codes[340] = 0x0000000f79208c5aUL;
+ tf->codes[341] = 0x0000000de88a1f91UL;
+ tf->codes[342] = 0x0000000574f9ddf6UL;
+ tf->codes[343] = 0x000000065f35f50aUL;
+ tf->codes[344] = 0x000000069ce08eadUL;
+ tf->codes[345] = 0x0000000490f4ee9eUL;
+ tf->codes[346] = 0x0000000c9282b88dUL;
+ tf->codes[347] = 0x0000000752c4c7b8UL;
+ tf->codes[348] = 0x0000000b364429cdUL;
+ tf->codes[349] = 0x00000008b53a7e34UL;
+ tf->codes[350] = 0x0000000be90ccefaUL;
+ tf->codes[351] = 0x0000000b0507dfa2UL;
+ tf->codes[352] = 0x000000000d525e90UL;
+ tf->codes[353] = 0x00000005c549eecdUL;
+ tf->codes[354] = 0x0000000e3bf5c446UL;
+ tf->codes[355] = 0x0000000936c6d936UL;
+ tf->codes[356] = 0x00000009747172d9UL;
+ tf->codes[357] = 0x0000000ca843c39fUL;
+ tf->codes[358] = 0x0000000d57f0d4eeUL;
+ tf->codes[359] = 0x00000002d5595f66UL;
+ tf->codes[360] = 0x0000000bfbb2462eUL;
+ tf->codes[361] = 0x0000000266727b98UL;
+ tf->codes[362] = 0x00000007ac679429UL;
+ tf->codes[363] = 0x000000026fc53732UL;
+ tf->codes[364] = 0x0000000656602d25UL;
+ tf->codes[365] = 0x00000002eb1a6a78UL;
+ tf->codes[366] = 0x00000004850392dbUL;
+ tf->codes[367] = 0x0000000e5b098af2UL;
+ tf->codes[368] = 0x0000000ab534c280UL;
+ tf->codes[369] = 0x00000009ce143f4aUL;
+ tf->codes[370] = 0x0000000f4b7cc9c2UL;
+ tf->codes[371] = 0x0000000035b8e0d6UL;
+ tf->codes[372] = 0x0000000871d5b08aUL;
+ tf->codes[373] = 0x00000005b958930aUL;
+ tf->codes[374] = 0x00000000b429a7faUL;
+ tf->codes[375] = 0x000000054d8d431aUL;
+ tf->codes[376] = 0x00000007d1b28291UL;
+ tf->codes[377] = 0x0000000a1e645021UL;
+ tf->codes[378] = 0x0000000b80da069dUL;
+ tf->codes[379] = 0x0000000eef3b5d28UL;
+ tf->codes[380] = 0x0000000263d3db6fUL;
+ tf->codes[381] = 0x000000009592d503UL;
+ tf->codes[382] = 0x00000004b9d86499UL;
+ tf->codes[383] = 0x00000006c8df9886UL;
+ tf->codes[384] = 0x0000000a3740ef11UL;
+ tf->codes[385] = 0x0000000c4963b6dcUL;
+ tf->codes[386] = 0x000000006da94672UL;
+ tf->codes[387] = 0x000000053b64bf9bUL;
+ tf->codes[388] = 0x0000000b2deb559dUL;
+ tf->codes[389] = 0x0000000f116ab7b2UL;
+ tf->codes[390] = 0x00000008ace1aa04UL;
+ tf->codes[391] = 0x00000008ea8c43a7UL;
+ tf->codes[392] = 0x00000006a4119dd3UL;
+ tf->codes[393] = 0x000000099d54e8d4UL;
+ tf->codes[394] = 0x0000000c969833d5UL;
+ tf->codes[395] = 0x0000000f554c7911UL;
+ tf->codes[396] = 0x00000003ade9e6b0UL;
+ tf->codes[397] = 0x00000006e1bc3776UL;
+ tf->codes[398] = 0x00000007916948c5UL;
+ tf->codes[399] = 0x0000000dbe7ee48cUL;
+ tf->codes[400] = 0x000000079484dca3UL;
+ tf->codes[401] = 0x0000000f992e3a70UL;
+ tf->codes[402] = 0x0000000884f81b73UL;
+ tf->codes[403] = 0x0000000c68777d88UL;
+ tf->codes[404] = 0x0000000603ee6fdaUL;
+ tf->codes[405] = 0x0000000728b98cb3UL;
+ tf->codes[406] = 0x0000000b12701684UL;
+ tf->codes[407] = 0x0000000d5f21e414UL;
+ tf->codes[408] = 0x0000000058652f15UL;
+ tf->codes[409] = 0x00000002dc8a6e8cUL;
+ tf->codes[410] = 0x00000004767396efUL;
+ tf->codes[411] = 0x0000000b8dc549caUL;
+ tf->codes[412] = 0x0000000f36b5a61aUL;
+ tf->codes[413] = 0x00000000d09ece7dUL;
+ tf->codes[414] = 0x0000000dda77175aUL;
+ tf->codes[415] = 0x000000005e9c56d1UL;
+ tf->codes[416] = 0x000000073e7a97c5UL;
+ tf->codes[417] = 0x0000000b21f9f9daUL;
+ tf->codes[418] = 0x0000000de3c9d2f4UL;
+ tf->codes[419] = 0x000000069504ae32UL;
+ tf->codes[420] = 0x000000077f40c546UL;
+ tf->codes[421] = 0x0000000ed1217de6UL;
+ tf->codes[422] = 0x00000003a1f88aedUL;
+ tf->codes[423] = 0x0000000e623a9a18UL;
+ tf->codes[424] = 0x00000000aeec67a8UL;
+ tf->codes[425] = 0x0000000bea83aa19UL;
+ tf->codes[426] = 0x000000092eeaf8bbUL;
+ tf->codes[427] = 0x0000000a5d08d12eUL;
+ tf->codes[428] = 0x0000000819a9bf38UL;
+ tf->codes[429] = 0x0000000473d4f6c6UL;
+ tf->codes[430] = 0x0000000b192431f5UL;
+ tf->codes[431] = 0x0000000a6c92b484UL;
+ tf->codes[432] = 0x00000007046885b5UL;
+ tf->codes[433] = 0x0000000b9ab08cf7UL;
+ tf->codes[434] = 0x0000000782d94cd9UL;
+ tf->codes[435] = 0x0000000f158032faUL;
+ tf->codes[436] = 0x0000000077f5e976UL;
+ tf->codes[437] = 0x000000012dda2281UL;
+ tf->codes[438] = 0x0000000e72417123UL;
+ tf->codes[439] = 0x00000003056de487UL;
+ tf->codes[440] = 0x0000000e3de9931aUL;
+ tf->codes[441] = 0x0000000eb3079ea4UL;
+ tf->codes[442] = 0x0000000e4420bad6UL;
+ tf->codes[443] = 0x0000000439c2e4b6UL;
+ tf->codes[444] = 0x000000047da4a615UL;
+ tf->codes[445] = 0x00000000d7cfdda3UL;
+ tf->codes[446] = 0x000000056afc5107UL;
+ tf->codes[447] = 0x0000000e978c5f8bUL;
+ tf->codes[448] = 0x00000005aede1266UL;
+ tf->codes[449] = 0x0000000af1b79719UL;
+ tf->codes[450] = 0x0000000f8b1b3239UL;
+ tf->codes[451] = 0x000000075e8845dbUL;
+ tf->codes[452] = 0x0000000bf1b4b93fUL;
+ tf->codes[453] = 0x0000000fd5341b54UL;
+ tf->codes[454] = 0x0000000a2373b2d3UL;
+ tf->codes[455] = 0x00000005967e672bUL;
+ tf->codes[456] = 0x0000000a2cc66e6dUL;
+ tf->codes[457] = 0x0000000b17028581UL;
+ tf->codes[458] = 0x0000000b54ad1f24UL;
+ tf->codes[459] = 0x0000000e91d22b84UL;
+ tf->codes[460] = 0x0000000de85c41f1UL;
+ tf->codes[461] = 0x000000053d588e6fUL;
+ tf->codes[462] = 0x0000000e9e407afcUL;
+ tf->codes[463] = 0x0000000fc6272bb3UL;
+ tf->codes[464] = 0x0000000a8ca0629aUL;
+ tf->codes[465] = 0x0000000b86665d04UL;
+ tf->codes[466] = 0x000000005a58fde9UL;
+ tf->codes[467] = 0x00000001855b427eUL;
+ tf->codes[468] = 0x0000000aabb42946UL;
+ tf->codes[469] = 0x0000000e204ca78dUL;
+ tf->codes[470] = 0x000000032897267bUL;
+ tf->codes[471] = 0x00000000a78d7ae2UL;
+ tf->codes[472] = 0x000000096536a598UL;
+ tf->codes[473] = 0x0000000bf2aea0a9UL;
+ tf->codes[474] = 0x00000000c9bcd56cUL;
+ tf->codes[475] = 0x000000081eb921eaUL;
+ tf->codes[476] = 0x00000002732fe125UL;
+ tf->codes[477] = 0x00000002eb69808dUL;
+ tf->codes[478] = 0x000000061f3bd153UL;
+ tf->codes[479] = 0x00000008ddf0168fUL;
+ tf->codes[480] = 0x0000000921d1d7eeUL;
+ tf->codes[481] = 0x00000002bd48ca40UL;
+ tf->codes[482] = 0x000000083ab154b8UL;
+ tf->codes[483] = 0x00000005f436aee4UL;
+ tf->codes[484] = 0x000000093dca0abcUL;
+ tf->codes[485] = 0x000000026d75ad1eUL;
+ tf->codes[486] = 0x0000000872a1ba54UL;
+ tf->codes[487] = 0x0000000373a9f700UL;
+ tf->codes[488] = 0x000000050d931f63UL;
+ tf->codes[489] = 0x000000012d2f512cUL;
+ tf->codes[490] = 0x0000000c6efdbb59UL;
+ tf->codes[491] = 0x000000088e99ed22UL;
+ tf->codes[492] = 0x0000000903b7f8acUL;
+ tf->codes[493] = 0x0000000a9da1210fUL;
+ tf->codes[494] = 0x0000000a2eba3d41UL;
+ tf->codes[495] = 0x0000000fe9cd615cUL;
+ tf->codes[496] = 0x0000000fb8911731UL;
+ tf->codes[497] = 0x00000001cab3defcUL;
+ tf->codes[498] = 0x0000000e6289b02dUL;
+ tf->codes[499] = 0x000000066d6a35b6UL;
+ tf->codes[500] = 0x0000000b0096a91aUL;
+ tf->codes[501] = 0x0000000c9afcc532UL;
+ tf->codes[502] = 0x000000080aebe5acUL;
+ tf->codes[503] = 0x0000000d2f2e9768UL;
+ tf->codes[504] = 0x0000000cc67edb56UL;
+ tf->codes[505] = 0x0000000a51e37f35UL;
+ tf->codes[506] = 0x00000006ac0eb6c3UL;
+ tf->codes[507] = 0x00000006af2a4aa1UL;
+ tf->codes[508] = 0x0000000e76290ecbUL;
+ tf->codes[509] = 0x000000037e738db9UL;
+ tf->codes[510] = 0x000000072d9b11c5UL;
+ tf->codes[511] = 0x000000076e613f46UL;
+ tf->codes[512] = 0x00000004f073278bUL;
+ tf->codes[513] = 0x00000000e1eea307UL;
+ tf->codes[514] = 0x0000000e9e8f9111UL;
+ tf->codes[515] = 0x00000000793ee6f5UL;
+ tf->codes[516] = 0x000000017304e15fUL;
+ tf->codes[517] = 0x00000007a3361104UL;
+ tf->codes[518] = 0x0000000731339958UL;
+ tf->codes[519] = 0x00000008daa6a511UL;
+ tf->codes[520] = 0x0000000a4037ef6bUL;
+ tf->codes[521] = 0x0000000210896f2fUL;
+ tf->codes[522] = 0x0000000afc535032UL;
+ tf->codes[523] = 0x0000000e3025a0f8UL;
+ tf->codes[524] = 0x000000063e21ba5fUL;
+ tf->codes[525] = 0x00000003ebb5b8c8UL;
+ tf->codes[526] = 0x0000000f9c6b06c3UL;
+ tf->codes[527] = 0x0000000ca95ee37eUL;
+ tf->codes[528] = 0x000000081f852bb4UL;
+ tf->codes[529] = 0x0000000d6895d823UL;
+ tf->codes[530] = 0x00000007040cca75UL;
+ tf->codes[531] = 0x00000004d66ec391UL;
+ tf->codes[532] = 0x00000004a216e588UL;
+ tf->codes[533] = 0x000000051d6c18ceUL;
+ tf->codes[534] = 0x000000047711c319UL;
+ tf->codes[535] = 0x00000006ae7f794cUL;
+ tf->codes[536] = 0x00000004abe694d7UL;
+ tf->codes[537] = 0x0000000bc96f6f6eUL;
+ tf->codes[538] = 0x000000057aa76cd2UL;
+ tf->codes[539] = 0x0000000f948f2648UL;
+ tf->codes[540] = 0x000000031bcd1bc3UL;
+ tf->codes[541] = 0x000000094c7b3f1dUL;
+ tf->codes[542] = 0x000000032eef86acUL;
+ tf->codes[543] = 0x0000000668f8ff2eUL;
+ tf->codes[544] = 0x00000006de170ab8UL;
+ tf->codes[545] = 0x00000009341b93e2UL;
+ tf->codes[546] = 0x0000000974e1c163UL;
+ tf->codes[547] = 0x000000073182af6dUL;
+ tf->codes[548] = 0x00000005d85fb48bUL;
+ tf->codes[549] = 0x000000078b257bdeUL;
+ tf->codes[550] = 0x0000000173d0eb29UL;
+ tf->codes[551] = 0x00000005add785d1UL;
+ tf->codes[552] = 0x0000000af6e83240UL;
+ tf->codes[553] = 0x0000000dce79166cUL;
+ tf->codes[554] = 0x000000022716840bUL;
+ tf->codes[555] = 0x00000007b408f1d9UL;
+ tf->codes[556] = 0x0000000de43a217eUL;
+ tf->codes[557] = 0x000000036e10fb6eUL;
+ tf->codes[558] = 0x0000000ea9a83ddfUL;
+ tf->codes[559] = 0x00000004dcf50162UL;
+ tf->codes[560] = 0x00000009aab07a8bUL;
+ tf->codes[561] = 0x0000000281364431UL;
+ tf->codes[562] = 0x00000008392dd46eUL;
+ tf->codes[563] = 0x00000000945e6aceUL;
+ tf->codes[564] = 0x00000008d07b3a82UL;
+ tf->codes[565] = 0x0000000d012f1990UL;
+ tf->codes[566] = 0x0000000b7098acc7UL;
+ tf->codes[567] = 0x0000000c2fcfa16cUL;
+ tf->codes[568] = 0x00000001e7c731a9UL;
+ tf->codes[569] = 0x0000000f16ea68eeUL;
+ tf->codes[570] = 0x00000002fa69cb03UL;
+ tf->codes[571] = 0x00000004cee1f92bUL;
+ tf->codes[572] = 0x00000001e20cfda2UL;
+ tf->codes[573] = 0x00000009e9d1ef4dUL;
+ tf->codes[574] = 0x0000000e83358a6dUL;
+ tf->codes[575] = 0x000000059a873d48UL;
+ tf->codes[576] = 0x0000000a6842b671UL;
+ tf->codes[577] = 0x00000006885bdbefUL;
+ tf->codes[578] = 0x000000073e4014faUL;
+ tf->codes[579] = 0x00000007b9954840UL;
+ tf->codes[580] = 0x0000000548157ffdUL;
+ tf->codes[581] = 0x00000008853a8c5dUL;
+ tf->codes[582] = 0x0000000eb8874fe0UL;
+ tf->codes[583] = 0x0000000b25d4f257UL;
+ tf->codes[584] = 0x000000036b447da5UL;
+ tf->codes[585] = 0x000000071d87958fUL;
+ tf->codes[586] = 0x0000000eedd91553UL;
+ tf->codes[587] = 0x00000004af23612aUL;
+ tf->codes[588] = 0x0000000278329eacUL;
+ tf->codes[589] = 0x0000000191121b76UL;
+ tf->codes[590] = 0x00000006e691175dUL;
+ tf->codes[591] = 0x00000000bd140329UL;
+ tf->codes[592] = 0x00000006ed4532ceUL;
+ tf->codes[593] = 0x0000000d5e3c8ff4UL;
+ tf->codes[594] = 0x0000000e26c64033UL;
+ tf->codes[595] = 0x0000000494a2097bUL;
+ tf->codes[596] = 0x0000000f5e36d440UL;
+ tf->codes[597] = 0x0000000b7818d202UL;
+ tf->codes[598] = 0x0000000a63548c34UL;
+ tf->codes[599] = 0x0000000682f0bdfdUL;
+ tf->codes[600] = 0x00000003d3c65c17UL;
+ tf->codes[601] = 0x00000004c4399ae7UL;
+ tf->codes[602] = 0x00000006b18e67ffUL;
+ tf->codes[603] = 0x00000004778211a3UL;
+ tf->codes[604] = 0x00000004089b2dd5UL;
+ tf->codes[605] = 0x000000021edee850UL;
+ tf->codes[606] = 0x0000000739cede72UL;
+ tf->codes[607] = 0x0000000858dfc744UL;
+ tf->codes[608] = 0x00000004bc5dba6cUL;
+ tf->codes[609] = 0x000000021cbd3bdcUL;
+ tf->codes[610] = 0x0000000ac83de313UL;
+ tf->codes[611] = 0x00000006135f08daUL;
+ tf->codes[612] = 0x0000000d3a3a9f0bUL;
+ tf->codes[613] = 0x0000000eb2716099UL;
+ tf->codes[614] = 0x000000040b88e413UL;
+ tf->codes[615] = 0x0000000992442a25UL;
+ tf->codes[616] = 0x0000000c639de695UL;
+ tf->codes[617] = 0x0000000683bcc7c7UL;
+ tf->codes[618] = 0x00000006245fc74fUL;
+ tf->codes[619] = 0x0000000543766bd5UL;
+ tf->codes[620] = 0x0000000375356569UL;
+ tf->codes[621] = 0x0000000fa45b7a88UL;
+ tf->codes[622] = 0x00000009f29b1207UL;
+ tf->codes[623] = 0x0000000ba245457cUL;
+ tf->codes[624] = 0x00000005b98e5ec9UL;
+ tf->codes[625] = 0x0000000204aca6b6UL;
+ tf->codes[626] = 0x0000000d52e9605bUL;
+ tf->codes[627] = 0x0000000504a40d28UL;
+ tf->codes[628] = 0x0000000ab6e1695eUL;
+ tf->codes[629] = 0x0000000a26481ebbUL;
+ tf->codes[630] = 0x00000009455ec341UL;
+ tf->codes[631] = 0x00000002f05f98e9UL;
+ tf->codes[632] = 0x0000000ead83365cUL;
+ tf->codes[633] = 0x0000000b928d8486UL;
+ tf->codes[634] = 0x00000007b860de0bUL;
+ tf->codes[635] = 0x0000000964ef7da2UL;
+ tf->codes[636] = 0x00000002422962b9UL;
+ tf->codes[637] = 0x000000028f5ddfb2UL;
+ tf->codes[638] = 0x00000008c5c63713UL;
+ tf->codes[639] = 0x00000009068c6494UL;
+ tf->codes[640] = 0x000000050aad5744UL;
+ tf->codes[641] = 0x000000093e7cca30UL;
+ tf->codes[642] = 0x00000009825e8b8fUL;
+ tf->codes[643] = 0x00000003a8b4947dUL;
+ tf->codes[644] = 0x0000000fa06737b5UL;
+ tf->codes[645] = 0x0000000cb9c963e8UL;
+ tf->codes[646] = 0x000000091a2bc332UL;
+ tf->codes[647] = 0x0000000284666b59UL;
+ tf->codes[648] = 0x0000000ceb82a1c8UL;
+ tf->codes[649] = 0x0000000a2fe9f06aUL;
+ tf->codes[650] = 0x000000006fa50365UL;
+ tf->codes[651] = 0x000000019aa747faUL;
+ tf->codes[652] = 0x0000000a6e117dc2UL;
+ tf->codes[653] = 0x00000002b3810910UL;
+ tf->codes[654] = 0x0000000ff7e857b2UL;
+ tf->codes[655] = 0x0000000048b55c3eUL;
+ tf->codes[656] = 0x0000000975456ac2UL;
+ tf->codes[657] = 0x0000000ad200ed37UL;
+ tf->codes[658] = 0x0000000e4d4d86efUL;
+ tf->codes[659] = 0x0000000b3ec62491UL;
+ tf->codes[660] = 0x00000008cd465c4eUL;
+ tf->codes[661] = 0x00000008a2be2d94UL;
+ tf->codes[662] = 0x00000005a9f7d648UL;
+ tf->codes[663] = 0x000000059e067a85UL;
+ tf->codes[664] = 0x0000000b5c35327eUL;
+ tf->codes[665] = 0x0000000c4ca8714eUL;
+ tf->codes[666] = 0x00000000e927a04cUL;
+ tf->codes[667] = 0x0000000f71eac628UL;
+ tf->codes[668] = 0x0000000109354e62UL;
+ tf->codes[669] = 0x00000001037b1a5bUL;
+ tf->codes[670] = 0x000000026c27f893UL;
+ tf->codes[671] = 0x00000003597fa385UL;
+ tf->codes[672] = 0x0000000ee24b62efUL;
+ tf->codes[673] = 0x00000004b31f921cUL;
+ tf->codes[674] = 0x0000000650244e5dUL;
+ tf->codes[675] = 0x0000000cfec64526UL;
+ tf->codes[676] = 0x0000000cd4bb0a21UL;
+ tf->codes[677] = 0x0000000648c56197UL;
+ tf->codes[678] = 0x0000000bd95056f8UL;
+ tf->codes[679] = 0x0000000983c8c183UL;
+ tf->codes[680] = 0x0000000ddc662f22UL;
+ tf->codes[681] = 0x00000005631bb980UL;
+ tf->codes[682] = 0x00000001203f56f3UL;
+ tf->codes[683] = 0x00000006a0c37549UL;
+ tf->codes[684] = 0x0000000a59baa8a4UL;
+ tf->codes[685] = 0x00000006a23a5068UL;
+ tf->codes[686] = 0x0000000ad609c354UL;
+ tf->codes[687] = 0x0000000f6f6d5e74UL;
+ tf->codes[688] = 0x000000036bc95f79UL;
+ tf->codes[689] = 0x0000000424c92c62UL;
+ tf->codes[690] = 0x00000003692abf50UL;
+ tf->codes[691] = 0x00000000a4bc460dUL;
+ tf->codes[692] = 0x00000001b4434b89UL;
+ tf->codes[693] = 0x00000004eb31302dUL;
+ tf->codes[694] = 0x00000009a3a891f9UL;
+ tf->codes[695] = 0x000000093af8d5e7UL;
+ tf->codes[696] = 0x0000000ef60bfa02UL;
+ tf->codes[697] = 0x0000000607a378d6UL;
+ tf->codes[698] = 0x00000005a5a7d835UL;
+ tf->codes[699] = 0x0000000536c0f467UL;
+ tf->codes[700] = 0x000000011f66a7feUL;
+ tf->codes[701] = 0x000000074c7c43c5UL;
+ tf->codes[702] = 0x000000066eae7c29UL;
+ tf->codes[703] = 0x0000000f5a785d2cUL;
+ tf->codes[704] = 0x0000000d3948a5c0UL;
+ tf->codes[705] = 0x0000000ec1094aa4UL;
+ tf->codes[706] = 0x0000000fcad61c19UL;
+ tf->codes[707] = 0x000000049ca7108aUL;
+ tf->codes[708] = 0x000000077437f4b6UL;
+ tf->codes[709] = 0x0000000553083d4aUL;
+ tf->codes[710] = 0x00000005c26c14cdUL;
+ tf->codes[711] = 0x00000006eb4caceeUL;
+ tf->codes[712] = 0x0000000e8aded63cUL;
+ tf->codes[713] = 0x0000000132aa4b5cUL;
+ tf->codes[714] = 0x000000057603a19eUL;
+ tf->codes[715] = 0x0000000ee359dda3UL;
+ tf->codes[716] = 0x0000000d0f5ea330UL;
+ tf->codes[717] = 0x000000046b0f0b1fUL;
+ tf->codes[718] = 0x0000000d47cbfc81UL;
+ tf->codes[719] = 0x00000003ed1b37b0UL;
+ tf->codes[720] = 0x00000004c8c752d8UL;
+ tf->codes[721] = 0x0000000ac202044bUL;
+ tf->codes[722] = 0x0000000207f16128UL;
+ tf->codes[723] = 0x000000053f5c3981UL;
+ tf->codes[724] = 0x000000070e1a33a2UL;
+ tf->codes[725] = 0x0000000ed8348baaUL;
+ tf->codes[726] = 0x0000000798f94a3eUL;
+ tf->codes[727] = 0x00000008896c890eUL;
+ tf->codes[728] = 0x0000000521425a3fUL;
+ tf->codes[729] = 0x0000000c8329e9eaUL;
+ tf->codes[730] = 0x000000041f238ba5UL;
+ tf->codes[731] = 0x00000001093d3c81UL;
+ tf->codes[732] = 0x0000000cab628e90UL;
+ tf->codes[733] = 0x0000000a1b4bf356UL;
+ tf->codes[734] = 0x000000092eee2fceUL;
+ tf->codes[735] = 0x000000059d35b9afUL;
+ tf->codes[736] = 0x00000002176e9f53UL;
+ tf->codes[737] = 0x00000007c9abfb89UL;
+ tf->codes[738] = 0x0000000b06d107e9UL;
+ tf->codes[739] = 0x0000000e94c318d5UL;
+ tf->codes[740] = 0x00000002f397ae30UL;
+ tf->codes[741] = 0x0000000d7e5a1a48UL;
+ tf->codes[742] = 0x000000098c4abc47UL;
+ tf->codes[743] = 0x0000000574737c29UL;
+ tf->codes[744] = 0x0000000d7f5401b2UL;
+ tf->codes[745] = 0x0000000852b87bc6UL;
+ tf->codes[746] = 0x00000001180fa957UL;
+ tf->codes[747] = 0x0000000501c63328UL;
+ tf->codes[748] = 0x0000000fd28c0d13UL;
+ tf->codes[749] = 0x0000000f764aa079UL;
+ tf->codes[750] = 0x0000000c8a6f8c5aUL;
+ tf->codes[751] = 0x0000000975b10240UL;
+ tf->codes[752] = 0x00000006bd33e4c0UL;
+ tf->codes[753] = 0x0000000a1113e39cUL;
+ tf->codes[754] = 0x0000000abcfa8fb8UL;
+ tf->codes[755] = 0x0000000149ea1facUL;
+ tf->codes[756] = 0x0000000f6443556fUL;
+ tf->codes[757] = 0x0000000959da07e7UL;
+ tf->codes[758] = 0x00000004721a2ac4UL;
+ tf->codes[759] = 0x000000030bde0f15UL;
+ tf->codes[760] = 0x0000000c7c4fdef8UL;
+ tf->codes[761] = 0x0000000b7feb4465UL;
+ tf->codes[762] = 0x000000056675073cUL;
+ tf->codes[763] = 0x000000096f3f57b9UL;
+ tf->codes[764] = 0x0000000876f0386eUL;
+ tf->codes[765] = 0x0000000393f0a7e8UL;
+ tf->codes[766] = 0x000000032b40ebd6UL;
+ tf->codes[767] = 0x000000010a11346aUL;
+ tf->codes[768] = 0x0000000fb81f48aeUL;
+ tf->codes[769] = 0x00000000a892877eUL;
+ tf->codes[770] = 0x000000086f636e08UL;
+ tf->codes[771] = 0x00000004fbc4d72bUL;
+ tf->codes[772] = 0x0000000561d5f314UL;
+ tf->codes[773] = 0x0000000ca18e2835UL;
+ tf->codes[774] = 0x00000007e6f519f5UL;
+ tf->codes[775] = 0x00000008b94e7983UL;
+ tf->codes[776] = 0x0000000619adfaf3UL;
+ tf->codes[777] = 0x00000004c9ddbbabUL;
+ tf->codes[778] = 0x0000000cb6a461f2UL;
+ tf->codes[779] = 0x00000000f6e22456UL;
+ tf->codes[780] = 0x0000000858c9b401UL;
+ tf->codes[781] = 0x000000092dc1b3b8UL;
+ tf->codes[782] = 0x00000003a783615bUL;
+ tf->codes[783] = 0x0000000c74b66f67UL;
+ tf->codes[784] = 0x0000000ea90891bcUL;
+ tf->codes[785] = 0x000000031a829e4bUL;
+ tf->codes[786] = 0x00000007bd38f505UL;
+ tf->codes[787] = 0x0000000b4476ea80UL;
+ tf->codes[788] = 0x00000001af371feaUL;
+ tf->codes[789] = 0x000000084894ff56UL;
+ tf->codes[790] = 0x00000000537584dfUL;
+ tf->codes[791] = 0x0000000d4ebdd1d0UL;
+ tf->codes[792] = 0x0000000b43cc192bUL;
+ tf->codes[793] = 0x000000076700d287UL;
+ tf->codes[794] = 0x0000000aaad9fa58UL;
+ tf->codes[795] = 0x0000000fa511710fUL;
+ tf->codes[796] = 0x00000006e54699e5UL;
+ tf->codes[797] = 0x00000006d73391aeUL;
+ tf->codes[798] = 0x00000003c2f1fb49UL;
+ tf->codes[799] = 0x00000004fbd96a75UL;
+ tf->codes[800] = 0x0000000252e6304bUL;
+ tf->codes[801] = 0x0000000e72826214UL;
+ tf->codes[802] = 0x00000008fe6c9336UL;
+ tf->codes[803] = 0x0000000326397743UL;
+ tf->codes[804] = 0x00000000e03bc524UL;
+ tf->codes[805] = 0x0000000dedac9594UL;
+ tf->codes[806] = 0x000000004ff19096UL;
+ tf->codes[807] = 0x0000000409b4cdbbUL;
+ tf->codes[808] = 0x0000000b15921888UL;
+ tf->codes[809] = 0x0000000a259bcd6dUL;
+ tf->codes[810] = 0x000000043c67f305UL;
+ tf->codes[811] = 0x00000001dc65dcecUL;
+ tf->codes[812] = 0x00000001730b4f85UL;
+ tf->codes[813] = 0x0000000f8a48f16aUL;
+ tf->codes[814] = 0x000000057a30e743UL;
+ tf->codes[815] = 0x000000005afd9839UL;
+ tf->codes[816] = 0x0000000682d5f3aeUL;
+ tf->codes[817] = 0x00000000b694337eUL;
+ tf->codes[818] = 0x0000000758c7dacfUL;
+ tf->codes[819] = 0x000000018fa1ae7dUL;
+ tf->codes[820] = 0x00000005ba9b5984UL;
+ tf->codes[821] = 0x000000032e1d45ddUL;
+ tf->codes[822] = 0x0000000672f05518UL;
+ tf->codes[823] = 0x0000000382ffc5b1UL;
+ tf->codes[824] = 0x00000008f0b08f33UL;
+ tf->codes[825] = 0x0000000b4a4d9ff0UL;
+ tf->codes[826] = 0x000000053ba0f980UL;
+ tf->codes[827] = 0x00000002ac0751fbUL;
+ tf->codes[828] = 0x0000000ab005de73UL;
+ tf->codes[829] = 0x000000061ab7be9bUL;
+ tf->codes[830] = 0x000000063078c9adUL;
+ tf->codes[831] = 0x000000027659d148UL;
+ tf->codes[832] = 0x0000000c653c684fUL;
+ tf->codes[833] = 0x0000000ecee05017UL;
+ tf->codes[834] = 0x000000024378ce5eUL;
+ tf->codes[835] = 0x0000000d0f7e5bacUL;
+ tf->codes[836] = 0x00000008b4bf4199UL;
+ tf->codes[837] = 0x0000000b7aa495fbUL;
+ tf->codes[838] = 0x0000000f3d6b78a5UL;
+ tf->codes[839] = 0x000000098bab1024UL;
+ tf->codes[840] = 0x00000006b4971fadUL;
+ tf->codes[841] = 0x00000000723d6c89UL;
+ tf->codes[842] = 0x0000000a17071a75UL;
+ tf->codes[843] = 0x0000000d55a301f4UL;
+ tf->codes[844] = 0x0000000988de925bUL;
+ tf->codes[845] = 0x00000007ca276f45UL;
+ tf->codes[846] = 0x0000000321b6e484UL;
+ tf->codes[847] = 0x0000000316149ed6UL;
+ tf->codes[848] = 0x0000000b8dba5bb9UL;
+ tf->codes[849] = 0x0000000aad4df3f4UL;
+ tf->codes[850] = 0x0000000178e204f5UL;
+ tf->codes[851] = 0x000000095cd2e357UL;
+ tf->codes[852] = 0x000000073fb8a733UL;
+ tf->codes[853] = 0x000000084ca10c86UL;
+ tf->codes[854] = 0x000000089498492dUL;
+ tf->codes[855] = 0x00000005b9bdf383UL;
+ tf->codes[856] = 0x000000066c58bb10UL;
+ tf->codes[857] = 0x0000000f153ac21eUL;
+ tf->codes[858] = 0x00000007ca5d3b04UL;
+ tf->codes[859] = 0x0000000637a521c7UL;
+ tf->codes[860] = 0x0000000b5ed589c1UL;
+ tf->codes[861] = 0x0000000d0a3c644eUL;
+ tf->codes[862] = 0x00000003d5d0754fUL;
+ tf->codes[863] = 0x0000000c6b901174UL;
+ tf->codes[864] = 0x00000003e96fd3edUL;
+ tf->codes[865] = 0x000000079c2fdf8cUL;
+ tf->codes[866] = 0x00000006594ae371UL;
+ tf->codes[867] = 0x00000003504fd77aUL;
+ tf->codes[868] = 0x000000032b81dcc7UL;
+ tf->codes[869] = 0x000000057b4f3e35UL;
+ tf->codes[870] = 0x00000004c9808072UL;
+ tf->codes[871] = 0x0000000a06c10993UL;
+ tf->codes[872] = 0x0000000b059666afUL;
+ tf->codes[873] = 0x000000072b69c034UL;
+ tf->codes[874] = 0x0000000e33ae836eUL;
+ tf->codes[875] = 0x0000000ad6cadf0dUL;
+ tf->codes[876] = 0x000000019573ace1UL;
+ tf->codes[877] = 0x0000000d562fd1e7UL;
+ tf->codes[878] = 0x0000000847804d9dUL;
+ tf->codes[879] = 0x00000009e32f6734UL;
+ tf->codes[880] = 0x00000002355c580fUL;
+ tf->codes[881] = 0x00000002f177b8d6UL;
+ tf->codes[882] = 0x0000000d689ac650UL;
+ tf->codes[883] = 0x0000000071b70abcUL;
+ tf->codes[884] = 0x0000000254915730UL;
+ tf->codes[885] = 0x0000000c5934f949UL;
+ tf->codes[886] = 0x0000000134d59d09UL;
+ tf->codes[887] = 0x00000002c731fb06UL;
+ tf->codes[888] = 0x0000000d90c6c5cbUL;
+ tf->codes[889] = 0x0000000cc3f9bca4UL;
+ tf->codes[890] = 0x0000000bf078980cUL;
+ tf->codes[891] = 0x000000080838e95aUL;
+ tf->codes[892] = 0x00000005ccd6f054UL;
+ tf->codes[893] = 0x00000000378bae56UL;
+ tf->codes[894] = 0x00000008d1ddb978UL;
+ tf->codes[895] = 0x000000093b875cf4UL;
+ tf->codes[896] = 0x0000000e2ce90bc6UL;
+ tf->codes[897] = 0x0000000ec291b91bUL;
+ tf->codes[898] = 0x0000000d0a11bdc1UL;
+ tf->codes[899] = 0x0000000751be7244UL;
+ tf->codes[900] = 0x0000000579fcd29eUL;
+ tf->codes[901] = 0x0000000f76e5ccb1UL;
+ tf->codes[902] = 0x0000000eb33da184UL;
+ tf->codes[903] = 0x0000000c0ac75b0fUL;
+ tf->codes[904] = 0x000000015454fb33UL;
+ tf->codes[905] = 0x00000008b92a411cUL;
+ tf->codes[906] = 0x00000007da34b476UL;
+ tf->codes[907] = 0x00000004f413d45eUL;
+ tf->codes[908] = 0x000000010ec1dbeaUL;
+ tf->codes[909] = 0x0000000650739b93UL;
+ tf->codes[910] = 0x0000000315e08a31UL;
+ tf->codes[911] = 0x0000000d4fd5f1bdUL;
+ tf->codes[912] = 0x0000000b5058a126UL;
+ tf->codes[913] = 0x000000020d5cb63bUL;
+ tf->codes[914] = 0x0000000c1598dfe7UL;
+ tf->codes[915] = 0x0000000a0a2a338dUL;
+ tf->codes[916] = 0x0000000e29af7686UL;
+ tf->codes[917] = 0x000000083fd0cac9UL;
+ tf->codes[918] = 0x000000014a8094d8UL;
+ tf->codes[919] = 0x0000000816e0afa3UL;
+ tf->codes[920] = 0x0000000499ef5d2cUL;
+ tf->codes[921] = 0x0000000bddbd0d95UL;
+ tf->codes[922] = 0x0000000a30839ca9UL;
+ tf->codes[923] = 0x00000004fd33fb4cUL;
+ tf->codes[924] = 0x0000000ef63557b7UL;
+ tf->codes[925] = 0x0000000535f06ab2UL;
+ tf->codes[926] = 0x00000000d47d352eUL;
+ tf->codes[927] = 0x0000000a371e6dc4UL;
+ tf->codes[928] = 0x00000008ac914b17UL;
+ tf->codes[929] = 0x00000006ba9d69d7UL;
+ tf->codes[930] = 0x0000000096da89aaUL;
+ tf->codes[931] = 0x000000065bbd5d14UL;
+ tf->codes[932] = 0x0000000d41d2c5c4UL;
+ tf->codes[933] = 0x000000052a283583UL;
+ tf->codes[934] = 0x00000004c1f56d26UL;
+ tf->codes[935] = 0x000000086cd9984aUL;
+ tf->codes[936] = 0x000000026cbcedc6UL;
+ tf->codes[937] = 0x00000001aa0eaa03UL;
+ tf->codes[938] = 0x0000000b95d5ad2cUL;
+ tf->codes[939] = 0x0000000e2eb56b20UL;
+ tf->codes[940] = 0x0000000ff49d9d5cUL;
+ tf->codes[941] = 0x0000000cce338378UL;
+ tf->codes[942] = 0x0000000330e9fac7UL;
+ tf->codes[943] = 0x0000000e2f53974aUL;
+ tf->codes[944] = 0x0000000668d1c6d5UL;
+ tf->codes[945] = 0x0000000eca0ba751UL;
+ tf->codes[946] = 0x00000008d48ab5e6UL;
+ tf->codes[947] = 0x0000000d205e18cdUL;
+ tf->codes[948] = 0x00000001c391633cUL;
+ tf->codes[949] = 0x0000000ef5d02e5fUL;
+ tf->codes[950] = 0x0000000d12bb5f20UL;
+ tf->codes[951] = 0x0000000323215199UL;
+ tf->codes[952] = 0x000000088f5b3ffcUL;
+ tf->codes[953] = 0x0000000931445f29UL;
+ tf->codes[954] = 0x0000000b893cb727UL;
+ tf->codes[955] = 0x000000032851ecc0UL;
+ tf->codes[956] = 0x000000080b44d81bUL;
+ tf->codes[957] = 0x00000005aa48da98UL;
+ tf->codes[958] = 0x000000046d1e1284UL;
+ tf->codes[959] = 0x00000004c837ba14UL;
+ tf->codes[960] = 0x0000000eb22c26deUL;
+ tf->codes[961] = 0x0000000e51e9d246UL;
+ tf->codes[962] = 0x00000008d03deee6UL;
+ tf->codes[963] = 0x00000005af8e0909UL;
+ tf->codes[964] = 0x0000000bde9773a4UL;
+ tf->codes[965] = 0x0000000bf611cabfUL;
+ tf->codes[966] = 0x0000000d24ac96e7UL;
+ tf->codes[967] = 0x00000009fe919318UL;
+ tf->codes[968] = 0x000000050d0206a6UL;
+ tf->codes[969] = 0x0000000b43b9741cUL;
+ tf->codes[970] = 0x0000000ba48d4fb3UL;
+ tf->codes[971] = 0x00000006bccd7290UL;
+ tf->codes[972] = 0x00000008bc6bfb9cUL;
+ tf->codes[973] = 0x0000000e5a036c9fUL;
+ tf->codes[974] = 0x0000000a80a2cfeeUL;
+ tf->codes[975] = 0x0000000c193655a7UL;
+ tf->codes[976] = 0x00000007c8e5170dUL;
+ tf->codes[977] = 0x00000006141edbbbUL;
+ tf->codes[978] = 0x00000004d6b990dcUL;
+ tf->codes[979] = 0x0000000cc49b5702UL;
+ tf->codes[980] = 0x00000002343fef58UL;
+ tf->codes[981] = 0x0000000d50cb593cUL;
+ tf->codes[982] = 0x00000004248a60cdUL;
+ tf->codes[983] = 0x0000000901cfbd4cUL;
+ tf->codes[984] = 0x000000064a4c8736UL;
+ tf->codes[985] = 0x00000001b2dcbaeaUL;
+ tf->codes[986] = 0x0000000d691e5f4cUL;
+ tf->codes[987] = 0x0000000df352a493UL;
+ tf->codes[988] = 0x00000001991ac7daUL;
+ tf->codes[989] = 0x00000004c4879f45UL;
+ tf->codes[990] = 0x00000009b34aadeeUL;
+ tf->codes[991] = 0x000000052bb3db0dUL;
+ tf->codes[992] = 0x00000007b9a8c9d3UL;
+ tf->codes[993] = 0x0000000d7ce6e47eUL;
+ tf->codes[994] = 0x0000000ec0b922d8UL;
+ tf->codes[995] = 0x00000008079cab6bUL;
+ tf->codes[996] = 0x0000000abadc8899UL;
+ tf->codes[997] = 0x00000000f57b93b7UL;
+ tf->codes[998] = 0x000000005c4ef219UL;
+ tf->codes[999] = 0x0000000d7a438d49UL;
+ tf->codes[1000] = 0x0000000f55ecca97UL;
+ tf->codes[1001] = 0x0000000d07899f1dUL;
+ tf->codes[1002] = 0x0000000260947d6cUL;
+ tf->codes[1003] = 0x0000000ffbd21ab6UL;
+ tf->codes[1004] = 0x0000000d04ff923eUL;
+ tf->codes[1005] = 0x0000000964b72033UL;
+ tf->codes[1006] = 0x000000031ac3fd7eUL;
+ tf->codes[1007] = 0x0000000d2c52e2c4UL;
+ tf->codes[1008] = 0x0000000799a640efUL;
+ tf->codes[1009] = 0x000000098dd061edUL;
+ tf->codes[1010] = 0x00000005cb2ab7b8UL;
+ tf->codes[1011] = 0x000000072f3881c8UL;
+ tf->codes[1012] = 0x0000000e65ed1164UL;
+ tf->codes[1013] = 0x000000034fa0bd5bUL;
+ tf->codes[1014] = 0x000000064f9823cdUL;
+ tf->codes[1015] = 0x00000003797e1ac0UL;
+ tf->codes[1016] = 0x00000002fb8a4751UL;
+ tf->codes[1017] = 0x00000006f347342eUL;
+ tf->codes[1018] = 0x000000022dd7ea0aUL;
+ tf->codes[1019] = 0x0000000b19b65e57UL;
+ tf->codes[1020] = 0x000000044fe83e8aUL;
+ tf->codes[1021] = 0x000000007732732eUL;
+ tf->codes[1022] = 0x000000064de20ed7UL;
+ tf->codes[1023] = 0x000000006c9ea834UL;
+ tf->codes[1024] = 0x00000008ce066650UL;
+ tf->codes[1025] = 0x0000000c2a685ff0UL;
+ tf->codes[1026] = 0x000000064f19b01fUL;
+ tf->codes[1027] = 0x0000000491ab8a88UL;
+ tf->codes[1028] = 0x000000041212fe5aUL;
+ tf->codes[1029] = 0x00000006f9916f3bUL;
+ tf->codes[1030] = 0x0000000694f72e71UL;
+ tf->codes[1031] = 0x0000000ad7a5b35eUL;
+ tf->codes[1032] = 0x0000000f62795292UL;
+ tf->codes[1033] = 0x0000000c8cdc3d3aUL;
+ tf->codes[1034] = 0x0000000fbc6b3518UL;
+ tf->codes[1035] = 0x000000067b631901UL;
+ tf->codes[1036] = 0x00000005b5ba79d5UL;
+ tf->codes[1037] = 0x0000000f4fadebddUL;
+ tf->codes[1038] = 0x0000000ac7c802e7UL;
+ tf->codes[1039] = 0x0000000385712d9dUL;
+ tf->codes[1040] = 0x000000064bd375b4UL;
+ tf->codes[1041] = 0x0000000c9a11df70UL;
+ tf->codes[1042] = 0x000000088355bf31UL;
+ tf->codes[1043] = 0x0000000606ffbb0aUL;
+ tf->codes[1044] = 0x0000000bda93c2d5UL;
+ tf->codes[1045] = 0x00000007c5f94f0aUL;
+ tf->codes[1046] = 0x000000076fe26501UL;
+ tf->codes[1047] = 0x00000005d8b9153cUL;
+ tf->codes[1048] = 0x0000000886bbb218UL;
+ tf->codes[1049] = 0x0000000acee2fecaUL;
+ tf->codes[1050] = 0x00000002ad19a925UL;
+ tf->codes[1051] = 0x000000083b97855cUL;
+ tf->codes[1052] = 0x0000000d36608312UL;
+ tf->codes[1053] = 0x00000008ac60dbc7UL;
+ tf->codes[1054] = 0x00000000885c8f58UL;
+ tf->codes[1055] = 0x00000008abbdf891UL;
+ tf->codes[1056] = 0x0000000ea1602271UL;
+ tf->codes[1057] = 0x0000000ad654fee1UL;
+ tf->codes[1058] = 0x00000006c461195eUL;
+ tf->codes[1059] = 0x00000005eeb1a327UL;
+ tf->codes[1060] = 0x000000018d743962UL;
+ tf->codes[1061] = 0x00000001fc7c55a5UL;
+ tf->codes[1062] = 0x0000000aba749670UL;
+ tf->codes[1063] = 0x00000009c9a59c60UL;
+ tf->codes[1064] = 0x00000006e5bafc06UL;
+ tf->codes[1065] = 0x000000096977db12UL;
+ tf->codes[1066] = 0x0000000a97b6ebfaUL;
+ tf->codes[1067] = 0x000000063d2d9da6UL;
+ tf->codes[1068] = 0x0000000fab00cd60UL;
+ tf->codes[1069] = 0x0000000d7bdf4632UL;
+ tf->codes[1070] = 0x0000000f83878d59UL;
+ tf->codes[1071] = 0x0000000b1c2c462eUL;
+ tf->codes[1072] = 0x000000014e5144a7UL;
+ tf->codes[1073] = 0x0000000f4a909b28UL;
+ tf->codes[1074] = 0x0000000e979a185bUL;
+ tf->codes[1075] = 0x0000000908090a64UL;
+ tf->codes[1076] = 0x000000099eccd798UL;
+ tf->codes[1077] = 0x0000000348780a96UL;
+ tf->codes[1078] = 0x0000000fdc7ad169UL;
+ tf->codes[1079] = 0x0000000a600c2e5bUL;
+ tf->codes[1080] = 0x0000000b0968cd98UL;
+ tf->codes[1081] = 0x00000001a45ec098UL;
+ tf->codes[1082] = 0x000000099118c1b4UL;
+ tf->codes[1083] = 0x00000008afa5cd5aUL;
+ tf->codes[1084] = 0x00000001db7e655eUL;
+ tf->codes[1085] = 0x00000009f637e452UL;
+ tf->codes[1086] = 0x00000009568504e3UL;
+ tf->codes[1087] = 0x0000000045b2a662UL;
+ tf->codes[1088] = 0x0000000f2a1455a2UL;
+ tf->codes[1089] = 0x00000006c1ca9e75UL;
+ tf->codes[1090] = 0x000000030a4a4639UL;
+ tf->codes[1091] = 0x0000000c6c2c1a30UL;
+ tf->codes[1092] = 0x000000087500b452UL;
+ tf->codes[1093] = 0x00000005e338bb2eUL;
+ tf->codes[1094] = 0x0000000d9dd11dffUL;
+ tf->codes[1095] = 0x00000008c4b5d012UL;
+ tf->codes[1096] = 0x00000008191194e0UL;
+ tf->codes[1097] = 0x0000000dd11db867UL;
+ tf->codes[1098] = 0x0000000c67c151ceUL;
+ tf->codes[1099] = 0x00000005cb1a00e4UL;
+ tf->codes[1100] = 0x0000000098b7a1c6UL;
+ tf->codes[1101] = 0x0000000369f35cd4UL;
+ tf->codes[1102] = 0x0000000ca2190bdbUL;
+ tf->codes[1103] = 0x00000006e14bb3b9UL;
+ tf->codes[1104] = 0x00000008d5692f8cUL;
+ tf->codes[1105] = 0x0000000ca4b2f4f8UL;
+ tf->codes[1106] = 0x0000000787f06877UL;
+ tf->codes[1107] = 0x00000008acbb8550UL;
+ tf->codes[1108] = 0x0000000535f4b56aUL;
+ tf->codes[1109] = 0x0000000f4caf7ecbUL;
+ tf->codes[1110] = 0x0000000d4615b258UL;
+ tf->codes[1111] = 0x0000000347ca7070UL;
+ tf->codes[1112] = 0x00000003c798c85dUL;
+ tf->codes[1113] = 0x0000000460506465UL;
+ tf->codes[1114] = 0x0000000870d0a5dcUL;
+ tf->codes[1115] = 0x00000006510b2464UL;
+ tf->codes[1116] = 0x0000000d1dba5544UL;
+ tf->codes[1117] = 0x0000000d57789a33UL;
+ tf->codes[1118] = 0x0000000e2417c5baUL;
+ tf->codes[1119] = 0x0000000b5ff8628cUL;
+ tf->codes[1120] = 0x0000000a3bb22787UL;
+ tf->codes[1121] = 0x0000000a16b64f34UL;
+ tf->codes[1122] = 0x0000000421e81d3dUL;
+ tf->codes[1123] = 0x000000035b4596a7UL;
+ tf->codes[1124] = 0x00000008d7a2dd7eUL;
+ tf->codes[1125] = 0x000000050b2d83faUL;
+ tf->codes[1126] = 0x00000009ea87e7c2UL;
+ tf->codes[1127] = 0x0000000d5055e752UL;
+ tf->codes[1128] = 0x0000000f96aa9da5UL;
+ tf->codes[1129] = 0x0000000b096e2a07UL;
+ tf->codes[1130] = 0x000000049970b44bUL;
+ tf->codes[1131] = 0x0000000867fb1518UL;
+ tf->codes[1132] = 0x00000005d0f5dba2UL;
+ tf->codes[1133] = 0x00000001b191d11eUL;
+ tf->codes[1134] = 0x00000008e839bb8fUL;
+ tf->codes[1135] = 0x00000001cd4aca15UL;
+ tf->codes[1136] = 0x0000000971ec5615UL;
+ tf->codes[1137] = 0x00000007d72a7ebdUL;
+ tf->codes[1138] = 0x00000008b1253bfbUL;
+ tf->codes[1139] = 0x0000000e11de1d25UL;
+ tf->codes[1140] = 0x00000000a7566839UL;
+ tf->codes[1141] = 0x0000000f4f3542e0UL;
+ tf->codes[1142] = 0x00000001ea791e32UL;
+ tf->codes[1143] = 0x000000032a84f759UL;
+ tf->codes[1144] = 0x0000000646f1844eUL;
+ tf->codes[1145] = 0x000000042af26809UL;
+ tf->codes[1146] = 0x00000001f4b464ffUL;
+ tf->codes[1147] = 0x0000000da684d2d9UL;
+ tf->codes[1148] = 0x0000000d854f5fb9UL;
+ tf->codes[1149] = 0x00000004d4d3e91aUL;
+ tf->codes[1150] = 0x00000005af3ef4e2UL;
+ tf->codes[1151] = 0x00000008a1ef5ce7UL;
+ tf->codes[1152] = 0x00000002354febf3UL;
+ tf->codes[1153] = 0x0000000b3c5a8944UL;
+ tf->codes[1154] = 0x000000098b62a144UL;
+ tf->codes[1155] = 0x00000009bdba0b4eUL;
+ tf->codes[1156] = 0x000000004aa99b42UL;
+ tf->codes[1157] = 0x00000008099ea151UL;
+ tf->codes[1158] = 0x00000002185463a3UL;
+ tf->codes[1159] = 0x0000000b0a1ae997UL;
+ tf->codes[1160] = 0x0000000e628d5770UL;
+ tf->codes[1161] = 0x0000000b40b5ac89UL;
+ tf->codes[1162] = 0x000000027213b17dUL;
+ tf->codes[1163] = 0x00000004d21db5b5UL;
+ tf->codes[1164] = 0x000000010d0748f7UL;
+ tf->codes[1165] = 0x00000002276c7876UL;
+ tf->codes[1166] = 0x0000000b98bee56dUL;
+ tf->codes[1167] = 0x0000000bd1ca6ae8UL;
+ tf->codes[1168] = 0x0000000824ab48faUL;
+ tf->codes[1169] = 0x0000000c6f35ae62UL;
+ tf->codes[1170] = 0x00000003547a563cUL;
+ tf->codes[1171] = 0x0000000f1fc0d824UL;
+ tf->codes[1172] = 0x000000058f55ed75UL;
+ tf->codes[1173] = 0x0000000aa9d0de01UL;
+ tf->codes[1174] = 0x00000004719dde60UL;
+ tf->codes[1175] = 0x0000000d5386b3ddUL;
+ tf->codes[1176] = 0x00000004d8d9f666UL;
+ tf->codes[1177] = 0x0000000aee36013bUL;
+ tf->codes[1178] = 0x0000000ba4ee322fUL;
+ tf->codes[1179] = 0x0000000898d2db4eUL;
+ tf->codes[1180] = 0x00000009fe364808UL;
+ tf->codes[1181] = 0x0000000bb13e8045UL;
+ tf->codes[1182] = 0x0000000be346d43aUL;
+ tf->codes[1183] = 0x0000000b4c9f886fUL;
+ tf->codes[1184] = 0x0000000c9a6f53b8UL;
+ tf->codes[1185] = 0x00000000ed5a7b6fUL;
+ tf->codes[1186] = 0x00000002a1fac740UL;
+ tf->codes[1187] = 0x0000000b8c134a59UL;
+ tf->codes[1188] = 0x0000000b1f773993UL;
+ tf->codes[1189] = 0x0000000c4d9d0025UL;
+ tf->codes[1190] = 0x0000000ca905bdcaUL;
+ tf->codes[1191] = 0x00000003150a39a7UL;
+ tf->codes[1192] = 0x0000000e8329fad5UL;
+ tf->codes[1193] = 0x0000000bd4f98059UL;
+ tf->codes[1194] = 0x00000003bc5cf6cdUL;
+ tf->codes[1195] = 0x0000000c982fdd03UL;
+ tf->codes[1196] = 0x00000000a372de28UL;
+ tf->codes[1197] = 0x000000073fe2e35aUL;
+ tf->codes[1198] = 0x00000000b9f684ecUL;
+ tf->codes[1199] = 0x0000000c543ff680UL;
+ tf->codes[1200] = 0x00000001bcf5f09aUL;
+ tf->codes[1201] = 0x000000051b2a8099UL;
+ tf->codes[1202] = 0x0000000ee53277c2UL;
+ tf->codes[1203] = 0x00000000b3835a6cUL;
+ tf->codes[1204] = 0x0000000aed6765c1UL;
+ tf->codes[1205] = 0x000000092cfd64c8UL;
+ tf->codes[1206] = 0x0000000d20c60ed2UL;
+ tf->codes[1207] = 0x000000059dbd9f51UL;
+ tf->codes[1208] = 0x0000000b6acb694bUL;
+ tf->codes[1209] = 0x0000000427dcd5fdUL;
+ tf->codes[1210] = 0x0000000646336a75UL;
+ tf->codes[1211] = 0x00000008008dea4dUL;
+ tf->codes[1212] = 0x00000000af2bdc7cUL;
+ tf->codes[1213] = 0x0000000b8a46478aUL;
+ tf->codes[1214] = 0x0000000b02c535b6UL;
+ tf->codes[1215] = 0x0000000c645d8631UL;
+ tf->codes[1216] = 0x0000000044b4af3dUL;
+ tf->codes[1217] = 0x0000000c9edfe6cbUL;
+ tf->codes[1218] = 0x000000032ac8ea2aUL;
+ tf->codes[1219] = 0x000000079266a23fUL;
+ tf->codes[1220] = 0x0000000c2d902e93UL;
+ tf->codes[1221] = 0x00000006ae5cfbdbUL;
+ tf->codes[1222] = 0x00000002c66c633eUL;
+ tf->codes[1223] = 0x0000000eb7a8a4e3UL;
+ tf->codes[1224] = 0x0000000cb17281cfUL;
+ tf->codes[1225] = 0x00000007ca378680UL;
+ tf->codes[1226] = 0x00000007ac81509dUL;
+ tf->codes[1227] = 0x0000000a59a05073UL;
+ tf->codes[1228] = 0x0000000c9cb9f18dUL;
+ tf->codes[1229] = 0x0000000b78100d29UL;
+ tf->codes[1230] = 0x0000000fab49420aUL;
+ tf->codes[1231] = 0x0000000d0a4e69c4UL;
+ tf->codes[1232] = 0x0000000d6c33f722UL;
+ tf->codes[1233] = 0x000000068d21bff8UL;
+ tf->codes[1234] = 0x00000001fdad8ca3UL;
+ tf->codes[1235] = 0x00000002884d6968UL;
+ tf->codes[1236] = 0x0000000b091ff264UL;
+ tf->codes[1237] = 0x0000000eb5fb236fUL;
+ tf->codes[1238] = 0x0000000a3d2a1839UL;
+ tf->codes[1239] = 0x0000000527db0bc8UL;
+ tf->codes[1240] = 0x00000002dc68cd9fUL;
+ tf->codes[1241] = 0x0000000e3f4ea98aUL;
+ tf->codes[1242] = 0x0000000a629fe44fUL;
+ tf->codes[1243] = 0x0000000b73bd7d66UL;
+ tf->codes[1244] = 0x00000002abfd7b6bUL;
+ tf->codes[1245] = 0x00000001b4056054UL;
+ tf->codes[1246] = 0x0000000d6efaac28UL;
+ tf->codes[1247] = 0x00000000d13cc950UL;
+ tf->codes[1248] = 0x0000000ef84ead94UL;
+ tf->codes[1249] = 0x00000005b6ee0d50UL;
+ tf->codes[1250] = 0x00000000f4bec692UL;
+ tf->codes[1251] = 0x0000000de1b98881UL;
+ tf->codes[1252] = 0x000000055ccccd31UL;
+ tf->codes[1253] = 0x0000000086d9b84dUL;
+ tf->codes[1254] = 0x00000005ab736e3dUL;
+ tf->codes[1255] = 0x0000000167d2f005UL;
+ tf->codes[1256] = 0x0000000118ed1522UL;
+ tf->codes[1257] = 0x000000038bbdc903UL;
+ tf->codes[1258] = 0x000000039cd31ac2UL;
+ tf->codes[1259] = 0x000000031091bc51UL;
+ tf->codes[1260] = 0x0000000d66a87d3fUL;
+ tf->codes[1261] = 0x0000000afdade6d3UL;
+ tf->codes[1262] = 0x00000002bd1fe097UL;
+ tf->codes[1263] = 0x00000005cf545dd2UL;
+ tf->codes[1264] = 0x00000005e0af578eUL;
+ tf->codes[1265] = 0x00000006fe6dd4c9UL;
+ tf->codes[1266] = 0x0000000862bc8fcaUL;
+ tf->codes[1267] = 0x0000000cbce0b4c6UL;
+ tf->codes[1268] = 0x000000008b7fa8ddUL;
+ tf->codes[1269] = 0x00000003d108ae9fUL;
+ tf->codes[1270] = 0x0000000fed2d914aUL;
+ tf->codes[1271] = 0x0000000bab304bd8UL;
+ tf->codes[1272] = 0x0000000debe74f8dUL;
+ tf->codes[1273] = 0x00000001e857e3dcUL;
+ tf->codes[1274] = 0x0000000570340581UL;
+ tf->codes[1275] = 0x0000000114bbf4f5UL;
+ tf->codes[1276] = 0x0000000a3cfc0566UL;
+ tf->codes[1277] = 0x00000004026cd686UL;
+ tf->codes[1278] = 0x0000000266fb76cdUL;
+ tf->codes[1279] = 0x0000000b715773bbUL;
+ tf->codes[1280] = 0x00000002fd2785fdUL;
+ tf->codes[1281] = 0x0000000481b34cadUL;
+ tf->codes[1282] = 0x000000011c58d2baUL;
+ tf->codes[1283] = 0x00000003a5186f4dUL;
+ tf->codes[1284] = 0x0000000da55ab71cUL;
+ tf->codes[1285] = 0x0000000ac887db92UL;
+ tf->codes[1286] = 0x00000009bd6d5592UL;
+ tf->codes[1287] = 0x000000045857d12aUL;
+ tf->codes[1288] = 0x00000008c862f0b9UL;
+ tf->codes[1289] = 0x0000000870c88666UL;
+ tf->codes[1290] = 0x00000004a4f4901fUL;
+ tf->codes[1291] = 0x0000000774a993d0UL;
+ tf->codes[1292] = 0x0000000c9f16c81dUL;
+ tf->codes[1293] = 0x0000000eb415e9efUL;
+ tf->codes[1294] = 0x0000000307aa6302UL;
+ tf->codes[1295] = 0x0000000a246f21eeUL;
+ tf->codes[1296] = 0x00000001a4f8a9c2UL;
+ tf->codes[1297] = 0x00000000cf09f9b4UL;
+ tf->codes[1298] = 0x0000000db30dbb49UL;
+ tf->codes[1299] = 0x00000003581be36fUL;
+ tf->codes[1300] = 0x00000006919a4318UL;
+ tf->codes[1301] = 0x00000008ee677afdUL;
+ tf->codes[1302] = 0x00000005944b9d59UL;
+ tf->codes[1303] = 0x00000008d5fe61aaUL;
+ tf->codes[1304] = 0x000000077c174b1dUL;
+ tf->codes[1305] = 0x00000005cff8fa10UL;
+ tf->codes[1306] = 0x0000000c1ce82f48UL;
+ tf->codes[1307] = 0x00000007fbb18e65UL;
+ tf->codes[1308] = 0x00000000b6737103UL;
+ tf->codes[1309] = 0x0000000e2d30a9b6UL;
+ tf->codes[1310] = 0x00000006481ff469UL;
+ tf->codes[1311] = 0x00000005834b4d26UL;
+ tf->codes[1312] = 0x00000003bba517d5UL;
+ tf->codes[1313] = 0x0000000eee6e8080UL;
+ tf->codes[1314] = 0x00000005fe4fea5eUL;
+ tf->codes[1315] = 0x0000000e84e94c8cUL;
+ tf->codes[1316] = 0x0000000ba2ad0a2aUL;
+ tf->codes[1317] = 0x0000000a7f2aead0UL;
+ tf->codes[1318] = 0x000000063cecb46dUL;
+ tf->codes[1319] = 0x00000008943d7229UL;
+ tf->codes[1320] = 0x00000001d3878b2bUL;
+ tf->codes[1321] = 0x0000000f2b4efe94UL;
+ tf->codes[1322] = 0x0000000d9af1949dUL;
+ tf->codes[1323] = 0x0000000bb5824d39UL;
+ tf->codes[1324] = 0x0000000b8d8f5090UL;
+ tf->codes[1325] = 0x0000000ed5e19d08UL;
+ tf->codes[1326] = 0x000000060287437eUL;
+ tf->codes[1327] = 0x00000008fe6ae5c2UL;
+ tf->codes[1328] = 0x00000006c85ac058UL;
+ tf->codes[1329] = 0x0000000b906be1b8UL;
+ tf->codes[1330] = 0x0000000f9d423f65UL;
+ tf->codes[1331] = 0x00000006efed81d6UL;
+ tf->codes[1332] = 0x0000000781b67fa2UL;
+ tf->codes[1333] = 0x0000000e1dd437acUL;
+ tf->codes[1334] = 0x00000007a9201a8cUL;
+ tf->codes[1335] = 0x0000000fb444c819UL;
+ tf->codes[1336] = 0x0000000ce75af959UL;
+ tf->codes[1337] = 0x000000086df6e72bUL;
+ tf->codes[1338] = 0x0000000756695aa7UL;
+ tf->codes[1339] = 0x0000000b7b2bddf2UL;
+ tf->codes[1340] = 0x0000000f19a1b99eUL;
+ tf->codes[1341] = 0x00000009a5790e90UL;
+ tf->codes[1342] = 0x00000001d3b3eac0UL;
+ tf->codes[1343] = 0x0000000a5c5d9d2bUL;
+ tf->codes[1344] = 0x0000000152850218UL;
+ tf->codes[1345] = 0x0000000025c4ba6eUL;
+ tf->codes[1346] = 0x0000000d4a5f4bebUL;
+ tf->codes[1347] = 0x0000000709cec10eUL;
+ tf->codes[1348] = 0x000000094ddbdb6cUL;
+ tf->codes[1349] = 0x00000009d1218277UL;
+ tf->codes[1350] = 0x00000006190ca34aUL;
+ tf->codes[1351] = 0x0000000468ed6a3fUL;
+ tf->codes[1352] = 0x0000000801bda52eUL;
+ tf->codes[1353] = 0x0000000261b3f1a9UL;
+ tf->codes[1354] = 0x00000000b3494d9bUL;
+ tf->codes[1355] = 0x0000000583e2d7e5UL;
+ tf->codes[1356] = 0x00000009407a80f2UL;
+ tf->codes[1357] = 0x000000058e902456UL;
+ tf->codes[1358] = 0x00000009108c2273UL;
+ tf->codes[1359] = 0x000000059778ff8cUL;
+ tf->codes[1360] = 0x0000000d6ce05028UL;
+ tf->codes[1361] = 0x00000000286adc62UL;
+ tf->codes[1362] = 0x00000007ed3060dcUL;
+ tf->codes[1363] = 0x000000057b7e03edUL;
+ tf->codes[1364] = 0x00000003e3dce5c1UL;
+ tf->codes[1365] = 0x00000001bebc2295UL;
+ tf->codes[1366] = 0x0000000014a17c9aUL;
+ tf->codes[1367] = 0x0000000c7d90fbdaUL;
+ tf->codes[1368] = 0x00000008158ae35aUL;
+ tf->codes[1369] = 0x000000069d70a335UL;
+ tf->codes[1370] = 0x0000000d3ef97931UL;
+ tf->codes[1371] = 0x00000005793efb7aUL;
+ tf->codes[1372] = 0x0000000e6989ef43UL;
+ tf->codes[1373] = 0x0000000cd15f0116UL;
+ tf->codes[1374] = 0x0000000f9dbc6e25UL;
+ tf->codes[1375] = 0x0000000da4a91117UL;
+ tf->codes[1376] = 0x0000000054d0917aUL;
+ tf->codes[1377] = 0x000000060f2c3f15UL;
+ tf->codes[1378] = 0x00000007393b0a66UL;
+ tf->codes[1379] = 0x00000006630ed79bUL;
+ tf->codes[1380] = 0x0000000ed8589c60UL;
+ tf->codes[1381] = 0x00000007db37ab26UL;
+ tf->codes[1382] = 0x0000000c4631e80aUL;
+ tf->codes[1383] = 0x00000001badaf501UL;
+ tf->codes[1384] = 0x00000009bdef764dUL;
+ tf->codes[1385] = 0x0000000dd0949b4bUL;
+ tf->codes[1386] = 0x000000086f116771UL;
+ tf->codes[1387] = 0x0000000acd7ea109UL;
+ tf->codes[1388] = 0x00000007cc9d2f6bUL;
+ tf->codes[1389] = 0x00000003f5598822UL;
+ tf->codes[1390] = 0x00000004ba5a8d0cUL;
+ tf->codes[1391] = 0x000000066e7f9c42UL;
+ tf->codes[1392] = 0x000000033127fb36UL;
+ tf->codes[1393] = 0x00000000c85ff976UL;
+ tf->codes[1394] = 0x00000009dbb32ddfUL;
+ tf->codes[1395] = 0x00000003d06c7a56UL;
+ tf->codes[1396] = 0x0000000ac07601ddUL;
+ tf->codes[1397] = 0x00000005fda3d7e9UL;
+ tf->codes[1398] = 0x000000040a47aef0UL;
+ tf->codes[1399] = 0x0000000139928cd0UL;
+ tf->codes[1400] = 0x0000000183ab75ebUL;
+ tf->codes[1401] = 0x00000009dd6d1f4bUL;
+ tf->codes[1402] = 0x0000000954afec44UL;
+ tf->codes[1403] = 0x000000029953fe22UL;
+ tf->codes[1404] = 0x0000000f947e49b1UL;
+ tf->codes[1405] = 0x0000000a74266cb0UL;
+ tf->codes[1406] = 0x00000003bbb7fdabUL;
+ tf->codes[1407] = 0x00000008a72b63d1UL;
+ tf->codes[1408] = 0x00000008763e2fbbUL;
+ tf->codes[1409] = 0x00000008c9b4f9a2UL;
+ tf->codes[1410] = 0x0000000a35f5a861UL;
+ tf->codes[1411] = 0x000000099e54752cUL;
+ tf->codes[1412] = 0x00000002fdb8e16fUL;
+ tf->codes[1413] = 0x00000002d083ed68UL;
+ tf->codes[1414] = 0x0000000a05d36c5eUL;
+ tf->codes[1415] = 0x00000005460842feUL;
+ tf->codes[1416] = 0x0000000173ae0ee6UL;
+ tf->codes[1417] = 0x000000038b3c62e5UL;
+ tf->codes[1418] = 0x0000000476c1ae99UL;
+ tf->codes[1419] = 0x00000009a8cb898aUL;
+ tf->codes[1420] = 0x000000019d4032acUL;
+ tf->codes[1421] = 0x0000000a9c01d80bUL;
+ tf->codes[1422] = 0x0000000ca7d5e4deUL;
+ tf->codes[1423] = 0x0000000295d53115UL;
+ tf->codes[1424] = 0x0000000b26740e51UL;
+ tf->codes[1425] = 0x0000000bf21b0988UL;
+ tf->codes[1426] = 0x0000000167391c15UL;
+ tf->codes[1427] = 0x0000000d10af35c6UL;
+ tf->codes[1428] = 0x0000000d94750799UL;
+ tf->codes[1429] = 0x0000000cb986d117UL;
+ tf->codes[1430] = 0x000000001dddf588UL;
+ tf->codes[1431] = 0x000000071ed85f46UL;
+ tf->codes[1432] = 0x0000000a5437d58fUL;
+ tf->codes[1433] = 0x00000004029d1e25UL;
+ tf->codes[1434] = 0x0000000c580ec972UL;
+ tf->codes[1435] = 0x00000006847df8baUL;
+ tf->codes[1436] = 0x0000000e294d997bUL;
+ tf->codes[1437] = 0x0000000e2e8b10eeUL;
+ tf->codes[1438] = 0x00000001593103ddUL;
+ tf->codes[1439] = 0x0000000222103857UL;
+ tf->codes[1440] = 0x00000001e035591dUL;
+ tf->codes[1441] = 0x0000000b5c9ef2e9UL;
+ tf->codes[1442] = 0x00000009f815ec3eUL;
+ tf->codes[1443] = 0x0000000d1da2a021UL;
+ tf->codes[1444] = 0x000000054f171191UL;
+ tf->codes[1445] = 0x0000000e51f4a05eUL;
+ tf->codes[1446] = 0x0000000c15e7d603UL;
+ tf->codes[1447] = 0x0000000ba7f16b87UL;
+ tf->codes[1448] = 0x000000080b7a83e1UL;
+ tf->codes[1449] = 0x0000000720e2b18dUL;
+ tf->codes[1450] = 0x00000005ec0c069dUL;
+ tf->codes[1451] = 0x0000000a4f9f689cUL;
+ tf->codes[1452] = 0x00000005871cafdaUL;
+ tf->codes[1453] = 0x0000000c913140a2UL;
+ tf->codes[1454] = 0x00000007a8f2efd1UL;
+ tf->codes[1455] = 0x000000077064952cUL;
+ tf->codes[1456] = 0x00000004ea2d857fUL;
+ tf->codes[1457] = 0x0000000484523555UL;
+ tf->codes[1458] = 0x000000054971a9e3UL;
+ tf->codes[1459] = 0x0000000eb0694eb2UL;
+ tf->codes[1460] = 0x0000000b513c8e63UL;
+ tf->codes[1461] = 0x00000005c910db58UL;
+ tf->codes[1462] = 0x0000000ca87a4dd7UL;
+ tf->codes[1463] = 0x0000000b8ca63158UL;
+ tf->codes[1464] = 0x0000000b4b09431dUL;
+ tf->codes[1465] = 0x00000003dc9d50b7UL;
+ tf->codes[1466] = 0x00000007d57f02acUL;
+ tf->codes[1467] = 0x00000005c595b1b2UL;
+ tf->codes[1468] = 0x00000009e0caf698UL;
+ tf->codes[1469] = 0x0000000136b48555UL;
+ tf->codes[1470] = 0x0000000687dbcc2bUL;
+ tf->codes[1471] = 0x000000054bae2294UL;
+ tf->codes[1472] = 0x00000006899bbd7bUL;
+ tf->codes[1473] = 0x00000008108f46deUL;
+ tf->codes[1474] = 0x00000001dbe8cf08UL;
+ tf->codes[1475] = 0x0000000a02e1ae1dUL;
+ tf->codes[1476] = 0x00000000f5f26d59UL;
+ tf->codes[1477] = 0x0000000805cf202bUL;
+ tf->codes[1478] = 0x0000000afede5687UL;
+ tf->codes[1479] = 0x00000001583d5b30UL;
+ tf->codes[1480] = 0x0000000da9ed0620UL;
+ tf->codes[1481] = 0x0000000cf1237338UL;
+ tf->codes[1482] = 0x00000003a5a77bc4UL;
+ tf->codes[1483] = 0x0000000a17ffa0c6UL;
+ tf->codes[1484] = 0x000000029de4c387UL;
+ tf->codes[1485] = 0x000000007825d431UL;
+ tf->codes[1486] = 0x000000002d7b9b38UL;
+ tf->codes[1487] = 0x00000008ed0f26aaUL;
+ tf->codes[1488] = 0x000000056e54e30dUL;
+ tf->codes[1489] = 0x00000009620ab0e7UL;
+ tf->codes[1490] = 0x0000000c7e3ea94cUL;
+ tf->codes[1491] = 0x0000000d288a41e2UL;
+ tf->codes[1492] = 0x0000000f68884f1eUL;
+ tf->codes[1493] = 0x00000005ee02df09UL;
+ tf->codes[1494] = 0x0000000c02dbf645UL;
+ tf->codes[1495] = 0x0000000eac4c2424UL;
+ tf->codes[1496] = 0x0000000cab2d51e1UL;
+ tf->codes[1497] = 0x0000000037439577UL;
+ tf->codes[1498] = 0x00000005618ada43UL;
+ tf->codes[1499] = 0x00000002683b5859UL;
+ tf->codes[1500] = 0x00000008a607c1ceUL;
+ tf->codes[1501] = 0x0000000795fd9198UL;
+ tf->codes[1502] = 0x0000000b3edb11b8UL;
+ tf->codes[1503] = 0x0000000846939c5cUL;
+ tf->codes[1504] = 0x00000008b1f6fa23UL;
+ tf->codes[1505] = 0x0000000b1a2f2bfeUL;
+ tf->codes[1506] = 0x0000000b63a07ad7UL;
+ tf->codes[1507] = 0x00000005f8ea7b00UL;
+ tf->codes[1508] = 0x00000004ee9c6d0cUL;
+ tf->codes[1509] = 0x0000000990f2889bUL;
+ tf->codes[1510] = 0x0000000b7f7251d0UL;
+ tf->codes[1511] = 0x0000000ac3291369UL;
+ tf->codes[1512] = 0x00000009d8f36a7bUL;
+ tf->codes[1513] = 0x0000000d57342897UL;
+ tf->codes[1514] = 0x0000000efca98365UL;
+ tf->codes[1515] = 0x0000000dacc69f0eUL;
+ tf->codes[1516] = 0x00000003a70e4b3cUL;
+ tf->codes[1517] = 0x00000001e95c34c2UL;
+ tf->codes[1518] = 0x00000004caab6c06UL;
+ tf->codes[1519] = 0x00000007231f6ee1UL;
+ tf->codes[1520] = 0x000000037909aa04UL;
+ tf->codes[1521] = 0x0000000048c9a9ccUL;
+ tf->codes[1522] = 0x000000059cd081bcUL;
+ tf->codes[1523] = 0x00000004dd78c2e4UL;
+ tf->codes[1524] = 0x00000004979da10fUL;
+ tf->codes[1525] = 0x000000004749d0c5UL;
+ tf->codes[1526] = 0x0000000a17a4283bUL;
+ tf->codes[1527] = 0x0000000de7e1d52dUL;
+ tf->codes[1528] = 0x00000000e47cedf1UL;
+ tf->codes[1529] = 0x00000004fa48cbffUL;
+ tf->codes[1530] = 0x0000000545a932a0UL;
+ tf->codes[1531] = 0x00000006c2bd9eb8UL;
+ tf->codes[1532] = 0x0000000dd9bd3b8cUL;
+ tf->codes[1533] = 0x000000043332c1baUL;
+ tf->codes[1534] = 0x0000000501fa761dUL;
+ tf->codes[1535] = 0x00000007ec40adbbUL;
+ tf->codes[1536] = 0x00000004049f2b33UL;
+ tf->codes[1537] = 0x0000000cde28f57bUL;
+ tf->codes[1538] = 0x0000000f68c804b9UL;
+ tf->codes[1539] = 0x00000008f50fbd3eUL;
+ tf->codes[1540] = 0x000000054e1bc344UL;
+ tf->codes[1541] = 0x000000036b26e3a2UL;
+ tf->codes[1542] = 0x000000002e5ac9b1UL;
+ tf->codes[1543] = 0x000000010837858dUL;
+ tf->codes[1544] = 0x00000006ccac9e0bUL;
+ tf->codes[1545] = 0x0000000625ba8a52UL;
+ tf->codes[1546] = 0x0000000ac4c8b45cUL;
+ tf->codes[1547] = 0x0000000868678237UL;
+ tf->codes[1548] = 0x00000004187235feUL;
+ tf->codes[1549] = 0x0000000bd62663ceUL;
+ tf->codes[1550] = 0x0000000ea832dfb2UL;
+ tf->codes[1551] = 0x0000000d5a72f0a7UL;
+ tf->codes[1552] = 0x00000000659c855eUL;
+ tf->codes[1553] = 0x0000000bea7f5e48UL;
+ tf->codes[1554] = 0x0000000ff9566715UL;
+ tf->codes[1555] = 0x00000001bd06d99aUL;
+ tf->codes[1556] = 0x00000009666c578cUL;
+ tf->codes[1557] = 0x0000000c6527d3ecUL;
+ tf->codes[1558] = 0x0000000b541f3c61UL;
+ tf->codes[1559] = 0x0000000678a9ad70UL;
+ tf->codes[1560] = 0x000000036eaadfa3UL;
+ tf->codes[1561] = 0x0000000af74b01deUL;
+ tf->codes[1562] = 0x000000054cc3cdc3UL;
+ tf->codes[1563] = 0x0000000d2e587ce6UL;
+ tf->codes[1564] = 0x00000008694b9349UL;
+ tf->codes[1565] = 0x0000000d309898feUL;
+ tf->codes[1566] = 0x00000005c3250e09UL;
+ tf->codes[1567] = 0x000000084dcac28eUL;
+ tf->codes[1568] = 0x0000000f72add2dfUL;
+ tf->codes[1569] = 0x00000001901681a3UL;
+ tf->codes[1570] = 0x000000009e6a8fd4UL;
+ tf->codes[1571] = 0x000000012f614cd1UL;
+ tf->codes[1572] = 0x00000006d7801ac4UL;
+ tf->codes[1573] = 0x000000014cf1ca54UL;
+ tf->codes[1574] = 0x000000012a7eb608UL;
+ tf->codes[1575] = 0x00000005e7a3bf62UL;
+ tf->codes[1576] = 0x00000000ba5056a2UL;
+ tf->codes[1577] = 0x00000005bee44c9bUL;
+ tf->codes[1578] = 0x0000000819d7dc86UL;
+ tf->codes[1579] = 0x0000000062adc8fdUL;
+ tf->codes[1580] = 0x0000000bd3155d41UL;
+ tf->codes[1581] = 0x0000000cd8c6b38aUL;
+ tf->codes[1582] = 0x0000000e320fd50eUL;
+ tf->codes[1583] = 0x0000000e189d6655UL;
+ tf->codes[1584] = 0x00000006863c2831UL;
+ tf->codes[1585] = 0x00000000d2b9058fUL;
+ tf->codes[1586] = 0x000000023bfad8faUL;
+ tf->codes[1587] = 0x0000000199bd1216UL;
+ tf->codes[1588] = 0x000000056138afd7UL;
+ tf->codes[1589] = 0x0000000face83a93UL;
+ tf->codes[1590] = 0x00000009554da725UL;
+ tf->codes[1591] = 0x00000009b614dd91UL;
+ tf->codes[1592] = 0x000000098acbca3fUL;
+ tf->codes[1593] = 0x0000000d5f0d5f21UL;
+ tf->codes[1594] = 0x0000000eb59039e1UL;
+ tf->codes[1595] = 0x000000051d1ec82aUL;
+ tf->codes[1596] = 0x0000000a366ef3baUL;
+ tf->codes[1597] = 0x00000001ad0e01f0UL;
+ tf->codes[1598] = 0x00000007f038ad0bUL;
+ tf->codes[1599] = 0x00000003ee055321UL;
+ tf->codes[1600] = 0x00000003bf2dcbb7UL;
+ tf->codes[1601] = 0x0000000210e9856cUL;
+ tf->codes[1602] = 0x0000000e4fea8231UL;
+ tf->codes[1603] = 0x0000000b89444937UL;
+ tf->codes[1604] = 0x000000058852cc34UL;
+ tf->codes[1605] = 0x00000001ee29eea9UL;
+ tf->codes[1606] = 0x0000000b919c79f2UL;
+ tf->codes[1607] = 0x0000000ddc44d3adUL;
+ tf->codes[1608] = 0x0000000ddcbd4777UL;
+ tf->codes[1609] = 0x00000003c3982ba1UL;
+ tf->codes[1610] = 0x0000000dc8ebc45dUL;
+ tf->codes[1611] = 0x00000008b97712b1UL;
+ tf->codes[1612] = 0x00000009702ea21eUL;
+ tf->codes[1613] = 0x00000001f457e726UL;
+ tf->codes[1614] = 0x000000027c6f6e26UL;
+ tf->codes[1615] = 0x00000000a9797770UL;
+ tf->codes[1616] = 0x0000000d7615f53bUL;
+ tf->codes[1617] = 0x000000074f1cb6e1UL;
+ tf->codes[1618] = 0x0000000a32e4d7dcUL;
+ tf->codes[1619] = 0x00000002e89afd1dUL;
+ tf->codes[1620] = 0x00000000b03704d5UL;
+ tf->codes[1621] = 0x0000000cca58aab0UL;
+ tf->codes[1622] = 0x00000001e5749225UL;
+ tf->codes[1623] = 0x00000006e63a36baUL;
+ tf->codes[1624] = 0x0000000562992099UL;
+ tf->codes[1625] = 0x000000064701b950UL;
+ tf->codes[1626] = 0x0000000f94ed6196UL;
+ tf->codes[1627] = 0x0000000b3441b5f1UL;
+ tf->codes[1628] = 0x0000000c64fac247UL;
+ tf->codes[1629] = 0x0000000d72ebd98bUL;
+ tf->codes[1630] = 0x0000000fa1985b23UL;
+ tf->codes[1631] = 0x00000002df788358UL;
+ tf->codes[1632] = 0x000000088838b488UL;
+ tf->codes[1633] = 0x00000006091032b4UL;
+ tf->codes[1634] = 0x000000025ff2d736UL;
+ tf->codes[1635] = 0x0000000dce63d3d5UL;
+ tf->codes[1636] = 0x0000000bb5970414UL;
+ tf->codes[1637] = 0x000000044d8b5ffeUL;
+ tf->codes[1638] = 0x0000000e1a5666d8UL;
+ tf->codes[1639] = 0x0000000e34129125UL;
+ tf->codes[1640] = 0x00000000e23854b1UL;
+ tf->codes[1641] = 0x000000001b2a6dbeUL;
+ tf->codes[1642] = 0x0000000d11507bcdUL;
+ tf->codes[1643] = 0x0000000844531e6bUL;
+ tf->codes[1644] = 0x0000000d864a8611UL;
+ tf->codes[1645] = 0x0000000e2a5a7700UL;
+ tf->codes[1646] = 0x00000002d178962aUL;
+ tf->codes[1647] = 0x0000000156b07f01UL;
+ tf->codes[1648] = 0x000000048b59fec3UL;
+ tf->codes[1649] = 0x00000003d3d9d79cUL;
+ tf->codes[1650] = 0x00000001846fb339UL;
+ tf->codes[1651] = 0x0000000ddf1d03caUL;
+ tf->codes[1652] = 0x00000000998abaf9UL;
+ tf->codes[1653] = 0x0000000c9d76190bUL;
+ tf->codes[1654] = 0x000000067354a1a8UL;
+ tf->codes[1655] = 0x0000000cc89e2b09UL;
+ tf->codes[1656] = 0x0000000353356834UL;
+ tf->codes[1657] = 0x00000007ad97470eUL;
+ tf->codes[1658] = 0x0000000f4d560524UL;
+ tf->codes[1659] = 0x0000000534b7804eUL;
+ tf->codes[1660] = 0x000000014290c632UL;
+ tf->codes[1661] = 0x0000000b67d39d60UL;
+ tf->codes[1662] = 0x000000035b166febUL;
+ tf->codes[1663] = 0x000000088e6fb681UL;
+ tf->codes[1664] = 0x0000000a0f82ae1aUL;
+ tf->codes[1665] = 0x000000008460ce52UL;
+ tf->codes[1666] = 0x00000008b06a9012UL;
+ tf->codes[1667] = 0x0000000daf1299dcUL;
+ tf->codes[1668] = 0x0000000629ab696cUL;
+ tf->codes[1669] = 0x00000003113b448aUL;
+ tf->codes[1670] = 0x00000000db5ca215UL;
+ tf->codes[1671] = 0x00000003e00b1e2dUL;
+ tf->codes[1672] = 0x000000085a87f5abUL;
+ tf->codes[1673] = 0x0000000b3995ff20UL;
+ tf->codes[1674] = 0x000000085661554dUL;
+ tf->codes[1675] = 0x0000000e709c5384UL;
+ tf->codes[1676] = 0x00000000111ca99bUL;
+ tf->codes[1677] = 0x000000049e614279UL;
+ tf->codes[1678] = 0x0000000f14677ec4UL;
+ tf->codes[1679] = 0x00000008f6439bfbUL;
+ tf->codes[1680] = 0x0000000749faa461UL;
+ tf->codes[1681] = 0x00000001c4f9189aUL;
+ tf->codes[1682] = 0x0000000e8e9015caUL;
+ tf->codes[1683] = 0x0000000f6e68d510UL;
+ tf->codes[1684] = 0x0000000b3819319fUL;
+ tf->codes[1685] = 0x0000000da9f7119fUL;
+ tf->codes[1686] = 0x00000007787f40f8UL;
+ tf->codes[1687] = 0x0000000bc57f5716UL;
+ tf->codes[1688] = 0x000000060ff2897eUL;
+ tf->codes[1689] = 0x0000000b3a28a934UL;
+ tf->codes[1690] = 0x000000010b34c97cUL;
+ tf->codes[1691] = 0x0000000c14f53aedUL;
+ tf->codes[1692] = 0x0000000d3c4eaf5dUL;
+ tf->codes[1693] = 0x0000000b3148d39eUL;
+ tf->codes[1694] = 0x000000007874ea02UL;
+ tf->codes[1695] = 0x0000000f86692b4aUL;
+ tf->codes[1696] = 0x00000005b03a0e8dUL;
+ tf->codes[1697] = 0x0000000ce6db8cc6UL;
+ tf->codes[1698] = 0x00000008233d5908UL;
+ tf->codes[1699] = 0x0000000f163e3c06UL;
+ tf->codes[1700] = 0x0000000dff854cceUL;
+ tf->codes[1701] = 0x000000026706f1bcUL;
+ tf->codes[1702] = 0x000000094c358653UL;
+ tf->codes[1703] = 0x00000007384c9821UL;
+ tf->codes[1704] = 0x0000000e51b8e5d5UL;
+ tf->codes[1705] = 0x0000000eda32963bUL;
+ tf->codes[1706] = 0x0000000a073f392fUL;
+ tf->codes[1707] = 0x0000000c3ccfa213UL;
+ tf->codes[1708] = 0x000000034adf5216UL;
+ tf->codes[1709] = 0x0000000cb8da286bUL;
+ tf->codes[1710] = 0x00000003b5fbbf08UL;
+ tf->codes[1711] = 0x000000012812d1f8UL;
+ tf->codes[1712] = 0x0000000c97c54c39UL;
+ tf->codes[1713] = 0x0000000e1c3e36b9UL;
+ tf->codes[1714] = 0x0000000abb8dc0edUL;
+ tf->codes[1715] = 0x0000000019dcbbf6UL;
+ tf->codes[1716] = 0x000000025b0d7c4dUL;
+ tf->codes[1717] = 0x0000000045e6b5ceUL;
+ tf->codes[1718] = 0x000000017dc086caUL;
+ tf->codes[1719] = 0x0000000c3f425e6bUL;
+ tf->codes[1720] = 0x00000006fdee14f8UL;
+ tf->codes[1721] = 0x000000039155e6b4UL;
+ tf->codes[1722] = 0x00000000a191ec15UL;
+ tf->codes[1723] = 0x0000000398fcd7f4UL;
+ tf->codes[1724] = 0x0000000a6e2b0594UL;
+ tf->codes[1725] = 0x0000000fe5678d82UL;
+ tf->codes[1726] = 0x0000000e317eba1fUL;
+ tf->codes[1727] = 0x00000002c4f10ca1UL;
+ tf->codes[1728] = 0x0000000ae239c19eUL;
+ tf->codes[1729] = 0x000000018e663ed2UL;
+ tf->codes[1730] = 0x00000004a040b7e7UL;
+ tf->codes[1731] = 0x0000000bbca0849cUL;
+ tf->codes[1732] = 0x0000000ce05b3a74UL;
+ tf->codes[1733] = 0x00000007cee982fdUL;
+ tf->codes[1734] = 0x000000078ee54fa7UL;
+ tf->codes[1735] = 0x00000007b47bb0bdUL;
+ tf->codes[1736] = 0x00000007e8f19216UL;
+ tf->codes[1737] = 0x0000000d67d91cedUL;
+ tf->codes[1738] = 0x0000000ef5effe94UL;
+ tf->codes[1739] = 0x0000000ec1d1938dUL;
+ tf->codes[1740] = 0x00000004c05ef70eUL;
+ tf->codes[1741] = 0x00000000324442d9UL;
+ tf->codes[1742] = 0x0000000fb0183bb4UL;
+ tf->codes[1743] = 0x0000000fb7a0bd50UL;
+ tf->codes[1744] = 0x000000089aa17d87UL;
+ tf->codes[1745] = 0x0000000e4e6aed89UL;
+ tf->codes[1746] = 0x0000000dbecf68b4UL;
+ tf->codes[1747] = 0x0000000683770de4UL;
+ tf->codes[1748] = 0x0000000b9f41a136UL;
+ tf->codes[1749] = 0x0000000c7614caceUL;
+ tf->codes[1750] = 0x000000089c298386UL;
+ tf->codes[1751] = 0x0000000959cf09deUL;
+ tf->codes[1752] = 0x0000000ab30b19e3UL;
+ tf->codes[1753] = 0x0000000db2e4b614UL;
+ tf->codes[1754] = 0x000000026d30d39bUL;
+ tf->codes[1755] = 0x00000006ccefe452UL;
+ tf->codes[1756] = 0x0000000587c5035cUL;
+ tf->codes[1757] = 0x0000000ea73bbbe0UL;
+ tf->codes[1758] = 0x0000000dd9d91a11UL;
+ tf->codes[1759] = 0x0000000dd8c5e851UL;
+ tf->codes[1760] = 0x0000000e8b4aa077UL;
+ tf->codes[1761] = 0x00000008ccf8faddUL;
+ tf->codes[1762] = 0x000000047ddd3c0bUL;
+ tf->codes[1763] = 0x0000000635a92f19UL;
+ tf->codes[1764] = 0x0000000f0edfd1a3UL;
+ tf->codes[1765] = 0x00000001f760bf5eUL;
+ tf->codes[1766] = 0x0000000a83feb68aUL;
+ tf->codes[1767] = 0x00000004f74da9ddUL;
+ tf->codes[1768] = 0x000000052f759252UL;
+ tf->codes[1769] = 0x000000098bee689eUL;
+ tf->codes[1770] = 0x0000000c5fc8c3d5UL;
+ tf->codes[1771] = 0x00000008373d1286UL;
+ tf->codes[1772] = 0x0000000f5f1cdabdUL;
+ tf->codes[1773] = 0x0000000ada68d3e5UL;
+ tf->codes[1774] = 0x00000003bbb9eb5eUL;
+ tf->codes[1775] = 0x000000050cde8478UL;
+ tf->codes[1776] = 0x0000000f01f956e0UL;
+ tf->codes[1777] = 0x0000000a922f2842UL;
+ tf->codes[1778] = 0x0000000233a8b25aUL;
+ tf->codes[1779] = 0x000000071118b754UL;
+ tf->codes[1780] = 0x0000000b7f874552UL;
+ tf->codes[1781] = 0x000000044d757121UL;
+ tf->codes[1782] = 0x0000000b873b14ccUL;
+ tf->codes[1783] = 0x00000005bcc1db5cUL;
+ tf->codes[1784] = 0x0000000bf9b895ceUL;
+ tf->codes[1785] = 0x00000005e65bb620UL;
+ tf->codes[1786] = 0x0000000bbd1ed35cUL;
+ tf->codes[1787] = 0x0000000358e79973UL;
+ tf->codes[1788] = 0x000000062aa5a4a5UL;
+ tf->codes[1789] = 0x000000081715fc0fUL;
+ tf->codes[1790] = 0x00000008df03a76eUL;
+ tf->codes[1791] = 0x0000000376b7c6c7UL;
+ tf->codes[1792] = 0x0000000a07a49f2eUL;
+ tf->codes[1793] = 0x000000045e159b63UL;
+ tf->codes[1794] = 0x0000000dae5706b0UL;
+ tf->codes[1795] = 0x0000000b5e52c7ccUL;
+ tf->codes[1796] = 0x0000000206935e8eUL;
+ tf->codes[1797] = 0x000000039f0c5119UL;
+ tf->codes[1798] = 0x00000003cd58c574UL;
+ tf->codes[1799] = 0x0000000571986d35UL;
+ tf->codes[1800] = 0x0000000ad66da60fUL;
+ tf->codes[1801] = 0x000000002b1a6315UL;
+ tf->codes[1802] = 0x0000000d0131b533UL;
+ tf->codes[1803] = 0x0000000741a195c5UL;
+ tf->codes[1804] = 0x00000000b8663437UL;
+ tf->codes[1805] = 0x00000001cde52798UL;
+ tf->codes[1806] = 0x00000006b8e658b1UL;
+ tf->codes[1807] = 0x0000000b43c0d44dUL;
+ tf->codes[1808] = 0x000000045481d697UL;
+ tf->codes[1809] = 0x000000029de93df5UL;
+ tf->codes[1810] = 0x000000010549b874UL;
+ tf->codes[1811] = 0x0000000c056b5828UL;
+ tf->codes[1812] = 0x000000003fa830adUL;
+ tf->codes[1813] = 0x00000009496d14faUL;
+ tf->codes[1814] = 0x0000000f540592a0UL;
+ tf->codes[1815] = 0x0000000f31c8b855UL;
+ tf->codes[1816] = 0x000000064f2ba36bUL;
+ tf->codes[1817] = 0x0000000fe7c6e4f5UL;
+ tf->codes[1818] = 0x00000005e42a78b0UL;
+ tf->codes[1819] = 0x00000009c2b8b096UL;
+ tf->codes[1820] = 0x0000000dcb4a6e71UL;
+ tf->codes[1821] = 0x0000000d63b0e7edUL;
+ tf->codes[1822] = 0x0000000de1bcbcdaUL;
+ tf->codes[1823] = 0x000000068e7161f2UL;
+ tf->codes[1824] = 0x00000003e5ddf88dUL;
+ tf->codes[1825] = 0x0000000419a37501UL;
+ tf->codes[1826] = 0x0000000fad63e7abUL;
+ tf->codes[1827] = 0x0000000c6e81b4baUL;
+ tf->codes[1828] = 0x00000008329315d3UL;
+ tf->codes[1829] = 0x0000000c88d267e6UL;
+ tf->codes[1830] = 0x000000073a0ac25fUL;
+ tf->codes[1831] = 0x0000000e7b75690fUL;
+ tf->codes[1832] = 0x0000000dcbb95be2UL;
+ tf->codes[1833] = 0x00000007a1d2a059UL;
+ tf->codes[1834] = 0x0000000d8fac361eUL;
+ tf->codes[1835] = 0x00000006312ff5c9UL;
+ tf->codes[1836] = 0x0000000d2cf50d54UL;
+ tf->codes[1837] = 0x00000008c65fd00fUL;
+ tf->codes[1838] = 0x0000000aa1636532UL;
+ tf->codes[1839] = 0x0000000870c7285dUL;
+ tf->codes[1840] = 0x00000001894f0b84UL;
+ tf->codes[1841] = 0x00000004260cc5c3UL;
+ tf->codes[1842] = 0x0000000e9997b9ecUL;
+ tf->codes[1843] = 0x000000087a052144UL;
+ tf->codes[1844] = 0x00000008706babf6UL;
+ tf->codes[1845] = 0x0000000bd5f62ad3UL;
+ tf->codes[1846] = 0x00000001a7895439UL;
+ tf->codes[1847] = 0x0000000f7e294bbcUL;
+ tf->codes[1848] = 0x0000000bcc27ca26UL;
+ tf->codes[1849] = 0x00000003186a63d4UL;
+ tf->codes[1850] = 0x00000007f3ede4a4UL;
+ tf->codes[1851] = 0x0000000b64e32468UL;
+ tf->codes[1852] = 0x000000071f250d53UL;
+ tf->codes[1853] = 0x00000007c6513783UL;
+ tf->codes[1854] = 0x0000000b1778714aUL;
+ tf->codes[1855] = 0x000000094bf2c57fUL;
+ tf->codes[1856] = 0x000000064a9f893aUL;
+ tf->codes[1857] = 0x00000001305be654UL;
+ tf->codes[1858] = 0x0000000493e0c9f6UL;
+ tf->codes[1859] = 0x000000005ba6fed8UL;
+ tf->codes[1860] = 0x0000000c4a0c7a06UL;
+ tf->codes[1861] = 0x00000000cc2ec0ddUL;
+ tf->codes[1862] = 0x0000000d9a6769afUL;
+ tf->codes[1863] = 0x0000000724c78a49UL;
+ tf->codes[1864] = 0x0000000c85c981a4UL;
+ tf->codes[1865] = 0x000000012553c4cdUL;
+ tf->codes[1866] = 0x000000083cb892b1UL;
+ tf->codes[1867] = 0x0000000bc324ccc7UL;
+ tf->codes[1868] = 0x0000000ef43f6c1dUL;
+ tf->codes[1869] = 0x00000002d6748bb7UL;
+ tf->codes[1870] = 0x00000005efdce2d7UL;
+ tf->codes[1871] = 0x000000094af64f28UL;
+ tf->codes[1872] = 0x0000000f9d58feb3UL;
+ tf->codes[1873] = 0x0000000cf547ac63UL;
+ tf->codes[1874] = 0x0000000ceb309febUL;
+ tf->codes[1875] = 0x000000030beba8caUL;
+ tf->codes[1876] = 0x00000008ab2e486aUL;
+ tf->codes[1877] = 0x00000004a95d58adUL;
+ tf->codes[1878] = 0x000000025ce07c46UL;
+ tf->codes[1879] = 0x0000000712b93fd7UL;
+ tf->codes[1880] = 0x00000007f46acc81UL;
+ tf->codes[1881] = 0x000000064049d4beUL;
+ tf->codes[1882] = 0x000000065303aa09UL;
+ tf->codes[1883] = 0x0000000f3aad21b3UL;
+ tf->codes[1884] = 0x00000002903a6cd0UL;
+ tf->codes[1885] = 0x00000005a0e0467dUL;
+ tf->codes[1886] = 0x00000003c4fa64e4UL;
+ tf->codes[1887] = 0x00000005c6655126UL;
+ tf->codes[1888] = 0x0000000b40a2a67fUL;
+ tf->codes[1889] = 0x0000000b0c22c6e5UL;
+ tf->codes[1890] = 0x00000001507e039bUL;
+ tf->codes[1891] = 0x0000000b282b16b8UL;
+ tf->codes[1892] = 0x0000000c0e14a3d3UL;
+ tf->codes[1893] = 0x000000093d381427UL;
+ tf->codes[1894] = 0x00000006bb55bb87UL;
+ tf->codes[1895] = 0x0000000b675af72fUL;
+ tf->codes[1896] = 0x0000000fceb4f95eUL;
+ tf->codes[1897] = 0x000000066af6ebbdUL;
+ tf->codes[1898] = 0x000000020a44d1f2UL;
+ tf->codes[1899] = 0x00000006bc873916UL;
+ tf->codes[1900] = 0x0000000b8947bee8UL;
+ tf->codes[1901] = 0x00000004b6bed8a6UL;
+ tf->codes[1902] = 0x00000007012f7867UL;
+ tf->codes[1903] = 0x00000007eda3c150UL;
+ tf->codes[1904] = 0x0000000ab3ef1b8eUL;
+ tf->codes[1905] = 0x00000006d71466eeUL;
+ tf->codes[1906] = 0x0000000408c4e225UL;
+ tf->codes[1907] = 0x0000000e117838b1UL;
+ tf->codes[1908] = 0x00000000aef3a075UL;
+ tf->codes[1909] = 0x00000005a0779d4fUL;
+ tf->codes[1910] = 0x000000070a3b1d69UL;
+ tf->codes[1911] = 0x000000026ccd31fdUL;
+ tf->codes[1912] = 0x0000000ed64dd1b2UL;
+ tf->codes[1913] = 0x0000000981d4f60cUL;
+ tf->codes[1914] = 0x00000006a6e4fb61UL;
+ tf->codes[1915] = 0x000000052f15fc93UL;
+ tf->codes[1916] = 0x0000000032b3a64dUL;
+ tf->codes[1917] = 0x0000000ecb17d667UL;
+ tf->codes[1918] = 0x0000000a983fb935UL;
+ tf->codes[1919] = 0x000000037d23c88dUL;
+ tf->codes[1920] = 0x0000000b8590fbcbUL;
+ tf->codes[1921] = 0x0000000ec2f1a277UL;
+ tf->codes[1922] = 0x000000090d3053e6UL;
+ tf->codes[1923] = 0x0000000a36fa8ccdUL;
+ tf->codes[1924] = 0x000000044bd08eccUL;
+ tf->codes[1925] = 0x000000061dd197d9UL;
+ tf->codes[1926] = 0x0000000a307cfd82UL;
+ tf->codes[1927] = 0x00000001d09c2de4UL;
+ tf->codes[1928] = 0x00000005f6d74368UL;
+ tf->codes[1929] = 0x00000001327d1b2dUL;
+ tf->codes[1930] = 0x0000000594cc36b9UL;
+ tf->codes[1931] = 0x0000000fea1cba7cUL;
+ tf->codes[1932] = 0x000000050c31262dUL;
+ tf->codes[1933] = 0x0000000d99b1a6baUL;
+ tf->codes[1934] = 0x00000001bf789cd2UL;
+ tf->codes[1935] = 0x0000000e2f6f66f9UL;
+ tf->codes[1936] = 0x000000013d5edfc6UL;
+ tf->codes[1937] = 0x0000000bc3a9ab0cUL;
+ tf->codes[1938] = 0x00000001da5b2734UL;
+ tf->codes[1939] = 0x000000025ef4f2deUL;
+ tf->codes[1940] = 0x0000000dcb55a50aUL;
+ tf->codes[1941] = 0x00000009c6dbc6acUL;
+ tf->codes[1942] = 0x000000089a838853UL;
+ tf->codes[1943] = 0x0000000168f099eeUL;
+ tf->codes[1944] = 0x0000000d51601760UL;
+ tf->codes[1945] = 0x000000089f324f1aUL;
+ tf->codes[1946] = 0x00000002cb1ec1eaUL;
+ tf->codes[1947] = 0x00000006306de366UL;
+ tf->codes[1948] = 0x0000000012a2f11eUL;
+ tf->codes[1949] = 0x0000000b5c0bf797UL;
+ tf->codes[1950] = 0x00000005c5f02be4UL;
+ tf->codes[1951] = 0x00000005019f54beUL;
+ tf->codes[1952] = 0x00000006ae4a096aUL;
+ tf->codes[1953] = 0x00000004bce78778UL;
+ tf->codes[1954] = 0x000000094b65b97fUL;
+ tf->codes[1955] = 0x0000000d3f6e7bd2UL;
+ tf->codes[1956] = 0x00000001fbd2a84cUL;
+ tf->codes[1957] = 0x00000006d0127ab1UL;
+ tf->codes[1958] = 0x00000003e82799aaUL;
+ tf->codes[1959] = 0x00000004c1264dfeUL;
+ tf->codes[1960] = 0x0000000cf69c9360UL;
+ tf->codes[1961] = 0x00000004b43e5342UL;
+ tf->codes[1962] = 0x000000035d1f0372UL;
+ tf->codes[1963] = 0x0000000d78c18eb4UL;
+ tf->codes[1964] = 0x0000000262574101UL;
+ tf->codes[1965] = 0x0000000c2c5c7335UL;
+ tf->codes[1966] = 0x0000000bad04051aUL;
+ tf->codes[1967] = 0x00000001c481f94eUL;
+ tf->codes[1968] = 0x00000003285aa0deUL;
+ tf->codes[1969] = 0x00000008973e1f69UL;
+ tf->codes[1970] = 0x00000005d238c694UL;
+ tf->codes[1971] = 0x00000007b71847b9UL;
+ tf->codes[1972] = 0x0000000242f5675cUL;
+ tf->codes[1973] = 0x0000000cc5751c2dUL;
+ tf->codes[1974] = 0x0000000e09bc620bUL;
+ tf->codes[1975] = 0x00000000e4e904ddUL;
+ tf->codes[1976] = 0x000000007ca4f1a7UL;
+ tf->codes[1977] = 0x00000002ac79ae43UL;
+ tf->codes[1978] = 0x0000000e213d4250UL;
+ tf->codes[1979] = 0x0000000d4137c2b5UL;
+ tf->codes[1980] = 0x0000000ddfce11bcUL;
+ tf->codes[1981] = 0x0000000d1d658566UL;
+ tf->codes[1982] = 0x0000000213f5b1bbUL;
+ tf->codes[1983] = 0x0000000cd35be0a8UL;
+ tf->codes[1984] = 0x0000000cc67d7f91UL;
+ tf->codes[1985] = 0x0000000509bde098UL;
+ tf->codes[1986] = 0x000000074d3d8f46UL;
+ tf->codes[1987] = 0x000000051309c970UL;
+ tf->codes[1988] = 0x000000053e2bdf66UL;
+ tf->codes[1989] = 0x0000000a5dd3fed3UL;
+ tf->codes[1990] = 0x0000000a4e69b212UL;
+ tf->codes[1991] = 0x0000000b1d39936dUL;
+ tf->codes[1992] = 0x00000006b6c8926bUL;
+ tf->codes[1993] = 0x000000046540a7b0UL;
+ tf->codes[1994] = 0x00000002eebc599fUL;
+ tf->codes[1995] = 0x00000002e54a283eUL;
+ tf->codes[1996] = 0x0000000f9a328a9cUL;
+ tf->codes[1997] = 0x00000007ea9cfc53UL;
+ tf->codes[1998] = 0x00000005cffa2bdbUL;
+ tf->codes[1999] = 0x0000000464d16f8eUL;
+ tf->codes[2000] = 0x0000000eb09444bcUL;
+ tf->codes[2001] = 0x00000003f341b259UL;
+ tf->codes[2002] = 0x00000004d112b108UL;
+ tf->codes[2003] = 0x000000070cb94242UL;
+ tf->codes[2004] = 0x0000000974ed4ffdUL;
+ tf->codes[2005] = 0x00000001084da291UL;
+ tf->codes[2006] = 0x000000085673ca39UL;
+ tf->codes[2007] = 0x0000000d4d74766fUL;
+ tf->codes[2008] = 0x000000064a68e1deUL;
+ tf->codes[2009] = 0x0000000e35630caeUL;
+ tf->codes[2010] = 0x00000002073229dbUL;
+ tf->codes[2011] = 0x000000063d3a3902UL;
+ tf->codes[2012] = 0x000000031598ee06UL;
+ tf->codes[2013] = 0x0000000808d61126UL;
+ tf->codes[2014] = 0x0000000029957984UL;
+ tf->codes[2015] = 0x0000000d4f5f2649UL;
+ tf->codes[2016] = 0x00000009ec8a706bUL;
+ tf->codes[2017] = 0x0000000349981760UL;
+ tf->codes[2018] = 0x0000000c93ab23a6UL;
+ tf->codes[2019] = 0x00000002c7aa80daUL;
+ tf->codes[2020] = 0x0000000866f102baUL;
+ tf->codes[2021] = 0x0000000b15cff7bcUL;
+ tf->codes[2022] = 0x000000066a13a4caUL;
+ tf->codes[2023] = 0x000000054a755048UL;
+ tf->codes[2024] = 0x0000000d13fdb8d9UL;
+ tf->codes[2025] = 0x000000016ad5edf3UL;
+ tf->codes[2026] = 0x0000000e043bb154UL;
+ tf->codes[2027] = 0x0000000cc8755671UL;
+ tf->codes[2028] = 0x0000000cf9b2bfd5UL;
+ tf->codes[2029] = 0x00000003608890b4UL;
+ tf->codes[2030] = 0x0000000330fef315UL;
+ tf->codes[2031] = 0x0000000e3299ca65UL;
+ tf->codes[2032] = 0x00000000b60765e1UL;
+ tf->codes[2033] = 0x00000000e9bb17dcUL;
+ tf->codes[2034] = 0x000000095f474d8bUL;
+ tf->codes[2035] = 0x0000000e721d3d00UL;
+ tf->codes[2036] = 0x0000000d4679e565UL;
+ tf->codes[2037] = 0x0000000c80da6113UL;
+ tf->codes[2038] = 0x000000098deeff30UL;
+ tf->codes[2039] = 0x0000000c293bb871UL;
+ tf->codes[2040] = 0x0000000e79132f48UL;
+ tf->codes[2041] = 0x0000000b152dafbbUL;
+ tf->codes[2042] = 0x000000055f6a4386UL;
+ tf->codes[2043] = 0x0000000a1b8a4044UL;
+ tf->codes[2044] = 0x00000004f4187b05UL;
+ tf->codes[2045] = 0x00000000b17c2ed3UL;
+ tf->codes[2046] = 0x000000095d75ba04UL;
+ tf->codes[2047] = 0x0000000bbf12e96dUL;
+ tf->codes[2048] = 0x00000006abd1a52fUL;
+ tf->codes[2049] = 0x0000000f300bc991UL;
+ tf->codes[2050] = 0x0000000f0a7385d4UL;
+ tf->codes[2051] = 0x000000052964f82aUL;
+ tf->codes[2052] = 0x0000000a9962925fUL;
+ tf->codes[2053] = 0x0000000613b2eef1UL;
+ tf->codes[2054] = 0x00000005fd2c92a8UL;
+ tf->codes[2055] = 0x000000009ebecd05UL;
+ tf->codes[2056] = 0x000000036002b87aUL;
+ tf->codes[2057] = 0x0000000902c79eefUL;
+ tf->codes[2058] = 0x0000000394e63c7eUL;
+ tf->codes[2059] = 0x0000000133285064UL;
+ tf->codes[2060] = 0x0000000f7cfe2d4bUL;
+ tf->codes[2061] = 0x00000004f068522cUL;
+ tf->codes[2062] = 0x000000096fea1a0fUL;
+ tf->codes[2063] = 0x0000000c5a927b13UL;
+ tf->codes[2064] = 0x0000000e9a2c1994UL;
+ tf->codes[2065] = 0x00000005c53b3803UL;
+ tf->codes[2066] = 0x0000000f636b6188UL;
+ tf->codes[2067] = 0x0000000007c656e3UL;
+ tf->codes[2068] = 0x000000026af1fc5fUL;
+ tf->codes[2069] = 0x0000000ec2f40b78UL;
+ tf->codes[2070] = 0x0000000faa1921e5UL;
+ tf->codes[2071] = 0x00000006137a8b30UL;
+ tf->codes[2072] = 0x0000000028674f7bUL;
+ tf->codes[2073] = 0x00000003de184e35UL;
+ tf->codes[2074] = 0x0000000eeef093e6UL;
+ tf->codes[2075] = 0x0000000d44b3dae0UL;
+ tf->codes[2076] = 0x0000000bb7ab7d93UL;
+ tf->codes[2077] = 0x00000002ae18c956UL;
+ tf->codes[2078] = 0x0000000cde492bd6UL;
+ tf->codes[2079] = 0x00000001cee0216eUL;
+ tf->codes[2080] = 0x0000000f1e5830adUL;
+ tf->codes[2081] = 0x000000076f6c3299UL;
+ tf->codes[2082] = 0x0000000dea24af84UL;
+ tf->codes[2083] = 0x0000000277e75586UL;
+ tf->codes[2084] = 0x0000000a17318024UL;
+ tf->codes[2085] = 0x00000005c4739486UL;
+ tf->codes[2086] = 0x00000005e3de4725UL;
+ tf->codes[2087] = 0x00000006f67c9f6dUL;
+ tf->codes[2088] = 0x000000025f42791dUL;
+ tf->codes[2089] = 0x00000003c54d15b3UL;
+ tf->codes[2090] = 0x0000000ef98d9c32UL;
+ tf->codes[2091] = 0x000000042f64819dUL;
+ tf->codes[2092] = 0x000000016d5fd070UL;
+ tf->codes[2093] = 0x000000063cb98d4fUL;
+ tf->codes[2094] = 0x000000045a3ad27cUL;
+ tf->codes[2095] = 0x00000001b496b0acUL;
+ tf->codes[2096] = 0x0000000aa471c42dUL;
+ tf->codes[2097] = 0x00000000599346a2UL;
+ tf->codes[2098] = 0x00000000dc8d1c2dUL;
+ tf->codes[2099] = 0x00000007498928c1UL;
+ tf->codes[2100] = 0x0000000ea06e90ffUL;
+ tf->codes[2101] = 0x0000000b683baa32UL;
+ tf->codes[2102] = 0x0000000f93014e16UL;
+ tf->codes[2103] = 0x000000020575d56eUL;
+ tf->codes[2104] = 0x0000000794325589UL;
+ tf->codes[2105] = 0x00000001533e9935UL;
+ tf->codes[2106] = 0x000000086b8bcb70UL;
+ tf->codes[2107] = 0x0000000ce11faf5dUL;
+ tf->codes[2108] = 0x000000036c0bd318UL;
+ tf->codes[2109] = 0x0000000e5e8c1167UL;
+ tf->codes[2110] = 0x0000000e1831ba64UL;
+ tf->codes[2111] = 0x0000000e088dbfa4UL;
+ tf->codes[2112] = 0x0000000984479674UL;
+ tf->codes[2113] = 0x0000000afef02b29UL;
+ tf->codes[2114] = 0x000000048518c716UL;
+ tf->codes[2115] = 0x00000004301564ceUL;
+ tf->codes[2116] = 0x000000021cc88710UL;
+ tf->codes[2117] = 0x0000000d5c995278UL;
+ tf->codes[2118] = 0x0000000d8367de1cUL;
+ tf->codes[2119] = 0x00000004a51125e8UL;
+ tf->codes[2120] = 0x0000000113e1c226UL;
+ tf->codes[2121] = 0x0000000ef141e076UL;
+ tf->codes[2122] = 0x000000044097011dUL;
+ tf->codes[2123] = 0x00000004ca9d707cUL;
+ tf->codes[2124] = 0x000000040d8831f1UL;
+ tf->codes[2125] = 0x0000000bd9c3b1d8UL;
+ tf->codes[2126] = 0x0000000978364177UL;
+ tf->codes[2127] = 0x000000010f7606a9UL;
+ tf->codes[2128] = 0x000000046a64270aUL;
+ tf->codes[2129] = 0x000000042df1b22bUL;
+ tf->codes[2130] = 0x0000000e906cf2a0UL;
+ tf->codes[2131] = 0x0000000997da6fa5UL;
+ tf->codes[2132] = 0x0000000a5722c26fUL;
+ tf->codes[2133] = 0x0000000b14f58aaaUL;
+ tf->codes[2134] = 0x0000000afc167ad8UL;
+ tf->codes[2135] = 0x000000037be56e60UL;
+ tf->codes[2136] = 0x0000000de7f80d62UL;
+ tf->codes[2137] = 0x00000000c3fb0a64UL;
+ tf->codes[2138] = 0x0000000ce8ca802cUL;
+ tf->codes[2139] = 0x000000035032ed9dUL;
+ tf->codes[2140] = 0x0000000aa8ba3ee6UL;
+ tf->codes[2141] = 0x000000094b2e707cUL;
+ tf->codes[2142] = 0x00000002debbdae1UL;
+ tf->codes[2143] = 0x0000000f53e25fcfUL;
+ tf->codes[2144] = 0x0000000e935543ebUL;
+ tf->codes[2145] = 0x00000001462f0e90UL;
+ tf->codes[2146] = 0x000000054ce7d18cUL;
+ tf->codes[2147] = 0x00000002ddafdc5fUL;
+ tf->codes[2148] = 0x0000000700565deeUL;
+ tf->codes[2149] = 0x0000000fd408e0afUL;
+ tf->codes[2150] = 0x000000017d089decUL;
+ tf->codes[2151] = 0x0000000833ea2459UL;
+ tf->codes[2152] = 0x00000003c8d3776aUL;
+ tf->codes[2153] = 0x00000002e5eebac8UL;
+ tf->codes[2154] = 0x000000020cbf49b0UL;
+ tf->codes[2155] = 0x0000000c44675eb7UL;
+ tf->codes[2156] = 0x00000003a4b6beb1UL;
+ tf->codes[2157] = 0x0000000ce6f37c1eUL;
+ tf->codes[2158] = 0x000000063fba2e7cUL;
+ tf->codes[2159] = 0x00000005a05b553dUL;
+ tf->codes[2160] = 0x00000001286445b0UL;
+ tf->codes[2161] = 0x00000005e07a9b61UL;
+ tf->codes[2162] = 0x00000007d8397ea4UL;
+ tf->codes[2163] = 0x00000008084b7bbbUL;
+ tf->codes[2164] = 0x0000000b05b38097UL;
+ tf->codes[2165] = 0x000000029c3019eeUL;
+ tf->codes[2166] = 0x0000000ed1d2708bUL;
+ tf->codes[2167] = 0x00000009df8a4d47UL;
+ tf->codes[2168] = 0x0000000e4891e436UL;
+ tf->codes[2169] = 0x00000002a762ab72UL;
+ tf->codes[2170] = 0x000000092f70600fUL;
+ tf->codes[2171] = 0x000000092329a2cdUL;
+ tf->codes[2172] = 0x00000003e200c6edUL;
+ tf->codes[2173] = 0x00000008c0a7233eUL;
+ tf->codes[2174] = 0x000000060866806aUL;
+ tf->codes[2175] = 0x0000000f4fddd24aUL;
+ tf->codes[2176] = 0x0000000f78464c71UL;
+ tf->codes[2177] = 0x00000009c3d22242UL;
+ tf->codes[2178] = 0x00000003877ea6d1UL;
+ tf->codes[2179] = 0x0000000e2a6d54acUL;
+ tf->codes[2180] = 0x0000000497d2a5e7UL;
+ tf->codes[2181] = 0x0000000ca82f781eUL;
+ tf->codes[2182] = 0x0000000481524f4cUL;
+ tf->codes[2183] = 0x0000000dee088814UL;
+ tf->codes[2184] = 0x0000000b2a82d3a4UL;
+ tf->codes[2185] = 0x00000008e6afe6e5UL;
+ tf->codes[2186] = 0x0000000d6279a5daUL;
+ tf->codes[2187] = 0x00000004567cbc1aUL;
+ tf->codes[2188] = 0x00000005bec2b2fdUL;
+ tf->codes[2189] = 0x00000004ef452505UL;
+ tf->codes[2190] = 0x000000061d992cbaUL;
+ tf->codes[2191] = 0x0000000ab96be0cbUL;
+ tf->codes[2192] = 0x0000000708ef35d9UL;
+ tf->codes[2193] = 0x0000000b3f6f3623UL;
+ tf->codes[2194] = 0x000000036eb1801dUL;
+ tf->codes[2195] = 0x0000000badfee917UL;
+ tf->codes[2196] = 0x0000000a3db13cd0UL;
+ tf->codes[2197] = 0x00000001d1a12828UL;
+ tf->codes[2198] = 0x00000002500816ceUL;
+ tf->codes[2199] = 0x0000000cf7612148UL;
+ tf->codes[2200] = 0x00000000be6a3f4bUL;
+ tf->codes[2201] = 0x000000074142f3daUL;
+ tf->codes[2202] = 0x0000000ce5deed92UL;
+ tf->codes[2203] = 0x0000000f9530a786UL;
+ tf->codes[2204] = 0x0000000047c8bb38UL;
+ tf->codes[2205] = 0x0000000fcabfe88fUL;
+ tf->codes[2206] = 0x0000000bc83accb1UL;
+ tf->codes[2207] = 0x000000020cd9fb1fUL;
+ tf->codes[2208] = 0x0000000023dcceb3UL;
+ tf->codes[2209] = 0x00000009e969b8c4UL;
+ tf->codes[2210] = 0x00000006e28de934UL;
+ tf->codes[2211] = 0x000000080a399667UL;
+ tf->codes[2212] = 0x000000076a0b85adUL;
+ tf->codes[2213] = 0x000000021a84be3cUL;
+ tf->codes[2214] = 0x0000000a28d028b5UL;
+ tf->codes[2215] = 0x0000000c4e7690dfUL;
+ tf->codes[2216] = 0x0000000bfd9621e8UL;
+ tf->codes[2217] = 0x00000006f4bc0c24UL;
+ tf->codes[2218] = 0x0000000aa8e76bd7UL;
+ tf->codes[2219] = 0x0000000deb55dac9UL;
+ tf->codes[2220] = 0x0000000bb344fa8bUL;
+ tf->codes[2221] = 0x0000000fcaab4decUL;
+ tf->codes[2222] = 0x0000000146aba6cbUL;
+ tf->codes[2223] = 0x0000000f49ed6eb8UL;
+ tf->codes[2224] = 0x0000000dd57e9deaUL;
+ tf->codes[2225] = 0x0000000225d5d090UL;
+ tf->codes[2226] = 0x0000000d6e86c1c5UL;
+ tf->codes[2227] = 0x0000000639be5f39UL;
+ tf->codes[2228] = 0x0000000f5e7a6132UL;
+ tf->codes[2229] = 0x0000000d2968b09fUL;
+ tf->codes[2230] = 0x000000082b30ba1eUL;
+ tf->codes[2231] = 0x0000000803fa46ccUL;
+ tf->codes[2232] = 0x0000000c290fab00UL;
+ tf->codes[2233] = 0x000000010df59de5UL;
+ tf->codes[2234] = 0x000000051ae9dcfbUL;
+ tf->codes[2235] = 0x000000049af8516dUL;
+ tf->codes[2236] = 0x000000002b564ce6UL;
+ tf->codes[2237] = 0x0000000c615a1de0UL;
+ tf->codes[2238] = 0x0000000fef9864a4UL;
+ tf->codes[2239] = 0x0000000c16e27341UL;
+ tf->codes[2240] = 0x000000039e846736UL;
+ tf->codes[2241] = 0x00000001ecbb6746UL;
+ tf->codes[2242] = 0x0000000588d03a7cUL;
+ tf->codes[2243] = 0x000000010a0eaf9cUL;
+ tf->codes[2244] = 0x0000000671ccea6bUL;
+ tf->codes[2245] = 0x000000033a154603UL;
+ tf->codes[2246] = 0x0000000a7b003bc1UL;
+ tf->codes[2247] = 0x0000000c5fc3848dUL;
+ tf->codes[2248] = 0x000000078e50a9c7UL;
+ tf->codes[2249] = 0x000000017dbfb88eUL;
+ tf->codes[2250] = 0x00000004fd0ed541UL;
+ tf->codes[2251] = 0x000000084221debaUL;
+ tf->codes[2252] = 0x00000003132cf7e6UL;
+ tf->codes[2253] = 0x0000000b67e7ac53UL;
+ tf->codes[2254] = 0x0000000df6b28024UL;
+ tf->codes[2255] = 0x0000000785b9f7edUL;
+ tf->codes[2256] = 0x0000000e3d35320dUL;
+ tf->codes[2257] = 0x0000000159c06583UL;
+ tf->codes[2258] = 0x00000005c54a80a3UL;
+ tf->codes[2259] = 0x0000000ed4d4533bUL;
+ tf->codes[2260] = 0x0000000cf16c601aUL;
+ tf->codes[2261] = 0x00000005e94efbd1UL;
+ tf->codes[2262] = 0x00000005d587126eUL;
+ tf->codes[2263] = 0x0000000eef2f2807UL;
+ tf->codes[2264] = 0x000000009f3c558eUL;
+ tf->codes[2265] = 0x0000000736cfd539UL;
+ tf->codes[2266] = 0x0000000f5a922ae1UL;
+ tf->codes[2267] = 0x00000004e2ab9959UL;
+ tf->codes[2268] = 0x00000006a2dd34e7UL;
+ tf->codes[2269] = 0x00000008c9d30d23UL;
+ tf->codes[2270] = 0x0000000eba20b791UL;
+ tf->codes[2271] = 0x0000000d5c5095e3UL;
+ tf->codes[2272] = 0x0000000423d75a82UL;
+ tf->codes[2273] = 0x000000040cebaafeUL;
+ tf->codes[2274] = 0x000000065e08d288UL;
+ tf->codes[2275] = 0x00000002e4f6d767UL;
+ tf->codes[2276] = 0x0000000fe10d2f21UL;
+ tf->codes[2277] = 0x0000000110347bdaUL;
+ tf->codes[2278] = 0x0000000e43a9bfb3UL;
+ tf->codes[2279] = 0x0000000cdea483ccUL;
+ tf->codes[2280] = 0x0000000fb1e2d8c6UL;
+ tf->codes[2281] = 0x0000000d8a0af7a7UL;
+ tf->codes[2282] = 0x000000037d05b182UL;
+ tf->codes[2283] = 0x00000008d1241d83UL;
+ tf->codes[2284] = 0x0000000da1ea7b6eUL;
+ tf->codes[2285] = 0x000000065bea93dbUL;
+ tf->codes[2286] = 0x00000002a02f8753UL;
+ tf->codes[2287] = 0x0000000454243289UL;
+ tf->codes[2288] = 0x00000004150bc5a2UL;
+ tf->codes[2289] = 0x0000000bbabe5911UL;
+ tf->codes[2290] = 0x00000004cbcdbc59UL;
+ tf->codes[2291] = 0x0000000f0e61340bUL;
+ tf->codes[2292] = 0x000000030a2cdea8UL;
+ tf->codes[2293] = 0x00000005daecb091UL;
+ tf->codes[2294] = 0x00000005dc93d891UL;
+ tf->codes[2295] = 0x0000000c501b4051UL;
+ tf->codes[2296] = 0x0000000782cfba78UL;
+ tf->codes[2297] = 0x00000004c191b61eUL;
+ tf->codes[2298] = 0x0000000b7e27ef35UL;
+ tf->codes[2299] = 0x000000005a476838UL;
+ tf->codes[2300] = 0x00000009b0209574UL;
+ tf->codes[2301] = 0x0000000a775164cfUL;
+ tf->codes[2302] = 0x0000000d33d21701UL;
+ tf->codes[2303] = 0x00000003afcb7d45UL;
+ tf->codes[2304] = 0x00000004df2035cdUL;
+ tf->codes[2305] = 0x0000000498819a21UL;
+ tf->codes[2306] = 0x0000000293f9e506UL;
+ tf->codes[2307] = 0x00000009a35ff1c8UL;
+ tf->codes[2308] = 0x0000000c090ebe6bUL;
+ tf->codes[2309] = 0x0000000a4f0551d4UL;
+ tf->codes[2310] = 0x00000005dc0dc194UL;
+ tf->codes[2311] = 0x00000001388aeb31UL;
+ tf->codes[2312] = 0x0000000340b27bf4UL;
+ tf->codes[2313] = 0x00000003a0f320abUL;
+ tf->codes[2314] = 0x00000000996be75dUL;
+ tf->codes[2315] = 0x0000000b257ecf39UL;
+ tf->codes[2316] = 0x000000078d86f2f1UL;
+ tf->codes[2317] = 0x0000000673f5ff91UL;
+ tf->codes[2318] = 0x00000004538d7e3eUL;
+ tf->codes[2319] = 0x0000000de5bc4369UL;
+ return tf;
+}
+
+void tag36h10_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag36h10.h b/3rdparty/apriltag/tag36h10.h
new file mode 100644
index 0000000..5c95229
--- /dev/null
+++ b/3rdparty/apriltag/tag36h10.h
@@ -0,0 +1,50 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TAG36H10
+#define _TAG36H10
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag36h10_create();
+void tag36h10_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/apriltag/tag36h11.c b/3rdparty/apriltag/tag36h11.c
new file mode 100644
index 0000000..e7c0cf8
--- /dev/null
+++ b/3rdparty/apriltag/tag36h11.c
@@ -0,0 +1,647 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#include <stdlib.h>
+#include "apriltag.h"
+
+apriltag_family_t *tag36h11_create()
+{
+ apriltag_family_t *tf = calloc(1, sizeof(apriltag_family_t));
+#ifdef WINRT
+ tf->name = _strdup("tag36h11");
+#else
+ tf->name = strdup("tag36h11");
+#endif
+ tf->black_border = 1;
+ tf->d = 6;
+ tf->h = 11;
+ tf->ncodes = 587;
+ tf->codes = calloc(587, sizeof(uint64_t));
+ tf->codes[0] = 0x0000000d5d628584UL;
+ tf->codes[1] = 0x0000000d97f18b49UL;
+ tf->codes[2] = 0x0000000dd280910eUL;
+ tf->codes[3] = 0x0000000e479e9c98UL;
+ tf->codes[4] = 0x0000000ebcbca822UL;
+ tf->codes[5] = 0x0000000f31dab3acUL;
+ tf->codes[6] = 0x0000000056a5d085UL;
+ tf->codes[7] = 0x000000010652e1d4UL;
+ tf->codes[8] = 0x000000022b1dfeadUL;
+ tf->codes[9] = 0x0000000265ad0472UL;
+ tf->codes[10] = 0x000000034fe91b86UL;
+ tf->codes[11] = 0x00000003ff962cd5UL;
+ tf->codes[12] = 0x000000043a25329aUL;
+ tf->codes[13] = 0x0000000474b4385fUL;
+ tf->codes[14] = 0x00000004e9d243e9UL;
+ tf->codes[15] = 0x00000005246149aeUL;
+ tf->codes[16] = 0x00000005997f5538UL;
+ tf->codes[17] = 0x0000000683bb6c4cUL;
+ tf->codes[18] = 0x00000006be4a7211UL;
+ tf->codes[19] = 0x00000007e3158eeaUL;
+ tf->codes[20] = 0x000000081da494afUL;
+ tf->codes[21] = 0x0000000858339a74UL;
+ tf->codes[22] = 0x00000008cd51a5feUL;
+ tf->codes[23] = 0x00000009f21cc2d7UL;
+ tf->codes[24] = 0x0000000a2cabc89cUL;
+ tf->codes[25] = 0x0000000adc58d9ebUL;
+ tf->codes[26] = 0x0000000b16e7dfb0UL;
+ tf->codes[27] = 0x0000000b8c05eb3aUL;
+ tf->codes[28] = 0x0000000d25ef139dUL;
+ tf->codes[29] = 0x0000000d607e1962UL;
+ tf->codes[30] = 0x0000000e4aba3076UL;
+ tf->codes[31] = 0x00000002dde6a3daUL;
+ tf->codes[32] = 0x000000043d40c678UL;
+ tf->codes[33] = 0x00000005620be351UL;
+ tf->codes[34] = 0x000000064c47fa65UL;
+ tf->codes[35] = 0x0000000686d7002aUL;
+ tf->codes[36] = 0x00000006c16605efUL;
+ tf->codes[37] = 0x00000006fbf50bb4UL;
+ tf->codes[38] = 0x00000008d06d39dcUL;
+ tf->codes[39] = 0x00000009f53856b5UL;
+ tf->codes[40] = 0x0000000adf746dc9UL;
+ tf->codes[41] = 0x0000000bc9b084ddUL;
+ tf->codes[42] = 0x0000000d290aa77bUL;
+ tf->codes[43] = 0x0000000d9e28b305UL;
+ tf->codes[44] = 0x0000000e4dd5c454UL;
+ tf->codes[45] = 0x0000000fad2fe6f2UL;
+ tf->codes[46] = 0x0000000181a8151aUL;
+ tf->codes[47] = 0x000000026be42c2eUL;
+ tf->codes[48] = 0x00000002e10237b8UL;
+ tf->codes[49] = 0x0000000405cd5491UL;
+ tf->codes[50] = 0x00000007742eab1cUL;
+ tf->codes[51] = 0x000000085e6ac230UL;
+ tf->codes[52] = 0x00000008d388cdbaUL;
+ tf->codes[53] = 0x00000009f853ea93UL;
+ tf->codes[54] = 0x0000000c41ea2445UL;
+ tf->codes[55] = 0x0000000cf1973594UL;
+ tf->codes[56] = 0x000000014a34a333UL;
+ tf->codes[57] = 0x000000031eacd15bUL;
+ tf->codes[58] = 0x00000006c79d2dabUL;
+ tf->codes[59] = 0x000000073cbb3935UL;
+ tf->codes[60] = 0x000000089c155bd3UL;
+ tf->codes[61] = 0x00000008d6a46198UL;
+ tf->codes[62] = 0x000000091133675dUL;
+ tf->codes[63] = 0x0000000a708d89fbUL;
+ tf->codes[64] = 0x0000000ae5ab9585UL;
+ tf->codes[65] = 0x0000000b9558a6d4UL;
+ tf->codes[66] = 0x0000000b98743ab2UL;
+ tf->codes[67] = 0x0000000d6cec68daUL;
+ tf->codes[68] = 0x00000001506bcaefUL;
+ tf->codes[69] = 0x00000004becd217aUL;
+ tf->codes[70] = 0x00000004f95c273fUL;
+ tf->codes[71] = 0x0000000658b649ddUL;
+ tf->codes[72] = 0x0000000a76c4b1b7UL;
+ tf->codes[73] = 0x0000000ecf621f56UL;
+ tf->codes[74] = 0x00000001c8a56a57UL;
+ tf->codes[75] = 0x00000003628e92baUL;
+ tf->codes[76] = 0x000000053706c0e2UL;
+ tf->codes[77] = 0x00000005e6b3d231UL;
+ tf->codes[78] = 0x00000007809cfa94UL;
+ tf->codes[79] = 0x0000000e97eead6fUL;
+ tf->codes[80] = 0x00000005af40604aUL;
+ tf->codes[81] = 0x00000007492988adUL;
+ tf->codes[82] = 0x0000000ed5994712UL;
+ tf->codes[83] = 0x00000005eceaf9edUL;
+ tf->codes[84] = 0x00000007c1632815UL;
+ tf->codes[85] = 0x0000000c1a0095b4UL;
+ tf->codes[86] = 0x0000000e9e25d52bUL;
+ tf->codes[87] = 0x00000003a6705419UL;
+ tf->codes[88] = 0x0000000a8333012fUL;
+ tf->codes[89] = 0x00000004ce5704d0UL;
+ tf->codes[90] = 0x0000000508e60a95UL;
+ tf->codes[91] = 0x0000000877476120UL;
+ tf->codes[92] = 0x0000000a864e950dUL;
+ tf->codes[93] = 0x0000000ea45cfce7UL;
+ tf->codes[94] = 0x000000019da047e8UL;
+ tf->codes[95] = 0x000000024d4d5937UL;
+ tf->codes[96] = 0x00000006e079cc9bUL;
+ tf->codes[97] = 0x000000099f2e11d7UL;
+ tf->codes[98] = 0x000000033aa50429UL;
+ tf->codes[99] = 0x0000000499ff26c7UL;
+ tf->codes[100] = 0x000000050f1d3251UL;
+ tf->codes[101] = 0x000000066e7754efUL;
+ tf->codes[102] = 0x000000096ad633ceUL;
+ tf->codes[103] = 0x00000009a5653993UL;
+ tf->codes[104] = 0x0000000aca30566cUL;
+ tf->codes[105] = 0x0000000c298a790aUL;
+ tf->codes[106] = 0x00000008be44b65dUL;
+ tf->codes[107] = 0x0000000dc68f354bUL;
+ tf->codes[108] = 0x000000016f7f919bUL;
+ tf->codes[109] = 0x00000004dde0e826UL;
+ tf->codes[110] = 0x0000000d548cbd9fUL;
+ tf->codes[111] = 0x0000000e0439ceeeUL;
+ tf->codes[112] = 0x0000000fd8b1fd16UL;
+ tf->codes[113] = 0x000000076521bb7bUL;
+ tf->codes[114] = 0x0000000d92375742UL;
+ tf->codes[115] = 0x0000000cab16d40cUL;
+ tf->codes[116] = 0x0000000730c9dd72UL;
+ tf->codes[117] = 0x0000000ad9ba39c2UL;
+ tf->codes[118] = 0x0000000b14493f87UL;
+ tf->codes[119] = 0x000000052b15651fUL;
+ tf->codes[120] = 0x0000000185409cadUL;
+ tf->codes[121] = 0x000000077ae2c68dUL;
+ tf->codes[122] = 0x000000094f5af4b5UL;
+ tf->codes[123] = 0x00000000a13bad55UL;
+ tf->codes[124] = 0x000000061ea437cdUL;
+ tf->codes[125] = 0x0000000a022399e2UL;
+ tf->codes[126] = 0x0000000203b163d1UL;
+ tf->codes[127] = 0x00000007bba8f40eUL;
+ tf->codes[128] = 0x000000095bc9442dUL;
+ tf->codes[129] = 0x000000041c0b5358UL;
+ tf->codes[130] = 0x00000008e9c6cc81UL;
+ tf->codes[131] = 0x00000000eb549670UL;
+ tf->codes[132] = 0x00000009da3a0b51UL;
+ tf->codes[133] = 0x0000000d832a67a1UL;
+ tf->codes[134] = 0x0000000dcd4350bcUL;
+ tf->codes[135] = 0x00000004aa05fdd2UL;
+ tf->codes[136] = 0x000000060c7bb44eUL;
+ tf->codes[137] = 0x00000004b358b96cUL;
+ tf->codes[138] = 0x0000000067299b45UL;
+ tf->codes[139] = 0x0000000b9c89b5faUL;
+ tf->codes[140] = 0x00000006975acaeaUL;
+ tf->codes[141] = 0x000000062b8f7afaUL;
+ tf->codes[142] = 0x000000033567c3d7UL;
+ tf->codes[143] = 0x0000000bac139950UL;
+ tf->codes[144] = 0x0000000a5927c62aUL;
+ tf->codes[145] = 0x00000005c916e6a4UL;
+ tf->codes[146] = 0x0000000260ecb7d5UL;
+ tf->codes[147] = 0x000000029b7bbd9aUL;
+ tf->codes[148] = 0x0000000903205f26UL;
+ tf->codes[149] = 0x0000000ae72270a4UL;
+ tf->codes[150] = 0x00000003d2ec51a7UL;
+ tf->codes[151] = 0x000000082ea55324UL;
+ tf->codes[152] = 0x000000011a6f3427UL;
+ tf->codes[153] = 0x00000001ca1c4576UL;
+ tf->codes[154] = 0x0000000a40c81aefUL;
+ tf->codes[155] = 0x0000000bddccd730UL;
+ tf->codes[156] = 0x00000000e617561eUL;
+ tf->codes[157] = 0x0000000969317b0fUL;
+ tf->codes[158] = 0x000000067f781364UL;
+ tf->codes[159] = 0x0000000610912f96UL;
+ tf->codes[160] = 0x0000000b2549fdfcUL;
+ tf->codes[161] = 0x000000006e5aaa6bUL;
+ tf->codes[162] = 0x0000000b6c475339UL;
+ tf->codes[163] = 0x0000000c56836a4dUL;
+ tf->codes[164] = 0x0000000844e351ebUL;
+ tf->codes[165] = 0x00000004647f83b4UL;
+ tf->codes[166] = 0x00000000908a04f5UL;
+ tf->codes[167] = 0x00000007f51034c9UL;
+ tf->codes[168] = 0x0000000aee537fcaUL;
+ tf->codes[169] = 0x00000005e92494baUL;
+ tf->codes[170] = 0x0000000d445808f4UL;
+ tf->codes[171] = 0x000000028d68b563UL;
+ tf->codes[172] = 0x000000004d25374bUL;
+ tf->codes[173] = 0x00000002bc065f65UL;
+ tf->codes[174] = 0x000000096dc3ea0cUL;
+ tf->codes[175] = 0x00000004b2ade817UL;
+ tf->codes[176] = 0x000000007c3fd502UL;
+ tf->codes[177] = 0x0000000e768b5cafUL;
+ tf->codes[178] = 0x000000017605cf6cUL;
+ tf->codes[179] = 0x0000000182741ee4UL;
+ tf->codes[180] = 0x000000062846097cUL;
+ tf->codes[181] = 0x000000072b5ebf80UL;
+ tf->codes[182] = 0x0000000263da6e13UL;
+ tf->codes[183] = 0x0000000fa841bcb5UL;
+ tf->codes[184] = 0x00000007e45e8c69UL;
+ tf->codes[185] = 0x0000000653c81fa0UL;
+ tf->codes[186] = 0x00000007443b5e70UL;
+ tf->codes[187] = 0x00000000a5234afdUL;
+ tf->codes[188] = 0x000000074756f24eUL;
+ tf->codes[189] = 0x0000000157ebf02aUL;
+ tf->codes[190] = 0x000000082ef46939UL;
+ tf->codes[191] = 0x000000080d420264UL;
+ tf->codes[192] = 0x00000002aeed3e98UL;
+ tf->codes[193] = 0x0000000b0a1dd4f8UL;
+ tf->codes[194] = 0x0000000b5436be13UL;
+ tf->codes[195] = 0x00000007b7b4b13bUL;
+ tf->codes[196] = 0x00000001ce80d6d3UL;
+ tf->codes[197] = 0x000000016c08427dUL;
+ tf->codes[198] = 0x0000000ee54462ddUL;
+ tf->codes[199] = 0x00000001f7644cceUL;
+ tf->codes[200] = 0x00000009c7b5cc92UL;
+ tf->codes[201] = 0x0000000e369138f8UL;
+ tf->codes[202] = 0x00000005d5a66e91UL;
+ tf->codes[203] = 0x0000000485d62f49UL;
+ tf->codes[204] = 0x0000000e6e819e94UL;
+ tf->codes[205] = 0x0000000b1f340eb5UL;
+ tf->codes[206] = 0x000000009d198ce2UL;
+ tf->codes[207] = 0x0000000d60717437UL;
+ tf->codes[208] = 0x00000000196b856cUL;
+ tf->codes[209] = 0x0000000f0a6173a5UL;
+ tf->codes[210] = 0x000000012c0e1ec6UL;
+ tf->codes[211] = 0x000000062b82d5cfUL;
+ tf->codes[212] = 0x0000000ad154c067UL;
+ tf->codes[213] = 0x0000000ce3778832UL;
+ tf->codes[214] = 0x00000006b0a7b864UL;
+ tf->codes[215] = 0x00000004c7686694UL;
+ tf->codes[216] = 0x00000005058ff3ecUL;
+ tf->codes[217] = 0x0000000d5e21ea23UL;
+ tf->codes[218] = 0x00000009ff4a76eeUL;
+ tf->codes[219] = 0x00000009dd981019UL;
+ tf->codes[220] = 0x00000001bad4d30aUL;
+ tf->codes[221] = 0x0000000c601896d1UL;
+ tf->codes[222] = 0x0000000973439b48UL;
+ tf->codes[223] = 0x00000001ce7431a8UL;
+ tf->codes[224] = 0x000000057a8021d6UL;
+ tf->codes[225] = 0x0000000f9dba96e6UL;
+ tf->codes[226] = 0x000000083a2e4e7cUL;
+ tf->codes[227] = 0x00000008ea585380UL;
+ tf->codes[228] = 0x0000000af6c0e744UL;
+ tf->codes[229] = 0x0000000875b73babUL;
+ tf->codes[230] = 0x0000000da34ca901UL;
+ tf->codes[231] = 0x00000002ab9727efUL;
+ tf->codes[232] = 0x0000000d39f21b9aUL;
+ tf->codes[233] = 0x00000008a10b742fUL;
+ tf->codes[234] = 0x00000005f8952dbaUL;
+ tf->codes[235] = 0x0000000f8da71ab0UL;
+ tf->codes[236] = 0x0000000c25f9df96UL;
+ tf->codes[237] = 0x000000006f8a5d94UL;
+ tf->codes[238] = 0x0000000e42e63e1aUL;
+ tf->codes[239] = 0x0000000b78409d1bUL;
+ tf->codes[240] = 0x0000000792229addUL;
+ tf->codes[241] = 0x00000005acf8c455UL;
+ tf->codes[242] = 0x00000002fc29a9b0UL;
+ tf->codes[243] = 0x0000000ea486237bUL;
+ tf->codes[244] = 0x0000000b0c9685a0UL;
+ tf->codes[245] = 0x00000001ad748a47UL;
+ tf->codes[246] = 0x000000003b4712d5UL;
+ tf->codes[247] = 0x0000000f29216d30UL;
+ tf->codes[248] = 0x00000008dad65e49UL;
+ tf->codes[249] = 0x00000000a2cf09ddUL;
+ tf->codes[250] = 0x00000000b5f174c6UL;
+ tf->codes[251] = 0x0000000e54f57743UL;
+ tf->codes[252] = 0x0000000b9cf54d78UL;
+ tf->codes[253] = 0x00000004a312a88aUL;
+ tf->codes[254] = 0x000000027babc962UL;
+ tf->codes[255] = 0x0000000b86897111UL;
+ tf->codes[256] = 0x0000000f2ff6c116UL;
+ tf->codes[257] = 0x000000082274bd8aUL;
+ tf->codes[258] = 0x000000097023505eUL;
+ tf->codes[259] = 0x000000052d46edd1UL;
+ tf->codes[260] = 0x0000000585c1f538UL;
+ tf->codes[261] = 0x0000000bddd00e43UL;
+ tf->codes[262] = 0x00000005590b74dfUL;
+ tf->codes[263] = 0x0000000729404a1fUL;
+ tf->codes[264] = 0x000000065320855eUL;
+ tf->codes[265] = 0x0000000d3d4b6956UL;
+ tf->codes[266] = 0x00000007ae374f14UL;
+ tf->codes[267] = 0x00000002d7a60e06UL;
+ tf->codes[268] = 0x0000000315cd9b5eUL;
+ tf->codes[269] = 0x0000000fd36b4eacUL;
+ tf->codes[270] = 0x0000000f1df7642bUL;
+ tf->codes[271] = 0x000000055db27726UL;
+ tf->codes[272] = 0x00000008f15ebc19UL;
+ tf->codes[273] = 0x0000000992f8c531UL;
+ tf->codes[274] = 0x000000062dea2a40UL;
+ tf->codes[275] = 0x0000000928275cabUL;
+ tf->codes[276] = 0x000000069c263cb9UL;
+ tf->codes[277] = 0x0000000a774cca9eUL;
+ tf->codes[278] = 0x0000000266b2110eUL;
+ tf->codes[279] = 0x00000001b14acbb8UL;
+ tf->codes[280] = 0x0000000624b8a71bUL;
+ tf->codes[281] = 0x00000001c539406bUL;
+ tf->codes[282] = 0x00000003086d529bUL;
+ tf->codes[283] = 0x00000000111dd66eUL;
+ tf->codes[284] = 0x000000098cd630bfUL;
+ tf->codes[285] = 0x00000008b9d1ffdcUL;
+ tf->codes[286] = 0x000000072b2f61e7UL;
+ tf->codes[287] = 0x00000009ed9d672bUL;
+ tf->codes[288] = 0x000000096cdd15f3UL;
+ tf->codes[289] = 0x00000006366c2504UL;
+ tf->codes[290] = 0x00000006ca9df73aUL;
+ tf->codes[291] = 0x0000000a066d60f0UL;
+ tf->codes[292] = 0x0000000e7a4b8addUL;
+ tf->codes[293] = 0x00000008264647efUL;
+ tf->codes[294] = 0x0000000aa195bf81UL;
+ tf->codes[295] = 0x00000009a3db8244UL;
+ tf->codes[296] = 0x0000000014d2df6aUL;
+ tf->codes[297] = 0x00000000b63265b7UL;
+ tf->codes[298] = 0x00000002f010de73UL;
+ tf->codes[299] = 0x000000097e774986UL;
+ tf->codes[300] = 0x0000000248affc29UL;
+ tf->codes[301] = 0x0000000fb57dcd11UL;
+ tf->codes[302] = 0x00000000b1a7e4d9UL;
+ tf->codes[303] = 0x00000004bfa2d07dUL;
+ tf->codes[304] = 0x000000054e5cdf96UL;
+ tf->codes[305] = 0x00000004c15c1c86UL;
+ tf->codes[306] = 0x0000000cd9c61166UL;
+ tf->codes[307] = 0x0000000499380b2aUL;
+ tf->codes[308] = 0x0000000540308d09UL;
+ tf->codes[309] = 0x00000008b63fe66fUL;
+ tf->codes[310] = 0x0000000c81aeb35eUL;
+ tf->codes[311] = 0x000000086fe0bd5cUL;
+ tf->codes[312] = 0x0000000ce2480c2aUL;
+ tf->codes[313] = 0x00000001ab29ee60UL;
+ tf->codes[314] = 0x00000008048daa15UL;
+ tf->codes[315] = 0x0000000dbfeb2d39UL;
+ tf->codes[316] = 0x0000000567c9858cUL;
+ tf->codes[317] = 0x00000002b6edc5bcUL;
+ tf->codes[318] = 0x00000002078fca82UL;
+ tf->codes[319] = 0x0000000adacc22aaUL;
+ tf->codes[320] = 0x0000000b92486f49UL;
+ tf->codes[321] = 0x000000051fac5964UL;
+ tf->codes[322] = 0x0000000691ee6420UL;
+ tf->codes[323] = 0x0000000f63b3e129UL;
+ tf->codes[324] = 0x000000039be7e572UL;
+ tf->codes[325] = 0x0000000da2ce6c74UL;
+ tf->codes[326] = 0x000000020cf17a5cUL;
+ tf->codes[327] = 0x0000000ee55f9b6eUL;
+ tf->codes[328] = 0x0000000fb8572726UL;
+ tf->codes[329] = 0x0000000b2c2de548UL;
+ tf->codes[330] = 0x0000000caa9bce92UL;
+ tf->codes[331] = 0x0000000ae9182db3UL;
+ tf->codes[332] = 0x000000074b6e5bd1UL;
+ tf->codes[333] = 0x0000000137b252afUL;
+ tf->codes[334] = 0x000000051f686881UL;
+ tf->codes[335] = 0x0000000d672f6c02UL;
+ tf->codes[336] = 0x0000000654146ce4UL;
+ tf->codes[337] = 0x0000000f944bc825UL;
+ tf->codes[338] = 0x0000000e8327f809UL;
+ tf->codes[339] = 0x000000076a73fd59UL;
+ tf->codes[340] = 0x0000000f79da4cb4UL;
+ tf->codes[341] = 0x0000000956f8099bUL;
+ tf->codes[342] = 0x00000007b5f2655cUL;
+ tf->codes[343] = 0x0000000d06b114a6UL;
+ tf->codes[344] = 0x0000000d0697ca50UL;
+ tf->codes[345] = 0x000000027c390797UL;
+ tf->codes[346] = 0x0000000bc61ed9b2UL;
+ tf->codes[347] = 0x0000000cc12dd19bUL;
+ tf->codes[348] = 0x0000000eb7818d2cUL;
+ tf->codes[349] = 0x0000000092fcecdaUL;
+ tf->codes[350] = 0x000000089ded4ea1UL;
+ tf->codes[351] = 0x0000000256a0ba34UL;
+ tf->codes[352] = 0x0000000b6948e627UL;
+ tf->codes[353] = 0x00000001ef6b1054UL;
+ tf->codes[354] = 0x00000008639294a2UL;
+ tf->codes[355] = 0x0000000eda3780a4UL;
+ tf->codes[356] = 0x000000039ee2af1dUL;
+ tf->codes[357] = 0x0000000cd257edc5UL;
+ tf->codes[358] = 0x00000002d9d6bc22UL;
+ tf->codes[359] = 0x0000000121d3b47dUL;
+ tf->codes[360] = 0x000000037e23f8adUL;
+ tf->codes[361] = 0x0000000119f31cf6UL;
+ tf->codes[362] = 0x00000002c97f4f09UL;
+ tf->codes[363] = 0x0000000d502abfe0UL;
+ tf->codes[364] = 0x000000010bc3ca77UL;
+ tf->codes[365] = 0x000000053d7190efUL;
+ tf->codes[366] = 0x000000090c3e62a6UL;
+ tf->codes[367] = 0x00000007e9ebf675UL;
+ tf->codes[368] = 0x0000000979ce23d1UL;
+ tf->codes[369] = 0x000000027f0c98e9UL;
+ tf->codes[370] = 0x0000000eafb4ae59UL;
+ tf->codes[371] = 0x00000007ca7fe2bdUL;
+ tf->codes[372] = 0x00000001490ca8f6UL;
+ tf->codes[373] = 0x00000009123387baUL;
+ tf->codes[374] = 0x0000000b3bc73888UL;
+ tf->codes[375] = 0x00000003ea87e325UL;
+ tf->codes[376] = 0x00000004888964aaUL;
+ tf->codes[377] = 0x0000000a0188a6b9UL;
+ tf->codes[378] = 0x0000000cd383c666UL;
+ tf->codes[379] = 0x000000040029a3fdUL;
+ tf->codes[380] = 0x0000000e1c00ac5cUL;
+ tf->codes[381] = 0x000000039e6f2b6eUL;
+ tf->codes[382] = 0x0000000de664f622UL;
+ tf->codes[383] = 0x0000000e979a75e8UL;
+ tf->codes[384] = 0x00000007c6b4c86cUL;
+ tf->codes[385] = 0x0000000fd492e071UL;
+ tf->codes[386] = 0x00000008fbb35118UL;
+ tf->codes[387] = 0x000000040b4a09b7UL;
+ tf->codes[388] = 0x0000000af80bd6daUL;
+ tf->codes[389] = 0x000000070e0b2521UL;
+ tf->codes[390] = 0x00000002f5c54d93UL;
+ tf->codes[391] = 0x00000003f4a118d5UL;
+ tf->codes[392] = 0x000000009c1897b9UL;
+ tf->codes[393] = 0x0000000079776eacUL;
+ tf->codes[394] = 0x0000000084b00b17UL;
+ tf->codes[395] = 0x00000003a95ad90eUL;
+ tf->codes[396] = 0x000000028c544095UL;
+ tf->codes[397] = 0x000000039d457c05UL;
+ tf->codes[398] = 0x00000007a3791a78UL;
+ tf->codes[399] = 0x0000000bb770e22eUL;
+ tf->codes[400] = 0x00000009a822bd6cUL;
+ tf->codes[401] = 0x000000068a4b1fedUL;
+ tf->codes[402] = 0x0000000a5fd27b3bUL;
+ tf->codes[403] = 0x00000000c3995b79UL;
+ tf->codes[404] = 0x0000000d1519dff1UL;
+ tf->codes[405] = 0x00000008e7eee359UL;
+ tf->codes[406] = 0x0000000cd3ca50b1UL;
+ tf->codes[407] = 0x0000000b73b8b793UL;
+ tf->codes[408] = 0x000000057aca1c43UL;
+ tf->codes[409] = 0x0000000ec2655277UL;
+ tf->codes[410] = 0x0000000785a2c1b3UL;
+ tf->codes[411] = 0x000000075a07985aUL;
+ tf->codes[412] = 0x0000000a4b01eb69UL;
+ tf->codes[413] = 0x0000000a18a11347UL;
+ tf->codes[414] = 0x0000000db1f28ca3UL;
+ tf->codes[415] = 0x0000000877ec3e25UL;
+ tf->codes[416] = 0x000000031f6341b8UL;
+ tf->codes[417] = 0x00000001363a3a4cUL;
+ tf->codes[418] = 0x0000000075d8b9baUL;
+ tf->codes[419] = 0x00000007ae0792a9UL;
+ tf->codes[420] = 0x0000000a83a21651UL;
+ tf->codes[421] = 0x00000007f08f9fb5UL;
+ tf->codes[422] = 0x00000000d0cf73a9UL;
+ tf->codes[423] = 0x0000000b04dcc98eUL;
+ tf->codes[424] = 0x0000000f65c7b0f8UL;
+ tf->codes[425] = 0x000000065ddaf69aUL;
+ tf->codes[426] = 0x00000002cf9b86b3UL;
+ tf->codes[427] = 0x000000014cb51e25UL;
+ tf->codes[428] = 0x0000000f48027b5bUL;
+ tf->codes[429] = 0x00000000ec26ea8bUL;
+ tf->codes[430] = 0x000000044bafd45cUL;
+ tf->codes[431] = 0x0000000b12c7c0c4UL;
+ tf->codes[432] = 0x0000000959fd9d82UL;
+ tf->codes[433] = 0x0000000c77c9725aUL;
+ tf->codes[434] = 0x000000048a22d462UL;
+ tf->codes[435] = 0x00000008398e8072UL;
+ tf->codes[436] = 0x0000000ec89b05ceUL;
+ tf->codes[437] = 0x0000000bb682d4c9UL;
+ tf->codes[438] = 0x0000000e5a86d2ffUL;
+ tf->codes[439] = 0x0000000358f01134UL;
+ tf->codes[440] = 0x00000008556ddcf6UL;
+ tf->codes[441] = 0x000000067584b6e2UL;
+ tf->codes[442] = 0x000000011609439fUL;
+ tf->codes[443] = 0x000000008488816eUL;
+ tf->codes[444] = 0x0000000aaf1a2c46UL;
+ tf->codes[445] = 0x0000000f879898cfUL;
+ tf->codes[446] = 0x00000008bbe5e2f7UL;
+ tf->codes[447] = 0x0000000101eee363UL;
+ tf->codes[448] = 0x0000000690f69377UL;
+ tf->codes[449] = 0x0000000f5bd93cd9UL;
+ tf->codes[450] = 0x0000000cea4c2bf6UL;
+ tf->codes[451] = 0x00000009550be706UL;
+ tf->codes[452] = 0x00000002c5b38a60UL;
+ tf->codes[453] = 0x0000000e72033547UL;
+ tf->codes[454] = 0x00000004458b0629UL;
+ tf->codes[455] = 0x0000000ee8d9ed41UL;
+ tf->codes[456] = 0x0000000d2f918d72UL;
+ tf->codes[457] = 0x000000078dc39fd3UL;
+ tf->codes[458] = 0x00000008212636f6UL;
+ tf->codes[459] = 0x00000007450a72a7UL;
+ tf->codes[460] = 0x0000000c4f0cf4c6UL;
+ tf->codes[461] = 0x0000000367bcddcdUL;
+ tf->codes[462] = 0x0000000c1caf8cc6UL;
+ tf->codes[463] = 0x0000000a7f5b853dUL;
+ tf->codes[464] = 0x00000009d536818bUL;
+ tf->codes[465] = 0x0000000535e021b0UL;
+ tf->codes[466] = 0x0000000a7eb8729eUL;
+ tf->codes[467] = 0x0000000422a67b49UL;
+ tf->codes[468] = 0x0000000929e928a6UL;
+ tf->codes[469] = 0x000000048e8aefccUL;
+ tf->codes[470] = 0x0000000a9897393cUL;
+ tf->codes[471] = 0x00000005eb81d37eUL;
+ tf->codes[472] = 0x00000001e80287b7UL;
+ tf->codes[473] = 0x000000034770d903UL;
+ tf->codes[474] = 0x00000002eef86728UL;
+ tf->codes[475] = 0x000000059266ccb6UL;
+ tf->codes[476] = 0x00000000110bba61UL;
+ tf->codes[477] = 0x00000001dfd284efUL;
+ tf->codes[478] = 0x0000000447439d1bUL;
+ tf->codes[479] = 0x0000000fece0e599UL;
+ tf->codes[480] = 0x00000009309f3703UL;
+ tf->codes[481] = 0x000000080764d1ddUL;
+ tf->codes[482] = 0x0000000353f1e6a0UL;
+ tf->codes[483] = 0x00000002c1c12dccUL;
+ tf->codes[484] = 0x0000000c1d21b9d7UL;
+ tf->codes[485] = 0x0000000457ee453eUL;
+ tf->codes[486] = 0x0000000d66faf540UL;
+ tf->codes[487] = 0x000000044831e652UL;
+ tf->codes[488] = 0x0000000cfd49a848UL;
+ tf->codes[489] = 0x00000009312d4133UL;
+ tf->codes[490] = 0x00000003f097d3eeUL;
+ tf->codes[491] = 0x00000008c9ebef7aUL;
+ tf->codes[492] = 0x0000000a99e29e88UL;
+ tf->codes[493] = 0x00000000e9fab22cUL;
+ tf->codes[494] = 0x00000004e748f4fbUL;
+ tf->codes[495] = 0x0000000ecdee4288UL;
+ tf->codes[496] = 0x0000000abce5f1d0UL;
+ tf->codes[497] = 0x0000000c42f6876cUL;
+ tf->codes[498] = 0x00000007ed402ea0UL;
+ tf->codes[499] = 0x0000000e5c4242c3UL;
+ tf->codes[500] = 0x0000000d5b2c31aeUL;
+ tf->codes[501] = 0x0000000286863be6UL;
+ tf->codes[502] = 0x0000000160444d94UL;
+ tf->codes[503] = 0x00000005f0f5808eUL;
+ tf->codes[504] = 0x0000000ae3d44b2aUL;
+ tf->codes[505] = 0x00000009f5c5d109UL;
+ tf->codes[506] = 0x00000008ad9316d7UL;
+ tf->codes[507] = 0x00000003422ba064UL;
+ tf->codes[508] = 0x00000002fed11d56UL;
+ tf->codes[509] = 0x0000000bea6e3e04UL;
+ tf->codes[510] = 0x000000004b029eecUL;
+ tf->codes[511] = 0x00000006deed7435UL;
+ tf->codes[512] = 0x00000003718ce17cUL;
+ tf->codes[513] = 0x000000055857f5e2UL;
+ tf->codes[514] = 0x00000002edac7b62UL;
+ tf->codes[515] = 0x0000000085d6c512UL;
+ tf->codes[516] = 0x0000000d6ca88e0fUL;
+ tf->codes[517] = 0x00000002b7e1fc69UL;
+ tf->codes[518] = 0x0000000a699d5c1bUL;
+ tf->codes[519] = 0x0000000f05ad74deUL;
+ tf->codes[520] = 0x00000004cf5fb56dUL;
+ tf->codes[521] = 0x00000005725e07e1UL;
+ tf->codes[522] = 0x000000072f18a2deUL;
+ tf->codes[523] = 0x00000001cec52609UL;
+ tf->codes[524] = 0x000000048534243cUL;
+ tf->codes[525] = 0x00000002523a4d69UL;
+ tf->codes[526] = 0x000000035c1b80d1UL;
+ tf->codes[527] = 0x0000000a4d7338a7UL;
+ tf->codes[528] = 0x00000000db1af012UL;
+ tf->codes[529] = 0x0000000e61a9475dUL;
+ tf->codes[530] = 0x000000005df03f91UL;
+ tf->codes[531] = 0x000000097ae260bbUL;
+ tf->codes[532] = 0x000000032d627fefUL;
+ tf->codes[533] = 0x0000000b640f73c2UL;
+ tf->codes[534] = 0x000000045a1ac9c6UL;
+ tf->codes[535] = 0x00000006a2202de1UL;
+ tf->codes[536] = 0x000000057d3e25f2UL;
+ tf->codes[537] = 0x00000005aa9f986eUL;
+ tf->codes[538] = 0x00000000cc859d8aUL;
+ tf->codes[539] = 0x0000000e3ec6cca8UL;
+ tf->codes[540] = 0x000000054e95e1aeUL;
+ tf->codes[541] = 0x0000000446887b06UL;
+ tf->codes[542] = 0x00000007516732beUL;
+ tf->codes[543] = 0x00000003817ac8f5UL;
+ tf->codes[544] = 0x00000003e26d938cUL;
+ tf->codes[545] = 0x0000000aa81bc235UL;
+ tf->codes[546] = 0x0000000df387ca1bUL;
+ tf->codes[547] = 0x00000000f3a3b3f2UL;
+ tf->codes[548] = 0x0000000b4bf69677UL;
+ tf->codes[549] = 0x0000000ae21868edUL;
+ tf->codes[550] = 0x000000081e1d2d9dUL;
+ tf->codes[551] = 0x0000000a0a9ea14cUL;
+ tf->codes[552] = 0x00000008eee297a9UL;
+ tf->codes[553] = 0x00000004740c0559UL;
+ tf->codes[554] = 0x0000000e8b141837UL;
+ tf->codes[555] = 0x0000000ac69e0a3dUL;
+ tf->codes[556] = 0x00000009ed83a1e1UL;
+ tf->codes[557] = 0x00000005edb55ecbUL;
+ tf->codes[558] = 0x000000007340fe81UL;
+ tf->codes[559] = 0x000000050dfbc6bfUL;
+ tf->codes[560] = 0x00000004f583508aUL;
+ tf->codes[561] = 0x0000000cb1fb78bcUL;
+ tf->codes[562] = 0x00000004025ced2fUL;
+ tf->codes[563] = 0x000000039791ebecUL;
+ tf->codes[564] = 0x000000053ee388f1UL;
+ tf->codes[565] = 0x00000007d6c0bd23UL;
+ tf->codes[566] = 0x000000093a995fbeUL;
+ tf->codes[567] = 0x00000008a41728deUL;
+ tf->codes[568] = 0x00000002fe70e053UL;
+ tf->codes[569] = 0x0000000ab3db443aUL;
+ tf->codes[570] = 0x00000001364edb05UL;
+ tf->codes[571] = 0x000000047b6eeed6UL;
+ tf->codes[572] = 0x000000012e71af01UL;
+ tf->codes[573] = 0x000000052ff83587UL;
+ tf->codes[574] = 0x00000003a1575dd8UL;
+ tf->codes[575] = 0x00000003feaa3564UL;
+ tf->codes[576] = 0x0000000eacf78ba7UL;
+ tf->codes[577] = 0x00000000872b94f8UL;
+ tf->codes[578] = 0x0000000da8ddf9a2UL;
+ tf->codes[579] = 0x00000009aa920d2bUL;
+ tf->codes[580] = 0x00000001f350ed36UL;
+ tf->codes[581] = 0x000000018a5e861fUL;
+ tf->codes[582] = 0x00000002c35b89c3UL;
+ tf->codes[583] = 0x00000003347ac48aUL;
+ tf->codes[584] = 0x00000007f23e022eUL;
+ tf->codes[585] = 0x00000002459068fbUL;
+ tf->codes[586] = 0x0000000e83be4b73UL;
+ return tf;
+}
+
+void tag36h11_destroy(apriltag_family_t *tf)
+{
+ free(tf->name);
+ free(tf->codes);
+ free(tf);
+}
diff --git a/3rdparty/apriltag/tag36h11.h b/3rdparty/apriltag/tag36h11.h
new file mode 100644
index 0000000..b04290f
--- /dev/null
+++ b/3rdparty/apriltag/tag36h11.h
@@ -0,0 +1,50 @@
+/* Copyright (C) 2013-2016, The Regents of The University of Michigan.
+All rights reserved.
+
+This software was developed in the APRIL Robotics Lab under the
+direction of Edwin Olson, ebolson at umich.edu. This software may be
+available under alternative licensing terms; contact the address above.
+
+An unlimited license is granted to use, adapt, modify, or embed the 2D
+barcodes into any medium.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+ list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright notice,
+ this list of conditions and the following disclaimer in the documentation
+ and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+The views and conclusions contained in the software and documentation are those
+of the authors and should not be interpreted as representing official policies,
+either expressed or implied, of the Regents of The University of Michigan.
+*/
+
+#ifndef _TAG36H11
+#define _TAG36H11
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+apriltag_family_t *tag36h11_create();
+void tag36h11_destroy(apriltag_family_t *tf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/3rdparty/atidaq/CMakeLists.txt b/3rdparty/atidaq/CMakeLists.txt
new file mode 100644
index 0000000..b588e23
--- /dev/null
+++ b/3rdparty/atidaq/CMakeLists.txt
@@ -0,0 +1,41 @@
+project(${ATIDAQ_LIBRARY})
+
+set(ATIDAQ_MAJOR_VERSION 1 PARENT_SCOPE)
+set(ATIDAQ_MINOR_VERSION 0 PARENT_SCOPE)
+set(ATIDAQ_PATCH_VERSION 6 PARENT_SCOPE)
+
+vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+file(GLOB lib_srcs *.c)
+file(GLOB lib_hdrs *.h)
+
+vp_list_filterout(lib_srcs xmltok_impl.c)
+vp_list_filterout(lib_hdrs xmltok_impl.h)
+vp_list_filterout(lib_srcs xmltok_ns.c)
+vp_list_filterout(lib_srcs events.c)
+
+add_library(${ATIDAQ_LIBRARY} STATIC ${lib_srcs} ${lib_hdrs})
+
+if(UNIX)
+ if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
+ endif()
+endif()
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${ATIDAQ_LIBRARY} PROPERTIES FOLDER "3rdparty")
+endif()
+
+vp_warnings_disable(CMAKE_C_FLAGS -Wunused-parameter)
+
+set_target_properties(${ATIDAQ_LIBRARY}
+ PROPERTIES OUTPUT_NAME ${ATIDAQ_LIBRARY}
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ COMPILE_PDB_NAME ${ATIDAQ_LIBRARY}
+ COMPILE_PDB_NAME_DEBUG "${ATIDAQ_LIBRARY}${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
+ )
+
+if(NOT BUILD_SHARED_LIBS)
+ vp_install_target(${ATIDAQ_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
+endif()
diff --git a/3rdparty/libatidaq-c/ascii.h b/3rdparty/atidaq/ascii.h
similarity index 100%
rename from 3rdparty/libatidaq-c/ascii.h
rename to 3rdparty/atidaq/ascii.h
diff --git a/3rdparty/libatidaq-c/asciitab.h b/3rdparty/atidaq/asciitab.h
similarity index 100%
rename from 3rdparty/libatidaq-c/asciitab.h
rename to 3rdparty/atidaq/asciitab.h
diff --git a/3rdparty/libatidaq-c/dom.c b/3rdparty/atidaq/dom.c
similarity index 100%
rename from 3rdparty/libatidaq-c/dom.c
rename to 3rdparty/atidaq/dom.c
diff --git a/3rdparty/libatidaq-c/dom.h b/3rdparty/atidaq/dom.h
similarity index 100%
rename from 3rdparty/libatidaq-c/dom.h
rename to 3rdparty/atidaq/dom.h
diff --git a/3rdparty/libatidaq-c/events.c b/3rdparty/atidaq/events.c
similarity index 100%
rename from 3rdparty/libatidaq-c/events.c
rename to 3rdparty/atidaq/events.c
diff --git a/3rdparty/libatidaq-c/expatls.c b/3rdparty/atidaq/expatls.c
similarity index 100%
rename from 3rdparty/libatidaq-c/expatls.c
rename to 3rdparty/atidaq/expatls.c
diff --git a/3rdparty/libatidaq-c/ftconfig.c b/3rdparty/atidaq/ftconfig.c
similarity index 100%
rename from 3rdparty/libatidaq-c/ftconfig.c
rename to 3rdparty/atidaq/ftconfig.c
diff --git a/3rdparty/libatidaq-c/ftconfig.h b/3rdparty/atidaq/ftconfig.h
similarity index 100%
rename from 3rdparty/libatidaq-c/ftconfig.h
rename to 3rdparty/atidaq/ftconfig.h
diff --git a/3rdparty/libatidaq-c/ftrt.c b/3rdparty/atidaq/ftrt.c
similarity index 100%
rename from 3rdparty/libatidaq-c/ftrt.c
rename to 3rdparty/atidaq/ftrt.c
diff --git a/3rdparty/libatidaq-c/ftrt.h b/3rdparty/atidaq/ftrt.h
similarity index 100%
rename from 3rdparty/libatidaq-c/ftrt.h
rename to 3rdparty/atidaq/ftrt.h
diff --git a/3rdparty/libatidaq-c/ftsharedrt.h b/3rdparty/atidaq/ftsharedrt.h
similarity index 100%
rename from 3rdparty/libatidaq-c/ftsharedrt.h
rename to 3rdparty/atidaq/ftsharedrt.h
diff --git a/3rdparty/libatidaq-c/iasciitab.h b/3rdparty/atidaq/iasciitab.h
similarity index 100%
rename from 3rdparty/libatidaq-c/iasciitab.h
rename to 3rdparty/atidaq/iasciitab.h
diff --git a/3rdparty/libatidaq-c/latin1tab.h b/3rdparty/atidaq/latin1tab.h
similarity index 100%
rename from 3rdparty/libatidaq-c/latin1tab.h
rename to 3rdparty/atidaq/latin1tab.h
diff --git a/3rdparty/libatidaq-c/nametab.h b/3rdparty/atidaq/nametab.h
similarity index 100%
rename from 3rdparty/libatidaq-c/nametab.h
rename to 3rdparty/atidaq/nametab.h
diff --git a/3rdparty/libatidaq-c/node.c b/3rdparty/atidaq/node.c
similarity index 100%
rename from 3rdparty/libatidaq-c/node.c
rename to 3rdparty/atidaq/node.c
diff --git a/3rdparty/atidaq/readme.txt b/3rdparty/atidaq/readme.txt
new file mode 100644
index 0000000..00c953f
--- /dev/null
+++ b/3rdparty/atidaq/readme.txt
@@ -0,0 +1,139 @@
+ATI DAQ F/T C Library (v1.0.6)
+-----------------------------------------
+
+The purpose of this C library is to load and parse calibration files for ATI
+DAQ F/T transducers, and perform the necessary calculations to convert raw
+voltages from a data acquisition system into forces and torques.
+
+Neither the library itself or the sample applications provide hardware I/O.
+These functions must be provided by your hardware's device drivers.
+
+If you are developing an application for the Windows environment, we
+recommend you use the ATIDAQFT ActiveX Automation Server or the
+ATICombinedDAQFT .NET assembly.
+
+
+Licensing
+---------
+All of the software including in this package is distributed under the MIT License:
+
+Permission is hereby granted, free of charge, to any person obtaining a copy of
+this software and associated documentation files (the "Software"), to deal in
+the Software without restriction, including without limitation the rights to
+use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
+of the Software, and to permit persons to whom the Software is furnished to do
+so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+
+http://www.opensource.org/licenses/mit-license.html
+
+
+Library Files
+-----------------
+The following files should be compiled as part of your project:
+ftconfig.c
+ftrt.c
+dom.c
+expatls.c
+node.c
+stack.c
+xmlparse.c
+xmlrole.c
+xmltok.c
+
+In addition to the above files, the files "events.c", "xmltok_impl.c", and
+"xmltok_ns.c" are also included in this library, but are not meant to be
+directly compiled, as they are '#include'd into other library files.
+
+None of these files are intended to be modified.
+
+
+Sample Applications
+----------------------------
+Two sample applications are included in the Samples folder.
+
+ftconvert.c - demonstrates configuring a DAQ F/T system and performing
+force/torque calculations; converts a sample set of voltages into forces and
+torques.
+calinfo.c - demonstrates retrieving information from the calibration file such
+as the transducer's rated loads, calibration date, and other information.
+
+
+Programming Interface
+-------------------------------
+The functions supported for public use are:
+
+createCalibration - Loads calibration info for a transducer from a calibration
+file
+destroyCalibration - Frees memory allocated by a successful call to
+createCalibration
+SetToolTransform - Performs a 6-axis translation/rotation on the transducer's
+coordinate system
+SetForceUnits - Sets the units of force output
+SetTorqueUnits - Sets the units of torque outputEnables or disables
+temperature compensation, if available
+Bias - Stores a voltage reading to be subtracted from subsequent readings,
+effectively "zeroing" the transducer output to remove tooling weight, etc.
+ConvertToFT - Converts an array of voltages into forces and torques
+
+These functions are documented in ftconfig.h.
+
+You can also read information about the transducer system and calibration from the Calibration and other structures. These structures should be treated as read-only and should only be manipulated through the functions listed above. The sample application calinfo.c shows how to retrieve this information.
+
+The general structure of an application should follow the following structure:
+
+1. Call the createCalibration() method to load a calibration file into
+ memory.
+2. Call the SetForceUnits(), SetTorqueUnits(), SetCalibration(), and
+ SetToolTransform() functions to configure the system.
+3. Use device driver calls to retrieve an array of voltages from your data
+ acquisition system.
+4. Use the Bias() function to store a baseline reading (zero the transducer)
+5. Use the ConvertToFT() function to convert another array of voltages into
+ forces and torques. The Bias reading will be subtracted automatically.
+6. Call the destroyCalibration() function to free memory allocated for the
+ Calibration structure created in step 1.
+
+
+Optimizing Your Application
+--------------------------------------
+The bare minimum calculations for converting voltages into forces and torques
+are in ftrt.c. To isolate this section of the code, simply compile ftrc.c.
+This file #includes ftrc.h, but none of the other files. This can be useful,
+for example, in an embedded application where the configuration never changes.
+You can write a separate configuration program to run on a PC, which sends the
+contents of the Calibration.rt structure to a local copy of the RTCoefs
+structure in the embedded system.
+
+
+Version History
+----------------------
+1.0.6 Added support for kN-m torque units, changed reference to ActiveX
+ server to also reference .NET assembly.
+1.0.5 Moved some declarations out of .h files into .c files to avoid
+ multiple definition errors. Added ftsharedrt.h
+1.0.4 Added newlines to the ends of several files to avoid warnings when
+ compiling with gcc. FTconfig.c was using '/t' instead of '\t' for tab
+ character, which caused warnings under gcc, but no warnings or errors
+ under Visual C++.
+1.0.3 Added comments to ftconvert.c explaining hardware temperature
+ compensation.
+1.0.2 Fixed memory leaks
+1.0.0 No changes
+ Released 2-1-02
+0.7.1 Added necessary licensing info to all files
+ Beta released 10-5-01
+0.7.0 Initial beta release; no known bugs; compiles with no errors or warnings
+ in Microsoft Visual Studio.NET
+ Beta released 10-2-01
+
diff --git a/3rdparty/libatidaq-c/stack.c b/3rdparty/atidaq/stack.c
similarity index 100%
rename from 3rdparty/libatidaq-c/stack.c
rename to 3rdparty/atidaq/stack.c
diff --git a/3rdparty/libatidaq-c/stack.h b/3rdparty/atidaq/stack.h
similarity index 100%
rename from 3rdparty/libatidaq-c/stack.h
rename to 3rdparty/atidaq/stack.h
diff --git a/3rdparty/libatidaq-c/utf8tab.h b/3rdparty/atidaq/utf8tab.h
similarity index 100%
rename from 3rdparty/libatidaq-c/utf8tab.h
rename to 3rdparty/atidaq/utf8tab.h
diff --git a/3rdparty/libatidaq-c/xmldef.h b/3rdparty/atidaq/xmldef.h
similarity index 100%
rename from 3rdparty/libatidaq-c/xmldef.h
rename to 3rdparty/atidaq/xmldef.h
diff --git a/3rdparty/libatidaq-c/xmlparse.c b/3rdparty/atidaq/xmlparse.c
similarity index 100%
rename from 3rdparty/libatidaq-c/xmlparse.c
rename to 3rdparty/atidaq/xmlparse.c
diff --git a/3rdparty/libatidaq-c/xmlparse.h b/3rdparty/atidaq/xmlparse.h
similarity index 100%
rename from 3rdparty/libatidaq-c/xmlparse.h
rename to 3rdparty/atidaq/xmlparse.h
diff --git a/3rdparty/libatidaq-c/xmlrole.c b/3rdparty/atidaq/xmlrole.c
similarity index 100%
rename from 3rdparty/libatidaq-c/xmlrole.c
rename to 3rdparty/atidaq/xmlrole.c
diff --git a/3rdparty/libatidaq-c/xmlrole.h b/3rdparty/atidaq/xmlrole.h
similarity index 100%
rename from 3rdparty/libatidaq-c/xmlrole.h
rename to 3rdparty/atidaq/xmlrole.h
diff --git a/3rdparty/libatidaq-c/xmltok.c b/3rdparty/atidaq/xmltok.c
similarity index 100%
rename from 3rdparty/libatidaq-c/xmltok.c
rename to 3rdparty/atidaq/xmltok.c
diff --git a/3rdparty/libatidaq-c/xmltok.h b/3rdparty/atidaq/xmltok.h
similarity index 100%
rename from 3rdparty/libatidaq-c/xmltok.h
rename to 3rdparty/atidaq/xmltok.h
diff --git a/3rdparty/libatidaq-c/xmltok_impl.c b/3rdparty/atidaq/xmltok_impl.c
similarity index 100%
rename from 3rdparty/libatidaq-c/xmltok_impl.c
rename to 3rdparty/atidaq/xmltok_impl.c
diff --git a/3rdparty/libatidaq-c/xmltok_impl.h b/3rdparty/atidaq/xmltok_impl.h
similarity index 100%
rename from 3rdparty/libatidaq-c/xmltok_impl.h
rename to 3rdparty/atidaq/xmltok_impl.h
diff --git a/3rdparty/libatidaq-c/xmltok_ns.c b/3rdparty/atidaq/xmltok_ns.c
similarity index 100%
rename from 3rdparty/libatidaq-c/xmltok_ns.c
rename to 3rdparty/atidaq/xmltok_ns.c
diff --git a/3rdparty/clapack/CMakeLists.txt b/3rdparty/clapack/CMakeLists.txt
new file mode 100644
index 0000000..6ef95a2
--- /dev/null
+++ b/3rdparty/clapack/CMakeLists.txt
@@ -0,0 +1,49 @@
+project(${LAPACK_LIBRARY} C)
+
+if(UNIX)
+ if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
+ endif()
+endif()
+
+set(LAPACK_MAJOR_VERSION 3 PARENT_SCOPE)
+set(LAPACK_MINOR_VERSION 2 PARENT_SCOPE)
+set(LAPACK_PATCH_VERSION 1 PARENT_SCOPE)
+
+if(WIN32 AND MSVC)
+ add_definitions(-DMSDOS -DNO_ISATTY)
+endif()
+
+file(GLOB lib_srcs src/*.c blas/*.c install/*.c f2c/*.c)
+file(GLOB lib_incs include/*.h)
+
+add_library(${LAPACK_LIBRARY} STATIC ${lib_srcs} ${lib_incs})
+vp_target_include_directories(${LAPACK_LIBRARY} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${LAPACK_LIBRARY} PROPERTIES FOLDER "3rdparty")
+endif()
+
+set_target_properties(${LAPACK_LIBRARY}
+ PROPERTIES OUTPUT_NAME ${LAPACK_LIBRARY}
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ COMPILE_PDB_NAME ${LAPACK_LIBRARY}
+ COMPILE_PDB_NAME_DEBUG "${LAPACK_LIBRARY}${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
+ )
+
+if(NOT BUILD_SHARED_LIBS)
+ vp_install_target(${LAPACK_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
+endif()
+
+if(MSVC)
+ # Disable Visual C++ warnings
+ foreach(f ${lib_srcs})
+ vp_set_source_file_compile_flag(${f} /wd"4244" /wd"4267" /wd"4273" /wd"4554" /wd"4996")
+ endforeach()
+ vp_set_source_file_compile_flag(f2c/pow_ii.c /wd"4723")
+else()
+ foreach(f ${lib_srcs})
+ vp_set_source_file_compile_flag(${f} -Wno-logical-op-parentheses -Wno-shift-op-parentheses -Wno-parentheses -Wno-format-extra-args -Wno-format)
+ endforeach()
+endif()
diff --git a/3rdparty/clapack/COPYING b/3rdparty/clapack/COPYING
new file mode 100644
index 0000000..d7bf953
--- /dev/null
+++ b/3rdparty/clapack/COPYING
@@ -0,0 +1,36 @@
+Copyright (c) 1992-2008 The University of Tennessee. All rights reserved.
+
+$COPYRIGHT$
+
+Additional copyrights may follow
+
+$HEADER$
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+- Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+
+- Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer listed
+ in this license in the documentation and/or other materials
+ provided with the distribution.
+
+- Neither the name of the copyright holders nor the names of its
+ contributors may be used to endorse or promote products derived from
+ this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
+DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
+THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
diff --git a/3rdparty/clapack/blas/dcopy.c b/3rdparty/clapack/blas/dcopy.c
new file mode 100644
index 0000000..9033cce
--- /dev/null
+++ b/3rdparty/clapack/blas/dcopy.c
@@ -0,0 +1,107 @@
+/* dcopy.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dcopy_(integer *n, doublereal *dx, integer *incx,
+ doublereal *dy, integer *incy)
+{
+ /* System generated locals */
+ integer i__1;
+
+ /* Local variables */
+ integer i__, m, ix, iy, mp1;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* copies a vector, x, to a vector, y. */
+/* uses unrolled loops for increments equal to one. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --dy;
+ --dx;
+
+ /* Function Body */
+ if (*n <= 0) {
+ return 0;
+ }
+ if (*incx == 1 && *incy == 1) {
+ goto L20;
+ }
+
+/* code for unequal increments or equal increments */
+/* not equal to 1 */
+
+ ix = 1;
+ iy = 1;
+ if (*incx < 0) {
+ ix = (-(*n) + 1) * *incx + 1;
+ }
+ if (*incy < 0) {
+ iy = (-(*n) + 1) * *incy + 1;
+ }
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dy[iy] = dx[ix];
+ ix += *incx;
+ iy += *incy;
+/* L10: */
+ }
+ return 0;
+
+/* code for both increments equal to 1 */
+
+
+/* clean-up loop */
+
+L20:
+ m = *n % 7;
+ if (m == 0) {
+ goto L40;
+ }
+ i__1 = m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dy[i__] = dx[i__];
+/* L30: */
+ }
+ if (*n < 7) {
+ return 0;
+ }
+L40:
+ mp1 = m + 1;
+ i__1 = *n;
+ for (i__ = mp1; i__ <= i__1; i__ += 7) {
+ dy[i__] = dx[i__];
+ dy[i__ + 1] = dx[i__ + 1];
+ dy[i__ + 2] = dx[i__ + 2];
+ dy[i__ + 3] = dx[i__ + 3];
+ dy[i__ + 4] = dx[i__ + 4];
+ dy[i__ + 5] = dx[i__ + 5];
+ dy[i__ + 6] = dx[i__ + 6];
+/* L50: */
+ }
+ return 0;
+} /* dcopy_ */
diff --git a/3rdparty/clapack/blas/ddot.c b/3rdparty/clapack/blas/ddot.c
new file mode 100644
index 0000000..331fe0a
--- /dev/null
+++ b/3rdparty/clapack/blas/ddot.c
@@ -0,0 +1,110 @@
+/* ddot.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+doublereal ddot_(integer *n, doublereal *dx, integer *incx, doublereal *dy,
+ integer *incy)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal ret_val;
+
+ /* Local variables */
+ integer i__, m, ix, iy, mp1;
+ doublereal dtemp;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* forms the dot product of two vectors. */
+/* uses unrolled loops for increments equal to one. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --dy;
+ --dx;
+
+ /* Function Body */
+ ret_val = 0.;
+ dtemp = 0.;
+ if (*n <= 0) {
+ return ret_val;
+ }
+ if (*incx == 1 && *incy == 1) {
+ goto L20;
+ }
+
+/* code for unequal increments or equal increments */
+/* not equal to 1 */
+
+ ix = 1;
+ iy = 1;
+ if (*incx < 0) {
+ ix = (-(*n) + 1) * *incx + 1;
+ }
+ if (*incy < 0) {
+ iy = (-(*n) + 1) * *incy + 1;
+ }
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp += dx[ix] * dy[iy];
+ ix += *incx;
+ iy += *incy;
+/* L10: */
+ }
+ ret_val = dtemp;
+ return ret_val;
+
+/* code for both increments equal to 1 */
+
+
+/* clean-up loop */
+
+L20:
+ m = *n % 5;
+ if (m == 0) {
+ goto L40;
+ }
+ i__1 = m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp += dx[i__] * dy[i__];
+/* L30: */
+ }
+ if (*n < 5) {
+ goto L60;
+ }
+L40:
+ mp1 = m + 1;
+ i__1 = *n;
+ for (i__ = mp1; i__ <= i__1; i__ += 5) {
+ dtemp = dtemp + dx[i__] * dy[i__] + dx[i__ + 1] * dy[i__ + 1] + dx[
+ i__ + 2] * dy[i__ + 2] + dx[i__ + 3] * dy[i__ + 3] + dx[i__ +
+ 4] * dy[i__ + 4];
+/* L50: */
+ }
+L60:
+ ret_val = dtemp;
+ return ret_val;
+} /* ddot_ */
diff --git a/3rdparty/clapack/blas/dgemm.c b/3rdparty/clapack/blas/dgemm.c
new file mode 100644
index 0000000..b802cb0
--- /dev/null
+++ b/3rdparty/clapack/blas/dgemm.c
@@ -0,0 +1,389 @@
+/* dgemm.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dgemm_(char *transa, char *transb, integer *m, integer *
+ n, integer *k, doublereal *alpha, doublereal *a, integer *lda,
+ doublereal *b, integer *ldb, doublereal *beta, doublereal *c__,
+ integer *ldc)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, b_dim1, b_offset, c_dim1, c_offset, i__1, i__2,
+ i__3;
+
+ /* Local variables */
+ integer i__, j, l, info;
+ logical nota, notb;
+ doublereal temp;
+ integer ncola;
+ extern logical lsame_(char *, char *);
+ integer nrowa, nrowb;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEMM performs one of the matrix-matrix operations */
+
+/* C := alpha*op( A )*op( B ) + beta*C, */
+
+/* where op( X ) is one of */
+
+/* op( X ) = X or op( X ) = X', */
+
+/* alpha and beta are scalars, and A, B and C are matrices, with op( A ) */
+/* an m by k matrix, op( B ) a k by n matrix and C an m by n matrix. */
+
+/* Arguments */
+/* ========== */
+
+/* TRANSA - CHARACTER*1. */
+/* On entry, TRANSA specifies the form of op( A ) to be used in */
+/* the matrix multiplication as follows: */
+
+/* TRANSA = 'N' or 'n', op( A ) = A. */
+
+/* TRANSA = 'T' or 't', op( A ) = A'. */
+
+/* TRANSA = 'C' or 'c', op( A ) = A'. */
+
+/* Unchanged on exit. */
+
+/* TRANSB - CHARACTER*1. */
+/* On entry, TRANSB specifies the form of op( B ) to be used in */
+/* the matrix multiplication as follows: */
+
+/* TRANSB = 'N' or 'n', op( B ) = B. */
+
+/* TRANSB = 'T' or 't', op( B ) = B'. */
+
+/* TRANSB = 'C' or 'c', op( B ) = B'. */
+
+/* Unchanged on exit. */
+
+/* M - INTEGER. */
+/* On entry, M specifies the number of rows of the matrix */
+/* op( A ) and of the matrix C. M must be at least zero. */
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the number of columns of the matrix */
+/* op( B ) and the number of columns of the matrix C. N must be */
+/* at least zero. */
+/* Unchanged on exit. */
+
+/* K - INTEGER. */
+/* On entry, K specifies the number of columns of the matrix */
+/* op( A ) and the number of rows of the matrix op( B ). K must */
+/* be at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, ka ), where ka is */
+/* k when TRANSA = 'N' or 'n', and is m otherwise. */
+/* Before entry with TRANSA = 'N' or 'n', the leading m by k */
+/* part of the array A must contain the matrix A, otherwise */
+/* the leading k by m part of the array A must contain the */
+/* matrix A. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. When TRANSA = 'N' or 'n' then */
+/* LDA must be at least max( 1, m ), otherwise LDA must be at */
+/* least max( 1, k ). */
+/* Unchanged on exit. */
+
+/* B - DOUBLE PRECISION array of DIMENSION ( LDB, kb ), where kb is */
+/* n when TRANSB = 'N' or 'n', and is k otherwise. */
+/* Before entry with TRANSB = 'N' or 'n', the leading k by n */
+/* part of the array B must contain the matrix B, otherwise */
+/* the leading n by k part of the array B must contain the */
+/* matrix B. */
+/* Unchanged on exit. */
+
+/* LDB - INTEGER. */
+/* On entry, LDB specifies the first dimension of B as declared */
+/* in the calling (sub) program. When TRANSB = 'N' or 'n' then */
+/* LDB must be at least max( 1, k ), otherwise LDB must be at */
+/* least max( 1, n ). */
+/* Unchanged on exit. */
+
+/* BETA - DOUBLE PRECISION. */
+/* On entry, BETA specifies the scalar beta. When BETA is */
+/* supplied as zero then C need not be set on input. */
+/* Unchanged on exit. */
+
+/* C - DOUBLE PRECISION array of DIMENSION ( LDC, n ). */
+/* Before entry, the leading m by n part of the array C must */
+/* contain the matrix C, except when beta is zero, in which */
+/* case C need not be set on entry. */
+/* On exit, the array C is overwritten by the m by n matrix */
+/* ( alpha*op( A )*op( B ) + beta*C ). */
+
+/* LDC - INTEGER. */
+/* On entry, LDC specifies the first dimension of C as declared */
+/* in the calling (sub) program. LDC must be at least */
+/* max( 1, m ). */
+/* Unchanged on exit. */
+
+
+/* Level 3 Blas routine. */
+
+/* -- Written on 8-February-1989. */
+/* Jack Dongarra, Argonne National Laboratory. */
+/* Iain Duff, AERE Harwell. */
+/* Jeremy Du Croz, Numerical Algorithms Group Ltd. */
+/* Sven Hammarling, Numerical Algorithms Group Ltd. */
+
+
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Parameters .. */
+/* .. */
+
+/* Set NOTA and NOTB as true if A and B respectively are not */
+/* transposed and set NROWA, NCOLA and NROWB as the number of rows */
+/* and columns of A and the number of rows of B respectively. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ b_dim1 = *ldb;
+ b_offset = 1 + b_dim1;
+ b -= b_offset;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+
+ /* Function Body */
+ nota = lsame_(transa, "N");
+ notb = lsame_(transb, "N");
+ if (nota) {
+ nrowa = *m;
+ ncola = *k;
+ } else {
+ nrowa = *k;
+ ncola = *m;
+ }
+ if (notb) {
+ nrowb = *k;
+ } else {
+ nrowb = *n;
+ }
+
+/* Test the input parameters. */
+
+ info = 0;
+ if (! nota && ! lsame_(transa, "C") && ! lsame_(
+ transa, "T")) {
+ info = 1;
+ } else if (! notb && ! lsame_(transb, "C") && !
+ lsame_(transb, "T")) {
+ info = 2;
+ } else if (*m < 0) {
+ info = 3;
+ } else if (*n < 0) {
+ info = 4;
+ } else if (*k < 0) {
+ info = 5;
+ } else if (*lda < max(1,nrowa)) {
+ info = 8;
+ } else if (*ldb < max(1,nrowb)) {
+ info = 10;
+ } else if (*ldc < max(1,*m)) {
+ info = 13;
+ }
+ if (info != 0) {
+ xerbla_("DGEMM ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*m == 0 || *n == 0 || (*alpha == 0. || *k == 0) && *beta == 1.) {
+ return 0;
+ }
+
+/* And if alpha.eq.zero. */
+
+ if (*alpha == 0.) {
+ if (*beta == 0.) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L30: */
+ }
+/* L40: */
+ }
+ }
+ return 0;
+ }
+
+/* Start the operations. */
+
+ if (notb) {
+ if (nota) {
+
+/* Form C := alpha*A*B + beta*C. */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*beta == 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L50: */
+ }
+ } else if (*beta != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L60: */
+ }
+ }
+ i__2 = *k;
+ for (l = 1; l <= i__2; ++l) {
+ if (b[l + j * b_dim1] != 0.) {
+ temp = *alpha * b[l + j * b_dim1];
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ c__[i__ + j * c_dim1] += temp * a[i__ + l *
+ a_dim1];
+/* L70: */
+ }
+ }
+/* L80: */
+ }
+/* L90: */
+ }
+ } else {
+
+/* Form C := alpha*A'*B + beta*C */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = 0.;
+ i__3 = *k;
+ for (l = 1; l <= i__3; ++l) {
+ temp += a[l + i__ * a_dim1] * b[l + j * b_dim1];
+/* L100: */
+ }
+ if (*beta == 0.) {
+ c__[i__ + j * c_dim1] = *alpha * temp;
+ } else {
+ c__[i__ + j * c_dim1] = *alpha * temp + *beta * c__[
+ i__ + j * c_dim1];
+ }
+/* L110: */
+ }
+/* L120: */
+ }
+ }
+ } else {
+ if (nota) {
+
+/* Form C := alpha*A*B' + beta*C */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*beta == 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L130: */
+ }
+ } else if (*beta != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L140: */
+ }
+ }
+ i__2 = *k;
+ for (l = 1; l <= i__2; ++l) {
+ if (b[j + l * b_dim1] != 0.) {
+ temp = *alpha * b[j + l * b_dim1];
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ c__[i__ + j * c_dim1] += temp * a[i__ + l *
+ a_dim1];
+/* L150: */
+ }
+ }
+/* L160: */
+ }
+/* L170: */
+ }
+ } else {
+
+/* Form C := alpha*A'*B' + beta*C */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = 0.;
+ i__3 = *k;
+ for (l = 1; l <= i__3; ++l) {
+ temp += a[l + i__ * a_dim1] * b[j + l * b_dim1];
+/* L180: */
+ }
+ if (*beta == 0.) {
+ c__[i__ + j * c_dim1] = *alpha * temp;
+ } else {
+ c__[i__ + j * c_dim1] = *alpha * temp + *beta * c__[
+ i__ + j * c_dim1];
+ }
+/* L190: */
+ }
+/* L200: */
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DGEMM . */
+
+} /* dgemm_ */
diff --git a/3rdparty/clapack/blas/dgemv.c b/3rdparty/clapack/blas/dgemv.c
new file mode 100644
index 0000000..b82a5b3
--- /dev/null
+++ b/3rdparty/clapack/blas/dgemv.c
@@ -0,0 +1,312 @@
+/* dgemv.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dgemv_(char *trans, integer *m, integer *n, doublereal *
+ alpha, doublereal *a, integer *lda, doublereal *x, integer *incx,
+ doublereal *beta, doublereal *y, integer *incy)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j, ix, iy, jx, jy, kx, ky, info;
+ doublereal temp;
+ integer lenx, leny;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEMV performs one of the matrix-vector operations */
+
+/* y := alpha*A*x + beta*y, or y := alpha*A'*x + beta*y, */
+
+/* where alpha and beta are scalars, x and y are vectors and A is an */
+/* m by n matrix. */
+
+/* Arguments */
+/* ========== */
+
+/* TRANS - CHARACTER*1. */
+/* On entry, TRANS specifies the operation to be performed as */
+/* follows: */
+
+/* TRANS = 'N' or 'n' y := alpha*A*x + beta*y. */
+
+/* TRANS = 'T' or 't' y := alpha*A'*x + beta*y. */
+
+/* TRANS = 'C' or 'c' y := alpha*A'*x + beta*y. */
+
+/* Unchanged on exit. */
+
+/* M - INTEGER. */
+/* On entry, M specifies the number of rows of the matrix A. */
+/* M must be at least zero. */
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the number of columns of the matrix A. */
+/* N must be at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). */
+/* Before entry, the leading m by n part of the array A must */
+/* contain the matrix of coefficients. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. LDA must be at least */
+/* max( 1, m ). */
+/* Unchanged on exit. */
+
+/* X - DOUBLE PRECISION array of DIMENSION at least */
+/* ( 1 + ( n - 1 )*abs( INCX ) ) when TRANS = 'N' or 'n' */
+/* and at least */
+/* ( 1 + ( m - 1 )*abs( INCX ) ) otherwise. */
+/* Before entry, the incremented array X must contain the */
+/* vector x. */
+/* Unchanged on exit. */
+
+/* INCX - INTEGER. */
+/* On entry, INCX specifies the increment for the elements of */
+/* X. INCX must not be zero. */
+/* Unchanged on exit. */
+
+/* BETA - DOUBLE PRECISION. */
+/* On entry, BETA specifies the scalar beta. When BETA is */
+/* supplied as zero then Y need not be set on input. */
+/* Unchanged on exit. */
+
+/* Y - DOUBLE PRECISION array of DIMENSION at least */
+/* ( 1 + ( m - 1 )*abs( INCY ) ) when TRANS = 'N' or 'n' */
+/* and at least */
+/* ( 1 + ( n - 1 )*abs( INCY ) ) otherwise. */
+/* Before entry with BETA non-zero, the incremented array Y */
+/* must contain the vector y. On exit, Y is overwritten by the */
+/* updated vector y. */
+
+/* INCY - INTEGER. */
+/* On entry, INCY specifies the increment for the elements of */
+/* Y. INCY must not be zero. */
+/* Unchanged on exit. */
+
+
+/* Level 2 Blas routine. */
+
+/* -- Written on 22-October-1986. */
+/* Jack Dongarra, Argonne National Lab. */
+/* Jeremy Du Croz, Nag Central Office. */
+/* Sven Hammarling, Nag Central Office. */
+/* Richard Hanson, Sandia National Labs. */
+
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --x;
+ --y;
+
+ /* Function Body */
+ info = 0;
+ if (! lsame_(trans, "N") && ! lsame_(trans, "T") && ! lsame_(trans, "C")
+ ) {
+ info = 1;
+ } else if (*m < 0) {
+ info = 2;
+ } else if (*n < 0) {
+ info = 3;
+ } else if (*lda < max(1,*m)) {
+ info = 6;
+ } else if (*incx == 0) {
+ info = 8;
+ } else if (*incy == 0) {
+ info = 11;
+ }
+ if (info != 0) {
+ xerbla_("DGEMV ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*m == 0 || *n == 0 || *alpha == 0. && *beta == 1.) {
+ return 0;
+ }
+
+/* Set LENX and LENY, the lengths of the vectors x and y, and set */
+/* up the start points in X and Y. */
+
+ if (lsame_(trans, "N")) {
+ lenx = *n;
+ leny = *m;
+ } else {
+ lenx = *m;
+ leny = *n;
+ }
+ if (*incx > 0) {
+ kx = 1;
+ } else {
+ kx = 1 - (lenx - 1) * *incx;
+ }
+ if (*incy > 0) {
+ ky = 1;
+ } else {
+ ky = 1 - (leny - 1) * *incy;
+ }
+
+/* Start the operations. In this version the elements of A are */
+/* accessed sequentially with one pass through A. */
+
+/* First form y := beta*y. */
+
+ if (*beta != 1.) {
+ if (*incy == 1) {
+ if (*beta == 0.) {
+ i__1 = leny;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ y[i__] = 0.;
+/* L10: */
+ }
+ } else {
+ i__1 = leny;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ y[i__] = *beta * y[i__];
+/* L20: */
+ }
+ }
+ } else {
+ iy = ky;
+ if (*beta == 0.) {
+ i__1 = leny;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ y[iy] = 0.;
+ iy += *incy;
+/* L30: */
+ }
+ } else {
+ i__1 = leny;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ y[iy] = *beta * y[iy];
+ iy += *incy;
+/* L40: */
+ }
+ }
+ }
+ }
+ if (*alpha == 0.) {
+ return 0;
+ }
+ if (lsame_(trans, "N")) {
+
+/* Form y := alpha*A*x + y. */
+
+ jx = kx;
+ if (*incy == 1) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (x[jx] != 0.) {
+ temp = *alpha * x[jx];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ y[i__] += temp * a[i__ + j * a_dim1];
+/* L50: */
+ }
+ }
+ jx += *incx;
+/* L60: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (x[jx] != 0.) {
+ temp = *alpha * x[jx];
+ iy = ky;
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ y[iy] += temp * a[i__ + j * a_dim1];
+ iy += *incy;
+/* L70: */
+ }
+ }
+ jx += *incx;
+/* L80: */
+ }
+ }
+ } else {
+
+/* Form y := alpha*A'*x + y. */
+
+ jy = ky;
+ if (*incx == 1) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ temp = 0.;
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp += a[i__ + j * a_dim1] * x[i__];
+/* L90: */
+ }
+ y[jy] += *alpha * temp;
+ jy += *incy;
+/* L100: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ temp = 0.;
+ ix = kx;
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp += a[i__ + j * a_dim1] * x[ix];
+ ix += *incx;
+/* L110: */
+ }
+ y[jy] += *alpha * temp;
+ jy += *incy;
+/* L120: */
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DGEMV . */
+
+} /* dgemv_ */
diff --git a/3rdparty/clapack/blas/dger.c b/3rdparty/clapack/blas/dger.c
new file mode 100644
index 0000000..085833b
--- /dev/null
+++ b/3rdparty/clapack/blas/dger.c
@@ -0,0 +1,194 @@
+/* dger.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dger_(integer *m, integer *n, doublereal *alpha,
+ doublereal *x, integer *incx, doublereal *y, integer *incy,
+ doublereal *a, integer *lda)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j, ix, jy, kx, info;
+ doublereal temp;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGER performs the rank 1 operation */
+
+/* A := alpha*x*y' + A, */
+
+/* where alpha is a scalar, x is an m element vector, y is an n element */
+/* vector and A is an m by n matrix. */
+
+/* Arguments */
+/* ========== */
+
+/* M - INTEGER. */
+/* On entry, M specifies the number of rows of the matrix A. */
+/* M must be at least zero. */
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the number of columns of the matrix A. */
+/* N must be at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. */
+/* Unchanged on exit. */
+
+/* X - DOUBLE PRECISION array of dimension at least */
+/* ( 1 + ( m - 1 )*abs( INCX ) ). */
+/* Before entry, the incremented array X must contain the m */
+/* element vector x. */
+/* Unchanged on exit. */
+
+/* INCX - INTEGER. */
+/* On entry, INCX specifies the increment for the elements of */
+/* X. INCX must not be zero. */
+/* Unchanged on exit. */
+
+/* Y - DOUBLE PRECISION array of dimension at least */
+/* ( 1 + ( n - 1 )*abs( INCY ) ). */
+/* Before entry, the incremented array Y must contain the n */
+/* element vector y. */
+/* Unchanged on exit. */
+
+/* INCY - INTEGER. */
+/* On entry, INCY specifies the increment for the elements of */
+/* Y. INCY must not be zero. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). */
+/* Before entry, the leading m by n part of the array A must */
+/* contain the matrix of coefficients. On exit, A is */
+/* overwritten by the updated matrix. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. LDA must be at least */
+/* max( 1, m ). */
+/* Unchanged on exit. */
+
+
+/* Level 2 Blas routine. */
+
+/* -- Written on 22-October-1986. */
+/* Jack Dongarra, Argonne National Lab. */
+/* Jeremy Du Croz, Nag Central Office. */
+/* Sven Hammarling, Nag Central Office. */
+/* Richard Hanson, Sandia National Labs. */
+
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --x;
+ --y;
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ info = 0;
+ if (*m < 0) {
+ info = 1;
+ } else if (*n < 0) {
+ info = 2;
+ } else if (*incx == 0) {
+ info = 5;
+ } else if (*incy == 0) {
+ info = 7;
+ } else if (*lda < max(1,*m)) {
+ info = 9;
+ }
+ if (info != 0) {
+ xerbla_("DGER ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*m == 0 || *n == 0 || *alpha == 0.) {
+ return 0;
+ }
+
+/* Start the operations. In this version the elements of A are */
+/* accessed sequentially with one pass through A. */
+
+ if (*incy > 0) {
+ jy = 1;
+ } else {
+ jy = 1 - (*n - 1) * *incy;
+ }
+ if (*incx == 1) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (y[jy] != 0.) {
+ temp = *alpha * y[jy];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] += x[i__] * temp;
+/* L10: */
+ }
+ }
+ jy += *incy;
+/* L20: */
+ }
+ } else {
+ if (*incx > 0) {
+ kx = 1;
+ } else {
+ kx = 1 - (*m - 1) * *incx;
+ }
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (y[jy] != 0.) {
+ temp = *alpha * y[jy];
+ ix = kx;
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] += x[ix] * temp;
+ ix += *incx;
+/* L30: */
+ }
+ }
+ jy += *incy;
+/* L40: */
+ }
+ }
+
+ return 0;
+
+/* End of DGER . */
+
+} /* dger_ */
diff --git a/3rdparty/clapack/blas/dnrm2.c b/3rdparty/clapack/blas/dnrm2.c
new file mode 100644
index 0000000..8c50ff5
--- /dev/null
+++ b/3rdparty/clapack/blas/dnrm2.c
@@ -0,0 +1,95 @@
+/* dnrm2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+doublereal dnrm2_(integer *n, doublereal *x, integer *incx)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+ doublereal ret_val, d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer ix;
+ doublereal ssq, norm, scale, absxi;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DNRM2 returns the euclidean norm of a vector via the function */
+/* name, so that */
+
+/* DNRM2 := sqrt( x'*x ) */
+
+
+/* -- This version written on 25-October-1982. */
+/* Modified on 14-October-1993 to inline the call to DLASSQ. */
+/* Sven Hammarling, Nag Ltd. */
+
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ if (*n < 1 || *incx < 1) {
+ norm = 0.;
+ } else if (*n == 1) {
+ norm = abs(x[1]);
+ } else {
+ scale = 0.;
+ ssq = 1.;
+/* The following loop is equivalent to this call to the LAPACK */
+/* auxiliary routine: */
+/* CALL DLASSQ( N, X, INCX, SCALE, SSQ ) */
+
+ i__1 = (*n - 1) * *incx + 1;
+ i__2 = *incx;
+ for (ix = 1; i__2 < 0 ? ix >= i__1 : ix <= i__1; ix += i__2) {
+ if (x[ix] != 0.) {
+ absxi = (d__1 = x[ix], abs(d__1));
+ if (scale < absxi) {
+/* Computing 2nd power */
+ d__1 = scale / absxi;
+ ssq = ssq * (d__1 * d__1) + 1.;
+ scale = absxi;
+ } else {
+/* Computing 2nd power */
+ d__1 = absxi / scale;
+ ssq += d__1 * d__1;
+ }
+ }
+/* L10: */
+ }
+ norm = scale * sqrt(ssq);
+ }
+
+ ret_val = norm;
+ return ret_val;
+
+/* End of DNRM2. */
+
+} /* dnrm2_ */
diff --git a/3rdparty/clapack/blas/drot.c b/3rdparty/clapack/blas/drot.c
new file mode 100644
index 0000000..d4bc6bd
--- /dev/null
+++ b/3rdparty/clapack/blas/drot.c
@@ -0,0 +1,86 @@
+/* drot.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int drot_(integer *n, doublereal *dx, integer *incx,
+ doublereal *dy, integer *incy, doublereal *c__, doublereal *s)
+{
+ /* System generated locals */
+ integer i__1;
+
+ /* Local variables */
+ integer i__, ix, iy;
+ doublereal dtemp;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* applies a plane rotation. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+ /* Parameter adjustments */
+ --dy;
+ --dx;
+
+ /* Function Body */
+ if (*n <= 0) {
+ return 0;
+ }
+ if (*incx == 1 && *incy == 1) {
+ goto L20;
+ }
+
+/* code for unequal increments or equal increments not equal */
+/* to 1 */
+
+ ix = 1;
+ iy = 1;
+ if (*incx < 0) {
+ ix = (-(*n) + 1) * *incx + 1;
+ }
+ if (*incy < 0) {
+ iy = (-(*n) + 1) * *incy + 1;
+ }
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp = *c__ * dx[ix] + *s * dy[iy];
+ dy[iy] = *c__ * dy[iy] - *s * dx[ix];
+ dx[ix] = dtemp;
+ ix += *incx;
+ iy += *incy;
+/* L10: */
+ }
+ return 0;
+
+/* code for both increments equal to 1 */
+
+L20:
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp = *c__ * dx[i__] + *s * dy[i__];
+ dy[i__] = *c__ * dy[i__] - *s * dx[i__];
+ dx[i__] = dtemp;
+/* L30: */
+ }
+ return 0;
+} /* drot_ */
diff --git a/3rdparty/clapack/blas/dscal.c b/3rdparty/clapack/blas/dscal.c
new file mode 100644
index 0000000..11548be
--- /dev/null
+++ b/3rdparty/clapack/blas/dscal.c
@@ -0,0 +1,96 @@
+/* dscal.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dscal_(integer *n, doublereal *da, doublereal *dx,
+ integer *incx)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+
+ /* Local variables */
+ integer i__, m, mp1, nincx;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+/* * */
+/* scales a vector by a constant. */
+/* uses unrolled loops for increment equal to one. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 3/93 to return if incx .le. 0. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --dx;
+
+ /* Function Body */
+ if (*n <= 0 || *incx <= 0) {
+ return 0;
+ }
+ if (*incx == 1) {
+ goto L20;
+ }
+
+/* code for increment not equal to 1 */
+
+ nincx = *n * *incx;
+ i__1 = nincx;
+ i__2 = *incx;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+ dx[i__] = *da * dx[i__];
+/* L10: */
+ }
+ return 0;
+
+/* code for increment equal to 1 */
+
+
+/* clean-up loop */
+
+L20:
+ m = *n % 5;
+ if (m == 0) {
+ goto L40;
+ }
+ i__2 = m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ dx[i__] = *da * dx[i__];
+/* L30: */
+ }
+ if (*n < 5) {
+ return 0;
+ }
+L40:
+ mp1 = m + 1;
+ i__2 = *n;
+ for (i__ = mp1; i__ <= i__2; i__ += 5) {
+ dx[i__] = *da * dx[i__];
+ dx[i__ + 1] = *da * dx[i__ + 1];
+ dx[i__ + 2] = *da * dx[i__ + 2];
+ dx[i__ + 3] = *da * dx[i__ + 3];
+ dx[i__ + 4] = *da * dx[i__ + 4];
+/* L50: */
+ }
+ return 0;
+} /* dscal_ */
diff --git a/3rdparty/clapack/blas/dswap.c b/3rdparty/clapack/blas/dswap.c
new file mode 100644
index 0000000..27a303e
--- /dev/null
+++ b/3rdparty/clapack/blas/dswap.c
@@ -0,0 +1,114 @@
+/* dswap.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dswap_(integer *n, doublereal *dx, integer *incx,
+ doublereal *dy, integer *incy)
+{
+ /* System generated locals */
+ integer i__1;
+
+ /* Local variables */
+ integer i__, m, ix, iy, mp1;
+ doublereal dtemp;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* interchanges two vectors. */
+/* uses unrolled loops for increments equal one. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --dy;
+ --dx;
+
+ /* Function Body */
+ if (*n <= 0) {
+ return 0;
+ }
+ if (*incx == 1 && *incy == 1) {
+ goto L20;
+ }
+
+/* code for unequal increments or equal increments not equal */
+/* to 1 */
+
+ ix = 1;
+ iy = 1;
+ if (*incx < 0) {
+ ix = (-(*n) + 1) * *incx + 1;
+ }
+ if (*incy < 0) {
+ iy = (-(*n) + 1) * *incy + 1;
+ }
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp = dx[ix];
+ dx[ix] = dy[iy];
+ dy[iy] = dtemp;
+ ix += *incx;
+ iy += *incy;
+/* L10: */
+ }
+ return 0;
+
+/* code for both increments equal to 1 */
+
+
+/* clean-up loop */
+
+L20:
+ m = *n % 3;
+ if (m == 0) {
+ goto L40;
+ }
+ i__1 = m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dtemp = dx[i__];
+ dx[i__] = dy[i__];
+ dy[i__] = dtemp;
+/* L30: */
+ }
+ if (*n < 3) {
+ return 0;
+ }
+L40:
+ mp1 = m + 1;
+ i__1 = *n;
+ for (i__ = mp1; i__ <= i__1; i__ += 3) {
+ dtemp = dx[i__];
+ dx[i__] = dy[i__];
+ dy[i__] = dtemp;
+ dtemp = dx[i__ + 1];
+ dx[i__ + 1] = dy[i__ + 1];
+ dy[i__ + 1] = dtemp;
+ dtemp = dx[i__ + 2];
+ dx[i__ + 2] = dy[i__ + 2];
+ dy[i__ + 2] = dtemp;
+/* L50: */
+ }
+ return 0;
+} /* dswap_ */
diff --git a/3rdparty/clapack/blas/dsyrk.c b/3rdparty/clapack/blas/dsyrk.c
new file mode 100644
index 0000000..393880d
--- /dev/null
+++ b/3rdparty/clapack/blas/dsyrk.c
@@ -0,0 +1,372 @@
+/* dsyrk.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dsyrk_(char *uplo, char *trans, integer *n, integer *k,
+ doublereal *alpha, doublereal *a, integer *lda, doublereal *beta,
+ doublereal *c__, integer *ldc)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, l, info;
+ doublereal temp;
+ extern logical lsame_(char *, char *);
+ integer nrowa;
+ logical upper;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DSYRK performs one of the symmetric rank k operations */
+
+/* C := alpha*A*A' + beta*C, */
+
+/* or */
+
+/* C := alpha*A'*A + beta*C, */
+
+/* where alpha and beta are scalars, C is an n by n symmetric matrix */
+/* and A is an n by k matrix in the first case and a k by n matrix */
+/* in the second case. */
+
+/* Arguments */
+/* ========== */
+
+/* UPLO - CHARACTER*1. */
+/* On entry, UPLO specifies whether the upper or lower */
+/* triangular part of the array C is to be referenced as */
+/* follows: */
+
+/* UPLO = 'U' or 'u' Only the upper triangular part of C */
+/* is to be referenced. */
+
+/* UPLO = 'L' or 'l' Only the lower triangular part of C */
+/* is to be referenced. */
+
+/* Unchanged on exit. */
+
+/* TRANS - CHARACTER*1. */
+/* On entry, TRANS specifies the operation to be performed as */
+/* follows: */
+
+/* TRANS = 'N' or 'n' C := alpha*A*A' + beta*C. */
+
+/* TRANS = 'T' or 't' C := alpha*A'*A + beta*C. */
+
+/* TRANS = 'C' or 'c' C := alpha*A'*A + beta*C. */
+
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the order of the matrix C. N must be */
+/* at least zero. */
+/* Unchanged on exit. */
+
+/* K - INTEGER. */
+/* On entry with TRANS = 'N' or 'n', K specifies the number */
+/* of columns of the matrix A, and on entry with */
+/* TRANS = 'T' or 't' or 'C' or 'c', K specifies the number */
+/* of rows of the matrix A. K must be at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, ka ), where ka is */
+/* k when TRANS = 'N' or 'n', and is n otherwise. */
+/* Before entry with TRANS = 'N' or 'n', the leading n by k */
+/* part of the array A must contain the matrix A, otherwise */
+/* the leading k by n part of the array A must contain the */
+/* matrix A. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. When TRANS = 'N' or 'n' */
+/* then LDA must be at least max( 1, n ), otherwise LDA must */
+/* be at least max( 1, k ). */
+/* Unchanged on exit. */
+
+/* BETA - DOUBLE PRECISION. */
+/* On entry, BETA specifies the scalar beta. */
+/* Unchanged on exit. */
+
+/* C - DOUBLE PRECISION array of DIMENSION ( LDC, n ). */
+/* Before entry with UPLO = 'U' or 'u', the leading n by n */
+/* upper triangular part of the array C must contain the upper */
+/* triangular part of the symmetric matrix and the strictly */
+/* lower triangular part of C is not referenced. On exit, the */
+/* upper triangular part of the array C is overwritten by the */
+/* upper triangular part of the updated matrix. */
+/* Before entry with UPLO = 'L' or 'l', the leading n by n */
+/* lower triangular part of the array C must contain the lower */
+/* triangular part of the symmetric matrix and the strictly */
+/* upper triangular part of C is not referenced. On exit, the */
+/* lower triangular part of the array C is overwritten by the */
+/* lower triangular part of the updated matrix. */
+
+/* LDC - INTEGER. */
+/* On entry, LDC specifies the first dimension of C as declared */
+/* in the calling (sub) program. LDC must be at least */
+/* max( 1, n ). */
+/* Unchanged on exit. */
+
+
+/* Level 3 Blas routine. */
+
+/* -- Written on 8-February-1989. */
+/* Jack Dongarra, Argonne National Laboratory. */
+/* Iain Duff, AERE Harwell. */
+/* Jeremy Du Croz, Numerical Algorithms Group Ltd. */
+/* Sven Hammarling, Numerical Algorithms Group Ltd. */
+
+
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Parameters .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+
+ /* Function Body */
+ if (lsame_(trans, "N")) {
+ nrowa = *n;
+ } else {
+ nrowa = *k;
+ }
+ upper = lsame_(uplo, "U");
+
+ info = 0;
+ if (! upper && ! lsame_(uplo, "L")) {
+ info = 1;
+ } else if (! lsame_(trans, "N") && ! lsame_(trans,
+ "T") && ! lsame_(trans, "C")) {
+ info = 2;
+ } else if (*n < 0) {
+ info = 3;
+ } else if (*k < 0) {
+ info = 4;
+ } else if (*lda < max(1,nrowa)) {
+ info = 7;
+ } else if (*ldc < max(1,*n)) {
+ info = 10;
+ }
+ if (info != 0) {
+ xerbla_("DSYRK ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*n == 0 || (*alpha == 0. || *k == 0) && *beta == 1.) {
+ return 0;
+ }
+
+/* And when alpha.eq.zero. */
+
+ if (*alpha == 0.) {
+ if (upper) {
+ if (*beta == 0.) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L30: */
+ }
+/* L40: */
+ }
+ }
+ } else {
+ if (*beta == 0.) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *n;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L50: */
+ }
+/* L60: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *n;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L70: */
+ }
+/* L80: */
+ }
+ }
+ }
+ return 0;
+ }
+
+/* Start the operations. */
+
+ if (lsame_(trans, "N")) {
+
+/* Form C := alpha*A*A' + beta*C. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*beta == 0.) {
+ i__2 = j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L90: */
+ }
+ } else if (*beta != 1.) {
+ i__2 = j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L100: */
+ }
+ }
+ i__2 = *k;
+ for (l = 1; l <= i__2; ++l) {
+ if (a[j + l * a_dim1] != 0.) {
+ temp = *alpha * a[j + l * a_dim1];
+ i__3 = j;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ c__[i__ + j * c_dim1] += temp * a[i__ + l *
+ a_dim1];
+/* L110: */
+ }
+ }
+/* L120: */
+ }
+/* L130: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*beta == 0.) {
+ i__2 = *n;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = 0.;
+/* L140: */
+ }
+ } else if (*beta != 1.) {
+ i__2 = *n;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] = *beta * c__[i__ + j * c_dim1];
+/* L150: */
+ }
+ }
+ i__2 = *k;
+ for (l = 1; l <= i__2; ++l) {
+ if (a[j + l * a_dim1] != 0.) {
+ temp = *alpha * a[j + l * a_dim1];
+ i__3 = *n;
+ for (i__ = j; i__ <= i__3; ++i__) {
+ c__[i__ + j * c_dim1] += temp * a[i__ + l *
+ a_dim1];
+/* L160: */
+ }
+ }
+/* L170: */
+ }
+/* L180: */
+ }
+ }
+ } else {
+
+/* Form C := alpha*A'*A + beta*C. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = 0.;
+ i__3 = *k;
+ for (l = 1; l <= i__3; ++l) {
+ temp += a[l + i__ * a_dim1] * a[l + j * a_dim1];
+/* L190: */
+ }
+ if (*beta == 0.) {
+ c__[i__ + j * c_dim1] = *alpha * temp;
+ } else {
+ c__[i__ + j * c_dim1] = *alpha * temp + *beta * c__[
+ i__ + j * c_dim1];
+ }
+/* L200: */
+ }
+/* L210: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *n;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ temp = 0.;
+ i__3 = *k;
+ for (l = 1; l <= i__3; ++l) {
+ temp += a[l + i__ * a_dim1] * a[l + j * a_dim1];
+/* L220: */
+ }
+ if (*beta == 0.) {
+ c__[i__ + j * c_dim1] = *alpha * temp;
+ } else {
+ c__[i__ + j * c_dim1] = *alpha * temp + *beta * c__[
+ i__ + j * c_dim1];
+ }
+/* L230: */
+ }
+/* L240: */
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DSYRK . */
+
+} /* dsyrk_ */
diff --git a/3rdparty/clapack/blas/dtrmm.c b/3rdparty/clapack/blas/dtrmm.c
new file mode 100644
index 0000000..4bf834f
--- /dev/null
+++ b/3rdparty/clapack/blas/dtrmm.c
@@ -0,0 +1,453 @@
+/* dtrmm.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dtrmm_(char *side, char *uplo, char *transa, char *diag,
+ integer *m, integer *n, doublereal *alpha, doublereal *a, integer *
+ lda, doublereal *b, integer *ldb)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, k, info;
+ doublereal temp;
+ logical lside;
+ extern logical lsame_(char *, char *);
+ integer nrowa;
+ logical upper;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ logical nounit;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DTRMM performs one of the matrix-matrix operations */
+
+/* B := alpha*op( A )*B, or B := alpha*B*op( A ), */
+
+/* where alpha is a scalar, B is an m by n matrix, A is a unit, or */
+/* non-unit, upper or lower triangular matrix and op( A ) is one of */
+
+/* op( A ) = A or op( A ) = A'. */
+
+/* Arguments */
+/* ========== */
+
+/* SIDE - CHARACTER*1. */
+/* On entry, SIDE specifies whether op( A ) multiplies B from */
+/* the left or right as follows: */
+
+/* SIDE = 'L' or 'l' B := alpha*op( A )*B. */
+
+/* SIDE = 'R' or 'r' B := alpha*B*op( A ). */
+
+/* Unchanged on exit. */
+
+/* UPLO - CHARACTER*1. */
+/* On entry, UPLO specifies whether the matrix A is an upper or */
+/* lower triangular matrix as follows: */
+
+/* UPLO = 'U' or 'u' A is an upper triangular matrix. */
+
+/* UPLO = 'L' or 'l' A is a lower triangular matrix. */
+
+/* Unchanged on exit. */
+
+/* TRANSA - CHARACTER*1. */
+/* On entry, TRANSA specifies the form of op( A ) to be used in */
+/* the matrix multiplication as follows: */
+
+/* TRANSA = 'N' or 'n' op( A ) = A. */
+
+/* TRANSA = 'T' or 't' op( A ) = A'. */
+
+/* TRANSA = 'C' or 'c' op( A ) = A'. */
+
+/* Unchanged on exit. */
+
+/* DIAG - CHARACTER*1. */
+/* On entry, DIAG specifies whether or not A is unit triangular */
+/* as follows: */
+
+/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */
+
+/* DIAG = 'N' or 'n' A is not assumed to be unit */
+/* triangular. */
+
+/* Unchanged on exit. */
+
+/* M - INTEGER. */
+/* On entry, M specifies the number of rows of B. M must be at */
+/* least zero. */
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the number of columns of B. N must be */
+/* at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. When alpha is */
+/* zero then A is not referenced and B need not be set before */
+/* entry. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, k ), where k is m */
+/* when SIDE = 'L' or 'l' and is n when SIDE = 'R' or 'r'. */
+/* Before entry with UPLO = 'U' or 'u', the leading k by k */
+/* upper triangular part of the array A must contain the upper */
+/* triangular matrix and the strictly lower triangular part of */
+/* A is not referenced. */
+/* Before entry with UPLO = 'L' or 'l', the leading k by k */
+/* lower triangular part of the array A must contain the lower */
+/* triangular matrix and the strictly upper triangular part of */
+/* A is not referenced. */
+/* Note that when DIAG = 'U' or 'u', the diagonal elements of */
+/* A are not referenced either, but are assumed to be unity. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. When SIDE = 'L' or 'l' then */
+/* LDA must be at least max( 1, m ), when SIDE = 'R' or 'r' */
+/* then LDA must be at least max( 1, n ). */
+/* Unchanged on exit. */
+
+/* B - DOUBLE PRECISION array of DIMENSION ( LDB, n ). */
+/* Before entry, the leading m by n part of the array B must */
+/* contain the matrix B, and on exit is overwritten by the */
+/* transformed matrix. */
+
+/* LDB - INTEGER. */
+/* On entry, LDB specifies the first dimension of B as declared */
+/* in the calling (sub) program. LDB must be at least */
+/* max( 1, m ). */
+/* Unchanged on exit. */
+
+
+/* Level 3 Blas routine. */
+
+/* -- Written on 8-February-1989. */
+/* Jack Dongarra, Argonne National Laboratory. */
+/* Iain Duff, AERE Harwell. */
+/* Jeremy Du Croz, Numerical Algorithms Group Ltd. */
+/* Sven Hammarling, Numerical Algorithms Group Ltd. */
+
+
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Parameters .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ b_dim1 = *ldb;
+ b_offset = 1 + b_dim1;
+ b -= b_offset;
+
+ /* Function Body */
+ lside = lsame_(side, "L");
+ if (lside) {
+ nrowa = *m;
+ } else {
+ nrowa = *n;
+ }
+ nounit = lsame_(diag, "N");
+ upper = lsame_(uplo, "U");
+
+ info = 0;
+ if (! lside && ! lsame_(side, "R")) {
+ info = 1;
+ } else if (! upper && ! lsame_(uplo, "L")) {
+ info = 2;
+ } else if (! lsame_(transa, "N") && ! lsame_(transa,
+ "T") && ! lsame_(transa, "C")) {
+ info = 3;
+ } else if (! lsame_(diag, "U") && ! lsame_(diag,
+ "N")) {
+ info = 4;
+ } else if (*m < 0) {
+ info = 5;
+ } else if (*n < 0) {
+ info = 6;
+ } else if (*lda < max(1,nrowa)) {
+ info = 9;
+ } else if (*ldb < max(1,*m)) {
+ info = 11;
+ }
+ if (info != 0) {
+ xerbla_("DTRMM ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+/* And when alpha.eq.zero. */
+
+ if (*alpha == 0.) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ return 0;
+ }
+
+/* Start the operations. */
+
+ if (lside) {
+ if (lsame_(transa, "N")) {
+
+/* Form B := alpha*A*B. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (k = 1; k <= i__2; ++k) {
+ if (b[k + j * b_dim1] != 0.) {
+ temp = *alpha * b[k + j * b_dim1];
+ i__3 = k - 1;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] += temp * a[i__ + k *
+ a_dim1];
+/* L30: */
+ }
+ if (nounit) {
+ temp *= a[k + k * a_dim1];
+ }
+ b[k + j * b_dim1] = temp;
+ }
+/* L40: */
+ }
+/* L50: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ for (k = *m; k >= 1; --k) {
+ if (b[k + j * b_dim1] != 0.) {
+ temp = *alpha * b[k + j * b_dim1];
+ b[k + j * b_dim1] = temp;
+ if (nounit) {
+ b[k + j * b_dim1] *= a[k + k * a_dim1];
+ }
+ i__2 = *m;
+ for (i__ = k + 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] += temp * a[i__ + k *
+ a_dim1];
+/* L60: */
+ }
+ }
+/* L70: */
+ }
+/* L80: */
+ }
+ }
+ } else {
+
+/* Form B := alpha*A'*B. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ for (i__ = *m; i__ >= 1; --i__) {
+ temp = b[i__ + j * b_dim1];
+ if (nounit) {
+ temp *= a[i__ + i__ * a_dim1];
+ }
+ i__2 = i__ - 1;
+ for (k = 1; k <= i__2; ++k) {
+ temp += a[k + i__ * a_dim1] * b[k + j * b_dim1];
+/* L90: */
+ }
+ b[i__ + j * b_dim1] = *alpha * temp;
+/* L100: */
+ }
+/* L110: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = b[i__ + j * b_dim1];
+ if (nounit) {
+ temp *= a[i__ + i__ * a_dim1];
+ }
+ i__3 = *m;
+ for (k = i__ + 1; k <= i__3; ++k) {
+ temp += a[k + i__ * a_dim1] * b[k + j * b_dim1];
+/* L120: */
+ }
+ b[i__ + j * b_dim1] = *alpha * temp;
+/* L130: */
+ }
+/* L140: */
+ }
+ }
+ }
+ } else {
+ if (lsame_(transa, "N")) {
+
+/* Form B := alpha*B*A. */
+
+ if (upper) {
+ for (j = *n; j >= 1; --j) {
+ temp = *alpha;
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + j * b_dim1] = temp * b[i__ + j * b_dim1];
+/* L150: */
+ }
+ i__1 = j - 1;
+ for (k = 1; k <= i__1; ++k) {
+ if (a[k + j * a_dim1] != 0.) {
+ temp = *alpha * a[k + j * a_dim1];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] += temp * b[i__ + k *
+ b_dim1];
+/* L160: */
+ }
+ }
+/* L170: */
+ }
+/* L180: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ temp = *alpha;
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = temp * b[i__ + j * b_dim1];
+/* L190: */
+ }
+ i__2 = *n;
+ for (k = j + 1; k <= i__2; ++k) {
+ if (a[k + j * a_dim1] != 0.) {
+ temp = *alpha * a[k + j * a_dim1];
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] += temp * b[i__ + k *
+ b_dim1];
+/* L200: */
+ }
+ }
+/* L210: */
+ }
+/* L220: */
+ }
+ }
+ } else {
+
+/* Form B := alpha*B*A'. */
+
+ if (upper) {
+ i__1 = *n;
+ for (k = 1; k <= i__1; ++k) {
+ i__2 = k - 1;
+ for (j = 1; j <= i__2; ++j) {
+ if (a[j + k * a_dim1] != 0.) {
+ temp = *alpha * a[j + k * a_dim1];
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] += temp * b[i__ + k *
+ b_dim1];
+/* L230: */
+ }
+ }
+/* L240: */
+ }
+ temp = *alpha;
+ if (nounit) {
+ temp *= a[k + k * a_dim1];
+ }
+ if (temp != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + k * b_dim1] = temp * b[i__ + k * b_dim1];
+/* L250: */
+ }
+ }
+/* L260: */
+ }
+ } else {
+ for (k = *n; k >= 1; --k) {
+ i__1 = *n;
+ for (j = k + 1; j <= i__1; ++j) {
+ if (a[j + k * a_dim1] != 0.) {
+ temp = *alpha * a[j + k * a_dim1];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] += temp * b[i__ + k *
+ b_dim1];
+/* L270: */
+ }
+ }
+/* L280: */
+ }
+ temp = *alpha;
+ if (nounit) {
+ temp *= a[k + k * a_dim1];
+ }
+ if (temp != 1.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + k * b_dim1] = temp * b[i__ + k * b_dim1];
+/* L290: */
+ }
+ }
+/* L300: */
+ }
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DTRMM . */
+
+} /* dtrmm_ */
diff --git a/3rdparty/clapack/blas/dtrmv.c b/3rdparty/clapack/blas/dtrmv.c
new file mode 100644
index 0000000..3acaa6d
--- /dev/null
+++ b/3rdparty/clapack/blas/dtrmv.c
@@ -0,0 +1,345 @@
+/* dtrmv.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dtrmv_(char *uplo, char *trans, char *diag, integer *n,
+ doublereal *a, integer *lda, doublereal *x, integer *incx)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j, ix, jx, kx, info;
+ doublereal temp;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ logical nounit;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DTRMV performs one of the matrix-vector operations */
+
+/* x := A*x, or x := A'*x, */
+
+/* where x is an n element vector and A is an n by n unit, or non-unit, */
+/* upper or lower triangular matrix. */
+
+/* Arguments */
+/* ========== */
+
+/* UPLO - CHARACTER*1. */
+/* On entry, UPLO specifies whether the matrix is an upper or */
+/* lower triangular matrix as follows: */
+
+/* UPLO = 'U' or 'u' A is an upper triangular matrix. */
+
+/* UPLO = 'L' or 'l' A is a lower triangular matrix. */
+
+/* Unchanged on exit. */
+
+/* TRANS - CHARACTER*1. */
+/* On entry, TRANS specifies the operation to be performed as */
+/* follows: */
+
+/* TRANS = 'N' or 'n' x := A*x. */
+
+/* TRANS = 'T' or 't' x := A'*x. */
+
+/* TRANS = 'C' or 'c' x := A'*x. */
+
+/* Unchanged on exit. */
+
+/* DIAG - CHARACTER*1. */
+/* On entry, DIAG specifies whether or not A is unit */
+/* triangular as follows: */
+
+/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */
+
+/* DIAG = 'N' or 'n' A is not assumed to be unit */
+/* triangular. */
+
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the order of the matrix A. */
+/* N must be at least zero. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, n ). */
+/* Before entry with UPLO = 'U' or 'u', the leading n by n */
+/* upper triangular part of the array A must contain the upper */
+/* triangular matrix and the strictly lower triangular part of */
+/* A is not referenced. */
+/* Before entry with UPLO = 'L' or 'l', the leading n by n */
+/* lower triangular part of the array A must contain the lower */
+/* triangular matrix and the strictly upper triangular part of */
+/* A is not referenced. */
+/* Note that when DIAG = 'U' or 'u', the diagonal elements of */
+/* A are not referenced either, but are assumed to be unity. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. LDA must be at least */
+/* max( 1, n ). */
+/* Unchanged on exit. */
+
+/* X - DOUBLE PRECISION array of dimension at least */
+/* ( 1 + ( n - 1 )*abs( INCX ) ). */
+/* Before entry, the incremented array X must contain the n */
+/* element vector x. On exit, X is overwritten with the */
+/* tranformed vector x. */
+
+/* INCX - INTEGER. */
+/* On entry, INCX specifies the increment for the elements of */
+/* X. INCX must not be zero. */
+/* Unchanged on exit. */
+
+
+/* Level 2 Blas routine. */
+
+/* -- Written on 22-October-1986. */
+/* Jack Dongarra, Argonne National Lab. */
+/* Jeremy Du Croz, Nag Central Office. */
+/* Sven Hammarling, Nag Central Office. */
+/* Richard Hanson, Sandia National Labs. */
+
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --x;
+
+ /* Function Body */
+ info = 0;
+ if (! lsame_(uplo, "U") && ! lsame_(uplo, "L")) {
+ info = 1;
+ } else if (! lsame_(trans, "N") && ! lsame_(trans,
+ "T") && ! lsame_(trans, "C")) {
+ info = 2;
+ } else if (! lsame_(diag, "U") && ! lsame_(diag,
+ "N")) {
+ info = 3;
+ } else if (*n < 0) {
+ info = 4;
+ } else if (*lda < max(1,*n)) {
+ info = 6;
+ } else if (*incx == 0) {
+ info = 8;
+ }
+ if (info != 0) {
+ xerbla_("DTRMV ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+ nounit = lsame_(diag, "N");
+
+/* Set up the start point in X if the increment is not unity. This */
+/* will be ( N - 1 )*INCX too small for descending loops. */
+
+ if (*incx <= 0) {
+ kx = 1 - (*n - 1) * *incx;
+ } else if (*incx != 1) {
+ kx = 1;
+ }
+
+/* Start the operations. In this version the elements of A are */
+/* accessed sequentially with one pass through A. */
+
+ if (lsame_(trans, "N")) {
+
+/* Form x := A*x. */
+
+ if (lsame_(uplo, "U")) {
+ if (*incx == 1) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (x[j] != 0.) {
+ temp = x[j];
+ i__2 = j - 1;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ x[i__] += temp * a[i__ + j * a_dim1];
+/* L10: */
+ }
+ if (nounit) {
+ x[j] *= a[j + j * a_dim1];
+ }
+ }
+/* L20: */
+ }
+ } else {
+ jx = kx;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (x[jx] != 0.) {
+ temp = x[jx];
+ ix = kx;
+ i__2 = j - 1;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ x[ix] += temp * a[i__ + j * a_dim1];
+ ix += *incx;
+/* L30: */
+ }
+ if (nounit) {
+ x[jx] *= a[j + j * a_dim1];
+ }
+ }
+ jx += *incx;
+/* L40: */
+ }
+ }
+ } else {
+ if (*incx == 1) {
+ for (j = *n; j >= 1; --j) {
+ if (x[j] != 0.) {
+ temp = x[j];
+ i__1 = j + 1;
+ for (i__ = *n; i__ >= i__1; --i__) {
+ x[i__] += temp * a[i__ + j * a_dim1];
+/* L50: */
+ }
+ if (nounit) {
+ x[j] *= a[j + j * a_dim1];
+ }
+ }
+/* L60: */
+ }
+ } else {
+ kx += (*n - 1) * *incx;
+ jx = kx;
+ for (j = *n; j >= 1; --j) {
+ if (x[jx] != 0.) {
+ temp = x[jx];
+ ix = kx;
+ i__1 = j + 1;
+ for (i__ = *n; i__ >= i__1; --i__) {
+ x[ix] += temp * a[i__ + j * a_dim1];
+ ix -= *incx;
+/* L70: */
+ }
+ if (nounit) {
+ x[jx] *= a[j + j * a_dim1];
+ }
+ }
+ jx -= *incx;
+/* L80: */
+ }
+ }
+ }
+ } else {
+
+/* Form x := A'*x. */
+
+ if (lsame_(uplo, "U")) {
+ if (*incx == 1) {
+ for (j = *n; j >= 1; --j) {
+ temp = x[j];
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ for (i__ = j - 1; i__ >= 1; --i__) {
+ temp += a[i__ + j * a_dim1] * x[i__];
+/* L90: */
+ }
+ x[j] = temp;
+/* L100: */
+ }
+ } else {
+ jx = kx + (*n - 1) * *incx;
+ for (j = *n; j >= 1; --j) {
+ temp = x[jx];
+ ix = jx;
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ for (i__ = j - 1; i__ >= 1; --i__) {
+ ix -= *incx;
+ temp += a[i__ + j * a_dim1] * x[ix];
+/* L110: */
+ }
+ x[jx] = temp;
+ jx -= *incx;
+/* L120: */
+ }
+ }
+ } else {
+ if (*incx == 1) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ temp = x[j];
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ i__2 = *n;
+ for (i__ = j + 1; i__ <= i__2; ++i__) {
+ temp += a[i__ + j * a_dim1] * x[i__];
+/* L130: */
+ }
+ x[j] = temp;
+/* L140: */
+ }
+ } else {
+ jx = kx;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ temp = x[jx];
+ ix = jx;
+ if (nounit) {
+ temp *= a[j + j * a_dim1];
+ }
+ i__2 = *n;
+ for (i__ = j + 1; i__ <= i__2; ++i__) {
+ ix += *incx;
+ temp += a[i__ + j * a_dim1] * x[ix];
+/* L150: */
+ }
+ x[jx] = temp;
+ jx += *incx;
+/* L160: */
+ }
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DTRMV . */
+
+} /* dtrmv_ */
diff --git a/3rdparty/clapack/blas/dtrsm.c b/3rdparty/clapack/blas/dtrsm.c
new file mode 100644
index 0000000..84ee3e7
--- /dev/null
+++ b/3rdparty/clapack/blas/dtrsm.c
@@ -0,0 +1,490 @@
+/* dtrsm.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dtrsm_(char *side, char *uplo, char *transa, char *diag,
+ integer *m, integer *n, doublereal *alpha, doublereal *a, integer *
+ lda, doublereal *b, integer *ldb)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, b_dim1, b_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, k, info;
+ doublereal temp;
+ logical lside;
+ extern logical lsame_(char *, char *);
+ integer nrowa;
+ logical upper;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ logical nounit;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DTRSM solves one of the matrix equations */
+
+/* op( A )*X = alpha*B, or X*op( A ) = alpha*B, */
+
+/* where alpha is a scalar, X and B are m by n matrices, A is a unit, or */
+/* non-unit, upper or lower triangular matrix and op( A ) is one of */
+
+/* op( A ) = A or op( A ) = A'. */
+
+/* The matrix X is overwritten on B. */
+
+/* Arguments */
+/* ========== */
+
+/* SIDE - CHARACTER*1. */
+/* On entry, SIDE specifies whether op( A ) appears on the left */
+/* or right of X as follows: */
+
+/* SIDE = 'L' or 'l' op( A )*X = alpha*B. */
+
+/* SIDE = 'R' or 'r' X*op( A ) = alpha*B. */
+
+/* Unchanged on exit. */
+
+/* UPLO - CHARACTER*1. */
+/* On entry, UPLO specifies whether the matrix A is an upper or */
+/* lower triangular matrix as follows: */
+
+/* UPLO = 'U' or 'u' A is an upper triangular matrix. */
+
+/* UPLO = 'L' or 'l' A is a lower triangular matrix. */
+
+/* Unchanged on exit. */
+
+/* TRANSA - CHARACTER*1. */
+/* On entry, TRANSA specifies the form of op( A ) to be used in */
+/* the matrix multiplication as follows: */
+
+/* TRANSA = 'N' or 'n' op( A ) = A. */
+
+/* TRANSA = 'T' or 't' op( A ) = A'. */
+
+/* TRANSA = 'C' or 'c' op( A ) = A'. */
+
+/* Unchanged on exit. */
+
+/* DIAG - CHARACTER*1. */
+/* On entry, DIAG specifies whether or not A is unit triangular */
+/* as follows: */
+
+/* DIAG = 'U' or 'u' A is assumed to be unit triangular. */
+
+/* DIAG = 'N' or 'n' A is not assumed to be unit */
+/* triangular. */
+
+/* Unchanged on exit. */
+
+/* M - INTEGER. */
+/* On entry, M specifies the number of rows of B. M must be at */
+/* least zero. */
+/* Unchanged on exit. */
+
+/* N - INTEGER. */
+/* On entry, N specifies the number of columns of B. N must be */
+/* at least zero. */
+/* Unchanged on exit. */
+
+/* ALPHA - DOUBLE PRECISION. */
+/* On entry, ALPHA specifies the scalar alpha. When alpha is */
+/* zero then A is not referenced and B need not be set before */
+/* entry. */
+/* Unchanged on exit. */
+
+/* A - DOUBLE PRECISION array of DIMENSION ( LDA, k ), where k is m */
+/* when SIDE = 'L' or 'l' and is n when SIDE = 'R' or 'r'. */
+/* Before entry with UPLO = 'U' or 'u', the leading k by k */
+/* upper triangular part of the array A must contain the upper */
+/* triangular matrix and the strictly lower triangular part of */
+/* A is not referenced. */
+/* Before entry with UPLO = 'L' or 'l', the leading k by k */
+/* lower triangular part of the array A must contain the lower */
+/* triangular matrix and the strictly upper triangular part of */
+/* A is not referenced. */
+/* Note that when DIAG = 'U' or 'u', the diagonal elements of */
+/* A are not referenced either, but are assumed to be unity. */
+/* Unchanged on exit. */
+
+/* LDA - INTEGER. */
+/* On entry, LDA specifies the first dimension of A as declared */
+/* in the calling (sub) program. When SIDE = 'L' or 'l' then */
+/* LDA must be at least max( 1, m ), when SIDE = 'R' or 'r' */
+/* then LDA must be at least max( 1, n ). */
+/* Unchanged on exit. */
+
+/* B - DOUBLE PRECISION array of DIMENSION ( LDB, n ). */
+/* Before entry, the leading m by n part of the array B must */
+/* contain the right-hand side matrix B, and on exit is */
+/* overwritten by the solution matrix X. */
+
+/* LDB - INTEGER. */
+/* On entry, LDB specifies the first dimension of B as declared */
+/* in the calling (sub) program. LDB must be at least */
+/* max( 1, m ). */
+/* Unchanged on exit. */
+
+
+/* Level 3 Blas routine. */
+
+
+/* -- Written on 8-February-1989. */
+/* Jack Dongarra, Argonne National Laboratory. */
+/* Iain Duff, AERE Harwell. */
+/* Jeremy Du Croz, Numerical Algorithms Group Ltd. */
+/* Sven Hammarling, Numerical Algorithms Group Ltd. */
+
+
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Parameters .. */
+/* .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ b_dim1 = *ldb;
+ b_offset = 1 + b_dim1;
+ b -= b_offset;
+
+ /* Function Body */
+ lside = lsame_(side, "L");
+ if (lside) {
+ nrowa = *m;
+ } else {
+ nrowa = *n;
+ }
+ nounit = lsame_(diag, "N");
+ upper = lsame_(uplo, "U");
+
+ info = 0;
+ if (! lside && ! lsame_(side, "R")) {
+ info = 1;
+ } else if (! upper && ! lsame_(uplo, "L")) {
+ info = 2;
+ } else if (! lsame_(transa, "N") && ! lsame_(transa,
+ "T") && ! lsame_(transa, "C")) {
+ info = 3;
+ } else if (! lsame_(diag, "U") && ! lsame_(diag,
+ "N")) {
+ info = 4;
+ } else if (*m < 0) {
+ info = 5;
+ } else if (*n < 0) {
+ info = 6;
+ } else if (*lda < max(1,nrowa)) {
+ info = 9;
+ } else if (*ldb < max(1,*m)) {
+ info = 11;
+ }
+ if (info != 0) {
+ xerbla_("DTRSM ", &info);
+ return 0;
+ }
+
+/* Quick return if possible. */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+/* And when alpha.eq.zero. */
+
+ if (*alpha == 0.) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ return 0;
+ }
+
+/* Start the operations. */
+
+ if (lside) {
+ if (lsame_(transa, "N")) {
+
+/* Form B := alpha*inv( A )*B. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*alpha != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = *alpha * b[i__ + j * b_dim1]
+ ;
+/* L30: */
+ }
+ }
+ for (k = *m; k >= 1; --k) {
+ if (b[k + j * b_dim1] != 0.) {
+ if (nounit) {
+ b[k + j * b_dim1] /= a[k + k * a_dim1];
+ }
+ i__2 = k - 1;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] -= b[k + j * b_dim1] * a[
+ i__ + k * a_dim1];
+/* L40: */
+ }
+ }
+/* L50: */
+ }
+/* L60: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*alpha != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = *alpha * b[i__ + j * b_dim1]
+ ;
+/* L70: */
+ }
+ }
+ i__2 = *m;
+ for (k = 1; k <= i__2; ++k) {
+ if (b[k + j * b_dim1] != 0.) {
+ if (nounit) {
+ b[k + j * b_dim1] /= a[k + k * a_dim1];
+ }
+ i__3 = *m;
+ for (i__ = k + 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] -= b[k + j * b_dim1] * a[
+ i__ + k * a_dim1];
+/* L80: */
+ }
+ }
+/* L90: */
+ }
+/* L100: */
+ }
+ }
+ } else {
+
+/* Form B := alpha*inv( A' )*B. */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = *alpha * b[i__ + j * b_dim1];
+ i__3 = i__ - 1;
+ for (k = 1; k <= i__3; ++k) {
+ temp -= a[k + i__ * a_dim1] * b[k + j * b_dim1];
+/* L110: */
+ }
+ if (nounit) {
+ temp /= a[i__ + i__ * a_dim1];
+ }
+ b[i__ + j * b_dim1] = temp;
+/* L120: */
+ }
+/* L130: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ for (i__ = *m; i__ >= 1; --i__) {
+ temp = *alpha * b[i__ + j * b_dim1];
+ i__2 = *m;
+ for (k = i__ + 1; k <= i__2; ++k) {
+ temp -= a[k + i__ * a_dim1] * b[k + j * b_dim1];
+/* L140: */
+ }
+ if (nounit) {
+ temp /= a[i__ + i__ * a_dim1];
+ }
+ b[i__ + j * b_dim1] = temp;
+/* L150: */
+ }
+/* L160: */
+ }
+ }
+ }
+ } else {
+ if (lsame_(transa, "N")) {
+
+/* Form B := alpha*B*inv( A ). */
+
+ if (upper) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (*alpha != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = *alpha * b[i__ + j * b_dim1]
+ ;
+/* L170: */
+ }
+ }
+ i__2 = j - 1;
+ for (k = 1; k <= i__2; ++k) {
+ if (a[k + j * a_dim1] != 0.) {
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] -= a[k + j * a_dim1] * b[
+ i__ + k * b_dim1];
+/* L180: */
+ }
+ }
+/* L190: */
+ }
+ if (nounit) {
+ temp = 1. / a[j + j * a_dim1];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = temp * b[i__ + j * b_dim1];
+/* L200: */
+ }
+ }
+/* L210: */
+ }
+ } else {
+ for (j = *n; j >= 1; --j) {
+ if (*alpha != 1.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + j * b_dim1] = *alpha * b[i__ + j * b_dim1]
+ ;
+/* L220: */
+ }
+ }
+ i__1 = *n;
+ for (k = j + 1; k <= i__1; ++k) {
+ if (a[k + j * a_dim1] != 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] -= a[k + j * a_dim1] * b[
+ i__ + k * b_dim1];
+/* L230: */
+ }
+ }
+/* L240: */
+ }
+ if (nounit) {
+ temp = 1. / a[j + j * a_dim1];
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + j * b_dim1] = temp * b[i__ + j * b_dim1];
+/* L250: */
+ }
+ }
+/* L260: */
+ }
+ }
+ } else {
+
+/* Form B := alpha*B*inv( A' ). */
+
+ if (upper) {
+ for (k = *n; k >= 1; --k) {
+ if (nounit) {
+ temp = 1. / a[k + k * a_dim1];
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + k * b_dim1] = temp * b[i__ + k * b_dim1];
+/* L270: */
+ }
+ }
+ i__1 = k - 1;
+ for (j = 1; j <= i__1; ++j) {
+ if (a[j + k * a_dim1] != 0.) {
+ temp = a[j + k * a_dim1];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] -= temp * b[i__ + k *
+ b_dim1];
+/* L280: */
+ }
+ }
+/* L290: */
+ }
+ if (*alpha != 1.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ b[i__ + k * b_dim1] = *alpha * b[i__ + k * b_dim1]
+ ;
+/* L300: */
+ }
+ }
+/* L310: */
+ }
+ } else {
+ i__1 = *n;
+ for (k = 1; k <= i__1; ++k) {
+ if (nounit) {
+ temp = 1. / a[k + k * a_dim1];
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + k * b_dim1] = temp * b[i__ + k * b_dim1];
+/* L320: */
+ }
+ }
+ i__2 = *n;
+ for (j = k + 1; j <= i__2; ++j) {
+ if (a[j + k * a_dim1] != 0.) {
+ temp = a[j + k * a_dim1];
+ i__3 = *m;
+ for (i__ = 1; i__ <= i__3; ++i__) {
+ b[i__ + j * b_dim1] -= temp * b[i__ + k *
+ b_dim1];
+/* L330: */
+ }
+ }
+/* L340: */
+ }
+ if (*alpha != 1.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + k * b_dim1] = *alpha * b[i__ + k * b_dim1]
+ ;
+/* L350: */
+ }
+ }
+/* L360: */
+ }
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DTRSM . */
+
+} /* dtrsm_ */
diff --git a/3rdparty/clapack/blas/idamax.c b/3rdparty/clapack/blas/idamax.c
new file mode 100644
index 0000000..9b9636a
--- /dev/null
+++ b/3rdparty/clapack/blas/idamax.c
@@ -0,0 +1,93 @@
+/* idamax.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+integer idamax_(integer *n, doublereal *dx, integer *incx)
+{
+ /* System generated locals */
+ integer ret_val, i__1;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__, ix;
+ doublereal dmax__;
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* finds the index of element having max. absolute value. */
+/* jack dongarra, linpack, 3/11/78. */
+/* modified 3/93 to return if incx .le. 0. */
+/* modified 12/3/93, array(1) declarations changed to array(*) */
+
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+ /* Parameter adjustments */
+ --dx;
+
+ /* Function Body */
+ ret_val = 0;
+ if (*n < 1 || *incx <= 0) {
+ return ret_val;
+ }
+ ret_val = 1;
+ if (*n == 1) {
+ return ret_val;
+ }
+ if (*incx == 1) {
+ goto L20;
+ }
+
+/* code for increment not equal to 1 */
+
+ ix = 1;
+ dmax__ = abs(dx[1]);
+ ix += *incx;
+ i__1 = *n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ if ((d__1 = dx[ix], abs(d__1)) <= dmax__) {
+ goto L5;
+ }
+ ret_val = i__;
+ dmax__ = (d__1 = dx[ix], abs(d__1));
+L5:
+ ix += *incx;
+/* L10: */
+ }
+ return ret_val;
+
+/* code for increment equal to 1 */
+
+L20:
+ dmax__ = abs(dx[1]);
+ i__1 = *n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ if ((d__1 = dx[i__], abs(d__1)) <= dmax__) {
+ goto L30;
+ }
+ ret_val = i__;
+ dmax__ = (d__1 = dx[i__], abs(d__1));
+L30:
+ ;
+ }
+ return ret_val;
+} /* idamax_ */
diff --git a/3rdparty/clapack/blas/lsame.c b/3rdparty/clapack/blas/lsame.c
new file mode 100644
index 0000000..0648674
--- /dev/null
+++ b/3rdparty/clapack/blas/lsame.c
@@ -0,0 +1,117 @@
+/* lsame.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+logical lsame_(char *ca, char *cb)
+{
+ /* System generated locals */
+ logical ret_val;
+
+ /* Local variables */
+ integer inta, intb, zcode;
+
+
+/* -- LAPACK auxiliary routine (version 3.1) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* LSAME returns .TRUE. if CA is the same letter as CB regardless of */
+/* case. */
+
+/* Arguments */
+/* ========= */
+
+/* CA (input) CHARACTER*1 */
+
+/* CB (input) CHARACTER*1 */
+/* CA and CB specify the single characters to be compared. */
+
+/* ===================================================================== */
+
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+
+/* Test if the characters are equal */
+
+ ret_val = *(unsigned char *)ca == *(unsigned char *)cb;
+ if (ret_val) {
+ return ret_val;
+ }
+
+/* Now test for equivalence if both characters are alphabetic. */
+
+ zcode = 'Z';
+
+/* Use 'Z' rather than 'A' so that ASCII can be detected on Prime */
+/* machines, on which ICHAR returns a value with bit 8 set. */
+/* ICHAR('A') on Prime machines returns 193 which is the same as */
+/* ICHAR('A') on an EBCDIC machine. */
+
+ inta = *(unsigned char *)ca;
+ intb = *(unsigned char *)cb;
+
+ if (zcode == 90 || zcode == 122) {
+
+/* ASCII is assumed - ZCODE is the ASCII code of either lower or */
+/* upper case 'Z'. */
+
+ if (inta >= 97 && inta <= 122) {
+ inta += -32;
+ }
+ if (intb >= 97 && intb <= 122) {
+ intb += -32;
+ }
+
+ } else if (zcode == 233 || zcode == 169) {
+
+/* EBCDIC is assumed - ZCODE is the EBCDIC code of either lower or */
+/* upper case 'Z'. */
+
+ if (inta >= 129 && inta <= 137 || inta >= 145 && inta <= 153 || inta
+ >= 162 && inta <= 169) {
+ inta += 64;
+ }
+ if (intb >= 129 && intb <= 137 || intb >= 145 && intb <= 153 || intb
+ >= 162 && intb <= 169) {
+ intb += 64;
+ }
+
+ } else if (zcode == 218 || zcode == 250) {
+
+/* ASCII is assumed, on Prime machines - ZCODE is the ASCII code */
+/* plus 128 of either lower or upper case 'Z'. */
+
+ if (inta >= 225 && inta <= 250) {
+ inta += -32;
+ }
+ if (intb >= 225 && intb <= 250) {
+ intb += -32;
+ }
+ }
+ ret_val = inta == intb;
+
+/* RETURN */
+
+/* End of LSAME */
+
+ return ret_val;
+} /* lsame_ */
diff --git a/3rdparty/clapack/f2c/Notice b/3rdparty/clapack/f2c/Notice
new file mode 100644
index 0000000..261b719
--- /dev/null
+++ b/3rdparty/clapack/f2c/Notice
@@ -0,0 +1,23 @@
+/****************************************************************
+Copyright 1990 - 1997 by AT&T, Lucent Technologies and Bellcore.
+
+Permission to use, copy, modify, and distribute this software
+and its documentation for any purpose and without fee is hereby
+granted, provided that the above copyright notice appear in all
+copies and that both that the copyright notice and this
+permission notice and warranty disclaimer appear in supporting
+documentation, and that the names of AT&T, Bell Laboratories,
+Lucent or Bellcore or any of their entities not be used in
+advertising or publicity pertaining to distribution of the
+software without specific, written prior permission.
+
+AT&T, Lucent and Bellcore disclaim all warranties with regard to
+this software, including all implied warranties of
+merchantability and fitness. In no event shall AT&T, Lucent or
+Bellcore be liable for any special, indirect or consequential
+damages or any damages whatsoever resulting from loss of use,
+data or profits, whether in an action of contract, negligence or
+other tortious action, arising out of or in connection with the
+use or performance of this software.
+****************************************************************/
+
diff --git a/3rdparty/clapack/f2c/README b/3rdparty/clapack/f2c/README
new file mode 100644
index 0000000..c163b87
--- /dev/null
+++ b/3rdparty/clapack/f2c/README
@@ -0,0 +1,374 @@
+As shipped, "makefile" is a copy of "makefile.u", a Unix makefile.
+Variants for other systems have names of the form makefile.* and
+have initial comments saying how to invoke them. You may wish to
+copy one of the other makefile.* files to makefile.
+
+If you use a C++ compiler, first say
+
+ make hadd
+
+to create a suitable f2c.h from f2c.h0 and f2ch.add. Otherwise,
+
+ make f2c.h
+
+will just copy f2c.h0 to f2c.h .
+
+If your compiler does not recognize ANSI C headers,
+compile with KR_headers defined: either add -DKR_headers
+to the definition of CFLAGS in the makefile, or insert
+
+#define KR_headers
+
+at the top of f2c.h .
+
+If your system lacks onexit() and you are not using an ANSI C
+compiler, then you should compile main.c with NO_ONEXIT defined.
+See the comments about onexit in makefile.u.
+
+If your system has a double drem() function such that drem(a,b)
+is the IEEE remainder function (with double a, b), then you may
+wish to compile r_mod.c and d_mod.c with IEEE_drem defined.
+
+To check for transmission errors, issue the command
+ make check
+or
+ make -f makefile.u check
+
+This assumes you have the xsum program whose source, xsum.c,
+is distributed as part of "all from f2c/src", and that it
+is installed somewhere in your search path. If you do not
+have xsum, you can obtain xsum.c by sending the following E-mail
+message to netlib at netlib.org
+ send xsum.c from f2c/src
+
+For convenience, the f2c.h0 in this directory is a copy of netlib's
+"f2c.h from f2c". It is best to install f2c.h in a standard place,
+so "include f2c.h" will work in any directory without further ado.
+Beware that the makefiles do not cause recompilation when f2c.h is
+changed.
+
+On machines, such as those using a DEC Alpha processor, on which
+sizeof(short) == 2, sizeof(int) == sizeof(float) == 4, and
+sizeof(long) == sizeof(double) == 8, it suffices to modify f2c.h by
+removing the first occurrence of "long " on each line containing
+"long ". On Unix systems, you can do this by issuing the commands
+ mv f2c.h f2c.h0
+ sed 's/long int /int /' f2c.h0 >f2c.h
+On such machines, one can enable INTEGER*8 by uncommenting the typedefs
+of longint and ulongint in f2c.h and adjusting them, so they read
+ typedef long longint;
+ typedef unsigned long ulongint;
+and by compiling libf2c with -DAllow_TYQUAD, as discussed below.
+
+
+Most of the routines in libf2c are support routines for Fortran
+intrinsic functions or for operations that f2c chooses not
+to do "in line". There are a few exceptions, summarized below --
+functions and subroutines that appear to your program as ordinary
+external Fortran routines.
+
+If you use the REAL valued functions listed below (ERF, ERFC,
+DTIME, and ETIME) with "f2c -R", then you need to compile the
+corresponding source files with -DREAL=float. To do this, it is
+perhaps simplest to add "-DREAL=float" to CFLAGS in the makefile.
+
+1. CALL ABORT prints a message and causes a core dump.
+
+2. ERF(r) and DERF(d) and the REAL and DOUBLE PRECISION
+ error functions (with x REAL and d DOUBLE PRECISION);
+ DERF must be declared DOUBLE PRECISION in your program.
+ Both ERF and DERF assume your C library provides the
+ underlying erf() function (which not all systems do).
+
+3. ERFC(r) and DERFC(d) are the complementary error functions:
+ ERFC(r) = 1 - ERF(r) and DERFC(d) = 1.d0 - DERFC(d)
+ (except that their results may be more accurate than
+ explicitly evaluating the above formulae would give).
+ Again, ERFC and r are REAL, and DERFC and d are DOUBLE
+ PRECISION (and must be declared as such in your program),
+ and ERFC and DERFC rely on your system's erfc().
+
+4. CALL GETARG(n,s), where n is an INTEGER and s is a CHARACTER
+ variable, sets s to the n-th command-line argument (or to
+ all blanks if there are fewer than n command-line arguments);
+ CALL GETARG(0,s) sets s to the name of the program (on systems
+ that support this feature). See IARGC below.
+
+5. CALL GETENV(name, value), where name and value are of type
+ CHARACTER, sets value to the environment value, $name, of
+ name (or to blanks if $name has not been set).
+
+6. NARGS = IARGC() sets NARGS to the number of command-line
+ arguments (an INTEGER value).
+
+7. CALL SIGNAL(n,func), where n is an INTEGER and func is an
+ EXTERNAL procedure, arranges for func to be invoked when n
+ occurs (on systems where this makes sense).
+
+If your compiler complains about the signal calls in main.c, s_paus.c,
+and signal_.c, you may need to adjust signal1.h suitably. See the
+comments in signal1.h.
+
+8. ETIME(ARR) and DTIME(ARR) are REAL functions that return
+ execution times. ARR is declared REAL ARR(2). The elapsed
+ user and system CPU times are stored in ARR(1) and ARR(2),
+ respectively. ETIME returns the total elapsed CPU time,
+ i.e., ARR(1) + ARR(2). DTIME returns total elapsed CPU
+ time since the previous call on DTIME.
+
+9. CALL SYSTEM(cmd), where cmd is of type CHARACTER, passes
+ cmd to the system's command processor (on systems where
+ this can be done).
+
+10. CALL FLUSH flushes all buffers.
+
+11. FTELL(i) is an INTEGER function that returns the current
+ offset of Fortran unit i (or -1 if unit i is not open).
+
+12. CALL FSEEK(i, offset, whence, *errlab) attemps to move
+ Fortran unit i to the specified offset: absolute offset
+ if whence = 0; relative to the current offset if whence = 1;
+ relative to the end of the file if whence = 2. It branches
+ to label errlab if unit i is not open or if the call
+ otherwise fails.
+
+The routines whose objects are makefile.u's $(I77) are for I/O.
+The following comments apply to them.
+
+If your system lacks /usr/include/local.h ,
+then you should create an appropriate local.h in
+this directory. An appropriate local.h may simply
+be empty, or it may #define VAX or #define CRAY
+(or whatever else you must do to make fp.h work right).
+Alternatively, edit fp.h to suite your machine.
+
+If your system lacks /usr/include/fcntl.h , then you
+should simply create an empty fcntl.h in this directory.
+If your compiler then complains about creat and open not
+having a prototype, compile with OPEN_DECL defined.
+On many systems, open and creat are declared in fcntl.h .
+
+If your system's sprintf does not work the way ANSI C
+specifies -- specifically, if it does not return the
+number of characters transmitted -- then insert the line
+
+#define USE_STRLEN
+
+at the end of fmt.h . This is necessary with
+at least some versions of Sun software.
+In particular, if you get a warning about an improper
+pointer/integer combination in compiling wref.c, then
+you need to compile with -DUSE_STRLEN .
+
+If your system's fopen does not like the ANSI binary
+reading and writing modes "rb" and "wb", then you should
+compile open.c with NON_ANSI_RW_MODES #defined.
+
+If you get error messages about references to cf->_ptr
+and cf->_base when compiling wrtfmt.c and wsfe.c or to
+stderr->_flag when compiling err.c, then insert the line
+
+#define NON_UNIX_STDIO
+
+at the beginning of fio.h, and recompile everything (or
+at least those modules that contain NON_UNIX_STDIO).
+
+Unformatted sequential records consist of a length of record
+contents, the record contents themselves, and the length of
+record contents again (for backspace). Prior to 17 Oct. 1991,
+the length was of type int; now it is of type long, but you
+can change it back to int by inserting
+
+#define UIOLEN_int
+
+at the beginning of fio.h. This affects only sue.c and uio.c .
+
+If you have a really ancient K&R C compiler that does not understand
+void, add -Dvoid=int to the definition of CFLAGS in the makefile.
+
+On VAX, Cray, or Research Tenth-Edition Unix systems, you may
+need to add -DVAX, -DCRAY, or -DV10 (respectively) to CFLAGS
+to make fp.h work correctly. Alternatively, you may need to
+edit fp.h to suit your machine.
+
+If your compiler complains about the signal calls in main.c, s_paus.c,
+and signal_.c, you may need to adjust signal1.h suitably. See the
+comments in signal1.h.
+
+You may need to supply the following non-ANSI routines:
+
+ fstat(int fileds, struct stat *buf) is similar
+to stat(char *name, struct stat *buf), except that
+the first argument, fileds, is the file descriptor
+returned by open rather than the name of the file.
+fstat is used in the system-dependent routine
+canseek (in the libf2c source file err.c), which
+is supposed to return 1 if it's possible to issue
+seeks on the file in question, 0 if it's not; you may
+need to suitably modify err.c . On non-UNIX systems,
+you can avoid references to fstat and stat by compiling
+with NON_UNIX_STDIO defined; in that case, you may need
+to supply access(char *Name,0), which is supposed to
+return 0 if file Name exists, nonzero otherwise.
+
+ char * mktemp(char *buf) is supposed to replace the
+6 trailing X's in buf with a unique number and then
+return buf. The idea is to get a unique name for
+a temporary file.
+
+On non-UNIX systems, you may need to change a few other,
+e.g.: the form of name computed by mktemp() in endfile.c and
+open.c; the use of the open(), close(), and creat() system
+calls in endfile.c, err.c, open.c; and the modes in calls on
+fopen() and fdopen() (and perhaps the use of fdopen() itself
+-- it's supposed to return a FILE* corresponding to a given
+an integer file descriptor) in err.c and open.c (component ufmt
+of struct unit is 1 for formatted I/O -- text mode on some systems
+-- and 0 for unformatted I/O -- binary mode on some systems).
+Compiling with -DNON_UNIX_STDIO omits all references to creat()
+and almost all references to open() and close(), the exception
+being in the function f__isdev() (in open.c).
+
+If you wish to use translated Fortran that has funny notions
+of record length for direct unformatted I/O (i.e., that assumes
+RECL= values in OPEN statements are not bytes but rather counts
+of some other units -- e.g., 4-character words for VMS), then you
+should insert an appropriate #define for url_Adjust at the
+beginning of open.c . For VMS Fortran, for example,
+#define url_Adjust(x) x *= 4
+would suffice.
+
+By default, Fortran I/O units 5, 6, and 0 are pre-connected to
+stdin, stdout, and stderr, respectively. You can change this
+behavior by changing f_init() in err.c to suit your needs.
+Note that f2c assumes READ(*... means READ(5... and WRITE(*...
+means WRITE(6... . Moreover, an OPEN(n,... statement that does
+not specify a file name (and does not specify STATUS='SCRATCH')
+assumes FILE='fort.n' . You can change this by editing open.c
+and endfile.c suitably.
+
+Unless you adjust the "#define MXUNIT" line in fio.h, Fortran units
+0, 1, ..., 99 are available, i.e., the highest allowed unit number
+is MXUNIT - 1.
+
+Lines protected from compilation by #ifdef Allow_TYQUAD
+are for a possible extension to 64-bit integers in which
+integer = int = 32 bits and longint = long = 64 bits.
+
+The makefile does not attempt to compile pow_qq.c, qbitbits.c,
+and qbitshft.c, which are meant for use with INTEGER*8. To use
+INTEGER*8, you must modify f2c.h to declare longint and ulongint
+appropriately; then add $(QINT) to the end of the makefile's
+dependency list for libf2c.a (if makefile is a copy of makefile.u;
+for the PC makefiles, add pow_qq.obj qbitbits.obj qbitshft.obj
+to the library's dependency list and adjust libf2c.lbc or libf2c.sy
+accordingly). Also add -DAllow_TYQUAD to the makefile's CFLAGS
+assignment. To make longint and ulongint available, it may suffice
+to add -DINTEGER_STAR_8 to the CFLAGS assignment.
+
+Following Fortran 90, s_cat.c and s_copy.c allow the target of a
+(character string) assignment to be appear on its right-hand, at
+the cost of some extra overhead for all run-time concatenations.
+If you prefer the extra efficiency that comes with the Fortran 77
+requirement that the left-hand side of a character assignment not
+be involved in the right-hand side, compile s_cat.c and s_copy.c
+with -DNO_OVERWRITE .
+
+Extensions (Feb. 1993) to NAMELIST processing:
+ 1. Reading a ? instead of &name (the start of a namelist) causes
+the namelist being sought to be written to stdout (unit 6);
+to omit this feature, compile rsne.c with -DNo_Namelist_Questions.
+ 2. Reading the wrong namelist name now leads to an error message
+and an attempt to skip input until the right namelist name is found;
+to omit this feature, compile rsne.c with -DNo_Bad_Namelist_Skip.
+ 3. Namelist writes now insert newlines before each variable; to omit
+this feature, compile xwsne.c with -DNo_Extra_Namelist_Newlines.
+ 4. (Sept. 1995) When looking for the &name that starts namelist
+input, lines whose first non-blank character is something other
+than &, $, or ? are treated as comment lines and ignored, unless
+rsne.c is compiled with -DNo_Namelist_Comments.
+
+Nonstandard extension (Feb. 1993) to open: for sequential files,
+ACCESS='APPEND' (or access='anything else starting with "A" or "a"')
+causes the file to be positioned at end-of-file, so a write will
+append to the file.
+
+Some buggy Fortran programs use unformatted direct I/O to write
+an incomplete record and later read more from that record than
+they have written. For records other than the last, the unwritten
+portion of the record reads as binary zeros. The last record is
+a special case: attempting to read more from it than was written
+gives end-of-file -- which may help one find a bug. Some other
+Fortran I/O libraries treat the last record no differently than
+others and thus give no help in finding the bug of reading more
+than was written. If you wish to have this behavior, compile
+uio.c with -DPad_UDread .
+
+If you want to be able to catch write failures (e.g., due to a
+disk being full) with an ERR= specifier, compile dfe.c, due.c,
+sfe.c, sue.c, and wsle.c with -DALWAYS_FLUSH. This will lead to
+slower execution and more I/O, but should make ERR= work as
+expected, provided fflush returns an error return when its
+physical write fails.
+
+Carriage controls are meant to be interpreted by the UNIX col
+program (or a similar program). Sometimes it's convenient to use
+only ' ' as the carriage control character (normal single spacing).
+If you compile lwrite.c and wsfe.c with -DOMIT_BLANK_CC, formatted
+external output lines will have an initial ' ' quietly omitted,
+making use of the col program unnecessary with output that only
+has ' ' for carriage control.
+
+The Fortran 77 Standard leaves it up to the implementation whether
+formatted writes of floating-point numbers of absolute value < 1 have
+a zero before the decimal point. By default, libI77 omits such
+superfluous zeros, but you can cause them to appear by compiling
+lwrite.c, wref.c, and wrtfmt.c with -DWANT_LEAD_0 .
+
+If your (Unix) system lacks a ranlib command, you don't need it.
+Either comment out the makefile's ranlib invocation, or install
+a harmless "ranlib" command somewhere in your PATH, such as the
+one-line shell script
+
+ exit 0
+
+or (on some systems)
+
+ exec /usr/bin/ar lts $1 >/dev/null
+
+By default, the routines that implement complex and double complex
+division, c_div.c and z_div.c, call sig_die to print an error message
+and exit if they see a divisor of 0, as this is sometimes helpful for
+debugging. On systems with IEEE arithmetic, compiling c_div.c and
+z_div.c with -DIEEE_COMPLEX_DIVIDE causes them instead to set both
+the real and imaginary parts of the result to +INFINITY if the
+numerator is nonzero, or to NaN if it vanishes.
+
+Nowadays most Unix and Linux systems have function
+ int ftruncate(int fildes, off_t len);
+defined in system header file unistd.h that adjusts the length of file
+descriptor fildes to length len. Unless endfile.c is compiled with
+-DNO_TRUNCATE, endfile.c #includes "unistd.h" and calls ftruncate() if
+necessary to shorten files. If your system lacks ftruncate(), compile
+endfile.c with -DNO_TRUNCATE to make endfile.c use the older and more
+portable scheme of shortening a file by copying to a temporary file
+and back again.
+
+The initializations for "f2c -trapuv" are done by _uninit_f2c(),
+whose source is uninit.c, introduced June 2001. On IEEE-arithmetic
+systems, _uninit_f2c should initialize floating-point variables to
+signaling NaNs and, at its first invocation, should enable the
+invalid operation exception. Alas, the rules for distinguishing
+signaling from quiet NaNs were not specified in the IEEE P754 standard,
+nor were the precise means of enabling and disabling IEEE-arithmetic
+exceptions, and these details are thus system dependent. There are
+#ifdef's in uninit.c that specify them for some popular systems. If
+yours is not one of these systems, it may take some detective work to
+discover the appropriate details for your system. Sometimes it helps
+to look in the standard include directories for header files with
+relevant-sounding names, such as ieeefp.h, nan.h, or trap.h, and
+it may be simplest to run experiments to see what distinguishes a
+signaling from a quiet NaN. (If x is initialized to a signaling
+NaN and the invalid operation exception is masked off, as it should
+be by default on IEEE-arithmetic systems, then computing, say,
+y = x + 1 will yield a quiet NaN.)
diff --git a/3rdparty/clapack/f2c/close.c b/3rdparty/clapack/f2c/close.c
new file mode 100644
index 0000000..e958c71
--- /dev/null
+++ b/3rdparty/clapack/f2c/close.c
@@ -0,0 +1,101 @@
+#include "f2c.h"
+#include "fio.h"
+#ifdef KR_headers
+integer f_clos(a) cllist *a;
+#else
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#ifdef NON_UNIX_STDIO
+#ifndef unlink
+#define unlink remove
+#endif
+#else
+#ifdef MSDOS
+#include "io.h"
+#else
+#ifdef __cplusplus
+extern "C" int unlink(const char*);
+#else
+extern int unlink(const char*);
+#endif
+#endif
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+integer f_clos(cllist *a)
+#endif
+{ unit *b;
+
+ if(a->cunit >= MXUNIT) return(0);
+ b= &f__units[a->cunit];
+ if(b->ufd==NULL)
+ goto done;
+ if (b->uscrtch == 1)
+ goto Delete;
+ if (!a->csta)
+ goto Keep;
+ switch(*a->csta) {
+ default:
+ Keep:
+ case 'k':
+ case 'K':
+ if(b->uwrt == 1)
+ t_runc((alist *)a);
+ if(b->ufnm) {
+ fclose(b->ufd);
+ free(b->ufnm);
+ }
+ break;
+ case 'd':
+ case 'D':
+ Delete:
+ fclose(b->ufd);
+ if(b->ufnm) {
+ unlink(b->ufnm); /*SYSDEP*/
+ free(b->ufnm);
+ }
+ }
+ b->ufd=NULL;
+ done:
+ b->uend=0;
+ b->ufnm=NULL;
+ return(0);
+ }
+ void
+#ifdef KR_headers
+f_exit()
+#else
+f_exit(void)
+#endif
+{ int i;
+ static cllist xx;
+ if (!xx.cerr) {
+ xx.cerr=1;
+ xx.csta=NULL;
+ for(i=0;i<MXUNIT;i++)
+ {
+ xx.cunit=i;
+ (void) f_clos(&xx);
+ }
+ }
+}
+ int
+#ifdef KR_headers
+flush_()
+#else
+flush_(void)
+#endif
+{ int i;
+ for(i=0;i<MXUNIT;i++)
+ if(f__units[i].ufd != NULL && f__units[i].uwrt)
+ fflush(f__units[i].ufd);
+return 0;
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/d_sign.c b/3rdparty/clapack/f2c/d_sign.c
new file mode 100644
index 0000000..d06e0d1
--- /dev/null
+++ b/3rdparty/clapack/f2c/d_sign.c
@@ -0,0 +1,18 @@
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef KR_headers
+double d_sign(a,b) doublereal *a, *b;
+#else
+double d_sign(doublereal *a, doublereal *b)
+#endif
+{
+double x;
+x = (*a >= 0 ? *a : - *a);
+return( *b >= 0 ? x : -x);
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/endfile.c b/3rdparty/clapack/f2c/endfile.c
new file mode 100644
index 0000000..04020d3
--- /dev/null
+++ b/3rdparty/clapack/f2c/endfile.c
@@ -0,0 +1,160 @@
+#include "f2c.h"
+#include "fio.h"
+
+/* Compile this with -DNO_TRUNCATE if unistd.h does not exist or */
+/* if it does not define int truncate(const char *name, off_t). */
+
+#ifdef MSDOS
+#undef NO_TRUNCATE
+#define NO_TRUNCATE
+#endif
+
+#ifndef NO_TRUNCATE
+#include "unistd.h"
+#endif
+
+#ifdef KR_headers
+extern char *strcpy();
+extern FILE *tmpfile();
+#else
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#include "string.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#endif
+
+extern char *f__r_mode[], *f__w_mode[];
+
+#ifdef KR_headers
+integer f_end(a) alist *a;
+#else
+integer f_end(alist *a)
+#endif
+{
+ unit *b;
+ FILE *tf;
+
+ if(a->aunit>=MXUNIT || a->aunit<0) err(a->aerr,101,"endfile");
+ b = &f__units[a->aunit];
+ if(b->ufd==NULL) {
+ char nbuf[10];
+ sprintf(nbuf,"fort.%ld",(long)a->aunit);
+ if (tf = FOPEN(nbuf, f__w_mode[0]))
+ fclose(tf);
+ return(0);
+ }
+ b->uend=1;
+ return(b->useek ? t_runc(a) : 0);
+}
+
+#ifdef NO_TRUNCATE
+ static int
+#ifdef KR_headers
+copy(from, len, to) FILE *from, *to; register long len;
+#else
+copy(FILE *from, register long len, FILE *to)
+#endif
+{
+ int len1;
+ char buf[BUFSIZ];
+
+ while(fread(buf, len1 = len > BUFSIZ ? BUFSIZ : (int)len, 1, from)) {
+ if (!fwrite(buf, len1, 1, to))
+ return 1;
+ if ((len -= len1) <= 0)
+ break;
+ }
+ return 0;
+ }
+#endif /* NO_TRUNCATE */
+
+ int
+#ifdef KR_headers
+t_runc(a) alist *a;
+#else
+t_runc(alist *a)
+#endif
+{
+ OFF_T loc, len;
+ unit *b;
+ int rc;
+ FILE *bf;
+#ifdef NO_TRUNCATE
+ FILE *tf;
+#endif
+
+ b = &f__units[a->aunit];
+ if(b->url)
+ return(0); /*don't truncate direct files*/
+ loc=FTELL(bf = b->ufd);
+ FSEEK(bf,(OFF_T)0,SEEK_END);
+ len=FTELL(bf);
+ if (loc >= len || b->useek == 0)
+ return(0);
+#ifdef NO_TRUNCATE
+ if (b->ufnm == NULL)
+ return 0;
+ rc = 0;
+ fclose(b->ufd);
+ if (!loc) {
+ if (!(bf = FOPEN(b->ufnm, f__w_mode[b->ufmt])))
+ rc = 1;
+ if (b->uwrt)
+ b->uwrt = 1;
+ goto done;
+ }
+ if (!(bf = FOPEN(b->ufnm, f__r_mode[0]))
+ || !(tf = tmpfile())) {
+#ifdef NON_UNIX_STDIO
+ bad:
+#endif
+ rc = 1;
+ goto done;
+ }
+ if (copy(bf, (long)loc, tf)) {
+ bad1:
+ rc = 1;
+ goto done1;
+ }
+ if (!(bf = FREOPEN(b->ufnm, f__w_mode[0], bf)))
+ goto bad1;
+ rewind(tf);
+ if (copy(tf, (long)loc, bf))
+ goto bad1;
+ b->uwrt = 1;
+ b->urw = 2;
+#ifdef NON_UNIX_STDIO
+ if (b->ufmt) {
+ fclose(bf);
+ if (!(bf = FOPEN(b->ufnm, f__w_mode[3])))
+ goto bad;
+ FSEEK(bf,(OFF_T)0,SEEK_END);
+ b->urw = 3;
+ }
+#endif
+done1:
+ fclose(tf);
+done:
+ f__cf = b->ufd = bf;
+#else /* NO_TRUNCATE */
+ if (b->urw & 2)
+ fflush(b->ufd); /* necessary on some Linux systems */
+#ifndef FTRUNCATE
+#define FTRUNCATE ftruncate
+#endif
+ rc = FTRUNCATE(fileno(b->ufd), loc);
+ /* The following FSEEK is unnecessary on some systems, */
+ /* but should be harmless. */
+ FSEEK(b->ufd, (OFF_T)0, SEEK_END);
+#endif /* NO_TRUNCATE */
+ if (rc)
+ err(a->aerr,111,"endfile");
+ return 0;
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/err.c b/3rdparty/clapack/f2c/err.c
new file mode 100644
index 0000000..80a3b74
--- /dev/null
+++ b/3rdparty/clapack/f2c/err.c
@@ -0,0 +1,293 @@
+#include "sysdep1.h" /* here to get stat64 on some badly designed Linux systems */
+#include "f2c.h"
+#ifdef KR_headers
+#define Const /*nothing*/
+extern char *malloc();
+#else
+#define Const const
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#endif
+#include "fio.h"
+#include "fmt.h" /* for struct syl */
+
+/* Compile this with -DNO_ISATTY if unistd.h does not exist or */
+/* if it does not define int isatty(int). */
+#ifdef NO_ISATTY
+#define isatty(x) 0
+#else
+#include <unistd.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*global definitions*/
+unit f__units[MXUNIT]; /*unit table*/
+flag f__init; /*0 on entry, 1 after initializations*/
+cilist *f__elist; /*active external io list*/
+icilist *f__svic; /*active internal io list*/
+flag f__reading; /*1 if reading, 0 if writing*/
+flag f__cplus,f__cblank;
+Const char *f__fmtbuf;
+flag f__external; /*1 if external io, 0 if internal */
+#ifdef KR_headers
+int (*f__doed)(),(*f__doned)();
+int (*f__doend)(),(*f__donewrec)(),(*f__dorevert)();
+int (*f__getn)(); /* for formatted input */
+void (*f__putn)(); /* for formatted output */
+#else
+int (*f__getn)(void); /* for formatted input */
+void (*f__putn)(int); /* for formatted output */
+int (*f__doed)(struct syl*, char*, ftnlen),(*f__doned)(struct syl*);
+int (*f__dorevert)(void),(*f__donewrec)(void),(*f__doend)(void);
+#endif
+flag f__sequential; /*1 if sequential io, 0 if direct*/
+flag f__formatted; /*1 if formatted io, 0 if unformatted*/
+FILE *f__cf; /*current file*/
+unit *f__curunit; /*current unit*/
+int f__recpos; /*place in current record*/
+OFF_T f__cursor, f__hiwater;
+int f__scale;
+char *f__icptr;
+
+/*error messages*/
+Const char *F_err[] =
+{
+ "error in format", /* 100 */
+ "illegal unit number", /* 101 */
+ "formatted io not allowed", /* 102 */
+ "unformatted io not allowed", /* 103 */
+ "direct io not allowed", /* 104 */
+ "sequential io not allowed", /* 105 */
+ "can't backspace file", /* 106 */
+ "null file name", /* 107 */
+ "can't stat file", /* 108 */
+ "unit not connected", /* 109 */
+ "off end of record", /* 110 */
+ "truncation failed in endfile", /* 111 */
+ "incomprehensible list input", /* 112 */
+ "out of free space", /* 113 */
+ "unit not connected", /* 114 */
+ "read unexpected character", /* 115 */
+ "bad logical input field", /* 116 */
+ "bad variable type", /* 117 */
+ "bad namelist name", /* 118 */
+ "variable not in namelist", /* 119 */
+ "no end record", /* 120 */
+ "variable count incorrect", /* 121 */
+ "subscript for scalar variable", /* 122 */
+ "invalid array section", /* 123 */
+ "substring out of bounds", /* 124 */
+ "subscript out of bounds", /* 125 */
+ "can't read file", /* 126 */
+ "can't write file", /* 127 */
+ "'new' file exists", /* 128 */
+ "can't append to file", /* 129 */
+ "non-positive record number", /* 130 */
+ "nmLbuf overflow" /* 131 */
+};
+#define MAXERR (sizeof(F_err)/sizeof(char *)+100)
+
+ int
+#ifdef KR_headers
+f__canseek(f) FILE *f; /*SYSDEP*/
+#else
+f__canseek(FILE *f) /*SYSDEP*/
+#endif
+{
+#ifdef NON_UNIX_STDIO
+ return !isatty(fileno(f));
+#else
+ struct STAT_ST x;
+
+ if (FSTAT(fileno(f),&x) < 0)
+ return(0);
+#ifdef S_IFMT
+ switch(x.st_mode & S_IFMT) {
+ case S_IFDIR:
+ case S_IFREG:
+ if(x.st_nlink > 0) /* !pipe */
+ return(1);
+ else
+ return(0);
+ case S_IFCHR:
+ if(isatty(fileno(f)))
+ return(0);
+ return(1);
+#ifdef S_IFBLK
+ case S_IFBLK:
+ return(1);
+#endif
+ }
+#else
+#ifdef S_ISDIR
+ /* POSIX version */
+ if (S_ISREG(x.st_mode) || S_ISDIR(x.st_mode)) {
+ if(x.st_nlink > 0) /* !pipe */
+ return(1);
+ else
+ return(0);
+ }
+ if (S_ISCHR(x.st_mode)) {
+ if(isatty(fileno(f)))
+ return(0);
+ return(1);
+ }
+ if (S_ISBLK(x.st_mode))
+ return(1);
+#else
+ Help! How does fstat work on this system?
+#endif
+#endif
+ return(0); /* who knows what it is? */
+#endif
+}
+
+ void
+#ifdef KR_headers
+f__fatal(n,s) char *s;
+#else
+f__fatal(int n, const char *s)
+#endif
+{
+ if(n<100 && n>=0) perror(s); /*SYSDEP*/
+ else if(n >= (int)MAXERR || n < -1)
+ { fprintf(stderr,"%s: illegal error number %d\n",s,n);
+ }
+ else if(n == -1) fprintf(stderr,"%s: end of file\n",s);
+ else
+ fprintf(stderr,"%s: %s\n",s,F_err[n-100]);
+ if (f__curunit) {
+ fprintf(stderr,"apparent state: unit %d ",
+ (int)(f__curunit-f__units));
+ fprintf(stderr, f__curunit->ufnm ? "named %s\n" : "(unnamed)\n",
+ f__curunit->ufnm);
+ }
+ else
+ fprintf(stderr,"apparent state: internal I/O\n");
+ if (f__fmtbuf)
+ fprintf(stderr,"last format: %s\n",f__fmtbuf);
+ fprintf(stderr,"lately %s %s %s %s",f__reading?"reading":"writing",
+ f__sequential?"sequential":"direct",f__formatted?"formatted":"unformatted",
+ f__external?"external":"internal");
+ sig_die(" IO", 1);
+}
+/*initialization routine*/
+ VOID
+f_init(Void)
+{ unit *p;
+
+ f__init=1;
+ p= &f__units[0];
+ p->ufd=stderr;
+ p->useek=f__canseek(stderr);
+ p->ufmt=1;
+ p->uwrt=1;
+ p = &f__units[5];
+ p->ufd=stdin;
+ p->useek=f__canseek(stdin);
+ p->ufmt=1;
+ p->uwrt=0;
+ p= &f__units[6];
+ p->ufd=stdout;
+ p->useek=f__canseek(stdout);
+ p->ufmt=1;
+ p->uwrt=1;
+}
+
+ int
+#ifdef KR_headers
+f__nowreading(x) unit *x;
+#else
+f__nowreading(unit *x)
+#endif
+{
+ OFF_T loc;
+ int ufmt, urw;
+ extern char *f__r_mode[], *f__w_mode[];
+
+ if (x->urw & 1)
+ goto done;
+ if (!x->ufnm)
+ goto cantread;
+ ufmt = x->url ? 0 : x->ufmt;
+ loc = FTELL(x->ufd);
+ urw = 3;
+ if (!FREOPEN(x->ufnm, f__w_mode[ufmt|2], x->ufd)) {
+ urw = 1;
+ if(!FREOPEN(x->ufnm, f__r_mode[ufmt], x->ufd)) {
+ cantread:
+ errno = 126;
+ return 1;
+ }
+ }
+ FSEEK(x->ufd,loc,SEEK_SET);
+ x->urw = urw;
+ done:
+ x->uwrt = 0;
+ return 0;
+}
+
+ int
+#ifdef KR_headers
+f__nowwriting(x) unit *x;
+#else
+f__nowwriting(unit *x)
+#endif
+{
+ OFF_T loc;
+ int ufmt;
+ extern char *f__w_mode[];
+
+ if (x->urw & 2) {
+ if (x->urw & 1)
+ FSEEK(x->ufd, (OFF_T)0, SEEK_CUR);
+ goto done;
+ }
+ if (!x->ufnm)
+ goto cantwrite;
+ ufmt = x->url ? 0 : x->ufmt;
+ if (x->uwrt == 3) { /* just did write, rewind */
+ if (!(f__cf = x->ufd =
+ FREOPEN(x->ufnm,f__w_mode[ufmt],x->ufd)))
+ goto cantwrite;
+ x->urw = 2;
+ }
+ else {
+ loc=FTELL(x->ufd);
+ if (!(f__cf = x->ufd =
+ FREOPEN(x->ufnm, f__w_mode[ufmt | 2], x->ufd)))
+ {
+ x->ufd = NULL;
+ cantwrite:
+ errno = 127;
+ return(1);
+ }
+ x->urw = 3;
+ FSEEK(x->ufd,loc,SEEK_SET);
+ }
+ done:
+ x->uwrt = 1;
+ return 0;
+}
+
+ int
+#ifdef KR_headers
+err__fl(f, m, s) int f, m; char *s;
+#else
+err__fl(int f, int m, const char *s)
+#endif
+{
+ if (!f)
+ f__fatal(m, s);
+ if (f__doend)
+ (*f__doend)();
+ return errno = m;
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/exit_.c b/3rdparty/clapack/f2c/exit_.c
new file mode 100644
index 0000000..08e9d07
--- /dev/null
+++ b/3rdparty/clapack/f2c/exit_.c
@@ -0,0 +1,43 @@
+/* This gives the effect of
+
+ subroutine exit(rc)
+ integer*4 rc
+ stop
+ end
+
+ * with the added side effect of supplying rc as the program's exit code.
+ */
+
+#include "f2c.h"
+#undef abs
+#undef min
+#undef max
+#ifndef KR_headers
+#include "stdlib.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern void f_exit(void);
+#endif
+
+ void
+#ifdef KR_headers
+exit_(rc) integer *rc;
+#else
+exit_(integer *rc)
+#endif
+{
+#ifdef NO_ONEXIT
+ f_exit();
+#endif
+ exit(*rc);
+ }
+#ifdef __cplusplus
+}
+#endif
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/f77_aloc.c b/3rdparty/clapack/f2c/f77_aloc.c
new file mode 100644
index 0000000..f536099
--- /dev/null
+++ b/3rdparty/clapack/f2c/f77_aloc.c
@@ -0,0 +1,44 @@
+#include "f2c.h"
+#undef abs
+#undef min
+#undef max
+#include "stdio.h"
+
+static integer memfailure = 3;
+
+#ifdef KR_headers
+extern char *malloc();
+extern void exit_();
+
+ char *
+F77_aloc(Len, whence) integer Len; char *whence;
+#else
+#include "stdlib.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern void exit_(integer*);
+#ifdef __cplusplus
+ }
+#endif
+
+ char *
+F77_aloc(integer Len, const char *whence)
+#endif
+{
+ char *rv;
+ unsigned int uLen = (unsigned int) Len; /* for K&R C */
+
+ if (!(rv = (char*)malloc(uLen))) {
+ fprintf(stderr, "malloc(%u) failure in %s\n",
+ uLen, whence);
+ exit_(&memfailure);
+ }
+ return rv;
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/fio.h b/3rdparty/clapack/f2c/fio.h
new file mode 100644
index 0000000..ebf7696
--- /dev/null
+++ b/3rdparty/clapack/f2c/fio.h
@@ -0,0 +1,141 @@
+#ifndef SYSDEP_H_INCLUDED
+#include "sysdep1.h"
+#endif
+#include "stdio.h"
+#include "errno.h"
+#ifndef NULL
+/* ANSI C */
+#include "stddef.h"
+#endif
+
+#ifndef SEEK_SET
+#define SEEK_SET 0
+#define SEEK_CUR 1
+#define SEEK_END 2
+#endif
+
+#ifndef FOPEN
+#define FOPEN fopen
+#endif
+
+#ifndef FREOPEN
+#define FREOPEN freopen
+#endif
+
+#ifndef FSEEK
+#define FSEEK fseek
+#endif
+
+#ifndef FSTAT
+#define FSTAT fstat
+#endif
+
+#ifndef FTELL
+#define FTELL ftell
+#endif
+
+#ifndef OFF_T
+#define OFF_T long
+#endif
+
+#ifndef STAT_ST
+#define STAT_ST stat
+#endif
+
+#ifndef STAT
+#define STAT stat
+#endif
+
+#ifdef MSDOS
+#ifndef NON_UNIX_STDIO
+#define NON_UNIX_STDIO
+#endif
+#endif
+
+#ifdef UIOLEN_int
+typedef int uiolen;
+#else
+typedef long uiolen;
+#endif
+
+/*units*/
+typedef struct
+{ FILE *ufd; /*0=unconnected*/
+ char *ufnm;
+#ifndef MSDOS
+ long uinode;
+ int udev;
+#endif
+ int url; /*0=sequential*/
+ flag useek; /*true=can backspace, use dir, ...*/
+ flag ufmt;
+ flag urw; /* (1 for can read) | (2 for can write) */
+ flag ublnk;
+ flag uend;
+ flag uwrt; /*last io was write*/
+ flag uscrtch;
+} unit;
+
+#undef Void
+#ifdef KR_headers
+#define Void /*void*/
+extern int (*f__getn)(); /* for formatted input */
+extern void (*f__putn)(); /* for formatted output */
+extern void x_putc();
+extern long f__inode();
+extern VOID sig_die();
+extern int (*f__donewrec)(), t_putc(), x_wSL();
+extern int c_sfe(), err__fl(), xrd_SL(), f__putbuf();
+#else
+#define Void void
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int (*f__getn)(void); /* for formatted input */
+extern void (*f__putn)(int); /* for formatted output */
+extern void x_putc(int);
+extern long f__inode(char*,int*);
+extern void sig_die(const char*,int);
+extern void f__fatal(int, const char*);
+extern int t_runc(alist*);
+extern int f__nowreading(unit*), f__nowwriting(unit*);
+extern int fk_open(int,int,ftnint);
+extern int en_fio(void);
+extern void f_init(void);
+extern int (*f__donewrec)(void), t_putc(int), x_wSL(void);
+extern void b_char(const char*,char*,ftnlen), g_char(const char*,ftnlen,char*);
+extern int c_sfe(cilist*), z_rnew(void);
+extern int err__fl(int,int,const char*);
+extern int xrd_SL(void);
+extern int f__putbuf(int);
+#endif
+extern flag f__init;
+extern cilist *f__elist; /*active external io list*/
+extern flag f__reading,f__external,f__sequential,f__formatted;
+extern int (*f__doend)(Void);
+extern FILE *f__cf; /*current file*/
+extern unit *f__curunit; /*current unit*/
+extern unit f__units[];
+#define err(f,m,s) {if(f) errno= m; else f__fatal(m,s); return(m);}
+#define errfl(f,m,s) return err__fl((int)f,m,s)
+
+/*Table sizes*/
+#define MXUNIT 100
+
+extern int f__recpos; /*position in current record*/
+extern OFF_T f__cursor; /* offset to move to */
+extern OFF_T f__hiwater; /* so TL doesn't confuse us */
+#ifdef __cplusplus
+ }
+#endif
+
+#define WRITE 1
+#define READ 2
+#define SEQ 3
+#define DIR 4
+#define FMT 5
+#define UNF 6
+#define EXT 7
+#define INT 8
+
+#define buf_end(x) (x->_flag & _IONBF ? x->_ptr : x->_base + BUFSIZ)
diff --git a/3rdparty/clapack/f2c/fmt.c b/3rdparty/clapack/f2c/fmt.c
new file mode 100644
index 0000000..286c98f
--- /dev/null
+++ b/3rdparty/clapack/f2c/fmt.c
@@ -0,0 +1,530 @@
+#include "f2c.h"
+#include "fio.h"
+#include "fmt.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#define skip(s) while(*s==' ') s++
+#ifdef interdata
+#define SYLMX 300
+#endif
+#ifdef pdp11
+#define SYLMX 300
+#endif
+#ifdef vax
+#define SYLMX 300
+#endif
+#ifndef SYLMX
+#define SYLMX 300
+#endif
+#define GLITCH '\2'
+ /* special quote character for stu */
+extern flag f__cblank,f__cplus; /*blanks in I and compulsory plus*/
+static struct syl f__syl[SYLMX];
+int f__parenlvl,f__pc,f__revloc;
+#ifdef KR_headers
+#define Const /*nothing*/
+#else
+#define Const const
+#endif
+
+ static
+#ifdef KR_headers
+char *ap_end(s) char *s;
+#else
+const char *ap_end(const char *s)
+#endif
+{ char quote;
+ quote= *s++;
+ for(;*s;s++)
+ { if(*s!=quote) continue;
+ if(*++s!=quote) return(s);
+ }
+ if(f__elist->cierr) {
+ errno = 100;
+ return(NULL);
+ }
+ f__fatal(100, "bad string");
+ /*NOTREACHED*/ return 0;
+}
+ static int
+#ifdef KR_headers
+op_gen(a,b,c,d)
+#else
+op_gen(int a, int b, int c, int d)
+#endif
+{ struct syl *p= &f__syl[f__pc];
+ if(f__pc>=SYLMX)
+ { fprintf(stderr,"format too complicated:\n");
+ sig_die(f__fmtbuf, 1);
+ }
+ p->op=a;
+ p->p1=b;
+ p->p2.i[0]=c;
+ p->p2.i[1]=d;
+ return(f__pc++);
+}
+#ifdef KR_headers
+static char *f_list();
+static char *gt_num(s,n,n1) char *s; int *n, n1;
+#else
+static const char *f_list(const char*);
+static const char *gt_num(const char *s, int *n, int n1)
+#endif
+{ int m=0,f__cnt=0;
+ char c;
+ for(c= *s;;c = *s)
+ { if(c==' ')
+ { s++;
+ continue;
+ }
+ if(c>'9' || c<'0') break;
+ m=10*m+c-'0';
+ f__cnt++;
+ s++;
+ }
+ if(f__cnt==0) {
+ if (!n1)
+ s = 0;
+ *n=n1;
+ }
+ else *n=m;
+ return(s);
+}
+
+ static
+#ifdef KR_headers
+char *f_s(s,curloc) char *s;
+#else
+const char *f_s(const char *s, int curloc)
+#endif
+{
+ skip(s);
+ if(*s++!='(')
+ {
+ return(NULL);
+ }
+ if(f__parenlvl++ ==1) f__revloc=curloc;
+ if(op_gen(RET1,curloc,0,0)<0 ||
+ (s=f_list(s))==NULL)
+ {
+ return(NULL);
+ }
+ skip(s);
+ return(s);
+}
+
+ static int
+#ifdef KR_headers
+ne_d(s,p) char *s,**p;
+#else
+ne_d(const char *s, const char **p)
+#endif
+{ int n,x,sign=0;
+ struct syl *sp;
+ switch(*s)
+ {
+ default:
+ return(0);
+ case ':': (void) op_gen(COLON,0,0,0); break;
+ case '$':
+ (void) op_gen(NONL, 0, 0, 0); break;
+ case 'B':
+ case 'b':
+ if(*++s=='z' || *s == 'Z') (void) op_gen(BZ,0,0,0);
+ else (void) op_gen(BN,0,0,0);
+ break;
+ case 'S':
+ case 's':
+ if(*(s+1)=='s' || *(s+1) == 'S')
+ { x=SS;
+ s++;
+ }
+ else if(*(s+1)=='p' || *(s+1) == 'P')
+ { x=SP;
+ s++;
+ }
+ else x=S;
+ (void) op_gen(x,0,0,0);
+ break;
+ case '/': (void) op_gen(SLASH,0,0,0); break;
+ case '-': sign=1;
+ case '+': s++; /*OUTRAGEOUS CODING TRICK*/
+ case '0': case '1': case '2': case '3': case '4':
+ case '5': case '6': case '7': case '8': case '9':
+ if (!(s=gt_num(s,&n,0))) {
+ bad: *p = 0;
+ return 1;
+ }
+ switch(*s)
+ {
+ default:
+ return(0);
+ case 'P':
+ case 'p': if(sign) n= -n; (void) op_gen(P,n,0,0); break;
+ case 'X':
+ case 'x': (void) op_gen(X,n,0,0); break;
+ case 'H':
+ case 'h':
+ sp = &f__syl[op_gen(H,n,0,0)];
+ sp->p2.s = (char*)s + 1;
+ s+=n;
+ break;
+ }
+ break;
+ case GLITCH:
+ case '"':
+ case '\'':
+ sp = &f__syl[op_gen(APOS,0,0,0)];
+ sp->p2.s = (char*)s;
+ if((*p = ap_end(s)) == NULL)
+ return(0);
+ return(1);
+ case 'T':
+ case 't':
+ if(*(s+1)=='l' || *(s+1) == 'L')
+ { x=TL;
+ s++;
+ }
+ else if(*(s+1)=='r'|| *(s+1) == 'R')
+ { x=TR;
+ s++;
+ }
+ else x=T;
+ if (!(s=gt_num(s+1,&n,0)))
+ goto bad;
+ s--;
+ (void) op_gen(x,n,0,0);
+ break;
+ case 'X':
+ case 'x': (void) op_gen(X,1,0,0); break;
+ case 'P':
+ case 'p': (void) op_gen(P,1,0,0); break;
+ }
+ s++;
+ *p=s;
+ return(1);
+}
+
+ static int
+#ifdef KR_headers
+e_d(s,p) char *s,**p;
+#else
+e_d(const char *s, const char **p)
+#endif
+{ int i,im,n,w,d,e,found=0,x=0;
+ Const char *sv=s;
+ s=gt_num(s,&n,1);
+ (void) op_gen(STACK,n,0,0);
+ switch(*s++)
+ {
+ default: break;
+ case 'E':
+ case 'e': x=1;
+ case 'G':
+ case 'g':
+ found=1;
+ if (!(s=gt_num(s,&w,0))) {
+ bad:
+ *p = 0;
+ return 1;
+ }
+ if(w==0) break;
+ if(*s=='.') {
+ if (!(s=gt_num(s+1,&d,0)))
+ goto bad;
+ }
+ else d=0;
+ if(*s!='E' && *s != 'e')
+ (void) op_gen(x==1?E:G,w,d,0); /* default is Ew.dE2 */
+ else {
+ if (!(s=gt_num(s+1,&e,0)))
+ goto bad;
+ (void) op_gen(x==1?EE:GE,w,d,e);
+ }
+ break;
+ case 'O':
+ case 'o':
+ i = O;
+ im = OM;
+ goto finish_I;
+ case 'Z':
+ case 'z':
+ i = Z;
+ im = ZM;
+ goto finish_I;
+ case 'L':
+ case 'l':
+ found=1;
+ if (!(s=gt_num(s,&w,0)))
+ goto bad;
+ if(w==0) break;
+ (void) op_gen(L,w,0,0);
+ break;
+ case 'A':
+ case 'a':
+ found=1;
+ skip(s);
+ if(*s>='0' && *s<='9')
+ { s=gt_num(s,&w,1);
+ if(w==0) break;
+ (void) op_gen(AW,w,0,0);
+ break;
+ }
+ (void) op_gen(A,0,0,0);
+ break;
+ case 'F':
+ case 'f':
+ if (!(s=gt_num(s,&w,0)))
+ goto bad;
+ found=1;
+ if(w==0) break;
+ if(*s=='.') {
+ if (!(s=gt_num(s+1,&d,0)))
+ goto bad;
+ }
+ else d=0;
+ (void) op_gen(F,w,d,0);
+ break;
+ case 'D':
+ case 'd':
+ found=1;
+ if (!(s=gt_num(s,&w,0)))
+ goto bad;
+ if(w==0) break;
+ if(*s=='.') {
+ if (!(s=gt_num(s+1,&d,0)))
+ goto bad;
+ }
+ else d=0;
+ (void) op_gen(D,w,d,0);
+ break;
+ case 'I':
+ case 'i':
+ i = I;
+ im = IM;
+ finish_I:
+ if (!(s=gt_num(s,&w,0)))
+ goto bad;
+ found=1;
+ if(w==0) break;
+ if(*s!='.')
+ { (void) op_gen(i,w,0,0);
+ break;
+ }
+ if (!(s=gt_num(s+1,&d,0)))
+ goto bad;
+ (void) op_gen(im,w,d,0);
+ break;
+ }
+ if(found==0)
+ { f__pc--; /*unSTACK*/
+ *p=sv;
+ return(0);
+ }
+ *p=s;
+ return(1);
+}
+ static
+#ifdef KR_headers
+char *i_tem(s) char *s;
+#else
+const char *i_tem(const char *s)
+#endif
+{ const char *t;
+ int n,curloc;
+ if(*s==')') return(s);
+ if(ne_d(s,&t)) return(t);
+ if(e_d(s,&t)) return(t);
+ s=gt_num(s,&n,1);
+ if((curloc=op_gen(STACK,n,0,0))<0) return(NULL);
+ return(f_s(s,curloc));
+}
+
+ static
+#ifdef KR_headers
+char *f_list(s) char *s;
+#else
+const char *f_list(const char *s)
+#endif
+{
+ for(;*s!=0;)
+ { skip(s);
+ if((s=i_tem(s))==NULL) return(NULL);
+ skip(s);
+ if(*s==',') s++;
+ else if(*s==')')
+ { if(--f__parenlvl==0)
+ {
+ (void) op_gen(REVERT,f__revloc,0,0);
+ return(++s);
+ }
+ (void) op_gen(GOTO,0,0,0);
+ return(++s);
+ }
+ }
+ return(NULL);
+}
+
+ int
+#ifdef KR_headers
+pars_f(s) char *s;
+#else
+pars_f(const char *s)
+#endif
+{
+ f__parenlvl=f__revloc=f__pc=0;
+ if(f_s(s,0) == NULL)
+ {
+ return(-1);
+ }
+ return(0);
+}
+#define STKSZ 10
+int f__cnt[STKSZ],f__ret[STKSZ],f__cp,f__rp;
+flag f__workdone, f__nonl;
+
+ static int
+#ifdef KR_headers
+type_f(n)
+#else
+type_f(int n)
+#endif
+{
+ switch(n)
+ {
+ default:
+ return(n);
+ case RET1:
+ return(RET1);
+ case REVERT: return(REVERT);
+ case GOTO: return(GOTO);
+ case STACK: return(STACK);
+ case X:
+ case SLASH:
+ case APOS: case H:
+ case T: case TL: case TR:
+ return(NED);
+ case F:
+ case I:
+ case IM:
+ case A: case AW:
+ case O: case OM:
+ case L:
+ case E: case EE: case D:
+ case G: case GE:
+ case Z: case ZM:
+ return(ED);
+ }
+}
+#ifdef KR_headers
+integer do_fio(number,ptr,len) ftnint *number; ftnlen len; char *ptr;
+#else
+integer do_fio(ftnint *number, char *ptr, ftnlen len)
+#endif
+{ struct syl *p;
+ int n,i;
+ for(i=0;i<*number;i++,ptr+=len)
+ {
+loop: switch(type_f((p= &f__syl[f__pc])->op))
+ {
+ default:
+ fprintf(stderr,"unknown code in do_fio: %d\n%s\n",
+ p->op,f__fmtbuf);
+ err(f__elist->cierr,100,"do_fio");
+ case NED:
+ if((*f__doned)(p))
+ { f__pc++;
+ goto loop;
+ }
+ f__pc++;
+ continue;
+ case ED:
+ if(f__cnt[f__cp]<=0)
+ { f__cp--;
+ f__pc++;
+ goto loop;
+ }
+ if(ptr==NULL)
+ return((*f__doend)());
+ f__cnt[f__cp]--;
+ f__workdone=1;
+ if((n=(*f__doed)(p,ptr,len))>0)
+ errfl(f__elist->cierr,errno,"fmt");
+ if(n<0)
+ err(f__elist->ciend,(EOF),"fmt");
+ continue;
+ case STACK:
+ f__cnt[++f__cp]=p->p1;
+ f__pc++;
+ goto loop;
+ case RET1:
+ f__ret[++f__rp]=p->p1;
+ f__pc++;
+ goto loop;
+ case GOTO:
+ if(--f__cnt[f__cp]<=0)
+ { f__cp--;
+ f__rp--;
+ f__pc++;
+ goto loop;
+ }
+ f__pc=1+f__ret[f__rp--];
+ goto loop;
+ case REVERT:
+ f__rp=f__cp=0;
+ f__pc = p->p1;
+ if(ptr==NULL)
+ return((*f__doend)());
+ if(!f__workdone) return(0);
+ if((n=(*f__dorevert)()) != 0) return(n);
+ goto loop;
+ case COLON:
+ if(ptr==NULL)
+ return((*f__doend)());
+ f__pc++;
+ goto loop;
+ case NONL:
+ f__nonl = 1;
+ f__pc++;
+ goto loop;
+ case S:
+ case SS:
+ f__cplus=0;
+ f__pc++;
+ goto loop;
+ case SP:
+ f__cplus = 1;
+ f__pc++;
+ goto loop;
+ case P: f__scale=p->p1;
+ f__pc++;
+ goto loop;
+ case BN:
+ f__cblank=0;
+ f__pc++;
+ goto loop;
+ case BZ:
+ f__cblank=1;
+ f__pc++;
+ goto loop;
+ }
+ }
+ return(0);
+}
+
+ int
+en_fio(Void)
+{ ftnint one=1;
+ return(do_fio(&one,(char *)NULL,(ftnint)0));
+}
+
+ VOID
+fmt_bg(Void)
+{
+ f__workdone=f__cp=f__rp=f__pc=f__cursor=0;
+ f__cnt[0]=f__ret[0]=0;
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/fmt.h b/3rdparty/clapack/f2c/fmt.h
new file mode 100644
index 0000000..ddfa551
--- /dev/null
+++ b/3rdparty/clapack/f2c/fmt.h
@@ -0,0 +1,105 @@
+struct syl
+{ int op;
+ int p1;
+ union { int i[2]; char *s;} p2;
+ };
+#define RET1 1
+#define REVERT 2
+#define GOTO 3
+#define X 4
+#define SLASH 5
+#define STACK 6
+#define I 7
+#define ED 8
+#define NED 9
+#define IM 10
+#define APOS 11
+#define H 12
+#define TL 13
+#define TR 14
+#define T 15
+#define COLON 16
+#define S 17
+#define SP 18
+#define SS 19
+#define P 20
+#define BN 21
+#define BZ 22
+#define F 23
+#define E 24
+#define EE 25
+#define D 26
+#define G 27
+#define GE 28
+#define L 29
+#define A 30
+#define AW 31
+#define O 32
+#define NONL 33
+#define OM 34
+#define Z 35
+#define ZM 36
+typedef union
+{ real pf;
+ doublereal pd;
+} ufloat;
+typedef union
+{ short is;
+#ifndef KR_headers
+ signed
+#endif
+ char ic;
+ integer il;
+#ifdef Allow_TYQUAD
+ longint ili;
+#endif
+} Uint;
+#ifdef KR_headers
+extern int (*f__doed)(),(*f__doned)();
+extern int (*f__dorevert)();
+extern int rd_ed(),rd_ned();
+extern int w_ed(),w_ned();
+extern int signbit_f2c();
+extern char *f__fmtbuf;
+#else
+#ifdef __cplusplus
+extern "C" {
+#define Cextern extern "C"
+#else
+#define Cextern extern
+#endif
+extern const char *f__fmtbuf;
+extern int (*f__doed)(struct syl*, char*, ftnlen),(*f__doned)(struct syl*);
+extern int (*f__dorevert)(void);
+extern void fmt_bg(void);
+extern int pars_f(const char*);
+extern int rd_ed(struct syl*, char*, ftnlen),rd_ned(struct syl*);
+extern int signbit_f2c(double*);
+extern int w_ed(struct syl*, char*, ftnlen),w_ned(struct syl*);
+extern int wrt_E(ufloat*, int, int, int, ftnlen);
+extern int wrt_F(ufloat*, int, int, ftnlen);
+extern int wrt_L(Uint*, int, ftnlen);
+#endif
+extern int f__pc,f__parenlvl,f__revloc;
+extern flag f__cblank,f__cplus,f__workdone, f__nonl;
+extern int f__scale;
+#ifdef __cplusplus
+ }
+#endif
+#define GET(x) if((x=(*f__getn)())<0) return(x)
+#define VAL(x) (x!='\n'?x:' ')
+#define PUT(x) (*f__putn)(x)
+
+#undef TYQUAD
+#ifndef Allow_TYQUAD
+#undef longint
+#define longint long
+#else
+#define TYQUAD 14
+#endif
+
+#ifdef KR_headers
+extern char *f__icvt();
+#else
+Cextern char *f__icvt(longint, int*, int*, int);
+#endif
diff --git a/3rdparty/clapack/f2c/fmtlib.c b/3rdparty/clapack/f2c/fmtlib.c
new file mode 100644
index 0000000..279f66f
--- /dev/null
+++ b/3rdparty/clapack/f2c/fmtlib.c
@@ -0,0 +1,51 @@
+/* @(#)fmtlib.c 1.2 */
+#define MAXINTLENGTH 23
+
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#ifndef Allow_TYQUAD
+#undef longint
+#define longint long
+#undef ulongint
+#define ulongint unsigned long
+#endif
+
+#ifdef KR_headers
+char *f__icvt(value,ndigit,sign, base) longint value; int *ndigit,*sign;
+ register int base;
+#else
+char *f__icvt(longint value, int *ndigit, int *sign, int base)
+#endif
+{
+ static char buf[MAXINTLENGTH+1];
+ register int i;
+ ulongint uvalue;
+
+ if(value > 0) {
+ uvalue = value;
+ *sign = 0;
+ }
+ else if (value < 0) {
+ uvalue = -value;
+ *sign = 1;
+ }
+ else {
+ *sign = 0;
+ *ndigit = 1;
+ buf[MAXINTLENGTH-1] = '0';
+ return &buf[MAXINTLENGTH-1];
+ }
+ i = MAXINTLENGTH;
+ do {
+ buf[--i] = (uvalue%base) + '0';
+ uvalue /= base;
+ }
+ while(uvalue > 0);
+ *ndigit = MAXINTLENGTH - i;
+ return &buf[i];
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/fp.h b/3rdparty/clapack/f2c/fp.h
new file mode 100644
index 0000000..40743d7
--- /dev/null
+++ b/3rdparty/clapack/f2c/fp.h
@@ -0,0 +1,28 @@
+#define FMAX 40
+#define EXPMAXDIGS 8
+#define EXPMAX 99999999
+/* FMAX = max number of nonzero digits passed to atof() */
+/* EXPMAX = 10^EXPMAXDIGS - 1 = largest allowed exponent absolute value */
+
+#ifdef V10 /* Research Tenth-Edition Unix */
+#include "local.h"
+#endif
+
+/* MAXFRACDIGS and MAXINTDIGS are for wrt_F -- bounds (not necessarily
+ tight) on the maximum number of digits to the right and left of
+ * the decimal point.
+ */
+
+#ifdef VAX
+#define MAXFRACDIGS 56
+#define MAXINTDIGS 38
+#else
+#ifdef CRAY
+#define MAXFRACDIGS 9880
+#define MAXINTDIGS 9864
+#else
+/* values that suffice for IEEE double */
+#define MAXFRACDIGS 344
+#define MAXINTDIGS 308
+#endif
+#endif
diff --git a/3rdparty/clapack/f2c/i_nint.c b/3rdparty/clapack/f2c/i_nint.c
new file mode 100644
index 0000000..fe9fd68
--- /dev/null
+++ b/3rdparty/clapack/f2c/i_nint.c
@@ -0,0 +1,19 @@
+#include "f2c.h"
+
+#ifdef KR_headers
+double floor();
+integer i_nint(x) real *x;
+#else
+#undef abs
+#include "math.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+integer i_nint(real *x)
+#endif
+{
+return (integer)(*x >= 0 ? floor(*x + .5) : -floor(.5 - *x));
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/lio.h b/3rdparty/clapack/f2c/lio.h
new file mode 100644
index 0000000..f9fd1cd
--- /dev/null
+++ b/3rdparty/clapack/f2c/lio.h
@@ -0,0 +1,74 @@
+/* copy of ftypes from the compiler */
+/* variable types
+ * numeric assumptions:
+ * int < reals < complexes
+ * TYDREAL-TYREAL = TYDCOMPLEX-TYCOMPLEX
+ */
+
+/* 0-10 retain their old (pre LOGICAL*1, etc.) */
+/* values to allow mixing old and new objects. */
+
+#define TYUNKNOWN 0
+#define TYADDR 1
+#define TYSHORT 2
+#define TYLONG 3
+#define TYREAL 4
+#define TYDREAL 5
+#define TYCOMPLEX 6
+#define TYDCOMPLEX 7
+#define TYLOGICAL 8
+#define TYCHAR 9
+#define TYSUBR 10
+#define TYINT1 11
+#define TYLOGICAL1 12
+#define TYLOGICAL2 13
+#ifdef Allow_TYQUAD
+#undef TYQUAD
+#define TYQUAD 14
+#endif
+
+#define LINTW 24
+#define LINE 80
+#define LLOGW 2
+#ifdef Old_list_output
+#define LLOW 1.0
+#define LHIGH 1.e9
+#define LEFMT " %# .8E"
+#define LFFMT " %# .9g"
+#else
+#define LGFMT "%.9G"
+#endif
+/* LEFBL 20 should suffice; 24 overcomes a NeXT bug. */
+#define LEFBL 24
+
+typedef union
+{
+ char flchar;
+ short flshort;
+ ftnint flint;
+#ifdef Allow_TYQUAD
+ longint fllongint;
+#endif
+ real flreal;
+ doublereal fldouble;
+} flex;
+#ifdef KR_headers
+extern int (*f__lioproc)(), (*l_getc)(), (*l_ungetc)();
+extern int l_read(), l_write();
+#else
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int (*f__lioproc)(ftnint*, char*, ftnlen, ftnint);
+extern int l_write(ftnint*, char*, ftnlen, ftnint);
+extern void x_wsne(cilist*);
+extern int c_le(cilist*), (*l_getc)(void), (*l_ungetc)(int,FILE*);
+extern int l_read(ftnint*,char*,ftnlen,ftnint);
+extern integer e_rsle(void), e_wsle(void), s_wsne(cilist*);
+extern int z_rnew(void);
+#endif
+extern ftnint L_len;
+extern int f__scale;
+#ifdef __cplusplus
+ }
+#endif
diff --git a/3rdparty/clapack/f2c/lwrite.c b/3rdparty/clapack/f2c/lwrite.c
new file mode 100644
index 0000000..9e0d93d
--- /dev/null
+++ b/3rdparty/clapack/f2c/lwrite.c
@@ -0,0 +1,314 @@
+#include "f2c.h"
+#include "fio.h"
+#include "fmt.h"
+#include "lio.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ftnint L_len;
+int f__Aquote;
+
+ static VOID
+donewrec(Void)
+{
+ if (f__recpos)
+ (*f__donewrec)();
+ }
+
+ static VOID
+#ifdef KR_headers
+lwrt_I(n) longint n;
+#else
+lwrt_I(longint n)
+#endif
+{
+ char *p;
+ int ndigit, sign;
+
+ p = f__icvt(n, &ndigit, &sign, 10);
+ if(f__recpos + ndigit >= L_len)
+ donewrec();
+ PUT(' ');
+ if (sign)
+ PUT('-');
+ while(*p)
+ PUT(*p++);
+}
+ static VOID
+#ifdef KR_headers
+lwrt_L(n, len) ftnint n; ftnlen len;
+#else
+lwrt_L(ftnint n, ftnlen len)
+#endif
+{
+ if(f__recpos+LLOGW>=L_len)
+ donewrec();
+ wrt_L((Uint *)&n,LLOGW, len);
+}
+ static VOID
+#ifdef KR_headers
+lwrt_A(p,len) char *p; ftnlen len;
+#else
+lwrt_A(char *p, ftnlen len)
+#endif
+{
+ int a;
+ char *p1, *pe;
+
+ a = 0;
+ pe = p + len;
+ if (f__Aquote) {
+ a = 3;
+ if (len > 1 && p[len-1] == ' ') {
+ while(--len > 1 && p[len-1] == ' ');
+ pe = p + len;
+ }
+ p1 = p;
+ while(p1 < pe)
+ if (*p1++ == '\'')
+ a++;
+ }
+ if(f__recpos+len+a >= L_len)
+ donewrec();
+ if (a
+#ifndef OMIT_BLANK_CC
+ || !f__recpos
+#endif
+ )
+ PUT(' ');
+ if (a) {
+ PUT('\'');
+ while(p < pe) {
+ if (*p == '\'')
+ PUT('\'');
+ PUT(*p++);
+ }
+ PUT('\'');
+ }
+ else
+ while(p < pe)
+ PUT(*p++);
+}
+
+ static int
+#ifdef KR_headers
+l_g(buf, n) char *buf; double n;
+#else
+l_g(char *buf, double n)
+#endif
+{
+#ifdef Old_list_output
+ doublereal absn;
+ char *fmt;
+
+ absn = n;
+ if (absn < 0)
+ absn = -absn;
+ fmt = LLOW <= absn && absn < LHIGH ? LFFMT : LEFMT;
+#ifdef USE_STRLEN
+ sprintf(buf, fmt, n);
+ return strlen(buf);
+#else
+ return sprintf(buf, fmt, n);
+#endif
+
+#else
+ register char *b, c, c1;
+
+ b = buf;
+ *b++ = ' ';
+ if (n < 0) {
+ *b++ = '-';
+ n = -n;
+ }
+ else
+ *b++ = ' ';
+ if (n == 0) {
+#ifdef SIGNED_ZEROS
+ if (signbit_f2c(&n))
+ *b++ = '-';
+#endif
+ *b++ = '0';
+ *b++ = '.';
+ *b = 0;
+ goto f__ret;
+ }
+ sprintf(b, LGFMT, n);
+ switch(*b) {
+#ifndef WANT_LEAD_0
+ case '0':
+ while(b[0] = b[1])
+ b++;
+ break;
+#endif
+ case 'i':
+ case 'I':
+ /* Infinity */
+ case 'n':
+ case 'N':
+ /* NaN */
+ while(*++b);
+ break;
+
+ default:
+ /* Fortran 77 insists on having a decimal point... */
+ for(;; b++)
+ switch(*b) {
+ case 0:
+ *b++ = '.';
+ *b = 0;
+ goto f__ret;
+ case '.':
+ while(*++b);
+ goto f__ret;
+ case 'E':
+ for(c1 = '.', c = 'E'; *b = c1;
+ c1 = c, c = *++b);
+ goto f__ret;
+ }
+ }
+ f__ret:
+ return b - buf;
+#endif
+ }
+
+ static VOID
+#ifdef KR_headers
+l_put(s) register char *s;
+#else
+l_put(register char *s)
+#endif
+{
+#ifdef KR_headers
+ register void (*pn)() = f__putn;
+#else
+ register void (*pn)(int) = f__putn;
+#endif
+ register int c;
+
+ while(c = *s++)
+ (*pn)(c);
+ }
+
+ static VOID
+#ifdef KR_headers
+lwrt_F(n) double n;
+#else
+lwrt_F(double n)
+#endif
+{
+ char buf[LEFBL];
+
+ if(f__recpos + l_g(buf,n) >= L_len)
+ donewrec();
+ l_put(buf);
+}
+ static VOID
+#ifdef KR_headers
+lwrt_C(a,b) double a,b;
+#else
+lwrt_C(double a, double b)
+#endif
+{
+ char *ba, *bb, bufa[LEFBL], bufb[LEFBL];
+ int al, bl;
+
+ al = l_g(bufa, a);
+ for(ba = bufa; *ba == ' '; ba++)
+ --al;
+ bl = l_g(bufb, b) + 1; /* intentionally high by 1 */
+ for(bb = bufb; *bb == ' '; bb++)
+ --bl;
+ if(f__recpos + al + bl + 3 >= L_len)
+ donewrec();
+#ifdef OMIT_BLANK_CC
+ else
+#endif
+ PUT(' ');
+ PUT('(');
+ l_put(ba);
+ PUT(',');
+ if (f__recpos + bl >= L_len) {
+ (*f__donewrec)();
+#ifndef OMIT_BLANK_CC
+ PUT(' ');
+#endif
+ }
+ l_put(bb);
+ PUT(')');
+}
+
+ int
+#ifdef KR_headers
+l_write(number,ptr,len,type) ftnint *number,type; char *ptr; ftnlen len;
+#else
+l_write(ftnint *number, char *ptr, ftnlen len, ftnint type)
+#endif
+{
+#define Ptr ((flex *)ptr)
+ int i;
+ longint x;
+ double y,z;
+ real *xx;
+ doublereal *yy;
+ for(i=0;i< *number; i++)
+ {
+ switch((int)type)
+ {
+ default: f__fatal(117,"unknown type in lio");
+ case TYINT1:
+ x = Ptr->flchar;
+ goto xint;
+ case TYSHORT:
+ x=Ptr->flshort;
+ goto xint;
+#ifdef Allow_TYQUAD
+ case TYQUAD:
+ x = Ptr->fllongint;
+ goto xint;
+#endif
+ case TYLONG:
+ x=Ptr->flint;
+ xint: lwrt_I(x);
+ break;
+ case TYREAL:
+ y=Ptr->flreal;
+ goto xfloat;
+ case TYDREAL:
+ y=Ptr->fldouble;
+ xfloat: lwrt_F(y);
+ break;
+ case TYCOMPLEX:
+ xx= &Ptr->flreal;
+ y = *xx++;
+ z = *xx;
+ goto xcomplex;
+ case TYDCOMPLEX:
+ yy = &Ptr->fldouble;
+ y= *yy++;
+ z = *yy;
+ xcomplex:
+ lwrt_C(y,z);
+ break;
+ case TYLOGICAL1:
+ x = Ptr->flchar;
+ goto xlog;
+ case TYLOGICAL2:
+ x = Ptr->flshort;
+ goto xlog;
+ case TYLOGICAL:
+ x = Ptr->flint;
+ xlog: lwrt_L(Ptr->flint, len);
+ break;
+ case TYCHAR:
+ lwrt_A(ptr,len);
+ break;
+ }
+ ptr += len;
+ }
+ return(0);
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/open.c b/3rdparty/clapack/f2c/open.c
new file mode 100644
index 0000000..6ecc7cb
--- /dev/null
+++ b/3rdparty/clapack/f2c/open.c
@@ -0,0 +1,311 @@
+#include "f2c.h"
+#include "fio.h"
+#include "string.h"
+#ifndef NON_POSIX_STDIO
+#ifdef MSDOS
+#include "io.h"
+#else
+#include "unistd.h" /* for access */
+#endif
+#endif
+
+#ifdef KR_headers
+extern char *malloc();
+#ifdef NON_ANSI_STDIO
+extern char *mktemp();
+#endif
+extern integer f_clos();
+#define Const /*nothing*/
+#else
+#define Const const
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern int f__canseek(FILE*);
+extern integer f_clos(cllist*);
+#endif
+
+#ifdef NON_ANSI_RW_MODES
+Const char *f__r_mode[2] = {"r", "r"};
+Const char *f__w_mode[4] = {"w", "w", "r+w", "r+w"};
+#else
+Const char *f__r_mode[2] = {"rb", "r"};
+Const char *f__w_mode[4] = {"wb", "w", "r+b", "r+"};
+#endif
+
+ static char f__buf0[400], *f__buf = f__buf0;
+ int f__buflen = (int)sizeof(f__buf0);
+
+ static void
+#ifdef KR_headers
+f__bufadj(n, c) int n, c;
+#else
+f__bufadj(int n, int c)
+#endif
+{
+ unsigned int len;
+ char *nbuf, *s, *t, *te;
+
+ if (f__buf == f__buf0)
+ f__buflen = 1024;
+ while(f__buflen <= n)
+ f__buflen <<= 1;
+ len = (unsigned int)f__buflen;
+ if (len != f__buflen || !(nbuf = (char*)malloc(len)))
+ f__fatal(113, "malloc failure");
+ else {
+ s = nbuf;
+ t = f__buf;
+ te = t + c;
+ while (t < te)
+ *s++ = *t++;
+ if (f__buf != f__buf0)
+ free(f__buf);
+ f__buf = nbuf;
+ }
+ }
+
+ int
+#ifdef KR_headers
+f__putbuf(c) int c;
+#else
+f__putbuf(int c)
+#endif
+{
+ char *s, *se;
+ int n;
+
+ if (f__hiwater > f__recpos)
+ f__recpos = f__hiwater;
+ n = f__recpos + 1;
+ if (n >= f__buflen)
+ f__bufadj(n, f__recpos);
+ s = f__buf;
+ se = s + f__recpos;
+ if (c)
+ *se++ = c;
+ *se = 0;
+ for(;;) {
+ fputs(s, f__cf);
+ s += strlen(s);
+ if (s >= se)
+ break; /* normally happens the first time */
+ putc(*s++, f__cf);
+ }
+ return 0;
+ }
+
+ void
+#ifdef KR_headers
+x_putc(c)
+#else
+x_putc(int c)
+#endif
+{
+ if (f__recpos >= f__buflen)
+ f__bufadj(f__recpos, f__buflen);
+ f__buf[f__recpos++] = c;
+ }
+
+#define opnerr(f,m,s) {if(f) errno= m; else opn_err(m,s,a); return(m);}
+
+ static void
+#ifdef KR_headers
+opn_err(m, s, a) int m; char *s; olist *a;
+#else
+opn_err(int m, const char *s, olist *a)
+#endif
+{
+ if (a->ofnm) {
+ /* supply file name to error message */
+ if (a->ofnmlen >= f__buflen)
+ f__bufadj((int)a->ofnmlen, 0);
+ g_char(a->ofnm, a->ofnmlen, f__curunit->ufnm = f__buf);
+ }
+ f__fatal(m, s);
+ }
+
+#ifdef KR_headers
+integer f_open(a) olist *a;
+#else
+integer f_open(olist *a)
+#endif
+{ unit *b;
+ integer rv;
+ char buf[256], *s;
+ cllist x;
+ int ufmt;
+ FILE *tf;
+#ifndef NON_UNIX_STDIO
+ int n;
+#endif
+ f__external = 1;
+ if(a->ounit>=MXUNIT || a->ounit<0)
+ err(a->oerr,101,"open")
+ if (!f__init)
+ f_init();
+ f__curunit = b = &f__units[a->ounit];
+ if(b->ufd) {
+ if(a->ofnm==0)
+ {
+ same: if (a->oblnk)
+ b->ublnk = *a->oblnk == 'z' || *a->oblnk == 'Z';
+ return(0);
+ }
+#ifdef NON_UNIX_STDIO
+ if (b->ufnm
+ && strlen(b->ufnm) == a->ofnmlen
+ && !strncmp(b->ufnm, a->ofnm, (unsigned)a->ofnmlen))
+ goto same;
+#else
+ g_char(a->ofnm,a->ofnmlen,buf);
+ if (f__inode(buf,&n) == b->uinode && n == b->udev)
+ goto same;
+#endif
+ x.cunit=a->ounit;
+ x.csta=0;
+ x.cerr=a->oerr;
+ if ((rv = f_clos(&x)) != 0)
+ return rv;
+ }
+ b->url = (int)a->orl;
+ b->ublnk = a->oblnk && (*a->oblnk == 'z' || *a->oblnk == 'Z');
+ if(a->ofm==0)
+ { if(b->url>0) b->ufmt=0;
+ else b->ufmt=1;
+ }
+ else if(*a->ofm=='f' || *a->ofm == 'F') b->ufmt=1;
+ else b->ufmt=0;
+ ufmt = b->ufmt;
+#ifdef url_Adjust
+ if (b->url && !ufmt)
+ url_Adjust(b->url);
+#endif
+ if (a->ofnm) {
+ g_char(a->ofnm,a->ofnmlen,buf);
+ if (!buf[0])
+ opnerr(a->oerr,107,"open")
+ }
+ else
+ sprintf(buf, "fort.%ld", (long)a->ounit);
+ b->uscrtch = 0;
+ b->uend=0;
+ b->uwrt = 0;
+ b->ufd = 0;
+ b->urw = 3;
+ switch(a->osta ? *a->osta : 'u')
+ {
+ case 'o':
+ case 'O':
+#ifdef NON_POSIX_STDIO
+ if (!(tf = FOPEN(buf,"r")))
+ opnerr(a->oerr,errno,"open")
+ fclose(tf);
+#else
+# ifdef _WIN32
+ if (_access(buf,0))
+#else
+ if (access(buf, 0))
+#endif
+ opnerr(a->oerr,errno,"open")
+#endif
+ break;
+ case 's':
+ case 'S':
+ b->uscrtch=1;
+#ifdef NON_ANSI_STDIO
+ (void) strcpy(buf,"tmp.FXXXXXX");
+ (void) mktemp(buf);
+ goto replace;
+#else
+ if (!(b->ufd = tmpfile()))
+ opnerr(a->oerr,errno,"open")
+ b->ufnm = 0;
+#ifndef NON_UNIX_STDIO
+ b->uinode = b->udev = -1;
+#endif
+ b->useek = 1;
+ return 0;
+#endif
+
+ case 'n':
+ case 'N':
+#ifdef NON_POSIX_STDIO
+ if ((tf = FOPEN(buf,"r")) || (tf = FOPEN(buf,"a"))) {
+ fclose(tf);
+ opnerr(a->oerr,128,"open")
+ }
+#else
+#ifdef _WIN32
+ if (!_access(buf, 0))
+#else
+ if (!access(buf,0))
+#endif
+ opnerr(a->oerr,128,"open")
+#endif
+ /* no break */
+ case 'r': /* Fortran 90 replace option */
+ case 'R':
+#ifdef NON_ANSI_STDIO
+ replace:
+#endif
+ if (tf = FOPEN(buf,f__w_mode[0]))
+ fclose(tf);
+ }
+
+ b->ufnm=(char *) malloc((unsigned int)(strlen(buf)+1));
+ if(b->ufnm==NULL) opnerr(a->oerr,113,"no space");
+ (void) strcpy(b->ufnm,buf);
+ if ((s = a->oacc) && b->url)
+ ufmt = 0;
+ if(!(tf = FOPEN(buf, f__w_mode[ufmt|2]))) {
+ if (tf = FOPEN(buf, f__r_mode[ufmt]))
+ b->urw = 1;
+ else if (tf = FOPEN(buf, f__w_mode[ufmt])) {
+ b->uwrt = 1;
+ b->urw = 2;
+ }
+ else
+ err(a->oerr, errno, "open");
+ }
+ b->useek = f__canseek(b->ufd = tf);
+#ifndef NON_UNIX_STDIO
+ if((b->uinode = f__inode(buf,&b->udev)) == -1)
+ opnerr(a->oerr,108,"open")
+#endif
+ if(b->useek)
+ if (a->orl)
+ rewind(b->ufd);
+ else if ((s = a->oacc) && (*s == 'a' || *s == 'A')
+ && FSEEK(b->ufd, 0L, SEEK_END))
+ opnerr(a->oerr,129,"open");
+ return(0);
+}
+
+ int
+#ifdef KR_headers
+fk_open(seq,fmt,n) ftnint n;
+#else
+fk_open(int seq, int fmt, ftnint n)
+#endif
+{ char nbuf[10];
+ olist a;
+ (void) sprintf(nbuf,"fort.%ld",(long)n);
+ a.oerr=1;
+ a.ounit=n;
+ a.ofnm=nbuf;
+ a.ofnmlen=strlen(nbuf);
+ a.osta=NULL;
+ a.oacc= (char*)(seq==SEQ?"s":"d");
+ a.ofm = (char*)(fmt==FMT?"f":"u");
+ a.orl = seq==DIR?1:0;
+ a.oblnk=NULL;
+ return(f_open(&a));
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/pow_dd.c b/3rdparty/clapack/f2c/pow_dd.c
new file mode 100644
index 0000000..08fc208
--- /dev/null
+++ b/3rdparty/clapack/f2c/pow_dd.c
@@ -0,0 +1,19 @@
+#include "f2c.h"
+
+#ifdef KR_headers
+double pow();
+double pow_dd(ap, bp) doublereal *ap, *bp;
+#else
+#undef abs
+#include "math.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+double pow_dd(doublereal *ap, doublereal *bp)
+#endif
+{
+return(pow(*ap, *bp) );
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/pow_di.c b/3rdparty/clapack/f2c/pow_di.c
new file mode 100644
index 0000000..abf36cb
--- /dev/null
+++ b/3rdparty/clapack/f2c/pow_di.c
@@ -0,0 +1,41 @@
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef KR_headers
+double pow_di(ap, bp) doublereal *ap; integer *bp;
+#else
+double pow_di(doublereal *ap, integer *bp)
+#endif
+{
+double pow, x;
+integer n;
+unsigned long u;
+
+pow = 1;
+x = *ap;
+n = *bp;
+
+if(n != 0)
+ {
+ if(n < 0)
+ {
+ n = -n;
+ x = 1/x;
+ }
+ for(u = n; ; )
+ {
+ if(u & 01)
+ pow *= x;
+ if(u >>= 1)
+ x *= x;
+ else
+ break;
+ }
+ }
+return(pow);
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/pow_ii.c b/3rdparty/clapack/f2c/pow_ii.c
new file mode 100644
index 0000000..748d121
--- /dev/null
+++ b/3rdparty/clapack/f2c/pow_ii.c
@@ -0,0 +1,39 @@
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef KR_headers
+integer pow_ii(ap, bp) integer *ap, *bp;
+#else
+integer pow_ii(integer *ap, integer *bp)
+#endif
+{
+ integer pow, x, n;
+ unsigned long u;
+
+ x = *ap;
+ n = *bp;
+
+ if (n <= 0) {
+ if (n == 0 || x == 1)
+ return 1;
+ if (x != -1)
+ return x == 0 ? 1/x : 0;
+ n = -n;
+ }
+ u = n;
+ for(pow = 1; ; )
+ {
+ if(u & 01)
+ pow *= x;
+ if(u >>= 1)
+ x *= x;
+ else
+ break;
+ }
+ return(pow);
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/s_cat.c b/3rdparty/clapack/f2c/s_cat.c
new file mode 100644
index 0000000..8d92a63
--- /dev/null
+++ b/3rdparty/clapack/f2c/s_cat.c
@@ -0,0 +1,86 @@
+/* Unless compiled with -DNO_OVERWRITE, this variant of s_cat allows the
+ * target of a concatenation to appear on its right-hand side (contrary
+ * to the Fortran 77 Standard, but in accordance with Fortran 90).
+ */
+
+#include "f2c.h"
+#ifndef NO_OVERWRITE
+#include "stdio.h"
+#undef abs
+#ifdef KR_headers
+ extern char *F77_aloc();
+ extern void free();
+ extern void exit_();
+#else
+#undef min
+#undef max
+#include "stdlib.h"
+extern
+#ifdef __cplusplus
+ "C"
+#endif
+ char *F77_aloc(ftnlen, const char*);
+#endif
+#include "string.h"
+#endif /* NO_OVERWRITE */
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ VOID
+#ifdef KR_headers
+s_cat(lp, rpp, rnp, np, ll) char *lp, *rpp[]; ftnint rnp[], *np; ftnlen ll;
+#else
+s_cat(char *lp, char *rpp[], ftnint rnp[], ftnint *np, ftnlen ll)
+#endif
+{
+ ftnlen i, nc;
+ char *rp;
+ ftnlen n = *np;
+#ifndef NO_OVERWRITE
+ ftnlen L, m;
+ char *lp0, *lp1;
+
+ lp0 = 0;
+ lp1 = lp;
+ L = ll;
+ i = 0;
+ while(i < n) {
+ rp = rpp[i];
+ m = rnp[i++];
+ if (rp >= lp1 || rp + m <= lp) {
+ if ((L -= m) <= 0) {
+ n = i;
+ break;
+ }
+ lp1 += m;
+ continue;
+ }
+ lp0 = lp;
+ lp = lp1 = F77_aloc(L = ll, "s_cat");
+ break;
+ }
+ lp1 = lp;
+#endif /* NO_OVERWRITE */
+ for(i = 0 ; i < n ; ++i) {
+ nc = ll;
+ if(rnp[i] < nc)
+ nc = rnp[i];
+ ll -= nc;
+ rp = rpp[i];
+ while(--nc >= 0)
+ *lp++ = *rp++;
+ }
+ while(--ll >= 0)
+ *lp++ = ' ';
+#ifndef NO_OVERWRITE
+ if (lp0) {
+ memcpy(lp0, lp1, L);
+ free(lp1);
+ }
+#endif
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/s_cmp.c b/3rdparty/clapack/f2c/s_cmp.c
new file mode 100644
index 0000000..3a2ea67
--- /dev/null
+++ b/3rdparty/clapack/f2c/s_cmp.c
@@ -0,0 +1,50 @@
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* compare two strings */
+
+#ifdef KR_headers
+integer s_cmp(a0, b0, la, lb) char *a0, *b0; ftnlen la, lb;
+#else
+integer s_cmp(char *a0, char *b0, ftnlen la, ftnlen lb)
+#endif
+{
+register unsigned char *a, *aend, *b, *bend;
+a = (unsigned char *)a0;
+b = (unsigned char *)b0;
+aend = a + la;
+bend = b + lb;
+
+if(la <= lb)
+ {
+ while(a < aend)
+ if(*a != *b)
+ return( *a - *b );
+ else
+ { ++a; ++b; }
+
+ while(b < bend)
+ if(*b != ' ')
+ return( ' ' - *b );
+ else ++b;
+ }
+
+else
+ {
+ while(b < bend)
+ if(*a == *b)
+ { ++a; ++b; }
+ else
+ return( *a - *b );
+ while(a < aend)
+ if(*a != ' ')
+ return(*a - ' ');
+ else ++a;
+ }
+return(0);
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/s_copy.c b/3rdparty/clapack/f2c/s_copy.c
new file mode 100644
index 0000000..9dacfc7
--- /dev/null
+++ b/3rdparty/clapack/f2c/s_copy.c
@@ -0,0 +1,57 @@
+/* Unless compiled with -DNO_OVERWRITE, this variant of s_copy allows the
+ * target of an assignment to appear on its right-hand side (contrary
+ * to the Fortran 77 Standard, but in accordance with Fortran 90),
+ * as in a(2:5) = a(4:7) .
+ */
+
+#include "f2c.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* assign strings: a = b */
+
+#ifdef KR_headers
+VOID s_copy(a, b, la, lb) register char *a, *b; ftnlen la, lb;
+#else
+void s_copy(register char *a, register char *b, ftnlen la, ftnlen lb)
+#endif
+{
+ register char *aend, *bend;
+
+ aend = a + la;
+
+ if(la <= lb)
+#ifndef NO_OVERWRITE
+ if (a <= b || a >= b + la)
+#endif
+ while(a < aend)
+ *a++ = *b++;
+#ifndef NO_OVERWRITE
+ else
+ for(b += la; a < aend; )
+ *--aend = *--b;
+#endif
+
+ else {
+ bend = b + lb;
+#ifndef NO_OVERWRITE
+ if (a <= b || a >= bend)
+#endif
+ while(b < bend)
+ *a++ = *b++;
+#ifndef NO_OVERWRITE
+ else {
+ a += lb;
+ while(b < bend)
+ *--a = *--bend;
+ a += lb;
+ }
+#endif
+ while(a < aend)
+ *a++ = ' ';
+ }
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/s_stop.c b/3rdparty/clapack/f2c/s_stop.c
new file mode 100644
index 0000000..68233ae
--- /dev/null
+++ b/3rdparty/clapack/f2c/s_stop.c
@@ -0,0 +1,48 @@
+#include "stdio.h"
+#include "f2c.h"
+
+#ifdef KR_headers
+extern void f_exit();
+int s_stop(s, n) char *s; ftnlen n;
+#else
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#ifdef __cplusplus
+extern "C" {
+#endif
+void f_exit(void);
+
+int s_stop(char *s, ftnlen n)
+#endif
+{
+int i;
+
+if(n > 0)
+ {
+ fprintf(stderr, "STOP ");
+ for(i = 0; i<n ; ++i)
+ putc(*s++, stderr);
+ fprintf(stderr, " statement executed\n");
+ }
+#ifdef NO_ONEXIT
+f_exit();
+#endif
+exit(0);
+
+/* We cannot avoid (useless) compiler diagnostics here: */
+/* some compilers complain if there is no return statement, */
+/* and others complain that this one cannot be reached. */
+
+return 0; /* NOT REACHED */
+}
+#ifdef __cplusplus
+}
+#endif
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/sfe.c b/3rdparty/clapack/f2c/sfe.c
new file mode 100644
index 0000000..d24af6d
--- /dev/null
+++ b/3rdparty/clapack/f2c/sfe.c
@@ -0,0 +1,47 @@
+/* sequential formatted external common routines*/
+#include "f2c.h"
+#include "fio.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef KR_headers
+extern char *f__fmtbuf;
+#else
+extern const char *f__fmtbuf;
+#endif
+
+integer e_rsfe(Void)
+{ int n;
+ n=en_fio();
+ f__fmtbuf=NULL;
+ return(n);
+}
+
+ int
+#ifdef KR_headers
+c_sfe(a) cilist *a; /* check */
+#else
+c_sfe(cilist *a) /* check */
+#endif
+{ unit *p;
+ f__curunit = p = &f__units[a->ciunit];
+ if(a->ciunit >= MXUNIT || a->ciunit<0)
+ err(a->cierr,101,"startio");
+ if(p->ufd==NULL && fk_open(SEQ,FMT,a->ciunit)) err(a->cierr,114,"sfe")
+ if(!p->ufmt) err(a->cierr,102,"sfe")
+ return(0);
+}
+integer e_wsfe(Void)
+{
+ int n = en_fio();
+ f__fmtbuf = NULL;
+#ifdef ALWAYS_FLUSH
+ if (!n && fflush(f__cf))
+ err(f__elist->cierr, errno, "write end");
+#endif
+ return n;
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/sig_die.c b/3rdparty/clapack/f2c/sig_die.c
new file mode 100644
index 0000000..63a73d9
--- /dev/null
+++ b/3rdparty/clapack/f2c/sig_die.c
@@ -0,0 +1,51 @@
+#include "stdio.h"
+#include "signal.h"
+
+#ifndef SIGIOT
+#ifdef SIGABRT
+#define SIGIOT SIGABRT
+#endif
+#endif
+
+#ifdef KR_headers
+void sig_die(s, kill) char *s; int kill;
+#else
+#include "stdlib.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#ifdef __cplusplus
+extern "C" {
+#endif
+ extern void f_exit(void);
+
+void sig_die(const char *s, int kill)
+#endif
+{
+ /* print error message, then clear buffers */
+ fprintf(stderr, "%s\n", s);
+
+ if(kill)
+ {
+ fflush(stderr);
+ f_exit();
+ fflush(stderr);
+ /* now get a core */
+#ifdef SIGIOT
+ signal(SIGIOT, SIG_DFL);
+#endif
+ abort();
+ }
+ else {
+#ifdef NO_ONEXIT
+ f_exit();
+#endif
+ exit(1);
+ }
+ }
+#ifdef __cplusplus
+}
+#endif
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/sysdep1.h b/3rdparty/clapack/f2c/sysdep1.h
new file mode 100644
index 0000000..4c026a2
--- /dev/null
+++ b/3rdparty/clapack/f2c/sysdep1.h
@@ -0,0 +1,66 @@
+#ifndef SYSDEP_H_INCLUDED
+#define SYSDEP_H_INCLUDED
+#undef USE_LARGEFILE
+#ifndef NO_LONG_LONG
+
+#ifdef __sun__
+#define USE_LARGEFILE
+#define OFF_T off64_t
+#endif
+
+#ifdef __linux__
+#define USE_LARGEFILE
+#define OFF_T __off64_t
+#endif
+
+#ifdef _AIX43
+#define _LARGE_FILES
+#define _LARGE_FILE_API
+#define USE_LARGEFILE
+#endif /*_AIX43*/
+
+#ifdef __hpux
+#define _FILE64
+#define _LARGEFILE64_SOURCE
+#define USE_LARGEFILE
+#endif /*__hpux*/
+
+#ifdef __sgi
+#define USE_LARGEFILE
+#endif /*__sgi*/
+
+#ifdef __FreeBSD__
+#define OFF_T off_t
+#define FSEEK fseeko
+#define FTELL ftello
+#endif
+
+#ifdef USE_LARGEFILE
+#ifndef OFF_T
+#define OFF_T off64_t
+#endif
+#define _LARGEFILE_SOURCE
+#define _LARGEFILE64_SOURCE
+#include <sys/types.h>
+#include <sys/stat.h>
+#define FOPEN fopen64
+#define FREOPEN freopen64
+#define FSEEK fseeko64
+#define FSTAT fstat64
+#define FTELL ftello64
+#define FTRUNCATE ftruncate64
+#define STAT stat64
+#define STAT_ST stat64
+#endif /*USE_LARGEFILE*/
+#endif /*NO_LONG_LONG*/
+
+#ifndef NON_UNIX_STDIO
+#ifndef USE_LARGEFILE
+#define _INCLUDE_POSIX_SOURCE /* for HP-UX */
+#define _INCLUDE_XOPEN_SOURCE /* for HP-UX */
+#include "sys/types.h"
+#include "sys/stat.h"
+#endif
+#endif
+
+#endif /*SYSDEP_H_INCLUDED*/
diff --git a/3rdparty/clapack/f2c/util.c b/3rdparty/clapack/f2c/util.c
new file mode 100644
index 0000000..ad4bec5
--- /dev/null
+++ b/3rdparty/clapack/f2c/util.c
@@ -0,0 +1,57 @@
+#include "sysdep1.h" /* here to get stat64 on some badly designed Linux systems */
+#include "f2c.h"
+#include "fio.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ VOID
+#ifdef KR_headers
+#define Const /*nothing*/
+g_char(a,alen,b) char *a,*b; ftnlen alen;
+#else
+#define Const const
+g_char(const char *a, ftnlen alen, char *b)
+#endif
+{
+ Const char *x = a + alen;
+ char *y = b + alen;
+
+ for(;; y--) {
+ if (x <= a) {
+ *b = 0;
+ return;
+ }
+ if (*--x != ' ')
+ break;
+ }
+ *y-- = 0;
+ do *y-- = *x;
+ while(x-- > a);
+ }
+
+ VOID
+#ifdef KR_headers
+b_char(a,b,blen) char *a,*b; ftnlen blen;
+#else
+b_char(const char *a, char *b, ftnlen blen)
+#endif
+{ int i;
+ for(i=0;i<blen && *a!=0;i++) *b++= *a++;
+ for(;i<blen;i++) *b++=' ';
+}
+#ifndef NON_UNIX_STDIO
+#ifdef KR_headers
+long f__inode(a, dev) char *a; int *dev;
+#else
+long f__inode(char *a, int *dev)
+#endif
+{ struct STAT_ST x;
+ if(STAT(a,&x)<0) return(-1);
+ *dev = x.st_dev;
+ return(x.st_ino);
+}
+#endif
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/wref.c b/3rdparty/clapack/f2c/wref.c
new file mode 100644
index 0000000..f2074b7
--- /dev/null
+++ b/3rdparty/clapack/f2c/wref.c
@@ -0,0 +1,294 @@
+#include "f2c.h"
+#include "fio.h"
+
+#ifndef KR_headers
+#undef abs
+#undef min
+#undef max
+#include "stdlib.h"
+#include "string.h"
+#endif
+
+#include "fmt.h"
+#include "fp.h"
+#ifndef VAX
+#include "ctype.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#endif
+
+ int
+#ifdef KR_headers
+wrt_E(p,w,d,e,len) ufloat *p; ftnlen len;
+#else
+wrt_E(ufloat *p, int w, int d, int e, ftnlen len)
+#endif
+{
+ char buf[FMAX+EXPMAXDIGS+4], *s, *se;
+ int d1, delta, e1, i, sign, signspace;
+ double dd;
+#ifdef WANT_LEAD_0
+ int insert0 = 0;
+#endif
+#ifndef VAX
+ int e0 = e;
+#endif
+
+ if(e <= 0)
+ e = 2;
+ if(f__scale) {
+ if(f__scale >= d + 2 || f__scale <= -d)
+ goto nogood;
+ }
+ if(f__scale <= 0)
+ --d;
+ if (len == sizeof(real))
+ dd = p->pf;
+ else
+ dd = p->pd;
+ if (dd < 0.) {
+ signspace = sign = 1;
+ dd = -dd;
+ }
+ else {
+ sign = 0;
+ signspace = (int)f__cplus;
+#ifndef VAX
+ if (!dd) {
+#ifdef SIGNED_ZEROS
+ if (signbit_f2c(&dd))
+ signspace = sign = 1;
+#endif
+ dd = 0.; /* avoid -0 */
+ }
+#endif
+ }
+ delta = w - (2 /* for the . and the d adjustment above */
+ + 2 /* for the E+ */ + signspace + d + e);
+#ifdef WANT_LEAD_0
+ if (f__scale <= 0 && delta > 0) {
+ delta--;
+ insert0 = 1;
+ }
+ else
+#endif
+ if (delta < 0) {
+nogood:
+ while(--w >= 0)
+ PUT('*');
+ return(0);
+ }
+ if (f__scale < 0)
+ d += f__scale;
+ if (d > FMAX) {
+ d1 = d - FMAX;
+ d = FMAX;
+ }
+ else
+ d1 = 0;
+ sprintf(buf,"%#.*E", d, dd);
+#ifndef VAX
+ /* check for NaN, Infinity */
+ if (!isdigit(buf[0])) {
+ switch(buf[0]) {
+ case 'n':
+ case 'N':
+ signspace = 0; /* no sign for NaNs */
+ }
+ delta = w - strlen(buf) - signspace;
+ if (delta < 0)
+ goto nogood;
+ while(--delta >= 0)
+ PUT(' ');
+ if (signspace)
+ PUT(sign ? '-' : '+');
+ for(s = buf; *s; s++)
+ PUT(*s);
+ return 0;
+ }
+#endif
+ se = buf + d + 3;
+#ifdef GOOD_SPRINTF_EXPONENT /* When possible, exponent has 2 digits. */
+ if (f__scale != 1 && dd)
+ sprintf(se, "%+.2d", atoi(se) + 1 - f__scale);
+#else
+ if (dd)
+ sprintf(se, "%+.2d", atoi(se) + 1 - f__scale);
+ else
+ strcpy(se, "+00");
+#endif
+ s = ++se;
+ if (e < 2) {
+ if (*s != '0')
+ goto nogood;
+ }
+#ifndef VAX
+ /* accommodate 3 significant digits in exponent */
+ if (s[2]) {
+#ifdef Pedantic
+ if (!e0 && !s[3])
+ for(s -= 2, e1 = 2; s[0] = s[1]; s++);
+
+ /* Pedantic gives the behavior that Fortran 77 specifies, */
+ /* i.e., requires that E be specified for exponent fields */
+ /* of more than 3 digits. With Pedantic undefined, we get */
+ /* the behavior that Cray displays -- you get a bigger */
+ /* exponent field if it fits. */
+#else
+ if (!e0) {
+ for(s -= 2, e1 = 2; s[0] = s[1]; s++)
+#ifdef CRAY
+ delta--;
+ if ((delta += 4) < 0)
+ goto nogood
+#endif
+ ;
+ }
+#endif
+ else if (e0 >= 0)
+ goto shift;
+ else
+ e1 = e;
+ }
+ else
+ shift:
+#endif
+ for(s += 2, e1 = 2; *s; ++e1, ++s)
+ if (e1 >= e)
+ goto nogood;
+ while(--delta >= 0)
+ PUT(' ');
+ if (signspace)
+ PUT(sign ? '-' : '+');
+ s = buf;
+ i = f__scale;
+ if (f__scale <= 0) {
+#ifdef WANT_LEAD_0
+ if (insert0)
+ PUT('0');
+#endif
+ PUT('.');
+ for(; i < 0; ++i)
+ PUT('0');
+ PUT(*s);
+ s += 2;
+ }
+ else if (f__scale > 1) {
+ PUT(*s);
+ s += 2;
+ while(--i > 0)
+ PUT(*s++);
+ PUT('.');
+ }
+ if (d1) {
+ se -= 2;
+ while(s < se) PUT(*s++);
+ se += 2;
+ do PUT('0'); while(--d1 > 0);
+ }
+ while(s < se)
+ PUT(*s++);
+ if (e < 2)
+ PUT(s[1]);
+ else {
+ while(++e1 <= e)
+ PUT('0');
+ while(*s)
+ PUT(*s++);
+ }
+ return 0;
+ }
+
+ int
+#ifdef KR_headers
+wrt_F(p,w,d,len) ufloat *p; ftnlen len;
+#else
+wrt_F(ufloat *p, int w, int d, ftnlen len)
+#endif
+{
+ int d1, sign, n;
+ double x;
+ char *b, buf[MAXINTDIGS+MAXFRACDIGS+4], *s;
+
+ x= (len==sizeof(real)?p->pf:p->pd);
+ if (d < MAXFRACDIGS)
+ d1 = 0;
+ else {
+ d1 = d - MAXFRACDIGS;
+ d = MAXFRACDIGS;
+ }
+ if (x < 0.)
+ { x = -x; sign = 1; }
+ else {
+ sign = 0;
+#ifndef VAX
+ if (!x) {
+#ifdef SIGNED_ZEROS
+ if (signbit_f2c(&x))
+ sign = 2;
+#endif
+ x = 0.;
+ }
+#endif
+ }
+
+ if (n = f__scale)
+ if (n > 0)
+ do x *= 10.; while(--n > 0);
+ else
+ do x *= 0.1; while(++n < 0);
+
+#ifdef USE_STRLEN
+ sprintf(b = buf, "%#.*f", d, x);
+ n = strlen(b) + d1;
+#else
+ n = sprintf(b = buf, "%#.*f", d, x) + d1;
+#endif
+
+#ifndef WANT_LEAD_0
+ if (buf[0] == '0' && d)
+ { ++b; --n; }
+#endif
+ if (sign == 1) {
+ /* check for all zeros */
+ for(s = b;;) {
+ while(*s == '0') s++;
+ switch(*s) {
+ case '.':
+ s++; continue;
+ case 0:
+ sign = 0;
+ }
+ break;
+ }
+ }
+ if (sign || f__cplus)
+ ++n;
+ if (n > w) {
+#ifdef WANT_LEAD_0
+ if (buf[0] == '0' && --n == w)
+ ++b;
+ else
+#endif
+ {
+ while(--w >= 0)
+ PUT('*');
+ return 0;
+ }
+ }
+ for(w -= n; --w >= 0; )
+ PUT(' ');
+ if (sign)
+ PUT('-');
+ else if (f__cplus)
+ PUT('+');
+ while(n = *b++)
+ PUT(n);
+ while(--d1 >= 0)
+ PUT('0');
+ return 0;
+ }
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/wrtfmt.c b/3rdparty/clapack/f2c/wrtfmt.c
new file mode 100644
index 0000000..a970db9
--- /dev/null
+++ b/3rdparty/clapack/f2c/wrtfmt.c
@@ -0,0 +1,377 @@
+#include "f2c.h"
+#include "fio.h"
+#include "fmt.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+extern icilist *f__svic;
+extern char *f__icptr;
+
+ static int
+mv_cur(Void) /* shouldn't use fseek because it insists on calling fflush */
+ /* instead we know too much about stdio */
+{
+ int cursor = f__cursor;
+ f__cursor = 0;
+ if(f__external == 0) {
+ if(cursor < 0) {
+ if(f__hiwater < f__recpos)
+ f__hiwater = f__recpos;
+ f__recpos += cursor;
+ f__icptr += cursor;
+ if(f__recpos < 0)
+ err(f__elist->cierr, 110, "left off");
+ }
+ else if(cursor > 0) {
+ if(f__recpos + cursor >= f__svic->icirlen)
+ err(f__elist->cierr, 110, "recend");
+ if(f__hiwater <= f__recpos)
+ for(; cursor > 0; cursor--)
+ (*f__putn)(' ');
+ else if(f__hiwater <= f__recpos + cursor) {
+ cursor -= f__hiwater - f__recpos;
+ f__icptr += f__hiwater - f__recpos;
+ f__recpos = f__hiwater;
+ for(; cursor > 0; cursor--)
+ (*f__putn)(' ');
+ }
+ else {
+ f__icptr += cursor;
+ f__recpos += cursor;
+ }
+ }
+ return(0);
+ }
+ if (cursor > 0) {
+ if(f__hiwater <= f__recpos)
+ for(;cursor>0;cursor--) (*f__putn)(' ');
+ else if(f__hiwater <= f__recpos + cursor) {
+ cursor -= f__hiwater - f__recpos;
+ f__recpos = f__hiwater;
+ for(; cursor > 0; cursor--)
+ (*f__putn)(' ');
+ }
+ else {
+ f__recpos += cursor;
+ }
+ }
+ else if (cursor < 0)
+ {
+ if(cursor + f__recpos < 0)
+ err(f__elist->cierr,110,"left off");
+ if(f__hiwater < f__recpos)
+ f__hiwater = f__recpos;
+ f__recpos += cursor;
+ }
+ return(0);
+}
+
+ static int
+#ifdef KR_headers
+wrt_Z(n,w,minlen,len) Uint *n; int w, minlen; ftnlen len;
+#else
+wrt_Z(Uint *n, int w, int minlen, ftnlen len)
+#endif
+{
+ register char *s, *se;
+ register int i, w1;
+ static int one = 1;
+ static char hex[] = "0123456789ABCDEF";
+ s = (char *)n;
+ --len;
+ if (*(char *)&one) {
+ /* little endian */
+ se = s;
+ s += len;
+ i = -1;
+ }
+ else {
+ se = s + len;
+ i = 1;
+ }
+ for(;; s += i)
+ if (s == se || *s)
+ break;
+ w1 = (i*(se-s) << 1) + 1;
+ if (*s & 0xf0)
+ w1++;
+ if (w1 > w)
+ for(i = 0; i < w; i++)
+ (*f__putn)('*');
+ else {
+ if ((minlen -= w1) > 0)
+ w1 += minlen;
+ while(--w >= w1)
+ (*f__putn)(' ');
+ while(--minlen >= 0)
+ (*f__putn)('0');
+ if (!(*s & 0xf0)) {
+ (*f__putn)(hex[*s & 0xf]);
+ if (s == se)
+ return 0;
+ s += i;
+ }
+ for(;; s += i) {
+ (*f__putn)(hex[*s >> 4 & 0xf]);
+ (*f__putn)(hex[*s & 0xf]);
+ if (s == se)
+ break;
+ }
+ }
+ return 0;
+ }
+
+ static int
+#ifdef KR_headers
+wrt_I(n,w,len, base) Uint *n; ftnlen len; register int base;
+#else
+wrt_I(Uint *n, int w, ftnlen len, register int base)
+#endif
+{ int ndigit,sign,spare,i;
+ longint x;
+ char *ans;
+ if(len==sizeof(integer)) x=n->il;
+ else if(len == sizeof(char)) x = n->ic;
+#ifdef Allow_TYQUAD
+ else if (len == sizeof(longint)) x = n->ili;
+#endif
+ else x=n->is;
+ ans=f__icvt(x,&ndigit,&sign, base);
+ spare=w-ndigit;
+ if(sign || f__cplus) spare--;
+ if(spare<0)
+ for(i=0;i<w;i++) (*f__putn)('*');
+ else
+ { for(i=0;i<spare;i++) (*f__putn)(' ');
+ if(sign) (*f__putn)('-');
+ else if(f__cplus) (*f__putn)('+');
+ for(i=0;i<ndigit;i++) (*f__putn)(*ans++);
+ }
+ return(0);
+}
+ static int
+#ifdef KR_headers
+wrt_IM(n,w,m,len,base) Uint *n; ftnlen len; int base;
+#else
+wrt_IM(Uint *n, int w, int m, ftnlen len, int base)
+#endif
+{ int ndigit,sign,spare,i,xsign;
+ longint x;
+ char *ans;
+ if(sizeof(integer)==len) x=n->il;
+ else if(len == sizeof(char)) x = n->ic;
+#ifdef Allow_TYQUAD
+ else if (len == sizeof(longint)) x = n->ili;
+#endif
+ else x=n->is;
+ ans=f__icvt(x,&ndigit,&sign, base);
+ if(sign || f__cplus) xsign=1;
+ else xsign=0;
+ if(ndigit+xsign>w || m+xsign>w)
+ { for(i=0;i<w;i++) (*f__putn)('*');
+ return(0);
+ }
+ if(x==0 && m==0)
+ { for(i=0;i<w;i++) (*f__putn)(' ');
+ return(0);
+ }
+ if(ndigit>=m)
+ spare=w-ndigit-xsign;
+ else
+ spare=w-m-xsign;
+ for(i=0;i<spare;i++) (*f__putn)(' ');
+ if(sign) (*f__putn)('-');
+ else if(f__cplus) (*f__putn)('+');
+ for(i=0;i<m-ndigit;i++) (*f__putn)('0');
+ for(i=0;i<ndigit;i++) (*f__putn)(*ans++);
+ return(0);
+}
+ static int
+#ifdef KR_headers
+wrt_AP(s) char *s;
+#else
+wrt_AP(char *s)
+#endif
+{ char quote;
+ int i;
+
+ if(f__cursor && (i = mv_cur()))
+ return i;
+ quote = *s++;
+ for(;*s;s++)
+ { if(*s!=quote) (*f__putn)(*s);
+ else if(*++s==quote) (*f__putn)(*s);
+ else return(1);
+ }
+ return(1);
+}
+ static int
+#ifdef KR_headers
+wrt_H(a,s) char *s;
+#else
+wrt_H(int a, char *s)
+#endif
+{
+ int i;
+
+ if(f__cursor && (i = mv_cur()))
+ return i;
+ while(a--) (*f__putn)(*s++);
+ return(1);
+}
+
+ int
+#ifdef KR_headers
+wrt_L(n,len, sz) Uint *n; ftnlen sz;
+#else
+wrt_L(Uint *n, int len, ftnlen sz)
+#endif
+{ int i;
+ long x;
+ if(sizeof(long)==sz) x=n->il;
+ else if(sz == sizeof(char)) x = n->ic;
+ else x=n->is;
+ for(i=0;i<len-1;i++)
+ (*f__putn)(' ');
+ if(x) (*f__putn)('T');
+ else (*f__putn)('F');
+ return(0);
+}
+ static int
+#ifdef KR_headers
+wrt_A(p,len) char *p; ftnlen len;
+#else
+wrt_A(char *p, ftnlen len)
+#endif
+{
+ while(len-- > 0) (*f__putn)(*p++);
+ return(0);
+}
+ static int
+#ifdef KR_headers
+wrt_AW(p,w,len) char * p; ftnlen len;
+#else
+wrt_AW(char * p, int w, ftnlen len)
+#endif
+{
+ while(w>len)
+ { w--;
+ (*f__putn)(' ');
+ }
+ while(w-- > 0)
+ (*f__putn)(*p++);
+ return(0);
+}
+
+ static int
+#ifdef KR_headers
+wrt_G(p,w,d,e,len) ufloat *p; ftnlen len;
+#else
+wrt_G(ufloat *p, int w, int d, int e, ftnlen len)
+#endif
+{ double up = 1,x;
+ int i=0,oldscale,n,j;
+ x = len==sizeof(real)?p->pf:p->pd;
+ if(x < 0 ) x = -x;
+ if(x<.1) {
+ if (x != 0.)
+ return(wrt_E(p,w,d,e,len));
+ i = 1;
+ goto have_i;
+ }
+ for(;i<=d;i++,up*=10)
+ { if(x>=up) continue;
+ have_i:
+ oldscale = f__scale;
+ f__scale = 0;
+ if(e==0) n=4;
+ else n=e+2;
+ i=wrt_F(p,w-n,d-i,len);
+ for(j=0;j<n;j++) (*f__putn)(' ');
+ f__scale=oldscale;
+ return(i);
+ }
+ return(wrt_E(p,w,d,e,len));
+}
+
+ int
+#ifdef KR_headers
+w_ed(p,ptr,len) struct syl *p; char *ptr; ftnlen len;
+#else
+w_ed(struct syl *p, char *ptr, ftnlen len)
+#endif
+{
+ int i;
+
+ if(f__cursor && (i = mv_cur()))
+ return i;
+ switch(p->op)
+ {
+ default:
+ fprintf(stderr,"w_ed, unexpected code: %d\n", p->op);
+ sig_die(f__fmtbuf, 1);
+ case I: return(wrt_I((Uint *)ptr,p->p1,len, 10));
+ case IM:
+ return(wrt_IM((Uint *)ptr,p->p1,p->p2.i[0],len,10));
+
+ /* O and OM don't work right for character, double, complex, */
+ /* or doublecomplex, and they differ from Fortran 90 in */
+ /* showing a minus sign for negative values. */
+
+ case O: return(wrt_I((Uint *)ptr, p->p1, len, 8));
+ case OM:
+ return(wrt_IM((Uint *)ptr,p->p1,p->p2.i[0],len,8));
+ case L: return(wrt_L((Uint *)ptr,p->p1, len));
+ case A: return(wrt_A(ptr,len));
+ case AW:
+ return(wrt_AW(ptr,p->p1,len));
+ case D:
+ case E:
+ case EE:
+ return(wrt_E((ufloat *)ptr,p->p1,p->p2.i[0],p->p2.i[1],len));
+ case G:
+ case GE:
+ return(wrt_G((ufloat *)ptr,p->p1,p->p2.i[0],p->p2.i[1],len));
+ case F: return(wrt_F((ufloat *)ptr,p->p1,p->p2.i[0],len));
+
+ /* Z and ZM assume 8-bit bytes. */
+
+ case Z: return(wrt_Z((Uint *)ptr,p->p1,0,len));
+ case ZM:
+ return(wrt_Z((Uint *)ptr,p->p1,p->p2.i[0],len));
+ }
+}
+
+ int
+#ifdef KR_headers
+w_ned(p) struct syl *p;
+#else
+w_ned(struct syl *p)
+#endif
+{
+ switch(p->op)
+ {
+ default: fprintf(stderr,"w_ned, unexpected code: %d\n", p->op);
+ sig_die(f__fmtbuf, 1);
+ case SLASH:
+ return((*f__donewrec)());
+ case T: f__cursor = p->p1-f__recpos - 1;
+ return(1);
+ case TL: f__cursor -= p->p1;
+ if(f__cursor < -f__recpos) /* TL1000, 1X */
+ f__cursor = -f__recpos;
+ return(1);
+ case TR:
+ case X:
+ f__cursor += p->p1;
+ return(1);
+ case APOS:
+ return(wrt_AP(p->p2.s));
+ case H:
+ return(wrt_H(p->p1,p->p2.s));
+ }
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/f2c/wsfe.c b/3rdparty/clapack/f2c/wsfe.c
new file mode 100644
index 0000000..8709f3b
--- /dev/null
+++ b/3rdparty/clapack/f2c/wsfe.c
@@ -0,0 +1,78 @@
+/*write sequential formatted external*/
+#include "f2c.h"
+#include "fio.h"
+#include "fmt.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+ int
+x_wSL(Void)
+{
+ int n = f__putbuf('\n');
+ f__hiwater = f__recpos = f__cursor = 0;
+ return(n == 0);
+}
+
+ static int
+xw_end(Void)
+{
+ int n;
+
+ if(f__nonl) {
+ f__putbuf(n = 0);
+ fflush(f__cf);
+ }
+ else
+ n = f__putbuf('\n');
+ f__hiwater = f__recpos = f__cursor = 0;
+ return n;
+}
+
+ static int
+xw_rev(Void)
+{
+ int n = 0;
+ if(f__workdone) {
+ n = f__putbuf('\n');
+ f__workdone = 0;
+ }
+ f__hiwater = f__recpos = f__cursor = 0;
+ return n;
+}
+
+#ifdef KR_headers
+integer s_wsfe(a) cilist *a; /*start*/
+#else
+integer s_wsfe(cilist *a) /*start*/
+#endif
+{ int n;
+ if(!f__init) f_init();
+ f__reading=0;
+ f__sequential=1;
+ f__formatted=1;
+ f__external=1;
+ if(n=c_sfe(a)) return(n);
+ f__elist=a;
+ f__hiwater = f__cursor=f__recpos=0;
+ f__nonl = 0;
+ f__scale=0;
+ f__fmtbuf=a->cifmt;
+ f__cf=f__curunit->ufd;
+ if(pars_f(f__fmtbuf)<0) err(a->cierr,100,"startio");
+ f__putn= x_putc;
+ f__doed= w_ed;
+ f__doned= w_ned;
+ f__doend=xw_end;
+ f__dorevert=xw_rev;
+ f__donewrec=x_wSL;
+ fmt_bg();
+ f__cplus=0;
+ f__cblank=f__curunit->ublnk;
+ if(f__curunit->uwrt != 1 && f__nowwriting(f__curunit))
+ err(a->cierr,errno,"write start");
+ return(0);
+}
+#ifdef __cplusplus
+}
+#endif
diff --git a/3rdparty/clapack/include/blaswrap.h b/3rdparty/clapack/include/blaswrap.h
new file mode 100644
index 0000000..333a17a
--- /dev/null
+++ b/3rdparty/clapack/include/blaswrap.h
@@ -0,0 +1,160 @@
+/* CLAPACK 3.0 BLAS wrapper macros
+ * Feb 5, 2000
+ */
+
+#ifndef __BLASWRAP_H
+#define __BLASWRAP_H
+
+#ifndef NO_BLAS_WRAP
+
+/* BLAS1 routines */
+#define srotg_ f2c_srotg
+#define drotg_ f2c_drotg
+#define srotmg_ f2c_srotmg
+#define drotmg_ f2c_drotmg
+#define srot_ f2c_srot
+#define drot_ f2c_drot
+#define srotm_ f2c_srotm
+#define drotm_ f2c_drotm
+#define csrot_ f2c_csrot
+#define zdrot_ f2c_zdrot
+#define sswap_ f2c_sswap
+#define dswap_ f2c_dswap
+#define cswap_ f2c_cswap
+#define zswap_ f2c_zswap
+#define sscal_ f2c_sscal
+#define dscal_ f2c_dscal
+#define cscal_ f2c_cscal
+#define zscal_ f2c_zscal
+#define csscal_ f2c_csscal
+#define zdscal_ f2c_zdscal
+#define scopy_ f2c_scopy
+#define dcopy_ f2c_dcopy
+#define ccopy_ f2c_ccopy
+#define zcopy_ f2c_zcopy
+#define saxpy_ f2c_saxpy
+#define daxpy_ f2c_daxpy
+#define caxpy_ f2c_caxpy
+#define zaxpy_ f2c_zaxpy
+#define sdot_ f2c_sdot
+#define ddot_ f2c_ddot
+#define cdotu_ f2c_cdotu
+#define zdotu_ f2c_zdotu
+#define cdotc_ f2c_cdotc
+#define zdotc_ f2c_zdotc
+#define snrm2_ f2c_snrm2
+#define dnrm2_ f2c_dnrm2
+#define scnrm2_ f2c_scnrm2
+#define dznrm2_ f2c_dznrm2
+#define sasum_ f2c_sasum
+#define dasum_ f2c_dasum
+#define scasum_ f2c_scasum
+#define dzasum_ f2c_dzasum
+#define isamax_ f2c_isamax
+#define idamax_ f2c_idamax
+#define icamax_ f2c_icamax
+#define izamax_ f2c_izamax
+
+/* BLAS2 routines */
+#define sgemv_ f2c_sgemv
+#define dgemv_ f2c_dgemv
+#define cgemv_ f2c_cgemv
+#define zgemv_ f2c_zgemv
+#define sgbmv_ f2c_sgbmv
+#define dgbmv_ f2c_dgbmv
+#define cgbmv_ f2c_cgbmv
+#define zgbmv_ f2c_zgbmv
+#define chemv_ f2c_chemv
+#define zhemv_ f2c_zhemv
+#define chbmv_ f2c_chbmv
+#define zhbmv_ f2c_zhbmv
+#define chpmv_ f2c_chpmv
+#define zhpmv_ f2c_zhpmv
+#define ssymv_ f2c_ssymv
+#define dsymv_ f2c_dsymv
+#define ssbmv_ f2c_ssbmv
+#define dsbmv_ f2c_dsbmv
+#define sspmv_ f2c_sspmv
+#define dspmv_ f2c_dspmv
+#define strmv_ f2c_strmv
+#define dtrmv_ f2c_dtrmv
+#define ctrmv_ f2c_ctrmv
+#define ztrmv_ f2c_ztrmv
+#define stbmv_ f2c_stbmv
+#define dtbmv_ f2c_dtbmv
+#define ctbmv_ f2c_ctbmv
+#define ztbmv_ f2c_ztbmv
+#define stpmv_ f2c_stpmv
+#define dtpmv_ f2c_dtpmv
+#define ctpmv_ f2c_ctpmv
+#define ztpmv_ f2c_ztpmv
+#define strsv_ f2c_strsv
+#define dtrsv_ f2c_dtrsv
+#define ctrsv_ f2c_ctrsv
+#define ztrsv_ f2c_ztrsv
+#define stbsv_ f2c_stbsv
+#define dtbsv_ f2c_dtbsv
+#define ctbsv_ f2c_ctbsv
+#define ztbsv_ f2c_ztbsv
+#define stpsv_ f2c_stpsv
+#define dtpsv_ f2c_dtpsv
+#define ctpsv_ f2c_ctpsv
+#define ztpsv_ f2c_ztpsv
+#define sger_ f2c_sger
+#define dger_ f2c_dger
+#define cgeru_ f2c_cgeru
+#define zgeru_ f2c_zgeru
+#define cgerc_ f2c_cgerc
+#define zgerc_ f2c_zgerc
+#define cher_ f2c_cher
+#define zher_ f2c_zher
+#define chpr_ f2c_chpr
+#define zhpr_ f2c_zhpr
+#define cher2_ f2c_cher2
+#define zher2_ f2c_zher2
+#define chpr2_ f2c_chpr2
+#define zhpr2_ f2c_zhpr2
+#define ssyr_ f2c_ssyr
+#define dsyr_ f2c_dsyr
+#define sspr_ f2c_sspr
+#define dspr_ f2c_dspr
+#define ssyr2_ f2c_ssyr2
+#define dsyr2_ f2c_dsyr2
+#define sspr2_ f2c_sspr2
+#define dspr2_ f2c_dspr2
+
+/* BLAS3 routines */
+#define sgemm_ f2c_sgemm
+#define dgemm_ f2c_dgemm
+#define cgemm_ f2c_cgemm
+#define zgemm_ f2c_zgemm
+#define ssymm_ f2c_ssymm
+#define dsymm_ f2c_dsymm
+#define csymm_ f2c_csymm
+#define zsymm_ f2c_zsymm
+#define chemm_ f2c_chemm
+#define zhemm_ f2c_zhemm
+#define ssyrk_ f2c_ssyrk
+#define dsyrk_ f2c_dsyrk
+#define csyrk_ f2c_csyrk
+#define zsyrk_ f2c_zsyrk
+#define cherk_ f2c_cherk
+#define zherk_ f2c_zherk
+#define ssyr2k_ f2c_ssyr2k
+#define dsyr2k_ f2c_dsyr2k
+#define csyr2k_ f2c_csyr2k
+#define zsyr2k_ f2c_zsyr2k
+#define cher2k_ f2c_cher2k
+#define zher2k_ f2c_zher2k
+#define strmm_ f2c_strmm
+#define dtrmm_ f2c_dtrmm
+#define ctrmm_ f2c_ctrmm
+#define ztrmm_ f2c_ztrmm
+#define strsm_ f2c_strsm
+#define dtrsm_ f2c_dtrsm
+#define ctrsm_ f2c_ctrsm
+#define ztrsm_ f2c_ztrsm
+
+#endif /* NO_BLAS_WRAP */
+
+#endif /* __BLASWRAP_H */
diff --git a/3rdparty/clapack/include/f2c.h b/3rdparty/clapack/include/f2c.h
new file mode 100644
index 0000000..b94ee7c
--- /dev/null
+++ b/3rdparty/clapack/include/f2c.h
@@ -0,0 +1,223 @@
+/* f2c.h -- Standard Fortran to C header file */
+
+/** barf [ba:rf] 2. "He suggested using FORTRAN, and everybody barfed."
+
+ - From The Shogakukan DICTIONARY OF NEW ENGLISH (Second edition) */
+
+#ifndef F2C_INCLUDE
+#define F2C_INCLUDE
+
+typedef long int integer;
+typedef unsigned long int uinteger;
+typedef char *address;
+typedef short int shortint;
+typedef float real;
+typedef double doublereal;
+typedef struct { real r, i; } complex;
+typedef struct { doublereal r, i; } doublecomplex;
+typedef long int logical;
+typedef short int shortlogical;
+typedef char logical1;
+typedef char integer1;
+#ifdef INTEGER_STAR_8 /* Adjust for integer*8. */
+typedef long long longint; /* system-dependent */
+typedef unsigned long long ulongint; /* system-dependent */
+#define qbit_clear(a,b) ((a) & ~((ulongint)1 << (b)))
+#define qbit_set(a,b) ((a) | ((ulongint)1 << (b)))
+#endif
+
+#define TRUE_ (1)
+#define FALSE_ (0)
+
+/* Extern is for use with -E */
+#ifndef Extern
+#define Extern extern
+#endif
+
+/* I/O stuff */
+
+#ifdef f2c_i2
+/* for -i2 */
+typedef short flag;
+typedef short ftnlen;
+typedef short ftnint;
+#else
+typedef long int flag;
+typedef long int ftnlen;
+typedef long int ftnint;
+#endif
+
+/*external read, write*/
+typedef struct
+{ flag cierr;
+ ftnint ciunit;
+ flag ciend;
+ char *cifmt;
+ ftnint cirec;
+} cilist;
+
+/*internal read, write*/
+typedef struct
+{ flag icierr;
+ char *iciunit;
+ flag iciend;
+ char *icifmt;
+ ftnint icirlen;
+ ftnint icirnum;
+} icilist;
+
+/*open*/
+typedef struct
+{ flag oerr;
+ ftnint ounit;
+ char *ofnm;
+ ftnlen ofnmlen;
+ char *osta;
+ char *oacc;
+ char *ofm;
+ ftnint orl;
+ char *oblnk;
+} olist;
+
+/*close*/
+typedef struct
+{ flag cerr;
+ ftnint cunit;
+ char *csta;
+} cllist;
+
+/*rewind, backspace, endfile*/
+typedef struct
+{ flag aerr;
+ ftnint aunit;
+} alist;
+
+/* inquire */
+typedef struct
+{ flag inerr;
+ ftnint inunit;
+ char *infile;
+ ftnlen infilen;
+ ftnint *inex; /*parameters in standard's order*/
+ ftnint *inopen;
+ ftnint *innum;
+ ftnint *innamed;
+ char *inname;
+ ftnlen innamlen;
+ char *inacc;
+ ftnlen inacclen;
+ char *inseq;
+ ftnlen inseqlen;
+ char *indir;
+ ftnlen indirlen;
+ char *infmt;
+ ftnlen infmtlen;
+ char *inform;
+ ftnint informlen;
+ char *inunf;
+ ftnlen inunflen;
+ ftnint *inrecl;
+ ftnint *innrec;
+ char *inblank;
+ ftnlen inblanklen;
+} inlist;
+
+#define VOID void
+
+union Multitype { /* for multiple entry points */
+ integer1 g;
+ shortint h;
+ integer i;
+ /* longint j; */
+ real r;
+ doublereal d;
+ complex c;
+ doublecomplex z;
+ };
+
+typedef union Multitype Multitype;
+
+/*typedef long int Long;*/ /* No longer used; formerly in Namelist */
+
+struct Vardesc { /* for Namelist */
+ char *name;
+ char *addr;
+ ftnlen *dims;
+ int type;
+ };
+typedef struct Vardesc Vardesc;
+
+struct Namelist {
+ char *name;
+ Vardesc **vars;
+ int nvars;
+ };
+typedef struct Namelist Namelist;
+
+#define abs(x) ((x) >= 0 ? (x) : -(x))
+#define dabs(x) (doublereal)abs(x)
+#define min(a,b) ((a) <= (b) ? (a) : (b))
+#define max(a,b) ((a) >= (b) ? (a) : (b))
+#define dmin(a,b) (doublereal)min(a,b)
+#define dmax(a,b) (doublereal)max(a,b)
+#define bit_test(a,b) ((a) >> (b) & 1)
+#define bit_clear(a,b) ((a) & ~((uinteger)1 << (b)))
+#define bit_set(a,b) ((a) | ((uinteger)1 << (b)))
+
+/* procedure parameter types for -A and -C++ */
+
+#define F2C_proc_par_types 1
+#ifdef __cplusplus
+typedef int /* Unknown procedure type */ (*U_fp)(...);
+typedef shortint (*J_fp)(...);
+typedef integer (*I_fp)(...);
+typedef real (*R_fp)(...);
+typedef doublereal (*D_fp)(...), (*E_fp)(...);
+typedef /* Complex */ VOID (*C_fp)(...);
+typedef /* Double Complex */ VOID (*Z_fp)(...);
+typedef logical (*L_fp)(...);
+typedef shortlogical (*K_fp)(...);
+typedef /* Character */ VOID (*H_fp)(...);
+typedef /* Subroutine */ int (*S_fp)(...);
+#else
+typedef int /* Unknown procedure type */ (*U_fp)();
+typedef shortint (*J_fp)();
+typedef integer (*I_fp)();
+typedef real (*R_fp)();
+typedef doublereal (*D_fp)(), (*E_fp)();
+typedef /* Complex */ VOID (*C_fp)();
+typedef /* Double Complex */ VOID (*Z_fp)();
+typedef logical (*L_fp)();
+typedef shortlogical (*K_fp)();
+typedef /* Character */ VOID (*H_fp)();
+typedef /* Subroutine */ int (*S_fp)();
+#endif
+/* E_fp is for real functions when -R is not specified */
+typedef VOID C_f; /* complex function */
+typedef VOID H_f; /* character function */
+typedef VOID Z_f; /* double complex function */
+typedef doublereal E_f; /* real function with -R not specified */
+
+/* undef any lower-case symbols that your C compiler predefines, e.g.: */
+
+#ifndef Skip_f2c_Undefs
+#undef cray
+#undef gcos
+#undef mc68010
+#undef mc68020
+#undef mips
+#undef pdp11
+#undef sgi
+#undef sparc
+#undef sun
+#undef sun2
+#undef sun3
+#undef sun4
+#undef u370
+#undef u3b
+#undef u3b2
+#undef u3b5
+#undef unix
+#undef vax
+#endif
+#endif
diff --git a/3rdparty/clapack/install/dlamch.c b/3rdparty/clapack/install/dlamch.c
new file mode 100644
index 0000000..1243e82
--- /dev/null
+++ b/3rdparty/clapack/install/dlamch.c
@@ -0,0 +1,1001 @@
+/* dlamch.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b32 = 0.;
+
+doublereal dlamch_(char *cmach)
+{
+ /* Initialized data */
+
+ static logical first = TRUE_;
+
+ /* System generated locals */
+ integer i__1;
+ doublereal ret_val;
+
+ /* Builtin functions */
+ double pow_di(doublereal *, integer *);
+
+ /* Local variables */
+ static doublereal t;
+ integer it;
+ static doublereal rnd, eps, base;
+ integer beta;
+ static doublereal emin, prec, emax;
+ integer imin, imax;
+ logical lrnd;
+ static doublereal rmin, rmax;
+ doublereal rmach;
+ extern logical lsame_(char *, char *);
+ doublereal small;
+ static doublereal sfmin;
+ extern /* Subroutine */ int dlamc2_(integer *, integer *, logical *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMCH determines double precision machine parameters. */
+
+/* Arguments */
+/* ========= */
+
+/* CMACH (input) CHARACTER*1 */
+/* Specifies the value to be returned by DLAMCH: */
+/* = 'E' or 'e', DLAMCH := eps */
+/* = 'S' or 's , DLAMCH := sfmin */
+/* = 'B' or 'b', DLAMCH := base */
+/* = 'P' or 'p', DLAMCH := eps*base */
+/* = 'N' or 'n', DLAMCH := t */
+/* = 'R' or 'r', DLAMCH := rnd */
+/* = 'M' or 'm', DLAMCH := emin */
+/* = 'U' or 'u', DLAMCH := rmin */
+/* = 'L' or 'l', DLAMCH := emax */
+/* = 'O' or 'o', DLAMCH := rmax */
+
+/* where */
+
+/* eps = relative machine precision */
+/* sfmin = safe minimum, such that 1/sfmin does not overflow */
+/* base = base of the machine */
+/* prec = eps*base */
+/* t = number of (base) digits in the mantissa */
+/* rnd = 1.0 when rounding occurs in addition, 0.0 otherwise */
+/* emin = minimum exponent before (gradual) underflow */
+/* rmin = underflow threshold - base**(emin-1) */
+/* emax = largest exponent before overflow */
+/* rmax = overflow threshold - (base**emax)*(1-eps) */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Save statement .. */
+/* .. */
+/* .. Data statements .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ if (first) {
+ dlamc2_(&beta, &it, &lrnd, &eps, &imin, &rmin, &imax, &rmax);
+ base = (doublereal) beta;
+ t = (doublereal) it;
+ if (lrnd) {
+ rnd = 1.;
+ i__1 = 1 - it;
+ eps = pow_di(&base, &i__1) / 2;
+ } else {
+ rnd = 0.;
+ i__1 = 1 - it;
+ eps = pow_di(&base, &i__1);
+ }
+ prec = eps * base;
+ emin = (doublereal) imin;
+ emax = (doublereal) imax;
+ sfmin = rmin;
+ small = 1. / rmax;
+ if (small >= sfmin) {
+
+/* Use SMALL plus a bit, to avoid the possibility of rounding */
+/* causing overflow when computing 1/sfmin. */
+
+ sfmin = small * (eps + 1.);
+ }
+ }
+
+ if (lsame_(cmach, "E")) {
+ rmach = eps;
+ } else if (lsame_(cmach, "S")) {
+ rmach = sfmin;
+ } else if (lsame_(cmach, "B")) {
+ rmach = base;
+ } else if (lsame_(cmach, "P")) {
+ rmach = prec;
+ } else if (lsame_(cmach, "N")) {
+ rmach = t;
+ } else if (lsame_(cmach, "R")) {
+ rmach = rnd;
+ } else if (lsame_(cmach, "M")) {
+ rmach = emin;
+ } else if (lsame_(cmach, "U")) {
+ rmach = rmin;
+ } else if (lsame_(cmach, "L")) {
+ rmach = emax;
+ } else if (lsame_(cmach, "O")) {
+ rmach = rmax;
+ }
+
+ ret_val = rmach;
+ first = FALSE_;
+ return ret_val;
+
+/* End of DLAMCH */
+
+} /* dlamch_ */
+
+
+/* *********************************************************************** */
+
+/* Subroutine */ int dlamc1_(integer *beta, integer *t, logical *rnd, logical
+ *ieee1)
+{
+ /* Initialized data */
+
+ static logical first = TRUE_;
+
+ /* System generated locals */
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ doublereal a, b, c__, f, t1, t2;
+ static integer lt;
+ doublereal one, qtr;
+ static logical lrnd;
+ static integer lbeta;
+ doublereal savec;
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+ static logical lieee1;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMC1 determines the machine parameters given by BETA, T, RND, and */
+/* IEEE1. */
+
+/* Arguments */
+/* ========= */
+
+/* BETA (output) INTEGER */
+/* The base of the machine. */
+
+/* T (output) INTEGER */
+/* The number of ( BETA ) digits in the mantissa. */
+
+/* RND (output) LOGICAL */
+/* Specifies whether proper rounding ( RND = .TRUE. ) or */
+/* chopping ( RND = .FALSE. ) occurs in addition. This may not */
+/* be a reliable guide to the way in which the machine performs */
+/* its arithmetic. */
+
+/* IEEE1 (output) LOGICAL */
+/* Specifies whether rounding appears to be done in the IEEE */
+/* 'round to nearest' style. */
+
+/* Further Details */
+/* =============== */
+
+/* The routine is based on the routine ENVRON by Malcolm and */
+/* incorporates suggestions by Gentleman and Marovich. See */
+
+/* Malcolm M. A. (1972) Algorithms to reveal properties of */
+/* floating-point arithmetic. Comms. of the ACM, 15, 949-951. */
+
+/* Gentleman W. M. and Marovich S. B. (1974) More on algorithms */
+/* that reveal properties of floating point arithmetic units. */
+/* Comms. of the ACM, 17, 276-277. */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Save statement .. */
+/* .. */
+/* .. Data statements .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ if (first) {
+ one = 1.;
+
+/* LBETA, LIEEE1, LT and LRND are the local values of BETA, */
+/* IEEE1, T and RND. */
+
+/* Throughout this routine we use the function DLAMC3 to ensure */
+/* that relevant values are stored and not held in registers, or */
+/* are not affected by optimizers. */
+
+/* Compute a = 2.0**m with the smallest positive integer m such */
+/* that */
+
+/* fl( a + 1.0 ) = a. */
+
+ a = 1.;
+ c__ = 1.;
+
+/* + WHILE( C.EQ.ONE )LOOP */
+L10:
+ if (c__ == one) {
+ a *= 2;
+ c__ = dlamc3_(&a, &one);
+ d__1 = -a;
+ c__ = dlamc3_(&c__, &d__1);
+ goto L10;
+ }
+/* + END WHILE */
+
+/* Now compute b = 2.0**m with the smallest positive integer m */
+/* such that */
+
+/* fl( a + b ) .gt. a. */
+
+ b = 1.;
+ c__ = dlamc3_(&a, &b);
+
+/* + WHILE( C.EQ.A )LOOP */
+L20:
+ if (c__ == a) {
+ b *= 2;
+ c__ = dlamc3_(&a, &b);
+ goto L20;
+ }
+/* + END WHILE */
+
+/* Now compute the base. a and c are neighbouring floating point */
+/* numbers in the interval ( beta**t, beta**( t + 1 ) ) and so */
+/* their difference is beta. Adding 0.25 to c is to ensure that it */
+/* is truncated to beta and not ( beta - 1 ). */
+
+ qtr = one / 4;
+ savec = c__;
+ d__1 = -a;
+ c__ = dlamc3_(&c__, &d__1);
+ lbeta = (integer) (c__ + qtr);
+
+/* Now determine whether rounding or chopping occurs, by adding a */
+/* bit less than beta/2 and a bit more than beta/2 to a. */
+
+ b = (doublereal) lbeta;
+ d__1 = b / 2;
+ d__2 = -b / 100;
+ f = dlamc3_(&d__1, &d__2);
+ c__ = dlamc3_(&f, &a);
+ if (c__ == a) {
+ lrnd = TRUE_;
+ } else {
+ lrnd = FALSE_;
+ }
+ d__1 = b / 2;
+ d__2 = b / 100;
+ f = dlamc3_(&d__1, &d__2);
+ c__ = dlamc3_(&f, &a);
+ if (lrnd && c__ == a) {
+ lrnd = FALSE_;
+ }
+
+/* Try and decide whether rounding is done in the IEEE 'round to */
+/* nearest' style. B/2 is half a unit in the last place of the two */
+/* numbers A and SAVEC. Furthermore, A is even, i.e. has last bit */
+/* zero, and SAVEC is odd. Thus adding B/2 to A should not change */
+/* A, but adding B/2 to SAVEC should change SAVEC. */
+
+ d__1 = b / 2;
+ t1 = dlamc3_(&d__1, &a);
+ d__1 = b / 2;
+ t2 = dlamc3_(&d__1, &savec);
+ lieee1 = t1 == a && t2 > savec && lrnd;
+
+/* Now find the mantissa, t. It should be the integer part of */
+/* log to the base beta of a, however it is safer to determine t */
+/* by powering. So we find t as the smallest positive integer for */
+/* which */
+
+/* fl( beta**t + 1.0 ) = 1.0. */
+
+ lt = 0;
+ a = 1.;
+ c__ = 1.;
+
+/* + WHILE( C.EQ.ONE )LOOP */
+L30:
+ if (c__ == one) {
+ ++lt;
+ a *= lbeta;
+ c__ = dlamc3_(&a, &one);
+ d__1 = -a;
+ c__ = dlamc3_(&c__, &d__1);
+ goto L30;
+ }
+/* + END WHILE */
+
+ }
+
+ *beta = lbeta;
+ *t = lt;
+ *rnd = lrnd;
+ *ieee1 = lieee1;
+ first = FALSE_;
+ return 0;
+
+/* End of DLAMC1 */
+
+} /* dlamc1_ */
+
+
+/* *********************************************************************** */
+
+/* Subroutine */ int dlamc2_(integer *beta, integer *t, logical *rnd,
+ doublereal *eps, integer *emin, doublereal *rmin, integer *emax,
+ doublereal *rmax)
+{
+ /* Initialized data */
+
+ static logical first = TRUE_;
+ static logical iwarn = FALSE_;
+
+ /* Format strings */
+ static char fmt_9999[] = "(//\002 WARNING. The value EMIN may be incorre"
+ "ct:-\002,\002 EMIN = \002,i8,/\002 If, after inspection, the va"
+ "lue EMIN looks\002,\002 acceptable please comment out \002,/\002"
+ " the IF block as marked within the code of routine\002,\002 DLAM"
+ "C2,\002,/\002 otherwise supply EMIN explicitly.\002,/)";
+
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2, d__3, d__4, d__5;
+
+ /* Builtin functions */
+ double pow_di(doublereal *, integer *);
+ integer s_wsfe(cilist *), do_fio(integer *, char *, ftnlen), e_wsfe(void);
+
+ /* Local variables */
+ doublereal a, b, c__;
+ integer i__;
+ static integer lt;
+ doublereal one, two;
+ logical ieee;
+ doublereal half;
+ logical lrnd;
+ static doublereal leps;
+ doublereal zero;
+ static integer lbeta;
+ doublereal rbase;
+ static integer lemin, lemax;
+ integer gnmin;
+ doublereal small;
+ integer gpmin;
+ doublereal third;
+ static doublereal lrmin, lrmax;
+ doublereal sixth;
+ extern /* Subroutine */ int dlamc1_(integer *, integer *, logical *,
+ logical *);
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+ logical lieee1;
+ extern /* Subroutine */ int dlamc4_(integer *, doublereal *, integer *),
+ dlamc5_(integer *, integer *, integer *, logical *, integer *,
+ doublereal *);
+ integer ngnmin, ngpmin;
+
+ /* Fortran I/O blocks */
+ static cilist io___58 = { 0, 6, 0, fmt_9999, 0 };
+
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMC2 determines the machine parameters specified in its argument */
+/* list. */
+
+/* Arguments */
+/* ========= */
+
+/* BETA (output) INTEGER */
+/* The base of the machine. */
+
+/* T (output) INTEGER */
+/* The number of ( BETA ) digits in the mantissa. */
+
+/* RND (output) LOGICAL */
+/* Specifies whether proper rounding ( RND = .TRUE. ) or */
+/* chopping ( RND = .FALSE. ) occurs in addition. This may not */
+/* be a reliable guide to the way in which the machine performs */
+/* its arithmetic. */
+
+/* EPS (output) DOUBLE PRECISION */
+/* The smallest positive number such that */
+
+/* fl( 1.0 - EPS ) .LT. 1.0, */
+
+/* where fl denotes the computed value. */
+
+/* EMIN (output) INTEGER */
+/* The minimum exponent before (gradual) underflow occurs. */
+
+/* RMIN (output) DOUBLE PRECISION */
+/* The smallest normalized number for the machine, given by */
+/* BASE**( EMIN - 1 ), where BASE is the floating point value */
+/* of BETA. */
+
+/* EMAX (output) INTEGER */
+/* The maximum exponent before overflow occurs. */
+
+/* RMAX (output) DOUBLE PRECISION */
+/* The largest positive number for the machine, given by */
+/* BASE**EMAX * ( 1 - EPS ), where BASE is the floating point */
+/* value of BETA. */
+
+/* Further Details */
+/* =============== */
+
+/* The computation of EPS is based on a routine PARANOIA by */
+/* W. Kahan of the University of California at Berkeley. */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Save statement .. */
+/* .. */
+/* .. Data statements .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ if (first) {
+ zero = 0.;
+ one = 1.;
+ two = 2.;
+
+/* LBETA, LT, LRND, LEPS, LEMIN and LRMIN are the local values of */
+/* BETA, T, RND, EPS, EMIN and RMIN. */
+
+/* Throughout this routine we use the function DLAMC3 to ensure */
+/* that relevant values are stored and not held in registers, or */
+/* are not affected by optimizers. */
+
+/* DLAMC1 returns the parameters LBETA, LT, LRND and LIEEE1. */
+
+ dlamc1_(&lbeta, <, &lrnd, &lieee1);
+
+/* Start to find EPS. */
+
+ b = (doublereal) lbeta;
+ i__1 = -lt;
+ a = pow_di(&b, &i__1);
+ leps = a;
+
+/* Try some tricks to see whether or not this is the correct EPS. */
+
+ b = two / 3;
+ half = one / 2;
+ d__1 = -half;
+ sixth = dlamc3_(&b, &d__1);
+ third = dlamc3_(&sixth, &sixth);
+ d__1 = -half;
+ b = dlamc3_(&third, &d__1);
+ b = dlamc3_(&b, &sixth);
+ b = abs(b);
+ if (b < leps) {
+ b = leps;
+ }
+
+ leps = 1.;
+
+/* + WHILE( ( LEPS.GT.B ).AND.( B.GT.ZERO ) )LOOP */
+L10:
+ if (leps > b && b > zero) {
+ leps = b;
+ d__1 = half * leps;
+/* Computing 5th power */
+ d__3 = two, d__4 = d__3, d__3 *= d__3;
+/* Computing 2nd power */
+ d__5 = leps;
+ d__2 = d__4 * (d__3 * d__3) * (d__5 * d__5);
+ c__ = dlamc3_(&d__1, &d__2);
+ d__1 = -c__;
+ c__ = dlamc3_(&half, &d__1);
+ b = dlamc3_(&half, &c__);
+ d__1 = -b;
+ c__ = dlamc3_(&half, &d__1);
+ b = dlamc3_(&half, &c__);
+ goto L10;
+ }
+/* + END WHILE */
+
+ if (a < leps) {
+ leps = a;
+ }
+
+/* Computation of EPS complete. */
+
+/* Now find EMIN. Let A = + or - 1, and + or - (1 + BASE**(-3)). */
+/* Keep dividing A by BETA until (gradual) underflow occurs. This */
+/* is detected when we cannot recover the previous A. */
+
+ rbase = one / lbeta;
+ small = one;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ d__1 = small * rbase;
+ small = dlamc3_(&d__1, &zero);
+/* L20: */
+ }
+ a = dlamc3_(&one, &small);
+ dlamc4_(&ngpmin, &one, &lbeta);
+ d__1 = -one;
+ dlamc4_(&ngnmin, &d__1, &lbeta);
+ dlamc4_(&gpmin, &a, &lbeta);
+ d__1 = -a;
+ dlamc4_(&gnmin, &d__1, &lbeta);
+ ieee = FALSE_;
+
+ if (ngpmin == ngnmin && gpmin == gnmin) {
+ if (ngpmin == gpmin) {
+ lemin = ngpmin;
+/* ( Non twos-complement machines, no gradual underflow; */
+/* e.g., VAX ) */
+ } else if (gpmin - ngpmin == 3) {
+ lemin = ngpmin - 1 + lt;
+ ieee = TRUE_;
+/* ( Non twos-complement machines, with gradual underflow; */
+/* e.g., IEEE standard followers ) */
+ } else {
+ lemin = min(ngpmin,gpmin);
+/* ( A guess; no known machine ) */
+ iwarn = TRUE_;
+ }
+
+ } else if (ngpmin == gpmin && ngnmin == gnmin) {
+ if ((i__1 = ngpmin - ngnmin, abs(i__1)) == 1) {
+ lemin = max(ngpmin,ngnmin);
+/* ( Twos-complement machines, no gradual underflow; */
+/* e.g., CYBER 205 ) */
+ } else {
+ lemin = min(ngpmin,ngnmin);
+/* ( A guess; no known machine ) */
+ iwarn = TRUE_;
+ }
+
+ } else if ((i__1 = ngpmin - ngnmin, abs(i__1)) == 1 && gpmin == gnmin)
+ {
+ if (gpmin - min(ngpmin,ngnmin) == 3) {
+ lemin = max(ngpmin,ngnmin) - 1 + lt;
+/* ( Twos-complement machines with gradual underflow; */
+/* no known machine ) */
+ } else {
+ lemin = min(ngpmin,ngnmin);
+/* ( A guess; no known machine ) */
+ iwarn = TRUE_;
+ }
+
+ } else {
+/* Computing MIN */
+ i__1 = min(ngpmin,ngnmin), i__1 = min(i__1,gpmin);
+ lemin = min(i__1,gnmin);
+/* ( A guess; no known machine ) */
+ iwarn = TRUE_;
+ }
+ first = FALSE_;
+/* ** */
+/* Comment out this if block if EMIN is ok */
+ if (iwarn) {
+ first = TRUE_;
+ s_wsfe(&io___58);
+ do_fio(&c__1, (char *)&lemin, (ftnlen)sizeof(integer));
+ e_wsfe();
+ }
+/* ** */
+
+/* Assume IEEE arithmetic if we found denormalised numbers above, */
+/* or if arithmetic seems to round in the IEEE style, determined */
+/* in routine DLAMC1. A true IEEE machine should have both things */
+/* true; however, faulty machines may have one or the other. */
+
+ ieee = ieee || lieee1;
+
+/* Compute RMIN by successive division by BETA. We could compute */
+/* RMIN as BASE**( EMIN - 1 ), but some machines underflow during */
+/* this computation. */
+
+ lrmin = 1.;
+ i__1 = 1 - lemin;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__1 = lrmin * rbase;
+ lrmin = dlamc3_(&d__1, &zero);
+/* L30: */
+ }
+
+/* Finally, call DLAMC5 to compute EMAX and RMAX. */
+
+ dlamc5_(&lbeta, <, &lemin, &ieee, &lemax, &lrmax);
+ }
+
+ *beta = lbeta;
+ *t = lt;
+ *rnd = lrnd;
+ *eps = leps;
+ *emin = lemin;
+ *rmin = lrmin;
+ *emax = lemax;
+ *rmax = lrmax;
+
+ return 0;
+
+
+/* End of DLAMC2 */
+
+} /* dlamc2_ */
+
+
+/* *********************************************************************** */
+
+doublereal dlamc3_(doublereal *a, doublereal *b)
+{
+ /* System generated locals */
+ doublereal ret_val;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMC3 is intended to force A and B to be stored prior to doing */
+/* the addition of A and B , for use in situations where optimizers */
+/* might hold one of these in a register. */
+
+/* Arguments */
+/* ========= */
+
+/* A (input) DOUBLE PRECISION */
+/* B (input) DOUBLE PRECISION */
+/* The values A and B. */
+
+/* ===================================================================== */
+
+/* .. Executable Statements .. */
+
+ ret_val = *a + *b;
+
+ return ret_val;
+
+/* End of DLAMC3 */
+
+} /* dlamc3_ */
+
+
+/* *********************************************************************** */
+
+/* Subroutine */ int dlamc4_(integer *emin, doublereal *start, integer *base)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1;
+
+ /* Local variables */
+ doublereal a;
+ integer i__;
+ doublereal b1, b2, c1, c2, d1, d2, one, zero, rbase;
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMC4 is a service routine for DLAMC2. */
+
+/* Arguments */
+/* ========= */
+
+/* EMIN (output) INTEGER */
+/* The minimum exponent before (gradual) underflow, computed by */
+/* setting A = START and dividing by BASE until the previous A */
+/* can not be recovered. */
+
+/* START (input) DOUBLE PRECISION */
+/* The starting point for determining EMIN. */
+
+/* BASE (input) INTEGER */
+/* The base of the machine. */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ a = *start;
+ one = 1.;
+ rbase = one / *base;
+ zero = 0.;
+ *emin = 1;
+ d__1 = a * rbase;
+ b1 = dlamc3_(&d__1, &zero);
+ c1 = a;
+ c2 = a;
+ d1 = a;
+ d2 = a;
+/* + WHILE( ( C1.EQ.A ).AND.( C2.EQ.A ).AND. */
+/* $ ( D1.EQ.A ).AND.( D2.EQ.A ) )LOOP */
+L10:
+ if (c1 == a && c2 == a && d1 == a && d2 == a) {
+ --(*emin);
+ a = b1;
+ d__1 = a / *base;
+ b1 = dlamc3_(&d__1, &zero);
+ d__1 = b1 * *base;
+ c1 = dlamc3_(&d__1, &zero);
+ d1 = zero;
+ i__1 = *base;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d1 += b1;
+/* L20: */
+ }
+ d__1 = a * rbase;
+ b2 = dlamc3_(&d__1, &zero);
+ d__1 = b2 / rbase;
+ c2 = dlamc3_(&d__1, &zero);
+ d2 = zero;
+ i__1 = *base;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d2 += b2;
+/* L30: */
+ }
+ goto L10;
+ }
+/* + END WHILE */
+
+ return 0;
+
+/* End of DLAMC4 */
+
+} /* dlamc4_ */
+
+
+/* *********************************************************************** */
+
+/* Subroutine */ int dlamc5_(integer *beta, integer *p, integer *emin,
+ logical *ieee, integer *emax, doublereal *rmax)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__;
+ doublereal y, z__;
+ integer try__, lexp;
+ doublereal oldy;
+ integer uexp, nbits;
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+ doublereal recbas;
+ integer exbits, expsum;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMC5 attempts to compute RMAX, the largest machine floating-point */
+/* number, without overflow. It assumes that EMAX + abs(EMIN) sum */
+/* approximately to a power of 2. It will fail on machines where this */
+/* assumption does not hold, for example, the Cyber 205 (EMIN = -28625, */
+/* EMAX = 28718). It will also fail if the value supplied for EMIN is */
+/* too large (i.e. too close to zero), probably with overflow. */
+
+/* Arguments */
+/* ========= */
+
+/* BETA (input) INTEGER */
+/* The base of floating-point arithmetic. */
+
+/* P (input) INTEGER */
+/* The number of base BETA digits in the mantissa of a */
+/* floating-point value. */
+
+/* EMIN (input) INTEGER */
+/* The minimum exponent before (gradual) underflow. */
+
+/* IEEE (input) LOGICAL */
+/* A logical flag specifying whether or not the arithmetic */
+/* system is thought to comply with the IEEE standard. */
+
+/* EMAX (output) INTEGER */
+/* The largest exponent before overflow */
+
+/* RMAX (output) DOUBLE PRECISION */
+/* The largest machine floating-point number. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* First compute LEXP and UEXP, two powers of 2 that bound */
+/* abs(EMIN). We then assume that EMAX + abs(EMIN) will sum */
+/* approximately to the bound that is closest to abs(EMIN). */
+/* (EMAX is the exponent of the required number RMAX). */
+
+ lexp = 1;
+ exbits = 1;
+L10:
+ try__ = lexp << 1;
+ if (try__ <= -(*emin)) {
+ lexp = try__;
+ ++exbits;
+ goto L10;
+ }
+ if (lexp == -(*emin)) {
+ uexp = lexp;
+ } else {
+ uexp = try__;
+ ++exbits;
+ }
+
+/* Now -LEXP is less than or equal to EMIN, and -UEXP is greater */
+/* than or equal to EMIN. EXBITS is the number of bits needed to */
+/* store the exponent. */
+
+ if (uexp + *emin > -lexp - *emin) {
+ expsum = lexp << 1;
+ } else {
+ expsum = uexp << 1;
+ }
+
+/* EXPSUM is the exponent range, approximately equal to */
+/* EMAX - EMIN + 1 . */
+
+ *emax = expsum + *emin - 1;
+ nbits = exbits + 1 + *p;
+
+/* NBITS is the total number of bits needed to store a */
+/* floating-point number. */
+
+ if (nbits % 2 == 1 && *beta == 2) {
+
+/* Either there are an odd number of bits used to store a */
+/* floating-point number, which is unlikely, or some bits are */
+/* not used in the representation of numbers, which is possible, */
+/* (e.g. Cray machines) or the mantissa has an implicit bit, */
+/* (e.g. IEEE machines, Dec Vax machines), which is perhaps the */
+/* most likely. We have to assume the last alternative. */
+/* If this is true, then we need to reduce EMAX by one because */
+/* there must be some way of representing zero in an implicit-bit */
+/* system. On machines like Cray, we are reducing EMAX by one */
+/* unnecessarily. */
+
+ --(*emax);
+ }
+
+ if (*ieee) {
+
+/* Assume we are on an IEEE machine which reserves one exponent */
+/* for infinity and NaN. */
+
+ --(*emax);
+ }
+
+/* Now create RMAX, the largest machine number, which should */
+/* be equal to (1.0 - BETA**(-P)) * BETA**EMAX . */
+
+/* First compute 1.0 - BETA**(-P), being careful that the */
+/* result is less than 1.0 . */
+
+ recbas = 1. / *beta;
+ z__ = *beta - 1.;
+ y = 0.;
+ i__1 = *p;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ z__ *= recbas;
+ if (y < 1.) {
+ oldy = y;
+ }
+ y = dlamc3_(&y, &z__);
+/* L20: */
+ }
+ if (y >= 1.) {
+ y = oldy;
+ }
+
+/* Now multiply by BETA**EMAX to get RMAX. */
+
+ i__1 = *emax;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__1 = y * *beta;
+ y = dlamc3_(&d__1, &c_b32);
+/* L30: */
+ }
+
+ *rmax = y;
+ return 0;
+
+/* End of DLAMC5 */
+
+} /* dlamc5_ */
diff --git a/3rdparty/clapack/src/dbdsdc.c b/3rdparty/clapack/src/dbdsdc.c
new file mode 100644
index 0000000..6096e4f
--- /dev/null
+++ b/3rdparty/clapack/src/dbdsdc.c
@@ -0,0 +1,514 @@
+/* dbdsdc.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__9 = 9;
+static integer c__0 = 0;
+static doublereal c_b15 = 1.;
+static integer c__1 = 1;
+static doublereal c_b29 = 0.;
+
+/* Subroutine */ int dbdsdc_(char *uplo, char *compq, integer *n, doublereal *
+ d__, doublereal *e, doublereal *u, integer *ldu, doublereal *vt,
+ integer *ldvt, doublereal *q, integer *iq, doublereal *work, integer *
+ iwork, integer *info)
+{
+ /* System generated locals */
+ integer u_dim1, u_offset, vt_dim1, vt_offset, i__1, i__2;
+ doublereal d__1;
+
+ /* Builtin functions */
+ double d_sign(doublereal *, doublereal *), log(doublereal);
+
+ /* Local variables */
+ integer i__, j, k;
+ doublereal p, r__;
+ integer z__, ic, ii, kk;
+ doublereal cs;
+ integer is, iu;
+ doublereal sn;
+ integer nm1;
+ doublereal eps;
+ integer ivt, difl, difr, ierr, perm, mlvl, sqre;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dlasr_(char *, char *, char *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, integer *), dcopy_(integer *, doublereal *, integer *
+, doublereal *, integer *), dswap_(integer *, doublereal *,
+ integer *, doublereal *, integer *);
+ integer poles, iuplo, nsize, start;
+ extern /* Subroutine */ int dlasd0_(integer *, integer *, doublereal *,
+ doublereal *, doublereal *, integer *, doublereal *, integer *,
+ integer *, integer *, doublereal *, integer *);
+ extern doublereal dlamch_(char *);
+ extern /* Subroutine */ int dlasda_(integer *, integer *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *, integer *, integer *, integer *,
+ doublereal *, doublereal *, doublereal *, doublereal *, integer *,
+ integer *), dlascl_(char *, integer *, integer *, doublereal *,
+ doublereal *, integer *, integer *, doublereal *, integer *,
+ integer *), dlasdq_(char *, integer *, integer *, integer
+ *, integer *, integer *, doublereal *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *), dlaset_(char *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, integer *), dlartg_(doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ integer givcol;
+ extern doublereal dlanst_(char *, integer *, doublereal *, doublereal *);
+ integer icompq;
+ doublereal orgnrm;
+ integer givnum, givptr, qstart, smlsiz, wstart, smlszp;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DBDSDC computes the singular value decomposition (SVD) of a real */
+/* N-by-N (upper or lower) bidiagonal matrix B: B = U * S * VT, */
+/* using a divide and conquer method, where S is a diagonal matrix */
+/* with non-negative diagonal elements (the singular values of B), and */
+/* U and VT are orthogonal matrices of left and right singular vectors, */
+/* respectively. DBDSDC can be used to compute all singular values, */
+/* and optionally, singular vectors or singular vectors in compact form. */
+
+/* This code makes very mild assumptions about floating point */
+/* arithmetic. It will work on machines with a guard digit in */
+/* add/subtract, or on those binary machines without guard digits */
+/* which subtract like the Cray X-MP, Cray Y-MP, Cray C-90, or Cray-2. */
+/* It could conceivably fail on hexadecimal or decimal machines */
+/* without guard digits, but we know of none. See DLASD3 for details. */
+
+/* The code currently calls DLASDQ if singular values only are desired. */
+/* However, it can be slightly modified to compute singular values */
+/* using the divide and conquer method. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* = 'U': B is upper bidiagonal. */
+/* = 'L': B is lower bidiagonal. */
+
+/* COMPQ (input) CHARACTER*1 */
+/* Specifies whether singular vectors are to be computed */
+/* as follows: */
+/* = 'N': Compute singular values only; */
+/* = 'P': Compute singular values and compute singular */
+/* vectors in compact form; */
+/* = 'I': Compute singular values and singular vectors. */
+
+/* N (input) INTEGER */
+/* The order of the matrix B. N >= 0. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, the n diagonal elements of the bidiagonal matrix B. */
+/* On exit, if INFO=0, the singular values of B. */
+
+/* E (input/output) DOUBLE PRECISION array, dimension (N-1) */
+/* On entry, the elements of E contain the offdiagonal */
+/* elements of the bidiagonal matrix whose SVD is desired. */
+/* On exit, E has been destroyed. */
+
+/* U (output) DOUBLE PRECISION array, dimension (LDU,N) */
+/* If COMPQ = 'I', then: */
+/* On exit, if INFO = 0, U contains the left singular vectors */
+/* of the bidiagonal matrix. */
+/* For other values of COMPQ, U is not referenced. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= 1. */
+/* If singular vectors are desired, then LDU >= max( 1, N ). */
+
+/* VT (output) DOUBLE PRECISION array, dimension (LDVT,N) */
+/* If COMPQ = 'I', then: */
+/* On exit, if INFO = 0, VT' contains the right singular */
+/* vectors of the bidiagonal matrix. */
+/* For other values of COMPQ, VT is not referenced. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. LDVT >= 1. */
+/* If singular vectors are desired, then LDVT >= max( 1, N ). */
+
+/* Q (output) DOUBLE PRECISION array, dimension (LDQ) */
+/* If COMPQ = 'P', then: */
+/* On exit, if INFO = 0, Q and IQ contain the left */
+/* and right singular vectors in a compact form, */
+/* requiring O(N log N) space instead of 2*N**2. */
+/* In particular, Q contains all the DOUBLE PRECISION data in */
+/* LDQ >= N*(11 + 2*SMLSIZ + 8*INT(LOG_2(N/(SMLSIZ+1)))) */
+/* words of memory, where SMLSIZ is returned by ILAENV and */
+/* is equal to the maximum size of the subproblems at the */
+/* bottom of the computation tree (usually about 25). */
+/* For other values of COMPQ, Q is not referenced. */
+
+/* IQ (output) INTEGER array, dimension (LDIQ) */
+/* If COMPQ = 'P', then: */
+/* On exit, if INFO = 0, Q and IQ contain the left */
+/* and right singular vectors in a compact form, */
+/* requiring O(N log N) space instead of 2*N**2. */
+/* In particular, IQ contains all INTEGER data in */
+/* LDIQ >= N*(3 + 3*INT(LOG_2(N/(SMLSIZ+1)))) */
+/* words of memory, where SMLSIZ is returned by ILAENV and */
+/* is equal to the maximum size of the subproblems at the */
+/* bottom of the computation tree (usually about 25). */
+/* For other values of COMPQ, IQ is not referenced. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* If COMPQ = 'N' then LWORK >= (4 * N). */
+/* If COMPQ = 'P' then LWORK >= (6 * N). */
+/* If COMPQ = 'I' then LWORK >= (3 * N**2 + 4 * N). */
+
+/* IWORK (workspace) INTEGER array, dimension (8*N) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: The algorithm failed to compute an singular value. */
+/* The update process of divide and conquer failed. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+/* Changed dimension statement in comment describing E from (N) to */
+/* (N-1). Sven, 17 Feb 05. */
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --e;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ --q;
+ --iq;
+ --work;
+ --iwork;
+
+ /* Function Body */
+ *info = 0;
+
+ iuplo = 0;
+ if (lsame_(uplo, "U")) {
+ iuplo = 1;
+ }
+ if (lsame_(uplo, "L")) {
+ iuplo = 2;
+ }
+ if (lsame_(compq, "N")) {
+ icompq = 0;
+ } else if (lsame_(compq, "P")) {
+ icompq = 1;
+ } else if (lsame_(compq, "I")) {
+ icompq = 2;
+ } else {
+ icompq = -1;
+ }
+ if (iuplo == 0) {
+ *info = -1;
+ } else if (icompq < 0) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*ldu < 1 || icompq == 2 && *ldu < *n) {
+ *info = -7;
+ } else if (*ldvt < 1 || icompq == 2 && *ldvt < *n) {
+ *info = -9;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DBDSDC", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+ smlsiz = ilaenv_(&c__9, "DBDSDC", " ", &c__0, &c__0, &c__0, &c__0);
+ if (*n == 1) {
+ if (icompq == 1) {
+ q[1] = d_sign(&c_b15, &d__[1]);
+ q[smlsiz * *n + 1] = 1.;
+ } else if (icompq == 2) {
+ u[u_dim1 + 1] = d_sign(&c_b15, &d__[1]);
+ vt[vt_dim1 + 1] = 1.;
+ }
+ d__[1] = abs(d__[1]);
+ return 0;
+ }
+ nm1 = *n - 1;
+
+/* If matrix lower bidiagonal, rotate to be upper bidiagonal */
+/* by applying Givens rotations on the left */
+
+ wstart = 1;
+ qstart = 3;
+ if (icompq == 1) {
+ dcopy_(n, &d__[1], &c__1, &q[1], &c__1);
+ i__1 = *n - 1;
+ dcopy_(&i__1, &e[1], &c__1, &q[*n + 1], &c__1);
+ }
+ if (iuplo == 2) {
+ qstart = 5;
+ wstart = (*n << 1) - 1;
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dlartg_(&d__[i__], &e[i__], &cs, &sn, &r__);
+ d__[i__] = r__;
+ e[i__] = sn * d__[i__ + 1];
+ d__[i__ + 1] = cs * d__[i__ + 1];
+ if (icompq == 1) {
+ q[i__ + (*n << 1)] = cs;
+ q[i__ + *n * 3] = sn;
+ } else if (icompq == 2) {
+ work[i__] = cs;
+ work[nm1 + i__] = -sn;
+ }
+/* L10: */
+ }
+ }
+
+/* If ICOMPQ = 0, use DLASDQ to compute the singular values. */
+
+ if (icompq == 0) {
+ dlasdq_("U", &c__0, n, &c__0, &c__0, &c__0, &d__[1], &e[1], &vt[
+ vt_offset], ldvt, &u[u_offset], ldu, &u[u_offset], ldu, &work[
+ wstart], info);
+ goto L40;
+ }
+
+/* If N is smaller than the minimum divide size SMLSIZ, then solve */
+/* the problem with another solver. */
+
+ if (*n <= smlsiz) {
+ if (icompq == 2) {
+ dlaset_("A", n, n, &c_b29, &c_b15, &u[u_offset], ldu);
+ dlaset_("A", n, n, &c_b29, &c_b15, &vt[vt_offset], ldvt);
+ dlasdq_("U", &c__0, n, n, n, &c__0, &d__[1], &e[1], &vt[vt_offset]
+, ldvt, &u[u_offset], ldu, &u[u_offset], ldu, &work[
+ wstart], info);
+ } else if (icompq == 1) {
+ iu = 1;
+ ivt = iu + *n;
+ dlaset_("A", n, n, &c_b29, &c_b15, &q[iu + (qstart - 1) * *n], n);
+ dlaset_("A", n, n, &c_b29, &c_b15, &q[ivt + (qstart - 1) * *n], n);
+ dlasdq_("U", &c__0, n, n, n, &c__0, &d__[1], &e[1], &q[ivt + (
+ qstart - 1) * *n], n, &q[iu + (qstart - 1) * *n], n, &q[
+ iu + (qstart - 1) * *n], n, &work[wstart], info);
+ }
+ goto L40;
+ }
+
+ if (icompq == 2) {
+ dlaset_("A", n, n, &c_b29, &c_b15, &u[u_offset], ldu);
+ dlaset_("A", n, n, &c_b29, &c_b15, &vt[vt_offset], ldvt);
+ }
+
+/* Scale. */
+
+ orgnrm = dlanst_("M", n, &d__[1], &e[1]);
+ if (orgnrm == 0.) {
+ return 0;
+ }
+ dlascl_("G", &c__0, &c__0, &orgnrm, &c_b15, n, &c__1, &d__[1], n, &ierr);
+ dlascl_("G", &c__0, &c__0, &orgnrm, &c_b15, &nm1, &c__1, &e[1], &nm1, &
+ ierr);
+
+ eps = dlamch_("Epsilon");
+
+ mlvl = (integer) (log((doublereal) (*n) / (doublereal) (smlsiz + 1)) /
+ log(2.)) + 1;
+ smlszp = smlsiz + 1;
+
+ if (icompq == 1) {
+ iu = 1;
+ ivt = smlsiz + 1;
+ difl = ivt + smlszp;
+ difr = difl + mlvl;
+ z__ = difr + (mlvl << 1);
+ ic = z__ + mlvl;
+ is = ic + 1;
+ poles = is + 1;
+ givnum = poles + (mlvl << 1);
+
+ k = 1;
+ givptr = 2;
+ perm = 3;
+ givcol = perm + mlvl;
+ }
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if ((d__1 = d__[i__], abs(d__1)) < eps) {
+ d__[i__] = d_sign(&eps, &d__[i__]);
+ }
+/* L20: */
+ }
+
+ start = 1;
+ sqre = 0;
+
+ i__1 = nm1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if ((d__1 = e[i__], abs(d__1)) < eps || i__ == nm1) {
+
+/* Subproblem found. First determine its size and then */
+/* apply divide and conquer on it. */
+
+ if (i__ < nm1) {
+
+/* A subproblem with E(I) small for I < NM1. */
+
+ nsize = i__ - start + 1;
+ } else if ((d__1 = e[i__], abs(d__1)) >= eps) {
+
+/* A subproblem with E(NM1) not too small but I = NM1. */
+
+ nsize = *n - start + 1;
+ } else {
+
+/* A subproblem with E(NM1) small. This implies an */
+/* 1-by-1 subproblem at D(N). Solve this 1-by-1 problem */
+/* first. */
+
+ nsize = i__ - start + 1;
+ if (icompq == 2) {
+ u[*n + *n * u_dim1] = d_sign(&c_b15, &d__[*n]);
+ vt[*n + *n * vt_dim1] = 1.;
+ } else if (icompq == 1) {
+ q[*n + (qstart - 1) * *n] = d_sign(&c_b15, &d__[*n]);
+ q[*n + (smlsiz + qstart - 1) * *n] = 1.;
+ }
+ d__[*n] = (d__1 = d__[*n], abs(d__1));
+ }
+ if (icompq == 2) {
+ dlasd0_(&nsize, &sqre, &d__[start], &e[start], &u[start +
+ start * u_dim1], ldu, &vt[start + start * vt_dim1],
+ ldvt, &smlsiz, &iwork[1], &work[wstart], info);
+ } else {
+ dlasda_(&icompq, &smlsiz, &nsize, &sqre, &d__[start], &e[
+ start], &q[start + (iu + qstart - 2) * *n], n, &q[
+ start + (ivt + qstart - 2) * *n], &iq[start + k * *n],
+ &q[start + (difl + qstart - 2) * *n], &q[start + (
+ difr + qstart - 2) * *n], &q[start + (z__ + qstart -
+ 2) * *n], &q[start + (poles + qstart - 2) * *n], &iq[
+ start + givptr * *n], &iq[start + givcol * *n], n, &
+ iq[start + perm * *n], &q[start + (givnum + qstart -
+ 2) * *n], &q[start + (ic + qstart - 2) * *n], &q[
+ start + (is + qstart - 2) * *n], &work[wstart], &
+ iwork[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+ }
+ start = i__ + 1;
+ }
+/* L30: */
+ }
+
+/* Unscale */
+
+ dlascl_("G", &c__0, &c__0, &c_b15, &orgnrm, n, &c__1, &d__[1], n, &ierr);
+L40:
+
+/* Use Selection Sort to minimize swaps of singular vectors */
+
+ i__1 = *n;
+ for (ii = 2; ii <= i__1; ++ii) {
+ i__ = ii - 1;
+ kk = i__;
+ p = d__[i__];
+ i__2 = *n;
+ for (j = ii; j <= i__2; ++j) {
+ if (d__[j] > p) {
+ kk = j;
+ p = d__[j];
+ }
+/* L50: */
+ }
+ if (kk != i__) {
+ d__[kk] = d__[i__];
+ d__[i__] = p;
+ if (icompq == 1) {
+ iq[i__] = kk;
+ } else if (icompq == 2) {
+ dswap_(n, &u[i__ * u_dim1 + 1], &c__1, &u[kk * u_dim1 + 1], &
+ c__1);
+ dswap_(n, &vt[i__ + vt_dim1], ldvt, &vt[kk + vt_dim1], ldvt);
+ }
+ } else if (icompq == 1) {
+ iq[i__] = i__;
+ }
+/* L60: */
+ }
+
+/* If ICOMPQ = 1, use IQ(N,1) as the indicator for UPLO */
+
+ if (icompq == 1) {
+ if (iuplo == 1) {
+ iq[*n] = 1;
+ } else {
+ iq[*n] = 0;
+ }
+ }
+
+/* If B is lower bidiagonal, update U by those Givens rotations */
+/* which rotated B to be upper bidiagonal */
+
+ if (iuplo == 2 && icompq == 2) {
+ dlasr_("L", "V", "B", n, n, &work[1], &work[*n], &u[u_offset], ldu);
+ }
+
+ return 0;
+
+/* End of DBDSDC */
+
+} /* dbdsdc_ */
diff --git a/3rdparty/clapack/src/dbdsqr.c b/3rdparty/clapack/src/dbdsqr.c
new file mode 100644
index 0000000..08b04fc
--- /dev/null
+++ b/3rdparty/clapack/src/dbdsqr.c
@@ -0,0 +1,918 @@
+/* dbdsqr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static doublereal c_b15 = -.125;
+static integer c__1 = 1;
+static doublereal c_b49 = 1.;
+static doublereal c_b72 = -1.;
+
+/* Subroutine */ int dbdsqr_(char *uplo, integer *n, integer *ncvt, integer *
+ nru, integer *ncc, doublereal *d__, doublereal *e, doublereal *vt,
+ integer *ldvt, doublereal *u, integer *ldu, doublereal *c__, integer *
+ ldc, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer c_dim1, c_offset, u_dim1, u_offset, vt_dim1, vt_offset, i__1,
+ i__2;
+ doublereal d__1, d__2, d__3, d__4;
+
+ /* Builtin functions */
+ double pow_dd(doublereal *, doublereal *), sqrt(doublereal), d_sign(
+ doublereal *, doublereal *);
+
+ /* Local variables */
+ doublereal f, g, h__;
+ integer i__, j, m;
+ doublereal r__, cs;
+ integer ll;
+ doublereal sn, mu;
+ integer nm1, nm12, nm13, lll;
+ doublereal eps, sll, tol, abse;
+ integer idir;
+ doublereal abss;
+ integer oldm;
+ doublereal cosl;
+ integer isub, iter;
+ doublereal unfl, sinl, cosr, smin, smax, sinr;
+ extern /* Subroutine */ int drot_(integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *), dlas2_(
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *), dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ extern logical lsame_(char *, char *);
+ doublereal oldcs;
+ extern /* Subroutine */ int dlasr_(char *, char *, char *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, integer *);
+ integer oldll;
+ doublereal shift, sigmn, oldsn;
+ extern /* Subroutine */ int dswap_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer maxit;
+ doublereal sminl, sigmx;
+ logical lower;
+ extern /* Subroutine */ int dlasq1_(integer *, doublereal *, doublereal *,
+ doublereal *, integer *), dlasv2_(doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *);
+ extern doublereal dlamch_(char *);
+ extern /* Subroutine */ int dlartg_(doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *), xerbla_(char *,
+ integer *);
+ doublereal sminoa, thresh;
+ logical rotate;
+ doublereal tolmul;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* January 2007 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DBDSQR computes the singular values and, optionally, the right and/or */
+/* left singular vectors from the singular value decomposition (SVD) of */
+/* a real N-by-N (upper or lower) bidiagonal matrix B using the implicit */
+/* zero-shift QR algorithm. The SVD of B has the form */
+
+/* B = Q * S * P**T */
+
+/* where S is the diagonal matrix of singular values, Q is an orthogonal */
+/* matrix of left singular vectors, and P is an orthogonal matrix of */
+/* right singular vectors. If left singular vectors are requested, this */
+/* subroutine actually returns U*Q instead of Q, and, if right singular */
+/* vectors are requested, this subroutine returns P**T*VT instead of */
+/* P**T, for given real input matrices U and VT. When U and VT are the */
+/* orthogonal matrices that reduce a general matrix A to bidiagonal */
+/* form: A = U*B*VT, as computed by DGEBRD, then */
+
+/* A = (U*Q) * S * (P**T*VT) */
+
+/* is the SVD of A. Optionally, the subroutine may also compute Q**T*C */
+/* for a given real input matrix C. */
+
+/* See "Computing Small Singular Values of Bidiagonal Matrices With */
+/* Guaranteed High Relative Accuracy," by J. Demmel and W. Kahan, */
+/* LAPACK Working Note #3 (or SIAM J. Sci. Statist. Comput. vol. 11, */
+/* no. 5, pp. 873-912, Sept 1990) and */
+/* "Accurate singular values and differential qd algorithms," by */
+/* B. Parlett and V. Fernando, Technical Report CPAM-554, Mathematics */
+/* Department, University of California at Berkeley, July 1992 */
+/* for a detailed description of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* = 'U': B is upper bidiagonal; */
+/* = 'L': B is lower bidiagonal. */
+
+/* N (input) INTEGER */
+/* The order of the matrix B. N >= 0. */
+
+/* NCVT (input) INTEGER */
+/* The number of columns of the matrix VT. NCVT >= 0. */
+
+/* NRU (input) INTEGER */
+/* The number of rows of the matrix U. NRU >= 0. */
+
+/* NCC (input) INTEGER */
+/* The number of columns of the matrix C. NCC >= 0. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, the n diagonal elements of the bidiagonal matrix B. */
+/* On exit, if INFO=0, the singular values of B in decreasing */
+/* order. */
+
+/* E (input/output) DOUBLE PRECISION array, dimension (N-1) */
+/* On entry, the N-1 offdiagonal elements of the bidiagonal */
+/* matrix B. */
+/* On exit, if INFO = 0, E is destroyed; if INFO > 0, D and E */
+/* will contain the diagonal and superdiagonal elements of a */
+/* bidiagonal matrix orthogonally equivalent to the one given */
+/* as input. */
+
+/* VT (input/output) DOUBLE PRECISION array, dimension (LDVT, NCVT) */
+/* On entry, an N-by-NCVT matrix VT. */
+/* On exit, VT is overwritten by P**T * VT. */
+/* Not referenced if NCVT = 0. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. */
+/* LDVT >= max(1,N) if NCVT > 0; LDVT >= 1 if NCVT = 0. */
+
+/* U (input/output) DOUBLE PRECISION array, dimension (LDU, N) */
+/* On entry, an NRU-by-N matrix U. */
+/* On exit, U is overwritten by U * Q. */
+/* Not referenced if NRU = 0. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= max(1,NRU). */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC, NCC) */
+/* On entry, an N-by-NCC matrix C. */
+/* On exit, C is overwritten by Q**T * C. */
+/* Not referenced if NCC = 0. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. */
+/* LDC >= max(1,N) if NCC > 0; LDC >=1 if NCC = 0. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (4*N) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: If INFO = -i, the i-th argument had an illegal value */
+/* > 0: */
+/* if NCVT = NRU = NCC = 0, */
+/* = 1, a split was marked by a positive value in E */
+/* = 2, current block of Z not diagonalized after 30*N */
+/* iterations (in inner while loop) */
+/* = 3, termination criterion of outer while loop not met */
+/* (program created more than N unreduced blocks) */
+/* else NCVT = NRU = NCC = 0, */
+/* the algorithm did not converge; D and E contain the */
+/* elements of a bidiagonal matrix which is orthogonally */
+/* similar to the input matrix B; if INFO = i, i */
+/* elements of E have not converged to zero. */
+
+/* Internal Parameters */
+/* =================== */
+
+/* TOLMUL DOUBLE PRECISION, default = max(10,min(100,EPS**(-1/8))) */
+/* TOLMUL controls the convergence criterion of the QR loop. */
+/* If it is positive, TOLMUL*EPS is the desired relative */
+/* precision in the computed singular values. */
+/* If it is negative, abs(TOLMUL*EPS*sigma_max) is the */
+/* desired absolute accuracy in the computed singular */
+/* values (corresponds to relative accuracy */
+/* abs(TOLMUL*EPS) in the largest singular value. */
+/* abs(TOLMUL) should be between 1 and 1/EPS, and preferably */
+/* between 10 (for fast convergence) and .1/EPS */
+/* (for there to be some accuracy in the results). */
+/* Default is to lose at either one eighth or 2 of the */
+/* available decimal digits in each computed singular value */
+/* (whichever is smaller). */
+
+/* MAXITR INTEGER, default = 6 */
+/* MAXITR controls the maximum number of passes of the */
+/* algorithm through its inner loop. The algorithms stops */
+/* (and so fails to converge) if the number of passes */
+/* through the inner loop exceeds MAXITR*N**2. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --e;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ lower = lsame_(uplo, "L");
+ if (! lsame_(uplo, "U") && ! lower) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*ncvt < 0) {
+ *info = -3;
+ } else if (*nru < 0) {
+ *info = -4;
+ } else if (*ncc < 0) {
+ *info = -5;
+ } else if (*ncvt == 0 && *ldvt < 1 || *ncvt > 0 && *ldvt < max(1,*n)) {
+ *info = -9;
+ } else if (*ldu < max(1,*nru)) {
+ *info = -11;
+ } else if (*ncc == 0 && *ldc < 1 || *ncc > 0 && *ldc < max(1,*n)) {
+ *info = -13;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DBDSQR", &i__1);
+ return 0;
+ }
+ if (*n == 0) {
+ return 0;
+ }
+ if (*n == 1) {
+ goto L160;
+ }
+
+/* ROTATE is true if any singular vectors desired, false otherwise */
+
+ rotate = *ncvt > 0 || *nru > 0 || *ncc > 0;
+
+/* If no singular vectors desired, use qd algorithm */
+
+ if (! rotate) {
+ dlasq1_(n, &d__[1], &e[1], &work[1], info);
+ return 0;
+ }
+
+ nm1 = *n - 1;
+ nm12 = nm1 + nm1;
+ nm13 = nm12 + nm1;
+ idir = 0;
+
+/* Get machine constants */
+
+ eps = dlamch_("Epsilon");
+ unfl = dlamch_("Safe minimum");
+
+/* If matrix lower bidiagonal, rotate to be upper bidiagonal */
+/* by applying Givens rotations on the left */
+
+ if (lower) {
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dlartg_(&d__[i__], &e[i__], &cs, &sn, &r__);
+ d__[i__] = r__;
+ e[i__] = sn * d__[i__ + 1];
+ d__[i__ + 1] = cs * d__[i__ + 1];
+ work[i__] = cs;
+ work[nm1 + i__] = sn;
+/* L10: */
+ }
+
+/* Update singular vectors if desired */
+
+ if (*nru > 0) {
+ dlasr_("R", "V", "F", nru, n, &work[1], &work[*n], &u[u_offset],
+ ldu);
+ }
+ if (*ncc > 0) {
+ dlasr_("L", "V", "F", n, ncc, &work[1], &work[*n], &c__[c_offset],
+ ldc);
+ }
+ }
+
+/* Compute singular values to relative accuracy TOL */
+/* (By setting TOL to be negative, algorithm will compute */
+/* singular values to absolute accuracy ABS(TOL)*norm(input matrix)) */
+
+/* Computing MAX */
+/* Computing MIN */
+ d__3 = 100., d__4 = pow_dd(&eps, &c_b15);
+ d__1 = 10., d__2 = min(d__3,d__4);
+ tolmul = max(d__1,d__2);
+ tol = tolmul * eps;
+
+/* Compute approximate maximum, minimum singular values */
+
+ smax = 0.;
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__2 = smax, d__3 = (d__1 = d__[i__], abs(d__1));
+ smax = max(d__2,d__3);
+/* L20: */
+ }
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__2 = smax, d__3 = (d__1 = e[i__], abs(d__1));
+ smax = max(d__2,d__3);
+/* L30: */
+ }
+ sminl = 0.;
+ if (tol >= 0.) {
+
+/* Relative accuracy desired */
+
+ sminoa = abs(d__[1]);
+ if (sminoa == 0.) {
+ goto L50;
+ }
+ mu = sminoa;
+ i__1 = *n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ mu = (d__2 = d__[i__], abs(d__2)) * (mu / (mu + (d__1 = e[i__ - 1]
+ , abs(d__1))));
+ sminoa = min(sminoa,mu);
+ if (sminoa == 0.) {
+ goto L50;
+ }
+/* L40: */
+ }
+L50:
+ sminoa /= sqrt((doublereal) (*n));
+/* Computing MAX */
+ d__1 = tol * sminoa, d__2 = *n * 6 * *n * unfl;
+ thresh = max(d__1,d__2);
+ } else {
+
+/* Absolute accuracy desired */
+
+/* Computing MAX */
+ d__1 = abs(tol) * smax, d__2 = *n * 6 * *n * unfl;
+ thresh = max(d__1,d__2);
+ }
+
+/* Prepare for main iteration loop for the singular values */
+/* (MAXIT is the maximum number of passes through the inner */
+/* loop permitted before nonconvergence signalled.) */
+
+ maxit = *n * 6 * *n;
+ iter = 0;
+ oldll = -1;
+ oldm = -1;
+
+/* M points to last element of unconverged part of matrix */
+
+ m = *n;
+
+/* Begin main iteration loop */
+
+L60:
+
+/* Check for convergence or exceeding iteration count */
+
+ if (m <= 1) {
+ goto L160;
+ }
+ if (iter > maxit) {
+ goto L200;
+ }
+
+/* Find diagonal block of matrix to work on */
+
+ if (tol < 0. && (d__1 = d__[m], abs(d__1)) <= thresh) {
+ d__[m] = 0.;
+ }
+ smax = (d__1 = d__[m], abs(d__1));
+ smin = smax;
+ i__1 = m - 1;
+ for (lll = 1; lll <= i__1; ++lll) {
+ ll = m - lll;
+ abss = (d__1 = d__[ll], abs(d__1));
+ abse = (d__1 = e[ll], abs(d__1));
+ if (tol < 0. && abss <= thresh) {
+ d__[ll] = 0.;
+ }
+ if (abse <= thresh) {
+ goto L80;
+ }
+ smin = min(smin,abss);
+/* Computing MAX */
+ d__1 = max(smax,abss);
+ smax = max(d__1,abse);
+/* L70: */
+ }
+ ll = 0;
+ goto L90;
+L80:
+ e[ll] = 0.;
+
+/* Matrix splits since E(LL) = 0 */
+
+ if (ll == m - 1) {
+
+/* Convergence of bottom singular value, return to top of loop */
+
+ --m;
+ goto L60;
+ }
+L90:
+ ++ll;
+
+/* E(LL) through E(M-1) are nonzero, E(LL-1) is zero */
+
+ if (ll == m - 1) {
+
+/* 2 by 2 block, handle separately */
+
+ dlasv2_(&d__[m - 1], &e[m - 1], &d__[m], &sigmn, &sigmx, &sinr, &cosr,
+ &sinl, &cosl);
+ d__[m - 1] = sigmx;
+ e[m - 1] = 0.;
+ d__[m] = sigmn;
+
+/* Compute singular vectors, if desired */
+
+ if (*ncvt > 0) {
+ drot_(ncvt, &vt[m - 1 + vt_dim1], ldvt, &vt[m + vt_dim1], ldvt, &
+ cosr, &sinr);
+ }
+ if (*nru > 0) {
+ drot_(nru, &u[(m - 1) * u_dim1 + 1], &c__1, &u[m * u_dim1 + 1], &
+ c__1, &cosl, &sinl);
+ }
+ if (*ncc > 0) {
+ drot_(ncc, &c__[m - 1 + c_dim1], ldc, &c__[m + c_dim1], ldc, &
+ cosl, &sinl);
+ }
+ m += -2;
+ goto L60;
+ }
+
+/* If working on new submatrix, choose shift direction */
+/* (from larger end diagonal element towards smaller) */
+
+ if (ll > oldm || m < oldll) {
+ if ((d__1 = d__[ll], abs(d__1)) >= (d__2 = d__[m], abs(d__2))) {
+
+/* Chase bulge from top (big end) to bottom (small end) */
+
+ idir = 1;
+ } else {
+
+/* Chase bulge from bottom (big end) to top (small end) */
+
+ idir = 2;
+ }
+ }
+
+/* Apply convergence tests */
+
+ if (idir == 1) {
+
+/* Run convergence test in forward direction */
+/* First apply standard test to bottom of matrix */
+
+ if ((d__2 = e[m - 1], abs(d__2)) <= abs(tol) * (d__1 = d__[m], abs(
+ d__1)) || tol < 0. && (d__3 = e[m - 1], abs(d__3)) <= thresh)
+ {
+ e[m - 1] = 0.;
+ goto L60;
+ }
+
+ if (tol >= 0.) {
+
+/* If relative accuracy desired, */
+/* apply convergence criterion forward */
+
+ mu = (d__1 = d__[ll], abs(d__1));
+ sminl = mu;
+ i__1 = m - 1;
+ for (lll = ll; lll <= i__1; ++lll) {
+ if ((d__1 = e[lll], abs(d__1)) <= tol * mu) {
+ e[lll] = 0.;
+ goto L60;
+ }
+ mu = (d__2 = d__[lll + 1], abs(d__2)) * (mu / (mu + (d__1 = e[
+ lll], abs(d__1))));
+ sminl = min(sminl,mu);
+/* L100: */
+ }
+ }
+
+ } else {
+
+/* Run convergence test in backward direction */
+/* First apply standard test to top of matrix */
+
+ if ((d__2 = e[ll], abs(d__2)) <= abs(tol) * (d__1 = d__[ll], abs(d__1)
+ ) || tol < 0. && (d__3 = e[ll], abs(d__3)) <= thresh) {
+ e[ll] = 0.;
+ goto L60;
+ }
+
+ if (tol >= 0.) {
+
+/* If relative accuracy desired, */
+/* apply convergence criterion backward */
+
+ mu = (d__1 = d__[m], abs(d__1));
+ sminl = mu;
+ i__1 = ll;
+ for (lll = m - 1; lll >= i__1; --lll) {
+ if ((d__1 = e[lll], abs(d__1)) <= tol * mu) {
+ e[lll] = 0.;
+ goto L60;
+ }
+ mu = (d__2 = d__[lll], abs(d__2)) * (mu / (mu + (d__1 = e[lll]
+ , abs(d__1))));
+ sminl = min(sminl,mu);
+/* L110: */
+ }
+ }
+ }
+ oldll = ll;
+ oldm = m;
+
+/* Compute shift. First, test if shifting would ruin relative */
+/* accuracy, and if so set the shift to zero. */
+
+/* Computing MAX */
+ d__1 = eps, d__2 = tol * .01;
+ if (tol >= 0. && *n * tol * (sminl / smax) <= max(d__1,d__2)) {
+
+/* Use a zero shift to avoid loss of relative accuracy */
+
+ shift = 0.;
+ } else {
+
+/* Compute the shift from 2-by-2 block at end of matrix */
+
+ if (idir == 1) {
+ sll = (d__1 = d__[ll], abs(d__1));
+ dlas2_(&d__[m - 1], &e[m - 1], &d__[m], &shift, &r__);
+ } else {
+ sll = (d__1 = d__[m], abs(d__1));
+ dlas2_(&d__[ll], &e[ll], &d__[ll + 1], &shift, &r__);
+ }
+
+/* Test if shift negligible, and if so set to zero */
+
+ if (sll > 0.) {
+/* Computing 2nd power */
+ d__1 = shift / sll;
+ if (d__1 * d__1 < eps) {
+ shift = 0.;
+ }
+ }
+ }
+
+/* Increment iteration count */
+
+ iter = iter + m - ll;
+
+/* If SHIFT = 0, do simplified QR iteration */
+
+ if (shift == 0.) {
+ if (idir == 1) {
+
+/* Chase bulge from top to bottom */
+/* Save cosines and sines for later singular vector updates */
+
+ cs = 1.;
+ oldcs = 1.;
+ i__1 = m - 1;
+ for (i__ = ll; i__ <= i__1; ++i__) {
+ d__1 = d__[i__] * cs;
+ dlartg_(&d__1, &e[i__], &cs, &sn, &r__);
+ if (i__ > ll) {
+ e[i__ - 1] = oldsn * r__;
+ }
+ d__1 = oldcs * r__;
+ d__2 = d__[i__ + 1] * sn;
+ dlartg_(&d__1, &d__2, &oldcs, &oldsn, &d__[i__]);
+ work[i__ - ll + 1] = cs;
+ work[i__ - ll + 1 + nm1] = sn;
+ work[i__ - ll + 1 + nm12] = oldcs;
+ work[i__ - ll + 1 + nm13] = oldsn;
+/* L120: */
+ }
+ h__ = d__[m] * cs;
+ d__[m] = h__ * oldcs;
+ e[m - 1] = h__ * oldsn;
+
+/* Update singular vectors */
+
+ if (*ncvt > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "F", &i__1, ncvt, &work[1], &work[*n], &vt[
+ ll + vt_dim1], ldvt);
+ }
+ if (*nru > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("R", "V", "F", nru, &i__1, &work[nm12 + 1], &work[nm13
+ + 1], &u[ll * u_dim1 + 1], ldu);
+ }
+ if (*ncc > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "F", &i__1, ncc, &work[nm12 + 1], &work[nm13
+ + 1], &c__[ll + c_dim1], ldc);
+ }
+
+/* Test convergence */
+
+ if ((d__1 = e[m - 1], abs(d__1)) <= thresh) {
+ e[m - 1] = 0.;
+ }
+
+ } else {
+
+/* Chase bulge from bottom to top */
+/* Save cosines and sines for later singular vector updates */
+
+ cs = 1.;
+ oldcs = 1.;
+ i__1 = ll + 1;
+ for (i__ = m; i__ >= i__1; --i__) {
+ d__1 = d__[i__] * cs;
+ dlartg_(&d__1, &e[i__ - 1], &cs, &sn, &r__);
+ if (i__ < m) {
+ e[i__] = oldsn * r__;
+ }
+ d__1 = oldcs * r__;
+ d__2 = d__[i__ - 1] * sn;
+ dlartg_(&d__1, &d__2, &oldcs, &oldsn, &d__[i__]);
+ work[i__ - ll] = cs;
+ work[i__ - ll + nm1] = -sn;
+ work[i__ - ll + nm12] = oldcs;
+ work[i__ - ll + nm13] = -oldsn;
+/* L130: */
+ }
+ h__ = d__[ll] * cs;
+ d__[ll] = h__ * oldcs;
+ e[ll] = h__ * oldsn;
+
+/* Update singular vectors */
+
+ if (*ncvt > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "B", &i__1, ncvt, &work[nm12 + 1], &work[
+ nm13 + 1], &vt[ll + vt_dim1], ldvt);
+ }
+ if (*nru > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("R", "V", "B", nru, &i__1, &work[1], &work[*n], &u[ll *
+ u_dim1 + 1], ldu);
+ }
+ if (*ncc > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "B", &i__1, ncc, &work[1], &work[*n], &c__[
+ ll + c_dim1], ldc);
+ }
+
+/* Test convergence */
+
+ if ((d__1 = e[ll], abs(d__1)) <= thresh) {
+ e[ll] = 0.;
+ }
+ }
+ } else {
+
+/* Use nonzero shift */
+
+ if (idir == 1) {
+
+/* Chase bulge from top to bottom */
+/* Save cosines and sines for later singular vector updates */
+
+ f = ((d__1 = d__[ll], abs(d__1)) - shift) * (d_sign(&c_b49, &d__[
+ ll]) + shift / d__[ll]);
+ g = e[ll];
+ i__1 = m - 1;
+ for (i__ = ll; i__ <= i__1; ++i__) {
+ dlartg_(&f, &g, &cosr, &sinr, &r__);
+ if (i__ > ll) {
+ e[i__ - 1] = r__;
+ }
+ f = cosr * d__[i__] + sinr * e[i__];
+ e[i__] = cosr * e[i__] - sinr * d__[i__];
+ g = sinr * d__[i__ + 1];
+ d__[i__ + 1] = cosr * d__[i__ + 1];
+ dlartg_(&f, &g, &cosl, &sinl, &r__);
+ d__[i__] = r__;
+ f = cosl * e[i__] + sinl * d__[i__ + 1];
+ d__[i__ + 1] = cosl * d__[i__ + 1] - sinl * e[i__];
+ if (i__ < m - 1) {
+ g = sinl * e[i__ + 1];
+ e[i__ + 1] = cosl * e[i__ + 1];
+ }
+ work[i__ - ll + 1] = cosr;
+ work[i__ - ll + 1 + nm1] = sinr;
+ work[i__ - ll + 1 + nm12] = cosl;
+ work[i__ - ll + 1 + nm13] = sinl;
+/* L140: */
+ }
+ e[m - 1] = f;
+
+/* Update singular vectors */
+
+ if (*ncvt > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "F", &i__1, ncvt, &work[1], &work[*n], &vt[
+ ll + vt_dim1], ldvt);
+ }
+ if (*nru > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("R", "V", "F", nru, &i__1, &work[nm12 + 1], &work[nm13
+ + 1], &u[ll * u_dim1 + 1], ldu);
+ }
+ if (*ncc > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "F", &i__1, ncc, &work[nm12 + 1], &work[nm13
+ + 1], &c__[ll + c_dim1], ldc);
+ }
+
+/* Test convergence */
+
+ if ((d__1 = e[m - 1], abs(d__1)) <= thresh) {
+ e[m - 1] = 0.;
+ }
+
+ } else {
+
+/* Chase bulge from bottom to top */
+/* Save cosines and sines for later singular vector updates */
+
+ f = ((d__1 = d__[m], abs(d__1)) - shift) * (d_sign(&c_b49, &d__[m]
+ ) + shift / d__[m]);
+ g = e[m - 1];
+ i__1 = ll + 1;
+ for (i__ = m; i__ >= i__1; --i__) {
+ dlartg_(&f, &g, &cosr, &sinr, &r__);
+ if (i__ < m) {
+ e[i__] = r__;
+ }
+ f = cosr * d__[i__] + sinr * e[i__ - 1];
+ e[i__ - 1] = cosr * e[i__ - 1] - sinr * d__[i__];
+ g = sinr * d__[i__ - 1];
+ d__[i__ - 1] = cosr * d__[i__ - 1];
+ dlartg_(&f, &g, &cosl, &sinl, &r__);
+ d__[i__] = r__;
+ f = cosl * e[i__ - 1] + sinl * d__[i__ - 1];
+ d__[i__ - 1] = cosl * d__[i__ - 1] - sinl * e[i__ - 1];
+ if (i__ > ll + 1) {
+ g = sinl * e[i__ - 2];
+ e[i__ - 2] = cosl * e[i__ - 2];
+ }
+ work[i__ - ll] = cosr;
+ work[i__ - ll + nm1] = -sinr;
+ work[i__ - ll + nm12] = cosl;
+ work[i__ - ll + nm13] = -sinl;
+/* L150: */
+ }
+ e[ll] = f;
+
+/* Test convergence */
+
+ if ((d__1 = e[ll], abs(d__1)) <= thresh) {
+ e[ll] = 0.;
+ }
+
+/* Update singular vectors if desired */
+
+ if (*ncvt > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "B", &i__1, ncvt, &work[nm12 + 1], &work[
+ nm13 + 1], &vt[ll + vt_dim1], ldvt);
+ }
+ if (*nru > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("R", "V", "B", nru, &i__1, &work[1], &work[*n], &u[ll *
+ u_dim1 + 1], ldu);
+ }
+ if (*ncc > 0) {
+ i__1 = m - ll + 1;
+ dlasr_("L", "V", "B", &i__1, ncc, &work[1], &work[*n], &c__[
+ ll + c_dim1], ldc);
+ }
+ }
+ }
+
+/* QR iteration finished, go back and check convergence */
+
+ goto L60;
+
+/* All singular values converged, so make them positive */
+
+L160:
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (d__[i__] < 0.) {
+ d__[i__] = -d__[i__];
+
+/* Change sign of singular vectors, if desired */
+
+ if (*ncvt > 0) {
+ dscal_(ncvt, &c_b72, &vt[i__ + vt_dim1], ldvt);
+ }
+ }
+/* L170: */
+ }
+
+/* Sort the singular values into decreasing order (insertion sort on */
+/* singular values, but only one transposition per singular vector) */
+
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Scan for smallest D(I) */
+
+ isub = 1;
+ smin = d__[1];
+ i__2 = *n + 1 - i__;
+ for (j = 2; j <= i__2; ++j) {
+ if (d__[j] <= smin) {
+ isub = j;
+ smin = d__[j];
+ }
+/* L180: */
+ }
+ if (isub != *n + 1 - i__) {
+
+/* Swap singular values and vectors */
+
+ d__[isub] = d__[*n + 1 - i__];
+ d__[*n + 1 - i__] = smin;
+ if (*ncvt > 0) {
+ dswap_(ncvt, &vt[isub + vt_dim1], ldvt, &vt[*n + 1 - i__ +
+ vt_dim1], ldvt);
+ }
+ if (*nru > 0) {
+ dswap_(nru, &u[isub * u_dim1 + 1], &c__1, &u[(*n + 1 - i__) *
+ u_dim1 + 1], &c__1);
+ }
+ if (*ncc > 0) {
+ dswap_(ncc, &c__[isub + c_dim1], ldc, &c__[*n + 1 - i__ +
+ c_dim1], ldc);
+ }
+ }
+/* L190: */
+ }
+ goto L220;
+
+/* Maximum number of iterations exceeded, failure to converge */
+
+L200:
+ *info = 0;
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (e[i__] != 0.) {
+ ++(*info);
+ }
+/* L210: */
+ }
+L220:
+ return 0;
+
+/* End of DBDSQR */
+
+} /* dbdsqr_ */
diff --git a/3rdparty/clapack/src/dgebd2.c b/3rdparty/clapack/src/dgebd2.c
new file mode 100644
index 0000000..e2e5472
--- /dev/null
+++ b/3rdparty/clapack/src/dgebd2.c
@@ -0,0 +1,304 @@
+/* dgebd2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dgebd2_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *d__, doublereal *e, doublereal *tauq, doublereal *
+ taup, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__;
+ extern /* Subroutine */ int dlarf_(char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *), dlarfg_(integer *, doublereal *,
+ doublereal *, integer *, doublereal *), xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEBD2 reduces a real general m by n matrix A to upper or lower */
+/* bidiagonal form B by an orthogonal transformation: Q' * A * P = B. */
+
+/* If m >= n, B is upper bidiagonal; if m < n, B is lower bidiagonal. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows in the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns in the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the m by n general matrix to be reduced. */
+/* On exit, */
+/* if m >= n, the diagonal and the first superdiagonal are */
+/* overwritten with the upper bidiagonal matrix B; the */
+/* elements below the diagonal, with the array TAUQ, represent */
+/* the orthogonal matrix Q as a product of elementary */
+/* reflectors, and the elements above the first superdiagonal, */
+/* with the array TAUP, represent the orthogonal matrix P as */
+/* a product of elementary reflectors; */
+/* if m < n, the diagonal and the first subdiagonal are */
+/* overwritten with the lower bidiagonal matrix B; the */
+/* elements below the first subdiagonal, with the array TAUQ, */
+/* represent the orthogonal matrix Q as a product of */
+/* elementary reflectors, and the elements above the diagonal, */
+/* with the array TAUP, represent the orthogonal matrix P as */
+/* a product of elementary reflectors. */
+/* See Further Details. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* D (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The diagonal elements of the bidiagonal matrix B: */
+/* D(i) = A(i,i). */
+
+/* E (output) DOUBLE PRECISION array, dimension (min(M,N)-1) */
+/* The off-diagonal elements of the bidiagonal matrix B: */
+/* if m >= n, E(i) = A(i,i+1) for i = 1,2,...,n-1; */
+/* if m < n, E(i) = A(i+1,i) for i = 1,2,...,m-1. */
+
+/* TAUQ (output) DOUBLE PRECISION array dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix Q. See Further Details. */
+
+/* TAUP (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix P. See Further Details. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (max(M,N)) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+
+/* Further Details */
+/* =============== */
+
+/* The matrices Q and P are represented as products of elementary */
+/* reflectors: */
+
+/* If m >= n, */
+
+/* Q = H(1) H(2) . . . H(n) and P = G(1) G(2) . . . G(n-1) */
+
+/* Each H(i) and G(i) has the form: */
+
+/* H(i) = I - tauq * v * v' and G(i) = I - taup * u * u' */
+
+/* where tauq and taup are real scalars, and v and u are real vectors; */
+/* v(1:i-1) = 0, v(i) = 1, and v(i+1:m) is stored on exit in A(i+1:m,i); */
+/* u(1:i) = 0, u(i+1) = 1, and u(i+2:n) is stored on exit in A(i,i+2:n); */
+/* tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* If m < n, */
+
+/* Q = H(1) H(2) . . . H(m-1) and P = G(1) G(2) . . . G(m) */
+
+/* Each H(i) and G(i) has the form: */
+
+/* H(i) = I - tauq * v * v' and G(i) = I - taup * u * u' */
+
+/* where tauq and taup are real scalars, and v and u are real vectors; */
+/* v(1:i) = 0, v(i+1) = 1, and v(i+2:m) is stored on exit in A(i+2:m,i); */
+/* u(1:i-1) = 0, u(i) = 1, and u(i+1:n) is stored on exit in A(i,i+1:n); */
+/* tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* The contents of A on exit are illustrated by the following examples: */
+
+/* m = 6 and n = 5 (m > n): m = 5 and n = 6 (m < n): */
+
+/* ( d e u1 u1 u1 ) ( d u1 u1 u1 u1 u1 ) */
+/* ( v1 d e u2 u2 ) ( e d u2 u2 u2 u2 ) */
+/* ( v1 v2 d e u3 ) ( v1 e d u3 u3 u3 ) */
+/* ( v1 v2 v3 d e ) ( v1 v2 e d u4 u4 ) */
+/* ( v1 v2 v3 v4 d ) ( v1 v2 v3 e d u5 ) */
+/* ( v1 v2 v3 v4 v5 ) */
+
+/* where d and e denote diagonal and off-diagonal elements of B, vi */
+/* denotes an element of the vector defining H(i), and ui an element of */
+/* the vector defining G(i). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --d__;
+ --e;
+ --tauq;
+ --taup;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ }
+ if (*info < 0) {
+ i__1 = -(*info);
+ xerbla_("DGEBD2", &i__1);
+ return 0;
+ }
+
+ if (*m >= *n) {
+
+/* Reduce to upper bidiagonal form */
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Generate elementary reflector H(i) to annihilate A(i+1:m,i) */
+
+ i__2 = *m - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfg_(&i__2, &a[i__ + i__ * a_dim1], &a[min(i__3, *m)+ i__ *
+ a_dim1], &c__1, &tauq[i__]);
+ d__[i__] = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+
+/* Apply H(i) to A(i:m,i+1:n) from the left */
+
+ if (i__ < *n) {
+ i__2 = *m - i__ + 1;
+ i__3 = *n - i__;
+ dlarf_("Left", &i__2, &i__3, &a[i__ + i__ * a_dim1], &c__1, &
+ tauq[i__], &a[i__ + (i__ + 1) * a_dim1], lda, &work[1]
+);
+ }
+ a[i__ + i__ * a_dim1] = d__[i__];
+
+ if (i__ < *n) {
+
+/* Generate elementary reflector G(i) to annihilate */
+/* A(i,i+2:n) */
+
+ i__2 = *n - i__;
+/* Computing MIN */
+ i__3 = i__ + 2;
+ dlarfg_(&i__2, &a[i__ + (i__ + 1) * a_dim1], &a[i__ + min(
+ i__3, *n)* a_dim1], lda, &taup[i__]);
+ e[i__] = a[i__ + (i__ + 1) * a_dim1];
+ a[i__ + (i__ + 1) * a_dim1] = 1.;
+
+/* Apply G(i) to A(i+1:m,i+1:n) from the right */
+
+ i__2 = *m - i__;
+ i__3 = *n - i__;
+ dlarf_("Right", &i__2, &i__3, &a[i__ + (i__ + 1) * a_dim1],
+ lda, &taup[i__], &a[i__ + 1 + (i__ + 1) * a_dim1],
+ lda, &work[1]);
+ a[i__ + (i__ + 1) * a_dim1] = e[i__];
+ } else {
+ taup[i__] = 0.;
+ }
+/* L10: */
+ }
+ } else {
+
+/* Reduce to lower bidiagonal form */
+
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Generate elementary reflector G(i) to annihilate A(i,i+1:n) */
+
+ i__2 = *n - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfg_(&i__2, &a[i__ + i__ * a_dim1], &a[i__ + min(i__3, *n)*
+ a_dim1], lda, &taup[i__]);
+ d__[i__] = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+
+/* Apply G(i) to A(i+1:m,i:n) from the right */
+
+ if (i__ < *m) {
+ i__2 = *m - i__;
+ i__3 = *n - i__ + 1;
+ dlarf_("Right", &i__2, &i__3, &a[i__ + i__ * a_dim1], lda, &
+ taup[i__], &a[i__ + 1 + i__ * a_dim1], lda, &work[1]);
+ }
+ a[i__ + i__ * a_dim1] = d__[i__];
+
+ if (i__ < *m) {
+
+/* Generate elementary reflector H(i) to annihilate */
+/* A(i+2:m,i) */
+
+ i__2 = *m - i__;
+/* Computing MIN */
+ i__3 = i__ + 2;
+ dlarfg_(&i__2, &a[i__ + 1 + i__ * a_dim1], &a[min(i__3, *m)+
+ i__ * a_dim1], &c__1, &tauq[i__]);
+ e[i__] = a[i__ + 1 + i__ * a_dim1];
+ a[i__ + 1 + i__ * a_dim1] = 1.;
+
+/* Apply H(i) to A(i+1:m,i+1:n) from the left */
+
+ i__2 = *m - i__;
+ i__3 = *n - i__;
+ dlarf_("Left", &i__2, &i__3, &a[i__ + 1 + i__ * a_dim1], &
+ c__1, &tauq[i__], &a[i__ + 1 + (i__ + 1) * a_dim1],
+ lda, &work[1]);
+ a[i__ + 1 + i__ * a_dim1] = e[i__];
+ } else {
+ tauq[i__] = 0.;
+ }
+/* L20: */
+ }
+ }
+ return 0;
+
+/* End of DGEBD2 */
+
+} /* dgebd2_ */
diff --git a/3rdparty/clapack/src/dgebrd.c b/3rdparty/clapack/src/dgebrd.c
new file mode 100644
index 0000000..d5202c8
--- /dev/null
+++ b/3rdparty/clapack/src/dgebrd.c
@@ -0,0 +1,336 @@
+/* dgebrd.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__3 = 3;
+static integer c__2 = 2;
+static doublereal c_b21 = -1.;
+static doublereal c_b22 = 1.;
+
+/* Subroutine */ int dgebrd_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *d__, doublereal *e, doublereal *tauq, doublereal *
+ taup, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer i__, j, nb, nx;
+ doublereal ws;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ integer nbmin, iinfo, minmn;
+ extern /* Subroutine */ int dgebd2_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *), dlabrd_(integer *, integer *, integer *
+, doublereal *, integer *, doublereal *, doublereal *, doublereal
+ *, doublereal *, doublereal *, integer *, doublereal *, integer *)
+ , xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwrkx, ldwrky, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEBRD reduces a general real M-by-N matrix A to upper or lower */
+/* bidiagonal form B by an orthogonal transformation: Q**T * A * P = B. */
+
+/* If m >= n, B is upper bidiagonal; if m < n, B is lower bidiagonal. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows in the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns in the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the M-by-N general matrix to be reduced. */
+/* On exit, */
+/* if m >= n, the diagonal and the first superdiagonal are */
+/* overwritten with the upper bidiagonal matrix B; the */
+/* elements below the diagonal, with the array TAUQ, represent */
+/* the orthogonal matrix Q as a product of elementary */
+/* reflectors, and the elements above the first superdiagonal, */
+/* with the array TAUP, represent the orthogonal matrix P as */
+/* a product of elementary reflectors; */
+/* if m < n, the diagonal and the first subdiagonal are */
+/* overwritten with the lower bidiagonal matrix B; the */
+/* elements below the first subdiagonal, with the array TAUQ, */
+/* represent the orthogonal matrix Q as a product of */
+/* elementary reflectors, and the elements above the diagonal, */
+/* with the array TAUP, represent the orthogonal matrix P as */
+/* a product of elementary reflectors. */
+/* See Further Details. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* D (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The diagonal elements of the bidiagonal matrix B: */
+/* D(i) = A(i,i). */
+
+/* E (output) DOUBLE PRECISION array, dimension (min(M,N)-1) */
+/* The off-diagonal elements of the bidiagonal matrix B: */
+/* if m >= n, E(i) = A(i,i+1) for i = 1,2,...,n-1; */
+/* if m < n, E(i) = A(i+1,i) for i = 1,2,...,m-1. */
+
+/* TAUQ (output) DOUBLE PRECISION array dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix Q. See Further Details. */
+
+/* TAUP (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix P. See Further Details. */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The length of the array WORK. LWORK >= max(1,M,N). */
+/* For optimum performance LWORK >= (M+N)*NB, where NB */
+/* is the optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+
+/* Further Details */
+/* =============== */
+
+/* The matrices Q and P are represented as products of elementary */
+/* reflectors: */
+
+/* If m >= n, */
+
+/* Q = H(1) H(2) . . . H(n) and P = G(1) G(2) . . . G(n-1) */
+
+/* Each H(i) and G(i) has the form: */
+
+/* H(i) = I - tauq * v * v' and G(i) = I - taup * u * u' */
+
+/* where tauq and taup are real scalars, and v and u are real vectors; */
+/* v(1:i-1) = 0, v(i) = 1, and v(i+1:m) is stored on exit in A(i+1:m,i); */
+/* u(1:i) = 0, u(i+1) = 1, and u(i+2:n) is stored on exit in A(i,i+2:n); */
+/* tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* If m < n, */
+
+/* Q = H(1) H(2) . . . H(m-1) and P = G(1) G(2) . . . G(m) */
+
+/* Each H(i) and G(i) has the form: */
+
+/* H(i) = I - tauq * v * v' and G(i) = I - taup * u * u' */
+
+/* where tauq and taup are real scalars, and v and u are real vectors; */
+/* v(1:i) = 0, v(i+1) = 1, and v(i+2:m) is stored on exit in A(i+2:m,i); */
+/* u(1:i-1) = 0, u(i) = 1, and u(i+1:n) is stored on exit in A(i,i+1:n); */
+/* tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* The contents of A on exit are illustrated by the following examples: */
+
+/* m = 6 and n = 5 (m > n): m = 5 and n = 6 (m < n): */
+
+/* ( d e u1 u1 u1 ) ( d u1 u1 u1 u1 u1 ) */
+/* ( v1 d e u2 u2 ) ( e d u2 u2 u2 u2 ) */
+/* ( v1 v2 d e u3 ) ( v1 e d u3 u3 u3 ) */
+/* ( v1 v2 v3 d e ) ( v1 v2 e d u4 u4 ) */
+/* ( v1 v2 v3 v4 d ) ( v1 v2 v3 e d u5 ) */
+/* ( v1 v2 v3 v4 v5 ) */
+
+/* where d and e denote diagonal and off-diagonal elements of B, vi */
+/* denotes an element of the vector defining H(i), and ui an element of */
+/* the vector defining G(i). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --d__;
+ --e;
+ --tauq;
+ --taup;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+/* Computing MAX */
+ i__1 = 1, i__2 = ilaenv_(&c__1, "DGEBRD", " ", m, n, &c_n1, &c_n1);
+ nb = max(i__1,i__2);
+ lwkopt = (*m + *n) * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ } else /* if(complicated condition) */ {
+/* Computing MAX */
+ i__1 = max(1,*m);
+ if (*lwork < max(i__1,*n) && ! lquery) {
+ *info = -10;
+ }
+ }
+ if (*info < 0) {
+ i__1 = -(*info);
+ xerbla_("DGEBRD", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ minmn = min(*m,*n);
+ if (minmn == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ ws = (doublereal) max(*m,*n);
+ ldwrkx = *m;
+ ldwrky = *n;
+
+ if (nb > 1 && nb < minmn) {
+
+/* Set the crossover point NX. */
+
+/* Computing MAX */
+ i__1 = nb, i__2 = ilaenv_(&c__3, "DGEBRD", " ", m, n, &c_n1, &c_n1);
+ nx = max(i__1,i__2);
+
+/* Determine when to switch from blocked to unblocked code. */
+
+ if (nx < minmn) {
+ ws = (doublereal) ((*m + *n) * nb);
+ if ((doublereal) (*lwork) < ws) {
+
+/* Not enough work space for the optimal NB, consider using */
+/* a smaller block size. */
+
+ nbmin = ilaenv_(&c__2, "DGEBRD", " ", m, n, &c_n1, &c_n1);
+ if (*lwork >= (*m + *n) * nbmin) {
+ nb = *lwork / (*m + *n);
+ } else {
+ nb = 1;
+ nx = minmn;
+ }
+ }
+ }
+ } else {
+ nx = minmn;
+ }
+
+ i__1 = minmn - nx;
+ i__2 = nb;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+
+/* Reduce rows and columns i:i+nb-1 to bidiagonal form and return */
+/* the matrices X and Y which are needed to update the unreduced */
+/* part of the matrix */
+
+ i__3 = *m - i__ + 1;
+ i__4 = *n - i__ + 1;
+ dlabrd_(&i__3, &i__4, &nb, &a[i__ + i__ * a_dim1], lda, &d__[i__], &e[
+ i__], &tauq[i__], &taup[i__], &work[1], &ldwrkx, &work[ldwrkx
+ * nb + 1], &ldwrky);
+
+/* Update the trailing submatrix A(i+nb:m,i+nb:n), using an update */
+/* of the form A := A - V*Y' - X*U' */
+
+ i__3 = *m - i__ - nb + 1;
+ i__4 = *n - i__ - nb + 1;
+ dgemm_("No transpose", "Transpose", &i__3, &i__4, &nb, &c_b21, &a[i__
+ + nb + i__ * a_dim1], lda, &work[ldwrkx * nb + nb + 1], &
+ ldwrky, &c_b22, &a[i__ + nb + (i__ + nb) * a_dim1], lda);
+ i__3 = *m - i__ - nb + 1;
+ i__4 = *n - i__ - nb + 1;
+ dgemm_("No transpose", "No transpose", &i__3, &i__4, &nb, &c_b21, &
+ work[nb + 1], &ldwrkx, &a[i__ + (i__ + nb) * a_dim1], lda, &
+ c_b22, &a[i__ + nb + (i__ + nb) * a_dim1], lda);
+
+/* Copy diagonal and off-diagonal elements of B back into A */
+
+ if (*m >= *n) {
+ i__3 = i__ + nb - 1;
+ for (j = i__; j <= i__3; ++j) {
+ a[j + j * a_dim1] = d__[j];
+ a[j + (j + 1) * a_dim1] = e[j];
+/* L10: */
+ }
+ } else {
+ i__3 = i__ + nb - 1;
+ for (j = i__; j <= i__3; ++j) {
+ a[j + j * a_dim1] = d__[j];
+ a[j + 1 + j * a_dim1] = e[j];
+/* L20: */
+ }
+ }
+/* L30: */
+ }
+
+/* Use unblocked code to reduce the remainder of the matrix */
+
+ i__2 = *m - i__ + 1;
+ i__1 = *n - i__ + 1;
+ dgebd2_(&i__2, &i__1, &a[i__ + i__ * a_dim1], lda, &d__[i__], &e[i__], &
+ tauq[i__], &taup[i__], &work[1], &iinfo);
+ work[1] = ws;
+ return 0;
+
+/* End of DGEBRD */
+
+} /* dgebrd_ */
diff --git a/3rdparty/clapack/src/dgelq2.c b/3rdparty/clapack/src/dgelq2.c
new file mode 100644
index 0000000..c77e5a8
--- /dev/null
+++ b/3rdparty/clapack/src/dgelq2.c
@@ -0,0 +1,157 @@
+/* dgelq2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dgelq2_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *tau, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, k;
+ doublereal aii;
+ extern /* Subroutine */ int dlarf_(char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *), dlarfp_(integer *, doublereal *,
+ doublereal *, integer *, doublereal *), xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGELQ2 computes an LQ factorization of a real m by n matrix A: */
+/* A = L * Q. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the m by n matrix A. */
+/* On exit, the elements on and below the diagonal of the array */
+/* contain the m by min(m,n) lower trapezoidal matrix L (L is */
+/* lower triangular if m <= n); the elements above the diagonal, */
+/* with the array TAU, represent the orthogonal matrix Q as a */
+/* product of elementary reflectors (see Further Details). */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors (see Further */
+/* Details). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (M) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* Further Details */
+/* =============== */
+
+/* The matrix Q is represented as a product of elementary reflectors */
+
+/* Q = H(k) . . . H(2) H(1), where k = min(m,n). */
+
+/* Each H(i) has the form */
+
+/* H(i) = I - tau * v * v' */
+
+/* where tau is a real scalar, and v is a real vector with */
+/* v(1:i-1) = 0 and v(i) = 1; v(i+1:n) is stored on exit in A(i,i+1:n), */
+/* and tau in TAU(i). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGELQ2", &i__1);
+ return 0;
+ }
+
+ k = min(*m,*n);
+
+ i__1 = k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Generate elementary reflector H(i) to annihilate A(i,i+1:n) */
+
+ i__2 = *n - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfp_(&i__2, &a[i__ + i__ * a_dim1], &a[i__ + min(i__3, *n)* a_dim1]
+, lda, &tau[i__]);
+ if (i__ < *m) {
+
+/* Apply H(i) to A(i+1:m,i:n) from the right */
+
+ aii = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+ i__2 = *m - i__;
+ i__3 = *n - i__ + 1;
+ dlarf_("Right", &i__2, &i__3, &a[i__ + i__ * a_dim1], lda, &tau[
+ i__], &a[i__ + 1 + i__ * a_dim1], lda, &work[1]);
+ a[i__ + i__ * a_dim1] = aii;
+ }
+/* L10: */
+ }
+ return 0;
+
+/* End of DGELQ2 */
+
+} /* dgelq2_ */
diff --git a/3rdparty/clapack/src/dgelqf.c b/3rdparty/clapack/src/dgelqf.c
new file mode 100644
index 0000000..08bc818
--- /dev/null
+++ b/3rdparty/clapack/src/dgelqf.c
@@ -0,0 +1,251 @@
+/* dgelqf.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__3 = 3;
+static integer c__2 = 2;
+
+/* Subroutine */ int dgelqf_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *tau, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer i__, k, ib, nb, nx, iws, nbmin, iinfo;
+ extern /* Subroutine */ int dgelq2_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *), dlarfb_(char *,
+ char *, char *, char *, integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *, doublereal
+ *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwork, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGELQF computes an LQ factorization of a real M-by-N matrix A: */
+/* A = L * Q. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the M-by-N matrix A. */
+/* On exit, the elements on and below the diagonal of the array */
+/* contain the m-by-min(m,n) lower trapezoidal matrix L (L is */
+/* lower triangular if m <= n); the elements above the diagonal, */
+/* with the array TAU, represent the orthogonal matrix Q as a */
+/* product of elementary reflectors (see Further Details). */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors (see Further */
+/* Details). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,M). */
+/* For optimum performance LWORK >= M*NB, where NB is the */
+/* optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* Further Details */
+/* =============== */
+
+/* The matrix Q is represented as a product of elementary reflectors */
+
+/* Q = H(k) . . . H(2) H(1), where k = min(m,n). */
+
+/* Each H(i) has the form */
+
+/* H(i) = I - tau * v * v' */
+
+/* where tau is a real scalar, and v is a real vector with */
+/* v(1:i-1) = 0 and v(i) = 1; v(i+1:n) is stored on exit in A(i,i+1:n), */
+/* and tau in TAU(i). */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ nb = ilaenv_(&c__1, "DGELQF", " ", m, n, &c_n1, &c_n1);
+ lwkopt = *m * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ } else if (*lwork < max(1,*m) && ! lquery) {
+ *info = -7;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGELQF", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ k = min(*m,*n);
+ if (k == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ nx = 0;
+ iws = *m;
+ if (nb > 1 && nb < k) {
+
+/* Determine when to cross over from blocked to unblocked code. */
+
+/* Computing MAX */
+ i__1 = 0, i__2 = ilaenv_(&c__3, "DGELQF", " ", m, n, &c_n1, &c_n1);
+ nx = max(i__1,i__2);
+ if (nx < k) {
+
+/* Determine if workspace is large enough for blocked code. */
+
+ ldwork = *m;
+ iws = ldwork * nb;
+ if (*lwork < iws) {
+
+/* Not enough workspace to use optimal NB: reduce NB and */
+/* determine the minimum value of NB. */
+
+ nb = *lwork / ldwork;
+/* Computing MAX */
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DGELQF", " ", m, n, &c_n1, &
+ c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ }
+ }
+
+ if (nb >= nbmin && nb < k && nx < k) {
+
+/* Use blocked code initially */
+
+ i__1 = k - nx;
+ i__2 = nb;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+/* Computing MIN */
+ i__3 = k - i__ + 1;
+ ib = min(i__3,nb);
+
+/* Compute the LQ factorization of the current block */
+/* A(i:i+ib-1,i:n) */
+
+ i__3 = *n - i__ + 1;
+ dgelq2_(&ib, &i__3, &a[i__ + i__ * a_dim1], lda, &tau[i__], &work[
+ 1], &iinfo);
+ if (i__ + ib <= *m) {
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__3 = *n - i__ + 1;
+ dlarft_("Forward", "Rowwise", &i__3, &ib, &a[i__ + i__ *
+ a_dim1], lda, &tau[i__], &work[1], &ldwork);
+
+/* Apply H to A(i+ib:m,i:n) from the right */
+
+ i__3 = *m - i__ - ib + 1;
+ i__4 = *n - i__ + 1;
+ dlarfb_("Right", "No transpose", "Forward", "Rowwise", &i__3,
+ &i__4, &ib, &a[i__ + i__ * a_dim1], lda, &work[1], &
+ ldwork, &a[i__ + ib + i__ * a_dim1], lda, &work[ib +
+ 1], &ldwork);
+ }
+/* L10: */
+ }
+ } else {
+ i__ = 1;
+ }
+
+/* Use unblocked code to factor the last or only block. */
+
+ if (i__ <= k) {
+ i__2 = *m - i__ + 1;
+ i__1 = *n - i__ + 1;
+ dgelq2_(&i__2, &i__1, &a[i__ + i__ * a_dim1], lda, &tau[i__], &work[1]
+, &iinfo);
+ }
+
+ work[1] = (doublereal) iws;
+ return 0;
+
+/* End of DGELQF */
+
+} /* dgelqf_ */
diff --git a/3rdparty/clapack/src/dgeqr2.c b/3rdparty/clapack/src/dgeqr2.c
new file mode 100644
index 0000000..663388f
--- /dev/null
+++ b/3rdparty/clapack/src/dgeqr2.c
@@ -0,0 +1,161 @@
+/* dgeqr2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dgeqr2_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *tau, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, k;
+ doublereal aii;
+ extern /* Subroutine */ int dlarf_(char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *), dlarfp_(integer *, doublereal *,
+ doublereal *, integer *, doublereal *), xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEQR2 computes a QR factorization of a real m by n matrix A: */
+/* A = Q * R. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the m by n matrix A. */
+/* On exit, the elements on and above the diagonal of the array */
+/* contain the min(m,n) by n upper trapezoidal matrix R (R is */
+/* upper triangular if m >= n); the elements below the diagonal, */
+/* with the array TAU, represent the orthogonal matrix Q as a */
+/* product of elementary reflectors (see Further Details). */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors (see Further */
+/* Details). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (N) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* Further Details */
+/* =============== */
+
+/* The matrix Q is represented as a product of elementary reflectors */
+
+/* Q = H(1) H(2) . . . H(k), where k = min(m,n). */
+
+/* Each H(i) has the form */
+
+/* H(i) = I - tau * v * v' */
+
+/* where tau is a real scalar, and v is a real vector with */
+/* v(1:i-1) = 0 and v(i) = 1; v(i+1:m) is stored on exit in A(i+1:m,i), */
+/* and tau in TAU(i). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGEQR2", &i__1);
+ return 0;
+ }
+
+ k = min(*m,*n);
+
+ i__1 = k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Generate elementary reflector H(i) to annihilate A(i+1:m,i) */
+
+ i__2 = *m - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfp_(&i__2, &a[i__ + i__ * a_dim1], &a[min(i__3, *m)+ i__ * a_dim1]
+, &c__1, &tau[i__]);
+ if (i__ < *n) {
+
+/* Apply H(i) to A(i:m,i+1:n) from the left */
+
+ aii = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+ i__2 = *m - i__ + 1;
+ i__3 = *n - i__;
+ dlarf_("Left", &i__2, &i__3, &a[i__ + i__ * a_dim1], &c__1, &tau[
+ i__], &a[i__ + (i__ + 1) * a_dim1], lda, &work[1]);
+ a[i__ + i__ * a_dim1] = aii;
+ }
+/* L10: */
+ }
+ return 0;
+
+/* End of DGEQR2 */
+
+} /* dgeqr2_ */
diff --git a/3rdparty/clapack/src/dgeqrf.c b/3rdparty/clapack/src/dgeqrf.c
new file mode 100644
index 0000000..1062e27
--- /dev/null
+++ b/3rdparty/clapack/src/dgeqrf.c
@@ -0,0 +1,252 @@
+/* dgeqrf.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__3 = 3;
+static integer c__2 = 2;
+
+/* Subroutine */ int dgeqrf_(integer *m, integer *n, doublereal *a, integer *
+ lda, doublereal *tau, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer i__, k, ib, nb, nx, iws, nbmin, iinfo;
+ extern /* Subroutine */ int dgeqr2_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *), dlarfb_(char *,
+ char *, char *, char *, integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *, doublereal
+ *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwork, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGEQRF computes a QR factorization of a real M-by-N matrix A: */
+/* A = Q * R. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the M-by-N matrix A. */
+/* On exit, the elements on and above the diagonal of the array */
+/* contain the min(M,N)-by-N upper trapezoidal matrix R (R is */
+/* upper triangular if m >= n); the elements below the diagonal, */
+/* with the array TAU, represent the orthogonal matrix Q as a */
+/* product of min(m,n) elementary reflectors (see Further */
+/* Details). */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The scalar factors of the elementary reflectors (see Further */
+/* Details). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,N). */
+/* For optimum performance LWORK >= N*NB, where NB is */
+/* the optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* Further Details */
+/* =============== */
+
+/* The matrix Q is represented as a product of elementary reflectors */
+
+/* Q = H(1) H(2) . . . H(k), where k = min(m,n). */
+
+/* Each H(i) has the form */
+
+/* H(i) = I - tau * v * v' */
+
+/* where tau is a real scalar, and v is a real vector with */
+/* v(1:i-1) = 0 and v(i) = 1; v(i+1:m) is stored on exit in A(i+1:m,i), */
+/* and tau in TAU(i). */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ nb = ilaenv_(&c__1, "DGEQRF", " ", m, n, &c_n1, &c_n1);
+ lwkopt = *n * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ } else if (*lwork < max(1,*n) && ! lquery) {
+ *info = -7;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGEQRF", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ k = min(*m,*n);
+ if (k == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ nx = 0;
+ iws = *n;
+ if (nb > 1 && nb < k) {
+
+/* Determine when to cross over from blocked to unblocked code. */
+
+/* Computing MAX */
+ i__1 = 0, i__2 = ilaenv_(&c__3, "DGEQRF", " ", m, n, &c_n1, &c_n1);
+ nx = max(i__1,i__2);
+ if (nx < k) {
+
+/* Determine if workspace is large enough for blocked code. */
+
+ ldwork = *n;
+ iws = ldwork * nb;
+ if (*lwork < iws) {
+
+/* Not enough workspace to use optimal NB: reduce NB and */
+/* determine the minimum value of NB. */
+
+ nb = *lwork / ldwork;
+/* Computing MAX */
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DGEQRF", " ", m, n, &c_n1, &
+ c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ }
+ }
+
+ if (nb >= nbmin && nb < k && nx < k) {
+
+/* Use blocked code initially */
+
+ i__1 = k - nx;
+ i__2 = nb;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+/* Computing MIN */
+ i__3 = k - i__ + 1;
+ ib = min(i__3,nb);
+
+/* Compute the QR factorization of the current block */
+/* A(i:m,i:i+ib-1) */
+
+ i__3 = *m - i__ + 1;
+ dgeqr2_(&i__3, &ib, &a[i__ + i__ * a_dim1], lda, &tau[i__], &work[
+ 1], &iinfo);
+ if (i__ + ib <= *n) {
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__3 = *m - i__ + 1;
+ dlarft_("Forward", "Columnwise", &i__3, &ib, &a[i__ + i__ *
+ a_dim1], lda, &tau[i__], &work[1], &ldwork);
+
+/* Apply H' to A(i:m,i+ib:n) from the left */
+
+ i__3 = *m - i__ + 1;
+ i__4 = *n - i__ - ib + 1;
+ dlarfb_("Left", "Transpose", "Forward", "Columnwise", &i__3, &
+ i__4, &ib, &a[i__ + i__ * a_dim1], lda, &work[1], &
+ ldwork, &a[i__ + (i__ + ib) * a_dim1], lda, &work[ib
+ + 1], &ldwork);
+ }
+/* L10: */
+ }
+ } else {
+ i__ = 1;
+ }
+
+/* Use unblocked code to factor the last or only block. */
+
+ if (i__ <= k) {
+ i__2 = *m - i__ + 1;
+ i__1 = *n - i__ + 1;
+ dgeqr2_(&i__2, &i__1, &a[i__ + i__ * a_dim1], lda, &tau[i__], &work[1]
+, &iinfo);
+ }
+
+ work[1] = (doublereal) iws;
+ return 0;
+
+/* End of DGEQRF */
+
+} /* dgeqrf_ */
diff --git a/3rdparty/clapack/src/dgesdd.c b/3rdparty/clapack/src/dgesdd.c
new file mode 100644
index 0000000..9a12830
--- /dev/null
+++ b/3rdparty/clapack/src/dgesdd.c
@@ -0,0 +1,1609 @@
+/* dgesdd.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__0 = 0;
+static doublereal c_b227 = 0.;
+static doublereal c_b248 = 1.;
+
+/* Subroutine */ int dgesdd_(char *jobz, integer *m, integer *n, doublereal *
+ a, integer *lda, doublereal *s, doublereal *u, integer *ldu,
+ doublereal *vt, integer *ldvt, doublereal *work, integer *lwork,
+ integer *iwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, u_dim1, u_offset, vt_dim1, vt_offset, i__1,
+ i__2, i__3;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer i__, ie, il, ir, iu, blk;
+ doublereal dum[1], eps;
+ integer ivt, iscl;
+ doublereal anrm;
+ integer idum[1], ierr, itau;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ extern logical lsame_(char *, char *);
+ integer chunk, minmn, wrkbl, itaup, itauq, mnthr;
+ logical wntqa;
+ integer nwork;
+ logical wntqn, wntqo, wntqs;
+ extern /* Subroutine */ int dbdsdc_(char *, char *, integer *, doublereal
+ *, doublereal *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *, integer *), dgebrd_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *, integer *);
+ extern doublereal dlamch_(char *), dlange_(char *, integer *,
+ integer *, doublereal *, integer *, doublereal *);
+ integer bdspac;
+ extern /* Subroutine */ int dgelqf_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *, integer *),
+ dlascl_(char *, integer *, integer *, doublereal *, doublereal *,
+ integer *, integer *, doublereal *, integer *, integer *),
+ dgeqrf_(integer *, integer *, doublereal *, integer *,
+ doublereal *, doublereal *, integer *, integer *), dlacpy_(char *,
+ integer *, integer *, doublereal *, integer *, doublereal *,
+ integer *), dlaset_(char *, integer *, integer *,
+ doublereal *, doublereal *, doublereal *, integer *),
+ xerbla_(char *, integer *), dorgbr_(char *, integer *,
+ integer *, integer *, doublereal *, integer *, doublereal *,
+ doublereal *, integer *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ doublereal bignum;
+ extern /* Subroutine */ int dormbr_(char *, char *, char *, integer *,
+ integer *, integer *, doublereal *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, integer *), dorglq_(integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ integer *), dorgqr_(integer *, integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *, integer *);
+ integer ldwrkl, ldwrkr, minwrk, ldwrku, maxwrk, ldwkvt;
+ doublereal smlnum;
+ logical wntqas, lquery;
+
+
+/* -- LAPACK driver routine (version 3.2.1) -- */
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+/* March 2009 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGESDD computes the singular value decomposition (SVD) of a real */
+/* M-by-N matrix A, optionally computing the left and right singular */
+/* vectors. If singular vectors are desired, it uses a */
+/* divide-and-conquer algorithm. */
+
+/* The SVD is written */
+
+/* A = U * SIGMA * transpose(V) */
+
+/* where SIGMA is an M-by-N matrix which is zero except for its */
+/* min(m,n) diagonal elements, U is an M-by-M orthogonal matrix, and */
+/* V is an N-by-N orthogonal matrix. The diagonal elements of SIGMA */
+/* are the singular values of A; they are real and non-negative, and */
+/* are returned in descending order. The first min(m,n) columns of */
+/* U and V are the left and right singular vectors of A. */
+
+/* Note that the routine returns VT = V**T, not V. */
+
+/* The divide and conquer algorithm makes very mild assumptions about */
+/* floating point arithmetic. It will work on machines with a guard */
+/* digit in add/subtract, or on those binary machines without guard */
+/* digits which subtract like the Cray X-MP, Cray Y-MP, Cray C-90, or */
+/* Cray-2. It could conceivably fail on hexadecimal or decimal machines */
+/* without guard digits, but we know of none. */
+
+/* Arguments */
+/* ========= */
+
+/* JOBZ (input) CHARACTER*1 */
+/* Specifies options for computing all or part of the matrix U: */
+/* = 'A': all M columns of U and all N rows of V**T are */
+/* returned in the arrays U and VT; */
+/* = 'S': the first min(M,N) columns of U and the first */
+/* min(M,N) rows of V**T are returned in the arrays U */
+/* and VT; */
+/* = 'O': If M >= N, the first N columns of U are overwritten */
+/* on the array A and all rows of V**T are returned in */
+/* the array VT; */
+/* otherwise, all columns of U are returned in the */
+/* array U and the first M rows of V**T are overwritten */
+/* in the array A; */
+/* = 'N': no columns of U or rows of V**T are computed. */
+
+/* M (input) INTEGER */
+/* The number of rows of the input matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the input matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the M-by-N matrix A. */
+/* On exit, */
+/* if JOBZ = 'O', A is overwritten with the first N columns */
+/* of U (the left singular vectors, stored */
+/* columnwise) if M >= N; */
+/* A is overwritten with the first M rows */
+/* of V**T (the right singular vectors, stored */
+/* rowwise) otherwise. */
+/* if JOBZ .ne. 'O', the contents of A are destroyed. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* S (output) DOUBLE PRECISION array, dimension (min(M,N)) */
+/* The singular values of A, sorted so that S(i) >= S(i+1). */
+
+/* U (output) DOUBLE PRECISION array, dimension (LDU,UCOL) */
+/* UCOL = M if JOBZ = 'A' or JOBZ = 'O' and M < N; */
+/* UCOL = min(M,N) if JOBZ = 'S'. */
+/* If JOBZ = 'A' or JOBZ = 'O' and M < N, U contains the M-by-M */
+/* orthogonal matrix U; */
+/* if JOBZ = 'S', U contains the first min(M,N) columns of U */
+/* (the left singular vectors, stored columnwise); */
+/* if JOBZ = 'O' and M >= N, or JOBZ = 'N', U is not referenced. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= 1; if */
+/* JOBZ = 'S' or 'A' or JOBZ = 'O' and M < N, LDU >= M. */
+
+/* VT (output) DOUBLE PRECISION array, dimension (LDVT,N) */
+/* If JOBZ = 'A' or JOBZ = 'O' and M >= N, VT contains the */
+/* N-by-N orthogonal matrix V**T; */
+/* if JOBZ = 'S', VT contains the first min(M,N) rows of */
+/* V**T (the right singular vectors, stored rowwise); */
+/* if JOBZ = 'O' and M < N, or JOBZ = 'N', VT is not referenced. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. LDVT >= 1; if */
+/* JOBZ = 'A' or JOBZ = 'O' and M >= N, LDVT >= N; */
+/* if JOBZ = 'S', LDVT >= min(M,N). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK; */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= 1. */
+/* If JOBZ = 'N', */
+/* LWORK >= 3*min(M,N) + max(max(M,N),7*min(M,N)). */
+/* If JOBZ = 'O', */
+/* LWORK >= 3*min(M,N) + */
+/* max(max(M,N),5*min(M,N)*min(M,N)+4*min(M,N)). */
+/* If JOBZ = 'S' or 'A' */
+/* LWORK >= 3*min(M,N) + */
+/* max(max(M,N),4*min(M,N)*min(M,N)+4*min(M,N)). */
+/* For good performance, LWORK should generally be larger. */
+/* If LWORK = -1 but other input arguments are legal, WORK(1) */
+/* returns the optimal LWORK. */
+
+/* IWORK (workspace) INTEGER array, dimension (8*min(M,N)) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: DBDSDC did not converge, updating process failed. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --s;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ --work;
+ --iwork;
+
+ /* Function Body */
+ *info = 0;
+ minmn = min(*m,*n);
+ wntqa = lsame_(jobz, "A");
+ wntqs = lsame_(jobz, "S");
+ wntqas = wntqa || wntqs;
+ wntqo = lsame_(jobz, "O");
+ wntqn = lsame_(jobz, "N");
+ lquery = *lwork == -1;
+
+ if (! (wntqa || wntqs || wntqo || wntqn)) {
+ *info = -1;
+ } else if (*m < 0) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*lda < max(1,*m)) {
+ *info = -5;
+ } else if (*ldu < 1 || wntqas && *ldu < *m || wntqo && *m < *n && *ldu < *
+ m) {
+ *info = -8;
+ } else if (*ldvt < 1 || wntqa && *ldvt < *n || wntqs && *ldvt < minmn ||
+ wntqo && *m >= *n && *ldvt < *n) {
+ *info = -10;
+ }
+
+/* Compute workspace */
+/* (Note: Comments in the code beginning "Workspace:" describe the */
+/* minimal amount of workspace needed at that point in the code, */
+/* as well as the preferred amount for good performance. */
+/* NB refers to the optimal block size for the immediately */
+/* following subroutine, as returned by ILAENV.) */
+
+ if (*info == 0) {
+ minwrk = 1;
+ maxwrk = 1;
+ if (*m >= *n && minmn > 0) {
+
+/* Compute space needed for DBDSDC */
+
+ mnthr = (integer) (minmn * 11. / 6.);
+ if (wntqn) {
+ bdspac = *n * 7;
+ } else {
+ bdspac = *n * 3 * *n + (*n << 2);
+ }
+ if (*m >= mnthr) {
+ if (wntqn) {
+
+/* Path 1 (M much larger than N, JOBZ='N') */
+
+ wrkbl = *n + *n * ilaenv_(&c__1, "DGEQRF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + (*n << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", n, n, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n;
+ maxwrk = max(i__1,i__2);
+ minwrk = bdspac + *n;
+ } else if (wntqo) {
+
+/* Path 2 (M much larger than N, JOBZ='O') */
+
+ wrkbl = *n + *n * ilaenv_(&c__1, "DGEQRF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n + *n * ilaenv_(&c__1, "DORGQR",
+ " ", m, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + (*n << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", n, n, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "QLN", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + (*n << 1) * *n;
+ minwrk = bdspac + (*n << 1) * *n + *n * 3;
+ } else if (wntqs) {
+
+/* Path 3 (M much larger than N, JOBZ='S') */
+
+ wrkbl = *n + *n * ilaenv_(&c__1, "DGEQRF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n + *n * ilaenv_(&c__1, "DORGQR",
+ " ", m, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + (*n << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", n, n, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "QLN", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *n * *n;
+ minwrk = bdspac + *n * *n + *n * 3;
+ } else if (wntqa) {
+
+/* Path 4 (M much larger than N, JOBZ='A') */
+
+ wrkbl = *n + *n * ilaenv_(&c__1, "DGEQRF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n + *m * ilaenv_(&c__1, "DORGQR",
+ " ", m, m, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + (*n << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", n, n, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "QLN", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *n * *n;
+ minwrk = bdspac + *n * *n + *n * 3;
+ }
+ } else {
+
+/* Path 5 (M at least N, but not much larger) */
+
+ wrkbl = *n * 3 + (*m + *n) * ilaenv_(&c__1, "DGEBRD", " ", m,
+ n, &c_n1, &c_n1);
+ if (wntqn) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *n * 3 + max(*m,bdspac);
+ } else if (wntqo) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *m * *n;
+/* Computing MAX */
+ i__1 = *m, i__2 = *n * *n + bdspac;
+ minwrk = *n * 3 + max(i__1,i__2);
+ } else if (wntqs) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *n * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *n * 3 + max(*m,bdspac);
+ } else if (wntqa) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *n * 3 + *n * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = maxwrk, i__2 = bdspac + *n * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *n * 3 + max(*m,bdspac);
+ }
+ }
+ } else if (minmn > 0) {
+
+/* Compute space needed for DBDSDC */
+
+ mnthr = (integer) (minmn * 11. / 6.);
+ if (wntqn) {
+ bdspac = *m * 7;
+ } else {
+ bdspac = *m * 3 * *m + (*m << 2);
+ }
+ if (*n >= mnthr) {
+ if (wntqn) {
+
+/* Path 1t (N much larger than M, JOBZ='N') */
+
+ wrkbl = *m + *m * ilaenv_(&c__1, "DGELQF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + (*m << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", m, m, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m;
+ maxwrk = max(i__1,i__2);
+ minwrk = bdspac + *m;
+ } else if (wntqo) {
+
+/* Path 2t (N much larger than M, JOBZ='O') */
+
+ wrkbl = *m + *m * ilaenv_(&c__1, "DGELQF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m + *m * ilaenv_(&c__1, "DORGLQ",
+ " ", m, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + (*m << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", m, m, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + (*m << 1) * *m;
+ minwrk = bdspac + (*m << 1) * *m + *m * 3;
+ } else if (wntqs) {
+
+/* Path 3t (N much larger than M, JOBZ='S') */
+
+ wrkbl = *m + *m * ilaenv_(&c__1, "DGELQF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m + *m * ilaenv_(&c__1, "DORGLQ",
+ " ", m, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + (*m << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", m, m, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *m * *m;
+ minwrk = bdspac + *m * *m + *m * 3;
+ } else if (wntqa) {
+
+/* Path 4t (N much larger than M, JOBZ='A') */
+
+ wrkbl = *m + *m * ilaenv_(&c__1, "DGELQF", " ", m, n, &
+ c_n1, &c_n1);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m + *n * ilaenv_(&c__1, "DORGLQ",
+ " ", n, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + (*m << 1) * ilaenv_(&c__1,
+ "DGEBRD", " ", m, m, &c_n1, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", m, m, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *m * *m;
+ minwrk = bdspac + *m * *m + *m * 3;
+ }
+ } else {
+
+/* Path 5t (N greater than M, but not much larger) */
+
+ wrkbl = *m * 3 + (*m + *n) * ilaenv_(&c__1, "DGEBRD", " ", m,
+ n, &c_n1, &c_n1);
+ if (wntqn) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *m * 3 + max(*n,bdspac);
+ } else if (wntqo) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", m, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ wrkbl = max(i__1,i__2);
+ maxwrk = wrkbl + *m * *n;
+/* Computing MAX */
+ i__1 = *n, i__2 = *m * *m + bdspac;
+ minwrk = *m * 3 + max(i__1,i__2);
+ } else if (wntqs) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", m, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *m * 3 + max(*n,bdspac);
+ } else if (wntqa) {
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "QLN", m, m, n, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = *m * 3 + *m * ilaenv_(&c__1, "DORMBR"
+, "PRT", n, n, m, &c_n1);
+ wrkbl = max(i__1,i__2);
+/* Computing MAX */
+ i__1 = wrkbl, i__2 = bdspac + *m * 3;
+ maxwrk = max(i__1,i__2);
+ minwrk = *m * 3 + max(*n,bdspac);
+ }
+ }
+ }
+ maxwrk = max(maxwrk,minwrk);
+ work[1] = (doublereal) maxwrk;
+
+ if (*lwork < minwrk && ! lquery) {
+ *info = -12;
+ }
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGESDD", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+/* Get machine constants */
+
+ eps = dlamch_("P");
+ smlnum = sqrt(dlamch_("S")) / eps;
+ bignum = 1. / smlnum;
+
+/* Scale A if max element outside range [SMLNUM,BIGNUM] */
+
+ anrm = dlange_("M", m, n, &a[a_offset], lda, dum);
+ iscl = 0;
+ if (anrm > 0. && anrm < smlnum) {
+ iscl = 1;
+ dlascl_("G", &c__0, &c__0, &anrm, &smlnum, m, n, &a[a_offset], lda, &
+ ierr);
+ } else if (anrm > bignum) {
+ iscl = 1;
+ dlascl_("G", &c__0, &c__0, &anrm, &bignum, m, n, &a[a_offset], lda, &
+ ierr);
+ }
+
+ if (*m >= *n) {
+
+/* A has at least as many rows as columns. If A has sufficiently */
+/* more rows than columns, first reduce using the QR */
+/* decomposition (if sufficient workspace available) */
+
+ if (*m >= mnthr) {
+
+ if (wntqn) {
+
+/* Path 1 (M much larger than N, JOBZ='N') */
+/* No singular vectors to be computed */
+
+ itau = 1;
+ nwork = itau + *n;
+
+/* Compute A=Q*R */
+/* (Workspace: need 2*N, prefer N+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgeqrf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__1, &ierr);
+
+/* Zero out below R */
+
+ i__1 = *n - 1;
+ i__2 = *n - 1;
+ dlaset_("L", &i__1, &i__2, &c_b227, &c_b227, &a[a_dim1 + 2],
+ lda);
+ ie = 1;
+ itauq = ie + *n;
+ itaup = itauq + *n;
+ nwork = itaup + *n;
+
+/* Bidiagonalize R in A */
+/* (Workspace: need 4*N, prefer 3*N+2*N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgebrd_(n, n, &a[a_offset], lda, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__1, &ierr);
+ nwork = ie + *n;
+
+/* Perform bidiagonal SVD, computing singular values only */
+/* (Workspace: need N+BDSPAC) */
+
+ dbdsdc_("U", "N", n, &s[1], &work[ie], dum, &c__1, dum, &c__1,
+ dum, idum, &work[nwork], &iwork[1], info);
+
+ } else if (wntqo) {
+
+/* Path 2 (M much larger than N, JOBZ = 'O') */
+/* N left singular vectors to be overwritten on A and */
+/* N right singular vectors to be computed in VT */
+
+ ir = 1;
+
+/* WORK(IR) is LDWRKR by N */
+
+ if (*lwork >= *lda * *n + *n * *n + *n * 3 + bdspac) {
+ ldwrkr = *lda;
+ } else {
+ ldwrkr = (*lwork - *n * *n - *n * 3 - bdspac) / *n;
+ }
+ itau = ir + ldwrkr * *n;
+ nwork = itau + *n;
+
+/* Compute A=Q*R */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgeqrf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__1, &ierr);
+
+/* Copy R to WORK(IR), zeroing out below it */
+
+ dlacpy_("U", n, n, &a[a_offset], lda, &work[ir], &ldwrkr);
+ i__1 = *n - 1;
+ i__2 = *n - 1;
+ dlaset_("L", &i__1, &i__2, &c_b227, &c_b227, &work[ir + 1], &
+ ldwrkr);
+
+/* Generate Q in A */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dorgqr_(m, n, n, &a[a_offset], lda, &work[itau], &work[nwork],
+ &i__1, &ierr);
+ ie = itau;
+ itauq = ie + *n;
+ itaup = itauq + *n;
+ nwork = itaup + *n;
+
+/* Bidiagonalize R in VT, copying result to WORK(IR) */
+/* (Workspace: need N*N+4*N, prefer N*N+3*N+2*N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgebrd_(n, n, &work[ir], &ldwrkr, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__1, &ierr);
+
+/* WORK(IU) is N by N */
+
+ iu = nwork;
+ nwork = iu + *n * *n;
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in WORK(IU) and computing right */
+/* singular vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+N*N+BDSPAC) */
+
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &work[iu], n, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite WORK(IU) by left singular vectors of R */
+/* and VT by right singular vectors of R */
+/* (Workspace: need 2*N*N+3*N, prefer 2*N*N+2*N+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", n, n, n, &work[ir], &ldwrkr, &work[
+ itauq], &work[iu], n, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, n, &work[ir], &ldwrkr, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__1, &
+ ierr);
+
+/* Multiply Q in A by left singular vectors of R in */
+/* WORK(IU), storing result in WORK(IR) and copying to A */
+/* (Workspace: need 2*N*N, prefer N*N+M*N) */
+
+ i__1 = *m;
+ i__2 = ldwrkr;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ +=
+ i__2) {
+/* Computing MIN */
+ i__3 = *m - i__ + 1;
+ chunk = min(i__3,ldwrkr);
+ dgemm_("N", "N", &chunk, n, n, &c_b248, &a[i__ + a_dim1],
+ lda, &work[iu], n, &c_b227, &work[ir], &ldwrkr);
+ dlacpy_("F", &chunk, n, &work[ir], &ldwrkr, &a[i__ +
+ a_dim1], lda);
+/* L10: */
+ }
+
+ } else if (wntqs) {
+
+/* Path 3 (M much larger than N, JOBZ='S') */
+/* N left singular vectors to be computed in U and */
+/* N right singular vectors to be computed in VT */
+
+ ir = 1;
+
+/* WORK(IR) is N by N */
+
+ ldwrkr = *n;
+ itau = ir + ldwrkr * *n;
+ nwork = itau + *n;
+
+/* Compute A=Q*R */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgeqrf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__2, &ierr);
+
+/* Copy R to WORK(IR), zeroing out below it */
+
+ dlacpy_("U", n, n, &a[a_offset], lda, &work[ir], &ldwrkr);
+ i__2 = *n - 1;
+ i__1 = *n - 1;
+ dlaset_("L", &i__2, &i__1, &c_b227, &c_b227, &work[ir + 1], &
+ ldwrkr);
+
+/* Generate Q in A */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dorgqr_(m, n, n, &a[a_offset], lda, &work[itau], &work[nwork],
+ &i__2, &ierr);
+ ie = itau;
+ itauq = ie + *n;
+ itaup = itauq + *n;
+ nwork = itaup + *n;
+
+/* Bidiagonalize R in WORK(IR) */
+/* (Workspace: need N*N+4*N, prefer N*N+3*N+2*N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(n, n, &work[ir], &ldwrkr, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__2, &ierr);
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagoal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+BDSPAC) */
+
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite U by left singular vectors of R and VT */
+/* by right singular vectors of R */
+/* (Workspace: need N*N+3*N, prefer N*N+2*N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", n, n, n, &work[ir], &ldwrkr, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__2, &ierr);
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, n, &work[ir], &ldwrkr, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__2, &
+ ierr);
+
+/* Multiply Q in A by left singular vectors of R in */
+/* WORK(IR), storing result in U */
+/* (Workspace: need N*N) */
+
+ dlacpy_("F", n, n, &u[u_offset], ldu, &work[ir], &ldwrkr);
+ dgemm_("N", "N", m, n, n, &c_b248, &a[a_offset], lda, &work[
+ ir], &ldwrkr, &c_b227, &u[u_offset], ldu);
+
+ } else if (wntqa) {
+
+/* Path 4 (M much larger than N, JOBZ='A') */
+/* M left singular vectors to be computed in U and */
+/* N right singular vectors to be computed in VT */
+
+ iu = 1;
+
+/* WORK(IU) is N by N */
+
+ ldwrku = *n;
+ itau = iu + ldwrku * *n;
+ nwork = itau + *n;
+
+/* Compute A=Q*R, copying result to U */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgeqrf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__2, &ierr);
+ dlacpy_("L", m, n, &a[a_offset], lda, &u[u_offset], ldu);
+
+/* Generate Q in U */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+ i__2 = *lwork - nwork + 1;
+ dorgqr_(m, m, n, &u[u_offset], ldu, &work[itau], &work[nwork],
+ &i__2, &ierr);
+
+/* Produce R in A, zeroing out other entries */
+
+ i__2 = *n - 1;
+ i__1 = *n - 1;
+ dlaset_("L", &i__2, &i__1, &c_b227, &c_b227, &a[a_dim1 + 2],
+ lda);
+ ie = itau;
+ itauq = ie + *n;
+ itaup = itauq + *n;
+ nwork = itaup + *n;
+
+/* Bidiagonalize R in A */
+/* (Workspace: need N*N+4*N, prefer N*N+3*N+2*N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(n, n, &a[a_offset], lda, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__2, &ierr);
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in WORK(IU) and computing right */
+/* singular vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+N*N+BDSPAC) */
+
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &work[iu], n, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite WORK(IU) by left singular vectors of R and VT */
+/* by right singular vectors of R */
+/* (Workspace: need N*N+3*N, prefer N*N+2*N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", n, n, n, &a[a_offset], lda, &work[
+ itauq], &work[iu], &ldwrku, &work[nwork], &i__2, &
+ ierr);
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, n, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__2, &
+ ierr);
+
+/* Multiply Q in U by left singular vectors of R in */
+/* WORK(IU), storing result in A */
+/* (Workspace: need N*N) */
+
+ dgemm_("N", "N", m, n, n, &c_b248, &u[u_offset], ldu, &work[
+ iu], &ldwrku, &c_b227, &a[a_offset], lda);
+
+/* Copy left singular vectors of A from A to U */
+
+ dlacpy_("F", m, n, &a[a_offset], lda, &u[u_offset], ldu);
+
+ }
+
+ } else {
+
+/* M .LT. MNTHR */
+
+/* Path 5 (M at least N, but not much larger) */
+/* Reduce to bidiagonal form without QR decomposition */
+
+ ie = 1;
+ itauq = ie + *n;
+ itaup = itauq + *n;
+ nwork = itaup + *n;
+
+/* Bidiagonalize A */
+/* (Workspace: need 3*N+M, prefer 3*N+(M+N)*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(m, n, &a[a_offset], lda, &s[1], &work[ie], &work[itauq], &
+ work[itaup], &work[nwork], &i__2, &ierr);
+ if (wntqn) {
+
+/* Perform bidiagonal SVD, only computing singular values */
+/* (Workspace: need N+BDSPAC) */
+
+ dbdsdc_("U", "N", n, &s[1], &work[ie], dum, &c__1, dum, &c__1,
+ dum, idum, &work[nwork], &iwork[1], info);
+ } else if (wntqo) {
+ iu = nwork;
+ if (*lwork >= *m * *n + *n * 3 + bdspac) {
+
+/* WORK( IU ) is M by N */
+
+ ldwrku = *m;
+ nwork = iu + ldwrku * *n;
+ dlaset_("F", m, n, &c_b227, &c_b227, &work[iu], &ldwrku);
+ } else {
+
+/* WORK( IU ) is N by N */
+
+ ldwrku = *n;
+ nwork = iu + ldwrku * *n;
+
+/* WORK(IR) is LDWRKR by N */
+
+ ir = nwork;
+ ldwrkr = (*lwork - *n * *n - *n * 3) / *n;
+ }
+ nwork = iu + ldwrku * *n;
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in WORK(IU) and computing right */
+/* singular vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+N*N+BDSPAC) */
+
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &work[iu], &ldwrku, &
+ vt[vt_offset], ldvt, dum, idum, &work[nwork], &iwork[
+ 1], info);
+
+/* Overwrite VT by right singular vectors of A */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, n, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__2, &
+ ierr);
+
+ if (*lwork >= *m * *n + *n * 3 + bdspac) {
+
+/* Overwrite WORK(IU) by left singular vectors of A */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, n, n, &a[a_offset], lda, &work[
+ itauq], &work[iu], &ldwrku, &work[nwork], &i__2, &
+ ierr);
+
+/* Copy left singular vectors of A from WORK(IU) to A */
+
+ dlacpy_("F", m, n, &work[iu], &ldwrku, &a[a_offset], lda);
+ } else {
+
+/* Generate Q in A */
+/* (Workspace: need N*N+2*N, prefer N*N+N+N*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dorgbr_("Q", m, n, n, &a[a_offset], lda, &work[itauq], &
+ work[nwork], &i__2, &ierr);
+
+/* Multiply Q in A by left singular vectors of */
+/* bidiagonal matrix in WORK(IU), storing result in */
+/* WORK(IR) and copying to A */
+/* (Workspace: need 2*N*N, prefer N*N+M*N) */
+
+ i__2 = *m;
+ i__1 = ldwrkr;
+ for (i__ = 1; i__1 < 0 ? i__ >= i__2 : i__ <= i__2; i__ +=
+ i__1) {
+/* Computing MIN */
+ i__3 = *m - i__ + 1;
+ chunk = min(i__3,ldwrkr);
+ dgemm_("N", "N", &chunk, n, n, &c_b248, &a[i__ +
+ a_dim1], lda, &work[iu], &ldwrku, &c_b227, &
+ work[ir], &ldwrkr);
+ dlacpy_("F", &chunk, n, &work[ir], &ldwrkr, &a[i__ +
+ a_dim1], lda);
+/* L20: */
+ }
+ }
+
+ } else if (wntqs) {
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+BDSPAC) */
+
+ dlaset_("F", m, n, &c_b227, &c_b227, &u[u_offset], ldu);
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite U by left singular vectors of A and VT */
+/* by right singular vectors of A */
+/* (Workspace: need 3*N, prefer 2*N+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, n, n, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, n, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__1, &
+ ierr);
+ } else if (wntqa) {
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need N+BDSPAC) */
+
+ dlaset_("F", m, m, &c_b227, &c_b227, &u[u_offset], ldu);
+ dbdsdc_("U", "I", n, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Set the right corner of U to identity matrix */
+
+ if (*m > *n) {
+ i__1 = *m - *n;
+ i__2 = *m - *n;
+ dlaset_("F", &i__1, &i__2, &c_b227, &c_b248, &u[*n + 1 + (
+ *n + 1) * u_dim1], ldu);
+ }
+
+/* Overwrite U by left singular vectors of A and VT */
+/* by right singular vectors of A */
+/* (Workspace: need N*N+2*N+M, prefer N*N+2*N+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, n, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, m, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__1, &
+ ierr);
+ }
+
+ }
+
+ } else {
+
+/* A has more columns than rows. If A has sufficiently more */
+/* columns than rows, first reduce using the LQ decomposition (if */
+/* sufficient workspace available) */
+
+ if (*n >= mnthr) {
+
+ if (wntqn) {
+
+/* Path 1t (N much larger than M, JOBZ='N') */
+/* No singular vectors to be computed */
+
+ itau = 1;
+ nwork = itau + *m;
+
+/* Compute A=L*Q */
+/* (Workspace: need 2*M, prefer M+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgelqf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__1, &ierr);
+
+/* Zero out above L */
+
+ i__1 = *m - 1;
+ i__2 = *m - 1;
+ dlaset_("U", &i__1, &i__2, &c_b227, &c_b227, &a[(a_dim1 << 1)
+ + 1], lda);
+ ie = 1;
+ itauq = ie + *m;
+ itaup = itauq + *m;
+ nwork = itaup + *m;
+
+/* Bidiagonalize L in A */
+/* (Workspace: need 4*M, prefer 3*M+2*M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgebrd_(m, m, &a[a_offset], lda, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__1, &ierr);
+ nwork = ie + *m;
+
+/* Perform bidiagonal SVD, computing singular values only */
+/* (Workspace: need M+BDSPAC) */
+
+ dbdsdc_("U", "N", m, &s[1], &work[ie], dum, &c__1, dum, &c__1,
+ dum, idum, &work[nwork], &iwork[1], info);
+
+ } else if (wntqo) {
+
+/* Path 2t (N much larger than M, JOBZ='O') */
+/* M right singular vectors to be overwritten on A and */
+/* M left singular vectors to be computed in U */
+
+ ivt = 1;
+
+/* IVT is M by M */
+
+ il = ivt + *m * *m;
+ if (*lwork >= *m * *n + *m * *m + *m * 3 + bdspac) {
+
+/* WORK(IL) is M by N */
+
+ ldwrkl = *m;
+ chunk = *n;
+ } else {
+ ldwrkl = *m;
+ chunk = (*lwork - *m * *m) / *m;
+ }
+ itau = il + ldwrkl * *m;
+ nwork = itau + *m;
+
+/* Compute A=L*Q */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgelqf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__1, &ierr);
+
+/* Copy L to WORK(IL), zeroing about above it */
+
+ dlacpy_("L", m, m, &a[a_offset], lda, &work[il], &ldwrkl);
+ i__1 = *m - 1;
+ i__2 = *m - 1;
+ dlaset_("U", &i__1, &i__2, &c_b227, &c_b227, &work[il +
+ ldwrkl], &ldwrkl);
+
+/* Generate Q in A */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dorglq_(m, n, m, &a[a_offset], lda, &work[itau], &work[nwork],
+ &i__1, &ierr);
+ ie = itau;
+ itauq = ie + *m;
+ itaup = itauq + *m;
+ nwork = itaup + *m;
+
+/* Bidiagonalize L in WORK(IL) */
+/* (Workspace: need M*M+4*M, prefer M*M+3*M+2*M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dgebrd_(m, m, &work[il], &ldwrkl, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__1, &ierr);
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U, and computing right singular */
+/* vectors of bidiagonal matrix in WORK(IVT) */
+/* (Workspace: need M+M*M+BDSPAC) */
+
+ dbdsdc_("U", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &
+ work[ivt], m, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite U by left singular vectors of L and WORK(IVT) */
+/* by right singular vectors of L */
+/* (Workspace: need 2*M*M+3*M, prefer 2*M*M+2*M+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, m, &work[il], &ldwrkl, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", m, m, m, &work[il], &ldwrkl, &work[
+ itaup], &work[ivt], m, &work[nwork], &i__1, &ierr);
+
+/* Multiply right singular vectors of L in WORK(IVT) by Q */
+/* in A, storing result in WORK(IL) and copying to A */
+/* (Workspace: need 2*M*M, prefer M*M+M*N) */
+
+ i__1 = *n;
+ i__2 = chunk;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ +=
+ i__2) {
+/* Computing MIN */
+ i__3 = *n - i__ + 1;
+ blk = min(i__3,chunk);
+ dgemm_("N", "N", m, &blk, m, &c_b248, &work[ivt], m, &a[
+ i__ * a_dim1 + 1], lda, &c_b227, &work[il], &
+ ldwrkl);
+ dlacpy_("F", m, &blk, &work[il], &ldwrkl, &a[i__ * a_dim1
+ + 1], lda);
+/* L30: */
+ }
+
+ } else if (wntqs) {
+
+/* Path 3t (N much larger than M, JOBZ='S') */
+/* M right singular vectors to be computed in VT and */
+/* M left singular vectors to be computed in U */
+
+ il = 1;
+
+/* WORK(IL) is M by M */
+
+ ldwrkl = *m;
+ itau = il + ldwrkl * *m;
+ nwork = itau + *m;
+
+/* Compute A=L*Q */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgelqf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__2, &ierr);
+
+/* Copy L to WORK(IL), zeroing out above it */
+
+ dlacpy_("L", m, m, &a[a_offset], lda, &work[il], &ldwrkl);
+ i__2 = *m - 1;
+ i__1 = *m - 1;
+ dlaset_("U", &i__2, &i__1, &c_b227, &c_b227, &work[il +
+ ldwrkl], &ldwrkl);
+
+/* Generate Q in A */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dorglq_(m, n, m, &a[a_offset], lda, &work[itau], &work[nwork],
+ &i__2, &ierr);
+ ie = itau;
+ itauq = ie + *m;
+ itaup = itauq + *m;
+ nwork = itaup + *m;
+
+/* Bidiagonalize L in WORK(IU), copying result to U */
+/* (Workspace: need M*M+4*M, prefer M*M+3*M+2*M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(m, m, &work[il], &ldwrkl, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__2, &ierr);
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need M+BDSPAC) */
+
+ dbdsdc_("U", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite U by left singular vectors of L and VT */
+/* by right singular vectors of L */
+/* (Workspace: need M*M+3*M, prefer M*M+2*M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, m, &work[il], &ldwrkl, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__2, &ierr);
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", m, m, m, &work[il], &ldwrkl, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__2, &
+ ierr);
+
+/* Multiply right singular vectors of L in WORK(IL) by */
+/* Q in A, storing result in VT */
+/* (Workspace: need M*M) */
+
+ dlacpy_("F", m, m, &vt[vt_offset], ldvt, &work[il], &ldwrkl);
+ dgemm_("N", "N", m, n, m, &c_b248, &work[il], &ldwrkl, &a[
+ a_offset], lda, &c_b227, &vt[vt_offset], ldvt);
+
+ } else if (wntqa) {
+
+/* Path 4t (N much larger than M, JOBZ='A') */
+/* N right singular vectors to be computed in VT and */
+/* M left singular vectors to be computed in U */
+
+ ivt = 1;
+
+/* WORK(IVT) is M by M */
+
+ ldwkvt = *m;
+ itau = ivt + ldwkvt * *m;
+ nwork = itau + *m;
+
+/* Compute A=L*Q, copying result to VT */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgelqf_(m, n, &a[a_offset], lda, &work[itau], &work[nwork], &
+ i__2, &ierr);
+ dlacpy_("U", m, n, &a[a_offset], lda, &vt[vt_offset], ldvt);
+
+/* Generate Q in VT */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dorglq_(n, n, m, &vt[vt_offset], ldvt, &work[itau], &work[
+ nwork], &i__2, &ierr);
+
+/* Produce L in A, zeroing out other entries */
+
+ i__2 = *m - 1;
+ i__1 = *m - 1;
+ dlaset_("U", &i__2, &i__1, &c_b227, &c_b227, &a[(a_dim1 << 1)
+ + 1], lda);
+ ie = itau;
+ itauq = ie + *m;
+ itaup = itauq + *m;
+ nwork = itaup + *m;
+
+/* Bidiagonalize L in A */
+/* (Workspace: need M*M+4*M, prefer M*M+3*M+2*M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(m, m, &a[a_offset], lda, &s[1], &work[ie], &work[
+ itauq], &work[itaup], &work[nwork], &i__2, &ierr);
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in WORK(IVT) */
+/* (Workspace: need M+M*M+BDSPAC) */
+
+ dbdsdc_("U", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &
+ work[ivt], &ldwkvt, dum, idum, &work[nwork], &iwork[1]
+, info);
+
+/* Overwrite U by left singular vectors of L and WORK(IVT) */
+/* by right singular vectors of L */
+/* (Workspace: need M*M+3*M, prefer M*M+2*M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, m, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__2, &ierr);
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", m, m, m, &a[a_offset], lda, &work[
+ itaup], &work[ivt], &ldwkvt, &work[nwork], &i__2, &
+ ierr);
+
+/* Multiply right singular vectors of L in WORK(IVT) by */
+/* Q in VT, storing result in A */
+/* (Workspace: need M*M) */
+
+ dgemm_("N", "N", m, n, m, &c_b248, &work[ivt], &ldwkvt, &vt[
+ vt_offset], ldvt, &c_b227, &a[a_offset], lda);
+
+/* Copy right singular vectors of A from A to VT */
+
+ dlacpy_("F", m, n, &a[a_offset], lda, &vt[vt_offset], ldvt);
+
+ }
+
+ } else {
+
+/* N .LT. MNTHR */
+
+/* Path 5t (N greater than M, but not much larger) */
+/* Reduce to bidiagonal form without LQ decomposition */
+
+ ie = 1;
+ itauq = ie + *m;
+ itaup = itauq + *m;
+ nwork = itaup + *m;
+
+/* Bidiagonalize A */
+/* (Workspace: need 3*M+N, prefer 3*M+(M+N)*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dgebrd_(m, n, &a[a_offset], lda, &s[1], &work[ie], &work[itauq], &
+ work[itaup], &work[nwork], &i__2, &ierr);
+ if (wntqn) {
+
+/* Perform bidiagonal SVD, only computing singular values */
+/* (Workspace: need M+BDSPAC) */
+
+ dbdsdc_("L", "N", m, &s[1], &work[ie], dum, &c__1, dum, &c__1,
+ dum, idum, &work[nwork], &iwork[1], info);
+ } else if (wntqo) {
+ ldwkvt = *m;
+ ivt = nwork;
+ if (*lwork >= *m * *n + *m * 3 + bdspac) {
+
+/* WORK( IVT ) is M by N */
+
+ dlaset_("F", m, n, &c_b227, &c_b227, &work[ivt], &ldwkvt);
+ nwork = ivt + ldwkvt * *n;
+ } else {
+
+/* WORK( IVT ) is M by M */
+
+ nwork = ivt + ldwkvt * *m;
+ il = nwork;
+
+/* WORK(IL) is M by CHUNK */
+
+ chunk = (*lwork - *m * *m - *m * 3) / *m;
+ }
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in WORK(IVT) */
+/* (Workspace: need M*M+BDSPAC) */
+
+ dbdsdc_("L", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &
+ work[ivt], &ldwkvt, dum, idum, &work[nwork], &iwork[1]
+, info);
+
+/* Overwrite U by left singular vectors of A */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, n, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__2, &ierr);
+
+ if (*lwork >= *m * *n + *m * 3 + bdspac) {
+
+/* Overwrite WORK(IVT) by left singular vectors of A */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", m, n, m, &a[a_offset], lda, &work[
+ itaup], &work[ivt], &ldwkvt, &work[nwork], &i__2,
+ &ierr);
+
+/* Copy right singular vectors of A from WORK(IVT) to A */
+
+ dlacpy_("F", m, n, &work[ivt], &ldwkvt, &a[a_offset], lda);
+ } else {
+
+/* Generate P**T in A */
+/* (Workspace: need M*M+2*M, prefer M*M+M+M*NB) */
+
+ i__2 = *lwork - nwork + 1;
+ dorgbr_("P", m, n, m, &a[a_offset], lda, &work[itaup], &
+ work[nwork], &i__2, &ierr);
+
+/* Multiply Q in A by right singular vectors of */
+/* bidiagonal matrix in WORK(IVT), storing result in */
+/* WORK(IL) and copying to A */
+/* (Workspace: need 2*M*M, prefer M*M+M*N) */
+
+ i__2 = *n;
+ i__1 = chunk;
+ for (i__ = 1; i__1 < 0 ? i__ >= i__2 : i__ <= i__2; i__ +=
+ i__1) {
+/* Computing MIN */
+ i__3 = *n - i__ + 1;
+ blk = min(i__3,chunk);
+ dgemm_("N", "N", m, &blk, m, &c_b248, &work[ivt], &
+ ldwkvt, &a[i__ * a_dim1 + 1], lda, &c_b227, &
+ work[il], m);
+ dlacpy_("F", m, &blk, &work[il], m, &a[i__ * a_dim1 +
+ 1], lda);
+/* L40: */
+ }
+ }
+ } else if (wntqs) {
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need M+BDSPAC) */
+
+ dlaset_("F", m, n, &c_b227, &c_b227, &vt[vt_offset], ldvt);
+ dbdsdc_("L", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Overwrite U by left singular vectors of A and VT */
+/* by right singular vectors of A */
+/* (Workspace: need 3*M, prefer 2*M+M*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, n, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", m, n, m, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__1, &
+ ierr);
+ } else if (wntqa) {
+
+/* Perform bidiagonal SVD, computing left singular vectors */
+/* of bidiagonal matrix in U and computing right singular */
+/* vectors of bidiagonal matrix in VT */
+/* (Workspace: need M+BDSPAC) */
+
+ dlaset_("F", n, n, &c_b227, &c_b227, &vt[vt_offset], ldvt);
+ dbdsdc_("L", "I", m, &s[1], &work[ie], &u[u_offset], ldu, &vt[
+ vt_offset], ldvt, dum, idum, &work[nwork], &iwork[1],
+ info);
+
+/* Set the right corner of VT to identity matrix */
+
+ if (*n > *m) {
+ i__1 = *n - *m;
+ i__2 = *n - *m;
+ dlaset_("F", &i__1, &i__2, &c_b227, &c_b248, &vt[*m + 1 +
+ (*m + 1) * vt_dim1], ldvt);
+ }
+
+/* Overwrite U by left singular vectors of A and VT */
+/* by right singular vectors of A */
+/* (Workspace: need 2*M+N, prefer 2*M+N*NB) */
+
+ i__1 = *lwork - nwork + 1;
+ dormbr_("Q", "L", "N", m, m, n, &a[a_offset], lda, &work[
+ itauq], &u[u_offset], ldu, &work[nwork], &i__1, &ierr);
+ i__1 = *lwork - nwork + 1;
+ dormbr_("P", "R", "T", n, n, m, &a[a_offset], lda, &work[
+ itaup], &vt[vt_offset], ldvt, &work[nwork], &i__1, &
+ ierr);
+ }
+
+ }
+
+ }
+
+/* Undo scaling if necessary */
+
+ if (iscl == 1) {
+ if (anrm > bignum) {
+ dlascl_("G", &c__0, &c__0, &bignum, &anrm, &minmn, &c__1, &s[1], &
+ minmn, &ierr);
+ }
+ if (anrm < smlnum) {
+ dlascl_("G", &c__0, &c__0, &smlnum, &anrm, &minmn, &c__1, &s[1], &
+ minmn, &ierr);
+ }
+ }
+
+/* Return optimal workspace in WORK(1) */
+
+ work[1] = (doublereal) maxwrk;
+
+ return 0;
+
+/* End of DGESDD */
+
+} /* dgesdd_ */
diff --git a/3rdparty/clapack/src/dgetf2.c b/3rdparty/clapack/src/dgetf2.c
new file mode 100644
index 0000000..be6639a
--- /dev/null
+++ b/3rdparty/clapack/src/dgetf2.c
@@ -0,0 +1,193 @@
+/* dgetf2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b8 = -1.;
+
+/* Subroutine */ int dgetf2_(integer *m, integer *n, doublereal *a, integer *
+ lda, integer *ipiv, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__, j, jp;
+ extern /* Subroutine */ int dger_(integer *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *), dscal_(integer *, doublereal *, doublereal *, integer
+ *);
+ doublereal sfmin;
+ extern /* Subroutine */ int dswap_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ extern doublereal dlamch_(char *);
+ extern integer idamax_(integer *, doublereal *, integer *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGETF2 computes an LU factorization of a general m-by-n matrix A */
+/* using partial pivoting with row interchanges. */
+
+/* The factorization has the form */
+/* A = P * L * U */
+/* where P is a permutation matrix, L is lower triangular with unit */
+/* diagonal elements (lower trapezoidal if m > n), and U is upper */
+/* triangular (upper trapezoidal if m < n). */
+
+/* This is the right-looking Level 2 BLAS version of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the m by n matrix to be factored. */
+/* On exit, the factors L and U from the factorization */
+/* A = P*L*U; the unit diagonal elements of L are not stored. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* IPIV (output) INTEGER array, dimension (min(M,N)) */
+/* The pivot indices; for 1 <= i <= min(M,N), row i of the */
+/* matrix was interchanged with row IPIV(i). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -k, the k-th argument had an illegal value */
+/* > 0: if INFO = k, U(k,k) is exactly zero. The factorization */
+/* has been completed, but the factor U is exactly */
+/* singular, and division by zero will occur if it is used */
+/* to solve a system of equations. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --ipiv;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGETF2", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+/* Compute machine safe minimum */
+
+ sfmin = dlamch_("S");
+
+ i__1 = min(*m,*n);
+ for (j = 1; j <= i__1; ++j) {
+
+/* Find pivot and test for singularity. */
+
+ i__2 = *m - j + 1;
+ jp = j - 1 + idamax_(&i__2, &a[j + j * a_dim1], &c__1);
+ ipiv[j] = jp;
+ if (a[jp + j * a_dim1] != 0.) {
+
+/* Apply the interchange to columns 1:N. */
+
+ if (jp != j) {
+ dswap_(n, &a[j + a_dim1], lda, &a[jp + a_dim1], lda);
+ }
+
+/* Compute elements J+1:M of J-th column. */
+
+ if (j < *m) {
+ if ((d__1 = a[j + j * a_dim1], abs(d__1)) >= sfmin) {
+ i__2 = *m - j;
+ d__1 = 1. / a[j + j * a_dim1];
+ dscal_(&i__2, &d__1, &a[j + 1 + j * a_dim1], &c__1);
+ } else {
+ i__2 = *m - j;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[j + i__ + j * a_dim1] /= a[j + j * a_dim1];
+/* L20: */
+ }
+ }
+ }
+
+ } else if (*info == 0) {
+
+ *info = j;
+ }
+
+ if (j < min(*m,*n)) {
+
+/* Update trailing submatrix. */
+
+ i__2 = *m - j;
+ i__3 = *n - j;
+ dger_(&i__2, &i__3, &c_b8, &a[j + 1 + j * a_dim1], &c__1, &a[j + (
+ j + 1) * a_dim1], lda, &a[j + 1 + (j + 1) * a_dim1], lda);
+ }
+/* L10: */
+ }
+ return 0;
+
+/* End of DGETF2 */
+
+} /* dgetf2_ */
diff --git a/3rdparty/clapack/src/dgetrf.c b/3rdparty/clapack/src/dgetrf.c
new file mode 100644
index 0000000..8a945af
--- /dev/null
+++ b/3rdparty/clapack/src/dgetrf.c
@@ -0,0 +1,219 @@
+/* dgetrf.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static doublereal c_b16 = 1.;
+static doublereal c_b19 = -1.;
+
+/* Subroutine */ int dgetrf_(integer *m, integer *n, doublereal *a, integer *
+ lda, integer *ipiv, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5;
+
+ /* Local variables */
+ integer i__, j, jb, nb;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ integer iinfo;
+ extern /* Subroutine */ int dtrsm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *), dgetf2_(
+ integer *, integer *, doublereal *, integer *, integer *, integer
+ *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ extern /* Subroutine */ int dlaswp_(integer *, doublereal *, integer *,
+ integer *, integer *, integer *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGETRF computes an LU factorization of a general M-by-N matrix A */
+/* using partial pivoting with row interchanges. */
+
+/* The factorization has the form */
+/* A = P * L * U */
+/* where P is a permutation matrix, L is lower triangular with unit */
+/* diagonal elements (lower trapezoidal if m > n), and U is upper */
+/* triangular (upper trapezoidal if m < n). */
+
+/* This is the right-looking Level 3 BLAS version of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the M-by-N matrix to be factored. */
+/* On exit, the factors L and U from the factorization */
+/* A = P*L*U; the unit diagonal elements of L are not stored. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* IPIV (output) INTEGER array, dimension (min(M,N)) */
+/* The pivot indices; for 1 <= i <= min(M,N), row i of the */
+/* matrix was interchanged with row IPIV(i). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: if INFO = i, U(i,i) is exactly zero. The factorization */
+/* has been completed, but the factor U is exactly */
+/* singular, and division by zero will occur if it is used */
+/* to solve a system of equations. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --ipiv;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*m)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGETRF", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+/* Determine the block size for this environment. */
+
+ nb = ilaenv_(&c__1, "DGETRF", " ", m, n, &c_n1, &c_n1);
+ if (nb <= 1 || nb >= min(*m,*n)) {
+
+/* Use unblocked code. */
+
+ dgetf2_(m, n, &a[a_offset], lda, &ipiv[1], info);
+ } else {
+
+/* Use blocked code. */
+
+ i__1 = min(*m,*n);
+ i__2 = nb;
+ for (j = 1; i__2 < 0 ? j >= i__1 : j <= i__1; j += i__2) {
+/* Computing MIN */
+ i__3 = min(*m,*n) - j + 1;
+ jb = min(i__3,nb);
+
+/* Factor diagonal and subdiagonal blocks and test for exact */
+/* singularity. */
+
+ i__3 = *m - j + 1;
+ dgetf2_(&i__3, &jb, &a[j + j * a_dim1], lda, &ipiv[j], &iinfo);
+
+/* Adjust INFO and the pivot indices. */
+
+ if (*info == 0 && iinfo > 0) {
+ *info = iinfo + j - 1;
+ }
+/* Computing MIN */
+ i__4 = *m, i__5 = j + jb - 1;
+ i__3 = min(i__4,i__5);
+ for (i__ = j; i__ <= i__3; ++i__) {
+ ipiv[i__] = j - 1 + ipiv[i__];
+/* L10: */
+ }
+
+/* Apply interchanges to columns 1:J-1. */
+
+ i__3 = j - 1;
+ i__4 = j + jb - 1;
+ dlaswp_(&i__3, &a[a_offset], lda, &j, &i__4, &ipiv[1], &c__1);
+
+ if (j + jb <= *n) {
+
+/* Apply interchanges to columns J+JB:N. */
+
+ i__3 = *n - j - jb + 1;
+ i__4 = j + jb - 1;
+ dlaswp_(&i__3, &a[(j + jb) * a_dim1 + 1], lda, &j, &i__4, &
+ ipiv[1], &c__1);
+
+/* Compute block row of U. */
+
+ i__3 = *n - j - jb + 1;
+ dtrsm_("Left", "Lower", "No transpose", "Unit", &jb, &i__3, &
+ c_b16, &a[j + j * a_dim1], lda, &a[j + (j + jb) *
+ a_dim1], lda);
+ if (j + jb <= *m) {
+
+/* Update trailing submatrix. */
+
+ i__3 = *m - j - jb + 1;
+ i__4 = *n - j - jb + 1;
+ dgemm_("No transpose", "No transpose", &i__3, &i__4, &jb,
+ &c_b19, &a[j + jb + j * a_dim1], lda, &a[j + (j +
+ jb) * a_dim1], lda, &c_b16, &a[j + jb + (j + jb) *
+ a_dim1], lda);
+ }
+ }
+/* L20: */
+ }
+ }
+ return 0;
+
+/* End of DGETRF */
+
+} /* dgetrf_ */
diff --git a/3rdparty/clapack/src/dgetri.c b/3rdparty/clapack/src/dgetri.c
new file mode 100644
index 0000000..d075f0c
--- /dev/null
+++ b/3rdparty/clapack/src/dgetri.c
@@ -0,0 +1,264 @@
+/* dgetri.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__2 = 2;
+static doublereal c_b20 = -1.;
+static doublereal c_b22 = 1.;
+
+/* Subroutine */ int dgetri_(integer *n, doublereal *a, integer *lda, integer
+ *ipiv, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, jb, nb, jj, jp, nn, iws;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *),
+ dgemv_(char *, integer *, integer *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *);
+ integer nbmin;
+ extern /* Subroutine */ int dswap_(integer *, doublereal *, integer *,
+ doublereal *, integer *), dtrsm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *), xerbla_(
+ char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwork;
+ extern /* Subroutine */ int dtrtri_(char *, char *, integer *, doublereal
+ *, integer *, integer *);
+ integer lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DGETRI computes the inverse of a matrix using the LU factorization */
+/* computed by DGETRF. */
+
+/* This method inverts U and then computes inv(A) by solving the system */
+/* inv(A)*L = inv(U) for inv(A). */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the factors L and U from the factorization */
+/* A = P*L*U as computed by DGETRF. */
+/* On exit, if INFO = 0, the inverse of the original matrix A. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* IPIV (input) INTEGER array, dimension (N) */
+/* The pivot indices from DGETRF; for 1<=i<=N, row i of the */
+/* matrix was interchanged with row IPIV(i). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO=0, then WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,N). */
+/* For optimal performance LWORK >= N*NB, where NB is */
+/* the optimal blocksize returned by ILAENV. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: if INFO = i, U(i,i) is exactly zero; the matrix is */
+/* singular and its inverse could not be computed. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --ipiv;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ nb = ilaenv_(&c__1, "DGETRI", " ", n, &c_n1, &c_n1, &c_n1);
+ lwkopt = *n * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*n < 0) {
+ *info = -1;
+ } else if (*lda < max(1,*n)) {
+ *info = -3;
+ } else if (*lwork < max(1,*n) && ! lquery) {
+ *info = -6;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DGETRI", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+/* Form inv(U). If INFO > 0 from DTRTRI, then U is singular, */
+/* and the inverse is not computed. */
+
+ dtrtri_("Upper", "Non-unit", n, &a[a_offset], lda, info);
+ if (*info > 0) {
+ return 0;
+ }
+
+ nbmin = 2;
+ ldwork = *n;
+ if (nb > 1 && nb < *n) {
+/* Computing MAX */
+ i__1 = ldwork * nb;
+ iws = max(i__1,1);
+ if (*lwork < iws) {
+ nb = *lwork / ldwork;
+/* Computing MAX */
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DGETRI", " ", n, &c_n1, &c_n1, &
+ c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ } else {
+ iws = *n;
+ }
+
+/* Solve the equation inv(A)*L = inv(U) for inv(A). */
+
+ if (nb < nbmin || nb >= *n) {
+
+/* Use unblocked code. */
+
+ for (j = *n; j >= 1; --j) {
+
+/* Copy current column of L to WORK and replace with zeros. */
+
+ i__1 = *n;
+ for (i__ = j + 1; i__ <= i__1; ++i__) {
+ work[i__] = a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = 0.;
+/* L10: */
+ }
+
+/* Compute current column of inv(A). */
+
+ if (j < *n) {
+ i__1 = *n - j;
+ dgemv_("No transpose", n, &i__1, &c_b20, &a[(j + 1) * a_dim1
+ + 1], lda, &work[j + 1], &c__1, &c_b22, &a[j * a_dim1
+ + 1], &c__1);
+ }
+/* L20: */
+ }
+ } else {
+
+/* Use blocked code. */
+
+ nn = (*n - 1) / nb * nb + 1;
+ i__1 = -nb;
+ for (j = nn; i__1 < 0 ? j >= 1 : j <= 1; j += i__1) {
+/* Computing MIN */
+ i__2 = nb, i__3 = *n - j + 1;
+ jb = min(i__2,i__3);
+
+/* Copy current block column of L to WORK and replace with */
+/* zeros. */
+
+ i__2 = j + jb - 1;
+ for (jj = j; jj <= i__2; ++jj) {
+ i__3 = *n;
+ for (i__ = jj + 1; i__ <= i__3; ++i__) {
+ work[i__ + (jj - j) * ldwork] = a[i__ + jj * a_dim1];
+ a[i__ + jj * a_dim1] = 0.;
+/* L30: */
+ }
+/* L40: */
+ }
+
+/* Compute current block column of inv(A). */
+
+ if (j + jb <= *n) {
+ i__2 = *n - j - jb + 1;
+ dgemm_("No transpose", "No transpose", n, &jb, &i__2, &c_b20,
+ &a[(j + jb) * a_dim1 + 1], lda, &work[j + jb], &
+ ldwork, &c_b22, &a[j * a_dim1 + 1], lda);
+ }
+ dtrsm_("Right", "Lower", "No transpose", "Unit", n, &jb, &c_b22, &
+ work[j], &ldwork, &a[j * a_dim1 + 1], lda);
+/* L50: */
+ }
+ }
+
+/* Apply column interchanges. */
+
+ for (j = *n - 1; j >= 1; --j) {
+ jp = ipiv[j];
+ if (jp != j) {
+ dswap_(n, &a[j * a_dim1 + 1], &c__1, &a[jp * a_dim1 + 1], &c__1);
+ }
+/* L60: */
+ }
+
+ work[1] = (doublereal) iws;
+ return 0;
+
+/* End of DGETRI */
+
+} /* dgetri_ */
diff --git a/3rdparty/clapack/src/disnan.c b/3rdparty/clapack/src/disnan.c
new file mode 100644
index 0000000..564ca2c
--- /dev/null
+++ b/3rdparty/clapack/src/disnan.c
@@ -0,0 +1,52 @@
+/* disnan.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+logical disnan_(doublereal *din)
+{
+ /* System generated locals */
+ logical ret_val;
+
+ /* Local variables */
+ extern logical dlaisnan_(doublereal *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DISNAN returns .TRUE. if its argument is NaN, and .FALSE. */
+/* otherwise. To be replaced by the Fortran 2003 intrinsic in the */
+/* future. */
+
+/* Arguments */
+/* ========= */
+
+/* DIN (input) DOUBLE PRECISION */
+/* Input to test for NaN. */
+
+/* ===================================================================== */
+
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+ ret_val = dlaisnan_(din, din);
+ return ret_val;
+} /* disnan_ */
diff --git a/3rdparty/clapack/src/dlabrd.c b/3rdparty/clapack/src/dlabrd.c
new file mode 100644
index 0000000..2048ef7
--- /dev/null
+++ b/3rdparty/clapack/src/dlabrd.c
@@ -0,0 +1,434 @@
+/* dlabrd.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static doublereal c_b4 = -1.;
+static doublereal c_b5 = 1.;
+static integer c__1 = 1;
+static doublereal c_b16 = 0.;
+
+/* Subroutine */ int dlabrd_(integer *m, integer *n, integer *nb, doublereal *
+ a, integer *lda, doublereal *d__, doublereal *e, doublereal *tauq,
+ doublereal *taup, doublereal *x, integer *ldx, doublereal *y, integer
+ *ldy)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, x_dim1, x_offset, y_dim1, y_offset, i__1, i__2,
+ i__3;
+
+ /* Local variables */
+ integer i__;
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *), dgemv_(char *, integer *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ doublereal *, integer *), dlarfg_(integer *, doublereal *,
+ doublereal *, integer *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLABRD reduces the first NB rows and columns of a real general */
+/* m by n matrix A to upper or lower bidiagonal form by an orthogonal */
+/* transformation Q' * A * P, and returns the matrices X and Y which */
+/* are needed to apply the transformation to the unreduced part of A. */
+
+/* If m >= n, A is reduced to upper bidiagonal form; if m < n, to lower */
+/* bidiagonal form. */
+
+/* This is an auxiliary routine called by DGEBRD */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows in the matrix A. */
+
+/* N (input) INTEGER */
+/* The number of columns in the matrix A. */
+
+/* NB (input) INTEGER */
+/* The number of leading rows and columns of A to be reduced. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the m by n general matrix to be reduced. */
+/* On exit, the first NB rows and columns of the matrix are */
+/* overwritten; the rest of the array is unchanged. */
+/* If m >= n, elements on and below the diagonal in the first NB */
+/* columns, with the array TAUQ, represent the orthogonal */
+/* matrix Q as a product of elementary reflectors; and */
+/* elements above the diagonal in the first NB rows, with the */
+/* array TAUP, represent the orthogonal matrix P as a product */
+/* of elementary reflectors. */
+/* If m < n, elements below the diagonal in the first NB */
+/* columns, with the array TAUQ, represent the orthogonal */
+/* matrix Q as a product of elementary reflectors, and */
+/* elements on and above the diagonal in the first NB rows, */
+/* with the array TAUP, represent the orthogonal matrix P as */
+/* a product of elementary reflectors. */
+/* See Further Details. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* D (output) DOUBLE PRECISION array, dimension (NB) */
+/* The diagonal elements of the first NB rows and columns of */
+/* the reduced matrix. D(i) = A(i,i). */
+
+/* E (output) DOUBLE PRECISION array, dimension (NB) */
+/* The off-diagonal elements of the first NB rows and columns of */
+/* the reduced matrix. */
+
+/* TAUQ (output) DOUBLE PRECISION array dimension (NB) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix Q. See Further Details. */
+
+/* TAUP (output) DOUBLE PRECISION array, dimension (NB) */
+/* The scalar factors of the elementary reflectors which */
+/* represent the orthogonal matrix P. See Further Details. */
+
+/* X (output) DOUBLE PRECISION array, dimension (LDX,NB) */
+/* The m-by-nb matrix X required to update the unreduced part */
+/* of A. */
+
+/* LDX (input) INTEGER */
+/* The leading dimension of the array X. LDX >= M. */
+
+/* Y (output) DOUBLE PRECISION array, dimension (LDY,NB) */
+/* The n-by-nb matrix Y required to update the unreduced part */
+/* of A. */
+
+/* LDY (input) INTEGER */
+/* The leading dimension of the array Y. LDY >= N. */
+
+/* Further Details */
+/* =============== */
+
+/* The matrices Q and P are represented as products of elementary */
+/* reflectors: */
+
+/* Q = H(1) H(2) . . . H(nb) and P = G(1) G(2) . . . G(nb) */
+
+/* Each H(i) and G(i) has the form: */
+
+/* H(i) = I - tauq * v * v' and G(i) = I - taup * u * u' */
+
+/* where tauq and taup are real scalars, and v and u are real vectors. */
+
+/* If m >= n, v(1:i-1) = 0, v(i) = 1, and v(i:m) is stored on exit in */
+/* A(i:m,i); u(1:i) = 0, u(i+1) = 1, and u(i+1:n) is stored on exit in */
+/* A(i,i+1:n); tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* If m < n, v(1:i) = 0, v(i+1) = 1, and v(i+1:m) is stored on exit in */
+/* A(i+2:m,i); u(1:i-1) = 0, u(i) = 1, and u(i:n) is stored on exit in */
+/* A(i,i+1:n); tauq is stored in TAUQ(i) and taup in TAUP(i). */
+
+/* The elements of the vectors v and u together form the m-by-nb matrix */
+/* V and the nb-by-n matrix U' which are needed, with X and Y, to apply */
+/* the transformation to the unreduced part of the matrix, using a block */
+/* update of the form: A := A - V*Y' - X*U'. */
+
+/* The contents of A on exit are illustrated by the following examples */
+/* with nb = 2: */
+
+/* m = 6 and n = 5 (m > n): m = 5 and n = 6 (m < n): */
+
+/* ( 1 1 u1 u1 u1 ) ( 1 u1 u1 u1 u1 u1 ) */
+/* ( v1 1 1 u2 u2 ) ( 1 1 u2 u2 u2 u2 ) */
+/* ( v1 v2 a a a ) ( v1 1 a a a a ) */
+/* ( v1 v2 a a a ) ( v1 v2 a a a a ) */
+/* ( v1 v2 a a a ) ( v1 v2 a a a a ) */
+/* ( v1 v2 a a a ) */
+
+/* where a denotes an element of the original matrix which is unchanged, */
+/* vi denotes an element of the vector defining H(i), and ui an element */
+/* of the vector defining G(i). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Quick return if possible */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --d__;
+ --e;
+ --tauq;
+ --taup;
+ x_dim1 = *ldx;
+ x_offset = 1 + x_dim1;
+ x -= x_offset;
+ y_dim1 = *ldy;
+ y_offset = 1 + y_dim1;
+ y -= y_offset;
+
+ /* Function Body */
+ if (*m <= 0 || *n <= 0) {
+ return 0;
+ }
+
+ if (*m >= *n) {
+
+/* Reduce to upper bidiagonal form */
+
+ i__1 = *nb;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Update A(i:m,i) */
+
+ i__2 = *m - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &a[i__ + a_dim1], lda,
+ &y[i__ + y_dim1], ldy, &c_b5, &a[i__ + i__ * a_dim1], &
+ c__1);
+ i__2 = *m - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &x[i__ + x_dim1], ldx,
+ &a[i__ * a_dim1 + 1], &c__1, &c_b5, &a[i__ + i__ *
+ a_dim1], &c__1);
+
+/* Generate reflection Q(i) to annihilate A(i+1:m,i) */
+
+ i__2 = *m - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfg_(&i__2, &a[i__ + i__ * a_dim1], &a[min(i__3, *m)+ i__ *
+ a_dim1], &c__1, &tauq[i__]);
+ d__[i__] = a[i__ + i__ * a_dim1];
+ if (i__ < *n) {
+ a[i__ + i__ * a_dim1] = 1.;
+
+/* Compute Y(i+1:n,i) */
+
+ i__2 = *m - i__ + 1;
+ i__3 = *n - i__;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &a[i__ + (i__ + 1) *
+ a_dim1], lda, &a[i__ + i__ * a_dim1], &c__1, &c_b16, &
+ y[i__ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *m - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &a[i__ + a_dim1],
+ lda, &a[i__ + i__ * a_dim1], &c__1, &c_b16, &y[i__ *
+ y_dim1 + 1], &c__1);
+ i__2 = *n - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &y[i__ + 1 +
+ y_dim1], ldy, &y[i__ * y_dim1 + 1], &c__1, &c_b5, &y[
+ i__ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *m - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &x[i__ + x_dim1],
+ ldx, &a[i__ + i__ * a_dim1], &c__1, &c_b16, &y[i__ *
+ y_dim1 + 1], &c__1);
+ i__2 = i__ - 1;
+ i__3 = *n - i__;
+ dgemv_("Transpose", &i__2, &i__3, &c_b4, &a[(i__ + 1) *
+ a_dim1 + 1], lda, &y[i__ * y_dim1 + 1], &c__1, &c_b5,
+ &y[i__ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *n - i__;
+ dscal_(&i__2, &tauq[i__], &y[i__ + 1 + i__ * y_dim1], &c__1);
+
+/* Update A(i,i+1:n) */
+
+ i__2 = *n - i__;
+ dgemv_("No transpose", &i__2, &i__, &c_b4, &y[i__ + 1 +
+ y_dim1], ldy, &a[i__ + a_dim1], lda, &c_b5, &a[i__ + (
+ i__ + 1) * a_dim1], lda);
+ i__2 = i__ - 1;
+ i__3 = *n - i__;
+ dgemv_("Transpose", &i__2, &i__3, &c_b4, &a[(i__ + 1) *
+ a_dim1 + 1], lda, &x[i__ + x_dim1], ldx, &c_b5, &a[
+ i__ + (i__ + 1) * a_dim1], lda);
+
+/* Generate reflection P(i) to annihilate A(i,i+2:n) */
+
+ i__2 = *n - i__;
+/* Computing MIN */
+ i__3 = i__ + 2;
+ dlarfg_(&i__2, &a[i__ + (i__ + 1) * a_dim1], &a[i__ + min(
+ i__3, *n)* a_dim1], lda, &taup[i__]);
+ e[i__] = a[i__ + (i__ + 1) * a_dim1];
+ a[i__ + (i__ + 1) * a_dim1] = 1.;
+
+/* Compute X(i+1:m,i) */
+
+ i__2 = *m - i__;
+ i__3 = *n - i__;
+ dgemv_("No transpose", &i__2, &i__3, &c_b5, &a[i__ + 1 + (i__
+ + 1) * a_dim1], lda, &a[i__ + (i__ + 1) * a_dim1],
+ lda, &c_b16, &x[i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = *n - i__;
+ dgemv_("Transpose", &i__2, &i__, &c_b5, &y[i__ + 1 + y_dim1],
+ ldy, &a[i__ + (i__ + 1) * a_dim1], lda, &c_b16, &x[
+ i__ * x_dim1 + 1], &c__1);
+ i__2 = *m - i__;
+ dgemv_("No transpose", &i__2, &i__, &c_b4, &a[i__ + 1 +
+ a_dim1], lda, &x[i__ * x_dim1 + 1], &c__1, &c_b5, &x[
+ i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = i__ - 1;
+ i__3 = *n - i__;
+ dgemv_("No transpose", &i__2, &i__3, &c_b5, &a[(i__ + 1) *
+ a_dim1 + 1], lda, &a[i__ + (i__ + 1) * a_dim1], lda, &
+ c_b16, &x[i__ * x_dim1 + 1], &c__1);
+ i__2 = *m - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &x[i__ + 1 +
+ x_dim1], ldx, &x[i__ * x_dim1 + 1], &c__1, &c_b5, &x[
+ i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = *m - i__;
+ dscal_(&i__2, &taup[i__], &x[i__ + 1 + i__ * x_dim1], &c__1);
+ }
+/* L10: */
+ }
+ } else {
+
+/* Reduce to lower bidiagonal form */
+
+ i__1 = *nb;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Update A(i,i:n) */
+
+ i__2 = *n - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &y[i__ + y_dim1], ldy,
+ &a[i__ + a_dim1], lda, &c_b5, &a[i__ + i__ * a_dim1],
+ lda);
+ i__2 = i__ - 1;
+ i__3 = *n - i__ + 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b4, &a[i__ * a_dim1 + 1],
+ lda, &x[i__ + x_dim1], ldx, &c_b5, &a[i__ + i__ * a_dim1],
+ lda);
+
+/* Generate reflection P(i) to annihilate A(i,i+1:n) */
+
+ i__2 = *n - i__ + 1;
+/* Computing MIN */
+ i__3 = i__ + 1;
+ dlarfg_(&i__2, &a[i__ + i__ * a_dim1], &a[i__ + min(i__3, *n)*
+ a_dim1], lda, &taup[i__]);
+ d__[i__] = a[i__ + i__ * a_dim1];
+ if (i__ < *m) {
+ a[i__ + i__ * a_dim1] = 1.;
+
+/* Compute X(i+1:m,i) */
+
+ i__2 = *m - i__;
+ i__3 = *n - i__ + 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b5, &a[i__ + 1 + i__ *
+ a_dim1], lda, &a[i__ + i__ * a_dim1], lda, &c_b16, &
+ x[i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = *n - i__ + 1;
+ i__3 = i__ - 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &y[i__ + y_dim1],
+ ldy, &a[i__ + i__ * a_dim1], lda, &c_b16, &x[i__ *
+ x_dim1 + 1], &c__1);
+ i__2 = *m - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &a[i__ + 1 +
+ a_dim1], lda, &x[i__ * x_dim1 + 1], &c__1, &c_b5, &x[
+ i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = i__ - 1;
+ i__3 = *n - i__ + 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b5, &a[i__ * a_dim1 +
+ 1], lda, &a[i__ + i__ * a_dim1], lda, &c_b16, &x[i__ *
+ x_dim1 + 1], &c__1);
+ i__2 = *m - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &x[i__ + 1 +
+ x_dim1], ldx, &x[i__ * x_dim1 + 1], &c__1, &c_b5, &x[
+ i__ + 1 + i__ * x_dim1], &c__1);
+ i__2 = *m - i__;
+ dscal_(&i__2, &taup[i__], &x[i__ + 1 + i__ * x_dim1], &c__1);
+
+/* Update A(i+1:m,i) */
+
+ i__2 = *m - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &a[i__ + 1 +
+ a_dim1], lda, &y[i__ + y_dim1], ldy, &c_b5, &a[i__ +
+ 1 + i__ * a_dim1], &c__1);
+ i__2 = *m - i__;
+ dgemv_("No transpose", &i__2, &i__, &c_b4, &x[i__ + 1 +
+ x_dim1], ldx, &a[i__ * a_dim1 + 1], &c__1, &c_b5, &a[
+ i__ + 1 + i__ * a_dim1], &c__1);
+
+/* Generate reflection Q(i) to annihilate A(i+2:m,i) */
+
+ i__2 = *m - i__;
+/* Computing MIN */
+ i__3 = i__ + 2;
+ dlarfg_(&i__2, &a[i__ + 1 + i__ * a_dim1], &a[min(i__3, *m)+
+ i__ * a_dim1], &c__1, &tauq[i__]);
+ e[i__] = a[i__ + 1 + i__ * a_dim1];
+ a[i__ + 1 + i__ * a_dim1] = 1.;
+
+/* Compute Y(i+1:n,i) */
+
+ i__2 = *m - i__;
+ i__3 = *n - i__;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &a[i__ + 1 + (i__ +
+ 1) * a_dim1], lda, &a[i__ + 1 + i__ * a_dim1], &c__1,
+ &c_b16, &y[i__ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *m - i__;
+ i__3 = i__ - 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b5, &a[i__ + 1 + a_dim1],
+ lda, &a[i__ + 1 + i__ * a_dim1], &c__1, &c_b16, &y[
+ i__ * y_dim1 + 1], &c__1);
+ i__2 = *n - i__;
+ i__3 = i__ - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b4, &y[i__ + 1 +
+ y_dim1], ldy, &y[i__ * y_dim1 + 1], &c__1, &c_b5, &y[
+ i__ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *m - i__;
+ dgemv_("Transpose", &i__2, &i__, &c_b5, &x[i__ + 1 + x_dim1],
+ ldx, &a[i__ + 1 + i__ * a_dim1], &c__1, &c_b16, &y[
+ i__ * y_dim1 + 1], &c__1);
+ i__2 = *n - i__;
+ dgemv_("Transpose", &i__, &i__2, &c_b4, &a[(i__ + 1) * a_dim1
+ + 1], lda, &y[i__ * y_dim1 + 1], &c__1, &c_b5, &y[i__
+ + 1 + i__ * y_dim1], &c__1);
+ i__2 = *n - i__;
+ dscal_(&i__2, &tauq[i__], &y[i__ + 1 + i__ * y_dim1], &c__1);
+ }
+/* L20: */
+ }
+ }
+ return 0;
+
+/* End of DLABRD */
+
+} /* dlabrd_ */
diff --git a/3rdparty/clapack/src/dlacpy.c b/3rdparty/clapack/src/dlacpy.c
new file mode 100644
index 0000000..9fff495
--- /dev/null
+++ b/3rdparty/clapack/src/dlacpy.c
@@ -0,0 +1,125 @@
+/* dlacpy.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlacpy_(char *uplo, integer *m, integer *n, doublereal *
+ a, integer *lda, doublereal *b, integer *ldb)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, b_dim1, b_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j;
+ extern logical lsame_(char *, char *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLACPY copies all or part of a two-dimensional matrix A to another */
+/* matrix B. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies the part of the matrix A to be copied to B. */
+/* = 'U': Upper triangular part */
+/* = 'L': Lower triangular part */
+/* Otherwise: All of the matrix A */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The m by n matrix A. If UPLO = 'U', only the upper triangle */
+/* or trapezoid is accessed; if UPLO = 'L', only the lower */
+/* triangle or trapezoid is accessed. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* B (output) DOUBLE PRECISION array, dimension (LDB,N) */
+/* On exit, B = A in the locations specified by UPLO. */
+
+/* LDB (input) INTEGER */
+/* The leading dimension of the array B. LDB >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ b_dim1 = *ldb;
+ b_offset = 1 + b_dim1;
+ b -= b_offset;
+
+ /* Function Body */
+ if (lsame_(uplo, "U")) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = min(j,*m);
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = a[i__ + j * a_dim1];
+/* L10: */
+ }
+/* L20: */
+ }
+ } else if (lsame_(uplo, "L")) {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = a[i__ + j * a_dim1];
+/* L30: */
+ }
+/* L40: */
+ }
+ } else {
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ b[i__ + j * b_dim1] = a[i__ + j * a_dim1];
+/* L50: */
+ }
+/* L60: */
+ }
+ }
+ return 0;
+
+/* End of DLACPY */
+
+} /* dlacpy_ */
diff --git a/3rdparty/clapack/src/dlaed6.c b/3rdparty/clapack/src/dlaed6.c
new file mode 100644
index 0000000..eff20c2
--- /dev/null
+++ b/3rdparty/clapack/src/dlaed6.c
@@ -0,0 +1,374 @@
+/* dlaed6.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlaed6_(integer *kniter, logical *orgati, doublereal *
+ rho, doublereal *d__, doublereal *z__, doublereal *finit, doublereal *
+ tau, integer *info)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2, d__3, d__4;
+
+ /* Builtin functions */
+ double sqrt(doublereal), log(doublereal), pow_di(doublereal *, integer *);
+
+ /* Local variables */
+ doublereal a, b, c__, f;
+ integer i__;
+ doublereal fc, df, ddf, lbd, eta, ubd, eps, base;
+ integer iter;
+ doublereal temp, temp1, temp2, temp3, temp4;
+ logical scale;
+ integer niter;
+ doublereal small1, small2, sminv1, sminv2;
+ extern doublereal dlamch_(char *);
+ doublereal dscale[3], sclfac, zscale[3], erretm, sclinv;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* February 2007 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAED6 computes the positive or negative root (closest to the origin) */
+/* of */
+/* z(1) z(2) z(3) */
+/* f(x) = rho + --------- + ---------- + --------- */
+/* d(1)-x d(2)-x d(3)-x */
+
+/* It is assumed that */
+
+/* if ORGATI = .true. the root is between d(2) and d(3); */
+/* otherwise it is between d(1) and d(2) */
+
+/* This routine will be called by DLAED4 when necessary. In most cases, */
+/* the root sought is the smallest in magnitude, though it might not be */
+/* in some extremely rare situations. */
+
+/* Arguments */
+/* ========= */
+
+/* KNITER (input) INTEGER */
+/* Refer to DLAED4 for its significance. */
+
+/* ORGATI (input) LOGICAL */
+/* If ORGATI is true, the needed root is between d(2) and */
+/* d(3); otherwise it is between d(1) and d(2). See */
+/* DLAED4 for further details. */
+
+/* RHO (input) DOUBLE PRECISION */
+/* Refer to the equation f(x) above. */
+
+/* D (input) DOUBLE PRECISION array, dimension (3) */
+/* D satisfies d(1) < d(2) < d(3). */
+
+/* Z (input) DOUBLE PRECISION array, dimension (3) */
+/* Each of the elements in z must be positive. */
+
+/* FINIT (input) DOUBLE PRECISION */
+/* The value of f at 0. It is more accurate than the one */
+/* evaluated inside this routine (if someone wants to do */
+/* so). */
+
+/* TAU (output) DOUBLE PRECISION */
+/* The root of the equation f(x). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* > 0: if INFO = 1, failure to converge */
+
+/* Further Details */
+/* =============== */
+
+/* 30/06/99: Based on contributions by */
+/* Ren-Cang Li, Computer Science Division, University of California */
+/* at Berkeley, USA */
+
+/* 10/02/03: This version has a few statements commented out for thread */
+/* safety (machine parameters are computed on each entry). SJH. */
+
+/* 05/10/06: Modified from a new version of Ren-Cang Li, use */
+/* Gragg-Thornton-Warner cubic convergent scheme for better stability. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --z__;
+ --d__;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*orgati) {
+ lbd = d__[2];
+ ubd = d__[3];
+ } else {
+ lbd = d__[1];
+ ubd = d__[2];
+ }
+ if (*finit < 0.) {
+ lbd = 0.;
+ } else {
+ ubd = 0.;
+ }
+
+ niter = 1;
+ *tau = 0.;
+ if (*kniter == 2) {
+ if (*orgati) {
+ temp = (d__[3] - d__[2]) / 2.;
+ c__ = *rho + z__[1] / (d__[1] - d__[2] - temp);
+ a = c__ * (d__[2] + d__[3]) + z__[2] + z__[3];
+ b = c__ * d__[2] * d__[3] + z__[2] * d__[3] + z__[3] * d__[2];
+ } else {
+ temp = (d__[1] - d__[2]) / 2.;
+ c__ = *rho + z__[3] / (d__[3] - d__[2] - temp);
+ a = c__ * (d__[1] + d__[2]) + z__[1] + z__[2];
+ b = c__ * d__[1] * d__[2] + z__[1] * d__[2] + z__[2] * d__[1];
+ }
+/* Computing MAX */
+ d__1 = abs(a), d__2 = abs(b), d__1 = max(d__1,d__2), d__2 = abs(c__);
+ temp = max(d__1,d__2);
+ a /= temp;
+ b /= temp;
+ c__ /= temp;
+ if (c__ == 0.) {
+ *tau = b / a;
+ } else if (a <= 0.) {
+ *tau = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (
+ c__ * 2.);
+ } else {
+ *tau = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1))
+ ));
+ }
+ if (*tau < lbd || *tau > ubd) {
+ *tau = (lbd + ubd) / 2.;
+ }
+ if (d__[1] == *tau || d__[2] == *tau || d__[3] == *tau) {
+ *tau = 0.;
+ } else {
+ temp = *finit + *tau * z__[1] / (d__[1] * (d__[1] - *tau)) + *tau
+ * z__[2] / (d__[2] * (d__[2] - *tau)) + *tau * z__[3] / (
+ d__[3] * (d__[3] - *tau));
+ if (temp <= 0.) {
+ lbd = *tau;
+ } else {
+ ubd = *tau;
+ }
+ if (abs(*finit) <= abs(temp)) {
+ *tau = 0.;
+ }
+ }
+ }
+
+/* get machine parameters for possible scaling to avoid overflow */
+
+/* modified by Sven: parameters SMALL1, SMINV1, SMALL2, */
+/* SMINV2, EPS are not SAVEd anymore between one call to the */
+/* others but recomputed at each call */
+
+ eps = dlamch_("Epsilon");
+ base = dlamch_("Base");
+ i__1 = (integer) (log(dlamch_("SafMin")) / log(base) / 3.);
+ small1 = pow_di(&base, &i__1);
+ sminv1 = 1. / small1;
+ small2 = small1 * small1;
+ sminv2 = sminv1 * sminv1;
+
+/* Determine if scaling of inputs necessary to avoid overflow */
+/* when computing 1/TEMP**3 */
+
+ if (*orgati) {
+/* Computing MIN */
+ d__3 = (d__1 = d__[2] - *tau, abs(d__1)), d__4 = (d__2 = d__[3] - *
+ tau, abs(d__2));
+ temp = min(d__3,d__4);
+ } else {
+/* Computing MIN */
+ d__3 = (d__1 = d__[1] - *tau, abs(d__1)), d__4 = (d__2 = d__[2] - *
+ tau, abs(d__2));
+ temp = min(d__3,d__4);
+ }
+ scale = FALSE_;
+ if (temp <= small1) {
+ scale = TRUE_;
+ if (temp <= small2) {
+
+/* Scale up by power of radix nearest 1/SAFMIN**(2/3) */
+
+ sclfac = sminv2;
+ sclinv = small2;
+ } else {
+
+/* Scale up by power of radix nearest 1/SAFMIN**(1/3) */
+
+ sclfac = sminv1;
+ sclinv = small1;
+ }
+
+/* Scaling up safe because D, Z, TAU scaled elsewhere to be O(1) */
+
+ for (i__ = 1; i__ <= 3; ++i__) {
+ dscale[i__ - 1] = d__[i__] * sclfac;
+ zscale[i__ - 1] = z__[i__] * sclfac;
+/* L10: */
+ }
+ *tau *= sclfac;
+ lbd *= sclfac;
+ ubd *= sclfac;
+ } else {
+
+/* Copy D and Z to DSCALE and ZSCALE */
+
+ for (i__ = 1; i__ <= 3; ++i__) {
+ dscale[i__ - 1] = d__[i__];
+ zscale[i__ - 1] = z__[i__];
+/* L20: */
+ }
+ }
+
+ fc = 0.;
+ df = 0.;
+ ddf = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ temp = 1. / (dscale[i__ - 1] - *tau);
+ temp1 = zscale[i__ - 1] * temp;
+ temp2 = temp1 * temp;
+ temp3 = temp2 * temp;
+ fc += temp1 / dscale[i__ - 1];
+ df += temp2;
+ ddf += temp3;
+/* L30: */
+ }
+ f = *finit + *tau * fc;
+
+ if (abs(f) <= 0.) {
+ goto L60;
+ }
+ if (f <= 0.) {
+ lbd = *tau;
+ } else {
+ ubd = *tau;
+ }
+
+/* Iteration begins -- Use Gragg-Thornton-Warner cubic convergent */
+/* scheme */
+
+/* It is not hard to see that */
+
+/* 1) Iterations will go up monotonically */
+/* if FINIT < 0; */
+
+/* 2) Iterations will go down monotonically */
+/* if FINIT > 0. */
+
+ iter = niter + 1;
+
+ for (niter = iter; niter <= 40; ++niter) {
+
+ if (*orgati) {
+ temp1 = dscale[1] - *tau;
+ temp2 = dscale[2] - *tau;
+ } else {
+ temp1 = dscale[0] - *tau;
+ temp2 = dscale[1] - *tau;
+ }
+ a = (temp1 + temp2) * f - temp1 * temp2 * df;
+ b = temp1 * temp2 * f;
+ c__ = f - (temp1 + temp2) * df + temp1 * temp2 * ddf;
+/* Computing MAX */
+ d__1 = abs(a), d__2 = abs(b), d__1 = max(d__1,d__2), d__2 = abs(c__);
+ temp = max(d__1,d__2);
+ a /= temp;
+ b /= temp;
+ c__ /= temp;
+ if (c__ == 0.) {
+ eta = b / a;
+ } else if (a <= 0.) {
+ eta = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (c__
+ * 2.);
+ } else {
+ eta = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))
+ );
+ }
+ if (f * eta >= 0.) {
+ eta = -f / df;
+ }
+
+ *tau += eta;
+ if (*tau < lbd || *tau > ubd) {
+ *tau = (lbd + ubd) / 2.;
+ }
+
+ fc = 0.;
+ erretm = 0.;
+ df = 0.;
+ ddf = 0.;
+ for (i__ = 1; i__ <= 3; ++i__) {
+ temp = 1. / (dscale[i__ - 1] - *tau);
+ temp1 = zscale[i__ - 1] * temp;
+ temp2 = temp1 * temp;
+ temp3 = temp2 * temp;
+ temp4 = temp1 / dscale[i__ - 1];
+ fc += temp4;
+ erretm += abs(temp4);
+ df += temp2;
+ ddf += temp3;
+/* L40: */
+ }
+ f = *finit + *tau * fc;
+ erretm = (abs(*finit) + abs(*tau) * erretm) * 8. + abs(*tau) * df;
+ if (abs(f) <= eps * erretm) {
+ goto L60;
+ }
+ if (f <= 0.) {
+ lbd = *tau;
+ } else {
+ ubd = *tau;
+ }
+/* L50: */
+ }
+ *info = 1;
+L60:
+
+/* Undo scaling */
+
+ if (scale) {
+ *tau *= sclinv;
+ }
+ return 0;
+
+/* End of DLAED6 */
+
+} /* dlaed6_ */
diff --git a/3rdparty/clapack/src/dlaisnan.c b/3rdparty/clapack/src/dlaisnan.c
new file mode 100644
index 0000000..ea4703c
--- /dev/null
+++ b/3rdparty/clapack/src/dlaisnan.c
@@ -0,0 +1,58 @@
+/* dlaisnan.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+logical dlaisnan_(doublereal *din1, doublereal *din2)
+{
+ /* System generated locals */
+ logical ret_val;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* This routine is not for general use. It exists solely to avoid */
+/* over-optimization in DISNAN. */
+
+/* DLAISNAN checks for NaNs by comparing its two arguments for */
+/* inequality. NaN is the only floating-point value where NaN != NaN */
+/* returns .TRUE. To check for NaNs, pass the same variable as both */
+/* arguments. */
+
+/* A compiler must assume that the two arguments are */
+/* not the same variable, and the test will not be optimized away. */
+/* Interprocedural or whole-program optimization may delete this */
+/* test. The ISNAN functions will be replaced by the correct */
+/* Fortran 03 intrinsic once the intrinsic is widely available. */
+
+/* Arguments */
+/* ========= */
+
+/* DIN1 (input) DOUBLE PRECISION */
+/* DIN2 (input) DOUBLE PRECISION */
+/* Two numbers to compare for inequality. */
+
+/* ===================================================================== */
+
+/* .. Executable Statements .. */
+ ret_val = *din1 != *din2;
+ return ret_val;
+} /* dlaisnan_ */
diff --git a/3rdparty/clapack/src/dlamrg.c b/3rdparty/clapack/src/dlamrg.c
new file mode 100644
index 0000000..ce814be
--- /dev/null
+++ b/3rdparty/clapack/src/dlamrg.c
@@ -0,0 +1,131 @@
+/* dlamrg.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlamrg_(integer *n1, integer *n2, doublereal *a, integer
+ *dtrd1, integer *dtrd2, integer *index)
+{
+ /* System generated locals */
+ integer i__1;
+
+ /* Local variables */
+ integer i__, ind1, ind2, n1sv, n2sv;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAMRG will create a permutation list which will merge the elements */
+/* of A (which is composed of two independently sorted sets) into a */
+/* single set which is sorted in ascending order. */
+
+/* Arguments */
+/* ========= */
+
+/* N1 (input) INTEGER */
+/* N2 (input) INTEGER */
+/* These arguements contain the respective lengths of the two */
+/* sorted lists to be merged. */
+
+/* A (input) DOUBLE PRECISION array, dimension (N1+N2) */
+/* The first N1 elements of A contain a list of numbers which */
+/* are sorted in either ascending or descending order. Likewise */
+/* for the final N2 elements. */
+
+/* DTRD1 (input) INTEGER */
+/* DTRD2 (input) INTEGER */
+/* These are the strides to be taken through the array A. */
+/* Allowable strides are 1 and -1. They indicate whether a */
+/* subset of A is sorted in ascending (DTRDx = 1) or descending */
+/* (DTRDx = -1) order. */
+
+/* INDEX (output) INTEGER array, dimension (N1+N2) */
+/* On exit this array will contain a permutation such that */
+/* if B( I ) = A( INDEX( I ) ) for I=1,N1+N2, then B will be */
+/* sorted in ascending order. */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --index;
+ --a;
+
+ /* Function Body */
+ n1sv = *n1;
+ n2sv = *n2;
+ if (*dtrd1 > 0) {
+ ind1 = 1;
+ } else {
+ ind1 = *n1;
+ }
+ if (*dtrd2 > 0) {
+ ind2 = *n1 + 1;
+ } else {
+ ind2 = *n1 + *n2;
+ }
+ i__ = 1;
+/* while ( (N1SV > 0) & (N2SV > 0) ) */
+L10:
+ if (n1sv > 0 && n2sv > 0) {
+ if (a[ind1] <= a[ind2]) {
+ index[i__] = ind1;
+ ++i__;
+ ind1 += *dtrd1;
+ --n1sv;
+ } else {
+ index[i__] = ind2;
+ ++i__;
+ ind2 += *dtrd2;
+ --n2sv;
+ }
+ goto L10;
+ }
+/* end while */
+ if (n1sv == 0) {
+ i__1 = n2sv;
+ for (n1sv = 1; n1sv <= i__1; ++n1sv) {
+ index[i__] = ind2;
+ ++i__;
+ ind2 += *dtrd2;
+/* L20: */
+ }
+ } else {
+/* N2SV .EQ. 0 */
+ i__1 = n1sv;
+ for (n2sv = 1; n2sv <= i__1; ++n2sv) {
+ index[i__] = ind1;
+ ++i__;
+ ind1 += *dtrd1;
+/* L30: */
+ }
+ }
+
+ return 0;
+
+/* End of DLAMRG */
+
+} /* dlamrg_ */
diff --git a/3rdparty/clapack/src/dlange.c b/3rdparty/clapack/src/dlange.c
new file mode 100644
index 0000000..34c3039
--- /dev/null
+++ b/3rdparty/clapack/src/dlange.c
@@ -0,0 +1,199 @@
+/* dlange.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+doublereal dlange_(char *norm, integer *m, integer *n, doublereal *a, integer
+ *lda, doublereal *work)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+ doublereal ret_val, d__1, d__2, d__3;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer i__, j;
+ doublereal sum, scale;
+ extern logical lsame_(char *, char *);
+ doublereal value;
+ extern /* Subroutine */ int dlassq_(integer *, doublereal *, integer *,
+ doublereal *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLANGE returns the value of the one norm, or the Frobenius norm, or */
+/* the infinity norm, or the element of largest absolute value of a */
+/* real matrix A. */
+
+/* Description */
+/* =========== */
+
+/* DLANGE returns the value */
+
+/* DLANGE = ( max(abs(A(i,j))), NORM = 'M' or 'm' */
+/* ( */
+/* ( norm1(A), NORM = '1', 'O' or 'o' */
+/* ( */
+/* ( normI(A), NORM = 'I' or 'i' */
+/* ( */
+/* ( normF(A), NORM = 'F', 'f', 'E' or 'e' */
+
+/* where norm1 denotes the one norm of a matrix (maximum column sum), */
+/* normI denotes the infinity norm of a matrix (maximum row sum) and */
+/* normF denotes the Frobenius norm of a matrix (square root of sum of */
+/* squares). Note that max(abs(A(i,j))) is not a consistent matrix norm. */
+
+/* Arguments */
+/* ========= */
+
+/* NORM (input) CHARACTER*1 */
+/* Specifies the value to be returned in DLANGE as described */
+/* above. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. When M = 0, */
+/* DLANGE is set to zero. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. When N = 0, */
+/* DLANGE is set to zero. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The m by n matrix A. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(M,1). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (MAX(1,LWORK)), */
+/* where LWORK >= M when NORM = 'I'; otherwise, WORK is not */
+/* referenced. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --work;
+
+ /* Function Body */
+ if (min(*m,*n) == 0) {
+ value = 0.;
+ } else if (lsame_(norm, "M")) {
+
+/* Find max(abs(A(i,j))). */
+
+ value = 0.;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+/* Computing MAX */
+ d__2 = value, d__3 = (d__1 = a[i__ + j * a_dim1], abs(d__1));
+ value = max(d__2,d__3);
+/* L10: */
+ }
+/* L20: */
+ }
+ } else if (lsame_(norm, "O") || *(unsigned char *)
+ norm == '1') {
+
+/* Find norm1(A). */
+
+ value = 0.;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ sum = 0.;
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ sum += (d__1 = a[i__ + j * a_dim1], abs(d__1));
+/* L30: */
+ }
+ value = max(value,sum);
+/* L40: */
+ }
+ } else if (lsame_(norm, "I")) {
+
+/* Find normI(A). */
+
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ work[i__] = 0.;
+/* L50: */
+ }
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ work[i__] += (d__1 = a[i__ + j * a_dim1], abs(d__1));
+/* L60: */
+ }
+/* L70: */
+ }
+ value = 0.;
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__1 = value, d__2 = work[i__];
+ value = max(d__1,d__2);
+/* L80: */
+ }
+ } else if (lsame_(norm, "F") || lsame_(norm, "E")) {
+
+/* Find normF(A). */
+
+ scale = 0.;
+ sum = 1.;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ dlassq_(m, &a[j * a_dim1 + 1], &c__1, &scale, &sum);
+/* L90: */
+ }
+ value = scale * sqrt(sum);
+ }
+
+ ret_val = value;
+ return ret_val;
+
+/* End of DLANGE */
+
+} /* dlange_ */
diff --git a/3rdparty/clapack/src/dlanst.c b/3rdparty/clapack/src/dlanst.c
new file mode 100644
index 0000000..323713d
--- /dev/null
+++ b/3rdparty/clapack/src/dlanst.c
@@ -0,0 +1,166 @@
+/* dlanst.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+doublereal dlanst_(char *norm, integer *n, doublereal *d__, doublereal *e)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal ret_val, d__1, d__2, d__3, d__4, d__5;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer i__;
+ doublereal sum, scale;
+ extern logical lsame_(char *, char *);
+ doublereal anorm;
+ extern /* Subroutine */ int dlassq_(integer *, doublereal *, integer *,
+ doublereal *, doublereal *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLANST returns the value of the one norm, or the Frobenius norm, or */
+/* the infinity norm, or the element of largest absolute value of a */
+/* real symmetric tridiagonal matrix A. */
+
+/* Description */
+/* =========== */
+
+/* DLANST returns the value */
+
+/* DLANST = ( max(abs(A(i,j))), NORM = 'M' or 'm' */
+/* ( */
+/* ( norm1(A), NORM = '1', 'O' or 'o' */
+/* ( */
+/* ( normI(A), NORM = 'I' or 'i' */
+/* ( */
+/* ( normF(A), NORM = 'F', 'f', 'E' or 'e' */
+
+/* where norm1 denotes the one norm of a matrix (maximum column sum), */
+/* normI denotes the infinity norm of a matrix (maximum row sum) and */
+/* normF denotes the Frobenius norm of a matrix (square root of sum of */
+/* squares). Note that max(abs(A(i,j))) is not a consistent matrix norm. */
+
+/* Arguments */
+/* ========= */
+
+/* NORM (input) CHARACTER*1 */
+/* Specifies the value to be returned in DLANST as described */
+/* above. */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. When N = 0, DLANST is */
+/* set to zero. */
+
+/* D (input) DOUBLE PRECISION array, dimension (N) */
+/* The diagonal elements of A. */
+
+/* E (input) DOUBLE PRECISION array, dimension (N-1) */
+/* The (n-1) sub-diagonal or super-diagonal elements of A. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --e;
+ --d__;
+
+ /* Function Body */
+ if (*n <= 0) {
+ anorm = 0.;
+ } else if (lsame_(norm, "M")) {
+
+/* Find max(abs(A(i,j))). */
+
+ anorm = (d__1 = d__[*n], abs(d__1));
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__2 = anorm, d__3 = (d__1 = d__[i__], abs(d__1));
+ anorm = max(d__2,d__3);
+/* Computing MAX */
+ d__2 = anorm, d__3 = (d__1 = e[i__], abs(d__1));
+ anorm = max(d__2,d__3);
+/* L10: */
+ }
+ } else if (lsame_(norm, "O") || *(unsigned char *)
+ norm == '1' || lsame_(norm, "I")) {
+
+/* Find norm1(A). */
+
+ if (*n == 1) {
+ anorm = abs(d__[1]);
+ } else {
+/* Computing MAX */
+ d__3 = abs(d__[1]) + abs(e[1]), d__4 = (d__1 = e[*n - 1], abs(
+ d__1)) + (d__2 = d__[*n], abs(d__2));
+ anorm = max(d__3,d__4);
+ i__1 = *n - 1;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__4 = anorm, d__5 = (d__1 = d__[i__], abs(d__1)) + (d__2 = e[
+ i__], abs(d__2)) + (d__3 = e[i__ - 1], abs(d__3));
+ anorm = max(d__4,d__5);
+/* L20: */
+ }
+ }
+ } else if (lsame_(norm, "F") || lsame_(norm, "E")) {
+
+/* Find normF(A). */
+
+ scale = 0.;
+ sum = 1.;
+ if (*n > 1) {
+ i__1 = *n - 1;
+ dlassq_(&i__1, &e[1], &c__1, &scale, &sum);
+ sum *= 2;
+ }
+ dlassq_(n, &d__[1], &c__1, &scale, &sum);
+ anorm = scale * sqrt(sum);
+ }
+
+ ret_val = anorm;
+ return ret_val;
+
+/* End of DLANST */
+
+} /* dlanst_ */
diff --git a/3rdparty/clapack/src/dlapy2.c b/3rdparty/clapack/src/dlapy2.c
new file mode 100644
index 0000000..6e88cd1
--- /dev/null
+++ b/3rdparty/clapack/src/dlapy2.c
@@ -0,0 +1,73 @@
+/* dlapy2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+doublereal dlapy2_(doublereal *x, doublereal *y)
+{
+ /* System generated locals */
+ doublereal ret_val, d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal w, z__, xabs, yabs;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAPY2 returns sqrt(x**2+y**2), taking care not to cause unnecessary */
+/* overflow. */
+
+/* Arguments */
+/* ========= */
+
+/* X (input) DOUBLE PRECISION */
+/* Y (input) DOUBLE PRECISION */
+/* X and Y specify the values x and y. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ xabs = abs(*x);
+ yabs = abs(*y);
+ w = max(xabs,yabs);
+ z__ = min(xabs,yabs);
+ if (z__ == 0.) {
+ ret_val = w;
+ } else {
+/* Computing 2nd power */
+ d__1 = z__ / w;
+ ret_val = w * sqrt(d__1 * d__1 + 1.);
+ }
+ return ret_val;
+
+/* End of DLAPY2 */
+
+} /* dlapy2_ */
diff --git a/3rdparty/clapack/src/dlarf.c b/3rdparty/clapack/src/dlarf.c
new file mode 100644
index 0000000..aba8a59
--- /dev/null
+++ b/3rdparty/clapack/src/dlarf.c
@@ -0,0 +1,193 @@
+/* dlarf.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static doublereal c_b4 = 1.;
+static doublereal c_b5 = 0.;
+static integer c__1 = 1;
+
+/* Subroutine */ int dlarf_(char *side, integer *m, integer *n, doublereal *v,
+ integer *incv, doublereal *tau, doublereal *c__, integer *ldc,
+ doublereal *work)
+{
+ /* System generated locals */
+ integer c_dim1, c_offset;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__;
+ logical applyleft;
+ extern /* Subroutine */ int dger_(integer *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dgemv_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, doublereal *, integer *);
+ integer lastc, lastv;
+ extern integer iladlc_(integer *, integer *, doublereal *, integer *),
+ iladlr_(integer *, integer *, doublereal *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARF applies a real elementary reflector H to a real m by n matrix */
+/* C, from either the left or the right. H is represented in the form */
+
+/* H = I - tau * v * v' */
+
+/* where tau is a real scalar and v is a real vector. */
+
+/* If tau = 0, then H is taken to be the unit matrix. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': form H * C */
+/* = 'R': form C * H */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. */
+
+/* V (input) DOUBLE PRECISION array, dimension */
+/* (1 + (M-1)*abs(INCV)) if SIDE = 'L' */
+/* or (1 + (N-1)*abs(INCV)) if SIDE = 'R' */
+/* The vector v in the representation of H. V is not used if */
+/* TAU = 0. */
+
+/* INCV (input) INTEGER */
+/* The increment between elements of v. INCV <> 0. */
+
+/* TAU (input) DOUBLE PRECISION */
+/* The value tau in the representation of H. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the m by n matrix C. */
+/* On exit, C is overwritten by the matrix H * C if SIDE = 'L', */
+/* or C * H if SIDE = 'R'. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension */
+/* (N) if SIDE = 'L' */
+/* or (M) if SIDE = 'R' */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --v;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ applyleft = lsame_(side, "L");
+ lastv = 0;
+ lastc = 0;
+ if (*tau != 0.) {
+/* Set up variables for scanning V. LASTV begins pointing to the end */
+/* of V. */
+ if (applyleft) {
+ lastv = *m;
+ } else {
+ lastv = *n;
+ }
+ if (*incv > 0) {
+ i__ = (lastv - 1) * *incv + 1;
+ } else {
+ i__ = 1;
+ }
+/* Look for the last non-zero row in V. */
+ while(lastv > 0 && v[i__] == 0.) {
+ --lastv;
+ i__ -= *incv;
+ }
+ if (applyleft) {
+/* Scan for the last non-zero column in C(1:lastv,:). */
+ lastc = iladlc_(&lastv, n, &c__[c_offset], ldc);
+ } else {
+/* Scan for the last non-zero row in C(:,1:lastv). */
+ lastc = iladlr_(m, &lastv, &c__[c_offset], ldc);
+ }
+ }
+/* Note that lastc.eq.0 renders the BLAS operations null; no special */
+/* case is needed at this level. */
+ if (applyleft) {
+
+/* Form H * C */
+
+ if (lastv > 0) {
+
+/* w(1:lastc,1) := C(1:lastv,1:lastc)' * v(1:lastv,1) */
+
+ dgemv_("Transpose", &lastv, &lastc, &c_b4, &c__[c_offset], ldc, &
+ v[1], incv, &c_b5, &work[1], &c__1);
+
+/* C(1:lastv,1:lastc) := C(...) - v(1:lastv,1) * w(1:lastc,1)' */
+
+ d__1 = -(*tau);
+ dger_(&lastv, &lastc, &d__1, &v[1], incv, &work[1], &c__1, &c__[
+ c_offset], ldc);
+ }
+ } else {
+
+/* Form C * H */
+
+ if (lastv > 0) {
+
+/* w(1:lastc,1) := C(1:lastc,1:lastv) * v(1:lastv,1) */
+
+ dgemv_("No transpose", &lastc, &lastv, &c_b4, &c__[c_offset], ldc,
+ &v[1], incv, &c_b5, &work[1], &c__1);
+
+/* C(1:lastc,1:lastv) := C(...) - w(1:lastc,1) * v(1:lastv,1)' */
+
+ d__1 = -(*tau);
+ dger_(&lastc, &lastv, &d__1, &work[1], &c__1, &v[1], incv, &c__[
+ c_offset], ldc);
+ }
+ }
+ return 0;
+
+/* End of DLARF */
+
+} /* dlarf_ */
diff --git a/3rdparty/clapack/src/dlarfb.c b/3rdparty/clapack/src/dlarfb.c
new file mode 100644
index 0000000..9833b69
--- /dev/null
+++ b/3rdparty/clapack/src/dlarfb.c
@@ -0,0 +1,774 @@
+/* dlarfb.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b14 = 1.;
+static doublereal c_b25 = -1.;
+
+/* Subroutine */ int dlarfb_(char *side, char *trans, char *direct, char *
+ storev, integer *m, integer *n, integer *k, doublereal *v, integer *
+ ldv, doublereal *t, integer *ldt, doublereal *c__, integer *ldc,
+ doublereal *work, integer *ldwork)
+{
+ /* System generated locals */
+ integer c_dim1, c_offset, t_dim1, t_offset, v_dim1, v_offset, work_dim1,
+ work_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ extern logical lsame_(char *, char *);
+ integer lastc;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *), dtrmm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer lastv;
+ extern integer iladlc_(integer *, integer *, doublereal *, integer *),
+ iladlr_(integer *, integer *, doublereal *, integer *);
+ char transt[1];
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARFB applies a real block reflector H or its transpose H' to a */
+/* real m by n matrix C, from either the left or the right. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply H or H' from the Left */
+/* = 'R': apply H or H' from the Right */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': apply H (No transpose) */
+/* = 'T': apply H' (Transpose) */
+
+/* DIRECT (input) CHARACTER*1 */
+/* Indicates how H is formed from a product of elementary */
+/* reflectors */
+/* = 'F': H = H(1) H(2) . . . H(k) (Forward) */
+/* = 'B': H = H(k) . . . H(2) H(1) (Backward) */
+
+/* STOREV (input) CHARACTER*1 */
+/* Indicates how the vectors which define the elementary */
+/* reflectors are stored: */
+/* = 'C': Columnwise */
+/* = 'R': Rowwise */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. */
+
+/* K (input) INTEGER */
+/* The order of the matrix T (= the number of elementary */
+/* reflectors whose product defines the block reflector). */
+
+/* V (input) DOUBLE PRECISION array, dimension */
+/* (LDV,K) if STOREV = 'C' */
+/* (LDV,M) if STOREV = 'R' and SIDE = 'L' */
+/* (LDV,N) if STOREV = 'R' and SIDE = 'R' */
+/* The matrix V. See further details. */
+
+/* LDV (input) INTEGER */
+/* The leading dimension of the array V. */
+/* If STOREV = 'C' and SIDE = 'L', LDV >= max(1,M); */
+/* if STOREV = 'C' and SIDE = 'R', LDV >= max(1,N); */
+/* if STOREV = 'R', LDV >= K. */
+
+/* T (input) DOUBLE PRECISION array, dimension (LDT,K) */
+/* The triangular k by k matrix T in the representation of the */
+/* block reflector. */
+
+/* LDT (input) INTEGER */
+/* The leading dimension of the array T. LDT >= K. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the m by n matrix C. */
+/* On exit, C is overwritten by H*C or H'*C or C*H or C*H'. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDA >= max(1,M). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (LDWORK,K) */
+
+/* LDWORK (input) INTEGER */
+/* The leading dimension of the array WORK. */
+/* If SIDE = 'L', LDWORK >= max(1,N); */
+/* if SIDE = 'R', LDWORK >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Quick return if possible */
+
+ /* Parameter adjustments */
+ v_dim1 = *ldv;
+ v_offset = 1 + v_dim1;
+ v -= v_offset;
+ t_dim1 = *ldt;
+ t_offset = 1 + t_dim1;
+ t -= t_offset;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ work_dim1 = *ldwork;
+ work_offset = 1 + work_dim1;
+ work -= work_offset;
+
+ /* Function Body */
+ if (*m <= 0 || *n <= 0) {
+ return 0;
+ }
+
+ if (lsame_(trans, "N")) {
+ *(unsigned char *)transt = 'T';
+ } else {
+ *(unsigned char *)transt = 'N';
+ }
+
+ if (lsame_(storev, "C")) {
+
+ if (lsame_(direct, "F")) {
+
+/* Let V = ( V1 ) (first K rows) */
+/* ( V2 ) */
+/* where V1 is unit lower triangular. */
+
+ if (lsame_(side, "L")) {
+
+/* Form H * C or H' * C where C = ( C1 ) */
+/* ( C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlr_(m, k, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlc_(&lastv, n, &c__[c_offset], ldc);
+
+/* W := C' * V = (C1'*V1 + C2'*V2) (stored in WORK) */
+
+/* W := C1' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[j + c_dim1], ldc, &work[j * work_dim1
+ + 1], &c__1);
+/* L10: */
+ }
+
+/* W := W * V1 */
+
+ dtrmm_("Right", "Lower", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C2'*V2 */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "No transpose", &lastc, k, &i__1, &
+ c_b14, &c__[*k + 1 + c_dim1], ldc, &v[*k + 1 +
+ v_dim1], ldv, &c_b14, &work[work_offset], ldwork);
+ }
+
+/* W := W * T' or W * T */
+
+ dtrmm_("Right", "Upper", transt, "Non-unit", &lastc, k, &
+ c_b14, &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - V * W' */
+
+ if (lastv > *k) {
+
+/* C2 := C2 - V2 * W' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &i__1, &lastc, k, &
+ c_b25, &v[*k + 1 + v_dim1], ldv, &work[
+ work_offset], ldwork, &c_b14, &c__[*k + 1 +
+ c_dim1], ldc);
+ }
+
+/* W := W * V1' */
+
+ dtrmm_("Right", "Lower", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+
+/* C1 := C1 - W' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[j + i__ * c_dim1] -= work[i__ + j * work_dim1];
+/* L20: */
+ }
+/* L30: */
+ }
+
+ } else if (lsame_(side, "R")) {
+
+/* Form C * H or C * H' where C = ( C1 C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlr_(n, k, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlr_(m, &lastv, &c__[c_offset], ldc);
+
+/* W := C * V = (C1*V1 + C2*V2) (stored in WORK) */
+
+/* W := C1 */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[j * c_dim1 + 1], &c__1, &work[j *
+ work_dim1 + 1], &c__1);
+/* L40: */
+ }
+
+/* W := W * V1 */
+
+ dtrmm_("Right", "Lower", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C2 * V2 */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "No transpose", &lastc, k, &i__1, &
+ c_b14, &c__[(*k + 1) * c_dim1 + 1], ldc, &v[*k +
+ 1 + v_dim1], ldv, &c_b14, &work[work_offset],
+ ldwork);
+ }
+
+/* W := W * T or W * T' */
+
+ dtrmm_("Right", "Upper", trans, "Non-unit", &lastc, k, &c_b14,
+ &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - W * V' */
+
+ if (lastv > *k) {
+
+/* C2 := C2 - W * V2' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &lastc, &i__1, k, &
+ c_b25, &work[work_offset], ldwork, &v[*k + 1 +
+ v_dim1], ldv, &c_b14, &c__[(*k + 1) * c_dim1 + 1],
+ ldc);
+ }
+
+/* W := W * V1' */
+
+ dtrmm_("Right", "Lower", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+
+/* C1 := C1 - W */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] -= work[i__ + j * work_dim1];
+/* L50: */
+ }
+/* L60: */
+ }
+ }
+
+ } else {
+
+/* Let V = ( V1 ) */
+/* ( V2 ) (last K rows) */
+/* where V2 is unit upper triangular. */
+
+ if (lsame_(side, "L")) {
+
+/* Form H * C or H' * C where C = ( C1 ) */
+/* ( C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlr_(m, k, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlc_(&lastv, n, &c__[c_offset], ldc);
+
+/* W := C' * V = (C1'*V1 + C2'*V2) (stored in WORK) */
+
+/* W := C2' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[lastv - *k + j + c_dim1], ldc, &work[
+ j * work_dim1 + 1], &c__1);
+/* L70: */
+ }
+
+/* W := W * V2 */
+
+ dtrmm_("Right", "Upper", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[lastv - *k + 1 + v_dim1], ldv, &work[
+ work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C1'*V1 */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "No transpose", &lastc, k, &i__1, &
+ c_b14, &c__[c_offset], ldc, &v[v_offset], ldv, &
+ c_b14, &work[work_offset], ldwork);
+ }
+
+/* W := W * T' or W * T */
+
+ dtrmm_("Right", "Lower", transt, "Non-unit", &lastc, k, &
+ c_b14, &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - V * W' */
+
+ if (lastv > *k) {
+
+/* C1 := C1 - V1 * W' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &i__1, &lastc, k, &
+ c_b25, &v[v_offset], ldv, &work[work_offset],
+ ldwork, &c_b14, &c__[c_offset], ldc);
+ }
+
+/* W := W * V2' */
+
+ dtrmm_("Right", "Upper", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[lastv - *k + 1 + v_dim1], ldv, &work[
+ work_offset], ldwork);
+
+/* C2 := C2 - W' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[lastv - *k + j + i__ * c_dim1] -= work[i__ + j *
+ work_dim1];
+/* L80: */
+ }
+/* L90: */
+ }
+
+ } else if (lsame_(side, "R")) {
+
+/* Form C * H or C * H' where C = ( C1 C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlr_(n, k, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlr_(m, &lastv, &c__[c_offset], ldc);
+
+/* W := C * V = (C1*V1 + C2*V2) (stored in WORK) */
+
+/* W := C2 */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[(*n - *k + j) * c_dim1 + 1], &c__1, &
+ work[j * work_dim1 + 1], &c__1);
+/* L100: */
+ }
+
+/* W := W * V2 */
+
+ dtrmm_("Right", "Upper", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[lastv - *k + 1 + v_dim1], ldv, &work[
+ work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C1 * V1 */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "No transpose", &lastc, k, &i__1, &
+ c_b14, &c__[c_offset], ldc, &v[v_offset], ldv, &
+ c_b14, &work[work_offset], ldwork);
+ }
+
+/* W := W * T or W * T' */
+
+ dtrmm_("Right", "Lower", trans, "Non-unit", &lastc, k, &c_b14,
+ &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - W * V' */
+
+ if (lastv > *k) {
+
+/* C1 := C1 - W * V1' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &lastc, &i__1, k, &
+ c_b25, &work[work_offset], ldwork, &v[v_offset],
+ ldv, &c_b14, &c__[c_offset], ldc);
+ }
+
+/* W := W * V2' */
+
+ dtrmm_("Right", "Upper", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[lastv - *k + 1 + v_dim1], ldv, &work[
+ work_offset], ldwork);
+
+/* C2 := C2 - W */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + (lastv - *k + j) * c_dim1] -= work[i__ + j *
+ work_dim1];
+/* L110: */
+ }
+/* L120: */
+ }
+ }
+ }
+
+ } else if (lsame_(storev, "R")) {
+
+ if (lsame_(direct, "F")) {
+
+/* Let V = ( V1 V2 ) (V1: first K columns) */
+/* where V1 is unit upper triangular. */
+
+ if (lsame_(side, "L")) {
+
+/* Form H * C or H' * C where C = ( C1 ) */
+/* ( C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlc_(k, m, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlc_(&lastv, n, &c__[c_offset], ldc);
+
+/* W := C' * V' = (C1'*V1' + C2'*V2') (stored in WORK) */
+
+/* W := C1' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[j + c_dim1], ldc, &work[j * work_dim1
+ + 1], &c__1);
+/* L130: */
+ }
+
+/* W := W * V1' */
+
+ dtrmm_("Right", "Upper", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C2'*V2' */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "Transpose", &lastc, k, &i__1, &c_b14,
+ &c__[*k + 1 + c_dim1], ldc, &v[(*k + 1) * v_dim1
+ + 1], ldv, &c_b14, &work[work_offset], ldwork);
+ }
+
+/* W := W * T' or W * T */
+
+ dtrmm_("Right", "Upper", transt, "Non-unit", &lastc, k, &
+ c_b14, &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - V' * W' */
+
+ if (lastv > *k) {
+
+/* C2 := C2 - V2' * W' */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "Transpose", &i__1, &lastc, k, &c_b25,
+ &v[(*k + 1) * v_dim1 + 1], ldv, &work[
+ work_offset], ldwork, &c_b14, &c__[*k + 1 +
+ c_dim1], ldc);
+ }
+
+/* W := W * V1 */
+
+ dtrmm_("Right", "Upper", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+
+/* C1 := C1 - W' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[j + i__ * c_dim1] -= work[i__ + j * work_dim1];
+/* L140: */
+ }
+/* L150: */
+ }
+
+ } else if (lsame_(side, "R")) {
+
+/* Form C * H or C * H' where C = ( C1 C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlc_(k, n, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlr_(m, &lastv, &c__[c_offset], ldc);
+
+/* W := C * V' = (C1*V1' + C2*V2') (stored in WORK) */
+
+/* W := C1 */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[j * c_dim1 + 1], &c__1, &work[j *
+ work_dim1 + 1], &c__1);
+/* L160: */
+ }
+
+/* W := W * V1' */
+
+ dtrmm_("Right", "Upper", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C2 * V2' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &lastc, k, &i__1, &
+ c_b14, &c__[(*k + 1) * c_dim1 + 1], ldc, &v[(*k +
+ 1) * v_dim1 + 1], ldv, &c_b14, &work[work_offset],
+ ldwork);
+ }
+
+/* W := W * T or W * T' */
+
+ dtrmm_("Right", "Upper", trans, "Non-unit", &lastc, k, &c_b14,
+ &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - W * V */
+
+ if (lastv > *k) {
+
+/* C2 := C2 - W * V2 */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "No transpose", &lastc, &i__1, k, &
+ c_b25, &work[work_offset], ldwork, &v[(*k + 1) *
+ v_dim1 + 1], ldv, &c_b14, &c__[(*k + 1) * c_dim1
+ + 1], ldc);
+ }
+
+/* W := W * V1 */
+
+ dtrmm_("Right", "Upper", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[v_offset], ldv, &work[work_offset], ldwork);
+
+/* C1 := C1 - W */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + j * c_dim1] -= work[i__ + j * work_dim1];
+/* L170: */
+ }
+/* L180: */
+ }
+
+ }
+
+ } else {
+
+/* Let V = ( V1 V2 ) (V2: last K columns) */
+/* where V2 is unit lower triangular. */
+
+ if (lsame_(side, "L")) {
+
+/* Form H * C or H' * C where C = ( C1 ) */
+/* ( C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlc_(k, m, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlc_(&lastv, n, &c__[c_offset], ldc);
+
+/* W := C' * V' = (C1'*V1' + C2'*V2') (stored in WORK) */
+
+/* W := C2' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[lastv - *k + j + c_dim1], ldc, &work[
+ j * work_dim1 + 1], &c__1);
+/* L190: */
+ }
+
+/* W := W * V2' */
+
+ dtrmm_("Right", "Lower", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[(lastv - *k + 1) * v_dim1 + 1], ldv, &work[
+ work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C1'*V1' */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "Transpose", &lastc, k, &i__1, &c_b14,
+ &c__[c_offset], ldc, &v[v_offset], ldv, &c_b14, &
+ work[work_offset], ldwork);
+ }
+
+/* W := W * T' or W * T */
+
+ dtrmm_("Right", "Lower", transt, "Non-unit", &lastc, k, &
+ c_b14, &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - V' * W' */
+
+ if (lastv > *k) {
+
+/* C1 := C1 - V1' * W' */
+
+ i__1 = lastv - *k;
+ dgemm_("Transpose", "Transpose", &i__1, &lastc, k, &c_b25,
+ &v[v_offset], ldv, &work[work_offset], ldwork, &
+ c_b14, &c__[c_offset], ldc);
+ }
+
+/* W := W * V2 */
+
+ dtrmm_("Right", "Lower", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[(lastv - *k + 1) * v_dim1 + 1], ldv, &work[
+ work_offset], ldwork);
+
+/* C2 := C2 - W' */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[lastv - *k + j + i__ * c_dim1] -= work[i__ + j *
+ work_dim1];
+/* L200: */
+ }
+/* L210: */
+ }
+
+ } else if (lsame_(side, "R")) {
+
+/* Form C * H or C * H' where C = ( C1 C2 ) */
+
+/* Computing MAX */
+ i__1 = *k, i__2 = iladlc_(k, n, &v[v_offset], ldv);
+ lastv = max(i__1,i__2);
+ lastc = iladlr_(m, &lastv, &c__[c_offset], ldc);
+
+/* W := C * V' = (C1*V1' + C2*V2') (stored in WORK) */
+
+/* W := C2 */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dcopy_(&lastc, &c__[(lastv - *k + j) * c_dim1 + 1], &c__1,
+ &work[j * work_dim1 + 1], &c__1);
+/* L220: */
+ }
+
+/* W := W * V2' */
+
+ dtrmm_("Right", "Lower", "Transpose", "Unit", &lastc, k, &
+ c_b14, &v[(lastv - *k + 1) * v_dim1 + 1], ldv, &work[
+ work_offset], ldwork);
+ if (lastv > *k) {
+
+/* W := W + C1 * V1' */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "Transpose", &lastc, k, &i__1, &
+ c_b14, &c__[c_offset], ldc, &v[v_offset], ldv, &
+ c_b14, &work[work_offset], ldwork);
+ }
+
+/* W := W * T or W * T' */
+
+ dtrmm_("Right", "Lower", trans, "Non-unit", &lastc, k, &c_b14,
+ &t[t_offset], ldt, &work[work_offset], ldwork);
+
+/* C := C - W * V */
+
+ if (lastv > *k) {
+
+/* C1 := C1 - W * V1 */
+
+ i__1 = lastv - *k;
+ dgemm_("No transpose", "No transpose", &lastc, &i__1, k, &
+ c_b25, &work[work_offset], ldwork, &v[v_offset],
+ ldv, &c_b14, &c__[c_offset], ldc);
+ }
+
+/* W := W * V2 */
+
+ dtrmm_("Right", "Lower", "No transpose", "Unit", &lastc, k, &
+ c_b14, &v[(lastv - *k + 1) * v_dim1 + 1], ldv, &work[
+ work_offset], ldwork);
+
+/* C1 := C1 - W */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = lastc;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ c__[i__ + (lastv - *k + j) * c_dim1] -= work[i__ + j *
+ work_dim1];
+/* L230: */
+ }
+/* L240: */
+ }
+
+ }
+
+ }
+ }
+
+ return 0;
+
+/* End of DLARFB */
+
+} /* dlarfb_ */
diff --git a/3rdparty/clapack/src/dlarfg.c b/3rdparty/clapack/src/dlarfg.c
new file mode 100644
index 0000000..2a052ca
--- /dev/null
+++ b/3rdparty/clapack/src/dlarfg.c
@@ -0,0 +1,170 @@
+/* dlarfg.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlarfg_(integer *n, doublereal *alpha, doublereal *x,
+ integer *incx, doublereal *tau)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1;
+
+ /* Builtin functions */
+ double d_sign(doublereal *, doublereal *);
+
+ /* Local variables */
+ integer j, knt;
+ doublereal beta;
+ extern doublereal dnrm2_(integer *, doublereal *, integer *);
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ doublereal xnorm;
+ extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
+ doublereal safmin, rsafmn;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARFG generates a real elementary reflector H of order n, such */
+/* that */
+
+/* H * ( alpha ) = ( beta ), H' * H = I. */
+/* ( x ) ( 0 ) */
+
+/* where alpha and beta are scalars, and x is an (n-1)-element real */
+/* vector. H is represented in the form */
+
+/* H = I - tau * ( 1 ) * ( 1 v' ) , */
+/* ( v ) */
+
+/* where tau is a real scalar and v is a real (n-1)-element */
+/* vector. */
+
+/* If the elements of x are all zero, then tau = 0 and H is taken to be */
+/* the unit matrix. */
+
+/* Otherwise 1 <= tau <= 2. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The order of the elementary reflector. */
+
+/* ALPHA (input/output) DOUBLE PRECISION */
+/* On entry, the value alpha. */
+/* On exit, it is overwritten with the value beta. */
+
+/* X (input/output) DOUBLE PRECISION array, dimension */
+/* (1+(N-2)*abs(INCX)) */
+/* On entry, the vector x. */
+/* On exit, it is overwritten with the vector v. */
+
+/* INCX (input) INTEGER */
+/* The increment between elements of X. INCX > 0. */
+
+/* TAU (output) DOUBLE PRECISION */
+/* The value tau. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ if (*n <= 1) {
+ *tau = 0.;
+ return 0;
+ }
+
+ i__1 = *n - 1;
+ xnorm = dnrm2_(&i__1, &x[1], incx);
+
+ if (xnorm == 0.) {
+
+/* H = I */
+
+ *tau = 0.;
+ } else {
+
+/* general case */
+
+ d__1 = dlapy2_(alpha, &xnorm);
+ beta = -d_sign(&d__1, alpha);
+ safmin = dlamch_("S") / dlamch_("E");
+ knt = 0;
+ if (abs(beta) < safmin) {
+
+/* XNORM, BETA may be inaccurate; scale X and recompute them */
+
+ rsafmn = 1. / safmin;
+L10:
+ ++knt;
+ i__1 = *n - 1;
+ dscal_(&i__1, &rsafmn, &x[1], incx);
+ beta *= rsafmn;
+ *alpha *= rsafmn;
+ if (abs(beta) < safmin) {
+ goto L10;
+ }
+
+/* New BETA is at most 1, at least SAFMIN */
+
+ i__1 = *n - 1;
+ xnorm = dnrm2_(&i__1, &x[1], incx);
+ d__1 = dlapy2_(alpha, &xnorm);
+ beta = -d_sign(&d__1, alpha);
+ }
+ *tau = (beta - *alpha) / beta;
+ i__1 = *n - 1;
+ d__1 = 1. / (*alpha - beta);
+ dscal_(&i__1, &d__1, &x[1], incx);
+
+/* If ALPHA is subnormal, it may lose relative accuracy */
+
+ i__1 = knt;
+ for (j = 1; j <= i__1; ++j) {
+ beta *= safmin;
+/* L20: */
+ }
+ *alpha = beta;
+ }
+
+ return 0;
+
+/* End of DLARFG */
+
+} /* dlarfg_ */
diff --git a/3rdparty/clapack/src/dlarfp.c b/3rdparty/clapack/src/dlarfp.c
new file mode 100644
index 0000000..234ee7a
--- /dev/null
+++ b/3rdparty/clapack/src/dlarfp.c
@@ -0,0 +1,192 @@
+/* dlarfp.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlarfp_(integer *n, doublereal *alpha, doublereal *x,
+ integer *incx, doublereal *tau)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1;
+
+ /* Builtin functions */
+ double d_sign(doublereal *, doublereal *);
+
+ /* Local variables */
+ integer j, knt;
+ doublereal beta;
+ extern doublereal dnrm2_(integer *, doublereal *, integer *);
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ doublereal xnorm;
+ extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
+ doublereal safmin, rsafmn;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARFP generates a real elementary reflector H of order n, such */
+/* that */
+
+/* H * ( alpha ) = ( beta ), H' * H = I. */
+/* ( x ) ( 0 ) */
+
+/* where alpha and beta are scalars, beta is non-negative, and x is */
+/* an (n-1)-element real vector. H is represented in the form */
+
+/* H = I - tau * ( 1 ) * ( 1 v' ) , */
+/* ( v ) */
+
+/* where tau is a real scalar and v is a real (n-1)-element */
+/* vector. */
+
+/* If the elements of x are all zero, then tau = 0 and H is taken to be */
+/* the unit matrix. */
+
+/* Otherwise 1 <= tau <= 2. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The order of the elementary reflector. */
+
+/* ALPHA (input/output) DOUBLE PRECISION */
+/* On entry, the value alpha. */
+/* On exit, it is overwritten with the value beta. */
+
+/* X (input/output) DOUBLE PRECISION array, dimension */
+/* (1+(N-2)*abs(INCX)) */
+/* On entry, the vector x. */
+/* On exit, it is overwritten with the vector v. */
+
+/* INCX (input) INTEGER */
+/* The increment between elements of X. INCX > 0. */
+
+/* TAU (output) DOUBLE PRECISION */
+/* The value tau. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ if (*n <= 0) {
+ *tau = 0.;
+ return 0;
+ }
+
+ i__1 = *n - 1;
+ xnorm = dnrm2_(&i__1, &x[1], incx);
+
+ if (xnorm == 0.) {
+
+/* H = [+/-1, 0; I], sign chosen so ALPHA >= 0 */
+
+ if (*alpha >= 0.) {
+/* When TAU.eq.ZERO, the vector is special-cased to be */
+/* all zeros in the application routines. We do not need */
+/* to clear it. */
+ *tau = 0.;
+ } else {
+/* However, the application routines rely on explicit */
+/* zero checks when TAU.ne.ZERO, and we must clear X. */
+ *tau = 2.;
+ i__1 = *n - 1;
+ for (j = 1; j <= i__1; ++j) {
+ x[(j - 1) * *incx + 1] = 0.;
+ }
+ *alpha = -(*alpha);
+ }
+ } else {
+
+/* general case */
+
+ d__1 = dlapy2_(alpha, &xnorm);
+ beta = d_sign(&d__1, alpha);
+ safmin = dlamch_("S") / dlamch_("E");
+ knt = 0;
+ if (abs(beta) < safmin) {
+
+/* XNORM, BETA may be inaccurate; scale X and recompute them */
+
+ rsafmn = 1. / safmin;
+L10:
+ ++knt;
+ i__1 = *n - 1;
+ dscal_(&i__1, &rsafmn, &x[1], incx);
+ beta *= rsafmn;
+ *alpha *= rsafmn;
+ if (abs(beta) < safmin) {
+ goto L10;
+ }
+
+/* New BETA is at most 1, at least SAFMIN */
+
+ i__1 = *n - 1;
+ xnorm = dnrm2_(&i__1, &x[1], incx);
+ d__1 = dlapy2_(alpha, &xnorm);
+ beta = d_sign(&d__1, alpha);
+ }
+ *alpha += beta;
+ if (beta < 0.) {
+ beta = -beta;
+ *tau = -(*alpha) / beta;
+ } else {
+ *alpha = xnorm * (xnorm / *alpha);
+ *tau = *alpha / beta;
+ *alpha = -(*alpha);
+ }
+ i__1 = *n - 1;
+ d__1 = 1. / *alpha;
+ dscal_(&i__1, &d__1, &x[1], incx);
+
+/* If BETA is subnormal, it may lose relative accuracy */
+
+ i__1 = knt;
+ for (j = 1; j <= i__1; ++j) {
+ beta *= safmin;
+/* L20: */
+ }
+ *alpha = beta;
+ }
+
+ return 0;
+
+/* End of DLARFP */
+
+} /* dlarfp_ */
diff --git a/3rdparty/clapack/src/dlarft.c b/3rdparty/clapack/src/dlarft.c
new file mode 100644
index 0000000..0d4951c
--- /dev/null
+++ b/3rdparty/clapack/src/dlarft.c
@@ -0,0 +1,325 @@
+/* dlarft.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b8 = 0.;
+
+/* Subroutine */ int dlarft_(char *direct, char *storev, integer *n, integer *
+ k, doublereal *v, integer *ldv, doublereal *tau, doublereal *t,
+ integer *ldt)
+{
+ /* System generated locals */
+ integer t_dim1, t_offset, v_dim1, v_offset, i__1, i__2, i__3;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__, j, prevlastv;
+ doublereal vii;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dgemv_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, doublereal *, integer *);
+ integer lastv;
+ extern /* Subroutine */ int dtrmv_(char *, char *, char *, integer *,
+ doublereal *, integer *, doublereal *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARFT forms the triangular factor T of a real block reflector H */
+/* of order n, which is defined as a product of k elementary reflectors. */
+
+/* If DIRECT = 'F', H = H(1) H(2) . . . H(k) and T is upper triangular; */
+
+/* If DIRECT = 'B', H = H(k) . . . H(2) H(1) and T is lower triangular. */
+
+/* If STOREV = 'C', the vector which defines the elementary reflector */
+/* H(i) is stored in the i-th column of the array V, and */
+
+/* H = I - V * T * V' */
+
+/* If STOREV = 'R', the vector which defines the elementary reflector */
+/* H(i) is stored in the i-th row of the array V, and */
+
+/* H = I - V' * T * V */
+
+/* Arguments */
+/* ========= */
+
+/* DIRECT (input) CHARACTER*1 */
+/* Specifies the order in which the elementary reflectors are */
+/* multiplied to form the block reflector: */
+/* = 'F': H = H(1) H(2) . . . H(k) (Forward) */
+/* = 'B': H = H(k) . . . H(2) H(1) (Backward) */
+
+/* STOREV (input) CHARACTER*1 */
+/* Specifies how the vectors which define the elementary */
+/* reflectors are stored (see also Further Details): */
+/* = 'C': columnwise */
+/* = 'R': rowwise */
+
+/* N (input) INTEGER */
+/* The order of the block reflector H. N >= 0. */
+
+/* K (input) INTEGER */
+/* The order of the triangular factor T (= the number of */
+/* elementary reflectors). K >= 1. */
+
+/* V (input/output) DOUBLE PRECISION array, dimension */
+/* (LDV,K) if STOREV = 'C' */
+/* (LDV,N) if STOREV = 'R' */
+/* The matrix V. See further details. */
+
+/* LDV (input) INTEGER */
+/* The leading dimension of the array V. */
+/* If STOREV = 'C', LDV >= max(1,N); if STOREV = 'R', LDV >= K. */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i). */
+
+/* T (output) DOUBLE PRECISION array, dimension (LDT,K) */
+/* The k by k triangular factor T of the block reflector. */
+/* If DIRECT = 'F', T is upper triangular; if DIRECT = 'B', T is */
+/* lower triangular. The rest of the array is not used. */
+
+/* LDT (input) INTEGER */
+/* The leading dimension of the array T. LDT >= K. */
+
+/* Further Details */
+/* =============== */
+
+/* The shape of the matrix V and the storage of the vectors which define */
+/* the H(i) is best illustrated by the following example with n = 5 and */
+/* k = 3. The elements equal to 1 are not stored; the corresponding */
+/* array elements are modified but restored on exit. The rest of the */
+/* array is not used. */
+
+/* DIRECT = 'F' and STOREV = 'C': DIRECT = 'F' and STOREV = 'R': */
+
+/* V = ( 1 ) V = ( 1 v1 v1 v1 v1 ) */
+/* ( v1 1 ) ( 1 v2 v2 v2 ) */
+/* ( v1 v2 1 ) ( 1 v3 v3 ) */
+/* ( v1 v2 v3 ) */
+/* ( v1 v2 v3 ) */
+
+/* DIRECT = 'B' and STOREV = 'C': DIRECT = 'B' and STOREV = 'R': */
+
+/* V = ( v1 v2 v3 ) V = ( v1 v1 1 ) */
+/* ( v1 v2 v3 ) ( v2 v2 v2 1 ) */
+/* ( 1 v2 v3 ) ( v3 v3 v3 v3 1 ) */
+/* ( 1 v3 ) */
+/* ( 1 ) */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Quick return if possible */
+
+ /* Parameter adjustments */
+ v_dim1 = *ldv;
+ v_offset = 1 + v_dim1;
+ v -= v_offset;
+ --tau;
+ t_dim1 = *ldt;
+ t_offset = 1 + t_dim1;
+ t -= t_offset;
+
+ /* Function Body */
+ if (*n == 0) {
+ return 0;
+ }
+
+ if (lsame_(direct, "F")) {
+ prevlastv = *n;
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ prevlastv = max(i__,prevlastv);
+ if (tau[i__] == 0.) {
+
+/* H(i) = I */
+
+ i__2 = i__;
+ for (j = 1; j <= i__2; ++j) {
+ t[j + i__ * t_dim1] = 0.;
+/* L10: */
+ }
+ } else {
+
+/* general case */
+
+ vii = v[i__ + i__ * v_dim1];
+ v[i__ + i__ * v_dim1] = 1.;
+ if (lsame_(storev, "C")) {
+/* Skip any trailing zeros. */
+ i__2 = i__ + 1;
+ for (lastv = *n; lastv >= i__2; --lastv) {
+ if (v[lastv + i__ * v_dim1] != 0.) {
+ break;
+ }
+ }
+ j = min(lastv,prevlastv);
+
+/* T(1:i-1,i) := - tau(i) * V(i:j,1:i-1)' * V(i:j,i) */
+
+ i__2 = j - i__ + 1;
+ i__3 = i__ - 1;
+ d__1 = -tau[i__];
+ dgemv_("Transpose", &i__2, &i__3, &d__1, &v[i__ + v_dim1],
+ ldv, &v[i__ + i__ * v_dim1], &c__1, &c_b8, &t[
+ i__ * t_dim1 + 1], &c__1);
+ } else {
+/* Skip any trailing zeros. */
+ i__2 = i__ + 1;
+ for (lastv = *n; lastv >= i__2; --lastv) {
+ if (v[i__ + lastv * v_dim1] != 0.) {
+ break;
+ }
+ }
+ j = min(lastv,prevlastv);
+
+/* T(1:i-1,i) := - tau(i) * V(1:i-1,i:j) * V(i,i:j)' */
+
+ i__2 = i__ - 1;
+ i__3 = j - i__ + 1;
+ d__1 = -tau[i__];
+ dgemv_("No transpose", &i__2, &i__3, &d__1, &v[i__ *
+ v_dim1 + 1], ldv, &v[i__ + i__ * v_dim1], ldv, &
+ c_b8, &t[i__ * t_dim1 + 1], &c__1);
+ }
+ v[i__ + i__ * v_dim1] = vii;
+
+/* T(1:i-1,i) := T(1:i-1,1:i-1) * T(1:i-1,i) */
+
+ i__2 = i__ - 1;
+ dtrmv_("Upper", "No transpose", "Non-unit", &i__2, &t[
+ t_offset], ldt, &t[i__ * t_dim1 + 1], &c__1);
+ t[i__ + i__ * t_dim1] = tau[i__];
+ if (i__ > 1) {
+ prevlastv = max(prevlastv,lastv);
+ } else {
+ prevlastv = lastv;
+ }
+ }
+/* L20: */
+ }
+ } else {
+ prevlastv = 1;
+ for (i__ = *k; i__ >= 1; --i__) {
+ if (tau[i__] == 0.) {
+
+/* H(i) = I */
+
+ i__1 = *k;
+ for (j = i__; j <= i__1; ++j) {
+ t[j + i__ * t_dim1] = 0.;
+/* L30: */
+ }
+ } else {
+
+/* general case */
+
+ if (i__ < *k) {
+ if (lsame_(storev, "C")) {
+ vii = v[*n - *k + i__ + i__ * v_dim1];
+ v[*n - *k + i__ + i__ * v_dim1] = 1.;
+/* Skip any leading zeros. */
+ i__1 = i__ - 1;
+ for (lastv = 1; lastv <= i__1; ++lastv) {
+ if (v[lastv + i__ * v_dim1] != 0.) {
+ break;
+ }
+ }
+ j = max(lastv,prevlastv);
+
+/* T(i+1:k,i) := */
+/* - tau(i) * V(j:n-k+i,i+1:k)' * V(j:n-k+i,i) */
+
+ i__1 = *n - *k + i__ - j + 1;
+ i__2 = *k - i__;
+ d__1 = -tau[i__];
+ dgemv_("Transpose", &i__1, &i__2, &d__1, &v[j + (i__
+ + 1) * v_dim1], ldv, &v[j + i__ * v_dim1], &
+ c__1, &c_b8, &t[i__ + 1 + i__ * t_dim1], &
+ c__1);
+ v[*n - *k + i__ + i__ * v_dim1] = vii;
+ } else {
+ vii = v[i__ + (*n - *k + i__) * v_dim1];
+ v[i__ + (*n - *k + i__) * v_dim1] = 1.;
+/* Skip any leading zeros. */
+ i__1 = i__ - 1;
+ for (lastv = 1; lastv <= i__1; ++lastv) {
+ if (v[i__ + lastv * v_dim1] != 0.) {
+ break;
+ }
+ }
+ j = max(lastv,prevlastv);
+
+/* T(i+1:k,i) := */
+/* - tau(i) * V(i+1:k,j:n-k+i) * V(i,j:n-k+i)' */
+
+ i__1 = *k - i__;
+ i__2 = *n - *k + i__ - j + 1;
+ d__1 = -tau[i__];
+ dgemv_("No transpose", &i__1, &i__2, &d__1, &v[i__ +
+ 1 + j * v_dim1], ldv, &v[i__ + j * v_dim1],
+ ldv, &c_b8, &t[i__ + 1 + i__ * t_dim1], &c__1);
+ v[i__ + (*n - *k + i__) * v_dim1] = vii;
+ }
+
+/* T(i+1:k,i) := T(i+1:k,i+1:k) * T(i+1:k,i) */
+
+ i__1 = *k - i__;
+ dtrmv_("Lower", "No transpose", "Non-unit", &i__1, &t[i__
+ + 1 + (i__ + 1) * t_dim1], ldt, &t[i__ + 1 + i__ *
+ t_dim1], &c__1)
+ ;
+ if (i__ > 1) {
+ prevlastv = min(prevlastv,lastv);
+ } else {
+ prevlastv = lastv;
+ }
+ }
+ t[i__ + i__ * t_dim1] = tau[i__];
+ }
+/* L40: */
+ }
+ }
+ return 0;
+
+/* End of DLARFT */
+
+} /* dlarft_ */
diff --git a/3rdparty/clapack/src/dlartg.c b/3rdparty/clapack/src/dlartg.c
new file mode 100644
index 0000000..40179e5
--- /dev/null
+++ b/3rdparty/clapack/src/dlartg.c
@@ -0,0 +1,190 @@
+/* dlartg.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlartg_(doublereal *f, doublereal *g, doublereal *cs,
+ doublereal *sn, doublereal *r__)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double log(doublereal), pow_di(doublereal *, integer *), sqrt(doublereal);
+
+ /* Local variables */
+ integer i__;
+ doublereal f1, g1, eps, scale;
+ integer count;
+ doublereal safmn2, safmx2;
+ extern doublereal dlamch_(char *);
+ doublereal safmin;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLARTG generate a plane rotation so that */
+
+/* [ CS SN ] . [ F ] = [ R ] where CS**2 + SN**2 = 1. */
+/* [ -SN CS ] [ G ] [ 0 ] */
+
+/* This is a slower, more accurate version of the BLAS1 routine DROTG, */
+/* with the following other differences: */
+/* F and G are unchanged on return. */
+/* If G=0, then CS=1 and SN=0. */
+/* If F=0 and (G .ne. 0), then CS=0 and SN=1 without doing any */
+/* floating point operations (saves work in DBDSQR when */
+/* there are zeros on the diagonal). */
+
+/* If F exceeds G in magnitude, CS will be positive. */
+
+/* Arguments */
+/* ========= */
+
+/* F (input) DOUBLE PRECISION */
+/* The first component of vector to be rotated. */
+
+/* G (input) DOUBLE PRECISION */
+/* The second component of vector to be rotated. */
+
+/* CS (output) DOUBLE PRECISION */
+/* The cosine of the rotation. */
+
+/* SN (output) DOUBLE PRECISION */
+/* The sine of the rotation. */
+
+/* R (output) DOUBLE PRECISION */
+/* The nonzero component of the rotated vector. */
+
+/* This version has a few statements commented out for thread safety */
+/* (machine parameters are computed on each entry). 10 feb 03, SJH. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* LOGICAL FIRST */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Save statement .. */
+/* SAVE FIRST, SAFMX2, SAFMIN, SAFMN2 */
+/* .. */
+/* .. Data statements .. */
+/* DATA FIRST / .TRUE. / */
+/* .. */
+/* .. Executable Statements .. */
+
+/* IF( FIRST ) THEN */
+ safmin = dlamch_("S");
+ eps = dlamch_("E");
+ d__1 = dlamch_("B");
+ i__1 = (integer) (log(safmin / eps) / log(dlamch_("B")) / 2.);
+ safmn2 = pow_di(&d__1, &i__1);
+ safmx2 = 1. / safmn2;
+/* FIRST = .FALSE. */
+/* END IF */
+ if (*g == 0.) {
+ *cs = 1.;
+ *sn = 0.;
+ *r__ = *f;
+ } else if (*f == 0.) {
+ *cs = 0.;
+ *sn = 1.;
+ *r__ = *g;
+ } else {
+ f1 = *f;
+ g1 = *g;
+/* Computing MAX */
+ d__1 = abs(f1), d__2 = abs(g1);
+ scale = max(d__1,d__2);
+ if (scale >= safmx2) {
+ count = 0;
+L10:
+ ++count;
+ f1 *= safmn2;
+ g1 *= safmn2;
+/* Computing MAX */
+ d__1 = abs(f1), d__2 = abs(g1);
+ scale = max(d__1,d__2);
+ if (scale >= safmx2) {
+ goto L10;
+ }
+/* Computing 2nd power */
+ d__1 = f1;
+/* Computing 2nd power */
+ d__2 = g1;
+ *r__ = sqrt(d__1 * d__1 + d__2 * d__2);
+ *cs = f1 / *r__;
+ *sn = g1 / *r__;
+ i__1 = count;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ *r__ *= safmx2;
+/* L20: */
+ }
+ } else if (scale <= safmn2) {
+ count = 0;
+L30:
+ ++count;
+ f1 *= safmx2;
+ g1 *= safmx2;
+/* Computing MAX */
+ d__1 = abs(f1), d__2 = abs(g1);
+ scale = max(d__1,d__2);
+ if (scale <= safmn2) {
+ goto L30;
+ }
+/* Computing 2nd power */
+ d__1 = f1;
+/* Computing 2nd power */
+ d__2 = g1;
+ *r__ = sqrt(d__1 * d__1 + d__2 * d__2);
+ *cs = f1 / *r__;
+ *sn = g1 / *r__;
+ i__1 = count;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ *r__ *= safmn2;
+/* L40: */
+ }
+ } else {
+/* Computing 2nd power */
+ d__1 = f1;
+/* Computing 2nd power */
+ d__2 = g1;
+ *r__ = sqrt(d__1 * d__1 + d__2 * d__2);
+ *cs = f1 / *r__;
+ *sn = g1 / *r__;
+ }
+ if (abs(*f) > abs(*g) && *cs < 0.) {
+ *cs = -(*cs);
+ *sn = -(*sn);
+ *r__ = -(*r__);
+ }
+ }
+ return 0;
+
+/* End of DLARTG */
+
+} /* dlartg_ */
diff --git a/3rdparty/clapack/src/dlas2.c b/3rdparty/clapack/src/dlas2.c
new file mode 100644
index 0000000..1362a1a
--- /dev/null
+++ b/3rdparty/clapack/src/dlas2.c
@@ -0,0 +1,144 @@
+/* dlas2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlas2_(doublereal *f, doublereal *g, doublereal *h__,
+ doublereal *ssmin, doublereal *ssmax)
+{
+ /* System generated locals */
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal c__, fa, ga, ha, as, at, au, fhmn, fhmx;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAS2 computes the singular values of the 2-by-2 matrix */
+/* [ F G ] */
+/* [ 0 H ]. */
+/* On return, SSMIN is the smaller singular value and SSMAX is the */
+/* larger singular value. */
+
+/* Arguments */
+/* ========= */
+
+/* F (input) DOUBLE PRECISION */
+/* The (1,1) element of the 2-by-2 matrix. */
+
+/* G (input) DOUBLE PRECISION */
+/* The (1,2) element of the 2-by-2 matrix. */
+
+/* H (input) DOUBLE PRECISION */
+/* The (2,2) element of the 2-by-2 matrix. */
+
+/* SSMIN (output) DOUBLE PRECISION */
+/* The smaller singular value. */
+
+/* SSMAX (output) DOUBLE PRECISION */
+/* The larger singular value. */
+
+/* Further Details */
+/* =============== */
+
+/* Barring over/underflow, all output quantities are correct to within */
+/* a few units in the last place (ulps), even in the absence of a guard */
+/* digit in addition/subtraction. */
+
+/* In IEEE arithmetic, the code works correctly if one matrix element is */
+/* infinite. */
+
+/* Overflow will not occur unless the largest singular value itself */
+/* overflows, or is within a few ulps of overflow. (On machines with */
+/* partial overflow, like the Cray, overflow may occur if the largest */
+/* singular value is within a factor of 2 of overflow.) */
+
+/* Underflow is harmless if underflow is gradual. Otherwise, results */
+/* may correspond to a matrix modified by perturbations of size near */
+/* the underflow threshold. */
+
+/* ==================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ fa = abs(*f);
+ ga = abs(*g);
+ ha = abs(*h__);
+ fhmn = min(fa,ha);
+ fhmx = max(fa,ha);
+ if (fhmn == 0.) {
+ *ssmin = 0.;
+ if (fhmx == 0.) {
+ *ssmax = ga;
+ } else {
+/* Computing 2nd power */
+ d__1 = min(fhmx,ga) / max(fhmx,ga);
+ *ssmax = max(fhmx,ga) * sqrt(d__1 * d__1 + 1.);
+ }
+ } else {
+ if (ga < fhmx) {
+ as = fhmn / fhmx + 1.;
+ at = (fhmx - fhmn) / fhmx;
+/* Computing 2nd power */
+ d__1 = ga / fhmx;
+ au = d__1 * d__1;
+ c__ = 2. / (sqrt(as * as + au) + sqrt(at * at + au));
+ *ssmin = fhmn * c__;
+ *ssmax = fhmx / c__;
+ } else {
+ au = fhmx / ga;
+ if (au == 0.) {
+
+/* Avoid possible harmful underflow if exponent range */
+/* asymmetric (true SSMIN may not underflow even if */
+/* AU underflows) */
+
+ *ssmin = fhmn * fhmx / ga;
+ *ssmax = ga;
+ } else {
+ as = fhmn / fhmx + 1.;
+ at = (fhmx - fhmn) / fhmx;
+/* Computing 2nd power */
+ d__1 = as * au;
+/* Computing 2nd power */
+ d__2 = at * au;
+ c__ = 1. / (sqrt(d__1 * d__1 + 1.) + sqrt(d__2 * d__2 + 1.));
+ *ssmin = fhmn * c__ * au;
+ *ssmin += *ssmin;
+ *ssmax = ga / (c__ + c__);
+ }
+ }
+ }
+ return 0;
+
+/* End of DLAS2 */
+
+} /* dlas2_ */
diff --git a/3rdparty/clapack/src/dlascl.c b/3rdparty/clapack/src/dlascl.c
new file mode 100644
index 0000000..b39a68b
--- /dev/null
+++ b/3rdparty/clapack/src/dlascl.c
@@ -0,0 +1,354 @@
+/* dlascl.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlascl_(char *type__, integer *kl, integer *ku,
+ doublereal *cfrom, doublereal *cto, integer *m, integer *n,
+ doublereal *a, integer *lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4, i__5;
+
+ /* Local variables */
+ integer i__, j, k1, k2, k3, k4;
+ doublereal mul, cto1;
+ logical done;
+ doublereal ctoc;
+ extern logical lsame_(char *, char *);
+ integer itype;
+ doublereal cfrom1;
+ extern doublereal dlamch_(char *);
+ doublereal cfromc;
+ extern logical disnan_(doublereal *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ doublereal bignum, smlnum;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASCL multiplies the M by N real matrix A by the real scalar */
+/* CTO/CFROM. This is done without over/underflow as long as the final */
+/* result CTO*A(I,J)/CFROM does not over/underflow. TYPE specifies that */
+/* A may be full, upper triangular, lower triangular, upper Hessenberg, */
+/* or banded. */
+
+/* Arguments */
+/* ========= */
+
+/* TYPE (input) CHARACTER*1 */
+/* TYPE indices the storage type of the input matrix. */
+/* = 'G': A is a full matrix. */
+/* = 'L': A is a lower triangular matrix. */
+/* = 'U': A is an upper triangular matrix. */
+/* = 'H': A is an upper Hessenberg matrix. */
+/* = 'B': A is a symmetric band matrix with lower bandwidth KL */
+/* and upper bandwidth KU and with the only the lower */
+/* half stored. */
+/* = 'Q': A is a symmetric band matrix with lower bandwidth KL */
+/* and upper bandwidth KU and with the only the upper */
+/* half stored. */
+/* = 'Z': A is a band matrix with lower bandwidth KL and upper */
+/* bandwidth KU. */
+
+/* KL (input) INTEGER */
+/* The lower bandwidth of A. Referenced only if TYPE = 'B', */
+/* 'Q' or 'Z'. */
+
+/* KU (input) INTEGER */
+/* The upper bandwidth of A. Referenced only if TYPE = 'B', */
+/* 'Q' or 'Z'. */
+
+/* CFROM (input) DOUBLE PRECISION */
+/* CTO (input) DOUBLE PRECISION */
+/* The matrix A is multiplied by CTO/CFROM. A(I,J) is computed */
+/* without over/underflow if the final result CTO*A(I,J)/CFROM */
+/* can be represented without over/underflow. CFROM must be */
+/* nonzero. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The matrix to be multiplied by CTO/CFROM. See TYPE for the */
+/* storage type. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* INFO (output) INTEGER */
+/* 0 - successful exit */
+/* <0 - if INFO = -i, the i-th argument had an illegal value. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+
+ if (lsame_(type__, "G")) {
+ itype = 0;
+ } else if (lsame_(type__, "L")) {
+ itype = 1;
+ } else if (lsame_(type__, "U")) {
+ itype = 2;
+ } else if (lsame_(type__, "H")) {
+ itype = 3;
+ } else if (lsame_(type__, "B")) {
+ itype = 4;
+ } else if (lsame_(type__, "Q")) {
+ itype = 5;
+ } else if (lsame_(type__, "Z")) {
+ itype = 6;
+ } else {
+ itype = -1;
+ }
+
+ if (itype == -1) {
+ *info = -1;
+ } else if (*cfrom == 0. || disnan_(cfrom)) {
+ *info = -4;
+ } else if (disnan_(cto)) {
+ *info = -5;
+ } else if (*m < 0) {
+ *info = -6;
+ } else if (*n < 0 || itype == 4 && *n != *m || itype == 5 && *n != *m) {
+ *info = -7;
+ } else if (itype <= 3 && *lda < max(1,*m)) {
+ *info = -9;
+ } else if (itype >= 4) {
+/* Computing MAX */
+ i__1 = *m - 1;
+ if (*kl < 0 || *kl > max(i__1,0)) {
+ *info = -2;
+ } else /* if(complicated condition) */ {
+/* Computing MAX */
+ i__1 = *n - 1;
+ if (*ku < 0 || *ku > max(i__1,0) || (itype == 4 || itype == 5) &&
+ *kl != *ku) {
+ *info = -3;
+ } else if (itype == 4 && *lda < *kl + 1 || itype == 5 && *lda < *
+ ku + 1 || itype == 6 && *lda < (*kl << 1) + *ku + 1) {
+ *info = -9;
+ }
+ }
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASCL", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0 || *m == 0) {
+ return 0;
+ }
+
+/* Get machine parameters */
+
+ smlnum = dlamch_("S");
+ bignum = 1. / smlnum;
+
+ cfromc = *cfrom;
+ ctoc = *cto;
+
+L10:
+ cfrom1 = cfromc * smlnum;
+ if (cfrom1 == cfromc) {
+/* CFROMC is an inf. Multiply by a correctly signed zero for */
+/* finite CTOC, or a NaN if CTOC is infinite. */
+ mul = ctoc / cfromc;
+ done = TRUE_;
+ cto1 = ctoc;
+ } else {
+ cto1 = ctoc / bignum;
+ if (cto1 == ctoc) {
+/* CTOC is either 0 or an inf. In both cases, CTOC itself */
+/* serves as the correct multiplication factor. */
+ mul = ctoc;
+ done = TRUE_;
+ cfromc = 1.;
+ } else if (abs(cfrom1) > abs(ctoc) && ctoc != 0.) {
+ mul = smlnum;
+ done = FALSE_;
+ cfromc = cfrom1;
+ } else if (abs(cto1) > abs(cfromc)) {
+ mul = bignum;
+ done = FALSE_;
+ ctoc = cto1;
+ } else {
+ mul = ctoc / cfromc;
+ done = TRUE_;
+ }
+ }
+
+ if (itype == 0) {
+
+/* Full matrix */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L20: */
+ }
+/* L30: */
+ }
+
+ } else if (itype == 1) {
+
+/* Lower triangular matrix */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = j; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L40: */
+ }
+/* L50: */
+ }
+
+ } else if (itype == 2) {
+
+/* Upper triangular matrix */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = min(j,*m);
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L60: */
+ }
+/* L70: */
+ }
+
+ } else if (itype == 3) {
+
+/* Upper Hessenberg matrix */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+/* Computing MIN */
+ i__3 = j + 1;
+ i__2 = min(i__3,*m);
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L80: */
+ }
+/* L90: */
+ }
+
+ } else if (itype == 4) {
+
+/* Lower half of a symmetric band matrix */
+
+ k3 = *kl + 1;
+ k4 = *n + 1;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+/* Computing MIN */
+ i__3 = k3, i__4 = k4 - j;
+ i__2 = min(i__3,i__4);
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L100: */
+ }
+/* L110: */
+ }
+
+ } else if (itype == 5) {
+
+/* Upper half of a symmetric band matrix */
+
+ k1 = *ku + 2;
+ k3 = *ku + 1;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+/* Computing MAX */
+ i__2 = k1 - j;
+ i__3 = k3;
+ for (i__ = max(i__2,1); i__ <= i__3; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L120: */
+ }
+/* L130: */
+ }
+
+ } else if (itype == 6) {
+
+/* Band matrix */
+
+ k1 = *kl + *ku + 2;
+ k2 = *kl + 1;
+ k3 = (*kl << 1) + *ku + 1;
+ k4 = *kl + *ku + 1 + *m;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+/* Computing MAX */
+ i__3 = k1 - j;
+/* Computing MIN */
+ i__4 = k3, i__5 = k4 - j;
+ i__2 = min(i__4,i__5);
+ for (i__ = max(i__3,k2); i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] *= mul;
+/* L140: */
+ }
+/* L150: */
+ }
+
+ }
+
+ if (! done) {
+ goto L10;
+ }
+
+ return 0;
+
+/* End of DLASCL */
+
+} /* dlascl_ */
diff --git a/3rdparty/clapack/src/dlasd0.c b/3rdparty/clapack/src/dlasd0.c
new file mode 100644
index 0000000..a8b7197
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd0.c
@@ -0,0 +1,291 @@
+/* dlasd0.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__0 = 0;
+static integer c__2 = 2;
+
+/* Subroutine */ int dlasd0_(integer *n, integer *sqre, doublereal *d__,
+ doublereal *e, doublereal *u, integer *ldu, doublereal *vt, integer *
+ ldvt, integer *smlsiz, integer *iwork, doublereal *work, integer *
+ info)
+{
+ /* System generated locals */
+ integer u_dim1, u_offset, vt_dim1, vt_offset, i__1, i__2;
+
+ /* Builtin functions */
+ integer pow_ii(integer *, integer *);
+
+ /* Local variables */
+ integer i__, j, m, i1, ic, lf, nd, ll, nl, nr, im1, ncc, nlf, nrf, iwk,
+ lvl, ndb1, nlp1, nrp1;
+ doublereal beta;
+ integer idxq, nlvl;
+ doublereal alpha;
+ integer inode, ndiml, idxqc, ndimr, itemp, sqrei;
+ extern /* Subroutine */ int dlasd1_(integer *, integer *, integer *,
+ doublereal *, doublereal *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *, integer *, integer *, doublereal *,
+ integer *), dlasdq_(char *, integer *, integer *, integer *,
+ integer *, integer *, doublereal *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *), dlasdt_(integer *, integer *,
+ integer *, integer *, integer *, integer *, integer *), xerbla_(
+ char *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* Using a divide and conquer approach, DLASD0 computes the singular */
+/* value decomposition (SVD) of a real upper bidiagonal N-by-M */
+/* matrix B with diagonal D and offdiagonal E, where M = N + SQRE. */
+/* The algorithm computes orthogonal matrices U and VT such that */
+/* B = U * S * VT. The singular values S are overwritten on D. */
+
+/* A related subroutine, DLASDA, computes only the singular values, */
+/* and optionally, the singular vectors in compact form. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* On entry, the row dimension of the upper bidiagonal matrix. */
+/* This is also the dimension of the main diagonal array D. */
+
+/* SQRE (input) INTEGER */
+/* Specifies the column dimension of the bidiagonal matrix. */
+/* = 0: The bidiagonal matrix has column dimension M = N; */
+/* = 1: The bidiagonal matrix has column dimension M = N+1; */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry D contains the main diagonal of the bidiagonal */
+/* matrix. */
+/* On exit D, if INFO = 0, contains its singular values. */
+
+/* E (input) DOUBLE PRECISION array, dimension (M-1) */
+/* Contains the subdiagonal entries of the bidiagonal matrix. */
+/* On exit, E has been destroyed. */
+
+/* U (output) DOUBLE PRECISION array, dimension at least (LDQ, N) */
+/* On exit, U contains the left singular vectors. */
+
+/* LDU (input) INTEGER */
+/* On entry, leading dimension of U. */
+
+/* VT (output) DOUBLE PRECISION array, dimension at least (LDVT, M) */
+/* On exit, VT' contains the right singular vectors. */
+
+/* LDVT (input) INTEGER */
+/* On entry, leading dimension of VT. */
+
+/* SMLSIZ (input) INTEGER */
+/* On entry, maximum size of the subproblems at the */
+/* bottom of the computation tree. */
+
+/* IWORK (workspace) INTEGER work array. */
+/* Dimension must be at least (8 * N) */
+
+/* WORK (workspace) DOUBLE PRECISION work array. */
+/* Dimension must be at least (3 * M**2 + 2 * M) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --e;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ --iwork;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*n < 0) {
+ *info = -1;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -2;
+ }
+
+ m = *n + *sqre;
+
+ if (*ldu < *n) {
+ *info = -6;
+ } else if (*ldvt < m) {
+ *info = -8;
+ } else if (*smlsiz < 3) {
+ *info = -9;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD0", &i__1);
+ return 0;
+ }
+
+/* If the input matrix is too small, call DLASDQ to find the SVD. */
+
+ if (*n <= *smlsiz) {
+ dlasdq_("U", sqre, n, &m, n, &c__0, &d__[1], &e[1], &vt[vt_offset],
+ ldvt, &u[u_offset], ldu, &u[u_offset], ldu, &work[1], info);
+ return 0;
+ }
+
+/* Set up the computation tree. */
+
+ inode = 1;
+ ndiml = inode + *n;
+ ndimr = ndiml + *n;
+ idxq = ndimr + *n;
+ iwk = idxq + *n;
+ dlasdt_(n, &nlvl, &nd, &iwork[inode], &iwork[ndiml], &iwork[ndimr],
+ smlsiz);
+
+/* For the nodes on bottom level of the tree, solve */
+/* their subproblems by DLASDQ. */
+
+ ndb1 = (nd + 1) / 2;
+ ncc = 0;
+ i__1 = nd;
+ for (i__ = ndb1; i__ <= i__1; ++i__) {
+
+/* IC : center row of each node */
+/* NL : number of rows of left subproblem */
+/* NR : number of rows of right subproblem */
+/* NLF: starting row of the left subproblem */
+/* NRF: starting row of the right subproblem */
+
+ i1 = i__ - 1;
+ ic = iwork[inode + i1];
+ nl = iwork[ndiml + i1];
+ nlp1 = nl + 1;
+ nr = iwork[ndimr + i1];
+ nrp1 = nr + 1;
+ nlf = ic - nl;
+ nrf = ic + 1;
+ sqrei = 1;
+ dlasdq_("U", &sqrei, &nl, &nlp1, &nl, &ncc, &d__[nlf], &e[nlf], &vt[
+ nlf + nlf * vt_dim1], ldvt, &u[nlf + nlf * u_dim1], ldu, &u[
+ nlf + nlf * u_dim1], ldu, &work[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+ itemp = idxq + nlf - 2;
+ i__2 = nl;
+ for (j = 1; j <= i__2; ++j) {
+ iwork[itemp + j] = j;
+/* L10: */
+ }
+ if (i__ == nd) {
+ sqrei = *sqre;
+ } else {
+ sqrei = 1;
+ }
+ nrp1 = nr + sqrei;
+ dlasdq_("U", &sqrei, &nr, &nrp1, &nr, &ncc, &d__[nrf], &e[nrf], &vt[
+ nrf + nrf * vt_dim1], ldvt, &u[nrf + nrf * u_dim1], ldu, &u[
+ nrf + nrf * u_dim1], ldu, &work[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+ itemp = idxq + ic;
+ i__2 = nr;
+ for (j = 1; j <= i__2; ++j) {
+ iwork[itemp + j - 1] = j;
+/* L20: */
+ }
+/* L30: */
+ }
+
+/* Now conquer each subproblem bottom-up. */
+
+ for (lvl = nlvl; lvl >= 1; --lvl) {
+
+/* Find the first node LF and last node LL on the */
+/* current level LVL. */
+
+ if (lvl == 1) {
+ lf = 1;
+ ll = 1;
+ } else {
+ i__1 = lvl - 1;
+ lf = pow_ii(&c__2, &i__1);
+ ll = (lf << 1) - 1;
+ }
+ i__1 = ll;
+ for (i__ = lf; i__ <= i__1; ++i__) {
+ im1 = i__ - 1;
+ ic = iwork[inode + im1];
+ nl = iwork[ndiml + im1];
+ nr = iwork[ndimr + im1];
+ nlf = ic - nl;
+ if (*sqre == 0 && i__ == ll) {
+ sqrei = *sqre;
+ } else {
+ sqrei = 1;
+ }
+ idxqc = idxq + nlf - 1;
+ alpha = d__[ic];
+ beta = e[ic];
+ dlasd1_(&nl, &nr, &sqrei, &d__[nlf], &alpha, &beta, &u[nlf + nlf *
+ u_dim1], ldu, &vt[nlf + nlf * vt_dim1], ldvt, &iwork[
+ idxqc], &iwork[iwk], &work[1], info);
+ if (*info != 0) {
+ return 0;
+ }
+/* L40: */
+ }
+/* L50: */
+ }
+
+ return 0;
+
+/* End of DLASD0 */
+
+} /* dlasd0_ */
diff --git a/3rdparty/clapack/src/dlasd1.c b/3rdparty/clapack/src/dlasd1.c
new file mode 100644
index 0000000..84fb8e5
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd1.c
@@ -0,0 +1,288 @@
+/* dlasd1.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__0 = 0;
+static doublereal c_b7 = 1.;
+static integer c__1 = 1;
+static integer c_n1 = -1;
+
+/* Subroutine */ int dlasd1_(integer *nl, integer *nr, integer *sqre,
+ doublereal *d__, doublereal *alpha, doublereal *beta, doublereal *u,
+ integer *ldu, doublereal *vt, integer *ldvt, integer *idxq, integer *
+ iwork, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer u_dim1, u_offset, vt_dim1, vt_offset, i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ integer i__, k, m, n, n1, n2, iq, iz, iu2, ldq, idx, ldu2, ivt2, idxc,
+ idxp, ldvt2;
+ extern /* Subroutine */ int dlasd2_(integer *, integer *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, integer *, integer *,
+ integer *, integer *, integer *, integer *, integer *), dlasd3_(
+ integer *, integer *, integer *, integer *, doublereal *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *, integer *, integer *, doublereal *, integer *),
+ dlascl_(char *, integer *, integer *, doublereal *, doublereal *,
+ integer *, integer *, doublereal *, integer *, integer *),
+ dlamrg_(integer *, integer *, doublereal *, integer *, integer *,
+ integer *);
+ integer isigma;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ doublereal orgnrm;
+ integer coltyp;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD1 computes the SVD of an upper bidiagonal N-by-M matrix B, */
+/* where N = NL + NR + 1 and M = N + SQRE. DLASD1 is called from DLASD0. */
+
+/* A related subroutine DLASD7 handles the case in which the singular */
+/* values (and the singular vectors in factored form) are desired. */
+
+/* DLASD1 computes the SVD as follows: */
+
+/* ( D1(in) 0 0 0 ) */
+/* B = U(in) * ( Z1' a Z2' b ) * VT(in) */
+/* ( 0 0 D2(in) 0 ) */
+
+/* = U(out) * ( D(out) 0) * VT(out) */
+
+/* where Z' = (Z1' a Z2' b) = u' VT', and u is a vector of dimension M */
+/* with ALPHA and BETA in the NL+1 and NL+2 th entries and zeros */
+/* elsewhere; and the entry b is empty if SQRE = 0. */
+
+/* The left singular vectors of the original matrix are stored in U, and */
+/* the transpose of the right singular vectors are stored in VT, and the */
+/* singular values are in D. The algorithm consists of three stages: */
+
+/* The first stage consists of deflating the size of the problem */
+/* when there are multiple singular values or when there are zeros in */
+/* the Z vector. For each such occurence the dimension of the */
+/* secular equation problem is reduced by one. This stage is */
+/* performed by the routine DLASD2. */
+
+/* The second stage consists of calculating the updated */
+/* singular values. This is done by finding the square roots of the */
+/* roots of the secular equation via the routine DLASD4 (as called */
+/* by DLASD3). This routine also calculates the singular vectors of */
+/* the current problem. */
+
+/* The final stage consists of computing the updated singular vectors */
+/* directly using the updated singular values. The singular vectors */
+/* for the current problem are multiplied with the singular vectors */
+/* from the overall problem. */
+
+/* Arguments */
+/* ========= */
+
+/* NL (input) INTEGER */
+/* The row dimension of the upper block. NL >= 1. */
+
+/* NR (input) INTEGER */
+/* The row dimension of the lower block. NR >= 1. */
+
+/* SQRE (input) INTEGER */
+/* = 0: the lower block is an NR-by-NR square matrix. */
+/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
+
+/* The bidiagonal matrix has row dimension N = NL + NR + 1, */
+/* and column dimension M = N + SQRE. */
+
+/* D (input/output) DOUBLE PRECISION array, */
+/* dimension (N = NL+NR+1). */
+/* On entry D(1:NL,1:NL) contains the singular values of the */
+/* upper block; and D(NL+2:N) contains the singular values of */
+/* the lower block. On exit D(1:N) contains the singular values */
+/* of the modified matrix. */
+
+/* ALPHA (input/output) DOUBLE PRECISION */
+/* Contains the diagonal element associated with the added row. */
+
+/* BETA (input/output) DOUBLE PRECISION */
+/* Contains the off-diagonal element associated with the added */
+/* row. */
+
+/* U (input/output) DOUBLE PRECISION array, dimension(LDU,N) */
+/* On entry U(1:NL, 1:NL) contains the left singular vectors of */
+/* the upper block; U(NL+2:N, NL+2:N) contains the left singular */
+/* vectors of the lower block. On exit U contains the left */
+/* singular vectors of the bidiagonal matrix. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= max( 1, N ). */
+
+/* VT (input/output) DOUBLE PRECISION array, dimension(LDVT,M) */
+/* where M = N + SQRE. */
+/* On entry VT(1:NL+1, 1:NL+1)' contains the right singular */
+/* vectors of the upper block; VT(NL+2:M, NL+2:M)' contains */
+/* the right singular vectors of the lower block. On exit */
+/* VT' contains the right singular vectors of the */
+/* bidiagonal matrix. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. LDVT >= max( 1, M ). */
+
+/* IDXQ (output) INTEGER array, dimension(N) */
+/* This contains the permutation which will reintegrate the */
+/* subproblem just solved back into sorted order, i.e. */
+/* D( IDXQ( I = 1, N ) ) will be in ascending order. */
+
+/* IWORK (workspace) INTEGER array, dimension( 4 * N ) */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension( 3*M**2 + 2*M ) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ --idxq;
+ --iwork;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*nl < 1) {
+ *info = -1;
+ } else if (*nr < 1) {
+ *info = -2;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -3;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD1", &i__1);
+ return 0;
+ }
+
+ n = *nl + *nr + 1;
+ m = n + *sqre;
+
+/* The following values are for bookkeeping purposes only. They are */
+/* integer pointers which indicate the portion of the workspace */
+/* used by a particular array in DLASD2 and DLASD3. */
+
+ ldu2 = n;
+ ldvt2 = m;
+
+ iz = 1;
+ isigma = iz + m;
+ iu2 = isigma + n;
+ ivt2 = iu2 + ldu2 * n;
+ iq = ivt2 + ldvt2 * m;
+
+ idx = 1;
+ idxc = idx + n;
+ coltyp = idxc + n;
+ idxp = coltyp + n;
+
+/* Scale. */
+
+/* Computing MAX */
+ d__1 = abs(*alpha), d__2 = abs(*beta);
+ orgnrm = max(d__1,d__2);
+ d__[*nl + 1] = 0.;
+ i__1 = n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if ((d__1 = d__[i__], abs(d__1)) > orgnrm) {
+ orgnrm = (d__1 = d__[i__], abs(d__1));
+ }
+/* L10: */
+ }
+ dlascl_("G", &c__0, &c__0, &orgnrm, &c_b7, &n, &c__1, &d__[1], &n, info);
+ *alpha /= orgnrm;
+ *beta /= orgnrm;
+
+/* Deflate singular values. */
+
+ dlasd2_(nl, nr, sqre, &k, &d__[1], &work[iz], alpha, beta, &u[u_offset],
+ ldu, &vt[vt_offset], ldvt, &work[isigma], &work[iu2], &ldu2, &
+ work[ivt2], &ldvt2, &iwork[idxp], &iwork[idx], &iwork[idxc], &
+ idxq[1], &iwork[coltyp], info);
+
+/* Solve Secular Equation and update singular vectors. */
+
+ ldq = k;
+ dlasd3_(nl, nr, sqre, &k, &d__[1], &work[iq], &ldq, &work[isigma], &u[
+ u_offset], ldu, &work[iu2], &ldu2, &vt[vt_offset], ldvt, &work[
+ ivt2], &ldvt2, &iwork[idxc], &iwork[coltyp], &work[iz], info);
+ if (*info != 0) {
+ return 0;
+ }
+
+/* Unscale. */
+
+ dlascl_("G", &c__0, &c__0, &c_b7, &orgnrm, &n, &c__1, &d__[1], &n, info);
+
+/* Prepare the IDXQ sorting permutation. */
+
+ n1 = k;
+ n2 = n - k;
+ dlamrg_(&n1, &n2, &d__[1], &c__1, &c_n1, &idxq[1]);
+
+ return 0;
+
+/* End of DLASD1 */
+
+} /* dlasd1_ */
diff --git a/3rdparty/clapack/src/dlasd2.c b/3rdparty/clapack/src/dlasd2.c
new file mode 100644
index 0000000..441aa7d
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd2.c
@@ -0,0 +1,609 @@
+/* dlasd2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b30 = 0.;
+
+/* Subroutine */ int dlasd2_(integer *nl, integer *nr, integer *sqre, integer
+ *k, doublereal *d__, doublereal *z__, doublereal *alpha, doublereal *
+ beta, doublereal *u, integer *ldu, doublereal *vt, integer *ldvt,
+ doublereal *dsigma, doublereal *u2, integer *ldu2, doublereal *vt2,
+ integer *ldvt2, integer *idxp, integer *idx, integer *idxc, integer *
+ idxq, integer *coltyp, integer *info)
+{
+ /* System generated locals */
+ integer u_dim1, u_offset, u2_dim1, u2_offset, vt_dim1, vt_offset,
+ vt2_dim1, vt2_offset, i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ doublereal c__;
+ integer i__, j, m, n;
+ doublereal s;
+ integer k2;
+ doublereal z1;
+ integer ct, jp;
+ doublereal eps, tau, tol;
+ integer psm[4], nlp1, nlp2, idxi, idxj;
+ extern /* Subroutine */ int drot_(integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *);
+ integer ctot[4], idxjp;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer jprev;
+ extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
+ extern /* Subroutine */ int dlamrg_(integer *, integer *, doublereal *,
+ integer *, integer *, integer *), dlacpy_(char *, integer *,
+ integer *, doublereal *, integer *, doublereal *, integer *), dlaset_(char *, integer *, integer *, doublereal *,
+ doublereal *, doublereal *, integer *), xerbla_(char *,
+ integer *);
+ doublereal hlftol;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD2 merges the two sets of singular values together into a single */
+/* sorted set. Then it tries to deflate the size of the problem. */
+/* There are two ways in which deflation can occur: when two or more */
+/* singular values are close together or if there is a tiny entry in the */
+/* Z vector. For each such occurrence the order of the related secular */
+/* equation problem is reduced by one. */
+
+/* DLASD2 is called from DLASD1. */
+
+/* Arguments */
+/* ========= */
+
+/* NL (input) INTEGER */
+/* The row dimension of the upper block. NL >= 1. */
+
+/* NR (input) INTEGER */
+/* The row dimension of the lower block. NR >= 1. */
+
+/* SQRE (input) INTEGER */
+/* = 0: the lower block is an NR-by-NR square matrix. */
+/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
+
+/* The bidiagonal matrix has N = NL + NR + 1 rows and */
+/* M = N + SQRE >= N columns. */
+
+/* K (output) INTEGER */
+/* Contains the dimension of the non-deflated matrix, */
+/* This is the order of the related secular equation. 1 <= K <=N. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension(N) */
+/* On entry D contains the singular values of the two submatrices */
+/* to be combined. On exit D contains the trailing (N-K) updated */
+/* singular values (those which were deflated) sorted into */
+/* increasing order. */
+
+/* Z (output) DOUBLE PRECISION array, dimension(N) */
+/* On exit Z contains the updating row vector in the secular */
+/* equation. */
+
+/* ALPHA (input) DOUBLE PRECISION */
+/* Contains the diagonal element associated with the added row. */
+
+/* BETA (input) DOUBLE PRECISION */
+/* Contains the off-diagonal element associated with the added */
+/* row. */
+
+/* U (input/output) DOUBLE PRECISION array, dimension(LDU,N) */
+/* On entry U contains the left singular vectors of two */
+/* submatrices in the two square blocks with corners at (1,1), */
+/* (NL, NL), and (NL+2, NL+2), (N,N). */
+/* On exit U contains the trailing (N-K) updated left singular */
+/* vectors (those which were deflated) in its last N-K columns. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= N. */
+
+/* VT (input/output) DOUBLE PRECISION array, dimension(LDVT,M) */
+/* On entry VT' contains the right singular vectors of two */
+/* submatrices in the two square blocks with corners at (1,1), */
+/* (NL+1, NL+1), and (NL+2, NL+2), (M,M). */
+/* On exit VT' contains the trailing (N-K) updated right singular */
+/* vectors (those which were deflated) in its last N-K columns. */
+/* In case SQRE =1, the last row of VT spans the right null */
+/* space. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. LDVT >= M. */
+
+/* DSIGMA (output) DOUBLE PRECISION array, dimension (N) */
+/* Contains a copy of the diagonal elements (K-1 singular values */
+/* and one zero) in the secular equation. */
+
+/* U2 (output) DOUBLE PRECISION array, dimension(LDU2,N) */
+/* Contains a copy of the first K-1 left singular vectors which */
+/* will be used by DLASD3 in a matrix multiply (DGEMM) to solve */
+/* for the new left singular vectors. U2 is arranged into four */
+/* blocks. The first block contains a column with 1 at NL+1 and */
+/* zero everywhere else; the second block contains non-zero */
+/* entries only at and above NL; the third contains non-zero */
+/* entries only below NL+1; and the fourth is dense. */
+
+/* LDU2 (input) INTEGER */
+/* The leading dimension of the array U2. LDU2 >= N. */
+
+/* VT2 (output) DOUBLE PRECISION array, dimension(LDVT2,N) */
+/* VT2' contains a copy of the first K right singular vectors */
+/* which will be used by DLASD3 in a matrix multiply (DGEMM) to */
+/* solve for the new right singular vectors. VT2 is arranged into */
+/* three blocks. The first block contains a row that corresponds */
+/* to the special 0 diagonal element in SIGMA; the second block */
+/* contains non-zeros only at and before NL +1; the third block */
+/* contains non-zeros only at and after NL +2. */
+
+/* LDVT2 (input) INTEGER */
+/* The leading dimension of the array VT2. LDVT2 >= M. */
+
+/* IDXP (workspace) INTEGER array dimension(N) */
+/* This will contain the permutation used to place deflated */
+/* values of D at the end of the array. On output IDXP(2:K) */
+/* points to the nondeflated D-values and IDXP(K+1:N) */
+/* points to the deflated singular values. */
+
+/* IDX (workspace) INTEGER array dimension(N) */
+/* This will contain the permutation used to sort the contents of */
+/* D into ascending order. */
+
+/* IDXC (output) INTEGER array dimension(N) */
+/* This will contain the permutation used to arrange the columns */
+/* of the deflated U matrix into three groups: the first group */
+/* contains non-zero entries only at and above NL, the second */
+/* contains non-zero entries only below NL+2, and the third is */
+/* dense. */
+
+/* IDXQ (input/output) INTEGER array dimension(N) */
+/* This contains the permutation which separately sorts the two */
+/* sub-problems in D into ascending order. Note that entries in */
+/* the first hlaf of this permutation must first be moved one */
+/* position backward; and entries in the second half */
+/* must first have NL+1 added to their values. */
+
+/* COLTYP (workspace/output) INTEGER array dimension(N) */
+/* As workspace, this will contain a label which will indicate */
+/* which of the following types a column in the U2 matrix or a */
+/* row in the VT2 matrix is: */
+/* 1 : non-zero in the upper half only */
+/* 2 : non-zero in the lower half only */
+/* 3 : dense */
+/* 4 : deflated */
+
+/* On exit, it is an array of dimension 4, with COLTYP(I) being */
+/* the dimension of the I-th type columns. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --z__;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ --dsigma;
+ u2_dim1 = *ldu2;
+ u2_offset = 1 + u2_dim1;
+ u2 -= u2_offset;
+ vt2_dim1 = *ldvt2;
+ vt2_offset = 1 + vt2_dim1;
+ vt2 -= vt2_offset;
+ --idxp;
+ --idx;
+ --idxc;
+ --idxq;
+ --coltyp;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*nl < 1) {
+ *info = -1;
+ } else if (*nr < 1) {
+ *info = -2;
+ } else if (*sqre != 1 && *sqre != 0) {
+ *info = -3;
+ }
+
+ n = *nl + *nr + 1;
+ m = n + *sqre;
+
+ if (*ldu < n) {
+ *info = -10;
+ } else if (*ldvt < m) {
+ *info = -12;
+ } else if (*ldu2 < n) {
+ *info = -15;
+ } else if (*ldvt2 < m) {
+ *info = -17;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD2", &i__1);
+ return 0;
+ }
+
+ nlp1 = *nl + 1;
+ nlp2 = *nl + 2;
+
+/* Generate the first part of the vector Z; and move the singular */
+/* values in the first part of D one position backward. */
+
+ z1 = *alpha * vt[nlp1 + nlp1 * vt_dim1];
+ z__[1] = z1;
+ for (i__ = *nl; i__ >= 1; --i__) {
+ z__[i__ + 1] = *alpha * vt[i__ + nlp1 * vt_dim1];
+ d__[i__ + 1] = d__[i__];
+ idxq[i__ + 1] = idxq[i__] + 1;
+/* L10: */
+ }
+
+/* Generate the second part of the vector Z. */
+
+ i__1 = m;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ z__[i__] = *beta * vt[i__ + nlp2 * vt_dim1];
+/* L20: */
+ }
+
+/* Initialize some reference arrays. */
+
+ i__1 = nlp1;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ coltyp[i__] = 1;
+/* L30: */
+ }
+ i__1 = n;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ coltyp[i__] = 2;
+/* L40: */
+ }
+
+/* Sort the singular values into increasing order */
+
+ i__1 = n;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ idxq[i__] += nlp1;
+/* L50: */
+ }
+
+/* DSIGMA, IDXC, IDXC, and the first column of U2 */
+/* are used as storage space. */
+
+ i__1 = n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ dsigma[i__] = d__[idxq[i__]];
+ u2[i__ + u2_dim1] = z__[idxq[i__]];
+ idxc[i__] = coltyp[idxq[i__]];
+/* L60: */
+ }
+
+ dlamrg_(nl, nr, &dsigma[2], &c__1, &c__1, &idx[2]);
+
+ i__1 = n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ idxi = idx[i__] + 1;
+ d__[i__] = dsigma[idxi];
+ z__[i__] = u2[idxi + u2_dim1];
+ coltyp[i__] = idxc[idxi];
+/* L70: */
+ }
+
+/* Calculate the allowable deflation tolerance */
+
+ eps = dlamch_("Epsilon");
+/* Computing MAX */
+ d__1 = abs(*alpha), d__2 = abs(*beta);
+ tol = max(d__1,d__2);
+/* Computing MAX */
+ d__2 = (d__1 = d__[n], abs(d__1));
+ tol = eps * 8. * max(d__2,tol);
+
+/* There are 2 kinds of deflation -- first a value in the z-vector */
+/* is small, second two (or more) singular values are very close */
+/* together (their difference is small). */
+
+/* If the value in the z-vector is small, we simply permute the */
+/* array so that the corresponding singular value is moved to the */
+/* end. */
+
+/* If two values in the D-vector are close, we perform a two-sided */
+/* rotation designed to make one of the corresponding z-vector */
+/* entries zero, and then permute the array so that the deflated */
+/* singular value is moved to the end. */
+
+/* If there are multiple singular values then the problem deflates. */
+/* Here the number of equal singular values are found. As each equal */
+/* singular value is found, an elementary reflector is computed to */
+/* rotate the corresponding singular subspace so that the */
+/* corresponding components of Z are zero in this new basis. */
+
+ *k = 1;
+ k2 = n + 1;
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ if ((d__1 = z__[j], abs(d__1)) <= tol) {
+
+/* Deflate due to small z component. */
+
+ --k2;
+ idxp[k2] = j;
+ coltyp[j] = 4;
+ if (j == n) {
+ goto L120;
+ }
+ } else {
+ jprev = j;
+ goto L90;
+ }
+/* L80: */
+ }
+L90:
+ j = jprev;
+L100:
+ ++j;
+ if (j > n) {
+ goto L110;
+ }
+ if ((d__1 = z__[j], abs(d__1)) <= tol) {
+
+/* Deflate due to small z component. */
+
+ --k2;
+ idxp[k2] = j;
+ coltyp[j] = 4;
+ } else {
+
+/* Check if singular values are close enough to allow deflation. */
+
+ if ((d__1 = d__[j] - d__[jprev], abs(d__1)) <= tol) {
+
+/* Deflation is possible. */
+
+ s = z__[jprev];
+ c__ = z__[j];
+
+/* Find sqrt(a**2+b**2) without overflow or */
+/* destructive underflow. */
+
+ tau = dlapy2_(&c__, &s);
+ c__ /= tau;
+ s = -s / tau;
+ z__[j] = tau;
+ z__[jprev] = 0.;
+
+/* Apply back the Givens rotation to the left and right */
+/* singular vector matrices. */
+
+ idxjp = idxq[idx[jprev] + 1];
+ idxj = idxq[idx[j] + 1];
+ if (idxjp <= nlp1) {
+ --idxjp;
+ }
+ if (idxj <= nlp1) {
+ --idxj;
+ }
+ drot_(&n, &u[idxjp * u_dim1 + 1], &c__1, &u[idxj * u_dim1 + 1], &
+ c__1, &c__, &s);
+ drot_(&m, &vt[idxjp + vt_dim1], ldvt, &vt[idxj + vt_dim1], ldvt, &
+ c__, &s);
+ if (coltyp[j] != coltyp[jprev]) {
+ coltyp[j] = 3;
+ }
+ coltyp[jprev] = 4;
+ --k2;
+ idxp[k2] = jprev;
+ jprev = j;
+ } else {
+ ++(*k);
+ u2[*k + u2_dim1] = z__[jprev];
+ dsigma[*k] = d__[jprev];
+ idxp[*k] = jprev;
+ jprev = j;
+ }
+ }
+ goto L100;
+L110:
+
+/* Record the last singular value. */
+
+ ++(*k);
+ u2[*k + u2_dim1] = z__[jprev];
+ dsigma[*k] = d__[jprev];
+ idxp[*k] = jprev;
+
+L120:
+
+/* Count up the total number of the various types of columns, then */
+/* form a permutation which positions the four column types into */
+/* four groups of uniform structure (although one or more of these */
+/* groups may be empty). */
+
+ for (j = 1; j <= 4; ++j) {
+ ctot[j - 1] = 0;
+/* L130: */
+ }
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ ct = coltyp[j];
+ ++ctot[ct - 1];
+/* L140: */
+ }
+
+/* PSM(*) = Position in SubMatrix (of types 1 through 4) */
+
+ psm[0] = 2;
+ psm[1] = ctot[0] + 2;
+ psm[2] = psm[1] + ctot[1];
+ psm[3] = psm[2] + ctot[2];
+
+/* Fill out the IDXC array so that the permutation which it induces */
+/* will place all type-1 columns first, all type-2 columns next, */
+/* then all type-3's, and finally all type-4's, starting from the */
+/* second column. This applies similarly to the rows of VT. */
+
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ jp = idxp[j];
+ ct = coltyp[jp];
+ idxc[psm[ct - 1]] = j;
+ ++psm[ct - 1];
+/* L150: */
+ }
+
+/* Sort the singular values and corresponding singular vectors into */
+/* DSIGMA, U2, and VT2 respectively. The singular values/vectors */
+/* which were not deflated go into the first K slots of DSIGMA, U2, */
+/* and VT2 respectively, while those which were deflated go into the */
+/* last N - K slots, except that the first column/row will be treated */
+/* separately. */
+
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ jp = idxp[j];
+ dsigma[j] = d__[jp];
+ idxj = idxq[idx[idxp[idxc[j]]] + 1];
+ if (idxj <= nlp1) {
+ --idxj;
+ }
+ dcopy_(&n, &u[idxj * u_dim1 + 1], &c__1, &u2[j * u2_dim1 + 1], &c__1);
+ dcopy_(&m, &vt[idxj + vt_dim1], ldvt, &vt2[j + vt2_dim1], ldvt2);
+/* L160: */
+ }
+
+/* Determine DSIGMA(1), DSIGMA(2) and Z(1) */
+
+ dsigma[1] = 0.;
+ hlftol = tol / 2.;
+ if (abs(dsigma[2]) <= hlftol) {
+ dsigma[2] = hlftol;
+ }
+ if (m > n) {
+ z__[1] = dlapy2_(&z1, &z__[m]);
+ if (z__[1] <= tol) {
+ c__ = 1.;
+ s = 0.;
+ z__[1] = tol;
+ } else {
+ c__ = z1 / z__[1];
+ s = z__[m] / z__[1];
+ }
+ } else {
+ if (abs(z1) <= tol) {
+ z__[1] = tol;
+ } else {
+ z__[1] = z1;
+ }
+ }
+
+/* Move the rest of the updating row to Z. */
+
+ i__1 = *k - 1;
+ dcopy_(&i__1, &u2[u2_dim1 + 2], &c__1, &z__[2], &c__1);
+
+/* Determine the first column of U2, the first row of VT2 and the */
+/* last row of VT. */
+
+ dlaset_("A", &n, &c__1, &c_b30, &c_b30, &u2[u2_offset], ldu2);
+ u2[nlp1 + u2_dim1] = 1.;
+ if (m > n) {
+ i__1 = nlp1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ vt[m + i__ * vt_dim1] = -s * vt[nlp1 + i__ * vt_dim1];
+ vt2[i__ * vt2_dim1 + 1] = c__ * vt[nlp1 + i__ * vt_dim1];
+/* L170: */
+ }
+ i__1 = m;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ vt2[i__ * vt2_dim1 + 1] = s * vt[m + i__ * vt_dim1];
+ vt[m + i__ * vt_dim1] = c__ * vt[m + i__ * vt_dim1];
+/* L180: */
+ }
+ } else {
+ dcopy_(&m, &vt[nlp1 + vt_dim1], ldvt, &vt2[vt2_dim1 + 1], ldvt2);
+ }
+ if (m > n) {
+ dcopy_(&m, &vt[m + vt_dim1], ldvt, &vt2[m + vt2_dim1], ldvt2);
+ }
+
+/* The deflated singular values and their corresponding vectors go */
+/* into the back of D, U, and V respectively. */
+
+ if (n > *k) {
+ i__1 = n - *k;
+ dcopy_(&i__1, &dsigma[*k + 1], &c__1, &d__[*k + 1], &c__1);
+ i__1 = n - *k;
+ dlacpy_("A", &n, &i__1, &u2[(*k + 1) * u2_dim1 + 1], ldu2, &u[(*k + 1)
+ * u_dim1 + 1], ldu);
+ i__1 = n - *k;
+ dlacpy_("A", &i__1, &m, &vt2[*k + 1 + vt2_dim1], ldvt2, &vt[*k + 1 +
+ vt_dim1], ldvt);
+ }
+
+/* Copy CTOT into COLTYP for referencing in DLASD3. */
+
+ for (j = 1; j <= 4; ++j) {
+ coltyp[j] = ctot[j - 1];
+/* L190: */
+ }
+
+ return 0;
+
+/* End of DLASD2 */
+
+} /* dlasd2_ */
diff --git a/3rdparty/clapack/src/dlasd3.c b/3rdparty/clapack/src/dlasd3.c
new file mode 100644
index 0000000..db8089b
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd3.c
@@ -0,0 +1,452 @@
+/* dlasd3.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c__0 = 0;
+static doublereal c_b13 = 1.;
+static doublereal c_b26 = 0.;
+
+/* Subroutine */ int dlasd3_(integer *nl, integer *nr, integer *sqre, integer
+ *k, doublereal *d__, doublereal *q, integer *ldq, doublereal *dsigma,
+ doublereal *u, integer *ldu, doublereal *u2, integer *ldu2,
+ doublereal *vt, integer *ldvt, doublereal *vt2, integer *ldvt2,
+ integer *idxc, integer *ctot, doublereal *z__, integer *info)
+{
+ /* System generated locals */
+ integer q_dim1, q_offset, u_dim1, u_offset, u2_dim1, u2_offset, vt_dim1,
+ vt_offset, vt2_dim1, vt2_offset, i__1, i__2;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal), d_sign(doublereal *, doublereal *);
+
+ /* Local variables */
+ integer i__, j, m, n, jc;
+ doublereal rho;
+ integer nlp1, nlp2, nrp1;
+ doublereal temp;
+ extern doublereal dnrm2_(integer *, doublereal *, integer *);
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ integer ctemp;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer ktemp;
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+ extern /* Subroutine */ int dlasd4_(integer *, integer *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *), dlascl_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, integer *, doublereal *,
+ integer *, integer *), dlacpy_(char *, integer *, integer
+ *, doublereal *, integer *, doublereal *, integer *),
+ xerbla_(char *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD3 finds all the square roots of the roots of the secular */
+/* equation, as defined by the values in D and Z. It makes the */
+/* appropriate calls to DLASD4 and then updates the singular */
+/* vectors by matrix multiplication. */
+
+/* This code makes very mild assumptions about floating point */
+/* arithmetic. It will work on machines with a guard digit in */
+/* add/subtract, or on those binary machines without guard digits */
+/* which subtract like the Cray XMP, Cray YMP, Cray C 90, or Cray 2. */
+/* It could conceivably fail on hexadecimal or decimal machines */
+/* without guard digits, but we know of none. */
+
+/* DLASD3 is called from DLASD1. */
+
+/* Arguments */
+/* ========= */
+
+/* NL (input) INTEGER */
+/* The row dimension of the upper block. NL >= 1. */
+
+/* NR (input) INTEGER */
+/* The row dimension of the lower block. NR >= 1. */
+
+/* SQRE (input) INTEGER */
+/* = 0: the lower block is an NR-by-NR square matrix. */
+/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
+
+/* The bidiagonal matrix has N = NL + NR + 1 rows and */
+/* M = N + SQRE >= N columns. */
+
+/* K (input) INTEGER */
+/* The size of the secular equation, 1 =< K = < N. */
+
+/* D (output) DOUBLE PRECISION array, dimension(K) */
+/* On exit the square roots of the roots of the secular equation, */
+/* in ascending order. */
+
+/* Q (workspace) DOUBLE PRECISION array, */
+/* dimension at least (LDQ,K). */
+
+/* LDQ (input) INTEGER */
+/* The leading dimension of the array Q. LDQ >= K. */
+
+/* DSIGMA (input) DOUBLE PRECISION array, dimension(K) */
+/* The first K elements of this array contain the old roots */
+/* of the deflated updating problem. These are the poles */
+/* of the secular equation. */
+
+/* U (output) DOUBLE PRECISION array, dimension (LDU, N) */
+/* The last N - K columns of this matrix contain the deflated */
+/* left singular vectors. */
+
+/* LDU (input) INTEGER */
+/* The leading dimension of the array U. LDU >= N. */
+
+/* U2 (input/output) DOUBLE PRECISION array, dimension (LDU2, N) */
+/* The first K columns of this matrix contain the non-deflated */
+/* left singular vectors for the split problem. */
+
+/* LDU2 (input) INTEGER */
+/* The leading dimension of the array U2. LDU2 >= N. */
+
+/* VT (output) DOUBLE PRECISION array, dimension (LDVT, M) */
+/* The last M - K columns of VT' contain the deflated */
+/* right singular vectors. */
+
+/* LDVT (input) INTEGER */
+/* The leading dimension of the array VT. LDVT >= N. */
+
+/* VT2 (input/output) DOUBLE PRECISION array, dimension (LDVT2, N) */
+/* The first K columns of VT2' contain the non-deflated */
+/* right singular vectors for the split problem. */
+
+/* LDVT2 (input) INTEGER */
+/* The leading dimension of the array VT2. LDVT2 >= N. */
+
+/* IDXC (input) INTEGER array, dimension ( N ) */
+/* The permutation used to arrange the columns of U (and rows of */
+/* VT) into three groups: the first group contains non-zero */
+/* entries only at and above (or before) NL +1; the second */
+/* contains non-zero entries only at and below (or after) NL+2; */
+/* and the third is dense. The first column of U and the row of */
+/* VT are treated separately, however. */
+
+/* The rows of the singular vectors found by DLASD4 */
+/* must be likewise permuted before the matrix multiplies can */
+/* take place. */
+
+/* CTOT (input) INTEGER array, dimension ( 4 ) */
+/* A count of the total number of the various types of columns */
+/* in U (or rows in VT), as described in IDXC. The fourth column */
+/* type is any column which has been deflated. */
+
+/* Z (input) DOUBLE PRECISION array, dimension (K) */
+/* The first K elements of this array contain the components */
+/* of the deflation-adjusted updating row vector. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ q_dim1 = *ldq;
+ q_offset = 1 + q_dim1;
+ q -= q_offset;
+ --dsigma;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ u2_dim1 = *ldu2;
+ u2_offset = 1 + u2_dim1;
+ u2 -= u2_offset;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ vt2_dim1 = *ldvt2;
+ vt2_offset = 1 + vt2_dim1;
+ vt2 -= vt2_offset;
+ --idxc;
+ --ctot;
+ --z__;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*nl < 1) {
+ *info = -1;
+ } else if (*nr < 1) {
+ *info = -2;
+ } else if (*sqre != 1 && *sqre != 0) {
+ *info = -3;
+ }
+
+ n = *nl + *nr + 1;
+ m = n + *sqre;
+ nlp1 = *nl + 1;
+ nlp2 = *nl + 2;
+
+ if (*k < 1 || *k > n) {
+ *info = -4;
+ } else if (*ldq < *k) {
+ *info = -7;
+ } else if (*ldu < n) {
+ *info = -10;
+ } else if (*ldu2 < n) {
+ *info = -12;
+ } else if (*ldvt < m) {
+ *info = -14;
+ } else if (*ldvt2 < m) {
+ *info = -16;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD3", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*k == 1) {
+ d__[1] = abs(z__[1]);
+ dcopy_(&m, &vt2[vt2_dim1 + 1], ldvt2, &vt[vt_dim1 + 1], ldvt);
+ if (z__[1] > 0.) {
+ dcopy_(&n, &u2[u2_dim1 + 1], &c__1, &u[u_dim1 + 1], &c__1);
+ } else {
+ i__1 = n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ u[i__ + u_dim1] = -u2[i__ + u2_dim1];
+/* L10: */
+ }
+ }
+ return 0;
+ }
+
+/* Modify values DSIGMA(i) to make sure all DSIGMA(i)-DSIGMA(j) can */
+/* be computed with high relative accuracy (barring over/underflow). */
+/* This is a problem on machines without a guard digit in */
+/* add/subtract (Cray XMP, Cray YMP, Cray C 90 and Cray 2). */
+/* The following code replaces DSIGMA(I) by 2*DSIGMA(I)-DSIGMA(I), */
+/* which on any of these machines zeros out the bottommost */
+/* bit of DSIGMA(I) if it is 1; this makes the subsequent */
+/* subtractions DSIGMA(I)-DSIGMA(J) unproblematic when cancellation */
+/* occurs. On binary machines with a guard digit (almost all */
+/* machines) it does not change DSIGMA(I) at all. On hexadecimal */
+/* and decimal machines with a guard digit, it slightly */
+/* changes the bottommost bits of DSIGMA(I). It does not account */
+/* for hexadecimal or decimal machines without guard digits */
+/* (we know of none). We use a subroutine call to compute */
+/* 2*DSIGMA(I) to prevent optimizing compilers from eliminating */
+/* this code. */
+
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dsigma[i__] = dlamc3_(&dsigma[i__], &dsigma[i__]) - dsigma[i__];
+/* L20: */
+ }
+
+/* Keep a copy of Z. */
+
+ dcopy_(k, &z__[1], &c__1, &q[q_offset], &c__1);
+
+/* Normalize Z. */
+
+ rho = dnrm2_(k, &z__[1], &c__1);
+ dlascl_("G", &c__0, &c__0, &rho, &c_b13, k, &c__1, &z__[1], k, info);
+ rho *= rho;
+
+/* Find the new singular values. */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dlasd4_(k, &j, &dsigma[1], &z__[1], &u[j * u_dim1 + 1], &rho, &d__[j],
+ &vt[j * vt_dim1 + 1], info);
+
+/* If the zero finder fails, the computation is terminated. */
+
+ if (*info != 0) {
+ return 0;
+ }
+/* L30: */
+ }
+
+/* Compute updated Z. */
+
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ z__[i__] = u[i__ + *k * u_dim1] * vt[i__ + *k * vt_dim1];
+ i__2 = i__ - 1;
+ for (j = 1; j <= i__2; ++j) {
+ z__[i__] *= u[i__ + j * u_dim1] * vt[i__ + j * vt_dim1] / (dsigma[
+ i__] - dsigma[j]) / (dsigma[i__] + dsigma[j]);
+/* L40: */
+ }
+ i__2 = *k - 1;
+ for (j = i__; j <= i__2; ++j) {
+ z__[i__] *= u[i__ + j * u_dim1] * vt[i__ + j * vt_dim1] / (dsigma[
+ i__] - dsigma[j + 1]) / (dsigma[i__] + dsigma[j + 1]);
+/* L50: */
+ }
+ d__2 = sqrt((d__1 = z__[i__], abs(d__1)));
+ z__[i__] = d_sign(&d__2, &q[i__ + q_dim1]);
+/* L60: */
+ }
+
+/* Compute left singular vectors of the modified diagonal matrix, */
+/* and store related information for the right singular vectors. */
+
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ vt[i__ * vt_dim1 + 1] = z__[1] / u[i__ * u_dim1 + 1] / vt[i__ *
+ vt_dim1 + 1];
+ u[i__ * u_dim1 + 1] = -1.;
+ i__2 = *k;
+ for (j = 2; j <= i__2; ++j) {
+ vt[j + i__ * vt_dim1] = z__[j] / u[j + i__ * u_dim1] / vt[j + i__
+ * vt_dim1];
+ u[j + i__ * u_dim1] = dsigma[j] * vt[j + i__ * vt_dim1];
+/* L70: */
+ }
+ temp = dnrm2_(k, &u[i__ * u_dim1 + 1], &c__1);
+ q[i__ * q_dim1 + 1] = u[i__ * u_dim1 + 1] / temp;
+ i__2 = *k;
+ for (j = 2; j <= i__2; ++j) {
+ jc = idxc[j];
+ q[j + i__ * q_dim1] = u[jc + i__ * u_dim1] / temp;
+/* L80: */
+ }
+/* L90: */
+ }
+
+/* Update the left singular vector matrix. */
+
+ if (*k == 2) {
+ dgemm_("N", "N", &n, k, k, &c_b13, &u2[u2_offset], ldu2, &q[q_offset],
+ ldq, &c_b26, &u[u_offset], ldu);
+ goto L100;
+ }
+ if (ctot[1] > 0) {
+ dgemm_("N", "N", nl, k, &ctot[1], &c_b13, &u2[(u2_dim1 << 1) + 1],
+ ldu2, &q[q_dim1 + 2], ldq, &c_b26, &u[u_dim1 + 1], ldu);
+ if (ctot[3] > 0) {
+ ktemp = ctot[1] + 2 + ctot[2];
+ dgemm_("N", "N", nl, k, &ctot[3], &c_b13, &u2[ktemp * u2_dim1 + 1]
+, ldu2, &q[ktemp + q_dim1], ldq, &c_b13, &u[u_dim1 + 1],
+ ldu);
+ }
+ } else if (ctot[3] > 0) {
+ ktemp = ctot[1] + 2 + ctot[2];
+ dgemm_("N", "N", nl, k, &ctot[3], &c_b13, &u2[ktemp * u2_dim1 + 1],
+ ldu2, &q[ktemp + q_dim1], ldq, &c_b26, &u[u_dim1 + 1], ldu);
+ } else {
+ dlacpy_("F", nl, k, &u2[u2_offset], ldu2, &u[u_offset], ldu);
+ }
+ dcopy_(k, &q[q_dim1 + 1], ldq, &u[nlp1 + u_dim1], ldu);
+ ktemp = ctot[1] + 2;
+ ctemp = ctot[2] + ctot[3];
+ dgemm_("N", "N", nr, k, &ctemp, &c_b13, &u2[nlp2 + ktemp * u2_dim1], ldu2,
+ &q[ktemp + q_dim1], ldq, &c_b26, &u[nlp2 + u_dim1], ldu);
+
+/* Generate the right singular vectors. */
+
+L100:
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = dnrm2_(k, &vt[i__ * vt_dim1 + 1], &c__1);
+ q[i__ + q_dim1] = vt[i__ * vt_dim1 + 1] / temp;
+ i__2 = *k;
+ for (j = 2; j <= i__2; ++j) {
+ jc = idxc[j];
+ q[i__ + j * q_dim1] = vt[jc + i__ * vt_dim1] / temp;
+/* L110: */
+ }
+/* L120: */
+ }
+
+/* Update the right singular vector matrix. */
+
+ if (*k == 2) {
+ dgemm_("N", "N", k, &m, k, &c_b13, &q[q_offset], ldq, &vt2[vt2_offset]
+, ldvt2, &c_b26, &vt[vt_offset], ldvt);
+ return 0;
+ }
+ ktemp = ctot[1] + 1;
+ dgemm_("N", "N", k, &nlp1, &ktemp, &c_b13, &q[q_dim1 + 1], ldq, &vt2[
+ vt2_dim1 + 1], ldvt2, &c_b26, &vt[vt_dim1 + 1], ldvt);
+ ktemp = ctot[1] + 2 + ctot[2];
+ if (ktemp <= *ldvt2) {
+ dgemm_("N", "N", k, &nlp1, &ctot[3], &c_b13, &q[ktemp * q_dim1 + 1],
+ ldq, &vt2[ktemp + vt2_dim1], ldvt2, &c_b13, &vt[vt_dim1 + 1],
+ ldvt);
+ }
+
+ ktemp = ctot[1] + 1;
+ nrp1 = *nr + *sqre;
+ if (ktemp > 1) {
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ q[i__ + ktemp * q_dim1] = q[i__ + q_dim1];
+/* L130: */
+ }
+ i__1 = m;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ vt2[ktemp + i__ * vt2_dim1] = vt2[i__ * vt2_dim1 + 1];
+/* L140: */
+ }
+ }
+ ctemp = ctot[2] + 1 + ctot[3];
+ dgemm_("N", "N", k, &nrp1, &ctemp, &c_b13, &q[ktemp * q_dim1 + 1], ldq, &
+ vt2[ktemp + nlp2 * vt2_dim1], ldvt2, &c_b26, &vt[nlp2 * vt_dim1 +
+ 1], ldvt);
+
+ return 0;
+
+/* End of DLASD3 */
+
+} /* dlasd3_ */
diff --git a/3rdparty/clapack/src/dlasd4.c b/3rdparty/clapack/src/dlasd4.c
new file mode 100644
index 0000000..54455ed
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd4.c
@@ -0,0 +1,1010 @@
+/* dlasd4.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasd4_(integer *n, integer *i__, doublereal *d__,
+ doublereal *z__, doublereal *delta, doublereal *rho, doublereal *
+ sigma, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal a, b, c__;
+ integer j;
+ doublereal w, dd[3];
+ integer ii;
+ doublereal dw, zz[3];
+ integer ip1;
+ doublereal eta, phi, eps, tau, psi;
+ integer iim1, iip1;
+ doublereal dphi, dpsi;
+ integer iter;
+ doublereal temp, prew, sg2lb, sg2ub, temp1, temp2, dtiim, delsq, dtiip;
+ integer niter;
+ doublereal dtisq;
+ logical swtch;
+ doublereal dtnsq;
+ extern /* Subroutine */ int dlaed6_(integer *, logical *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *, integer *)
+ , dlasd5_(integer *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *);
+ doublereal delsq2, dtnsq1;
+ logical swtch3;
+ extern doublereal dlamch_(char *);
+ logical orgati;
+ doublereal erretm, dtipsq, rhoinv;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* This subroutine computes the square root of the I-th updated */
+/* eigenvalue of a positive symmetric rank-one modification to */
+/* a positive diagonal matrix whose entries are given as the squares */
+/* of the corresponding entries in the array d, and that */
+
+/* 0 <= D(i) < D(j) for i < j */
+
+/* and that RHO > 0. This is arranged by the calling routine, and is */
+/* no loss in generality. The rank-one modified system is thus */
+
+/* diag( D ) * diag( D ) + RHO * Z * Z_transpose. */
+
+/* where we assume the Euclidean norm of Z is 1. */
+
+/* The method consists of approximating the rational functions in the */
+/* secular equation by simpler interpolating rational functions. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The length of all arrays. */
+
+/* I (input) INTEGER */
+/* The index of the eigenvalue to be computed. 1 <= I <= N. */
+
+/* D (input) DOUBLE PRECISION array, dimension ( N ) */
+/* The original eigenvalues. It is assumed that they are in */
+/* order, 0 <= D(I) < D(J) for I < J. */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( N ) */
+/* The components of the updating vector. */
+
+/* DELTA (output) DOUBLE PRECISION array, dimension ( N ) */
+/* If N .ne. 1, DELTA contains (D(j) - sigma_I) in its j-th */
+/* component. If N = 1, then DELTA(1) = 1. The vector DELTA */
+/* contains the information necessary to construct the */
+/* (singular) eigenvectors. */
+
+/* RHO (input) DOUBLE PRECISION */
+/* The scalar in the symmetric updating formula. */
+
+/* SIGMA (output) DOUBLE PRECISION */
+/* The computed sigma_I, the I-th updated eigenvalue. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension ( N ) */
+/* If N .ne. 1, WORK contains (D(j) + sigma_I) in its j-th */
+/* component. If N = 1, then WORK( 1 ) = 1. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* > 0: if INFO = 1, the updating process failed. */
+
+/* Internal Parameters */
+/* =================== */
+
+/* Logical variable ORGATI (origin-at-i?) is used for distinguishing */
+/* whether D(i) or D(i+1) is treated as the origin. */
+
+/* ORGATI = .true. origin at i */
+/* ORGATI = .false. origin at i+1 */
+
+/* Logical variable SWTCH3 (switch-for-3-poles?) is for noting */
+/* if we are working with THREE poles! */
+
+/* MAXIT is the maximum number of iterations allowed for each */
+/* eigenvalue. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ren-Cang Li, Computer Science Division, University of California */
+/* at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Since this routine is called in an inner loop, we do no argument */
+/* checking. */
+
+/* Quick return for N=1 and 2. */
+
+ /* Parameter adjustments */
+ --work;
+ --delta;
+ --z__;
+ --d__;
+
+ /* Function Body */
+ *info = 0;
+ if (*n == 1) {
+
+/* Presumably, I=1 upon entry */
+
+ *sigma = sqrt(d__[1] * d__[1] + *rho * z__[1] * z__[1]);
+ delta[1] = 1.;
+ work[1] = 1.;
+ return 0;
+ }
+ if (*n == 2) {
+ dlasd5_(i__, &d__[1], &z__[1], &delta[1], rho, sigma, &work[1]);
+ return 0;
+ }
+
+/* Compute machine epsilon */
+
+ eps = dlamch_("Epsilon");
+ rhoinv = 1. / *rho;
+
+/* The case I = N */
+
+ if (*i__ == *n) {
+
+/* Initialize some basic variables */
+
+ ii = *n - 1;
+ niter = 1;
+
+/* Calculate initial guess */
+
+ temp = *rho / 2.;
+
+/* If ||Z||_2 is not one, then TEMP should be set to */
+/* RHO * ||Z||_2^2 / TWO */
+
+ temp1 = temp / (d__[*n] + sqrt(d__[*n] * d__[*n] + temp));
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] = d__[j] + d__[*n] + temp1;
+ delta[j] = d__[j] - d__[*n] - temp1;
+/* L10: */
+ }
+
+ psi = 0.;
+ i__1 = *n - 2;
+ for (j = 1; j <= i__1; ++j) {
+ psi += z__[j] * z__[j] / (delta[j] * work[j]);
+/* L20: */
+ }
+
+ c__ = rhoinv + psi;
+ w = c__ + z__[ii] * z__[ii] / (delta[ii] * work[ii]) + z__[*n] * z__[*
+ n] / (delta[*n] * work[*n]);
+
+ if (w <= 0.) {
+ temp1 = sqrt(d__[*n] * d__[*n] + *rho);
+ temp = z__[*n - 1] * z__[*n - 1] / ((d__[*n - 1] + temp1) * (d__[*
+ n] - d__[*n - 1] + *rho / (d__[*n] + temp1))) + z__[*n] *
+ z__[*n] / *rho;
+
+/* The following TAU is to approximate */
+/* SIGMA_n^2 - D( N )*D( N ) */
+
+ if (c__ <= temp) {
+ tau = *rho;
+ } else {
+ delsq = (d__[*n] - d__[*n - 1]) * (d__[*n] + d__[*n - 1]);
+ a = -c__ * delsq + z__[*n - 1] * z__[*n - 1] + z__[*n] * z__[*
+ n];
+ b = z__[*n] * z__[*n] * delsq;
+ if (a < 0.) {
+ tau = b * 2. / (sqrt(a * a + b * 4. * c__) - a);
+ } else {
+ tau = (a + sqrt(a * a + b * 4. * c__)) / (c__ * 2.);
+ }
+ }
+
+/* It can be proved that */
+/* D(N)^2+RHO/2 <= SIGMA_n^2 < D(N)^2+TAU <= D(N)^2+RHO */
+
+ } else {
+ delsq = (d__[*n] - d__[*n - 1]) * (d__[*n] + d__[*n - 1]);
+ a = -c__ * delsq + z__[*n - 1] * z__[*n - 1] + z__[*n] * z__[*n];
+ b = z__[*n] * z__[*n] * delsq;
+
+/* The following TAU is to approximate */
+/* SIGMA_n^2 - D( N )*D( N ) */
+
+ if (a < 0.) {
+ tau = b * 2. / (sqrt(a * a + b * 4. * c__) - a);
+ } else {
+ tau = (a + sqrt(a * a + b * 4. * c__)) / (c__ * 2.);
+ }
+
+/* It can be proved that */
+/* D(N)^2 < D(N)^2+TAU < SIGMA(N)^2 < D(N)^2+RHO/2 */
+
+ }
+
+/* The following ETA is to approximate SIGMA_n - D( N ) */
+
+ eta = tau / (d__[*n] + sqrt(d__[*n] * d__[*n] + tau));
+
+ *sigma = d__[*n] + eta;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ delta[j] = d__[j] - d__[*i__] - eta;
+ work[j] = d__[j] + d__[*i__] + eta;
+/* L30: */
+ }
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = ii;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (delta[j] * work[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L40: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ temp = z__[*n] / (delta[*n] * work[*n]);
+ phi = z__[*n] * temp;
+ dphi = temp * temp;
+ erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + abs(tau) * (dpsi
+ + dphi);
+
+ w = rhoinv + phi + psi;
+
+/* Test for convergence */
+
+ if (abs(w) <= eps * erretm) {
+ goto L240;
+ }
+
+/* Calculate the new step */
+
+ ++niter;
+ dtnsq1 = work[*n - 1] * delta[*n - 1];
+ dtnsq = work[*n] * delta[*n];
+ c__ = w - dtnsq1 * dpsi - dtnsq * dphi;
+ a = (dtnsq + dtnsq1) * w - dtnsq * dtnsq1 * (dpsi + dphi);
+ b = dtnsq * dtnsq1 * w;
+ if (c__ < 0.) {
+ c__ = abs(c__);
+ }
+ if (c__ == 0.) {
+ eta = *rho - *sigma * *sigma;
+ } else if (a >= 0.) {
+ eta = (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (c__
+ * 2.);
+ } else {
+ eta = b * 2. / (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))
+ );
+ }
+
+/* Note, eta should be positive if w is negative, and */
+/* eta should be negative otherwise. However, */
+/* if for some reason caused by roundoff, eta*w > 0, */
+/* we simply use one Newton step instead. This way */
+/* will guarantee eta*w < 0. */
+
+ if (w * eta > 0.) {
+ eta = -w / (dpsi + dphi);
+ }
+ temp = eta - dtnsq;
+ if (temp > *rho) {
+ eta = *rho + dtnsq;
+ }
+
+ tau += eta;
+ eta /= *sigma + sqrt(eta + *sigma * *sigma);
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ delta[j] -= eta;
+ work[j] += eta;
+/* L50: */
+ }
+
+ *sigma += eta;
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = ii;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L60: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ temp = z__[*n] / (work[*n] * delta[*n]);
+ phi = z__[*n] * temp;
+ dphi = temp * temp;
+ erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + abs(tau) * (dpsi
+ + dphi);
+
+ w = rhoinv + phi + psi;
+
+/* Main loop to update the values of the array DELTA */
+
+ iter = niter + 1;
+
+ for (niter = iter; niter <= 20; ++niter) {
+
+/* Test for convergence */
+
+ if (abs(w) <= eps * erretm) {
+ goto L240;
+ }
+
+/* Calculate the new step */
+
+ dtnsq1 = work[*n - 1] * delta[*n - 1];
+ dtnsq = work[*n] * delta[*n];
+ c__ = w - dtnsq1 * dpsi - dtnsq * dphi;
+ a = (dtnsq + dtnsq1) * w - dtnsq1 * dtnsq * (dpsi + dphi);
+ b = dtnsq1 * dtnsq * w;
+ if (a >= 0.) {
+ eta = (a + sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (
+ c__ * 2.);
+ } else {
+ eta = b * 2. / (a - sqrt((d__1 = a * a - b * 4. * c__, abs(
+ d__1))));
+ }
+
+/* Note, eta should be positive if w is negative, and */
+/* eta should be negative otherwise. However, */
+/* if for some reason caused by roundoff, eta*w > 0, */
+/* we simply use one Newton step instead. This way */
+/* will guarantee eta*w < 0. */
+
+ if (w * eta > 0.) {
+ eta = -w / (dpsi + dphi);
+ }
+ temp = eta - dtnsq;
+ if (temp <= 0.) {
+ eta /= 2.;
+ }
+
+ tau += eta;
+ eta /= *sigma + sqrt(eta + *sigma * *sigma);
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ delta[j] -= eta;
+ work[j] += eta;
+/* L70: */
+ }
+
+ *sigma += eta;
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = ii;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L80: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ temp = z__[*n] / (work[*n] * delta[*n]);
+ phi = z__[*n] * temp;
+ dphi = temp * temp;
+ erretm = (-phi - psi) * 8. + erretm - phi + rhoinv + abs(tau) * (
+ dpsi + dphi);
+
+ w = rhoinv + phi + psi;
+/* L90: */
+ }
+
+/* Return with INFO = 1, NITER = MAXIT and not converged */
+
+ *info = 1;
+ goto L240;
+
+/* End for the case I = N */
+
+ } else {
+
+/* The case for I < N */
+
+ niter = 1;
+ ip1 = *i__ + 1;
+
+/* Calculate initial guess */
+
+ delsq = (d__[ip1] - d__[*i__]) * (d__[ip1] + d__[*i__]);
+ delsq2 = delsq / 2.;
+ temp = delsq2 / (d__[*i__] + sqrt(d__[*i__] * d__[*i__] + delsq2));
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] = d__[j] + d__[*i__] + temp;
+ delta[j] = d__[j] - d__[*i__] - temp;
+/* L100: */
+ }
+
+ psi = 0.;
+ i__1 = *i__ - 1;
+ for (j = 1; j <= i__1; ++j) {
+ psi += z__[j] * z__[j] / (work[j] * delta[j]);
+/* L110: */
+ }
+
+ phi = 0.;
+ i__1 = *i__ + 2;
+ for (j = *n; j >= i__1; --j) {
+ phi += z__[j] * z__[j] / (work[j] * delta[j]);
+/* L120: */
+ }
+ c__ = rhoinv + psi + phi;
+ w = c__ + z__[*i__] * z__[*i__] / (work[*i__] * delta[*i__]) + z__[
+ ip1] * z__[ip1] / (work[ip1] * delta[ip1]);
+
+ if (w > 0.) {
+
+/* d(i)^2 < the ith sigma^2 < (d(i)^2+d(i+1)^2)/2 */
+
+/* We choose d(i) as origin. */
+
+ orgati = TRUE_;
+ sg2lb = 0.;
+ sg2ub = delsq2;
+ a = c__ * delsq + z__[*i__] * z__[*i__] + z__[ip1] * z__[ip1];
+ b = z__[*i__] * z__[*i__] * delsq;
+ if (a > 0.) {
+ tau = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(
+ d__1))));
+ } else {
+ tau = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (
+ c__ * 2.);
+ }
+
+/* TAU now is an estimation of SIGMA^2 - D( I )^2. The */
+/* following, however, is the corresponding estimation of */
+/* SIGMA - D( I ). */
+
+ eta = tau / (d__[*i__] + sqrt(d__[*i__] * d__[*i__] + tau));
+ } else {
+
+/* (d(i)^2+d(i+1)^2)/2 <= the ith sigma^2 < d(i+1)^2/2 */
+
+/* We choose d(i+1) as origin. */
+
+ orgati = FALSE_;
+ sg2lb = -delsq2;
+ sg2ub = 0.;
+ a = c__ * delsq - z__[*i__] * z__[*i__] - z__[ip1] * z__[ip1];
+ b = z__[ip1] * z__[ip1] * delsq;
+ if (a < 0.) {
+ tau = b * 2. / (a - sqrt((d__1 = a * a + b * 4. * c__, abs(
+ d__1))));
+ } else {
+ tau = -(a + sqrt((d__1 = a * a + b * 4. * c__, abs(d__1)))) /
+ (c__ * 2.);
+ }
+
+/* TAU now is an estimation of SIGMA^2 - D( IP1 )^2. The */
+/* following, however, is the corresponding estimation of */
+/* SIGMA - D( IP1 ). */
+
+ eta = tau / (d__[ip1] + sqrt((d__1 = d__[ip1] * d__[ip1] + tau,
+ abs(d__1))));
+ }
+
+ if (orgati) {
+ ii = *i__;
+ *sigma = d__[*i__] + eta;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] = d__[j] + d__[*i__] + eta;
+ delta[j] = d__[j] - d__[*i__] - eta;
+/* L130: */
+ }
+ } else {
+ ii = *i__ + 1;
+ *sigma = d__[ip1] + eta;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] = d__[j] + d__[ip1] + eta;
+ delta[j] = d__[j] - d__[ip1] - eta;
+/* L140: */
+ }
+ }
+ iim1 = ii - 1;
+ iip1 = ii + 1;
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = iim1;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L150: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ dphi = 0.;
+ phi = 0.;
+ i__1 = iip1;
+ for (j = *n; j >= i__1; --j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ phi += z__[j] * temp;
+ dphi += temp * temp;
+ erretm += phi;
+/* L160: */
+ }
+
+ w = rhoinv + phi + psi;
+
+/* W is the value of the secular function with */
+/* its ii-th element removed. */
+
+ swtch3 = FALSE_;
+ if (orgati) {
+ if (w < 0.) {
+ swtch3 = TRUE_;
+ }
+ } else {
+ if (w > 0.) {
+ swtch3 = TRUE_;
+ }
+ }
+ if (ii == 1 || ii == *n) {
+ swtch3 = FALSE_;
+ }
+
+ temp = z__[ii] / (work[ii] * delta[ii]);
+ dw = dpsi + dphi + temp * temp;
+ temp = z__[ii] * temp;
+ w += temp;
+ erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + abs(temp) * 3. +
+ abs(tau) * dw;
+
+/* Test for convergence */
+
+ if (abs(w) <= eps * erretm) {
+ goto L240;
+ }
+
+ if (w <= 0.) {
+ sg2lb = max(sg2lb,tau);
+ } else {
+ sg2ub = min(sg2ub,tau);
+ }
+
+/* Calculate the new step */
+
+ ++niter;
+ if (! swtch3) {
+ dtipsq = work[ip1] * delta[ip1];
+ dtisq = work[*i__] * delta[*i__];
+ if (orgati) {
+/* Computing 2nd power */
+ d__1 = z__[*i__] / dtisq;
+ c__ = w - dtipsq * dw + delsq * (d__1 * d__1);
+ } else {
+/* Computing 2nd power */
+ d__1 = z__[ip1] / dtipsq;
+ c__ = w - dtisq * dw - delsq * (d__1 * d__1);
+ }
+ a = (dtipsq + dtisq) * w - dtipsq * dtisq * dw;
+ b = dtipsq * dtisq * w;
+ if (c__ == 0.) {
+ if (a == 0.) {
+ if (orgati) {
+ a = z__[*i__] * z__[*i__] + dtipsq * dtipsq * (dpsi +
+ dphi);
+ } else {
+ a = z__[ip1] * z__[ip1] + dtisq * dtisq * (dpsi +
+ dphi);
+ }
+ }
+ eta = b / a;
+ } else if (a <= 0.) {
+ eta = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1)))) / (
+ c__ * 2.);
+ } else {
+ eta = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__, abs(
+ d__1))));
+ }
+ } else {
+
+/* Interpolation using THREE most relevant poles */
+
+ dtiim = work[iim1] * delta[iim1];
+ dtiip = work[iip1] * delta[iip1];
+ temp = rhoinv + psi + phi;
+ if (orgati) {
+ temp1 = z__[iim1] / dtiim;
+ temp1 *= temp1;
+ c__ = temp - dtiip * (dpsi + dphi) - (d__[iim1] - d__[iip1]) *
+ (d__[iim1] + d__[iip1]) * temp1;
+ zz[0] = z__[iim1] * z__[iim1];
+ if (dpsi < temp1) {
+ zz[2] = dtiip * dtiip * dphi;
+ } else {
+ zz[2] = dtiip * dtiip * (dpsi - temp1 + dphi);
+ }
+ } else {
+ temp1 = z__[iip1] / dtiip;
+ temp1 *= temp1;
+ c__ = temp - dtiim * (dpsi + dphi) - (d__[iip1] - d__[iim1]) *
+ (d__[iim1] + d__[iip1]) * temp1;
+ if (dphi < temp1) {
+ zz[0] = dtiim * dtiim * dpsi;
+ } else {
+ zz[0] = dtiim * dtiim * (dpsi + (dphi - temp1));
+ }
+ zz[2] = z__[iip1] * z__[iip1];
+ }
+ zz[1] = z__[ii] * z__[ii];
+ dd[0] = dtiim;
+ dd[1] = delta[ii] * work[ii];
+ dd[2] = dtiip;
+ dlaed6_(&niter, &orgati, &c__, dd, zz, &w, &eta, info);
+ if (*info != 0) {
+ goto L240;
+ }
+ }
+
+/* Note, eta should be positive if w is negative, and */
+/* eta should be negative otherwise. However, */
+/* if for some reason caused by roundoff, eta*w > 0, */
+/* we simply use one Newton step instead. This way */
+/* will guarantee eta*w < 0. */
+
+ if (w * eta >= 0.) {
+ eta = -w / dw;
+ }
+ if (orgati) {
+ temp1 = work[*i__] * delta[*i__];
+ temp = eta - temp1;
+ } else {
+ temp1 = work[ip1] * delta[ip1];
+ temp = eta - temp1;
+ }
+ if (temp > sg2ub || temp < sg2lb) {
+ if (w < 0.) {
+ eta = (sg2ub - tau) / 2.;
+ } else {
+ eta = (sg2lb - tau) / 2.;
+ }
+ }
+
+ tau += eta;
+ eta /= *sigma + sqrt(*sigma * *sigma + eta);
+
+ prew = w;
+
+ *sigma += eta;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] += eta;
+ delta[j] -= eta;
+/* L170: */
+ }
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = iim1;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L180: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ dphi = 0.;
+ phi = 0.;
+ i__1 = iip1;
+ for (j = *n; j >= i__1; --j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ phi += z__[j] * temp;
+ dphi += temp * temp;
+ erretm += phi;
+/* L190: */
+ }
+
+ temp = z__[ii] / (work[ii] * delta[ii]);
+ dw = dpsi + dphi + temp * temp;
+ temp = z__[ii] * temp;
+ w = rhoinv + phi + psi + temp;
+ erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + abs(temp) * 3. +
+ abs(tau) * dw;
+
+ if (w <= 0.) {
+ sg2lb = max(sg2lb,tau);
+ } else {
+ sg2ub = min(sg2ub,tau);
+ }
+
+ swtch = FALSE_;
+ if (orgati) {
+ if (-w > abs(prew) / 10.) {
+ swtch = TRUE_;
+ }
+ } else {
+ if (w > abs(prew) / 10.) {
+ swtch = TRUE_;
+ }
+ }
+
+/* Main loop to update the values of the array DELTA and WORK */
+
+ iter = niter + 1;
+
+ for (niter = iter; niter <= 20; ++niter) {
+
+/* Test for convergence */
+
+ if (abs(w) <= eps * erretm) {
+ goto L240;
+ }
+
+/* Calculate the new step */
+
+ if (! swtch3) {
+ dtipsq = work[ip1] * delta[ip1];
+ dtisq = work[*i__] * delta[*i__];
+ if (! swtch) {
+ if (orgati) {
+/* Computing 2nd power */
+ d__1 = z__[*i__] / dtisq;
+ c__ = w - dtipsq * dw + delsq * (d__1 * d__1);
+ } else {
+/* Computing 2nd power */
+ d__1 = z__[ip1] / dtipsq;
+ c__ = w - dtisq * dw - delsq * (d__1 * d__1);
+ }
+ } else {
+ temp = z__[ii] / (work[ii] * delta[ii]);
+ if (orgati) {
+ dpsi += temp * temp;
+ } else {
+ dphi += temp * temp;
+ }
+ c__ = w - dtisq * dpsi - dtipsq * dphi;
+ }
+ a = (dtipsq + dtisq) * w - dtipsq * dtisq * dw;
+ b = dtipsq * dtisq * w;
+ if (c__ == 0.) {
+ if (a == 0.) {
+ if (! swtch) {
+ if (orgati) {
+ a = z__[*i__] * z__[*i__] + dtipsq * dtipsq *
+ (dpsi + dphi);
+ } else {
+ a = z__[ip1] * z__[ip1] + dtisq * dtisq * (
+ dpsi + dphi);
+ }
+ } else {
+ a = dtisq * dtisq * dpsi + dtipsq * dtipsq * dphi;
+ }
+ }
+ eta = b / a;
+ } else if (a <= 0.) {
+ eta = (a - sqrt((d__1 = a * a - b * 4. * c__, abs(d__1))))
+ / (c__ * 2.);
+ } else {
+ eta = b * 2. / (a + sqrt((d__1 = a * a - b * 4. * c__,
+ abs(d__1))));
+ }
+ } else {
+
+/* Interpolation using THREE most relevant poles */
+
+ dtiim = work[iim1] * delta[iim1];
+ dtiip = work[iip1] * delta[iip1];
+ temp = rhoinv + psi + phi;
+ if (swtch) {
+ c__ = temp - dtiim * dpsi - dtiip * dphi;
+ zz[0] = dtiim * dtiim * dpsi;
+ zz[2] = dtiip * dtiip * dphi;
+ } else {
+ if (orgati) {
+ temp1 = z__[iim1] / dtiim;
+ temp1 *= temp1;
+ temp2 = (d__[iim1] - d__[iip1]) * (d__[iim1] + d__[
+ iip1]) * temp1;
+ c__ = temp - dtiip * (dpsi + dphi) - temp2;
+ zz[0] = z__[iim1] * z__[iim1];
+ if (dpsi < temp1) {
+ zz[2] = dtiip * dtiip * dphi;
+ } else {
+ zz[2] = dtiip * dtiip * (dpsi - temp1 + dphi);
+ }
+ } else {
+ temp1 = z__[iip1] / dtiip;
+ temp1 *= temp1;
+ temp2 = (d__[iip1] - d__[iim1]) * (d__[iim1] + d__[
+ iip1]) * temp1;
+ c__ = temp - dtiim * (dpsi + dphi) - temp2;
+ if (dphi < temp1) {
+ zz[0] = dtiim * dtiim * dpsi;
+ } else {
+ zz[0] = dtiim * dtiim * (dpsi + (dphi - temp1));
+ }
+ zz[2] = z__[iip1] * z__[iip1];
+ }
+ }
+ dd[0] = dtiim;
+ dd[1] = delta[ii] * work[ii];
+ dd[2] = dtiip;
+ dlaed6_(&niter, &orgati, &c__, dd, zz, &w, &eta, info);
+ if (*info != 0) {
+ goto L240;
+ }
+ }
+
+/* Note, eta should be positive if w is negative, and */
+/* eta should be negative otherwise. However, */
+/* if for some reason caused by roundoff, eta*w > 0, */
+/* we simply use one Newton step instead. This way */
+/* will guarantee eta*w < 0. */
+
+ if (w * eta >= 0.) {
+ eta = -w / dw;
+ }
+ if (orgati) {
+ temp1 = work[*i__] * delta[*i__];
+ temp = eta - temp1;
+ } else {
+ temp1 = work[ip1] * delta[ip1];
+ temp = eta - temp1;
+ }
+ if (temp > sg2ub || temp < sg2lb) {
+ if (w < 0.) {
+ eta = (sg2ub - tau) / 2.;
+ } else {
+ eta = (sg2lb - tau) / 2.;
+ }
+ }
+
+ tau += eta;
+ eta /= *sigma + sqrt(*sigma * *sigma + eta);
+
+ *sigma += eta;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ work[j] += eta;
+ delta[j] -= eta;
+/* L200: */
+ }
+
+ prew = w;
+
+/* Evaluate PSI and the derivative DPSI */
+
+ dpsi = 0.;
+ psi = 0.;
+ erretm = 0.;
+ i__1 = iim1;
+ for (j = 1; j <= i__1; ++j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ psi += z__[j] * temp;
+ dpsi += temp * temp;
+ erretm += psi;
+/* L210: */
+ }
+ erretm = abs(erretm);
+
+/* Evaluate PHI and the derivative DPHI */
+
+ dphi = 0.;
+ phi = 0.;
+ i__1 = iip1;
+ for (j = *n; j >= i__1; --j) {
+ temp = z__[j] / (work[j] * delta[j]);
+ phi += z__[j] * temp;
+ dphi += temp * temp;
+ erretm += phi;
+/* L220: */
+ }
+
+ temp = z__[ii] / (work[ii] * delta[ii]);
+ dw = dpsi + dphi + temp * temp;
+ temp = z__[ii] * temp;
+ w = rhoinv + phi + psi + temp;
+ erretm = (phi - psi) * 8. + erretm + rhoinv * 2. + abs(temp) * 3.
+ + abs(tau) * dw;
+ if (w * prew > 0. && abs(w) > abs(prew) / 10.) {
+ swtch = ! swtch;
+ }
+
+ if (w <= 0.) {
+ sg2lb = max(sg2lb,tau);
+ } else {
+ sg2ub = min(sg2ub,tau);
+ }
+
+/* L230: */
+ }
+
+/* Return with INFO = 1, NITER = MAXIT and not converged */
+
+ *info = 1;
+
+ }
+
+L240:
+ return 0;
+
+/* End of DLASD4 */
+
+} /* dlasd4_ */
diff --git a/3rdparty/clapack/src/dlasd5.c b/3rdparty/clapack/src/dlasd5.c
new file mode 100644
index 0000000..26fff26
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd5.c
@@ -0,0 +1,189 @@
+/* dlasd5.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasd5_(integer *i__, doublereal *d__, doublereal *z__,
+ doublereal *delta, doublereal *rho, doublereal *dsigma, doublereal *
+ work)
+{
+ /* System generated locals */
+ doublereal d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal b, c__, w, del, tau, delsq;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* This subroutine computes the square root of the I-th eigenvalue */
+/* of a positive symmetric rank-one modification of a 2-by-2 diagonal */
+/* matrix */
+
+/* diag( D ) * diag( D ) + RHO * Z * transpose(Z) . */
+
+/* The diagonal entries in the array D are assumed to satisfy */
+
+/* 0 <= D(i) < D(j) for i < j . */
+
+/* We also assume RHO > 0 and that the Euclidean norm of the vector */
+/* Z is one. */
+
+/* Arguments */
+/* ========= */
+
+/* I (input) INTEGER */
+/* The index of the eigenvalue to be computed. I = 1 or I = 2. */
+
+/* D (input) DOUBLE PRECISION array, dimension ( 2 ) */
+/* The original eigenvalues. We assume 0 <= D(1) < D(2). */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( 2 ) */
+/* The components of the updating vector. */
+
+/* DELTA (output) DOUBLE PRECISION array, dimension ( 2 ) */
+/* Contains (D(j) - sigma_I) in its j-th component. */
+/* The vector DELTA contains the information necessary */
+/* to construct the eigenvectors. */
+
+/* RHO (input) DOUBLE PRECISION */
+/* The scalar in the symmetric updating formula. */
+
+/* DSIGMA (output) DOUBLE PRECISION */
+/* The computed sigma_I, the I-th updated eigenvalue. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension ( 2 ) */
+/* WORK contains (D(j) + sigma_I) in its j-th component. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ren-Cang Li, Computer Science Division, University of California */
+/* at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --work;
+ --delta;
+ --z__;
+ --d__;
+
+ /* Function Body */
+ del = d__[2] - d__[1];
+ delsq = del * (d__[2] + d__[1]);
+ if (*i__ == 1) {
+ w = *rho * 4. * (z__[2] * z__[2] / (d__[1] + d__[2] * 3.) - z__[1] *
+ z__[1] / (d__[1] * 3. + d__[2])) / del + 1.;
+ if (w > 0.) {
+ b = delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
+ c__ = *rho * z__[1] * z__[1] * delsq;
+
+/* B > ZERO, always */
+
+/* The following TAU is DSIGMA * DSIGMA - D( 1 ) * D( 1 ) */
+
+ tau = c__ * 2. / (b + sqrt((d__1 = b * b - c__ * 4., abs(d__1))));
+
+/* The following TAU is DSIGMA - D( 1 ) */
+
+ tau /= d__[1] + sqrt(d__[1] * d__[1] + tau);
+ *dsigma = d__[1] + tau;
+ delta[1] = -tau;
+ delta[2] = del - tau;
+ work[1] = d__[1] * 2. + tau;
+ work[2] = d__[1] + tau + d__[2];
+/* DELTA( 1 ) = -Z( 1 ) / TAU */
+/* DELTA( 2 ) = Z( 2 ) / ( DEL-TAU ) */
+ } else {
+ b = -delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
+ c__ = *rho * z__[2] * z__[2] * delsq;
+
+/* The following TAU is DSIGMA * DSIGMA - D( 2 ) * D( 2 ) */
+
+ if (b > 0.) {
+ tau = c__ * -2. / (b + sqrt(b * b + c__ * 4.));
+ } else {
+ tau = (b - sqrt(b * b + c__ * 4.)) / 2.;
+ }
+
+/* The following TAU is DSIGMA - D( 2 ) */
+
+ tau /= d__[2] + sqrt((d__1 = d__[2] * d__[2] + tau, abs(d__1)));
+ *dsigma = d__[2] + tau;
+ delta[1] = -(del + tau);
+ delta[2] = -tau;
+ work[1] = d__[1] + tau + d__[2];
+ work[2] = d__[2] * 2. + tau;
+/* DELTA( 1 ) = -Z( 1 ) / ( DEL+TAU ) */
+/* DELTA( 2 ) = -Z( 2 ) / TAU */
+ }
+/* TEMP = SQRT( DELTA( 1 )*DELTA( 1 )+DELTA( 2 )*DELTA( 2 ) ) */
+/* DELTA( 1 ) = DELTA( 1 ) / TEMP */
+/* DELTA( 2 ) = DELTA( 2 ) / TEMP */
+ } else {
+
+/* Now I=2 */
+
+ b = -delsq + *rho * (z__[1] * z__[1] + z__[2] * z__[2]);
+ c__ = *rho * z__[2] * z__[2] * delsq;
+
+/* The following TAU is DSIGMA * DSIGMA - D( 2 ) * D( 2 ) */
+
+ if (b > 0.) {
+ tau = (b + sqrt(b * b + c__ * 4.)) / 2.;
+ } else {
+ tau = c__ * 2. / (-b + sqrt(b * b + c__ * 4.));
+ }
+
+/* The following TAU is DSIGMA - D( 2 ) */
+
+ tau /= d__[2] + sqrt(d__[2] * d__[2] + tau);
+ *dsigma = d__[2] + tau;
+ delta[1] = -(del + tau);
+ delta[2] = -tau;
+ work[1] = d__[1] + tau + d__[2];
+ work[2] = d__[2] * 2. + tau;
+/* DELTA( 1 ) = -Z( 1 ) / ( DEL+TAU ) */
+/* DELTA( 2 ) = -Z( 2 ) / TAU */
+/* TEMP = SQRT( DELTA( 1 )*DELTA( 1 )+DELTA( 2 )*DELTA( 2 ) ) */
+/* DELTA( 1 ) = DELTA( 1 ) / TEMP */
+/* DELTA( 2 ) = DELTA( 2 ) / TEMP */
+ }
+ return 0;
+
+/* End of DLASD5 */
+
+} /* dlasd5_ */
diff --git a/3rdparty/clapack/src/dlasd6.c b/3rdparty/clapack/src/dlasd6.c
new file mode 100644
index 0000000..f1d0ec6
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd6.c
@@ -0,0 +1,367 @@
+/* dlasd6.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__0 = 0;
+static doublereal c_b7 = 1.;
+static integer c__1 = 1;
+static integer c_n1 = -1;
+
+/* Subroutine */ int dlasd6_(integer *icompq, integer *nl, integer *nr,
+ integer *sqre, doublereal *d__, doublereal *vf, doublereal *vl,
+ doublereal *alpha, doublereal *beta, integer *idxq, integer *perm,
+ integer *givptr, integer *givcol, integer *ldgcol, doublereal *givnum,
+ integer *ldgnum, doublereal *poles, doublereal *difl, doublereal *
+ difr, doublereal *z__, integer *k, doublereal *c__, doublereal *s,
+ doublereal *work, integer *iwork, integer *info)
+{
+ /* System generated locals */
+ integer givcol_dim1, givcol_offset, givnum_dim1, givnum_offset,
+ poles_dim1, poles_offset, i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ integer i__, m, n, n1, n2, iw, idx, idxc, idxp, ivfw, ivlw;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *), dlasd7_(integer *, integer *, integer *,
+ integer *, integer *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, integer *, integer *,
+ integer *, integer *, integer *, integer *, integer *, doublereal
+ *, integer *, doublereal *, doublereal *, integer *), dlasd8_(
+ integer *, integer *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, integer *, doublereal *,
+ doublereal *, integer *), dlascl_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, integer *, doublereal *,
+ integer *, integer *), dlamrg_(integer *, integer *,
+ doublereal *, integer *, integer *, integer *);
+ integer isigma;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ doublereal orgnrm;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD6 computes the SVD of an updated upper bidiagonal matrix B */
+/* obtained by merging two smaller ones by appending a row. This */
+/* routine is used only for the problem which requires all singular */
+/* values and optionally singular vector matrices in factored form. */
+/* B is an N-by-M matrix with N = NL + NR + 1 and M = N + SQRE. */
+/* A related subroutine, DLASD1, handles the case in which all singular */
+/* values and singular vectors of the bidiagonal matrix are desired. */
+
+/* DLASD6 computes the SVD as follows: */
+
+/* ( D1(in) 0 0 0 ) */
+/* B = U(in) * ( Z1' a Z2' b ) * VT(in) */
+/* ( 0 0 D2(in) 0 ) */
+
+/* = U(out) * ( D(out) 0) * VT(out) */
+
+/* where Z' = (Z1' a Z2' b) = u' VT', and u is a vector of dimension M */
+/* with ALPHA and BETA in the NL+1 and NL+2 th entries and zeros */
+/* elsewhere; and the entry b is empty if SQRE = 0. */
+
+/* The singular values of B can be computed using D1, D2, the first */
+/* components of all the right singular vectors of the lower block, and */
+/* the last components of all the right singular vectors of the upper */
+/* block. These components are stored and updated in VF and VL, */
+/* respectively, in DLASD6. Hence U and VT are not explicitly */
+/* referenced. */
+
+/* The singular values are stored in D. The algorithm consists of two */
+/* stages: */
+
+/* The first stage consists of deflating the size of the problem */
+/* when there are multiple singular values or if there is a zero */
+/* in the Z vector. For each such occurence the dimension of the */
+/* secular equation problem is reduced by one. This stage is */
+/* performed by the routine DLASD7. */
+
+/* The second stage consists of calculating the updated */
+/* singular values. This is done by finding the roots of the */
+/* secular equation via the routine DLASD4 (as called by DLASD8). */
+/* This routine also updates VF and VL and computes the distances */
+/* between the updated singular values and the old singular */
+/* values. */
+
+/* DLASD6 is called from DLASDA. */
+
+/* Arguments */
+/* ========= */
+
+/* ICOMPQ (input) INTEGER */
+/* Specifies whether singular vectors are to be computed in */
+/* factored form: */
+/* = 0: Compute singular values only. */
+/* = 1: Compute singular vectors in factored form as well. */
+
+/* NL (input) INTEGER */
+/* The row dimension of the upper block. NL >= 1. */
+
+/* NR (input) INTEGER */
+/* The row dimension of the lower block. NR >= 1. */
+
+/* SQRE (input) INTEGER */
+/* = 0: the lower block is an NR-by-NR square matrix. */
+/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
+
+/* The bidiagonal matrix has row dimension N = NL + NR + 1, */
+/* and column dimension M = N + SQRE. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension ( NL+NR+1 ). */
+/* On entry D(1:NL,1:NL) contains the singular values of the */
+/* upper block, and D(NL+2:N) contains the singular values */
+/* of the lower block. On exit D(1:N) contains the singular */
+/* values of the modified matrix. */
+
+/* VF (input/output) DOUBLE PRECISION array, dimension ( M ) */
+/* On entry, VF(1:NL+1) contains the first components of all */
+/* right singular vectors of the upper block; and VF(NL+2:M) */
+/* contains the first components of all right singular vectors */
+/* of the lower block. On exit, VF contains the first components */
+/* of all right singular vectors of the bidiagonal matrix. */
+
+/* VL (input/output) DOUBLE PRECISION array, dimension ( M ) */
+/* On entry, VL(1:NL+1) contains the last components of all */
+/* right singular vectors of the upper block; and VL(NL+2:M) */
+/* contains the last components of all right singular vectors of */
+/* the lower block. On exit, VL contains the last components of */
+/* all right singular vectors of the bidiagonal matrix. */
+
+/* ALPHA (input/output) DOUBLE PRECISION */
+/* Contains the diagonal element associated with the added row. */
+
+/* BETA (input/output) DOUBLE PRECISION */
+/* Contains the off-diagonal element associated with the added */
+/* row. */
+
+/* IDXQ (output) INTEGER array, dimension ( N ) */
+/* This contains the permutation which will reintegrate the */
+/* subproblem just solved back into sorted order, i.e. */
+/* D( IDXQ( I = 1, N ) ) will be in ascending order. */
+
+/* PERM (output) INTEGER array, dimension ( N ) */
+/* The permutations (from deflation and sorting) to be applied */
+/* to each block. Not referenced if ICOMPQ = 0. */
+
+/* GIVPTR (output) INTEGER */
+/* The number of Givens rotations which took place in this */
+/* subproblem. Not referenced if ICOMPQ = 0. */
+
+/* GIVCOL (output) INTEGER array, dimension ( LDGCOL, 2 ) */
+/* Each pair of numbers indicates a pair of columns to take place */
+/* in a Givens rotation. Not referenced if ICOMPQ = 0. */
+
+/* LDGCOL (input) INTEGER */
+/* leading dimension of GIVCOL, must be at least N. */
+
+/* GIVNUM (output) DOUBLE PRECISION array, dimension ( LDGNUM, 2 ) */
+/* Each number indicates the C or S value to be used in the */
+/* corresponding Givens rotation. Not referenced if ICOMPQ = 0. */
+
+/* LDGNUM (input) INTEGER */
+/* The leading dimension of GIVNUM and POLES, must be at least N. */
+
+/* POLES (output) DOUBLE PRECISION array, dimension ( LDGNUM, 2 ) */
+/* On exit, POLES(1,*) is an array containing the new singular */
+/* values obtained from solving the secular equation, and */
+/* POLES(2,*) is an array containing the poles in the secular */
+/* equation. Not referenced if ICOMPQ = 0. */
+
+/* DIFL (output) DOUBLE PRECISION array, dimension ( N ) */
+/* On exit, DIFL(I) is the distance between I-th updated */
+/* (undeflated) singular value and the I-th (undeflated) old */
+/* singular value. */
+
+/* DIFR (output) DOUBLE PRECISION array, */
+/* dimension ( LDGNUM, 2 ) if ICOMPQ = 1 and */
+/* dimension ( N ) if ICOMPQ = 0. */
+/* On exit, DIFR(I, 1) is the distance between I-th updated */
+/* (undeflated) singular value and the I+1-th (undeflated) old */
+/* singular value. */
+
+/* If ICOMPQ = 1, DIFR(1:K,2) is an array containing the */
+/* normalizing factors for the right singular vector matrix. */
+
+/* See DLASD8 for details on DIFL and DIFR. */
+
+/* Z (output) DOUBLE PRECISION array, dimension ( M ) */
+/* The first elements of this array contain the components */
+/* of the deflation-adjusted updating row vector. */
+
+/* K (output) INTEGER */
+/* Contains the dimension of the non-deflated matrix, */
+/* This is the order of the related secular equation. 1 <= K <=N. */
+
+/* C (output) DOUBLE PRECISION */
+/* C contains garbage if SQRE =0 and the C-value of a Givens */
+/* rotation related to the right null space if SQRE = 1. */
+
+/* S (output) DOUBLE PRECISION */
+/* S contains garbage if SQRE =0 and the S-value of a Givens */
+/* rotation related to the right null space if SQRE = 1. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension ( 4 * M ) */
+
+/* IWORK (workspace) INTEGER array, dimension ( 3 * N ) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --vf;
+ --vl;
+ --idxq;
+ --perm;
+ givcol_dim1 = *ldgcol;
+ givcol_offset = 1 + givcol_dim1;
+ givcol -= givcol_offset;
+ poles_dim1 = *ldgnum;
+ poles_offset = 1 + poles_dim1;
+ poles -= poles_offset;
+ givnum_dim1 = *ldgnum;
+ givnum_offset = 1 + givnum_dim1;
+ givnum -= givnum_offset;
+ --difl;
+ --difr;
+ --z__;
+ --work;
+ --iwork;
+
+ /* Function Body */
+ *info = 0;
+ n = *nl + *nr + 1;
+ m = n + *sqre;
+
+ if (*icompq < 0 || *icompq > 1) {
+ *info = -1;
+ } else if (*nl < 1) {
+ *info = -2;
+ } else if (*nr < 1) {
+ *info = -3;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -4;
+ } else if (*ldgcol < n) {
+ *info = -14;
+ } else if (*ldgnum < n) {
+ *info = -16;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD6", &i__1);
+ return 0;
+ }
+
+/* The following values are for bookkeeping purposes only. They are */
+/* integer pointers which indicate the portion of the workspace */
+/* used by a particular array in DLASD7 and DLASD8. */
+
+ isigma = 1;
+ iw = isigma + n;
+ ivfw = iw + m;
+ ivlw = ivfw + m;
+
+ idx = 1;
+ idxc = idx + n;
+ idxp = idxc + n;
+
+/* Scale. */
+
+/* Computing MAX */
+ d__1 = abs(*alpha), d__2 = abs(*beta);
+ orgnrm = max(d__1,d__2);
+ d__[*nl + 1] = 0.;
+ i__1 = n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if ((d__1 = d__[i__], abs(d__1)) > orgnrm) {
+ orgnrm = (d__1 = d__[i__], abs(d__1));
+ }
+/* L10: */
+ }
+ dlascl_("G", &c__0, &c__0, &orgnrm, &c_b7, &n, &c__1, &d__[1], &n, info);
+ *alpha /= orgnrm;
+ *beta /= orgnrm;
+
+/* Sort and Deflate singular values. */
+
+ dlasd7_(icompq, nl, nr, sqre, k, &d__[1], &z__[1], &work[iw], &vf[1], &
+ work[ivfw], &vl[1], &work[ivlw], alpha, beta, &work[isigma], &
+ iwork[idx], &iwork[idxp], &idxq[1], &perm[1], givptr, &givcol[
+ givcol_offset], ldgcol, &givnum[givnum_offset], ldgnum, c__, s,
+ info);
+
+/* Solve Secular Equation, compute DIFL, DIFR, and update VF, VL. */
+
+ dlasd8_(icompq, k, &d__[1], &z__[1], &vf[1], &vl[1], &difl[1], &difr[1],
+ ldgnum, &work[isigma], &work[iw], info);
+
+/* Save the poles if ICOMPQ = 1. */
+
+ if (*icompq == 1) {
+ dcopy_(k, &d__[1], &c__1, &poles[poles_dim1 + 1], &c__1);
+ dcopy_(k, &work[isigma], &c__1, &poles[(poles_dim1 << 1) + 1], &c__1);
+ }
+
+/* Unscale. */
+
+ dlascl_("G", &c__0, &c__0, &c_b7, &orgnrm, &n, &c__1, &d__[1], &n, info);
+
+/* Prepare the IDXQ sorting permutation. */
+
+ n1 = *k;
+ n2 = n - *k;
+ dlamrg_(&n1, &n2, &d__[1], &c__1, &c_n1, &idxq[1]);
+
+ return 0;
+
+/* End of DLASD6 */
+
+} /* dlasd6_ */
diff --git a/3rdparty/clapack/src/dlasd7.c b/3rdparty/clapack/src/dlasd7.c
new file mode 100644
index 0000000..ea4ca05
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd7.c
@@ -0,0 +1,518 @@
+/* dlasd7.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dlasd7_(integer *icompq, integer *nl, integer *nr,
+ integer *sqre, integer *k, doublereal *d__, doublereal *z__,
+ doublereal *zw, doublereal *vf, doublereal *vfw, doublereal *vl,
+ doublereal *vlw, doublereal *alpha, doublereal *beta, doublereal *
+ dsigma, integer *idx, integer *idxp, integer *idxq, integer *perm,
+ integer *givptr, integer *givcol, integer *ldgcol, doublereal *givnum,
+ integer *ldgnum, doublereal *c__, doublereal *s, integer *info)
+{
+ /* System generated locals */
+ integer givcol_dim1, givcol_offset, givnum_dim1, givnum_offset, i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ integer i__, j, m, n, k2;
+ doublereal z1;
+ integer jp;
+ doublereal eps, tau, tol;
+ integer nlp1, nlp2, idxi, idxj;
+ extern /* Subroutine */ int drot_(integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *);
+ integer idxjp;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer jprev;
+ extern doublereal dlapy2_(doublereal *, doublereal *), dlamch_(char *);
+ extern /* Subroutine */ int dlamrg_(integer *, integer *, doublereal *,
+ integer *, integer *, integer *), xerbla_(char *, integer *);
+ doublereal hlftol;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD7 merges the two sets of singular values together into a single */
+/* sorted set. Then it tries to deflate the size of the problem. There */
+/* are two ways in which deflation can occur: when two or more singular */
+/* values are close together or if there is a tiny entry in the Z */
+/* vector. For each such occurrence the order of the related */
+/* secular equation problem is reduced by one. */
+
+/* DLASD7 is called from DLASD6. */
+
+/* Arguments */
+/* ========= */
+
+/* ICOMPQ (input) INTEGER */
+/* Specifies whether singular vectors are to be computed */
+/* in compact form, as follows: */
+/* = 0: Compute singular values only. */
+/* = 1: Compute singular vectors of upper */
+/* bidiagonal matrix in compact form. */
+
+/* NL (input) INTEGER */
+/* The row dimension of the upper block. NL >= 1. */
+
+/* NR (input) INTEGER */
+/* The row dimension of the lower block. NR >= 1. */
+
+/* SQRE (input) INTEGER */
+/* = 0: the lower block is an NR-by-NR square matrix. */
+/* = 1: the lower block is an NR-by-(NR+1) rectangular matrix. */
+
+/* The bidiagonal matrix has */
+/* N = NL + NR + 1 rows and */
+/* M = N + SQRE >= N columns. */
+
+/* K (output) INTEGER */
+/* Contains the dimension of the non-deflated matrix, this is */
+/* the order of the related secular equation. 1 <= K <=N. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension ( N ) */
+/* On entry D contains the singular values of the two submatrices */
+/* to be combined. On exit D contains the trailing (N-K) updated */
+/* singular values (those which were deflated) sorted into */
+/* increasing order. */
+
+/* Z (output) DOUBLE PRECISION array, dimension ( M ) */
+/* On exit Z contains the updating row vector in the secular */
+/* equation. */
+
+/* ZW (workspace) DOUBLE PRECISION array, dimension ( M ) */
+/* Workspace for Z. */
+
+/* VF (input/output) DOUBLE PRECISION array, dimension ( M ) */
+/* On entry, VF(1:NL+1) contains the first components of all */
+/* right singular vectors of the upper block; and VF(NL+2:M) */
+/* contains the first components of all right singular vectors */
+/* of the lower block. On exit, VF contains the first components */
+/* of all right singular vectors of the bidiagonal matrix. */
+
+/* VFW (workspace) DOUBLE PRECISION array, dimension ( M ) */
+/* Workspace for VF. */
+
+/* VL (input/output) DOUBLE PRECISION array, dimension ( M ) */
+/* On entry, VL(1:NL+1) contains the last components of all */
+/* right singular vectors of the upper block; and VL(NL+2:M) */
+/* contains the last components of all right singular vectors */
+/* of the lower block. On exit, VL contains the last components */
+/* of all right singular vectors of the bidiagonal matrix. */
+
+/* VLW (workspace) DOUBLE PRECISION array, dimension ( M ) */
+/* Workspace for VL. */
+
+/* ALPHA (input) DOUBLE PRECISION */
+/* Contains the diagonal element associated with the added row. */
+
+/* BETA (input) DOUBLE PRECISION */
+/* Contains the off-diagonal element associated with the added */
+/* row. */
+
+/* DSIGMA (output) DOUBLE PRECISION array, dimension ( N ) */
+/* Contains a copy of the diagonal elements (K-1 singular values */
+/* and one zero) in the secular equation. */
+
+/* IDX (workspace) INTEGER array, dimension ( N ) */
+/* This will contain the permutation used to sort the contents of */
+/* D into ascending order. */
+
+/* IDXP (workspace) INTEGER array, dimension ( N ) */
+/* This will contain the permutation used to place deflated */
+/* values of D at the end of the array. On output IDXP(2:K) */
+/* points to the nondeflated D-values and IDXP(K+1:N) */
+/* points to the deflated singular values. */
+
+/* IDXQ (input) INTEGER array, dimension ( N ) */
+/* This contains the permutation which separately sorts the two */
+/* sub-problems in D into ascending order. Note that entries in */
+/* the first half of this permutation must first be moved one */
+/* position backward; and entries in the second half */
+/* must first have NL+1 added to their values. */
+
+/* PERM (output) INTEGER array, dimension ( N ) */
+/* The permutations (from deflation and sorting) to be applied */
+/* to each singular block. Not referenced if ICOMPQ = 0. */
+
+/* GIVPTR (output) INTEGER */
+/* The number of Givens rotations which took place in this */
+/* subproblem. Not referenced if ICOMPQ = 0. */
+
+/* GIVCOL (output) INTEGER array, dimension ( LDGCOL, 2 ) */
+/* Each pair of numbers indicates a pair of columns to take place */
+/* in a Givens rotation. Not referenced if ICOMPQ = 0. */
+
+/* LDGCOL (input) INTEGER */
+/* The leading dimension of GIVCOL, must be at least N. */
+
+/* GIVNUM (output) DOUBLE PRECISION array, dimension ( LDGNUM, 2 ) */
+/* Each number indicates the C or S value to be used in the */
+/* corresponding Givens rotation. Not referenced if ICOMPQ = 0. */
+
+/* LDGNUM (input) INTEGER */
+/* The leading dimension of GIVNUM, must be at least N. */
+
+/* C (output) DOUBLE PRECISION */
+/* C contains garbage if SQRE =0 and the C-value of a Givens */
+/* rotation related to the right null space if SQRE = 1. */
+
+/* S (output) DOUBLE PRECISION */
+/* S contains garbage if SQRE =0 and the S-value of a Givens */
+/* rotation related to the right null space if SQRE = 1. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --z__;
+ --zw;
+ --vf;
+ --vfw;
+ --vl;
+ --vlw;
+ --dsigma;
+ --idx;
+ --idxp;
+ --idxq;
+ --perm;
+ givcol_dim1 = *ldgcol;
+ givcol_offset = 1 + givcol_dim1;
+ givcol -= givcol_offset;
+ givnum_dim1 = *ldgnum;
+ givnum_offset = 1 + givnum_dim1;
+ givnum -= givnum_offset;
+
+ /* Function Body */
+ *info = 0;
+ n = *nl + *nr + 1;
+ m = n + *sqre;
+
+ if (*icompq < 0 || *icompq > 1) {
+ *info = -1;
+ } else if (*nl < 1) {
+ *info = -2;
+ } else if (*nr < 1) {
+ *info = -3;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -4;
+ } else if (*ldgcol < n) {
+ *info = -22;
+ } else if (*ldgnum < n) {
+ *info = -24;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD7", &i__1);
+ return 0;
+ }
+
+ nlp1 = *nl + 1;
+ nlp2 = *nl + 2;
+ if (*icompq == 1) {
+ *givptr = 0;
+ }
+
+/* Generate the first part of the vector Z and move the singular */
+/* values in the first part of D one position backward. */
+
+ z1 = *alpha * vl[nlp1];
+ vl[nlp1] = 0.;
+ tau = vf[nlp1];
+ for (i__ = *nl; i__ >= 1; --i__) {
+ z__[i__ + 1] = *alpha * vl[i__];
+ vl[i__] = 0.;
+ vf[i__ + 1] = vf[i__];
+ d__[i__ + 1] = d__[i__];
+ idxq[i__ + 1] = idxq[i__] + 1;
+/* L10: */
+ }
+ vf[1] = tau;
+
+/* Generate the second part of the vector Z. */
+
+ i__1 = m;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ z__[i__] = *beta * vf[i__];
+ vf[i__] = 0.;
+/* L20: */
+ }
+
+/* Sort the singular values into increasing order */
+
+ i__1 = n;
+ for (i__ = nlp2; i__ <= i__1; ++i__) {
+ idxq[i__] += nlp1;
+/* L30: */
+ }
+
+/* DSIGMA, IDXC, IDXC, and ZW are used as storage space. */
+
+ i__1 = n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ dsigma[i__] = d__[idxq[i__]];
+ zw[i__] = z__[idxq[i__]];
+ vfw[i__] = vf[idxq[i__]];
+ vlw[i__] = vl[idxq[i__]];
+/* L40: */
+ }
+
+ dlamrg_(nl, nr, &dsigma[2], &c__1, &c__1, &idx[2]);
+
+ i__1 = n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ idxi = idx[i__] + 1;
+ d__[i__] = dsigma[idxi];
+ z__[i__] = zw[idxi];
+ vf[i__] = vfw[idxi];
+ vl[i__] = vlw[idxi];
+/* L50: */
+ }
+
+/* Calculate the allowable deflation tolerence */
+
+ eps = dlamch_("Epsilon");
+/* Computing MAX */
+ d__1 = abs(*alpha), d__2 = abs(*beta);
+ tol = max(d__1,d__2);
+/* Computing MAX */
+ d__2 = (d__1 = d__[n], abs(d__1));
+ tol = eps * 64. * max(d__2,tol);
+
+/* There are 2 kinds of deflation -- first a value in the z-vector */
+/* is small, second two (or more) singular values are very close */
+/* together (their difference is small). */
+
+/* If the value in the z-vector is small, we simply permute the */
+/* array so that the corresponding singular value is moved to the */
+/* end. */
+
+/* If two values in the D-vector are close, we perform a two-sided */
+/* rotation designed to make one of the corresponding z-vector */
+/* entries zero, and then permute the array so that the deflated */
+/* singular value is moved to the end. */
+
+/* If there are multiple singular values then the problem deflates. */
+/* Here the number of equal singular values are found. As each equal */
+/* singular value is found, an elementary reflector is computed to */
+/* rotate the corresponding singular subspace so that the */
+/* corresponding components of Z are zero in this new basis. */
+
+ *k = 1;
+ k2 = n + 1;
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ if ((d__1 = z__[j], abs(d__1)) <= tol) {
+
+/* Deflate due to small z component. */
+
+ --k2;
+ idxp[k2] = j;
+ if (j == n) {
+ goto L100;
+ }
+ } else {
+ jprev = j;
+ goto L70;
+ }
+/* L60: */
+ }
+L70:
+ j = jprev;
+L80:
+ ++j;
+ if (j > n) {
+ goto L90;
+ }
+ if ((d__1 = z__[j], abs(d__1)) <= tol) {
+
+/* Deflate due to small z component. */
+
+ --k2;
+ idxp[k2] = j;
+ } else {
+
+/* Check if singular values are close enough to allow deflation. */
+
+ if ((d__1 = d__[j] - d__[jprev], abs(d__1)) <= tol) {
+
+/* Deflation is possible. */
+
+ *s = z__[jprev];
+ *c__ = z__[j];
+
+/* Find sqrt(a**2+b**2) without overflow or */
+/* destructive underflow. */
+
+ tau = dlapy2_(c__, s);
+ z__[j] = tau;
+ z__[jprev] = 0.;
+ *c__ /= tau;
+ *s = -(*s) / tau;
+
+/* Record the appropriate Givens rotation */
+
+ if (*icompq == 1) {
+ ++(*givptr);
+ idxjp = idxq[idx[jprev] + 1];
+ idxj = idxq[idx[j] + 1];
+ if (idxjp <= nlp1) {
+ --idxjp;
+ }
+ if (idxj <= nlp1) {
+ --idxj;
+ }
+ givcol[*givptr + (givcol_dim1 << 1)] = idxjp;
+ givcol[*givptr + givcol_dim1] = idxj;
+ givnum[*givptr + (givnum_dim1 << 1)] = *c__;
+ givnum[*givptr + givnum_dim1] = *s;
+ }
+ drot_(&c__1, &vf[jprev], &c__1, &vf[j], &c__1, c__, s);
+ drot_(&c__1, &vl[jprev], &c__1, &vl[j], &c__1, c__, s);
+ --k2;
+ idxp[k2] = jprev;
+ jprev = j;
+ } else {
+ ++(*k);
+ zw[*k] = z__[jprev];
+ dsigma[*k] = d__[jprev];
+ idxp[*k] = jprev;
+ jprev = j;
+ }
+ }
+ goto L80;
+L90:
+
+/* Record the last singular value. */
+
+ ++(*k);
+ zw[*k] = z__[jprev];
+ dsigma[*k] = d__[jprev];
+ idxp[*k] = jprev;
+
+L100:
+
+/* Sort the singular values into DSIGMA. The singular values which */
+/* were not deflated go into the first K slots of DSIGMA, except */
+/* that DSIGMA(1) is treated separately. */
+
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ jp = idxp[j];
+ dsigma[j] = d__[jp];
+ vfw[j] = vf[jp];
+ vlw[j] = vl[jp];
+/* L110: */
+ }
+ if (*icompq == 1) {
+ i__1 = n;
+ for (j = 2; j <= i__1; ++j) {
+ jp = idxp[j];
+ perm[j] = idxq[idx[jp] + 1];
+ if (perm[j] <= nlp1) {
+ --perm[j];
+ }
+/* L120: */
+ }
+ }
+
+/* The deflated singular values go back into the last N - K slots of */
+/* D. */
+
+ i__1 = n - *k;
+ dcopy_(&i__1, &dsigma[*k + 1], &c__1, &d__[*k + 1], &c__1);
+
+/* Determine DSIGMA(1), DSIGMA(2), Z(1), VF(1), VL(1), VF(M), and */
+/* VL(M). */
+
+ dsigma[1] = 0.;
+ hlftol = tol / 2.;
+ if (abs(dsigma[2]) <= hlftol) {
+ dsigma[2] = hlftol;
+ }
+ if (m > n) {
+ z__[1] = dlapy2_(&z1, &z__[m]);
+ if (z__[1] <= tol) {
+ *c__ = 1.;
+ *s = 0.;
+ z__[1] = tol;
+ } else {
+ *c__ = z1 / z__[1];
+ *s = -z__[m] / z__[1];
+ }
+ drot_(&c__1, &vf[m], &c__1, &vf[1], &c__1, c__, s);
+ drot_(&c__1, &vl[m], &c__1, &vl[1], &c__1, c__, s);
+ } else {
+ if (abs(z1) <= tol) {
+ z__[1] = tol;
+ } else {
+ z__[1] = z1;
+ }
+ }
+
+/* Restore Z, VF, and VL. */
+
+ i__1 = *k - 1;
+ dcopy_(&i__1, &zw[2], &c__1, &z__[2], &c__1);
+ i__1 = n - 1;
+ dcopy_(&i__1, &vfw[2], &c__1, &vf[2], &c__1);
+ i__1 = n - 1;
+ dcopy_(&i__1, &vlw[2], &c__1, &vl[2], &c__1);
+
+ return 0;
+
+/* End of DLASD7 */
+
+} /* dlasd7_ */
diff --git a/3rdparty/clapack/src/dlasd8.c b/3rdparty/clapack/src/dlasd8.c
new file mode 100644
index 0000000..ab1b6c2
--- /dev/null
+++ b/3rdparty/clapack/src/dlasd8.c
@@ -0,0 +1,326 @@
+/* dlasd8.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c__0 = 0;
+static doublereal c_b8 = 1.;
+
+/* Subroutine */ int dlasd8_(integer *icompq, integer *k, doublereal *d__,
+ doublereal *z__, doublereal *vf, doublereal *vl, doublereal *difl,
+ doublereal *difr, integer *lddifr, doublereal *dsigma, doublereal *
+ work, integer *info)
+{
+ /* System generated locals */
+ integer difr_dim1, difr_offset, i__1, i__2;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal), d_sign(doublereal *, doublereal *);
+
+ /* Local variables */
+ integer i__, j;
+ doublereal dj, rho;
+ integer iwk1, iwk2, iwk3;
+ extern doublereal ddot_(integer *, doublereal *, integer *, doublereal *,
+ integer *);
+ doublereal temp;
+ extern doublereal dnrm2_(integer *, doublereal *, integer *);
+ integer iwk2i, iwk3i;
+ doublereal diflj, difrj, dsigj;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ extern doublereal dlamc3_(doublereal *, doublereal *);
+ extern /* Subroutine */ int dlasd4_(integer *, integer *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *), dlascl_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, integer *, doublereal *,
+ integer *, integer *), dlaset_(char *, integer *, integer
+ *, doublereal *, doublereal *, doublereal *, integer *),
+ xerbla_(char *, integer *);
+ doublereal dsigjp;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* October 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASD8 finds the square roots of the roots of the secular equation, */
+/* as defined by the values in DSIGMA and Z. It makes the appropriate */
+/* calls to DLASD4, and stores, for each element in D, the distance */
+/* to its two nearest poles (elements in DSIGMA). It also updates */
+/* the arrays VF and VL, the first and last components of all the */
+/* right singular vectors of the original bidiagonal matrix. */
+
+/* DLASD8 is called from DLASD6. */
+
+/* Arguments */
+/* ========= */
+
+/* ICOMPQ (input) INTEGER */
+/* Specifies whether singular vectors are to be computed in */
+/* factored form in the calling routine: */
+/* = 0: Compute singular values only. */
+/* = 1: Compute singular vectors in factored form as well. */
+
+/* K (input) INTEGER */
+/* The number of terms in the rational function to be solved */
+/* by DLASD4. K >= 1. */
+
+/* D (output) DOUBLE PRECISION array, dimension ( K ) */
+/* On output, D contains the updated singular values. */
+
+/* Z (input/output) DOUBLE PRECISION array, dimension ( K ) */
+/* On entry, the first K elements of this array contain the */
+/* components of the deflation-adjusted updating row vector. */
+/* On exit, Z is updated. */
+
+/* VF (input/output) DOUBLE PRECISION array, dimension ( K ) */
+/* On entry, VF contains information passed through DBEDE8. */
+/* On exit, VF contains the first K components of the first */
+/* components of all right singular vectors of the bidiagonal */
+/* matrix. */
+
+/* VL (input/output) DOUBLE PRECISION array, dimension ( K ) */
+/* On entry, VL contains information passed through DBEDE8. */
+/* On exit, VL contains the first K components of the last */
+/* components of all right singular vectors of the bidiagonal */
+/* matrix. */
+
+/* DIFL (output) DOUBLE PRECISION array, dimension ( K ) */
+/* On exit, DIFL(I) = D(I) - DSIGMA(I). */
+
+/* DIFR (output) DOUBLE PRECISION array, */
+/* dimension ( LDDIFR, 2 ) if ICOMPQ = 1 and */
+/* dimension ( K ) if ICOMPQ = 0. */
+/* On exit, DIFR(I,1) = D(I) - DSIGMA(I+1), DIFR(K,1) is not */
+/* defined and will not be referenced. */
+
+/* If ICOMPQ = 1, DIFR(1:K,2) is an array containing the */
+/* normalizing factors for the right singular vector matrix. */
+
+/* LDDIFR (input) INTEGER */
+/* The leading dimension of DIFR, must be at least K. */
+
+/* DSIGMA (input/output) DOUBLE PRECISION array, dimension ( K ) */
+/* On entry, the first K elements of this array contain the old */
+/* roots of the deflated updating problem. These are the poles */
+/* of the secular equation. */
+/* On exit, the elements of DSIGMA may be very slightly altered */
+/* in value. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension at least 3 * K */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --z__;
+ --vf;
+ --vl;
+ --difl;
+ difr_dim1 = *lddifr;
+ difr_offset = 1 + difr_dim1;
+ difr -= difr_offset;
+ --dsigma;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*icompq < 0 || *icompq > 1) {
+ *info = -1;
+ } else if (*k < 1) {
+ *info = -2;
+ } else if (*lddifr < *k) {
+ *info = -9;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASD8", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*k == 1) {
+ d__[1] = abs(z__[1]);
+ difl[1] = d__[1];
+ if (*icompq == 1) {
+ difl[2] = 1.;
+ difr[(difr_dim1 << 1) + 1] = 1.;
+ }
+ return 0;
+ }
+
+/* Modify values DSIGMA(i) to make sure all DSIGMA(i)-DSIGMA(j) can */
+/* be computed with high relative accuracy (barring over/underflow). */
+/* This is a problem on machines without a guard digit in */
+/* add/subtract (Cray XMP, Cray YMP, Cray C 90 and Cray 2). */
+/* The following code replaces DSIGMA(I) by 2*DSIGMA(I)-DSIGMA(I), */
+/* which on any of these machines zeros out the bottommost */
+/* bit of DSIGMA(I) if it is 1; this makes the subsequent */
+/* subtractions DSIGMA(I)-DSIGMA(J) unproblematic when cancellation */
+/* occurs. On binary machines with a guard digit (almost all */
+/* machines) it does not change DSIGMA(I) at all. On hexadecimal */
+/* and decimal machines with a guard digit, it slightly */
+/* changes the bottommost bits of DSIGMA(I). It does not account */
+/* for hexadecimal or decimal machines without guard digits */
+/* (we know of none). We use a subroutine call to compute */
+/* 2*DLAMBDA(I) to prevent optimizing compilers from eliminating */
+/* this code. */
+
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dsigma[i__] = dlamc3_(&dsigma[i__], &dsigma[i__]) - dsigma[i__];
+/* L10: */
+ }
+
+/* Book keeping. */
+
+ iwk1 = 1;
+ iwk2 = iwk1 + *k;
+ iwk3 = iwk2 + *k;
+ iwk2i = iwk2 - 1;
+ iwk3i = iwk3 - 1;
+
+/* Normalize Z. */
+
+ rho = dnrm2_(k, &z__[1], &c__1);
+ dlascl_("G", &c__0, &c__0, &rho, &c_b8, k, &c__1, &z__[1], k, info);
+ rho *= rho;
+
+/* Initialize WORK(IWK3). */
+
+ dlaset_("A", k, &c__1, &c_b8, &c_b8, &work[iwk3], k);
+
+/* Compute the updated singular values, the arrays DIFL, DIFR, */
+/* and the updated Z. */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ dlasd4_(k, &j, &dsigma[1], &z__[1], &work[iwk1], &rho, &d__[j], &work[
+ iwk2], info);
+
+/* If the root finder fails, the computation is terminated. */
+
+ if (*info != 0) {
+ return 0;
+ }
+ work[iwk3i + j] = work[iwk3i + j] * work[j] * work[iwk2i + j];
+ difl[j] = -work[j];
+ difr[j + difr_dim1] = -work[j + 1];
+ i__2 = j - 1;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ work[iwk3i + i__] = work[iwk3i + i__] * work[i__] * work[iwk2i +
+ i__] / (dsigma[i__] - dsigma[j]) / (dsigma[i__] + dsigma[
+ j]);
+/* L20: */
+ }
+ i__2 = *k;
+ for (i__ = j + 1; i__ <= i__2; ++i__) {
+ work[iwk3i + i__] = work[iwk3i + i__] * work[i__] * work[iwk2i +
+ i__] / (dsigma[i__] - dsigma[j]) / (dsigma[i__] + dsigma[
+ j]);
+/* L30: */
+ }
+/* L40: */
+ }
+
+/* Compute updated Z. */
+
+ i__1 = *k;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__2 = sqrt((d__1 = work[iwk3i + i__], abs(d__1)));
+ z__[i__] = d_sign(&d__2, &z__[i__]);
+/* L50: */
+ }
+
+/* Update VF and VL. */
+
+ i__1 = *k;
+ for (j = 1; j <= i__1; ++j) {
+ diflj = difl[j];
+ dj = d__[j];
+ dsigj = -dsigma[j];
+ if (j < *k) {
+ difrj = -difr[j + difr_dim1];
+ dsigjp = -dsigma[j + 1];
+ }
+ work[j] = -z__[j] / diflj / (dsigma[j] + dj);
+ i__2 = j - 1;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ work[i__] = z__[i__] / (dlamc3_(&dsigma[i__], &dsigj) - diflj) / (
+ dsigma[i__] + dj);
+/* L60: */
+ }
+ i__2 = *k;
+ for (i__ = j + 1; i__ <= i__2; ++i__) {
+ work[i__] = z__[i__] / (dlamc3_(&dsigma[i__], &dsigjp) + difrj) /
+ (dsigma[i__] + dj);
+/* L70: */
+ }
+ temp = dnrm2_(k, &work[1], &c__1);
+ work[iwk2i + j] = ddot_(k, &work[1], &c__1, &vf[1], &c__1) / temp;
+ work[iwk3i + j] = ddot_(k, &work[1], &c__1, &vl[1], &c__1) / temp;
+ if (*icompq == 1) {
+ difr[j + (difr_dim1 << 1)] = temp;
+ }
+/* L80: */
+ }
+
+ dcopy_(k, &work[iwk2], &c__1, &vf[1], &c__1);
+ dcopy_(k, &work[iwk3], &c__1, &vl[1], &c__1);
+
+ return 0;
+
+/* End of DLASD8 */
+
+} /* dlasd8_ */
diff --git a/3rdparty/clapack/src/dlasda.c b/3rdparty/clapack/src/dlasda.c
new file mode 100644
index 0000000..4a501d1
--- /dev/null
+++ b/3rdparty/clapack/src/dlasda.c
@@ -0,0 +1,488 @@
+/* dlasda.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__0 = 0;
+static doublereal c_b11 = 0.;
+static doublereal c_b12 = 1.;
+static integer c__1 = 1;
+static integer c__2 = 2;
+
+/* Subroutine */ int dlasda_(integer *icompq, integer *smlsiz, integer *n,
+ integer *sqre, doublereal *d__, doublereal *e, doublereal *u, integer
+ *ldu, doublereal *vt, integer *k, doublereal *difl, doublereal *difr,
+ doublereal *z__, doublereal *poles, integer *givptr, integer *givcol,
+ integer *ldgcol, integer *perm, doublereal *givnum, doublereal *c__,
+ doublereal *s, doublereal *work, integer *iwork, integer *info)
+{
+ /* System generated locals */
+ integer givcol_dim1, givcol_offset, perm_dim1, perm_offset, difl_dim1,
+ difl_offset, difr_dim1, difr_offset, givnum_dim1, givnum_offset,
+ poles_dim1, poles_offset, u_dim1, u_offset, vt_dim1, vt_offset,
+ z_dim1, z_offset, i__1, i__2;
+
+ /* Builtin functions */
+ integer pow_ii(integer *, integer *);
+
+ /* Local variables */
+ integer i__, j, m, i1, ic, lf, nd, ll, nl, vf, nr, vl, im1, ncc, nlf, nrf,
+ vfi, iwk, vli, lvl, nru, ndb1, nlp1, lvl2, nrp1;
+ doublereal beta;
+ integer idxq, nlvl;
+ doublereal alpha;
+ integer inode, ndiml, ndimr, idxqi, itemp;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ integer sqrei;
+ extern /* Subroutine */ int dlasd6_(integer *, integer *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, integer *, integer *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ doublereal *, doublereal *, integer *, doublereal *, doublereal *,
+ doublereal *, integer *, integer *);
+ integer nwork1, nwork2;
+ extern /* Subroutine */ int dlasdq_(char *, integer *, integer *, integer
+ *, integer *, integer *, doublereal *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *), dlasdt_(integer *, integer *,
+ integer *, integer *, integer *, integer *, integer *), dlaset_(
+ char *, integer *, integer *, doublereal *, doublereal *,
+ doublereal *, integer *), xerbla_(char *, integer *);
+ integer smlszp;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* Using a divide and conquer approach, DLASDA computes the singular */
+/* value decomposition (SVD) of a real upper bidiagonal N-by-M matrix */
+/* B with diagonal D and offdiagonal E, where M = N + SQRE. The */
+/* algorithm computes the singular values in the SVD B = U * S * VT. */
+/* The orthogonal matrices U and VT are optionally computed in */
+/* compact form. */
+
+/* A related subroutine, DLASD0, computes the singular values and */
+/* the singular vectors in explicit form. */
+
+/* Arguments */
+/* ========= */
+
+/* ICOMPQ (input) INTEGER */
+/* Specifies whether singular vectors are to be computed */
+/* in compact form, as follows */
+/* = 0: Compute singular values only. */
+/* = 1: Compute singular vectors of upper bidiagonal */
+/* matrix in compact form. */
+
+/* SMLSIZ (input) INTEGER */
+/* The maximum size of the subproblems at the bottom of the */
+/* computation tree. */
+
+/* N (input) INTEGER */
+/* The row dimension of the upper bidiagonal matrix. This is */
+/* also the dimension of the main diagonal array D. */
+
+/* SQRE (input) INTEGER */
+/* Specifies the column dimension of the bidiagonal matrix. */
+/* = 0: The bidiagonal matrix has column dimension M = N; */
+/* = 1: The bidiagonal matrix has column dimension M = N + 1. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension ( N ) */
+/* On entry D contains the main diagonal of the bidiagonal */
+/* matrix. On exit D, if INFO = 0, contains its singular values. */
+
+/* E (input) DOUBLE PRECISION array, dimension ( M-1 ) */
+/* Contains the subdiagonal entries of the bidiagonal matrix. */
+/* On exit, E has been destroyed. */
+
+/* U (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, SMLSIZ ) if ICOMPQ = 1, and not referenced */
+/* if ICOMPQ = 0. If ICOMPQ = 1, on exit, U contains the left */
+/* singular vector matrices of all subproblems at the bottom */
+/* level. */
+
+/* LDU (input) INTEGER, LDU = > N. */
+/* The leading dimension of arrays U, VT, DIFL, DIFR, POLES, */
+/* GIVNUM, and Z. */
+
+/* VT (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, SMLSIZ+1 ) if ICOMPQ = 1, and not referenced */
+/* if ICOMPQ = 0. If ICOMPQ = 1, on exit, VT' contains the right */
+/* singular vector matrices of all subproblems at the bottom */
+/* level. */
+
+/* K (output) INTEGER array, */
+/* dimension ( N ) if ICOMPQ = 1 and dimension 1 if ICOMPQ = 0. */
+/* If ICOMPQ = 1, on exit, K(I) is the dimension of the I-th */
+/* secular equation on the computation tree. */
+
+/* DIFL (output) DOUBLE PRECISION array, dimension ( LDU, NLVL ), */
+/* where NLVL = floor(log_2 (N/SMLSIZ))). */
+
+/* DIFR (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, 2 * NLVL ) if ICOMPQ = 1 and */
+/* dimension ( N ) if ICOMPQ = 0. */
+/* If ICOMPQ = 1, on exit, DIFL(1:N, I) and DIFR(1:N, 2 * I - 1) */
+/* record distances between singular values on the I-th */
+/* level and singular values on the (I -1)-th level, and */
+/* DIFR(1:N, 2 * I ) contains the normalizing factors for */
+/* the right singular vector matrix. See DLASD8 for details. */
+
+/* Z (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, NLVL ) if ICOMPQ = 1 and */
+/* dimension ( N ) if ICOMPQ = 0. */
+/* The first K elements of Z(1, I) contain the components of */
+/* the deflation-adjusted updating row vector for subproblems */
+/* on the I-th level. */
+
+/* POLES (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, 2 * NLVL ) if ICOMPQ = 1, and not referenced */
+/* if ICOMPQ = 0. If ICOMPQ = 1, on exit, POLES(1, 2*I - 1) and */
+/* POLES(1, 2*I) contain the new and old singular values */
+/* involved in the secular equations on the I-th level. */
+
+/* GIVPTR (output) INTEGER array, */
+/* dimension ( N ) if ICOMPQ = 1, and not referenced if */
+/* ICOMPQ = 0. If ICOMPQ = 1, on exit, GIVPTR( I ) records */
+/* the number of Givens rotations performed on the I-th */
+/* problem on the computation tree. */
+
+/* GIVCOL (output) INTEGER array, */
+/* dimension ( LDGCOL, 2 * NLVL ) if ICOMPQ = 1, and not */
+/* referenced if ICOMPQ = 0. If ICOMPQ = 1, on exit, for each I, */
+/* GIVCOL(1, 2 *I - 1) and GIVCOL(1, 2 *I) record the locations */
+/* of Givens rotations performed on the I-th level on the */
+/* computation tree. */
+
+/* LDGCOL (input) INTEGER, LDGCOL = > N. */
+/* The leading dimension of arrays GIVCOL and PERM. */
+
+/* PERM (output) INTEGER array, */
+/* dimension ( LDGCOL, NLVL ) if ICOMPQ = 1, and not referenced */
+/* if ICOMPQ = 0. If ICOMPQ = 1, on exit, PERM(1, I) records */
+/* permutations done on the I-th level of the computation tree. */
+
+/* GIVNUM (output) DOUBLE PRECISION array, */
+/* dimension ( LDU, 2 * NLVL ) if ICOMPQ = 1, and not */
+/* referenced if ICOMPQ = 0. If ICOMPQ = 1, on exit, for each I, */
+/* GIVNUM(1, 2 *I - 1) and GIVNUM(1, 2 *I) record the C- and S- */
+/* values of Givens rotations performed on the I-th level on */
+/* the computation tree. */
+
+/* C (output) DOUBLE PRECISION array, */
+/* dimension ( N ) if ICOMPQ = 1, and dimension 1 if ICOMPQ = 0. */
+/* If ICOMPQ = 1 and the I-th subproblem is not square, on exit, */
+/* C( I ) contains the C-value of a Givens rotation related to */
+/* the right null space of the I-th subproblem. */
+
+/* S (output) DOUBLE PRECISION array, dimension ( N ) if */
+/* ICOMPQ = 1, and dimension 1 if ICOMPQ = 0. If ICOMPQ = 1 */
+/* and the I-th subproblem is not square, on exit, S( I ) */
+/* contains the S-value of a Givens rotation related to */
+/* the right null space of the I-th subproblem. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension */
+/* (6 * N + (SMLSIZ + 1)*(SMLSIZ + 1)). */
+
+/* IWORK (workspace) INTEGER array. */
+/* Dimension must be at least (7 * N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit. */
+/* < 0: if INFO = -i, the i-th argument had an illegal value. */
+/* > 0: if INFO = 1, an singular value did not converge */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --e;
+ givnum_dim1 = *ldu;
+ givnum_offset = 1 + givnum_dim1;
+ givnum -= givnum_offset;
+ poles_dim1 = *ldu;
+ poles_offset = 1 + poles_dim1;
+ poles -= poles_offset;
+ z_dim1 = *ldu;
+ z_offset = 1 + z_dim1;
+ z__ -= z_offset;
+ difr_dim1 = *ldu;
+ difr_offset = 1 + difr_dim1;
+ difr -= difr_offset;
+ difl_dim1 = *ldu;
+ difl_offset = 1 + difl_dim1;
+ difl -= difl_offset;
+ vt_dim1 = *ldu;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ --k;
+ --givptr;
+ perm_dim1 = *ldgcol;
+ perm_offset = 1 + perm_dim1;
+ perm -= perm_offset;
+ givcol_dim1 = *ldgcol;
+ givcol_offset = 1 + givcol_dim1;
+ givcol -= givcol_offset;
+ --c__;
+ --s;
+ --work;
+ --iwork;
+
+ /* Function Body */
+ *info = 0;
+
+ if (*icompq < 0 || *icompq > 1) {
+ *info = -1;
+ } else if (*smlsiz < 3) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -4;
+ } else if (*ldu < *n + *sqre) {
+ *info = -8;
+ } else if (*ldgcol < *n) {
+ *info = -17;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASDA", &i__1);
+ return 0;
+ }
+
+ m = *n + *sqre;
+
+/* If the input matrix is too small, call DLASDQ to find the SVD. */
+
+ if (*n <= *smlsiz) {
+ if (*icompq == 0) {
+ dlasdq_("U", sqre, n, &c__0, &c__0, &c__0, &d__[1], &e[1], &vt[
+ vt_offset], ldu, &u[u_offset], ldu, &u[u_offset], ldu, &
+ work[1], info);
+ } else {
+ dlasdq_("U", sqre, n, &m, n, &c__0, &d__[1], &e[1], &vt[vt_offset]
+, ldu, &u[u_offset], ldu, &u[u_offset], ldu, &work[1],
+ info);
+ }
+ return 0;
+ }
+
+/* Book-keeping and set up the computation tree. */
+
+ inode = 1;
+ ndiml = inode + *n;
+ ndimr = ndiml + *n;
+ idxq = ndimr + *n;
+ iwk = idxq + *n;
+
+ ncc = 0;
+ nru = 0;
+
+ smlszp = *smlsiz + 1;
+ vf = 1;
+ vl = vf + m;
+ nwork1 = vl + m;
+ nwork2 = nwork1 + smlszp * smlszp;
+
+ dlasdt_(n, &nlvl, &nd, &iwork[inode], &iwork[ndiml], &iwork[ndimr],
+ smlsiz);
+
+/* for the nodes on bottom level of the tree, solve */
+/* their subproblems by DLASDQ. */
+
+ ndb1 = (nd + 1) / 2;
+ i__1 = nd;
+ for (i__ = ndb1; i__ <= i__1; ++i__) {
+
+/* IC : center row of each node */
+/* NL : number of rows of left subproblem */
+/* NR : number of rows of right subproblem */
+/* NLF: starting row of the left subproblem */
+/* NRF: starting row of the right subproblem */
+
+ i1 = i__ - 1;
+ ic = iwork[inode + i1];
+ nl = iwork[ndiml + i1];
+ nlp1 = nl + 1;
+ nr = iwork[ndimr + i1];
+ nlf = ic - nl;
+ nrf = ic + 1;
+ idxqi = idxq + nlf - 2;
+ vfi = vf + nlf - 1;
+ vli = vl + nlf - 1;
+ sqrei = 1;
+ if (*icompq == 0) {
+ dlaset_("A", &nlp1, &nlp1, &c_b11, &c_b12, &work[nwork1], &smlszp);
+ dlasdq_("U", &sqrei, &nl, &nlp1, &nru, &ncc, &d__[nlf], &e[nlf], &
+ work[nwork1], &smlszp, &work[nwork2], &nl, &work[nwork2],
+ &nl, &work[nwork2], info);
+ itemp = nwork1 + nl * smlszp;
+ dcopy_(&nlp1, &work[nwork1], &c__1, &work[vfi], &c__1);
+ dcopy_(&nlp1, &work[itemp], &c__1, &work[vli], &c__1);
+ } else {
+ dlaset_("A", &nl, &nl, &c_b11, &c_b12, &u[nlf + u_dim1], ldu);
+ dlaset_("A", &nlp1, &nlp1, &c_b11, &c_b12, &vt[nlf + vt_dim1],
+ ldu);
+ dlasdq_("U", &sqrei, &nl, &nlp1, &nl, &ncc, &d__[nlf], &e[nlf], &
+ vt[nlf + vt_dim1], ldu, &u[nlf + u_dim1], ldu, &u[nlf +
+ u_dim1], ldu, &work[nwork1], info);
+ dcopy_(&nlp1, &vt[nlf + vt_dim1], &c__1, &work[vfi], &c__1);
+ dcopy_(&nlp1, &vt[nlf + nlp1 * vt_dim1], &c__1, &work[vli], &c__1)
+ ;
+ }
+ if (*info != 0) {
+ return 0;
+ }
+ i__2 = nl;
+ for (j = 1; j <= i__2; ++j) {
+ iwork[idxqi + j] = j;
+/* L10: */
+ }
+ if (i__ == nd && *sqre == 0) {
+ sqrei = 0;
+ } else {
+ sqrei = 1;
+ }
+ idxqi += nlp1;
+ vfi += nlp1;
+ vli += nlp1;
+ nrp1 = nr + sqrei;
+ if (*icompq == 0) {
+ dlaset_("A", &nrp1, &nrp1, &c_b11, &c_b12, &work[nwork1], &smlszp);
+ dlasdq_("U", &sqrei, &nr, &nrp1, &nru, &ncc, &d__[nrf], &e[nrf], &
+ work[nwork1], &smlszp, &work[nwork2], &nr, &work[nwork2],
+ &nr, &work[nwork2], info);
+ itemp = nwork1 + (nrp1 - 1) * smlszp;
+ dcopy_(&nrp1, &work[nwork1], &c__1, &work[vfi], &c__1);
+ dcopy_(&nrp1, &work[itemp], &c__1, &work[vli], &c__1);
+ } else {
+ dlaset_("A", &nr, &nr, &c_b11, &c_b12, &u[nrf + u_dim1], ldu);
+ dlaset_("A", &nrp1, &nrp1, &c_b11, &c_b12, &vt[nrf + vt_dim1],
+ ldu);
+ dlasdq_("U", &sqrei, &nr, &nrp1, &nr, &ncc, &d__[nrf], &e[nrf], &
+ vt[nrf + vt_dim1], ldu, &u[nrf + u_dim1], ldu, &u[nrf +
+ u_dim1], ldu, &work[nwork1], info);
+ dcopy_(&nrp1, &vt[nrf + vt_dim1], &c__1, &work[vfi], &c__1);
+ dcopy_(&nrp1, &vt[nrf + nrp1 * vt_dim1], &c__1, &work[vli], &c__1)
+ ;
+ }
+ if (*info != 0) {
+ return 0;
+ }
+ i__2 = nr;
+ for (j = 1; j <= i__2; ++j) {
+ iwork[idxqi + j] = j;
+/* L20: */
+ }
+/* L30: */
+ }
+
+/* Now conquer each subproblem bottom-up. */
+
+ j = pow_ii(&c__2, &nlvl);
+ for (lvl = nlvl; lvl >= 1; --lvl) {
+ lvl2 = (lvl << 1) - 1;
+
+/* Find the first node LF and last node LL on */
+/* the current level LVL. */
+
+ if (lvl == 1) {
+ lf = 1;
+ ll = 1;
+ } else {
+ i__1 = lvl - 1;
+ lf = pow_ii(&c__2, &i__1);
+ ll = (lf << 1) - 1;
+ }
+ i__1 = ll;
+ for (i__ = lf; i__ <= i__1; ++i__) {
+ im1 = i__ - 1;
+ ic = iwork[inode + im1];
+ nl = iwork[ndiml + im1];
+ nr = iwork[ndimr + im1];
+ nlf = ic - nl;
+ nrf = ic + 1;
+ if (i__ == ll) {
+ sqrei = *sqre;
+ } else {
+ sqrei = 1;
+ }
+ vfi = vf + nlf - 1;
+ vli = vl + nlf - 1;
+ idxqi = idxq + nlf - 1;
+ alpha = d__[ic];
+ beta = e[ic];
+ if (*icompq == 0) {
+ dlasd6_(icompq, &nl, &nr, &sqrei, &d__[nlf], &work[vfi], &
+ work[vli], &alpha, &beta, &iwork[idxqi], &perm[
+ perm_offset], &givptr[1], &givcol[givcol_offset],
+ ldgcol, &givnum[givnum_offset], ldu, &poles[
+ poles_offset], &difl[difl_offset], &difr[difr_offset],
+ &z__[z_offset], &k[1], &c__[1], &s[1], &work[nwork1],
+ &iwork[iwk], info);
+ } else {
+ --j;
+ dlasd6_(icompq, &nl, &nr, &sqrei, &d__[nlf], &work[vfi], &
+ work[vli], &alpha, &beta, &iwork[idxqi], &perm[nlf +
+ lvl * perm_dim1], &givptr[j], &givcol[nlf + lvl2 *
+ givcol_dim1], ldgcol, &givnum[nlf + lvl2 *
+ givnum_dim1], ldu, &poles[nlf + lvl2 * poles_dim1], &
+ difl[nlf + lvl * difl_dim1], &difr[nlf + lvl2 *
+ difr_dim1], &z__[nlf + lvl * z_dim1], &k[j], &c__[j],
+ &s[j], &work[nwork1], &iwork[iwk], info);
+ }
+ if (*info != 0) {
+ return 0;
+ }
+/* L40: */
+ }
+/* L50: */
+ }
+
+ return 0;
+
+/* End of DLASDA */
+
+} /* dlasda_ */
diff --git a/3rdparty/clapack/src/dlasdq.c b/3rdparty/clapack/src/dlasdq.c
new file mode 100644
index 0000000..581a872
--- /dev/null
+++ b/3rdparty/clapack/src/dlasdq.c
@@ -0,0 +1,380 @@
+/* dlasdq.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dlasdq_(char *uplo, integer *sqre, integer *n, integer *
+ ncvt, integer *nru, integer *ncc, doublereal *d__, doublereal *e,
+ doublereal *vt, integer *ldvt, doublereal *u, integer *ldu,
+ doublereal *c__, integer *ldc, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer c_dim1, c_offset, u_dim1, u_offset, vt_dim1, vt_offset, i__1,
+ i__2;
+
+ /* Local variables */
+ integer i__, j;
+ doublereal r__, cs, sn;
+ integer np1, isub;
+ doublereal smin;
+ integer sqre1;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dlasr_(char *, char *, char *, integer *,
+ integer *, doublereal *, doublereal *, doublereal *, integer *), dswap_(integer *, doublereal *, integer *
+, doublereal *, integer *);
+ integer iuplo;
+ extern /* Subroutine */ int dlartg_(doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *), xerbla_(char *,
+ integer *), dbdsqr_(char *, integer *, integer *, integer
+ *, integer *, doublereal *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *);
+ logical rotate;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASDQ computes the singular value decomposition (SVD) of a real */
+/* (upper or lower) bidiagonal matrix with diagonal D and offdiagonal */
+/* E, accumulating the transformations if desired. Letting B denote */
+/* the input bidiagonal matrix, the algorithm computes orthogonal */
+/* matrices Q and P such that B = Q * S * P' (P' denotes the transpose */
+/* of P). The singular values S are overwritten on D. */
+
+/* The input matrix U is changed to U * Q if desired. */
+/* The input matrix VT is changed to P' * VT if desired. */
+/* The input matrix C is changed to Q' * C if desired. */
+
+/* See "Computing Small Singular Values of Bidiagonal Matrices With */
+/* Guaranteed High Relative Accuracy," by J. Demmel and W. Kahan, */
+/* LAPACK Working Note #3, for a detailed description of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* On entry, UPLO specifies whether the input bidiagonal matrix */
+/* is upper or lower bidiagonal, and wether it is square are */
+/* not. */
+/* UPLO = 'U' or 'u' B is upper bidiagonal. */
+/* UPLO = 'L' or 'l' B is lower bidiagonal. */
+
+/* SQRE (input) INTEGER */
+/* = 0: then the input matrix is N-by-N. */
+/* = 1: then the input matrix is N-by-(N+1) if UPLU = 'U' and */
+/* (N+1)-by-N if UPLU = 'L'. */
+
+/* The bidiagonal matrix has */
+/* N = NL + NR + 1 rows and */
+/* M = N + SQRE >= N columns. */
+
+/* N (input) INTEGER */
+/* On entry, N specifies the number of rows and columns */
+/* in the matrix. N must be at least 0. */
+
+/* NCVT (input) INTEGER */
+/* On entry, NCVT specifies the number of columns of */
+/* the matrix VT. NCVT must be at least 0. */
+
+/* NRU (input) INTEGER */
+/* On entry, NRU specifies the number of rows of */
+/* the matrix U. NRU must be at least 0. */
+
+/* NCC (input) INTEGER */
+/* On entry, NCC specifies the number of columns of */
+/* the matrix C. NCC must be at least 0. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, D contains the diagonal entries of the */
+/* bidiagonal matrix whose SVD is desired. On normal exit, */
+/* D contains the singular values in ascending order. */
+
+/* E (input/output) DOUBLE PRECISION array. */
+/* dimension is (N-1) if SQRE = 0 and N if SQRE = 1. */
+/* On entry, the entries of E contain the offdiagonal entries */
+/* of the bidiagonal matrix whose SVD is desired. On normal */
+/* exit, E will contain 0. If the algorithm does not converge, */
+/* D and E will contain the diagonal and superdiagonal entries */
+/* of a bidiagonal matrix orthogonally equivalent to the one */
+/* given as input. */
+
+/* VT (input/output) DOUBLE PRECISION array, dimension (LDVT, NCVT) */
+/* On entry, contains a matrix which on exit has been */
+/* premultiplied by P', dimension N-by-NCVT if SQRE = 0 */
+/* and (N+1)-by-NCVT if SQRE = 1 (not referenced if NCVT=0). */
+
+/* LDVT (input) INTEGER */
+/* On entry, LDVT specifies the leading dimension of VT as */
+/* declared in the calling (sub) program. LDVT must be at */
+/* least 1. If NCVT is nonzero LDVT must also be at least N. */
+
+/* U (input/output) DOUBLE PRECISION array, dimension (LDU, N) */
+/* On entry, contains a matrix which on exit has been */
+/* postmultiplied by Q, dimension NRU-by-N if SQRE = 0 */
+/* and NRU-by-(N+1) if SQRE = 1 (not referenced if NRU=0). */
+
+/* LDU (input) INTEGER */
+/* On entry, LDU specifies the leading dimension of U as */
+/* declared in the calling (sub) program. LDU must be at */
+/* least max( 1, NRU ) . */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC, NCC) */
+/* On entry, contains an N-by-NCC matrix which on exit */
+/* has been premultiplied by Q' dimension N-by-NCC if SQRE = 0 */
+/* and (N+1)-by-NCC if SQRE = 1 (not referenced if NCC=0). */
+
+/* LDC (input) INTEGER */
+/* On entry, LDC specifies the leading dimension of C as */
+/* declared in the calling (sub) program. LDC must be at */
+/* least 1. If NCC is nonzero, LDC must also be at least N. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (4*N) */
+/* Workspace. Only referenced if one of NCVT, NRU, or NCC is */
+/* nonzero, and if N is at least 2. */
+
+/* INFO (output) INTEGER */
+/* On exit, a value of 0 indicates a successful exit. */
+/* If INFO < 0, argument number -INFO is illegal. */
+/* If INFO > 0, the algorithm did not converge, and INFO */
+/* specifies how many superdiagonals did not converge. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ --d__;
+ --e;
+ vt_dim1 = *ldvt;
+ vt_offset = 1 + vt_dim1;
+ vt -= vt_offset;
+ u_dim1 = *ldu;
+ u_offset = 1 + u_dim1;
+ u -= u_offset;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ iuplo = 0;
+ if (lsame_(uplo, "U")) {
+ iuplo = 1;
+ }
+ if (lsame_(uplo, "L")) {
+ iuplo = 2;
+ }
+ if (iuplo == 0) {
+ *info = -1;
+ } else if (*sqre < 0 || *sqre > 1) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*ncvt < 0) {
+ *info = -4;
+ } else if (*nru < 0) {
+ *info = -5;
+ } else if (*ncc < 0) {
+ *info = -6;
+ } else if (*ncvt == 0 && *ldvt < 1 || *ncvt > 0 && *ldvt < max(1,*n)) {
+ *info = -10;
+ } else if (*ldu < max(1,*nru)) {
+ *info = -12;
+ } else if (*ncc == 0 && *ldc < 1 || *ncc > 0 && *ldc < max(1,*n)) {
+ *info = -14;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASDQ", &i__1);
+ return 0;
+ }
+ if (*n == 0) {
+ return 0;
+ }
+
+/* ROTATE is true if any singular vectors desired, false otherwise */
+
+ rotate = *ncvt > 0 || *nru > 0 || *ncc > 0;
+ np1 = *n + 1;
+ sqre1 = *sqre;
+
+/* If matrix non-square upper bidiagonal, rotate to be lower */
+/* bidiagonal. The rotations are on the right. */
+
+ if (iuplo == 1 && sqre1 == 1) {
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dlartg_(&d__[i__], &e[i__], &cs, &sn, &r__);
+ d__[i__] = r__;
+ e[i__] = sn * d__[i__ + 1];
+ d__[i__ + 1] = cs * d__[i__ + 1];
+ if (rotate) {
+ work[i__] = cs;
+ work[*n + i__] = sn;
+ }
+/* L10: */
+ }
+ dlartg_(&d__[*n], &e[*n], &cs, &sn, &r__);
+ d__[*n] = r__;
+ e[*n] = 0.;
+ if (rotate) {
+ work[*n] = cs;
+ work[*n + *n] = sn;
+ }
+ iuplo = 2;
+ sqre1 = 0;
+
+/* Update singular vectors if desired. */
+
+ if (*ncvt > 0) {
+ dlasr_("L", "V", "F", &np1, ncvt, &work[1], &work[np1], &vt[
+ vt_offset], ldvt);
+ }
+ }
+
+/* If matrix lower bidiagonal, rotate to be upper bidiagonal */
+/* by applying Givens rotations on the left. */
+
+ if (iuplo == 2) {
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ dlartg_(&d__[i__], &e[i__], &cs, &sn, &r__);
+ d__[i__] = r__;
+ e[i__] = sn * d__[i__ + 1];
+ d__[i__ + 1] = cs * d__[i__ + 1];
+ if (rotate) {
+ work[i__] = cs;
+ work[*n + i__] = sn;
+ }
+/* L20: */
+ }
+
+/* If matrix (N+1)-by-N lower bidiagonal, one additional */
+/* rotation is needed. */
+
+ if (sqre1 == 1) {
+ dlartg_(&d__[*n], &e[*n], &cs, &sn, &r__);
+ d__[*n] = r__;
+ if (rotate) {
+ work[*n] = cs;
+ work[*n + *n] = sn;
+ }
+ }
+
+/* Update singular vectors if desired. */
+
+ if (*nru > 0) {
+ if (sqre1 == 0) {
+ dlasr_("R", "V", "F", nru, n, &work[1], &work[np1], &u[
+ u_offset], ldu);
+ } else {
+ dlasr_("R", "V", "F", nru, &np1, &work[1], &work[np1], &u[
+ u_offset], ldu);
+ }
+ }
+ if (*ncc > 0) {
+ if (sqre1 == 0) {
+ dlasr_("L", "V", "F", n, ncc, &work[1], &work[np1], &c__[
+ c_offset], ldc);
+ } else {
+ dlasr_("L", "V", "F", &np1, ncc, &work[1], &work[np1], &c__[
+ c_offset], ldc);
+ }
+ }
+ }
+
+/* Call DBDSQR to compute the SVD of the reduced real */
+/* N-by-N upper bidiagonal matrix. */
+
+ dbdsqr_("U", n, ncvt, nru, ncc, &d__[1], &e[1], &vt[vt_offset], ldvt, &u[
+ u_offset], ldu, &c__[c_offset], ldc, &work[1], info);
+
+/* Sort the singular values into ascending order (insertion sort on */
+/* singular values, but only one transposition per singular vector) */
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+
+/* Scan for smallest D(I). */
+
+ isub = i__;
+ smin = d__[i__];
+ i__2 = *n;
+ for (j = i__ + 1; j <= i__2; ++j) {
+ if (d__[j] < smin) {
+ isub = j;
+ smin = d__[j];
+ }
+/* L30: */
+ }
+ if (isub != i__) {
+
+/* Swap singular values and vectors. */
+
+ d__[isub] = d__[i__];
+ d__[i__] = smin;
+ if (*ncvt > 0) {
+ dswap_(ncvt, &vt[isub + vt_dim1], ldvt, &vt[i__ + vt_dim1],
+ ldvt);
+ }
+ if (*nru > 0) {
+ dswap_(nru, &u[isub * u_dim1 + 1], &c__1, &u[i__ * u_dim1 + 1]
+, &c__1);
+ }
+ if (*ncc > 0) {
+ dswap_(ncc, &c__[isub + c_dim1], ldc, &c__[i__ + c_dim1], ldc)
+ ;
+ }
+ }
+/* L40: */
+ }
+
+ return 0;
+
+/* End of DLASDQ */
+
+} /* dlasdq_ */
diff --git a/3rdparty/clapack/src/dlasdt.c b/3rdparty/clapack/src/dlasdt.c
new file mode 100644
index 0000000..0f25bdd
--- /dev/null
+++ b/3rdparty/clapack/src/dlasdt.c
@@ -0,0 +1,136 @@
+/* dlasdt.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasdt_(integer *n, integer *lvl, integer *nd, integer *
+ inode, integer *ndiml, integer *ndimr, integer *msub)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+
+ /* Builtin functions */
+ double log(doublereal);
+
+ /* Local variables */
+ integer i__, il, ir, maxn;
+ doublereal temp;
+ integer nlvl, llst, ncrnt;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASDT creates a tree of subproblems for bidiagonal divide and */
+/* conquer. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* On entry, the number of diagonal elements of the */
+/* bidiagonal matrix. */
+
+/* LVL (output) INTEGER */
+/* On exit, the number of levels on the computation tree. */
+
+/* ND (output) INTEGER */
+/* On exit, the number of nodes on the tree. */
+
+/* INODE (output) INTEGER array, dimension ( N ) */
+/* On exit, centers of subproblems. */
+
+/* NDIML (output) INTEGER array, dimension ( N ) */
+/* On exit, row dimensions of left children. */
+
+/* NDIMR (output) INTEGER array, dimension ( N ) */
+/* On exit, row dimensions of right children. */
+
+/* MSUB (input) INTEGER. */
+/* On entry, the maximum row dimension each subproblem at the */
+/* bottom of the tree can be of. */
+
+/* Further Details */
+/* =============== */
+
+/* Based on contributions by */
+/* Ming Gu and Huan Ren, Computer Science Division, University of */
+/* California at Berkeley, USA */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Find the number of levels on the tree. */
+
+ /* Parameter adjustments */
+ --ndimr;
+ --ndiml;
+ --inode;
+
+ /* Function Body */
+ maxn = max(1,*n);
+ temp = log((doublereal) maxn / (doublereal) (*msub + 1)) / log(2.);
+ *lvl = (integer) temp + 1;
+
+ i__ = *n / 2;
+ inode[1] = i__ + 1;
+ ndiml[1] = i__;
+ ndimr[1] = *n - i__ - 1;
+ il = 0;
+ ir = 1;
+ llst = 1;
+ i__1 = *lvl - 1;
+ for (nlvl = 1; nlvl <= i__1; ++nlvl) {
+
+/* Constructing the tree at (NLVL+1)-st level. The number of */
+/* nodes created on this level is LLST * 2. */
+
+ i__2 = llst - 1;
+ for (i__ = 0; i__ <= i__2; ++i__) {
+ il += 2;
+ ir += 2;
+ ncrnt = llst + i__;
+ ndiml[il] = ndiml[ncrnt] / 2;
+ ndimr[il] = ndiml[ncrnt] - ndiml[il] - 1;
+ inode[il] = inode[ncrnt] - ndimr[il] - 1;
+ ndiml[ir] = ndimr[ncrnt] / 2;
+ ndimr[ir] = ndimr[ncrnt] - ndiml[ir] - 1;
+ inode[ir] = inode[ncrnt] + ndiml[ir] + 1;
+/* L10: */
+ }
+ llst <<= 1;
+/* L20: */
+ }
+ *nd = (llst << 1) - 1;
+
+ return 0;
+
+/* End of DLASDT */
+
+} /* dlasdt_ */
diff --git a/3rdparty/clapack/src/dlaset.c b/3rdparty/clapack/src/dlaset.c
new file mode 100644
index 0000000..98d304e
--- /dev/null
+++ b/3rdparty/clapack/src/dlaset.c
@@ -0,0 +1,152 @@
+/* dlaset.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlaset_(char *uplo, integer *m, integer *n, doublereal *
+ alpha, doublereal *beta, doublereal *a, integer *lda)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j;
+ extern logical lsame_(char *, char *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASET initializes an m-by-n matrix A to BETA on the diagonal and */
+/* ALPHA on the offdiagonals. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies the part of the matrix A to be set. */
+/* = 'U': Upper triangular part is set; the strictly lower */
+/* triangular part of A is not changed. */
+/* = 'L': Lower triangular part is set; the strictly upper */
+/* triangular part of A is not changed. */
+/* Otherwise: All of the matrix A is set. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. N >= 0. */
+
+/* ALPHA (input) DOUBLE PRECISION */
+/* The constant to which the offdiagonal elements are to be set. */
+
+/* BETA (input) DOUBLE PRECISION */
+/* The constant to which the diagonal elements are to be set. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On exit, the leading m-by-n submatrix of A is set as follows: */
+
+/* if UPLO = 'U', A(i,j) = ALPHA, 1<=i<=j-1, 1<=j<=n, */
+/* if UPLO = 'L', A(i,j) = ALPHA, j+1<=i<=m, 1<=j<=n, */
+/* otherwise, A(i,j) = ALPHA, 1<=i<=m, 1<=j<=n, i.ne.j, */
+
+/* and, for all UPLO, A(i,i) = BETA, 1<=i<=min(m,n). */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ if (lsame_(uplo, "U")) {
+
+/* Set the strictly upper triangular or trapezoidal part of the */
+/* array to ALPHA. */
+
+ i__1 = *n;
+ for (j = 2; j <= i__1; ++j) {
+/* Computing MIN */
+ i__3 = j - 1;
+ i__2 = min(i__3,*m);
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] = *alpha;
+/* L10: */
+ }
+/* L20: */
+ }
+
+ } else if (lsame_(uplo, "L")) {
+
+/* Set the strictly lower triangular or trapezoidal part of the */
+/* array to ALPHA. */
+
+ i__1 = min(*m,*n);
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = j + 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] = *alpha;
+/* L30: */
+ }
+/* L40: */
+ }
+
+ } else {
+
+/* Set the leading m-by-n submatrix to ALPHA. */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] = *alpha;
+/* L50: */
+ }
+/* L60: */
+ }
+ }
+
+/* Set the first min(M,N) diagonal elements to BETA. */
+
+ i__1 = min(*m,*n);
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ a[i__ + i__ * a_dim1] = *beta;
+/* L70: */
+ }
+
+ return 0;
+
+/* End of DLASET */
+
+} /* dlaset_ */
diff --git a/3rdparty/clapack/src/dlasq1.c b/3rdparty/clapack/src/dlasq1.c
new file mode 100644
index 0000000..7a6e8b7
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq1.c
@@ -0,0 +1,219 @@
+/* dlasq1.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c__2 = 2;
+static integer c__0 = 0;
+
+/* Subroutine */ int dlasq1_(integer *n, doublereal *d__, doublereal *e,
+ doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+ doublereal d__1, d__2, d__3;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer i__;
+ doublereal eps;
+ extern /* Subroutine */ int dlas2_(doublereal *, doublereal *, doublereal
+ *, doublereal *, doublereal *);
+ doublereal scale;
+ integer iinfo;
+ doublereal sigmn;
+ extern /* Subroutine */ int dcopy_(integer *, doublereal *, integer *,
+ doublereal *, integer *);
+ doublereal sigmx;
+ extern /* Subroutine */ int dlasq2_(integer *, doublereal *, integer *);
+ extern doublereal dlamch_(char *);
+ extern /* Subroutine */ int dlascl_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, integer *, doublereal *,
+ integer *, integer *);
+ doublereal safmin;
+ extern /* Subroutine */ int xerbla_(char *, integer *), dlasrt_(
+ char *, integer *, doublereal *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ1 computes the singular values of a real N-by-N bidiagonal */
+/* matrix with diagonal D and off-diagonal E. The singular values */
+/* are computed to high relative accuracy, in the absence of */
+/* denormalization, underflow and overflow. The algorithm was first */
+/* presented in */
+
+/* "Accurate singular values and differential qd algorithms" by K. V. */
+/* Fernando and B. N. Parlett, Numer. Math., Vol-67, No. 2, pp. 191-230, */
+/* 1994, */
+
+/* and the present implementation is described in "An implementation of */
+/* the dqds Algorithm (Positive Case)", LAPACK Working Note. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The number of rows and columns in the matrix. N >= 0. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, D contains the diagonal elements of the */
+/* bidiagonal matrix whose SVD is desired. On normal exit, */
+/* D contains the singular values in decreasing order. */
+
+/* E (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, elements E(1:N-1) contain the off-diagonal elements */
+/* of the bidiagonal matrix whose SVD is desired. */
+/* On exit, E is overwritten. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (4*N) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: the algorithm failed */
+/* = 1, a split was marked by a positive value in E */
+/* = 2, current block of Z not diagonalized after 30*N */
+/* iterations (in inner while loop) */
+/* = 3, termination criterion of outer while loop not met */
+/* (program created more than N unreduced blocks) */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --work;
+ --e;
+ --d__;
+
+ /* Function Body */
+ *info = 0;
+ if (*n < 0) {
+ *info = -2;
+ i__1 = -(*info);
+ xerbla_("DLASQ1", &i__1);
+ return 0;
+ } else if (*n == 0) {
+ return 0;
+ } else if (*n == 1) {
+ d__[1] = abs(d__[1]);
+ return 0;
+ } else if (*n == 2) {
+ dlas2_(&d__[1], &e[1], &d__[2], &sigmn, &sigmx);
+ d__[1] = sigmx;
+ d__[2] = sigmn;
+ return 0;
+ }
+
+/* Estimate the largest singular value. */
+
+ sigmx = 0.;
+ i__1 = *n - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__[i__] = (d__1 = d__[i__], abs(d__1));
+/* Computing MAX */
+ d__2 = sigmx, d__3 = (d__1 = e[i__], abs(d__1));
+ sigmx = max(d__2,d__3);
+/* L10: */
+ }
+ d__[*n] = (d__1 = d__[*n], abs(d__1));
+
+/* Early return if SIGMX is zero (matrix is already diagonal). */
+
+ if (sigmx == 0.) {
+ dlasrt_("D", n, &d__[1], &iinfo);
+ return 0;
+ }
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing MAX */
+ d__1 = sigmx, d__2 = d__[i__];
+ sigmx = max(d__1,d__2);
+/* L20: */
+ }
+
+/* Copy D and E into WORK (in the Z format) and scale (squaring the */
+/* input data makes scaling by a power of the radix pointless). */
+
+ eps = dlamch_("Precision");
+ safmin = dlamch_("Safe minimum");
+ scale = sqrt(eps / safmin);
+ dcopy_(n, &d__[1], &c__1, &work[1], &c__2);
+ i__1 = *n - 1;
+ dcopy_(&i__1, &e[1], &c__1, &work[2], &c__2);
+ i__1 = (*n << 1) - 1;
+ i__2 = (*n << 1) - 1;
+ dlascl_("G", &c__0, &c__0, &sigmx, &scale, &i__1, &c__1, &work[1], &i__2,
+ &iinfo);
+
+/* Compute the q's and e's. */
+
+ i__1 = (*n << 1) - 1;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+/* Computing 2nd power */
+ d__1 = work[i__];
+ work[i__] = d__1 * d__1;
+/* L30: */
+ }
+ work[*n * 2] = 0.;
+
+ dlasq2_(n, &work[1], info);
+
+ if (*info == 0) {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ d__[i__] = sqrt(work[i__]);
+/* L40: */
+ }
+ dlascl_("G", &c__0, &c__0, &scale, &sigmx, n, &c__1, &d__[1], n, &
+ iinfo);
+ }
+
+ return 0;
+
+/* End of DLASQ1 */
+
+} /* dlasq1_ */
diff --git a/3rdparty/clapack/src/dlasq2.c b/3rdparty/clapack/src/dlasq2.c
new file mode 100644
index 0000000..3b041a2
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq2.c
@@ -0,0 +1,602 @@
+/* dlasq2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c__2 = 2;
+static integer c__10 = 10;
+static integer c__3 = 3;
+static integer c__4 = 4;
+static integer c__11 = 11;
+
+/* Subroutine */ int dlasq2_(integer *n, doublereal *z__, integer *info)
+{
+ /* System generated locals */
+ integer i__1, i__2, i__3;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal d__, e, g;
+ integer k;
+ doublereal s, t;
+ integer i0, i4, n0;
+ doublereal dn;
+ integer pp;
+ doublereal dn1, dn2, dee, eps, tau, tol;
+ integer ipn4;
+ doublereal tol2;
+ logical ieee;
+ integer nbig;
+ doublereal dmin__, emin, emax;
+ integer kmin, ndiv, iter;
+ doublereal qmin, temp, qmax, zmax;
+ integer splt;
+ doublereal dmin1, dmin2;
+ integer nfail;
+ doublereal desig, trace, sigma;
+ integer iinfo, ttype;
+ extern /* Subroutine */ int dlasq3_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ integer *, integer *, integer *, logical *, integer *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *);
+ extern doublereal dlamch_(char *);
+ doublereal deemin;
+ integer iwhila, iwhilb;
+ doublereal oldemn, safmin;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ extern /* Subroutine */ int dlasrt_(char *, integer *, doublereal *,
+ integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ2 computes all the eigenvalues of the symmetric positive */
+/* definite tridiagonal matrix associated with the qd array Z to high */
+/* relative accuracy are computed to high relative accuracy, in the */
+/* absence of denormalization, underflow and overflow. */
+
+/* To see the relation of Z to the tridiagonal matrix, let L be a */
+/* unit lower bidiagonal matrix with subdiagonals Z(2,4,6,,..) and */
+/* let U be an upper bidiagonal matrix with 1's above and diagonal */
+/* Z(1,3,5,,..). The tridiagonal is L*U or, if you prefer, the */
+/* symmetric tridiagonal to which it is similar. */
+
+/* Note : DLASQ2 defines a logical variable, IEEE, which is true */
+/* on machines which follow ieee-754 floating-point standard in their */
+/* handling of infinities and NaNs, and false otherwise. This variable */
+/* is passed to DLASQ3. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The number of rows and columns in the matrix. N >= 0. */
+
+/* Z (input/output) DOUBLE PRECISION array, dimension ( 4*N ) */
+/* On entry Z holds the qd array. On exit, entries 1 to N hold */
+/* the eigenvalues in decreasing order, Z( 2*N+1 ) holds the */
+/* trace, and Z( 2*N+2 ) holds the sum of the eigenvalues. If */
+/* N > 2, then Z( 2*N+3 ) holds the iteration count, Z( 2*N+4 ) */
+/* holds NDIVS/NIN^2, and Z( 2*N+5 ) holds the percentage of */
+/* shifts that failed. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if the i-th argument is a scalar and had an illegal */
+/* value, then INFO = -i, if the i-th argument is an */
+/* array and the j-entry had an illegal value, then */
+/* INFO = -(i*100+j) */
+/* > 0: the algorithm failed */
+/* = 1, a split was marked by a positive value in E */
+/* = 2, current block of Z not diagonalized after 30*N */
+/* iterations (in inner while loop) */
+/* = 3, termination criterion of outer while loop not met */
+/* (program created more than N unreduced blocks) */
+
+/* Further Details */
+/* =============== */
+/* Local Variables: I0:N0 defines a current unreduced segment of Z. */
+/* The shifts are accumulated in SIGMA. Iteration count is in ITER. */
+/* Ping-pong is controlled by PP (alternates between 0 and 1). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments. */
+/* (in case DLASQ2 is not called by DLASQ1) */
+
+ /* Parameter adjustments */
+ --z__;
+
+ /* Function Body */
+ *info = 0;
+ eps = dlamch_("Precision");
+ safmin = dlamch_("Safe minimum");
+ tol = eps * 100.;
+/* Computing 2nd power */
+ d__1 = tol;
+ tol2 = d__1 * d__1;
+
+ if (*n < 0) {
+ *info = -1;
+ xerbla_("DLASQ2", &c__1);
+ return 0;
+ } else if (*n == 0) {
+ return 0;
+ } else if (*n == 1) {
+
+/* 1-by-1 case. */
+
+ if (z__[1] < 0.) {
+ *info = -201;
+ xerbla_("DLASQ2", &c__2);
+ }
+ return 0;
+ } else if (*n == 2) {
+
+/* 2-by-2 case. */
+
+ if (z__[2] < 0. || z__[3] < 0.) {
+ *info = -2;
+ xerbla_("DLASQ2", &c__2);
+ return 0;
+ } else if (z__[3] > z__[1]) {
+ d__ = z__[3];
+ z__[3] = z__[1];
+ z__[1] = d__;
+ }
+ z__[5] = z__[1] + z__[2] + z__[3];
+ if (z__[2] > z__[3] * tol2) {
+ t = (z__[1] - z__[3] + z__[2]) * .5;
+ s = z__[3] * (z__[2] / t);
+ if (s <= t) {
+ s = z__[3] * (z__[2] / (t * (sqrt(s / t + 1.) + 1.)));
+ } else {
+ s = z__[3] * (z__[2] / (t + sqrt(t) * sqrt(t + s)));
+ }
+ t = z__[1] + (s + z__[2]);
+ z__[3] *= z__[1] / t;
+ z__[1] = t;
+ }
+ z__[2] = z__[3];
+ z__[6] = z__[2] + z__[1];
+ return 0;
+ }
+
+/* Check for negative data and compute sums of q's and e's. */
+
+ z__[*n * 2] = 0.;
+ emin = z__[2];
+ qmax = 0.;
+ zmax = 0.;
+ d__ = 0.;
+ e = 0.;
+
+ i__1 = *n - 1 << 1;
+ for (k = 1; k <= i__1; k += 2) {
+ if (z__[k] < 0.) {
+ *info = -(k + 200);
+ xerbla_("DLASQ2", &c__2);
+ return 0;
+ } else if (z__[k + 1] < 0.) {
+ *info = -(k + 201);
+ xerbla_("DLASQ2", &c__2);
+ return 0;
+ }
+ d__ += z__[k];
+ e += z__[k + 1];
+/* Computing MAX */
+ d__1 = qmax, d__2 = z__[k];
+ qmax = max(d__1,d__2);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[k + 1];
+ emin = min(d__1,d__2);
+/* Computing MAX */
+ d__1 = max(qmax,zmax), d__2 = z__[k + 1];
+ zmax = max(d__1,d__2);
+/* L10: */
+ }
+ if (z__[(*n << 1) - 1] < 0.) {
+ *info = -((*n << 1) + 199);
+ xerbla_("DLASQ2", &c__2);
+ return 0;
+ }
+ d__ += z__[(*n << 1) - 1];
+/* Computing MAX */
+ d__1 = qmax, d__2 = z__[(*n << 1) - 1];
+ qmax = max(d__1,d__2);
+ zmax = max(qmax,zmax);
+
+/* Check for diagonality. */
+
+ if (e == 0.) {
+ i__1 = *n;
+ for (k = 2; k <= i__1; ++k) {
+ z__[k] = z__[(k << 1) - 1];
+/* L20: */
+ }
+ dlasrt_("D", n, &z__[1], &iinfo);
+ z__[(*n << 1) - 1] = d__;
+ return 0;
+ }
+
+ trace = d__ + e;
+
+/* Check for zero data. */
+
+ if (trace == 0.) {
+ z__[(*n << 1) - 1] = 0.;
+ return 0;
+ }
+
+/* Check whether the machine is IEEE conformable. */
+
+ ieee = ilaenv_(&c__10, "DLASQ2", "N", &c__1, &c__2, &c__3, &c__4) == 1 && ilaenv_(&c__11, "DLASQ2", "N", &c__1, &c__2,
+ &c__3, &c__4) == 1;
+
+/* Rearrange data for locality: Z=(q1,qq1,e1,ee1,q2,qq2,e2,ee2,...). */
+
+ for (k = *n << 1; k >= 2; k += -2) {
+ z__[k * 2] = 0.;
+ z__[(k << 1) - 1] = z__[k];
+ z__[(k << 1) - 2] = 0.;
+ z__[(k << 1) - 3] = z__[k - 1];
+/* L30: */
+ }
+
+ i0 = 1;
+ n0 = *n;
+
+/* Reverse the qd-array, if warranted. */
+
+ if (z__[(i0 << 2) - 3] * 1.5 < z__[(n0 << 2) - 3]) {
+ ipn4 = i0 + n0 << 2;
+ i__1 = i0 + n0 - 1 << 1;
+ for (i4 = i0 << 2; i4 <= i__1; i4 += 4) {
+ temp = z__[i4 - 3];
+ z__[i4 - 3] = z__[ipn4 - i4 - 3];
+ z__[ipn4 - i4 - 3] = temp;
+ temp = z__[i4 - 1];
+ z__[i4 - 1] = z__[ipn4 - i4 - 5];
+ z__[ipn4 - i4 - 5] = temp;
+/* L40: */
+ }
+ }
+
+/* Initial split checking via dqd and Li's test. */
+
+ pp = 0;
+
+ for (k = 1; k <= 2; ++k) {
+
+ d__ = z__[(n0 << 2) + pp - 3];
+ i__1 = (i0 << 2) + pp;
+ for (i4 = (n0 - 1 << 2) + pp; i4 >= i__1; i4 += -4) {
+ if (z__[i4 - 1] <= tol2 * d__) {
+ z__[i4 - 1] = -0.;
+ d__ = z__[i4 - 3];
+ } else {
+ d__ = z__[i4 - 3] * (d__ / (d__ + z__[i4 - 1]));
+ }
+/* L50: */
+ }
+
+/* dqd maps Z to ZZ plus Li's test. */
+
+ emin = z__[(i0 << 2) + pp + 1];
+ d__ = z__[(i0 << 2) + pp - 3];
+ i__1 = (n0 - 1 << 2) + pp;
+ for (i4 = (i0 << 2) + pp; i4 <= i__1; i4 += 4) {
+ z__[i4 - (pp << 1) - 2] = d__ + z__[i4 - 1];
+ if (z__[i4 - 1] <= tol2 * d__) {
+ z__[i4 - 1] = -0.;
+ z__[i4 - (pp << 1) - 2] = d__;
+ z__[i4 - (pp << 1)] = 0.;
+ d__ = z__[i4 + 1];
+ } else if (safmin * z__[i4 + 1] < z__[i4 - (pp << 1) - 2] &&
+ safmin * z__[i4 - (pp << 1) - 2] < z__[i4 + 1]) {
+ temp = z__[i4 + 1] / z__[i4 - (pp << 1) - 2];
+ z__[i4 - (pp << 1)] = z__[i4 - 1] * temp;
+ d__ *= temp;
+ } else {
+ z__[i4 - (pp << 1)] = z__[i4 + 1] * (z__[i4 - 1] / z__[i4 - (
+ pp << 1) - 2]);
+ d__ = z__[i4 + 1] * (d__ / z__[i4 - (pp << 1) - 2]);
+ }
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[i4 - (pp << 1)];
+ emin = min(d__1,d__2);
+/* L60: */
+ }
+ z__[(n0 << 2) - pp - 2] = d__;
+
+/* Now find qmax. */
+
+ qmax = z__[(i0 << 2) - pp - 2];
+ i__1 = (n0 << 2) - pp - 2;
+ for (i4 = (i0 << 2) - pp + 2; i4 <= i__1; i4 += 4) {
+/* Computing MAX */
+ d__1 = qmax, d__2 = z__[i4];
+ qmax = max(d__1,d__2);
+/* L70: */
+ }
+
+/* Prepare for the next iteration on K. */
+
+ pp = 1 - pp;
+/* L80: */
+ }
+
+/* Initialise variables to pass to DLASQ3. */
+
+ ttype = 0;
+ dmin1 = 0.;
+ dmin2 = 0.;
+ dn = 0.;
+ dn1 = 0.;
+ dn2 = 0.;
+ g = 0.;
+ tau = 0.;
+
+ iter = 2;
+ nfail = 0;
+ ndiv = n0 - i0 << 1;
+
+ i__1 = *n + 1;
+ for (iwhila = 1; iwhila <= i__1; ++iwhila) {
+ if (n0 < 1) {
+ goto L170;
+ }
+
+/* While array unfinished do */
+
+/* E(N0) holds the value of SIGMA when submatrix in I0:N0 */
+/* splits from the rest of the array, but is negated. */
+
+ desig = 0.;
+ if (n0 == *n) {
+ sigma = 0.;
+ } else {
+ sigma = -z__[(n0 << 2) - 1];
+ }
+ if (sigma < 0.) {
+ *info = 1;
+ return 0;
+ }
+
+/* Find last unreduced submatrix's top index I0, find QMAX and */
+/* EMIN. Find Gershgorin-type bound if Q's much greater than E's. */
+
+ emax = 0.;
+ if (n0 > i0) {
+ emin = (d__1 = z__[(n0 << 2) - 5], abs(d__1));
+ } else {
+ emin = 0.;
+ }
+ qmin = z__[(n0 << 2) - 3];
+ qmax = qmin;
+ for (i4 = n0 << 2; i4 >= 8; i4 += -4) {
+ if (z__[i4 - 5] <= 0.) {
+ goto L100;
+ }
+ if (qmin >= emax * 4.) {
+/* Computing MIN */
+ d__1 = qmin, d__2 = z__[i4 - 3];
+ qmin = min(d__1,d__2);
+/* Computing MAX */
+ d__1 = emax, d__2 = z__[i4 - 5];
+ emax = max(d__1,d__2);
+ }
+/* Computing MAX */
+ d__1 = qmax, d__2 = z__[i4 - 7] + z__[i4 - 5];
+ qmax = max(d__1,d__2);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[i4 - 5];
+ emin = min(d__1,d__2);
+/* L90: */
+ }
+ i4 = 4;
+
+L100:
+ i0 = i4 / 4;
+ pp = 0;
+
+ if (n0 - i0 > 1) {
+ dee = z__[(i0 << 2) - 3];
+ deemin = dee;
+ kmin = i0;
+ i__2 = (n0 << 2) - 3;
+ for (i4 = (i0 << 2) + 1; i4 <= i__2; i4 += 4) {
+ dee = z__[i4] * (dee / (dee + z__[i4 - 2]));
+ if (dee <= deemin) {
+ deemin = dee;
+ kmin = (i4 + 3) / 4;
+ }
+/* L110: */
+ }
+ if (kmin - i0 << 1 < n0 - kmin && deemin <= z__[(n0 << 2) - 3] *
+ .5) {
+ ipn4 = i0 + n0 << 2;
+ pp = 2;
+ i__2 = i0 + n0 - 1 << 1;
+ for (i4 = i0 << 2; i4 <= i__2; i4 += 4) {
+ temp = z__[i4 - 3];
+ z__[i4 - 3] = z__[ipn4 - i4 - 3];
+ z__[ipn4 - i4 - 3] = temp;
+ temp = z__[i4 - 2];
+ z__[i4 - 2] = z__[ipn4 - i4 - 2];
+ z__[ipn4 - i4 - 2] = temp;
+ temp = z__[i4 - 1];
+ z__[i4 - 1] = z__[ipn4 - i4 - 5];
+ z__[ipn4 - i4 - 5] = temp;
+ temp = z__[i4];
+ z__[i4] = z__[ipn4 - i4 - 4];
+ z__[ipn4 - i4 - 4] = temp;
+/* L120: */
+ }
+ }
+ }
+
+/* Put -(initial shift) into DMIN. */
+
+/* Computing MAX */
+ d__1 = 0., d__2 = qmin - sqrt(qmin) * 2. * sqrt(emax);
+ dmin__ = -max(d__1,d__2);
+
+/* Now I0:N0 is unreduced. */
+/* PP = 0 for ping, PP = 1 for pong. */
+/* PP = 2 indicates that flipping was applied to the Z array and */
+/* and that the tests for deflation upon entry in DLASQ3 */
+/* should not be performed. */
+
+ nbig = (n0 - i0 + 1) * 30;
+ i__2 = nbig;
+ for (iwhilb = 1; iwhilb <= i__2; ++iwhilb) {
+ if (i0 > n0) {
+ goto L150;
+ }
+
+/* While submatrix unfinished take a good dqds step. */
+
+ dlasq3_(&i0, &n0, &z__[1], &pp, &dmin__, &sigma, &desig, &qmax, &
+ nfail, &iter, &ndiv, &ieee, &ttype, &dmin1, &dmin2, &dn, &
+ dn1, &dn2, &g, &tau);
+
+ pp = 1 - pp;
+
+/* When EMIN is very small check for splits. */
+
+ if (pp == 0 && n0 - i0 >= 3) {
+ if (z__[n0 * 4] <= tol2 * qmax || z__[(n0 << 2) - 1] <= tol2 *
+ sigma) {
+ splt = i0 - 1;
+ qmax = z__[(i0 << 2) - 3];
+ emin = z__[(i0 << 2) - 1];
+ oldemn = z__[i0 * 4];
+ i__3 = n0 - 3 << 2;
+ for (i4 = i0 << 2; i4 <= i__3; i4 += 4) {
+ if (z__[i4] <= tol2 * z__[i4 - 3] || z__[i4 - 1] <=
+ tol2 * sigma) {
+ z__[i4 - 1] = -sigma;
+ splt = i4 / 4;
+ qmax = 0.;
+ emin = z__[i4 + 3];
+ oldemn = z__[i4 + 4];
+ } else {
+/* Computing MAX */
+ d__1 = qmax, d__2 = z__[i4 + 1];
+ qmax = max(d__1,d__2);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[i4 - 1];
+ emin = min(d__1,d__2);
+/* Computing MIN */
+ d__1 = oldemn, d__2 = z__[i4];
+ oldemn = min(d__1,d__2);
+ }
+/* L130: */
+ }
+ z__[(n0 << 2) - 1] = emin;
+ z__[n0 * 4] = oldemn;
+ i0 = splt + 1;
+ }
+ }
+
+/* L140: */
+ }
+
+ *info = 2;
+ return 0;
+
+/* end IWHILB */
+
+L150:
+
+/* L160: */
+ ;
+ }
+
+ *info = 3;
+ return 0;
+
+/* end IWHILA */
+
+L170:
+
+/* Move q's to the front. */
+
+ i__1 = *n;
+ for (k = 2; k <= i__1; ++k) {
+ z__[k] = z__[(k << 2) - 3];
+/* L180: */
+ }
+
+/* Sort and compute sum of eigenvalues. */
+
+ dlasrt_("D", n, &z__[1], &iinfo);
+
+ e = 0.;
+ for (k = *n; k >= 1; --k) {
+ e += z__[k];
+/* L190: */
+ }
+
+/* Store trace, sum(eigenvalues) and information on performance. */
+
+ z__[(*n << 1) + 1] = trace;
+ z__[(*n << 1) + 2] = e;
+ z__[(*n << 1) + 3] = (doublereal) iter;
+/* Computing 2nd power */
+ i__1 = *n;
+ z__[(*n << 1) + 4] = (doublereal) ndiv / (doublereal) (i__1 * i__1);
+ z__[(*n << 1) + 5] = nfail * 100. / (doublereal) iter;
+ return 0;
+
+/* End of DLASQ2 */
+
+} /* dlasq2_ */
diff --git a/3rdparty/clapack/src/dlasq3.c b/3rdparty/clapack/src/dlasq3.c
new file mode 100644
index 0000000..0e59c94
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq3.c
@@ -0,0 +1,350 @@
+/* dlasq3.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasq3_(integer *i0, integer *n0, doublereal *z__,
+ integer *pp, doublereal *dmin__, doublereal *sigma, doublereal *desig,
+ doublereal *qmax, integer *nfail, integer *iter, integer *ndiv,
+ logical *ieee, integer *ttype, doublereal *dmin1, doublereal *dmin2,
+ doublereal *dn, doublereal *dn1, doublereal *dn2, doublereal *g,
+ doublereal *tau)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal s, t;
+ integer j4, nn;
+ doublereal eps, tol;
+ integer n0in, ipn4;
+ doublereal tol2, temp;
+ extern /* Subroutine */ int dlasq4_(integer *, integer *, doublereal *,
+ integer *, integer *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *, integer *,
+ doublereal *), dlasq5_(integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *, doublereal *, doublereal *, logical *), dlasq6_(
+ integer *, integer *, doublereal *, integer *, doublereal *,
+ doublereal *, doublereal *, doublereal *, doublereal *,
+ doublereal *);
+ extern doublereal dlamch_(char *);
+ extern logical disnan_(doublereal *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ3 checks for deflation, computes a shift (TAU) and calls dqds. */
+/* In case of failure it changes shifts, and tries again until output */
+/* is positive. */
+
+/* Arguments */
+/* ========= */
+
+/* I0 (input) INTEGER */
+/* First index. */
+
+/* N0 (input) INTEGER */
+/* Last index. */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( 4*N ) */
+/* Z holds the qd array. */
+
+/* PP (input/output) INTEGER */
+/* PP=0 for ping, PP=1 for pong. */
+/* PP=2 indicates that flipping was applied to the Z array */
+/* and that the initial tests for deflation should not be */
+/* performed. */
+
+/* DMIN (output) DOUBLE PRECISION */
+/* Minimum value of d. */
+
+/* SIGMA (output) DOUBLE PRECISION */
+/* Sum of shifts used in current segment. */
+
+/* DESIG (input/output) DOUBLE PRECISION */
+/* Lower order part of SIGMA */
+
+/* QMAX (input) DOUBLE PRECISION */
+/* Maximum value of q. */
+
+/* NFAIL (output) INTEGER */
+/* Number of times shift was too big. */
+
+/* ITER (output) INTEGER */
+/* Number of iterations. */
+
+/* NDIV (output) INTEGER */
+/* Number of divisions. */
+
+/* IEEE (input) LOGICAL */
+/* Flag for IEEE or non IEEE arithmetic (passed to DLASQ5). */
+
+/* TTYPE (input/output) INTEGER */
+/* Shift type. */
+
+/* DMIN1, DMIN2, DN, DN1, DN2, G, TAU (input/output) DOUBLE PRECISION */
+/* These are passed as arguments in order to save their values */
+/* between calls to DLASQ3. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. External Function .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --z__;
+
+ /* Function Body */
+ n0in = *n0;
+ eps = dlamch_("Precision");
+ tol = eps * 100.;
+/* Computing 2nd power */
+ d__1 = tol;
+ tol2 = d__1 * d__1;
+
+/* Check for deflation. */
+
+L10:
+
+ if (*n0 < *i0) {
+ return 0;
+ }
+ if (*n0 == *i0) {
+ goto L20;
+ }
+ nn = (*n0 << 2) + *pp;
+ if (*n0 == *i0 + 1) {
+ goto L40;
+ }
+
+/* Check whether E(N0-1) is negligible, 1 eigenvalue. */
+
+ if (z__[nn - 5] > tol2 * (*sigma + z__[nn - 3]) && z__[nn - (*pp << 1) -
+ 4] > tol2 * z__[nn - 7]) {
+ goto L30;
+ }
+
+L20:
+
+ z__[(*n0 << 2) - 3] = z__[(*n0 << 2) + *pp - 3] + *sigma;
+ --(*n0);
+ goto L10;
+
+/* Check whether E(N0-2) is negligible, 2 eigenvalues. */
+
+L30:
+
+ if (z__[nn - 9] > tol2 * *sigma && z__[nn - (*pp << 1) - 8] > tol2 * z__[
+ nn - 11]) {
+ goto L50;
+ }
+
+L40:
+
+ if (z__[nn - 3] > z__[nn - 7]) {
+ s = z__[nn - 3];
+ z__[nn - 3] = z__[nn - 7];
+ z__[nn - 7] = s;
+ }
+ if (z__[nn - 5] > z__[nn - 3] * tol2) {
+ t = (z__[nn - 7] - z__[nn - 3] + z__[nn - 5]) * .5;
+ s = z__[nn - 3] * (z__[nn - 5] / t);
+ if (s <= t) {
+ s = z__[nn - 3] * (z__[nn - 5] / (t * (sqrt(s / t + 1.) + 1.)));
+ } else {
+ s = z__[nn - 3] * (z__[nn - 5] / (t + sqrt(t) * sqrt(t + s)));
+ }
+ t = z__[nn - 7] + (s + z__[nn - 5]);
+ z__[nn - 3] *= z__[nn - 7] / t;
+ z__[nn - 7] = t;
+ }
+ z__[(*n0 << 2) - 7] = z__[nn - 7] + *sigma;
+ z__[(*n0 << 2) - 3] = z__[nn - 3] + *sigma;
+ *n0 += -2;
+ goto L10;
+
+L50:
+ if (*pp == 2) {
+ *pp = 0;
+ }
+
+/* Reverse the qd-array, if warranted. */
+
+ if (*dmin__ <= 0. || *n0 < n0in) {
+ if (z__[(*i0 << 2) + *pp - 3] * 1.5 < z__[(*n0 << 2) + *pp - 3]) {
+ ipn4 = *i0 + *n0 << 2;
+ i__1 = *i0 + *n0 - 1 << 1;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ temp = z__[j4 - 3];
+ z__[j4 - 3] = z__[ipn4 - j4 - 3];
+ z__[ipn4 - j4 - 3] = temp;
+ temp = z__[j4 - 2];
+ z__[j4 - 2] = z__[ipn4 - j4 - 2];
+ z__[ipn4 - j4 - 2] = temp;
+ temp = z__[j4 - 1];
+ z__[j4 - 1] = z__[ipn4 - j4 - 5];
+ z__[ipn4 - j4 - 5] = temp;
+ temp = z__[j4];
+ z__[j4] = z__[ipn4 - j4 - 4];
+ z__[ipn4 - j4 - 4] = temp;
+/* L60: */
+ }
+ if (*n0 - *i0 <= 4) {
+ z__[(*n0 << 2) + *pp - 1] = z__[(*i0 << 2) + *pp - 1];
+ z__[(*n0 << 2) - *pp] = z__[(*i0 << 2) - *pp];
+ }
+/* Computing MIN */
+ d__1 = *dmin2, d__2 = z__[(*n0 << 2) + *pp - 1];
+ *dmin2 = min(d__1,d__2);
+/* Computing MIN */
+ d__1 = z__[(*n0 << 2) + *pp - 1], d__2 = z__[(*i0 << 2) + *pp - 1]
+ , d__1 = min(d__1,d__2), d__2 = z__[(*i0 << 2) + *pp + 3];
+ z__[(*n0 << 2) + *pp - 1] = min(d__1,d__2);
+/* Computing MIN */
+ d__1 = z__[(*n0 << 2) - *pp], d__2 = z__[(*i0 << 2) - *pp], d__1 =
+ min(d__1,d__2), d__2 = z__[(*i0 << 2) - *pp + 4];
+ z__[(*n0 << 2) - *pp] = min(d__1,d__2);
+/* Computing MAX */
+ d__1 = *qmax, d__2 = z__[(*i0 << 2) + *pp - 3], d__1 = max(d__1,
+ d__2), d__2 = z__[(*i0 << 2) + *pp + 1];
+ *qmax = max(d__1,d__2);
+ *dmin__ = -0.;
+ }
+ }
+
+/* Choose a shift. */
+
+ dlasq4_(i0, n0, &z__[1], pp, &n0in, dmin__, dmin1, dmin2, dn, dn1, dn2,
+ tau, ttype, g);
+
+/* Call dqds until DMIN > 0. */
+
+L70:
+
+ dlasq5_(i0, n0, &z__[1], pp, tau, dmin__, dmin1, dmin2, dn, dn1, dn2,
+ ieee);
+
+ *ndiv += *n0 - *i0 + 2;
+ ++(*iter);
+
+/* Check status. */
+
+ if (*dmin__ >= 0. && *dmin1 > 0.) {
+
+/* Success. */
+
+ goto L90;
+
+ } else if (*dmin__ < 0. && *dmin1 > 0. && z__[(*n0 - 1 << 2) - *pp] < tol
+ * (*sigma + *dn1) && abs(*dn) < tol * *sigma) {
+
+/* Convergence hidden by negative DN. */
+
+ z__[(*n0 - 1 << 2) - *pp + 2] = 0.;
+ *dmin__ = 0.;
+ goto L90;
+ } else if (*dmin__ < 0.) {
+
+/* TAU too big. Select new TAU and try again. */
+
+ ++(*nfail);
+ if (*ttype < -22) {
+
+/* Failed twice. Play it safe. */
+
+ *tau = 0.;
+ } else if (*dmin1 > 0.) {
+
+/* Late failure. Gives excellent shift. */
+
+ *tau = (*tau + *dmin__) * (1. - eps * 2.);
+ *ttype += -11;
+ } else {
+
+/* Early failure. Divide by 4. */
+
+ *tau *= .25;
+ *ttype += -12;
+ }
+ goto L70;
+ } else if (disnan_(dmin__)) {
+
+/* NaN. */
+
+ if (*tau == 0.) {
+ goto L80;
+ } else {
+ *tau = 0.;
+ goto L70;
+ }
+ } else {
+
+/* Possible underflow. Play it safe. */
+
+ goto L80;
+ }
+
+/* Risk of underflow. */
+
+L80:
+ dlasq6_(i0, n0, &z__[1], pp, dmin__, dmin1, dmin2, dn, dn1, dn2);
+ *ndiv += *n0 - *i0 + 2;
+ ++(*iter);
+ *tau = 0.;
+
+L90:
+ if (*tau < *sigma) {
+ *desig += *tau;
+ t = *sigma + *desig;
+ *desig -= t - *sigma;
+ } else {
+ t = *sigma + *tau;
+ *desig = *sigma - (t - *tau) + *desig;
+ }
+ *sigma = t;
+
+ return 0;
+
+/* End of DLASQ3 */
+
+} /* dlasq3_ */
diff --git a/3rdparty/clapack/src/dlasq4.c b/3rdparty/clapack/src/dlasq4.c
new file mode 100644
index 0000000..333a1d2
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq4.c
@@ -0,0 +1,403 @@
+/* dlasq4.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasq4_(integer *i0, integer *n0, doublereal *z__,
+ integer *pp, integer *n0in, doublereal *dmin__, doublereal *dmin1,
+ doublereal *dmin2, doublereal *dn, doublereal *dn1, doublereal *dn2,
+ doublereal *tau, integer *ttype, doublereal *g)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ doublereal s, a2, b1, b2;
+ integer i4, nn, np;
+ doublereal gam, gap1, gap2;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ4 computes an approximation TAU to the smallest eigenvalue */
+/* using values of d from the previous transform. */
+
+/* I0 (input) INTEGER */
+/* First index. */
+
+/* N0 (input) INTEGER */
+/* Last index. */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( 4*N ) */
+/* Z holds the qd array. */
+
+/* PP (input) INTEGER */
+/* PP=0 for ping, PP=1 for pong. */
+
+/* NOIN (input) INTEGER */
+/* The value of N0 at start of EIGTEST. */
+
+/* DMIN (input) DOUBLE PRECISION */
+/* Minimum value of d. */
+
+/* DMIN1 (input) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ). */
+
+/* DMIN2 (input) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ) and D( N0-1 ). */
+
+/* DN (input) DOUBLE PRECISION */
+/* d(N) */
+
+/* DN1 (input) DOUBLE PRECISION */
+/* d(N-1) */
+
+/* DN2 (input) DOUBLE PRECISION */
+/* d(N-2) */
+
+/* TAU (output) DOUBLE PRECISION */
+/* This is the shift. */
+
+/* TTYPE (output) INTEGER */
+/* Shift type. */
+
+/* G (input/output) REAL */
+/* G is passed as an argument in order to save its value between */
+/* calls to DLASQ4. */
+
+/* Further Details */
+/* =============== */
+/* CNST1 = 9/16 */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* A negative DMIN forces the shift to take that absolute value */
+/* TTYPE records the type of shift. */
+
+ /* Parameter adjustments */
+ --z__;
+
+ /* Function Body */
+ if (*dmin__ <= 0.) {
+ *tau = -(*dmin__);
+ *ttype = -1;
+ return 0;
+ }
+
+ nn = (*n0 << 2) + *pp;
+ if (*n0in == *n0) {
+
+/* No eigenvalues deflated. */
+
+ if (*dmin__ == *dn || *dmin__ == *dn1) {
+
+ b1 = sqrt(z__[nn - 3]) * sqrt(z__[nn - 5]);
+ b2 = sqrt(z__[nn - 7]) * sqrt(z__[nn - 9]);
+ a2 = z__[nn - 7] + z__[nn - 5];
+
+/* Cases 2 and 3. */
+
+ if (*dmin__ == *dn && *dmin1 == *dn1) {
+ gap2 = *dmin2 - a2 - *dmin2 * .25;
+ if (gap2 > 0. && gap2 > b2) {
+ gap1 = a2 - *dn - b2 / gap2 * b2;
+ } else {
+ gap1 = a2 - *dn - (b1 + b2);
+ }
+ if (gap1 > 0. && gap1 > b1) {
+/* Computing MAX */
+ d__1 = *dn - b1 / gap1 * b1, d__2 = *dmin__ * .5;
+ s = max(d__1,d__2);
+ *ttype = -2;
+ } else {
+ s = 0.;
+ if (*dn > b1) {
+ s = *dn - b1;
+ }
+ if (a2 > b1 + b2) {
+/* Computing MIN */
+ d__1 = s, d__2 = a2 - (b1 + b2);
+ s = min(d__1,d__2);
+ }
+/* Computing MAX */
+ d__1 = s, d__2 = *dmin__ * .333;
+ s = max(d__1,d__2);
+ *ttype = -3;
+ }
+ } else {
+
+/* Case 4. */
+
+ *ttype = -4;
+ s = *dmin__ * .25;
+ if (*dmin__ == *dn) {
+ gam = *dn;
+ a2 = 0.;
+ if (z__[nn - 5] > z__[nn - 7]) {
+ return 0;
+ }
+ b2 = z__[nn - 5] / z__[nn - 7];
+ np = nn - 9;
+ } else {
+ np = nn - (*pp << 1);
+ b2 = z__[np - 2];
+ gam = *dn1;
+ if (z__[np - 4] > z__[np - 2]) {
+ return 0;
+ }
+ a2 = z__[np - 4] / z__[np - 2];
+ if (z__[nn - 9] > z__[nn - 11]) {
+ return 0;
+ }
+ b2 = z__[nn - 9] / z__[nn - 11];
+ np = nn - 13;
+ }
+
+/* Approximate contribution to norm squared from I < NN-1. */
+
+ a2 += b2;
+ i__1 = (*i0 << 2) - 1 + *pp;
+ for (i4 = np; i4 >= i__1; i4 += -4) {
+ if (b2 == 0.) {
+ goto L20;
+ }
+ b1 = b2;
+ if (z__[i4] > z__[i4 - 2]) {
+ return 0;
+ }
+ b2 *= z__[i4] / z__[i4 - 2];
+ a2 += b2;
+ if (max(b2,b1) * 100. < a2 || .563 < a2) {
+ goto L20;
+ }
+/* L10: */
+ }
+L20:
+ a2 *= 1.05;
+
+/* Rayleigh quotient residual bound. */
+
+ if (a2 < .563) {
+ s = gam * (1. - sqrt(a2)) / (a2 + 1.);
+ }
+ }
+ } else if (*dmin__ == *dn2) {
+
+/* Case 5. */
+
+ *ttype = -5;
+ s = *dmin__ * .25;
+
+/* Compute contribution to norm squared from I > NN-2. */
+
+ np = nn - (*pp << 1);
+ b1 = z__[np - 2];
+ b2 = z__[np - 6];
+ gam = *dn2;
+ if (z__[np - 8] > b2 || z__[np - 4] > b1) {
+ return 0;
+ }
+ a2 = z__[np - 8] / b2 * (z__[np - 4] / b1 + 1.);
+
+/* Approximate contribution to norm squared from I < NN-2. */
+
+ if (*n0 - *i0 > 2) {
+ b2 = z__[nn - 13] / z__[nn - 15];
+ a2 += b2;
+ i__1 = (*i0 << 2) - 1 + *pp;
+ for (i4 = nn - 17; i4 >= i__1; i4 += -4) {
+ if (b2 == 0.) {
+ goto L40;
+ }
+ b1 = b2;
+ if (z__[i4] > z__[i4 - 2]) {
+ return 0;
+ }
+ b2 *= z__[i4] / z__[i4 - 2];
+ a2 += b2;
+ if (max(b2,b1) * 100. < a2 || .563 < a2) {
+ goto L40;
+ }
+/* L30: */
+ }
+L40:
+ a2 *= 1.05;
+ }
+
+ if (a2 < .563) {
+ s = gam * (1. - sqrt(a2)) / (a2 + 1.);
+ }
+ } else {
+
+/* Case 6, no information to guide us. */
+
+ if (*ttype == -6) {
+ *g += (1. - *g) * .333;
+ } else if (*ttype == -18) {
+ *g = .083250000000000005;
+ } else {
+ *g = .25;
+ }
+ s = *g * *dmin__;
+ *ttype = -6;
+ }
+
+ } else if (*n0in == *n0 + 1) {
+
+/* One eigenvalue just deflated. Use DMIN1, DN1 for DMIN and DN. */
+
+ if (*dmin1 == *dn1 && *dmin2 == *dn2) {
+
+/* Cases 7 and 8. */
+
+ *ttype = -7;
+ s = *dmin1 * .333;
+ if (z__[nn - 5] > z__[nn - 7]) {
+ return 0;
+ }
+ b1 = z__[nn - 5] / z__[nn - 7];
+ b2 = b1;
+ if (b2 == 0.) {
+ goto L60;
+ }
+ i__1 = (*i0 << 2) - 1 + *pp;
+ for (i4 = (*n0 << 2) - 9 + *pp; i4 >= i__1; i4 += -4) {
+ a2 = b1;
+ if (z__[i4] > z__[i4 - 2]) {
+ return 0;
+ }
+ b1 *= z__[i4] / z__[i4 - 2];
+ b2 += b1;
+ if (max(b1,a2) * 100. < b2) {
+ goto L60;
+ }
+/* L50: */
+ }
+L60:
+ b2 = sqrt(b2 * 1.05);
+/* Computing 2nd power */
+ d__1 = b2;
+ a2 = *dmin1 / (d__1 * d__1 + 1.);
+ gap2 = *dmin2 * .5 - a2;
+ if (gap2 > 0. && gap2 > b2 * a2) {
+/* Computing MAX */
+ d__1 = s, d__2 = a2 * (1. - a2 * 1.01 * (b2 / gap2) * b2);
+ s = max(d__1,d__2);
+ } else {
+/* Computing MAX */
+ d__1 = s, d__2 = a2 * (1. - b2 * 1.01);
+ s = max(d__1,d__2);
+ *ttype = -8;
+ }
+ } else {
+
+/* Case 9. */
+
+ s = *dmin1 * .25;
+ if (*dmin1 == *dn1) {
+ s = *dmin1 * .5;
+ }
+ *ttype = -9;
+ }
+
+ } else if (*n0in == *n0 + 2) {
+
+/* Two eigenvalues deflated. Use DMIN2, DN2 for DMIN and DN. */
+
+/* Cases 10 and 11. */
+
+ if (*dmin2 == *dn2 && z__[nn - 5] * 2. < z__[nn - 7]) {
+ *ttype = -10;
+ s = *dmin2 * .333;
+ if (z__[nn - 5] > z__[nn - 7]) {
+ return 0;
+ }
+ b1 = z__[nn - 5] / z__[nn - 7];
+ b2 = b1;
+ if (b2 == 0.) {
+ goto L80;
+ }
+ i__1 = (*i0 << 2) - 1 + *pp;
+ for (i4 = (*n0 << 2) - 9 + *pp; i4 >= i__1; i4 += -4) {
+ if (z__[i4] > z__[i4 - 2]) {
+ return 0;
+ }
+ b1 *= z__[i4] / z__[i4 - 2];
+ b2 += b1;
+ if (b1 * 100. < b2) {
+ goto L80;
+ }
+/* L70: */
+ }
+L80:
+ b2 = sqrt(b2 * 1.05);
+/* Computing 2nd power */
+ d__1 = b2;
+ a2 = *dmin2 / (d__1 * d__1 + 1.);
+ gap2 = z__[nn - 7] + z__[nn - 9] - sqrt(z__[nn - 11]) * sqrt(z__[
+ nn - 9]) - a2;
+ if (gap2 > 0. && gap2 > b2 * a2) {
+/* Computing MAX */
+ d__1 = s, d__2 = a2 * (1. - a2 * 1.01 * (b2 / gap2) * b2);
+ s = max(d__1,d__2);
+ } else {
+/* Computing MAX */
+ d__1 = s, d__2 = a2 * (1. - b2 * 1.01);
+ s = max(d__1,d__2);
+ }
+ } else {
+ s = *dmin2 * .25;
+ *ttype = -11;
+ }
+ } else if (*n0in > *n0 + 2) {
+
+/* Case 12, more than two eigenvalues deflated. No information. */
+
+ s = 0.;
+ *ttype = -12;
+ }
+
+ *tau = s;
+ return 0;
+
+/* End of DLASQ4 */
+
+} /* dlasq4_ */
diff --git a/3rdparty/clapack/src/dlasq5.c b/3rdparty/clapack/src/dlasq5.c
new file mode 100644
index 0000000..df306bd
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq5.c
@@ -0,0 +1,240 @@
+/* dlasq5.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasq5_(integer *i0, integer *n0, doublereal *z__,
+ integer *pp, doublereal *tau, doublereal *dmin__, doublereal *dmin1,
+ doublereal *dmin2, doublereal *dn, doublereal *dnm1, doublereal *dnm2,
+ logical *ieee)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ doublereal d__;
+ integer j4, j4p2;
+ doublereal emin, temp;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ5 computes one dqds transform in ping-pong form, one */
+/* version for IEEE machines another for non IEEE machines. */
+
+/* Arguments */
+/* ========= */
+
+/* I0 (input) INTEGER */
+/* First index. */
+
+/* N0 (input) INTEGER */
+/* Last index. */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( 4*N ) */
+/* Z holds the qd array. EMIN is stored in Z(4*N0) to avoid */
+/* an extra argument. */
+
+/* PP (input) INTEGER */
+/* PP=0 for ping, PP=1 for pong. */
+
+/* TAU (input) DOUBLE PRECISION */
+/* This is the shift. */
+
+/* DMIN (output) DOUBLE PRECISION */
+/* Minimum value of d. */
+
+/* DMIN1 (output) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ). */
+
+/* DMIN2 (output) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ) and D( N0-1 ). */
+
+/* DN (output) DOUBLE PRECISION */
+/* d(N0), the last value of d. */
+
+/* DNM1 (output) DOUBLE PRECISION */
+/* d(N0-1). */
+
+/* DNM2 (output) DOUBLE PRECISION */
+/* d(N0-2). */
+
+/* IEEE (input) LOGICAL */
+/* Flag for IEEE or non IEEE arithmetic. */
+
+/* ===================================================================== */
+
+/* .. Parameter .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --z__;
+
+ /* Function Body */
+ if (*n0 - *i0 - 1 <= 0) {
+ return 0;
+ }
+
+ j4 = (*i0 << 2) + *pp - 3;
+ emin = z__[j4 + 4];
+ d__ = z__[j4] - *tau;
+ *dmin__ = d__;
+ *dmin1 = -z__[j4];
+
+ if (*ieee) {
+
+/* Code for IEEE arithmetic. */
+
+ if (*pp == 0) {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 2] = d__ + z__[j4 - 1];
+ temp = z__[j4 + 1] / z__[j4 - 2];
+ d__ = d__ * temp - *tau;
+ *dmin__ = min(*dmin__,d__);
+ z__[j4] = z__[j4 - 1] * temp;
+/* Computing MIN */
+ d__1 = z__[j4];
+ emin = min(d__1,emin);
+/* L10: */
+ }
+ } else {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 3] = d__ + z__[j4];
+ temp = z__[j4 + 2] / z__[j4 - 3];
+ d__ = d__ * temp - *tau;
+ *dmin__ = min(*dmin__,d__);
+ z__[j4 - 1] = z__[j4] * temp;
+/* Computing MIN */
+ d__1 = z__[j4 - 1];
+ emin = min(d__1,emin);
+/* L20: */
+ }
+ }
+
+/* Unroll last two steps. */
+
+ *dnm2 = d__;
+ *dmin2 = *dmin__;
+ j4 = (*n0 - 2 << 2) - *pp;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm2 + z__[j4p2];
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dnm1 = z__[j4p2 + 2] * (*dnm2 / z__[j4 - 2]) - *tau;
+ *dmin__ = min(*dmin__,*dnm1);
+
+ *dmin1 = *dmin__;
+ j4 += 4;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm1 + z__[j4p2];
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dn = z__[j4p2 + 2] * (*dnm1 / z__[j4 - 2]) - *tau;
+ *dmin__ = min(*dmin__,*dn);
+
+ } else {
+
+/* Code for non IEEE arithmetic. */
+
+ if (*pp == 0) {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 2] = d__ + z__[j4 - 1];
+ if (d__ < 0.) {
+ return 0;
+ } else {
+ z__[j4] = z__[j4 + 1] * (z__[j4 - 1] / z__[j4 - 2]);
+ d__ = z__[j4 + 1] * (d__ / z__[j4 - 2]) - *tau;
+ }
+ *dmin__ = min(*dmin__,d__);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[j4];
+ emin = min(d__1,d__2);
+/* L30: */
+ }
+ } else {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 3] = d__ + z__[j4];
+ if (d__ < 0.) {
+ return 0;
+ } else {
+ z__[j4 - 1] = z__[j4 + 2] * (z__[j4] / z__[j4 - 3]);
+ d__ = z__[j4 + 2] * (d__ / z__[j4 - 3]) - *tau;
+ }
+ *dmin__ = min(*dmin__,d__);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[j4 - 1];
+ emin = min(d__1,d__2);
+/* L40: */
+ }
+ }
+
+/* Unroll last two steps. */
+
+ *dnm2 = d__;
+ *dmin2 = *dmin__;
+ j4 = (*n0 - 2 << 2) - *pp;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm2 + z__[j4p2];
+ if (*dnm2 < 0.) {
+ return 0;
+ } else {
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dnm1 = z__[j4p2 + 2] * (*dnm2 / z__[j4 - 2]) - *tau;
+ }
+ *dmin__ = min(*dmin__,*dnm1);
+
+ *dmin1 = *dmin__;
+ j4 += 4;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm1 + z__[j4p2];
+ if (*dnm1 < 0.) {
+ return 0;
+ } else {
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dn = z__[j4p2 + 2] * (*dnm1 / z__[j4 - 2]) - *tau;
+ }
+ *dmin__ = min(*dmin__,*dn);
+
+ }
+
+ z__[j4 + 2] = *dn;
+ z__[(*n0 << 2) - *pp] = emin;
+ return 0;
+
+/* End of DLASQ5 */
+
+} /* dlasq5_ */
diff --git a/3rdparty/clapack/src/dlasq6.c b/3rdparty/clapack/src/dlasq6.c
new file mode 100644
index 0000000..22d809f
--- /dev/null
+++ b/3rdparty/clapack/src/dlasq6.c
@@ -0,0 +1,212 @@
+/* dlasq6.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasq6_(integer *i0, integer *n0, doublereal *z__,
+ integer *pp, doublereal *dmin__, doublereal *dmin1, doublereal *dmin2,
+ doublereal *dn, doublereal *dnm1, doublereal *dnm2)
+{
+ /* System generated locals */
+ integer i__1;
+ doublereal d__1, d__2;
+
+ /* Local variables */
+ doublereal d__;
+ integer j4, j4p2;
+ doublereal emin, temp;
+ extern doublereal dlamch_(char *);
+ doublereal safmin;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+
+/* -- Contributed by Osni Marques of the Lawrence Berkeley National -- */
+/* -- Laboratory and Beresford Parlett of the Univ. of California at -- */
+/* -- Berkeley -- */
+/* -- November 2008 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASQ6 computes one dqd (shift equal to zero) transform in */
+/* ping-pong form, with protection against underflow and overflow. */
+
+/* Arguments */
+/* ========= */
+
+/* I0 (input) INTEGER */
+/* First index. */
+
+/* N0 (input) INTEGER */
+/* Last index. */
+
+/* Z (input) DOUBLE PRECISION array, dimension ( 4*N ) */
+/* Z holds the qd array. EMIN is stored in Z(4*N0) to avoid */
+/* an extra argument. */
+
+/* PP (input) INTEGER */
+/* PP=0 for ping, PP=1 for pong. */
+
+/* DMIN (output) DOUBLE PRECISION */
+/* Minimum value of d. */
+
+/* DMIN1 (output) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ). */
+
+/* DMIN2 (output) DOUBLE PRECISION */
+/* Minimum value of d, excluding D( N0 ) and D( N0-1 ). */
+
+/* DN (output) DOUBLE PRECISION */
+/* d(N0), the last value of d. */
+
+/* DNM1 (output) DOUBLE PRECISION */
+/* d(N0-1). */
+
+/* DNM2 (output) DOUBLE PRECISION */
+/* d(N0-2). */
+
+/* ===================================================================== */
+
+/* .. Parameter .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Function .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --z__;
+
+ /* Function Body */
+ if (*n0 - *i0 - 1 <= 0) {
+ return 0;
+ }
+
+ safmin = dlamch_("Safe minimum");
+ j4 = (*i0 << 2) + *pp - 3;
+ emin = z__[j4 + 4];
+ d__ = z__[j4];
+ *dmin__ = d__;
+
+ if (*pp == 0) {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 2] = d__ + z__[j4 - 1];
+ if (z__[j4 - 2] == 0.) {
+ z__[j4] = 0.;
+ d__ = z__[j4 + 1];
+ *dmin__ = d__;
+ emin = 0.;
+ } else if (safmin * z__[j4 + 1] < z__[j4 - 2] && safmin * z__[j4
+ - 2] < z__[j4 + 1]) {
+ temp = z__[j4 + 1] / z__[j4 - 2];
+ z__[j4] = z__[j4 - 1] * temp;
+ d__ *= temp;
+ } else {
+ z__[j4] = z__[j4 + 1] * (z__[j4 - 1] / z__[j4 - 2]);
+ d__ = z__[j4 + 1] * (d__ / z__[j4 - 2]);
+ }
+ *dmin__ = min(*dmin__,d__);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[j4];
+ emin = min(d__1,d__2);
+/* L10: */
+ }
+ } else {
+ i__1 = *n0 - 3 << 2;
+ for (j4 = *i0 << 2; j4 <= i__1; j4 += 4) {
+ z__[j4 - 3] = d__ + z__[j4];
+ if (z__[j4 - 3] == 0.) {
+ z__[j4 - 1] = 0.;
+ d__ = z__[j4 + 2];
+ *dmin__ = d__;
+ emin = 0.;
+ } else if (safmin * z__[j4 + 2] < z__[j4 - 3] && safmin * z__[j4
+ - 3] < z__[j4 + 2]) {
+ temp = z__[j4 + 2] / z__[j4 - 3];
+ z__[j4 - 1] = z__[j4] * temp;
+ d__ *= temp;
+ } else {
+ z__[j4 - 1] = z__[j4 + 2] * (z__[j4] / z__[j4 - 3]);
+ d__ = z__[j4 + 2] * (d__ / z__[j4 - 3]);
+ }
+ *dmin__ = min(*dmin__,d__);
+/* Computing MIN */
+ d__1 = emin, d__2 = z__[j4 - 1];
+ emin = min(d__1,d__2);
+/* L20: */
+ }
+ }
+
+/* Unroll last two steps. */
+
+ *dnm2 = d__;
+ *dmin2 = *dmin__;
+ j4 = (*n0 - 2 << 2) - *pp;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm2 + z__[j4p2];
+ if (z__[j4 - 2] == 0.) {
+ z__[j4] = 0.;
+ *dnm1 = z__[j4p2 + 2];
+ *dmin__ = *dnm1;
+ emin = 0.;
+ } else if (safmin * z__[j4p2 + 2] < z__[j4 - 2] && safmin * z__[j4 - 2] <
+ z__[j4p2 + 2]) {
+ temp = z__[j4p2 + 2] / z__[j4 - 2];
+ z__[j4] = z__[j4p2] * temp;
+ *dnm1 = *dnm2 * temp;
+ } else {
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dnm1 = z__[j4p2 + 2] * (*dnm2 / z__[j4 - 2]);
+ }
+ *dmin__ = min(*dmin__,*dnm1);
+
+ *dmin1 = *dmin__;
+ j4 += 4;
+ j4p2 = j4 + (*pp << 1) - 1;
+ z__[j4 - 2] = *dnm1 + z__[j4p2];
+ if (z__[j4 - 2] == 0.) {
+ z__[j4] = 0.;
+ *dn = z__[j4p2 + 2];
+ *dmin__ = *dn;
+ emin = 0.;
+ } else if (safmin * z__[j4p2 + 2] < z__[j4 - 2] && safmin * z__[j4 - 2] <
+ z__[j4p2 + 2]) {
+ temp = z__[j4p2 + 2] / z__[j4 - 2];
+ z__[j4] = z__[j4p2] * temp;
+ *dn = *dnm1 * temp;
+ } else {
+ z__[j4] = z__[j4p2 + 2] * (z__[j4p2] / z__[j4 - 2]);
+ *dn = z__[j4p2 + 2] * (*dnm1 / z__[j4 - 2]);
+ }
+ *dmin__ = min(*dmin__,*dn);
+
+ z__[j4 + 2] = *dn;
+ z__[(*n0 << 2) - *pp] = emin;
+ return 0;
+
+/* End of DLASQ6 */
+
+} /* dlasq6_ */
diff --git a/3rdparty/clapack/src/dlasr.c b/3rdparty/clapack/src/dlasr.c
new file mode 100644
index 0000000..6abfa81
--- /dev/null
+++ b/3rdparty/clapack/src/dlasr.c
@@ -0,0 +1,453 @@
+/* dlasr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasr_(char *side, char *pivot, char *direct, integer *m,
+ integer *n, doublereal *c__, doublereal *s, doublereal *a, integer *
+ lda)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, j, info;
+ doublereal temp;
+ extern logical lsame_(char *, char *);
+ doublereal ctemp, stemp;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASR applies a sequence of plane rotations to a real matrix A, */
+/* from either the left or the right. */
+
+/* When SIDE = 'L', the transformation takes the form */
+
+/* A := P*A */
+
+/* and when SIDE = 'R', the transformation takes the form */
+
+/* A := A*P**T */
+
+/* where P is an orthogonal matrix consisting of a sequence of z plane */
+/* rotations, with z = M when SIDE = 'L' and z = N when SIDE = 'R', */
+/* and P**T is the transpose of P. */
+
+/* When DIRECT = 'F' (Forward sequence), then */
+
+/* P = P(z-1) * ... * P(2) * P(1) */
+
+/* and when DIRECT = 'B' (Backward sequence), then */
+
+/* P = P(1) * P(2) * ... * P(z-1) */
+
+/* where P(k) is a plane rotation matrix defined by the 2-by-2 rotation */
+
+/* R(k) = ( c(k) s(k) ) */
+/* = ( -s(k) c(k) ). */
+
+/* When PIVOT = 'V' (Variable pivot), the rotation is performed */
+/* for the plane (k,k+1), i.e., P(k) has the form */
+
+/* P(k) = ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+/* ( c(k) s(k) ) */
+/* ( -s(k) c(k) ) */
+/* ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+
+/* where R(k) appears as a rank-2 modification to the identity matrix in */
+/* rows and columns k and k+1. */
+
+/* When PIVOT = 'T' (Top pivot), the rotation is performed for the */
+/* plane (1,k+1), so P(k) has the form */
+
+/* P(k) = ( c(k) s(k) ) */
+/* ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+/* ( -s(k) c(k) ) */
+/* ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+
+/* where R(k) appears in rows and columns 1 and k+1. */
+
+/* Similarly, when PIVOT = 'B' (Bottom pivot), the rotation is */
+/* performed for the plane (k,z), giving P(k) the form */
+
+/* P(k) = ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+/* ( c(k) s(k) ) */
+/* ( 1 ) */
+/* ( ... ) */
+/* ( 1 ) */
+/* ( -s(k) c(k) ) */
+
+/* where R(k) appears in rows and columns k and z. The rotations are */
+/* performed without ever forming P(k) explicitly. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* Specifies whether the plane rotation matrix P is applied to */
+/* A on the left or the right. */
+/* = 'L': Left, compute A := P*A */
+/* = 'R': Right, compute A:= A*P**T */
+
+/* PIVOT (input) CHARACTER*1 */
+/* Specifies the plane for which P(k) is a plane rotation */
+/* matrix. */
+/* = 'V': Variable pivot, the plane (k,k+1) */
+/* = 'T': Top pivot, the plane (1,k+1) */
+/* = 'B': Bottom pivot, the plane (k,z) */
+
+/* DIRECT (input) CHARACTER*1 */
+/* Specifies whether P is a forward or backward sequence of */
+/* plane rotations. */
+/* = 'F': Forward, P = P(z-1)*...*P(2)*P(1) */
+/* = 'B': Backward, P = P(1)*P(2)*...*P(z-1) */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. If m <= 1, an immediate */
+/* return is effected. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. If n <= 1, an */
+/* immediate return is effected. */
+
+/* C (input) DOUBLE PRECISION array, dimension */
+/* (M-1) if SIDE = 'L' */
+/* (N-1) if SIDE = 'R' */
+/* The cosines c(k) of the plane rotations. */
+
+/* S (input) DOUBLE PRECISION array, dimension */
+/* (M-1) if SIDE = 'L' */
+/* (N-1) if SIDE = 'R' */
+/* The sines s(k) of the plane rotations. The 2-by-2 plane */
+/* rotation part of the matrix P(k), R(k), has the form */
+/* R(k) = ( c(k) s(k) ) */
+/* ( -s(k) c(k) ). */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The M-by-N matrix A. On exit, A is overwritten by P*A if */
+/* SIDE = 'R' or by A*P**T if SIDE = 'L'. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters */
+
+ /* Parameter adjustments */
+ --c__;
+ --s;
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ info = 0;
+ if (! (lsame_(side, "L") || lsame_(side, "R"))) {
+ info = 1;
+ } else if (! (lsame_(pivot, "V") || lsame_(pivot,
+ "T") || lsame_(pivot, "B"))) {
+ info = 2;
+ } else if (! (lsame_(direct, "F") || lsame_(direct,
+ "B"))) {
+ info = 3;
+ } else if (*m < 0) {
+ info = 4;
+ } else if (*n < 0) {
+ info = 5;
+ } else if (*lda < max(1,*m)) {
+ info = 9;
+ }
+ if (info != 0) {
+ xerbla_("DLASR ", &info);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+ if (lsame_(side, "L")) {
+
+/* Form P * A */
+
+ if (lsame_(pivot, "V")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *m - 1;
+ for (j = 1; j <= i__1; ++j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[j + 1 + i__ * a_dim1];
+ a[j + 1 + i__ * a_dim1] = ctemp * temp - stemp *
+ a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = stemp * temp + ctemp * a[j
+ + i__ * a_dim1];
+/* L10: */
+ }
+ }
+/* L20: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *m - 1; j >= 1; --j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[j + 1 + i__ * a_dim1];
+ a[j + 1 + i__ * a_dim1] = ctemp * temp - stemp *
+ a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = stemp * temp + ctemp * a[j
+ + i__ * a_dim1];
+/* L30: */
+ }
+ }
+/* L40: */
+ }
+ }
+ } else if (lsame_(pivot, "T")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *m;
+ for (j = 2; j <= i__1; ++j) {
+ ctemp = c__[j - 1];
+ stemp = s[j - 1];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = ctemp * temp - stemp * a[
+ i__ * a_dim1 + 1];
+ a[i__ * a_dim1 + 1] = stemp * temp + ctemp * a[
+ i__ * a_dim1 + 1];
+/* L50: */
+ }
+ }
+/* L60: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *m; j >= 2; --j) {
+ ctemp = c__[j - 1];
+ stemp = s[j - 1];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = ctemp * temp - stemp * a[
+ i__ * a_dim1 + 1];
+ a[i__ * a_dim1 + 1] = stemp * temp + ctemp * a[
+ i__ * a_dim1 + 1];
+/* L70: */
+ }
+ }
+/* L80: */
+ }
+ }
+ } else if (lsame_(pivot, "B")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *m - 1;
+ for (j = 1; j <= i__1; ++j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *n;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = stemp * a[*m + i__ * a_dim1]
+ + ctemp * temp;
+ a[*m + i__ * a_dim1] = ctemp * a[*m + i__ *
+ a_dim1] - stemp * temp;
+/* L90: */
+ }
+ }
+/* L100: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *m - 1; j >= 1; --j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[j + i__ * a_dim1];
+ a[j + i__ * a_dim1] = stemp * a[*m + i__ * a_dim1]
+ + ctemp * temp;
+ a[*m + i__ * a_dim1] = ctemp * a[*m + i__ *
+ a_dim1] - stemp * temp;
+/* L110: */
+ }
+ }
+/* L120: */
+ }
+ }
+ }
+ } else if (lsame_(side, "R")) {
+
+/* Form A * P' */
+
+ if (lsame_(pivot, "V")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *n - 1;
+ for (j = 1; j <= i__1; ++j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[i__ + (j + 1) * a_dim1];
+ a[i__ + (j + 1) * a_dim1] = ctemp * temp - stemp *
+ a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = stemp * temp + ctemp * a[
+ i__ + j * a_dim1];
+/* L130: */
+ }
+ }
+/* L140: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *n - 1; j >= 1; --j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[i__ + (j + 1) * a_dim1];
+ a[i__ + (j + 1) * a_dim1] = ctemp * temp - stemp *
+ a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = stemp * temp + ctemp * a[
+ i__ + j * a_dim1];
+/* L150: */
+ }
+ }
+/* L160: */
+ }
+ }
+ } else if (lsame_(pivot, "T")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *n;
+ for (j = 2; j <= i__1; ++j) {
+ ctemp = c__[j - 1];
+ stemp = s[j - 1];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = ctemp * temp - stemp * a[
+ i__ + a_dim1];
+ a[i__ + a_dim1] = stemp * temp + ctemp * a[i__ +
+ a_dim1];
+/* L170: */
+ }
+ }
+/* L180: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *n; j >= 2; --j) {
+ ctemp = c__[j - 1];
+ stemp = s[j - 1];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = ctemp * temp - stemp * a[
+ i__ + a_dim1];
+ a[i__ + a_dim1] = stemp * temp + ctemp * a[i__ +
+ a_dim1];
+/* L190: */
+ }
+ }
+/* L200: */
+ }
+ }
+ } else if (lsame_(pivot, "B")) {
+ if (lsame_(direct, "F")) {
+ i__1 = *n - 1;
+ for (j = 1; j <= i__1; ++j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__2 = *m;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ temp = a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = stemp * a[i__ + *n * a_dim1]
+ + ctemp * temp;
+ a[i__ + *n * a_dim1] = ctemp * a[i__ + *n *
+ a_dim1] - stemp * temp;
+/* L210: */
+ }
+ }
+/* L220: */
+ }
+ } else if (lsame_(direct, "B")) {
+ for (j = *n - 1; j >= 1; --j) {
+ ctemp = c__[j];
+ stemp = s[j];
+ if (ctemp != 1. || stemp != 0.) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ temp = a[i__ + j * a_dim1];
+ a[i__ + j * a_dim1] = stemp * a[i__ + *n * a_dim1]
+ + ctemp * temp;
+ a[i__ + *n * a_dim1] = ctemp * a[i__ + *n *
+ a_dim1] - stemp * temp;
+/* L230: */
+ }
+ }
+/* L240: */
+ }
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DLASR */
+
+} /* dlasr_ */
diff --git a/3rdparty/clapack/src/dlasrt.c b/3rdparty/clapack/src/dlasrt.c
new file mode 100644
index 0000000..5df285c
--- /dev/null
+++ b/3rdparty/clapack/src/dlasrt.c
@@ -0,0 +1,286 @@
+/* dlasrt.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlasrt_(char *id, integer *n, doublereal *d__, integer *
+ info)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+
+ /* Local variables */
+ integer i__, j;
+ doublereal d1, d2, d3;
+ integer dir;
+ doublereal tmp;
+ integer endd;
+ extern logical lsame_(char *, char *);
+ integer stack[64] /* was [2][32] */;
+ doublereal dmnmx;
+ integer start;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ integer stkpnt;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* Sort the numbers in D in increasing order (if ID = 'I') or */
+/* in decreasing order (if ID = 'D' ). */
+
+/* Use Quick Sort, reverting to Insertion sort on arrays of */
+/* size <= 20. Dimension of STACK limits N to about 2**32. */
+
+/* Arguments */
+/* ========= */
+
+/* ID (input) CHARACTER*1 */
+/* = 'I': sort D in increasing order; */
+/* = 'D': sort D in decreasing order. */
+
+/* N (input) INTEGER */
+/* The length of the array D. */
+
+/* D (input/output) DOUBLE PRECISION array, dimension (N) */
+/* On entry, the array to be sorted. */
+/* On exit, D has been sorted into increasing order */
+/* (D(1) <= ... <= D(N) ) or into decreasing order */
+/* (D(1) >= ... >= D(N) ), depending on ID. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input paramters. */
+
+ /* Parameter adjustments */
+ --d__;
+
+ /* Function Body */
+ *info = 0;
+ dir = -1;
+ if (lsame_(id, "D")) {
+ dir = 0;
+ } else if (lsame_(id, "I")) {
+ dir = 1;
+ }
+ if (dir == -1) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLASRT", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n <= 1) {
+ return 0;
+ }
+
+ stkpnt = 1;
+ stack[0] = 1;
+ stack[1] = *n;
+L10:
+ start = stack[(stkpnt << 1) - 2];
+ endd = stack[(stkpnt << 1) - 1];
+ --stkpnt;
+ if (endd - start <= 20 && endd - start > 0) {
+
+/* Do Insertion sort on D( START:ENDD ) */
+
+ if (dir == 0) {
+
+/* Sort into decreasing order */
+
+ i__1 = endd;
+ for (i__ = start + 1; i__ <= i__1; ++i__) {
+ i__2 = start + 1;
+ for (j = i__; j >= i__2; --j) {
+ if (d__[j] > d__[j - 1]) {
+ dmnmx = d__[j];
+ d__[j] = d__[j - 1];
+ d__[j - 1] = dmnmx;
+ } else {
+ goto L30;
+ }
+/* L20: */
+ }
+L30:
+ ;
+ }
+
+ } else {
+
+/* Sort into increasing order */
+
+ i__1 = endd;
+ for (i__ = start + 1; i__ <= i__1; ++i__) {
+ i__2 = start + 1;
+ for (j = i__; j >= i__2; --j) {
+ if (d__[j] < d__[j - 1]) {
+ dmnmx = d__[j];
+ d__[j] = d__[j - 1];
+ d__[j - 1] = dmnmx;
+ } else {
+ goto L50;
+ }
+/* L40: */
+ }
+L50:
+ ;
+ }
+
+ }
+
+ } else if (endd - start > 20) {
+
+/* Partition D( START:ENDD ) and stack parts, largest one first */
+
+/* Choose partition entry as median of 3 */
+
+ d1 = d__[start];
+ d2 = d__[endd];
+ i__ = (start + endd) / 2;
+ d3 = d__[i__];
+ if (d1 < d2) {
+ if (d3 < d1) {
+ dmnmx = d1;
+ } else if (d3 < d2) {
+ dmnmx = d3;
+ } else {
+ dmnmx = d2;
+ }
+ } else {
+ if (d3 < d2) {
+ dmnmx = d2;
+ } else if (d3 < d1) {
+ dmnmx = d3;
+ } else {
+ dmnmx = d1;
+ }
+ }
+
+ if (dir == 0) {
+
+/* Sort into decreasing order */
+
+ i__ = start - 1;
+ j = endd + 1;
+L60:
+L70:
+ --j;
+ if (d__[j] < dmnmx) {
+ goto L70;
+ }
+L80:
+ ++i__;
+ if (d__[i__] > dmnmx) {
+ goto L80;
+ }
+ if (i__ < j) {
+ tmp = d__[i__];
+ d__[i__] = d__[j];
+ d__[j] = tmp;
+ goto L60;
+ }
+ if (j - start > endd - j - 1) {
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = start;
+ stack[(stkpnt << 1) - 1] = j;
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = j + 1;
+ stack[(stkpnt << 1) - 1] = endd;
+ } else {
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = j + 1;
+ stack[(stkpnt << 1) - 1] = endd;
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = start;
+ stack[(stkpnt << 1) - 1] = j;
+ }
+ } else {
+
+/* Sort into increasing order */
+
+ i__ = start - 1;
+ j = endd + 1;
+L90:
+L100:
+ --j;
+ if (d__[j] > dmnmx) {
+ goto L100;
+ }
+L110:
+ ++i__;
+ if (d__[i__] < dmnmx) {
+ goto L110;
+ }
+ if (i__ < j) {
+ tmp = d__[i__];
+ d__[i__] = d__[j];
+ d__[j] = tmp;
+ goto L90;
+ }
+ if (j - start > endd - j - 1) {
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = start;
+ stack[(stkpnt << 1) - 1] = j;
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = j + 1;
+ stack[(stkpnt << 1) - 1] = endd;
+ } else {
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = j + 1;
+ stack[(stkpnt << 1) - 1] = endd;
+ ++stkpnt;
+ stack[(stkpnt << 1) - 2] = start;
+ stack[(stkpnt << 1) - 1] = j;
+ }
+ }
+ }
+ if (stkpnt > 0) {
+ goto L10;
+ }
+ return 0;
+
+/* End of DLASRT */
+
+} /* dlasrt_ */
diff --git a/3rdparty/clapack/src/dlassq.c b/3rdparty/clapack/src/dlassq.c
new file mode 100644
index 0000000..9b3aee5
--- /dev/null
+++ b/3rdparty/clapack/src/dlassq.c
@@ -0,0 +1,116 @@
+/* dlassq.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlassq_(integer *n, doublereal *x, integer *incx,
+ doublereal *scale, doublereal *sumsq)
+{
+ /* System generated locals */
+ integer i__1, i__2;
+ doublereal d__1;
+
+ /* Local variables */
+ integer ix;
+ doublereal absxi;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASSQ returns the values scl and smsq such that */
+
+/* ( scl**2 )*smsq = x( 1 )**2 +...+ x( n )**2 + ( scale**2 )*sumsq, */
+
+/* where x( i ) = X( 1 + ( i - 1 )*INCX ). The value of sumsq is */
+/* assumed to be non-negative and scl returns the value */
+
+/* scl = max( scale, abs( x( i ) ) ). */
+
+/* scale and sumsq must be supplied in SCALE and SUMSQ and */
+/* scl and smsq are overwritten on SCALE and SUMSQ respectively. */
+
+/* The routine makes only one pass through the vector x. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The number of elements to be used from the vector X. */
+
+/* X (input) DOUBLE PRECISION array, dimension (N) */
+/* The vector for which a scaled sum of squares is computed. */
+/* x( i ) = X( 1 + ( i - 1 )*INCX ), 1 <= i <= n. */
+
+/* INCX (input) INTEGER */
+/* The increment between successive values of the vector X. */
+/* INCX > 0. */
+
+/* SCALE (input/output) DOUBLE PRECISION */
+/* On entry, the value scale in the equation above. */
+/* On exit, SCALE is overwritten with scl , the scaling factor */
+/* for the sum of squares. */
+
+/* SUMSQ (input/output) DOUBLE PRECISION */
+/* On entry, the value sumsq in the equation above. */
+/* On exit, SUMSQ is overwritten with smsq , the basic sum of */
+/* squares from which scl has been factored out. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ /* Parameter adjustments */
+ --x;
+
+ /* Function Body */
+ if (*n > 0) {
+ i__1 = (*n - 1) * *incx + 1;
+ i__2 = *incx;
+ for (ix = 1; i__2 < 0 ? ix >= i__1 : ix <= i__1; ix += i__2) {
+ if (x[ix] != 0.) {
+ absxi = (d__1 = x[ix], abs(d__1));
+ if (*scale < absxi) {
+/* Computing 2nd power */
+ d__1 = *scale / absxi;
+ *sumsq = *sumsq * (d__1 * d__1) + 1;
+ *scale = absxi;
+ } else {
+/* Computing 2nd power */
+ d__1 = absxi / *scale;
+ *sumsq += d__1 * d__1;
+ }
+ }
+/* L10: */
+ }
+ }
+ return 0;
+
+/* End of DLASSQ */
+
+} /* dlassq_ */
diff --git a/3rdparty/clapack/src/dlasv2.c b/3rdparty/clapack/src/dlasv2.c
new file mode 100644
index 0000000..2418af3
--- /dev/null
+++ b/3rdparty/clapack/src/dlasv2.c
@@ -0,0 +1,274 @@
+/* dlasv2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static doublereal c_b3 = 2.;
+static doublereal c_b4 = 1.;
+
+/* Subroutine */ int dlasv2_(doublereal *f, doublereal *g, doublereal *h__,
+ doublereal *ssmin, doublereal *ssmax, doublereal *snr, doublereal *
+ csr, doublereal *snl, doublereal *csl)
+{
+ /* System generated locals */
+ doublereal d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal), d_sign(doublereal *, doublereal *);
+
+ /* Local variables */
+ doublereal a, d__, l, m, r__, s, t, fa, ga, ha, ft, gt, ht, mm, tt, clt,
+ crt, slt, srt;
+ integer pmax;
+ doublereal temp;
+ logical swap;
+ doublereal tsign;
+ extern doublereal dlamch_(char *);
+ logical gasmal;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASV2 computes the singular value decomposition of a 2-by-2 */
+/* triangular matrix */
+/* [ F G ] */
+/* [ 0 H ]. */
+/* On return, abs(SSMAX) is the larger singular value, abs(SSMIN) is the */
+/* smaller singular value, and (CSL,SNL) and (CSR,SNR) are the left and */
+/* right singular vectors for abs(SSMAX), giving the decomposition */
+
+/* [ CSL SNL ] [ F G ] [ CSR -SNR ] = [ SSMAX 0 ] */
+/* [-SNL CSL ] [ 0 H ] [ SNR CSR ] [ 0 SSMIN ]. */
+
+/* Arguments */
+/* ========= */
+
+/* F (input) DOUBLE PRECISION */
+/* The (1,1) element of the 2-by-2 matrix. */
+
+/* G (input) DOUBLE PRECISION */
+/* The (1,2) element of the 2-by-2 matrix. */
+
+/* H (input) DOUBLE PRECISION */
+/* The (2,2) element of the 2-by-2 matrix. */
+
+/* SSMIN (output) DOUBLE PRECISION */
+/* abs(SSMIN) is the smaller singular value. */
+
+/* SSMAX (output) DOUBLE PRECISION */
+/* abs(SSMAX) is the larger singular value. */
+
+/* SNL (output) DOUBLE PRECISION */
+/* CSL (output) DOUBLE PRECISION */
+/* The vector (CSL, SNL) is a unit left singular vector for the */
+/* singular value abs(SSMAX). */
+
+/* SNR (output) DOUBLE PRECISION */
+/* CSR (output) DOUBLE PRECISION */
+/* The vector (CSR, SNR) is a unit right singular vector for the */
+/* singular value abs(SSMAX). */
+
+/* Further Details */
+/* =============== */
+
+/* Any input parameter may be aliased with any output parameter. */
+
+/* Barring over/underflow and assuming a guard digit in subtraction, all */
+/* output quantities are correct to within a few units in the last */
+/* place (ulps). */
+
+/* In IEEE arithmetic, the code works correctly if one matrix element is */
+/* infinite. */
+
+/* Overflow will not occur unless the largest singular value itself */
+/* overflows or is within a few ulps of overflow. (On machines with */
+/* partial overflow, like the Cray, overflow may occur if the largest */
+/* singular value is within a factor of 2 of overflow.) */
+
+/* Underflow is harmless if underflow is gradual. Otherwise, results */
+/* may correspond to a matrix modified by perturbations of size near */
+/* the underflow threshold. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ ft = *f;
+ fa = abs(ft);
+ ht = *h__;
+ ha = abs(*h__);
+
+/* PMAX points to the maximum absolute element of matrix */
+/* PMAX = 1 if F largest in absolute values */
+/* PMAX = 2 if G largest in absolute values */
+/* PMAX = 3 if H largest in absolute values */
+
+ pmax = 1;
+ swap = ha > fa;
+ if (swap) {
+ pmax = 3;
+ temp = ft;
+ ft = ht;
+ ht = temp;
+ temp = fa;
+ fa = ha;
+ ha = temp;
+
+/* Now FA .ge. HA */
+
+ }
+ gt = *g;
+ ga = abs(gt);
+ if (ga == 0.) {
+
+/* Diagonal matrix */
+
+ *ssmin = ha;
+ *ssmax = fa;
+ clt = 1.;
+ crt = 1.;
+ slt = 0.;
+ srt = 0.;
+ } else {
+ gasmal = TRUE_;
+ if (ga > fa) {
+ pmax = 2;
+ if (fa / ga < dlamch_("EPS")) {
+
+/* Case of very large GA */
+
+ gasmal = FALSE_;
+ *ssmax = ga;
+ if (ha > 1.) {
+ *ssmin = fa / (ga / ha);
+ } else {
+ *ssmin = fa / ga * ha;
+ }
+ clt = 1.;
+ slt = ht / gt;
+ srt = 1.;
+ crt = ft / gt;
+ }
+ }
+ if (gasmal) {
+
+/* Normal case */
+
+ d__ = fa - ha;
+ if (d__ == fa) {
+
+/* Copes with infinite F or H */
+
+ l = 1.;
+ } else {
+ l = d__ / fa;
+ }
+
+/* Note that 0 .le. L .le. 1 */
+
+ m = gt / ft;
+
+/* Note that abs(M) .le. 1/macheps */
+
+ t = 2. - l;
+
+/* Note that T .ge. 1 */
+
+ mm = m * m;
+ tt = t * t;
+ s = sqrt(tt + mm);
+
+/* Note that 1 .le. S .le. 1 + 1/macheps */
+
+ if (l == 0.) {
+ r__ = abs(m);
+ } else {
+ r__ = sqrt(l * l + mm);
+ }
+
+/* Note that 0 .le. R .le. 1 + 1/macheps */
+
+ a = (s + r__) * .5;
+
+/* Note that 1 .le. A .le. 1 + abs(M) */
+
+ *ssmin = ha / a;
+ *ssmax = fa * a;
+ if (mm == 0.) {
+
+/* Note that M is very tiny */
+
+ if (l == 0.) {
+ t = d_sign(&c_b3, &ft) * d_sign(&c_b4, >);
+ } else {
+ t = gt / d_sign(&d__, &ft) + m / t;
+ }
+ } else {
+ t = (m / (s + t) + m / (r__ + l)) * (a + 1.);
+ }
+ l = sqrt(t * t + 4.);
+ crt = 2. / l;
+ srt = t / l;
+ clt = (crt + srt * m) / a;
+ slt = ht / ft * srt / a;
+ }
+ }
+ if (swap) {
+ *csl = srt;
+ *snl = crt;
+ *csr = slt;
+ *snr = clt;
+ } else {
+ *csl = clt;
+ *snl = slt;
+ *csr = crt;
+ *snr = srt;
+ }
+
+/* Correct signs of SSMAX and SSMIN */
+
+ if (pmax == 1) {
+ tsign = d_sign(&c_b4, csr) * d_sign(&c_b4, csl) * d_sign(&c_b4, f);
+ }
+ if (pmax == 2) {
+ tsign = d_sign(&c_b4, snr) * d_sign(&c_b4, csl) * d_sign(&c_b4, g);
+ }
+ if (pmax == 3) {
+ tsign = d_sign(&c_b4, snr) * d_sign(&c_b4, snl) * d_sign(&c_b4, h__);
+ }
+ *ssmax = d_sign(ssmax, &tsign);
+ d__1 = tsign * d_sign(&c_b4, f) * d_sign(&c_b4, h__);
+ *ssmin = d_sign(ssmin, &d__1);
+ return 0;
+
+/* End of DLASV2 */
+
+} /* dlasv2_ */
diff --git a/3rdparty/clapack/src/dlaswp.c b/3rdparty/clapack/src/dlaswp.c
new file mode 100644
index 0000000..862938b
--- /dev/null
+++ b/3rdparty/clapack/src/dlaswp.c
@@ -0,0 +1,158 @@
+/* dlaswp.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dlaswp_(integer *n, doublereal *a, integer *lda, integer
+ *k1, integer *k2, integer *ipiv, integer *incx)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer i__, j, k, i1, i2, n32, ip, ix, ix0, inc;
+ doublereal temp;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLASWP performs a series of row interchanges on the matrix A. */
+/* One row interchange is initiated for each of rows K1 through K2 of A. */
+
+/* Arguments */
+/* ========= */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the matrix of column dimension N to which the row */
+/* interchanges will be applied. */
+/* On exit, the permuted matrix. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. */
+
+/* K1 (input) INTEGER */
+/* The first element of IPIV for which a row interchange will */
+/* be done. */
+
+/* K2 (input) INTEGER */
+/* The last element of IPIV for which a row interchange will */
+/* be done. */
+
+/* IPIV (input) INTEGER array, dimension (K2*abs(INCX)) */
+/* The vector of pivot indices. Only the elements in positions */
+/* K1 through K2 of IPIV are accessed. */
+/* IPIV(K) = L implies rows K and L are to be interchanged. */
+
+/* INCX (input) INTEGER */
+/* The increment between successive values of IPIV. If IPIV */
+/* is negative, the pivots are applied in reverse order. */
+
+/* Further Details */
+/* =============== */
+
+/* Modified by */
+/* R. C. Whaley, Computer Science Dept., Univ. of Tenn., Knoxville, USA */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Interchange row I with row IPIV(I) for each of rows K1 through K2. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --ipiv;
+
+ /* Function Body */
+ if (*incx > 0) {
+ ix0 = *k1;
+ i1 = *k1;
+ i2 = *k2;
+ inc = 1;
+ } else if (*incx < 0) {
+ ix0 = (1 - *k2) * *incx + 1;
+ i1 = *k2;
+ i2 = *k1;
+ inc = -1;
+ } else {
+ return 0;
+ }
+
+ n32 = *n / 32 << 5;
+ if (n32 != 0) {
+ i__1 = n32;
+ for (j = 1; j <= i__1; j += 32) {
+ ix = ix0;
+ i__2 = i2;
+ i__3 = inc;
+ for (i__ = i1; i__3 < 0 ? i__ >= i__2 : i__ <= i__2; i__ += i__3)
+ {
+ ip = ipiv[ix];
+ if (ip != i__) {
+ i__4 = j + 31;
+ for (k = j; k <= i__4; ++k) {
+ temp = a[i__ + k * a_dim1];
+ a[i__ + k * a_dim1] = a[ip + k * a_dim1];
+ a[ip + k * a_dim1] = temp;
+/* L10: */
+ }
+ }
+ ix += *incx;
+/* L20: */
+ }
+/* L30: */
+ }
+ }
+ if (n32 != *n) {
+ ++n32;
+ ix = ix0;
+ i__1 = i2;
+ i__3 = inc;
+ for (i__ = i1; i__3 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__3) {
+ ip = ipiv[ix];
+ if (ip != i__) {
+ i__2 = *n;
+ for (k = n32; k <= i__2; ++k) {
+ temp = a[i__ + k * a_dim1];
+ a[i__ + k * a_dim1] = a[ip + k * a_dim1];
+ a[ip + k * a_dim1] = temp;
+/* L40: */
+ }
+ }
+ ix += *incx;
+/* L50: */
+ }
+ }
+
+ return 0;
+
+/* End of DLASWP */
+
+} /* dlaswp_ */
diff --git a/3rdparty/clapack/src/dlauu2.c b/3rdparty/clapack/src/dlauu2.c
new file mode 100644
index 0000000..1f8f03f
--- /dev/null
+++ b/3rdparty/clapack/src/dlauu2.c
@@ -0,0 +1,183 @@
+/* dlauu2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static doublereal c_b7 = 1.;
+static integer c__1 = 1;
+
+/* Subroutine */ int dlauu2_(char *uplo, integer *n, doublereal *a, integer *
+ lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__;
+ doublereal aii;
+ extern doublereal ddot_(integer *, doublereal *, integer *, doublereal *,
+ integer *);
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dgemv_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, doublereal *, integer *);
+ logical upper;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAUU2 computes the product U * U' or L' * L, where the triangular */
+/* factor U or L is stored in the upper or lower triangular part of */
+/* the array A. */
+
+/* If UPLO = 'U' or 'u' then the upper triangle of the result is stored, */
+/* overwriting the factor U in A. */
+/* If UPLO = 'L' or 'l' then the lower triangle of the result is stored, */
+/* overwriting the factor L in A. */
+
+/* This is the unblocked form of the algorithm, calling Level 2 BLAS. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies whether the triangular factor stored in the array A */
+/* is upper or lower triangular: */
+/* = 'U': Upper triangular */
+/* = 'L': Lower triangular */
+
+/* N (input) INTEGER */
+/* The order of the triangular factor U or L. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the triangular factor U or L. */
+/* On exit, if UPLO = 'U', the upper triangle of A is */
+/* overwritten with the upper triangle of the product U * U'; */
+/* if UPLO = 'L', the lower triangle of A is overwritten with */
+/* the lower triangle of the product L' * L. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -k, the k-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*n)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLAUU2", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+ if (upper) {
+
+/* Compute the product U * U'. */
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ aii = a[i__ + i__ * a_dim1];
+ if (i__ < *n) {
+ i__2 = *n - i__ + 1;
+ a[i__ + i__ * a_dim1] = ddot_(&i__2, &a[i__ + i__ * a_dim1],
+ lda, &a[i__ + i__ * a_dim1], lda);
+ i__2 = i__ - 1;
+ i__3 = *n - i__;
+ dgemv_("No transpose", &i__2, &i__3, &c_b7, &a[(i__ + 1) *
+ a_dim1 + 1], lda, &a[i__ + (i__ + 1) * a_dim1], lda, &
+ aii, &a[i__ * a_dim1 + 1], &c__1);
+ } else {
+ dscal_(&i__, &aii, &a[i__ * a_dim1 + 1], &c__1);
+ }
+/* L10: */
+ }
+
+ } else {
+
+/* Compute the product L' * L. */
+
+ i__1 = *n;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ aii = a[i__ + i__ * a_dim1];
+ if (i__ < *n) {
+ i__2 = *n - i__ + 1;
+ a[i__ + i__ * a_dim1] = ddot_(&i__2, &a[i__ + i__ * a_dim1], &
+ c__1, &a[i__ + i__ * a_dim1], &c__1);
+ i__2 = *n - i__;
+ i__3 = i__ - 1;
+ dgemv_("Transpose", &i__2, &i__3, &c_b7, &a[i__ + 1 + a_dim1],
+ lda, &a[i__ + 1 + i__ * a_dim1], &c__1, &aii, &a[i__
+ + a_dim1], lda);
+ } else {
+ dscal_(&i__, &aii, &a[i__ + a_dim1], lda);
+ }
+/* L20: */
+ }
+ }
+
+ return 0;
+
+/* End of DLAUU2 */
+
+} /* dlauu2_ */
diff --git a/3rdparty/clapack/src/dlauum.c b/3rdparty/clapack/src/dlauum.c
new file mode 100644
index 0000000..0d5d706
--- /dev/null
+++ b/3rdparty/clapack/src/dlauum.c
@@ -0,0 +1,217 @@
+/* dlauum.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static doublereal c_b15 = 1.;
+
+/* Subroutine */ int dlauum_(char *uplo, integer *n, doublereal *a, integer *
+ lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer i__, ib, nb;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dtrmm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *);
+ logical upper;
+ extern /* Subroutine */ int dsyrk_(char *, char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *), dlauu2_(char *, integer *,
+ doublereal *, integer *, integer *), xerbla_(char *,
+ integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DLAUUM computes the product U * U' or L' * L, where the triangular */
+/* factor U or L is stored in the upper or lower triangular part of */
+/* the array A. */
+
+/* If UPLO = 'U' or 'u' then the upper triangle of the result is stored, */
+/* overwriting the factor U in A. */
+/* If UPLO = 'L' or 'l' then the lower triangle of the result is stored, */
+/* overwriting the factor L in A. */
+
+/* This is the blocked form of the algorithm, calling Level 3 BLAS. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies whether the triangular factor stored in the array A */
+/* is upper or lower triangular: */
+/* = 'U': Upper triangular */
+/* = 'L': Lower triangular */
+
+/* N (input) INTEGER */
+/* The order of the triangular factor U or L. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the triangular factor U or L. */
+/* On exit, if UPLO = 'U', the upper triangle of A is */
+/* overwritten with the upper triangle of the product U * U'; */
+/* if UPLO = 'L', the lower triangle of A is overwritten with */
+/* the lower triangle of the product L' * L. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -k, the k-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*n)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DLAUUM", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+/* Determine the block size for this environment. */
+
+ nb = ilaenv_(&c__1, "DLAUUM", uplo, n, &c_n1, &c_n1, &c_n1);
+
+ if (nb <= 1 || nb >= *n) {
+
+/* Use unblocked code */
+
+ dlauu2_(uplo, n, &a[a_offset], lda, info);
+ } else {
+
+/* Use blocked code */
+
+ if (upper) {
+
+/* Compute the product U * U'. */
+
+ i__1 = *n;
+ i__2 = nb;
+ for (i__ = 1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+/* Computing MIN */
+ i__3 = nb, i__4 = *n - i__ + 1;
+ ib = min(i__3,i__4);
+ i__3 = i__ - 1;
+ dtrmm_("Right", "Upper", "Transpose", "Non-unit", &i__3, &ib,
+ &c_b15, &a[i__ + i__ * a_dim1], lda, &a[i__ * a_dim1
+ + 1], lda)
+ ;
+ dlauu2_("Upper", &ib, &a[i__ + i__ * a_dim1], lda, info);
+ if (i__ + ib <= *n) {
+ i__3 = i__ - 1;
+ i__4 = *n - i__ - ib + 1;
+ dgemm_("No transpose", "Transpose", &i__3, &ib, &i__4, &
+ c_b15, &a[(i__ + ib) * a_dim1 + 1], lda, &a[i__ +
+ (i__ + ib) * a_dim1], lda, &c_b15, &a[i__ *
+ a_dim1 + 1], lda);
+ i__3 = *n - i__ - ib + 1;
+ dsyrk_("Upper", "No transpose", &ib, &i__3, &c_b15, &a[
+ i__ + (i__ + ib) * a_dim1], lda, &c_b15, &a[i__ +
+ i__ * a_dim1], lda);
+ }
+/* L10: */
+ }
+ } else {
+
+/* Compute the product L' * L. */
+
+ i__2 = *n;
+ i__1 = nb;
+ for (i__ = 1; i__1 < 0 ? i__ >= i__2 : i__ <= i__2; i__ += i__1) {
+/* Computing MIN */
+ i__3 = nb, i__4 = *n - i__ + 1;
+ ib = min(i__3,i__4);
+ i__3 = i__ - 1;
+ dtrmm_("Left", "Lower", "Transpose", "Non-unit", &ib, &i__3, &
+ c_b15, &a[i__ + i__ * a_dim1], lda, &a[i__ + a_dim1],
+ lda);
+ dlauu2_("Lower", &ib, &a[i__ + i__ * a_dim1], lda, info);
+ if (i__ + ib <= *n) {
+ i__3 = i__ - 1;
+ i__4 = *n - i__ - ib + 1;
+ dgemm_("Transpose", "No transpose", &ib, &i__3, &i__4, &
+ c_b15, &a[i__ + ib + i__ * a_dim1], lda, &a[i__ +
+ ib + a_dim1], lda, &c_b15, &a[i__ + a_dim1], lda);
+ i__3 = *n - i__ - ib + 1;
+ dsyrk_("Lower", "Transpose", &ib, &i__3, &c_b15, &a[i__ +
+ ib + i__ * a_dim1], lda, &c_b15, &a[i__ + i__ *
+ a_dim1], lda);
+ }
+/* L20: */
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DLAUUM */
+
+} /* dlauum_ */
diff --git a/3rdparty/clapack/src/dorg2r.c b/3rdparty/clapack/src/dorg2r.c
new file mode 100644
index 0000000..892807c
--- /dev/null
+++ b/3rdparty/clapack/src/dorg2r.c
@@ -0,0 +1,175 @@
+/* dorg2r.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dorg2r_(integer *m, integer *n, integer *k, doublereal *
+ a, integer *lda, doublereal *tau, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__, j, l;
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *), dlarf_(char *, integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *), xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORG2R generates an m by n real matrix Q with orthonormal columns, */
+/* which is defined as the first n columns of a product of k elementary */
+/* reflectors of order m */
+
+/* Q = H(1) H(2) . . . H(k) */
+
+/* as returned by DGEQRF. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix Q. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix Q. M >= N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines the */
+/* matrix Q. N >= K >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the i-th column must contain the vector which */
+/* defines the elementary reflector H(i), for i = 1,2,...,k, as */
+/* returned by DGEQRF in the first k columns of its array */
+/* argument A. */
+/* On exit, the m-by-n matrix Q. */
+
+/* LDA (input) INTEGER */
+/* The first dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGEQRF. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (N) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument has an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0 || *n > *m) {
+ *info = -2;
+ } else if (*k < 0 || *k > *n) {
+ *info = -3;
+ } else if (*lda < max(1,*m)) {
+ *info = -5;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORG2R", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n <= 0) {
+ return 0;
+ }
+
+/* Initialise columns k+1:n to columns of the unit matrix */
+
+ i__1 = *n;
+ for (j = *k + 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (l = 1; l <= i__2; ++l) {
+ a[l + j * a_dim1] = 0.;
+/* L10: */
+ }
+ a[j + j * a_dim1] = 1.;
+/* L20: */
+ }
+
+ for (i__ = *k; i__ >= 1; --i__) {
+
+/* Apply H(i) to A(i:m,i:n) from the left */
+
+ if (i__ < *n) {
+ a[i__ + i__ * a_dim1] = 1.;
+ i__1 = *m - i__ + 1;
+ i__2 = *n - i__;
+ dlarf_("Left", &i__1, &i__2, &a[i__ + i__ * a_dim1], &c__1, &tau[
+ i__], &a[i__ + (i__ + 1) * a_dim1], lda, &work[1]);
+ }
+ if (i__ < *m) {
+ i__1 = *m - i__;
+ d__1 = -tau[i__];
+ dscal_(&i__1, &d__1, &a[i__ + 1 + i__ * a_dim1], &c__1);
+ }
+ a[i__ + i__ * a_dim1] = 1. - tau[i__];
+
+/* Set A(1:i-1,i) to zero */
+
+ i__1 = i__ - 1;
+ for (l = 1; l <= i__1; ++l) {
+ a[l + i__ * a_dim1] = 0.;
+/* L30: */
+ }
+/* L40: */
+ }
+ return 0;
+
+/* End of DORG2R */
+
+} /* dorg2r_ */
diff --git a/3rdparty/clapack/src/dorgbr.c b/3rdparty/clapack/src/dorgbr.c
new file mode 100644
index 0000000..e649640
--- /dev/null
+++ b/3rdparty/clapack/src/dorgbr.c
@@ -0,0 +1,299 @@
+/* dorgbr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+
+/* Subroutine */ int dorgbr_(char *vect, integer *m, integer *n, integer *k,
+ doublereal *a, integer *lda, doublereal *tau, doublereal *work,
+ integer *lwork, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, nb, mn;
+ extern logical lsame_(char *, char *);
+ integer iinfo;
+ logical wantq;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ extern /* Subroutine */ int dorglq_(integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ integer *), dorgqr_(integer *, integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *, integer *);
+ integer lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORGBR generates one of the real orthogonal matrices Q or P**T */
+/* determined by DGEBRD when reducing a real matrix A to bidiagonal */
+/* form: A = Q * B * P**T. Q and P**T are defined as products of */
+/* elementary reflectors H(i) or G(i) respectively. */
+
+/* If VECT = 'Q', A is assumed to have been an M-by-K matrix, and Q */
+/* is of order M: */
+/* if m >= k, Q = H(1) H(2) . . . H(k) and DORGBR returns the first n */
+/* columns of Q, where m >= n >= k; */
+/* if m < k, Q = H(1) H(2) . . . H(m-1) and DORGBR returns Q as an */
+/* M-by-M matrix. */
+
+/* If VECT = 'P', A is assumed to have been a K-by-N matrix, and P**T */
+/* is of order N: */
+/* if k < n, P**T = G(k) . . . G(2) G(1) and DORGBR returns the first m */
+/* rows of P**T, where n >= m >= k; */
+/* if k >= n, P**T = G(n-1) . . . G(2) G(1) and DORGBR returns P**T as */
+/* an N-by-N matrix. */
+
+/* Arguments */
+/* ========= */
+
+/* VECT (input) CHARACTER*1 */
+/* Specifies whether the matrix Q or the matrix P**T is */
+/* required, as defined in the transformation applied by DGEBRD: */
+/* = 'Q': generate Q; */
+/* = 'P': generate P**T. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix Q or P**T to be returned. */
+/* M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix Q or P**T to be returned. */
+/* N >= 0. */
+/* If VECT = 'Q', M >= N >= min(M,K); */
+/* if VECT = 'P', N >= M >= min(N,K). */
+
+/* K (input) INTEGER */
+/* If VECT = 'Q', the number of columns in the original M-by-K */
+/* matrix reduced by DGEBRD. */
+/* If VECT = 'P', the number of rows in the original K-by-N */
+/* matrix reduced by DGEBRD. */
+/* K >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the vectors which define the elementary reflectors, */
+/* as returned by DGEBRD. */
+/* On exit, the M-by-N matrix Q or P**T. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension */
+/* (min(M,K)) if VECT = 'Q' */
+/* (min(N,K)) if VECT = 'P' */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i) or G(i), which determines Q or P**T, as */
+/* returned by DGEBRD in its array argument TAUQ or TAUP. */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,min(M,N)). */
+/* For optimum performance LWORK >= min(M,N)*NB, where NB */
+/* is the optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ wantq = lsame_(vect, "Q");
+ mn = min(*m,*n);
+ lquery = *lwork == -1;
+ if (! wantq && ! lsame_(vect, "P")) {
+ *info = -1;
+ } else if (*m < 0) {
+ *info = -2;
+ } else if (*n < 0 || wantq && (*n > *m || *n < min(*m,*k)) || ! wantq && (
+ *m > *n || *m < min(*n,*k))) {
+ *info = -3;
+ } else if (*k < 0) {
+ *info = -4;
+ } else if (*lda < max(1,*m)) {
+ *info = -6;
+ } else if (*lwork < max(1,mn) && ! lquery) {
+ *info = -9;
+ }
+
+ if (*info == 0) {
+ if (wantq) {
+ nb = ilaenv_(&c__1, "DORGQR", " ", m, n, k, &c_n1);
+ } else {
+ nb = ilaenv_(&c__1, "DORGLQ", " ", m, n, k, &c_n1);
+ }
+ lwkopt = max(1,mn) * nb;
+ work[1] = (doublereal) lwkopt;
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORGBR", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ if (wantq) {
+
+/* Form Q, determined by a call to DGEBRD to reduce an m-by-k */
+/* matrix */
+
+ if (*m >= *k) {
+
+/* If m >= k, assume m >= n >= k */
+
+ dorgqr_(m, n, k, &a[a_offset], lda, &tau[1], &work[1], lwork, &
+ iinfo);
+
+ } else {
+
+/* If m < k, assume m = n */
+
+/* Shift the vectors which define the elementary reflectors one */
+/* column to the right, and set the first row and column of Q */
+/* to those of the unit matrix */
+
+ for (j = *m; j >= 2; --j) {
+ a[j * a_dim1 + 1] = 0.;
+ i__1 = *m;
+ for (i__ = j + 1; i__ <= i__1; ++i__) {
+ a[i__ + j * a_dim1] = a[i__ + (j - 1) * a_dim1];
+/* L10: */
+ }
+/* L20: */
+ }
+ a[a_dim1 + 1] = 1.;
+ i__1 = *m;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ a[i__ + a_dim1] = 0.;
+/* L30: */
+ }
+ if (*m > 1) {
+
+/* Form Q(2:m,2:m) */
+
+ i__1 = *m - 1;
+ i__2 = *m - 1;
+ i__3 = *m - 1;
+ dorgqr_(&i__1, &i__2, &i__3, &a[(a_dim1 << 1) + 2], lda, &tau[
+ 1], &work[1], lwork, &iinfo);
+ }
+ }
+ } else {
+
+/* Form P', determined by a call to DGEBRD to reduce a k-by-n */
+/* matrix */
+
+ if (*k < *n) {
+
+/* If k < n, assume k <= m <= n */
+
+ dorglq_(m, n, k, &a[a_offset], lda, &tau[1], &work[1], lwork, &
+ iinfo);
+
+ } else {
+
+/* If k >= n, assume m = n */
+
+/* Shift the vectors which define the elementary reflectors one */
+/* row downward, and set the first row and column of P' to */
+/* those of the unit matrix */
+
+ a[a_dim1 + 1] = 1.;
+ i__1 = *n;
+ for (i__ = 2; i__ <= i__1; ++i__) {
+ a[i__ + a_dim1] = 0.;
+/* L40: */
+ }
+ i__1 = *n;
+ for (j = 2; j <= i__1; ++j) {
+ for (i__ = j - 1; i__ >= 2; --i__) {
+ a[i__ + j * a_dim1] = a[i__ - 1 + j * a_dim1];
+/* L50: */
+ }
+ a[j * a_dim1 + 1] = 0.;
+/* L60: */
+ }
+ if (*n > 1) {
+
+/* Form P'(2:n,2:n) */
+
+ i__1 = *n - 1;
+ i__2 = *n - 1;
+ i__3 = *n - 1;
+ dorglq_(&i__1, &i__2, &i__3, &a[(a_dim1 << 1) + 2], lda, &tau[
+ 1], &work[1], lwork, &iinfo);
+ }
+ }
+ }
+ work[1] = (doublereal) lwkopt;
+ return 0;
+
+/* End of DORGBR */
+
+} /* dorgbr_ */
diff --git a/3rdparty/clapack/src/dorgl2.c b/3rdparty/clapack/src/dorgl2.c
new file mode 100644
index 0000000..f880992
--- /dev/null
+++ b/3rdparty/clapack/src/dorgl2.c
@@ -0,0 +1,175 @@
+/* dorgl2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dorgl2_(integer *m, integer *n, integer *k, doublereal *
+ a, integer *lda, doublereal *tau, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+ doublereal d__1;
+
+ /* Local variables */
+ integer i__, j, l;
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *), dlarf_(char *, integer *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *), xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORGL2 generates an m by n real matrix Q with orthonormal rows, */
+/* which is defined as the first m rows of a product of k elementary */
+/* reflectors of order n */
+
+/* Q = H(k) . . . H(2) H(1) */
+
+/* as returned by DGELQF. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix Q. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix Q. N >= M. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines the */
+/* matrix Q. M >= K >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the i-th row must contain the vector which defines */
+/* the elementary reflector H(i), for i = 1,2,...,k, as returned */
+/* by DGELQF in the first k rows of its array argument A. */
+/* On exit, the m-by-n matrix Q. */
+
+/* LDA (input) INTEGER */
+/* The first dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGELQF. */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension (M) */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument has an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < *m) {
+ *info = -2;
+ } else if (*k < 0 || *k > *m) {
+ *info = -3;
+ } else if (*lda < max(1,*m)) {
+ *info = -5;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORGL2", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m <= 0) {
+ return 0;
+ }
+
+ if (*k < *m) {
+
+/* Initialise rows k+1:m to rows of the unit matrix */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (l = *k + 1; l <= i__2; ++l) {
+ a[l + j * a_dim1] = 0.;
+/* L10: */
+ }
+ if (j > *k && j <= *m) {
+ a[j + j * a_dim1] = 1.;
+ }
+/* L20: */
+ }
+ }
+
+ for (i__ = *k; i__ >= 1; --i__) {
+
+/* Apply H(i) to A(i:m,i:n) from the right */
+
+ if (i__ < *n) {
+ if (i__ < *m) {
+ a[i__ + i__ * a_dim1] = 1.;
+ i__1 = *m - i__;
+ i__2 = *n - i__ + 1;
+ dlarf_("Right", &i__1, &i__2, &a[i__ + i__ * a_dim1], lda, &
+ tau[i__], &a[i__ + 1 + i__ * a_dim1], lda, &work[1]);
+ }
+ i__1 = *n - i__;
+ d__1 = -tau[i__];
+ dscal_(&i__1, &d__1, &a[i__ + (i__ + 1) * a_dim1], lda);
+ }
+ a[i__ + i__ * a_dim1] = 1. - tau[i__];
+
+/* Set A(i,1:i-1) to zero */
+
+ i__1 = i__ - 1;
+ for (l = 1; l <= i__1; ++l) {
+ a[i__ + l * a_dim1] = 0.;
+/* L30: */
+ }
+/* L40: */
+ }
+ return 0;
+
+/* End of DORGL2 */
+
+} /* dorgl2_ */
diff --git a/3rdparty/clapack/src/dorglq.c b/3rdparty/clapack/src/dorglq.c
new file mode 100644
index 0000000..a5b9019
--- /dev/null
+++ b/3rdparty/clapack/src/dorglq.c
@@ -0,0 +1,280 @@
+/* dorglq.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__3 = 3;
+static integer c__2 = 2;
+
+/* Subroutine */ int dorglq_(integer *m, integer *n, integer *k, doublereal *
+ a, integer *lda, doublereal *tau, doublereal *work, integer *lwork,
+ integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, l, ib, nb, ki, kk, nx, iws, nbmin, iinfo;
+ extern /* Subroutine */ int dorgl2_(integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *),
+ dlarfb_(char *, char *, char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwork, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORGLQ generates an M-by-N real matrix Q with orthonormal rows, */
+/* which is defined as the first M rows of a product of K elementary */
+/* reflectors of order N */
+
+/* Q = H(k) . . . H(2) H(1) */
+
+/* as returned by DGELQF. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix Q. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix Q. N >= M. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines the */
+/* matrix Q. M >= K >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the i-th row must contain the vector which defines */
+/* the elementary reflector H(i), for i = 1,2,...,k, as returned */
+/* by DGELQF in the first k rows of its array argument A. */
+/* On exit, the M-by-N matrix Q. */
+
+/* LDA (input) INTEGER */
+/* The first dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGELQF. */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,M). */
+/* For optimum performance LWORK >= M*NB, where NB is */
+/* the optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument has an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ nb = ilaenv_(&c__1, "DORGLQ", " ", m, n, k, &c_n1);
+ lwkopt = max(1,*m) * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < *m) {
+ *info = -2;
+ } else if (*k < 0 || *k > *m) {
+ *info = -3;
+ } else if (*lda < max(1,*m)) {
+ *info = -5;
+ } else if (*lwork < max(1,*m) && ! lquery) {
+ *info = -8;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORGLQ", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m <= 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ nx = 0;
+ iws = *m;
+ if (nb > 1 && nb < *k) {
+
+/* Determine when to cross over from blocked to unblocked code. */
+
+/* Computing MAX */
+ i__1 = 0, i__2 = ilaenv_(&c__3, "DORGLQ", " ", m, n, k, &c_n1);
+ nx = max(i__1,i__2);
+ if (nx < *k) {
+
+/* Determine if workspace is large enough for blocked code. */
+
+ ldwork = *m;
+ iws = ldwork * nb;
+ if (*lwork < iws) {
+
+/* Not enough workspace to use optimal NB: reduce NB and */
+/* determine the minimum value of NB. */
+
+ nb = *lwork / ldwork;
+/* Computing MAX */
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DORGLQ", " ", m, n, k, &c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ }
+ }
+
+ if (nb >= nbmin && nb < *k && nx < *k) {
+
+/* Use blocked code after the last block. */
+/* The first kk rows are handled by the block method. */
+
+ ki = (*k - nx - 1) / nb * nb;
+/* Computing MIN */
+ i__1 = *k, i__2 = ki + nb;
+ kk = min(i__1,i__2);
+
+/* Set A(kk+1:m,1:kk) to zero. */
+
+ i__1 = kk;
+ for (j = 1; j <= i__1; ++j) {
+ i__2 = *m;
+ for (i__ = kk + 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ } else {
+ kk = 0;
+ }
+
+/* Use unblocked code for the last or only block. */
+
+ if (kk < *m) {
+ i__1 = *m - kk;
+ i__2 = *n - kk;
+ i__3 = *k - kk;
+ dorgl2_(&i__1, &i__2, &i__3, &a[kk + 1 + (kk + 1) * a_dim1], lda, &
+ tau[kk + 1], &work[1], &iinfo);
+ }
+
+ if (kk > 0) {
+
+/* Use blocked code */
+
+ i__1 = -nb;
+ for (i__ = ki + 1; i__1 < 0 ? i__ >= 1 : i__ <= 1; i__ += i__1) {
+/* Computing MIN */
+ i__2 = nb, i__3 = *k - i__ + 1;
+ ib = min(i__2,i__3);
+ if (i__ + ib <= *m) {
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__2 = *n - i__ + 1;
+ dlarft_("Forward", "Rowwise", &i__2, &ib, &a[i__ + i__ *
+ a_dim1], lda, &tau[i__], &work[1], &ldwork);
+
+/* Apply H' to A(i+ib:m,i:n) from the right */
+
+ i__2 = *m - i__ - ib + 1;
+ i__3 = *n - i__ + 1;
+ dlarfb_("Right", "Transpose", "Forward", "Rowwise", &i__2, &
+ i__3, &ib, &a[i__ + i__ * a_dim1], lda, &work[1], &
+ ldwork, &a[i__ + ib + i__ * a_dim1], lda, &work[ib +
+ 1], &ldwork);
+ }
+
+/* Apply H' to columns i:n of current block */
+
+ i__2 = *n - i__ + 1;
+ dorgl2_(&ib, &i__2, &ib, &a[i__ + i__ * a_dim1], lda, &tau[i__], &
+ work[1], &iinfo);
+
+/* Set columns 1:i-1 of current block to zero */
+
+ i__2 = i__ - 1;
+ for (j = 1; j <= i__2; ++j) {
+ i__3 = i__ + ib - 1;
+ for (l = i__; l <= i__3; ++l) {
+ a[l + j * a_dim1] = 0.;
+/* L30: */
+ }
+/* L40: */
+ }
+/* L50: */
+ }
+ }
+
+ work[1] = (doublereal) iws;
+ return 0;
+
+/* End of DORGLQ */
+
+} /* dorglq_ */
diff --git a/3rdparty/clapack/src/dorgqr.c b/3rdparty/clapack/src/dorgqr.c
new file mode 100644
index 0000000..3b72b73
--- /dev/null
+++ b/3rdparty/clapack/src/dorgqr.c
@@ -0,0 +1,281 @@
+/* dorgqr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__3 = 3;
+static integer c__2 = 2;
+
+/* Subroutine */ int dorgqr_(integer *m, integer *n, integer *k, doublereal *
+ a, integer *lda, doublereal *tau, doublereal *work, integer *lwork,
+ integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+
+ /* Local variables */
+ integer i__, j, l, ib, nb, ki, kk, nx, iws, nbmin, iinfo;
+ extern /* Subroutine */ int dorg2r_(integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *),
+ dlarfb_(char *, char *, char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ integer ldwork, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORGQR generates an M-by-N real matrix Q with orthonormal columns, */
+/* which is defined as the first N columns of a product of K elementary */
+/* reflectors of order M */
+
+/* Q = H(1) H(2) . . . H(k) */
+
+/* as returned by DGEQRF. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix Q. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix Q. M >= N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines the */
+/* matrix Q. N >= K >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the i-th column must contain the vector which */
+/* defines the elementary reflector H(i), for i = 1,2,...,k, as */
+/* returned by DGEQRF in the first k columns of its array */
+/* argument A. */
+/* On exit, the M-by-N matrix Q. */
+
+/* LDA (input) INTEGER */
+/* The first dimension of the array A. LDA >= max(1,M). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGEQRF. */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. LWORK >= max(1,N). */
+/* For optimum performance LWORK >= N*NB, where NB is the */
+/* optimal blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument has an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ nb = ilaenv_(&c__1, "DORGQR", " ", m, n, k, &c_n1);
+ lwkopt = max(1,*n) * nb;
+ work[1] = (doublereal) lwkopt;
+ lquery = *lwork == -1;
+ if (*m < 0) {
+ *info = -1;
+ } else if (*n < 0 || *n > *m) {
+ *info = -2;
+ } else if (*k < 0 || *k > *n) {
+ *info = -3;
+ } else if (*lda < max(1,*m)) {
+ *info = -5;
+ } else if (*lwork < max(1,*n) && ! lquery) {
+ *info = -8;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORGQR", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n <= 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ nx = 0;
+ iws = *n;
+ if (nb > 1 && nb < *k) {
+
+/* Determine when to cross over from blocked to unblocked code. */
+
+/* Computing MAX */
+ i__1 = 0, i__2 = ilaenv_(&c__3, "DORGQR", " ", m, n, k, &c_n1);
+ nx = max(i__1,i__2);
+ if (nx < *k) {
+
+/* Determine if workspace is large enough for blocked code. */
+
+ ldwork = *n;
+ iws = ldwork * nb;
+ if (*lwork < iws) {
+
+/* Not enough workspace to use optimal NB: reduce NB and */
+/* determine the minimum value of NB. */
+
+ nb = *lwork / ldwork;
+/* Computing MAX */
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DORGQR", " ", m, n, k, &c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ }
+ }
+
+ if (nb >= nbmin && nb < *k && nx < *k) {
+
+/* Use blocked code after the last block. */
+/* The first kk columns are handled by the block method. */
+
+ ki = (*k - nx - 1) / nb * nb;
+/* Computing MIN */
+ i__1 = *k, i__2 = ki + nb;
+ kk = min(i__1,i__2);
+
+/* Set A(1:kk,kk+1:n) to zero. */
+
+ i__1 = *n;
+ for (j = kk + 1; j <= i__1; ++j) {
+ i__2 = kk;
+ for (i__ = 1; i__ <= i__2; ++i__) {
+ a[i__ + j * a_dim1] = 0.;
+/* L10: */
+ }
+/* L20: */
+ }
+ } else {
+ kk = 0;
+ }
+
+/* Use unblocked code for the last or only block. */
+
+ if (kk < *n) {
+ i__1 = *m - kk;
+ i__2 = *n - kk;
+ i__3 = *k - kk;
+ dorg2r_(&i__1, &i__2, &i__3, &a[kk + 1 + (kk + 1) * a_dim1], lda, &
+ tau[kk + 1], &work[1], &iinfo);
+ }
+
+ if (kk > 0) {
+
+/* Use blocked code */
+
+ i__1 = -nb;
+ for (i__ = ki + 1; i__1 < 0 ? i__ >= 1 : i__ <= 1; i__ += i__1) {
+/* Computing MIN */
+ i__2 = nb, i__3 = *k - i__ + 1;
+ ib = min(i__2,i__3);
+ if (i__ + ib <= *n) {
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__2 = *m - i__ + 1;
+ dlarft_("Forward", "Columnwise", &i__2, &ib, &a[i__ + i__ *
+ a_dim1], lda, &tau[i__], &work[1], &ldwork);
+
+/* Apply H to A(i:m,i+ib:n) from the left */
+
+ i__2 = *m - i__ + 1;
+ i__3 = *n - i__ - ib + 1;
+ dlarfb_("Left", "No transpose", "Forward", "Columnwise", &
+ i__2, &i__3, &ib, &a[i__ + i__ * a_dim1], lda, &work[
+ 1], &ldwork, &a[i__ + (i__ + ib) * a_dim1], lda, &
+ work[ib + 1], &ldwork);
+ }
+
+/* Apply H to rows i:m of current block */
+
+ i__2 = *m - i__ + 1;
+ dorg2r_(&i__2, &ib, &ib, &a[i__ + i__ * a_dim1], lda, &tau[i__], &
+ work[1], &iinfo);
+
+/* Set rows 1:i-1 of current block to zero */
+
+ i__2 = i__ + ib - 1;
+ for (j = i__; j <= i__2; ++j) {
+ i__3 = i__ - 1;
+ for (l = 1; l <= i__3; ++l) {
+ a[l + j * a_dim1] = 0.;
+/* L30: */
+ }
+/* L40: */
+ }
+/* L50: */
+ }
+ }
+
+ work[1] = (doublereal) iws;
+ return 0;
+
+/* End of DORGQR */
+
+} /* dorgqr_ */
diff --git a/3rdparty/clapack/src/dorm2r.c b/3rdparty/clapack/src/dorm2r.c
new file mode 100644
index 0000000..3a71756
--- /dev/null
+++ b/3rdparty/clapack/src/dorm2r.c
@@ -0,0 +1,235 @@
+/* dorm2r.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dorm2r_(char *side, char *trans, integer *m, integer *n,
+ integer *k, doublereal *a, integer *lda, doublereal *tau, doublereal *
+ c__, integer *ldc, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, i1, i2, i3, ic, jc, mi, ni, nq;
+ doublereal aii;
+ logical left;
+ extern /* Subroutine */ int dlarf_(char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ logical notran;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORM2R overwrites the general real m by n matrix C with */
+
+/* Q * C if SIDE = 'L' and TRANS = 'N', or */
+
+/* Q'* C if SIDE = 'L' and TRANS = 'T', or */
+
+/* C * Q if SIDE = 'R' and TRANS = 'N', or */
+
+/* C * Q' if SIDE = 'R' and TRANS = 'T', */
+
+/* where Q is a real orthogonal matrix defined as the product of k */
+/* elementary reflectors */
+
+/* Q = H(1) H(2) . . . H(k) */
+
+/* as returned by DGEQRF. Q is of order m if SIDE = 'L' and of order n */
+/* if SIDE = 'R'. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply Q or Q' from the Left */
+/* = 'R': apply Q or Q' from the Right */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': apply Q (No transpose) */
+/* = 'T': apply Q' (Transpose) */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines */
+/* the matrix Q. */
+/* If SIDE = 'L', M >= K >= 0; */
+/* if SIDE = 'R', N >= K >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,K) */
+/* The i-th column must contain the vector which defines the */
+/* elementary reflector H(i), for i = 1,2,...,k, as returned by */
+/* DGEQRF in the first k columns of its array argument A. */
+/* A is modified by the routine but restored on exit. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. */
+/* If SIDE = 'L', LDA >= max(1,M); */
+/* if SIDE = 'R', LDA >= max(1,N). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGEQRF. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the m by n matrix C. */
+/* On exit, C is overwritten by Q*C or Q'*C or C*Q' or C*Q. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension */
+/* (N) if SIDE = 'L', */
+/* (M) if SIDE = 'R' */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ left = lsame_(side, "L");
+ notran = lsame_(trans, "N");
+
+/* NQ is the order of Q */
+
+ if (left) {
+ nq = *m;
+ } else {
+ nq = *n;
+ }
+ if (! left && ! lsame_(side, "R")) {
+ *info = -1;
+ } else if (! notran && ! lsame_(trans, "T")) {
+ *info = -2;
+ } else if (*m < 0) {
+ *info = -3;
+ } else if (*n < 0) {
+ *info = -4;
+ } else if (*k < 0 || *k > nq) {
+ *info = -5;
+ } else if (*lda < max(1,nq)) {
+ *info = -7;
+ } else if (*ldc < max(1,*m)) {
+ *info = -10;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORM2R", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0 || *k == 0) {
+ return 0;
+ }
+
+ if (left && ! notran || ! left && notran) {
+ i1 = 1;
+ i2 = *k;
+ i3 = 1;
+ } else {
+ i1 = *k;
+ i2 = 1;
+ i3 = -1;
+ }
+
+ if (left) {
+ ni = *n;
+ jc = 1;
+ } else {
+ mi = *m;
+ ic = 1;
+ }
+
+ i__1 = i2;
+ i__2 = i3;
+ for (i__ = i1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+ if (left) {
+
+/* H(i) is applied to C(i:m,1:n) */
+
+ mi = *m - i__ + 1;
+ ic = i__;
+ } else {
+
+/* H(i) is applied to C(1:m,i:n) */
+
+ ni = *n - i__ + 1;
+ jc = i__;
+ }
+
+/* Apply H(i) */
+
+ aii = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+ dlarf_(side, &mi, &ni, &a[i__ + i__ * a_dim1], &c__1, &tau[i__], &c__[
+ ic + jc * c_dim1], ldc, &work[1]);
+ a[i__ + i__ * a_dim1] = aii;
+/* L10: */
+ }
+ return 0;
+
+/* End of DORM2R */
+
+} /* dorm2r_ */
diff --git a/3rdparty/clapack/src/dormbr.c b/3rdparty/clapack/src/dormbr.c
new file mode 100644
index 0000000..ca7e42a
--- /dev/null
+++ b/3rdparty/clapack/src/dormbr.c
@@ -0,0 +1,360 @@
+/* dormbr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__2 = 2;
+
+/* Subroutine */ int dormbr_(char *vect, char *side, char *trans, integer *m,
+ integer *n, integer *k, doublereal *a, integer *lda, doublereal *tau,
+ doublereal *c__, integer *ldc, doublereal *work, integer *lwork,
+ integer *info)
+{
+ /* System generated locals */
+ address a__1[2];
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2, i__3[2];
+ char ch__1[2];
+
+ /* Builtin functions */
+ /* Subroutine */ int s_cat(char *, char **, integer *, integer *, ftnlen);
+
+ /* Local variables */
+ integer i1, i2, nb, mi, ni, nq, nw;
+ logical left;
+ extern logical lsame_(char *, char *);
+ integer iinfo;
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ extern /* Subroutine */ int dormlq_(char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, integer *);
+ logical notran;
+ extern /* Subroutine */ int dormqr_(char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *, integer *);
+ logical applyq;
+ char transt[1];
+ integer lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* If VECT = 'Q', DORMBR overwrites the general real M-by-N matrix C */
+/* with */
+/* SIDE = 'L' SIDE = 'R' */
+/* TRANS = 'N': Q * C C * Q */
+/* TRANS = 'T': Q**T * C C * Q**T */
+
+/* If VECT = 'P', DORMBR overwrites the general real M-by-N matrix C */
+/* with */
+/* SIDE = 'L' SIDE = 'R' */
+/* TRANS = 'N': P * C C * P */
+/* TRANS = 'T': P**T * C C * P**T */
+
+/* Here Q and P**T are the orthogonal matrices determined by DGEBRD when */
+/* reducing a real matrix A to bidiagonal form: A = Q * B * P**T. Q and */
+/* P**T are defined as products of elementary reflectors H(i) and G(i) */
+/* respectively. */
+
+/* Let nq = m if SIDE = 'L' and nq = n if SIDE = 'R'. Thus nq is the */
+/* order of the orthogonal matrix Q or P**T that is applied. */
+
+/* If VECT = 'Q', A is assumed to have been an NQ-by-K matrix: */
+/* if nq >= k, Q = H(1) H(2) . . . H(k); */
+/* if nq < k, Q = H(1) H(2) . . . H(nq-1). */
+
+/* If VECT = 'P', A is assumed to have been a K-by-NQ matrix: */
+/* if k < nq, P = G(1) G(2) . . . G(k); */
+/* if k >= nq, P = G(1) G(2) . . . G(nq-1). */
+
+/* Arguments */
+/* ========= */
+
+/* VECT (input) CHARACTER*1 */
+/* = 'Q': apply Q or Q**T; */
+/* = 'P': apply P or P**T. */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply Q, Q**T, P or P**T from the Left; */
+/* = 'R': apply Q, Q**T, P or P**T from the Right. */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': No transpose, apply Q or P; */
+/* = 'T': Transpose, apply Q**T or P**T. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. N >= 0. */
+
+/* K (input) INTEGER */
+/* If VECT = 'Q', the number of columns in the original */
+/* matrix reduced by DGEBRD. */
+/* If VECT = 'P', the number of rows in the original */
+/* matrix reduced by DGEBRD. */
+/* K >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension */
+/* (LDA,min(nq,K)) if VECT = 'Q' */
+/* (LDA,nq) if VECT = 'P' */
+/* The vectors which define the elementary reflectors H(i) and */
+/* G(i), whose products determine the matrices Q and P, as */
+/* returned by DGEBRD. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. */
+/* If VECT = 'Q', LDA >= max(1,nq); */
+/* if VECT = 'P', LDA >= max(1,min(nq,K)). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (min(nq,K)) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i) or G(i) which determines Q or P, as returned */
+/* by DGEBRD in the array argument TAUQ or TAUP. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the M-by-N matrix C. */
+/* On exit, C is overwritten by Q*C or Q**T*C or C*Q**T or C*Q */
+/* or P*C or P**T*C or C*P or C*P**T. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. */
+/* If SIDE = 'L', LWORK >= max(1,N); */
+/* if SIDE = 'R', LWORK >= max(1,M). */
+/* For optimum performance LWORK >= N*NB if SIDE = 'L', and */
+/* LWORK >= M*NB if SIDE = 'R', where NB is the optimal */
+/* blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ applyq = lsame_(vect, "Q");
+ left = lsame_(side, "L");
+ notran = lsame_(trans, "N");
+ lquery = *lwork == -1;
+
+/* NQ is the order of Q or P and NW is the minimum dimension of WORK */
+
+ if (left) {
+ nq = *m;
+ nw = *n;
+ } else {
+ nq = *n;
+ nw = *m;
+ }
+ if (! applyq && ! lsame_(vect, "P")) {
+ *info = -1;
+ } else if (! left && ! lsame_(side, "R")) {
+ *info = -2;
+ } else if (! notran && ! lsame_(trans, "T")) {
+ *info = -3;
+ } else if (*m < 0) {
+ *info = -4;
+ } else if (*n < 0) {
+ *info = -5;
+ } else if (*k < 0) {
+ *info = -6;
+ } else /* if(complicated condition) */ {
+/* Computing MAX */
+ i__1 = 1, i__2 = min(nq,*k);
+ if (applyq && *lda < max(1,nq) || ! applyq && *lda < max(i__1,i__2)) {
+ *info = -8;
+ } else if (*ldc < max(1,*m)) {
+ *info = -11;
+ } else if (*lwork < max(1,nw) && ! lquery) {
+ *info = -13;
+ }
+ }
+
+ if (*info == 0) {
+ if (applyq) {
+ if (left) {
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = *m - 1;
+ i__2 = *m - 1;
+ nb = ilaenv_(&c__1, "DORMQR", ch__1, &i__1, n, &i__2, &c_n1);
+ } else {
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = *n - 1;
+ i__2 = *n - 1;
+ nb = ilaenv_(&c__1, "DORMQR", ch__1, m, &i__1, &i__2, &c_n1);
+ }
+ } else {
+ if (left) {
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = *m - 1;
+ i__2 = *m - 1;
+ nb = ilaenv_(&c__1, "DORMLQ", ch__1, &i__1, n, &i__2, &c_n1);
+ } else {
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = *n - 1;
+ i__2 = *n - 1;
+ nb = ilaenv_(&c__1, "DORMLQ", ch__1, m, &i__1, &i__2, &c_n1);
+ }
+ }
+ lwkopt = max(1,nw) * nb;
+ work[1] = (doublereal) lwkopt;
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORMBR", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ work[1] = 1.;
+ if (*m == 0 || *n == 0) {
+ return 0;
+ }
+
+ if (applyq) {
+
+/* Apply Q */
+
+ if (nq >= *k) {
+
+/* Q was determined by a call to DGEBRD with nq >= k */
+
+ dormqr_(side, trans, m, n, k, &a[a_offset], lda, &tau[1], &c__[
+ c_offset], ldc, &work[1], lwork, &iinfo);
+ } else if (nq > 1) {
+
+/* Q was determined by a call to DGEBRD with nq < k */
+
+ if (left) {
+ mi = *m - 1;
+ ni = *n;
+ i1 = 2;
+ i2 = 1;
+ } else {
+ mi = *m;
+ ni = *n - 1;
+ i1 = 1;
+ i2 = 2;
+ }
+ i__1 = nq - 1;
+ dormqr_(side, trans, &mi, &ni, &i__1, &a[a_dim1 + 2], lda, &tau[1]
+, &c__[i1 + i2 * c_dim1], ldc, &work[1], lwork, &iinfo);
+ }
+ } else {
+
+/* Apply P */
+
+ if (notran) {
+ *(unsigned char *)transt = 'T';
+ } else {
+ *(unsigned char *)transt = 'N';
+ }
+ if (nq > *k) {
+
+/* P was determined by a call to DGEBRD with nq > k */
+
+ dormlq_(side, transt, m, n, k, &a[a_offset], lda, &tau[1], &c__[
+ c_offset], ldc, &work[1], lwork, &iinfo);
+ } else if (nq > 1) {
+
+/* P was determined by a call to DGEBRD with nq <= k */
+
+ if (left) {
+ mi = *m - 1;
+ ni = *n;
+ i1 = 2;
+ i2 = 1;
+ } else {
+ mi = *m;
+ ni = *n - 1;
+ i1 = 1;
+ i2 = 2;
+ }
+ i__1 = nq - 1;
+ dormlq_(side, transt, &mi, &ni, &i__1, &a[(a_dim1 << 1) + 1], lda,
+ &tau[1], &c__[i1 + i2 * c_dim1], ldc, &work[1], lwork, &
+ iinfo);
+ }
+ }
+ work[1] = (doublereal) lwkopt;
+ return 0;
+
+/* End of DORMBR */
+
+} /* dormbr_ */
diff --git a/3rdparty/clapack/src/dorml2.c b/3rdparty/clapack/src/dorml2.c
new file mode 100644
index 0000000..d482168
--- /dev/null
+++ b/3rdparty/clapack/src/dorml2.c
@@ -0,0 +1,231 @@
+/* dorml2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dorml2_(char *side, char *trans, integer *m, integer *n,
+ integer *k, doublereal *a, integer *lda, doublereal *tau, doublereal *
+ c__, integer *ldc, doublereal *work, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2;
+
+ /* Local variables */
+ integer i__, i1, i2, i3, ic, jc, mi, ni, nq;
+ doublereal aii;
+ logical left;
+ extern /* Subroutine */ int dlarf_(char *, integer *, integer *,
+ doublereal *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+ logical notran;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORML2 overwrites the general real m by n matrix C with */
+
+/* Q * C if SIDE = 'L' and TRANS = 'N', or */
+
+/* Q'* C if SIDE = 'L' and TRANS = 'T', or */
+
+/* C * Q if SIDE = 'R' and TRANS = 'N', or */
+
+/* C * Q' if SIDE = 'R' and TRANS = 'T', */
+
+/* where Q is a real orthogonal matrix defined as the product of k */
+/* elementary reflectors */
+
+/* Q = H(k) . . . H(2) H(1) */
+
+/* as returned by DGELQF. Q is of order m if SIDE = 'L' and of order n */
+/* if SIDE = 'R'. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply Q or Q' from the Left */
+/* = 'R': apply Q or Q' from the Right */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': apply Q (No transpose) */
+/* = 'T': apply Q' (Transpose) */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines */
+/* the matrix Q. */
+/* If SIDE = 'L', M >= K >= 0; */
+/* if SIDE = 'R', N >= K >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension */
+/* (LDA,M) if SIDE = 'L', */
+/* (LDA,N) if SIDE = 'R' */
+/* The i-th row must contain the vector which defines the */
+/* elementary reflector H(i), for i = 1,2,...,k, as returned by */
+/* DGELQF in the first k rows of its array argument A. */
+/* A is modified by the routine but restored on exit. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,K). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGELQF. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the m by n matrix C. */
+/* On exit, C is overwritten by Q*C or Q'*C or C*Q' or C*Q. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace) DOUBLE PRECISION array, dimension */
+/* (N) if SIDE = 'L', */
+/* (M) if SIDE = 'R' */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ left = lsame_(side, "L");
+ notran = lsame_(trans, "N");
+
+/* NQ is the order of Q */
+
+ if (left) {
+ nq = *m;
+ } else {
+ nq = *n;
+ }
+ if (! left && ! lsame_(side, "R")) {
+ *info = -1;
+ } else if (! notran && ! lsame_(trans, "T")) {
+ *info = -2;
+ } else if (*m < 0) {
+ *info = -3;
+ } else if (*n < 0) {
+ *info = -4;
+ } else if (*k < 0 || *k > nq) {
+ *info = -5;
+ } else if (*lda < max(1,*k)) {
+ *info = -7;
+ } else if (*ldc < max(1,*m)) {
+ *info = -10;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORML2", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0 || *k == 0) {
+ return 0;
+ }
+
+ if (left && notran || ! left && ! notran) {
+ i1 = 1;
+ i2 = *k;
+ i3 = 1;
+ } else {
+ i1 = *k;
+ i2 = 1;
+ i3 = -1;
+ }
+
+ if (left) {
+ ni = *n;
+ jc = 1;
+ } else {
+ mi = *m;
+ ic = 1;
+ }
+
+ i__1 = i2;
+ i__2 = i3;
+ for (i__ = i1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+ if (left) {
+
+/* H(i) is applied to C(i:m,1:n) */
+
+ mi = *m - i__ + 1;
+ ic = i__;
+ } else {
+
+/* H(i) is applied to C(1:m,i:n) */
+
+ ni = *n - i__ + 1;
+ jc = i__;
+ }
+
+/* Apply H(i) */
+
+ aii = a[i__ + i__ * a_dim1];
+ a[i__ + i__ * a_dim1] = 1.;
+ dlarf_(side, &mi, &ni, &a[i__ + i__ * a_dim1], lda, &tau[i__], &c__[
+ ic + jc * c_dim1], ldc, &work[1]);
+ a[i__ + i__ * a_dim1] = aii;
+/* L10: */
+ }
+ return 0;
+
+/* End of DORML2 */
+
+} /* dorml2_ */
diff --git a/3rdparty/clapack/src/dormlq.c b/3rdparty/clapack/src/dormlq.c
new file mode 100644
index 0000000..c0d4b7b
--- /dev/null
+++ b/3rdparty/clapack/src/dormlq.c
@@ -0,0 +1,334 @@
+/* dormlq.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__2 = 2;
+static integer c__65 = 65;
+
+/* Subroutine */ int dormlq_(char *side, char *trans, integer *m, integer *n,
+ integer *k, doublereal *a, integer *lda, doublereal *tau, doublereal *
+ c__, integer *ldc, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ address a__1[2];
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2, i__3[2], i__4,
+ i__5;
+ char ch__1[2];
+
+ /* Builtin functions */
+ /* Subroutine */ int s_cat(char *, char **, integer *, integer *, ftnlen);
+
+ /* Local variables */
+ integer i__;
+ doublereal t[4160] /* was [65][64] */;
+ integer i1, i2, i3, ib, ic, jc, nb, mi, ni, nq, nw, iws;
+ logical left;
+ extern logical lsame_(char *, char *);
+ integer nbmin, iinfo;
+ extern /* Subroutine */ int dorml2_(char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *), dlarfb_(char
+ *, char *, char *, char *, integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *, doublereal
+ *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ logical notran;
+ integer ldwork;
+ char transt[1];
+ integer lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORMLQ overwrites the general real M-by-N matrix C with */
+
+/* SIDE = 'L' SIDE = 'R' */
+/* TRANS = 'N': Q * C C * Q */
+/* TRANS = 'T': Q**T * C C * Q**T */
+
+/* where Q is a real orthogonal matrix defined as the product of k */
+/* elementary reflectors */
+
+/* Q = H(k) . . . H(2) H(1) */
+
+/* as returned by DGELQF. Q is of order M if SIDE = 'L' and of order N */
+/* if SIDE = 'R'. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply Q or Q**T from the Left; */
+/* = 'R': apply Q or Q**T from the Right. */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': No transpose, apply Q; */
+/* = 'T': Transpose, apply Q**T. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines */
+/* the matrix Q. */
+/* If SIDE = 'L', M >= K >= 0; */
+/* if SIDE = 'R', N >= K >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension */
+/* (LDA,M) if SIDE = 'L', */
+/* (LDA,N) if SIDE = 'R' */
+/* The i-th row must contain the vector which defines the */
+/* elementary reflector H(i), for i = 1,2,...,k, as returned by */
+/* DGELQF in the first k rows of its array argument A. */
+/* A is modified by the routine but restored on exit. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,K). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGELQF. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the M-by-N matrix C. */
+/* On exit, C is overwritten by Q*C or Q**T*C or C*Q**T or C*Q. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. */
+/* If SIDE = 'L', LWORK >= max(1,N); */
+/* if SIDE = 'R', LWORK >= max(1,M). */
+/* For optimum performance LWORK >= N*NB if SIDE = 'L', and */
+/* LWORK >= M*NB if SIDE = 'R', where NB is the optimal */
+/* blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ left = lsame_(side, "L");
+ notran = lsame_(trans, "N");
+ lquery = *lwork == -1;
+
+/* NQ is the order of Q and NW is the minimum dimension of WORK */
+
+ if (left) {
+ nq = *m;
+ nw = *n;
+ } else {
+ nq = *n;
+ nw = *m;
+ }
+ if (! left && ! lsame_(side, "R")) {
+ *info = -1;
+ } else if (! notran && ! lsame_(trans, "T")) {
+ *info = -2;
+ } else if (*m < 0) {
+ *info = -3;
+ } else if (*n < 0) {
+ *info = -4;
+ } else if (*k < 0 || *k > nq) {
+ *info = -5;
+ } else if (*lda < max(1,*k)) {
+ *info = -7;
+ } else if (*ldc < max(1,*m)) {
+ *info = -10;
+ } else if (*lwork < max(1,nw) && ! lquery) {
+ *info = -12;
+ }
+
+ if (*info == 0) {
+
+/* Determine the block size. NB may be at most NBMAX, where NBMAX */
+/* is used to define the local array T. */
+
+/* Computing MIN */
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = 64, i__2 = ilaenv_(&c__1, "DORMLQ", ch__1, m, n, k, &c_n1);
+ nb = min(i__1,i__2);
+ lwkopt = max(1,nw) * nb;
+ work[1] = (doublereal) lwkopt;
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORMLQ", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0 || *k == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ ldwork = nw;
+ if (nb > 1 && nb < *k) {
+ iws = nw * nb;
+ if (*lwork < iws) {
+ nb = *lwork / ldwork;
+/* Computing MAX */
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DORMLQ", ch__1, m, n, k, &c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ } else {
+ iws = nw;
+ }
+
+ if (nb < nbmin || nb >= *k) {
+
+/* Use unblocked code */
+
+ dorml2_(side, trans, m, n, k, &a[a_offset], lda, &tau[1], &c__[
+ c_offset], ldc, &work[1], &iinfo);
+ } else {
+
+/* Use blocked code */
+
+ if (left && notran || ! left && ! notran) {
+ i1 = 1;
+ i2 = *k;
+ i3 = nb;
+ } else {
+ i1 = (*k - 1) / nb * nb + 1;
+ i2 = 1;
+ i3 = -nb;
+ }
+
+ if (left) {
+ ni = *n;
+ jc = 1;
+ } else {
+ mi = *m;
+ ic = 1;
+ }
+
+ if (notran) {
+ *(unsigned char *)transt = 'T';
+ } else {
+ *(unsigned char *)transt = 'N';
+ }
+
+ i__1 = i2;
+ i__2 = i3;
+ for (i__ = i1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+/* Computing MIN */
+ i__4 = nb, i__5 = *k - i__ + 1;
+ ib = min(i__4,i__5);
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__4 = nq - i__ + 1;
+ dlarft_("Forward", "Rowwise", &i__4, &ib, &a[i__ + i__ * a_dim1],
+ lda, &tau[i__], t, &c__65);
+ if (left) {
+
+/* H or H' is applied to C(i:m,1:n) */
+
+ mi = *m - i__ + 1;
+ ic = i__;
+ } else {
+
+/* H or H' is applied to C(1:m,i:n) */
+
+ ni = *n - i__ + 1;
+ jc = i__;
+ }
+
+/* Apply H or H' */
+
+ dlarfb_(side, transt, "Forward", "Rowwise", &mi, &ni, &ib, &a[i__
+ + i__ * a_dim1], lda, t, &c__65, &c__[ic + jc * c_dim1],
+ ldc, &work[1], &ldwork);
+/* L10: */
+ }
+ }
+ work[1] = (doublereal) lwkopt;
+ return 0;
+
+/* End of DORMLQ */
+
+} /* dormlq_ */
diff --git a/3rdparty/clapack/src/dormqr.c b/3rdparty/clapack/src/dormqr.c
new file mode 100644
index 0000000..aed95f2
--- /dev/null
+++ b/3rdparty/clapack/src/dormqr.c
@@ -0,0 +1,327 @@
+/* dormqr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__2 = 2;
+static integer c__65 = 65;
+
+/* Subroutine */ int dormqr_(char *side, char *trans, integer *m, integer *n,
+ integer *k, doublereal *a, integer *lda, doublereal *tau, doublereal *
+ c__, integer *ldc, doublereal *work, integer *lwork, integer *info)
+{
+ /* System generated locals */
+ address a__1[2];
+ integer a_dim1, a_offset, c_dim1, c_offset, i__1, i__2, i__3[2], i__4,
+ i__5;
+ char ch__1[2];
+
+ /* Builtin functions */
+ /* Subroutine */ int s_cat(char *, char **, integer *, integer *, ftnlen);
+
+ /* Local variables */
+ integer i__;
+ doublereal t[4160] /* was [65][64] */;
+ integer i1, i2, i3, ib, ic, jc, nb, mi, ni, nq, nw, iws;
+ logical left;
+ extern logical lsame_(char *, char *);
+ integer nbmin, iinfo;
+ extern /* Subroutine */ int dorm2r_(char *, char *, integer *, integer *,
+ integer *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *, doublereal *, integer *), dlarfb_(char
+ *, char *, char *, char *, integer *, integer *, integer *,
+ doublereal *, integer *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, integer *), dlarft_(char *, char *, integer *, integer *, doublereal
+ *, integer *, doublereal *, doublereal *, integer *), xerbla_(char *, integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ logical notran;
+ integer ldwork, lwkopt;
+ logical lquery;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DORMQR overwrites the general real M-by-N matrix C with */
+
+/* SIDE = 'L' SIDE = 'R' */
+/* TRANS = 'N': Q * C C * Q */
+/* TRANS = 'T': Q**T * C C * Q**T */
+
+/* where Q is a real orthogonal matrix defined as the product of k */
+/* elementary reflectors */
+
+/* Q = H(1) H(2) . . . H(k) */
+
+/* as returned by DGEQRF. Q is of order M if SIDE = 'L' and of order N */
+/* if SIDE = 'R'. */
+
+/* Arguments */
+/* ========= */
+
+/* SIDE (input) CHARACTER*1 */
+/* = 'L': apply Q or Q**T from the Left; */
+/* = 'R': apply Q or Q**T from the Right. */
+
+/* TRANS (input) CHARACTER*1 */
+/* = 'N': No transpose, apply Q; */
+/* = 'T': Transpose, apply Q**T. */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix C. M >= 0. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix C. N >= 0. */
+
+/* K (input) INTEGER */
+/* The number of elementary reflectors whose product defines */
+/* the matrix Q. */
+/* If SIDE = 'L', M >= K >= 0; */
+/* if SIDE = 'R', N >= K >= 0. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,K) */
+/* The i-th column must contain the vector which defines the */
+/* elementary reflector H(i), for i = 1,2,...,k, as returned by */
+/* DGEQRF in the first k columns of its array argument A. */
+/* A is modified by the routine but restored on exit. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. */
+/* If SIDE = 'L', LDA >= max(1,M); */
+/* if SIDE = 'R', LDA >= max(1,N). */
+
+/* TAU (input) DOUBLE PRECISION array, dimension (K) */
+/* TAU(i) must contain the scalar factor of the elementary */
+/* reflector H(i), as returned by DGEQRF. */
+
+/* C (input/output) DOUBLE PRECISION array, dimension (LDC,N) */
+/* On entry, the M-by-N matrix C. */
+/* On exit, C is overwritten by Q*C or Q**T*C or C*Q**T or C*Q. */
+
+/* LDC (input) INTEGER */
+/* The leading dimension of the array C. LDC >= max(1,M). */
+
+/* WORK (workspace/output) DOUBLE PRECISION array, dimension (MAX(1,LWORK)) */
+/* On exit, if INFO = 0, WORK(1) returns the optimal LWORK. */
+
+/* LWORK (input) INTEGER */
+/* The dimension of the array WORK. */
+/* If SIDE = 'L', LWORK >= max(1,N); */
+/* if SIDE = 'R', LWORK >= max(1,M). */
+/* For optimum performance LWORK >= N*NB if SIDE = 'L', and */
+/* LWORK >= M*NB if SIDE = 'R', where NB is the optimal */
+/* blocksize. */
+
+/* If LWORK = -1, then a workspace query is assumed; the routine */
+/* only calculates the optimal size of the WORK array, returns */
+/* this value as the first entry of the WORK array, and no error */
+/* message related to LWORK is issued by XERBLA. */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Local Arrays .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input arguments */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+ --tau;
+ c_dim1 = *ldc;
+ c_offset = 1 + c_dim1;
+ c__ -= c_offset;
+ --work;
+
+ /* Function Body */
+ *info = 0;
+ left = lsame_(side, "L");
+ notran = lsame_(trans, "N");
+ lquery = *lwork == -1;
+
+/* NQ is the order of Q and NW is the minimum dimension of WORK */
+
+ if (left) {
+ nq = *m;
+ nw = *n;
+ } else {
+ nq = *n;
+ nw = *m;
+ }
+ if (! left && ! lsame_(side, "R")) {
+ *info = -1;
+ } else if (! notran && ! lsame_(trans, "T")) {
+ *info = -2;
+ } else if (*m < 0) {
+ *info = -3;
+ } else if (*n < 0) {
+ *info = -4;
+ } else if (*k < 0 || *k > nq) {
+ *info = -5;
+ } else if (*lda < max(1,nq)) {
+ *info = -7;
+ } else if (*ldc < max(1,*m)) {
+ *info = -10;
+ } else if (*lwork < max(1,nw) && ! lquery) {
+ *info = -12;
+ }
+
+ if (*info == 0) {
+
+/* Determine the block size. NB may be at most NBMAX, where NBMAX */
+/* is used to define the local array T. */
+
+/* Computing MIN */
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = 64, i__2 = ilaenv_(&c__1, "DORMQR", ch__1, m, n, k, &c_n1);
+ nb = min(i__1,i__2);
+ lwkopt = max(1,nw) * nb;
+ work[1] = (doublereal) lwkopt;
+ }
+
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DORMQR", &i__1);
+ return 0;
+ } else if (lquery) {
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*m == 0 || *n == 0 || *k == 0) {
+ work[1] = 1.;
+ return 0;
+ }
+
+ nbmin = 2;
+ ldwork = nw;
+ if (nb > 1 && nb < *k) {
+ iws = nw * nb;
+ if (*lwork < iws) {
+ nb = *lwork / ldwork;
+/* Computing MAX */
+/* Writing concatenation */
+ i__3[0] = 1, a__1[0] = side;
+ i__3[1] = 1, a__1[1] = trans;
+ s_cat(ch__1, a__1, i__3, &c__2, (ftnlen)2);
+ i__1 = 2, i__2 = ilaenv_(&c__2, "DORMQR", ch__1, m, n, k, &c_n1);
+ nbmin = max(i__1,i__2);
+ }
+ } else {
+ iws = nw;
+ }
+
+ if (nb < nbmin || nb >= *k) {
+
+/* Use unblocked code */
+
+ dorm2r_(side, trans, m, n, k, &a[a_offset], lda, &tau[1], &c__[
+ c_offset], ldc, &work[1], &iinfo);
+ } else {
+
+/* Use blocked code */
+
+ if (left && ! notran || ! left && notran) {
+ i1 = 1;
+ i2 = *k;
+ i3 = nb;
+ } else {
+ i1 = (*k - 1) / nb * nb + 1;
+ i2 = 1;
+ i3 = -nb;
+ }
+
+ if (left) {
+ ni = *n;
+ jc = 1;
+ } else {
+ mi = *m;
+ ic = 1;
+ }
+
+ i__1 = i2;
+ i__2 = i3;
+ for (i__ = i1; i__2 < 0 ? i__ >= i__1 : i__ <= i__1; i__ += i__2) {
+/* Computing MIN */
+ i__4 = nb, i__5 = *k - i__ + 1;
+ ib = min(i__4,i__5);
+
+/* Form the triangular factor of the block reflector */
+/* H = H(i) H(i+1) . . . H(i+ib-1) */
+
+ i__4 = nq - i__ + 1;
+ dlarft_("Forward", "Columnwise", &i__4, &ib, &a[i__ + i__ *
+ a_dim1], lda, &tau[i__], t, &c__65)
+ ;
+ if (left) {
+
+/* H or H' is applied to C(i:m,1:n) */
+
+ mi = *m - i__ + 1;
+ ic = i__;
+ } else {
+
+/* H or H' is applied to C(1:m,i:n) */
+
+ ni = *n - i__ + 1;
+ jc = i__;
+ }
+
+/* Apply H or H' */
+
+ dlarfb_(side, trans, "Forward", "Columnwise", &mi, &ni, &ib, &a[
+ i__ + i__ * a_dim1], lda, t, &c__65, &c__[ic + jc *
+ c_dim1], ldc, &work[1], &ldwork);
+/* L10: */
+ }
+ }
+ work[1] = (doublereal) lwkopt;
+ return 0;
+
+/* End of DORMQR */
+
+} /* dormqr_ */
diff --git a/3rdparty/clapack/src/dpotf2.c b/3rdparty/clapack/src/dpotf2.c
new file mode 100644
index 0000000..fb237c3
--- /dev/null
+++ b/3rdparty/clapack/src/dpotf2.c
@@ -0,0 +1,224 @@
+/* dpotf2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static doublereal c_b10 = -1.;
+static doublereal c_b12 = 1.;
+
+/* Subroutine */ int dpotf2_(char *uplo, integer *n, doublereal *a, integer *
+ lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3;
+ doublereal d__1;
+
+ /* Builtin functions */
+ double sqrt(doublereal);
+
+ /* Local variables */
+ integer j;
+ doublereal ajj;
+ extern doublereal ddot_(integer *, doublereal *, integer *, doublereal *,
+ integer *);
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dgemv_(char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, integer *,
+ doublereal *, doublereal *, integer *);
+ logical upper;
+ extern logical disnan_(doublereal *);
+ extern /* Subroutine */ int xerbla_(char *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DPOTF2 computes the Cholesky factorization of a real symmetric */
+/* positive definite matrix A. */
+
+/* The factorization has the form */
+/* A = U' * U , if UPLO = 'U', or */
+/* A = L * L', if UPLO = 'L', */
+/* where U is an upper triangular matrix and L is lower triangular. */
+
+/* This is the unblocked version of the algorithm, calling Level 2 BLAS. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies whether the upper or lower triangular part of the */
+/* symmetric matrix A is stored. */
+/* = 'U': Upper triangular */
+/* = 'L': Lower triangular */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the symmetric matrix A. If UPLO = 'U', the leading */
+/* n by n upper triangular part of A contains the upper */
+/* triangular part of the matrix A, and the strictly lower */
+/* triangular part of A is not referenced. If UPLO = 'L', the */
+/* leading n by n lower triangular part of A contains the lower */
+/* triangular part of the matrix A, and the strictly upper */
+/* triangular part of A is not referenced. */
+
+/* On exit, if INFO = 0, the factor U or L from the Cholesky */
+/* factorization A = U'*U or A = L*L'. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -k, the k-th argument had an illegal value */
+/* > 0: if INFO = k, the leading minor of order k is not */
+/* positive definite, and the factorization could not be */
+/* completed. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*n)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DPOTF2", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+ if (upper) {
+
+/* Compute the Cholesky factorization A = U'*U. */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+
+/* Compute U(J,J) and test for non-positive-definiteness. */
+
+ i__2 = j - 1;
+ ajj = a[j + j * a_dim1] - ddot_(&i__2, &a[j * a_dim1 + 1], &c__1,
+ &a[j * a_dim1 + 1], &c__1);
+ if (ajj <= 0. || disnan_(&ajj)) {
+ a[j + j * a_dim1] = ajj;
+ goto L30;
+ }
+ ajj = sqrt(ajj);
+ a[j + j * a_dim1] = ajj;
+
+/* Compute elements J+1:N of row J. */
+
+ if (j < *n) {
+ i__2 = j - 1;
+ i__3 = *n - j;
+ dgemv_("Transpose", &i__2, &i__3, &c_b10, &a[(j + 1) * a_dim1
+ + 1], lda, &a[j * a_dim1 + 1], &c__1, &c_b12, &a[j + (
+ j + 1) * a_dim1], lda);
+ i__2 = *n - j;
+ d__1 = 1. / ajj;
+ dscal_(&i__2, &d__1, &a[j + (j + 1) * a_dim1], lda);
+ }
+/* L10: */
+ }
+ } else {
+
+/* Compute the Cholesky factorization A = L*L'. */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+
+/* Compute L(J,J) and test for non-positive-definiteness. */
+
+ i__2 = j - 1;
+ ajj = a[j + j * a_dim1] - ddot_(&i__2, &a[j + a_dim1], lda, &a[j
+ + a_dim1], lda);
+ if (ajj <= 0. || disnan_(&ajj)) {
+ a[j + j * a_dim1] = ajj;
+ goto L30;
+ }
+ ajj = sqrt(ajj);
+ a[j + j * a_dim1] = ajj;
+
+/* Compute elements J+1:N of column J. */
+
+ if (j < *n) {
+ i__2 = *n - j;
+ i__3 = j - 1;
+ dgemv_("No transpose", &i__2, &i__3, &c_b10, &a[j + 1 +
+ a_dim1], lda, &a[j + a_dim1], lda, &c_b12, &a[j + 1 +
+ j * a_dim1], &c__1);
+ i__2 = *n - j;
+ d__1 = 1. / ajj;
+ dscal_(&i__2, &d__1, &a[j + 1 + j * a_dim1], &c__1);
+ }
+/* L20: */
+ }
+ }
+ goto L40;
+
+L30:
+ *info = j;
+
+L40:
+ return 0;
+
+/* End of DPOTF2 */
+
+} /* dpotf2_ */
diff --git a/3rdparty/clapack/src/dpotrf.c b/3rdparty/clapack/src/dpotrf.c
new file mode 100644
index 0000000..5ebdcfc
--- /dev/null
+++ b/3rdparty/clapack/src/dpotrf.c
@@ -0,0 +1,245 @@
+/* dpotrf.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static doublereal c_b13 = -1.;
+static doublereal c_b14 = 1.;
+
+/* Subroutine */ int dpotrf_(char *uplo, integer *n, doublereal *a, integer *
+ lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2, i__3, i__4;
+
+ /* Local variables */
+ integer j, jb, nb;
+ extern /* Subroutine */ int dgemm_(char *, char *, integer *, integer *,
+ integer *, doublereal *, doublereal *, integer *, doublereal *,
+ integer *, doublereal *, doublereal *, integer *);
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dtrsm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *);
+ logical upper;
+ extern /* Subroutine */ int dsyrk_(char *, char *, integer *, integer *,
+ doublereal *, doublereal *, integer *, doublereal *, doublereal *,
+ integer *), dpotf2_(char *, integer *,
+ doublereal *, integer *, integer *), xerbla_(char *,
+ integer *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DPOTRF computes the Cholesky factorization of a real symmetric */
+/* positive definite matrix A. */
+
+/* The factorization has the form */
+/* A = U**T * U, if UPLO = 'U', or */
+/* A = L * L**T, if UPLO = 'L', */
+/* where U is an upper triangular matrix and L is lower triangular. */
+
+/* This is the block version of the algorithm, calling Level 3 BLAS. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* = 'U': Upper triangle of A is stored; */
+/* = 'L': Lower triangle of A is stored. */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the symmetric matrix A. If UPLO = 'U', the leading */
+/* N-by-N upper triangular part of A contains the upper */
+/* triangular part of the matrix A, and the strictly lower */
+/* triangular part of A is not referenced. If UPLO = 'L', the */
+/* leading N-by-N lower triangular part of A contains the lower */
+/* triangular part of the matrix A, and the strictly upper */
+/* triangular part of A is not referenced. */
+
+/* On exit, if INFO = 0, the factor U or L from the Cholesky */
+/* factorization A = U**T*U or A = L*L**T. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: if INFO = i, the leading minor of order i is not */
+/* positive definite, and the factorization could not be */
+/* completed. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*n)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DPOTRF", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+/* Determine the block size for this environment. */
+
+ nb = ilaenv_(&c__1, "DPOTRF", uplo, n, &c_n1, &c_n1, &c_n1);
+ if (nb <= 1 || nb >= *n) {
+
+/* Use unblocked code. */
+
+ dpotf2_(uplo, n, &a[a_offset], lda, info);
+ } else {
+
+/* Use blocked code. */
+
+ if (upper) {
+
+/* Compute the Cholesky factorization A = U'*U. */
+
+ i__1 = *n;
+ i__2 = nb;
+ for (j = 1; i__2 < 0 ? j >= i__1 : j <= i__1; j += i__2) {
+
+/* Update and factorize the current diagonal block and test */
+/* for non-positive-definiteness. */
+
+/* Computing MIN */
+ i__3 = nb, i__4 = *n - j + 1;
+ jb = min(i__3,i__4);
+ i__3 = j - 1;
+ dsyrk_("Upper", "Transpose", &jb, &i__3, &c_b13, &a[j *
+ a_dim1 + 1], lda, &c_b14, &a[j + j * a_dim1], lda);
+ dpotf2_("Upper", &jb, &a[j + j * a_dim1], lda, info);
+ if (*info != 0) {
+ goto L30;
+ }
+ if (j + jb <= *n) {
+
+/* Compute the current block row. */
+
+ i__3 = *n - j - jb + 1;
+ i__4 = j - 1;
+ dgemm_("Transpose", "No transpose", &jb, &i__3, &i__4, &
+ c_b13, &a[j * a_dim1 + 1], lda, &a[(j + jb) *
+ a_dim1 + 1], lda, &c_b14, &a[j + (j + jb) *
+ a_dim1], lda);
+ i__3 = *n - j - jb + 1;
+ dtrsm_("Left", "Upper", "Transpose", "Non-unit", &jb, &
+ i__3, &c_b14, &a[j + j * a_dim1], lda, &a[j + (j
+ + jb) * a_dim1], lda);
+ }
+/* L10: */
+ }
+
+ } else {
+
+/* Compute the Cholesky factorization A = L*L'. */
+
+ i__2 = *n;
+ i__1 = nb;
+ for (j = 1; i__1 < 0 ? j >= i__2 : j <= i__2; j += i__1) {
+
+/* Update and factorize the current diagonal block and test */
+/* for non-positive-definiteness. */
+
+/* Computing MIN */
+ i__3 = nb, i__4 = *n - j + 1;
+ jb = min(i__3,i__4);
+ i__3 = j - 1;
+ dsyrk_("Lower", "No transpose", &jb, &i__3, &c_b13, &a[j +
+ a_dim1], lda, &c_b14, &a[j + j * a_dim1], lda);
+ dpotf2_("Lower", &jb, &a[j + j * a_dim1], lda, info);
+ if (*info != 0) {
+ goto L30;
+ }
+ if (j + jb <= *n) {
+
+/* Compute the current block column. */
+
+ i__3 = *n - j - jb + 1;
+ i__4 = j - 1;
+ dgemm_("No transpose", "Transpose", &i__3, &jb, &i__4, &
+ c_b13, &a[j + jb + a_dim1], lda, &a[j + a_dim1],
+ lda, &c_b14, &a[j + jb + j * a_dim1], lda);
+ i__3 = *n - j - jb + 1;
+ dtrsm_("Right", "Lower", "Transpose", "Non-unit", &i__3, &
+ jb, &c_b14, &a[j + j * a_dim1], lda, &a[j + jb +
+ j * a_dim1], lda);
+ }
+/* L20: */
+ }
+ }
+ }
+ goto L40;
+
+L30:
+ *info = *info + j - 1;
+
+L40:
+ return 0;
+
+/* End of DPOTRF */
+
+} /* dpotrf_ */
diff --git a/3rdparty/clapack/src/dpotri.c b/3rdparty/clapack/src/dpotri.c
new file mode 100644
index 0000000..9141e4c
--- /dev/null
+++ b/3rdparty/clapack/src/dpotri.c
@@ -0,0 +1,125 @@
+/* dpotri.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Subroutine */ int dpotri_(char *uplo, integer *n, doublereal *a, integer *
+ lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1;
+
+ /* Local variables */
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int xerbla_(char *, integer *), dlauum_(
+ char *, integer *, doublereal *, integer *, integer *),
+ dtrtri_(char *, char *, integer *, doublereal *, integer *,
+ integer *);
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DPOTRI computes the inverse of a real symmetric positive definite */
+/* matrix A using the Cholesky factorization A = U**T*U or A = L*L**T */
+/* computed by DPOTRF. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* = 'U': Upper triangle of A is stored; */
+/* = 'L': Lower triangle of A is stored. */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the triangular factor U or L from the Cholesky */
+/* factorization A = U**T*U or A = L*L**T, as computed by */
+/* DPOTRF. */
+/* On exit, the upper or lower triangle of the (symmetric) */
+/* inverse of A, overwriting the input factor U or L. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: if INFO = i, the (i,i) element of the factor U or L is */
+/* zero, and the inverse could not be computed. */
+
+/* ===================================================================== */
+
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ if (! lsame_(uplo, "U") && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (*n < 0) {
+ *info = -2;
+ } else if (*lda < max(1,*n)) {
+ *info = -4;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DPOTRI", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+/* Invert the triangular Cholesky factor U or L. */
+
+ dtrtri_(uplo, "Non-unit", n, &a[a_offset], lda, info);
+ if (*info > 0) {
+ return 0;
+ }
+
+/* Form inv(U)*inv(U)' or inv(L)'*inv(L). */
+
+ dlauum_(uplo, n, &a[a_offset], lda, info);
+
+ return 0;
+
+/* End of DPOTRI */
+
+} /* dpotri_ */
diff --git a/3rdparty/clapack/src/dtrti2.c b/3rdparty/clapack/src/dtrti2.c
new file mode 100644
index 0000000..2631b19
--- /dev/null
+++ b/3rdparty/clapack/src/dtrti2.c
@@ -0,0 +1,183 @@
+/* dtrti2.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int dtrti2_(char *uplo, char *diag, integer *n, doublereal *
+ a, integer *lda, integer *info)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, i__1, i__2;
+
+ /* Local variables */
+ integer j;
+ doublereal ajj;
+ extern /* Subroutine */ int dscal_(integer *, doublereal *, doublereal *,
+ integer *);
+ extern logical lsame_(char *, char *);
+ logical upper;
+ extern /* Subroutine */ int dtrmv_(char *, char *, char *, integer *,
+ doublereal *, integer *, doublereal *, integer *), xerbla_(char *, integer *);
+ logical nounit;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DTRTI2 computes the inverse of a real upper or lower triangular */
+/* matrix. */
+
+/* This is the Level 2 BLAS version of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* Specifies whether the matrix A is upper or lower triangular. */
+/* = 'U': Upper triangular */
+/* = 'L': Lower triangular */
+
+/* DIAG (input) CHARACTER*1 */
+/* Specifies whether or not the matrix A is unit triangular. */
+/* = 'N': Non-unit triangular */
+/* = 'U': Unit triangular */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the triangular matrix A. If UPLO = 'U', the */
+/* leading n by n upper triangular part of the array A contains */
+/* the upper triangular matrix, and the strictly lower */
+/* triangular part of A is not referenced. If UPLO = 'L', the */
+/* leading n by n lower triangular part of the array A contains */
+/* the lower triangular matrix, and the strictly upper */
+/* triangular part of A is not referenced. If DIAG = 'U', the */
+/* diagonal elements of A are also not referenced and are */
+/* assumed to be 1. */
+
+/* On exit, the (triangular) inverse of the original matrix, in */
+/* the same storage format. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -k, the k-th argument had an illegal value */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ nounit = lsame_(diag, "N");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (! nounit && ! lsame_(diag, "U")) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*lda < max(1,*n)) {
+ *info = -5;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DTRTI2", &i__1);
+ return 0;
+ }
+
+ if (upper) {
+
+/* Compute inverse of upper triangular matrix. */
+
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ if (nounit) {
+ a[j + j * a_dim1] = 1. / a[j + j * a_dim1];
+ ajj = -a[j + j * a_dim1];
+ } else {
+ ajj = -1.;
+ }
+
+/* Compute elements 1:j-1 of j-th column. */
+
+ i__2 = j - 1;
+ dtrmv_("Upper", "No transpose", diag, &i__2, &a[a_offset], lda, &
+ a[j * a_dim1 + 1], &c__1);
+ i__2 = j - 1;
+ dscal_(&i__2, &ajj, &a[j * a_dim1 + 1], &c__1);
+/* L10: */
+ }
+ } else {
+
+/* Compute inverse of lower triangular matrix. */
+
+ for (j = *n; j >= 1; --j) {
+ if (nounit) {
+ a[j + j * a_dim1] = 1. / a[j + j * a_dim1];
+ ajj = -a[j + j * a_dim1];
+ } else {
+ ajj = -1.;
+ }
+ if (j < *n) {
+
+/* Compute elements j+1:n of j-th column. */
+
+ i__1 = *n - j;
+ dtrmv_("Lower", "No transpose", diag, &i__1, &a[j + 1 + (j +
+ 1) * a_dim1], lda, &a[j + 1 + j * a_dim1], &c__1);
+ i__1 = *n - j;
+ dscal_(&i__1, &ajj, &a[j + 1 + j * a_dim1], &c__1);
+ }
+/* L20: */
+ }
+ }
+
+ return 0;
+
+/* End of DTRTI2 */
+
+} /* dtrti2_ */
diff --git a/3rdparty/clapack/src/dtrtri.c b/3rdparty/clapack/src/dtrtri.c
new file mode 100644
index 0000000..cbc2264
--- /dev/null
+++ b/3rdparty/clapack/src/dtrtri.c
@@ -0,0 +1,242 @@
+/* dtrtri.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static integer c_n1 = -1;
+static integer c__2 = 2;
+static doublereal c_b18 = 1.;
+static doublereal c_b22 = -1.;
+
+/* Subroutine */ int dtrtri_(char *uplo, char *diag, integer *n, doublereal *
+ a, integer *lda, integer *info)
+{
+ /* System generated locals */
+ address a__1[2];
+ integer a_dim1, a_offset, i__1, i__2[2], i__3, i__4, i__5;
+ char ch__1[2];
+
+ /* Builtin functions */
+ /* Subroutine */ int s_cat(char *, char **, integer *, integer *, ftnlen);
+
+ /* Local variables */
+ integer j, jb, nb, nn;
+ extern logical lsame_(char *, char *);
+ extern /* Subroutine */ int dtrmm_(char *, char *, char *, char *,
+ integer *, integer *, doublereal *, doublereal *, integer *,
+ doublereal *, integer *), dtrsm_(
+ char *, char *, char *, char *, integer *, integer *, doublereal *
+, doublereal *, integer *, doublereal *, integer *);
+ logical upper;
+ extern /* Subroutine */ int dtrti2_(char *, char *, integer *, doublereal
+ *, integer *, integer *), xerbla_(char *, integer
+ *);
+ extern integer ilaenv_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+ logical nounit;
+
+
+/* -- LAPACK routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* DTRTRI computes the inverse of a real upper or lower triangular */
+/* matrix A. */
+
+/* This is the Level 3 BLAS version of the algorithm. */
+
+/* Arguments */
+/* ========= */
+
+/* UPLO (input) CHARACTER*1 */
+/* = 'U': A is upper triangular; */
+/* = 'L': A is lower triangular. */
+
+/* DIAG (input) CHARACTER*1 */
+/* = 'N': A is non-unit triangular; */
+/* = 'U': A is unit triangular. */
+
+/* N (input) INTEGER */
+/* The order of the matrix A. N >= 0. */
+
+/* A (input/output) DOUBLE PRECISION array, dimension (LDA,N) */
+/* On entry, the triangular matrix A. If UPLO = 'U', the */
+/* leading N-by-N upper triangular part of the array A contains */
+/* the upper triangular matrix, and the strictly lower */
+/* triangular part of A is not referenced. If UPLO = 'L', the */
+/* leading N-by-N lower triangular part of the array A contains */
+/* the lower triangular matrix, and the strictly upper */
+/* triangular part of A is not referenced. If DIAG = 'U', the */
+/* diagonal elements of A are also not referenced and are */
+/* assumed to be 1. */
+/* On exit, the (triangular) inverse of the original matrix, in */
+/* the same storage format. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,N). */
+
+/* INFO (output) INTEGER */
+/* = 0: successful exit */
+/* < 0: if INFO = -i, the i-th argument had an illegal value */
+/* > 0: if INFO = i, A(i,i) is exactly zero. The triangular */
+/* matrix is singular and its inverse can not be computed. */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. External Subroutines .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Test the input parameters. */
+
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ *info = 0;
+ upper = lsame_(uplo, "U");
+ nounit = lsame_(diag, "N");
+ if (! upper && ! lsame_(uplo, "L")) {
+ *info = -1;
+ } else if (! nounit && ! lsame_(diag, "U")) {
+ *info = -2;
+ } else if (*n < 0) {
+ *info = -3;
+ } else if (*lda < max(1,*n)) {
+ *info = -5;
+ }
+ if (*info != 0) {
+ i__1 = -(*info);
+ xerbla_("DTRTRI", &i__1);
+ return 0;
+ }
+
+/* Quick return if possible */
+
+ if (*n == 0) {
+ return 0;
+ }
+
+/* Check for singularity if non-unit. */
+
+ if (nounit) {
+ i__1 = *n;
+ for (*info = 1; *info <= i__1; ++(*info)) {
+ if (a[*info + *info * a_dim1] == 0.) {
+ return 0;
+ }
+/* L10: */
+ }
+ *info = 0;
+ }
+
+/* Determine the block size for this environment. */
+
+/* Writing concatenation */
+ i__2[0] = 1, a__1[0] = uplo;
+ i__2[1] = 1, a__1[1] = diag;
+ s_cat(ch__1, a__1, i__2, &c__2, (ftnlen)2);
+ nb = ilaenv_(&c__1, "DTRTRI", ch__1, n, &c_n1, &c_n1, &c_n1);
+ if (nb <= 1 || nb >= *n) {
+
+/* Use unblocked code */
+
+ dtrti2_(uplo, diag, n, &a[a_offset], lda, info);
+ } else {
+
+/* Use blocked code */
+
+ if (upper) {
+
+/* Compute inverse of upper triangular matrix */
+
+ i__1 = *n;
+ i__3 = nb;
+ for (j = 1; i__3 < 0 ? j >= i__1 : j <= i__1; j += i__3) {
+/* Computing MIN */
+ i__4 = nb, i__5 = *n - j + 1;
+ jb = min(i__4,i__5);
+
+/* Compute rows 1:j-1 of current block column */
+
+ i__4 = j - 1;
+ dtrmm_("Left", "Upper", "No transpose", diag, &i__4, &jb, &
+ c_b18, &a[a_offset], lda, &a[j * a_dim1 + 1], lda);
+ i__4 = j - 1;
+ dtrsm_("Right", "Upper", "No transpose", diag, &i__4, &jb, &
+ c_b22, &a[j + j * a_dim1], lda, &a[j * a_dim1 + 1],
+ lda);
+
+/* Compute inverse of current diagonal block */
+
+ dtrti2_("Upper", diag, &jb, &a[j + j * a_dim1], lda, info);
+/* L20: */
+ }
+ } else {
+
+/* Compute inverse of lower triangular matrix */
+
+ nn = (*n - 1) / nb * nb + 1;
+ i__3 = -nb;
+ for (j = nn; i__3 < 0 ? j >= 1 : j <= 1; j += i__3) {
+/* Computing MIN */
+ i__1 = nb, i__4 = *n - j + 1;
+ jb = min(i__1,i__4);
+ if (j + jb <= *n) {
+
+/* Compute rows j+jb:n of current block column */
+
+ i__1 = *n - j - jb + 1;
+ dtrmm_("Left", "Lower", "No transpose", diag, &i__1, &jb,
+ &c_b18, &a[j + jb + (j + jb) * a_dim1], lda, &a[j
+ + jb + j * a_dim1], lda);
+ i__1 = *n - j - jb + 1;
+ dtrsm_("Right", "Lower", "No transpose", diag, &i__1, &jb,
+ &c_b22, &a[j + j * a_dim1], lda, &a[j + jb + j *
+ a_dim1], lda);
+ }
+
+/* Compute inverse of current diagonal block */
+
+ dtrti2_("Lower", diag, &jb, &a[j + j * a_dim1], lda, info);
+/* L30: */
+ }
+ }
+ }
+
+ return 0;
+
+/* End of DTRTRI */
+
+} /* dtrtri_ */
diff --git a/3rdparty/clapack/src/ieeeck.c b/3rdparty/clapack/src/ieeeck.c
new file mode 100644
index 0000000..3d6f0b5
--- /dev/null
+++ b/3rdparty/clapack/src/ieeeck.c
@@ -0,0 +1,166 @@
+/* ieeeck.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+integer ieeeck_(integer *ispec, real *zero, real *one)
+{
+ /* System generated locals */
+ integer ret_val;
+
+ /* Local variables */
+ real nan1, nan2, nan3, nan4, nan5, nan6, neginf, posinf, negzro, newzro;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* IEEECK is called from the ILAENV to verify that Infinity and */
+/* possibly NaN arithmetic is safe (i.e. will not trap). */
+
+/* Arguments */
+/* ========= */
+
+/* ISPEC (input) INTEGER */
+/* Specifies whether to test just for inifinity arithmetic */
+/* or whether to test for infinity and NaN arithmetic. */
+/* = 0: Verify infinity arithmetic only. */
+/* = 1: Verify infinity and NaN arithmetic. */
+
+/* ZERO (input) REAL */
+/* Must contain the value 0.0 */
+/* This is passed to prevent the compiler from optimizing */
+/* away this code. */
+
+/* ONE (input) REAL */
+/* Must contain the value 1.0 */
+/* This is passed to prevent the compiler from optimizing */
+/* away this code. */
+
+/* RETURN VALUE: INTEGER */
+/* = 0: Arithmetic failed to produce the correct answers */
+/* = 1: Arithmetic produced the correct answers */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Executable Statements .. */
+ ret_val = 1;
+
+ posinf = *one / *zero;
+ if (posinf <= *one) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ neginf = -(*one) / *zero;
+ if (neginf >= *zero) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ negzro = *one / (neginf + *one);
+ if (negzro != *zero) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ neginf = *one / negzro;
+ if (neginf >= *zero) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ newzro = negzro + *zero;
+ if (newzro != *zero) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ posinf = *one / newzro;
+ if (posinf <= *one) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ neginf *= posinf;
+ if (neginf >= *zero) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ posinf *= posinf;
+ if (posinf <= *one) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+
+
+
+/* Return if we were only asked to check infinity arithmetic */
+
+ if (*ispec == 0) {
+ return ret_val;
+ }
+
+ nan1 = posinf + neginf;
+
+ nan2 = posinf / neginf;
+
+ nan3 = posinf / posinf;
+
+ nan4 = posinf * *zero;
+
+ nan5 = neginf * negzro;
+
+ nan6 = nan5 * 0.f;
+
+ if (nan1 == nan1) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ if (nan2 == nan2) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ if (nan3 == nan3) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ if (nan4 == nan4) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ if (nan5 == nan5) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ if (nan6 == nan6) {
+ ret_val = 0;
+ return ret_val;
+ }
+
+ return ret_val;
+} /* ieeeck_ */
diff --git a/3rdparty/clapack/src/iladlc.c b/3rdparty/clapack/src/iladlc.c
new file mode 100644
index 0000000..a18f02e
--- /dev/null
+++ b/3rdparty/clapack/src/iladlc.c
@@ -0,0 +1,88 @@
+/* iladlc.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+integer iladlc_(integer *m, integer *n, doublereal *a, integer *lda)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, ret_val, i__1;
+
+ /* Local variables */
+ integer i__;
+
+
+/* -- LAPACK auxiliary routine (version 3.2.1) -- */
+
+/* -- April 2009 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* ILADLC scans A for its last non-zero column. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The m by n matrix A. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Quick test for the common case where one corner is non-zero. */
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ if (*n == 0) {
+ ret_val = *n;
+ } else if (a[*n * a_dim1 + 1] != 0. || a[*m + *n * a_dim1] != 0.) {
+ ret_val = *n;
+ } else {
+/* Now scan each column from the end, returning with the first non-zero. */
+ for (ret_val = *n; ret_val >= 1; --ret_val) {
+ i__1 = *m;
+ for (i__ = 1; i__ <= i__1; ++i__) {
+ if (a[i__ + ret_val * a_dim1] != 0.) {
+ return ret_val;
+ }
+ }
+ }
+ }
+ return ret_val;
+} /* iladlc_ */
diff --git a/3rdparty/clapack/src/iladlr.c b/3rdparty/clapack/src/iladlr.c
new file mode 100644
index 0000000..f1626e4
--- /dev/null
+++ b/3rdparty/clapack/src/iladlr.c
@@ -0,0 +1,90 @@
+/* iladlr.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+integer iladlr_(integer *m, integer *n, doublereal *a, integer *lda)
+{
+ /* System generated locals */
+ integer a_dim1, a_offset, ret_val, i__1;
+
+ /* Local variables */
+ integer i__, j;
+
+
+/* -- LAPACK auxiliary routine (version 3.2.1) -- */
+
+/* -- April 2009 -- */
+
+/* -- LAPACK is a software package provided by Univ. of Tennessee, -- */
+/* -- Univ. of California Berkeley, Univ. of Colorado Denver and NAG Ltd..-- */
+
+/* .. Scalar Arguments .. */
+/* .. */
+/* .. Array Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* ILADLR scans A for its last non-zero row. */
+
+/* Arguments */
+/* ========= */
+
+/* M (input) INTEGER */
+/* The number of rows of the matrix A. */
+
+/* N (input) INTEGER */
+/* The number of columns of the matrix A. */
+
+/* A (input) DOUBLE PRECISION array, dimension (LDA,N) */
+/* The m by n matrix A. */
+
+/* LDA (input) INTEGER */
+/* The leading dimension of the array A. LDA >= max(1,M). */
+
+/* ===================================================================== */
+
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Executable Statements .. */
+
+/* Quick test for the common case where one corner is non-zero. */
+ /* Parameter adjustments */
+ a_dim1 = *lda;
+ a_offset = 1 + a_dim1;
+ a -= a_offset;
+
+ /* Function Body */
+ if (*m == 0) {
+ ret_val = *m;
+ } else if (a[*m + a_dim1] != 0. || a[*m + *n * a_dim1] != 0.) {
+ ret_val = *m;
+ } else {
+/* Scan up each column tracking the last zero row seen. */
+ ret_val = 0;
+ i__1 = *n;
+ for (j = 1; j <= i__1; ++j) {
+ for (i__ = *m; i__ >= 1; --i__) {
+ if (a[i__ + j * a_dim1] != 0.) {
+ break;
+ }
+ }
+ ret_val = max(ret_val,i__);
+ }
+ }
+ return ret_val;
+} /* iladlr_ */
diff --git a/3rdparty/clapack/src/ilaenv.c b/3rdparty/clapack/src/ilaenv.c
new file mode 100644
index 0000000..9565433
--- /dev/null
+++ b/3rdparty/clapack/src/ilaenv.c
@@ -0,0 +1,654 @@
+/* ilaenv.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+#include "string.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+static real c_b163 = 0.f;
+static real c_b164 = 1.f;
+static integer c__0 = 0;
+
+integer ilaenv_(integer *ispec, char *name__, char *opts, integer *n1,
+ integer *n2, integer *n3, integer *n4)
+{
+ /* System generated locals */
+ integer ret_val;
+
+ /* Builtin functions */
+ /* Subroutine */ int s_copy(char *, char *, ftnlen, ftnlen);
+ integer s_cmp(char *, char *, ftnlen, ftnlen);
+
+ /* Local variables */
+ integer i__;
+ char c1[1], c2[2], c3[3], c4[2];
+ integer ic, nb, iz, nx;
+ logical cname;
+ integer nbmin;
+ logical sname;
+ extern integer ieeeck_(integer *, real *, real *);
+ char subnam[6];
+ extern integer iparmq_(integer *, char *, char *, integer *, integer *,
+ integer *, integer *);
+
+ ftnlen name_len, opts_len;
+
+ name_len = strlen (name__);
+ opts_len = strlen (opts);
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* January 2007 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* ILAENV is called from the LAPACK routines to choose problem-dependent */
+/* parameters for the local environment. See ISPEC for a description of */
+/* the parameters. */
+
+/* ILAENV returns an INTEGER */
+/* if ILAENV >= 0: ILAENV returns the value of the parameter specified by ISPEC */
+/* if ILAENV < 0: if ILAENV = -k, the k-th argument had an illegal value. */
+
+/* This version provides a set of parameters which should give good, */
+/* but not optimal, performance on many of the currently available */
+/* computers. Users are encouraged to modify this subroutine to set */
+/* the tuning parameters for their particular machine using the option */
+/* and problem size information in the arguments. */
+
+/* This routine will not function correctly if it is converted to all */
+/* lower case. Converting it to all upper case is allowed. */
+
+/* Arguments */
+/* ========= */
+
+/* ISPEC (input) INTEGER */
+/* Specifies the parameter to be returned as the value of */
+/* ILAENV. */
+/* = 1: the optimal blocksize; if this value is 1, an unblocked */
+/* algorithm will give the best performance. */
+/* = 2: the minimum block size for which the block routine */
+/* should be used; if the usable block size is less than */
+/* this value, an unblocked routine should be used. */
+/* = 3: the crossover point (in a block routine, for N less */
+/* than this value, an unblocked routine should be used) */
+/* = 4: the number of shifts, used in the nonsymmetric */
+/* eigenvalue routines (DEPRECATED) */
+/* = 5: the minimum column dimension for blocking to be used; */
+/* rectangular blocks must have dimension at least k by m, */
+/* where k is given by ILAENV(2,...) and m by ILAENV(5,...) */
+/* = 6: the crossover point for the SVD (when reducing an m by n */
+/* matrix to bidiagonal form, if max(m,n)/min(m,n) exceeds */
+/* this value, a QR factorization is used first to reduce */
+/* the matrix to a triangular form.) */
+/* = 7: the number of processors */
+/* = 8: the crossover point for the multishift QR method */
+/* for nonsymmetric eigenvalue problems (DEPRECATED) */
+/* = 9: maximum size of the subproblems at the bottom of the */
+/* computation tree in the divide-and-conquer algorithm */
+/* (used by xGELSD and xGESDD) */
+/* =10: ieee NaN arithmetic can be trusted not to trap */
+/* =11: infinity arithmetic can be trusted not to trap */
+/* 12 <= ISPEC <= 16: */
+/* xHSEQR or one of its subroutines, */
+/* see IPARMQ for detailed explanation */
+
+/* NAME (input) CHARACTER*(*) */
+/* The name of the calling subroutine, in either upper case or */
+/* lower case. */
+
+/* OPTS (input) CHARACTER*(*) */
+/* The character options to the subroutine NAME, concatenated */
+/* into a single character string. For example, UPLO = 'U', */
+/* TRANS = 'T', and DIAG = 'N' for a triangular routine would */
+/* be specified as OPTS = 'UTN'. */
+
+/* N1 (input) INTEGER */
+/* N2 (input) INTEGER */
+/* N3 (input) INTEGER */
+/* N4 (input) INTEGER */
+/* Problem dimensions for the subroutine NAME; these may not all */
+/* be required. */
+
+/* Further Details */
+/* =============== */
+
+/* The following conventions have been used when calling ILAENV from the */
+/* LAPACK routines: */
+/* 1) OPTS is a concatenation of all of the character options to */
+/* subroutine NAME, in the same order that they appear in the */
+/* argument list for NAME, even if they are not used in determining */
+/* the value of the parameter specified by ISPEC. */
+/* 2) The problem dimensions N1, N2, N3, N4 are specified in the order */
+/* that they appear in the argument list for NAME. N1 is used */
+/* first, N2 second, and so on, and unused problem dimensions are */
+/* passed a value of -1. */
+/* 3) The parameter value returned by ILAENV is checked for validity in */
+/* the calling subroutine. For example, ILAENV is used to retrieve */
+/* the optimal blocksize for STRTRI as follows: */
+
+/* NB = ILAENV( 1, 'STRTRI', UPLO // DIAG, N, -1, -1, -1 ) */
+/* IF( NB.LE.1 ) NB = MAX( 1, N ) */
+
+/* ===================================================================== */
+
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. External Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ switch (*ispec) {
+ case 1: goto L10;
+ case 2: goto L10;
+ case 3: goto L10;
+ case 4: goto L80;
+ case 5: goto L90;
+ case 6: goto L100;
+ case 7: goto L110;
+ case 8: goto L120;
+ case 9: goto L130;
+ case 10: goto L140;
+ case 11: goto L150;
+ case 12: goto L160;
+ case 13: goto L160;
+ case 14: goto L160;
+ case 15: goto L160;
+ case 16: goto L160;
+ }
+
+/* Invalid value for ISPEC */
+
+ ret_val = -1;
+ return ret_val;
+
+L10:
+
+/* Convert NAME to upper case if the first character is lower case. */
+
+ ret_val = 1;
+ s_copy(subnam, name__, (ftnlen)1, name_len);
+ ic = *(unsigned char *)subnam;
+ iz = 'Z';
+ if (iz == 90 || iz == 122) {
+
+/* ASCII character set */
+
+ if (ic >= 97 && ic <= 122) {
+ *(unsigned char *)subnam = (char) (ic - 32);
+ for (i__ = 2; i__ <= 6; ++i__) {
+ ic = *(unsigned char *)&subnam[i__ - 1];
+ if (ic >= 97 && ic <= 122) {
+ *(unsigned char *)&subnam[i__ - 1] = (char) (ic - 32);
+ }
+/* L20: */
+ }
+ }
+
+ } else if (iz == 233 || iz == 169) {
+
+/* EBCDIC character set */
+
+ if (ic >= 129 && ic <= 137 || ic >= 145 && ic <= 153 || ic >= 162 &&
+ ic <= 169) {
+ *(unsigned char *)subnam = (char) (ic + 64);
+ for (i__ = 2; i__ <= 6; ++i__) {
+ ic = *(unsigned char *)&subnam[i__ - 1];
+ if (ic >= 129 && ic <= 137 || ic >= 145 && ic <= 153 || ic >=
+ 162 && ic <= 169) {
+ *(unsigned char *)&subnam[i__ - 1] = (char) (ic + 64);
+ }
+/* L30: */
+ }
+ }
+
+ } else if (iz == 218 || iz == 250) {
+
+/* Prime machines: ASCII+128 */
+
+ if (ic >= 225 && ic <= 250) {
+ *(unsigned char *)subnam = (char) (ic - 32);
+ for (i__ = 2; i__ <= 6; ++i__) {
+ ic = *(unsigned char *)&subnam[i__ - 1];
+ if (ic >= 225 && ic <= 250) {
+ *(unsigned char *)&subnam[i__ - 1] = (char) (ic - 32);
+ }
+/* L40: */
+ }
+ }
+ }
+
+ *(unsigned char *)c1 = *(unsigned char *)subnam;
+ sname = *(unsigned char *)c1 == 'S' || *(unsigned char *)c1 == 'D';
+ cname = *(unsigned char *)c1 == 'C' || *(unsigned char *)c1 == 'Z';
+ if (! (cname || sname)) {
+ return ret_val;
+ }
+ s_copy(c2, subnam + 1, (ftnlen)1, (ftnlen)2);
+ s_copy(c3, subnam + 3, (ftnlen)1, (ftnlen)3);
+ s_copy(c4, c3 + 1, (ftnlen)1, (ftnlen)2);
+
+ switch (*ispec) {
+ case 1: goto L50;
+ case 2: goto L60;
+ case 3: goto L70;
+ }
+
+L50:
+
+/* ISPEC = 1: block size */
+
+/* In these examples, separate code is provided for setting NB for */
+/* real and complex. We assume that NB will take the same value in */
+/* single or double precision. */
+
+ nb = 1;
+
+ if (s_cmp(c2, "GE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ } else if (s_cmp(c3, "QRF", (ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3,
+ "RQF", (ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3, "LQF", (ftnlen)
+ 1, (ftnlen)3) == 0 || s_cmp(c3, "QLF", (ftnlen)1, (ftnlen)3)
+ == 0) {
+ if (sname) {
+ nb = 32;
+ } else {
+ nb = 32;
+ }
+ } else if (s_cmp(c3, "HRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 32;
+ } else {
+ nb = 32;
+ }
+ } else if (s_cmp(c3, "BRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 32;
+ } else {
+ nb = 32;
+ }
+ } else if (s_cmp(c3, "TRI", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ }
+ } else if (s_cmp(c2, "PO", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ }
+ } else if (s_cmp(c2, "SY", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ } else if (sname && s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 32;
+ } else if (sname && s_cmp(c3, "GST", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 64;
+ }
+ } else if (cname && s_cmp(c2, "HE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 64;
+ } else if (s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 32;
+ } else if (s_cmp(c3, "GST", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 64;
+ }
+ } else if (sname && s_cmp(c2, "OR", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nb = 32;
+ }
+ } else if (*(unsigned char *)c3 == 'M') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nb = 32;
+ }
+ }
+ } else if (cname && s_cmp(c2, "UN", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nb = 32;
+ }
+ } else if (*(unsigned char *)c3 == 'M') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nb = 32;
+ }
+ }
+ } else if (s_cmp(c2, "GB", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ if (*n4 <= 64) {
+ nb = 1;
+ } else {
+ nb = 32;
+ }
+ } else {
+ if (*n4 <= 64) {
+ nb = 1;
+ } else {
+ nb = 32;
+ }
+ }
+ }
+ } else if (s_cmp(c2, "PB", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ if (*n2 <= 64) {
+ nb = 1;
+ } else {
+ nb = 32;
+ }
+ } else {
+ if (*n2 <= 64) {
+ nb = 1;
+ } else {
+ nb = 32;
+ }
+ }
+ }
+ } else if (s_cmp(c2, "TR", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRI", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ }
+ } else if (s_cmp(c2, "LA", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "UUM", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nb = 64;
+ } else {
+ nb = 64;
+ }
+ }
+ } else if (sname && s_cmp(c2, "ST", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "EBZ", (ftnlen)1, (ftnlen)3) == 0) {
+ nb = 1;
+ }
+ }
+ ret_val = nb;
+ return ret_val;
+
+L60:
+
+/* ISPEC = 2: minimum block size */
+
+ nbmin = 2;
+ if (s_cmp(c2, "GE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "QRF", (ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3, "RQF", (
+ ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3, "LQF", (ftnlen)1, (
+ ftnlen)3) == 0 || s_cmp(c3, "QLF", (ftnlen)1, (ftnlen)3) == 0)
+ {
+ if (sname) {
+ nbmin = 2;
+ } else {
+ nbmin = 2;
+ }
+ } else if (s_cmp(c3, "HRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nbmin = 2;
+ } else {
+ nbmin = 2;
+ }
+ } else if (s_cmp(c3, "BRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nbmin = 2;
+ } else {
+ nbmin = 2;
+ }
+ } else if (s_cmp(c3, "TRI", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nbmin = 2;
+ } else {
+ nbmin = 2;
+ }
+ }
+ } else if (s_cmp(c2, "SY", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRF", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nbmin = 8;
+ } else {
+ nbmin = 8;
+ }
+ } else if (sname && s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nbmin = 2;
+ }
+ } else if (cname && s_cmp(c2, "HE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nbmin = 2;
+ }
+ } else if (sname && s_cmp(c2, "OR", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nbmin = 2;
+ }
+ } else if (*(unsigned char *)c3 == 'M') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nbmin = 2;
+ }
+ }
+ } else if (cname && s_cmp(c2, "UN", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nbmin = 2;
+ }
+ } else if (*(unsigned char *)c3 == 'M') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nbmin = 2;
+ }
+ }
+ }
+ ret_val = nbmin;
+ return ret_val;
+
+L70:
+
+/* ISPEC = 3: crossover point */
+
+ nx = 0;
+ if (s_cmp(c2, "GE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "QRF", (ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3, "RQF", (
+ ftnlen)1, (ftnlen)3) == 0 || s_cmp(c3, "LQF", (ftnlen)1, (
+ ftnlen)3) == 0 || s_cmp(c3, "QLF", (ftnlen)1, (ftnlen)3) == 0)
+ {
+ if (sname) {
+ nx = 128;
+ } else {
+ nx = 128;
+ }
+ } else if (s_cmp(c3, "HRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nx = 128;
+ } else {
+ nx = 128;
+ }
+ } else if (s_cmp(c3, "BRD", (ftnlen)1, (ftnlen)3) == 0) {
+ if (sname) {
+ nx = 128;
+ } else {
+ nx = 128;
+ }
+ }
+ } else if (s_cmp(c2, "SY", (ftnlen)1, (ftnlen)2) == 0) {
+ if (sname && s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nx = 32;
+ }
+ } else if (cname && s_cmp(c2, "HE", (ftnlen)1, (ftnlen)2) == 0) {
+ if (s_cmp(c3, "TRD", (ftnlen)1, (ftnlen)3) == 0) {
+ nx = 32;
+ }
+ } else if (sname && s_cmp(c2, "OR", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nx = 128;
+ }
+ }
+ } else if (cname && s_cmp(c2, "UN", (ftnlen)1, (ftnlen)2) == 0) {
+ if (*(unsigned char *)c3 == 'G') {
+ if (s_cmp(c4, "QR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "RQ",
+ (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "LQ", (ftnlen)1, (
+ ftnlen)2) == 0 || s_cmp(c4, "QL", (ftnlen)1, (ftnlen)2) ==
+ 0 || s_cmp(c4, "HR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(
+ c4, "TR", (ftnlen)1, (ftnlen)2) == 0 || s_cmp(c4, "BR", (
+ ftnlen)1, (ftnlen)2) == 0) {
+ nx = 128;
+ }
+ }
+ }
+ ret_val = nx;
+ return ret_val;
+
+L80:
+
+/* ISPEC = 4: number of shifts (used by xHSEQR) */
+
+ ret_val = 6;
+ return ret_val;
+
+L90:
+
+/* ISPEC = 5: minimum column dimension (not used) */
+
+ ret_val = 2;
+ return ret_val;
+
+L100:
+
+/* ISPEC = 6: crossover point for SVD (used by xGELSS and xGESVD) */
+
+ ret_val = (integer) ((real) min(*n1,*n2) * 1.6f);
+ return ret_val;
+
+L110:
+
+/* ISPEC = 7: number of processors (not used) */
+
+ ret_val = 1;
+ return ret_val;
+
+L120:
+
+/* ISPEC = 8: crossover point for multishift (used by xHSEQR) */
+
+ ret_val = 50;
+ return ret_val;
+
+L130:
+
+/* ISPEC = 9: maximum size of the subproblems at the bottom of the */
+/* computation tree in the divide-and-conquer algorithm */
+/* (used by xGELSD and xGESDD) */
+
+ ret_val = 25;
+ return ret_val;
+
+L140:
+
+/* ISPEC = 10: ieee NaN arithmetic can be trusted not to trap */
+
+/* ILAENV = 0 */
+ ret_val = 1;
+ if (ret_val == 1) {
+ ret_val = ieeeck_(&c__1, &c_b163, &c_b164);
+ }
+ return ret_val;
+
+L150:
+
+/* ISPEC = 11: infinity arithmetic can be trusted not to trap */
+
+/* ILAENV = 0 */
+ ret_val = 1;
+ if (ret_val == 1) {
+ ret_val = ieeeck_(&c__0, &c_b163, &c_b164);
+ }
+ return ret_val;
+
+L160:
+
+/* 12 <= ISPEC <= 16: xHSEQR or one of its subroutines. */
+
+ ret_val = iparmq_(ispec, name__, opts, n1, n2, n3, n4)
+ ;
+ return ret_val;
+
+/* End of ILAENV */
+
+} /* ilaenv_ */
diff --git a/3rdparty/clapack/src/iparmq.c b/3rdparty/clapack/src/iparmq.c
new file mode 100644
index 0000000..13fb9aa
--- /dev/null
+++ b/3rdparty/clapack/src/iparmq.c
@@ -0,0 +1,282 @@
+/* iparmq.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+
+integer iparmq_(integer *ispec, char *name__, char *opts, integer *n, integer
+ *ilo, integer *ihi, integer *lwork)
+{
+ /* System generated locals */
+ integer ret_val, i__1, i__2;
+ real r__1;
+
+ /* Builtin functions */
+ double log(doublereal);
+ integer i_nint(real *);
+
+ /* Local variables */
+ integer nh, ns;
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+
+/* Purpose */
+/* ======= */
+
+/* This program sets problem and machine dependent parameters */
+/* useful for xHSEQR and its subroutines. It is called whenever */
+/* ILAENV is called with 12 <= ISPEC <= 16 */
+
+/* Arguments */
+/* ========= */
+
+/* ISPEC (input) integer scalar */
+/* ISPEC specifies which tunable parameter IPARMQ should */
+/* return. */
+
+/* ISPEC=12: (INMIN) Matrices of order nmin or less */
+/* are sent directly to xLAHQR, the implicit */
+/* double shift QR algorithm. NMIN must be */
+/* at least 11. */
+
+/* ISPEC=13: (INWIN) Size of the deflation window. */
+/* This is best set greater than or equal to */
+/* the number of simultaneous shifts NS. */
+/* Larger matrices benefit from larger deflation */
+/* windows. */
+
+/* ISPEC=14: (INIBL) Determines when to stop nibbling and */
+/* invest in an (expensive) multi-shift QR sweep. */
+/* If the aggressive early deflation subroutine */
+/* finds LD converged eigenvalues from an order */
+/* NW deflation window and LD.GT.(NW*NIBBLE)/100, */
+/* then the next QR sweep is skipped and early */
+/* deflation is applied immediately to the */
+/* remaining active diagonal block. Setting */
+/* IPARMQ(ISPEC=14) = 0 causes TTQRE to skip a */
+/* multi-shift QR sweep whenever early deflation */
+/* finds a converged eigenvalue. Setting */
+/* IPARMQ(ISPEC=14) greater than or equal to 100 */
+/* prevents TTQRE from skipping a multi-shift */
+/* QR sweep. */
+
+/* ISPEC=15: (NSHFTS) The number of simultaneous shifts in */
+/* a multi-shift QR iteration. */
+
+/* ISPEC=16: (IACC22) IPARMQ is set to 0, 1 or 2 with the */
+/* following meanings. */
+/* 0: During the multi-shift QR sweep, */
+/* xLAQR5 does not accumulate reflections and */
+/* does not use matrix-matrix multiply to */
+/* update the far-from-diagonal matrix */
+/* entries. */
+/* 1: During the multi-shift QR sweep, */
+/* xLAQR5 and/or xLAQRaccumulates reflections and uses */
+/* matrix-matrix multiply to update the */
+/* far-from-diagonal matrix entries. */
+/* 2: During the multi-shift QR sweep. */
+/* xLAQR5 accumulates reflections and takes */
+/* advantage of 2-by-2 block structure during */
+/* matrix-matrix multiplies. */
+/* (If xTRMM is slower than xGEMM, then */
+/* IPARMQ(ISPEC=16)=1 may be more efficient than */
+/* IPARMQ(ISPEC=16)=2 despite the greater level of */
+/* arithmetic work implied by the latter choice.) */
+
+/* NAME (input) character string */
+/* Name of the calling subroutine */
+
+/* OPTS (input) character string */
+/* This is a concatenation of the string arguments to */
+/* TTQRE. */
+
+/* N (input) integer scalar */
+/* N is the order of the Hessenberg matrix H. */
+
+/* ILO (input) INTEGER */
+/* IHI (input) INTEGER */
+/* It is assumed that H is already upper triangular */
+/* in rows and columns 1:ILO-1 and IHI+1:N. */
+
+/* LWORK (input) integer scalar */
+/* The amount of workspace available. */
+
+/* Further Details */
+/* =============== */
+
+/* Little is known about how best to choose these parameters. */
+/* It is possible to use different values of the parameters */
+/* for each of CHSEQR, DHSEQR, SHSEQR and ZHSEQR. */
+
+/* It is probably best to choose different parameters for */
+/* different matrices and different parameters at different */
+/* times during the iteration, but this has not been */
+/* implemented --- yet. */
+
+
+/* The best choices of most of the parameters depend */
+/* in an ill-understood way on the relative execution */
+/* rate of xLAQR3 and xLAQR5 and on the nature of each */
+/* particular eigenvalue problem. Experiment may be the */
+/* only practical way to determine which choices are most */
+/* effective. */
+
+/* Following is a list of default values supplied by IPARMQ. */
+/* These defaults may be adjusted in order to attain better */
+/* performance in any particular computational environment. */
+
+/* IPARMQ(ISPEC=12) The xLAHQR vs xLAQR0 crossover point. */
+/* Default: 75. (Must be at least 11.) */
+
+/* IPARMQ(ISPEC=13) Recommended deflation window size. */
+/* This depends on ILO, IHI and NS, the */
+/* number of simultaneous shifts returned */
+/* by IPARMQ(ISPEC=15). The default for */
+/* (IHI-ILO+1).LE.500 is NS. The default */
+/* for (IHI-ILO+1).GT.500 is 3*NS/2. */
+
+/* IPARMQ(ISPEC=14) Nibble crossover point. Default: 14. */
+
+/* IPARMQ(ISPEC=15) Number of simultaneous shifts, NS. */
+/* a multi-shift QR iteration. */
+
+/* If IHI-ILO+1 is ... */
+
+/* greater than ...but less ... the */
+/* or equal to ... than default is */
+
+/* 0 30 NS = 2+ */
+/* 30 60 NS = 4+ */
+/* 60 150 NS = 10 */
+/* 150 590 NS = ** */
+/* 590 3000 NS = 64 */
+/* 3000 6000 NS = 128 */
+/* 6000 infinity NS = 256 */
+
+/* (+) By default matrices of this order are */
+/* passed to the implicit double shift routine */
+/* xLAHQR. See IPARMQ(ISPEC=12) above. These */
+/* values of NS are used only in case of a rare */
+/* xLAHQR failure. */
+
+/* (**) The asterisks (**) indicate an ad-hoc */
+/* function increasing from 10 to 64. */
+
+/* IPARMQ(ISPEC=16) Select structured matrix multiply. */
+/* (See ISPEC=16 above for details.) */
+/* Default: 3. */
+
+/* ================================================================ */
+/* .. Parameters .. */
+/* .. */
+/* .. Local Scalars .. */
+/* .. */
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+ if (*ispec == 15 || *ispec == 13 || *ispec == 16) {
+
+/* ==== Set the number simultaneous shifts ==== */
+
+ nh = *ihi - *ilo + 1;
+ ns = 2;
+ if (nh >= 30) {
+ ns = 4;
+ }
+ if (nh >= 60) {
+ ns = 10;
+ }
+ if (nh >= 150) {
+/* Computing MAX */
+ r__1 = log((real) nh) / log(2.f);
+ i__1 = 10, i__2 = nh / i_nint(&r__1);
+ ns = max(i__1,i__2);
+ }
+ if (nh >= 590) {
+ ns = 64;
+ }
+ if (nh >= 3000) {
+ ns = 128;
+ }
+ if (nh >= 6000) {
+ ns = 256;
+ }
+/* Computing MAX */
+ i__1 = 2, i__2 = ns - ns % 2;
+ ns = max(i__1,i__2);
+ }
+
+ if (*ispec == 12) {
+
+
+/* ===== Matrices of order smaller than NMIN get sent */
+/* . to xLAHQR, the classic double shift algorithm. */
+/* . This must be at least 11. ==== */
+
+ ret_val = 75;
+
+ } else if (*ispec == 14) {
+
+/* ==== INIBL: skip a multi-shift qr iteration and */
+/* . whenever aggressive early deflation finds */
+/* . at least (NIBBLE*(window size)/100) deflations. ==== */
+
+ ret_val = 14;
+
+ } else if (*ispec == 15) {
+
+/* ==== NSHFTS: The number of simultaneous shifts ===== */
+
+ ret_val = ns;
+
+ } else if (*ispec == 13) {
+
+/* ==== NW: deflation window size. ==== */
+
+ if (nh <= 500) {
+ ret_val = ns;
+ } else {
+ ret_val = ns * 3 / 2;
+ }
+
+ } else if (*ispec == 16) {
+
+/* ==== IACC22: Whether to accumulate reflections */
+/* . before updating the far-from-diagonal elements */
+/* . and whether to use 2-by-2 block structure while */
+/* . doing it. A small amount of work could be saved */
+/* . by making this choice dependent also upon the */
+/* . NH=IHI-ILO+1. */
+
+ ret_val = 0;
+ if (ns >= 14) {
+ ret_val = 1;
+ }
+ if (ns >= 14) {
+ ret_val = 2;
+ }
+
+ } else {
+/* ===== invalid value of ispec ===== */
+ ret_val = -1;
+
+ }
+
+/* ==== End of IPARMQ ==== */
+
+ return ret_val;
+} /* iparmq_ */
diff --git a/3rdparty/clapack/src/xerbla.c b/3rdparty/clapack/src/xerbla.c
new file mode 100644
index 0000000..f552601
--- /dev/null
+++ b/3rdparty/clapack/src/xerbla.c
@@ -0,0 +1,65 @@
+/* xerbla.f -- translated by f2c (version 20061008).
+ You must link the resulting object file with libf2c:
+ on Microsoft Windows system, link with libf2c.lib;
+ on Linux or Unix systems, link with .../path/to/libf2c.a -lm
+ or, if you install libf2c.a in a standard place, with -lf2c -lm
+ -- in that order, at the end of the command line, as in
+ cc *.o -lf2c -lm
+ Source for libf2c is in /netlib/f2c/libf2c.zip, e.g.,
+
+ http://www.netlib.org/f2c/libf2c.zip
+*/
+
+#include "f2c.h"
+#include "blaswrap.h"
+#include "stdio.h"
+
+/* Table of constant values */
+
+static integer c__1 = 1;
+
+/* Subroutine */ int xerbla_(char *srname, integer *info)
+{
+
+
+/* -- LAPACK auxiliary routine (version 3.2) -- */
+/* Univ. of Tennessee, Univ. of California Berkeley and NAG Ltd.. */
+/* November 2006 */
+
+/* .. Scalar Arguments .. */
+/* .. */
+
+/* Purpose */
+/* ======= */
+
+/* XERBLA is an error handler for the LAPACK routines. */
+/* It is called by an LAPACK routine if an input parameter has an */
+/* invalid value. A message is printed and execution stops. */
+
+/* Installers may consider modifying the STOP statement in order to */
+/* call system-specific exception-handling facilities. */
+
+/* Arguments */
+/* ========= */
+
+/* SRNAME (input) CHARACTER*(*) */
+/* The name of the routine which called XERBLA. */
+
+/* INFO (input) INTEGER */
+/* The position of the invalid parameter in the parameter list */
+/* of the calling routine. */
+
+/* ===================================================================== */
+
+/* .. Intrinsic Functions .. */
+/* .. */
+/* .. Executable Statements .. */
+
+ printf("** On entry to %6s, parameter number %2i had an illegal value\n",
+ srname, (int)(*info));
+
+
+/* End of XERBLA */
+
+ return 0;
+} /* xerbla_ */
diff --git a/3rdparty/clipper/CMakeLists.txt b/3rdparty/clipper/CMakeLists.txt
new file mode 100644
index 0000000..a2aea78
--- /dev/null
+++ b/3rdparty/clipper/CMakeLists.txt
@@ -0,0 +1,34 @@
+project(${CLIPPER_LIBRARY})
+
+set(CLIPPER_MAJOR_VERSION 6 PARENT_SCOPE)
+set(CLIPPER_MINOR_VERSION 4 PARENT_SCOPE)
+set(CLIPPER_PATCH_VERSION 2 PARENT_SCOPE)
+
+vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+
+file(GLOB lib_srcs *.cpp)
+file(GLOB lib_hdrs *.hpp)
+
+add_library(${CLIPPER_LIBRARY} STATIC ${lib_srcs} ${lib_hdrs})
+
+if(UNIX)
+ if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
+ endif()
+endif()
+
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${CLIPPER_LIBRARY} PROPERTIES FOLDER "3rdparty")
+endif()
+
+set_target_properties(${CLIPPER_LIBRARY}
+ PROPERTIES OUTPUT_NAME ${CLIPPER_LIBRARY}
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ COMPILE_PDB_NAME ${CLIPPER_LIBRARY}
+ COMPILE_PDB_NAME_DEBUG "${CLIPPER_LIBRARY}${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
+ )
+
+if(NOT BUILD_SHARED_LIBS)
+ vp_install_target(${CLIPPER_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
+endif()
diff --git a/3rdparty/clipper/License.txt b/3rdparty/clipper/License.txt
new file mode 100644
index 0000000..3d94797
--- /dev/null
+++ b/3rdparty/clipper/License.txt
@@ -0,0 +1,24 @@
+Boost Software License - Version 1.0 - August 17th, 2003
+http://www.boost.org/LICENSE_1_0.txt
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
\ No newline at end of file
diff --git a/3rdparty/clipper/README b/3rdparty/clipper/README
new file mode 100644
index 0000000..a3ed26f
--- /dev/null
+++ b/3rdparty/clipper/README
@@ -0,0 +1,413 @@
+=====================================================================
+Clipper Change Log
+=====================================================================
+v6.4.2 (27 February 2017) Rev 512
+* Several minor bugfixes: #152 #160 #161 #162
+
+v6.4 (2 July 2015) Rev 495
+* Numerous generally minor bugfixes
+
+v6.2.1 (31 October 2014) Rev 482
+* Bugfix in ClipperOffset.Execute where the Polytree.IsHole property
+ was returning incorrect values with negative offsets
+* Very minor improvement to join rounding in ClipperOffset
+* Fixed CPP OpenGL demo.
+
+v6.2.0 (17 October 2014) Rev 477
+* Numerous minor bugfixes, too many to list.
+ (See revisions 454-475 in Sourceforge Repository)
+* The ZFillFunction (custom callback function) has had its parameters
+ changed.
+* Curves demo removed (temporarily).
+* Deprecated functions have been removed.
+
+v6.1.5 (26 February 2014) Rev 460
+* Improved the joining of output polygons sharing a common edge
+ when those common edges are horizontal.
+* Fixed a bug in ClipperOffset.AddPath() which would produce
+ incorrect solutions when open paths were added before closed paths.
+* Minor code tidy and performance improvement
+
+v6.1.4 (6 February 2014)
+* Fixed bugs in MinkowskiSum
+* Fixed minor bug when using Clipper.ForceSimplify.
+* Modified use_xyz callback so that all 4 vertices around an
+ intersection point are now passed to the callback function.
+
+v6.1.3a (22 January 2014) Rev 453
+* Fixed buggy PointInPolygon function (C++ and C# only).
+ Note this bug only affected the newly exported function, the
+ internal PointInPolygon function used by Clipper was OK.
+
+v6.1.3 (19 January 2014) Rev 452
+* Fixed potential endless loop condition when adding open
+ paths to Clipper.
+* Fixed missing implementation of SimplifyPolygon function
+ in C++ code.
+* Fixed incorrect upper range constant for polygon coordinates
+ in Delphi code.
+* Added PointInPolygon function.
+* Overloaded MinkowskiSum function to accommodate multi-contour
+ paths.
+
+v6.1.2 (15 December 2013) Rev 444
+* Fixed broken C++ header file.
+* Minor improvement to joining polygons.
+
+v6.1.1 (13 December 2013) Rev 441
+* Fixed a couple of bugs affecting open paths that could
+ raise unhandled exceptions.
+
+v6.1.0 (12 December 2013)
+* Deleted: Previously deprecated code has been removed.
+* Modified: The OffsetPaths function is now deprecated as it has
+ been replaced by the ClipperOffset class which is much more
+ flexible.
+* Bugfixes: Several minor bugs have been fixed including
+ occasionally an incorrect nesting within the PolyTree structure.
+
+v6.0.0 (30 October 2013)
+* Added: Open path (polyline) clipping. A new 'Curves' demo
+ application showcases this (see the 'Curves' directory).
+* Update: Major improvement in the merging of
+ shared/collinear edges in clip solutions (see Execute).
+* Added: The IntPoint structure now has an optional 'Z' member.
+ (See the precompiler directive use_xyz.)
+* Added: Users can now force Clipper to use 32bit integers
+ (via the precompiler directive use_int32) instead of using
+ 64bit integers.
+* Modified: To accommodate open paths, the Polygon and Polygons
+ structures have been renamed Path and Paths respectively. The
+ AddPolygon and AddPolygons methods of the ClipperBase class
+ have been renamed AddPath and AddPaths respectively. Several
+ other functions have been similarly renamed.
+* Modified: The PolyNode Class has a new IsOpen property.
+* Modified: The Clipper class has a new ZFillFunction property.
+* Added: MinkowskiSum and MinkowskiDiff functions added.
+* Added: Several other new functions have been added including
+ PolyTreeToPaths, OpenPathsFromPolyTree and ClosedPathsFromPolyTree.
+* Added: The Clipper constructor now accepts an optional InitOptions
+ parameter to simplify setting properties.
+* Bugfixes: Numerous minor bugs have been fixed.
+* Deprecated: Version 6 is a major upgrade from previous versions
+ and quite a number of changes have been made to exposed structures
+ and functions. To minimize inconvenience to existing library users,
+ some code has been retained and some added to maintain backward
+ compatibility. However, because this code will be removed in a
+ future update, it has been marked as deprecated and a precompiler
+ directive use_deprecated has been defined.
+
+v5.1.6 (23 May 2013)
+* BugFix: CleanPolygon function was buggy.
+* Changed: The behaviour of the 'miter' JoinType has been
+ changed so that when squaring occurs, it's no longer
+ extended up to the miter limit but is squared off at
+ exactly 'delta' units. (This improves the look of mitering
+ with larger limits at acute angles.)
+* Added: New OffsetPolyLines function
+* Update: Minor code refactoring and optimisations
+
+v5.1.5 (5 May 2013)
+* Added: ForceSimple property to Clipper class
+* Update: Improved documentation
+
+v5.1.4 (24 March 2013)
+* Update: CleanPolygon function enhanced.
+* Update: Documentation improved.
+
+v5.1.3 (14 March 2013)
+* Bugfix: Minor bugfixes.
+* Update: Documentation significantly improved.
+
+v5.1.2 (26 February 2013)
+* Bugfix: PolyNode class was missing a constructor.
+* Update: The MiterLimit parameter in the OffsetPolygons
+ function has been renamed Limit and can now also be used to
+ limit the number of vertices used to construct arcs when
+ JoinType is set to jtRound.
+
+v5.1.0 (17 February 2013)
+* Update: ExPolygons has been replaced with the PolyTree &
+ PolyNode classes to more fully represent the parent-child
+ relationships of the polygons returned by Clipper.
+* Added: New CleanPolygon and CleanPolygons functions.
+* Bugfix: Another orientation bug fixed.
+
+v5.0.2 - 30 December 2012
+* Bugfix: Significant fixes in and tidy of the internal
+ Int128 class (which is used only when polygon coordinate
+ values are greater than �0x3FFFFFFF (~1.07e9)).
+* Update: The Area algorithm has been updated and is faster.
+* Update: Documentation updates. The newish but undocumented
+ 'CheckInputs' parameter of the OffsetPolygons function has been
+ renamed 'AutoFix' and documented too. The comments on rounding
+ have also been improved (ie clearer and expanded).
+
+v4.10.0 - 25 December 2012
+* Bugfix: Orientation bugs should now be resolved (finally!).
+* Bugfix: Bug in Int128 class
+
+v4.9.8 - 2 December 2012
+* Bugfix: Further fixes to rare Orientation bug.
+
+v4.9.7 - 29 November 2012
+* Bugfix: Bug that very rarely returned the wrong polygon
+ orientation.
+* Bugfix: Obscure bug affecting OffsetPolygons when using
+ jtRound for the JoinType parameter and when polygons also
+ contain very large coordinate values (> +/-100000000000).
+
+v4.9.6 - 9 November 2012
+* Bugfix: Another obscure bug related to joining polygons.
+
+v4.9.4 - 2 November 2012
+* Bugfix: Bugs in Int128 class occasionally causing
+ wrong orientations.
+* Bugfix: Further fixes related to joining polygons.
+
+v4.9.0 - 9 October 2012
+* Bugfix: Obscure bug related to joining polygons.
+
+v4.8.9 - 25 September 2012
+* Bugfix: Obscure bug related to precision of intersections.
+
+v4.8.8 - 30 August 2012
+* Bugfix: Fixed bug in OffsetPolygons function introduced in
+ version 4.8.5.
+
+v4.8.7 - 24 August 2012
+* Bugfix: ReversePolygon function in C++ translation was broken.
+* Bugfix: Two obscure bugs affecting orientation fixed too.
+
+v4.8.6 - 11 August 2012
+* Bugfix: Potential for memory overflow errors when using
+ ExPolygons structure.
+* Bugfix: The polygon coordinate range has been reduced to
+ +/- 0x3FFFFFFFFFFFFFFF (4.6e18).
+* Update: ReversePolygons function was misnamed ReversePoints in C++.
+* Update: SimplifyPolygon function now takes a PolyFillType parameter.
+
+v4.8.5 - 15 July 2012
+* Bugfix: Potential for memory overflow errors in OffsetPolygons().
+
+v4.8.4 - 1 June 2012
+* Bugfix: Another obscure bug affecting ExPolygons structure.
+
+v4.8.3 - 27 May 2012
+* Bugfix: Obscure bug causing incorrect removal of a vertex.
+
+v4.8.2 - 21 May 2012
+* Bugfix: Obscure bug could cause an exception when using
+ ExPolygon structure.
+
+v4.8.1 - 12 May 2012
+* Update: Cody tidy and minor bug fixes.
+
+v4.8.0 - 30 April 2012
+* Bugfix: Occasional errors in orientation fixed.
+* Update: Added notes on rounding to the documentation.
+
+v4.7.6 - 11 April 2012
+* Fixed a bug in Orientation function (affecting C# translations only).
+* Minor documentation update.
+
+v4.7.5 - 28 March 2012
+* Bugfix: Fixed a recently introduced bug that occasionally caused an
+ unhandled exception in C++ and C# translations.
+
+v4.7.4 - 15 March 2012
+* Bugfix: Another minor bugfix.
+
+v4.7.2 - 4 March 2012
+* Bugfix: Fixed bug introduced in ver 4.7 which sometimes caused
+ an exception if ExPolygon structure was passed to Clipper's
+ Execute method.
+
+v4.7.1 - 3 March 2012
+* Bugfix: Rare crash when JoinCommonEdges joined polygons that
+ 'cancelled' each other.
+* Bugfix: Clipper's internal Orientation method occasionally
+ returned wrong result.
+* Update: Improved C# code (thanks to numerous excellent suggestions
+ from David Piepgrass)
+
+v4.7 - 10 February 2012
+* Improved the joining of output polygons sharing a common edge.
+
+v4.6.6 - 3 February 2012
+* Bugfix: Another obscure bug occasionally causing incorrect
+ polygon orientation.
+
+v4.6.5 - 17 January 2012
+* Bugfix: Obscure bug occasionally causing incorrect hole
+ assignment in ExPolygon structure.
+
+v4.6.4 - 8 November 2011
+* Added: SimplifyPolygon and SimplifyPolygons functions.
+
+v4.6.3 - 11 November 2011
+* Bugfix: Fixed another minor mitering bug in OffsetPolygons.
+
+v4.6.2 - 10 November 2011
+* Bugfix: Fixed a rare bug in the orientation of polygons
+ returned by Clipper's Execute() method.
+* Bugfix: Previous update introduced a mitering bug in the
+ OffsetPolygons function.
+
+v4.6 - 29 October 2011
+* Added: Support for Positive and Negative polygon fill
+ types (in addition to the EvenOdd and NonZero fill types).
+* Bugfix: The OffsetPolygons function was generating the
+ occasional artefact when 'shrinking' polygons.
+
+v4.5.5 - 8 October 2011
+* Bugfix: Fixed an obscure bug in Clipper's JoinCommonEdges
+ method.
+* Update: Replaced IsClockwise function with Orientation
+ function. The orientation issues affecting OffsetPolygons
+ should now be finally resolved.
+* Change: The Area function once again returns a signed value.
+
+v4.5.1 - 28 September 2011
+* Deleted: The UseFullCoordinateRange property has been
+ deleted since integer range is now managed implicitly.
+* BugFix: Minor bug in OffsetPolygon mitering.
+* Change: C# JoinType enum moved from Clipper class to
+ ClipperLib namespace.
+* Change: The Area function now returns the absolute area
+ (irrespective of orientation).
+* Change: The IsClockwise function now requires a second
+ parameter - YAxisPositiveUpward - to accommodate displays
+ with Y-axis oriented in either direction
+
+v4.4.4 - 10 September 2011
+* Change: Deleted jtButt from JoinType (used by the
+ OffsetPolygons function).
+* BugFix: Fixed another minor bug in OffsetPolygons function.
+* Update: Further improvements to the help file
+
+v4.4.3 - 29 August 2011
+* BugFix: fixed a minor rounding issue in OffsetPolygons
+ function (affected C++ & C# translations).
+* BugFix: fixed a minor bug in OffsetPolygons' function
+ declaration (affected C++ translation only).
+* Change: 'clipper' namespace changed to 'ClipperLib'
+ namespace in both C++ and C# code to remove the ambiguity
+ between the Clipper class and the namespace. (This also
+ required numerous updates to the accompanying demos.)
+
+v4.4.2 - 26 August 2011
+* BugFix: minor bugfixes in Clipper.
+* Update: the OffsetPolygons function has been significantly
+ improved by offering 4 different join styles.
+
+v4.4.0 - 6 August 2011
+* BugFix: A number of minor bugs have been fixed that mostly
+ affected the new ExPolygons structure.
+
+v4.3.0 - 17 June 2011
+* New: ExPolygons structure that explicitly associates 'hole'
+ polygons with their 'outer' container polygons.
+* New: Execute method overloaded so the solution parameter
+ can now be either Polygons or ExPolygons.
+* BugFix: Fixed a rare bug in solution polygons orientation.
+
+v4.2.8 - 21 May 2011
+* Update: JoinCommonEdges() improved once more.
+* BugFix: Several minor bugs fixed.
+
+v4.2.6 - 1 May 2011
+* Bugfix: minor bug in SlopesEqual function.
+* Update: Merging of output polygons sharing common edges
+ has been significantly inproved
+
+v4.2.4 - 26 April 2011
+ Input polygon coordinates can now contain the full range of
+ signed 64bit integers (ie +/-9,223,372,036,854,775,807). This
+ means that floating point values can be converted to and from
+ Clipper's 64bit integer coordinates structure (IntPoint) and
+ still retain a precision of up to 18 decimal places. However,
+ since the large-integer math that supports this expanded range
+ imposes a small cost on performance (~15%), a new property
+ UseFullCoordinateRange has been added to the Clipper class to
+ allow users the choice of whether or not to use this expanded
+ coordinate range. If this property is disabled, coordinate values
+ are restricted to +/-1,500,000,000.
+
+v4.2 - 12 April 2011
+ JoinCommonEdges() code significantly improved plus other minor
+ improvements.
+
+v4.1.2 - 9 April 2011
+* Update: Minor code tidy.
+* Bugfix: Possible endless loop in JoinCommonEdges() in clipper.pas.
+
+v4.1.1 - 8 April 2011
+* Update: All polygon coordinates are now stored as 64bit integers
+ (though they're still restricted to range -1.5e9 to +1.5e9 pending
+ the inclusion of code supporting 64bit math).
+* Change: AddPolygon and AddPolygons methods now return boolean
+ values.
+* Bugfix: Bug in JoinCommonEdges() caused potential endless loop.
+* Bugfix: Bug in IsClockwise(). (C++ code only)
+
+v4.0 - 5 April 2011
+* Clipper 4 is a major rewrite of earlier versions. The biggest
+ change is that floating point values are no longer used,
+ except for the storing of edge slope values. The main benefit
+ of this is the issue of numerical robustness has been
+ addressed. Due to other major code improvements Clipper v4
+ is approximately 40% faster than Clipper v3.
+* The AddPolyPolygon method has been renamed to AddPolygons.
+* The IgnoreOrientation property has been removed.
+* The clipper_misc library has been merged back into the
+ main clipper library.
+
+v3.1.0 - 17 February 2011
+* Bugfix: Obscure bug in TClipperBase.SetDx method that caused
+ problems with very small edges ( edges <1/1000th pixel in size).
+
+v3.0.3 - 9 February 2011
+* Bugfix: Significant bug, but only in C# code.
+* Update: Minor refactoring.
+
+v3.0 - 31 January 2011
+* Update: Major rewrite of the portion of code that calculates
+ the output polygons' orientation.
+* Update: Help file significantly improved.
+* Change: Renamed ForceOrientation property to IgnoreOrientation.
+ If the orientation of output polygons is not important, or can
+ be managed separately, clipping routines can be sped up by about
+ 60% by setting IgnoreOrientation to true. Defaults to false.
+* Change: The OffsetPolygon and Area functions have been moved to
+ the new unit - clipper_misc.
+
+2.99 - 15 January 2011
+* Bugfix: Obscure bug in AddPolygon method could cause an endless loop.
+
+2.8 - 20 November 2010
+* Updated: Output polygons which previously shared a common
+ edge are now merged.
+* Changed: The orientation of outer polygons is now clockwise
+ when the display's Y axis is positive downwards (as is
+ typical for most Windows applications). Inner polygons
+ (holes) have the opposite orientation.
+* Added: Support module for Cairo Graphics Library (with demo).
+* Updated: C# and C++ demos.
+
+2.522 - 15 October 2010
+* Added C# translation (thanks to Olivier Lejeune) and
+ a link to Ruby bindings (thanks to Mike Owens).
+
+2.0 - 30 July 2010
+* Clipper now clips using both the Even-Odd (alternate) and
+ Non-Zero (winding) polygon filling rules. (Previously Clipper
+ assumed the Even-Odd rule for polygon filling.)
+
+1.4c - 16 June 2010
+* Added C++ support for AGG graphics library
+
+1.2s - 2 June 2010
+* Added C++ translation of clipper.pas
+
+1.0 - 9 May 2010
\ No newline at end of file
diff --git a/3rdparty/clipper/clipper.cpp b/3rdparty/clipper/clipper.cpp
new file mode 100644
index 0000000..d0e5ba2
--- /dev/null
+++ b/3rdparty/clipper/clipper.cpp
@@ -0,0 +1,4629 @@
+/*******************************************************************************
+* *
+* Author : Angus Johnson *
+* Version : 6.4.2 *
+* Date : 27 February 2017 *
+* Website : http://www.angusj.com *
+* Copyright : Angus Johnson 2010-2017 *
+* *
+* License: *
+* Use, modification & distribution is subject to Boost Software License Ver 1. *
+* http://www.boost.org/LICENSE_1_0.txt *
+* *
+* Attributions: *
+* The code in this library is an extension of Bala Vatti's clipping algorithm: *
+* "A generic solution to polygon clipping" *
+* Communications of the ACM, Vol 35, Issue 7 (July 1992) pp 56-63. *
+* http://portal.acm.org/citation.cfm?id=129906 *
+* *
+* Computer graphics and geometric modeling: implementation and algorithms *
+* By Max K. Agoston *
+* Springer; 1 edition (January 4, 2005) *
+* http://books.google.com/books?q=vatti+clipping+agoston *
+* *
+* See also: *
+* "Polygon Offsetting by Computing Winding Numbers" *
+* Paper no. DETC2005-85513 pp. 565-575 *
+* ASME 2005 International Design Engineering Technical Conferences *
+* and Computers and Information in Engineering Conference (IDETC/CIE2005) *
+* September 24-28, 2005 , Long Beach, California, USA *
+* http://www.me.berkeley.edu/~mcmains/pubs/DAC05OffsetPolygon.pdf *
+* *
+*******************************************************************************/
+
+/*******************************************************************************
+* *
+* This is a translation of the Delphi Clipper library and the naming style *
+* used has retained a Delphi flavour. *
+* *
+*******************************************************************************/
+
+#include "clipper.hpp"
+#include <cmath>
+#include <vector>
+#include <algorithm>
+#include <stdexcept>
+#include <cstring>
+#include <cstdlib>
+#include <ostream>
+#include <functional>
+
+namespace ClipperLib {
+
+static double const pi = 3.141592653589793238;
+static double const two_pi = pi *2;
+static double const def_arc_tolerance = 0.25;
+
+enum Direction { dRightToLeft, dLeftToRight };
+
+static int const Unassigned = -1; //edge not currently 'owning' a solution
+static int const Skip = -2; //edge that would otherwise close a path
+
+#define HORIZONTAL (-1.0E+40)
+#define TOLERANCE (1.0e-20)
+#define NEAR_ZERO(val) (((val) > -TOLERANCE) && ((val) < TOLERANCE))
+
+struct TEdge {
+ IntPoint Bot;
+ IntPoint Curr; //current (updated for every new scanbeam)
+ IntPoint Top;
+ double Dx;
+ PolyType PolyTyp;
+ EdgeSide Side; //side only refers to current side of solution poly
+ int WindDelta; //1 or -1 depending on winding direction
+ int WindCnt;
+ int WindCnt2; //winding count of the opposite polytype
+ int OutIdx;
+ TEdge *Next;
+ TEdge *Prev;
+ TEdge *NextInLML;
+ TEdge *NextInAEL;
+ TEdge *PrevInAEL;
+ TEdge *NextInSEL;
+ TEdge *PrevInSEL;
+};
+
+struct IntersectNode {
+ TEdge *Edge1;
+ TEdge *Edge2;
+ IntPoint Pt;
+};
+
+struct LocalMinimum {
+ cInt Y;
+ TEdge *LeftBound;
+ TEdge *RightBound;
+};
+
+struct OutPt;
+
+//OutRec: contains a path in the clipping solution. Edges in the AEL will
+//carry a pointer to an OutRec when they are part of the clipping solution.
+struct OutRec {
+ int Idx;
+ bool IsHole;
+ bool IsOpen;
+ OutRec *FirstLeft; //see comments in clipper.pas
+ PolyNode *PolyNd;
+ OutPt *Pts;
+ OutPt *BottomPt;
+};
+
+struct OutPt {
+ int Idx;
+ IntPoint Pt;
+ OutPt *Next;
+ OutPt *Prev;
+};
+
+struct Join {
+ OutPt *OutPt1;
+ OutPt *OutPt2;
+ IntPoint OffPt;
+};
+
+struct LocMinSorter
+{
+ inline bool operator()(const LocalMinimum& locMin1, const LocalMinimum& locMin2)
+ {
+ return locMin2.Y < locMin1.Y;
+ }
+};
+
+//------------------------------------------------------------------------------
+//------------------------------------------------------------------------------
+
+inline cInt Round(double val)
+{
+ if ((val < 0)) return static_cast<cInt>(val - 0.5);
+ else return static_cast<cInt>(val + 0.5);
+}
+//------------------------------------------------------------------------------
+
+inline cInt Abs(cInt val)
+{
+ return val < 0 ? -val : val;
+}
+
+//------------------------------------------------------------------------------
+// PolyTree methods ...
+//------------------------------------------------------------------------------
+
+void PolyTree::Clear()
+{
+ for (PolyNodes::size_type i = 0; i < AllNodes.size(); ++i)
+ delete AllNodes[i];
+ AllNodes.resize(0);
+ Childs.resize(0);
+}
+//------------------------------------------------------------------------------
+
+PolyNode* PolyTree::GetFirst() const
+{
+ if (!Childs.empty())
+ return Childs[0];
+ else
+ return 0;
+}
+//------------------------------------------------------------------------------
+
+int PolyTree::Total() const
+{
+ int result = (int)AllNodes.size();
+ //with negative offsets, ignore the hidden outer polygon ...
+ if (result > 0 && Childs[0] != AllNodes[0]) result--;
+ return result;
+}
+
+//------------------------------------------------------------------------------
+// PolyNode methods ...
+//------------------------------------------------------------------------------
+
+PolyNode::PolyNode(): Parent(0), Index(0), m_IsOpen(false)
+{
+}
+//------------------------------------------------------------------------------
+
+int PolyNode::ChildCount() const
+{
+ return (int)Childs.size();
+}
+//------------------------------------------------------------------------------
+
+void PolyNode::AddChild(PolyNode& child)
+{
+ unsigned cnt = (unsigned)Childs.size();
+ Childs.push_back(&child);
+ child.Parent = this;
+ child.Index = cnt;
+}
+//------------------------------------------------------------------------------
+
+PolyNode* PolyNode::GetNext() const
+{
+ if (!Childs.empty())
+ return Childs[0];
+ else
+ return GetNextSiblingUp();
+}
+//------------------------------------------------------------------------------
+
+PolyNode* PolyNode::GetNextSiblingUp() const
+{
+ if (!Parent) //protects against PolyTree.GetNextSiblingUp()
+ return 0;
+ else if (Index == Parent->Childs.size() - 1)
+ return Parent->GetNextSiblingUp();
+ else
+ return Parent->Childs[Index + 1];
+}
+//------------------------------------------------------------------------------
+
+bool PolyNode::IsHole() const
+{
+ bool result = true;
+ PolyNode* node = Parent;
+ while (node)
+ {
+ result = !result;
+ node = node->Parent;
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+bool PolyNode::IsOpen() const
+{
+ return m_IsOpen;
+}
+//------------------------------------------------------------------------------
+
+#ifndef use_int32
+
+//------------------------------------------------------------------------------
+// Int128 class (enables safe math on signed 64bit integers)
+// eg Int128 val1((long64)9223372036854775807); //ie 2^63 -1
+// Int128 val2((long64)9223372036854775807);
+// Int128 val3 = val1 * val2;
+// val3.AsString => "85070591730234615847396907784232501249" (8.5e+37)
+//------------------------------------------------------------------------------
+
+class Int128
+{
+ public:
+ ulong64 lo;
+ long64 hi;
+
+ Int128(long64 _lo = 0)
+ {
+ lo = (ulong64)_lo;
+ if (_lo < 0) hi = -1; else hi = 0;
+ }
+
+
+ Int128(const Int128 &val): lo(val.lo), hi(val.hi){}
+
+ Int128(const long64& _hi, const ulong64& _lo): lo(_lo), hi(_hi){}
+
+ Int128& operator = (const long64 &val)
+ {
+ lo = (ulong64)val;
+ if (val < 0) hi = -1; else hi = 0;
+ return *this;
+ }
+
+ bool operator == (const Int128 &val) const
+ {return (hi == val.hi && lo == val.lo);}
+
+ bool operator != (const Int128 &val) const
+ { return !(*this == val);}
+
+ bool operator > (const Int128 &val) const
+ {
+ if (hi != val.hi)
+ return hi > val.hi;
+ else
+ return lo > val.lo;
+ }
+
+ bool operator < (const Int128 &val) const
+ {
+ if (hi != val.hi)
+ return hi < val.hi;
+ else
+ return lo < val.lo;
+ }
+
+ bool operator >= (const Int128 &val) const
+ { return !(*this < val);}
+
+ bool operator <= (const Int128 &val) const
+ { return !(*this > val);}
+
+ Int128& operator += (const Int128 &rhs)
+ {
+ hi += rhs.hi;
+ lo += rhs.lo;
+ if (lo < rhs.lo) hi++;
+ return *this;
+ }
+
+ Int128 operator + (const Int128 &rhs) const
+ {
+ Int128 result(*this);
+ result+= rhs;
+ return result;
+ }
+
+ Int128& operator -= (const Int128 &rhs)
+ {
+ *this += -rhs;
+ return *this;
+ }
+
+ Int128 operator - (const Int128 &rhs) const
+ {
+ Int128 result(*this);
+ result -= rhs;
+ return result;
+ }
+
+ Int128 operator-() const //unary negation
+ {
+ if (lo == 0)
+ return Int128(-hi, 0);
+ else
+ return Int128(~hi, ~lo + 1);
+ }
+
+ operator double() const
+ {
+ const double shift64 = 18446744073709551616.0; //2^64
+ if (hi < 0)
+ {
+ if (lo == 0) return (double)hi * shift64;
+ else return -(double)(~lo + ~hi * shift64);
+ }
+ else
+ return (double)(lo + hi * shift64);
+ }
+
+};
+//------------------------------------------------------------------------------
+
+Int128 Int128Mul (long64 lhs, long64 rhs)
+{
+ bool negate = (lhs < 0) != (rhs < 0);
+
+ if (lhs < 0) lhs = -lhs;
+ ulong64 int1Hi = ulong64(lhs) >> 32;
+ ulong64 int1Lo = ulong64(lhs & 0xFFFFFFFF);
+
+ if (rhs < 0) rhs = -rhs;
+ ulong64 int2Hi = ulong64(rhs) >> 32;
+ ulong64 int2Lo = ulong64(rhs & 0xFFFFFFFF);
+
+ //nb: see comments in clipper.pas
+ ulong64 a = int1Hi * int2Hi;
+ ulong64 b = int1Lo * int2Lo;
+ ulong64 c = int1Hi * int2Lo + int1Lo * int2Hi;
+
+ Int128 tmp;
+ tmp.hi = long64(a + (c >> 32));
+ tmp.lo = long64(c << 32);
+ tmp.lo += long64(b);
+ if (tmp.lo < b) tmp.hi++;
+ if (negate) tmp = -tmp;
+ return tmp;
+};
+#endif
+
+//------------------------------------------------------------------------------
+// Miscellaneous global functions
+//------------------------------------------------------------------------------
+
+bool Orientation(const Path &poly)
+{
+ return Area(poly) >= 0;
+}
+//------------------------------------------------------------------------------
+
+double Area(const Path &poly)
+{
+ int size = (int)poly.size();
+ if (size < 3) return 0;
+
+ double a = 0;
+ for (int i = 0, j = size -1; i < size; ++i)
+ {
+ a += ((double)poly[j].X + poly[i].X) * ((double)poly[j].Y - poly[i].Y);
+ j = i;
+ }
+ return -a * 0.5;
+}
+//------------------------------------------------------------------------------
+
+double Area(const OutPt *op)
+{
+ const OutPt *startOp = op;
+ if (!op) return 0;
+ double a = 0;
+ do {
+ a += (double)(op->Prev->Pt.X + op->Pt.X) * (double)(op->Prev->Pt.Y - op->Pt.Y);
+ op = op->Next;
+ } while (op != startOp);
+ return a * 0.5;
+}
+//------------------------------------------------------------------------------
+
+double Area(const OutRec &outRec)
+{
+ return Area(outRec.Pts);
+}
+//------------------------------------------------------------------------------
+
+bool PointIsVertex(const IntPoint &Pt, OutPt *pp)
+{
+ OutPt *pp2 = pp;
+ do
+ {
+ if (pp2->Pt == Pt) return true;
+ pp2 = pp2->Next;
+ }
+ while (pp2 != pp);
+ return false;
+}
+//------------------------------------------------------------------------------
+
+//See "The Point in Polygon Problem for Arbitrary Polygons" by Hormann & Agathos
+//http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.88.5498&rep=rep1&type=pdf
+int PointInPolygon(const IntPoint &pt, const Path &path)
+{
+ //returns 0 if false, +1 if true, -1 if pt ON polygon boundary
+ int result = 0;
+ size_t cnt = path.size();
+ if (cnt < 3) return 0;
+ IntPoint ip = path[0];
+ for(size_t i = 1; i <= cnt; ++i)
+ {
+ IntPoint ipNext = (i == cnt ? path[0] : path[i]);
+ if (ipNext.Y == pt.Y)
+ {
+ if ((ipNext.X == pt.X) || (ip.Y == pt.Y &&
+ ((ipNext.X > pt.X) == (ip.X < pt.X)))) return -1;
+ }
+ if ((ip.Y < pt.Y) != (ipNext.Y < pt.Y))
+ {
+ if (ip.X >= pt.X)
+ {
+ if (ipNext.X > pt.X) result = 1 - result;
+ else
+ {
+ double d = (double)(ip.X - pt.X) * (ipNext.Y - pt.Y) -
+ (double)(ipNext.X - pt.X) * (ip.Y - pt.Y);
+ if (!d) return -1;
+ if ((d > 0) == (ipNext.Y > ip.Y)) result = 1 - result;
+ }
+ } else
+ {
+ if (ipNext.X > pt.X)
+ {
+ double d = (double)(ip.X - pt.X) * (ipNext.Y - pt.Y) -
+ (double)(ipNext.X - pt.X) * (ip.Y - pt.Y);
+ if (!d) return -1;
+ if ((d > 0) == (ipNext.Y > ip.Y)) result = 1 - result;
+ }
+ }
+ }
+ ip = ipNext;
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+int PointInPolygon (const IntPoint &pt, OutPt *op)
+{
+ //returns 0 if false, +1 if true, -1 if pt ON polygon boundary
+ int result = 0;
+ OutPt* startOp = op;
+ for(;;)
+ {
+ if (op->Next->Pt.Y == pt.Y)
+ {
+ if ((op->Next->Pt.X == pt.X) || (op->Pt.Y == pt.Y &&
+ ((op->Next->Pt.X > pt.X) == (op->Pt.X < pt.X)))) return -1;
+ }
+ if ((op->Pt.Y < pt.Y) != (op->Next->Pt.Y < pt.Y))
+ {
+ if (op->Pt.X >= pt.X)
+ {
+ if (op->Next->Pt.X > pt.X) result = 1 - result;
+ else
+ {
+ double d = (double)(op->Pt.X - pt.X) * (op->Next->Pt.Y - pt.Y) -
+ (double)(op->Next->Pt.X - pt.X) * (op->Pt.Y - pt.Y);
+ if (!d) return -1;
+ if ((d > 0) == (op->Next->Pt.Y > op->Pt.Y)) result = 1 - result;
+ }
+ } else
+ {
+ if (op->Next->Pt.X > pt.X)
+ {
+ double d = (double)(op->Pt.X - pt.X) * (op->Next->Pt.Y - pt.Y) -
+ (double)(op->Next->Pt.X - pt.X) * (op->Pt.Y - pt.Y);
+ if (!d) return -1;
+ if ((d > 0) == (op->Next->Pt.Y > op->Pt.Y)) result = 1 - result;
+ }
+ }
+ }
+ op = op->Next;
+ if (startOp == op) break;
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+bool Poly2ContainsPoly1(OutPt *OutPt1, OutPt *OutPt2)
+{
+ OutPt* op = OutPt1;
+ do
+ {
+ //nb: PointInPolygon returns 0 if false, +1 if true, -1 if pt on polygon
+ int res = PointInPolygon(op->Pt, OutPt2);
+ if (res >= 0) return res > 0;
+ op = op->Next;
+ }
+ while (op != OutPt1);
+ return true;
+}
+//----------------------------------------------------------------------
+
+bool SlopesEqual(const TEdge &e1, const TEdge &e2, bool UseFullInt64Range)
+{
+#ifndef use_int32
+ if (UseFullInt64Range)
+ return Int128Mul(e1.Top.Y - e1.Bot.Y, e2.Top.X - e2.Bot.X) ==
+ Int128Mul(e1.Top.X - e1.Bot.X, e2.Top.Y - e2.Bot.Y);
+ else
+#endif
+ return (e1.Top.Y - e1.Bot.Y) * (e2.Top.X - e2.Bot.X) ==
+ (e1.Top.X - e1.Bot.X) * (e2.Top.Y - e2.Bot.Y);
+}
+//------------------------------------------------------------------------------
+
+bool SlopesEqual(const IntPoint pt1, const IntPoint pt2,
+ const IntPoint pt3, bool UseFullInt64Range)
+{
+#ifndef use_int32
+ if (UseFullInt64Range)
+ return Int128Mul(pt1.Y-pt2.Y, pt2.X-pt3.X) == Int128Mul(pt1.X-pt2.X, pt2.Y-pt3.Y);
+ else
+#endif
+ return (pt1.Y-pt2.Y)*(pt2.X-pt3.X) == (pt1.X-pt2.X)*(pt2.Y-pt3.Y);
+}
+//------------------------------------------------------------------------------
+
+bool SlopesEqual(const IntPoint pt1, const IntPoint pt2,
+ const IntPoint pt3, const IntPoint pt4, bool UseFullInt64Range)
+{
+#ifndef use_int32
+ if (UseFullInt64Range)
+ return Int128Mul(pt1.Y-pt2.Y, pt3.X-pt4.X) == Int128Mul(pt1.X-pt2.X, pt3.Y-pt4.Y);
+ else
+#endif
+ return (pt1.Y-pt2.Y)*(pt3.X-pt4.X) == (pt1.X-pt2.X)*(pt3.Y-pt4.Y);
+}
+//------------------------------------------------------------------------------
+
+inline bool IsHorizontal(TEdge &e)
+{
+ return e.Dx == HORIZONTAL;
+}
+//------------------------------------------------------------------------------
+
+inline double GetDx(const IntPoint pt1, const IntPoint pt2)
+{
+ return (pt1.Y == pt2.Y) ?
+ HORIZONTAL : (double)(pt2.X - pt1.X) / (pt2.Y - pt1.Y);
+}
+//---------------------------------------------------------------------------
+
+inline void SetDx(TEdge &e)
+{
+ cInt dy = (e.Top.Y - e.Bot.Y);
+ if (dy == 0) e.Dx = HORIZONTAL;
+ else e.Dx = (double)(e.Top.X - e.Bot.X) / dy;
+}
+//---------------------------------------------------------------------------
+
+inline void SwapSides(TEdge &Edge1, TEdge &Edge2)
+{
+ EdgeSide Side = Edge1.Side;
+ Edge1.Side = Edge2.Side;
+ Edge2.Side = Side;
+}
+//------------------------------------------------------------------------------
+
+inline void SwapPolyIndexes(TEdge &Edge1, TEdge &Edge2)
+{
+ int OutIdx = Edge1.OutIdx;
+ Edge1.OutIdx = Edge2.OutIdx;
+ Edge2.OutIdx = OutIdx;
+}
+//------------------------------------------------------------------------------
+
+inline cInt TopX(TEdge &edge, const cInt currentY)
+{
+ return ( currentY == edge.Top.Y ) ?
+ edge.Top.X : edge.Bot.X + Round(edge.Dx *(currentY - edge.Bot.Y));
+}
+//------------------------------------------------------------------------------
+
+void IntersectPoint(TEdge &Edge1, TEdge &Edge2, IntPoint &ip)
+{
+#ifdef use_xyz
+ ip.Z = 0;
+#endif
+
+ double b1, b2;
+ if (Edge1.Dx == Edge2.Dx)
+ {
+ ip.Y = Edge1.Curr.Y;
+ ip.X = TopX(Edge1, ip.Y);
+ return;
+ }
+ else if (Edge1.Dx == 0)
+ {
+ ip.X = Edge1.Bot.X;
+ if (IsHorizontal(Edge2))
+ ip.Y = Edge2.Bot.Y;
+ else
+ {
+ b2 = Edge2.Bot.Y - (Edge2.Bot.X / Edge2.Dx);
+ ip.Y = Round(ip.X / Edge2.Dx + b2);
+ }
+ }
+ else if (Edge2.Dx == 0)
+ {
+ ip.X = Edge2.Bot.X;
+ if (IsHorizontal(Edge1))
+ ip.Y = Edge1.Bot.Y;
+ else
+ {
+ b1 = Edge1.Bot.Y - (Edge1.Bot.X / Edge1.Dx);
+ ip.Y = Round(ip.X / Edge1.Dx + b1);
+ }
+ }
+ else
+ {
+ b1 = Edge1.Bot.X - Edge1.Bot.Y * Edge1.Dx;
+ b2 = Edge2.Bot.X - Edge2.Bot.Y * Edge2.Dx;
+ double q = (b2-b1) / (Edge1.Dx - Edge2.Dx);
+ ip.Y = Round(q);
+ if (std::fabs(Edge1.Dx) < std::fabs(Edge2.Dx))
+ ip.X = Round(Edge1.Dx * q + b1);
+ else
+ ip.X = Round(Edge2.Dx * q + b2);
+ }
+
+ if (ip.Y < Edge1.Top.Y || ip.Y < Edge2.Top.Y)
+ {
+ if (Edge1.Top.Y > Edge2.Top.Y)
+ ip.Y = Edge1.Top.Y;
+ else
+ ip.Y = Edge2.Top.Y;
+ if (std::fabs(Edge1.Dx) < std::fabs(Edge2.Dx))
+ ip.X = TopX(Edge1, ip.Y);
+ else
+ ip.X = TopX(Edge2, ip.Y);
+ }
+ //finally, don't allow 'ip' to be BELOW curr.Y (ie bottom of scanbeam) ...
+ if (ip.Y > Edge1.Curr.Y)
+ {
+ ip.Y = Edge1.Curr.Y;
+ //use the more vertical edge to derive X ...
+ if (std::fabs(Edge1.Dx) > std::fabs(Edge2.Dx))
+ ip.X = TopX(Edge2, ip.Y); else
+ ip.X = TopX(Edge1, ip.Y);
+ }
+}
+//------------------------------------------------------------------------------
+
+void ReversePolyPtLinks(OutPt *pp)
+{
+ if (!pp) return;
+ OutPt *pp1, *pp2;
+ pp1 = pp;
+ do {
+ pp2 = pp1->Next;
+ pp1->Next = pp1->Prev;
+ pp1->Prev = pp2;
+ pp1 = pp2;
+ } while( pp1 != pp );
+}
+//------------------------------------------------------------------------------
+
+void DisposeOutPts(OutPt*& pp)
+{
+ if (pp == 0) return;
+ pp->Prev->Next = 0;
+ while( pp )
+ {
+ OutPt *tmpPp = pp;
+ pp = pp->Next;
+ delete tmpPp;
+ }
+}
+//------------------------------------------------------------------------------
+
+inline void InitEdge(TEdge* e, TEdge* eNext, TEdge* ePrev, const IntPoint& Pt)
+{
+ std::memset(e, 0, sizeof(TEdge));
+ e->Next = eNext;
+ e->Prev = ePrev;
+ e->Curr = Pt;
+ e->OutIdx = Unassigned;
+}
+//------------------------------------------------------------------------------
+
+void InitEdge2(TEdge& e, PolyType Pt)
+{
+ if (e.Curr.Y >= e.Next->Curr.Y)
+ {
+ e.Bot = e.Curr;
+ e.Top = e.Next->Curr;
+ } else
+ {
+ e.Top = e.Curr;
+ e.Bot = e.Next->Curr;
+ }
+ SetDx(e);
+ e.PolyTyp = Pt;
+}
+//------------------------------------------------------------------------------
+
+TEdge* RemoveEdge(TEdge* e)
+{
+ //removes e from double_linked_list (but without removing from memory)
+ e->Prev->Next = e->Next;
+ e->Next->Prev = e->Prev;
+ TEdge* result = e->Next;
+ e->Prev = 0; //flag as removed (see ClipperBase.Clear)
+ return result;
+}
+//------------------------------------------------------------------------------
+
+inline void ReverseHorizontal(TEdge &e)
+{
+ //swap horizontal edges' Top and Bottom x's so they follow the natural
+ //progression of the bounds - ie so their xbots will align with the
+ //adjoining lower edge. [Helpful in the ProcessHorizontal() method.]
+ std::swap(e.Top.X, e.Bot.X);
+#ifdef use_xyz
+ std::swap(e.Top.Z, e.Bot.Z);
+#endif
+}
+//------------------------------------------------------------------------------
+
+void SwapPoints(IntPoint &pt1, IntPoint &pt2)
+{
+ IntPoint tmp = pt1;
+ pt1 = pt2;
+ pt2 = tmp;
+}
+//------------------------------------------------------------------------------
+
+bool GetOverlapSegment(IntPoint pt1a, IntPoint pt1b, IntPoint pt2a,
+ IntPoint pt2b, IntPoint &pt1, IntPoint &pt2)
+{
+ //precondition: segments are Collinear.
+ if (Abs(pt1a.X - pt1b.X) > Abs(pt1a.Y - pt1b.Y))
+ {
+ if (pt1a.X > pt1b.X) SwapPoints(pt1a, pt1b);
+ if (pt2a.X > pt2b.X) SwapPoints(pt2a, pt2b);
+ if (pt1a.X > pt2a.X) pt1 = pt1a; else pt1 = pt2a;
+ if (pt1b.X < pt2b.X) pt2 = pt1b; else pt2 = pt2b;
+ return pt1.X < pt2.X;
+ } else
+ {
+ if (pt1a.Y < pt1b.Y) SwapPoints(pt1a, pt1b);
+ if (pt2a.Y < pt2b.Y) SwapPoints(pt2a, pt2b);
+ if (pt1a.Y < pt2a.Y) pt1 = pt1a; else pt1 = pt2a;
+ if (pt1b.Y > pt2b.Y) pt2 = pt1b; else pt2 = pt2b;
+ return pt1.Y > pt2.Y;
+ }
+}
+//------------------------------------------------------------------------------
+
+bool FirstIsBottomPt(const OutPt* btmPt1, const OutPt* btmPt2)
+{
+ OutPt *p = btmPt1->Prev;
+ while ((p->Pt == btmPt1->Pt) && (p != btmPt1)) p = p->Prev;
+ double dx1p = std::fabs(GetDx(btmPt1->Pt, p->Pt));
+ p = btmPt1->Next;
+ while ((p->Pt == btmPt1->Pt) && (p != btmPt1)) p = p->Next;
+ double dx1n = std::fabs(GetDx(btmPt1->Pt, p->Pt));
+
+ p = btmPt2->Prev;
+ while ((p->Pt == btmPt2->Pt) && (p != btmPt2)) p = p->Prev;
+ double dx2p = std::fabs(GetDx(btmPt2->Pt, p->Pt));
+ p = btmPt2->Next;
+ while ((p->Pt == btmPt2->Pt) && (p != btmPt2)) p = p->Next;
+ double dx2n = std::fabs(GetDx(btmPt2->Pt, p->Pt));
+
+ if (std::max(dx1p, dx1n) == std::max(dx2p, dx2n) &&
+ std::min(dx1p, dx1n) == std::min(dx2p, dx2n))
+ return Area(btmPt1) > 0; //if otherwise identical use orientation
+ else
+ return (dx1p >= dx2p && dx1p >= dx2n) || (dx1n >= dx2p && dx1n >= dx2n);
+}
+//------------------------------------------------------------------------------
+
+OutPt* GetBottomPt(OutPt *pp)
+{
+ OutPt* dups = 0;
+ OutPt* p = pp->Next;
+ while (p != pp)
+ {
+ if (p->Pt.Y > pp->Pt.Y)
+ {
+ pp = p;
+ dups = 0;
+ }
+ else if (p->Pt.Y == pp->Pt.Y && p->Pt.X <= pp->Pt.X)
+ {
+ if (p->Pt.X < pp->Pt.X)
+ {
+ dups = 0;
+ pp = p;
+ } else
+ {
+ if (p->Next != pp && p->Prev != pp) dups = p;
+ }
+ }
+ p = p->Next;
+ }
+ if (dups)
+ {
+ //there appears to be at least 2 vertices at BottomPt so ...
+ while (dups != p)
+ {
+ if (!FirstIsBottomPt(p, dups)) pp = dups;
+ dups = dups->Next;
+ while (dups->Pt != pp->Pt) dups = dups->Next;
+ }
+ }
+ return pp;
+}
+//------------------------------------------------------------------------------
+
+bool Pt2IsBetweenPt1AndPt3(const IntPoint pt1,
+ const IntPoint pt2, const IntPoint pt3)
+{
+ if ((pt1 == pt3) || (pt1 == pt2) || (pt3 == pt2))
+ return false;
+ else if (pt1.X != pt3.X)
+ return (pt2.X > pt1.X) == (pt2.X < pt3.X);
+ else
+ return (pt2.Y > pt1.Y) == (pt2.Y < pt3.Y);
+}
+//------------------------------------------------------------------------------
+
+bool HorzSegmentsOverlap(cInt seg1a, cInt seg1b, cInt seg2a, cInt seg2b)
+{
+ if (seg1a > seg1b) std::swap(seg1a, seg1b);
+ if (seg2a > seg2b) std::swap(seg2a, seg2b);
+ return (seg1a < seg2b) && (seg2a < seg1b);
+}
+
+//------------------------------------------------------------------------------
+// ClipperBase class methods ...
+//------------------------------------------------------------------------------
+
+ClipperBase::ClipperBase() //constructor
+{
+ m_CurrentLM = m_MinimaList.begin(); //begin() == end() here
+ m_UseFullRange = false;
+}
+//------------------------------------------------------------------------------
+
+ClipperBase::~ClipperBase() //destructor
+{
+ Clear();
+}
+//------------------------------------------------------------------------------
+
+void RangeTest(const IntPoint& Pt, bool& useFullRange)
+{
+ if (useFullRange)
+ {
+ if (Pt.X > hiRange || Pt.Y > hiRange || -Pt.X > hiRange || -Pt.Y > hiRange)
+ throw clipperException("Coordinate outside allowed range");
+ }
+ else if (Pt.X > loRange|| Pt.Y > loRange || -Pt.X > loRange || -Pt.Y > loRange)
+ {
+ useFullRange = true;
+ RangeTest(Pt, useFullRange);
+ }
+}
+//------------------------------------------------------------------------------
+
+TEdge* FindNextLocMin(TEdge* E)
+{
+ for (;;)
+ {
+ while (E->Bot != E->Prev->Bot || E->Curr == E->Top) E = E->Next;
+ if (!IsHorizontal(*E) && !IsHorizontal(*E->Prev)) break;
+ while (IsHorizontal(*E->Prev)) E = E->Prev;
+ TEdge* E2 = E;
+ while (IsHorizontal(*E)) E = E->Next;
+ if (E->Top.Y == E->Prev->Bot.Y) continue; //ie just an intermediate horz.
+ if (E2->Prev->Bot.X < E->Bot.X) E = E2;
+ break;
+ }
+ return E;
+}
+//------------------------------------------------------------------------------
+
+TEdge* ClipperBase::ProcessBound(TEdge* E, bool NextIsForward)
+{
+ TEdge *Result = E;
+ TEdge *Horz = 0;
+
+ if (E->OutIdx == Skip)
+ {
+ //if edges still remain in the current bound beyond the skip edge then
+ //create another LocMin and call ProcessBound once more
+ if (NextIsForward)
+ {
+ while (E->Top.Y == E->Next->Bot.Y) E = E->Next;
+ //don't include top horizontals when parsing a bound a second time,
+ //they will be contained in the opposite bound ...
+ while (E != Result && IsHorizontal(*E)) E = E->Prev;
+ }
+ else
+ {
+ while (E->Top.Y == E->Prev->Bot.Y) E = E->Prev;
+ while (E != Result && IsHorizontal(*E)) E = E->Next;
+ }
+
+ if (E == Result)
+ {
+ if (NextIsForward) Result = E->Next;
+ else Result = E->Prev;
+ }
+ else
+ {
+ //there are more edges in the bound beyond result starting with E
+ if (NextIsForward)
+ E = Result->Next;
+ else
+ E = Result->Prev;
+ MinimaList::value_type locMin;
+ locMin.Y = E->Bot.Y;
+ locMin.LeftBound = 0;
+ locMin.RightBound = E;
+ E->WindDelta = 0;
+ Result = ProcessBound(E, NextIsForward);
+ m_MinimaList.push_back(locMin);
+ }
+ return Result;
+ }
+
+ TEdge *EStart;
+
+ if (IsHorizontal(*E))
+ {
+ //We need to be careful with open paths because this may not be a
+ //true local minima (ie E may be following a skip edge).
+ //Also, consecutive horz. edges may start heading left before going right.
+ if (NextIsForward)
+ EStart = E->Prev;
+ else
+ EStart = E->Next;
+ if (IsHorizontal(*EStart)) //ie an adjoining horizontal skip edge
+ {
+ if (EStart->Bot.X != E->Bot.X && EStart->Top.X != E->Bot.X)
+ ReverseHorizontal(*E);
+ }
+ else if (EStart->Bot.X != E->Bot.X)
+ ReverseHorizontal(*E);
+ }
+
+ EStart = E;
+ if (NextIsForward)
+ {
+ while (Result->Top.Y == Result->Next->Bot.Y && Result->Next->OutIdx != Skip)
+ Result = Result->Next;
+ if (IsHorizontal(*Result) && Result->Next->OutIdx != Skip)
+ {
+ //nb: at the top of a bound, horizontals are added to the bound
+ //only when the preceding edge attaches to the horizontal's left vertex
+ //unless a Skip edge is encountered when that becomes the top divide
+ Horz = Result;
+ while (IsHorizontal(*Horz->Prev)) Horz = Horz->Prev;
+ if (Horz->Prev->Top.X > Result->Next->Top.X) Result = Horz->Prev;
+ }
+ while (E != Result)
+ {
+ E->NextInLML = E->Next;
+ if (IsHorizontal(*E) && E != EStart &&
+ E->Bot.X != E->Prev->Top.X) ReverseHorizontal(*E);
+ E = E->Next;
+ }
+ if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Prev->Top.X)
+ ReverseHorizontal(*E);
+ Result = Result->Next; //move to the edge just beyond current bound
+ } else
+ {
+ while (Result->Top.Y == Result->Prev->Bot.Y && Result->Prev->OutIdx != Skip)
+ Result = Result->Prev;
+ if (IsHorizontal(*Result) && Result->Prev->OutIdx != Skip)
+ {
+ Horz = Result;
+ while (IsHorizontal(*Horz->Next)) Horz = Horz->Next;
+ if (Horz->Next->Top.X == Result->Prev->Top.X ||
+ Horz->Next->Top.X > Result->Prev->Top.X) Result = Horz->Next;
+ }
+
+ while (E != Result)
+ {
+ E->NextInLML = E->Prev;
+ if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Next->Top.X)
+ ReverseHorizontal(*E);
+ E = E->Prev;
+ }
+ if (IsHorizontal(*E) && E != EStart && E->Bot.X != E->Next->Top.X)
+ ReverseHorizontal(*E);
+ Result = Result->Prev; //move to the edge just beyond current bound
+ }
+
+ return Result;
+}
+//------------------------------------------------------------------------------
+
+bool ClipperBase::AddPath(const Path &pg, PolyType PolyTyp, bool Closed)
+{
+#ifdef use_lines
+ if (!Closed && PolyTyp == ptClip)
+ throw clipperException("AddPath: Open paths must be subject.");
+#else
+ if (!Closed)
+ throw clipperException("AddPath: Open paths have been disabled.");
+#endif
+
+ int highI = (int)pg.size() -1;
+ if (Closed) while (highI > 0 && (pg[highI] == pg[0])) --highI;
+ while (highI > 0 && (pg[highI] == pg[highI -1])) --highI;
+ if ((Closed && highI < 2) || (!Closed && highI < 1)) return false;
+
+ //create a new edge array ...
+ TEdge *edges = new TEdge [highI +1];
+
+ bool IsFlat = true;
+ //1. Basic (first) edge initialization ...
+ try
+ {
+ edges[1].Curr = pg[1];
+ RangeTest(pg[0], m_UseFullRange);
+ RangeTest(pg[highI], m_UseFullRange);
+ InitEdge(&edges[0], &edges[1], &edges[highI], pg[0]);
+ InitEdge(&edges[highI], &edges[0], &edges[highI-1], pg[highI]);
+ for (int i = highI - 1; i >= 1; --i)
+ {
+ RangeTest(pg[i], m_UseFullRange);
+ InitEdge(&edges[i], &edges[i+1], &edges[i-1], pg[i]);
+ }
+ }
+ catch(...)
+ {
+ delete [] edges;
+ throw; //range test fails
+ }
+ TEdge *eStart = &edges[0];
+
+ //2. Remove duplicate vertices, and (when closed) collinear edges ...
+ TEdge *E = eStart, *eLoopStop = eStart;
+ for (;;)
+ {
+ //nb: allows matching start and end points when not Closed ...
+ if (E->Curr == E->Next->Curr && (Closed || E->Next != eStart))
+ {
+ if (E == E->Next) break;
+ if (E == eStart) eStart = E->Next;
+ E = RemoveEdge(E);
+ eLoopStop = E;
+ continue;
+ }
+ if (E->Prev == E->Next)
+ break; //only two vertices
+ else if (Closed &&
+ SlopesEqual(E->Prev->Curr, E->Curr, E->Next->Curr, m_UseFullRange) &&
+ (!m_PreserveCollinear ||
+ !Pt2IsBetweenPt1AndPt3(E->Prev->Curr, E->Curr, E->Next->Curr)))
+ {
+ //Collinear edges are allowed for open paths but in closed paths
+ //the default is to merge adjacent collinear edges into a single edge.
+ //However, if the PreserveCollinear property is enabled, only overlapping
+ //collinear edges (ie spikes) will be removed from closed paths.
+ if (E == eStart) eStart = E->Next;
+ E = RemoveEdge(E);
+ E = E->Prev;
+ eLoopStop = E;
+ continue;
+ }
+ E = E->Next;
+ if ((E == eLoopStop) || (!Closed && E->Next == eStart)) break;
+ }
+
+ if ((!Closed && (E == E->Next)) || (Closed && (E->Prev == E->Next)))
+ {
+ delete [] edges;
+ return false;
+ }
+
+ if (!Closed)
+ {
+ m_HasOpenPaths = true;
+ eStart->Prev->OutIdx = Skip;
+ }
+
+ //3. Do second stage of edge initialization ...
+ E = eStart;
+ do
+ {
+ InitEdge2(*E, PolyTyp);
+ E = E->Next;
+ if (IsFlat && E->Curr.Y != eStart->Curr.Y) IsFlat = false;
+ }
+ while (E != eStart);
+
+ //4. Finally, add edge bounds to LocalMinima list ...
+
+ //Totally flat paths must be handled differently when adding them
+ //to LocalMinima list to avoid endless loops etc ...
+ if (IsFlat)
+ {
+ if (Closed)
+ {
+ delete [] edges;
+ return false;
+ }
+ E->Prev->OutIdx = Skip;
+ MinimaList::value_type locMin;
+ locMin.Y = E->Bot.Y;
+ locMin.LeftBound = 0;
+ locMin.RightBound = E;
+ locMin.RightBound->Side = esRight;
+ locMin.RightBound->WindDelta = 0;
+ for (;;)
+ {
+ if (E->Bot.X != E->Prev->Top.X) ReverseHorizontal(*E);
+ if (E->Next->OutIdx == Skip) break;
+ E->NextInLML = E->Next;
+ E = E->Next;
+ }
+ m_MinimaList.push_back(locMin);
+ m_edges.push_back(edges);
+ return true;
+ }
+
+ m_edges.push_back(edges);
+ bool leftBoundIsForward;
+ TEdge* EMin = 0;
+
+ //workaround to avoid an endless loop in the while loop below when
+ //open paths have matching start and end points ...
+ if (E->Prev->Bot == E->Prev->Top) E = E->Next;
+
+ for (;;)
+ {
+ E = FindNextLocMin(E);
+ if (E == EMin) break;
+ else if (!EMin) EMin = E;
+
+ //E and E.Prev now share a local minima (left aligned if horizontal).
+ //Compare their slopes to find which starts which bound ...
+ MinimaList::value_type locMin;
+ locMin.Y = E->Bot.Y;
+ if (E->Dx < E->Prev->Dx)
+ {
+ locMin.LeftBound = E->Prev;
+ locMin.RightBound = E;
+ leftBoundIsForward = false; //Q.nextInLML = Q.prev
+ } else
+ {
+ locMin.LeftBound = E;
+ locMin.RightBound = E->Prev;
+ leftBoundIsForward = true; //Q.nextInLML = Q.next
+ }
+
+ if (!Closed) locMin.LeftBound->WindDelta = 0;
+ else if (locMin.LeftBound->Next == locMin.RightBound)
+ locMin.LeftBound->WindDelta = -1;
+ else locMin.LeftBound->WindDelta = 1;
+ locMin.RightBound->WindDelta = -locMin.LeftBound->WindDelta;
+
+ E = ProcessBound(locMin.LeftBound, leftBoundIsForward);
+ if (E->OutIdx == Skip) E = ProcessBound(E, leftBoundIsForward);
+
+ TEdge* E2 = ProcessBound(locMin.RightBound, !leftBoundIsForward);
+ if (E2->OutIdx == Skip) E2 = ProcessBound(E2, !leftBoundIsForward);
+
+ if (locMin.LeftBound->OutIdx == Skip)
+ locMin.LeftBound = 0;
+ else if (locMin.RightBound->OutIdx == Skip)
+ locMin.RightBound = 0;
+ m_MinimaList.push_back(locMin);
+ if (!leftBoundIsForward) E = E2;
+ }
+ return true;
+}
+//------------------------------------------------------------------------------
+
+bool ClipperBase::AddPaths(const Paths &ppg, PolyType PolyTyp, bool Closed)
+{
+ bool result = false;
+ for (Paths::size_type i = 0; i < ppg.size(); ++i)
+ if (AddPath(ppg[i], PolyTyp, Closed)) result = true;
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::Clear()
+{
+ DisposeLocalMinimaList();
+ for (EdgeList::size_type i = 0; i < m_edges.size(); ++i)
+ {
+ TEdge* edges = m_edges[i];
+ delete [] edges;
+ }
+ m_edges.clear();
+ m_UseFullRange = false;
+ m_HasOpenPaths = false;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::Reset()
+{
+ m_CurrentLM = m_MinimaList.begin();
+ if (m_CurrentLM == m_MinimaList.end()) return; //ie nothing to process
+ std::sort(m_MinimaList.begin(), m_MinimaList.end(), LocMinSorter());
+
+ m_Scanbeam = ScanbeamList(); //clears/resets priority_queue
+ //reset all edges ...
+ for (MinimaList::iterator lm = m_MinimaList.begin(); lm != m_MinimaList.end(); ++lm)
+ {
+ InsertScanbeam(lm->Y);
+ TEdge* e = lm->LeftBound;
+ if (e)
+ {
+ e->Curr = e->Bot;
+ e->Side = esLeft;
+ e->OutIdx = Unassigned;
+ }
+
+ e = lm->RightBound;
+ if (e)
+ {
+ e->Curr = e->Bot;
+ e->Side = esRight;
+ e->OutIdx = Unassigned;
+ }
+ }
+ m_ActiveEdges = 0;
+ m_CurrentLM = m_MinimaList.begin();
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::DisposeLocalMinimaList()
+{
+ m_MinimaList.clear();
+ m_CurrentLM = m_MinimaList.begin();
+}
+//------------------------------------------------------------------------------
+
+bool ClipperBase::PopLocalMinima(cInt Y, const LocalMinimum *&locMin)
+{
+ if (m_CurrentLM == m_MinimaList.end() || (*m_CurrentLM).Y != Y) return false;
+ locMin = &(*m_CurrentLM);
+ ++m_CurrentLM;
+ return true;
+}
+//------------------------------------------------------------------------------
+
+IntRect ClipperBase::GetBounds()
+{
+ IntRect result;
+ MinimaList::iterator lm = m_MinimaList.begin();
+ if (lm == m_MinimaList.end())
+ {
+ result.left = result.top = result.right = result.bottom = 0;
+ return result;
+ }
+ result.left = lm->LeftBound->Bot.X;
+ result.top = lm->LeftBound->Bot.Y;
+ result.right = lm->LeftBound->Bot.X;
+ result.bottom = lm->LeftBound->Bot.Y;
+ while (lm != m_MinimaList.end())
+ {
+ //todo - needs fixing for open paths
+ result.bottom = std::max(result.bottom, lm->LeftBound->Bot.Y);
+ TEdge* e = lm->LeftBound;
+ for (;;) {
+ TEdge* bottomE = e;
+ while (e->NextInLML)
+ {
+ if (e->Bot.X < result.left) result.left = e->Bot.X;
+ if (e->Bot.X > result.right) result.right = e->Bot.X;
+ e = e->NextInLML;
+ }
+ result.left = std::min(result.left, e->Bot.X);
+ result.right = std::max(result.right, e->Bot.X);
+ result.left = std::min(result.left, e->Top.X);
+ result.right = std::max(result.right, e->Top.X);
+ result.top = std::min(result.top, e->Top.Y);
+ if (bottomE == lm->LeftBound) e = lm->RightBound;
+ else break;
+ }
+ ++lm;
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::InsertScanbeam(const cInt Y)
+{
+ m_Scanbeam.push(Y);
+}
+//------------------------------------------------------------------------------
+
+bool ClipperBase::PopScanbeam(cInt &Y)
+{
+ if (m_Scanbeam.empty()) return false;
+ Y = m_Scanbeam.top();
+ m_Scanbeam.pop();
+ while (!m_Scanbeam.empty() && Y == m_Scanbeam.top()) { m_Scanbeam.pop(); } // Pop duplicates.
+ return true;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::DisposeAllOutRecs(){
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ DisposeOutRec(i);
+ m_PolyOuts.clear();
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::DisposeOutRec(PolyOutList::size_type index)
+{
+ OutRec *outRec = m_PolyOuts[index];
+ if (outRec->Pts) DisposeOutPts(outRec->Pts);
+ delete outRec;
+ m_PolyOuts[index] = 0;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::DeleteFromAEL(TEdge *e)
+{
+ TEdge* AelPrev = e->PrevInAEL;
+ TEdge* AelNext = e->NextInAEL;
+ if (!AelPrev && !AelNext && (e != m_ActiveEdges)) return; //already deleted
+ if (AelPrev) AelPrev->NextInAEL = AelNext;
+ else m_ActiveEdges = AelNext;
+ if (AelNext) AelNext->PrevInAEL = AelPrev;
+ e->NextInAEL = 0;
+ e->PrevInAEL = 0;
+}
+//------------------------------------------------------------------------------
+
+OutRec* ClipperBase::CreateOutRec()
+{
+ OutRec* result = new OutRec;
+ result->IsHole = false;
+ result->IsOpen = false;
+ result->FirstLeft = 0;
+ result->Pts = 0;
+ result->BottomPt = 0;
+ result->PolyNd = 0;
+ m_PolyOuts.push_back(result);
+ result->Idx = (int)m_PolyOuts.size() - 1;
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::SwapPositionsInAEL(TEdge *Edge1, TEdge *Edge2)
+{
+ //check that one or other edge hasn't already been removed from AEL ...
+ if (Edge1->NextInAEL == Edge1->PrevInAEL ||
+ Edge2->NextInAEL == Edge2->PrevInAEL) return;
+
+ if (Edge1->NextInAEL == Edge2)
+ {
+ TEdge* Next = Edge2->NextInAEL;
+ if (Next) Next->PrevInAEL = Edge1;
+ TEdge* Prev = Edge1->PrevInAEL;
+ if (Prev) Prev->NextInAEL = Edge2;
+ Edge2->PrevInAEL = Prev;
+ Edge2->NextInAEL = Edge1;
+ Edge1->PrevInAEL = Edge2;
+ Edge1->NextInAEL = Next;
+ }
+ else if (Edge2->NextInAEL == Edge1)
+ {
+ TEdge* Next = Edge1->NextInAEL;
+ if (Next) Next->PrevInAEL = Edge2;
+ TEdge* Prev = Edge2->PrevInAEL;
+ if (Prev) Prev->NextInAEL = Edge1;
+ Edge1->PrevInAEL = Prev;
+ Edge1->NextInAEL = Edge2;
+ Edge2->PrevInAEL = Edge1;
+ Edge2->NextInAEL = Next;
+ }
+ else
+ {
+ TEdge* Next = Edge1->NextInAEL;
+ TEdge* Prev = Edge1->PrevInAEL;
+ Edge1->NextInAEL = Edge2->NextInAEL;
+ if (Edge1->NextInAEL) Edge1->NextInAEL->PrevInAEL = Edge1;
+ Edge1->PrevInAEL = Edge2->PrevInAEL;
+ if (Edge1->PrevInAEL) Edge1->PrevInAEL->NextInAEL = Edge1;
+ Edge2->NextInAEL = Next;
+ if (Edge2->NextInAEL) Edge2->NextInAEL->PrevInAEL = Edge2;
+ Edge2->PrevInAEL = Prev;
+ if (Edge2->PrevInAEL) Edge2->PrevInAEL->NextInAEL = Edge2;
+ }
+
+ if (!Edge1->PrevInAEL) m_ActiveEdges = Edge1;
+ else if (!Edge2->PrevInAEL) m_ActiveEdges = Edge2;
+}
+//------------------------------------------------------------------------------
+
+void ClipperBase::UpdateEdgeIntoAEL(TEdge *&e)
+{
+ if (!e->NextInLML)
+ throw clipperException("UpdateEdgeIntoAEL: invalid call");
+
+ e->NextInLML->OutIdx = e->OutIdx;
+ TEdge* AelPrev = e->PrevInAEL;
+ TEdge* AelNext = e->NextInAEL;
+ if (AelPrev) AelPrev->NextInAEL = e->NextInLML;
+ else m_ActiveEdges = e->NextInLML;
+ if (AelNext) AelNext->PrevInAEL = e->NextInLML;
+ e->NextInLML->Side = e->Side;
+ e->NextInLML->WindDelta = e->WindDelta;
+ e->NextInLML->WindCnt = e->WindCnt;
+ e->NextInLML->WindCnt2 = e->WindCnt2;
+ e = e->NextInLML;
+ e->Curr = e->Bot;
+ e->PrevInAEL = AelPrev;
+ e->NextInAEL = AelNext;
+ if (!IsHorizontal(*e)) InsertScanbeam(e->Top.Y);
+}
+//------------------------------------------------------------------------------
+
+bool ClipperBase::LocalMinimaPending()
+{
+ return (m_CurrentLM != m_MinimaList.end());
+}
+
+//------------------------------------------------------------------------------
+// TClipper methods ...
+//------------------------------------------------------------------------------
+
+Clipper::Clipper(int initOptions) : ClipperBase() //constructor
+{
+ m_ExecuteLocked = false;
+ m_UseFullRange = false;
+ m_ReverseOutput = ((initOptions & ioReverseSolution) != 0);
+ m_StrictSimple = ((initOptions & ioStrictlySimple) != 0);
+ m_PreserveCollinear = ((initOptions & ioPreserveCollinear) != 0);
+ m_HasOpenPaths = false;
+#ifdef use_xyz
+ m_ZFill = 0;
+#endif
+}
+//------------------------------------------------------------------------------
+
+#ifdef use_xyz
+void Clipper::ZFillFunction(ZFillCallback zFillFunc)
+{
+ m_ZFill = zFillFunc;
+}
+//------------------------------------------------------------------------------
+#endif
+
+bool Clipper::Execute(ClipType clipType, Paths &solution, PolyFillType fillType)
+{
+ return Execute(clipType, solution, fillType, fillType);
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::Execute(ClipType clipType, PolyTree &polytree, PolyFillType fillType)
+{
+ return Execute(clipType, polytree, fillType, fillType);
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::Execute(ClipType clipType, Paths &solution,
+ PolyFillType subjFillType, PolyFillType clipFillType)
+{
+ if( m_ExecuteLocked ) return false;
+ if (m_HasOpenPaths)
+ throw clipperException("Error: PolyTree struct is needed for open path clipping.");
+ m_ExecuteLocked = true;
+ solution.resize(0);
+ m_SubjFillType = subjFillType;
+ m_ClipFillType = clipFillType;
+ m_ClipType = clipType;
+ m_UsingPolyTree = false;
+ bool succeeded = ExecuteInternal();
+ if (succeeded) BuildResult(solution);
+ DisposeAllOutRecs();
+ m_ExecuteLocked = false;
+ return succeeded;
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::Execute(ClipType clipType, PolyTree& polytree,
+ PolyFillType subjFillType, PolyFillType clipFillType)
+{
+ if( m_ExecuteLocked ) return false;
+ m_ExecuteLocked = true;
+ m_SubjFillType = subjFillType;
+ m_ClipFillType = clipFillType;
+ m_ClipType = clipType;
+ m_UsingPolyTree = true;
+ bool succeeded = ExecuteInternal();
+ if (succeeded) BuildResult2(polytree);
+ DisposeAllOutRecs();
+ m_ExecuteLocked = false;
+ return succeeded;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::FixHoleLinkage(OutRec &outrec)
+{
+ //skip OutRecs that (a) contain outermost polygons or
+ //(b) already have the correct owner/child linkage ...
+ if (!outrec.FirstLeft ||
+ (outrec.IsHole != outrec.FirstLeft->IsHole &&
+ outrec.FirstLeft->Pts)) return;
+
+ OutRec* orfl = outrec.FirstLeft;
+ while (orfl && ((orfl->IsHole == outrec.IsHole) || !orfl->Pts))
+ orfl = orfl->FirstLeft;
+ outrec.FirstLeft = orfl;
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::ExecuteInternal()
+{
+ bool succeeded = true;
+ try {
+ Reset();
+ m_Maxima = MaximaList();
+ m_SortedEdges = 0;
+
+ succeeded = true;
+ cInt botY, topY;
+ if (!PopScanbeam(botY)) return false;
+ InsertLocalMinimaIntoAEL(botY);
+ while (PopScanbeam(topY) || LocalMinimaPending())
+ {
+ ProcessHorizontals();
+ ClearGhostJoins();
+ if (!ProcessIntersections(topY))
+ {
+ succeeded = false;
+ break;
+ }
+ ProcessEdgesAtTopOfScanbeam(topY);
+ botY = topY;
+ InsertLocalMinimaIntoAEL(botY);
+ }
+ }
+ catch(...)
+ {
+ succeeded = false;
+ }
+
+ if (succeeded)
+ {
+ //fix orientations ...
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ OutRec *outRec = m_PolyOuts[i];
+ if (!outRec->Pts || outRec->IsOpen) continue;
+ if ((outRec->IsHole ^ m_ReverseOutput) == (Area(*outRec) > 0))
+ ReversePolyPtLinks(outRec->Pts);
+ }
+
+ if (!m_Joins.empty()) JoinCommonEdges();
+
+ //unfortunately FixupOutPolygon() must be done after JoinCommonEdges()
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ OutRec *outRec = m_PolyOuts[i];
+ if (!outRec->Pts) continue;
+ if (outRec->IsOpen)
+ FixupOutPolyline(*outRec);
+ else
+ FixupOutPolygon(*outRec);
+ }
+
+ if (m_StrictSimple) DoSimplePolygons();
+ }
+
+ ClearJoins();
+ ClearGhostJoins();
+ return succeeded;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::SetWindingCount(TEdge &edge)
+{
+ TEdge *e = edge.PrevInAEL;
+ //find the edge of the same polytype that immediately preceeds 'edge' in AEL
+ while (e && ((e->PolyTyp != edge.PolyTyp) || (e->WindDelta == 0))) e = e->PrevInAEL;
+ if (!e)
+ {
+ if (edge.WindDelta == 0)
+ {
+ PolyFillType pft = (edge.PolyTyp == ptSubject ? m_SubjFillType : m_ClipFillType);
+ edge.WindCnt = (pft == pftNegative ? -1 : 1);
+ }
+ else
+ edge.WindCnt = edge.WindDelta;
+ edge.WindCnt2 = 0;
+ e = m_ActiveEdges; //ie get ready to calc WindCnt2
+ }
+ else if (edge.WindDelta == 0 && m_ClipType != ctUnion)
+ {
+ edge.WindCnt = 1;
+ edge.WindCnt2 = e->WindCnt2;
+ e = e->NextInAEL; //ie get ready to calc WindCnt2
+ }
+ else if (IsEvenOddFillType(edge))
+ {
+ //EvenOdd filling ...
+ if (edge.WindDelta == 0)
+ {
+ //are we inside a subj polygon ...
+ bool Inside = true;
+ TEdge *e2 = e->PrevInAEL;
+ while (e2)
+ {
+ if (e2->PolyTyp == e->PolyTyp && e2->WindDelta != 0)
+ Inside = !Inside;
+ e2 = e2->PrevInAEL;
+ }
+ edge.WindCnt = (Inside ? 0 : 1);
+ }
+ else
+ {
+ edge.WindCnt = edge.WindDelta;
+ }
+ edge.WindCnt2 = e->WindCnt2;
+ e = e->NextInAEL; //ie get ready to calc WindCnt2
+ }
+ else
+ {
+ //nonZero, Positive or Negative filling ...
+ if (e->WindCnt * e->WindDelta < 0)
+ {
+ //prev edge is 'decreasing' WindCount (WC) toward zero
+ //so we're outside the previous polygon ...
+ if (Abs(e->WindCnt) > 1)
+ {
+ //outside prev poly but still inside another.
+ //when reversing direction of prev poly use the same WC
+ if (e->WindDelta * edge.WindDelta < 0) edge.WindCnt = e->WindCnt;
+ //otherwise continue to 'decrease' WC ...
+ else edge.WindCnt = e->WindCnt + edge.WindDelta;
+ }
+ else
+ //now outside all polys of same polytype so set own WC ...
+ edge.WindCnt = (edge.WindDelta == 0 ? 1 : edge.WindDelta);
+ } else
+ {
+ //prev edge is 'increasing' WindCount (WC) away from zero
+ //so we're inside the previous polygon ...
+ if (edge.WindDelta == 0)
+ edge.WindCnt = (e->WindCnt < 0 ? e->WindCnt - 1 : e->WindCnt + 1);
+ //if wind direction is reversing prev then use same WC
+ else if (e->WindDelta * edge.WindDelta < 0) edge.WindCnt = e->WindCnt;
+ //otherwise add to WC ...
+ else edge.WindCnt = e->WindCnt + edge.WindDelta;
+ }
+ edge.WindCnt2 = e->WindCnt2;
+ e = e->NextInAEL; //ie get ready to calc WindCnt2
+ }
+
+ //update WindCnt2 ...
+ if (IsEvenOddAltFillType(edge))
+ {
+ //EvenOdd filling ...
+ while (e != &edge)
+ {
+ if (e->WindDelta != 0)
+ edge.WindCnt2 = (edge.WindCnt2 == 0 ? 1 : 0);
+ e = e->NextInAEL;
+ }
+ } else
+ {
+ //nonZero, Positive or Negative filling ...
+ while ( e != &edge )
+ {
+ edge.WindCnt2 += e->WindDelta;
+ e = e->NextInAEL;
+ }
+ }
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::IsEvenOddFillType(const TEdge& edge) const
+{
+ if (edge.PolyTyp == ptSubject)
+ return m_SubjFillType == pftEvenOdd; else
+ return m_ClipFillType == pftEvenOdd;
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::IsEvenOddAltFillType(const TEdge& edge) const
+{
+ if (edge.PolyTyp == ptSubject)
+ return m_ClipFillType == pftEvenOdd; else
+ return m_SubjFillType == pftEvenOdd;
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::IsContributing(const TEdge& edge) const
+{
+ PolyFillType pft, pft2;
+ if (edge.PolyTyp == ptSubject)
+ {
+ pft = m_SubjFillType;
+ pft2 = m_ClipFillType;
+ } else
+ {
+ pft = m_ClipFillType;
+ pft2 = m_SubjFillType;
+ }
+
+ switch(pft)
+ {
+ case pftEvenOdd:
+ //return false if a subj line has been flagged as inside a subj polygon
+ if (edge.WindDelta == 0 && edge.WindCnt != 1) return false;
+ break;
+ case pftNonZero:
+ if (Abs(edge.WindCnt) != 1) return false;
+ break;
+ case pftPositive:
+ if (edge.WindCnt != 1) return false;
+ break;
+ default: //pftNegative
+ if (edge.WindCnt != -1) return false;
+ }
+
+ switch(m_ClipType)
+ {
+ case ctIntersection:
+ switch(pft2)
+ {
+ case pftEvenOdd:
+ case pftNonZero:
+ return (edge.WindCnt2 != 0);
+ case pftPositive:
+ return (edge.WindCnt2 > 0);
+ default:
+ return (edge.WindCnt2 < 0);
+ }
+ break;
+ case ctUnion:
+ switch(pft2)
+ {
+ case pftEvenOdd:
+ case pftNonZero:
+ return (edge.WindCnt2 == 0);
+ case pftPositive:
+ return (edge.WindCnt2 <= 0);
+ default:
+ return (edge.WindCnt2 >= 0);
+ }
+ break;
+ case ctDifference:
+ if (edge.PolyTyp == ptSubject)
+ switch(pft2)
+ {
+ case pftEvenOdd:
+ case pftNonZero:
+ return (edge.WindCnt2 == 0);
+ case pftPositive:
+ return (edge.WindCnt2 <= 0);
+ default:
+ return (edge.WindCnt2 >= 0);
+ }
+ else
+ switch(pft2)
+ {
+ case pftEvenOdd:
+ case pftNonZero:
+ return (edge.WindCnt2 != 0);
+ case pftPositive:
+ return (edge.WindCnt2 > 0);
+ default:
+ return (edge.WindCnt2 < 0);
+ }
+ break;
+ case ctXor:
+ if (edge.WindDelta == 0) //XOr always contributing unless open
+ switch(pft2)
+ {
+ case pftEvenOdd:
+ case pftNonZero:
+ return (edge.WindCnt2 == 0);
+ case pftPositive:
+ return (edge.WindCnt2 <= 0);
+ default:
+ return (edge.WindCnt2 >= 0);
+ }
+ else
+ return true;
+ break;
+ default:
+ return true;
+ }
+}
+//------------------------------------------------------------------------------
+
+OutPt* Clipper::AddLocalMinPoly(TEdge *e1, TEdge *e2, const IntPoint &Pt)
+{
+ OutPt* result;
+ TEdge *e, *prevE;
+ if (IsHorizontal(*e2) || ( e1->Dx > e2->Dx ))
+ {
+ result = AddOutPt(e1, Pt);
+ e2->OutIdx = e1->OutIdx;
+ e1->Side = esLeft;
+ e2->Side = esRight;
+ e = e1;
+ if (e->PrevInAEL == e2)
+ prevE = e2->PrevInAEL;
+ else
+ prevE = e->PrevInAEL;
+ } else
+ {
+ result = AddOutPt(e2, Pt);
+ e1->OutIdx = e2->OutIdx;
+ e1->Side = esRight;
+ e2->Side = esLeft;
+ e = e2;
+ if (e->PrevInAEL == e1)
+ prevE = e1->PrevInAEL;
+ else
+ prevE = e->PrevInAEL;
+ }
+
+ if (prevE && prevE->OutIdx >= 0 && prevE->Top.Y < Pt.Y && e->Top.Y < Pt.Y)
+ {
+ cInt xPrev = TopX(*prevE, Pt.Y);
+ cInt xE = TopX(*e, Pt.Y);
+ if (xPrev == xE && (e->WindDelta != 0) && (prevE->WindDelta != 0) &&
+ SlopesEqual(IntPoint(xPrev, Pt.Y), prevE->Top, IntPoint(xE, Pt.Y), e->Top, m_UseFullRange))
+ {
+ OutPt* outPt = AddOutPt(prevE, Pt);
+ AddJoin(result, outPt, e->Top);
+ }
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::AddLocalMaxPoly(TEdge *e1, TEdge *e2, const IntPoint &Pt)
+{
+ AddOutPt( e1, Pt );
+ if (e2->WindDelta == 0) AddOutPt(e2, Pt);
+ if( e1->OutIdx == e2->OutIdx )
+ {
+ e1->OutIdx = Unassigned;
+ e2->OutIdx = Unassigned;
+ }
+ else if (e1->OutIdx < e2->OutIdx)
+ AppendPolygon(e1, e2);
+ else
+ AppendPolygon(e2, e1);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::AddEdgeToSEL(TEdge *edge)
+{
+ //SEL pointers in PEdge are reused to build a list of horizontal edges.
+ //However, we don't need to worry about order with horizontal edge processing.
+ if( !m_SortedEdges )
+ {
+ m_SortedEdges = edge;
+ edge->PrevInSEL = 0;
+ edge->NextInSEL = 0;
+ }
+ else
+ {
+ edge->NextInSEL = m_SortedEdges;
+ edge->PrevInSEL = 0;
+ m_SortedEdges->PrevInSEL = edge;
+ m_SortedEdges = edge;
+ }
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::PopEdgeFromSEL(TEdge *&edge)
+{
+ if (!m_SortedEdges) return false;
+ edge = m_SortedEdges;
+ DeleteFromSEL(m_SortedEdges);
+ return true;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::CopyAELToSEL()
+{
+ TEdge* e = m_ActiveEdges;
+ m_SortedEdges = e;
+ while ( e )
+ {
+ e->PrevInSEL = e->PrevInAEL;
+ e->NextInSEL = e->NextInAEL;
+ e = e->NextInAEL;
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::AddJoin(OutPt *op1, OutPt *op2, const IntPoint OffPt)
+{
+ Join* j = new Join;
+ j->OutPt1 = op1;
+ j->OutPt2 = op2;
+ j->OffPt = OffPt;
+ m_Joins.push_back(j);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::ClearJoins()
+{
+ for (JoinList::size_type i = 0; i < m_Joins.size(); i++)
+ delete m_Joins[i];
+ m_Joins.resize(0);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::ClearGhostJoins()
+{
+ for (JoinList::size_type i = 0; i < m_GhostJoins.size(); i++)
+ delete m_GhostJoins[i];
+ m_GhostJoins.resize(0);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::AddGhostJoin(OutPt *op, const IntPoint OffPt)
+{
+ Join* j = new Join;
+ j->OutPt1 = op;
+ j->OutPt2 = 0;
+ j->OffPt = OffPt;
+ m_GhostJoins.push_back(j);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::InsertLocalMinimaIntoAEL(const cInt botY)
+{
+ const LocalMinimum *lm;
+ while (PopLocalMinima(botY, lm))
+ {
+ TEdge* lb = lm->LeftBound;
+ TEdge* rb = lm->RightBound;
+
+ OutPt *Op1 = 0;
+ if (!lb)
+ {
+ //nb: don't insert LB into either AEL or SEL
+ InsertEdgeIntoAEL(rb, 0);
+ SetWindingCount(*rb);
+ if (IsContributing(*rb))
+ Op1 = AddOutPt(rb, rb->Bot);
+ }
+ else if (!rb)
+ {
+ InsertEdgeIntoAEL(lb, 0);
+ SetWindingCount(*lb);
+ if (IsContributing(*lb))
+ Op1 = AddOutPt(lb, lb->Bot);
+ InsertScanbeam(lb->Top.Y);
+ }
+ else
+ {
+ InsertEdgeIntoAEL(lb, 0);
+ InsertEdgeIntoAEL(rb, lb);
+ SetWindingCount( *lb );
+ rb->WindCnt = lb->WindCnt;
+ rb->WindCnt2 = lb->WindCnt2;
+ if (IsContributing(*lb))
+ Op1 = AddLocalMinPoly(lb, rb, lb->Bot);
+ InsertScanbeam(lb->Top.Y);
+ }
+
+ if (rb)
+ {
+ if (IsHorizontal(*rb))
+ {
+ AddEdgeToSEL(rb);
+ if (rb->NextInLML)
+ InsertScanbeam(rb->NextInLML->Top.Y);
+ }
+ else InsertScanbeam( rb->Top.Y );
+ }
+
+ if (!lb || !rb) continue;
+
+ //if any output polygons share an edge, they'll need joining later ...
+ if (Op1 && IsHorizontal(*rb) &&
+ m_GhostJoins.size() > 0 && (rb->WindDelta != 0))
+ {
+ for (JoinList::size_type i = 0; i < m_GhostJoins.size(); ++i)
+ {
+ Join* jr = m_GhostJoins[i];
+ //if the horizontal Rb and a 'ghost' horizontal overlap, then convert
+ //the 'ghost' join to a real join ready for later ...
+ if (HorzSegmentsOverlap(jr->OutPt1->Pt.X, jr->OffPt.X, rb->Bot.X, rb->Top.X))
+ AddJoin(jr->OutPt1, Op1, jr->OffPt);
+ }
+ }
+
+ if (lb->OutIdx >= 0 && lb->PrevInAEL &&
+ lb->PrevInAEL->Curr.X == lb->Bot.X &&
+ lb->PrevInAEL->OutIdx >= 0 &&
+ SlopesEqual(lb->PrevInAEL->Bot, lb->PrevInAEL->Top, lb->Curr, lb->Top, m_UseFullRange) &&
+ (lb->WindDelta != 0) && (lb->PrevInAEL->WindDelta != 0))
+ {
+ OutPt *Op2 = AddOutPt(lb->PrevInAEL, lb->Bot);
+ AddJoin(Op1, Op2, lb->Top);
+ }
+
+ if(lb->NextInAEL != rb)
+ {
+
+ if (rb->OutIdx >= 0 && rb->PrevInAEL->OutIdx >= 0 &&
+ SlopesEqual(rb->PrevInAEL->Curr, rb->PrevInAEL->Top, rb->Curr, rb->Top, m_UseFullRange) &&
+ (rb->WindDelta != 0) && (rb->PrevInAEL->WindDelta != 0))
+ {
+ OutPt *Op2 = AddOutPt(rb->PrevInAEL, rb->Bot);
+ AddJoin(Op1, Op2, rb->Top);
+ }
+
+ TEdge* e = lb->NextInAEL;
+ if (e)
+ {
+ while( e != rb )
+ {
+ //nb: For calculating winding counts etc, IntersectEdges() assumes
+ //that param1 will be to the Right of param2 ABOVE the intersection ...
+ IntersectEdges(rb , e , lb->Curr); //order important here
+ e = e->NextInAEL;
+ }
+ }
+ }
+
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::DeleteFromSEL(TEdge *e)
+{
+ TEdge* SelPrev = e->PrevInSEL;
+ TEdge* SelNext = e->NextInSEL;
+ if( !SelPrev && !SelNext && (e != m_SortedEdges) ) return; //already deleted
+ if( SelPrev ) SelPrev->NextInSEL = SelNext;
+ else m_SortedEdges = SelNext;
+ if( SelNext ) SelNext->PrevInSEL = SelPrev;
+ e->NextInSEL = 0;
+ e->PrevInSEL = 0;
+}
+//------------------------------------------------------------------------------
+
+#ifdef use_xyz
+void Clipper::SetZ(IntPoint& pt, TEdge& e1, TEdge& e2)
+{
+ if (pt.Z != 0 || !m_ZFill) return;
+ else if (pt == e1.Bot) pt.Z = e1.Bot.Z;
+ else if (pt == e1.Top) pt.Z = e1.Top.Z;
+ else if (pt == e2.Bot) pt.Z = e2.Bot.Z;
+ else if (pt == e2.Top) pt.Z = e2.Top.Z;
+ else (*m_ZFill)(e1.Bot, e1.Top, e2.Bot, e2.Top, pt);
+}
+//------------------------------------------------------------------------------
+#endif
+
+void Clipper::IntersectEdges(TEdge *e1, TEdge *e2, IntPoint &Pt)
+{
+ bool e1Contributing = ( e1->OutIdx >= 0 );
+ bool e2Contributing = ( e2->OutIdx >= 0 );
+
+#ifdef use_xyz
+ SetZ(Pt, *e1, *e2);
+#endif
+
+#ifdef use_lines
+ //if either edge is on an OPEN path ...
+ if (e1->WindDelta == 0 || e2->WindDelta == 0)
+ {
+ //ignore subject-subject open path intersections UNLESS they
+ //are both open paths, AND they are both 'contributing maximas' ...
+ if (e1->WindDelta == 0 && e2->WindDelta == 0) return;
+
+ //if intersecting a subj line with a subj poly ...
+ else if (e1->PolyTyp == e2->PolyTyp &&
+ e1->WindDelta != e2->WindDelta && m_ClipType == ctUnion)
+ {
+ if (e1->WindDelta == 0)
+ {
+ if (e2Contributing)
+ {
+ AddOutPt(e1, Pt);
+ if (e1Contributing) e1->OutIdx = Unassigned;
+ }
+ }
+ else
+ {
+ if (e1Contributing)
+ {
+ AddOutPt(e2, Pt);
+ if (e2Contributing) e2->OutIdx = Unassigned;
+ }
+ }
+ }
+ else if (e1->PolyTyp != e2->PolyTyp)
+ {
+ //toggle subj open path OutIdx on/off when Abs(clip.WndCnt) == 1 ...
+ if ((e1->WindDelta == 0) && abs(e2->WindCnt) == 1 &&
+ (m_ClipType != ctUnion || e2->WindCnt2 == 0))
+ {
+ AddOutPt(e1, Pt);
+ if (e1Contributing) e1->OutIdx = Unassigned;
+ }
+ else if ((e2->WindDelta == 0) && (abs(e1->WindCnt) == 1) &&
+ (m_ClipType != ctUnion || e1->WindCnt2 == 0))
+ {
+ AddOutPt(e2, Pt);
+ if (e2Contributing) e2->OutIdx = Unassigned;
+ }
+ }
+ return;
+ }
+#endif
+
+ //update winding counts...
+ //assumes that e1 will be to the Right of e2 ABOVE the intersection
+ if ( e1->PolyTyp == e2->PolyTyp )
+ {
+ if ( IsEvenOddFillType( *e1) )
+ {
+ int oldE1WindCnt = e1->WindCnt;
+ e1->WindCnt = e2->WindCnt;
+ e2->WindCnt = oldE1WindCnt;
+ } else
+ {
+ if (e1->WindCnt + e2->WindDelta == 0 ) e1->WindCnt = -e1->WindCnt;
+ else e1->WindCnt += e2->WindDelta;
+ if ( e2->WindCnt - e1->WindDelta == 0 ) e2->WindCnt = -e2->WindCnt;
+ else e2->WindCnt -= e1->WindDelta;
+ }
+ } else
+ {
+ if (!IsEvenOddFillType(*e2)) e1->WindCnt2 += e2->WindDelta;
+ else e1->WindCnt2 = ( e1->WindCnt2 == 0 ) ? 1 : 0;
+ if (!IsEvenOddFillType(*e1)) e2->WindCnt2 -= e1->WindDelta;
+ else e2->WindCnt2 = ( e2->WindCnt2 == 0 ) ? 1 : 0;
+ }
+
+ PolyFillType e1FillType, e2FillType, e1FillType2, e2FillType2;
+ if (e1->PolyTyp == ptSubject)
+ {
+ e1FillType = m_SubjFillType;
+ e1FillType2 = m_ClipFillType;
+ } else
+ {
+ e1FillType = m_ClipFillType;
+ e1FillType2 = m_SubjFillType;
+ }
+ if (e2->PolyTyp == ptSubject)
+ {
+ e2FillType = m_SubjFillType;
+ e2FillType2 = m_ClipFillType;
+ } else
+ {
+ e2FillType = m_ClipFillType;
+ e2FillType2 = m_SubjFillType;
+ }
+
+ cInt e1Wc, e2Wc;
+ switch (e1FillType)
+ {
+ case pftPositive: e1Wc = e1->WindCnt; break;
+ case pftNegative: e1Wc = -e1->WindCnt; break;
+ default: e1Wc = Abs(e1->WindCnt);
+ }
+ switch(e2FillType)
+ {
+ case pftPositive: e2Wc = e2->WindCnt; break;
+ case pftNegative: e2Wc = -e2->WindCnt; break;
+ default: e2Wc = Abs(e2->WindCnt);
+ }
+
+ if ( e1Contributing && e2Contributing )
+ {
+ if ((e1Wc != 0 && e1Wc != 1) || (e2Wc != 0 && e2Wc != 1) ||
+ (e1->PolyTyp != e2->PolyTyp && m_ClipType != ctXor) )
+ {
+ AddLocalMaxPoly(e1, e2, Pt);
+ }
+ else
+ {
+ AddOutPt(e1, Pt);
+ AddOutPt(e2, Pt);
+ SwapSides( *e1 , *e2 );
+ SwapPolyIndexes( *e1 , *e2 );
+ }
+ }
+ else if ( e1Contributing )
+ {
+ if (e2Wc == 0 || e2Wc == 1)
+ {
+ AddOutPt(e1, Pt);
+ SwapSides(*e1, *e2);
+ SwapPolyIndexes(*e1, *e2);
+ }
+ }
+ else if ( e2Contributing )
+ {
+ if (e1Wc == 0 || e1Wc == 1)
+ {
+ AddOutPt(e2, Pt);
+ SwapSides(*e1, *e2);
+ SwapPolyIndexes(*e1, *e2);
+ }
+ }
+ else if ( (e1Wc == 0 || e1Wc == 1) && (e2Wc == 0 || e2Wc == 1))
+ {
+ //neither edge is currently contributing ...
+
+ cInt e1Wc2, e2Wc2;
+ switch (e1FillType2)
+ {
+ case pftPositive: e1Wc2 = e1->WindCnt2; break;
+ case pftNegative : e1Wc2 = -e1->WindCnt2; break;
+ default: e1Wc2 = Abs(e1->WindCnt2);
+ }
+ switch (e2FillType2)
+ {
+ case pftPositive: e2Wc2 = e2->WindCnt2; break;
+ case pftNegative: e2Wc2 = -e2->WindCnt2; break;
+ default: e2Wc2 = Abs(e2->WindCnt2);
+ }
+
+ if (e1->PolyTyp != e2->PolyTyp)
+ {
+ AddLocalMinPoly(e1, e2, Pt);
+ }
+ else if (e1Wc == 1 && e2Wc == 1)
+ switch( m_ClipType ) {
+ case ctIntersection:
+ if (e1Wc2 > 0 && e2Wc2 > 0)
+ AddLocalMinPoly(e1, e2, Pt);
+ break;
+ case ctUnion:
+ if ( e1Wc2 <= 0 && e2Wc2 <= 0 )
+ AddLocalMinPoly(e1, e2, Pt);
+ break;
+ case ctDifference:
+ if (((e1->PolyTyp == ptClip) && (e1Wc2 > 0) && (e2Wc2 > 0)) ||
+ ((e1->PolyTyp == ptSubject) && (e1Wc2 <= 0) && (e2Wc2 <= 0)))
+ AddLocalMinPoly(e1, e2, Pt);
+ break;
+ case ctXor:
+ AddLocalMinPoly(e1, e2, Pt);
+ }
+ else
+ SwapSides( *e1, *e2 );
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::SetHoleState(TEdge *e, OutRec *outrec)
+{
+ TEdge *e2 = e->PrevInAEL;
+ TEdge *eTmp = 0;
+ while (e2)
+ {
+ if (e2->OutIdx >= 0 && e2->WindDelta != 0)
+ {
+ if (!eTmp) eTmp = e2;
+ else if (eTmp->OutIdx == e2->OutIdx) eTmp = 0;
+ }
+ e2 = e2->PrevInAEL;
+ }
+ if (!eTmp)
+ {
+ outrec->FirstLeft = 0;
+ outrec->IsHole = false;
+ }
+ else
+ {
+ outrec->FirstLeft = m_PolyOuts[eTmp->OutIdx];
+ outrec->IsHole = !outrec->FirstLeft->IsHole;
+ }
+}
+//------------------------------------------------------------------------------
+
+OutRec* GetLowermostRec(OutRec *outRec1, OutRec *outRec2)
+{
+ //work out which polygon fragment has the correct hole state ...
+ if (!outRec1->BottomPt)
+ outRec1->BottomPt = GetBottomPt(outRec1->Pts);
+ if (!outRec2->BottomPt)
+ outRec2->BottomPt = GetBottomPt(outRec2->Pts);
+ OutPt *OutPt1 = outRec1->BottomPt;
+ OutPt *OutPt2 = outRec2->BottomPt;
+ if (OutPt1->Pt.Y > OutPt2->Pt.Y) return outRec1;
+ else if (OutPt1->Pt.Y < OutPt2->Pt.Y) return outRec2;
+ else if (OutPt1->Pt.X < OutPt2->Pt.X) return outRec1;
+ else if (OutPt1->Pt.X > OutPt2->Pt.X) return outRec2;
+ else if (OutPt1->Next == OutPt1) return outRec2;
+ else if (OutPt2->Next == OutPt2) return outRec1;
+ else if (FirstIsBottomPt(OutPt1, OutPt2)) return outRec1;
+ else return outRec2;
+}
+//------------------------------------------------------------------------------
+
+bool OutRec1RightOfOutRec2(OutRec* outRec1, OutRec* outRec2)
+{
+ do
+ {
+ outRec1 = outRec1->FirstLeft;
+ if (outRec1 == outRec2) return true;
+ } while (outRec1);
+ return false;
+}
+//------------------------------------------------------------------------------
+
+OutRec* Clipper::GetOutRec(int Idx)
+{
+ OutRec* outrec = m_PolyOuts[Idx];
+ while (outrec != m_PolyOuts[outrec->Idx])
+ outrec = m_PolyOuts[outrec->Idx];
+ return outrec;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::AppendPolygon(TEdge *e1, TEdge *e2)
+{
+ //get the start and ends of both output polygons ...
+ OutRec *outRec1 = m_PolyOuts[e1->OutIdx];
+ OutRec *outRec2 = m_PolyOuts[e2->OutIdx];
+
+ OutRec *holeStateRec;
+ if (OutRec1RightOfOutRec2(outRec1, outRec2))
+ holeStateRec = outRec2;
+ else if (OutRec1RightOfOutRec2(outRec2, outRec1))
+ holeStateRec = outRec1;
+ else
+ holeStateRec = GetLowermostRec(outRec1, outRec2);
+
+ //get the start and ends of both output polygons and
+ //join e2 poly onto e1 poly and delete pointers to e2 ...
+
+ OutPt* p1_lft = outRec1->Pts;
+ OutPt* p1_rt = p1_lft->Prev;
+ OutPt* p2_lft = outRec2->Pts;
+ OutPt* p2_rt = p2_lft->Prev;
+
+ //join e2 poly onto e1 poly and delete pointers to e2 ...
+ if( e1->Side == esLeft )
+ {
+ if( e2->Side == esLeft )
+ {
+ //z y x a b c
+ ReversePolyPtLinks(p2_lft);
+ p2_lft->Next = p1_lft;
+ p1_lft->Prev = p2_lft;
+ p1_rt->Next = p2_rt;
+ p2_rt->Prev = p1_rt;
+ outRec1->Pts = p2_rt;
+ } else
+ {
+ //x y z a b c
+ p2_rt->Next = p1_lft;
+ p1_lft->Prev = p2_rt;
+ p2_lft->Prev = p1_rt;
+ p1_rt->Next = p2_lft;
+ outRec1->Pts = p2_lft;
+ }
+ } else
+ {
+ if( e2->Side == esRight )
+ {
+ //a b c z y x
+ ReversePolyPtLinks(p2_lft);
+ p1_rt->Next = p2_rt;
+ p2_rt->Prev = p1_rt;
+ p2_lft->Next = p1_lft;
+ p1_lft->Prev = p2_lft;
+ } else
+ {
+ //a b c x y z
+ p1_rt->Next = p2_lft;
+ p2_lft->Prev = p1_rt;
+ p1_lft->Prev = p2_rt;
+ p2_rt->Next = p1_lft;
+ }
+ }
+
+ outRec1->BottomPt = 0;
+ if (holeStateRec == outRec2)
+ {
+ if (outRec2->FirstLeft != outRec1)
+ outRec1->FirstLeft = outRec2->FirstLeft;
+ outRec1->IsHole = outRec2->IsHole;
+ }
+ outRec2->Pts = 0;
+ outRec2->BottomPt = 0;
+ outRec2->FirstLeft = outRec1;
+
+ int OKIdx = e1->OutIdx;
+ int ObsoleteIdx = e2->OutIdx;
+
+ e1->OutIdx = Unassigned; //nb: safe because we only get here via AddLocalMaxPoly
+ e2->OutIdx = Unassigned;
+
+ TEdge* e = m_ActiveEdges;
+ while( e )
+ {
+ if( e->OutIdx == ObsoleteIdx )
+ {
+ e->OutIdx = OKIdx;
+ e->Side = e1->Side;
+ break;
+ }
+ e = e->NextInAEL;
+ }
+
+ outRec2->Idx = outRec1->Idx;
+}
+//------------------------------------------------------------------------------
+
+OutPt* Clipper::AddOutPt(TEdge *e, const IntPoint &pt)
+{
+ if( e->OutIdx < 0 )
+ {
+ OutRec *outRec = CreateOutRec();
+ outRec->IsOpen = (e->WindDelta == 0);
+ OutPt* newOp = new OutPt;
+ outRec->Pts = newOp;
+ newOp->Idx = outRec->Idx;
+ newOp->Pt = pt;
+ newOp->Next = newOp;
+ newOp->Prev = newOp;
+ if (!outRec->IsOpen)
+ SetHoleState(e, outRec);
+ e->OutIdx = outRec->Idx;
+ return newOp;
+ } else
+ {
+ OutRec *outRec = m_PolyOuts[e->OutIdx];
+ //OutRec.Pts is the 'Left-most' point & OutRec.Pts.Prev is the 'Right-most'
+ OutPt* op = outRec->Pts;
+
+ bool ToFront = (e->Side == esLeft);
+ if (ToFront && (pt == op->Pt)) return op;
+ else if (!ToFront && (pt == op->Prev->Pt)) return op->Prev;
+
+ OutPt* newOp = new OutPt;
+ newOp->Idx = outRec->Idx;
+ newOp->Pt = pt;
+ newOp->Next = op;
+ newOp->Prev = op->Prev;
+ newOp->Prev->Next = newOp;
+ op->Prev = newOp;
+ if (ToFront) outRec->Pts = newOp;
+ return newOp;
+ }
+}
+//------------------------------------------------------------------------------
+
+OutPt* Clipper::GetLastOutPt(TEdge *e)
+{
+ OutRec *outRec = m_PolyOuts[e->OutIdx];
+ if (e->Side == esLeft)
+ return outRec->Pts;
+ else
+ return outRec->Pts->Prev;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::ProcessHorizontals()
+{
+ TEdge* horzEdge;
+ while (PopEdgeFromSEL(horzEdge))
+ ProcessHorizontal(horzEdge);
+}
+//------------------------------------------------------------------------------
+
+inline bool IsMinima(TEdge *e)
+{
+ return e && (e->Prev->NextInLML != e) && (e->Next->NextInLML != e);
+}
+//------------------------------------------------------------------------------
+
+inline bool IsMaxima(TEdge *e, const cInt Y)
+{
+ return e && e->Top.Y == Y && !e->NextInLML;
+}
+//------------------------------------------------------------------------------
+
+inline bool IsIntermediate(TEdge *e, const cInt Y)
+{
+ return e->Top.Y == Y && e->NextInLML;
+}
+//------------------------------------------------------------------------------
+
+TEdge *GetMaximaPair(TEdge *e)
+{
+ if ((e->Next->Top == e->Top) && !e->Next->NextInLML)
+ return e->Next;
+ else if ((e->Prev->Top == e->Top) && !e->Prev->NextInLML)
+ return e->Prev;
+ else return 0;
+}
+//------------------------------------------------------------------------------
+
+TEdge *GetMaximaPairEx(TEdge *e)
+{
+ //as GetMaximaPair() but returns 0 if MaxPair isn't in AEL (unless it's horizontal)
+ TEdge* result = GetMaximaPair(e);
+ if (result && (result->OutIdx == Skip ||
+ (result->NextInAEL == result->PrevInAEL && !IsHorizontal(*result)))) return 0;
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::SwapPositionsInSEL(TEdge *Edge1, TEdge *Edge2)
+{
+ if( !( Edge1->NextInSEL ) && !( Edge1->PrevInSEL ) ) return;
+ if( !( Edge2->NextInSEL ) && !( Edge2->PrevInSEL ) ) return;
+
+ if( Edge1->NextInSEL == Edge2 )
+ {
+ TEdge* Next = Edge2->NextInSEL;
+ if( Next ) Next->PrevInSEL = Edge1;
+ TEdge* Prev = Edge1->PrevInSEL;
+ if( Prev ) Prev->NextInSEL = Edge2;
+ Edge2->PrevInSEL = Prev;
+ Edge2->NextInSEL = Edge1;
+ Edge1->PrevInSEL = Edge2;
+ Edge1->NextInSEL = Next;
+ }
+ else if( Edge2->NextInSEL == Edge1 )
+ {
+ TEdge* Next = Edge1->NextInSEL;
+ if( Next ) Next->PrevInSEL = Edge2;
+ TEdge* Prev = Edge2->PrevInSEL;
+ if( Prev ) Prev->NextInSEL = Edge1;
+ Edge1->PrevInSEL = Prev;
+ Edge1->NextInSEL = Edge2;
+ Edge2->PrevInSEL = Edge1;
+ Edge2->NextInSEL = Next;
+ }
+ else
+ {
+ TEdge* Next = Edge1->NextInSEL;
+ TEdge* Prev = Edge1->PrevInSEL;
+ Edge1->NextInSEL = Edge2->NextInSEL;
+ if( Edge1->NextInSEL ) Edge1->NextInSEL->PrevInSEL = Edge1;
+ Edge1->PrevInSEL = Edge2->PrevInSEL;
+ if( Edge1->PrevInSEL ) Edge1->PrevInSEL->NextInSEL = Edge1;
+ Edge2->NextInSEL = Next;
+ if( Edge2->NextInSEL ) Edge2->NextInSEL->PrevInSEL = Edge2;
+ Edge2->PrevInSEL = Prev;
+ if( Edge2->PrevInSEL ) Edge2->PrevInSEL->NextInSEL = Edge2;
+ }
+
+ if( !Edge1->PrevInSEL ) m_SortedEdges = Edge1;
+ else if( !Edge2->PrevInSEL ) m_SortedEdges = Edge2;
+}
+//------------------------------------------------------------------------------
+
+TEdge* GetNextInAEL(TEdge *e, Direction dir)
+{
+ return dir == dLeftToRight ? e->NextInAEL : e->PrevInAEL;
+}
+//------------------------------------------------------------------------------
+
+void GetHorzDirection(TEdge& HorzEdge, Direction& Dir, cInt& Left, cInt& Right)
+{
+ if (HorzEdge.Bot.X < HorzEdge.Top.X)
+ {
+ Left = HorzEdge.Bot.X;
+ Right = HorzEdge.Top.X;
+ Dir = dLeftToRight;
+ } else
+ {
+ Left = HorzEdge.Top.X;
+ Right = HorzEdge.Bot.X;
+ Dir = dRightToLeft;
+ }
+}
+//------------------------------------------------------------------------
+
+/*******************************************************************************
+* Notes: Horizontal edges (HEs) at scanline intersections (ie at the Top or *
+* Bottom of a scanbeam) are processed as if layered. The order in which HEs *
+* are processed doesn't matter. HEs intersect with other HE Bot.Xs only [#] *
+* (or they could intersect with Top.Xs only, ie EITHER Bot.Xs OR Top.Xs), *
+* and with other non-horizontal edges [*]. Once these intersections are *
+* processed, intermediate HEs then 'promote' the Edge above (NextInLML) into *
+* the AEL. These 'promoted' edges may in turn intersect [%] with other HEs. *
+*******************************************************************************/
+
+void Clipper::ProcessHorizontal(TEdge *horzEdge)
+{
+ Direction dir;
+ cInt horzLeft, horzRight;
+ bool IsOpen = (horzEdge->WindDelta == 0);
+
+ GetHorzDirection(*horzEdge, dir, horzLeft, horzRight);
+
+ TEdge* eLastHorz = horzEdge, *eMaxPair = 0;
+ while (eLastHorz->NextInLML && IsHorizontal(*eLastHorz->NextInLML))
+ eLastHorz = eLastHorz->NextInLML;
+ if (!eLastHorz->NextInLML)
+ eMaxPair = GetMaximaPair(eLastHorz);
+
+ MaximaList::const_iterator maxIt;
+ MaximaList::const_reverse_iterator maxRit;
+ if (m_Maxima.size() > 0)
+ {
+ //get the first maxima in range (X) ...
+ if (dir == dLeftToRight)
+ {
+ maxIt = m_Maxima.begin();
+ while (maxIt != m_Maxima.end() && *maxIt <= horzEdge->Bot.X) maxIt++;
+ if (maxIt != m_Maxima.end() && *maxIt >= eLastHorz->Top.X)
+ maxIt = m_Maxima.end();
+ }
+ else
+ {
+ maxRit = m_Maxima.rbegin();
+ while (maxRit != m_Maxima.rend() && *maxRit > horzEdge->Bot.X) maxRit++;
+ if (maxRit != m_Maxima.rend() && *maxRit <= eLastHorz->Top.X)
+ maxRit = m_Maxima.rend();
+ }
+ }
+
+ OutPt* op1 = 0;
+
+ for (;;) //loop through consec. horizontal edges
+ {
+
+ bool IsLastHorz = (horzEdge == eLastHorz);
+ TEdge* e = GetNextInAEL(horzEdge, dir);
+ while(e)
+ {
+
+ //this code block inserts extra coords into horizontal edges (in output
+ //polygons) whereever maxima touch these horizontal edges. This helps
+ //'simplifying' polygons (ie if the Simplify property is set).
+ if (m_Maxima.size() > 0)
+ {
+ if (dir == dLeftToRight)
+ {
+ while (maxIt != m_Maxima.end() && *maxIt < e->Curr.X)
+ {
+ if (horzEdge->OutIdx >= 0 && !IsOpen)
+ AddOutPt(horzEdge, IntPoint(*maxIt, horzEdge->Bot.Y));
+ maxIt++;
+ }
+ }
+ else
+ {
+ while (maxRit != m_Maxima.rend() && *maxRit > e->Curr.X)
+ {
+ if (horzEdge->OutIdx >= 0 && !IsOpen)
+ AddOutPt(horzEdge, IntPoint(*maxRit, horzEdge->Bot.Y));
+ maxRit++;
+ }
+ }
+ };
+
+ if ((dir == dLeftToRight && e->Curr.X > horzRight) ||
+ (dir == dRightToLeft && e->Curr.X < horzLeft)) break;
+
+ //Also break if we've got to the end of an intermediate horizontal edge ...
+ //nb: Smaller Dx's are to the right of larger Dx's ABOVE the horizontal.
+ if (e->Curr.X == horzEdge->Top.X && horzEdge->NextInLML &&
+ e->Dx < horzEdge->NextInLML->Dx) break;
+
+ if (horzEdge->OutIdx >= 0 && !IsOpen) //note: may be done multiple times
+ {
+#ifdef use_xyz
+ if (dir == dLeftToRight) SetZ(e->Curr, *horzEdge, *e);
+ else SetZ(e->Curr, *e, *horzEdge);
+#endif
+ op1 = AddOutPt(horzEdge, e->Curr);
+ TEdge* eNextHorz = m_SortedEdges;
+ while (eNextHorz)
+ {
+ if (eNextHorz->OutIdx >= 0 &&
+ HorzSegmentsOverlap(horzEdge->Bot.X,
+ horzEdge->Top.X, eNextHorz->Bot.X, eNextHorz->Top.X))
+ {
+ OutPt* op2 = GetLastOutPt(eNextHorz);
+ AddJoin(op2, op1, eNextHorz->Top);
+ }
+ eNextHorz = eNextHorz->NextInSEL;
+ }
+ AddGhostJoin(op1, horzEdge->Bot);
+ }
+
+ //OK, so far we're still in range of the horizontal Edge but make sure
+ //we're at the last of consec. horizontals when matching with eMaxPair
+ if(e == eMaxPair && IsLastHorz)
+ {
+ if (horzEdge->OutIdx >= 0)
+ AddLocalMaxPoly(horzEdge, eMaxPair, horzEdge->Top);
+ DeleteFromAEL(horzEdge);
+ DeleteFromAEL(eMaxPair);
+ return;
+ }
+
+ if(dir == dLeftToRight)
+ {
+ IntPoint Pt = IntPoint(e->Curr.X, horzEdge->Curr.Y);
+ IntersectEdges(horzEdge, e, Pt);
+ }
+ else
+ {
+ IntPoint Pt = IntPoint(e->Curr.X, horzEdge->Curr.Y);
+ IntersectEdges( e, horzEdge, Pt);
+ }
+ TEdge* eNext = GetNextInAEL(e, dir);
+ SwapPositionsInAEL( horzEdge, e );
+ e = eNext;
+ } //end while(e)
+
+ //Break out of loop if HorzEdge.NextInLML is not also horizontal ...
+ if (!horzEdge->NextInLML || !IsHorizontal(*horzEdge->NextInLML)) break;
+
+ UpdateEdgeIntoAEL(horzEdge);
+ if (horzEdge->OutIdx >= 0) AddOutPt(horzEdge, horzEdge->Bot);
+ GetHorzDirection(*horzEdge, dir, horzLeft, horzRight);
+
+ } //end for (;;)
+
+ if (horzEdge->OutIdx >= 0 && !op1)
+ {
+ op1 = GetLastOutPt(horzEdge);
+ TEdge* eNextHorz = m_SortedEdges;
+ while (eNextHorz)
+ {
+ if (eNextHorz->OutIdx >= 0 &&
+ HorzSegmentsOverlap(horzEdge->Bot.X,
+ horzEdge->Top.X, eNextHorz->Bot.X, eNextHorz->Top.X))
+ {
+ OutPt* op2 = GetLastOutPt(eNextHorz);
+ AddJoin(op2, op1, eNextHorz->Top);
+ }
+ eNextHorz = eNextHorz->NextInSEL;
+ }
+ AddGhostJoin(op1, horzEdge->Top);
+ }
+
+ if (horzEdge->NextInLML)
+ {
+ if(horzEdge->OutIdx >= 0)
+ {
+ op1 = AddOutPt( horzEdge, horzEdge->Top);
+ UpdateEdgeIntoAEL(horzEdge);
+ if (horzEdge->WindDelta == 0) return;
+ //nb: HorzEdge is no longer horizontal here
+ TEdge* ePrev = horzEdge->PrevInAEL;
+ TEdge* eNext = horzEdge->NextInAEL;
+ if (ePrev && ePrev->Curr.X == horzEdge->Bot.X &&
+ ePrev->Curr.Y == horzEdge->Bot.Y && ePrev->WindDelta != 0 &&
+ (ePrev->OutIdx >= 0 && ePrev->Curr.Y > ePrev->Top.Y &&
+ SlopesEqual(*horzEdge, *ePrev, m_UseFullRange)))
+ {
+ OutPt* op2 = AddOutPt(ePrev, horzEdge->Bot);
+ AddJoin(op1, op2, horzEdge->Top);
+ }
+ else if (eNext && eNext->Curr.X == horzEdge->Bot.X &&
+ eNext->Curr.Y == horzEdge->Bot.Y && eNext->WindDelta != 0 &&
+ eNext->OutIdx >= 0 && eNext->Curr.Y > eNext->Top.Y &&
+ SlopesEqual(*horzEdge, *eNext, m_UseFullRange))
+ {
+ OutPt* op2 = AddOutPt(eNext, horzEdge->Bot);
+ AddJoin(op1, op2, horzEdge->Top);
+ }
+ }
+ else
+ UpdateEdgeIntoAEL(horzEdge);
+ }
+ else
+ {
+ if (horzEdge->OutIdx >= 0) AddOutPt(horzEdge, horzEdge->Top);
+ DeleteFromAEL(horzEdge);
+ }
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::ProcessIntersections(const cInt topY)
+{
+ if( !m_ActiveEdges ) return true;
+ try {
+ BuildIntersectList(topY);
+ size_t IlSize = m_IntersectList.size();
+ if (IlSize == 0) return true;
+ if (IlSize == 1 || FixupIntersectionOrder()) ProcessIntersectList();
+ else return false;
+ }
+ catch(...)
+ {
+ m_SortedEdges = 0;
+ DisposeIntersectNodes();
+ throw clipperException("ProcessIntersections error");
+ }
+ m_SortedEdges = 0;
+ return true;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::DisposeIntersectNodes()
+{
+ for (size_t i = 0; i < m_IntersectList.size(); ++i )
+ delete m_IntersectList[i];
+ m_IntersectList.clear();
+}
+//------------------------------------------------------------------------------
+
+void Clipper::BuildIntersectList(const cInt topY)
+{
+ if ( !m_ActiveEdges ) return;
+
+ //prepare for sorting ...
+ TEdge* e = m_ActiveEdges;
+ m_SortedEdges = e;
+ while( e )
+ {
+ e->PrevInSEL = e->PrevInAEL;
+ e->NextInSEL = e->NextInAEL;
+ e->Curr.X = TopX( *e, topY );
+ e = e->NextInAEL;
+ }
+
+ //bubblesort ...
+ bool isModified;
+ do
+ {
+ isModified = false;
+ e = m_SortedEdges;
+ while( e->NextInSEL )
+ {
+ TEdge *eNext = e->NextInSEL;
+ IntPoint Pt;
+ if(e->Curr.X > eNext->Curr.X)
+ {
+ IntersectPoint(*e, *eNext, Pt);
+ if (Pt.Y < topY) Pt = IntPoint(TopX(*e, topY), topY);
+ IntersectNode * newNode = new IntersectNode;
+ newNode->Edge1 = e;
+ newNode->Edge2 = eNext;
+ newNode->Pt = Pt;
+ m_IntersectList.push_back(newNode);
+
+ SwapPositionsInSEL(e, eNext);
+ isModified = true;
+ }
+ else
+ e = eNext;
+ }
+ if( e->PrevInSEL ) e->PrevInSEL->NextInSEL = 0;
+ else break;
+ }
+ while ( isModified );
+ m_SortedEdges = 0; //important
+}
+//------------------------------------------------------------------------------
+
+
+void Clipper::ProcessIntersectList()
+{
+ for (size_t i = 0; i < m_IntersectList.size(); ++i)
+ {
+ IntersectNode* iNode = m_IntersectList[i];
+ {
+ IntersectEdges( iNode->Edge1, iNode->Edge2, iNode->Pt);
+ SwapPositionsInAEL( iNode->Edge1 , iNode->Edge2 );
+ }
+ delete iNode;
+ }
+ m_IntersectList.clear();
+}
+//------------------------------------------------------------------------------
+
+bool IntersectListSort(IntersectNode* node1, IntersectNode* node2)
+{
+ return node2->Pt.Y < node1->Pt.Y;
+}
+//------------------------------------------------------------------------------
+
+inline bool EdgesAdjacent(const IntersectNode &inode)
+{
+ return (inode.Edge1->NextInSEL == inode.Edge2) ||
+ (inode.Edge1->PrevInSEL == inode.Edge2);
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::FixupIntersectionOrder()
+{
+ //pre-condition: intersections are sorted Bottom-most first.
+ //Now it's crucial that intersections are made only between adjacent edges,
+ //so to ensure this the order of intersections may need adjusting ...
+ CopyAELToSEL();
+ std::sort(m_IntersectList.begin(), m_IntersectList.end(), IntersectListSort);
+ size_t cnt = m_IntersectList.size();
+ for (size_t i = 0; i < cnt; ++i)
+ {
+ if (!EdgesAdjacent(*m_IntersectList[i]))
+ {
+ size_t j = i + 1;
+ while (j < cnt && !EdgesAdjacent(*m_IntersectList[j])) j++;
+ if (j == cnt) return false;
+ std::swap(m_IntersectList[i], m_IntersectList[j]);
+ }
+ SwapPositionsInSEL(m_IntersectList[i]->Edge1, m_IntersectList[i]->Edge2);
+ }
+ return true;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::DoMaxima(TEdge *e)
+{
+ TEdge* eMaxPair = GetMaximaPairEx(e);
+ if (!eMaxPair)
+ {
+ if (e->OutIdx >= 0)
+ AddOutPt(e, e->Top);
+ DeleteFromAEL(e);
+ return;
+ }
+
+ TEdge* eNext = e->NextInAEL;
+ while(eNext && eNext != eMaxPair)
+ {
+ IntersectEdges(e, eNext, e->Top);
+ SwapPositionsInAEL(e, eNext);
+ eNext = e->NextInAEL;
+ }
+
+ if(e->OutIdx == Unassigned && eMaxPair->OutIdx == Unassigned)
+ {
+ DeleteFromAEL(e);
+ DeleteFromAEL(eMaxPair);
+ }
+ else if( e->OutIdx >= 0 && eMaxPair->OutIdx >= 0 )
+ {
+ if (e->OutIdx >= 0) AddLocalMaxPoly(e, eMaxPair, e->Top);
+ DeleteFromAEL(e);
+ DeleteFromAEL(eMaxPair);
+ }
+#ifdef use_lines
+ else if (e->WindDelta == 0)
+ {
+ if (e->OutIdx >= 0)
+ {
+ AddOutPt(e, e->Top);
+ e->OutIdx = Unassigned;
+ }
+ DeleteFromAEL(e);
+
+ if (eMaxPair->OutIdx >= 0)
+ {
+ AddOutPt(eMaxPair, e->Top);
+ eMaxPair->OutIdx = Unassigned;
+ }
+ DeleteFromAEL(eMaxPair);
+ }
+#endif
+ else throw clipperException("DoMaxima error");
+}
+//------------------------------------------------------------------------------
+
+void Clipper::ProcessEdgesAtTopOfScanbeam(const cInt topY)
+{
+ TEdge* e = m_ActiveEdges;
+ while( e )
+ {
+ //1. process maxima, treating them as if they're 'bent' horizontal edges,
+ // but exclude maxima with horizontal edges. nb: e can't be a horizontal.
+ bool IsMaximaEdge = IsMaxima(e, topY);
+
+ if(IsMaximaEdge)
+ {
+ TEdge* eMaxPair = GetMaximaPairEx(e);
+ IsMaximaEdge = (!eMaxPair || !IsHorizontal(*eMaxPair));
+ }
+
+ if(IsMaximaEdge)
+ {
+ if (m_StrictSimple) m_Maxima.push_back(e->Top.X);
+ TEdge* ePrev = e->PrevInAEL;
+ DoMaxima(e);
+ if( !ePrev ) e = m_ActiveEdges;
+ else e = ePrev->NextInAEL;
+ }
+ else
+ {
+ //2. promote horizontal edges, otherwise update Curr.X and Curr.Y ...
+ if (IsIntermediate(e, topY) && IsHorizontal(*e->NextInLML))
+ {
+ UpdateEdgeIntoAEL(e);
+ if (e->OutIdx >= 0)
+ AddOutPt(e, e->Bot);
+ AddEdgeToSEL(e);
+ }
+ else
+ {
+ e->Curr.X = TopX( *e, topY );
+ e->Curr.Y = topY;
+#ifdef use_xyz
+ e->Curr.Z = topY == e->Top.Y ? e->Top.Z : (topY == e->Bot.Y ? e->Bot.Z : 0);
+#endif
+ }
+
+ //When StrictlySimple and 'e' is being touched by another edge, then
+ //make sure both edges have a vertex here ...
+ if (m_StrictSimple)
+ {
+ TEdge* ePrev = e->PrevInAEL;
+ if ((e->OutIdx >= 0) && (e->WindDelta != 0) && ePrev && (ePrev->OutIdx >= 0) &&
+ (ePrev->Curr.X == e->Curr.X) && (ePrev->WindDelta != 0))
+ {
+ IntPoint pt = e->Curr;
+#ifdef use_xyz
+ SetZ(pt, *ePrev, *e);
+#endif
+ OutPt* op = AddOutPt(ePrev, pt);
+ OutPt* op2 = AddOutPt(e, pt);
+ AddJoin(op, op2, pt); //StrictlySimple (type-3) join
+ }
+ }
+
+ e = e->NextInAEL;
+ }
+ }
+
+ //3. Process horizontals at the Top of the scanbeam ...
+ m_Maxima.sort();
+ ProcessHorizontals();
+ m_Maxima.clear();
+
+ //4. Promote intermediate vertices ...
+ e = m_ActiveEdges;
+ while(e)
+ {
+ if(IsIntermediate(e, topY))
+ {
+ OutPt* op = 0;
+ if( e->OutIdx >= 0 )
+ op = AddOutPt(e, e->Top);
+ UpdateEdgeIntoAEL(e);
+
+ //if output polygons share an edge, they'll need joining later ...
+ TEdge* ePrev = e->PrevInAEL;
+ TEdge* eNext = e->NextInAEL;
+ if (ePrev && ePrev->Curr.X == e->Bot.X &&
+ ePrev->Curr.Y == e->Bot.Y && op &&
+ ePrev->OutIdx >= 0 && ePrev->Curr.Y > ePrev->Top.Y &&
+ SlopesEqual(e->Curr, e->Top, ePrev->Curr, ePrev->Top, m_UseFullRange) &&
+ (e->WindDelta != 0) && (ePrev->WindDelta != 0))
+ {
+ OutPt* op2 = AddOutPt(ePrev, e->Bot);
+ AddJoin(op, op2, e->Top);
+ }
+ else if (eNext && eNext->Curr.X == e->Bot.X &&
+ eNext->Curr.Y == e->Bot.Y && op &&
+ eNext->OutIdx >= 0 && eNext->Curr.Y > eNext->Top.Y &&
+ SlopesEqual(e->Curr, e->Top, eNext->Curr, eNext->Top, m_UseFullRange) &&
+ (e->WindDelta != 0) && (eNext->WindDelta != 0))
+ {
+ OutPt* op2 = AddOutPt(eNext, e->Bot);
+ AddJoin(op, op2, e->Top);
+ }
+ }
+ e = e->NextInAEL;
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::FixupOutPolyline(OutRec &outrec)
+{
+ OutPt *pp = outrec.Pts;
+ OutPt *lastPP = pp->Prev;
+ while (pp != lastPP)
+ {
+ pp = pp->Next;
+ if (pp->Pt == pp->Prev->Pt)
+ {
+ if (pp == lastPP) lastPP = pp->Prev;
+ OutPt *tmpPP = pp->Prev;
+ tmpPP->Next = pp->Next;
+ pp->Next->Prev = tmpPP;
+ delete pp;
+ pp = tmpPP;
+ }
+ }
+
+ if (pp == pp->Prev)
+ {
+ DisposeOutPts(pp);
+ outrec.Pts = 0;
+ return;
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::FixupOutPolygon(OutRec &outrec)
+{
+ //FixupOutPolygon() - removes duplicate points and simplifies consecutive
+ //parallel edges by removing the middle vertex.
+ OutPt *lastOK = 0;
+ outrec.BottomPt = 0;
+ OutPt *pp = outrec.Pts;
+ bool preserveCol = m_PreserveCollinear || m_StrictSimple;
+
+ for (;;)
+ {
+ if (pp->Prev == pp || pp->Prev == pp->Next)
+ {
+ DisposeOutPts(pp);
+ outrec.Pts = 0;
+ return;
+ }
+
+ //test for duplicate points and collinear edges ...
+ if ((pp->Pt == pp->Next->Pt) || (pp->Pt == pp->Prev->Pt) ||
+ (SlopesEqual(pp->Prev->Pt, pp->Pt, pp->Next->Pt, m_UseFullRange) &&
+ (!preserveCol || !Pt2IsBetweenPt1AndPt3(pp->Prev->Pt, pp->Pt, pp->Next->Pt))))
+ {
+ lastOK = 0;
+ OutPt *tmp = pp;
+ pp->Prev->Next = pp->Next;
+ pp->Next->Prev = pp->Prev;
+ pp = pp->Prev;
+ delete tmp;
+ }
+ else if (pp == lastOK) break;
+ else
+ {
+ if (!lastOK) lastOK = pp;
+ pp = pp->Next;
+ }
+ }
+ outrec.Pts = pp;
+}
+//------------------------------------------------------------------------------
+
+int PointCount(OutPt *Pts)
+{
+ if (!Pts) return 0;
+ int result = 0;
+ OutPt* p = Pts;
+ do
+ {
+ result++;
+ p = p->Next;
+ }
+ while (p != Pts);
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::BuildResult(Paths &polys)
+{
+ polys.reserve(m_PolyOuts.size());
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ if (!m_PolyOuts[i]->Pts) continue;
+ Path pg;
+ OutPt* p = m_PolyOuts[i]->Pts->Prev;
+ int cnt = PointCount(p);
+ if (cnt < 2) continue;
+ pg.reserve(cnt);
+ for (int i = 0; i < cnt; ++i)
+ {
+ pg.push_back(p->Pt);
+ p = p->Prev;
+ }
+ polys.push_back(pg);
+ }
+}
+//------------------------------------------------------------------------------
+
+void Clipper::BuildResult2(PolyTree& polytree)
+{
+ polytree.Clear();
+ polytree.AllNodes.reserve(m_PolyOuts.size());
+ //add each output polygon/contour to polytree ...
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); i++)
+ {
+ OutRec* outRec = m_PolyOuts[i];
+ int cnt = PointCount(outRec->Pts);
+ if ((outRec->IsOpen && cnt < 2) || (!outRec->IsOpen && cnt < 3)) continue;
+ FixHoleLinkage(*outRec);
+ PolyNode* pn = new PolyNode();
+ //nb: polytree takes ownership of all the PolyNodes
+ polytree.AllNodes.push_back(pn);
+ outRec->PolyNd = pn;
+ pn->Parent = 0;
+ pn->Index = 0;
+ pn->Contour.reserve(cnt);
+ OutPt *op = outRec->Pts->Prev;
+ for (int j = 0; j < cnt; j++)
+ {
+ pn->Contour.push_back(op->Pt);
+ op = op->Prev;
+ }
+ }
+
+ //fixup PolyNode links etc ...
+ polytree.Childs.reserve(m_PolyOuts.size());
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); i++)
+ {
+ OutRec* outRec = m_PolyOuts[i];
+ if (!outRec->PolyNd) continue;
+ if (outRec->IsOpen)
+ {
+ outRec->PolyNd->m_IsOpen = true;
+ polytree.AddChild(*outRec->PolyNd);
+ }
+ else if (outRec->FirstLeft && outRec->FirstLeft->PolyNd)
+ outRec->FirstLeft->PolyNd->AddChild(*outRec->PolyNd);
+ else
+ polytree.AddChild(*outRec->PolyNd);
+ }
+}
+//------------------------------------------------------------------------------
+
+void SwapIntersectNodes(IntersectNode &int1, IntersectNode &int2)
+{
+ //just swap the contents (because fIntersectNodes is a single-linked-list)
+ IntersectNode inode = int1; //gets a copy of Int1
+ int1.Edge1 = int2.Edge1;
+ int1.Edge2 = int2.Edge2;
+ int1.Pt = int2.Pt;
+ int2.Edge1 = inode.Edge1;
+ int2.Edge2 = inode.Edge2;
+ int2.Pt = inode.Pt;
+}
+//------------------------------------------------------------------------------
+
+inline bool E2InsertsBeforeE1(TEdge &e1, TEdge &e2)
+{
+ if (e2.Curr.X == e1.Curr.X)
+ {
+ if (e2.Top.Y > e1.Top.Y)
+ return e2.Top.X < TopX(e1, e2.Top.Y);
+ else return e1.Top.X > TopX(e2, e1.Top.Y);
+ }
+ else return e2.Curr.X < e1.Curr.X;
+}
+//------------------------------------------------------------------------------
+
+bool GetOverlap(const cInt a1, const cInt a2, const cInt b1, const cInt b2,
+ cInt& Left, cInt& Right)
+{
+ if (a1 < a2)
+ {
+ if (b1 < b2) {Left = std::max(a1,b1); Right = std::min(a2,b2);}
+ else {Left = std::max(a1,b2); Right = std::min(a2,b1);}
+ }
+ else
+ {
+ if (b1 < b2) {Left = std::max(a2,b1); Right = std::min(a1,b2);}
+ else {Left = std::max(a2,b2); Right = std::min(a1,b1);}
+ }
+ return Left < Right;
+}
+//------------------------------------------------------------------------------
+
+inline void UpdateOutPtIdxs(OutRec& outrec)
+{
+ OutPt* op = outrec.Pts;
+ do
+ {
+ op->Idx = outrec.Idx;
+ op = op->Prev;
+ }
+ while(op != outrec.Pts);
+}
+//------------------------------------------------------------------------------
+
+void Clipper::InsertEdgeIntoAEL(TEdge *edge, TEdge* startEdge)
+{
+ if(!m_ActiveEdges)
+ {
+ edge->PrevInAEL = 0;
+ edge->NextInAEL = 0;
+ m_ActiveEdges = edge;
+ }
+ else if(!startEdge && E2InsertsBeforeE1(*m_ActiveEdges, *edge))
+ {
+ edge->PrevInAEL = 0;
+ edge->NextInAEL = m_ActiveEdges;
+ m_ActiveEdges->PrevInAEL = edge;
+ m_ActiveEdges = edge;
+ }
+ else
+ {
+ if(!startEdge) startEdge = m_ActiveEdges;
+ while(startEdge->NextInAEL &&
+ !E2InsertsBeforeE1(*startEdge->NextInAEL , *edge))
+ startEdge = startEdge->NextInAEL;
+ edge->NextInAEL = startEdge->NextInAEL;
+ if(startEdge->NextInAEL) startEdge->NextInAEL->PrevInAEL = edge;
+ edge->PrevInAEL = startEdge;
+ startEdge->NextInAEL = edge;
+ }
+}
+//----------------------------------------------------------------------
+
+OutPt* DupOutPt(OutPt* outPt, bool InsertAfter)
+{
+ OutPt* result = new OutPt;
+ result->Pt = outPt->Pt;
+ result->Idx = outPt->Idx;
+ if (InsertAfter)
+ {
+ result->Next = outPt->Next;
+ result->Prev = outPt;
+ outPt->Next->Prev = result;
+ outPt->Next = result;
+ }
+ else
+ {
+ result->Prev = outPt->Prev;
+ result->Next = outPt;
+ outPt->Prev->Next = result;
+ outPt->Prev = result;
+ }
+ return result;
+}
+//------------------------------------------------------------------------------
+
+bool JoinHorz(OutPt* op1, OutPt* op1b, OutPt* op2, OutPt* op2b,
+ const IntPoint Pt, bool DiscardLeft)
+{
+ Direction Dir1 = (op1->Pt.X > op1b->Pt.X ? dRightToLeft : dLeftToRight);
+ Direction Dir2 = (op2->Pt.X > op2b->Pt.X ? dRightToLeft : dLeftToRight);
+ if (Dir1 == Dir2) return false;
+
+ //When DiscardLeft, we want Op1b to be on the Left of Op1, otherwise we
+ //want Op1b to be on the Right. (And likewise with Op2 and Op2b.)
+ //So, to facilitate this while inserting Op1b and Op2b ...
+ //when DiscardLeft, make sure we're AT or RIGHT of Pt before adding Op1b,
+ //otherwise make sure we're AT or LEFT of Pt. (Likewise with Op2b.)
+ if (Dir1 == dLeftToRight)
+ {
+ while (op1->Next->Pt.X <= Pt.X &&
+ op1->Next->Pt.X >= op1->Pt.X && op1->Next->Pt.Y == Pt.Y)
+ op1 = op1->Next;
+ if (DiscardLeft && (op1->Pt.X != Pt.X)) op1 = op1->Next;
+ op1b = DupOutPt(op1, !DiscardLeft);
+ if (op1b->Pt != Pt)
+ {
+ op1 = op1b;
+ op1->Pt = Pt;
+ op1b = DupOutPt(op1, !DiscardLeft);
+ }
+ }
+ else
+ {
+ while (op1->Next->Pt.X >= Pt.X &&
+ op1->Next->Pt.X <= op1->Pt.X && op1->Next->Pt.Y == Pt.Y)
+ op1 = op1->Next;
+ if (!DiscardLeft && (op1->Pt.X != Pt.X)) op1 = op1->Next;
+ op1b = DupOutPt(op1, DiscardLeft);
+ if (op1b->Pt != Pt)
+ {
+ op1 = op1b;
+ op1->Pt = Pt;
+ op1b = DupOutPt(op1, DiscardLeft);
+ }
+ }
+
+ if (Dir2 == dLeftToRight)
+ {
+ while (op2->Next->Pt.X <= Pt.X &&
+ op2->Next->Pt.X >= op2->Pt.X && op2->Next->Pt.Y == Pt.Y)
+ op2 = op2->Next;
+ if (DiscardLeft && (op2->Pt.X != Pt.X)) op2 = op2->Next;
+ op2b = DupOutPt(op2, !DiscardLeft);
+ if (op2b->Pt != Pt)
+ {
+ op2 = op2b;
+ op2->Pt = Pt;
+ op2b = DupOutPt(op2, !DiscardLeft);
+ };
+ } else
+ {
+ while (op2->Next->Pt.X >= Pt.X &&
+ op2->Next->Pt.X <= op2->Pt.X && op2->Next->Pt.Y == Pt.Y)
+ op2 = op2->Next;
+ if (!DiscardLeft && (op2->Pt.X != Pt.X)) op2 = op2->Next;
+ op2b = DupOutPt(op2, DiscardLeft);
+ if (op2b->Pt != Pt)
+ {
+ op2 = op2b;
+ op2->Pt = Pt;
+ op2b = DupOutPt(op2, DiscardLeft);
+ };
+ };
+
+ if ((Dir1 == dLeftToRight) == DiscardLeft)
+ {
+ op1->Prev = op2;
+ op2->Next = op1;
+ op1b->Next = op2b;
+ op2b->Prev = op1b;
+ }
+ else
+ {
+ op1->Next = op2;
+ op2->Prev = op1;
+ op1b->Prev = op2b;
+ op2b->Next = op1b;
+ }
+ return true;
+}
+//------------------------------------------------------------------------------
+
+bool Clipper::JoinPoints(Join *j, OutRec* outRec1, OutRec* outRec2)
+{
+ OutPt *op1 = j->OutPt1, *op1b;
+ OutPt *op2 = j->OutPt2, *op2b;
+
+ //There are 3 kinds of joins for output polygons ...
+ //1. Horizontal joins where Join.OutPt1 & Join.OutPt2 are vertices anywhere
+ //along (horizontal) collinear edges (& Join.OffPt is on the same horizontal).
+ //2. Non-horizontal joins where Join.OutPt1 & Join.OutPt2 are at the same
+ //location at the Bottom of the overlapping segment (& Join.OffPt is above).
+ //3. StrictSimple joins where edges touch but are not collinear and where
+ //Join.OutPt1, Join.OutPt2 & Join.OffPt all share the same point.
+ bool isHorizontal = (j->OutPt1->Pt.Y == j->OffPt.Y);
+
+ if (isHorizontal && (j->OffPt == j->OutPt1->Pt) &&
+ (j->OffPt == j->OutPt2->Pt))
+ {
+ //Strictly Simple join ...
+ if (outRec1 != outRec2) return false;
+ op1b = j->OutPt1->Next;
+ while (op1b != op1 && (op1b->Pt == j->OffPt))
+ op1b = op1b->Next;
+ bool reverse1 = (op1b->Pt.Y > j->OffPt.Y);
+ op2b = j->OutPt2->Next;
+ while (op2b != op2 && (op2b->Pt == j->OffPt))
+ op2b = op2b->Next;
+ bool reverse2 = (op2b->Pt.Y > j->OffPt.Y);
+ if (reverse1 == reverse2) return false;
+ if (reverse1)
+ {
+ op1b = DupOutPt(op1, false);
+ op2b = DupOutPt(op2, true);
+ op1->Prev = op2;
+ op2->Next = op1;
+ op1b->Next = op2b;
+ op2b->Prev = op1b;
+ j->OutPt1 = op1;
+ j->OutPt2 = op1b;
+ return true;
+ } else
+ {
+ op1b = DupOutPt(op1, true);
+ op2b = DupOutPt(op2, false);
+ op1->Next = op2;
+ op2->Prev = op1;
+ op1b->Prev = op2b;
+ op2b->Next = op1b;
+ j->OutPt1 = op1;
+ j->OutPt2 = op1b;
+ return true;
+ }
+ }
+ else if (isHorizontal)
+ {
+ //treat horizontal joins differently to non-horizontal joins since with
+ //them we're not yet sure where the overlapping is. OutPt1.Pt & OutPt2.Pt
+ //may be anywhere along the horizontal edge.
+ op1b = op1;
+ while (op1->Prev->Pt.Y == op1->Pt.Y && op1->Prev != op1b && op1->Prev != op2)
+ op1 = op1->Prev;
+ while (op1b->Next->Pt.Y == op1b->Pt.Y && op1b->Next != op1 && op1b->Next != op2)
+ op1b = op1b->Next;
+ if (op1b->Next == op1 || op1b->Next == op2) return false; //a flat 'polygon'
+
+ op2b = op2;
+ while (op2->Prev->Pt.Y == op2->Pt.Y && op2->Prev != op2b && op2->Prev != op1b)
+ op2 = op2->Prev;
+ while (op2b->Next->Pt.Y == op2b->Pt.Y && op2b->Next != op2 && op2b->Next != op1)
+ op2b = op2b->Next;
+ if (op2b->Next == op2 || op2b->Next == op1) return false; //a flat 'polygon'
+
+ cInt Left, Right;
+ //Op1 --> Op1b & Op2 --> Op2b are the extremites of the horizontal edges
+ if (!GetOverlap(op1->Pt.X, op1b->Pt.X, op2->Pt.X, op2b->Pt.X, Left, Right))
+ return false;
+
+ //DiscardLeftSide: when overlapping edges are joined, a spike will created
+ //which needs to be cleaned up. However, we don't want Op1 or Op2 caught up
+ //on the discard Side as either may still be needed for other joins ...
+ IntPoint Pt;
+ bool DiscardLeftSide;
+ if (op1->Pt.X >= Left && op1->Pt.X <= Right)
+ {
+ Pt = op1->Pt; DiscardLeftSide = (op1->Pt.X > op1b->Pt.X);
+ }
+ else if (op2->Pt.X >= Left&& op2->Pt.X <= Right)
+ {
+ Pt = op2->Pt; DiscardLeftSide = (op2->Pt.X > op2b->Pt.X);
+ }
+ else if (op1b->Pt.X >= Left && op1b->Pt.X <= Right)
+ {
+ Pt = op1b->Pt; DiscardLeftSide = op1b->Pt.X > op1->Pt.X;
+ }
+ else
+ {
+ Pt = op2b->Pt; DiscardLeftSide = (op2b->Pt.X > op2->Pt.X);
+ }
+ j->OutPt1 = op1; j->OutPt2 = op2;
+ return JoinHorz(op1, op1b, op2, op2b, Pt, DiscardLeftSide);
+ } else
+ {
+ //nb: For non-horizontal joins ...
+ // 1. Jr.OutPt1.Pt.Y == Jr.OutPt2.Pt.Y
+ // 2. Jr.OutPt1.Pt > Jr.OffPt.Y
+
+ //make sure the polygons are correctly oriented ...
+ op1b = op1->Next;
+ while ((op1b->Pt == op1->Pt) && (op1b != op1)) op1b = op1b->Next;
+ bool Reverse1 = ((op1b->Pt.Y > op1->Pt.Y) ||
+ !SlopesEqual(op1->Pt, op1b->Pt, j->OffPt, m_UseFullRange));
+ if (Reverse1)
+ {
+ op1b = op1->Prev;
+ while ((op1b->Pt == op1->Pt) && (op1b != op1)) op1b = op1b->Prev;
+ if ((op1b->Pt.Y > op1->Pt.Y) ||
+ !SlopesEqual(op1->Pt, op1b->Pt, j->OffPt, m_UseFullRange)) return false;
+ };
+ op2b = op2->Next;
+ while ((op2b->Pt == op2->Pt) && (op2b != op2))op2b = op2b->Next;
+ bool Reverse2 = ((op2b->Pt.Y > op2->Pt.Y) ||
+ !SlopesEqual(op2->Pt, op2b->Pt, j->OffPt, m_UseFullRange));
+ if (Reverse2)
+ {
+ op2b = op2->Prev;
+ while ((op2b->Pt == op2->Pt) && (op2b != op2)) op2b = op2b->Prev;
+ if ((op2b->Pt.Y > op2->Pt.Y) ||
+ !SlopesEqual(op2->Pt, op2b->Pt, j->OffPt, m_UseFullRange)) return false;
+ }
+
+ if ((op1b == op1) || (op2b == op2) || (op1b == op2b) ||
+ ((outRec1 == outRec2) && (Reverse1 == Reverse2))) return false;
+
+ if (Reverse1)
+ {
+ op1b = DupOutPt(op1, false);
+ op2b = DupOutPt(op2, true);
+ op1->Prev = op2;
+ op2->Next = op1;
+ op1b->Next = op2b;
+ op2b->Prev = op1b;
+ j->OutPt1 = op1;
+ j->OutPt2 = op1b;
+ return true;
+ } else
+ {
+ op1b = DupOutPt(op1, true);
+ op2b = DupOutPt(op2, false);
+ op1->Next = op2;
+ op2->Prev = op1;
+ op1b->Prev = op2b;
+ op2b->Next = op1b;
+ j->OutPt1 = op1;
+ j->OutPt2 = op1b;
+ return true;
+ }
+ }
+}
+//----------------------------------------------------------------------
+
+static OutRec* ParseFirstLeft(OutRec* FirstLeft)
+{
+ while (FirstLeft && !FirstLeft->Pts)
+ FirstLeft = FirstLeft->FirstLeft;
+ return FirstLeft;
+}
+//------------------------------------------------------------------------------
+
+void Clipper::FixupFirstLefts1(OutRec* OldOutRec, OutRec* NewOutRec)
+{
+ //tests if NewOutRec contains the polygon before reassigning FirstLeft
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ OutRec* outRec = m_PolyOuts[i];
+ OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft);
+ if (outRec->Pts && firstLeft == OldOutRec)
+ {
+ if (Poly2ContainsPoly1(outRec->Pts, NewOutRec->Pts))
+ outRec->FirstLeft = NewOutRec;
+ }
+ }
+}
+//----------------------------------------------------------------------
+
+void Clipper::FixupFirstLefts2(OutRec* InnerOutRec, OutRec* OuterOutRec)
+{
+ //A polygon has split into two such that one is now the inner of the other.
+ //It's possible that these polygons now wrap around other polygons, so check
+ //every polygon that's also contained by OuterOutRec's FirstLeft container
+ //(including 0) to see if they've become inner to the new inner polygon ...
+ OutRec* orfl = OuterOutRec->FirstLeft;
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ OutRec* outRec = m_PolyOuts[i];
+
+ if (!outRec->Pts || outRec == OuterOutRec || outRec == InnerOutRec)
+ continue;
+ OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft);
+ if (firstLeft != orfl && firstLeft != InnerOutRec && firstLeft != OuterOutRec)
+ continue;
+ if (Poly2ContainsPoly1(outRec->Pts, InnerOutRec->Pts))
+ outRec->FirstLeft = InnerOutRec;
+ else if (Poly2ContainsPoly1(outRec->Pts, OuterOutRec->Pts))
+ outRec->FirstLeft = OuterOutRec;
+ else if (outRec->FirstLeft == InnerOutRec || outRec->FirstLeft == OuterOutRec)
+ outRec->FirstLeft = orfl;
+ }
+}
+//----------------------------------------------------------------------
+void Clipper::FixupFirstLefts3(OutRec* OldOutRec, OutRec* NewOutRec)
+{
+ //reassigns FirstLeft WITHOUT testing if NewOutRec contains the polygon
+ for (PolyOutList::size_type i = 0; i < m_PolyOuts.size(); ++i)
+ {
+ OutRec* outRec = m_PolyOuts[i];
+ OutRec* firstLeft = ParseFirstLeft(outRec->FirstLeft);
+ if (outRec->Pts && firstLeft == OldOutRec)
+ outRec->FirstLeft = NewOutRec;
+ }
+}
+//----------------------------------------------------------------------
+
+void Clipper::JoinCommonEdges()
+{
+ for (JoinList::size_type i = 0; i < m_Joins.size(); i++)
+ {
+ Join* join = m_Joins[i];
+
+ OutRec *outRec1 = GetOutRec(join->OutPt1->Idx);
+ OutRec *outRec2 = GetOutRec(join->OutPt2->Idx);
+
+ if (!outRec1->Pts || !outRec2->Pts) continue;
+ if (outRec1->IsOpen || outRec2->IsOpen) continue;
+
+ //get the polygon fragment with the correct hole state (FirstLeft)
+ //before calling JoinPoints() ...
+ OutRec *holeStateRec;
+ if (outRec1 == outRec2) holeStateRec = outRec1;
+ else if (OutRec1RightOfOutRec2(outRec1, outRec2)) holeStateRec = outRec2;
+ else if (OutRec1RightOfOutRec2(outRec2, outRec1)) holeStateRec = outRec1;
+ else holeStateRec = GetLowermostRec(outRec1, outRec2);
+
+ if (!JoinPoints(join, outRec1, outRec2)) continue;
+
+ if (outRec1 == outRec2)
+ {
+ //instead of joining two polygons, we've just created a new one by
+ //splitting one polygon into two.
+ outRec1->Pts = join->OutPt1;
+ outRec1->BottomPt = 0;
+ outRec2 = CreateOutRec();
+ outRec2->Pts = join->OutPt2;
+
+ //update all OutRec2.Pts Idx's ...
+ UpdateOutPtIdxs(*outRec2);
+
+ if (Poly2ContainsPoly1(outRec2->Pts, outRec1->Pts))
+ {
+ //outRec1 contains outRec2 ...
+ outRec2->IsHole = !outRec1->IsHole;
+ outRec2->FirstLeft = outRec1;
+
+ if (m_UsingPolyTree) FixupFirstLefts2(outRec2, outRec1);
+
+ if ((outRec2->IsHole ^ m_ReverseOutput) == (Area(*outRec2) > 0))
+ ReversePolyPtLinks(outRec2->Pts);
+
+ } else if (Poly2ContainsPoly1(outRec1->Pts, outRec2->Pts))
+ {
+ //outRec2 contains outRec1 ...
+ outRec2->IsHole = outRec1->IsHole;
+ outRec1->IsHole = !outRec2->IsHole;
+ outRec2->FirstLeft = outRec1->FirstLeft;
+ outRec1->FirstLeft = outRec2;
+
+ if (m_UsingPolyTree) FixupFirstLefts2(outRec1, outRec2);
+
+ if ((outRec1->IsHole ^ m_ReverseOutput) == (Area(*outRec1) > 0))
+ ReversePolyPtLinks(outRec1->Pts);
+ }
+ else
+ {
+ //the 2 polygons are completely separate ...
+ outRec2->IsHole = outRec1->IsHole;
+ outRec2->FirstLeft = outRec1->FirstLeft;
+
+ //fixup FirstLeft pointers that may need reassigning to OutRec2
+ if (m_UsingPolyTree) FixupFirstLefts1(outRec1, outRec2);
+ }
+
+ } else
+ {
+ //joined 2 polygons together ...
+
+ outRec2->Pts = 0;
+ outRec2->BottomPt = 0;
+ outRec2->Idx = outRec1->Idx;
+
+ outRec1->IsHole = holeStateRec->IsHole;
+ if (holeStateRec == outRec2)
+ outRec1->FirstLeft = outRec2->FirstLeft;
+ outRec2->FirstLeft = outRec1;
+
+ if (m_UsingPolyTree) FixupFirstLefts3(outRec2, outRec1);
+ }
+ }
+}
+
+//------------------------------------------------------------------------------
+// ClipperOffset support functions ...
+//------------------------------------------------------------------------------
+
+DoublePoint GetUnitNormal(const IntPoint &pt1, const IntPoint &pt2)
+{
+ if(pt2.X == pt1.X && pt2.Y == pt1.Y)
+ return DoublePoint(0, 0);
+
+ double Dx = (double)(pt2.X - pt1.X);
+ double dy = (double)(pt2.Y - pt1.Y);
+ double f = 1 *1.0/ std::sqrt( Dx*Dx + dy*dy );
+ Dx *= f;
+ dy *= f;
+ return DoublePoint(dy, -Dx);
+}
+
+//------------------------------------------------------------------------------
+// ClipperOffset class
+//------------------------------------------------------------------------------
+
+ClipperOffset::ClipperOffset(double miterLimit, double arcTolerance)
+{
+ this->MiterLimit = miterLimit;
+ this->ArcTolerance = arcTolerance;
+ m_lowest.X = -1;
+}
+//------------------------------------------------------------------------------
+
+ClipperOffset::~ClipperOffset()
+{
+ Clear();
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::Clear()
+{
+ for (int i = 0; i < m_polyNodes.ChildCount(); ++i)
+ delete m_polyNodes.Childs[i];
+ m_polyNodes.Childs.clear();
+ m_lowest.X = -1;
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::AddPath(const Path& path, JoinType joinType, EndType endType)
+{
+ int highI = (int)path.size() - 1;
+ if (highI < 0) return;
+ PolyNode* newNode = new PolyNode();
+ newNode->m_jointype = joinType;
+ newNode->m_endtype = endType;
+
+ //strip duplicate points from path and also get index to the lowest point ...
+ if (endType == etClosedLine || endType == etClosedPolygon)
+ while (highI > 0 && path[0] == path[highI]) highI--;
+ newNode->Contour.reserve(highI + 1);
+ newNode->Contour.push_back(path[0]);
+ int j = 0, k = 0;
+ for (int i = 1; i <= highI; i++)
+ if (newNode->Contour[j] != path[i])
+ {
+ j++;
+ newNode->Contour.push_back(path[i]);
+ if (path[i].Y > newNode->Contour[k].Y ||
+ (path[i].Y == newNode->Contour[k].Y &&
+ path[i].X < newNode->Contour[k].X)) k = j;
+ }
+ if (endType == etClosedPolygon && j < 2)
+ {
+ delete newNode;
+ return;
+ }
+ m_polyNodes.AddChild(*newNode);
+
+ //if this path's lowest pt is lower than all the others then update m_lowest
+ if (endType != etClosedPolygon) return;
+ if (m_lowest.X < 0)
+ m_lowest = IntPoint(m_polyNodes.ChildCount() - 1, k);
+ else
+ {
+ IntPoint ip = m_polyNodes.Childs[(int)m_lowest.X]->Contour[(int)m_lowest.Y];
+ if (newNode->Contour[k].Y > ip.Y ||
+ (newNode->Contour[k].Y == ip.Y &&
+ newNode->Contour[k].X < ip.X))
+ m_lowest = IntPoint(m_polyNodes.ChildCount() - 1, k);
+ }
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::AddPaths(const Paths& paths, JoinType joinType, EndType endType)
+{
+ for (Paths::size_type i = 0; i < paths.size(); ++i)
+ AddPath(paths[i], joinType, endType);
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::FixOrientations()
+{
+ //fixup orientations of all closed paths if the orientation of the
+ //closed path with the lowermost vertex is wrong ...
+ if (m_lowest.X >= 0 &&
+ !Orientation(m_polyNodes.Childs[(int)m_lowest.X]->Contour))
+ {
+ for (int i = 0; i < m_polyNodes.ChildCount(); ++i)
+ {
+ PolyNode& node = *m_polyNodes.Childs[i];
+ if (node.m_endtype == etClosedPolygon ||
+ (node.m_endtype == etClosedLine && Orientation(node.Contour)))
+ ReversePath(node.Contour);
+ }
+ } else
+ {
+ for (int i = 0; i < m_polyNodes.ChildCount(); ++i)
+ {
+ PolyNode& node = *m_polyNodes.Childs[i];
+ if (node.m_endtype == etClosedLine && !Orientation(node.Contour))
+ ReversePath(node.Contour);
+ }
+ }
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::Execute(Paths& solution, double delta)
+{
+ solution.clear();
+ FixOrientations();
+ DoOffset(delta);
+
+ //now clean up 'corners' ...
+ Clipper clpr;
+ clpr.AddPaths(m_destPolys, ptSubject, true);
+ if (delta > 0)
+ {
+ clpr.Execute(ctUnion, solution, pftPositive, pftPositive);
+ }
+ else
+ {
+ IntRect r = clpr.GetBounds();
+ Path outer(4);
+ outer[0] = IntPoint(r.left - 10, r.bottom + 10);
+ outer[1] = IntPoint(r.right + 10, r.bottom + 10);
+ outer[2] = IntPoint(r.right + 10, r.top - 10);
+ outer[3] = IntPoint(r.left - 10, r.top - 10);
+
+ clpr.AddPath(outer, ptSubject, true);
+ clpr.ReverseSolution(true);
+ clpr.Execute(ctUnion, solution, pftNegative, pftNegative);
+ if (solution.size() > 0) solution.erase(solution.begin());
+ }
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::Execute(PolyTree& solution, double delta)
+{
+ solution.Clear();
+ FixOrientations();
+ DoOffset(delta);
+
+ //now clean up 'corners' ...
+ Clipper clpr;
+ clpr.AddPaths(m_destPolys, ptSubject, true);
+ if (delta > 0)
+ {
+ clpr.Execute(ctUnion, solution, pftPositive, pftPositive);
+ }
+ else
+ {
+ IntRect r = clpr.GetBounds();
+ Path outer(4);
+ outer[0] = IntPoint(r.left - 10, r.bottom + 10);
+ outer[1] = IntPoint(r.right + 10, r.bottom + 10);
+ outer[2] = IntPoint(r.right + 10, r.top - 10);
+ outer[3] = IntPoint(r.left - 10, r.top - 10);
+
+ clpr.AddPath(outer, ptSubject, true);
+ clpr.ReverseSolution(true);
+ clpr.Execute(ctUnion, solution, pftNegative, pftNegative);
+ //remove the outer PolyNode rectangle ...
+ if (solution.ChildCount() == 1 && solution.Childs[0]->ChildCount() > 0)
+ {
+ PolyNode* outerNode = solution.Childs[0];
+ solution.Childs.reserve(outerNode->ChildCount());
+ solution.Childs[0] = outerNode->Childs[0];
+ solution.Childs[0]->Parent = outerNode->Parent;
+ for (int i = 1; i < outerNode->ChildCount(); ++i)
+ solution.AddChild(*outerNode->Childs[i]);
+ }
+ else
+ solution.Clear();
+ }
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::DoOffset(double delta)
+{
+ m_destPolys.clear();
+ m_delta = delta;
+
+ //if Zero offset, just copy any CLOSED polygons to m_p and return ...
+ if (NEAR_ZERO(delta))
+ {
+ m_destPolys.reserve(m_polyNodes.ChildCount());
+ for (int i = 0; i < m_polyNodes.ChildCount(); i++)
+ {
+ PolyNode& node = *m_polyNodes.Childs[i];
+ if (node.m_endtype == etClosedPolygon)
+ m_destPolys.push_back(node.Contour);
+ }
+ return;
+ }
+
+ //see offset_triginometry3.svg in the documentation folder ...
+ if (MiterLimit > 2) m_miterLim = 2/(MiterLimit * MiterLimit);
+ else m_miterLim = 0.5;
+
+ double y;
+ if (ArcTolerance <= 0.0) y = def_arc_tolerance;
+ else if (ArcTolerance > std::fabs(delta) * def_arc_tolerance)
+ y = std::fabs(delta) * def_arc_tolerance;
+ else y = ArcTolerance;
+ //see offset_triginometry2.svg in the documentation folder ...
+ double steps = pi / std::acos(1 - y / std::fabs(delta));
+ if (steps > std::fabs(delta) * pi)
+ steps = std::fabs(delta) * pi; //ie excessive precision check
+ m_sin = std::sin(two_pi / steps);
+ m_cos = std::cos(two_pi / steps);
+ m_StepsPerRad = steps / two_pi;
+ if (delta < 0.0) m_sin = -m_sin;
+
+ m_destPolys.reserve(m_polyNodes.ChildCount() * 2);
+ for (int i = 0; i < m_polyNodes.ChildCount(); i++)
+ {
+ PolyNode& node = *m_polyNodes.Childs[i];
+ m_srcPoly = node.Contour;
+
+ int len = (int)m_srcPoly.size();
+ if (len == 0 || (delta <= 0 && (len < 3 || node.m_endtype != etClosedPolygon)))
+ continue;
+
+ m_destPoly.clear();
+ if (len == 1)
+ {
+ if (node.m_jointype == jtRound)
+ {
+ double X = 1.0, Y = 0.0;
+ for (cInt j = 1; j <= steps; j++)
+ {
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[0].X + X * delta),
+ Round(m_srcPoly[0].Y + Y * delta)));
+ double X2 = X;
+ X = X * m_cos - m_sin * Y;
+ Y = X2 * m_sin + Y * m_cos;
+ }
+ }
+ else
+ {
+ double X = -1.0, Y = -1.0;
+ for (int j = 0; j < 4; ++j)
+ {
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[0].X + X * delta),
+ Round(m_srcPoly[0].Y + Y * delta)));
+ if (X < 0) X = 1;
+ else if (Y < 0) Y = 1;
+ else X = -1;
+ }
+ }
+ m_destPolys.push_back(m_destPoly);
+ continue;
+ }
+ //build m_normals ...
+ m_normals.clear();
+ m_normals.reserve(len);
+ for (int j = 0; j < len - 1; ++j)
+ m_normals.push_back(GetUnitNormal(m_srcPoly[j], m_srcPoly[j + 1]));
+ if (node.m_endtype == etClosedLine || node.m_endtype == etClosedPolygon)
+ m_normals.push_back(GetUnitNormal(m_srcPoly[len - 1], m_srcPoly[0]));
+ else
+ m_normals.push_back(DoublePoint(m_normals[len - 2]));
+
+ if (node.m_endtype == etClosedPolygon)
+ {
+ int k = len - 1;
+ for (int j = 0; j < len; ++j)
+ OffsetPoint(j, k, node.m_jointype);
+ m_destPolys.push_back(m_destPoly);
+ }
+ else if (node.m_endtype == etClosedLine)
+ {
+ int k = len - 1;
+ for (int j = 0; j < len; ++j)
+ OffsetPoint(j, k, node.m_jointype);
+ m_destPolys.push_back(m_destPoly);
+ m_destPoly.clear();
+ //re-build m_normals ...
+ DoublePoint n = m_normals[len -1];
+ for (int j = len - 1; j > 0; j--)
+ m_normals[j] = DoublePoint(-m_normals[j - 1].X, -m_normals[j - 1].Y);
+ m_normals[0] = DoublePoint(-n.X, -n.Y);
+ k = 0;
+ for (int j = len - 1; j >= 0; j--)
+ OffsetPoint(j, k, node.m_jointype);
+ m_destPolys.push_back(m_destPoly);
+ }
+ else
+ {
+ int k = 0;
+ for (int j = 1; j < len - 1; ++j)
+ OffsetPoint(j, k, node.m_jointype);
+
+ IntPoint pt1;
+ if (node.m_endtype == etOpenButt)
+ {
+ int j = len - 1;
+ pt1 = IntPoint((cInt)Round(m_srcPoly[j].X + m_normals[j].X *
+ delta), (cInt)Round(m_srcPoly[j].Y + m_normals[j].Y * delta));
+ m_destPoly.push_back(pt1);
+ pt1 = IntPoint((cInt)Round(m_srcPoly[j].X - m_normals[j].X *
+ delta), (cInt)Round(m_srcPoly[j].Y - m_normals[j].Y * delta));
+ m_destPoly.push_back(pt1);
+ }
+ else
+ {
+ int j = len - 1;
+ k = len - 2;
+ m_sinA = 0;
+ m_normals[j] = DoublePoint(-m_normals[j].X, -m_normals[j].Y);
+ if (node.m_endtype == etOpenSquare)
+ DoSquare(j, k);
+ else
+ DoRound(j, k);
+ }
+
+ //re-build m_normals ...
+ for (int j = len - 1; j > 0; j--)
+ m_normals[j] = DoublePoint(-m_normals[j - 1].X, -m_normals[j - 1].Y);
+ m_normals[0] = DoublePoint(-m_normals[1].X, -m_normals[1].Y);
+
+ k = len - 1;
+ for (int j = k - 1; j > 0; --j) OffsetPoint(j, k, node.m_jointype);
+
+ if (node.m_endtype == etOpenButt)
+ {
+ pt1 = IntPoint((cInt)Round(m_srcPoly[0].X - m_normals[0].X * delta),
+ (cInt)Round(m_srcPoly[0].Y - m_normals[0].Y * delta));
+ m_destPoly.push_back(pt1);
+ pt1 = IntPoint((cInt)Round(m_srcPoly[0].X + m_normals[0].X * delta),
+ (cInt)Round(m_srcPoly[0].Y + m_normals[0].Y * delta));
+ m_destPoly.push_back(pt1);
+ }
+ else
+ {
+ k = 1;
+ m_sinA = 0;
+ if (node.m_endtype == etOpenSquare)
+ DoSquare(0, 1);
+ else
+ DoRound(0, 1);
+ }
+ m_destPolys.push_back(m_destPoly);
+ }
+ }
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::OffsetPoint(int j, int& k, JoinType jointype)
+{
+ //cross product ...
+ m_sinA = (m_normals[k].X * m_normals[j].Y - m_normals[j].X * m_normals[k].Y);
+ if (std::fabs(m_sinA * m_delta) < 1.0)
+ {
+ //dot product ...
+ double cosA = (m_normals[k].X * m_normals[j].X + m_normals[j].Y * m_normals[k].Y );
+ if (cosA > 0) // angle => 0 degrees
+ {
+ m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[k].X * m_delta),
+ Round(m_srcPoly[j].Y + m_normals[k].Y * m_delta)));
+ return;
+ }
+ //else angle => 180 degrees
+ }
+ else if (m_sinA > 1.0) m_sinA = 1.0;
+ else if (m_sinA < -1.0) m_sinA = -1.0;
+
+ if (m_sinA * m_delta < 0)
+ {
+ m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[k].X * m_delta),
+ Round(m_srcPoly[j].Y + m_normals[k].Y * m_delta)));
+ m_destPoly.push_back(m_srcPoly[j]);
+ m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + m_normals[j].X * m_delta),
+ Round(m_srcPoly[j].Y + m_normals[j].Y * m_delta)));
+ }
+ else
+ switch (jointype)
+ {
+ case jtMiter:
+ {
+ double r = 1 + (m_normals[j].X * m_normals[k].X +
+ m_normals[j].Y * m_normals[k].Y);
+ if (r >= m_miterLim) DoMiter(j, k, r); else DoSquare(j, k);
+ break;
+ }
+ case jtSquare: DoSquare(j, k); break;
+ case jtRound: DoRound(j, k); break;
+ }
+ k = j;
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::DoSquare(int j, int k)
+{
+ double dx = std::tan(std::atan2(m_sinA,
+ m_normals[k].X * m_normals[j].X + m_normals[k].Y * m_normals[j].Y) / 4);
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[j].X + m_delta * (m_normals[k].X - m_normals[k].Y * dx)),
+ Round(m_srcPoly[j].Y + m_delta * (m_normals[k].Y + m_normals[k].X * dx))));
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[j].X + m_delta * (m_normals[j].X + m_normals[j].Y * dx)),
+ Round(m_srcPoly[j].Y + m_delta * (m_normals[j].Y - m_normals[j].X * dx))));
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::DoMiter(int j, int k, double r)
+{
+ double q = m_delta / r;
+ m_destPoly.push_back(IntPoint(Round(m_srcPoly[j].X + (m_normals[k].X + m_normals[j].X) * q),
+ Round(m_srcPoly[j].Y + (m_normals[k].Y + m_normals[j].Y) * q)));
+}
+//------------------------------------------------------------------------------
+
+void ClipperOffset::DoRound(int j, int k)
+{
+ double a = std::atan2(m_sinA,
+ m_normals[k].X * m_normals[j].X + m_normals[k].Y * m_normals[j].Y);
+ int steps = std::max((int)Round(m_StepsPerRad * std::fabs(a)), 1);
+
+ double X = m_normals[k].X, Y = m_normals[k].Y, X2;
+ for (int i = 0; i < steps; ++i)
+ {
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[j].X + X * m_delta),
+ Round(m_srcPoly[j].Y + Y * m_delta)));
+ X2 = X;
+ X = X * m_cos - m_sin * Y;
+ Y = X2 * m_sin + Y * m_cos;
+ }
+ m_destPoly.push_back(IntPoint(
+ Round(m_srcPoly[j].X + m_normals[j].X * m_delta),
+ Round(m_srcPoly[j].Y + m_normals[j].Y * m_delta)));
+}
+
+//------------------------------------------------------------------------------
+// Miscellaneous public functions
+//------------------------------------------------------------------------------
+
+void Clipper::DoSimplePolygons()
+{
+ PolyOutList::size_type i = 0;
+ while (i < m_PolyOuts.size())
+ {
+ OutRec* outrec = m_PolyOuts[i++];
+ OutPt* op = outrec->Pts;
+ if (!op || outrec->IsOpen) continue;
+ do //for each Pt in Polygon until duplicate found do ...
+ {
+ OutPt* op2 = op->Next;
+ while (op2 != outrec->Pts)
+ {
+ if ((op->Pt == op2->Pt) && op2->Next != op && op2->Prev != op)
+ {
+ //split the polygon into two ...
+ OutPt* op3 = op->Prev;
+ OutPt* op4 = op2->Prev;
+ op->Prev = op4;
+ op4->Next = op;
+ op2->Prev = op3;
+ op3->Next = op2;
+
+ outrec->Pts = op;
+ OutRec* outrec2 = CreateOutRec();
+ outrec2->Pts = op2;
+ UpdateOutPtIdxs(*outrec2);
+ if (Poly2ContainsPoly1(outrec2->Pts, outrec->Pts))
+ {
+ //OutRec2 is contained by OutRec1 ...
+ outrec2->IsHole = !outrec->IsHole;
+ outrec2->FirstLeft = outrec;
+ if (m_UsingPolyTree) FixupFirstLefts2(outrec2, outrec);
+ }
+ else
+ if (Poly2ContainsPoly1(outrec->Pts, outrec2->Pts))
+ {
+ //OutRec1 is contained by OutRec2 ...
+ outrec2->IsHole = outrec->IsHole;
+ outrec->IsHole = !outrec2->IsHole;
+ outrec2->FirstLeft = outrec->FirstLeft;
+ outrec->FirstLeft = outrec2;
+ if (m_UsingPolyTree) FixupFirstLefts2(outrec, outrec2);
+ }
+ else
+ {
+ //the 2 polygons are separate ...
+ outrec2->IsHole = outrec->IsHole;
+ outrec2->FirstLeft = outrec->FirstLeft;
+ if (m_UsingPolyTree) FixupFirstLefts1(outrec, outrec2);
+ }
+ op2 = op; //ie get ready for the Next iteration
+ }
+ op2 = op2->Next;
+ }
+ op = op->Next;
+ }
+ while (op != outrec->Pts);
+ }
+}
+//------------------------------------------------------------------------------
+
+void ReversePath(Path& p)
+{
+ std::reverse(p.begin(), p.end());
+}
+//------------------------------------------------------------------------------
+
+void ReversePaths(Paths& p)
+{
+ for (Paths::size_type i = 0; i < p.size(); ++i)
+ ReversePath(p[i]);
+}
+//------------------------------------------------------------------------------
+
+void SimplifyPolygon(const Path &in_poly, Paths &out_polys, PolyFillType fillType)
+{
+ Clipper c;
+ c.StrictlySimple(true);
+ c.AddPath(in_poly, ptSubject, true);
+ c.Execute(ctUnion, out_polys, fillType, fillType);
+}
+//------------------------------------------------------------------------------
+
+void SimplifyPolygons(const Paths &in_polys, Paths &out_polys, PolyFillType fillType)
+{
+ Clipper c;
+ c.StrictlySimple(true);
+ c.AddPaths(in_polys, ptSubject, true);
+ c.Execute(ctUnion, out_polys, fillType, fillType);
+}
+//------------------------------------------------------------------------------
+
+void SimplifyPolygons(Paths &polys, PolyFillType fillType)
+{
+ SimplifyPolygons(polys, polys, fillType);
+}
+//------------------------------------------------------------------------------
+
+inline double DistanceSqrd(const IntPoint& pt1, const IntPoint& pt2)
+{
+ double Dx = ((double)pt1.X - pt2.X);
+ double dy = ((double)pt1.Y - pt2.Y);
+ return (Dx*Dx + dy*dy);
+}
+//------------------------------------------------------------------------------
+
+double DistanceFromLineSqrd(
+ const IntPoint& pt, const IntPoint& ln1, const IntPoint& ln2)
+{
+ //The equation of a line in general form (Ax + By + C = 0)
+ //given 2 points (x�,y�) & (x�,y�) is ...
+ //(y� - y�)x + (x� - x�)y + (y� - y�)x� - (x� - x�)y� = 0
+ //A = (y� - y�); B = (x� - x�); C = (y� - y�)x� - (x� - x�)y�
+ //perpendicular distance of point (x�,y�) = (Ax� + By� + C)/Sqrt(A� + B�)
+ //see http://en.wikipedia.org/wiki/Perpendicular_distance
+ double A = double(ln1.Y - ln2.Y);
+ double B = double(ln2.X - ln1.X);
+ double C = A * ln1.X + B * ln1.Y;
+ C = A * pt.X + B * pt.Y - C;
+ return (C * C) / (A * A + B * B);
+}
+//---------------------------------------------------------------------------
+
+bool SlopesNearCollinear(const IntPoint& pt1,
+ const IntPoint& pt2, const IntPoint& pt3, double distSqrd)
+{
+ //this function is more accurate when the point that's geometrically
+ //between the other 2 points is the one that's tested for distance.
+ //ie makes it more likely to pick up 'spikes' ...
+ if (Abs(pt1.X - pt2.X) > Abs(pt1.Y - pt2.Y))
+ {
+ if ((pt1.X > pt2.X) == (pt1.X < pt3.X))
+ return DistanceFromLineSqrd(pt1, pt2, pt3) < distSqrd;
+ else if ((pt2.X > pt1.X) == (pt2.X < pt3.X))
+ return DistanceFromLineSqrd(pt2, pt1, pt3) < distSqrd;
+ else
+ return DistanceFromLineSqrd(pt3, pt1, pt2) < distSqrd;
+ }
+ else
+ {
+ if ((pt1.Y > pt2.Y) == (pt1.Y < pt3.Y))
+ return DistanceFromLineSqrd(pt1, pt2, pt3) < distSqrd;
+ else if ((pt2.Y > pt1.Y) == (pt2.Y < pt3.Y))
+ return DistanceFromLineSqrd(pt2, pt1, pt3) < distSqrd;
+ else
+ return DistanceFromLineSqrd(pt3, pt1, pt2) < distSqrd;
+ }
+}
+//------------------------------------------------------------------------------
+
+bool PointsAreClose(IntPoint pt1, IntPoint pt2, double distSqrd)
+{
+ double Dx = (double)pt1.X - pt2.X;
+ double dy = (double)pt1.Y - pt2.Y;
+ return ((Dx * Dx) + (dy * dy) <= distSqrd);
+}
+//------------------------------------------------------------------------------
+
+OutPt* ExcludeOp(OutPt* op)
+{
+ OutPt* result = op->Prev;
+ result->Next = op->Next;
+ op->Next->Prev = result;
+ result->Idx = 0;
+ return result;
+}
+//------------------------------------------------------------------------------
+
+void CleanPolygon(const Path& in_poly, Path& out_poly, double distance)
+{
+ //distance = proximity in units/pixels below which vertices
+ //will be stripped. Default ~= sqrt(2).
+
+ size_t size = in_poly.size();
+
+ if (size == 0)
+ {
+ out_poly.clear();
+ return;
+ }
+
+ OutPt* outPts = new OutPt[size];
+ for (size_t i = 0; i < size; ++i)
+ {
+ outPts[i].Pt = in_poly[i];
+ outPts[i].Next = &outPts[(i + 1) % size];
+ outPts[i].Next->Prev = &outPts[i];
+ outPts[i].Idx = 0;
+ }
+
+ double distSqrd = distance * distance;
+ OutPt* op = &outPts[0];
+ while (op->Idx == 0 && op->Next != op->Prev)
+ {
+ if (PointsAreClose(op->Pt, op->Prev->Pt, distSqrd))
+ {
+ op = ExcludeOp(op);
+ size--;
+ }
+ else if (PointsAreClose(op->Prev->Pt, op->Next->Pt, distSqrd))
+ {
+ ExcludeOp(op->Next);
+ op = ExcludeOp(op);
+ size -= 2;
+ }
+ else if (SlopesNearCollinear(op->Prev->Pt, op->Pt, op->Next->Pt, distSqrd))
+ {
+ op = ExcludeOp(op);
+ size--;
+ }
+ else
+ {
+ op->Idx = 1;
+ op = op->Next;
+ }
+ }
+
+ if (size < 3) size = 0;
+ out_poly.resize(size);
+ for (size_t i = 0; i < size; ++i)
+ {
+ out_poly[i] = op->Pt;
+ op = op->Next;
+ }
+ delete [] outPts;
+}
+//------------------------------------------------------------------------------
+
+void CleanPolygon(Path& poly, double distance)
+{
+ CleanPolygon(poly, poly, distance);
+}
+//------------------------------------------------------------------------------
+
+void CleanPolygons(const Paths& in_polys, Paths& out_polys, double distance)
+{
+ out_polys.resize(in_polys.size());
+ for (Paths::size_type i = 0; i < in_polys.size(); ++i)
+ CleanPolygon(in_polys[i], out_polys[i], distance);
+}
+//------------------------------------------------------------------------------
+
+void CleanPolygons(Paths& polys, double distance)
+{
+ CleanPolygons(polys, polys, distance);
+}
+//------------------------------------------------------------------------------
+
+void Minkowski(const Path& poly, const Path& path,
+ Paths& solution, bool isSum, bool isClosed)
+{
+ int delta = (isClosed ? 1 : 0);
+ size_t polyCnt = poly.size();
+ size_t pathCnt = path.size();
+ Paths pp;
+ pp.reserve(pathCnt);
+ if (isSum)
+ for (size_t i = 0; i < pathCnt; ++i)
+ {
+ Path p;
+ p.reserve(polyCnt);
+ for (size_t j = 0; j < poly.size(); ++j)
+ p.push_back(IntPoint(path[i].X + poly[j].X, path[i].Y + poly[j].Y));
+ pp.push_back(p);
+ }
+ else
+ for (size_t i = 0; i < pathCnt; ++i)
+ {
+ Path p;
+ p.reserve(polyCnt);
+ for (size_t j = 0; j < poly.size(); ++j)
+ p.push_back(IntPoint(path[i].X - poly[j].X, path[i].Y - poly[j].Y));
+ pp.push_back(p);
+ }
+
+ solution.clear();
+ solution.reserve((pathCnt + delta) * (polyCnt + 1));
+ for (size_t i = 0; i < pathCnt - 1 + delta; ++i)
+ for (size_t j = 0; j < polyCnt; ++j)
+ {
+ Path quad;
+ quad.reserve(4);
+ quad.push_back(pp[i % pathCnt][j % polyCnt]);
+ quad.push_back(pp[(i + 1) % pathCnt][j % polyCnt]);
+ quad.push_back(pp[(i + 1) % pathCnt][(j + 1) % polyCnt]);
+ quad.push_back(pp[i % pathCnt][(j + 1) % polyCnt]);
+ if (!Orientation(quad)) ReversePath(quad);
+ solution.push_back(quad);
+ }
+}
+//------------------------------------------------------------------------------
+
+void MinkowskiSum(const Path& pattern, const Path& path, Paths& solution, bool pathIsClosed)
+{
+ Minkowski(pattern, path, solution, true, pathIsClosed);
+ Clipper c;
+ c.AddPaths(solution, ptSubject, true);
+ c.Execute(ctUnion, solution, pftNonZero, pftNonZero);
+}
+//------------------------------------------------------------------------------
+
+void TranslatePath(const Path& input, Path& output, const IntPoint delta)
+{
+ //precondition: input != output
+ output.resize(input.size());
+ for (size_t i = 0; i < input.size(); ++i)
+ output[i] = IntPoint(input[i].X + delta.X, input[i].Y + delta.Y);
+}
+//------------------------------------------------------------------------------
+
+void MinkowskiSum(const Path& pattern, const Paths& paths, Paths& solution, bool pathIsClosed)
+{
+ Clipper c;
+ for (size_t i = 0; i < paths.size(); ++i)
+ {
+ Paths tmp;
+ Minkowski(pattern, paths[i], tmp, true, pathIsClosed);
+ c.AddPaths(tmp, ptSubject, true);
+ if (pathIsClosed)
+ {
+ Path tmp2;
+ TranslatePath(paths[i], tmp2, pattern[0]);
+ c.AddPath(tmp2, ptClip, true);
+ }
+ }
+ c.Execute(ctUnion, solution, pftNonZero, pftNonZero);
+}
+//------------------------------------------------------------------------------
+
+void MinkowskiDiff(const Path& poly1, const Path& poly2, Paths& solution)
+{
+ Minkowski(poly1, poly2, solution, false, true);
+ Clipper c;
+ c.AddPaths(solution, ptSubject, true);
+ c.Execute(ctUnion, solution, pftNonZero, pftNonZero);
+}
+//------------------------------------------------------------------------------
+
+enum NodeType {ntAny, ntOpen, ntClosed};
+
+void AddPolyNodeToPaths(const PolyNode& polynode, NodeType nodetype, Paths& paths)
+{
+ bool match = true;
+ if (nodetype == ntClosed) match = !polynode.IsOpen();
+ else if (nodetype == ntOpen) return;
+
+ if (!polynode.Contour.empty() && match)
+ paths.push_back(polynode.Contour);
+ for (int i = 0; i < polynode.ChildCount(); ++i)
+ AddPolyNodeToPaths(*polynode.Childs[i], nodetype, paths);
+}
+//------------------------------------------------------------------------------
+
+void PolyTreeToPaths(const PolyTree& polytree, Paths& paths)
+{
+ paths.resize(0);
+ paths.reserve(polytree.Total());
+ AddPolyNodeToPaths(polytree, ntAny, paths);
+}
+//------------------------------------------------------------------------------
+
+void ClosedPathsFromPolyTree(const PolyTree& polytree, Paths& paths)
+{
+ paths.resize(0);
+ paths.reserve(polytree.Total());
+ AddPolyNodeToPaths(polytree, ntClosed, paths);
+}
+//------------------------------------------------------------------------------
+
+void OpenPathsFromPolyTree(PolyTree& polytree, Paths& paths)
+{
+ paths.resize(0);
+ paths.reserve(polytree.Total());
+ //Open paths are top level only, so ...
+ for (int i = 0; i < polytree.ChildCount(); ++i)
+ if (polytree.Childs[i]->IsOpen())
+ paths.push_back(polytree.Childs[i]->Contour);
+}
+//------------------------------------------------------------------------------
+
+std::ostream& operator <<(std::ostream &s, const IntPoint &p)
+{
+ s << "(" << p.X << "," << p.Y << ")";
+ return s;
+}
+//------------------------------------------------------------------------------
+
+std::ostream& operator <<(std::ostream &s, const Path &p)
+{
+ if (p.empty()) return s;
+ Path::size_type last = p.size() -1;
+ for (Path::size_type i = 0; i < last; i++)
+ s << "(" << p[i].X << "," << p[i].Y << "), ";
+ s << "(" << p[last].X << "," << p[last].Y << ")\n";
+ return s;
+}
+//------------------------------------------------------------------------------
+
+std::ostream& operator <<(std::ostream &s, const Paths &p)
+{
+ for (Paths::size_type i = 0; i < p.size(); i++)
+ s << p[i];
+ s << "\n";
+ return s;
+}
+//------------------------------------------------------------------------------
+
+} //ClipperLib namespace
diff --git a/3rdparty/clipper/clipper.hpp b/3rdparty/clipper/clipper.hpp
new file mode 100644
index 0000000..df1f813
--- /dev/null
+++ b/3rdparty/clipper/clipper.hpp
@@ -0,0 +1,406 @@
+/*******************************************************************************
+* *
+* Author : Angus Johnson *
+* Version : 6.4.2 *
+* Date : 27 February 2017 *
+* Website : http://www.angusj.com *
+* Copyright : Angus Johnson 2010-2017 *
+* *
+* License: *
+* Use, modification & distribution is subject to Boost Software License Ver 1. *
+* http://www.boost.org/LICENSE_1_0.txt *
+* *
+* Attributions: *
+* The code in this library is an extension of Bala Vatti's clipping algorithm: *
+* "A generic solution to polygon clipping" *
+* Communications of the ACM, Vol 35, Issue 7 (July 1992) pp 56-63. *
+* http://portal.acm.org/citation.cfm?id=129906 *
+* *
+* Computer graphics and geometric modeling: implementation and algorithms *
+* By Max K. Agoston *
+* Springer; 1 edition (January 4, 2005) *
+* http://books.google.com/books?q=vatti+clipping+agoston *
+* *
+* See also: *
+* "Polygon Offsetting by Computing Winding Numbers" *
+* Paper no. DETC2005-85513 pp. 565-575 *
+* ASME 2005 International Design Engineering Technical Conferences *
+* and Computers and Information in Engineering Conference (IDETC/CIE2005) *
+* September 24-28, 2005 , Long Beach, California, USA *
+* http://www.me.berkeley.edu/~mcmains/pubs/DAC05OffsetPolygon.pdf *
+* *
+*******************************************************************************/
+
+#ifndef clipper_hpp
+#define clipper_hpp
+
+#define CLIPPER_VERSION "6.4.2"
+
+//use_int32: When enabled 32bit ints are used instead of 64bit ints. This
+//improve performance but coordinate values are limited to the range +/- 46340
+//#define use_int32
+
+//use_xyz: adds a Z member to IntPoint. Adds a minor cost to perfomance.
+//#define use_xyz
+
+//use_lines: Enables line clipping. Adds a very minor cost to performance.
+#define use_lines
+
+//use_deprecated: Enables temporary support for the obsolete functions
+//#define use_deprecated
+
+#include <vector>
+#include <list>
+#include <set>
+#include <stdexcept>
+#include <cstring>
+#include <cstdlib>
+#include <ostream>
+#include <functional>
+#include <queue>
+
+namespace ClipperLib {
+
+enum ClipType { ctIntersection, ctUnion, ctDifference, ctXor };
+enum PolyType { ptSubject, ptClip };
+//By far the most widely used winding rules for polygon filling are
+//EvenOdd & NonZero (GDI, GDI+, XLib, OpenGL, Cairo, AGG, Quartz, SVG, Gr32)
+//Others rules include Positive, Negative and ABS_GTR_EQ_TWO (only in OpenGL)
+//see http://glprogramming.com/red/chapter11.html
+enum PolyFillType { pftEvenOdd, pftNonZero, pftPositive, pftNegative };
+
+#ifdef use_int32
+ typedef int cInt;
+ static cInt const loRange = 0x7FFF;
+ static cInt const hiRange = 0x7FFF;
+#else
+ typedef signed long long cInt;
+ static cInt const loRange = 0x3FFFFFFF;
+ static cInt const hiRange = 0x3FFFFFFFFFFFFFFFLL;
+ typedef signed long long long64; //used by Int128 class
+ typedef unsigned long long ulong64;
+
+#endif
+
+struct IntPoint {
+ cInt X;
+ cInt Y;
+#ifdef use_xyz
+ cInt Z;
+ IntPoint(cInt x = 0, cInt y = 0, cInt z = 0): X(x), Y(y), Z(z) {};
+#else
+ IntPoint(cInt x = 0, cInt y = 0): X(x), Y(y) {};
+#endif
+
+ friend inline bool operator== (const IntPoint& a, const IntPoint& b)
+ {
+ return a.X == b.X && a.Y == b.Y;
+ }
+ friend inline bool operator!= (const IntPoint& a, const IntPoint& b)
+ {
+ return a.X != b.X || a.Y != b.Y;
+ }
+};
+//------------------------------------------------------------------------------
+
+typedef std::vector< IntPoint > Path;
+typedef std::vector< Path > Paths;
+
+inline Path& operator <<(Path& poly, const IntPoint& p) {poly.push_back(p); return poly;}
+inline Paths& operator <<(Paths& polys, const Path& p) {polys.push_back(p); return polys;}
+
+std::ostream& operator <<(std::ostream &s, const IntPoint &p);
+std::ostream& operator <<(std::ostream &s, const Path &p);
+std::ostream& operator <<(std::ostream &s, const Paths &p);
+
+struct DoublePoint
+{
+ double X;
+ double Y;
+ DoublePoint(double x = 0, double y = 0) : X(x), Y(y) {}
+ DoublePoint(IntPoint ip) : X((double)ip.X), Y((double)ip.Y) {}
+};
+//------------------------------------------------------------------------------
+
+#ifdef use_xyz
+typedef void (*ZFillCallback)(IntPoint& e1bot, IntPoint& e1top, IntPoint& e2bot, IntPoint& e2top, IntPoint& pt);
+#endif
+
+enum InitOptions {ioReverseSolution = 1, ioStrictlySimple = 2, ioPreserveCollinear = 4};
+enum JoinType {jtSquare, jtRound, jtMiter};
+enum EndType {etClosedPolygon, etClosedLine, etOpenButt, etOpenSquare, etOpenRound};
+
+class PolyNode;
+typedef std::vector< PolyNode* > PolyNodes;
+
+class PolyNode
+{
+public:
+ PolyNode();
+ virtual ~PolyNode(){};
+ Path Contour;
+ PolyNodes Childs;
+ PolyNode* Parent;
+ PolyNode* GetNext() const;
+ bool IsHole() const;
+ bool IsOpen() const;
+ int ChildCount() const;
+private:
+ //PolyNode& operator =(PolyNode& other);
+ unsigned Index; //node index in Parent.Childs
+ bool m_IsOpen;
+ JoinType m_jointype;
+ EndType m_endtype;
+ PolyNode* GetNextSiblingUp() const;
+ void AddChild(PolyNode& child);
+ friend class Clipper; //to access Index
+ friend class ClipperOffset;
+};
+
+class PolyTree: public PolyNode
+{
+public:
+ ~PolyTree(){ Clear(); };
+ PolyNode* GetFirst() const;
+ void Clear();
+ int Total() const;
+private:
+ //PolyTree& operator =(PolyTree& other);
+ PolyNodes AllNodes;
+ friend class Clipper; //to access AllNodes
+};
+
+bool Orientation(const Path &poly);
+double Area(const Path &poly);
+int PointInPolygon(const IntPoint &pt, const Path &path);
+
+void SimplifyPolygon(const Path &in_poly, Paths &out_polys, PolyFillType fillType = pftEvenOdd);
+void SimplifyPolygons(const Paths &in_polys, Paths &out_polys, PolyFillType fillType = pftEvenOdd);
+void SimplifyPolygons(Paths &polys, PolyFillType fillType = pftEvenOdd);
+
+void CleanPolygon(const Path& in_poly, Path& out_poly, double distance = 1.415);
+void CleanPolygon(Path& poly, double distance = 1.415);
+void CleanPolygons(const Paths& in_polys, Paths& out_polys, double distance = 1.415);
+void CleanPolygons(Paths& polys, double distance = 1.415);
+
+void MinkowskiSum(const Path& pattern, const Path& path, Paths& solution, bool pathIsClosed);
+void MinkowskiSum(const Path& pattern, const Paths& paths, Paths& solution, bool pathIsClosed);
+void MinkowskiDiff(const Path& poly1, const Path& poly2, Paths& solution);
+
+void PolyTreeToPaths(const PolyTree& polytree, Paths& paths);
+void ClosedPathsFromPolyTree(const PolyTree& polytree, Paths& paths);
+void OpenPathsFromPolyTree(PolyTree& polytree, Paths& paths);
+
+void ReversePath(Path& p);
+void ReversePaths(Paths& p);
+
+struct IntRect { cInt left; cInt top; cInt right; cInt bottom; };
+
+//enums that are used internally ...
+enum EdgeSide { esLeft = 1, esRight = 2};
+
+//forward declarations (for stuff used internally) ...
+struct TEdge;
+struct IntersectNode;
+struct LocalMinimum;
+struct OutPt;
+struct OutRec;
+struct Join;
+
+typedef std::vector < OutRec* > PolyOutList;
+typedef std::vector < TEdge* > EdgeList;
+typedef std::vector < Join* > JoinList;
+typedef std::vector < IntersectNode* > IntersectList;
+
+//------------------------------------------------------------------------------
+
+//ClipperBase is the ancestor to the Clipper class. It should not be
+//instantiated directly. This class simply abstracts the conversion of sets of
+//polygon coordinates into edge objects that are stored in a LocalMinima list.
+class ClipperBase
+{
+public:
+ ClipperBase();
+ virtual ~ClipperBase();
+ virtual bool AddPath(const Path &pg, PolyType PolyTyp, bool Closed);
+ bool AddPaths(const Paths &ppg, PolyType PolyTyp, bool Closed);
+ virtual void Clear();
+ IntRect GetBounds();
+ bool PreserveCollinear() {return m_PreserveCollinear;};
+ void PreserveCollinear(bool value) {m_PreserveCollinear = value;};
+protected:
+ void DisposeLocalMinimaList();
+ TEdge* AddBoundsToLML(TEdge *e, bool IsClosed);
+ virtual void Reset();
+ TEdge* ProcessBound(TEdge* E, bool IsClockwise);
+ void InsertScanbeam(const cInt Y);
+ bool PopScanbeam(cInt &Y);
+ bool LocalMinimaPending();
+ bool PopLocalMinima(cInt Y, const LocalMinimum *&locMin);
+ OutRec* CreateOutRec();
+ void DisposeAllOutRecs();
+ void DisposeOutRec(PolyOutList::size_type index);
+ void SwapPositionsInAEL(TEdge *edge1, TEdge *edge2);
+ void DeleteFromAEL(TEdge *e);
+ void UpdateEdgeIntoAEL(TEdge *&e);
+
+ typedef std::vector<LocalMinimum> MinimaList;
+ MinimaList::iterator m_CurrentLM;
+ MinimaList m_MinimaList;
+
+ bool m_UseFullRange;
+ EdgeList m_edges;
+ bool m_PreserveCollinear;
+ bool m_HasOpenPaths;
+ PolyOutList m_PolyOuts;
+ TEdge *m_ActiveEdges;
+
+ typedef std::priority_queue<cInt> ScanbeamList;
+ ScanbeamList m_Scanbeam;
+};
+//------------------------------------------------------------------------------
+
+class Clipper : public virtual ClipperBase
+{
+public:
+ Clipper(int initOptions = 0);
+ bool Execute(ClipType clipType,
+ Paths &solution,
+ PolyFillType fillType = pftEvenOdd);
+ bool Execute(ClipType clipType,
+ Paths &solution,
+ PolyFillType subjFillType,
+ PolyFillType clipFillType);
+ bool Execute(ClipType clipType,
+ PolyTree &polytree,
+ PolyFillType fillType = pftEvenOdd);
+ bool Execute(ClipType clipType,
+ PolyTree &polytree,
+ PolyFillType subjFillType,
+ PolyFillType clipFillType);
+ bool ReverseSolution() { return m_ReverseOutput; };
+ void ReverseSolution(bool value) {m_ReverseOutput = value;};
+ bool StrictlySimple() {return m_StrictSimple;};
+ void StrictlySimple(bool value) {m_StrictSimple = value;};
+ //set the callback function for z value filling on intersections (otherwise Z is 0)
+#ifdef use_xyz
+ void ZFillFunction(ZFillCallback zFillFunc);
+#endif
+protected:
+ virtual bool ExecuteInternal();
+private:
+ JoinList m_Joins;
+ JoinList m_GhostJoins;
+ IntersectList m_IntersectList;
+ ClipType m_ClipType;
+ typedef std::list<cInt> MaximaList;
+ MaximaList m_Maxima;
+ TEdge *m_SortedEdges;
+ bool m_ExecuteLocked;
+ PolyFillType m_ClipFillType;
+ PolyFillType m_SubjFillType;
+ bool m_ReverseOutput;
+ bool m_UsingPolyTree;
+ bool m_StrictSimple;
+#ifdef use_xyz
+ ZFillCallback m_ZFill; //custom callback
+#endif
+ void SetWindingCount(TEdge& edge);
+ bool IsEvenOddFillType(const TEdge& edge) const;
+ bool IsEvenOddAltFillType(const TEdge& edge) const;
+ void InsertLocalMinimaIntoAEL(const cInt botY);
+ void InsertEdgeIntoAEL(TEdge *edge, TEdge* startEdge);
+ void AddEdgeToSEL(TEdge *edge);
+ bool PopEdgeFromSEL(TEdge *&edge);
+ void CopyAELToSEL();
+ void DeleteFromSEL(TEdge *e);
+ void SwapPositionsInSEL(TEdge *edge1, TEdge *edge2);
+ bool IsContributing(const TEdge& edge) const;
+ bool IsTopHorz(const cInt XPos);
+ void DoMaxima(TEdge *e);
+ void ProcessHorizontals();
+ void ProcessHorizontal(TEdge *horzEdge);
+ void AddLocalMaxPoly(TEdge *e1, TEdge *e2, const IntPoint &pt);
+ OutPt* AddLocalMinPoly(TEdge *e1, TEdge *e2, const IntPoint &pt);
+ OutRec* GetOutRec(int idx);
+ void AppendPolygon(TEdge *e1, TEdge *e2);
+ void IntersectEdges(TEdge *e1, TEdge *e2, IntPoint &pt);
+ OutPt* AddOutPt(TEdge *e, const IntPoint &pt);
+ OutPt* GetLastOutPt(TEdge *e);
+ bool ProcessIntersections(const cInt topY);
+ void BuildIntersectList(const cInt topY);
+ void ProcessIntersectList();
+ void ProcessEdgesAtTopOfScanbeam(const cInt topY);
+ void BuildResult(Paths& polys);
+ void BuildResult2(PolyTree& polytree);
+ void SetHoleState(TEdge *e, OutRec *outrec);
+ void DisposeIntersectNodes();
+ bool FixupIntersectionOrder();
+ void FixupOutPolygon(OutRec &outrec);
+ void FixupOutPolyline(OutRec &outrec);
+ bool IsHole(TEdge *e);
+ bool FindOwnerFromSplitRecs(OutRec &outRec, OutRec *&currOrfl);
+ void FixHoleLinkage(OutRec &outrec);
+ void AddJoin(OutPt *op1, OutPt *op2, const IntPoint offPt);
+ void ClearJoins();
+ void ClearGhostJoins();
+ void AddGhostJoin(OutPt *op, const IntPoint offPt);
+ bool JoinPoints(Join *j, OutRec* outRec1, OutRec* outRec2);
+ void JoinCommonEdges();
+ void DoSimplePolygons();
+ void FixupFirstLefts1(OutRec* OldOutRec, OutRec* NewOutRec);
+ void FixupFirstLefts2(OutRec* InnerOutRec, OutRec* OuterOutRec);
+ void FixupFirstLefts3(OutRec* OldOutRec, OutRec* NewOutRec);
+#ifdef use_xyz
+ void SetZ(IntPoint& pt, TEdge& e1, TEdge& e2);
+#endif
+};
+//------------------------------------------------------------------------------
+
+class ClipperOffset
+{
+public:
+ ClipperOffset(double miterLimit = 2.0, double roundPrecision = 0.25);
+ ~ClipperOffset();
+ void AddPath(const Path& path, JoinType joinType, EndType endType);
+ void AddPaths(const Paths& paths, JoinType joinType, EndType endType);
+ void Execute(Paths& solution, double delta);
+ void Execute(PolyTree& solution, double delta);
+ void Clear();
+ double MiterLimit;
+ double ArcTolerance;
+private:
+ Paths m_destPolys;
+ Path m_srcPoly;
+ Path m_destPoly;
+ std::vector<DoublePoint> m_normals;
+ double m_delta, m_sinA, m_sin, m_cos;
+ double m_miterLim, m_StepsPerRad;
+ IntPoint m_lowest;
+ PolyNode m_polyNodes;
+
+ void FixOrientations();
+ void DoOffset(double delta);
+ void OffsetPoint(int j, int& k, JoinType jointype);
+ void DoSquare(int j, int k);
+ void DoMiter(int j, int k, double r);
+ void DoRound(int j, int k);
+};
+//------------------------------------------------------------------------------
+
+class clipperException : public std::exception
+{
+ public:
+ clipperException(const char* description): m_descr(description) {}
+ virtual ~clipperException() throw() {}
+ virtual const char* what() const throw() {return m_descr.c_str();}
+ private:
+ std::string m_descr;
+};
+//------------------------------------------------------------------------------
+
+} //ClipperLib namespace
+
+#endif //clipper_hpp
+
+
diff --git a/3rdparty/libatidaq-c.orig/CMakeLists.txt b/3rdparty/libatidaq-c.orig/CMakeLists.txt
deleted file mode 100644
index 5a3be1b..0000000
--- a/3rdparty/libatidaq-c.orig/CMakeLists.txt
+++ /dev/null
@@ -1,149 +0,0 @@
-project(ati)
-
-cmake_minimum_required(VERSION 2.8)
-
-########### already in VISP
-# remove all matching elements from the list
-macro(vp_list_filterout lst regex)
- foreach(item ${${lst}})
- if(item MATCHES "${regex}")
- list(REMOVE_ITEM ${lst} "${item}")
- endif()
- endforeach()
-endmacro()
-
-set(VP_COMPILER_FAIL_REGEX
- "command line option .* is valid for .* but not for C\\+\\+" # GNU
- "command line option .* is valid for .* but not for C" # GNU
- "unrecognized .*option" # GNU
- "unknown .*option" # Clang
- "ignoring unknown option" # MSVC
- "warning D9002" # MSVC, any lang
- "option .*not supported" # Intel
- "[Uu]nknown option" # HP
- "[Ww]arning: [Oo]ption" # SunPro
- "command option .* is not recognized" # XL
- "not supported in this configuration; ignored" # AIX
- "File with unknown suffix passed to linker" # PGI
- "WARNING: unknown flag:" # Open64
- )
-
-macro(vp_check_compiler_flag LANG FLAG RESULT)
- if(NOT DEFINED ${RESULT})
- if("_${LANG}_" MATCHES "_CXX_")
- set(_fname "${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/CMakeTmp/src.cxx")
- if("${CMAKE_CXX_FLAGS} ${FLAG} " MATCHES "-Werror " OR "${CMAKE_CXX_FLAGS} ${FLAG} " MATCHES "-Werror=unknown-pragmas ")
- file(WRITE "${_fname}" "int main() { return 0; }\n")
- else()
- file(WRITE "${_fname}" "#pragma\nint main() { return 0; }\n")
- endif()
- elseif("_${LANG}_" MATCHES "_C_")
- set(_fname "${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/CMakeTmp/src.c")
- if("${CMAKE_C_FLAGS} ${FLAG} " MATCHES "-Werror " OR "${CMAKE_C_FLAGS} ${FLAG} " MATCHES "-Werror=unknown-pragmas ")
- file(WRITE "${_fname}" "int main(void) { return 0; }\n")
- else()
- file(WRITE "${_fname}" "#pragma\nint main(void) { return 0; }\n")
- endif()
- elseif("_${LANG}_" MATCHES "_OBJCXX_")
- set(_fname "${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/CMakeTmp/src.mm")
- if("${CMAKE_CXX_FLAGS} ${FLAG} " MATCHES "-Werror " OR "${CMAKE_CXX_FLAGS} ${FLAG} " MATCHES "-Werror=unknown-pragmas ")
- file(WRITE "${_fname}" "int main() { return 0; }\n")
- else()
- file(WRITE "${_fname}" "#pragma\nint main() { return 0; }\n")
- endif()
- else()
- unset(_fname)
- endif()
- if(_fname)
- message(STATUS "Performing Test ${RESULT}")
- try_compile(${RESULT}
- "${CMAKE_BINARY_DIR}"
- "${_fname}"
- COMPILE_DEFINITIONS "${FLAG}"
- OUTPUT_VARIABLE OUTPUT)
-
- foreach(_regex ${VP_COMPILER_FAIL_REGEX})
- if("${OUTPUT}" MATCHES "${_regex}")
- set(${RESULT} 0)
- break()
- endif()
- endforeach()
-
- if(${RESULT})
- set(${RESULT} 1 CACHE INTERNAL "Test ${RESULT}")
- message(STATUS "Performing Test ${RESULT} - Success")
- else()
- message(STATUS "Performing Test ${RESULT} - Failed")
- set(${RESULT} "" CACHE INTERNAL "Test ${RESULT}")
- endif()
- else()
- set(${RESULT} 0)
- endif()
- endif()
-endmacro()
-
-macro(vp_check_flag_support lang flag varname)
- if("_${lang}_" MATCHES "_CXX_")
- set(_lang CXX)
- elseif("_${lang}_" MATCHES "_C_")
- set(_lang C)
- else()
- set(_lang ${lang})
- endif()
-
- string(TOUPPER "${flag}" ${varname})
- string(REGEX REPLACE "^(/|-)" "HAVE_${_lang}_" ${varname} "${${varname}}")
- string(REGEX REPLACE " -|-|=| |\\." "_" ${varname} "${${varname}}")
-
- vp_check_compiler_flag("${_lang}" "${ARGN} ${flag}" ${${varname}})
-endmacro()
-
-# turns off warnings
-macro(vp_warnings_disable)
- set(_flag_vars "")
- set(_msvc_warnings "")
- set(_gxx_warnings "")
- foreach(arg ${ARGN})
- if(arg MATCHES "^CMAKE_")
- list(APPEND _flag_vars ${arg})
- elseif(arg MATCHES "^/wd")
- list(APPEND _msvc_warnings ${arg})
- elseif(arg MATCHES "^-W")
- list(APPEND _gxx_warnings ${arg})
- endif()
- endforeach()
- if(MSVC AND _msvc_warnings AND _flag_vars)
- foreach(var ${_flag_vars})
- foreach(warning ${_msvc_warnings})
- set(${var} "${${var}} ${warning}")
- endforeach()
- endforeach()
- elseif((CMAKE_COMPILER_IS_GNUCXX OR ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")) AND _gxx_warnings AND _flag_vars)
- foreach(var ${_flag_vars})
- foreach(warning ${_gxx_warnings})
- if(NOT warning MATCHES "^-Wno-")
- string(REPLACE "${warning}" "" ${var} "${${var}}")
- string(REPLACE "-W" "-Wno-" warning "${warning}")
- endif()
- vp_check_flag_support(${var} "${warning}" _varname)
- if(${_varname})
- set(${var} "${${var}} ${warning}")
- endif()
- endforeach()
- endforeach()
- endif()
- unset(_flag_vars)
- unset(_msvc_warnings)
- unset(_gxx_warnings)
-endmacro()
-
-
-set(ATIDAQ_LIBRARY atidaq)
-
-add_subdirectory(ATIDAQ)
-
-include_directories(${PROJECT_SOURCE_DIR}/ATIDAQ)
-#add_executable(ftconvert Samples/ftconvert.c)
-#target_link_libraries(ftconvert ${ATIDAQ_LIBRARY})
-add_executable(ftconvert test/ftconvert.cpp)
-target_link_libraries(ftconvert ${ATIDAQ_LIBRARY})
diff --git a/3rdparty/libatidaq-c.orig/readme.txt b/3rdparty/libatidaq-c.orig/readme.txt
deleted file mode 100644
index 69df9a4..0000000
--- a/3rdparty/libatidaq-c.orig/readme.txt
+++ /dev/null
@@ -1,138 +0,0 @@
-ATI DAQ F/T C Library (v1.0.6)
------------------------------------------
-
-The purpose of this C library is to load and parse calibration files for ATI
-DAQ F/T transducers, and perform the necessary calculations to convert raw
-voltages from a data acquisition system into forces and torques.
-
-Neither the library itself or the sample applications provide hardware I/O.
-These functions must be provided by your hardware's device drivers.
-
-If you are developing an application for the Windows environment, we
-recommend you use the ATIDAQFT ActiveX Automation Server or the
-ATICombinedDAQFT .NET assembly.
-
-
-Licensing
----------
-All of the software including in this package is distributed under the MIT License:
-
-Permission is hereby granted, free of charge, to any person obtaining a copy of
-this software and associated documentation files (the "Software"), to deal in
-the Software without restriction, including without limitation the rights to
-use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
-of the Software, and to permit persons to whom the Software is furnished to do
-so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-
-http://www.opensource.org/licenses/mit-license.html
-
-
-Library Files
------------------
-The following files should be compiled as part of your project:
-ftconfig.c
-ftrt.c
-dom.c
-expatls.c
-node.c
-stack.c
-xmlparse.c
-xmlrole.c
-xmltok.c
-
-In addition to the above files, the files "events.c", "xmltok_impl.c", and
-"xmltok_ns.c" are also included in this library, but are not meant to be
-directly compiled, as they are '#include'd into other library files.
-
-None of these files are intended to be modified.
-
-
-Sample Applications
-----------------------------
-Two sample applications are included in the Samples folder.
-
-ftconvert.c - demonstrates configuring a DAQ F/T system and performing
-force/torque calculations; converts a sample set of voltages into forces and
-torques.
-calinfo.c - demonstrates retrieving information from the calibration file such
-as the transducer's rated loads, calibration date, and other information.
-
-
-Programming Interface
--------------------------------
-The functions supported for public use are:
-
-createCalibration - Loads calibration info for a transducer from a calibration
-file
-destroyCalibration - Frees memory allocated by a successful call to
-createCalibration
-SetToolTransform - Performs a 6-axis translation/rotation on the transducer's
-coordinate system
-SetForceUnits - Sets the units of force output
-SetTorqueUnits - Sets the units of torque outputEnables or disables
-temperature compensation, if available
-Bias - Stores a voltage reading to be subtracted from subsequent readings,
-effectively "zeroing" the transducer output to remove tooling weight, etc.
-ConvertToFT - Converts an array of voltages into forces and torques
-
-These functions are documented in ftconfig.h.
-
-You can also read information about the transducer system and calibration from the Calibration and other structures. These structures should be treated as read-only and should only be manipulated through the functions listed above. The sample application calinfo.c shows how to retrieve this information.
-
-The general structure of an application should follow the following structure:
-
-1. Call the createCalibration() method to load a calibration file into
- memory.
-2. Call the SetForceUnits(), SetTorqueUnits(), SetCalibration(), and
- SetToolTransform() functions to configure the system.
-3. Use device driver calls to retrieve an array of voltages from your data
- acquisition system.
-4. Use the Bias() function to store a baseline reading (zero the transducer)
-5. Use the ConvertToFT() function to convert another array of voltages into
- forces and torques. The Bias reading will be subtracted automatically.
-6. Call the destroyCalibration() function to free memory allocated for the
- Calibration structure created in step 1.
-
-
-Optimizing Your Application
---------------------------------------
-The bare minimum calculations for converting voltages into forces and torques
-are in ftrt.c. To isolate this section of the code, simply compile ftrc.c.
-This file #includes ftrc.h, but none of the other files. This can be useful,
-for example, in an embedded application where the configuration never changes.
-You can write a separate configuration program to run on a PC, which sends the
-contents of the Calibration.rt structure to a local copy of the RTCoefs
-structure in the embedded system.
-
-
-Version History
-----------------------
-1.0.6 Added support for kN-m torque units, changed reference to ActiveX
- server to also reference .NET assembly.
-1.0.5 Moved some declarations out of .h files into .c files to avoid
- multiple definition errors. Added ftsharedrt.h
-1.0.4 Added newlines to the ends of several files to avoid warnings when
- compiling with gcc. FTconfig.c was using '/t' instead of '\t' for tab
- character, which caused warnings under gcc, but no warnings or errors
- under Visual C++.
-1.0.3 Added comments to ftconvert.c explaining hardware temperature
- compensation.
-1.0.2 Fixed memory leaks
-1.0.0 No changes
- Released 2-1-02
-0.7.1 Added necessary licensing info to all files
- Beta released 10-5-01
-0.7.0 Initial beta release; no known bugs; compiles with no errors or warnings
- in Microsoft Visual Studio.NET
- Beta released 10-2-01
\ No newline at end of file
diff --git a/3rdparty/libatidaq-c.orig/samples/calinfo.c b/3rdparty/libatidaq-c.orig/samples/calinfo.c
deleted file mode 100644
index fcee67d..0000000
--- a/3rdparty/libatidaq-c.orig/samples/calinfo.c
+++ /dev/null
@@ -1,122 +0,0 @@
-/* ATIDAQ F/T C Library
- * v1.0.1
- * Copyright (c) 2001 ATI Industrial Automation
- *
- * The MIT License
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included
- * in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
- * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-/* calinfo.c - demonstrates retrieving information from the calibration file such as the
- transducer's rated loads, calibration date, and other information.
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include "..\ATIDAQ\ftconfig.h"
-
-int main(int argc, char *argv[]) {
- char *calfilepath; // name of calibration file
- unsigned short index; // index of calibration in file (second parameter; default = 1)
- Calibration *cal; // struct containing calibration information
- unsigned short i,j; // loop variables
-
- // parse arguments
- switch (argc) {
- case 2:
- index=1;
- break;
- case 3:
- index=atoi(argv[2]);
- break;
- default:
- printf("\nUsage: calinfo [calfile] [index]\n");
- printf(" [calfile]: path to calibration file\n");
- printf(" [index] : index of calibration in file (default=1)\n");
- scanf(".");
- return 0;
- }
- calfilepath=argv[1];
-
- // create Calibration struct
- cal=createCalibration(calfilepath,index);
- if (cal==NULL) {
- printf("\nSpecified calibration could not be loaded.\n");
- scanf(".");
- return 0;
- }
-
- // display info from calibration file
- printf("Calibration Information for %s, index #%i\n",calfilepath,index);
- printf(" Serial: %s\n",cal->Serial);
- printf(" Body Style: %s\n",cal->BodyStyle);
- printf(" Calibration: %s\n",cal->PartNumber);
- printf(" Calibration Date: %s\n",cal->CalDate);
- printf(" Family: %s\n",cal->Family);
- printf(" # Channels: %i\n",cal->rt.NumChannels);
- printf(" # Axes: %i\n",cal->rt.NumAxes);
- printf(" Force Units: %s\n",cal->ForceUnits);
- printf(" Torque Units: %s\n",cal->TorqueUnits);
- printf("Temperature Compensation: %s\n",(cal->TempCompAvailable ? "Yes" : "No"));
-
- // print maximum loads of axes
- printf("\nRated Loads\n");
- for (i=0;i<cal->rt.NumAxes;i++) {
- char *units;
- if ((cal->AxisNames[i])[0]=='F') {
- units=cal->ForceUnits;
- } else units=cal->TorqueUnits;
- printf("%s: %f %s\n",cal->AxisNames[i],cal->MaxLoads[i],units);
- }
-
- // print working calibration matrix
- printf("\nWorking Calibration Matrix\n");
- printf(" ");
- for (i=0;i<cal->rt.NumChannels-1;i++)
- printf("G%i ",i);
- printf("\n");
- for (i=0;i<cal->rt.NumAxes;i++) {
- printf("%s: ",cal->AxisNames[i]);
- for (j=0;j<cal->rt.NumChannels-1;j++)
- printf("%13.5e ",cal->rt.working_matrix[i][j]);
- printf("\n");
- }
-
- // print temperature compensation information, if available
- if (cal->TempCompAvailable) {
- printf("\nTemperature Compensation Information\n");
- printf("BS: ");
- for (i=0;i<cal->rt.NumChannels-1;i++) {
- printf("%13.5e ",cal->rt.bias_slopes[i]);
- }
- printf("\nGS: ");
- for (i=0;i<cal->rt.NumChannels-1;i++) {
- printf("%13.5e ",cal->rt.gain_slopes[i]);
- }
- printf("\nTherm: %f\n",cal->rt.thermistor);
- }
-
- // free memory allocated to Calibration structure
- destroyCalibration(cal);
-
- //wait for a keypress
- scanf(".");
-
- return 0;
-}
diff --git a/3rdparty/libatidaq-c.orig/samples/ftconvert.c b/3rdparty/libatidaq-c.orig/samples/ftconvert.c
deleted file mode 100644
index 697fc92..0000000
--- a/3rdparty/libatidaq-c.orig/samples/ftconvert.c
+++ /dev/null
@@ -1,156 +0,0 @@
-/* ATIDAQ F/T C Library
- * v1.0.2
- * Copyright (c) 2001 ATI Industrial Automation
- *
- * The MIT License
- *
- * Permission is hereby granted, free of charge, to any person obtaining a
- * copy of this software and associated documentation files (the "Software"),
- * to deal in the Software without restriction, including without limitation
- * the rights to use, copy, modify, merge, publish, distribute, sublicense,
- * and/or sell copies of the Software, and to permit persons to whom the
- * Software is furnished to do so, subject to the following conditions:
- *
- * The above copyright notice and this permission notice shall be included
- * in all copies or substantial portions of the Software.
- *
- * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
- * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
- * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
- * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
- * OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
- * ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
- * OTHER DEALINGS IN THE SOFTWARE.
- */
-
-/* ftconvert.c - demonstrates configuring a DAQ F/T system and performing force/torque
-calculations; converts a sample set of voltages into forces and torques.
-
-Sam Skuce - v.1.0.2 - February.19.2004 - added some comments just before the declaration
- of SampleBias explaining the difference between hardware and software temp comp
- */
-
-#include <stdio.h>
-#include <stdlib.h>
-#include "ftconfig.h"
-
-int main(int argc, char *argv[]) {
- char *calfilepath; // name of calibration file
- unsigned short index; // index of calibration in file (second parameter; default = 1)
- Calibration *cal; // struct containing calibration information
- unsigned short i; // loop variable used to print results
- short sts; // return value from functions
-
- // In this sample application, readings have been hard-coded for demonstration.
- // A working application would retrieve these vectors from a data acquisition system.
- // PLEASE NOTE: There are 7 elements in the bias and reading arrays. The first 6 are
- // the gage values you would retrieve from the transducer. The seventh represents the
- // thermistor gage, which is only meaningful if your sensor uses software temperature
- // compensation. If your sensor uses hardware temperature compensation (all sensors
- // sold after mid-2003 do), the last value is meaningless, and you can just use a 6 element
- // array.
- float SampleBias[7]={0.2651,-0.0177,-0.0384,-0.0427,-0.1891,0.1373,-3.2423};
- float SampleReading[7]={-3.2863,0.3875,-3.4877,0.4043,-3.9341,0.5474,-3.2106};
-
- // This sample transform includes a translation along the Z-axis and a rotation about the X-axis.
- float SampleTT[6]={0,0,20,45,0,0};
-
- float FT[6]; // This array will hold the resultant force/torque vector.
-
- // parse arguments
- switch (argc) {
- case 2:
- index=1;
- break;
- case 3:
- index=atoi(argv[2]);
- break;
- default:
- printf("\nUsage: calinfo [calfile] [index]\n");
- printf(" [calfile]: path to calibration file\n");
- printf(" [index] : index of calibration in file (default=1)\n");
- scanf(".");
- return 0;
- }
- calfilepath=argv[1];
-
- // create Calibration struct
- cal=createCalibration(calfilepath,index);
- if (cal==NULL) {
- printf("\nSpecified calibration could not be loaded.\n");
- scanf(".");
- return 0;
- }
-
- // Set force units.
- // This step is optional; by default, the units are inherited from the calibration file.
- sts=SetForceUnits(cal,"N");
- switch (sts) {
- case 0: break; // successful completion
- case 1: printf("Invalid Calibration struct"); return 0;
- case 2: printf("Invalid force units"); return 0;
- default: printf("Unknown error"); return 0;
- }
-
-
- // Set torque units.
- // This step is optional; by default, the units are inherited from the calibration file.
- sts=SetTorqueUnits(cal,"N-m");
- switch (sts) {
- case 0: break; // successful completion
- case 1: printf("Invalid Calibration struct"); return 0;
- case 2: printf("Invalid torque units"); return 0;
- default: printf("Unknown error"); return 0;
- }
-
-
- // Set tool transform.
- // This line is only required if you want to move or rotate the sensor's coordinate system.
- // This example tool transform translates the coordinate system 20 mm along the Z-axis
- // and rotates it 45 degrees about the X-axis.
- sts=SetToolTransform(cal,SampleTT,"mm","degrees");
- switch (sts) {
- case 0: break; // successful completion
- case 1: printf("Invalid Calibration struct"); return 0;
- case 2: printf("Invalid distance units"); return 0;
- case 3: printf("Invalid angle units"); return 0;
- default: printf("Unknown error"); return 0;
- }
-
-
- // Temperature compensation is on by default if it is available.
- // To explicitly disable temperature compensation, uncomment the following code
- /*SetTempComp(cal,FALSE); // disable temperature compensation
- switch (sts) {
- case 0: break; // successful completion
- case 1: printf("Invalid Calibration struct"); return 0;
- case 2: printf("Temperature Compensation not available on this transducer"); return 0;
- default: printf("Unknown error"); return 0;
- }*/
-
- // store an unloaded measurement; this removes the effect of tooling weight
- Bias(cal,SampleBias);
-
- // convert a loaded measurement into forces and torques
- ConvertToFT(cal,SampleReading,FT);
-
-
- // print results
- printf("Bias reading:\n");
- for (i=0;i<7;i++)
- printf("%9.6f ",SampleBias[i]);
- printf("\nMeasurement:\n");
- for (i=0;i<7;i++)
- printf("%9.6f ",SampleReading[i]);
- printf("\nResult:\n");
- for (i=0;i<6;i++)
- printf("%9.6f ",FT[i]);
-
- // free memory allocated to Calibration structure
- destroyCalibration(cal);
-
- //wait for a keypress
- scanf(".");
-
- return 0;
-}
diff --git a/3rdparty/libatidaq-c.orig/samples/makefile b/3rdparty/libatidaq-c.orig/samples/makefile
deleted file mode 100644
index 831de28..0000000
--- a/3rdparty/libatidaq-c.orig/samples/makefile
+++ /dev/null
@@ -1,34 +0,0 @@
-#makefile for ftconvert and calinfo
-#use 'make ftconvert' to make ftconvert sample and 'make calinfo' to make calinfo
-#uses gcc, needs modifications for other compilers.
-object_compile = gcc -c
-exe_compile = gcc -o
-remove_command = rm
-ftconvert: ftconvert.o ftconfig.o ftrt.o dom.o expatls.o node.o stack.o xmlparse.o xmlrole.o xmltok.o
- $(exe_compile) ftconvert.exe ftconvert.o ftconfig.o ftrt.o dom.o expatls.o node.o stack.o xmlparse.o xmlrole.o xmltok.o
-ftconvert.o: ftconvert.c ../atidaq/ftconfig.h
- $(object_compile) ftconvert.c
-ftconfig.o: ../atidaq/ftconfig.c ../atidaq/ftconfig.h
- $(object_compile) ../atidaq/ftconfig.c
-ftrt.o: ../atidaq/ftrt.c ../atidaq/ftrt.h ../atidaq/ftsharedrt.h
- $(object_compile) ../atidaq/ftrt.c
-dom.o: ../atidaq/dom.c
- $(object_compile) ../atidaq/dom.c
-expatls.o: ../atidaq/expatls.c
- $(object_compile) ../atidaq/expatls.c
-node.o: ../atidaq/node.c
- $(object_compile) ../atidaq/node.c
-stack.o: ../atidaq/stack.c
- $(object_compile) ../atidaq/stack.c
-xmlparse.o: ../atidaq/xmlparse.c
- $(object_compile) ../atidaq/xmlparse.c
-xmlrole.o: ../atidaq/xmlrole.c
- $(object_compile) ../atidaq/xmlrole.c
-xmltok.o: ../atidaq/xmltok.c
- $(object_compile) ../atidaq/xmltok.c
-calinfo: calinfo.o ftconfig.o ftrt.o dom.o expatls.o node.o stack.o xmlparse.o xmlrole.o xmltok.o
- $(exe_compile) calinfo.exe calinfo.o ftconfig.o ftrt.o dom.o expatls.o node.o stack.o xmlparse.o xmlrole.o xmltok.o
-calinfo.o: calinfo.c ../atidaq/ftconfig.h
- $(object_compile) calinfo.c
-clean:
- $(remove_command) *.o *.exe
diff --git a/3rdparty/libatidaq-c/CMakeLists.txt b/3rdparty/libatidaq-c/CMakeLists.txt
deleted file mode 100644
index 1061cc4..0000000
--- a/3rdparty/libatidaq-c/CMakeLists.txt
+++ /dev/null
@@ -1,39 +0,0 @@
-project(${ATIDAQ_LIBRARY})
-
-vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
-
-file(GLOB lib_srcs *.c)
-file(GLOB lib_hdrs *.h)
-
-vp_list_filterout(lib_srcs xmltok_impl.c)
-vp_list_filterout(lib_hdrs xmltok_impl.h)
-vp_list_filterout(lib_srcs xmltok_ns.c)
-vp_list_filterout(lib_srcs events.c)
-
-add_library(${ATIDAQ_LIBRARY} STATIC ${lib_srcs} ${lib_hdrs})
-
-if(CMAKE_COMPILER_IS_GNUCXX)
- set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -fPIC")
-endif()
-
-if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${ATIDAQ_LIBRARY} PROPERTIES FOLDER "3rdparty")
-endif()
-
-vp_warnings_disable(CMAKE_C_FLAGS -Wunused-parameter)
-
-if(ENABLE_SOLUTION_FOLDERS)
- set_target_properties(${ATIDAQ_LIBRARY} PROPERTIES FOLDER "3rdparty")
-endif()
-
-set_target_properties(${ATIDAQ_LIBRARY}
- PROPERTIES OUTPUT_NAME ${ATIDAQ_LIBRARY}
- DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
- COMPILE_PDB_NAME ${ATIDAQ_LIBRARY}
- COMPILE_PDB_NAME_DEBUG "${ATIDAQ_LIBRARY}${VISP_DEBUG_POSTFIX}"
- ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
- )
-
-if(NOT BUILD_SHARED_LIBS)
- vp_install_target(${ATIDAQ_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
-endif()
diff --git a/3rdparty/libatidaq-c/readme.txt b/3rdparty/libatidaq-c/readme.txt
deleted file mode 100644
index 69df9a4..0000000
--- a/3rdparty/libatidaq-c/readme.txt
+++ /dev/null
@@ -1,138 +0,0 @@
-ATI DAQ F/T C Library (v1.0.6)
------------------------------------------
-
-The purpose of this C library is to load and parse calibration files for ATI
-DAQ F/T transducers, and perform the necessary calculations to convert raw
-voltages from a data acquisition system into forces and torques.
-
-Neither the library itself or the sample applications provide hardware I/O.
-These functions must be provided by your hardware's device drivers.
-
-If you are developing an application for the Windows environment, we
-recommend you use the ATIDAQFT ActiveX Automation Server or the
-ATICombinedDAQFT .NET assembly.
-
-
-Licensing
----------
-All of the software including in this package is distributed under the MIT License:
-
-Permission is hereby granted, free of charge, to any person obtaining a copy of
-this software and associated documentation files (the "Software"), to deal in
-the Software without restriction, including without limitation the rights to
-use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
-of the Software, and to permit persons to whom the Software is furnished to do
-so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-
-http://www.opensource.org/licenses/mit-license.html
-
-
-Library Files
------------------
-The following files should be compiled as part of your project:
-ftconfig.c
-ftrt.c
-dom.c
-expatls.c
-node.c
-stack.c
-xmlparse.c
-xmlrole.c
-xmltok.c
-
-In addition to the above files, the files "events.c", "xmltok_impl.c", and
-"xmltok_ns.c" are also included in this library, but are not meant to be
-directly compiled, as they are '#include'd into other library files.
-
-None of these files are intended to be modified.
-
-
-Sample Applications
-----------------------------
-Two sample applications are included in the Samples folder.
-
-ftconvert.c - demonstrates configuring a DAQ F/T system and performing
-force/torque calculations; converts a sample set of voltages into forces and
-torques.
-calinfo.c - demonstrates retrieving information from the calibration file such
-as the transducer's rated loads, calibration date, and other information.
-
-
-Programming Interface
--------------------------------
-The functions supported for public use are:
-
-createCalibration - Loads calibration info for a transducer from a calibration
-file
-destroyCalibration - Frees memory allocated by a successful call to
-createCalibration
-SetToolTransform - Performs a 6-axis translation/rotation on the transducer's
-coordinate system
-SetForceUnits - Sets the units of force output
-SetTorqueUnits - Sets the units of torque outputEnables or disables
-temperature compensation, if available
-Bias - Stores a voltage reading to be subtracted from subsequent readings,
-effectively "zeroing" the transducer output to remove tooling weight, etc.
-ConvertToFT - Converts an array of voltages into forces and torques
-
-These functions are documented in ftconfig.h.
-
-You can also read information about the transducer system and calibration from the Calibration and other structures. These structures should be treated as read-only and should only be manipulated through the functions listed above. The sample application calinfo.c shows how to retrieve this information.
-
-The general structure of an application should follow the following structure:
-
-1. Call the createCalibration() method to load a calibration file into
- memory.
-2. Call the SetForceUnits(), SetTorqueUnits(), SetCalibration(), and
- SetToolTransform() functions to configure the system.
-3. Use device driver calls to retrieve an array of voltages from your data
- acquisition system.
-4. Use the Bias() function to store a baseline reading (zero the transducer)
-5. Use the ConvertToFT() function to convert another array of voltages into
- forces and torques. The Bias reading will be subtracted automatically.
-6. Call the destroyCalibration() function to free memory allocated for the
- Calibration structure created in step 1.
-
-
-Optimizing Your Application
---------------------------------------
-The bare minimum calculations for converting voltages into forces and torques
-are in ftrt.c. To isolate this section of the code, simply compile ftrc.c.
-This file #includes ftrc.h, but none of the other files. This can be useful,
-for example, in an embedded application where the configuration never changes.
-You can write a separate configuration program to run on a PC, which sends the
-contents of the Calibration.rt structure to a local copy of the RTCoefs
-structure in the embedded system.
-
-
-Version History
-----------------------
-1.0.6 Added support for kN-m torque units, changed reference to ActiveX
- server to also reference .NET assembly.
-1.0.5 Moved some declarations out of .h files into .c files to avoid
- multiple definition errors. Added ftsharedrt.h
-1.0.4 Added newlines to the ends of several files to avoid warnings when
- compiling with gcc. FTconfig.c was using '/t' instead of '\t' for tab
- character, which caused warnings under gcc, but no warnings or errors
- under Visual C++.
-1.0.3 Added comments to ftconvert.c explaining hardware temperature
- compensation.
-1.0.2 Fixed memory leaks
-1.0.0 No changes
- Released 2-1-02
-0.7.1 Added necessary licensing info to all files
- Beta released 10-5-01
-0.7.0 Initial beta release; no known bugs; compiles with no errors or warnings
- in Microsoft Visual Studio.NET
- Beta released 10-2-01
\ No newline at end of file
diff --git a/3rdparty/pthreads-win32/ANNOUNCE b/3rdparty/pthreads-win32/ANNOUNCE
new file mode 100644
index 0000000..950c86f
--- /dev/null
+++ b/3rdparty/pthreads-win32/ANNOUNCE
@@ -0,0 +1,483 @@
+PTHREADS-WIN32 RELEASE 2.9.0 (2012-05-25)
+-----------------------------------------
+Web Site: http://sourceware.org/pthreads-win32/
+FTP Site: ftp://sourceware.org/pub/pthreads-win32
+Maintainer: Ross Johnson <ross.johnson at loungebythelake.net>
+
+
+We are pleased to announce the availability of a new release of
+Pthreads-win32, an Open Source Software implementation of the
+Threads component of the POSIX 1003.1 2001 Standard for Microsoft's
+Win32 environment. Some functions from other sections of POSIX
+1003.1 2001 are also supported including semaphores and scheduling
+functions.
+
+Some common non-portable functions are also implemented for
+additional compatibility, as are a few functions specific
+to pthreads-win32 for easier integration with Win32 applications.
+
+Pthreads-win32 is free software, distributed under the GNU Lesser
+General Public License (LGPL).
+
+
+Acknowledgements
+----------------
+This library is based originally on a Win32 pthreads
+implementation contributed by John Bossom.
+
+The implementation of Condition Variables uses algorithms developed
+by Alexander Terekhov and Louis Thomas.
+
+The implementation of POSIX mutexes has been improved by Thomas Pfaff
+and later by Alexander Terekhov.
+
+The implementation of Spinlocks and Barriers was contributed
+by Ross Johnson.
+
+The implementation of read/write locks was contributed by
+Aurelio Medina and improved by Alexander Terekhov.
+
+Many others have contributed significant time and effort to solve crutial
+problems in order to make the library workable, robust and reliable.
+
+Thanks to Xavier Leroy for granting permission to use and modify his
+LinuxThreads manual pages.
+
+Thanks to The Open Group for making the Single Unix Specification
+publicly available - many of the manual pages included in the package
+were extracted from it.
+
+There is also a separate CONTRIBUTORS file. This file and others are
+on the web site:
+
+ http://sourceware.org/pthreads-win32
+
+As much as possible, the ChangeLog file acknowledges contributions to the
+code base in more detail.
+
+
+Changes since the last release
+------------------------------
+These are now documented in the NEWS file.
+See the ChangeLog file also.
+
+
+Known Bugs
+----------
+These are now documented in the BUGS file.
+
+
+Level of standards conformance
+------------------------------
+
+The following POSIX 1003.1 2001 options are defined and set to 200112L:
+
+ _POSIX_THREADS
+ _POSIX_THREAD_SAFE_FUNCTIONS
+ _POSIX_THREAD_ATTR_STACKSIZE
+ _POSIX_THREAD_PRIORITY_SCHEDULING
+ _POSIX_SEMAPHORES
+ _POSIX_READER_WRITER_LOCKS
+ _POSIX_SPIN_LOCKS
+ _POSIX_BARRIERS
+
+
+The following POSIX 1003.1 2001 options are defined and set to -1:
+
+ _POSIX_THREAD_ATTR_STACKADDR
+ _POSIX_THREAD_PRIO_INHERIT
+ _POSIX_THREAD_PRIO_PROTECT
+ _POSIX_THREAD_PROCESS_SHARED
+
+
+The following POSIX 1003.1 2001 limits are defined and set:
+
+ _POSIX_THREAD_THREADS_MAX
+ _POSIX_SEM_VALUE_MAX
+ _POSIX_SEM_NSEMS_MAX
+ _POSIX_THREAD_KEYS_MAX
+ _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+ PTHREAD_STACK_MIN
+ PTHREAD_THREADS_MAX
+ SEM_VALUE_MAX
+ SEM_NSEMS_MAX
+ PTHREAD_KEYS_MAX
+ PTHREAD_DESTRUCTOR_ITERATIONS
+
+
+The following functions are implemented:
+
+ ---------------------------
+ PThreads
+ ---------------------------
+ pthread_attr_init
+ pthread_attr_destroy
+ pthread_attr_getdetachstate
+ pthread_attr_getstackaddr
+ pthread_attr_getstacksize
+ pthread_attr_setdetachstate
+ pthread_attr_setstackaddr
+ pthread_attr_setstacksize
+
+ pthread_create
+ pthread_detach
+ pthread_equal
+ pthread_exit
+ pthread_join
+ pthread_once
+ pthread_self
+
+ pthread_cancel
+ pthread_cleanup_pop
+ pthread_cleanup_push
+ pthread_setcancelstate
+ pthread_setcanceltype
+ pthread_testcancel
+
+ ---------------------------
+ Thread Specific Data
+ ---------------------------
+ pthread_key_create
+ pthread_key_delete
+ pthread_setspecific
+ pthread_getspecific
+
+ ---------------------------
+ Mutexes
+ ---------------------------
+ pthread_mutexattr_init
+ pthread_mutexattr_destroy
+ pthread_mutexattr_getpshared
+ pthread_mutexattr_setpshared
+ pthread_mutexattr_gettype
+ pthread_mutexattr_settype (types: PTHREAD_MUTEX_DEFAULT
+ PTHREAD_MUTEX_NORMAL
+ PTHREAD_MUTEX_ERRORCHECK
+ PTHREAD_MUTEX_RECURSIVE )
+ pthread_mutexattr_getrobust
+ pthread_mutexattr_setrobust (values: PTHREAD_MUTEX_STALLED
+ PTHREAD_MUTEX_ROBUST)
+ pthread_mutex_init
+ pthread_mutex_destroy
+ pthread_mutex_lock
+ pthread_mutex_trylock
+ pthread_mutex_timedlock
+ pthread_mutex_unlock
+ pthread_mutex_consistent
+
+ ---------------------------
+ Condition Variables
+ ---------------------------
+ pthread_condattr_init
+ pthread_condattr_destroy
+ pthread_condattr_getpshared
+ pthread_condattr_setpshared
+
+ pthread_cond_init
+ pthread_cond_destroy
+ pthread_cond_wait
+ pthread_cond_timedwait
+ pthread_cond_signal
+ pthread_cond_broadcast
+
+ ---------------------------
+ Read/Write Locks
+ ---------------------------
+ pthread_rwlock_init
+ pthread_rwlock_destroy
+ pthread_rwlock_tryrdlock
+ pthread_rwlock_trywrlock
+ pthread_rwlock_rdlock
+ pthread_rwlock_timedrdlock
+ pthread_rwlock_rwlock
+ pthread_rwlock_timedwrlock
+ pthread_rwlock_unlock
+ pthread_rwlockattr_init
+ pthread_rwlockattr_destroy
+ pthread_rwlockattr_getpshared
+ pthread_rwlockattr_setpshared
+
+ ---------------------------
+ Spin Locks
+ ---------------------------
+ pthread_spin_init
+ pthread_spin_destroy
+ pthread_spin_lock
+ pthread_spin_unlock
+ pthread_spin_trylock
+
+ ---------------------------
+ Barriers
+ ---------------------------
+ pthread_barrier_init
+ pthread_barrier_destroy
+ pthread_barrier_wait
+ pthread_barrierattr_init
+ pthread_barrierattr_destroy
+ pthread_barrierattr_getpshared
+ pthread_barrierattr_setpshared
+
+ ---------------------------
+ Semaphores
+ ---------------------------
+ sem_init
+ sem_destroy
+ sem_post
+ sem_wait
+ sem_trywait
+ sem_timedwait
+ sem_getvalue (# free if +ve, # of waiters if -ve)
+ sem_open (returns an error ENOSYS)
+ sem_close (returns an error ENOSYS)
+ sem_unlink (returns an error ENOSYS)
+
+ ---------------------------
+ RealTime Scheduling
+ ---------------------------
+ pthread_attr_getschedparam
+ pthread_attr_setschedparam
+ pthread_attr_getinheritsched
+ pthread_attr_setinheritsched
+ pthread_attr_getschedpolicy (only supports SCHED_OTHER)
+ pthread_attr_setschedpolicy (only supports SCHED_OTHER)
+ pthread_getschedparam
+ pthread_setschedparam
+ pthread_getconcurrency
+ pthread_setconcurrency
+ pthread_attr_getscope
+ pthread_attr_setscope (only supports PTHREAD_SCOPE_SYSTEM)
+ sched_get_priority_max
+ sched_get_priority_min
+ sched_rr_get_interval (returns an error ENOTSUP)
+ sched_setscheduler (only supports SCHED_OTHER)
+ sched_getscheduler (only supports SCHED_OTHER)
+ sched_yield
+
+ ---------------------------
+ Signals
+ ---------------------------
+ pthread_sigmask
+ pthread_kill (only supports zero sig value,
+ for thread validity checking)
+
+ ---------------------------
+ Non-portable routines (see the README.NONPORTABLE file for usage)
+ ---------------------------
+ pthread_getw32threadhandle_np
+ pthread_timechange_handler_np
+ pthread_delay_np
+ pthread_getunique_np
+ pthread_mutexattr_getkind_np
+ pthread_mutexattr_setkind_np (types: PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_ERRORCHECK_NP,
+ PTHREAD_MUTEX_RECURSIVE_NP,
+ PTHREAD_MUTEX_ADAPTIVE_NP,
+ PTHREAD_MUTEX_TIMED_NP)
+ pthread_num_processors_np
+ (The following four routines may be required when linking statically.
+ The process_* routines should not be needed for MSVC or GCC.)
+ pthread_win32_process_attach_np
+ pthread_win32_process_detach_np
+ (The following routines should only be needed to manage implicit
+ POSIX handles i.e. when Win native threads call POSIX thread routines
+ (other than pthread_create))
+ pthread_win32_thread_attach_np
+ pthread_win32_thread_detach_np
+
+ ---------------------------
+ Static Initializers
+ ---------------------------
+ PTHREAD_ONCE_INIT
+ PTHREAD_MUTEX_INITIALIZER
+ PTHREAD_RECURSIVE_MUTEX_INITIALIZER
+ PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP
+ PTHREAD_ERRORCHECK_MUTEX_INITIALIZER
+ PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP
+ PTHREAD_COND_INITIALIZER
+ PTHREAD_RWLOCK_INITIALIZER
+ PTHREAD_SPINLOCK_INITIALIZER
+
+
+The library includes two non-API functions for creating cancellation
+points in applications and libraries:
+
+ pthreadCancelableWait
+ pthreadCancelableTimedWait
+
+
+The following functions are not implemented:
+
+ ---------------------------
+ RealTime Scheduling
+ ---------------------------
+ pthread_mutex_getprioceiling
+ pthread_mutex_setprioceiling
+ pthread_mutex_attr_getprioceiling
+ pthread_mutex_attr_getprotocol
+ pthread_mutex_attr_setprioceiling
+ pthread_mutex_attr_setprotocol
+
+ ---------------------------
+ Fork Handlers
+ ---------------------------
+ pthread_atfork
+
+ ---------------------------
+ Stdio
+ ---------------------------
+ flockfile
+ ftrylockfile
+ funlockfile
+ getc_unlocked
+ getchar_unlocked
+ putc_unlocked
+ putchar_unlocked
+
+ ---------------------------
+ Thread-Safe C Runtime Library
+ ---------------------------
+ readdir_r
+ getgrgid_r
+ getgrnam_r
+ getpwuid_r
+ getpwnam_r
+
+ ---------------------------
+ Signals
+ ---------------------------
+ sigtimedwait
+ sigwait
+ sigwaitinfo
+
+ ---------------------------
+ General
+ ---------------------------
+ sysconf
+
+ ---------------------------
+ Thread-Safe C Runtime Library (macros)
+ ---------------------------
+ strtok_r
+ asctime_r
+ ctime_r
+ gmtime_r
+ localtime_r
+ rand_r
+
+
+Availability
+------------
+
+The prebuilt DLL, export libs (for both MSVC and Mingw32), and the header
+files (pthread.h, semaphore.h, sched.h) are available along with the
+complete source code.
+
+The source code can be found at:
+
+ ftp://sources.redhat.com/pub/pthreads-win32
+
+and as individual source code files at
+
+ ftp://sources.redhat.com/pub/pthreads-win32/source
+
+The pre-built DLL, export libraries and include files can be found at:
+
+ ftp://sources.redhat.com/pub/pthreads-win32/dll-latest
+
+
+
+Mailing List
+------------
+
+There is a mailing list for discussing pthreads on Win32. To join,
+send email to:
+
+ pthreads-win32-subscribe at sourceware.cygnus.com
+
+
+Application Development Environments
+------------------------------------
+
+See the README file for more information.
+
+MSVC:
+MSVC using SEH works. Distribute pthreadVSE.dll with your application.
+MSVC using C++ EH works. Distribute pthreadVCE.dll with your application.
+MSVC using C setjmp/longjmp works. Distribute pthreadVC.dll with your application.
+
+
+Mingw32:
+See the FAQ, Questions 6 and 10.
+
+Mingw using C++ EH works. Distribute pthreadGCE.dll with your application.
+Mingw using C setjmp/longjmp works. Distribute pthreadGC.dll with your application.
+
+
+Cygwin: (http://sourceware.cygnus.com/cygwin/)
+Developers using Cygwin do not need pthreads-win32 since it has POSIX threads
+support. Refer to its documentation for details and extent.
+
+
+UWIN:
+UWIN is a complete Unix-like environment for Windows from AT&T. Pthreads-win32
+doesn't currently support UWIN (and vice versa), but that may change in the
+future.
+
+Generally:
+For convenience, the following pre-built files are available on the FTP site
+(see Availability above):
+
+ pthread.h - for POSIX threads
+ semaphore.h - for POSIX semaphores
+ sched.h - for POSIX scheduling
+ pthreadVCE.dll - built with MSVC++ compiler using C++ EH
+ pthreadVCE.lib
+ pthreadVC.dll - built with MSVC compiler using C setjmp/longjmp
+ pthreadVC.lib
+ pthreadVSE.dll - built with MSVC compiler using SEH
+ pthreadVSE.lib
+ pthreadGCE.dll - built with Mingw32 G++ 2.95.2-1
+ pthreadGC.dll - built with Mingw32 GCC 2.95.2-1 using setjmp/longjmp
+ libpthreadGCE.a - derived from pthreadGCE.dll
+ libpthreadGC.a - derived from pthreadGC.dll
+ gcc.dll - needed if distributing applications that use
+ pthreadGCE.dll (but see the FAQ Q 10 for the latest
+ related information)
+
+These are the only files you need in order to build POSIX threads
+applications for Win32 using either MSVC or Mingw32.
+
+See the FAQ file in the source tree for additional information.
+
+
+Documentation
+-------------
+
+For the authoritative reference, see the online POSIX
+standard reference at:
+
+ http://www.OpenGroup.org
+
+For POSIX Thread API programming, several reference books are
+available:
+
+ Programming with POSIX Threads
+ David R. Butenhof
+ Addison-Wesley (pub)
+
+ Pthreads Programming
+ By Bradford Nichols, Dick Buttlar & Jacqueline Proulx Farrell
+ O'Reilly (pub)
+
+On the web: see the links at the bottom of the pthreads-win32 site:
+
+ http://sources.redhat.com/pthreads-win32/
+
+ Currently, there is no documentation included in the package apart
+ from the copious comments in the source code.
+
+
+
+Enjoy!
+
+Ross Johnson
diff --git a/3rdparty/pthreads-win32/BUGS b/3rdparty/pthreads-win32/BUGS
new file mode 100644
index 0000000..285ba4e
--- /dev/null
+++ b/3rdparty/pthreads-win32/BUGS
@@ -0,0 +1,141 @@
+----------
+Known bugs
+----------
+
+1. Not strictly a bug, more of a gotcha.
+
+ Under MS VC++ (only tested with version 6.0), a term_func
+ set via the standard C++ set_terminate() function causes the
+ application to abort.
+
+ Notes from the MSVC++ manual:
+ 1) A term_func() should call exit(), otherwise
+ abort() will be called on return to the caller.
+ A call to abort() raises SIGABRT and the default signal handler
+ for all signals terminates the calling program with
+ exit code 3.
+ 2) A term_func() must not throw an exception. Therefore
+ term_func() should not call pthread_exit(), which
+ works by throwing an exception (pthreadVCE or pthreadVSE)
+ or by calling longjmp (pthreadVC).
+
+ Workaround: avoid using pthread_exit() in C++ applications. Exit
+ threads by dropping through the end of the thread routine.
+
+2. Cancellation problems in C++ builds
+ - Milan Gardian
+
+ [Note: It's not clear if this problem isn't simply due to the context
+ switch in pthread_cancel() which occurs unless the QueueUserAPCEx
+ library and driver are installed and used. Just like setjmp/longjmp,
+ this is probably not going to work well in C++. In any case, unless for
+ some very unusual reason you really must use the C++ build then please
+ use the C build pthreadVC2.dll or pthreadGC2.dll, i.e. for C++
+ applications.]
+
+ This is suspected to be a compiler bug in VC6.0, and also seen in
+ VC7.0 and VS .NET 2003. The GNU C++ compiler does not have a problem
+ with this, and it has been reported that the Intel C++ 8.1 compiler
+ and Visual C++ 2005 Express Edition Beta2 pass tests\semaphore4.c
+ (which exposes the bug).
+
+ Workaround [rpj - 2 Feb 2002]
+ -----------------------------
+ [Please note: this workaround did not solve a similar problem in
+ snapshot-2004-11-03 or later, even though similar symptoms were seen.
+ tests\semaphore4.c fails in that snapshot for the VCE version of the
+ DLL.]
+
+ The problem disappears when /Ob0 is used, i.e. /O2 /Ob0 works OK,
+ but if you want to use inlining optimisation you can be much more
+ specific about where it's switched off and on by using a pragma.
+
+ So the inlining optimisation is interfering with the way that cleanup
+ handlers are run. It appears to relate to auto-inlining of class methods
+ since this is the only auto inlining that is performed at /O1 optimisation
+ (functions with the "inline" qualifier are also inlined, but the problem
+ doesn't appear to involve any such functions in the library or testsuite).
+
+ In order to confirm the inlining culprit, the following use of pragmas
+ eliminate the problem but I don't know how to make it transparent, putting
+ it in, say, pthread.h where pthread_cleanup_push defined as a macro.
+
+ #pragma inline_depth(0)
+ pthread_cleanup_push(handlerFunc, (void *) &arg);
+
+ /* ... */
+
+ pthread_cleanup_pop(0);
+ #pragma inline_depth()
+
+ Note the empty () pragma value after the pop macro. This resets depth to the
+ default. Or you can specify a non-zero depth here.
+
+ The pragma is also needed (and now used) within the library itself wherever
+ cleanup handlers are used (condvar.c and rwlock.c).
+
+ Use of these pragmas allows compiler optimisations /O1 and /O2 to be
+ used for either or both the library and applications.
+
+ Experimenting further, I found that wrapping the actual cleanup handler
+ function with #pragma auto_inline(off|on) does NOT work.
+
+ MSVC6.0 doesn't appear to support the C99 standard's _Pragma directive,
+ however, later versions may. This form is embeddable inside #define
+ macros, which would be ideal because it would mean that it could be added
+ to the push/pop macro definitions in pthread.h and hidden from the
+ application programmer.
+
+ [/rpj]
+
+ Original problem description
+ ----------------------------
+
+ The cancellation (actually, cleanup-after-cancel) tests fail when using VC
+ (professional) optimisation switches (/O1 or /O2) in pthreads library. I
+ have not investigated which concrete optimisation technique causes this
+ problem (/Og, /Oi, /Ot, /Oy, /Ob1, /Gs, /Gf, /Gy, etc.), but here is a
+ summary of builds and corresponding failures:
+
+ * pthreads VSE (optimised tests): OK
+ * pthreads VCE (optimised tests): Failed "cleanup1" test (runtime)
+
+ * pthreads VSE (DLL in CRT, optimised tests): OK
+ * pthreads VCE (DLL in CRT, optimised tests): Failed "cleanup1" test
+ (runtime)
+
+ Please note that while in VSE version of the pthreads library the
+ optimisation does not really have any impact on the tests (they pass OK), in
+ VCE version addition of optimisation (/O2 in this case) causes the tests to
+ fail uniformly - either in "cleanup0" or "cleanup1" test cases.
+
+ Please note that all the tests above use default pthreads DLL (no
+ optimisations, linked with either static or DLL CRT, based on test type).
+ Therefore the problem lies not within the pthreads DLL but within the
+ compiled client code (the application using pthreads -> involvement of
+ "pthread.h").
+
+ I think the message of this section is that usage of VCE version of pthreads
+ in applications relying on cancellation/cleanup AND using optimisations for
+ creation of production code is highly unreliable for the current version of
+ the pthreads library.
+
+3. The Borland Builder 5.5 version of the library produces memory read exceptions
+in some tests.
+
+4. pthread_barrier_wait() can deadlock if the number of potential calling
+threads for a particular barrier is greater than the barrier count parameter
+given to pthread_barrier_init() for that barrier.
+
+This is due to the very lightweight implementation of pthread-win32 barriers.
+To cope with more than "count" possible waiters, barriers must effectively
+implement all the same safeguards as condition variables, making them much
+"heavier" than at present.
+
+The workaround is to ensure that no more than "count" threads attempt to wait
+at the barrier.
+
+5. Canceling a thread blocked on pthread_once appears not to work in the MSVC++
+version of the library "pthreadVCE.dll". The test case "once3.c" hangs. I have no
+clues on this at present. All other versions pass this test ok - pthreadsVC.dll,
+pthreadsVSE.dll, pthreadsGC.dll and pthreadsGCE.dll.
diff --git a/3rdparty/pthreads-win32/Bmakefile b/3rdparty/pthreads-win32/Bmakefile
new file mode 100644
index 0000000..ea25dec
--- /dev/null
+++ b/3rdparty/pthreads-win32/Bmakefile
@@ -0,0 +1,268 @@
+# This makefile is compatible with BCB make. Use "make -fBMakefile" to compile.
+#
+# The variables $DLLDEST and $LIBDEST hold the destination directories for the
+# dll and the lib, respectively. Probably all that needs to change is $DEVROOT.
+#
+# Currently only the recommended pthreadBC.dll is built by this makefile.
+#
+
+
+DLL_VER = 2
+
+DEVROOT = .
+
+DLLDEST = $(DEVROOT)\DLL
+LIBDEST = $(DEVROOT)\DLL
+
+DLLS = pthreadBC$(DLL_VER).dll
+
+OPTIM = /O2
+
+RC = brcc32
+RCFLAGS = -i.
+
+CFLAGS = /q /I. /D_WIN32_WINNT=0x400 /DHAVE_PTW32_CONFIG_H=1 /4 /tWD /tWM \
+ /w-aus /w-asc /w-par
+
+#C cleanup code
+BCFLAGS = $(PTW32_FLAGS) $(CFLAGS)
+
+# Agregate modules for inlinability
+DLL_OBJS = \
+ attr.obj \
+ barrier.obj \
+ cancel.obj \
+ cleanup.obj \
+ condvar.obj \
+ create.obj \
+ dll.obj \
+ errno.obj \
+ exit.obj \
+ fork.obj \
+ global.obj \
+ misc.obj \
+ mutex.obj \
+ nonportable.obj \
+ private.obj \
+ rwlock.obj \
+ sched.obj \
+ semaphore.obj \
+ signal.obj \
+ spin.obj \
+ sync.obj \
+ tsd.obj
+
+INCL = config.h implement.h semaphore.h pthread.h need_errno.h
+
+ATTR_SRCS = \
+ pthread_attr_init.c \
+ pthread_attr_destroy.c \
+ pthread_attr_getdetachstate.c \
+ pthread_attr_setdetachstate.c \
+ pthread_attr_getstackaddr.c \
+ pthread_attr_setstackaddr.c \
+ pthread_attr_getstacksize.c \
+ pthread_attr_setstacksize.c \
+ pthread_attr_getscope.c \
+ pthread_attr_setscope.c
+
+BARRIER_SRCS = \
+ pthread_barrier_init.c \
+ pthread_barrier_destroy.c \
+ pthread_barrier_wait.c \
+ pthread_barrierattr_init.c \
+ pthread_barrierattr_destroy.c \
+ pthread_barrierattr_setpshared.c \
+ pthread_barrierattr_getpshared.c
+
+CANCEL_SRCS = \
+ pthread_setcancelstate.c \
+ pthread_setcanceltype.c \
+ pthread_testcancel.c \
+ pthread_cancel.c
+
+CONDVAR_SRCS = \
+ ptw32_cond_check_need_init.c \
+ pthread_condattr_destroy.c \
+ pthread_condattr_getpshared.c \
+ pthread_condattr_init.c \
+ pthread_condattr_setpshared.c \
+ pthread_cond_destroy.c \
+ pthread_cond_init.c \
+ pthread_cond_signal.c \
+ pthread_cond_wait.c
+
+EXIT_SRCS = \
+ pthread_exit.c
+
+MISC_SRCS = \
+ pthread_equal.c \
+ pthread_getconcurrency.c \
+ pthread_once.c \
+ pthread_self.c \
+ pthread_setconcurrency.c \
+ ptw32_calloc.c \
+ ptw32_MCS_lock.c \
+ ptw32_new.c \
+ w32_CancelableWait.c
+
+MUTEX_SRCS = \
+ ptw32_mutex_check_need_init.c \
+ pthread_mutex_init.c \
+ pthread_mutex_destroy.c \
+ pthread_mutexattr_init.c \
+ pthread_mutexattr_destroy.c \
+ pthread_mutexattr_getpshared.c \
+ pthread_mutexattr_setpshared.c \
+ pthread_mutexattr_settype.c \
+ pthread_mutexattr_gettype.c \
+ pthread_mutexattr_setrobust.c \
+ pthread_mutexattr_getrobust.c \
+ pthread_mutex_lock.c \
+ pthread_mutex_timedlock.c \
+ pthread_mutex_unlock.c \
+ pthread_mutex_trylock.c \
+ pthread_mutex_consistent.c
+
+NONPORTABLE_SRCS = \
+ pthread_mutexattr_setkind_np.c \
+ pthread_mutexattr_getkind_np.c \
+ pthread_getw32threadhandle_np.c \
+ pthread_delay_np.c \
+ pthread_num_processors_np.c \
+ pthread_win32_attach_detach_np.c \
+ pthread_timechange_handler_np.c
+
+PRIVATE_SRCS = \
+ ptw32_is_attr.c \
+ ptw32_processInitialize.c \
+ ptw32_processTerminate.c \
+ ptw32_threadStart.c \
+ ptw32_threadDestroy.c \
+ ptw32_tkAssocCreate.c \
+ ptw32_tkAssocDestroy.c \
+ ptw32_callUserDestroyRoutines.c \
+ ptw32_timespec.c \
+ ptw32_relmillisecs.c \
+ ptw32_throw.c \
+ ptw32_getprocessors.c
+
+RWLOCK_SRCS = \
+ ptw32_rwlock_check_need_init.c \
+ ptw32_rwlock_cancelwrwait.c \
+ pthread_rwlock_init.c \
+ pthread_rwlock_destroy.c \
+ pthread_rwlockattr_init.c \
+ pthread_rwlockattr_destroy.c \
+ pthread_rwlockattr_getpshared.c \
+ pthread_rwlockattr_setpshared.c \
+ pthread_rwlock_rdlock.c \
+ pthread_rwlock_timedrdlock.c \
+ pthread_rwlock_wrlock.c \
+ pthread_rwlock_timedwrlock.c \
+ pthread_rwlock_unlock.c \
+ pthread_rwlock_tryrdlock.c \
+ pthread_rwlock_trywrlock.c
+
+SCHED_SRCS = \
+ pthread_attr_setschedpolicy.c \
+ pthread_attr_getschedpolicy.c \
+ pthread_attr_setschedparam.c \
+ pthread_attr_getschedparam.c \
+ pthread_attr_setinheritsched.c \
+ pthread_attr_getinheritsched.c \
+ pthread_setschedparam.c \
+ pthread_getschedparam.c \
+ sched_get_priority_max.c \
+ sched_get_priority_min.c \
+ sched_setscheduler.c \
+ sched_getscheduler.c \
+ sched_yield.c
+
+SEMAPHORE_SRCS = \
+ sem_init.c \
+ sem_destroy.c \
+ sem_trywait.c \
+ sem_timedwait.c \
+ sem_wait.c \
+ sem_post.c \
+ sem_post_multiple.c \
+ sem_getvalue.c \
+ sem_open.c \
+ sem_close.c \
+ sem_unlink.c
+
+SPIN_SRCS = \
+ ptw32_spinlock_check_need_init.c \
+ pthread_spin_init.c \
+ pthread_spin_destroy.c \
+ pthread_spin_lock.c \
+ pthread_spin_unlock.c \
+ pthread_spin_trylock.c
+
+SYNC_SRCS = \
+ pthread_detach.c \
+ pthread_join.c
+
+TSD_SRCS = \
+ pthread_key_create.c \
+ pthread_key_delete.c \
+ pthread_setspecific.c \
+ pthread_getspecific.c
+
+
+all: clean $(DLLS)
+
+realclean: clean
+ if exist pthread*.dll del pthread*.dll
+ if exist pthread*.lib del pthread*.lib
+ if exist *.stamp del *.stamp
+
+clean:
+ if exist *.obj del *.obj
+ if exist *.ilk del *.ilk
+ if exist *.ilc del *.ilc
+ if exist *.ild del *.ild
+ if exist *.ilf del *.ilf
+ if exist *.ils del *.ils
+ if exist *.tds del *.tds
+ if exist *.pdb del *.pdb
+ if exist *.exp del *.exp
+ if exist *.map del *.map
+ if exist *.o del *.o
+ if exist *.i del *.i
+ if exist *.res del *.res
+
+
+install: $(DLLS)
+ copy pthread*.dll $(DLLDEST)
+ copy pthread*.lib $(LIBDEST)
+
+$(DLLS): $(DLL_OBJS) version.res
+ ilink32 /Tpd /Gi c0d32x.obj $(DLL_OBJS), \
+ $@, ,\
+ cw32mti.lib import32.lib, ,\
+ version.res
+
+.c.obj:
+ $(CC) $(OPTIM) $(BCFLAGS) -c $<
+
+.rc.res:
+ $(RC) $(RCFLAGS) $<
+
+attr.obj: attr.c $(ATTR_SRCS) $(INCL)
+barrier.obj: barrier.c $(BARRIER_SRCS) $(INCL)
+cancel.obj: cancel.c $(CANCEL_SRCS) $(INCL)
+condvar.obj: condvar.c $(CONDVAR_SRCS) $(INCL)
+exit.obj: exit.c $(EXIT_SRCS) $(INCL)
+misc.obj: misc.c $(MISC_SRCS) $(INCL)
+mutex.obj: mutex.c $(MUTEX_SRCS) $(INCL)
+nonportable.obj: nonportable.c $(NONPORTABLE_SRCS) $(INCL)
+private.obj: private.c $(PRIVATE_SRCS) $(INCL)
+rwlock.obj: rwlock.c $(RWLOCK_SRCS) $(INCL)
+sched.obj: sched.c $(SCHED_SRCS) $(INCL)
+semaphore.obj: semaphore.c $(SEMAPHORE_SRCS) $(INCL)
+spin.obj: spin.c $(SPIN_SRCS) $(INCL)
+sync.obj: sync.c $(SYNC_SRCS) $(INCL)
+tsd.obj: tsd.c $(TSD_SRCS) $(INCL)
+version.res: version.rc $(INCL)
diff --git a/3rdparty/pthreads-win32/CMakeLists.txt b/3rdparty/pthreads-win32/CMakeLists.txt
new file mode 100644
index 0000000..5b51534
--- /dev/null
+++ b/3rdparty/pthreads-win32/CMakeLists.txt
@@ -0,0 +1,191 @@
+project(${PTHREADS_LIBRARY})
+
+set(PTHREADS_MAJOR_VERSION 2 PARENT_SCOPE)
+set(PTHREADS_MINOR_VERSION 9 PARENT_SCOPE)
+set(PTHREADS_PATCH_VERSION 1 PARENT_SCOPE)
+
+set(PTHREAD_API
+ pthread.h
+ sched.h
+ semaphore.h)
+
+source_group(PTHREAD_API FILES ${PTHREAD_API})
+
+set(PTHREAD_HEADERS
+ config.h
+ implement.h
+ need_errno.h)
+
+set(PTHREAD_STATIC_SOURCES
+ pthread_attr_init.c
+ pthread_attr_destroy.c
+ pthread_attr_getdetachstate.c
+ pthread_attr_setdetachstate.c
+ pthread_attr_getstackaddr.c
+ pthread_attr_setstackaddr.c
+ pthread_attr_getstacksize.c
+ pthread_attr_setstacksize.c
+ pthread_attr_getscope.c
+ pthread_attr_setscope.c
+ pthread_attr_setschedpolicy.c
+ pthread_attr_getschedpolicy.c
+ pthread_attr_setschedparam.c
+ pthread_attr_getschedparam.c
+ pthread_attr_setinheritsched.c
+ pthread_attr_getinheritsched.c
+ pthread_barrier_init.c
+ pthread_barrier_destroy.c
+ pthread_barrier_wait.c
+ pthread_barrierattr_init.c
+ pthread_barrierattr_destroy.c
+ pthread_barrierattr_setpshared.c
+ pthread_barrierattr_getpshared.c
+ pthread_setcancelstate.c
+ pthread_setcanceltype.c
+ pthread_testcancel.c
+ pthread_cancel.c
+ cleanup.c
+ pthread_condattr_destroy.c
+ pthread_condattr_getpshared.c
+ pthread_condattr_init.c
+ pthread_condattr_setpshared.c
+ pthread_cond_destroy.c
+ pthread_cond_init.c
+ pthread_cond_signal.c
+ pthread_cond_wait.c
+ create.c
+ dll.c
+ autostatic.c
+ errno.c
+ pthread_exit.c
+ fork.c
+ global.c
+ pthread_mutex_init.c
+ pthread_mutex_destroy.c
+ pthread_mutexattr_init.c
+ pthread_mutexattr_destroy.c
+ pthread_mutexattr_getpshared.c
+ pthread_mutexattr_setpshared.c
+ pthread_mutexattr_settype.c
+ pthread_mutexattr_gettype.c
+ pthread_mutexattr_setrobust.c
+ pthread_mutexattr_getrobust.c
+ pthread_mutex_lock.c
+ pthread_mutex_timedlock.c
+ pthread_mutex_unlock.c
+ pthread_mutex_trylock.c
+ pthread_mutex_consistent.c
+ pthread_mutexattr_setkind_np.c
+ pthread_mutexattr_getkind_np.c
+ pthread_getw32threadhandle_np.c
+ pthread_getunique_np.c
+ pthread_delay_np.c
+ pthread_num_processors_np.c
+ pthread_win32_attach_detach_np.c
+ pthread_equal.c
+ pthread_getconcurrency.c
+ pthread_once.c
+ pthread_self.c
+ pthread_setconcurrency.c
+ pthread_rwlock_init.c
+ pthread_rwlock_destroy.c
+ pthread_rwlockattr_init.c
+ pthread_rwlockattr_destroy.c
+ pthread_rwlockattr_getpshared.c
+ pthread_rwlockattr_setpshared.c
+ pthread_rwlock_rdlock.c
+ pthread_rwlock_wrlock.c
+ pthread_rwlock_unlock.c
+ pthread_rwlock_tryrdlock.c
+ pthread_rwlock_trywrlock.c
+ pthread_setschedparam.c
+ pthread_getschedparam.c
+ pthread_timechange_handler_np.c
+ ptw32_is_attr.c
+ ptw32_processInitialize.c
+ ptw32_processTerminate.c
+ ptw32_threadStart.c
+ ptw32_threadDestroy.c
+ ptw32_tkAssocCreate.c
+ ptw32_tkAssocDestroy.c
+ ptw32_callUserDestroyRoutines.c
+ ptw32_timespec.c
+ ptw32_throw.c
+ ptw32_getprocessors.c
+ ptw32_calloc.c
+ ptw32_new.c
+ ptw32_reuse.c
+ ptw32_rwlock_check_need_init.c
+ ptw32_cond_check_need_init.c
+ ptw32_mutex_check_need_init.c
+ ptw32_semwait.c
+ ptw32_relmillisecs.c
+ ptw32_MCS_lock.c
+ sched_get_priority_max.c
+ sched_get_priority_min.c
+ sched_setscheduler.c
+ sched_getscheduler.c
+ sched_yield.c
+ sem_init.c
+ sem_destroy.c
+ sem_trywait.c
+ sem_timedwait.c
+ sem_wait.c
+ sem_post.c
+ sem_post_multiple.c
+ sem_getvalue.c
+ sem_open.c
+ sem_close.c
+ sem_unlink.c
+ signal.c
+ pthread_kill.c
+ ptw32_spinlock_check_need_init.c
+ pthread_spin_init.c
+ pthread_spin_destroy.c
+ pthread_spin_lock.c
+ pthread_spin_unlock.c
+ pthread_spin_trylock.c
+ pthread_detach.c
+ pthread_join.c
+ pthread_key_create.c
+ pthread_key_delete.c
+ pthread_setspecific.c
+ pthread_getspecific.c
+ w32_CancelableWait.c)
+
+add_definitions( -DPTW32_BUILD_INLINED )
+
+vp_include_directories(${CMAKE_CURRENT_SOURCE_DIR})
+add_library(${PTHREADS_LIBRARY} STATIC ${PTHREAD_STATIC_SOURCES} ${PTHREAD_API} ${PTHREAD_HEADERS})
+
+if(CMAKE_COMPILER_IS_GNUCXX AND UNIX)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC")
+endif()
+
+#set_target_properties(${PTHREADS_LIBRARY} PROPERTIES PUBLIC_HEADER "${PTHREAD_API}")
+if(ENABLE_SOLUTION_FOLDERS)
+ set_target_properties(${PTHREADS_LIBRARY} PROPERTIES FOLDER "3rdparty")
+endif()
+
+set_target_properties(${PTHREADS_LIBRARY}
+ PROPERTIES OUTPUT_NAME ${PTHREADS_LIBRARY}
+ DEBUG_POSTFIX "${VISP_DEBUG_POSTFIX}"
+ COMPILE_PDB_NAME ${PTHREADS_LIBRARY}
+ COMPILE_PDB_NAME_DEBUG "${PTHREADS_LIBRARY}${VISP_DEBUG_POSTFIX}"
+ ARCHIVE_OUTPUT_DIRECTORY ${VISP_3P_LIBRARY_OUTPUT_PATH}
+ )
+target_compile_definitions(${PTHREADS_LIBRARY} PUBLIC PTW32_STATIC_LIB HAVE_PTW32_CONFIG_H)
+target_include_directories(${PTHREADS_LIBRARY} PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>)
+#set_property(TARGET ${PTHREADS_LIBRARY} APPEND PROPERTY
+# INTERFACE_INCLUDE_DIRECTORIES ${CMAKE_CURRENT_SOURCE_DIR})
+
+if(NOT BUILD_SHARED_LIBS)
+ vp_install_target(${PTHREADS_LIBRARY} EXPORT VISPModules ARCHIVE DESTINATION ${VISP_3P_LIB_INSTALL_PATH} COMPONENT dev)
+endif()
+
+if(MSVC)
+ vp_set_source_file_compile_flag(pthread_spin_lock.c /wd"4013")
+ vp_set_source_file_compile_flag(pthread_spin_trylock.c /wd"4013")
+ vp_set_source_file_compile_flag(pthread_win32_attach_detach_np.c /wd"4101")
+ vp_set_source_file_compile_flag(ptw32_getprocessors.c /wd"4013")
+endif()
\ No newline at end of file
diff --git a/3rdparty/pthreads-win32/CONTRIBUTORS b/3rdparty/pthreads-win32/CONTRIBUTORS
new file mode 100644
index 0000000..da31ff2
--- /dev/null
+++ b/3rdparty/pthreads-win32/CONTRIBUTORS
@@ -0,0 +1,140 @@
+Contributors (in approximate order of appearance)
+
+[See also the ChangeLog file where individuals are
+attributed in log entries. Likewise in the FAQ file.]
+
+Ben Elliston bje at cygnus dot com
+ Initiated the project;
+ setup the project infrastructure (CVS, web page, etc.);
+ early prototype routines.
+Ross Johnson Ross dot Johnson at dot homemail dot com dot au
+ early prototype routines;
+ ongoing project coordination/maintenance;
+ implementation of spin locks and barriers;
+ various enhancements;
+ bug fixes;
+ documentation;
+ testsuite.
+Robert Colquhoun rjc at trump dot net dot au
+ Early bug fixes.
+John E. Bossom John dot Bossom at cognos dot com
+ Contributed substantial original working implementation;
+ bug fixes;
+ ongoing guidance and standards interpretation.
+Anders Norlander anorland at hem2 dot passagen dot se
+ Early enhancements and runtime checking for supported
+ Win32 routines.
+Tor Lillqvist tml at iki dot fi
+ General enhancements;
+ early bug fixes to condition variables.
+Scott Lightner scott at curriculum dot com
+ Bug fix.
+Kevin Ruland Kevin dot Ruland at anheuser-busch dot com
+ Various bug fixes.
+Mike Russo miker at eai dot com
+ Bug fix.
+Mark E. Armstrong avail at pacbell dot net
+ Bug fixes.
+Lorin Hochstein lmh at xiphos dot ca
+ general bug fixes; bug fixes to condition variables.
+Peter Slacik Peter dot Slacik at tatramed dot sk
+ Bug fixes.
+Mumit Khan khan at xraylith dot wisc dot edu
+ Fixes to work with Mingw32.
+Milan Gardian mg at tatramed dot sk
+ Bug fixes and reports/analyses of obscure problems.
+Aurelio Medina aureliom at crt dot com
+ First implementation of read-write locks.
+Graham Dumpleton Graham dot Dumpleton at ra dot pad dot otc dot telstra dot com dot au
+ Bug fix in condition variables.
+Tristan Savatier tristan at mpegtv dot com
+ WinCE port.
+Erik Hensema erik at hensema dot xs4all dot nl
+ Bug fixes.
+Rich Peters rpeters at micro-magic dot com
+Todd Owen towen at lucidcalm dot dropbear dot id dot au
+ Bug fixes to dll loading.
+Jason Nye jnye at nbnet dot nb dot ca
+ Implementation of async cancelation.
+Fred Forester fforest at eticomm dot net
+Kevin D. Clark kclark at cabletron dot com
+David Baggett dmb at itasoftware dot com
+ Bug fixes.
+Paul Redondo paul at matchvision dot com
+Scott McCaskill scott at 3dfx dot com
+ Bug fixes.
+Jef Gearhart jgearhart at tpssys dot com
+ Bug fix.
+Arthur Kantor akantor at bexusa dot com
+ Mutex enhancements.
+Steven Reddie smr at essemer dot com dot au
+ Bug fix.
+Alexander Terekhov TEREKHOV at de dot ibm dot com
+ Re-implemented and improved read-write locks;
+ (with Louis Thomas) re-implemented and improved
+ condition variables;
+ enhancements to semaphores;
+ enhancements to mutexes;
+ new mutex implementation in 'futex' style;
+ suggested a robust implementation of pthread_once
+ similar to that implemented by V.Kliathcko;
+ system clock change handling re CV timeouts;
+ bug fixes.
+Thomas Pfaff tpfaff at gmx dot net
+ Changes to make C version usable with C++ applications;
+ re-implemented mutex routines to avoid Win32 mutexes
+ and TryEnterCriticalSection;
+ procedure to fix Mingw32 thread-safety issues.
+Franco Bez franco dot bez at gmx dot de
+ procedure to fix Mingw32 thread-safety issues.
+Louis Thomas lthomas at arbitrade dot com
+ (with Alexander Terekhov) re-implemented and improved
+ condition variables.
+David Korn dgk at research dot att dot com
+ Ported to UWIN.
+Phil Frisbie, Jr. phil at hawksoft dot com
+ Bug fix.
+Ralf Brese Ralf dot Brese at pdb4 dot siemens dot de
+ Bug fix.
+prionx at juno dot com prionx at juno dot com
+ Bug fixes.
+Max Woodbury mtew at cds dot duke dot edu
+ POSIX versioning conditionals;
+ reduced namespace pollution;
+ idea to separate routines to reduce statically
+ linked image sizes.
+Rob Fanner rfanner at stonethree dot com
+ Bug fix.
+Michael Johnson michaelj at maine dot rr dot com
+ Bug fix.
+Nicolas Barry boozai at yahoo dot com
+ Bug fixes.
+Piet van Bruggen pietvb at newbridges dot nl
+ Bug fix.
+Makoto Kato raven at oldskool dot jp
+ AMD64 port.
+Panagiotis E. Hadjidoukas peh at hpclab dot ceid dot upatras dot gr
+ phadjido at cs dot uoi dot gr
+ Contributed the QueueUserAPCEx package which
+ makes preemptive async cancelation possible.
+Will Bryant will dot bryant at ecosm dot com
+ Borland compiler patch and makefile.
+Anuj Goyal anuj dot goyal at gmail dot com
+ Port to Digital Mars compiler.
+Gottlob Frege gottlobfrege at gmail dot com
+ re-implemented pthread_once (version 2)
+ (pthread_once cancellation added by rpj).
+Vladimir Kliatchko vladimir at kliatchko dot com
+ reimplemented pthread_once with the same form
+ as described by A.Terekhov (later version 2);
+ implementation of MCS (Mellor-Crummey/Scott) locks.
+Ramiro Polla ramiro.polla at gmail dot com
+ static library auto init/cleanup on application
+ start/exit via RT hooks (MSC and GCC compilers only).
+Daniel Richard G. skunk at iSKUNK dot org
+ Patches and cleanups for x86 and x64, particularly
+ across a range of MS build environments.
+John Kamp john dot kamp at globalgraphics dot com
+ Patches to fix various problems on x64; brutal testing
+ particularly using high memory run environments.
+
diff --git a/3rdparty/pthreads-win32/COPYING b/3rdparty/pthreads-win32/COPYING
new file mode 100644
index 0000000..5cfea0d
--- /dev/null
+++ b/3rdparty/pthreads-win32/COPYING
@@ -0,0 +1,150 @@
+ pthreads-win32 - a POSIX threads library for Microsoft Windows
+
+
+This file is Copyrighted
+------------------------
+
+ This file is covered under the following Copyright:
+
+ Copyright (C) 2001,2006 Ross P. Johnson
+ All rights reserved.
+
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+Pthreads-win32 is covered by the GNU Lesser General Public License
+------------------------------------------------------------------
+
+ Pthreads-win32 is open software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public License
+ as published by the Free Software Foundation version 2.1 of the
+ License.
+
+ Pthreads-win32 is several binary link libraries, several modules,
+ associated interface definition files and scripts used to control
+ its compilation and installation.
+
+ Pthreads-win32 is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU Lesser General Public License for more details.
+
+ A copy of the GNU Lesser General Public License is distributed with
+ pthreads-win32 under the filename:
+
+ COPYING.LIB
+
+ You should have received a copy of the version 2.1 GNU Lesser General
+ Public License with pthreads-win32; if not, write to:
+
+ Free Software Foundation, Inc.
+ 59 Temple Place
+ Suite 330
+ Boston, MA 02111-1307
+ USA
+
+ The contact addresses for pthreads-win32 is as follows:
+
+ Web: http://sources.redhat.com/pthreads-win32
+ Email: Ross Johnson
+ Please use: Firstname.Lastname at homemail.com.au
+
+
+
+Pthreads-win32 copyrights and exception files
+---------------------------------------------
+
+ With the exception of the files listed below, Pthreads-win32
+ is covered under the following GNU Lesser General Public License
+ Copyrights:
+
+ Pthreads-win32 - POSIX Threads Library for Win32
+ Copyright(C) 1998 John E. Bossom
+ Copyright(C) 1999,2006 Pthreads-win32 contributors
+
+ The current list of contributors is contained
+ in the file CONTRIBUTORS included with the source
+ code distribution. The current list of CONTRIBUTORS
+ can also be seen at the following WWW location:
+ http://sources.redhat.com/pthreads-win32/contributors.html
+
+ Contact Email: Ross Johnson
+ Please use: Firstname.Lastname at homemail.com.au
+
+ These files are not covered under one of the Copyrights listed above:
+
+ COPYING
+ COPYING.LIB
+ tests/rwlock7.c
+
+ This file, COPYING, is distributed under the Copyright found at the
+ top of this file. It is important to note that you may distribute
+ verbatim copies of this file but you may not modify this file.
+
+ The file COPYING.LIB, which contains a copy of the version 2.1
+ GNU Lesser General Public License, is itself copyrighted by the
+ Free Software Foundation, Inc. Please note that the Free Software
+ Foundation, Inc. does NOT have a copyright over Pthreads-win32,
+ only the COPYING.LIB that is supplied with pthreads-win32.
+
+ The file tests/rwlock7.c is derived from code written by
+ Dave Butenhof for his book 'Programming With POSIX(R) Threads'.
+ The original code was obtained by free download from his website
+ http://home.earthlink.net/~anneart/family/Threads/source.html
+ and did not contain a copyright or author notice. It is assumed to
+ be freely distributable.
+
+ In all cases one may use and distribute these exception files freely.
+ And because one may freely distribute the LGPL covered files, the
+ entire pthreads-win32 source may be freely used and distributed.
+
+
+
+General Copyleft and License info
+---------------------------------
+
+ For general information on Copylefts, see:
+
+ http://www.gnu.org/copyleft/
+
+ For information on GNU Lesser General Public Licenses, see:
+
+ http://www.gnu.org/copyleft/lesser.html
+ http://www.gnu.org/copyleft/lesser.txt
+
+
+Why pthreads-win32 did not use the GNU General Public License
+-------------------------------------------------------------
+
+ The goal of the pthreads-win32 project has been to
+ provide a quality and complete implementation of the POSIX
+ threads API for Microsoft Windows within the limits imposed
+ by virtue of it being a stand-alone library and not
+ linked directly to other POSIX compliant libraries. For
+ example, some functions and features, such as those based
+ on POSIX signals, are missing.
+
+ Pthreads-win32 is a library, available in several different
+ versions depending on supported compilers, and may be used
+ as a dynamically linked module or a statically linked set of
+ binary modules. It is not an application on it's own.
+
+ It was fully intended that pthreads-win32 be usable with
+ commercial software not covered by either the GPL or the LGPL
+ licenses. Pthreads-win32 has many contributors to it's
+ code base, many of whom have done so because they have
+ used the library in commercial or proprietry software
+ projects.
+
+ Releasing pthreads-win32 under the LGPL ensures that the
+ library can be used widely, while at the same time ensures
+ that bug fixes and improvements to the pthreads-win32 code
+ itself is returned to benefit all current and future users
+ of the library.
+
+ Although pthreads-win32 makes it possible for applications
+ that use POSIX threads to be ported to Win32 platforms, the
+ broader goal of the project is to encourage the use of open
+ standards, and in particular, to make it just a little easier
+ for developers writing Win32 applications to consider
+ widening the potential market for their products.
diff --git a/3rdparty/pthreads-win32/COPYING.LIB b/3rdparty/pthreads-win32/COPYING.LIB
new file mode 100644
index 0000000..b1e3f5a
--- /dev/null
+++ b/3rdparty/pthreads-win32/COPYING.LIB
@@ -0,0 +1,504 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL. It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+ This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it. You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+ When we speak of free software, we are referring to freedom of use,
+not price. Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+ To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights. These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+ For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you. You must make sure that they, too, receive or can get the source
+code. If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it. And you must show them these terms so they know their rights.
+
+ We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+ To protect each distributor, we want to make it very clear that
+there is no warranty for the free library. Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+ Finally, software patents pose a constant threat to the existence of
+any free program. We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder. Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+ Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License. This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License. We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+ When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library. The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom. The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+ We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License. It also provides other free software developers Less
+of an advantage over competing non-free programs. These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries. However, the Lesser license provides advantages in certain
+special circumstances.
+
+ For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard. To achieve this, non-free programs must be
+allowed to use the library. A more frequent case is that a free
+library does the same job as widely used non-free libraries. In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+ In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software. For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+ Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+ The precise terms and conditions for copying, distribution and
+modification follow. Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library". The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+ GNU LESSER GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+ A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+ The "Library", below, refers to any such software library or work
+which has been distributed under these terms. A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language. (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+ "Source code" for a work means the preferred form of the work for
+making modifications to it. For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+ Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it). Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+
+ 1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+ You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+ 2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) The modified work must itself be a software library.
+
+ b) You must cause the files modified to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ c) You must cause the whole of the work to be licensed at no
+ charge to all third parties under the terms of this License.
+
+ d) If a facility in the modified Library refers to a function or a
+ table of data to be supplied by an application program that uses
+ the facility, other than as an argument passed when the facility
+ is invoked, then you must make a good faith effort to ensure that,
+ in the event an application does not supply such function or
+ table, the facility still operates, and performs whatever part of
+ its purpose remains meaningful.
+
+ (For example, a function in a library to compute square roots has
+ a purpose that is entirely well-defined independent of the
+ application. Therefore, Subsection 2d requires that any
+ application-supplied function or table used by this function must
+ be optional: if the application does not supply it, the square
+ root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library. To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License. (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.) Do not make any other change in
+these notices.
+
+ Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+ This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+ 4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+ If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library". Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+ However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library". The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+ When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library. The
+threshold for this to be true is not precisely defined by law.
+
+ If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work. (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+ Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+ 6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+ You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License. You must supply a copy of this License. If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License. Also, you must do one
+of these things:
+
+ a) Accompany the work with the complete corresponding
+ machine-readable source code for the Library including whatever
+ changes were used in the work (which must be distributed under
+ Sections 1 and 2 above); and, if the work is an executable linked
+ with the Library, with the complete machine-readable "work that
+ uses the Library", as object code and/or source code, so that the
+ user can modify the Library and then relink to produce a modified
+ executable containing the modified Library. (It is understood
+ that the user who changes the contents of definitions files in the
+ Library will not necessarily be able to recompile the application
+ to use the modified definitions.)
+
+ b) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (1) uses at run time a
+ copy of the library already present on the user's computer system,
+ rather than copying library functions into the executable, and (2)
+ will operate properly with a modified version of the library, if
+ the user installs one, as long as the modified version is
+ interface-compatible with the version that the work was made with.
+
+ c) Accompany the work with a written offer, valid for at
+ least three years, to give the same user the materials
+ specified in Subsection 6a, above, for a charge no more
+ than the cost of performing this distribution.
+
+ d) If distribution of the work is made by offering access to copy
+ from a designated place, offer equivalent access to copy the above
+ specified materials from the same place.
+
+ e) Verify that the user has already received a copy of these
+ materials or that you have already sent this user a copy.
+
+ For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it. However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+ It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system. Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+ 7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+ a) Accompany the combined library with a copy of the same work
+ based on the Library, uncombined with any other library
+ facilities. This must be distributed under the terms of the
+ Sections above.
+
+ b) Give prominent notice with the combined library of the fact
+ that part of it is a work based on the Library, and explaining
+ where to find the accompanying uncombined form of the same work.
+
+ 8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License. Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License. However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+ 9. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Library or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+ 10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+ 11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all. For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded. In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+ 13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation. If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+ 14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission. For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this. Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+ NO WARRANTY
+
+ 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Libraries
+
+ If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change. You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+ To apply these terms, attach the following notices to the library. It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the library's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This library is free software; you can redistribute it and/or
+ modify it under the terms of the GNU Lesser General Public
+ License as published by the Free Software Foundation; either
+ version 2.1 of the License, or (at your option) any later version.
+
+ This library is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ Lesser General Public License for more details.
+
+ You should have received a copy of the GNU Lesser General Public
+ License along with this library; if not, write to the Free Software
+ Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the
+ library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+ <signature of Ty Coon>, 1 April 1990
+ Ty Coon, President of Vice
+
+That's all there is to it!
+
+
diff --git a/3rdparty/pthreads-win32/ChangeLog b/3rdparty/pthreads-win32/ChangeLog
new file mode 100644
index 0000000..42abcc4
--- /dev/null
+++ b/3rdparty/pthreads-win32/ChangeLog
@@ -0,0 +1,5211 @@
+2012-03-18 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * create.c (pthread_create): add __cdecl attribute to thread routine
+ arg
+ * implement.h (pthread_key_t): add __cdecl attribute to destructor
+ element
+ (ThreadParms): likewise for start element
+ * pthread.h (pthread_create): add __cdecl to prototype start arg
+ (pthread_once): likewise for init_routine arg
+ (pthread_key_create): likewise for destructor arg
+ (ptw32_cleanup_push): replace type of routine arg with previously
+ defined ptw32_cleanup_callback_t
+ * pthread_key_create.c: add __cdecl attribute to destructor arg
+ * pthread_once.c: add __cdecl attribute to init_routine arg
+ * ptw32_threadStart.c (start): add __cdecl to start variable type
+
+
+2011-07-06 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * pthread_cond_wait.c (pragma inline_depth): this is almost redundant
+ now nevertheless fixed thei controlling MSC_VER from "< 800" to
+ "< 1400" (i.e. any prior to VC++ 8.0).
+ * pthread_once.ci (pragma inline_depth): Likewise.
+ * pthread_rwlock_timedwrlock.ci (pragma inline_depth): Likewise.
+ * pthread_rwlock_wrlock.ci (pragma inline_depth): Likewise.
+ * sem_timedwait.ci (pragma inline_depth): Likewise.
+ * sem_wait.ci (pragma inline_depth): Likewise.
+
+2011-07-05 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * pthread_win32_attach_detach_np.c: Use strncat_s if available
+ to removei a compile warning; MingW supports this routine but we
+ continue to use strncat anyway there because it is secure if
+ given the correct parameters; fix strncat param 3 to avoid
+ buffer overrun exploitation potential.
+
+2011-07-03 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * pthread_spin_unlock.c (EPERM): Return success if unlocking a lock
+ that is not locked, because single CPU machines wrap a
+ PTHREAD_MUTEX_NORMAL mutex, which returns success in this case.
+ * pthread_win32_attach_detach_np.c (QUSEREX.DLL): Load from an
+ absolute path only which must be the Windows System folder.
+
+2011-07-03 Daniel Richard G. <skunk at iskunk dot org>
+
+ * Makefile (_WIN32_WINNT): Removed; duplicate definition in
+ implement.h; more cleanup and enhancements.
+
+2011-07-02 Daniel Richard G. <skunk at iskunk dot org>
+
+ * Makefile: Cleanups and implovements.
+ * ptw32_MCS_locks.c: Casting fixes.
+ * implement.h: Interlocked call and argument casting macro fixes
+ to support older and newer build environments.
+
+2011-07-01 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * *.[ch] (PTW32_INTERLOCKED_*): Redo 23 and 64 bit versions of these
+ macros and re-apply in code to undo the incorrect changes from
+ 2011-06-29; remove some size_t casts which should not be required
+ and may be problematic.a
+ There are now two sets of macros:
+ PTW32_INTERLOCKED_*_LONG which work only on 32 bit integer variables;
+ PTW32_INTERLOCKED_*_SIZE which work on size_t integer variables, i.e.
+ LONG for 32 bit systems and LONGLONG for 64 bit systems.
+ * implement.h (MCS locks): nextFlag and waitFlag are now HANDLE type.
+ * ptw32_MCS_locks.c: Likewise.
+ * pthread.h (#include <setjmp.h>): Removed.
+ * ptw32_throw.c (#include <setjmp.h>): Added.
+ * ptw32_threadStart.c (#include <setjmp.h>): Added.
+ * implement.h (#include <setjmp.h>): Added.
+
+2011-06-30 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * pthread_once.c: Tighten 'if' statement casting; fix interlocked
+ pointer cast for 64 bit compatibility (missed yesterday); remove
+ the superfluous static cleanup routine and call the release routine
+ directly if popped.
+ * create.c (stackSize): Now type size_t.
+ * pthread.h (struct ptw32_thread_t_): Rearrange to fix element alignments.
+
+2011-06-29 Daniel Richard G. <skunk at iskunk dot org>
+
+ * ptw32_relmillisecs.c (ftime):
+ _ftime64_s() is only available in MSVC 2005 or later;
+ _ftime64() is available in MinGW or MSVC 2002 or later;
+ _ftime() is always available.
+ * pthread.h (long long): Not defined in older MSVC 6.
+ * implement.h (long long): Likewise.
+ * pthread_getunique_np.c (long long): Likewise.
+
+2011-06-29 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * *.[ch] (PTW32_INTERLOCKED_*): These macros should now work for
+ both 32 and 64 bit builds. The MingW versions are all inlined asm
+ while the MSVC versions expand to their Interlocked* or Interlocked*64
+ counterparts appropriately. The argument type have also been changed
+ to cast to the appropriate value or pointer size for the architecture.
+
+2011-05-29 Ross Johnson <ross dot johnson at homemail dot com dot au>
+
+ * *.[ch] (#ifdef): Extended cleanup to whole project.
+
+2011-05-29 Daniel Richard G. <skunk at iskunk dot org>
+
+ * Makefile (CC): Define CC to allow use of other compatible
+ compilers such as the Intel compilter icl.
+ * implement.h (#if): Fix forms like #if HAVE_SOMETHING.
+ * pthread.h: Likewise.
+ * sched.h: Likewise; PTW32_LEVEL_* becomes PTW32_SCHED_LEVEL_*.
+ * semaphore.h: Likewise.
+
+2011-05-11 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * ptw32_callUserDestroyRoutines.c (terminate): Altered includes
+ to match ptw32_threadStart.c.
+ * GNUmakefile (GCE-inlined-debug, DOPT): Fixed.
+
+2011-04-31 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * (robust mutexes): Added this API. The API is not
+ mandatory for implementations that don't support PROCESS_SHARED
+ mutexes, nevertheless it was considered useful both functionally
+ and for source-level compatibility.
+
+2011-03-26 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * pthread_getunique_np.c: New non-POSIX interface for compatibility
+ with some other implementations; returns a 64 bit sequence number
+ that is unique to each thread in the process.
+ * pthread.h (pthread_getunique_np): Added.
+ * global.c: Add global sequence counter for above.
+ * implement.h: Likewise.
+
+2011-03-25 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * (cancelLock): Convert to an MCS lock and rename to stateLock.
+ * (threadLock): Likewise.
+ * (keyLock): Likewise.
+ * pthread_mutex*.c: First working robust mutexes.
+
+2011-03-11 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * implement.h (PTW32_INTERLOCKED_*CREMENT macros): increment/decrement
+ using ++/-- instead of add/subtract 1.
+ * ptw32_MCS_lock.c: Make casts consistent.
+
+2011-03-09 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * implement.h (ptw32_thread_t_): Add process unique sequence number.
+ * global.c: Replace global Critical Section objects with MCS
+ queue locks.
+ * implement.h: Likewise.
+ * pthread_cond_destroy.c: Likewise.
+ * pthread_cond_init.c: Likewise.
+ * pthread_detach.c: Likewise.
+ * pthread_join.c: Likewise.
+ * pthread_kill.c: Likewise.
+ * pthread_mutex_destroy.c: Likewise.
+ * pthread_rwlock_destroy.c: Likewise.
+ * pthread_spin_destroy.c: Likewise.
+ * pthread_timechange_handler_np.c: Likewise.
+ * ptw32_cond_check_need_init.c: Likewise.
+ * ptw32_mutex_check_need_init.c: Likewise.
+ * ptw32_processInitialize.c: Likewise.
+ * ptw32_processTerminate.c: Likewise.
+ * ptw32_reuse.c: Likewise.
+ * ptw32_rwlock_check_need_init.c: Likewise.
+ * ptw32_spinlock_check_need_init.c: Likewise.
+
+2011-03-06 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * several (MINGW64): Cast and call fixups for 64 bit compatibility;
+ clean build via x86_64-w64-mingw32 cross toolchain on Linux i686
+ targeting x86_64 win64.
+ * ptw32_threadStart.c (ptw32_threadStart): Routine no longer attempts
+ to pass [unexpected C++] exceptions out of scope but ends the thread
+ normally setting EINTR as the exit status.
+ * ptw32_throw.c: Fix C++ exception throwing warnings; ignore
+ informational warning.
+ * implement.h: Likewise with the corresponding header definition.
+
+2011-03-04 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * implement.h (PTW32_INTERLOCKED_*): Mingw32 does not provide
+ the __sync_* intrinsics so implemented them here as macro
+ assembler routines. MSVS Interlocked* are emmitted as intrinsics
+ wherever possible, so we want mingw to match it; Extended to
+ include all interlocked routines used by the library; implemented
+ x86_64 versions also.
+ * ptw32_InterlockedCompareExchange.c: No code remaining here.
+ * ptw32_MCS_lock.c: Converted interlocked calls to use new macros.
+ * pthread_barrier_wait.c: Likewise.
+ * pthread_once.c: Likewise.
+ * ptw32_MCS_lock.c (ptw32_mcs_node_substitute): Name changed to
+ ptw32_mcs_node_transfer.
+
+2011-02-28 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * ptw32_relmillisecs.c: If possible, use _ftime64_s or _ftime64
+ before resorting to _ftime.
+
+2011-02-27 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * sched_setscheduler.c: Ensure the handle is closed after use.
+ * sched_getscheduler.c: Likewise.
+ * pthread.h: Remove POSIX compatibility macros; don't define
+ timespec if already defined.
+ * context.h: Changes for 64 bit.
+ * pthread_cancel.c: Likewise.
+ * pthread_exit.c: Likewise.
+ * pthread_spin_destroy.c: Likewise.
+ * pthread_timechange_handler_np.c: Likewise.
+ * ptw32_MCS_lock.c: Likewise; some of these changes may
+ not be compatible with pre Windows 2000 systems; reverse the order of
+ the includes.
+ * ptw32_threadStart.c: Likewise.
+ * ptw32_throw.c: Likewise.
+
+2011-02-13 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * pthread_self: Add comment re returning 'nil' value to
+ indicate failure only to win32 threads that call us.
+ * pthread_attr_setstackaddr: Fix comments; note this
+ function and it's compliment are now removed from SUSv4.
+
+2011-02-12 Ross Johnson <ross.johnson at homemail.com.au>
+
+ README.NONPORTABLE: Record a description of an obvious
+ method for nulling/comparing/hashing pthread_t using a
+ union; plus and investigation of a change of type for
+ pthread_t (to a union) to neutralise any padding bits and
+ bytes if they occur in pthread_t (the current pthread_t struct
+ does not contain padding AFAIK, but porting the library to a
+ future architecture may introduce them). Padding affects
+ byte-by-byte copies and compare operations.
+
+2010-11-16 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * ChangeLog: Add this entry ;-)
+ Restore entries from 2007 through 2009 that went missing
+ at the last update.
+
+2010-06-19 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * ptw32_MCS_lock.c (ptw32_mcs_node_substitute): Fix variable
+ names to avoid using C++ keyword ("new").
+ * implement.h (ptw32_mcs_node_substitute): Likewise.
+ * pthread_barrier_wait.c: Fix signed/unsigned comparison warning.
+
+2010-06-18 Ramiro Polla <ramiro.polla at gmail.com >
+
+ * autostatic.c: New file; call pthread_win32_process_*()
+ libary init/cleanup routines automatically on application start
+ when statically linked.
+ * pthread.c (autostatic.c): Included.
+ * pthread.h (declspec): Remove import/export defines if compiler
+ is MINGW.
+ * sched.h (declspec): Likewise.
+ * semaphore.h (declspec): Likewise.
+ * need_errno.h (declspec): Likewise.
+ * Makefile (autostatic.obj): Add for small static builds.
+ * GNUmakefile (autostatic.o): Likewise.
+ * NEWS (Version 2.9.0): Add changes.
+ * README.NONPORTABLE (pthread_win32_process_*): Update
+ description.
+
+2010-06-15 Ramiro Polla <ramiro.polla at gmail.com >
+
+ * Makefile: Remove linkage with the winsock library by default.
+ * GNUmakefile: Likewise.
+ * pthread_getspecific.c: Likewise by removing calls to WSA
+ functions.
+ * config.h (RETAIN_WSALASTERROR): Can be defined if necessary.
+
+2010-01-26 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * ptw32_MCS_lock.c (ptw32_mcs_node_substitute): New routine
+ to allow relocating the lock owners thread-local node to somewhere
+ else, e.g. to global space so that another thread can release the
+ lock. Used in pthread_barrier_wait.
+ (ptw32_mcs_lock_try_acquire): New routine.
+ * pthread_barrier_init: Only one semaphore is used now.
+ * pthread_barrier_wait: Added an MCS guard lock with the last thread
+ to leave the barrier releasing the lock. This removes a deadlock bug
+ observed when there are greater than barrier-count threads
+ attempting to cross.
+ * pthread_barrier_destroy: Added an MCS guard lock.
+
+2009-03-03 Stephan O'Farrill <stephan dot ofarrill at gmail dot com>
+
+ * pthread_attr_getschedpolicy.c: Add "const" to function parameter
+ in accordance with SUSv3 (POSIX).
+ * pthread_attr_getinheritsched.c: Likewise.
+ * pthread_mutexattr_gettype.c: Likewise.
+
+2008-06-06 Robert Kindred <RKindred at SwRI dot edu>
+
+ * ptw32_throw.c (ptw32_throw): Remove possible reference to NULL
+ pointer. (At the same time made the switch block conditionally
+ included only if exitCode is needed - RPJ.)
+ * pthread_testcancel.c (pthread_testcancel): Remove duplicate and
+ misplaced pthread_mutex_unlock().
+
+2008-02-21 Sebastian Gottschalk <seppig_relay at gmx dot de>
+
+ * pthread_attr_getdetachstate.c (pthread_attr_getdetachstate):
+ Remove potential and superfluous null pointer assignment.
+
+2007-11-22 Ivan Pizhenko <ivanp4 at ua dot fm>
+
+ * pthread.h (gmtime_r): gmtime returns 0 if tm represents a time
+ prior to 1/1/1970. Notice this to prevent raising an exception.
+ * pthread.h (localtime_r): Likewise for localtime.
+
+2007-07-14 Marcel Ruff <mr at marcelruff dot info>
+
+ * errno.c (_errno): Fix test for pthread_self() success.
+ * need_errno.h: Remove unintentional line wrap from #if line.
+
+2007-07-14 Mike Romanchuk <mromanchuk at empirix dot com>
+
+ * pthread.h (timespec): Fix tv_sec type.
+
+2007-01-07 Sinan Kaya <sinan.kaya at siemens dot com>
+
+ * need_errno.h: Fix declaration of _errno - the local version of
+ _errno() is used, e.g. by WinCE.
+
+2007-01-06 Ross Johnson <ross.johnson at homemail dot com dot au>
+
+ * ptw32_semwait.c: Add check for invalid sem_t after acquiring the
+ sem_t state guard mutex and before affecting changes to sema state.
+
+2007-01-06 Marcel Ruff <mr at marcelruff dot info>
+
+ * error.c: Fix reference to pthread handle exitStatus member for
+ builds that use NEED_ERRNO (i.e. WINCE).
+ * context.h: Add support for ARM processor (WinCE).
+ * mutex.c (process.h): Exclude for WINCE.
+ * create.c: Likewise.
+ * exit.c: Likewise.
+ * implement.h: Likewise.
+ * pthread_detach.c (signal.h): Exclude for WINCE.
+ * pthread_join.c: Likewise.
+ * pthread_kill.c: Likewise.
+ * pthread_rwlock_init.c (errno.h): Remove - included by pthread.h.
+ * pthread_rwlock_destroy.c: Likewise.
+ * pthread_rwlock_rdlock.c: Likewise.
+ * pthread_rwlock_timedrdlock.c: Likewise.
+ * pthread_rwlock_timedwrlock.c: Likewise.
+ * pthread_rwlock_tryrdlock.c: Likewise.
+ * pthread_rwlock_trywrlock.c: likewise.
+ * pthread_rwlock_unlock.c: Likewise.
+ * pthread_rwlock_wrlock.c: Likewise.
+ * pthread_rwlockattr_destroy.c: Likewise.
+ * pthread_rwlockattr_getpshared.c: Likewise.
+ * pthread_rwlockattr_init.c: Likewise.
+ * pthread_rwlockattr_setpshared.c: Likewise.
+
+2007-01-06 Romano Paolo Tenca <rotenca at telvia dot it>
+
+ * pthread_cond_destroy.c: Replace sem_wait() with non-cancelable
+ ptw32_semwait() since pthread_cond_destroy() is not a cancelation
+ point.
+ * implement.h (ptw32_spinlock_check_need_init): Add prototype.
+ * ptw32_MCS_lock.c: Reverse order of includes.
+
+2007-01-06 Eric Berge <eric dot berge at quantum dot com>
+
+ * pthread_cond_destroy.c: Add LeaveCriticalSection before returning
+ after errors.
+
+2007-01-04 Ross Johnson <ross.johnson at homemail dot com dot au>
+
+ * ptw32_InterlockedCompareExchange.c: Conditionally skip for
+ Win64 as not required.
+ * pthread_win32_attach_detach_np.c (pthread_win32_process_attach_np):
+ Test for InterlockedCompareExchange is not required for Win64.
+ * context.h: New file. Included by pthread_cancel.h and any tests
+ that need it (e.g. context1.c).
+ * pthread_cancel.c: Architecture-dependent context macros moved
+ to context.h.
+
+2007-01-04 Kip Streithorst <KSTREITH at ball dot com>
+
+ * implement.h (PTW32_INTERLOCKED_COMPARE_EXCHANGE): Add Win64
+ support.
+
+2006-12-20 Ross Johnson <ross.johnson at homemail.com.au>
+
+ * sem_destroy.c: Fix the race involving invalidation of the sema;
+ fix incorrect return of EBUSY resulting from the mutex trylock
+ on the private mutex guard.
+ * sem_wait.c: Add check for invalid sem_t after acquiring the
+ sem_t state guard mutex and before affecting changes to sema state.
+ * sem_trywait.c: Likewise.
+ * sem_timedwait.c: Likewise.
+ * sem_getvalue.c: Likewise.
+ * sem_post.c: Similar.
+ * sem_post_multiple.c: Likewise.
+ * sem_init.c: Set max Win32 semaphore count to SEM_VALUE_MAX (was
+ _POSIX_SEM_VALUE_MAX, which is a lower value - the minimum).
+
+ * pthread_win32_attach_detach_np.c (pthread_win32_process_attach_np):
+ Load COREDLL.DLL under WINCE to check existence of
+ InterlockedCompareExchange() routine. This used to be done to test
+ for TryEnterCriticalSection() but was removed when this was no
+ longer needed.
+
+2006-01-25 Prashant Thakre <prashant.thakre at gmail.com>
+
+ * pthread_cancel.c: Added _M_IA64 register context support.
+
+2005-05-13 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread_kill.c (pthread_kill): Remove check for Win32 thread
+ priority (to confirm HANDLE validity). Useless since thread HANDLEs
+ a not recycle-unique.
+
+2005-05-30 Vladimir Kliatchko <vladimir at kliatchko.com>
+
+ * pthread_once.c: Re-implement using an MCS queue-based lock. The form
+ of pthread_once is as proposed by Alexander Terekhov (see entry of
+ 2005-03-13). The MCS lock implementation does not require a unique
+ 'name' to identify the lock between threads. Attempts to get the Event
+ or Semaphore based versions of pthread_once to a satisfactory level
+ of robustness have thus far failed. The last problem (avoiding races
+ involving non recycle-unique Win32 HANDLEs) was giving everyone
+ grey hair trying to solve it.
+
+ * ptw32_MCS_lock.c: New MCS queue-based lock implementation. These
+ locks are efficient: they have very low overhead in the uncontended case;
+ are efficient in contention and minimise cache-coherence updates in
+ managing the user level FIFO queue; do not require an ABI change in the
+ library.
+
+2005-05-27 Alexander Gottwald <alexander.gottwald at s1999.tu-chemnitz.de>
+
+ * pthread.h: Some things, like HANDLE, were only defined if
+ PTW32_LEVEL was >= 3. They should always be defined.
+
+2005-05-25 Vladimir Kliatchko <vladimir at kliatchko.com>
+
+ * pthread_once.c: Eliminate all priority operations and other
+ complexity by replacing the event with a semaphore. The advantage
+ of the change is the ability to release just one waiter if the
+ init_routine thread is cancelled yet still release all waiters when
+ done. Simplify once_control state checks to improve efficiency
+ further.
+
+2005-05-24 Mikael Magnusson <mikaelmagnusson at glocalnet.net>
+
+ * GNUmakefile: Patched to allow cross-compile with mingw32 on Linux.
+ It uses macros instead of referencing dlltool, gcc and g++ directly;
+ added a call to ranlib. For example the GC static library can be
+ built with:
+ make CC=i586-mingw32msvc-gcc RC=i586-mingw32msvc-windres \
+ RANLIB=i586-mingw32msvc-ranlib clean GC-static
+
+2005-05-13 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread_win32_attach_detach_np.c (pthread_win32_thread_detach_np):
+ Move on-exit-only stuff from ptw32_threadDestroy() to here.
+ * ptw32_threadDestroy.c: It's purpose is now only to reclaim thread
+ resources for detached threads, or via pthread_join() or
+ pthread_detach() on joinable threads.
+ * ptw32_threadStart.c: Calling user destruct routines has moved to
+ pthread_win32_thread_detach_np(); call pthread_win32_thread_detach_np()
+ directly if statically linking, otherwise do so via dllMain; store
+ thread return value in thread struct for all cases, including
+ cancellation and exception exits; thread abnormal exits go via
+ pthread_win32_thread_detach_np.
+ * pthread_join.c (pthread_join): Don't try to get return code from
+ Win32 thread - always get it from he thread struct.
+ * pthread_detach.c (pthread_detach): reduce extent of the thread
+ existence check since we now don't care if the Win32 thread HANDLE has
+ been closed; reclaim thread resources if the thread has exited already.
+ * ptw32_throw.c (ptw32_throw): For Win32 threads that are not implicit,
+ only Call thread cleanup if statically linking, otherwise leave it to
+ dllMain.
+ * sem_post.c (_POSIX_SEM_VALUE_MAX): Change to SEM_VALUE_MAX.
+ * sem_post_multiple.c: Likewise.
+ * sem_init.c: Likewise.
+
+2005-05-10 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread_join.c (pthread_join): Add missing check for thread ID
+ reference count in thread existence test; reduce extent of the
+ existence test since we don't care if the Win32 thread HANDLE has
+ been closed.
+
+2005-05-09 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * ptw32_callUserDestroyRoutines.c: Run destructor process (i.e.
+ loop over all keys calling destructors) up to
+ PTHREAD_DESTRUCTOR_ITERATIONS times if TSD value isn't NULL yet;
+ modify assoc management.
+ * pthread_key_delete.c: Modify assoc management.
+ * ptw32_tkAssocDestroy.c: Fix error in assoc removal from chains.
+ * pthread.h
+ (_POSIX_THREAD_DESTRUCTOR_ITERATIONS): Define to value specified by
+ POSIX.
+ (_POSIX_THREAD_KEYS_MAX): Define to value specified by POSIX.
+ (PTHREAD_KEYS_MAX): Redefine [upward] to minimum required by POSIX.
+ (SEM_NSEMS_MAX): Define to implementation value.
+ (SEM_VALUE_MAX): Define to implementation value.
+ (_POSIX_SEM_NSEMS_MAX): Redefine to value specified by POSIX.
+ (_POSIX_SEM_VALUE_MAX): Redefine to value specified by POSIX.
+
+2005-05-06 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * signal.c (sigwait): Add a cancellation point to this otherwise
+ no-op.
+ * sem_init.c (sem_init): Check for and return ERANGE error.
+ * sem_post.c (sem_post): Likewise.
+ * sem_post_multiple.c (sem_post_multiple): Likewise.
+ * manual (directory): Added; see ChangeLog inside.
+
+2005-05-02 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * implement.h (struct pthread_key_t_): Change threadsLock to keyLock
+ so as not to be confused with the per thread lock 'threadlock';
+ change all references to it.
+ * implement.h (struct ThreadKeyAssoc): Remove lock; add prevKey
+ and prevThread pointers; re-implemented all routines that use this
+ struct. The effect of this is to save one handle per association,
+ which could potentially equal the number of keys multiplied by the
+ number of threads, accumulating over time - and to free the
+ association memory as soon as it is no longer referenced by either
+ the key or the thread. Previously, the handle and memory were
+ released only after BOTH key and thread no longer referenced the
+ association. That is, often no association resources were released
+ until the process itself exited. In addition, at least one race
+ condition has been removed - where two threads could attempt to
+ release the association resources simultaneously - one via
+ ptw32_callUserDestroyRoutines and the other via
+ pthread_key_delete.
+ - thanks to Richard Hughes at Aculab for discovering the problem.
+ * pthread_key_create.c: See above.
+ * pthread_key_delete.c: See above.
+ * pthread_setspecific.c: See above.
+ * ptw32_callUserDestroyRoutines.c: See above.
+ * ptw32_tkAssocCreate.c: See above.
+ * ptw32_tkAssocDestroy.c: See above.
+
+2005-04-27 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * sem_wait.c (ptw32_sem_wait_cleanup): after cancellation re-attempt
+ to acquire the semaphore to avoid a race with a late sem_post.
+ * sem_timedwait.c: Modify comments.
+
+2005-04-25 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * ptw32_relmillisecs.c: New module; converts future abstime to
+ milliseconds relative to 'now'.
+ * pthread_mutex_timedlock.c: Use new ptw32_relmillisecs routine in
+ place of internal code; remove the NEED_SEM code - this routine is now
+ implemented for builds that define NEED_SEM (WinCE etc)
+ * sem_timedwait.c: Likewise; after timeout or cancellation,
+ re-attempt to acquire the semaphore in case one has been posted since
+ the timeout/cancel occurred. Thanks to Stefan Mueller.
+ * Makefile: Add ptw32_relmillisecs.c module; remove
+ ptw32_{in,de}crease_semaphore.c modules.
+ * GNUmakefile: Likewise.
+ * Bmakefile: Likewise.
+
+ * sem_init.c: Re-write the NEED_SEM code to be consistent with the
+ non-NEED_SEM code, but retaining use of an event in place of the w32 sema
+ for w32 systems that don't include semaphores (WinCE);
+ the NEED_SEM versions of semaphores has been broken for a long time but is
+ now fixed and supports all of the same routines as the non-NEED_SEM case.
+ * sem_destroy.c: Likewise.
+ * sem_wait.c: Likewise.
+ * sem_post.c: Likewise.
+ * sem_post_multple.c: Likewise.
+ * implement.h: Likewise.
+ * sem_timedwait.c: Likewise; this routine is now
+ implemented for builds that define NEED_SEM (WinCE etc).
+ * sem_trywait.c: Likewise.
+ * sem_getvalue.c: Likewise.
+
+ * pthread_once.c: Yet more changes, reverting closer to Gottlob Frege's
+ first design, but retaining cancellation, priority boosting, and adding
+ preservation of W32 error codes to make pthread_once transparent to
+ GetLastError.
+
+2005-04-11 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread_once.c (pthread_once): Added priority boosting to
+ solve starvation problem after once_routine cancellation.
+ See notes in file.
+
+2005-04-06 Kevin Lussier <Kevin at codegreennetworks.com>
+
+ * Makefile: Added debug targets for all versions of the library.
+
+2005-04-01 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * GNUmakefile: Add target to build libpthreadGC1.a as a static link
+ library.
+ * Makefile: Likewise for pthreadGC1.lib.
+
+2005-04-01 Kevin Lussier <Kevin at codegreennetworks.com>
+
+ * sem_timedwait.c (sem_timedwait): Increase size of temp variables to
+ avoid int overflows for large timeout values.
+ * implement.h (int64_t): Include or define.
+
+2005-03-31 Dimitar Panayotov <develop at mail.bg>^M
+
+ * pthread.h: Fix conditional defines for static linking.
+ * sched.h: Liekwise.
+ * semaphore.h: Likewise.
+ * dll.c (PTW32_STATIC_LIB): Module is conditionally included
+ in the build.
+
+2005-03-16 Ross Johnson <ross at callisto.canberra.edu.au>^M
+
+ * pthread_setcancelstate.c: Undo the last change.
+
+2005-03-16 Ross Johnson <ross at callisto.canberra.edu.au>^M
+
+ * pthread_setcancelstate.c: Don't check for an async cancel event
+ if the library is using alertable async cancel..
+
+2005-03-14 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread_once.c (pthread_once): Downgrade interlocked operations to simple
+ memory operations where these are protected by the critical section; edit
+ comments.
+
+2005-03-13 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_once.c (pthread_once): Completely redesigned; a change was
+ required to the ABI (pthread_once_t_), and resulting in a version
+ compatibility index increment.
+
+ NOTES:
+ The design (based on pseudo code contributed by Gottlob Frege) avoids
+ creating a kernel object if there is no contention. See URL for details:-
+ http://sources.redhat.com/ml/pthreads-win32/2005/msg00029.html
+ This uses late initialisation similar to the technique already used for
+ pthreads-win32 mutexes and semaphores (from Alexander Terekhov).
+
+ The subsequent cancelation cleanup additions (by rpj) could not be implemented
+ without sacrificing some of the efficiency in Gottlob's design. In particular,
+ although each once_control uses it's own event to block on, a global CS is
+ required to manage it - since the event must be either re-usable or
+ re-creatable under cancelation. This is not needed in the non-cancelable
+ design because it is able to mark the event as closed (forever).
+
+ When uncontested, a CS operation is equivalent to an Interlocked operation
+ in speed. So, in the final design with cancelability, an uncontested
+ once_control operation involves a minimum of five interlocked operations
+ (including the LeaveCS operation).
+
+ ALTERNATIVES:
+ An alternative design from Alexander Terekhov proposed using a named mutex,
+ as sketched below:-
+
+ if (!once_control) { // May be in TLS
+ named_mutex::guard guard(&once_control2);
+ if (!once_control2) {
+ <init>
+ once_control2 = true;
+ }
+ once_control = true;
+ }
+
+ A more detailed description of this can be found here:-
+ http://groups.yahoo.com/group/boost/message/15442
+
+ [Although the definition of a suitable PTHREAD_ONCE_INIT precludes use of the
+ TLS located flag, this is not critical.]
+
+ There are three primary concerns though:-
+ 1) The [named] mutex is 'created' even in the uncontended case.
+ 2) A system wide unique name must be generated.
+ 3) Win32 mutexes are VERY slow even in the uncontended case. An uncontested
+ Win32 mutex lock operation can be 50 (or more) times slower than an
+ uncontested EnterCS operation.
+
+ Ultimately, the named mutex trick is making use of the global locks maintained
+ by the kernel.
+
+ * pthread.h (pthread_once_t_): One flag and an event HANDLE added.
+ (PTHREAD_ONCE_INIT): Additional values included.
+
+2005-03-08 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_once.c (pthread_once): Redesigned to elliminate potential
+ starvation problem.
+ - reported by Gottlob Frege <gottlobfrege at gmail.com>
+
+ * ptw32_threadDestroy.c (ptw32_threadDestroy): Implicit threads were
+ not closing their Win32 thread duplicate handle.
+ - reported by Dmitrii Semii <bogolt at gmail.com>
+
+2005-01-25 Ralf Kubis <RKubis at mc.com>
+
+ * Attempted acquisition of recursive mutex was causing waiting
+ threads to not be woken when the mutex is released.
+
+ * GNUmakefile (GCE): Generate correct version resource comments.
+
+2005-01-01 Konstantin Voronkov <beowinkle at yahoo.com>
+
+ * pthread_mutex_lock.c (pthread_mutex_lock): The new atomic exchange
+ mutex algorithm is known to allow a thread to steal the lock off
+ FIFO waiting threads. The next waiting FIFO thread gets a spurious
+ wake-up and must attempt to re-acquire the lock. The woken thread
+ was setting itself as the mutex's owner before the re-acquisition.
+
+2004-11-22 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_cond_wait.c (ptw32_cond_wait_cleanup): Undo change
+ from 2004-11-02.
+ * Makefile (DLL_VER): Added for DLL naming suffix - see README.
+ * GNUmakefile (DLL_VER): Likewise.
+ * Wmakefile (DLL_VER): Likewise.
+ * Bmakefile (DLL_VER): Likewise.
+ * pthread.dsw (version.rc): Added to MSVS workspace.
+
+2004-11-20 Boudewijn Dekker <b.dekker at ellipsis.nl>
+
+ * pthread_getspecific.c (pthread_getspecific): Check for
+ invalid (NULL) key argument.
+
+2004-11-19 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * config.h (PTW32_THREAD_ID_REUSE_INCREMENT): Added to allow
+ building the library for either unique thread IDs like Solaris
+ or non-unique thread IDs like Linux; allows application developers
+ to override the library's default insensitivity to some apps
+ that may not be strictly POSIX compliant.
+ * version.rc: New resource module to encode version information
+ within the DLL.
+ * pthread.h: Added PTW32_VERSION* defines and grouped sections
+ required by resource compiler together; bulk of file is skipped
+ if RC_INVOKED. Defined some error numbers and other names for
+ Borland compiler.
+
+2004-11-02 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_cond_wait.c (ptw32_cond_wait_cleanup): Lock CV mutex at
+ start of cleanup handler rather than at the end.
+ * implement.h (PTW32_THREAD_REUSE_EMPTY): Renamed from *_BOTTOM.
+ (ptw32_threadReuseBottom): New global variable.
+ * global.c (ptw32_threadReuseBottom): Declare new variable.
+ * ptw32_reuse.c (ptw32_reuse): Change reuse LIFO stack to LILO queue
+ to more evenly distribute use of reusable thread IDs; use renamed
+ PTW32_THREAD_REUSE_EMPTY.
+ * ptw32_processTerminate.c (ptw2_processTerminate): Use renamed
+ PTW32_THREAD_REUSE_EMPTY.
+
+2004-10-31 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * implement.h (PThreadState): Add new state value
+ 'PThreadStateCancelPending'.
+ * pthread_testcancel.c (pthread_testcancel): Use new thread
+ 'PThreadStateCancelPending' state as short cut to avoid entering
+ kernel space via WaitForSingleObject() call. This was obviated
+ by user space sema acquisition in sem_wait() and sem_timedwait(),
+ which are also cancelation points. A call to pthread_testcancel()
+ was required, which introduced a kernel call, effectively nullifying
+ any gains made by the user space sem acquisition checks.
+ * pthread_cancel.c (pthread_cancel): Set new thread
+ 'PThreadStateCancelPending' state.
+
+2004-10-29 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * implement.h (pthread_t): Renamed to ptw32_thread_t; struct contains
+ all thread state.
+ * pthread.h (ptw32_handle_t): New general purpose struct to serve
+ as a handle for various reusable object IDs - currently only used
+ by pthread_t; contains a pointer to ptw32_thread_t (thread state)
+ and a general purpose uint for use as a reuse counter or flags etc.
+ (pthread_t): typedef'ed to ptw32_handle_t; the uint is the reuse
+ counter that allows the library to maintain unique POSIX thread IDs.
+ When the pthread struct reuse stack was introduced, threads would
+ often acquire an identical ID to a previously destroyed thread. The
+ same was true for the pre-reuse stack library, by virtue of pthread_t
+ being the address of the thread struct. The new pthread_t retains
+ the reuse stack but provides virtually unique thread IDs.
+ * sem_wait.c (ptw32_sem_wait_cleanup): New routine used for
+ cancelation cleanup.
+ * sem_timedwait.c (ptw32_sem_timedwait_cleanup): Likewise.
+
+2004-10-22 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * sem_init.c (sem_init): Introduce a 'lock' element in order to
+ replace the interlocked operations with conventional serialisation.
+ This is needed in order to be able to atomically modify the sema
+ value and perform Win32 sema release operations. Win32 semaphores are
+ used instead of events in order to support efficient multiple posting.
+ If the whole modify/release isn't atomic, a race between
+ sem_timedwait() and sem_post() could result in a release when there is
+ no waiting semaphore, which would cause too many threads to proceed.
+ * sem_wait.c (sem_wait): Use new 'lock'element.
+ * sem_timedwait.c (sem_timedwait): Likewise.
+ * sem_trywait.c (sem_trywait): Likewise.
+ * sem_post.c (sem_post): Likewise.
+ * sem_post_multiple.c (sem_post_multiple): Likewise.
+ * sem_getvalue.c (sem_getvalue): Likewise.
+ * ptw32_semwait.c (ptw32_semwait): Likewise.
+ * sem_destroy.c (sem_destroy): Likewise; also tightened the conditions
+ for semaphore destruction; in particular, a semaphore will not be
+ destroyed if it has waiters.
+ * sem_timedwait.c (sem_timedwait): Added cancel cleanup handler to
+ restore sema value when cancelled.
+ * sem_wait.c (sem_wait): Likewise.
+
+2004-10-21 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_mutex_unlock.c (pthread_mutex_unlock): Must use PulseEvent()
+ rather than SetEvent() to reset the event if there are no waiters.
+
+2004-10-19 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * sem_init.c (sem_init): New semaphore model based on the same idea
+ as mutexes, i.e. user space interlocked check to avoid
+ unnecessarily entering kernel space. Wraps the Win32 semaphore and
+ keeps it's own counter. Although the motivation to do this has existed
+ for a long time, credit goes to Alexander Terekhov for providing
+ the logic. I have deviated slightly from AT's logic to add the waiters
+ count, which has made the code more complicated by adding cancelation
+ cleanup. This also appears to have broken the VCE (C++ EH) version of
+ the library (the same problem as previously reported - see BUGS #2),
+ only apparently not fixable using the usual workaround, nor by turning
+ all optimisation off. The GCE version works fine, so it is presumed to
+ be a bug in MSVC++ 6.0. The cancelation exception is thrown and caught
+ correctly, but the cleanup class destructor is never called. The failing
+ test is tests\semaphore4.c.
+ * sem_wait.c (sem_wait): Implemented user space check model.
+ * sem_post.c (sem_post): Likewise.
+ * sem_trywait.c (sem_trywait): Likewise.
+ * sem_timedwait.c (sem_timedwait): Likewise.
+ * sem_post_multiple.c (sem_post_multiple): Likewise.
+ * sem_getvalue.c (sem_getvalue): Likewise.
+ * ptw32_semwait.c (ptw32_semwait): Likewise.
+ * implement.h (sem_t_): Add counter element.
+
+2004-10-15 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * implement.h (pthread_mutex_t_): Use an event in place of
+ the POSIX semaphore.
+ * pthread_mutex_init.c: Create the event; remove semaphore init.
+ * pthread_mutex_destroy.c: Delete the event.
+ * pthread_mutex_lock.c: Replace the semaphore wait with the event wait.
+ * pthread_mutex_trylock.c: Likewise.
+ * pthread_mutex_timedlock.c: Likewise.
+ * pthread_mutex_unlock.c: Set the event.
+
+2004-10-14 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_mutex_lock.c (pthread_mutex_lock): New algorithm using
+ Terekhov's xchg based variation of Drepper's cmpxchg model.
+ Theoretically, xchg uses fewer clock cycles than cmpxchg (using IA-32
+ as a reference), however, in my opinion bus locking dominates the
+ equation on smp systems, so the model with the least number of bus
+ lock operations in the execution path should win, which is Terekhov's
+ variant. On IA-32 uni-processor systems, it's faster to use the
+ CMPXCHG instruction without locking the bus than to use the XCHG
+ instruction, which always locks the bus. This makes the two variants
+ equal for the non-contended lock (fast lane) execution path on up
+ IA-32. Testing shows that the xchg variant is faster on up IA-32 as
+ well if the test forces higher lock contention frequency, even though
+ kernel calls should be dominating the times (on up IA-32, both
+ variants used CMPXCHG instructions and neither locked the bus).
+ * pthread_mutex_timedlock.c pthread_mutex_timedlock(): Similarly.
+ * pthread_mutex_trylock.c (pthread_mutex_trylock): Similarly.
+ * pthread_mutex_unlock.c (pthread_mutex_unlock): Similarly.
+ * ptw32_InterlockedCompareExchange.c (ptw32_InterlockExchange): New
+ function.
+ (PTW32_INTERLOCKED_EXCHANGE): Sets up macro to use inlined
+ ptw32_InterlockedExchange.
+ * implement.h (PTW32_INTERLOCKED_EXCHANGE): Set default to
+ InterlockedExchange().
+ * Makefile: Building using /Ob2 so that asm sections within inline
+ functions are inlined.
+
+2004-10-08 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_mutex_destroy.c (pthread_mutex_destroy): Critical Section
+ element is no longer required.
+ * pthread_mutex_init.c (pthread_mutex_init): Likewise.
+ * pthread_mutex_lock.c (pthread_mutex_lock): New algorithm following
+ Drepper's paper at http://people.redhat.com/drepper/futex.pdf, but
+ using the existing semaphore in place of the futex described in the
+ paper. Idea suggested by Alexander Terekhov - see:
+ http://sources.redhat.com/ml/pthreads-win32/2003/msg00108.html
+ * pthread_mutex_timedlock.c pthread_mutex_timedlock(): Similarly.
+ * pthread_mutex_trylock.c (pthread_mutex_trylock): Similarly.
+ * pthread_mutex_unlock.c (pthread_mutex_unlock): Similarly.
+ * pthread_barrier_wait.c (pthread_barrier_wait): Use inlined version
+ of InterlockedCompareExchange() if possible - determined at
+ build-time.
+ * pthread_spin_destroy.c pthread_spin_destroy(): Likewise.
+ * pthread_spin_lock.c pthread_spin_lock():Likewise.
+ * pthread_spin_trylock.c (pthread_spin_trylock):Likewise.
+ * pthread_spin_unlock.c (pthread_spin_unlock):Likewise.
+ * ptw32_InterlockedCompareExchange.c: Sets up macro for inlined use.
+ * implement.h (pthread_mutex_t_): Remove Critical Section element.
+ (PTW32_INTERLOCKED_COMPARE_EXCHANGE): Set to default non-inlined
+ version of InterlockedCompareExchange().
+ * private.c: Include ptw32_InterlockedCompareExchange.c first for
+ inlining.
+ * GNUmakefile: Add commandline option to use inlined
+ InterlockedCompareExchange().
+ * Makefile: Likewise.
+
+2004-09-27 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_mutex_lock.c (pthread_mutex_lock): Separate
+ PTHREAD_MUTEX_NORMAL logic since we do not need to keep or check some
+ state required by other mutex types; do not check mutex pointer arg
+ for validity - leave this to the system since we are only checking
+ for NULL pointers. This should improve speed of NORMAL mutexes and
+ marginally improve speed of other type.
+ * pthread_mutex_trylock.c (pthread_mutex_trylock): Likewise.
+ * pthread_mutex_unlock.c (pthread_mutex_unlock): Likewise; also avoid
+ entering the critical section for the no-waiters case, with approx.
+ 30% reduction in lock/unlock overhead for this case.
+ * pthread_mutex_timedlock.c (pthread_mutex_timedlock): Likewise; also
+ no longer keeps mutex if post-timeout second attempt succeeds - this
+ will assist applications that wish to impose strict lock deadlines,
+ rather than simply to escape from frozen locks.
+
+2004-09-09 Tristan Savatier <tristan at mpegtv.com>
+ * pthread.h (struct pthread_once_t_): Qualify the 'done' element
+ as 'volatile'.
+ * pthread_once.c: Concerned about possible race condition,
+ specifically on MPU systems re concurrent access to multibyte types.
+ [Maintainer's note: the race condition is harmless on SPU systems
+ and only a problem on MPU systems if concurrent access results in an
+ exception (presumably generated by a hardware interrupt). There are
+ other instances of similar harmless race conditions that have not
+ been identified as issues.]
+
+2004-09-09 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread.h: Declare additional types as volatile.
+
+2004-08-27 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_barrier_wait.c (pthread_barrier_wait): Remove excessive code
+ by substituting the internal non-cancelable version of sem_wait
+ (ptw32_semwait).
+
+2004-08-25 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_join.c (pthread_join): Rewrite and re-order the conditional
+ tests in an attempt to improve efficiency and remove a race
+ condition.
+
+2004-08-23 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * create.c (pthread_create): Don't create a thread if the thread
+ id pointer location (first arg) is inaccessible. A memory
+ protection fault will result if the thread id arg isn't an accessible
+ location. This is consistent with GNU/Linux but different to
+ Solaris or MKS (and possibly others), which accept NULL as meaning
+ 'don't return the created thread's ID'. Applications that run
+ using pthreads-win32 will run on all other POSIX threads
+ implementations, at least w.r.t. this feature.
+
+ It was decided not to copy the Solaris et al behaviour because,
+ although it would have simplified some application porting (but only
+ from Solaris to Windows), the feature is not technically necessary,
+ and the alternative segfault behaviour helps avoid buggy application
+ code.
+
+2004-07-01 Anuj Goyal <anuj.goyal at gmail.com>
+
+ * builddmc.bat: New; Windows bat file to build the library.
+ * config.h (__DMC__): Support for Digital Mars compiler.
+ * create.c (__DMC__): Likewise.
+ * pthread_exit.c (__DMC__): Likewise.
+ * pthread_join.c (__DMC__): Likewise.
+ * ptw32_threadDestroy.c (__DMC__): Likewise.
+ * ptw32_threadStart.c (__DMC__): Likewise.
+ * ptw32_throw.c (__DMC__): Likewise.
+
+2004-06-29 Anuj Goyal <anuj.goyal at gmail.com>
+
+ * pthread.h (__DMC__): Initial support for Digital Mars compiler.
+
+2004-06-29 Will Bryant <will.bryant at ecosm.com>
+
+ * README.Borland: New; description of Borland changes.
+ * Bmakefile: New makefile for the Borland make utility.
+ * ptw32_InterlockedCompareExchange.c:
+ Add Borland compatible asm code.
+
+2004-06-26 Jason Bard <BardJA at Npt.NUWC.Navy.Mil>
+
+ * pthread.h (HAVE_STRUCT_TIMESPEC): If undefined, define it
+ to avoid timespec struct redefined errors elsewhere in an
+ application.
+
+2004-06-21 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread.h (PTHREAD_RECURSIVE_MUTEX_INITIALIZER): Mutex
+ initialiser added for compatibility with Linux threads and
+ others; currently not included in SUSV3.
+ * pthread.h (PTHREAD_ERRORCHECK_MUTEX_INITIALIZER): Likewise.
+ * pthread.h (PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP): Likewise.
+ * pthread.h (PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP): Likewise.
+
+ * ptw32_mutex_check_need_init.c (ptw32_mutex_check_need_init):
+ Add new initialisers.
+
+ * pthread_mutex_lock.c (pthread_mutex_lock): Check for new
+ initialisers.
+ * pthread_mutex_trylock.c (pthread_mutex_trylock): Likewise.
+ * pthread_mutex_timedlock.c (pthread_mutex_timedlock): Likewise.
+ * pthread_mutex_unlock.c (pthread_mutex_unlock): Likewise.
+ * pthread_mutex_destroy.c (pthread_mutex_destroy): Likewise.
+
+2004-05-20 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * README.NONPORTABLE: Document pthread_win32_test_features_np().
+ * FAQ: Update various answers.
+
+2004-05-19 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * Makefile: Don't define _WIN32_WINNT on compiler command line.
+ * GNUmakefile: Likewise.
+
+2004-05-16 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_cancel.c (pthread_cancel): Adapted to use auto-detected
+ QueueUserAPCEx features at run-time.
+ (ptw32_RegisterCancelation): Drop in replacement for QueueUserAPCEx()
+ if it can't be used. Provides older style non-preemptive async
+ cancelation.
+ * pthread_win32_attach_detach_np.c (pthread_win32_attach_np):
+ Auto-detect quserex.dll and the availability of alertdrv.sys;
+ initialise and close on process attach/detach.
+ * global.c (ptw32_register_cancelation): Pointer to either
+ QueueUserAPCEx() or ptw32_RegisterCancelation() depending on
+ availability. QueueUserAPCEx makes pre-emptive async cancelation
+ possible.
+ * implement.h: Add definitions and prototypes related to QueueUserAPC.
+
+2004-05-16 Panagiotis E. Hadjidoukas <peh at hpclab.ceid.upatras.gr>
+
+ * QueueUserAPCEx (separate contributed package): Provides preemptive
+ APC feature.
+ * pthread_cancel.c (pthread_cancel): Initial integration of
+ QueueUserAPCEx into pthreads-win32 to provide true pre-emptive
+ async cancelation of threads, including blocked threads.
+
+2004-05-06 Makoto Kato <raven at oldskool.jp>
+
+ * pthread.h (DWORD_PTR): Define typedef for older MSVC.
+ * pthread_cancel.c (AMD64): Add architecture specific Context register.
+ * ptw32_getprocessors.c: Use correct types (DWORD_PTR) for mask
+ variables.
+
+2004-04-06 P. van Bruggen <pietvb at newbridges.nl>
+
+ * ptw32_threadDestroy.c: Destroy threadLock mutex to
+ close a memory leak.
+
+2004-02-13 Gustav Hallberg <gustav at virtutech.com>
+
+ * pthread_equal.c: Remove redundant equality logic.
+
+2003-12-10 Philippe Di Cristo <philipped at voicebox.com>
+
+ * sem_timedwait.c (sem_timedwait): Fix timeout calculations.
+
+2003-10-20 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * pthread_mutex_timedlock.c (ptw32_semwait): Move to individual module.
+ * ptw32_semwait.c: New module.
+ * pthread_cond_wait.c (ptw32_cond_wait_cleanup): Replace cancelable
+ sem_wait() call with non-cancelable ptw32_semwait() call.
+ * pthread.c (private.c): Re-order for inlining. GNU C warned that
+ function ptw32_semwait() was defined 'inline' after it was called.
+ * pthread_cond_signal.c (ptw32_cond_unblock): Likewise.
+ * pthread_delay_np.c: Disable Watcom warning with comment.
+ * *.c (process.h): Remove include from .c files. This is conditionally
+ included by the common project include files.
+
+2003-10-20 James Ewing <james.ewing at sveasoft.com>
+
+ * ptw32_getprocessors.c: Some Win32 environments don't have
+ GetProcessAffinityMask(), so always return CPU count = 1 for them.
+ * config.h (NEED_PROCESSOR_AFFINITY_MASK): Define for WinCE.
+
+2003-10-15 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * Re-indented all .c files using default GNU style to remove assorted
+ editor ugliness (used GNU indent utility in default style).
+
+2003-10-15 Alex Blanco <Alex.Blanco at motorola.com>
+
+ * sem_init.c (sem_init): Would call CreateSemaphore even if the sema
+ struct calloc failed; was not freeing calloced memory if either
+ CreateSemaphore or CreateEvent failed.
+
+2003-10-14 Ross Johnson <ross at callisto.canberra.edu.au>
+
+ * pthread.h: Add Watcom compiler compatibility. Esssentially just add
+ the cdecl attribute to all exposed function prototypes so that Watcom
+ generates function call code compatible with non-Watcom built libraries.
+ By default, Watcom uses registers to pass function args if possible rather
+ than pushing to stack.
+ * semaphore.h: Likewise.
+ * sched.h: Likewise.
+ * pthread_cond_wait.c (ptw32_cond_wait_cleanup): Define with cdecl attribute
+ for Watcom compatibility. This routine is called via pthread_cleanup_push so
+ it had to match function arg definition.
+ * Wmakefile: New makefile for Watcom builds.
+
+2003-09-14 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_setschedparam.c (pthread_setschedparam): Attempt to map
+ all priority levels between max and min (as returned by
+ sched_get_priority_min/max) to reasonable Win32 priority levels - i.e.
+ levels between THREAD_PRIORITY_LOWEST/IDLE to THREAD_PRIORITY_LOWEST and
+ between THREAD_PRIORITY_HIGHEST/TIME_CRITICAL to THREAD_PRIORITY_HIGHEST
+ while others remain unchanged; record specified thread priority level
+ for return by pthread_getschedparam.
+
+ Note that, previously, specified levels not matching Win32 priority levels
+ would silently leave the current thread priority unaltered.
+
+ * pthread_getschedparam.c (pthread_getschedparam): Return the priority
+ level specified by the latest pthread_setschedparam or pthread_create rather
+ than the actual running thread priority as returned by GetThreadPriority - as
+ required by POSIX. I.e. temporary or adjusted actual priority levels are not
+ returned by this routine.
+
+ * pthread_create.c (pthread_create): For priority levels specified via
+ pthread attributes, attempt to map all priority levels between max and
+ min (as returned by sched_get_priority_min/max) to reasonable Win32
+ priority levels; record priority level given via attributes, or
+ inherited from parent thread, for later return by pthread_getschedparam.
+
+ * ptw32_new.c (ptw32_new): Initialise pthread_t_ sched_priority element.
+
+ * pthread_self.c (pthread_self): Set newly created implicit POSIX thread
+ sched_priority to Win32 thread's current actual priority. Temporarily
+ altered priorities can't be avoided in this case.
+
+ * implement.h (struct pthread_t_): Add new sched_priority element.
+
+2003-09-12 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * sched_get_priority_min.c (sched_get_priority_min): On error should return -1
+ with errno set.
+ * sched_get_priority_max.c (sched_get_priority_max): Likewise.
+
+2003-09-03 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * w32_cancelableWait.c (ptw32_cancelable_wait): Allow cancelation
+ of implicit POSIX threads as well.
+
+2003-09-02 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_win32_attach_detach_np.c (pthread_win32_thread_detach_np):
+ Add comment.
+
+ * pthread_exit.c (pthread_exit): Fix to recycle the POSIX thread handle in
+ addition to calling user TSD destructors. Move the implicit POSIX thread exit
+ handling to ptw32_throw to centralise the logic.
+
+ * ptw32_throw.c (ptw32_throw): Implicit POSIX threads have no point
+ to jump or throw to, so cleanup and exit the thread here in this case. For
+ processes using the C runtime, the exit code will be set to the POSIX
+ reason for the throw (i.e. PTHREAD_CANCEL or the value given to pthread_exit).
+ Note that pthread_exit() already had similar logic, which has been moved to
+ here.
+
+ * ptw32_threadDestroy.c (ptw32_threadDestroy): Don't close the Win32 handle
+ of implicit POSIX threads - expect this to be done by Win32?
+
+2003-09-01 Ross Johnson <rpj at callisto.canberra.edu.au>
+
+ * pthread_self.c (pthread_self): The newly aquired pthread_t must be
+ assigned to the reuse stack, not freed, if the routine fails somehow.
+
+2003-08-13 Ross Johnson <rpj at ise.canberra.edu.au>
+
+ * pthread_getschedparam.c (pthread_getschedparam): An invalid thread ID
+ parameter was returning an incorrect error value; now uses a more exhaustive
+ check for validity.
+
+ * pthread_setschedparam.c (pthread_setschedparam): Likewise.
+
+ * pthread_join.c (pthread_join): Now uses a more exhaustive
+ check for validity.
+
+ * pthread_detach.c (pthread_detach): Likewise.
+
+ * pthread_cancel.c (pthread_cancel): Likewise.
+
+ * ptw32_threadDestroy.c (ptw32_threadDestroy): pthread_t structs are
+ never freed - push them onto a stack for reuse.
+
+ * ptw32_new.c (ptw32_new): Check for reusable pthread_t before dynamically
+ allocating new memory for the struct.
+
+ * pthread_kill.c (pthread_kill): New file; new routine; takes only a zero
+ signal arg so that applications can check the thread arg for validity; checks
+ that the underlying Win32 thread HANDLE is valid.
+
+ * pthread.h (pthread_kill): Add prototype.
+
+ * ptw32_reuse.c (ptw32_threadReusePop): New file; new routine; pop a
+ pthread_t off the reuse stack. pthread_t_ structs that have been destroyed, i.e.
+ have exited detached or have been joined, are cleaned up and put onto a reuse
+ stack. Consequently, thread IDs are no longer freed once calloced. The library
+ will attempt to get a struct off this stack before asking the system to alloc
+ new memory when creating threads. The stack is guarded by a global mutex.
+ (ptw32_threadReusePush): New routine; push a pthread_t onto the reuse stack.
+
+ * implement.h (ptw32_threadReusePush): Add new prototype.
+ (ptw32_threadReusePop): Likewise.
+ (pthread_t): Add new element.
+
+ * ptw32_processTerminate.c (ptw32_processTerminate): Delete the thread
+ reuse lock; free all thread ID structs on the thread reuse stack.
+
+ * ptw32_processInitialize.c (ptw32_processInitialize): Initialise the
+ thread reuse lock.
+
+2003-07-19 Ross Johnson <rpj at ise.canberra.edu.au>
+
+ * GNUmakefile: modified to work under MsysDTK environment.
+ * pthread_spin_lock.c (pthread_spin_lock): Check for NULL arg.
+ * pthread_spin_unlock.c (pthread_spin_unlock): Likewise.
+ * pthread_spin_trylock.c (pthread_spin_trylock): Likewise;
+ fix incorrect pointer value if lock is dynamically initialised by
+ this function.
+ * sem_init.c (sem_init): Initialise sem_t value to quell compiler warning.
+ * sem_destroy.c (sem_destroy): Likewise.
+ * ptw32_threadStart.c (non-MSVC code sections): Include <exception> rather
+ than old-style <new.h>; fix all std:: namespace entities such as
+ std::terminate_handler instances and associated methods.
+ * ptw32_callUserDestroyRoutines.c (non-MSVC code sections): Likewise.
+
+2003-06-24 Piet van Bruggen <pietvb at newbridges.nl>
+
+ * pthread_spin_destroy.c (pthread_spin_destroy): Was not freeing the
+ spinlock struct.
+
+2003-06-22 Nicolas Barry <boozai at yahoo.com>
+
+ * pthread_mutex_destroy.c (pthread_mutex_destroy): When called
+ with a recursive mutex that was locked by the current thread, the
+ function was failing with a success return code.
+
+2003-05-15 Steven Reddie <Steven.Reddie at ca.com>
+
+ * pthread_win32_attach_detach_np.c (pthread_win32_process_detach_np):
+ NULLify ptw32_selfThreadKey after the thread is destroyed, otherwise
+ destructors calling pthreads routines might resurrect it again, creating
+ memory leaks. Call the underlying Win32 Tls routine directly rather than
+ pthread_setspecific().
+ (pthread_win32_thread_detach_np): Likewise.
+
+2003-05-14 Viv <vcotirlea at hotmail.com>
+
+ * pthread.dsp: Change /MT compile flag to /MD.
+
+2003-03-04 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * pthread_mutex_timedlock.c (pthread_mutex_timedlock): Fix failure to
+ set ownership of mutex on second grab after abstime timeout.
+ - bug reported by Robert Strycek <strycek at posam.sk>
+
+2002-12-17 Thomas Pfaff <tpfaff at gmx.net>
+
+ * pthread_mutex_lock.c (ptw32_semwait): New static routine to provide
+ a non-cancelable sem_wait() function. This is consistent with the
+ way that pthread_mutex_timedlock.c does it.
+ (pthread_mutex_lock): Use ptw32_semwait() instead of sem_wait().
+
+2002-12-11 Thomas Pfaff <tpfaff at gmx.net>
+
+ * pthread_mutex_trylock.c: Should return EBUSY rather than EDEADLK.
+ * pthread_mutex_destroy.c: Remove redundant ownership test (the
+ trylock call does this for us); do not destroy a recursively locked
+ mutex.
+
+2002-09-20 Michael Johnson <michaelj at maine.rr.com>
+
+ * pthread_cond_destroy.c (pthread_cond_destroy):
+ When two different threads exist, and one is attempting to
+ destroy a condition variable while the other is attempting to
+ initialize a condition variable that was created with
+ PTHREAD_COND_INITIALIZER, a deadlock can occur. Shrink
+ the ptw32_cond_list_lock critical section to fix it.
+
+2002-07-31 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * ptw32_threadStart.c (ptw32_threadStart): Thread cancelLock
+ destruction moved to ptw32_threadDestroy().
+
+ * ptw32_threadDestroy.c (ptw32_threadDestroy): Destroy
+ the thread's cancelLock. Moved here from ptw32_threadStart.c
+ to cleanup implicit threads as well.
+
+2002-07-30 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * pthread_cond_wait.c (ptw32_cond_wait_cleanup):
+ Remove code designed to avoid/prevent spurious wakeup
+ problems. It is believed that the sem_timedwait() call
+ is consuming a CV signal that it shouldn't and this is
+ breaking the avoidance logic.
+
+2002-07-30 Ross Johnson <rpj at ise.canberra.edu.au>
+
+ * sem_timedwait.c (sem_timedwait): Tighten checks for
+ unreasonable abstime values - that would result in
+ unexpected timeout values.
+
+ * w32_CancelableWait.c (ptw32_cancelable_wait):
+ Tighten up return value checking and add comments.
+
+
+2002-06-08 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * sem_getvalue.c (sem_getvalue): Now returns a value for the
+ NEED_SEM version (i.e. earlier versions of WinCE).
+
+
+2002-06-04 Rob Fanner <rfanner at stonethree.com>
+
+ * sem_getvalue.c (sem_getvalue): The Johnson M. Hart
+ approach didn't work - we are forced to take an
+ intrusive approach. We try to decrement the sema
+ and then immediately release it again to get the
+ value. There is a small probability that this may
+ block other threads, but only momentarily.
+
+2002-06-03 Ross Johnson <rpj at ise.canberra.edu.au>
+
+ * sem_init.c (sem_init): Initialise Win32 semaphores
+ to _POSIX_SEM_VALUE_MAX (which this implementation
+ defines in pthread.h) so that sem_getvalue() can use
+ the trick described in the comments in sem_getvalue().
+ * pthread.h (_POSIX_SEM_VALUE_MAX): Defined.
+ (_POSIX_SEM_NSEMS_MAX): Defined - not used but may be
+ useful for source code portability.
+
+2002-06-03 Rob Fanner <rfanner at stonethree.com>
+
+ * sem_getvalue.c (sem_getvalue): Did not work on NT.
+ Use approach suggested by Johnson M. Hart in his book
+ "Win32 System Programming".
+
+2002-02-28 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * errno.c: Compiler directive was incorrectly including code.
+ * pthread.h: Conditionally added some #defines from config.h
+ needed when not building the library. e.g. NEED_ERRNO, NEED_SEM.
+ (PTW32_DLLPORT): Now only defined if _DLL defined.
+ (_errno): Compiler directive was incorrectly including prototype.
+ * sched.h: Conditionally added some #defines from config.h
+ needed when not building the library.
+ * semaphore.h: Replace an instance of NEED_SEM that should
+ have been NEED_ERRNO. This change currently has nil effect.
+
+ * GNUmakefile: Correct some recent changes.
+
+ * Makefile: Add rule to generate pre-processor output.
+
+2002-02-23 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * pthread_rwlock_timedrdlock.c: New - untested.
+ * pthread_rwlock_timedwrlock.c: New - untested.
+
+ * Testsuite passed (except known MSVC++ problems)
+
+ * pthread_cond_destroy.c: Expand the time change
+ critical section to solve deadlock problem.
+
+ * pthread.c: Add all remaining C modules.
+ * pthread.h: Use dllexport/dllimport attributes on functions
+ to avoid using pthread.def.
+ * sched.h: Likewise.
+ * semaphore.h: Likewise.
+ * GNUmakefile: Add new targets for single translation
+ unit build to maximise inlining potential; generate
+ pthread.def automatically.
+ * Makefile: Likewise, but no longer uses pthread.def.
+
+2002-02-20 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * pthread_cond_destroy.c (pthread_cond_destroy):
+ Enter the time change critical section earlier.
+
+2002-02-17 Ross Johnson <rpj at setup1.ise.canberra.edu.au
+
+ * Testsuite passed.
+
+ * pthread_timechange_handler_np.c: New; following
+ a suggestion from Alexander Terekhov that CVs should
+ be broadcast so that they all re-evaluate their
+ condition variables and reset a new timeout if
+ required, whenever an application receives a
+ WM_TIMECHANGE message. This message indicates that
+ the system time has been changed. Therefore, CVs
+ waiting for a timeout set as an abs_time will possibly
+ not wake up at the expected time. Some applications
+ may not be tolerant of this.
+ * pthread_cond_init.c: Add CV to linked list.
+ * pthread_cond_destroy.c: Remove CV from linked list.
+ * global.c (ptw32_cond_list_head): New variable.
+ (ptw32_cond_list_tail): New variable.
+ (ptw32_cond_list_cs): New critical section.
+ * ptw32_processInitialize (ptw32_cond_list_cs): Initialize.
+ * ptw32_processTerminate (ptw32_cond_list_cs): Delete.
+
+
+ * Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * spin.c: Split file into function segments.
+ * ptw32_spinlock_check_need_init.c: Separated routine from spin.c.
+ * pthread_spin_init.c: Likewise.
+ * pthread_spin_destroy.c: Likewise.
+ * pthread_spin_lock.c: Likewise.
+ * pthread_spin_unlock.c: Likewise.
+ * pthread_spin_trylock.c: Likewise.
+
+ * sync.c: Split file into function segments.
+ * pthread_detach.c: Separated routine from sync.c.
+ * pthread_join.c: Likewise.
+
+ * tsd.c: Split file into function segments.
+ * pthread_key_create.c: Separated routine from tsd.c.
+ * pthread_key_delete.c: Likewise.
+ * pthread_setspecific.c: Likewise.
+ * pthread_getspecific.c: Likewise.
+
+ * sched.c: Split file into function segments.
+ * pthread_attr_setschedpolicy.c: Separated routine from sched.c.
+ * pthread_attr_getschedpolicy.c: Likewise.
+ * pthread_attr_setschedparam.c: Likewise.
+ * pthread_attr_getschedparam.c: Likewise.
+ * pthread_attr_setinheritsched.c: Likewise.
+ * pthread_attr_getinheritsched.c: Likewise.
+ * pthread_setschedparam.c: Likewise.
+ * pthread_getschedparam.c: Likewise.
+ * sched_get_priority_max.c: Likewise.
+ * sched_get_priority_min.c: Likewise.
+ * sched_setscheduler.c: Likewise.
+ * sched_getscheduler.c: Likewise.
+ * sched_yield.c: Likewise.
+
+
+2002-02-16 Ross Johnson <rpj at setup1.ise.canberra.edu.au
+
+ Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * mutex.c: Split file into function segments.
+ * pthread_mutexattr_destroy.c: Separated routine from mutex.c
+ * pthread_mutexattr_getpshared.c: Likewise.
+ * pthread_mutexattr_gettype.c: Likewise.
+ * pthread_mutexattr_init.c: Likewise.
+ * pthread_mutexattr_setpshared.c: Likewise.
+ * pthread_mutexattr_settype.c: Likewise.
+ * ptw32_mutex_check_need_init.c: Likewise.
+ * pthread_mutex_destroy.c: Likewise.
+ * pthread_mutex_init.c: Likewise.
+ * pthread_mutex_lock.c: Likewise.
+ * pthread_mutex_timedlock.c: Likewise.
+ * pthread_mutex_trylock.c: Likewise.
+ * pthread_mutex_unlock.c: Likewise.
+
+ * private.c: Split file into function segments.
+ * ptw32_InterlockedCompareExchange.c: Separated routine from private.c
+ * ptw32_callUserDestroyRoutines.c: Likewise.
+ * ptw32_getprocessors.c: Likewise.
+ * ptw32_processInitialize.c: Likewise.
+ * ptw32_processTerminate.c: Likewise.
+ * ptw32_threadDestroy.c: Likewise.
+ * ptw32_threadStart.c: Likewise.
+ * ptw32_throw.c: Likewise.
+ * ptw32_timespec.c: Likewise.
+ * ptw32_tkAssocCreate.c: Likewise.
+ * ptw32_tkAssocDestroy.c: Likewise.
+
+ * rwlock.c: Split file into function segments.
+ * pthread_rwlockattr_destroy.c: Separated routine from rwlock.c
+ * pthread_rwlockattr_getpshared.c: Likewise.
+ * pthread_rwlockattr_init.c: Likewise.
+ * pthread_rwlockattr_setpshared.c: Likewise.
+ * ptw32_rwlock_check_need_init.c: Likewise.
+ * pthread_rwlock_destroy.c: Likewise.
+ * pthread_rwlock_init.c: Likewise.
+ * pthread_rwlock_rdlock.c: Likewise.
+ * pthread_rwlock_tryrdlock.c: Likewise.
+ * pthread_rwlock_trywrlock.c: Likewise.
+ * pthread_rwlock_unlock.c: Likewise.
+ * pthread_rwlock_wrlock.c: Likewise.
+
+2002-02-10 Ross Johnson <rpj at setup1.ise.canberra.edu.au
+
+ Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * nonportable.c: Split file into function segments.
+ * np_delay.c: Separated routine from nonportable.c
+ * np_getw32threadhandle.c: Likewise.
+ * np_mutexattr_setkind.c: Likewise.
+ * np_mutexattr_getkind.c: Likewise.
+ * np_num_processors.c: Likewise.
+ * np_win32_attach_detach.c: Likewise.
+
+ * misc.c: Split file into function segments.
+ * pthread_equal.c: Separated routine from nonportable.c.
+ * pthread_getconcurrency.c: Likewise.
+ * pthread_once.c: Likewise.
+ * pthread_self.c: Likewise.
+ * pthread_setconcurrency.c: Likewise.
+ * ptw32_calloc.c: Likewise.
+ * ptw32_new.c: Likewise.
+ * w32_CancelableWait.c: Likewise.
+
+2002-02-09 Ross Johnson <rpj at setup1.ise.canberra.edu.au
+
+ Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * condvar.c: Split file into function segments.
+ * pthread_condattr_destroy.c: Separated routine from condvar.c.
+ * pthread_condattr_getpshared.c: Likewise.
+ * pthread_condattr_init.c: Likewise.
+ * pthread_condattr_setpshared.c: Likewise.
+ * ptw32_cond_check_need_init.c: Likewise.
+ * pthread_cond_destroy.c: Likewise.
+ * pthread_cond_init.c: Likewise.
+ * pthread_cond_signal.c: Likewise.
+ * pthread_cond_wait.c: Likewise.
+
+2002-02-07 Alexander Terekhov<TEREKHOV at de.ibm.com>
+
+ * nonportable.c (pthread_delay_np): Make a true
+ cancelation point. Deferred cancels will interrupt the
+ wait.
+
+2002-02-07 Ross Johnson <rpj at setup1.ise.canberra.edu.au
+
+ * misc.c (ptw32_new): Add creation of cancelEvent so that
+ implicit POSIX threads (Win32 threads with a POSIX face)
+ are cancelable; mainly so that pthread_delay_np doesn't fail
+ if called from the main thread.
+ * create.c (pthread_create): Remove creation of cancelEvent
+ from here; now in ptw32_new().
+
+ Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * barrier.c: All routines are now in separate compilation units;
+ This file is used to congregate the separate modules for
+ potential inline optimisation and backward build compatibility.
+ * cancel.c: Likewise.
+ * pthread_barrierattr_destroy.c: Separated routine from cancel.c.
+ * pthread_barrierattr_getpshared.c: Likewise.
+ * pthread_barrierattr_init.c: Likewise.
+ * pthread_barrierattr_setpshared.c: Likewise.
+ * pthread_barrier_destroy.c: Likewise.
+ * pthread_barrier_init.c: Likewise.
+ * pthread_barrier_wait.c: Likewise.
+ * pthread_cancel.c: Likewise.
+ * pthread_setcancelstate.c: Likewise.
+ * pthread_setcanceltype.c: Likewise.
+ * pthread_testcancel.c: Likewise.
+
+2002-02-04 Max Woodbury <mtew at cds.duke.edu>
+
+ Reduced name space pollution.
+ -----------------------------
+ When the appropriate symbols are defined, the headers
+ will restrict the definitions of new names. In particular,
+ it must be possible to NOT include the <windows.h>
+ header and related definitions with some combination
+ of symbol definitions. Secondly, it should be possible
+ that additional definitions should be limited to POSIX
+ compliant symbols by the definition of appropriate symbols.
+
+ * pthread.h: POSIX conditionals.
+ * sched.h: POSIX conditionals.
+ * semaphore.h: POSIX conditionals.
+
+ * semaphore.c: Included <limits.h>.
+ (sem_init): Changed magic 0x7FFFFFFFL to INT_MAX.
+ (sem_getvalue): Trial version.
+
+ Reduce executable size.
+ -----------------------
+ When linking with the static library, only those
+ routines actually called, either directly or indirectly
+ should be included.
+
+ [Gcc has the -ffunction-segments option to do this but MSVC
+ doesn't have this feature as far as I can determine. Other
+ compilers are undetermined as well. - rpj]
+
+ * semaphore.c: All routines are now in separate compilation units;
+ This file is used to congregate the separate modules for
+ potential inline optimisation and backward build compatibility.
+ * sem_close.c: Separated routine from semaphore.c.
+ * ptw32_decrease_semaphore.c: Likewise.
+ * sem_destroy.c: Likewise.
+ * sem_getvalue.c: Likewise.
+ * ptw32_increase_semaphore.c: Likewise.
+ * sem_init.c: Likewise.
+ * sem_open.c: Likewise.
+ * sem_post.c: Likewise.
+ * sem_post_multiple.c: Likewise.
+ * sem_timedwait.c: Likewise.
+ * sem_trywait.c: Likewise.
+ * sem_unlink.c: Likewise.
+ * sem_wait.c: Likewise.
+
+2002-02-04 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ The following extends the idea above to the rest of pthreads-win32 - rpj
+
+ * attr.c: All routines are now in separate compilation units;
+ This file is used to congregate the separate modules for
+ potential inline optimisation and backward build compatibility.
+ * pthread_attr_destroy.c: Separated routine from attr.c.
+ * pthread_attr_getdetachstate.c: Likewise.
+ * pthread_attr_getscope.c: Likewise.
+ * pthread_attr_getstackaddr.c: Likewise.
+ * pthread_attr_getstacksize.c: Likewise.
+ * pthread_attr_init.c: Likewise.
+ * pthread_attr_is_attr.c: Likewise.
+ * pthread_attr_setdetachstate.c: Likewise.
+ * pthread_attr_setscope.c: Likewise.
+ * pthread_attr_setstackaddr.c: Likewise.
+ * pthread_attr_setstacksize.c: Likewise.
+
+ * pthread.c: Agregation of agregate modules for super-inlineability.
+
+2002-02-02 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * cancel.c: Rearranged some code and introduced checks
+ to disable cancelation at the start of a thread's cancelation
+ run to prevent double cancelation. The main problem
+ arises if a thread is canceling and then receives a subsequent
+ async cancel request.
+ * private.c: Likewise.
+ * condvar.c: Place pragmas around cleanup_push/pop to turn
+ off inline optimisation (/Obn where n>0 - MSVC only). Various
+ optimisation switches in MSVC turn this on, which interferes with
+ the way that cleanup handlers are run in C++ EH and SEH
+ code. Application code compiled with inline optimisation must
+ also wrap cleanup_push/pop blocks with the pragmas, e.g.
+ #pragma inline_depth(0)
+ pthread_cleanup_push(...)
+ ...
+ pthread_cleanup_pop(...)
+ #pragma inline_depth(8)
+ * rwlock.c: Likewise.
+ * mutex.c: Remove attempts to inline some functions.
+ * signal.c: Modify misleading comment.
+
+2002-02-01 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * semaphore.c (sem_trywait): Fix missing errno return
+ for systems that define NEED_SEM (e.g. early WinCE).
+ * mutex.c (pthread_mutex_timedlock): Return ENOTSUP
+ for systems that define NEED_SEM since they don't
+ have sem_trywait().
+
+2002-01-27 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * mutex.c (pthread_mutex_timedlock): New function suggested by
+ Alexander Terekhov. The logic required to implement this
+ properly came from Alexander, with some collaboration
+ with Thomas Pfaff.
+ (pthread_mutex_unlock): Wrap the waiters check and sema
+ post in a critical section to prevent a race with
+ pthread_mutex_timedlock.
+ (ptw32_timed_semwait): New function;
+ returns a special result if the absolute timeout parameter
+ represents a time already passed when called; used by
+ pthread_mutex_timedwait(). Have deliberately not reused
+ the name "ptw32_sem_timedwait" because they are not the same
+ routine.
+ * condvar.c (ptw32_cond_timedwait): Use the new sem_timedwait()
+ instead of ptw32_sem_timedwait(), which now has a different
+ function. See previous.
+ * implement.h: Remove prototype for ptw32_sem_timedwait.
+ See next.
+ (pthread_mutex_t_): Add critical section element for access
+ to lock_idx during mutex post-timeout processing.
+ * semaphore.h (sem_timedwait): See next.
+ * semaphore.c (sem_timedwait): See next.
+ * private.c (ptw32_sem_timedwait): Move to semaphore.c
+ and rename as sem_timedwait().
+
+2002-01-18 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * sync.c (pthread_join): Was getting the exit code from the
+ calling thread rather than the joined thread if
+ defined(__MINGW32__) && !defined(__MSVCRT__).
+
+2002-01-15 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h: Unless the build explicitly defines __CLEANUP_SEH,
+ __CLEANUP_CXX, or __CLEANUP_C, then the build defaults to
+ __CLEANUP_C style cleanup. This style uses setjmp/longjmp
+ in the cancelation and thread exit implementations and therefore
+ won't do stack unwinding if linked to applications that have it
+ (e.g. C++ apps). This is currently consistent with most/all
+ commercial Unix POSIX threads implementations.
+
+ * spin.c (pthread_spin_init): Edit renamed function call.
+ * nonportable.c (pthread_num_processors_np): New.
+ (pthread_getprocessors_np): Renamed to ptw32_getprocessors
+ and moved to private.c.
+ * private.c (pthread_getprocessors): Moved here from
+ nonportable.c.
+ * pthread.def (pthread_getprocessors_np): Removed
+ from export list.
+
+ * rwlock.c (pthread_rwlockattr_init): New.
+ (pthread_rwlockattr_destroy): New.
+ (pthread_rwlockattr_getpshared): New.
+ (pthread_rwlockattr_setpshared): New.
+
+2002-01-14 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * attr.c (pthread_attr_setscope): Fix struct pointer
+ indirection error introduced 2002-01-04.
+ (pthread_attr_getscope): Likewise.
+
+2002-01-12 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.dsp (SOURCE): Add missing source files.
+
+2002-01-08 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * mutex.c (pthread_mutex_trylock): use
+ ptw32_interlocked_compare_exchange function pointer
+ rather than ptw32_InterlockedCompareExchange() directly
+ to retain portability to non-iX86 processors,
+ e.g. WinCE etc. The pointer will point to the native
+ OS version of InterlockedCompareExchange() if the
+ OS supports it (see ChangeLog entry of 2001-10-17).
+
+2002-01-07 Thomas Pfaff <tpfaff at gmx.net>, Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * mutex.c (pthread_mutex_init): Remove critical
+ section calls.
+ (pthread_mutex_destroy): Likewise.
+ (pthread_mutex_unlock): Likewise.
+ (pthread_mutex_trylock): Likewise; uses
+ ptw32_InterlockedCompareExchange() to avoid need for
+ critical section; library is no longer i386 compatible;
+ recursive mutexes now increment the lock count rather
+ than return EBUSY; errorcheck mutexes return EDEADLCK
+ rather than EBUSY. This behaviour is consistent with the
+ Solaris pthreads implementation.
+ * implement.h (pthread_mutex_t_): Remove critical
+ section element - no longer needed.
+
+
+2002-01-04 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * attr.c (pthread_attr_setscope): Add more error
+ checking and actually store the scope value even
+ though it's not really necessary.
+ (pthread_attr_getscope): Return stored value.
+ * implement.h (pthread_attr_t_): Add new scope element.
+ * ANNOUNCE: Fix out of date comment next to
+ pthread_attr_setscope in conformance section.
+
+2001-12-21 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * mutex.c (pthread_mutex_lock): Decrementing lock_idx was
+ not thread-safe.
+ (pthread_mutex_trylock): Likewise.
+
+2001-10-26 prionx at juno.com
+
+ * semaphore.c (sem_init): Fix typo and missing bracket
+ in conditionally compiled code. Only older versions of
+ WinCE require this code, hence it doesn't normally get
+ tested; somehow when sem_t reverted to an opaque struct
+ the calloc NULL check was left in the conditionally included
+ section.
+ (sem_destroy): Likewise, the calloced sem_t wasn't being freed.
+
+2001-10-25 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * GNUmakefile (libwsock32): Add to linker flags for
+ WSAGetLastError() and WSASetLastError().
+ * Makefile (wsock32.lib): Likewise.
+ * create.c: Minor mostly inert changes.
+ * implement.h (PTW32_MAX): Move into here and renamed
+ from sched.h.
+ (PTW32_MIN): Likewise.
+ * GNUmakefile (TEST_ICE): Define if testing internal
+ implementation of InterlockedCompareExchange.
+ * Makefile (TEST_ICE): Likewise.
+ * private.c (TEST_ICE): Likewise.
+
+2001-10-24 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * attr.c (pthread_attr_setstacksize): Quell warning
+ from LCC by conditionally compiling the stacksize
+ validity check. LCC correctly warns that the condition
+ (stacksize < PTHREAD_STACK_MIN) is suspicious
+ because STACK_MIN is 0 and stacksize is of type
+ size_t (or unsigned int).
+
+2001-10-17 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Move _LONG and _LPLONG defines into
+ implement.h; rename to PTW32_INTERLOCKED_LONG and
+ PTW32_INTERLOCKED_LPLONG respectively.
+ * spin.c: Likewise; ptw32_interlocked_compare_exchange used
+ in place of InterlockedCompareExchange directly.
+ * global.c (ptw32_interlocked_compare_exchange): Add
+ prototype for this new routine pointer to be used when
+ InterlockedCompareExchange isn't supported by Windows.
+ * nonportable.c (pthread_win32_process_attach_np): Check for
+ support of InterlockedCompareExchange in kernel32 and assign its
+ address to ptw32_interlocked_compare_exchange if it exists, or
+ our own ix86 specific implementation ptw32_InterlockedCompareExchange.
+ *private.c (ptw32_InterlockedCompareExchange): An
+ implementation of InterlockedCompareExchange() which is
+ specific to ix86; written directly in assembler for either
+ MSVC or GNU C; needed because Windows 95 doesn't support
+ InterlockedCompareExchange().
+
+ * sched.c (sched_get_priority_min): Extend to return
+ THREAD_PRIORITY_IDLE.
+ (sched_get_priority_max): Extend to return
+ THREAD_PRIORITY_CRITICAL.
+
+2001-10-15 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * spin.c (pthread_spin_lock): PTHREAD_SPINLOCK_INITIALIZER
+ was causing a program fault.
+ (pthread_spin_init): Could have alloced memory
+ without freeing under some error conditions.
+
+ * mutex.c (pthread_mutex_init): Move memory
+ allocation of mutex struct after checking for
+ PROCESS_SHARED.
+
+2001-10-12 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * spin.c (pthread_spin_unlock): Was not returning
+ EPERM if the spinlock was not locked, for multi CPU
+ machines.
+
+2001-10-08 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * spin.c (pthread_spin_trylock): Was not returning
+ EBUSY for multi CPU machines.
+
+2001-08-24 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * condvar.c (pthread_cond_destroy): Remove cv element
+ that is no longer used.
+ * implement.h: Likewise.
+
+2001-08-23 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * condvar.c (pthread_cond_destroy): fix bug with
+ respect to deadlock in the case of concurrent
+ _destroy/_unblock; a condition variable can be destroyed
+ immediately after all the threads that are blocked on
+ it are awakened.
+
+2001-08-23 Phil Frisbie, Jr. <phil at hawksoft.com>
+
+ * tsd.c (pthread_getspecific): Preserve the last
+ winsock error [from WSAGetLastError()].
+
+2001-07-18 Scott McCaskill <scott at magruder.org>
+
+ * mutex.c (pthread_mutexattr_init): Return ENOMEM
+ immediately and don't dereference the NULL pointer
+ if calloc fails.
+ (pthread_mutexattr_getpshared): Don't dereference
+ a pointer that is possibly NULL.
+ * barrier.c (pthread_barrierattr_init): Likewise
+ (pthread_barrierattr_getpshared): Don't dereference
+ a pointer that is possibly NULL.
+ * condvar.c (pthread_condattr_getpshared): Don't dereference
+ a pointer that is possibly NULL.
+
+2001-07-15 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * rwlock.c (pthread_rwlock_wrlock): Is allowed to be
+ a cancelation point; re-enable deferred cancelability
+ around the CV call.
+
+2001-07-10 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Still more revamping. The exclusive access
+ mutex isn't really needed so it has been removed and replaced
+ by an InterlockedDecrement(). nSerial has been removed.
+ iStep is now dual-purpose. The process shared attribute
+ is now stored in the barrier struct.
+ * implement.h (pthread_barrier_t_): Lost some/gained one
+ elements.
+ * private.c (ptw32_threadStart): Removed some comments.
+
+2001-07-10 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Revamped to fix the race condition. Two alternating
+ semaphores are used instead of the PulseEvent. Also improved
+ overall throughput by returning PTHREAD_BARRIER_SERIAL_THREAD
+ to the first waking thread.
+ * implement.h (pthread_barrier_t_): Revamped.
+
+2001-07-09 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Fix several bugs in all routines. Now passes
+ tests/barrier5.c which is fairly rigorous. There is still
+ a non-optimal work-around for a race condition between
+ the barrier breeched event signal and event wait. Basically
+ the last (signalling) thread to hit the barrier yields
+ to allow any other threads, which may have lost the race,
+ to complete.
+
+2001-07-07 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Changed synchronisation mechanism to a
+ Win32 manual reset Event and use PulseEvent to signal
+ waiting threads. If the implementation continued to use
+ a semaphore it would require a second semaphore and
+ some management to use them alternately as barriers. A
+ single semaphore allows threads to cascade from one barrier
+ through the next, leaving some threads blocked at the first.
+ * implement.h (pthread_barrier_t_): As per above.
+ * general: Made a number of other routines inlinable.
+
+2001-07-07 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * spin.c: Revamped and working; included static initialiser.
+ Now beta level.
+ * barrier.c: Likewise.
+ * condvar.c: Macro constant change; inline auto init routine.
+ * mutex.c: Likewise.
+ * rwlock.c: Likewise.
+ * private.c: Add support for spinlock initialiser.
+ * global.c: Likewise.
+ * implement.h: Likewise.
+ * pthread.h (PTHREAD_SPINLOCK_INITIALIZER): Fix typo.
+
+2001-07-05 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * barrier.c: Remove static initialisation - irrelevent
+ for this object.
+ * pthread.h (PTHREAD_BARRIER_INITIALIZER): Removed.
+ * rwlock.c (pthread_rwlock_wrlock): This routine is
+ not a cancelation point - disable deferred
+ cancelation around call to pthread_cond_wait().
+
+2001-07-05 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * spin.c: New module implementing spin locks.
+ * barrier.c: New module implementing barriers.
+ * pthread.h (_POSIX_SPIN_LOCKS): defined.
+ (_POSIX_BARRIERS): Defined.
+ (pthread_spin_*): Defined.
+ (pthread_barrier*): Defined.
+ (PTHREAD_BARRIER_SERIAL_THREAD): Defined.
+ * implement.h (pthread_spinlock_t_): Defined.
+ (pthread_barrier_t_): Defined.
+ (pthread_barrierattr_t_): Defined.
+
+ * mutex.c (pthread_mutex_lock): Return with the error
+ if an auto-initialiser initialisation fails.
+
+ * nonportable.c (pthread_getprocessors_np): New; gets the
+ number of available processors for the current process.
+
+2001-07-03 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * pthread.h (_POSIX_READER_WRITER_LOCKS): Define it
+ if not already defined.
+
+2001-07-01 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * condvar.c: Fixed lost signal bug reported by Timur Aydin
+ (taydin at snet.net).
+ [RPJ (me) didn't translate the original algorithm
+ correctly.]
+ * semaphore.c: Added sem_post_multiple; this is a useful
+ routine, but it doesn't appear to be standard. For now it's
+ not an exported function.
+
+2001-06-25 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * create.c (pthread_create): Add priority inheritance
+ attributes.
+ * mutex.c (pthread_mutex_lock): Remove some overhead for
+ PTHREAD_MUTEX_NORMAL mutex types. Specifically, avoid
+ calling pthread_self() and pthread_equal() to check/set
+ the mutex owner. Introduce a new pseudo owner for this
+ type. Test results suggest increases in speed of up to
+ 90% for non-blocking locks.
+ This is the default type of mutex used internally by other
+ synchronising objects, ie. condition variables and
+ read-write locks. The test rwlock7.c shows about a
+ 30-35% speed increase over snapshot 2001-06-06. The
+ price of this is that the application developer
+ must ensure correct behaviour, or explicitly set the
+ mutex to a safer type such as PTHREAD_MUTEX_ERRORCHECK.
+ For example, PTHREAD_MUTEX_NORMAL (or PTHREAD_MUTEX_DEFAULT)
+ type mutexes will not return an error if a thread which is not
+ the owner calls pthread_mutex_unlock. The call will succeed
+ in unlocking the mutex if it is currently locked, but a
+ subsequent unlock by the true owner will then fail with EPERM.
+ This is however consistent with some other implementations.
+ (pthread_mutex_unlock): Likewise.
+ (pthread_mutex_trylock): Likewise.
+ (pthread_mutex_destroy): Likewise.
+ * attr.c (pthread_attr_init): PTHREAD_EXPLICIT_SCHED is the
+ default inheritance attribute; THREAD_PRIORITY_NORMAL is
+ the default priority for new threads.
+ * sched.c (pthread_attr_setschedpolicy): Added routine.
+ (pthread_attr_getschedpolicy): Added routine.
+ (pthread_attr_setinheritsched): Added routine.
+ (pthread_attr_getinheritsched): Added routine.
+ * pthread.h (sched_rr_set_interval): Added as a macro;
+ returns -1 with errno set to ENOSYS.
+
+2001-06-23 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ *sched.c (pthread_attr_setschedparam): Add priority range
+ check.
+ (sched_setscheduler): New function; checks for a valid
+ pid and policy; checks for permission to set information
+ in the target process; expects pid to be a Win32 process ID,
+ not a process handle; the only scheduler policy allowed is
+ SCHED_OTHER.
+ (sched_getscheduler): Likewise, but checks for permission
+ to query.
+ * pthread.h (SCHED_*): Moved to sched.h as defined in the
+ POSIX standard.
+ * sched.h (SCHED_*): Moved from pthread.h.
+ (pid_t): Defined if necessary.
+ (sched_setscheduler): Defined.
+ (sched_getscheduler): Defined.
+ * pthread.def (sched_setscheduler): Exported.
+ (sched_getscheduler): Likewise.
+
+2001-06-23 Ralf Brese <Ralf.Brese at pdb4.siemens.de>
+
+ * create.c (pthread_create): Set thread priority from
+ thread attributes.
+
+2001-06-18 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * Made organisational-only changes to UWIN additions.
+ * dll.c (dllMain): Moved UWIN process attach code
+ to pthread_win32_process_attach_np(); moved
+ instance of pthread_count to global.c.
+ * global.c (pthread_count): Moved from dll.c.
+ * nonportable.c (pthread_win32_process_attach_np):
+ Moved _UWIN code to here from dll.c.
+ * implement.h (pthread_count): Define extern int.
+ * create.c (pthread_count): Remove extern int.
+ * private.c (pthread_count): Likewise.
+ * exit.c (pthread_count): Likewise.
+
+2001-06-18 David Korn <dgk at research.att.com>
+
+ * dll.c: Added changes necessary to work with UWIN.
+ * create.c: Likewise.
+ * pthread.h: Likewise.
+ * misc.c: Likewise.
+ * exit.c: Likewise.
+ * private.c: Likewise.
+ * implement.h: Likewise.
+ There is some room at the start of struct pthread_t_
+ to implement the signal semantics in UWIN's posix.dll
+ although this is not yet complete.
+ * Nmakefile: Compatible with UWIN's Nmake utility.
+ * Nmakefile.tests: Likewise - for running the tests.
+
+2001-06-08 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * semaphore.h (sem_t): Fixed for compile and test.
+ * implement.h (sem_t_): Likewise.
+ * semaphore.c: Likewise.
+ * private.c (ptw32_sem_timedwait): Updated to use new
+ opaque sem_t.
+
+2001-06-06 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * semaphore.h (sem_t): Is now an opaque pointer;
+ moved actual definition to implement.h.
+ * implement.h (sem_t_): Move here from semaphore.h;
+ was the definition of sem_t.
+ * semaphore.c: Wherever necessary, changed use of sem
+ from that of a pointer to a pointer-pointer; added
+ extra checks for a valid sem_t; NULL sem_t when
+ it is destroyed; added extra checks when creating
+ and destroying sem_t elements in the NEED_SEM
+ code branches; changed from using a pthread_mutex_t
+ ((*sem)->mutex) to CRITICAL_SECTION ((*sem)->sem_lock_cs)
+ in NEED_SEM branches for access serialisation.
+
+2001-06-06 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * mutex.c (pthread_mutexattr_init): Remove
+ ptw32_mutex_default_kind.
+
+2001-06-05 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * nonportable.c (pthread_mutex_setdefaultkind_np):
+ Remove - should not have been included in the first place.
+ (pthread_mutex_getdefaultkind_np): Likewise.
+ * global.c (ptw32_mutex_default_kind): Likewise.
+ * mutex.c (pthread_mutex_init): Remove use of
+ ptw32_mutex_default_kind.
+ * pthread.h (pthread_mutex_setdefaultkind_np): Likewise.
+ (pthread_mutex_getdefaultkind_np): Likewise.
+ * pthread.def (pthread_mutexattr_setkind_np): Added.
+ (pthread_mutexattr_getkind_np): Likewise.
+
+ * README: Many changes that should have gone in before
+ the last snapshot.
+ * README.NONPORTABLE: New - referred to by ANNOUNCE
+ but never created; documents the non-portable routines
+ included in the library - moved from README with new
+ routines added.
+ * ANNOUNCE (pthread_mutexattr_setkind_np): Added to
+ compliance list.
+ (pthread_mutexattr_getkind_np): Likewise.
+
+2001-06-04 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * condvar.c: Add original description of the algorithm as
+ developed by Terekhov and Thomas, plus reference to
+ README.CV.
+
+2001-06-03 Alexander Terekhov <TEREKHOV at de.ibm.com>, Louis Thomas <lthomas at arbitrade.com>
+
+ * condvar.c (pthread_cond_init): Completely revamped.
+ (pthread_cond_destroy): Likewise.
+ (ptw32_cond_wait_cleanup): Likewise.
+ (ptw32_cond_timedwait): Likewise.
+ (ptw32_cond_unblock): New general signaling routine.
+ (pthread_cond_signal): Now calls ptw32_cond_unblock.
+ (pthread_cond_broadcast): Likewise.
+ * implement.h (pthread_cond_t_): Revamped.
+ * README.CV: New; explanation of the above changes.
+
+2001-05-30 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * pthread.h (rand_r): Fake using _seed argument to quell
+ compiler warning (compiler should optimise this away later).
+
+ * GNUmakefile (OPT): Leave symbolic information out of the library
+ and increase optimisation level - for smaller faster prebuilt
+ dlls.
+
+2001-05-29 Milan Gardian <Milan.Gardian at LEIBINGER.com>
+
+ * Makefile: fix typo.
+ * pthreads.h: Fix problems with stdcall/cdecl conventions, in particular
+ remove the need for PT_STDCALL everywhere; remove warning supression.
+ * (errno): Fix the longstanding "inconsistent dll linkage" problem
+ with errno; now also works with /MD debugging libs -
+ warnings emerged when compiling pthreads library with /MD (or /MDd)
+ compiler switch, instead of /MT (or /MTd) (i.e. when compiling pthreads
+ using Multithreaded DLL CRT instead of Multithreaded statically linked
+ CRT).
+ * create.c (pthread_create): Likewise; fix typo.
+ * private.c (ptw32_threadStart): Eliminate use of terminate() which doesn't
+ throw exceptions.
+ * Remove unnecessary #includes from a number of modules -
+ [I had to #include malloc.h in implement.h for gcc - rpj].
+
+2001-05-29 Thomas Pfaff <tpfaff at gmx.net>
+
+ * pthread.h (PTHREAD_MUTEX_DEFAULT): New; equivalent to
+ PTHREAD_MUTEX_DEFAULT_NP.
+ * (PTHREAD_MUTEX_NORMAL): Similarly.
+ * (PTHREAD_MUTEX_ERRORCHECK): Similarly.
+ * (PTHREAD_MUTEX_RECURSIVE): Similarly.
+ * (pthread_mutex_setdefaultkind_np): New; Linux compatibility stub
+ for pthread_mutexattr_settype.
+ * (pthread_mutexattr_getkind_np): New; Linux compatibility stub
+ for pthread_mutexattr_gettype.
+ * mutex.c (pthread_mutexattr_settype): New; allow
+ the following types of mutex:
+ PTHREAD_MUTEX_DEFAULT_NP
+ PTHREAD_MUTEX_NORMAL_NP
+ PTHREAD_MUTEX_ERRORCHECK_NP
+ PTHREAD_MUTEX_RECURSIVE_NP
+ * Note that PTHREAD_MUTEX_DEFAULT is equivalent to
+ PTHREAD_MUTEX_NORMAL - ie. mutexes should no longer
+ be recursive by default, and a thread will deadlock if it
+ tries to relock a mutex it already owns. This is inline with
+ other pthreads implementations.
+ * (pthread_mutex_lock): Process the lock request
+ according to the mutex type.
+ * (pthread_mutex_init): Eliminate use of Win32 mutexes as the
+ basis of POSIX mutexes - instead, a combination of one critical section
+ and one semaphore are used in conjunction with Win32 Interlocked* routines.
+ * (pthread_mutex_destroy): Likewise.
+ * (pthread_mutex_lock): Likewise.
+ * (pthread_mutex_trylock): Likewise.
+ * (pthread_mutex_unlock): Likewise.
+ * Use longjmp/setjmp to implement cancelation when building the library
+ using a C compiler which doesn't support exceptions, e.g. gcc -x c (note
+ that gcc -x c++ uses exceptions).
+ * Also fixed some of the same typos and eliminated PT_STDCALL as
+ Milan Gardian's patches above.
+
+2001-02-07 Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+ * rwlock.c: Revamped.
+ * implement.h (pthread_rwlock_t_): Redefined.
+ This implementation does not have reader/writer starvation problem.
+ Rwlock attempts to behave more like a normal mutex with
+ races and scheduling policy determining who is more important;
+ It also supports recursive locking,
+ has less synchronization overhead (no broadcasts at all,
+ readers are not blocked on any condition variable) and seem to
+ be faster than the current implementation [W98 appears to be
+ approximately 15 percent faster at least - on top of speed increase
+ from Thomas Pfaff's changes to mutex.c - rpj].
+
+2000-12-29 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * Makefile: Back-out "for" loops which don't work.
+
+ * GNUmakefile: Remove the fake.a target; add the "realclean"
+ target; don't remove built libs under the "clean" target.
+
+ * config.h: Add a guard against multiple inclusion.
+
+ * semaphore.h: Add some defines from config.h to make
+ semaphore.h independent of config.h when building apps.
+
+ * pthread.h (_errno): Back-out previous fix until we know how to
+ fix it properly.
+
+ * implement.h (lockCount): Add missing element to pthread_mutex_t_.
+
+ * sync.c (pthread_join): Spelling fix in comment.
+
+ * private.c (ptw32_threadStart): Reset original termination
+ function (C++).
+ (ptw32_threadStart): Cleanup detached threads early in case
+ the library is statically linked.
+ (ptw32_callUserDestroyRoutines): Remove [SEH] __try block from
+ destructor call so that unhandled exceptions will be passed through
+ to the system; call terminate() from [C++] try block for the same
+ reason.
+
+ * tsd.c (pthread_getspecific): Add comment.
+
+ * mutex.c (pthread_mutex_init): Initialise new elements in
+ pthread_mutex_t.
+ (pthread_mutex_unlock): Invert "pthread_equal()" test.
+
+2000-12-28 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * semaphore.c (mode_t): Use ifndef HAVE_MODE_T to include definition.
+
+ * config.h.in (HAVE_MODE_T): Added.
+ (_UWIN): Start adding defines for the UWIN package.
+
+ * private.c (ptw32_threadStart): Unhandled exceptions are
+ now passed through to the system to deal with. This is consistent
+ with normal Windows behaviour. C++ applications may use
+ set_terminate() to override the default behaviour which is
+ to call ptw32_terminate(). Ptw32_terminate() cleans up some
+ POSIX thread stuff before calling the system default function
+ which calls abort(). The users termination function should conform
+ to standard C++ semantics which is to not return. It should
+ exit the thread (call pthread_exit()) or exit the application.
+ * private.c (ptw32_terminate): Added as the default set_terminate()
+ function. It calls the system default function after cleaning up
+ some POSIX thread stuff.
+
+ * implement.h (ptw32_try_enter_critical_section): Move
+ declaration.
+ * global.c (ptw32_try_enter_critical_section): Moved
+ from dll.c.
+ * dll.c: Move process and thread attach/detach code into
+ functions in nonportable.c.
+ * nonportable.c (pthread_win32_process_attach_np): Process
+ attach code from dll.c is now available to static linked
+ applications.
+ * nonportable.c (pthread_win32_process_detach_np): Likewise.
+ * nonportable.c (pthread_win32_thread_attach_np): Likewise.
+ * nonportable.c (pthread_win32_thread_detach_np): Likewise.
+
+ * pthread.h: Add new non-portable prototypes for static
+ linked applications.
+
+ * GNUmakefile (OPT): Increase optimisation flag and remove
+ debug info flag.
+
+ * pthread.def: Add new non-portable exports for static
+ linked applications.
+
+2000-12-11 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * FAQ: Update Answer 6 re getting a fully working
+ Mingw32 built library.
+
+2000-10-10 Steven Reddie <smr at essemer.com.au>
+
+ * misc.c (pthread_self): Restore Win32 "last error"
+ cleared by TlsGetValue() call in
+ pthread_getspecific()
+
+2000-09-20 Arthur Kantor <akantor at bexusa.com>
+
+ * mutex.c (pthread_mutex_lock): Record the owner
+ of the mutex. This requires also keeping count of
+ recursive locks ourselves rather than leaving it
+ to Win32 since we need to know when to NULL the
+ thread owner when the mutex is unlocked.
+ (pthread_mutex_trylock): Likewise.
+ (pthread_mutex_unlock): Check that the calling
+ thread owns the mutex, decrement the recursive
+ lock count, and NULL the owner if zero. Return
+ EPERM if the mutex is owned by another thread.
+ * implement.h (pthread_mutex_t_): Add ownerThread
+ and lockCount members.
+
+2000-09-13 Jef Gearhart <jgearhart at tpssys.com>
+
+ * mutex.c (pthread_mutex_init): Call
+ TryEnterCriticalSection through the pointer
+ rather than directly so that the dll can load
+ on Windows versions that can't resolve the
+ function, eg. Windows 95
+
+2000-09-09 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h (ctime_r): Fix arg.
+
+2000-09-08 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * GNUmakefile(_WIN32_WINNT=0x400): Define in CFLAGS;
+ doesn't seem to be needed though.
+
+ * cancel.c (pthread_cancel): Must get "self" through
+ calling pthread_self() which will ensure a POSIX thread
+ struct is built for non-POSIX threads; return an error
+ if this fails
+ - Ollie Leahy <ollie at mpt.ie>
+ (pthread_setcancelstate): Likewise.
+ (pthread_setcanceltype): Likewise.
+ * misc.c (ptw32_cancelable_wait): Likewise.
+
+ * private.c (ptw32_tkAssocCreate): Remove unused #if 0
+ wrapped code.
+
+ * pthread.h (ptw32_get_exception_services_code):
+ Needed to be forward declared unconditionally.
+
+2000-09-06 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * cancel.c (pthread_cancel): If called from the main
+ thread "self" would be NULL; get "self" via pthread_self()
+ instead of directly from TLS so that an implicit
+ pthread object is created.
+
+ * misc.c (pthread_equal): Strengthen test for NULLs.
+
+2000-09-02 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * condvar.c (ptw32_cond_wait_cleanup): Ensure that all
+ waking threads check if they are the last, and notify
+ the broadcaster if so - even if an error occurs in the
+ waiter.
+
+ * semaphore.c (_decrease_semaphore): Should be
+ a call to ptw32_decrease_semaphore.
+ (_increase_semaphore): Should be a call to
+ ptw32_increase_semaphore.
+
+ * misc.c (ptw32_cancelable_wait): Renamed from
+ CancelableWait.
+ * rwlock.c (_rwlock_check*): Renamed to
+ ptw32_rwlock_check*.
+ * mutex.c (_mutex_check*): Renamed to ptw32_mutex_check*.
+ * condvar.c (cond_timed*): Renamed to ptw32_cond_timed*.
+ (_cond_check*): Renamed to ptw32_cond_check*.
+ (cond_wait_cleanup*): Rename to ptw32_cond_wait_cleanup*.
+ (ptw32_cond_timedwait): Add comments.
+
+2000-08-22 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * private.c (ptw32_throw): Fix exception test;
+ move exceptionInformation declaration.
+
+ * tsd.c (pthread_key_create): newkey wrongly declared.
+
+ * pthread.h: Fix comment block.
+
+2000-08-18 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * mutex.c (pthread_mutex_destroy): Check that the mutex isn't
+ held; invalidate the mutex as early as possible to avoid
+ contention; not perfect - FIXME!
+
+ * rwlock.c (pthread_rwlock_init): Remove redundant assignment
+ to "rw".
+ (pthread_rwlock_destroy): Invalidate the rwlock before
+ freeing up any of it's resources - to avoid contention.
+
+ * private.c (ptw32_tkAssocCreate): Change assoc->lock
+ to use a dynamically initialised mutex - only consumes
+ a W32 mutex or critical section when first used,
+ not before.
+
+ * mutex.c (pthread_mutex_init): Remove redundant assignment
+ to "mx".
+ (pthread_mutexattr_destroy): Set attribute to NULL
+ before freeing it's memory - to avoid contention.
+
+ * implement.h (PTW32_EPS_CANCEL/PTW32_EPS_EXIT):
+ Must be defined for all compilers - used as generic
+ exception selectors by ptw32_throw().
+
+ * Several: Fix typos from scripted edit session
+ yesterday.
+
+ * nonportable.c (pthread_mutexattr_setforcecs_np):
+ Moved this function from mutex.c.
+ (pthread_getw32threadhandle_np): New function to
+ return the win32 thread handle that the POSIX
+ thread is using.
+ * mutex.c (pthread_mutexattr_setforcecs_np):
+ Moved to new file "nonportable.c".
+
+ * pthread.h (PTW32_BUILD): Only redefine __except
+ and catch compiler keywords if we aren't building
+ the library (ie. PTW32_BUILD is not defined) -
+ this is safer than defining and then undefining
+ if not building the library.
+ * implement.h: Remove __except and catch undefines.
+ * Makefile (CFLAGS): Define PTW32_BUILD.
+ * GNUmakefile (CFLAGS): Define PTW32_BUILD.
+
+ * All appropriate: Change Pthread_exception* to
+ ptw32_exception* to be consistent with internal
+ identifier naming.
+
+ * private.c (ptw32_throw): New function to provide
+ a generic exception throw for all internal
+ exceptions and EH schemes.
+ (ptw32_threadStart): pthread_exit() value is now
+ returned via the thread structure exitStatus
+ element.
+ * exit.c (pthread_exit): pthread_exit() value is now
+ returned via the thread structure exitStatus
+ element.
+ * cancel.c (ptw32_cancel_self): Now uses ptw32_throw.
+ (pthread_setcancelstate): Ditto.
+ (pthread_setcanceltype): Ditto.
+ (pthread_testcancel): Ditto.
+ (pthread_cancel): Ditto.
+ * misc.c (CancelableWait): Ditto.
+ * exit.c (pthread_exit): Ditto.
+ * All applicable: Change PTW32_ prefix to
+ PTW32_ prefix to remove leading underscores
+ from private library identifiers.
+
+2000-08-17 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * All applicable: Change _pthread_ prefix to
+ ptw32_ prefix to remove leading underscores
+ from private library identifiers (single
+ and double leading underscores are reserved in the
+ ANSI C standard for compiler implementations).
+
+ * tsd.c (pthread_create_key): Initialise temporary
+ key before returning it's address to avoid race
+ conditions.
+
+2000-08-13 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * errno.c: Add _MD precompile condition; thus far
+ had no effect when using /MD compile option but I
+ thnk it should be there.
+
+ * exit.c: Add __cplusplus to various #if lines;
+ was compiling SEH code even when VC++ had
+ C++ compile options.
+
+ * private.c: ditto.
+
+ * create.c (pthread_create): Add PT_STDCALL macro to
+ function pointer arg in _beginthread().
+
+ * pthread.h: PT_STDCALL really does need to be defined
+ in both this and impliment.h; don't set it to __cdecl
+ - this macro is only used to extend function pointer
+ casting for functions that will be passed as parameters.
+ (~PThreadCleanup): add cast and group expression.
+ (_errno): Add _MD compile conditional.
+ (PtW32NoCatchWarn): Change pragma message.
+
+ * implement.h: Move and change PT_STDCALL define.
+
+ * need_errno.h: Add _MD to compilation conditional.
+
+ * GNUmakefile: Substantial rewrite for new naming
+ convention; set for nil optimisation (turn it up
+ when we have a working library build; add target
+ "fake.a" to build a libpthreadw32.a from the VC++
+ built DLL pthreadVCE.dll.
+
+ * pthread.def (LIBRARY): Don't specify in the .def
+ file - it is specified on the linker command line
+ since we now use the same .def file for variously
+ named .dlls.
+
+ * Makefile: Substantial rewrite for new naming
+ convention; default nmake target only issues a
+ help message; run nmake with specific target
+ corresponding to the EH scheme being used.
+
+ * README: Update information; add naming convention
+ explanation.
+
+ * ANNOUNCE: Update information.
+
+2000-08-12 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h: Add compile-time message when using
+ MSC_VER compiler and C++ EH to warn application
+ programmers to use PtW32Catch instead of catch(...)
+ if they want cancelation and pthread_exit to work.
+
+ * implement.h: Remove #include <semaphore.h>; we
+ use our own local semaphore.h.
+
+2000-08-10 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * cleanup.c (pthread_pop_cleanup): Remove _pthread
+ prefix from __except and catch keywords; implement.h
+ now simply undefines ptw32__except and
+ ptw32_catch if defined; VC++ was not textually
+ substituting ptw32_catch etc back to catch as
+ it was redefined; the reason for using the prefixed
+ version was to make it clear that it was not using
+ the pthread.h redefined catch keyword.
+
+ * private.c (ptw32_threadStart): Ditto.
+ (ptw32_callUserDestroyRoutines): Ditto.
+
+ * implement.h (ptw32__except): Remove #define.
+ (ptw32_catch): Remove #define.
+
+ * GNUmakefile (pthread.a): New target to build
+ libpthread32.a from pthread.dll using dlltool.
+
+ * buildlib.bat: Duplicate cl commands with args to
+ build C++ EH version of pthread.dll; use of .bat
+ files is redundant now that nmake compatible
+ Makefile is included; used as a kludge only now.
+
+ * Makefile: Localise some macros and fix up the clean:
+ target to extend it and work properly.
+
+ * CONTRIBUTORS: Add contributors.
+
+ * ANNOUNCE: Updated.
+
+ * README: Updated.
+
+2000-08-06 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h: Remove #warning - VC++ doesn't accept it.
+
+2000-08-05 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h (PtW32CatchAll): Add macro. When compiling
+ applications using VC++ with C++ EH rather than SEH
+ 'PtW32CatchAll' must be used in place of any 'catch( ... )'
+ if the application wants pthread cancelation or
+ pthread_exit() to work.
+
+2000-08-03 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * pthread.h: Add a base class ptw32_exception for
+ library internal exceptions and change the "catch"
+ re-define macro to use it.
+
+2000-08-02 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * GNUmakefile (CFLAGS): Add -mthreads.
+ Add new targets to generate cpp and asm output.
+
+ * sync.c (pthread_join): Remove dead code.
+
+2000-07-25 Tristan Savatier <tristan at mpegtv.com>
+
+ * sched.c (sched_get_priority_max): Handle different WinCE and
+ Win32 priority values together.
+ (sched_get_priority_min): Ditto.
+
+2000-07-25 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * create.c (pthread_create): Force new threads to wait until
+ pthread_create has the new thread's handle; we also retain
+ a local copy of the handle for internal use until
+ pthread_create returns.
+
+ * private.c (ptw32_threadStart): Initialise ei[].
+ (ptw32_threadStart): When beginthread is used to start the
+ thread, force waiting until the creator thread had the
+ thread handle.
+
+ * cancel.c (ptw32_cancel_thread): Include context switch
+ code for defined(_X86_) environments in addition to _M_IX86.
+
+ * rwlock.c (pthread_rwlock_destroy): Assignment changed
+ to avoid compiler warning.
+
+ * private.c (ptw32_get_exception_services_code): Cast
+ NULL return value to avoid compiler warning.
+
+ * cleanup.c (pthread_pop_cleanup): Initialise "cleanup" variable
+ to avoid compiler warnings.
+
+ * misc.c (ptw32_new): Change "new" variable to "t" to avoid
+ confusion with the C++ keyword of the same name.
+
+ * condvar.c (cond_wait_cleanup): Initialise lastWaiter variable.
+ (cond_timedwait): Remove unused local variables. to avoid
+ compiler warnings.
+
+ * dll.c (dllMain): Remove 2000-07-21 change - problem
+ appears to be in pthread_create().
+
+2000-07-22 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * tsd.c (pthread_key_create): If a destructor was given
+ and the pthread_mutex_init failed, then would try to
+ reference a NULL pointer (*key); eliminate this section of
+ code by using a dynamically initialised mutex
+ (PTHREAD_MUTEX_INITIALIZER).
+
+ * tsd.c (pthread_setspecific): Return an error if
+ unable to set the value; simplify cryptic conditional.
+
+ * tsd.c (pthread_key_delete): Locking threadsLock relied
+ on mutex_lock returning an error if the key has no destructor.
+ ThreadsLock is only initialised if the key has a destructor.
+ Making this mutex a static could reduce the number of mutexes
+ used by an application since it is actually created only at
+ first use and it's often destroyed soon after.
+
+2000-07-22 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * FAQ: Added Q5 and Q6.
+
+2000-07-21 David Baggett <dmb at itasoftware.com>
+
+ * dll.c: Include resource leakage work-around. This is a
+ partial FIXME which doesn't stop all leakage. The real
+ problem needs to be found and fixed.
+
+2000-07-21 Ross Johnson <rpj at setup1.ise.canberra.edu.au>
+
+ * create.c (pthread_create): Set threadH to 0 (zero)
+ everywhere. Some assignments were using NULL. Maybe
+ it should be NULL everywhere - need to check. (I know
+ they are nearly always the same thing - but not by
+ definition.)
+
+ * misc.c (pthread_self): Try to catch NULL thread handles
+ at the point where they might be generated, even though
+ they should always be valid at this point.
+
+ * tsd.c (pthread_setspecific): return an error value if
+ pthread_self() returns NULL.
+
+ * sync.c (pthread_join): return an error value if
+ pthread_self() returns NULL.
+
+ * signal.c (pthread_sigmask): return an error value if
+ pthread_self() returns NULL.
+
+2000-03-02 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * attr.c (pthread_attr_init): Set default stacksize to zero (0)
+ rather than PTHREAD_STACK_MIN even though these are now the same.
+
+ * pthread.h (PTHREAD_STACK_MIN): Lowered to 0.
+
+2000-01-28 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * mutex.c (pthread_mutex_init): Free mutex if it has been alloced;
+ if critical sections can be used instead of Win32 mutexes, test
+ that the critical section works and return an error if not.
+
+2000-01-07 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * cleanup.c (pthread_pop_cleanup): Include SEH code only if MSC is not
+ compiling as C++.
+ (pthread_push_cleanup): Include SEH code only if MSC is not
+ compiling as C++.
+
+ * pthread.h: Include SEH code only if MSC is not
+ compiling as C++.
+
+ * implement.h: Include SEH code only if MSC is not
+ compiling as C++.
+
+ * cancel.c (ptw32_cancel_thread): Add _M_IX86 check.
+ (pthread_testcancel): Include SEH code only if MSC is not
+ compiling as C++.
+ (ptw32_cancel_self): Include SEH code only if MSC is not
+ compiling as C++.
+
+2000-01-06 Erik Hensema <erik.hensema at group2000.nl>
+
+ * Makefile: Remove inconsistencies in 'cl' args
+
+2000-01-04 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * private.c (ptw32_get_exception_services_code): New; returns
+ value of EXCEPTION_PTW32_SERVICES.
+ (ptw32_processInitialize): Remove initialisation of
+ ptw32_exception_services which is no longer needed.
+
+ * pthread.h (ptw32_exception_services): Remove extern.
+ (ptw32_get_exception_services_code): Add function prototype;
+ use this to return EXCEPTION_PTW32_SERVICES value instead of
+ using the ptw32_exception_services variable which I had
+ trouble exporting through pthread.def.
+
+ * global.c (ptw32_exception_services): Remove declaration.
+
+1999-11-22 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * implement.h: Forward declare ptw32_new();
+
+ * misc.c (ptw32_new): New; alloc and initialise a new pthread_t.
+ (pthread_self): New thread struct is generated by new routine
+ ptw32_new().
+
+ * create.c (pthread_create): New thread struct is generated
+ by new routine ptw32_new().
+
+1999-11-21 Ross Johnson <rpj at special.ise.canberra.edu.au>
+
+ * global.c (ptw32_exception_services): Declare new variable.
+
+ * private.c (ptw32_threadStart): Destroy thread's
+ cancelLock mutex; make 'catch' and '__except' usageimmune to
+ redfinitions in pthread.h.
+ (ptw32_processInitialize): Init new constant ptw32_exception_services.
+
+ * create.c (pthread_create): Initialise thread's cancelLock
+ mutex.
+
+ * cleanup.c (pthread_pop_cleanup): Make 'catch' and '__except'
+ usage immune to redfinition s in pthread.h.
+
+ * private.c: Ditto.
+
+ * pthread.h (catch): Redefine 'catch' so that C++ applications
+ won't catch our internal exceptions.
+ (__except): ditto for __except.
+
+ * implement.h (ptw32_catch): Define internal version
+ of 'catch' because 'catch' is redefined by pthread.h.
+ (__except): ditto for __except.
+ (struct pthread_t_): Add cancelLock mutex for async cancel
+ safety.
+
+1999-11-21 Jason Nye <jnye at nbnet.nb.ca>, Erik Hensema <erik.hensema at group2000.nl>
+
+ * cancel.c (ptw32_cancel_self): New; part of the async
+ cancellation implementation.
+ (ptw32_cancel_thread): Ditto; this function is X86
+ processor specific.
+ (pthread_setcancelstate): Add check for pending async
+ cancel request and cancel the calling thread if
+ required; add async-cancel safety lock.
+ (pthread_setcanceltype): Ditto.
+
+1999-11-13 Erik Hensema <erik.hensema at group2000.nl>
+
+ * configure.in (AC_OUTPUT): Put generated output into GNUmakefile
+ rather than Makefile. Makefile will become the MSC nmake compatible
+ version
+
+1999-11-13 John Bossom (John.Bossom at cognos.com>
+
+ * misc.c (pthread_self): Add a note about GetCurrentThread
+ returning a pseudo-handle
+
+1999-11-10 Todd Owen <towen at lucidcalm.dropbear.id.au>
+
+ * dll.c (dllMain): Free kernel32 ASAP.
+ If TryEnterCriticalSection is not being used, then free
+ the kernel32.dll handle now, rather than leaving it until
+ DLL_PROCESS_DETACH.
+
+ Note: this is not a pedantic exercise in freeing unused
+ resources! It is a work-around for a bug in Windows 95
+ (see microsoft knowledge base article, Q187684) which
+ does Bad Things when FreeLibrary is called within
+ the DLL_PROCESS_DETACH code, in certain situations.
+ Since w95 just happens to be a platform which does not
+ provide TryEnterCriticalSection, the bug will be
+ effortlessly avoided.
+
+1999-11-10 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c (pthread_join): Make it a deferred cancelation point.
+
+ * misc.c (pthread_self): Explicitly initialise implicitly
+ created thread state to default values.
+
+1999-11-05 Tristan Savatier <tristan at mpegtv.com>
+
+ * pthread.h (winsock.h): Include unconditionally.
+ (ETIMEDOUT): Change fallback value to that defined by winsock.h.
+
+ * general: Patched for portability to WinCE. The details are
+ described in the file WinCE-PORT. Follow the instructions
+ in README.WinCE to make the appropriate changes in config.h.
+
+1999-10-30 Erik Hensema <erik.hensema at group2000.nl>
+
+ * create.c (pthread_create): Explicitly initialise thread state to
+ default values.
+
+ * cancel.c (pthread_setcancelstate): Check for NULL 'oldstate'
+ for compatibility with Solaris pthreads;
+ (pthread_setcanceltype): ditto:
+
+1999-10-23 Erik Hensema <erik.hensema at group2000.nl>
+
+ * pthread.h (ctime_r): Fix incorrect argument "_tm"
+
+1999-10-21 Aurelio Medina <aureliom at crt.com>
+
+ * pthread.h (_POSIX_THREADS): Only define it if it isn't
+ already defined. Projects may need to define this on
+ the CC command line under Win32 as it doesn't have unistd.h
+
+1999-10-17 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * rwlock.c (pthread_rwlock_destroy): Add cast to remove compile
+ warning.
+
+ * condvar.c (pthread_cond_broadcast): Only release semaphores
+ if there are waiting threads.
+
+1999-10-15 Lorin Hochstein <lmh at xiphos.ca>, Peter Slacik <Peter.Slacik at tatramed.sk>
+
+ * condvar.c (cond_wait_cleanup): New static cleanup handler for
+ cond_timedwait;
+ (cond_timedwait): pthread_cleanup_push args changed;
+ canceling a thread while it's in pthread_cond_wait
+ will now decrement the waiters count and cleanup if it's the
+ last waiter.
+
+1999-10-15 Graham Dumpleton <Graham.Dumpleton at ra.pad.otc.telstra.com.au>
+
+ * condvar.c (cond_wait_cleanup): the last waiter will now reset the CV's
+ wasBroadcast flag
+
+Thu Sep 16 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * rwlock.c (pthread_rwlock_destroy): Add serialisation.
+ (_rwlock_check_need_init): Check for detroyed rwlock.
+ * rwlock.c: Check return codes from _rwlock_check_need_init();
+ modify comments; serialise access to rwlock objects during
+ operations; rename rw_mutex to rw_lock.
+ * implement.h: Rename rw_mutex to rw_lock.
+ * mutex.c (pthread_mutex_destroy): Add serialisation.
+ (_mutex_check_need_init): Check for detroyed mutex.
+ * condvar.c (pthread_cond_destroy): Add serialisation.
+ (_cond_check_need_init): Check for detroyed condvar.
+ * mutex.c: Modify comments.
+ * condvar.c: Modify comments.
+
+1999-08-10 Aurelio Medina <aureliom at crt.com>
+
+ * implement.h (pthread_rwlock_t_): Add.
+ * pthread.h (pthread_rwlock_t): Add.
+ (PTHREAD_RWLOCK_INITIALIZER): Add.
+ Add rwlock function prototypes.
+ * rwlock.c: New module.
+ * pthread.def: Add new rwlock functions.
+ * private.c (ptw32_processInitialize): initialise
+ ptw32_rwlock_test_init_lock critical section.
+ * global.c (ptw32_rwlock_test_init_lock): Add.
+
+ * mutex.c (pthread_mutex_destroy): Don't free mutex memory
+ if mutex is PTHREAD_MUTEX_INITIALIZER and has not been
+ initialised yet.
+
+1999-08-08 Milan Gardian <mg at tatramed.sk>
+
+ * mutex.c (pthread_mutex_destroy): Free mutex memory.
+
+1999-08-22 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * exit.c (pthread_exit): Fix reference to potentially
+ uninitialised pointer.
+
+1999-08-21 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_threadStart): Apply fix of 1999-08-19
+ this time to C++ and non-trapped C versions. Ommitted to
+ do this the first time through.
+
+1999-08-19 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_threadStart): Return exit status from
+ the application thread startup routine.
+ - Milan Gardian <mg at tatramed.sk>
+
+1999-08-18 John Bossom <john.Bossom at cognos.com>
+
+ * exit.c (pthread_exit): Put status into pthread_t->exitStatus
+ * private.c (ptw32_threadStart): Set pthread->exitStatus
+ on exit of try{} block.
+ * sync.c (pthread_join): use pthread_exitStatus value if the
+ thread exit doesn't return a value (for Mingw32 CRTDLL
+ which uses endthread instead of _endthreadex).
+
+Tue Aug 17 20:17:58 CDT 1999 Mumit Khan <khan at xraylith.wisc.edu>
+
+ * create.c (pthread_create): Add CRTDLL suppport.
+ * exit.c (pthread_exit): Likewise.
+ * private.c (ptw32_threadStart): Likewise.
+ (ptw32_threadDestroy): Likewise.
+ * sync.c (pthread_join): Likewise.
+ * tests/join1.c (main): Warn about partial support for CRTDLL.
+
+Tue Aug 17 20:00:08 1999 Mumit Khan <khan at xraylith.wisc.edu>
+
+ * Makefile.in (LD): Delete entry point.
+ * acconfig.h (STDCALL): Delete unused macro.
+ * configure.in: Remove test for STDCALL.
+ * config.h.in: Regenerate.
+ * errno.c (_errno): Fix self type.
+ * pthread.h (PT_STDCALL): Move from here to
+ * implement.h (PT_STDCALL): here.
+ (ptw32_threadStart): Fix prototype.
+ * private.c (ptw32_threadStart): Likewise.
+
+1999-08-14 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * exit.c (pthread_exit): Don't call pthread_self() but
+ get thread handle directly from TSD for efficiency.
+
+1999-08-12 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_threadStart): ei[] only declared if _MSC_VER.
+
+ * exit.c (pthread_exit): Check for implicitly created threads
+ to avoid raising an unhandled exception.
+
+1999-07-12 Peter Slacik <Peter.Slacik at tatramed.sk>
+
+ * condvar.c (pthread_cond_destroy): Add critical section.
+ (cond_timedwait): Add critical section; check for timeout
+ waiting on semaphore.
+ (pthread_cond_broadcast): Add critical section.
+
+1999-07-09 Lorin Hochstein <lmh at xiphos.ca>, John Bossom <John.Bossom at Cognos.COM>
+
+ The problem was that cleanup handlers were not executed when
+ pthread_exit() was called.
+
+ * implement.h (pthread_t_): Add exceptionInformation element for
+ C++ per-thread exception information.
+ (general): Define and rename exceptions.
+
+1999-07-09 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (CancelableWait): PTW32_EPS_CANCEL (SEH) and
+ ptw32_exception_cancel (C++) used to identify the exception.
+
+ * cancel.c (pthread_testcancel): PTW32_EPS_CANCEL (SEH) and
+ ptw32_exception_cancel (C++) used to identify the exception.
+
+ * exit.c (pthread_exit): throw/raise an exception to return to
+ ptw32_threadStart() to exit the thread. PTW32_EPS_EXIT (SEH)
+ and ptw32_exception_exit (C++) used to identify the exception.
+
+ * private.c (ptw32_threadStart): Add pthread_exit exception trap;
+ clean up and exit the thread directly rather than via pthread_exit().
+
+Sun May 30 00:25:02 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * semaphore.h (mode_t): Conditionally typedef it.
+
+Fri May 28 13:33:05 1999 Mark E. Armstrong <avail at pacbell.net>
+
+ * condvar.c (pthread_cond_broadcast): Fix possible memory fault
+
+Thu May 27 13:08:46 1999 Peter Slacik <Peter.Slacik at tatramed.sk>
+
+ * condvar.c (pthread_cond_broadcast): Fix logic bug
+
+Thu May 27 13:08:46 1999 Bossom, John <John.Bossom at Cognos.COM>
+
+ * condvar.c (pthread_cond_broadcast): optimise sem_post loop
+
+Fri May 14 12:13:18 1999 Mike Russo <miker at eai.com>
+
+ * attr.c (pthread_attr_setdetachstate): Fix logic bug
+
+Sat May 8 09:42:30 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.def (sem_open): Add.
+ (sem_close): Add.
+ (sem_unlink): Add.
+ (sem_getvalue): Add.
+
+ * FAQ (Question 3): Add.
+
+Thu Apr 8 01:16:23 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * semaphore.c (sem_open): New function; returns an error (ENOSYS).
+ (sem_close): ditto.
+ (sem_unlink): ditto.
+ (sem_getvalue): ditto.
+
+ * semaphore.h (_POSIX_SEMAPHORES): define.
+
+Wed Apr 7 14:09:52 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * errno.c (_REENTRANT || _MT): Invert condition.
+
+ * pthread.h (_errno): Conditionally include prototype.
+
+Wed Apr 7 09:37:00 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * *.c (comments): Remove individual attributions - these are
+ documented sufficiently elsewhere.
+
+ * implement.h (pthread.h): Remove extraneous include.
+
+Sun Apr 4 11:05:57 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sched.c (sched.h): Include.
+
+ * sched.h: New file for POSIX 1b scheduling.
+
+ * pthread.h: Move opaque structures to implement.h; move sched_*
+ prototypes out and into sched.h.
+
+ * implement.h: Add opaque structures from pthread.h.
+
+ * sched.c (sched_yield): New function.
+
+ * condvar.c (ptw32_sem_*): Rename to sem_*; except for
+ ptw32_sem_timedwait which is an private function.
+
+Sat Apr 3 23:28:00 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * Makefile.in (OBJS): Add errno.o.
+
+Fri Apr 2 11:08:50 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h (ptw32_sem_*): Remove prototypes now defined in
+ semaphore.h.
+
+ * pthread.h (sempahore.h): Include.
+
+ * semaphore.h: New file for POSIX 1b semaphores.
+
+ * semaphore.c (ptw32_sem_timedwait): Moved to private.c.
+
+ * pthread.h (ptw32_sem_t): Change to sem_t.
+
+ * private.c (ptw32_sem_timedwait): Moved from semaphore.c;
+ set errno on error.
+
+ * pthread.h (pthread_t_): Add per-thread errno element.
+
+Fri Apr 2 11:08:50 1999 John Bossom <jebossom at cognos.com>
+
+ * semaphore.c (ptw32_sem_*): Change to sem_*; these functions
+ will be exported from the library; set errno on error.
+
+ * errno.c (_errno): New file. New function.
+
+Fri Mar 26 14:11:45 1999 Tor Lillqvist <tml at iki.fi>
+
+ * semaphore.c (ptw32_sem_timedwait): Check for negative
+ milliseconds.
+
+Wed Mar 24 11:32:07 1999 John Bossom <jebossom at cognos.com>
+
+ * misc.c (CancelableWait): Initialise exceptionInformation[2].
+ (pthread_self): Get a real Win32 thread handle for implicit threads.
+
+ * cancel.c (pthread_testcancel): Initialise exceptionInformation[2].
+
+ * implement.h (SE_INFORMATION): Fix values.
+
+ * private.c (ptw32_threadDestroy): Close the thread handle.
+
+Fri Mar 19 12:57:27 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cancel.c (comments): Update and cleanup.
+
+Fri Mar 19 09:12:59 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_threadStart): status returns PTHREAD_CANCELED.
+
+ * pthread.h (PTHREAD_CANCELED): defined.
+
+Tue Mar 16 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * all: Add GNU LGPL and Copyright and Warranty.
+
+Mon Mar 15 00:20:13 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (pthread_cond_init): fix possible uninitialised use
+ of cv.
+
+Sun Mar 14 21:01:59 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (pthread_cond_destroy): don't do full cleanup if
+ static initialised cv has never been used.
+ (cond_timedwait): check result of auto-initialisation.
+
+Thu Mar 11 09:01:48 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (pthread_mutex_t): revert to (pthread_mutex_t *);
+ define a value to serve as PTHREAD_MUTEX_INITIALIZER.
+ (pthread_mutex_t_): remove staticinit and valid elements.
+ (pthread_cond_t): revert to (pthread_cond_t_ *);
+ define a value to serve as PTHREAD_COND_INITIALIZER.
+ (pthread_cond_t_): remove staticinit and valid elements.
+
+ * mutex.c (pthread_mutex_t args): adjust indirection of references.
+ (all functions): check for PTHREAD_MUTEX_INITIALIZER value;
+ check for NULL (invalid).
+
+ * condvar.c (pthread_cond_t args): adjust indirection of references.
+ (all functions): check for PTHREAD_COND_INITIALIZER value;
+ check for NULL (invalid).
+
+Wed Mar 10 17:18:12 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (CancelableWait): Undo changes from Mar 8 and 7.
+
+Mon Mar 8 11:18:59 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (CancelableWait): Ensure cancelEvent handle is the lowest
+ indexed element in the handles array. Enhance test for abandoned
+ objects.
+
+ * pthread.h (PTHREAD_MUTEX_INITIALIZER): Trailing elements not
+ initialised are set to zero by the compiler. This avoids the
+ problem of initialising the opaque critical section element in it.
+ (PTHREAD_COND_INITIALIZER): Ditto.
+
+ * semaphore.c (ptw32_sem_timedwait): Check sem == NULL earlier.
+
+Sun Mar 7 12:31:14 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (pthread_cond_init): set semaphore initial value
+ to 0, not 1. cond_timedwait was returning signaled immediately.
+
+ * misc.c (CancelableWait): Place the cancel event handle first
+ in the handle table for WaitForMultipleObjects. This ensures that
+ the cancel event is recognised and acted apon if both objects
+ happen to be signaled together.
+
+ * private.c (ptw32_cond_test_init_lock): Initialise and destroy.
+
+ * implement.h (ptw32_cond_test_init_lock): Add extern.
+
+ * global.c (ptw32_cond_test_init_lock): Add declaration.
+
+ * condvar.c (pthread_cond_destroy): check for valid initialised CV;
+ flag destroyed CVs as invalid.
+ (pthread_cond_init): pthread_cond_t is no longer just a pointer.
+ This is because PTHREAD_COND_INITIALIZER needs state info to reside
+ in pthread_cond_t so that it can initialise on first use. Will work on
+ making pthread_cond_t (and other objects like it) opaque again, if
+ possible, later.
+ (cond_timedwait): add check for statically initialisation of
+ CV; initialise on first use.
+ (pthread_cond_signal): check for valid CV.
+ (pthread_cond_broadcast): check for valid CV.
+ (_cond_check_need_init): Add.
+
+ * pthread.h (PTHREAD_COND_INITIALIZER): Fix.
+ (pthread_cond_t): no longer a pointer to pthread_cond_t_.
+ (pthread_cond_t_): add 'staticinit' and 'valid' elements.
+
+Sat Mar 6 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Undate comments.
+
+Sun Feb 21 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (PTHREAD_MUTEX_INITIALIZER): missing braces around
+ cs element initialiser.
+
+1999-02-21 Ben Elliston <bje at cygnus.com>
+
+ * pthread.h (pthread_exit): The return type of this function is
+ void, not int.
+
+ * exit.c (pthread_exit): Do not return 0.
+
+Sat Feb 20 16:03:30 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * dll.c (DLLMain): Expand TryEnterCriticalSection support test.
+
+ * mutex.c (pthread_mutex_trylock): The check for
+ ptw32_try_enter_critical_section == NULL should have been
+ removed long ago.
+
+Fri Feb 19 16:03:30 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c (pthread_join): Fix pthread_equal() test.
+
+ * mutex.c (pthread_mutex_trylock): Check mutex != NULL before
+ using it.
+
+Thu Feb 18 16:17:30 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (pthread_equal): Fix inverted result.
+
+ * Makefile.in: Use libpthread32.a as the name of the DLL export
+ library instead of pthread.lib.
+
+ * condvar.c (pthread_cond_init): cv could have been used unitialised;
+ initialise.
+
+ * create.c (pthread_create): parms could have been used unitialised;
+ initialise.
+
+ * pthread.h (struct pthread_once_t_): Remove redefinition.
+
+Sat Feb 13 03:03:30 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (struct pthread_once_t_): Replaced.
+
+ * misc.c (pthread_once): Replace with John Bossom's version;
+ has lighter weight serialisation; fixes problem of not holding
+ competing threads until after the init_routine completes.
+
+Thu Feb 11 13:34:14 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (CancelableWait): Change C++ exception throw.
+
+ * sync.c (pthread_join): Change FIXME comment - issue resolved.
+
+Wed Feb 10 12:49:11 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * configure: Various temporary changes.
+ - Kevin Ruland <Kevin.Ruland at anheuser-busch.com>
+
+ * README: Update.
+
+ * pthread.def (pthread_attr_getstackaddr): uncomment
+ (pthread_attr_setstackaddr): uncomment
+
+Fri Feb 5 13:42:30 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * semaphore.c: Comment format changes.
+
+Thu Feb 4 10:07:28 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * global.c: Remove ptw32_exception instantiation.
+
+ * cancel.c (pthread_testcancel): Change C++ exception throw.
+
+ * implement.h: Remove extern declaration.
+
+Wed Feb 3 13:04:44 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c: Rename ptw32_*_cleanup() to pthread_*_cleanup().
+
+ * pthread.def: Ditto.
+
+ * pthread.h: Ditto.
+
+ * pthread.def (pthread_cleanup_push): Remove from export list;
+ the function is defined as a macro under all compilers.
+ (pthread_cleanup_pop): Ditto.
+
+ * pthread.h: Remove #if defined().
+
+Wed Feb 3 10:13:48 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c (pthread_join): Check for NULL value_ptr arg;
+ check for detached threads.
+
+Tue Feb 2 18:07:43 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * implement.h: Add #include <pthread.h>.
+ Change sem_t to ptw32_sem_t.
+
+Tue Feb 2 18:07:43 1999 Kevin Ruland <Kevin.Ruland at anheuser-busch.com>
+
+ * signal.c (pthread_sigmask): Add and modify casts.
+ Reverse LHS/RHS bitwise assignments.
+
+ * pthread.h: Remove #include <semaphore.h>.
+ (PTW32_ATTR_VALID): Add cast.
+ (struct pthread_t_): Add sigmask element.
+
+ * dll.c: Add "extern C" for DLLMain.
+ (DllMain): Add cast.
+
+ * create.c (pthread_create): Set sigmask in thread.
+
+ * condvar.c: Remove #include. Change sem_* to ptw32_sem_*.
+
+ * attr.c: Changed #include.
+
+ * Makefile.in: Additional targets and changes to build the library
+ as a DLL.
+
+Fri Jan 29 11:56:28 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * Makefile.in (OBJS): Add semaphore.o to list.
+
+ * semaphore.c (ptw32_sem_timedwait): Move from private.c.
+ Rename sem_* to ptw32_sem_*.
+
+ * pthread.h (pthread_cond_t): Change type of sem_t.
+ _POSIX_SEMAPHORES no longer defined.
+
+ * semaphore.h: Contents moved to implement.h.
+ Removed from source tree.
+
+ * implement.h: Add semaphore function prototypes and rename all
+ functions to prepend 'ptw32_'. They are
+ now private to the pthreads-win32 implementation.
+
+ * private.c: Change #warning.
+ Move ptw32_sem_timedwait() to semaphore.c.
+
+ * cleanup.c: Change #warning.
+
+ * misc.c: Remove #include <errno.h>
+
+ * pthread.def: Cleanup CVS merge conflicts.
+
+ * global.c: Ditto.
+
+ * ChangeLog: Ditto.
+
+ * cleanup.c: Ditto.
+
+Sun Jan 24 01:34:52 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * semaphore.c (sem_wait): Remove second arg to
+ pthreadCancelableWait() call.
+
+Sat Jan 23 17:36:40 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.def: Add new functions to export list.
+
+ * pthread.h (PTHREAD_MUTEX_AUTO_CS_NP): New.
+ (PTHREAD_MUTEX_FORCE_CS_NP): New.
+
+ * README: Updated.
+
+Fri Jan 22 14:31:59 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * Makefile.in (CFLAGS): Remove -fhandle-exceptions. Not needed
+ with egcs. Add -g for debugging.
+
+ * create.c (pthread_create): Replace __stdcall with PT_STDCALL
+ macro. This is a hack and must be fixed.
+
+ * misc.c (CancelableWait): Remove redundant statement.
+
+ * mutex.c (pthread_mutexattr_init): Cast calloc return value.
+
+ * misc.c (CancelableWait): Add cast.
+ (pthread_self): Add cast.
+
+ * exit.c (pthread_exit): Add cast.
+
+ * condvar.c (pthread_condattr_init): Cast calloc return value.
+
+ * cleanup.c: Reorganise conditional compilation.
+
+ * attr.c (pthread_attr_init): Remove unused 'result'.
+ Cast malloc return value.
+
+ * private.c (ptw32_callUserDestroyRoutines): Redo conditional
+ compilation.
+
+ * misc.c (CancelableWait): C++ version uses 'throw'.
+
+ * cancel.c (pthread_testcancel): Ditto.
+
+ * implement.h (class ptw32_exception): Define for C++.
+
+ * pthread.h: Fix C, C++, and Win32 SEH condition compilation
+ mayhem around pthread_cleanup_* defines. C++ version now uses John
+ Bossom's cleanup handlers.
+ (pthread_attr_t): Make 'valid' unsigned.
+ Define '_timeb' as 'timeb' for Ming32.
+ Define PT_STDCALL as nothing for Mingw32. May be temporary.
+
+ * cancel.c (pthread_testcancel): Cast return value.
+
+Wed Jan 20 09:31:28 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (pthread_mutexattr_t): Changed to a pointer.
+
+ * mutex.c (pthread_mutex_init): Conditionally create Win32 mutex
+ - from John Bossom's implementation.
+ (pthread_mutex_destroy): Conditionally close Win32 mutex
+ - from John Bossom's implementation.
+ (pthread_mutexattr_init): Replaced by John Bossom's version.
+ (pthread_mutexattr_destroy): Ditto.
+ (pthread_mutexattr_getpshared): New function from John Bossom's
+ implementation.
+ (pthread_mutexattr_setpshared): New function from John Bossom's
+ implementation.
+
+Tue Jan 19 18:27:42 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * pthread.h (pthreadCancelableTimedWait): New prototype.
+ (pthreadCancelableWait): Remove second argument.
+
+ * misc.c (CancelableWait): New static function is
+ pthreadCancelableWait() renamed.
+ (pthreadCancelableWait): Now just calls CancelableWait() with
+ INFINITE timeout.
+ (pthreadCancelableTimedWait): Just calls CancelableWait()
+ with passed in timeout.
+
+Tue Jan 19 18:27:42 1999 Scott Lightner <scott at curriculum.com>
+
+ * private.c (ptw32_sem_timedwait): 'abstime' arg really is
+ absolute time. Calculate relative time to wait from current
+ time before passing timeout to new routine
+ pthreadCancelableTimedWait().
+
+Tue Jan 19 10:27:39 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (pthread_mutexattr_setforcecs_np): New prototype.
+
+ * mutex.c (pthread_mutexattr_init): Init 'pshared' and 'forcecs'
+ attributes to 0.
+ (pthread_mutexattr_setforcecs_np): New function (not portable).
+
+ * pthread.h (pthread_mutex_t):
+ Add 'mutex' element. Set to NULL in PTHREAD_MUTEX_INITIALIZER.
+ The pthread_mutex_*() routines will try to optimise performance
+ by choosing either mutexes or critical sections as the basis
+ for pthread mutexes for each indevidual mutex.
+ (pthread_mutexattr_t_): Add 'forcecs' element.
+ Some applications may choose to force use of critical sections
+ if they know that:-
+ the mutex is PROCESS_PRIVATE and,
+ either the OS supports TryEnterCriticalSection() or
+ pthread_mutex_trylock() will never be called on the mutex.
+ This attribute will be setable via a non-portable routine.
+
+ Note: We don't yet support PROCESS_SHARED mutexes, so the
+ implementation as it stands will default to Win32 mutexes only if
+ the OS doesn't support TryEnterCriticalSection. On Win9x, and early
+ versions of NT 'forcecs' will need to be set in order to get
+ critical section based mutexes.
+
+Sun Jan 17 12:01:26 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (PTHREAD_MUTEX_INITIALIZER): Init new 'staticinit'
+ value to '1' and existing 'valid' value to '1'.
+
+ * global.c (ptw32_mutex_test_init_lock): Add.
+
+ * implement.h (ptw32_mutex_test_init_lock.): Add extern.
+
+ * private.c (ptw32_processInitialize): Init critical section for
+ global lock used by _mutex_check_need_init().
+ (ptw32_processTerminate): Ditto (:s/Init/Destroy/).
+
+ * dll.c (dllMain): Move call to FreeLibrary() so that it is only
+ called once when the process detaches.
+
+ * mutex.c (_mutex_check_need_init): New static function to test
+ and init PTHREAD_MUTEX_INITIALIZER mutexes. Provides serialised
+ access to the internal state of the uninitialised static mutex.
+ Called from pthread_mutex_trylock() and pthread_mutex_lock() which
+ do a quick unguarded test to check if _mutex_check_need_init()
+ needs to be called. This is safe as the test is conservative
+ and is repeated inside the guarded section of
+ _mutex_check_need_init(). Thus in all calls except the first
+ calls to lock static mutexes, the additional overhead to lock any
+ mutex is a single memory fetch and test for zero.
+
+ * pthread.h (pthread_mutex_t_): Add 'staticinit' member. Mutexes
+ initialised by PTHREAD_MUTEX_INITIALIZER aren't really initialised
+ until the first attempt to lock it. Using the 'valid'
+ flag (which flags the mutex as destroyed or not) to record this
+ information would be messy. It is possible for a statically
+ initialised mutex such as this to be destroyed before ever being
+ used.
+
+ * mutex.c (pthread_mutex_trylock): Call _mutex_check_need_init()
+ to test/init PTHREAD_MUTEX_INITIALIZER mutexes.
+ (pthread_mutex_lock): Ditto.
+ (pthread_mutex_unlock): Add check to ensure we don't try to unlock
+ an unitialised static mutex.
+ (pthread_mutex_destroy): Add check to ensure we don't try to delete
+ a critical section that we never created. Allows us to destroy
+ a static mutex that has never been locked (and hence initialised).
+ (pthread_mutex_init): Set 'staticinit' flag to 0 for the new mutex.
+
+Sun Jan 17 12:01:26 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_sem_timedwait): Move from semaphore.c.
+
+ * semaphore.c : Remove redundant #includes.
+ (ptw32_sem_timedwait): Move to private.c.
+ (sem_wait): Add missing abstime arg to pthreadCancelableWait() call.
+
+Fri Jan 15 23:38:05 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (cond_timedwait): Remove comment.
+
+Fri Jan 15 15:41:28 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * pthread.h: Add new 'abstime' arg to pthreadCancelableWait()
+ prototype.
+
+ * condvar.c (cond_timedwait): New generalised function called by
+ both pthread_cond_wait() and pthread_cond_timedwait(). This is
+ essentially pthread_cond_wait() renamed and modified to add the
+ 'abstime' arg and call the new ptw32_sem_timedwait() instead of
+ sem_wait().
+ (pthread_cond_wait): Now just calls the internal static
+ function cond_timedwait() with an INFINITE wait.
+ (pthread_cond_timedwait): Now implemented. Calls the internal
+ static function cond_timedwait().
+
+ * implement.h (ptw32_sem_timedwait): New internal function
+ prototype.
+
+ * misc.c (pthreadCancelableWait): Added new 'abstime' argument
+ to allow shorter than INFINITE wait.
+
+ * semaphore.c (ptw32_sem_timedwait): New function for internal
+ use. This is essentially sem_wait() modified to add the
+ 'abstime' arg and call the modified (see above)
+ pthreadCancelableWait().
+
+Thu Jan 14 14:27:13 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c: Correct _cplusplus to __cplusplus wherever used.
+
+ * Makefile.in: Add CC=g++ and add -fhandle-exceptions to CFLAGS.
+ The derived Makefile will compile all units of the package as C++
+ so that those which include try/catch exception handling should work
+ properly. The package should compile ok if CC=gcc, however, exception
+ handling will not be included and thus thread cancellation, for
+ example, will not work.
+
+ * cleanup.c (ptw32_pop_cleanup): Add #warning to compile this
+ file as C++ if using a cygwin32 environment. Perhaps the whole package
+ should be compiled using g++ under cygwin.
+
+ * private.c (ptw32_threadStart): Change #error directive
+ into #warning and bracket for __CYGWIN__ and derivative compilers.
+
+Wed Jan 13 09:34:52 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * build.bat: Delete old binaries before compiling/linking.
+
+Tue Jan 12 09:58:38 1999 Tor Lillqvist <tml at iki.fi>
+
+ * dll.c: The Microsoft compiler pragmas probably are more
+ appropriately protected by _MSC_VER than by _WIN32.
+
+ * pthread.h: Define ETIMEDOUT. This should be returned by
+ pthread_cond_timedwait which is not implemented yet as of
+ snapshot-1999-01-04-1305. It was implemented in the older version.
+ The Microsoft compiler pragmas probably are more appropriately
+ protected by _MSC_VER than by _WIN32.
+
+ * pthread.def: pthread_mutex_destroy was missing from the def file
+
+ * condvar.c (pthread_cond_broadcast): Ensure we only wait on threads
+ if there were any waiting on the condition.
+ I think pthread_cond_broadcast should do the WaitForSingleObject
+ only if cv->waiters > 0? Otherwise it seems to hang, at least in the
+ testg thread program from glib.
+
+Tue Jan 12 09:58:38 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (pthread_cond_timedwait): Fix function description
+ comments.
+
+ * semaphore.c (sem_post): Correct typo in comment.
+
+Mon Jan 11 20:33:19 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h: Re-arrange conditional compile of pthread_cleanup-*
+ macros.
+
+ * cleanup.c (ptw32_push_cleanup): Provide conditional
+ compile of cleanup->prev.
+
+1999-01-11 Tor Lillqvist <tml at iki.fi>
+
+ * condvar.c (pthread_cond_init): Invert logic when testing the
+ return value from calloc().
+
+Sat Jan 9 14:32:08 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Compile-time switch for CYGWIN derived environments
+ to use CreateThread instead of _beginthreadex. Ditto for ExitThread.
+ Patch provided by Anders Norlander <anorland at hem2.passagen.se>.
+
+Tue Jan 5 16:33:04 1999 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c (ptw32_pop_cleanup): Add C++ version of __try/__except
+ block. Move trailing "}" out of #ifdef _WIN32 block left there by
+ (rpj's) mistake.
+
+ * private.c: Remove #include <errno.h> which is included by pthread.h.
+
+1998-12-11 Ben Elliston <bje at toilet.to.cygnus.com>
+
+ * README: Update info about subscribing to the mailing list.
+
+Mon Jan 4 11:23:40 1999 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * all: No code changes, just cleanup.
+ - remove #if 0 /* Pre Bossom */ enclosed code.
+ - Remove some redundant #includes.
+ * pthread.h: Update implemented/unimplemented routines list.
+ * Tag the bossom merge branch getting ready to merge back to main
+ trunk.
+
+Tue Dec 29 13:11:16 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Move the following struct definitions to pthread.h:
+ pthread_t_, pthread_attr_t_, pthread_mutex_t_, pthread_mutex_t_,
+ pthread_mutexattr_t_, pthread_key_t_, pthread_cond_t_,
+ pthread_condattr_t_, pthread_once_t_.
+
+ * pthread.h: Add "_" prefix to pthread_push_cleanup and
+ pthread_pop_cleanup internal routines, and associated struct and
+ typedefs.
+
+ * buildlib.bat: Add compile command for semaphore.c
+
+ * pthread.def: Comment out pthread_atfork routine name.
+ Now unimplemented.
+
+ * tsd.c (pthread_setspecific): Rename tkAssocCreate to
+ ptw32_tkAssocCreate.
+ (pthread_key_delete): Rename tkAssocDestroy to
+ ptw32_tkAssocDestroy.
+
+ * sync.c (pthread_join): Rename threadDestroy to ptw32_threadDestroy
+
+ * sched.c (is_attr): attr is now **attr (was *attr), so add extra
+ NULL pointer test.
+ (pthread_attr_setschedparam): Increase redirection for attr which is
+ now a **.
+ (pthread_attr_getschedparam): Ditto.
+ (pthread_setschedparam): Change thread validation and rename "thread"
+ Win32 thread Handle element name to match John Bossom's version.
+ (pthread_getschedparam): Ditto.
+
+ * private.c (ptw32_threadDestroy): Rename call to
+ callUserDestroyRoutines() as ptw32_callUserDestroyRoutines()
+
+ * misc.c: Add #include "implement.h".
+
+ * dll.c: Remove defined(KLUDGE) wrapped code.
+
+ * fork.c: Remove redefinition of ENOMEM.
+ Remove pthread_atfork() and fork() with #if 0/#endif.
+
+ * create.c (pthread_create): Rename threadStart and threadDestroy calls
+ to ptw32_threadStart and ptw32_threadDestroy.
+
+ * implement.h: Rename "detachedstate" to "detachstate".
+
+ * attr.c: Rename "detachedstate" to "detachstate".
+
+Mon Dec 28 09:54:39 1998 John Bossom
+
+ * semaphore.c: Initial version.
+ * semaphore.h: Initial version.
+
+Mon Dec 28 09:54:39 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.h (pthread_attr_t_): Change to *pthread_attr_t.
+
+Mon Dec 28 09:54:39 1998 John Bossom, Ben Elliston
+
+ * attr.c (pthread_attr_setstacksize): Merge with John's version.
+ (pthread_attr_getstacksize): Merge with John's version.
+ (pthread_attr_setstackaddr): Merge with John's version.
+ (pthread_attr_getstackaddr): Merge with John's version.
+ (pthread_attr_init): Merge with John's version.
+ (pthread_attr_destroy): Merge with John's version.
+ (pthread_attr_getdetachstate): Merge with John's version.
+ (pthread_attr_setdetachstate): Merge with John's version.
+ (is_attr): attr is now **attr (was *attr), so add extra NULL pointer
+ test.
+
+Mon Dec 28 09:54:39 1998 Ross Johnson
+
+ * implement.h (pthread_attr_t_): Add and rename elements in JEB's
+ version to correspond to original, so that it can be used with
+ original attr routines.
+
+ * pthread.h: Add #endif at end which was truncated in merging.
+
+Sun Dec 20 14:51:58 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * misc.c (pthreadCancelableWait): New function by John Bossom. Non-standard
+ but provides a hook that can be used to implement cancellation points in
+ applications that use this library.
+
+ * pthread.h (pthread_cleanup_pop): C++ (non-WIN32) version uses
+ try/catch to emulate John Bossom's WIN32 __try/__finally behaviour.
+ In the WIN32 version __finally block, add a test for AbnormalTermination otherwise
+ cleanup is only run if the cleanup_pop execute arg is non-zero. Cancellation
+ should cause the cleanup to run irrespective of the execute arg.
+
+ * condvar.c (pthread_condattr_init): Replaced by John Bossom's version.
+ (pthread_condattr_destroy): Replaced by John Bossom's version.
+ (pthread_condattr_getpshared): Replaced by John Bossom's version.
+ (pthread_condattr_setpshared): Replaced by John Bossom's version.
+ (pthread_cond_init): Replaced by John Bossom's version.
+ Fix comment (refered to mutex rather than condition variable).
+ (pthread_cond_destroy): Replaced by John Bossom's version.
+ (pthread_cond_wait): Replaced by John Bossom's version.
+ (pthread_cond_timedwait): Replaced by John Bossom's version.
+ (pthread_cond_signal): Replaced by John Bossom's version.
+ (pthread_cond_broadcast): Replaced by John Bossom's version.
+
+Thu Dec 17 19:10:46 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * tsd.c (pthread_key_create): Replaced by John Bossom's version.
+ (pthread_key_delete): Replaced by John Bossom's version.
+ (pthread_setspecific): Replaced by John Bossom's version.
+ (pthread_getspecific): Replaced by John Bossom's version.
+
+Mon Dec 7 09:44:40 1998 John Bossom
+
+ * cancel.c (pthread_setcancelstate): Replaced.
+ (pthread_setcanceltype): Replaced.
+ (pthread_testcancel): Replaced.
+ (pthread_cancel): Replaced.
+
+ * exit.c (pthread_exit): Replaced.
+
+ * misc.c (pthread_self): Replaced.
+ (pthread_equal): Replaced.
+
+ * sync.c (pthread_detach): Replaced.
+ (pthread_join): Replaced.
+
+ * create.c (pthread_create): Replaced.
+
+ * private.c (ptw32_processInitialize): New.
+ (ptw32_processTerminate): New.
+ (ptw32_threadStart): New.
+ (ptw32_threadDestroy): New.
+ (ptw32_cleanupStack): New.
+ (ptw32_tkAssocCreate): New.
+ (ptw32_tkAssocDestroy): New.
+ (ptw32_callUserDestroyRoutines): New.
+
+ * implement.h: Added non-API structures and declarations.
+
+ * dll.c (PthreadsEntryPoint): Cast return value of GetProcAddress
+ to resolve compile warning from MSVC.
+
+ * dll.c (DLLmain): Replaced.
+ * dll.c (PthreadsEntryPoint):
+ Re-applied Anders Norlander's patch:-
+ Initialize ptw32_try_enter_critical_section at startup
+ and release kernel32 handle when DLL is being unloaded.
+
+Sun Dec 6 21:54:35 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * buildlib.bat: Fix args to CL when building the .DLL
+
+ * cleanup.c (ptw32_destructor_run_all): Fix TSD key management.
+ This is a tidy-up before TSD and Thread management is completely
+ replaced by John Bossom's code.
+
+ * tsd.c (pthread_key_create): Fix TSD key management.
+
+ * global.c (ptw32_key_virgin_next): Initialise.
+
+ * build.bat: New DOS script to compile and link a pthreads app
+ using Microsoft's CL compiler linker.
+ * buildlib.bat: New DOS script to compile all the object files
+ and create pthread.lib and pthread.dll using Microsoft's CL
+ compiler linker.
+
+1998-12-05 Anders Norlander <anorland at hem2.passagen.se>
+
+ * implement.h (ptw32_try_enter_critical_section): New extern
+ * dll.c (ptw32_try_enter_critical_section): New pointer to
+ TryEnterCriticalSection if it exists; otherwise NULL.
+ * dll.c (PthreadsEntryPoint):
+ Initialize ptw32_try_enter_critical_section at startup
+ and release kernel32 handle when DLL is being unloaded.
+ * mutex.c (pthread_mutex_trylock): Replaced check for NT with
+ a check if ptw32_try_enter_critical_section is valid
+ pointer to a function. Call ptw32_try_enter_critical_section
+ instead of TryEnterCriticalSection to avoid errors on Win95.
+
+Thu Dec 3 13:32:00 1998 Ross Johnson <rpj at ise.canberra.edu.au>
+
+ * README: Correct cygwin32 compatibility statement.
+
+Sun Nov 15 21:24:06 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * cleanup.c (ptw32_destructor_run_all): Declare missing void * arg.
+ Fixup CVS merge conflicts.
+
+1998-10-30 Ben Elliston <bje at cygnus.com>
+
+ * condvar.c (cond_wait): Fix semantic error. Test for equality
+ instead of making an assignment.
+
+Fri Oct 30 15:15:50 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c (ptw32_handler_push): Fixed bug appending new
+ handler to list reported by Peter Slacik
+ <Peter.Slacik at leibinger.freinet.de>.
+ (new_thread): Rename poorly named local variable to
+ "new_handler".
+
+Sat Oct 24 18:34:59 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * global.c: Add TSD key management array and index declarations.
+
+ * implement.h: Ditto for externs.
+
+Fri Oct 23 00:08:09 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h (PTW32_TSD_KEY_REUSE): Add enum.
+
+ * private.c (ptw32_delete_thread): Add call to
+ ptw32_destructor_run_all() to clean up the threads keys.
+
+ * cleanup.c (ptw32_destructor_run_all): Check for no more dirty
+ keys to run destructors on. Assume that the destructor call always
+ succeeds and set the key value to NULL.
+
+Thu Oct 22 21:44:44 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * tsd.c (pthread_setspecific): Add key management code.
+ (pthread_key_create): Ditto.
+ (pthread_key_delete): Ditto.
+
+ * implement.h (struct ptw32_tsd_key): Add status member.
+
+ * tsd.c: Add description of pthread_key_delete() from the
+ standard as a comment.
+
+Fri Oct 16 17:38:47 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c (ptw32_destructor_run_all): Fix and improve
+ stepping through the key table.
+
+Thu Oct 15 14:05:01 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * private.c (ptw32_new_thread): Remove init of destructorstack.
+ No longer an element of pthread_t.
+
+ * tsd.c (pthread_setspecific): Fix type declaration and cast.
+ (pthread_getspecific): Ditto.
+ (pthread_getspecific): Change error return value to NULL if key
+ is not in use.
+
+Thu Oct 15 11:53:21 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * global.c (ptw32_tsd_key_table): Fix declaration.
+
+ * implement.h(ptw32_TSD_keys_TlsIndex): Add missing extern.
+ (ptw32_tsd_mutex): Ditto.
+
+ * create.c (ptw32_start_call): Fix "keys" array declaration.
+ Add comment.
+
+ * tsd.c (pthread_setspecific): Fix type declaration and cast.
+ (pthread_getspecific): Ditto.
+
+ * cleanup.c (ptw32_destructor_run_all): Declare missing loop
+ counter.
+
+Wed Oct 14 21:09:24 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_new_thread): Increment ptw32_threads_count.
+ (ptw32_delete_thread): Decrement ptw32_threads_count.
+ Remove some comments.
+
+ * exit.c (ptw32_exit): : Fix two pthread_mutex_lock() calls that
+ should have been pthread_mutex_unlock() calls.
+ (ptw32_vacuum): Remove call to ptw32_destructor_pop_all().
+
+ * create.c (pthread_create): Fix two pthread_mutex_lock() calls that
+ should have been pthread_mutex_unlock() calls.
+
+ * global.c (ptw32_tsd_mutex): Add mutex for TSD operations.
+
+ * tsd.c (pthread_key_create): Add critical section.
+ (pthread_setspecific): Ditto.
+ (pthread_getspecific): Ditto.
+ (pthread_key_delete): Ditto.
+
+ * sync.c (pthread_join): Fix two pthread_mutex_lock() calls that
+ should have been pthread_mutex_unlock() calls.
+
+Mon Oct 12 00:00:44 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h (ptw32_tsd_key_table): New.
+
+ * create.c (ptw32_start_call): Initialise per-thread TSD keys
+ to NULL.
+
+ * misc.c (pthread_once): Correct typo in comment.
+
+ * implement.h (ptw32_destructor_push): Remove.
+ (ptw32_destructor_pop): Remove.
+ (ptw32_destructor_run_all): Rename from ptw32_destructor_pop_all.
+ (PTW32_TSD_KEY_DELETED): Add enum.
+ (PTW32_TSD_KEY_INUSE): Add enum.
+
+ * cleanup.c (ptw32_destructor_push): Remove.
+ (ptw32_destructor_pop): Remove.
+ (ptw32_destructor_run_all): Totally revamped TSD.
+
+ * dll.c (ptw32_TSD_keys_TlsIndex): Initialise.
+
+ * tsd.c (pthread_setspecific): Totally revamped TSD.
+ (pthread_getspecific): Ditto.
+ (pthread_create): Ditto.
+ (pthread_delete): Ditto.
+
+Sun Oct 11 22:44:55 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * global.c (ptw32_tsd_key_table): Add new global.
+
+ * implement.h (ptw32_tsd_key_t and struct ptw32_tsd_key):
+ Add.
+ (struct _pthread): Remove destructorstack.
+
+ * cleanup.c (ptw32_destructor_run_all): Rename from
+ ptw32_destructor_pop_all. The key destructor stack was made
+ global rather than per-thread. No longer removes destructor nodes
+ from the stack. Comments updated.
+
+1998-10-06 Ben Elliston <bje at cygnus.com>
+
+ * condvar.c (cond_wait): Use POSIX, not Win32 mutex calls.
+ (pthread_cond_broadcast): Likewise.
+ (pthread_cond_signal): Likewise.
+
+1998-10-05 Ben Elliston <bje at cygnus.com>
+
+ * pthread.def: Update. Some functions aren't available yet, others
+ are macros in <pthread.h>.
+
+ * tests/join.c: Remove; useless.
+
+Mon Oct 5 14:25:08 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * pthread.def: New file for building the DLL.
+
+1998-10-05 Ben Elliston <bje at cygnus.com>
+
+ * misc.c (pthread_equal): Correct inverted logic bug.
+ (pthread_once): Use the POSIX mutex primitives, not Win32. Remove
+ irrelevant FIXME comment.
+
+ * global.c (PTHREAD_MUTEX_INITIALIZER): Move to pthread.h.
+
+ * pthread.h (PTHREAD_MUTEX_INITIALIZER): Define.
+ (pthread_mutex_t): Reimplement as a struct containing a valid
+ flag. If the flag is ever down upon entry to a mutex operation,
+ we call pthread_mutex_create() to initialise the object. This
+ fixes the problem of how to handle statically initialised objects
+ that can't call InitializeCriticalSection() due to their context.
+ (PTHREAD_ONCE_INIT): Define.
+
+ * mutex.c (pthread_mutex_init): Set valid flag.
+ (pthread_mutex_destroy): Clear valid flag.
+ (pthread_mutex_lock): Check and handle the valid flag.
+ (pthread_mutex_unlock): Likewise.
+ (pthread_mutex_trylock): Likewise.
+
+ * tests/mutex3.c: New file; test for the static initialisation
+ macro. Passes.
+
+ * tests/create1.c: New file; test pthread_create(). Passes.
+
+ * tests/equal.c: Poor test; remove.
+
+ * tests/equal1.c New file; test pthread_equal(). Passes.
+
+ * tests/once1.c: New file; test for pthread_once(). Passes.
+
+ * tests/self.c: Remove; rename to self1.c.
+
+ * tests/self1.c: This is the old self.c.
+
+ * tests/self2.c: New file. Test pthread_self() with a single
+ thread. Passes.
+
+ * tests/self3.c: New file. Test pthread_self() with a couple of
+ threads to ensure their thread IDs differ. Passes.
+
+1998-10-04 Ben Elliston <bje at cygnus.com>
+
+ * tests/mutex2.c: Test pthread_mutex_trylock(). Passes.
+
+ * tests/mutex1.c: New basic test for mutex functions (it passes).
+ (main): Eliminate warning.
+
+ * configure.in: Test for __stdcall, not _stdcall. Typo.
+
+ * configure: Regenerate.
+
+ * attr.c (pthread_attr_setstackaddr): Remove FIXME comment. Win32
+ does know about ENOSYS after all.
+ (pthread_attr_setstackaddr): Likewise.
+
+1998-10-03 Ben Elliston <bje at cygnus.com>
+
+ * configure.in: Test for the `_stdcall' keyword. Define `STDCALL'
+ to `_stdcall' if we have it, null otherwise.
+
+ * configure: Regenerate.
+
+ * acconfig.h (STDCALL): New define.
+
+ * config.h.in: Regenerate.
+
+ * create.c (ptw32_start_call): Add STDCALL prefix.
+
+ * mutex.c (pthread_mutex_init): Correct function signature.
+
+ * attr.c (pthread_attr_init): Only zero out the `sigmask' member
+ if we have the sigset_t type.
+
+ * pthread.h: No need to include <unistd.h>. It doesn't even exist
+ on Win32! Again, an artifact of cross-compilation.
+ (pthread_sigmask): Only provide if we have the sigset_t type.
+
+ * process.h: Remove. This was a stand-in before we started doing
+ native compilation under Win32.
+
+ * pthread.h (pthread_mutex_init): Make `attr' argument const.
+
+1998-10-02 Ben Elliston <bje at cygnus.com>
+
+ * COPYING: Remove.
+
+ * COPYING.LIB: Add. This library is under the LGPL.
+
+1998-09-13 Ben Elliston <bje at cygnus.com>
+
+ * configure.in: Test for required system features.
+
+ * configure: Generate.
+
+ * acconfig.h: New file.
+
+ * config.h.in: Generate.
+
+ * Makefile.in: Renamed from Makefile.
+
+ * COPYING: Import from a recent GNU package.
+
+ * config.guess: Likewise.
+
+ * config.sub: Likewise.
+
+ * install-sh: Likewise.
+
+ * config.h: Remove.
+
+ * Makefile: Likewise.
+
+1998-09-12 Ben Elliston <bje at cygnus.com>
+
+ * windows.h: No longer needed; remove.
+
+ * windows.c: Likewise.
+
+Sat Sep 12 20:09:24 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * windows.h: Remove error number definitions. These are in <errno.h>
+
+ * tsd.c: Add comment explaining rationale for not building
+ POSIX TSD on top of Win32 TLS.
+
+1998-09-12 Ben Elliston <bje at cygnus.com>
+
+ * {most}.c: Include <errno.h> to get POSIX error values.
+
+ * signal.c (pthread_sigmask): Only provide if HAVE_SIGSET_T is
+ defined.
+
+ * config.h: #undef features, don't #define them. This will be
+ generated by autoconf very soon.
+
+1998-08-11 Ben Elliston <bje at cygnus.com>
+
+ * Makefile (LIB): Define.
+ (clean): Define target.
+ (all): Build a library not just the object files.
+
+ * pthread.h: Provide a definition for struct timespec if we don't
+ already have one.
+
+ * windows.c (TlsGetValue): Bug fix.
+
+Thu Aug 6 15:19:22 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * misc.c (pthread_once): Fix arg 1 of EnterCriticalSection()
+ and LeaveCriticalSection() calls to pass address-of lock.
+
+ * fork.c (pthread_atfork): Typecast (void (*)(void *)) funcptr
+ in each ptw32_handler_push() call.
+
+ * exit.c (ptw32_exit): Fix attr arg in
+ pthread_attr_getdetachstate() call.
+
+ * private.c (ptw32_new_thread): Typecast (HANDLE) NULL.
+ (ptw32_delete_thread): Ditto.
+
+ * implement.h: (PTW32_MAX_THREADS): Add define. This keeps
+ changing in an attempt to make thread administration data types
+ opaque and cleanup DLL startup.
+
+ * dll.c (PthreadsEntryPoint):
+ (ptw32_virgins): Remove malloc() and free() calls.
+ (ptw32_reuse): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * global.c (_POSIX_THREAD_THREADS_MAX): Initialise with
+ PTW32_MAX_THREADS.
+ (ptw32_virgins): Ditto.
+ (ptw32_reuse): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * create.c (pthread_create): Typecast (HANDLE) NULL.
+ Typecast (unsigned (*)(void *)) start_routine.
+
+ * condvar.c (pthread_cond_init): Add address-of operator & to
+ arg 1 of pthread_mutex_init() call.
+ (pthread_cond_destroy): Add address-of operator & to
+ arg 1 of pthread_mutex_destroy() call.
+
+ * cleanup.c (ptw32_destructor_pop_all): Add (int) cast to
+ pthread_getspecific() arg.
+ (ptw32_destructor_pop): Add (void *) cast to "if" conditional.
+ (ptw32_destructor_push): Add (void *) cast to
+ ptw32_handler_push() "key" arg.
+ (malloc.h): Add include.
+
+ * implement.h (ptw32_destructor_pop): Add prototype.
+
+ * tsd.c (implement.h): Add include.
+
+ * sync.c (pthread_join): Remove target_thread_mutex and it's
+ initialisation. Rename getdetachedstate to getdetachstate.
+ Remove unused variable "exitcode".
+ (pthread_detach): Remove target_thread_mutex and it's
+ initialisation. Rename getdetachedstate to getdetachstate.
+ Rename setdetachedstate to setdetachstate.
+
+ * signal.c (pthread_sigmask): Rename SIG_SET to SIG_SETMASK.
+ Cast "set" to (long *) in assignment to passify compiler warning.
+ Add address-of operator & to thread->attr.sigmask in memcpy() call
+ and assignment.
+ (pthread_sigmask): Add address-of operator & to thread->attr.sigmask
+ in memcpy() call and assignment.
+
+ * windows.h (THREAD_PRIORITY_ERROR_RETURN): Add.
+ (THREAD_PRIORITY_LOWEST): Add.
+ (THREAD_PRIORITY_HIGHEST): Add.
+
+ * sched.c (is_attr): Add function.
+ (implement.h): Add include.
+ (pthread_setschedparam): Rename all instances of "sched_policy"
+ to "sched_priority".
+ (pthread_getschedparam): Ditto.
+
+Tue Aug 4 16:57:58 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * private.c (ptw32_delete_thread): Fix typo. Add missing ';'.
+
+ * global.c (ptw32_virgins): Change types from pointer to
+ array pointer.
+ (ptw32_reuse): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * implement.h(ptw32_virgins): Change types from pointer to
+ array pointer.
+ (ptw32_reuse): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * private.c (ptw32_delete_thread): Fix "entry" should be "thread".
+
+ * misc.c (pthread_self): Add extern for ptw32_threadID_TlsIndex.
+
+ * global.c: Add comment.
+
+ * misc.c (pthread_once): Fix member -> dereferences.
+ Change ptw32_once_flag to once_control->flag in "if" test.
+
+Tue Aug 4 00:09:30 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h(ptw32_virgins): Add extern.
+ (ptw32_virgin_next): Ditto.
+ (ptw32_reuse): Ditto.
+ (ptw32_reuse_top): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * global.c (ptw32_virgins): Changed from array to pointer.
+ Storage allocation for the array moved into dll.c.
+ (ptw32_reuse): Ditto.
+ (ptw32_win32handle_map): Ditto.
+ (ptw32_threads_mutex_table): Ditto.
+
+ * dll.c (PthreadsEntryPoint): Set up thread admin storage when
+ DLL is loaded.
+
+ * fork.c (pthread_atfork): Fix function pointer arg to all
+ ptw32_handler_push() calls. Change "arg" arg to NULL in child push.
+
+ * exit.c: Add windows.h and process.h includes.
+ (ptw32_exit): Add local detachstate declaration.
+ (ptw32_exit): Fix incorrect name for pthread_attr_getdetachstate().
+
+ * pthread.h (_POSIX_THREAD_ATTR_STACKSIZE): Move from global.c
+ (_POSIX_THREAD_ATTR_STACKADDR): Ditto.
+
+ * create.c (pthread_create): Fix #if should be #ifdef.
+ (ptw32_start_call): Remove usused variables.
+
+ * process.h: Create.
+
+ * windows.h: Move _beginthreadex and _endthreadex into
+ process.h
+
+Mon Aug 3 21:19:57 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * condvar.c (pthread_cond_init): Add NULL attr to
+ pthread_mutex_init() call - default attributes will be used.
+ (cond_wait): Fix typo.
+ (cond_wait): Fix typo - cv was ev.
+ (pthread_cond_broadcast): Fix two identical typos.
+
+ * cleanup.c (ptw32_destructor_pop_all): Remove _ prefix from
+ PTHREAD_DESTRUCTOR_ITERATIONS.
+
+ * pthread.h: Move _POSIX_* values into posix.h
+
+ * pthread.h: Fix typo in pthread_mutex_init() prototype.
+
+ * attr.c (pthread_attr_init): Fix error in priority member init.
+
+ * windows.h (THREAD_PRIORITY_NORMAL): Add.
+
+ * pthread.h (sched_param): Add missing ';' to struct definition.
+
+ * attr.c (pthread_attr_init): Remove obsolete pthread_attr_t
+ member initialisation - cancelstate, canceltype, cancel_pending.
+ (is_attr): Make arg "attr" a const.
+
+ * implement.h (PTW32_HANDLER_POP_LIFO): Remove definition.
+ (PTW32_HANDLER_POP_FIFO): Ditto.
+ (PTW32_VALID): Add missing newline escape (\).
+ (ptw32_handler_node): Make element "next" a pointer.
+
+1998-08-02 Ben Elliston <bje at cygnus.com>
+
+ * windows.h: Remove duplicate TlsSetValue() prototype. Add
+ TlsGetValue() prototype.
+ (FALSE): Define.
+ (TRUE): Likewise.
+ Add forgotten errno values. Guard against multiple #includes.
+
+ * windows.c: New file. Implement stubs for Win32 functions.
+
+ * Makefile (SRCS): Remove. Not explicitly needed.
+ (CFLAGS): Add -Wall for all warnings with GCC.
+
+Sun Aug 2 19:03:42 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * config.h: Create. This is a temporary stand-in for autoconf yet
+ to be done.
+ (HAVE_SIGNAL_H): Add.
+
+ * pthread.h: Minor rearrangement for temporary config.h.
+
+Fri Jul 31 14:00:29 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * cleanup.c (ptw32_destructor_pop): Implement. Removes
+ destructors associated with a key without executing them.
+ (ptw32_destructor_pop_all): Add FIXME comment.
+
+ * tsd.c (pthread_key_delete): Add call to ptw32_destructor_pop().
+
+Fri Jul 31 00:05:45 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * tsd.c (pthread_key_create): Update to properly associate
+ the destructor routine with the key.
+ (pthread_key_delete): Add FIXME comment.
+
+ * exit.c (ptw32_vacuum): Add call to
+ ptw32_destructor_pop_all().
+
+ * implement.h (ptw32_handler_pop_all): Add prototype.
+ (ptw32_destructor_pop_all): Ditto.
+
+ * cleanup.c (ptw32_destructor_push): Implement. This is just a
+ call to ptw32_handler_push().
+ (ptw32_destructor_pop_all): Implement. This is significantly
+ different to ptw32_handler_pop_all().
+
+ * Makefile (SRCS): Create. Preliminary.
+
+ * windows.h: Create. Contains Win32 definitions for compile
+ testing. This is just a standin for the real one.
+
+ * pthread.h (SIG_UNBLOCK): Fix typo. Was SIG_BLOCK.
+ (windows.h): Add include. Required for CRITICAL_SECTION.
+ (pthread_cond_t): Move enum declaration outside of struct
+ definition.
+ (unistd.h): Add include - may be temporary.
+
+ * condvar.c (windows.h): Add include.
+
+ * implement.h (PTW32_THIS): Remove - no longer required.
+ (PTW32_STACK): Use pthread_self() instead of PTW32_THIS.
+
+Thu Jul 30 23:12:45 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Remove ptw32_find_entry() prototype.
+
+ * private.c: Extend comments.
+ Remove ptw32_find_entry() - no longer needed.
+
+ * create.c (ptw32_start_call): Add call to TlsSetValue() to
+ store the thread ID.
+
+ * dll.c (PthreadsEntryPoint): Implement. This is called
+ whenever a process loads the DLL. Used to initialise thread
+ local storage.
+
+ * implement.h: Add ptw32_threadID_TlsIndex.
+ Add ()s around PTW32_VALID expression.
+
+ * misc.c (pthread_self): Re-implement using Win32 TLS to store
+ the threads own ID.
+
+Wed Jul 29 11:39:03 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c: Corrections in comments.
+ (ptw32_new_thread): Alter "if" flow to be more natural.
+
+ * cleanup.c (ptw32_handler_push): Same as below.
+
+ * create.c (pthread_create): Same as below.
+
+ * private.c (ptw32_new_thread): Rename "new" to "new_thread".
+ Since when has a C programmer been required to know C++?
+
+Tue Jul 28 14:04:29 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * implement.h: Add PTW32_VALID macro.
+
+ * sync.c (pthread_join): Modify to use the new thread
+ type and ptw32_delete_thread(). Rename "target" to "thread".
+ Remove extra local variable "target".
+ (pthread_detach): Ditto.
+
+ * signal.c (pthread_sigmask): Move init of "us" out of inner block.
+ Fix instance of "this" should have been "us". Rename "us" to "thread".
+
+ * sched.c (pthread_setschedparam): Modify to use the new thread
+ type.
+ (pthread_getschedparam): Ditto.
+
+ * private.c (ptw32_find_thread): Fix return type and arg.
+
+ * implement.h: Remove PTW32_YES and PTW32_NO.
+ (ptw32_new_thread): Add prototype.
+ (ptw32_find_thread): Ditto.
+ (ptw32_delete_thread): Ditto.
+ (ptw32_new_thread_entry): Remove prototype.
+ (ptw32_find_thread_entry): Ditto.
+ (ptw32_delete_thread_entry): Ditto.
+ ( PTW32_NEW, PTW32_INUSE, PTW32_EXITED, PTW32_REUSE):
+ Add.
+
+
+ * create.c (pthread_create): Minor rename "us" to "new" (I need
+ these cues but it doesn't stop me coming out with some major bugs
+ at times).
+ Load start_routine and arg into the thread so the wrapper can
+ call it.
+
+ * exit.c (pthread_exit): Fix pthread_this should be pthread_self.
+
+ * cancel.c (pthread_setcancelstate): Change
+ ptw32_threads_thread_t * to pthread_t and init with
+ pthread_this().
+ (pthread_setcanceltype): Ditto.
+
+ * exit.c (ptw32_exit): Add new pthread_t arg.
+ Rename ptw32_delete_thread_entry to ptw32_delete_thread.
+ Rename "us" to "thread".
+ (pthread_exit): Call ptw32_exit with added thread arg.
+
+ * create.c (ptw32_start_call): Insert missing ")".
+ Add "us" arg to ptw32_exit() call.
+ (pthread_create): Modify to use new thread allocation scheme.
+
+ * private.c: Added detailed explanation of the new thread
+ allocation scheme.
+ (ptw32_new_thread): Totally rewritten to use
+ new thread allocation scheme.
+ (ptw32_delete_thread): Ditto.
+ (ptw32_find_thread): Obsolete.
+
+Mon Jul 27 17:46:37 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * create.c (pthread_create): Start of rewrite. Not completed yet.
+
+ * private.c (ptw32_new_thread_entry): Start of rewrite. Not
+ complete.
+
+ * implement.h (ptw32_threads_thread): Rename, remove thread
+ member, add win32handle and ptstatus members.
+ (ptw32_t): Add.
+
+ * pthread.h: pthread_t is no longer mapped directly to a Win32
+ HANDLE type. This is so we can let the Win32 thread terminate and
+ reuse the HANDLE while pthreads holds it's own thread ID until
+ the last waiting join exits.
+
+Mon Jul 27 00:20:37 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_delete_thread_entry): Destroy the thread
+ entry attribute object before deleting the thread entry itself.
+
+ * attr.c (pthread_attr_init): Initialise cancel_pending = FALSE.
+ (pthread_attr_setdetachstate): Rename "detached" to "detachedstate".
+ (pthread_attr_getdetachstate): Ditto.
+
+ * exit.c (ptw32_exit): Fix incorrect check for detachedstate.
+
+ * implement.h (ptw32_call_t): Remove env member.
+
+Sun Jul 26 13:06:12 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h (ptw32_new_thread_entry): Fix prototype.
+ (ptw32_find_thread_entry): Ditto.
+ (ptw32_delete_thread_entry): Ditto.
+ (ptw32_exit): Add prototype.
+
+ * exit.c (ptw32_exit): New function. Called from pthread_exit()
+ and ptw32_start_call() to exit the thread. It allows an extra
+ argument which is the return code passed to _endthreadex().
+ (ptw32_exit): Move thread entry delete call from ptw32_vacuum()
+ into here. Add more explanation of thread entry deletion.
+ (ptw32_exit): Clarify comment.
+
+ * create.c (ptw32_start_call): Change pthread_exit() call to
+ ptw32_exit() call.
+
+ * exit.c (ptw32_vacuum): Add thread entry deletion code
+ moved from ptw32_start_call(). See next item.
+ (pthread_exit): Remove longjmp(). Add mutex lock around thread table
+ manipulation code. This routine now calls _enthreadex().
+
+ * create.c (ptw32_start_call): Remove setjmp() call and move
+ cleanup code out. Call pthread_exit(NULL) to terminate the thread.
+
+1998-07-26 Ben Elliston <bje at cygnus.com>
+
+ * tsd.c (pthread_getspecific): Update comments.
+
+ * mutex.c (pthread_mutexattr_setpshared): Not supported; remove.
+ (pthread_mutexattr_getpshared): Likewise.
+
+ * pthread.h (pthread_mutexattr_setpshared): Remove prototype.
+ (pthread_mutexattr_getpshared): Likewise.
+
+Sun Jul 26 00:09:59 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c: Rename all instances of ptw32_count_mutex to
+ ptw32_table_mutex.
+
+ * implement.h: Rename ptw32_count_mutex to
+ ptw32_table_mutex.
+
+ * global.c: Rename ptw32_count_mutex to
+ ptw32_table_mutex.
+
+ * create.c (pthread_create): Add critical sections.
+ (ptw32_start_call): Rename ptw32_count_mutex to
+ ptw32_table_mutex.
+
+ * cancel.c (pthread_setcancelstate): Fix indirection bug and rename
+ "this" to "us".
+
+ * signal.c (pthread_sigmask): Rename "this" to "us" and fix some
+ minor syntax errors. Declare "us" and initialise it.
+
+ * sync.c (pthread_detach): Rename "this" to "target".
+
+ * pthread.h: Converting PTHREAD_* defines to alias the (const int)
+ values in global.c.
+
+ * global.c: Started converting PTHREAD_* defines to (const int) as
+ a part of making the eventual pthreads DLL binary compatible
+ through version changes.
+
+ * condvar.c (cond_wait): Add cancelation point. This applies the
+ point to both pthread_cond_wait() and pthread_cond_timedwait().
+
+ * exit.c (pthread_exit): Rename "this" to "us".
+
+ * implement.h: Add comment.
+
+ * sync.c (pthread_join): I've satisfied myself that pthread_detach()
+ does set the detached attribute in the thread entry attributes
+ to PTHREAD_CREATE_DETACHED. "if" conditions were changed to test
+ that attribute instead of a separate flag.
+
+ * create.c (pthread_create): Rename "this" to "us".
+ (pthread_create): cancelstate and canceltype are not attributes
+ so the copy to thread entry attribute storage was removed.
+ Only the thread itself can change it's cancelstate or canceltype,
+ ie. the thread must exist already.
+
+ * private.c (ptw32_delete_thread_entry): Mutex locks removed.
+ Mutexes must be applied at the caller level.
+ (ptw32_new_thread_entry): Ditto.
+ (ptw32_new_thread_entry): Init cancelstate, canceltype, and
+ cancel_pending to default values.
+ (ptw32_new_thread_entry): Rename "this" to "new".
+ (ptw32_find_thread_entry): Rename "this" to "entry".
+ (ptw32_delete_thread_entry): Rename "thread_entry" to "entry".
+
+ * create.c (ptw32_start_call): Mutexes changed to
+ ptw32_count_mutex. All access to the threads table entries is
+ under the one mutex. Otherwise chaos reigns.
+
+Sat Jul 25 23:16:51 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h (ptw32_threads_thread): Move cancelstate and
+ canceltype members out of pthread_attr_t into here.
+
+ * fork.c (fork): Add comment.
+
+1998-07-25 Ben Elliston <bje at cygnus.com>
+
+ * fork.c (fork): Autoconfiscate.
+
+Sat Jul 25 00:00:13 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * create.c (ptw32_start_call): Set thread priority. Ensure our
+ thread entry is removed from the thread table but only if
+ pthread_detach() was called and there are no waiting joins.
+ (pthread_create): Set detach flag in thread entry if the
+ thread is created PTHREAD_CREATE_DETACHED.
+
+ * pthread.h (pthread_attr_t): Rename member "detachedstate".
+
+ * attr.c (pthread_attr_init): Rename attr members.
+
+ * exit.c (pthread_exit): Fix indirection mistake.
+
+ * implement.h (PTW32_THREADS_TABLE_INDEX): Add.
+
+ * exit.c (ptw32_vacuum): Fix incorrect args to
+ ptw32_handler_pop_all() calls.
+ Make thread entry removal conditional.
+
+ * sync.c (pthread_join): Add multiple join and async detach handling.
+
+ * implement.h (PTW32_THREADS_TABLE_INDEX): Add.
+
+ * global.c (ptw32_threads_mutex_table): Add.
+
+ * implement.h (ptw32_once_flag): Remove.
+ (ptw32_once_lock): Ditto.
+ (ptw32_threads_mutex_table): Add.
+
+ * global.c (ptw32_once_flag): Remove.
+ (ptw32_once_lock): Ditto.
+
+ * sync.c (pthread_join): Fix tests involving new return value
+ from ptw32_find_thread_entry().
+ (pthread_detach): Ditto.
+
+ * private.c (ptw32_find_thread_entry): Failure return code
+ changed from -1 to NULL.
+
+Fri Jul 24 23:09:33 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * create.c (pthread_create): Change . to -> in sigmask memcpy() args.
+
+ * pthread.h: (pthread_cancel): Add function prototype.
+ (pthread_testcancel): Ditto.
+
+1998-07-24 Ben Elliston <bje at cygnus.com>
+
+ * pthread.h (pthread_condattr_t): Rename dummy structure member.
+ (pthread_mutexattr_t): Likewise.
+
+Fri Jul 24 21:13:55 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * cancel.c (pthread_cancel): Implement.
+ (pthread_testcancel): Implement.
+
+ * exit.c (pthread_exit): Add comment explaining the longjmp().
+
+ * implement.h (ptw32_threads_thread_t): New member cancelthread.
+ (PTW32_YES): Define.
+ (PTW32_NO): Define.
+ (RND_SIZEOF): Remove.
+
+ * create.c (pthread_create): Rename cancelability to cancelstate.
+
+ * pthread.h (pthread_attr_t): Rename cancelability to cancelstate.
+ (PTHREAD_CANCELED): Define.
+
+1998-07-24 Ben Elliston <bje at cygnus.com>
+
+ * pthread.h (SIG_BLOCK): Define if not already defined.
+ (SIG_UNBLOCK): Likewise.
+ (SIG_SETMASK): Likewise.
+ (pthread_attr_t): Add signal mask member.
+ (pthread_sigmask): Add function prototype.
+
+ * signal.c (pthread_sigmask): Implement.
+
+ * create.c: #include <string.h> to get a prototype for memcpy().
+ (pthread_create): New threads inherit their creator's signal
+ mask. Copy the signal mask to the new thread structure if we know
+ about signals.
+
+Fri Jul 24 16:33:17 1998 Ross Johnson <rpj at swan.canberra.edu.au>
+
+ * fork.c (pthread_atfork): Add all the necessary push calls.
+ Local implementation semantics:
+ If we get an ENOMEM at any time then ALL handlers
+ (including those from previous pthread_atfork() calls) will be
+ popped off each of the three atfork stacks before we return.
+ (fork): Add all the necessary pop calls. Add the thread cancellation
+ and join calls to the child fork.
+ Add #includes.
+
+ * implement.h: (ptw32_handler_push): Fix return type and stack arg
+ type in prototype.
+ (ptw32_handler_pop): Fix stack arg type in prototype.
+ (ptw32_handler_pop_all): Fix stack arg type in prototype.
+
+ * cleanup.c (ptw32_handler_push): Change return type to int and
+ return ENOMEM if malloc() fails.
+
+ * sync.c (pthread_detach): Use equality test, not assignment.
+
+ * create.c (ptw32_start_call): Add call to Win32 CloseHandle()
+ if thread is detached.
+
+1998-07-24 Ben Elliston <bje at cygnus.com>
+
+ * sync.c (pthread_detach): Close the Win32 thread handle to
+ emulate detached (or daemon) threads.
+
+Fri Jul 24 03:00:25 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c (pthread_join): Save valueptr arg in joinvalueptr for
+ pthread_exit() to use.
+
+ * private.c (ptw32_new_thread_entry): Initialise joinvalueptr to
+ NULL.
+
+ * create.c (ptw32_start_call): Rewrite to facilitate joins.
+ pthread_exit() will do a longjmp() back to here. Does appropriate
+ cleanup and exit/return from the thread.
+ (pthread_create): _beginthreadex() now passes a pointer to our
+ thread table entry instead of just the call member of that entry.
+
+ * implement.h (ptw32_threads_thread): New member
+ void ** joinvalueptr.
+ (ptw32_call_t): New member jmpbuf env.
+
+ * exit.c (pthread_exit): Major rewrite to handle joins and handing
+ value pointer to joining thread. Uses longjmp() back to
+ ptw32_start_call().
+
+ * create.c (pthread_create): Ensure values of new attribute members
+ are copied to the thread attribute object.
+
+ * attr.c (pthread_attr_destroy): Fix merge conflicts.
+ (pthread_attr_getdetachstate): Fix merge conflicts.
+ (pthread_attr_setdetachstate): Fix merge conflicts.
+
+ * pthread.h: Fix merge conflicts.
+
+ * sync.c (pthread_join): Fix merge conflicts.
+
+Fri Jul 24 00:21:21 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * sync.c (pthread_join): Add check for valid and joinable
+ thread.
+ (pthread_detach): Implement. After checking for a valid and joinable
+ thread, it's still a no-op.
+
+ * private.c (ptw32_find_thread_entry): Bug prevented returning
+ an error value in some cases.
+
+ * attr.c (pthread_attr_setdetachedstate): Implement.
+ (pthread_attr_getdetachedstate): Implement.
+
+ * implement.h: Move more hidden definitions into here from
+ pthread.h.
+
+1998-07-24 Ben Elliston <bje at cygnus.com>
+
+ * pthread.h (PTHREAD_CREATE_JOINABLE): Define.
+ (PTHREAD_CREATE_DETACHED): Likewise.
+ (pthread_attr_t): Add new structure member `detached'.
+ (pthread_attr_getdetachstate): Add function prototype.
+ (pthread_attr_setdetachstate): Likewise.
+
+ * sync.c (pthread_join): Return if the target thread is detached.
+
+ * attr.c (pthread_attr_init): Initialise cancelability and
+ canceltype structure members.
+ (pthread_attr_getdetachstate): Implement.
+ (pthread_attr_setdetachstate): Likewise.
+
+ * implement.h (PTW32_CANCEL_DEFAULTS): Remove. Bit fields
+ proved to be too cumbersome. Set the defaults in attr.c using the
+ public PTHREAD_CANCEL_* constants.
+
+ * cancel.c: New file.
+
+ * pthread.h (sched_param): Define this type.
+ (pthread_attr_getschedparam): Add function prototype.
+ (pthread_attr_setschedparam): Likewise.
+ (pthread_setcancelstate): Likewise.
+ (pthread_setcanceltype): Likewise.
+ (sched_get_priority_min): Likewise.
+ (sched_get_priority_max): Likewise.
+ (pthread_mutexattr_setprotocol): Remove; not supported.
+ (pthread_mutexattr_getprotocol): Likewise.
+ (pthread_mutexattr_setprioceiling): Likewise.
+ (pthread_mutexattr_getprioceiling): Likewise.
+ (pthread_attr_t): Add canceltype member. Update comments.
+ (SCHED_OTHER): Define this scheduling policy constant.
+ (SCHED_FIFO): Likewise.
+ (SCHED_RR): Likewise.
+ (SCHED_MIN): Define the lowest possible value for this constant.
+ (SCHED_MAX): Likewise, the maximum possible value.
+ (PTHREAD_CANCEL_ASYNCHRONOUS): Redefine.
+ (PTHREAD_CANCEL_DEFERRED): Likewise.
+
+ * sched.c: New file.
+ (pthread_setschedparam): Implement.
+ (pthread_getschedparam): Implement.
+ (sched_get_priority_max): Validate policy argument.
+ (sched_get_priority_min): Likewise.
+
+ * mutex.c (pthread_mutexattr_setprotocol): Remove; not supported.
+ (pthread_mutexattr_getprotocol): Likewise.
+ (pthread_mutexattr_setprioceiling): Likewise.
+ (pthread_mutexattr_getprioceiling): Likewise.
+
+Fri Jul 24 00:21:21 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * create.c (pthread_create): Arg to ptw32_new_thread_entry()
+ changed. See next entry. Move mutex locks out. Changes made yesterday
+ and today allow us to start the new thread running rather than
+ temporarily suspended.
+
+ * private.c (ptw32_new_thread_entry): ptw32_thread_table
+ was changed back to a table of thread structures rather than pointers.
+ As such we're trading storage for increaded speed. This routine
+ was modified to work with the new table. Mutex lock put in around
+ global data accesses.
+ (ptw32_find_thread_entry): Ditto
+ (ptw32_delete_thread_entry): Ditto
+
+Thu Jul 23 23:25:30 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * global.c: New. Global data objects declared here. These moved from
+ pthread.h.
+
+ * pthread.h: Move implementation hidden definitions into
+ implement.h.
+
+ * implement.h: Move implementation hidden definitions from
+ pthread.h. Add constants to index into the different handler stacks.
+
+ * cleanup.c (ptw32_handler_push): Simplify args. Restructure.
+ (ptw32_handler_pop): Simplify args. Restructure.
+ (ptw32_handler_pop_all): Simplify args. Restructure.
+
+Wed Jul 22 00:16:22 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * attr.c, implement.h, pthread.h, ChangeLog: Resolve CVS merge
+ conflicts.
+
+ * private.c (ptw32_find_thread_entry): Changes to return type
+ to support leaner ptw32_threads_table[] which now only stores
+ ptw32_thread_thread_t *.
+ (ptw32_new_thread_entry): Internal changes.
+ (ptw32_delete_thread_entry): Internal changes to avoid contention.
+ Calling routines changed accordingly.
+
+ * pthread.h: Modified cleanup macros to use new generic push and pop.
+ Added destructor and atfork stacks to ptw32_threads_thread_t.
+
+ * cleanup.c (ptw32_handler_push, ptw32_handler_pop,
+ ptw32_handler_pop_all): Renamed cleanup push and pop routines
+ and made generic to handle destructors and atfork handlers as
+ well.
+
+ * create.c (ptw32_start_call): New function is a wrapper for
+ all new threads. It allows us to do some cleanup when the thread
+ returns, ie. that is otherwise only done if the thread is cancelled.
+
+ * exit.c (ptw32_vacuum): New function contains code from
+ pthread_exit() that we need in the new ptw32_start_call()
+ as well.
+
+ * implement.h: Various additions and minor changes.
+
+ * pthread.h: Various additions and minor changes.
+ Change cleanup handler macros to use generic handler push and pop
+ functions.
+
+ * attr.c: Minor mods to all functions.
+ (is_attr): Implemented missing function.
+
+ * create.c (pthread_create): More clean up.
+
+ * private.c (ptw32_find_thread_entry): Implement.
+ (ptw32_delete_thread_entry): Implement.
+ (ptw32_new_thread_entry): Implement.
+ These functions manipulate the implementations internal thread
+ table and are part of general code cleanup and modularisation.
+ They replace ptw32_getthreadindex() which was removed.
+
+ * exit.c (pthread_exit): Changed to use the new code above.
+
+ * pthread.h: Add cancelability constants. Update comments.
+
+1998-07-22 Ben Elliston <bje at cygnus.com>
+
+ * attr.c (pthread_setstacksize): Update test of attr argument.
+ (pthread_getstacksize): Likewise.
+ (pthread_setstackaddr): Likewise.
+ (pthread_getstackaddr): Likewise.
+ (pthread_attr_init): No need to allocate any storage.
+ (pthread_attr_destroy): No need to free any storage.
+
+ * mutex.c (is_attr): Not likely to be needed; remove.
+ (remove_attr): Likewise.
+ (insert_attr): Likewise.
+
+ * implement.h (ptw32_mutexattr_t): Moved to a public definition
+ in pthread.h. There was little gain in hiding these details.
+ (ptw32_condattr_t): Likewise.
+ (ptw32_attr_t): Likewise.
+
+ * pthread.h (pthread_atfork): Add function prototype.
+ (pthread_attr_t): Moved here from implement.h.
+
+ * fork.c (pthread_atfork): Preliminary implementation.
+ (ptw32_fork): Likewise.
+
+Wed Jul 22 00:16:22 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * cleanup.c (ptw32_cleanup_push): Implement.
+ (ptw32_cleanup_pop): Implement.
+ (ptw32_do_cancellation): Implement.
+ These are private to the implementation. The real cleanup functions
+ are macros. See below.
+
+ * pthread.h (pthread_cleanup_push): Implement as a macro.
+ (pthread_cleanup_pop): Implement as a macro.
+ Because these are macros which start and end a block, the POSIX scoping
+ requirement is observed. See the comment in the file.
+
+ * exit.c (pthread_exit): Refine the code.
+
+ * create.c (pthread_create): Code cleanup.
+
+ * implement.h (RND_SIZEOF): Add RND_SIZEOF(T) to round sizeof(T)
+ up to multiple of DWORD.
+ Add function prototypes.
+
+ * private.c (ptw32_getthreadindex): "*thread" should have been
+ "thread". Detect empty slot fail condition.
+
+1998-07-20 Ben Elliston <bje at cygnus.com>
+
+ * misc.c (pthread_once): Implement. Don't use a per-application
+ flag and mutex--make `pthread_once_t' contain these elements in
+ their structure. The earlier version had incorrect semantics.
+
+ * pthread.h (ptw32_once_flag): Add new variable. Remove.
+ (ptw32_once_lock): Add new mutex lock to ensure integrity of
+ access to ptw32_once_flag. Remove.
+ (pthread_once): Add function prototype.
+ (pthread_once_t): Define this type.
+
+Mon Jul 20 02:31:05 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * private.c (ptw32_getthreadindex): Implement.
+
+ * pthread.h: Add application static data dependent on
+ _PTHREADS_BUILD_DLL define. This is needed to avoid allocating
+ non-sharable static data within the pthread DLL.
+
+ * implement.h: Add ptw32_cleanup_stack_t, ptw32_cleanup_node_t
+ and PTW32_HASH_INDEX.
+
+ * exit.c (pthread_exit): Begin work on cleanup and de-allocate
+ thread-private storage.
+
+ * create.c (pthread_create): Add thread to thread table.
+ Keep a thread-private copy of the attributes with default values
+ filled in when necessary. Same for the cleanup stack. Make
+ pthread_create C run-time library friendly by using _beginthreadex()
+ instead of CreateThread(). Fix error returns.
+
+Sun Jul 19 16:26:23 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Rename pthreads_thread_count to ptw32_threads_count.
+ Create ptw32_threads_thread_t struct to keep thread specific data.
+
+ * create.c: Rename pthreads_thread_count to ptw32_threads_count.
+ (pthread_create): Handle errors from CreateThread().
+
+1998-07-19 Ben Elliston <bje at cygnus.com>
+
+ * condvar.c (pthread_cond_wait): Generalise. Moved from here ..
+ (cond_wait): To here.
+ (pthread_cond_timedwait): Implement; use generalised cond_wait().
+
+ * pthread.h (pthread_key_t): Define this type.
+ (pthread_key_create): Add function prototype.
+ (pthread_setspecific): Likewise.
+ (pthread_getspecific): Likwise.
+ (pthread_key_delete): Likewise.
+
+ * tsd.c (pthread_key_create): Implement.
+ (pthread_setspecific): Likewise.
+ (pthread_getspecific): Likewise.
+ (pthread_key_delete): Likewise.
+
+ * mutex.c (pthread_mutex_trylock): Return ENOSYS if this function
+ is called on a Win32 platform which is not Windows NT.
+
+1998-07-18 Ben Elliston <bje at cygnus.com>
+
+ * condvar.c (pthread_condattr_init): Do not attempt to malloc any
+ storage; none is needed now that condattr_t is an empty struct.
+ (pthread_condattr_destory): Likewise; do not free storage.
+ (pthread_condattr_setpshared): No longer supported; return ENOSYS.
+ (pthread_condattr_getpshared): Likewise.
+ (pthread_cond_init): Implement with help from Douglas Schmidt.
+ Remember to initialise the cv's internal mutex.
+ (pthread_cond_wait): Likewise.
+ (pthread_cond_signal): Likewise.
+ (pthread_cond_broadcast): Likewise.
+ (pthread_cond_timedwait): Preliminary implementation, but I need
+ to see some API documentation for `WaitForMultipleObject'.
+ (pthread_destory): Implement.
+
+ * pthread.h (pthread_cond_init): Add function protoype.
+ (pthread_cond_broadcast): Likewise.
+ (pthread_cond_signal): Likewise.
+ (pthread_cond_timedwait): Likewise.
+ (pthread_cond_wait): Likewise.
+ (pthread_cond_destroy): Likewise.
+ (pthread_cond_t): Define this type. Fix for u_int. Do not assume
+ that the mutex contained withing the pthread_cond_t structure will
+ be a critical section. Use our new POSIX type!
+
+ * implement.h (ptw32_condattr_t): Remove shared attribute.
+
+1998-07-17 Ben Elliston <bje at cygnus.com>
+
+ * pthread.h (PTHREADS_PROCESS_PRIVATE): Remove.
+ (PTHREAD_PROCESS_SHARED): Likewise. No support for mutexes shared
+ across processes for now.
+ (pthread_mutex_t): Use a Win32 CRITICAL_SECTION type for better
+ performance.
+
+ * implement.h (ptw32_mutexattr_t): Remove shared attribute.
+
+ * mutex.c (pthread_mutexattr_setpshared): This optional function
+ is no longer supported, since we want to implement POSIX mutex
+ variables using the much more efficient Win32 critical section
+ primitives. Critical section objects in Win32 cannot be shared
+ between processes.
+ (pthread_mutexattr_getpshared): Likewise.
+ (pthread_mutexattr_init): No need to malloc any storage; the
+ attributes structure is now empty.
+ (pthread_mutexattr_destroy): This is now a nop.
+ (pthread_mutex_init): Use InitializeCriticalSection().
+ (pthread_mutex_destroy): Use DeleteCriticalSection().
+ (pthread_mutex_lock): Use EnterCriticalSection().
+ (pthread_mutex_trylock): Use TryEnterCriticalSection(). This is
+ not supported by Windows 9x, but trylock is a hack anyway, IMHO.
+ (pthread_mutex_unlock): Use LeaveCriticalSection().
+
+1998-07-14 Ben Elliston <bje at cygnus.com>
+
+ * attr.c (pthread_attr_setstacksize): Implement.
+ (pthread_attr_getstacksize): Likewise.
+ (pthread_attr_setstackaddr): Likewise.
+ (pthread_attr_getstackaddr): Likewise.
+ (pthread_attr_init): Likewise.
+ (pthread_attr_destroy): Likewise.
+
+ * condvar.c (pthread_condattr_init): Add `_cond' to function name.
+
+ * mutex.c (pthread_mutex_lock): Add `_mutex' to function name.
+ (pthread_mutex_trylock): Likewise.
+ (pthread_mutex_unlock): Likewise.
+
+ * pthread.h (pthread_condattr_setpshared): Fix typo.
+ (pthread_attr_init): Add function prototype.
+ (pthread_attr_destroy): Likewise.
+ (pthread_attr_setstacksize): Likewise.
+ (pthread_attr_getstacksize): Likewise.
+ (pthread_attr_setstackaddr): Likewise.
+ (pthread_attr_getstackaddr): Likewise.
+
+Mon Jul 13 01:09:55 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Wrap in #ifndef _IMPLEMENT_H
+
+ * create.c (pthread_create): Map stacksize attr to Win32.
+
+ * mutex.c: Include implement.h
+
+1998-07-13 Ben Elliston <bje at cygnus.com>
+
+ * condvar.c (pthread_condattr_init): Implement.
+ (pthread_condattr_destroy): Likewise.
+ (pthread_condattr_setpshared): Likewise.
+ (pthread_condattr_getpshared): Likewise.
+
+ * implement.h (PTHREAD_THREADS_MAX): Remove trailing semicolon.
+ (PTHREAD_STACK_MIN): Specify; needs confirming.
+ (ptw32_attr_t): Define this type.
+ (ptw32_condattr_t): Likewise.
+
+ * pthread.h (pthread_mutex_t): Define this type.
+ (pthread_condattr_t): Likewise.
+ (pthread_mutex_destroy): Add function prototype.
+ (pthread_lock): Likewise.
+ (pthread_trylock): Likewise.
+ (pthread_unlock): Likewise.
+ (pthread_condattr_init): Likewise.
+ (pthread_condattr_destroy): Likewise.
+ (pthread_condattr_setpshared): Likewise.
+ (pthread_condattr_getpshared): Likewise.
+
+ * mutex.c (pthread_mutex_init): Implement.
+ (pthread_mutex_destroy): Likewise.
+ (pthread_lock): Likewise.
+ (pthread_trylock): Likewise.
+ (pthread_unlock): Likewise.
+
+1998-07-12 Ben Elliston <bje at cygnus.com>
+
+ * implement.h (ptw32_mutexattr_t): Define this implementation
+ internal type. Application programmers only see a mutex attribute
+ object as a void pointer.
+
+ * pthread.h (pthread_mutexattr_t): Define this type.
+ (pthread_mutexattr_init): Add function prototype.
+ (pthread_mutexattr_destroy): Likewise.
+ (pthread_mutexattr_setpshared): Likewise.
+ (pthread_mutexattr_getpshared): Likewise.
+ (pthread_mutexattr_setprotocol): Likewise.
+ (pthread_mutexattr_getprotocol): Likewise.
+ (pthread_mutexattr_setprioceiling): Likewise.
+ (pthread_mutexattr_getprioceiling): Likewise.
+ (PTHREAD_PROCESS_PRIVATE): Define.
+ (PTHREAD_PROCESS_SHARED): Define.
+
+ * mutex.c (pthread_mutexattr_init): Implement.
+ (pthread_mutexattr_destroy): Implement.
+ (pthread_mutexattr_setprotocol): Implement.
+ (pthread_mutexattr_getprotocol): Likewise.
+ (pthread_mutexattr_setprioceiling): Likewise.
+ (pthread_mutexattr_getprioceiling): Likewise.
+ (pthread_mutexattr_setpshared): Likewise.
+ (pthread_mutexattr_getpshared): Likewise.
+ (insert_attr): New function; very preliminary implementation!
+ (is_attr): Likewise.
+ (remove_attr): Likewise.
+
+Sat Jul 11 14:48:54 1998 Ross Johnson <rpj at ixobrychus.canberra.edu.au>
+
+ * implement.h: Preliminary implementation specific defines.
+
+ * create.c (pthread_create): Preliminary implementation.
+
+1998-07-11 Ben Elliston <bje at cygnus.com>
+
+ * sync.c (pthread_join): Implement.
+
+ * misc.c (pthread_equal): Likewise.
+
+ * pthread.h (pthread_join): Add function prototype.
+ (pthread_equal): Likewise.
+
+1998-07-10 Ben Elliston <bje at cygnus.com>
+
+ * misc.c (pthread_self): Implement.
+
+ * exit.c (pthread_exit): Implement.
+
+ * pthread.h (pthread_exit): Add function prototype.
+ (pthread_self): Likewise.
+ (pthread_t): Define this type.
+
+1998-07-09 Ben Elliston <bje at cygnus.com>
+
+ * create.c (pthread_create): A dummy stub right now.
+
+ * pthread.h (pthread_create): Add function prototype.
diff --git a/3rdparty/pthreads-win32/FAQ b/3rdparty/pthreads-win32/FAQ
new file mode 100644
index 0000000..cb1786c
--- /dev/null
+++ b/3rdparty/pthreads-win32/FAQ
@@ -0,0 +1,451 @@
+ =========================================
+ PTHREADS-WIN32 Frequently Asked Questions
+ =========================================
+
+INDEX
+-----
+
+Q 1 What is it?
+
+Q 2 Which of the several dll versions do I use?
+ or,
+ What are all these pthread*.dll and pthread*.lib files?
+
+Q 3 What is the library naming convention?
+
+Q 4 Cleanup code default style or: it used to work when I built
+ the library myself, but now it doesn't - why?
+
+Q 5 Why is the default library version now less exception-friendly?
+
+Q 6 Should I use Cygwin or Mingw32 as a development environment?
+
+Q 7 Now that pthreads-win32 builds under Mingw32, why do I get
+ memory access violations (segfaults)?
+
+Q 8 How do I use pthread.dll for Win32 (Visual C++ 5.0)
+
+Q 9 Cancelation doesn't work for me, why?
+
+Q 10 How do I generate pthreadGCE.dll and libpthreadw32.a for use
+ with Mingw32?
+
+Q 11 Why isn't pthread_t defined as a scalar (e.g. pointer or int)
+ like it is for other POSIX threads implementations?
+
+=============================================================================
+
+Q 1 What is it?
+---
+
+Pthreads-win32 is an Open Source Software implementation of the
+Threads component of the POSIX 1003.1c 1995 Standard for Microsoft's
+Win32 environment. Some functions from POSIX 1003.1b are also
+supported including semaphores. Other related functions include
+the set of read-write lock functions. The library also supports
+some of the functionality of the Open Group's Single Unix
+specification, version 2, namely mutex types.
+
+See the file "ANNOUNCE" for more information including standards
+conformance details and list of supported routines.
+
+
+------------------------------------------------------------------------------
+
+Q 2 Which of the several dll versions do I use?
+--- or,
+ What are all these pthread*.dll and pthread*.lib files?
+
+Simply, you only use one of them, but you need to choose carefully.
+
+The most important choice you need to make is whether to use a
+version that uses exceptions internally, or not (there are versions
+of the library that use exceptions as part of the thread
+cancelation and cleanup implementation, and one that uses
+setjmp/longjmp instead).
+
+There is some contension amongst POSIX threads experts as
+to how POSIX threads cancelation and exit should work
+with languages that include exceptions and handlers, e.g.
+C++ and even C (Microsoft's Structured Exceptions).
+
+The issue is: should cancelation of a thread in, say,
+a C++ application cause object destructors and C++ exception
+handlers to be invoked as the stack unwinds during thread
+exit, or not?
+
+There seems to be more opinion in favour of using the
+standard C version of the library (no EH) with C++ applications
+since this appears to be the assumption commercial pthreads
+implementations make. Therefore, if you use an EH version
+of pthreads-win32 then you may be under the illusion that
+your application will be portable, when in fact it is likely to
+behave very differently linked with other pthreads libraries.
+
+Now you may be asking: why have you kept the EH versions of
+the library?
+
+There are a couple of reasons:
+- there is division amongst the experts and so the code may
+ be needed in the future. (Yes, it's in the repository and we
+ can get it out anytime in the future, but ...)
+- pthreads-win32 is one of the few implementations, and possibly
+ the only freely available one, that has EH versions. It may be
+ useful to people who want to play with or study application
+ behaviour under these conditions.
+
+
+------------------------------------------------------------------------------
+
+Q 3 What is the library naming convention?
+---
+
+Because the library is being built using various exception
+handling schemes and compilers - and because the library
+may not work reliably if these are mixed in an application,
+each different version of the library has it's own name.
+
+Note 1: the incompatibility is really between EH implementations
+of the different compilers. It should be possible to use the
+standard C version from either compiler with C++ applications
+built with a different compiler. If you use an EH version of
+the library, then you must use the same compiler for the
+application. This is another complication and dependency that
+can be avoided by using only the standard C library version.
+
+Note 2: if you use a standard C pthread*.dll with a C++
+application, then any functions that you define that are
+intended to be called via pthread_cleanup_push() must be
+__cdecl.
+
+Note 3: the intention is to also name either the VC or GC
+version (it should be arbitrary) as pthread.dll, including
+pthread.lib and libpthread.a as appropriate.
+
+In general:
+ pthread[VG]{SE,CE,C}.dll
+ pthread[VG]{SE,CE,C}.lib
+
+where:
+ [VG] indicates the compiler
+ V - MS VC
+ G - GNU C
+
+ {SE,CE,C} indicates the exception handling scheme
+ SE - Structured EH
+ CE - C++ EH
+ C - no exceptions - uses setjmp/longjmp
+
+For example:
+ pthreadVSE.dll (MSVC/SEH)
+ pthreadGCE.dll (GNUC/C++ EH)
+ pthreadGC.dll (GNUC/not dependent on exceptions)
+
+The GNU library archive file names have changed to:
+
+ libpthreadGCE.a
+ libpthreadGC.a
+
+
+------------------------------------------------------------------------------
+
+Q 4 Cleanup code default style or: it used to work when I built
+--- the library myself, but now it doesn't - why?
+
+Up to and including snapshot 2001-07-12, if not defined, the cleanup
+style was determined automatically from the compiler used, and one
+of the following was defined accordingly:
+
+ __CLEANUP_SEH MSVC only
+ __CLEANUP_CXX C++, including MSVC++, GNU G++
+ __CLEANUP_C C, including GNU GCC, not MSVC
+
+These defines determine the style of cleanup (see pthread.h) and,
+most importantly, the way that cancelation and thread exit (via
+pthread_exit) is performed (see the routine ptw32_throw() in private.c).
+
+In short, the exceptions versions of the library throw an exception
+when a thread is canceled or exits (via pthread_exit()), which is
+caught by a handler in the thread startup routine, so that the
+the correct stack unwinding occurs regardless of where the thread
+is when it's canceled or exits via pthread_exit().
+
+After snapshot 2001-07-12, unless your build explicitly defines (e.g.
+via a compiler option) __CLEANUP_SEH, __CLEANUP_CXX, or __CLEANUP_C, then
+the build now ALWAYS defaults to __CLEANUP_C style cleanup. This style
+uses setjmp/longjmp in the cancelation and pthread_exit implementations,
+and therefore won't do stack unwinding even when linked to applications
+that have it (e.g. C++ apps). This is for consistency with most/all
+commercial Unix POSIX threads implementations.
+
+Although it was not clearly documented before, it is still necessary to
+build your application using the same __CLEANUP_* define as was
+used for the version of the library that you link with, so that the
+correct parts of pthread.h are included. That is, the possible
+defines require the following library versions:
+
+ __CLEANUP_SEH pthreadVSE.dll
+ __CLEANUP_CXX pthreadVCE.dll or pthreadGCE.dll
+ __CLEANUP_C pthreadVC.dll or pthreadGC.dll
+
+THE POINT OF ALL THIS IS: if you have not been defining one of these
+explicitly, then the defaults have been set according to the compiler
+and language you are using, as described at the top of this
+section.
+
+THIS NOW CHANGES, as has been explained above. For example:
+
+If you were building your application with MSVC++ i.e. using C++
+exceptions (rather than SEH) and not explicitly defining one of
+__CLEANUP_*, then __CLEANUP_C++ was defined for you in pthread.h.
+You should have been linking with pthreadVCE.dll, which does
+stack unwinding.
+
+If you now build your application as you had before, pthread.h will now
+set __CLEANUP_C as the default style, and you will need to link
+with pthreadVC.dll. Stack unwinding will now NOT occur when a
+thread is canceled, nor when the thread calls pthread_exit().
+
+Your application will now most likely behave differently to previous
+versions, and in non-obvious ways. Most likely is that local
+objects may not be destroyed or cleaned up after a thread
+is canceled.
+
+If you want the same behaviour as before, then you must now define
+__CLEANUP_C++ explicitly using a compiler option and link with
+pthreadVCE.dll as you did before.
+
+
+------------------------------------------------------------------------------
+
+Q 5 Why is the default library version now less exception-friendly?
+---
+
+Because most commercial Unix POSIX threads implementations don't allow you to
+choose to have stack unwinding. (Compaq's TRU64 Unix is possibly an exception.)
+
+Therefore, providing it in pthread-win32 as a default could be dangerous
+and non-portable. We still provide the choice but you must now consciously
+make it.
+
+WHY NOT REMOVE THE EXCEPTIONS VERSIONS OF THE LIBRARY ALTOGETHER?
+There are a few reasons:
+- because there are well respected POSIX threads people who believe
+ that POSIX threads implementations should be exceptions-aware and
+ do the expected thing in that context. (There are equally respected
+ people who believe it should not be easily accessible, if it's there
+ at all.)
+- because pthreads-win32 is one of the few implementations that has
+ the choice, perhaps the only freely available one, and so offers
+ a laboratory to people who may want to explore the effects;
+- although the code will always be around somewhere for anyone who
+ wants it, once it's removed from the current version it will not be
+ nearly as visible to people who may have a use for it.
+
+
+------------------------------------------------------------------------------
+
+Q 6 Should I use Cygwin or Mingw32 as a development environment?
+---
+
+Important: see Q7 also.
+
+Use Mingw32 with the MSVCRT library to build applications that use
+the pthreads DLL.
+
+Cygwin's own internal support for POSIX threads is growing.
+Consult that project's documentation for more information.
+
+------------------------------------------------------------------------------
+
+Q 7 Now that pthreads-win32 builds under Mingw32, why do I get
+--- memory access violations (segfaults)?
+
+The latest Mingw32 package has thread-safe exception handling (see Q10).
+Also, see Q6 above.
+
+------------------------------------------------------------------------------
+
+Q 8 How do I use pthread.dll for Win32 (Visual C++ 5.0)
+---
+
+>
+> I'm a "rookie" when it comes to your pthread implementation. I'm currently
+> desperately trying to install the prebuilt .dll file into my MSVC compiler.
+> Could you please provide me with explicit instructions on how to do this (or
+> direct me to a resource(s) where I can acquire such information)?
+>
+> Thank you,
+>
+
+You should have a .dll, .lib, .def, and three .h files. It is recommended
+that you use pthreadVC.dll, rather than pthreadVCE.dll or pthreadVSE.dll
+(see Q2 above).
+
+The .dll can go in any directory listed in your PATH environment
+variable, so putting it into C:\WINDOWS should work.
+
+The .lib file can go in any directory listed in your LIB environment
+variable.
+
+The .h files can go in any directory listed in your INCLUDE
+environment variable.
+
+Or you might prefer to put the .lib and .h files into a new directory
+and add its path to LIB and INCLUDE. You can probably do this easiest
+by editing the file:-
+
+C:\Program Files\DevStudio\vc\bin\vcvars32.bat
+
+The .def file isn't used by anything in the pre-compiled version but
+is included for information.
+
+Cheers.
+Ross
+
+------------------------------------------------------------------------------
+
+Q 9 Cancelation doesn't work for me, why?
+---
+
+> I'm investigating a problem regarding thread cancelation. The thread I want
+> to cancel has PTHREAD_CANCEL_ASYNCHRONOUS, however, this piece of code
+> blocks on the join():
+>
+> if ((retv = Pthread_cancel( recvThread )) == 0)
+> {
+> retv = Pthread_join( recvThread, 0 );
+> }
+>
+> Pthread_* are just macro's; they call pthread_*.
+>
+> The thread recvThread seems to block on a select() call. It doesn't get
+> cancelled.
+>
+> Two questions:
+>
+> 1) is this normal behaviour?
+>
+> 2) if not, how does the cancel mechanism work? I'm not very familliar to
+> win32 programming, so I don't really understand how the *Event() family of
+> calls work.
+
+The answer to your first question is, normal POSIX behaviour would
+be to asynchronously cancel the thread. However, even that doesn't
+guarantee cancelation as the standard only says it should be
+cancelled as soon as possible.
+
+Snapshot 99-11-02 or earlier only partially supports asynchronous cancellation.
+Snapshots since then simulate async cancelation by poking the address of
+a cancelation routine into the PC of the threads context. This requires
+the thread to be resumed in some way for the cancelation to actually
+proceed. This is not true async cancelation, but it is as close as we've
+been able to get to it.
+
+If the thread you're trying to cancel is blocked (for instance, it could be
+waiting for data from the network), it will only get cancelled when it unblocks
+(when the data arrives). For true pre-emptive cancelation in these cases,
+pthreads-win32 from snapshot 2004-05-16 can automatically recognise and use the
+QueueUserAPCEx package by Panagiotis E. Hadjidoukas. This package is available
+from the pthreads-win32 ftp site and is included in the pthreads-win32
+self-unpacking zip from 2004-05-16 onwards.
+
+Using deferred cancelation would normally be the way to go, however,
+even though the POSIX threads standard lists a number of C library
+functions that are defined as deferred cancelation points, there is
+no hookup between those which are provided by Windows and the
+pthreads-win32 library.
+
+Incidently, it's worth noting for code portability that the older POSIX
+threads standards cancelation point lists didn't include "select" because
+(as I read in Butenhof) it wasn't part of POSIX. However, it does appear in
+the SUSV3.
+
+Effectively, the only mandatory cancelation points that pthreads-win32
+recognises are those the library implements itself, ie.
+
+ pthread_testcancel
+ pthread_cond_wait
+ pthread_cond_timedwait
+ pthread_join
+ sem_wait
+ sem_timedwait
+ pthread_delay_np
+
+The following routines from the non-mandatory list in SUSV3 are
+cancelation points in pthreads-win32:
+
+ pthread_rwlock_wrlock
+ pthread_rwlock_timedwrlock
+
+The following routines from the non-mandatory list in SUSV3 are not
+cancelation points in pthreads-win32:
+
+ pthread_rwlock_rdlock
+ pthread_rwlock_timedrdlock
+
+Pthreads-win32 also provides two functions that allow you to create
+cancelation points within your application, but only for cases where
+a thread is going to block on a Win32 handle. These are:
+
+ pthreadCancelableWait(HANDLE waitHandle) /* Infinite wait */
+
+ pthreadCancelableTimedWait(HANDLE waitHandle, DWORD timeout)
+
+------------------------------------------------------------------------------
+
+
+Q 10 How do I create thread-safe applications using
+---- pthreadGCE.dll, libpthreadw32.a and Mingw32?
+
+This should not be a problem with recent versions of MinGW32.
+
+For early versions, see Thomas Pfaff's email at:
+http://sources.redhat.com/ml/pthreads-win32/2002/msg00000.html
+------------------------------------------------------------------------------
+
+Q 11 Why isn't pthread_t defined as a scalar (e.g. pointer or int)
+ like it is for other POSIX threads implementations?
+----
+
+Originally pthread_t was defined as a pointer (to the opaque pthread_t_
+struct) and later it was changed to a struct containing the original
+pointer plus a sequence counter. This is allowed under both the original
+POSIX Threads Standard and the current Single Unix Specification.
+
+When pthread_t is a simple pointer to a struct some very difficult to
+debug problems arise from the process of freeing and later allocing
+thread structs because new pthread_t handles can acquire the identity of
+previously detached threads. The change to a struct was made, along with
+some changes to their internal managment, in order to guarantee (for
+practical applications) that the pthread_t handle will be unique over the
+life of the running process.
+
+Where application code attempts to compare one pthread_t against another
+directly, a compiler error will be emitted because structs can't be
+compared at that level. This should signal a potentially serious problem
+in the code design, which would go undetected if pthread_t was a scalar.
+
+The POSIX Threading API provides a function named pthread_equal() to
+compare pthread_t thread handles.
+
+Other pthreads implementations, such as Sun's, use an int as the handle
+but do guarantee uniqueness within the process scope. Win32 scalar typed
+thread handles also guarantee uniqueness in system scope. It wasn't clear
+how well the internal management of these handles would scale as the
+number of threads and the fragmentation of the sequence numbering
+increased for applications where thousands or millions of threads are
+created and detached over time. The current management of threads within
+pthreads-win32 using structs for pthread_t, and reusing without ever
+freeing them, reduces the management time overheads to a constant, which
+could be important given that pthreads-win32 threads are built on top of
+Win32 threads and will therefore include that management overhead on top
+of their own. The cost is that the memory resources used for thread
+handles will remain at the peak level until the process exits.
+
+While it may be inconvenient for developers to be forced away from making
+assumptions about the internals of pthread_t, the advantage for the
+future development of pthread-win32, as well as those applications that
+use it and other pthread implementations, is that the library is free to
+change pthread_t internals and management as better methods arise.
+
diff --git a/3rdparty/pthreads-win32/GNUmakefile b/3rdparty/pthreads-win32/GNUmakefile
new file mode 100644
index 0000000..e489f00
--- /dev/null
+++ b/3rdparty/pthreads-win32/GNUmakefile
@@ -0,0 +1,593 @@
+#
+# --------------------------------------------------------------------------
+#
+# Pthreads-win32 - POSIX Threads Library for Win32
+# Copyright(C) 1998 John E. Bossom
+# Copyright(C) 1999,2005 Pthreads-win32 contributors
+#
+# Contact Email: rpj at callisto.canberra.edu.au
+#
+# The current list of contributors is contained
+# in the file CONTRIBUTORS included with the source
+# code distribution. The list can also be seen at the
+# following World Wide Web location:
+# http://sources.redhat.com/pthreads-win32/contributors.html
+#
+# This library is free software; you can redistribute it and/or
+# modify it under the terms of the GNU Lesser General Public
+# License as published by the Free Software Foundation; either
+# version 2 of the License, or (at your option) any later version.
+#
+# This library is distributed in the hope that it will be useful,
+# but WITHOUT ANY WARRANTY; without even the implied warranty of
+# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+# Lesser General Public License for more details.
+#
+# You should have received a copy of the GNU Lesser General Public
+# License along with this library in the file COPYING.LIB;
+# if not, write to the Free Software Foundation, Inc.,
+# 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+#
+
+DLL_VER = 2
+DLL_VERD= $(DLL_VER)d
+
+DEVROOT = C:\PTHREADS
+
+DLLDEST = $(DEVROOT)\DLL
+LIBDEST = $(DEVROOT)\DLL
+
+# If Running MsysDTK
+RM = rm -f
+MV = mv -f
+CP = cp -f
+
+# If not.
+#RM = erase
+#MV = rename
+#CP = copy
+
+# For cross compiling use e.g.
+# make CROSS=x86_64-w64-mingw32- clean GC-inlined
+CROSS =
+
+AR = $(CROSS)ar
+DLLTOOL = $(CROSS)dlltool
+CC = $(CROSS)gcc
+CXX = $(CROSS)g++
+RANLIB = $(CROSS)ranlib
+RC = $(CROSS)windres
+
+OPT = $(CLEANUP) -O3 # -finline-functions -findirect-inlining
+XOPT =
+
+RCFLAGS = --include-dir=.
+# Uncomment this if config.h defines RETAIN_WSALASTERROR
+#LFLAGS = -lws2_32
+
+# ----------------------------------------------------------------------
+# The library can be built with some alternative behaviour to
+# facilitate development of applications on Win32 that will be ported
+# to other POSIX systems. Nothing definable here will make the library
+# non-compliant, but applications that make assumptions that POSIX
+# does not garrantee may fail or misbehave under some settings.
+#
+# PTW32_THREAD_ID_REUSE_INCREMENT
+# Purpose:
+# POSIX says that applications should assume that thread IDs can be
+# recycled. However, Solaris and some other systems use a [very large]
+# sequence number as the thread ID, which provides virtual uniqueness.
+# Pthreads-win32 provides pseudo-unique IDs when the default increment
+# (1) is used, but pthread_t is not a scalar type like Solaris's.
+#
+# Usage:
+# Set to any value in the range: 0 <= value <= 2^wordsize
+#
+# Examples:
+# Set to 0 to emulate non recycle-unique behaviour like Linux or *BSD.
+# Set to 1 for recycle-unique thread IDs (this is the default).
+# Set to some other +ve value to emulate smaller word size types
+# (i.e. will wrap sooner).
+#
+#PTW32_FLAGS = "-DPTW32_THREAD_ID_REUSE_INCREMENT=0"
+#
+# ----------------------------------------------------------------------
+
+GC_CFLAGS = $(PTW32_FLAGS)
+GCE_CFLAGS = $(PTW32_FLAGS) -mthreads
+
+## Mingw
+MAKE ?= make
+CFLAGS = $(OPT) $(XOPT) -I. -DHAVE_PTW32_CONFIG_H -Wall
+
+DLL_INLINED_OBJS = \
+ pthread.o \
+ version.o
+
+# Agregate modules for inlinability
+DLL_OBJS = \
+ attr.o \
+ barrier.o \
+ cancel.o \
+ cleanup.o \
+ condvar.o \
+ create.o \
+ dll.o \
+ errno.o \
+ exit.o \
+ fork.o \
+ global.o \
+ misc.o \
+ mutex.o \
+ nonportable.o \
+ private.o \
+ rwlock.o \
+ sched.o \
+ semaphore.o \
+ signal.o \
+ spin.o \
+ sync.o \
+ tsd.o \
+ version.o
+
+# Separate modules for minimum size statically linked images
+SMALL_STATIC_OBJS = \
+ pthread_attr_init.o \
+ pthread_attr_destroy.o \
+ pthread_attr_getdetachstate.o \
+ pthread_attr_setdetachstate.o \
+ pthread_attr_getstackaddr.o \
+ pthread_attr_setstackaddr.o \
+ pthread_attr_getstacksize.o \
+ pthread_attr_setstacksize.o \
+ pthread_attr_getscope.o \
+ pthread_attr_setscope.o \
+ pthread_attr_setschedpolicy.o \
+ pthread_attr_getschedpolicy.o \
+ pthread_attr_setschedparam.o \
+ pthread_attr_getschedparam.o \
+ pthread_attr_setinheritsched.o \
+ pthread_attr_getinheritsched.o \
+ pthread_barrier_init.o \
+ pthread_barrier_destroy.o \
+ pthread_barrier_wait.o \
+ pthread_barrierattr_init.o \
+ pthread_barrierattr_destroy.o \
+ pthread_barrierattr_setpshared.o \
+ pthread_barrierattr_getpshared.o \
+ pthread_setcancelstate.o \
+ pthread_setcanceltype.o \
+ pthread_testcancel.o \
+ pthread_cancel.o \
+ cleanup.o \
+ pthread_condattr_destroy.o \
+ pthread_condattr_getpshared.o \
+ pthread_condattr_init.o \
+ pthread_condattr_setpshared.o \
+ pthread_cond_destroy.o \
+ pthread_cond_init.o \
+ pthread_cond_signal.o \
+ pthread_cond_wait.o \
+ create.o \
+ dll.o \
+ autostatic.o \
+ errno.o \
+ pthread_exit.o \
+ fork.o \
+ global.o \
+ pthread_mutex_init.o \
+ pthread_mutex_destroy.o \
+ pthread_mutexattr_init.o \
+ pthread_mutexattr_destroy.o \
+ pthread_mutexattr_getpshared.o \
+ pthread_mutexattr_setpshared.o \
+ pthread_mutexattr_settype.o \
+ pthread_mutexattr_gettype.o \
+ pthread_mutexattr_setrobust.o \
+ pthread_mutexattr_getrobust.o \
+ pthread_mutex_lock.o \
+ pthread_mutex_timedlock.o \
+ pthread_mutex_unlock.o \
+ pthread_mutex_trylock.o \
+ pthread_mutex_consistent.o \
+ pthread_mutexattr_setkind_np.o \
+ pthread_mutexattr_getkind_np.o \
+ pthread_getw32threadhandle_np.o \
+ pthread_getunique_np.o \
+ pthread_delay_np.o \
+ pthread_num_processors_np.o \
+ pthread_win32_attach_detach_np.o \
+ pthread_equal.o \
+ pthread_getconcurrency.o \
+ pthread_once.o \
+ pthread_self.o \
+ pthread_setconcurrency.o \
+ pthread_rwlock_init.o \
+ pthread_rwlock_destroy.o \
+ pthread_rwlockattr_init.o \
+ pthread_rwlockattr_destroy.o \
+ pthread_rwlockattr_getpshared.o \
+ pthread_rwlockattr_setpshared.o \
+ pthread_rwlock_rdlock.o \
+ pthread_rwlock_wrlock.o \
+ pthread_rwlock_unlock.o \
+ pthread_rwlock_tryrdlock.o \
+ pthread_rwlock_trywrlock.o \
+ pthread_setschedparam.o \
+ pthread_getschedparam.o \
+ pthread_timechange_handler_np.o \
+ ptw32_is_attr.o \
+ ptw32_cond_check_need_init.o \
+ ptw32_MCS_lock.o \
+ ptw32_mutex_check_need_init.o \
+ ptw32_processInitialize.o \
+ ptw32_processTerminate.o \
+ ptw32_threadStart.o \
+ ptw32_threadDestroy.o \
+ ptw32_tkAssocCreate.o \
+ ptw32_tkAssocDestroy.o \
+ ptw32_callUserDestroyRoutines.o \
+ ptw32_timespec.o \
+ ptw32_throw.o \
+ ptw32_getprocessors.o \
+ ptw32_calloc.o \
+ ptw32_new.o \
+ ptw32_reuse.o \
+ ptw32_semwait.o \
+ ptw32_relmillisecs.o \
+ ptw32_rwlock_check_need_init.o \
+ sched_get_priority_max.o \
+ sched_get_priority_min.o \
+ sched_setscheduler.o \
+ sched_getscheduler.o \
+ sched_yield.o \
+ sem_init.o \
+ sem_destroy.o \
+ sem_trywait.o \
+ sem_timedwait.o \
+ sem_wait.o \
+ sem_post.o \
+ sem_post_multiple.o \
+ sem_getvalue.o \
+ sem_open.o \
+ sem_close.o \
+ sem_unlink.o \
+ signal.o \
+ pthread_kill.o \
+ ptw32_spinlock_check_need_init.o \
+ pthread_spin_init.o \
+ pthread_spin_destroy.o \
+ pthread_spin_lock.o \
+ pthread_spin_unlock.o \
+ pthread_spin_trylock.o \
+ pthread_detach.o \
+ pthread_join.o \
+ pthread_key_create.o \
+ pthread_key_delete.o \
+ pthread_setspecific.o \
+ pthread_getspecific.o \
+ w32_CancelableWait.o \
+ version.o
+
+INCL = \
+ config.h \
+ implement.h \
+ semaphore.h \
+ pthread.h \
+ need_errno.h
+
+ATTR_SRCS = \
+ pthread_attr_init.c \
+ pthread_attr_destroy.c \
+ pthread_attr_getdetachstate.c \
+ pthread_attr_setdetachstate.c \
+ pthread_attr_getstackaddr.c \
+ pthread_attr_setstackaddr.c \
+ pthread_attr_getstacksize.c \
+ pthread_attr_setstacksize.c \
+ pthread_attr_getscope.c \
+ pthread_attr_setscope.c
+
+BARRIER_SRCS = \
+ pthread_barrier_init.c \
+ pthread_barrier_destroy.c \
+ pthread_barrier_wait.c \
+ pthread_barrierattr_init.c \
+ pthread_barrierattr_destroy.c \
+ pthread_barrierattr_setpshared.c \
+ pthread_barrierattr_getpshared.c
+
+CANCEL_SRCS = \
+ pthread_setcancelstate.c \
+ pthread_setcanceltype.c \
+ pthread_testcancel.c \
+ pthread_cancel.c
+
+CONDVAR_SRCS = \
+ ptw32_cond_check_need_init.c \
+ pthread_condattr_destroy.c \
+ pthread_condattr_getpshared.c \
+ pthread_condattr_init.c \
+ pthread_condattr_setpshared.c \
+ pthread_cond_destroy.c \
+ pthread_cond_init.c \
+ pthread_cond_signal.c \
+ pthread_cond_wait.c
+
+EXIT_SRCS = \
+ pthread_exit.c
+
+MISC_SRCS = \
+ pthread_equal.c \
+ pthread_getconcurrency.c \
+ pthread_kill.c \
+ pthread_once.c \
+ pthread_self.c \
+ pthread_setconcurrency.c \
+ ptw32_calloc.c \
+ ptw32_MCS_lock.c \
+ ptw32_new.c \
+ ptw32_reuse.c \
+ w32_CancelableWait.c
+
+MUTEX_SRCS = \
+ ptw32_mutex_check_need_init.c \
+ pthread_mutex_init.c \
+ pthread_mutex_destroy.c \
+ pthread_mutexattr_init.c \
+ pthread_mutexattr_destroy.c \
+ pthread_mutexattr_getpshared.c \
+ pthread_mutexattr_setpshared.c \
+ pthread_mutexattr_settype.c \
+ pthread_mutexattr_gettype.c \
+ pthread_mutexattr_setrobust.c \
+ pthread_mutexattr_getrobust.c \
+ pthread_mutex_lock.c \
+ pthread_mutex_timedlock.c \
+ pthread_mutex_unlock.c \
+ pthread_mutex_trylock.c \
+ pthread_mutex_consistent.c
+
+NONPORTABLE_SRCS = \
+ pthread_mutexattr_setkind_np.c \
+ pthread_mutexattr_getkind_np.c \
+ pthread_getw32threadhandle_np.c \
+ pthread_getunique_np.c \
+ pthread_delay_np.c \
+ pthread_num_processors_np.c \
+ pthread_win32_attach_detach_np.c \
+ pthread_timechange_handler_np.c
+
+PRIVATE_SRCS = \
+ ptw32_is_attr.c \
+ ptw32_processInitialize.c \
+ ptw32_processTerminate.c \
+ ptw32_threadStart.c \
+ ptw32_threadDestroy.c \
+ ptw32_tkAssocCreate.c \
+ ptw32_tkAssocDestroy.c \
+ ptw32_callUserDestroyRoutines.c \
+ ptw32_semwait.c \
+ ptw32_relmillisecs.c \
+ ptw32_timespec.c \
+ ptw32_throw.c \
+ ptw32_getprocessors.c
+
+RWLOCK_SRCS = \
+ ptw32_rwlock_check_need_init.c \
+ ptw32_rwlock_cancelwrwait.c \
+ pthread_rwlock_init.c \
+ pthread_rwlock_destroy.c \
+ pthread_rwlockattr_init.c \
+ pthread_rwlockattr_destroy.c \
+ pthread_rwlockattr_getpshared.c \
+ pthread_rwlockattr_setpshared.c \
+ pthread_rwlock_rdlock.c \
+ pthread_rwlock_timedrdlock.c \
+ pthread_rwlock_wrlock.c \
+ pthread_rwlock_timedwrlock.c \
+ pthread_rwlock_unlock.c \
+ pthread_rwlock_tryrdlock.c \
+ pthread_rwlock_trywrlock.c
+
+SCHED_SRCS = \
+ pthread_attr_setschedpolicy.c \
+ pthread_attr_getschedpolicy.c \
+ pthread_attr_setschedparam.c \
+ pthread_attr_getschedparam.c \
+ pthread_attr_setinheritsched.c \
+ pthread_attr_getinheritsched.c \
+ pthread_setschedparam.c \
+ pthread_getschedparam.c \
+ sched_get_priority_max.c \
+ sched_get_priority_min.c \
+ sched_setscheduler.c \
+ sched_getscheduler.c \
+ sched_yield.c
+
+SEMAPHORE_SRCS = \
+ sem_init.c \
+ sem_destroy.c \
+ sem_trywait.c \
+ sem_timedwait.c \
+ sem_wait.c \
+ sem_post.c \
+ sem_post_multiple.c \
+ sem_getvalue.c \
+ sem_open.c \
+ sem_close.c \
+ sem_unlink.c
+
+SPIN_SRCS = \
+ ptw32_spinlock_check_need_init.c \
+ pthread_spin_init.c \
+ pthread_spin_destroy.c \
+ pthread_spin_lock.c \
+ pthread_spin_unlock.c \
+ pthread_spin_trylock.c
+
+SYNC_SRCS = \
+ pthread_detach.c \
+ pthread_join.c
+
+TSD_SRCS = \
+ pthread_key_create.c \
+ pthread_key_delete.c \
+ pthread_setspecific.c \
+ pthread_getspecific.c
+
+
+GCE_DLL = pthreadGCE$(DLL_VER).dll
+GCED_DLL= pthreadGCE$(DLL_VERD).dll
+GCE_LIB = libpthreadGCE$(DLL_VER).a
+GCED_LIB= libpthreadGCE$(DLL_VERD).a
+GCE_INLINED_STAMP = pthreadGCE$(DLL_VER).stamp
+GCED_INLINED_STAMP = pthreadGCE$(DLL_VERD).stamp
+GCE_STATIC_STAMP = libpthreadGCE$(DLL_VER).stamp
+GCED_STATIC_STAMP = libpthreadGCE$(DLL_VERD).stamp
+
+GC_DLL = pthreadGC$(DLL_VER).dll
+GCD_DLL = pthreadGC$(DLL_VERD).dll
+GC_LIB = libpthreadGC$(DLL_VER).a
+GCD_LIB = libpthreadGC$(DLL_VERD).a
+GC_INLINED_STAMP = pthreadGC$(DLL_VER).stamp
+GCD_INLINED_STAMP = pthreadGC$(DLL_VERD).stamp
+GC_STATIC_STAMP = libpthreadGC$(DLL_VER).stamp
+GCD_STATIC_STAMP = libpthreadGC$(DLL_VERD).stamp
+
+PTHREAD_DEF = pthread.def
+
+help:
+ @ echo "Run one of the following command lines:"
+ @ echo "make clean GC (to build the GNU C dll with C cleanup code)"
+ @ echo "make clean GCE (to build the GNU C dll with C++ exception handling)"
+ @ echo "make clean GC-inlined (to build the GNU C inlined dll with C cleanup code)"
+ @ echo "make clean GCE-inlined (to build the GNU C inlined dll with C++ exception handling)"
+ @ echo "make clean GC-static (to build the GNU C inlined static lib with C cleanup code)"
+ @ echo "make clean GC-debug (to build the GNU C debug dll with C cleanup code)"
+ @ echo "make clean GCE-debug (to build the GNU C debug dll with C++ exception handling)"
+ @ echo "make clean GC-inlined-debug (to build the GNU C inlined debug dll with C cleanup code)"
+ @ echo "make clean GCE-inlined-debug (to build the GNU C inlined debug dll with C++ exception handling)"
+ @ echo "make clean GC-static-debug (to build the GNU C inlined static debug lib with C cleanup code)"
+
+all:
+ @ $(MAKE) clean GCE
+ @ $(MAKE) clean GC
+
+GC:
+ $(MAKE) CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_OBJS)" $(GC_DLL)
+
+GC-debug:
+ $(MAKE) CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_OBJS)" DLL_VER=$(DLL_VERD) OPT="-D__CLEANUP_C -g -O0" $(GCD_DLL)
+
+GCE:
+ $(MAKE) CC=$(CXX) CLEANUP=-D__CLEANUP_CXX XC_FLAGS="$(GCE_CFLAGS)" OBJ="$(DLL_OBJS)" $(GCE_DLL)
+
+GCE-debug:
+ $(MAKE) CC=$(CXX) CLEANUP=-D__CLEANUP_CXX XC_FLAGS="$(GCE_CFLAGS)" OBJ="$(DLL_OBJS)" DLL_VER=$(DLL_VERD) OPT="-D__CLEANUP_CXX -g -O0" $(GCED_DLL)
+
+GC-inlined:
+ $(MAKE) XOPT="-DPTW32_BUILD_INLINED" CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" $(GC_INLINED_STAMP)
+
+GC-inlined-debug:
+ $(MAKE) XOPT="-DPTW32_BUILD_INLINED" CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" DLL_VER=$(DLL_VERD) OPT="-D__CLEANUP_C -g -O0" $(GCD_INLINED_STAMP)
+
+GCE-inlined:
+ $(MAKE) CC=$(CXX) XOPT="-DPTW32_BUILD_INLINED" CLEANUP=-D__CLEANUP_CXX XC_FLAGS="$(GCE_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" $(GCE_INLINED_STAMP)
+
+GCE-inlined-debug:
+ $(MAKE) CC=$(CXX) XOPT="-DPTW32_BUILD_INLINED" CLEANUP=-D__CLEANUP_CXX XC_FLAGS="$(GCE_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" DLL_VER=$(DLL_VERD) OPT="-D__CLEANUP_CXX -g -O0" $(GCED_INLINED_STAMP)
+
+GC-static:
+ $(MAKE) XOPT="-DPTW32_BUILD_INLINED -DPTW32_STATIC_LIB" CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" $(GC_STATIC_STAMP)
+
+GC-static-debug:
+ $(MAKE) XOPT="-DPTW32_BUILD_INLINED -DPTW32_STATIC_LIB" CLEANUP=-D__CLEANUP_C XC_FLAGS="$(GC_CFLAGS)" OBJ="$(DLL_INLINED_OBJS)" DLL_VER=$(DLL_VERD) OPT="-D__CLEANUP_C -g -O0" $(GCD_STATIC_STAMP)
+
+tests:
+ @ cd tests
+ @ $(MAKE) auto
+
+%.pre: %.c
+ $(CC) -E -o $@ $(CFLAGS) $^
+
+%.s: %.c
+ $(CC) -c $(CFLAGS) -DPTW32_BUILD_INLINED -Wa,-ahl $^ > $@
+
+%.o: %.rc
+ $(RC) $(RCFLAGS) $(CLEANUP) -o $@ -i $<
+
+.SUFFIXES: .dll .rc .c .o
+
+.c.o:; $(CC) -c -o $@ $(CFLAGS) $(XC_FLAGS) $<
+
+
+$(GC_DLL) $(GCD_DLL): $(DLL_OBJS)
+ $(CC) $(OPT) -shared -o $(GC_DLL) $(DLL_OBJS) $(LFLAGS)
+ $(DLLTOOL) -z pthread.def $(DLL_OBJS)
+ $(DLLTOOL) -k --dllname $@ --output-lib $(GC_LIB) --def $(PTHREAD_DEF)
+
+$(GCE_DLL): $(DLL_OBJS)
+ $(CC) $(OPT) -mthreads -shared -o $(GCE_DLL) $(DLL_OBJS) $(LFLAGS)
+ $(DLLTOOL) -z pthread.def $(DLL_OBJS)
+ $(DLLTOOL) -k --dllname $@ --output-lib $(GCE_LIB) --def $(PTHREAD_DEF)
+
+$(GC_INLINED_STAMP) $(GCD_INLINED_STAMP): $(DLL_INLINED_OBJS)
+ $(CC) $(OPT) $(XOPT) -shared -o $(GC_DLL) $(DLL_INLINED_OBJS) $(LFLAGS)
+ $(DLLTOOL) -z pthread.def $(DLL_INLINED_OBJS)
+ $(DLLTOOL) -k --dllname $(GC_DLL) --output-lib $(GC_LIB) --def $(PTHREAD_DEF)
+ echo touched > $(GC_INLINED_STAMP)
+
+$(GCE_INLINED_STAMP) $(GCED_INLINED_STAMP): $(DLL_INLINED_OBJS)
+ $(CC) $(OPT) $(XOPT) -mthreads -shared -o $(GCE_DLL) $(DLL_INLINED_OBJS) $(LFLAGS)
+ $(DLLTOOL) -z pthread.def $(DLL_INLINED_OBJS)
+ $(DLLTOOL) -k --dllname $(GCE_DLL) --output-lib $(GCE_LIB) --def $(PTHREAD_DEF)
+ echo touched > $(GCE_INLINED_STAMP)
+
+$(GC_STATIC_STAMP) $(GCD_STATIC_STAMP): $(DLL_INLINED_OBJS)
+ $(RM) $(GC_LIB)
+ $(AR) -rv $(GC_LIB) $(DLL_INLINED_OBJS)
+ $(RANLIB) $(GC_LIB)
+ echo touched > $(GC_STATIC_STAMP)
+
+clean:
+ -$(RM) *~
+ -$(RM) *.i
+ -$(RM) *.s
+ -$(RM) *.o
+ -$(RM) *.obj
+ -$(RM) *.exe
+ -$(RM) $(PTHREAD_DEF)
+
+realclean: clean
+ -$(RM) $(GC_LIB)
+ -$(RM) $(GCE_LIB)
+ -$(RM) $(GC_DLL)
+ -$(RM) $(GCE_DLL)
+ -$(RM) $(GC_INLINED_STAMP)
+ -$(RM) $(GCE_INLINED_STAMP)
+ -$(RM) $(GC_STATIC_STAMP)
+ -$(RM) $(GCD_LIB)
+ -$(RM) $(GCED_LIB)
+ -$(RM) $(GCD_DLL)
+ -$(RM) $(GCED_DLL)
+ -$(RM) $(GCD_INLINED_STAMP)
+ -$(RM) $(GCED_INLINED_STAMP)
+ -$(RM) $(GCD_STATIC_STAMP)
+
+attr.o: attr.c $(ATTR_SRCS) $(INCL)
+barrier.o: barrier.c $(BARRIER_SRCS) $(INCL)
+cancel.o: cancel.c $(CANCEL_SRCS) $(INCL)
+condvar.o: condvar.c $(CONDVAR_SRCS) $(INCL)
+exit.o: exit.c $(EXIT_SRCS) $(INCL)
+misc.o: misc.c $(MISC_SRCS) $(INCL)
+mutex.o: mutex.c $(MUTEX_SRCS) $(INCL)
+nonportable.o: nonportable.c $(NONPORTABLE_SRCS) $(INCL)
+private.o: private.c $(PRIVATE_SRCS) $(INCL)
+rwlock.o: rwlock.c $(RWLOCK_SRCS) $(INCL)
+sched.o: sched.c $(SCHED_SRCS) $(INCL)
+semaphore.o: semaphore.c $(SEMAPHORE_SRCS) $(INCL)
+spin.o: spin.c $(SPIN_SRCS) $(INCL)
+sync.o: sync.c $(SYNC_SRCS) $(INCL)
+tsd.o: tsd.c $(TSD_SRCS) $(INCL)
+version.o: version.rc $(INCL)
diff --git a/3rdparty/pthreads-win32/MAINTAINERS b/3rdparty/pthreads-win32/MAINTAINERS
new file mode 100644
index 0000000..d253c1f
--- /dev/null
+++ b/3rdparty/pthreads-win32/MAINTAINERS
@@ -0,0 +1,4 @@
+CVS Repository maintainers
+
+Ross Johnson rpj at ise.canberra.edu.au
+Ben Elliston bje at cygnus.com
diff --git a/3rdparty/pthreads-win32/Makefile b/3rdparty/pthreads-win32/Makefile
new file mode 100644
index 0000000..472969c
--- /dev/null
+++ b/3rdparty/pthreads-win32/Makefile
@@ -0,0 +1,514 @@
+# This makefile is compatible with MS nmake and can be used as a
+# replacement for buildlib.bat. I've changed the target from an ordinary dll
+# (/LD) to a debugging dll (/LDd).
+#
+# The variables $DLLDEST and $LIBDEST hold the destination directories for the
+# dll and the lib, respectively. Probably all that needs to change is $DEVROOT.
+
+
+# DLL_VER:
+# See pthread.h and README - This number is computed as 'current - age'
+DLL_VER = 2
+DLL_VERD= $(DLL_VER)d
+
+DEVROOT = C:\pthreads
+
+DLLDEST = $(DEVROOT)\dll
+LIBDEST = $(DEVROOT)\lib
+HDRDEST = $(DEVROOT)\include
+
+DLLS = pthreadVCE$(DLL_VER).dll pthreadVSE$(DLL_VER).dll pthreadVC$(DLL_VER).dll \
+ pthreadVCE$(DLL_VERD).dll pthreadVSE$(DLL_VERD).dll pthreadVC$(DLL_VERD).dll
+INLINED_STAMPS = pthreadVCE$(DLL_VER).stamp pthreadVSE$(DLL_VER).stamp pthreadVC$(DLL_VER).stamp \
+ pthreadVCE$(DLL_VERD).stamp pthreadVSE$(DLL_VERD).stamp pthreadVC$(DLL_VERD).stamp
+STATIC_STAMPS = pthreadVCE$(DLL_VER).static pthreadVSE$(DLL_VER).static pthreadVC$(DLL_VER).static \
+ pthreadVCE$(DLL_VERD).static pthreadVSE$(DLL_VERD).static pthreadVC$(DLL_VERD).static
+
+CC = cl
+CPPFLAGS = /I. /DHAVE_PTW32_CONFIG_H
+XCFLAGS = /W3 /MD /nologo
+CFLAGS = /O2 /Ob2 $(XCFLAGS)
+CFLAGSD = /Z7 $(XCFLAGS)
+
+# Uncomment this if config.h defines RETAIN_WSALASTERROR
+#XLIBS = wsock32.lib
+
+# Default cleanup style
+CLEANUP = __CLEANUP_C
+
+# C++ Exceptions
+VCEFLAGS = /EHsc /TP $(CPPFLAGS) $(CFLAGS)
+VCEFLAGSD = /EHsc /TP $(CPPFLAGS) $(CFLAGSD)
+#Structured Exceptions
+VSEFLAGS = $(CPPFLAGS) $(CFLAGS)
+VSEFLAGSD = $(CPPFLAGS) $(CFLAGSD)
+#C cleanup code
+VCFLAGS = $(CPPFLAGS) $(CFLAGS)
+VCFLAGSD = $(CPPFLAGS) $(CFLAGSD)
+
+DLL_INLINED_OBJS = \
+ pthread.obj \
+ version.res
+
+# Aggregate modules for inlinability
+DLL_OBJS = \
+ attr.obj \
+ barrier.obj \
+ cancel.obj \
+ cleanup.obj \
+ condvar.obj \
+ create.obj \
+ dll.obj \
+ autostatic.obj \
+ errno.obj \
+ exit.obj \
+ fork.obj \
+ global.obj \
+ misc.obj \
+ mutex.obj \
+ nonportable.obj \
+ private.obj \
+ rwlock.obj \
+ sched.obj \
+ semaphore.obj \
+ signal.obj \
+ spin.obj \
+ sync.obj \
+ tsd.obj \
+ version.res
+
+# Separate modules for minimising the size of statically linked images
+SMALL_STATIC_OBJS = \
+ pthread_attr_init.obj \
+ pthread_attr_destroy.obj \
+ pthread_attr_getdetachstate.obj \
+ pthread_attr_setdetachstate.obj \
+ pthread_attr_getstackaddr.obj \
+ pthread_attr_setstackaddr.obj \
+ pthread_attr_getstacksize.obj \
+ pthread_attr_setstacksize.obj \
+ pthread_attr_getscope.obj \
+ pthread_attr_setscope.obj \
+ pthread_attr_setschedpolicy.obj \
+ pthread_attr_getschedpolicy.obj \
+ pthread_attr_setschedparam.obj \
+ pthread_attr_getschedparam.obj \
+ pthread_attr_setinheritsched.obj \
+ pthread_attr_getinheritsched.obj \
+ pthread_barrier_init.obj \
+ pthread_barrier_destroy.obj \
+ pthread_barrier_wait.obj \
+ pthread_barrierattr_init.obj \
+ pthread_barrierattr_destroy.obj \
+ pthread_barrierattr_setpshared.obj \
+ pthread_barrierattr_getpshared.obj \
+ pthread_setcancelstate.obj \
+ pthread_setcanceltype.obj \
+ pthread_testcancel.obj \
+ pthread_cancel.obj \
+ cleanup.obj \
+ pthread_condattr_destroy.obj \
+ pthread_condattr_getpshared.obj \
+ pthread_condattr_init.obj \
+ pthread_condattr_setpshared.obj \
+ pthread_cond_destroy.obj \
+ pthread_cond_init.obj \
+ pthread_cond_signal.obj \
+ pthread_cond_wait.obj \
+ create.obj \
+ dll.obj \
+ autostatic.obj \
+ errno.obj \
+ pthread_exit.obj \
+ fork.obj \
+ global.obj \
+ pthread_mutex_init.obj \
+ pthread_mutex_destroy.obj \
+ pthread_mutexattr_init.obj \
+ pthread_mutexattr_destroy.obj \
+ pthread_mutexattr_getpshared.obj \
+ pthread_mutexattr_setpshared.obj \
+ pthread_mutexattr_settype.obj \
+ pthread_mutexattr_gettype.obj \
+ pthread_mutexattr_setrobust.obj \
+ pthread_mutexattr_getrobust.obj \
+ pthread_mutex_lock.obj \
+ pthread_mutex_timedlock.obj \
+ pthread_mutex_unlock.obj \
+ pthread_mutex_trylock.obj \
+ pthread_mutex_consistent.obj \
+ pthread_mutexattr_setkind_np.obj \
+ pthread_mutexattr_getkind_np.obj \
+ pthread_getw32threadhandle_np.obj \
+ pthread_getunique_np.obj \
+ pthread_delay_np.obj \
+ pthread_num_processors_np.obj \
+ pthread_win32_attach_detach_np.obj \
+ pthread_equal.obj \
+ pthread_getconcurrency.obj \
+ pthread_once.obj \
+ pthread_self.obj \
+ pthread_setconcurrency.obj \
+ pthread_rwlock_init.obj \
+ pthread_rwlock_destroy.obj \
+ pthread_rwlockattr_init.obj \
+ pthread_rwlockattr_destroy.obj \
+ pthread_rwlockattr_getpshared.obj \
+ pthread_rwlockattr_setpshared.obj \
+ pthread_rwlock_rdlock.obj \
+ pthread_rwlock_wrlock.obj \
+ pthread_rwlock_unlock.obj \
+ pthread_rwlock_tryrdlock.obj \
+ pthread_rwlock_trywrlock.obj \
+ pthread_setschedparam.obj \
+ pthread_getschedparam.obj \
+ pthread_timechange_handler_np.obj \
+ ptw32_is_attr.obj \
+ ptw32_processInitialize.obj \
+ ptw32_processTerminate.obj \
+ ptw32_threadStart.obj \
+ ptw32_threadDestroy.obj \
+ ptw32_tkAssocCreate.obj \
+ ptw32_tkAssocDestroy.obj \
+ ptw32_callUserDestroyRoutines.obj \
+ ptw32_timespec.obj \
+ ptw32_throw.obj \
+ ptw32_getprocessors.obj \
+ ptw32_calloc.obj \
+ ptw32_new.obj \
+ ptw32_reuse.obj \
+ ptw32_rwlock_check_need_init.obj \
+ ptw32_cond_check_need_init.obj \
+ ptw32_mutex_check_need_init.obj \
+ ptw32_semwait.obj \
+ ptw32_relmillisecs.obj \
+ ptw32_MCS_lock.obj \
+ sched_get_priority_max.obj \
+ sched_get_priority_min.obj \
+ sched_setscheduler.obj \
+ sched_getscheduler.obj \
+ sched_yield.obj \
+ sem_init.obj \
+ sem_destroy.obj \
+ sem_trywait.obj \
+ sem_timedwait.obj \
+ sem_wait.obj \
+ sem_post.obj \
+ sem_post_multiple.obj \
+ sem_getvalue.obj \
+ sem_open.obj \
+ sem_close.obj \
+ sem_unlink.obj \
+ signal.obj \
+ pthread_kill.obj \
+ ptw32_spinlock_check_need_init.obj \
+ pthread_spin_init.obj \
+ pthread_spin_destroy.obj \
+ pthread_spin_lock.obj \
+ pthread_spin_unlock.obj \
+ pthread_spin_trylock.obj \
+ pthread_detach.obj \
+ pthread_join.obj \
+ pthread_key_create.obj \
+ pthread_key_delete.obj \
+ pthread_setspecific.obj \
+ pthread_getspecific.obj \
+ w32_CancelableWait.obj \
+ version.res
+
+INCL = config.h implement.h semaphore.h pthread.h need_errno.h
+
+ATTR_SRCS = \
+ pthread_attr_init.c \
+ pthread_attr_destroy.c \
+ pthread_attr_getdetachstate.c \
+ pthread_attr_setdetachstate.c \
+ pthread_attr_getstackaddr.c \
+ pthread_attr_setstackaddr.c \
+ pthread_attr_getstacksize.c \
+ pthread_attr_setstacksize.c \
+ pthread_attr_getscope.c \
+ pthread_attr_setscope.c
+
+BARRIER_SRCS = \
+ pthread_barrier_init.c \
+ pthread_barrier_destroy.c \
+ pthread_barrier_wait.c \
+ pthread_barrierattr_init.c \
+ pthread_barrierattr_destroy.c \
+ pthread_barrierattr_setpshared.c \
+ pthread_barrierattr_getpshared.c
+
+CANCEL_SRCS = \
+ pthread_setcancelstate.c \
+ pthread_setcanceltype.c \
+ pthread_testcancel.c \
+ pthread_cancel.c
+
+CONDVAR_SRCS = \
+ ptw32_cond_check_need_init.c \
+ pthread_condattr_destroy.c \
+ pthread_condattr_getpshared.c \
+ pthread_condattr_init.c \
+ pthread_condattr_setpshared.c \
+ pthread_cond_destroy.c \
+ pthread_cond_init.c \
+ pthread_cond_signal.c \
+ pthread_cond_wait.c
+
+EXIT_SRCS = \
+ pthread_exit.c
+
+MISC_SRCS = \
+ pthread_equal.c \
+ pthread_getconcurrency.c \
+ pthread_kill.c \
+ pthread_once.c \
+ pthread_self.c \
+ pthread_setconcurrency.c \
+ ptw32_calloc.c \
+ ptw32_MCS_lock.c \
+ ptw32_new.c \
+ ptw32_reuse.c \
+ ptw32_relmillisecs.c \
+ w32_CancelableWait.c
+
+MUTEX_SRCS = \
+ ptw32_mutex_check_need_init.c \
+ pthread_mutex_init.c \
+ pthread_mutex_destroy.c \
+ pthread_mutexattr_init.c \
+ pthread_mutexattr_destroy.c \
+ pthread_mutexattr_getpshared.c \
+ pthread_mutexattr_setpshared.c \
+ pthread_mutexattr_settype.c \
+ pthread_mutexattr_gettype.c \
+ pthread_mutexattr_setrobust.c \
+ pthread_mutexattr_getrobust.c \
+ pthread_mutex_lock.c \
+ pthread_mutex_timedlock.c \
+ pthread_mutex_unlock.c \
+ pthread_mutex_trylock.c \
+ pthread_mutex_consistent.c
+
+NONPORTABLE_SRCS = \
+ pthread_mutexattr_setkind_np.c \
+ pthread_mutexattr_getkind_np.c \
+ pthread_getw32threadhandle_np.c \
+ pthread_getunique_np.c \
+ pthread_delay_np.c \
+ pthread_num_processors_np.c \
+ pthread_win32_attach_detach_np.c \
+ pthread_timechange_handler_np.c
+
+PRIVATE_SRCS = \
+ ptw32_is_attr.c \
+ ptw32_processInitialize.c \
+ ptw32_processTerminate.c \
+ ptw32_threadStart.c \
+ ptw32_threadDestroy.c \
+ ptw32_tkAssocCreate.c \
+ ptw32_tkAssocDestroy.c \
+ ptw32_callUserDestroyRoutines.c \
+ ptw32_semwait.c \
+ ptw32_timespec.c \
+ ptw32_throw.c \
+ ptw32_getprocessors.c
+
+RWLOCK_SRCS = \
+ ptw32_rwlock_check_need_init.c \
+ ptw32_rwlock_cancelwrwait.c \
+ pthread_rwlock_init.c \
+ pthread_rwlock_destroy.c \
+ pthread_rwlockattr_init.c \
+ pthread_rwlockattr_destroy.c \
+ pthread_rwlockattr_getpshared.c \
+ pthread_rwlockattr_setpshared.c \
+ pthread_rwlock_rdlock.c \
+ pthread_rwlock_timedrdlock.c \
+ pthread_rwlock_wrlock.c \
+ pthread_rwlock_timedwrlock.c \
+ pthread_rwlock_unlock.c \
+ pthread_rwlock_tryrdlock.c \
+ pthread_rwlock_trywrlock.c
+
+SCHED_SRCS = \
+ pthread_attr_setschedpolicy.c \
+ pthread_attr_getschedpolicy.c \
+ pthread_attr_setschedparam.c \
+ pthread_attr_getschedparam.c \
+ pthread_attr_setinheritsched.c \
+ pthread_attr_getinheritsched.c \
+ pthread_setschedparam.c \
+ pthread_getschedparam.c \
+ sched_get_priority_max.c \
+ sched_get_priority_min.c \
+ sched_setscheduler.c \
+ sched_getscheduler.c \
+ sched_yield.c
+
+SEMAPHORE_SRCS = \
+ sem_init.c \
+ sem_destroy.c \
+ sem_trywait.c \
+ sem_timedwait.c \
+ sem_wait.c \
+ sem_post.c \
+ sem_post_multiple.c \
+ sem_getvalue.c \
+ sem_open.c \
+ sem_close.c \
+ sem_unlink.c
+
+SPIN_SRCS = \
+ ptw32_spinlock_check_need_init.c \
+ pthread_spin_init.c \
+ pthread_spin_destroy.c \
+ pthread_spin_lock.c \
+ pthread_spin_unlock.c \
+ pthread_spin_trylock.c
+
+SYNC_SRCS = \
+ pthread_detach.c \
+ pthread_join.c
+
+TSD_SRCS = \
+ pthread_key_create.c \
+ pthread_key_delete.c \
+ pthread_setspecific.c \
+ pthread_getspecific.c
+
+
+help:
+ @ echo Run one of the following command lines:
+ @ echo nmake clean VCE (to build the MSVC dll with C++ exception handling)
+ @ echo nmake clean VSE (to build the MSVC dll with structured exception handling)
+ @ echo nmake clean VC (to build the MSVC dll with C cleanup code)
+ @ echo nmake clean VCE-inlined (to build the MSVC inlined dll with C++ exception handling)
+ @ echo nmake clean VSE-inlined (to build the MSVC inlined dll with structured exception handling)
+ @ echo nmake clean VC-inlined (to build the MSVC inlined dll with C cleanup code)
+ @ echo nmake clean VC-static (to build the MSVC static lib with C cleanup code)
+ @ echo nmake clean VCE-debug (to build the debug MSVC dll with C++ exception handling)
+ @ echo nmake clean VSE-debug (to build the debug MSVC dll with structured exception handling)
+ @ echo nmake clean VC-debug (to build the debug MSVC dll with C cleanup code)
+ @ echo nmake clean VCE-inlined-debug (to build the debug MSVC inlined dll with C++ exception handling)
+ @ echo nmake clean VSE-inlined-debug (to build the debug MSVC inlined dll with structured exception handling)
+ @ echo nmake clean VC-inlined-debug (to build the debug MSVC inlined dll with C cleanup code)
+ @ echo nmake clean VC-static-debug (to build the debug MSVC static lib with C cleanup code)
+
+all:
+ @ $(MAKE) /E clean VCE-inlined
+ @ $(MAKE) /E clean VSE-inlined
+ @ $(MAKE) /E clean VC-inlined
+ @ $(MAKE) /E clean VCE-inlined-debug
+ @ $(MAKE) /E clean VSE-inlined-debug
+ @ $(MAKE) /E clean VC-inlined-debug
+
+VCE:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCEFLAGS)" CLEANUP=__CLEANUP_CXX pthreadVCE$(DLL_VER).dll
+
+VCE-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCEFLAGSD)" CLEANUP=__CLEANUP_CXX pthreadVCE$(DLL_VERD).dll
+
+VSE:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VSEFLAGS)" CLEANUP=__CLEANUP_SEH pthreadVSE$(DLL_VER).dll
+
+VSE-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VSEFLAGSD)" CLEANUP=__CLEANUP_SEH pthreadVSE$(DLL_VERD).dll
+
+VC:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGS)" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VER).dll
+
+VC-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGSD)" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VERD).dll
+
+#
+# The so-called inlined DLL is just a single translation unit with
+# inlining optimisation turned on.
+#
+VCE-inlined:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCEFLAGS) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_CXX pthreadVCE$(DLL_VER).stamp
+
+VCE-inlined-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCEFLAGSD) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_CXX pthreadVCE$(DLL_VERD).stamp
+
+VSE-inlined:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VSEFLAGS) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_SEH pthreadVSE$(DLL_VER).stamp
+
+VSE-inlined-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VSEFLAGSD) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_SEH pthreadVSE$(DLL_VERD).stamp
+
+VC-inlined:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGS) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VER).stamp
+
+VC-inlined-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGSD) /DPTW32_BUILD_INLINED" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VERD).stamp
+
+VC-static:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGS) /DPTW32_BUILD_INLINED /DPTW32_STATIC_LIB" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VER).static
+
+VC-static-debug:
+ @ $(MAKE) /E /nologo EHFLAGS="$(VCFLAGSD) /DPTW32_BUILD_INLINED /DPTW32_STATIC_LIB" CLEANUP=__CLEANUP_C pthreadVC$(DLL_VERD).static
+
+realclean: clean
+ if exist pthread*.dll del pthread*.dll
+ if exist pthread*.lib del pthread*.lib
+ if exist *.manifest del *.manifest
+ if exist *.stamp del *.stamp
+
+clean:
+ if exist *.obj del *.obj
+ if exist *.def del *.def
+ if exist *.ilk del *.ilk
+ if exist *.pdb del *.pdb
+ if exist *.exp del *.exp
+ if exist *.map del *.map
+ if exist *.o del *.o
+ if exist *.i del *.i
+ if exist *.res del *.res
+
+
+install:
+ copy pthread*.dll $(DLLDEST)
+ copy pthread*.lib $(LIBDEST)
+ copy pthread.h $(HDRDEST)
+ copy sched.h $(HDRDEST)
+ copy semaphore.h $(HDRDEST)
+
+$(DLLS): $(DLL_OBJS)
+ $(CC) /LDd /Zi /nologo $(DLL_OBJS) /link /implib:$*.lib $(XLIBS) /out:$@
+
+$(INLINED_STAMPS): $(DLL_INLINED_OBJS)
+ $(CC) /LDd /Zi /nologo $(DLL_INLINED_OBJS) /link /implib:$*.lib $(XLIBS) /out:$*.dll
+
+$(STATIC_STAMPS): $(DLL_INLINED_OBJS)
+ if exist $*.lib del $*.lib
+ lib $(DLL_INLINED_OBJS) /out:$*.lib
+
+.c.obj:
+ $(CC) $(EHFLAGS) /D$(CLEANUP) -c $<
+
+# TARGET_CPU is an environment variable set by Visual Studio Command Prompt
+# as provided by the SDK
+.rc.res:
+ rc /dPTW32_ARCH$(TARGET_CPU) /dPTW32_RC_MSC /d$(CLEANUP) $<
+
+.c.i:
+ $(CC) /P /O2 /Ob1 $(VCFLAGS) $<
+
+attr.obj: attr.c $(ATTR_SRCS) $(INCL)
+barrier.obj: barrier.c $(BARRIER_SRCS) $(INCL)
+cancel.obj: cancel.c $(CANCEL_SRCS) $(INCL)
+condvar.obj: condvar.c $(CONDVAR_SRCS) $(INCL)
+exit.obj: exit.c $(EXIT_SRCS) $(INCL)
+misc.obj: misc.c $(MISC_SRCS) $(INCL)
+mutex.obj: mutex.c $(MUTEX_SRCS) $(INCL)
+nonportable.obj: nonportable.c $(NONPORTABLE_SRCS) $(INCL)
+private.obj: private.c $(PRIVATE_SRCS) $(INCL)
+rwlock.obj: rwlock.c $(RWLOCK_SRCS) $(INCL)
+sched.obj: sched.c $(SCHED_SRCS) $(INCL)
+semaphore.obj: semaphore.c $(SEMAPHORE_SRCS) $(INCL)
+spin.obj: spin.c $(SPIN_SRCS) $(INCL)
+sync.obj: sync.c $(SYNC_SRCS) $(INCL)
+tsd.obj: tsd.c $(TSD_SRCS) $(INCL)
+version.res: version.rc $(INCL)
diff --git a/3rdparty/pthreads-win32/NEWS b/3rdparty/pthreads-win32/NEWS
new file mode 100644
index 0000000..d1b7896
--- /dev/null
+++ b/3rdparty/pthreads-win32/NEWS
@@ -0,0 +1,1241 @@
+RELEASE 2.9.0
+-------------
+(2012-05-25)
+
+General
+-------
+New bug fixes in this release since 2.8.0 have NOT been applied to the
+1.x.x series.
+
+Some changes post 2011-02-26 in CVS may not be compatible with pre
+Windows 2000 systems.
+
+Use of other than the "C" version of the library is now discouraged.
+That is, the "C++" version fails some tests and does not provide any
+additional functionality.
+
+Testing and verification
+------------------------
+This version has been tested on SMP architecture (Intel x64 Hex Core)
+by completing the included test suite, stress and bench tests.
+
+New Features
+------------
+DLL properties now properly includes the target architecture, i.e.
+right-click on the file pthreadVC2.dll in explorer and choose the Detail
+tab will show the compiler and architecture in the description field, e.g.
+"MS C x64" or "MS C x86".
+- Ross Johnson
+
+(MSC and GNU builds) The statically linked library now automatically
+initialises and cleans up on program start/exit, i.e. statically linked
+applications need not call the routines pthread_win32_process_attach_np()
+and pthread_win32_process_detach_np() explicitly. The per-thread routine
+pthread_win32_thread_detach_np() is also called at program exit to cleanup
+POSIX resources acquired by the primary Windows native thread, if I (RJ)
+understand the process correctly. Other Windows native threads that call
+POSIX API routines may need to call the thread detach routine on thread
+exit if the application depends on reclaimed POSIX resources or running
+POSIX TSD (TLS) destructors.
+See README.NONPORTABLE for descriptions of these routines.
+- Ramiro Polla
+
+Robust mutexes are implemented within the PROCESS_PRIVATE scope. NOTE that
+pthread_mutex_* functions may return different error codes for robust
+mutexes than they otherwise do in normal usage, e.g. pthread_mutex_unlock
+is required to check ownership for all mutex types when the mutex is
+robust, whereas this does not occur for the "normal" non-robust mutex type.
+- Ross Johnson
+
+pthread_getunique_np is implemented for source level compatibility
+with some other implementations. This routine returns a 64 bit
+sequence number that is uniquely associated with a thread. It can be
+used by applications to order or hash POSIX thread handles.
+- Ross Johnson
+
+Bug fixes
+---------
+Many more changes for 64 bit systems.
+- Kai Tietz
+
+Various modifications and fixes to build and test for WinCE.
+- Marcel Ruff, Sinan Kaya
+
+Fix pthread_cond_destroy() - should not be a cancellation point. Other
+minor build problems fixed.
+- Romano Paolo Tenca
+
+Remove potential deadlock condition from pthread_cond_destroy().
+- Eric Berge
+
+Various modifications to build and test for Win64.
+- Kip Streithorst
+
+Various fixes to the QueueUserAPCEx async cancellation helper DLL
+(this is a separate download) and pthreads code cleanups.
+- Sebastian Gottschalk
+
+Removed potential NULL pointer reference.
+- Robert Kindred
+
+Removed the requirement that applications restrict the number of threads
+calling pthread_barrier_wait to just the barrier count. Also reduced the
+contention between barrier_wait and barrier_destroy. This change will have
+slowed barriers down slightly but halves the number of semaphores consumed
+per barrier to one.
+- Ross Johnson
+
+Fixed a handle leak in sched_[gs]etscheduler.
+- Mark Pizzolato
+
+Removed all of the POSIX re-entrant function compatibility macros from pthread.h.
+Some were simply not semanticly correct.
+- Igor Lubashev
+
+Threads no longer attempt to pass uncaught exceptions out of thread scope (C++
+and SEH builds only). Uncaught exceptions now cause the thread to exit with
+the return code PTHREAD_CANCELED.
+- Ross Johnson
+
+Lots of casting fixes particularly for x64, Interlocked fixes and reworking
+for x64.
+- Daniel Richard G., John Kamp
+
+Other changes
+-------------
+Dependence on the winsock library is now discretionary via
+#define RETAIN_WSALASTERROR in config.h. It is undefined by default unless
+WINCE is defined (because RJ is unsure of the dependency there).
+- Ramiro Polla
+
+Several static POSIX mutexes used for internal management were replaced by
+MCS queue-based locks to reduce resource consumption, in particular use of Win32
+objects.
+- Ross Johnson
+
+For security, the QuserEx.dll if used must now be installed in the Windows System
+folder.
+- Ross Johnson
+
+New tests
+---------
+robust[1-5].c - Robust mutexes
+sequence1.c - per-thread unique sequence numbers
+
+Modified tests and benchtests
+-----------------------------
+All mutex*.c tests wherever appropriate have been modified to also test
+robust mutexes under the same conditions.
+Added robust mutex benchtests to benchtest*.c wherever appropriate.
+
+
+RELEASE 2.8.0
+-------------
+(2006-12-22)
+
+General
+-------
+New bug fixes in this release since 2.7.0 have not been applied to the
+version 1.x.x series. It is probably time to drop version 1.
+
+Testing and verification
+------------------------
+This release has not yet been tested on SMP architechtures. All tests pass
+on a uni-processor system.
+
+Bug fixes
+---------
+Sem_destroy could return EBUSY even though no threads were waiting on the
+semaphore. Other races around invalidating semaphore structs (internally)
+have been removed as well.
+
+New tests
+---------
+semaphore5.c - tests the bug fix referred to above.
+
+
+RELEASE 2.7.0
+-------------
+(2005-06-04)
+
+General
+-------
+All new features in this release have been back-ported in release 1.11.0,
+including the incorporation of MCS locks in pthread_once, however, versions
+1 and 2 remain incompatible even though they are now identical in
+performance and functionality.
+
+Testing and verification
+------------------------
+This release has been tested (passed the test suite) on both uni-processor
+and multi-processor systems.
+- Tim Theisen
+
+Bug fixes
+---------
+Pthread_once has been re-implemented to remove priority boosting and other
+complexity to improve robustness. Races for Win32 handles that are not
+recycle-unique have been removed. The general form of pthread_once is now
+the same as that suggested earlier by Alexander Terekhov, but instead of the
+'named mutex', a queue-based lock has been implemented which has the required
+properties of dynamic self initialisation and destruction. This lock is also
+efficient. The ABI is unaffected in as much as the size of pthread_once_t has
+not changed and PTHREAD_ONCE_INIT has not changed, however, applications that
+peek inside pthread_once_t, which is supposed to be opaque, will break.
+- Vladimir Kliatchko
+
+New features
+------------
+* Support for Mingw cross development tools added to GNUmakefile.
+Mingw cross tools allow building the libraries on Linux.
+- Mikael Magnusson
+
+
+RELEASE 2.6.0
+-------------
+(2005-05-19)
+
+General
+-------
+All of the bug fixes and new features in this release have been
+back-ported in release 1.10.0.
+
+Testing and verification
+------------------------
+This release has been tested (passed the test suite) on both uni-processor
+and multi-processor systems. Thanks to Tim Theisen at TomoTherapy for
+exhaustively running the MP tests and for providing crutial observations
+and data when faults are detected.
+
+Bugs fixed
+----------
+
+* pthread_detach() now reclaims remaining thread resources if called after
+the target thread has terminated. Previously, this routine did nothing in
+this case.
+
+New tests
+---------
+
+* detach1.c - tests that pthread_detach properly invalidates the target
+thread, which indicates that the thread resources have been reclaimed.
+
+
+RELEASE 2.5.0
+-------------
+(2005-05-09)
+
+General
+-------
+
+The package now includes a reference documentation set consisting of
+HTML formatted Unix-style manual pages that have been edited for
+consistency with Pthreads-w32. The set can also be read online at:
+http://sources.redhat.com/pthreads-win32/manual/index.html
+
+Thanks again to Tim Theisen for running the test suite pre-release
+on an MP system.
+
+All of the bug fixes and new features in this release have been
+back-ported in release 1.9.0.
+
+Bugs fixed
+----------
+
+* Thread Specific Data (TSD) key management has been ammended to
+eliminate a source of (what was effectively) resource leakage (a HANDLE
+plus memory for each key destruct routine/thread association). This was
+not a true leak because these resources were eventually reclaimed when
+pthread_key_delete was run AND each thread referencing the key had exited.
+The problem was that these two conditions are often not met until very
+late, and often not until the process is about to exit.
+
+The ammended implementation avoids the need for the problematic HANDLE
+and reclaims the memory as soon as either the key is deleted OR the
+thread exits, whichever is first.
+
+Thanks to Richard Hughes at Aculab for identifying and locating the leak.
+
+* TSD key destructors are now processed up to PTHREAD_DESTRUCTOR_ITERATIONS
+times instead of just once. PTHREAD_DESTRUCTOR_ITERATIONS has been
+defined in pthread.h for some time but not used.
+
+* Fix a semaphore accounting race between sem_post/sem_post_multiple
+and sem_wait cancellation. This is the same issue as with
+sem_timedwait that was fixed in the last release.
+
+* sem_init, sem_post, and sem_post_multiple now check that the
+semaphore count never exceeds _POSIX_SEM_VALUE_MAX.
+
+* Although sigwait() is nothing more than a no-op, it should at least
+be a cancellation point to be consistent with the standard.
+
+New tests
+---------
+
+* stress1.c - attempts to expose problems in condition variable
+and semaphore timed wait logic. This test was inspired by Stephan
+Mueller's sample test code used to identify the sem_timedwait bug
+from the last release. It's not a part of the regular test suite
+because it can take awhile to run. To run it:
+nmake clean VC-stress
+
+* tsd2.c - tests that key destructors are re-run if the tsd key value is
+not NULL after the destructor routine has run. Also tests that
+pthread_setspecific() and pthread_getspecific() are callable from
+destructors.
+
+
+RELEASE 2.4.0
+-------------
+(2005-04-26)
+
+General
+-------
+
+There is now no plan to release a version 3.0.0 to fix problems in
+pthread_once(). Other possible implementations of pthread_once
+will still be investigated for a possible future release in an attempt
+to reduce the current implementation's complexity.
+
+All of the bug fixes and new features in this release have been
+back-ported for release 1.8.0.
+
+Bugs fixed
+----------
+
+* Fixed pthread_once race (failures on an MP system). Thanks to
+Tim Theisen for running exhaustive pre-release testing on his MP system
+using a range of compilers:
+ VC++ 6
+ VC++ 7.1
+ Intel C++ version 8.0
+All tests passed.
+Some minor speed improvements were also done.
+
+* Fix integer overrun error in pthread_mutex_timedlock() - missed when
+sem_timedwait() was fixed in release 2.2.0. This routine no longer returns
+ENOTSUP when NEED_SEM is defined - it is supported (NEED_SEM is only
+required for WinCE versions prior to 3.0).
+
+* Fix timeout bug in sem_timedwait().
+- Thanks to Stephan Mueller for reporting, providing diagnostic output
+and test code.
+
+* Fix several problems in the NEED_SEM conditionally included code.
+NEED_SEM included code is provided for systems that don't implement W32
+semaphores, such as WinCE prior to version 3.0. An alternate implementation
+of POSIX semaphores is built using W32 events for these systems when
+NEED_SEM is defined. This code has been completely rewritten in this
+release to reuse most of the default POSIX semaphore code, and particularly,
+to implement all of the sem_* routines supported by pthreads-win32. Tim
+Theisen also run the test suite over the NEED_SEM code on his MP system. All
+tests passed.
+
+* The library now builds without errors for the Borland Builder 5.5 compiler.
+
+New features
+------------
+
+* pthread_mutex_timedlock() and all sem_* routines provided by
+pthreads-win32 are now implemented for WinCE versions prior to 3.0. Those
+versions did not implement W32 semaphores. Define NEED_SEM in config.h when
+building the library for these systems.
+
+Known issues in this release
+----------------------------
+
+* pthread_once is too complicated - but it works as far as testing can
+determine..
+
+* The Borland version of the dll fails some of the tests with a memory read
+exception. The cause is not yet known but a compiler bug has not been ruled
+out.
+
+
+RELEASE 2.3.0
+-------------
+(2005-04-12)
+
+General
+-------
+
+Release 1.7.0 is a backport of features and bug fixes new in
+this release. See earlier notes under Release 2.0.0/General.
+
+Bugs fixed
+----------
+
+* Fixed pthread_once potential for post once_routine cancellation
+hanging due to starvation. See comments in pthread_once.c.
+Momentary priority boosting is used to ensure that, after a
+once_routine is cancelled, the thread that will run the
+once_routine is not starved by higher priority waiting threads at
+critical times. Priority boosting occurs only AFTER a once_routine
+cancellation, and is applied only to that once_control. The
+once_routine is run at the thread's normal base priority.
+
+New tests
+---------
+
+* once4.c: Aggressively tests pthread_once() under realtime
+conditions using threads with varying priorities. Windows'
+random priority boosting does not occur for threads with realtime
+priority levels.
+
+
+RELEASE 2.2.0
+-------------
+(2005-04-04)
+
+General
+-------
+
+* Added makefile targets to build static link versions of the library.
+Both MinGW and MSVC. Please note that this does not imply any change
+to the LGPL licensing, which still imposes psecific conditions on
+distributing software that has been statically linked with this library.
+
+* There is a known bug in pthread_once(). Cancellation of the init_routine
+exposes a potential starvation (i.e. deadlock) problem if a waiting thread
+has a higher priority than the initting thread. This problem will be fixed
+in version 3.0.0 of the library.
+
+Bugs fixed
+----------
+
+* Fix integer overrun error in sem_timedwait().
+Kevin Lussier
+
+* Fix preprocessor directives for static linking.
+Dimitar Panayotov
+
+
+RELEASE 2.1.0
+-------------
+(2005-03-16)
+
+Bugs fixed
+----------
+
+* Reverse change to pthread_setcancelstate() in 2.0.0.
+
+
+RELEASE 2.0.0
+-------------
+(2005-03-16)
+
+General
+-------
+
+This release represents an ABI change and the DLL version naming has
+incremented from 1 to 2, e.g. pthreadVC2.dll.
+
+Version 1.4.0 back-ports the new functionality included in this
+release. Please distribute DLLs built from that version with updates
+to applications built on pthreads-win32 version 1.x.x.
+
+The package naming has changed, replacing the snapshot date with
+the version number + descriptive information. E.g. this
+release is "pthreads-w32-2-0-0-release".
+
+Bugs fixed
+----------
+
+* pthread_setcancelstate() no longer checks for a pending
+async cancel event if the library is using alertable async
+cancel. See the README file (Prerequisites section) for info
+on adding alertable async cancelation.
+
+New features
+------------
+
+* pthread_once() now supports init_routine cancellability.
+
+New tests
+---------
+
+* Agressively test pthread_once() init_routine cancellability.
+
+
+SNAPSHOT 2005-03-08
+-------------------
+Version 1.3.0
+
+Bug reports (fixed)
+-------------------
+
+* Implicitly created threads leave Win32 handles behind after exiting.
+- Dmitrii Semii
+
+* pthread_once() starvation problem.
+- Gottlob Frege
+
+New tests
+---------
+
+* More intense testing of pthread_once().
+
+
+SNAPSHOT 2005-01-25
+-------------------
+Version 1.2.0
+
+Bug fixes
+---------
+
+* Attempted acquisition of a recursive mutex could cause waiting threads
+to not be woken when the mutex was released.
+- Ralf Kubis <RKubis at mc.com>
+
+* Various package omissions have been fixed.
+
+
+SNAPSHOT 2005-01-03
+-------------------
+Version 1.1.0
+
+Bug fixes
+---------
+
+* Unlocking recursive or errorcheck mutexes would sometimes
+unexpectedly return an EPERM error (bug introduced in
+snapshot-2004-11-03).
+- Konstantin Voronkov <beowinkle at yahoo.com>
+
+
+SNAPSHOT 2004-11-22
+-------------------
+Version 1.0.0
+
+This snapshot primarily fixes the condvar bug introduced in
+snapshot-2004-11-03. DLL versioning has also been included to allow
+applications to runtime check the Microsoft compatible DLL version
+information, and to extend the DLL naming system for ABI and major
+(non-backward compatible) API changes. See the README file for details.
+
+Bug fixes
+---------
+
+* Condition variables no longer deadlock (bug introduced in
+snapshot-2004-11-03).
+- Alexander Kotliarov and Nicolas at saintmac
+
+* DLL naming extended to avoid 'DLL hell' in the future, and to
+accommodate the ABI change introduced in snapshot-2004-11-03. Snapshot
+2004-11-03 will be removed from FTP sites.
+
+New features
+------------
+
+* A Microsoft-style version resource has been added to the DLL for
+applications that wish to check DLL compatibility at runtime.
+
+* Pthreads-win32 DLL naming has been extended to allow incompatible DLL
+versions to co-exist in the same filesystem. See the README file for details,
+but briefly: while the version information inside the DLL will change with
+each release from now on, the DLL version names will only change if the new
+DLL is not backward compatible with older applications.
+
+The versioning scheme has been borrowed from GNU Libtool, and the DLL
+naming scheme is from Cygwin. Provided the Libtool-style numbering rules are
+honoured, the Cygwin DLL naming scheme automatcally ensures that DLL name
+changes are minimal and that applications will not load an incompatible
+pthreads-win32 DLL.
+
+Those who use the pre-built DLLs will find that the DLL/LIB names have a new
+suffix (1) in this snapshot. E.g. pthreadVC1.dll etc.
+
+* The POSIX thread ID reuse uniqueness feature introduced in the last snapshot
+has been kept as default, but the behaviour can now be controlled when the DLL
+is built to effectively switch it off. This makes the library much more
+sensitive to applications that assume that POSIX thread IDs are unique, i.e.
+are not strictly compliant with POSIX. See the PTW32_THREAD_ID_REUSE_INCREMENT
+macro comments in config.h for details.
+
+Other changes
+-------------
+Certain POSIX macros have changed.
+
+These changes are intended to conform to the Single Unix Specification version 3,
+which states that, if set to 0 (zero) or not defined, then applications may use
+sysconf() to determine their values at runtime. Pthreads-win32 does not
+implement sysconf().
+
+The following macros are no longer undefined, but defined and set to -1
+(not implemented):
+
+ _POSIX_THREAD_ATTR_STACKADDR
+ _POSIX_THREAD_PRIO_INHERIT
+ _POSIX_THREAD_PRIO_PROTECT
+ _POSIX_THREAD_PROCESS_SHARED
+
+The following macros are defined and set to 200112L (implemented):
+
+ _POSIX_THREADS
+ _POSIX_THREAD_SAFE_FUNCTIONS
+ _POSIX_THREAD_ATTR_STACKSIZE
+ _POSIX_THREAD_PRIORITY_SCHEDULING
+ _POSIX_SEMAPHORES
+ _POSIX_READER_WRITER_LOCKS
+ _POSIX_SPIN_LOCKS
+ _POSIX_BARRIERS
+
+The following macros are defined and set to appropriate values:
+
+ _POSIX_THREAD_THREADS_MAX
+ _POSIX_SEM_VALUE_MAX
+ _POSIX_SEM_NSEMS_MAX
+ PTHREAD_DESTRUCTOR_ITERATIONS
+ PTHREAD_KEYS_MAX
+ PTHREAD_STACK_MIN
+ PTHREAD_THREADS_MAX
+
+
+SNAPSHOT 2004-11-03
+-------------------
+
+DLLs produced from this snapshot cannot be used with older applications without
+recompiling the application, due to a change to pthread_t to provide unique POSIX
+thread IDs.
+
+Although this snapshot passes the extended test suite, many of the changes are
+fairly major, and some applications may show different behaviour than previously,
+so adopt with care. Hopefully, any changed behaviour will be due to the library
+being better at it's job, not worse.
+
+Bug fixes
+---------
+
+* pthread_create() no longer accepts NULL as the thread reference arg.
+A segfault (memory access fault) will result, and no thread will be
+created.
+
+* pthread_barrier_wait() no longer acts as a cancelation point.
+
+* Fix potential race condition in pthread_once()
+- Tristan Savatier <tristan at mpegtv.com>
+
+* Changes to pthread_cond_destroy() exposed some coding weaknesses in several
+test suite mini-apps because pthread_cond_destroy() now returns EBUSY if the CV
+is still in use.
+
+New features
+------------
+
+* Added for compatibility:
+PTHREAD_RECURSIVE_MUTEX_INITIALIZER,
+PTHREAD_ERRORCHECK_MUTEX_INITIALIZER,
+PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP,
+PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP
+
+* Initial support for Digital Mars compiler
+- Anuj Goyal <anuj.goyal at gmail.com>
+
+* Faster Mutexes. These have been been rewritten following a model provided by
+Alexander Terekhov that reduces kernel space checks, and eliminates some additional
+critical sections used to manage a race between timedlock expiration and unlock.
+Please be aware that the new mutexes do not enforce strict absolute FIFO scheduling
+of mutexes, however any out-of-order lock acquisition should be very rare.
+
+* Faster semaphores. Following a similar model to mutexes above, these have been
+rewritten to use preliminary users space checks.
+
+* sem_getvalue() now returns the number of waiters.
+
+* The POSIX thread ID now has much stronger uniqueness characteristics. The library
+garrantees not to reuse the same thread ID for at least 2^(wordsize) thread
+destruction/creation cycles.
+
+New tests
+---------
+
+* semaphore4.c: Tests cancelation of the new sem_wait().
+
+* semaphore4t.c: Likewise for sem_timedwait().
+
+* rwlock8.c: Tests and times the slow execution paths of r/w locks, and the CVs,
+mutexes, and semaphores that they're built on.
+
+
+SNAPSHOT 2004-05-16
+-------------------
+
+Attempt to add Watcom to the list of compilers that can build the library.
+This failed in the end due to it's non-thread-aware errno. The library
+builds but the test suite fails. See README.Watcom for more details.
+
+Bug fixes
+---------
+* Bug and memory leak in sem_init()
+- Alex Blanco <Alex.Blanco at motorola.com>
+
+* ptw32_getprocessors() now returns CPU count of 1 for WinCE.
+- James Ewing <james.ewing at sveasoft.com>
+
+* pthread_cond_wait() could be canceled at a point where it should not
+be cancelable. Fixed.
+- Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+* sem_timedwait() had an incorrect timeout calculation.
+- Philippe Di Cristo <philipped at voicebox.com>
+
+* Fix a memory leak left behind after threads are destroyed.
+- P. van Bruggen <pietvb at newbridges.nl>
+
+New features
+------------
+* Ported to AMD64.
+- Makoto Kato <raven at oldskool.jp>
+
+* True pre-emptive asynchronous cancelation of threads. This is optional
+and requires that Panagiotis E. Hadjidoukas's QueueUserAPCEx package be
+installed. This package is included in the pthreads-win32 self-unpacking
+Zip archive starting from this snapshot. See the README.txt file inside
+the package for installation details.
+
+Note: If you don't use async cancelation in your application, or don't need
+to cancel threads that are blocked on system resources such as network I/O,
+then the default non-preemptive async cancelation is probably good enough.
+However, pthreads-win32 auto-detects the availability of these components
+at run-time, so you don't need to rebuild the library from source if you
+change your mind later.
+
+All of the advice available in books and elsewhere on the undesirability
+of using async cancelation in any application still stands, but this
+feature is a welcome addition with respect to the library's conformance to
+the POSIX standard.
+
+SNAPSHOT 2003-09-18
+-------------------
+
+Cleanup of thread priority management. In particular, setting of thread
+priority now attempts to map invalid Win32 values within the range returned
+by sched_get_priority_min/max() to useful values. See README.NONPORTABLE
+under "Thread priority".
+
+Bug fixes
+---------
+* pthread_getschedparam() now returns the priority given by the most recent
+call to pthread_setschedparam() or established by pthread_create(), as
+required by the standard. Previously, pthread_getschedparam() incorrectly
+returned the running thread priority at the time of the call, which may have
+been adjusted or temporarily promoted/demoted.
+
+* sched_get_priority_min() and sched_get_priority_max() now return -1 on error
+and set errno. Previously, they incorrectly returned the error value directly.
+
+
+SNAPSHOT 2003-09-04
+-------------------
+
+Bug fixes
+---------
+* ptw32_cancelableWait() now allows cancelation of waiting implicit POSIX
+threads.
+
+New test
+--------
+* cancel8.c tests cancelation of Win32 threads waiting at a POSIX cancelation
+point.
+
+
+SNAPSHOT 2003-09-03
+-------------------
+
+Bug fixes
+---------
+* pthread_self() would free the newly created implicit POSIX thread handle if
+DuplicateHandle failed instead of recycle it (very unlikely).
+
+* pthread_exit() was neither freeing nor recycling the POSIX thread struct
+for implicit POSIX threads.
+
+New feature - Cancelation of/by Win32 (non-POSIX) threads
+---------------------------------------------------------
+Since John Bossom's original implementation, the library has allowed non-POSIX
+initialised threads (Win32 threads) to call pthreads-win32 routines and
+therefore interact with POSIX threads. This is done by creating an on-the-fly
+POSIX thread ID for the Win32 thread that, once created, allows fully
+reciprical interaction. This did not extend to thread cancelation (async or
+deferred). Now it does.
+
+Any thread can be canceled by any other thread (Win32 or POSIX) if the former
+thread's POSIX pthread_t value is known. It's TSD destructors and POSIX
+cleanup handlers will be run before the thread exits with an exit code of
+PTHREAD_CANCELED (retrieved with GetExitCodeThread()).
+
+This allows a Win32 thread to, for example, call POSIX CV routines in the same way
+that POSIX threads would/should, with pthread_cond_wait() cancelability and
+cleanup handlers (pthread_cond_wait() is a POSIX cancelation point).
+
+By adding cancelation, Win32 threads should now be able to call all POSIX
+threads routines that make sense including semaphores, mutexes, condition
+variables, read/write locks, barriers, spinlocks, tsd, cleanup push/pop,
+cancelation, pthread_exit, scheduling, etc.
+
+Note that these on-the-fly 'implicit' POSIX thread IDs are initialised as detached
+(not joinable) with deferred cancelation type. The POSIX thread ID will be created
+automatically by any POSIX routines that need a POSIX handle (unless the routine
+needs a pthread_t as a parameter of course). A Win32 thread can discover it's own
+POSIX thread ID by calling pthread_self(), which will create the handle if
+necessary and return the pthread_t value.
+
+New tests
+---------
+Test the above new feature.
+
+
+SNAPSHOT 2003-08-19
+-------------------
+
+This snapshot fixes some accidental corruption to new test case sources.
+There are no changes to the library source code.
+
+
+SNAPSHOT 2003-08-15
+-------------------
+
+Bug fixes
+---------
+
+* pthread.dsp now uses correct compile flags (/MD).
+- Viv <vcotirlea at hotmail.com>
+
+* pthread_win32_process_detach_np() fixed memory leak.
+- Steven Reddie <Steven.Reddie at ca.com>
+
+* pthread_mutex_destroy() fixed incorrect return code.
+- Nicolas Barry <boozai at yahoo.com>
+
+* pthread_spin_destroy() fixed memory leak.
+- Piet van Bruggen <pietvb at newbridges.nl>
+
+* Various changes to tighten arg checking, and to work with later versions of
+MinGW32 and MsysDTK.
+
+* pthread_getschedparam() etc, fixed dangerous thread validity checking.
+- Nicolas Barry <boozai at yahoo.com>
+
+* POSIX thread handles are now reused and their memory is not freed on thread exit.
+This allows for stronger thread validity checking.
+
+New standard routine
+--------------------
+
+* pthread_kill() added to provide thread validity checking to applications.
+It does not accept any non zero values for the signal arg.
+
+New test cases
+--------------
+
+* New test cases to confirm validity checking, pthread_kill(), and thread reuse.
+
+
+SNAPSHOT 2003-05-10
+-------------------
+
+Bug fixes
+---------
+
+* pthread_mutex_trylock() now returns correct error values.
+pthread_mutex_destroy() will no longer destroy a recursively locked mutex.
+pthread_mutex_lock() is no longer inadvertantly behaving as a cancelation point.
+- Thomas Pfaff <tpfaff at gmx.net>
+
+* pthread_mutex_timedlock() no longer occasionally sets incorrect mutex
+ownership, causing deadlocks in some applications.
+- Robert Strycek <strycek at posam.sk> and Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+
+SNAPSHOT 2002-11-04
+-------------------
+
+Bug fixes
+---------
+
+* sem_getvalue() now returns the correct value under Win NT and WinCE.
+- Rob Fanner <rfanner at stonethree.com>
+
+* sem_timedwait() now uses tighter checks for unreasonable
+abstime values - that would result in unexpected timeout values.
+
+* ptw32_cond_wait_cleanup() no longer mysteriously consumes
+CV signals but may produce more spurious wakeups. It is believed
+that the sem_timedwait() call is consuming a CV signal that it
+shouldn't.
+- Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+* Fixed a memory leak in ptw32_threadDestroy() for implicit threads.
+
+* Fixed potential for deadlock in pthread_cond_destroy().
+A deadlock could occur for statically declared CVs (PTHREAD_COND_INITIALIZER),
+when one thread is attempting to destroy the condition variable while another
+is attempting to dynamically initialize it.
+- Michael Johnson <michaelj at maine.rr.com>
+
+
+SNAPSHOT 2002-03-02
+-------------------
+
+Cleanup code default style. (IMPORTANT)
+----------------------------------------------------------------------
+Previously, if not defined, the cleanup style was determined automatically
+from the compiler/language, and one of the following was defined accordingly:
+
+ __CLEANUP_SEH MSVC only
+ __CLEANUP_CXX C++, including MSVC++, GNU G++
+ __CLEANUP_C C, including GNU GCC, not MSVC
+
+These defines determine the style of cleanup (see pthread.h) and,
+most importantly, the way that cancelation and thread exit (via
+pthread_exit) is performed (see the routine ptw32_throw() in private.c).
+
+In short, the exceptions versions of the library throw an exception
+when a thread is canceled or exits (via pthread_exit()), which is
+caught by a handler in the thread startup routine, so that the
+the correct stack unwinding occurs regardless of where the thread
+is when it's canceled or exits via pthread_exit().
+
+In this and future snapshots, unless the build explicitly defines (e.g.
+via a compiler option) __CLEANUP_SEH, __CLEANUP_CXX, or __CLEANUP_C, then
+the build NOW always defaults to __CLEANUP_C style cleanup. This style
+uses setjmp/longjmp in the cancelation and pthread_exit implementations,
+and therefore won't do stack unwinding even when linked to applications
+that have it (e.g. C++ apps). This is for consistency with most
+current commercial Unix POSIX threads implementations. Compaq's TRU64
+may be an exception (no pun intended) and possible future trend.
+
+Although it was not clearly documented before, it is still necessary to
+build your application using the same __CLEANUP_* define as was
+used for the version of the library that you link with, so that the
+correct parts of pthread.h are included. That is, the possible
+defines require the following library versions:
+
+ __CLEANUP_SEH pthreadVSE.dll
+ __CLEANUP_CXX pthreadVCE.dll or pthreadGCE.dll
+ __CLEANUP_C pthreadVC.dll or pthreadGC.dll
+
+E.g. regardless of whether your app is C or C++, if you link with
+pthreadVC.lib or libpthreadGC.a, then you must define __CLEANUP_C.
+
+
+THE POINT OF ALL THIS IS: if you have not been defining one of these
+explicitly, then the defaults as described at the top of this
+section were being used.
+
+THIS NOW CHANGES, as has been explained above, but to try to make this
+clearer here's an example:
+
+If you were building your application with MSVC++ i.e. using C++
+exceptions and not explicitly defining one of __CLEANUP_*, then
+__CLEANUP_C++ was automatically defined for you in pthread.h.
+You should have been linking with pthreadVCE.dll, which does
+stack unwinding.
+
+If you now build your application as you had before, pthread.h will now
+automatically set __CLEANUP_C as the default style, and you will need to
+link with pthreadVC.dll. Stack unwinding will now NOT occur when a thread
+is canceled, or the thread calls pthread_exit().
+
+Your application will now most likely behave differently to previous
+versions, and in non-obvious ways. Most likely is that locally
+instantiated objects may not be destroyed or cleaned up after a thread
+is canceled.
+
+If you want the same behaviour as before, then you must now define
+__CLEANUP_C++ explicitly using a compiler option and link with
+pthreadVCE.dll as you did before.
+
+
+WHY ARE WE MAKING THE DEFAULT STYLE LESS EXCEPTION-FRIENDLY?
+Because no commercial Unix POSIX threads implementation allows you to
+choose to have stack unwinding. Therefore, providing it in pthread-win32
+as a default is dangerous. We still provide the choice but unless
+you consciously choose to do otherwise, your pthreads applications will
+now run or crash in similar ways irrespective of the threads platform
+you use. Or at least this is the hope.
+
+
+WHY NOT REMOVE THE EXCEPTIONS VERSIONS OF THE LIBRARY ALTOGETHER?
+There are a few reasons:
+- because there are well respected POSIX threads people who believe
+ that POSIX threads implementations should be exceptions aware and
+ do the expected thing in that context. (There are equally respected
+ people who believe it should not be easily accessible, if it's there
+ at all, for unconditional conformity to other implementations.)
+- because pthreads-win32 is one of the few implementations that has
+ the choice, perhaps the only freely available one, and so offers
+ a laboratory to people who may want to explore the effects;
+- although the code will always be around somewhere for anyone who
+ wants it, once it's removed from the current version it will not be
+ nearly as visible to people who may have a use for it.
+
+
+Source module splitting
+-----------------------
+In order to enable smaller image sizes to be generated
+for applications that link statically with the library,
+most routines have been separated out into individual
+source code files.
+
+This is being done in such a way as to be backward compatible.
+The old source files are reused to congregate the individual
+routine files into larger translation units (via a bunch of
+# includes) so that the compiler can still optimise wherever
+possible, e.g. through inlining, which can only be done
+within the same translation unit.
+
+It is also possible to build the entire library by compiling
+the single file named "pthread.c", which just #includes all
+the secondary congregation source files. The compiler
+may be able to use this to do more inlining of routines.
+
+Although the GNU compiler is able to produce libraries with
+the necessary separation (the -ffunction-segments switch),
+AFAIK, the MSVC and other compilers don't have this feature.
+
+Finally, since I use makefiles and command-line compilation,
+I don't know what havoc this reorganisation may wreak amongst
+IDE project file users. You should be able to continue
+using your existing project files without modification.
+
+
+New non-portable functions
+--------------------------
+pthread_num_processors_np():
+ Returns the number of processors in the system that are
+ available to the process, as determined from the processor
+ affinity mask.
+
+pthread_timechange_handler_np():
+ To improve tolerance against operator or time service initiated
+ system clock changes.
+
+ This routine can be called by an application when it
+ receives a WM_TIMECHANGE message from the system. At present
+ it broadcasts all condition variables so that waiting threads
+ can wake up and re-evaluate their conditions and restart
+ their timed waits if required.
+ - Suggested by Alexander Terekhov
+
+
+Platform dependence
+-------------------
+As Win95 doesn't provide one, the library now contains
+it's own InterlockedCompareExchange() routine, which is used
+whenever Windows doesn't provide it. InterlockedCompareExchange()
+is used to implement spinlocks and barriers, and also in mutexes.
+This routine relies on the CMPXCHG machine instruction which
+is not available on i386 CPUs. This library (from snapshot
+20010712 onwards) is therefore no longer supported on i386
+processor platforms.
+
+
+New standard routines
+---------------------
+For source code portability only - rwlocks cannot be process shared yet.
+
+ pthread_rwlockattr_init()
+ pthread_rwlockattr_destroy()
+ pthread_rwlockattr_setpshared()
+ pthread_rwlockattr_getpshared()
+
+As defined in the new POSIX standard, and the Single Unix Spec version 3:
+
+ sem_timedwait()
+ pthread_mutex_timedlock() - Alexander Terekhov and Thomas Pfaff
+ pthread_rwlock_timedrdlock() - adapted from pthread_rwlock_rdlock()
+ pthread_rwlock_timedwrlock() - adapted from pthread_rwlock_wrlock()
+
+
+pthread.h no longer includes windows.h
+--------------------------------------
+[Not yet for G++]
+
+This was done to prevent conflicts.
+
+HANDLE, DWORD, and NULL are temporarily defined within pthread.h if
+they are not already.
+
+
+pthread.h, sched.h and semaphore.h now use dllexport/dllimport
+--------------------------------------------------------------
+Not only to avoid the need for the pthread.def file, but to
+improve performance. Apparently, declaring functions with dllimport
+generates a direct call to the function and avoids the overhead
+of a stub function call.
+
+Bug fixes
+---------
+* Fixed potential NULL pointer dereferences in pthread_mutexattr_init,
+pthread_mutexattr_getpshared, pthread_barrierattr_init,
+pthread_barrierattr_getpshared, and pthread_condattr_getpshared.
+- Scott McCaskill <scott at magruder.org>
+
+* Removed potential race condition in pthread_mutex_trylock and
+pthread_mutex_lock;
+- Alexander Terekhov <TEREKHOV at de.ibm.com>
+
+* The behaviour of pthread_mutex_trylock in relation to
+recursive mutexes was inconsistent with commercial implementations.
+Trylock would return EBUSY if the lock was owned already by the
+calling thread regardless of mutex type. Trylock now increments the
+recursion count and returns 0 for RECURSIVE mutexes, and will
+return EDEADLK rather than EBUSY for ERRORCHECK mutexes. This is
+consistent with Solaris.
+- Thomas Pfaff <tpfaff at gmx.net>
+
+* Found a fix for the library and workaround for applications for
+the known bug #2, i.e. where __CLEANUP_CXX or __CLEANUP_SEH is defined.
+See the "Known Bugs in this snapshot" section below.
+
+This could be made transparent to applications by replacing the macros that
+define the current C++ and SEH versions of pthread_cleanup_push/pop
+with the C version, but AFAIK cleanup handlers would not then run in the
+correct sequence with destructors and exception cleanup handlers when
+an exception occurs.
+
+* Cancelation once started in a thread cannot now be inadvertantly
+double canceled. That is, once a thread begins it's cancelation run,
+cancelation is disabled and a subsequent cancel request will
+return an error (ESRCH).
+
+* errno: An incorrect compiler directive caused a local version
+of errno to be used instead of the Win32 errno. Both instances are
+thread-safe but applications checking errno after a pthreads-win32
+call would be wrong. Fixing this also fixed a bad compiler
+option in the testsuite (/MT should have been /MD) which is
+needed to link with the correct library MSVCRT.LIB.
+
+
+SNAPSHOT 2001-07-12
+-------------------
+
+To be added
+
+
+SNAPSHOT 2001-07-03
+-------------------
+
+To be added
+
+
+SNAPSHOT 2000-08-13
+-------------------
+
+New:
+- Renamed DLL and LIB files:
+ pthreadVSE.dll (MS VC++/Structured EH)
+ pthreadVSE.lib
+ pthreadVCE.dll (MS VC++/C++ EH)
+ pthreadVCE.lib
+ pthreadGCE.dll (GNU G++/C++ EH)
+ libpthreadw32.a
+
+ Both your application and the pthread dll should use the
+ same exception handling scheme.
+
+Bugs fixed:
+- MSVC++ C++ exception handling.
+
+Some new tests have been added.
+
+
+SNAPSHOT 2000-08-10
+-------------------
+
+New:
+- asynchronous cancelation on X86 (Jason Nye)
+- Makefile compatible with MS nmake to replace
+ buildlib.bat
+- GNUmakefile for Mingw32
+- tests/Makefile for MS nmake replaces runall.bat
+- tests/GNUmakefile for Mingw32
+
+Bugs fixed:
+- kernel32 load/free problem
+- attempt to hide internel exceptions from application
+ exception handlers (__try/__except and try/catch blocks)
+- Win32 thread handle leakage bug
+ (David Baggett/Paul Redondo/Eyal Lebedinsky)
+
+Some new tests have been added.
+
+
+SNAPSHOT 1999-11-02
+-------------------
+
+Bugs fixed:
+- ctime_r macro had an incorrect argument (Erik Hensema),
+- threads were not being created
+ PTHREAD_CANCEL_DEFERRED. This should have
+ had little effect as deferred is the only
+ supported type. (Ross Johnson).
+
+Some compatibility improvements added, eg.
+- pthread_setcancelstate accepts NULL pointer
+ for the previous value argument. Ditto for
+ pthread_setcanceltype. This is compatible
+ with Solaris but should not affect
+ standard applications (Erik Hensema)
+
+Some new tests have been added.
+
+
+SNAPSHOT 1999-10-17
+-------------------
+
+Bug fix - Cancelation of threads waiting on condition variables
+now works properly (Lorin Hochstein and Peter Slacik)
+
+
+SNAPSHOT 1999-08-12
+-------------------
+
+Fixed exception stack cleanup if calling pthread_exit()
+- (Lorin Hochstein and John Bossom).
+
+Fixed bugs in condition variables - (Peter Slacik):
+ - additional contention checks
+ - properly adjust number of waiting threads after timed
+ condvar timeout.
+
+
+SNAPSHOT 1999-05-30
+-------------------
+
+Some minor bugs have been fixed. See the ChangeLog file for details.
+
+Some more POSIX 1b functions are now included but ony return an
+error (ENOSYS) if called. They are:
+
+ sem_open
+ sem_close
+ sem_unlink
+ sem_getvalue
+
+
+SNAPSHOT 1999-04-07
+-------------------
+
+Some POSIX 1b functions which were internally supported are now
+available as exported functions:
+
+ sem_init
+ sem_destroy
+ sem_wait
+ sem_trywait
+ sem_post
+ sched_yield
+ sched_get_priority_min
+ sched_get_priority_max
+
+Some minor bugs have been fixed. See the ChangeLog file for details.
+
+
+SNAPSHOT 1999-03-16
+-------------------
+
+Initial release.
+
diff --git a/3rdparty/pthreads-win32/Nmakefile b/3rdparty/pthreads-win32/Nmakefile
new file mode 100644
index 0000000..d9e5bf1
--- /dev/null
+++ b/3rdparty/pthreads-win32/Nmakefile
@@ -0,0 +1,24 @@
+/*
+ * nmake file for uwin pthread library
+ */
+
+VERSION = -
+CCFLAGS = -V -g $(CC.DLL)
+HAVE_PTW32_CONFIG_H == 1
+_MT == 1
+_timeb == timeb
+_ftime == ftime
+_errno == _ast_errno
+
+$(INCLUDEDIR) :INSTALLDIR: pthread.h sched.h
+
+pthread $(VERSION) :LIBRARY: attr.c barrier.c cancel.c cleanup.c condvar.c \
+ create.c dll.c exit.c fork.c global.c misc.c mutex.c private.c \
+ rwlock.c sched.c semaphore.c spin.c sync.c tsd.c nonportable.c
+
+:: ANNOUNCE CONTRIBUTORS COPYING.LIB ChangeLog FAQ GNUmakefile MAINTAINERS \
+ Makefile Makefile.in Makefile.vc NEWS PROGRESS README README.WinCE \
+ TODO WinCE-PORT install-sh errno.c tests tests.mk acconfig.h \
+ config.guess config.h.in config.sub configure configure.in signal.c \
+ README.CV README.NONPORTABLE pthread.dsp pthread.dsw
+
diff --git a/3rdparty/pthreads-win32/Nmakefile.tests b/3rdparty/pthreads-win32/Nmakefile.tests
new file mode 100644
index 0000000..203560b
--- /dev/null
+++ b/3rdparty/pthreads-win32/Nmakefile.tests
@@ -0,0 +1,260 @@
+/* for running tests */
+CCFLAGS = -g
+_MT == 1
+_timeb == timeb
+_ftime == ftime
+
+.SOURCE: tests
+/*
+:PACKAGE: pthread
+*/
+
+set keepgoing
+
+":test:" : .MAKE .OPERATOR
+ local I
+ $(<:D:B:S=.pass) : .IMPLICIT $(>:D:B:S=.pass)
+ for I $(<) $(>)
+ $(I:D:B:S=.pass) : .VIRTUAL .FORCE $(I)
+ $(>)
+ end
+sizes:: sizes.c
+loadfree:: loadfree.c
+mutex1:: mutex1.c
+mutex1e:: mutex1e.c
+mutex1n:: mutex1n.c
+mutex1r:: mutex1r.c
+mutex2:: mutex2.c
+mutex2r:: mutex2r.c
+mutex2e:: mutex2e.c
+exit1:: exit1.c
+condvar1:: condvar1.c
+condvar1_1:: condvar1_1.c
+condvar1_2:: condvar1_2.c
+self1:: self1.c
+condvar2:: condvar2.c
+condvar2_1:: condvar2_1.c
+condvar3_1:: condvar3_1.c
+condvar3_2:: condvar3_2.c
+condvar3_3:: condvar3_3.c
+create1.:: create1.c
+create2.:: create2.c
+cancel1:: cancel1.c
+cancel2:: cancel2.c
+mutex3:: mutex3.c
+mutex3r:: mutex3r.c
+mutex3e:: mutex3e.c
+mutex4:: mutex4.c
+mutex5:: mutex5.c
+mutex6:: mutex6.c
+mutex6e:: mutex6e.c
+mutex6n:: mutex6n.c
+mutex6r:: mutex6r.c
+mutex7:: mutex7.c
+mutex6s:: mutex6s.c
+mutex6rs:: mutex6rs.c
+mutex6es:: mutex6es.c
+mutex7e:: mutex7e.c
+mutex7n:: mutex7n.c
+mutex7r:: mutex7r.c
+mutex8:: mutex8.c
+mutex8e:: mutex8e.c
+mutex8n:: mutex8n.c
+mutex8r:: mutex8r.c
+equal1:: equal1.c
+exit2:: exit2.c
+exit3:: exit3.c
+exit4:: exit4.c
+exit5:: exit5.c
+join0:: join0.c
+join1:: join1.c
+join2:: join2.c
+join3:: join3.c
+kill1:: kill1.c
+count1:: count1.c
+once1:: once1.c
+tsd1:: tsd1.c
+self2:: self2.c
+eyal1:: eyal1.c
+condvar3:: condvar3.c
+condvar4:: condvar4.c
+condvar5:: condvar5.c
+condvar6:: condvar6.c
+condvar7:: condvar7.c
+condvar8:: condvar8.c
+condvar9:: condvar9.c
+errno1:: errno1.c
+reuse1.:: reuse1.c
+reuse2.:: reuse2.c
+rwlock1:: rwlock1.c
+rwlock2:: rwlock2.c
+rwlock3:: rwlock3.c
+rwlock4:: rwlock4.c
+rwlock5:: rwlock5.c
+rwlock6:: rwlock6.c
+rwlock7:: rwlock7.c
+rwlock8:: rwlock8.c
+rwlock2_t:: rwlock2_t.c
+rwlock3_t:: rwlock3_t.c
+rwlock4_t:: rwlock4_t.c
+rwlock5_t:: rwlock5_t.c
+rwlock6_t:: rwlock6_t.c
+rwlock6_t2:: rwlock6_t2.c
+semaphore1:: semaphore1.c
+semaphore2:: semaphore2.c
+semaphore3:: semaphore3.c
+context1:: context1.c
+cancel3:: cancel3.c
+cancel4:: cancel4.c
+cancel5:: cancel5.c
+cancel6a:: cancel6a.c
+cancel6d:: cancel6d.c
+cancel7:: cancel7.c
+cleanup0:: cleanup0.c
+cleanup1:: cleanup1.c
+cleanup2:: cleanup2.c
+cleanup3:: cleanup3.c
+priority1:: priority1.c
+priority2:: priority2.c
+inherit1:: inherit1.c
+spin1:: spin1.c
+spin2:: spin2.c
+spin3:: spin3.c
+spin4:: spin4.c
+barrier1:: barrier1.c
+barrier2:: barrier2.c
+barrier3:: barrier3.c
+barrier4:: barrier4.c
+barrier5:: barrier5.c
+exception1:: exception1.c
+exception2:: exception2.c
+exception3:: exception3.c
+benchtest1:: benchtest1.c
+benchtest2:: benchtest2.c
+benchtest3:: benchtest3.c
+benchtest4:: benchtest4.c
+benchtest5:: benchtest5.c
+valid1:: valid1.c
+valid2:: valid2.c
+cancel9:: cancel9.c
+
+sizes: :test: sizes
+loadfree: :test:
+mutex5 :test: loadfree
+mutex1 :test: loadfree
+mutex1n :test: loadfree
+mutex1r :test: loadfree
+mutex1e :test: loadfree
+semaphore1 :test: loadfree
+semaphore2 :test: loadfree
+semaphore3 :test: loadfree
+mutex2 :test: loadfree
+mutex2r :test: loadfree
+mutex2e :test: loadfree
+exit1 :test: loadfree
+condvar1 :test: loadfree
+kill1 :test: loadfree
+condvar1_1 :test: condvar1
+condvar1_2 :test: join2
+self1 :test: loadfree
+condvar2 :test: condvar1
+condvar2_1 :test: condvar2
+create1 :test: mutex2
+create2 :test: create1
+reuse1 :test: create2
+reuse2 :test: reuse1
+cancel1 :test: create1
+cancel2 :test: cancel1
+mutex3 :test: create1
+mutex3r :test: create1
+mutex3e :test: create1
+mutex4 :test: mutex3
+mutex6 :test: mutex4
+mutex6n :test: mutex4
+mutex6e :test: mutex4
+mutex6r :test: mutex4
+mutex6s :test: mutex6
+mutex6rs :test: mutex6r
+mutex6es :test: mutex6e
+mutex7 :test: mutex6
+mutex7n :test: mutex6n
+mutex7e :test: mutex6e
+mutex7r :test: mutex6r
+mutex8 :test: mutex7
+mutex8n :test: mutex7n
+mutex8e :test: mutex7e
+mutex8r :test: mutex7r
+equal1 :test: create1
+exit2 :test: create1
+exit3 :test: create1
+exit4 :test: kill1
+exit5 :test: exit4
+join0 :test: create1
+join1 :test: create1
+join2 :test: create1
+join3 :test: join2
+count1 :test: join1
+once1 :test: create1
+tsd1 :test: join1
+self2 :test: create1
+eyal1 :test: tsd1
+condvar3 :test: create1
+condvar3_1 :test: condvar3
+condvar3_2 :test: condvar3_1
+condvar3_3 :test: condvar3_2
+condvar4 :test: create1
+condvar5 :test: condvar4
+condvar6 :test: condvar5
+condvar7 :test: condvar6 cleanup1
+condvar8 :test: condvar7
+condvar9 :test: condvar8
+errno1 :test: mutex3
+rwlock1 :test: condvar6
+rwlock2 :test: rwlock1
+rwlock3 :test: rwlock2
+rwlock4 :test: rwlock3
+rwlock5 :test: rwlock4
+rwlock6 :test: rwlock5
+rwlock7 :test: rwlock6
+rwlock8 :test: rwlock7
+rwlock2_t :test: rwlock2
+rwlock3_t :test: rwlock2_t
+rwlock4_t :test: rwlock3_t
+rwlock5_t :test: rwlock4_t
+rwlock6_t :test: rwlock5_t
+rwlock6_t2 :test: rwlock6_t
+context1 :test: cancel2
+cancel3 :test: context1
+cancel4 :test: cancel3
+cancel5 :test: cancel3
+cancel6a :test: cancel3
+cancel6d :test: cancel3
+cancel7 :test: kill1
+cleanup0 :test: cancel5
+cleanup1 :test: cleanup0
+cleanup2 :test: cleanup1
+cleanup3 :test: cleanup2
+priority1 :test: join1
+priority2 :test: priority1
+inherit1 :test: join1
+spin1 :test:
+spin2 :test: spin1.c
+spin3 :test: spin2.c
+spin4 :test: spin3.c
+barrier1 :test:
+barrier2 :test: barrier1.c
+barrier3 :test: barrier2.c
+barrier4 :test: barrier3.c
+barrier5 :test: barrier4.c
+benchtest1 :test: mutex3
+benchtest2 :test: benchtest1
+benchtest3 :test: benchtest2
+benchtest4 :test: benchtest3
+benchtest5 :test: benchtest4
+exception1 :test: cancel4
+exception2 :test: exception1
+exception3 :test: exception2
+exit4 :test: exit3
+valid1 :test: join1
+valid2 :test: valid1
+cancel9 :test: cancel8
diff --git a/3rdparty/pthreads-win32/PROGRESS b/3rdparty/pthreads-win32/PROGRESS
new file mode 100644
index 0000000..9abf0bc
--- /dev/null
+++ b/3rdparty/pthreads-win32/PROGRESS
@@ -0,0 +1,4 @@
+Please see the ANNOUNCE file "Level of Standards Conformance"
+or the web page:
+
+http://sources.redhat.com/pthreads-win32/conformance.html
diff --git a/3rdparty/pthreads-win32/README b/3rdparty/pthreads-win32/README
new file mode 100644
index 0000000..545360b
--- /dev/null
+++ b/3rdparty/pthreads-win32/README
@@ -0,0 +1,601 @@
+PTHREADS-WIN32
+==============
+
+Pthreads-win32 is free software, distributed under the GNU Lesser
+General Public License (LGPL). See the file 'COPYING.LIB' for terms
+and conditions. Also see the file 'COPYING' for information
+specific to pthreads-win32, copyrights and the LGPL.
+
+
+What is it?
+-----------
+
+Pthreads-win32 is an Open Source Software implementation of the
+Threads component of the POSIX 1003.1c 1995 Standard (or later)
+for Microsoft's Win32 environment. Some functions from POSIX
+1003.1b are also supported including semaphores. Other related
+functions include the set of read-write lock functions. The
+library also supports some of the functionality of the Open
+Group's Single Unix specification, version 2, namely mutex types,
+plus some common and pthreads-win32 specific non-portable
+routines (see README.NONPORTABLE).
+
+See the file "ANNOUNCE" for more information including standards
+conformance details and the list of supported and unsupported
+routines.
+
+
+Prerequisites
+-------------
+MSVC or GNU C (MinGW32 MSys development kit)
+ To build from source.
+
+QueueUserAPCEx by Panagiotis E. Hadjidoukas
+ To support any thread cancelation in C++ library builds or
+ to support cancelation of blocked threads in any build.
+ This library is not required otherwise.
+
+ For true async cancelation of threads (including blocked threads).
+ This is a DLL and Windows driver that provides pre-emptive APC
+ by forcing threads into an alertable state when the APC is queued.
+ Both the DLL and driver are provided with the pthreads-win32.exe
+ self-unpacking ZIP, and on the pthreads-win32 FTP site (in source
+ and pre-built forms). Currently this is a separate LGPL package to
+ pthreads-win32. See the README in the QueueUserAPCEx folder for
+ installation instructions.
+
+ Pthreads-win32 will automatically detect if the QueueUserAPCEx DLL
+ QuserEx.DLL is available and whether the driver AlertDrv.sys is
+ loaded. If it is not available, pthreads-win32 will simulate async
+ cancelation, which means that it can async cancel only threads that
+ are runnable. The simulated async cancellation cannot cancel blocked
+ threads.
+
+ [FOR SECURITY] To be found Quserex.dll MUST be installed in the
+ Windows System Folder. This is not an unreasonable constraint given a
+ driver must also be installed and loaded at system startup.
+
+
+Library naming
+--------------
+
+Because the library is being built using various exception
+handling schemes and compilers - and because the library
+may not work reliably if these are mixed in an application,
+each different version of the library has it's own name.
+
+Note 1: the incompatibility is really between EH implementations
+of the different compilers. It should be possible to use the
+standard C version from either compiler with C++ applications
+built with a different compiler. If you use an EH version of
+the library, then you must use the same compiler for the
+application. This is another complication and dependency that
+can be avoided by using only the standard C library version.
+
+Note 2: if you use a standard C pthread*.dll with a C++
+application, then any functions that you define that are
+intended to be called via pthread_cleanup_push() must be
+__cdecl.
+
+Note 3: the intention was to also name either the VC or GC
+version (it should be arbitrary) as pthread.dll, including
+pthread.lib and libpthread.a as appropriate. This is no longer
+likely to happen.
+
+Note 4: the compatibility number was added so that applications
+can differentiate between binary incompatible versions of the
+libs and dlls.
+
+In general:
+ pthread[VG]{SE,CE,C}[c].dll
+ pthread[VG]{SE,CE,C}[c].lib
+
+where:
+ [VG] indicates the compiler
+ V - MS VC, or
+ G - GNU C
+
+ {SE,CE,C} indicates the exception handling scheme
+ SE - Structured EH, or
+ CE - C++ EH, or
+ C - no exceptions - uses setjmp/longjmp
+
+ c - DLL compatibility number indicating ABI and API
+ compatibility with applications built against
+ a snapshot with the same compatibility number.
+ See 'Version numbering' below.
+
+The name may also be suffixed by a 'd' to indicate a debugging version
+of the library. E.g. pthreadVC2d.lib. Debugging versions contain
+additional information for debugging (symbols etc) and are often not
+optimised in any way (compiled with optimisation turned off).
+
+Examples:
+ pthreadVSE.dll (MSVC/SEH)
+ pthreadGCE.dll (GNUC/C++ EH)
+ pthreadGC.dll (GNUC/not dependent on exceptions)
+ pthreadVC1.dll (MSVC/not dependent on exceptions - not binary
+ compatible with pthreadVC.dll)
+ pthreadVC2.dll (MSVC/not dependent on exceptions - not binary
+ compatible with pthreadVC1.dll or pthreadVC.dll)
+
+The GNU library archive file names have correspondingly changed to:
+
+ libpthreadGCEc.a
+ libpthreadGCc.a
+
+
+Versioning numbering
+--------------------
+
+Version numbering is separate from the snapshot dating system, and
+is the canonical version identification system embedded within the
+DLL using the Microsoft version resource system. The versioning
+system chosen follows the GNU Libtool system. See
+http://www.gnu.org/software/libtool/manual.html section 6.2.
+
+See the resource file 'version.rc'.
+
+Microsoft version numbers use 4 integers:
+
+ 0.0.0.0
+
+Pthreads-win32 uses the first 3 following the Libtool convention.
+The fourth is commonly used for the build number, but will be reserved
+for future use.
+
+ current.revision.age.0
+
+The numbers are changed as follows:
+
+1. If the library source code has changed at all since the last update,
+ then increment revision (`c:r:a' becomes `c:r+1:a').
+2. If any interfaces have been added, removed, or changed since the last
+ update, increment current, and set revision to 0.
+3. If any interfaces have been added since the last public release, then
+ increment age.
+4. If any interfaces have been removed or changed since the last public
+ release, then set age to 0.
+
+
+DLL compatibility numbering is an attempt to ensure that applications
+always load a compatible pthreads-win32 DLL by using a DLL naming system
+that is consistent with the version numbering system. It also allows
+older and newer DLLs to coexist in the same filesystem so that older
+applications can continue to be used. For pre .NET Windows systems,
+this inevitably requires incompatible versions of the same DLLs to have
+different names.
+
+Pthreads-win32 has adopted the Cygwin convention of appending a single
+integer number to the DLL name. The number used is based on the library
+version number and is computed as 'current' - 'age'.
+
+(See http://home.att.net/~perlspinr/libversioning.html for a nicely
+detailed explanation.)
+
+Using this method, DLL name/s will only change when the DLL's
+backwards compatibility changes. Note that the addition of new
+'interfaces' will not of itself change the DLL's compatibility for older
+applications.
+
+
+Which of the several dll versions to use?
+-----------------------------------------
+or,
+---
+What are all these pthread*.dll and pthread*.lib files?
+-------------------------------------------------------
+
+Simple, use either pthreadGCv.* if you use GCC, or pthreadVCv.* if you
+use MSVC - where 'v' is the DLL versioning (compatibility) number.
+
+Otherwise, you need to choose carefully and know WHY.
+
+The most important choice you need to make is whether to use a
+version that uses exceptions internally, or not. There are versions
+of the library that use exceptions as part of the thread
+cancelation and exit implementation. The default version uses
+setjmp/longjmp.
+
+There is some contension amongst POSIX threads experts as
+to how POSIX threads cancelation and exit should work
+with languages that use exceptions, e.g. C++ and even C
+(Microsoft's Structured Exceptions).
+
+The issue is: should cancelation of a thread in, say,
+a C++ application cause object destructors and C++ exception
+handlers to be invoked as the stack unwinds during thread
+exit, or not?
+
+There seems to be more opinion in favour of using the
+standard C version of the library (no EH) with C++ applications
+for the reason that this appears to be the assumption commercial
+pthreads implementations make. Therefore, if you use an EH version
+of pthreads-win32 then you may be under the illusion that
+your application will be portable, when in fact it is likely to
+behave differently when linked with other pthreads libraries.
+
+Now you may be asking: then why have you kept the EH versions of
+the library?
+
+There are a couple of reasons:
+- there is division amongst the experts and so the code may
+ be needed in the future. Yes, it's in the repository and we
+ can get it out anytime in the future, but it would be difficult
+ to find.
+- pthreads-win32 is one of the few implementations, and possibly
+ the only freely available one, that has EH versions. It may be
+ useful to people who want to play with or study application
+ behaviour under these conditions.
+
+Notes:
+
+[If you use either pthreadVCE or pthreadGCE]
+
+1. [See also the discussion in the FAQ file - Q2, Q4, and Q5]
+
+If your application contains catch(...) blocks in your POSIX
+threads then you will need to replace the "catch(...)" with the macro
+"PtW32Catch", eg.
+
+ #ifdef PtW32Catch
+ PtW32Catch {
+ ...
+ }
+ #else
+ catch(...) {
+ ...
+ }
+ #endif
+
+Otherwise neither pthreads cancelation nor pthread_exit() will work
+reliably when using versions of the library that use C++ exceptions
+for cancelation and thread exit.
+
+This is due to what is believed to be a C++ compliance error in VC++
+whereby you may not have multiple handlers for the same exception in
+the same try/catch block. GNU G++ doesn't have this restriction.
+
+
+Other name changes
+------------------
+
+All snapshots prior to and including snapshot 2000-08-13
+used "_pthread_" as the prefix to library internal
+functions, and "_PTHREAD_" to many library internal
+macros. These have now been changed to "ptw32_" and "PTW32_"
+respectively so as to not conflict with the ANSI standard's
+reservation of identifiers beginning with "_" and "__" for
+use by compiler implementations only.
+
+If you have written any applications and you are linking
+statically with the pthreads-win32 library then you may have
+included a call to _pthread_processInitialize. You will
+now have to change that to ptw32_processInitialize.
+
+
+Cleanup code default style
+--------------------------
+
+Previously, if not defined, the cleanup style was determined automatically
+from the compiler used, and one of the following was defined accordingly:
+
+ __CLEANUP_SEH MSVC only
+ __CLEANUP_CXX C++, including MSVC++, GNU G++
+ __CLEANUP_C C, including GNU GCC, not MSVC
+
+These defines determine the style of cleanup (see pthread.h) and,
+most importantly, the way that cancelation and thread exit (via
+pthread_exit) is performed (see the routine ptw32_throw()).
+
+In short, the exceptions versions of the library throw an exception
+when a thread is canceled, or exits via pthread_exit(). This exception is
+caught by a handler in the thread startup routine, so that the
+the correct stack unwinding occurs regardless of where the thread
+is when it's canceled or exits via pthread_exit().
+
+In this snapshot, unless the build explicitly defines (e.g. via a
+compiler option) __CLEANUP_SEH, __CLEANUP_CXX, or __CLEANUP_C, then
+the build NOW always defaults to __CLEANUP_C style cleanup. This style
+uses setjmp/longjmp in the cancelation and pthread_exit implementations,
+and therefore won't do stack unwinding even when linked to applications
+that have it (e.g. C++ apps). This is for consistency with most/all
+commercial Unix POSIX threads implementations.
+
+Although it was not clearly documented before, it is still necessary to
+build your application using the same __CLEANUP_* define as was
+used for the version of the library that you link with, so that the
+correct parts of pthread.h are included. That is, the possible
+defines require the following library versions:
+
+ __CLEANUP_SEH pthreadVSE.dll
+ __CLEANUP_CXX pthreadVCE.dll or pthreadGCE.dll
+ __CLEANUP_C pthreadVC.dll or pthreadGC.dll
+
+It is recommended that you let pthread.h use it's default __CLEANUP_C
+for both library and application builds. That is, don't define any of
+the above, and then link with pthreadVC.lib (MSVC or MSVC++) and
+libpthreadGC.a (MinGW GCC or G++). The reason is explained below, but
+another reason is that the prebuilt pthreadVCE.dll is currently broken.
+Versions built with MSVC++ later than version 6 may not be broken, but I
+can't verify this yet.
+
+WHY ARE WE MAKING THE DEFAULT STYLE LESS EXCEPTION-FRIENDLY?
+Because no commercial Unix POSIX threads implementation allows you to
+choose to have stack unwinding. Therefore, providing it in pthread-win32
+as a default is dangerous. We still provide the choice but unless
+you consciously choose to do otherwise, your pthreads applications will
+now run or crash in similar ways irrespective of the pthreads platform
+you use. Or at least this is the hope.
+
+
+Building under VC++ using C++ EH, Structured EH, or just C
+----------------------------------------------------------
+
+From the source directory run nmake without any arguments to list
+help information. E.g.
+
+$ nmake
+
+Microsoft (R) Program Maintenance Utility Version 6.00.8168.0
+Copyright (C) Microsoft Corp 1988-1998. All rights reserved.
+
+Run one of the following command lines:
+nmake clean VCE (to build the MSVC dll with C++ exception handling)
+nmake clean VSE (to build the MSVC dll with structured exception handling)
+nmake clean VC (to build the MSVC dll with C cleanup code)
+nmake clean VCE-inlined (to build the MSVC inlined dll with C++ exception handling)
+nmake clean VSE-inlined (to build the MSVC inlined dll with structured exception handling)
+nmake clean VC-inlined (to build the MSVC inlined dll with C cleanup code)
+nmake clean VC-static (to build the MSVC static lib with C cleanup code)
+nmake clean VCE-debug (to build the debug MSVC dll with C++ exception handling)
+nmake clean VSE-debug (to build the debug MSVC dll with structured exception handling)
+nmake clean VC-debug (to build the debug MSVC dll with C cleanup code)
+nmake clean VCE-inlined-debug (to build the debug MSVC inlined dll with C++ exception handling)
+nmake clean VSE-inlined-debug (to build the debug MSVC inlined dll with structured exception handling)
+nmake clean VC-inlined-debug (to build the debug MSVC inlined dll with C cleanup code)
+nmake clean VC-static-debug (to build the debug MSVC static lib with C cleanup code)
+
+
+The pre-built dlls are normally built using the *-inlined targets.
+
+You can run the testsuite by changing to the "tests" directory and
+running nmake. E.g.:
+
+$ cd tests
+$ nmake
+
+Microsoft (R) Program Maintenance Utility Version 6.00.8168.0
+Copyright (C) Microsoft Corp 1988-1998. All rights reserved.
+
+Run one of the following command lines:
+nmake clean VC (to test using VC dll with VC (no EH) applications)
+nmake clean VCX (to test using VC dll with VC++ (EH) applications)
+nmake clean VCE (to test using the VCE dll with VC++ EH applications)
+nmake clean VSE (to test using VSE dll with VC (SEH) applications)
+nmake clean VC-bench (to benchtest using VC dll with C bench app)
+nmake clean VCX-bench (to benchtest using VC dll with C++ bench app)
+nmake clean VCE-bench (to benchtest using VCE dll with C++ bench app)
+nmake clean VSE-bench (to benchtest using VSE dll with SEH bench app)
+nmake clean VC-static (to test using VC static lib with VC (no EH) applications)
+
+
+Building under Mingw32
+----------------------
+
+The dll can be built easily with recent versions of Mingw32.
+(The distributed versions are built using Mingw32 and MsysDTK
+from www.mingw32.org.)
+
+From the source directory, run make for help information. E.g.:
+
+$ make
+Run one of the following command lines:
+make clean GC (to build the GNU C dll with C cleanup code)
+make clean GCE (to build the GNU C dll with C++ exception handling)
+make clean GC-inlined (to build the GNU C inlined dll with C cleanup code)
+make clean GCE-inlined (to build the GNU C inlined dll with C++ exception handling)
+make clean GC-static (to build the GNU C inlined static lib with C cleanup code)
+make clean GC-debug (to build the GNU C debug dll with C cleanup code)
+make clean GCE-debug (to build the GNU C debug dll with C++ exception handling)
+make clean GC-inlined-debug (to build the GNU C inlined debug dll with C cleanup code)
+make clean GCE-inlined-debug (to build the GNU C inlined debug dll with C++ exception handling)
+make clean GC-static-debug (to build the GNU C inlined static debug lib with C cleanup code)
+
+
+The pre-built dlls are normally built using the *-inlined targets.
+
+You can run the testsuite by changing to the "tests" directory and
+running make for help information. E.g.:
+
+$ cd tests
+$ make
+Run one of the following command lines:
+make clean GC (to test using GC dll with C (no EH) applications)
+make clean GCX (to test using GC dll with C++ (EH) applications)
+make clean GCE (to test using GCE dll with C++ (EH) applications)
+make clean GC-bench (to benchtest using GNU C dll with C cleanup code)
+make clean GCE-bench (to benchtest using GNU C dll with C++ exception handling)
+make clean GC-static (to test using GC static lib with C (no EH) applications)
+
+
+Building under Linux using the Mingw32 cross development tools
+--------------------------------------------------------------
+
+You can build the library without leaving Linux by using the Mingw32 cross
+development toolchain. See http://www.libsdl.org/extras/win32/cross/ for
+tools and info. The GNUmakefile contains some support for this, for example:
+
+make CROSS=i386-mingw32msvc- clean GC-inlined
+
+will build pthreadGCn.dll and libpthreadGCn.a (n=version#), provided your
+cross-tools/bin directory is in your PATH (or use the cross-make.sh script
+at the URL above).
+
+
+Building the library as a statically linkable library
+-----------------------------------------------------
+
+General: PTW32_STATIC_LIB must be defined for both the library build and the
+application build. The makefiles supplied and used by the following 'make'
+command lines will define this for you.
+
+MSVC (creates pthreadVCn.lib as a static link lib):
+
+nmake clean VC-static
+
+
+MinGW32 (creates libpthreadGCn.a as a static link lib):
+
+make clean GC-static
+
+
+Define PTW32_STATIC_LIB when building your application. Also, your
+application must call a two non-portable routines to initialise the
+some state on startup and cleanup before exit. One other routine needs
+to be called to cleanup after any Win32 threads have called POSIX API
+routines. See README.NONPORTABLE or the html reference manual pages for
+details on these routines:
+
+BOOL pthread_win32_process_attach_np (void);
+BOOL pthread_win32_process_detach_np (void);
+BOOL pthread_win32_thread_attach_np (void); // Currently a no-op
+BOOL pthread_win32_thread_detach_np (void);
+
+
+The tests makefiles have the same targets but only check that the
+static library is statically linkable. They don't run the full
+testsuite. To run the full testsuite, build the dlls and run the
+dll test targets.
+
+
+Building the library under Cygwin
+---------------------------------
+
+Cygwin is implementing it's own POSIX threads routines and these
+will be the ones to use if you develop using Cygwin.
+
+
+Ready to run binaries
+---------------------
+
+For convenience, the following ready-to-run files can be downloaded
+from the FTP site (see under "Availability" below):
+
+ pthread.h
+ semaphore.h
+ sched.h
+ pthreadVC.dll - built with MSVC compiler using C setjmp/longjmp
+ pthreadVC.lib
+ pthreadVCE.dll - built with MSVC++ compiler using C++ EH
+ pthreadVCE.lib
+ pthreadVSE.dll - built with MSVC compiler using SEH
+ pthreadVSE.lib
+ pthreadGC.dll - built with Mingw32 GCC
+ libpthreadGC.a - derived from pthreadGC.dll
+ pthreadGCE.dll - built with Mingw32 G++
+ libpthreadGCE.a - derived from pthreadGCE.dll
+
+As of August 2003 pthreads-win32 pthreadG* versions are built and tested
+using the MinGW + MsysDTK environment current as of that date or later.
+The following file MAY be needed for older MinGW environments.
+
+ gcc.dll - needed to build and run applications that use
+ pthreadGCE.dll.
+
+
+Building applications with GNU compilers
+----------------------------------------
+
+If you're using pthreadGC.dll:
+
+With the three header files, pthreadGC.dll and libpthreadGC.a in the
+same directory as your application myapp.c, you could compile, link
+and run myapp.c under Mingw32 as follows:
+
+ gcc -o myapp.exe myapp.c -I. -L. -lpthreadGC
+ myapp
+
+Or put pthreadGC.dll in an appropriate directory in your PATH,
+put libpthreadGC.a in your system lib directory, and
+put the three header files in your system include directory,
+then use:
+
+ gcc -o myapp.exe myapp.c -lpthreadGC
+ myapp
+
+
+If you're using pthreadGCE.dll:
+
+With the three header files, pthreadGCE.dll, gcc.dll and libpthreadGCE.a
+in the same directory as your application myapp.c, you could compile,
+link and run myapp.c under Mingw32 as follows:
+
+ gcc -x c++ -o myapp.exe myapp.c -I. -L. -lpthreadGCE
+ myapp
+
+Or put pthreadGCE.dll and gcc.dll in an appropriate directory in
+your PATH, put libpthreadGCE.a in your system lib directory, and
+put the three header files in your system include directory,
+then use:
+
+ gcc -x c++ -o myapp.exe myapp.c -lpthreadGCE
+ myapp
+
+
+Availability
+------------
+
+The complete source code in either unbundled, self-extracting
+Zip file, or tar/gzipped format can be found at:
+
+ ftp://sources.redhat.com/pub/pthreads-win32
+
+The pre-built DLL, export libraries and matching pthread.h can
+be found at:
+
+ ftp://sources.redhat.com/pub/pthreads-win32/dll-latest
+
+Home page:
+
+ http://sources.redhat.com/pthreads-win32/
+
+
+Mailing list
+------------
+
+There is a mailing list for discussing pthreads on Win32.
+To join, send email to:
+
+ pthreads-win32-subscribe at sources.redhat.com
+
+Unsubscribe by sending mail to:
+
+ pthreads-win32-unsubscribe at sources.redhat.com
+
+
+Acknowledgements
+----------------
+
+See the ANNOUNCE file for acknowledgements.
+See the 'CONTRIBUTORS' file for the list of contributors.
+
+As much as possible, the ChangeLog file attributes
+contributions and patches that have been incorporated
+in the library to the individuals responsible.
+
+Finally, thanks to all those who work on and contribute to the
+POSIX and Single Unix Specification standards. The maturity of an
+industry can be measured by it's open standards.
+
+----
+Ross Johnson
+<rpj at callisto.canberra.edu.au>
+
+
+
+
+
+
+
+
diff --git a/3rdparty/pthreads-win32/README.Borland b/3rdparty/pthreads-win32/README.Borland
new file mode 100644
index 0000000..a130d2b
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.Borland
@@ -0,0 +1,57 @@
+In ptw32_InterlockedCompareExchange.c, I've added a section for
+Borland's compiler; it's identical to that for the MS compiler except
+that it uses /* ... */ comments instead of ; comments.
+
+[RPJ: need to define HAVE_TASM32 in config.h to use the above.]
+
+
+The other file is a makefile suitable for use with Borland's compiler
+(run "make -fBmakefile" in the directory). It builds a single version
+of the library, pthreadBC.dll and the corresponding pthreadBC.lib
+import library, which is comparable to the pthreadVC version; I can't
+personally see any demand for the versions that include structured or
+C++ exception cancellation handling so I haven't attempted to build
+those versions of the library. (I imagine a static version might be
+of use to some, but we can't legally use that on my commercial
+projects so I can't try that out, unfortunately.)
+
+[RPJ: Added tests\Bmakefile as well.]
+
+Borland C++ doesn't define the ENOSYS constant used by pthreads-win32;
+rather than make more extensive patches to the pthreads-win32 source I
+have a mostly-arbitrary constant for it in the makefile. However this
+doesn't make it visible to the application using the library, so if
+anyone actually wants to use this constant in their apps (why?)
+someone might like to make a seperate NEED_BCC_something define to add
+this stuff.
+
+The makefile also #defines EDEADLK as EDEADLOCK, _timeb as timeb, and
+_ftime as ftime, to deal with the minor differences between the two
+RTLs' naming conventions, and sets the compiler flags as required to
+get a normal compile of the library.
+
+[RPJ: Moved errno values and _timeb etc to pthread.h, so apps will also
+use them.]
+
+(While I'm on the subject, the reason Borland users should recompile
+the library, rather than using the impdef/implib technique suggested
+previously on the mailing list, is that a) the errno constants are
+different, so the results returned by the pthread_* functions can be
+meaningless, and b) the errno variable/pseudo-variable itself is
+different in the MS & BCC runtimes, so you can't access the
+pthreadVC's errno from a Borland C++-compiled host application
+correctly - I imagine there are other potential problems from the RTL
+mismatch too.)
+
+[RPJ: Make sure you use the same RTL in both dll and application builds.
+The dll and tests Bmakefiles use cw32mti.lib. Having some trouble with
+memory read exceptions running the test suite using BCC55.]
+
+Best regards,
+Will
+
+--
+Will Bryant
+Systems Architect, eCOSM Limited
+Cell +64 21 655 443, office +64 3 365 4176
+http://www.ecosm.com/
diff --git a/3rdparty/pthreads-win32/README.CV b/3rdparty/pthreads-win32/README.CV
new file mode 100644
index 0000000..698728b
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.CV
@@ -0,0 +1,3036 @@
+README.CV -- Condition Variables
+--------------------------------
+
+The original implementation of condition variables in
+pthreads-win32 was based on a discussion paper:
+
+"Strategies for Implementing POSIX Condition Variables
+on Win32": http://www.cs.wustl.edu/~schmidt/win32-cv-1.html
+
+The changes suggested below were made on Feb 6 2001. This
+file is included in the package for the benefit of anyone
+interested in understanding the pthreads-win32 implementation
+of condition variables and the (sometimes subtle) issues that
+it attempts to resolve.
+
+Thanks go to the individuals whose names appear throughout
+the following text.
+
+Ross Johnson
+
+--------------------------------------------------------------------
+
+fyi.. (more detailed problem description/demos + possible fix/patch)
+
+regards,
+alexander.
+
+
+Alexander Terekhov
+31.01.2001 17:43
+
+To: ace-bugs at cs.wustl.edu
+cc:
+From: Alexander Terekhov/Germany/IBM at IBMDE
+Subject: Implementation of POSIX CVs: spur.wakeups/lost
+ signals/deadlocks/unfairness
+
+
+
+ ACE VERSION:
+
+ 5.1.12 (pthread-win32 snapshot 2000-12-29)
+
+ HOST MACHINE and OPERATING SYSTEM:
+
+ IBM IntelliStation Z Pro, 2 x XEON 1GHz, Win2K
+
+ TARGET MACHINE and OPERATING SYSTEM, if different from HOST:
+ COMPILER NAME AND VERSION (AND PATCHLEVEL):
+
+ Microsoft Visual C++ 6.0
+
+ AREA/CLASS/EXAMPLE AFFECTED:
+
+ Implementation of POSIX condition variables - OS.cpp/.h
+
+ DOES THE PROBLEM AFFECT:
+
+ EXECUTION? YES!
+
+ SYNOPSIS:
+
+ a) spurious wakeups (minor problem)
+ b) lost signals
+ c) broadcast deadlock
+ d) unfairness (minor problem)
+
+ DESCRIPTION:
+
+ Please see attached copy of discussion thread
+ from comp.programming.threads for more details on
+ some reported problems. (i've also posted a "fyi"
+ message to ace-users a week or two ago but
+ unfortunately did not get any response so far).
+
+ It seems that current implementation suffers from
+ two essential problems:
+
+ 1) cond.waiters_count does not accurately reflect
+ number of waiters blocked on semaphore - w/o
+ proper synchronisation that could result (in the
+ time window when counter is not accurate)
+ in spurious wakeups organised by subsequent
+ _signals and _broadcasts.
+
+ 2) Always having (with no e.g. copy_and_clear/..)
+ the same queue in use (semaphore+counter)
+ neither signal nor broadcast provide 'atomic'
+ behaviour with respect to other threads/subsequent
+ calls to signal/broadcast/wait.
+
+ Each problem and combination of both could produce
+ various nasty things:
+
+ a) spurious wakeups (minor problem)
+
+ it is possible that waiter(s) which was already
+ unblocked even so is still counted as blocked
+ waiter. signal and broadcast will release
+ semaphore which will produce a spurious wakeup
+ for a 'real' waiter coming later.
+
+ b) lost signals
+
+ signalling thread ends up consuming its own
+ signal. please see demo/discussion below.
+
+ c) broadcast deadlock
+
+ last_waiter processing code does not correctly
+ handle the case with multiple threads
+ waiting for the end of broadcast.
+ please see demo/discussion below.
+
+ d) unfairness (minor problem)
+
+ without SignalObjectAndWait some waiter(s)
+ may end up consuming broadcasted signals
+ multiple times (spurious wakeups) because waiter
+ thread(s) can be preempted before they call
+ semaphore wait (but after count++ and mtx.unlock).
+
+ REPEAT BY:
+
+ See below... run problem demos programs (tennis.cpp and
+ tennisb.cpp) number of times concurrently (on multiprocessor)
+ and in multiple sessions or just add a couple of "Sleep"s
+ as described in the attached copy of discussion thread
+ from comp.programming.threads
+
+ SAMPLE FIX/WORKAROUND:
+
+ See attached patch to pthread-win32.. well, I can not
+ claim that it is completely bug free but at least my
+ test and tests provided by pthreads-win32 seem to work.
+ Perhaps that will help.
+
+ regards,
+ alexander.
+
+
+>> Forum: comp.programming.threads
+>> Thread: pthread_cond_* implementation questions
+.
+.
+.
+David Schwartz <davids at webmaster.com> wrote:
+
+> terekhov at my-deja.com wrote:
+>
+>> BTW, could you please also share your view on other perceived
+>> "problems" such as nested broadcast deadlock, spurious wakeups
+>> and (the latest one) lost signals??
+>
+>I'm not sure what you mean. The standard allows an implementation
+>to do almost whatever it likes. In fact, you could implement
+>pthread_cond_wait by releasing the mutex, sleeping a random
+>amount of time, and then reacquiring the mutex. Of course,
+>this would be a pretty poor implementation, but any code that
+>didn't work under that implementation wouldn't be strictly
+>compliant.
+
+The implementation you suggested is indeed correct
+one (yes, now I see it :). However it requires from
+signal/broadcast nothing more than to "{ return 0; }"
+That is not the case for pthread-win32 and ACE
+implementations. I do think that these implementations
+(basically the same implementation) have some serious
+problems with wait/signal/broadcast calls. I am looking
+for help to clarify whether these problems are real
+or not. I think that I can demonstrate what I mean
+using one or two small sample programs.
+.
+.
+.
+==========
+tennis.cpp
+==========
+
+#include "ace/Synch.h"
+#include "ace/Thread.h"
+
+enum GAME_STATE {
+
+ START_GAME,
+ PLAYER_A, // Player A playes the ball
+ PLAYER_B, // Player B playes the ball
+ GAME_OVER,
+ ONE_PLAYER_GONE,
+ BOTH_PLAYERS_GONE
+
+};
+
+enum GAME_STATE eGameState;
+ACE_Mutex* pmtxGameStateLock;
+ACE_Condition< ACE_Mutex >* pcndGameStateChange;
+
+void*
+ playerA(
+ void* pParm
+ )
+{
+
+ // For access to game state variable
+ pmtxGameStateLock->acquire();
+
+ // Play loop
+ while ( eGameState < GAME_OVER ) {
+
+ // Play the ball
+ cout << endl << "PLAYER-A" << endl;
+
+ // Now its PLAYER-B's turn
+ eGameState = PLAYER_B;
+
+ // Signal to PLAYER-B that now it is his turn
+ pcndGameStateChange->signal();
+
+ // Wait until PLAYER-B finishes playing the ball
+ do {
+
+ pcndGameStateChange->wait();
+
+ if ( PLAYER_B == eGameState )
+ cout << endl << "----PLAYER-A: SPURIOUS WAKEUP!!!" << endl;
+
+ } while ( PLAYER_B == eGameState );
+
+ }
+
+ // PLAYER-A gone
+ eGameState = (GAME_STATE)(eGameState+1);
+ cout << endl << "PLAYER-A GONE" << endl;
+
+ // No more access to state variable needed
+ pmtxGameStateLock->release();
+
+ // Signal PLAYER-A gone event
+ pcndGameStateChange->broadcast();
+
+ return 0;
+
+}
+
+void*
+ playerB(
+ void* pParm
+ )
+{
+
+ // For access to game state variable
+ pmtxGameStateLock->acquire();
+
+ // Play loop
+ while ( eGameState < GAME_OVER ) {
+
+ // Play the ball
+ cout << endl << "PLAYER-B" << endl;
+
+ // Now its PLAYER-A's turn
+ eGameState = PLAYER_A;
+
+ // Signal to PLAYER-A that now it is his turn
+ pcndGameStateChange->signal();
+
+ // Wait until PLAYER-A finishes playing the ball
+ do {
+
+ pcndGameStateChange->wait();
+
+ if ( PLAYER_A == eGameState )
+ cout << endl << "----PLAYER-B: SPURIOUS WAKEUP!!!" << endl;
+
+ } while ( PLAYER_A == eGameState );
+
+ }
+
+ // PLAYER-B gone
+ eGameState = (GAME_STATE)(eGameState+1);
+ cout << endl << "PLAYER-B GONE" << endl;
+
+ // No more access to state variable needed
+ pmtxGameStateLock->release();
+
+ // Signal PLAYER-B gone event
+ pcndGameStateChange->broadcast();
+
+ return 0;
+
+}
+
+
+int
+main (int, ACE_TCHAR *[])
+{
+
+ pmtxGameStateLock = new ACE_Mutex();
+ pcndGameStateChange = new ACE_Condition< ACE_Mutex >( *pmtxGameStateLock
+);
+
+ // Set initial state
+ eGameState = START_GAME;
+
+ // Create players
+ ACE_Thread::spawn( playerA );
+ ACE_Thread::spawn( playerB );
+
+ // Give them 5 sec. to play
+ Sleep( 5000 );//sleep( 5 );
+
+ // Set game over state
+ pmtxGameStateLock->acquire();
+ eGameState = GAME_OVER;
+
+ // Let them know
+ pcndGameStateChange->broadcast();
+
+ // Wait for players to stop
+ do {
+
+ pcndGameStateChange->wait();
+
+ } while ( eGameState < BOTH_PLAYERS_GONE );
+
+ // Cleanup
+ cout << endl << "GAME OVER" << endl;
+ pmtxGameStateLock->release();
+ delete pcndGameStateChange;
+ delete pmtxGameStateLock;
+
+ return 0;
+
+}
+
+===========
+tennisb.cpp
+===========
+#include "ace/Synch.h"
+#include "ace/Thread.h"
+
+enum GAME_STATE {
+
+ START_GAME,
+ PLAYER_A, // Player A playes the ball
+ PLAYER_B, // Player B playes the ball
+ GAME_OVER,
+ ONE_PLAYER_GONE,
+ BOTH_PLAYERS_GONE
+
+};
+
+enum GAME_STATE eGameState;
+ACE_Mutex* pmtxGameStateLock;
+ACE_Condition< ACE_Mutex >* pcndGameStateChange;
+
+void*
+ playerA(
+ void* pParm
+ )
+{
+
+ // For access to game state variable
+ pmtxGameStateLock->acquire();
+
+ // Play loop
+ while ( eGameState < GAME_OVER ) {
+
+ // Play the ball
+ cout << endl << "PLAYER-A" << endl;
+
+ // Now its PLAYER-B's turn
+ eGameState = PLAYER_B;
+
+ // Signal to PLAYER-B that now it is his turn
+ pcndGameStateChange->broadcast();
+
+ // Wait until PLAYER-B finishes playing the ball
+ do {
+
+ pcndGameStateChange->wait();
+
+ if ( PLAYER_B == eGameState )
+ cout << endl << "----PLAYER-A: SPURIOUS WAKEUP!!!" << endl;
+
+ } while ( PLAYER_B == eGameState );
+
+ }
+
+ // PLAYER-A gone
+ eGameState = (GAME_STATE)(eGameState+1);
+ cout << endl << "PLAYER-A GONE" << endl;
+
+ // No more access to state variable needed
+ pmtxGameStateLock->release();
+
+ // Signal PLAYER-A gone event
+ pcndGameStateChange->broadcast();
+
+ return 0;
+
+}
+
+void*
+ playerB(
+ void* pParm
+ )
+{
+
+ // For access to game state variable
+ pmtxGameStateLock->acquire();
+
+ // Play loop
+ while ( eGameState < GAME_OVER ) {
+
+ // Play the ball
+ cout << endl << "PLAYER-B" << endl;
+
+ // Now its PLAYER-A's turn
+ eGameState = PLAYER_A;
+
+ // Signal to PLAYER-A that now it is his turn
+ pcndGameStateChange->broadcast();
+
+ // Wait until PLAYER-A finishes playing the ball
+ do {
+
+ pcndGameStateChange->wait();
+
+ if ( PLAYER_A == eGameState )
+ cout << endl << "----PLAYER-B: SPURIOUS WAKEUP!!!" << endl;
+
+ } while ( PLAYER_A == eGameState );
+
+ }
+
+ // PLAYER-B gone
+ eGameState = (GAME_STATE)(eGameState+1);
+ cout << endl << "PLAYER-B GONE" << endl;
+
+ // No more access to state variable needed
+ pmtxGameStateLock->release();
+
+ // Signal PLAYER-B gone event
+ pcndGameStateChange->broadcast();
+
+ return 0;
+
+}
+
+
+int
+main (int, ACE_TCHAR *[])
+{
+
+ pmtxGameStateLock = new ACE_Mutex();
+ pcndGameStateChange = new ACE_Condition< ACE_Mutex >( *pmtxGameStateLock
+);
+
+ // Set initial state
+ eGameState = START_GAME;
+
+ // Create players
+ ACE_Thread::spawn( playerA );
+ ACE_Thread::spawn( playerB );
+
+ // Give them 5 sec. to play
+ Sleep( 5000 );//sleep( 5 );
+
+ // Make some noise
+ pmtxGameStateLock->acquire();
+ cout << endl << "---Noise ON..." << endl;
+ pmtxGameStateLock->release();
+ for ( int i = 0; i < 100000; i++ )
+ pcndGameStateChange->broadcast();
+ cout << endl << "---Noise OFF" << endl;
+
+ // Set game over state
+ pmtxGameStateLock->acquire();
+ eGameState = GAME_OVER;
+ cout << endl << "---Stopping the game..." << endl;
+
+ // Let them know
+ pcndGameStateChange->broadcast();
+
+ // Wait for players to stop
+ do {
+
+ pcndGameStateChange->wait();
+
+ } while ( eGameState < BOTH_PLAYERS_GONE );
+
+ // Cleanup
+ cout << endl << "GAME OVER" << endl;
+ pmtxGameStateLock->release();
+ delete pcndGameStateChange;
+ delete pmtxGameStateLock;
+
+ return 0;
+
+}
+.
+.
+.
+David Schwartz <davids at webmaster.com> wrote:
+>> > It's compliant
+>>
+>> That is really good.
+>
+>> Tomorrow (I have to go urgently now) I will try to
+>> demonstrate the lost-signal "problem" of current
+>> pthread-win32 and ACE-(variant w/o SingleObjectAndWait)
+>> implementations: players start suddenly drop their balls :-)
+>> (with no change in source code).
+>
+>Signals aren't lost, they're going to the main thread,
+>which isn't coded correctly to handle them. Try this:
+>
+> // Wait for players to stop
+> do {
+>
+> pthread_cond_wait( &cndGameStateChange,&mtxGameStateLock );
+>printf("Main thread stole a signal\n");
+>
+> } while ( eGameState < BOTH_PLAYERS_GONE );
+>
+>I bet everytime you thing a signal is lost, you'll see that printf.
+>The signal isn't lost, it was stolen by another thread.
+
+well, you can probably loose your bet.. it was indeed stolen
+by "another" thread but not the one you seem to think of.
+
+I think that what actually happens is the following:
+
+H:\SA\UXX\pt\PTHREADS\TESTS>tennis3.exe
+
+PLAYER-A
+
+PLAYER-B
+
+----PLAYER-B: SPURIOUS WAKEUP!!!
+
+PLAYER-A GONE
+
+PLAYER-B GONE
+
+GAME OVER
+
+H:\SA\UXX\pt\PTHREADS\TESTS>
+
+here you can see that PLAYER-B after playing his first
+ball (which came via signal from PLAYER-A) just dropped
+it down. What happened is that his signal to player A
+was consumed as spurious wakeup by himself (player B).
+
+The implementation has a problem:
+
+================
+waiting threads:
+================
+
+{ /** Critical Section
+
+ inc cond.waiters_count
+
+}
+
+ /*
+ /* Atomic only if using Win32 SignalObjectAndWait
+ /*
+ cond.mtx.release
+
+ /*** ^^-- A THREAD WHICH DID SIGNAL MAY ACQUIRE THE MUTEX,
+ /*** GO INTO WAIT ON THE SAME CONDITION AND OVERTAKE
+ /*** ORIGINAL WAITER(S) CONSUMING ITS OWN SIGNAL!
+
+ cond.sem.wait
+
+Player-A after playing game's initial ball went into
+wait (called _wait) but was pre-empted before reaching
+wait semaphore. He was counted as waiter but was not
+actually waiting/blocked yet.
+
+===============
+signal threads:
+===============
+
+{ /** Critical Section
+
+ waiters_count = cond.waiters_count
+
+}
+
+ if ( waiters_count != 0 )
+
+ sem.post 1
+
+ endif
+
+Player-B after he received signal/ball from Player A
+called _signal. The _signal did see that there was
+one waiter blocked on the condition (Player-A) and
+released the semaphore.. (but it did not unblock
+Player-A because he was not actually blocked).
+Player-B thread continued its execution, called _wait,
+was counted as second waiter BUT was allowed to slip
+through opened semaphore gate (which was opened for
+Player-B) and received his own signal. Player B remained
+blocked followed by Player A. Deadlock happened which
+lasted until main thread came in and said game over.
+
+It seems to me that the implementation fails to
+correctly implement the following statement
+from specification:
+
+http://www.opengroup.org/
+onlinepubs/007908799/xsh/pthread_cond_wait.html
+
+"These functions atomically release mutex and cause
+the calling thread to block on the condition variable
+cond; atomically here means "atomically with respect
+to access by another thread to the mutex and then the
+condition variable". That is, if another thread is
+able to acquire the mutex after the about-to-block
+thread has released it, then a subsequent call to
+pthread_cond_signal() or pthread_cond_broadcast()
+in that thread behaves as if it were issued after
+the about-to-block thread has blocked."
+
+Question: Am I right?
+
+(I produced the program output above by simply
+adding ?Sleep( 1 )?:
+
+================
+waiting threads:
+================
+
+{ /** Critical Section
+
+ inc cond.waiters_count
+
+}
+
+ /*
+ /* Atomic only if using Win32 SignalObjectAndWait
+ /*
+ cond.mtx.release
+
+Sleep( 1 ); // Win32
+
+ /*** ^^-- A THREAD WHICH DID SIGNAL MAY ACQUIRE THE MUTEX,
+ /*** GO INTO WAIT ON THE SAME CONDITION AND OVERTAKE
+ /*** ORIGINAL WAITER(S) CONSUMING ITS OWN SIGNAL!
+
+ cond.sem.wait
+
+to the source code of pthread-win32 implementation:
+
+http://sources.redhat.com/cgi-bin/cvsweb.cgi/pthreads/
+condvar.c?rev=1.36&content-type=text/
+x-cvsweb-markup&cvsroot=pthreads-win32
+
+
+ /*
+ * We keep the lock held just long enough to increment the count of
+ * waiters by one (above).
+ * Note that we can't keep it held across the
+ * call to sem_wait since that will deadlock other calls
+ * to pthread_cond_signal
+ */
+ cleanup_args.mutexPtr = mutex;
+ cleanup_args.cv = cv;
+ cleanup_args.resultPtr = &result;
+
+ pthread_cleanup_push (ptw32_cond_wait_cleanup, (void *)
+&cleanup_args);
+
+ if ((result = pthread_mutex_unlock (mutex)) == 0)
+ {((result
+Sleep( 1 ); // @AT
+
+ /*
+ * Wait to be awakened by
+ * pthread_cond_signal, or
+ * pthread_cond_broadcast, or
+ * a timeout
+ *
+ * Note:
+ * ptw32_sem_timedwait is a cancelation point,
+ * hence providing the
+ * mechanism for making pthread_cond_wait a cancelation
+ * point. We use the cleanup mechanism to ensure we
+ * re-lock the mutex and decrement the waiters count
+ * if we are canceled.
+ */
+ if (ptw32_sem_timedwait (&(cv->sema), abstime) == -1) {
+ result = errno;
+ }
+ }
+
+ pthread_cleanup_pop (1); /* Always cleanup */
+
+
+BTW, on my system (2 CPUs) I can manage to get
+signals lost even without any source code modification
+if I run the tennis program many times in different
+shell sessions.
+.
+.
+.
+David Schwartz <davids at webmaster.com> wrote:
+>terekhov at my-deja.com wrote:
+>
+>> well, it might be that the program is in fact buggy.
+>> but you did not show me any bug.
+>
+>You're right. I was close but not dead on. I was correct, however,
+>that the code is buggy because it uses 'pthread_cond_signal' even
+>though not any thread waiting on the condition variable can do the
+>job. I was wrong in which thread could be waiting on the cv but
+>unable to do the job.
+
+Okay, lets change 'pthread_cond_signal' to 'pthread_cond_broadcast'
+but also add some noise from main() right before declaring the game
+to be over (I need it in order to demonstrate another problem of
+pthread-win32/ACE implementations - broadcast deadlock)...
+.
+.
+.
+It is my understanding of POSIX conditions,
+that on correct implementation added noise
+in form of unnecessary broadcasts from main,
+should not break the tennis program. The
+only 'side effect' of added noise on correct
+implementation would be 'spurious wakeups' of
+players (in fact they are not spurious,
+players just see them as spurious) unblocked,
+not by another player but by main before
+another player had a chance to acquire the
+mutex and change the game state variable:
+.
+.
+.
+
+PLAYER-B
+
+PLAYER-A
+
+---Noise ON...
+
+PLAYER-B
+
+PLAYER-A
+
+.
+.
+.
+
+PLAYER-B
+
+PLAYER-A
+
+----PLAYER-A: SPURIOUS WAKEUP!!!
+
+PLAYER-B
+
+PLAYER-A
+
+---Noise OFF
+
+PLAYER-B
+
+---Stopping the game...
+
+PLAYER-A GONE
+
+PLAYER-B GONE
+
+GAME OVER
+
+H:\SA\UXX\pt\PTHREADS\TESTS>
+
+On pthread-win32/ACE implementations the
+program could stall:
+
+.
+.
+.
+
+PLAYER-A
+
+PLAYER-B
+
+PLAYER-A
+
+PLAYER-B
+
+PLAYER-A
+
+PLAYER-B
+
+PLAYER-A
+
+PLAYER-B
+
+---Noise ON...
+
+PLAYER-A
+
+---Noise OFF
+^C
+H:\SA\UXX\pt\PTHREADS\TESTS>
+
+
+The implementation has problems:
+
+================
+waiting threads:
+================
+
+{ /** Critical Section
+
+ inc cond.waiters_count
+
+}
+
+ /*
+ /* Atomic only if using Win32 SignalObjectAndWait
+ /*
+ cond.mtx.release
+ cond.sem.wait
+
+ /*** ^^-- WAITER CAN BE PREEMPTED AFTER BEING UNBLOCKED...
+
+{ /** Critical Section
+
+ dec cond.waiters_count
+
+ /*** ^^- ...AND BEFORE DECREMENTING THE COUNT (1)
+
+ last_waiter = ( cond.was_broadcast &&
+ cond.waiters_count == 0 )
+
+ if ( last_waiter )
+
+ cond.was_broadcast = FALSE
+
+ endif
+
+}
+
+ if ( last_waiter )
+
+ /*
+ /* Atomic only if using Win32 SignalObjectAndWait
+ /*
+ cond.auto_reset_event_or_sem.post /* Event for Win32
+ cond.mtx.acquire
+
+ /*** ^^-- ...AND BEFORE CALL TO mtx.acquire (2)
+
+ /*** ^^-- NESTED BROADCASTS RESULT IN A DEADLOCK
+
+
+ else
+
+ cond.mtx.acquire
+
+ /*** ^^-- ...AND BEFORE CALL TO mtx.acquire (3)
+
+ endif
+
+
+==================
+broadcast threads:
+==================
+
+{ /** Critical Section
+
+ waiters_count = cond.waiters_count
+
+ if ( waiters_count != 0 )
+
+ cond.was_broadcast = TRUE
+
+ endif
+
+}
+
+if ( waiters_count != 0 )
+
+ cond.sem.post waiters_count
+
+ /*** ^^^^^--- SPURIOUS WAKEUPS DUE TO (1)
+
+ cond.auto_reset_event_or_sem.wait /* Event for Win32
+
+ /*** ^^^^^--- DEADLOCK FOR FURTHER BROADCASTS IF THEY
+ HAPPEN TO GO INTO WAIT WHILE PREVIOUS
+ BROADCAST IS STILL IN PROGRESS/WAITING
+
+endif
+
+a) cond.waiters_count does not accurately reflect
+number of waiters blocked on semaphore - that could
+result (in the time window when counter is not accurate)
+in spurios wakeups organised by subsequent _signals
+and _broadcasts. From standard compliance point of view
+that is OK but that could be a real problem from
+performance/efficiency point of view.
+
+b) If subsequent broadcast happen to go into wait on
+cond.auto_reset_event_or_sem before previous
+broadcast was unblocked from cond.auto_reset_event_or_sem
+by its last waiter, one of two blocked threads will
+remain blocked because last_waiter processing code
+fails to unblock both threads.
+
+In the situation with tennisb.c the Player-B was put
+in a deadlock by noise (broadcast) coming from main
+thread. And since Player-B holds the game state
+mutex when it calls broadcast, the whole program
+stalled: Player-A was deadlocked on mutex and
+main thread after finishing with producing the noise
+was deadlocked on mutex too (needed to declare the
+game over)
+
+(I produced the program output above by simply
+adding ?Sleep( 1 )?:
+
+==================
+broadcast threads:
+==================
+
+{ /** Critical Section
+
+ waiters_count = cond.waiters_count
+
+ if ( waiters_count != 0 )
+
+ cond.was_broadcast = TRUE
+
+ endif
+
+}
+
+if ( waiters_count != 0 )
+
+Sleep( 1 ); //Win32
+
+ cond.sem.post waiters_count
+
+ /*** ^^^^^--- SPURIOUS WAKEUPS DUE TO (1)
+
+ cond.auto_reset_event_or_sem.wait /* Event for Win32
+
+ /*** ^^^^^--- DEADLOCK FOR FURTHER BROADCASTS IF THEY
+ HAPPEN TO GO INTO WAIT WHILE PREVIOUS
+ BROADCAST IS STILL IN PROGRESS/WAITING
+
+endif
+
+to the source code of pthread-win32 implementation:
+
+http://sources.redhat.com/cgi-bin/cvsweb.cgi/pthreads/
+condvar.c?rev=1.36&content-type=text/
+x-cvsweb-markup&cvsroot=pthreads-win32
+
+ if (wereWaiters)
+ {(wereWaiters)sroot=pthreads-win32eb.cgi/pthreads/Yem...m
+ /*
+ * Wake up all waiters
+ */
+
+Sleep( 1 ); //@AT
+
+#ifdef NEED_SEM
+
+ result = (ptw32_increase_semaphore( &cv->sema, cv->waiters )
+ ? 0
+ : EINVAL);
+
+#else /* NEED_SEM */
+
+ result = (ReleaseSemaphore( cv->sema, cv->waiters, NULL )
+ ? 0
+ : EINVAL);
+
+#endif /* NEED_SEM */
+
+ }
+
+ (void) pthread_mutex_unlock(&(cv->waitersLock));
+
+ if (wereWaiters && result == 0)
+ {(wereWaiters
+ /*
+ * Wait for all the awakened threads to acquire their part of
+ * the counting semaphore
+ */
+
+ if (WaitForSingleObject (cv->waitersDone, INFINITE)
+ == WAIT_OBJECT_0)
+ {
+ result = 0;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ }
+
+ return (result);
+
+}
+
+BTW, on my system (2 CPUs) I can manage to get
+the program stalled even without any source code
+modification if I run the tennisb program many
+times in different shell sessions.
+
+===================
+pthread-win32 patch
+===================
+struct pthread_cond_t_ {
+ long nWaitersBlocked; /* Number of threads blocked
+*/
+ long nWaitersUnblocked; /* Number of threads unblocked
+*/
+ long nWaitersToUnblock; /* Number of threads to unblock
+*/
+ sem_t semBlockQueue; /* Queue up threads waiting for the
+*/
+ /* condition to become signalled
+*/
+ sem_t semBlockLock; /* Semaphore that guards access to
+*/
+ /* | waiters blocked count/block queue
+*/
+ /* +-> Mandatory Sync.LEVEL-1
+*/
+ pthread_mutex_t mtxUnblockLock; /* Mutex that guards access to
+*/
+ /* | waiters (to)unblock(ed) counts
+*/
+ /* +-> Optional* Sync.LEVEL-2
+*/
+}; /* Opt*) for _timedwait and
+cancellation*/
+
+int
+pthread_cond_init (pthread_cond_t * cond, const pthread_condattr_t * attr)
+ int result = EAGAIN;
+ pthread_cond_t cv = NULL;
+
+ if (cond == NULL)
+ {(cond
+ return EINVAL;
+ }
+
+ if ((attr != NULL && *attr != NULL) &&
+ ((*attr)->pshared == PTHREAD_PROCESS_SHARED))
+ {
+ /*
+ * Creating condition variable that can be shared between
+ * processes.
+ */
+ result = ENOSYS;
+
+ goto FAIL0;
+ }
+
+ cv = (pthread_cond_t) calloc (1, sizeof (*cv));
+
+ if (cv == NULL)
+ {(cv
+ result = ENOMEM;
+ goto FAIL0;
+ }
+
+ cv->nWaitersBlocked = 0;
+ cv->nWaitersUnblocked = 0;
+ cv->nWaitersToUnblock = 0;
+
+ if (sem_init (&(cv->semBlockLock), 0, 1) != 0)
+ {(sem_init
+ goto FAIL0;
+ }
+
+ if (sem_init (&(cv->semBlockQueue), 0, 0) != 0)
+ {(sem_init
+ goto FAIL1;
+ }
+
+ if (pthread_mutex_init (&(cv->mtxUnblockLock), 0) != 0)
+ {(pthread_mutex_init
+ goto FAIL2;
+ }
+
+
+ result = 0;
+
+ goto DONE;
+
+ /*
+ * -------------
+ * Failed...
+ * -------------
+ */
+FAIL2:
+ (void) sem_destroy (&(cv->semBlockQueue));
+
+FAIL1:
+ (void) sem_destroy (&(cv->semBlockLock));
+
+FAIL0:
+DONE:
+ *cond = cv;
+
+ return (result);
+
+} /* pthread_cond_init */
+
+int
+pthread_cond_destroy (pthread_cond_t * cond)
+{
+ int result = 0;
+ pthread_cond_t cv;
+
+ /*
+ * Assuming any race condition here is harmless.
+ */
+ if (cond == NULL
+ || *cond == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (*cond != (pthread_cond_t) PTW32_OBJECT_AUTO_INIT)
+ {(*cond
+ cv = *cond;
+
+ /*
+ * Synchronize access to waiters blocked count (LEVEL-1)
+ */
+ if (sem_wait(&(cv->semBlockLock)) != 0)
+ {(sem_wait(&(cv->semBlockLock))
+ return errno;
+ }
+
+ /*
+ * Synchronize access to waiters (to)unblock(ed) counts (LEVEL-2)
+ */
+ if ((result = pthread_mutex_lock(&(cv->mtxUnblockLock))) != 0)
+ {((result
+ (void) sem_post(&(cv->semBlockLock));
+ return result;
+ }
+
+ /*
+ * Check whether cv is still busy (still has waiters blocked)
+ */
+ if (cv->nWaitersBlocked - cv->nWaitersUnblocked > 0)
+ {(cv->nWaitersBlocked
+ (void) sem_post(&(cv->semBlockLock));
+ (void) pthread_mutex_unlock(&(cv->mtxUnblockLock));
+ return EBUSY;
+ }
+
+ /*
+ * Now it is safe to destroy
+ */
+ (void) sem_destroy (&(cv->semBlockLock));
+ (void) sem_destroy (&(cv->semBlockQueue));
+ (void) pthread_mutex_unlock (&(cv->mtxUnblockLock));
+ (void) pthread_mutex_destroy (&(cv->mtxUnblockLock));
+
+ free(cv);
+ *cond = NULL;
+ }
+ else
+ {
+ /*
+ * See notes in ptw32_cond_check_need_init() above also.
+ */
+ EnterCriticalSection(&ptw32_cond_test_init_lock);
+
+ /*
+ * Check again.
+ */
+ if (*cond == (pthread_cond_t) PTW32_OBJECT_AUTO_INIT)
+ {(*cond
+ /*
+ * This is all we need to do to destroy a statically
+ * initialised cond that has not yet been used (initialised).
+ * If we get to here, another thread
+ * waiting to initialise this cond will get an EINVAL.
+ */
+ *cond = NULL;
+ }
+ else
+ {
+ /*
+ * The cv has been initialised while we were waiting
+ * so assume it's in use.
+ */
+ result = EBUSY;
+ }
+
+ LeaveCriticalSection(&ptw32_cond_test_init_lock);
+ }
+
+ return (result);
+}
+
+/*
+ * Arguments for cond_wait_cleanup, since we can only pass a
+ * single void * to it.
+ */
+typedef struct {
+ pthread_mutex_t * mutexPtr;
+ pthread_cond_t cv;
+ int * resultPtr;
+} ptw32_cond_wait_cleanup_args_t;
+
+static void
+ptw32_cond_wait_cleanup(void * args)
+{
+ ptw32_cond_wait_cleanup_args_t * cleanup_args =
+(ptw32_cond_wait_cleanup_args_t *) args;
+ pthread_cond_t cv = cleanup_args->cv;
+ int * resultPtr = cleanup_args->resultPtr;
+ int eLastSignal; /* enum: 1=yes 0=no -1=cancelled/timedout w/o signal(s)
+*/
+ int result;
+
+ /*
+ * Whether we got here as a result of signal/broadcast or because of
+ * timeout on wait or thread cancellation we indicate that we are no
+ * longer waiting. The waiter is responsible for adjusting waiters
+ * (to)unblock(ed) counts (protected by unblock lock).
+ * Unblock lock/Sync.LEVEL-2 supports _timedwait and cancellation.
+ */
+ if ((result = pthread_mutex_lock(&(cv->mtxUnblockLock))) != 0)
+ {((result
+ *resultPtr = result;
+ return;
+ }
+
+ cv->nWaitersUnblocked++;
+
+ eLastSignal = (cv->nWaitersToUnblock == 0) ?
+ -1 : (--cv->nWaitersToUnblock == 0);
+
+ /*
+ * No more LEVEL-2 access to waiters (to)unblock(ed) counts needed
+ */
+ if ((result = pthread_mutex_unlock(&(cv->mtxUnblockLock))) != 0)
+ {((result
+ *resultPtr = result;
+ return;
+ }
+
+ /*
+ * If last signal...
+ */
+ if (eLastSignal == 1)
+ {(eLastSignal
+ /*
+ * ...it means that we have end of 'atomic' signal/broadcast
+ */
+ if (sem_post(&(cv->semBlockLock)) != 0)
+ {(sem_post(&(cv->semBlockLock))
+ *resultPtr = errno;
+ return;
+ }
+ }
+ /*
+ * If not last signal and not timed out/cancelled wait w/o signal...
+ */
+ else if (eLastSignal == 0)
+ {
+ /*
+ * ...it means that next waiter can go through semaphore
+ */
+ if (sem_post(&(cv->semBlockQueue)) != 0)
+ {(sem_post(&(cv->semBlockQueue))
+ *resultPtr = errno;
+ return;
+ }
+ }
+
+ /*
+ * XSH: Upon successful return, the mutex has been locked and is owned
+ * by the calling thread
+ */
+ if ((result = pthread_mutex_lock(cleanup_args->mutexPtr)) != 0)
+ {((result
+ *resultPtr = result;
+ }
+
+} /* ptw32_cond_wait_cleanup */
+
+static int
+ptw32_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex,
+ const struct timespec *abstime)
+{
+ int result = 0;
+ pthread_cond_t cv;
+ ptw32_cond_wait_cleanup_args_t cleanup_args;
+
+ if (cond == NULL || *cond == NULL)
+ {(cond
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static condition variable. We check
+ * again inside the guarded section of ptw32_cond_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*cond == (pthread_cond_t) PTW32_OBJECT_AUTO_INIT)
+ {(*cond
+ result = ptw32_cond_check_need_init(cond);
+ }
+
+ if (result != 0 && result != EBUSY)
+ {(result
+ return result;
+ }
+
+ cv = *cond;
+
+ /*
+ * Synchronize access to waiters blocked count (LEVEL-1)
+ */
+ if (sem_wait(&(cv->semBlockLock)) != 0)
+ {(sem_wait(&(cv->semBlockLock))
+ return errno;
+ }
+
+ cv->nWaitersBlocked++;
+
+ /*
+ * Thats it. Counted means waiting, no more access needed
+ */
+ if (sem_post(&(cv->semBlockLock)) != 0)
+ {(sem_post(&(cv->semBlockLock))
+ return errno;
+ }
+
+ /*
+ * Setup this waiter cleanup handler
+ */
+ cleanup_args.mutexPtr = mutex;
+ cleanup_args.cv = cv;
+ cleanup_args.resultPtr = &result;
+
+ pthread_cleanup_push (ptw32_cond_wait_cleanup, (void *) &cleanup_args);
+
+ /*
+ * Now we can release 'mutex' and...
+ */
+ if ((result = pthread_mutex_unlock (mutex)) == 0)
+ {((result
+
+ /*
+ * ...wait to be awakened by
+ * pthread_cond_signal, or
+ * pthread_cond_broadcast, or
+ * timeout, or
+ * thread cancellation
+ *
+ * Note:
+ *
+ * ptw32_sem_timedwait is a cancellation point,
+ * hence providing the mechanism for making
+ * pthread_cond_wait a cancellation point.
+ * We use the cleanup mechanism to ensure we
+ * re-lock the mutex and adjust (to)unblock(ed) waiters
+ * counts if we are cancelled, timed out or signalled.
+ */
+ if (ptw32_sem_timedwait (&(cv->semBlockQueue), abstime) != 0)
+ {(ptw32_sem_timedwait
+ result = errno;
+ }
+ }
+
+ /*
+ * Always cleanup
+ */
+ pthread_cleanup_pop (1);
+
+
+ /*
+ * "result" can be modified by the cleanup handler.
+ */
+ return (result);
+
+} /* ptw32_cond_timedwait */
+
+
+static int
+ptw32_cond_unblock (pthread_cond_t * cond,
+ int unblockAll)
+{
+ int result;
+ pthread_cond_t cv;
+
+ if (cond == NULL || *cond == NULL)
+ {(cond
+ return EINVAL;
+ }
+
+ cv = *cond;
+
+ /*
+ * No-op if the CV is static and hasn't been initialised yet.
+ * Assuming that any race condition is harmless.
+ */
+ if (cv == (pthread_cond_t) PTW32_OBJECT_AUTO_INIT)
+ {(cv
+ return 0;
+ }
+
+ /*
+ * Synchronize access to waiters blocked count (LEVEL-1)
+ */
+ if (sem_wait(&(cv->semBlockLock)) != 0)
+ {(sem_wait(&(cv->semBlockLock))
+ return errno;
+ }
+
+ /*
+ * Synchronize access to waiters (to)unblock(ed) counts (LEVEL-2)
+ * This sync.level supports _timedwait and cancellation
+ */
+ if ((result = pthread_mutex_lock(&(cv->mtxUnblockLock))) != 0)
+ {((result
+ return result;
+ }
+
+ /*
+ * Adjust waiters blocked and unblocked counts (collect garbage)
+ */
+ if (cv->nWaitersUnblocked != 0)
+ {(cv->nWaitersUnblocked
+ cv->nWaitersBlocked -= cv->nWaitersUnblocked;
+ cv->nWaitersUnblocked = 0;
+ }
+
+ /*
+ * If (after adjustment) there are still some waiters blocked counted...
+ */
+ if ( cv->nWaitersBlocked > 0)
+ {(
+ /*
+ * We will unblock first waiter and leave semBlockLock/LEVEL-1 locked
+ * LEVEL-1 access is left disabled until last signal/unblock
+completes
+ */
+ cv->nWaitersToUnblock = (unblockAll) ? cv->nWaitersBlocked : 1;
+
+ /*
+ * No more LEVEL-2 access to waiters (to)unblock(ed) counts needed
+ * This sync.level supports _timedwait and cancellation
+ */
+ if ((result = pthread_mutex_unlock(&(cv->mtxUnblockLock))) != 0)
+ {((result
+ return result;
+ }
+
+
+ /*
+ * Now, with LEVEL-2 lock released let first waiter go through
+semaphore
+ */
+ if (sem_post(&(cv->semBlockQueue)) != 0)
+ {(sem_post(&(cv->semBlockQueue))
+ return errno;
+ }
+ }
+ /*
+ * No waiter blocked - no more LEVEL-1 access to blocked count needed...
+ */
+ else if (sem_post(&(cv->semBlockLock)) != 0)
+ {
+ return errno;
+ }
+ /*
+ * ...and no more LEVEL-2 access to waiters (to)unblock(ed) counts needed
+too
+ * This sync.level supports _timedwait and cancellation
+ */
+ else
+ {
+ result = pthread_mutex_unlock(&(cv->mtxUnblockLock));
+ }
+
+ return(result);
+
+} /* ptw32_cond_unblock */
+
+int
+pthread_cond_wait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex)
+{
+ /* The NULL abstime arg means INFINITE waiting. */
+ return(ptw32_cond_timedwait(cond, mutex, NULL));
+} /* pthread_cond_wait */
+
+
+int
+pthread_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex,
+ const struct timespec *abstime)
+{
+ if (abstime == NULL)
+ {(abstime
+ return EINVAL;
+ }
+
+ return(ptw32_cond_timedwait(cond, mutex, abstime));
+} /* pthread_cond_timedwait */
+
+
+int
+pthread_cond_signal (pthread_cond_t * cond)
+{
+ /* The '0'(FALSE) unblockAll arg means unblock ONE waiter. */
+ return(ptw32_cond_unblock(cond, 0));
+} /* pthread_cond_signal */
+
+int
+pthread_cond_broadcast (pthread_cond_t * cond)
+{
+ /* The '1'(TRUE) unblockAll arg means unblock ALL waiters. */
+ return(ptw32_cond_unblock(cond, 1));
+} /* pthread_cond_broadcast */
+
+
+
+
+TEREKHOV at de.ibm.com on 17.01.2001 01:00:57
+
+Please respond to TEREKHOV at de.ibm.com
+
+To: pthreads-win32 at sourceware.cygnus.com
+cc: schmidt at uci.edu
+Subject: win32 conditions: sem+counter+event = broadcast_deadlock +
+ spur.wakeup/unfairness/incorrectness ??
+
+
+
+
+
+
+
+Hi,
+
+Problem 1: broadcast_deadlock
+
+It seems that current implementation does not provide "atomic"
+broadcasts. That may lead to "nested" broadcasts... and it seems
+that nested case is not handled correctly -> producing a broadcast
+DEADLOCK as a result.
+
+Scenario:
+
+N (>1) waiting threads W1..N are blocked (in _wait) on condition's
+semaphore.
+
+Thread B1 calls pthread_cond_broadcast, which results in "releasing" N
+W threads via incrementing semaphore counter by N (stored in
+cv->waiters) BUT cv->waiters counter does not change!! The caller
+thread B1 remains blocked on cv->waitersDone event (auto-reset!!) BUT
+condition is not protected from starting another broadcast (when called
+on another thread) while still waiting for the "old" broadcast to
+complete on thread B1.
+
+M (>=0, <N) W threads are fast enough to go thru their _wait call and
+decrement cv->waiters counter.
+
+L (N-M) "late" waiter W threads are a) still blocked/not returned from
+their semaphore wait call or b) were preempted after sem_wait but before
+lock( &cv->waitersLock ) or c) are blocked on cv->waitersLock.
+
+cv->waiters is still > 0 (= L).
+
+Another thread B2 (or some W thread from M group) calls
+pthread_cond_broadcast and gains access to counter... neither a) nor b)
+prevent thread B2 in pthread_cond_broadcast from gaining access to
+counter and starting another broadcast ( for c) - it depends on
+cv->waitersLock scheduling rules: FIFO=OK, PRTY=PROBLEM,... )
+
+That call to pthread_cond_broadcast (on thread B2) will result in
+incrementing semaphore by cv->waiters (=L) which is INCORRECT (all
+W1..N were in fact already released by thread B1) and waiting on
+_auto-reset_ event cv->waitersDone which is DEADLY WRONG (produces a
+deadlock)...
+
+All late W1..L threads now have a chance to complete their _wait call.
+Last W_L thread sets an auto-reselt event cv->waitersDone which will
+release either B1 or B2 leaving one of B threads in a deadlock.
+
+Problem 2: spur.wakeup/unfairness/incorrectness
+
+It seems that:
+
+a) because of the same problem with counter which does not reflect the
+actual number of NOT RELEASED waiters, the signal call may increment
+a semaphore counter w/o having a waiter blocked on it. That will result
+in (best case) spurious wake ups - performance degradation due to
+unnecessary context switches and predicate re-checks and (in worth case)
+unfairness/incorrectness problem - see b)
+
+b) neither signal nor broadcast prevent other threads - "new waiters"
+(and in the case of signal, the caller thread as well) from going into
+_wait and overtaking "old" waiters (already released but still not returned
+from sem_wait on condition's semaphore). Win semaphore just [API DOC]:
+"Maintains a count between zero and some maximum value, limiting the number
+of threads that are simultaneously accessing a shared resource." Calling
+ReleaseSemaphore does not imply (at least not documented) that on return
+from ReleaseSemaphore all waiters will in fact become released (returned
+from their Wait... call) and/or that new waiters calling Wait... afterwards
+will become less importance. It is NOT documented to be an atomic release
+of
+waiters... And even if it would be there is still a problem with a thread
+being preempted after Wait on semaphore and before Wait on cv->waitersLock
+and scheduling rules for cv->waitersLock itself
+(??WaitForMultipleObjects??)
+That may result in unfairness/incorrectness problem as described
+for SetEvent impl. in "Strategies for Implementing POSIX Condition
+Variables
+on Win32": http://www.cs.wustl.edu/~schmidt/win32-cv-1.html
+
+Unfairness -- The semantics of the POSIX pthread_cond_broadcast function is
+to wake up all threads currently blocked in wait calls on the condition
+variable. The awakened threads then compete for the external_mutex. To
+ensure
+fairness, all of these threads should be released from their
+pthread_cond_wait calls and allowed to recheck their condition expressions
+before other threads can successfully complete a wait on the condition
+variable.
+
+Unfortunately, the SetEvent implementation above does not guarantee that
+all
+threads sleeping on the condition variable when cond_broadcast is called
+will
+acquire the external_mutex and check their condition expressions. Although
+the Pthreads specification does not mandate this degree of fairness, the
+lack of fairness can cause starvation.
+
+To illustrate the unfairness problem, imagine there are 2 threads, C1 and
+C2,
+that are blocked in pthread_cond_wait on condition variable not_empty_ that
+is guarding a thread-safe message queue. Another thread, P1 then places two
+messages onto the queue and calls pthread_cond_broadcast. If C1 returns
+from
+pthread_cond_wait, dequeues and processes the message, and immediately
+waits
+again then it and only it may end up acquiring both messages. Thus, C2 will
+never get a chance to dequeue a message and run.
+
+The following illustrates the sequence of events:
+
+1. Thread C1 attempts to dequeue and waits on CV non_empty_
+2. Thread C2 attempts to dequeue and waits on CV non_empty_
+3. Thread P1 enqueues 2 messages and broadcasts to CV not_empty_
+4. Thread P1 exits
+5. Thread C1 wakes up from CV not_empty_, dequeues a message and runs
+6. Thread C1 waits again on CV not_empty_, immediately dequeues the 2nd
+ message and runs
+7. Thread C1 exits
+8. Thread C2 is the only thread left and blocks forever since
+ not_empty_ will never be signaled
+
+Depending on the algorithm being implemented, this lack of fairness may
+yield
+concurrent programs that have subtle bugs. Of course, application
+developers
+should not rely on the fairness semantics of pthread_cond_broadcast.
+However,
+there are many cases where fair implementations of condition variables can
+simplify application code.
+
+Incorrectness -- A variation on the unfairness problem described above
+occurs
+when a third consumer thread, C3, is allowed to slip through even though it
+was not waiting on condition variable not_empty_ when a broadcast occurred.
+
+To illustrate this, we will use the same scenario as above: 2 threads, C1
+and
+C2, are blocked dequeuing messages from the message queue. Another thread,
+P1
+then places two messages onto the queue and calls pthread_cond_broadcast.
+C1
+returns from pthread_cond_wait, dequeues and processes the message. At this
+time, C3 acquires the external_mutex, calls pthread_cond_wait and waits on
+the events in WaitForMultipleObjects. Since C2 has not had a chance to run
+yet, the BROADCAST event is still signaled. C3 then returns from
+WaitForMultipleObjects, and dequeues and processes the message in the
+queue.
+Thus, C2 will never get a chance to dequeue a message and run.
+
+The following illustrates the sequence of events:
+
+1. Thread C1 attempts to dequeue and waits on CV non_empty_
+2. Thread C2 attempts to dequeue and waits on CV non_empty_
+3. Thread P1 enqueues 2 messages and broadcasts to CV not_empty_
+4. Thread P1 exits
+5. Thread C1 wakes up from CV not_empty_, dequeues a message and runs
+6. Thread C1 exits
+7. Thread C3 waits on CV not_empty_, immediately dequeues the 2nd
+ message and runs
+8. Thread C3 exits
+9. Thread C2 is the only thread left and blocks forever since
+ not_empty_ will never be signaled
+
+In the above case, a thread that was not waiting on the condition variable
+when a broadcast occurred was allowed to proceed. This leads to incorrect
+semantics for a condition variable.
+
+
+COMMENTS???
+
+regards,
+alexander.
+
+-----------------------------------------------------------------------------
+
+Subject: RE: FYI/comp.programming.threads/Re: pthread_cond_*
+ implementation questions
+Date: Wed, 21 Feb 2001 11:54:47 +0100
+From: TEREKHOV at de.ibm.com
+To: lthomas at arbitrade.com
+CC: rpj at ise.canberra.edu.au, Thomas Pfaff <tpfaff at gmx.net>,
+ Nanbor Wang <nanbor at cs.wustl.edu>
+
+Hi Louis,
+
+generation number 8..
+
+had some time to revisit timeouts/spurious wakeup problem..
+found some bugs (in 7.b/c/d) and something to improve
+(7a - using IPC semaphores but it should speedup Win32
+version as well).
+
+regards,
+alexander.
+
+---------- Algorithm 8a / IMPL_SEM,UNBLOCK_STRATEGY == UNBLOCK_ALL ------
+given:
+semBlockLock - bin.semaphore
+semBlockQueue - semaphore
+mtxExternal - mutex or CS
+mtxUnblockLock - mutex or CS
+nWaitersGone - int
+nWaitersBlocked - int
+nWaitersToUnblock - int
+
+wait( timeout ) {
+
+ [auto: register int result ] // error checking omitted
+ [auto: register int nSignalsWasLeft ]
+ [auto: register int nWaitersWasGone ]
+
+ sem_wait( semBlockLock );
+ nWaitersBlocked++;
+ sem_post( semBlockLock );
+
+ unlock( mtxExternal );
+ bTimedOut = sem_wait( semBlockQueue,timeout );
+
+ lock( mtxUnblockLock );
+ if ( 0 != (nSignalsWasLeft = nWaitersToUnblock) ) {
+ if ( bTimeout ) { // timeout (or canceled)
+ if ( 0 != nWaitersBlocked ) {
+ nWaitersBlocked--;
+ }
+ else {
+ nWaitersGone++; // count spurious wakeups
+ }
+ }
+ if ( 0 == --nWaitersToUnblock ) {
+ if ( 0 != nWaitersBlocked ) {
+ sem_post( semBlockLock ); // open the gate
+ nSignalsWasLeft = 0; // do not open the gate below
+again
+ }
+ else if ( 0 != (nWaitersWasGone = nWaitersGone) ) {
+ nWaitersGone = 0;
+ }
+ }
+ }
+ else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or spurious
+semaphore :-)
+ sem_wait( semBlockLock );
+ nWaitersBlocked -= nWaitersGone; // something is going on here -
+test of timeouts? :-)
+ sem_post( semBlockLock );
+ nWaitersGone = 0;
+ }
+ unlock( mtxUnblockLock );
+
+ if ( 1 == nSignalsWasLeft ) {
+ if ( 0 != nWaitersWasGone ) {
+ // sem_adjust( -nWaitersWasGone );
+ while ( nWaitersWasGone-- ) {
+ sem_wait( semBlockLock ); // better now than spurious
+later
+ }
+ }
+ sem_post( semBlockLock ); // open the gate
+ }
+
+ lock( mtxExternal );
+
+ return ( bTimedOut ) ? ETIMEOUT : 0;
+}
+
+signal(bAll) {
+
+ [auto: register int result ]
+ [auto: register int nSignalsToIssue]
+
+ lock( mtxUnblockLock );
+
+ if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ if ( 0 == nWaitersBlocked ) { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+ if (bAll) {
+ nWaitersToUnblock += nSignalsToIssue=nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nSignalsToIssue = 1;
+ nWaitersToUnblock++;
+ nWaitersBlocked--;
+ }
+ }
+ else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ sem_wait( semBlockLock ); // close the gate
+ if ( 0 != nWaitersGone ) {
+ nWaitersBlocked -= nWaitersGone;
+ nWaitersGone = 0;
+ }
+ if (bAll) {
+ nSignalsToIssue = nWaitersToUnblock = nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nSignalsToIssue = nWaitersToUnblock = 1;
+ nWaitersBlocked--;
+ }
+ }
+ else { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+
+ unlock( mtxUnblockLock );
+ sem_post( semBlockQueue,nSignalsToIssue );
+ return result;
+}
+
+---------- Algorithm 8b / IMPL_SEM,UNBLOCK_STRATEGY == UNBLOCK_ONEBYONE
+------
+given:
+semBlockLock - bin.semaphore
+semBlockQueue - bin.semaphore
+mtxExternal - mutex or CS
+mtxUnblockLock - mutex or CS
+nWaitersGone - int
+nWaitersBlocked - int
+nWaitersToUnblock - int
+
+wait( timeout ) {
+
+ [auto: register int result ] // error checking omitted
+ [auto: register int nWaitersWasGone ]
+ [auto: register int nSignalsWasLeft ]
+
+ sem_wait( semBlockLock );
+ nWaitersBlocked++;
+ sem_post( semBlockLock );
+
+ unlock( mtxExternal );
+ bTimedOut = sem_wait( semBlockQueue,timeout );
+
+ lock( mtxUnblockLock );
+ if ( 0 != (nSignalsWasLeft = nWaitersToUnblock) ) {
+ if ( bTimeout ) { // timeout (or canceled)
+ if ( 0 != nWaitersBlocked ) {
+ nWaitersBlocked--;
+ nSignalsWasLeft = 0; // do not unblock next waiter
+below (already unblocked)
+ }
+ else {
+ nWaitersGone = 1; // spurious wakeup pending!!
+ }
+ }
+ if ( 0 == --nWaitersToUnblock &&
+ if ( 0 != nWaitersBlocked ) {
+ sem_post( semBlockLock ); // open the gate
+ nSignalsWasLeft = 0; // do not open the gate below
+again
+ }
+ else if ( 0 != (nWaitersWasGone = nWaitersGone) ) {
+ nWaitersGone = 0;
+ }
+ }
+ }
+ else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or spurious
+semaphore :-)
+ sem_wait( semBlockLock );
+ nWaitersBlocked -= nWaitersGone; // something is going on here -
+test of timeouts? :-)
+ sem_post( semBlockLock );
+ nWaitersGone = 0;
+ }
+ unlock( mtxUnblockLock );
+
+ if ( 1 == nSignalsWasLeft ) {
+ if ( 0 != nWaitersWasGone ) {
+ // sem_adjust( -1 );
+ sem_wait( semBlockQueue ); // better now than spurious
+later
+ }
+ sem_post( semBlockLock ); // open the gate
+ }
+ else if ( 0 != nSignalsWasLeft ) {
+ sem_post( semBlockQueue ); // unblock next waiter
+ }
+
+ lock( mtxExternal );
+
+ return ( bTimedOut ) ? ETIMEOUT : 0;
+}
+
+signal(bAll) {
+
+ [auto: register int result ]
+
+ lock( mtxUnblockLock );
+
+ if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ if ( 0 == nWaitersBlocked ) { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+ if (bAll) {
+ nWaitersToUnblock += nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nWaitersToUnblock++;
+ nWaitersBlocked--;
+ }
+ unlock( mtxUnblockLock );
+ }
+ else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ sem_wait( semBlockLock ); // close the gate
+ if ( 0 != nWaitersGone ) {
+ nWaitersBlocked -= nWaitersGone;
+ nWaitersGone = 0;
+ }
+ if (bAll) {
+ nWaitersToUnblock = nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nWaitersToUnblock = 1;
+ nWaitersBlocked--;
+ }
+ unlock( mtxUnblockLock );
+ sem_post( semBlockQueue );
+ }
+ else { // NO-OP
+ unlock( mtxUnblockLock );
+ }
+
+ return result;
+}
+
+---------- Algorithm 8c / IMPL_EVENT,UNBLOCK_STRATEGY == UNBLOCK_ONEBYONE
+---------
+given:
+hevBlockLock - auto-reset event
+hevBlockQueue - auto-reset event
+mtxExternal - mutex or CS
+mtxUnblockLock - mutex or CS
+nWaitersGone - int
+nWaitersBlocked - int
+nWaitersToUnblock - int
+
+wait( timeout ) {
+
+ [auto: register int result ] // error checking omitted
+ [auto: register int nSignalsWasLeft ]
+ [auto: register int nWaitersWasGone ]
+
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked++;
+ set_event( hevBlockLock );
+
+ unlock( mtxExternal );
+ bTimedOut = wait( hevBlockQueue,timeout );
+
+ lock( mtxUnblockLock );
+ if ( 0 != (SignalsWasLeft = nWaitersToUnblock) ) {
+ if ( bTimeout ) { // timeout (or canceled)
+ if ( 0 != nWaitersBlocked ) {
+ nWaitersBlocked--;
+ nSignalsWasLeft = 0; // do not unblock next waiter
+below (already unblocked)
+ }
+ else {
+ nWaitersGone = 1; // spurious wakeup pending!!
+ }
+ }
+ if ( 0 == --nWaitersToUnblock )
+ if ( 0 != nWaitersBlocked ) {
+ set_event( hevBlockLock ); // open the gate
+ nSignalsWasLeft = 0; // do not open the gate below
+again
+ }
+ else if ( 0 != (nWaitersWasGone = nWaitersGone) ) {
+ nWaitersGone = 0;
+ }
+ }
+ }
+ else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or spurious
+event :-)
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked -= nWaitersGone; // something is going on here -
+test of timeouts? :-)
+ set_event( hevBlockLock );
+ nWaitersGone = 0;
+ }
+ unlock( mtxUnblockLock );
+
+ if ( 1 == nSignalsWasLeft ) {
+ if ( 0 != nWaitersWasGone ) {
+ reset_event( hevBlockQueue ); // better now than spurious
+later
+ }
+ set_event( hevBlockLock ); // open the gate
+ }
+ else if ( 0 != nSignalsWasLeft ) {
+ set_event( hevBlockQueue ); // unblock next waiter
+ }
+
+ lock( mtxExternal );
+
+ return ( bTimedOut ) ? ETIMEOUT : 0;
+}
+
+signal(bAll) {
+
+ [auto: register int result ]
+
+ lock( mtxUnblockLock );
+
+ if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ if ( 0 == nWaitersBlocked ) { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+ if (bAll) {
+ nWaitersToUnblock += nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nWaitersToUnblock++;
+ nWaitersBlocked--;
+ }
+ unlock( mtxUnblockLock );
+ }
+ else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ wait( hevBlockLock,INFINITE ); // close the gate
+ if ( 0 != nWaitersGone ) {
+ nWaitersBlocked -= nWaitersGone;
+ nWaitersGone = 0;
+ }
+ if (bAll) {
+ nWaitersToUnblock = nWaitersBlocked;
+ nWaitersBlocked = 0;
+ }
+ else {
+ nWaitersToUnblock = 1;
+ nWaitersBlocked--;
+ }
+ unlock( mtxUnblockLock );
+ set_event( hevBlockQueue );
+ }
+ else { // NO-OP
+ unlock( mtxUnblockLock );
+ }
+
+ return result;
+}
+
+---------- Algorithm 8d / IMPL_EVENT,UNBLOCK_STRATEGY == UNBLOCK_ALL ------
+given:
+hevBlockLock - auto-reset event
+hevBlockQueueS - auto-reset event // for signals
+hevBlockQueueB - manual-reset even // for broadcasts
+mtxExternal - mutex or CS
+mtxUnblockLock - mutex or CS
+eBroadcast - int // 0: no broadcast, 1: broadcast, 2:
+broadcast after signal(s)
+nWaitersGone - int
+nWaitersBlocked - int
+nWaitersToUnblock - int
+
+wait( timeout ) {
+
+ [auto: register int result ] // error checking omitted
+ [auto: register int eWasBroadcast ]
+ [auto: register int nSignalsWasLeft ]
+ [auto: register int nWaitersWasGone ]
+
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked++;
+ set_event( hevBlockLock );
+
+ unlock( mtxExternal );
+ bTimedOut = waitformultiple( hevBlockQueueS,hevBlockQueueB,timeout,ONE );
+
+ lock( mtxUnblockLock );
+ if ( 0 != (SignalsWasLeft = nWaitersToUnblock) ) {
+ if ( bTimeout ) { // timeout (or canceled)
+ if ( 0 != nWaitersBlocked ) {
+ nWaitersBlocked--;
+ nSignalsWasLeft = 0; // do not unblock next waiter
+below (already unblocked)
+ }
+ else if ( 1 != eBroadcast ) {
+ nWaitersGone = 1;
+ }
+ }
+ if ( 0 == --nWaitersToUnblock ) {
+ if ( 0 != nWaitersBlocked ) {
+ set_event( hevBlockLock ); // open the gate
+ nSignalsWasLeft = 0; // do not open the gate below
+again
+ }
+ else {
+ if ( 0 != (eWasBroadcast = eBroadcast) ) {
+ eBroadcast = 0;
+ }
+ if ( 0 != (nWaitersWasGone = nWaitersGone ) {
+ nWaitersGone = 0;
+ }
+ }
+ }
+ else if ( 0 != eBroadcast ) {
+ nSignalsWasLeft = 0; // do not unblock next waiter
+below (already unblocked)
+ }
+ }
+ else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or spurious
+event :-)
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked -= nWaitersGone; // something is going on here -
+test of timeouts? :-)
+ set_event( hevBlockLock );
+ nWaitersGone = 0;
+ }
+ unlock( mtxUnblockLock );
+
+ if ( 1 == nSignalsWasLeft ) {
+ if ( 0 != eWasBroadcast ) {
+ reset_event( hevBlockQueueB );
+ }
+ if ( 0 != nWaitersWasGone ) {
+ reset_event( hevBlockQueueS ); // better now than spurious
+later
+ }
+ set_event( hevBlockLock ); // open the gate
+ }
+ else if ( 0 != nSignalsWasLeft ) {
+ set_event( hevBlockQueueS ); // unblock next waiter
+ }
+
+ lock( mtxExternal );
+
+ return ( bTimedOut ) ? ETIMEOUT : 0;
+}
+
+signal(bAll) {
+
+ [auto: register int result ]
+ [auto: register HANDLE hevBlockQueue ]
+
+ lock( mtxUnblockLock );
+
+ if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ if ( 0 == nWaitersBlocked ) { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+ if (bAll) {
+ nWaitersToUnblock += nWaitersBlocked;
+ nWaitersBlocked = 0;
+ eBroadcast = 2;
+ hevBlockQueue = hevBlockQueueB;
+ }
+ else {
+ nWaitersToUnblock++;
+ nWaitersBlocked--;
+ return unlock( mtxUnblockLock );
+ }
+ }
+ else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ wait( hevBlockLock,INFINITE ); // close the gate
+ if ( 0 != nWaitersGone ) {
+ nWaitersBlocked -= nWaitersGone;
+ nWaitersGone = 0;
+ }
+ if (bAll) {
+ nWaitersToUnblock = nWaitersBlocked;
+ nWaitersBlocked = 0;
+ eBroadcast = 1;
+ hevBlockQueue = hevBlockQueueB;
+ }
+ else {
+ nWaitersToUnblock = 1;
+ nWaitersBlocked--;
+ hevBlockQueue = hevBlockQueueS;
+ }
+ }
+ else { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+
+ unlock( mtxUnblockLock );
+ set_event( hevBlockQueue );
+ return result;
+}
+---------------------- Forwarded by Alexander Terekhov/Germany/IBM on
+02/21/2001 09:13 AM ---------------------------
+
+Alexander Terekhov
+02/20/2001 04:33 PM
+
+To: Louis Thomas <lthomas at arbitrade.com>
+cc:
+
+From: Alexander Terekhov/Germany/IBM at IBMDE
+Subject: RE: FYI/comp.programming.threads/Re: pthread_cond_* implementatio
+ n questions
+Importance: Normal
+
+>Sorry, gotta take a break and work on something else for a while.
+>Real work
+>calls, unfortunately. I'll get back to you in two or three days.
+
+ok. no problem. here is some more stuff for pauses you might have
+in between :)
+
+---------- Algorithm 7d / IMPL_EVENT,UNBLOCK_STRATEGY == UNBLOCK_ALL ------
+given:
+hevBlockLock - auto-reset event
+hevBlockQueueS - auto-reset event // for signals
+hevBlockQueueB - manual-reset even // for broadcasts
+mtxExternal - mutex or CS
+mtxUnblockLock - mutex or CS
+bBroadcast - int
+nWaitersGone - int
+nWaitersBlocked - int
+nWaitersToUnblock - int
+
+wait( timeout ) {
+
+ [auto: register int result ] // error checking omitted
+ [auto: register int bWasBroadcast ]
+ [auto: register int nSignalsWasLeft ]
+
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked++;
+ set_event( hevBlockLock );
+
+ unlock( mtxExternal );
+ bTimedOut = waitformultiple( hevBlockQueueS,hevBlockQueueB,timeout,ONE );
+
+ lock( mtxUnblockLock );
+ if ( 0 != (SignalsWasLeft = nWaitersToUnblock) ) {
+ if ( bTimeout ) { // timeout (or canceled)
+ if ( 0 != nWaitersBlocked ) {
+ nWaitersBlocked--;
+ nSignalsWasLeft = 0; // do not unblock next waiter
+below (already unblocked)
+ }
+ else if ( !bBroadcast ) {
+ wait( hevBlockQueueS,INFINITE ); // better now than spurious
+later
+ }
+ }
+ if ( 0 == --nWaitersToUnblock ) {
+ if ( 0 != nWaitersBlocked ) {
+ if ( bBroadcast ) {
+ reset_event( hevBlockQueueB );
+ bBroadcast = false;
+ }
+ set_event( hevBlockLock ); // open the gate
+ nSignalsWasLeft = 0; // do not open the gate below
+again
+ }
+ else if ( false != (bWasBroadcast = bBroadcast) ) {
+ bBroadcast = false;
+ }
+ }
+ else {
+ bWasBroadcast = bBroadcast;
+ }
+ }
+ else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or spurious
+event :-)
+ wait( hevBlockLock,INFINITE );
+ nWaitersBlocked -= nWaitersGone; // something is going on here -
+test of timeouts? :-)
+ set_event( hevBlockLock );
+ nWaitersGone = 0;
+ }
+ unlock( mtxUnblockLock );
+
+ if ( 1 == nSignalsWasLeft ) {
+ if ( bWasBroadcast ) {
+ reset_event( hevBlockQueueB );
+ }
+ set_event( hevBlockLock ); // open the gate
+ }
+ else if ( 0 != nSignalsWasLeft && !bWasBroadcast ) {
+ set_event( hevBlockQueueS ); // unblock next waiter
+ }
+
+ lock( mtxExternal );
+
+ return ( bTimedOut ) ? ETIMEOUT : 0;
+}
+
+signal(bAll) {
+
+ [auto: register int result ]
+ [auto: register HANDLE hevBlockQueue ]
+
+ lock( mtxUnblockLock );
+
+ if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ if ( 0 == nWaitersBlocked ) { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+ if (bAll) {
+ nWaitersToUnblock += nWaitersBlocked;
+ nWaitersBlocked = 0;
+ bBroadcast = true;
+ hevBlockQueue = hevBlockQueueB;
+ }
+ else {
+ nWaitersToUnblock++;
+ nWaitersBlocked--;
+ return unlock( mtxUnblockLock );
+ }
+ }
+ else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ wait( hevBlockLock,INFINITE ); // close the gate
+ if ( 0 != nWaitersGone ) {
+ nWaitersBlocked -= nWaitersGone;
+ nWaitersGone = 0;
+ }
+ if (bAll) {
+ nWaitersToUnblock = nWaitersBlocked;
+ nWaitersBlocked = 0;
+ bBroadcast = true;
+ hevBlockQueue = hevBlockQueueB;
+ }
+ else {
+ nWaitersToUnblock = 1;
+ nWaitersBlocked--;
+ hevBlockQueue = hevBlockQueueS;
+ }
+ }
+ else { // NO-OP
+ return unlock( mtxUnblockLock );
+ }
+
+ unlock( mtxUnblockLock );
+ set_event( hevBlockQueue );
+ return result;
+}
+
+
+----------------------------------------------------------------------------
+
+Subject: RE: FYI/comp.programming.threads/Re: pthread_cond_* implementatio
+ n questions
+Date: Mon, 26 Feb 2001 22:20:12 -0600
+From: Louis Thomas <lthomas at arbitrade.com>
+To: "'TEREKHOV at de.ibm.com'" <TEREKHOV at de.ibm.com>
+CC: rpj at ise.canberra.edu.au, Thomas Pfaff <tpfaff at gmx.net>,
+ Nanbor Wang
+ <nanbor at cs.wustl.edu>
+
+Sorry all. Busy week.
+
+> this insures the fairness
+> which POSIX does not (e.g. two subsequent broadcasts - the gate does
+insure
+> that first wave waiters will start the race for the mutex before waiters
+> from the second wave - Linux pthreads process/unblock both waves
+> concurrently...)
+
+I'm not sure how we are any more fair about this than Linux. We certainly
+don't guarantee that the threads released by the first broadcast will get
+the external mutex before the threads of the second wave. In fact, it is
+possible that those threads will never get the external mutex if there is
+enough contention for it.
+
+> e.g. i was thinking about implementation with a pool of
+> N semaphores/counters [...]
+
+I considered that too. The problem is as you mentioned in a). You really
+need to assign threads to semaphores once you know how you want to wake them
+up, not when they first begin waiting which is the only time you can assign
+them.
+
+> well, i am not quite sure that i've fully understood your scenario,
+
+Hmm. Well, it think it's an important example, so I'll try again. First, we
+have thread A which we KNOW is waiting on a condition. As soon as it becomes
+unblocked for any reason, we will know because it will set a flag. Since the
+flag is not set, we are 100% confident that thread A is waiting on the
+condition. We have another thread, thread B, which has acquired the mutex
+and is about to wait on the condition. Thus it is pretty clear that at any
+point, either just A is waiting, or A and B are waiting. Now thread C comes
+along. C is about to do a broadcast on the condition. A broadcast is
+guaranteed to unblock all threads currently waiting on a condition, right?
+Again, we said that either just A is waiting, or A and B are both waiting.
+So, when C does its broadcast, depending upon whether B has started waiting
+or not, thread C will unblock A or unblock A and B. Either way, C must
+unblock A, right?
+
+Now, you said anything that happens is correct so long as a) "a signal is
+not lost between unlocking the mutex and waiting on the condition" and b) "a
+thread must not steal a signal it sent", correct? Requirement b) is easy to
+satisfy: in this scenario, thread C will never wait on the condition, so it
+won't steal any signals. Requirement a) is not hard either. The only way we
+could fail to meet requirement a) in this scenario is if thread B was
+started waiting but didn't wake up because a signal was lost. This will not
+happen.
+
+Now, here is what happens. Assume thread C beats thread B. Thread C looks to
+see how many threads are waiting on the condition. Thread C sees just one
+thread, thread A, waiting. It does a broadcast waking up just one thread
+because just one thread is waiting. Next, before A can become unblocked,
+thread B begins waiting. Now there are two threads waiting, but only one
+will be unblocked. Suppose B wins. B will become unblocked. A will not
+become unblocked, because C only unblocked one thread (sema_post cond, 1).
+So at the end, B finishes and A remains blocked.
+
+We have met both of your requirements, so by your rules, this is an
+acceptable outcome. However, I think that the spec says this is an
+unacceptable outcome! We know for certain that A was waiting and that C did
+a broadcast, but A did not become unblocked! Yet, the spec says that a
+broadcast wakes up all waiting threads. This did not happen. Do you agree
+that this shows your rules are not strict enough?
+
+> and what about N2? :) this one does allow almost everything.
+
+Don't get me started about rule #2. I'll NEVER advocate an algorithm that
+uses rule 2 as an excuse to suck!
+
+> but it is done (decrement)under mutex protection - this is not a subject
+> of a race condition.
+
+You are correct. My mistake.
+
+> i would remove "_bTimedOut=false".. after all, it was a real timeout..
+
+I disagree. A thread that can't successfully retract its waiter status can't
+really have timed out. If a thread can't return without executing extra code
+to deal with the fact that someone tried to unblock it, I think it is a poor
+idea to pretend we
+didn't realize someone was trying to signal us. After all, a signal is more
+important than a time out.
+
+> when nSignaled != 0, it is possible to update nWaiters (--) and do not
+> touch nGone
+
+I realize this, but I was thinking that writing it the other ways saves
+another if statement.
+
+> adjust only if nGone != 0 and save one cache memory write - probably much
+slower than 'if'
+
+Hmm. You are probably right.
+
+> well, in a strange (e.g. timeout test) program you may (theoretically)
+> have an overflow of nWaiters/nGone counters (with waiters repeatedly
+timing
+> out and no signals at all).
+
+Also true. Not only that, but you also have the possibility that one could
+overflow the number of waiters as well! However, considering the limit you
+have chosen for nWaitersGone, I suppose it is unlikely that anyone would be
+able to get INT_MAX/2 threads waiting on a single condition. :)
+
+Analysis of 8a:
+
+It looks correct to me.
+
+What are IPC semaphores?
+
+In the line where you state, "else if ( nWaitersBlocked > nWaitersGone ) {
+// HARMLESS RACE CONDITION!" there is no race condition for nWaitersGone
+because nWaitersGone is never modified without holding mtxUnblockLock. You
+are correct that there is a harmless race on nWaitersBlocked, which can
+increase and make the condition become true just after we check it. If this
+happens, we interpret it as the wait starting after the signal.
+
+I like your optimization of this. You could improve Alg. 6 as follows:
+---------- Algorithm 6b ----------
+signal(bAll) {
+ _nSig=0
+ lock counters
+ // this is safe because nWaiting can only be decremented by a thread that
+ // owns counters and nGone can only be changed by a thread that owns
+counters.
+ if (nWaiting>nGone) {
+ if (0==nSignaled) {
+ sema_wait gate // close gate if not already closed
+ }
+ if (nGone>0) {
+ nWaiting-=nGone
+ nGone=0
+ }
+ _nSig=bAll?nWaiting:1
+ nSignaled+=_nSig
+ nWaiting-=_nSig
+ }
+ unlock counters
+ if (0!=_nSig) {
+ sema_post queue, _nSig
+ }
+}
+---------- ---------- ----------
+I guess this wouldn't apply to Alg 8a because nWaitersGone changes meanings
+depending upon whether the gate is open or closed.
+
+In the loop "while ( nWaitersWasGone-- ) {" you do a sema_wait on
+semBlockLock. Perhaps waiting on semBlockQueue would be a better idea.
+
+What have you gained by making the last thread to be signaled do the waits
+for all the timed out threads, besides added complexity? It took me a long
+time to figure out what your objective was with this, to realize you were
+using nWaitersGone to mean two different things, and to verify that you
+hadn't introduced any bug by doing this. Even now I'm not 100% sure.
+
+What has all this playing about with nWaitersGone really gained us besides a
+lot of complexity (it is much harder to verify that this solution is
+correct), execution overhead (we now have a lot more if statements to
+evaluate), and space overhead (more space for the extra code, and another
+integer in our data)? We did manage to save a lock/unlock pair in an
+uncommon case (when a time out occurs) at the above mentioned expenses in
+the common cases.
+
+As for 8b, c, and d, they look ok though I haven't studied them thoroughly.
+What would you use them for?
+
+ Later,
+ -Louis! :)
+
+-----------------------------------------------------------------------------
+
+Subject: RE: FYI/comp.programming.threads/Re: pthread_cond_* implementatio
+ n questions
+Date: Tue, 27 Feb 2001 15:51:28 +0100
+From: TEREKHOV at de.ibm.com
+To: Louis Thomas <lthomas at arbitrade.com>
+CC: rpj at ise.canberra.edu.au, Thomas Pfaff <tpfaff at gmx.net>,
+ Nanbor Wang <nanbor at cs.wustl.edu>
+
+Hi Louis,
+
+>> that first wave waiters will start the race for the mutex before waiters
+>> from the second wave - Linux pthreads process/unblock both waves
+>> concurrently...)
+>
+>I'm not sure how we are any more fair about this than Linux. We certainly
+>don't guarantee that the threads released by the first broadcast will get
+>the external mutex before the threads of the second wave. In fact, it is
+>possible that those threads will never get the external mutex if there is
+>enough contention for it.
+
+correct. but gate is nevertheless more fair than Linux because of the
+barrier it establishes between two races (1st and 2nd wave waiters) for
+the mutex which under 'normal' circumstances (e.g. all threads of equal
+priorities,..) will 'probably' result in fair behaviour with respect to
+mutex ownership.
+
+>> well, i am not quite sure that i've fully understood your scenario,
+>
+>Hmm. Well, it think it's an important example, so I'll try again. ...
+
+ok. now i seem to understand this example. well, now it seems to me
+that the only meaningful rule is just:
+
+a) "a signal is not lost between unlocking the mutex and waiting on the
+condition"
+
+and that the rule
+
+b) "a thread must not steal a signal it sent"
+
+is not needed at all because a thread which violates b) also violates a).
+
+i'll try to explain..
+
+i think that the most important thing is how POSIX defines waiter's
+visibility:
+
+"if another thread is able to acquire the mutex after the about-to-block
+thread
+has released it, then a subsequent call to pthread_cond_signal() or
+pthread_cond_broadcast() in that thread behaves as if it were issued after
+the about-to-block thread has blocked. "
+
+my understanding is the following:
+
+1) there is no guarantees whatsoever with respect to whether
+signal/broadcast
+will actually unblock any 'waiter' if it is done w/o acquiring the mutex
+first
+(note that a thread may release it before signal/broadcast - it does not
+matter).
+
+2) it is guaranteed that waiters become 'visible' - eligible for unblock as
+soon
+as signalling thread acquires the mutex (but not before!!)
+
+so..
+
+>So, when C does its broadcast, depending upon whether B has started
+waiting
+>or not, thread C will unblock A or unblock A and B. Either way, C must
+>unblock A, right?
+
+right. but only if C did acquire the mutex prior to broadcast (it may
+release it before broadcast as well).
+
+implementation will violate waiters visibility rule (signal will become
+lost)
+if C will not unblock A.
+
+>Now, here is what happens. Assume thread C beats thread B. Thread C looks
+to
+>see how many threads are waiting on the condition. Thread C sees just one
+>thread, thread A, waiting. It does a broadcast waking up just one thread
+>because just one thread is waiting. Next, before A can become unblocked,
+>thread B begins waiting. Now there are two threads waiting, but only one
+>will be unblocked. Suppose B wins. B will become unblocked. A will not
+>become unblocked, because C only unblocked one thread (sema_post cond, 1).
+>So at the end, B finishes and A remains blocked.
+
+thread C did acquire the mutex ("Thread C sees just one thread, thread A,
+waiting"). beginning from that moment it is guaranteed that subsequent
+broadcast will unblock A. Otherwise we will have a lost signal with respect
+to A. I do think that it does not matter whether the signal was physically
+(completely) lost or was just stolen by another thread (B) - in both cases
+it was simply lost with respect to A.
+
+>..Do you agree that this shows your rules are not strict enough?
+
+probably the opposite.. :-) i think that it shows that the only meaningful
+rule is
+
+a) "a signal is not lost between unlocking the mutex and waiting on the
+condition"
+
+with clarification of waiters visibility as defined by POSIX above.
+
+>> i would remove "_bTimedOut=false".. after all, it was a real timeout..
+>
+>I disagree. A thread that can't successfully retract its waiter status
+can't
+>really have timed out. If a thread can't return without executing extra
+code
+>to deal with the fact that someone tried to unblock it, I think it is a
+poor
+>idea to pretend we
+>didn't realize someone was trying to signal us. After all, a signal is
+more
+>important than a time out.
+
+a) POSIX does allow timed out thread to consume a signal (cancelled is
+not).
+b) ETIMEDOUT status just says that: "The time specified by abstime to
+pthread_cond_timedwait() has passed."
+c) it seem to me that hiding timeouts would violate "The
+pthread_cond_timedwait()
+function is the same as pthread_cond_wait() except that an error is
+returned if
+the absolute time specified by abstime passes (that is, system time equals
+or
+exceeds abstime) before the condition cond is signaled or broadcasted"
+because
+the abs. time did really pass before cond was signaled (waiter was
+released via semaphore). however, if it really matters, i could imaging
+that we
+can save an abs. time of signal/broadcast and compare it with timeout after
+unblock to find out whether it was a 'real' timeout or not. absent this
+check
+i do think that hiding timeouts would result in technical violation of
+specification.. but i think that this check is not important and we can
+simply
+trust timeout error code provided by wait since we are not trying to make
+'hard' realtime implementation.
+
+>What are IPC semaphores?
+
+<sys/sem.h>
+int semctl(int, int, int, ...);
+int semget(key_t, int, int);
+int semop(int, struct sembuf *, size_t);
+
+they support adjustment of semaphore counter (semvalue)
+in one single call - imaging Win32 ReleaseSemaphore( hsem,-N )
+
+>In the line where you state, "else if ( nWaitersBlocked > nWaitersGone ) {
+>// HARMLESS RACE CONDITION!" there is no race condition for nWaitersGone
+>because nWaitersGone is never modified without holding mtxUnblockLock. You
+>are correct that there is a harmless race on nWaitersBlocked, which can
+>increase and make the condition become true just after we check it. If
+this
+>happens, we interpret it as the wait starting after the signal.
+
+well, the reason why i've asked on comp.programming.threads whether this
+race
+condition is harmless or not is that in order to be harmless it should not
+violate the waiters visibility rule (see above). Fortunately, we increment
+the counter under protection of external mutex.. so that any (signalling)
+thread which will acquire the mutex next, should see the updated counter
+(in signal) according to POSIX memory visibility rules and mutexes
+(memory barriers). But i am not so sure how it actually works on
+Win32/INTEL
+which does not explicitly define any memory visibility rules :(
+
+>I like your optimization of this. You could improve Alg. 6 as follows:
+>---------- Algorithm 6b ----------
+>signal(bAll) {
+> _nSig=0
+> lock counters
+> // this is safe because nWaiting can only be decremented by a thread
+that
+> // owns counters and nGone can only be changed by a thread that owns
+>counters.
+> if (nWaiting>nGone) {
+> if (0==nSignaled) {
+> sema_wait gate // close gate if not already closed
+> }
+> if (nGone>0) {
+> nWaiting-=nGone
+> nGone=0
+> }
+> _nSig=bAll?nWaiting:1
+> nSignaled+=_nSig
+> nWaiting-=_nSig
+> }
+> unlock counters
+> if (0!=_nSig) {
+> sema_post queue, _nSig
+> }
+>}
+>---------- ---------- ----------
+>I guess this wouldn't apply to Alg 8a because nWaitersGone changes
+meanings
+>depending upon whether the gate is open or closed.
+
+agree.
+
+>In the loop "while ( nWaitersWasGone-- ) {" you do a sema_wait on
+>semBlockLock. Perhaps waiting on semBlockQueue would be a better idea.
+
+you are correct. my mistake.
+
+>What have you gained by making the last thread to be signaled do the waits
+>for all the timed out threads, besides added complexity? It took me a long
+>time to figure out what your objective was with this, to realize you were
+>using nWaitersGone to mean two different things, and to verify that you
+>hadn't introduced any bug by doing this. Even now I'm not 100% sure.
+>
+>What has all this playing about with nWaitersGone really gained us besides
+a
+>lot of complexity (it is much harder to verify that this solution is
+>correct), execution overhead (we now have a lot more if statements to
+>evaluate), and space overhead (more space for the extra code, and another
+>integer in our data)? We did manage to save a lock/unlock pair in an
+>uncommon case (when a time out occurs) at the above mentioned expenses in
+>the common cases.
+
+well, please consider the following:
+
+1) with multiple waiters unblocked (but some timed out) the trick with
+counter
+seem to ensure potentially higher level of concurrency by not delaying
+most of unblocked waiters for semaphore cleanup - only the last one
+will be delayed but all others would already contend/acquire/release
+the external mutex - the critical section protected by mtxUnblockLock is
+made smaller (increment + couple of IFs is faster than system/kernel call)
+which i think is good in general. however, you are right, this is done
+at expense of 'normal' waiters..
+
+2) some semaphore APIs (e.g. POSIX IPC sems) do allow to adjust the
+semaphore counter in one call => less system/kernel calls.. imagine:
+
+if ( 1 == nSignalsWasLeft ) {
+ if ( 0 != nWaitersWasGone ) {
+ ReleaseSemaphore( semBlockQueue,-nWaitersWasGone ); // better now
+than spurious later
+ }
+ sem_post( semBlockLock ); // open the gate
+ }
+
+3) even on win32 a single thread doing multiple cleanup calls (to wait)
+will probably result in faster execution (because of processor caching)
+than multiple threads each doing a single call to wait.
+
+>As for 8b, c, and d, they look ok though I haven't studied them
+thoroughly.
+>What would you use them for?
+
+8b) for semaphores which do not allow to unblock multiple waiters
+in a single call to post/release (e.g. POSIX realtime semaphores -
+<semaphore.h>)
+
+8c/8d) for WinCE prior to 3.0 (WinCE 3.0 does have semaphores)
+
+ok. so, which one is the 'final' algorithm(s) which we should use in
+pthreads-win32??
+
+regards,
+alexander.
+
+----------------------------------------------------------------------------
+
+Louis Thomas <lthomas at arbitrade.com> on 02/27/2001 05:20:12 AM
+
+Please respond to Louis Thomas <lthomas at arbitrade.com>
+
+To: Alexander Terekhov/Germany/IBM at IBMDE
+cc: rpj at ise.canberra.edu.au, Thomas Pfaff <tpfaff at gmx.net>, Nanbor Wang
+ <nanbor at cs.wustl.edu>
+Subject: RE: FYI/comp.programming.threads/Re: pthread_cond_* implementatio
+ n questions
+
+Sorry all. Busy week.
+
+> this insures the fairness
+> which POSIX does not (e.g. two subsequent broadcasts - the gate does
+insure
+> that first wave waiters will start the race for the mutex before waiters
+> from the second wave - Linux pthreads process/unblock both waves
+> concurrently...)
+
+I'm not sure how we are any more fair about this than Linux. We certainly
+don't guarantee that the threads released by the first broadcast will get
+the external mutex before the threads of the second wave. In fact, it is
+possible that those threads will never get the external mutex if there is
+enough contention for it.
+
+> e.g. i was thinking about implementation with a pool of
+> N semaphores/counters [...]
+
+I considered that too. The problem is as you mentioned in a). You really
+need to assign threads to semaphores once you know how you want to wake
+them
+up, not when they first begin waiting which is the only time you can assign
+them.
+
+> well, i am not quite sure that i've fully understood your scenario,
+
+Hmm. Well, it think it's an important example, so I'll try again. First, we
+have thread A which we KNOW is waiting on a condition. As soon as it
+becomes
+unblocked for any reason, we will know because it will set a flag. Since
+the
+flag is not set, we are 100% confident that thread A is waiting on the
+condition. We have another thread, thread B, which has acquired the mutex
+and is about to wait on the condition. Thus it is pretty clear that at any
+point, either just A is waiting, or A and B are waiting. Now thread C comes
+along. C is about to do a broadcast on the condition. A broadcast is
+guaranteed to unblock all threads currently waiting on a condition, right?
+Again, we said that either just A is waiting, or A and B are both waiting.
+So, when C does its broadcast, depending upon whether B has started waiting
+or not, thread C will unblock A or unblock A and B. Either way, C must
+unblock A, right?
+
+Now, you said anything that happens is correct so long as a) "a signal is
+not lost between unlocking the mutex and waiting on the condition" and b)
+"a
+thread must not steal a signal it sent", correct? Requirement b) is easy to
+satisfy: in this scenario, thread C will never wait on the condition, so it
+won't steal any signals. Requirement a) is not hard either. The only way
+we
+could fail to meet requirement a) in this scenario is if thread B was
+started waiting but didn't wake up because a signal was lost. This will not
+happen.
+
+Now, here is what happens. Assume thread C beats thread B. Thread C looks
+to
+see how many threads are waiting on the condition. Thread C sees just one
+thread, thread A, waiting. It does a broadcast waking up just one thread
+because just one thread is waiting. Next, before A can become unblocked,
+thread B begins waiting. Now there are two threads waiting, but only one
+will be unblocked. Suppose B wins. B will become unblocked. A will not
+become unblocked, because C only unblocked one thread (sema_post cond, 1).
+So at the end, B finishes and A remains blocked.
+
+We have met both of your requirements, so by your rules, this is an
+acceptable outcome. However, I think that the spec says this is an
+unacceptable outcome! We know for certain that A was waiting and that C did
+a broadcast, but A did not become unblocked! Yet, the spec says that a
+broadcast wakes up all waiting threads. This did not happen. Do you agree
+that this shows your rules are not strict enough?
+
+> and what about N2? :) this one does allow almost everything.
+
+Don't get me started about rule #2. I'll NEVER advocate an algorithm that
+uses rule 2 as an excuse to suck!
+
+> but it is done (decrement)under mutex protection - this is not a subject
+> of a race condition.
+
+You are correct. My mistake.
+
+> i would remove "_bTimedOut=false".. after all, it was a real timeout..
+
+I disagree. A thread that can't successfully retract its waiter status
+can't
+really have timed out. If a thread can't return without executing extra
+code
+to deal with the fact that someone tried to unblock it, I think it is a
+poor
+idea to pretend we
+didn't realize someone was trying to signal us. After all, a signal is more
+important than a time out.
+
+> when nSignaled != 0, it is possible to update nWaiters (--) and do not
+> touch nGone
+
+I realize this, but I was thinking that writing it the other ways saves
+another if statement.
+
+> adjust only if nGone != 0 and save one cache memory write - probably much
+slower than 'if'
+
+Hmm. You are probably right.
+
+> well, in a strange (e.g. timeout test) program you may (theoretically)
+> have an overflow of nWaiters/nGone counters (with waiters repeatedly
+timing
+> out and no signals at all).
+
+Also true. Not only that, but you also have the possibility that one could
+overflow the number of waiters as well! However, considering the limit you
+have chosen for nWaitersGone, I suppose it is unlikely that anyone would be
+able to get INT_MAX/2 threads waiting on a single condition. :)
+
+Analysis of 8a:
+
+It looks correct to me.
+
+What are IPC semaphores?
+
+In the line where you state, "else if ( nWaitersBlocked > nWaitersGone ) {
+// HARMLESS RACE CONDITION!" there is no race condition for nWaitersGone
+because nWaitersGone is never modified without holding mtxUnblockLock. You
+are correct that there is a harmless race on nWaitersBlocked, which can
+increase and make the condition become true just after we check it. If this
+happens, we interpret it as the wait starting after the signal.
+
+I like your optimization of this. You could improve Alg. 6 as follows:
+---------- Algorithm 6b ----------
+signal(bAll) {
+ _nSig=0
+ lock counters
+ // this is safe because nWaiting can only be decremented by a thread that
+ // owns counters and nGone can only be changed by a thread that owns
+counters.
+ if (nWaiting>nGone) {
+ if (0==nSignaled) {
+ sema_wait gate // close gate if not already closed
+ }
+ if (nGone>0) {
+ nWaiting-=nGone
+ nGone=0
+ }
+ _nSig=bAll?nWaiting:1
+ nSignaled+=_nSig
+ nWaiting-=_nSig
+ }
+ unlock counters
+ if (0!=_nSig) {
+ sema_post queue, _nSig
+ }
+}
+---------- ---------- ----------
+I guess this wouldn't apply to Alg 8a because nWaitersGone changes meanings
+depending upon whether the gate is open or closed.
+
+In the loop "while ( nWaitersWasGone-- ) {" you do a sema_wait on
+semBlockLock. Perhaps waiting on semBlockQueue would be a better idea.
+
+What have you gained by making the last thread to be signaled do the waits
+for all the timed out threads, besides added complexity? It took me a long
+time to figure out what your objective was with this, to realize you were
+using nWaitersGone to mean two different things, and to verify that you
+hadn't introduced any bug by doing this. Even now I'm not 100% sure.
+
+What has all this playing about with nWaitersGone really gained us besides
+a
+lot of complexity (it is much harder to verify that this solution is
+correct), execution overhead (we now have a lot more if statements to
+evaluate), and space overhead (more space for the extra code, and another
+integer in our data)? We did manage to save a lock/unlock pair in an
+uncommon case (when a time out occurs) at the above mentioned expenses in
+the common cases.
+
+As for 8b, c, and d, they look ok though I haven't studied them thoroughly.
+What would you use them for?
+
+ Later,
+ -Louis! :)
+
diff --git a/3rdparty/pthreads-win32/README.NONPORTABLE b/3rdparty/pthreads-win32/README.NONPORTABLE
new file mode 100644
index 0000000..9bdc445
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.NONPORTABLE
@@ -0,0 +1,783 @@
+This file documents non-portable functions and other issues.
+
+Non-portable functions included in pthreads-win32
+-------------------------------------------------
+
+BOOL
+pthread_win32_test_features_np(int mask)
+
+ This routine allows an application to check which
+ run-time auto-detected features are available within
+ the library.
+
+ The possible features are:
+
+ PTW32_SYSTEM_INTERLOCKED_COMPARE_EXCHANGE
+ Return TRUE if the native version of
+ InterlockedCompareExchange() is being used.
+ This feature is not meaningful in recent
+ library versions as MSVC builds only support
+ system implemented ICE. Note that all Mingw
+ builds use inlined asm versions of all the
+ Interlocked routines.
+ PTW32_ALERTABLE_ASYNC_CANCEL
+ Return TRUE is the QueueUserAPCEx package
+ QUSEREX.DLL is available and the AlertDrv.sys
+ driver is loaded into Windows, providing
+ alertable (pre-emptive) asyncronous threads
+ cancelation. If this feature returns FALSE
+ then the default async cancel scheme is in
+ use, which cannot cancel blocked threads.
+
+ Features may be Or'ed into the mask parameter, in which case
+ the routine returns TRUE if any of the Or'ed features would
+ return TRUE. At this stage it doesn't make sense to Or features
+ but it may some day.
+
+
+void *
+pthread_timechange_handler_np(void *)
+
+ To improve tolerance against operator or time service
+ initiated system clock changes.
+
+ This routine can be called by an application when it
+ receives a WM_TIMECHANGE message from the system. At
+ present it broadcasts all condition variables so that
+ waiting threads can wake up and re-evaluate their
+ conditions and restart their timed waits if required.
+
+ It has the same return type and argument type as a
+ thread routine so that it may be called directly
+ through pthread_create(), i.e. as a separate thread.
+
+ Parameters
+
+ Although a parameter must be supplied, it is ignored.
+ The value NULL can be used.
+
+ Return values
+
+ It can return an error EAGAIN to indicate that not
+ all condition variables were broadcast for some reason.
+ Otherwise, 0 is returned.
+
+ If run as a thread, the return value is returned
+ through pthread_join().
+
+ The return value should be cast to an integer.
+
+
+HANDLE
+pthread_getw32threadhandle_np(pthread_t thread);
+
+ Returns the win32 thread handle that the POSIX
+ thread "thread" is running as.
+
+ Applications can use the win32 handle to set
+ win32 specific attributes of the thread.
+
+DWORD
+pthread_getw32threadid_np (pthread_t thread)
+
+ Returns the Windows native thread ID that the POSIX
+ thread "thread" is running as.
+
+ Only valid when the library is built where
+ ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ and otherwise returns 0.
+
+
+int
+pthread_mutexattr_setkind_np(pthread_mutexattr_t * attr, int kind)
+
+int
+pthread_mutexattr_getkind_np(pthread_mutexattr_t * attr, int *kind)
+
+ These two routines are included for Linux compatibility
+ and are direct equivalents to the standard routines
+ pthread_mutexattr_settype
+ pthread_mutexattr_gettype
+
+ pthread_mutexattr_setkind_np accepts the following
+ mutex kinds:
+ PTHREAD_MUTEX_FAST_NP
+ PTHREAD_MUTEX_ERRORCHECK_NP
+ PTHREAD_MUTEX_RECURSIVE_NP
+
+ These are really just equivalent to (respectively):
+ PTHREAD_MUTEX_NORMAL
+ PTHREAD_MUTEX_ERRORCHECK
+ PTHREAD_MUTEX_RECURSIVE
+
+int
+pthread_delay_np (const struct timespec *interval);
+
+ This routine causes a thread to delay execution for a specific period of time.
+ This period ends at the current time plus the specified interval. The routine
+ will not return before the end of the period is reached, but may return an
+ arbitrary amount of time after the period has gone by. This can be due to
+ system load, thread priorities, and system timer granularity.
+
+ Specifying an interval of zero (0) seconds and zero (0) nanoseconds is
+ allowed and can be used to force the thread to give up the processor or to
+ deliver a pending cancelation request.
+
+ This routine is a cancelation point.
+
+ The timespec structure contains the following two fields:
+
+ tv_sec is an integer number of seconds.
+ tv_nsec is an integer number of nanoseconds.
+
+ Return Values
+
+ If an error condition occurs, this routine returns an integer value
+ indicating the type of error. Possible return values are as follows:
+
+ 0 Successful completion.
+ [EINVAL] The value specified by interval is invalid.
+
+int
+pthread_num_processors_np (void)
+
+ This routine (found on HPUX systems) returns the number of processors
+ in the system. This implementation actually returns the number of
+ processors available to the process, which can be a lower number
+ than the system's number, depending on the process's affinity mask.
+
+BOOL
+pthread_win32_process_attach_np (void);
+
+BOOL
+pthread_win32_process_detach_np (void);
+
+BOOL
+pthread_win32_thread_attach_np (void);
+
+BOOL
+pthread_win32_thread_detach_np (void);
+
+ These functions contain the code normally run via dllMain
+ when the library is used as a dll but which need to be
+ called explicitly by an application when the library
+ is statically linked. As of version 2.9.0 of the library, static
+ builds using either MSC or GCC will call pthread_win32_process_*
+ automatically at application startup and exit respectively.
+
+ Otherwise, you will need to call pthread_win32_process_attach_np()
+ before you can call any pthread routines when statically linking.
+ You should call pthread_win32_process_detach_np() before
+ exiting your application to clean up.
+
+ pthread_win32_thread_attach_np() is currently a no-op, but
+ pthread_win32_thread_detach_np() is needed to clean up
+ the implicit pthread handle that is allocated to a Win32 thread if
+ it calls any pthreads routines. Call this routine when the
+ Win32 thread exits.
+
+ Threads created through pthread_create() do not need to call
+ pthread_win32_thread_detach_np().
+
+ These functions invariably return TRUE except for
+ pthread_win32_process_attach_np() which will return FALSE
+ if pthreads-win32 initialisation fails.
+
+int
+pthreadCancelableWait (HANDLE waitHandle);
+
+int
+pthreadCancelableTimedWait (HANDLE waitHandle, DWORD timeout);
+
+ These two functions provide hooks into the pthread_cancel
+ mechanism that will allow you to wait on a Windows handle
+ and make it a cancellation point. Both functions block
+ until either the given w32 handle is signaled, or
+ pthread_cancel has been called. It is implemented using
+ WaitForMultipleObjects on 'waitHandle' and a manually
+ reset w32 event used to implement pthread_cancel.
+
+
+Non-portable issues
+-------------------
+
+Thread priority
+
+ POSIX defines a single contiguous range of numbers that determine a
+ thread's priority. Win32 defines priority classes and priority
+ levels relative to these classes. Classes are simply priority base
+ levels that the defined priority levels are relative to such that,
+ changing a process's priority class will change the priority of all
+ of it's threads, while the threads retain the same relativity to each
+ other.
+
+ A Win32 system defines a single contiguous monotonic range of values
+ that define system priority levels, just like POSIX. However, Win32
+ restricts individual threads to a subset of this range on a
+ per-process basis.
+
+ The following table shows the base priority levels for combinations
+ of priority class and priority value in Win32.
+
+ Process Priority Class Thread Priority Level
+ -----------------------------------------------------------------
+ 1 IDLE_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 1 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 1 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 1 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 1 HIGH_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 2 IDLE_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 3 IDLE_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 4 IDLE_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 4 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 5 IDLE_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 5 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 5 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 6 IDLE_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 6 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 6 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 7 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 7 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 7 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 8 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 8 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 8 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 8 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 9 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 9 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 9 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 10 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 10 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 11 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 11 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 11 HIGH_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 12 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 12 HIGH_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 13 HIGH_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 14 HIGH_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ 15 IDLE_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ 15 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ 15 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ 15 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ 16 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ 17 REALTIME_PRIORITY_CLASS -7
+ 18 REALTIME_PRIORITY_CLASS -6
+ 19 REALTIME_PRIORITY_CLASS -5
+ 20 REALTIME_PRIORITY_CLASS -4
+ 21 REALTIME_PRIORITY_CLASS -3
+ 22 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ 23 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ 24 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ 25 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ 26 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ 27 REALTIME_PRIORITY_CLASS 3
+ 28 REALTIME_PRIORITY_CLASS 4
+ 29 REALTIME_PRIORITY_CLASS 5
+ 30 REALTIME_PRIORITY_CLASS 6
+ 31 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+
+ Windows NT: Values -7, -6, -5, -4, -3, 3, 4, 5, and 6 are not supported.
+
+
+ As you can see, the real priority levels available to any individual
+ Win32 thread are non-contiguous.
+
+ An application using pthreads-win32 should not make assumptions about
+ the numbers used to represent thread priority levels, except that they
+ are monotonic between the values returned by sched_get_priority_min()
+ and sched_get_priority_max(). E.g. Windows 95, 98, NT, 2000, XP make
+ available a non-contiguous range of numbers between -15 and 15, while
+ at least one version of WinCE (3.0) defines the minimum priority
+ (THREAD_PRIORITY_LOWEST) as 5, and the maximum priority
+ (THREAD_PRIORITY_HIGHEST) as 1.
+
+ Internally, pthreads-win32 maps any priority levels between
+ THREAD_PRIORITY_IDLE and THREAD_PRIORITY_LOWEST to THREAD_PRIORITY_LOWEST,
+ or between THREAD_PRIORITY_TIME_CRITICAL and THREAD_PRIORITY_HIGHEST to
+ THREAD_PRIORITY_HIGHEST. Currently, this also applies to
+ REALTIME_PRIORITY_CLASSi even if levels -7, -6, -5, -4, -3, 3, 4, 5, and 6
+ are supported.
+
+ If it wishes, a Win32 application using pthreads-win32 can use the Win32
+ defined priority macros THREAD_PRIORITY_IDLE through
+ THREAD_PRIORITY_TIME_CRITICAL.
+
+
+The opacity of the pthread_t datatype
+-------------------------------------
+and possible solutions for portable null/compare/hash, etc
+----------------------------------------------------------
+
+Because pthread_t is an opague datatype an implementation is permitted to define
+pthread_t in any way it wishes. That includes defining some bits, if it is
+scalar, or members, if it is an aggregate, to store information that may be
+extra to the unique identifying value of the ID. As a result, pthread_t values
+may not be directly comparable.
+
+If you want your code to be portable you must adhere to the following contraints:
+
+1) Don't assume it is a scalar data type, e.g. an integer or pointer value. There
+are several other implementations where pthread_t is also a struct. See our FAQ
+Question 11 for our reasons for defining pthread_t as a struct.
+
+2) You must not compare them using relational or equality operators. You must use
+the API function pthread_equal() to test for equality.
+
+3) Never attempt to reference individual members.
+
+
+The problem
+
+Certain applications would like to be able to access only the 'pure' pthread_t
+id values, primarily to use as keys into data structures to manage threads or
+thread-related data, but this is not possible in a maximally portable and
+standards compliant way for current POSIX threads implementations.
+
+For implementations that define pthread_t as a scalar, programmers often employ
+direct relational and equality operators on pthread_t. This code will break when
+ported to an implementation that defines pthread_t as an aggregate type.
+
+For implementations that define pthread_t as an aggregate, e.g. a struct,
+programmers can use memcmp etc., but then face the prospect that the struct may
+include alignment padding bytes or bits as well as extra implementation-specific
+members that are not part of the unique identifying value.
+
+[While this is not currently the case for pthreads-win32, opacity also
+means that an implementation is free to change the definition, which should
+generally only require that applications be recompiled and relinked, not
+rewritten.]
+
+
+Doesn't the compiler take care of padding?
+
+The C89 and later standards only effectively guarrantee element-by-element
+equivalence following an assignment or pass by value of a struct or union,
+therefore undefined areas of any two otherwise equivalent pthread_t instances
+can still compare differently, e.g. attempting to compare two such pthread_t
+variables byte-by-byte, e.g. memcmp(&t1, &t2, sizeof(pthread_t) may give an
+incorrect result. In practice I'm reasonably confident that compilers routinely
+also copy the padding bytes, mainly because assignment of unions would be far
+too complicated otherwise. But it just isn't guarranteed by the standard.
+
+Illustration:
+
+We have two thread IDs t1 and t2
+
+pthread_t t1, t2;
+
+In an application we create the threads and intend to store the thread IDs in an
+ordered data structure (linked list, tree, etc) so we need to be able to compare
+them in order to insert them initially and also to traverse.
+
+Suppose pthread_t contains undefined padding bits and our compiler copies our
+pthread_t [struct] element-by-element, then for the assignment:
+
+pthread_t temp = t1;
+
+temp and t1 will be equivalent and correct but a byte-for-byte comparison such as
+memcmp(&temp, &t1, sizeof(pthread_t)) == 0 may not return true as we expect because
+the undefined bits may not have the same values in the two variable instances.
+
+Similarly if passing by value under the same conditions.
+
+If, on the other hand, the undefined bits are at least constant through every
+assignment and pass-by-value then the byte-for-byte comparison
+memcmp(&temp, &t1, sizeof(pthread_t)) == 0 will always return the expected result.
+How can we force the behaviour we need?
+
+
+Solutions
+
+Adding new functions to the standard API or as non-portable extentions is
+the only reliable and portable way to provide the necessary operations.
+Remember also that POSIX is not tied to the C language. The most common
+functions that have been suggested are:
+
+pthread_null()
+pthread_compare()
+pthread_hash()
+
+A single more general purpose function could also be defined as a
+basis for at least the last two of the above functions.
+
+First we need to list the freedoms and constraints with restpect
+to pthread_t so that we can be sure our solution is compatible with the
+standard.
+
+What is known or may be deduced from the standard:
+1) pthread_t must be able to be passed by value, so it must be a single object.
+2) from (1) it must be copyable so cannot embed thread-state information, locks
+or other volatile objects required to manage the thread it associates with.
+3) pthread_t may carry additional information, e.g. for debugging or to manage
+itself.
+4) there is an implicit requirement that the size of pthread_t is determinable
+at compile-time and size-invariant, because it must be able to copy the object
+(i.e. through assignment and pass-by-value). Such copies must be genuine
+duplicates, not merely a copy of a pointer to a common instance such as
+would be the case if pthread_t were defined as an array.
+
+
+Suppose we define the following function:
+
+/* This function shall return it's argument */
+pthread_t* pthread_normalize(pthread_t* thread);
+
+For scalar or aggregate pthread_t types this function would simply zero any bits
+within the pthread_t that don't uniquely identify the thread, including padding,
+such that client code can return consistent results from operations done on the
+result. If the additional bits are a pointer to an associate structure then
+this function would ensure that the memory used to store that associate
+structure does not leak. After normalization the following compare would be
+valid and repeatable:
+
+memcmp(pthread_normalize(&t1),pthread_normalize(&t2),sizeof(pthread_t))
+
+Note 1: such comparisons are intended merely to order and sort pthread_t values
+and allow them to index various data structures. They are not intended to reveal
+anything about the relationships between threads, like startup order.
+
+Note 2: the normalized pthread_t is also a valid pthread_t that uniquely
+identifies the same thread.
+
+Advantages:
+1) In most existing implementations this function would reduce to a no-op that
+emits no additional instructions, i.e after in-lining or optimisation, or if
+defined as a macro:
+#define pthread_normalise(tptr) (tptr)
+
+2) This single function allows an application to portably derive
+application-level versions of any of the other required functions.
+
+3) It is a generic function that could enable unanticipated uses.
+
+Disadvantages:
+1) Less efficient than dedicated compare or hash functions for implementations
+that include significant extra non-id elements in pthread_t.
+
+2) Still need to be concerned about padding if copying normalized pthread_t.
+See the later section on defining pthread_t to neutralise padding issues.
+
+Generally a pthread_t may need to be normalized every time it is used,
+which could have a significant impact. However, this is a design decision
+for the implementor in a competitive environment. An implementation is free
+to define a pthread_t in a way that minimises or eliminates padding or
+renders this function a no-op.
+
+Hazards:
+1) Pass-by-reference directly modifies 'thread' so the application must
+synchronise access or ensure that the pointer refers to a copy. The alternative
+of pass-by-value/return-by-value was considered but then this requires two copy
+operations, disadvantaging implementations where this function is not a no-op
+in terms of speed of execution. This function is intended to be used in high
+frequency situations and needs to be efficient, or at least not unnecessarily
+inefficient. The alternative also sits awkwardly with functions like memcmp.
+
+2) [Non-compliant] code that uses relational and equality operators on
+arithmetic or pointer style pthread_t types would need to be rewritten, but it
+should be rewritten anyway.
+
+
+C implementation of null/compare/hash functions using pthread_normalize():
+
+/* In pthread.h */
+pthread_t* pthread_normalize(pthread_t* thread);
+
+/* In user code */
+/* User-level bitclear function - clear bits in loc corresponding to mask */
+void* bitclear (void* loc, void* mask, size_t count);
+
+typedef unsigned int hash_t;
+
+/* User-level hash function */
+hash_t hash(void* ptr, size_t count);
+
+/*
+ * User-level pthr_null function - modifies the origin thread handle.
+ * The concept of a null pthread_t is highly implementation dependent
+ * and this design may be far from the mark. For example, in an
+ * implementation "null" may mean setting a special value inside one
+ * element of pthread_t to mean "INVALID". However, if that value was zero and
+ * formed part of the id component then we may get away with this design.
+ */
+pthread_t* pthr_null(pthread_t* tp)
+{
+ /*
+ * This should have the same effect as memset(tp, 0, sizeof(pthread_t))
+ * We're just showing that we can do it.
+ */
+ void* p = (void*) pthread_normalize(tp);
+ return (pthread_t*) bitclear(p, p, sizeof(pthread_t));
+}
+
+/*
+ * Safe user-level pthr_compare function - modifies temporary thread handle copies
+ */
+int pthr_compare_safe(pthread_t thread1, pthread_t thread2)
+{
+ return memcmp(pthread_normalize(&thread1), pthread_normalize(&thread2), sizeof(pthread_t));
+}
+
+/*
+ * Fast user-level pthr_compare function - modifies origin thread handles
+ */
+int pthr_compare_fast(pthread_t* thread1, pthread_t* thread2)
+{
+ return memcmp(pthread_normalize(&thread1), pthread_normalize(&thread2), sizeof(pthread_t));
+}
+
+/*
+ * Safe user-level pthr_hash function - modifies temporary thread handle copy
+ */
+hash_t pthr_hash_safe(pthread_t thread)
+{
+ return hash((void *) pthread_normalize(&thread), sizeof(pthread_t));
+}
+
+/*
+ * Fast user-level pthr_hash function - modifies origin thread handle
+ */
+hash_t pthr_hash_fast(pthread_t thread)
+{
+ return hash((void *) pthread_normalize(&thread), sizeof(pthread_t));
+}
+
+/* User-level bitclear function - modifies the origin array */
+void* bitclear(void* loc, void* mask, size_t count)
+{
+ int i;
+ for (i=0; i < count; i++) {
+ (unsigned char) *loc++ &= ~((unsigned char) *mask++);
+ }
+}
+
+/* Donald Knuth hash */
+hash_t hash(void* str, size_t count)
+{
+ hash_t hash = (hash_t) count;
+ unsigned int i = 0;
+
+ for(i = 0; i < len; str++, i++)
+ {
+ hash = ((hash << 5) ^ (hash >> 27)) ^ (*str);
+ }
+ return hash;
+}
+
+/* Example of advantage point (3) - split a thread handle into its id and non-id values */
+pthread_t id = thread, non-id = thread;
+bitclear((void*) &non-id, (void*) pthread_normalize(&id), sizeof(pthread_t));
+
+
+A pthread_t type change proposal to neutralise the effects of padding
+
+Even if pthread_nornalize() is available, padding is still a problem because
+the standard only garrantees element-by-element equivalence through
+copy operations (assignment and pass-by-value). So padding bit values can
+still change randomly after calls to pthread_normalize().
+
+[I suspect that most compilers take the easy path and always byte-copy anyway,
+partly because it becomes too complex to do (e.g. unions that contain sub-aggregates)
+but also because programmers can easily design their aggregates to minimise and
+often eliminate padding].
+
+How can we eliminate the problem of padding bytes in structs? Could
+defining pthread_t as a union rather than a struct provide a solution?
+
+In fact, the Linux pthread.h defines most of it's pthread_*_t objects (but not
+pthread_t itself) as unions, possibly for this and/or other reasons. We'll
+borrow some element naming from there but the ideas themselves are well known
+- the __align element used to force alignment of the union comes from K&R's
+storage allocator example.
+
+/* Essentially our current pthread_t renamed */
+typedef struct {
+ struct thread_state_t * __p;
+ long __x; /* sequence counter */
+} thread_id_t;
+
+Ensuring that the last element in the above struct is a long ensures that the
+overall struct size is a multiple of sizeof(long), so there should be no trailing
+padding in this struct or the union we define below.
+(Later we'll see that we can handle internal but not trailing padding.)
+
+/* New pthread_t */
+typedef union {
+ char __size[sizeof(thread_id_t)]; /* array as the first element */
+ thread_id_t __tid;
+ long __align; /* Ensure that the union starts on long boundary */
+} pthread_t;
+
+This guarrantees that, during an assignment or pass-by-value, the compiler copies
+every byte in our thread_id_t because the compiler guarrantees that the __size
+array, which we have ensured is the equal-largest element in the union, retains
+equivalence.
+
+This means that pthread_t values stored, assigned and passed by value will at least
+carry the value of any undefined padding bytes along and therefore ensure that
+those values remain consistent. Our comparisons will return consistent results and
+our hashes of [zero initialised] pthread_t values will also return consistent
+results.
+
+We have also removed the need for a pthread_null() function; we can initialise
+at declaration time or easily create our own const pthread_t to use in assignments
+later:
+
+const pthread_t null_tid = {0}; /* braces are required */
+
+pthread_t t;
+...
+t = null_tid;
+
+
+Note that we don't have to explicitly make use of the __size array at all. It's
+there just to force the compiler behaviour we want.
+
+
+Partial solutions without a pthread_normalize function
+
+
+An application-level pthread_null and pthread_compare proposal
+(and pthread_hash proposal by extention)
+
+In order to deal with the problem of scalar/aggregate pthread_t type disparity in
+portable code I suggest using an old-fashioned union, e.g.:
+
+Contraints:
+- there is no padding, or padding values are preserved through assignment and
+ pass-by-value (see above);
+- there are no extra non-id values in the pthread_t.
+
+
+Example 1: A null initialiser for pthread_t variables...
+
+typedef union {
+ unsigned char b[sizeof(pthread_t)];
+ pthread_t t;
+} init_t;
+
+const init_t initial = {0};
+
+pthread_t tid = initial.t; /* init tid to all zeroes */
+
+
+Example 2: A comparison function for pthread_t values
+
+typedef union {
+ unsigned char b[sizeof(pthread_t)];
+ pthread_t t;
+} pthcmp_t;
+
+int pthcmp(pthread_t left, pthread_t right)
+{
+ /*
+ * Compare two pthread handles in a way that imposes a repeatable but arbitrary
+ * ordering on them.
+ * I.e. given the same set of pthread_t handles the ordering should be the same
+ * each time but the order has no particular meaning other than that. E.g.
+ * the ordering does not imply the thread start sequence, or any other
+ * relationship between threads.
+ *
+ * Return values are:
+ * 1 : left is greater than right
+ * 0 : left is equal to right
+ * -1 : left is less than right
+ */
+ int i;
+ pthcmp_t L, R;
+ L.t = left;
+ R.t = right;
+ for (i = 0; i < sizeof(pthread_t); i++)
+ {
+ if (L.b[i] > R.b[i])
+ return 1;
+ else if (L.b[i] < R.b[i])
+ return -1;
+ }
+ return 0;
+}
+
+It has been pointed out that the C99 standard allows for the possibility that
+integer types also may include padding bits, which could invalidate the above
+method. This addition to C99 was specifically included after it was pointed
+out that there was one, presumably not particularly well known, architecture
+that included a padding bit in it's 32 bit integer type. See section 6.2.6.2
+of both the standard and the rationale, specifically the paragraph starting at
+line 16 on page 43 of the rationale.
+
+
+An aside
+
+Certain compilers, e.g. gcc and one of the IBM compilers, include a feature
+extention: provided the union contains a member of the same type as the
+object then the object may be cast to the union itself.
+
+We could use this feature to speed up the pthrcmp() function from example 2
+above by casting rather than assigning the pthread_t arguments to the union, e.g.:
+
+int pthcmp(pthread_t left, pthread_t right)
+{
+ /*
+ * Compare two pthread handles in a way that imposes a repeatable but arbitrary
+ * ordering on them.
+ * I.e. given the same set of pthread_t handles the ordering should be the same
+ * each time but the order has no particular meaning other than that. E.g.
+ * the ordering does not imply the thread start sequence, or any other
+ * relationship between threads.
+ *
+ * Return values are:
+ * 1 : left is greater than right
+ * 0 : left is equal to right
+ * -1 : left is less than right
+ */
+ int i;
+ for (i = 0; i < sizeof(pthread_t); i++)
+ {
+ if (((pthcmp_t)left).b[i] > ((pthcmp_t)right).b[i])
+ return 1;
+ else if (((pthcmp_t)left).b[i] < ((pthcmp_t)right).b[i])
+ return -1;
+ }
+ return 0;
+}
+
+
+Result thus far
+
+We can't remove undefined bits if they are there in pthread_t already, but we have
+attempted to render them inert for comparison and hashing functions by making them
+consistent through assignment, copy and pass-by-value.
+
+Note: Hashing pthread_t values requires that all pthread_t variables be initialised
+to the same value (usually all zeros) before being assigned a proper thread ID, i.e.
+to ensure that any padding bits are zero, or at least the same value for all
+pthread_t. Since all pthread_t values are generated by the library in the first
+instance this need not be an application-level operation.
+
+
+Conclusion
+
+I've attempted to resolve the multiple issues of type opacity and the possible
+presence of undefined bits and bytes in pthread_t values, which prevent
+applications from comparing or hashing pthread handles.
+
+Two complimentary partial solutions have been proposed, one an application-level
+scheme to handle both scalar and aggregate pthread_t types equally, plus a
+definition of pthread_t itself that neutralises padding bits and bytes by
+coercing semantics out of the compiler to eliminate variations in the values of
+padding bits.
+
+I have not provided any solution to the problem of handling extra values embedded
+in pthread_t, e.g. debugging or trap information that an implementation is entitled
+to include. Therefore none of this replaces the portability and flexibility of API
+functions but what functions are needed? The threads standard is unlikely to
+include that can be implemented by a combination of existing features and more
+generic functions (several references in the threads rationale suggest this.
+Therefore I propose that the following function could replace the several functions
+that have been suggested in conversations:
+
+pthread_t * pthread_normalize(pthread_t * handle);
+
+For most existing pthreads implementations this function, or macro, would reduce to
+a no-op with zero call overhead.
diff --git a/3rdparty/pthreads-win32/README.ViSP b/3rdparty/pthreads-win32/README.ViSP
new file mode 100644
index 0000000..351fd18
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.ViSP
@@ -0,0 +1,6 @@
+Pthreads-win32 downloaded from https://www.sourceware.org/pthreads-win32/
+Version: 2.9.1 (2012-05-27)
+Download link: ftp://sourceware.org/pub/pthreads-win32/pthreads-w32-2-9-1-release.zip
+
+Windows UWP porting (MSVC):
+ - remove functions that are using missing win32 functions not available on UWP (see #ifdef WINRT and #ifndef WINRT)
diff --git a/3rdparty/pthreads-win32/README.Watcom b/3rdparty/pthreads-win32/README.Watcom
new file mode 100644
index 0000000..2974928
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.Watcom
@@ -0,0 +1,62 @@
+Watcom compiler notes
+=====================
+
+Status
+------
+Not yet usable. Although the library builds under Watcom it
+substantially fails the test suite.
+
+There is a working Wmakefile for wmake for the library build.
+
+invoke as any of:
+wmake -f Wmakefile clean WC
+wmake -f Wmakefile clean WC-inlined
+wmake -f Wmakefile clean WCE
+wmake -f Wmakefile clean WCE-inlined
+
+These build pthreadWC.dll and pthreadWCE.dll.
+
+There is a working Wmakefile for wmake for the test suite.
+
+invoke as any of:
+wmake -f Wmakefile clean WC
+wmake -f Wmakefile clean WCX
+wmake -f Wmakefile clean WCE
+wmake -f Wmakefile clean WC-bench
+wmake -f Wmakefile clean WCX-bench
+wmake -f Wmakefile clean WCE-bench
+
+
+Current known problems
+----------------------
+
+Library build:
+The Watcom compiler uses a different default call convention to MS C or GNU C and so
+applications are not compatible with pthreadVC.dll etc using pre 2003-10-14 versions
+of pthread.h, sched.h, or semaphore.h. The cdecl attribute can be used on exposed
+function prototypes to force compatibility with MS C built DLLs.
+
+However, there appear to be other incompatibilities. Errno.h, for example, defines
+different values for the standard C and POSIX errors to those defined by the MS C
+errno.h. It may be that references to Watcom's threads compatible 'errno' do set
+and return translated numbers consistently, but I have not verified this.
+
+Watcom defines errno as a dereferenced pointer returned by the function
+_get_errno_ptr(). This is similar to both the MS and GNU C environments for
+multithreaded use. However, the Watcom version appears to have a number of problems:
+
+- different threads return the same pointer value. Compare with the MS and GNU C
+versions which correctly return different values (since each thread must maintain
+a thread specific errno value).
+
+- an errno value set within the DLL appears as zero in the application even though
+both share the same thread.
+
+Therefore applications built using the Watcom compiler may need to use
+a Watcom built version of the library (pthreadWC.dll). If this is the case, then
+the cdecl function attribute should not be required.
+
+Application builds:
+The test suite fails with the Watcom compiler.
+
+Test semaphore1.c fails for pthreadWC.dll because errno returns 0 instead of EAGAIN.
diff --git a/3rdparty/pthreads-win32/README.WinCE b/3rdparty/pthreads-win32/README.WinCE
new file mode 100644
index 0000000..a2cd8c2
--- /dev/null
+++ b/3rdparty/pthreads-win32/README.WinCE
@@ -0,0 +1,6 @@
+WinCE port
+----------
+(See the file WinCE-PORT for a detailed explanation.)
+
+Make sure you define "WINCE" amongst your compiler flags (eg. -DWINCE).
+The config.h file will define all the necessary defines for you.
diff --git a/3rdparty/pthreads-win32/TODO b/3rdparty/pthreads-win32/TODO
new file mode 100644
index 0000000..fa9efc4
--- /dev/null
+++ b/3rdparty/pthreads-win32/TODO
@@ -0,0 +1,7 @@
+ Things that aren't done yet
+ ---------------------------
+
+1. Implement PTHREAD_PROCESS_SHARED for semaphores, mutexes,
+ condition variables, read/write locks, barriers.
+
+
diff --git a/3rdparty/pthreads-win32/WinCE-PORT b/3rdparty/pthreads-win32/WinCE-PORT
new file mode 100644
index 0000000..7bcfdea
--- /dev/null
+++ b/3rdparty/pthreads-win32/WinCE-PORT
@@ -0,0 +1,222 @@
+NOTE: The comments in this file relate to the original WinCE port
+done by Tristan Savatier. The semaphore routines have been
+completely rewritten since (2005-04-25), having been progressively
+broken more and more by changes to the library. All of the semaphore
+routines implemented for W9x/WNT/2000 and up should now also work for
+WinCE. Also, pthread_mutex_timedlock should now work.
+
+Additional WinCE updates have been applied since this as well. Check the
+ChangeLog file and search for WINCE for example. (2007-01-07)
+
+[RPJ]
+
+----
+
+Some interesting news:
+
+I have been able to port pthread-win32 to Windows-CE,
+which uses a subset of the WIN32 API.
+
+Since we intend to keep using pthread-win32 for our
+Commercial WinCE developments, I would be very interested
+if WinCE support could be added to the main source tree
+of pthread-win32. Also, I would like to be credited
+for this port :-)
+
+Now, here is the story...
+
+The port was performed and tested on a Casio "Cassiopeia"
+PalmSize PC, which runs a MIP processor. The OS in the
+Casio is WinCE version 2.11, but I used VC++ 6.0 with
+the WinCE SDK for version 2.01.
+
+I used pthread-win32 to port a heavily multithreaded
+commercial application (real-time MPEG video player)
+from Linux to WinCE. I consider the changes that
+I have done to be quite well tested.
+
+Overall the modifications that we had to do are minor.
+
+The WinCE port were based on pthread-win32-snap-1999-05-30,
+but I am certain that they can be integrated very easiely
+to more recent versions of the source.
+
+I have attached the modified source code:
+pthread-win32-snap-1999-05-30-WinCE.
+
+All the changes do not affect the code compiled on non-WinCE
+environment, provided that the macros used for WinCE compilation
+are not used, of course!
+
+Overall description of the WinCE port:
+-------------------------------------
+
+Most of the changes had to be made in areas where
+pthread-win32 was relying on some standard-C librairies
+(e.g. _ftime, calloc, errno), which are not available
+on WinCE. We have changed the code to use native Win32
+API instead (or in some cases we made wrappers).
+
+The Win32 Semaphores are not available,
+so we had to re-implement Semaphores using mutexes
+and events.
+
+Limitations / known problems of the WinCE port:
+----------------------------------------------
+
+Not all the semaphore routines have been ported
+(semaphores are defined by Posix but are not part
+pf pthread). I have just done enough to make
+pthread routines (that rely internally on semaphores)
+work, like signal conditions.
+
+I noticed that the Win32 threads work slightly
+differently on WinCE. This may have some impact
+on some tricky parts of pthread-win32, but I have
+not really investigated. For example, on WinCE,
+the process is killed if the main thread falls off
+the bottom (or calls pthread_exit), regardless
+of the existence of any other detached thread.
+Microsoft manual indicates that this behavior is
+deffirent from that of Windows Threads for other
+Win32 platforms.
+
+
+Detailed descriptions of the changes and rationals:
+
+------------------------------------
+- use a new macro NEED_ERRNO.
+
+If defined, the code in errno.c that defines a reentrant errno
+is compiled, regardless of _MT and _REENTRANT.
+
+Rational: On WinCE, there is no support for <stdio.h>, <errno.h> or
+any other standard C library, i.e. even if _MT or _REENTRANT
+is defined, errno is not provided by any library. NEED_ERRNO
+must be set to compile for WinCE.
+
+------------------------------------
+- In implement.h, change #include <semaphore.h> to #include "semaphore.h".
+
+Rational: semaphore.h is provided in pthread-win32 and should not
+be searched in the systems standard include. would not compile.
+This change does not seem to create problems on "classic" win32
+(e.g. win95).
+
+------------------------------------
+- use a new macro NEED_CALLOC.
+
+If defined, some code in misc.c will provide a replacement
+for calloc, which is not available on Win32.
+
+
+------------------------------------
+- use a new macro NEED_CREATETHREAD.
+
+If defined, implement.h defines the macro _beginthreadex
+and _endthreadex.
+
+Rational: On WinCE, the wrappers _beginthreadex and _endthreadex
+do not exist. The native Win32 routines must be used.
+
+------------------------------------
+- in misc.c:
+
+#ifdef NEED_DUPLICATEHANDLE
+ /* DuplicateHandle does not exist on WinCE */
+ self->threadH = GetCurrentThread();
+#else
+ if( !DuplicateHandle(
+ GetCurrentProcess(),
+ GetCurrentThread(),
+ GetCurrentProcess(),
+ &self->threadH,
+ 0,
+ FALSE,
+ DUPLICATE_SAME_ACCESS ) )
+ {
+ free( self );
+ return (NULL);
+ }
+#endif
+
+Rational: On WinCE, DuplicateHandle does not exist. I could not understand
+why DuplicateHandle must be used. It seems to me that getting the current
+thread handle with GetCurrentThread() is sufficient, and it seems to work
+perfectly fine, so maybe DuplicateHandle was just plain useless to begin with ?
+
+------------------------------------
+- In private.c, added some code at the beginning of ptw32_processInitialize
+to detect the case of multiple calls to ptw32_processInitialize.
+
+Rational: In order to debug pthread-win32, it is easier to compile
+it as a regular library (it is not possible to debug DLL's on winCE).
+In that case, the application must call ptw32_rocessInitialize()
+explicitely, to initialize pthread-win32. It is safer in this circumstance
+to handle the case where ptw32_processInitialize() is called on
+an already initialized library:
+
+int
+ptw32_processInitialize (void)
+{
+ if (ptw32_processInitialized) {
+ /*
+ * ignore if already initialized. this is useful for
+ * programs that uses a non-dll pthread
+ * library. such programs must call ptw32_processInitialize() explicitely,
+ * since this initialization routine is automatically called only when
+ * the dll is loaded.
+ */
+ return TRUE;
+ }
+ ptw32_processInitialized = TRUE;
+ [...]
+}
+
+------------------------------------
+- in private.c, if macro NEED_FTIME is defined, add routines to
+convert timespec_to_filetime and filetime_to_timespec, and modified
+code that was using _ftime() to use Win32 API instead.
+
+Rational: _ftime is not available on WinCE. It is necessary to use
+the native Win32 time API instead.
+
+Note: the routine timespec_to_filetime is provided as a convenience and a mean
+to test that filetime_to_timespec works, but it is not used by the library.
+
+------------------------------------
+- in semaphore.c, if macro NEED_SEM is defined, add code for the routines
+_increase_semaphore and _decrease_semaphore, and modify significantly
+the implementation of the semaphores so that it does not use CreateSemaphore.
+
+Rational: CreateSemaphore is not available on WinCE. I had to re-implement
+semaphores using mutexes and Events.
+
+Note: Only the semaphore routines that are used by pthread are implemented
+(i.e. signal conditions rely on a subset of the semaphores routines, and
+this subset works). Some other semaphore routines (e.g. sem_trywait) are
+not yet supported on my WinCE port (and since I don't need them, I am not
+planning to do anything about them).
+
+------------------------------------
+- in tsd.c, changed the code that defines TLS_OUT_OF_INDEXES
+
+/* TLS_OUT_OF_INDEXES not defined on WinCE */
+#ifndef TLS_OUT_OF_INDEXES
+#define TLS_OUT_OF_INDEXES 0xffffffff
+#endif
+
+Rational: TLS_OUT_OF_INDEXES is not defined in any standard include file
+on WinCE.
+
+------------------------------------
+- added file need_errno.h
+
+Rational: On WinCE, there is no errno.h file. need_errno.h is just a
+copy of windows version of errno.h, with minor modifications due to the fact
+that some of the error codes are defined by the WinCE socket library.
+In pthread.h, if NEED_ERRNO is defined, the file need_errno.h is
+included (instead of <errno.h>).
+
+
+-- eof
diff --git a/3rdparty/pthreads-win32/attr.c b/3rdparty/pthreads-win32/attr.c
new file mode 100644
index 0000000..a9d55f4
--- /dev/null
+++ b/3rdparty/pthreads-win32/attr.c
@@ -0,0 +1,53 @@
+/*
+ * attr.c
+ *
+ * Description:
+ * This translation unit agregates operations on thread attribute objects.
+ * It is used for inline optimisation.
+ *
+ * The included modules are used separately when static executable sizes
+ * must be minimised.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#include "pthread_attr_init.c"
+#include "pthread_attr_destroy.c"
+#include "pthread_attr_getdetachstate.c"
+#include "pthread_attr_setdetachstate.c"
+#include "pthread_attr_getstackaddr.c"
+#include "pthread_attr_setstackaddr.c"
+#include "pthread_attr_getstacksize.c"
+#include "pthread_attr_setstacksize.c"
+#include "pthread_attr_getscope.c"
+#include "pthread_attr_setscope.c"
diff --git a/3rdparty/pthreads-win32/autostatic.c b/3rdparty/pthreads-win32/autostatic.c
new file mode 100644
index 0000000..092aff2
--- /dev/null
+++ b/3rdparty/pthreads-win32/autostatic.c
@@ -0,0 +1,69 @@
+/*
+ * autostatic.c
+ *
+ * Description:
+ * This translation unit implements static auto-init and auto-exit logic.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if defined(PTW32_STATIC_LIB)
+
+#if defined(__MINGW64__) || defined(__MINGW32__) || defined(_MSC_VER)
+
+#include "pthread.h"
+#include "implement.h"
+
+static void on_process_init(void)
+{
+ pthread_win32_process_attach_np ();
+}
+
+static void on_process_exit(void)
+{
+ pthread_win32_thread_detach_np ();
+ pthread_win32_process_detach_np ();
+}
+
+#if defined(__MINGW64__) || defined(__MINGW32__)
+# define attribute_section(a) __attribute__((section(a)))
+#elif defined(_MSC_VER)
+# define attribute_section(a) __pragma(section(a,long,read)); __declspec(allocate(a))
+#endif
+
+attribute_section(".ctors") void *gcc_ctor = on_process_init;
+attribute_section(".dtors") void *gcc_dtor = on_process_exit;
+
+attribute_section(".CRT$XCU") void *msc_ctor = on_process_init;
+attribute_section(".CRT$XPU") void *msc_dtor = on_process_exit;
+
+#endif /* defined(__MINGW64__) || defined(__MINGW32__) || defined(_MSC_VER) */
+
+#endif /* PTW32_STATIC_LIB */
diff --git a/3rdparty/pthreads-win32/barrier.c b/3rdparty/pthreads-win32/barrier.c
new file mode 100644
index 0000000..41b950c
--- /dev/null
+++ b/3rdparty/pthreads-win32/barrier.c
@@ -0,0 +1,47 @@
+/*
+ * barrier.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "pthread_barrier_init.c"
+#include "pthread_barrier_destroy.c"
+#include "pthread_barrier_wait.c"
+#include "pthread_barrierattr_init.c"
+#include "pthread_barrierattr_destroy.c"
+#include "pthread_barrierattr_getpshared.c"
+#include "pthread_barrierattr_setpshared.c"
diff --git a/3rdparty/pthreads-win32/builddmc.bat b/3rdparty/pthreads-win32/builddmc.bat
new file mode 100644
index 0000000..bf813d8
--- /dev/null
+++ b/3rdparty/pthreads-win32/builddmc.bat
@@ -0,0 +1,9 @@
+; Build the pthreads library with the Digital Mars Compiler
+;
+set DMCDIR=c:\dm
+
+; RELEASE
+%DMCDIR%\bin\dmc -D_WIN32_WINNT -D_MT -DHAVE_PTW32_CONFIG_H -I.;c:\dm\include -o+all -WD pthread.c user32.lib+kernel32.lib+wsock32.lib -L/impl -L/NODEBUG -L/SU:WINDOWS
+
+; DEBUG
+%DMCDIR%\bin\dmc -g -D_WIN32_WINNT -D_MT -DHAVE_PTW32_CONFIG_H -I.;c:\dm\include -o+all -WD pthread.c user32.lib+kernel32.lib+wsock32.lib -L/impl -L/SU:WINDOWS
diff --git a/3rdparty/pthreads-win32/cancel.c b/3rdparty/pthreads-win32/cancel.c
new file mode 100644
index 0000000..1bd14eb
--- /dev/null
+++ b/3rdparty/pthreads-win32/cancel.c
@@ -0,0 +1,44 @@
+/*
+ * cancel.c
+ *
+ * Description:
+ * POSIX thread functions related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "pthread_setcancelstate.c"
+#include "pthread_setcanceltype.c"
+#include "pthread_testcancel.c"
+#include "pthread_cancel.c"
diff --git a/3rdparty/pthreads-win32/cleanup.c b/3rdparty/pthreads-win32/cleanup.c
new file mode 100644
index 0000000..381d1e8
--- /dev/null
+++ b/3rdparty/pthreads-win32/cleanup.c
@@ -0,0 +1,148 @@
+/*
+ * cleanup.c
+ *
+ * Description:
+ * This translation unit implements routines associated
+ * with cleaning up threads.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+/*
+ * The functions ptw32_pop_cleanup and ptw32_push_cleanup
+ * are implemented here for applications written in C with no
+ * SEH or C++ destructor support.
+ */
+
+ptw32_cleanup_t *
+ptw32_pop_cleanup (int execute)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function pops the most recently pushed cleanup
+ * handler. If execute is nonzero, then the cleanup handler
+ * is executed if non-null.
+ *
+ * PARAMETERS
+ * execute
+ * if nonzero, execute the cleanup handler
+ *
+ *
+ * DESCRIPTION
+ * This function pops the most recently pushed cleanup
+ * handler. If execute is nonzero, then the cleanup handler
+ * is executed if non-null.
+ * NOTE: specify 'execute' as nonzero to avoid duplication
+ * of common cleanup code.
+ *
+ * RESULTS
+ * N/A
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_cleanup_t *cleanup;
+
+ cleanup = (ptw32_cleanup_t *) pthread_getspecific (ptw32_cleanupKey);
+
+ if (cleanup != NULL)
+ {
+ if (execute && (cleanup->routine != NULL))
+ {
+
+ (*cleanup->routine) (cleanup->arg);
+
+ }
+
+ pthread_setspecific (ptw32_cleanupKey, (void *) cleanup->prev);
+
+ }
+
+ return (cleanup);
+
+} /* ptw32_pop_cleanup */
+
+
+void
+ptw32_push_cleanup (ptw32_cleanup_t * cleanup,
+ ptw32_cleanup_callback_t routine, void *arg)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function pushes a new cleanup handler onto the thread's stack
+ * of cleanup handlers. Each cleanup handler pushed onto the stack is
+ * popped and invoked with the argument 'arg' when
+ * a) the thread exits by calling 'pthread_exit',
+ * b) when the thread acts on a cancellation request,
+ * c) or when the thread calls pthread_cleanup_pop with a nonzero
+ * 'execute' argument
+ *
+ * PARAMETERS
+ * cleanup
+ * a pointer to an instance of pthread_cleanup_t,
+ *
+ * routine
+ * pointer to a cleanup handler,
+ *
+ * arg
+ * parameter to be passed to the cleanup handler
+ *
+ *
+ * DESCRIPTION
+ * This function pushes a new cleanup handler onto the thread's stack
+ * of cleanup handlers. Each cleanup handler pushed onto the stack is
+ * popped and invoked with the argument 'arg' when
+ * a) the thread exits by calling 'pthread_exit',
+ * b) when the thread acts on a cancellation request,
+ * c) or when the thrad calls pthread_cleanup_pop with a nonzero
+ * 'execute' argument
+ * NOTE: pthread_push_cleanup, ptw32_pop_cleanup must be paired
+ * in the same lexical scope.
+ *
+ * RESULTS
+ * pthread_cleanup_t *
+ * pointer to the previous cleanup
+ *
+ * ------------------------------------------------------
+ */
+{
+ cleanup->routine = routine;
+ cleanup->arg = arg;
+
+ cleanup->prev = (ptw32_cleanup_t *) pthread_getspecific (ptw32_cleanupKey);
+
+ pthread_setspecific (ptw32_cleanupKey, (void *) cleanup);
+
+} /* ptw32_push_cleanup */
diff --git a/3rdparty/pthreads-win32/condvar.c b/3rdparty/pthreads-win32/condvar.c
new file mode 100644
index 0000000..704f4d7
--- /dev/null
+++ b/3rdparty/pthreads-win32/condvar.c
@@ -0,0 +1,50 @@
+/*
+ * condvar.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ *
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#include "ptw32_cond_check_need_init.c"
+#include "pthread_condattr_init.c"
+#include "pthread_condattr_destroy.c"
+#include "pthread_condattr_getpshared.c"
+#include "pthread_condattr_setpshared.c"
+#include "pthread_cond_init.c"
+#include "pthread_cond_destroy.c"
+#include "pthread_cond_wait.c"
+#include "pthread_cond_signal.c"
diff --git a/3rdparty/pthreads-win32/config.h b/3rdparty/pthreads-win32/config.h
new file mode 100644
index 0000000..79da085
--- /dev/null
+++ b/3rdparty/pthreads-win32/config.h
@@ -0,0 +1,159 @@
+/* config.h */
+
+#ifndef PTW32_CONFIG_H
+#define PTW32_CONFIG_H
+
+/*********************************************************************
+ * Defaults: see target specific redefinitions below.
+ *********************************************************************/
+
+/* We're building the pthreads-win32 library */
+#define PTW32_BUILD
+
+/* Do we know about the C type sigset_t? */
+#undef HAVE_SIGSET_T
+
+/* Define if you have the <signal.h> header file. */
+#undef HAVE_SIGNAL_H
+
+/* Define if you have the Borland TASM32 or compatible assembler. */
+#undef HAVE_TASM32
+
+/* Define if you don't have Win32 DuplicateHandle. (eg. WinCE) */
+#undef NEED_DUPLICATEHANDLE
+
+/* Define if you don't have Win32 _beginthreadex. (eg. WinCE) */
+#undef NEED_CREATETHREAD
+
+/* Define if you don't have Win32 errno. (eg. WinCE) */
+#undef NEED_ERRNO
+
+/* Define if you don't have Win32 calloc. (eg. WinCE) */
+#undef NEED_CALLOC
+
+/* Define if you don't have Win32 ftime. (eg. WinCE) */
+#undef NEED_FTIME
+
+/* Define if you don't have Win32 semaphores. (eg. WinCE 2.1 or earlier) */
+#undef NEED_SEM
+
+/* Define if you need to convert string parameters to unicode. (eg. WinCE) */
+#undef NEED_UNICODE_CONSTS
+
+/* Define if your C (not C++) compiler supports "inline" functions. */
+#undef HAVE_C_INLINE
+
+/* Do we know about type mode_t? */
+#undef HAVE_MODE_T
+
+/*
+ * Define if GCC has atomic builtins, i.e. __sync_* intrinsics
+ * __sync_lock_* is implemented in mingw32 gcc 4.5.2 at least
+ * so this define does not turn those on or off. If you get an
+ * error from __sync_lock* then consider upgrading your gcc.
+ */
+#undef HAVE_GCC_ATOMIC_BUILTINS
+
+/* Define if you have the timespec struct */
+#undef HAVE_STRUCT_TIMESPEC
+
+/* Define if you don't have the GetProcessAffinityMask() */
+#undef NEED_PROCESS_AFFINITY_MASK
+
+/* Define if your version of Windows TLSGetValue() clears WSALastError
+ * and calling SetLastError() isn't enough restore it. You'll also need to
+ * link against wsock32.lib (or libwsock32.a for MinGW).
+ */
+#undef RETAIN_WSALASTERROR
+
+/*
+# ----------------------------------------------------------------------
+# The library can be built with some alternative behaviour to better
+# facilitate development of applications on Win32 that will be ported
+# to other POSIX systems.
+#
+# Nothing described here will make the library non-compliant and strictly
+# compliant applications will not be affected in any way, but
+# applications that make assumptions that POSIX does not guarantee are
+# not strictly compliant and may fail or misbehave with some settings.
+#
+# PTW32_THREAD_ID_REUSE_INCREMENT
+# Purpose:
+# POSIX says that applications should assume that thread IDs can be
+# recycled. However, Solaris (and some other systems) use a [very large]
+# sequence number as the thread ID, which provides virtual uniqueness.
+# This provides a very high but finite level of safety for applications
+# that are not meticulous in tracking thread lifecycles e.g. applications
+# that call functions which target detached threads without some form of
+# thread exit synchronisation.
+#
+# Usage:
+# Set to any value in the range: 0 <= value < 2^wordsize.
+# Set to 0 to emulate reusable thread ID behaviour like Linux or *BSD.
+# Set to 1 for unique thread IDs like Solaris (this is the default).
+# Set to some factor of 2^wordsize to emulate smaller word size types
+# (i.e. will wrap sooner). This might be useful to emulate some embedded
+# systems.
+#
+# define PTW32_THREAD_ID_REUSE_INCREMENT 0
+#
+# ----------------------------------------------------------------------
+ */
+#undef PTW32_THREAD_ID_REUSE_INCREMENT
+
+
+/*********************************************************************
+ * Target specific groups
+ *
+ * If you find that these are incorrect or incomplete please report it
+ * to the pthreads-win32 maintainer. Thanks.
+ *********************************************************************/
+#if defined(WINCE)
+#define NEED_DUPLICATEHANDLE
+#define NEED_CREATETHREAD
+#define NEED_ERRNO
+#define NEED_CALLOC
+#define NEED_FTIME
+/* #define NEED_SEM */
+#define NEED_UNICODE_CONSTS
+#define NEED_PROCESS_AFFINITY_MASK
+/* This may not be needed */
+#define RETAIN_WSALASTERROR
+#endif
+
+#if defined(_UWIN)
+#define HAVE_MODE_T
+#define HAVE_STRUCT_TIMESPEC
+#endif
+
+#if defined(__GNUC__)
+#define HAVE_C_INLINE
+#endif
+
+#if defined(__MINGW64__)
+#define HAVE_MODE_T
+#define HAVE_STRUCT_TIMESPEC
+#elif defined(__MINGW32__)
+#define HAVE_MODE_T
+#endif
+
+// struct timespec undefined with Visual Studio 2013 and previous (_MSC_VER 1800)
+// introduced in Visual Studio 2015 (_MSC_VER 1900)
+#if defined(_MSC_VER) && (_MSC_VER >= 1900) && !defined(_CRT_NO_TIME_T)
+#define HAVE_STRUCT_TIMESPEC
+#endif
+
+#if defined(__BORLANDC__)
+#endif
+
+#if defined(__WATCOMC__)
+#endif
+
+#if defined(__DMC__)
+#define HAVE_SIGNAL_H
+#define HAVE_C_INLINE
+#endif
+
+
+
+#endif
diff --git a/3rdparty/pthreads-win32/context.h b/3rdparty/pthreads-win32/context.h
new file mode 100644
index 0000000..3d4511f
--- /dev/null
+++ b/3rdparty/pthreads-win32/context.h
@@ -0,0 +1,74 @@
+/*
+ * context.h
+ *
+ * Description:
+ * POSIX thread macros related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#ifndef PTW32_CONTEXT_H
+#define PTW32_CONTEXT_H
+
+#undef PTW32_PROGCTR
+
+#if defined(_M_IX86) || (defined(_X86_) && !defined(__amd64__))
+#define PTW32_PROGCTR(Context) ((Context).Eip)
+#endif
+
+#if defined (_M_IA64) || defined(_IA64)
+#define PTW32_PROGCTR(Context) ((Context).StIIP)
+#endif
+
+#if defined(_MIPS_) || defined(MIPS)
+#define PTW32_PROGCTR(Context) ((Context).Fir)
+#endif
+
+#if defined(_ALPHA_)
+#define PTW32_PROGCTR(Context) ((Context).Fir)
+#endif
+
+#if defined(_PPC_)
+#define PTW32_PROGCTR(Context) ((Context).Iar)
+#endif
+
+#if defined(_AMD64_) || defined(__amd64__)
+#define PTW32_PROGCTR(Context) ((Context).Rip)
+#endif
+
+#if defined(_ARM_) || defined(ARM)
+#define PTW32_PROGCTR(Context) ((Context).Pc)
+#endif
+
+#if !defined(PTW32_PROGCTR)
+#error Module contains CPU-specific code; modify and recompile.
+#endif
+
+#endif
diff --git a/3rdparty/pthreads-win32/create.c b/3rdparty/pthreads-win32/create.c
new file mode 100644
index 0000000..8b036cc
--- /dev/null
+++ b/3rdparty/pthreads-win32/create.c
@@ -0,0 +1,308 @@
+/*
+ * create.c
+ *
+ * Description:
+ * This translation unit implements routines associated with spawning a new
+ * thread.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#if ! defined(_UWIN) && ! defined(WINCE)
+#include <process.h>
+#endif
+
+int
+pthread_create (pthread_t * tid,
+ const pthread_attr_t * attr,
+ void *(PTW32_CDECL *start) (void *), void *arg)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function creates a thread running the start function,
+ * passing it the parameter value, 'arg'. The 'attr'
+ * argument specifies optional creation attributes.
+ * The identity of the new thread is returned
+ * via 'tid', which should not be NULL.
+ *
+ * PARAMETERS
+ * tid
+ * pointer to an instance of pthread_t
+ *
+ * attr
+ * optional pointer to an instance of pthread_attr_t
+ *
+ * start
+ * pointer to the starting routine for the new thread
+ *
+ * arg
+ * optional parameter passed to 'start'
+ *
+ *
+ * DESCRIPTION
+ * This function creates a thread running the start function,
+ * passing it the parameter value, 'arg'. The 'attr'
+ * argument specifies optional creation attributes.
+ * The identity of the new thread is returned
+ * via 'tid', which should not be the NULL pointer.
+ *
+ * RESULTS
+ * 0 successfully created thread,
+ * EINVAL attr invalid,
+ * EAGAIN insufficient resources.
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_t thread;
+ ptw32_thread_t * tp;
+ register pthread_attr_t a;
+ HANDLE threadH = 0;
+ int result = EAGAIN;
+ int run = PTW32_TRUE;
+ ThreadParms *parms = NULL;
+ unsigned int stackSize;
+ int priority;
+ pthread_t self;
+
+ /*
+ * Before doing anything, check that tid can be stored through
+ * without invoking a memory protection error (segfault).
+ * Make sure that the assignment below can't be optimised out by the compiler.
+ * This is assured by conditionally assigning *tid again at the end.
+ */
+ tid->x = 0;
+
+ if (attr != NULL)
+ {
+ a = *attr;
+ }
+ else
+ {
+ a = NULL;
+ }
+
+ if ((thread = ptw32_new ()).p == NULL)
+ {
+ goto FAIL0;
+ }
+
+ tp = (ptw32_thread_t *) thread.p;
+
+ priority = tp->sched_priority;
+
+ if ((parms = (ThreadParms *) malloc (sizeof (*parms))) == NULL)
+ {
+ goto FAIL0;
+ }
+
+ parms->tid = thread;
+ parms->start = start;
+ parms->arg = arg;
+
+#if defined(HAVE_SIGSET_T)
+
+ /*
+ * Threads inherit their initial sigmask from their creator thread.
+ */
+ self = pthread_self();
+ tp->sigmask = ((ptw32_thread_t *)self.p)->sigmask;
+
+#endif /* HAVE_SIGSET_T */
+
+
+ if (a != NULL)
+ {
+ stackSize = (unsigned int)a->stacksize;
+ tp->detachState = a->detachstate;
+ priority = a->param.sched_priority;
+
+#if (THREAD_PRIORITY_LOWEST > THREAD_PRIORITY_NORMAL)
+ /* WinCE */
+#else
+ /* Everything else */
+
+ /*
+ * Thread priority must be set to a valid system level
+ * without altering the value set by pthread_attr_setschedparam().
+ */
+
+ /*
+ * PTHREAD_EXPLICIT_SCHED is the default because Win32 threads
+ * don't inherit their creator's priority. They are started with
+ * THREAD_PRIORITY_NORMAL (win32 value). The result of not supplying
+ * an 'attr' arg to pthread_create() is equivalent to defaulting to
+ * PTHREAD_EXPLICIT_SCHED and priority THREAD_PRIORITY_NORMAL.
+ */
+ if (PTHREAD_INHERIT_SCHED == a->inheritsched)
+ {
+ /*
+ * If the thread that called pthread_create() is a Win32 thread
+ * then the inherited priority could be the result of a temporary
+ * system adjustment. This is not the case for POSIX threads.
+ */
+#if ! defined(HAVE_SIGSET_T)
+ self = pthread_self ();
+#endif
+ priority = ((ptw32_thread_t *) self.p)->sched_priority;
+ }
+
+#endif
+
+ }
+ else
+ {
+ /*
+ * Default stackSize
+ */
+ stackSize = PTHREAD_STACK_MIN;
+ }
+
+ tp->state = run ? PThreadStateInitial : PThreadStateSuspended;
+
+ tp->keys = NULL;
+
+ /*
+ * Threads must be started in suspended mode and resumed if necessary
+ * after _beginthreadex returns us the handle. Otherwise we set up a
+ * race condition between the creating and the created threads.
+ * Note that we also retain a local copy of the handle for use
+ * by us in case thread.p->threadH gets NULLed later but before we've
+ * finished with it here.
+ */
+
+#if ! (defined (__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+
+ tp->threadH =
+ threadH =
+ (HANDLE) _beginthreadex ((void *) NULL, /* No security info */
+ stackSize, /* default stack size */
+ ptw32_threadStart,
+ parms,
+ (unsigned)
+ CREATE_SUSPENDED,
+ (unsigned *) &(tp->thread));
+
+ if (threadH != 0)
+ {
+ if (a != NULL)
+ {
+ (void) ptw32_setthreadpriority (thread, SCHED_OTHER, priority);
+ }
+
+ if (run)
+ {
+ ResumeThread (threadH);
+ }
+ }
+
+#else
+
+ {
+ ptw32_mcs_local_node_t stateLock;
+
+ /*
+ * This lock will force pthread_threadStart() to wait until we have
+ * the thread handle and have set the priority.
+ */
+ ptw32_mcs_lock_acquire(&tp->stateLock, &stateLock);
+
+ tp->threadH =
+ threadH =
+ (HANDLE) _beginthread (ptw32_threadStart, stackSize, /* default stack size */
+ parms);
+
+ /*
+ * Make the return code match _beginthreadex's.
+ */
+ if (threadH == (HANDLE) - 1L)
+ {
+ tp->threadH = threadH = 0;
+ }
+ else
+ {
+ if (!run)
+ {
+ /*
+ * beginthread does not allow for create flags, so we do it now.
+ * Note that beginthread itself creates the thread in SUSPENDED
+ * mode, and then calls ResumeThread to start it.
+ */
+ SuspendThread (threadH);
+ }
+
+ if (a != NULL)
+ {
+ (void) ptw32_setthreadpriority (thread, SCHED_OTHER, priority);
+ }
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+ }
+#endif
+
+ result = (threadH != 0) ? 0 : EAGAIN;
+
+ /*
+ * Fall Through Intentionally
+ */
+
+ /*
+ * ------------
+ * Failure Code
+ * ------------
+ */
+
+FAIL0:
+ if (result != 0)
+ {
+
+ ptw32_threadDestroy (thread);
+ tp = NULL;
+
+ if (parms != NULL)
+ {
+ free (parms);
+ }
+ }
+ else
+ {
+ *tid = thread;
+ }
+
+#if defined(_UWIN)
+ if (result == 0)
+ pthread_count++;
+#endif
+ return (result);
+
+} /* pthread_create */
diff --git a/3rdparty/pthreads-win32/dll.c b/3rdparty/pthreads-win32/dll.c
new file mode 100644
index 0000000..05e01be
--- /dev/null
+++ b/3rdparty/pthreads-win32/dll.c
@@ -0,0 +1,92 @@
+/*
+ * dll.c
+ *
+ * Description:
+ * This translation unit implements DLL initialisation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined(PTW32_STATIC_LIB)
+
+#include "pthread.h"
+#include "implement.h"
+
+#if defined(_MSC_VER)
+/*
+ * lpvReserved yields an unreferenced formal parameter;
+ * ignore it
+ */
+#pragma warning( disable : 4100 )
+#endif
+
+#if defined(__cplusplus)
+/*
+ * Dear c++: Please don't mangle this name. -thanks
+ */
+extern "C"
+#endif /* __cplusplus */
+ BOOL WINAPI
+DllMain (HINSTANCE hinstDll, DWORD fdwReason, LPVOID lpvReserved)
+{
+ BOOL result = PTW32_TRUE;
+
+ switch (fdwReason)
+ {
+
+ case DLL_PROCESS_ATTACH:
+ result = pthread_win32_process_attach_np ();
+ break;
+
+ case DLL_THREAD_ATTACH:
+ /*
+ * A thread is being created
+ */
+ result = pthread_win32_thread_attach_np ();
+ break;
+
+ case DLL_THREAD_DETACH:
+ /*
+ * A thread is exiting cleanly
+ */
+ result = pthread_win32_thread_detach_np ();
+ break;
+
+ case DLL_PROCESS_DETACH:
+ (void) pthread_win32_thread_detach_np ();
+ result = pthread_win32_process_detach_np ();
+ break;
+ }
+
+ return (result);
+
+} /* DllMain */
+
+#endif /* PTW32_STATIC_LIB */
diff --git a/3rdparty/pthreads-win32/errno.c b/3rdparty/pthreads-win32/errno.c
new file mode 100644
index 0000000..78aa920
--- /dev/null
+++ b/3rdparty/pthreads-win32/errno.c
@@ -0,0 +1,94 @@
+/*
+ * errno.c
+ *
+ * Description:
+ * This translation unit implements routines associated with spawning a new
+ * thread.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if defined(NEED_ERRNO)
+
+#include "pthread.h"
+#include "implement.h"
+
+static int reallyBad = ENOMEM;
+
+/*
+ * Re-entrant errno.
+ *
+ * Each thread has it's own errno variable in pthread_t.
+ *
+ * The benefit of using the pthread_t structure
+ * instead of another TSD key is TSD keys are limited
+ * on Win32 to 64 per process. Secondly, to implement
+ * it properly without using pthread_t you'd need
+ * to dynamically allocate an int on starting the thread
+ * and store it manually into TLS and then ensure that you free
+ * it on thread termination. We get all that for free
+ * by simply storing the errno on the pthread_t structure.
+ *
+ * MSVC and Mingw32 already have their own thread-safe errno.
+ *
+ * #if defined( _REENTRANT ) || defined( _MT )
+ * #define errno *_errno()
+ *
+ * int *_errno( void );
+ * #else
+ * extern int errno;
+ * #endif
+ *
+ */
+
+int *
+_errno (void)
+{
+ pthread_t self;
+ int *result;
+
+ if ((self = pthread_self ()).p == NULL)
+ {
+ /*
+ * Yikes! unable to allocate a thread!
+ * Throw an exception? return an error?
+ */
+ result = &reallyBad;
+ }
+ else
+ {
+ result = (int *)(&self.p->exitStatus);
+ }
+
+ return (result);
+
+} /* _errno */
+
+#endif /* (NEED_ERRNO) */
diff --git a/3rdparty/pthreads-win32/exit.c b/3rdparty/pthreads-win32/exit.c
new file mode 100644
index 0000000..94369d0
--- /dev/null
+++ b/3rdparty/pthreads-win32/exit.c
@@ -0,0 +1,44 @@
+/*
+ * exit.c
+ *
+ * Description:
+ * This translation unit implements routines associated with exiting from
+ * a thread.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#if ! defined(_UWIN) && ! defined(WINCE)
+# include <process.h>
+#endif
+
+#include "pthread_exit.c"
diff --git a/3rdparty/pthreads-win32/fork.c b/3rdparty/pthreads-win32/fork.c
new file mode 100644
index 0000000..8a29550
--- /dev/null
+++ b/3rdparty/pthreads-win32/fork.c
@@ -0,0 +1,39 @@
+/*
+ * fork.c
+ *
+ * Description:
+ * Implementation of fork() for POSIX threads.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+
+#include "pthread.h"
+#include "implement.h"
diff --git a/3rdparty/pthreads-win32/global.c b/3rdparty/pthreads-win32/global.c
new file mode 100644
index 0000000..f1e9b3f
--- /dev/null
+++ b/3rdparty/pthreads-win32/global.c
@@ -0,0 +1,107 @@
+/*
+ * global.c
+ *
+ * Description:
+ * This translation unit instantiates data associated with the implementation
+ * as a whole.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int ptw32_processInitialized = PTW32_FALSE;
+ptw32_thread_t * ptw32_threadReuseTop = PTW32_THREAD_REUSE_EMPTY;
+ptw32_thread_t * ptw32_threadReuseBottom = PTW32_THREAD_REUSE_EMPTY;
+pthread_key_t ptw32_selfThreadKey = NULL;
+pthread_key_t ptw32_cleanupKey = NULL;
+pthread_cond_t ptw32_cond_list_head = NULL;
+pthread_cond_t ptw32_cond_list_tail = NULL;
+
+int ptw32_concurrency = 0;
+
+/* What features have been auto-detected */
+int ptw32_features = 0;
+
+/*
+ * Global [process wide] thread sequence Number
+ */
+unsigned __int64 ptw32_threadSeqNumber = 0;
+
+/*
+ * Function pointer to QueueUserAPCEx if it exists, otherwise
+ * it will be set at runtime to a substitute routine which cannot unblock
+ * blocked threads.
+ */
+DWORD (*ptw32_register_cancelation) (PAPCFUNC, HANDLE, DWORD) = NULL;
+
+/*
+ * Global lock for managing pthread_t struct reuse.
+ */
+ptw32_mcs_lock_t ptw32_thread_reuse_lock = 0;
+
+/*
+ * Global lock for testing internal state of statically declared mutexes.
+ */
+ptw32_mcs_lock_t ptw32_mutex_test_init_lock = 0;
+
+/*
+ * Global lock for testing internal state of PTHREAD_COND_INITIALIZER
+ * created condition variables.
+ */
+ptw32_mcs_lock_t ptw32_cond_test_init_lock = 0;
+
+/*
+ * Global lock for testing internal state of PTHREAD_RWLOCK_INITIALIZER
+ * created read/write locks.
+ */
+ptw32_mcs_lock_t ptw32_rwlock_test_init_lock = 0;
+
+/*
+ * Global lock for testing internal state of PTHREAD_SPINLOCK_INITIALIZER
+ * created spin locks.
+ */
+ptw32_mcs_lock_t ptw32_spinlock_test_init_lock = 0;
+
+/*
+ * Global lock for condition variable linked list. The list exists
+ * to wake up CVs when a WM_TIMECHANGE message arrives. See
+ * w32_TimeChangeHandler.c.
+ */
+ptw32_mcs_lock_t ptw32_cond_list_lock = 0;
+
+#if defined(_UWIN)
+/*
+ * Keep a count of the number of threads.
+ */
+int pthread_count = 0;
+#endif
diff --git a/3rdparty/pthreads-win32/implement.h b/3rdparty/pthreads-win32/implement.h
new file mode 100644
index 0000000..693be26
--- /dev/null
+++ b/3rdparty/pthreads-win32/implement.h
@@ -0,0 +1,943 @@
+/*
+ * implement.h
+ *
+ * Definitions that don't need to be public.
+ *
+ * Keeps all the internals out of pthread.h
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: Ross.Johnson at homemail.com.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined(_IMPLEMENT_H)
+#define _IMPLEMENT_H
+
+#if !defined(_WIN32_WINNT)
+#define _WIN32_WINNT 0x0400
+#endif
+
+#include <windows.h>
+
+/*
+ * In case windows.h doesn't define it (e.g. WinCE perhaps)
+ */
+#if defined(WINCE)
+typedef VOID (APIENTRY *PAPCFUNC)(DWORD dwParam);
+#endif
+
+/*
+ * note: ETIMEDOUT is correctly defined in winsock.h
+ */
+#include <winsock.h>
+
+/*
+ * In case ETIMEDOUT hasn't been defined above somehow.
+ */
+#if !defined(ETIMEDOUT)
+# define ETIMEDOUT 10060 /* This is the value in winsock.h. */
+#endif
+
+#if !defined(malloc)
+#include <malloc.h>
+#endif
+
+#if defined(__CLEANUP_C)
+# include <setjmp.h>
+#endif
+
+#if !defined(INT_MAX)
+#include <limits.h>
+#endif
+
+/* use local include files during development */
+#include "semaphore.h"
+#include "sched.h"
+
+#if defined(HAVE_C_INLINE) || defined(__cplusplus)
+#define INLINE inline
+#else
+#define INLINE
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER < 1300
+/*
+ * MSVC 6 does not use the "volatile" qualifier
+ */
+#define PTW32_INTERLOCKED_VOLATILE
+#else
+#define PTW32_INTERLOCKED_VOLATILE volatile
+#endif
+#define PTW32_INTERLOCKED_LONG long
+#define PTW32_INTERLOCKED_SIZE size_t
+#define PTW32_INTERLOCKED_PVOID PVOID
+#define PTW32_INTERLOCKED_LONGPTR PTW32_INTERLOCKED_VOLATILE long*
+#define PTW32_INTERLOCKED_SIZEPTR PTW32_INTERLOCKED_VOLATILE size_t*
+#define PTW32_INTERLOCKED_PVOID_PTR PTW32_INTERLOCKED_VOLATILE PVOID*
+
+#if defined(__MINGW64__) || defined(__MINGW32__)
+# include <stdint.h>
+#elif defined(__BORLANDC__)
+# define int64_t ULONGLONG
+#else
+# define int64_t _int64
+# if defined(_MSC_VER) && _MSC_VER < 1300
+ typedef long intptr_t;
+# endif
+#endif
+
+typedef enum
+{
+ /*
+ * This enumeration represents the state of the thread;
+ * The thread is still "alive" if the numeric value of the
+ * state is greater or equal "PThreadStateRunning".
+ */
+ PThreadStateInitial = 0, /* Thread not running */
+ PThreadStateRunning, /* Thread alive & kicking */
+ PThreadStateSuspended, /* Thread alive but suspended */
+ PThreadStateCancelPending, /* Thread alive but */
+ /* has cancelation pending. */
+ PThreadStateCanceling, /* Thread alive but is */
+ /* in the process of terminating */
+ /* due to a cancellation request */
+ PThreadStateExiting, /* Thread alive but exiting */
+ /* due to an exception */
+ PThreadStateLast, /* All handlers have been run and now */
+ /* final cleanup can be done. */
+ PThreadStateReuse /* In reuse pool. */
+}
+PThreadState;
+
+typedef struct ptw32_mcs_node_t_ ptw32_mcs_local_node_t;
+typedef struct ptw32_mcs_node_t_* ptw32_mcs_lock_t;
+typedef struct ptw32_robust_node_t_ ptw32_robust_node_t;
+typedef struct ptw32_thread_t_ ptw32_thread_t;
+
+
+struct ptw32_thread_t_
+{
+ unsigned __int64 seqNumber; /* Process-unique thread sequence number */
+ HANDLE threadH; /* Win32 thread handle - POSIX thread is invalid if threadH == 0 */
+ pthread_t ptHandle; /* This thread's permanent pthread_t handle */
+ ptw32_thread_t * prevReuse; /* Links threads on reuse stack */
+ volatile PThreadState state;
+ ptw32_mcs_lock_t threadLock; /* Used for serialised access to public thread state */
+ ptw32_mcs_lock_t stateLock; /* Used for async-cancel safety */
+ HANDLE cancelEvent;
+ void *exitStatus;
+ void *parms;
+ void *keys;
+ void *nextAssoc;
+#if defined(__CLEANUP_C)
+ jmp_buf start_mark; /* Jump buffer follows void* so should be aligned */
+#endif /* __CLEANUP_C */
+#if defined(HAVE_SIGSET_T)
+ sigset_t sigmask;
+#endif /* HAVE_SIGSET_T */
+ ptw32_mcs_lock_t
+ robustMxListLock; /* robustMxList lock */
+ ptw32_robust_node_t*
+ robustMxList; /* List of currenty held robust mutexes */
+ int ptErrno;
+ int detachState;
+ int sched_priority; /* As set, not as currently is */
+ int cancelState;
+ int cancelType;
+ int implicit:1;
+ DWORD thread; /* Win32 thread ID */
+#if defined(_UWIN)
+ DWORD dummy[5];
+#endif
+ size_t align; /* Force alignment if this struct is packed */
+};
+
+
+/*
+ * Special value to mark attribute objects as valid.
+ */
+#define PTW32_ATTR_VALID ((unsigned long) 0xC4C0FFEE)
+
+struct pthread_attr_t_
+{
+ unsigned long valid;
+ void *stackaddr;
+ size_t stacksize;
+ int detachstate;
+ struct sched_param param;
+ int inheritsched;
+ int contentionscope;
+#if defined(HAVE_SIGSET_T)
+ sigset_t sigmask;
+#endif /* HAVE_SIGSET_T */
+};
+
+
+/*
+ * ====================
+ * ====================
+ * Semaphores, Mutexes and Condition Variables
+ * ====================
+ * ====================
+ */
+
+struct sem_t_
+{
+ int value;
+ pthread_mutex_t lock;
+ HANDLE sem;
+#if defined(NEED_SEM)
+ int leftToUnblock;
+#endif
+};
+
+#define PTW32_OBJECT_AUTO_INIT ((void *)(size_t) -1)
+#define PTW32_OBJECT_INVALID NULL
+
+struct pthread_mutex_t_
+{
+ LONG lock_idx; /* Provides exclusive access to mutex state
+ via the Interlocked* mechanism.
+ 0: unlocked/free.
+ 1: locked - no other waiters.
+ -1: locked - with possible other waiters.
+ */
+ int recursive_count; /* Number of unlocks a thread needs to perform
+ before the lock is released (recursive
+ mutexes only). */
+ int kind; /* Mutex type. */
+ pthread_t ownerThread;
+ HANDLE event; /* Mutex release notification to waiting
+ threads. */
+ ptw32_robust_node_t*
+ robustNode; /* Extra state for robust mutexes */
+};
+
+enum ptw32_robust_state_t_
+{
+ PTW32_ROBUST_CONSISTENT,
+ PTW32_ROBUST_INCONSISTENT,
+ PTW32_ROBUST_NOTRECOVERABLE
+};
+
+typedef enum ptw32_robust_state_t_ ptw32_robust_state_t;
+
+/*
+ * Node used to manage per-thread lists of currently-held robust mutexes.
+ */
+struct ptw32_robust_node_t_
+{
+ pthread_mutex_t mx;
+ ptw32_robust_state_t stateInconsistent;
+ ptw32_robust_node_t* prev;
+ ptw32_robust_node_t* next;
+};
+
+struct pthread_mutexattr_t_
+{
+ int pshared;
+ int kind;
+ int robustness;
+};
+
+/*
+ * Possible values, other than PTW32_OBJECT_INVALID,
+ * for the "interlock" element in a spinlock.
+ *
+ * In this implementation, when a spinlock is initialised,
+ * the number of cpus available to the process is checked.
+ * If there is only one cpu then "interlock" is set equal to
+ * PTW32_SPIN_USE_MUTEX and u.mutex is an initialised mutex.
+ * If the number of cpus is greater than 1 then "interlock"
+ * is set equal to PTW32_SPIN_UNLOCKED and the number is
+ * stored in u.cpus. This arrangement allows the spinlock
+ * routines to attempt an InterlockedCompareExchange on "interlock"
+ * immediately and, if that fails, to try the inferior mutex.
+ *
+ * "u.cpus" isn't used for anything yet, but could be used at
+ * some point to optimise spinlock behaviour.
+ */
+#define PTW32_SPIN_INVALID (0)
+#define PTW32_SPIN_UNLOCKED (1)
+#define PTW32_SPIN_LOCKED (2)
+#define PTW32_SPIN_USE_MUTEX (3)
+
+struct pthread_spinlock_t_
+{
+ long interlock; /* Locking element for multi-cpus. */
+ union
+ {
+ int cpus; /* No. of cpus if multi cpus, or */
+ pthread_mutex_t mutex; /* mutex if single cpu. */
+ } u;
+};
+
+/*
+ * MCS lock queue node - see ptw32_MCS_lock.c
+ */
+struct ptw32_mcs_node_t_
+{
+ struct ptw32_mcs_node_t_ **lock; /* ptr to tail of queue */
+ struct ptw32_mcs_node_t_ *next; /* ptr to successor in queue */
+ HANDLE readyFlag; /* set after lock is released by
+ predecessor */
+ HANDLE nextFlag; /* set after 'next' ptr is set by
+ successor */
+};
+
+
+struct pthread_barrier_t_
+{
+ unsigned int nCurrentBarrierHeight;
+ unsigned int nInitialBarrierHeight;
+ int pshared;
+ sem_t semBarrierBreeched;
+ ptw32_mcs_lock_t lock;
+ ptw32_mcs_local_node_t proxynode;
+};
+
+struct pthread_barrierattr_t_
+{
+ int pshared;
+};
+
+struct pthread_key_t_
+{
+ DWORD key;
+ void (PTW32_CDECL *destructor) (void *);
+ ptw32_mcs_lock_t keyLock;
+ void *threads;
+};
+
+
+typedef struct ThreadParms ThreadParms;
+
+struct ThreadParms
+{
+ pthread_t tid;
+ void *(PTW32_CDECL *start) (void *);
+ void *arg;
+};
+
+
+struct pthread_cond_t_
+{
+ long nWaitersBlocked; /* Number of threads blocked */
+ long nWaitersGone; /* Number of threads timed out */
+ long nWaitersToUnblock; /* Number of threads to unblock */
+ sem_t semBlockQueue; /* Queue up threads waiting for the */
+ /* condition to become signalled */
+ sem_t semBlockLock; /* Semaphore that guards access to */
+ /* | waiters blocked count/block queue */
+ /* +-> Mandatory Sync.LEVEL-1 */
+ pthread_mutex_t mtxUnblockLock; /* Mutex that guards access to */
+ /* | waiters (to)unblock(ed) counts */
+ /* +-> Optional* Sync.LEVEL-2 */
+ pthread_cond_t next; /* Doubly linked list */
+ pthread_cond_t prev;
+};
+
+
+struct pthread_condattr_t_
+{
+ int pshared;
+};
+
+#define PTW32_RWLOCK_MAGIC 0xfacade2
+
+struct pthread_rwlock_t_
+{
+ pthread_mutex_t mtxExclusiveAccess;
+ pthread_mutex_t mtxSharedAccessCompleted;
+ pthread_cond_t cndSharedAccessCompleted;
+ int nSharedAccessCount;
+ int nExclusiveAccessCount;
+ int nCompletedSharedAccessCount;
+ int nMagic;
+};
+
+struct pthread_rwlockattr_t_
+{
+ int pshared;
+};
+
+typedef struct ThreadKeyAssoc ThreadKeyAssoc;
+
+struct ThreadKeyAssoc
+{
+ /*
+ * Purpose:
+ * This structure creates an association between a thread and a key.
+ * It is used to implement the implicit invocation of a user defined
+ * destroy routine for thread specific data registered by a user upon
+ * exiting a thread.
+ *
+ * Graphically, the arrangement is as follows, where:
+ *
+ * K - Key with destructor
+ * (head of chain is key->threads)
+ * T - Thread that has called pthread_setspecific(Kn)
+ * (head of chain is thread->keys)
+ * A - Association. Each association is a node at the
+ * intersection of two doubly-linked lists.
+ *
+ * T1 T2 T3
+ * | | |
+ * | | |
+ * K1 -----+-----A-----A----->
+ * | | |
+ * | | |
+ * K2 -----A-----A-----+----->
+ * | | |
+ * | | |
+ * K3 -----A-----+-----A----->
+ * | | |
+ * | | |
+ * V V V
+ *
+ * Access to the association is guarded by two locks: the key's
+ * general lock (guarding the row) and the thread's general
+ * lock (guarding the column). This avoids the need for a
+ * dedicated lock for each association, which not only consumes
+ * more handles but requires that the lock resources persist
+ * until both the key is deleted and the thread has called the
+ * destructor. The two-lock arrangement allows those resources
+ * to be freed as soon as either thread or key is concluded.
+ *
+ * To avoid deadlock, whenever both locks are required both the
+ * key and thread locks are acquired consistently in the order
+ * "key lock then thread lock". An exception to this exists
+ * when a thread calls the destructors, however, this is done
+ * carefully (but inelegantly) to avoid deadlock.
+ *
+ * An association is created when a thread first calls
+ * pthread_setspecific() on a key that has a specified
+ * destructor.
+ *
+ * An association is destroyed either immediately after the
+ * thread calls the key destructor function on thread exit, or
+ * when the key is deleted.
+ *
+ * Attributes:
+ * thread
+ * reference to the thread that owns the
+ * association. This is actually the pointer to the
+ * thread struct itself. Since the association is
+ * destroyed before the thread exits, this can never
+ * point to a different logical thread to the one that
+ * created the assoc, i.e. after thread struct reuse.
+ *
+ * key
+ * reference to the key that owns the association.
+ *
+ * nextKey
+ * The pthread_t->keys attribute is the head of a
+ * chain of associations that runs through the nextKey
+ * link. This chain provides the 1 to many relationship
+ * between a pthread_t and all pthread_key_t on which
+ * it called pthread_setspecific.
+ *
+ * prevKey
+ * Similarly.
+ *
+ * nextThread
+ * The pthread_key_t->threads attribute is the head of
+ * a chain of associations that runs through the
+ * nextThreads link. This chain provides the 1 to many
+ * relationship between a pthread_key_t and all the
+ * PThreads that have called pthread_setspecific for
+ * this pthread_key_t.
+ *
+ * prevThread
+ * Similarly.
+ *
+ * Notes:
+ * 1) As soon as either the key or the thread is no longer
+ * referencing the association, it can be destroyed. The
+ * association will be removed from both chains.
+ *
+ * 2) Under WIN32, an association is only created by
+ * pthread_setspecific if the user provided a
+ * destroyRoutine when they created the key.
+ *
+ *
+ */
+ ptw32_thread_t * thread;
+ pthread_key_t key;
+ ThreadKeyAssoc *nextKey;
+ ThreadKeyAssoc *nextThread;
+ ThreadKeyAssoc *prevKey;
+ ThreadKeyAssoc *prevThread;
+};
+
+
+#if defined(__CLEANUP_SEH)
+/*
+ * --------------------------------------------------------------
+ * MAKE_SOFTWARE_EXCEPTION
+ * This macro constructs a software exception code following
+ * the same format as the standard Win32 error codes as defined
+ * in WINERROR.H
+ * Values are 32 bit values laid out as follows:
+ *
+ * 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0 9 8 7 6 5 4 3 2 1 0
+ * +---+-+-+-----------------------+-------------------------------+
+ * |Sev|C|R| Facility | Code |
+ * +---+-+-+-----------------------+-------------------------------+
+ *
+ * Severity Values:
+ */
+#define SE_SUCCESS 0x00
+#define SE_INFORMATION 0x01
+#define SE_WARNING 0x02
+#define SE_ERROR 0x03
+
+#define MAKE_SOFTWARE_EXCEPTION( _severity, _facility, _exception ) \
+( (DWORD) ( ( (_severity) << 30 ) | /* Severity code */ \
+ ( 1 << 29 ) | /* MS=0, User=1 */ \
+ ( 0 << 28 ) | /* Reserved */ \
+ ( (_facility) << 16 ) | /* Facility Code */ \
+ ( (_exception) << 0 ) /* Exception Code */ \
+ ) )
+
+/*
+ * We choose one specific Facility/Error code combination to
+ * identify our software exceptions vs. WIN32 exceptions.
+ * We store our actual component and error code within
+ * the optional information array.
+ */
+#define EXCEPTION_PTW32_SERVICES \
+ MAKE_SOFTWARE_EXCEPTION( SE_ERROR, \
+ PTW32_SERVICES_FACILITY, \
+ PTW32_SERVICES_ERROR )
+
+#define PTW32_SERVICES_FACILITY 0xBAD
+#define PTW32_SERVICES_ERROR 0xDEED
+
+#endif /* __CLEANUP_SEH */
+
+/*
+ * Services available through EXCEPTION_PTW32_SERVICES
+ * and also used [as parameters to ptw32_throw()] as
+ * generic exception selectors.
+ */
+
+#define PTW32_EPS_EXIT (1)
+#define PTW32_EPS_CANCEL (2)
+
+
+/* Useful macros */
+#define PTW32_MAX(a,b) ((a)<(b)?(b):(a))
+#define PTW32_MIN(a,b) ((a)>(b)?(b):(a))
+
+
+/* Declared in pthread_cancel.c */
+extern DWORD (*ptw32_register_cancelation) (PAPCFUNC, HANDLE, DWORD);
+
+/* Thread Reuse stack bottom marker. Must not be NULL or any valid pointer to memory. */
+#define PTW32_THREAD_REUSE_EMPTY ((ptw32_thread_t *)(size_t) 1)
+
+extern int ptw32_processInitialized;
+extern ptw32_thread_t * ptw32_threadReuseTop;
+extern ptw32_thread_t * ptw32_threadReuseBottom;
+extern pthread_key_t ptw32_selfThreadKey;
+extern pthread_key_t ptw32_cleanupKey;
+extern pthread_cond_t ptw32_cond_list_head;
+extern pthread_cond_t ptw32_cond_list_tail;
+
+extern int ptw32_mutex_default_kind;
+
+extern unsigned __int64 ptw32_threadSeqNumber;
+
+extern int ptw32_concurrency;
+
+extern int ptw32_features;
+
+extern ptw32_mcs_lock_t ptw32_thread_reuse_lock;
+extern ptw32_mcs_lock_t ptw32_mutex_test_init_lock;
+extern ptw32_mcs_lock_t ptw32_cond_list_lock;
+extern ptw32_mcs_lock_t ptw32_cond_test_init_lock;
+extern ptw32_mcs_lock_t ptw32_rwlock_test_init_lock;
+extern ptw32_mcs_lock_t ptw32_spinlock_test_init_lock;
+
+#if defined(_UWIN)
+extern int pthread_count;
+#endif
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif /* __cplusplus */
+
+/*
+ * =====================
+ * =====================
+ * Forward Declarations
+ * =====================
+ * =====================
+ */
+
+ int ptw32_is_attr (const pthread_attr_t * attr);
+
+ int ptw32_cond_check_need_init (pthread_cond_t * cond);
+ int ptw32_mutex_check_need_init (pthread_mutex_t * mutex);
+ int ptw32_rwlock_check_need_init (pthread_rwlock_t * rwlock);
+
+ int ptw32_robust_mutex_inherit(pthread_mutex_t * mutex);
+ void ptw32_robust_mutex_add(pthread_mutex_t* mutex, pthread_t self);
+ void ptw32_robust_mutex_remove(pthread_mutex_t* mutex, ptw32_thread_t* otp);
+
+ DWORD
+ ptw32_RegisterCancelation (PAPCFUNC callback,
+ HANDLE threadH, DWORD callback_arg);
+
+ int ptw32_processInitialize (void);
+
+ void ptw32_processTerminate (void);
+
+ void ptw32_threadDestroy (pthread_t tid);
+
+ void ptw32_pop_cleanup_all (int execute);
+
+ pthread_t ptw32_new (void);
+
+ pthread_t ptw32_threadReusePop (void);
+
+ void ptw32_threadReusePush (pthread_t thread);
+
+ int ptw32_getprocessors (int *count);
+
+ int ptw32_setthreadpriority (pthread_t thread, int policy, int priority);
+
+ void ptw32_rwlock_cancelwrwait (void *arg);
+
+#if ! (defined (__MINGW64__) || defined(__MINGW32__)) || (defined(__MSVCRT__) && ! defined(__DMC__))
+ unsigned __stdcall
+#else
+ void
+#endif
+ ptw32_threadStart (void *vthreadParms);
+
+ void ptw32_callUserDestroyRoutines (pthread_t thread);
+
+ int ptw32_tkAssocCreate (ptw32_thread_t * thread, pthread_key_t key);
+
+ void ptw32_tkAssocDestroy (ThreadKeyAssoc * assoc);
+
+ int ptw32_semwait (sem_t * sem);
+
+ DWORD ptw32_relmillisecs (const struct timespec * abstime);
+
+ void ptw32_mcs_lock_acquire (ptw32_mcs_lock_t * lock, ptw32_mcs_local_node_t * node);
+
+ int ptw32_mcs_lock_try_acquire (ptw32_mcs_lock_t * lock, ptw32_mcs_local_node_t * node);
+
+ void ptw32_mcs_lock_release (ptw32_mcs_local_node_t * node);
+
+ void ptw32_mcs_node_transfer (ptw32_mcs_local_node_t * new_node, ptw32_mcs_local_node_t * old_node);
+
+#if defined(NEED_FTIME)
+ void ptw32_timespec_to_filetime (const struct timespec *ts, FILETIME * ft);
+ void ptw32_filetime_to_timespec (const FILETIME * ft, struct timespec *ts);
+#endif
+
+/* Declared in misc.c */
+#if defined(NEED_CALLOC)
+#define calloc(n, s) ptw32_calloc(n, s)
+ void *ptw32_calloc (size_t n, size_t s);
+#endif
+
+/* Declared in private.c */
+#if defined(_MSC_VER)
+/*
+ * Ignore the warning:
+ * "C++ exception specification ignored except to indicate that
+ * the function is not __declspec(nothrow)."
+ */
+#pragma warning(disable:4290)
+#endif
+ void ptw32_throw (DWORD exception)
+#if defined(__CLEANUP_CXX)
+ throw(ptw32_exception_cancel,ptw32_exception_exit)
+#endif
+;
+
+#if defined(__cplusplus)
+}
+#endif /* __cplusplus */
+
+
+#if defined(_UWIN_)
+# if defined(_MT)
+# if defined(__cplusplus)
+extern "C"
+{
+# endif
+ _CRTIMP unsigned long __cdecl _beginthread (void (__cdecl *) (void *),
+ unsigned, void *);
+ _CRTIMP void __cdecl _endthread (void);
+ _CRTIMP unsigned long __cdecl _beginthreadex (void *, unsigned,
+ unsigned (__stdcall *) (void *),
+ void *, unsigned, unsigned *);
+ _CRTIMP void __cdecl _endthreadex (unsigned);
+# if defined(__cplusplus)
+}
+# endif
+# endif
+#else
+# include <process.h>
+# endif
+
+
+/*
+ * Use intrinsic versions wherever possible. VC will do this
+ * automatically where possible and GCC define these if available:
+ * __GCC_HAVE_SYNC_COMPARE_AND_SWAP_1
+ * __GCC_HAVE_SYNC_COMPARE_AND_SWAP_2
+ * __GCC_HAVE_SYNC_COMPARE_AND_SWAP_4
+ * __GCC_HAVE_SYNC_COMPARE_AND_SWAP_8
+ * __GCC_HAVE_SYNC_COMPARE_AND_SWAP_16
+ *
+ * The full set of Interlocked intrinsics in GCC are (check versions):
+ * type __sync_fetch_and_add (type *ptr, type value, ...)
+ * type __sync_fetch_and_sub (type *ptr, type value, ...)
+ * type __sync_fetch_and_or (type *ptr, type value, ...)
+ * type __sync_fetch_and_and (type *ptr, type value, ...)
+ * type __sync_fetch_and_xor (type *ptr, type value, ...)
+ * type __sync_fetch_and_nand (type *ptr, type value, ...)
+ * type __sync_add_and_fetch (type *ptr, type value, ...)
+ * type __sync_sub_and_fetch (type *ptr, type value, ...)
+ * type __sync_or_and_fetch (type *ptr, type value, ...)
+ * type __sync_and_and_fetch (type *ptr, type value, ...)
+ * type __sync_xor_and_fetch (type *ptr, type value, ...)
+ * type __sync_nand_and_fetch (type *ptr, type value, ...)
+ * bool __sync_bool_compare_and_swap (type *ptr, type oldval type newval, ...)
+ * type __sync_val_compare_and_swap (type *ptr, type oldval type newval, ...)
+ * __sync_synchronize (...) // Full memory barrier
+ * type __sync_lock_test_and_set (type *ptr, type value, ...) // Acquire barrier
+ * void __sync_lock_release (type *ptr, ...) // Release barrier
+ *
+ * These are all overloaded and take 1,2,4,8 byte scalar or pointer types.
+ *
+ * The above aren't available in Mingw32 as of gcc 4.5.2 so define our own.
+ */
+#if defined(__GNUC__)
+# if defined(_WIN64)
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_64(location, value, comparand) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "cmpxchgq %2,(%1)" \
+ :"=a" (_result) \
+ :"r" (location), "r" (value), "a" (comparand) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_EXCHANGE_64(location, value) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "xchgq %0,(%1)" \
+ :"=r" (_result) \
+ :"r" (location), "0" (value) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_64(location, value) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddq %0,(%1)" \
+ :"=r" (_result) \
+ :"r" (location), "0" (value) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_INCREMENT_64(location) \
+ ({ \
+ PTW32_INTERLOCKED_LONG _temp = 1; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddq %0,(%1)" \
+ :"+r" (_temp) \
+ :"r" (location) \
+ :"memory", "cc"); \
+ ++_temp; \
+ })
+# define PTW32_INTERLOCKED_DECREMENT_64(location) \
+ ({ \
+ PTW32_INTERLOCKED_LONG _temp = -1; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddq %2,(%1)" \
+ :"+r" (_temp) \
+ :"r" (location) \
+ :"memory", "cc"); \
+ --_temp; \
+ })
+#endif
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(location, value, comparand) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "cmpxchgl %2,(%1)" \
+ :"=a" (_result) \
+ :"r" (location), "r" (value), "a" (comparand) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_EXCHANGE_LONG(location, value) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "xchgl %0,(%1)" \
+ :"=r" (_result) \
+ :"r" (location), "0" (value) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(location, value) \
+ ({ \
+ __typeof (value) _result; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddl %0,(%1)" \
+ :"=r" (_result) \
+ :"r" (location), "0" (value) \
+ :"memory", "cc"); \
+ _result; \
+ })
+# define PTW32_INTERLOCKED_INCREMENT_LONG(location) \
+ ({ \
+ PTW32_INTERLOCKED_LONG _temp = 1; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddl %0,(%1)" \
+ :"+r" (_temp) \
+ :"r" (location) \
+ :"memory", "cc"); \
+ ++_temp; \
+ })
+# define PTW32_INTERLOCKED_DECREMENT_LONG(location) \
+ ({ \
+ PTW32_INTERLOCKED_LONG _temp = -1; \
+ __asm__ __volatile__ \
+ ( \
+ "lock\n\t" \
+ "xaddl %0,(%1)" \
+ :"+r" (_temp) \
+ :"r" (location) \
+ :"memory", "cc"); \
+ --_temp; \
+ })
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR(location, value, comparand) \
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_SIZE((PTW32_INTERLOCKED_SIZEPTR)location, \
+ (PTW32_INTERLOCKED_SIZE)value, \
+ (PTW32_INTERLOCKED_SIZE)comparand)
+# define PTW32_INTERLOCKED_EXCHANGE_PTR(location, value) \
+ PTW32_INTERLOCKED_EXCHANGE_SIZE((PTW32_INTERLOCKED_SIZEPTR)location, \
+ (PTW32_INTERLOCKED_SIZE)value)
+#else
+# if defined(_WIN64)
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_64 InterlockedCompareExchange64
+# define PTW32_INTERLOCKED_EXCHANGE_64 InterlockedExchange64
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_64 InterlockedExchangeAdd64
+# define PTW32_INTERLOCKED_INCREMENT_64 InterlockedIncrement64
+# define PTW32_INTERLOCKED_DECREMENT_64 InterlockedDecrement64
+# endif
+# if defined(_MSC_VER) && _MSC_VER < 1300 && !defined(_WIN64) /* MSVC 6 */
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(location, value, comparand) \
+ ((LONG)InterlockedCompareExchange((PVOID *)(location), (PVOID)(value), (PVOID)(comparand)))
+# else
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG InterlockedCompareExchange
+# endif
+# define PTW32_INTERLOCKED_EXCHANGE_LONG InterlockedExchange
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_LONG InterlockedExchangeAdd
+# define PTW32_INTERLOCKED_INCREMENT_LONG InterlockedIncrement
+# define PTW32_INTERLOCKED_DECREMENT_LONG InterlockedDecrement
+# if defined(_MSC_VER) && _MSC_VER < 1300 && !defined(_WIN64) /* MSVC 6 */
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR InterlockedCompareExchange
+# define PTW32_INTERLOCKED_EXCHANGE_PTR(location, value) \
+ ((PVOID)InterlockedExchange((LPLONG)(location), (LONG)(value)))
+# else
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR InterlockedCompareExchangePointer
+# define PTW32_INTERLOCKED_EXCHANGE_PTR InterlockedExchangePointer
+# endif
+#endif
+#if defined(_WIN64)
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_SIZE PTW32_INTERLOCKED_COMPARE_EXCHANGE_64
+# define PTW32_INTERLOCKED_EXCHANGE_SIZE PTW32_INTERLOCKED_EXCHANGE_64
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_SIZE PTW32_INTERLOCKED_EXCHANGE_ADD_64
+# define PTW32_INTERLOCKED_INCREMENT_SIZE PTW32_INTERLOCKED_INCREMENT_64
+# define PTW32_INTERLOCKED_DECREMENT_SIZE PTW32_INTERLOCKED_DECREMENT_64
+#else
+# define PTW32_INTERLOCKED_COMPARE_EXCHANGE_SIZE PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG
+# define PTW32_INTERLOCKED_EXCHANGE_SIZE PTW32_INTERLOCKED_EXCHANGE_LONG
+# define PTW32_INTERLOCKED_EXCHANGE_ADD_SIZE PTW32_INTERLOCKED_EXCHANGE_ADD_LONG
+# define PTW32_INTERLOCKED_INCREMENT_SIZE PTW32_INTERLOCKED_INCREMENT_LONG
+# define PTW32_INTERLOCKED_DECREMENT_SIZE PTW32_INTERLOCKED_DECREMENT_LONG
+#endif
+
+#if defined(NEED_CREATETHREAD)
+
+/*
+ * Macro uses args so we can cast start_proc to LPTHREAD_START_ROUTINE
+ * in order to avoid warnings because of return type
+ */
+
+#define _beginthreadex(security, \
+ stack_size, \
+ start_proc, \
+ arg, \
+ flags, \
+ pid) \
+ CreateThread(security, \
+ stack_size, \
+ (LPTHREAD_START_ROUTINE) start_proc, \
+ arg, \
+ flags, \
+ pid)
+
+#define _endthreadex ExitThread
+
+#endif /* NEED_CREATETHREAD */
+
+
+#endif /* _IMPLEMENT_H */
diff --git a/3rdparty/pthreads-win32/misc.c b/3rdparty/pthreads-win32/misc.c
new file mode 100644
index 0000000..06d1d21
--- /dev/null
+++ b/3rdparty/pthreads-win32/misc.c
@@ -0,0 +1,50 @@
+/*
+ * misc.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "pthread_kill.c"
+#include "pthread_once.c"
+#include "pthread_self.c"
+#include "pthread_equal.c"
+#include "pthread_setconcurrency.c"
+#include "pthread_getconcurrency.c"
+#include "ptw32_new.c"
+#include "ptw32_calloc.c"
+#include "ptw32_reuse.c"
+#include "w32_CancelableWait.c"
diff --git a/3rdparty/pthreads-win32/mutex.c b/3rdparty/pthreads-win32/mutex.c
new file mode 100644
index 0000000..c2b3607
--- /dev/null
+++ b/3rdparty/pthreads-win32/mutex.c
@@ -0,0 +1,62 @@
+/*
+ * mutex.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if ! defined(_UWIN) && ! defined(WINCE)
+# include <process.h>
+#endif
+#if !defined(NEED_FTIME)
+#include <sys/timeb.h>
+#endif
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "ptw32_mutex_check_need_init.c"
+#include "pthread_mutex_init.c"
+#include "pthread_mutex_destroy.c"
+#include "pthread_mutexattr_init.c"
+#include "pthread_mutexattr_destroy.c"
+#include "pthread_mutexattr_getpshared.c"
+#include "pthread_mutexattr_setpshared.c"
+#include "pthread_mutexattr_settype.c"
+#include "pthread_mutexattr_gettype.c"
+#include "pthread_mutexattr_setrobust.c"
+#include "pthread_mutexattr_getrobust.c"
+#include "pthread_mutex_lock.c"
+#include "pthread_mutex_timedlock.c"
+#include "pthread_mutex_unlock.c"
+#include "pthread_mutex_trylock.c"
+#include "pthread_mutex_consistent.c"
diff --git a/3rdparty/pthreads-win32/need_errno.h b/3rdparty/pthreads-win32/need_errno.h
new file mode 100644
index 0000000..abf1c95
--- /dev/null
+++ b/3rdparty/pthreads-win32/need_errno.h
@@ -0,0 +1,145 @@
+/***
+* errno.h - system wide error numbers (set by system calls)
+*
+* Copyright (c) 1985-1997, Microsoft Corporation. All rights reserved.
+*
+* Purpose:
+* This file defines the system-wide error numbers (set by
+* system calls). Conforms to the XENIX standard. Extended
+* for compatibility with Uniforum standard.
+* [System V]
+*
+* [Public]
+*
+****/
+
+#if _MSC_VER > 1000
+#pragma once
+#endif
+
+#if !defined(_INC_ERRNO)
+#define _INC_ERRNO
+
+#if !defined(_WIN32)
+#error ERROR: Only Win32 targets supported!
+#endif
+
+#include <winsock.h>
+
+#if defined(__cplusplus)
+extern "C" {
+#endif
+
+
+
+/* Define _CRTIMP */
+
+#ifndef _CRTIMP
+#if defined(_DLL)
+#define _CRTIMP __declspec(dllimport)
+#else /* ndef _DLL */
+#define _CRTIMP
+#endif /* _DLL */
+#endif /* _CRTIMP */
+
+
+/* Define __cdecl for non-Microsoft compilers */
+
+#if ( !defined(_MSC_VER) && !defined(__cdecl) )
+#define __cdecl
+#endif
+
+/* Define _CRTAPI1 (for compatibility with the NT SDK) */
+
+#if !defined(_CRTAPI1)
+#if _MSC_VER >= 800 && _M_IX86 >= 300
+#define _CRTAPI1 __cdecl
+#else
+#define _CRTAPI1
+#endif
+#endif
+
+#if !defined(PTW32_STATIC_LIB)
+# if defined(PTW32_BUILD)
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/* declare reference to errno */
+
+#if (defined(_MT) || defined(_MD) || defined(_DLL)) && !defined(_MAC)
+PTW32_DLLPORT int * __cdecl _errno(void);
+#define errno (*_errno())
+#else /* ndef _MT && ndef _MD && ndef _DLL */
+_CRTIMP extern int errno;
+#endif /* _MT || _MD || _DLL */
+
+/* Error Codes */
+
+#define EPERM 1
+#define ENOENT 2
+#define ESRCH 3
+#define EINTR 4
+#define EIO 5
+#define ENXIO 6
+#define E2BIG 7
+#define ENOEXEC 8
+#define EBADF 9
+#define ECHILD 10
+#define EAGAIN 11
+#define ENOMEM 12
+#define EACCES 13
+#define EFAULT 14
+#define EBUSY 16
+#define EEXIST 17
+#define EXDEV 18
+#define ENODEV 19
+#define ENOTDIR 20
+#define EISDIR 21
+#define EINVAL 22
+#define ENFILE 23
+#define EMFILE 24
+#define ENOTTY 25
+#define EFBIG 27
+#define ENOSPC 28
+#define ESPIPE 29
+#define EROFS 30
+#define EMLINK 31
+#define EPIPE 32
+#define EDOM 33
+#define ERANGE 34
+#define EDEADLK 36
+
+/* defined differently in winsock.h on WinCE */
+#if !defined(ENAMETOOLONG)
+#define ENAMETOOLONG 38
+#endif
+
+#define ENOLCK 39
+#define ENOSYS 40
+
+/* defined differently in winsock.h on WinCE */
+#if !defined(ENOTEMPTY)
+#define ENOTEMPTY 41
+#endif
+
+#define EILSEQ 42
+
+/* POSIX 2008 - robust mutexes */
+#define EOWNERDEAD 43
+#define ENOTRECOVERABLE 44
+
+/*
+ * Support EDEADLOCK for compatibiity with older MS-C versions.
+ */
+#define EDEADLOCK EDEADLK
+
+#if defined(__cplusplus)
+}
+#endif
+
+#endif /* _INC_ERRNO */
diff --git a/3rdparty/pthreads-win32/nonportable.c b/3rdparty/pthreads-win32/nonportable.c
new file mode 100644
index 0000000..742cb96
--- /dev/null
+++ b/3rdparty/pthreads-win32/nonportable.c
@@ -0,0 +1,47 @@
+/*
+ * nonportable.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#include "pthread_mutexattr_setkind_np.c"
+#include "pthread_mutexattr_getkind_np.c"
+#include "pthread_getw32threadhandle_np.c"
+#include "pthread_getunique_np.c"
+#include "pthread_delay_np.c"
+#include "pthread_num_processors_np.c"
+#include "pthread_win32_attach_detach_np.c"
+#include "pthread_timechange_handler_np.c"
diff --git a/3rdparty/pthreads-win32/private.c b/3rdparty/pthreads-win32/private.c
new file mode 100644
index 0000000..1b1ccb7
--- /dev/null
+++ b/3rdparty/pthreads-win32/private.c
@@ -0,0 +1,54 @@
+/*
+ * private.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#include "ptw32_MCS_lock.c"
+#include "ptw32_is_attr.c"
+#include "ptw32_processInitialize.c"
+#include "ptw32_processTerminate.c"
+#include "ptw32_threadStart.c"
+#include "ptw32_threadDestroy.c"
+#include "ptw32_tkAssocCreate.c"
+#include "ptw32_tkAssocDestroy.c"
+#include "ptw32_callUserDestroyRoutines.c"
+#include "ptw32_semwait.c"
+#include "ptw32_timespec.c"
+#include "ptw32_relmillisecs.c"
+#include "ptw32_throw.c"
+#include "ptw32_getprocessors.c"
diff --git a/3rdparty/pthreads-win32/pthread.c b/3rdparty/pthreads-win32/pthread.c
new file mode 100644
index 0000000..60b5341
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread.c
@@ -0,0 +1,66 @@
+/*
+ * pthread.c
+ *
+ * Description:
+ * This translation unit agregates pthreads-win32 translation units.
+ * It is used for inline optimisation of the library,
+ * maximising for speed at the expense of size.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/* The following are ordered for inlining */
+
+#include "private.c"
+#include "attr.c"
+#include "barrier.c"
+#include "cancel.c"
+#include "cleanup.c"
+#include "condvar.c"
+#include "create.c"
+#include "dll.c"
+#include "autostatic.c"
+#include "errno.c"
+#include "exit.c"
+#include "fork.c"
+#include "global.c"
+#include "misc.c"
+#include "mutex.c"
+#include "nonportable.c"
+#include "rwlock.c"
+#include "sched.c"
+#include "semaphore.c"
+#include "signal.c"
+#include "spin.c"
+#include "sync.c"
+#include "tsd.c"
diff --git a/3rdparty/pthreads-win32/pthread.h b/3rdparty/pthreads-win32/pthread.h
new file mode 100644
index 0000000..cad8bc5
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread.h
@@ -0,0 +1,1368 @@
+/* This is an implementation of the threads API of POSIX 1003.1-2001.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined( PTHREAD_H )
+#define PTHREAD_H
+
+/*
+ * See the README file for an explanation of the pthreads-win32 version
+ * numbering scheme and how the DLL is named etc.
+ */
+#define PTW32_VERSION 2,9,1,0
+#define PTW32_VERSION_STRING "2, 9, 1, 0\0"
+
+/* There are three implementations of cancel cleanup.
+ * Note that pthread.h is included in both application
+ * compilation units and also internally for the library.
+ * The code here and within the library aims to work
+ * for all reasonable combinations of environments.
+ *
+ * The three implementations are:
+ *
+ * WIN32 SEH
+ * C
+ * C++
+ *
+ * Please note that exiting a push/pop block via
+ * "return", "exit", "break", or "continue" will
+ * lead to different behaviour amongst applications
+ * depending upon whether the library was built
+ * using SEH, C++, or C. For example, a library built
+ * with SEH will call the cleanup routine, while both
+ * C++ and C built versions will not.
+ */
+
+/*
+ * Define defaults for cleanup code.
+ * Note: Unless the build explicitly defines one of the following, then
+ * we default to standard C style cleanup. This style uses setjmp/longjmp
+ * in the cancelation and thread exit implementations and therefore won't
+ * do stack unwinding if linked to applications that have it (e.g.
+ * C++ apps). This is currently consistent with most/all commercial Unix
+ * POSIX threads implementations.
+ */
+#if !defined( __CLEANUP_SEH ) && !defined( __CLEANUP_CXX ) && !defined( __CLEANUP_C )
+# define __CLEANUP_C
+#endif
+
+#if defined( __CLEANUP_SEH ) && ( !defined( _MSC_VER ) && !defined(PTW32_RC_MSC))
+#error ERROR [__FILE__, line __LINE__]: SEH is not supported for this compiler.
+#endif
+
+/*
+ * Stop here if we are being included by the resource compiler.
+ */
+#if !defined(RC_INVOKED)
+
+#undef PTW32_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_LEVEL
+#define PTW32_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_LEVEL_MAX 3
+
+#if ( defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 200112 ) || !defined(PTW32_LEVEL)
+#define PTW32_LEVEL PTW32_LEVEL_MAX
+/* Include everything */
+#endif
+
+#if defined(_UWIN)
+# define HAVE_STRUCT_TIMESPEC 1
+# define HAVE_SIGNAL_H 1
+# undef HAVE_PTW32_CONFIG_H
+# pragma comment(lib, "pthread")
+#endif
+
+/*
+ * -------------------------------------------------------------
+ *
+ *
+ * Module: pthread.h
+ *
+ * Purpose:
+ * Provides an implementation of PThreads based upon the
+ * standard:
+ *
+ * POSIX 1003.1-2001
+ * and
+ * The Single Unix Specification version 3
+ *
+ * (these two are equivalent)
+ *
+ * in order to enhance code portability between Windows,
+ * various commercial Unix implementations, and Linux.
+ *
+ * See the ANNOUNCE file for a full list of conforming
+ * routines and defined constants, and a list of missing
+ * routines and constants not defined in this implementation.
+ *
+ * Authors:
+ * There have been many contributors to this library.
+ * The initial implementation was contributed by
+ * John Bossom, and several others have provided major
+ * sections or revisions of parts of the implementation.
+ * Often significant effort has been contributed to
+ * find and fix important bugs and other problems to
+ * improve the reliability of the library, which sometimes
+ * is not reflected in the amount of code which changed as
+ * result.
+ * As much as possible, the contributors are acknowledged
+ * in the ChangeLog file in the source code distribution
+ * where their changes are noted in detail.
+ *
+ * Contributors are listed in the CONTRIBUTORS file.
+ *
+ * As usual, all bouquets go to the contributors, and all
+ * brickbats go to the project maintainer.
+ *
+ * Maintainer:
+ * The code base for this project is coordinated and
+ * eventually pre-tested, packaged, and made available by
+ *
+ * Ross Johnson <rpj at callisto.canberra.edu.au>
+ *
+ * QA Testers:
+ * Ultimately, the library is tested in the real world by
+ * a host of competent and demanding scientists and
+ * engineers who report bugs and/or provide solutions
+ * which are then fixed or incorporated into subsequent
+ * versions of the library. Each time a bug is fixed, a
+ * test case is written to prove the fix and ensure
+ * that later changes to the code don't reintroduce the
+ * same error. The number of test cases is slowly growing
+ * and therefore so is the code reliability.
+ *
+ * Compliance:
+ * See the file ANNOUNCE for the list of implemented
+ * and not-implemented routines and defined options.
+ * Of course, these are all defined is this file as well.
+ *
+ * Web site:
+ * The source code and other information about this library
+ * are available from
+ *
+ * http://sources.redhat.com/pthreads-win32/
+ *
+ * -------------------------------------------------------------
+ */
+
+/* Try to avoid including windows.h */
+#if (defined(__MINGW64__) || defined(__MINGW32__)) && defined(__cplusplus)
+#define PTW32_INCLUDE_WINDOWS_H
+#endif
+
+#if defined(PTW32_INCLUDE_WINDOWS_H)
+#include <windows.h>
+#endif
+
+#if defined(_MSC_VER) && _MSC_VER < 1300 || defined(__DMC__)
+/*
+ * VC++6.0 or early compiler's header has no DWORD_PTR type.
+ */
+typedef unsigned long DWORD_PTR;
+typedef unsigned long ULONG_PTR;
+#endif
+/*
+ * -----------------
+ * autoconf switches
+ * -----------------
+ */
+
+#if defined(HAVE_PTW32_CONFIG_H)
+#include "config.h"
+#endif /* HAVE_PTW32_CONFIG_H */
+
+#if !defined(NEED_FTIME)
+#include <time.h>
+#else /* NEED_FTIME */
+/* use native WIN32 time API */
+#endif /* NEED_FTIME */
+
+#if defined(HAVE_SIGNAL_H)
+#include <signal.h>
+#endif /* HAVE_SIGNAL_H */
+
+#include <limits.h>
+
+/*
+ * Boolean values to make us independent of system includes.
+ */
+enum {
+ PTW32_FALSE = 0,
+ PTW32_TRUE = (! PTW32_FALSE)
+};
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#if !defined(PTW32_CONFIG_H)
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(__MINGW64__)
+# define HAVE_STRUCT_TIMESPEC
+# define HAVE_MODE_T
+# elif defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+#if defined(NEED_ERRNO)
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Several systems don't define some error numbers.
+ */
+#if !defined(ENOTSUP)
+# define ENOTSUP 48 /* This is the value in Solaris. */
+#endif
+
+#if !defined(ETIMEDOUT)
+# define ETIMEDOUT 10060 /* Same as WSAETIMEDOUT */
+#endif
+
+#if !defined(ENOSYS)
+# define ENOSYS 140 /* Semi-arbitrary value */
+#endif
+
+#if !defined(EDEADLK)
+# if defined(EDEADLOCK)
+# define EDEADLK EDEADLOCK
+# else
+# define EDEADLK 36 /* This is the value in MSVC. */
+# endif
+#endif
+
+/* POSIX 2008 - related to robust mutexes */
+#if !defined(EOWNERDEAD)
+# define EOWNERDEAD 43
+#endif
+#if !defined(ENOTRECOVERABLE)
+# define ENOTRECOVERABLE 44
+#endif
+
+#include "sched.h"
+
+/*
+ * To avoid including windows.h we define only those things that we
+ * actually need from it.
+ */
+#if !defined(PTW32_INCLUDE_WINDOWS_H)
+#if !defined(HANDLE)
+# define PTW32__HANDLE_DEF
+# define HANDLE void *
+#endif
+#if !defined(DWORD)
+# define PTW32__DWORD_DEF
+# define DWORD unsigned long
+#endif
+#endif
+
+#if !defined(HAVE_STRUCT_TIMESPEC)
+#define HAVE_STRUCT_TIMESPEC
+#if !defined(_TIMESPEC_DEFINED)
+#define _TIMESPEC_DEFINED
+struct timespec {
+ time_t tv_sec;
+ long tv_nsec;
+};
+#endif /* _TIMESPEC_DEFINED */
+#endif /* HAVE_STRUCT_TIMESPEC */
+
+#if !defined(SIG_BLOCK)
+#define SIG_BLOCK 0
+#endif /* SIG_BLOCK */
+
+#if !defined(SIG_UNBLOCK)
+#define SIG_UNBLOCK 1
+#endif /* SIG_UNBLOCK */
+
+#if !defined(SIG_SETMASK)
+#define SIG_SETMASK 2
+#endif /* SIG_SETMASK */
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif /* __cplusplus */
+
+/*
+ * -------------------------------------------------------------
+ *
+ * POSIX 1003.1-2001 Options
+ * =========================
+ *
+ * Options are normally set in <unistd.h>, which is not provided
+ * with pthreads-win32.
+ *
+ * For conformance with the Single Unix Specification (version 3), all of the
+ * options below are defined, and have a value of either -1 (not supported)
+ * or 200112L (supported).
+ *
+ * These options can neither be left undefined nor have a value of 0, because
+ * either indicates that sysconf(), which is not implemented, may be used at
+ * runtime to check the status of the option.
+ *
+ * _POSIX_THREADS (== 200112L)
+ * If == 200112L, you can use threads
+ *
+ * _POSIX_THREAD_ATTR_STACKSIZE (== 200112L)
+ * If == 200112L, you can control the size of a thread's
+ * stack
+ * pthread_attr_getstacksize
+ * pthread_attr_setstacksize
+ *
+ * _POSIX_THREAD_ATTR_STACKADDR (== -1)
+ * If == 200112L, you can allocate and control a thread's
+ * stack. If not supported, the following functions
+ * will return ENOSYS, indicating they are not
+ * supported:
+ * pthread_attr_getstackaddr
+ * pthread_attr_setstackaddr
+ *
+ * _POSIX_THREAD_PRIORITY_SCHEDULING (== -1)
+ * If == 200112L, you can use realtime scheduling.
+ * This option indicates that the behaviour of some
+ * implemented functions conforms to the additional TPS
+ * requirements in the standard. E.g. rwlocks favour
+ * writers over readers when threads have equal priority.
+ *
+ * _POSIX_THREAD_PRIO_INHERIT (== -1)
+ * If == 200112L, you can create priority inheritance
+ * mutexes.
+ * pthread_mutexattr_getprotocol +
+ * pthread_mutexattr_setprotocol +
+ *
+ * _POSIX_THREAD_PRIO_PROTECT (== -1)
+ * If == 200112L, you can create priority ceiling mutexes
+ * Indicates the availability of:
+ * pthread_mutex_getprioceiling
+ * pthread_mutex_setprioceiling
+ * pthread_mutexattr_getprioceiling
+ * pthread_mutexattr_getprotocol +
+ * pthread_mutexattr_setprioceiling
+ * pthread_mutexattr_setprotocol +
+ *
+ * _POSIX_THREAD_PROCESS_SHARED (== -1)
+ * If set, you can create mutexes and condition
+ * variables that can be shared with another
+ * process.If set, indicates the availability
+ * of:
+ * pthread_mutexattr_getpshared
+ * pthread_mutexattr_setpshared
+ * pthread_condattr_getpshared
+ * pthread_condattr_setpshared
+ *
+ * _POSIX_THREAD_SAFE_FUNCTIONS (== 200112L)
+ * If == 200112L you can use the special *_r library
+ * functions that provide thread-safe behaviour
+ *
+ * _POSIX_READER_WRITER_LOCKS (== 200112L)
+ * If == 200112L, you can use read/write locks
+ *
+ * _POSIX_SPIN_LOCKS (== 200112L)
+ * If == 200112L, you can use spin locks
+ *
+ * _POSIX_BARRIERS (== 200112L)
+ * If == 200112L, you can use barriers
+ *
+ * + These functions provide both 'inherit' and/or
+ * 'protect' protocol, based upon these macro
+ * settings.
+ *
+ * -------------------------------------------------------------
+ */
+
+/*
+ * POSIX Options
+ */
+#undef _POSIX_THREADS
+#define _POSIX_THREADS 200809L
+
+#undef _POSIX_READER_WRITER_LOCKS
+#define _POSIX_READER_WRITER_LOCKS 200809L
+
+#undef _POSIX_SPIN_LOCKS
+#define _POSIX_SPIN_LOCKS 200809L
+
+#undef _POSIX_BARRIERS
+#define _POSIX_BARRIERS 200809L
+
+#undef _POSIX_THREAD_SAFE_FUNCTIONS
+#define _POSIX_THREAD_SAFE_FUNCTIONS 200809L
+
+#undef _POSIX_THREAD_ATTR_STACKSIZE
+#define _POSIX_THREAD_ATTR_STACKSIZE 200809L
+
+/*
+ * The following options are not supported
+ */
+#undef _POSIX_THREAD_ATTR_STACKADDR
+#define _POSIX_THREAD_ATTR_STACKADDR -1
+
+#undef _POSIX_THREAD_PRIO_INHERIT
+#define _POSIX_THREAD_PRIO_INHERIT -1
+
+#undef _POSIX_THREAD_PRIO_PROTECT
+#define _POSIX_THREAD_PRIO_PROTECT -1
+
+/* TPS is not fully supported. */
+#undef _POSIX_THREAD_PRIORITY_SCHEDULING
+#define _POSIX_THREAD_PRIORITY_SCHEDULING -1
+
+#undef _POSIX_THREAD_PROCESS_SHARED
+#define _POSIX_THREAD_PROCESS_SHARED -1
+
+
+/*
+ * POSIX 1003.1-2001 Limits
+ * ===========================
+ *
+ * These limits are normally set in <limits.h>, which is not provided with
+ * pthreads-win32.
+ *
+ * PTHREAD_DESTRUCTOR_ITERATIONS
+ * Maximum number of attempts to destroy
+ * a thread's thread-specific data on
+ * termination (must be at least 4)
+ *
+ * PTHREAD_KEYS_MAX
+ * Maximum number of thread-specific data keys
+ * available per process (must be at least 128)
+ *
+ * PTHREAD_STACK_MIN
+ * Minimum supported stack size for a thread
+ *
+ * PTHREAD_THREADS_MAX
+ * Maximum number of threads supported per
+ * process (must be at least 64).
+ *
+ * SEM_NSEMS_MAX
+ * The maximum number of semaphores a process can have.
+ * (must be at least 256)
+ *
+ * SEM_VALUE_MAX
+ * The maximum value a semaphore can have.
+ * (must be at least 32767)
+ *
+ */
+#undef _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+#define _POSIX_THREAD_DESTRUCTOR_ITERATIONS 4
+
+#undef PTHREAD_DESTRUCTOR_ITERATIONS
+#define PTHREAD_DESTRUCTOR_ITERATIONS _POSIX_THREAD_DESTRUCTOR_ITERATIONS
+
+#undef _POSIX_THREAD_KEYS_MAX
+#define _POSIX_THREAD_KEYS_MAX 128
+
+#undef PTHREAD_KEYS_MAX
+#define PTHREAD_KEYS_MAX _POSIX_THREAD_KEYS_MAX
+
+#undef PTHREAD_STACK_MIN
+#define PTHREAD_STACK_MIN 0
+
+#undef _POSIX_THREAD_THREADS_MAX
+#define _POSIX_THREAD_THREADS_MAX 64
+
+ /* Arbitrary value */
+#undef PTHREAD_THREADS_MAX
+#define PTHREAD_THREADS_MAX 2019
+
+#undef _POSIX_SEM_NSEMS_MAX
+#define _POSIX_SEM_NSEMS_MAX 256
+
+ /* Arbitrary value */
+#undef SEM_NSEMS_MAX
+#define SEM_NSEMS_MAX 1024
+
+#undef _POSIX_SEM_VALUE_MAX
+#define _POSIX_SEM_VALUE_MAX 32767
+
+#undef SEM_VALUE_MAX
+#define SEM_VALUE_MAX INT_MAX
+
+
+#if defined(__GNUC__) && !defined(__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the library, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the library,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#if !defined(PTW32_STATIC_LIB)
+# if defined(PTW32_BUILD)
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * The Open Watcom C/C++ compiler uses a non-standard calling convention
+ * that passes function args in registers unless __cdecl is explicitly specified
+ * in exposed function prototypes.
+ *
+ * We force all calls to cdecl even though this could slow Watcom code down
+ * slightly. If you know that the Watcom compiler will be used to build both
+ * the DLL and application, then you can probably define this as a null string.
+ * Remember that pthread.h (this file) is used for both the DLL and application builds.
+ */
+#define PTW32_CDECL __cdecl
+
+#if defined(_UWIN) && PTW32_LEVEL >= PTW32_LEVEL_MAX
+# include <sys/types.h>
+#else
+/*
+ * Generic handle type - intended to extend uniqueness beyond
+ * that available with a simple pointer. It should scale for either
+ * IA-32 or IA-64.
+ */
+typedef struct {
+ void * p; /* Pointer to actual object */
+ unsigned int x; /* Extra information - reuse count etc */
+} ptw32_handle_t;
+
+typedef ptw32_handle_t pthread_t;
+typedef struct pthread_attr_t_ * pthread_attr_t;
+typedef struct pthread_once_t_ pthread_once_t;
+typedef struct pthread_key_t_ * pthread_key_t;
+typedef struct pthread_mutex_t_ * pthread_mutex_t;
+typedef struct pthread_mutexattr_t_ * pthread_mutexattr_t;
+typedef struct pthread_cond_t_ * pthread_cond_t;
+typedef struct pthread_condattr_t_ * pthread_condattr_t;
+#endif
+typedef struct pthread_rwlock_t_ * pthread_rwlock_t;
+typedef struct pthread_rwlockattr_t_ * pthread_rwlockattr_t;
+typedef struct pthread_spinlock_t_ * pthread_spinlock_t;
+typedef struct pthread_barrier_t_ * pthread_barrier_t;
+typedef struct pthread_barrierattr_t_ * pthread_barrierattr_t;
+
+/*
+ * ====================
+ * ====================
+ * POSIX Threads
+ * ====================
+ * ====================
+ */
+
+enum {
+/*
+ * pthread_attr_{get,set}detachstate
+ */
+ PTHREAD_CREATE_JOINABLE = 0, /* Default */
+ PTHREAD_CREATE_DETACHED = 1,
+
+/*
+ * pthread_attr_{get,set}inheritsched
+ */
+ PTHREAD_INHERIT_SCHED = 0,
+ PTHREAD_EXPLICIT_SCHED = 1, /* Default */
+
+/*
+ * pthread_{get,set}scope
+ */
+ PTHREAD_SCOPE_PROCESS = 0,
+ PTHREAD_SCOPE_SYSTEM = 1, /* Default */
+
+/*
+ * pthread_setcancelstate paramters
+ */
+ PTHREAD_CANCEL_ENABLE = 0, /* Default */
+ PTHREAD_CANCEL_DISABLE = 1,
+
+/*
+ * pthread_setcanceltype parameters
+ */
+ PTHREAD_CANCEL_ASYNCHRONOUS = 0,
+ PTHREAD_CANCEL_DEFERRED = 1, /* Default */
+
+/*
+ * pthread_mutexattr_{get,set}pshared
+ * pthread_condattr_{get,set}pshared
+ */
+ PTHREAD_PROCESS_PRIVATE = 0,
+ PTHREAD_PROCESS_SHARED = 1,
+
+/*
+ * pthread_mutexattr_{get,set}robust
+ */
+ PTHREAD_MUTEX_STALLED = 0, /* Default */
+ PTHREAD_MUTEX_ROBUST = 1,
+
+/*
+ * pthread_barrier_wait
+ */
+ PTHREAD_BARRIER_SERIAL_THREAD = -1
+};
+
+/*
+ * ====================
+ * ====================
+ * Cancelation
+ * ====================
+ * ====================
+ */
+#define PTHREAD_CANCELED ((void *)(size_t) -1)
+
+
+/*
+ * ====================
+ * ====================
+ * Once Key
+ * ====================
+ * ====================
+ */
+#define PTHREAD_ONCE_INIT { PTW32_FALSE, 0, 0, 0}
+
+struct pthread_once_t_
+{
+ int done; /* indicates if user function has been executed */
+ void * lock;
+ int reserved1;
+ int reserved2;
+};
+
+
+/*
+ * ====================
+ * ====================
+ * Object initialisers
+ * ====================
+ * ====================
+ */
+#define PTHREAD_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -1)
+#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -2)
+#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER ((pthread_mutex_t)(size_t) -3)
+
+/*
+ * Compatibility with LinuxThreads
+ */
+#define PTHREAD_RECURSIVE_MUTEX_INITIALIZER_NP PTHREAD_RECURSIVE_MUTEX_INITIALIZER
+#define PTHREAD_ERRORCHECK_MUTEX_INITIALIZER_NP PTHREAD_ERRORCHECK_MUTEX_INITIALIZER
+
+#define PTHREAD_COND_INITIALIZER ((pthread_cond_t)(size_t) -1)
+
+#define PTHREAD_RWLOCK_INITIALIZER ((pthread_rwlock_t)(size_t) -1)
+
+#define PTHREAD_SPINLOCK_INITIALIZER ((pthread_spinlock_t)(size_t) -1)
+
+
+/*
+ * Mutex types.
+ */
+enum
+{
+ /* Compatibility with LinuxThreads */
+ PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_RECURSIVE_NP,
+ PTHREAD_MUTEX_ERRORCHECK_NP,
+ PTHREAD_MUTEX_TIMED_NP = PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_ADAPTIVE_NP = PTHREAD_MUTEX_FAST_NP,
+ /* For compatibility with POSIX */
+ PTHREAD_MUTEX_NORMAL = PTHREAD_MUTEX_FAST_NP,
+ PTHREAD_MUTEX_RECURSIVE = PTHREAD_MUTEX_RECURSIVE_NP,
+ PTHREAD_MUTEX_ERRORCHECK = PTHREAD_MUTEX_ERRORCHECK_NP,
+ PTHREAD_MUTEX_DEFAULT = PTHREAD_MUTEX_NORMAL
+};
+
+
+typedef struct ptw32_cleanup_t ptw32_cleanup_t;
+
+#if defined(_MSC_VER)
+/* Disable MSVC 'anachronism used' warning */
+#pragma warning( disable : 4229 )
+#endif
+
+typedef void (* PTW32_CDECL ptw32_cleanup_callback_t)(void *);
+
+#if defined(_MSC_VER)
+#pragma warning( default : 4229 )
+#endif
+
+struct ptw32_cleanup_t
+{
+ ptw32_cleanup_callback_t routine;
+ void *arg;
+ struct ptw32_cleanup_t *prev;
+};
+
+#if defined(__CLEANUP_SEH)
+ /*
+ * WIN32 SEH version of cancel cleanup.
+ */
+
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ ptw32_cleanup_t _cleanup; \
+ \
+ _cleanup.routine = (ptw32_cleanup_callback_t)(_rout); \
+ _cleanup.arg = (_arg); \
+ __try \
+ { \
+
+#define pthread_cleanup_pop( _execute ) \
+ } \
+ __finally \
+ { \
+ if( _execute || AbnormalTermination()) \
+ { \
+ (*(_cleanup.routine))( _cleanup.arg ); \
+ } \
+ } \
+ }
+
+#else /* __CLEANUP_SEH */
+
+#if defined(__CLEANUP_C)
+
+ /*
+ * C implementation of PThreads cancel cleanup
+ */
+
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ ptw32_cleanup_t _cleanup; \
+ \
+ ptw32_push_cleanup( &_cleanup, (ptw32_cleanup_callback_t) (_rout), (_arg) ); \
+
+#define pthread_cleanup_pop( _execute ) \
+ (void) ptw32_pop_cleanup( _execute ); \
+ }
+
+#else /* __CLEANUP_C */
+
+#if defined(__CLEANUP_CXX)
+
+ /*
+ * C++ version of cancel cleanup.
+ * - John E. Bossom.
+ */
+
+ class PThreadCleanup {
+ /*
+ * PThreadCleanup
+ *
+ * Purpose
+ * This class is a C++ helper class that is
+ * used to implement pthread_cleanup_push/
+ * pthread_cleanup_pop.
+ * The destructor of this class automatically
+ * pops the pushed cleanup routine regardless
+ * of how the code exits the scope
+ * (i.e. such as by an exception)
+ */
+ ptw32_cleanup_callback_t cleanUpRout;
+ void * obj;
+ int executeIt;
+
+ public:
+ PThreadCleanup() :
+ cleanUpRout( 0 ),
+ obj( 0 ),
+ executeIt( 0 )
+ /*
+ * No cleanup performed
+ */
+ {
+ }
+
+ PThreadCleanup(
+ ptw32_cleanup_callback_t routine,
+ void * arg ) :
+ cleanUpRout( routine ),
+ obj( arg ),
+ executeIt( 1 )
+ /*
+ * Registers a cleanup routine for 'arg'
+ */
+ {
+ }
+
+ ~PThreadCleanup()
+ {
+ if ( executeIt && ((void *) cleanUpRout != (void *) 0) )
+ {
+ (void) (*cleanUpRout)( obj );
+ }
+ }
+
+ void execute( int exec )
+ {
+ executeIt = exec;
+ }
+ };
+
+ /*
+ * C++ implementation of PThreads cancel cleanup;
+ * This implementation takes advantage of a helper
+ * class who's destructor automatically calls the
+ * cleanup routine if we exit our scope weirdly
+ */
+#define pthread_cleanup_push( _rout, _arg ) \
+ { \
+ PThreadCleanup cleanup((ptw32_cleanup_callback_t)(_rout), \
+ (void *) (_arg) );
+
+#define pthread_cleanup_pop( _execute ) \
+ cleanup.execute( _execute ); \
+ }
+
+#else
+
+#error ERROR [__FILE__, line __LINE__]: Cleanup type undefined.
+
+#endif /* __CLEANUP_CXX */
+
+#endif /* __CLEANUP_C */
+
+#endif /* __CLEANUP_SEH */
+
+/*
+ * ===============
+ * ===============
+ * Methods
+ * ===============
+ * ===============
+ */
+
+/*
+ * PThread Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_init (pthread_attr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_destroy (pthread_attr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getdetachstate (const pthread_attr_t * attr,
+ int *detachstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstackaddr (const pthread_attr_t * attr,
+ void **stackaddr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getstacksize (const pthread_attr_t * attr,
+ size_t * stacksize);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setdetachstate (pthread_attr_t * attr,
+ int detachstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstackaddr (pthread_attr_t * attr,
+ void *stackaddr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setstacksize (pthread_attr_t * attr,
+ size_t stacksize);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedparam (const pthread_attr_t *attr,
+ struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedparam (pthread_attr_t *attr,
+ const struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setschedpolicy (pthread_attr_t *,
+ int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getschedpolicy (const pthread_attr_t *,
+ int *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setinheritsched(pthread_attr_t * attr,
+ int inheritsched);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getinheritsched(const pthread_attr_t * attr,
+ int * inheritsched);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_setscope (pthread_attr_t *,
+ int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_attr_getscope (const pthread_attr_t *,
+ int *);
+
+/*
+ * PThread Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_create (pthread_t * tid,
+ const pthread_attr_t * attr,
+ void *(PTW32_CDECL *start) (void *),
+ void *arg);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_detach (pthread_t tid);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_equal (pthread_t t1,
+ pthread_t t2);
+
+PTW32_DLLPORT void PTW32_CDECL pthread_exit (void *value_ptr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_join (pthread_t thread,
+ void **value_ptr);
+
+PTW32_DLLPORT pthread_t PTW32_CDECL pthread_self (void);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cancel (pthread_t thread);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setcancelstate (int state,
+ int *oldstate);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setcanceltype (int type,
+ int *oldtype);
+
+PTW32_DLLPORT void PTW32_CDECL pthread_testcancel (void);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_once (pthread_once_t * once_control,
+ void (PTW32_CDECL *init_routine) (void));
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+PTW32_DLLPORT ptw32_cleanup_t * PTW32_CDECL ptw32_pop_cleanup (int execute);
+
+PTW32_DLLPORT void PTW32_CDECL ptw32_push_cleanup (ptw32_cleanup_t * cleanup,
+ ptw32_cleanup_callback_t routine,
+ void *arg);
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Thread Specific Data Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_key_create (pthread_key_t * key,
+ void (PTW32_CDECL *destructor) (void *));
+
+PTW32_DLLPORT int PTW32_CDECL pthread_key_delete (pthread_key_t key);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setspecific (pthread_key_t key,
+ const void *value);
+
+PTW32_DLLPORT void * PTW32_CDECL pthread_getspecific (pthread_key_t key);
+
+
+/*
+ * Mutex Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_init (pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_destroy (pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getpshared (const pthread_mutexattr_t
+ * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setpshared (pthread_mutexattr_t * attr,
+ int pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_settype (pthread_mutexattr_t * attr, int kind);
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_gettype (const pthread_mutexattr_t * attr, int *kind);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setrobust(
+ pthread_mutexattr_t *attr,
+ int robust);
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getrobust(
+ const pthread_mutexattr_t * attr,
+ int * robust);
+
+/*
+ * Barrier Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_init (pthread_barrierattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_destroy (pthread_barrierattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_getpshared (const pthread_barrierattr_t
+ * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrierattr_setpshared (pthread_barrierattr_t * attr,
+ int pshared);
+
+/*
+ * Mutex Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_init (pthread_mutex_t * mutex,
+ const pthread_mutexattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_destroy (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_lock (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_timedlock(pthread_mutex_t * mutex,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_trylock (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_unlock (pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_mutex_consistent (pthread_mutex_t * mutex);
+
+/*
+ * Spinlock Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_init (pthread_spinlock_t * lock, int pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_destroy (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_lock (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_trylock (pthread_spinlock_t * lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_spin_unlock (pthread_spinlock_t * lock);
+
+/*
+ * Barrier Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_init (pthread_barrier_t * barrier,
+ const pthread_barrierattr_t * attr,
+ unsigned int count);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_destroy (pthread_barrier_t * barrier);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_barrier_wait (pthread_barrier_t * barrier);
+
+/*
+ * Condition Variable Attribute Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_init (pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_destroy (pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_getpshared (const pthread_condattr_t * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_condattr_setpshared (pthread_condattr_t * attr,
+ int pshared);
+
+/*
+ * Condition Variable Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_init (pthread_cond_t * cond,
+ const pthread_condattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_destroy (pthread_cond_t * cond);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_wait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_signal (pthread_cond_t * cond);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_cond_broadcast (pthread_cond_t * cond);
+
+/*
+ * Scheduling
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_setschedparam (pthread_t thread,
+ int policy,
+ const struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_getschedparam (pthread_t thread,
+ int *policy,
+ struct sched_param *param);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_setconcurrency (int);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_getconcurrency (void);
+
+/*
+ * Read-Write Lock Functions
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_init(pthread_rwlock_t *lock,
+ const pthread_rwlockattr_t *attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_destroy(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_tryrdlock(pthread_rwlock_t *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_trywrlock(pthread_rwlock_t *);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_rdlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedrdlock(pthread_rwlock_t *lock,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_wrlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_timedwrlock(pthread_rwlock_t *lock,
+ const struct timespec *abstime);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlock_unlock(pthread_rwlock_t *lock);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_init (pthread_rwlockattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_destroy (pthread_rwlockattr_t * attr);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_getpshared (const pthread_rwlockattr_t * attr,
+ int *pshared);
+
+PTW32_DLLPORT int PTW32_CDECL pthread_rwlockattr_setpshared (pthread_rwlockattr_t * attr,
+ int pshared);
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX - 1
+
+/*
+ * Signal Functions. Should be defined in <signal.h> but MSVC and MinGW32
+ * already have signal.h that don't define these.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_kill(pthread_t thread, int sig);
+
+/*
+ * Non-portable functions
+ */
+
+/*
+ * Compatibility with Linux.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_setkind_np(pthread_mutexattr_t * attr,
+ int kind);
+PTW32_DLLPORT int PTW32_CDECL pthread_mutexattr_getkind_np(pthread_mutexattr_t * attr,
+ int *kind);
+
+/*
+ * Possibly supported by other POSIX threads implementations
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_delay_np (struct timespec * interval);
+PTW32_DLLPORT int PTW32_CDECL pthread_num_processors_np(void);
+PTW32_DLLPORT unsigned __int64 PTW32_CDECL pthread_getunique_np(pthread_t thread);
+
+/*
+ * Useful if an application wants to statically link
+ * the lib rather than load the DLL at run-time.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_attach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_process_detach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_attach_np(void);
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_thread_detach_np(void);
+
+/*
+ * Features that are auto-detected at load/run time.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthread_win32_test_features_np(int);
+enum ptw32_features {
+ PTW32_SYSTEM_INTERLOCKED_COMPARE_EXCHANGE = 0x0001, /* System provides it. */
+ PTW32_ALERTABLE_ASYNC_CANCEL = 0x0002 /* Can cancel blocked threads. */
+};
+
+/*
+ * Register a system time change with the library.
+ * Causes the library to perform various functions
+ * in response to the change. Should be called whenever
+ * the application's top level window receives a
+ * WM_TIMECHANGE message. It can be passed directly to
+ * pthread_create() as a new thread if desired.
+ */
+PTW32_DLLPORT void * PTW32_CDECL pthread_timechange_handler_np(void *);
+
+#endif /*PTW32_LEVEL >= PTW32_LEVEL_MAX - 1 */
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+
+/*
+ * Returns the Win32 HANDLE for the POSIX thread.
+ */
+PTW32_DLLPORT HANDLE PTW32_CDECL pthread_getw32threadhandle_np(pthread_t thread);
+/*
+ * Returns the win32 thread ID for POSIX thread.
+ */
+PTW32_DLLPORT DWORD PTW32_CDECL pthread_getw32threadid_np (pthread_t thread);
+
+
+/*
+ * Protected Methods
+ *
+ * This function blocks until the given WIN32 handle
+ * is signaled or pthread_cancel had been called.
+ * This function allows the caller to hook into the
+ * PThreads cancel mechanism. It is implemented using
+ *
+ * WaitForMultipleObjects
+ *
+ * on 'waitHandle' and a manually reset WIN32 Event
+ * used to implement pthread_cancel. The 'timeout'
+ * argument to TimedWait is simply passed to
+ * WaitForMultipleObjects.
+ */
+PTW32_DLLPORT int PTW32_CDECL pthreadCancelableWait (HANDLE waitHandle);
+PTW32_DLLPORT int PTW32_CDECL pthreadCancelableTimedWait (HANDLE waitHandle,
+ DWORD timeout);
+
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+/*
+ * Thread-Safe C Runtime Library Mappings.
+ */
+#if !defined(_UWIN)
+# if defined(NEED_ERRNO)
+ PTW32_DLLPORT int * PTW32_CDECL _errno( void );
+# else
+# if !defined(errno)
+# if (defined(_MT) || defined(_DLL))
+ __declspec(dllimport) extern int * __cdecl _errno(void);
+# define errno (*_errno())
+# endif
+# endif
+# endif
+#endif
+
+/*
+ * Some compiler environments don't define some things.
+ */
+#if defined(__BORLANDC__)
+# define _ftime ftime
+# define _timeb timeb
+#endif
+
+#if defined(__cplusplus)
+
+/*
+ * Internal exceptions
+ */
+class ptw32_exception {};
+class ptw32_exception_cancel : public ptw32_exception {};
+class ptw32_exception_exit : public ptw32_exception {};
+
+#endif
+
+#if PTW32_LEVEL >= PTW32_LEVEL_MAX
+
+/* FIXME: This is only required if the library was built using SEH */
+/*
+ * Get internal SEH tag
+ */
+PTW32_DLLPORT DWORD PTW32_CDECL ptw32_get_exception_services_code(void);
+
+#endif /* PTW32_LEVEL >= PTW32_LEVEL_MAX */
+
+#if !defined(PTW32_BUILD)
+
+#if defined(__CLEANUP_SEH)
+
+/*
+ * Redefine the SEH __except keyword to ensure that applications
+ * propagate our internal exceptions up to the library's internal handlers.
+ */
+#define __except( E ) \
+ __except( ( GetExceptionCode() == ptw32_get_exception_services_code() ) \
+ ? EXCEPTION_CONTINUE_SEARCH : ( E ) )
+
+#endif /* __CLEANUP_SEH */
+
+#if defined(__CLEANUP_CXX)
+
+/*
+ * Redefine the C++ catch keyword to ensure that applications
+ * propagate our internal exceptions up to the library's internal handlers.
+ */
+#if defined(_MSC_VER)
+ /*
+ * WARNING: Replace any 'catch( ... )' with 'PtW32CatchAll'
+ * if you want Pthread-Win32 cancelation and pthread_exit to work.
+ */
+
+#if !defined(PtW32NoCatchWarn)
+
+#pragma message("Specify \"/DPtW32NoCatchWarn\" compiler flag to skip this message.")
+#pragma message("------------------------------------------------------------------")
+#pragma message("When compiling applications with MSVC++ and C++ exception handling:")
+#pragma message(" Replace any 'catch( ... )' in routines called from POSIX threads")
+#pragma message(" with 'PtW32CatchAll' or 'CATCHALL' if you want POSIX thread")
+#pragma message(" cancelation and pthread_exit to work. For example:")
+#pragma message("")
+#pragma message(" #if defined(PtW32CatchAll)")
+#pragma message(" PtW32CatchAll")
+#pragma message(" #else")
+#pragma message(" catch(...)")
+#pragma message(" #endif")
+#pragma message(" {")
+#pragma message(" /* Catchall block processing */")
+#pragma message(" }")
+#pragma message("------------------------------------------------------------------")
+
+#endif
+
+#define PtW32CatchAll \
+ catch( ptw32_exception & ) { throw; } \
+ catch( ... )
+
+#else /* _MSC_VER */
+
+#define catch( E ) \
+ catch( ptw32_exception & ) { throw; } \
+ catch( E )
+
+#endif /* _MSC_VER */
+
+#endif /* __CLEANUP_CXX */
+
+#endif /* ! PTW32_BUILD */
+
+#if defined(__cplusplus)
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#if defined(PTW32__HANDLE_DEF)
+# undef HANDLE
+#endif
+#if defined(PTW32__DWORD_DEF)
+# undef DWORD
+#endif
+
+#undef PTW32_LEVEL
+#undef PTW32_LEVEL_MAX
+
+#endif /* ! RC_INVOKED */
+
+#endif /* PTHREAD_H */
diff --git a/3rdparty/pthreads-win32/pthread_attr_destroy.c b/3rdparty/pthreads-win32/pthread_attr_destroy.c
new file mode 100644
index 0000000..8b3e04c
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_destroy.c
@@ -0,0 +1,79 @@
+/*
+ * pthread_attr_destroy.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_destroy (pthread_attr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Destroys a thread attributes object.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ *
+ * DESCRIPTION
+ * Destroys a thread attributes object.
+ *
+ * NOTES:
+ * 1) Does not affect threads created with 'attr'.
+ *
+ * RESULTS
+ * 0 successfully destroyed attr,
+ * EINVAL 'attr' is invalid.
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * Set the attribute object to a specific invalid value.
+ */
+ (*attr)->valid = 0;
+ free (*attr);
+ *attr = NULL;
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getdetachstate.c b/3rdparty/pthreads-win32/pthread_attr_getdetachstate.c
new file mode 100644
index 0000000..188533b
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getdetachstate.c
@@ -0,0 +1,86 @@
+/*
+ * pthread_attr_getdetachstate.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_getdetachstate (const pthread_attr_t * attr, int *detachstate)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function determines whether threads created with
+ * 'attr' will run detached.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * detachstate
+ * pointer to an integer into which is returned one
+ * of:
+ *
+ * PTHREAD_CREATE_JOINABLE
+ * Thread ID is valid, must be joined
+ *
+ * PTHREAD_CREATE_DETACHED
+ * Thread ID is invalid, cannot be joined,
+ * canceled, or modified
+ *
+ *
+ * DESCRIPTION
+ * This function determines whether threads created with
+ * 'attr' will run detached.
+ *
+ * NOTES:
+ * 1) You cannot join or cancel detached threads.
+ *
+ * RESULTS
+ * 0 successfully retrieved detach state,
+ * EINVAL 'attr' is invalid
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (ptw32_is_attr (attr) != 0 || detachstate == NULL)
+ {
+ return EINVAL;
+ }
+
+ *detachstate = (*attr)->detachstate;
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getinheritsched.c b/3rdparty/pthreads-win32/pthread_attr_getinheritsched.c
new file mode 100644
index 0000000..9c6885e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getinheritsched.c
@@ -0,0 +1,51 @@
+/*
+ * pthread_attr_getinheritsched.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_getinheritsched (const pthread_attr_t * attr, int *inheritsched)
+{
+ if (ptw32_is_attr (attr) != 0 || inheritsched == NULL)
+ {
+ return EINVAL;
+ }
+
+ *inheritsched = (*attr)->inheritsched;
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getschedparam.c b/3rdparty/pthreads-win32/pthread_attr_getschedparam.c
new file mode 100644
index 0000000..ab89b22
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getschedparam.c
@@ -0,0 +1,52 @@
+/*
+ * pthread_attr_getschedparam.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_getschedparam (const pthread_attr_t * attr,
+ struct sched_param *param)
+{
+ if (ptw32_is_attr (attr) != 0 || param == NULL)
+ {
+ return EINVAL;
+ }
+
+ memcpy (param, &(*attr)->param, sizeof (*param));
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getschedpolicy.c b/3rdparty/pthreads-win32/pthread_attr_getschedpolicy.c
new file mode 100644
index 0000000..94a257d
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getschedpolicy.c
@@ -0,0 +1,61 @@
+/*
+ * pthread_attr_getschedpolicy.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_getschedpolicy (const pthread_attr_t * attr, int *policy)
+{
+ if (ptw32_is_attr (attr) != 0 || policy == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * Validate the policy arg.
+ * Check that a policy constant wasn't passed rather than &policy.
+ */
+ if (policy <= (int *) SCHED_MAX)
+ {
+ return EINVAL;
+ }
+
+ *policy = SCHED_OTHER;
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getscope.c b/3rdparty/pthreads-win32/pthread_attr_getscope.c
new file mode 100644
index 0000000..2efdb2f
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getscope.c
@@ -0,0 +1,54 @@
+/*
+ * pthread_attr_getscope.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+pthread_attr_getscope (const pthread_attr_t * attr, int *contentionscope)
+{
+#if defined(_POSIX_THREAD_PRIORITY_SCHEDULING)
+ *contentionscope = (*attr)->contentionscope;
+ return 0;
+#else
+ return ENOSYS;
+#endif
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getstackaddr.c b/3rdparty/pthreads-win32/pthread_attr_getstackaddr.c
new file mode 100644
index 0000000..1a2da01
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getstackaddr.c
@@ -0,0 +1,97 @@
+/*
+ * pthread_attr_getstackaddr.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+pthread_attr_getstackaddr (const pthread_attr_t * attr, void **stackaddr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function determines the address of the stack
+ * on which threads created with 'attr' will run.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * stackaddr
+ * pointer into which is returned the stack address.
+ *
+ *
+ * DESCRIPTION
+ * This function determines the address of the stack
+ * on which threads created with 'attr' will run.
+ *
+ * NOTES:
+ * 1) Function supported only if this macro is
+ * defined:
+ *
+ * _POSIX_THREAD_ATTR_STACKADDR
+ *
+ * 2) Create only one thread for each stack
+ * address..
+ *
+ * RESULTS
+ * 0 successfully retreived stack address,
+ * EINVAL 'attr' is invalid
+ * ENOSYS function not supported
+ *
+ * ------------------------------------------------------
+ */
+{
+#if defined( _POSIX_THREAD_ATTR_STACKADDR )
+
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ *stackaddr = (*attr)->stackaddr;
+ return 0;
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_ATTR_STACKADDR */
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_getstacksize.c b/3rdparty/pthreads-win32/pthread_attr_getstacksize.c
new file mode 100644
index 0000000..dff9230
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_getstacksize.c
@@ -0,0 +1,100 @@
+/*
+ * pthread_attr_getstacksize.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+pthread_attr_getstacksize (const pthread_attr_t * attr, size_t * stacksize)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function determines the size of the stack on
+ * which threads created with 'attr' will run.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * stacksize
+ * pointer to size_t into which is returned the
+ * stack size, in bytes.
+ *
+ *
+ * DESCRIPTION
+ * This function determines the size of the stack on
+ * which threads created with 'attr' will run.
+ *
+ * NOTES:
+ * 1) Function supported only if this macro is
+ * defined:
+ *
+ * _POSIX_THREAD_ATTR_STACKSIZE
+ *
+ * 2) Use on newly created attributes object to
+ * find the default stack size.
+ *
+ * RESULTS
+ * 0 successfully retrieved stack size,
+ * EINVAL 'attr' is invalid
+ * ENOSYS function not supported
+ *
+ * ------------------------------------------------------
+ */
+{
+#if defined(_POSIX_THREAD_ATTR_STACKSIZE)
+
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ /* Everything is okay. */
+ *stacksize = (*attr)->stacksize;
+ return 0;
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_ATTR_STACKSIZE */
+
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_init.c b/3rdparty/pthreads-win32/pthread_attr_init.c
new file mode 100644
index 0000000..ae9d3eb
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_init.c
@@ -0,0 +1,117 @@
+/*
+ * pthread_attr_init.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_init (pthread_attr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Initializes a thread attributes object with default
+ * attributes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ *
+ * DESCRIPTION
+ * Initializes a thread attributes object with default
+ * attributes.
+ *
+ * NOTES:
+ * 1) Used to define thread attributes
+ *
+ * RESULTS
+ * 0 successfully initialized attr,
+ * ENOMEM insufficient memory for attr.
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_attr_t attr_result;
+
+ if (attr == NULL)
+ {
+ /* This is disallowed. */
+ return EINVAL;
+ }
+
+ attr_result = (pthread_attr_t) malloc (sizeof (*attr_result));
+
+ if (attr_result == NULL)
+ {
+ return ENOMEM;
+ }
+
+#if defined(_POSIX_THREAD_ATTR_STACKSIZE)
+ /*
+ * Default to zero size. Unless changed explicitly this
+ * will allow Win32 to set the size to that of the
+ * main thread.
+ */
+ attr_result->stacksize = 0;
+#endif
+
+#if defined(_POSIX_THREAD_ATTR_STACKADDR)
+ /* FIXME: Set this to something sensible when we support it. */
+ attr_result->stackaddr = NULL;
+#endif
+
+ attr_result->detachstate = PTHREAD_CREATE_JOINABLE;
+
+#if defined(HAVE_SIGSET_T)
+ memset (&(attr_result->sigmask), 0, sizeof (sigset_t));
+#endif /* HAVE_SIGSET_T */
+
+ /*
+ * Win32 sets new threads to THREAD_PRIORITY_NORMAL and
+ * not to that of the parent thread. We choose to default to
+ * this arrangement.
+ */
+ attr_result->param.sched_priority = THREAD_PRIORITY_NORMAL;
+ attr_result->inheritsched = PTHREAD_EXPLICIT_SCHED;
+ attr_result->contentionscope = PTHREAD_SCOPE_SYSTEM;
+
+ attr_result->valid = PTW32_ATTR_VALID;
+
+ *attr = attr_result;
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setdetachstate.c b/3rdparty/pthreads-win32/pthread_attr_setdetachstate.c
new file mode 100644
index 0000000..784642a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setdetachstate.c
@@ -0,0 +1,91 @@
+/*
+ * pthread_attr_setdetachstate.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_setdetachstate (pthread_attr_t * attr, int detachstate)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function specifies whether threads created with
+ * 'attr' will run detached.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * detachstate
+ * an integer containing one of:
+ *
+ * PTHREAD_CREATE_JOINABLE
+ * Thread ID is valid, must be joined
+ *
+ * PTHREAD_CREATE_DETACHED
+ * Thread ID is invalid, cannot be joined,
+ * canceled, or modified
+ *
+ *
+ * DESCRIPTION
+ * This function specifies whether threads created with
+ * 'attr' will run detached.
+ *
+ * NOTES:
+ * 1) You cannot join or cancel detached threads.
+ *
+ * RESULTS
+ * 0 successfully set detach state,
+ * EINVAL 'attr' or 'detachstate' is invalid
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ if (detachstate != PTHREAD_CREATE_JOINABLE &&
+ detachstate != PTHREAD_CREATE_DETACHED)
+ {
+ return EINVAL;
+ }
+
+ (*attr)->detachstate = detachstate;
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setinheritsched.c b/3rdparty/pthreads-win32/pthread_attr_setinheritsched.c
new file mode 100644
index 0000000..e0a407a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setinheritsched.c
@@ -0,0 +1,57 @@
+/*
+ * pthread_attr_setinheritsched.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_setinheritsched (pthread_attr_t * attr, int inheritsched)
+{
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ if (PTHREAD_INHERIT_SCHED != inheritsched
+ && PTHREAD_EXPLICIT_SCHED != inheritsched)
+ {
+ return EINVAL;
+ }
+
+ (*attr)->inheritsched = inheritsched;
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setschedparam.c b/3rdparty/pthreads-win32/pthread_attr_setschedparam.c
new file mode 100644
index 0000000..f246bfa
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setschedparam.c
@@ -0,0 +1,63 @@
+/*
+ * pthread_attr_setschedparam.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_setschedparam (pthread_attr_t * attr,
+ const struct sched_param *param)
+{
+ int priority;
+
+ if (ptw32_is_attr (attr) != 0 || param == NULL)
+ {
+ return EINVAL;
+ }
+
+ priority = param->sched_priority;
+
+ /* Validate priority level. */
+ if (priority < sched_get_priority_min (SCHED_OTHER) ||
+ priority > sched_get_priority_max (SCHED_OTHER))
+ {
+ return EINVAL;
+ }
+
+ memcpy (&(*attr)->param, param, sizeof (*param));
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setschedpolicy.c b/3rdparty/pthreads-win32/pthread_attr_setschedpolicy.c
new file mode 100644
index 0000000..45ff165
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setschedpolicy.c
@@ -0,0 +1,55 @@
+/*
+ * pthread_attr_setschedpolicy.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_attr_setschedpolicy (pthread_attr_t * attr, int policy)
+{
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ if (policy != SCHED_OTHER)
+ {
+ return ENOTSUP;
+ }
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setscope.c b/3rdparty/pthreads-win32/pthread_attr_setscope.c
new file mode 100644
index 0000000..39a51df
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setscope.c
@@ -0,0 +1,62 @@
+/*
+ * pthread_attr_setscope.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+pthread_attr_setscope (pthread_attr_t * attr, int contentionscope)
+{
+#if defined(_POSIX_THREAD_PRIORITY_SCHEDULING)
+ switch (contentionscope)
+ {
+ case PTHREAD_SCOPE_SYSTEM:
+ (*attr)->contentionscope = contentionscope;
+ return 0;
+ case PTHREAD_SCOPE_PROCESS:
+ return ENOTSUP;
+ default:
+ return EINVAL;
+ }
+#else
+ return ENOSYS;
+#endif
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setstackaddr.c b/3rdparty/pthreads-win32/pthread_attr_setstackaddr.c
new file mode 100644
index 0000000..1316c06
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setstackaddr.c
@@ -0,0 +1,97 @@
+/*
+ * pthread_attr_setstackaddr.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_setstackaddr (pthread_attr_t * attr, void *stackaddr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Threads created with 'attr' will run on the stack
+ * starting at 'stackaddr'.
+ * Stack must be at least PTHREAD_STACK_MIN bytes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * stackaddr
+ * the address of the stack to use
+ *
+ *
+ * DESCRIPTION
+ * Threads created with 'attr' will run on the stack
+ * starting at 'stackaddr'.
+ * Stack must be at least PTHREAD_STACK_MIN bytes.
+ *
+ * NOTES:
+ * 1) Function supported only if this macro is
+ * defined:
+ *
+ * _POSIX_THREAD_ATTR_STACKADDR
+ *
+ * 2) Create only one thread for each stack
+ * address..
+ *
+ * 3) Ensure that stackaddr is aligned.
+ *
+ * RESULTS
+ * 0 successfully set stack address,
+ * EINVAL 'attr' is invalid
+ * ENOSYS function not supported
+ *
+ * ------------------------------------------------------
+ */
+{
+#if defined( _POSIX_THREAD_ATTR_STACKADDR )
+
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ (*attr)->stackaddr = stackaddr;
+ return 0;
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_ATTR_STACKADDR */
+}
diff --git a/3rdparty/pthreads-win32/pthread_attr_setstacksize.c b/3rdparty/pthreads-win32/pthread_attr_setstacksize.c
new file mode 100644
index 0000000..eb13589
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_attr_setstacksize.c
@@ -0,0 +1,110 @@
+/*
+ * pthread_attr_setstacksize.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_attr_setstacksize (pthread_attr_t * attr, size_t stacksize)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function specifies the size of the stack on
+ * which threads created with 'attr' will run.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_attr_t
+ *
+ * stacksize
+ * stack size, in bytes.
+ *
+ *
+ * DESCRIPTION
+ * This function specifies the size of the stack on
+ * which threads created with 'attr' will run.
+ *
+ * NOTES:
+ * 1) Function supported only if this macro is
+ * defined:
+ *
+ * _POSIX_THREAD_ATTR_STACKSIZE
+ *
+ * 2) Find the default first (using
+ * pthread_attr_getstacksize), then increase
+ * by multiplying.
+ *
+ * 3) Only use if thread needs more than the
+ * default.
+ *
+ * RESULTS
+ * 0 successfully set stack size,
+ * EINVAL 'attr' is invalid or stacksize too
+ * small or too big.
+ * ENOSYS function not supported
+ *
+ * ------------------------------------------------------
+ */
+{
+#if defined(_POSIX_THREAD_ATTR_STACKSIZE)
+
+#if PTHREAD_STACK_MIN > 0
+
+ /* Verify that the stack size is within range. */
+ if (stacksize < PTHREAD_STACK_MIN)
+ {
+ return EINVAL;
+ }
+
+#endif
+
+ if (ptw32_is_attr (attr) != 0)
+ {
+ return EINVAL;
+ }
+
+ /* Everything is okay. */
+ (*attr)->stacksize = stacksize;
+ return 0;
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_ATTR_STACKSIZE */
+
+}
diff --git a/3rdparty/pthreads-win32/pthread_barrier_destroy.c b/3rdparty/pthreads-win32/pthread_barrier_destroy.c
new file mode 100644
index 0000000..55163cc
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrier_destroy.c
@@ -0,0 +1,103 @@
+/*
+ * pthread_barrier_destroy.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_barrier_destroy (pthread_barrier_t * barrier)
+{
+ int result = 0;
+ pthread_barrier_t b;
+ ptw32_mcs_local_node_t node;
+
+ if (barrier == NULL || *barrier == (pthread_barrier_t) PTW32_OBJECT_INVALID)
+ {
+ return EINVAL;
+ }
+
+ if (0 != ptw32_mcs_lock_try_acquire(&(*barrier)->lock, &node))
+ {
+ return EBUSY;
+ }
+
+ b = *barrier;
+
+ if (b->nCurrentBarrierHeight < b->nInitialBarrierHeight)
+ {
+ result = EBUSY;
+ }
+ else
+ {
+ if (0 == (result = sem_destroy (&(b->semBarrierBreeched))))
+ {
+ *barrier = (pthread_barrier_t) PTW32_OBJECT_INVALID;
+ /*
+ * Release the lock before freeing b.
+ *
+ * FIXME: There may be successors which, when we release the lock,
+ * will be linked into b->lock, which will be corrupted at some
+ * point with undefined results for the application. To fix this
+ * will require changing pthread_barrier_t from a pointer to
+ * pthread_barrier_t_ to an instance. This is a change to the ABI
+ * and will require a major version number increment.
+ */
+ ptw32_mcs_lock_release(&node);
+ (void) free (b);
+ return 0;
+ }
+ else
+ {
+ /*
+ * This should not ever be reached.
+ * Restore the barrier to working condition before returning.
+ */
+ (void) sem_init (&(b->semBarrierBreeched), b->pshared, 0);
+ }
+
+ if (result != 0)
+ {
+ /*
+ * The barrier still exists and is valid
+ * in the event of any error above.
+ */
+ result = EBUSY;
+ }
+ }
+
+ ptw32_mcs_lock_release(&node);
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_barrier_init.c b/3rdparty/pthreads-win32/pthread_barrier_init.c
new file mode 100644
index 0000000..618bfae
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrier_init.c
@@ -0,0 +1,69 @@
+/*
+ * pthread_barrier_init.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrier_init (pthread_barrier_t * barrier,
+ const pthread_barrierattr_t * attr, unsigned int count)
+{
+ pthread_barrier_t b;
+
+ if (barrier == NULL || count == 0)
+ {
+ return EINVAL;
+ }
+
+ if (NULL != (b = (pthread_barrier_t) calloc (1, sizeof (*b))))
+ {
+ b->pshared = (attr != NULL && *attr != NULL
+ ? (*attr)->pshared : PTHREAD_PROCESS_PRIVATE);
+
+ b->nCurrentBarrierHeight = b->nInitialBarrierHeight = count;
+ b->lock = 0;
+
+ if (0 == sem_init (&(b->semBarrierBreeched), b->pshared, 0))
+ {
+ *barrier = b;
+ return 0;
+ }
+ (void) free (b);
+ }
+
+ return ENOMEM;
+}
diff --git a/3rdparty/pthreads-win32/pthread_barrier_wait.c b/3rdparty/pthreads-win32/pthread_barrier_wait.c
new file mode 100644
index 0000000..e0e97e6
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrier_wait.c
@@ -0,0 +1,104 @@
+/*
+ * pthread_barrier_wait.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrier_wait (pthread_barrier_t * barrier)
+{
+ int result;
+ pthread_barrier_t b;
+
+ ptw32_mcs_local_node_t node;
+
+ if (barrier == NULL || *barrier == (pthread_barrier_t) PTW32_OBJECT_INVALID)
+ {
+ return EINVAL;
+ }
+
+ ptw32_mcs_lock_acquire(&(*barrier)->lock, &node);
+
+ b = *barrier;
+ if (--b->nCurrentBarrierHeight == 0)
+ {
+ /*
+ * We are the last thread to arrive at the barrier before it releases us.
+ * Move our MCS local node to the global scope barrier handle so that the
+ * last thread out (not necessarily us) can release the lock.
+ */
+ ptw32_mcs_node_transfer(&b->proxynode, &node);
+
+ /*
+ * Any threads that have not quite entered sem_wait below when the
+ * multiple_post has completed will nevertheless continue through
+ * the semaphore (barrier).
+ */
+ result = (b->nInitialBarrierHeight > 1
+ ? sem_post_multiple (&(b->semBarrierBreeched),
+ b->nInitialBarrierHeight - 1) : 0);
+ }
+ else
+ {
+ ptw32_mcs_lock_release(&node);
+ /*
+ * Use the non-cancelable version of sem_wait().
+ *
+ * It is possible that all nInitialBarrierHeight-1 threads are
+ * at this point when the last thread enters the barrier, resets
+ * nCurrentBarrierHeight = nInitialBarrierHeight and leaves.
+ * If pthread_barrier_destroy is called at that moment then the
+ * barrier will be destroyed along with the semas.
+ */
+ result = ptw32_semwait (&(b->semBarrierBreeched));
+ }
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_INTERLOCKED_INCREMENT_LONG((PTW32_INTERLOCKED_LONGPTR)&b->nCurrentBarrierHeight)
+ == (PTW32_INTERLOCKED_LONG)b->nInitialBarrierHeight)
+ {
+ /*
+ * We are the last thread to cross this barrier
+ */
+ ptw32_mcs_lock_release(&b->proxynode);
+ if (0 == result)
+ {
+ result = PTHREAD_BARRIER_SERIAL_THREAD;
+ }
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_barrierattr_destroy.c b/3rdparty/pthreads-win32/pthread_barrierattr_destroy.c
new file mode 100644
index 0000000..5ab662e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrierattr_destroy.c
@@ -0,0 +1,83 @@
+/*
+ * pthread_barrier_attr_destroy.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrierattr_destroy (pthread_barrierattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Destroys a barrier attributes object. The object can
+ * no longer be used.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_barrierattr_t
+ *
+ *
+ * DESCRIPTION
+ * Destroys a barrier attributes object. The object can
+ * no longer be used.
+ *
+ * NOTES:
+ * 1) Does not affect barrieres created using 'attr'
+ *
+ * RESULTS
+ * 0 successfully released attr,
+ * EINVAL 'attr' is invalid.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+
+ if (attr == NULL || *attr == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ pthread_barrierattr_t ba = *attr;
+
+ *attr = NULL;
+ free (ba);
+ }
+
+ return (result);
+} /* pthread_barrierattr_destroy */
diff --git a/3rdparty/pthreads-win32/pthread_barrierattr_getpshared.c b/3rdparty/pthreads-win32/pthread_barrierattr_getpshared.c
new file mode 100644
index 0000000..44c467e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrierattr_getpshared.c
@@ -0,0 +1,95 @@
+/*
+ * pthread_barrier_attr_getpshared.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrierattr_getpshared (const pthread_barrierattr_t * attr,
+ int *pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Determine whether barriers created with 'attr' can be
+ * shared between processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_barrierattr_t
+ *
+ * pshared
+ * will be set to one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ *
+ * DESCRIPTION
+ * Mutexes creatd with 'attr' can be shared between
+ * processes if pthread_barrier_t variable is allocated
+ * in memory shared by these processes.
+ * NOTES:
+ * 1) pshared barriers MUST be allocated in shared
+ * memory.
+ * 2) The following macro is defined if shared barriers
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully retrieved attribute,
+ * EINVAL 'attr' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) && (pshared != NULL))
+ {
+ *pshared = (*attr)->pshared;
+ result = 0;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+} /* pthread_barrierattr_getpshared */
diff --git a/3rdparty/pthreads-win32/pthread_barrierattr_init.c b/3rdparty/pthreads-win32/pthread_barrierattr_init.c
new file mode 100644
index 0000000..342f8b0
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrierattr_init.c
@@ -0,0 +1,85 @@
+/*
+ * pthread_barrier_attr_init.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrierattr_init (pthread_barrierattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Initializes a barrier attributes object with default
+ * attributes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_barrierattr_t
+ *
+ *
+ * DESCRIPTION
+ * Initializes a barrier attributes object with default
+ * attributes.
+ *
+ * NOTES:
+ * 1) Used to define barrier types
+ *
+ * RESULTS
+ * 0 successfully initialized attr,
+ * ENOMEM insufficient memory for attr.
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_barrierattr_t ba;
+ int result = 0;
+
+ ba = (pthread_barrierattr_t) calloc (1, sizeof (*ba));
+
+ if (ba == NULL)
+ {
+ result = ENOMEM;
+ }
+ else
+ {
+ ba->pshared = PTHREAD_PROCESS_PRIVATE;
+ }
+
+ *attr = ba;
+
+ return (result);
+} /* pthread_barrierattr_init */
diff --git a/3rdparty/pthreads-win32/pthread_barrierattr_setpshared.c b/3rdparty/pthreads-win32/pthread_barrierattr_setpshared.c
new file mode 100644
index 0000000..08c6fde
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_barrierattr_setpshared.c
@@ -0,0 +1,119 @@
+/*
+ * pthread_barrier_attr_setpshared.c
+ *
+ * Description:
+ * This translation unit implements barrier primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_barrierattr_setpshared (pthread_barrierattr_t * attr, int pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Barriers created with 'attr' can be shared between
+ * processes if pthread_barrier_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_barrierattr_t
+ *
+ * pshared
+ * must be one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ * DESCRIPTION
+ * Mutexes creatd with 'attr' can be shared between
+ * processes if pthread_barrier_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * NOTES:
+ * 1) pshared barriers MUST be allocated in shared
+ * memory.
+ *
+ * 2) The following macro is defined if shared barriers
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or pshared is invalid,
+ * ENOSYS PTHREAD_PROCESS_SHARED not supported,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) &&
+ ((pshared == PTHREAD_PROCESS_SHARED) ||
+ (pshared == PTHREAD_PROCESS_PRIVATE)))
+ {
+ if (pshared == PTHREAD_PROCESS_SHARED)
+ {
+
+#if !defined( _POSIX_THREAD_PROCESS_SHARED )
+
+ result = ENOSYS;
+ pshared = PTHREAD_PROCESS_PRIVATE;
+
+#else
+
+ result = 0;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+
+ }
+ else
+ {
+ result = 0;
+ }
+
+ (*attr)->pshared = pshared;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+
+} /* pthread_barrierattr_setpshared */
diff --git a/3rdparty/pthreads-win32/pthread_cancel.c b/3rdparty/pthreads-win32/pthread_cancel.c
new file mode 100644
index 0000000..c36bfd6
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_cancel.c
@@ -0,0 +1,197 @@
+/*
+ * pthread_cancel.c
+ *
+ * Description:
+ * POSIX thread functions related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "context.h"
+
+static void
+ptw32_cancel_self (void)
+{
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+}
+
+static void CALLBACK
+ptw32_cancel_callback (ULONG_PTR unused)
+{
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+}
+
+/*
+ * ptw32_RegisterCancelation() -
+ * Must have args of same type as QueueUserAPCEx because this function
+ * is a substitute for QueueUserAPCEx if it's not available.
+ */
+DWORD
+ptw32_RegisterCancelation (PAPCFUNC unused1, HANDLE threadH, DWORD unused2)
+{
+#ifndef WINRT
+ CONTEXT context;
+
+ context.ContextFlags = CONTEXT_CONTROL;
+ GetThreadContext (threadH, &context);
+ PTW32_PROGCTR (context) = (DWORD_PTR) ptw32_cancel_self;
+ SetThreadContext (threadH, &context);
+#endif
+ return 0;
+}
+
+int
+pthread_cancel (pthread_t thread)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function requests cancellation of 'thread'.
+ *
+ * PARAMETERS
+ * thread
+ * reference to an instance of pthread_t
+ *
+ *
+ * DESCRIPTION
+ * This function requests cancellation of 'thread'.
+ * NOTE: cancellation is asynchronous; use pthread_join to
+ * wait for termination of 'thread' if necessary.
+ *
+ * RESULTS
+ * 0 successfully requested cancellation,
+ * ESRCH no thread found corresponding to 'thread',
+ * ENOMEM implicit self thread create failed.
+ * ------------------------------------------------------
+ */
+{
+ int result;
+ int cancel_self;
+ pthread_t self;
+ ptw32_thread_t * tp;
+ ptw32_mcs_local_node_t stateLock;
+
+ result = pthread_kill (thread, 0);
+
+ if (0 != result)
+ {
+ return result;
+ }
+
+ if ((self = pthread_self ()).p == NULL)
+ {
+ return ENOMEM;
+ };
+
+ /*
+ * For self cancellation we need to ensure that a thread can't
+ * deadlock itself trying to cancel itself asynchronously
+ * (pthread_cancel is required to be an async-cancel
+ * safe function).
+ */
+ cancel_self = pthread_equal (thread, self);
+
+ tp = (ptw32_thread_t *) thread.p;
+
+ /*
+ * Lock for async-cancel safety.
+ */
+ ptw32_mcs_lock_acquire (&tp->stateLock, &stateLock);
+
+ if (tp->cancelType == PTHREAD_CANCEL_ASYNCHRONOUS
+ && tp->cancelState == PTHREAD_CANCEL_ENABLE
+ && tp->state < PThreadStateCanceling)
+ {
+ if (cancel_self)
+ {
+ tp->state = PThreadStateCanceling;
+ tp->cancelState = PTHREAD_CANCEL_DISABLE;
+
+ ptw32_mcs_lock_release (&stateLock);
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+ }
+ else
+ {
+ HANDLE threadH = tp->threadH;
+
+ SuspendThread (threadH);
+
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WaitForSingleObjectEx(threadH, 0, FALSE) == WAIT_TIMEOUT)
+# else
+ if (WaitForSingleObject (threadH, 0) == WAIT_TIMEOUT)
+# endif
+#endif
+ {
+ tp->state = PThreadStateCanceling;
+ tp->cancelState = PTHREAD_CANCEL_DISABLE;
+ /*
+ * If alertdrv and QueueUserAPCEx is available then the following
+ * will result in a call to QueueUserAPCEx with the args given, otherwise
+ * this will result in a call to ptw32_RegisterCancelation and only
+ * the threadH arg will be used.
+ */
+ ptw32_register_cancelation ((PAPCFUNC)ptw32_cancel_callback, threadH, 0);
+ ptw32_mcs_lock_release (&stateLock);
+ ResumeThread (threadH);
+ }
+ }
+ }
+ else
+ {
+ /*
+ * Set for deferred cancellation.
+ */
+ if (tp->state < PThreadStateCancelPending)
+ {
+ tp->state = PThreadStateCancelPending;
+ if (!SetEvent (tp->cancelEvent))
+ {
+ result = ESRCH;
+ }
+ }
+ else if (tp->state >= PThreadStateCanceling)
+ {
+ result = ESRCH;
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_cond_destroy.c b/3rdparty/pthreads-win32/pthread_cond_destroy.c
new file mode 100644
index 0000000..40d4a08
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_cond_destroy.c
@@ -0,0 +1,253 @@
+/*
+ * pthread_cond_destroy.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_cond_destroy (pthread_cond_t * cond)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function destroys a condition variable
+ *
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ *
+ * DESCRIPTION
+ * This function destroys a condition variable.
+ *
+ * NOTES:
+ * 1) A condition variable can be destroyed
+ * immediately after all the threads that
+ * are blocked on it are awakened. e.g.
+ *
+ * struct list {
+ * pthread_mutex_t lm;
+ * ...
+ * }
+ *
+ * struct elt {
+ * key k;
+ * int busy;
+ * pthread_cond_t notbusy;
+ * ...
+ * }
+ *
+ *
+ * struct elt *
+ * list_find(struct list *lp, key k)
+ * {
+ * struct elt *ep;
+ *
+ * pthread_mutex_lock(&lp->lm);
+ * while ((ep = find_elt(l,k) != NULL) && ep->busy)
+ * pthread_cond_wait(&ep->notbusy, &lp->lm);
+ * if (ep != NULL)
+ * ep->busy = 1;
+ * pthread_mutex_unlock(&lp->lm);
+ * return(ep);
+ * }
+ *
+ * delete_elt(struct list *lp, struct elt *ep)
+ * {
+ * pthread_mutex_lock(&lp->lm);
+ * assert(ep->busy);
+ * ... remove ep from list ...
+ * ep->busy = 0;
+ * (A) pthread_cond_broadcast(&ep->notbusy);
+ * pthread_mutex_unlock(&lp->lm);
+ * (B) pthread_cond_destroy(&rp->notbusy);
+ * free(ep);
+ * }
+ *
+ * In this example, the condition variable
+ * and its list element may be freed (line B)
+ * immediately after all threads waiting for
+ * it are awakened (line A), since the mutex
+ * and the code ensure that no other thread
+ * can touch the element to be deleted.
+ *
+ * RESULTS
+ * 0 successfully released condition variable,
+ * EINVAL 'cond' is invalid,
+ * EBUSY 'cond' is in use,
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_cond_t cv;
+ int result = 0, result1 = 0, result2 = 0;
+
+ /*
+ * Assuming any race condition here is harmless.
+ */
+ if (cond == NULL || *cond == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (*cond != PTHREAD_COND_INITIALIZER)
+ {
+ ptw32_mcs_local_node_t node;
+ ptw32_mcs_lock_acquire(&ptw32_cond_list_lock, &node);
+
+ cv = *cond;
+
+ /*
+ * Close the gate; this will synchronize this thread with
+ * all already signaled waiters to let them retract their
+ * waiter status - SEE NOTE 1 ABOVE!!!
+ */
+ if (ptw32_semwait (&(cv->semBlockLock)) != 0) /* Non-cancelable */
+ {
+ result = errno;
+ }
+ else
+ {
+ /*
+ * !TRY! lock mtxUnblockLock; try will detect busy condition
+ * and will not cause a deadlock with respect to concurrent
+ * signal/broadcast.
+ */
+ if ((result = pthread_mutex_trylock (&(cv->mtxUnblockLock))) != 0)
+ {
+ (void) sem_post (&(cv->semBlockLock));
+ }
+ }
+
+ if (result != 0)
+ {
+ ptw32_mcs_lock_release(&node);
+ return result;
+ }
+
+ /*
+ * Check whether cv is still busy (still has waiters)
+ */
+ if (cv->nWaitersBlocked > cv->nWaitersGone)
+ {
+ if (sem_post (&(cv->semBlockLock)) != 0)
+ {
+ result = errno;
+ }
+ result1 = pthread_mutex_unlock (&(cv->mtxUnblockLock));
+ result2 = EBUSY;
+ }
+ else
+ {
+ /*
+ * Now it is safe to destroy
+ */
+ *cond = NULL;
+
+ if (sem_destroy (&(cv->semBlockLock)) != 0)
+ {
+ result = errno;
+ }
+ if (sem_destroy (&(cv->semBlockQueue)) != 0)
+ {
+ result1 = errno;
+ }
+ if ((result2 = pthread_mutex_unlock (&(cv->mtxUnblockLock))) == 0)
+ {
+ result2 = pthread_mutex_destroy (&(cv->mtxUnblockLock));
+ }
+
+ /* Unlink the CV from the list */
+
+ if (ptw32_cond_list_head == cv)
+ {
+ ptw32_cond_list_head = cv->next;
+ }
+ else
+ {
+ cv->prev->next = cv->next;
+ }
+
+ if (ptw32_cond_list_tail == cv)
+ {
+ ptw32_cond_list_tail = cv->prev;
+ }
+ else
+ {
+ cv->next->prev = cv->prev;
+ }
+
+ (void) free (cv);
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+ else
+ {
+ ptw32_mcs_local_node_t node;
+ /*
+ * See notes in ptw32_cond_check_need_init() above also.
+ */
+ ptw32_mcs_lock_acquire(&ptw32_cond_test_init_lock, &node);
+
+ /*
+ * Check again.
+ */
+ if (*cond == PTHREAD_COND_INITIALIZER)
+ {
+ /*
+ * This is all we need to do to destroy a statically
+ * initialised cond that has not yet been used (initialised).
+ * If we get to here, another thread waiting to initialise
+ * this cond will get an EINVAL. That's OK.
+ */
+ *cond = NULL;
+ }
+ else
+ {
+ /*
+ * The cv has been initialised while we were waiting
+ * so assume it's in use.
+ */
+ result = EBUSY;
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+
+ return ((result != 0) ? result : ((result1 != 0) ? result1 : result2));
+}
diff --git a/3rdparty/pthreads-win32/pthread_cond_init.c b/3rdparty/pthreads-win32/pthread_cond_init.c
new file mode 100644
index 0000000..f28fd67
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_cond_init.c
@@ -0,0 +1,167 @@
+/*
+ * pthread_cond_init.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_cond_init (pthread_cond_t * cond, const pthread_condattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function initializes a condition variable.
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ * attr
+ * specifies optional creation attributes.
+ *
+ *
+ * DESCRIPTION
+ * This function initializes a condition variable.
+ *
+ * RESULTS
+ * 0 successfully created condition variable,
+ * EINVAL 'attr' is invalid,
+ * EAGAIN insufficient resources (other than
+ * memory,
+ * ENOMEM insufficient memory,
+ * EBUSY 'cond' is already initialized,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+ pthread_cond_t cv = NULL;
+
+ if (cond == NULL)
+ {
+ return EINVAL;
+ }
+
+ if ((attr != NULL && *attr != NULL) &&
+ ((*attr)->pshared == PTHREAD_PROCESS_SHARED))
+ {
+ /*
+ * Creating condition variable that can be shared between
+ * processes.
+ */
+ result = ENOSYS;
+ goto DONE;
+ }
+
+ cv = (pthread_cond_t) calloc (1, sizeof (*cv));
+
+ if (cv == NULL)
+ {
+ result = ENOMEM;
+ goto DONE;
+ }
+
+ cv->nWaitersBlocked = 0;
+ cv->nWaitersToUnblock = 0;
+ cv->nWaitersGone = 0;
+
+ if (sem_init (&(cv->semBlockLock), 0, 1) != 0)
+ {
+ result = errno;
+ goto FAIL0;
+ }
+
+ if (sem_init (&(cv->semBlockQueue), 0, 0) != 0)
+ {
+ result = errno;
+ goto FAIL1;
+ }
+
+ if ((result = pthread_mutex_init (&(cv->mtxUnblockLock), 0)) != 0)
+ {
+ goto FAIL2;
+ }
+
+ result = 0;
+
+ goto DONE;
+
+ /*
+ * -------------
+ * Failed...
+ * -------------
+ */
+FAIL2:
+ (void) sem_destroy (&(cv->semBlockQueue));
+
+FAIL1:
+ (void) sem_destroy (&(cv->semBlockLock));
+
+FAIL0:
+ (void) free (cv);
+ cv = NULL;
+
+DONE:
+ if (0 == result)
+ {
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_cond_list_lock, &node);
+
+ cv->next = NULL;
+ cv->prev = ptw32_cond_list_tail;
+
+ if (ptw32_cond_list_tail != NULL)
+ {
+ ptw32_cond_list_tail->next = cv;
+ }
+
+ ptw32_cond_list_tail = cv;
+
+ if (ptw32_cond_list_head == NULL)
+ {
+ ptw32_cond_list_head = cv;
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+
+ *cond = cv;
+
+ return result;
+
+} /* pthread_cond_init */
diff --git a/3rdparty/pthreads-win32/pthread_cond_signal.c b/3rdparty/pthreads-win32/pthread_cond_signal.c
new file mode 100644
index 0000000..2b4f6d4
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_cond_signal.c
@@ -0,0 +1,231 @@
+/*
+ * pthread_cond_signal.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ *
+ * -------------------------------------------------------------
+ * Algorithm:
+ * See the comments at the top of pthread_cond_wait.c.
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+static INLINE int
+ptw32_cond_unblock (pthread_cond_t * cond, int unblockAll)
+ /*
+ * Notes.
+ *
+ * Does not use the external mutex for synchronisation,
+ * therefore semBlockLock is needed.
+ * mtxUnblockLock is for LEVEL-2 synch. LEVEL-2 is the
+ * state where the external mutex is not necessarily locked by
+ * any thread, ie. between cond_wait unlocking and re-acquiring
+ * the lock after having been signaled or a timeout or
+ * cancellation.
+ *
+ * Uses the following CV elements:
+ * nWaitersBlocked
+ * nWaitersToUnblock
+ * nWaitersGone
+ * mtxUnblockLock
+ * semBlockLock
+ * semBlockQueue
+ */
+{
+ int result;
+ pthread_cond_t cv;
+ int nSignalsToIssue;
+
+ if (cond == NULL || *cond == NULL)
+ {
+ return EINVAL;
+ }
+
+ cv = *cond;
+
+ /*
+ * No-op if the CV is static and hasn't been initialised yet.
+ * Assuming that any race condition is harmless.
+ */
+ if (cv == PTHREAD_COND_INITIALIZER)
+ {
+ return 0;
+ }
+
+ if ((result = pthread_mutex_lock (&(cv->mtxUnblockLock))) != 0)
+ {
+ return result;
+ }
+
+ if (0 != cv->nWaitersToUnblock)
+ {
+ if (0 == cv->nWaitersBlocked)
+ {
+ return pthread_mutex_unlock (&(cv->mtxUnblockLock));
+ }
+ if (unblockAll)
+ {
+ cv->nWaitersToUnblock += (nSignalsToIssue = cv->nWaitersBlocked);
+ cv->nWaitersBlocked = 0;
+ }
+ else
+ {
+ nSignalsToIssue = 1;
+ cv->nWaitersToUnblock++;
+ cv->nWaitersBlocked--;
+ }
+ }
+ else if (cv->nWaitersBlocked > cv->nWaitersGone)
+ {
+ /* Use the non-cancellable version of sem_wait() */
+ if (ptw32_semwait (&(cv->semBlockLock)) != 0)
+ {
+ result = errno;
+ (void) pthread_mutex_unlock (&(cv->mtxUnblockLock));
+ return result;
+ }
+ if (0 != cv->nWaitersGone)
+ {
+ cv->nWaitersBlocked -= cv->nWaitersGone;
+ cv->nWaitersGone = 0;
+ }
+ if (unblockAll)
+ {
+ nSignalsToIssue = cv->nWaitersToUnblock = cv->nWaitersBlocked;
+ cv->nWaitersBlocked = 0;
+ }
+ else
+ {
+ nSignalsToIssue = cv->nWaitersToUnblock = 1;
+ cv->nWaitersBlocked--;
+ }
+ }
+ else
+ {
+ return pthread_mutex_unlock (&(cv->mtxUnblockLock));
+ }
+
+ if ((result = pthread_mutex_unlock (&(cv->mtxUnblockLock))) == 0)
+ {
+ if (sem_post_multiple (&(cv->semBlockQueue), nSignalsToIssue) != 0)
+ {
+ result = errno;
+ }
+ }
+
+ return result;
+
+} /* ptw32_cond_unblock */
+
+int
+pthread_cond_signal (pthread_cond_t * cond)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function signals a condition variable, waking
+ * one waiting thread.
+ * If SCHED_FIFO or SCHED_RR policy threads are waiting
+ * the highest priority waiter is awakened; otherwise,
+ * an unspecified waiter is awakened.
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ *
+ * DESCRIPTION
+ * This function signals a condition variable, waking
+ * one waiting thread.
+ * If SCHED_FIFO or SCHED_RR policy threads are waiting
+ * the highest priority waiter is awakened; otherwise,
+ * an unspecified waiter is awakened.
+ *
+ * NOTES:
+ *
+ * 1) Use when any waiter can respond and only one need
+ * respond (all waiters being equal).
+ *
+ * RESULTS
+ * 0 successfully signaled condition,
+ * EINVAL 'cond' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ /*
+ * The '0'(FALSE) unblockAll arg means unblock ONE waiter.
+ */
+ return (ptw32_cond_unblock (cond, 0));
+
+} /* pthread_cond_signal */
+
+int
+pthread_cond_broadcast (pthread_cond_t * cond)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function broadcasts the condition variable,
+ * waking all current waiters.
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ *
+ * DESCRIPTION
+ * This function signals a condition variable, waking
+ * all waiting threads.
+ *
+ * NOTES:
+ *
+ * 1) Use when more than one waiter may respond to
+ * predicate change or if any waiting thread may
+ * not be able to respond
+ *
+ * RESULTS
+ * 0 successfully signalled condition to all
+ * waiting threads,
+ * EINVAL 'cond' is invalid
+ * ENOSPC a required resource has been exhausted,
+ *
+ * ------------------------------------------------------
+ */
+{
+ /*
+ * The TRUE unblockAll arg means unblock ALL waiters.
+ */
+ return (ptw32_cond_unblock (cond, PTW32_TRUE));
+
+} /* pthread_cond_broadcast */
diff --git a/3rdparty/pthreads-win32/pthread_cond_wait.c b/3rdparty/pthreads-win32/pthread_cond_wait.c
new file mode 100644
index 0000000..359219a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_cond_wait.c
@@ -0,0 +1,567 @@
+/*
+ * pthread_cond_wait.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ *
+ * -------------------------------------------------------------
+ * Algorithm:
+ * The algorithm used in this implementation is that developed by
+ * Alexander Terekhov in colaboration with Louis Thomas. The bulk
+ * of the discussion is recorded in the file README.CV, which contains
+ * several generations of both colaborators original algorithms. The final
+ * algorithm used here is the one referred to as
+ *
+ * Algorithm 8a / IMPL_SEM,UNBLOCK_STRATEGY == UNBLOCK_ALL
+ *
+ * presented below in pseudo-code as it appeared:
+ *
+ *
+ * given:
+ * semBlockLock - bin.semaphore
+ * semBlockQueue - semaphore
+ * mtxExternal - mutex or CS
+ * mtxUnblockLock - mutex or CS
+ * nWaitersGone - int
+ * nWaitersBlocked - int
+ * nWaitersToUnblock - int
+ *
+ * wait( timeout ) {
+ *
+ * [auto: register int result ] // error checking omitted
+ * [auto: register int nSignalsWasLeft ]
+ * [auto: register int nWaitersWasGone ]
+ *
+ * sem_wait( semBlockLock );
+ * nWaitersBlocked++;
+ * sem_post( semBlockLock );
+ *
+ * unlock( mtxExternal );
+ * bTimedOut = sem_wait( semBlockQueue,timeout );
+ *
+ * lock( mtxUnblockLock );
+ * if ( 0 != (nSignalsWasLeft = nWaitersToUnblock) ) {
+ * if ( bTimeout ) { // timeout (or canceled)
+ * if ( 0 != nWaitersBlocked ) {
+ * nWaitersBlocked--;
+ * }
+ * else {
+ * nWaitersGone++; // count spurious wakeups.
+ * }
+ * }
+ * if ( 0 == --nWaitersToUnblock ) {
+ * if ( 0 != nWaitersBlocked ) {
+ * sem_post( semBlockLock ); // open the gate.
+ * nSignalsWasLeft = 0; // do not open the gate
+ * // below again.
+ * }
+ * else if ( 0 != (nWaitersWasGone = nWaitersGone) ) {
+ * nWaitersGone = 0;
+ * }
+ * }
+ * }
+ * else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or
+ * // spurious semaphore :-)
+ * sem_wait( semBlockLock );
+ * nWaitersBlocked -= nWaitersGone; // something is going on here
+ * // - test of timeouts? :-)
+ * sem_post( semBlockLock );
+ * nWaitersGone = 0;
+ * }
+ * unlock( mtxUnblockLock );
+ *
+ * if ( 1 == nSignalsWasLeft ) {
+ * if ( 0 != nWaitersWasGone ) {
+ * // sem_adjust( semBlockQueue,-nWaitersWasGone );
+ * while ( nWaitersWasGone-- ) {
+ * sem_wait( semBlockQueue ); // better now than spurious later
+ * }
+ * } sem_post( semBlockLock ); // open the gate
+ * }
+ *
+ * lock( mtxExternal );
+ *
+ * return ( bTimedOut ) ? ETIMEOUT : 0;
+ * }
+ *
+ * signal(bAll) {
+ *
+ * [auto: register int result ]
+ * [auto: register int nSignalsToIssue]
+ *
+ * lock( mtxUnblockLock );
+ *
+ * if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ * if ( 0 == nWaitersBlocked ) { // NO-OP
+ * return unlock( mtxUnblockLock );
+ * }
+ * if (bAll) {
+ * nWaitersToUnblock += nSignalsToIssue=nWaitersBlocked;
+ * nWaitersBlocked = 0;
+ * }
+ * else {
+ * nSignalsToIssue = 1;
+ * nWaitersToUnblock++;
+ * nWaitersBlocked--;
+ * }
+ * }
+ * else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ * sem_wait( semBlockLock ); // close the gate
+ * if ( 0 != nWaitersGone ) {
+ * nWaitersBlocked -= nWaitersGone;
+ * nWaitersGone = 0;
+ * }
+ * if (bAll) {
+ * nSignalsToIssue = nWaitersToUnblock = nWaitersBlocked;
+ * nWaitersBlocked = 0;
+ * }
+ * else {
+ * nSignalsToIssue = nWaitersToUnblock = 1;
+ * nWaitersBlocked--;
+ * }
+ * }
+ * else { // NO-OP
+ * return unlock( mtxUnblockLock );
+ * }
+ *
+ * unlock( mtxUnblockLock );
+ * sem_post( semBlockQueue,nSignalsToIssue );
+ * return result;
+ * }
+ * -------------------------------------------------------------
+ *
+ * Algorithm 9 / IMPL_SEM,UNBLOCK_STRATEGY == UNBLOCK_ALL
+ *
+ * presented below in pseudo-code; basically 8a...
+ * ...BUT W/O "spurious wakes" prevention:
+ *
+ *
+ * given:
+ * semBlockLock - bin.semaphore
+ * semBlockQueue - semaphore
+ * mtxExternal - mutex or CS
+ * mtxUnblockLock - mutex or CS
+ * nWaitersGone - int
+ * nWaitersBlocked - int
+ * nWaitersToUnblock - int
+ *
+ * wait( timeout ) {
+ *
+ * [auto: register int result ] // error checking omitted
+ * [auto: register int nSignalsWasLeft ]
+ *
+ * sem_wait( semBlockLock );
+ * ++nWaitersBlocked;
+ * sem_post( semBlockLock );
+ *
+ * unlock( mtxExternal );
+ * bTimedOut = sem_wait( semBlockQueue,timeout );
+ *
+ * lock( mtxUnblockLock );
+ * if ( 0 != (nSignalsWasLeft = nWaitersToUnblock) ) {
+ * --nWaitersToUnblock;
+ * }
+ * else if ( INT_MAX/2 == ++nWaitersGone ) { // timeout/canceled or
+ * // spurious semaphore :-)
+ * sem_wait( semBlockLock );
+ * nWaitersBlocked -= nWaitersGone; // something is going on here
+ * // - test of timeouts? :-)
+ * sem_post( semBlockLock );
+ * nWaitersGone = 0;
+ * }
+ * unlock( mtxUnblockLock );
+ *
+ * if ( 1 == nSignalsWasLeft ) {
+ * sem_post( semBlockLock ); // open the gate
+ * }
+ *
+ * lock( mtxExternal );
+ *
+ * return ( bTimedOut ) ? ETIMEOUT : 0;
+ * }
+ *
+ * signal(bAll) {
+ *
+ * [auto: register int result ]
+ * [auto: register int nSignalsToIssue]
+ *
+ * lock( mtxUnblockLock );
+ *
+ * if ( 0 != nWaitersToUnblock ) { // the gate is closed!!!
+ * if ( 0 == nWaitersBlocked ) { // NO-OP
+ * return unlock( mtxUnblockLock );
+ * }
+ * if (bAll) {
+ * nWaitersToUnblock += nSignalsToIssue=nWaitersBlocked;
+ * nWaitersBlocked = 0;
+ * }
+ * else {
+ * nSignalsToIssue = 1;
+ * ++nWaitersToUnblock;
+ * --nWaitersBlocked;
+ * }
+ * }
+ * else if ( nWaitersBlocked > nWaitersGone ) { // HARMLESS RACE CONDITION!
+ * sem_wait( semBlockLock ); // close the gate
+ * if ( 0 != nWaitersGone ) {
+ * nWaitersBlocked -= nWaitersGone;
+ * nWaitersGone = 0;
+ * }
+ * if (bAll) {
+ * nSignalsToIssue = nWaitersToUnblock = nWaitersBlocked;
+ * nWaitersBlocked = 0;
+ * }
+ * else {
+ * nSignalsToIssue = nWaitersToUnblock = 1;
+ * --nWaitersBlocked;
+ * }
+ * }
+ * else { // NO-OP
+ * return unlock( mtxUnblockLock );
+ * }
+ *
+ * unlock( mtxUnblockLock );
+ * sem_post( semBlockQueue,nSignalsToIssue );
+ * return result;
+ * }
+ * -------------------------------------------------------------
+ *
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Arguments for cond_wait_cleanup, since we can only pass a
+ * single void * to it.
+ */
+typedef struct
+{
+ pthread_mutex_t *mutexPtr;
+ pthread_cond_t cv;
+ int *resultPtr;
+} ptw32_cond_wait_cleanup_args_t;
+
+static void PTW32_CDECL
+ptw32_cond_wait_cleanup (void *args)
+{
+ ptw32_cond_wait_cleanup_args_t *cleanup_args =
+ (ptw32_cond_wait_cleanup_args_t *) args;
+ pthread_cond_t cv = cleanup_args->cv;
+ int *resultPtr = cleanup_args->resultPtr;
+ int nSignalsWasLeft;
+ int result;
+
+ /*
+ * Whether we got here as a result of signal/broadcast or because of
+ * timeout on wait or thread cancellation we indicate that we are no
+ * longer waiting. The waiter is responsible for adjusting waiters
+ * (to)unblock(ed) counts (protected by unblock lock).
+ */
+ if ((result = pthread_mutex_lock (&(cv->mtxUnblockLock))) != 0)
+ {
+ *resultPtr = result;
+ return;
+ }
+
+ if (0 != (nSignalsWasLeft = cv->nWaitersToUnblock))
+ {
+ --(cv->nWaitersToUnblock);
+ }
+ else if (INT_MAX / 2 == ++(cv->nWaitersGone))
+ {
+ /* Use the non-cancellable version of sem_wait() */
+ if (ptw32_semwait (&(cv->semBlockLock)) != 0)
+ {
+ *resultPtr = errno;
+ /*
+ * This is a fatal error for this CV,
+ * so we deliberately don't unlock
+ * cv->mtxUnblockLock before returning.
+ */
+ return;
+ }
+ cv->nWaitersBlocked -= cv->nWaitersGone;
+ if (sem_post (&(cv->semBlockLock)) != 0)
+ {
+ *resultPtr = errno;
+ /*
+ * This is a fatal error for this CV,
+ * so we deliberately don't unlock
+ * cv->mtxUnblockLock before returning.
+ */
+ return;
+ }
+ cv->nWaitersGone = 0;
+ }
+
+ if ((result = pthread_mutex_unlock (&(cv->mtxUnblockLock))) != 0)
+ {
+ *resultPtr = result;
+ return;
+ }
+
+ if (1 == nSignalsWasLeft)
+ {
+ if (sem_post (&(cv->semBlockLock)) != 0)
+ {
+ *resultPtr = errno;
+ return;
+ }
+ }
+
+ /*
+ * XSH: Upon successful return, the mutex has been locked and is owned
+ * by the calling thread.
+ */
+ if ((result = pthread_mutex_lock (cleanup_args->mutexPtr)) != 0)
+ {
+ *resultPtr = result;
+ }
+} /* ptw32_cond_wait_cleanup */
+
+static INLINE int
+ptw32_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex, const struct timespec *abstime)
+{
+ int result = 0;
+ pthread_cond_t cv;
+ ptw32_cond_wait_cleanup_args_t cleanup_args;
+
+ if (cond == NULL || *cond == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static condition variable. We check
+ * again inside the guarded section of ptw32_cond_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*cond == PTHREAD_COND_INITIALIZER)
+ {
+ result = ptw32_cond_check_need_init (cond);
+ }
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+
+ cv = *cond;
+
+ /* Thread can be cancelled in sem_wait() but this is OK */
+ if (sem_wait (&(cv->semBlockLock)) != 0)
+ {
+ return errno;
+ }
+
+ ++(cv->nWaitersBlocked);
+
+ if (sem_post (&(cv->semBlockLock)) != 0)
+ {
+ return errno;
+ }
+
+ /*
+ * Setup this waiter cleanup handler
+ */
+ cleanup_args.mutexPtr = mutex;
+ cleanup_args.cv = cv;
+ cleanup_args.resultPtr = &result;
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+ pthread_cleanup_push (ptw32_cond_wait_cleanup, (void *) &cleanup_args);
+
+ /*
+ * Now we can release 'mutex' and...
+ */
+ if ((result = pthread_mutex_unlock (mutex)) == 0)
+ {
+
+ /*
+ * ...wait to be awakened by
+ * pthread_cond_signal, or
+ * pthread_cond_broadcast, or
+ * timeout, or
+ * thread cancellation
+ *
+ * Note:
+ *
+ * sem_timedwait is a cancellation point,
+ * hence providing the mechanism for making
+ * pthread_cond_wait a cancellation point.
+ * We use the cleanup mechanism to ensure we
+ * re-lock the mutex and adjust (to)unblock(ed) waiters
+ * counts if we are cancelled, timed out or signalled.
+ */
+ if (sem_timedwait (&(cv->semBlockQueue), abstime) != 0)
+ {
+ result = errno;
+ }
+ }
+
+ /*
+ * Always cleanup
+ */
+ pthread_cleanup_pop (1);
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+
+ /*
+ * "result" can be modified by the cleanup handler.
+ */
+ return result;
+
+} /* ptw32_cond_timedwait */
+
+
+int
+pthread_cond_wait (pthread_cond_t * cond, pthread_mutex_t * mutex)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function waits on a condition variable until
+ * awakened by a signal or broadcast.
+ *
+ * Caller MUST be holding the mutex lock; the
+ * lock is released and the caller is blocked waiting
+ * on 'cond'. When 'cond' is signaled, the mutex
+ * is re-acquired before returning to the caller.
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ * mutex
+ * pointer to an instance of pthread_mutex_t
+ *
+ *
+ * DESCRIPTION
+ * This function waits on a condition variable until
+ * awakened by a signal or broadcast.
+ *
+ * NOTES:
+ *
+ * 1) The function must be called with 'mutex' LOCKED
+ * by the calling thread, or undefined behaviour
+ * will result.
+ *
+ * 2) This routine atomically releases 'mutex' and causes
+ * the calling thread to block on the condition variable.
+ * The blocked thread may be awakened by
+ * pthread_cond_signal or
+ * pthread_cond_broadcast.
+ *
+ * Upon successful completion, the 'mutex' has been locked and
+ * is owned by the calling thread.
+ *
+ *
+ * RESULTS
+ * 0 caught condition; mutex released,
+ * EINVAL 'cond' or 'mutex' is invalid,
+ * EINVAL different mutexes for concurrent waits,
+ * EINVAL mutex is not held by the calling thread,
+ *
+ * ------------------------------------------------------
+ */
+{
+ /*
+ * The NULL abstime arg means INFINITE waiting.
+ */
+ return (ptw32_cond_timedwait (cond, mutex, NULL));
+
+} /* pthread_cond_wait */
+
+
+int
+pthread_cond_timedwait (pthread_cond_t * cond,
+ pthread_mutex_t * mutex,
+ const struct timespec *abstime)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function waits on a condition variable either until
+ * awakened by a signal or broadcast; or until the time
+ * specified by abstime passes.
+ *
+ * PARAMETERS
+ * cond
+ * pointer to an instance of pthread_cond_t
+ *
+ * mutex
+ * pointer to an instance of pthread_mutex_t
+ *
+ * abstime
+ * pointer to an instance of (const struct timespec)
+ *
+ *
+ * DESCRIPTION
+ * This function waits on a condition variable either until
+ * awakened by a signal or broadcast; or until the time
+ * specified by abstime passes.
+ *
+ * NOTES:
+ * 1) The function must be called with 'mutex' LOCKED
+ * by the calling thread, or undefined behaviour
+ * will result.
+ *
+ * 2) This routine atomically releases 'mutex' and causes
+ * the calling thread to block on the condition variable.
+ * The blocked thread may be awakened by
+ * pthread_cond_signal or
+ * pthread_cond_broadcast.
+ *
+ *
+ * RESULTS
+ * 0 caught condition; mutex released,
+ * EINVAL 'cond', 'mutex', or abstime is invalid,
+ * EINVAL different mutexes for concurrent waits,
+ * EINVAL mutex is not held by the calling thread,
+ * ETIMEDOUT abstime ellapsed before cond was signaled.
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (abstime == NULL)
+ {
+ return EINVAL;
+ }
+
+ return (ptw32_cond_timedwait (cond, mutex, abstime));
+
+} /* pthread_cond_timedwait */
diff --git a/3rdparty/pthreads-win32/pthread_condattr_destroy.c b/3rdparty/pthreads-win32/pthread_condattr_destroy.c
new file mode 100644
index 0000000..58a1482
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_condattr_destroy.c
@@ -0,0 +1,86 @@
+/*
+ * condvar_attr_destroy.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_condattr_destroy (pthread_condattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Destroys a condition variable attributes object.
+ * The object can no longer be used.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_condattr_t
+ *
+ *
+ * DESCRIPTION
+ * Destroys a condition variable attributes object.
+ * The object can no longer be used.
+ *
+ * NOTES:
+ * 1) Does not affect condition variables created
+ * using 'attr'
+ *
+ * RESULTS
+ * 0 successfully released attr,
+ * EINVAL 'attr' is invalid.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+
+ if (attr == NULL || *attr == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ (void) free (*attr);
+
+ *attr = NULL;
+ result = 0;
+ }
+
+ return result;
+
+} /* pthread_condattr_destroy */
diff --git a/3rdparty/pthreads-win32/pthread_condattr_getpshared.c b/3rdparty/pthreads-win32/pthread_condattr_getpshared.c
new file mode 100644
index 0000000..a0ac6d8
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_condattr_getpshared.c
@@ -0,0 +1,97 @@
+/*
+ * pthread_condattr_getpshared.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_condattr_getpshared (const pthread_condattr_t * attr, int *pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Determine whether condition variables created with 'attr'
+ * can be shared between processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_condattr_t
+ *
+ * pshared
+ * will be set to one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ *
+ * DESCRIPTION
+ * Condition Variables created with 'attr' can be shared
+ * between processes if pthread_cond_t variable is allocated
+ * in memory shared by these processes.
+ * NOTES:
+ * 1) pshared condition variables MUST be allocated in
+ * shared memory.
+ *
+ * 2) The following macro is defined if shared mutexes
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully retrieved attribute,
+ * EINVAL 'attr' or 'pshared' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) && (pshared != NULL))
+ {
+ *pshared = (*attr)->pshared;
+ result = 0;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return result;
+
+} /* pthread_condattr_getpshared */
diff --git a/3rdparty/pthreads-win32/pthread_condattr_init.c b/3rdparty/pthreads-win32/pthread_condattr_init.c
new file mode 100644
index 0000000..5987878
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_condattr_init.c
@@ -0,0 +1,87 @@
+/*
+ * pthread_condattr_init.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_condattr_init (pthread_condattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Initializes a condition variable attributes object
+ * with default attributes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_condattr_t
+ *
+ *
+ * DESCRIPTION
+ * Initializes a condition variable attributes object
+ * with default attributes.
+ *
+ * NOTES:
+ * 1) Use to define condition variable types
+ * 2) It is up to the application to ensure
+ * that it doesn't re-init an attribute
+ * without destroying it first. Otherwise
+ * a memory leak is created.
+ *
+ * RESULTS
+ * 0 successfully initialized attr,
+ * ENOMEM insufficient memory for attr.
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_condattr_t attr_result;
+ int result = 0;
+
+ attr_result = (pthread_condattr_t) calloc (1, sizeof (*attr_result));
+
+ if (attr_result == NULL)
+ {
+ result = ENOMEM;
+ }
+
+ *attr = attr_result;
+
+ return result;
+
+} /* pthread_condattr_init */
diff --git a/3rdparty/pthreads-win32/pthread_condattr_setpshared.c b/3rdparty/pthreads-win32/pthread_condattr_setpshared.c
new file mode 100644
index 0000000..954fb38
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_condattr_setpshared.c
@@ -0,0 +1,117 @@
+/*
+ * pthread_condattr_setpshared.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_condattr_setpshared (pthread_condattr_t * attr, int pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Mutexes created with 'attr' can be shared between
+ * processes if pthread_mutex_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * pshared
+ * must be one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ * DESCRIPTION
+ * Mutexes creatd with 'attr' can be shared between
+ * processes if pthread_mutex_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * NOTES:
+ * 1) pshared mutexes MUST be allocated in shared
+ * memory.
+ *
+ * 2) The following macro is defined if shared mutexes
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or pshared is invalid,
+ * ENOSYS PTHREAD_PROCESS_SHARED not supported,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL)
+ && ((pshared == PTHREAD_PROCESS_SHARED)
+ || (pshared == PTHREAD_PROCESS_PRIVATE)))
+ {
+ if (pshared == PTHREAD_PROCESS_SHARED)
+ {
+
+#if !defined( _POSIX_THREAD_PROCESS_SHARED )
+ result = ENOSYS;
+ pshared = PTHREAD_PROCESS_PRIVATE;
+#else
+ result = 0;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+
+ }
+ else
+ {
+ result = 0;
+ }
+
+ (*attr)->pshared = pshared;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return result;
+
+} /* pthread_condattr_setpshared */
diff --git a/3rdparty/pthreads-win32/pthread_delay_np.c b/3rdparty/pthreads-win32/pthread_delay_np.c
new file mode 100644
index 0000000..e6c96d8
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_delay_np.c
@@ -0,0 +1,172 @@
+/*
+ * pthreads_delay_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * pthread_delay_np
+ *
+ * DESCRIPTION
+ *
+ * This routine causes a thread to delay execution for a specific period of time.
+ * This period ends at the current time plus the specified interval. The routine
+ * will not return before the end of the period is reached, but may return an
+ * arbitrary amount of time after the period has gone by. This can be due to
+ * system load, thread priorities, and system timer granularity.
+ *
+ * Specifying an interval of zero (0) seconds and zero (0) nanoseconds is
+ * allowed and can be used to force the thread to give up the processor or to
+ * deliver a pending cancelation request.
+ *
+ * The timespec structure contains the following two fields:
+ *
+ * tv_sec is an integer number of seconds.
+ * tv_nsec is an integer number of nanoseconds.
+ *
+ * Return Values
+ *
+ * If an error condition occurs, this routine returns an integer value indicating
+ * the type of error. Possible return values are as follows:
+ *
+ * 0
+ * Successful completion.
+ * [EINVAL]
+ * The value specified by interval is invalid.
+ *
+ * Example
+ *
+ * The following code segment would wait for 5 and 1/2 seconds
+ *
+ * struct timespec tsWait;
+ * int intRC;
+ *
+ * tsWait.tv_sec = 5;
+ * tsWait.tv_nsec = 500000000L;
+ * intRC = pthread_delay_np(&tsWait);
+ */
+int
+pthread_delay_np (struct timespec *interval)
+{
+ DWORD wait_time;
+ DWORD secs_in_millisecs;
+ DWORD millisecs;
+ DWORD status;
+ pthread_t self;
+ ptw32_thread_t * sp;
+
+ if (interval == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (interval->tv_sec == 0L && interval->tv_nsec == 0L)
+ {
+ pthread_testcancel ();
+ Sleep (0);
+ pthread_testcancel ();
+ return (0);
+ }
+
+ /* convert secs to millisecs */
+ secs_in_millisecs = (DWORD)interval->tv_sec * 1000L;
+
+ /* convert nanosecs to millisecs (rounding up) */
+ millisecs = (interval->tv_nsec + 999999L) / 1000000L;
+
+#if defined(__WATCOMC__)
+#pragma disable_message (124)
+#endif
+
+ /*
+ * Most compilers will issue a warning 'comparison always 0'
+ * because the variable type is unsigned, but we need to keep this
+ * for some reason I can't recall now.
+ */
+ if (0 > (wait_time = secs_in_millisecs + millisecs))
+ {
+ return EINVAL;
+ }
+
+#if defined(__WATCOMC__)
+#pragma enable_message (124)
+#endif
+
+ if (NULL == (self = pthread_self ()).p)
+ {
+ return ENOMEM;
+ }
+
+ sp = (ptw32_thread_t *) self.p;
+
+ if (sp->cancelState == PTHREAD_CANCEL_ENABLE)
+ {
+ /*
+ * Async cancelation won't catch us until wait_time is up.
+ * Deferred cancelation will cancel us immediately.
+ */
+ if (WAIT_OBJECT_0 ==
+ (status = WaitForSingleObject (sp->cancelEvent, wait_time)))
+ {
+ ptw32_mcs_local_node_t stateLock;
+ /*
+ * Canceling!
+ */
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+ if (sp->state < PThreadStateCanceling)
+ {
+ sp->state = PThreadStateCanceling;
+ sp->cancelState = PTHREAD_CANCEL_DISABLE;
+ ptw32_mcs_lock_release (&stateLock);
+
+ ptw32_throw (PTW32_EPS_CANCEL);
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+ return ESRCH;
+ }
+ else if (status != WAIT_TIMEOUT)
+ {
+ return EINVAL;
+ }
+ }
+ else
+ {
+ Sleep (wait_time);
+ }
+
+ return (0);
+}
diff --git a/3rdparty/pthreads-win32/pthread_detach.c b/3rdparty/pthreads-win32/pthread_detach.c
new file mode 100644
index 0000000..f7bf0d8
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_detach.c
@@ -0,0 +1,142 @@
+/*
+ * pthread_detach.c
+ *
+ * Description:
+ * This translation unit implements functions related to thread
+ * synchronisation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Not needed yet, but defining it should indicate clashes with build target
+ * environment that should be fixed.
+ */
+#if !defined(WINCE)
+# include <signal.h>
+#endif
+
+
+int
+pthread_detach (pthread_t thread)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function detaches the given thread.
+ *
+ * PARAMETERS
+ * thread
+ * an instance of a pthread_t
+ *
+ *
+ * DESCRIPTION
+ * This function detaches the given thread. You may use it to
+ * detach the main thread or to detach a joinable thread.
+ * NOTE: detached threads cannot be joined;
+ * storage is freed immediately on termination.
+ *
+ * RESULTS
+ * 0 successfully detached the thread,
+ * EINVAL thread is not a joinable thread,
+ * ENOSPC a required resource has been exhausted,
+ * ESRCH no thread could be found for 'thread',
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+ BOOL destroyIt = PTW32_FALSE;
+ ptw32_thread_t * tp = (ptw32_thread_t *) thread.p;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ if (NULL == tp
+ || thread.x != tp->ptHandle.x)
+ {
+ result = ESRCH;
+ }
+ else if (PTHREAD_CREATE_DETACHED == tp->detachState)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ ptw32_mcs_local_node_t stateLock;
+ /*
+ * Joinable ptw32_thread_t structs are not scavenged until
+ * a join or detach is done. The thread may have exited already,
+ * but all of the state and locks etc are still there.
+ */
+ result = 0;
+
+ ptw32_mcs_lock_acquire (&tp->stateLock, &stateLock);
+ if (tp->state != PThreadStateLast)
+ {
+ tp->detachState = PTHREAD_CREATE_DETACHED;
+ }
+ else if (tp->detachState != PTHREAD_CREATE_DETACHED)
+ {
+ /*
+ * Thread is joinable and has exited or is exiting.
+ */
+ destroyIt = PTW32_TRUE;
+ }
+ ptw32_mcs_lock_release (&stateLock);
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ if (result == 0)
+ {
+ /* Thread is joinable */
+
+ if (destroyIt)
+ {
+ /* The thread has exited or is exiting but has not been joined or
+ * detached. Need to wait in case it's still exiting.
+ */
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ (void) WaitForSingleObjectEx(tp->threadH, INFINITE, FALSE);
+# else
+ (void) WaitForSingleObject(tp->threadH, INFINITE);
+# endif
+#endif
+ ptw32_threadDestroy (thread);
+ }
+ }
+
+ return (result);
+
+} /* pthread_detach */
diff --git a/3rdparty/pthreads-win32/pthread_equal.c b/3rdparty/pthreads-win32/pthread_equal.c
new file mode 100644
index 0000000..5ddd82a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_equal.c
@@ -0,0 +1,76 @@
+/*
+ * pthread_equal.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_equal (pthread_t t1, pthread_t t2)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function returns nonzero if t1 and t2 are equal, else
+ * returns zero
+ *
+ * PARAMETERS
+ * t1,
+ * t2
+ * thread IDs
+ *
+ *
+ * DESCRIPTION
+ * This function returns nonzero if t1 and t2 are equal, else
+ * returns zero.
+ *
+ * RESULTS
+ * non-zero if t1 and t2 refer to the same thread,
+ * 0 t1 and t2 do not refer to the same thread
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ /*
+ * We also accept NULL == NULL - treating NULL as a thread
+ * for this special case, because there is no error that we can return.
+ */
+ result = ( t1.p == t2.p && t1.x == t2.x );
+
+ return (result);
+
+} /* pthread_equal */
diff --git a/3rdparty/pthreads-win32/pthread_exit.c b/3rdparty/pthreads-win32/pthread_exit.c
new file mode 100644
index 0000000..37b3c09
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_exit.c
@@ -0,0 +1,106 @@
+/*
+ * pthread_exit.c
+ *
+ * Description:
+ * This translation unit implements routines associated with exiting from
+ * a thread.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#if !defined(_UWIN)
+/*# include <process.h> */
+#endif
+
+void
+pthread_exit (void *value_ptr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function terminates the calling thread, returning
+ * the value 'value_ptr' to any joining thread.
+ *
+ * PARAMETERS
+ * value_ptr
+ * a generic data value (i.e. not the address of a value)
+ *
+ *
+ * DESCRIPTION
+ * This function terminates the calling thread, returning
+ * the value 'value_ptr' to any joining thread.
+ * NOTE: thread should be joinable.
+ *
+ * RESULTS
+ * N/A
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_thread_t * sp;
+
+ /*
+ * Don't use pthread_self() to avoid creating an implicit POSIX thread handle
+ * unnecessarily.
+ */
+ sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+#if defined(_UWIN)
+ if (--pthread_count <= 0)
+ exit ((int) value_ptr);
+#endif
+
+ if (NULL == sp)
+ {
+ /*
+ * A POSIX thread handle was never created. I.e. this is a
+ * Win32 thread that has never called a pthreads-win32 routine that
+ * required a POSIX handle.
+ *
+ * Implicit POSIX handles are cleaned up in ptw32_throw() now.
+ */
+
+#if ! (defined (__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ _endthreadex ((unsigned) (size_t) value_ptr);
+#else
+ _endthread ();
+#endif
+
+ /* Never reached */
+ }
+
+ sp->exitStatus = value_ptr;
+
+ ptw32_throw (PTW32_EPS_EXIT);
+
+ /* Never reached. */
+
+}
diff --git a/3rdparty/pthreads-win32/pthread_getconcurrency.c b/3rdparty/pthreads-win32/pthread_getconcurrency.c
new file mode 100644
index 0000000..cf9e9c8
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_getconcurrency.c
@@ -0,0 +1,45 @@
+/*
+ * pthread_getconcurrency.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_getconcurrency (void)
+{
+ return ptw32_concurrency;
+}
diff --git a/3rdparty/pthreads-win32/pthread_getschedparam.c b/3rdparty/pthreads-win32/pthread_getschedparam.c
new file mode 100644
index 0000000..0afcfb7
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_getschedparam.c
@@ -0,0 +1,75 @@
+/*
+ * sched_getschedparam.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_getschedparam (pthread_t thread, int *policy,
+ struct sched_param *param)
+{
+ int result;
+
+ /* Validate the thread id. */
+ result = pthread_kill (thread, 0);
+ if (0 != result)
+ {
+ return result;
+ }
+
+ /*
+ * Validate the policy and param args.
+ * Check that a policy constant wasn't passed rather than &policy.
+ */
+ if (policy <= (int *) SCHED_MAX || param == NULL)
+ {
+ return EINVAL;
+ }
+
+ /* Fill out the policy. */
+ *policy = SCHED_OTHER;
+
+ /*
+ * This function must return the priority value set by
+ * the most recent pthread_setschedparam() or pthread_create()
+ * for the target thread. It must not return the actual thread
+ * priority as altered by any system priority adjustments etc.
+ */
+ param->sched_priority = ((ptw32_thread_t *)thread.p)->sched_priority;
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/pthread_getspecific.c b/3rdparty/pthreads-win32/pthread_getspecific.c
new file mode 100644
index 0000000..5ee1641
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_getspecific.c
@@ -0,0 +1,87 @@
+/*
+ * pthread_getspecific.c
+ *
+ * Description:
+ * POSIX thread functions which implement thread-specific data (TSD).
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+void *
+pthread_getspecific (pthread_key_t key)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function returns the current value of key in the
+ * calling thread. If no value has been set for 'key' in
+ * the thread, NULL is returned.
+ *
+ * PARAMETERS
+ * key
+ * an instance of pthread_key_t
+ *
+ *
+ * DESCRIPTION
+ * This function returns the current value of key in the
+ * calling thread. If no value has been set for 'key' in
+ * the thread, NULL is returned.
+ *
+ * RESULTS
+ * key value or NULL on failure
+ *
+ * ------------------------------------------------------
+ */
+{
+ void * ptr;
+
+ if (key == NULL)
+ {
+ ptr = NULL;
+ }
+ else
+ {
+ int lasterror = GetLastError ();
+#if defined(RETAIN_WSALASTERROR)
+ int lastWSAerror = WSAGetLastError ();
+#endif
+ ptr = TlsGetValue (key->key);
+
+ SetLastError (lasterror);
+#if defined(RETAIN_WSALASTERROR)
+ WSASetLastError (lastWSAerror);
+#endif
+ }
+
+ return ptr;
+}
diff --git a/3rdparty/pthreads-win32/pthread_getunique_np.c b/3rdparty/pthreads-win32/pthread_getunique_np.c
new file mode 100644
index 0000000..4496c68
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_getunique_np.c
@@ -0,0 +1,47 @@
+/*
+ * pthread_getunique_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ *
+ */
+unsigned __int64
+pthread_getunique_np (pthread_t thread)
+{
+ return ((ptw32_thread_t*)thread.p)->seqNumber;
+}
diff --git a/3rdparty/pthreads-win32/pthread_getw32threadhandle_np.c b/3rdparty/pthreads-win32/pthread_getw32threadhandle_np.c
new file mode 100644
index 0000000..309a8f2
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_getw32threadhandle_np.c
@@ -0,0 +1,65 @@
+/*
+ * pthread_getw32threadhandle_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * pthread_getw32threadhandle_np()
+ *
+ * Returns the win32 thread handle that the POSIX
+ * thread "thread" is running as.
+ *
+ * Applications can use the win32 handle to set
+ * win32 specific attributes of the thread.
+ */
+HANDLE
+pthread_getw32threadhandle_np (pthread_t thread)
+{
+ return ((ptw32_thread_t *)thread.p)->threadH;
+}
+
+/*
+ * pthread_getw32threadid_np()
+ *
+ * Returns the win32 thread id that the POSIX
+ * thread "thread" is running as.
+ */
+DWORD
+pthread_getw32threadid_np (pthread_t thread)
+{
+ return ((ptw32_thread_t *)thread.p)->thread;
+}
diff --git a/3rdparty/pthreads-win32/pthread_join.c b/3rdparty/pthreads-win32/pthread_join.c
new file mode 100644
index 0000000..c2b7c1e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_join.c
@@ -0,0 +1,157 @@
+/*
+ * pthread_join.c
+ *
+ * Description:
+ * This translation unit implements functions related to thread
+ * synchronisation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Not needed yet, but defining it should indicate clashes with build target
+ * environment that should be fixed.
+ */
+#if !defined(WINCE)
+# include <signal.h>
+#endif
+
+
+int
+pthread_join (pthread_t thread, void **value_ptr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function waits for 'thread' to terminate and
+ * returns the thread's exit value if 'value_ptr' is not
+ * NULL. This also detaches the thread on successful
+ * completion.
+ *
+ * PARAMETERS
+ * thread
+ * an instance of pthread_t
+ *
+ * value_ptr
+ * pointer to an instance of pointer to void
+ *
+ *
+ * DESCRIPTION
+ * This function waits for 'thread' to terminate and
+ * returns the thread's exit value if 'value_ptr' is not
+ * NULL. This also detaches the thread on successful
+ * completion.
+ * NOTE: detached threads cannot be joined or canceled
+ *
+ * RESULTS
+ * 0 'thread' has completed
+ * EINVAL thread is not a joinable thread,
+ * ESRCH no thread could be found with ID 'thread',
+ * ENOENT thread couldn't find it's own valid handle,
+ * EDEADLK attempt to join thread with self
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+ pthread_t self;
+ ptw32_thread_t * tp = (ptw32_thread_t *) thread.p;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ if (NULL == tp
+ || thread.x != tp->ptHandle.x)
+ {
+ result = ESRCH;
+ }
+ else if (PTHREAD_CREATE_DETACHED == tp->detachState)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ result = 0;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ if (result == 0)
+ {
+ /*
+ * The target thread is joinable and can't be reused before we join it.
+ */
+ self = pthread_self();
+
+ if (NULL == self.p)
+ {
+ result = ENOENT;
+ }
+ else if (pthread_equal (self, thread))
+ {
+ result = EDEADLK;
+ }
+ else
+ {
+ /*
+ * Pthread_join is a cancelation point.
+ * If we are canceled then our target thread must not be
+ * detached (destroyed). This is guarranteed because
+ * pthreadCancelableWait will not return if we
+ * are canceled.
+ */
+ result = pthreadCancelableWait (tp->threadH);
+
+ if (0 == result)
+ {
+ if (value_ptr != NULL)
+ {
+ *value_ptr = tp->exitStatus;
+ }
+
+ /*
+ * The result of making multiple simultaneous calls to
+ * pthread_join() or pthread_detach() specifying the same
+ * target is undefined.
+ */
+ result = pthread_detach (thread);
+ }
+ else
+ {
+ result = ESRCH;
+ }
+ }
+ }
+
+ return (result);
+
+} /* pthread_join */
diff --git a/3rdparty/pthreads-win32/pthread_key_create.c b/3rdparty/pthreads-win32/pthread_key_create.c
new file mode 100644
index 0000000..65c6f95
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_key_create.c
@@ -0,0 +1,108 @@
+/*
+ * pthread_key_create.c
+ *
+ * Description:
+ * POSIX thread functions which implement thread-specific data (TSD).
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+/* TLS_OUT_OF_INDEXES not defined on WinCE */
+#if !defined(TLS_OUT_OF_INDEXES)
+#define TLS_OUT_OF_INDEXES 0xffffffff
+#endif
+
+int
+pthread_key_create (pthread_key_t * key, void (PTW32_CDECL *destructor) (void *))
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function creates a thread-specific data key visible
+ * to all threads. All existing and new threads have a value
+ * NULL for key until set using pthread_setspecific. When any
+ * thread with a non-NULL value for key terminates, 'destructor'
+ * is called with key's current value for that thread.
+ *
+ * PARAMETERS
+ * key
+ * pointer to an instance of pthread_key_t
+ *
+ *
+ * DESCRIPTION
+ * This function creates a thread-specific data key visible
+ * to all threads. All existing and new threads have a value
+ * NULL for key until set using pthread_setspecific. When any
+ * thread with a non-NULL value for key terminates, 'destructor'
+ * is called with key's current value for that thread.
+ *
+ * RESULTS
+ * 0 successfully created semaphore,
+ * EAGAIN insufficient resources or PTHREAD_KEYS_MAX
+ * exceeded,
+ * ENOMEM insufficient memory to create the key,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ pthread_key_t newkey;
+
+ if ((newkey = (pthread_key_t) calloc (1, sizeof (*newkey))) == NULL)
+ {
+ result = ENOMEM;
+ }
+ else if ((newkey->key = TlsAlloc ()) == TLS_OUT_OF_INDEXES)
+ {
+ result = EAGAIN;
+
+ free (newkey);
+ newkey = NULL;
+ }
+ else if (destructor != NULL)
+ {
+ /*
+ * Have to manage associations between thread and key;
+ * Therefore, need a lock that allows competing threads
+ * to gain exclusive access to the key->threads list.
+ *
+ * The mutex will only be created when it is first locked.
+ */
+ newkey->keyLock = 0;
+ newkey->destructor = destructor;
+ }
+
+ *key = newkey;
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_key_delete.c b/3rdparty/pthreads-win32/pthread_key_delete.c
new file mode 100644
index 0000000..09d70c6
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_key_delete.c
@@ -0,0 +1,125 @@
+/*
+ * pthread_key_delete.c
+ *
+ * Description:
+ * POSIX thread functions which implement thread-specific data (TSD).
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_key_delete (pthread_key_t key)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function deletes a thread-specific data key. This
+ * does not change the value of the thread specific data key
+ * for any thread and does not run the key's destructor
+ * in any thread so it should be used with caution.
+ *
+ * PARAMETERS
+ * key
+ * pointer to an instance of pthread_key_t
+ *
+ *
+ * DESCRIPTION
+ * This function deletes a thread-specific data key. This
+ * does not change the value of the thread specific data key
+ * for any thread and does not run the key's destructor
+ * in any thread so it should be used with caution.
+ *
+ * RESULTS
+ * 0 successfully deleted the key,
+ * EINVAL key is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_mcs_local_node_t keyLock;
+ int result = 0;
+
+ if (key != NULL)
+ {
+ if (key->threads != NULL && key->destructor != NULL)
+ {
+ ThreadKeyAssoc *assoc;
+ ptw32_mcs_lock_acquire (&(key->keyLock), &keyLock);
+ /*
+ * Run through all Thread<-->Key associations
+ * for this key.
+ *
+ * While we hold at least one of the locks guarding
+ * the assoc, we know that the assoc pointed to by
+ * key->threads is valid.
+ */
+ while ((assoc = (ThreadKeyAssoc *) key->threads) != NULL)
+ {
+ ptw32_mcs_local_node_t threadLock;
+ ptw32_thread_t * thread = assoc->thread;
+
+ if (assoc == NULL)
+ {
+ /* Finished */
+ break;
+ }
+
+ ptw32_mcs_lock_acquire (&(thread->threadLock), &threadLock);
+ /*
+ * Since we are starting at the head of the key's threads
+ * chain, this will also point key->threads at the next assoc.
+ * While we hold key->keyLock, no other thread can insert
+ * a new assoc via pthread_setspecific.
+ */
+ ptw32_tkAssocDestroy (assoc);
+ ptw32_mcs_lock_release (&threadLock);
+ ptw32_mcs_lock_release (&keyLock);
+ }
+ }
+
+ TlsFree (key->key);
+ if (key->destructor != NULL)
+ {
+ /* A thread could be holding the keyLock */
+ ptw32_mcs_lock_acquire (&(key->keyLock), &keyLock);
+ ptw32_mcs_lock_release (&keyLock);
+ }
+
+#if defined( _DEBUG )
+ memset ((char *) key, 0, sizeof (*key));
+#endif
+ free (key);
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_kill.c b/3rdparty/pthreads-win32/pthread_kill.c
new file mode 100644
index 0000000..5473b43
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_kill.c
@@ -0,0 +1,105 @@
+/*
+ * pthread_kill.c
+ *
+ * Description:
+ * This translation unit implements the pthread_kill routine.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Not needed yet, but defining it should indicate clashes with build target
+ * environment that should be fixed.
+ */
+#if !defined(WINCE)
+# include <signal.h>
+#endif
+
+int
+pthread_kill (pthread_t thread, int sig)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function requests that a signal be delivered to the
+ * specified thread. If sig is zero, error checking is
+ * performed but no signal is actually sent such that this
+ * function can be used to check for a valid thread ID.
+ *
+ * PARAMETERS
+ * thread reference to an instances of pthread_t
+ * sig signal. Currently only a value of 0 is supported.
+ *
+ *
+ * DESCRIPTION
+ * This function requests that a signal be delivered to the
+ * specified thread. If sig is zero, error checking is
+ * performed but no signal is actually sent such that this
+ * function can be used to check for a valid thread ID.
+ *
+ * RESULTS
+ * ESRCH the thread is not a valid thread ID,
+ * EINVAL the value of the signal is invalid
+ * or unsupported.
+ * 0 the signal was successfully sent.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ ptw32_thread_t * tp;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ tp = (ptw32_thread_t *) thread.p;
+
+ if (NULL == tp
+ || thread.x != tp->ptHandle.x
+ || NULL == tp->threadH)
+ {
+ result = ESRCH;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ if (0 == result && 0 != sig)
+ {
+ /*
+ * Currently does not support any signals.
+ */
+ result = EINVAL;
+ }
+
+ return result;
+
+} /* pthread_kill */
diff --git a/3rdparty/pthreads-win32/pthread_mutex_consistent.c b/3rdparty/pthreads-win32/pthread_mutex_consistent.c
new file mode 100644
index 0000000..b7805e7
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_consistent.c
@@ -0,0 +1,190 @@
+/*
+ * pthread_mutex_consistent.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+/*
+ * From the Sun Multi-threaded Programming Guide
+ *
+ * robustness defines the behavior when the owner of the mutex terminates without unlocking the
+ * mutex, usually because its process terminated abnormally. The value of robustness that is
+ * defined in pthread.h is PTHREAD_MUTEX_ROBUST or PTHREAD_MUTEX_STALLED. The
+ * default value is PTHREAD_MUTEX_STALLED .
+ * ■ PTHREAD_MUTEX_STALLED
+ * When the owner of the mutex terminates without unlocking the mutex, all subsequent calls
+ * to pthread_mutex_lock() are blocked from progress in an unspecified manner.
+ * ■ PTHREAD_MUTEX_ROBUST
+ * When the owner of the mutex terminates without unlocking the mutex, the mutex is
+ * unlocked. The next owner of this mutex acquires the mutex with an error return of
+ * EOWNERDEAD.
+ * Note – Your application must always check the return code from pthread_mutex_lock() for
+ * a mutex initialized with the PTHREAD_MUTEX_ROBUST attribute.
+ * ■ The new owner of this mutex should make the state protected by the mutex consistent.
+ * This state might have been left inconsistent when the previous owner terminated.
+ * ■ If the new owner is able to make the state consistent, call
+ * pthread_mutex_consistent() for the mutex before unlocking the mutex. This
+ * marks the mutex as consistent and subsequent calls to pthread_mutex_lock() and
+ * pthread_mutex_unlock() will behave in the normal manner.
+ * ■ If the new owner is not able to make the state consistent, do not call
+ * pthread_mutex_consistent() for the mutex, but unlock the mutex.
+ * All waiters are woken up and all subsequent calls to pthread_mutex_lock() fail to
+ * acquire the mutex. The return code is ENOTRECOVERABLE. The mutex can be made
+ * consistent by calling pthread_mutex_destroy() to uninitialize the mutex, and calling
+ * pthread_mutex_int() to reinitialize the mutex.However, the state that was protected
+ * by the mutex remains inconsistent and some form of application recovery is required.
+ * ■ If the thread that acquires the lock with EOWNERDEAD terminates without unlocking the
+ * mutex, the next owner acquires the lock with an EOWNERDEAD return code.
+ */
+#if !defined(_UWIN)
+/*# include <process.h> */
+#endif
+#include "pthread.h"
+#include "implement.h"
+
+INLINE
+int
+ptw32_robust_mutex_inherit(pthread_mutex_t * mutex)
+{
+ int result;
+ pthread_mutex_t mx = *mutex;
+ ptw32_robust_node_t* robust = mx->robustNode;
+
+ switch ((LONG)PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)&robust->stateInconsistent,
+ (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_INCONSISTENT,
+ (PTW32_INTERLOCKED_LONG)-1 /* The terminating thread sets this */))
+ {
+ case -1L:
+ result = EOWNERDEAD;
+ break;
+ case (LONG)PTW32_ROBUST_NOTRECOVERABLE:
+ result = ENOTRECOVERABLE;
+ break;
+ default:
+ result = 0;
+ break;
+ }
+
+ return result;
+}
+
+/*
+ * The next two internal support functions depend on only being
+ * called by the thread that owns the robust mutex. This enables
+ * us to avoid additional locks.
+ * Any mutex currently in the thread's robust mutex list is held
+ * by the thread, again eliminating the need for locks.
+ * The forward/backward links allow the thread to unlock mutexes
+ * in any order, not necessarily the reverse locking order.
+ * This is all possible because it is an error if a thread that
+ * does not own the [robust] mutex attempts to unlock it.
+ */
+
+INLINE
+void
+ptw32_robust_mutex_add(pthread_mutex_t* mutex, pthread_t self)
+{
+ ptw32_robust_node_t** list;
+ pthread_mutex_t mx = *mutex;
+ ptw32_thread_t* tp = (ptw32_thread_t*)self.p;
+ ptw32_robust_node_t* robust = mx->robustNode;
+
+ list = &tp->robustMxList;
+ mx->ownerThread = self;
+ if (NULL == *list)
+ {
+ robust->prev = NULL;
+ robust->next = NULL;
+ *list = robust;
+ }
+ else
+ {
+ robust->prev = NULL;
+ robust->next = *list;
+ (*list)->prev = robust;
+ *list = robust;
+ }
+}
+
+INLINE
+void
+ptw32_robust_mutex_remove(pthread_mutex_t* mutex, ptw32_thread_t* otp)
+{
+ ptw32_robust_node_t** list;
+ pthread_mutex_t mx = *mutex;
+ ptw32_robust_node_t* robust = mx->robustNode;
+
+ list = &(((ptw32_thread_t*)mx->ownerThread.p)->robustMxList);
+ mx->ownerThread.p = otp;
+ if (robust->next != NULL)
+ {
+ robust->next->prev = robust->prev;
+ }
+ if (robust->prev != NULL)
+ {
+ robust->prev->next = robust->next;
+ }
+ if (*list == robust)
+ {
+ *list = robust->next;
+ }
+}
+
+
+int
+pthread_mutex_consistent (pthread_mutex_t* mutex)
+{
+ pthread_mutex_t mx = *mutex;
+ int result = 0;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+ if (mx == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (mx->kind >= 0
+ || (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_INCONSISTENT != PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)&mx->robustNode->stateInconsistent,
+ (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_CONSISTENT,
+ (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_INCONSISTENT))
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+}
+
diff --git a/3rdparty/pthreads-win32/pthread_mutex_destroy.c b/3rdparty/pthreads-win32/pthread_mutex_destroy.c
new file mode 100644
index 0000000..7b8c9cd
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_destroy.c
@@ -0,0 +1,148 @@
+/*
+ * pthread_mutex_destroy.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutex_destroy (pthread_mutex_t * mutex)
+{
+ int result = 0;
+ pthread_mutex_t mx;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+
+ /*
+ * Check to see if we have something to delete.
+ */
+ if (*mutex < PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ mx = *mutex;
+
+ result = pthread_mutex_trylock (&mx);
+
+ /*
+ * If trylock succeeded and the mutex is not recursively locked it
+ * can be destroyed.
+ */
+ if (0 == result || ENOTRECOVERABLE == result)
+ {
+ if (mx->kind != PTHREAD_MUTEX_RECURSIVE || 1 == mx->recursive_count)
+ {
+ /*
+ * FIXME!!!
+ * The mutex isn't held by another thread but we could still
+ * be too late invalidating the mutex below since another thread
+ * may already have entered mutex_lock and the check for a valid
+ * *mutex != NULL.
+ */
+ *mutex = NULL;
+
+ result = (0 == result)?pthread_mutex_unlock(&mx):0;
+
+ if (0 == result)
+ {
+ if (mx->robustNode != NULL)
+ {
+ free(mx->robustNode);
+ }
+ if (!CloseHandle (mx->event))
+ {
+ *mutex = mx;
+ result = EINVAL;
+ }
+ else
+ {
+ free (mx);
+ }
+ }
+ else
+ {
+ /*
+ * Restore the mutex before we return the error.
+ */
+ *mutex = mx;
+ }
+ }
+ else /* mx->recursive_count > 1 */
+ {
+ /*
+ * The mutex must be recursive and already locked by us (this thread).
+ */
+ mx->recursive_count--; /* Undo effect of pthread_mutex_trylock() above */
+ result = EBUSY;
+ }
+ }
+ }
+ else
+ {
+ ptw32_mcs_local_node_t node;
+
+ /*
+ * See notes in ptw32_mutex_check_need_init() above also.
+ */
+
+ ptw32_mcs_lock_acquire(&ptw32_mutex_test_init_lock, &node);
+
+ /*
+ * Check again.
+ */
+ if (*mutex >= PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ /*
+ * This is all we need to do to destroy a statically
+ * initialised mutex that has not yet been used (initialised).
+ * If we get to here, another thread
+ * waiting to initialise this mutex will get an EINVAL.
+ */
+ *mutex = NULL;
+ }
+ else
+ {
+ /*
+ * The mutex has been initialised while we were waiting
+ * so assume it's in use.
+ */
+ result = EBUSY;
+ }
+ ptw32_mcs_lock_release(&node);
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutex_init.c b/3rdparty/pthreads-win32/pthread_mutex_init.c
new file mode 100644
index 0000000..ff5f899
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_init.c
@@ -0,0 +1,136 @@
+/*
+ * pthread_mutex_init.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutex_init (pthread_mutex_t * mutex, const pthread_mutexattr_t * attr)
+{
+ int result = 0;
+ pthread_mutex_t mx;
+
+ if (mutex == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (attr != NULL && *attr != NULL)
+ {
+ if ((*attr)->pshared == PTHREAD_PROCESS_SHARED)
+ {
+ /*
+ * Creating mutex that can be shared between
+ * processes.
+ */
+#if _POSIX_THREAD_PROCESS_SHARED >= 0
+
+ /*
+ * Not implemented yet.
+ */
+
+#error ERROR [__FILE__, line __LINE__]: Process shared mutexes are not supported yet.
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+ }
+ }
+
+ mx = (pthread_mutex_t) calloc (1, sizeof (*mx));
+
+ if (mx == NULL)
+ {
+ result = ENOMEM;
+ }
+ else
+ {
+ mx->lock_idx = 0;
+ mx->recursive_count = 0;
+ mx->robustNode = NULL;
+ if (attr == NULL || *attr == NULL)
+ {
+ mx->kind = PTHREAD_MUTEX_DEFAULT;
+ }
+ else
+ {
+ mx->kind = (*attr)->kind;
+ if ((*attr)->robustness == PTHREAD_MUTEX_ROBUST)
+ {
+ /*
+ * Use the negative range to represent robust types.
+ * Replaces a memory fetch with a register negate and incr
+ * in pthread_mutex_lock etc.
+ *
+ * Map 0,1,..,n to -1,-2,..,(-n)-1
+ */
+ mx->kind = -mx->kind - 1;
+
+ mx->robustNode = (ptw32_robust_node_t*) malloc(sizeof(ptw32_robust_node_t));
+ mx->robustNode->stateInconsistent = PTW32_ROBUST_CONSISTENT;
+ mx->robustNode->mx = mx;
+ mx->robustNode->next = NULL;
+ mx->robustNode->prev = NULL;
+ }
+ }
+
+ mx->ownerThread.p = NULL;
+
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ mx->event = CreateEventEx(NULL, NULL, 0, EVENT_ALL_ACCESS);
+# else
+ mx->event = CreateEvent (NULL, PTW32_FALSE, /* manual reset = No */
+ PTW32_FALSE, /* initial state = not signaled */
+ NULL); /* event name */
+# endif
+#endif
+
+ if (0 == mx->event)
+ {
+ result = ENOSPC;
+ free (mx);
+ mx = NULL;
+ }
+ }
+
+ *mutex = mx;
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutex_lock.c b/3rdparty/pthreads-win32/pthread_mutex_lock.c
new file mode 100644
index 0000000..de2c09e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_lock.c
@@ -0,0 +1,293 @@
+/*
+ * pthread_mutex_lock.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined(_UWIN)
+/*# include <process.h> */
+#endif
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_mutex_lock (pthread_mutex_t * mutex)
+{
+ int kind;
+ pthread_mutex_t mx;
+ int result = 0;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+ if (*mutex == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static mutex. We check
+ * again inside the guarded section of ptw32_mutex_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*mutex >= PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ if ((result = ptw32_mutex_check_need_init (mutex)) != 0)
+ {
+ return (result);
+ }
+ }
+
+ mx = *mutex;
+ kind = mx->kind;
+
+ if (kind >= 0)
+ {
+ /* Non-robust */
+ if (PTHREAD_MUTEX_NORMAL == kind)
+ {
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1) != 0)
+ {
+ while ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WAIT_OBJECT_0 != WaitForSingleObjectEx(mx->event, INFINITE, FALSE))
+# else
+ if (WAIT_OBJECT_0 != WaitForSingleObject (mx->event, INFINITE))
+# endif
+#endif
+ {
+ result = EINVAL;
+ break;
+ }
+ }
+ }
+ }
+ else
+ {
+ pthread_t self = pthread_self();
+
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0) == 0)
+ {
+ mx->recursive_count = 1;
+ mx->ownerThread = self;
+ }
+ else
+ {
+ if (pthread_equal (mx->ownerThread, self))
+ {
+ if (kind == PTHREAD_MUTEX_RECURSIVE)
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ result = EDEADLK;
+ }
+ }
+ else
+ {
+ while ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WAIT_OBJECT_0 != WaitForSingleObjectEx(mx->event, INFINITE, FALSE))
+# else
+ if (WAIT_OBJECT_0 != WaitForSingleObject (mx->event, INFINITE))
+# endif
+#endif
+ {
+ result = EINVAL;
+ break;
+ }
+ }
+
+ if (0 == result)
+ {
+ mx->recursive_count = 1;
+ mx->ownerThread = self;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ /*
+ * Robust types
+ * All types record the current owner thread.
+ * The mutex is added to a per thread list when ownership is acquired.
+ */
+ ptw32_robust_state_t* statePtr = &mx->robustNode->stateInconsistent;
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE == PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ result = ENOTRECOVERABLE;
+ }
+ else
+ {
+ pthread_t self = pthread_self();
+
+ kind = -kind - 1; /* Convert to non-robust range */
+
+ if (PTHREAD_MUTEX_NORMAL == kind)
+ {
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1) != 0)
+ {
+ while (0 == (result = ptw32_robust_mutex_inherit(mutex))
+ && (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WAIT_OBJECT_0 != WaitForSingleObjectEx(mx->event, INFINITE, FALSE))
+# else
+ if (WAIT_OBJECT_0 != WaitForSingleObject (mx->event, INFINITE))
+# endif
+#endif
+ {
+ result = EINVAL;
+ break;
+ }
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ /* Unblock the next thread */
+ SetEvent(mx->event);
+ result = ENOTRECOVERABLE;
+ break;
+ }
+ }
+ }
+ if (0 == result || EOWNERDEAD == result)
+ {
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ }
+ else
+ {
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0) == 0)
+ {
+ mx->recursive_count = 1;
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ else
+ {
+ if (pthread_equal (mx->ownerThread, self))
+ {
+ if (PTHREAD_MUTEX_RECURSIVE == kind)
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ result = EDEADLK;
+ }
+ }
+ else
+ {
+ while (0 == (result = ptw32_robust_mutex_inherit(mutex))
+ && (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WAIT_OBJECT_0 != WaitForSingleObjectEx(mx->event, INFINITE, FALSE))
+# else
+ if (WAIT_OBJECT_0 != WaitForSingleObject (mx->event, INFINITE))
+# endif
+#endif
+ {
+ result = EINVAL;
+ break;
+ }
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ /* Unblock the next thread */
+ SetEvent(mx->event);
+ result = ENOTRECOVERABLE;
+ break;
+ }
+ }
+
+ if (0 == result || EOWNERDEAD == result)
+ {
+ mx->recursive_count = 1;
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return (result);
+}
+
diff --git a/3rdparty/pthreads-win32/pthread_mutex_timedlock.c b/3rdparty/pthreads-win32/pthread_mutex_timedlock.c
new file mode 100644
index 0000000..1745316
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_timedlock.c
@@ -0,0 +1,324 @@
+/*
+ * pthread_mutex_timedlock.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+static INLINE int
+ptw32_timed_eventwait (HANDLE event, const struct timespec *abstime)
+ /*
+ * ------------------------------------------------------
+ * DESCRIPTION
+ * This function waits on an event until signaled or until
+ * abstime passes.
+ * If abstime has passed when this routine is called then
+ * it returns a result to indicate this.
+ *
+ * If 'abstime' is a NULL pointer then this function will
+ * block until it can successfully decrease the value or
+ * until interrupted by a signal.
+ *
+ * This routine is not a cancelation point.
+ *
+ * RESULTS
+ * 0 successfully signaled,
+ * ETIMEDOUT abstime passed
+ * EINVAL 'event' is not a valid event,
+ *
+ * ------------------------------------------------------
+ */
+{
+
+ DWORD milliseconds;
+ DWORD status;
+
+ if (event == NULL)
+ {
+ return EINVAL;
+ }
+ else
+ {
+ if (abstime == NULL)
+ {
+ milliseconds = INFINITE;
+ }
+ else
+ {
+ /*
+ * Calculate timeout as milliseconds from current system time.
+ */
+ milliseconds = ptw32_relmillisecs (abstime);
+ }
+
+ status = WaitForSingleObject (event, milliseconds);
+
+ if (status == WAIT_OBJECT_0)
+ {
+ return 0;
+ }
+ else if (status == WAIT_TIMEOUT)
+ {
+ return ETIMEDOUT;
+ }
+ else
+ {
+ return EINVAL;
+ }
+ }
+
+ return 0;
+
+} /* ptw32_timed_semwait */
+
+
+int
+pthread_mutex_timedlock (pthread_mutex_t * mutex,
+ const struct timespec *abstime)
+{
+ pthread_mutex_t mx;
+ int kind;
+ int result = 0;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static mutex. We check
+ * again inside the guarded section of ptw32_mutex_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*mutex >= PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ if ((result = ptw32_mutex_check_need_init (mutex)) != 0)
+ {
+ return (result);
+ }
+ }
+
+ mx = *mutex;
+ kind = mx->kind;
+
+ if (kind >= 0)
+ {
+ if (mx->kind == PTHREAD_MUTEX_NORMAL)
+ {
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1) != 0)
+ {
+ while ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+ if (0 != (result = ptw32_timed_eventwait (mx->event, abstime)))
+ {
+ return result;
+ }
+ }
+ }
+ }
+ else
+ {
+ pthread_t self = pthread_self();
+
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0) == 0)
+ {
+ mx->recursive_count = 1;
+ mx->ownerThread = self;
+ }
+ else
+ {
+ if (pthread_equal (mx->ownerThread, self))
+ {
+ if (mx->kind == PTHREAD_MUTEX_RECURSIVE)
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ return EDEADLK;
+ }
+ }
+ else
+ {
+ while ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+ if (0 != (result = ptw32_timed_eventwait (mx->event, abstime)))
+ {
+ return result;
+ }
+ }
+
+ mx->recursive_count = 1;
+ mx->ownerThread = self;
+ }
+ }
+ }
+ }
+ else
+ {
+ /*
+ * Robust types
+ * All types record the current owner thread.
+ * The mutex is added to a per thread list when ownership is acquired.
+ */
+ ptw32_robust_state_t* statePtr = &mx->robustNode->stateInconsistent;
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE == PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ result = ENOTRECOVERABLE;
+ }
+ else
+ {
+ pthread_t self = pthread_self();
+
+ kind = -kind - 1; /* Convert to non-robust range */
+
+ if (PTHREAD_MUTEX_NORMAL == kind)
+ {
+ if ((PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1) != 0)
+ {
+ while (0 == (result = ptw32_robust_mutex_inherit(mutex))
+ && (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+ if (0 != (result = ptw32_timed_eventwait (mx->event, abstime)))
+ {
+ return result;
+ }
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ /* Unblock the next thread */
+ SetEvent(mx->event);
+ result = ENOTRECOVERABLE;
+ break;
+ }
+ }
+
+ if (0 == result || EOWNERDEAD == result)
+ {
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ }
+ }
+ else
+ {
+ pthread_t self = pthread_self();
+
+ if (0 == (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0))
+ {
+ mx->recursive_count = 1;
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ else
+ {
+ if (pthread_equal (mx->ownerThread, self))
+ {
+ if (PTHREAD_MUTEX_RECURSIVE == kind)
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ return EDEADLK;
+ }
+ }
+ else
+ {
+ while (0 == (result = ptw32_robust_mutex_inherit(mutex))
+ && (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) -1) != 0)
+ {
+ if (0 != (result = ptw32_timed_eventwait (mx->event, abstime)))
+ {
+ return result;
+ }
+ }
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ /* Unblock the next thread */
+ SetEvent(mx->event);
+ result = ENOTRECOVERABLE;
+ }
+ else if (0 == result || EOWNERDEAD == result)
+ {
+ mx->recursive_count = 1;
+ /*
+ * Add mutex to the per-thread robust mutex currently-held list.
+ * If the thread terminates, all mutexes in this list will be unlocked.
+ */
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ }
+ }
+ }
+ }
+ }
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutex_trylock.c b/3rdparty/pthreads-win32/pthread_mutex_trylock.c
new file mode 100644
index 0000000..d6b6872
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_trylock.c
@@ -0,0 +1,154 @@
+/*
+ * pthread_mutex_trylock.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutex_trylock (pthread_mutex_t * mutex)
+{
+ pthread_mutex_t mx;
+ int kind;
+ int result = 0;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static mutex. We check
+ * again inside the guarded section of ptw32_mutex_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*mutex >= PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ if ((result = ptw32_mutex_check_need_init (mutex)) != 0)
+ {
+ return (result);
+ }
+ }
+
+ mx = *mutex;
+ kind = mx->kind;
+
+ if (kind >= 0)
+ {
+ /* Non-robust */
+ if (0 == (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG (
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0))
+ {
+ if (kind != PTHREAD_MUTEX_NORMAL)
+ {
+ mx->recursive_count = 1;
+ mx->ownerThread = pthread_self ();
+ }
+ }
+ else
+ {
+ if (kind == PTHREAD_MUTEX_RECURSIVE &&
+ pthread_equal (mx->ownerThread, pthread_self ()))
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ result = EBUSY;
+ }
+ }
+ }
+ else
+ {
+ /*
+ * Robust types
+ * All types record the current owner thread.
+ * The mutex is added to a per thread list when ownership is acquired.
+ */
+ pthread_t self;
+ ptw32_robust_state_t* statePtr = &mx->robustNode->stateInconsistent;
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)statePtr,
+ (PTW32_INTERLOCKED_LONG)0))
+ {
+ return ENOTRECOVERABLE;
+ }
+
+ self = pthread_self();
+ kind = -kind - 1; /* Convert to non-robust range */
+
+ if (0 == (PTW32_INTERLOCKED_LONG) PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG (
+ (PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 1,
+ (PTW32_INTERLOCKED_LONG) 0))
+ {
+ if (kind != PTHREAD_MUTEX_NORMAL)
+ {
+ mx->recursive_count = 1;
+ }
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ else
+ {
+ if (PTHREAD_MUTEX_RECURSIVE == kind &&
+ pthread_equal (mx->ownerThread, pthread_self ()))
+ {
+ mx->recursive_count++;
+ }
+ else
+ {
+ if (EOWNERDEAD == (result = ptw32_robust_mutex_inherit(mutex)))
+ {
+ mx->recursive_count = 1;
+ ptw32_robust_mutex_add(mutex, self);
+ }
+ else
+ {
+ if (0 == result)
+ {
+ result = EBUSY;
+ }
+ }
+ }
+ }
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutex_unlock.c b/3rdparty/pthreads-win32/pthread_mutex_unlock.c
new file mode 100644
index 0000000..3d65d1a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutex_unlock.c
@@ -0,0 +1,175 @@
+/*
+ * pthread_mutex_unlock.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutex_unlock (pthread_mutex_t * mutex)
+{
+ int result = 0;
+ int kind;
+ pthread_mutex_t mx;
+
+ /*
+ * Let the system deal with invalid pointers.
+ */
+
+ mx = *mutex;
+
+ /*
+ * If the thread calling us holds the mutex then there is no
+ * race condition. If another thread holds the
+ * lock then we shouldn't be in here.
+ */
+ if (mx < PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ kind = mx->kind;
+
+ if (kind >= 0)
+ {
+ if (kind == PTHREAD_MUTEX_NORMAL)
+ {
+ LONG idx;
+
+ idx = (LONG) PTW32_INTERLOCKED_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR)&mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG)0);
+ if (idx != 0)
+ {
+ if (idx < 0)
+ {
+ /*
+ * Someone may be waiting on that mutex.
+ */
+ if (SetEvent (mx->event) == 0)
+ {
+ result = EINVAL;
+ }
+ }
+ }
+ }
+ else
+ {
+ if (pthread_equal (mx->ownerThread, pthread_self()))
+ {
+ if (kind != PTHREAD_MUTEX_RECURSIVE
+ || 0 == --mx->recursive_count)
+ {
+ mx->ownerThread.p = NULL;
+
+ if ((LONG) PTW32_INTERLOCKED_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR)&mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG)0) < 0L)
+ {
+ /* Someone may be waiting on that mutex */
+ if (SetEvent (mx->event) == 0)
+ {
+ result = EINVAL;
+ }
+ }
+ }
+ }
+ else
+ {
+ result = EPERM;
+ }
+ }
+ }
+ else
+ {
+ /* Robust types */
+ pthread_t self = pthread_self();
+ kind = -kind - 1; /* Convert to non-robust range */
+
+ /*
+ * The thread must own the lock regardless of type if the mutex
+ * is robust.
+ */
+ if (pthread_equal (mx->ownerThread, self))
+ {
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG((PTW32_INTERLOCKED_LONGPTR) &mx->robustNode->stateInconsistent,
+ (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_NOTRECOVERABLE,
+ (PTW32_INTERLOCKED_LONG)PTW32_ROBUST_INCONSISTENT);
+ if (PTHREAD_MUTEX_NORMAL == kind)
+ {
+ ptw32_robust_mutex_remove(mutex, NULL);
+
+ if ((LONG) PTW32_INTERLOCKED_EXCHANGE_LONG((PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 0) < 0)
+ {
+ /*
+ * Someone may be waiting on that mutex.
+ */
+ if (SetEvent (mx->event) == 0)
+ {
+ result = EINVAL;
+ }
+ }
+ }
+ else
+ {
+ if (kind != PTHREAD_MUTEX_RECURSIVE
+ || 0 == --mx->recursive_count)
+ {
+ ptw32_robust_mutex_remove(mutex, NULL);
+
+ if ((LONG) PTW32_INTERLOCKED_EXCHANGE_LONG((PTW32_INTERLOCKED_LONGPTR) &mx->lock_idx,
+ (PTW32_INTERLOCKED_LONG) 0) < 0)
+ {
+ /*
+ * Someone may be waiting on that mutex.
+ */
+ if (SetEvent (mx->event) == 0)
+ {
+ result = EINVAL;
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ result = EPERM;
+ }
+ }
+ }
+ else if (mx != PTHREAD_MUTEX_INITIALIZER)
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_destroy.c b/3rdparty/pthreads-win32/pthread_mutexattr_destroy.c
new file mode 100644
index 0000000..9d424bf
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_destroy.c
@@ -0,0 +1,83 @@
+/*
+ * pthread_mutexattr_destroy.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_destroy (pthread_mutexattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Destroys a mutex attributes object. The object can
+ * no longer be used.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ *
+ * DESCRIPTION
+ * Destroys a mutex attributes object. The object can
+ * no longer be used.
+ *
+ * NOTES:
+ * 1) Does not affect mutexes created using 'attr'
+ *
+ * RESULTS
+ * 0 successfully released attr,
+ * EINVAL 'attr' is invalid.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+
+ if (attr == NULL || *attr == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ pthread_mutexattr_t ma = *attr;
+
+ *attr = NULL;
+ free (ma);
+ }
+
+ return (result);
+} /* pthread_mutexattr_destroy */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_getkind_np.c b/3rdparty/pthreads-win32/pthread_mutexattr_getkind_np.c
new file mode 100644
index 0000000..2d82ec6
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_getkind_np.c
@@ -0,0 +1,44 @@
+/*
+ * pthread_mutexattr_getkind_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_mutexattr_getkind_np (pthread_mutexattr_t * attr, int *kind)
+{
+ return pthread_mutexattr_gettype (attr, kind);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_getpshared.c b/3rdparty/pthreads-win32/pthread_mutexattr_getpshared.c
new file mode 100644
index 0000000..42f9589
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_getpshared.c
@@ -0,0 +1,95 @@
+/*
+ * pthread_mutexattr_getpshared.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_getpshared (const pthread_mutexattr_t * attr, int *pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Determine whether mutexes created with 'attr' can be
+ * shared between processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * pshared
+ * will be set to one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ *
+ * DESCRIPTION
+ * Mutexes creatd with 'attr' can be shared between
+ * processes if pthread_mutex_t variable is allocated
+ * in memory shared by these processes.
+ * NOTES:
+ * 1) pshared mutexes MUST be allocated in shared
+ * memory.
+ * 2) The following macro is defined if shared mutexes
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully retrieved attribute,
+ * EINVAL 'attr' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) && (pshared != NULL))
+ {
+ *pshared = (*attr)->pshared;
+ result = 0;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+
+} /* pthread_mutexattr_getpshared */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_getrobust.c b/3rdparty/pthreads-win32/pthread_mutexattr_getrobust.c
new file mode 100644
index 0000000..be00483
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_getrobust.c
@@ -0,0 +1,113 @@
+/*
+ * pthread_mutexattr_getrobust.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_getrobust (const pthread_mutexattr_t * attr, int * robust)
+ /*
+ * ------------------------------------------------------
+ *
+ * DOCPUBLIC
+ * The pthread_mutexattr_setrobust() and
+ * pthread_mutexattr_getrobust() functions respectively set and
+ * get the mutex robust attribute. This attribute is set in the
+ * robust parameter to these functions.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * robust
+ * must be one of:
+ *
+ * PTHREAD_MUTEX_STALLED
+ *
+ * PTHREAD_MUTEX_ROBUST
+ *
+ * DESCRIPTION
+ * The pthread_mutexattr_setrobust() and
+ * pthread_mutexattr_getrobust() functions respectively set and
+ * get the mutex robust attribute. This attribute is set in the
+ * robust parameter to these functions. The default value of the
+ * robust attribute is PTHREAD_MUTEX_STALLED.
+ *
+ * The robustness of mutex is contained in the robustness attribute
+ * of the mutex attributes. Valid mutex robustness values are:
+ *
+ * PTHREAD_MUTEX_STALLED
+ * No special actions are taken if the owner of the mutex is
+ * terminated while holding the mutex lock. This can lead to
+ * deadlocks if no other thread can unlock the mutex.
+ * This is the default value.
+ *
+ * PTHREAD_MUTEX_ROBUST
+ * If the process containing the owning thread of a robust mutex
+ * terminates while holding the mutex lock, the next thread that
+ * acquires the mutex shall be notified about the termination by
+ * the return value [EOWNERDEAD] from the locking function. If the
+ * owning thread of a robust mutex terminates while holding the mutex
+ * lock, the next thread that acquires the mutex may be notified
+ * about the termination by the return value [EOWNERDEAD]. The
+ * notified thread can then attempt to mark the state protected by
+ * the mutex as consistent again by a call to
+ * pthread_mutex_consistent(). After a subsequent successful call to
+ * pthread_mutex_unlock(), the mutex lock shall be released and can
+ * be used normally by other threads. If the mutex is unlocked without
+ * a call to pthread_mutex_consistent(), it shall be in a permanently
+ * unusable state and all attempts to lock the mutex shall fail with
+ * the error [ENOTRECOVERABLE]. The only permissible operation on such
+ * a mutex is pthread_mutex_destroy().
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or 'robust' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = EINVAL;
+
+ if ((attr != NULL && *attr != NULL && robust != NULL))
+ {
+ *robust = (*attr)->robustness;
+ result = 0;
+ }
+
+ return (result);
+} /* pthread_mutexattr_getrobust */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_gettype.c b/3rdparty/pthreads-win32/pthread_mutexattr_gettype.c
new file mode 100644
index 0000000..c63fcfa
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_gettype.c
@@ -0,0 +1,56 @@
+/*
+ * pthread_mutexattr_gettype.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_gettype (const pthread_mutexattr_t * attr, int *kind)
+{
+ int result = 0;
+
+ if (attr != NULL && *attr != NULL && kind != NULL)
+ {
+ *kind = (*attr)->kind;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_init.c b/3rdparty/pthreads-win32/pthread_mutexattr_init.c
new file mode 100644
index 0000000..d2797ff
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_init.c
@@ -0,0 +1,86 @@
+/*
+ * pthread_mutexattr_init.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_init (pthread_mutexattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Initializes a mutex attributes object with default
+ * attributes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ *
+ * DESCRIPTION
+ * Initializes a mutex attributes object with default
+ * attributes.
+ *
+ * NOTES:
+ * 1) Used to define mutex types
+ *
+ * RESULTS
+ * 0 successfully initialized attr,
+ * ENOMEM insufficient memory for attr.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ pthread_mutexattr_t ma;
+
+ ma = (pthread_mutexattr_t) calloc (1, sizeof (*ma));
+
+ if (ma == NULL)
+ {
+ result = ENOMEM;
+ }
+ else
+ {
+ ma->pshared = PTHREAD_PROCESS_PRIVATE;
+ ma->kind = PTHREAD_MUTEX_DEFAULT;
+ }
+
+ *attr = ma;
+
+ return (result);
+} /* pthread_mutexattr_init */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_setkind_np.c b/3rdparty/pthreads-win32/pthread_mutexattr_setkind_np.c
new file mode 100644
index 0000000..faa9366
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_setkind_np.c
@@ -0,0 +1,44 @@
+/*
+ * pthread_mutexattr_setkind_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_mutexattr_setkind_np (pthread_mutexattr_t * attr, int kind)
+{
+ return pthread_mutexattr_settype (attr, kind);
+}
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_setpshared.c b/3rdparty/pthreads-win32/pthread_mutexattr_setpshared.c
new file mode 100644
index 0000000..cfa6f71
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_setpshared.c
@@ -0,0 +1,119 @@
+/*
+ * pthread_mutexattr_setpshared.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_setpshared (pthread_mutexattr_t * attr, int pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Mutexes created with 'attr' can be shared between
+ * processes if pthread_mutex_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * pshared
+ * must be one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ * DESCRIPTION
+ * Mutexes creatd with 'attr' can be shared between
+ * processes if pthread_mutex_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * NOTES:
+ * 1) pshared mutexes MUST be allocated in shared
+ * memory.
+ *
+ * 2) The following macro is defined if shared mutexes
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or pshared is invalid,
+ * ENOSYS PTHREAD_PROCESS_SHARED not supported,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) &&
+ ((pshared == PTHREAD_PROCESS_SHARED) ||
+ (pshared == PTHREAD_PROCESS_PRIVATE)))
+ {
+ if (pshared == PTHREAD_PROCESS_SHARED)
+ {
+
+#if !defined( _POSIX_THREAD_PROCESS_SHARED )
+
+ result = ENOSYS;
+ pshared = PTHREAD_PROCESS_PRIVATE;
+
+#else
+
+ result = 0;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+
+ }
+ else
+ {
+ result = 0;
+ }
+
+ (*attr)->pshared = pshared;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+
+} /* pthread_mutexattr_setpshared */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_setrobust.c b/3rdparty/pthreads-win32/pthread_mutexattr_setrobust.c
new file mode 100644
index 0000000..b1acef7
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_setrobust.c
@@ -0,0 +1,119 @@
+/*
+ * pthread_mutexattr_setrobust.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_setrobust (pthread_mutexattr_t * attr, int robust)
+ /*
+ * ------------------------------------------------------
+ *
+ * DOCPUBLIC
+ * The pthread_mutexattr_setrobust() and
+ * pthread_mutexattr_getrobust() functions respectively set and
+ * get the mutex robust attribute. This attribute is set in the
+ * robust parameter to these functions.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * robust
+ * must be one of:
+ *
+ * PTHREAD_MUTEX_STALLED
+ *
+ * PTHREAD_MUTEX_ROBUST
+ *
+ * DESCRIPTION
+ * The pthread_mutexattr_setrobust() and
+ * pthread_mutexattr_getrobust() functions respectively set and
+ * get the mutex robust attribute. This attribute is set in the
+ * robust parameter to these functions. The default value of the
+ * robust attribute is PTHREAD_MUTEX_STALLED.
+ *
+ * The robustness of mutex is contained in the robustness attribute
+ * of the mutex attributes. Valid mutex robustness values are:
+ *
+ * PTHREAD_MUTEX_STALLED
+ * No special actions are taken if the owner of the mutex is
+ * terminated while holding the mutex lock. This can lead to
+ * deadlocks if no other thread can unlock the mutex.
+ * This is the default value.
+ *
+ * PTHREAD_MUTEX_ROBUST
+ * If the process containing the owning thread of a robust mutex
+ * terminates while holding the mutex lock, the next thread that
+ * acquires the mutex shall be notified about the termination by
+ * the return value [EOWNERDEAD] from the locking function. If the
+ * owning thread of a robust mutex terminates while holding the mutex
+ * lock, the next thread that acquires the mutex may be notified
+ * about the termination by the return value [EOWNERDEAD]. The
+ * notified thread can then attempt to mark the state protected by
+ * the mutex as consistent again by a call to
+ * pthread_mutex_consistent(). After a subsequent successful call to
+ * pthread_mutex_unlock(), the mutex lock shall be released and can
+ * be used normally by other threads. If the mutex is unlocked without
+ * a call to pthread_mutex_consistent(), it shall be in a permanently
+ * unusable state and all attempts to lock the mutex shall fail with
+ * the error [ENOTRECOVERABLE]. The only permissible operation on such
+ * a mutex is pthread_mutex_destroy().
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or 'robust' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = EINVAL;
+
+ if ((attr != NULL && *attr != NULL))
+ {
+ switch (robust)
+ {
+ case PTHREAD_MUTEX_STALLED:
+ case PTHREAD_MUTEX_ROBUST:
+ (*attr)->robustness = robust;
+ result = 0;
+ break;
+ }
+ }
+
+ return (result);
+} /* pthread_mutexattr_setrobust */
diff --git a/3rdparty/pthreads-win32/pthread_mutexattr_settype.c b/3rdparty/pthreads-win32/pthread_mutexattr_settype.c
new file mode 100644
index 0000000..8365daf
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_mutexattr_settype.c
@@ -0,0 +1,143 @@
+/*
+ * pthread_mutexattr_settype.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_mutexattr_settype (pthread_mutexattr_t * attr, int kind)
+ /*
+ * ------------------------------------------------------
+ *
+ * DOCPUBLIC
+ * The pthread_mutexattr_settype() and
+ * pthread_mutexattr_gettype() functions respectively set and
+ * get the mutex type attribute. This attribute is set in the
+ * type parameter to these functions.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_mutexattr_t
+ *
+ * type
+ * must be one of:
+ *
+ * PTHREAD_MUTEX_DEFAULT
+ *
+ * PTHREAD_MUTEX_NORMAL
+ *
+ * PTHREAD_MUTEX_ERRORCHECK
+ *
+ * PTHREAD_MUTEX_RECURSIVE
+ *
+ * DESCRIPTION
+ * The pthread_mutexattr_settype() and
+ * pthread_mutexattr_gettype() functions respectively set and
+ * get the mutex type attribute. This attribute is set in the
+ * type parameter to these functions. The default value of the
+ * type attribute is PTHREAD_MUTEX_DEFAULT.
+ *
+ * The type of mutex is contained in the type attribute of the
+ * mutex attributes. Valid mutex types include:
+ *
+ * PTHREAD_MUTEX_NORMAL
+ * This type of mutex does not detect deadlock. A
+ * thread attempting to relock this mutex without
+ * first unlocking it will deadlock. Attempting to
+ * unlock a mutex locked by a different thread
+ * results in undefined behavior. Attempting to
+ * unlock an unlocked mutex results in undefined
+ * behavior.
+ *
+ * PTHREAD_MUTEX_ERRORCHECK
+ * This type of mutex provides error checking. A
+ * thread attempting to relock this mutex without
+ * first unlocking it will return with an error. A
+ * thread attempting to unlock a mutex which another
+ * thread has locked will return with an error. A
+ * thread attempting to unlock an unlocked mutex will
+ * return with an error.
+ *
+ * PTHREAD_MUTEX_DEFAULT
+ * Same as PTHREAD_MUTEX_NORMAL.
+ *
+ * PTHREAD_MUTEX_RECURSIVE
+ * A thread attempting to relock this mutex without
+ * first unlocking it will succeed in locking the
+ * mutex. The relocking deadlock which can occur with
+ * mutexes of type PTHREAD_MUTEX_NORMAL cannot occur
+ * with this type of mutex. Multiple locks of this
+ * mutex require the same number of unlocks to
+ * release the mutex before another thread can
+ * acquire the mutex. A thread attempting to unlock a
+ * mutex which another thread has locked will return
+ * with an error. A thread attempting to unlock an
+ * unlocked mutex will return with an error. This
+ * type of mutex is only supported for mutexes whose
+ * process shared attribute is
+ * PTHREAD_PROCESS_PRIVATE.
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or 'type' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+
+ if ((attr != NULL && *attr != NULL))
+ {
+ switch (kind)
+ {
+ case PTHREAD_MUTEX_FAST_NP:
+ case PTHREAD_MUTEX_RECURSIVE_NP:
+ case PTHREAD_MUTEX_ERRORCHECK_NP:
+ (*attr)->kind = kind;
+ break;
+ default:
+ result = EINVAL;
+ break;
+ }
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+} /* pthread_mutexattr_settype */
diff --git a/3rdparty/pthreads-win32/pthread_num_processors_np.c b/3rdparty/pthreads-win32/pthread_num_processors_np.c
new file mode 100644
index 0000000..3067d11
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_num_processors_np.c
@@ -0,0 +1,56 @@
+/*
+ * pthread_num_processors_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * pthread_num_processors_np()
+ *
+ * Get the number of CPUs available to the process.
+ */
+int
+pthread_num_processors_np (void)
+{
+ int count;
+
+ if (ptw32_getprocessors (&count) != 0)
+ {
+ count = 1;
+ }
+
+ return (count);
+}
diff --git a/3rdparty/pthreads-win32/pthread_once.c b/3rdparty/pthreads-win32/pthread_once.c
new file mode 100644
index 0000000..3bfeb9f
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_once.c
@@ -0,0 +1,79 @@
+/*
+ * pthread_once.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_once (pthread_once_t * once_control, void (PTW32_CDECL *init_routine) (void))
+{
+ if (once_control == NULL || init_routine == NULL)
+ {
+ return EINVAL;
+ }
+
+ if ((PTW32_INTERLOCKED_LONG)PTW32_FALSE ==
+ (PTW32_INTERLOCKED_LONG)PTW32_INTERLOCKED_EXCHANGE_ADD_LONG((PTW32_INTERLOCKED_LONGPTR)&once_control->done,
+ (PTW32_INTERLOCKED_LONG)0)) /* MBR fence */
+ {
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire((ptw32_mcs_lock_t *)&once_control->lock, &node);
+
+ if (!once_control->done)
+ {
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+
+ pthread_cleanup_push(ptw32_mcs_lock_release, &node);
+ (*init_routine)();
+ pthread_cleanup_pop(0);
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+
+ once_control->done = PTW32_TRUE;
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+
+ return 0;
+
+} /* pthread_once */
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_destroy.c b/3rdparty/pthreads-win32/pthread_rwlock_destroy.c
new file mode 100644
index 0000000..245a892
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_destroy.c
@@ -0,0 +1,143 @@
+/*
+ * pthread_rwlock_destroy.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_destroy (pthread_rwlock_t * rwlock)
+{
+ pthread_rwlock_t rwl;
+ int result = 0, result1 = 0, result2 = 0;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (*rwlock != PTHREAD_RWLOCK_INITIALIZER)
+ {
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result = pthread_mutex_lock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ if ((result =
+ pthread_mutex_lock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ /*
+ * Check whether any threads own/wait for the lock (wait for ex.access);
+ * report "BUSY" if so.
+ */
+ if (rwl->nExclusiveAccessCount > 0
+ || rwl->nSharedAccessCount > rwl->nCompletedSharedAccessCount)
+ {
+ result = pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted));
+ result1 = pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ result2 = EBUSY;
+ }
+ else
+ {
+ rwl->nMagic = 0;
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ pthread_mutex_unlock (&rwl->mtxExclusiveAccess);
+ return result;
+ }
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ *rwlock = NULL; /* Invalidate rwlock before anything else */
+ result = pthread_cond_destroy (&(rwl->cndSharedAccessCompleted));
+ result1 = pthread_mutex_destroy (&(rwl->mtxSharedAccessCompleted));
+ result2 = pthread_mutex_destroy (&(rwl->mtxExclusiveAccess));
+ (void) free (rwl);
+ }
+ }
+ else
+ {
+ ptw32_mcs_local_node_t node;
+ /*
+ * See notes in ptw32_rwlock_check_need_init() above also.
+ */
+ ptw32_mcs_lock_acquire(&ptw32_rwlock_test_init_lock, &node);
+
+ /*
+ * Check again.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ /*
+ * This is all we need to do to destroy a statically
+ * initialised rwlock that has not yet been used (initialised).
+ * If we get to here, another thread
+ * waiting to initialise this rwlock will get an EINVAL.
+ */
+ *rwlock = NULL;
+ }
+ else
+ {
+ /*
+ * The rwlock has been initialised while we were waiting
+ * so assume it's in use.
+ */
+ result = EBUSY;
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+
+ return ((result != 0) ? result : ((result1 != 0) ? result1 : result2));
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_init.c b/3rdparty/pthreads-win32/pthread_rwlock_init.c
new file mode 100644
index 0000000..597c1ff
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_init.c
@@ -0,0 +1,109 @@
+/*
+ * pthread_rwlock_init.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_init (pthread_rwlock_t * rwlock,
+ const pthread_rwlockattr_t * attr)
+{
+ int result;
+ pthread_rwlock_t rwl = 0;
+
+ if (rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (attr != NULL && *attr != NULL)
+ {
+ result = EINVAL; /* Not supported */
+ goto DONE;
+ }
+
+ rwl = (pthread_rwlock_t) calloc (1, sizeof (*rwl));
+
+ if (rwl == NULL)
+ {
+ result = ENOMEM;
+ goto DONE;
+ }
+
+ rwl->nSharedAccessCount = 0;
+ rwl->nExclusiveAccessCount = 0;
+ rwl->nCompletedSharedAccessCount = 0;
+
+ result = pthread_mutex_init (&rwl->mtxExclusiveAccess, NULL);
+ if (result != 0)
+ {
+ goto FAIL0;
+ }
+
+ result = pthread_mutex_init (&rwl->mtxSharedAccessCompleted, NULL);
+ if (result != 0)
+ {
+ goto FAIL1;
+ }
+
+ result = pthread_cond_init (&rwl->cndSharedAccessCompleted, NULL);
+ if (result != 0)
+ {
+ goto FAIL2;
+ }
+
+ rwl->nMagic = PTW32_RWLOCK_MAGIC;
+
+ result = 0;
+ goto DONE;
+
+FAIL2:
+ (void) pthread_mutex_destroy (&(rwl->mtxSharedAccessCompleted));
+
+FAIL1:
+ (void) pthread_mutex_destroy (&(rwl->mtxExclusiveAccess));
+
+FAIL0:
+ (void) free (rwl);
+ rwl = NULL;
+
+DONE:
+ *rwlock = rwl;
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_rdlock.c b/3rdparty/pthreads-win32/pthread_rwlock_rdlock.c
new file mode 100644
index 0000000..91e1808
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_rdlock.c
@@ -0,0 +1,102 @@
+/*
+ * pthread_rwlock_rdlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_rdlock (pthread_rwlock_t * rwlock)
+{
+ int result;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result = pthread_mutex_lock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ if (++rwl->nSharedAccessCount == INT_MAX)
+ {
+ if ((result =
+ pthread_mutex_lock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+ }
+
+ return (pthread_mutex_unlock (&(rwl->mtxExclusiveAccess)));
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_timedrdlock.c b/3rdparty/pthreads-win32/pthread_rwlock_timedrdlock.c
new file mode 100644
index 0000000..7133778
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_timedrdlock.c
@@ -0,0 +1,109 @@
+/*
+ * pthread_rwlock_timedrdlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_timedrdlock (pthread_rwlock_t * rwlock,
+ const struct timespec *abstime)
+{
+ int result;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result =
+ pthread_mutex_timedlock (&(rwl->mtxExclusiveAccess), abstime)) != 0)
+ {
+ return result;
+ }
+
+ if (++rwl->nSharedAccessCount == INT_MAX)
+ {
+ if ((result =
+ pthread_mutex_timedlock (&(rwl->mtxSharedAccessCompleted),
+ abstime)) != 0)
+ {
+ if (result == ETIMEDOUT)
+ {
+ ++rwl->nCompletedSharedAccessCount;
+ }
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+ }
+
+ return (pthread_mutex_unlock (&(rwl->mtxExclusiveAccess)));
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_timedwrlock.c b/3rdparty/pthreads-win32/pthread_rwlock_timedwrlock.c
new file mode 100644
index 0000000..8c111bb
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_timedwrlock.c
@@ -0,0 +1,139 @@
+/*
+ * pthread_rwlock_timedwrlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_timedwrlock (pthread_rwlock_t * rwlock,
+ const struct timespec *abstime)
+{
+ int result;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result =
+ pthread_mutex_timedlock (&(rwl->mtxExclusiveAccess), abstime)) != 0)
+ {
+ return result;
+ }
+
+ if ((result =
+ pthread_mutex_timedlock (&(rwl->mtxSharedAccessCompleted),
+ abstime)) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ if (rwl->nExclusiveAccessCount == 0)
+ {
+ if (rwl->nCompletedSharedAccessCount > 0)
+ {
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+ }
+
+ if (rwl->nSharedAccessCount > 0)
+ {
+ rwl->nCompletedSharedAccessCount = -rwl->nSharedAccessCount;
+
+ /*
+ * This routine may be a cancelation point
+ * according to POSIX 1003.1j section 18.1.2.
+ */
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+ pthread_cleanup_push (ptw32_rwlock_cancelwrwait, (void *) rwl);
+
+ do
+ {
+ result =
+ pthread_cond_timedwait (&(rwl->cndSharedAccessCompleted),
+ &(rwl->mtxSharedAccessCompleted),
+ abstime);
+ }
+ while (result == 0 && rwl->nCompletedSharedAccessCount < 0);
+
+ pthread_cleanup_pop ((result != 0) ? 1 : 0);
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+
+ if (result == 0)
+ {
+ rwl->nSharedAccessCount = 0;
+ }
+ }
+ }
+
+ if (result == 0)
+ {
+ rwl->nExclusiveAccessCount++;
+ }
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_tryrdlock.c b/3rdparty/pthreads-win32/pthread_rwlock_tryrdlock.c
new file mode 100644
index 0000000..0fc5458
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_tryrdlock.c
@@ -0,0 +1,102 @@
+/*
+ * pthread_rwlock_tryrdlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_tryrdlock (pthread_rwlock_t * rwlock)
+{
+ int result;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result = pthread_mutex_trylock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ if (++rwl->nSharedAccessCount == INT_MAX)
+ {
+ if ((result =
+ pthread_mutex_lock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+ }
+
+ return (pthread_mutex_unlock (&rwl->mtxExclusiveAccess));
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_trywrlock.c b/3rdparty/pthreads-win32/pthread_rwlock_trywrlock.c
new file mode 100644
index 0000000..9997c5d
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_trywrlock.c
@@ -0,0 +1,122 @@
+/*
+ * pthread_rwlock_trywrlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_trywrlock (pthread_rwlock_t * rwlock)
+{
+ int result, result1;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result = pthread_mutex_trylock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ if ((result =
+ pthread_mutex_trylock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ result1 = pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return ((result1 != 0) ? result1 : result);
+ }
+
+ if (rwl->nExclusiveAccessCount == 0)
+ {
+ if (rwl->nCompletedSharedAccessCount > 0)
+ {
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+ }
+
+ if (rwl->nSharedAccessCount > 0)
+ {
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ if ((result =
+ pthread_mutex_unlock (&(rwl->mtxExclusiveAccess))) == 0)
+ {
+ result = EBUSY;
+ }
+ }
+ else
+ {
+ rwl->nExclusiveAccessCount = 1;
+ }
+ }
+ else
+ {
+ result = EBUSY;
+ }
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_unlock.c b/3rdparty/pthreads-win32/pthread_rwlock_unlock.c
new file mode 100644
index 0000000..d48d187
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_unlock.c
@@ -0,0 +1,93 @@
+/*
+ * pthread_rwlock_unlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_unlock (pthread_rwlock_t * rwlock)
+{
+ int result, result1;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return (EINVAL);
+ }
+
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ /*
+ * Assume any race condition here is harmless.
+ */
+ return 0;
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if (rwl->nExclusiveAccessCount == 0)
+ {
+ if ((result =
+ pthread_mutex_lock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ return result;
+ }
+
+ if (++rwl->nCompletedSharedAccessCount == 0)
+ {
+ result = pthread_cond_signal (&(rwl->cndSharedAccessCompleted));
+ }
+
+ result1 = pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted));
+ }
+ else
+ {
+ rwl->nExclusiveAccessCount--;
+
+ result = pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted));
+ result1 = pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+
+ }
+
+ return ((result != 0) ? result : result1);
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlock_wrlock.c b/3rdparty/pthreads-win32/pthread_rwlock_wrlock.c
new file mode 100644
index 0000000..e8b4fbb
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlock_wrlock.c
@@ -0,0 +1,133 @@
+/*
+ * pthread_rwlock_wrlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlock_wrlock (pthread_rwlock_t * rwlock)
+{
+ int result;
+ pthread_rwlock_t rwl;
+
+ if (rwlock == NULL || *rwlock == NULL)
+ {
+ return EINVAL;
+ }
+
+ /*
+ * We do a quick check to see if we need to do more work
+ * to initialise a static rwlock. We check
+ * again inside the guarded section of ptw32_rwlock_check_need_init()
+ * to avoid race conditions.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = ptw32_rwlock_check_need_init (rwlock);
+
+ if (result != 0 && result != EBUSY)
+ {
+ return result;
+ }
+ }
+
+ rwl = *rwlock;
+
+ if (rwl->nMagic != PTW32_RWLOCK_MAGIC)
+ {
+ return EINVAL;
+ }
+
+ if ((result = pthread_mutex_lock (&(rwl->mtxExclusiveAccess))) != 0)
+ {
+ return result;
+ }
+
+ if ((result = pthread_mutex_lock (&(rwl->mtxSharedAccessCompleted))) != 0)
+ {
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+ return result;
+ }
+
+ if (rwl->nExclusiveAccessCount == 0)
+ {
+ if (rwl->nCompletedSharedAccessCount > 0)
+ {
+ rwl->nSharedAccessCount -= rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+ }
+
+ if (rwl->nSharedAccessCount > 0)
+ {
+ rwl->nCompletedSharedAccessCount = -rwl->nSharedAccessCount;
+
+ /*
+ * This routine may be a cancelation point
+ * according to POSIX 1003.1j section 18.1.2.
+ */
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+ pthread_cleanup_push (ptw32_rwlock_cancelwrwait, (void *) rwl);
+
+ do
+ {
+ result = pthread_cond_wait (&(rwl->cndSharedAccessCompleted),
+ &(rwl->mtxSharedAccessCompleted));
+ }
+ while (result == 0 && rwl->nCompletedSharedAccessCount < 0);
+
+ pthread_cleanup_pop ((result != 0) ? 1 : 0);
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+
+ if (result == 0)
+ {
+ rwl->nSharedAccessCount = 0;
+ }
+ }
+ }
+
+ if (result == 0)
+ {
+ rwl->nExclusiveAccessCount++;
+ }
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_rwlockattr_destroy.c b/3rdparty/pthreads-win32/pthread_rwlockattr_destroy.c
new file mode 100644
index 0000000..868e727
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlockattr_destroy.c
@@ -0,0 +1,84 @@
+/*
+ * pthread_rwlockattr_destroy.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlockattr_destroy (pthread_rwlockattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Destroys a rwlock attributes object. The object can
+ * no longer be used.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_rwlockattr_t
+ *
+ *
+ * DESCRIPTION
+ * Destroys a rwlock attributes object. The object can
+ * no longer be used.
+ *
+ * NOTES:
+ * 1) Does not affect rwlockss created using 'attr'
+ *
+ * RESULTS
+ * 0 successfully released attr,
+ * EINVAL 'attr' is invalid.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+
+ if (attr == NULL || *attr == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ pthread_rwlockattr_t rwa = *attr;
+
+ *attr = NULL;
+ free (rwa);
+ }
+
+ return (result);
+} /* pthread_rwlockattr_destroy */
diff --git a/3rdparty/pthreads-win32/pthread_rwlockattr_getpshared.c b/3rdparty/pthreads-win32/pthread_rwlockattr_getpshared.c
new file mode 100644
index 0000000..eeace20
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlockattr_getpshared.c
@@ -0,0 +1,97 @@
+/*
+ * pthread_rwlockattr_getpshared.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlockattr_getpshared (const pthread_rwlockattr_t * attr,
+ int *pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Determine whether rwlocks created with 'attr' can be
+ * shared between processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_rwlockattr_t
+ *
+ * pshared
+ * will be set to one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ *
+ * DESCRIPTION
+ * Rwlocks creatd with 'attr' can be shared between
+ * processes if pthread_rwlock_t variable is allocated
+ * in memory shared by these processes.
+ * NOTES:
+ * 1) pshared rwlocks MUST be allocated in shared
+ * memory.
+ * 2) The following macro is defined if shared rwlocks
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully retrieved attribute,
+ * EINVAL 'attr' is invalid,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) && (pshared != NULL))
+ {
+ *pshared = (*attr)->pshared;
+ result = 0;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+
+} /* pthread_rwlockattr_getpshared */
diff --git a/3rdparty/pthreads-win32/pthread_rwlockattr_init.c b/3rdparty/pthreads-win32/pthread_rwlockattr_init.c
new file mode 100644
index 0000000..a2d2b94
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlockattr_init.c
@@ -0,0 +1,83 @@
+/*
+ * pthread_rwlockattr_init.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlockattr_init (pthread_rwlockattr_t * attr)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Initializes a rwlock attributes object with default
+ * attributes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_rwlockattr_t
+ *
+ *
+ * DESCRIPTION
+ * Initializes a rwlock attributes object with default
+ * attributes.
+ *
+ * RESULTS
+ * 0 successfully initialized attr,
+ * ENOMEM insufficient memory for attr.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ pthread_rwlockattr_t rwa;
+
+ rwa = (pthread_rwlockattr_t) calloc (1, sizeof (*rwa));
+
+ if (rwa == NULL)
+ {
+ result = ENOMEM;
+ }
+ else
+ {
+ rwa->pshared = PTHREAD_PROCESS_PRIVATE;
+ }
+
+ *attr = rwa;
+
+ return (result);
+} /* pthread_rwlockattr_init */
diff --git a/3rdparty/pthreads-win32/pthread_rwlockattr_setpshared.c b/3rdparty/pthreads-win32/pthread_rwlockattr_setpshared.c
new file mode 100644
index 0000000..a83dd70
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_rwlockattr_setpshared.c
@@ -0,0 +1,120 @@
+/*
+ * pthread_rwlockattr_setpshared.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+pthread_rwlockattr_setpshared (pthread_rwlockattr_t * attr, int pshared)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Rwlocks created with 'attr' can be shared between
+ * processes if pthread_rwlock_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * PARAMETERS
+ * attr
+ * pointer to an instance of pthread_rwlockattr_t
+ *
+ * pshared
+ * must be one of:
+ *
+ * PTHREAD_PROCESS_SHARED
+ * May be shared if in shared memory
+ *
+ * PTHREAD_PROCESS_PRIVATE
+ * Cannot be shared.
+ *
+ * DESCRIPTION
+ * Rwlocks creatd with 'attr' can be shared between
+ * processes if pthread_rwlock_t variable is allocated
+ * in memory shared by these processes.
+ *
+ * NOTES:
+ * 1) pshared rwlocks MUST be allocated in shared
+ * memory.
+ *
+ * 2) The following macro is defined if shared rwlocks
+ * are supported:
+ * _POSIX_THREAD_PROCESS_SHARED
+ *
+ * RESULTS
+ * 0 successfully set attribute,
+ * EINVAL 'attr' or pshared is invalid,
+ * ENOSYS PTHREAD_PROCESS_SHARED not supported,
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result;
+
+ if ((attr != NULL && *attr != NULL) &&
+ ((pshared == PTHREAD_PROCESS_SHARED) ||
+ (pshared == PTHREAD_PROCESS_PRIVATE)))
+ {
+ if (pshared == PTHREAD_PROCESS_SHARED)
+ {
+
+#if !defined( _POSIX_THREAD_PROCESS_SHARED )
+
+ result = ENOSYS;
+ pshared = PTHREAD_PROCESS_PRIVATE;
+
+#else
+
+ result = 0;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+
+ }
+ else
+ {
+ result = 0;
+ }
+
+ (*attr)->pshared = pshared;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+
+ return (result);
+
+} /* pthread_rwlockattr_setpshared */
diff --git a/3rdparty/pthreads-win32/pthread_self.c b/3rdparty/pthreads-win32/pthread_self.c
new file mode 100644
index 0000000..9a1765f
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_self.c
@@ -0,0 +1,141 @@
+/*
+ * pthread_self.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+pthread_t
+pthread_self (void)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function returns a reference to the current running
+ * thread.
+ *
+ * PARAMETERS
+ * N/A
+ *
+ *
+ * DESCRIPTION
+ * This function returns a reference to the current running
+ * thread.
+ *
+ * RESULTS
+ * pthread_t reference to the current thread
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_t self;
+ pthread_t nil = {NULL, 0};
+ ptw32_thread_t * sp;
+
+#if defined(_UWIN)
+ if (!ptw32_selfThreadKey)
+ return nil;
+#endif
+
+ sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+ if (sp != NULL)
+ {
+ self = sp->ptHandle;
+ }
+ else
+ {
+ /*
+ * Need to create an implicit 'self' for the currently
+ * executing thread.
+ */
+ self = ptw32_new ();
+ sp = (ptw32_thread_t *) self.p;
+
+ if (sp != NULL)
+ {
+ /*
+ * This is a non-POSIX thread which has chosen to call
+ * a POSIX threads function for some reason. We assume that
+ * it isn't joinable, but we do assume that it's
+ * (deferred) cancelable.
+ */
+ sp->implicit = 1;
+ sp->detachState = PTHREAD_CREATE_DETACHED;
+ sp->thread = GetCurrentThreadId ();
+
+#if defined(NEED_DUPLICATEHANDLE)
+ /*
+ * DuplicateHandle does not exist on WinCE.
+ *
+ * NOTE:
+ * GetCurrentThread only returns a pseudo-handle
+ * which is only valid in the current thread context.
+ * Therefore, you should not pass the handle to
+ * other threads for whatever purpose.
+ */
+ sp->threadH = GetCurrentThread ();
+#else
+ if (!DuplicateHandle (GetCurrentProcess (),
+ GetCurrentThread (),
+ GetCurrentProcess (),
+ &sp->threadH,
+ 0, FALSE, DUPLICATE_SAME_ACCESS))
+ {
+ /*
+ * Should not do this, but we have no alternative if
+ * we can't get a Win32 thread handle.
+ * Thread structs are never freed.
+ */
+ ptw32_threadReusePush (self);
+ /*
+ * As this is a win32 thread calling us and we have failed,
+ * return a value that makes sense to win32.
+ */
+ return nil;
+ }
+#endif
+
+ /*
+ * No need to explicitly serialise access to sched_priority
+ * because the new handle is not yet public.
+ */
+ sp->sched_priority = GetThreadPriority (sp->threadH);
+ pthread_setspecific (ptw32_selfThreadKey, (void *) sp);
+ }
+ }
+
+ return (self);
+
+} /* pthread_self */
diff --git a/3rdparty/pthreads-win32/pthread_setcancelstate.c b/3rdparty/pthreads-win32/pthread_setcancelstate.c
new file mode 100644
index 0000000..bbcd624
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_setcancelstate.c
@@ -0,0 +1,125 @@
+/*
+ * pthread_setcancelstate.c
+ *
+ * Description:
+ * POSIX thread functions related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_setcancelstate (int state, int *oldstate)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function atomically sets the calling thread's
+ * cancelability state to 'state' and returns the previous
+ * cancelability state at the location referenced by
+ * 'oldstate'
+ *
+ * PARAMETERS
+ * state,
+ * oldstate
+ * PTHREAD_CANCEL_ENABLE
+ * cancellation is enabled,
+ *
+ * PTHREAD_CANCEL_DISABLE
+ * cancellation is disabled
+ *
+ *
+ * DESCRIPTION
+ * This function atomically sets the calling thread's
+ * cancelability state to 'state' and returns the previous
+ * cancelability state at the location referenced by
+ * 'oldstate'.
+ *
+ * NOTES:
+ * 1) Use to disable cancellation around 'atomic' code that
+ * includes cancellation points
+ *
+ * COMPATIBILITY ADDITIONS
+ * If 'oldstate' is NULL then the previous state is not returned
+ * but the function still succeeds. (Solaris)
+ *
+ * RESULTS
+ * 0 successfully set cancelability type,
+ * EINVAL 'state' is invalid
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_mcs_local_node_t stateLock;
+ int result = 0;
+ pthread_t self = pthread_self ();
+ ptw32_thread_t * sp = (ptw32_thread_t *) self.p;
+
+ if (sp == NULL
+ || (state != PTHREAD_CANCEL_ENABLE && state != PTHREAD_CANCEL_DISABLE))
+ {
+ return EINVAL;
+ }
+
+ /*
+ * Lock for async-cancel safety.
+ */
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+
+ if (oldstate != NULL)
+ {
+ *oldstate = sp->cancelState;
+ }
+
+ sp->cancelState = state;
+
+ /*
+ * Check if there is a pending asynchronous cancel
+ */
+ if (state == PTHREAD_CANCEL_ENABLE
+ && sp->cancelType == PTHREAD_CANCEL_ASYNCHRONOUS
+ && WaitForSingleObject (sp->cancelEvent, 0) == WAIT_OBJECT_0)
+ {
+ sp->state = PThreadStateCanceling;
+ sp->cancelState = PTHREAD_CANCEL_DISABLE;
+ ResetEvent (sp->cancelEvent);
+ ptw32_mcs_lock_release (&stateLock);
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+
+ return (result);
+
+} /* pthread_setcancelstate */
diff --git a/3rdparty/pthreads-win32/pthread_setcanceltype.c b/3rdparty/pthreads-win32/pthread_setcanceltype.c
new file mode 100644
index 0000000..72b0af5
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_setcanceltype.c
@@ -0,0 +1,126 @@
+/*
+ * pthread_setcanceltype.c
+ *
+ * Description:
+ * POSIX thread functions related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_setcanceltype (int type, int *oldtype)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function atomically sets the calling thread's
+ * cancelability type to 'type' and returns the previous
+ * cancelability type at the location referenced by
+ * 'oldtype'
+ *
+ * PARAMETERS
+ * type,
+ * oldtype
+ * PTHREAD_CANCEL_DEFERRED
+ * only deferred cancelation is allowed,
+ *
+ * PTHREAD_CANCEL_ASYNCHRONOUS
+ * Asynchronous cancellation is allowed
+ *
+ *
+ * DESCRIPTION
+ * This function atomically sets the calling thread's
+ * cancelability type to 'type' and returns the previous
+ * cancelability type at the location referenced by
+ * 'oldtype'
+ *
+ * NOTES:
+ * 1) Use with caution; most code is not safe for use
+ * with asynchronous cancelability.
+ *
+ * COMPATIBILITY ADDITIONS
+ * If 'oldtype' is NULL then the previous type is not returned
+ * but the function still succeeds. (Solaris)
+ *
+ * RESULTS
+ * 0 successfully set cancelability type,
+ * EINVAL 'type' is invalid
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_mcs_local_node_t stateLock;
+ int result = 0;
+ pthread_t self = pthread_self ();
+ ptw32_thread_t * sp = (ptw32_thread_t *) self.p;
+
+ if (sp == NULL
+ || (type != PTHREAD_CANCEL_DEFERRED
+ && type != PTHREAD_CANCEL_ASYNCHRONOUS))
+ {
+ return EINVAL;
+ }
+
+ /*
+ * Lock for async-cancel safety.
+ */
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+
+ if (oldtype != NULL)
+ {
+ *oldtype = sp->cancelType;
+ }
+
+ sp->cancelType = type;
+
+ /*
+ * Check if there is a pending asynchronous cancel
+ */
+ if (sp->cancelState == PTHREAD_CANCEL_ENABLE
+ && type == PTHREAD_CANCEL_ASYNCHRONOUS
+ && WaitForSingleObject (sp->cancelEvent, 0) == WAIT_OBJECT_0)
+ {
+ sp->state = PThreadStateCanceling;
+ sp->cancelState = PTHREAD_CANCEL_DISABLE;
+ ResetEvent (sp->cancelEvent);
+ ptw32_mcs_lock_release (&stateLock);
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+
+ return (result);
+
+} /* pthread_setcanceltype */
diff --git a/3rdparty/pthreads-win32/pthread_setconcurrency.c b/3rdparty/pthreads-win32/pthread_setconcurrency.c
new file mode 100644
index 0000000..f62346f
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_setconcurrency.c
@@ -0,0 +1,53 @@
+/*
+ * pthread_setconcurrency.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_setconcurrency (int level)
+{
+ if (level < 0)
+ {
+ return EINVAL;
+ }
+ else
+ {
+ ptw32_concurrency = level;
+ return 0;
+ }
+}
diff --git a/3rdparty/pthreads-win32/pthread_setschedparam.c b/3rdparty/pthreads-win32/pthread_setschedparam.c
new file mode 100644
index 0000000..b762753
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_setschedparam.c
@@ -0,0 +1,123 @@
+/*
+ * sched_setschedparam.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+pthread_setschedparam (pthread_t thread, int policy,
+ const struct sched_param *param)
+{
+ int result;
+
+ /* Validate the thread id. */
+ result = pthread_kill (thread, 0);
+ if (0 != result)
+ {
+ return result;
+ }
+
+ /* Validate the scheduling policy. */
+ if (policy < SCHED_MIN || policy > SCHED_MAX)
+ {
+ return EINVAL;
+ }
+
+ /* Ensure the policy is SCHED_OTHER. */
+ if (policy != SCHED_OTHER)
+ {
+ return ENOTSUP;
+ }
+
+ return (ptw32_setthreadpriority (thread, policy, param->sched_priority));
+}
+
+
+int
+ptw32_setthreadpriority (pthread_t thread, int policy, int priority)
+{
+ int prio;
+ ptw32_mcs_local_node_t threadLock;
+ int result = 0;
+ ptw32_thread_t * tp = (ptw32_thread_t *) thread.p;
+
+ prio = priority;
+
+ /* Validate priority level. */
+ if (prio < sched_get_priority_min (policy) ||
+ prio > sched_get_priority_max (policy))
+ {
+ return EINVAL;
+ }
+
+#if (THREAD_PRIORITY_LOWEST > THREAD_PRIORITY_NORMAL)
+/* WinCE */
+#else
+/* Everything else */
+
+ if (THREAD_PRIORITY_IDLE < prio && THREAD_PRIORITY_LOWEST > prio)
+ {
+ prio = THREAD_PRIORITY_LOWEST;
+ }
+ else if (THREAD_PRIORITY_TIME_CRITICAL > prio
+ && THREAD_PRIORITY_HIGHEST < prio)
+ {
+ prio = THREAD_PRIORITY_HIGHEST;
+ }
+
+#endif
+
+ ptw32_mcs_lock_acquire (&tp->threadLock, &threadLock);
+
+ /* If this fails, the current priority is unchanged. */
+ if (0 == SetThreadPriority (tp->threadH, prio))
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ /*
+ * Must record the thread's sched_priority as given,
+ * not as finally adjusted.
+ */
+ tp->sched_priority = priority;
+ }
+
+ ptw32_mcs_lock_release (&threadLock);
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/pthread_setspecific.c b/3rdparty/pthreads-win32/pthread_setspecific.c
new file mode 100644
index 0000000..0f29e70
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_setspecific.c
@@ -0,0 +1,167 @@
+/*
+ * pthread_setspecific.c
+ *
+ * Description:
+ * POSIX thread functions which implement thread-specific data (TSD).
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_setspecific (pthread_key_t key, const void *value)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function sets the value of the thread specific
+ * key in the calling thread.
+ *
+ * PARAMETERS
+ * key
+ * an instance of pthread_key_t
+ * value
+ * the value to set key to
+ *
+ *
+ * DESCRIPTION
+ * This function sets the value of the thread specific
+ * key in the calling thread.
+ *
+ * RESULTS
+ * 0 successfully set value
+ * EAGAIN could not set value
+ * ENOENT SERIOUS!!
+ *
+ * ------------------------------------------------------
+ */
+{
+ pthread_t self;
+ int result = 0;
+
+ if (key != ptw32_selfThreadKey)
+ {
+ /*
+ * Using pthread_self will implicitly create
+ * an instance of pthread_t for the current
+ * thread if one wasn't explicitly created
+ */
+ self = pthread_self ();
+ if (self.p == NULL)
+ {
+ return ENOENT;
+ }
+ }
+ else
+ {
+ /*
+ * Resolve catch-22 of registering thread with selfThread
+ * key
+ */
+ ptw32_thread_t * sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+ if (sp == NULL)
+ {
+ if (value == NULL)
+ {
+ return ENOENT;
+ }
+ self = *((pthread_t *) value);
+ }
+ else
+ {
+ self = sp->ptHandle;
+ }
+ }
+
+ result = 0;
+
+ if (key != NULL)
+ {
+ if (self.p != NULL && key->destructor != NULL && value != NULL)
+ {
+ ptw32_mcs_local_node_t keyLock;
+ ptw32_mcs_local_node_t threadLock;
+ ptw32_thread_t * sp = (ptw32_thread_t *) self.p;
+ /*
+ * Only require associations if we have to
+ * call user destroy routine.
+ * Don't need to locate an existing association
+ * when setting data to NULL for WIN32 since the
+ * data is stored with the operating system; not
+ * on the association; setting assoc to NULL short
+ * circuits the search.
+ */
+ ThreadKeyAssoc *assoc;
+
+ ptw32_mcs_lock_acquire(&(key->keyLock), &keyLock);
+ ptw32_mcs_lock_acquire(&(sp->threadLock), &threadLock);
+
+ assoc = (ThreadKeyAssoc *) sp->keys;
+ /*
+ * Locate existing association
+ */
+ while (assoc != NULL)
+ {
+ if (assoc->key == key)
+ {
+ /*
+ * Association already exists
+ */
+ break;
+ }
+ assoc = assoc->nextKey;
+ }
+
+ /*
+ * create an association if not found
+ */
+ if (assoc == NULL)
+ {
+ result = ptw32_tkAssocCreate (sp, key);
+ }
+
+ ptw32_mcs_lock_release(&threadLock);
+ ptw32_mcs_lock_release(&keyLock);
+ }
+
+ if (result == 0)
+ {
+ if (!TlsSetValue (key->key, (LPVOID) value))
+ {
+ result = EAGAIN;
+ }
+ }
+ }
+
+ return (result);
+} /* pthread_setspecific */
diff --git a/3rdparty/pthreads-win32/pthread_spin_destroy.c b/3rdparty/pthreads-win32/pthread_spin_destroy.c
new file mode 100644
index 0000000..786c4e3
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_spin_destroy.c
@@ -0,0 +1,111 @@
+/*
+ * pthread_spin_destroy.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_spin_destroy (pthread_spinlock_t * lock)
+{
+ register pthread_spinlock_t s;
+ int result = 0;
+
+ if (lock == NULL || *lock == NULL)
+ {
+ return EINVAL;
+ }
+
+ if ((s = *lock) != PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ if (s->interlock == PTW32_SPIN_USE_MUTEX)
+ {
+ result = pthread_mutex_destroy (&(s->u.mutex));
+ }
+ else if ((PTW32_INTERLOCKED_LONG) PTW32_SPIN_UNLOCKED !=
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR) &s->interlock,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_INVALID,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_UNLOCKED))
+ {
+ result = EINVAL;
+ }
+
+ if (0 == result)
+ {
+ /*
+ * We are relying on the application to ensure that all other threads
+ * have finished with the spinlock before destroying it.
+ */
+ *lock = NULL;
+ (void) free (s);
+ }
+ }
+ else
+ {
+ /*
+ * See notes in ptw32_spinlock_check_need_init() above also.
+ */
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_spinlock_test_init_lock, &node);
+
+ /*
+ * Check again.
+ */
+ if (*lock == PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ /*
+ * This is all we need to do to destroy a statically
+ * initialised spinlock that has not yet been used (initialised).
+ * If we get to here, another thread
+ * waiting to initialise this mutex will get an EINVAL.
+ */
+ *lock = NULL;
+ }
+ else
+ {
+ /*
+ * The spinlock has been initialised while we were waiting
+ * so assume it's in use.
+ */
+ result = EBUSY;
+ }
+
+ ptw32_mcs_lock_release(&node);
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_spin_init.c b/3rdparty/pthreads-win32/pthread_spin_init.c
new file mode 100644
index 0000000..553af7e
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_spin_init.c
@@ -0,0 +1,123 @@
+/*
+ * pthread_spin_init.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_spin_init (pthread_spinlock_t * lock, int pshared)
+{
+ pthread_spinlock_t s;
+ int cpus = 0;
+ int result = 0;
+
+ if (lock == NULL)
+ {
+ return EINVAL;
+ }
+
+ if (0 != ptw32_getprocessors (&cpus))
+ {
+ cpus = 1;
+ }
+
+ if (cpus > 1)
+ {
+ if (pshared == PTHREAD_PROCESS_SHARED)
+ {
+ /*
+ * Creating spinlock that can be shared between
+ * processes.
+ */
+#if _POSIX_THREAD_PROCESS_SHARED >= 0
+
+ /*
+ * Not implemented yet.
+ */
+
+#error ERROR [__FILE__, line __LINE__]: Process shared spin locks are not supported yet.
+
+#else
+
+ return ENOSYS;
+
+#endif /* _POSIX_THREAD_PROCESS_SHARED */
+
+ }
+ }
+
+ s = (pthread_spinlock_t) calloc (1, sizeof (*s));
+
+ if (s == NULL)
+ {
+ return ENOMEM;
+ }
+
+ if (cpus > 1)
+ {
+ s->u.cpus = cpus;
+ s->interlock = PTW32_SPIN_UNLOCKED;
+ }
+ else
+ {
+ pthread_mutexattr_t ma;
+ result = pthread_mutexattr_init (&ma);
+
+ if (0 == result)
+ {
+ ma->pshared = pshared;
+ result = pthread_mutex_init (&(s->u.mutex), &ma);
+ if (0 == result)
+ {
+ s->interlock = PTW32_SPIN_USE_MUTEX;
+ }
+ }
+ (void) pthread_mutexattr_destroy (&ma);
+ }
+
+ if (0 == result)
+ {
+ *lock = s;
+ }
+ else
+ {
+ (void) free (s);
+ *lock = NULL;
+ }
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/pthread_spin_lock.c b/3rdparty/pthreads-win32/pthread_spin_lock.c
new file mode 100644
index 0000000..b560e14
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_spin_lock.c
@@ -0,0 +1,80 @@
+/*
+ * pthread_spin_lock.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_spin_lock (pthread_spinlock_t * lock)
+{
+ register pthread_spinlock_t s;
+
+ if (NULL == lock || NULL == *lock)
+ {
+ return (EINVAL);
+ }
+
+ if (*lock == PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ int result;
+
+ if ((result = ptw32_spinlock_check_need_init (lock)) != 0)
+ {
+ return (result);
+ }
+ }
+
+ s = *lock;
+
+ while ((PTW32_INTERLOCKED_LONG) PTW32_SPIN_LOCKED ==
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR) &s->interlock,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_LOCKED,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_UNLOCKED))
+ {
+ }
+
+ if (s->interlock == PTW32_SPIN_LOCKED)
+ {
+ return 0;
+ }
+ else if (s->interlock == PTW32_SPIN_USE_MUTEX)
+ {
+ return pthread_mutex_lock (&(s->u.mutex));
+ }
+
+ return EINVAL;
+}
diff --git a/3rdparty/pthreads-win32/pthread_spin_trylock.c b/3rdparty/pthreads-win32/pthread_spin_trylock.c
new file mode 100644
index 0000000..a6c65af
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_spin_trylock.c
@@ -0,0 +1,77 @@
+/*
+ * pthread_spin_trylock.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_spin_trylock (pthread_spinlock_t * lock)
+{
+ register pthread_spinlock_t s;
+
+ if (NULL == lock || NULL == *lock)
+ {
+ return (EINVAL);
+ }
+
+ if (*lock == PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ int result;
+
+ if ((result = ptw32_spinlock_check_need_init (lock)) != 0)
+ {
+ return (result);
+ }
+ }
+
+ s = *lock;
+
+ switch ((long)
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR) &s->interlock,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_LOCKED,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_UNLOCKED))
+ {
+ case PTW32_SPIN_UNLOCKED:
+ return 0;
+ case PTW32_SPIN_LOCKED:
+ return EBUSY;
+ case PTW32_SPIN_USE_MUTEX:
+ return pthread_mutex_trylock (&(s->u.mutex));
+ }
+
+ return EINVAL;
+}
diff --git a/3rdparty/pthreads-win32/pthread_spin_unlock.c b/3rdparty/pthreads-win32/pthread_spin_unlock.c
new file mode 100644
index 0000000..3a6932a
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_spin_unlock.c
@@ -0,0 +1,71 @@
+/*
+ * pthread_spin_unlock.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+pthread_spin_unlock (pthread_spinlock_t * lock)
+{
+ register pthread_spinlock_t s;
+
+ if (NULL == lock || NULL == *lock)
+ {
+ return (EINVAL);
+ }
+
+ s = *lock;
+
+ if (s == PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ return EPERM;
+ }
+
+ switch ((long)
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_LONG ((PTW32_INTERLOCKED_LONGPTR) &s->interlock,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_UNLOCKED,
+ (PTW32_INTERLOCKED_LONG) PTW32_SPIN_LOCKED))
+ {
+ case PTW32_SPIN_LOCKED:
+ case PTW32_SPIN_UNLOCKED:
+ return 0;
+ case PTW32_SPIN_USE_MUTEX:
+ return pthread_mutex_unlock (&(s->u.mutex));
+ }
+
+ return EINVAL;
+}
diff --git a/3rdparty/pthreads-win32/pthread_testcancel.c b/3rdparty/pthreads-win32/pthread_testcancel.c
new file mode 100644
index 0000000..6658650
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_testcancel.c
@@ -0,0 +1,103 @@
+/*
+ * pthread_testcancel.c
+ *
+ * Description:
+ * POSIX thread functions related to thread cancellation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+void
+pthread_testcancel (void)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function creates a deferred cancellation point
+ * in the calling thread. The call has no effect if the
+ * current cancelability state is
+ * PTHREAD_CANCEL_DISABLE
+ *
+ * PARAMETERS
+ * N/A
+ *
+ *
+ * DESCRIPTION
+ * This function creates a deferred cancellation point
+ * in the calling thread. The call has no effect if the
+ * current cancelability state is
+ * PTHREAD_CANCEL_DISABLE
+ *
+ * NOTES:
+ * 1) Cancellation is asynchronous. Use pthread_join
+ * to wait for termination of thread if necessary
+ *
+ * RESULTS
+ * N/A
+ *
+ * ------------------------------------------------------
+ */
+{
+ ptw32_mcs_local_node_t stateLock;
+ pthread_t self = pthread_self ();
+ ptw32_thread_t * sp = (ptw32_thread_t *) self.p;
+
+ if (sp == NULL)
+ {
+ return;
+ }
+
+ /*
+ * Pthread_cancel() will have set sp->state to PThreadStateCancelPending
+ * and set an event, so no need to enter kernel space if
+ * sp->state != PThreadStateCancelPending - that only slows us down.
+ */
+ if (sp->state != PThreadStateCancelPending)
+ {
+ return;
+ }
+
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+
+ if (sp->cancelState != PTHREAD_CANCEL_DISABLE)
+ {
+ ResetEvent(sp->cancelEvent);
+ sp->state = PThreadStateCanceling;
+ sp->cancelState = PTHREAD_CANCEL_DISABLE;
+ ptw32_mcs_lock_release (&stateLock);
+ ptw32_throw (PTW32_EPS_CANCEL);
+ /* Never returns here */
+ }
+
+ ptw32_mcs_lock_release (&stateLock);
+} /* pthread_testcancel */
diff --git a/3rdparty/pthreads-win32/pthread_timechange_handler_np.c b/3rdparty/pthreads-win32/pthread_timechange_handler_np.c
new file mode 100644
index 0000000..0f97e74
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_timechange_handler_np.c
@@ -0,0 +1,108 @@
+/*
+ * pthread_timechange_handler_np.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Notes on handling system time adjustments (especially negative ones).
+ * ---------------------------------------------------------------------
+ *
+ * This solution was suggested by Alexander Terekhov, but any errors
+ * in the implementation are mine - [Ross Johnson]
+ *
+ * 1) The problem: threads doing a timedwait on a CV may expect to timeout
+ * at a specific absolute time according to a system timer. If the
+ * system clock is adjusted backwards then those threads sleep longer than
+ * expected. Also, pthreads-win32 converts absolute times to intervals in
+ * order to make use of the underlying Win32, and so waiting threads may
+ * awake before their proper abstimes.
+ *
+ * 2) We aren't able to distinquish between threads on timed or untimed waits,
+ * so we wake them all at the time of the adjustment so that they can
+ * re-evaluate their conditions and re-compute their timeouts.
+ *
+ * 3) We rely on correctly written applications for this to work. Specifically,
+ * they must be able to deal properly with spurious wakeups. That is,
+ * they must re-test their condition upon wakeup and wait again if
+ * the condition is not satisfied.
+ */
+
+void *
+pthread_timechange_handler_np (void *arg)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * Broadcasts all CVs to force re-evaluation and
+ * new timeouts if required.
+ *
+ * PARAMETERS
+ * NONE
+ *
+ *
+ * DESCRIPTION
+ * Broadcasts all CVs to force re-evaluation and
+ * new timeouts if required.
+ *
+ * This routine may be passed directly to pthread_create()
+ * as a new thread in order to run asynchronously.
+ *
+ *
+ * RESULTS
+ * 0 successfully broadcast all CVs
+ * EAGAIN Not all CVs were broadcast
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ pthread_cond_t cv;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_cond_list_lock, &node);
+
+ cv = ptw32_cond_list_head;
+
+ while (cv != NULL && 0 == result)
+ {
+ result = pthread_cond_broadcast (&cv);
+ cv = cv->next;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return (void *) (size_t) (result != 0 ? EAGAIN : 0);
+}
diff --git a/3rdparty/pthreads-win32/pthread_win32_attach_detach_np.c b/3rdparty/pthreads-win32/pthread_win32_attach_detach_np.c
new file mode 100644
index 0000000..badb559
--- /dev/null
+++ b/3rdparty/pthreads-win32/pthread_win32_attach_detach_np.c
@@ -0,0 +1,258 @@
+/*
+ * pthread_win32_attach_detach_np.c
+ *
+ * Description:
+ * This translation unit implements non-portable thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+/*
+ * Handle to quserex.dll
+ */
+static HINSTANCE ptw32_h_quserex;
+
+BOOL
+pthread_win32_process_attach_np ()
+{
+ TCHAR QuserExDLLPathBuf[1024];
+ BOOL result = TRUE;
+
+ result = ptw32_processInitialize ();
+
+#if defined(_UWIN)
+ pthread_count++;
+#endif
+
+#if defined(__GNUC__)
+ ptw32_features = 0;
+#else
+ /*
+ * This is obsolete now.
+ */
+ ptw32_features = PTW32_SYSTEM_INTERLOCKED_COMPARE_EXCHANGE;
+#endif
+
+ /*
+ * Load QUSEREX.DLL and try to get address of QueueUserAPCEx.
+ * Because QUSEREX.DLL requires a driver to be installed we will
+ * assume the DLL is in the system directory.
+ *
+ * This should take care of any security issues.
+ */
+#if defined(__GNUC__) || _MSC_VER < 1400
+ if(GetSystemDirectory(QuserExDLLPathBuf, sizeof(QuserExDLLPathBuf)))
+ {
+ (void) strncat(QuserExDLLPathBuf,
+ "\\QUSEREX.DLL",
+ sizeof(QuserExDLLPathBuf) - strlen(QuserExDLLPathBuf) - 1);
+ ptw32_h_quserex = LoadLibrary(QuserExDLLPathBuf);
+ }
+#else
+# ifndef WINRT
+ /* strncat is secure - this is just to avoid a warning */
+ if(GetSystemDirectory(QuserExDLLPathBuf, sizeof(QuserExDLLPathBuf)) &&
+ 0 == strncat_s(QuserExDLLPathBuf, sizeof(QuserExDLLPathBuf), "\\QUSEREX.DLL", 12))
+ {
+ ptw32_h_quserex = LoadLibrary(QuserExDLLPathBuf);
+ }
+# endif
+#endif
+
+ if (ptw32_h_quserex != NULL)
+ {
+ ptw32_register_cancelation = (DWORD (*)(PAPCFUNC, HANDLE, DWORD))
+#if defined(NEED_UNICODE_CONSTS)
+ GetProcAddress (ptw32_h_quserex,
+ (const TCHAR *) TEXT ("QueueUserAPCEx"));
+#else
+ GetProcAddress (ptw32_h_quserex, (LPCSTR) "QueueUserAPCEx");
+#endif
+ }
+
+ if (NULL == ptw32_register_cancelation)
+ {
+ ptw32_register_cancelation = ptw32_RegisterCancelation;
+
+ if (ptw32_h_quserex != NULL)
+ {
+ (void) FreeLibrary (ptw32_h_quserex);
+ }
+ ptw32_h_quserex = 0;
+ }
+ else
+ {
+ /* Initialise QueueUserAPCEx */
+ BOOL (*queue_user_apc_ex_init) (VOID);
+
+ queue_user_apc_ex_init = (BOOL (*)(VOID))
+#if defined(NEED_UNICODE_CONSTS)
+ GetProcAddress (ptw32_h_quserex,
+ (const TCHAR *) TEXT ("QueueUserAPCEx_Init"));
+#else
+ GetProcAddress (ptw32_h_quserex, (LPCSTR) "QueueUserAPCEx_Init");
+#endif
+
+ if (queue_user_apc_ex_init == NULL || !queue_user_apc_ex_init ())
+ {
+ ptw32_register_cancelation = ptw32_RegisterCancelation;
+
+ (void) FreeLibrary (ptw32_h_quserex);
+ ptw32_h_quserex = 0;
+ }
+ }
+
+ if (ptw32_h_quserex)
+ {
+ ptw32_features |= PTW32_ALERTABLE_ASYNC_CANCEL;
+ }
+
+ return result;
+}
+
+
+BOOL
+pthread_win32_process_detach_np ()
+{
+ if (ptw32_processInitialized)
+ {
+ ptw32_thread_t * sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+ if (sp != NULL)
+ {
+ /*
+ * Detached threads have their resources automatically
+ * cleaned up upon exit (others must be 'joined').
+ */
+ if (sp->detachState == PTHREAD_CREATE_DETACHED)
+ {
+ ptw32_threadDestroy (sp->ptHandle);
+ TlsSetValue (ptw32_selfThreadKey->key, NULL);
+ }
+ }
+
+ /*
+ * The DLL is being unmapped from the process's address space
+ */
+ ptw32_processTerminate ();
+
+ if (ptw32_h_quserex)
+ {
+ /* Close QueueUserAPCEx */
+ BOOL (*queue_user_apc_ex_fini) (VOID);
+
+ queue_user_apc_ex_fini = (BOOL (*)(VOID))
+#if defined(NEED_UNICODE_CONSTS)
+ GetProcAddress (ptw32_h_quserex,
+ (const TCHAR *) TEXT ("QueueUserAPCEx_Fini"));
+#else
+ GetProcAddress (ptw32_h_quserex, (LPCSTR) "QueueUserAPCEx_Fini");
+#endif
+
+ if (queue_user_apc_ex_fini != NULL)
+ {
+ (void) queue_user_apc_ex_fini ();
+ }
+ (void) FreeLibrary (ptw32_h_quserex);
+ }
+ }
+
+ return TRUE;
+}
+
+BOOL
+pthread_win32_thread_attach_np ()
+{
+ return TRUE;
+}
+
+BOOL
+pthread_win32_thread_detach_np ()
+{
+ if (ptw32_processInitialized)
+ {
+ /*
+ * Don't use pthread_self() - to avoid creating an implicit POSIX thread handle
+ * unnecessarily.
+ */
+ ptw32_thread_t * sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+ if (sp != NULL) // otherwise Win32 thread with no implicit POSIX handle.
+ {
+ ptw32_mcs_local_node_t stateLock;
+ ptw32_callUserDestroyRoutines (sp->ptHandle);
+
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+ sp->state = PThreadStateLast;
+ /*
+ * If the thread is joinable at this point then it MUST be joined
+ * or detached explicitly by the application.
+ */
+ ptw32_mcs_lock_release (&stateLock);
+
+ /*
+ * Robust Mutexes
+ */
+ while (sp->robustMxList != NULL)
+ {
+ pthread_mutex_t mx = sp->robustMxList->mx;
+ ptw32_robust_mutex_remove(&mx, sp);
+ (void) PTW32_INTERLOCKED_EXCHANGE_LONG(
+ (PTW32_INTERLOCKED_LONGPTR)&mx->robustNode->stateInconsistent,
+ (PTW32_INTERLOCKED_LONG)-1);
+ /*
+ * If there are no waiters then the next thread to block will
+ * sleep, wakeup immediately and then go back to sleep.
+ * See pthread_mutex_lock.c.
+ */
+ SetEvent(mx->event);
+ }
+
+
+ if (sp->detachState == PTHREAD_CREATE_DETACHED)
+ {
+ ptw32_threadDestroy (sp->ptHandle);
+
+ TlsSetValue (ptw32_selfThreadKey->key, NULL);
+ }
+ }
+ }
+
+ return TRUE;
+}
+
+BOOL
+pthread_win32_test_features_np (int feature_mask)
+{
+ return ((ptw32_features & feature_mask) == feature_mask);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_MCS_lock.c b/3rdparty/pthreads-win32/ptw32_MCS_lock.c
new file mode 100644
index 0000000..cdb8b7e
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_MCS_lock.c
@@ -0,0 +1,290 @@
+/*
+ * ptw32_MCS_lock.c
+ *
+ * Description:
+ * This translation unit implements queue-based locks.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+/*
+ * About MCS locks:
+ *
+ * MCS locks are queue-based locks, where the queue nodes are local to the
+ * thread. The 'lock' is nothing more than a global pointer that points to
+ * the last node in the queue, or is NULL if the queue is empty.
+ *
+ * Originally designed for use as spin locks requiring no kernel resources
+ * for synchronisation or blocking, the implementation below has adapted
+ * the MCS spin lock for use as a general mutex that will suspend threads
+ * when there is lock contention.
+ *
+ * Because the queue nodes are thread-local, most of the memory read/write
+ * operations required to add or remove nodes from the queue do not trigger
+ * cache-coherence updates.
+ *
+ * Like 'named' mutexes, MCS locks consume system resources transiently -
+ * they are able to acquire and free resources automatically - but MCS
+ * locks do not require any unique 'name' to identify the lock to all
+ * threads using it.
+ *
+ * Usage of MCS locks:
+ *
+ * - you need a global ptw32_mcs_lock_t instance initialised to 0 or NULL.
+ * - you need a local thread-scope ptw32_mcs_local_node_t instance, which
+ * may serve several different locks but you need at least one node for
+ * every lock held concurrently by a thread.
+ *
+ * E.g.:
+ *
+ * ptw32_mcs_lock_t lock1 = 0;
+ * ptw32_mcs_lock_t lock2 = 0;
+ *
+ * void *mythread(void *arg)
+ * {
+ * ptw32_mcs_local_node_t node;
+ *
+ * ptw32_mcs_acquire (&lock1, &node);
+ * ptw32_mcs_lock_release (&node);
+ *
+ * ptw32_mcs_lock_acquire (&lock2, &node);
+ * ptw32_mcs_lock_release (&node);
+ * {
+ * ptw32_mcs_local_node_t nodex;
+ *
+ * ptw32_mcs_lock_acquire (&lock1, &node);
+ * ptw32_mcs_lock_acquire (&lock2, &nodex);
+ *
+ * ptw32_mcs_lock_release (&nodex);
+ * ptw32_mcs_lock_release (&node);
+ * }
+ * return (void *)0;
+ * }
+ */
+
+#include "pthread.h"
+#include "sched.h"
+#include "implement.h"
+
+/*
+ * ptw32_mcs_flag_set -- notify another thread about an event.
+ *
+ * Set event if an event handle has been stored in the flag, and
+ * set flag to -1 otherwise. Note that -1 cannot be a valid handle value.
+ */
+INLINE void
+ptw32_mcs_flag_set (HANDLE * flag)
+{
+ HANDLE e = (HANDLE)(PTW32_INTERLOCKED_SIZE)PTW32_INTERLOCKED_COMPARE_EXCHANGE_SIZE(
+ (PTW32_INTERLOCKED_SIZEPTR)flag,
+ (PTW32_INTERLOCKED_SIZE)-1,
+ (PTW32_INTERLOCKED_SIZE)0);
+ if ((HANDLE)0 != e)
+ {
+ /* another thread has already stored an event handle in the flag */
+ SetEvent(e);
+ }
+}
+
+/*
+ * ptw32_mcs_flag_set -- wait for notification from another.
+ *
+ * Store an event handle in the flag and wait on it if the flag has not been
+ * set, and proceed without creating an event otherwise.
+ */
+INLINE void
+ptw32_mcs_flag_wait (HANDLE * flag)
+{
+ if ((PTW32_INTERLOCKED_LONG)0 ==
+ PTW32_INTERLOCKED_EXCHANGE_ADD_SIZE((PTW32_INTERLOCKED_SIZEPTR)flag,
+ (PTW32_INTERLOCKED_SIZE)0)) /* MBR fence */
+ {
+ /* the flag is not set. create event. */
+
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ HANDLE e = CreateEventEx(NULL, NULL, 0, EVENT_ALL_ACCESS);
+# else
+ HANDLE e = CreateEvent(NULL, PTW32_FALSE, PTW32_FALSE, NULL);
+# endif
+#endif
+
+ if ((PTW32_INTERLOCKED_SIZE)0 == PTW32_INTERLOCKED_COMPARE_EXCHANGE_SIZE(
+ (PTW32_INTERLOCKED_SIZEPTR)flag,
+ (PTW32_INTERLOCKED_SIZE)e,
+ (PTW32_INTERLOCKED_SIZE)0))
+ {
+ /* stored handle in the flag. wait on it now. */
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ WaitForSingleObjectEx(e, INFINITE, FALSE);
+# else
+ WaitForSingleObject(e, INFINITE);
+# endif
+#endif
+ }
+
+ CloseHandle(e);
+ }
+}
+
+/*
+ * ptw32_mcs_lock_acquire -- acquire an MCS lock.
+ *
+ * See:
+ * J. M. Mellor-Crummey and M. L. Scott.
+ * Algorithms for Scalable Synchronization on Shared-Memory Multiprocessors.
+ * ACM Transactions on Computer Systems, 9(1):21-65, Feb. 1991.
+ */
+#if defined(PTW32_BUILD_INLINED)
+INLINE
+#endif /* PTW32_BUILD_INLINED */
+void
+ptw32_mcs_lock_acquire (ptw32_mcs_lock_t * lock, ptw32_mcs_local_node_t * node)
+{
+ ptw32_mcs_local_node_t *pred;
+
+ node->lock = lock;
+ node->nextFlag = 0;
+ node->readyFlag = 0;
+ node->next = 0; /* initially, no successor */
+
+ /* queue for the lock */
+ pred = (ptw32_mcs_local_node_t *)PTW32_INTERLOCKED_EXCHANGE_PTR((PTW32_INTERLOCKED_PVOID_PTR)lock,
+ (PTW32_INTERLOCKED_PVOID)node);
+
+ if (0 != pred)
+ {
+ /* the lock was not free. link behind predecessor. */
+ pred->next = node;
+ ptw32_mcs_flag_set(&pred->nextFlag);
+ ptw32_mcs_flag_wait(&node->readyFlag);
+ }
+}
+
+/*
+ * ptw32_mcs_lock_release -- release an MCS lock.
+ *
+ * See:
+ * J. M. Mellor-Crummey and M. L. Scott.
+ * Algorithms for Scalable Synchronization on Shared-Memory Multiprocessors.
+ * ACM Transactions on Computer Systems, 9(1):21-65, Feb. 1991.
+ */
+#if defined(PTW32_BUILD_INLINED)
+INLINE
+#endif /* PTW32_BUILD_INLINED */
+void
+ptw32_mcs_lock_release (ptw32_mcs_local_node_t * node)
+{
+ ptw32_mcs_lock_t *lock = node->lock;
+ ptw32_mcs_local_node_t *next =
+ (ptw32_mcs_local_node_t *)
+ PTW32_INTERLOCKED_EXCHANGE_ADD_SIZE((PTW32_INTERLOCKED_SIZEPTR)&node->next, (PTW32_INTERLOCKED_SIZE)0); /* MBR fence */
+
+ if (0 == next)
+ {
+ /* no known successor */
+
+ if (node == (ptw32_mcs_local_node_t *)
+ PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR((PTW32_INTERLOCKED_PVOID_PTR)lock,
+ (PTW32_INTERLOCKED_PVOID)0,
+ (PTW32_INTERLOCKED_PVOID)node))
+ {
+ /* no successor, lock is free now */
+ return;
+ }
+
+ /* A successor has started enqueueing behind us so wait for them to link to us */
+ ptw32_mcs_flag_wait(&node->nextFlag);
+ next = (ptw32_mcs_local_node_t *)
+ PTW32_INTERLOCKED_EXCHANGE_ADD_SIZE((PTW32_INTERLOCKED_SIZEPTR)&node->next, (PTW32_INTERLOCKED_SIZE)0); /* MBR fence */
+ }
+
+ /* pass the lock */
+ ptw32_mcs_flag_set(&next->readyFlag);
+}
+
+/*
+ * ptw32_mcs_lock_try_acquire
+ */
+#if defined(PTW32_BUILD_INLINED)
+INLINE
+#endif /* PTW32_BUILD_INLINED */
+int
+ptw32_mcs_lock_try_acquire (ptw32_mcs_lock_t * lock, ptw32_mcs_local_node_t * node)
+{
+ node->lock = lock;
+ node->nextFlag = 0;
+ node->readyFlag = 0;
+ node->next = 0; /* initially, no successor */
+
+ return ((PTW32_INTERLOCKED_PVOID)PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR((PTW32_INTERLOCKED_PVOID_PTR)lock,
+ (PTW32_INTERLOCKED_PVOID)node,
+ (PTW32_INTERLOCKED_PVOID)0)
+ == (PTW32_INTERLOCKED_PVOID)0) ? 0 : EBUSY;
+}
+
+/*
+ * ptw32_mcs_node_transfer -- move an MCS lock local node, usually from thread
+ * space to, for example, global space so that another thread can release
+ * the lock on behalf of the current lock owner.
+ *
+ * Example: used in pthread_barrier_wait where we want the last thread out of
+ * the barrier to release the lock owned by the last thread to enter the barrier
+ * (the one that releases all threads but not necessarily the last to leave).
+ *
+ * Should only be called by the thread that has the lock.
+ */
+#if defined(PTW32_BUILD_INLINED)
+INLINE
+#endif /* PTW32_BUILD_INLINED */
+void
+ptw32_mcs_node_transfer (ptw32_mcs_local_node_t * new_node, ptw32_mcs_local_node_t * old_node)
+{
+ new_node->lock = old_node->lock;
+ new_node->nextFlag = 0; /* Not needed - used only in initial Acquire */
+ new_node->readyFlag = 0; /* Not needed - we were waiting on this */
+ new_node->next = 0;
+
+ if ((ptw32_mcs_local_node_t *)PTW32_INTERLOCKED_COMPARE_EXCHANGE_PTR((PTW32_INTERLOCKED_PVOID_PTR)new_node->lock,
+ (PTW32_INTERLOCKED_PVOID)new_node,
+ (PTW32_INTERLOCKED_PVOID)old_node)
+ != old_node)
+ {
+ /*
+ * A successor has queued after us, so wait for them to link to us
+ */
+ while (old_node->next == 0)
+ {
+ sched_yield();
+ }
+ new_node->next = old_node->next;
+ }
+}
diff --git a/3rdparty/pthreads-win32/ptw32_callUserDestroyRoutines.c b/3rdparty/pthreads-win32/ptw32_callUserDestroyRoutines.c
new file mode 100644
index 0000000..f290f7b
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_callUserDestroyRoutines.c
@@ -0,0 +1,232 @@
+/*
+ * ptw32_callUserDestroyRoutines.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#if defined(__CLEANUP_CXX)
+# if defined(_MSC_VER)
+# include <eh.h>
+# elif defined(__WATCOMC__)
+# include <eh.h>
+# include <exceptio.h>
+# else
+# if defined(__GNUC__) && __GNUC__ < 3
+# include <new.h>
+# else
+# include <new>
+ using
+ std::terminate;
+# endif
+# endif
+#endif
+
+void
+ptw32_callUserDestroyRoutines (pthread_t thread)
+ /*
+ * -------------------------------------------------------------------
+ * DOCPRIVATE
+ *
+ * This the routine runs through all thread keys and calls
+ * the destroy routines on the user's data for the current thread.
+ * It simulates the behaviour of POSIX Threads.
+ *
+ * PARAMETERS
+ * thread
+ * an instance of pthread_t
+ *
+ * RETURNS
+ * N/A
+ * -------------------------------------------------------------------
+ */
+{
+ ThreadKeyAssoc * assoc;
+
+ if (thread.p != NULL)
+ {
+ ptw32_mcs_local_node_t threadLock;
+ ptw32_mcs_local_node_t keyLock;
+ int assocsRemaining;
+ int iterations = 0;
+ ptw32_thread_t * sp = (ptw32_thread_t *) thread.p;
+
+ /*
+ * Run through all Thread<-->Key associations
+ * for the current thread.
+ *
+ * Do this process at most PTHREAD_DESTRUCTOR_ITERATIONS times.
+ */
+ do
+ {
+ assocsRemaining = 0;
+ iterations++;
+
+ ptw32_mcs_lock_acquire(&(sp->threadLock), &threadLock);
+ /*
+ * The pointer to the next assoc is stored in the thread struct so that
+ * the assoc destructor in pthread_key_delete can adjust it
+ * if it deletes this assoc. This can happen if we fail to acquire
+ * both locks below, and are forced to release all of our locks,
+ * leaving open the opportunity for pthread_key_delete to get in
+ * before us.
+ */
+ sp->nextAssoc = sp->keys;
+ ptw32_mcs_lock_release(&threadLock);
+
+ for (;;)
+ {
+ void * value;
+ pthread_key_t k;
+ void (*destructor) (void *);
+
+ /*
+ * First we need to serialise with pthread_key_delete by locking
+ * both assoc guards, but in the reverse order to our convention,
+ * so we must be careful to avoid deadlock.
+ */
+ ptw32_mcs_lock_acquire(&(sp->threadLock), &threadLock);
+
+ if ((assoc = (ThreadKeyAssoc *)sp->nextAssoc) == NULL)
+ {
+ /* Finished */
+ ptw32_mcs_lock_release(&threadLock);
+ break;
+ }
+ else
+ {
+ /*
+ * assoc->key must be valid because assoc can't change or be
+ * removed from our chain while we hold at least one lock. If
+ * the assoc was on our key chain then the key has not been
+ * deleted yet.
+ *
+ * Now try to acquire the second lock without deadlocking.
+ * If we fail, we need to relinquish the first lock and the
+ * processor and then try to acquire them all again.
+ */
+ if (ptw32_mcs_lock_try_acquire(&(assoc->key->keyLock), &keyLock) == EBUSY)
+ {
+ ptw32_mcs_lock_release(&threadLock);
+ Sleep(0);
+ /*
+ * Go around again.
+ * If pthread_key_delete has removed this assoc in the meantime,
+ * sp->nextAssoc will point to a new assoc.
+ */
+ continue;
+ }
+ }
+
+ /* We now hold both locks */
+
+ sp->nextAssoc = assoc->nextKey;
+
+ /*
+ * Key still active; pthread_key_delete
+ * will block on these same mutexes before
+ * it can release actual key; therefore,
+ * key is valid and we can call the destroy
+ * routine;
+ */
+ k = assoc->key;
+ destructor = k->destructor;
+ value = TlsGetValue(k->key);
+ TlsSetValue (k->key, NULL);
+
+ // Every assoc->key exists and has a destructor
+ if (value != NULL && iterations <= PTHREAD_DESTRUCTOR_ITERATIONS)
+ {
+ /*
+ * Unlock both locks before the destructor runs.
+ * POSIX says pthread_key_delete can be run from destructors,
+ * and that probably includes with this key as target.
+ * pthread_setspecific can also be run from destructors and
+ * also needs to be able to access the assocs.
+ */
+ ptw32_mcs_lock_release(&threadLock);
+ ptw32_mcs_lock_release(&keyLock);
+
+ assocsRemaining++;
+
+#if defined(__cplusplus)
+
+ try
+ {
+ /*
+ * Run the caller's cleanup routine.
+ */
+ destructor (value);
+ }
+ catch (...)
+ {
+ /*
+ * A system unexpected exception has occurred
+ * running the user's destructor.
+ * We get control back within this block in case
+ * the application has set up it's own terminate
+ * handler. Since we are leaving the thread we
+ * should not get any internal pthreads
+ * exceptions.
+ */
+ terminate ();
+ }
+
+#else /* __cplusplus */
+
+ /*
+ * Run the caller's cleanup routine.
+ */
+ destructor (value);
+
+#endif /* __cplusplus */
+
+ }
+ else
+ {
+ /*
+ * Remove association from both the key and thread chains
+ * and reclaim it's memory resources.
+ */
+ ptw32_tkAssocDestroy (assoc);
+ ptw32_mcs_lock_release(&threadLock);
+ ptw32_mcs_lock_release(&keyLock);
+ }
+ }
+ }
+ while (assocsRemaining);
+ }
+} /* ptw32_callUserDestroyRoutines */
diff --git a/3rdparty/pthreads-win32/ptw32_calloc.c b/3rdparty/pthreads-win32/ptw32_calloc.c
new file mode 100644
index 0000000..e7b9e64
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_calloc.c
@@ -0,0 +1,56 @@
+/*
+ * ptw32_calloc.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#if defined(NEED_CALLOC)
+void *
+ptw32_calloc (size_t n, size_t s)
+{
+ unsigned int m = n * s;
+ void *p;
+
+ p = malloc (m);
+ if (p == NULL)
+ return NULL;
+
+ memset (p, 0, m);
+
+ return p;
+}
+#endif
diff --git a/3rdparty/pthreads-win32/ptw32_cond_check_need_init.c b/3rdparty/pthreads-win32/ptw32_cond_check_need_init.c
new file mode 100644
index 0000000..ec3e8bb
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_cond_check_need_init.c
@@ -0,0 +1,78 @@
+/*
+ * ptw32_cond_check_need_init.c
+ *
+ * Description:
+ * This translation unit implements condition variables and their primitives.
+ *
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+INLINE int
+ptw32_cond_check_need_init (pthread_cond_t * cond)
+{
+ int result = 0;
+ ptw32_mcs_local_node_t node;
+
+ /*
+ * The following guarded test is specifically for statically
+ * initialised condition variables (via PTHREAD_OBJECT_INITIALIZER).
+ */
+ ptw32_mcs_lock_acquire(&ptw32_cond_test_init_lock, &node);
+
+ /*
+ * We got here possibly under race
+ * conditions. Check again inside the critical section.
+ * If a static cv has been destroyed, the application can
+ * re-initialise it only by calling pthread_cond_init()
+ * explicitly.
+ */
+ if (*cond == PTHREAD_COND_INITIALIZER)
+ {
+ result = pthread_cond_init (cond, NULL);
+ }
+ else if (*cond == NULL)
+ {
+ /*
+ * The cv has been destroyed while we were waiting to
+ * initialise it, so the operation that caused the
+ * auto-initialisation should fail.
+ */
+ result = EINVAL;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/ptw32_getprocessors.c b/3rdparty/pthreads-win32/ptw32_getprocessors.c
new file mode 100644
index 0000000..e60c314
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_getprocessors.c
@@ -0,0 +1,91 @@
+/*
+ * ptw32_getprocessors.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+/*
+ * ptw32_getprocessors()
+ *
+ * Get the number of CPUs available to the process.
+ *
+ * If the available number of CPUs is 1 then pthread_spin_lock()
+ * will block rather than spin if the lock is already owned.
+ *
+ * pthread_spin_init() calls this routine when initialising
+ * a spinlock. If the number of available processors changes
+ * (after a call to SetProcessAffinityMask()) then only
+ * newly initialised spinlocks will notice.
+ */
+int
+ptw32_getprocessors (int *count)
+{
+ DWORD_PTR vProcessCPUs;
+ DWORD_PTR vSystemCPUs;
+ int result = 0;
+
+#if defined(NEED_PROCESS_AFFINITY_MASK)
+
+ *count = 1;
+
+#else
+
+ if (GetProcessAffinityMask (GetCurrentProcess (),
+ &vProcessCPUs, &vSystemCPUs))
+ {
+ DWORD_PTR bit;
+ int CPUs = 0;
+
+ for (bit = 1; bit != 0; bit <<= 1)
+ {
+ if (vProcessCPUs & bit)
+ {
+ CPUs++;
+ }
+ }
+ *count = CPUs;
+ }
+ else
+ {
+ result = EAGAIN;
+ }
+
+#endif
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_is_attr.c b/3rdparty/pthreads-win32/ptw32_is_attr.c
new file mode 100644
index 0000000..36395f8
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_is_attr.c
@@ -0,0 +1,47 @@
+/*
+ * ptw32_is_attr.c
+ *
+ * Description:
+ * This translation unit implements operations on thread attribute objects.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+int
+ptw32_is_attr (const pthread_attr_t * attr)
+{
+ /* Return 0 if the attr object is valid, non-zero otherwise. */
+
+ return (attr == NULL ||
+ *attr == NULL || (*attr)->valid != PTW32_ATTR_VALID);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_mutex_check_need_init.c b/3rdparty/pthreads-win32/ptw32_mutex_check_need_init.c
new file mode 100644
index 0000000..897db3c
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_mutex_check_need_init.c
@@ -0,0 +1,92 @@
+/*
+ * ptw32_mutex_check_need_init.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+static struct pthread_mutexattr_t_ ptw32_recursive_mutexattr_s =
+ {PTHREAD_PROCESS_PRIVATE, PTHREAD_MUTEX_RECURSIVE};
+static struct pthread_mutexattr_t_ ptw32_errorcheck_mutexattr_s =
+ {PTHREAD_PROCESS_PRIVATE, PTHREAD_MUTEX_ERRORCHECK};
+static pthread_mutexattr_t ptw32_recursive_mutexattr = &ptw32_recursive_mutexattr_s;
+static pthread_mutexattr_t ptw32_errorcheck_mutexattr = &ptw32_errorcheck_mutexattr_s;
+
+
+INLINE int
+ptw32_mutex_check_need_init (pthread_mutex_t * mutex)
+{
+ register int result = 0;
+ register pthread_mutex_t mtx;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_mutex_test_init_lock, &node);
+
+ /*
+ * We got here possibly under race
+ * conditions. Check again inside the critical section
+ * and only initialise if the mutex is valid (not been destroyed).
+ * If a static mutex has been destroyed, the application can
+ * re-initialise it only by calling pthread_mutex_init()
+ * explicitly.
+ */
+ mtx = *mutex;
+
+ if (mtx == PTHREAD_MUTEX_INITIALIZER)
+ {
+ result = pthread_mutex_init (mutex, NULL);
+ }
+ else if (mtx == PTHREAD_RECURSIVE_MUTEX_INITIALIZER)
+ {
+ result = pthread_mutex_init (mutex, &ptw32_recursive_mutexattr);
+ }
+ else if (mtx == PTHREAD_ERRORCHECK_MUTEX_INITIALIZER)
+ {
+ result = pthread_mutex_init (mutex, &ptw32_errorcheck_mutexattr);
+ }
+ else if (mtx == NULL)
+ {
+ /*
+ * The mutex has been destroyed while we were waiting to
+ * initialise it, so the operation that caused the
+ * auto-initialisation should fail.
+ */
+ result = EINVAL;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_new.c b/3rdparty/pthreads-win32/ptw32_new.c
new file mode 100644
index 0000000..e771c02
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_new.c
@@ -0,0 +1,100 @@
+/*
+ * ptw32_new.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+pthread_t
+ptw32_new (void)
+{
+ pthread_t t;
+ pthread_t nil = {NULL, 0};
+ ptw32_thread_t * tp;
+
+ /*
+ * If there's a reusable pthread_t then use it.
+ */
+ t = ptw32_threadReusePop ();
+
+ if (NULL != t.p)
+ {
+ tp = (ptw32_thread_t *) t.p;
+ }
+ else
+ {
+ /* No reuse threads available */
+ tp = (ptw32_thread_t *) calloc (1, sizeof(ptw32_thread_t));
+
+ if (tp == NULL)
+ {
+ return nil;
+ }
+
+ /* ptHandle.p needs to point to it's parent ptw32_thread_t. */
+ t.p = tp->ptHandle.p = tp;
+ t.x = tp->ptHandle.x = 0;
+ }
+
+ /* Set default state. */
+ tp->seqNumber = ++ptw32_threadSeqNumber;
+ tp->sched_priority = THREAD_PRIORITY_NORMAL;
+ tp->detachState = PTHREAD_CREATE_JOINABLE;
+ tp->cancelState = PTHREAD_CANCEL_ENABLE;
+ tp->cancelType = PTHREAD_CANCEL_DEFERRED;
+ tp->stateLock = 0;
+ tp->threadLock = 0;
+ tp->robustMxListLock = 0;
+ tp->robustMxList = NULL;
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ tp->cancelEvent = CreateEventEx(0, NULL, CREATE_EVENT_MANUAL_RESET, EVENT_ALL_ACCESS);
+# else
+ tp->cancelEvent = CreateEvent (0, (int) PTW32_TRUE, /* manualReset */
+ (int) PTW32_FALSE, /* setSignaled */
+ NULL);
+# endif
+#endif
+
+ if (tp->cancelEvent == NULL)
+ {
+ ptw32_threadReusePush (tp->ptHandle);
+ return nil;
+ }
+
+ return t;
+
+}
diff --git a/3rdparty/pthreads-win32/ptw32_processInitialize.c b/3rdparty/pthreads-win32/ptw32_processInitialize.c
new file mode 100644
index 0000000..8da3e41
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_processInitialize.c
@@ -0,0 +1,92 @@
+/*
+ * ptw32_processInitialize.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+ptw32_processInitialize (void)
+ /*
+ * ------------------------------------------------------
+ * DOCPRIVATE
+ * This function performs process wide initialization for
+ * the pthread library.
+ *
+ * PARAMETERS
+ * N/A
+ *
+ * DESCRIPTION
+ * This function performs process wide initialization for
+ * the pthread library.
+ * If successful, this routine sets the global variable
+ * ptw32_processInitialized to TRUE.
+ *
+ * RESULTS
+ * TRUE if successful,
+ * FALSE otherwise
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (ptw32_processInitialized)
+ {
+ /*
+ * Ignore if already initialized. this is useful for
+ * programs that uses a non-dll pthread
+ * library. Such programs must call ptw32_processInitialize() explicitly,
+ * since this initialization routine is automatically called only when
+ * the dll is loaded.
+ */
+ return PTW32_TRUE;
+ }
+
+ ptw32_processInitialized = PTW32_TRUE;
+
+ /*
+ * Initialize Keys
+ */
+ if ((pthread_key_create (&ptw32_selfThreadKey, NULL) != 0) ||
+ (pthread_key_create (&ptw32_cleanupKey, NULL) != 0))
+ {
+
+ ptw32_processTerminate ();
+ }
+
+ return (ptw32_processInitialized);
+
+} /* processInitialize */
diff --git a/3rdparty/pthreads-win32/ptw32_processTerminate.c b/3rdparty/pthreads-win32/ptw32_processTerminate.c
new file mode 100644
index 0000000..83f0f23
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_processTerminate.c
@@ -0,0 +1,105 @@
+/*
+ * ptw32_processTerminate.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+void
+ptw32_processTerminate (void)
+ /*
+ * ------------------------------------------------------
+ * DOCPRIVATE
+ * This function performs process wide termination for
+ * the pthread library.
+ *
+ * PARAMETERS
+ * N/A
+ *
+ * DESCRIPTION
+ * This function performs process wide termination for
+ * the pthread library.
+ * This routine sets the global variable
+ * ptw32_processInitialized to FALSE
+ *
+ * RESULTS
+ * N/A
+ *
+ * ------------------------------------------------------
+ */
+{
+ if (ptw32_processInitialized)
+ {
+ ptw32_thread_t * tp, * tpNext;
+ ptw32_mcs_local_node_t node;
+
+ if (ptw32_selfThreadKey != NULL)
+ {
+ /*
+ * Release ptw32_selfThreadKey
+ */
+ pthread_key_delete (ptw32_selfThreadKey);
+
+ ptw32_selfThreadKey = NULL;
+ }
+
+ if (ptw32_cleanupKey != NULL)
+ {
+ /*
+ * Release ptw32_cleanupKey
+ */
+ pthread_key_delete (ptw32_cleanupKey);
+
+ ptw32_cleanupKey = NULL;
+ }
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ tp = ptw32_threadReuseTop;
+ while (tp != PTW32_THREAD_REUSE_EMPTY)
+ {
+ tpNext = tp->prevReuse;
+ free (tp);
+ tp = tpNext;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ ptw32_processInitialized = PTW32_FALSE;
+ }
+
+} /* processTerminate */
diff --git a/3rdparty/pthreads-win32/ptw32_relmillisecs.c b/3rdparty/pthreads-win32/ptw32_relmillisecs.c
new file mode 100644
index 0000000..894d5c9
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_relmillisecs.c
@@ -0,0 +1,132 @@
+/*
+ * ptw32_relmillisecs.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#if !defined(NEED_FTIME)
+#include <sys/timeb.h>
+#endif
+
+
+#if defined(PTW32_BUILD_INLINED)
+INLINE
+#endif /* PTW32_BUILD_INLINED */
+DWORD
+ptw32_relmillisecs (const struct timespec * abstime)
+{
+ const int64_t NANOSEC_PER_MILLISEC = 1000000;
+ const int64_t MILLISEC_PER_SEC = 1000;
+ DWORD milliseconds;
+ int64_t tmpAbsMilliseconds;
+ int64_t tmpCurrMilliseconds;
+#if defined(NEED_FTIME)
+ struct timespec currSysTime;
+ FILETIME ft;
+ SYSTEMTIME st;
+#else /* ! NEED_FTIME */
+#if ( defined(_MSC_VER) && _MSC_VER >= 1300 ) || \
+ ( (defined(__MINGW64__) || defined(__MINGW32__)) && __MSVCRT_VERSION__ >= 0x0601 )
+ struct __timeb64 currSysTime;
+#else
+ struct _timeb currSysTime;
+#endif
+#endif /* NEED_FTIME */
+
+
+ /*
+ * Calculate timeout as milliseconds from current system time.
+ */
+
+ /*
+ * subtract current system time from abstime in a way that checks
+ * that abstime is never in the past, or is never equivalent to the
+ * defined INFINITE value (0xFFFFFFFF).
+ *
+ * Assume all integers are unsigned, i.e. cannot test if less than 0.
+ */
+ tmpAbsMilliseconds = (int64_t)abstime->tv_sec * MILLISEC_PER_SEC;
+ tmpAbsMilliseconds += ((int64_t)abstime->tv_nsec + (NANOSEC_PER_MILLISEC/2)) / NANOSEC_PER_MILLISEC;
+
+ /* get current system time */
+
+#if defined(NEED_FTIME)
+
+ GetSystemTime(&st);
+ SystemTimeToFileTime(&st, &ft);
+ /*
+ * GetSystemTimeAsFileTime(&ft); would be faster,
+ * but it does not exist on WinCE
+ */
+
+ ptw32_filetime_to_timespec(&ft, &currSysTime);
+
+ tmpCurrMilliseconds = (int64_t)currSysTime.tv_sec * MILLISEC_PER_SEC;
+ tmpCurrMilliseconds += ((int64_t)currSysTime.tv_nsec + (NANOSEC_PER_MILLISEC/2))
+ / NANOSEC_PER_MILLISEC;
+
+#else /* ! NEED_FTIME */
+
+#if defined(_MSC_VER) && _MSC_VER >= 1400
+ _ftime64_s(&currSysTime);
+#elif ( defined(_MSC_VER) && _MSC_VER >= 1300 ) || \
+ ( (defined(__MINGW64__) || defined(__MINGW32__)) && __MSVCRT_VERSION__ >= 0x0601 )
+ _ftime64(&currSysTime);
+#else
+ _ftime(&currSysTime);
+#endif
+
+ tmpCurrMilliseconds = (int64_t) currSysTime.time * MILLISEC_PER_SEC;
+ tmpCurrMilliseconds += (int64_t) currSysTime.millitm;
+
+#endif /* NEED_FTIME */
+
+ if (tmpAbsMilliseconds > tmpCurrMilliseconds)
+ {
+ milliseconds = (DWORD) (tmpAbsMilliseconds - tmpCurrMilliseconds);
+ if (milliseconds == INFINITE)
+ {
+ /* Timeouts must be finite */
+ milliseconds--;
+ }
+ }
+ else
+ {
+ /* The abstime given is in the past */
+ milliseconds = 0;
+ }
+
+ return milliseconds;
+}
diff --git a/3rdparty/pthreads-win32/ptw32_reuse.c b/3rdparty/pthreads-win32/ptw32_reuse.c
new file mode 100644
index 0000000..7325857
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_reuse.c
@@ -0,0 +1,151 @@
+/*
+ * ptw32_threadReuse.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+/*
+ * How it works:
+ * A pthread_t is a struct (2x32 bit scalar types on IA-32, 2x64 bit on IA-64)
+ * which is normally passed/returned by value to/from pthreads routines.
+ * Applications are therefore storing a copy of the struct as it is at that
+ * time.
+ *
+ * The original pthread_t struct plus all copies of it contain the address of
+ * the thread state struct ptw32_thread_t_ (p), plus a reuse counter (x). Each
+ * ptw32_thread_t contains the original copy of it's pthread_t.
+ * Once malloced, a ptw32_thread_t_ struct is not freed until the process exits.
+ *
+ * The thread reuse stack is a simple LILO stack managed through a singly
+ * linked list element in the ptw32_thread_t.
+ *
+ * Each time a thread is destroyed, the ptw32_thread_t address is pushed onto the
+ * reuse stack after it's ptHandle's reuse counter has been incremented.
+ *
+ * The following can now be said from this:
+ * - two pthread_t's are identical if their ptw32_thread_t reference pointers
+ * are equal and their reuse counters are equal. That is,
+ *
+ * equal = (a.p == b.p && a.x == b.x)
+ *
+ * - a pthread_t copy refers to a destroyed thread if the reuse counter in
+ * the copy is not equal to the reuse counter in the original.
+ *
+ * threadDestroyed = (copy.x != ((ptw32_thread_t *)copy.p)->ptHandle.x)
+ *
+ */
+
+/*
+ * Pop a clean pthread_t struct off the reuse stack.
+ */
+pthread_t
+ptw32_threadReusePop (void)
+{
+ pthread_t t = {NULL, 0};
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ if (PTW32_THREAD_REUSE_EMPTY != ptw32_threadReuseTop)
+ {
+ ptw32_thread_t * tp;
+
+ tp = ptw32_threadReuseTop;
+
+ ptw32_threadReuseTop = tp->prevReuse;
+
+ if (PTW32_THREAD_REUSE_EMPTY == ptw32_threadReuseTop)
+ {
+ ptw32_threadReuseBottom = PTW32_THREAD_REUSE_EMPTY;
+ }
+
+ tp->prevReuse = NULL;
+
+ t = tp->ptHandle;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return t;
+
+}
+
+/*
+ * Push a clean pthread_t struct onto the reuse stack.
+ * Must be re-initialised when reused.
+ * All object elements (mutexes, events etc) must have been either
+ * detroyed before this, or never initialised.
+ */
+void
+ptw32_threadReusePush (pthread_t thread)
+{
+ ptw32_thread_t * tp = (ptw32_thread_t *) thread.p;
+ pthread_t t;
+ ptw32_mcs_local_node_t node;
+
+ ptw32_mcs_lock_acquire(&ptw32_thread_reuse_lock, &node);
+
+ t = tp->ptHandle;
+ memset(tp, 0, sizeof(ptw32_thread_t));
+
+ /* Must restore the original POSIX handle that we just wiped. */
+ tp->ptHandle = t;
+
+ /* Bump the reuse counter now */
+#if defined(PTW32_THREAD_ID_REUSE_INCREMENT)
+ tp->ptHandle.x += PTW32_THREAD_ID_REUSE_INCREMENT;
+#else
+ tp->ptHandle.x++;
+#endif
+
+ tp->state = PThreadStateReuse;
+
+ tp->prevReuse = PTW32_THREAD_REUSE_EMPTY;
+
+ if (PTW32_THREAD_REUSE_EMPTY != ptw32_threadReuseBottom)
+ {
+ ptw32_threadReuseBottom->prevReuse = tp;
+ }
+ else
+ {
+ ptw32_threadReuseTop = tp;
+ }
+
+ ptw32_threadReuseBottom = tp;
+
+ ptw32_mcs_lock_release(&node);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_rwlock_cancelwrwait.c b/3rdparty/pthreads-win32/ptw32_rwlock_cancelwrwait.c
new file mode 100644
index 0000000..a057bd1
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_rwlock_cancelwrwait.c
@@ -0,0 +1,50 @@
+/*
+ * ptw32_rwlock_cancelwrwait.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+void
+ptw32_rwlock_cancelwrwait (void *arg)
+{
+ pthread_rwlock_t rwl = (pthread_rwlock_t) arg;
+
+ rwl->nSharedAccessCount = -rwl->nCompletedSharedAccessCount;
+ rwl->nCompletedSharedAccessCount = 0;
+
+ (void) pthread_mutex_unlock (&(rwl->mtxSharedAccessCompleted));
+ (void) pthread_mutex_unlock (&(rwl->mtxExclusiveAccess));
+}
diff --git a/3rdparty/pthreads-win32/ptw32_rwlock_check_need_init.c b/3rdparty/pthreads-win32/ptw32_rwlock_check_need_init.c
new file mode 100644
index 0000000..858ee27
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_rwlock_check_need_init.c
@@ -0,0 +1,77 @@
+/*
+ * pthread_rwlock_check_need_init.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+INLINE int
+ptw32_rwlock_check_need_init (pthread_rwlock_t * rwlock)
+{
+ int result = 0;
+ ptw32_mcs_local_node_t node;
+
+ /*
+ * The following guarded test is specifically for statically
+ * initialised rwlocks (via PTHREAD_RWLOCK_INITIALIZER).
+ */
+ ptw32_mcs_lock_acquire(&ptw32_rwlock_test_init_lock, &node);
+
+ /*
+ * We got here possibly under race
+ * conditions. Check again inside the critical section
+ * and only initialise if the rwlock is valid (not been destroyed).
+ * If a static rwlock has been destroyed, the application can
+ * re-initialise it only by calling pthread_rwlock_init()
+ * explicitly.
+ */
+ if (*rwlock == PTHREAD_RWLOCK_INITIALIZER)
+ {
+ result = pthread_rwlock_init (rwlock, NULL);
+ }
+ else if (*rwlock == NULL)
+ {
+ /*
+ * The rwlock has been destroyed while we were waiting to
+ * initialise it, so the operation that caused the
+ * auto-initialisation should fail.
+ */
+ result = EINVAL;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return result;
+}
diff --git a/3rdparty/pthreads-win32/ptw32_semwait.c b/3rdparty/pthreads-win32/ptw32_semwait.c
new file mode 100644
index 0000000..729f25a
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_semwait.c
@@ -0,0 +1,141 @@
+/*
+ * ptw32_semwait.c
+ *
+ * Description:
+ * This translation unit implements mutual exclusion (mutex) primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined(_UWIN)
+/*# include <process.h> */
+#endif
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+ptw32_semwait (sem_t * sem)
+ /*
+ * ------------------------------------------------------
+ * DESCRIPTION
+ * This function waits on a POSIX semaphore. If the
+ * semaphore value is greater than zero, it decreases
+ * its value by one. If the semaphore value is zero, then
+ * the calling thread (or process) is blocked until it can
+ * successfully decrease the value.
+ *
+ * Unlike sem_wait(), this routine is non-cancelable.
+ *
+ * RESULTS
+ * 0 successfully decreased semaphore,
+ * -1 failed, error in errno.
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS semaphores are not supported,
+ * EINTR the function was interrupted by a signal,
+ * EDEADLK a deadlock condition was detected.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = *sem;
+
+ if (s == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ int v;
+
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ v = --s->value;
+ (void) pthread_mutex_unlock (&s->lock);
+
+ if (v < 0)
+ {
+ /* Must wait */
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WaitForSingleObjectEx(s->sem, INFINITE, FALSE) == WAIT_OBJECT_0)
+# else
+ if (WaitForSingleObject (s->sem, INFINITE) == WAIT_OBJECT_0)
+# endif
+#endif
+ {
+#if defined(NEED_SEM)
+ if (pthread_mutex_lock (&s->lock) == 0)
+ {
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ if (s->leftToUnblock > 0)
+ {
+ --s->leftToUnblock;
+ SetEvent(s->sem);
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+#endif
+ return 0;
+ }
+ }
+ else
+ {
+ return 0;
+ }
+ }
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ return 0;
+
+} /* ptw32_semwait */
diff --git a/3rdparty/pthreads-win32/ptw32_spinlock_check_need_init.c b/3rdparty/pthreads-win32/ptw32_spinlock_check_need_init.c
new file mode 100644
index 0000000..8808454
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_spinlock_check_need_init.c
@@ -0,0 +1,78 @@
+/*
+ * ptw32_spinlock_check_need_init.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+INLINE int
+ptw32_spinlock_check_need_init (pthread_spinlock_t * lock)
+{
+ int result = 0;
+ ptw32_mcs_local_node_t node;
+
+ /*
+ * The following guarded test is specifically for statically
+ * initialised spinlocks (via PTHREAD_SPINLOCK_INITIALIZER).
+ */
+ ptw32_mcs_lock_acquire(&ptw32_spinlock_test_init_lock, &node);
+
+ /*
+ * We got here possibly under race
+ * conditions. Check again inside the critical section
+ * and only initialise if the spinlock is valid (not been destroyed).
+ * If a static spinlock has been destroyed, the application can
+ * re-initialise it only by calling pthread_spin_init()
+ * explicitly.
+ */
+ if (*lock == PTHREAD_SPINLOCK_INITIALIZER)
+ {
+ result = pthread_spin_init (lock, PTHREAD_PROCESS_PRIVATE);
+ }
+ else if (*lock == NULL)
+ {
+ /*
+ * The spinlock has been destroyed while we were waiting to
+ * initialise it, so the operation that caused the
+ * auto-initialisation should fail.
+ */
+ result = EINVAL;
+ }
+
+ ptw32_mcs_lock_release(&node);
+
+ return (result);
+}
diff --git a/3rdparty/pthreads-win32/ptw32_threadDestroy.c b/3rdparty/pthreads-win32/ptw32_threadDestroy.c
new file mode 100644
index 0000000..41499b1
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_threadDestroy.c
@@ -0,0 +1,79 @@
+/*
+ * ptw32_threadDestroy.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+void
+ptw32_threadDestroy (pthread_t thread)
+{
+ ptw32_thread_t * tp = (ptw32_thread_t *) thread.p;
+ ptw32_thread_t threadCopy;
+
+ if (tp != NULL)
+ {
+ /*
+ * Copy thread state so that the thread can be atomically NULLed.
+ */
+ memcpy (&threadCopy, tp, sizeof (threadCopy));
+
+ /*
+ * Thread ID structs are never freed. They're NULLed and reused.
+ * This also sets the thread to PThreadStateInitial (invalid).
+ */
+ ptw32_threadReusePush (thread);
+
+ /* Now work on the copy. */
+ if (threadCopy.cancelEvent != NULL)
+ {
+ CloseHandle (threadCopy.cancelEvent);
+ }
+
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ /*
+ * See documentation for endthread vs endthreadex.
+ */
+ if (threadCopy.threadH != 0)
+ {
+ CloseHandle (threadCopy.threadH);
+ }
+#endif
+
+ }
+} /* ptw32_threadDestroy */
+
diff --git a/3rdparty/pthreads-win32/ptw32_threadStart.c b/3rdparty/pthreads-win32/ptw32_threadStart.c
new file mode 100644
index 0000000..e83ede0
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_threadStart.c
@@ -0,0 +1,357 @@
+/*
+ * ptw32_threadStart.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include <stdio.h>
+
+#if defined(__CLEANUP_C)
+# include <setjmp.h>
+#endif
+
+#if defined(__CLEANUP_SEH)
+
+static DWORD
+ExceptionFilter (EXCEPTION_POINTERS * ep, DWORD * ei)
+{
+ switch (ep->ExceptionRecord->ExceptionCode)
+ {
+ case EXCEPTION_PTW32_SERVICES:
+ {
+ DWORD param;
+ DWORD numParams = ep->ExceptionRecord->NumberParameters;
+
+ numParams = (numParams > 3) ? 3 : numParams;
+
+ for (param = 0; param < numParams; param++)
+ {
+ ei[param] = ep->ExceptionRecord->ExceptionInformation[param];
+ }
+
+ return EXCEPTION_EXECUTE_HANDLER;
+ break;
+ }
+ default:
+ {
+ /*
+ * A system unexpected exception has occurred running the user's
+ * routine. We need to cleanup before letting the exception
+ * out of thread scope.
+ */
+ pthread_t self = pthread_self ();
+
+ ptw32_callUserDestroyRoutines (self);
+
+ return EXCEPTION_CONTINUE_SEARCH;
+ break;
+ }
+ }
+}
+
+#elif defined(__CLEANUP_CXX)
+
+#if defined(_MSC_VER)
+# include <eh.h>
+#elif defined(__WATCOMC__)
+# include <eh.h>
+# include <exceptio.h>
+typedef terminate_handler
+ terminate_function;
+#else
+# if defined(__GNUC__) && __GNUC__ < 3
+# include <new.h>
+# else
+# include <new>
+using
+ std::terminate_handler;
+using
+ std::terminate;
+using
+ std::set_terminate;
+# endif
+typedef terminate_handler
+ terminate_function;
+#endif
+
+static terminate_function
+ ptw32_oldTerminate;
+
+void
+ptw32_terminate ()
+{
+ set_terminate (ptw32_oldTerminate);
+ (void) pthread_win32_thread_detach_np ();
+ terminate ();
+}
+
+#endif
+
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || (defined (__MSVCRT__) && ! defined (__DMC__))
+unsigned
+ __stdcall
+#else
+void
+#endif
+ptw32_threadStart (void *vthreadParms)
+{
+ ThreadParms * threadParms = (ThreadParms *) vthreadParms;
+ pthread_t self;
+ ptw32_thread_t * sp;
+ void * (PTW32_CDECL *start) (void *);
+ void * arg;
+
+#if defined(__CLEANUP_SEH)
+ DWORD
+ ei[] = { 0, 0, 0 };
+#endif
+
+#if defined(__CLEANUP_C)
+ int setjmp_rc;
+#endif
+
+ ptw32_mcs_local_node_t stateLock;
+ void * status = (void *) 0;
+
+ self = threadParms->tid;
+ sp = (ptw32_thread_t *) self.p;
+ start = threadParms->start;
+ arg = threadParms->arg;
+
+ free (threadParms);
+
+#if (defined(__MINGW64__) || defined(__MINGW32__)) && ! defined (__MSVCRT__)
+ /*
+ * beginthread does not return the thread id and is running
+ * before it returns us the thread handle, and so we do it here.
+ */
+ sp->thread = GetCurrentThreadId ();
+ /*
+ * Here we're using stateLock as a general-purpose lock
+ * to make the new thread wait until the creating thread
+ * has the new handle.
+ */
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+ pthread_setspecific (ptw32_selfThreadKey, sp);
+#else
+ pthread_setspecific (ptw32_selfThreadKey, sp);
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+#endif
+
+ sp->state = PThreadStateRunning;
+ ptw32_mcs_lock_release (&stateLock);
+
+#if defined(__CLEANUP_SEH)
+
+ __try
+ {
+ /*
+ * Run the caller's routine;
+ */
+ status = sp->exitStatus = (*start) (arg);
+ sp->state = PThreadStateExiting;
+
+#if defined(_UWIN)
+ if (--pthread_count <= 0)
+ exit (0);
+#endif
+
+ }
+ __except (ExceptionFilter (GetExceptionInformation (), ei))
+ {
+ switch (ei[0])
+ {
+ case PTW32_EPS_CANCEL:
+ status = sp->exitStatus = PTHREAD_CANCELED;
+#if defined(_UWIN)
+ if (--pthread_count <= 0)
+ exit (0);
+#endif
+ break;
+ case PTW32_EPS_EXIT:
+ status = sp->exitStatus;
+ break;
+ default:
+ status = sp->exitStatus = PTHREAD_CANCELED;
+ break;
+ }
+ }
+
+#else /* __CLEANUP_SEH */
+
+#if defined(__CLEANUP_C)
+
+ setjmp_rc = setjmp (sp->start_mark);
+
+ if (0 == setjmp_rc)
+ {
+
+ /*
+ * Run the caller's routine;
+ */
+ status = sp->exitStatus = (*start) (arg);
+ sp->state = PThreadStateExiting;
+ }
+ else
+ {
+ switch (setjmp_rc)
+ {
+ case PTW32_EPS_CANCEL:
+ status = sp->exitStatus = PTHREAD_CANCELED;
+ break;
+ case PTW32_EPS_EXIT:
+ status = sp->exitStatus;
+ break;
+ default:
+ status = sp->exitStatus = PTHREAD_CANCELED;
+ break;
+ }
+ }
+
+#else /* __CLEANUP_C */
+
+#if defined(__CLEANUP_CXX)
+
+ ptw32_oldTerminate = set_terminate (&ptw32_terminate);
+
+ try
+ {
+ /*
+ * Run the caller's routine in a nested try block so that we
+ * can run the user's terminate function, which may call
+ * pthread_exit() or be canceled.
+ */
+ try
+ {
+ status = sp->exitStatus = (*start) (arg);
+ sp->state = PThreadStateExiting;
+ }
+ catch (ptw32_exception &)
+ {
+ /*
+ * Pass these through to the outer block.
+ */
+ throw;
+ }
+ catch (...)
+ {
+ /*
+ * We want to run the user's terminate function if supplied.
+ * That function may call pthread_exit() or be canceled, which will
+ * be handled by the outer try block.
+ *
+ * ptw32_terminate() will be called if there is no user
+ * supplied function.
+ */
+ terminate_function
+ term_func = set_terminate (0);
+ set_terminate (term_func);
+
+ if (term_func != 0)
+ {
+ term_func ();
+ }
+ throw;
+ }
+ }
+ catch (ptw32_exception_cancel &)
+ {
+ /*
+ * Thread was canceled.
+ */
+ status = sp->exitStatus = PTHREAD_CANCELED;
+ }
+ catch (ptw32_exception_exit &)
+ {
+ /*
+ * Thread was exited via pthread_exit().
+ */
+ status = sp->exitStatus;
+ }
+ catch (...)
+ {
+ /*
+ * A system unexpected exception has occurred running the user's
+ * terminate routine. We get control back within this block
+ * and exit with a substitute status. If the thread was not
+ * cancelled then this indicates the unhandled exception.
+ */
+ status = sp->exitStatus = PTHREAD_CANCELED;
+ }
+
+ (void) set_terminate (ptw32_oldTerminate);
+
+#else
+
+#error ERROR [__FILE__, line __LINE__]: Cleanup type undefined.
+
+#endif /* __CLEANUP_CXX */
+#endif /* __CLEANUP_C */
+#endif /* __CLEANUP_SEH */
+
+#if defined(PTW32_STATIC_LIB)
+ /*
+ * We need to cleanup the pthread now if we have
+ * been statically linked, in which case the cleanup
+ * in dllMain won't get done. Joinable threads will
+ * only be partially cleaned up and must be fully cleaned
+ * up by pthread_join() or pthread_detach().
+ *
+ * Note: if this library has been statically linked,
+ * implicitly created pthreads (those created
+ * for Win32 threads which have called pthreads routines)
+ * must be cleaned up explicitly by the application
+ * (by calling pthread_win32_thread_detach_np()).
+ * For the dll, dllMain will do the cleanup automatically.
+ */
+ (void) pthread_win32_thread_detach_np ();
+#endif
+
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ _endthreadex ((unsigned)(size_t) status);
+#else
+ _endthread ();
+#endif
+
+ /*
+ * Never reached.
+ */
+
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ return (unsigned)(size_t) status;
+#endif
+
+} /* ptw32_threadStart */
diff --git a/3rdparty/pthreads-win32/ptw32_throw.c b/3rdparty/pthreads-win32/ptw32_throw.c
new file mode 100644
index 0000000..1404e94
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_throw.c
@@ -0,0 +1,189 @@
+/*
+ * ptw32_throw.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#if defined(__CLEANUP_C)
+# include <setjmp.h>
+#endif
+
+/*
+ * ptw32_throw
+ *
+ * All canceled and explicitly exited POSIX threads go through
+ * here. This routine knows how to exit both POSIX initiated threads and
+ * 'implicit' POSIX threads for each of the possible language modes (C,
+ * C++, and SEH).
+ */
+#if defined(_MSC_VER)
+/*
+ * Ignore the warning:
+ * "C++ exception specification ignored except to indicate that
+ * the function is not __declspec(nothrow)."
+ */
+#pragma warning(disable:4290)
+#endif
+void
+ptw32_throw (DWORD exception)
+#if defined(__CLEANUP_CXX)
+ throw(ptw32_exception_cancel,ptw32_exception_exit)
+#endif
+{
+ /*
+ * Don't use pthread_self() to avoid creating an implicit POSIX thread handle
+ * unnecessarily.
+ */
+ ptw32_thread_t * sp = (ptw32_thread_t *) pthread_getspecific (ptw32_selfThreadKey);
+
+#if defined(__CLEANUP_SEH)
+ DWORD exceptionInformation[3];
+#endif
+
+ sp->state = PThreadStateExiting;
+
+ if (exception != PTW32_EPS_CANCEL && exception != PTW32_EPS_EXIT)
+ {
+ /* Should never enter here */
+ exit (1);
+ }
+
+ if (NULL == sp || sp->implicit)
+ {
+ /*
+ * We're inside a non-POSIX initialised Win32 thread
+ * so there is no point to jump or throw back to. Just do an
+ * explicit thread exit here after cleaning up POSIX
+ * residue (i.e. cleanup handlers, POSIX thread handle etc).
+ */
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ unsigned exitCode = 0;
+
+ switch (exception)
+ {
+ case PTW32_EPS_CANCEL:
+ exitCode = (unsigned)(size_t) PTHREAD_CANCELED;
+ break;
+ case PTW32_EPS_EXIT:
+ if (NULL != sp)
+ {
+ exitCode = (unsigned)(size_t) sp->exitStatus;
+ }
+ break;
+ }
+#endif
+
+#if defined(PTW32_STATIC_LIB)
+
+ pthread_win32_thread_detach_np ();
+
+#endif
+
+#if ! (defined(__MINGW64__) || defined(__MINGW32__)) || defined (__MSVCRT__) || defined (__DMC__)
+ _endthreadex (exitCode);
+#else
+ _endthread ();
+#endif
+
+ }
+
+#if defined(__CLEANUP_SEH)
+
+
+ exceptionInformation[0] = (DWORD) (exception);
+ exceptionInformation[1] = (DWORD) (0);
+ exceptionInformation[2] = (DWORD) (0);
+
+ RaiseException (EXCEPTION_PTW32_SERVICES, 0, 3, exceptionInformation);
+
+#else /* __CLEANUP_SEH */
+
+#if defined(__CLEANUP_C)
+
+ ptw32_pop_cleanup_all (1);
+ longjmp (sp->start_mark, exception);
+
+#else /* __CLEANUP_C */
+
+#if defined(__CLEANUP_CXX)
+
+ switch (exception)
+ {
+ case PTW32_EPS_CANCEL:
+ throw ptw32_exception_cancel ();
+ break;
+ case PTW32_EPS_EXIT:
+ throw ptw32_exception_exit ();
+ break;
+ }
+
+#else
+
+#error ERROR [__FILE__, line __LINE__]: Cleanup type undefined.
+
+#endif /* __CLEANUP_CXX */
+
+#endif /* __CLEANUP_C */
+
+#endif /* __CLEANUP_SEH */
+
+ /* Never reached */
+}
+
+
+void
+ptw32_pop_cleanup_all (int execute)
+{
+ while (NULL != ptw32_pop_cleanup (execute))
+ {
+ }
+}
+
+
+DWORD
+ptw32_get_exception_services_code (void)
+{
+#if defined(__CLEANUP_SEH)
+
+ return EXCEPTION_PTW32_SERVICES;
+
+#else
+
+ return (DWORD)0;
+
+#endif
+}
diff --git a/3rdparty/pthreads-win32/ptw32_timespec.c b/3rdparty/pthreads-win32/ptw32_timespec.c
new file mode 100644
index 0000000..6318957
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_timespec.c
@@ -0,0 +1,83 @@
+/*
+ * ptw32_timespec.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#if defined(NEED_FTIME)
+
+/*
+ * time between jan 1, 1601 and jan 1, 1970 in units of 100 nanoseconds
+ */
+#define PTW32_TIMESPEC_TO_FILETIME_OFFSET \
+ ( ((int64_t) 27111902 << 32) + (int64_t) 3577643008 )
+
+INLINE void
+ptw32_timespec_to_filetime (const struct timespec *ts, FILETIME * ft)
+ /*
+ * -------------------------------------------------------------------
+ * converts struct timespec
+ * where the time is expressed in seconds and nanoseconds from Jan 1, 1970.
+ * into FILETIME (as set by GetSystemTimeAsFileTime), where the time is
+ * expressed in 100 nanoseconds from Jan 1, 1601,
+ * -------------------------------------------------------------------
+ */
+{
+ *(int64_t *) ft = ts->tv_sec * 10000000
+ + (ts->tv_nsec + 50) / 100 + PTW32_TIMESPEC_TO_FILETIME_OFFSET;
+}
+
+INLINE void
+ptw32_filetime_to_timespec (const FILETIME * ft, struct timespec *ts)
+ /*
+ * -------------------------------------------------------------------
+ * converts FILETIME (as set by GetSystemTimeAsFileTime), where the time is
+ * expressed in 100 nanoseconds from Jan 1, 1601,
+ * into struct timespec
+ * where the time is expressed in seconds and nanoseconds from Jan 1, 1970.
+ * -------------------------------------------------------------------
+ */
+{
+ ts->tv_sec =
+ (int) ((*(int64_t *) ft - PTW32_TIMESPEC_TO_FILETIME_OFFSET) / 10000000);
+ ts->tv_nsec =
+ (int) ((*(int64_t *) ft - PTW32_TIMESPEC_TO_FILETIME_OFFSET -
+ ((int64_t) ts->tv_sec * (int64_t) 10000000)) * 100);
+}
+
+#endif /* NEED_FTIME */
diff --git a/3rdparty/pthreads-win32/ptw32_tkAssocCreate.c b/3rdparty/pthreads-win32/ptw32_tkAssocCreate.c
new file mode 100644
index 0000000..50d6c50
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_tkAssocCreate.c
@@ -0,0 +1,118 @@
+/*
+ * ptw32_tkAssocCreate.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+int
+ptw32_tkAssocCreate (ptw32_thread_t * sp, pthread_key_t key)
+ /*
+ * -------------------------------------------------------------------
+ * This routine creates an association that
+ * is unique for the given (thread,key) combination.The association
+ * is referenced by both the thread and the key.
+ * This association allows us to determine what keys the
+ * current thread references and what threads a given key
+ * references.
+ * See the detailed description
+ * at the beginning of this file for further details.
+ *
+ * Notes:
+ * 1) New associations are pushed to the beginning of the
+ * chain so that the internal ptw32_selfThreadKey association
+ * is always last, thus allowing selfThreadExit to
+ * be implicitly called last by pthread_exit.
+ * 2)
+ *
+ * Parameters:
+ * thread
+ * current running thread.
+ * key
+ * key on which to create an association.
+ * Returns:
+ * 0 - if successful,
+ * ENOMEM - not enough memory to create assoc or other object
+ * EINVAL - an internal error occurred
+ * ENOSYS - an internal error occurred
+ * -------------------------------------------------------------------
+ */
+{
+ ThreadKeyAssoc *assoc;
+
+ /*
+ * Have to create an association and add it
+ * to both the key and the thread.
+ *
+ * Both key->keyLock and thread->threadLock are locked before
+ * entry to this routine.
+ */
+ assoc = (ThreadKeyAssoc *) calloc (1, sizeof (*assoc));
+
+ if (assoc == NULL)
+ {
+ return ENOMEM;
+ }
+
+ assoc->thread = sp;
+ assoc->key = key;
+
+ /*
+ * Register assoc with key
+ */
+ assoc->prevThread = NULL;
+ assoc->nextThread = (ThreadKeyAssoc *) key->threads;
+ if (assoc->nextThread != NULL)
+ {
+ assoc->nextThread->prevThread = assoc;
+ }
+ key->threads = (void *) assoc;
+
+ /*
+ * Register assoc with thread
+ */
+ assoc->prevKey = NULL;
+ assoc->nextKey = (ThreadKeyAssoc *) sp->keys;
+ if (assoc->nextKey != NULL)
+ {
+ assoc->nextKey->prevKey = assoc;
+ }
+ sp->keys = (void *) assoc;
+
+ return (0);
+
+} /* ptw32_tkAssocCreate */
diff --git a/3rdparty/pthreads-win32/ptw32_tkAssocDestroy.c b/3rdparty/pthreads-win32/ptw32_tkAssocDestroy.c
new file mode 100644
index 0000000..fedebf5
--- /dev/null
+++ b/3rdparty/pthreads-win32/ptw32_tkAssocDestroy.c
@@ -0,0 +1,114 @@
+/*
+ * ptw32_tkAssocDestroy.c
+ *
+ * Description:
+ * This translation unit implements routines which are private to
+ * the implementation and may be used throughout it.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+void
+ptw32_tkAssocDestroy (ThreadKeyAssoc * assoc)
+ /*
+ * -------------------------------------------------------------------
+ * This routine releases all resources for the given ThreadKeyAssoc
+ * once it is no longer being referenced
+ * ie) either the key or thread has stopped referencing it.
+ *
+ * Parameters:
+ * assoc
+ * an instance of ThreadKeyAssoc.
+ * Returns:
+ * N/A
+ * -------------------------------------------------------------------
+ */
+{
+
+ /*
+ * Both key->keyLock and thread->threadLock are locked before
+ * entry to this routine.
+ */
+ if (assoc != NULL)
+ {
+ ThreadKeyAssoc * prev, * next;
+
+ /* Remove assoc from thread's keys chain */
+ prev = assoc->prevKey;
+ next = assoc->nextKey;
+ if (prev != NULL)
+ {
+ prev->nextKey = next;
+ }
+ if (next != NULL)
+ {
+ next->prevKey = prev;
+ }
+
+ if (assoc->thread->keys == assoc)
+ {
+ /* We're at the head of the thread's keys chain */
+ assoc->thread->keys = next;
+ }
+ if (assoc->thread->nextAssoc == assoc)
+ {
+ /*
+ * Thread is exiting and we're deleting the assoc to be processed next.
+ * Hand thread the assoc after this one.
+ */
+ assoc->thread->nextAssoc = next;
+ }
+
+ /* Remove assoc from key's threads chain */
+ prev = assoc->prevThread;
+ next = assoc->nextThread;
+ if (prev != NULL)
+ {
+ prev->nextThread = next;
+ }
+ if (next != NULL)
+ {
+ next->prevThread = prev;
+ }
+
+ if (assoc->key->threads == assoc)
+ {
+ /* We're at the head of the key's threads chain */
+ assoc->key->threads = next;
+ }
+
+ free (assoc);
+ }
+
+} /* ptw32_tkAssocDestroy */
diff --git a/3rdparty/pthreads-win32/rwlock.c b/3rdparty/pthreads-win32/rwlock.c
new file mode 100644
index 0000000..4a3cd25
--- /dev/null
+++ b/3rdparty/pthreads-win32/rwlock.c
@@ -0,0 +1,51 @@
+/*
+ * rwlock.c
+ *
+ * Description:
+ * This translation unit implements read/write lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "ptw32_rwlock_check_need_init.c"
+#include "ptw32_rwlock_cancelwrwait.c"
+#include "pthread_rwlock_init.c"
+#include "pthread_rwlock_destroy.c"
+#include "pthread_rwlockattr_init.c"
+#include "pthread_rwlockattr_destroy.c"
+#include "pthread_rwlockattr_getpshared.c"
+#include "pthread_rwlockattr_setpshared.c"
+#include "pthread_rwlock_rdlock.c"
+#include "pthread_rwlock_timedrdlock.c"
+#include "pthread_rwlock_wrlock.c"
+#include "pthread_rwlock_timedwrlock.c"
+#include "pthread_rwlock_unlock.c"
+#include "pthread_rwlock_tryrdlock.c"
+#include "pthread_rwlock_trywrlock.c"
diff --git a/3rdparty/pthreads-win32/sched.c b/3rdparty/pthreads-win32/sched.c
new file mode 100644
index 0000000..ed30ea7
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched.c
@@ -0,0 +1,53 @@
+/*
+ * sched.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+#include "pthread_attr_setschedpolicy.c"
+#include "pthread_attr_getschedpolicy.c"
+#include "pthread_attr_setschedparam.c"
+#include "pthread_attr_getschedparam.c"
+#include "pthread_attr_setinheritsched.c"
+#include "pthread_attr_getinheritsched.c"
+#include "pthread_setschedparam.c"
+#include "pthread_getschedparam.c"
+#include "sched_get_priority_max.c"
+#include "sched_get_priority_min.c"
+#include "sched_setscheduler.c"
+#include "sched_getscheduler.c"
+#include "sched_yield.c"
diff --git a/3rdparty/pthreads-win32/sched.h b/3rdparty/pthreads-win32/sched.h
new file mode 100644
index 0000000..f36a97a
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched.h
@@ -0,0 +1,183 @@
+/*
+ * Module: sched.h
+ *
+ * Purpose:
+ * Provides an implementation of POSIX realtime extensions
+ * as defined in
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#if !defined(_SCHED_H)
+#define _SCHED_H
+
+#undef PTW32_SCHED_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_SCHED_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_SCHED_LEVEL
+#define PTW32_SCHED_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_SCHED_LEVEL
+#define PTW32_SCHED_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_SCHED_LEVEL_MAX 3
+
+#if ( defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 200112 ) || !defined(PTW32_SCHED_LEVEL)
+#define PTW32_SCHED_LEVEL PTW32_SCHED_LEVEL_MAX
+/* Include everything */
+#endif
+
+
+#if defined(__GNUC__) && !defined(__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the library, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the library,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#if !defined(PTW32_STATIC_LIB)
+# if defined(PTW32_BUILD)
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#if !defined(PTW32_CONFIG_H)
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(__MINGW64__)
+# define HAVE_STRUCT_TIMESPEC
+# define HAVE_MODE_T
+# elif defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX
+#if defined(NEED_ERRNO)
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX */
+
+#if (defined(__MINGW64__) || defined(__MINGW32__)) || defined(_UWIN)
+# if PTW32_SCHED_LEVEL >= PTW32_SCHED_LEVEL_MAX
+/* For pid_t */
+# include <sys/types.h>
+/* Required by Unix 98 */
+# include <time.h>
+# else
+ typedef int pid_t;
+# endif
+#else
+ typedef int pid_t;
+#endif
+
+/* Thread scheduling policies */
+
+enum {
+ SCHED_OTHER = 0,
+ SCHED_FIFO,
+ SCHED_RR,
+ SCHED_MIN = SCHED_OTHER,
+ SCHED_MAX = SCHED_RR
+};
+
+struct sched_param {
+ int sched_priority;
+};
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif /* __cplusplus */
+
+PTW32_DLLPORT int __cdecl sched_yield (void);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_min (int policy);
+
+PTW32_DLLPORT int __cdecl sched_get_priority_max (int policy);
+
+PTW32_DLLPORT int __cdecl sched_setscheduler (pid_t pid, int policy);
+
+PTW32_DLLPORT int __cdecl sched_getscheduler (pid_t pid);
+
+/*
+ * Note that this macro returns ENOTSUP rather than
+ * ENOSYS as might be expected. However, returning ENOSYS
+ * should mean that sched_get_priority_{min,max} are
+ * not implemented as well as sched_rr_get_interval.
+ * This is not the case, since we just don't support
+ * round-robin scheduling. Therefore I have chosen to
+ * return the same value as sched_setscheduler when
+ * SCHED_RR is passed to it.
+ */
+#define sched_rr_get_interval(_pid, _interval) \
+ ( errno = ENOTSUP, (int) -1 )
+
+
+#if defined(__cplusplus)
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#undef PTW32_SCHED_LEVEL
+#undef PTW32_SCHED_LEVEL_MAX
+
+#endif /* !_SCHED_H */
+
diff --git a/3rdparty/pthreads-win32/sched_get_priority_max.c b/3rdparty/pthreads-win32/sched_get_priority_max.c
new file mode 100644
index 0000000..cabf232
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched_get_priority_max.c
@@ -0,0 +1,134 @@
+/*
+ * sched_get_priority_max.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+/*
+ * On Windows98, THREAD_PRIORITY_LOWEST is (-2) and
+ * THREAD_PRIORITY_HIGHEST is 2, and everything works just fine.
+ *
+ * On WinCE 3.0, it so happen that THREAD_PRIORITY_LOWEST is 5
+ * and THREAD_PRIORITY_HIGHEST is 1 (yes, I know, it is funny:
+ * highest priority use smaller numbers) and the following happens:
+ *
+ * sched_get_priority_min() returns 5
+ * sched_get_priority_max() returns 1
+ *
+ * The following table shows the base priority levels for combinations
+ * of priority class and priority value in Win32.
+ *
+ * Process Priority Class Thread Priority Level
+ * -----------------------------------------------------------------
+ * 1 IDLE_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 HIGH_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 2 IDLE_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 3 IDLE_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 4 IDLE_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 4 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 5 IDLE_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 5 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 5 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 6 IDLE_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 6 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 6 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 7 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 7 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 7 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 8 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 8 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 8 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 8 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 9 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 9 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 9 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 10 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 10 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 11 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 11 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 11 HIGH_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 12 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 12 HIGH_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 13 HIGH_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 14 HIGH_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 IDLE_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 16 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 17 REALTIME_PRIORITY_CLASS -7
+ * 18 REALTIME_PRIORITY_CLASS -6
+ * 19 REALTIME_PRIORITY_CLASS -5
+ * 20 REALTIME_PRIORITY_CLASS -4
+ * 21 REALTIME_PRIORITY_CLASS -3
+ * 22 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 23 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 24 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 25 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 26 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 27 REALTIME_PRIORITY_CLASS 3
+ * 28 REALTIME_PRIORITY_CLASS 4
+ * 29 REALTIME_PRIORITY_CLASS 5
+ * 30 REALTIME_PRIORITY_CLASS 6
+ * 31 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ *
+ * Windows NT: Values -7, -6, -5, -4, -3, 3, 4, 5, and 6 are not supported.
+ */
+
+
+int
+sched_get_priority_max (int policy)
+{
+ if (policy < SCHED_MIN || policy > SCHED_MAX)
+ {
+ errno = EINVAL;
+ return -1;
+ }
+
+#if (THREAD_PRIORITY_LOWEST > THREAD_PRIORITY_NORMAL)
+ /* WinCE? */
+ return PTW32_MAX (THREAD_PRIORITY_IDLE, THREAD_PRIORITY_TIME_CRITICAL);
+#else
+ /* This is independent of scheduling policy in Win32. */
+ return PTW32_MAX (THREAD_PRIORITY_IDLE, THREAD_PRIORITY_TIME_CRITICAL);
+#endif
+}
diff --git a/3rdparty/pthreads-win32/sched_get_priority_min.c b/3rdparty/pthreads-win32/sched_get_priority_min.c
new file mode 100644
index 0000000..9c4f859
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched_get_priority_min.c
@@ -0,0 +1,135 @@
+/*
+ * sched_get_priority_min.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+/*
+ * On Windows98, THREAD_PRIORITY_LOWEST is (-2) and
+ * THREAD_PRIORITY_HIGHEST is 2, and everything works just fine.
+ *
+ * On WinCE 3.0, it so happen that THREAD_PRIORITY_LOWEST is 5
+ * and THREAD_PRIORITY_HIGHEST is 1 (yes, I know, it is funny:
+ * highest priority use smaller numbers) and the following happens:
+ *
+ * sched_get_priority_min() returns 5
+ * sched_get_priority_max() returns 1
+ *
+ * The following table shows the base priority levels for combinations
+ * of priority class and priority value in Win32.
+ *
+ * Process Priority Class Thread Priority Level
+ * -----------------------------------------------------------------
+ * 1 IDLE_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 1 HIGH_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 2 IDLE_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 3 IDLE_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 4 IDLE_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 4 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 5 IDLE_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 5 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 5 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 6 IDLE_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 6 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 6 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 7 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 7 Background NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 7 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 8 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 8 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 8 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 8 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 9 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 9 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 9 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 10 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 10 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 11 Foreground NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 11 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 11 HIGH_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 12 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 12 HIGH_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 13 HIGH_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 14 HIGH_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 15 HIGH_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 IDLE_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 BELOW_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 15 ABOVE_NORMAL_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ * 16 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_IDLE
+ * 17 REALTIME_PRIORITY_CLASS -7
+ * 18 REALTIME_PRIORITY_CLASS -6
+ * 19 REALTIME_PRIORITY_CLASS -5
+ * 20 REALTIME_PRIORITY_CLASS -4
+ * 21 REALTIME_PRIORITY_CLASS -3
+ * 22 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_LOWEST
+ * 23 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_BELOW_NORMAL
+ * 24 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_NORMAL
+ * 25 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_ABOVE_NORMAL
+ * 26 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_HIGHEST
+ * 27 REALTIME_PRIORITY_CLASS 3
+ * 28 REALTIME_PRIORITY_CLASS 4
+ * 29 REALTIME_PRIORITY_CLASS 5
+ * 30 REALTIME_PRIORITY_CLASS 6
+ * 31 REALTIME_PRIORITY_CLASS THREAD_PRIORITY_TIME_CRITICAL
+ *
+ * Windows NT: Values -7, -6, -5, -4, -3, 3, 4, 5, and 6 are not supported.
+ *
+ */
+
+
+int
+sched_get_priority_min (int policy)
+{
+ if (policy < SCHED_MIN || policy > SCHED_MAX)
+ {
+ errno = EINVAL;
+ return -1;
+ }
+
+#if (THREAD_PRIORITY_LOWEST > THREAD_PRIORITY_NORMAL)
+ /* WinCE? */
+ return PTW32_MIN (THREAD_PRIORITY_IDLE, THREAD_PRIORITY_TIME_CRITICAL);
+#else
+ /* This is independent of scheduling policy in Win32. */
+ return PTW32_MIN (THREAD_PRIORITY_IDLE, THREAD_PRIORITY_TIME_CRITICAL);
+#endif
+}
diff --git a/3rdparty/pthreads-win32/sched_getscheduler.c b/3rdparty/pthreads-win32/sched_getscheduler.c
new file mode 100644
index 0000000..8769c15
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched_getscheduler.c
@@ -0,0 +1,71 @@
+/*
+ * sched_getscheduler.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+sched_getscheduler (pid_t pid)
+{
+ /*
+ * Win32 only has one policy which we call SCHED_OTHER.
+ * However, we try to provide other valid side-effects
+ * such as EPERM and ESRCH errors.
+ */
+ if (0 != pid)
+ {
+ int selfPid = (int) GetCurrentProcessId ();
+
+ if (pid != selfPid)
+ {
+ HANDLE h =
+ OpenProcess (PROCESS_QUERY_INFORMATION, PTW32_FALSE, (DWORD) pid);
+
+ if (NULL == h)
+ {
+ errno =
+ (GetLastError () ==
+ (0xFF & ERROR_ACCESS_DENIED)) ? EPERM : ESRCH;
+ return -1;
+ }
+ else
+ CloseHandle(h);
+ }
+ }
+
+ return SCHED_OTHER;
+}
diff --git a/3rdparty/pthreads-win32/sched_setscheduler.c b/3rdparty/pthreads-win32/sched_setscheduler.c
new file mode 100644
index 0000000..8691316
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched_setscheduler.c
@@ -0,0 +1,83 @@
+/*
+ * sched_setscheduler.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+sched_setscheduler (pid_t pid, int policy)
+{
+ /*
+ * Win32 only has one policy which we call SCHED_OTHER.
+ * However, we try to provide other valid side-effects
+ * such as EPERM and ESRCH errors. Choosing to check
+ * for a valid policy last allows us to get the most value out
+ * of this function.
+ */
+ if (0 != pid)
+ {
+ int selfPid = (int) GetCurrentProcessId ();
+
+ if (pid != selfPid)
+ {
+ HANDLE h =
+ OpenProcess (PROCESS_SET_INFORMATION, PTW32_FALSE, (DWORD) pid);
+
+ if (NULL == h)
+ {
+ errno =
+ (GetLastError () ==
+ (0xFF & ERROR_ACCESS_DENIED)) ? EPERM : ESRCH;
+ return -1;
+ }
+ else
+ CloseHandle(h);
+ }
+ }
+
+ if (SCHED_OTHER != policy)
+ {
+ errno = ENOSYS;
+ return -1;
+ }
+
+ /*
+ * Don't set anything because there is nothing to set.
+ * Just return the current (the only possible) value.
+ */
+ return SCHED_OTHER;
+}
diff --git a/3rdparty/pthreads-win32/sched_yield.c b/3rdparty/pthreads-win32/sched_yield.c
new file mode 100644
index 0000000..6ac5ed9
--- /dev/null
+++ b/3rdparty/pthreads-win32/sched_yield.c
@@ -0,0 +1,71 @@
+/*
+ * sched_yield.c
+ *
+ * Description:
+ * POSIX thread functions that deal with thread scheduling.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+#include "sched.h"
+
+int
+sched_yield (void)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function indicates that the calling thread is
+ * willing to give up some time slices to other threads.
+ *
+ * PARAMETERS
+ * N/A
+ *
+ *
+ * DESCRIPTION
+ * This function indicates that the calling thread is
+ * willing to give up some time slices to other threads.
+ * NOTE: Since this is part of POSIX 1003.1b
+ * (realtime extensions), it is defined as returning
+ * -1 if an error occurs and sets errno to the actual
+ * error.
+ *
+ * RESULTS
+ * 0 successfully created semaphore,
+ * ENOSYS sched_yield not supported,
+ *
+ * ------------------------------------------------------
+ */
+{
+ Sleep (0);
+
+ return 0;
+}
diff --git a/3rdparty/pthreads-win32/sem_close.c b/3rdparty/pthreads-win32/sem_close.c
new file mode 100644
index 0000000..6d7280f
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_close.c
@@ -0,0 +1,58 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_close.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+sem_close (sem_t * sem)
+{
+ errno = ENOSYS;
+ return -1;
+} /* sem_close */
diff --git a/3rdparty/pthreads-win32/sem_destroy.c b/3rdparty/pthreads-win32/sem_destroy.c
new file mode 100644
index 0000000..6c98e80
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_destroy.c
@@ -0,0 +1,144 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_destroy.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+int
+sem_destroy (sem_t * sem)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function destroys an unnamed semaphore.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * DESCRIPTION
+ * This function destroys an unnamed semaphore.
+ *
+ * RESULTS
+ * 0 successfully destroyed semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS semaphores are not supported,
+ * EBUSY threads (or processes) are currently
+ * blocked on 'sem'
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = NULL;
+
+ if (sem == NULL || *sem == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ s = *sem;
+
+ if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ if (s->value < 0)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ result = EBUSY;
+ }
+ else
+ {
+ /* There are no threads currently blocked on this semaphore. */
+
+ if (!CloseHandle (s->sem))
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ result = EINVAL;
+ }
+ else
+ {
+ /*
+ * Invalidate the semaphore handle when we have the lock.
+ * Other sema operations should test this after acquiring the lock
+ * to check that the sema is still valid, i.e. before performing any
+ * operations. This may only be necessary before the sema op routine
+ * returns so that the routine can return EINVAL - e.g. if setting
+ * s->value to SEM_VALUE_MAX below does force a fall-through.
+ */
+ *sem = NULL;
+
+ /* Prevent anyone else actually waiting on or posting this sema.
+ */
+ s->value = SEM_VALUE_MAX;
+
+ (void) pthread_mutex_unlock (&s->lock);
+
+ do
+ {
+ /* Give other threads a chance to run and exit any sema op
+ * routines. Due to the SEM_VALUE_MAX value, if sem_post or
+ * sem_wait were blocked by us they should fall through.
+ */
+ Sleep(0);
+ }
+ while (pthread_mutex_destroy (&s->lock) == EBUSY);
+ }
+ }
+ }
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ free (s);
+
+ return 0;
+
+} /* sem_destroy */
diff --git a/3rdparty/pthreads-win32/sem_getvalue.c b/3rdparty/pthreads-win32/sem_getvalue.c
new file mode 100644
index 0000000..baafb02
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_getvalue.c
@@ -0,0 +1,110 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_getvalue.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of PThreads.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1-2001
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+int
+sem_getvalue (sem_t * sem, int *sval)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function stores the current count value of the
+ * semaphore.
+ * RESULTS
+ *
+ * Return value
+ *
+ * 0 sval has been set.
+ * -1 failed, error in errno
+ *
+ * in global errno
+ *
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS this function is not supported,
+ *
+ *
+ * PARAMETERS
+ *
+ * sem pointer to an instance of sem_t
+ *
+ * sval pointer to int.
+ *
+ * DESCRIPTION
+ * This function stores the current count value of the semaphore
+ * pointed to by sem in the int pointed to by sval.
+ */
+{
+ if (sem == NULL || *sem == NULL || sval == NULL)
+ {
+ errno = EINVAL;
+ return -1;
+ }
+ else
+ {
+ long value;
+ register sem_t s = *sem;
+ int result = 0;
+
+ if ((result = pthread_mutex_lock(&s->lock)) == 0)
+ {
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ value = s->value;
+ (void) pthread_mutex_unlock(&s->lock);
+ *sval = value;
+ }
+
+ return result;
+ }
+
+} /* sem_getvalue */
diff --git a/3rdparty/pthreads-win32/sem_init.c b/3rdparty/pthreads-win32/sem_init.c
new file mode 100644
index 0000000..0bc0ca0
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_init.c
@@ -0,0 +1,178 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_init.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of PThreads.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1-2001
+ *
+ * -------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+int
+sem_init (sem_t * sem, int pshared, unsigned int value)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function initializes a semaphore. The
+ * initial value of the semaphore is 'value'
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * pshared
+ * if zero, this semaphore may only be shared between
+ * threads in the same process.
+ * if nonzero, the semaphore can be shared between
+ * processes
+ *
+ * value
+ * initial value of the semaphore counter
+ *
+ * DESCRIPTION
+ * This function initializes a semaphore. The
+ * initial value of the semaphore is set to 'value'.
+ *
+ * RESULTS
+ * 0 successfully created semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore, or
+ * 'value' >= SEM_VALUE_MAX
+ * ENOMEM out of memory,
+ * ENOSPC a required resource has been exhausted,
+ * ENOSYS semaphores are not supported,
+ * EPERM the process lacks appropriate privilege
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = NULL;
+
+ if (pshared != 0)
+ {
+ /*
+ * Creating a semaphore that can be shared between
+ * processes
+ */
+ result = EPERM;
+ }
+ else if (value > (unsigned int)SEM_VALUE_MAX)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ s = (sem_t) calloc (1, sizeof (*s));
+
+ if (NULL == s)
+ {
+ result = ENOMEM;
+ }
+ else
+ {
+
+ s->value = value;
+ if (pthread_mutex_init(&s->lock, NULL) == 0)
+ {
+
+#if defined(NEED_SEM)
+
+ s->sem = CreateEvent (NULL,
+ PTW32_FALSE, /* auto (not manual) reset */
+ PTW32_FALSE, /* initial state is unset */
+ NULL);
+
+ if (0 == s->sem)
+ {
+ free (s);
+ (void) pthread_mutex_destroy(&s->lock);
+ result = ENOSPC;
+ }
+ else
+ {
+ s->leftToUnblock = 0;
+ }
+
+#else /* NEED_SEM */
+
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if ((s->sem = CreateSemaphoreEx(NULL, /* Always NULL */
+ (long)0, /* Force threads to wait */
+ (long)SEM_VALUE_MAX, /* Maximum value */
+ NULL, 0, EVENT_ALL_ACCESS)) == 0) /* Name */
+# else
+ if ((s->sem = CreateSemaphore (NULL, /* Always NULL */
+ (long) 0, /* Force threads to wait */
+ (long) SEM_VALUE_MAX, /* Maximum value */
+ NULL)) == 0) /* Name */
+# endif
+#endif
+ {
+ (void) pthread_mutex_destroy(&s->lock);
+ result = ENOSPC;
+ }
+
+#endif /* NEED_SEM */
+
+ }
+ else
+ {
+ result = ENOSPC;
+ }
+
+ if (result != 0)
+ {
+ free(s);
+ }
+ }
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ *sem = s;
+
+ return 0;
+
+} /* sem_init */
diff --git a/3rdparty/pthreads-win32/sem_open.c b/3rdparty/pthreads-win32/sem_open.c
new file mode 100644
index 0000000..fb1cc54
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_open.c
@@ -0,0 +1,58 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_open.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+sem_open (const char *name, int oflag, mode_t mode, unsigned int value)
+{
+ errno = ENOSYS;
+ return -1;
+} /* sem_open */
diff --git a/3rdparty/pthreads-win32/sem_post.c b/3rdparty/pthreads-win32/sem_post.c
new file mode 100644
index 0000000..3483252
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_post.c
@@ -0,0 +1,128 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_post.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+int
+sem_post (sem_t * sem)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function posts a wakeup to a semaphore.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * DESCRIPTION
+ * This function posts a wakeup to a semaphore. If there
+ * are waiting threads (or processes), one is awakened;
+ * otherwise, the semaphore value is incremented by one.
+ *
+ * RESULTS
+ * 0 successfully posted semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS semaphores are not supported,
+ * ERANGE semaphore count is too big
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = *sem;
+
+ if (s == NULL)
+ {
+ result = EINVAL;
+ }
+ else if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ result = EINVAL;
+ return -1;
+ }
+
+ if (s->value < SEM_VALUE_MAX)
+ {
+#if defined(NEED_SEM)
+ if (++s->value <= 0
+ && !SetEvent(s->sem))
+ {
+ s->value--;
+ result = EINVAL;
+ }
+#else
+ if (++s->value <= 0
+ && !ReleaseSemaphore (s->sem, 1, NULL))
+ {
+ s->value--;
+ result = EINVAL;
+ }
+#endif /* NEED_SEM */
+ }
+ else
+ {
+ result = ERANGE;
+ }
+
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ return 0;
+
+} /* sem_post */
diff --git a/3rdparty/pthreads-win32/sem_post_multiple.c b/3rdparty/pthreads-win32/sem_post_multiple.c
new file mode 100644
index 0000000..44c168c
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_post_multiple.c
@@ -0,0 +1,142 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_post_multiple.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+int
+sem_post_multiple (sem_t * sem, int count)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function posts multiple wakeups to a semaphore.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * count
+ * counter, must be greater than zero.
+ *
+ * DESCRIPTION
+ * This function posts multiple wakeups to a semaphore. If there
+ * are waiting threads (or processes), n <= count are awakened;
+ * the semaphore value is incremented by count - n.
+ *
+ * RESULTS
+ * 0 successfully posted semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore
+ * or count is less than or equal to zero.
+ * ERANGE semaphore count is too big
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ long waiters;
+ sem_t s = *sem;
+
+ if (s == NULL || count <= 0)
+ {
+ result = EINVAL;
+ }
+ else if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ result = EINVAL;
+ return -1;
+ }
+
+ if (s->value <= (SEM_VALUE_MAX - count))
+ {
+ waiters = -s->value;
+ s->value += count;
+ if (waiters > 0)
+ {
+#if defined(NEED_SEM)
+ if (SetEvent(s->sem))
+ {
+ waiters--;
+ s->leftToUnblock += count - 1;
+ if (s->leftToUnblock > waiters)
+ {
+ s->leftToUnblock = waiters;
+ }
+ }
+#else
+ if (ReleaseSemaphore (s->sem, (waiters<=count)?waiters:count, 0))
+ {
+ /* No action */
+ }
+#endif
+ else
+ {
+ s->value -= count;
+ result = EINVAL;
+ }
+ }
+ }
+ else
+ {
+ result = ERANGE;
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ return 0;
+
+} /* sem_post_multiple */
diff --git a/3rdparty/pthreads-win32/sem_timedwait.c b/3rdparty/pthreads-win32/sem_timedwait.c
new file mode 100644
index 0000000..57f131a
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_timedwait.c
@@ -0,0 +1,244 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_timedwait.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+typedef struct {
+ sem_t sem;
+ int * resultPtr;
+} sem_timedwait_cleanup_args_t;
+
+
+static void PTW32_CDECL
+ptw32_sem_timedwait_cleanup (void * args)
+{
+ sem_timedwait_cleanup_args_t * a = (sem_timedwait_cleanup_args_t *)args;
+ sem_t s = a->sem;
+
+ if (pthread_mutex_lock (&s->lock) == 0)
+ {
+ /*
+ * We either timed out or were cancelled.
+ * If someone has posted between then and now we try to take the semaphore.
+ * Otherwise the semaphore count may be wrong after we
+ * return. In the case of a cancellation, it is as if we
+ * were cancelled just before we return (after taking the semaphore)
+ * which is ok.
+ */
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (WaitForSingleObjectEx(s->sem, 0, FALSE) == WAIT_OBJECT_0)
+# else
+ if (WaitForSingleObject(s->sem, 0) == WAIT_OBJECT_0)
+# endif
+#endif
+ {
+ /* We got the semaphore on the second attempt */
+ *(a->resultPtr) = 0;
+ }
+ else
+ {
+ /* Indicate we're no longer waiting */
+ s->value++;
+#if defined(NEED_SEM)
+ if (s->value > 0)
+ {
+ s->leftToUnblock = 0;
+ }
+#else
+ /*
+ * Don't release the W32 sema, it doesn't need adjustment
+ * because it doesn't record the number of waiters.
+ */
+#endif
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+}
+
+
+int
+sem_timedwait (sem_t * sem, const struct timespec *abstime)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function waits on a semaphore possibly until
+ * 'abstime' time.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * abstime
+ * pointer to an instance of struct timespec
+ *
+ * DESCRIPTION
+ * This function waits on a semaphore. If the
+ * semaphore value is greater than zero, it decreases
+ * its value by one. If the semaphore value is zero, then
+ * the calling thread (or process) is blocked until it can
+ * successfully decrease the value or until interrupted by
+ * a signal.
+ *
+ * If 'abstime' is a NULL pointer then this function will
+ * block until it can successfully decrease the value or
+ * until interrupted by a signal.
+ *
+ * RESULTS
+ * 0 successfully decreased semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS semaphores are not supported,
+ * EINTR the function was interrupted by a signal,
+ * EDEADLK a deadlock condition was detected.
+ * ETIMEDOUT abstime elapsed before success.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = *sem;
+
+ pthread_testcancel();
+
+ if (sem == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ DWORD milliseconds;
+
+ if (abstime == NULL)
+ {
+ milliseconds = INFINITE;
+ }
+ else
+ {
+ /*
+ * Calculate timeout as milliseconds from current system time.
+ */
+ milliseconds = ptw32_relmillisecs (abstime);
+ }
+
+ if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ int v;
+
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ v = --s->value;
+ (void) pthread_mutex_unlock (&s->lock);
+
+ if (v < 0)
+ {
+#if defined(NEED_SEM)
+ int timedout;
+#endif
+ sem_timedwait_cleanup_args_t cleanup_args;
+
+ cleanup_args.sem = s;
+ cleanup_args.resultPtr = &result;
+
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+ /* Must wait */
+ pthread_cleanup_push(ptw32_sem_timedwait_cleanup, (void *) &cleanup_args);
+#if defined(NEED_SEM)
+ timedout =
+#endif
+ result = pthreadCancelableTimedWait (s->sem, milliseconds);
+ pthread_cleanup_pop(result);
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+
+#if defined(NEED_SEM)
+
+ if (!timedout && pthread_mutex_lock (&s->lock) == 0)
+ {
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ if (s->leftToUnblock > 0)
+ {
+ --s->leftToUnblock;
+ SetEvent(s->sem);
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+
+#endif /* NEED_SEM */
+
+ }
+ }
+
+ }
+
+ if (result != 0)
+ {
+
+ errno = result;
+ return -1;
+
+ }
+
+ return 0;
+
+} /* sem_timedwait */
diff --git a/3rdparty/pthreads-win32/sem_trywait.c b/3rdparty/pthreads-win32/sem_trywait.c
new file mode 100644
index 0000000..63614ba
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_trywait.c
@@ -0,0 +1,117 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_trywait.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+int
+sem_trywait (sem_t * sem)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function tries to wait on a semaphore.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * DESCRIPTION
+ * This function tries to wait on a semaphore. If the
+ * semaphore value is greater than zero, it decreases
+ * its value by one. If the semaphore value is zero, then
+ * this function returns immediately with the error EAGAIN
+ *
+ * RESULTS
+ * 0 successfully decreased semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EAGAIN the semaphore was already locked,
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOTSUP sem_trywait is not supported,
+ * EINTR the function was interrupted by a signal,
+ * EDEADLK a deadlock condition was detected.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = *sem;
+
+ if (s == NULL)
+ {
+ result = EINVAL;
+ }
+ else if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ if (s->value > 0)
+ {
+ s->value--;
+ }
+ else
+ {
+ result = EAGAIN;
+ }
+
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ return 0;
+
+} /* sem_trywait */
diff --git a/3rdparty/pthreads-win32/sem_unlink.c b/3rdparty/pthreads-win32/sem_unlink.c
new file mode 100644
index 0000000..fb80569
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_unlink.c
@@ -0,0 +1,58 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_unlink.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+/* ignore warning "unreferenced formal parameter" */
+#if defined(_MSC_VER)
+#pragma warning( disable : 4100 )
+#endif
+
+int
+sem_unlink (const char *name)
+{
+ errno = ENOSYS;
+ return -1;
+} /* sem_unlink */
diff --git a/3rdparty/pthreads-win32/sem_wait.c b/3rdparty/pthreads-win32/sem_wait.c
new file mode 100644
index 0000000..0c25de3
--- /dev/null
+++ b/3rdparty/pthreads-win32/sem_wait.c
@@ -0,0 +1,193 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: sem_wait.c
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+static void PTW32_CDECL
+ptw32_sem_wait_cleanup(void * sem)
+{
+ sem_t s = (sem_t) sem;
+
+ if (pthread_mutex_lock (&s->lock) == 0)
+ {
+ /*
+ * If sema is destroyed do nothing, otherwise:-
+ * If the sema is posted between us being cancelled and us locking
+ * the sema again above then we need to consume that post but cancel
+ * anyway. If we don't get the semaphore we indicate that we're no
+ * longer waiting.
+ */
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ if (*((sem_t *)sem) != NULL && !(WaitForSingleObjectEx(s->sem, 0, FALSE) == WAIT_OBJECT_0))
+# else
+ if (*((sem_t *)sem) != NULL && !(WaitForSingleObject(s->sem, 0) == WAIT_OBJECT_0))
+# endif
+#endif
+ {
+ ++s->value;
+#if defined(NEED_SEM)
+ if (s->value > 0)
+ {
+ s->leftToUnblock = 0;
+ }
+#else
+ /*
+ * Don't release the W32 sema, it doesn't need adjustment
+ * because it doesn't record the number of waiters.
+ */
+#endif /* NEED_SEM */
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+}
+
+int
+sem_wait (sem_t * sem)
+ /*
+ * ------------------------------------------------------
+ * DOCPUBLIC
+ * This function waits on a semaphore.
+ *
+ * PARAMETERS
+ * sem
+ * pointer to an instance of sem_t
+ *
+ * DESCRIPTION
+ * This function waits on a semaphore. If the
+ * semaphore value is greater than zero, it decreases
+ * its value by one. If the semaphore value is zero, then
+ * the calling thread (or process) is blocked until it can
+ * successfully decrease the value or until interrupted by
+ * a signal.
+ *
+ * RESULTS
+ * 0 successfully decreased semaphore,
+ * -1 failed, error in errno
+ * ERRNO
+ * EINVAL 'sem' is not a valid semaphore,
+ * ENOSYS semaphores are not supported,
+ * EINTR the function was interrupted by a signal,
+ * EDEADLK a deadlock condition was detected.
+ *
+ * ------------------------------------------------------
+ */
+{
+ int result = 0;
+ sem_t s = *sem;
+
+ pthread_testcancel();
+
+ if (s == NULL)
+ {
+ result = EINVAL;
+ }
+ else
+ {
+ if ((result = pthread_mutex_lock (&s->lock)) == 0)
+ {
+ int v;
+
+ /* See sem_destroy.c
+ */
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ v = --s->value;
+ (void) pthread_mutex_unlock (&s->lock);
+
+ if (v < 0)
+ {
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth(0)
+#endif
+ /* Must wait */
+ pthread_cleanup_push(ptw32_sem_wait_cleanup, (void *) s);
+ result = pthreadCancelableWait (s->sem);
+ /* Cleanup if we're canceled or on any other error */
+ pthread_cleanup_pop(result);
+#if defined(_MSC_VER) && _MSC_VER < 1400
+#pragma inline_depth()
+#endif
+ }
+#if defined(NEED_SEM)
+
+ if (!result && pthread_mutex_lock (&s->lock) == 0)
+ {
+ if (*sem == NULL)
+ {
+ (void) pthread_mutex_unlock (&s->lock);
+ errno = EINVAL;
+ return -1;
+ }
+
+ if (s->leftToUnblock > 0)
+ {
+ --s->leftToUnblock;
+ SetEvent(s->sem);
+ }
+ (void) pthread_mutex_unlock (&s->lock);
+ }
+
+#endif /* NEED_SEM */
+
+ }
+
+ }
+
+ if (result != 0)
+ {
+ errno = result;
+ return -1;
+ }
+
+ return 0;
+
+} /* sem_wait */
diff --git a/3rdparty/pthreads-win32/semaphore.c b/3rdparty/pthreads-win32/semaphore.c
new file mode 100644
index 0000000..64fc0e3
--- /dev/null
+++ b/3rdparty/pthreads-win32/semaphore.c
@@ -0,0 +1,69 @@
+/*
+ * -------------------------------------------------------------
+ *
+ * Module: semaphore.c
+ *
+ * Purpose:
+ * Concatenated version of separate modules to allow
+ * inlining optimisation, which it is assumed can only
+ * be effective within a single module.
+ *
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * -------------------------------------------------------------
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#if !defined(NEED_FTIME)
+# include <sys/timeb.h>
+#endif
+
+#include <limits.h>
+
+#include "pthread.h"
+#include "semaphore.h"
+#include "implement.h"
+
+
+#include "sem_init.c"
+#include "sem_destroy.c"
+#include "sem_trywait.c"
+#include "sem_wait.c"
+#include "sem_timedwait.c"
+#include "sem_post.c"
+#include "sem_post_multiple.c"
+#include "sem_getvalue.c"
+#include "sem_open.c"
+#include "sem_close.c"
+#include "sem_unlink.c"
diff --git a/3rdparty/pthreads-win32/semaphore.h b/3rdparty/pthreads-win32/semaphore.h
new file mode 100644
index 0000000..c6e9407
--- /dev/null
+++ b/3rdparty/pthreads-win32/semaphore.h
@@ -0,0 +1,169 @@
+/*
+ * Module: semaphore.h
+ *
+ * Purpose:
+ * Semaphores aren't actually part of the PThreads standard.
+ * They are defined by the POSIX Standard:
+ *
+ * POSIX 1003.1b-1993 (POSIX.1b)
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+#if !defined( SEMAPHORE_H )
+#define SEMAPHORE_H
+
+#undef PTW32_SEMAPHORE_LEVEL
+
+#if defined(_POSIX_SOURCE)
+#define PTW32_SEMAPHORE_LEVEL 0
+/* Early POSIX */
+#endif
+
+#if defined(_POSIX_C_SOURCE) && _POSIX_C_SOURCE >= 199309
+#undef PTW32_SEMAPHORE_LEVEL
+#define PTW32_SEMAPHORE_LEVEL 1
+/* Include 1b, 1c and 1d */
+#endif
+
+#if defined(INCLUDE_NP)
+#undef PTW32_SEMAPHORE_LEVEL
+#define PTW32_SEMAPHORE_LEVEL 2
+/* Include Non-Portable extensions */
+#endif
+
+#define PTW32_SEMAPHORE_LEVEL_MAX 3
+
+#if !defined(PTW32_SEMAPHORE_LEVEL)
+#define PTW32_SEMAPHORE_LEVEL PTW32_SEMAPHORE_LEVEL_MAX
+/* Include everything */
+#endif
+
+#if defined(__GNUC__) && ! defined (__declspec)
+# error Please upgrade your GNU compiler to one that supports __declspec.
+#endif
+
+/*
+ * When building the library, you should define PTW32_BUILD so that
+ * the variables/functions are exported correctly. When using the library,
+ * do NOT define PTW32_BUILD, and then the variables/functions will
+ * be imported correctly.
+ */
+#if !defined(PTW32_STATIC_LIB)
+# if defined(PTW32_BUILD)
+# define PTW32_DLLPORT __declspec (dllexport)
+# else
+# define PTW32_DLLPORT __declspec (dllimport)
+# endif
+#else
+# define PTW32_DLLPORT
+#endif
+
+/*
+ * This is a duplicate of what is in the autoconf config.h,
+ * which is only used when building the pthread-win32 libraries.
+ */
+
+#if !defined(PTW32_CONFIG_H)
+# if defined(WINCE)
+# define NEED_ERRNO
+# define NEED_SEM
+# endif
+# if defined(__MINGW64__)
+# define HAVE_STRUCT_TIMESPEC
+# define HAVE_MODE_T
+# elif defined(_UWIN) || defined(__MINGW32__)
+# define HAVE_MODE_T
+# endif
+#endif
+
+/*
+ *
+ */
+
+#if PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX
+#if defined(NEED_ERRNO)
+#include "need_errno.h"
+#else
+#include <errno.h>
+#endif
+#endif /* PTW32_SEMAPHORE_LEVEL >= PTW32_SEMAPHORE_LEVEL_MAX */
+
+#define _POSIX_SEMAPHORES
+
+#if defined(__cplusplus)
+extern "C"
+{
+#endif /* __cplusplus */
+
+#if !defined(HAVE_MODE_T)
+typedef unsigned int mode_t;
+#endif
+
+
+typedef struct sem_t_ * sem_t;
+
+PTW32_DLLPORT int __cdecl sem_init (sem_t * sem,
+ int pshared,
+ unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_destroy (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_trywait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_wait (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_timedwait (sem_t * sem,
+ const struct timespec * abstime);
+
+PTW32_DLLPORT int __cdecl sem_post (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_post_multiple (sem_t * sem,
+ int count);
+
+PTW32_DLLPORT int __cdecl sem_open (const char * name,
+ int oflag,
+ mode_t mode,
+ unsigned int value);
+
+PTW32_DLLPORT int __cdecl sem_close (sem_t * sem);
+
+PTW32_DLLPORT int __cdecl sem_unlink (const char * name);
+
+PTW32_DLLPORT int __cdecl sem_getvalue (sem_t * sem,
+ int * sval);
+
+#if defined(__cplusplus)
+} /* End of extern "C" */
+#endif /* __cplusplus */
+
+#undef PTW32_SEMAPHORE_LEVEL
+#undef PTW32_SEMAPHORE_LEVEL_MAX
+
+#endif /* !SEMAPHORE_H */
diff --git a/3rdparty/pthreads-win32/signal.c b/3rdparty/pthreads-win32/signal.c
new file mode 100644
index 0000000..eef4669
--- /dev/null
+++ b/3rdparty/pthreads-win32/signal.c
@@ -0,0 +1,179 @@
+/*
+ * signal.c
+ *
+ * Description:
+ * Thread-aware signal functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+/*
+ * Possible future strategy for implementing pthread_kill()
+ * ========================================================
+ *
+ * Win32 does not implement signals.
+ * Signals are simply software interrupts.
+ * pthread_kill() asks the system to deliver a specified
+ * signal (interrupt) to a specified thread in the same
+ * process.
+ * Signals are always asynchronous (no deferred signals).
+ * Pthread-win32 has an async cancelation mechanism.
+ * A similar system can be written to deliver signals
+ * within the same process (on ix86 processors at least).
+ *
+ * Each thread maintains information about which
+ * signals it will respond to. Handler routines
+ * are set on a per-process basis - not per-thread.
+ * When signalled, a thread will check it's sigmask
+ * and, if the signal is not being ignored, call the
+ * handler routine associated with the signal. The
+ * thread must then (except for some signals) return to
+ * the point where it was interrupted.
+ *
+ * Ideally the system itself would check the target thread's
+ * mask before possibly needlessly bothering the thread
+ * itself. This could be done by pthread_kill(), that is,
+ * in the signaling thread since it has access to
+ * all pthread_t structures. It could also retrieve
+ * the handler routine address to minimise the target
+ * threads response overhead. This may also simplify
+ * serialisation of the access to the per-thread signal
+ * structures.
+ *
+ * pthread_kill() eventually calls a routine similar to
+ * ptw32_cancel_thread() which manipulates the target
+ * threads processor context to cause the thread to
+ * run the handler launcher routine. pthread_kill() must
+ * save the target threads current context so that the
+ * handler launcher routine can restore the context after
+ * the signal handler has returned. Some handlers will not
+ * return, eg. the default SIGKILL handler may simply
+ * call pthread_exit().
+ *
+ * The current context is saved in the target threads
+ * pthread_t structure.
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+#if defined(HAVE_SIGSET_T)
+
+static void
+ptw32_signal_thread ()
+{
+}
+
+static void
+ptw32_signal_callhandler ()
+{
+}
+
+int
+pthread_sigmask (int how, sigset_t const *set, sigset_t * oset)
+{
+ pthread_t thread = pthread_self ();
+
+ if (thread.p == NULL)
+ {
+ return ENOENT;
+ }
+
+ /* Validate the `how' argument. */
+ if (set != NULL)
+ {
+ switch (how)
+ {
+ case SIG_BLOCK:
+ break;
+ case SIG_UNBLOCK:
+ break;
+ case SIG_SETMASK:
+ break;
+ default:
+ /* Invalid `how' argument. */
+ return EINVAL;
+ }
+ }
+
+ /* Copy the old mask before modifying it. */
+ if (oset != NULL)
+ {
+ memcpy (oset, &(thread.p->sigmask), sizeof (sigset_t));
+ }
+
+ if (set != NULL)
+ {
+ unsigned int i;
+
+ /* FIXME: this code assumes that sigmask is an even multiple of
+ the size of a long integer. */
+
+ unsigned long *src = (unsigned long const *) set;
+ unsigned long *dest = (unsigned long *) &(thread.p->sigmask);
+
+ switch (how)
+ {
+ case SIG_BLOCK:
+ for (i = 0; i < (sizeof (sigset_t) / sizeof (unsigned long)); i++)
+ {
+ /* OR the bit field longword-wise. */
+ *dest++ |= *src++;
+ }
+ break;
+ case SIG_UNBLOCK:
+ for (i = 0; i < (sizeof (sigset_t) / sizeof (unsigned long)); i++)
+ {
+ /* XOR the bitfield longword-wise. */
+ *dest++ ^= *src++;
+ }
+ case SIG_SETMASK:
+ /* Replace the whole sigmask. */
+ memcpy (&(thread.p->sigmask), set, sizeof (sigset_t));
+ break;
+ }
+ }
+
+ return 0;
+}
+
+int
+sigwait (const sigset_t * set, int *sig)
+{
+ /* This routine is a cancellation point */
+ pthread_test_cancel();
+}
+
+int
+sigaction (int signum, const struct sigaction *act, struct sigaction *oldact)
+{
+}
+
+#endif /* HAVE_SIGSET_T */
diff --git a/3rdparty/pthreads-win32/spin.c b/3rdparty/pthreads-win32/spin.c
new file mode 100644
index 0000000..41b5aa5
--- /dev/null
+++ b/3rdparty/pthreads-win32/spin.c
@@ -0,0 +1,46 @@
+/*
+ * spin.c
+ *
+ * Description:
+ * This translation unit implements spin lock primitives.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "ptw32_spinlock_check_need_init.c"
+#include "pthread_spin_init.c"
+#include "pthread_spin_destroy.c"
+#include "pthread_spin_lock.c"
+#include "pthread_spin_unlock.c"
+#include "pthread_spin_trylock.c"
diff --git a/3rdparty/pthreads-win32/sync.c b/3rdparty/pthreads-win32/sync.c
new file mode 100644
index 0000000..5e56fa9
--- /dev/null
+++ b/3rdparty/pthreads-win32/sync.c
@@ -0,0 +1,43 @@
+/*
+ * sync.c
+ *
+ * Description:
+ * This translation unit implements functions related to thread
+ * synchronisation.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "pthread_detach.c"
+#include "pthread_join.c"
diff --git a/3rdparty/pthreads-win32/tsd.c b/3rdparty/pthreads-win32/tsd.c
new file mode 100644
index 0000000..ed44fe6
--- /dev/null
+++ b/3rdparty/pthreads-win32/tsd.c
@@ -0,0 +1,44 @@
+/*
+ * tsd.c
+ *
+ * Description:
+ * POSIX thread functions which implement thread-specific data (TSD).
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+#include "pthread_key_create.c"
+#include "pthread_key_delete.c"
+#include "pthread_setspecific.c"
+#include "pthread_getspecific.c"
diff --git a/3rdparty/pthreads-win32/version.rc b/3rdparty/pthreads-win32/version.rc
new file mode 100644
index 0000000..02ade07
--- /dev/null
+++ b/3rdparty/pthreads-win32/version.rc
@@ -0,0 +1,394 @@
+/* This is an implementation of the threads API of POSIX 1003.1-2001.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include <winver.h>
+#include "pthread.h"
+
+/*
+ * Note: the correct __CLEANUP_* macro must be defined corresponding to
+ * the definition used for the object file builds. This is done in the
+ * relevent makefiles for the command line builds, but users should ensure
+ * that their resource compiler knows what it is too.
+ * If using the default (no __CLEANUP_* defined), pthread.h will define it
+ * as __CLEANUP_C.
+ */
+
+#if defined(__MINGW64__)
+# define PTW32_ARCH "x64 (mingw64)"
+#elif defined (__MINGW32__)
+# define PTW32_ARCH "x86 (mingw32)"
+#endif
+#if defined(PTW32_ARCHx64)
+# define PTW32_ARCH "x64"
+#elif defined(PTW32_ARCHx86)
+# define PTW32_ARCH "x86"
+#endif
+
+#if defined(PTW32_RC_MSC)
+# if defined(__CLEANUP_C)
+# define PTW32_VERSIONINFO_NAME "pthreadVC2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "MS C " PTW32_ARCH "\0"
+# elif defined(__CLEANUP_CXX)
+# define PTW32_VERSIONINFO_NAME "pthreadVCE2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "MS C++ " PTW32_ARCH "\0"
+# elif defined(__CLEANUP_SEH)
+# define PTW32_VERSIONINFO_NAME "pthreadVSE2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "MS C SEH " PTW32_ARCH "\0"
+# else
+# error Resource compiler doesn't know which cleanup style you're using - see version.rc
+# endif
+#elif defined(__GNUC__)
+# if defined(__CLEANUP_C)
+# define PTW32_VERSIONINFO_NAME "pthreadGC2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "GNU C " PTW32_ARCH "\0"
+# elif defined(__CLEANUP_CXX)
+# define PTW32_VERSIONINFO_NAME "pthreadGCE2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "GNU C++ " PTW32_ARCH "\0"
+# else
+# error Resource compiler doesn't know which cleanup style you're using - see version.rc
+# endif
+#elif defined(__BORLANDC__)
+# if defined(__CLEANUP_C)
+# define PTW32_VERSIONINFO_NAME "pthreadBC2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "BORLAND C " PTW32_ARCH "\0"
+# elif defined(__CLEANUP_CXX)
+# define PTW32_VERSIONINFO_NAME "pthreadBCE2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "BORLAND C++ " PTW32_ARCH "\0"
+# else
+# error Resource compiler doesn't know which cleanup style you're using - see version.rc
+# endif
+#elif defined(__WATCOMC__)
+# if defined(__CLEANUP_C)
+# define PTW32_VERSIONINFO_NAME "pthreadWC2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "WATCOM C " PTW32_ARCH "\0"
+# elif defined(__CLEANUP_CXX)
+# define PTW32_VERSIONINFO_NAME "pthreadWCE2.DLL\0"
+# define PTW32_VERSIONINFO_DESCRIPTION "WATCOM C++ " PTW32_ARCH "\0"
+# else
+# error Resource compiler doesn't know which cleanup style you're using - see version.rc
+# endif
+#else
+# error Resource compiler doesn't know which compiler you're using - see version.rc
+#endif
+
+
+VS_VERSION_INFO VERSIONINFO
+ FILEVERSION PTW32_VERSION
+ PRODUCTVERSION PTW32_VERSION
+ FILEFLAGSMASK VS_FFI_FILEFLAGSMASK
+ FILEFLAGS 0
+ FILEOS VOS__WINDOWS32
+ FILETYPE VFT_DLL
+BEGIN
+ BLOCK "StringFileInfo"
+ BEGIN
+ BLOCK "040904b0"
+ BEGIN
+ VALUE "ProductName", "POSIX Threads for Windows LPGL\0"
+ VALUE "ProductVersion", PTW32_VERSION_STRING
+ VALUE "FileVersion", PTW32_VERSION_STRING
+ VALUE "FileDescription", PTW32_VERSIONINFO_DESCRIPTION
+ VALUE "InternalName", PTW32_VERSIONINFO_NAME
+ VALUE "OriginalFilename", PTW32_VERSIONINFO_NAME
+ VALUE "CompanyName", "Open Source Software community LGPL\0"
+ VALUE "LegalCopyright", "Copyright (C) Project contributors 2012\0"
+ VALUE "Comments", "http://sourceware.org/pthreads-win32/\0"
+ END
+ END
+ BLOCK "VarFileInfo"
+ BEGIN
+ VALUE "Translation", 0x409, 1200
+ END
+END
+
+/*
+VERSIONINFO Resource
+
+The VERSIONINFO resource-definition statement creates a version-information
+resource. The resource contains such information about the file as its
+version number, its intended operating system, and its original filename.
+The resource is intended to be used with the Version Information functions.
+
+versionID VERSIONINFO fixed-info { block-statement...}
+
+versionID
+ Version-information resource identifier. This value must be 1.
+
+fixed-info
+ Version information, such as the file version and the intended operating
+ system. This parameter consists of the following statements.
+
+
+ Statement Description
+ --------------------------------------------------------------------------
+ FILEVERSION
+ version Binary version number for the file. The version
+ consists of two 32-bit integers, defined by four
+ 16-bit integers. For example, "FILEVERSION 3,10,0,61"
+ is translated into two doublewords: 0x0003000a and
+ 0x0000003d, in that order. Therefore, if version is
+ defined by the DWORD values dw1 and dw2, they need
+ to appear in the FILEVERSION statement as follows:
+ HIWORD(dw1), LOWORD(dw1), HIWORD(dw2), LOWORD(dw2).
+ PRODUCTVERSION
+ version Binary version number for the product with which the
+ file is distributed. The version parameter is two
+ 32-bit integers, defined by four 16-bit integers.
+ For more information about version, see the
+ FILEVERSION description.
+ FILEFLAGSMASK
+ fileflagsmask Bits in the FILEFLAGS statement are valid. If a bit
+ is set, the corresponding bit in FILEFLAGS is valid.
+ FILEFLAGSfileflags Attributes of the file. The fileflags parameter must
+ be the combination of all the file flags that are
+ valid at compile time. For 16-bit Windows, this
+ value is 0x3f.
+ FILEOSfileos Operating system for which this file was designed.
+ The fileos parameter can be one of the operating
+ system values given in the Remarks section.
+ FILETYPEfiletype General type of file. The filetype parameter can be
+ one of the file type values listed in the Remarks
+ section.
+ FILESUBTYPE
+ subtype Function of the file. The subtype parameter is zero
+ unless the type parameter in the FILETYPE statement
+ is VFT_DRV, VFT_FONT, or VFT_VXD. For a list of file
+ subtype values, see the Remarks section.
+
+block-statement
+ Specifies one or more version-information blocks. A block can contain
+ string information or variable information. For more information, see
+ StringFileInfo Block or VarFileInfo Block.
+
+Remarks
+
+To use the constants specified with the VERSIONINFO statement, you must
+include the Winver.h or Windows.h header file in the resource-definition file.
+
+The following list describes the parameters used in the VERSIONINFO statement:
+
+fileflags
+ A combination of the following values.
+
+ Value Description
+
+ VS_FF_DEBUG File contains debugging information or is compiled
+ with debugging features enabled.
+ VS_FF_PATCHED File has been modified and is not identical to the
+ original shipping file of the same version number.
+ VS_FF_PRERELEASE File is a development version, not a commercially
+ released product.
+ VS_FF_PRIVATEBUILD File was not built using standard release procedures.
+ If this value is given, the StringFileInfo block must
+ contain a PrivateBuild string.
+ VS_FF_SPECIALBUILD File was built by the original company using standard
+ release procedures but is a variation of the standard
+ file of the same version number. If this value is
+ given, the StringFileInfo block must contain a
+ SpecialBuild string.
+
+fileos
+ One of the following values.
+
+ Value Description
+
+ VOS_UNKNOWN The operating system for which the file was designed
+ is unknown.
+ VOS_DOS File was designed for MS-DOS.
+ VOS_NT File was designed for Windows Server 2003 family,
+ Windows XP, Windows 2000, or Windows NT.
+ VOS__WINDOWS16 File was designed for 16-bit Windows.
+ VOS__WINDOWS32 File was designed for 32-bit Windows.
+ VOS_DOS_WINDOWS16 File was designed for 16-bit Windows running with
+ MS-DOS.
+ VOS_DOS_WINDOWS32 File was designed for 32-bit Windows running with
+ MS-DOS.
+ VOS_NT_WINDOWS32 File was designed for Windows Server 2003 family,
+ Windows XP, Windows 2000, or Windows NT.
+
+ The values 0x00002L, 0x00003L, 0x20000L and 0x30000L are reserved.
+
+filetype
+ One of the following values.
+
+ Value Description
+
+ VFT_UNKNOWN File type is unknown.
+ VFT_APP File contains an application.
+ VFT_DLL File contains a dynamic-link library (DLL).
+ VFT_DRV File contains a device driver. If filetype is
+ VFT_DRV, subtype contains a more specific
+ description of the driver.
+ VFT_FONT File contains a font. If filetype is VFT_FONT,
+ subtype contains a more specific description of the
+ font.
+ VFT_VXD File contains a virtual device.
+ VFT_STATIC_LIB File contains a static-link library.
+
+ All other values are reserved for use by Microsoft.
+
+subtype
+ Additional information about the file type.
+
+ If filetype specifies VFT_DRV, this parameter can be one of the
+ following values.
+
+ Value Description
+
+ VFT2_UNKNOWN Driver type is unknown.
+ VFT2_DRV_COMM File contains a communications driver.
+ VFT2_DRV_PRINTER File contains a printer driver.
+ VFT2_DRV_KEYBOARD File contains a keyboard driver.
+ VFT2_DRV_LANGUAGE File contains a language driver.
+ VFT2_DRV_DISPLAY File contains a display driver.
+ VFT2_DRV_MOUSE File contains a mouse driver.
+ VFT2_DRV_NETWORK File contains a network driver.
+ VFT2_DRV_SYSTEM File contains a system driver.
+ VFT2_DRV_INSTALLABLE File contains an installable driver.
+ VFT2_DRV_SOUND File contains a sound driver.
+ VFT2_DRV_VERSIONED_PRINTER File contains a versioned printer driver.
+
+ If filetype specifies VFT_FONT, this parameter can be one of the
+ following values.
+
+ Value Description
+
+ VFT2_UNKNOWN Font type is unknown.
+ VFT2_FONT_RASTER File contains a raster font.
+ VFT2_FONT_VECTOR File contains a vector font.
+ VFT2_FONT_TRUETYPE File contains a TrueType font.
+
+ If filetype specifies VFT_VXD, this parameter must be the virtual-device
+ identifier included in the virtual-device control block.
+
+ All subtype values not listed here are reserved for use by Microsoft.
+
+langID
+ One of the following language codes.
+
+ Code Language Code Language
+
+ 0x0401 Arabic 0x0415 Polish
+ 0x0402 Bulgarian 0x0416 Portuguese (Brazil)
+ 0x0403 Catalan 0x0417 Rhaeto-Romanic
+ 0x0404 Traditional Chinese 0x0418 Romanian
+ 0x0405 Czech 0x0419 Russian
+ 0x0406 Danish 0x041A Croato-Serbian (Latin)
+ 0x0407 German 0x041B Slovak
+ 0x0408 Greek 0x041C Albanian
+ 0x0409 U.S. English 0x041D Swedish
+ 0x040A Castilian Spanish 0x041E Thai
+ 0x040B Finnish 0x041F Turkish
+ 0x040C French 0x0420 Urdu
+ 0x040D Hebrew 0x0421 Bahasa
+ 0x040E Hungarian 0x0804 Simplified Chinese
+ 0x040F Icelandic 0x0807 Swiss German
+ 0x0410 Italian 0x0809 U.K. English
+ 0x0411 Japanese 0x080A Mexican Spanish
+ 0x0412 Korean 0x080C Belgian French
+ 0x0413 Dutch 0x0C0C Canadian French
+ 0x0414 Norwegian – Bokmal 0x100C Swiss French
+ 0x0810 Swiss Italian 0x0816 Portuguese (Portugal)
+ 0x0813 Belgian Dutch 0x081A Serbo-Croatian (Cyrillic)
+ 0x0814 Norwegian – Nynorsk
+
+charsetID
+ One of the following character-set identifiers.
+
+ Identifier Character Set
+
+ 0 7-bit ASCII
+ 932 Japan (Shift %G–%@ JIS X-0208)
+ 949 Korea (Shift %G–%@ KSC 5601)
+ 950 Taiwan (Big5)
+ 1200 Unicode
+ 1250 Latin-2 (Eastern European)
+ 1251 Cyrillic
+ 1252 Multilingual
+ 1253 Greek
+ 1254 Turkish
+ 1255 Hebrew
+ 1256 Arabic
+
+string-name
+ One of the following predefined names.
+
+ Name Description
+
+ Comments Additional information that should be displayed for
+ diagnostic purposes.
+ CompanyName Company that produced the file%G—%@for example,
+ "Microsoft Corporation" or "Standard Microsystems
+ Corporation, Inc." This string is required.
+ FileDescription File description to be presented to users. This
+ string may be displayed in a list box when the user
+ is choosing files to install%G—%@for example,
+ "Keyboard Driver for AT-Style Keyboards". This
+ string is required.
+ FileVersion Version number of the file%G—%@for example,
+ "3.10" or "5.00.RC2". This string is required.
+ InternalName Internal name of the file, if one exists — for
+ example, a module name if the file is a dynamic-link
+ library. If the file has no internal name, this
+ string should be the original filename, without
+ extension. This string is required.
+ LegalCopyright Copyright notices that apply to the file. This
+ should include the full text of all notices, legal
+ symbols, copyright dates, and so on — for example,
+ "Copyright (C) Microsoft Corporation 1990–1999".
+ This string is optional.
+ LegalTrademarks Trademarks and registered trademarks that apply to
+ the file. This should include the full text of all
+ notices, legal symbols, trademark numbers, and so on.
+ This string is optional.
+ OriginalFilename Original name of the file, not including a path.
+ This information enables an application to determine
+ whether a file has been renamed by a user. The
+ format of the name depends on the file system for
+ which the file was created. This string is required.
+ PrivateBuild Information about a private version of the file — for
+ example, "Built by TESTER1 on \TESTBED". This string
+ should be present only if VS_FF_PRIVATEBUILD is
+ specified in the fileflags parameter of the root
+ block.
+ ProductName Name of the product with which the file is
+ distributed. This string is required.
+ ProductVersion Version of the product with which the file is
+ distributed — for example, "3.10" or "5.00.RC2".
+ This string is required.
+ SpecialBuild Text that indicates how this version of the file
+ differs from the standard version — for example,
+ "Private build for TESTER1 solving mouse problems
+ on M250 and M250E computers". This string should be
+ present only if VS_FF_SPECIALBUILD is specified in
+ the fileflags parameter of the root block.
+ */
diff --git a/3rdparty/pthreads-win32/w32_CancelableWait.c b/3rdparty/pthreads-win32/w32_CancelableWait.c
new file mode 100644
index 0000000..f2ed32d
--- /dev/null
+++ b/3rdparty/pthreads-win32/w32_CancelableWait.c
@@ -0,0 +1,167 @@
+/*
+ * w32_CancelableWait.c
+ *
+ * Description:
+ * This translation unit implements miscellaneous thread functions.
+ *
+ * --------------------------------------------------------------------------
+ *
+ * Pthreads-win32 - POSIX Threads Library for Win32
+ * Copyright(C) 1998 John E. Bossom
+ * Copyright(C) 1999,2005 Pthreads-win32 contributors
+ *
+ * Contact Email: rpj at callisto.canberra.edu.au
+ *
+ * The current list of contributors is contained
+ * in the file CONTRIBUTORS included with the source
+ * code distribution. The list can also be seen at the
+ * following World Wide Web location:
+ * http://sources.redhat.com/pthreads-win32/contributors.html
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library in the file COPYING.LIB;
+ * if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA
+ */
+
+#include "pthread.h"
+#include "implement.h"
+
+
+static INLINE int
+ptw32_cancelable_wait (HANDLE waitHandle, DWORD timeout)
+ /*
+ * -------------------------------------------------------------------
+ * This provides an extra hook into the pthread_cancel
+ * mechanism that will allow you to wait on a Windows handle and make it a
+ * cancellation point. This function blocks until the given WIN32 handle is
+ * signaled or pthread_cancel has been called. It is implemented using
+ * WaitForMultipleObjects on 'waitHandle' and a manually reset WIN32
+ * event used to implement pthread_cancel.
+ *
+ * Given this hook it would be possible to implement more of the cancellation
+ * points.
+ * -------------------------------------------------------------------
+ */
+{
+ int result;
+ pthread_t self;
+ ptw32_thread_t * sp;
+ HANDLE handles[2];
+ DWORD nHandles = 1;
+ DWORD status;
+
+ handles[0] = waitHandle;
+
+ self = pthread_self();
+ sp = (ptw32_thread_t *) self.p;
+
+ if (sp != NULL)
+ {
+ /*
+ * Get cancelEvent handle
+ */
+ if (sp->cancelState == PTHREAD_CANCEL_ENABLE)
+ {
+
+ if ((handles[1] = sp->cancelEvent) != NULL)
+ {
+ nHandles++;
+ }
+ }
+ }
+ else
+ {
+ handles[1] = NULL;
+ }
+
+#if defined(_WIN32)
+# if defined(WINRT_8_1)
+ status = WaitForMultipleObjectsEx(nHandles, handles, PTW32_FALSE, timeout, FALSE);
+# else
+ status = WaitForMultipleObjects (nHandles, handles, PTW32_FALSE, timeout);
+# endif
+#endif
+
+ switch (status - WAIT_OBJECT_0)
+ {
+ case 0:
+ /*
+ * Got the handle.
+ * In the event that both handles are signalled, the smallest index
+ * value (us) is returned. As it has been arranged, this ensures that
+ * we don't drop a signal that we should act on (i.e. semaphore,
+ * mutex, or condition variable etc).
+ */
+ result = 0;
+ break;
+
+ case 1:
+ /*
+ * Got cancel request.
+ * In the event that both handles are signaled, the cancel will
+ * be ignored (see case 0 comment).
+ */
+ ResetEvent (handles[1]);
+
+ if (sp != NULL)
+ {
+ ptw32_mcs_local_node_t stateLock;
+ /*
+ * Should handle POSIX and implicit POSIX threads..
+ * Make sure we haven't been async-canceled in the meantime.
+ */
+ ptw32_mcs_lock_acquire (&sp->stateLock, &stateLock);
+ if (sp->state < PThreadStateCanceling)
+ {
+ sp->state = PThreadStateCanceling;
+ sp->cancelState = PTHREAD_CANCEL_DISABLE;
+ ptw32_mcs_lock_release (&stateLock);
+ ptw32_throw (PTW32_EPS_CANCEL);
+
+ /* Never reached */
+ }
+ ptw32_mcs_lock_release (&stateLock);
+ }
+
+ /* Should never get to here. */
+ result = EINVAL;
+ break;
+
+ default:
+ if (status == WAIT_TIMEOUT)
+ {
+ result = ETIMEDOUT;
+ }
+ else
+ {
+ result = EINVAL;
+ }
+ break;
+ }
+
+ return (result);
+
+} /* CancelableWait */
+
+int
+pthreadCancelableWait (HANDLE waitHandle)
+{
+ return (ptw32_cancelable_wait (waitHandle, INFINITE));
+}
+
+int
+pthreadCancelableTimedWait (HANDLE waitHandle, DWORD timeout)
+{
+ return (ptw32_cancelable_wait (waitHandle, timeout));
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 694e188..d952c18 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -52,7 +53,7 @@ endif(CMAKE_SYSTEM_NAME MATCHES WindowsPhone OR CMAKE_SYSTEM_NAME MATCHES Window
if(WINRT)
add_definitions(-DWINRT)
-
+
if(CMAKE_SYSTEM_NAME MATCHES WindowsPhone)
set(WINRT_PHONE TRUE)
add_definitions(-DWINRT_PHONE)
@@ -85,11 +86,20 @@ if(CMAKE_TOOLCHAIN_FILE AND I_AM_A_ROBOT)
include(platforms/naoqi/cmake/extra.cmake)
endif()
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 2.8.12.2)
if(POLICY CMP0022)
cmake_policy(SET CMP0022 NEW) # Due to sensor ATIDAQ_LIBRARIES
endif()
+if(POLICY CMP0053)
+ cmake_policy(SET CMP0053 NEW) # For UsTK: VTK and Qt5
+endif()
+if(POLICY CMP0020)
+ cmake_policy(SET CMP0020 NEW) # For UsTK: Qt5
+endif()
+if(POLICY CMP0054)
+ cmake_policy(SET CMP0054 NEW) # To turn off a warning in native FindOpenMP with cmake 3.9.2
+endif()
list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
@@ -100,8 +110,8 @@ vp_clear_vars(VISPModules_TARGETS)
#-----------------------------------------------------------------------------
# VISP version number. An even minor number corresponds to releases.
set(VISP_VERSION_MAJOR "3")
-set(VISP_VERSION_MINOR "0")
-set(VISP_VERSION_PATCH "1")
+set(VISP_VERSION_MINOR "1")
+set(VISP_VERSION_PATCH "0")
set(VISP_VERSION "${VISP_VERSION_MAJOR}.${VISP_VERSION_MINOR}.${VISP_VERSION_PATCH}")
# Package revision number
set(VISP_REVISION "1")
@@ -179,7 +189,7 @@ include_directories(${VISP_INCLUDE_DIR})
#----------------------------------------------------------------------
VP_OPTION(ENABLE_SSE2 "" "" "Enable SSE2 instructions" "" ON IF ((MSVC OR CMAKE_COMPILER_IS_GNUCXX) AND (X86 OR X86_64)) )
VP_OPTION(ENABLE_SSE3 "" "" "Enable SSE3 instructions" "" ON IF ((MSVC OR CMAKE_COMPILER_IS_GNUCXX) AND (X86 OR X86_64)) )
-VP_OPTION(ENABLE_SSSE3 "" "" "Enable SSSE3 instructions" "" ON IF ((MSVC OR CMAKE_COMPILER_IS_GNUCXX) AND (X86 OR X86_64)) )
+VP_OPTION(ENABLE_SSSE3 "" "" "Enable SSSE3 instructions" "" ON IF ((MSVC OR CMAKE_COMPILER_IS_GNUCXX) AND (X86_64)) ) # X86 disabled since it produces an issue on Debian i386
# ----------------------------------------------------------------------------
# Handle always full RPATH
@@ -218,20 +228,24 @@ set(VISP_CONTRIB_MODULES_PATH "" CACHE PATH "Where to look for additional contri
# Get the OS
set(OS ${CMAKE_SYSTEM_NAME})
-set(OGRE_HOME $ENV{OGRE_HOME})
-if(OGRE_HOME)
+set(OGRE_DIR $ENV{OGRE_DIR})
+if(NOT OGRE_DIR) # For compat with previous ViSP versions
+ set(OGRE_DIR $ENV{OGRE_HOME})
+endif()
+if(OGRE_DIR)
# replace \ with / especially for windows
- STRING(REGEX REPLACE "\\\\" "/" OGRE_HOME ${OGRE_HOME})
+ STRING(REGEX REPLACE "\\\\" "/" OGRE_DIR ${OGRE_DIR})
endif()
# add the path to detect Ogre3D
if(WIN32)
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_DIR}/cmake")
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_DIR}/CMake")
endif(WIN32)
-
+
if(UNIX)
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/cmake")
- list(APPEND CMAKE_MODULE_PATH "${OGRE_HOME}/CMake")
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_DIR}/cmake")
+ list(APPEND CMAKE_MODULE_PATH "${OGRE_DIR}/CMake")
list(APPEND CMAKE_MODULE_PATH "/usr/local/lib/OGRE/cmake")
list(APPEND CMAKE_MODULE_PATH "/usr/lib/OGRE/cmake")
list(APPEND CMAKE_MODULE_PATH "/usr/local/lib64/OGRE/cmake")
@@ -253,12 +267,29 @@ VP_OPTION(BUILD_COVERAGE "" "" "Enables test coverage" "" OFF IF (BUILD_TESTS AN
# Build demos as an option.
VP_OPTION(BUILD_DEMOS "" "" "Build ViSP demos" "" ON)
-# Build demos as an option.
+# Build tutorials as an option.
VP_OPTION(BUILD_TUTORIALS "" "" "Build ViSP tutorials" "" ON)
+# Build apps as an option.
+vp_check_subdirectories(VISP_CONTRIB_MODULES_PATH apps APPS_FOUND)
+if(APPS_FOUND)
+ VP_OPTION(BUILD_APPS "" "" "Build ViSP apps" "" ON)
+endif()
# Build deprecated functions as an option.
VP_OPTION(BUILD_DEPRECATED_FUNCTIONS "" "" "Build deprecated functionalities" "" ON)
# Debug and trace cflags
-VP_OPTION(ACTIVATE_DEBUG_TRACE "" "" "Enable debug and trace printings" "" ON)
+set(ENABLE_DEBUG_LEVEL 0 CACHE STRING "Set a debug level value between 0 and 9")
+
+if(ENABLE_DEBUG_LEVEL)
+ if(ENABLE_DEBUG_LEVEL MATCHES "^([0-9]+)?$")
+ set(VP_DEBUG_MODE ${CMAKE_MATCH_1})
+ set(VP_DEBUG TRUE)
+ set(VP_TRACE TRUE)
+ else()
+ set(VP_DEBUG_MODE 0)
+ endif()
+else()
+ set(VP_DEBUG_MODE 0)
+endif()
VP_OPTION(BUILD_WITH_STATIC_CRT "" "" "Enables use of staticaly linked CRT for staticaly linked ViSP" "" ON IF MSVC)
@@ -316,43 +347,56 @@ endif()
find_host_program(XRANDR xrandr)
-VP_OPTION(USE_AFMA4 "" "" "Include Afma4 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
-VP_OPTION(USE_AFMA6 "" "" "Include Afma6 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
-VP_OPTION(USE_VIPER650 "" "" "Include Viper s650 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
-VP_OPTION(USE_VIPER850 "" "" "Include Viper s850 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
-VP_OPTION(USE_VIRTUOSE Virtuose "" "Include Virtuose SDK support for Haption devices" "" ON IF NOT WINRT)
-VP_OPTION(USE_DC1394 DC1394 "" "Include dc1394 support" "" ON IF UNIX AND NOT WINRT)
-VP_OPTION(USE_V4L2 V4L2 "" "Include v4l2 support" "" ON IF UNIX AND NOT WINRT)
-VP_OPTION(USE_FLYCAPTURE FlyCapture "" "Include FlyCapture SDK support for PointGrey cameras" "" ON IF NOT WINRT)
-VP_OPTION(USE_COMEDI Comedi "" "Include comedi (linux control and measurement device interface) support" "" ON IF NOT WINRT)
-VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF NOT WINRT)
-VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX AND NOT WINRT)
-VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32 AND NOT WINRT)
-VP_OPTION(USE_GDI GDI "" "Include gdi support" "" ON IF WIN32 AND NOT WINRT)
-VP_OPTION(USE_DIRECT3D DIRECT3D "" "Include d3d support" "" ON IF WIN32 AND NOT WINRT)
-VP_OPTION(USE_DIRECTSHOW DIRECTSHOW "" "Include dshow support" "" ON IF WIN32 AND NOT WINRT)
-VP_OPTION(USE_OPENMP OpenMP "" "Include openmp support" "" ON)
+VP_CHECK_PACKAGE(C99)
+
+VP_OPTION(USE_AFMA4 "" "" "Include Afma4 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
+VP_OPTION(USE_AFMA6 "" "" "Include Afma6 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
+VP_OPTION(USE_VIPER650 "" "" "Include Viper s650 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
+VP_OPTION(USE_VIPER850 "" "" "Include Viper s850 robot support" "" ON IF (RAW1394_FOUND AND RT_FOUND AND CALINUX_FOUND AND IRISA_FOUND AND NOT WINRT))
+VP_OPTION(USE_VIRTUOSE Virtuose "" "Include Virtuose SDK support for Haption devices" "" ON IF NOT WINRT)
+VP_OPTION(USE_FRANKA Franka QUIET "Include libfranka SDK support for Franka Emika robots" "" ON IF NOT WINRT)
+VP_OPTION(USE_DC1394 DC1394 "" "Include dc1394 support" "" ON IF UNIX AND NOT WINRT)
+VP_OPTION(USE_V4L2 V4L2 "" "Include v4l2 support" "" ON IF UNIX AND NOT WINRT)
+VP_OPTION(USE_FLYCAPTURE FlyCapture "" "Include FlyCapture SDK support for PointGrey cameras" "" ON IF NOT WINRT)
+VP_OPTION(USE_PYLON Pylon "" "Include Pylon SDK support for Basler cameras" "" ON IF NOT WINRT)
+VP_OPTION(USE_COMEDI Comedi "" "Include comedi (linux control and measurement device interface) support" "" ON IF NOT WINRT)
+VP_OPTION(USE_BICLOPS BICLOPS "" "Include biclops support" "" ON IF NOT WINRT)
+VP_OPTION(USE_PTU46 PTU46 "" "Include ptu-46 support" "" ON IF UNIX AND NOT WINRT)
+VP_OPTION(USE_CMU1394 CMU1394 "" "Include cmu1494 support" "" ON IF WIN32 AND NOT WINRT)
+VP_OPTION(USE_GDI GDI "" "Include gdi support" "" ON IF WIN32 AND NOT WINRT)
+#VP_OPTION(USE_DIRECT3D DIRECT3D "" "Include d3d support" "" ON IF WIN32 AND NOT WINRT)
+VP_OPTION(USE_DIRECTSHOW DIRECTSHOW "" "Include dshow support" "" ON IF WIN32 AND NOT WINRT)
+VP_OPTION(USE_OPENMP OpenMP "" "Include openmp support" "" ON)
+VP_OPTION(USE_EIGEN3 Eigen3 QUIET "Include eigen3 support" "" ON IF NOT WINRT)
# Since the FindLAPACK.cmake provided with CMake is for Fortran language,
# in CMakeModules we have added FindLAPACK_C.cmake for C language
-VP_OPTION(USE_LAPACK LAPACK_C "" "Include lapack support" "" ON IF NOT WINRT)
-VP_OPTION(USE_GSL GSL "" "Include gsl support" "" ON IF NOT WINRT)
-VP_OPTION(USE_COIN3D "Coin3D;MyCoin3D" "" "Include coin3d support" "" ON IF OPENGL_FOUND AND NOT WINRT)
-VP_OPTION(USE_YARP YARP QUIET "Include yarp support" "YARP_DIR" ON IF NOT WINRT)
-VP_OPTION(USE_OGRE OGRE QUIET "Include Ogre support" "OGRE_DIR" ON IF NOT WINRT)
-VP_OPTION(USE_OIS OIS QUIET "Include Ogre/ois support" "OIS_DIR" ON IF USE_OGRE AND NOT WINRT)
-VP_OPTION(USE_LIBFREENECT LIBFREENECT "" "Include libfreenect support" "" ON IF NOT WINRT)
-VP_OPTION(USE_LIBUSB_1 LIBUSB_1 "" "Include libusb-1 support" "" ON IF NOT WINRT)
-VP_OPTION(USE_REALSENSE RealSense "" "Include RealSense SDK support" "" ON IF NOT WINRT)
-if(USE_REALSENSE)
- VP_OPTION(USE_CPP11 CPP11 "" "Include c++11 support" "" ON)
+VP_OPTION(USE_LAPACK LAPACK_C "" "Include lapack support" "" ON IF NOT WINRT)
+VP_OPTION(USE_GSL GSL "" "Include gsl support" "" ON IF NOT WINRT)
+VP_OPTION(USE_COIN3D "Coin3D;MyCoin3D" "" "Include coin3d support" "" ON IF OPENGL_FOUND AND NOT WINRT)
+VP_OPTION(USE_YARP YARP QUIET "Include yarp support" "YARP_DIR" ON IF NOT WINRT)
+VP_OPTION(USE_OGRE OGRE QUIET "Include Ogre support" "OGRE_DIR" ON IF NOT WINRT)
+VP_OPTION(USE_OIS OIS QUIET "Include Ogre/ois support" "OIS_DIR" ON IF USE_OGRE AND NOT WINRT)
+VP_OPTION(USE_LIBFREENECT LIBFREENECT "" "Include libfreenect support" "" ON IF NOT WINRT)
+VP_OPTION(USE_LIBUSB_1 LIBUSB_1 "" "Include libusb-1 support" "" ON IF NOT WINRT)
+VP_OPTION(USE_REALSENSE RealSense "" "Include RealSense SDK support" "" ON IF NOT WINRT)
+VP_OPTION(USE_REALSENSE2 RealSense2 "" "Include RealSense2 SDK support" "" ON IF NOT WINRT)
+if(USE_REALSENSE OR USE_REALSENSE2)
+ VP_OPTION(USE_CPP11 CPP11 "" "Include c++11 support" "" ON)
# if option is OFF, force to ON
if(NOT USE_CPP11 AND CPP11_FOUND)
+ if(USE_REALSENSE)
+ message(WARNING "Realsense 3rd party was detected but turned OFF since USE_CPP11 is OFF. Realsense 3rd party needs cxx11 compiler flags. Thus to enable Realsense usage you have to turn USE_REALSENSE=ON and USE_CPP11=ON.")
+ endif()
unset(USE_REALSENSE)
set(USE_REALSENSE OFF CACHE BOOL "Include RealSense SDK support" FORCE)
- message(WARNING "Realsense 3rd party was detected but turned OFF since USE_CPP11 is OFF. Realsense 3rd party needs cxx11 compiler flags. Thus to enable Realsense usage you have to turn USE_REALSENSE=ON and USE_CPP11=ON.")
+ if(USE_REALSENSE2)
+ message(WARNING "Realsense2 3rd party was detected but turned OFF since USE_CPP11 is OFF. Realsense2 3rd party needs cxx11 compiler flags. Thus to enable Realsense2 usage you have to turn USE_REALSENSE2=ON and USE_CPP11=ON.")
+ endif()
+ unset(USE_REALSENSE2)
+ set(USE_REALSENSE2 OFF CACHE BOOL "Include RealSense2 SDK support" FORCE)
endif()
else()
- VP_OPTION(USE_CPP11 CPP11 "" "Include c++11 support" "" OFF)
+ VP_OPTION(USE_CPP11 CPP11 "" "Include c++11 support" "" OFF)
endif()
VP_OPTION(USE_SOWIN SOWIN "" "Include Coin/SoWin support" "" OFF IF (WIN32 AND USE_COIN3D) AND NOT WINRT)
VP_OPTION(USE_SOQT SOQT "" "Include Coin/SoQt support" "" OFF IF USE_COIN3D AND NOT WINRT)
@@ -367,12 +411,11 @@ else()
endif()
VP_OPTION(USE_ZLIB "ZLIB;MyZLIB" "" "Include zlib support" "" ON IF NOT WINRT)
VP_OPTION(USE_X11 X11 "" "Include X11 support" "" ON IF NOT WINRT)
-# The native FindGTK2.cmake doesn't consider libgobject-2.0 that is
-# requested by ViSP. That's why we use our FindMyGTK2.cmake
+# The native FindGTK2.cmake doesn't consider libgobject-2.0 that is
+# requested by ViSP. That's why we use our FindMyGTK2.cmake
VP_OPTION(USE_GTK2 MyGTK2 "" "Include gtk2 support" "" OFF IF NOT WINRT)
VP_OPTION(USE_JPEG "JPEG;MyJPEG" "" "Include jpeg support" "" ON)
VP_OPTION(USE_PNG "PNG;MyPNG" "" "Include png support" "" ON)
-VP_OPTION(USE_FFMPEG FFMPEG "" "Include ffmpeg support" "" OFF)
# To control Pioneer mobile robots, under UNIX we need Aria, pthread, rt and dl 3rd party libraries
VP_OPTION(USE_ARIA ARIA "" "Include aria support" "" ON IF NOT WINRT)
#VP_OPTION(USE_RT RT "" "Include rt support" "" ON)
@@ -380,8 +423,12 @@ VP_OPTION(USE_ARIA ARIA "" "Include aria support" "" O
# bar codes
VP_OPTION(USE_ZBAR ZBAR "" "Include zbar support" "" ON IF NOT WINRT)
VP_OPTION(USE_DMTX DMTX "" "Include dmtx support" "" ON IF NOT WINRT)
-VP_OPTION(USE_PCL PCL QUIET "Include Point Cloud Library support" "" ON IF USE_REALSENSE AND NOT WINRT)
-VP_OPTION(BUILD_ATIDAQ "" "" "Include atidaq-c support" "" ON IF USE_COMEDI AND NOT WINRT)
+VP_OPTION(USE_PCL PCL QUIET "Include Point Cloud Library support" "" ON IF (USE_REALSENSE OR USE_REALSENSE2) AND NOT WINRT AND NOT IOS)
+VP_OPTION(WITH_PTHREAD "" "" "Build pthread as built-in library" "" ON IF (NOT USE_PTHREAD) AND (WIN32 OR MINGW) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0))
+VP_OPTION(WITH_APRILTAG "" "" "Build AprilTag as built-in library" "" ON IF (USE_PTHREAD OR WITH_PTHREAD) AND (NOT WINRT_8_1) AND (NOT WINRT_8_0))
+VP_OPTION(WITH_ATIDAQ "" "" "Build atidaq-c as built-in library" "" ON IF USE_COMEDI AND NOT WINRT)
+VP_OPTION(WITH_CLIPPER "" "" "Build clipper as built-in library" "" ON IF USE_OPENCV)
+VP_OPTION(WITH_LAPACK "" "" "Build lapack as built-in library" "" ON IF NOT USE_LAPACK)
# Find IsNaN (should be after USE_CPP11)
VP_CHECK_PACKAGE(IsNaN)
@@ -393,6 +440,8 @@ VP_CHECK_PACKAGE(Round)
VP_CHECK_PACKAGE(Erfc)
# Find Strtof
VP_CHECK_PACKAGE(Strtof)
+# Find Log1p
+VP_CHECK_PACKAGE(Log1p)
#----------------------------------------------------------------------
# For Dart server and tests
@@ -442,6 +491,14 @@ if(DOXYGEN_FOUND)
endif()
endforeach()
endif()
+ # - DOXYGEN_CITE_BIB_FILES corresponding to CITE_BIB_FILES in the doxy file
+ set(DOXYGEN_CITE_BIB_FILES "\"${VISP_SOURCE_DIR}/doc/biblio/references.bib\"")
+ if(VISP_CONTRIB_MODULES_PATH)
+ set(cite_bib_file_ "${VISP_CONTRIB_MODULES_PATH}/doc/biblio/references.bib")
+ if(EXISTS ${cite_bib_file_})
+ set(DOXYGEN_CITE_BIB_FILES "${DOXYGEN_CITE_BIB_FILES} \\ \n\t\t\t \"${cite_bib_file_}\"")
+ endif()
+ endif()
# - DOXYGEN_SHOULD_SKIP_THIS var
if (ENABLE_FULL_DOC)
set(DOXYGEN_SHOULD_SKIP_THIS "")
@@ -532,39 +589,6 @@ if(MSVC)
endif(MSVC)
#----------------------------------------------------------------------
-# Configure the files that depend on the build <binary dir> or
-# installation <install dir> usage. This is the case of:
-# vpConfig.h
-#----------------------------------------------------------------------
-
-# case 1: when ViSP is build with make; files are used in <binary dir>
-#--------------
-set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${VISP_BINARY_DIR}")
-
-set(VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
-set(VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
-
-# case 2: when ViSP is build with make install; files are used in <install dir>
-#--------------
-IF(UNIX)
- set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}")
-ELSE()
- set(VISP_ROOT_DIR_DATA_CONFIGCMAKE "${CMAKE_INSTALL_PREFIX}")
-ENDIF()
-
-list(APPEND VISP_SCENES_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/wireframe-simulator)
-list(APPEND VISP_ROBOT_ARMS_DIR ${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/robot-simulator)
-# Only if resources.cfg created by hand, we change the path to resources.cfg in install/vpConfig.h
-if(VISP_INSTALL_DIR_OGRE_RESOURCES)
- list(APPEND VISP_HAVE_OGRE_RESOURCES_PATH "${VISP_ROOT_DIR_DATA_CONFIGCMAKE}/data/ogre-simulator")
-endif()
-# append to VISP_HAVE_OGRE_PLUGINS_PATH the path of the installed plugins.cfg file
-# to be able to use ViSP from build tree or install tree
-if(VISP_INSTALL_DIR_OGRE_RESOURCES)
- list(APPEND VISP_HAVE_OGRE_PLUGINS_PATH "${CMAKE_INSTALL_PREFIX}/${VISP_LIB_INSTALL_PATH}/visp/data/ogre-simulator")
-endif()
-
-#----------------------------------------------------------------------
# Create and install visp-config.1.gz man page
#----------------------------------------------------------------------
if(UNIX)
@@ -618,11 +642,12 @@ VP_SET(VISP_HAVE_D3D9 TRUE IF (BUILD_MODULE_visp_core AND USE_DIRECT3D))
VP_SET(VISP_HAVE_JPEG TRUE IF (BUILD_MODULE_visp_core AND USE_JPEG))
VP_SET(VISP_HAVE_PNG TRUE IF (BUILD_MODULE_visp_core AND USE_PNG))
VP_SET(VISP_HAVE_YARP TRUE IF (BUILD_MODULE_visp_core AND USE_YARP))
+VP_SET(VISP_HAVE_EIGEN3 TRUE IF (BUILD_MODULE_visp_core AND USE_EIGEN3))
VP_SET(VISP_HAVE_GSL TRUE IF (BUILD_MODULE_visp_core AND USE_GSL))
-VP_SET(VISP_HAVE_LAPACK_C TRUE IF (BUILD_MODULE_visp_core AND USE_LAPACK))
+VP_SET(VISP_HAVE_LAPACK TRUE IF (BUILD_MODULE_visp_core AND (USE_LAPACK OR WITH_LAPACK)))
+VP_SET(VISP_HAVE_LAPACK_BUILT_IN TRUE IF (BUILD_MODULE_visp_core AND WITH_LAPACK))
VP_SET(VISP_HAVE_PTHREAD TRUE IF (BUILD_MODULE_visp_core AND USE_PTHREAD))
VP_SET(VISP_HAVE_XML2 TRUE IF (BUILD_MODULE_visp_core AND USE_XML2))
-VP_SET(VISP_HAVE_FFMPEG TRUE IF (BUILD_MODULE_visp_core AND USE_FFMPEG))
VP_SET(VISP_HAVE_PCL TRUE IF (BUILD_MODULE_visp_core AND USE_PCL))
VP_SET(VISP_HAVE_OGRE TRUE IF (BUILD_MODULE_visp_ar AND USE_OGRE))
@@ -640,6 +665,12 @@ VP_SET(VISP_HAVE_AFMA4 TRUE IF (BUILD_MODULE_visp_robot AND USE_AFMA4))
VP_SET(VISP_HAVE_AFMA6 TRUE IF (BUILD_MODULE_visp_robot AND USE_AFMA6))
VP_SET(VISP_HAVE_VIPER650 TRUE IF (BUILD_MODULE_visp_robot AND USE_VIPER650))
VP_SET(VISP_HAVE_VIPER850 TRUE IF (BUILD_MODULE_visp_robot AND USE_VIPER850))
+VP_SET(VISP_HAVE_FRANKA TRUE IF (BUILD_MODULE_visp_robot AND USE_FRANKA))
+if(VISP_HAVE_FRANKA)
+ set(CMAKE_CXX_STANDARD 14)
+ set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
VP_SET(VISP_HAVE_BICLOPS TRUE IF (BUILD_MODULE_visp_robot AND USE_BICLOPS))
VP_SET(VISP_HAVE_PTU46 TRUE IF (BUILD_MODULE_visp_robot AND USE_PTU46))
#VP_SET(VISP_HAVE_PIONEER TRUE IF (BUILD_MODULE_visp_robot AND USE_ARIA))
@@ -667,9 +698,13 @@ VP_SET(VISP_HAVE_DIRECTSHOW TRUE IF (BUILD_MODULE_visp_sensor AND USE_DIRECTSHO
VP_SET(VISP_HAVE_LIBFREENECT TRUE IF (BUILD_MODULE_visp_sensor AND USE_LIBFREENECT))
VP_SET(VISP_HAVE_LIBUSB_1 TRUE IF (BUILD_MODULE_visp_sensor AND USE_LIBUSB_1))
VP_SET(VISP_HAVE_REALSENSE TRUE IF (BUILD_MODULE_visp_sensor AND USE_REALSENSE))
+VP_SET(VISP_HAVE_REALSENSE2 TRUE IF (BUILD_MODULE_visp_sensor AND USE_REALSENSE2))
VP_SET(VISP_HAVE_FLYCAPTURE TRUE IF (BUILD_MODULE_visp_sensor AND USE_FLYCAPTURE))
+VP_SET(VISP_HAVE_PYLON TRUE IF (BUILD_MODULE_visp_sensor AND USE_PYLON))
VP_SET(VISP_HAVE_COMEDI TRUE IF (BUILD_MODULE_visp_sensor AND USE_COMEDI))
-VP_SET(VISP_HAVE_ATIDAQ TRUE IF (BUILD_MODULE_visp_sensor AND BUILD_ATIDAQ))
+VP_SET(VISP_HAVE_ATIDAQ TRUE IF (BUILD_MODULE_visp_sensor AND WITH_ATIDAQ))
+VP_SET(VISP_HAVE_CLIPPER TRUE IF (BUILD_MODULE_visp_mbt AND BUILD_MODULE_visp_klt AND WITH_CLIPPER))
+VP_SET(VISP_HAVE_APRILTAG TRUE IF (BUILD_MODULE_visp_detection AND WITH_APRILTAG))
VP_SET(VISP_BUILD_SHARED_LIBS TRUE IF BUILD_SHARED_LIBS) # for header vpConfig.h
VP_SET(VISP_HAVE_DC1394_CAMERA_ENUMERATE TRUE IF (USE_DC1394 AND DC1394_CAMERA_ENUMERATE_FOUND)) # for header vpConfig.h
@@ -761,11 +796,15 @@ VP_SET(VISP_HAVE_FUNC_ERFC TRUE IF HAVE_FUNC_ERFC) # for header vpConfig.h
VP_SET(VISP_HAVE_FUNC_STD_ERFC TRUE IF HAVE_FUNC_STD_ERFC) # for header vpConfig.h
# Find strtof function (stdlib.h)
VP_SET(VISP_HAVE_FUNC_STRTOF TRUE IF HAVE_FUNC_STRTOF) # for header vpConfig.h
+# Find log1p function (math.h)
+VP_SET(VISP_HAVE_FUNC_LOG1P TRUE IF HAVE_FUNC_LOG1P) # for header vpConfig.h
VP_SET(VISP_BUILD_DEPRECATED_FUNCTIONS TRUE IF BUILD_DEPRECATED_FUNCTIONS) # for header vpConfig.h
VP_SET(VISP_MOMENTS_COMBINE_MATRICES TRUE IF ENABLE_MOMENTS_COMBINE_MATRICES) # for header vpConfig.h
VP_SET(VISP_USE_MSVC TRUE IF MSVC) # for header vpConfig.h
-VP_SET(VISP_HAVE_CPP11_COMPATIBILITY TRUE IF USE_CPP11) # for header vpConfig.h
+# Hack for msvc12 (Visual 2013) where C++11 implementation is incomplete
+VP_SET(VISP_HAVE_CPP11_COMPATIBILITY TRUE IF USE_CPP11 OR (MSVC_VERSION EQUAL 1800)) # for header vpConfig.h
+
VP_SET(VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION TRUE IF (USE_BICLOPS AND BICLOPS_HAVE_GET_HOMED_STATE_FUNCTION)) # for header vpConfig.h
# libraries for Pioneer mobile robots
@@ -832,7 +871,7 @@ endif(BUILD_PACKAGE)
# Usage:
# visp-config --cflags ...
#----------------------------------------------------------------------
-include(cmake/VISPGeneratePkgConfigScript.cmake)
+include(cmake/VISPGenerateConfigScript.cmake)
#----------------------------------------------------------------------
# Propagation in sub dirs to build demo, example, test, tutorial
@@ -854,6 +893,9 @@ if(BUILD_TUTORIALS)
add_subdirectory(tutorial)
vp_add_subdirectories(VISP_CONTRIB_MODULES_PATH tutorial)
endif()
+if(BUILD_APPS)
+ vp_add_subdirectories(VISP_CONTRIB_MODULES_PATH apps)
+endif()
# ----------------------------------------------------------------------------
# Update version of 3rd party for which info is missing
@@ -867,6 +909,33 @@ if(USE_JPEG)
string(REPLACE "lib" "include" JPEG_INC_DIR ${JPEG_LIB_DIR})
vp_parse_header("${JPEG_INC_DIR}/jconfig.h" JPEG_VERSION_LINES JPEG_LIB_VERSION)
endif()
+ if(NOT JPEG_LIB_VERSION)
+ vp_parse_header("${JPEG_INCLUDE_DIR}/jconfig.h" JPEG_VERSION_LINES JPEG_LIB_VERSION)
+ if(NOT JPEG_LIB_VERSION)
+ vp_parse_header("${JPEG_INCLUDE_DIR}/jconfig-32.h" JPEG_LIB_VERSION JPEG_LIB_VERSION)
+ if(NOT JPEG_LIB_VERSION)
+ vp_parse_header("${JPEG_INCLUDE_DIR}/jconfig-64.h" JPEG_LIB_VERSION JPEG_LIB_VERSION)
+ endif()
+ endif()
+ endif()
+endif()
+if(USE_OIS)
+ vp_parse_header("${OIS_INCLUDE_DIR}/OISPrereqs.h" OIS_VERSION_LINES OIS_VERSION_MAJOR OIS_VERSION_MINOR OIS_VERSION_PATCH)
+ set(OIS_VERSION "${OIS_VERSION_MAJOR}.${OIS_VERSION_MINOR}.${OIS_VERSION_PATCH}")
+endif()
+if(USE_EIGEN3)
+ # Additionnal check to be sure that Eigen3 include dir os well detected
+ if(NOT EXISTS ${EIGEN3_INCLUDE_DIR})
+ message(WARNING "Eigen3 config file is detected in ${Eigen3_DIR}. Using ${Eigen3_DIR}/Eigen3Config.cmake doesn't allow to use a valid Eigen3 include dir: ${EIGEN3_INCLUDE_DIR}. Modify Eigen3_DIR to point to Eigen3Config.cmake file located in Eigen3 installation folder or turn USE_EIGEN3 OFF.")
+ endif()
+ if(NOT EIGEN3_VERSION_STRING)
+ vp_parse_header("${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" EIGEN3_VERSION_LINES EIGEN_WORLD_VERSION EIGEN_MAJOR_VERSION EIGEN_MINOR_VERSION)
+ set(EIGEN3_VERSION_STRING "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
+ endif()
+endif()
+if(USE_COIN3D)
+ vp_parse_header("${COIN3D_INCLUDE_DIRS}/Inventor/C/basic.h" COIN_VERSION_LINES COIN_MAJOR_VERSION COIN_MINOR_VERSION COIN_MICRO_VERSION)
+ set(COIN3D_VERSION "${COIN_MAJOR_VERSION}.${COIN_MINOR_VERSION}.${COIN_MICRO_VERSION}")
endif()
# ----------------------------------------------------------------------------
@@ -1035,89 +1104,100 @@ status(" Build with moment combine:" ENABLE_MOMENTS_COMBINE_MATRICES THEN "y
# ===================== Optional 3rd parties =====================
status("")
status(" Mathematics: ")
-status(" Use GSL:" USE_GSL THEN "yes (ver ${GSL_VERSION})" ELSE "no")
-status(" Use Lapack/blas:" USE_LAPACK THEN "yes (ver ${LAPACK_C_VERSION})" ELSE "no")
+status(" Use Lapack/blas:" USE_LAPACK THEN "yes (ver ${LAPACK_C_VERSION})" ELSE "no")
+status(" Use Lapack (built-in):" WITH_LAPACK THEN "yes (ver ${LAPACK_VERSION})" ELSE "no")
+status(" Use Eigen3:" USE_EIGEN3 THEN "yes (ver ${EIGEN3_VERSION_STRING})" ELSE "no")
+status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
+status(" Use GSL:" USE_GSL THEN "yes (ver ${GSL_VERSION})" ELSE "no")
status("")
status(" Simulator: ")
status(" Ogre simulator: ")
-status(" \\- Use Ogre3D:" USE_OGRE THEN "yes" ELSE "no")
-status(" \\- Use OIS:" USE_OIS THEN "yes" ELSE "no")
+status(" \\- Use Ogre3D:" USE_OGRE THEN "yes (ver ${OGRE_VERSION})" ELSE "no")
+status(" \\- Use OIS:" USE_OIS THEN "yes (ver ${OIS_VERSION})" ELSE "no")
status(" Coin simulator: ")
-status(" \\- Use Coin3D:" USE_OGRE THEN "yes" ELSE "no")
-status(" \\- Use SoWin:" USE_SOWIN THEN "yes" ELSE "no")
-status(" \\- Use SoXt:" USE_SOXT THEN "yes" ELSE "no")
-status(" \\- Use SoQt:" USE_SOQT THEN "yes" ELSE "no")
-status(" \\- Use Qt4:" USE_QT AND DESIRED_QT_VERSION MATCHES 4 THEN "yes" ELSE "no")
-status(" \\- Use Qt3:" USE_QT AND DESIRED_QT_VERSION MATCHES 3 THEN "yes" ELSE "no")
+status(" \\- Use Coin3D:" USE_OGRE THEN "yes (ver ${COIN3D_VERSION})" ELSE "no")
+status(" \\- Use SoWin:" USE_SOWIN THEN "yes" ELSE "no")
+status(" \\- Use SoXt:" USE_SOXT THEN "yes" ELSE "no")
+status(" \\- Use SoQt:" USE_SOQT THEN "yes" ELSE "no")
+status(" \\- Use Qt4:" USE_QT AND DESIRED_QT_VERSION MATCHES 4 THEN "yes" ELSE "no")
+status(" \\- Use Qt3:" USE_QT AND DESIRED_QT_VERSION MATCHES 3 THEN "yes" ELSE "no")
status("")
status(" Media I/O: ")
-status(" Use JPEG:" USE_JPEG THEN "yes (ver ${JPEG_LIB_VERSION})" ELSE "no")
-status(" Use PNG:" USE_PNG THEN "yes (ver ${PNG_VERSION_STRING})" ELSE "no")
-status(" \\- Use ZLIB:" USE_ZLIB THEN "yes (ver ${ZLIB_VERSION_STRING})" ELSE "no")
-status(" Use FFMPEG:" USE_FFMPEG THEN "yes" ELSE "no")
-status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
+status(" Use JPEG:" USE_JPEG THEN "yes (ver ${JPEG_LIB_VERSION})" ELSE "no")
+status(" Use PNG:" USE_PNG THEN "yes (ver ${PNG_VERSION_STRING})" ELSE "no")
+status(" \\- Use ZLIB:" USE_ZLIB THEN "yes (ver ${ZLIB_VERSION_STRING})" ELSE "no")
+status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
status("")
status(" Real robots: ")
-status(" Use Afma4:" USE_AFMA4 THEN "yes" ELSE "no")
-status(" Use Afma6:" USE_AFMA6 THEN "yes" ELSE "no")
-status(" Use Viper650:" USE_VIPER850 THEN "yes" ELSE "no")
-status(" Use Viper850:" USE_VIPER850 THEN "yes" ELSE "no")
-status(" Use aria (Pioneer):" USE_ARIA THEN "yes" ELSE "no")
-status(" Use PTU46:" USE_PTU46 THEN "yes" ELSE "no")
-status(" Use Biclops PT:" USE_BICLOPS THEN "yes" ELSE "no")
+status(" Use Afma4:" USE_AFMA4 THEN "yes" ELSE "no")
+status(" Use Afma6:" USE_AFMA6 THEN "yes" ELSE "no")
+status(" Use Franka:" USE_FRANKA THEN "yes" ELSE "no")
+status(" Use Viper650:" USE_VIPER850 THEN "yes" ELSE "no")
+status(" Use Viper850:" USE_VIPER850 THEN "yes" ELSE "no")
+status(" Use aria (Pioneer):" USE_ARIA THEN "yes" ELSE "no")
+status(" Use PTU46:" USE_PTU46 THEN "yes" ELSE "no")
+status(" Use Biclops PT:" USE_BICLOPS THEN "yes" ELSE "no")
status("")
status(" GUI: ")
-status(" Use X11:" USE_X11 THEN "yes" ELSE "no")
-status(" Use GTK:" USE_GTK2 THEN "yes (ver ${GTK2_VERSION})" ELSE "no")
-status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
-status(" Use GDI:" USE_GDI THEN "yes" ELSE "no")
-status(" Use Direct3D:" USE_DIRECT3D THEN "yes" ELSE "no")
+status(" Use X11:" USE_X11 THEN "yes" ELSE "no")
+status(" Use GTK:" USE_GTK2 THEN "yes (ver ${GTK2_VERSION})" ELSE "no")
+status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
+status(" Use GDI:" USE_GDI THEN "yes" ELSE "no")
+status(" Use Direct3D:" USE_DIRECT3D THEN "yes" ELSE "no")
status("")
status(" Cameras: ")
-status(" Use DC1394-2.x:" USE_DC1394 THEN "yes (ver ${DC1394_VERSION})" ELSE "no")
-status(" Use CMU 1394:" USE_CMU1394 THEN "yes" ELSE "no")
-status(" Use V4L2:" USE_V4L2 THEN "yes (ver ${V4L2_VERSION})" ELSE "no")
-status(" Use directshow:" USE_DIRECTSHOW THEN "yes" ELSE "no")
-status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
-status(" Use Flycapture:" USE_FLYCAPTURE THEN "yes (ver ${FLYCAPTURE_VERSION})" ELSE "no")
+status(" Use DC1394-2.x:" USE_DC1394 THEN "yes (ver ${DC1394_VERSION})" ELSE "no")
+status(" Use CMU 1394:" USE_CMU1394 THEN "yes" ELSE "no")
+status(" Use V4L2:" USE_V4L2 THEN "yes (ver ${V4L2_VERSION})" ELSE "no")
+status(" Use directshow:" USE_DIRECTSHOW THEN "yes" ELSE "no")
+status(" Use OpenCV:" USE_OPENCV THEN "yes (ver ${OpenCV_VERSION})" ELSE "no")
+status(" Use Flycapture:" USE_FLYCAPTURE THEN "yes (ver ${FLYCAPTURE_VERSION})" ELSE "no")
+status(" Use Pylon:" USE_PYLON THEN "yes (ver ${PYLON_VERSION})" ELSE "no")
status("")
status(" RGB-D sensors: ")
-status(" Use Realsense:" USE_REALSENSE THEN "yes (ver ${REALSENSE_VERSION})" ELSE "no")
-status(" \\- Use PCL:" USE_PCL THEN "yes (ver ${PCL_VERSION})" ELSE "no")
-status(" \\- Use VTK:" VTK_FOUND THEN "yes (ver ${VTK_VERSION})" ELSE "no")
-status(" Use Kinect:" USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD THEN "yes" ELSE "no")
-status(" \\- Use libfreenect:" USE_LIBFREENECT THEN "yes (ver ${LIBFREENECT_VERSION})" ELSE "no")
-status(" \\- Use libusb-1:" USE_LIBUSB_1 THEN "yes (ver ${LIBUSB_1_VERSION})" ELSE "no")
-status(" \\- Use pthread:" USE_PTHREAD THEN "yes" ELSE "no")
+status(" Use Realsense:" USE_REALSENSE THEN "yes (ver ${REALSENSE_VERSION})" ELSE "no")
+status(" Use Realsense2:" USE_REALSENSE2 THEN "yes (ver ${REALSENSE2_VERSION})" ELSE "no")
+status(" \\- Use PCL:" USE_PCL THEN "yes (ver ${PCL_VERSION})" ELSE "no")
+status(" \\- Use VTK:" VTK_FOUND THEN "yes (ver ${VTK_VERSION})" ELSE "no")
+status(" Use Kinect:" USE_LIBFREENECT AND USE_LIBUSB_1 AND USE_PTHREAD THEN "yes" ELSE "no")
+status(" \\- Use libfreenect:" USE_LIBFREENECT THEN "yes (ver ${LIBFREENECT_VERSION})" ELSE "no")
+status(" \\- Use libusb-1:" USE_LIBUSB_1 THEN "yes (ver ${LIBUSB_1_VERSION})" ELSE "no")
+status(" \\- Use pthread:" USE_PTHREAD THEN "yes" ELSE "no")
status("")
status(" F/T sensors: ")
-status(" Use atidaq:" BUILD_ATIDAQ THEN "yes (build)" ELSE "no")
-status(" Use comedi:" USE_COMEDI THEN "yes (ver ${COMEDI_VERSION})" ELSE "no")
+status(" Use atidaq (built-in):" WITH_ATIDAQ THEN "yes (ver ${ATIDAQ_VERSION})" ELSE "no")
+status(" Use comedi:" USE_COMEDI THEN "yes (ver ${COMEDI_VERSION})" ELSE "no")
status("")
status(" Detection: ")
-status(" Use zbar:" USE_ZBAR THEN "yes (ver ${ZBAR_VERSION})" ELSE "no")
-status(" Use dmtx:" USE_DMTX THEN "yes (ver ${DMTX_VERSION})" ELSE "no")
+status(" Use zbar:" USE_ZBAR THEN "yes (ver ${ZBAR_VERSION})" ELSE "no")
+status(" Use dmtx:" USE_DMTX THEN "yes (ver ${DMTX_VERSION})" ELSE "no")
+status(" Use AprilTag (built-in):" WITH_APRILTAG THEN "yes (ver ${APRILTAG_VERSION})" ELSE "no")
status("")
status(" Misc: ")
-status(" Use XML2:" USE_XML2 THEN "yes (ver ${XML2_VERSION_STRING})" ELSE "no")
+status(" Use Clipper (built-in):" WITH_CLIPPER THEN "yes (ver ${CLIPPER_VERSION})" ELSE "no")
+status(" Use XML2:" USE_XML2 THEN "yes (ver ${XML2_VERSION_STRING})" ELSE "no")
status("")
status(" Optimization: ")
-status(" Use OpenMP:" USE_OPENMP THEN "yes" ELSE "no")
-status(" Use pthread:" USE_PTHREAD THEN "yes" ELSE "no")
-status(" Use cxx11:" USE_CPP11 THEN "yes" ELSE "no")
+status(" Use OpenMP:" USE_OPENMP THEN "yes" ELSE "no")
+status(" Use pthread:" USE_PTHREAD THEN "yes" ELSE "no")
+status(" Use pthread (built-in):" WITH_PTHREAD THEN "yes (ver ${PTHREADS_VERSION})" ELSE "no")
+status(" Use cxx11:" USE_CPP11 THEN "yes" ELSE "no")
# ========================== documentation ==========================
status("")
status(" Documentation: ")
-status(" Use doxygen:" DOXYGEN_FOUND THEN "yes" ELSE "no")
+status(" Use doxygen:" DOXYGEN_FOUND THEN "yes" ELSE "no")
# ========================== samples and tests ==========================
status("")
status(" Tests and samples:")
-status(" Tests:" BUILD_TESTS THEN "yes" ELSE "no")
-status(" Demos:" BUILD_DEMOS THEN "yes" ELSE "no")
-status(" Examples:" BUILD_EXAMPLES THEN "yes" ELSE "no")
-status(" Tutorials:" BUILD_TUTORIALS THEN "yes" ELSE "no")
+status(" Tests:" BUILD_TESTS THEN "yes" ELSE "no")
+status(" Demos:" BUILD_DEMOS THEN "yes" ELSE "no")
+status(" Examples:" BUILD_EXAMPLES THEN "yes" ELSE "no")
+status(" Tutorials:" BUILD_TUTORIALS THEN "yes" ELSE "no")
+if(APPS_FOUND)
+ status(" Apps:" BUILD_APPS THEN "yes" ELSE "no")
+endif()
# ========================== auxiliary ==========================
status("")
diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md
new file mode 100644
index 0000000..3f482f2
--- /dev/null
+++ b/CONTRIBUTING.md
@@ -0,0 +1,3 @@
+## Contributing guidelines
+
+All guidelines for contributing to the ViSP repository can be found at [`Contribution code`](https://visp.inria.fr/contributing-code/) web page.
diff --git a/CTestConfig.cmake b/CTestConfig.cmake
index b7d596f..0c3db76 100644
--- a/CTestConfig.cmake
+++ b/CTestConfig.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -45,7 +46,7 @@ set(CTEST_DROP_SITE_CDASH TRUE)
#--------------------------------------------------------------------
# BUILNAME variable construction
-# This variable will be used to set the build name which will appear
+# This variable will be used to set the build name which will appear
# on the ViSP dashboard http://cdash.irisa.fr/CDash/
#--------------------------------------------------------------------
# Start with the short system name, e.g. "Linux", "FreeBSD" or "Windows"
@@ -105,7 +106,7 @@ if(CMAKE_COMPILER_IS_GNUCC)
#message("COMPILER_VERSION 2: ${COMPILER_VERSION}")
set(BUILDNAME "${BUILDNAME}${COMPILER_VERSION}")
-
+
endif(CMAKE_COMPILER_IS_GNUCC)
# Add the type of library generation, e.g. "Dynamic or Static"
@@ -125,7 +126,7 @@ endif(CMAKE_BUILD_TYPE)
if(VISP_HAVE_AFMA4)
set(BUILDNAME "${BUILDNAME}-Afma4")
endif(VISP_HAVE_AFMA4)
-
+
# Add specific Afma6 robots
if(VISP_HAVE_AFMA6)
set(BUILDNAME "${BUILDNAME}-Afma6")
@@ -152,7 +153,7 @@ if(VISP_HAVE_VIRTUOSE)
endif()
#---- Framegrabers/Sensors ----
-# Firewire dc1394-2.x
+# Firewire dc1394-2.x
if(VISP_HAVE_DC1394)
set(BUILDNAME "${BUILDNAME}-dc1394")
endif(VISP_HAVE_DC1394)
@@ -182,6 +183,9 @@ endif()
if(VISP_HAVE_FLYCAPTURE)
set(BUILDNAME "${BUILDNAME}-flycap")
endif()
+if(VISP_HAVE_PYLON)
+ set(BUILDNAME "${BUILDNAME}-pylon")
+endif()
if(VISP_HAVE_COMEDI)
set(BUILDNAME "${BUILDNAME}-comedi")
endif()
@@ -221,13 +225,16 @@ endif(VISP_HAVE_OPENCV)
#---- Mathematics ----
# Lapack (Linear Algebra PACKage)
-if(VISP_HAVE_LAPACK_C)
+if(VISP_HAVE_LAPACK)
set(BUILDNAME "${BUILDNAME}-lapack")
endif()
# GSL (Gnu Scientific Library)
if(VISP_HAVE_GSL)
set(BUILDNAME "${BUILDNAME}-gsl")
-endif(VISP_HAVE_GSL)
+endif()
+if(VISP_HAVE_EIGEN3)
+ set(BUILDNAME "${BUILDNAME}-eigen3")
+endif()
#---- Simulator ----
# Ogre
@@ -259,9 +266,6 @@ if(VISP_HAVE_SOXT)
endif(VISP_HAVE_SOXT)
#---- Images ----
-if(VISP_HAVE_FFMPEG)
- set(BUILDNAME "${BUILDNAME}-ffmpeg")
-endif(VISP_HAVE_FFMPEG)
if(VISP_HAVE_JPEG)
set(BUILDNAME "${BUILDNAME}-jpeg")
endif(VISP_HAVE_JPEG)
@@ -291,6 +295,9 @@ endif()
if(VISP_HAVE_ZBAR)
set(BUILDNAME "${BUILDNAME}-zbar")
endif()
+if(VISP_HAVE_APRILTAG)
+ set(BUILDNAME "${BUILDNAME}-apriltag")
+endif()
#---- Special compiler flags ----
@@ -306,6 +313,9 @@ endif()
if(ENABLE_MOMENTS_COMBINE_MATRICES)
set(BUILDNAME "${BUILDNAME}-Moment")
endif()
+if(ENABLE_SSE2 OR ENABLE_SSE3 OR ENABLE_SSSE3)
+ set(BUILDNAME "${BUILDNAME}-sse")
+endif()
#---- Suffix contrib ----
if(VISP_CONTRIB_MODULES_PATH)
diff --git a/ChangeLog b/ChangeLog
index c74598d..391ed26 100644
--- a/ChangeLog
+++ b/ChangeLog
@@ -5,7 +5,85 @@
Copyright (C) 2005 - 2017 by Inria. All rights reserved.
https://visp.inria.fr
+ViSP 3.1.0 (released December 22nd, 2017)
+ - Contributors:
+ . This release was possible thanks to contributions from Fabien Spindler,
+ Souriya Trinh, Wenfeng Cai, Oleksandr Borysov, Marc Pouliquen, Ronald Ensing,
+ Yuriy Baranov, Jason Chevie
+ . Google Summer of Code 2017: Vikas Thamizharasan that developed for the
+ model-based tracker Blender import/export CAD model plugins and a Qt-based
+ CAD model viewer
+ - New features
+ . Improve vpRealSense to better support R200 device
+ . Improve vpConfig.h header content to remove path to build tree when installed
+ . Speed-up vpMbKltTracker and vpMbEdgeKltTracker during reinit
+ . Remove numerical recipes code that is not open-source
+ . Changes done in vpMatrix to use either Lapack, OpenCV or GSL 3rd party
+ . Eigen 3rd party was introduced as an alternative for matrix computation
+ . Lapack is now embedded as a build-in when Lapack system library is not found
+ . vpVideoReader was improved to better detect first and last image numbers of an
+ image sequence and better handle the frame step
+ . Speed-up vpColVector::insert(), vpMatrix::insert() and vpMatrix::stack()
+ . Refactor model-based tracker (+ some segfault / memory fixes)
+ . Introduce vpMbGenericTracker a new class that can handle all the features
+ supported by the model-based tracker but also consider stereo or multi-view
+ tracking
+ . Add model-based depth tracking using face normals or using a dense approach
+ . Use BLAS library to compute dgemm and dgemv operations if available
+ . Add Hadamard product
+ . Add an explicit recopy parameter when resizing a vpArray2D
+ . Add move constructor / assignment operator for vpMatrix, vpColVector and vpImage
+ . Introduce vpPylonGrabber a wrapper for Basler cameras using Pylon SDK
+ . Introduce AprilTag as a 3rd party and vpDetectorAprilTag wrapper
+ . Remove ffmpeg 3rd party usage
+ . Introduce AppVeyor check for continuous integration (MSVC + mingw-w64)
+ . Fix slow acquisition of color pointcloud with RealSense camera
+ . Improve vpIoTools to make recursive folder and sort files
+ . Add display of polygon lines (vpDisplay::displayLine() /
+ vpDisplay::displayDotLine())
+ . Add CPU capabilities check
+ . Improve VISP_INPUT_IMAGE_PATH env var usage to get more flexibility
+ . Update ViSP license to GPLv2 or later
+ . Introduce new imgproc module dedicated to image processing and tutorials
+ . Add UDP client / server
+ . Add compat with Intel Compiler icc
+ . Add new class vpRealSense2 to support Realsense 2 SDK
+ . Add Franka Control Interface C++ API detection and native examples from Franka
+ - Tutorials
+ . New tutorial: How to extend ViSP creating a new contrib module
+ . New tutorial: AprilTag marker detection on iOS
+ . New tutorial: Installation from source for Windows with Visual C++ 2017 (vc15)
+ . New tutorial: Installation from source for UWP with Visual C++ 2017 (vc15)
+ . New tutorial: How to import a CMake project that uses ViSP in Eclipse IDE
+ . New tutorial: Brightness and contrast adjustment
+ . New tutorial: Contrast and image sharpening techniques
+ . New tutorial: Automatic thresholding
+ . New tutorial: Contours extraction from a binary image
+ . New tutorial: Connected-components labeling
+ . New tutorial: Flood fill algorithm
+ . New tutorial: Practical example: count the number of coins in an image
+ . New tutorial: Markerless generic model-based tracking
+ . New tutorial: AprilTag marker detection
+ . New tutorial: Markerless model-based tracker CAD model editor - GSoC 2017 project
+ - Bug fixed
+ . [#137] Fix bug in extration of vpRotationMatrix from vpPoseVector using
+ vpPoseVector::getRotationMatrix()
+ . [#149] Bug in vpTime::measureTimeMicros() (OS detection was done wrong)
+ . [#160] Build issue with msvc arround std::min() std::max()
+ . [#162] Redefinition errors in WinSock2.h and winsock.h
+ . [#165] Segmentation fault when cout a null row vector
+ . [#171] Segfault when using vpMatrix::pseudoInverse()
+ . [#175] Unable to track an ellipse with vpMeEllipse
+ . [#177] Bug in line 124 of VISPConfig.cmake
+ . [#178] Comments are not handled in vpMbTracker::initFromPoints()
+ . [#183] Inversion between pixel size and optical center in vpRealSense class
+ . [#187] Integer division by zero in vpDisplayWin32.cpp
+ . [#200] Issue when reading PNG images with 16bits depth with libpng
+ . [#201] Fix read PNG images with 16 bits depth with libpng
+ . [#207] CMake option 'ACTIVATE_DEBUG_TRACE' has never been used
+ . [#226] Planar pose Lagrange computation returns a left handed frame
+----------------------------------------------
ViSP 3.0.1 (released February 3rd, 2017)
- New features
. Compatibility with Microsoft Visual C++ 2017 (MSCV 15)
diff --git a/LICENSE.txt b/LICENSE.txt
index 3912109..7dc01a3 100644
--- a/LICENSE.txt
+++ b/LICENSE.txt
@@ -1,5 +1,5 @@
GNU GENERAL PUBLIC LICENSE
- Version 2, June 1991
+ Version 2 or (at your option) any later version, June 1991
Copyright (C) 1989, 1991 Free Software Foundation, Inc.
51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
diff --git a/README.md b/README.md
index dd4973f..d597588 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,11 @@
### ViSP: Open source Visual Servoing Platform
-[](https://travis-ci.org/lagadic/visp)
+Platform | Build Status |
+-------- | ------------ |
+Linux and OS X | [](https://travis-ci.org/lagadic/visp) |
+Windows | [](https://ci.appveyor.com/project/fspindle/visp/branch/master) |
+
+This project is a cross-platform library (Linux, Windows, Mac) that allows prototyping and developing applications using visual tracking and visual servoing technics at the heart of the researches done by Inria <a href="http://team.inria.fr/lagadic">Lagadic team</a>. ViSP is able to compute control laws that can be applied to robotic systems. It provides a set of visual features that can be tracked using real time image processing or computer vision algorithms. ViSP provides also simulat [...]
#### Resources
- Homepage: http://visp.inria.fr
@@ -8,6 +13,7 @@
- Code documentation: http://visp-doc.inria.fr/doxygen/visp-daily
- Q&A forum: http://gforge.inria.fr/forum/?group_id=397
- Issue tracking: https://github.com/lagadic/visp/issues
+- YouTube: https://www.youtube.com/user/VispTeam
#### Contributing
diff --git a/appveyor.yml b/appveyor.yml
new file mode 100644
index 0000000..2487ca1
--- /dev/null
+++ b/appveyor.yml
@@ -0,0 +1,99 @@
+# Specify version format
+version: "3.1.0-{build}"
+
+os: Visual Studio 2015
+
+# to add several platforms to build matrix
+platform:
+ - x64
+
+configuration:
+ - Release
+
+environment:
+ VISP_OPENCV_MINGW_FILE: opencv-3.3.0-MinGW-build-package.7z
+ VISP_OPENCV_MINGW_DIR: opencv-3.3.0-MinGW-build-package
+ VISP_OPENCV_MINGW_PKG: https://github.com/s-trinh/visp-dependencies/blob/master/opencv-3.3.0-MinGW-build-package.7z?raw=true
+
+ matrix:
+ - CMAKE_GENERATOR: MinGW Makefiles
+ BUILD_NAME: mingw
+
+ - CMAKE_GENERATOR: Visual Studio 14 2015 Win64
+ BUILD_NAME: vs2015
+
+# Scripts that are called at very beginning, before repo cloning
+init:
+ - cmake --version
+ - msbuild /version
+
+install:
+ # Create temp dir to collect test outputs
+ - md C:\temp
+
+ # All external dependencies are installed in C:\projects\deps
+ - mkdir C:\projects\deps
+ - cd C:\projects\deps
+
+ # visp-images
+ - git clone https://github.com/lagadic/ViSP-images C:\projects\deps\ViSP-images
+ - set VISP_INPUT_IMAGE_PATH=C:\projects\deps
+
+ # opencv
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" choco install OpenCV
+ - ps: >-
+ if (${env:CMAKE_GENERATOR} -imatch "MinGW Makefiles" -and !(Test-Path ${env:VISP_OPENCV_MINGW_FILE})) {
+ echo "Downloading OpenCV MinGW from ${env:VISP_OPENCV_MINGW_PKG} ..."
+ appveyor DownloadFile "${env:VISP_OPENCV_MINGW_PKG}" -FileName ${env:VISP_OPENCV_MINGW_FILE} -Timeout 1200000
+ }
+
+ - ps: >-
+ if (${env:CMAKE_GENERATOR} -imatch "MinGW Makefiles") {
+ echo "Extracting OpenCV MinGW to C:\projects\deps ..."
+ 7z x ${env:VISP_OPENCV_MINGW_FILE} -y -o"C:\projects\deps"
+ }
+
+cache:
+ - opencv-3.3.0-MinGW-build-package.7z -> appveyor.yml
+
+# visp clone directory
+clone_folder: C:\projects\visp\visp
+
+before_build:
+ # Use MinGW-w64 in C:\mingw-w64\i686-6.3.0-posix-dwarf-rt_v5-rev1
+ - cd C:\
+ - ren MinGW MinGW-w32
+ # Workaround for CMake not wanting sh.exe on PATH for MinGW
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set PATH=%PATH:C:\Program Files\Git\usr\bin;=%
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set PATH=C:\mingw-w64\i686-6.3.0-posix-dwarf-rt_v5-rev1\mingw32\bin;%PATH%
+ - dir C:\mingw-w64\i686-6.3.0-posix-dwarf-rt_v5-rev1\mingw32\bin
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" mingw32-make --version
+ # Setup ViSP
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" set VISP_DLL_DIR=C:\projects\visp\build\install\%platform%\vc14\bin
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set VISP_DLL_DIR=C:\projects\visp\build\install\%platform%\mingw\bin
+ - SET PATH=%VISP_DLL_DIR%;%PATH%
+ # Setup OpenCV
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" set OpenCV_DIR=C:\tools\opencv\build
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" set OpenCV_DLL_DIR=%OpenCV_DIR%\%platform%\vc14\bin
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set OpenCV_DIR=C:\projects\deps\%VISP_OPENCV_MINGW_DIR%\install
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set OpenCV_DLL_DIR=%OpenCV_DIR%\x86\mingw\bin
+ - set PATH=%OPENCV_DLL_DIR%;%PATH%
+ # Issue with pthreads threading on MinGW AppVeyor
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" set APPVEYOR_THREADING=true
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" set APPVEYOR_THREADING=false
+
+build_script:
+ - echo %PATH%
+ - dir C:\projects\deps
+ - dir C:\tools
+ - dir %OpenCV_DLL_DIR%
+ - md C:\projects\visp\build
+ - cd C:\projects\visp\build
+ - cmake -G "%CMAKE_GENERATOR%" -DCMAKE_BUILD_TYPE=%configuration% ..\visp
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" cmake --build . --config %configuration% --target install -- /m:2
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" cmake --build . --config %configuration% --target install -- -j2
+ - dir C:\projects\visp\build\install
+ - dir %VISP_DLL_DIR%
+ - if "%CMAKE_GENERATOR%"=="Visual Studio 14 2015 Win64" ctest
+ # Issue with exception on MinGW AppVeyor
+ - if "%CMAKE_GENERATOR%"=="MinGW Makefiles" ctest -E testMatrixException
diff --git a/cmake/AddExtraCompilationFlags.cmake b/cmake/AddExtraCompilationFlags.cmake
index e2b2d68..49c0215 100644
--- a/cmake/AddExtraCompilationFlags.cmake
+++ b/cmake/AddExtraCompilationFlags.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -127,10 +128,14 @@ if(CMAKE_COMPILER_IS_GNUCXX)
elseif(X86 OR X86_64)
add_extra_compiler_option(-mno-ssse3)
endif()
+
+ if(X86)
+ add_extra_compiler_option(-ffloat-store)
+ endif()
endif()
if(UNIX)
- if(CMAKE_COMPILER_IS_GNUCXX OR CV_ICC)
+ if(CMAKE_COMPILER_IS_GNUCXX)
add_extra_compiler_option(-fPIC)
endif()
endif()
diff --git a/cmake/CPackConfig.cmake b/cmake/CPackConfig.cmake
index 916077f..659f3b3 100644
--- a/cmake/CPackConfig.cmake
+++ b/cmake/CPackConfig.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/CPackConfigDeb.cmake b/cmake/CPackConfigDeb.cmake
index 8c44a57..57a0409 100644
--- a/cmake/CPackConfigDeb.cmake
+++ b/cmake/CPackConfigDeb.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -114,9 +115,6 @@ IF(USE_LIBJPEG)
set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libjpeg-dev (>=6b-15)")
endif()
ENDIF()
-IF(USE_FFMPEG)
- set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libswscale-dev (>=4:0.5.1), libavutil-dev (>=4:0.5.1), libavformat-dev (>=4:0.5.1), libavcodec-dev (>=4:0.5.1), libbz2-dev (>=1.0.5-4), libbz2-1.0 (>=1.0.5-4)")
-ENDIF()
IF(USE_COIN)
set(CPACK_DEBIAN_PACKAGE_DEPENDS "${CPACK_DEBIAN_PACKAGE_DEPENDS}, libcoin60-dev (>=3.1.2-1)")
ENDIF()
diff --git a/cmake/CPackConfigNsis.cmake b/cmake/CPackConfigNsis.cmake
index 365d10a..20c8b27 100644
--- a/cmake/CPackConfigNsis.cmake
+++ b/cmake/CPackConfigNsis.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/CPackConfigRpm.cmake b/cmake/CPackConfigRpm.cmake
index 2964c3b..7e30bf2 100644
--- a/cmake/CPackConfigRpm.cmake
+++ b/cmake/CPackConfigRpm.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindARIA.cmake b/cmake/FindARIA.cmake
index 91c2ce1..62c5651 100644
--- a/cmake/FindARIA.cmake
+++ b/cmake/FindARIA.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindAfma6_data.cmake b/cmake/FindAfma6_data.cmake
index 9be1248..1c109b1 100644
--- a/cmake/FindAfma6_data.cmake
+++ b/cmake/FindAfma6_data.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindBICLOPS.cmake b/cmake/FindBICLOPS.cmake
index 08bbb41..a93e9d7 100644
--- a/cmake/FindBICLOPS.cmake
+++ b/cmake/FindBICLOPS.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindC99.cmake b/cmake/FindC99.cmake
new file mode 100644
index 0000000..a958c0a
--- /dev/null
+++ b/cmake/FindC99.cmake
@@ -0,0 +1,85 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# Try to find C99 flags only if requested by the compiler.
+# for (int i=0; i<4; i++) does't build with gcc 4.8.2 without c99 flags
+# while it builds without the c99 flag with gcc 6.2.0.
+#
+# C99_FOUND - true if C99 support is detected
+# C99_C_FLAGS - flags to add to the C compiler for C99 support
+#
+#############################################################################
+
+
+include(CheckCCompilerFlag)
+include(FindPackageHandleStandardArgs)
+
+set(C99_C_FLAG_CANDIDATES
+ "-std=gnu99"
+ "-std=c99"
+)
+
+# check C C99 compiler flag
+foreach(FLAG ${C99_C_FLAG_CANDIDATES})
+ set(SAFE_CMAKE_REQUIRED_FLAGS "${CMAKE_REQUIRED_FLAGS}")
+ set(CMAKE_REQUIRED_FLAGS "${FLAG}")
+ unset(C99_FLAG_DETECTED CACHE)
+ message(STATUS "Try C99 flag = [${FLAG}]")
+ CHECK_C_COMPILER_FLAG(${FLAG} C99_FLAG_DETECTED)
+ set(CMAKE_REQUIRED_FLAGS "${SAFE_CMAKE_REQUIRED_FLAGS}")
+ if(C99_FLAG_DETECTED)
+ set(C99_C_FLAGS_INTERNAL "${FLAG}")
+ break()
+ endif()
+endforeach()
+
+if(C99_FLAG_DETECTED)
+ # Check if C99 flag is requested by the compiler
+ include(CheckCSourceCompiles)
+ #set(CMAKE_REQUIRED_FLAG "${C99_C_FLAGS_INTERNAL}")
+ check_c_source_compiles("
+ int main(){
+ for(int i=0; i<4; i++) {}
+ return 0;
+ }
+ " __compiler_doesnt_need_c99_flag)
+endif()
+
+if(NOT __compiler_doesnt_need_c99_flag)
+ set(C99_C_FLAGS "${C99_C_FLAGS_INTERNAL}")
+ set(C99_FOUND TRUE)
+else()
+ set(C99_FOUND FALSE)
+endif()
+
+mark_as_advanced(
+ C99_C_FLAGS
+)
diff --git a/cmake/FindCALINUX.cmake b/cmake/FindCALINUX.cmake
index 466e983..f6193e7 100644
--- a/cmake/FindCALINUX.cmake
+++ b/cmake/FindCALINUX.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindCMU1394.cmake b/cmake/FindCMU1394.cmake
index a1d36ed..c3c76c7 100644
--- a/cmake/FindCMU1394.cmake
+++ b/cmake/FindCMU1394.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindCPP11.cmake b/cmake/FindCPP11.cmake
index b1db46d..b68faec 100644
--- a/cmake/FindCPP11.cmake
+++ b/cmake/FindCPP11.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindComedi.cmake b/cmake/FindComedi.cmake
index 3dff9ca..92971c4 100644
--- a/cmake/FindComedi.cmake
+++ b/cmake/FindComedi.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindDC1394.cmake b/cmake/FindDC1394.cmake
index 2bd4d26..97fa2a1 100644
--- a/cmake/FindDC1394.cmake
+++ b/cmake/FindDC1394.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindDIRECT3D.cmake b/cmake/FindDIRECT3D.cmake
index cfad6c2..901923b 100644
--- a/cmake/FindDIRECT3D.cmake
+++ b/cmake/FindDIRECT3D.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindDIRECTSHOW.cmake b/cmake/FindDIRECTSHOW.cmake
index 6d8edf9..bbb12ee 100644
--- a/cmake/FindDIRECTSHOW.cmake
+++ b/cmake/FindDIRECTSHOW.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindDL.cmake b/cmake/FindDL.cmake
index 0735b44..4d48b19 100644
--- a/cmake/FindDL.cmake
+++ b/cmake/FindDL.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindDMTX.cmake b/cmake/FindDMTX.cmake
index 63ba100..7d43300 100644
--- a/cmake/FindDMTX.cmake
+++ b/cmake/FindDMTX.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindErfc.cmake b/cmake/FindErfc.cmake
index 6735004..47f732c 100644
--- a/cmake/FindErfc.cmake
+++ b/cmake/FindErfc.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindFFMPEG.cmake b/cmake/FindFFMPEG.cmake
deleted file mode 100644
index cd7e6f6..0000000
--- a/cmake/FindFFMPEG.cmake
+++ /dev/null
@@ -1,311 +0,0 @@
-#############################################################################
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# See the file LICENSE.txt at the root directory of this source
-# distribution for additional information about the GNU GPL.
-#
-# For using ViSP with software that can not be combined with the GNU
-# GPL, please contact Inria about acquiring a ViSP Professional
-# Edition License.
-#
-# See http://visp.inria.fr for more information.
-#
-# This software was developed at:
-# Inria Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-#
-# If you have questions regarding the use of this file, please contact
-# Inria at visp at inria.fr
-#
-# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
-# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
-#
-# Description:
-# Try to find FFMPEG. FFMpeg depend son Zlib.
-# Once run this will define:
-#
-# FFMPEG_FOUND - system has FFMPEG
-# FFMPEG_INCLUDE_DIRS - the FFMPEG include directory
-# FFMPEG_LIBRARIES - Link these to use FFMPEG
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-# detection of the FFMPEG headers location
-if(MINGW)
- find_path(FFMPEG_INCLUDE_DIR_AVCODEC
- NAMES
- libavcodec/avcodec.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
- NAMES
- libavformat/avformat.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVUTIL
- NAMES
- libavutil/avutil.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_SWSCALE
- NAMES
- libswscale/swscale.h
- PATHS
- "$ENV{MINGW_DIR}/include"
- C:/mingw/include
- PATH_SUFFIXES
- libswscale
- ffmpeg
- )
-
- # Detection of the FFMPEG library on Unix
- find_library(FFMPEG_AVUTIL_LIBRARY
- NAMES
- avutil
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
- find_library(FFMPEG_AVCODEC_LIBRARY
- NAMES
- avcodec
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
- find_library(FFMPEG_AVFORMAT_LIBRARY
- NAMES
- avformat
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-
- find_library(FFMPEG_AVCORE_LIBRARY
- NAMES
- avcore
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-
- find_library(FFMPEG_SWSCALE_LIBRARY
- NAMES
- swscale
- PATHS
- "$ENV{MINGW_DIR}/lib64"
- C:/mingw/lib64
- )
-else(MINGW)
- find_path(FFMPEG_INCLUDE_DIR_AVCODEC
- NAMES
- libavcodec/avcodec.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVFORMAT
- NAMES
- libavformat/avformat.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_AVUTIL
- NAMES
- libavutil/avutil.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- ffmpeg
- )
-
- find_path(FFMPEG_INCLUDE_DIR_SWSCALE
- NAMES
- libswscale/swscale.h
- PATHS
- "/usr/include"
- "/usr/local/include"
- $ENV{FFMPEG_DIR}/include
- $ENV{FFMPEG_DIR}
- PATH_SUFFIXES
- libswscale
- ffmpeg
- )
-
- # Detection of the FFMPEG library on Unix
- find_library(FFMPEG_AVUTIL_LIBRARY
- NAMES
- avutil
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
- find_library(FFMPEG_AVCODEC_LIBRARY
- NAMES
- avcodec
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
- find_library(FFMPEG_AVFORMAT_LIBRARY
- NAMES
- avformat
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-
- find_library(FFMPEG_AVCORE_LIBRARY
- NAMES
- avcore
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-
- find_library(FFMPEG_SWSCALE_LIBRARY
- NAMES
- swscale
- PATHS
- /usr/lib
- /usr/local/lib
- /lib
- $ENV{FFMPEG_DIR}/lib
- $ENV{FFMPEG_DIR}/Release
- $ENV{FFMPEG_DIR}
- )
-endif(MINGW)
-
- # FFMpeg depend son Zlib
- find_package(ZLIB)
- if(NOT ZLIB_FOUND)
- find_package(MyZLIB)
- endif()
-
- # FFMpeg depend son BZip2
- find_package(BZip2 QUIET)
-
- # FFMpeg may depend also on iconv since probably version 1.1.3 where if detected,
- # iconv usage is enabled by default
- find_package(ICONV QUIET)
- #message("ICONV_FOUND: ${ICONV_FOUND}")
-
-IF(FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES AND BZIP2_LIBRARIES)
- SET(FFMPEG_FOUND TRUE)
- SET(FFMPEG_INCLUDE_DIRS
- ${FFMPEG_INCLUDE_DIR_AVCODEC}
- ${FFMPEG_INCLUDE_DIR_AVFORMAT}
- ${FFMPEG_INCLUDE_DIR_AVUTIL}
- ${FFMPEG_INCLUDE_DIR_SWSCALE}
- )
- SET(FFMPEG_LIBRARIES
- ${FFMPEG_SWSCALE_LIBRARY}
- ${FFMPEG_AVFORMAT_LIBRARY}
- ${FFMPEG_AVCODEC_LIBRARY}
- ${FFMPEG_AVUTIL_LIBRARY}
- )
- if(FFMPEG_AVCORE_LIBRARY)
- LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
- endif()
- list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES} ${BZIP2_LIBRARIES})
- if(ICONV_FOUND)
- list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
- endif()
-
-elseif(MINGW AND FFMPEG_INCLUDE_DIR_AVCODEC AND FFMPEG_INCLUDE_DIR_AVFORMAT AND FFMPEG_INCLUDE_DIR_AVUTIL AND FFMPEG_INCLUDE_DIR_SWSCALE AND FFMPEG_SWSCALE_LIBRARY AND FFMPEG_AVFORMAT_LIBRARY AND FFMPEG_AVCODEC_LIBRARY AND FFMPEG_AVUTIL_LIBRARY AND ZLIB_LIBRARIES)
- # Bzip2 is nor requested with mingw-w64
- SET(FFMPEG_FOUND TRUE)
- SET(FFMPEG_INCLUDE_DIRS
- ${FFMPEG_INCLUDE_DIR_AVCODEC}
- ${FFMPEG_INCLUDE_DIR_AVFORMAT}
- ${FFMPEG_INCLUDE_DIR_AVUTIL}
- ${FFMPEG_INCLUDE_DIR_SWSCALE}
- )
- SET(FFMPEG_LIBRARIES
- ${FFMPEG_SWSCALE_LIBRARY}
- ${FFMPEG_AVFORMAT_LIBRARY}
- ${FFMPEG_AVCODEC_LIBRARY}
- ${FFMPEG_AVUTIL_LIBRARY}
- )
- if(FFMPEG_AVCORE_LIBRARY)
- LIST(APPEND FFMPEG_LIBRARIES ${FFMPEG_AVCORE_LIBRARY})
- endif()
- list(APPEND FFMPEG_LIBRARIES ${ZLIB_LIBRARIES})
- if(ICONV_FOUND)
- list(APPEND FFMPEG_LIBRARIES ${ICONV_LIBRARIES})
- endif()
-
-ELSE()
- SET(FFMPEG_FOUND FALSE)
-ENDIF ()
-
-MARK_AS_ADVANCED(
- BZIP2_DIR
- FFMPEG_INCLUDE_DIR_AVCODEC
- FFMPEG_INCLUDE_DIR_AVFORMAT
- FFMPEG_INCLUDE_DIR_AVUTIL
- FFMPEG_INCLUDE_DIR_SWSCALE
- FFMPEG_AVUTIL_LIBRARY
- FFMPEG_AVFORMAT_LIBRARY
- FFMPEG_AVCODEC_LIBRARY
- FFMPEG_SWSCALE_LIBRARY
- FFMPEG_AVCORE_LIBRARY
- FFMPEG_INCLUDE_DIRS
- FFMPEG_LIBRARIES
-)
-
diff --git a/cmake/FindFlyCapture.cmake b/cmake/FindFlyCapture.cmake
index d90da94..6e2d3e8 100644
--- a/cmake/FindFlyCapture.cmake
+++ b/cmake/FindFlyCapture.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindGDI.cmake b/cmake/FindGDI.cmake
index 7cc0bcf..344404d 100644
--- a/cmake/FindGDI.cmake
+++ b/cmake/FindGDI.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -68,7 +69,9 @@ IF(WIN32)
"C:/Program Files (x86)/Microsoft SDKs/Windows/v7.0A/Lib/x64"
"C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib/x64"
"C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x64"
- "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x64"
+ "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x64"
+ "C:/Program Files (x86)/Windows Kits/10/Lib/10.0.15063.0/um/x64"
+ "C:/Program Files (x86)/Windows Kits/10/Lib/10.0.14393.0/um/x86"
"C:/Program Files/Microsoft Platform SDK/Lib/x64"
"C:/DXSDK/Include/Lib/x64"
DOC "Where can the GDI (Graphics Device Interface) library be found"
@@ -113,7 +116,9 @@ IF(WIN32)
"C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1/Lib"
"C:/Program Files (x86)/Microsoft SDKs/Windows/v7.1A/Lib"
"C:/Program Files (x86)/Windows Kits/8.0/Lib/win8/um/x86"
- "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x86"
+ "C:/Program Files (x86)/Windows Kits/8.1/Lib/winv6.3/um/x86"
+ "C:/Program Files (x86)/Windows Kits/10/Lib/10.0.15063.0/um/x86"
+ "C:/Program Files (x86)/Windows Kits/10/Lib/10.0.14393.0/um/x86"
"C:/Program Files/Microsoft Platform SDK/Lib"
"C:/DXSDK/Include/Lib"
DOC "Where can the GDI (Graphics Device Interface) library be found"
diff --git a/cmake/FindGSL.cmake b/cmake/FindGSL.cmake
index af50b82..116523d 100644
--- a/cmake/FindGSL.cmake
+++ b/cmake/FindGSL.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -154,26 +155,24 @@ else()
vp_get_version_from_pkg("gsl" "${GSL_LIB_DIR}/pkgconfig" GSL_VERSION)
else()
- # Try to add gslcblas library if requested
-
- find_library(GSL_cblas_LIBRARY
- NAMES gslcblas
- PATHS ${GSL_LIB_SEARCH_PATH}
- )
- if(GSL_cblas_LIBRARY)
- list(APPEND GSL_LIBRARIES ${GSL_cblas_LIBRARY})
- #message("add cblas to GSL_LIBRARIES: ${GSL_LIBRARIES}")
- CheckCompilation_gsl2(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} ${GSL_cblas_LIBRARY} BUILD_SUCCEED2)
- #message("BUILD_STATUS 2: ${BUILD_SUCCEED2}")
- if(BUILD_SUCCEED2)
- set(GSL_FOUND TRUE)
-
- get_filename_component(GSL_LIB_DIR ${GSL_gsl_LIBRARY} PATH)
- vp_get_version_from_pkg("gsl" "${GSL_LIB_DIR}/pkgconfig" GSL_VERSION)
-
- else()
- set(GSL_FOUND FALSE)
- endif()
+ set(GSL_FOUND FALSE)
+ endif()
+
+ # Try to add gslcblas library if requested
+ find_library(GSL_cblas_LIBRARY
+ NAMES gslcblas
+ PATHS ${GSL_LIB_SEARCH_PATH}
+ )
+ if(GSL_cblas_LIBRARY)
+ list(APPEND GSL_LIBRARIES ${GSL_cblas_LIBRARY})
+ #message("add cblas to GSL_LIBRARIES: ${GSL_LIBRARIES}")
+ CheckCompilation_gsl2(${GSL_INCLUDE_DIRS} ${GSL_gsl_LIBRARY} ${GSL_cblas_LIBRARY} BUILD_SUCCEED2)
+ #message("BUILD_STATUS 2: ${BUILD_SUCCEED2}")
+ if(BUILD_SUCCEED2)
+ set(GSL_FOUND TRUE)
+
+ get_filename_component(GSL_LIB_DIR ${GSL_gsl_LIBRARY} PATH)
+ vp_get_version_from_pkg("gsl" "${GSL_LIB_DIR}/pkgconfig" GSL_VERSION)
else()
set(GSL_FOUND FALSE)
endif()
diff --git a/cmake/FindICONV.cmake b/cmake/FindICONV.cmake
index 2e76a3f..a77ce95 100644
--- a/cmake/FindICONV.cmake
+++ b/cmake/FindICONV.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindIRISA.cmake b/cmake/FindIRISA.cmake
index 3ed8b4d..81a91b1 100644
--- a/cmake/FindIRISA.cmake
+++ b/cmake/FindIRISA.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindIsInf.cmake b/cmake/FindIsInf.cmake
index 6ca46ff..97da4a9 100644
--- a/cmake/FindIsInf.cmake
+++ b/cmake/FindIsInf.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindIsNaN.cmake b/cmake/FindIsNaN.cmake
index 4da038a..1a01f8f 100644
--- a/cmake/FindIsNaN.cmake
+++ b/cmake/FindIsNaN.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindLAPACK_C.cmake b/cmake/FindLAPACK_C.cmake
index 3bdf3fa..20669b5 100644
--- a/cmake/FindLAPACK_C.cmake
+++ b/cmake/FindLAPACK_C.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindLIBFREENECT.cmake b/cmake/FindLIBFREENECT.cmake
index f9c980e..157cdd4 100644
--- a/cmake/FindLIBFREENECT.cmake
+++ b/cmake/FindLIBFREENECT.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindLIBUSB_1.cmake b/cmake/FindLIBUSB_1.cmake
index 70269a7..ac2fd0c 100644
--- a/cmake/FindLIBUSB_1.cmake
+++ b/cmake/FindLIBUSB_1.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindLog1p.cmake b/cmake/FindLog1p.cmake
new file mode 100644
index 0000000..efeaa08
--- /dev/null
+++ b/cmake/FindLog1p.cmake
@@ -0,0 +1,57 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# Try to find log1p function.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+include(CheckCXXSourceCompiles)
+
+macro(check_math_expr _expr _var)
+ unset(${_var} CACHE)
+ if(USE_CPP11)
+ set(CMAKE_REQUIRED_FLAGS ${CPP11_CXX_FLAGS})
+ endif()
+ check_cxx_source_compiles("
+#include <math.h>
+int main(int argc, char ** argv)
+{
+ (void)${_expr};
+ return 0;
+}
+" ${_var})
+endmacro()
+
+check_math_expr("log1p(1.0)" HAVE_FUNC_LOG1P)
+
diff --git a/cmake/FindMyCoin3D.cmake b/cmake/FindMyCoin3D.cmake
index e7ef8d6..ece8c6b 100644
--- a/cmake/FindMyCoin3D.cmake
+++ b/cmake/FindMyCoin3D.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindMyGTK2.cmake b/cmake/FindMyGTK2.cmake
index fe8a855..0ab0e37 100644
--- a/cmake/FindMyGTK2.cmake
+++ b/cmake/FindMyGTK2.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindMyJPEG.cmake b/cmake/FindMyJPEG.cmake
index 7e2e9e1..7baca07 100644
--- a/cmake/FindMyJPEG.cmake
+++ b/cmake/FindMyJPEG.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindMyOpenCV.cmake b/cmake/FindMyOpenCV.cmake
index 0ce633e..8d095c1 100644
--- a/cmake/FindMyOpenCV.cmake
+++ b/cmake/FindMyOpenCV.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindMyPNG.cmake b/cmake/FindMyPNG.cmake
index 188edc0..7c3bf03 100644
--- a/cmake/FindMyPNG.cmake
+++ b/cmake/FindMyPNG.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindMyZLIB.cmake b/cmake/FindMyZLIB.cmake
index 7da6036..bcb46f6 100644
--- a/cmake/FindMyZLIB.cmake
+++ b/cmake/FindMyZLIB.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindPARPORT.cmake b/cmake/FindPARPORT.cmake
index a92a29f..47ce064 100644
--- a/cmake/FindPARPORT.cmake
+++ b/cmake/FindPARPORT.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindPTHREAD.cmake b/cmake/FindPTHREAD.cmake
index c3e7e93..d115ad7 100644
--- a/cmake/FindPTHREAD.cmake
+++ b/cmake/FindPTHREAD.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindPTU46.cmake b/cmake/FindPTU46.cmake
index d911c82..77359d0 100644
--- a/cmake/FindPTU46.cmake
+++ b/cmake/FindPTU46.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindPylon.cmake b/cmake/FindPylon.cmake
new file mode 100644
index 0000000..254fde3
--- /dev/null
+++ b/cmake/FindPylon.cmake
@@ -0,0 +1,163 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# Try to find Basler Pylon library API
+#
+# PYLON_FOUND
+# PYLON_INCLUDE_DIRS
+# PYLON_LIBRARIES
+# PYLON_VERSION
+#
+# Authors:
+# Wenfeng CAI
+# Fabien Spindler : compat with OSX and Windows
+#
+#############################################################################
+
+if(APPLE)
+ find_path(PYLON_INCLUDE_DIR pylon/PylonIncludes.h)
+ find_path(PYLON_BASE_INCLUDE_DIR
+ NAMES Base/GCTypes.h
+ HINTS ${PYLON_INCLUDE_DIR}/Headers
+ PATH_SUFFIXES GenICam)
+
+ find_library(PYLON_LIBRARIES pylon)
+
+ find_program(PYLON_CONFIG pylon-config
+ HINTS ${PYLON_INCLUDE_DIR}
+ PATH_SUFFIXES Versions/A/Resources/Tools)
+
+ if(PYLON_INCLUDE_DIR)
+ list(APPEND PYLON_INCLUDE_DIRS ${PYLON_INCLUDE_DIR})
+ endif()
+ if(PYLON_BASE_INCLUDE_DIR)
+ list(APPEND PYLON_INCLUDE_DIRS ${PYLON_BASE_INCLUDE_DIR})
+ endif()
+
+ if(PYLON_CONFIG)
+ execute_process(COMMAND ${PYLON_CONFIG} "--version"
+ OUTPUT_VARIABLE PYLON_VERSION_TMP
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ string(REPLACE "-n" "" PYLON_VERSION ${PYLON_VERSION_TMP})
+ endif()
+
+ if(PYLON_INCLUDE_DIRS AND PYLON_LIBRARIES)
+ set(PYLON_FOUND TRUE)
+ endif()
+
+ mark_as_advanced(PYLON_BASE_INCLUDE_DIR)
+elseif(WIN32)
+ find_path(PYLON_INCLUDE_DIR pylon/PylonIncludes.h
+ PATHS "$ENV{PYLON_HOME}/include"
+ "C:/Program Files/Basler/pylon 5/Development/include")
+
+ set(PYLON_LIB_SEARCH_PATH "$ENV{PYLON_HOME}/lib/x64")
+
+ if(CMAKE_CL_64)
+ list(APPEND PYLON_LIB_SEARCH_PATH "C:/Program Files/Basler/pylon 5/Development/lib/x64")
+ else()
+ list(APPEND PYLON_LIB_SEARCH_PATH "C:/Program Files/Basler/pylon 5/Development/lib/Win32")
+ endif()
+
+ find_library(PYLON_BASE_LIBRARY
+ NAMES PylonBase_MD_VC120_v5_0.lib
+ PATHS ${PYLON_LIB_SEARCH_PATH})
+ find_library(PYLON_GCBASE_LIBRARY
+ NAMES GCBase_MD_VC120_v3_0_Basler_pylon_v5_0.lib
+ PATHS ${PYLON_LIB_SEARCH_PATH})
+ find_library(PYLON_GENAPI_LIBRARY
+ NAMES GenApi_MD_VC120_v3_0_Basler_pylon_v5_0.lib
+ PATHS ${PYLON_LIB_SEARCH_PATH})
+ find_library(PYLON_UTILITY_LIBRARY
+ NAMES PylonUtility_MD_VC120_v5_0.lib
+ PATHS ${PYLON_LIB_SEARCH_PATH})
+
+ if(PYLON_INCLUDE_DIR)
+ list(APPEND PYLON_INCLUDE_DIRS ${PYLON_INCLUDE_DIR})
+ endif()
+ if(PYLON_BASE_LIBRARY AND PYLON_GCBASE_LIBRARY)
+ list(APPEND PYLON_LIBRARIES ${PYLON_BASE_LIBRARY})
+ list(APPEND PYLON_LIBRARIES ${PYLON_GCBASE_LIBRARY})
+ list(APPEND PYLON_LIBRARIES ${PYLON_GENAPI_LIBRARY})
+ list(APPEND PYLON_LIBRARIES ${PYLON_UTILITY_LIBRARY})
+ endif()
+
+ if(PYLON_INCLUDE_DIRS AND PYLON_LIBRARIES)
+ vp_parse_header("${PYLON_INCLUDE_DIR}/pylon/PylonVersionNumber.h" PYLON_VERSION_LINES PYLON_VERSION_MAJOR PYLON_VERSION_MINOR PYLON_VERSION_SUBMINOR)
+ set(PYLON_VERSION "${PYLON_VERSION_MAJOR}.${PYLON_VERSION_MINOR}.${PYLON_VERSION_SUBMINOR}")
+ set(PYLON_FOUND TRUE)
+ endif()
+
+ mark_as_advanced(
+ PYLON_BASE_LIBRARY
+ PYLON_GCBASE_LIBRARY
+ PYLON_GENAPI_LIBRARY
+ PYLON_UTILITY_LIBRARY
+ )
+
+elseif(UNIX)
+ set(PYLON_ROOT_SEARCH_PATH /opt/pylon5)
+ # For more possible versions, just add more paths below.
+ # list(APPEND PYLON_ROOT_SEARCH_PATH "/somepath/include")
+
+ find_program(PYLON_CONFIG pylon-config
+ PATHS ${PYLON_ROOT}
+ PATHS $ENV{PYLON_ROOT}
+ PATHS ${PYLON_ROOT_SEARCH_PATH}
+ PATH_SUFFIXES bin)
+
+ if(PYLON_CONFIG)
+ execute_process(COMMAND ${PYLON_CONFIG} "--version"
+ OUTPUT_VARIABLE PYLON_VERSION
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ execute_process(COMMAND ${PYLON_CONFIG} "--libs" "--libs-rpath"
+ OUTPUT_VARIABLE PYLON_LIBRARIES
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ execute_process(COMMAND ${PYLON_CONFIG} "--cflags-only-I"
+ OUTPUT_VARIABLE PYLON_INC_TMP
+ OUTPUT_STRIP_TRAILING_WHITESPACE)
+ string(REPLACE "-I" "" PYLON_INCLUDE_DIRS ${PYLON_INC_TMP})
+
+ set(PYLON_FOUND TRUE)
+ endif()
+else()
+ set(PYLON_FOUND FALSE)
+ message(STATUS "Pylon SDK not found.
+ If you are sure Pylon SDK is installed, set CMake variable or
+ environment variable `PYLON_ROOT' to help CMake to find Pylon SDK.")
+endif()
+
+mark_as_advanced(
+ PYLON_INCLUDE_DIR
+ PYLON_INCLUDE_DIRS
+ PYLON_LIBRARIES
+ PYLON_CONFIG
+)
diff --git a/cmake/FindRAW1394.cmake b/cmake/FindRAW1394.cmake
index ea47586..cbc7836 100644
--- a/cmake/FindRAW1394.cmake
+++ b/cmake/FindRAW1394.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindRT.cmake b/cmake/FindRT.cmake
index ef191b4..f3b04df 100644
--- a/cmake/FindRT.cmake
+++ b/cmake/FindRT.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindRealSense.cmake b/cmake/FindRealSense.cmake
index 3952149..3b7a94c 100644
--- a/cmake/FindRealSense.cmake
+++ b/cmake/FindRealSense.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -48,13 +49,19 @@ if(MSVC)
list(APPEND REALSENSE_INC_SEARCH_PATH "C:/librealsense/include")
list(APPEND REALSENSE_INC_SEARCH_PATH $ENV{REALSENSE_HOME}/include)
+ list(APPEND REALSENSE_INC_SEARCH_PATH $ENV{REALSENSE_DIR}/include)
+
+ list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_HOME}/lib)
+ list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_DIR}/lib)
if(CMAKE_CL_64)
list(APPEND REALSENSE_LIB_SEARCH_PATH "C:/librealsense/bin/x64")
list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_HOME}/bin/x64)
+ list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_DIR}/bin/x64)
else()
list(APPEND REALSENSE_LIB_SEARCH_PATH "C:/librealsense/bin/Win32")
list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_HOME}/bin/Win32)
+ list(APPEND REALSENSE_LIB_SEARCH_PATH $ENV{REALSENSE_DIR}/bin/Win32)
endif()
else()
@@ -81,11 +88,13 @@ if(MSVC)
PATHS
${REALSENSE_LIB_SEARCH_PATH}
)
- if(REALSENSE_LIBRARIES_OPT)
+ if(REALSENSE_LIBRARIES_DBG AND REALSENSE_LIBRARIES_OPT)
set(REALSENSE_LIBRARIES optimized ${REALSENSE_LIBRARIES_OPT})
- endif()
- if(REALSENSE_LIBRARIES_DBG)
list(APPEND REALSENSE_LIBRARIES debug ${REALSENSE_LIBRARIES_DBG})
+ elseif(REALSENSE_LIBRARIES_OPT)
+ set(REALSENSE_LIBRARIES ${REALSENSE_LIBRARIES_OPT})
+ elseif(REALSENSE_LIBRARIES_DBG)
+ set(REALSENSE_LIBRARIES ${REALSENSE_LIBRARIES_DBG})
endif()
mark_as_advanced(REALSENSE_LIBRARIES_OPT REALSENSE_LIBRARIES_DBG)
diff --git a/cmake/FindRealSense2.cmake b/cmake/FindRealSense2.cmake
new file mode 100644
index 0000000..1bc5d41
--- /dev/null
+++ b/cmake/FindRealSense2.cmake
@@ -0,0 +1,94 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# Try to find Intel RealSense SDK to work with R200, F200 and SR300 devices.
+# Once run this will define:
+#
+# REALSENSE2_FOUND
+# REALSENSE2_INCLUDE_DIRS
+# REALSENSE2_LIBRARIES
+# REALSENSE2_VERSION
+#
+#############################################################################
+
+set(REALSENSE2_INC_SEARCH_PATH /usr/local/include)
+set(REALSENSE2_LIB_SEARCH_PATH /usr/local/lib)
+
+if(MSVC)
+ list(APPEND REALSENSE2_INC_SEARCH_PATH "C:/librealsense2/include")
+
+ list(APPEND REALSENSE2_INC_SEARCH_PATH $ENV{REALSENSE2_HOME}/include)
+ list(APPEND REALSENSE2_INC_SEARCH_PATH $ENV{REALSENSE2_DIR}/include)
+ list(APPEND REALSENSE2_INC_SEARCH_PATH "C:/Program Files (x86)/Intel RealSense SDK 2.0/include")
+
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH $ENV{REALSENSE2_HOME}/lib)
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH $ENV{REALSENSE2_DIR}/lib)
+
+ if(CMAKE_CL_64)
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH "C:/Program Files (x86)/Intel RealSense SDK 2.0/lib/x64")
+ else()
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH "C:/Program Files (x86)/Intel RealSense SDK 2.0/lib/x86")
+ endif()
+else()
+ list(APPEND REALSENSE2_INC_SEARCH_PATH /usr/include)
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH /usr/lib)
+
+ list(APPEND REALSENSE2_INC_SEARCH_PATH $ENV{REALSENSE2_HOME}/include)
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH $ENV{REALSENSE2_HOME}/lib)
+ list(APPEND REALSENSE2_INC_SEARCH_PATH $ENV{REALSENSE2_DIR}/include)
+ list(APPEND REALSENSE2_LIB_SEARCH_PATH $ENV{REALSENSE2_DIR}/lib)
+endif()
+
+find_path(REALSENSE2_INCLUDE_DIRS librealsense2/rs.hpp
+ PATHS
+ ${REALSENSE2_INC_SEARCH_PATH}
+)
+
+find_library(REALSENSE2_LIBRARIES
+ NAMES realsense2
+ PATHS
+ ${REALSENSE2_LIB_SEARCH_PATH}
+)
+
+if(REALSENSE2_LIBRARIES AND REALSENSE2_INCLUDE_DIRS)
+ set(REALSENSE2_FOUND TRUE)
+ vp_parse_header("${REALSENSE2_INCLUDE_DIRS}/librealsense2/rs.h" REALSENSE2_VERSION_LINES RS2_API_MAJOR_VERSION RS2_API_MINOR_VERSION RS2_API_PATCH_VERSION)
+ set(REALSENSE2_VERSION "${RS2_API_MAJOR_VERSION}.${RS2_API_MINOR_VERSION}.${RS2_API_PATCH_VERSION}")
+else()
+ set(REALSENSE2_FOUND FALSE)
+endif()
+
+mark_as_advanced(
+ REALSENSE2_INCLUDE_DIRS
+ REALSENSE2_LIBRARIES
+ REALSENSE2_INC_SEARCH_PATH
+ REALSENSE2_LIB_SEARCH_PATH
+)
diff --git a/cmake/FindRound.cmake b/cmake/FindRound.cmake
index 4cd15d9..614c62e 100644
--- a/cmake/FindRound.cmake
+++ b/cmake/FindRound.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindSOQT.cmake b/cmake/FindSOQT.cmake
index eeb585d..0b54daa 100644
--- a/cmake/FindSOQT.cmake
+++ b/cmake/FindSOQT.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindSOWIN.cmake b/cmake/FindSOWIN.cmake
index dad959c..b8a2e0d 100644
--- a/cmake/FindSOWIN.cmake
+++ b/cmake/FindSOWIN.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindSOXT.cmake b/cmake/FindSOXT.cmake
index 962e2b1..39c6680 100644
--- a/cmake/FindSOXT.cmake
+++ b/cmake/FindSOXT.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindStrtof.cmake b/cmake/FindStrtof.cmake
index 44a4edf..f3d8500 100644
--- a/cmake/FindStrtof.cmake
+++ b/cmake/FindStrtof.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindV4L2.cmake b/cmake/FindV4L2.cmake
index cd62472..4f5b325 100644
--- a/cmake/FindV4L2.cmake
+++ b/cmake/FindV4L2.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindViper650_data.cmake b/cmake/FindViper650_data.cmake
index 760675e..595598f 100644
--- a/cmake/FindViper650_data.cmake
+++ b/cmake/FindViper650_data.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindViper850_data.cmake b/cmake/FindViper850_data.cmake
index 4f00c1f..c6b0bd4 100644
--- a/cmake/FindViper850_data.cmake
+++ b/cmake/FindViper850_data.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindVirtuose.cmake b/cmake/FindVirtuose.cmake
index 325af68..9d37f63 100644
--- a/cmake/FindVirtuose.cmake
+++ b/cmake/FindVirtuose.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindXML2.cmake b/cmake/FindXML2.cmake
index 0b3c6c1..6d79e48 100644
--- a/cmake/FindXML2.cmake
+++ b/cmake/FindXML2.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/FindZBAR.cmake b/cmake/FindZBAR.cmake
index 6963f97..27d3aca 100644
--- a/cmake/FindZBAR.cmake
+++ b/cmake/FindZBAR.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -47,15 +48,18 @@ set(ZBAR_LIB_SEARCH_PATH "")
if(MSVC)
list(APPEND ZBAR_INC_SEARCH_PATH $ENV{ZBAR_DIR}/include)
list(APPEND ZBAR_INC_SEARCH_PATH "C:/ZBarWin64/include")
+ list(APPEND ZBAR_INC_SEARCH_PATH "C:/workspace/ZBarWin64/include")
list(APPEND ZBAR_INC_SEARCH_PATH "C:/zbar/ZBarWin64/include")
if(CMAKE_CL_64)
list(APPEND ZBAR_LIB_SEARCH_PATH $ENV{ZBAR_DIR}/x64)
list(APPEND ZBAR_LIB_SEARCH_PATH "C:/ZBarWin64/x64")
+ list(APPEND ZBAR_LIB_SEARCH_PATH "C:/workspace/ZBarWin64/x64")
list(APPEND ZBAR_LIB_SEARCH_PATH "C:/zbar/ZBarWin64/x64")
else()
list(APPEND ZBAR_LIB_SEARCH_PATH $ENV{ZBAR_DIR})
list(APPEND ZBAR_LIB_SEARCH_PATH "C:/ZBarWin64")
+ list(APPEND ZBAR_LIB_SEARCH_PATH "C:/workspace/ZBarWin64")
list(APPEND ZBAR_LIB_SEARCH_PATH "C:/zbar/ZBarWin64")
endif()
@@ -90,11 +94,14 @@ if(MSVC)
PATHS
${ZBAR_LIB_SEARCH_PATH_DBG}
)
- if(ZBAR_LIBRARIES_OPT)
+
+ if(ZBAR_LIBRARIES_OPT AND ZBAR_LIBRARIES_DBG)
set(ZBAR_LIBRARIES optimized ${ZBAR_LIBRARIES_OPT})
- endif()
- if(ZBAR_LIBRARIES_DBG)
list(APPEND ZBAR_LIBRARIES debug ${ZBAR_LIBRARIES_DBG})
+ elseif(ZBAR_LIBRARIES_OPT)
+ list(APPEND ZBAR_LIBRARIES ${ZBAR_LIBRARIES_OPT})
+ elseif(ZBAR_LIBRARIES_DBG)
+ list(APPEND ZBAR_LIBRARIES ${ZBAR_LIBRARIES_DBG})
endif()
mark_as_advanced(ZBAR_LIBRARIES_OPT ZBAR_LIBRARIES_DBG)
diff --git a/cmake/OgreTools.cmake b/cmake/OgreTools.cmake
index b4b7755..1b9e53f 100644
--- a/cmake/OgreTools.cmake
+++ b/cmake/OgreTools.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -39,6 +40,8 @@ if(WIN32)
mark_as_advanced(OGRE_FRAMEWORK_PATH)
endif()
+include(FindPkgMacros OPTIONAL)
+
#########################################################
# Find Ogre plugins
#
@@ -100,7 +103,8 @@ macro(vp_create_ogre_plugin_config_file)
if(OGRE_RenderSystem_GL_LIBRARY_REL)
get_filename_component(OGRE_PLUGIN_DIR_REL ${OGRE_RenderSystem_GL_LIBRARY_REL} PATH)
#message("set manually OGRE_PLUGIN_DIR_REL to ${OGRE_PLUGIN_DIR_REL}")
- elseif(OGRE_RenderSystem_GL_LIBRARY_DBG)
+ endif()
+ if(OGRE_RenderSystem_GL_LIBRARY_DBG)
get_filename_component(OGRE_PLUGIN_DIR_DBG ${OGRE_RenderSystem_GL_LIBRARY_DBG} PATH)
#message("set manually OGRE_PLUGIN_DIR_DBG to ${OGRE_PLUGIN_DIR_DBG}")
endif()
@@ -137,7 +141,7 @@ macro(vp_create_ogre_plugin_config_file)
#MESSAGE("PLUGINS_CONTENT_REL: ${PLUGINS_CONTENT_REL}")
file(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins.cfg" ${PLUGINS_CONTENT_REL})
endif()
-
+
if(OGRE_PLUGIN_DIR_DBG)
list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D9_LIBRARY_DBG})
list(APPEND PLUGIN_DBG ${OGRE_RenderSystem_Direct3D10_LIBRARY_DBG})
@@ -165,7 +169,7 @@ macro(vp_create_ogre_plugin_config_file)
list(APPEND PLUGINS_CONTENT_DBG " Plugin=${PLUGIN_NAME}\n")
endif()
endforeach()
-
+
#MESSAGE("PLUGINS_CONTENT_DBG: ${PLUGINS_CONTENT_DBG}")
file(WRITE "${VISP_HAVE_OGRE_PLUGINS_PATH}/plugins_d.cfg" ${PLUGINS_CONTENT_DBG})
endif()
diff --git a/cmake/PCLTools.cmake b/cmake/PCLTools.cmake
index e7226c6..3170c44 100644
--- a/cmake/PCLTools.cmake
+++ b/cmake/PCLTools.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/VISP3rdParty.cmake b/cmake/VISP3rdParty.cmake
index 11f6bd8..24141ca 100644
--- a/cmake/VISP3rdParty.cmake
+++ b/cmake/VISP3rdParty.cmake
@@ -2,9 +2,41 @@
# Add 3rd-party libraries build as static libs
#----------------------------------------------------------------------
-if(BUILD_ATIDAQ)
- set(ATIDAQ_LIBRARY atidaq_c)
- add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/libatidaq-c")
+if(WITH_ATIDAQ)
+ set(ATIDAQ_LIBRARY visp_atidaq)
+ add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/atidaq")
set(ATIDAQ_INCLUDE_DIRS "${${ATIDAQ_LIBRARY}_SOURCE_DIR}" "${${ATIDAQ_LIBRARY}_BINARY_DIR}")
set(ATIDAQ_LIBRARIES ${ATIDAQ_LIBRARY})
+ set(ATIDAQ_VERSION ${ATIDAQ_MAJOR_VERSION}.${ATIDAQ_MINOR_VERSION}.${ATIDAQ_PATCH_VERSION})
+endif()
+
+if(WITH_CLIPPER)
+ set(CLIPPER_LIBRARY visp_clipper)
+ add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/clipper")
+ set(CLIPPER_INCLUDE_DIRS "${${CLIPPER_LIBRARY}_SOURCE_DIR}" "${${CLIPPER_LIBRARY}_BINARY_DIR}")
+ set(CLIPPER_LIBRARIES ${CLIPPER_LIBRARY})
+ set(CLIPPER_VERSION ${CLIPPER_MAJOR_VERSION}.${CLIPPER_MINOR_VERSION}.${CLIPPER_PATCH_VERSION})
+endif()
+
+if(WITH_LAPACK)
+ set(LAPACK_LIBRARY visp_lapack)
+ add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/clapack")
+ set(LAPACK_LIBRARIES ${LAPACK_LIBRARY})
+ set(LAPACK_VERSION ${LAPACK_MAJOR_VERSION}.${LAPACK_MINOR_VERSION}.${LAPACK_PATCH_VERSION})
+endif()
+
+if(WITH_PTHREAD)
+ set(PTHREADS_LIBRARY visp_pthread)
+ add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/pthreads-win32")
+ set(PTHREADS_INCLUDE_DIRS "${${PTHREADS_LIBRARY}_SOURCE_DIR}" "${${PTHREADS_LIBRARY}_BINARY_DIR}")
+ set(PTHREADS_LIBRARIES ${PTHREADS_LIBRARY})
+ set(PTHREADS_VERSION ${PTHREADS_MAJOR_VERSION}.${PTHREADS_MINOR_VERSION}.${PTHREADS_PATCH_VERSION})
+endif()
+
+if(WITH_APRILTAG)
+ set(APRILTAG_LIBRARY visp_apriltag)
+ add_subdirectory("${VISP_SOURCE_DIR}/3rdparty/apriltag")
+ set(APRILTAG_INCLUDE_DIRS "${${APRILTAG_LIBRARY}_SOURCE_DIR}" "${${APRILTAG_LIBRARY}_BINARY_DIR}")
+ set(APRILTAG_LIBRARIES ${APRILTAG_LIBRARY})
+ set(APRILTAG_VERSION ${APRILTAG_MAJOR_VERSION}.${APRILTAG_MINOR_VERSION}.${APRILTAG_PATCH_VERSION})
endif()
diff --git a/cmake/VISPConfig.cmake b/cmake/VISPConfig.cmake
index 5059608..47173a1 100644
--- a/cmake/VISPConfig.cmake
+++ b/cmake/VISPConfig.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -48,7 +49,7 @@
# - VISP_USE_FILE : File to include to use ViSP without specific cmake code
#
# Windows specific variables:
-# - VISP_STATIC : If set to TRUE uses ViSP static library (.lib) rather then dynamic (.dll)
+# - VISP_STATIC : If set to TRUE uses ViSP static library (.lib) rather then dynamic (.dll)
#
# Typical usage in user project:
#
@@ -87,8 +88,10 @@ if(MSVC)
set(VISP_RUNTIME vc12)
elseif(MSVC_VERSION EQUAL 1900)
set(VISP_RUNTIME vc14)
- elseif(MSVC_VERSION EQUAL 1910)
+ elseif(MSVC_VERSION MATCHES "^191[0-9]$")
set(VISP_RUNTIME vc15)
+ else()
+ message(WARNING "ViSP does not recognize MSVC_VERSION \"${MSVC_VERSION}\". Cannot set VISP_RUNTIME")
endif()
elseif(MINGW)
set(VISP_RUNTIME mingw)
@@ -121,7 +124,7 @@ if(VISP_RUNTIME AND VISP_ARCH)
set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
elseif(VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib/VISPConfig.cmake")
set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/staticlib")
- elseif(VISP_STATIC EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
+ elseif(VISP_STATIC AND EXISTS "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib/VISPConfig.cmake")
set(VISP_LIB_PATH "${VISP_CONFIG_PATH}/${VISP_ARCH}/${VISP_RUNTIME}/lib")
endif()
endif()
diff --git a/cmake/VISPDetectPlatform.cmake b/cmake/VISPDetectPlatform.cmake
index 956334e..d7d1bd1 100644
--- a/cmake/VISPDetectPlatform.cmake
+++ b/cmake/VISPDetectPlatform.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -45,6 +46,33 @@ if(CMAKE_C_COMPILER_ID STREQUAL "Clang")
set(CMAKE_COMPILER_IS_CLANGCC 1)
endif()
+# ----------------------------------------------------------------------------
+# Detect Intel ICC compiler for -fPIC in 3rdparty ( UNIX ONLY )
+# NOTE: The system needs to determine if the '-fPIC' option needs to be added
+# for the 3rdparty static libs being compiled. The CMakeLists.txt files
+# in 3rdparty use the CV_ICC definition being set here to determine if
+# the -fPIC flag should be used.
+# ----------------------------------------------------------------------------
+if(UNIX)
+ if (__ICL)
+ set(CV_ICC __ICL)
+ elseif(__ICC)
+ set(CV_ICC __ICC)
+ elseif(__ECL)
+ set(CV_ICC __ECL)
+ elseif(__ECC)
+ set(CV_ICC __ECC)
+ elseif(__INTEL_COMPILER)
+ set(CV_ICC __INTEL_COMPILER)
+ elseif(CMAKE_C_COMPILER MATCHES "icc")
+ set(CV_ICC icc_matches_c_compiler)
+ endif()
+endif()
+
+if(MSVC AND CMAKE_C_COMPILER MATCHES "icc|icl")
+ set(CV_ICC __INTEL_COMPILER_FOR_WINDOWS)
+endif()
+
if(CMAKE_COMPILER_IS_GNUCXX)
if(WIN32)
execute_process(COMMAND ${CMAKE_CXX_COMPILER} -dumpmachine
@@ -68,6 +96,8 @@ elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^(arm.*|ARM.*)")
set(ARM 1)
elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^(aarch64.*|AARCH64.*)")
set(AARCH64 1)
+elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "^ppc64le.*|PPC64LE.*")
+ set(PPC64LE 1)
endif()
# Workaround for 32-bit operating systems on 64-bit x86_64 processor
@@ -108,8 +138,10 @@ if(MSVC)
set(VISP_RUNTIME vc12)
elseif(MSVC_VERSION EQUAL 1900)
set(VISP_RUNTIME vc14)
- elseif(MSVC_VERSION EQUAL 1910)
+ elseif(MSVC_VERSION MATCHES "^191[0-9]$")
set(VISP_RUNTIME vc15)
+ else()
+ message(WARNING "ViSP does not recognize MSVC_VERSION \"${MSVC_VERSION}\". Cannot set VISP_RUNTIME")
endif()
elseif(MINGW)
set(VISP_RUNTIME mingw)
diff --git a/cmake/VISPExtraTargets.cmake b/cmake/VISPExtraTargets.cmake
index e88453e..1dfe81b 100644
--- a/cmake/VISPExtraTargets.cmake
+++ b/cmake/VISPExtraTargets.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -51,8 +52,14 @@ endif()
# Doxygen documentation target, for "make visp_doc" and "make html-doc" (to keep compat with previous versions)
# ----------------------------------------------------------------------------
if(DOXYGEN_FOUND)
- add_custom_target(html-doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen) # for compat with previous versions
- add_custom_target(visp_doc ${DOXYGEN_EXECUTABLE} ${VISP_DOC_DIR}/config-doxygen)
+ add_custom_target(html-doc
+ COMMAND "${DOXYGEN_EXECUTABLE}" "${VISP_DOC_DIR}/config-doxygen"
+ DEPENDS "${VISP_DOC_DIR}/config-doxygen"
+ ) # for compat with previous versions
+ add_custom_target(visp_doc
+ COMMAND "${DOXYGEN_EXECUTABLE}" "${VISP_DOC_DIR}/config-doxygen"
+ DEPENDS "${VISP_DOC_DIR}/config-doxygen"
+ )
if(ENABLE_SOLUTION_FOLDERS)
set_target_properties(visp_doc PROPERTIES FOLDER "extra")
set_target_properties(html-doc PROPERTIES FOLDER "extra")
diff --git a/cmake/VISPGenerateConfig.cmake b/cmake/VISPGenerateConfig.cmake
index b2ee79f..51568d3 100644
--- a/cmake/VISPGenerateConfig.cmake
+++ b/cmake/VISPGenerateConfig.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -28,7 +29,7 @@
# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
#
# Description:
-# This file generates the ViSPConfig.cmake file:
+# This file generates the ViSPConfig.cmake file:
# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
# Part 2/3: ${BIN_DIR}/unix-install/VISPConfig.cmake -> For use with "make install"
# Part 3/3: ${BIN_DIR}/win-install/VISPConfig.cmake -> For use within binary installers/packages
@@ -71,6 +72,21 @@ if(VISP_LIB_COMPONENTS)
list(REMOVE_ITEM VISP_MODULES_CONFIGCMAKE ${VISP_LIB_COMPONENTS})
endif()
+# -------------------------------------------------------------------------------------------
+# Handle cmake extra vars (like USTK_HAVE_FFTW) that should be exported in VISPConfig.cmake thanks
+# to vp_cmake_configure()
+# All these vars are added in VISP_CONTRIB_MODULES_CONFIGCMAKE that is used in VISPConfig.cmake.in
+# See ustk_core/cmake/templates/VISPConfig-ustk_core.cmake.in example
+# -------------------------------------------------------------------------------------------
+set(VISP_CONTRIB_MODULES_CONFIGCMAKE "")
+foreach(m ${VISP_MODULES_BUILD} ${VISP_MODULES_DISABLED_USER} ${VISP_MODULES_DISABLED_AUTO} ${VISP_MODULES_DISABLED_FORCE})
+ set(__shortname ${m})
+ vp_short_module_name(__shortname)
+ if(EXISTS "${CMAKE_BINARY_DIR}/CMakeConfig-${__shortname}.cmake")
+ file(READ "${CMAKE_BINARY_DIR}/CMakeConfig-${__shortname}.cmake" __var)
+ set(VISP_CONTRIB_MODULES_CONFIGCMAKE "${VISP_CONTRIB_MODULES_CONFIGCMAKE}\n${__var}")
+ endif()
+endforeach()
# -------------------------------------------------------------------------------------------
# Part 1/3: ${BIN_DIR}/VISPConfig.cmake -> For use *without* "make install"
diff --git a/cmake/VISPGenerateConfigScript.cmake b/cmake/VISPGenerateConfigScript.cmake
new file mode 100644
index 0000000..a071425
--- /dev/null
+++ b/cmake/VISPGenerateConfigScript.cmake
@@ -0,0 +1,416 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# This file generates the ViSP library config shell scripts:
+# - visp-config in <build dir>/bin from visp-config.in
+# - visp-config in <build dir>/install from visp-config.install.in
+# When make install, this file is copied in <install dir>/bin
+# - visp.pc in <build dir>/install from visp.pc.in
+# When make install, this file is copied in <install dir>/lib/pkgconfig
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+if(MINGW OR IOS)
+ # not implemented yet
+ return()
+endif()
+
+if(NOT DEFINED CMAKE_HELPER_SCRIPT)
+
+ if(UNIX)
+ # for unix/osx platforms
+ set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/unix-install/visp-config")
+ set(FILE_VISP_CONFIG_PC_INSTALL "${VISP_BINARY_DIR}/unix-install/visp.pc")
+ else()
+ # for windows platforms
+ set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config.bat")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/win-install/visp-config-${VISP_ARCH}-${VISP_RUNTIME}.bat")
+ endif()
+
+ #---------------------------------------------------------------------
+ # Get cxx_flags to propagate in scripts
+ #----------------------------------------------------------------------
+ if(BUILD_TEST_COVERAGE)
+ # Add build options for test coverage. Currently coverage is only supported
+ # on gcc compiler
+ # Because using -fprofile-arcs with shared lib can cause problems like:
+ # hidden symbol `__bb_init_func', we add this option only for static
+ # library build
+ list(APPEND _cxx_flags "-ftest-coverage")
+ list(APPEND _cxx_flags "-fprofile-arcs")
+ endif()
+
+ # Propagate cxx11 compiler option if enabled during ViSP build
+ if(VISP_HAVE_CPP11_COMPATIBILITY)
+ list(APPEND _cxx_flags ${CPP11_CXX_FLAGS})
+ endif()
+ # Propagate openmp compiler option if enabled during ViSP build
+ set(VISP_OPENMP_SUPPORT "no")
+ if(VISP_HAVE_OPENMP)
+ list(APPEND _cxx_flags ${OpenMP_CXX_FLAGS})
+ set(VISP_OPENMP_SUPPORT "yes")
+ endif()
+
+ #---------------------------------------------------------------------
+ # Get include_flags to propagate in scripts
+ #----------------------------------------------------------------------
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND _include_flags_deps ${VISP_MODULE_${m}_INC_DEPS})
+ if(EXISTS "${VISP_MODULE_${m}_LOCATION}/include")
+ list(APPEND _include_flags_src "${VISP_MODULE_${m}_LOCATION}/include")
+ endif()
+ endforeach()
+
+ set(HELPER_SCRIPT "")
+
+ vp_cmake_script_append_var(HELPER_SCRIPT
+ BUILD_SHARED_LIBS
+ CMAKE_BINARY_DIR
+ CMAKE_INSTALL_PREFIX
+ BINARY_OUTPUT_PATH
+
+ VISP_SOURCE_DIR
+ VISP_VERSION
+ VISP_BINARY_DIR
+ VISP_LIB_INSTALL_PATH
+ VISP_BIN_INSTALL_PATH
+ VISP_INC_INSTALL_PATH
+ VISP_DEBUG_POSTFIX
+ VISP_OPENMP_SUPPORT
+
+ _cxx_flags
+ _include_flags_deps
+ _include_flags_src
+
+ FILE_VISP_CONFIG_SCRIPT
+ FILE_VISP_CONFIG_SCRIPT_INSTALL
+ FILE_VISP_CONFIG_PC_INSTALL
+
+ VISP_MODULES_BUILD
+ )
+
+ foreach(item ${VISP_MODULES_BUILD})
+ if(TARGET ${item})
+ if(UNIX)
+ set(HELPER_SCRIPT "${HELPER_SCRIPT}
+set(TARGET_LOCATION_${item} \"$<TARGET_FILE:${item}>\")
+")
+ else()
+ set(HELPER_SCRIPT "${HELPER_SCRIPT}
+set(TARGET_LOCATION_${item} \"${item}${VISP_VERSION_MAJOR}${VISP_VERSION_MINOR}${VISP_VERSION_PATCH}.dll\")
+")
+ endif()
+ endif()
+ foreach(dep ${VISP_MODULE_${item}_LINK_DEPS})
+ if(dep MATCHES "framework$")
+ get_filename_component(_fpath "${dep}" DIRECTORY)
+ get_filename_component(_fname "${dep}" NAME_WE)
+ list(APPEND _deps "-F${_fpath} -framework ${_fname}")
+ else()
+ list(APPEND _deps "${dep}")
+ endif()
+ endforeach()
+
+ vp_list_unique(_deps)
+ set(HELPER_SCRIPT "${HELPER_SCRIPT}
+set(VISP_MODULE_${item}_LINK_DEPS \"${_deps}\")
+")
+ endforeach()
+
+ set(CMAKE_HELPER_SCRIPT "${CMAKE_BINARY_DIR}/VISPGenerateConfigScript.info.cmake")
+ file(GENERATE OUTPUT "${CMAKE_HELPER_SCRIPT}" CONTENT "${HELPER_SCRIPT}")
+
+ add_custom_command(
+ OUTPUT "${FILE_VISP_CONFIG_SCRIPT}"
+ COMMAND ${CMAKE_COMMAND} "-DCMAKE_HELPER_SCRIPT=${CMAKE_HELPER_SCRIPT}" -P "${VISP_SOURCE_DIR}/cmake/VISPGenerateConfigScript.cmake"
+ DEPENDS "${CMAKE_BINARY_DIR}/VISPGenerateConfigScript.info.cmake"
+ "${VISP_SOURCE_DIR}/cmake/VISPGenerateConfigScript.cmake"
+ COMMENT "Generate visp-config"
+ )
+ add_custom_target(developer_scripts ALL SOURCES "${FILE_VISP_CONFIG_SCRIPT}")
+
+ #----------------------------------------------------------------------
+ # customize install target
+ #----------------------------------------------------------------------
+ # install rule for visp-config shell script
+ install(FILES ${FILE_VISP_CONFIG_SCRIPT_INSTALL}
+ DESTINATION ${VISP_BIN_INSTALL_PATH}
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
+ OWNER_WRITE
+ COMPONENT dev
+ )
+
+ # install rule for visp.pc pkg-config file
+ if(UNIX)
+ install(FILES ${FILE_VISP_CONFIG_PC_INSTALL}
+ DESTINATION ${VISP_LIB_INSTALL_PATH}/pkgconfig
+ PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
+ OWNER_WRITE
+ COMPONENT dev
+ )
+ else()
+ # not implemented yet
+ endif()
+
+# =============================================================================
+else() # DEFINED CMAKE_HELPER_SCRIPT
+
+ cmake_minimum_required(VERSION 2.8.12.2)
+ include("${CMAKE_HELPER_SCRIPT}")
+ include("${VISP_SOURCE_DIR}/cmake/VISPUtils.cmake")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_PREFIX
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
+
+ if(UNIX)
+ #######################################################################
+ #
+ # for Unix platforms: Linux, OSX
+ #
+ #######################################################################
+ set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.in")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.install.in")
+ set(FILE_VISP_CONFIG_PC_INSTALL_IN "cmake/templates/visp.pc.in")
+
+ #---------------------------------------------------------------------
+ # Updates vars needed to update VISP_CONFIG_LIBS_SCRIPT
+ #----------------------------------------------------------------------
+ # prepend with ViSP own modules first
+ set(VISP_CONFIG_LIBS_SCRIPT "")
+ foreach(m ${VISP_MODULES_BUILD})
+ get_filename_component(m_libname "${TARGET_LOCATION_${m}}" NAME)
+ list(APPEND VISP_CONFIG_LIBS_SCRIPT "$PREFIX/${VISP_LIB_INSTALL_PATH}/${m_libname}")
+ endforeach()
+ # append deps
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_CONFIG_LIBS_SCRIPT ${VISP_MODULE_${m}_LINK_DEPS})
+ endforeach()
+ vp_list_unique(VISP_CONFIG_LIBS_SCRIPT)
+ vp_list_filterout(VISP_CONFIG_LIBS_SCRIPT "debug")
+ vp_list_filterout(VISP_CONFIG_LIBS_SCRIPT "optimized")
+ # Format the string to suppress CMake separators ";"
+ vp_list_remove_separator(VISP_CONFIG_LIBS_SCRIPT)
+
+ set(VISP_ECHO_NO_NEWLINE_CHARACTER "")
+ set(VISP_ECHO_NO_NEWLINE_OPTION "")
+ if(APPLE)
+ set(VISP_ECHO_NO_NEWLINE_CHARACTER "\\c")
+ else()
+ set(VISP_ECHO_NO_NEWLINE_OPTION "-n")
+ endif()
+
+ #---------------------------------------------------------------------
+ # Updates vars needed to update VISP_CONFIG_CFLAGS_SCRIPT
+ # 1/ For usage with the build tree (where location of the <source tree>/<module>/include should be added)
+ # VISP_CONFIG_CFLAGS_SCRIPT = _cxx_flags + _include_flags_deps + _include_flags_src
+ # 2/ For usage with the install tree
+ # VISP_CONFIG_CFLAGS_SCRIPT = _cxx_flags + _include_flags_deps
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_CFLAGS_SCRIPT ${_cxx_flags})
+ list(APPEND VISP_CONFIG_CFLAGS_SCRIPT "-I$PREFIX/${VISP_INC_INSTALL_PATH}")
+ foreach(_inc ${_include_flags_src})
+ list(APPEND VISP_CONFIG_CFLAGS_SCRIPT "-I${_inc}")
+ endforeach()
+ foreach(_inc ${_include_flags_deps})
+ list(APPEND VISP_CONFIG_CFLAGS_SCRIPT "-I${_inc}")
+ endforeach()
+ vp_list_unique(VISP_CONFIG_CFLAGS_SCRIPT)
+ # Format the string to suppress CMake separators ";"
+ vp_list_remove_separator(VISP_CONFIG_CFLAGS_SCRIPT)
+ configure_file("${VISP_SOURCE_DIR}/${FILE_VISP_CONFIG_SCRIPT_IN}" "${FILE_VISP_CONFIG_SCRIPT}" @ONLY)
+
+ #---------------------------------------------------------------------
+ # Configure to create <build dir>/unix-install/visp-config shell script
+ # that will call visp.pc
+ #----------------------------------------------------------------------
+ configure_file("${VISP_SOURCE_DIR}/${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN}" "${FILE_VISP_CONFIG_SCRIPT_INSTALL}" @ONLY)
+
+ #---------------------------------------------------------------------
+ # Updates the <build dir>/install/visp.pc pkg-config file
+ # Updates VISP_CONFIG_CFLAGS_PC (for libvisp.pc used by pkg-config)
+ # Updates VISP_CONFIG_LIBS_PC (for libvisp.pc used by pkg-config)
+ #----------------------------------------------------------------------
+ set(exec_prefix "\${prefix}")
+ set(includedir "\${prefix}/${VISP_INC_INSTALL_PATH}")
+ set(libdir "\${prefix}/${VISP_LIB_INSTALL_PATH}")
+
+ # prepend with ViSP own include dir
+ set(VISP_CONFIG_CFLAGS_PC "-I\${includedir}")
+ list(APPEND VISP_CONFIG_CFLAGS_PC ${_cxx_flags})
+ foreach(_inc ${_include_flags_deps})
+ list(APPEND VISP_CONFIG_CFLAGS_PC "-I${_inc}")
+ endforeach()
+
+ vp_list_unique(VISP_CONFIG_CFLAGS_PC)
+ # Format the string to suppress CMake separators ";"
+ vp_list_remove_separator(VISP_CONFIG_CFLAGS_PC)
+
+ # prepend with ViSP own modules first
+ set(VISP_CONFIG_LIBS_PC "")
+ foreach(m ${VISP_MODULES_BUILD})
+ get_filename_component(m_libname "${TARGET_LOCATION_${m}}" NAME)
+ list(APPEND VISP_CONFIG_LIBS_PC "\${libdir}/${m_libname}")
+ endforeach()
+ # append deps
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND VISP_CONFIG_LIBS_PC ${VISP_MODULE_${m}_LINK_DEPS})
+ endforeach()
+ vp_list_filterout(VISP_CONFIG_LIBS_PC "optimized")
+ vp_list_filterout(VISP_CONFIG_LIBS_PC "debug")
+ vp_list_unique(VISP_CONFIG_LIBS_PC)
+ vp_list_remove_separator(VISP_CONFIG_LIBS_PC)
+
+ configure_file("${VISP_SOURCE_DIR}/${FILE_VISP_CONFIG_PC_INSTALL_IN}" "${FILE_VISP_CONFIG_PC_INSTALL}" @ONLY)
+
+ else()
+ #######################################################################
+ #
+ # for windows platforms
+ #
+ #######################################################################
+ set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.bat.in")
+ set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.bat.in")
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_DEF
+ #----------------------------------------------------------------------
+ vp_list_replace_separator(_cxx_flags ", ")
+ set(VISP_CONFIG_SCRIPT_DEFS ${_cxx_flags})
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_INC
+ # 1/ For usage with the build tree (where location of the <source tree>/<module>/include should be added)
+ # VISP_CONFIG_SCRIPT_INC = VISP_CONFIG_SCRIPT_INC_BUILD + _include_flags_deps
+ # 2/ For usage with the install tree
+ # VISP_CONFIG_SCRIPT_INC = VISP_CONFIG_SCRIPT_INC_BUILD + _include_flags_deps + _include_flags_src
+ #----------------------------------------------------------------------
+ set(VISP_CONFIG_SCRIPT_INC_BUILD "%PREFIX%/${VISP_INC_INSTALL_PATH}")
+ set(VISP_CONFIG_SCRIPT_INC_SRC_TREE ${_include_flags_src})
+
+ #---------------------------------------------------------------------
+ # Updates VISP_CONFIG_SCRIPT_LIBDIR
+ # 1/ For usage with the build tree
+ # 2/ For usage with the install tree
+ #
+ # and updates VISP_CONFIG_SCRIPT_LIBS_${config}
+ #----------------------------------------------------------------------
+
+ # prepend with ViSP own modules first
+ set(TMP_SCRIPT_LIBS_DBG "")
+ set(TMP_SCRIPT_LIBS_OPT "")
+ foreach(m ${VISP_MODULES_BUILD})
+ get_filename_component(m_libname "${TARGET_LOCATION_${m}}" NAME_WE)
+ list(APPEND TMP_SCRIPT_LIBS_DBG "${m_libname}${VISP_DEBUG_POSTFIX}.lib")
+ list(APPEND TMP_SCRIPT_LIBS_OPT "${m_libname}.lib")
+ endforeach()
+
+ # append deps
+ set(TMP_SCRIPT_LIBS_DEPS "")
+ foreach(m ${VISP_MODULES_BUILD})
+ list(APPEND TMP_SCRIPT_LIBS_DEPS ${VISP_MODULE_${m}_LINK_DEPS})
+ endforeach()
+ vp_list_unique(TMP_SCRIPT_LIBS_DEPS)
+
+ set(TMP_IS_DBG FALSE)
+ set(TMP_IS_OPT FALSE)
+ foreach(lib ${TMP_SCRIPT_LIBS_DEPS})
+ if("${lib}" MATCHES "[d][e][b][u][g]")
+ set(TMP_IS_DBG TRUE)
+ elseif("${lib}" MATCHES "[o][p][t][i][m][i][z][e][d]")
+ set(TMP_IS_OPT TRUE)
+ else()
+ # Get the library name
+ get_filename_component(libname ${lib} NAME)
+ if("${libname}" MATCHES ".+[.][l][i][b]" OR "${libname}" MATCHES ".+[.][L][i][b]")
+ #MESSAGE("${libname} matches .lib or .Lib")
+ else()
+ # We need to add .lib suffix
+ #MESSAGE("For ${libname} we add .lib suffix")
+ set(libname "${libname}.lib")
+ endif()
+
+ # Get the library path
+ get_filename_component(libpath ${lib} PATH)
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR_ "${libpath}")
+
+ if(TMP_IS_DBG)
+ set(TMP_IS_DBG FALSE)
+ list(APPEND TMP_SCRIPT_LIBS_DBG ${libname})
+ elseif(TMP_IS_OPT)
+ set(TMP_IS_OPT FALSE)
+ list(APPEND TMP_SCRIPT_LIBS_OPT ${libname})
+ else()
+ list(APPEND TMP_SCRIPT_LIBS_DBG ${libname})
+ list(APPEND TMP_SCRIPT_LIBS_OPT ${libname})
+ endif()
+ endif()
+ endforeach(lib)
+
+ vp_list_unique(TMP_SCRIPT_LIBS_DBG)
+ vp_list_unique(TMP_SCRIPT_LIBS_OPT)
+
+ # Format the string
+ set(VISP_CONFIG_SCRIPT_LIBS_DEBUG "${TMP_SCRIPT_LIBS_DBG}")
+ set(VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED "${TMP_SCRIPT_LIBS_OPT}")
+
+ # Format the string
+ string(REGEX REPLACE "lib/Release" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
+ string(REGEX REPLACE "lib/Debug" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
+
+ # 1/ For usage with the build tree
+ set(VISP_CONFIG_SCRIPT_INC "${VISP_CONFIG_SCRIPT_INC_BUILD}")
+ list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_SRC_TREE})
+ list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_DEPS})
+ vp_list_unique(VISP_CONFIG_SCRIPT_INC)
+ set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib/$(ConfigurationName)")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
+ vp_list_unique(VISP_CONFIG_SCRIPT_LIBDIR)
+ configure_file("${VISP_SOURCE_DIR}/${FILE_VISP_CONFIG_SCRIPT_IN}" "${FILE_VISP_CONFIG_SCRIPT}" @ONLY)
+
+ # 2/ For usage with the install tree
+ set(VISP_CONFIG_SCRIPT_INC "${VISP_CONFIG_SCRIPT_INC_BUILD}")
+ list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_DEPS})
+ vp_list_unique(VISP_CONFIG_SCRIPT_INC)
+ set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/${VISP_ARCH}/${VISP_RUNTIME}/lib")
+ list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
+ vp_list_unique(VISP_CONFIG_SCRIPT_LIBDIR)
+ configure_file("${VISP_SOURCE_DIR}/${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN}" "${FILE_VISP_CONFIG_SCRIPT_INSTALL}" @ONLY)
+ endif()
+
+endif() # DEFINED CMAKE_HELPER_SCRIPT
diff --git a/cmake/VISPGenerateHeaders.cmake b/cmake/VISPGenerateHeaders.cmake
index 12f2c85..e5c45dc 100644
--- a/cmake/VISPGenerateHeaders.cmake
+++ b/cmake/VISPGenerateHeaders.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -32,12 +33,66 @@
#
#############################################################################
-# platform-specific config file
+# ----------------------------------------------------------------------------
+# platform-specific config file vpConfig.h
+# ----------------------------------------------------------------------------
+
+# case 1: when ViSP is build with make; files are used in <binary dir>
+set(data_location_ ${VISP_BINARY_DIR})
+set(VISP_SCENES_DIR ${data_location_}/data/wireframe-simulator)
+set(VISP_ROBOT_ARMS_DIR ${data_location_}/data/robot-simulator)
+# Note that VISP_HAVE_OGRE_RESOURCES_PATH and VISP_HAVE_OGRE_PLUGINS_PATH are set in OgreTools.cmake
+if(UNIX)
+ set(data_location_ "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}")
+else()
+ set(data_location_ "${CMAKE_INSTALL_PREFIX}")
+endif()
+list(APPEND VISP_SCENES_DIR ${data_location_}/data/wireframe-simulator)
+list(APPEND VISP_ROBOT_ARMS_DIR ${data_location_}/data/robot-simulator)
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ list(APPEND VISP_HAVE_OGRE_RESOURCES_PATH "${data_location_}/data/ogre-simulator")
+endif()
+# append to VISP_HAVE_OGRE_PLUGINS_PATH the path of the installed plugins.cfg file
+# to be able to use ViSP from install tree
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ list(APPEND VISP_HAVE_OGRE_PLUGINS_PATH "${CMAKE_INSTALL_PREFIX}/${VISP_LIB_INSTALL_PATH}/visp/data/ogre-simulator")
+endif()
+
configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpConfig.h.in" "${VISP_INCLUDE_DIR}/visp3/core/vpConfig.h")
-install(FILES "${VISP_INCLUDE_DIR}/visp3/core/vpConfig.h"
- DESTINATION ${VISP_INC_INSTALL_PATH}/visp3/core
- COMPONENT dev
-)
+
+# case 2: when ViSP is build with make install; files are used in <install dir>
+if(UNIX)
+ set(data_location_ "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_DATAROOTDIR}/visp-${VISP_VERSION}")
+else()
+ set(data_location_ "${CMAKE_INSTALL_PREFIX}")
+endif()
+set(VISP_SCENES_DIR ${data_location_}/data/wireframe-simulator)
+set(VISP_ROBOT_ARMS_DIR ${data_location_}/data/robot-simulator)
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ set(VISP_HAVE_OGRE_RESOURCES_PATH "${data_location_}/data/ogre-simulator")
+endif()
+# append to VISP_HAVE_OGRE_PLUGINS_PATH the path of the installed plugins.cfg file
+# to be able to use ViSP from install tree
+if(VISP_INSTALL_DIR_OGRE_RESOURCES)
+ set(VISP_HAVE_OGRE_PLUGINS_PATH "${CMAKE_INSTALL_PREFIX}/${VISP_LIB_INSTALL_PATH}/visp/data/ogre-simulator")
+endif()
+
+if(UNIX)
+ configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpConfig.h.in" "${VISP_BINARY_DIR}/unix-install/vpConfig.h")
+
+ install(FILES "${VISP_BINARY_DIR}/unix-install/vpConfig.h"
+ DESTINATION ${VISP_INC_INSTALL_PATH}/visp3/core
+ COMPONENT dev
+ )
+
+else()
+ configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpConfig.h.in" "${VISP_BINARY_DIR}/win-install/vpConfig.h")
+
+ install(FILES "${VISP_BINARY_DIR}/win-install/vpConfig.h"
+ DESTINATION ${VISP_INC_INSTALL_PATH}/visp3/core
+ COMPONENT dev
+ )
+endif()
# ----------------------------------------------------------------------------
# visp_modules.h that contains all the build modules defines
diff --git a/cmake/VISPGeneratePkgConfigScript.cmake b/cmake/VISPGeneratePkgConfigScript.cmake
deleted file mode 100644
index efd50e4..0000000
--- a/cmake/VISPGeneratePkgConfigScript.cmake
+++ /dev/null
@@ -1,383 +0,0 @@
-#############################################################################
-#
-# This file is part of the ViSP software.
-# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
-#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
-# See the file LICENSE.txt at the root directory of this source
-# distribution for additional information about the GNU GPL.
-#
-# For using ViSP with software that can not be combined with the GNU
-# GPL, please contact Inria about acquiring a ViSP Professional
-# Edition License.
-#
-# See http://visp.inria.fr for more information.
-#
-# This software was developed at:
-# Inria Rennes - Bretagne Atlantique
-# Campus Universitaire de Beaulieu
-# 35042 Rennes Cedex
-# France
-#
-# If you have questions regarding the use of this file, please contact
-# Inria at visp at inria.fr
-#
-# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
-# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
-#
-# Description:
-# This file generates the ViSP library config shell scripts:
-# - visp-config in <build dir>/bin from visp-config.in
-# - visp-config in <build dir>/install from visp-config.install.in
-# When make install, this file is copied in <install dir>/bin
-# - visp.pc in <build dir>/install from visp.pc.in
-# When make install, this file is copied in <install dir>/lib/pkgconfig
-#
-# Authors:
-# Fabien Spindler
-#
-#############################################################################
-
-if(MINGW)
- # not implemented yet
- return()
-endif()
-
-if(UNIX)
- #######################################################################
- #
- # for Unix platforms: Linux, OSX
- #
- #######################################################################
- set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.in")
- set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config")
-
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.install.in")
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/unix-install/visp-config")
-
- set(FILE_VISP_CONFIG_PC_INSTALL_IN "cmake/templates/visp.pc.in")
- set(FILE_VISP_CONFIG_PC_INSTALL "${VISP_BINARY_DIR}/unix-install/visp.pc")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_PREFIX
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
- #---------------------------------------------------------------------
- # Updates vars needed to update VISP_CONFIG_CFLAGS_SCRIPT
- # 1/ For usage with the build tree (where location of the <source tree>/<module>/include should be added)
- # VISP_CONFIG_CFLAGS_SCRIPT = VISP_CONFIG_CFLAGS + VISP_CONFIG_CFLAGS_DEPS + VISP_CONFIG_CFLAGS_SRC_TREE
- # 2/ For usage with the install tree
- # VISP_CONFIG_CFLAGS_SCRIPT = VISP_CONFIG_CFLAGS + VISP_CONFIG_CFLAGS_DEPS
- #----------------------------------------------------------------------
- if(BUILD_TEST_COVERAGE)
- # Add build options for test coverage. Currently coverage is only supported
- # on gcc compiler
- # Because using -fprofile-arcs with shared lib can cause problems like:
- # hidden symbol `__bb_init_func', we add this option only for static
- # library build
- list(APPEND VISP_CONFIG_CFLAGS "-ftest-coverage")
- list(APPEND VISP_CONFIG_CFLAGS "-fprofile-arcs")
- endif()
-
- # Propagate cxx11 compiler option if enabled during ViSP build
- if(VISP_HAVE_CPP11_COMPATIBILITY)
- list(APPEND VISP_CONFIG_CFLAGS ${CPP11_CXX_FLAGS})
- endif()
- # Propagate openmp compiler option if enabled during ViSP build
- if(VISP_HAVE_OPENMP)
- list(APPEND VISP_CONFIG_CFLAGS ${OpenMP_CXX_FLAGS})
- endif()
-
- # include ViSP own include dir
- set(VISP_CONFIG_CFLAGS_SRC_TREE "-I$PREFIX/${VISP_INC_INSTALL_PATH}")
-
- foreach(m ${VISP_MODULES_BUILD})
- if(EXISTS "${VISP_MODULE_${m}_LOCATION}/include")
- list(APPEND VISP_CONFIG_CFLAGS_SRC_TREE "-I${VISP_MODULE_${m}_LOCATION}/include")
- endif()
- endforeach()
-
- foreach(m ${VISP_MODULES_BUILD})
- foreach(inc ${VISP_MODULE_${m}_INC_DEPS})
- list(APPEND VISP_CONFIG_CFLAGS_DEPS "-I${inc}")
- endforeach()
- endforeach()
-
- #---------------------------------------------------------------------
- # Updates vars needed to update VISP_CONFIG_LIBS_SCRIPT
- #----------------------------------------------------------------------
- # prepend with ViSP own modules first
- set(VISP_CONFIG_LIBS_SCRIPT "")
- foreach(m ${VISP_MODULES_BUILD})
- # need to be improved
- if(POLICY CMP0026)
- cmake_policy(PUSH)
- cmake_policy(SET CMP0026 OLD)
- get_target_property(m_libpath ${m} LOCATION_Release)
- cmake_policy(POP)
- else()
- get_target_property(m_libpath ${m} LOCATION_Release)
- endif()
- get_filename_component(m_libname "${m_libpath}" NAME)
- list(APPEND VISP_CONFIG_LIBS_SCRIPT "$PREFIX/${VISP_LIB_INSTALL_PATH}/${m_libname}")
- endforeach()
- # append deps
- foreach(m ${VISP_MODULES_BUILD})
- list(APPEND VISP_CONFIG_LIBS_SCRIPT ${VISP_MODULE_${m}_LINK_DEPS})
- endforeach()
- vp_list_unique(VISP_CONFIG_LIBS_SCRIPT)
- vp_list_filterout(VISP_CONFIG_LIBS_SCRIPT "debug")
- vp_list_filterout(VISP_CONFIG_LIBS_SCRIPT "optimized")
-# message("VISP_CONFIG_LIBS_SCRIPT: ${VISP_CONFIG_LIBS_SCRIPT}")
- # Format the string to suppress CMake separators ";"
- vp_list_remove_separator(VISP_CONFIG_LIBS_SCRIPT)
-
- set(VISP_ECHO_NO_NEWLINE_CHARACTER "")
- set(VISP_ECHO_NO_NEWLINE_OPTION "")
- if(APPLE)
- set(VISP_ECHO_NO_NEWLINE_CHARACTER "\\c")
- else()
- set(VISP_ECHO_NO_NEWLINE_OPTION "-n")
- endif()
-
- #---------------------------------------------------------------------
- # Configure to create <build dir>/bin/visp-config shell script
- # Updates VISP_CONFIG_CFLAGS_SCRIPT before
- #----------------------------------------------------------------------
- set(VISP_CONFIG_CFLAGS_SCRIPT ${VISP_CONFIG_CFLAGS})
- list(APPEND VISP_CONFIG_CFLAGS_SCRIPT ${VISP_CONFIG_CFLAGS_SRC_TREE})
- list(APPEND VISP_CONFIG_CFLAGS_SCRIPT ${VISP_CONFIG_CFLAGS_DEPS})
- vp_list_unique(VISP_CONFIG_CFLAGS_SCRIPT)
- # Format the string to suppress CMake separators ";"
- vp_list_remove_separator(VISP_CONFIG_CFLAGS_SCRIPT)
- configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
-
- #---------------------------------------------------------------------
- # Configure to create <build dir>/unix-install/visp-config shell script
- # that will call visp.pc
- #----------------------------------------------------------------------
- configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
-
- #---------------------------------------------------------------------
- # Updates the <build dir>/install/visp.pc pkg-config file
- # Updates VISP_CONFIG_CFLAGS_PC (for libvisp.pc used by pkg-config)
- # Updates VISP_CONFIG_LIBS_PC (for libvisp.pc used by pkg-config)
- #----------------------------------------------------------------------
- set(exec_prefix "\${prefix}")
- set(includedir "\${prefix}/${VISP_INC_INSTALL_PATH}")
- set(libdir "\${prefix}/${VISP_LIB_INSTALL_PATH}")
-
- # prepend with ViSP own include dir
- set(VISP_CONFIG_CFLAGS_PC "-I\${includedir}")
- list(APPEND VISP_CONFIG_CFLAGS_PC ${VISP_CONFIG_CFLAGS})
- list(APPEND VISP_CONFIG_CFLAGS_PC ${VISP_CONFIG_CFLAGS_DEPS})
- vp_list_unique(VISP_CONFIG_CFLAGS_PC)
- # Format the string to suppress CMake separators ";"
- vp_list_remove_separator(VISP_CONFIG_CFLAGS_PC)
-
- # prepend with ViSP own modules first
- set(VISP_CONFIG_LIBS_PC "")
- foreach(m ${VISP_MODULES_BUILD})
- # need to be improved
- if(POLICY CMP0026)
- cmake_policy(PUSH)
- cmake_policy(SET CMP0026 OLD)
- get_target_property(m_libpath ${m} LOCATION_Release)
- cmake_policy(POP)
- else()
- get_target_property(m_libpath ${m} LOCATION_Release)
- endif()
- get_filename_component(m_libname "${m_libpath}" NAME)
- list(APPEND VISP_CONFIG_LIBS_PC "\${libdir}/${m_libname}")
- endforeach()
- # append deps
- foreach(m ${VISP_MODULES_BUILD})
- list(APPEND VISP_CONFIG_LIBS_PC ${VISP_MODULE_${m}_LINK_DEPS})
- endforeach()
- vp_list_filterout(VISP_CONFIG_LIBS_PC "optimized")
- vp_list_filterout(VISP_CONFIG_LIBS_PC "debug")
- vp_list_remove_separator(VISP_CONFIG_LIBS_PC)
-
-# message("VISP_CONFIG_LIBS_PC: ${VISP_CONFIG_LIBS_PC}")
-
- configure_file(${FILE_VISP_CONFIG_PC_INSTALL_IN} ${FILE_VISP_CONFIG_PC_INSTALL})
-
-else()
- #######################################################################
- #
- # for windows platforms
- #
- #######################################################################
- set(FILE_VISP_CONFIG_SCRIPT_IN "cmake/templates/visp-config.bat.in")
- set(FILE_VISP_CONFIG_SCRIPT "${BINARY_OUTPUT_PATH}/visp-config.bat")
-
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL_IN "cmake/templates/visp-config.bat.in")
- set(FILE_VISP_CONFIG_SCRIPT_INSTALL "${VISP_BINARY_DIR}/win-install/visp-config-${VISP_ARCH}-${VISP_RUNTIME}.bat")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_PREFIX
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_PREFIX "${CMAKE_INSTALL_PREFIX}")
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_DEF
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_DEFS "")
- set(VISP_OPENMP_SUPPORT "no")
- if(VISP_HAVE_OPENMP)
- set(VISP_CONFIG_SCRIPT_DEFS "${OpenMP_CXX_FLAGS}")
- set(VISP_OPENMP_SUPPORT "yes")
- endif()
- if(VISP_HAVE_CPP11_COMPATIBILITY)
- set(VISP_CONFIG_SCRIPT_DEFS "${CPP11_CXX_FLAGS}, ${VISP_CONFIG_SCRIPT_DEFS}")
- endif()
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_INC
- # 1/ For usage with the build tree (where location of the <source tree>/<module>/include should be added)
- # VISP_CONFIG_SCRIPT_INC = VISP_CONFIG_SCRIPT_INC_BUILD + VISP_CONFIG_SCRIPT_INC_DEPS
- # 2/ For usage with the install tree
- # VISP_CONFIG_SCRIPT_INC = VISP_CONFIG_SCRIPT_INC_BUILD + VISP_CONFIG_SCRIPT_INC_SRC_TREE + VISP_CONFIG_SCRIPT_INC_DEPS
- #----------------------------------------------------------------------
- set(VISP_CONFIG_SCRIPT_INC_BUILD "%PREFIX%/${VISP_INC_INSTALL_PATH}")
- set(VISP_CONFIG_SCRIPT_INC_SRC_TREE "")
- foreach(m ${VISP_MODULES_BUILD})
- if(EXISTS "${VISP_MODULE_${m}_LOCATION}/include")
- list(APPEND VISP_CONFIG_SCRIPT_INC_SRC_TREE "${VISP_MODULE_${m}_LOCATION}/include")
- endif()
- endforeach()
- set(VISP_CONFIG_SCRIPT_INC_DEPS "")
- foreach(m ${VISP_MODULES_BUILD})
- foreach(inc ${VISP_MODULE_${m}_INC_DEPS})
- list(APPEND VISP_CONFIG_SCRIPT_INC_DEPS "${inc}")
- endforeach()
- endforeach()
-
- #---------------------------------------------------------------------
- # Updates VISP_CONFIG_SCRIPT_LIBDIR
- # 1/ For usage with the build tree
- # 2/ For usage with the install tree
- #
- # and updates VISP_CONFIG_SCRIPT_LIBS_${config}
- #----------------------------------------------------------------------
-
- # prepend with ViSP own modules first
- set(TMP_SCRIPT_LIBS_DBG "")
- set(TMP_SCRIPT_LIBS_OPT "")
- foreach(m ${VISP_MODULES_BUILD})
- # need to be improved
- if(POLICY CMP0026)
- cmake_policy(PUSH)
- cmake_policy(SET CMP0026 OLD)
- get_target_property(m_libpath ${m} LOCATION_Release)
- cmake_policy(POP)
- else()
- get_target_property(m_libpath ${m} LOCATION_Release)
- endif()
- get_filename_component(m_libname "${m_libpath}" NAME_WE)
- list(APPEND TMP_SCRIPT_LIBS_DBG "${m_libname}${VISP_DEBUG_POSTFIX}.lib")
- list(APPEND TMP_SCRIPT_LIBS_OPT "${m_libname}.lib")
- endforeach()
-
- # append deps
- set(TMP_SCRIPT_LIBS_DEPS "")
- foreach(m ${VISP_MODULES_BUILD})
- list(APPEND TMP_SCRIPT_LIBS_DEPS ${VISP_MODULE_${m}_LINK_DEPS})
- endforeach()
- vp_list_unique(TMP_SCRIPT_LIBS_DEPS)
-
- set(TMP_IS_DBG FALSE)
- set(TMP_IS_OPT FALSE)
- foreach(lib ${TMP_SCRIPT_LIBS_DEPS})
- if("${lib}" MATCHES "[d][e][b][u][g]")
- set(TMP_IS_DBG TRUE)
- elseif("${lib}" MATCHES "[o][p][t][i][m][i][z][e][d]")
- set(TMP_IS_OPT TRUE)
- else()
- # Get the library name
- get_filename_component(libname ${lib} NAME)
- if("${libname}" MATCHES ".+[.][l][i][b]" OR "${libname}" MATCHES ".+[.][L][i][b]")
- #MESSAGE("${libname} matches .lib or .Lib")
- else()
- # We need to add .lib suffix
- #MESSAGE("For ${libname} we add .lib suffix")
- set(libname "${libname}.lib")
- endif()
-
- # Get the library path
- get_filename_component(libpath ${lib} PATH)
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR_ "${libpath}")
-
- if(TMP_IS_DBG)
- set(TMP_IS_DBG FALSE)
- list(APPEND TMP_SCRIPT_LIBS_DBG ${libname})
- elseif(TMP_IS_OPT)
- set(TMP_IS_OPT FALSE)
- list(APPEND TMP_SCRIPT_LIBS_OPT ${libname})
- else()
- list(APPEND TMP_SCRIPT_LIBS_DBG ${libname})
- list(APPEND TMP_SCRIPT_LIBS_OPT ${libname})
- endif()
- endif()
- endforeach(lib)
-
- vp_list_unique(TMP_SCRIPT_LIBS_DBG)
- vp_list_unique(TMP_SCRIPT_LIBS_OPT)
-
- # Format the string
- set(VISP_CONFIG_SCRIPT_LIBS_DEBUG "${TMP_SCRIPT_LIBS_DBG}")
- set(VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED "${TMP_SCRIPT_LIBS_OPT}")
-
- # Format the string
- string(REGEX REPLACE "lib/Release" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
- string(REGEX REPLACE "lib/Debug" "lib/$(ConfigurationName)" VISP_CONFIG_SCRIPT_LIBDIR_ "${VISP_CONFIG_SCRIPT_LIBDIR_}")
-
- # 1/ For usage with the build tree
- set(VISP_CONFIG_SCRIPT_INC "${VISP_CONFIG_SCRIPT_INC_BUILD}")
- list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_SRC_TREE})
- list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_DEPS})
- vp_list_unique(VISP_CONFIG_SCRIPT_INC)
- set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/lib/$(ConfigurationName)")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
- list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
- configure_file(${FILE_VISP_CONFIG_SCRIPT_IN} ${FILE_VISP_CONFIG_SCRIPT})
-
- # 2/ For usage with the install tree
- set(VISP_CONFIG_SCRIPT_INC "${VISP_CONFIG_SCRIPT_INC_BUILD}")
- list(APPEND VISP_CONFIG_SCRIPT_INC ${VISP_CONFIG_SCRIPT_INC_DEPS})
- vp_list_unique(VISP_CONFIG_SCRIPT_INC)
- set(VISP_CONFIG_SCRIPT_LIBDIR "%PREFIX%/${VISP_ARCH}/${VISP_RUNTIME}/lib")
- list(APPEND VISP_CONFIG_SCRIPT_LIBDIR ${VISP_CONFIG_SCRIPT_LIBDIR_})
- list(REMOVE_DUPLICATES VISP_CONFIG_SCRIPT_LIBDIR)
- configure_file(${FILE_VISP_CONFIG_SCRIPT_INSTALL_IN} ${FILE_VISP_CONFIG_SCRIPT_INSTALL})
-endif()
-
-#----------------------------------------------------------------------
-# customize install target
-#----------------------------------------------------------------------
-# install rule for visp-config shell script
-install(FILES ${FILE_VISP_CONFIG_SCRIPT_INSTALL}
- DESTINATION ${VISP_BIN_INSTALL_PATH}
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_EXECUTE GROUP_EXECUTE WORLD_EXECUTE
- OWNER_WRITE
- COMPONENT dev
-)
-
-# install rule for visp.pc pkg-config file
-if(UNIX)
- install(FILES ${FILE_VISP_CONFIG_PC_INSTALL}
- DESTINATION ${VISP_LIB_INSTALL_PATH}/pkgconfig
- PERMISSIONS OWNER_READ GROUP_READ WORLD_READ
- OWNER_WRITE
- COMPONENT dev
- )
-else()
- # not implemented yet
-endif()
-
-
diff --git a/cmake/VISPModule.cmake b/cmake/VISPModule.cmake
index 62c224f..126eb3c 100644
--- a/cmake/VISPModule.cmake
+++ b/cmake/VISPModule.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -146,7 +147,7 @@ macro(vp_add_module _name)
string(TOLOWER "${_name}" name)
set(the_module visp_${name})
#message("Found module: ${the_module}")
-
+
# the first pass - collect modules info, the second pass - create targets
if(VISP_INITIAL_PASS)
#guard agains redefinition
@@ -167,7 +168,7 @@ macro(vp_add_module _name)
# create option to enable/disable this module
option(BUILD_MODULE_${the_module} "Include ${the_module} module into ViSP build" ${BUILD_${the_module}_INIT})
-
+
# remember the module details
set(VISP_MODULE_${the_module}_DESCRIPTION "${the_description}" CACHE INTERNAL "Brief description of ${the_module} module")
set(VISP_MODULE_${the_module}_LOCATION "${CMAKE_CURRENT_SOURCE_DIR}" CACHE INTERNAL "Location of ${the_module} module sources")
@@ -249,7 +250,12 @@ macro(vp_glob_modules)
vp_list_remove_item(__vpmodules "tutorial")
vp_list_remove_item(__vpmodules "example")
vp_list_remove_item(__vpmodules "demo")
+ vp_list_remove_item(__vpmodules "apps")
vp_list_remove_item(__vpmodules "doc")
+ vp_list_remove_item(__vpmodules ".gitignore")
+ vp_list_remove_item(__vpmodules ".travis.yml")
+ vp_list_remove_item(__vpmodules "LICENSE")
+ vp_list_remove_item(__vpmodules "README.md")
endif()
# TODO: Improve the following if to put a macro instead of manually copying the code
# Here we have 3 internal loops. The depth of the loop (3) could be a var
@@ -258,18 +264,27 @@ macro(vp_glob_modules)
list(SORT __vpmodules)
foreach(mod ${__vpmodules})
get_filename_component(__modpath "${__path}/${mod}" ABSOLUTE)
-
+ set(__propagate FALSE) # Indicate if we should check for subdirs that may contain a CMakeLists.txt file
if(EXISTS "${__modpath}/CMakeLists.txt")
- list(FIND __directories_observed "${__modpath}" __pathIdx)
- if(__pathIdx GREATER -1)
- message(FATAL_ERROR "The module from ${__modpath} is already loaded.")
- endif()
- list(APPEND __directories_observed "${__modpath}")
- add_subdirectory("${__modpath}" "${CMAKE_CURRENT_BINARY_DIR}/${mod}/.${mod}")
- if (DEFINED VISP_MODULE_visp_${mod}_LOCATION)
- math(EXPR __count "${__count} + 1")
+ if(${mod} STREQUAL "modules")
+ # Process specific case where there is a <contrib>/modules/CMakeLists.txt file common to all <contrib> modules
+ include(${__modpath}/CMakeLists.txt)
+ set(__propagate TRUE)
+ else()
+ list(FIND __directories_observed "${__modpath}" __pathIdx)
+ if(__pathIdx GREATER -1)
+ message(FATAL_ERROR "The module from ${__modpath} is already loaded.")
+ endif()
+ list(APPEND __directories_observed "${__modpath}")
+ add_subdirectory("${__modpath}" "${CMAKE_CURRENT_BINARY_DIR}/${mod}/.${mod}")
+ if (DEFINED VISP_MODULE_visp_${mod}_LOCATION)
+ math(EXPR __count "${__count} + 1")
+ endif()
endif()
else()
+ set(__propagate TRUE)
+ endif()
+ if(__propagate)
# modules in visp/tracker
get_filename_component(__subpath "${__path}/${mod}" ABSOLUTE)
file(GLOB __vpsubmodules RELATIVE "${__subpath}" "${__subpath}/*")
@@ -669,7 +684,7 @@ macro(vp_create_compat_headers)
foreach(h ${ARGN})
get_filename_component(__h_name_we ${h} NAME_WE)
get_filename_component(__h_name ${h} NAME)
- set(VISP_HEADER_CONTENT_CONFIGMAKE "#ifndef __${__h_name_we}_h_\n#define __${__h_name_we}_h_\n\n#include <visp3/${name}/${__h_name}>\n\n#endif\n")
+ set(VISP_HEADER_CONTENT_CONFIGMAKE "#ifndef __${__h_name_we}_gen_h_\n#define __${__h_name_we}_gen_h_\n\n#include <visp3/${name}/${__h_name}>\n\n#endif\n")
set(__compat_header_dst "${VISP_INCLUDE_DIR}/visp/${__h_name_we}.h")
configure_file("${VISP_SOURCE_DIR}/cmake/templates/vpHeader.h.in" ${__compat_header_dst})
endforeach()
@@ -984,9 +999,12 @@ endmacro()
# If the input config filename is suffixed by .in or .cmake the suffix is removed
# in the configured file.
#
+# Warning: Should be called after add_module()
+#
# Example:
-# vp_add_config_file(cmake/template/vpConfigModule.h.in)
-# creates include/visp3/module_name/vpConfigModule.h
+# add_module(my_module visp_core)
+# vp_add_config_file(cmake/template/vpConfigMyModule.h.in)
+# creates include/visp3/my_module/vpConfigMyModule.h
macro(vp_add_config_file)
foreach(d ${ARGN})
# Removes first "/" if it exists
@@ -1025,7 +1043,6 @@ macro(vp_add_config_file)
if(MODULE_NAME MATCHES "^visp_")
string(REGEX REPLACE "^visp_" "" MODULE_NAME "${MODULE_NAME}")
endif()
-
configure_file("${VISP_MODULE_${the_module}_LOCATION}/${FILENAME_CONFIG}" "${VISP_INCLUDE_DIR}/visp3/${MODULE_NAME}/${FILENAME_CONFIG_SHORT}")
vp_create_compat_headers("${VISP_INCLUDE_DIR}/visp3/${MODULE_NAME}/${FILENAME_CONFIG_SHORT}")
@@ -1056,3 +1073,11 @@ macro(vp_add_cmake_module_path)
endif()
endforeach()
endmacro()
+
+macro(vp_cmake_configure file_name)
+ set(__shortname ${the_module})
+ vp_short_module_name(__shortname)
+ if(EXISTS ${VISP_MODULE_${the_module}_LOCATION}/${file_name})
+ configure_file(${VISP_MODULE_${the_module}_LOCATION}/${file_name} "${CMAKE_BINARY_DIR}/CMakeConfig-${__shortname}.cmake" @ONLY)
+ endif()
+endmacro()
diff --git a/cmake/VISPUse.cmake.in b/cmake/VISPUse.cmake.in
index e4e9007..47f0632 100644
--- a/cmake/VISPUse.cmake.in
+++ b/cmake/VISPUse.cmake.in
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/VISPUtils.cmake b/cmake/VISPUtils.cmake
index defeb11..6db30b4 100644
--- a/cmake/VISPUtils.cmake
+++ b/cmake/VISPUtils.cmake
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -58,7 +59,10 @@ function(vp_include_directories)
set(__add_before "")
foreach(dir ${ARGN})
get_filename_component(__abs_dir "${dir}" ABSOLUTE)
- if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}")
+# if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}") # not compatible with cmake 2.8.12.2
+ if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}")
+ list(APPEND __add_before "${dir}")
+ elseif("${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}")
list(APPEND __add_before "${dir}")
else()
include_directories(AFTER SYSTEM "${dir}")
@@ -73,8 +77,10 @@ function(vp_target_include_directories target)
foreach(dir ${ARGN})
get_filename_component(__abs_dir "${dir}" ABSOLUTE)
string(REPLACE "+" "\\+" __VISP_BINARY_DIR_filtered ${VISP_BINARY_DIR})
-# if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${VISP_BINARY_DIR}")
- if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${__VISP_BINARY_DIR_filtered}")
+# if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}" OR "${__abs_dir}" MATCHES "^${__VISP_BINARY_DIR_filtered}") # not compatible with cmake 2.8.12.2
+ if("${__abs_dir}" MATCHES "^${VISP_SOURCE_DIR}")
+ list(APPEND __params "${__abs_dir}")
+ elseif("${__abs_dir}" MATCHES "^${__VISP_BINARY_DIR_filtered}")
list(APPEND __params "${__abs_dir}")
else()
list(APPEND __params "${dir}")
@@ -176,6 +182,24 @@ macro(vp_list_remove_separator __lst)
endif()
endmacro()
+# remove cmake ; list separator
+macro(vp_list_replace_separator __lst __separator)
+ if(${__lst})
+ list(GET ${__lst} 0 __lst_reformated)
+ list(LENGTH ${__lst} __lenght)
+ if(__lenght GREATER 1)
+ MATH(EXPR __lenght "${__lenght} - 1")
+ message("__lenght: ${__lenght}")
+ foreach(_i RANGE 1 ${__lenght}-1)
+ list(GET ${__lst} ${_i} element)
+ message("element ${_i}: ${element}")
+ set(__lst_reformated "${__lst_reformated}${__separator}${element}")
+ endforeach()
+ endif()
+ set(${__lst} ${__lst_reformated})
+ endif()
+endmacro()
+
# Provides an option that the user can optionally select.
# Can accept condition to control when option is available for user.
# Usage:
@@ -560,6 +584,9 @@ macro(vp_check_compiler_flag LANG FLAG RESULT)
#else()
# file(WRITE "${_fname}" "#pragma\nint main() { return 0; }\n")
#endif()
+ elseif("_${LANG}_" MATCHES "_Fortran_")
+ set(_fname "${CMAKE_BINARY_DIR}${CMAKE_FILES_DIRECTORY}/CMakeTmp/src.f")
+ file(WRITE "${_fname}" " PROGRAM TEST\n RETURN\n END\n")
else()
unset(_fname)
endif()
@@ -604,6 +631,8 @@ macro(vp_check_flag_support lang flag varname)
set(_lang CXX)
elseif("_${lang}_" MATCHES "_C_")
set(_lang C)
+ elseif("_${lang}_" MATCHES "_Fortran_")
+ set(_lang Fortran)
else()
set(_lang ${lang})
endif()
@@ -611,7 +640,6 @@ macro(vp_check_flag_support lang flag varname)
string(TOUPPER "${flag}" ${varname})
string(REGEX REPLACE "^(/|-)" "HAVE_${_lang}_" ${varname} "${${varname}}")
string(REGEX REPLACE " -|-|=| |\\." "_" ${varname} "${${varname}}")
-
vp_check_compiler_flag("${_lang}" "${ARGN} ${flag}" ${${varname}})
endmacro()
@@ -657,8 +685,17 @@ endmacro()
macro(vp_set_source_file_compile_flag file)
if(ACTIVATE_WARNING_3PARTY_MUTE)
set(__cxxflags "")
+ set(__lang "CXX")
+ get_filename_component(__ext ${file} EXT)
+ if(__ext MATCHES "$.c")
+ set(__lang "C")
+ elseif(__ext MATCHES "$.f")
+ set(__lang "Fortran")
+ elseif(__ext MATCHES "$.F")
+ set(__lang "Fortran")
+ endif()
foreach(cxxflag ${ARGN})
- vp_check_flag_support(CXX ${cxxflag} __support_flag)
+ vp_check_flag_support(${__lang} ${cxxflag} __support_flag)
if(${__support_flag})
set(__cxxflags "${__cxxflags} ${cxxflag}")
endif()
@@ -666,8 +703,8 @@ macro(vp_set_source_file_compile_flag file)
if(NOT ${__cxxflags} STREQUAL "")
if(EXISTS ${CMAKE_CURRENT_LIST_DIR}/${file})
set_source_files_properties(${CMAKE_CURRENT_LIST_DIR}/${file} PROPERTIES COMPILE_FLAGS ${__cxxflags})
- elseif(EXISTS ${file}) # for files that are in the build tree (like those produced by qt moc
- set_source_files_properties(${CMAKE_CURRENT_LIST_DIR}/${file} PROPERTIES COMPILE_FLAGS ${__cxxflags})
+ elseif(EXISTS ${file}) # for files that are in the build tree (like those produced by qt moc)
+ set_source_files_properties(${file} PROPERTIES COMPILE_FLAGS ${__cxxflags})
endif()
endif()
endif()
@@ -688,6 +725,22 @@ macro(vp_add_subdirectories lst subdir)
endif()
endmacro()
+macro(vp_check_subdirectories lst subdir result)
+ set(${result} FALSE)
+ if(${lst})
+ foreach(__path ${${lst}})
+ if(EXISTS ${__path}/${subdir})
+ file(GLOB __subdirs RELATIVE "${__path}/${subdir}" "${__path}/${subdir}/*")
+ foreach(__s ${__subdirs})
+ if(EXISTS "${__path}/${subdir}/${__s}/CMakeLists.txt")
+ set(${result} TRUE)
+ endif()
+ endforeach()
+ endif()
+ endforeach()
+ endif()
+endmacro()
+
set(VISP_BUILD_INFO_FILE "${CMAKE_BINARY_DIR}/visp-build-info.tmp")
set(VISP_TXT_INFO_FILE "${CMAKE_BINARY_DIR}/ViSP-third-party.txt")
file(REMOVE "${VISP_BUILD_INFO_FILE}")
@@ -845,3 +898,11 @@ macro(vp_get_version_from_pkg LIBNAME PKG_PATH OUTPUT_VAR)
string(REGEX REPLACE ".*Version: ([^ ]+).*" "\\1" ${OUTPUT_VAR} "${line_to_parse}" )
endif()
endmacro()
+
+macro(vp_cmake_script_append_var content_var)
+ foreach(var_name ${ARGN})
+ set(${content_var} "${${content_var}}
+set(${var_name} \"${${var_name}}\")
+")
+ endforeach()
+endmacro()
diff --git a/cmake/templates/VISPConfig.cmake.in b/cmake/templates/VISPConfig.cmake.in
index 4cce425..fcfabfb 100644
--- a/cmake/templates/VISPConfig.cmake.in
+++ b/cmake/templates/VISPConfig.cmake.in
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -38,7 +39,7 @@
# - VISP_LIBRARIES : ViSP library to link against. Third-party libraries are
# linked automatically thanks to cmake export file VISPModules.cmake
# - VISP_VERSION_STRING : Full ViSP version that is build, "@VISP_VERSION@"
-# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION: "@VISP_VERSION_MAJOR@"
+# - VISP_VERSION_MAJOR : Major version part of VISP_VERSION: "@VISP_VERSION_MAJOR@"
# - VISP_VERSION_MINOR : Minor version part of VISP_VERSION: "@VISP_VERSION_MINOR@"
# - VISP_VERSION_PATCH : Patch version part of VISP_VERSION: "@VISP_VERSION_PATCH@"
#
@@ -63,7 +64,7 @@
#
# Or you can search for specific OpenCV modules:
#
-# find_package(VISP REQUIRED core videoio)
+# find_package(VISP REQUIRED core io)
#
# If the module is found then VISP_<MODULE>_FOUND is set to TRUE.
#
@@ -158,7 +159,7 @@ if(POLICY CMP0024)
# Fix to prevent multiple includes
if(NOT TARGET visp_core)
cmake_policy(PUSH)
- cmake_policy(SET CMP0024 OLD)
+ cmake_policy(SET CMP0024 NEW)
# Our library dependencies (contains definitions for IMPORTED targets)
include("${CMAKE_CURRENT_LIST_DIR}/VISPModules.cmake")
cmake_policy(POP)
@@ -176,9 +177,9 @@ set(VISP_USE_FILE "${CMAKE_CURRENT_LIST_DIR}/VISPUse.cmake")
if(BUILD_TEST_COVERAGE)
# Add build options for test coverage. Currently coverage is only supported
- # on gcc compiler
+ # on gcc compiler
# Because using -fprofile-arcs with shared lib can cause problems like:
- # hidden symbol `__bb_init_func', we add this option only for static
+ # hidden symbol `__bb_init_func', we add this option only for static
# library build
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -ftest-coverage -fprofile-arcs")
endif()
@@ -186,54 +187,66 @@ endif()
#----------------------------------------------------------------------
# Remember VISP' third party libs configuration:
#----------------------------------------------------------------------
-set(VISP_HAVE_X11 "@VISP_HAVE_X11@")
-set(VISP_HAVE_PTHREAD "@VISP_HAVE_PTHREAD@")
-set(VISP_HAVE_GTK "@VISP_HAVE_GTK@")
-set(VISP_HAVE_GDI "@VISP_HAVE_GDI@")
+set(VISP_HAVE_AFMA4 "@VISP_HAVE_AFMA4@")
+set(VISP_HAVE_AFMA6 "@VISP_HAVE_AFMA6@")
+set(VISP_HAVE_APRILTAG "@VISP_HAVE_APRILTAG@")
+set(VISP_HAVE_ATIDAQ "@VISP_HAVE_ATIDAQ@")
+set(VISP_HAVE_BICLOPS "@VISP_HAVE_BICLOPS@")
+set(VISP_HAVE_CLIPPER "@VISP_HAVE_CLIPPER@")
+set(VISP_HAVE_CMU1394 "@VISP_HAVE_CMU1394@")
+set(VISP_HAVE_COIN3D "@VISP_HAVE_COIN3D@")
+set(VISP_HAVE_COIN3D_AND_GUI "@VISP_HAVE_COIN3D_AND_GUI@")
+set(VISP_HAVE_COMEDI "@VISP_HAVE_COMEDI@")
+set(VISP_HAVE_CPP11_COMPATIBILITY "@VISP_HAVE_CPP11_COMPATIBILITY@")
set(VISP_HAVE_D3D9 "@VISP_HAVE_D3D9@")
+set(VISP_HAVE_DC1394 "@VISP_HAVE_DC1394@")
+set(VISP_HAVE_DIRECTSHOW "@VISP_HAVE_DIRECTSHOW@")
set(VISP_HAVE_DISPLAY "@VISP_HAVE_DISPLAY@")
+set(VISP_HAVE_DMTX "@VISP_HAVE_DMTX@")
+set(VISP_HAVE_EIGEN3 "@VISP_HAVE_EIGEN3@")
+set(VISP_HAVE_FRANKA "@VISP_HAVE_FRANKA@")
+set(VISP_HAVE_FLYCAPTURE "@VISP_HAVE_FLYCAPTURE@")
+set(VISP_HAVE_GDI "@VISP_HAVE_GDI@")
+set(VISP_HAVE_GTK "@VISP_HAVE_GTK@")
set(VISP_HAVE_GSL "@VISP_HAVE_GSL@")
+set(VISP_HAVE_JPEG "@VISP_HAVE_JPEG@")
+set(VISP_HAVE_LAPACK "@VISP_HAVE_LAPACK@")
set(VISP_HAVE_LAPACK_C "@VISP_HAVE_LAPACK_C@")
+set(VISP_HAVE_LAPACK_BUILT_IN "@VISP_HAVE_LAPACK_BUILT_IN@")
set(VISP_HAVE_LIBFREENECT "@VISP_HAVE_LIBFREENECT@")
set(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES "@VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES@")
set(VISP_HAVE_LIBFREENECT_OLD "@VISP_HAVE_LIBFREENECT_OLD@")
set(VISP_HAVE_LIBUSB_1 "@VISP_HAVE_LIBUSB_1@")
-set(VISP_HAVE_REALSENSE "@VISP_HAVE_REALSENSE@")
+set(VISP_HAVE_OGRE "@VISP_HAVE_OGRE@")
+set(VISP_HAVE_OIS "@VISP_HAVE_OIS@")
set(VISP_HAVE_OPENCV "@VISP_HAVE_OPENCV@")
set(VISP_HAVE_OPENCV_NONFREE "@VISP_HAVE_OPENCV_NONFREE@")
set(VISP_HAVE_OPENCV_VERSION "@VISP_HAVE_OPENCV_VERSION@")
-set(VISP_HAVE_OGRE "@VISP_HAVE_OGRE@")
set(VISP_HAVE_OPENGL "@VISP_HAVE_OPENGL@")
-set(VISP_HAVE_OIS "@VISP_HAVE_OIS@")
-set(VISP_HAVE_COIN3D_AND_GUI "@VISP_HAVE_COIN3D_AND_GUI@")
-set(VISP_HAVE_COIN3D "@VISP_HAVE_COIN3D@")
+set(VISP_HAVE_OPENMP "@VISP_HAVE_OPENMP@")
+set(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@")
+set(VISP_HAVE_PCL "@VISP_HAVE_PCL@")
+set(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@")
+set(VISP_HAVE_PNG "@VISP_HAVE_PNG@")
+set(VISP_HAVE_PTHREAD "@VISP_HAVE_PTHREAD@")
+set(VISP_HAVE_PTU46 "@VISP_HAVE_PTU46@")
+set(VISP_HAVE_PYLON "@VISP_HAVE_PYLON@")
set(VISP_HAVE_QT "@VISP_HAVE_QT@")
+set(VISP_HAVE_REALSENSE "@VISP_HAVE_REALSENSE@")
+set(VISP_HAVE_REALSENSE2 "@VISP_HAVE_REALSENSE2@")
set(VISP_HAVE_SOQT "@VISP_HAVE_SOQT@")
set(VISP_HAVE_SOWIN "@VISP_HAVE_SOWIN@")
set(VISP_HAVE_SOXT "@VISP_HAVE_SOXT@")
-set(VISP_HAVE_DC1394 "@VISP_HAVE_DC1394@")
-set(VISP_HAVE_CMU1394 "@VISP_HAVE_CMU1394@")
set(VISP_HAVE_V4L2 "@VISP_HAVE_V4L2@")
-set(VISP_HAVE_DIRECTSHOW "@VISP_HAVE_DIRECTSHOW@")
-set(VISP_HAVE_FLYCAPTURE "@VISP_HAVE_FLYCAPTURE@")
-set(VISP_HAVE_COMEDI "@VISP_HAVE_COMEDI@")
-set(VISP_HAVE_AFMA4 "@VISP_HAVE_AFMA4@")
-set(VISP_HAVE_AFMA6 "@VISP_HAVE_AFMA6@")
-set(VISP_HAVE_BICLOPS "@VISP_HAVE_BICLOPS@")
-set(VISP_HAVE_PTU46 "@VISP_HAVE_PTU46@")
set(VISP_HAVE_VIPER650 "@VISP_HAVE_VIPER650@")
set(VISP_HAVE_VIPER850 "@VISP_HAVE_VIPER850@")
-set(VISP_HAVE_PIONEER "@VISP_HAVE_PIONEER@")
set(VISP_HAVE_VIRTUOSE "@VISP_HAVE_VIRTUOSE@")
-set(VISP_HAVE_PARPORT "@VISP_HAVE_PARPORT@")
+set(VISP_HAVE_X11 "@VISP_HAVE_X11@")
set(VISP_HAVE_XML2 "@VISP_HAVE_XML2@")
-set(VISP_HAVE_JPEG "@VISP_HAVE_JPEG@")
-set(VISP_HAVE_PNG "@VISP_HAVE_PNG@")
-set(VISP_HAVE_FFMPEG "@VISP_HAVE_FFMPEG@")
set(VISP_HAVE_YARP "@VISP_HAVE_YARP@")
-set(VISP_HAVE_PCL "@VISP_HAVE_PCL@")
-set(VISP_HAVE_OPENMP "@VISP_HAVE_OPENMP@")
-set(VISP_HAVE_CPP11_COMPATIBILITY "@VISP_HAVE_CPP11_COMPATIBILITY@")
+set(VISP_HAVE_ZBAR "@VISP_HAVE_ZBAR@")
+
+ at VISP_CONTRIB_MODULES_CONFIGCMAKE@
set(VISP_COMPILER_FAIL_REGEX
"command line option .* is valid for .* but not for C\\+\\+" # GNU
@@ -265,15 +278,30 @@ if(VISP_HAVE_OPENMP)
endif()
#----------------------------------------------------------------------
+# Propagate c++14 option requested with libfranka
+#----------------------------------------------------------------------
+if(VISP_HAVE_FRANKA)
+ set(CMAKE_CXX_STANDARD 14)
+ set(CMAKE_CXX_STANDARD_REQUIRED ON)
+endif()
+
+#----------------------------------------------------------------------
# Some useful macro to be able to build the tutorials along side ViSP
#----------------------------------------------------------------------
# Create a target from the *.cpp file, link against ViSP libraries and add a dependency to ViSP library
# tu ensure that the library is build before this target.
macro(visp_add_target file_cpp)
+ if(POLICY CMP0020)
+ cmake_policy(PUSH)
+ cmake_policy(SET CMP0020 NEW) # For UsTK: Qt5
+ endif()
get_filename_component(target ${file_cpp} NAME_WE)
include_directories(${VISP_INCLUDE_DIRS})
add_executable(${target} ${file_cpp})
target_link_libraries(${target} ${VISP_LIBRARIES})
+ if(POLICY CMP0020)
+ cmake_policy(POP) #cmake_policy(SET CMP0020 NEW) # For UsTK: Qt5
+ endif()
endmacro()
# Create a dependency to the target extracted from the *.cpp file and put the target in the solution dependency folder.
diff --git a/cmake/templates/cmake_uninstall.cmake.in b/cmake/templates/cmake_uninstall.cmake.in
index e4fad57..84e76a9 100644
--- a/cmake/templates/cmake_uninstall.cmake.in
+++ b/cmake/templates/cmake_uninstall.cmake.in
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/cmake/templates/visp-config.bat.in b/cmake/templates/visp-config.bat.in
index 8841884..39d796b 100755
--- a/cmake/templates/visp-config.bat.in
+++ b/cmake/templates/visp-config.bat.in
@@ -3,9 +3,10 @@
@rem # This file is part of the ViSP software.
@rem # Copyright (C) 2005 - 2017 by Inria. All rights reserved.
@rem #
- at rem # This software is free software; you can redistribute it and/or
- at rem # modify it under the terms of the GNU General Public License
- at rem # ("GPL") version 2 as published by the Free Software Foundation.
+ at rem # This software is free software; you can redistribute it and/or modify
+ at rem # it under the terms of the GNU General Public License as published by
+ at rem # the Free Software Foundation; either version 2 of the License, or
+ at rem # (at your option) any later version.
@rem # See the file LICENSE.txt at the root directory of this source
@rem # distribution for additional information about the GNU GPL.
@rem #
@@ -47,19 +48,19 @@ set CURPATH=%CD /d %%~dp0%
set PREFIX=%CURPATH%\..
-set VERSION=${VISP_VERSION}
+set VERSION=@VISP_VERSION@
-set DEFS=${VISP_CONFIG_SCRIPT_DEFS}
+set DEFS=@VISP_CONFIG_SCRIPT_DEFS@
-set INCLUDE="${VISP_CONFIG_SCRIPT_INC}"
+set INCLUDE="@VISP_CONFIG_SCRIPT_INC@"
-set OPENMP_SUPPORT=${VISP_OPENMP_SUPPORT}
+set OPENMP_SUPPORT=@VISP_OPENMP_SUPPORT@
-set LIBDIR="${VISP_CONFIG_SCRIPT_LIBDIR}"
+set LIBDIR="@VISP_CONFIG_SCRIPT_LIBDIR@"
-set LIBS_DEBUG=${VISP_CONFIG_SCRIPT_LIBS_DEBUG}
+set LIBS_DEBUG=@VISP_CONFIG_SCRIPT_LIBS_DEBUG@
-set LIBS_OPTIMIZED=${VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED}
+set LIBS_OPTIMIZED=@VISP_CONFIG_SCRIPT_LIBS_OPTIMIZED@
@rem Test if an argument is provided
diff --git a/cmake/templates/visp-config.in b/cmake/templates/visp-config.in
index 6eb8e02..5915c0b 100755
--- a/cmake/templates/visp-config.in
+++ b/cmake/templates/visp-config.in
@@ -5,9 +5,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -44,14 +45,14 @@ relpath=`(cd $relpath/..; pwd)`
PREFIX=$relpath
-CFLAGS="${VISP_CONFIG_CFLAGS_SCRIPT}"
+CFLAGS="@VISP_CONFIG_CFLAGS_SCRIPT@"
-LIBS="${VISP_CONFIG_LIBS_SCRIPT}"
+LIBS="@VISP_CONFIG_LIBS_SCRIPT@"
-VERSION="${VISP_VERSION}"
+VERSION="@VISP_VERSION@"
-NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
-NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
+NO_NEWLINE_CHARACTER="@VISP_ECHO_NO_NEWLINE_CHARACTER@"
+NO_NEWLINE_OPTION="@VISP_ECHO_NO_NEWLINE_OPTION@"
usage()
{
@@ -82,14 +83,14 @@ fi;
for arg in $@; do
case $arg in
- --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
- --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
- --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
- --version)
- echo "ViSP $VERSION (Visual Servoing Platform)"
- echo ""
- echo "Copyright (C) 2005 - 2017 Inria. All rights reserved.";;
- --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
+ --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
+ --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
+ --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
+ --version)
+ echo "ViSP $VERSION (Visual Servoing Platform)"
+ echo ""
+ echo "Copyright (C) 2005 - 2017 Inria. All rights reserved.";;
+ --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
*) usage; exit 0 ;;
esac;
done;
diff --git a/cmake/templates/visp-config.install.in b/cmake/templates/visp-config.install.in
index 166c2e2..617f493 100755
--- a/cmake/templates/visp-config.install.in
+++ b/cmake/templates/visp-config.install.in
@@ -5,9 +5,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -31,7 +32,7 @@
#
# Description:
# visp-config shell script.
-# Auto-generated from visp-config.install.in by cmake. For backward
+# Auto-generated from visp-config.install.in by cmake. For backward
# compatibility used visp.pc.
#
# Authors:
@@ -54,8 +55,8 @@ LIBS=$LIBS_CMD
VERSION_CMD=`pkg-config --modversion visp`
VERSION=$VERSION_CMD
-NO_NEWLINE_CHARACTER="${VISP_ECHO_NO_NEWLINE_CHARACTER}"
-NO_NEWLINE_OPTION="${VISP_ECHO_NO_NEWLINE_OPTION}"
+NO_NEWLINE_CHARACTER="@VISP_ECHO_NO_NEWLINE_CHARACTER@"
+NO_NEWLINE_OPTION="@VISP_ECHO_NO_NEWLINE_OPTION@"
usage()
{
@@ -86,14 +87,14 @@ fi;
for arg in $@; do
case $arg in
- --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
- --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
- --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
- --version)
- echo "ViSP $VERSION (Visual Servoing Platform)"
- echo ""
- echo "Copyright (C) 2005 - 2017 Inria. All rights reserved.";;
- --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
+ --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
+ --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
+ --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
+ --version)
+ echo "ViSP $VERSION (Visual Servoing Platform)"
+ echo ""
+ echo "Copyright (C) 2005 - 2017 Inria. All rights reserved.";;
+ --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
*) usage; exit 0 ;;
esac;
done;
diff --git a/cmake/templates/visp.pc.in b/cmake/templates/visp.pc.in
index 4941a6a..451913c 100644
--- a/cmake/templates/visp.pc.in
+++ b/cmake/templates/visp.pc.in
@@ -1,14 +1,14 @@
# Package Information for pkg-config
-prefix=${CMAKE_INSTALL_PREFIX}
+prefix=@CMAKE_INSTALL_PREFIX@
exec_prefix=@exec_prefix@
libdir=@libdir@
includedir=@includedir@
Name: ViSP
Description: Visual Servoing Platform
-URL: http://visp.inria.fr
-Version: ${VISP_VERSION}
-Libs: ${VISP_CONFIG_LIBS_PC}
+URL: https://visp.inria.fr
+Version: @VISP_VERSION@
+Libs: @VISP_CONFIG_LIBS_PC@
-Cflags: ${VISP_CONFIG_CFLAGS_PC}
+Cflags: @VISP_CONFIG_CFLAGS_PC@
diff --git a/cmake/templates/vpConfig.h.in b/cmake/templates/vpConfig.h.in
index 65d0190..f026579 100644
--- a/cmake/templates/vpConfig.h.in
+++ b/cmake/templates/vpConfig.h.in
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -62,7 +63,7 @@
// 4710 : 'function' : function not inlined
// 4820 : 'bytes' bytes padding added after construct 'member_name'
// 4986 : undocumented
-
+
#ifndef NOMINMAX
#define NOMINMAX
#endif
@@ -111,6 +112,11 @@
VISP_VERSION_MINOR, \
VISP_VERSION_PATCH)
+// Enable debug and trace printings
+#cmakedefine VP_TRACE
+#cmakedefine VP_DEBUG
+#cmakedefine VP_DEBUG_MODE ${VP_DEBUG_MODE}
+
// ViSP library is either compiled static or shared
// Used to set declspec(import, export) in headers if required under Windows
#cmakedefine VISP_BUILD_SHARED_LIBS
@@ -121,6 +127,12 @@
// Defined if MSVC is the compiler
#cmakedefine VISP_USE_MSVC
+// Defined if Clipper is build and available.
+#cmakedefine VISP_HAVE_CLIPPER
+
+// Defined if April Tag is build and available.
+#cmakedefine VISP_HAVE_APRILTAG
+
// Defined if X11 library available.
#cmakedefine VISP_HAVE_X11
@@ -161,11 +173,21 @@
# define VISP_HAVE_DISPLAY
#endif
-// Defined if GSL library available (-lgsl -lgslcblas).
+// Defined if Eigen3 library available
+#cmakedefine VISP_HAVE_EIGEN3
+
+// Defined if GSL library available (-lgsl -lgslcblas)
#cmakedefine VISP_HAVE_GSL
-// Defined if lapack/blas libraries are available (-lblas -llapack).
-#cmakedefine VISP_HAVE_LAPACK_C
+// Defined if lapack/blas libraries are available (-lblas -llapack)
+#cmakedefine VISP_HAVE_LAPACK
+// To keep compat with previous versions
+#ifdef VISP_HAVE_LAPACK
+# define VISP_HAVE_LAPACK_C
+#endif
+
+// Defined if clapack built-in
+#cmakedefine VISP_HAVE_LAPACK_BUILT_IN
// Defined the path to the basic scenes used by the simulator
#cmakedefine VISP_SCENES_DIR "${VISP_SCENES_DIR}"
@@ -185,7 +207,7 @@
// Defined if OIS (Object Oriented Input System) library available.
#cmakedefine VISP_HAVE_OIS
-// Defined if Coin3D and one of the GUI (SoXt, SoWin, SoQt + Qt)
+// Defined if Coin3D and one of the GUI (SoXt, SoWin, SoQt + Qt)
// libraries are available.
#cmakedefine VISP_HAVE_COIN3D_AND_GUI
// provided for compat with previous releases
@@ -233,9 +255,9 @@
// Defined if libfreenect library available.
#cmakedefine VISP_HAVE_LIBFREENECT
-// Defined if libfreenect library in an old version package for
+// Defined if libfreenect library in an old version package for
// ubuntu 10.04 lucid is available.
-// This is a workaround useful to initialise vpKinect depending on the
+// This is a workaround useful to initialise vpKinect depending on the
// libfreenect version
//#ifdef VISP_HAVE_LIBFREENECT_OLD
// Freenect::Freenect<vpKinect> freenect;
@@ -249,11 +271,11 @@
// Defined if libusb-1.0 library available.
#cmakedefine VISP_HAVE_LIBUSB_1
-// Defined if RealSense library available.
+// Defined if librealSense library is available.
#cmakedefine VISP_HAVE_REALSENSE
-// Defined if ffmpeg library available.
-#cmakedefine VISP_HAVE_FFMPEG
+// Defined if librealSense2 library is available.
+#cmakedefine VISP_HAVE_REALSENSE2
// Defined if raw1394 and dc1394-2.x libraries available.
#cmakedefine VISP_HAVE_DC1394
@@ -289,6 +311,9 @@
// Defined if PointGrey FlyCapture SDK available.
#cmakedefine VISP_HAVE_FLYCAPTURE
+// Defined if Basler Pylon SDK available.
+#cmakedefine VISP_HAVE_PYLON
+
// Defined if Comedi (linux control and measurement cdevice interface) available.
#cmakedefine VISP_HAVE_COMEDI
@@ -326,8 +351,11 @@
#cmakedefine VISP_HAVE_VIPER850_DATA
#define VISP_VIPER850_DATA_PATH "${VISP_VIPER850_DATA_PATH}"
-// Defined if the Aria library and (pthread, rt, dl libraries under Unix) is found.
-// These libraries are used to control Pioneer mobile robots.
+// Defined if Franka Emika robot available.
+#cmakedefine VISP_HAVE_FRANKA
+
+// Defined if the Aria library and (pthread, rt, dl libraries under Unix) is found.
+// These libraries are used to control Pioneer mobile robots.
#cmakedefine VISP_HAVE_PIONEER
// Defined if Haption Virtuose SDK available.
@@ -348,7 +376,7 @@
// Defined if Doxygen documentation tool is found
#cmakedefine VISP_HAVE_DOXYGEN
-// Defined if we want to compute interaction matrices by combining
+// Defined if we want to compute interaction matrices by combining
// other interaction matrices
#cmakedefine VISP_MOMENTS_COMBINE_MATRICES
@@ -391,6 +419,9 @@
// Defined if strtof function is available
#cmakedefine VISP_HAVE_FUNC_STRTOF
+// Defined if log1p function is available
+#cmakedefine VISP_HAVE_FUNC_LOG1P
+
// Defined if xrandr program available
#cmakedefine VISP_HAVE_XRANDR
@@ -425,16 +456,16 @@
// Under Windows, for shared libraries (DLL) we need to define export on
// compilation or import on use (like a third party project).
-// We exploit here the fact that cmake auto set xxx_EXPORTS (with S) on
+// We exploit here the fact that cmake auto set xxx_EXPORTS (with S) on
// compilation.
#if defined(VISP_BUILD_SHARED_LIBS)
// Depending on whether one is building or using the
// library define VISP_EXPORT to import or export.
# ifdef visp_EXPORTS
# define VISP_EXPORT VISP_DLLEXPORT
-# else
+# else
# define VISP_EXPORT VISP_DLLIMPORT
-# endif
+# endif
# define VISP_LOCAL VISP_DLLLOCAL
#else
// If one is using the library statically, get rid of
diff --git a/demo/CMakeLists.txt b/demo/CMakeLists.txt
index eb3e59d..490f032 100644
--- a/demo/CMakeLists.txt
+++ b/demo/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/demo/wireframe-simulator/CMakeLists.txt b/demo/wireframe-simulator/CMakeLists.txt
index 703f3ec..c3ebd52 100644
--- a/demo/wireframe-simulator/CMakeLists.txt
+++ b/demo/wireframe-simulator/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -28,7 +29,7 @@
# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
#
# Description:
-# ViSP overall configuration file.
+# ViSP overall configuration file.
#
# Authors:
# Fabien Spindler
@@ -63,7 +64,7 @@ endforeach()
# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
# environment variable to the ViSP test sequences location.
# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
+# https://visp.inria.fr/download/
add_test(servoSimu4Points servoSimu4Points -d)
add_test(servoSimuCylinder servoSimuCylinder -d)
add_test(servoSimuSphere servoSimuSphere -d)
diff --git a/demo/wireframe-simulator/servoSimu4Points.cpp b/demo/wireframe-simulator/servoSimu4Points.cpp
index 00b8347..c897540 100644
--- a/demo/wireframe-simulator/servoSimu4Points.cpp
+++ b/demo/wireframe-simulator/servoSimu4Points.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,26 +45,26 @@
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "dh"
+#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
@@ -102,7 +103,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set mire.pgm image input path.\n\
- From this path read \"ViSP-images/mire/mire.pgm\" video.\n\
+ From this path read \"mire/mire.pgm\" image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment variable \n\
produces the same behaviour than using this option.\n\
\n\
@@ -116,7 +117,6 @@ OPTIONS: Default\n\
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
-
/*!
Set the program options.
@@ -132,17 +132,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0],ipath, NULL); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], ipath, NULL);
+ return false;
+ break;
default:
- usage(argv[0],ipath, optarg_);
- return false; break;
+ usage(argv[0], ipath, optarg_);
+ return false;
+ break;
}
}
@@ -157,25 +165,23 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
std::string opt_ipath;
std::string env_ipath;
- std::string ipath ;
+ std::string ipath;
std::string filename;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<vpRGBa> Iint(480,640,255);
- vpImage<vpRGBa> Iext1(480,640,255);
- vpImage<vpRGBa> Iext2(480,640,255);
+ vpImage<vpRGBa> Iint(480, 640, 255);
+ vpImage<vpRGBa> Iext1(480, 640, 255);
+ vpImage<vpRGBa> Iext2(480, 640, 255);
#if defined VISP_HAVE_X11
vpDisplayX display[3];
@@ -189,15 +195,14 @@ main(int argc, const char ** argv)
vpDisplayGTK display[3];
#endif
- if (opt_display)
- {
+ if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(Iint, 100, 100,"The internal view") ;
- display[1].init(Iext1, 100, 100,"The first external view") ;
- display[2].init(Iext2, 100, 100,"The second external view") ;
- vpDisplay::setWindowPosition (Iint, 0, 0);
- vpDisplay::setWindowPosition (Iext1, 700, 0);
- vpDisplay::setWindowPosition (Iext2, 0, 550);
+ display[0].init(Iint, 100, 100, "The internal view");
+ display[1].init(Iext1, 100, 100, "The first external view");
+ display[2].init(Iext2, 100, 100, "The second external view");
+ vpDisplay::setWindowPosition(Iint, 0, 0);
+ vpDisplay::setWindowPosition(Iext1, 700, 0);
+ vpDisplay::setWindowPosition(Iext2, 0, 550);
vpDisplay::display(Iint);
vpDisplay::flush(Iint);
vpDisplay::display(Iext1);
@@ -207,46 +212,47 @@ main(int argc, const char ** argv)
}
vpServo task;
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
- // Since the task gain lambda is very high, we need to increase default max velocities
+ // Since the task gain lambda is very high, we need to increase default
+ // max velocities
robot.setMaxTranslationVelocity(10);
robot.setMaxRotationVelocity(vpMath::rad(180));
// Set initial position of the object in the camera frame
- vpHomogeneousMatrix cMo(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
+ 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);
+ vpHomogeneousMatrix wMo(0.0, 0.0, 0.2, 0, 0, 0);
// Position of the camera in the world frame
vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
- //The four point used as visual features
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
+ // The four point used as visual features
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
// Projection of the points
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
point[i].track(cMo);
- //Set the current visual feature
+ // Set the current visual feature
vpFeaturePoint p[4];
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
vpFeatureBuilder::create(p[i], point[i]);
// Projection of the points
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
point[i].track(cdMo);
vpFeaturePoint pd[4];
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0; i < 4; i++)
vpFeatureBuilder::create(pd[i], point[i]);
task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
@@ -260,8 +266,8 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe);
task.set_eJe(eJe);
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
task.setLambda(10);
@@ -269,7 +275,8 @@ main(int argc, const char ** argv)
vpImageSimulator imsim;
vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
+ for (int i = 0; i < 4; i++)
+ X[i].resize(3);
X[0][0] = -0.2;
X[0][1] = -0.2;
X[0][2] = 0;
@@ -286,16 +293,17 @@ main(int argc, const char ** argv)
X[3][1] = 0.2;
X[3][2] = 0;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
if (!opt_ipath.empty())
ipath = opt_ipath;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/mire/mire.pgm");
+ filename = vpIoTools::createFilePath(ipath, "mire/mire.pgm");
imsim.init(filename.c_str(), X);
@@ -304,124 +312,125 @@ 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.setCameraPositionRelObj(cMo) ; // initial position of the camera
+ 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
- vpHomogeneousMatrix camoMf(0,0.0,1.5,0,vpMath::rad(140),0);
- camoMf = camoMf*(sim.get_fMo().inverse());
+ // Computes the position of a camera which is fixed in the object frame
+ vpHomogeneousMatrix camoMf(0, 0.0, 1.5, 0, vpMath::rad(140), 0);
+ camoMf = camoMf * (sim.get_fMo().inverse());
- //Set the parameters of the cameras (internal and external)
- vpCameraParameters camera(1000,1000,320,240);
+ // Set the parameters of the cameras (internal and external)
+ vpCameraParameters camera(1000, 1000, 320, 240);
sim.setInternalCameraParameters(camera);
sim.setExternalCameraParameters(camera);
int stop = 10;
- if (opt_display)
- {
+ if (opt_display) {
stop = 2500;
- //Get the internal and external views
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
sim.getExternalImage(Iext2, camoMf);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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,camera,0.2,vpColor::none);
+ // 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, 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);
+ // 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::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) && !vpDisplay::getClick(Iext2,false)){};
+ while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
+ !vpDisplay::getClick(Iext2, false)) {
+ };
}
- robot.setPosition( wMc );
- //Print the task
- task.print() ;
+ robot.setPosition(wMc);
+ // Print the task
+ task.print();
int iter = 0;
- vpColVector v ;
-
- while(iter++ < stop)
- {
- if (opt_display)
- {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext1) ;
- vpDisplay::display(Iext2) ;
+ vpColVector v;
+
+ while (iter++ < stop) {
+ if (opt_display) {
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext1);
+ vpDisplay::display(Iext2);
}
double t = vpTime::measureTimeMs();
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
- wMc = robot.getPosition() ;
+ wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (int i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ vpFeatureBuilder::create(p[i], point[i]);
}
- v = task.computeControlLaw() ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ v = task.computeControlLaw();
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- //Compute the movement of the object around the world reference frame.
+ // Compute the movement of the object around the world reference frame.
vpHomogeneousMatrix a(0, 0, 0.2, 0, 0, 0);
- vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5*iter), 0, 0);
- vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5*iter), 0);
+ vpHomogeneousMatrix b(0, 0, 0, vpMath::rad(1.5 * iter), 0, 0);
+ vpHomogeneousMatrix c(0, 0, 0, 0, vpMath::rad(2.5 * iter), 0);
// Move the object in the world frame
- wMo = b*c*a;
+ wMo = b * c * a;
- sim.set_fMo( wMo ); //Move the object in the simulator
+ sim.set_fMo(wMo); // Move the object in the simulator
sim.setCameraPositionRelObj(cMo);
- //Compute the position of the external view which is fixed in the object frame
- camoMf.buildFrom(0,0.0,1.5,0,vpMath::rad(150),0);
- camoMf = camoMf*(sim.get_fMo().inverse());
+ // Compute the position of the external view which is fixed in the
+ // object frame
+ camoMf.buildFrom(0, 0.0, 1.5, 0, vpMath::rad(150), 0);
+ camoMf = camoMf * (sim.get_fMo().inverse());
- if (opt_display)
- {
- //Get the internal and external views
+ if (opt_display) {
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
- sim.getExternalImage(Iext2,camoMf);
+ sim.getExternalImage(Iext2, camoMf);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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);
+ // 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);
- //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);
+ // 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::flush(Iint);
vpDisplay::flush(Iext1);
@@ -430,24 +439,23 @@ 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() ;
- task.kill() ;
+ task.print();
+ task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
+ vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display "
+ "functionalities...");
}
#endif
diff --git a/demo/wireframe-simulator/servoSimuCylinder.cpp b/demo/wireframe-simulator/servoSimuCylinder.cpp
index d64d4e3..70a80cb 100644
--- a/demo/wireframe-simulator/servoSimuCylinder.cpp
+++ b/demo/wireframe-simulator/servoSimuCylinder.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,30 +42,29 @@
Demonstration of the wireframe simulator with a simple visual servoing.
*/
-
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCylinder.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "dh"
+#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
void usage(const char *name, const char *badparam);
@@ -110,7 +110,6 @@ OPTIONS: \n\
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
-
/*!
Set the program options.
@@ -125,16 +124,22 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -149,20 +154,18 @@ bool getOptions(int argc, const char **argv, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<vpRGBa> Iint(480,640,255);
- vpImage<vpRGBa> Iext(480,640,255);
+ vpImage<vpRGBa> Iint(480, 640, 255);
+ vpImage<vpRGBa> Iext(480, 640, 255);
#if defined VISP_HAVE_X11
vpDisplayX display[2];
@@ -176,13 +179,12 @@ main(int argc, const char ** argv)
vpDisplayGTK display[2];
#endif
- if (opt_display)
- {
+ if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(Iint, 100, 100,"The internal view") ;
- display[1].init(Iext, 100, 100,"The first external view") ;
- vpDisplay::setWindowPosition (Iint, 0, 0);
- vpDisplay::setWindowPosition (Iext, 700, 0);
+ display[0].init(Iint, 100, 100, "The internal view");
+ display[1].init(Iext, 100, 100, "The first external view");
+ vpDisplay::setWindowPosition(Iint, 0, 0);
+ vpDisplay::setWindowPosition(Iext, 700, 0);
vpDisplay::display(Iint);
vpDisplay::flush(Iint);
vpDisplay::display(Iext);
@@ -190,31 +192,31 @@ main(int argc, const char ** argv)
}
vpServo task;
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
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);
+ vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
// Position of the camera in the world frame
vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
// Create a cylinder
- vpCylinder cylinder(0,0,1,0,0,0,0.1);
+ vpCylinder cylinder(0, 0, 1, 0, 0, 0, 0.1);
// Projection of the cylinder
cylinder.track(cMo);
- //Set the current visual feature
+ // Set the current visual feature
vpFeatureLine l[2];
vpFeatureBuilder::create(l[0], cylinder, vpCylinder::line1);
vpFeatureBuilder::create(l[1], cylinder, vpCylinder::line2);
-
+
// Projection of the cylinder
cylinder.track(cdMo);
@@ -233,8 +235,8 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe);
task.set_eJe(eJe);
- for (int i = 0 ; i < 2 ; i++)
- task.addFeature(l[i],ld[i]) ;
+ for (int i = 0; i < 2; i++)
+ task.addFeature(l[i], ld[i]);
task.setLambda(1);
@@ -244,146 +246,146 @@ main(int argc, const char ** argv)
sim.initScene(vpWireFrameSimulator::CYLINDER, vpWireFrameSimulator::D_STANDARD);
// Initialize simulator frames
- sim.set_fMo( wMo ); // Position of the object in the world reference frame
- sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
+ 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)
- vpCameraParameters camera(1000,1000,320,240);
+ vpCameraParameters camera(1000, 1000, 320, 240);
sim.setInternalCameraParameters(camera);
sim.setExternalCameraParameters(camera);
int stop = 10;
- if (opt_display)
- {
+ if (opt_display) {
stop = 2500;
- //Get the internal and external views
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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,camera,0.2,vpColor::none);
+ // 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, 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)) {
+ };
}
- robot.setPosition( wMc );
+ robot.setPosition(wMc);
- //Print the task
+ // Print the task
task.print();
int iter = 0;
- vpColVector v ;
-
- //Set the secondary task parameters
- vpColVector e1(6) ; e1 = 0 ;
- vpColVector e2(6) ; e2 = 0 ;
- vpColVector proj_e1 ;
- vpColVector proj_e2 ;
+ vpColVector v;
+
+ // Set the secondary task parameters
+ vpColVector e1(6);
+ e1 = 0;
+ vpColVector e2(6);
+ e2 = 0;
+ vpColVector proj_e1;
+ vpColVector proj_e2;
iter = 0;
double rapport = 0;
double vitesse = 0.3;
int tempo = 600;
- while(iter++ < stop)
- {
- if (opt_display)
- {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
+ while (iter++ < stop) {
+ if (opt_display) {
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
}
double t = vpTime::measureTimeMs();
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- cylinder.track(cMo) ;
+ cylinder.track(cMo);
vpFeatureBuilder::create(l[0], cylinder, vpCylinder::line1);
vpFeatureBuilder::create(l[1], cylinder, vpCylinder::line2);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- //Compute the velocity with the secondary task
- if ( iter%tempo < 200 && iter%tempo >= 0)
- {
+ // Compute the velocity with the secondary task
+ if (iter % tempo < 200 && iter % tempo >= 0) {
e2 = 0;
- e1[0] = -fabs(vitesse) ;
+ e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = -vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
+ rapport = -vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
}
- if ( iter%tempo < 300 && iter%tempo >= 200)
- {
+ if (iter % tempo < 300 && iter % tempo >= 200) {
e1 = 0;
- e2[1] = -fabs(vitesse) ;
+ e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = -vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = -vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
- if ( iter%tempo < 500 && iter%tempo >= 300)
- {
+ if (iter % tempo < 500 && iter % tempo >= 300) {
e2 = 0;
- e1[0] = -fabs(vitesse) ;
+ e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
+ rapport = vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
}
- if ( iter%tempo < 600 && iter%tempo >= 500)
- {
+ if (iter % tempo < 600 && iter % tempo >= 500) {
e1 = 0;
- e2[1] = -fabs(vitesse) ;
+ e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// Update the simulator frames
- sim.set_fMo( wMo ); // This line is not really requested since the object doesn't move
- sim.setCameraPositionRelObj( cMo );
+ sim.set_fMo(wMo); // This line is not really requested since the object
+ // doesn't move
+ sim.setCameraPositionRelObj(cMo);
- if (opt_display)
- {
- //Get the internal and external views
+ if (opt_display) {
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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);;
+ // 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::flush(Iint);
vpDisplay::flush(Iext);
@@ -391,24 +393,23 @@ 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() ;
- task.kill() ;
+ task.print();
+ task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
+ vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display "
+ "functionalities...");
}
#endif
diff --git a/demo/wireframe-simulator/servoSimuSphere.cpp b/demo/wireframe-simulator/servoSimuSphere.cpp
index f779ebf..8b9ef5e 100644
--- a/demo/wireframe-simulator/servoSimuSphere.cpp
+++ b/demo/wireframe-simulator/servoSimuSphere.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,39 +42,39 @@
Demonstration of the wireframe simulator with a simple visual servoing.
*/
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpGenericFeature.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/core/vpSphere.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "dh"
+#define GETOPTARGS "dh"
#ifdef VISP_HAVE_DISPLAY
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);
/*!
@@ -121,16 +122,22 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -145,15 +152,13 @@ bool getOptions(int argc, const char **argv, bool &display)
return true;
}
-
/*
- Computes the virtual visual features corresponding to the sphere and stores it in the generic feature.
-
- The visual feature vector is computed thanks to the following formula : s = {sx, sy, sz}
- sx = gx*h2/(sqrt(h2+1)
- sx = gy*h2/(sqrt(h2+1)
- sz = sqrt(h2+1)
-
+ Computes the virtual visual features corresponding to the sphere and stores
+ it in the generic feature.
+
+ The visual feature vector is computed thanks to the following formula : s =
+ {sx, sy, sz} sx = gx*h2/(sqrt(h2+1) sx = gy*h2/(sqrt(h2+1) sz = sqrt(h2+1)
+
with gx and gy the center of gravity of the ellipse,
with h2 = (gx²+gy²)/(4*n20*gy²+4*n02*gx²-8n11gxgy)
with n20,n02,n11 the second order moments of the sphere
@@ -166,60 +171,59 @@ void computeVisualFeatures(const vpSphere &sphere, vpGenericFeature &s)
double m20 = sphere.get_mu20();
double m11 = sphere.get_mu11();
double h2;
- //if (gx != 0 || gy != 0)
+ // if (gx != 0 || gy != 0)
if (std::fabs(gx) > std::numeric_limits<double>::epsilon() || std::fabs(gy) > std::numeric_limits<double>::epsilon())
- h2 = (vpMath::sqr(gx)+vpMath::sqr(gy))/(4*m20*vpMath::sqr(gy)+4*m02*vpMath::sqr(gx)-8*m11*gx*gy);
+ h2 = (vpMath::sqr(gx) + vpMath::sqr(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));
-
- s.set_s(sx,sy,sz);
+ 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));
+
+ s.set_s(sx, sy, sz);
}
/*
Computes the interaction matrix such as L = [-1/R*I3 [s]x]
-
+
with R the radius of the sphere
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();
- L[1][1] = -1/sphere.getR();
- L[2][2] = -1/sphere.getR();
-
- double s0,s1,s2;
- s.get_s(s0,s1,s2);
-
- vpTranslationVector c(s0,s1,s2);
+ L[0][0] = -1 / sphere.getR();
+ L[1][1] = -1 / sphere.getR();
+ L[2][2] = -1 / sphere.getR();
+
+ double s0, s1, s2;
+ s.get_s(s0, s1, s2);
+
+ vpTranslationVector c(s0, s1, s2);
vpMatrix sk;
sk = c.skew();
-
- for(unsigned int i = 0; i < 3; i++)
- for(unsigned int j = 0; j < 3; j++)
- L[i][j+3] = sk[i][j];
-}
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ L[i][j + 3] = sk[i][j];
+}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<vpRGBa> Iint(480,640,255);
- vpImage<vpRGBa> Iext1(480,640,255);
- vpImage<vpRGBa> Iext2(480,640,255);
+ vpImage<vpRGBa> Iint(480, 640, 255);
+ vpImage<vpRGBa> Iext1(480, 640, 255);
+ vpImage<vpRGBa> Iext2(480, 640, 255);
#if defined VISP_HAVE_X11
vpDisplayX display[3];
@@ -233,15 +237,14 @@ main(int argc, const char ** argv)
vpDisplayGTK display[3];
#endif
- if (opt_display)
- {
+ if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(Iint, 100, 100,"The internal view") ;
- display[1].init(Iext1, 100, 100,"The first external view") ;
- display[2].init(Iext2, 100, 100,"The second external view") ;
- vpDisplay::setWindowPosition (Iint, 0, 0);
- vpDisplay::setWindowPosition (Iext1, 700, 0);
- vpDisplay::setWindowPosition (Iext2, 0, 550);
+ display[0].init(Iint, 100, 100, "The internal view");
+ display[1].init(Iext1, 100, 100, "The first external view");
+ display[2].init(Iext2, 100, 100, "The second external view");
+ vpDisplay::setWindowPosition(Iint, 0, 0);
+ vpDisplay::setWindowPosition(Iext1, 700, 0);
+ vpDisplay::setWindowPosition(Iext2, 0, 550);
vpDisplay::display(Iint);
vpDisplay::flush(Iint);
vpDisplay::display(Iext1);
@@ -251,44 +254,45 @@ main(int argc, const char ** argv)
}
vpServo task;
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
- // Since the task gain lambda is very high, we need to increase default max velocities
+ // Since the task gain lambda is very high, we need to increase default
+ // max velocities
robot.setMaxTranslationVelocity(10);
robot.setMaxRotationVelocity(vpMath::rad(180));
// Set initial position of the object in the camera frame
- vpHomogeneousMatrix cMo(0,0.1,2.0,vpMath::rad(35),vpMath::rad(25),0);
+ 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);
+ vpHomogeneousMatrix wMo(0.0, 0.0, 0, 0, 0, 0);
// Position of the camera in the world frame
vpHomogeneousMatrix wMc;
wMc = wMo * cMo.inverse();
- robot.setPosition( wMc );
+ robot.setPosition(wMc);
- //The sphere
- vpSphere sphere(0,0,0,0.15);
+ // The sphere
+ vpSphere sphere(0, 0, 0, 0.15);
// Projection of the sphere
sphere.track(cMo);
- //Set the current visual feature
+ // Set the current visual feature
vpGenericFeature s(3);
computeVisualFeatures(sphere, s);
-
+
// Projection of the points
sphere.track(cdMo);
vpGenericFeature sd(3);
computeVisualFeatures(sphere, sd);
- vpMatrix L(3,6);
- computeInteractionMatrix(sd,sphere,L);
+ vpMatrix L(3, 6);
+ computeInteractionMatrix(sd, sphere, L);
sd.setInteractionMatrix(L);
task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
@@ -302,7 +306,7 @@ main(int argc, const char ** argv)
robot.get_eJe(eJe);
task.set_eJe(eJe);
- task.addFeature(s,sd) ;
+ task.addFeature(s, sd);
task.setLambda(7);
@@ -312,109 +316,111 @@ main(int argc, const char ** argv)
sim.initScene(vpWireFrameSimulator::SPHERE, vpWireFrameSimulator::D_STANDARD);
// Initialize simulator frames
- sim.set_fMo( wMo ); // Position of the object in the world reference frame
- sim.setCameraPositionRelObj(cMo) ; // initial position of the camera
+ 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
- vpHomogeneousMatrix camoMf(0,0.0,2.5,0,vpMath::rad(140),0);
- camoMf = camoMf*(sim.get_fMo().inverse());
+ vpHomogeneousMatrix camoMf(0, 0.0, 2.5, 0, vpMath::rad(140), 0);
+ camoMf = camoMf * (sim.get_fMo().inverse());
// Set the parameters of the cameras (internal and external)
- vpCameraParameters camera(1000,1000,320,240);
+ vpCameraParameters camera(1000, 1000, 320, 240);
sim.setInternalCameraParameters(camera);
sim.setExternalCameraParameters(camera);
int stop = 10;
- if (opt_display)
- {
+ if (opt_display) {
stop = 1000;
- //Get the internal and external views
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
- sim.getExternalImage(Iext2,camoMf);
+ sim.getExternalImage(Iext2, camoMf);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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,camera,0.2,vpColor::none);
+ // 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, 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);
+ // 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::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) && !vpDisplay::getClick(Iext2,false)){};
+ while (!vpDisplay::getClick(Iint, false) && !vpDisplay::getClick(Iext1, false) &&
+ !vpDisplay::getClick(Iext2, false)) {
+ };
}
- //Print the task
- task.print() ;
+ // Print the task
+ task.print();
int iter = 0;
- vpColVector v ;
-
- while(iter++ < stop)
- {
- if (opt_display)
- {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext1) ;
- vpDisplay::display(Iext2) ;
+ vpColVector v;
+
+ while (iter++ < stop) {
+ if (opt_display) {
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext1);
+ vpDisplay::display(Iext2);
}
double t = vpTime::measureTimeMs();
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
- wMc = robot.getPosition() ;
+ wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
sphere.track(cMo);
- //Set the current visual feature
+ // Set the current visual feature
computeVisualFeatures(sphere, s);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
sim.setCameraPositionRelObj(cMo);
- //Compute the position of the external view which is fixed in the object frame
- camoMf.buildFrom(0,0.0,2.5,0,vpMath::rad(150),0);
- camoMf = camoMf*(sim.get_fMo().inverse());
+ // Compute the position of the external view which is fixed in the
+ // object frame
+ camoMf.buildFrom(0, 0.0, 2.5, 0, vpMath::rad(150), 0);
+ camoMf = camoMf * (sim.get_fMo().inverse());
- if (opt_display)
- {
- //Get the internal and external views
+ if (opt_display) {
+ // Get the internal and external views
sim.getInternalImage(Iint);
sim.getExternalImage(Iext1);
- sim.getExternalImage(Iext2,camoMf);
+ sim.getExternalImage(Iext2, camoMf);
- //Display the object frame (current and desired position)
- vpDisplay::displayFrame(Iint,cMo,camera,0.2,vpColor::none);
- vpDisplay::displayFrame(Iint,cdMo,camera,0.2,vpColor::none);
+ // Display the object frame (current and desired position)
+ vpDisplay::displayFrame(Iint, cMo, camera, 0.2, vpColor::none);
+ vpDisplay::displayFrame(Iint, cdMo, camera, 0.2, vpColor::none);
- //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);
+ // 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);
- //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);
+ // 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::flush(Iint);
vpDisplay::flush(Iext1);
@@ -423,23 +429,22 @@ 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() ;
- task.kill() ;
+ task.print();
+ task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
+ vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display "
+ "functionalities...");
}
#endif
diff --git a/doc/biblio/references.bib b/doc/biblio/references.bib
index 20928c3..0563ae3 100644
--- a/doc/biblio/references.bib
+++ b/doc/biblio/references.bib
@@ -377,7 +377,7 @@ x-advisor = {Chaumette, F.},
@inproceedings{Marey:2010,
TITLE = {{A new large projection operator for the redundancy framework}},
- AUTHOR = {Marey, Mohammed and Chaumette, Francois},
+ AUTHOR = {Marey, M. and Chaumette, F.},
URL = {https://hal.inria.fr/inria-00544789},
BOOKTITLE = {{IEEE Int. Conf. on Robotics and Automation, ICRA'10}},
ADDRESS = {Anchorage, Alaska, United States},
@@ -391,7 +391,7 @@ x-advisor = {Chaumette, F.},
@inproceedings{Marey:2010b,
TITLE = {{New strategies for avoiding robot joint limits: Application to visual servoing using a large projection operator}},
- AUTHOR = {Marey, Mohammed and Chaumette, Fran{\c c}ois},
+ AUTHOR = {Marey, M. and Chaumette, F.},
URL = {https://hal.inria.fr/inria-00544790},
BOOKTITLE = {{IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, IROS'10}},
ADDRESS = {Taipei, Taiwan, Taiwan},
@@ -420,7 +420,8 @@ x-advisor = {Chaumette, F.},
Pages = {1525--1528},
Address = {Brussels, Belgium},
Month = {September},
- Year = {2011}
+ Year = {2011},
+ url = {https://hal.inria.fr/hal-00639686}
}
@article{Dame12a,
@@ -435,10 +436,161 @@ x-advisor = {Chaumette, F.},
url = {https://hal.inria.fr/hal-00750528}
}
+ at article{Kermorgant14a,
+ Author = {Kermorgant, O. and Chaumette, F.},
+ Title = {Dealing with constraints in sensor-based robot control},
+ Journal = {IEEE Trans. on Robotics},
+ Volume = { 30},
+ Number = {1},
+ Pages = {244--257},
+ Publisher = {IEEE},
+ Month = {February},
+ Year = {2014}
+ url = {https://hal.inria.fr/hal-00855724}
+}
+
@article{Marchand16a,
Author = {Marchand, E. and Uchiyama, H. and Spindler, F.},
Title = {Pose estimation for augmented reality: a hands-on survey},
Journal = {IEEE Trans. on Visualization and Computer Graphics},
Year = {2016},
url = {https://hal.inria.fr/hal-01246370}
-}
\ No newline at end of file
+}
+
+ at inproceedings{olson2011tags,
+ TITLE = {{AprilTag}: A robust and flexible visual fiducial system},
+ AUTHOR = {Edwin Olson},
+ BOOKTITLE = {Proceedings of the {IEEE} International Conference on Robotics and
+ Automation ({ICRA})},
+ YEAR = {2011},
+ MONTH = {May},
+ PAGES = {3400-3407},
+ KEYWORDS = {Robot navigation, SLAM, Visual Fiducial, ARToolkit},
+ PUBLISHER = {IEEE},
+}
+
+ at inproceedings{wang2016iros,
+ AUTHOR = {John Wang and Edwin Olson},
+ TITLE = {{AprilTag} 2: Efficient and robust fiducial detection},
+ BOOKTITLE = {Proceedings of the {IEEE/RSJ} International Conference on Intelligent
+ Robots and Systems {(IROS)}},
+ YEAR = {2016},
+ MONTH = {October},
+}
+
+ at article{Pressigout:2007,
+ author = {Muriel Pressigout and Eric Marchand},
+ title = {Real-time Hybrid Tracking using Edge and Texture Information},
+ journal = {The International Journal of Robotics Research},
+ volume = {26},
+ number = {7},
+ pages = {689-713},
+ year = {2007},
+ doi = {10.1177/0278364907080477},
+ URL = {https://doi.org/10.1177/0278364907080477},
+ eprint = {https://doi.org/10.1177/0278364907080477},
+ abstract = { This paper proposes a real-time, robust and effective tracking framework for visual servoing applications. The algorithm is based on the fusion of visual cues and on the estimation of a transformation (either a homography or a 3D pose). The parameters of this transformation are estimated using a non-linear minimization of a unique criterion that integrates information both on the texture and the edges of the tracked object. The proposed tracker is more robust and performs [...]
+}
+
+
+ at INPROCEEDINGS{Huang_imagethresholding,
+ author = {Liang-kai Huang and Mao-j Iun and J. Wangt},
+ title = {Image thresholding by minimizing the measure of fuzziness},
+ booktitle = {Pattern Recognition, 1995,28(1):41-51. 113 International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences. Vol. XXXVII. Part B2. Beijing 2008},
+ year = {}
+}
+
+ at article{NYAS:NYAS1035,
+ author = {Prewitt, Judith M. S. and Mendelsohn, Mortimer L.},
+ title = {THE ANALYSIS OF CELL IMAGES*},
+ journal = {Annals of the New York Academy of Sciences},
+ volume = {128},
+ number = {3},
+ publisher = {Blackwell Publishing Ltd},
+ issn = {1749-6632},
+ url = {http://dx.doi.org/10.1111/j.1749-6632.1965.tb11715.x},
+ doi = {10.1111/j.1749-6632.1965.tb11715.x},
+ pages = {1035--1053},
+ year = {1966},
+}
+
+ at article{article4310039,
+ author = {Ridler, T.W. and Calvard, S},
+ year = {1978},
+ month = {08},
+ pages = {630-632},
+ title = {Picture Thresholding Using an Iterative Selection Method},
+ volume = {8},
+ booktitle = {IEEE Transactions on Systems, Man, and Cybernetics - TSMC}
+}
+
+ at article{Glasbey:1993:AHT:167725.167747,
+ author = {Glasbey, C. A.},
+ title = {An Analysis of Histogram-based Thresholding Algorithms},
+ journal = {CVGIP: Graph. Models Image Process.},
+ issue_date = {Nov. 1993},
+ volume = {55},
+ number = {6},
+ month = nov,
+ year = {1993},
+ issn = {1049-9652},
+ pages = {532--537},
+ numpages = {6},
+ url = {http://dx.doi.org/10.1006/cgip.1993.1040},
+ doi = {10.1006/cgip.1993.1040},
+ acmid = {167747},
+ publisher = {Academic Press, Inc.},
+ address = {Orlando, FL, USA},
+}
+
+ at article{article4310076,
+ author = {N. Otsu},
+ journal = {IEEE Transactions on Systems, Man, and Cybernetics},
+ title = {A Threshold Selection Method from Gray-Level Histograms},
+ volume = {9},
+ number = {1},
+ pages = {62-66},
+ month = {Jan},
+ year = {1979},
+}
+
+ at article{doi:10.1177/25.7.70454,
+ author = {G W Zack and W E Rogers and S A Latt},
+ title = {Automatic measurement of sister chromatid exchange frequency.},
+ journal = {Journal of Histochemistry \& Cytochemistry},
+ volume = {25},
+ number = {7},
+ pages = {741-753},
+ year = {1977},
+ doi = {10.1177/25.7.70454},
+ note = {PMID: 70454},
+ URL = {https://doi.org/10.1177/25.7.70454},
+ eprint = {https://doi.org/10.1177/25.7.70454},
+}
+
+ at article{articleSuzuki,
+ author = {Suzuki, Satoshi and be, KeiichiA},
+ year = {1985},
+ month = {03},
+ pages = {32-46},
+ title = {Topological structural analysis of digitized binary images by border following},
+ volume = {30},
+ booktitle = {Computer Vision, Graphics, and Image Processing}
+}
+
+ at inproceedings{Hare:2011:OIJ:2072298.2072421,
+ author = {Hare, Jonathon S. and Samangooei, Sina and Dupplaw, David P.},
+ title = {OpenIMAJ and ImageTerrier: Java libraries and tools for scalable multimedia analysis and indexing of images},
+ booktitle = {Proceedings of the 19th ACM international conference on Multimedia},
+ series = {MM '11},
+ year = {2011},
+ isbn = {978-1-4503-0616-4},
+ location = {Scottsdale, Arizona, USA},
+ pages = {691--694},
+ numpages = {4},
+ url = {http://doi.acm.org/10.1145/2072298.2072421},
+ doi = {10.1145/2072298.2072421},
+ acmid = {2072421},
+ publisher = {ACM},
+ address = {New York, NY, USA},
+}
diff --git a/doc/config-doxygen.in b/doc/config-doxygen.in
index acd9098..badb3d7 100644
--- a/doc/config-doxygen.in
+++ b/doc/config-doxygen.in
@@ -51,7 +51,7 @@ PROJECT_BRIEF =
# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
# the logo to the output directory.
-PROJECT_LOGO = "@VISP_SOURCE_DIR@/doc/image/img-logo-visp.png"
+PROJECT_LOGO = "@VISP_SOURCE_DIR@/doc/image/logo/img-logo-visp.png"
# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
# into which the generated documentation will be written. If a relative path is
@@ -703,7 +703,7 @@ LAYOUT_FILE =
# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
# search path. See also \cite for info how to create references.
-CITE_BIB_FILES = "@VISP_SOURCE_DIR@/doc/biblio/references.bib"
+CITE_BIB_FILES = @DOXYGEN_CITE_BIB_FILES@
#---------------------------------------------------------------------------
# Configuration options related to warning and progress messages
@@ -1648,14 +1648,14 @@ LATEX_OUTPUT = latex
# The default file is: latex.
# This tag requires that the tag GENERATE_LATEX is set to YES.
-LATEX_CMD_NAME = @LATEX_COMPILER@
+LATEX_CMD_NAME = "@LATEX_COMPILER@"
# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
# index for LaTeX.
# The default file is: makeindex.
# This tag requires that the tag GENERATE_LATEX is set to YES.
-MAKEINDEX_CMD_NAME = @MAKEINDEX_COMPILER@
+MAKEINDEX_CMD_NAME = "@MAKEINDEX_COMPILER@"
# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
# documents. This may be useful for small projects and may help to save some
@@ -2046,7 +2046,7 @@ SEARCH_INCLUDES = YES
# preprocessor.
# This tag requires that the tag SEARCH_INCLUDES is set to YES.
-INCLUDE_PATH =
+INCLUDE_PATH = "@VISP_BINARY_DIR@/modules/core"
# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
# patterns (like *.h and *.hpp) to filter out the header-files in the
@@ -2065,65 +2065,72 @@ INCLUDE_FILE_PATTERNS =
# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
PREDEFINED = @DOXYGEN_SHOULD_SKIP_THIS@ \
- VISP_HAVE_X11 \
- VISP_HAVE_PTHREAD \
- VISP_HAVE_GTK \
- VISP_HAVE_GDI \
+ VISP_HAVE_AFMA4 \
+ VISP_HAVE_AFMA6 \
+ VISP_HAVE_AFMA6_DATA \
+ VISP_HAVE_APRILTAG \
+ VISP_HAVE_ATIDAQ \
+ VISP_HAVE_CMU1394 \
+ VISP_HAVE_BICLOPS \
+ VISP_HAVE_CLIPPER \
+ VISP_HAVE_COIN3D \
+ VISP_HAVE_COIN3D_AND_GUI \
+ VISP_HAVE_COMEDI \
+ VISP_HAVE_CPP11_COMPATIBILITY \
VISP_HAVE_D3D9 \
- VISP_HAVE_GSL \
+ VISP_HAVE_DC1394 \
+ VISP_HAVE_DIRECTSHOW \
VISP_HAVE_DISPLAY \
- VISP_HAVE_LAPACK_C \
- VISP_HAVE_OPENCV \
- VISP_HAVE_OPENCV_NONFREE \
- VISP_HAVE_OPENCV_VERSION=0x020408 \
+ VISP_HAVE_DMTX \
+ VISP_HAVE_EIGEN3 \
+ VISP_HAVE_FRANKA \
+ VISP_HAVE_FLYCAPTURE \
+ VISP_HAVE_GDI \
+ VISP_HAVE_GTK \
+ VISP_HAVE_GSL \
+ VISP_HAVE_JPEG \
+ VISP_HAVE_LAPACK \
+ VISP_HAVE_LIBFREENECT \
+ VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES \
+ VISP_HAVE_LIBUSB_1 \
VISP_HAVE_OGRE \
VISP_HAVE_OGRE_PLUGINS_PATH \
VISP_HAVE_OGRE_RESOURCES_PATH \
VISP_HAVE_OIS \
- VISP_HAVE_COIN3D_AND_GUI \
- VISP_HAVE_COIN3D \
+ VISP_HAVE_OPENCV \
+ VISP_HAVE_OPENCV_NONFREE \
+ VISP_HAVE_OPENCV_VERSION=0x020408 \
VISP_HAVE_OPENGL \
+ VISP_HAVE_PARPORT \
+ VISP_HAVE_PCL \
+ VISP_HAVE_PIONEER \
+ VISP_HAVE_PNG \
+ VISP_HAVE_PTHREAD \
+ VISP_HAVE_PTU46 \
+ VISP_HAVE_PYLON \
VISP_HAVE_QT \
- VISP_HAVE_SOQT \
+ VISP_HAVE_REALSENSE \
+ VISP_HAVE_REALSENSE_VERSION=0x020000 \
+ VISP_HAVE_REALSENSE2 \
VISP_HAVE_SOWIN \
+ VISP_HAVE_SOQT \
VISP_HAVE_SOXT \
- VISP_HAVE_DC1394 \
- VISP_HAVE_CMU1394 \
VISP_HAVE_V4L2 \
- VISP_HAVE_DIRECTSHOW \
- VISP_HAVE_COMEDI \
- VISP_HAVE_ATIDAQ \
- VISP_HAVE_AFMA4 \
- VISP_HAVE_AFMA6 \
- VISP_HAVE_AFMA6_DATA \
- VISP_HAVE_BICLOPS \
- VISP_HAVE_PTU46 \
VISP_HAVE_VIPER650 \
VISP_HAVE_VIPER650_DATA \
VISP_HAVE_VIPER850 \
VISP_HAVE_VIPER850_DATA \
- VISP_HAVE_PIONEER \
- VISP_HAVE_PARPORT \
+ VISP_HAVE_VIRTUOSE \
+ VISP_HAVE_X11 \
VISP_HAVE_XML2 \
- VISP_HAVE_JPEG \
- VISP_HAVE_PNG \
- VISP_HAVE_FFMPEG \
- VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES \
- VISP_HAVE_LIBFREENECT \
- VISP_HAVE_LIBUSB_1 \
- VISP_HAVE_REALSENSE \
VISP_HAVE_YARP \
VISP_HAVE_ZBAR \
- VISP_HAVE_DMTX \
- VISP_HAVE_FLYCAPTURE \
- VISP_HAVE_VIRTUOSE \
- VISP_HAVE_PCL \
- VISP_HAVE_CPP11_COMPATIBILITY \
VISP_HAVE_MODULE_AR \
VISP_HAVE_MODULE_CORE \
VISP_HAVE_MODULE_DETECTION \
VISP_HAVE_MODULE_GUI \
VISP_HAVE_MODULE_IO \
+ VISP_HAVE_MODULE_IMGPROC \
VISP_HAVE_MODULE_ROBOT \
VISP_HAVE_MODULE_SENSOR \
VISP_HAVE_MODULE_BLOB \
diff --git a/doc/image/image-data-structure.fig b/doc/image/cpp/image-data-structure.fig
similarity index 100%
rename from doc/image/image-data-structure.fig
rename to doc/image/cpp/image-data-structure.fig
diff --git a/doc/image/image-data-structure.gif b/doc/image/cpp/image-data-structure.gif
similarity index 100%
rename from doc/image/image-data-structure.gif
rename to doc/image/cpp/image-data-structure.gif
diff --git a/doc/image/image-data-structure.ps b/doc/image/cpp/image-data-structure.ps
similarity index 100%
rename from doc/image/image-data-structure.ps
rename to doc/image/cpp/image-data-structure.ps
diff --git a/doc/image/model-viper.png b/doc/image/cpp/model-viper.png
similarity index 100%
rename from doc/image/model-viper.png
rename to doc/image/cpp/model-viper.png
diff --git a/doc/image/pioneer-pan.fig b/doc/image/cpp/pioneer-pan.fig
similarity index 100%
rename from doc/image/pioneer-pan.fig
rename to doc/image/cpp/pioneer-pan.fig
diff --git a/doc/image/pioneer.fig b/doc/image/cpp/pioneer.fig
similarity index 100%
rename from doc/image/pioneer.fig
rename to doc/image/cpp/pioneer.fig
diff --git a/doc/image/cpp/soft_archi_v6.png b/doc/image/cpp/soft_archi_v6.png
new file mode 100644
index 0000000..a832e8b
Binary files /dev/null and b/doc/image/cpp/soft_archi_v6.png differ
diff --git a/doc/image/vpFeatureLine.fig b/doc/image/cpp/vpFeatureLine.fig
similarity index 100%
rename from doc/image/vpFeatureLine.fig
rename to doc/image/cpp/vpFeatureLine.fig
diff --git a/doc/image/vpFeatureLine.gif b/doc/image/cpp/vpFeatureLine.gif
similarity index 100%
rename from doc/image/vpFeatureLine.gif
rename to doc/image/cpp/vpFeatureLine.gif
diff --git a/doc/image/vpFeatureLine.ps b/doc/image/cpp/vpFeatureLine.ps
similarity index 100%
rename from doc/image/vpFeatureLine.ps
rename to doc/image/cpp/vpFeatureLine.ps
diff --git a/doc/image/vpImagePoint.fig b/doc/image/cpp/vpImagePoint.fig
similarity index 100%
rename from doc/image/vpImagePoint.fig
rename to doc/image/cpp/vpImagePoint.fig
diff --git a/doc/image/vpImagePoint.gif b/doc/image/cpp/vpImagePoint.gif
similarity index 100%
rename from doc/image/vpImagePoint.gif
rename to doc/image/cpp/vpImagePoint.gif
diff --git a/doc/image/vpImagePoint.ps b/doc/image/cpp/vpImagePoint.ps
similarity index 100%
rename from doc/image/vpImagePoint.ps
rename to doc/image/cpp/vpImagePoint.ps
diff --git a/doc/image/vpMeEllipse.fig b/doc/image/cpp/vpMeEllipse.fig
similarity index 100%
rename from doc/image/vpMeEllipse.fig
rename to doc/image/cpp/vpMeEllipse.fig
diff --git a/doc/image/vpMeEllipse.gif b/doc/image/cpp/vpMeEllipse.gif
similarity index 100%
rename from doc/image/vpMeEllipse.gif
rename to doc/image/cpp/vpMeEllipse.gif
diff --git a/doc/image/vpMeEllipse.ps b/doc/image/cpp/vpMeEllipse.ps
similarity index 100%
rename from doc/image/vpMeEllipse.ps
rename to doc/image/cpp/vpMeEllipse.ps
diff --git a/doc/image/vpMeEllipse2.fig b/doc/image/cpp/vpMeEllipse2.fig
similarity index 100%
rename from doc/image/vpMeEllipse2.fig
rename to doc/image/cpp/vpMeEllipse2.fig
diff --git a/doc/image/vpMeEllipse2.gif b/doc/image/cpp/vpMeEllipse2.gif
similarity index 100%
rename from doc/image/vpMeEllipse2.gif
rename to doc/image/cpp/vpMeEllipse2.gif
diff --git a/doc/image/vpMeEllipse2.ps b/doc/image/cpp/vpMeEllipse2.ps
similarity index 100%
rename from doc/image/vpMeEllipse2.ps
rename to doc/image/cpp/vpMeEllipse2.ps
diff --git a/doc/image/vpMeLine.fig b/doc/image/cpp/vpMeLine.fig
similarity index 100%
rename from doc/image/vpMeLine.fig
rename to doc/image/cpp/vpMeLine.fig
diff --git a/doc/image/vpMeLine.gif b/doc/image/cpp/vpMeLine.gif
similarity index 100%
rename from doc/image/vpMeLine.gif
rename to doc/image/cpp/vpMeLine.gif
diff --git a/doc/image/vpMeLine.ps b/doc/image/cpp/vpMeLine.ps
similarity index 100%
rename from doc/image/vpMeLine.ps
rename to doc/image/cpp/vpMeLine.ps
diff --git a/doc/image/vpWireFrameSimulator.fig b/doc/image/cpp/vpWireFrameSimulator.fig
similarity index 100%
rename from doc/image/vpWireFrameSimulator.fig
rename to doc/image/cpp/vpWireFrameSimulator.fig
diff --git a/doc/image/vpWireFrameSimulator.jpeg b/doc/image/cpp/vpWireFrameSimulator.jpeg
similarity index 100%
rename from doc/image/vpWireFrameSimulator.jpeg
rename to doc/image/cpp/vpWireFrameSimulator.jpeg
diff --git a/doc/image/vpWireFrameSimulator.ps b/doc/image/cpp/vpWireFrameSimulator.ps
similarity index 100%
rename from doc/image/vpWireFrameSimulator.ps
rename to doc/image/cpp/vpWireFrameSimulator.ps
diff --git a/doc/image/img-ccmake-centos-all.png b/doc/image/img-ccmake-centos-all.png
deleted file mode 100644
index 56a9a7d..0000000
Binary files a/doc/image/img-ccmake-centos-all.png and /dev/null differ
diff --git a/doc/image/img-ccmake-ubuntu-all.jpeg b/doc/image/img-ccmake-ubuntu-all.jpeg
deleted file mode 100644
index 2c420a3..0000000
Binary files a/doc/image/img-ccmake-ubuntu-all.jpeg and /dev/null differ
diff --git a/doc/image/img-cmake-debug-trace.jpg b/doc/image/img-cmake-debug-trace.jpg
deleted file mode 100644
index 1ef2ed2..0000000
Binary files a/doc/image/img-cmake-debug-trace.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win-opencv-advanced.jpg b/doc/image/img-cmake-win-opencv-advanced.jpg
deleted file mode 100755
index a0bd2f4..0000000
Binary files a/doc/image/img-cmake-win-opencv-advanced.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win-shared.jpg b/doc/image/img-cmake-win-shared.jpg
deleted file mode 100644
index 7a7b99e..0000000
Binary files a/doc/image/img-cmake-win-shared.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win10-config-end.png b/doc/image/img-cmake-win10-config-end.png
deleted file mode 100644
index bb7cd97..0000000
Binary files a/doc/image/img-cmake-win10-config-end.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-config.png b/doc/image/img-cmake-win10-config.png
deleted file mode 100644
index ba44f9b..0000000
Binary files a/doc/image/img-cmake-win10-config.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-create-build-folder.png b/doc/image/img-cmake-win10-create-build-folder.png
deleted file mode 100644
index 9c5cecf..0000000
Binary files a/doc/image/img-cmake-win10-create-build-folder.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-msvc-generate.png b/doc/image/img-cmake-win10-msvc-generate.png
deleted file mode 100644
index 5d99682..0000000
Binary files a/doc/image/img-cmake-win10-msvc-generate.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-msvc-launch.png b/doc/image/img-cmake-win10-msvc-launch.png
deleted file mode 100644
index ef8ec9d..0000000
Binary files a/doc/image/img-cmake-win10-msvc-launch.png and /dev/null differ
diff --git a/doc/image/img-cmake-win10-msvc-version.png b/doc/image/img-cmake-win10-msvc-version.png
deleted file mode 100644
index a56df1e..0000000
Binary files a/doc/image/img-cmake-win10-msvc-version.png and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-config-end.jpg b/doc/image/img-cmake-win8.1-config-end.jpg
deleted file mode 100644
index 0610b50..0000000
Binary files a/doc/image/img-cmake-win8.1-config-end.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-config.jpg b/doc/image/img-cmake-win8.1-config.jpg
deleted file mode 100644
index f50c36f..0000000
Binary files a/doc/image/img-cmake-win8.1-config.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-create-build-folder.jpg b/doc/image/img-cmake-win8.1-create-build-folder.jpg
deleted file mode 100644
index 2aa758b..0000000
Binary files a/doc/image/img-cmake-win8.1-create-build-folder.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-configure-end.jpg b/doc/image/img-cmake-win8.1-mingw-configure-end.jpg
deleted file mode 100644
index f6c649d..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-configure-end.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-configure.jpg b/doc/image/img-cmake-win8.1-mingw-configure.jpg
deleted file mode 100644
index 3f6826e..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-configure.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-generate.jpg b/doc/image/img-cmake-win8.1-mingw-generate.jpg
deleted file mode 100644
index eb658dd..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-generate.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-mingw-version.jpg b/doc/image/img-cmake-win8.1-mingw-version.jpg
deleted file mode 100644
index 1207901..0000000
Binary files a/doc/image/img-cmake-win8.1-mingw-version.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-msvc-generate.jpg b/doc/image/img-cmake-win8.1-msvc-generate.jpg
deleted file mode 100644
index 32819b9..0000000
Binary files a/doc/image/img-cmake-win8.1-msvc-generate.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-msvc-launch.jpg b/doc/image/img-cmake-win8.1-msvc-launch.jpg
deleted file mode 100644
index 966d638..0000000
Binary files a/doc/image/img-cmake-win8.1-msvc-launch.jpg and /dev/null differ
diff --git a/doc/image/img-cmake-win8.1-msvc-version.jpg b/doc/image/img-cmake-win8.1-msvc-version.jpg
deleted file mode 100644
index adb01ea..0000000
Binary files a/doc/image/img-cmake-win8.1-msvc-version.jpg and /dev/null differ
diff --git a/doc/image/img-configure-generator-iOS.png b/doc/image/img-configure-generator-iOS.png
deleted file mode 100644
index a4bc246..0000000
Binary files a/doc/image/img-configure-generator-iOS.png and /dev/null differ
diff --git a/doc/image/img-configure-options-iOS.png b/doc/image/img-configure-options-iOS.png
deleted file mode 100644
index ba79c7d..0000000
Binary files a/doc/image/img-configure-options-iOS.png and /dev/null differ
diff --git a/doc/image/img-configure-path-iOS.png b/doc/image/img-configure-path-iOS.png
deleted file mode 100644
index 0bbac08..0000000
Binary files a/doc/image/img-configure-path-iOS.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-code.jpeg b/doc/image/img-getting-started-iOS-code.jpeg
deleted file mode 100644
index d60a31c..0000000
Binary files a/doc/image/img-getting-started-iOS-code.jpeg and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-drag-drop.png b/doc/image/img-getting-started-iOS-drag-drop.png
deleted file mode 100644
index 63b25a0..0000000
Binary files a/doc/image/img-getting-started-iOS-drag-drop.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-link-visp.png b/doc/image/img-getting-started-iOS-link-visp.png
deleted file mode 100644
index d178ef1..0000000
Binary files a/doc/image/img-getting-started-iOS-link-visp.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-log.jpeg b/doc/image/img-getting-started-iOS-log.jpeg
deleted file mode 100644
index b427724..0000000
Binary files a/doc/image/img-getting-started-iOS-log.jpeg and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-navigator.png b/doc/image/img-getting-started-iOS-navigator.png
deleted file mode 100644
index 823b588..0000000
Binary files a/doc/image/img-getting-started-iOS-navigator.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-new.png b/doc/image/img-getting-started-iOS-new.png
deleted file mode 100644
index 6aef8d8..0000000
Binary files a/doc/image/img-getting-started-iOS-new.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-options.png b/doc/image/img-getting-started-iOS-options.png
deleted file mode 100644
index 25feafb..0000000
Binary files a/doc/image/img-getting-started-iOS-options.png and /dev/null differ
diff --git a/doc/image/img-getting-started-iOS-rename.png b/doc/image/img-getting-started-iOS-rename.png
deleted file mode 100644
index aa0a0bc..0000000
Binary files a/doc/image/img-getting-started-iOS-rename.png and /dev/null differ
diff --git a/doc/image/img-started-imgproc-ios-output-canny.jpeg b/doc/image/img-started-imgproc-ios-output-canny.jpeg
deleted file mode 100644
index bef94da..0000000
Binary files a/doc/image/img-started-imgproc-ios-output-canny.jpeg and /dev/null differ
diff --git a/doc/image/img-started-imgproc-ios-output-color.jpeg b/doc/image/img-started-imgproc-ios-output-color.jpeg
deleted file mode 100644
index 3089502..0000000
Binary files a/doc/image/img-started-imgproc-ios-output-color.jpeg and /dev/null differ
diff --git a/doc/image/img-started-imgproc-ios-output-gradient.jpeg b/doc/image/img-started-imgproc-ios-output-gradient.jpeg
deleted file mode 100644
index 24abef6..0000000
Binary files a/doc/image/img-started-imgproc-ios-output-gradient.jpeg and /dev/null differ
diff --git a/doc/image/img-started-imgproc-ios-output-gray.jpeg b/doc/image/img-started-imgproc-ios-output-gray.jpeg
deleted file mode 100644
index 76406c3..0000000
Binary files a/doc/image/img-started-imgproc-ios-output-gray.jpeg and /dev/null differ
diff --git a/doc/image/img-started-imgproc-ios.jpeg b/doc/image/img-started-imgproc-ios.jpeg
deleted file mode 100644
index 0e35ce9..0000000
Binary files a/doc/image/img-started-imgproc-ios.jpeg and /dev/null differ
diff --git a/doc/image/img-win10-explorer-install-end.png b/doc/image/img-win10-explorer-install-end.png
deleted file mode 100644
index 93164ce..0000000
Binary files a/doc/image/img-win10-explorer-install-end.png and /dev/null differ
diff --git a/doc/image/img-win10-msvc-build-end.png b/doc/image/img-win10-msvc-build-end.png
deleted file mode 100644
index 860d4cc..0000000
Binary files a/doc/image/img-win10-msvc-build-end.png and /dev/null differ
diff --git a/doc/image/img-win10-msvc-build.jpeg b/doc/image/img-win10-msvc-build.jpeg
deleted file mode 100644
index 04d779f..0000000
Binary files a/doc/image/img-win10-msvc-build.jpeg and /dev/null differ
diff --git a/doc/image/img-win10-msvc-install-end.png b/doc/image/img-win10-msvc-install-end.png
deleted file mode 100644
index 1e188be..0000000
Binary files a/doc/image/img-win10-msvc-install-end.png and /dev/null differ
diff --git a/doc/image/img-win10-msvc-install.jpeg b/doc/image/img-win10-msvc-install.jpeg
deleted file mode 100644
index b92f0ab..0000000
Binary files a/doc/image/img-win10-msvc-install.jpeg and /dev/null differ
diff --git a/doc/image/img-win10-msvc-open.png b/doc/image/img-win10-msvc-open.png
deleted file mode 100644
index 7c096c1..0000000
Binary files a/doc/image/img-win10-msvc-open.png and /dev/null differ
diff --git a/doc/image/img-win10-msvc-release.jpeg b/doc/image/img-win10-msvc-release.jpeg
deleted file mode 100644
index 7d74032..0000000
Binary files a/doc/image/img-win10-msvc-release.jpeg and /dev/null differ
diff --git a/doc/image/img-win8.1-cmake-ogre.jpg b/doc/image/img-win8.1-cmake-ogre.jpg
deleted file mode 100644
index eebb3f4..0000000
Binary files a/doc/image/img-win8.1-cmake-ogre.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-cmake-ogredeps.jpg b/doc/image/img-win8.1-cmake-ogredeps.jpg
deleted file mode 100644
index ec6a8fe..0000000
Binary files a/doc/image/img-win8.1-cmake-ogredeps.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-explorer-install-end.jpg b/doc/image/img-win8.1-explorer-install-end.jpg
deleted file mode 100644
index 7133f83..0000000
Binary files a/doc/image/img-win8.1-explorer-install-end.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg b/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg
deleted file mode 100644
index 9868b95..0000000
Binary files a/doc/image/img-win8.1-mingw-opencv-cmake-configure-install.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg b/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg
deleted file mode 100644
index 2aacbe5..0000000
Binary files a/doc/image/img-win8.1-mingw-opencv-cmake-configure.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-build-end.jpg b/doc/image/img-win8.1-msvc-build-end.jpg
deleted file mode 100644
index 7601401..0000000
Binary files a/doc/image/img-win8.1-msvc-build-end.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-build-ogredeps.jpg b/doc/image/img-win8.1-msvc-build-ogredeps.jpg
deleted file mode 100644
index cf3e409..0000000
Binary files a/doc/image/img-win8.1-msvc-build-ogredeps.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-build.jpg b/doc/image/img-win8.1-msvc-build.jpg
deleted file mode 100644
index 398b848..0000000
Binary files a/doc/image/img-win8.1-msvc-build.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-install-end.jpg b/doc/image/img-win8.1-msvc-install-end.jpg
deleted file mode 100644
index 1363c1b..0000000
Binary files a/doc/image/img-win8.1-msvc-install-end.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-install.jpg b/doc/image/img-win8.1-msvc-install.jpg
deleted file mode 100644
index 44a8961..0000000
Binary files a/doc/image/img-win8.1-msvc-install.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-open.jpg b/doc/image/img-win8.1-msvc-open.jpg
deleted file mode 100644
index 282605d..0000000
Binary files a/doc/image/img-win8.1-msvc-open.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-release.jpg b/doc/image/img-win8.1-msvc-release.jpg
deleted file mode 100644
index bd737a6..0000000
Binary files a/doc/image/img-win8.1-msvc-release.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-msvc-solution.jpg b/doc/image/img-win8.1-msvc-solution.jpg
deleted file mode 100644
index ad854c3..0000000
Binary files a/doc/image/img-win8.1-msvc-solution.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-ogre-explorer.jpg b/doc/image/img-win8.1-ogre-explorer.jpg
deleted file mode 100644
index efbf4ba..0000000
Binary files a/doc/image/img-win8.1-ogre-explorer.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-ogre-tortoisehg.jpg b/doc/image/img-win8.1-ogre-tortoisehg.jpg
deleted file mode 100644
index 6a5d82b..0000000
Binary files a/doc/image/img-win8.1-ogre-tortoisehg.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-ogredeps-explorer.jpg b/doc/image/img-win8.1-ogredeps-explorer.jpg
deleted file mode 100644
index c77d60d..0000000
Binary files a/doc/image/img-win8.1-ogredeps-explorer.jpg and /dev/null differ
diff --git a/doc/image/img-win8.1-ogredeps-tortoisehg.jpg b/doc/image/img-win8.1-ogredeps-tortoisehg.jpg
deleted file mode 100644
index e18955a..0000000
Binary files a/doc/image/img-win8.1-ogredeps-tortoisehg.jpg and /dev/null differ
diff --git a/doc/image/img-logo-visp.png b/doc/image/logo/img-logo-visp.png
similarity index 100%
rename from doc/image/img-logo-visp.png
rename to doc/image/logo/img-logo-visp.png
diff --git a/doc/image/img-chessboard-01.png b/doc/image/tutorial/calibration/img-chessboard-01.png
similarity index 100%
rename from doc/image/img-chessboard-01.png
rename to doc/image/tutorial/calibration/img-chessboard-01.png
diff --git a/doc/image/img-chessboard-undistort.png b/doc/image/tutorial/calibration/img-chessboard-undistort.png
similarity index 100%
rename from doc/image/img-chessboard-undistort.png
rename to doc/image/tutorial/calibration/img-chessboard-undistort.png
diff --git a/doc/image/img-circles-grid-02.png b/doc/image/tutorial/calibration/img-circles-grid-02.png
similarity index 100%
rename from doc/image/img-circles-grid-02.png
rename to doc/image/tutorial/calibration/img-circles-grid-02.png
diff --git a/doc/image/tutorial/detection/img-apriltag-image.png b/doc/image/tutorial/detection/img-apriltag-image.png
new file mode 100644
index 0000000..e43861f
Binary files /dev/null and b/doc/image/tutorial/detection/img-apriltag-image.png differ
diff --git a/doc/image/tutorial/detection/img-apriltag-pose.png b/doc/image/tutorial/detection/img-apriltag-pose.png
new file mode 100644
index 0000000..a80e77b
Binary files /dev/null and b/doc/image/tutorial/detection/img-apriltag-pose.png differ
diff --git a/doc/image/img-detection-datamatrix.png b/doc/image/tutorial/detection/img-detection-datamatrix.png
similarity index 100%
rename from doc/image/img-detection-datamatrix.png
rename to doc/image/tutorial/detection/img-detection-datamatrix.png
diff --git a/doc/image/img-detection-qrcode.png b/doc/image/tutorial/detection/img-detection-qrcode.png
similarity index 100%
rename from doc/image/img-detection-qrcode.png
rename to doc/image/tutorial/detection/img-detection-qrcode.png
diff --git a/doc/image/img-detection-step.jpeg b/doc/image/tutorial/detection/img-detection-step.jpeg
similarity index 100%
rename from doc/image/img-detection-step.jpeg
rename to doc/image/tutorial/detection/img-detection-step.jpeg
diff --git a/doc/image/img-learning-step.jpeg b/doc/image/tutorial/detection/img-learning-step.jpeg
similarity index 100%
rename from doc/image/img-learning-step.jpeg
rename to doc/image/tutorial/detection/img-learning-step.jpeg
diff --git a/doc/image/img-monkey-blured-default.png b/doc/image/tutorial/image/img-monkey-blured-default.png
similarity index 100%
rename from doc/image/img-monkey-blured-default.png
rename to doc/image/tutorial/image/img-monkey-blured-default.png
diff --git a/doc/image/img-monkey-blured-var2.png b/doc/image/tutorial/image/img-monkey-blured-var2.png
similarity index 100%
rename from doc/image/img-monkey-blured-var2.png
rename to doc/image/tutorial/image/img-monkey-blured-var2.png
diff --git a/doc/image/img-monkey-canny.png b/doc/image/tutorial/image/img-monkey-canny.png
similarity index 100%
rename from doc/image/img-monkey-canny.png
rename to doc/image/tutorial/image/img-monkey-canny.png
diff --git a/doc/image/img-monkey-dIxy.png b/doc/image/tutorial/image/img-monkey-dIxy.png
similarity index 100%
rename from doc/image/img-monkey-dIxy.png
rename to doc/image/tutorial/image/img-monkey-dIxy.png
diff --git a/doc/image/img-monkey-gray.png b/doc/image/tutorial/image/img-monkey-gray.png
similarity index 100%
rename from doc/image/img-monkey-gray.png
rename to doc/image/tutorial/image/img-monkey-gray.png
diff --git a/doc/image/img-monkey-pyr.png b/doc/image/tutorial/image/img-monkey-pyr.png
similarity index 100%
rename from doc/image/img-monkey-pyr.png
rename to doc/image/tutorial/image/img-monkey-pyr.png
diff --git a/doc/image/img-monkey-sobel.png b/doc/image/tutorial/image/img-monkey-sobel.png
similarity index 100%
rename from doc/image/img-monkey-sobel.png
rename to doc/image/tutorial/image/img-monkey-sobel.png
diff --git a/doc/image/img-simu-image-target-square.png b/doc/image/tutorial/image/img-simu-image-target-square.png
similarity index 100%
rename from doc/image/img-simu-image-target-square.png
rename to doc/image/tutorial/image/img-simu-image-target-square.png
diff --git a/doc/image/img-target-square.png b/doc/image/tutorial/image/img-target-square.png
similarity index 100%
rename from doc/image/img-target-square.png
rename to doc/image/tutorial/image/img-target-square.png
diff --git a/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03-otsu.png b/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03-otsu.png
new file mode 100644
index 0000000..e15f73d
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03-otsu.png differ
diff --git a/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03.png b/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03.png
new file mode 100644
index 0000000..77769e3
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-auto-threshold-grid36-03.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-autothreshold.png b/doc/image/tutorial/imgproc/img-tutorial-autothreshold.png
new file mode 100644
index 0000000..0fa1c48
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-autothreshold.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-adjust-alpha10-beta50.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-adjust-alpha10-beta50.png
new file mode 100644
index 0000000..0200050
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-adjust-alpha10-beta50.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-3.5.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-3.5.png
new file mode 100644
index 0000000..50ed49f
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-3.5.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-plot.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-plot.png
new file mode 100644
index 0000000..45fb679
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-gamma-correction-plot.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative.png
new file mode 100644
index 0000000..566eed9
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative2.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative2.png
new file mode 100644
index 0000000..9a83865
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq-cumulative2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq.png
new file mode 100644
index 0000000..889c5ab
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-hist-eq.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-brighness-retinex-dynamic-3.png b/doc/image/tutorial/imgproc/img-tutorial-brighness-retinex-dynamic-3.png
new file mode 100644
index 0000000..00ced52
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-brighness-retinex-dynamic-3.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-connected-components-img.png b/doc/image/tutorial/imgproc/img-tutorial-connected-components-img.png
new file mode 100644
index 0000000..70ce645
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-connected-components-img.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-connected-components-labeling.png b/doc/image/tutorial/imgproc/img-tutorial-connected-components-labeling.png
new file mode 100644
index 0000000..57300ed
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-connected-components-labeling.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation.png b/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation.png
new file mode 100644
index 0000000..d07ff13
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation2.png b/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation2.png
new file mode 100644
index 0000000..f59364a
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contour-binarisation2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours.png b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours.png
new file mode 100644
index 0000000..4bf665d
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours2.png b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours2.png
new file mode 100644
index 0000000..3b2f0d6
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours3.png b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours3.png
new file mode 100644
index 0000000..14478a4
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contour-draw-contours3.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE-hist.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE-hist.png
new file mode 100644
index 0000000..70bc580
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE-hist.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE.png
new file mode 100644
index 0000000..365a881
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-CLAHE.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast-hist.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast-hist.png
new file mode 100644
index 0000000..90bc34e
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast-hist.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast.png
new file mode 100644
index 0000000..8bdb51f
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-Crayfish-low-contrast.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization-hist.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization-hist.png
new file mode 100644
index 0000000..fc9f672
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization-hist.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization.png
new file mode 100644
index 0000000..0602da7
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-equalization.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV-hist.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV-hist.png
new file mode 100644
index 0000000..8f1a19a
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV-hist.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV.png
new file mode 100644
index 0000000..372a7ed
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-HSV.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-hist.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-hist.png
new file mode 100644
index 0000000..9df461c
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching-hist.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching.png
new file mode 100644
index 0000000..9291291
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-histogram-stretching.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-unsharp-masking.png b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-unsharp-masking.png
new file mode 100644
index 0000000..3595315
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-contrast-sharpening-unsharp-masking.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation1.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation1.png
new file mode 100644
index 0000000..00a9de6
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation1.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation2.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation2.png
new file mode 100644
index 0000000..6c9abd4
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-binarisation2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-close1.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-close1.png
new file mode 100644
index 0000000..7eab9b6
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-close1.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins1.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins1.png
new file mode 100644
index 0000000..8db9442
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins1.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins2.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins2.png
new file mode 100644
index 0000000..b53635e
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-coins2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins1.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins1.png
new file mode 100644
index 0000000..4ff6f88
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins1.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins2.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins2.png
new file mode 100644
index 0000000..cbccdae
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-count-coins2.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill-holes.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill-holes.png
new file mode 100644
index 0000000..76fdb87
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill-holes.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill1.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill1.png
new file mode 100644
index 0000000..0608fa5
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-fill1.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-mask.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-mask.png
new file mode 100644
index 0000000..45380ab
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-mask.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-count-coins-white-holes.png b/doc/image/tutorial/imgproc/img-tutorial-count-coins-white-holes.png
new file mode 100644
index 0000000..9daffb6
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-count-coins-white-holes.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-flood-fill-4-connexity.png b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-4-connexity.png
new file mode 100644
index 0000000..bf9887a
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-4-connexity.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-flood-fill-8-connexity.png b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-8-connexity.png
new file mode 100644
index 0000000..23ba417
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-8-connexity.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-flood-fill-bucket-fill.png b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-bucket-fill.png
new file mode 100644
index 0000000..a405a36
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-bucket-fill.png differ
diff --git a/doc/image/tutorial/imgproc/img-tutorial-flood-fill-draw-polygons.png b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-draw-polygons.png
new file mode 100644
index 0000000..adc7aee
Binary files /dev/null and b/doc/image/tutorial/imgproc/img-tutorial-flood-fill-draw-polygons.png differ
diff --git a/doc/image/tutorial/ios/img-detection-apriltag-ios-drag-drop.png b/doc/image/tutorial/ios/img-detection-apriltag-ios-drag-drop.png
new file mode 100644
index 0000000..49f3eab
Binary files /dev/null and b/doc/image/tutorial/ios/img-detection-apriltag-ios-drag-drop.png differ
diff --git a/doc/image/tutorial/ios/img-detection-apriltag-ios-output.png b/doc/image/tutorial/ios/img-detection-apriltag-ios-output.png
new file mode 100644
index 0000000..277ef5f
Binary files /dev/null and b/doc/image/tutorial/ios/img-detection-apriltag-ios-output.png differ
diff --git a/doc/image/tutorial/ios/img-detection-apriltag-ios-xcode.png b/doc/image/tutorial/ios/img-detection-apriltag-ios-xcode.png
new file mode 100644
index 0000000..2080b61
Binary files /dev/null and b/doc/image/tutorial/ios/img-detection-apriltag-ios-xcode.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-code.jpeg b/doc/image/tutorial/ios/img-getting-started-iOS-code.jpeg
new file mode 100644
index 0000000..ec0c26a
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-code.jpeg differ
diff --git a/doc/image/img-getting-started-iOS-create.png b/doc/image/tutorial/ios/img-getting-started-iOS-create.png
similarity index 100%
rename from doc/image/img-getting-started-iOS-create.png
rename to doc/image/tutorial/ios/img-getting-started-iOS-create.png
diff --git a/doc/image/img-getting-started-iOS-drag-drop-dialog.png b/doc/image/tutorial/ios/img-getting-started-iOS-drag-drop-dialog.png
similarity index 100%
rename from doc/image/img-getting-started-iOS-drag-drop-dialog.png
rename to doc/image/tutorial/ios/img-getting-started-iOS-drag-drop-dialog.png
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-drag-drop.png b/doc/image/tutorial/ios/img-getting-started-iOS-drag-drop.png
new file mode 100644
index 0000000..a957538
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-drag-drop.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-link-visp.png b/doc/image/tutorial/ios/img-getting-started-iOS-link-visp.png
new file mode 100644
index 0000000..4ad1223
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-link-visp.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-log.jpeg b/doc/image/tutorial/ios/img-getting-started-iOS-log.jpeg
new file mode 100644
index 0000000..9d4da66
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-log.jpeg differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-navigator.png b/doc/image/tutorial/ios/img-getting-started-iOS-navigator.png
new file mode 100644
index 0000000..ff4d21d
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-navigator.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-new.png b/doc/image/tutorial/ios/img-getting-started-iOS-new.png
new file mode 100644
index 0000000..07756ec
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-new.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-options.png b/doc/image/tutorial/ios/img-getting-started-iOS-options.png
new file mode 100644
index 0000000..c96836d
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-options.png differ
diff --git a/doc/image/tutorial/ios/img-getting-started-iOS-rename.png b/doc/image/tutorial/ios/img-getting-started-iOS-rename.png
new file mode 100644
index 0000000..36c7c58
Binary files /dev/null and b/doc/image/tutorial/ios/img-getting-started-iOS-rename.png differ
diff --git a/doc/image/img-started-imgproc-ios-drag-drop-dialog.png b/doc/image/tutorial/ios/img-started-imgproc-ios-drag-drop-dialog.png
similarity index 100%
rename from doc/image/img-started-imgproc-ios-drag-drop-dialog.png
rename to doc/image/tutorial/ios/img-started-imgproc-ios-drag-drop-dialog.png
diff --git a/doc/image/img-started-imgproc-ios-drag-drop.jpeg b/doc/image/tutorial/ios/img-started-imgproc-ios-drag-drop.jpeg
similarity index 100%
rename from doc/image/img-started-imgproc-ios-drag-drop.jpeg
rename to doc/image/tutorial/ios/img-started-imgproc-ios-drag-drop.jpeg
diff --git a/doc/image/tutorial/ios/img-started-imgproc-ios-output-canny.png b/doc/image/tutorial/ios/img-started-imgproc-ios-output-canny.png
new file mode 100644
index 0000000..f38891f
Binary files /dev/null and b/doc/image/tutorial/ios/img-started-imgproc-ios-output-canny.png differ
diff --git a/doc/image/tutorial/ios/img-started-imgproc-ios-output-color.jpeg b/doc/image/tutorial/ios/img-started-imgproc-ios-output-color.jpeg
new file mode 100644
index 0000000..3b28418
Binary files /dev/null and b/doc/image/tutorial/ios/img-started-imgproc-ios-output-color.jpeg differ
diff --git a/doc/image/tutorial/ios/img-started-imgproc-ios-output-gradient.png b/doc/image/tutorial/ios/img-started-imgproc-ios-output-gradient.png
new file mode 100644
index 0000000..05bb23b
Binary files /dev/null and b/doc/image/tutorial/ios/img-started-imgproc-ios-output-gradient.png differ
diff --git a/doc/image/tutorial/ios/img-started-imgproc-ios-output-gray.png b/doc/image/tutorial/ios/img-started-imgproc-ios-output-gray.png
new file mode 100644
index 0000000..28f0e88
Binary files /dev/null and b/doc/image/tutorial/ios/img-started-imgproc-ios-output-gray.png differ
diff --git a/doc/image/tutorial/ios/img-started-imgproc-ios.jpeg b/doc/image/tutorial/ios/img-started-imgproc-ios.jpeg
new file mode 100644
index 0000000..6d79b79
Binary files /dev/null and b/doc/image/tutorial/ios/img-started-imgproc-ios.jpeg differ
diff --git a/doc/image/tutorial/misc/img-cmake-debug-trace.jpg b/doc/image/tutorial/misc/img-cmake-debug-trace.jpg
new file mode 100644
index 0000000..40efc6f
Binary files /dev/null and b/doc/image/tutorial/misc/img-cmake-debug-trace.jpg differ
diff --git a/doc/image/tutorial/started/img-contrib-module-1.png b/doc/image/tutorial/started/img-contrib-module-1.png
new file mode 100644
index 0000000..6a760e0
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-1.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-2.png b/doc/image/tutorial/started/img-contrib-module-2.png
new file mode 100644
index 0000000..95a7a51
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-2.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-3.png b/doc/image/tutorial/started/img-contrib-module-3.png
new file mode 100644
index 0000000..4fc4074
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-3.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-4.png b/doc/image/tutorial/started/img-contrib-module-4.png
new file mode 100644
index 0000000..dabb7c4
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-4.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-5.png b/doc/image/tutorial/started/img-contrib-module-5.png
new file mode 100644
index 0000000..2ec65b9
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-5.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-6.png b/doc/image/tutorial/started/img-contrib-module-6.png
new file mode 100644
index 0000000..c42e469
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-6.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-7.png b/doc/image/tutorial/started/img-contrib-module-7.png
new file mode 100644
index 0000000..c42e469
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-7.png differ
diff --git a/doc/image/tutorial/started/img-contrib-module-tree.jpg b/doc/image/tutorial/started/img-contrib-module-tree.jpg
new file mode 100644
index 0000000..3983684
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module-tree.jpg differ
diff --git a/doc/image/tutorial/started/img-contrib-module.jpg b/doc/image/tutorial/started/img-contrib-module.jpg
new file mode 100644
index 0000000..7977bb7
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-module.jpg differ
diff --git a/doc/image/tutorial/started/img-contrib-visp-module.jpg b/doc/image/tutorial/started/img-contrib-visp-module.jpg
new file mode 100644
index 0000000..dae9ad4
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib-visp-module.jpg differ
diff --git a/doc/image/tutorial/started/img-contrib.pptx b/doc/image/tutorial/started/img-contrib.pptx
new file mode 100644
index 0000000..68d9106
Binary files /dev/null and b/doc/image/tutorial/started/img-contrib.pptx differ
diff --git a/doc/image/img-monkey-win.jpg b/doc/image/tutorial/started/img-monkey-win.jpg
similarity index 100%
rename from doc/image/img-monkey-win.jpg
rename to doc/image/tutorial/started/img-monkey-win.jpg
diff --git a/doc/image/img-monkey.png b/doc/image/tutorial/started/img-monkey.png
similarity index 100%
rename from doc/image/img-monkey.png
rename to doc/image/tutorial/started/img-monkey.png
diff --git a/doc/image/img-started-win-cmake-1.jpg b/doc/image/tutorial/started/img-started-win-cmake-1.jpg
similarity index 100%
rename from doc/image/img-started-win-cmake-1.jpg
rename to doc/image/tutorial/started/img-started-win-cmake-1.jpg
diff --git a/doc/image/img-started-win-cmake-2.jpg b/doc/image/tutorial/started/img-started-win-cmake-2.jpg
similarity index 100%
rename from doc/image/img-started-win-cmake-2.jpg
rename to doc/image/tutorial/started/img-started-win-cmake-2.jpg
diff --git a/doc/image/img-started-win-cmake-3.jpg b/doc/image/tutorial/started/img-started-win-cmake-3.jpg
similarity index 100%
rename from doc/image/img-started-win-cmake-3.jpg
rename to doc/image/tutorial/started/img-started-win-cmake-3.jpg
diff --git a/doc/image/img-started-win-msvc-1.jpg b/doc/image/tutorial/started/img-started-win-msvc-1.jpg
similarity index 100%
rename from doc/image/img-started-win-msvc-1.jpg
rename to doc/image/tutorial/started/img-started-win-msvc-1.jpg
diff --git a/doc/image/img-started-win-msvc-2.jpg b/doc/image/tutorial/started/img-started-win-msvc-2.jpg
similarity index 100%
rename from doc/image/img-started-win-msvc-2.jpg
rename to doc/image/tutorial/started/img-started-win-msvc-2.jpg
diff --git a/doc/image/img-started-win-run.jpg b/doc/image/tutorial/started/img-started-win-run.jpg
similarity index 100%
rename from doc/image/img-started-win-run.jpg
rename to doc/image/tutorial/started/img-started-win-run.jpg
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-10.png b/doc/image/tutorial/started/img-visp-into-eclipse-10.png
new file mode 100644
index 0000000..49220cf
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-10.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-11.png b/doc/image/tutorial/started/img-visp-into-eclipse-11.png
new file mode 100644
index 0000000..e95561c
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-11.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-12.png b/doc/image/tutorial/started/img-visp-into-eclipse-12.png
new file mode 100644
index 0000000..76a14f3
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-12.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-4.png b/doc/image/tutorial/started/img-visp-into-eclipse-4.png
new file mode 100644
index 0000000..e7ff78b
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-4.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-5.png b/doc/image/tutorial/started/img-visp-into-eclipse-5.png
new file mode 100644
index 0000000..124034c
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-5.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-6.png b/doc/image/tutorial/started/img-visp-into-eclipse-6.png
new file mode 100644
index 0000000..1c2d313
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-6.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-7.png b/doc/image/tutorial/started/img-visp-into-eclipse-7.png
new file mode 100644
index 0000000..2043266
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-7.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-8.png b/doc/image/tutorial/started/img-visp-into-eclipse-8.png
new file mode 100644
index 0000000..99aa077
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-8.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-9.png b/doc/image/tutorial/started/img-visp-into-eclipse-9.png
new file mode 100644
index 0000000..ad8fd98
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-9.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-arg-1.png b/doc/image/tutorial/started/img-visp-into-eclipse-arg-1.png
new file mode 100644
index 0000000..915816d
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-arg-1.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-arg-2.png b/doc/image/tutorial/started/img-visp-into-eclipse-arg-2.png
new file mode 100644
index 0000000..b51e128
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-arg-2.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-unr-1.png b/doc/image/tutorial/started/img-visp-into-eclipse-unr-1.png
new file mode 100644
index 0000000..31d9144
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-unr-1.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-unr-2.png b/doc/image/tutorial/started/img-visp-into-eclipse-unr-2.png
new file mode 100644
index 0000000..14d411c
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-unr-2.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-unr-3.png b/doc/image/tutorial/started/img-visp-into-eclipse-unr-3.png
new file mode 100644
index 0000000..f6f67cb
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-unr-3.png differ
diff --git a/doc/image/tutorial/started/img-visp-into-eclipse-unr-4.png b/doc/image/tutorial/started/img-visp-into-eclipse-unr-4.png
new file mode 100644
index 0000000..672bf5e
Binary files /dev/null and b/doc/image/tutorial/started/img-visp-into-eclipse-unr-4.png differ
diff --git a/doc/image/img-blob-auto-detection.png b/doc/image/tutorial/tracking/img-blob-auto-detection.png
similarity index 100%
rename from doc/image/img-blob-auto-detection.png
rename to doc/image/tutorial/tracking/img-blob-auto-detection.png
diff --git a/doc/image/img-circle.png b/doc/image/tutorial/tracking/img-circle.png
similarity index 100%
rename from doc/image/img-circle.png
rename to doc/image/tutorial/tracking/img-circle.png
diff --git a/doc/image/img-cylinder.png b/doc/image/tutorial/tracking/img-cylinder.png
similarity index 100%
rename from doc/image/img-cylinder.png
rename to doc/image/tutorial/tracking/img-cylinder.png
diff --git a/doc/image/img-fov.png b/doc/image/tutorial/tracking/img-fov.png
similarity index 100%
rename from doc/image/img-fov.png
rename to doc/image/tutorial/tracking/img-fov.png
diff --git a/doc/image/img-initClickTemplateTracker.png b/doc/image/tutorial/tracking/img-initClickTemplateTracker.png
similarity index 100%
rename from doc/image/img-initClickTemplateTracker.png
rename to doc/image/tutorial/tracking/img-initClickTemplateTracker.png
diff --git a/doc/image/img-mbt-multi-class-diagram-resize.jpeg b/doc/image/tutorial/tracking/img-mbt-multi-class-diagram-resize.jpeg
similarity index 100%
rename from doc/image/img-mbt-multi-class-diagram-resize.jpeg
rename to doc/image/tutorial/tracking/img-mbt-multi-class-diagram-resize.jpeg
diff --git a/doc/image/img-multi-cameras-config.png b/doc/image/tutorial/tracking/img-multi-cameras-config.png
similarity index 100%
rename from doc/image/img-multi-cameras-config.png
rename to doc/image/tutorial/tracking/img-multi-cameras-config.png
diff --git a/doc/image/img-plane-hierarchical-diagram.jpg b/doc/image/tutorial/tracking/img-plane-hierarchical-diagram.jpg
similarity index 100%
rename from doc/image/img-plane-hierarchical-diagram.jpg
rename to doc/image/tutorial/tracking/img-plane-hierarchical-diagram.jpg
diff --git a/doc/image/img-teabox-cao-triangle.jpg b/doc/image/tutorial/tracking/img-teabox-cao-triangle.jpg
similarity index 100%
rename from doc/image/img-teabox-cao-triangle.jpg
rename to doc/image/tutorial/tracking/img-teabox-cao-triangle.jpg
diff --git a/doc/image/img-teabox-cao.jpg b/doc/image/tutorial/tracking/img-teabox-cao.jpg
similarity index 100%
rename from doc/image/img-teabox-cao.jpg
rename to doc/image/tutorial/tracking/img-teabox-cao.jpg
diff --git a/doc/image/img-teabox-click.jpg b/doc/image/tutorial/tracking/img-teabox-click.jpg
similarity index 100%
rename from doc/image/img-teabox-click.jpg
rename to doc/image/tutorial/tracking/img-teabox-click.jpg
diff --git a/doc/image/img-template-tracker.jpg b/doc/image/tutorial/tracking/img-template-tracker.jpg
similarity index 100%
rename from doc/image/img-template-tracker.jpg
rename to doc/image/tutorial/tracking/img-template-tracker.jpg
diff --git a/doc/image/img-tracker-mb-visibility-ogre-advanced.png b/doc/image/tutorial/tracking/img-tracker-mb-visibility-ogre-advanced.png
similarity index 100%
rename from doc/image/img-tracker-mb-visibility-ogre-advanced.png
rename to doc/image/tutorial/tracking/img-tracker-mb-visibility-ogre-advanced.png
diff --git a/doc/image/img-tracker-mb-visibility-ogre.png b/doc/image/tutorial/tracking/img-tracker-mb-visibility-ogre.png
similarity index 100%
rename from doc/image/img-tracker-mb-visibility-ogre.png
rename to doc/image/tutorial/tracking/img-tracker-mb-visibility-ogre.png
diff --git a/doc/image/img-tracker-mb-visibility-scanline.png b/doc/image/tutorial/tracking/img-tracker-mb-visibility-scanline.png
similarity index 100%
rename from doc/image/img-tracker-mb-visibility-scanline.png
rename to doc/image/tutorial/tracking/img-tracker-mb-visibility-scanline.png
diff --git a/doc/image/img-tracker-mb-visibility.jpg b/doc/image/tutorial/tracking/img-tracker-mb-visibility.jpg
similarity index 100%
rename from doc/image/img-tracker-mb-visibility.jpg
rename to doc/image/tutorial/tracking/img-tracker-mb-visibility.jpg
diff --git a/doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg b/doc/image/tutorial/tracking/img-win8.1-msvc-mbtracker-ogre-issue.jpg
similarity index 100%
rename from doc/image/img-win8.1-msvc-mbtracker-ogre-issue.jpg
rename to doc/image/tutorial/tracking/img-win8.1-msvc-mbtracker-ogre-issue.jpg
diff --git a/doc/image/img-win8.1-msvc-mbtracker-ogre-opengl.jpg b/doc/image/tutorial/tracking/img-win8.1-msvc-mbtracker-ogre-opengl.jpg
similarity index 100%
rename from doc/image/img-win8.1-msvc-mbtracker-ogre-opengl.jpg
rename to doc/image/tutorial/tracking/img-win8.1-msvc-mbtracker-ogre-opengl.jpg
diff --git a/doc/image/tutorial/tracking/tutorial-tracking-mb-generic-depth-map.png b/doc/image/tutorial/tracking/tutorial-tracking-mb-generic-depth-map.png
new file mode 100644
index 0000000..09d51eb
Binary files /dev/null and b/doc/image/tutorial/tracking/tutorial-tracking-mb-generic-depth-map.png differ
diff --git a/doc/image/tutorial/unix/img-ccmake-centos-all.jpg b/doc/image/tutorial/unix/img-ccmake-centos-all.jpg
new file mode 100644
index 0000000..f7c3bdd
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-centos-all.jpg differ
diff --git a/doc/image/tutorial/unix/img-ccmake-fedora-all.jpeg b/doc/image/tutorial/unix/img-ccmake-fedora-all.jpeg
new file mode 100644
index 0000000..28209a5
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-fedora-all.jpeg differ
diff --git a/doc/image/tutorial/unix/img-ccmake-opensuse-all.png b/doc/image/tutorial/unix/img-ccmake-opensuse-all.png
new file mode 100644
index 0000000..6b962df
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-opensuse-all.png differ
diff --git a/doc/image/tutorial/unix/img-ccmake-osx-all.jpg b/doc/image/tutorial/unix/img-ccmake-osx-all.jpg
new file mode 100644
index 0000000..168b768
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-osx-all.jpg differ
diff --git a/doc/image/tutorial/unix/img-ccmake-raspberry-all.png b/doc/image/tutorial/unix/img-ccmake-raspberry-all.png
new file mode 100644
index 0000000..0abc10a
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-raspberry-all.png differ
diff --git a/doc/image/tutorial/unix/img-ccmake-ubuntu-all.jpeg b/doc/image/tutorial/unix/img-ccmake-ubuntu-all.jpeg
new file mode 100644
index 0000000..a529d1a
Binary files /dev/null and b/doc/image/tutorial/unix/img-ccmake-ubuntu-all.jpeg differ
diff --git a/doc/image/img-raspberry-pi.jpg b/doc/image/tutorial/unix/img-raspberry-pi.jpg
similarity index 100%
rename from doc/image/img-raspberry-pi.jpg
rename to doc/image/tutorial/unix/img-raspberry-pi.jpg
diff --git a/doc/image/img-pose-qrcode.jpg b/doc/image/tutorial/vision/img-pose-qrcode.jpg
similarity index 100%
rename from doc/image/img-pose-qrcode.jpg
rename to doc/image/tutorial/vision/img-pose-qrcode.jpg
diff --git a/doc/image/img-pose-square.jpg b/doc/image/tutorial/vision/img-pose-square.jpg
similarity index 100%
rename from doc/image/img-pose-square.jpg
rename to doc/image/tutorial/vision/img-pose-square.jpg
diff --git a/doc/image/img-ibvs-control-law-adaptive.png b/doc/image/tutorial/visual-servo/img-ibvs-control-law-adaptive.png
similarity index 100%
rename from doc/image/img-ibvs-control-law-adaptive.png
rename to doc/image/tutorial/visual-servo/img-ibvs-control-law-adaptive.png
diff --git a/doc/image/img-ibvs-control-law-continuous-adaptive.png b/doc/image/tutorial/visual-servo/img-ibvs-control-law-continuous-adaptive.png
similarity index 100%
rename from doc/image/img-ibvs-control-law-continuous-adaptive.png
rename to doc/image/tutorial/visual-servo/img-ibvs-control-law-continuous-adaptive.png
diff --git a/doc/image/img-ibvs-control-law-exponential.png b/doc/image/tutorial/visual-servo/img-ibvs-control-law-exponential.png
similarity index 100%
rename from doc/image/img-ibvs-control-law-exponential.png
rename to doc/image/tutorial/visual-servo/img-ibvs-control-law-exponential.png
diff --git a/doc/image/img-ibvs-plotter.jpg b/doc/image/tutorial/visual-servo/img-ibvs-plotter.jpg
similarity index 100%
rename from doc/image/img-ibvs-plotter.jpg
rename to doc/image/tutorial/visual-servo/img-ibvs-plotter.jpg
diff --git a/doc/image/pioneer-pan.png b/doc/image/tutorial/visual-servo/pioneer-pan.png
similarity index 100%
rename from doc/image/pioneer-pan.png
rename to doc/image/tutorial/visual-servo/pioneer-pan.png
diff --git a/doc/image/pioneer.png b/doc/image/tutorial/visual-servo/pioneer.png
similarity index 100%
rename from doc/image/pioneer.png
rename to doc/image/tutorial/visual-servo/pioneer.png
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-makefiles.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-makefiles.png
new file mode 100644
index 0000000..c7aa285
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-makefiles.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-install.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-install.png
new file mode 100644
index 0000000..bd39164
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-issue-precompiled-headers.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-issue-precompiled-headers.png
new file mode 100644
index 0000000..37c1e57
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-issue-precompiled-headers.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-launch.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-launch.png
new file mode 100644
index 0000000..af91f9f
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-opencv-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config.png
new file mode 100644
index 0000000..1a44794
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config2.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config2.png
new file mode 100644
index 0000000..0d79697
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-config2.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-create-folder.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-create-folder.png
new file mode 100644
index 0000000..aafb489
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-create-folder.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-generate.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-generate.png
new file mode 100644
index 0000000..a0222f1
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-generate.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-launch.png b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-launch.png
new file mode 100644
index 0000000..353527e
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-mingw-visp-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-msvc-version.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-msvc-version.png
new file mode 100644
index 0000000..bf2741f
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-msvc-version.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogre.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogre.png
new file mode 100644
index 0000000..9dae604
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogre.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogredeps.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogredeps.png
new file mode 100644
index 0000000..616d01e
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-ogredeps.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-config.png
new file mode 100644
index 0000000..e072d9d
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-install.png
new file mode 100644
index 0000000..5337037
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-realsense-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config.png
new file mode 100644
index 0000000..f4e0847
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config2.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config2.png
new file mode 100644
index 0000000..61ac94b
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-config2.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-create-folder.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-create-folder.png
new file mode 100644
index 0000000..56c44d0
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-create-folder.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-generate.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-generate.png
new file mode 100644
index 0000000..8392484
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-generate.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-launch.png
new file mode 100644
index 0000000..b082e8b
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc12-visp-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-install.png
new file mode 100644
index 0000000..aa6a36f
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-launch.png
new file mode 100644
index 0000000..86fc8dc
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-eigen-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-msvc-version.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-msvc-version.png
new file mode 100644
index 0000000..6ec00d2
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-msvc-version.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-config.png
new file mode 100644
index 0000000..737074a
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-install.png
new file mode 100644
index 0000000..a0a104b
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-realsense-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config.png
new file mode 100644
index 0000000..d1357cc
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config2.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config2.png
new file mode 100644
index 0000000..9c42255
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-config2.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-create-folder.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-create-folder.png
new file mode 100644
index 0000000..61752d5
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-create-folder.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-generate.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-generate.png
new file mode 100644
index 0000000..05de47f
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-generate.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-launch.png
new file mode 100644
index 0000000..76df2dd
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc14-visp-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-install.png
new file mode 100644
index 0000000..79f832e
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-launch.png
new file mode 100644
index 0000000..de8b7db
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-eigen-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-msvc-version.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-msvc-version.png
new file mode 100644
index 0000000..7259397
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-msvc-version.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-folder.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-folder.png
new file mode 100644
index 0000000..554a1c9
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-folder.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-generate.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-generate.png
new file mode 100644
index 0000000..2117fc1
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-generate.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-install.png
new file mode 100644
index 0000000..e4abbcd
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-launch.png
new file mode 100644
index 0000000..6cb3d4e
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-opencv-launch.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-config.png
new file mode 100644
index 0000000..efd5e2c
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-install.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-install.png
new file mode 100644
index 0000000..da477af
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-realsense-install.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config.png
new file mode 100644
index 0000000..aec7887
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config2.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config2.png
new file mode 100644
index 0000000..4f63233
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-config2.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-create-folder.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-create-folder.png
new file mode 100644
index 0000000..9f8190b
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-create-folder.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-generate.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-generate.png
new file mode 100644
index 0000000..f65f1bc
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-generate.png differ
diff --git a/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-launch.png b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-launch.png
new file mode 100644
index 0000000..0d31880
Binary files /dev/null and b/doc/image/tutorial/windows/img-cmake-win10-msvc15-visp-launch.png differ
diff --git a/doc/image/img-cmake-win7-create-build-folder.jpg b/doc/image/tutorial/windows/img-cmake-win7-create-build-folder.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-create-build-folder.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-create-build-folder.jpg
diff --git a/doc/image/img-cmake-win7-generate.jpg b/doc/image/tutorial/windows/img-cmake-win7-generate.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-generate.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-generate.jpg
diff --git a/doc/image/img-cmake-win7-msvc-config-end.jpg b/doc/image/tutorial/windows/img-cmake-win7-msvc-config-end.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-msvc-config-end.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-msvc-config-end.jpg
diff --git a/doc/image/img-cmake-win7-msvc-config.jpg b/doc/image/tutorial/windows/img-cmake-win7-msvc-config.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-msvc-config.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-msvc-config.jpg
diff --git a/doc/image/img-cmake-win7-msvc-launch.jpg b/doc/image/tutorial/windows/img-cmake-win7-msvc-launch.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-msvc-launch.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-msvc-launch.jpg
diff --git a/doc/image/img-cmake-win7-msvc-version.jpg b/doc/image/tutorial/windows/img-cmake-win7-msvc-version.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-msvc-version.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-msvc-version.jpg
diff --git a/doc/image/img-cmake-win7-solution.jpg b/doc/image/tutorial/windows/img-cmake-win7-solution.jpg
similarity index 100%
rename from doc/image/img-cmake-win7-solution.jpg
rename to doc/image/tutorial/windows/img-cmake-win7-solution.jpg
diff --git a/doc/image/img-mingw64-installer-process.jpg b/doc/image/tutorial/windows/img-mingw64-installer-process.jpg
similarity index 100%
rename from doc/image/img-mingw64-installer-process.jpg
rename to doc/image/tutorial/windows/img-mingw64-installer-process.jpg
diff --git a/doc/image/img-mingw64-installer-started.jpg b/doc/image/tutorial/windows/img-mingw64-installer-started.jpg
similarity index 100%
rename from doc/image/img-mingw64-installer-started.jpg
rename to doc/image/tutorial/windows/img-mingw64-installer-started.jpg
diff --git a/doc/image/img-opencv-issue-ipp.jpg b/doc/image/tutorial/windows/img-opencv-issue-ipp.jpg
similarity index 100%
rename from doc/image/img-opencv-issue-ipp.jpg
rename to doc/image/tutorial/windows/img-opencv-issue-ipp.jpg
diff --git a/doc/image/img-opencv-issue-test-big-endian.jpg b/doc/image/tutorial/windows/img-opencv-issue-test-big-endian.jpg
similarity index 100%
rename from doc/image/img-opencv-issue-test-big-endian.jpg
rename to doc/image/tutorial/windows/img-opencv-issue-test-big-endian.jpg
diff --git a/doc/image/img-opencv-issue-tiff.jpg b/doc/image/tutorial/windows/img-opencv-issue-tiff.jpg
similarity index 100%
rename from doc/image/img-opencv-issue-tiff.jpg
rename to doc/image/tutorial/windows/img-opencv-issue-tiff.jpg
diff --git a/doc/image/img-uwp-msvc-modify.png b/doc/image/tutorial/windows/img-uwp-msvc-modify.png
similarity index 100%
rename from doc/image/img-uwp-msvc-modify.png
rename to doc/image/tutorial/windows/img-uwp-msvc-modify.png
diff --git a/doc/image/img-uwp-msvc-modify2.png b/doc/image/tutorial/windows/img-uwp-msvc-modify2.png
similarity index 100%
rename from doc/image/img-uwp-msvc-modify2.png
rename to doc/image/tutorial/windows/img-uwp-msvc-modify2.png
diff --git a/doc/image/img-uwp-msvc-modify3.png b/doc/image/tutorial/windows/img-uwp-msvc-modify3.png
similarity index 100%
rename from doc/image/img-uwp-msvc-modify3.png
rename to doc/image/tutorial/windows/img-uwp-msvc-modify3.png
diff --git a/doc/image/img-win10-cmake-error-sdk-missing.png b/doc/image/tutorial/windows/img-win10-cmake-error-sdk-missing.png
similarity index 100%
rename from doc/image/img-win10-cmake-error-sdk-missing.png
rename to doc/image/tutorial/windows/img-win10-cmake-error-sdk-missing.png
diff --git a/doc/image/tutorial/windows/img-win10-extract-opencv.png b/doc/image/tutorial/windows/img-win10-extract-opencv.png
new file mode 100644
index 0000000..18f039e
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-extract-opencv.png differ
diff --git a/doc/image/tutorial/windows/img-win10-mingw-cmd-displayGDI.png b/doc/image/tutorial/windows/img-win10-mingw-cmd-displayGDI.png
new file mode 100644
index 0000000..b590e21
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-mingw-cmd-displayGDI.png differ
diff --git a/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-cap-dshow.png b/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-cap-dshow.png
new file mode 100644
index 0000000..e102311
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-cap-dshow.png differ
diff --git a/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-precompiled-headers.png b/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-precompiled-headers.png
new file mode 100644
index 0000000..a249300
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-mingw-opencv-issue-precompiled-headers.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-build-ogre-install-release.png b/doc/image/tutorial/windows/img-win10-msvc12-build-ogre-install-release.png
new file mode 100644
index 0000000..0fe1512
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-build-ogre-install-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps-install-release.png b/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps-install-release.png
new file mode 100644
index 0000000..6827084
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps-install-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps.png b/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps.png
new file mode 100644
index 0000000..8da6f4a
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-build-ogredeps.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-realsense-build.png b/doc/image/tutorial/windows/img-win10-msvc12-realsense-build.png
new file mode 100644
index 0000000..624daed
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-realsense-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-build-end.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-build-end.png
new file mode 100644
index 0000000..b61bc3a
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-build-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-explorer-install.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-explorer-install.png
new file mode 100644
index 0000000..ca32470
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-explorer-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-install-end.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-install-end.png
new file mode 100644
index 0000000..e2fa3ed
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-install-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-install.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-install.png
new file mode 100644
index 0000000..d3f5806
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-open.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-open.png
new file mode 100644
index 0000000..4544a86
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-open.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc12-visp-release.png b/doc/image/tutorial/windows/img-win10-msvc12-visp-release.png
new file mode 100644
index 0000000..c2d495d
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc12-visp-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-cmd-displayGDI.png b/doc/image/tutorial/windows/img-win10-msvc14-cmd-displayGDI.png
new file mode 100644
index 0000000..09f874d
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-cmd-displayGDI.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-eigen-build-install.png b/doc/image/tutorial/windows/img-win10-msvc14-eigen-build-install.png
new file mode 100644
index 0000000..f65f392
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-eigen-build-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-realsense-build.png b/doc/image/tutorial/windows/img-win10-msvc14-realsense-build.png
new file mode 100644
index 0000000..f1fa2d8
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-realsense-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-build-end.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-build-end.png
new file mode 100644
index 0000000..0bf43d9
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-build-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-build.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-build.png
new file mode 100644
index 0000000..9635171
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-explorer-install.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-explorer-install.png
new file mode 100644
index 0000000..a6912a6
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-explorer-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-install-end.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-install-end.png
new file mode 100644
index 0000000..59106b1
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-install-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-install.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-install.png
new file mode 100644
index 0000000..69f7f2d
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-open.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-open.png
new file mode 100644
index 0000000..1733474
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-open.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc14-visp-release.png b/doc/image/tutorial/windows/img-win10-msvc14-visp-release.png
new file mode 100644
index 0000000..6ec65cb
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc14-visp-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-debug.png b/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-debug.png
new file mode 100644
index 0000000..c7404f7
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-debug.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-release.png b/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-release.png
new file mode 100644
index 0000000..aba9ab0
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-build-opencv-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-cmd-displayGDI.png b/doc/image/tutorial/windows/img-win10-msvc15-cmd-displayGDI.png
new file mode 100644
index 0000000..f78cee6
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-cmd-displayGDI.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-eigen-build-install.png b/doc/image/tutorial/windows/img-win10-msvc15-eigen-build-install.png
new file mode 100644
index 0000000..f21fd29
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-eigen-build-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-realsense-build.png b/doc/image/tutorial/windows/img-win10-msvc15-realsense-build.png
new file mode 100644
index 0000000..bb53369
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-realsense-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-build-end.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-build-end.png
new file mode 100644
index 0000000..b0fdbce
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-build-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-build.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-build.png
new file mode 100644
index 0000000..b4983f6
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-explorer-install.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-explorer-install.png
new file mode 100644
index 0000000..2b7b335
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-explorer-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-install-end.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-install-end.png
new file mode 100644
index 0000000..6eb6d3f
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-install-end.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-install.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-install.png
new file mode 100644
index 0000000..29f487b
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-install.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-open.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-open.png
new file mode 100644
index 0000000..6b2eabf
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-open.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-visp-release.png b/doc/image/tutorial/windows/img-win10-msvc15-visp-release.png
new file mode 100644
index 0000000..10bbd80
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-visp-release.png differ
diff --git a/doc/image/tutorial/windows/img-win10-msvc15-zbar-build.png b/doc/image/tutorial/windows/img-win10-msvc15-zbar-build.png
new file mode 100644
index 0000000..b55a1b2
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-msvc15-zbar-build.png differ
diff --git a/doc/image/tutorial/windows/img-win10-ogre-explorer.png b/doc/image/tutorial/windows/img-win10-ogre-explorer.png
new file mode 100644
index 0000000..d85ab50
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-ogre-explorer.png differ
diff --git a/doc/image/tutorial/windows/img-win10-ogre-tortoisehg.png b/doc/image/tutorial/windows/img-win10-ogre-tortoisehg.png
new file mode 100644
index 0000000..93ee7e4
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-ogre-tortoisehg.png differ
diff --git a/doc/image/tutorial/windows/img-win10-ogredeps-tortoisehg.png b/doc/image/tutorial/windows/img-win10-ogredeps-tortoisehg.png
new file mode 100644
index 0000000..e62648d
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-ogredeps-tortoisehg.png differ
diff --git a/doc/image/tutorial/windows/img-win10-pylon-developer.png b/doc/image/tutorial/windows/img-win10-pylon-developer.png
new file mode 100644
index 0000000..bf1f3fc
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-pylon-developer.png differ
diff --git a/doc/image/tutorial/windows/img-win10-visp-images.png b/doc/image/tutorial/windows/img-win10-visp-images.png
new file mode 100644
index 0000000..23eaaaf
Binary files /dev/null and b/doc/image/tutorial/windows/img-win10-visp-images.png differ
diff --git a/doc/image/img-win7-ViSP-images.jpg b/doc/image/tutorial/windows/img-win7-ViSP-images.jpg
similarity index 100%
rename from doc/image/img-win7-ViSP-images.jpg
rename to doc/image/tutorial/windows/img-win7-ViSP-images.jpg
diff --git a/doc/image/img-win7-cmd-displayGDI.jpg b/doc/image/tutorial/windows/img-win7-cmd-displayGDI.jpg
similarity index 100%
rename from doc/image/img-win7-cmd-displayGDI.jpg
rename to doc/image/tutorial/windows/img-win7-cmd-displayGDI.jpg
diff --git a/doc/image/img-win7-cygwin-git.jpg b/doc/image/tutorial/windows/img-win7-cygwin-git.jpg
similarity index 100%
rename from doc/image/img-win7-cygwin-git.jpg
rename to doc/image/tutorial/windows/img-win7-cygwin-git.jpg
diff --git a/doc/image/img-win7-msvc-build-succeed.jpg b/doc/image/tutorial/windows/img-win7-msvc-build-succeed.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-build-succeed.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-build-succeed.jpg
diff --git a/doc/image/img-win7-msvc-build.jpg b/doc/image/tutorial/windows/img-win7-msvc-build.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-build.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-build.jpg
diff --git a/doc/image/img-win7-msvc-install-end.jpg b/doc/image/tutorial/windows/img-win7-msvc-install-end.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-install-end.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-install-end.jpg
diff --git a/doc/image/img-win7-msvc-install-succeed.jpg b/doc/image/tutorial/windows/img-win7-msvc-install-succeed.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-install-succeed.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-install-succeed.jpg
diff --git a/doc/image/img-win7-msvc-install.jpg b/doc/image/tutorial/windows/img-win7-msvc-install.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-install.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-install.jpg
diff --git a/doc/image/img-win7-msvc-open.jpg b/doc/image/tutorial/windows/img-win7-msvc-open.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-open.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-open.jpg
diff --git a/doc/image/img-win7-msvc-release.jpg b/doc/image/tutorial/windows/img-win7-msvc-release.jpg
similarity index 100%
rename from doc/image/img-win7-msvc-release.jpg
rename to doc/image/tutorial/windows/img-win7-msvc-release.jpg
diff --git a/doc/image/img-win8.1-cmake-ogredeps-issue.jpg b/doc/image/tutorial/windows/img-win8.1-cmake-ogredeps-issue.jpg
similarity index 100%
rename from doc/image/img-win8.1-cmake-ogredeps-issue.jpg
rename to doc/image/tutorial/windows/img-win8.1-cmake-ogredeps-issue.jpg
diff --git a/doc/image/img-win8.1-cmd-displayGDI.jpg b/doc/image/tutorial/windows/img-win8.1-cmd-displayGDI.jpg
similarity index 100%
rename from doc/image/img-win8.1-cmd-displayGDI.jpg
rename to doc/image/tutorial/windows/img-win8.1-cmd-displayGDI.jpg
diff --git a/doc/image/img-win8.1-mingw-cmd-displayGDI.jpg b/doc/image/tutorial/windows/img-win8.1-mingw-cmd-displayGDI.jpg
similarity index 100%
rename from doc/image/img-win8.1-mingw-cmd-displayGDI.jpg
rename to doc/image/tutorial/windows/img-win8.1-mingw-cmd-displayGDI.jpg
diff --git a/doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg b/doc/image/tutorial/windows/img-win8.1-mingw-opencv-issue-dtor.jpg
similarity index 100%
rename from doc/image/img-win8.1-mingw-opencv-issue-dtor.jpg
rename to doc/image/tutorial/windows/img-win8.1-mingw-opencv-issue-dtor.jpg
diff --git a/doc/image/img-win8.1-msvc-ogredeps-issue.jpg b/doc/image/tutorial/windows/img-win8.1-msvc-ogredeps-issue.jpg
similarity index 100%
rename from doc/image/img-win8.1-msvc-ogredeps-issue.jpg
rename to doc/image/tutorial/windows/img-win8.1-msvc-ogredeps-issue.jpg
diff --git a/doc/image/img-win8.1-visp-images.jpg b/doc/image/tutorial/windows/img-win8.1-visp-images.jpg
similarity index 100%
rename from doc/image/img-win8.1-visp-images.jpg
rename to doc/image/tutorial/windows/img-win8.1-visp-images.jpg
diff --git a/doc/mainpage.doc.in b/doc/mainpage.doc.in
index 3343adf..678b5a8 100644
--- a/doc/mainpage.doc.in
+++ b/doc/mainpage.doc.in
@@ -1,4 +1,4 @@
-/*!
+/*!
\mainpage ViSP @VISP_VERSION@ main page
@@ -15,13 +15,19 @@ href="http://www.inria.fr/en/centre/rennes" target="_parent">Inria Rennes</a>.</
<p>ViSP wiki is <a href="https://github.com/lagadic/visp/wiki" target="_parent">https://github.com/lagadic/visp/wiki</a></p>
-<p>ViSP source code is now available on GitHub <a href="https://github.com/lagadic/visp" target="_parent">https://github.com/lagadic/visp</a></p>
-
-<p>If you have any problems or find any bugs, please report them in
-<a href="https://github.com/lagadic/visp/issues">the bug tracker</a>. If you may need help, please use the available <a href="http://gforge.inria.fr/forum/?group_id=397" target="_parent">forums</a>.<p>
+<p>ViSP source code available on GitHub <a href="https://github.com/lagadic/visp" target="_parent">https://github.com/lagadic/visp</a></p>
+
+<p>If you have any problems or find any bugs, please report them in
+<a href="https://github.com/lagadic/visp/issues">the bug tracker</a>. If you may need help, please use the available <a href="http://gforge.inria.fr/forum/?group_id=397" target="_parent">forums</a>.<p>
It is also possible to contact ViSP main developers using: <a href="mailto:visp at inria.fr?subject=[visp]">visp at inria.fr</a>
+\section arch_sec Software architecture
+
+Since ViSP 3.0.0, we design a new modular software architecture where ViSP capabilities are grouped in several modules (core, io, gui, vision, …). In ViSP 3.1.0 we introduced a new module called imgproc. As a result, the user will find several shared or static libraries, one for each module. The following figure highlights the module dependencies and the third-party libraries that may be used by each module. The central module is the core module. All other modules depend on core.
+
+\image html soft_archi_v6.png "ViSP modules and their optional dependencies"
+
\section download_sec Download
<p>From <a
@@ -42,18 +48,25 @@ href="https://visp.inria.fr/software-architecture" target="_parent">https://visp
<p>ViSP full installation procedure using CMake is detailed in the \ref tuto_install_sdk "Installation from prebuild SDK tutorials" and \ref tuto_install_src "Installation from source tutorials". \ref tuto_started "Getting started tutorials" and documents in pdf are also available from <a
href="https://visp.inria.fr/publications" target="_parent">https://visp.inria.fr/publications</a>.</p>
-
+
\section tutorial Tutorials
<p>Here after you will find a list of tutorials that show the basic use of <a href="classes.html">ViSP classes</a> with a small first code.
+\subsection tuto_ios ViSP iOS
+
+- \ref tutorial-install-ios-package <br>In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
+- \ref tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
+- \ref tutorial-getting-started-iOS <br>This tutorial shows how to build a project that uses ViSP on iOS devices.
+- \ref tutorial-image-ios <br>This tutorial shows how to do simple image processing on iOS devices.
+- \ref tutorial-detection-apriltag-ios <br>This tutorial shows how to do detct an AprilTag marker on iOS devices.
+
\subsection tuto_install_sdk Installation from prebuild SDK
- \ref tutorial-install-ubuntu-package <br>In this first tutorial you will learn how to install ViSP prebuilt SDK from Ubuntu or Debian packages.
- \ref tutorial-install-archlinux-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for Arch Linux.
- \ref tutorial-install-osx-homebrew-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK on OSX with Homebrew.
- \ref tutorial-install-ros-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for ROS.
-- \ref tutorial-install-ios-package <br>In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
\subsection tuto_install_src Installation from source
@@ -62,13 +75,15 @@ href="https://visp.inria.fr/publications" target="_parent">https://visp.inria.fr
- \ref tutorial-install-centos <br>In this other tutorial you will learn how to install ViSP from source on Linux CentOS.
- \ref tutorial-install-opensuse <br>In this other tutorial you will learn how to install ViSP from source on Linux openSUSE.
- \ref tutorial-install-raspberry <br>In this tutorial you will learn how to install ViSP from source on Raspberry Pi.
-- \ref tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012.
-- \ref tutorial-install-win81-msvc <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++ 2013.
-- \ref tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015.
-- \ref tutorial-install-win10-msvc14-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2015.
-- \ref tutorial-install-win81-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 or Windows 10 with Mingw-w64.
+- \ref tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012 (vc11).
+- \ref tutorial-install-win10-msvc12 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2013 (vc12).
+- \ref tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015 (vc14).
+- \ref tutorial-install-win10-msvc15 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2017 (vc15).
+- \ref tutorial-install-win10-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 or Windows 10 with Mingw-w64.
+- \ref tutorial-install-win10-msvc14-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2015 (vc14).
+- \ref tutorial-install-win10-msvc15-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2017 (vc15).
- \ref tutorial-install-osx-homebrew <br>In this tutorial you will learn how to install ViSP from source on OSX with Homebrew.
-- \ref tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
+
\subsection tuto_install_crosscompiling Installation from cross-compiling
@@ -78,16 +93,27 @@ href="https://visp.inria.fr/publications" target="_parent">https://visp.inria.fr
\subsection tuto_started Getting started
- \ref tutorial-getting-started <br>This tutorial shows how to build a project that uses ViSP to read and display an image.
-- \ref tutorial-getting-started-iOS <br>This tutorial shows how to build a project that uses ViSP on iOS devices.
- \ref tutorial-getting-started-naoqi <br>This tutorial shows how to build a project that uses ViSP on NAOqi OS running on NAO, Romeo or Pepper humanoid robots.
+- \ref tutorial-import-visp-into-EclipseIDE <br>This tutorial shows how to import a CMake project that uses ViSP in Eclipse IDE.
+- \ref tutorial-contrib-module <br>This tutorial explains how to exploit ViSP modular architecture in order to introduce a new contrib module.
\subsection tuto_image Image manipulation
- \ref tutorial-image-display <br>In this tutorial you will learn how to display an image in a window and how to down scale the window size to be able to display images that are larger than the screen resolution.
- \ref tutorial-grabber <br>This tutorial shows how to acquire images from a camera.
- \ref tutorial-image-filtering <br>This tutorial shows how to filter an image with ViSP.
-- \ref tutorial-simu-image <br>This tutorial shows how to project the image of a planar scene to a given camera position.
-- \ref tutorial-image-ios <br>This tutorial shows how to do simple image processing on iOS devices.
+- \ref tutorial-simu-image <br>This tutorial shows how to project the image of a planar scene to a given camera position.
+
+\subsection tuto_imgproc Image processing
+
+- \ref tutorial-imgproc-brightness <br>This tutorial will show you some simple techniques to adjust or improve the brightness / contrast of an image.
+- \ref tutorial-imgproc-contrast-sharpening <br>This tutorial will show you some simple techniques to improve the contrast and the sharpness of an image.
+- \ref tutorial-imgproc-autothreshold <br>This tutorial will show you different methods to automatically threshold, binarize an image.
+- \ref tutorial-imgproc-contour <br>This tutorial will show you how to extract the contours from a binary image.
+- \ref tutorial-imgproc-connected-components <br>This tutorial will show you how to do a connected-components labeling.
+- \ref tutorial-imgproc-flood-fill <br>This tutorial will show you how to use the flood fill algorithm.
+- \ref tutorial-imgproc-count-coins <br>This tutorial will show you how to count the number of coins in an image.
+
\subsection tuto_calib Camera calibration
@@ -97,18 +123,20 @@ href="https://visp.inria.fr/publications" target="_parent">https://visp.inria.fr
- \ref tutorial-tracking-blob <br>This tutorial introduces blob tracking and detection.
- \ref tutorial-tracking-keypoint <br>This tutorial focuses on keypoint tracking using Kanade-Lucas-Tomasi feature tracker.
-- \ref tutorial-tracking-me <br>This tutorial focuses on line and ellipse tracking using moving-edges.
+- \ref tutorial-tracking-me <br>This tutorial focuses on line and ellipse tracking using moving-edges.
- \ref tutorial-tracking-mb <br>This tutorial focuses on model-based trackers using either edges, keypoints or and hybrid scheme that uses edges and keypoints.
- \ref tutorial-tracking-mb-stereo <br>This tutorial focuses on model-based trackers with stereo cameras.
+- \ref tutorial-tracking-mb-generic <br>This tutorial focuses on generic model-based trackers.
- \ref tutorial-tracking-tt <br>This tutorial focuses on template trackers based on image registration approaches.
-\subsection tuto_detection Detection
+\subsection tuto_detection Detection
- \ref tutorial-matching <br>This tutorial shows how to detect and match keypoints.
- \ref tutorial-matching-deprecated <br>This tutorial shows how to detect and match SURF keypoints. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
- \ref tutorial-detection-barcode <br>This tutorial focuses on bar code (QR code, Data Matrix code) detection.
- \ref tutorial-detection-face <br>This tutorial focuses on face detection thanks to OpenCV Haar cascade classifier.
- \ref tutorial-detection-object <br>This tutorial shows how to learn keypoints detected on a known object and how to use the matched correspondences to detect and estimate the pose of the object.
+- \ref tutorial-detection-apriltag <br>This tutorial focuses on AprilTag patterns detection and pose estimation.
\subsection tuto_computer_vision Computer vision
@@ -139,7 +167,7 @@ in one thread and display these images in an other thread.
\section Examples_sec Using ViSP
<p>ViSP C++ classes are organized in <a href="modules.html">modules</a> that may help the user during his project implementation.</p>
-
+
<p>From the <a href="examples.html">example</a> page, you will also find
examples showing how to use the library to acquire and display an
image, compute a camera pose, estimate an homography, servo a real
@@ -155,7 +183,7 @@ scheme, ...</p>
- You can write new tutorials, new documentations or simply improve the existing documentation.
- If you just want to say you've been happy with the library, you can send us a postcard from your place, to the following address: <em>Inria Rennes Bretagne Atlantique, Lagadic team, Campus de Beaulieu, 35042 Rennes Cedex, FRANCE.</em>
-<p>You can also ask for help using either <a href="https://gforge.inria.fr/forum/?group_id=397">the french or the english forum</a>.
+<p>You can also ask for help using either <a href="https://gforge.inria.fr/forum/?group_id=397">the french or the english forum</a>.
*/
@@ -388,6 +416,11 @@ scheme, ...</p>
\defgroup group_detection_face Face detection
Face detection.
*/
+/*!
+ \ingroup module_detection
+ \defgroup group_detection_tag Tag detection
+ Tag detection.
+*/
/*******************************************
* Module tracker
@@ -409,8 +442,8 @@ scheme, ...</p>
\ingroup module_mbt
\defgroup group_mbt_trackers Trackers
Model-based trackers. Two group of classes may be used:
- - the one that allows to track an object on a single image: vpMbEdgeTracker, vpMbKltTracker, vpMbEdgeKltTracker;
- - the one that allows to track an object seen in two (stereo) or more images: vpMbEdgeMultiTracker, vpMbKltMultiTracker, vpMbEdgeKltMultiTracker.
+ - the one that allows to track an object on a single image: vpMbGenericTracker (recommended), vpMbEdgeTracker, vpMbKltTracker, vpMbEdgeKltTracker;
+ - the one that allows to track an object seen in two (stereo) or more images: vpMbGenericTracker (recommended), vpMbEdgeMultiTracker, vpMbKltMultiTracker, vpMbEdgeKltMultiTracker.
*/
/*!
\ingroup module_mbt
@@ -432,7 +465,7 @@ scheme, ...</p>
*******************************************/
/*!
\defgroup module_robot robot: Robot module
- Robot module content.
+ Robot module content.
*/
/*!
\ingroup module_robot
@@ -517,17 +550,17 @@ scheme, ...</p>
\defgroup group_sensor_laserscanner Laser scanner
Laser scanner interfaces.
*/
-/*!
+/*!
\ingroup module_sensor
\defgroup group_sensor_camera Camera
Camera interfaces.
*/
-/*!
+/*!
\ingroup module_sensor
\defgroup group_sensor_rgbd RGB-D sensor
RGB-D sensor interfaces.
*/
-/*!
+/*!
\ingroup module_sensor
\defgroup group_sensor_ft Force/Torque sensor
Force/Torque sensor interfaces.
@@ -562,7 +595,7 @@ scheme, ...</p>
\ingroup module_tracker
\defgroup module_tt_mi tt_mi: Template tracker based on mutual information module
Template tracker based on mutual information module content. This module is an
- extension of \ref module_tt where mutual information cost function is introduced.
+ extension of \ref module_tt where mutual information cost function is introduced.
*/
/*!
\ingroup module_tt_mi
diff --git a/doc/tutorial-boost-vs.doc b/doc/tutorial-boost-vs.doc
deleted file mode 100644
index 5ae2366..0000000
--- a/doc/tutorial-boost-vs.doc
+++ /dev/null
@@ -1,104 +0,0 @@
-/**
-
-\page tutorial-boost-vs Tutorial: How to boost your visual servo control law
-\tableofcontents
-
-\section intro_boost_vs Introduction
-
-This tutorial gives some hints to boost your visual servo control law in order to speed up the time to convergence.
-
-Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/visual-servoing/ibvs
-\endcode
-
-To illustrate this tutorial let us consider the example tutorial-ibvs-4pts-plotter.cpp introduced in \ref tutorial-ibvs. This example consider an image based visual servoing using four points as visual features.
-
-In the general case, considering \f$ \dot {\bf q} \f$ as the input velocities to the robot controller, the control laws provided in vpServo class lead to the following control law \f$ \dot {\bf q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e}\f$ where the sign is negative for an eye in hand servo and positive for an eye to hand servo, \f$\lambda\f$ is a constant gain, \f$ {\bf \widehat J}_e\f$ is the task Jacobian and \f$\bf e \f$ is the error to regulate to zero. As described in \cite Ch [...]
-
-This behavior is illustrated with the next figure, where we see the exponential decrease of the eight visual features (x and y for each point) and the corresponding six velocities that are applied to the robot controller. As a consequence, velocities are high when the error is important, and very low when the error is small near the convergence. At the beginning, we can also notice velocity discontinuities with velocities varying from zero to high values in one iteration.
-
-\image html img-ibvs-control-law-exponential.png "Convergence in 191 iterations with a constant gain."
-
-This behavior can be reproduced running tutorial-ibvs-4pts-plotter.cpp example. Here after we recall the important lines of code used to compute the control law:
-
-\code
- vpServo task ;
- task.setServo(vpServo::EYEINHAND_CAMERA);
- task.setInteractionMatrixType(vpServo::CURRENT);
- task.setLambda(0.5); // Set the constant gain value
- for (unsigned int i = 0 ; i < 4 ; i++) {
- ...
- task.addFeature(p[i], pd[i]); // Add visual features to the task
- }
- while(1) {
- for (unsigned int i = 0 ; i < 4 ; i++) {
- ...
- vpFeatureBuilder::create(p[i], point[i]); // Update the visual features used in the task
- }
- vpColVector v = task.computeControlLaw(); // Compute the control law
- }
-\endcode
-
-
-\section adaptive_gain Using an adaptive gain
-
-As implemented in tutorial-ibvs-4pts-plotter-gain-adaptive.cpp it is possible to adapt the gain \f$ \lambda \f$ in order to depend on the infinity norm of the task Jacobian. The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time. In that case the gain becomes:
-
- \f[ \lambda (x) = a * exp (-b*x) + c \f]
-
- where \f$ a \f$, \f$ b \f$ and \f$ c \f$ are constant parameters and \f$ x \f$ is the infinity norm of the task Jacobian to consider.
-
- The parameters \f$a,b,c\f$ are not set directly. They are computed from three other parameters
- \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ that are more intuitive to tune:
- \f[ a = \lambda(0) - \lambda(\infty) \f]
- \f[ b = {\dot \lambda}(0) / a \f]
- \f[ c = \lambda(\infty) \f]
-
- Here \f$ \lambda(0)\f$ represents the gain when \f$x=0\f$, \f$ \lambda(\infty)\f$ represents the gain when \f$x=\infty\f$
- and \f$ {\dot \lambda}(0)\f$ represents the slope of \f$\lambda(x)\f$ when \f$x=0\f$.
-
-The impact of the adaptive gain is illustrated in the next figure. During the servo, velocities applied to the controller are higher, especially when the visual error \f${\bf e}\f$ is small. But as in the previous section, using an adaptive gain doesn't insure continuous velocities especially at the first iteration.
-
-\image html img-ibvs-control-law-adaptive.png "Convergence in 91 iterations with an adaptive gain."
-
-This behavior can be reproduced running tutorial-ibvs-4pts-plotter-gain-adaptive.cpp example. Compared to the previous code given in \ref intro and available in tutorial-ibvs-4pts-plotter.cpp, here after we give the new lines of code that were introduced to use an adaptive gain:
-
-\code
- vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda_dot(0)=30
- task.setLambda(lambda);
-\endcode
-
-
-\section continuous_adaptive_gain Continuous sequencing
-
-As implemented in tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp it is also possible to ensure continuous sequencing to avoid velocity discontinuities. This behavior is achieved by introducing an additional term to the general form of the control law. This additional term comes from the task sequencing approach described in \cite Mansard07e equation (17). It allows to compute continuous velocities by avoiding abrupt changes in the command.
-
- The form of the control law considered here is the following:
-
- \f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t)
- \f]
-
- where :
- - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot controller.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task.
- - \f$\bf e = (s-s^*)\f$ is the error to regulate.
- - \f$t\f$ is the time.
- - \f$\mu\f$ is a gain. We recommend to set this value to 4.
- - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
-
-
-The effect of continuous sequencing is illustrated in the next figure where during the first iterations velocities are starting from zero.
-
-\image html img-ibvs-control-law-continuous-adaptive.png "Convergence in 98 iterations with an adaptive gain and continuous sequencing."
-
-This behavior can be reproduced running tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp example. Compared to the previous code given in \ref adaptive_gain and available in tutorial-ibvs-4pts-plotter-gain-adaptive.cpp, here after we give the new line of code that were introduced to ensure continuous sequencing:
-
-\code
- vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());
-\endcode
-
-*/
diff --git a/doc/tutorial-detection-barcode.doc b/doc/tutorial-detection-barcode.doc
deleted file mode 100644
index 2c4f77d..0000000
--- a/doc/tutorial-detection-barcode.doc
+++ /dev/null
@@ -1,106 +0,0 @@
-/**
-
-\page tutorial-detection-barcode Tutorial: Bar code detection
-\tableofcontents
-
-\section intro_barcode Introduction
-
-This tutorial shows how to detect one or more barcodes with ViSP. To this end we provide two classes that are wrapper over 3rd party libraries:
-- vpDetectorQRCode that is a wrappers over zbar library <http://zbar.sourceforge.net>. It allows to detect QR codes. See <a href="https://visp.inria.fr/3rd_zbar">zbar quick installation guide</a>.
-- vpDetectorDataMatrix this is a wrapper over dmtx library <http://www.libdmtx.org>. It allows to detect Data Matrix codes. See <a href="https://visp.inria.fr/3rd_dmtx">dmtx quick installation guide</a>.
-
-These classes inherit from vpDetectorBase class, a generic class dedicated to detection. For each detected bar code, it allows to retrieve some characteristics such as the bar code message if it exists, and in the image the polygon that contains the bar code, the bounding box or the center of gravity of the bar code.
-
-In the next sections you will find examples that show how to detect codes in a single image and or in images acquired from a camera connected to your computer.
-
-Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/detection/barcode
-\endcode
-
-
-\section barcode_detection_basic Bar code detection (single image)
-
-The following example also available in tutorial-barcode-detector.cpp allows to detect either a QR code or Data Matrix on a single image. The user can select with --code-type option which code to detect.
-
-\include tutorial-barcode-detector.cpp
-
-To detect a QR code just run:
-\code
-$ ./tutorial-barcode-detector --code-type 0
-\endcode
-You will get the following result:
-
-\image html img-detection-qrcode.png
-
-To detect a Data Matrix code just run:
-\code
-$ ./tutorial-barcode-detector --code-type 1
-\endcode
-You will get the following result:
-
-\image html img-detection-datamatrix.png
-
-Now we explain the main lines of the source.
-
-First we have to include the header of the two classes that allow to detect either the QR code or the Data Matrix code.
-\snippet tutorial-barcode-detector.cpp Include
-
-Then in the main() function before going further we need to check if at least one of the third party (zbar or dmtx libraries) were used to build ViSP. We also check if ViSP is able to display images using either X11, or the Graphical Device Interface (GDI) under Windows, or OpenCV.
-
-\snippet tutorial-barcode-detector.cpp Macro defined
-
-After reading the input image \e bar-code.pgm and creation of a display device in order to visualize the image, we create a NULL pointer to a detector base class.
-
-\snippet tutorial-barcode-detector.cpp Create base detector
-
-Since the classes that allow to detect QR codes and Data Matrix codes inherit from vpDetectorBase, using the variable \e opt_barcode we are able to construct the requested detector.
-
-\snippet tutorial-barcode-detector.cpp Create detector
-
-We are now reader to detect the bar code in the image.
-
-\snippet tutorial-barcode-detector.cpp Detection
-
-If one or more bar codes are detected, the variable \e status is set to \e true.
-In that case, we can retrieve the number of detected bar codes in order to create a for loop over the codes.
-
-\snippet tutorial-barcode-detector.cpp Parse detected codes
-
-For each code, we can then get the location of the 4 points that define the polygon that contains the code, but also the bounding box.
-
-\snippet tutorial-barcode-detector.cpp Get location
-
-And finally, if it exists, we are also able to get the message that is coded in the bar code.
-
-\snippet tutorial-barcode-detector.cpp Get message
-
-
-\section barcode_detection_live Bar code detection (live camera)
-
-This other example also available in tutorial-barcode-detector-live.cpp shows how to couple the bar code detector to an image grabber in order to detect bar codes on each new image acquired by a camera connected to your computer.
-
-\include tutorial-barcode-detector-live.cpp
-
-The usage of this example is similar to the previous one:
-- with option --code-type you select if you want to detect a QR code (use --code-type 0) or a Data Matrix (use --code-type 1).
-- if more than one camera is connected to you computer, with option --device you can select which camera to use. The first camera that is found has number 0.
-
-To detect QR codes on images acquired by a second camera connected to your computer use:
-\code
-$ ./tutorial-barcode-detector-live --code-type 0 --device 1
-\endcode
-
-The source code of this example is very similar to the previous one except that here we use camera framegrabber devices (see \ref tutorial-grabber). Two different grabber may be used:
-- If ViSP was build with Video For Linux (V4L2) support available for example on Fedora or Ubuntu distribution, VISP_HAVE_V4L2 macro is defined. In that case, images coming from an USB camera are acquired using vpV4l2Grabber class.
-- If ViSP wasn't build with V4L2 support, but with OpenCV we use cv::VideoCapture class to grab the images. Notice that when images are acquired with OpenCV there is an additional conversion from cv::Mat to vpImage.
-
-\snippet tutorial-barcode-detector-live.cpp Construct grabber
-
-Then in the while loop, at each iteration we acquire a new image
-\snippet tutorial-barcode-detector-live.cpp Acquisition
-
-This new image is then given as input to the bar code detector.
-
-*/
diff --git a/doc/tutorial-getting-started-iOS.doc b/doc/tutorial-getting-started-iOS.doc
deleted file mode 100644
index 7d9e784..0000000
--- a/doc/tutorial-getting-started-iOS.doc
+++ /dev/null
@@ -1,159 +0,0 @@
-
-/**
- \page tutorial-getting-started-iOS Tutorial: How to create a basic iOS application that uses ViSP
- \tableofcontents
-
-
- \note We assume that you have \c "ViSP for iOS" either after following \ref tutorial-install-ios-package or \ref tutorial-install-iOS. Following one of these tutorial allows to exploit \c visp3.framework to build an application for iOS devices.
-
-In this tutorial we suppose that you install \c visp3.framework in a folder named \c \<framework_dir\>/ios. If \c \<framework_dir\> corresponds to \c ~/framework, you should get the following:
-
-\code
-$ ls ~/framework/ios
-opencv2.framework
-visp3.framework
-\endcode
-
-\section getting-started-iOS-create Create a new Xcode project
-
-- Launch Xcode
-- Follow <tt>"File>New>Project"</tt> menu and create a new <tt>"Single View Application"</tt>
-
-\image html img-getting-started-iOS-create.png
-
-- Click on <tt>"Next"</tt> button and complete the options for your new project:
-
-\image html img-getting-started-iOS-options.png
-
-- Click on <tt>"Next"</tt> button and select the folder where the new project will be saved. Once done click on <tt>"Create"</tt>. Now you should have something similar to:
-
-\image html img-getting-started-iOS-new.png
-
-\section getting-started-iOS-link-visp Linking ViSP framework
-
-Now we need to link \c visp3.framework with the Xcode project.
-
-- Select the project navigator in the left hand panel (1) and click on project name <tt>"Getting Started"</tt> (2)
-
-\image html img-getting-started-iOS-navigator.png
-
-- Use the Finder to drag & drop ViSP and OpenCV frameworks located in \c \<framework_dir\>/ios folder in the left hand panel containing all the project files.
-\image html img-getting-started-iOS-drag-drop.png
-
-- In the dialog box, enable check box <tt>"Copy item if needed"</tt> to ease \c visp3.framework and \c opencv.framework headers location addition to the build options
-
-\image html img-getting-started-iOS-drag-drop-dialog.png
-
-- Click on <tt>"Finish"</tt>. You should now get something similar to the following image
-
-\image html img-getting-started-iOS-link-visp.png
-
-
-\section getting-started-iOS-app Writing a ViSP iOS application
-
-- Because we will mix Objective C and ViSP C++ Code, rename \c ViewController.m file into \c ViewController.mm
-
-\image html img-getting-started-iOS-rename.png
-
-- Now copy/paste the following getting started sample code (inspired from tutorial-homography-from-points.cpp) into \c ViewController.mm
-\code
-#import "ViewController.h"
-
-#ifdef __cplusplus
-#import <visp3/visp.h>
-#endif
-
- at interface ViewController ()
-
- at end
-
- at implementation ViewController
-
-#pragma mark - Example of a function that uses ViSP
-- (void)processViSPHomography{
-
- std::vector<vpPoint> oP(4), aP(4), bP(4);
- double L = 0.1;
-
- oP[0].setWorldCoordinates( -L,-L, 0);
- oP[1].setWorldCoordinates(2*L,-L, 0);
- oP[2].setWorldCoordinates( L, 3*L, 0);
- oP[3].setWorldCoordinates( -L, 4*L, 0);
-
- vpHomogeneousMatrix bMo(0,0, 1, 0, 0, 0) ;
- vpHomogeneousMatrix aMb(0.2, 0, 0.1, 0,vpMath::rad(20), 0);
- vpHomogeneousMatrix aMo = aMb*bMo ;
-
- // Normalized coordinates of points in the image frame
- std::vector<double> xa(4), ya(4), xb(4), yb(4);
-
- for(int i=0 ; i < 4; i++){
- oP[i].project(aMo);
- xa[i] = oP[i].get_x();
- ya[i] = oP[i].get_y();
- oP[i].project(bMo);
- xb[i] = oP[i].get_x();
- yb[i] = oP[i].get_y();
- }
-
- vpHomography aHb ;
-
- // Compute the homography
- vpHomography::DLT(xb, yb, xa, ya, aHb, true);
-
- std::cout << "Homography:\n" << aHb << std::endl;
-
- vpRotationMatrix aRb;
- vpTranslationVector atb;
- vpColVector n;
-
- // Compute the 3D transformation
- aHb.computeDisplacement(aRb, atb, n);
-
- std::cout << "atb: " << atb.t() << std::endl;
-
- // Compute coordinates in pixels of point 3
- vpImagePoint iPa, iPb;
- vpCameraParameters cam;
- vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
- vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
-
- std::cout << "Ground truth:" << std::endl;
- std::cout << " Point 3 in pixels in frame b: " << iPb << std::endl;
- std::cout << " Point 3 in pixels in frame a: " << iPa << std::endl;
-
- // Estimate the position in pixel of point 3 from the homography
- vpMatrix H = cam.get_K() * aHb * cam.get_K_inverse();
-
- // Project the position in pixel of point 3 from the homography
- std::cout << "Estimation from homography:" << std::endl;
- std::cout << " Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb) << std::endl;
-}
-
-- (void)viewDidLoad {
- [super viewDidLoad];
- // Do any additional setup after loading the view, typically from a nib.
- [self processViSPHomography];
-}
-
-- (void)didReceiveMemoryWarning {
- [super didReceiveMemoryWarning];
- // Dispose of any resources that can be recreated.
-}
- at end
-\endcode
-In this sample, we first import the headers to use vpHomography class. Then we create a new function called \c processViSPHomography(). This function is finally called in viewDibLoad().
-- After the previous copy/paste, you should have something similar to
-
-\image html img-getting-started-iOS-code.jpeg
-
-- Now we are ready to build this simple <tt>"Getting Started"</tt> application using Xcode <tt>"Product>Build"</tt> menu.
-
-- You can now run your code using <tt>"Product>Run"</tt> menu (Simulator or device does not bother because we are just executing code). You should obtain these logs showing that visp code was correctly executed by your iOS project.
-
-\image html img-getting-started-iOS-log.jpeg
-\section getting-started-iOS-next Next tutorial
-
-You are now ready to see the \ref tutorial-image-ios.
-
-*/
diff --git a/doc/tutorial-getting-started-naoqi.doc b/doc/tutorial-getting-started-naoqi.doc
deleted file mode 100644
index 0f2ad11..0000000
--- a/doc/tutorial-getting-started-naoqi.doc
+++ /dev/null
@@ -1,84 +0,0 @@
-
-/**
- \page tutorial-getting-started-naoqi Tutorial: How to create an application that uses ViSP on NAOqi OS
- \tableofcontents
-
-We assume in this tutorial that you have successfully cross-compiled ViSP following \ref tutorial-install-crosscompiling-naoqi.
-The aim of this tutorial is to explain how to create an application that uses ViSP cross-build for NAOqi OS as third-party and how to run this application on Nao, Romeo or Pepper robots.
-
-To illustrate this tutorial, we will consider all the examples that are provided in the tutorial dedicated to images (ie. the one located in \c visp/tutorial/image folder). If you are not familiar with ViSP read first \ref tutorial-getting-started.
-
-This tutorial was tested on an Ubuntu 14.04 LTS host computer with:
-- Cross Toolchain 2.3.1 Linux 64 (\c ctc-linux32-atom-2.3.1.23) on Romeo robot
-- Cross Toolchain 2.4.3 Linux 64 (\c ctc-linux64-atom-2.4.3.28) on Pepper robot
-
-
-\section started_naoqi_code Create a workspace for your project
-
-We assume that as described in \ref tutorial-install-crosscompiling-naoqi, your workspace dedicated to visp contains the following folders:
-\code
-$ ls $HOME/soft
-visp
-visp-build-ctc-linux64-atom-2.4.3.28
-\endcode
-
-Create a new folder to host the project you want to run on NAOqi OS
-\code
-$ mkdir $HOME/soft/tutorial
-\endcode
-
-Get existing tutorial/image folder from ViSP source code
-
-\code
-$ cd $HOME/soft/tutorial
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image
-\endcode
-
-\section started_naoqi_build Cross-build your project
-
-Create first a build folder associated to your Cross Toolchain like:
-\code
-$ cd $HOME/soft/tutorial
-$ mkdir image-build-ctc-linux64-atom-2.4.3.28
-\endcode
-
-Configure your project setting \c VISP_DIR and \c CMAKE_TOOLCHAIN_FILE respectively to ViSP and Cross Toolchain location:
-\code
-$ cd image-build-ctc-linux64-atom-2.4.3.28
-$ cmake ../image -DVISP_DIR=$HOME/soft/visp-build-ctc-linux64-atom-2.4.3.28/install/lib/cmake/visp -DCMAKE_TOOLCHAIN_FILE=$HOME/softbank/ctc-linux64-atom-2.4.3.28/toolchain.cmake
-\endcode
-
-Cross-build your project:
-\code
-$ make -j4
-\endcode
-
-\section started_naoqi_install Install the binaries on NAOqi
-
-First create an archive for the binaries:
-\code
-$ tar cvzf binary.tar.gz --exclude "*Make*" --exclude "cmake*" *
-\endcode
-
-Then copy the binaries to NAOqi OS:
-\code
-$ scp binary.tar.gz nao@<your_robot_ip>:
-\endcode
-
-Move the binaries to a more friend location like \c $HOME/tutorial:
-\code
-$ ssh nao@<your_robot_ip>
-nao~ $ mkdir tutorial
-nao~ $ tar xvzf binary.tar.gz --directory tutorial
-nao~ $ rm binary.tar.gz
-\endcode
-
-Run the binaries:
-\code
-nao~ $ cd tutorial
-nao~ $ ./tutorial-image-manipulation
-nao~ $ ./tutorial-undistort
-\endcode
-
-You are now ready to create your own project for Nao, Romeo or Pepper robots.
-*/
diff --git a/doc/tutorial-getting-started.doc b/doc/tutorial-getting-started.doc
deleted file mode 100644
index 7905b6c..0000000
--- a/doc/tutorial-getting-started.doc
+++ /dev/null
@@ -1,204 +0,0 @@
-
-/**
- \page tutorial-getting-started Tutorial: How to create and build a CMake project that uses ViSP on Unix or Windows
- \tableofcontents
-
-\note We assume in this tutorial that you have successfully installed ViSP either with an \ref tuto_install_sdk or with an \ref tuto_install_src.
-
-In this tutorial you will learn how to use ViSP either on Unix-like systems (including OSX, Fedora, Ubuntu, Debian, ...) or on Windows.
-
-The easiest way of using ViSP in your project is to use <a href="http://www.cmake.org/">CMake</a>. If you are not familiar with CMake, you can check the <a href="http://www.cmake.org/cmake/help/cmake_tutorial.html">tutorial</a>.
-
-Note also that all the material (source code and images) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image
-\endcode
-
-\section image_code Create a program using ViSP
-Let's start to write our first program to see how to read an image and open a window to display the image. This simple program is provided in tutorial-viewer.cpp example and given below:
-
-\include tutorial-viewer.cpp
-
-Here is the detailed explanation of the source, line by line :
-\snippet tutorial-viewer.cpp Include display
-
-Include all the headers for image viewers. The two first one are for Windows systems. They require that Direct 3D or the \e Graphical \e Device \e Interface (\e GDI) coming with the installation of Visual Studio are available. The third one needs GTK that is cross-platform. The fourth is for unix-like systems and requires that \e libX11 is available. The last one is also cross-platform and requires that OpenCV is available.
-
-\snippet tutorial-viewer.cpp Include io
-Include the header that allows to read/write PGM, PPM, PNG and JPEG images from the disk using vpImageIo class.
-
-\snippet tutorial-viewer.cpp vpImage construction
-Create an instance of a color image where each pixel is coded in RGBa.
-
-\snippet tutorial-viewer.cpp vpImage reading
-The image \c I is initialized by reading an image file from the disk. If the image format is not supported we throw an exception.
-
-\snippet tutorial-viewer.cpp vpDisplay construction
-Create an instance of an image display window for image \c I. The first viewer that is available is used. Here we create the link between the image \c I and the display \c d. Note that an image can only have one display.
-
-\snippet tutorial-viewer.cpp vpDisplay set title
-The title of the display is then set to \c "My image".
-
-\snippet tutorial-viewer.cpp vpDisplay display
-First we display the content of the image \c I, then we flush the display to render the image.
-
-\snippet tutorial-viewer.cpp vpDisplay get click
-Here we handle mouse events. We are waiting for a blocking mouse click to end the program.
-
-\section image_cmake Create a CMake file
-
-Now you have to create your \c CMakeLists.txt file. It should look like this:
-
-\code
-project(tutorial-image)
-
-cmake_minimum_required(VERSION 2.8)
-
-find_package(VISP REQUIRED)
-
-include_directories(${VISP_INCLUDE_DIRS})
-
-add_executable(tutorial-viewer tutorial-viewer.cpp)
-target_link_libraries(tutorial-viewer ${VISP_LIBRARIES})
-\endcode
-
-The \c find_package() CMake command searches for a \c VISPConfig.cmake file that will define the corresponding variables:
-- \c VISP_INCLUDE_DIRS : ViSP and third-party headers location
-- \c VISP_LIBRARIES : ViSP and third-party libraries name and location
-
-Note that the previous \c CMakeLists.txt file can also be:
-\code
-project(tutorial-image)
-
-cmake_minimum_required(VERSION 2.8)
-
-find_package(VISP REQUIRED)
-if(VISP_FOUND)
- include(${VISP_USE_FILE})
-endif(VISP_FOUND)
-
-add_executable(tutorial-viewer tutorial-viewer.cpp)
-\endcode
-
-where \c VISP_USE_FILE variable is set to the full path to \c VISPUse.cmake file that contains all the CMake material that allow to build your project with ViSP. In other terms, the line
-\code
- include(${VISP_USE_FILE})
-\endcode
-will include the following lines to your \c CMakeFile.txt
-\code
- include_directories(${VISP_INCLUDE_DIRS})
- link_libraries(${VISP_LIBRARIES})
-\endcode
-
-
-\section image_unix On Unix-like systems
-
-\subsection image_unix_config Configure your project
-
-Proceed as with any other project using CMake:
-
-\code
-$ cmake .
-\endcode
-
-By default \c cmake searches \c VISPConfig.cmake file in folders like \c /usr/share or \c /usr/local/share. If ViSP was not installed in \c /usr or \c /usr/local it is possible that you get the following error:
-\code
-CMake Error at CMakeLists.txt:5 (find_package):
- Could not find module FindVISP.cmake or a configuration file for package
- VISP.
-
- Adjust CMAKE_MODULE_PATH to find FindVISP.cmake or set VISP_DIR to the
- directory containing a CMake configuration file for VISP. The file will
- have one of the following names:
-
- VISPConfig.cmake
- visp-config.cmake
-\endcode
-
-To help \c cmake to find \c VISPConfig.cmake file, just set \c VISP_DIR environment variable and call \c cmake again:
-\code
-$ export VISP_DIR=/home/ViSP-install-folder/lib/<multi-arch-folder>/cmake/visp
-$ cmake .
-\endcode
-or run cmake with the additional VISP_DIR definition
-\code
-$ cmake -DVISP_DIR=/home/ViSP-install-folder/lib/<multi-arch-folder>/cmake/visp .
-\endcode
-
-Depending on the platform \c \<multi-arch-folder\> can be empty (OSX) or for example equal to \c x86_64-linux-gnu on Ubuntu.
-
-\note If you build ViSP from source following one of the tutorials \ref tutorial_install_src, you can avoid the \c make \c install step and set \c VISP_DIR to the build location instead of the install location.
-
-\subsection image_unix_build Generate the executable
-
-Just run:
-\code
-$ make
-\endcode
-
-\subsection image_unix_output Run the executable
-
-By now you should have an executable called \c tutorial-viewer. You just have to run it giving an image location as an argument:
-
-\code
-$ ./tutorial-viewer monkey.ppm
-\endcode
-
-
-Here is a screen shot of the resulting output window :
-
- \image html img-monkey.png
-
-\section image_win On Windows
-
-We suppose from now, that you have created a folder (let say \c C:/ViSP/ViSP-started) that contains the following files: CMakeLists.txt, tutorial-viewer.cpp and monkey.ppm. These files are also provider in ViSP source code, in \c tutorial/image folder.
-
-
-\subsection image_win_config Configure your project
-
-- Launch CMake (cmake-gui) from Windows "Start" menu. Set the source code location as \c C:/ViSP/ViSP-started. Here we set the build location to the same folder.
-
-\image html img-started-win-cmake-1.jpg
-
-- Press "Configure" button, and select your compiler. In our case we will use Visual Studio 11 Win64. Press then "Finish". The configuration is now under progress and should lead to the following image.
-
-\image html img-started-win-cmake-2.jpg
-
-- Note here that CMake has automatically found the location of ViSP install folder; \c C:/ViSP/ViSP-install.
-
-- Press then "Configure" button to remove the red lines, and then "Generate" button. As presented in the following image, all the red lines should disappear.
-
-\image html img-started-win-cmake-3.jpg
-
-- From now, in \c C:/ViSP/ViSP-started you should have \c tutorial-image.sln Visual Studio solution file.
-
-\subsection image_win_build Generate the executable
-
-- To open the project in Visual Studio C++ just double click on \c C:/ViSP/ViSP-started/tutorial-image.sln solution file.
-
-\image html img-started-win-msvc-1.jpg
-
-- Now to build the solution, enter "BUILD/Build Solution" menu.
-
-\image html img-started-win-msvc-2.jpg
-
-- In \c C:/ViSP/ViSP-started/Debug folder you have now \c tutorial-image.exe executable.
-
-
-\subsection image_win_output Run the executable
-
-- In your "Start" menu click on "Run" and type in \c cmd.exe to run a terminal.
-- Enter in \c C:/ViSP/ViSP-started/Debug folder, and run \c tutorial-viewer.exe with an image location as argument:
-
-\image html img-started-win-run.jpg
-
-
-- Here is a screen shot of the resulting output window :
-
- \image html img-monkey-win.jpg
-
-\section image_next Next tutorial
-
-You are now ready to see the \ref tutorial-image-display.
-*/
diff --git a/doc/tutorial-grabber.doc b/doc/tutorial-grabber.doc
deleted file mode 100644
index 9709aca..0000000
--- a/doc/tutorial-grabber.doc
+++ /dev/null
@@ -1,155 +0,0 @@
-
-/**
- \page tutorial-grabber Tutorial: Image frame grabbing
- \tableofcontents
-
-
-\section grabber-into Introduction
-
-In this tutorial you will learn how to grab images with ViSP, either from cameras or from a video stream.
-
-All the material (source code and videos) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/grabber
-\endcode
-
-\section grabber-camera-pgr Images from PointGrey cameras
-
-After ViSP 3.0.0, we introduce vpFlyCaptureGrabber class, a wrapper over PointGrey FlyCapture SDK that allows to grab images from any PointGrey camera. This grabber was tested under Ubuntu and Windows with the following cameras:
- - Flea3 USB 3.0 cameras (FL3-U3-32S2M-CS, FL3-U3-13E4C-C)
- - Flea2 firewire camera (FL2-03S2C)
- - Dragonfly2 firewire camera (DR2-COL)
-
-It should also work with GigE PGR cameras.
-
-The following example also available in tutorial-grabber-flycapture.cpp shows how to use vpFlyCaptureGrabber to capture grey level images from a PointGrey camera under Ubuntu or Windows. The following example suppose that a window renderer (libX11 on Ubuntu or GDI on Windows) and FlyCapture SDK 3rd party are available throw VISP.
-
-\include tutorial-grabber-flycapture.cpp
-
-Here after we explain the source code.
-
-First an instance of the frame grabber is created.
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber construction
-
-Once the grabber is created, we turn auto shutter and auto gain on and set the camera image size, color coding, and framerate.
-These settings are enclosed in a try/catch to be able to continue if one of these settings are not supported by the camera.
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber settings
-
-Then the grabber is initialized using:
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber open
-
-From now the grey level image \c I is also initialized with the size corresponding to the grabber settings.
-
-Then we enter in a while loop where image acquisition is simply done by:
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber acquire
-
-This image is then displayed using libX11 or GDI renderer:
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber display
-
-We are waiting for a non blocking mouse event to break the while loop before ending the program.
-\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber click to exit
-
-\section grabber-camera-firewire-unix Images from firewire cameras
-
-The next example also available in tutorial-grabber-1394.cpp shows how to use a framegrabber to acquire color images from a firewire camera under Unix. The following example suppose that libX11 and libdc1394-2 3rd party are available.
-
-\include tutorial-grabber-1394.cpp
-
-Here after we explain the new lines that are introduced.
-
-First an instance of the frame grabber is created. During the creating a bus reset is send. If you don't want to reset the firewire bus, just turn reset to false.
-\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber construction
-
-Once the grabber is created, we set the camera image size, color coding, and framerate.
-\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber settings
-
-Note that here you can specify some other settings such as the firewire transmission speed. For a more complete list of settings see vp1394TwoGrabber class.
-\code
- g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800);
-\endcode
-
-Then the grabber is initialized using:
-\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber open
-
-From now the color image \c I is also initialized with the size corresponding to the grabber settings.
-
-Then we enter in a while loop where image acquisition is simply done by:
-\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber acquire
-
-We are waiting for a non blocking mouse event to break the while loop before ending the program.
-\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber click to exit
-
-In the previous example we use vp1394TwoGrabber class that works for firewire cameras under Unix. If you are under Windows, you may use vp1394CMUGrabber class. A similar example is provided in tutorial-grabber-CMU1394.cpp.
-
-\section grabber-camera-other Images from other cameras
-
-If you want to grab images from an usb camera under Unix, you may use vpV4l2Grabber class. To this end libv4l should be installed. An example is provided in tutorial-grabber-v4l2.cpp.
-
-It is also possible to grab images using OpenCV. You may find an example in tutorial-grabber-opencv.cpp.
-
-\section grabber-video-stream Images from a video stream
-
-With ViSP it also possible to get images from an input video stream. Supported formats are *.avi, *.mp4, *.mov, *.ogv, *.flv and many others... To this end we exploit ffmpeg or OpenCV 3rd parties.
-
-If ViSP was build with ffmpeg 3rd party support (cmake -DUSE_FFMPEG=ON ...), we use ffmpeg capabilities to decode the video stream. If ffmpeg is not found or used (cmake -DUSE_FFMPEG=OFF ...) and if OpenCV is available (cmake -DUSE_OPENCV=ON ...) we use rather OpenCV capabilities. This new feature was introduced in ViSP 2.10.0 and is especially useful under Windows where installing ffmpeg is quite complex.
-
-The example below available in tutorial-video-reader.cpp shows how o consider an mpeg video stream.
-
-\warning We recall that this example works only if ViSP was build with ffmpeg or OpenCV support.
-
-\include tutorial-video-reader.cpp
-
-We explain now the new lines that were introduced.
-
-\snippet tutorial-video-reader.cpp Include
-Include the header of the vpTime class that allows to measure time, and of the vpVideoReader class that allows to read a video stream.
-
-\snippet tutorial-video-reader.cpp vpVideoReader construction
-Create an instance of a video reader.
-
-\snippet tutorial-video-reader.cpp vpVideoReader setting
-Set the name of the video stream. Here \c videoname corresponds to a video file name location. For example we provide the file \c video.mpg located in the same folder than the executable.
-
-The vpVideoReader class can also handle a sequence of images. For example, to read the following images:
-
-\code
-% ls *.png
-image0000.png
-image0001.png
-image0002.png
-image0003.png
-image0004.png
-...
-\endcode
-
-you may use the following:
-
-\code
- g.setFileName("./image%04d.png");
-\endcode
-where you specify that each image number is coded with 4 digits. Here, ffmpeg is no yet mandatory, but rather \c libpng or OpenCV that should be available to read PNG images. Supported image formats are PPM, PGM, PNG and JPEG.
-
-Then as for any other grabber, you have to initialize the frame grabber using:
-
-\snippet tutorial-video-reader.cpp vpVideoReader open
-
-Then we enter in the while loop until the last image was not reached:
-\snippet tutorial-video-reader.cpp vpVideoReader while loop
-
-To get the next image in the stream, we just use:
-\snippet tutorial-video-reader.cpp vpVideoReader acquire
-
-To synchronize the video decoding with the video framerate, we measure the beginning time of each loop iteration:
-
-\snippet tutorial-video-reader.cpp vpVideoReader loop start time
-
-The synchronization is done by waiting from the beginning of the iteration the corresponding time expressed in milliseconds by using:
-\snippet tutorial-video-reader.cpp vpVideoReader loop rate
-
-\section grabber-next Next tutorial
-
-You are now ready to see how to continue with:
-- \ref tutorial-multi-threading if you want to see how to extend these examples with one thread for capture and an other one for display
-- or with a simple image processing that shows how to track blobs explained in \ref tutorial-tracking-blob.
-*/
diff --git a/doc/tutorial-image-ios.doc b/doc/tutorial-image-ios.doc
deleted file mode 100644
index 12c41c6..0000000
--- a/doc/tutorial-image-ios.doc
+++ /dev/null
@@ -1,79 +0,0 @@
-/**
-
-\page tutorial-image-ios Tutorial: Image processing on iOS
-\tableofcontents
-
-This tutorial supposes that you have followed the \ref tutorial-getting-started-iOS.
-
-\section image_ios_intro Introduction
-
-In this tutorial you will learn how to do simple image processing on iOS devices with ViSP. This application loads a color image (<a href="https://github.com/lagadic/visp/blob/master/tutorial/ios/StartedImageProc/StartedImageProc/monkey.png">monkey.png</a>) and allows the user to visualize either this image in grey level, either the image gradients, or either canny edges on iOS simulator or devices.
-
-In ViSP images are carried out using vpImage class. However in iOS, image rendering has to be done using UIImage class that is part of the Core Graphics framework available in iOS. In this tutorial we provide the functions that allow to convert a vpImage to an UIImage and \e vice \e versa.
-
-Note that all the material (source code and image) used in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedImageProc
-\endcode
-
-\section image_ios_convert Image conversion functions
-
-\subsection image_ios_convert_uiimage_vpimage_color UIImage to color vpImage
-
-The following function implemented in ViewController.mm show how to convert an UIImage into a vpImage<vpRGBa> instanciated as a color image.
-
-\snippet ViewController.mm vpImageColorFromUIImage
-
-\subsection image_ios_convert_uiimage_vpimage_gray UIImage to gray vpImage
-
-The following function implemented in ViewController.mm show how to convert an UIImage into a vpImage<unsigned char> instanciated as a grey level image.
-
-\snippet ViewController.mm vpImageGrayFromUIImage
-
-\subsection image_ios_convert_vpimage_color_uiimage Color vpImage to UIImage
-
-The following function implemented in ViewController.mm show how to convert a gray level vpImage<unsigned char> into an UIImage.
-
-\snippet ViewController.mm UIImageFromVpImageColor
-
-\subsection image_ios_convert_vpimage_gray_uiimage Gray vpImage to UIImage
-
-The following function implemented in ViewController.mm show how to convert a color vpImage<vpRGBa> into an UIImage.
-
-\snippet ViewController.mm UIImageFromVpImageGray
-
-\section image_ios_app StartedImageProc application
-
-Let us consider the Xcode project named \c StartedImageProc that is part of ViSP source code. This project is a Xcode \c "Single view application" where we renamed \c ViewController.m into \c ViewController.mm, introduced minor modifications in ViewController.h and add <a href="https://github.com/lagadic/visp/blob/master/tutorial/ios/StartedImageProc/StartedImageProc/monkey.png">monkey.png</a> image.
-
-\image html img-started-imgproc-ios.jpeg
-
-To download the complete \c StartedImageProc project, run the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedImageProc
-\endcode
-
-Once downloaded, you have just to drag & drop ViSP and OpenCV frameworks available following \ref tutorial-install-ios-package.
-\image html img-started-imgproc-ios-drag-drop.jpeg
-
-In the dialog box, enable check box \c "Copy item if needed" to add \c visp3.framework and \c opencv.framework to the project.
-\image html img-started-imgproc-ios-drag-drop-dialog.png
-
-Now you should be able to build and run your application.
-
-\section image_ios_output Application output
-
-Once build, if you run \c StartedImageProc application on your device, you should be able to see the followings screen shots.
-
-- Selecting \c "load image" gives the following result:
-\image html img-started-imgproc-ios-output-color.jpeg
-- Selecting \c "convert to gray" gives the following result:
-\image html img-started-imgproc-ios-output-gray.jpeg
-- Selecting \c "compute gradient" gives the following result:
-\image html img-started-imgproc-ios-output-gradient.jpeg
-- Selecting \c "canny detector" gives the following result:
-\image html img-started-imgproc-ios-output-canny.jpeg
-
-*/
diff --git a/doc/tutorial-install-centos.doc b/doc/tutorial-install-centos.doc
deleted file mode 100644
index ba71969..0000000
--- a/doc/tutorial-install-centos.doc
+++ /dev/null
@@ -1,353 +0,0 @@
-/**
-
-\page tutorial-install-centos Tutorial: Installation from source for Linux CentOS
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on CentOS. These steps have been tested for CentOS 7.0 (x86_64) distribution, but should work with any other distribution as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_centos_required Install prerequisities
-
-- gcc 4.4.x or later. This can be installed with:
-\code
-$ sudo yum install gcc-c++
-\endcode
-- CMake 2.6 or higher that could be installed with:
-\code
-$ sudo yum install cmake
-\endcode
-
-
-\section install_centos_3rdparty Install 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_centos_3rdparty_recommended Recommended 3rd parties
-
-We recommend to install the following:
-
-- OpenCV
-\code
-$ sudo yum install opencv-devel
-\endcode
-- libX11 to be able to open a window to display images
-\code
-$ sudo yum install libX11-devel
-\endcode
-- lapack to benefit from optimized mathematical capabilities
-\code
-$ sudo yum install lapack-devel
-\endcode
-- libdc1394 to grab images from firewire cameras
-\code
-$ sudo yum install libdc1394-devel
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-$ sudo yum install libv4l-devel
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-$ sudo yum install libxml2-devel
-\endcode
-
-\subsection install_centos_3rdparty_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-
-- Coin, to be able to support vrml cad model used by the model-based trackers
-\code
-$ sudo yum install Coin2-devel
-\endcode
-- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
-\code
-$ sudo yum install libjpeg-devel libpng-devel
-\endcode
-
-
-\section install_centos_install_visp Install ViSP from source code
-\subsection install_centos_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c $HOME/visp
-
-\subsection install_centos_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd $HOME; mkdir visp-build
-\endcode
-
-- Enter the \c \<binary_dir\> and configure the build:
-\code
-$ cd $HOME/visp-build
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-centos-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
-
-\subsection install_centos_build Building ViSP libraries
-
-- To build ViSP proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsection install_centos_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ sudo yum install doxygen graphviz
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-\section install_centos_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_centos_tips Tips and tricks
-
-\subsection install_centos_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_centos_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_centos_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_centos_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_gui
-... visp_io
-... visp_klt
-... visp_me
-... visp_robot
-... visp_sensor
-... visp_ar
-... visp_blob
-... visp_visual_features
-... visp_vs
-... visp_vision
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_centos_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : no
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : yes
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : yes
- Video For Linux Two : yes
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : no
- \-libfreenect : no
- \-libusb-1.0 : no
- \-pthread : yes
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : no
- libpng : no
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : yes
- zbar : no
- dmtx : no
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_centos_issues Known issues
-\subsection install_centos_issues_pthread libpthread may be hidden by files in //lib64
-
-On CentOS 7.0 with cmake 2.8.11, during cmake configuration you may encounter the following issue:
-\code
-CMake Warning at src/CMakeLists.txt:80 (add_library):
- Cannot generate a safe runtime search path for target visp because files in
- some directories may conflict with libraries in implicit directories:
-
- runtime library [libpthread.so] in /usr/lib64 may be hidden by files in:
- //lib64
-
- Some of these libraries may not be found correctly.
-\endcode
-The problem was that libpthread.so exists in /usr/lib64 and in //lib64. In //lib64 it should be a symbolic link to /usr/lib64.
-\code
-$ ls -als //lib64
- 0 lrwxrwxrwx. 1 root root 9 Feb 4 12:16 //lib64 -> usr/lib64
-$ ls -als //lib64/libpthread*
-140 -rwxr-xr-x. 1 root root 141616 Jan 27 15:13 //lib64/libpthread-2.17.so
- 4 -rw-r--r--. 1 root root 222 Jan 27 14:42 //lib64/libpthread.so
- 0 lrwxrwxrwx. 1 root root 18 Feb 4 12:34 //lib64/libpthread.so.0 -> libpthread-2.17.so
-\endcode
-The fix consists in removing //lib64/libpthread.so and creating a new symbolic link
-\code
-$ cd //lib64
-$ sudo rm libpthread.so
-$ sudo ln -s libpthread-2.17.so libpthread.so
-$ ls -als libpthread*
-140 -rwxr-xr-x. 1 root root 141616 Jan 27 15:13 libpthread-2.17.so
- 0 lrwxrwxrwx. 1 root root 18 Feb 4 16:09 libpthread.so -> libpthread-2.17.so
- 0 lrwxrwxrwx. 1 root root 18 Feb 4 12:34 libpthread.so.0 -> libpthread-2.17.so
-\endcode
-
-\subsection install_centos_issues_videoreader vpVideoReader is not able to read mpeg videos
-
-On CentOS 7.0, vpVideoReader is not able to read mpeg videos. If ffmpeg is not installed (this is our case since ffmpeg is not packaged for CentOS 7.0), this class uses OpenCV to read and decode videos. Some examples or tutorials provided in ViSP hang during cv::Capture::open() call. The reason is that OpenCV 2.4.5 cv::Capture seams buggy. This is for example the case if you run:
-\code
-$ ./example/video/videoReader
-\endcode
-A work arround consists in installing a more recent OpenCV version from source.
-
-
-\section install_centos_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-fedora.doc b/doc/tutorial-install-fedora.doc
deleted file mode 100644
index 1060f8c..0000000
--- a/doc/tutorial-install-fedora.doc
+++ /dev/null
@@ -1,326 +0,0 @@
-/**
-
-\page tutorial-install-fedora Tutorial: Installation from source for Linux Fedora
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Linux Fedora. These steps have been tested for Fedora 21 (64 bit) distribution, but should work with any other distribution as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_fedora_required Install prerequisities
-
-- gcc 4.4.x or later. This can be istalled with:
-\code
-$ su -c "dnf install gcc-c++"
-\endcode
-- CMake 2.6 or higher that could be installed with:
-\code
-$ su -c "dnf install cmake"
-\endcode
-
-\section install_fedora_3rdparty Install 3rd parties
-
-ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_fedora_3rdparty_recommended Recommended 3rd parties
-
-We recommend to install the following:
-
-- OpenCV
-\code
-$ su -c "dnf install opencv-devel"
-\endcode
-- libX11 to be able to open a window to display images
-\code
-$ su -c "dnf install libX11-devel"
-\endcode
-- lapack to benefit from optimized mathematical capabilities
-\code
-$ su -c "dnf install lapack-devel"
-\endcode
-- libdc1394 to grab images from firewire cameras
-\code
-$ su -c "dnf install libdc1394-devel"
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-$ su -c "dnf install libv4l-devel"
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-$ su -c "dnf install libxml2-devel"
-\endcode
-- QR code detection
-\code
-$ su -c "dnf install zbar-devel"
-\endcode
-
-\subsection install_fedora_3rdparty_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-
-- Coin, to be able to support vrml cad model used by the model-based trackers
-\code
-$ su -c "dnf install Coin3-devel"
-\endcode
-- libjpeg, libpng to support jpeg and png images (only useful if OpenCV is not installed)
-\code
-$ su -c "dnf install libjpeg-devel libpng-devel"
-\endcode
-- ffmpeg, to be able to read or encode compressed video streams (only useful if OpenCV is not installed)
-\code
-$ su -c "dnf install ffmpeg-devel"
-\endcode
-- Ogre 3D if you want to do augmented reality or simulation
-\code
-$ su -c "dnf install ogre-devel ogre-samples ois-devel"
-\endcode
-- Datamatrix code detection
-\code
-$ su -c "dnf install libdmtx-devel"
-\endcode
-
-\section install_fedora_install_visp Install ViSP from source code
-\subsection install_fedora_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c $HOME/visp
-
-\subsection install_fedora_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd $HOME; mkdir visp-build
-\endcode
-
-- Enter the \c \<binary_dir\> and configure the build:
-\code
-$ cd $HOME/visp-build
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-ubuntu-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
-
-Now we can build ViSP.
-
-\subsection install_fedora_build Building ViSP libraries
-
-- To build ViSP libraries proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsection install_fedora_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ su -c "dnf install doxygen graphviz"
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-\section install_fedora_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_fedora_tips Tips and tricks
-
-\subsection install_fedora_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_fedora_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_fedora_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_fedora_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_gui
-... visp_io
-... visp_klt
-... visp_me
-... visp_robot
-... visp_sensor
-... visp_ar
-... visp_blob
-... visp_visual_features
-... visp_vs
-... visp_vision
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_fedora_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : no
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : yes
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : yes
- Video For Linux Two : yes
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : no
- \-libfreenect : no
- \-libusb-1.0 : no
- \-pthread : yes
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : no
- libpng : no
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : yes
- zbar : yes
- dmtx : no
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_fedora_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-iOS.doc b/doc/tutorial-install-iOS.doc
deleted file mode 100644
index f3c243d..0000000
--- a/doc/tutorial-install-iOS.doc
+++ /dev/null
@@ -1,103 +0,0 @@
-/**
-
-\page tutorial-install-iOS Tutorial: Installation from source for iOS devices
-\tableofcontents
-
-In this tutorial you will learn how to build ViSP framework from source on OSX in order to use it for iOS developments. These steps have been tested on OSX El Capitan 10.11.45 and 10.11.6, with CMake 3.5.2, Xcode 7.3.1 and 8.0, and Python 2.7.10.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_iOS_required Install required software
-
-- CMake gui 2.8 for OSX or higher that could be download at : http://www.cmake.org
-- Xcode
-- Python 2.7 (not tested with Python 3)
-
-\section install_iOS_opencv Getting OpenCV framework (optional)
-
-We suggest to follow these steps in order to install OpenCV framework in such a way that the resulting ViSP framework builds with OpenCV as 3rd party. This could be useful for users who want to use keypoints throw ViSP classes (vpKltOpencv, vpKeyPoint) or the model-based tracker in his hybrid version (vpMbKltTracker). If you are not interested in ViSP capabilities that use OpenCV as 3rd party, just skip this section.
-
-- Download <a href="http://opencv.org/downloads.html">OpenCV for iOS</a>
-- Unzip the archive in a directory denoted \c \<framework_dir\>/ios. If \c \<framework_dir\> is for example the following folder \c ~/framework just do the following:
-\code
-$ mkdir -p ~/framework/ios
-\endcode
-If the download was performed with Safari, do the following
-\code
-$ mv ~/Downloads/opencv2.framework ~/framework/ios/
-\endcode
-or if the download was either performed with Firefox, do the following
-\code
-$ unzip ~/Downloads/opencv2.framework.zip -d ~/framework/ios
-\endcode
-- Add \c opencv2.framework to iPhoneOS existing frameworks
-\code
-$ cd `xcrun --sdk iphoneos --show-sdk-platform-path`/Developer/Library/Frameworks
-$ sudo ln -s ~/framework/ios/opencv2.framework
-\endcode
-- Add \c opencv2.framework to iPhoneSimulator existing frameworks
-\code
-$ cd `xcrun --sdk iphonesimulator --show-sdk-platform-path`/Developer/Library/Frameworks
-$ sudo ln -s ~/framework/ios/opencv2.framework
-\endcode
-
-
-\section install_iOS_get_source Getting ViSP source code
-
-\note The following steps are only working with ViSP 3.0.1 or higher.
-
-Here the goal is to get ViSP source code in a directory denoted \c \<framework_dir\>. If \c \<framework_dir\> is for example the following folder \c ~/framework there are different ways to get ViSP:
-
-- You can get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ cd ~/framework
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C ~/framework
-\endcode
-
-- Or you can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz -C ~/framework
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip -d ~/framework
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<framework_dir\>.
-\code
-$ ls ~/framework
-visp
-\endcode
-
-\section install_iOS_build Building ViSP framework
-
-- Enter in the \c \<framework_dir\> and check the name of he folder containing ViSP source code
-\code
-$ cd ~/framework
-$ ls
-visp
-\endcode
-
-- Build ViSP framework
-\code
-$ python visp/platforms/ios/build_framework.py ios
-\endcode
-
-Once build, you will have ``~/framework/ios/visp3.framework`` that could be added to a Xcode project for iOS devices. If you follow \ref install_iOS_opencv you have also ``~/framework/ios/opencv2.framework`` that should also be added to the Xcode project.
-
-\code
-$ ls ~/framework/ios
-opencv2.framework
-visp3.framework
-\endcode
-
-\section install_iOS_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started-iOS that shows how to use ViSP as a 3rd party to build your own project for iOS devices.
-
-*/
diff --git a/doc/tutorial-install-ios-package.doc b/doc/tutorial-install-ios-package.doc
deleted file mode 100644
index 943e05b..0000000
--- a/doc/tutorial-install-ios-package.doc
+++ /dev/null
@@ -1,40 +0,0 @@
-/**
-
-\page tutorial-install-ios-package Tutorial: Installation from prebuilt packages for iOS devices
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_ios_package_list ViSP framework for iOS
-
-ViSP is available as a prebuilt framework for iOS devices. Go to ViSP <a href="https://visp.inria.fr/download">download page</a> and get \c "ViSP for iOS" as a zip archive. Since this package was build with OpenCV as 3rd party, the zip archive contains also OpenCV framework it was built with to ensure compatibility.
-
-To install ViSP framework in a folder denoted \c \<framework_dir\>/ios that could be set to \c ~/framework/ios download \c "ViSP for iOS" with Safari or Firefox and do the following:
-
-\code
-$ mkdir -p ~/framework/ios
-\endcode
-
-If downloaded with Safari continue with:
-\code
-$ mv ~/Downloads/visp3/* ~/framework/ios/
-\endcode
-or if downloaded with Firefox, run:
-\code
-$ unzip ~/Downloads/visp3.framework*.zip -d ~/framework/ios
-\endcode
-
-The content of \c \<framework_dir\>/ios should be the following:
-\code
-$ ls ~/framework/ios
-opencv2.framework
-visp3.framework
-\endcode
-
-\section install_ios_package_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started-iOS that will show you how to use ViSP as a 3rd party to build your own iOS application.
-
-*/
diff --git a/doc/tutorial-install-opensuse.doc b/doc/tutorial-install-opensuse.doc
deleted file mode 100644
index 682872d..0000000
--- a/doc/tutorial-install-opensuse.doc
+++ /dev/null
@@ -1,307 +0,0 @@
-/**
-
-\page tutorial-install-opensuse Tutorial: Installation from source for Linux openSUSE
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on openSUSE. These steps have been tested for openSUSE 14.1 (x86_64) distribution, but should work with any other distribution as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_opensuse_required Install prerequisities
-
-- gcc 4.4.x or later. This can be installed with:
-\code
-$ sudo zypper install gcc-c++
-\endcode
-- CMake 2.6 or higher that could be installed with:
-\code
-$ sudo zypper install cmake
-\endcode
-- Git if you want to get the source code from http://github.com/lagadic/visp
-\code
-$ sudo zypper install git-core
-\endcode
-
-\section install_opensuse_3rdparty Install 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_opensuse_3rdparty_recommended Recommended 3rd parties
-
-We recommend to install the following:
-
-- OpenCV
-\code
-$ sudo zypper install opencv-devel
-\endcode
-- libX11 to be able to open a window to display images
-\code
-$ sudo zypper install libX11-devel
-\endcode
-- lapack to benefit from optimized mathematical capabilities
-\code
-$ sudo zypper install lapack-devel
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-$ sudo zypper install libv4l-devel
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-$ sudo zypper install libxml2-devel
-\endcode
-
-\subsection install_opensuse_3rdparty_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-
-- libjpeg and libpng to support jpeg and png images (only useful if OpenCV is not installed)
-\code
-$ sudo zypper install libjpeg8-devel libpng-devel
-\endcode
-
-\section install_opensuse_install_visp Install ViSP from source code
-\subsection install_opensuse_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c $HOME/visp
-
-\subsection install_opensuse_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd $HOME; mkdir visp-build
-\endcode
-
-- Enter the \c \<binary_dir\> and configure the build:
-\code
-$ cd $HOME/Vvisp-build
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-ubuntu-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
-
-\subsection install_opensuse_build Building ViSP libraries
-
-- To build ViSP libraries proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsection install_opensuse_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ sudo zypper install doxygen graphviz
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-\section install_opensuse_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_opensuse_tips Tips and tricks
-
-\subsection install_opensuse_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_opensuse_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_opensuse_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_opensuse_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_robot
-... visp_sensor
-... visp_vision
-... visp_vs
-... visp_ar
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_opensuse_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : yes
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : no
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : yes
- Video For Linux Two : no
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : yes
- \-libfreenect : yes
- \-libusb-1.0 : yes
- \-pthread : yes
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : yes
- libpng : yes
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : no
- zbar : no
- dmtx : yes
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_opensuse_issues Known issues
-\subsection install_opensuse_issues_libjpeg libjpeg.so.8 may conflict with libjpeg.so.62
-
-Note that with openSUSE 12.04 but also with 13.02, libjpeg-devel package lead to libjpeg62.so installation, that may conflict with libjpeg8.so that is also installed. That's why we recommend to not install libjpeg-devel, but rather install libjpeg8-devel.
-\code
-Linking CXX executable HelloWorld
-/usr/lib64/gcc/x86_64-suse-linux/4.8/../../../../x86_64-suse-linux/bin/ld: warning: libjpeg.so.8, needed by /usr/lib64/libopencv_highgui.so.2.4.9, may conflict with libjpeg.so.62
-\endcode
-
-\section install_opensuse_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-osx-homebrew.doc b/doc/tutorial-install-osx-homebrew.doc
deleted file mode 100644
index d6e9f33..0000000
--- a/doc/tutorial-install-osx-homebrew.doc
+++ /dev/null
@@ -1,379 +0,0 @@
-/**
-
-\page tutorial-install-osx-homebrew Tutorial: Installation from source for OSX with Homebrew
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on OSX with Homebrew. These steps have been tested with Mac OS X 10.9.5 Mavericks and with 10.10.3 Yosemite.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_osx_brew_required Install prerequisities
-
-First, go to http://brew.sh to install Homebrew.
-
-Next use homebrew to install additional software.
-
-\code
-$ brew update
-$ brew install cmake
-\endcode
-
-You will need to add the next line to your \c ~/.bashrc or \c ~/.bash_profile to have Homebrew be at the front of the PATH.
-\code
-export PATH=/usr/local/bin:$PATH
-\endcode
-
-In order for the above changes to take effect reopen the terminal or run this command:
-\code
-$ source ~/.bashrc
-\endcode
-
-
-\section install_osx_brew_3rdparty Install 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_osx_brew_3rdparty_recommended Recommended 3rd party
-
-We recommend to install the following packages.
-
-\subsubsection install_osx_brew_opencv OpenCV 3rd party
-
-\code
-$ brew install homebrew/science/opencv3
-\endcode
-
-You can find OpenCV at:
-
-\code
-/usr/local/opt/opencv3
-\endcode
-
-Now to indicate to CMake where OpenCV is installed you will need to add the following line in your \c ~/.bashrc or \c ~/.bash_profile
-\code
-export OpenCV_DIR=/usr/local/opt/opencv3/share/OpenCV
-\endcode
-Note that \c OpenCV_DIR var gives now the location of OpenCVConfig.cmake file.
-
-In order for the above changes to take effect reopen the terminal or run this command:
-\code
-$ source ~/.bashrc
-\endcode
-
-\subsubsection install_osx_brew_libxml2 libxml2 3rd party
-
-Just run the following instruction:
-\code
-$ brew install libxml2
-\endcode
-
-\subsubsection install_osx_brew_libdc1394 libdc1394 3rd party
-
-Just run the following instruction:
-\code
-$ brew install libdc1394
-\endcode
-
-\subsubsection install_osx_brew_zbar zbar 3rd party
-
-To install the zbar library used in detection module for QR code detection, run:
-\code
-$ brew install zbar
-\endcode
-
-\subsection install_osx_brew_3rdparty_other Other 3rd party
-
-\subsubsection install_osx_brew_gsl GSL 3rd party
-
-If lapack 3rd party is not detected during CMake configuration it may be useful to install the Gnu Scientific Library (GSL) to benefit from optimized mathematical capabilities. To this end run the following instruction:
-\code
-$ brew install gsl
-\endcode
-
-\section install_osx_brew_install_visp Install ViSP from source code
-\subsection install_osx_brew_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> repository using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c $HOME/visp
-
-\subsection install_osx_brew_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd $HOME; mkdir visp-build
-\endcode
-
-- Enter the \c \<binary_dir\> and configure the build:
-\code
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The previous command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd party that will be used.
-Then to generate the makefiles, just press [g] key in the ccmake gui.
-Now we can build ViSP.
-
-\subsection install_osx_brew_build Building ViSP libraries
-
-- To build ViSP proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsection install_osx_brew_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ brew install doxygen
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-
-\section install_brew_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_brew_tips Tips and tricks
-
-\subsection install_brew_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_brew_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_brew_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_brew_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_gui
-... visp_io
-... visp_klt
-... visp_me
-... visp_robot
-... visp_sensor
-... visp_ar
-... visp_blob
-... visp_visual_features
-... visp_vs
-... visp_vision
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_brew_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : yes
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : yes
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : yes
- Video For Linux Two : no
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : yes
- \-libfreenect : yes
- \-libusb-1.0 : yes
- \-pthread : yes
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : yes
- libpng : yes
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : no
- zbar : no
- dmtx : yes
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_osx_brew_issues Kwown issues
-\subsection install_osx_brew_issues_opencv3 /usr/local/lib/pkgconfig is not writable
-
-- During OpenCV installation, if you get the following errors:
-\code
-$ brew install opencv3
-Could not symlink lib/pkgconfig/isl.pc
-/usr/local/lib/pkgconfig is not writable.
-
-You can try again using:
- brew link isl
-...
-\endcode
-it means maybe that you install other softwares without brew in /usr/local.
-A work arround is to change the owner of the corresponding folder like:
-\code
-$ sudo chown {your-user-name} /usr/local/lib/pkgconfig
-\endcode
-
-- If you enter into troubles with the packages you install with brew, a good stating is to run:
-\code
-$ brew doctor
-\endcode
-
-\subsection install_osx_brew_issues_libpng Application built with libpng-1.5.18 but running with 1.6.17
-- If you encounter the following issue
-\code
-$ ./modules/vision/testKeypoint-5
-libpng warning: Application built with libpng-1.5.18 but running with 1.6.17
-error: can't create a png read structure!
-error reading png file
-\endcode
-It means that apparently there is a conflict between libpng version installed by "brew install opencv3" (1.6.17), and the one used by X11/XQuartz (1.5.18).
-A work arround is to turn off libpng usage in ViSP. To configure and build again ViSP without png support:
-\code
-$ ccmake -DUSE_PNG=OFF ../ViSP
-$ make -j4
-\endcode
-An other work arround option is to turn off X11 usage in ViSP. Display capabilities will be then the one from OpenCV. To this end, configure and build again ViSP without X11 support:
-\code
-$ ccmake -DUSE_X11=OFF ../ViSP
-$ make -j4
-\endcode
-
-\section install_osx_brew_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-raspberry.doc b/doc/tutorial-install-raspberry.doc
deleted file mode 100644
index 9def103..0000000
--- a/doc/tutorial-install-raspberry.doc
+++ /dev/null
@@ -1,387 +0,0 @@
-/**
-
-\page tutorial-install-raspberry Tutorial: Installation from source for Raspberry Pi
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on <a href="http://www.raspberrypi.org/help/what-is-a-raspberry-pi">Raspberry Pi 1 or Pi 2</a> equipped with an optional HD camera module.
-
-\image html img-raspberry-pi.jpg
-
-In a first section we give some useful instructions to start with a Raspberry PI. Then in the second section, we focus on the installation of ViSP from source.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_raspberry Raspberry Pi installation
-
-\subsection install_raspberry_setup Setting up Raspberry Pi
-
-There are a lot of documentation and tutorial that explain different ways to setup a Raspberry Pi. A good reference is the official page <a href="http://www.raspberrypi.org/">http://www.raspberrypi.org</a>
-
-We suggest to start with NOOBS (New Out Of the Box Software). Bellow we resume the different steps that are fully described in <a href="http://www.raspberrypi.org/help/noobs-setup/">http://www.raspberrypi.org/help/noobs-setup</a>:
-
-- Format you SD card (at least 4Go are requested) on your computer. To this end you can use "GParted partition editor" under Ubuntu.
-- Download Noobs "Offline and network install" zip file from <a href="http://www.raspberrypi.org/downloads/">http://www.raspberrypi.org/downloads</a>.
-- Unzip all the files on the SD card.
-- Insert your SD Card into your Raspberry Pi.
-- Plug in your keyboard, mouse, monitor and Ethernet cables.
-- Now plug in the USB power cable to your Pi.
-- Your Raspberry Pi will boot, and a window will appear with a list of different operating systems that you can install. Select the "recommended Raspbian" checkbox and click on "Install" button.
-- When the install process has completed, the Raspberry Pi configuration menu (raspi-config) will load. Here you are able to set the time and date for your region and enable a Raspberry Pi camera board, or even create users. You can exit this menu by using Tab on your keyboard to move to Finish.
-
-\subsection install_raspberry_login Logging in
-
-The default login for Raspbian is username \c pi with the password \c raspberry.
-
-\subsection update Update your Raspbian distribution
-
-If your Raspberry Pi is connected to Ethernet you can update your Raspbian distribution:
-\code
-$ sudo apt-get update
-$ sudo apt-get upgrade
-\endcode
-
-\subsection install_raspberry_camera Setting up a Raspberry Pi camera
-
-If you have a Raspberry Pi camera module see <a href="http://www.raspberrypi.org/help/camera-module-setup/">http://www.raspberrypi.org/help/camera-module-setup</a>. To resume, enable the camera using:
-\code
-$ sudo raspi-config
-\endcode
-Enter in menu "5/ Interfacing Options" to enable the camera.
-
-Do a rebooot
-\code
-$ sudo reboot
-\endcode
-
-Connect again and load the camera module:
-\code
-$ sudo modprobe bcm2835-v4l2
-\endcode
-
-This will add the following modules:
-
-\code
-$ lsmod
-Module Size Used by
-bcm2835_v4l2 37611 0
-videobuf2_core 30853 1 bcm2835_v4l2
-v4l2_common 7792 1 bcm2835_v4l2
-videodev 121362 3 bcm2835_v4l2,v4l2_common,videobuf2_core
-\endcode
-
-To check if the camera is recognized and connected, run:
-\code
-$ v4l2-ctl --list-formats
-\endcode
-
-After each Raspberry Pi reboot you need to relaunch the previous modprobe command. To load bcm2835-v4l2 module during reboot, you can edit \c /etc/modules file
-\code
-$ sudo nano /etc/modules
-\endcode
-
-and add a line with the name of the module:
-\code
-bcm2835-v4l2
-\endcode
-
-
-\subsection install_raspberry_startx Start graphical user interface
-
-To load the graphical user interface, type \c startx and press Enter on your keyboard. This will later allow to use ViSP vpDisplayX or vpDisplayOpenCV classes useful to display images in a X11 window.
-
-\section install_raspberry_visp ViSP installation
-\subsection install_raspberry_visp_prerequisities Install prerequisities
-
-First you need to install the following packagages (g++, CMake, Git) that are requested to get and build ViSP:
-\code
-$ sudo apt-get install build-essential cmake-curses-gui git
-\endcode
-
-\subsection install_raspberry_visp_3rd_party Install 3rd parties
-
-ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsubsection install_raspberry_visp_3rd_party_recommended Recommended 3rd parties
-
-We recommend to install the following:
-
-- OpenCV
-\code
-$ sudo apt-get install libopencv-dev
-\endcode
-- libX11 to be able to open a window to display images
-\code
-$ sudo apt-get install libx11-dev
-\endcode
-- lapack to benefit from optimized mathematical capabilities
-\code
-$ sudo apt-get install liblapack-dev
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-$ sudo apt-get install libv4l-dev
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-$ sudo apt-get install libxml2-dev
-\endcode
-- QR code detection
-\code
-$ sudo apt-get install libzbar-dev
-\endcode
-
-\subsubsection install_raspberry_visp_3rd_party_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-
-- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
-\code
-$ sudo apt-get install libjpeg-dev libpng12-dev
-\endcode
-
-\subsection install_raspberry_visp_install Install ViSP from source code
-\subsubsection install_raspberry_visp_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code on Raspberry Pi:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ mkdir /home/pi/soft; cd /home/pi/soft
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ mkdir /home/pi/soft; cd /home/pi/soft
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c /home/pi/soft/visp
-
-\subsubsection install_raspberry_visp_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd /home/pi/soft; mkdir visp-build
-\endcode
-
-- Enter \c \<binary_dir\> and configure the build:
-\code
-$ cd /home/pi/soft/visp-build
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. Then to generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-ubuntu-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
-
-
-\subsubsection install_raspberry_visp_build Building ViSP libraries
-
-- To build ViSP proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsubsection install_raspberry_visp_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ sudo apt-get install doxygen graphviz texlive-latex-base
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-\subsection install_raspberry_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_raspberry_tips Tips and tricks
-
-\subsection install_raspberry_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_raspberry_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_raspberry_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_raspberry_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_gui
-... visp_io
-... visp_klt
-... visp_me
-... visp_robot
-... visp_sensor
-... visp_ar
-... visp_blob
-... visp_visual_features
-... visp_vs
-... visp_vision
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_raspberry_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : no
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : yes
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : no
- Video For Linux Two : yes
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : no
- \-libfreenect : no
- \-libusb-1.0 : no
- \-pthread : no
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : yes
- libpng : yes
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : no
- zbar : yes
- dmtx : no
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_raspberry_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project on Raspberry Pi or on any other system. Then if you have a Raspberry Pi camera module, you can also follow the \ref tutorial-tracking-blob especially subsection Tracking form v4l2 live cameras.
-
-You may also be interested in \ref tutorial-install-crosscompiling-raspberry if you want to speed up the build process or deploy ViSP on other Rapberry boards.
-*/
diff --git a/doc/tutorial-install-ubuntu-package.doc b/doc/tutorial-install-ubuntu-package.doc
deleted file mode 100644
index cbeb9d6..0000000
--- a/doc/tutorial-install-ubuntu-package.doc
+++ /dev/null
@@ -1,36 +0,0 @@
-/**
-
-\page tutorial-install-ubuntu-package Tutorial: Installation from prebuilt packages for Linux Ubuntu/Debian
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from prebuilt official packages for Linux Ubuntu or Debian.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_ubuntu_package_list ViSP official packages
-
-Since Ubuntu 14.04 LTS, ViSP library can be installed from <a href="http://packages.ubuntu.com/search?keywords=visp&searchon=names&suite=trusty§ion=all">existing packages</a> on amd64 or i386 architectures. In Ubuntu 13.10, the packages were only available for amd64 architecture.
-
-To install ViSP library and headers just run:
-
-\code
-$ sudo apt-get install libvisp-dev
-\endcode
-
-\note This package comes build with 3rd parties such as OpenCV, X11, xml2...
-
-To install ViSP images data set useful to run ViSP examples you can run:
-\code
-$ sudo apt-get install visp-images
-\endcode
-
-To install ViSP html documentation you can run:
-\code
-$ sudo apt-get install visp-doc
-\endcode
-
-\section install_ubuntu_package_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-ubuntu.doc b/doc/tutorial-install-ubuntu.doc
deleted file mode 100644
index 5c27666..0000000
--- a/doc/tutorial-install-ubuntu.doc
+++ /dev/null
@@ -1,326 +0,0 @@
-/**
-
-\page tutorial-install-ubuntu Tutorial: Installation from source for Linux Ubuntu
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Linux Ubuntu. These steps have been tested for Ubuntu 14.04 (64 bit) distribution, but should work with any other distribution as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_ubuntu_required Install prerequisities
-
-- gcc 4.4.x or later. This can be installed with:
-\code
-$ sudo apt-get install build-essential
-\endcode
-- CMake 2.6 or higher that could be installed with:
-\code
-$ sudo apt-get install cmake-curses-gui
-\endcode
-
-\section install_ubuntu_3rdparty Install 3rd parties
-
-ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_ubuntu_3rdparty_recommended Recommended 3rd parties
-
-We recommend to install the following:
-
-- OpenCV
-\code
-$ sudo apt-get install libopencv-dev
-\endcode
-- libX11 to be able to open a window to display images
-\code
-$ sudo apt-get install libx11-dev
-\endcode
-- lapack to benefit from optimized mathematical capabilities
-\code
-$ sudo apt-get install liblapack-dev
-\endcode
-- libdc1394 to grab images from firewire cameras
-\code
-$ sudo apt-get install libdc1394-22-dev
-\endcode
-- libv4l to grab images from usb or analogic cameras
-\code
-$ sudo apt-get install libv4l-dev
-\endcode
-- libxml2 to be able to configure the model-based trackers from xml files
-\code
-$ sudo apt-get install libxml2-dev
-\endcode
-- QR code detection
-\code
-$ sudo apt-get install libzbar-dev
-\endcode
-
-\subsection install_ubuntu_3rdparty_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-
-- Coin, to be able to support vrml cad model used by the model-based trackers
-\code
-$ sudo apt-get install libcoin80-dev
-\endcode
-- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
-\code
-$ sudo apt-get install libjpeg-dev libpng12-dev
-\endcode
-- ffmpeg, to be able to read or encode compressed video streams (only useful if OpenCV is not installed)
-\code
-$ sudo apt-get install libswscale-dev libavutil-dev libavformat-dev libavcodec-dev libbz2-dev libbz2-1.0
-\endcode
-- Ogre 3D if you want to do augmented reality or simulation
-\code
-$ sudo apt-get install libogre-1.9-dev libois-dev
-\endcode
-- Datamatrix code detection
-\code
-$ sudo apt-get install libdmtx-dev
-\endcode
-
-\section install_ubuntu_visp Install ViSP from source code
-\subsection install_ubuntu_visp_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
-\code
-$ tar xvzf visp-x.y.z.tar.gz
-\endcode
-or
-\code
-$ unzip visp-x.y.z.zip
-\endcode
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
-\code
-$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz
-\endcode
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
-\code
-$ git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c $HOME/visp
-
-\subsection install_ubuntu_visp_config Configuring ViSP from source
-
-- Create first a directory denoted \c \<binary_dir\> where you want to build ViSP. This directory will contain generated Makefiles, object files, and output libraries and binaries.
-\code
-$ cd $HOME; mkdir visp-build
-\endcode
-
-- Enter the \c \<binary_dir\> and configure the build:
-\code
-$ cd $HOME/visp-build
-$ cmake ../visp
-\endcode
-A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
-\code
-$ ccmake ../visp
-\endcode
-The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned On/Off. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
-\image html img-ccmake-ubuntu-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
-
-Now we can build ViSP.
-
-\subsection install_ubuntu_visp_build Building ViSP libraries
-
-- To build ViSP libraries proceed with:
-\code
-$ make -j4
-\endcode
-
-- To install ViSP in \c /usr/local which is the default install location, proceed with:
-\code
-$ sudo make install
-\endcode
-\note This stage is optional, since ViSP could be used as a 3rd party without installation.
-\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var.
-
-\subsection install_ubuntu_visp_doc Building ViSP documentation
-
-- To build ViSP documentation, you have first to install Doxygen package:
-\code
-$ sudo apt-get install doxygen graphviz texlive-latex-base
-\endcode
-Then you can proceed with:
-\code
-$ cmake ../visp
-$ make -j4 visp_doc
-\endcode
-The generated documentation is then available in \c \<binary_dir\>/doc/html/index\.html
-
-- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC=ON like:
-\code
-$ cmake ../visp -DENABLE_FULL_DOC=ON
-$ make -j4 visp_doc
-\endcode
-
-\section install_ubuntu_dataset Install ViSP dataset
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-\code
-$ cd $HOME
-$ unzip ViSP-images-x.y.z.zip
-\endcode
-
-We suppose now that the data are located in \c $HOME/ViSP-images.
-\code
-$ ls $HOME/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It's convenient if the environment variables is automatically added to your bash session every time a new shell is launched:
-
-\code
-$ echo "export VISP_INPUT_IMAGE_PATH=$HOME" >> ~/.bashrc
-$ source ~/.bashrc
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
-
-\code
-$ cd $HOME/visp-build
-$ ./example/device/display/displayX
-
-A click to close the windows...
-
-A click to display a cross...
-Cross position: 201, 441
-
-A click to exit the program...
-Bye
-\endcode
-
-\section install_ubuntu_tips Tips and tricks
-
-\subsection install_ubuntu_tips_uninstall How to uninstall ViSP
-After ViSP installation, you can remove installed material using:
-\code
-$ sudo make uninstall
-\endcode
-
-\subsection install_ubuntu_tips_modules How to build only ViSP libraries
-
-If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
-\code
-$ make -j4 visp_modules
-\endcode
-
-\subsection install_ubuntu_tips_module_once How to build a ViSP specific module
-
-If you want to build a given module and all the dependencies:
-\code
-$ make -j4 visp_<module_name>
-\endcode
-For example to build the model-based tracker module named mbt, run:
-\code
-$ make -j4 visp_mbt
-\endcode
-
-\subsection install_ubuntu_tips_target Which are the targets that could be run with make ?
-
-To know which are the target available with \c make:
-\code
-$ make help | grep visp
-... visp_tests
-... visp_demos
-... visp_tutorials
-... visp_examples
-... visp_modules
-... visp_doc
-... visp_core
-... visp_detection
-... visp_gui
-... visp_io
-... visp_klt
-... visp_me
-... visp_robot
-... visp_sensor
-... visp_ar
-... visp_blob
-... visp_visual_features
-... visp_vs
-... visp_vision
-... visp_mbt
-... visp_tt
-... visp_tt_mi
-\endcode
-
-\subsection install_ubuntu_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
-
-To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c \<binary_dir\>. We provide hereafter an example of a possible content of this file:
-\code
- ViSP third-party libraries
-
-Below you will find the list of third party libraries used to
-build ViSP on your computer.
-
-Mathematics:
- Gnu Scientific Library : no
- Lapack/blas : yes
-Simulator:
- Ogre simulator : no
- \- Ogre3D : no
- \- OIS : no
- Coin simulator : no
- \- Coin3D : no
- \- SoWin : no
- \- SoXt : no
- \- SoQt : no
- \- Qt4 : no
- \- Qt3 : no
-Robots
- Afma6 : no
- Afma4 : no
- Biclops : no
- Ptu46 : no
- Pioneer : no
- Viper S650 : no
- Viper S850 : no
-Video devices (display)
- X11 : yes
- GTK : no
- OpenCV : yes
- GDI : no
- Direct3D : no
-Framegrabbers
- Firewire libdc1394-2.x : yes
- Video For Linux Two : yes
- DirectShow : no
- CMU 1394 Digital Camera SDK : no
- OpenCV : yes
-Specific devices
- Yarp : no
- Kinect : no
- \-libfreenect : no
- \-libusb-1.0 : no
- \-pthread : yes
-Video and image Read/Write:
- FFMPEG : no
- libjpeg : no
- libpng : no
-Misc:
- XML2 : yes
- pthread : yes
- OpenMP : yes
- zbar : yes
- dmtx : no
-Documentation:
- Doxygen : no
- Graphviz dot : no
-ViSP built with C++11 features: no
-\endcode
-
-\section install_ubuntu_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-win10-msvc14-uwp.doc b/doc/tutorial-install-win10-msvc14-uwp.doc
deleted file mode 100644
index 5d1232f..0000000
--- a/doc/tutorial-install-win10-msvc14-uwp.doc
+++ /dev/null
@@ -1,147 +0,0 @@
-/**
-
-\page tutorial-install-win10-msvc14-uwp Tutorial: Installation from source for Universal Windows Platform (UWP)
-\tableofcontents
-
-In this tutorial you will learn how to build ViSP framework for Universal Windows Platform (UWP) to include ViSP functionalities in apps developments that target a wide range of devices including PC, mobile, Xbox, HoloLens, IoT, and Surface Hub.
-
-This tutorial has been tested on Windows 10 (64 bit), with CMake 3.7.1 and Visual Studio Community 2015 and is only working since ViSP 3.0.1.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section tutorial-install-uwp_required Install required software
-
-- CMake last version that could be download at http://www.cmake.org
-- <a href="https://www.visualstudio.com/downloads/">Visual Studio Community 2015</a>. Make sure that the Universal Windows App Development Tools are selected from the optional features list. Without these tools, you won't be able to create your universal apps. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2015.
-
-\note If you have already Visual Stodio 2015 installed and don't know if the Universal Windows App Development Tools are enabled see \ref install_uwp_tips_msvc.
-
-After installing Visual Studio software, you need to <a href="https://msdn.microsoft.com/windows/uwp/get-started/enable-your-device-for-development">enable your Windows 10 device</a> for development.
-
-
-\section install_uwp_get_visp_source Get ViSP source code
-
-There are different ways to get ViSP source code.
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file.
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file.
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
-\code
-C:\ViSP> git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp.
-
-Next we suggest to first build ViSP without OpenCV.
-
-\section install_uwp_build_without_opencv Build ViSP without OpenCV
-
-\subsection install_uwp_build_ws_10 Build for Windows Store 10.0
-
-- Configure ViSP build for Windows Store 10 targeting a x86 platform, by opening a cmd terminal and running:
-\code
-$ cd C:\ViSP
-$ md visp-build-msvc14-uwp-ws-10
-$ cd visp-build-msvc14-uwp-ws-10
-$ cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
-\endcode
-
-- To build ViSP just double click on \c C:/ViSP/visp-build-msvc14-uwp-ws-10/VISP.sln solution file. This action will open ViSP project in Visual Studio C++.
-
-- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in \c C:\\ViSP\\visp-build-msvc14-uwp-ws-10\\install folder.
-
-\subsection install_uwp_build_ws_8_1 Build for Windows Store 8.1
-
-- Configure ViSP build for Windows Store 8.1 targeting a x86 platform, by opening a cmd terminal and running:
-\code
-$ cd C:\ViSP
-$ md visp-build-msvc14-uwp-ws-8.1
-$ cd visp-build-msvc14-uwp-ws-8.1
-$ cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="8.1" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
-\endcode
-
-- To build ViSP just double click on \c C:/ViSP/visp-build-msvc14-uwp-ws-8.1/VISP.sln solution file. This action will open ViSP project in Visual Studio C++.
-
-- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in \c C:\\ViSP\\visp-build-msvc14-uwp-ws-8.1\\install folder.
-
-\subsection install_uwp_build_wp_8_1 Build for Windows Phone 8.1
-
-- Configure ViSP build for Windows Phone 8.1 targeting a x86 platform, by opening a cmd terminal and running:
-\code
-$ cd C:\ViSP
-$ md visp-build-msvc14-uwp-wp-8.1
-$ cd visp-build-msvc14-uwp-wp-8.1
-$ cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsPhone -DCMAKE_SYSTEM_VERSION:String="8.1" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
-\endcode
-
-- To build ViSP just double click on \c C:/ViSP/visp-build-msvc14-uwp-wp-8.1/VISP.sln solution file. This action will open ViSP project in Visual Studio C++.
-
-- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in \c C:\\ViSP\\visp-build-msvc14-uwp-wp-8.1\\install folder.
-
-Now if you want that ViSP benefits from OpenCV functionalities, you can follow the next section.
-
-\section install_uwp_build_with_opencv Build ViSP with OpenCV
-
-\note The following steps are given for Windows Store 10.0 targetting a x86 platform. They need to be adapted to your use case.
-
-Building ViSP with OpenCV as 3rd party needs first to succeed in building OpenCV for UWP. As of writing on January 2017, the way is to use Microsoft fork <a href="https://github.com/Microsoft/opencv">https://github.com/Microsoft/opencv</a> and branch \c vs2015-samples.
-
-- Get OpenCV source code
-\code
-$ cd C:\OpenCV
-$ git clone https://github.com/Microsoft/opencv.git
-$ cd opencv
-$ git checkout vs2015-samples
-\endcode
-
-- Download <a href="http://visp-doc.inria.fr/download/opencv/patch/opencv-microsoft-branch-vs2015-samples-patch.diff">opencv-microsoft-branch-vs2015-samples-patch.diff</a> patch and save it in \c C:\\OpenCV folder. This patch allows to add \c /WZ compiler option when building highgui and videoio modules.
-- Modify the source code applying the patch
-\code
-$ git apply --ignore-space-change --ignore-whitespace ..\opencv-microsoft-branch-vs2015-samples-patch.diff
-\endcode
-
-- Configure OpenCV build. The following command is for Windows Store 10.0 targetting a x86 platform but could be easily adapted:
-\code
-$ cd C:\OpenCV
-$ md opencv-build-msvc14-uwp-ws-10.0
-$ cd opencv-build-msvc14-uwp-ws-10.0
-$ cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 -DBUILD_opencv_ts=OFF ../opencv
-\endcode
-
-- Open \c C:/OpenCV/opencv-build-msvc14-uwp-ws-10/opencv.sln file with Visual Studio 2015, build and install all the solution
-
-- If everything goes right you will find OpenCV libraries and headers in \c C:\\OpenCV\\opencv-build-msvc14-uwp-ws-10\\install folder.
-
-Now you can use OpenCV as third-party and configure ViSP in order to use OpenCV. To this end modify the cmake command that was used to \ref install_uwp_build_without_opencv by adding \c -DOpenCV_DIR=C:\\OpenCV\\opencv-build-msvc14-uwp-ws-10\\install option to cmake command line:
-
-\code
-$ cd C:\ViSP\visp-build-msvc14-uwp-ws-10
-$ cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 -DOpenCV_DIR=C:/OpenCV/opencv-build-msvc14-uwp-ws-10/install ../visp
-\endcode
-
-As described in \ref install_uwp_build_without_opencv, open again ViSP solution file, build and install ViSP.
-
-\section install_uwp_tips Tips and tricks
-\subsection install_uwp_tips_msvc How to add Universal Windows App Development Tools support
-
-In case you're trying to build Windows Store and Windows Phone 10.0 projects having only default Visual Studio 2015 installation you'll likely get the following error during \c cmake run:
-\code
-A Windows Store component with CMake requires both the Windows Desktop SDK
-as well as the Windows Store '10.0' SDK. Please make sure that you have
-both installed
-\endcode
-
-Resolution consists in:
-- On the Start menu select Settings
-- Select System > Apps & features.
-- Select Microsoft Visual Studio 2015 program, and right click to modify installation
-\image html img-uwp-msvc-modify.png
-- Select Modify
-\image html img-uwp-msvc-modify2.png
-- Enable "Universal Windows App Development Tools" and "Windows 8.1 and Windows Phone 8.0/8.1 Tools"
-\image html img-uwp-msvc-modify3.png
-- Select Update button
-
-*/
diff --git a/doc/tutorial-install-win10-msvc14.doc b/doc/tutorial-install-win10-msvc14.doc
deleted file mode 100644
index d255038..0000000
--- a/doc/tutorial-install-win10-msvc14.doc
+++ /dev/null
@@ -1,174 +0,0 @@
-/**
-
-\page tutorial-install-win10-msvc14 Tutorial: Installation from source for Windows 10 with Visual C++ 2015
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++. These steps have been tested on Windows 10 (64 bit), with CMake 3.3.2 and Visual Studio Community 2015.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_win10_msvc14_required Install prerequisities
-
-- Visual C++. Visual Studio Community 2015 that includes Visual C++ could be downloaded from https://www.visualstudio.com. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2015.
-- CMake that could be download at : http://www.cmake.org.
-\warning We notice that CMake 3.4.1 was not able to detect Visual C++ 2015. This issue is kwown by CMake developers and registered as a bug https://cmake.org/Bug/view.php?id=15831. That is why we suggest to install CMake 3.3.2 to work with Visual C++ 2015.
-
-To install CMake 3.3.2, download the previous release binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-3.3.2-win32-x86.exe you downloaded.
-- Windows Software Developement Kit (SDK) for Windows 10. This SDK could be downloaded from https://dev.windows.com/en-US/downloads/windows-10-sdk. This SDK is requested by CMake and allows also to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
-
-\section install_win10_msvc14_3rdparty Install 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_win10_msvc14_3rdparty_recommended Recommended 3rd parties
-We recommend to install OpenCV 3rd party. Other 3rd parties should be considered only by expert developers.
-
-\subsubsection install_win10_msvc10_3rdparty_opencv OpenCV 3rd party
-
-- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.1.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.1.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
-\note OpenCV 3.1.0 windows installer contains pre-build libraries compatible with Visual C++ 2015 (in \c C:\\OpenCV\\opencv\\build\\x64\\vc14) and also libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x64\\vc12). With any other compiler version you need to build yourself OpenCV from source.
-
-\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
-where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc14_env_var.
-
-\section install_win10_msvc14_install_visp Install ViSP from source code
-\subsection install_win10_msvc14_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code.
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file.
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file.
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
-\code
-C:\ViSP> git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp
-
-\subsection install_win10_msvc14_config Configuring ViSP from source
-
-The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \c \<binary_dir\>, for example \c C:/ViSP/visp-build.
-
-- Launch CMake (cmake-gui) and complete the \c \<source_dir\> and \c \<binary_dir\> locations as in the next image.
-
-\image html img-cmake-win10-msvc-launch.png
-
-- Click then on "Configure" button.
-
-\image html img-cmake-win10-create-build-folder.png
-
-- Click on "Yes" to create the \c C:/ViSP/visp-build folder.
-- Select then your compiler, for example here Visual Studio Express 2013 Win64, and click on "Finish" button.
-
-\image html img-cmake-win10-msvc-version.png
-
-- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV and OpenMP 3rd parties are automatically detected.
-
-\image html img-cmake-win10-config.png
-
-- As given in the previous image, note also that the installation folder is set to \c C:/ViSP/visp-build/install.
-\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
-
-- Click then on "Configure" button. All the red lines should disappear.
-\image html img-cmake-win10-config-end.png
-\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-
-- To finish the configuration, click on "Generate" button.
-
-\image html img-cmake-win10-msvc-generate.png
-
-- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
-
-\image html img-win8.1-msvc-solution.jpg
-
-\subsection install_win10_msvc14_build Building ViSP from source
-
-- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
-
-\image html img-win10-msvc-open.png
-
-- Enter menu "BUILD/Build Solution" to build ViSP.
-
-\image html img-win10-msvc-build.jpeg
-
-- At the end of the build process you should have the following indicating that all the build succeeded.
-
-\image html img-win10-msvc-build-end.png
-
-- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
-
-\image html img-win10-msvc-install.jpeg
-
-- At the end of the installation, you should have the following.
-
-\image html img-win10-msvc-install-end.png
-
-- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
-
-- This ends ViSP installation with \c Debug configuration.
-
-- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
-
-\image html img-win10-msvc-release.jpeg
-
-- Now, as previously, build and install ViSP again.
-
-- At the end, in \c C:/ViSP/visp-build/install/x64/vc14/bin folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
-
-\image html img-win10-explorer-install-end.png
-
-\subsection install_win10_msvc14_env_var Setting up PATH variable
-
-If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
-
-To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc14\bin"
-\endcode
-
-Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc14\bin"
-\endcode
-
-Then close and re-open a cmd terminal to check if the PATH var was well positioned.
-
-\code
-C:\> echo %PATH%
-\endcode
-
-\section install_win10_msvc14_dataset Install ViSP dataset
-
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
-
-\image html img-win8.1-visp-images.jpg
-
-ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
-\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
-
-\image html img-win8.1-cmd-displayGDI.jpg
-
-\section install_win10_msvc14_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-win7.doc b/doc/tutorial-install-win7.doc
deleted file mode 100644
index 2e8571f..0000000
--- a/doc/tutorial-install-win7.doc
+++ /dev/null
@@ -1,185 +0,0 @@
-/**
-
-\page tutorial-install-win7 Tutorial: Installation from source for Windows 7 with Visual C++ 2012
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Windows with Visual C++. These steps have been tested on Windows 7 (64 bit), with CMake 3.3.2 and Visual Studio 2012 but should work with any other version as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_win7_required Install prerequisities
-
-- Visual Studio C++. In this tutorial we use Visual Studio 2012. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products
-\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
-- CMake 2.8.3 or higher that could be download at : http://www.cmake.org. To install CMake, download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-x.y.z-win32-x86.exe you downloaded.
-
-
-\section install_win7_3rdparty Install recommended 3rd party
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install the following:
-
-\subsection install_win7_3rdparty_opencv OpenCV 3rd party
-
-We recommend to install OpenCV.
-
-- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
-\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc12 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc12 for the Win64 compiler) and also libraries compatible with Visual C++ 2012 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc11 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc11 for the Win64 compiler). With any other compiler version you need to build yourself OpenCV from source.
-
-\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
-where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win7_env_var.
-
-\section install_win7_install_visp Install ViSP from source code
-\subsection install_win7_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code:
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
-\code
-C:\ViSP> git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp
-
-\subsection install_win7_config Configuring ViSP from source
-
-The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \c \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
-
-- Launch CMake (cmake-gui) and complete the \c \<source_dir\> and \c \<binary_dir\> locations as in the next image.
-
-\image html img-cmake-win7-msvc-launch.jpg
-
-- Click then on "Configure" button.
-
-\image html img-cmake-win7-create-build-folder.jpg
-
-- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
-- Select then your compiler, for example here Visual Studio 11 Win64, and click on "Finish" button.
-
-\image html img-cmake-win7-msvc-version.jpg
-
-- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV and OpenMP 3rd parties are automatically detected.
-
-\image html img-cmake-win7-msvc-config.jpg
-
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
-\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
-
-- Click then on "Configure" button. All the red lines should disappear.
-\image html img-cmake-win7-msvc-config-end.jpg
-\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-
-- To finish the configuration, click on "Generate" button.
-
-\image html img-cmake-win7-generate.jpg
-
-- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
-
-\image html img-cmake-win7-solution.jpg
-
-\subsection install_win7_build Building ViSP from source
-
-- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
-
-\image html img-win7-msvc-open.jpg
-
-- Enter menu "BUILD/Build Solution" to build ViSP.
-
-\image html img-win7-msvc-build.jpg
-
-- At the end of the build process you should have the following indicating that all the build succeeded.
-
-\image html img-win7-msvc-build-succeed.jpg
-
-- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
-
-\image html img-win7-msvc-install.jpg
-
-- At the end of the installation, you should have the following.
-
-\image html img-win7-msvc-install-succeed.jpg
-
-- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
-
-- This ends ViSP installation with \c Debug configuration.
-
-- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
-
-\image html img-win7-msvc-release.jpg
-
-- Now, as previously, build and install ViSP again.
-
-- At the end, in \c C:/ViSP/visp-build/install/bin folder you have two versions of ViSP DLL libraries for each module; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
-
-\image html img-win7-msvc-install-end.jpg
-
-\subsection install_win7_env_var Setting up PATH variable
-
-If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
-
-To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc11\bin"
-\endcode
-
-Then to add the path to OpenCV 3rd party libraries, close and re-open a cmd-terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc11\bin"
-\endcode
-
-Then close and re-open a cmd terminal to check if the PATH var was well positioned
-
-\code
-C:\> echo %PATH%
-\endcode
-
-
-\section install_win7_dataset Install ViSP dataset
-
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
-
-\image html img-win7-ViSP-images.jpg
-
-We suppose now that the data are located in \c C:/ViSP/ViSP-images.
-\code
-C:\> dir ViSP/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
-\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
-
-\image html img-win7-cmd-displayGDI.jpg
-
-\section install_win7_tip Tip and tricks
-
-\subsection install_win7_tip_git How to install Git
-
-We recommand to install Git for Windows from https://git-for-windows.github.io/. This installation allows then to use git in a cmd.exe command dos shell.
-
-\section install_win7_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-win8.1-mingw-w64.doc b/doc/tutorial-install-win8.1-mingw-w64.doc
deleted file mode 100644
index b9717b5..0000000
--- a/doc/tutorial-install-win8.1-mingw-w64.doc
+++ /dev/null
@@ -1,275 +0,0 @@
-/**
-
-\page tutorial-install-win81-mingw64 Tutorial: Installation from source for Windows 8.1 or Windows 10 with Mingw-w64
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.3.2 and Mingw-w64 - GCC for Windows 64 & 32 bits. It should also work under Windows 10.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_win81_mingw64_required Install prerequisities
-
-\subsection install_win81_mingw64_required_mingw Mingw-w64
-
-Mingw-w64 could be found following http://mingw-w64.org/doku.php/download From that page, click on download "Win-builds" that will bring you to the page http://mingw-w64.org/doku.php/download/win-builds from where you can access to http://win-builds.org/doku.php/download_and_installation_from_windows Here simply download and run the package manager, in our case we downloaded win-builds-1.5.0.exe file.
-
-- Once downloaded, double click on the the exe file. It will open an installation window.
-- Select x86_64 and Mingw installation folder \c C:\\mingw as in the following image:
-\image html img-mingw64-installer-started.jpg
-\note It you want to install Mingw in an other folder, you have to set \c MINGW_DIR environment variable to your installation location in order to allow CMake to detect 3rd party libraries that come with Mingw installer. Fo example, if you install Mingw in \c C:\\folder\\mingw-w64, setting \c MINGW_DIR is simply done running in a cmd terminal:
-\code
-C:> setx MINGW_DIR "C:\folder\mingw-w64"
-\endcode
-- Click on "OK" button. It will open an other window:
-\image html img-mingw64-installer-process.jpg
-- Where you can click on "Process" button to start the installation
-- A window shows the progression of the installation. When all the packages are installed you can close all the installation windows.
-- To finish the installation, just add \c C:\\mingw\\bin folder to the PATH variable. To this end open a cmd terminal and do the following:
-\code
-C:> echo %PATH%
-C:> setx PATH "%PATH%;C:\mingw\bin"
-\endcode
-- Close and re-open a cmd terminal, then run again
-\code
-C:> echo %PATH%
-\endcode
-to check that C:\\mingw\\bin was added.
-
-
-\subsection install_win81_mingw64_required_cmake CMake
-
-CMake 2.8.3 or higher that could be download at : http://www.cmake.org.
-- Download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>.
-- Install CMake just by double clicking on the binary \c cmake-x.y.z-win32-x86.exe you downloaded.
-
-\section install_win81_mingw64_3rdparty Install recommended 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. Some of them are automatically detected since they are part of MinGW distribution. Other may be installed from source. We recommend to install the following:
-
-\subsection install_win81_mingw64_3rdparty_opencv OpenCV 3rd party
-
-We recommend to install OpenCV.
-
-- Since OpenCV is not packaged for Mingw, you have to install OpenCV from source. From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the prebuilt binaries, <b>but also the source code</b> in \c C:\\OpenCV\\opencv.
-
-- Now we have to use CMake to configure OpenCV from source. As \c \<source_dir\> we will set \c C:\\OpenCV\\opencv\\sources and as \c \<binary_dir\> we set \c C:\\OpenCV\\opencv\\sources\\build-mingw.
-\image html img-win8.1-mingw-opencv-cmake-configure.jpg
-
-- Click then on "Configure" button.
-- Click on "Yes" to create \c C:\\OpenCV\\opencv\\sources\\build-mingw folder.
-- Select then "MinGW Makefiles" and click on "Finish" button.
-- Modify "CMAKE_INSTALL_PREFIX" to \c C:\\OpenCV\\opencv\\build folder. This is the location of the pre-build libraries that come with the installer. Doing that, allows to install OpenCV libraries in a same parent folder.
-\image html img-win8.1-mingw-opencv-cmake-configure-install.jpg
-- Click then on "Configure" button to remove the red lines.
-- Click on "Generate" button to generate the Makefiles for Mingw.
-- Open a cmd terminal, enter in the \c \<binary_dir\> folder and start mingw32-make
-\code
-C:\> cd C:\OpenCV\opencv\sources\build-mingw
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make
-\endcode
-\note Depending on OpenCV version you may encounter the following issues, for which we propose work arrounds; \ref issue_win81_mingw_opencv_dtor, \ref issue_win81_mingw_opencv_tiff, or \ref issue_win81_mingw_opencv_ipp.
-
-- To install OpenCV run:
-\code
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
-\endcode
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up cmd terminal and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
-where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- Here the directory is the one where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win81_mingw64_env_var.
-
-\section install_win81_mingw64_install_visp Install ViSP from source code
-\subsection install_win81_mingw64_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code.
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
-\code
-C:\ViSP> git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp
-
-\subsection install_win81_mingw64_config Configuring ViSP from source
-
-The goal of the configuration step is now to use CMake to produce all the material requested to build ViSP with Mingw. This material will be located in \c \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
-
-- Launch CMake (cmake-gui) and complete the \c \<source_dir\> and \c \<binary_dir\> locations as in the next image.
-\image html img-cmake-win8.1-msvc-launch.jpg
-
-- Click then on "Configure" button.
-\image html img-cmake-win8.1-create-build-folder.jpg
-
-- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
-- Select then "MinGW Makefiles" and click on "Finish" button.
-\image html img-cmake-win8.1-mingw-version.jpg
-
-- This will start CMake configuration. As shown in the next image, OpenCV, ffmpeg, gdi, libjpeg, libpng, pthread and libxml2 3rd party are detected.
-\image html img-cmake-win8.1-mingw-configure.jpg
-\note If OpenCV is not detected, you may encounter the following issue \ref issue_win81_mingw_opencv_not_detected.
-
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
-\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
-
-- Click then on "Configure" button. All the red lines should disappear.
-\image html img-cmake-win8.1-mingw-configure-end.jpg
-\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-
-- To finish the configuration, click on "Generate" button.
-\image html img-cmake-win8.1-mingw-generate.jpg
-
-- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Makefile file that will be used by Mingw to build the entire project.
-
-\subsection install_win81_mingw64_build Building ViSP from source
-
-- To build ViSP, open a cmd terminal, change to \c C:\\ViSP\\visp-build folder and run mingw32-make:
-\code
-C:\> cd C:\ViSP\visp-build
-C:\ViSP\visp-build> mingw32-make
-\endcode
-
-- Now to install ViSP, in the same cmd terminal run:
-\code
-C:\ViSP\visp-build> mingw32-make install
-\endcode
-
-- At the end, in \c C:/ViSP/visp-build/install/x64/mingw/bin folder you will find ViSP DLL libraries corresponding to the build modules.
-\note When CMAKE\_BUILD\_TYPE is set to Debug, the library names are suffixed by "d".
-
-\subsection install_win81_mingw64_env_var Setting up PATH variable
-
-If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
-
-To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\mingw\bin"
-\endcode
-
-Then to add the path to OpenCV 3rd party library, close and re-open a cmd-terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\mingw\bin"
-\endcode
-
-Then close and re-open a cmd terminal to check if the PATH var was well positioned
-
-\code
-C:\> echo %PATH%
-\endcode
-
-
-\section install_win81_mingw64_dataset Install ViSP dataset
-
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
-
-\image html img-win8.1-visp-images.jpg
-
-We suppose now that the data are located in \c C:/ViSP/ViSP-images.
-\code
-C:\> dir ViSP/ViSP-images
-Klimt README.md circle ellipse iv mbt mire-2
-LICENSE.txt calibration cube ellipse-1 line mire video
-\endcode
-
-ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
-\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
-
-\image html img-win8.1-mingw-cmd-displayGDI.jpg
-
-\section issue_win81_mingw Known issues
-
-\subsection issue_win81_mingw_opencv_endianness OpenCV endianness failure during CMake configuration
-
-\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
-
-If you encounter the following issue during CMake configuration
-\image html img-opencv-issue-test-big-endian.jpg
-
-Edit C:\\OpenCV\\opencv\\sources\\CMakeLists.txt file, and line 464 replace:
-\code
-test_big_endian(WORDS_BIGENDIAN)
-\endcode
-by:
-\code
-#test_big_endian(WORDS_BIGENDIAN)
-set(WORDS_BIGENDIAN 0)
-\endcode
-
-\subsection issue_win81_mingw_opencv_tiff OpenCV build error: cannot build with tiff support
-
-\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
-
-If you encounter a build issue during libtiff build as given in the next image:
-\image html img-opencv-issue-tiff.jpg
-
-- Open CMake GUI on OpenCV, turn BUILD_TIFF=OFF and also WITH_TIFF=OFF
-- Click on "Configure" button, and then on "Generate" one.
-- Build again OpenCV using
-\code
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make
-\endcode
-
-\subsection issue_win81_mingw_opencv_ipp OpenCV link error: cannot find -lRunTmChk
-
-\note This issue occurs with OpenCV 2.3.0-beta and 2.3.0 releases.
-
-The following image shows the link issue that may appear when building OpenCV with mingw:
-\image html img-opencv-issue-ipp.jpg
-
-A work arround is to configure OpenCV without ipp support turning WITH_IPP=OFF and then trying to build again.
-
-\subsection issue_win81_mingw_opencv_dtor OpenCV build error: struct has virtual functions and accessible non-virtual destructor
-
-This error that occurs with OpenCV 3.0.0 during \c cap_dshow.cpp build is known and reported as an issue in https://github.com/Itseez/opencv/pull/5282/commits.
-\image html img-win8.1-mingw-opencv-issue-dtor.jpg
-
-- The fix consists in modifying \c modules/videoio/src/cap_dshow.cpp by adding near line 96:
-\code
-#ifdef __MINGW32__
-// MinGW does not understand COM interfaces
-#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
-#endif
-\endcode
-
-\subsection issue_win81_mingw_opencv_not_detected OpenCV not detected with Mingw build
-
-\note This issue occurs with OpenCV 2.4.9, 2.4.10 and 2.3.0-beta.
-
-- To fix this issue, edit \c C:\\OpenCV\\opencv\\sources\\cmake\\OpenCVConfig.cmake, and line 89 replace:
-\code
- if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64")
-\endcode
-by:
-\code
- if(OPENCV_GCC_TARGET_MACHINE MATCHES "64")
-\endcode
-
-- Then open a new cmd terminal to build and install OpenCV again:
-\code
-C:\OpenCV\opencv\sources\build-mingw> mingw32-make install
-\endcode
-
-\section install_win81_mingw_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-install-win8.1-msvc.doc b/doc/tutorial-install-win8.1-msvc.doc
deleted file mode 100644
index 7b12846..0000000
--- a/doc/tutorial-install-win8.1-msvc.doc
+++ /dev/null
@@ -1,282 +0,0 @@
-/**
-
-\page tutorial-install-win81-msvc Tutorial: Installation from source for Windows 8.1 with Visual C++ 2013
-\tableofcontents
-
-In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++. These steps have been tested on Windows 8.1 (64 bit), with CMake 3.3.2 and Visual Studio Express 2013 but should work with any other version as well.
-
-\note Concerning ViSP installation, we provide also other \ref tutorial.
-
-\section install_win81_msvc_required Install prerequisities
-
-- Visual Studio C++. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products . In this tutorial we use Visual Studio Express 2013.
-\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
-- CMake 2.8.3 or higher that could be download at : http://www.cmake.org. To install CMake, download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-x.y.z-win32-x86.exe you downloaded.
-
-\section install_win81_msvc_3rdparty Install 3rd parties
-
-ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
-
-\subsection install_win81_msvc_3rdparty_recommended Recommended 3rd parties
-We recommend to install the following:
-
-\subsubsection install_win81_msvc_3rdparty_gdi Windows SDK 3rd party
-
-We recommend to install the "Windows Software Development Kit (SDK) for windows 8.1" to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
-
-- This SDK can be downloaded from https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx
-
-
-\subsubsection install_win81_msvc_3rdparty_opencv OpenCV 3rd party
-
-We recommend also to install OpenCV.
-
-- From http://opencv.org/downloads.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
-\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc12 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc12 for the Win64 compiler) and also libraries compatible with Visual C++ 2012 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc11 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc11 for the Win64 compiler). With any other compiler version you need to build yourself OpenCV from source.
-
-\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
-
-- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
-\code
-setx OpenCV_DIR C:\OpenCV\opencv\build
-\endcode
-where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
-
-- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win81_msvc_env_var.
-
-\subsection install_win81_msvc_3rdparty_other Other optional 3rd parties
-
-We give also the way to install other 3rd party libraries to enable specific capabilities.
-\subsubsection install_win81_msvc_3rdparty_ogre Ogre3D 3rd party
-
-\warning Ogre3D installation from source code described in this section is reserved to expert users.
-
-If you are interested in augmented reality (see vpAROgre), or if you want to enable advanced visibility computation during model-based tracking (see vpMbTracker::setOgreVisibilityTest() and \ref tutorial-tracking-mb) we recommend to install Ogre3D. You can check if a <a href="http://www.ogre3d.org/download/sdk">prebuild SDK</A> matches your Visual C++ version, and follow the <a href="http://www.ogre3d.org/tikiwiki/Installing+the+Ogre+SDK">SDK installation instructions</a>. You can also i [...]
-
-Ogre source code is hosted on <a href="https://bitbucket.org/sinbad/ogre">BitBucket</a>. On Ogre wiki you will find useful information to <a href="http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Building+Ogre">build and install Ogre from source</a>. Hereafter we give the main steps.
-
-<b>Ogre prerequisities</b>
-- To build Ogre on Windows you will need to install the DirectX SDK. Since Windows 8, DirectX is no longer a standalone package, rather it is bundled in the Windows 8.x SDK. The latest version is the <a href="https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx">Windows 8.1 SDK</a>.
-- To get the Ogre source code but also the source code of all the dependencies hosted on BitBucket and available with Mercurial, you have to install <a href="http://tortoisehg.bitbucket.org">TortoiseHg</a>.
-
-<b>Getting Ogre dependencies</b>
-- A repository containing Ogre dependencies source code is available <a href="https://bitbucket.org/cabalistic/ogredeps">here at BitBucket</a>.
-- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/cabalistic/ogredeps and as destination \c C:\\Ogre\\ogredeps as in the next image:
-\image html img-win8.1-ogredeps-tortoisehg.jpg
-- Click on Clone button to get dependencies source code in \c C:\\Ogre\\ogredeps
-
-<b>Building Ogre dependencies</b>
-- To build the dependencies, open CMake GUI and set source code location pointing to \c C:\\Ogre\\ogredeps and build location pointing to \c C:\\Ogre\\ogredeps\\build. Select Visual Studio 12 2013 Win64 as generator, click on "Configure" button twice and then on "Generate" button. You should have something similar to the next image:
-\image html img-win8.1-cmake-ogredeps.jpg
-- Open \c C:\\Ogre\\ogredeps\\build\\OGREDEPS.sln solution file with Visual C++.
-- In Visual, select Debug configuration and build INSTALL project:
-\image html img-win8.1-msvc-build-ogredeps.jpg
-\note If the build fails due to the environment variable \c DXSDK_DIR that is not set, see \ref known_issues_win81_msvc_ogredeps for a work arround.
-- In Visual, select also Release configuration and build again INSTALL project.
-- Now in \c C:\\Ogre\\ogredeps\\build\\ogredeps\\bin you will find the Cg and OIS dependencies build in Debug and Release:
-\image html img-win8.1-ogredeps-explorer.jpg
-- To finish dependencies installation, set \c OGRE_DEPENDENCIES_DIR environment variable pointing to \c C:\\Ogre\\ogredeps\\build\\ogredeps
-\code
-setx OGRE_DEPENDENCIES_DIR C:\Ogre\ogredeps\build\ogredeps
-\endcode
-
-<b>Getting Ogre source</b>
-
-- From <a href="http://www.ogre3d.org/download/source">Ogre download page</a>, identify the last release. In our case Ogre 1.9 that is on <a href="https://bitbucket.org/sinbad/ogre/branches">BitBucket repository</a> under branch v1-9.
-- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/sinbad/ogre, as destination \c C:\\Ogre\\ogre and select revision v1-9 corresponding to the branch of the latest release as presented in the next image:
-\image html img-win8.1-ogre-tortoisehg.jpg
-- Click on Clone button to get Ogre source code in \c C:\\Ogre\\ogre
-
-<b>Building Ogre source</b>
-
-- Close and re-open CMake Gui. Set source code location pointing to \c C:\\Ogre\\ogre and build location pointing to \c C:\\Ogre\\ogre\\build. Select Visual Studio 12 2013 Win64 as generator, click on "Configure" button and then on "Generate" button. You should have something similar to the next image:
-\image html img-win8.1-cmake-ogre.jpg
-\note As shown in the previous image, Ogre installation folder is set to \c C:\\Ogre\\ogre\\build\\sdk.
-- Open \c C:\\Ogre\\ogre\\build\\OGRE.sln solution file with Visual C++.
-- In Visual, select Debug configuration and build INSTALL project.
-- In Visual, select also Release configuration and build again INSTALL project.
-- Now in \c C:\\Ogre\\ogre\\build\\sdk\\bin you will find all the libraries in Debug and Release configuration, including Cg and OIS dependencies there where build previously.
-\image html img-win8.1-ogre-explorer.jpg
-- To finish Ogre installation set OGRE_HOME environment variables to \c C:\\Ogre\\ogre\\build\\sdk location.
-\code
-setx OGRE_HOME C:\Ogre\ogre\build\sdk
-\endcode
-
-\section install_win81_msvc_install_visp Install ViSP from source code
-\subsection install_win81_msvc_get_source Getting ViSP source code
-
-There are different ways to get ViSP source code.
-
-- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
-
-- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
-\code
-C:\ViSP> git clone https://github.com/lagadic/visp.git
-\endcode
-
-We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp
-
-\subsection install_win81_msvc_config Configuring ViSP from source
-
-The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \c \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
-
-- Launch CMake (cmake-gui) and complete the \c \<source_dir\> and \c \<binary_dir\> locations as in the next image.
-
-\image html img-cmake-win8.1-msvc-launch.jpg
-
-- Click then on "Configure" button.
-
-\image html img-cmake-win8.1-create-build-folder.jpg
-
-- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
-- Select then your compiler, for example here Visual Studio Express 2013 Win64, and click on "Finish" button.
-
-\image html img-cmake-win8.1-msvc-version.jpg
-
-- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV, Ogre and OpenMP 3rd parties are automatically detected.
-
-\image html img-cmake-win8.1-config.jpg
-
-- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
-\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
-
-- Click then on "Configure" button. All the red lines should disappear.
-\image html img-cmake-win8.1-config-end.jpg
-\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
-
-- To finish the configuration, click on "Generate" button.
-
-\image html img-cmake-win8.1-msvc-generate.jpg
-
-- Once the generation is done, in \c C:/ViSP/visp-build folder you have the Visual Studio \c VISP.sln generated solution file.
-
-\image html img-win8.1-msvc-solution.jpg
-
-\subsection install_win81_msvc_build Building ViSP from source
-
-- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
-
-\image html img-win8.1-msvc-open.jpg
-
-- Enter menu "BUILD/Build Solution" to build ViSP.
-
-\image html img-win8.1-msvc-build.jpg
-
-- At the end of the build process you should have the following indicating that all the build succeeded.
-
-\image html img-win8.1-msvc-build-end.jpg
-
-- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
-
-\image html img-win8.1-msvc-install.jpg
-
-- At the end of the installation, you should have the following.
-
-\image html img-win8.1-msvc-install-end.jpg
-
-- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
-
-- This ends ViSP installation with \c Debug configuration.
-
-- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
-
-\image html img-win8.1-msvc-release.jpg
-
-- Now, as previously, build and install ViSP again.
-
-- At the end, in \c C:/ViSP/visp-build/install/x64/vc12/bin folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
-
-\image html img-win8.1-explorer-install-end.jpg
-
-\subsection install_win81_msvc_env_var Setting up PATH variable
-
-If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
-
-To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc12\bin"
-\endcode
-
-Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc12\bin"
-\endcode
-
-If installed, you have also to add the location of Ogre 3rd party libraries to the path. Close and re-open a cmd terminal to run:
-\code
-C:\> echo %PATH%
-C:\> setx PATH "%PATH%;C:\Ogre\ogre\build\sdk\bin\release"
-C:\> setx PATH "%PATH%;C:\Ogre\ogre\build\sdk\bin\debug"
-\endcode
-
-Then close and re-open a cmd terminal to check if the PATH var was well positioned.
-
-\code
-C:\> echo %PATH%
-\endcode
-
-\section install_win81_msvc_dataset Install ViSP dataset
-
-Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
-
-Download ViSP-images-x.y.z.zip from http://visp.inria.fr/download and uncompress it for example in \c C:/ViSP.
-
-\image html img-win8.1-visp-images.jpg
-
-ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called \c VISP_INPUT_IMAGE_PATH. In our case, this variable should be set to \c C:\\ViSP.
-\code
-setx VISP_INPUT_IMAGE_PATH C:\ViSP
-\endcode
-
-\note For historical reasons \c VISP_INPUT_IMAGE_PATH should not contain the folder \c ViSP-images, but the parent folder.
-
-From now, you can try to run ViSP examples and tests. For example, if you want to run \c \<binary dir\>/example/device/display/Debug/displayGDI.exe, open a command window, enter in the right folder, and run:
-
-\image html img-win8.1-cmd-displayGDI.jpg
-
-\section known_issues_win81_msvc Known issues
-
-\subsection known_issues_win81_msvc_ogredeps Unable to build ogredeps: DXSDK_DIR missing
-
-During ogredeps build as described in \ref install_win81_msvc_3rdparty_ogre installation you may get in Visual C++ the following error when building SDL2 project:
-\code
-error: DIRECTX requires $DXSDK_DIR environment variable to be set
-\endcode
-\image html img-win8.1-msvc-ogredeps-issue.jpg
-
-To fix the issue:
-- Quit Visual C++ and start again CMake over ogredeps project
-- Disable SDL2 dependency build turning \c OGREDEPS_BUILD_SDL2 option off
-\image html img-win8.1-cmake-ogredeps-issue.jpg
-- Configure and generate again the project
-- Open \c C:\\Ogre\\ogredeps\\build\\OGREDEPS.sln solution file with Visual C++ and try a new build as described in \ref install_win81_msvc_3rdparty_ogre
-
-\subsection known_issues_win81_msvc_ogre_mbtracker Model-based trackers are not working with Ogre visibility ckeck
-
-If you run \c mbtEdgeTracking.exe, \c mbtKltTracking.exe or \c mbtEdgeKltTracking.exe enabling Ogre visibility check (using "-o" option), you may encounter the following issue:
-\code
-C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
-...
-OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
-...
-*** Initializing OIS ***
-\endcode
-and then a wonderful runtime issue as in the next image:
-\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
-
-It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
-\code
-C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
-\endcode
-Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
-\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
-
-\section install_win81_msvc_next Next tutorial
-
-You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
-
-*/
diff --git a/doc/tutorial-matching-deprecated.doc b/doc/tutorial-matching-deprecated.doc
deleted file mode 100644
index f347d30..0000000
--- a/doc/tutorial-matching-deprecated.doc
+++ /dev/null
@@ -1,63 +0,0 @@
-/**
-
-\page tutorial-matching-deprecated Tutorial: Keypoint matching (deprecated)
-\tableofcontents
-
-\section intro_matching_deprecated Introduction
-
-\note This tutorial is deprecated if your OpenCV version is equal to 3.0.0 or more recent. If so you should rather follow \ref tutorial-matching.
-
-This tutorial focuses on SURF key points manipulation. You will learn how to detect SURF key points on a reference image considered here as the first image of an mpeg video. Then in the next images of the video, key points that match those detected in the reference image using SURF descriptor are displayed.
-
-\note We assume that you are familiar with video framegrabbing described in \ref tutorial-grabber and with the way to display an image in a window described in \ref tutorial-getting-started.
-
-\section surf_deprecated SURF key points detection and matching
-
-Let us consider the following source code also available in tutorial-matching-surf-deprecated.cpp.
-
-\include tutorial-matching-surf-deprecated.cpp
-
-Here after is the resulting video. The left image represents the reference image. The right images correspond to the successive images of the input video. All the green lines extremities represent the points that are matched.
-
-\htmlonly
-<iframe width="560" height="315" src="http://www.youtube.com/embed/sMbed_oYJgQ" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-Now, let us explain the lines dedicated to the SURF keypoint usage.
-
-First we have to include the header of the vpKeyPointSurf class that is a wrapper over OpenCV classes.
-\snippet tutorial-matching-surf-deprecated.cpp Include
-
-Note that this class is only available if ViSP was build with OpenCV non free module. This is ensured by the check of VISP_HAVE_OPENCV_NONFREE macro. To grab the images from the mpeg video stream we need also that ViSP was build with ffmpeg 3rd party. That's why we check VISP_HAVE_FFMPEG macro definition:
-\snippet tutorial-matching-surf-deprecated.cpp Define
-
-Then we open the mpeg video stream and grab the first image of the video that is stored in \c I container. A Surf keypoint class is instantiated and keypoints are detected on the first image which is considered as the reference image:
-\snippet tutorial-matching-surf-deprecated.cpp Construction
-
-The next lines are used to create image \c Idisp to render the matching results; left image for the reference image, right image for the current image that is processed:
-\snippet tutorial-matching-surf-deprecated.cpp Create image
-
-Then a display using OpenCV is created and image \c Idisp is rendered:
-\snippet tutorial-matching-surf-deprecated.cpp Init display
-
-We enter then in the \c while() loop where a new image is acquired from the video stream and inserted in the right part of image \c Idisp dedicated to rendering of the matching results.
-\snippet tutorial-matching-surf-deprecated.cpp Acquisition
-
-We start the rendering by displaying the rendered image and by drawing a white vertical line to separate the reference image from the current one:
-\snippet tutorial-matching-surf-deprecated.cpp Display
-
-Keypoint matches between the reference image and the current image \c I are detected using:
-\snippet tutorial-matching-surf-deprecated.cpp Matching
-
-Then we parse all the matches to retrieve the coordinates of the points in the reference image (in \c iPref variable) and in the current image (in \c iPcur variable):
-\snippet tutorial-matching-surf-deprecated.cpp Get matches
-
-Next we draw green lines between the matched points:
-\snippet tutorial-matching-surf-deprecated.cpp Display matches
-
-At the end of the iteration, we flush all the previous display to the render window:
-\snippet tutorial-matching-surf-deprecated.cpp Display flush
-
-You can now follow \ref tutorial-homography-deprecated to see how to exploit couple of matched points in order to estimate an homography that allows to track the position of an object.
-
-*/
diff --git a/doc/tutorial-trace.doc b/doc/tutorial-trace.doc
deleted file mode 100644
index 3c743f6..0000000
--- a/doc/tutorial-trace.doc
+++ /dev/null
@@ -1,138 +0,0 @@
-/**
-
-\page tutorial-trace Tutorial: Debug and trace printings
-\tableofcontents
-
-
-\section intro_trace Introduction
-
-ViSP allows to introduce trace and debug printings that may help debugging. To this end ViSP provides C or C++ macros that allows to print messages to the standard output std::cout or to std::cerr.
-
-~~~
-|----------|-------|-------------------------------------|---------------------------------------|
-| output | type | std::cout | std::cerr |
-|----------|-------|-------------------------------------|---------------------------------------|
-| C-like | trace | vpTRACE, vpTRACE(level) | vpERROR_TRACE, vpERROR_TRACE(level) |
-| | trace | vpIN_FCT, vpOUT_FCT | |
-| | debug | vpDEBUG_TRACE, vpDEBUG_TRACE(level) | vpDERROR_TRACE, vpDERROR_TRACE(level) |
-|----------|-------|-------------------------------------|---------------------------------------|
-| C++-like | trace | vpCTRACE | vpCERROR |
-| | debug | vpCDEBUG(level) | |
-|----------|-------|-------------------------------------|---------------------------------------|
-~~~
-
-\subsection trace_macro Macros for trace
-
-Macro for tracing vpTRACE(), vpTRACE(level), vpERROR_TRACE(), vpERROR_TRACE(level),
-vpIN_FCT() and vpOUT_FCT()
-work like printf with carrier return at the end of the string, while
-vpCTRACE() and vpCERROR() work like the C++ output streams std::cout
-and std::cerr. All these macro print messages only if VP_TRACE macro is defined.
-
-\subsection debug_macro Macros for debug
-
-Macros for debug vpDEBUG_TRACE(level) and vpDERROR_TRACE(level)
-work like printf while vpCDEBUG(level) works like the C++ output stream std::cout.
-These macro print messages only if VP_DEBUG macro is defined and
-if the debug \e level is greater than the one defined in VP_DEBUG_MODE
-macro. Moreover vpDEBUG_ENABLE(level) can be used to check if a given debug level is active; vpDEBUG_ENABLE(level) is
-equal to 1 if the debug \e level is greater than the debug mode
-VP_DEBUG_MODE, 0 else.
-
-\section debug_trace_usage Debug and trace usage in ViSP library
-
-In ViSP, before an exception is thrown, trace macro are widely used to inform the user that an error occur. This is redundant, since the same trace message in generally associated to the exception that is thrown. Since ViSP 2.9.0, during CMake configuration it is possible to disable debug and trace printings by turning \c ACTIVATE_DEBUG_TRACE cmake variable to \c OFF.
-
-- Using cmake command just run:
-\code
-%cmake -DACTIVATE_DEBUG_TRACE=OFF <path to ViSP source code>
-\endcode
-
-- or using ccmake GUI as shown in the next snapshot:
-
-\image html img-cmake-debug-trace.jpg
-
-\note When \c ACTIVATE_DEBUG_TRACE is turned to \c ON (this is the default behavior in ViSP), we simply define VP_TRACE and VP_DEBUG macro using the compiler -D option.
-
-\section example Debug and trace usage in your own project
-
-Note that all the material (source code) described in this section is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/trace
-\endcode
-
-If you develop a project that uses ViSP library as a 3rd party, there are different ways to benefit from debug and trace macro described previously.
-
-- If ViSP was build with debug and trace enabled using \c cmake \c ACTIVATE_DEBUG_TRACE=ON, debug and trace are also enabled in your development.
-- If debug and trace were disabled in ViSP (\c ACTIVATE_DEBUG_TRACE=OFF), you can enable debug and trace in your own development either by defining \c VP_DEBUG and/or \c VP_TRACE macro in your code using
-\code
-#define VP_TRACE
-#define VP_DEBUG
-
-#include <visp3/core/vpDebug.h>
-\endcode
-either by modifying your \c CMakeLists.txt file by adding an option as in ViSP:
-\code
-option(ACTIVATE_DEBUG_TRACE "Enable debug and trace printings" ON)
-
-if(ACTIVATE_DEBUG_TRACE)
- add_definitions("-DVP_DEBUG -DVP_TRACE")
-endif()
-\endcode
-
-
-The following example also available in tutorial-trace.cpp shows how to use the previous macro.
-
-\includelineno tutorial-trace.cpp
-
-
-\note In the previous example it is important to notice that the following lines have to be put prior to any other ViSP includes:
-\code
-#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
-#include <visp3/core/vpDebug.h>
-\endcode
-For example, if you modify the previous example just by including <visp3/core/vpImage.h> on the top of the file, you will get the following warnings:
-\code
-Building CXX object tutorial/trace/CMakeFiles/tutorial-trace.dir/tutorial-trace.cpp.o
-.../ViSP-code/tutorial/trace/tutorial-trace.cpp:5:1: warning: "VP_DEBUG_MODE" redefined
-In file included from .../ViSP-build-debug/include/visp3/core/vpImage.h:52,
- from .../ViSP-code/tutorial/trace/tutorial-trace.cpp:2:
-.../ViSP-build-debug/include/visp3/core/vpDebug.h:67:1: warning: this is the location of the previous definition
-\endcode
-
-
-When ViSP library was built without debug and trace the previous example produces the output:
-\code
-%./tutorial-trace
-Debug level 1 active: 0
-Debug level 2 active: 0
-Debug level 3 active: 0
-\endcode
-
-When ViSP is rather build with debug and trace the previous example produces the output:
-\code
-%./tutorial-trace
-(L0) begin /tmp/tutorial-trace.cpp: main(#9) : main()
-Debug level 1 active: 1
-Debug level 2 active: 1
-Debug level 3 active: 0
-(L0) /tmp/tutorial-trace.cpp: main(#17) : C-like trace
-(L1) /tmp/tutorial-trace.cpp: main(#18) : C-like trace level 1
-(L0) !! /tmp/tutorial-trace.cpp: main(#20) : C-like error trace
-(L1) !! /tmp/tutorial-trace.cpp: main(#21) : C-like error trace level 1
-(L0) /tmp/tutorial-trace.cpp: main(#24) : C-like debug trace
-(L0) !! /tmp/tutorial-trace.cpp: main(#25) : C-like error trace
-(L2) /tmp/tutorial-trace.cpp: main(#27) : C-like debug trace level 2
-(L2) !! /tmp/tutorial-trace.cpp: main(#28) : C-like error trace level 2
-(L0) /tmp/tutorial-trace.cpp: main(#31) : C++-like trace
-(L0) !! /tmp/tutorial-trace.cpp: main(#32) : C++-like error trace
-(L2) /tmp/tutorial-trace.cpp: main(#35) : C++-like debug trace level 2
-(L0) end /tmp/tutorial-trace.cpp: main(#37) : main()
-\endcode
-
-In the previous printings:
-- the number after "L" indicates the debug or trace level; example (L2) is for level 2.
-- the number after "#" indicates the line of the code that produce the printing; example main(#37) means in function main() at line 37.
-- the "!!" indicate that the printing is on std::cerr. Others are on std::cout.
-*/
diff --git a/doc/tutorial-tracking-mb-stereo.doc b/doc/tutorial-tracking-mb-stereo.doc
deleted file mode 100644
index e1e9a6e..0000000
--- a/doc/tutorial-tracking-mb-stereo.doc
+++ /dev/null
@@ -1,224 +0,0 @@
-/**
-
-\page tutorial-tracking-mb-stereo Tutorial: Markerless model-based tracking with stereo cameras
-\tableofcontents
-
-\section mb_stereo_intro Introduction
-
-This tutorial describes the model-based tracking of objects using simultaneously multiple cameras views. It allows to simultaneously track the object in the images viewed by a set of cameras while providing its 3D localization (i.e., the object pose expressed in a reference camera frame) when calibrated cameras are used.
-
-The software allows the tracking of a markerless object using the knowledge of its CAD model. Considered objects have to be modeled by segments, circles or cylinders. The model of the object could be defined in vrml format (except for circles), or in cao format.
-
-Next section highlights the different versions of the markerless multi-view model-based trackers that have been developed. The multi-view model-based tracker can consider moving-edges behind the lines of the model (thanks to vpMbEdgeMultiTracker class). It can also consider keypoints that are detected and tracked on each visible face of the model (thanks to vpMbKltMultiTracker class). The tracker can also handle moving-edges and keypoints in a hybrid scheme (thanks to vpMbEdgeKltMultiTra [...]
-
-While the multi-view model-based edges tracker implemented in vpMbEdgeMultiTracker is appropriate to track texture less objects, the multi-view model-based keypoints tracker implemented in vpMbKltMultiTracker is more designed to exploit textured objects with edges that are not really visible. The multi-view model-based hybrid tracker implemented in vpMbEdgeKltMultiTracker is appropriate to track textured objects with visible edges.
-
-These classes allow tracking the same object as two cameras or more sees it.
-The main advantages of this configuration with respect to the mono-camera case (see \ref tutorial-tracking-mb) concern:
-- the possibility to extend the application field of view;
-- a more robust tracking as the configuration of the stereo rig allows to track the object under multiple viewpoints and thus with more visual features.
-
-In order to achieve this, the following information are required:
-- the intrinsic parameters of each camera;
-- the transformation matrix between each camera and a reference camera: \f$ ^{c_{current}}{\bf M}_{c_{reference}} \f$.
-
-In the following sections, we consider the tracking of a tea box modeled in cao format. A stereo camera sees this object. Thus to illustrate the behavior of the tracker, the following video shows the resulting tracking performed with vpMbEdgeMultiTracker. In this example the fixed cameras located on Romeo Humanoid robot head captured the images.
-
-\htmlonly
-<iframe width="560" height="315" src="https://www.youtube.com/embed/aWXvvkeytp0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-This other video shows the behavior of the hybrid tracking performed with vpMbEdgeKltMultiTracker where features are the teabox edges and the keypoints on the visible faces.
-
-\htmlonly
-<iframe width="560" height="315" src="https://www.youtube.com/embed/0poP7gONdMc" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-\note The cameras can move, but the tracking will be effective as long as the transformation matrix between the cameras and the reference camera is known and updated at each iteration.
-\note The new introduced classes are not restricted to stereo configuration. They allow the usage of multiple cameras (see \ref mb_stereo_moving_cameras).
-
-Next sections will highlight how to easily adapt your code to use multiple cameras with the model-based tracker. As only the new methods dedicated to multiple views tracking will be presented,
-you are highly recommended to follow \ref tutorial-tracking-mb in order to be familiar with the model-based tracking concepts, the different trackers that are available in ViSP (the edge tracker: vpMbEdgeTracker, the klt feature points tracker: vpMbKltTracker
-and the hybrid tracker: vpMbEdgeKltTracker) and with the configuration loading part.
-
-Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/model-based/stereo
-\endcode
-
-\section mb_stereo_started Getting started
-
-\subsection mb_stereo_overview Overview
-
-The model-based trackers available for multiple views tracking rely on the same trackers than in the monocular case:
-- a vpMbEdgeMultiTracker similar to vpMbEdgeTracker which tracks moving-edges corresponding to the visible lines of the model protected in the image plane at the current pose (suitable for textureless objects).
-- a vpMbKltMultiTracker similar to vpMbKltTracker which uses the optical flow information to track the object (suitable for textured objects).
-- a vpMbEdgeKltMultiTracker similar to vpMbEdgeKltTracker which merges the two information (edge and texture information) for better robustness of the tracking (can deal with both types of objects).
-
-The following class diagram offers an overview of the hierarchy between the different classes:
-
-\image html img-mbt-multi-class-diagram-resize.jpeg Simplified class diagram.
-
-The vpMbEdgeMultiTracker class inherits from the vpMbEdgeTracker class, the vpMbKltMultiTracker inherits from the vpMbKltTracker class and the vpMbEdgeKltMultiTracker class inherits from the
-vpMbEdgeMultiTracker and vpMbKltMultiTracker classes.
-This conception permits to easily extend the usage of the model-based tracker to multiple cameras with the guarantee to preserve the same behavior compared to the tracking in the monocular configuration
-(more precisely, only the model-based edge and the model-based klt should have the same behavior, the hybrid multi class has a slight different implementation that will lead to minor
-differences compared to vpMbEdgeKltTracker).
-
-As you will see after, the principal methods present in the parent class are accessible and used for single view tracking.
-Lot of new overridden methods have been introduced to deal with the different cameras configuration (single camera, stereo cameras and multiple cameras).
-
-\subsection mb_stereo_implementation_detail Implementation detail
-
-Each tracker is stored in a map, the key corresponding to the name of the camera on which the tracker will process. By default, the camera names are set to:
-- "Camera" when the tracker is constructed with one camera.
-- "Camera1" to "CameraN" when the tracker is constructed with N cameras.
-- The default reference camera will be "Camera1" in the multiple cameras case.
-
-\image html img-multi-cameras-config.png Default name convention and reference camera ("Camera1").
-
-To deal with multiple cameras, in the virtual visual servoing control law we concatenate all the interaction matrices and residual vectors and transform them in a single reference camera frame to compute
-the reference camera velocity.
-Thus, we have to know the transformation matrix between each camera and the reference camera.
-
-For example, if the reference camera is "Camera1" (\f$ c_1 \f$), we need the following information:
-\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$.
-
-\subsection mb_stereo_interface_with_the_code Interfacing with the code
-
-Each essential method used to initialize the tracker and process the tracking have three signatures in order to ease the call to the method and according to three working modes:
-- tracking using one camera, the signature remains the same than the previous classes (vpMbEdgeTracker, vpMbKltTracker, vpMbEdgeKltTracker).
-- tracking using two cameras, all the necessary methods accept directly the corresponding parameter for each camera. By default, the first parameter corresponds to the reference camera.
-- tracking using multiple cameras, you have to supply the different parameters with a map. The key corresponds to the name of the camera and the value is the value to the parameter.
-
-The following table sums up how to call the different methods based on the camera configuration for the main functions.
-
-<table>
-<caption id="method_example_table">Example of the different method signatures.</caption>
-<tr><th>Method calling example: <th>Monocular case <th>Stereo case <th>Multiple cameras case <th>Remarks
-<tr><td>Construct a model-based edge tracker: <td>vpMbEdgeMultiTracker tracker <td>vpMbEdgeMultiTracker tracker(2) <td>vpMbEdgeMultiTracker tracker(5) <td>The default constructor corresponds to the monocular configuration.
-<tr><td>Load a configuration file: <td>tracker.loadConfigFile("config.xml") <td>tracker.loadConfigFile("config1.xml", "config2.xml") <td>tracker.loadConfigFile(mapOfConfigFiles) <td>Each tracker can have different parameters (intrinsic parameters, visibility angles, etc.).
-<tr><td>Load a model file: <td>tracker.loadModel("model.cao") <td>tracker.loadModel("model.cao") <td>tracker.loadModel("model.cao") <td>All the trackers must used the same 3D model.
-<tr><td>Get the intrinsic camera parameters: <td>tracker.getCameraParameters(cam) <td>tracker.getCameraParameters(cam1, cam2) <td>tracker.getCameraParameters(mapOfCam) <td>
-<tr><td>Set the transformation matrix between each camera and the reference one: <td> <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>For the reference camera, the identity homogeneous matrix must be set.
-<tr><td>Setting to display the features:<td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>This is a general parameter.
-<tr><td>Initialize the pose by click: <td>tracker.initClick(I, "f_init.init") <td>tracker.initClick(I1, I2, "f_init1.init", "f_init2.init") <td>tracker.initClick(mapOfImg, mapOfInitFiles) <td>If the transformation matrices between the cameras have been set, some init files can be omitted as long as the reference camera has an init file.
-<tr><td>Track the object: <td>tracker.track(I) <td>tracker.track(I1, I2) <td>tracker.track(mapOfImg) <td>
-<tr><td>Get the pose: <td>tracker.getPose(cMo) <td>tracker.getPose(c1Mo, c2Mo) <td>tracker.getPose(mapOfPoses) <td>tracker.getPose(cMo) will return the pose for the reference camera in the multiple cameras configurations.
-<tr><td>Display the model: <td>tracker.display(I, cMo, cam, ...) <td>tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, ...) <td>tracker.display(mapOfImg, mapOfPoses, mapOfCam) <td>
-</table>
-
-\note As the trackers are stored in an alphabetic order internally, you have to match the method parameters with the correct
-tracker position in the map in the stereo cameras case.
-
-\subsection mb_stereo_example_code Example code
-The following example comes from tutorial-mb-tracker-stereo.cpp and allows to track a tea box modeled in cao format using one of the three multi-view markerless trackers implemented in ViSP. In this example we consider a stereo configuration.
-
-Once built, to choose which tracker to use, run the binary with the following argument:
-\code
-$ ./tutorial-mb-tracker-stereo --tracker <0=egde|1=klt|2=hybrid>
-\endcode
-
-The source code is the following:
-\include tutorial-mb-tracker-stereo.cpp
-
-\subsection mb_stereo_explanation_of_the_code Explanation of the code
-
-The previous source code shows how to implement model-based tracking on stereo images using the standard procedure to configure the tracker:
-- construct the tracker
-- initialize the tracker by loading a configuration file
-- load a 3D model
-- process the tracking
-- get the pose and display the model in the image
-
-\warning The xml2 library, used to load the configuration file, is required to build the tutorial example. OpenCV is required to use the KLT functionality.
-
-Please refer to the tutorial \ref tutorial-tracking-mb in order to have explanations on the configuration parameters and for information on how to model an object in a ViSP compatible format.
-
-To test the three kind of trackers, only vpMbEdgeKltMultiTracker.h header is required as the others (vpMbEdgeMultiTracker.h and vpMbKltMultiTracker.h) are already included in the hybrid header class.
-
-\snippet tutorial-mb-tracker-stereo.cpp Include
-
-We declare two images for the left and right camera views.
-
-\snippet tutorial-mb-tracker-stereo.cpp Image
-
-To construct a stereo tracker, we have to specify the desired number of cameras (in our case 2) as argument given to the tracker constructors:
-
-\snippet tutorial-mb-tracker-stereo.cpp Constructor
-
-\note We used a pointer to vpMbTracker to be able to construct a tracker according to the desired type (edge, klt or hybrid) but you could directly declare the desired tracker class in your program.
-
-All the configuration parameters for the tracker are stored in xml configuration files. To load the different files, we use:
-
-\snippet tutorial-mb-tracker-stereo.cpp Load config file
-
-\note The dynamic cast is necessary to access to the specific methods that are not declared in vpMbTracker.
-
-The following code is used in order to retrieve the intrinsic camera parameters:
-
-\snippet tutorial-mb-tracker-stereo.cpp Get camera parameters
-
-To load the 3D object model, we use:
-
-\snippet tutorial-mb-tracker-stereo.cpp Load cao
-
-We can also use the following setting that enables the display of the features used during the tracking:
-
-\snippet tutorial-mb-tracker-stereo.cpp Set display features
-
-We have to set the transformation matrices between the cameras and the reference camera to be able to compute the control law in a reference camera frame. In the code we consider the left camera with name "Camera1" as the reference camera. For the right camera with name "Camera2" we have to set the transformation (\f$ ^{c_{right}}{\bf M}_{c_{left}} \f$). This transformation is read from cRightMcLeft.txt file. Since our left and right cameras are not moving, this transformation is constan [...]
-
-\note For the reference camera, the camera transformation matrix has to be specified as an identity homogeneous matrix (no rotation, no translation). By default the vpHomogeneousMatrix constructor builds an identity matrix.
-
-\snippet tutorial-mb-tracker-stereo.cpp Set camera transformation matrix
-
-The initial pose is set by clicking on specific points in the image:
-
-\snippet tutorial-mb-tracker-stereo.cpp Init
-
-The poses for the left and right views have to be declared:
-
-\snippet tutorial-mb-tracker-stereo.cpp cMo
-
-The tracking is done by:
-
-\snippet tutorial-mb-tracker-stereo.cpp Track
-
-The poses for each camera are retrieved with:
-
-\snippet tutorial-mb-tracker-stereo.cpp Get pose
-
-To display the model with the estimated pose, we use:
-
-\snippet tutorial-mb-tracker-stereo.cpp Display
-
-Finally, do not forget to delete the pointers:
-
-\snippet tutorial-mb-tracker-stereo.cpp Cleanup
-
-\section mb_stereo_advanced Advanced
-
-\subsection mb_stereo_moving_cameras How to deal with moving cameras
-
-The principle remains the same than with static cameras. You have to supply the camera transformation matrices to the tracker each time the cameras move and before calling the track method:
-\code
-mapOfCamTrans["Camera1"] = vpHomogeneousMatrix(); //The Camera1 is the reference camera.
-mapOfCamTrans["Camera2"] = get_c2Mc1(); //Get the new transformation between the two cameras.
-tracker.setCameraTransformationMatrix(mapOfCamTrans);
-tracker.track(mapOfImg);
-\endcode
-
-This information can be available through the robot kinematics and different kind of sensors for example.
-
-The following video shows how the result of a stereo hybrid model-based tracking based on object edges and keypoints located on visible faces could be used to servo Romeo humanoid robot eyes to gaze the object. Here the images were captured by Romeo eyes that are moving.
-
-\htmlonly
-<iframe width="560" height="315" src="https://www.youtube.com/embed/IguBxf4VRUc" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-\section mb_stereo_next Next tutorial
-You are now ready to see the next \ref tutorial-tracking-tt.
-
-*/
diff --git a/doc/tutorial-tracking-mb.doc b/doc/tutorial-tracking-mb.doc
deleted file mode 100644
index 0d534f2..0000000
--- a/doc/tutorial-tracking-mb.doc
+++ /dev/null
@@ -1,876 +0,0 @@
-/**
-
-\page tutorial-tracking-mb Tutorial: Markerless model-based tracking
-\tableofcontents
-
-\section mb_intro Introduction
-
-ViSP allows simultaneously the tracking of a markerless object using the knowledge of its CAD model while providing its 3D localization (i.e., the object pose expressed in the camera frame) when a calibrated camera is used \cite Comport06b. Considered objects should be modeled by lines, circles or cylinders. The CAD model of the object could be defined in vrml format (except for circles), or in cao format.
-
-Next sections highlight how to use the differents versions of the markerless model-based trackers that are implemented in ViSP.
-
-Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/model-based/generic
-\endcode
-
-\section mb_started Getting started
-
-In ViSP, depending on the visual features that are used three trackers are available:
-- a tracker implemented in vpMbEdgeTracker that consider moving-edges behind the visible lines of the model. This tracker is appropriate to track texture less objects.
-- an other tracker implemented in vpMbKltTracker that consider KLT keypoints that are detected and tracked on each visible face of the model. This tracker is more designed to track textured objects with edges that are not really visible.
-- an hybrid version implemented in vpMbEdgeKltTracker that is able to consider moving-edges and KLT keypoints. This tracker is appropriate to track textured objects with visible edges.
-
-\subsection mb_started_src Example source code
-The following example that comes from tutorial-mb-tracker.cpp allows to track a tea box modeled in cao format using one of the markerless model-based tracker implemented in ViSP.
-
-\include tutorial-mb-tracker.cpp
-
-\note An extension of the previous getting started example is proposed in tutorial-mb-tracker-full.cpp where advanced functionalities such as reading tracker settings from an XML file or visibility computation are implemented.
-
-\note Other tutorials that are specific to a given tracker are provided in tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp.
-
-\subsection mb_started_input Example input data
-
-The previous example uses the following data as input:
-- a video file; "teabox.mpg" is the default video.
-- a cad model that describes the object to track. In our case the file "teabox.cao" is the default one. See \ref mb_model section to learn how the teabox is modelled and section \ref mb_advanced_cao to learn how to model an other object.
-- a file with extension "*.init" that contains the 3D coordinates of some points used to compute an initial pose which serves to initialize the tracker. The user has than to click in the image on the corresponding 2D points. The default file is named "teabox.init". The content of this file is detailed in \ref mb_started_src_explained section.
-- an optional image with extension "*.ppm" that may help the user to remember the location of the corresponding 3D points specified in "*.init" file.
-
-
-\subsection mb_started_exe Running the example
-
-Once build, to see the options that are available in the previous source code, just run:
-\code
-$ ./tutorial-mb-tracker --help
-Usage: ./tutorial-mb-tracker [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]
-\endcode
-By default, teabox.mpg video is used as input. Using "--tracker" option, you can specify which tracker has to be used:
-- Using vpMbEdgeTracker to track only moving-edges:
-\code
-$ ./tutorial-mb-tracker --tracker 0
-\endcode
-will produce results similar to:
-\htmlonly
-<br>
-<iframe width="420" height="315" src="http://www.youtube.com/embed/b__u_yGEbmc?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-- Using vpMbKltTracker to track only keypoints:
-\code
-$ ./tutorial-mb-tracker --tracker 1
-\endcode
-will produce results similar to:
-\htmlonly
-<br>
-<iframe width="420" height="315" src="http://www.youtube.com/embed/eZmUw9r6Idw?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-- Using vpMbEdgeKltTracker to track moving-edges and keypoints in an hybrid scheme:
-\code
-$ ./tutorial-mb-tracker --tracker 2
-\endcode
-will produce results similar to:
-\htmlonly
-<br>
-<iframe width="420" height="315" src="http://www.youtube.com/embed/a-RX9NPF2k0?rel=0" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-With this example it is also possible to work on an other data set using "--name" command line option. For example, if you run:
-\code
-$ ./tutorial-mb-tracker --name myobject
-\endcode
-you have to put the following data in the same folder than the binary:
-- myobject.mpg: A video that contains the object to track.
-- myobject.init: The coordinates of at least four 3D points used for the initialization.
-- myobject.cao: The CAD model of the object to track.
-
-Since the video myobject.mpg was acquired by an other camera, you have to set the camera intrinsic parameters in the source code modifying the line:
-\snippet tutorial-mb-tracker.cpp Set camera parameters
-
-and build again before using "--name" command line option.
-
-\subsection mb_started_src_explained Source code explained
-
-Hereafter is the description of the some lines introduced in the previous example.
-
-First we include the header of the hybrid tracker that includes internally vpMbEdgeTracker and vpMbKltTracker classes.
-\snippet tutorial-mb-tracker.cpp Include
-
-The tracker uses image \c I and the intrinsic camera parameters \c cam as input.
-\snippet tutorial-mb-tracker.cpp Image
-
-As output, it estimates \c cMo, the pose of the object in the camera frame.
-\snippet tutorial-mb-tracker.cpp cMo
-
-Once input image \c teabox.pgm is loaded in \c I, a window is created and initialized with image \c I. Then we create an instance of the tracker depending on "--tracker" command line option.
-\snippet tutorial-mb-tracker.cpp Constructor
-
-Then the corresponding tracker settings are initialized. More details are given in \ref mb_settings section.
-\snippet tutorial-mb-tracker.cpp Set parameters
-
-Now we are ready to load the cad model of the object. ViSP supports cad model in cao format or in vrml format. The cao format is a particular format only supported by ViSP. It doesn't require an additional 3rd party rather then vrml format that require Coin 3rd party. We load the cad model in cao format from teabox.cao file which complete description is provided in \ref mb_teabox_cao with:
-\snippet tutorial-mb-tracker.cpp Load cao
-
-It is also possible to modify the code to load the cad model in vrml format from teabox.wrl file described in \ref mb_teabox_vrml. To this end modify the previous line with:
-\code
-tracker->loadModel(objectname + ".wrl");
-\endcode
-
-Once the model of the object to track is loaded, with the next line the display in the image window of additional drawings in overlay such as the moving edges positions, is then enabled by:
-\snippet tutorial-mb-tracker.cpp Set display
-
-Now we have to initialize the tracker. With the next line we choose to use a user interaction.
-\snippet tutorial-mb-tracker.cpp Init
-
-The user has to click in the image on four vertices with their 3D coordinates defined in the "teabox.init" file. The following image shows where the user has to click.
-
-\image html img-teabox-click.jpg Image "teabox.ppm" used to help the user to initialize the tracker.
-
-Matched 2D and 3D coordinates are then used to compute an initial pose used to initialize the tracker. Note also that the third optional argument "true" is used here to enable the display of an image that may help the user for the initialization. The name of this image is the same as the "*.init" file except the extension that sould be ".ppm". In our case it will be "teabox.ppm".
-
-The content of \c teabox.init file that defines 3D coordinates of some points of the model used during user intialization is provided hereafter. Note that all the characters after character '#' are considered as comments.
-
-\includelineno teabox.init
-
-We give now the signification of each line of this file:
-- line 1: Number of 3D points that are defined in this file. At least 4 points are required. Four is the minimal number of points requested to compute a pose.
-- line 2: Each point is defined by its 3D coordinates. Here we define the first point with coordinates (0,0,0). In the previous figure it corresponds to vertex 0 of the tea box. This point is also the origin of the frame in which all the points are defined.
-- line 3: 3D coordinates of vertex 3.
-- line 4: 3D coordinates of vertex 2.
-- line 5: 3D coordinates of vertex 5.
-
-Here the user has to click on vertex 0, 3, 2 and 5 in the window that displays image \c I. From the 3D coordinates defined in \c teabox.init and the corresponding 2D coordinates of the vertices obtained by user interaction a pose is computed that is then used to initialize the tracker.
-
-Next, in the infinite while loop, after displaying the next image, we track the object on a new image \c I.
-\snippet tutorial-mb-tracker.cpp Track
-
-The result of the tracking is a pose \c cMo that can be obtained by:
-\snippet tutorial-mb-tracker.cpp Get pose
-
-Next lines are used first to retrieve the camera parameters used by the tracker, then to display the visible part of the cad model using red lines with 2 as thickness, and finally to display the object frame at the estimated position \c cMo. Each axis of the frame are 0.025 meters long. Using vpColor::none indicates that x-axis is displayed in red, y-axis in green, while z-axis in blue. The thickness of the axis is 3.
-\snippet tutorial-mb-tracker.cpp Display
-
-The last lines are here to free the memory allocated for the display and tracker:
-\snippet tutorial-mb-tracker.cpp Cleanup
-
-\section mb_model Teabox CAD model
-
-ViSP supports two different ways to describe CAD models, either in cao or in vrml format.
-- cao format is specific to ViSP. It allows to describe the CAD model of an object using a text file with extension \c .cao.
-- vrml format is supported only if Coin 3rd party is installed. This format allows to describe the CAD model of an object using a text file with extension \c .wrl.
-
-
-\subsection mb_teabox_cao teabox.cao example
-
-The content of the file teabox.cao used in the getting started \ref mb_started_src but also in tutorial-mb-edge-tracker.cpp and in tutorial-mb-hybrid-tracker.cpp examples is given here:
-
-\includelineno teabox.cao
-
-This file describes the model of the tea box corresponding to the next image:
-
-\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format.
-
-We make the choice to describe the faces of the box from the 3D points that correspond to the vertices. We provide now a line by line description of the file. Notice that the characters after the '#' are considered as comments.
-- line 1: Header of the \c .cao file
-- line 3: The model is defined by 8 3D points. Here the 8 points correspond to the 8 vertices of the tea box presented in the previous figure. Thus, next 8 lines define the 3D points coordinates.
-- line 4: 3D point with coordinate (0,0,0) corresponding to vertex 0 of the tea box. This point is also the origin of the frame in which all the 3D points are defined.
-- line 5: 3D point with coordinate (0,0,-0.08) corresponding to vertex 1.
-- line 6 to 11: The other 3D points corresponding to vertices 2 to 7 respectively.
-- line 13: Number of 3D lines defined from two 3D points. It is possible to introduce 3D lines and then use these lines to define faces from these 3D lines. This is particularly useful to define faces from non-closed polygons. For instance, it can be used to specify the tracking of only 3 edges of a rectangle. Notice also that a 3D line that doesn't belong to a face is always visible and consequently always tracked.
-- line 15: Number of faces defined from 3D lines. In our teabox example we decide to define all the faces from 3D points, that is why this value is set to 0.
-- line 17: The number of faces defined by a set of 3D points. Here our teabox has 6 faces. Thus, next 6 lines describe each face from the 3D points defined previously line 4 to 11. Notice here that all the faces defined from 3D points corresponds to closed polygons.
-- line 18: First face defined by 4 3D points, respectively vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
-- line 19: Second face also defined by 4 points, respectively vertices 1,6,5,2 to have a counter clockwise orientation.
-- line 20 to 23: The four other faces of the box.
-- line 25: Number of 3D cylinders describing the model. Since we model a simple box, the number of cylinders is 0.
-- line 27: Number of 3D circles describing the model. For the same reason, the number of circles is 0.
-
-\subsection mb_teabox_cao_triangle teabox-triangle.cao example
-
-The content of the file teabox-triangle.cao used in the tutorial-mb-klt-tracker.cpp example is given here:
-
-\includelineno teabox-triangle.cao
-
-This file describes the model of the tea box corresponding to the next image:
-
-\image html img-teabox-cao-triangle.jpg Index of the vertices used to model the tea box in cao format with triangles.
-
-Until line 15, the content of this file is similar to the one described in
-\ref mb_teabox_cao. Line 17 we specify that the model contains 12 faces. Each face is then described as a triangle.
-
-\note Since some lines of the model (for example the one between points 0 and 2, or 7 and 3...) don't correspond to teabox edges, this CAD model is not suited for moving-edges and hybrid trackers.
-
-\subsection mb_teabox_vrml teabox.wrl example
-
-The content of the teabox.wrl file used in tutorial-mb-tracker-full.cpp and tutorial-mb-edge-tracker.cpp when teabox.cao is missing is given hereafter. This content is to make into relation with teabox.cao described in \ref mb_teabox_cao. As for the cao format, teabox.wrl describes first the vertices of the box, then the edges that corresponds to the faces.
-
-\includelineno teabox.wrl
-
-This file describes the model of the tea box corresponding to the next image:
-
-\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in vrml format.
-
-We provide now a line by line description of the file where the faces of the box are defined from the vertices:
-- line 1 to 10: Header of the \c .wrl file.
-- line 13 to 20: 3D coordinates of the 8 tea box vertices.
-- line 34 to 29: Each line describe a face. In this example, a face is defined by 4 vertices. For example, the first face join vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
-
-
-\section mb_settings Tracker settings
-
-\subsection mb_settings_xml Settings from an XML file
-
-
-Instead of setting the tracker parameters from source code, if libxml2 is available it is possible to specify the settings from an XML file. As done in tutorial-mb-tracker-full.cpp example, to read the parameters from an XML file, simply modify the code like:
-\snippet tutorial-mb-tracker-full.cpp Load xml
-The content of the XML file teabox.xml that is considered by default is the following:
-\code
-<?xml version="1.0"?>
-<conf>
- <ecm>
- <mask>
- <size>5</size>
- <nb_mask>180</nb_mask>
- </mask>
- <range>
- <tracking>8</tracking>
- </range>
- <contrast>
- <edge_threshold>10000</edge_threshold>
- <mu1>0.5</mu1>
- <mu2>0.5</mu2>
- </contrast>
- <sample>
- <step>4</step>
- </sample>
- </ecm>
- <klt>
- <mask_border>5</mask_border>
- <max_features>300</max_features>
- <window_size>5</window_size>
- <quality>0.015</quality>
- <min_distance>8</min_distance>
- <harris>0.01</harris>
- <size_block>3</size_block>
- <pyramid_lvl>3</pyramid_lvl>
- </klt>
- <camera>
- <u0>325.66776</u0>
- <v0>243.69727</v0>
- <px>839.21470</px>
- <py>839.44555</py>
- </camera>
- <face>
- <angle_appear>70</angle_appear>
- <angle_disappear>80</angle_disappear>
- <near_clipping>0.1</near_clipping>
- <far_clipping>100</far_clipping>
- <fov_clipping>1</fov_clipping>
- </face>
-</conf>
-\endcode
-
-Depending on the tracker all the XML tags are not useful:
-- \<ecm\> tag corresponds to the moving-edges settings. This tag is used by vpMbEdgeTracker and vpMbEdgeKltTracker.
-- \<klt\> tag corresponds to the keypoint settings. This tag is used by vpMbKltTracker and vpMbEdgeKltTracker.
-- \<camera\> and \<face\> tags are used by all the trackers.
-
-\subsection mb_settings_ecm Moving-edges settings
-Moving-edges settings affect the way the visible edges of an object are tracked.
-These settings could be tuned either from XML using \<ecm\> tag as:
-
-\code
-<conf>
- ...
- <ecm>
- <mask>
- <size>5</size>
- <nb_mask>180</nb_mask>
- </mask>
- <range>
- <tracking>8</tracking>
- </range>
- <contrast>
- <edge_threshold>10000</edge_threshold>
- <mu1>0.5</mu1>
- <mu2>0.5</mu2>
- </contrast>
- <sample>
- <step>4</step>
- </sample>
- </ecm>
- ...
-</conf>
-\endcode
-
-of from source code using vpMbEdgeTracker::setMovingEdge() method:
-\snippet tutorial-mb-tracker-full.cpp Set moving-edges parameters
-
-With the previous parameters you can set:
-- mask_size: defines the size of the convolution mask used to detect an edge.
-- nb_mask: number of mask applied to determine the object contour. The number of mask determines the precision of the normal of the edge for every sample. If precision is 2deg, then there are 360/2 = 180 masks.
-- range_tracking: range on both sides of the reference pixel along the normal of the contour used to track a moving-edge. If the displacement of the tracked object in two successive images is large, you have to increase this parameter.
-- edge_threshold: likelihood threshold used to determined if the moving edge is valid or not.
-- mu1: minimum image contrast allowed to detect a contour.
-- mu2: maximum image contrast allowed to detect a contour.
-- sample_step: minimum distance in pixel between two discretized moving-edges. To increase the number of moving-edges you have to reduce this parameter.
-
-\note Most important parameters are \e range_tracking and \e sample_step.
-
-
-\subsection mb_settings_klt Keypoints settings
-Keypoint settings affect tracking of keypoints on visible faces using KLT.
-These settings could be tuned either from XML using \<klt\> tag as:
-
-\code
-<conf>
- ...
- <klt>
- <mask_border>5</mask_border>
- <max_features>300</max_features>
- <window_size>5</window_size>
- <quality>0.015</quality>
- <min_distance>8</min_distance>
- <harris>0.01</harris>
- <size_block>3</size_block>
- <pyramid_lvl>3</pyramid_lvl>
- </klt>
- ...
-</conf>
-\endcode
-
-of from source code using vpMbKltTracker::setKltOpencv() and vpMbKltTracker::setMaskBorder() methods:
-\snippet tutorial-mb-tracker-full.cpp Set klt parameters
-
-With the previous parameters you can set:
-- mask_border: erosion number of the mask used on each face.
-- max_features: maximum number of keypoint features to track in the image.
-- window_size: window size used to refine the corner locations.
-- quality: parameter characterizing the minimal accepted quality of image corners. Corners with quality measure less than this parameter are rejected. This means that if you want to have more keypoints on a face, you have to reduce this parameter.
-- min_distance: minimal Euclidean distance between detected corners during keypoint detection stage used to initialize keypoint location.
-- harris: free parameter of the Harris detector.
-- size_block: size of the averaging block used to track the keypoint features.
-- pyramid_lvl: maximal pyramid level. If the level is zero, then no pyramid is computed for the optical flow.
-
-\note Most important parameters are \e min_distance and \e quality.
-
-\subsection mb_settings_cam Camera settings
-Camera settings correspond to the intrinsic camera parameters without distorsion. If images are acquired by a camera that has a large field of view that introduce distorsion, images need to be undistorded before processed by the tracker. The camera parameters are then the one obtained on undistorded images.
-
-Camera settings could be set from XML using \<camera\> tag as:
-\code
-<conf>
- ...
- <camera>
- <u0>325.66776</u0>
- <v0>243.69727</v0>
- <px>839.21470</px>
- <py>839.44555</py>
- </camera>
- ...
-</conf>
-\endcode
-of from source code using vpMbTracker::setCameraParameters() method:
-\snippet tutorial-mb-tracker-full.cpp Set camera parameters
-
-As described in vpCameraParameters class, these parameters correspond to \f$(p_x, p_y)\f$ the ratio between the focal length and the size of a pixel, and \f$(u_0, v_0)\f$ the coordinates of the principal point in pixel.
-
-\note The \ref tutorial-calibration explains how to obtain these parameters from a camera calibration stage.
-
-\subsection mb_settings_visibility Visibility settings
-
-An important setting concerns the visibility test that is used to determine if a face is visible or not. Note that moving-edges and keypoints are only tracked on visible faces. Three different visibility tests are implemented; with or without Ogre ray tracing and with or without scanline rendering. The default test is the one without Ogre and scanline. The functions vpMbTracker::setOgreVisibilityTest() and vpMbTracker::setScanLineVisibilityTest() allow to select which test to use.
-
-\subsubsection mb_settings_visibility_default Default visibility based on normals
-Let us now highlight how the default visibility test works. As illustrated in the following figure, the angle \f$ \alpha \f$ between the normal of the face and the line going from the camera to the center of gravity of the face is used to determine if the face is visible.
-
-\image html img-tracker-mb-visibility.jpg Principle of the visibility test used to determine if a face is visible.
-
-When no advanced visibility test is enable (we recall that this is the default behavior), the algorithm that computes the normal of the face is very simple. It makes the assumption that faces are convex and oriented counter clockwise. If we consider two parameters; the angle to determine if a face is appearing \f$ \alpha_{appear} \f$, and the angle to determine if the face is disappearing \f$ \alpha_{disappear} \f$, a face will be considered as visible if \f$ \alpha \leq \alpha_{disapp [...]
-\code
-<conf>
- ...
- <face>
- <angle_appear>70</angle_appear>
- <angle_disappear>80</angle_disappear>
- </face>
-\endcode
-or in the code:
-\snippet tutorial-mb-tracker-full.cpp Set angles
-
-Here the face is considered as appearing if \f$ \alpha \leq 70\f$ degrees, and disappearing if \f$ \alpha > 80\f$ degrees.
-
-\note When these two angle parameters are not set, their default values set to 89 degrees are used.
-
-\subsubsection mb_settings_visibility_ogre Advanced visibility via Ogre3D
-
-The Ogre3D visibility test approach is based on ray tracing. When this test is enabled, the algorithm used to determine the visibility of a face performs (<b>in addition to the previous test based on normals, i.e on the visible faces resulting from the previous test</b>) another test which sets the faces that are <b>partially occluded as non-visible</b>. It can be enabled via:
-\code
- tracker->setOgreVisibilityTest(true);
-\endcode
-\image html img-tracker-mb-visibility-ogre.png "Ogre visibility test on both polygons."
-
-When using the <b>classical version of the ogre visibility test</b> (which is the default behavior when activating this test), <b>only one ray</b> is used per polygon to test its visibility. As shown on the figure above, this only ray is sent from the camera to the center of gravity of the considered polygon. If the ray intersects another polygon before the considered one, it is set as non-visible. Intersections are computed <b>between the ray and the axis-aligned bounding-box (AABB)</b> [...]
-
-Additionnaly, it is also possible to use a statistical approach during the ray tracing phase in order to improve the visibility results.
-\code
- tracker->setNbRayCastingAttemptsForVisibility(4);
- tracker->setGoodNbRayCastingAttemptsRatio(0.70);
-\endcode
-\image html img-tracker-mb-visibility-ogre-advanced.png "Ogre visibility test on the first polygon, using a statistical approach."
-
-Contrary to the classical version of this test, the <b>statistical approach uses multiple rays per polygons</b> (4 in the example above). Each ray is sent randomly toward the considered polygon. If a specified ratio of rays do not have intersected another polygon before the considered one, the polygon is set as visible. In the example above, three ray on four return the first polygon as visible. As the ratio of good matches is more than 70% (which corresponds to the chosen ratio in this [...]
-
-\note Since ViSP 3.0.0 we have introduced vpMbTracker::setOgreShowConfigDialog() method that allows to open the Ogre configuration pannel which can be used to select the renderer. To enable this feature, use:
-
-\code
- tracker->setOgreShowConfigDialog(true);
-\endcode
-
-\subsubsection mb_settings_visibility_scanline Advanced visibility via Scanline Rendering
-
-Contrary to the visibility test using Ogre3D, this method <b>does not require any additional third-party library</b>. As for the advanced visibility using Ogre3D, <b>this test is applied in addition to the test based on normals (i.e on the faces set as visible during this test) and also in addition to the test with Ogre3D if it has been activated</b>. This test is based on the scanline rendering algorithm and can be enabled via:
-\code
- tracker->setScanLineVisibilityTest(true);
-\endcode
-\image html img-tracker-mb-visibility-scanline.png "Scanline visibility test on both polygons."
-
-Even if this approach requires a bit <b>more computing power</b>, it is a <b>pixel perfect visibility test</b>. According to the camera point of view, polygons will be <b>decomposed in order to consider only their visible parts</b>. As a result, if we consider the example above, dashed red lines won't be considered during the tracking phase and detected keypoints will be correctly matched with the closest (in term of depth from the camera position) polygon.
-
-\subsection mb_settings_clipping Clipping settings
-
-Additionally to the visibility test described above, it is also possible to use clipping. Firstly, the algorithm removes the faces that are not visibles, according to the visibility test used, then it will also remove the faces or parts of the faces that are out of the clipping planes. As illustrated in the following figure, different clipping planes can be enabled.
-
-\image html img-fov.png Camera field of view and clipping planes.
-
-Let us consider two plane categories: the ones belonging to the field of view or FOV (Left, Right, Up and Down), and the Near and Far clipping planes. The FOV planes can be enabled by:
-
-\snippet tutorial-mb-tracker-full.cpp Set clipping fov
-
-which is equivalent to:
-
-\code
- tracker->setClipping(vpMbtPolygon::LEFT_CLIPPING
- | vpMbtPolygon::RIGHT_CLIPPING
- | vpMbtPolygon::UP_CLIPPING
- | vpMbtPolygon::DOWN_CLIPPING);
-\endcode
-
-Of course, if the user just wants to activate Right and Up clipping, he will use:
-
-\code
- tracker->setClipping(vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING);
-\endcode
-
-For the Near and Far clipping it is quite different. Indeed, those planes require clipping distances. Here there are two choices, either the user uses default values and activate them with:
-
-\code
- tracker->setClipping(vpMbtPolygon::NEAR_CLIPPING | vpMbtPolygon::FAR_CLIPPING);
-\endcode
-
-or the user can specify the distances in meters, which will automatically activate the clipping for those planes:
-
-\snippet tutorial-mb-tracker-full.cpp Set clipping distance
-
-It is also possible to enable them in the XML file. This is done with the following lines:
-
-\code
-<conf>
- ...
- <face>
- ...
- <near_clipping>0.1</near_clipping>
- <far_clipping>100.0</far_clipping>
- <fov_clipping>0</fov_clipping>
- </face>
-\endcode
-
-Here for simplicity, the user just has the possibility to either activate all the FOV clipping planes or none of them (fov_clipping requires a boolean).
-
-\note When clipping parameters are not set in the XML file, nor in the code, clipping is not used. Usually clipping is not helpful when the oject to track is simple.
-
-
-\section mb_advanced Advanced
-\subsection mb_advanced_model How to manipulate the model
-
-The following code shows how to access to the CAD model
-- to check if a face is visible,
-- to get the name of the face (only with models in .cao format for the moment)
-- to check if the level of detail is enable/disable (only with models in .cao format for the moment)
-- to access to the coordinates of the 3D points used to model a face
-- from the pose \e cMo estimated by the tracker to compute the coordinates of the 3D points in the image
-
-\code
- vpMbHiddenFaces<vpMbtPolygon> &faces = tracker.getFaces();
- std::cout << "Number of faces: " << faces.size() << std::endl;
- for (unsigned int i=0; i < faces.size(); i++) {
- std::vector<vpMbtPolygon*> &poly = faces.getPolygon();
- std::cout << "face " << i << " with index: " << poly[i]->getIndex()
- << (poly[i]->getName().empty() ? "" : (" with name: " + poly[i]->getName()))
- << " is " << (poly[i]->isVisible() ? "visible" : "not visible")
- << " and has " << poly[i]->getNbPoint() << " points"
- << " and LOD is" << (poly[i]->useLod ? "enabled" : "disabled") << std::endl;
-
- for (unsigned j=0; j<poly[i]->getNbPoint(); j++) {
- vpPoint P = poly[i]->getPoint(j);
- P.project(cMo);
- std::cout << " P obj " << j << ": " << P.get_oX() << " " << P.get_oY() << " " << P.get_oZ() << std::endl;
- std::cout << " P cam" << j << ": " << P.get_X() << " " << P.get_Y() << " " << P.get_Z() << std::endl;
-
- vpImagePoint iP;
- vpMeterPixelConversion::convertPoint(cam, P.get_x(), P.get_y(), iP);
- std::cout << " iP " << j << ": " << iP.get_u() << " " << iP.get_v() << std::endl;
- }
- }
-\endcode
-
-
-\subsection mb_advanced_lod Level of detail (LOD)
-
-The level of detail (LOD) consists in introducing additional constraints to the visibility check to determine if the features of a face have to be tracked or not. Two parameters are used
-- the line length (in pixel)
-- the area of the face (in pixel²), that could be closed or not (you can define an open face by adding all the segments without the last one which closes the face)
-
-The tracker allows to enable/disable the level of detail concept using vpMbTracker::setLod() function.
-This example permits to set LOD settings to all elements :
-\code
-tracker.setLod(true);
-tracker.setMinLineLengthThresh(40.0);
-tracker.setMinPolygonAreaThresh(500.0);
-\endcode
-
-This example permits to set LOD settings to specific elements denominated by his name.
-\code
-tracker.setLod(false);
-tracker.setLod(true, "Left line");
-tracker.setLod(true, "Front face");
-tracker.setMinLineLengthThresh(35.0, "Left line");
-tracker.setMinPolygonAreaThresh(120.0, "Front face");
-\endcode
-
-Furthermore, to set a name to a face see \ref mb_advanced_cao_nam.
-
-
-\subsection mb_advanced_cao CAD model in cao format
-
-\subsubsection mb_advanced_cao_lin How to model faces from lines
-The first thing to do is to declare the differents points. Then you define each segment of the face with the index of the start point and with the index of the end point.
-Finally, you define the face with the index of the segments which constitute the face.
-
-\note The way you declare the face segments (clockwise or counter clockwise) will determine the direction of the normal of the face and so will influe on the visibility of the face.
-
-\code
-V1
-# Left wing model
-6 # Number of points
-# 3D points
--4 -3.8 0.7
--6 -8.8 0.2
--12 -21.7 -1.2
--9 -21.7 -1.2
- 0.8 -8.8 0.2
- 4.6 -3.8 0.7
-# 3D lines
-6 # Number of lines
-0 1 # line 0
-1 2
-2 3
-3 4
-4 5
-5 0 # line 5
-# Faces from 3D lines
-1 # Number of faces defined by lines
-6 0 1 2 3 4 5 # face 0: [number of lines] [index of the lines]...
-# Faces from 3D points
-0
-# 3D cylinders
-0
-# 3D circles
-0
-\endcode
-
-
-\subsubsection mb_advanced_cao_cyl How to model cylinders
-The first thing to do is to declare the two points defining the cylinder axis of revolution. Then you declare the cylinder with the index of the points that define the cylinder axis of revolution and with the cylinder radius.
-
-\note For the level of detail, in a case of a cylinder, this is taking into account by using the length of the axis of revolution to determine the visibility.
-
-\image html img-cylinder.png Example of a cylinder.
-
-\code
-V1
-# Cylinder model
-2 # Number of points
-# 3D points
-16.9 0 0.5 # point 0
--20 0 0.5 # point 1
-# 3D lines
-0
-# Faces from 3D lines
-0
-# Faces from 3D points
-0
-# 3D cylinders
-1 # Number of cylinders
-0 1 2.4 # cylinder 0: [1st point on revolution axis] [2nd point on revolution axis] [radius]
-# 3D circles
-0
-\endcode
-
-
-\subsubsection mb_advanced_cao_cir How to model circles
-The first thing to do is to declare three points: one point for the center of the circle and two points on the circle plane (i.e. not necessary located on the perimeter of the circle but on the plane of the circle). Then you declare your circle with the radius and with index of the three points.
-
-\note The way you declare the two points on the circle plane (clockwise or counter clockwise) will determine the direction of the normal of the circle and so will influe on the visibility of the circle.
-For the level of detail, in a case of a circle, this is taking into account by using the area of the bounding box of the circle to determine the visibility.
-
-\image html img-circle.png Example of a circle.
-
-\code
-V1
-# Circle model
-3 # Number of points
-# 3D points
--3.4 14.6 1.1 # point 0
--3.4 15.4 1.1
--3.4 14.6 1.8 # point 2
-# 3D lines
-0
-# Faces from 3D lines
-0
-# Faces from 3D points
-0
-# 3D cylinders
-0
-# 3D circles
-1 # Number of circles
-0.8 0 2 1 # circle 0: [radius] [circle center] [1st point on circle plane] [2nd point on circle plane]
-\endcode
-
-
-\subsubsection mb_advanced_cao_hie How to create a hierarchical model
-It could be useful to define a complex model instead of using one big model file with all the declaration with different sub-models, each one representing a specific part of the complex model in a specific model file.
-To create a hierarchical model, the first step is to define all the elementary parts and then regroup them.
-
-\image html img-plane-hierarchical-diagram.jpg Example of a possible hierarchical modelling of a plane.
-
-For example, if we want to have a model of a plane, we could represent as elementary parts the left and right wings, the tailplane (which is constituted of some other parts) and a cylinder for the plane fuselage.
-The following lines represent the top model of the plane.
-
-\code
-V1
-# header
-# load the different parts of the plane
-load("wings.cao") # load the left and right wings
-load("tailplane.cao")
-# 3D points
-2 # Number of points
-16.9 0 0.5
--20 0 0.5
-# 3D lines
-0
-# Faces from 3D lines
-0
-# Faces from 3D points
-0
-# 3D cylinders
-1 # Number of cylinders
-0 1 2.4 # define the plane fuselage as a cylinder
-# 3D circles
-0
-\endcode
-
-\note The path to include another model can be expressed as an absolute or a relative path (relative to the file which includes the model).
-
-
-\subsubsection mb_advanced_cao_nam How to set a name to a face
-To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
-
-It could be useful to give a name for a face in a model in order to easily modify his LOD parameters for example, or to decide if you want to use this face or not during the tracking phase.
-This is done directly in the model file :
-\code
-V1
-# header
-# load the different parts of the plane
-load("wings.cao")
-load("tailplane.cao")
-# 3D points
-5 # Number of points
-16.9 0 0.5
--20 0 0.5
--3.4 14.6 1.1
--3.4 15.4 1.1
--3.4 14.6 1.8
-# 3D lines
-0
-# Faces from 3D lines
-0
-# Faces from 3D points
-0
-# 3D cylinders
-1 # Number of cylinders
-0 1 2.4 name=plane_fuselage
-# 3D circles
-1 # Number of circles
-0.8 2 4 3 name="right reactor"
-\endcode
-
-\note If the name contains space characters, it must be surrounded by quotes.
-You can give a name to all the elements excepts for points.
-
-
-\subsubsection mb_advanced_cao_lod How to tune the level of detail
-
-As explained in section \ref mb_advanced_lod the parameters of the lod can be set in the source code. They can also be set directly in the configuration file or in the CAD model in cao format.
-
-The following lines show the content of the configuration file :
-
-\code
-<?xml version="1.0"?>
-<conf>
- <lod>
- <use_lod>1</use_lod>
- <min_line_length_threshold>40</min_line_length_threshold>
- <min_polygon_area_threshold>150</min_polygon_area_threshold>
- </lod>
-</conf>
-\endcode
-
-In CAD model file, you can specify the LOD settings to the desired elements :
-
-\code
-V1
-# header
-# load the different parts of the plane
-load("wings.cao")
-load("tailplane.cao")
-# 3D points
-5 # number of points
-16.9 0 0.5
--20 0 0.5
--3.4 14.6 1.1
--3.4 15.4 1.1
--3.4 14.6 1.8
-# 3D lines
-0
-# Faces from 3D lines
-0
-# Faces from 3D points
-0
-# 3D cylinders
-1 # Number of cylinders
-0 1 2.4 name=plane_fuselage useLod=true minLineLengthThreshold=100.0
-# 3D circles
-1 # Number of circles
-0.8 2 4 3 name="right reactor" useLod=true minPolygonAreaThreshold=40.0
-\endcode
-
-\note The order you call the methods to load the configuration file and to load the CAD model in the code will modify the result of the LOD parameters.
-Basically, the LOD settings expressed in configuration file will have effect on all the elements in the CAD model while the LOD settings expressed in CAD model will be specific to an element.
-The natural order would be to load first the configuration file and after the CAD model.
-
-\subsection mb_advanced_wrml CAD model in wrml format
-\subsubsection mb_advanced_wrml_nam How to set a name to a face
-
-To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
-
-When using a wrml file, names are associated with shapes. In the example below (the model of a teabox), as only one shape is defined, all its faces will have the same name: "teabox_name".
-
-\note If you want to set different names for different faces, you have to define them in different shapes.
-
-\code
-#VRML V2.0 utf8
-
-DEF fst_0 Group {
-children [
-
-# Object "teabox"
-
-DEF teabox_name Shape {
-
-geometry DEF cube IndexedFaceSet {
-
-coord Coordinate {
-point [
-0 0 0 ,
-0 0 -0.08,
-0.165 0 -0.08,
-0.165 0 0 ,
-0.165 0.068 0 ,
-0.165 0.068 -0.08,
-0 0.068 -0.08,
-0 0.068 0 ]
-}
-
-coordIndex [
- 0,1,2,3,-1,
- 1,6,5,2,-1,
- 4,5,6,7,-1,
- 0,3,4,7,-1,
- 5,4,3,2,-1,
- 0,7,6,1,-1]}
-}
-
-]
-}
-\endcode
-
-\subsection mb_advanced_exclude How not to consider specific polygons
-
-When using model-based trackers, it is possible not to consider edge tracking or keypoint tracking for specific faces. To do so, <b>the faces you want to consider must have a name</b>.
-
-If you want to enable (default behavior) or disable the edge tracking on specific face it can be done via:
-
-\code
-vpMbEdgeTracker::setUseEdgeTracking("name of the face", boolean);
-\endcode
-
-If the boolean is set to False, the tracking of the edges of the faces that have the given name will be disable. If it is set to True (default behavior), it will be enable.
-
-As for the edge tracking, the same functionnality is also available when using keypoints via:
-
-\code
-vpMbKltTracker::setUseKltTracking("name of the face", boolean);
-\endcode
-
-\section mb_known_issues Known issues
-\subsection mb_known_issues_example_with_ogre Model-based trackers examples are not working with Ogre visibility ckeck
-
-If you run mbtEdgeTracking.cpp, mbtKltTracking.cpp or mbtEdgeKltTracking.cpp examples enabling Ogre visibility check (using "-o" option), you may encounter the following issue that was obtained following \ref tutorial-install-win81-msvc :
-\code
-C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
-...
-OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
-...
-*** Initializing OIS ***
-\endcode
-and then a wonderful runtime issue as in the next image:
-\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
-
-It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
-\code
-C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
-\endcode
-Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
-\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
-
-\subsection mb_known_issues_tutorial_with_ogre Model-based trackers tutorials are not working with Ogre visibility ckeck
-
-This issue is similar to \ref mb_known_issues_example_with_ogre. It may occur with tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp. To make working the tutorials:
-
-- modify the code like:
-\code
- tracker.setOgreVisibilityTest(true);
- tracker.setOgreShowConfigDialog(true);
-\endcode
-- build the modified tutorial
-- run the binary. Now the binary should open the Ogre configuration window where you have to select an Ogre renderer that is working on your computer.
-\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
-- Press then OK to continue and start the tracking of the object.
-
-\section mb_next Next tutorial
-You are now ready to see the next \ref tutorial-tracking-mb-stereo if you want to extend the tracking on multiple view images or \ref tutorial-tracking-tt.
-
-
-*/
diff --git a/doc/tutorial-tracking-tt.doc b/doc/tutorial-tracking-tt.doc
deleted file mode 100644
index 58b9f95..0000000
--- a/doc/tutorial-tracking-tt.doc
+++ /dev/null
@@ -1,226 +0,0 @@
-/**
-
-\page tutorial-tracking-tt Tutorial: Template tracking
-\tableofcontents
-
-\section tracking_tt_intro Introduction
-
-With ViSP it is possible to track a template using image registration algorithms \cite Dame10c \cite Dame11c. Contrary to common approaches based on visual features, this method allows to be much more robust to scene variations.
-
-In the following sections, we consider the tracking of a pattern. To simplify the source code, the tracking is performed on a single image. The extension to a sequence of images or to images acquired from a camera is easy. To this end see \ref tutorial-grabber.
-
-Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
-
-\code
-$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/template-tracker
-\endcode
-
-
-\section tracking_tt Track the painting
-
-The following example that comes from tutorial-template-tracker.cpp allows to track a template using vpTemplateTrackerSSDInverseCompositional class.
-Let us denote that "SSDInverseCompositional" refers to the similarity function used for the image registration. In ViSP, we have implemented, for now, three different similarity functions: the "Sum of Square Differences" (vpTemplateTrackerSSD classes \cite Baker04a), the "Zero-mean Normalized Cross Correlation" (vpTemplateTrackerZNCC classes \cite Irani98a) and the "Mutual Information" (vpTemplateTrackerMI classes \cite Dame10c). All the methods can be used in different ways: Inverse Com [...]
-
-As detailed in \ref warp_tt section, the tracker is able to track a reference template (in our case a painting) and to estimate the transformation between the reference template and its current position. The estimated transformationn could be modeled as:
-- a pure 2D translation (see vpTemplateTrackerWarpTranslation)
-- a 2D translation with an additional scaling factor (see vpTemplateTrackerWarpSRT)
-- an affine transformation (see vpTemplateTrackerWarpAffine)
-- a homography (see vpTemplateTrackerWarpHomography or vpTemplateTrackerWarpHomographySL3).
-
-\include tutorial-template-tracker.cpp
-
-The video below shows the result of the template tracking.
-
-\htmlonly
-<iframe width="420" height="315" src="http://www.youtube.com/embed/hniUcaUSVBM" frameborder="0" allowfullscreen></iframe>
-\endhtmlonly
-
-Hereafter is the description of the new lines introduced in this example.
-
-\snippet tutorial-template-tracker.cpp Include
-
-Here we include the header of the vpTemplateTrackerSSDInverseCompositional class that allows to track the template. Actually, the tracker estimates the displacement of the template in the current image according to its initial pose. The computed displacement can be represented by multiple transformations, also called warps (vpTemplateTrackerWarp classes). In this example, we include the header vpTemplateTrackerWarpHomography class to define the possible transformation of the template as [...]
-
-\snippet tutorial-template-tracker.cpp Construction
-
-Once the tracker is created with the desired warp function, parameters can be tuned to be more consistent with the expected behavior. Depending on these parameters the perfomances of the tracker in terms of processing time and estimation could be affected. Since here we deal with 640 by 480 pixel wide images, the images are significantly subsampled in order to reduce the time of the image processing to be compatible with real-time.
-
-\code
- tracker.setSampling(2, 2); // Will consider only one pixel from two along rows and columns
- // to create the reference template
- tracker.setLambda(0.001); // Gain used in the optimization loop
- tracker.setIterationMax(200); // Maximum number of iterations for the optimization loop
- tracker.setPyramidal(2, 1); // First and last level of the pyramid. Full resolution image is at level 0.
-\endcode
-
-The last step of the initialization is to select the template that will be tracked during the sequence.
-
-\snippet tutorial-template-tracker.cpp Init
-
-The vpTemplateTracker classes proposed in ViSP offer you the possibility to defined your template as multiple planar triangles. When calling the previous line, you will have to specify the triangles that define the template.
-
-\image html img-initClickTemplateTracker.png Initialization of the template without Delaunay triangulation.
-
-Let us denote that those triangles don't have to be spatially tied up. However, if you want to track a simple image as in this example, you should initialize the template as on the figure above. Left clicks on point number zero, one and two create the green triangle. Left clicks on point three and four and then right click on point number five create the red triangle and ends the initialization.
-If ViSP is build with OpenCV, we also provide an initialization with automatic triangulation using Delaunay. To use it, you just have to call vpTemplateTracker::initClick(I, true). Then by left clicking on points number zero, one, two, four and right clicking on point number five initializes the tracker as on the image above.
-
-Next, in the infinite while loop, after displaying the next image, we track the object on a new image I.
-
-\snippet tutorial-template-tracker.cpp Track
-
-If you need to get the parameters of the current transformation of the template, it can be done by calling:
-
-\snippet tutorial-template-tracker.cpp Homography
-
-For further information about the warping parameters, see the following \ref warp_tt section.
-
-Then according to the computed transformation obtained from the last call to track() function, next line is used to display the template using red lines.
-
-\snippet tutorial-template-tracker.cpp Display
-
-\subsection warp_tt Warping classes
-
-In the example presented above, we focused on the vpTemplateTrackerWarpHomography warping class which is the most generic transformation available in ViSP for the template trackers. However, if you know that the template you want to track is constrained, other warps might be more suitable.
-
-\b vpTemplateTrackerWarpTranslation
-
-\f$w({\bf x},{\bf p}) = {\bf x} + {\bf t}\f$ with the following estimated parameters \f$ {\bf p} = (t_x, t_y)\f$
-
-This class is the most simple transformation available for the template trackers. It only considers translation on two-axis (x-axis and y-axis).
-
-\b vpTemplateTrackerWarpSRT
-
-\f$w({\bf x},{\bf p}) = (1+s){\bf Rx} + {\bf t}\f$ with \f${\bf p} = (s, \theta, t_x, t_y)\f$
-
-The SRT warp considers a scale factor, a rotation on z-axis and a 2D translation as in vpTemplateTrackerWarpTranslation.
-
-\b vpTemplateTrackerWarpAffine
-
-
-\f$ w({\bf x},{\bf p}) = {\bf Ax} + {\bf t}\f$ with \f${\bf A} = \left( \begin{array}{cc}
- 1+a_0 & a_2 \\
- a_1 & 1+a_3
- \end{array} \right)\f$, \f${\bf t} = \left( \begin{array}{c}
- t_x \\
- t_y
- \end{array} \right)\f$ and the estimated parameters \f${\bf p} = (a_0 ... a_3, t_x, t_y)\f$
-
-The template transformation can also be defined as an affine transformation. This warping function preserves points, straight lines, and planes.
-
-\b vpTemplateTrackerWarpHomography
-
-\f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f$ {\bf H}=\left( \begin{array}{ccc}
- 1 + p_0 & p_3 & p_6 \\
- p_1 & 1+p_4 & p_7 \\
- p_2 & p_5 & 1.0
- \end{array} \right) \f$ and the estimated parameters \f${\bf p} = (p_0 ... p_7)\f$
-
-As remind, the vpTemplateTrackerWarpHomography estimates the eight parameters of the homography matrix \f$ {\bf H}\f$.
-
-\b vpTemplateTrackerWarpHomographySL3
-
-\f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f${\bf p} = (p_0 ... p_7)\f$
-
-The vpTemplateTrackerWarpHomographySL3 warp works exactly the same as the vpTemplateTrackerWarpHomography warp. The only difference is that here, the parameters of the homography are estimated in the SL3 reference frame.
-
-\subsection tune_tt How to tune the tracker
-
-When you want to obtain a perfect pose estimation, it is often time-consuming. However, by tuning the tracker, you can find a good compromise between speed and efficiency. Basically, what will make the difference is the size of the reference template. The more pixels it contains, the more time-consuming it will be. Fortunately, the solutions to avoid this problem are multiple. First of all lets come back on the vpTemplateTracker::setSampling() function.
-
-\code
-tracker.setSampling(4, 4); // Will consider only one pixel from four along rows and columns
- // to create the reference template.
-\endcode
-
-In the example above, we decided to consider only one pixel from 16 (4 by 4) to create the reference template. Obviously, by increasing those values it will consider much less pixels, which unfortunately decrease the efficiency, but the tracking phase will be much faster.
-
-The tracking phase relies on an iterative algorithm minimizing a cost function. What does it mean? It means this algorithm has, at some point, to stop! Once again, you have the possibility to reduce the number of iterations of the algorithm by taking the risk to fall in a local minimum.
-
-\code
-tracker.setIterationMax(50); // Maximum number of iterations for the optimization loop.
-\endcode
-
-If this is still not enough for you, let's remember that all of our trackers can be used in a pyramidal way. By reducing the number of levels considered by the algorithm, you will consider, once again, much less pixels and be faster.
-
-\code
-tracker.setPyramidal(3, 2); // First and last level of the pyramid
-\endcode
-Note here that when vpTemplateTracker::setPyramidal() function is not used, the pyramidal approach to speed up the algorithm is not used.
-
-Let us denote that if you're using vpTemplateTrackerSSDInverseCompositional or vpTemplateTrackerZNCCInverseCompositional, you also have another interesting option to speed up your tracking phase.
-
-\code
-tracker.setUseTemplateSelect(true);
-\endcode
-
-This function will force the tracker to only consider, in the reference template, the pixels that have an high gradient value. This is another solution to limit the number of considered pixels.
-
-As well as vpTemplateTrackerSSDInverseCompositional::setUseTemplateSelect() or vpTemplateTrackerZNCCInverseCompositional::setUseTemplateSelect(), another function, only available in vpTemplateTrackerSSDInverseCompositional and vpTemplateTrackerZNCCInverseCompositional is:
-
-\code
-tracker.setThresholdRMS(1e-6);
-\endcode
-
-By increasing this root mean square threshold value, the algorithm will reduce its number of iterations which should also speed up the tracking phase. This function should be used wisely with the vpTemplateTracker::setIterationMax() function.
-
-\subsection points_tt How to get the points of the template
-
-The previous code provided in tutorial-template-tracker.cpp can be modified to get the coordinates of the corners of the triangles that define the zone to track. To this end, as shown in the next lines, before the while loop we first define a reference zone and the corresponding warped zone. Then in the loop, we update the warped zone using the parameters of the warping model that is estimated by the tracker. From the warped zone, we extract all the triangles, and then for each triangles [...]
-
-\code
- // Instantiate and get the reference zone
- vpTemplateTrackerZone zone_ref = tracker.getZoneRef();
- // Instantiate a warped zone
- vpTemplateTrackerZone zone_warped;
-
- while(!g.end()){
- g.acquire(I);
- vpDisplay::display(I);
- tracker.track(I);
-
- tracker.display(I, vpColor::red);
-
- // Get the estimated parameters
- vpColVector p = tracker.getp();
-
- // Update the warped zone given the tracker estimated parameters
- warp.warpZone(zone_ref, p, zone_warped);
-
- // Parse all the triangles that describe the zone
- for (int i=0; i < zone_warped.getNbTriangle(); i++) {
- vpTemplateTrackerTriangle triangle;
- // Get a triangle
- zone_warped.getTriangle(i, triangle);
- std::vector<vpImagePoint> corners;
- // Get the 3 triangle corners
- triangle.getCorners( corners );
-
- // From here, each corner triangle is available in
- // corners[0], corners[1] and corners[2]
-
- // Display a green cross over each corner
- for(unsigned int j=0; j<corners.size(); j++)
- vpDisplay::displayCross(I, corners[j], 15, vpColor::green, 2);
- }
- vpDisplay::displayRectangle(I, zone_warped.getBoundingBox(), vpColor::orange);
-\endcode
-
-With the last line, we also sho how to get and display an orange rectangle that corresponds to the bounding box of all the triangles that define the zone.
-
-The resulting drawings introduced previously are shown in the next image. Here we initialize the tracker with 2 triangles that are not connex.
-
-\image html img-template-tracker.jpg
-
-
-\section tracking_tt_example More examples
-
-The templateTracker.cpp source code provided in the example/tracking folder allows to test all the template tracker classes that derive from vpTemplateTracker as well as all the warping classes that derive from vpTemplateTrackerWarp.
-
-Once build, in a terminal just run:
-
-\code
-./templateTracker -h
-\endcode
-
-to see which are the command lines options.
-*/
diff --git a/doc/tutorial.doc b/doc/tutorial.doc
deleted file mode 100644
index fd67c2f..0000000
--- a/doc/tutorial.doc
+++ /dev/null
@@ -1,150 +0,0 @@
-/*! \page tutorial_mainpage Tutorials
-This page references all the tutorials.
-
-- \subpage tutorial_install_pkg
-
-- \subpage tutorial_install_src
-
-- \subpage tutorial_install_crosscompiling
-
-- \subpage tutorial_started
-
-- \subpage tutorial_image
-
-- \subpage tutorial_calib
-
-- \subpage tutorial_tracking
-
-- \subpage tutorial_detection
-
-- \subpage tutorial_computer_vision
-
-- \subpage tutorial_vs
-
-- \subpage tutorial_bridge
-
-- \subpage tutorial_tools
-
-*/
-
-/*! \page tutorial_install_pkg Installation from packages
-This page introduces the user to the way to install ViSP from existing prebuild packages.
-
-- \subpage tutorial-install-ubuntu-package <br>In this first tutorial you will learn how to install ViSP prebuilt library SDK from Ubuntu or Debian packages.
-- \subpage tutorial-install-archlinux-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for Arch Linux.
-- \subpage tutorial-install-osx-homebrew-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK on OSX with Homebrew.
-- \subpage tutorial-install-ros-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for ROS.
-- \subpage tutorial-install-ios-package <br>In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
-
-*/
-
-/*! \page tutorial_install_src Installation from source code
-This page introduces the user to the way to install ViSP from source code.
-
-- \subpage tutorial-install-ubuntu <br>In this tutorial you will learn how to install ViSP from source on Linux Ubuntu.
-- \subpage tutorial-install-fedora <br>In this other tutorial you will learn how to install ViSP from source on Linux Fedora.
-- \subpage tutorial-install-centos <br>In this other tutorial you will learn how to install ViSP from source on Linux CentOS.
-- \subpage tutorial-install-opensuse <br>In this other tutorial you will learn how to install ViSP from source on Linux openSUSE.
-- \subpage tutorial-install-raspberry <br>In this tutorial you will learn how to install ViSP from source on Raspberry Pi.
-- \subpage tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012.
-- \subpage tutorial-install-win81-msvc <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Visual C++ 2013.
-- \subpage tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015.
-- \subpage tutorial-install-win10-msvc14-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2015.
-- \subpage tutorial-install-win81-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 or Windows 10 with Mingw-w64.
-- \subpage tutorial-install-osx-homebrew <br>In this tutorial you will learn how to install ViSP from source on OSX with Homebrew.
-- \subpage tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
-
-*/
-
-/*! \page tutorial_install_crosscompiling Installation from cross-compiling
-This page introduces the user to the way to cross-compile ViSP from source code.
-
-- \subpage tutorial-install-crosscompiling-raspberry <br>In this tutorial you will learn how to cross-compile ViSP on Ubuntu host for Raspberry Pi target.
-- \subpage tutorial-install-crosscompiling-naoqi <br>In this tutorial you will learn how to cross-compile ViSP on Ubuntu host for NAOqi targets (NAO, Romeo, Pepper).
-
-*/
-/*! \page tutorial_started Getting started
-This page introduces the user to the way to start with ViSP.
-
-- \subpage tutorial-getting-started <br>This tutorial shows how to build a project that uses ViSP to read and display an image.
-- \subpage tutorial-getting-started-iOS <br>This tutorial shows how to build a project that uses ViSP on iOS devices.
-- \subpage tutorial-getting-started-naoqi <br>This tutorial shows how to build a project that uses ViSP on NAOqi OS running on NAO, Romeo or Pepper humanoid robots.
-
-*/
-
-/*! \page tutorial_image Image manipulation
-This page introduces the user to the way to get, to filter or to render images.
-
-- \subpage tutorial-image-display <br>In this tutorial you will learn how to display an image in a window and how to down scale the window size to be able to display images that are larger than the screen resolution.
-- \subpage tutorial-grabber <br>This tutorial shows how to acquire images from a camera.
-- \subpage tutorial-image-filtering <br>This tutorial shows how to filter an image with ViSP.
-- \subpage tutorial-simu-image <br>This tutorial shows how to project the image of a planar scene to a given camera position.
-- \subpage tutorial-image-ios <br>This tutorial shows how to do simple image processing on iOS devices.
-
-*/
-
-/*! \page tutorial_calib Camera calibration
-This page introduces the user to the way to calibrate a camera.
-
-- \subpage tutorial-calibration <br>This tutorial explains how to calibrate a camera.
-
-*/
-
-/*! \page tutorial_tracking Tracking
-This page introduces the user to the way to track objects in images.
-
-- \subpage tutorial-tracking-blob <br>This tutorial introduces blob tracking and detection.
-- \subpage tutorial-tracking-keypoint <br>This tutorial focuses on keypoint tracking using Kanade-Lucas-Tomasi feature tracker.
-- \subpage tutorial-tracking-me <br>This tutorial focuses on line and ellipse tracking using moving-edges.
-- \subpage tutorial-tracking-mb <br>This tutorial focuses on model-based trackers using either edges, keypoints or and hybrid scheme that uses edges and keypoints.
-- \subpage tutorial-tracking-mb-stereo <br>This tutorial focuses on model-based trackers using stereo cameras.
-- \subpage tutorial-tracking-tt <br>This tutorial focuses on template trackers based on image registration approaches.
-
-*/
-
-/*! \page tutorial_detection Detection
-This page introduces the user to the way to detect features or objects in images.
-
-- \subpage tutorial-matching <br>This tutorial shows how to detect and match keypoints.
-- \subpage tutorial-matching-deprecated <br>This tutorial shows how to detect and match SURF keypoints. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
-- \subpage tutorial-detection-barcode <br>This tutorial focuses on bar code (QR code, Data Matrix code) detection.
-- \subpage tutorial-detection-face <br>This tutorial focuses on face detection thanks to OpenCV Haar cascade classifier.
-- \subpage tutorial-detection-object <br>This tutorial shows how to learn keypoints detected on a known object and how to use the matched correspondences to detect and estimate the pose of the object.
-
-*/
-
-/*! \page tutorial_computer_vision Computer vision
-This page introduces the user to the way to estimate a pose or an homography.
-
-- \subpage tutorial-pose-estimation <br>This tutorial focuses on pose estimation from planar or non planar points.
-- \subpage tutorial-pose-estimation-qrcode <br>This tutorial focuses on pose estimation of a QRcode using it's four corners location.
-- \subpage tutorial-homography <br>Here we explain how to estimate an homography from couples of matched points.
-- \subpage tutorial-homography-deprecated <br>Here we explain how to estimate an homography from couples of matched points. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
-
-*/
-
-/*! \page tutorial_vs Visual servoing
-This page introduces the user to the way to achieve a visual servoing.
-
-- \subpage tutorial-ibvs <br>This tutorial explains how to simulate an IBVS.
-- \subpage tutorial-simu-robot-pioneer <br>This tutorial focuses on visual servoing simulation on a unicycle robot. The study case is a Pioneer P3-DX mobile robot equipped with a camera.
-- \subpage tutorial-boost-vs <br>This tutorial explains how to speed up the time to convergence of a visual servo.
-
-*/
-
-/*! \page tutorial_bridge Bridges over other frameworks
-This page introduces the user to the way to bridge other frameworks like OpenCV.
-
-- \subpage tutorial-bridge-opencv <br>This tutorial explicit how to convert OpenCV to/from ViSP structures such as camera parameters, images...
-
-*/
-
-/*! \page tutorial_tools Other tools
-This page introduces the user to other tools that may be useful.
-
-- \subpage tutorial-plotter <br>This tutorial explains how to plot curves in real-time during a visual servo.
-- \subpage tutorial-trace <br>This tutorial explains how to introduce trace in the code that could be enabled for debugging or disabled.
-- \subpage tutorial-multi-threading <br>This tutorial explains how to implement multi-threaded applications to capture images from a camera
-in one thread and display these images in an other thread.
-
-*/
diff --git a/doc/tutorial-bridge-opencv.doc b/doc/tutorial/bridge/tutorial-bridge-opencv.doc
similarity index 100%
rename from doc/tutorial-bridge-opencv.doc
rename to doc/tutorial/bridge/tutorial-bridge-opencv.doc
diff --git a/doc/tutorial-calibration.doc b/doc/tutorial/calibration/tutorial-calibration.doc
similarity index 100%
rename from doc/tutorial-calibration.doc
rename to doc/tutorial/calibration/tutorial-calibration.doc
diff --git a/doc/tutorial-install-crosscompiling-naoqi.doc b/doc/tutorial/cross-compilation/tutorial-install-crosscompiling-naoqi.doc
similarity index 100%
rename from doc/tutorial-install-crosscompiling-naoqi.doc
rename to doc/tutorial/cross-compilation/tutorial-install-crosscompiling-naoqi.doc
diff --git a/doc/tutorial-install-crosscompiling-raspberry.doc b/doc/tutorial/cross-compilation/tutorial-install-crosscompiling-raspberry.doc
similarity index 100%
rename from doc/tutorial-install-crosscompiling-raspberry.doc
rename to doc/tutorial/cross-compilation/tutorial-install-crosscompiling-raspberry.doc
diff --git a/doc/tutorial/detection/tutorial-detection-apriltag.doc b/doc/tutorial/detection/tutorial-detection-apriltag.doc
new file mode 100644
index 0000000..d29e916
--- /dev/null
+++ b/doc/tutorial/detection/tutorial-detection-apriltag.doc
@@ -0,0 +1,138 @@
+/**
+
+\page tutorial-detection-apriltag Tutorial: AprilTag marker detection
+\tableofcontents
+
+\section intro_apriltag Introduction
+
+This tutorial shows how to detect one or more AprilTag marker with ViSP. To this end, we provide vpDetectorAprilTag class that is a wrapper over <a href="https://april.eecs.umich.edu/software/apriltag.html">Apriltag</a> 3rd party library. Notice that there is no need to install this 3rd party, since AprilTag source code is embedded in ViSP.
+
+The vpDetectorAprilTag class inherits from vpDetectorBase class, a generic class dedicated to detection. For each detected tag, it allows to retrieve some characteristics such as the tag id, and in the image, the polygon that contains the tag and corresponds to its 4 corner coordinates, the bounding box or the center of gravity of the tag.
+
+Moreover vpDetectorAprilTag class allows to estimate the 3D pose of the tag. To this end, the camera parameters as well as the size of the tag are required.
+
+In the next sections you will find examples that show how to detect tags in a single image or in images acquired from a camera connected to your computer.
+
+Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/detection/tag
+\endcode
+
+\section apriltag_detection_print Print an AprilTag marker
+
+You can directly download on the <a href="https://april.eecs.umich.edu/software/apriltag.html">Apriltag</a> website some pre-generated tag families:
+- <a href="https://april.eecs.umich.edu/media/apriltag/tag36h11.tgz">36h11 (recommended)</a>
+- <a href="https://april.eecs.umich.edu/media/apriltag/tag36h10.tgz">36h10</a>
+- <a href="https://april.eecs.umich.edu/media/apriltag/tag25h9.tgz">25h9</a>
+- <a href="https://april.eecs.umich.edu/media/apriltag/tag16h5.tgz">16h5</a>
+
+In each archive you will find a PNG image of each tag, a mosaic in PNG containing every tag and a ready-to-print postscript file with one tag per page.
+If you want to print an individual tag, you can manually scale the corresponding PNG image using two methods:
+- on Unix with ImageMagick, e.g.:
+\code
+$ convert tag36_11_00000.png -scale 5000% tag36_11_00000_big.png
+\endcode
+- or open the image with <a href="https://www.gimp.org/">Gimp</a>:
+ - then from the pulldown menu, select **Image** > **Scale Image**
+ - set the unit and the size
+ - set the **Interpolation** mode to **None**
+ - click on the **Scale** button
+ - From the pulldown menu, select **File** > **Export As**
+ - Save the image as a new PNG image, e.g., `/tmp/tag36_11_00000-rescaled.png`
+ - Send the PNG file to your printer
+
+\section apriltag_detection_basic AprilTag detection and pose estimation (single image)
+
+The following example also available in tutorial-apriltag-detector.cpp allows to detect a tag on a single image.
+
+\include tutorial-apriltag-detector.cpp
+
+The default behavior is to detect <a href="https://april.eecs.umich.edu/software/apriltag.html">36h11</a> marker in \c AprilTag.pgm image, but \c --tag_family \<family\> option allows to consider other tags. To see which are the options, just run:
+\code
+$ ./tutorial-apriltag-detector --help
+\endcode
+
+To detect multiple 36h11 tags in the \c AprilTag.pgm image that is provided just run:
+\code
+$ ./tutorial-apriltag-detector
+\endcode
+You will get the following result:
+
+\image html img-apriltag-image.png
+
+After a user click in the image, you will get the following image where the frames correspond to the 3D pose of each tag.
+\image html img-apriltag-pose.png
+
+Now we explain the main lines of the source.
+
+First we have to include the header corresponding to vpDetectorAprilTag class that allow to detect one or multiple tags.
+\snippet tutorial-apriltag-detector.cpp Include
+
+Then in the \c main() function before going further we need to check if ViSP was built with AprilTag 3rd party. We also check if ViSP is able to display images using either X11, or the Graphical Device Interface (GDI) under Windows, or OpenCV.
+
+\snippet tutorial-apriltag-detector.cpp Macro defined
+
+After reading the input image \c AprilTag.pgm and the creation of a display device in order to visualize the image, since vpDetectorAprilTag class inherits from vpDetectorBase we are able to construct the requested detector (you can also use directly vpDetectorAprilTag).
+
+\snippet tutorial-apriltag-detector.cpp Create base detector
+
+Then we are applying some settings. There is especially vpDetectorAprilTag::setAprilTagQuadDecimate() function that could be used to decimate the input image in order to speed-up the detection.
+
+\snippet tutorial-apriltag-detector.cpp AprilTag detector settings
+
+We are now ready to detect any 36h11 tags in the image. There is the vpDetectorAprilTag::detect(const vpImage<unsigned char> &) function that allows to simply detect any tags in the image, but since here we want also to estimate the 3D pose of the tags, we call rather vpDetectorAprilTag::detect(const vpImage<unsigned char> &, const double, const vpCameraParameters &, std::vector<vpHomogeneousMatrix> &) that returns the pose of each tag as a vector of vpHomogeneousMatrix in \c cMo_vec var.
+
+\snippet tutorial-apriltag-detector.cpp Detect and compute pose
+
+If one or more tags are detected, we can retrieve the number of detected tags in order to create a for loop over the tags.
+
+\snippet tutorial-apriltag-detector.cpp Parse detected codes
+
+For each tag, we can then get the location of the 4 points that define the polygon that contains the tag and the corresponding bounding box.
+
+\snippet tutorial-apriltag-detector.cpp Get location
+
+And finally, we are also able to get the tag id calling vpDetectorAprilTag::getMessage().
+
+\snippet tutorial-apriltag-detector.cpp Get message
+
+Next in the code we display the 3D pose of each tag as a RGB frame.
+
+\snippet tutorial-apriltag-detector.cpp Display camera pose for each tag
+
+\note
+ - To get absolute pose (not relative to a scale factor), you have to provide the real size of the marker (length of a marker side).
+ - To calibrate your camera, you can follow this tutorial: \ref tutorial-calibration
+
+\section apriltag_detection_live AprilTag detection and pose estimation (live camera)
+
+This other example also available in tutorial-apriltag-detector-live.cpp shows how to couple the AprilTag detector to an image grabber in order to detect tags on each new image acquired by a camera connected to your computer.
+
+\include tutorial-apriltag-detector-live.cpp
+
+The usage of this example is similar to the previous one:
+- with option \c --tag_family you select the kind of tag that you want to detect.
+- if more than one camera is connected to you computer, with option \c --input you can select which camera to use. The first camera that is found has number 0.
+
+To detect 36h11 tags on images acquired by a second camera connected to your computer use:
+\code
+$ ./tutorial-apriltag-detector-live --tag_family 0 --input 1
+\endcode
+
+The source code of this example is very similar to the previous one except that here we use camera framegrabber devices (see \ref tutorial-grabber). Two different grabber may be used:
+- If ViSP was built with Video For Linux (V4L2) support available for example on Fedora or Ubuntu distribution, VISP_HAVE_V4L2 macro is defined. In that case, images coming from an USB camera are acquired using vpV4l2Grabber class.
+- If ViSP wasn't built with V4L2 support but with OpenCV, we use cv::VideoCapture class to grab the images. Notice that when images are acquired with OpenCV there is an additional conversion from cv::Mat to vpImage.
+
+\snippet tutorial-apriltag-detector-live.cpp Construct grabber
+
+Then in the while loop, at each iteration we acquire a new image
+\snippet tutorial-apriltag-detector-live.cpp Acquisition
+
+This new image is then given as input to the AprilTag detector.
+
+\section apriltag_detection_next Next tutorial
+
+You are now ready to see the \ref tutorial-detection-barcode, that illustrates how to detect QR codes in an image.
+
+*/
diff --git a/doc/tutorial/detection/tutorial-detection-barcode.doc b/doc/tutorial/detection/tutorial-detection-barcode.doc
new file mode 100644
index 0000000..a602adc
--- /dev/null
+++ b/doc/tutorial/detection/tutorial-detection-barcode.doc
@@ -0,0 +1,113 @@
+/**
+
+\page tutorial-detection-barcode Tutorial: Bar code detection
+\tableofcontents
+
+\section intro_barcode Introduction
+
+This tutorial shows how to detect one or more barcodes with ViSP. To this end we provide two classes that are wrapper over 3rd party libraries:
+- vpDetectorQRCode that is a wrappers over zbar library <http://zbar.sourceforge.net>. It allows to detect QR codes. See <a href="https://visp.inria.fr/3rd_zbar">zbar quick installation guide</a>.
+- vpDetectorDataMatrix this is a wrapper over dmtx library <http://www.libdmtx.org>. It allows to detect Data Matrix codes. See <a href="https://visp.inria.fr/3rd_dmtx">dmtx quick installation guide</a>.
+
+These classes inherit from vpDetectorBase class, a generic class dedicated to detection. For each detected bar code, it allows to retrieve some characteristics such as the bar code message if it exists, and in the image the polygon that contains the bar code, the bounding box or the center of gravity of the bar code.
+
+In the next sections you will find examples that show how to detect codes in a single image and or in images acquired from a camera connected to your computer.
+
+Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/detection/barcode
+\endcode
+
+
+\section barcode_detection_basic Bar code detection (single image)
+
+The following example also available in tutorial-barcode-detector.cpp allows to detect either a QR code or Data Matrix on a single image. The user can select with --code-type option which code to detect.
+
+\include tutorial-barcode-detector.cpp
+
+To detect a QR code just run:
+\code
+$ ./tutorial-barcode-detector --code-type 0
+\endcode
+You will get the following result:
+
+\image html img-detection-qrcode.png
+
+To detect a Data Matrix code just run:
+\code
+$ ./tutorial-barcode-detector --code-type 1
+\endcode
+You will get the following result:
+
+\image html img-detection-datamatrix.png
+
+Now we explain the main lines of the source.
+
+First we have to include the header of the two classes that allow to detect either the QR code or the Data Matrix code.
+\snippet tutorial-barcode-detector.cpp Include
+
+Then in the main() function before going further we need to check if at least one of the third party (zbar or dmtx libraries) were used to build ViSP. We also check if ViSP is able to display images using either X11, or the Graphical Device Interface (GDI) under Windows, or OpenCV.
+
+\snippet tutorial-barcode-detector.cpp Macro defined
+
+After reading the input image \e bar-code.pgm and creation of a display device in order to visualize the image, we create a NULL pointer to a detector base class.
+
+\snippet tutorial-barcode-detector.cpp Create base detector
+
+Since the classes that allow to detect QR codes and Data Matrix codes inherit from vpDetectorBase, using the variable \e opt_barcode we are able to construct the requested detector.
+
+\snippet tutorial-barcode-detector.cpp Create detector
+
+We are now ready to detect the bar code in the image.
+
+\snippet tutorial-barcode-detector.cpp Detection
+
+If one or more bar codes are detected, the variable \e status is set to \e true.
+In that case, we can retrieve the number of detected bar codes in order to create a for loop over the codes.
+
+\snippet tutorial-barcode-detector.cpp Parse detected codes
+
+For each code, we can then get the location of the 4 points that define the polygon that contains the code, but also the bounding box.
+
+\snippet tutorial-barcode-detector.cpp Get location
+
+And finally, if it exists, we are also able to get the message that is coded in the bar code.
+
+\snippet tutorial-barcode-detector.cpp Get message
+
+
+\section barcode_detection_live Bar code detection (live camera)
+
+This other example also available in tutorial-barcode-detector-live.cpp shows how to couple the bar code detector to an image grabber in order to detect bar codes on each new image acquired by a camera connected to your computer.
+
+\include tutorial-barcode-detector-live.cpp
+
+The usage of this example is similar to the previous one:
+- with option --code-type you select if you want to detect a QR code (use --code-type 0) or a Data Matrix (use --code-type 1).
+- if more than one camera is connected to you computer, with option --device you can select which camera to use. The first camera that is found has number 0.
+
+To detect QR codes on images acquired by a second camera connected to your computer use:
+\code
+$ ./tutorial-barcode-detector-live --code-type 0 --device 1
+\endcode
+
+The source code of this example is very similar to the previous one except that here we use camera framegrabber devices (see \ref tutorial-grabber). Two different grabber may be used:
+- If ViSP was build with Video For Linux (V4L2) support available for example on Fedora or Ubuntu distribution, VISP_HAVE_V4L2 macro is defined. In that case, images coming from an USB camera are acquired using vpV4l2Grabber class.
+- If ViSP wasn't build with V4L2 support, but with OpenCV we use cv::VideoCapture class to grab the images. Notice that when images are acquired with OpenCV there is an additional conversion from cv::Mat to vpImage.
+
+\snippet tutorial-barcode-detector-live.cpp Construct grabber
+
+Then in the while loop, at each iteration we acquire a new image
+\snippet tutorial-barcode-detector-live.cpp Acquisition
+
+This new image is then given as input to the bar code detector.
+
+\section barcode_detection_next Next tutorial
+
+Now you can follow \ref tutorial-pose-estimation that explains how to compute a 3D pose from points. This tutorial could be useful if you want to compute the pose of a QR code from it’s 4 corner coordinates in the image that could be retrieved using getPolygon(), it’s 3D size and camera parameters.
+
+You are now also ready to see \ref tutorial-detection-apriltag, that illustrates how to detect AprilTag patterns in an image and compute the 3D pose of each pattern.
+
+
+*/
diff --git a/doc/tutorial-detection-face.doc b/doc/tutorial/detection/tutorial-detection-face.doc
similarity index 100%
rename from doc/tutorial-detection-face.doc
rename to doc/tutorial/detection/tutorial-detection-face.doc
diff --git a/doc/tutorial-detection-object.doc b/doc/tutorial/detection/tutorial-detection-object.doc
similarity index 100%
rename from doc/tutorial-detection-object.doc
rename to doc/tutorial/detection/tutorial-detection-object.doc
diff --git a/doc/tutorial/detection/tutorial-matching-deprecated.doc b/doc/tutorial/detection/tutorial-matching-deprecated.doc
new file mode 100644
index 0000000..5bcafc8
--- /dev/null
+++ b/doc/tutorial/detection/tutorial-matching-deprecated.doc
@@ -0,0 +1,63 @@
+/**
+
+\page tutorial-matching-deprecated Tutorial: Keypoint matching (deprecated)
+\tableofcontents
+
+\section intro_matching_deprecated Introduction
+
+\note This tutorial is deprecated if your OpenCV version is equal to 3.0.0 or more recent. If so you should rather follow \ref tutorial-matching.
+
+This tutorial focuses on SURF key points manipulation. You will learn how to detect SURF key points on a reference image considered here as the first image of an mpeg video. Then in the next images of the video, key points that match those detected in the reference image using SURF descriptor are displayed.
+
+\note We assume that you are familiar with video framegrabbing described in \ref tutorial-grabber and with the way to display an image in a window described in \ref tutorial-getting-started.
+
+\section surf_deprecated SURF key points detection and matching
+
+Let us consider the following source code also available in tutorial-matching-surf-deprecated.cpp.
+
+\include tutorial-matching-surf-deprecated.cpp
+
+Here after is the resulting video. The left image represents the reference image. The right images correspond to the successive images of the input video. All the green lines extremities represent the points that are matched.
+
+\htmlonly
+<iframe width="560" height="315" src="http://www.youtube.com/embed/sMbed_oYJgQ" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+
+Now, let us explain the lines dedicated to the SURF keypoint usage.
+
+First we have to include the header of the vpKeyPointSurf class that is a wrapper over OpenCV classes.
+\snippet tutorial-matching-surf-deprecated.cpp Include
+
+Note that this class is only available if ViSP was build with OpenCV non free module. This is ensured by the check of VISP_HAVE_OPENCV_NONFREE macro.
+\snippet tutorial-matching-surf-deprecated.cpp Define
+
+Then we open the mpeg video stream and grab the first image of the video that is stored in \c I container. A Surf keypoint class is instantiated and keypoints are detected on the first image which is considered as the reference image:
+\snippet tutorial-matching-surf-deprecated.cpp Construction
+
+The next lines are used to create image \c Idisp to render the matching results; left image for the reference image, right image for the current image that is processed:
+\snippet tutorial-matching-surf-deprecated.cpp Create image
+
+Then a display using OpenCV is created and image \c Idisp is rendered:
+\snippet tutorial-matching-surf-deprecated.cpp Init display
+
+We enter then in the \c while() loop where a new image is acquired from the video stream and inserted in the right part of image \c Idisp dedicated to rendering of the matching results.
+\snippet tutorial-matching-surf-deprecated.cpp Acquisition
+
+We start the rendering by displaying the rendered image and by drawing a white vertical line to separate the reference image from the current one:
+\snippet tutorial-matching-surf-deprecated.cpp Display
+
+Keypoint matches between the reference image and the current image \c I are detected using:
+\snippet tutorial-matching-surf-deprecated.cpp Matching
+
+Then we parse all the matches to retrieve the coordinates of the points in the reference image (in \c iPref variable) and in the current image (in \c iPcur variable):
+\snippet tutorial-matching-surf-deprecated.cpp Get matches
+
+Next we draw green lines between the matched points:
+\snippet tutorial-matching-surf-deprecated.cpp Display matches
+
+At the end of the iteration, we flush all the previous display to the render window:
+\snippet tutorial-matching-surf-deprecated.cpp Display flush
+
+You can now follow \ref tutorial-homography-deprecated to see how to exploit couple of matched points in order to estimate an homography that allows to track the position of an object.
+
+*/
diff --git a/doc/tutorial-matching.doc b/doc/tutorial/detection/tutorial-matching.doc
similarity index 100%
rename from doc/tutorial-matching.doc
rename to doc/tutorial/detection/tutorial-matching.doc
diff --git a/doc/tutorial/image/tutorial-grabber.doc b/doc/tutorial/image/tutorial-grabber.doc
new file mode 100644
index 0000000..56d324c
--- /dev/null
+++ b/doc/tutorial/image/tutorial-grabber.doc
@@ -0,0 +1,153 @@
+
+/**
+ \page tutorial-grabber Tutorial: Image frame grabbing
+ \tableofcontents
+
+
+\section grabber-into Introduction
+
+In this tutorial you will learn how to grab images with ViSP, either from cameras or from a video stream.
+
+All the material (source code and videos) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/grabber
+\endcode
+
+\section grabber-camera-pgr Images from PointGrey cameras
+
+After ViSP 3.0.0, we introduce vpFlyCaptureGrabber class, a wrapper over PointGrey FlyCapture SDK that allows to grab images from any PointGrey camera. This grabber was tested under Ubuntu and Windows with the following cameras:
+ - Flea3 USB 3.0 cameras (FL3-U3-32S2M-CS, FL3-U3-13E4C-C)
+ - Flea2 firewire camera (FL2-03S2C)
+ - Dragonfly2 firewire camera (DR2-COL)
+
+It should also work with GigE PGR cameras.
+
+The following example also available in tutorial-grabber-flycapture.cpp shows how to use vpFlyCaptureGrabber to capture grey level images from a PointGrey camera under Ubuntu or Windows. The following example suppose that a window renderer (libX11 on Ubuntu or GDI on Windows) and FlyCapture SDK 3rd party are available throw VISP.
+
+\include tutorial-grabber-flycapture.cpp
+
+Here after we explain the source code.
+
+First an instance of the frame grabber is created.
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber construction
+
+Once the grabber is created, we turn auto shutter and auto gain on and set the camera image size, color coding, and framerate.
+These settings are enclosed in a try/catch to be able to continue if one of these settings are not supported by the camera.
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber settings
+
+Then the grabber is initialized using:
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber open
+
+From now the grey level image \c I is also initialized with the size corresponding to the grabber settings.
+
+Then we enter in a while loop where image acquisition is simply done by:
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber acquire
+
+This image is then displayed using libX11 or GDI renderer:
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber display
+
+We are waiting for a non blocking mouse event to break the while loop before ending the program.
+\snippet tutorial-grabber-flycapture.cpp vpFlyCaptureGrabber click to exit
+
+\section grabber-camera-firewire-unix Images from firewire cameras
+
+The next example also available in tutorial-grabber-1394.cpp shows how to use a framegrabber to acquire color images from a firewire camera under Unix. The following example suppose that libX11 and libdc1394-2 3rd party are available.
+
+\include tutorial-grabber-1394.cpp
+
+Here after we explain the new lines that are introduced.
+
+First an instance of the frame grabber is created. During the creating a bus reset is send. If you don't want to reset the firewire bus, just turn reset to false.
+\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber construction
+
+Once the grabber is created, we set the camera image size, color coding, and framerate.
+\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber settings
+
+Note that here you can specify some other settings such as the firewire transmission speed. For a more complete list of settings see vp1394TwoGrabber class.
+\code
+ g.setIsoTransmissionSpeed(vp1394TwoGrabber::vpISO_SPEED_800);
+\endcode
+
+Then the grabber is initialized using:
+\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber open
+
+From now the color image \c I is also initialized with the size corresponding to the grabber settings.
+
+Then we enter in a while loop where image acquisition is simply done by:
+\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber acquire
+
+We are waiting for a non blocking mouse event to break the while loop before ending the program.
+\snippet tutorial-grabber-1394.cpp vp1394TwoGrabber click to exit
+
+In the previous example we use vp1394TwoGrabber class that works for firewire cameras under Unix. If you are under Windows, you may use vp1394CMUGrabber class. A similar example is provided in tutorial-grabber-CMU1394.cpp.
+
+\section grabber-camera-other Images from other cameras
+
+If you want to grab images from an usb camera under Unix, you may use vpV4l2Grabber class. To this end libv4l should be installed. An example is provided in tutorial-grabber-v4l2.cpp.
+
+It is also possible to grab images using OpenCV. You may find an example in tutorial-grabber-opencv.cpp.
+
+\section grabber-video-stream Images from a video stream
+
+With ViSP it also possible to get images from an input video stream. Supported formats are *.avi, *.mp4, *.mov, *.ogv, *.flv and many others... To this end we exploit OpenCV 3rd party.
+
+The example below available in tutorial-video-reader.cpp shows how o consider an mpeg video stream.
+
+\warning We recall that this example works only if ViSP was build with OpenCV support.
+
+\include tutorial-video-reader.cpp
+
+We explain now the new lines that were introduced.
+
+\snippet tutorial-video-reader.cpp Include
+Include the header of the vpTime class that allows to measure time, and of the vpVideoReader class that allows to read a video stream.
+
+\snippet tutorial-video-reader.cpp vpVideoReader construction
+Create an instance of a video reader.
+
+\snippet tutorial-video-reader.cpp vpVideoReader setting
+Set the name of the video stream. Here \c videoname corresponds to a video file name location. For example we provide the file \c video.mpg located in the same folder than the executable.
+
+The vpVideoReader class can also handle a sequence of images. For example, to read the following images:
+
+\code
+% ls *.png
+image0000.png
+image0001.png
+image0002.png
+image0003.png
+image0004.png
+...
+\endcode
+
+you may use the following:
+
+\code
+ g.setFileName("./image%04d.png");
+\endcode
+where you specify that each image number is coded with 4 digits. Here, we will use \c libpng or \c OpenCV to read PNG images. Supported image formats are PPM, PGM, PNG and JPEG.
+
+Then as for any other grabber, you have to initialize the frame grabber using:
+
+\snippet tutorial-video-reader.cpp vpVideoReader open
+
+Then we enter in the while loop until the last image was not reached:
+\snippet tutorial-video-reader.cpp vpVideoReader while loop
+
+To get the next image in the stream, we just use:
+\snippet tutorial-video-reader.cpp vpVideoReader acquire
+
+To synchronize the video decoding with the video framerate, we measure the beginning time of each loop iteration:
+
+\snippet tutorial-video-reader.cpp vpVideoReader loop start time
+
+The synchronization is done by waiting from the beginning of the iteration the corresponding time expressed in milliseconds by using:
+\snippet tutorial-video-reader.cpp vpVideoReader loop rate
+
+\section grabber-next Next tutorial
+
+You are now ready to see how to continue with:
+- \ref tutorial-multi-threading if you want to see how to extend these examples with one thread for capture and an other one for display
+- or with a simple image processing that shows how to track blobs explained in \ref tutorial-tracking-blob.
+*/
diff --git a/doc/tutorial-image-display.doc b/doc/tutorial/image/tutorial-image-display.doc
similarity index 100%
rename from doc/tutorial-image-display.doc
rename to doc/tutorial/image/tutorial-image-display.doc
diff --git a/doc/tutorial-image-filtering.doc b/doc/tutorial/image/tutorial-image-filtering.doc
similarity index 100%
rename from doc/tutorial-image-filtering.doc
rename to doc/tutorial/image/tutorial-image-filtering.doc
diff --git a/doc/tutorial-simu-image.doc b/doc/tutorial/image/tutorial-simu-image.doc
similarity index 100%
rename from doc/tutorial-simu-image.doc
rename to doc/tutorial/image/tutorial-simu-image.doc
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-autothreshold.doc b/doc/tutorial/imgproc/tutorial-imgproc-autothreshold.doc
new file mode 100644
index 0000000..bce84b5
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-autothreshold.doc
@@ -0,0 +1,61 @@
+/**
+
+\page tutorial-imgproc-autothreshold Tutorial: Automatic thresholding
+\tableofcontents
+
+\section imgproc_autothreshold_intro Introduction
+
+This tutorial will show you how to automatically threshold, binarize an image using different methods:
+- Huang \cite Huang_imagethresholding
+- Intermodes \cite NYAS:NYAS1035
+- Isodata \cite article4310039
+- Mean \cite Glasbey:1993:AHT:167725.167747
+- Otsu \cite article4310076
+- Triangle \cite doi:10.1177/25.7.70454
+
+These functions have been ported from the <a href="https://imagej.net/Auto_Threshold">Auto Threshold ImageJ plugin</a> and you can refer to the corresponding documentation for more information.
+
+\section imgproc_autothreshold_example Example code
+
+The following example also available in tutorial-autothreshold.cpp will demonstrate on a sample image the result of each of these methods:
+
+\include tutorial-autothreshold.cpp
+
+These functions are provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-autothreshold.cpp Include
+
+The code to use is pretty straightword:
+- Huang:
+\snippet tutorial-autothreshold.cpp Huang
+- Intermodes:
+\snippet tutorial-autothreshold.cpp Intermodes
+- IsoData:
+\snippet tutorial-autothreshold.cpp IsoData
+- Mean:
+\snippet tutorial-autothreshold.cpp Mean
+- Otsu:
+\snippet tutorial-autothreshold.cpp Otsu
+- Triangle:
+\snippet tutorial-autothreshold.cpp Triangle
+
+
+The following image presents the results for each method:
+
+\image html img-tutorial-autothreshold.png "Comparison of different binarizations using the threshold value returned by each method"
+
+The function vp::autoThreshold(vpImage<unsigned char> &, const vp::vpAutoThresholdMethod &, const unsigned char, const unsigned char) has two parameters to specify the pixel values to use for the background and the foreground. By default, it is (see vpImageTools::binarise(vpImage<Type> &, Type, Type, Type, Type, Type, const bool)):
+
+\f[
+ I_{bin}\left ( i,j \right ) =
+ \left \{ \begin{matrix}
+ 0 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\
+ 255 \text{ otherwise}
+ \end{matrix} \right.
+\f]
+
+\section imgproc_autothreshold_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-contour, to learn how to extract the contours from a binary image.
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-brightness.doc b/doc/tutorial/imgproc/tutorial-imgproc-brightness.doc
new file mode 100644
index 0000000..1797cf6
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-brightness.doc
@@ -0,0 +1,84 @@
+/**
+
+\page tutorial-imgproc-brightness Tutorial: Brightness and contrast adjustment
+\tableofcontents
+
+\section imgproc_brightness_intro Introduction
+
+While the ViSP library is not intended to be an image processing library or replace a raster graphics editor, some easy image processing techniques can be used to improve or adjust the brightness and the contrast of an image.
+
+The different techniques used in this tutorial are:
+- brightness and contrast adjustment using a linear function
+- gamma correction
+- histogram equalization
+- Retinex algorithm
+
+The following example also available in tutorial-brightness-adjustment.cpp will demonstrate on a real underexposed photo the result of each of these methods:
+
+\include tutorial-brightness-adjustment.cpp
+
+These functions are provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-brightness-adjustment.cpp Include
+
+\section imgproc_brightness_adjust Brightness and contrast adjustment
+
+The brightness and the contrast of an image can be adjusted using a linear function:
+\f[I_{res}\left ( i,j \right ) = \alpha \cdot I_{src}\left ( i,j \right ) + \beta\f]
+
+The \f$\alpha\f$ value will behave as a gain factor and the \f$\beta\f$ value as an offset.
+
+The code to use is straightforward:
+
+\snippet tutorial-brightness-adjustment.cpp Brightness contrast adjustment
+
+The result image is the following:
+
+\image html img-tutorial-brighness-adjust-alpha10-beta50.png "Left: underexposed image - Right: image adjusted with alpha=10, beta=50"
+
+\section imgproc_brightness_gamma_correction Gamma correction
+
+<a href="https://en.wikipedia.org/wiki/Gamma_correction">Gamma correction</a> is a simple technique allowing to correct an image using a non-linear operation. The formula used is:
+\f[I_{res}\left ( i,j \right ) = \left ( \frac{I_{src}\left ( i,j \right )}{255} \right )^{\frac{1}{\gamma}} \cdot 255\f]
+
+The image below shows in x the input pixel values and in y the output pixel values as they would be transformed by a gamma correction function according to different gamma values.
+
+\image html img-tutorial-brighness-gamma-correction-plot.png "Visualization of the gamma correction function"
+
+The result image is the following:
+
+\image html img-tutorial-brighness-gamma-correction-3.5.png "Left: underexposed image - Right: image corrected with gamma=3.5"
+
+\section imgproc_brightness_histogram_equalization Histogram equalization
+
+<a href="https://en.wikipedia.org/wiki/Histogram_equalization">Histogram equalization</a> is an image processing method that will adjust the contrast of an image by stretching or shrinking the intensity distribution in order to have a linear cumulative histogram distribution.
+
+In the next figure, you can observe the histogram for the original underexposed photo where most of the pixel intensities are located in the [0, 30] range. The cumulative histogram distribution has a strong slope for very low pixel intensities.
+
+\image html img-tutorial-brighness-hist-eq-cumulative.png "Histogram and normalized cumulative histogram of the underexposed photo"
+
+The histogram for the equalized photo is displayed in the next figure. This time, the bins are spread more uniformally along the intensity range and the cumulative histogram distribution presents a more linear shape.
+
+\image html img-tutorial-brighness-hist-eq-cumulative2.png "Histogram and normalized cumulative histogram of the equalized photo"
+
+The result of the histogram equalized image is displayed below:
+
+\image html img-tutorial-brighness-hist-eq.png "Left: underexposed image - Right: histogram equalized image"
+
+\section imgproc_brightness_retinex Retinex
+
+The Retinex algorithm implemented is ported from the <a href="http://imagej.net/Retinex">Retinex ImageJ</a> plugin:
+
+<blockquote>
+Retinex filtering is based on Land's theory of image perception, proposed to explain the perceived colour constancy of objects under varying illumination conditions. Several approaches exist to implement the retinex principles, among these the multiscale retinex with colour restoration algorithm (MSRCR) combines colour constancy with local contrast enhancement so images are rendered similarly to how human vision is believed to operate.
+</blockquote>
+
+The original photo after the Retinex processing:
+
+\image html img-tutorial-brighness-retinex-dynamic-3.png "Left: underexposed image - Right: result of the Retinex algorithm with default parameters and dynamic=3"
+
+\section imgproc_brightness_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-contrast-sharpening, for additional contrast and sharpness improvement techniques.
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-connected-components.doc b/doc/tutorial/imgproc/tutorial-imgproc-connected-components.doc
new file mode 100644
index 0000000..792a09d
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-connected-components.doc
@@ -0,0 +1,46 @@
+/**
+
+\page tutorial-imgproc-connected-components Tutorial: Connected-components labeling
+\tableofcontents
+
+\section imgproc_connected_components_intro Introduction
+
+This tutorial will show you how to perform a <a href="https://en.wikipedia.org/wiki/Connected-component_labeling">connected-components labeling</a>.
+
+\section imgproc_connected_components_example Example code
+
+The corresponding code is available in tutorial-connected-components.cpp:
+
+\include tutorial-connected-components.cpp
+
+The function is provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-connected-components.cpp Include
+
+The first step is to read an image:
+
+\snippet tutorial-connected-components.cpp Read
+
+\image html img-tutorial-connected-components-img.png "Input image"
+
+The connected-components labeling can be done with:
+
+\snippet tutorial-connected-components.cpp Connected components
+
+Each pixel other than the background (0 pixel value in the original image) is assigned a label stored in \a vpImage<int> variable. The number of connected-components is returned in \a nbComponents variable. The connexity can be 4-connexity or 8-connexity.
+
+To visualize the labeling, we can use these lines of code:
+
+\snippet tutorial-connected-components.cpp Draw connected components
+
+Each label is assigned a specific color. The result image is:
+
+\image html img-tutorial-connected-components-labeling.png "Connected-components labeling"
+
+\note As you can see, the input image does not need to be a binary image but must be a grayscale image.
+
+\section imgproc_connected_components_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-flood-fill, to learn how to do a flood fill.
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-contour.doc b/doc/tutorial/imgproc/tutorial-imgproc-contour.doc
new file mode 100644
index 0000000..aeb8e8e
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-contour.doc
@@ -0,0 +1,269 @@
+/**
+
+\page tutorial-imgproc-contour Tutorial: Contours extraction from a binary image
+\tableofcontents
+
+\section imgproc_contour_intro Introduction
+
+This tutorial will show you how to extract the contours from a binary image. The contour extraction algorithm is based on \cite articleSuzuki article and most of the implementation has been ported from \cite Hare:2011:OIJ:2072298.2072421 library.
+
+The function to call is vp::findContours(const vpImage<unsigned char> &, vpContour &, std::vector<std::vector<vpImagePoint> > &, const vpContourRetrievalType&):
+- the first argument is the image where '0' pixel value means the background and '1' pixel value means the foreground. **Other values are not allowed.**
+- the second argument is a vp::vpContour structure that contains the list of contours in a tree
+- the third argument is the list of contours
+- the last argument is an option to choose the type of contour extraction, see vp::vpContourRetrievalType
+
+The vp::vpContour structure is composed of:
+- std::vector< \ref vp::vpContour * > m_children, the list of children contours for the current contour
+- vp::vpContourType m_contourType, the type of contour (vp::CONTOUR_OUTER or vp::CONTOUR_HOLE)
+- vp::vpContour * m_parent, the parent contour for the current contour
+- std::vector< \ref vpImagePoint > m_points, the list of contour points
+- the first or top level contour is called the root contour (with vp::CONTOUR_HOLE type by default) and contains in \a m_children the list of contours
+
+The different contour extraction methods are:
+- vp::CONTOUR_RETR_TREE, all the contours are extracted and stored in a hierarchical tree.
+- vp::CONTOUR_RETR_LIST, all the contours are extracted and stored in a list. The top level contour contains in \a m_children the list of all the extracted contours.
+- vp::CONTOUR_RETR_EXTERNAL, only the external contours are extracted and stored in a list. The top level contour contains in \a m_children the list of the external extracted contours.
+
+The next section will provide a concrete example for better understanding.
+
+\section imgproc_contour_example Example code
+
+The following example also available in tutorial-contour.cpp will demonstrate on a sample image the result of each of these methods:
+
+\include tutorial-contour.cpp
+
+These functions are provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-contour.cpp Include
+
+The first steps are:
+- read an image in grayscale
+\snippet tutorial-contour.cpp Read
+- threshold / binarize the image, here with the Otsu method.
+\snippet tutorial-contour.cpp Otsu
+If the object of interest is in white in the image, the formula for the binarization is:
+\f[
+ I_{bin}\left ( i,j \right ) =
+ \left \{ \begin{matrix}
+ 0 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\
+ 1 \text{ otherwise}
+ \end{matrix} \right.
+\f]
+If the object of interest is in black in the image, the formula for the binarization is:
+\f[
+ I_{bin}\left ( i,j \right ) =
+ \left \{ \begin{matrix}
+ 1 \text{ if } I_{src}\left ( i,j \right ) < \text{threshold} \\
+ 0 \text{ otherwise}
+ \end{matrix} \right.
+\f]
+- extract the contours (by default, it is the vp::CONTOUR_RETR_TREE method)
+\snippet tutorial-contour.cpp Find contours
+- draw the contours if wanted
+\snippet tutorial-contour.cpp Draw contours
+
+
+The result images for each step are:
+
+\image html img-auto-threshold-grid36-03.png "Input image"
+
+\image html img-tutorial-contour-binarisation.png "Image after binarization using the Otsu method"
+
+\image html img-tutorial-contour-draw-contours.png "Contours extracted and displayed on a new image"
+
+To understand how the hierarchical contours extraction works, let's switch on another example. In a terminal, run:
+
+\code
+$ ./tutorial-contour --input Contours_tree.pgm --white_foreground
+\endcode
+
+The image after binarisation:
+
+\image html img-tutorial-contour-binarisation2.png "Image after binarization using the Otsu method"
+
+Instead of drawing all the contours with the same color, we can assign a first color for vp::CONTOUR_OUTER contour and a second color for vp::CONTOUR_HOLE contour.
+
+The function to navigate in the contour tree is the following:
+
+\snippet tutorial-contour.cpp Draw contours hierarchical func
+
+The call to draw the hierarchical contours:
+
+\snippet tutorial-contour.cpp Draw contours hierarchical
+
+The result image is:
+
+\image html img-tutorial-contour-draw-contours2.png "Contours extracted and displayed on a new image, in red outer contours, in green hole contours"
+
+To display the hierarchy, we can use this function:
+
+\snippet tutorial-contour.cpp Print contours hierarchy func
+
+For the vp::CONTOUR_RETR_TREE method, the output is:
+
+<blockquote>
+Contour:\n
+ level: 0\n
+ contour type: hole contour\n
+ contour size: 0\n
+ nb children: 3\n\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 438\n
+ nb children: 0\n\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 748\n
+ nb children: 0\n\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 2012\n
+ nb children: 1\n\n
+
+Contour:\n
+ level: 2\n
+ contour type: hole contour\n
+ contour size: 1906\n
+ nb children: 1\n\n
+
+Contour:\n
+ level: 3\n
+ contour type: outer contour\n
+ contour size: 1610\n
+ nb children: 1\n\n
+
+Contour:\n
+ level: 4\n
+ contour type: hole contour\n
+ contour size: 1494\n
+ nb children: 1\n\n
+
+Contour:\n
+ level: 5\n
+ contour type: outer contour\n
+ contour size: 792\n
+ nb children: 2\n\n
+
+Contour:\n
+ level: 6\n
+ contour type: hole contour\n
+ contour size: 372\n
+ nb children: 0\n\n
+
+Contour:\n
+ level: 6\n
+ contour type: hole contour\n
+ contour size: 392\n
+ nb children: 0\n
+</blockquote>
+
+The top level contour is always the root contour with zero contour point and which contains the list of contours.
+
+For the vp::CONTOUR_RETR_EXTERNAL method, the output is:
+
+<blockquote>
+Contour:\n
+ level: 0\n
+ contour type: hole contour\n
+ contour size: 0\n
+ nb children: 3\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 438\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 748\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 2012\n
+ nb children: 0\n
+</blockquote>
+
+The result image is:
+
+\image html img-tutorial-contour-draw-contours3.png "External contours extracted and displayed on a new image"
+
+For the vp::CONTOUR_RETR_LIST method, the output is:
+
+<blockquote>
+Contour:\n
+ level: 0\n
+ contour type: hole contour\n
+ contour size: 0\n
+ nb children: 9\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 438\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 748\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 2012\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: hole contour\n
+ contour size: 1906\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 1610\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: hole contour\n
+ contour size: 1494\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: outer contour\n
+ contour size: 792\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: hole contour\n
+ contour size: 372\n
+ nb children: 0\n
+
+Contour:\n
+ level: 1\n
+ contour type: hole contour\n
+ contour size: 392\n
+ nb children: 0\n
+</blockquote>
+
+\section imgproc_contour_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-connected-components, for a similar method to extract the connected-components in a grayscale image.
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-contrast-sharpening.doc b/doc/tutorial/imgproc/tutorial-imgproc-contrast-sharpening.doc
new file mode 100644
index 0000000..2ef50e7
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-contrast-sharpening.doc
@@ -0,0 +1,124 @@
+/**
+
+\page tutorial-imgproc-contrast-sharpening Tutorial: Contrast and image sharpening techniques
+\tableofcontents
+
+\section imgproc_contrast_sharpening_intro Introduction
+
+While the ViSP library is not intended to be an image processing library or replace a raster graphics editor, some easy image processing techniques can be used to improve the contrast and the sharpness of an image.
+
+The different methods presented are:
+- histogram stretching, see the corresponding <a href="https://docs.gimp.org/en/plug-in-c-astretch.html">Gimp documentation</a>.
+- histogram stretching in the HSV color space, see the corresponding <a href="https://docs.gimp.org/en/plug-in-autostretch-hsv.html">Gimp documentation</a>.
+- histogram equalization, see the corresponding <a href="https://en.wikipedia.org/wiki/Histogram_equalization">Wikipedia entry</a>.
+- contrast limited adaptive histogram equalization (CLAHE), see the corresponding <a href="https://en.wikipedia.org/wiki/Adaptive_histogram_equalization#Contrast_Limited_AHE">Wikipedia entry</a>.
+- unsharp masking, an image sharpening technique, see the corresponding <a href="https://en.wikipedia.org/wiki/Unsharp_masking">Wikipedia entry</a>.
+
+The first two methods consist of stretching the histogram of an image to make it use the entire range of values. It is more or less similar to the histogram equalization technique (presented in \ref imgproc_brightness_histogram_equalization).
+The stretching will act like a direct mapping between the old and new intensity values whereas the histogram equalization will linearize the cumulative histogram distribution to try to make each intensity values the same weighting in the image.
+The CLAHE algorithm will limit the contrast enhancement using a maximum slope value to avoid to amplify too much the image noise. It will also compute the cumulative histogram locally by sliding a window around the current pixel location.
+
+\section imgproc_contrast_sharpening_example Example code
+
+The following example also available in tutorial-contrast-sharpening.cpp will show the result of each of these methods on a low contrast image.
+
+\include tutorial-contrast-sharpening.cpp
+
+These functions are provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-contrast-sharpening.cpp Include
+
+The first step is to read the input image:
+
+\snippet tutorial-contrast-sharpening.cpp Read
+
+The low contrast color image used in this tutorial can be downloaded <a href="https://upload.wikimedia.org/wikipedia/commons/f/fd/Crayfish_low_contrast.JPG">here</a> (By Biem (Own work) [Public domain], via Wikimedia Commons):
+
+\image html img-tutorial-contrast-sharpening-Crayfish-low-contrast.png "Input low contrast color image"
+
+The figure below represents the histogram and the cumulative histogram of the low contrast image. Most of the histogram bins are approximately in the [80 - 140] range, resulting in an image with low dynamic.
+
+\image html img-tutorial-contrast-sharpening-Crayfish-low-contrast-hist.png "Histogram and cumulative histogram of the input image"
+
+The histogram stretching can be done with:
+
+\snippet tutorial-contrast-sharpening.cpp Stretch contrast
+
+The result is:
+
+\image html img-tutorial-contrast-sharpening-histogram-stretching.png "Histogram stretching"
+
+The corresponding histogram and cumulative histogram are the following:
+
+\image html img-tutorial-contrast-sharpening-histogram-stretching-hist.png "Histogram and cumulative histogram of the stretched histogram image"
+
+This method stretches the histogram with a direct mapping between the old and the new intensity values. The histogram bins are more spread out and the image gains some dynamic. It will not change the intensity distribution as the histogram equalization method could do.
+
+The histogram stretching on HSV colorspace can be done with:
+
+\snippet tutorial-contrast-sharpening.cpp Stretch contrast HSV
+
+\image html img-tutorial-contrast-sharpening-histogram-stretching-HSV.png "Histogram stretching on HSV colorspace"
+
+The main difference is that this method will stretch the Saturation and Value components and preserve the Hue channel.
+
+From the Gimp documentation:
+<blockquote>
+it works in HSV color space, rather than RGB color space, and it preserves the Hue. Thus, it independently stretches the ranges of the Hue, Saturation and Value components of the colors. Occasionally the results are good, often they are a bit odd.
+</blockquote>
+
+\image html img-tutorial-contrast-sharpening-histogram-stretching-HSV-hist.png "Histogram and cumulative histogram of the stretched histogram image in HSV colorspace"
+
+The histogram and cumulative histogram are similar to the previous method as expected.
+
+To improve the image contrast using the histogram equalization method:
+
+\snippet tutorial-contrast-sharpening.cpp Histogram equalization
+
+The result is:
+
+\image html img-tutorial-contrast-sharpening-histogram-equalization.png "Histogram equalization"
+
+If we look at the histogram and the cumulative histogram:
+
+\image html img-tutorial-contrast-sharpening-histogram-equalization-hist.png "Histogram and cumulative histogram of the histogram equalized image"
+
+The cumulative histogram is more linear which can be related to a more equal distribution of the pixel intensities in the image.
+
+To use the CLAHE algorithm:
+
+\snippet tutorial-contrast-sharpening.cpp CLAHE
+
+The CLAHE method avoid the over amplification of the noise compared to the histogram equalization method:
+
+\image html img-tutorial-contrast-sharpening-CLAHE.png "Contrast limited adaptive histogram equalization (blockRadius=150, bins=256, slope=3)"
+
+The parameters are:
+- the block radius: the size (2*blockRadius+1) of the neighborhood to consider around the current pixel location
+- the number of bins for the histogram computation
+- the maximum slope to limit the contrast enhancement
+
+The histogram of the corrected image is stretched and the local processing plus the limitation of the contrast enhancement avoid the over boosting of the contrast as in the histogram equalization case.
+
+\image html img-tutorial-contrast-sharpening-CLAHE-hist.png "Histogram and cumulative histogram after using the CLAHE method"
+
+The unsharp masking will sharpen the edges in an image:
+
+\snippet tutorial-contrast-sharpening.cpp Unsharp mask
+
+It is applied here on the image after using the CLAHE algorithm:
+
+\image html img-tutorial-contrast-sharpening-unsharp-masking.png "Unsharp masking (weight=0.5, Gaussian blur size=11) on the processed image after CLAHE"
+
+Two parameters can be modified:
+- the size of the Gaussian kernel, see vpImageFilter::gaussianBlur(const vpImage<double> &, vpImage<double> &, unsigned int, double, bool)
+- the unsharp masking weighting: \f$ I_{sharpen} = \frac{\left( I_{original} - weight \times I_{blurred} \right)}{\left( 1 - weight \right)} \f$
+
+To summarize, the techniques presented to improve the contrast of an image can do a good job in some situations and not in another.
+The CLAHE algorithm and the unsharp masking can be tuned (but the default values should be good enough in most of the situation).
+
+\section imgproc_contrast_sharpening_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-autothreshold, to learn how to automatically threshold / binarise a grayscale image.
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-count-coins.doc b/doc/tutorial/imgproc/tutorial-imgproc-count-coins.doc
new file mode 100644
index 0000000..c29d637
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-count-coins.doc
@@ -0,0 +1,123 @@
+/**
+
+\page tutorial-imgproc-count-coins Practical example: count the number of coins in an image
+\tableofcontents
+
+\section imgproc_count_coins_intro Introduction
+
+This tutorial will show you how to count the number of coins in an image. To do this, we will reuse some of the image processing techniques presented before and some other image processing methods:
+- automatic thresholding (\ref tutorial-imgproc-autothreshold)
+- flood fill (\ref tutorial-imgproc-flood-fill) / fill holes
+- find contours (\ref tutorial-imgproc-contour)
+- morphological operations
+- image moments
+
+The two sample images to test the image processing pipeline are:
+
+\image html img-tutorial-count-coins-coins1.png "Sample image 1"
+
+\image html img-tutorial-count-coins-coins2.png "Sample image 2"
+
+The second image can be downloaded <a href="https://upload.wikimedia.org/wikipedia/commons/c/c8/Fransiz_frangi_madeni_paralari.PNG">here</a> (by Atacanus (Own work) [Public domain], via Wikimedia Commons).
+
+We assume similar lighting condition and coins placement in the image to build the image processing pipeline.
+
+\section imgproc_count_coins_example Example code
+
+The example code also available in tutorial-count-coins.cpp is:
+
+\include tutorial-count-coins.cpp
+
+To run the demo code for the sample image 1:
+
+\code
+$ ./tutorial-count-coins
+\endcode
+
+To run the demo code for the sample image 2:
+
+\code
+$ ./tutorial-count-coins --input coins2.pgm --white_foreground
+\endcode
+
+The functions we will use needs the following includes:
+
+\snippet tutorial-count-coins.cpp Include
+
+The image processing functions in the \a imgproc module are declared in the \a vp:: namespace.
+
+The first thing to do is to read the image using:
+
+\snippet tutorial-count-coins.cpp Read
+
+As we assume the coins are placed on an uniform background with a distinct color, we can use an automatic thresholding method to binarize the image:
+
+\snippet tutorial-count-coins.cpp Binarisation
+
+We use an option to switch between dark or bright background. The coins will be represented with 255 values after the binarisation, as you can see in the following images:
+
+\image html img-tutorial-count-coins-binarisation1.png "Otsu's thresholding for the sample image 1"
+
+\image html img-tutorial-count-coins-binarisation2.png "Otsu's thresholding for the sample image 2"
+
+You can notice some "holes" in the binarisation due to some shiny parts of the coin for the first case and to some dark parts of the coin in the second case.
+
+We can now use a function to fill the holes in the binary images:
+
+\snippet tutorial-count-coins.cpp Fill holes
+
+The fill holes algorithm is basic:
+- flood fill the binary image using a seed point just outside of the image
+
+\image html img-tutorial-count-coins-mask.png "Left: binary image, right: result of the flood fill operation"
+
+- substract the flood fill image from a white image to get only the holes
+
+\image html img-tutorial-count-coins-white-holes.png "Top left: white image, bottom left: flood fill image, right: I_holes = I_white - I_flood_fill"
+
+- add the holes image to the binary image to get an image without holes
+
+\image html img-tutorial-count-coins-fill-holes.png "Top left: binary image, bottom left: holes image, right: I_fill = I_bin + I_holes"
+
+Similarly for the sample image 1, the binary image with holes filled is:
+
+\image html img-tutorial-count-coins-fill1.png "Binary image with holes filled for the sample image 1"
+
+To "clean" the binary image, we will now perform some morphological operations:
+- an <a href="https://en.wikipedia.org/wiki/Opening_(morphology)">opening</a> (an erosion followed by a dilatation) to remove some potential single foreground pixels
+\snippet tutorial-count-coins.cpp Opening
+
+- a <a href="https://en.wikipedia.org/wiki/Closing_(morphology)">closing</a> (a dilatation followed by an erosion) to fill some remaining small holes
+\snippet tutorial-count-coins.cpp Closing
+
+
+The result image for the sample image 1 is (the morphological operations do not improve the binarisation for the sample 2):
+
+\image html img-tutorial-count-coins-close1.png "Binary image after an opening and a closing"
+
+Now that we have properly binarized the images to get only the coins, we can extract the contours. As the function expects a binary image with values 0/1, we need to create a new image accordingly:
+
+\snippet tutorial-count-coins.cpp Find contours
+
+To display the extracted contours, we can use:
+
+\snippet tutorial-count-coins.cpp Draw contours
+
+To count the number of coins, we use the number of extracted contours. But to be robust to some remaining bad binarized pixels, we will measure the area of the contour (\f$ m_{00} \f$) and discard too small area contours.
+The image moments are used to compute the area of the contour and the centroid (\f$ x_{centroid}=\frac{m_{10}}{m_{00}} \f$, \f$ y_{centroid}=\frac{m_{01}}{m_{00}} \f$) of the contour to display some texts:
+
+\snippet tutorial-count-coins.cpp Count coins
+
+The final result images are:
+
+\image html img-tutorial-count-coins-count-coins1.png "9 coins have been detected for the sample image 1"
+
+\image html img-tutorial-count-coins-count-coins2.png "11 coins have been detected for the sample image 2"
+
+This tutorial showed you how some basic image processing techniques can be used to create an application to count the number of coins in an image. Some assumptions must be made to guarantee that the image processing pipeline will work:
+- the coins are placed on an uniform background with a different color (to be able to automatically threshold the image)
+- the coins must be isolated from each other (to be able to extract the contours)
+- the image must be clean (to avoid to use too much some morphological operations)
+- the size of the coins in the image is more or less defined (to be able to discard contours that are not coins using the contour area)
+
+*/
diff --git a/doc/tutorial/imgproc/tutorial-imgproc-flood-fill.doc b/doc/tutorial/imgproc/tutorial-imgproc-flood-fill.doc
new file mode 100644
index 0000000..9788266
--- /dev/null
+++ b/doc/tutorial/imgproc/tutorial-imgproc-flood-fill.doc
@@ -0,0 +1,77 @@
+/**
+
+\page tutorial-imgproc-flood-fill Tutorial: Flood fill algorithm
+\tableofcontents
+
+\section imgproc_flood_fill_intro Introduction
+
+This tutorial will show you how to use the <a href="https://en.wikipedia.org/wiki/Flood_fill">flood fill</a> algorithm to fill connected areas in an image. This method is used for instance in raster graphics editor to perform the "bucket" fill tool.
+
+The principle for a 2D image is the following:
+- in input: a seed point, the pixel value to be replaced that forms the connected area, the pixel value to replace
+- starting from a seed point
+- explore the neighborhood looking for same pixel values
+- replace the old pixel value by the desired one
+
+To explore the neighborhood, a 4-connexity or 8-connexity search are possible:
+
+\image html img-tutorial-flood-fill-4-connexity.png "4-connexity"
+
+\image html img-tutorial-flood-fill-8-connexity.png "8-connexity"
+
+\section imgproc_flood_fill_example Example code
+
+The following example also available in tutorial-flood-fill.cpp will mimic the "bucket" fill tool:
+
+\include tutorial-flood-fill.cpp
+
+What the tutorial code does from the user side is:
+- let the user draw 3 polygons on a raster (bitmap) image
+- do the "bucket" fill tool when the user clicks on a pixel location
+
+The flood fill function is provided in a \a vp:: namespace and accessible using this include:
+
+\snippet tutorial-flood-fill.cpp Include
+
+The first thing to do is to create a raster image of 640x480 size:
+
+\snippet tutorial-flood-fill.cpp Create bitmap
+
+These lines of code allows the user to click in the image to draw a polygon shape:
+
+\snippet tutorial-flood-fill.cpp Draw polygons
+
+Now, we need to draw these polygons on a grayscale image (the current flood fill implementation needs a grayscale image) as the ViSP display is done on an internal image. This will be used as a mask: 255 pixel values are used for locations where we need to paint in the raster image.
+
+For this, we can use the <a href="https://en.wikipedia.org/wiki/Bresenham%27s_line_algorithm">Bresenham's line algorithm</a> to actually draw a line from a starting and ending point.
+The direct C++ implementation of the Bresenham's line algorithm is the following:
+
+\snippet tutorial-flood-fill.cpp Bresenham's line algorithm
+
+Draw the polygon lines on a grayscale image is then simple:
+
+\snippet tutorial-flood-fill.cpp Draw polygon lines
+
+Now we need to be able to get the 2D image location when the user click on the image to select the seed point:
+
+\snippet tutorial-flood-fill.cpp Seed point click
+
+The flood fill is performed using the desired seed point and by replacing 0 pixel values by 255 values in the mask image. A 4-connexity is used as the 8-connexity will overflow due to the thin line drawing.
+
+\snippet tutorial-flood-fill.cpp Flood fill
+
+The final step is to update the raster image:
+
+\snippet tutorial-flood-fill.cpp Bucket fill
+
+Here are some steps illustrated with images:
+
+\image html img-tutorial-flood-fill-draw-polygons.png "Polygons drawn by the user"
+
+\image html img-tutorial-flood-fill-bucket-fill.png "Raster image after bucket fill"
+
+\section imgproc_flood_fill_next Next tutorial
+
+You can now read the \ref tutorial-imgproc-count-coins, for a final tutorial that will illustrate some of the image processing techniques presented in these tutorials on a specific use case: how to count the number of coins in an image.
+
+*/
diff --git a/doc/tutorial/ios/tutorial-detection-apriltag-ios.doc b/doc/tutorial/ios/tutorial-detection-apriltag-ios.doc
new file mode 100644
index 0000000..a3bdc38
--- /dev/null
+++ b/doc/tutorial/ios/tutorial-detection-apriltag-ios.doc
@@ -0,0 +1,62 @@
+/**
+
+\page tutorial-detection-apriltag-ios Tutorial: AprilTag marker detection on iOS
+\tableofcontents
+
+\section intro_apriltag_ios Introduction
+
+This tutorial follows the \ref tutorial-detection-apriltag and shows how AprilTag marker detection could be achieved with ViSP on iOS devices.
+
+In the next section you will find an example that show how to detect tags in a single image. To know how to print an AprilTag marker, see \ref apriltag_detection_print.
+
+Note that all the material (Xcode project and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedAprilTag
+\endcode
+
+
+\section apriltag_detection_basic_ios AprilTag detection and pose estimation (single image)
+
+Let us consider the Xcode project named \c StartedAprilTag that is part of ViSP source code. This project is a Xcode \c "Single view application" that contain \c ImageConversion.h and \c ImageConversion.mm to convert from/to UIImage to ViSP images (see \ref image_ios_convert). It contains also \c ImageDisplay.h and \c ImageDisplay.mm files useful to display lines and frames in an image overlay, \c ViewController.mm that handles the tag detection, and an image \c AprilTag.png used as input.
+\image html img-detection-apriltag-ios-xcode.png
+
+To download the complete \c StartedAprilTag project, run the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedAprilTag
+\endcode
+
+Once downloaded, you have just to drag & drop ViSP and OpenCV frameworks available following \ref tutorial-install-ios-package.
+\image html img-detection-apriltag-ios-drag-drop.png
+
+In the dialog box, enable check box \c "Copy item if needed" to add \c visp3.framework and \c opencv.framework to the project.
+\image html img-started-imgproc-ios-drag-drop-dialog.png
+
+Now you should be able to build and run your application.
+
+
+\section apriltag_detection_display Image display functions
+
+The Xcode project \c StartedAprilTag contains \c ImageDisplay.h and \c ImageDisplay.mm files that implement the functions to display a line or a frame in overlay of an UIImage.
+
+\subsection apriltag_detection_display_line Display a line
+
+The following function implemented in \c ImageDisplay.mm show how to display a line.
+
+\snippet ImageDisplay.mm display line
+
+\subsection apriltag_detection_display_frame Display a 3D frame
+
+The following function implemented in \c ImageDisplay.mm show how to display a 3D frame; red line for x, green for y and blue for z axis.
+
+\snippet ImageDisplay.mm display frame
+
+\section apriltag_detection_basic_output Application output
+
+Once build, if you run \c StartedAprilTag application on your device, you should be able to see the following screen shot:
+
+\image html img-detection-apriltag-ios-output.png
+
+
+*/
diff --git a/doc/tutorial/ios/tutorial-getting-started-iOS.doc b/doc/tutorial/ios/tutorial-getting-started-iOS.doc
new file mode 100644
index 0000000..1171016
--- /dev/null
+++ b/doc/tutorial/ios/tutorial-getting-started-iOS.doc
@@ -0,0 +1,160 @@
+
+/**
+ \page tutorial-getting-started-iOS Tutorial: How to create a basic iOS application that uses ViSP
+ \tableofcontents
+
+
+ \note We assume that you have \c "ViSP for iOS" either after following \ref tutorial-install-ios-package or \ref tutorial-install-iOS. Following one of these tutorials allows to exploit \c visp3.framework and \c opencv.framework to build an application for iOS devices.
+
+In this tutorial we suppose that you install \c visp3.framework in a folder named \c \<framework_dir\>/ios. If \c \<framework_dir\> corresponds to \c ~/framework, you should get the following:
+
+\code
+$ ls ~/framework/ios
+opencv2.framework visp3.framework
+\endcode
+
+Note also that all the material (source code and Xcode project) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image/ios/GettingStarted
+\endcode
+
+\section getting-started-iOS-create Create a new Xcode project
+
+- Launch Xcode
+- Follow <tt>"File>New>Project"</tt> menu and create a new <tt>"Single View Application"</tt>
+
+\image html img-getting-started-iOS-create.png
+
+- Click on <tt>"Next"</tt> button and complete the options for your new project:
+
+\image html img-getting-started-iOS-options.png
+
+- Click on <tt>"Next"</tt> button and select the folder where the new project will be saved. Once done click on <tt>"Create"</tt>. Now you should have something similar to:
+
+\image html img-getting-started-iOS-new.png
+
+\section getting-started-iOS-link-visp Linking ViSP framework
+
+Now we need to link \c visp3.framework with the Xcode project.
+
+- Select the project navigator in the left hand panel (1) and click on project name <tt>"Getting Started"</tt> (2)
+
+\image html img-getting-started-iOS-navigator.png
+
+- Use the Finder to drag & drop ViSP and OpenCV frameworks located in \c \<framework_dir\>/ios folder in the left hand panel containing all the project files.
+\image html img-getting-started-iOS-drag-drop.png
+
+- In the dialog box, enable check box <tt>"Copy item if needed"</tt> to ease \c visp3.framework and \c opencv.framework headers location addition to the build options
+
+\image html img-getting-started-iOS-drag-drop-dialog.png
+
+- Click on <tt>"Finish"</tt>. You should now get something similar to the following image
+\image html img-getting-started-iOS-link-visp.png
+
+\section getting-started-iOS-app Writing a ViSP iOS application
+
+- Because we will mix Objective C and ViSP C++ Code, rename \c ViewController.m file into \c ViewController.mm
+\image html img-getting-started-iOS-rename.png
+
+- Now copy/paste the following getting started sample code (inspired from tutorial-homography-from-points.cpp) into \c ViewController.mm
+\code
+#import "ViewController.h"
+
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+ at interface ViewController ()
+
+ at end
+
+ at implementation ViewController
+
+#pragma mark - Example of a function that uses ViSP
+- (void)processViSPHomography{
+
+ std::vector<vpPoint> oP(4), aP(4), bP(4);
+ double L = 0.1;
+
+ oP[0].setWorldCoordinates( -L,-L, 0);
+ oP[1].setWorldCoordinates(2*L,-L, 0);
+ oP[2].setWorldCoordinates( L, 3*L, 0);
+ oP[3].setWorldCoordinates( -L, 4*L, 0);
+
+ vpHomogeneousMatrix bMo(0,0, 1, 0, 0, 0) ;
+ vpHomogeneousMatrix aMb(0.2, 0, 0.1, 0,vpMath::rad(20), 0);
+ vpHomogeneousMatrix aMo = aMb*bMo ;
+
+ // Normalized coordinates of points in the image frame
+ std::vector<double> xa(4), ya(4), xb(4), yb(4);
+
+ for(int i=0 ; i < 4; i++){
+ oP[i].project(aMo);
+ xa[i] = oP[i].get_x();
+ ya[i] = oP[i].get_y();
+ oP[i].project(bMo);
+ xb[i] = oP[i].get_x();
+ yb[i] = oP[i].get_y();
+ }
+
+ vpHomography aHb ;
+
+ // Compute the homography
+ vpHomography::DLT(xb, yb, xa, ya, aHb, true);
+
+ std::cout << "Homography:\n" << aHb << std::endl;
+
+ vpRotationMatrix aRb;
+ vpTranslationVector atb;
+ vpColVector n;
+
+ // Compute the 3D transformation
+ aHb.computeDisplacement(aRb, atb, n);
+
+ std::cout << "atb: " << atb.t() << std::endl;
+
+ // Compute coordinates in pixels of point 3
+ vpImagePoint iPa, iPb;
+ vpCameraParameters cam;
+ vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
+ vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
+
+ std::cout << "Ground truth:" << std::endl;
+ std::cout << " Point 3 in pixels in frame b: " << iPb << std::endl;
+ std::cout << " Point 3 in pixels in frame a: " << iPa << std::endl;
+
+ // Estimate the position in pixel of point 3 from the homography
+ vpMatrix H = cam.get_K() * aHb * cam.get_K_inverse();
+
+ // Project the position in pixel of point 3 from the homography
+ std::cout << "Estimation from homography:" << std::endl;
+ std::cout << " Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb) << std::endl;
+}
+
+- (void)viewDidLoad {
+ [super viewDidLoad];
+ // Do any additional setup after loading the view, typically from a nib.
+ [self processViSPHomography];
+}
+
+- (void)didReceiveMemoryWarning {
+ [super didReceiveMemoryWarning];
+ // Dispose of any resources that can be recreated.
+}
+ at end
+\endcode
+In this sample, we first import the headers to use vpHomography class. Then we create a new function called \c processViSPHomography(). This function is finally called in viewDibLoad().
+- After the previous copy/paste, you should have something similar to
+\image html img-getting-started-iOS-code.jpeg
+
+- Now we are ready to build this simple <tt>"Getting Started"</tt> application using Xcode <tt>"Product>Build"</tt> menu.
+
+- You can now run your code using <tt>"Product>Run"</tt> menu (Simulator or device does not bother because we are just executing code). You should obtain these logs showing that visp code was correctly executed by your iOS project.
+\image html img-getting-started-iOS-log.jpeg
+
+\section getting-started-iOS-next Next tutorial
+
+You are now ready to see the \ref tutorial-image-ios.
+
+*/
diff --git a/doc/tutorial/ios/tutorial-image-ios.doc b/doc/tutorial/ios/tutorial-image-ios.doc
new file mode 100644
index 0000000..d6ce871
--- /dev/null
+++ b/doc/tutorial/ios/tutorial-image-ios.doc
@@ -0,0 +1,85 @@
+/**
+
+\page tutorial-image-ios Tutorial: Image processing on iOS
+\tableofcontents
+
+This tutorial supposes that you have followed the \ref tutorial-getting-started-iOS.
+
+\section image_ios_intro Introduction
+
+In this tutorial you will learn how to do simple image processing on iOS devices with ViSP. This application loads a color image (<a href="https://github.com/lagadic/visp/blob/master/tutorial/ios/StartedImageProc/StartedImageProc/monkey.png">monkey.png</a>) and allows the user to visualize either this image in grey level, either the image gradients, or either canny edges on iOS simulator or devices.
+
+In ViSP images are carried out using vpImage class. However in iOS, image rendering has to be done using UIImage class that is part of the Core Graphics framework available in iOS. In this tutorial we provide the functions that allow to convert a vpImage to an UIImage and \e vice \e versa.
+
+Note that all the material (source code and image) used in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedImageProc
+\endcode
+
+
+\section image_ios_app StartedImageProc application
+
+Let us consider the Xcode project named \c StartedImageProc that is part of ViSP source code. This project is a Xcode \c "Single view application" where we renamed \c ViewController.m into \c ViewController.mm, introduced minor modifications in ViewController.h and add <a href="https://github.com/lagadic/visp/blob/master/tutorial/ios/StartedImageProc/StartedImageProc/monkey.png">monkey.png</a> image.
+
+\image html img-started-imgproc-ios.jpeg
+
+To download the complete \c StartedImageProc project, run the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/ios/StartedImageProc
+\endcode
+
+Once downloaded, you have just to drag & drop ViSP and OpenCV frameworks available following \ref tutorial-install-ios-package.
+\image html img-started-imgproc-ios-drag-drop.jpeg
+
+In the dialog box, enable check box \c "Copy item if needed" to add \c visp3.framework and \c opencv.framework to the project.
+\image html img-started-imgproc-ios-drag-drop-dialog.png
+
+Now you should be able to build and run your application.
+
+\section image_ios_convert Image conversion functions
+
+The Xcode project \c StartedImageProc contains \c ImageConversion.h and \c ImageConversion.mm files that implement the functions to convert UIImage to ViSP vpImage and vice versa.
+\subsection image_ios_convert_uiimage_vpimage_color UIImage to color vpImage
+
+The following function implemented in \c ImageConversion.mm show how to convert an UIImage into a vpImage<vpRGBa> instanciated as a color image.
+
+\snippet ImageConversion.mm vpImageColorFromUIImage
+
+\subsection image_ios_convert_uiimage_vpimage_gray UIImage to gray vpImage
+
+The following function implemented in \c ImageConversion.mm show how to convert an UIImage into a vpImage<unsigned char> instanciated as a grey level image.
+
+\snippet ImageConversion.mm vpImageGrayFromUIImage
+
+\subsection image_ios_convert_vpimage_color_uiimage Color vpImage to UIImage
+
+The following function implemented in \c ImageConversion.mm show how to convert a gray level vpImage<unsigned char> into an UIImage.
+
+\snippet ImageConversion.mm UIImageFromVpImageColor
+
+\subsection image_ios_convert_vpimage_gray_uiimage Gray vpImage to UIImage
+
+The following function implemented in \c ImageConversion.mm show how to convert a color vpImage<vpRGBa> into an UIImage.
+
+\snippet ImageConversion.mm UIImageFromVpImageGray
+
+\section image_ios_output Application output
+
+Once build, if you run \c StartedImageProc application on your device, you should be able to see the following screen shots.
+
+- Selecting \c "load image" gives the following result:
+\image html img-started-imgproc-ios-output-color.jpeg
+- Selecting \c "convert to gray" gives the following result:
+\image html img-started-imgproc-ios-output-gray.png
+- Selecting \c "compute gradient" gives the following result:
+\image html img-started-imgproc-ios-output-gradient.png
+- Selecting \c "canny detector" gives the following result:
+\image html img-started-imgproc-ios-output-canny.png
+
+\section image_ios_next Next tutorial
+
+You are now ready to see the \ref tutorial-detection-apriltag-ios.
+
+*/
diff --git a/doc/tutorial/ios/tutorial-install-iOS.doc b/doc/tutorial/ios/tutorial-install-iOS.doc
new file mode 100644
index 0000000..652afd3
--- /dev/null
+++ b/doc/tutorial/ios/tutorial-install-iOS.doc
@@ -0,0 +1,100 @@
+/**
+
+\page tutorial-install-iOS Tutorial: Installation from source for iOS devices
+\tableofcontents
+
+In this tutorial you will learn how to build ViSP framework from source on OSX in order to use it for iOS developments. These steps have been tested:
+- on OSX El Capitan 10.11.45 and 10.11.6, with CMake 3.5.2, Xcode 7.3.1 and 8.0, and Python 2.7.10
+- on OSX Sierra 10.12.5, with CMake 3.9.1, Xcode 8.3.3, and Python 2.7.10.
+- on OSX Sierra 10.12.6, with CMake 3.9.1, Xcode 9.0.1, and Python 2.7.10.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_iOS_required Install required software
+
+- CMake gui 2.8.12.2 for OSX or higher that could be download at : http://www.cmake.org
+- Xcode
+- Python 2.7 (not tested with Python 3)
+
+\section install_iOS_opencv Getting OpenCV framework (optional)
+
+We suggest to follow these steps in order to install OpenCV framework in such a way that the resulting ViSP framework builds with OpenCV as 3rd party. This could be useful for users who want to use ViSP and OpenCV together. It will also allow to exploit keypoints throw ViSP classes (vpKltOpencv, vpKeyPoint) or the model-based tracker in his hybrid version (vpMbKltTracker). If you are not interested in capabilities leveraged by using OpenCV as 3rd party, just skip this section.
+
+- Download the latest <a href="http://opencv.org/releases.html">OpenCV for iOS</a> like \c opencv-3.3.1-ios-framework.zip
+- Unzip the archive in a directory denoted \c \<framework_dir\>/ios. If \c \<framework_dir\> is for example the following folder \c ~/framework just do the following:
+\code
+$ mkdir -p ~/framework/ios
+\endcode
+If the download was performed with Safari, do the following
+\code
+$ mv ~/Downloads/opencv2.framework ~/framework/ios/
+\endcode
+or if the download was either performed with Firefox, do the following
+\code
+$ unzip ~/Downloads/opencv-3.3.0-ios-framework.zip -d ~/framework/ios
+\endcode
+- Add \c opencv2.framework to iPhoneOS existing frameworks
+\code
+$ cd `xcrun --sdk iphoneos --show-sdk-platform-path`/Developer/Library/Frameworks
+$ sudo ln -s ~/framework/ios/opencv2.framework
+\endcode
+- Add \c opencv2.framework to iPhoneSimulator existing frameworks
+\code
+$ cd `xcrun --sdk iphonesimulator --show-sdk-platform-path`/Developer/Library/Frameworks
+$ sudo ln -s ~/framework/ios/opencv2.framework
+\endcode
+
+
+\section install_iOS_get_source Getting ViSP source code
+
+\note The following steps are only working with ViSP 3.0.1 or higher.
+
+Here the goal is to get ViSP source code in a directory denoted \c \<framework_dir\>. If \c \<framework_dir\> is for example the following folder \c ~/framework there are different ways to get ViSP:
+
+- You can get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd ~/framework
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C ~/framework
+\endcode
+
+- Or you can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C ~/framework
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d ~/framework
+\endcode
+
+- We suppose now that ViSP source is in a directory denoted \c \<framework_dir\> besides ios folder.
+\code
+$ ls ~/framework
+ios visp
+\endcode
+
+\section install_iOS_build Building ViSP framework
+
+- Build ViSP framework
+\code
+$ cd ~/framework
+$ python visp/platforms/ios/build_framework.py ios
+\endcode
+
+- Once build, you will have ``~/framework/ios/visp3.framework`` that could be added to a Xcode project for iOS devices. If you follow \ref install_iOS_opencv you have also ``~/framework/ios/opencv2.framework`` that should also be added to the Xcode project.
+\code
+$ ls ~/framework/ios
+build opencv2.framework visp3.framework
+\endcode
+
+\note ViSP framework is build with capabilities that enables the usage of the following 3rd parties: XML, OpenCV, pthread, apriltag, lapack.
+
+\section install_iOS_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started-iOS that shows how to use ViSP as a 3rd party to build your own project for iOS devices.
+
+*/
diff --git a/doc/tutorial/ios/tutorial-install-ios-package.doc b/doc/tutorial/ios/tutorial-install-ios-package.doc
new file mode 100644
index 0000000..921a420
--- /dev/null
+++ b/doc/tutorial/ios/tutorial-install-ios-package.doc
@@ -0,0 +1,41 @@
+/**
+
+\page tutorial-install-ios-package Tutorial: Installation from prebuilt packages for iOS devices
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_ios_package_list ViSP framework for iOS
+
+ViSP is available as a prebuilt framework for iOS devices. Go to ViSP <a href="https://visp.inria.fr/download">download page</a> and get \c "ViSP for iOS" as a zip archive. Since this package was build with OpenCV as 3rd party, the zip archive contains also OpenCV framework it was built with to ensure compatibility.
+
+To install ViSP framework in a folder denoted \c \<framework_dir\>/ios that could be set to \c ~/framework/ios download \c "ViSP for iOS" with Safari or Firefox and do the following:
+
+\code
+$ mkdir -p ~/framework/ios
+\endcode
+
+If downloaded with Safari continue with:
+\code
+$ mv ~/Downloads/visp3/* ~/framework/ios/
+\endcode
+or if downloaded with Firefox, run:
+\code
+$ unzip ~/Downloads/visp3.framework*.zip -d ~/framework/ios
+\endcode
+
+The content of \c \<framework_dir\>/ios should be the following:
+\code
+$ ls ~/framework/ios
+opencv2.framework visp3.framework
+\endcode
+
+\note ViSP framework is build with capabilities that enables the usage of the following 3rd parties: XML, OpenCV, pthread, apriltag, lapack.
+
+\section install_ios_package_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started-iOS that will show you how to use ViSP as a 3rd party to build your own iOS application.
+
+*/
diff --git a/doc/tutorial-multi-threading.doc b/doc/tutorial/misc/tutorial-multi-threading.doc
similarity index 100%
rename from doc/tutorial-multi-threading.doc
rename to doc/tutorial/misc/tutorial-multi-threading.doc
diff --git a/doc/tutorial-plotter.doc b/doc/tutorial/misc/tutorial-plotter.doc
similarity index 100%
rename from doc/tutorial-plotter.doc
rename to doc/tutorial/misc/tutorial-plotter.doc
diff --git a/doc/tutorial/misc/tutorial-trace.doc b/doc/tutorial/misc/tutorial-trace.doc
new file mode 100644
index 0000000..59e2241
--- /dev/null
+++ b/doc/tutorial/misc/tutorial-trace.doc
@@ -0,0 +1,142 @@
+/**
+
+\page tutorial-trace Tutorial: Debug and trace printings
+\tableofcontents
+
+
+\section intro_trace Introduction
+
+ViSP allows to introduce trace and debug printings that may help debugging. To this end ViSP provides C or C++ macros that allows to print messages to the standard output std::cout or to std::cerr. The following table summarizes the macro defined in visp3/code/vpDebug.h header.
+
+~~~
+|----------|-------|-------------------------------------|---------------------------------------|
+| output | type | std::cout | std::cerr |
+|----------|-------|-------------------------------------|---------------------------------------|
+| C-like | trace | vpTRACE, vpTRACE(level) | vpERROR_TRACE, vpERROR_TRACE(level) |
+| | trace | vpIN_FCT, vpOUT_FCT | |
+| | debug | vpDEBUG_TRACE, vpDEBUG_TRACE(level) | vpDERROR_TRACE, vpDERROR_TRACE(level) |
+|----------|-------|-------------------------------------|---------------------------------------|
+| C++-like | trace | vpCTRACE | vpCERROR |
+| | debug | vpCDEBUG(level) | |
+|----------|-------|-------------------------------------|---------------------------------------|
+~~~
+
+\subsection trace_macro Macros for trace
+
+Macro for tracing vpTRACE(), vpTRACE(level), vpERROR_TRACE(), vpERROR_TRACE(level),
+vpIN_FCT() and vpOUT_FCT()
+work like printf with carrier return at the end of the string, while
+vpCTRACE() and vpCERROR() work like the C++ output streams std::cout
+and std::cerr. All these macro print messages only if VP_TRACE macro is defined. Macro that has \e level as parameter like vpTRACE(level) or vpERROR_TRACE(level) use an additional define named VP_DEBUG_MODE. They print only messages if VP_DEBUG_MODE >= \e level.
+
+\subsection debug_macro Macros for debug
+
+Macros for debug vpDEBUG_TRACE(), vpDEBUG_TRACE(level), vpDERROR_TRACE() and vpDERROR_TRACE(level)
+work like printf while vpCDEBUG(level) works like the C++ output stream std::cout.
+These macro print messages only if VP_DEBUG macro is defined. Macro that has \e level as parameter like vpDEBUG_TRACE(level) or vpDERROR_TRACE(level) use an additional define named VP_DEBUG_MODE. They print only messages if VP_DEBUG_MODE >= \e level.
+
+Moreover vpDEBUG_ENABLE(level) can be used to check if a given debug level is active; vpDEBUG_ENABLE(level) is
+equal to 1 if VP_DEBUG_MODE >= \e level, otherwise vpDEBUG_ENABLE(level) is equal to 0.
+
+\section debug_trace_usage Debug and trace usage in ViSP library
+
+In ViSP, before an exception is thrown, trace macro are widely used to inform the user that an error occur. This is redundant, since the same trace message in generally associated to the exception that is thrown. Since ViSP 3.1.0, during CMake configuration it is possible to tune debug and trace printings by setting \c ENABLE_DEBUG_LEVEL cmake variable.
+
+- To turn off debug and trace printings (this is the default), using cmake command just run :
+\code
+%cmake -DENABLE_DEBUG_LEVEL=0 <path to ViSP source code>
+\endcode
+
+- To turn on debug and trace printings with a debug level of 3, using cmake command just run :
+\code
+%cmake -DENABLE_DEBUG_LEVEL=3 <path to ViSP source code>
+\endcode
+
+- or using ccmake GUI as shown in the next snapshot:
+
+\image html img-cmake-debug-trace.jpg
+
+\note When \c ENABLE_DEBUG_LEVEL is set to 0 (this is the default behavior in ViSP), we don't define VP_TRACE and VP_DEBUG macro.
+
+\section example Debug and trace usage in your own project
+
+Note that all the material (source code) described in this section is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/trace
+\endcode
+
+If you develop a project that uses ViSP library as a 3rd party, there are different ways to benefit from debug and trace macro described previously.
+
+- If ViSP was build with debug and trace enabled using \c cmake \c ENABLE_DEBUG_LEVEL=\<level\>, debug and trace are also enabled in your development.
+- If debug and trace were disabled in ViSP (\c ENABLE_DEBUG_LEVEL=0), you can enable debug and trace in your own development either by defining \c VP_DEBUG and/or \c VP_TRACE macro in your code using
+\code
+#define VP_TRACE
+#define VP_DEBUG
+
+#include <visp3/core/vpDebug.h>
+\endcode
+either by modifying your \c CMakeLists.txt file by adding an option like:
+\code
+option(ENABLE_DEBUG_MODE "Enable debug and trace printings" ON)
+
+if(ENABLE_DEBUG_MODE)
+ add_definitions("-DVP_DEBUG -DVP_TRACE")
+endif()
+\endcode
+
+
+The following example also available in tutorial-trace.cpp shows how to use the previous macro.
+
+\includelineno tutorial-trace.cpp
+
+
+\note In the previous example it is important to notice that the following lines have to be put prior to any other ViSP includes:
+\code
+#define VP_DEBUG_MODE 2 // Activate debug level 1 and 2
+#include <visp3/core/vpDebug.h>
+\endcode
+For example, if you modify the previous example just by including <visp3/core/vpImage.h> on the top of the file, you will get the following warnings:
+\code
+Building CXX object tutorial/trace/CMakeFiles/tutorial-trace.dir/tutorial-trace.cpp.o
+.../ViSP-code/tutorial/trace/tutorial-trace.cpp:5:1: warning: "VP_DEBUG_MODE" redefined
+In file included from .../ViSP-build-debug/include/visp3/core/vpImage.h:52,
+ from .../ViSP-code/tutorial/trace/tutorial-trace.cpp:2:
+.../ViSP-build-debug/include/visp3/core/vpDebug.h:67:1: warning: this is the location of the previous definition
+\endcode
+
+
+When ViSP library was built without debug and trace the previous example produces the output:
+\code
+%./tutorial-trace
+Debug level 1 active: 0
+Debug level 2 active: 0
+Debug level 3 active: 0
+\endcode
+
+When ViSP is rather build with debug and trace the previous example produces the output:
+\code
+%./tutorial-trace
+(L0) begin /tmp/tutorial-trace.cpp: main(#9) : main()
+Debug level 1 active: 1
+Debug level 2 active: 1
+Debug level 3 active: 0
+(L0) /tmp/tutorial-trace.cpp: main(#17) : C-like trace
+(L1) /tmp/tutorial-trace.cpp: main(#18) : C-like trace level 1
+(L0) !! /tmp/tutorial-trace.cpp: main(#20) : C-like error trace
+(L1) !! /tmp/tutorial-trace.cpp: main(#21) : C-like error trace level 1
+(L0) /tmp/tutorial-trace.cpp: main(#24) : C-like debug trace
+(L0) !! /tmp/tutorial-trace.cpp: main(#25) : C-like error trace
+(L2) /tmp/tutorial-trace.cpp: main(#27) : C-like debug trace level 2
+(L2) !! /tmp/tutorial-trace.cpp: main(#28) : C-like error trace level 2
+(L0) /tmp/tutorial-trace.cpp: main(#31) : C++-like trace
+(L0) !! /tmp/tutorial-trace.cpp: main(#32) : C++-like error trace
+(L2) /tmp/tutorial-trace.cpp: main(#35) : C++-like debug trace level 2
+(L0) end /tmp/tutorial-trace.cpp: main(#37) : main()
+\endcode
+
+In the previous printings:
+- the number after "L" indicates the debug or trace level; example (L2) is for level 2.
+- the number after "#" indicates the line of the code that produce the printing; example main(#37) means in function main() at line 37.
+- the "!!" indicate that the printing is on std::cerr. Others are on std::cout.
+*/
diff --git a/doc/tutorial-install-archlinux-package.doc b/doc/tutorial/package/tutorial-install-archlinux-package.doc
similarity index 100%
rename from doc/tutorial-install-archlinux-package.doc
rename to doc/tutorial/package/tutorial-install-archlinux-package.doc
diff --git a/doc/tutorial-install-osx-homebrew-package.doc b/doc/tutorial/package/tutorial-install-osx-homebrew-package.doc
similarity index 100%
rename from doc/tutorial-install-osx-homebrew-package.doc
rename to doc/tutorial/package/tutorial-install-osx-homebrew-package.doc
diff --git a/doc/tutorial-install-ros-package.doc b/doc/tutorial/package/tutorial-install-ros-package.doc
similarity index 100%
rename from doc/tutorial-install-ros-package.doc
rename to doc/tutorial/package/tutorial-install-ros-package.doc
diff --git a/doc/tutorial/package/tutorial-install-ubuntu-package.doc b/doc/tutorial/package/tutorial-install-ubuntu-package.doc
new file mode 100644
index 0000000..7487cdc
--- /dev/null
+++ b/doc/tutorial/package/tutorial-install-ubuntu-package.doc
@@ -0,0 +1,36 @@
+/**
+
+\page tutorial-install-ubuntu-package Tutorial: Installation from prebuilt packages for Linux Ubuntu/Debian
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from prebuilt official packages for Linux Ubuntu or Debian.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_ubuntu_package_list ViSP official packages
+
+Since Ubuntu 14.04 LTS, ViSP library can be installed from <a href="http://packages.ubuntu.com/search?keywords=visp&searchon=names&suite=trusty§ion=all">existing packages</a> on amd64 or i386 architectures. In Ubuntu 13.10, the packages were only available for amd64 architecture.
+
+To install ViSP library and headers just run:
+
+\code
+$ sudo apt-get install libvisp-dev
+\endcode
+
+\note This package comes build with 3rd parties such as OpenCV, X11, xml2...
+
+To install ViSP images data set useful to run ViSP examples you can run:
+\code
+$ sudo apt-get install visp-images-data
+\endcode
+
+To install ViSP html documentation you can run:
+\code
+$ sudo apt-get install visp-doc
+\endcode
+
+\section install_ubuntu_package_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/started/tutorial-contrib-module.doc b/doc/tutorial/started/tutorial-contrib-module.doc
new file mode 100644
index 0000000..ae851ac
--- /dev/null
+++ b/doc/tutorial/started/tutorial-contrib-module.doc
@@ -0,0 +1,209 @@
+/**
+
+\page tutorial-contrib-module Tutorial: How to extend ViSP creating a new contrib module
+\tableofcontents
+
+\section contrib_intro Introduction
+
+ViSP implements a <a href="https://visp.inria.fr/software-architecture/">modular architecture</a> where each module:
+- corresponds to a library (.so, .a, .lib, .dll) prefixed with <b>\c visp_</b>
+- has headers that are grouped in a single folder
+- has dependencies with other modules and at least with the core module
+- may have dependencies with 3rd parties (OpenCV…)
+
+The following image illustrates this modular architecture, where for example the <b>\c tt_mi</b> (template tracker with mutual information) module depends on the <b>\c tt</b> (template tracker) module which depends on the <b>\c vision</b> (computer vision) module that depends at the end on the <b>\c core</b> module. Note that the <b>\c gui</b> (graphical user interface) module and the <b>\c io</b> (input/output) modules are optional.
+
+\image html img-contrib-visp-module.jpg ViSP modular architecture
+
+In this tutorial you will learn how to extend ViSP architecture introducing new contrib modules.
+
+\note We assume here that you are familiar with an \ref tuto_install_src and that you succeed in creating a first project that uses ViSP by following \ref tutorial-getting-started.
+
+
+\section contrib_model Contribution model
+
+
+User may contribute to ViSP creating new modules. The following image shows the introduction of two new modules called <b> \c contrib_1</b> and <b>\c contrib_2</b>. They depend on ViSP existing modules.
+
+\image html img-contrib-module.jpg ViSP architecture extended with new \c contrib_1 and \c contrib_2 modules
+
+\subsection contrib_model_src What about the source tree
+
+Even if it is possible to introduce these new contrib modules in ViSP source code tree, we suggest to separate them in different folders. It will ease ViSP upgrade to future releases avoiding mixing all the source code. In that case, contributions are nothing more than new modules apart from ViSP.
+
+\image html img-contrib-module-tree.jpg Source tree organization; on the left a folder (\c visp) that contains ViSP source code, on the right one (\c visp_contrib) or more other separate folders for the contrib modules
+
+A typical source tree is the following:
+
+\code
+$ cd workspace
+$ ls
+visp visp_contrib
+\endcode
+
+In our previous example, in \c visp_contrib folder we may have the following tree:
+\code
+└── visp_contrib
+ └── modules
+ ├── contrib_1
+ │ └── ...
+ └── contrib_2
+ └── ...
+\endcode
+
+\subsection contrib_model_build What about the build tree
+
+Even if the source code is located in separate folders, ViSP build mechanism allows to build ViSP and the contrib modules together in a single build tree.
+Once build contrib modules will be part of ViSP; two libraries named <b>\c visp_contrib_1</b> and <b>\c visp_contrib_2</b> will be created near ViSP libraries (<b>\c visp_core</b>, …)
+
+If we come back to our small example, building ViSP with our contrib modules is done with the following command:
+\code
+$ cd workspace
+$ mkdir visp_contrib-build; cd visp_contrib-build
+$ cmake -DVISP_CONTRIB_MODULES_PATH=../visp_contrib/modules ../visp
+\endcode
+
+
+\section contrib_create_module Creating a new contrib module
+
+In ViSP source code you will find a python script in \c script/create_module.py that allows to create a new module from scratch. The structure of the module created by the script is the following:
+
+\code
+<root directory>
+|-- <parent name>
+ |-- modules
+ |-- <module name>
+ |-- CMakeLists.txt
+ |-- include
+ | |-- visp3
+ | |-- <module name>
+ | |-- <class name>.h
+ |-- src
+ |-- <class name>.cpp
+\endcode
+
+To know how to use this script, enter in ViSP source tree and run:
+\code
+$ python script/create_module.py --help
+\endcode
+
+The following instructions allow to create a new module named <b>\c contrib</b> from scratch in a parent folder named <b>\c visp_contrib</b>. In this module we will introduce <b>\c vpContrib.h</b> and <b>\c vpContrib.cpp</b> files that correspond to <b>\c vpContrib</b> class implementation and the file <b>\c test-vpContrib.cpp</b> corresponding to a test that calls \c vpContrib constructor. There is also a <b>\c CMakeLists.txt</b> file that allows to build the module and the test.
+
+- first we have to get ViSP source code
+\code
+$ cd workspace
+$ git clone https://github.com/lagadic/visp
+$ ls
+visp
+\endcode
+- then we can create the new module using the script
+\code
+$ python visp/script/create_module.py --parent-name=visp_contrib --module-name=contrib --class-name=vpContrib
+$ ls
+visp visp_contrib
+\endcode
+
+The content of the \c visp_contrib folder is the following:
+
+\code
+visp_contrib
+└── modules
+ └── contrib
+ ├── CMakeLists.txt
+ ├── include
+ │ └── visp3
+ │ └── contrib
+ │ └── vpContrib.h
+ ├── src
+ │ └── vpContrib.cpp
+ └── test
+ └── test-vpContrib.cpp
+\endcode
+
+\section contrib_build_module Building a new contrib module
+
+Now we are ready to build this new module.
+
+\subsection contrib_build_module_unix On a unix-like platform
+
+- create a new folder to host the build and enter in it
+\code
+$ cd workspace
+$ mkdir visp_contrib-build
+$ ls
+visp visp_contrib visp_contrib-build
+$ cd visp_contrib-build
+\endcode
+- now configure the build
+\code
+$ cmake -DVISP_CONTRIB_MODULES_PATH=../visp_contrib/modules ../visp
+\endcode
+- finally build the module using
+\code
+$ make -j4 visp_contrib
+\endcode
+- you can also build all the modules, tests and examples as usual by
+\code
+$ make -j4
+\endcode
+- Run the test:
+\code
+$ ./modules/contrib/test-vpContrib
+I’m in my first contrib module
+\endcode
+
+\subsection contrib_build_module_win On a windows-like platform
+
+We consider here that the workspace folder is \c C:/ViSP and that in this folder we have:
+\code
+$ dir C:\ViSP
+visp visp_contrib
+\endcode
+
+- create a new folder to host the build and enter in it
+\code
+$ cd C:\ViSP
+$ mkdir visp_contrib-build
+$ dir
+visp visp_contrib visp_contrib-build
+$ cd visp_contrib-build
+\endcode
+
+- start \c cmake-gui
+\code
+$ cmake-gui ../visp
+\endcode
+\image html img-contrib-module-1.png
+
+- press "Configure" button and specify the generator for this project. Here we are using Visual Studio 14 2015 Win64
+\image html img-contrib-module-2.png
+
+- press "Finish" button
+\image html img-contrib-module-3.png
+
+- scroll CMake variables list until appearing \c VISP_CONTRIB_MODULES_PATH var
+\image html img-contrib-module-4.png
+
+- set this var to \c C:/ViSP/visp_contrib
+\image html img-contrib-module-5.png
+
+- press "Configure" button. A new module named \c visp_contrib is appearing
+\image html img-contrib-module-6.png
+
+- press "Generate" button.
+\image html img-contrib-module-7.png
+
+- This ends the CMake configuration stage. Start now Visual Studio and open the \c C:/ViSP/visp_contrib-build/VISP.sln solution file. As described in \ref tutorial-install-win10-msvc14 you have now to build ViSP from source.
+
+
+\section contrib_advanced Advanced contrib module
+
+We provide here after the link to existing contrib modules that use advanced functionalities:
+- https://github.com/lagadic/visp_contrib : contains a module name <b>imgproc</b> that implements various image processing algorithms. In this module you will also find the way to introduce tests
+- https://github.com/lagadic/ustk : a ViSP extension for ultrasound images called UsTK for Ultrasound Toolkit. This extension contains multiple modules. The \c modules/CMakeLists.txt file allows here to detect optional 3rd parties. It allows also to generate a separate doxygen documentation that contains only the classes part or UsTK. Tutorials and tests are also considered in UsTK.
+
+\section contrib_next Next tutorial
+
+You are now ready to see the \ref tutorial-image-display.
+
+*/
diff --git a/doc/tutorial/started/tutorial-getting-started-naoqi.doc b/doc/tutorial/started/tutorial-getting-started-naoqi.doc
new file mode 100644
index 0000000..db6d4a5
--- /dev/null
+++ b/doc/tutorial/started/tutorial-getting-started-naoqi.doc
@@ -0,0 +1,88 @@
+
+/**
+ \page tutorial-getting-started-naoqi Tutorial: How to create an application that uses ViSP on NAOqi OS
+ \tableofcontents
+
+We assume in this tutorial that you have successfully cross-compiled ViSP following \ref tutorial-install-crosscompiling-naoqi.
+The aim of this tutorial is to explain how to create an application that uses ViSP cross-build for NAOqi OS as third-party and how to run this application on Nao, Romeo or Pepper robots.
+
+To illustrate this tutorial, we will consider all the examples that are provided in the tutorial dedicated to images (ie. the one located in \c visp/tutorial/image folder). If you are not familiar with ViSP read first \ref tutorial-getting-started.
+
+This tutorial was tested on an Ubuntu 14.04 LTS host computer with:
+- Cross Toolchain 2.3.1 Linux 64 (\c ctc-linux32-atom-2.3.1.23) on Romeo robot
+- Cross Toolchain 2.4.3 Linux 64 (\c ctc-linux64-atom-2.4.3.28) on Pepper robot
+
+
+\section started_naoqi_code Create a workspace for your project
+
+We assume that as described in \ref tutorial-install-crosscompiling-naoqi, your workspace dedicated to visp contains the following folders:
+\code
+$ ls $HOME/soft
+visp
+visp-build-ctc-linux64-atom-2.4.3.28
+\endcode
+
+Create a new folder to host the project you want to run on NAOqi OS
+\code
+$ mkdir $HOME/soft/tutorial
+\endcode
+
+Get existing tutorial/image folder from ViSP source code
+
+\code
+$ cd $HOME/soft/tutorial
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image
+\endcode
+
+\section started_naoqi_build Cross-build your project
+
+Create first a build folder associated to your Cross Toolchain like:
+\code
+$ cd $HOME/soft/tutorial
+$ mkdir image-build-ctc-linux64-atom-2.4.3.28
+\endcode
+
+Configure your project setting \c VISP_DIR and \c CMAKE_TOOLCHAIN_FILE respectively to ViSP and Cross Toolchain location:
+\code
+$ cd image-build-ctc-linux64-atom-2.4.3.28
+$ cmake ../image -DVISP_DIR=$HOME/soft/visp-build-ctc-linux64-atom-2.4.3.28/install/lib/cmake/visp -DCMAKE_TOOLCHAIN_FILE=$HOME/softbank/ctc-linux64-atom-2.4.3.28/toolchain.cmake
+\endcode
+
+Cross-build your project:
+\code
+$ make -j4
+\endcode
+
+\section started_naoqi_install Install the binaries on NAOqi
+
+First create an archive for the binaries:
+\code
+$ tar cvzf binary.tar.gz --exclude "*Make*" --exclude "cmake*" *
+\endcode
+
+Then copy the binaries to NAOqi OS:
+\code
+$ scp binary.tar.gz nao@<your_robot_ip>:
+\endcode
+
+Move the binaries to a more friend location like \c $HOME/tutorial:
+\code
+$ ssh nao@<your_robot_ip>
+nao~ $ mkdir tutorial
+nao~ $ tar xvzf binary.tar.gz --directory tutorial
+nao~ $ rm binary.tar.gz
+\endcode
+
+Run the binaries:
+\code
+nao~ $ cd tutorial
+nao~ $ ./tutorial-image-manipulation
+nao~ $ ./tutorial-undistort
+\endcode
+
+You are now ready to create your own project for Nao, Romeo or Pepper robots.
+
+\section naoqi_image_next Next tutorial
+
+You are now ready to see the \ref tutorial-import-visp-into-EclipseIDE.
+*/
diff --git a/doc/tutorial/started/tutorial-getting-started.doc b/doc/tutorial/started/tutorial-getting-started.doc
new file mode 100644
index 0000000..53a3e0f
--- /dev/null
+++ b/doc/tutorial/started/tutorial-getting-started.doc
@@ -0,0 +1,205 @@
+
+/**
+ \page tutorial-getting-started Tutorial: How to create and build a CMake project that uses ViSP on Unix or Windows
+ \tableofcontents
+
+\note We assume in this tutorial that you have successfully installed ViSP either with an \ref tuto_install_sdk or with an \ref tuto_install_src.
+
+In this tutorial you will learn how to use ViSP either on Unix-like systems (including OSX, Fedora, Ubuntu, Debian, ...) or on Windows.
+
+The easiest way of using ViSP in your project is to use <a href="http://www.cmake.org/">CMake</a>. If you are not familiar with CMake, you can check the <a href="http://www.cmake.org/cmake/help/cmake_tutorial.html">tutorial</a>.
+
+Note also that all the material (source code and images) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/image
+\endcode
+
+\section image_code Create a program using ViSP
+Let's start to write our first program to see how to read an image and open a window to display the image. This simple program is provided in tutorial-viewer.cpp example and given below:
+
+\include tutorial-viewer.cpp
+
+Here is the detailed explanation of the source, line by line :
+\snippet tutorial-viewer.cpp Include display
+
+Include all the headers for image viewers. The two first one are for Windows systems. They require that Direct 3D or the \e Graphical \e Device \e Interface (\e GDI) coming with the installation of Visual Studio are available. The third one needs GTK that is cross-platform. The fourth is for unix-like systems and requires that \e libX11 is available. The last one is also cross-platform and requires that OpenCV is available.
+
+\snippet tutorial-viewer.cpp Include io
+Include the header that allows to read/write PGM, PPM, PNG and JPEG images from the disk using vpImageIo class.
+
+\snippet tutorial-viewer.cpp vpImage construction
+Create an instance of a color image where each pixel is coded in RGBa.
+
+\snippet tutorial-viewer.cpp vpImage reading
+The image \c I is initialized by reading an image file from the disk. If the image format is not supported we throw an exception.
+
+\snippet tutorial-viewer.cpp vpDisplay construction
+Create an instance of an image display window for image \c I. The first viewer that is available is used. Here we create the link between the image \c I and the display \c d. Note that an image can only have one display.
+
+\snippet tutorial-viewer.cpp vpDisplay set title
+The title of the display is then set to \c "My image".
+
+\snippet tutorial-viewer.cpp vpDisplay display
+First we display the content of the image \c I, then we flush the display to render the image.
+
+\snippet tutorial-viewer.cpp vpDisplay get click
+Here we handle mouse events. We are waiting for a blocking mouse click to end the program.
+
+\section image_cmake Create a CMake file
+
+Now you have to create your \c CMakeLists.txt file. It should look like this:
+
+\code
+project(tutorial-image)
+
+cmake_minimum_required(VERSION 2.8)
+
+find_package(VISP REQUIRED)
+
+include_directories(${VISP_INCLUDE_DIRS})
+
+add_executable(tutorial-viewer tutorial-viewer.cpp)
+target_link_libraries(tutorial-viewer ${VISP_LIBRARIES})
+\endcode
+
+The \c find_package() CMake command searches for a \c VISPConfig.cmake file that will define the corresponding variables:
+- \c VISP_INCLUDE_DIRS : ViSP and third-party headers location
+- \c VISP_LIBRARIES : ViSP and third-party libraries name and location
+
+Note that the previous \c CMakeLists.txt file can also be:
+\code
+project(tutorial-image)
+
+cmake_minimum_required(VERSION 2.8)
+
+find_package(VISP REQUIRED)
+if(VISP_FOUND)
+ include(${VISP_USE_FILE})
+endif(VISP_FOUND)
+
+add_executable(tutorial-viewer tutorial-viewer.cpp)
+\endcode
+
+where \c VISP_USE_FILE variable is set to the full path to \c VISPUse.cmake file that contains all the CMake material that allow to build your project with ViSP. In other terms, the line
+\code
+ include(${VISP_USE_FILE})
+\endcode
+will include the following lines to your \c CMakeFile.txt
+\code
+ include_directories(${VISP_INCLUDE_DIRS})
+ link_libraries(${VISP_LIBRARIES})
+\endcode
+
+
+\section image_unix On Unix-like systems
+
+\subsection image_unix_config Configure your project
+
+Proceed as with any other project using CMake:
+
+\code
+$ cmake .
+\endcode
+
+By default \c cmake searches \c VISPConfig.cmake file in folders like \c /usr/share or \c /usr/local/share. If ViSP was not installed in \c /usr or \c /usr/local it is possible that you get the following error:
+\code
+CMake Error at CMakeLists.txt:5 (find_package):
+ Could not find module FindVISP.cmake or a configuration file for package
+ VISP.
+
+ Adjust CMAKE_MODULE_PATH to find FindVISP.cmake or set VISP_DIR to the
+ directory containing a CMake configuration file for VISP. The file will
+ have one of the following names:
+
+ VISPConfig.cmake
+ visp-config.cmake
+\endcode
+
+To help \c cmake to find \c VISPConfig.cmake file, just set \c VISP_DIR environment variable and call \c cmake again:
+\code
+$ export VISP_DIR=/home/ViSP-install-folder/lib/<multi-arch-folder>/cmake/visp
+$ cmake .
+\endcode
+or run cmake with the additional VISP_DIR definition
+\code
+$ cmake -DVISP_DIR=/home/ViSP-install-folder/lib/<multi-arch-folder>/cmake/visp .
+\endcode
+
+Depending on the platform \c \<multi-arch-folder\> can be empty (OSX) or for example equal to \c x86_64-linux-gnu on Ubuntu.
+
+\note If you build ViSP from source following one of the tutorials \ref tutorial_install_src, you can avoid the \c make \c install step and set \c VISP_DIR to the build location instead of the install location.
+
+\subsection image_unix_build Generate the executable
+
+Just run:
+\code
+$ make
+\endcode
+
+\subsection image_unix_output Run the executable
+
+By now you should have an executable called \c tutorial-viewer. You just have to run it giving an image location as an argument:
+
+\code
+$ ./tutorial-viewer monkey.ppm
+\endcode
+
+
+Here is a screen shot of the resulting output window :
+
+ \image html img-monkey.png
+
+\section image_win On Windows
+
+We suppose from now, that you have created a folder (let say \c C:/ViSP/ViSP-started) that contains the following files: CMakeLists.txt, tutorial-viewer.cpp and monkey.ppm. These files are also provided in ViSP source code, in \c tutorial/image folder.
+
+
+\subsection image_win_config Configure your project
+
+- Launch CMake (cmake-gui) from Windows "Start" menu. Set the source code location as \c C:/ViSP/ViSP-started. Here we set the build location to the same folder.
+
+\image html img-started-win-cmake-1.jpg
+
+- Press "Configure" button, and select your compiler. In our case we will use Visual Studio 11 Win64. Press then "Finish". The configuration is now under progress and should lead to the following image.
+
+\image html img-started-win-cmake-2.jpg
+
+- Note here that CMake has automatically found the location of ViSP install folder; \c C:/ViSP/ViSP-install.
+
+- Press then "Configure" button to remove the red lines, and then "Generate" button. As presented in the following image, all the red lines should disappear.
+
+\image html img-started-win-cmake-3.jpg
+
+- From now, in \c C:/ViSP/ViSP-started you should have \c tutorial-image.sln Visual Studio solution file.
+
+\subsection image_win_build Generate the executable
+
+- To open the project in Visual Studio C++ just double click on \c C:/ViSP/ViSP-started/tutorial-image.sln solution file.
+
+\image html img-started-win-msvc-1.jpg
+
+- Now to build the solution, enter "BUILD/Build Solution" menu.
+
+\image html img-started-win-msvc-2.jpg
+
+- In \c C:/ViSP/ViSP-started/Debug folder you have now \c tutorial-image.exe executable.
+
+
+\subsection image_win_output Run the executable
+
+- In your "Start" menu click on "Run" and type in \c cmd.exe to run a terminal.
+- Enter in \c C:/ViSP/ViSP-started/Debug folder, and run \c tutorial-viewer.exe with an image location as argument:
+
+\image html img-started-win-run.jpg
+
+
+- Here is a screen shot of the resulting output window :
+
+ \image html img-monkey-win.jpg
+
+\section image_next Next tutorial
+
+You are now ready to see the \ref tutorial-image-display.
+There is also the \ref tutorial-contrib-module that could be useful to understand how to introduce new developments in ViSP.
+*/
diff --git a/doc/tutorial/started/tutorial-import-visp-into-EclipseIDE.doc b/doc/tutorial/started/tutorial-import-visp-into-EclipseIDE.doc
new file mode 100644
index 0000000..9d20747
--- /dev/null
+++ b/doc/tutorial/started/tutorial-import-visp-into-EclipseIDE.doc
@@ -0,0 +1,84 @@
+/**
+ \page tutorial-import-visp-into-EclipseIDE Tutorial: How to import a CMake project that uses ViSP in Eclipse IDE
+ \tableofcontents
+
+\note We assume in this tutorial that you have successfully installed ViSP either with an \ref tuto_install_sdk or with an \ref tuto_install_src.
+
+In this tutorial you will learn how to import a CMake project that uses ViSP into Eclipse IDE.
+
+This tutorial uses CMake to build projects. If you are not familiar with CMake, you can check the <a href="http://www.cmake.org/cmake/help/cmake_tutorial.html">tutorial</a>.
+
+\section image_adjust_eclipse Adjust Eclipse settings
+
+1) You need to download Eclipse IDE <a href="https://eclipse.org/downloads">archive</a>.
+
+2) Extract Eclipse from archive
+\code
+$ tar -zxvf eclipse-***.tar.gz -C {target directory} (For Linux)
+\endcode
+
+3) Create a CMake project or take the project from \ref tutorial-getting-started which is used in this tutorial
+
+4) Open Eclipse
+
+\image html img-visp-into-eclipse-4.png
+
+5) File > New > Makefile Project with Existing Code
+\image html img-visp-into-eclipse-5.png
+
+6) Browse > Select directory to the CMake project > Press "Finish"
+\image html img-visp-into-eclipse-6.png
+
+7) Goto to the Project directory and execute "cmake . " in console in order to \ref image_unix_config
+
+\image html img-visp-into-eclipse-7.png
+
+8) Right click on Project > Press "Properties" > C/C++ Build > Builder Settings
+
+- Make sure that "Build command" contains "make"
+- Make sure that "Build directory" contains path to the folder which contains MakeFile of the project
+\image html img-visp-into-eclipse-8.png
+
+9) Project > Build all
+
+- Make sure that Project builds
+\image html img-visp-into-eclipse-9.png
+
+10) Right click on Project > Run As > Run Configurations > C/C++ Application > "Project" Default
+\image html img-visp-into-eclipse-10.png
+
+11) C/C++ Application > Browse > Browse path to the executable file of your project > Apply
+\image html img-visp-into-eclipse-11.png
+
+12) Now run the Project from Eclipse as usual
+\image html img-visp-into-eclipse-12.png
+
+\section image_unresolved_inclusion Unresolved inclusion
+1) If you see issues such as "Unresolved inclusion"
+You need to perform next steps to solve it
+\image html img-visp-into-eclipse-unr-1.png
+
+2) Right click on Project > Press "Properties"
+\image html img-visp-into-eclipse-unr-2.png
+
+3) C/C++ General > Preprocessor Include Path etc. > Providers > Enable "CDT GCC Built-in Compiler Settings"
+\image html img-visp-into-eclipse-unr-3.png
+
+4) Wait until C++ indexer finished his work
+\image html img-visp-into-eclipse-unr-4.png
+
+\section image_eclipse_arguments Program arguments
+For some projects you need to specified input arguments
+\image html img-visp-into-eclipse-arg-1.png
+
+Right click on Project > Run As > Run Configurations > C/C++ Application > "Project" Default > Arguments
+
+- Write arguments to "Program arguments"
+\image html img-visp-into-eclipse-arg-2.png
+
+\section eclipse_image_next Next tutorial
+
+You are now ready to see the \ref tutorial-image-display
+
+*/
+
diff --git a/doc/tutorial-tracking-blob.doc b/doc/tutorial/tracking/tutorial-tracking-blob.doc
similarity index 100%
rename from doc/tutorial-tracking-blob.doc
rename to doc/tutorial/tracking/tutorial-tracking-blob.doc
diff --git a/doc/tutorial-tracking-keypoint.doc b/doc/tutorial/tracking/tutorial-tracking-keypoint.doc
similarity index 100%
rename from doc/tutorial-tracking-keypoint.doc
rename to doc/tutorial/tracking/tutorial-tracking-keypoint.doc
diff --git a/doc/tutorial/tracking/tutorial-tracking-mb-CAO-editor.doc b/doc/tutorial/tracking/tutorial-tracking-mb-CAO-editor.doc
new file mode 100644
index 0000000..66cc05f
--- /dev/null
+++ b/doc/tutorial/tracking/tutorial-tracking-mb-CAO-editor.doc
@@ -0,0 +1,27 @@
+/**
+
+\page tutorial-tracking-mb-CAO-editor Tutorial: Markerless model-based tracker CAD model editor - GSoC 2017 project
+\tableofcontents
+
+\section mb_GSoC_17_intro Introduction
+
+The ViSP organization was happy to be accepted in the Google Summer of Code (<a href="https://summerofcode.withgoogle.com/archive/2017/organizations/4892929554382848/">GSoC 2017</a>) project for its first participation. This <a href="https://github.com/lagadic/visp/wiki/GSoC_2017#project-2-markerless-model-based-tracker-cad-model-editor">wiki page</a> summarizes the project ideas.
+
+\section mb_GSoC_17_cao_editor ViSP CAO editor
+
+This project is the result of the work done by Vikas Thamizharasan to develop the project Markerless model-based tracker CAD model editor.
+
+It contains useful tools related to the CAD model in .cao format used by the ViSP model-based tracker.
+
+The following tools have been developed for this project:
+
+- a Blender .cao CAD model exporter, see this <a href="https://github.com/lagadic/visp_cao_editor/blob/master/blenderAddons/vispCAOExport/README.md">tutorial</a>
+- a Blender .cao CAD model importer, see this <a href="https://github.com/lagadic/visp_cao_editor/blob/master/blenderAddons/vispCAOImport/README.md">tutorial</a>
+- a QT-based .cao CAD model editor/viewer, see this <a href="https://github.com/lagadic/visp_cao_editor/wiki/GSoC-2017-with-ViSP">description</a>
+
+A more complete description of the project is also available <a href="https://github.com/lagadic/visp_cao_editor/wiki/GSoC-2017-with-ViSP">here</a>.
+
+\section mb_GSoC_17_next Next tutorial
+You are now ready to see the next \ref tutorial-tracking-tt.
+
+*/
diff --git a/doc/tutorial/tracking/tutorial-tracking-mb-generic.doc b/doc/tutorial/tracking/tutorial-tracking-mb-generic.doc
new file mode 100644
index 0000000..16ebd86
--- /dev/null
+++ b/doc/tutorial/tracking/tutorial-tracking-mb-generic.doc
@@ -0,0 +1,388 @@
+/**
+
+\page tutorial-tracking-mb-generic Tutorial: Markerless generic model-based tracking
+\tableofcontents
+
+\section mb_generic_intro Introduction
+
+This tutorial describes how to use the generic model-based tracker. You are advised to have read these tutorials:
+- \ref tutorial-tracking-mb to have an overview of the model-based edge / KLT / hybrid tracker and to know how to model an object to track and how to configure the tracker
+- \ref tutorial-tracking-mb-stereo and specifically the \ref mb_stereo_intro section for some details about advantages and limitations of using multiple cameras and the \ref mb_stereo_moving_cameras section
+
+\subsection mb_generic_overview Overview
+
+The main motivations for the introduction of this new class (vpMbGenericTracker) are:
+- provide a unique class for the user to choose which type of features to use for the tracking (the features can be combined)
+- extend the single view tracking to multiple cameras (similarly to the vpMbEdgeMultiTracker, vpMbKltMultiTracker, vpMbEdgeKltMultiTracker)
+
+In this perspective, this class can be used instead of the following classes:
+- vpMbEdgeTracker
+- vpMbKltTracker
+- vpMbDepthNormalTracker (since ViSP 3.1.0)
+- vpMbDepthDenseTracker (since ViSP 3.1.0)
+- vpMbEdgeKltTracker
+- vpMbEdgeMultiTracker (extension to the multi-view case)
+- vpMbKltMultiTracker (extension to the multi-view case)
+- vpMbEdgeKltMultiTracker (extension to the multi-view case)
+
+Moreover, the features can be combined to improve the tracking robustness (e.g. a stereo tracker with edge + KLT for the left camera and dense depth features for the right camera).
+
+The following video demonstrates the use of the generic tracker with a RGB-D sensor using the following features:
+- edge + KLT features for the color camera
+- depth normal features for the depth sensor
+
+\htmlonly
+<p align="center"><iframe width="640" height="360" src="https://www.youtube.com/embed/4FARYLYzNL8" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+In this video, the same setup is used but with:
+- edge features for the color camera
+- dense depth features for the depth sensor
+
+\htmlonly
+<p align="center"><iframe width="640" height="360" src="https://www.youtube.com/embed/LFej9NF6F1A" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/model-based/stereo-generic
+\endcode
+
+\subsection mb_generic_features Features overview
+
+The basis type of features that can be used with the model-based tracker are:
+- moving-edges feature (line, face, cylinder and circle primitive) \cite Comport06b
+- KLT feature (face and cylinder primitives) \cite Pressigout:2007
+- depth normal feature (face primitives) (since ViSP 3.1.0)
+- dense depth feature (face primitives) (since ViSP 3.1.0)
+
+The moving-edges and KLT features require a RGB camera, more precisely these features operate on grayscale image. The depth normal and dense depth features require a depth map that can be obtained from a RGB-D camera for example.
+
+If you want to use a depth feature, we advise you to choose the dense depth features that is a much more robust method compared to the depth normal feature. Keep in mind also that Kinect-like sensors have a limited depth range (from ~0.8m to ~3.5m).
+
+\note
+It is recommended to install an optimized BLAS library (for instance <a href="http://www.openblas.net/">OpenBLAS</a>) to get better performance with the dense depth feature approach which requires large matrix operations.
+On Ubuntu Xenial, it is the <a href="https://packages.ubuntu.com/xenial/libopenblas-dev">libopenblas-dev</a> package that should be installed.
+To select or switch the BLAS library to use, see <a href="https://wiki.debian.org/DebianScience/LinearAlgebraLibraries">Handle different versions of BLAS and LAPACK</a>:
+\code
+$ update-alternatives --config libblas.so.3
+\endcode
+You should have something similar to this:
+<blockquote>
+There are 3 choices for the alternative libblas.so.3 (providing /usr/lib/libblas.so.3).
+<table>
+ <tr>
+ <th>Selection</th>
+ <th>Path</th>
+ <th>Priority</th>
+ <th>Status</th>
+ </tr>
+ <tr>
+ <td>0</td>
+ <td>/usr/lib/openblas-base/libblas.so.3</td>
+ <td>40</td>
+ <td>auto mode</td>
+ </tr>
+ <tr>
+ <td>1</td>
+ <td>/usr/lib/atlas-base/atlas/libblas.so.3</td>
+ <td>35</td>
+ <td>manual mode</td>
+ </tr>
+ <tr>
+ <td>2</td>
+ <td>/usr/lib/libblas/libblas.so.3</td>
+ <td>10</td>
+ <td>manual mode</td>
+ </tr>
+ <tr>
+ <td>3</td>
+ <td>/usr/lib/openblas-base/libblas.so.3</td>
+ <td>40</td>
+ <td>manual mode</td>
+ </tr>
+</table>
+</blockquote>
+
+\subsection mb_generic_input Input data
+
+For classical features working on grayscale image, you have to use the following data type:
+
+\code
+vpImage<unsigned char> I;
+\endcode
+
+You can convert to a grayscale image if the image acquired is in RGBa data type:
+
+\code
+vpImage<vpRGBa> I_color;
+// Color image acquisition
+vpImage<unsigned char> I;
+vpImageConvert::convert(I_color, I);
+\endcode
+
+For depth features, you need to supply a pointcloud, that means a 2D array where each element contains the X, Y and Z coordinate in the depth sensor frame. The following data type are accepted:
+- a vector of vpColVector: the column vector being a `3x1` (X, Y, Z)
+\code
+std::vector<vpColVector> pointcloud(640*480);
+// Fill the pointcloud
+vpColVector coordinate(3);
+coordinate[0] = X;
+coordinate[1] = Y;
+coordinate[2] = Z;
+pointcloud[0] = coordinate;
+\endcode
+
+- a <a href="http://docs.pointclouds.org/trunk/classpcl_1_1_point_cloud.html">PCL pointcloud</a> smart pointer of <a href="http://docs.pointclouds.org/trunk/structpcl_1_1_point_x_y_z.html">pcl::PointXYZ</a> data type:
+\code
+pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud;
+\endcode
+
+If you have only a depth map, a 2D array where each element is the distance in meter between the depth sensor frame to the object, you will need to compute the 3D coordinates using the depth sensor intrinsic parameters.
+For instance, without taking into account the distortion (see also vpPixelMeterConversion::convertPoint), the conversion is (u and v are the pixel coordinates, u0, v0, px, py are the intrinsic parameters):
+
+\f[
+\left\{\begin{matrix}
+X = \frac{u - u_0}{px} \times Z \\
+Y = \frac{v - v_0}{px} \times Z
+\end{matrix}\right.
+\f]
+
+Here an example of a depth map:
+
+\image html tutorial-tracking-mb-generic-depth-map.png
+
+\section mb_generic_started Getting started
+
+\subsection mb_generic_implementation_detail Implementation detail
+
+\note
+This section is similar to the \ref mb_stereo_implementation_detail section in \ref tutorial-tracking-mb-stereo
+
+Each tracker is stored in a map, the key corresponding to the name of the camera on which the tracker will process. By default, the camera names are set to:
+- "Camera" when the tracker is constructed with one camera.
+- "Camera1" to "CameraN" when the tracker is constructed with N cameras.
+- The default reference camera will be "Camera1" in the multiple cameras case.
+
+\image html img-multi-cameras-config.png Default name convention and reference camera ("Camera1").
+
+To deal with multiple cameras, in the virtual visual servoing control law we concatenate all the interaction matrices and residual vectors and transform them in a single reference camera frame to compute
+the reference camera velocity.
+Thus, we have to know the transformation matrix between each camera and the reference camera.
+
+For example, if the reference camera is "Camera1" (\f$ c_1 \f$), we need the following information:
+\f$ _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$.
+
+\subsection mb_generic_choose_features Declare a model-based tracker of the desired feature type
+
+The following enumeration (vpMbGenericTracker::vpTrackerType) values are available to get the desired model-based tracker:
+- moving-edges feature: vpMbGenericTracker::EDGE_TRACKER
+- KLT feature: vpMbGenericTracker::KLT_TRACKER
+- depth normal feature: vpMbGenericTracker::DEPTH_NORMAL_TRACKER
+- depth dense feature: vpMbGenericTracker::DEPTH_DENSE_TRACKER
+
+The tracker declaration is then:
+\code
+vpMbGenericTracker tracker(vpMbGenericTracker::EDGE_TRACKER);
+\endcode
+
+To combine the features:
+\code
+vpMbGenericTracker tracker(vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER);
+\endcode
+
+To "fully exploit" the capabilities of a RGB-D sensor, you can use for instance:
+
+\code
+std::vector<int> trackerTypes(2);
+trackerTypes[0] = vpMbGenericTracker::EDGE_TRACKER | vpMbGenericTracker::KLT_TRACKER;
+trackerTypes[1] = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
+vpMbGenericTracker tracker(trackerTypes);
+\endcode
+
+This will declare a tracker with edge + KLT features for the color camera and dense depth feature for the depth sensor.
+
+\subsection mb_generic_interface_with_the_code Interfacing with the code
+
+\note
+This section is more or less similar to the \ref mb_stereo_interface_with_the_code section in \ref tutorial-tracking-mb-stereo
+
+Each essential method used to initialize the tracker and process the tracking have three signatures in order to ease the call to the method and according to three working modes:
+- tracking using one camera, the signature remains the same.
+- tracking using two cameras, all the necessary methods accept directly the corresponding parameters for each camera.
+- tracking using multiple cameras, you have to supply the different parameters within a map. The key corresponds to the name of the camera and the value is the parameter.
+
+The following table sums up how to call the different methods based on the camera configuration for the main functions.
+
+<table>
+<caption id="mb_generic_method_example_table">Example of the different method signatures.</caption>
+<tr><th>Method calling example: <th>Monocular case <th>Stereo case <th>Multiple cameras case <th>Remarks
+<tr><td>Construct a model-based edge tracker: <td>vpMbGenericTracker tracker <td>vpMbGenericTracker tracker(2, vpMbGenericTracker::EDGE_TRACKER) <td>vpMbGenericTracker tracker(5, vpMbGenericTracker::EDGE_TRACKER) <td>The default constructor corresponds to a monocular edge tracker.
+<tr><td>Load a configuration file: <td>tracker.loadConfigFile("config.xml") <td>tracker.loadConfigFile("config1.xml", "config2.xml") <td>tracker.loadConfigFile(mapOfConfigFiles) <td>Each tracker can have different parameters (intrinsic parameters, visibility angles, etc.).
+<tr><td>Load a model file: <td>tracker.loadModel("model.cao") <td>tracker.loadModel("model1.cao", "model2.cao") <td>tracker.loadModel(mapOfModelFiles) <td>Different models can be used.
+<tr><td>Get the intrinsic camera parameters: <td>tracker.getCameraParameters(cam) <td>tracker.getCameraParameters(cam1, cam2) <td>tracker.getCameraParameters(mapOfCam) <td>
+<tr><td>Set the transformation matrix between each camera and the reference one: <td> <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>For the reference camera, the identity homogeneous matrix must be used.
+<tr><td>Setting to display the features: <td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>This is a general parameter.
+<tr><td>Initialize the pose by click: <td>tracker.initClick(I, "f_init.init") <td>tracker.initClick(I1, I2, "f_init1.init", "f_init2.init") <td>tracker.initClick(mapOfImg, mapOfInitFiles) <td>Assuming the transformation matrices between the cameras have been set, some init files can be omitted as long as the reference camera has an init file. The corresponding images must be supplied.
+<tr><td>Track the object: <td>tracker.track(I) <td>tracker.track(I1, I2) <td>tracker.track(mapOfImg) <td>See the documentation to track with pointcloud.
+<tr><td>Get the pose: <td>tracker.getPose(cMo) <td>tracker.getPose(c1Mo, c2Mo) <td>tracker.getPose(mapOfPoses) <td>tracker.getPose(cMo) will return the pose for the reference camera in the stereo/multiple cameras configurations.
+<tr><td>Display the model: <td>tracker.display(I, cMo, cam, ...) <td>tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, ...) <td>tracker.display(mapOfImg, mapOfPoses, mapOfCam, ...) <td>
+</table>
+
+\note As the trackers are stored in an alphabetic order internally, you have to match the method parameters with the correct
+tracker position in the map in the stereo cameras case.
+
+\section mb_generic_example Example code
+
+\subsection mb_generic_mono_example_code Monocular model-based tracking
+The following example comes from tutorial-mb-generic-tracker-stereo-mono.cpp and allows to track a tea box modeled in cao format.
+
+Once built, to choose which tracker to use, run the binary with the following argument:
+\code
+$ ./tutorial-mb-generic-tracker-stereo-mono --name <video name> --tracker <0=egde|1=klt|2=hybrid>
+\endcode
+The config, model, init files should be named according to the video name: teabox.mpg / teabox.cao / teabox.xml / teabox.init
+
+\warning
+ViSP must have been built with OpenCV and the KLT module must be enabled to run this tutorial. The xml2 library is needed if you want to use a configuration file, otherwise you will have to configure the tracker in the code.
+
+\note
+The command line:
+\code
+$ ./tutorial-mb-generic-tracker-stereo-mono
+\endcode
+will run the model-based edge tracker on the teabox sequence.
+
+The source code is the following:
+\include tutorial-mb-generic-tracker-stereo-mono.cpp
+
+\subsection mb_generic_mono_explanation_of_the_code Explanation of the code (monocular model-based tracking)
+
+The previous source code shows how to use the new vpMbGenericTracker class as a replacement of the classical classes. The procedure to configure the tracker remains the same:
+- construct the tracker
+- configure the tracker by loading a configuration file
+- load a 3D model
+- track on the current image
+- get the current pose and display the model in the image
+
+Please refer to the tutorial \ref tutorial-tracking-mb in order to have explanations about the configuration parameters (\ref mb_settings) and how to model an object in a ViSP compatible format (\ref mb_advanced_cao).
+
+The required include is:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Include
+
+We need a grayscale image for the tracking:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Image
+
+To set the type of the tracker (the first parameter is the number of cameras, the second parameter is the type of the tracker(s)):
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Constructor
+
+To load the configuration file, we use:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Load config file
+
+Otherwise you can parameter the tracker directly in the code:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Set parameters
+
+To load the 3D object model, we use:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Load cao
+
+We can also use the following setting that enables the display of the features used during the tracking:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Set display features
+
+The initial pose is set by clicking on specific points in the image:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Init
+
+The tracking is done by:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Track
+
+To get the current camera pose:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Get pose
+
+To display the model with the estimated pose:
+
+\snippet tutorial-mb-generic-tracker-stereo-mono.cpp Display
+
+\subsection mb_generic_stereo_example_code Stereo model-based tracking
+The following example comes from tutorial-mb-generic-tracker-stereo.cpp and allows to track a tea box modeled in cao format using a stereo rig.
+
+Once built, to choose which tracker to use, run the binary with the following argument:
+\code
+$ ./tutorial-mb-generic-tracker-stereo --name <video name left> <video name right>] --tracker <0=egde|1=klt|2=hybrid> <0=egde|1=klt|2=hybrid>
+\endcode
+The config, model, init files should be named according to the video name: teabox_left.mpg / teabox_left.cao / teabox_left.xml / teabox_left.init, ...
+
+\warning
+ViSP must have been built with OpenCV and the KLT module must be enabled to run this tutorial. The xml2 library is **required** in this tutorial code.
+
+\note
+The command line:
+\code
+$ ./tutorial-mb-generic-tracker-stereo
+\endcode
+will run the stereo model-based edge tracker on the stereo teabox sequence.
+
+\subsection mb_generic_stereo_explanation_of_the_code Explanation of the code (stereo model-based tracking)
+
+The procedure is almost identical to the monocular case.
+
+The include is:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Include
+
+Two images are needed:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Images
+
+To set the types of the tracker:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Constructor
+
+To load the configuration files, we use:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Load config file
+
+We have to set the transformation matrices between the cameras and the reference camera to be able to compute the control law in a reference camera frame. In the code we consider the left camera with the name "Camera1" as the reference camera.
+For the right camera with the name "Camera2" we have to set the transformation (\f$ ^{c_{right}}{\bf M}_{c_{left}} \f$). This transformation is read from cRightMcLeft.txt file. Since our left and right cameras are not moving, this transformation is constant and does not need to be updated in the tracking loop:
+
+\note For the reference camera, the camera transformation matrix has to be specified as an identity homogeneous matrix (no rotation, no translation). By default the vpHomogeneousMatrix constructor builds an identity matrix.
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Set camera transformation matrix
+
+To load the 3D object model, we use:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Load cao
+
+We can also use the following setting that enables the display of the features used during the tracking:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Set display features
+
+The initial poses are set by clicking on specific points in the image:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Init
+
+The tracking is done by:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Track
+
+To get the current camera poses:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Get pose
+
+To display the model with the estimated poses:
+
+\snippet tutorial-mb-generic-tracker-stereo.cpp Display
+
+\section mb_generic_next Next tutorial
+You are now ready to see the next \ref tutorial-tracking-mb-CAO-editor.
+
+*/
diff --git a/doc/tutorial/tracking/tutorial-tracking-mb-stereo.doc b/doc/tutorial/tracking/tutorial-tracking-mb-stereo.doc
new file mode 100644
index 0000000..9ce71c5
--- /dev/null
+++ b/doc/tutorial/tracking/tutorial-tracking-mb-stereo.doc
@@ -0,0 +1,227 @@
+/**
+
+\page tutorial-tracking-mb-stereo Tutorial: Markerless model-based tracking with stereo cameras
+\tableofcontents
+
+\section mb_stereo_intro Introduction
+
+\warning
+This tutorial can be considered obsolete since ViSP 3.1.0 version as we have introduced a generic tracker (vpMbGenericTracker) that can replace the vpMbEdgeMultiTracker, vpMbKltMultiTracker and vpMbEdgeKltMultiTracker classes. The explanations about the multi-views model-based tracking remain valid though.
+
+This tutorial describes the model-based tracking of object using simultaneously multiple cameras views. It allows to track the object in the images viewed by a set of cameras while providing its 3D localization. Calibrated cameras (intrinsic and extrinsic between the reference and the other cameras) are required.
+
+The mbt ViSP module allows the tracking of a markerless object using the knowledge of its CAD model. Considered objects have to be modeled by segment, circle or cylinder primitives. The model of the object could be defined in vrml format (except for circles) or in cao (our own format).
+
+Next section highlights the different versions of the markerless multi-view model-based trackers that have been developed. The multi-view model-based tracker can consider moving-edges (thanks to the vpMbEdgeMultiTracker class). It can also consider KLT features that are detected and tracked on each visible face of the model (thanks to the vpMbKltMultiTracker class). The tracker can also handle moving-edges and KLT features in a hybrid scheme (thanks to vpMbEdgeKltMultiTracker the class).
+
+While the multi-view model-based edges tracker implemented in vpMbEdgeMultiTracker is appropriate to track texture-less objects (with visible edges), the multi-view model-based KLT tracker implemented in vpMbKltMultiTracker is suitable for textured objects. The multi-view model-based hybrid tracker implemented in vpMbEdgeKltMultiTracker is appropriate to track objects with texture and or with visible edges.
+
+These classes allow the tracking of the same object assuming two or more cameras:
+The main advantages of this configuration with respect to the mono-camera case (see \ref tutorial-tracking-mb) concern:
+- the possibility to extend the application field of view;
+- a more robust tracking as the configuration of the stereo rig allows to track the object under multiple viewpoints and thus with more visual features.
+
+In order to achieve this, the following information are required:
+- the intrinsic parameters of each camera;
+- the transformation matrix between each camera and a reference camera: \f$ ^{c_{current}}{\bf M}_{c_{reference}} \f$.
+
+In the following sections, we consider the tracking of a tea box modeled in cao format. A stereo camera sees this object. The following video shows the tracking performed with vpMbEdgeMultiTracker. In this example ,the fixed cameras located on the Romeo Humanoid robot head captured the images.
+
+\htmlonly
+<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/aWXvvkeytp0" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+This other video shows the behavior of the hybrid tracking performed with vpMbEdgeKltMultiTracker.
+
+\htmlonly
+<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/0poP7gONdMc" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+\note The cameras can move, but the tracking will be effective as long as the transformation matrix between the cameras and the reference camera is known and updated at each iteration.
+\note The new introduced classes are not restricted to stereo configuration. They allow the usage of multiple cameras (see \ref mb_stereo_moving_cameras).
+
+Next sections will highlight how to easily adapt your code to use multiple cameras with the model-based tracker. As only the new methods dedicated to multiple views tracking will be presented,
+you are highly recommended to follow \ref tutorial-tracking-mb in order to be familiar with the model-based tracking concepts, the different trackers that are available in ViSP (the edge tracker: vpMbEdgeTracker, the klt feature points tracker: vpMbKltTracker
+and the hybrid tracker: vpMbEdgeKltTracker) and with the configuration part.
+
+Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/model-based/stereo
+\endcode
+
+\section mb_stereo_started Getting started
+
+\subsection mb_stereo_overview Overview
+
+The model-based trackers available for multiple views tracking rely on the same trackers than in the monocular case:
+- a vpMbEdgeMultiTracker similar to vpMbEdgeTracker which tracks moving-edges corresponding to the visible lines of the model projected in the image plane at the current pose (suitable for texture-less objects).
+- a vpMbKltMultiTracker similar to vpMbKltTracker which uses the optical flow information to track the object (suitable for textured objects).
+- a vpMbEdgeKltMultiTracker similar to vpMbEdgeKltTracker which merges the two information (edge and texture information) for better robustness of the tracking (can deal with both types of objects).
+
+The following class diagram offers an overview of the hierarchy between the different classes:
+
+\image html img-mbt-multi-class-diagram-resize.jpeg Simplified class diagram.
+
+The vpMbEdgeMultiTracker class inherits from the vpMbEdgeTracker class, the vpMbKltMultiTracker inherits from the vpMbKltTracker class and the vpMbEdgeKltMultiTracker class inherits from the
+vpMbEdgeMultiTracker and vpMbKltMultiTracker classes.
+This conception permits to easily extend the usage of the model-based tracker to multiple cameras with the guarantee to preserve the same behavior compared to the tracking in the monocular configuration
+(more precisely, only the model-based edge and the model-based klt should have the same behavior, the hybrid multi class has a slight different implementation that will lead to minor
+differences compared to vpMbEdgeKltTracker).
+
+As you will see after, the principal methods present in the parent class are accessible and used for single view tracking.
+Lot of new overridden methods have been introduced to deal with the different cameras configuration (single camera, stereo cameras and multiple cameras).
+
+\subsection mb_stereo_implementation_detail Implementation detail
+
+Each tracker is stored in a map, the key corresponding to the name of the camera on which the tracker will process. By default, the camera names are set to:
+- "Camera" when the tracker is constructed with one camera.
+- "Camera1" to "CameraN" when the tracker is constructed with N cameras.
+- The default reference camera will be "Camera1" in the multiple cameras case.
+
+\image html img-multi-cameras-config.png Default name convention and reference camera ("Camera1").
+
+To deal with multiple cameras, in the virtual visual servoing control law we concatenate all the interaction matrices and residual vectors and transform them in a single reference camera frame to compute
+the reference camera velocity.
+Thus, we have to know the transformation matrix between each camera and the reference camera.
+
+For example, if the reference camera is "Camera1" (\f$ c_1 \f$), we need the following information:
+\f$ _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$.
+
+\subsection mb_stereo_interface_with_the_code Interfacing with the code
+
+Each essential method used to initialize the tracker and process the tracking have three signatures in order to ease the call to the method and according to three working modes:
+- tracking using one camera, the signature remains the same than the previous classes (vpMbEdgeTracker, vpMbKltTracker, vpMbEdgeKltTracker).
+- tracking using two cameras, all the necessary methods accept directly the corresponding parameter for each camera. By default, the first parameter corresponds to the reference camera.
+- tracking using multiple cameras, you have to supply the different parameters with a map. The key corresponds to the name of the camera and the value to the parameter.
+
+The following table sums up how to call the different methods based on the camera configuration for the main functions.
+
+<table>
+<caption id="mb_stereo_method_example_table">Example of the different method signatures.</caption>
+<tr><th>Method calling example: <th>Monocular case <th>Stereo case <th>Multiple cameras case <th>Remarks
+<tr><td>Construct a model-based edge tracker: <td>vpMbEdgeMultiTracker tracker <td>vpMbEdgeMultiTracker tracker(2) <td>vpMbEdgeMultiTracker tracker(5) <td>The default constructor corresponds to the monocular configuration.
+<tr><td>Load a configuration file: <td>tracker.loadConfigFile("config.xml") <td>tracker.loadConfigFile("config1.xml", "config2.xml") <td>tracker.loadConfigFile(mapOfConfigFiles) <td>Each tracker can have different parameters (intrinsic parameters, visibility angles, etc.).
+<tr><td>Load a model file: <td>tracker.loadModel("model.cao") <td>tracker.loadModel("model.cao") <td>tracker.loadModel("model.cao") <td>All the trackers must used the same 3D model.
+<tr><td>Get the intrinsic camera parameters: <td>tracker.getCameraParameters(cam) <td>tracker.getCameraParameters(cam1, cam2) <td>tracker.getCameraParameters(mapOfCam) <td>
+<tr><td>Set the transformation matrix between each camera and the reference one: <td> <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>tracker.setCameraTransformationMatrix(mapOfCamTrans) <td>For the reference camera, the identity homogeneous matrix must be set.
+<tr><td>Setting to display the features:<td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>tracker.setDisplayFeatures(true) <td>This is a general parameter.
+<tr><td>Initialize the pose by click: <td>tracker.initClick(I, "f_init.init") <td>tracker.initClick(I1, I2, "f_init1.init", "f_init2.init") <td>tracker.initClick(mapOfImg, mapOfInitFiles) <td>If the transformation matrices between the cameras have been set, some init files can be omitted as long as the reference camera has an init file.
+<tr><td>Track the object: <td>tracker.track(I) <td>tracker.track(I1, I2) <td>tracker.track(mapOfImg) <td>
+<tr><td>Get the pose: <td>tracker.getPose(cMo) <td>tracker.getPose(c1Mo, c2Mo) <td>tracker.getPose(mapOfPoses) <td>tracker.getPose(cMo) will return the pose for the reference camera in the multiple cameras configurations.
+<tr><td>Display the model: <td>tracker.display(I, cMo, cam, ...) <td>tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, ...) <td>tracker.display(mapOfImg, mapOfPoses, mapOfCam) <td>
+</table>
+
+\note As the trackers are stored in an alphabetic order internally, you have to match the method parameters with the correct tracker position in the map in the stereo cameras case.
+
+\subsection mb_stereo_example_code Example code
+The following example comes from tutorial-mb-tracker-stereo.cpp and allows to track a tea box modeled in cao format using one of the three multi-view markerless trackers implemented in ViSP. In this example we consider a stereo configuration.
+
+Once built, to choose which tracker to use, run the binary with the following argument:
+\code
+$ ./tutorial-mb-tracker-stereo --tracker <0=egde|1=klt|2=hybrid>
+\endcode
+
+The source code is the following:
+\include tutorial-mb-tracker-stereo.cpp
+
+\subsection mb_stereo_explanation_of_the_code Explanation of the code
+
+The previous source code shows how to use a model-based tracking on stereo images using the standard procedure to configure the tracker:
+- construct the tracker
+- initialize the tracker by loading a configuration file
+- load a 3D model
+- process the tracking
+- get the pose and display the model in the image
+
+\warning The xml2 library, used to load the configuration file, is required to build the tutorial example. OpenCV is also required and the KLT module has to be enabled to use the KLT functionality.
+
+Please refer to the tutorial \ref tutorial-tracking-mb in order to have the explanations about the configuration parameters (\ref mb_settings) and how to model an object in a ViSP compatible format (\ref mb_advanced_cao).
+
+To test the three kind of trackers, only vpMbEdgeKltMultiTracker.h header is required as the others (vpMbEdgeMultiTracker.h and vpMbKltMultiTracker.h) are already included in the hybrid header class.
+
+\snippet tutorial-mb-tracker-stereo.cpp Include
+
+We declare two images for the left and right camera views.
+
+\snippet tutorial-mb-tracker-stereo.cpp Image
+
+To construct a stereo tracker, we have to specify the desired number of cameras (in our case 2) as argument given to the tracker constructors:
+
+\snippet tutorial-mb-tracker-stereo.cpp Constructor
+
+\note We used a pointer to vpMbTracker to be able to construct a tracker according to the desired type (edge, klt or hybrid) but you could directly declare the desired tracker class in your program.
+
+All the configuration parameters for the tracker are stored in xml configuration files. To load the different files, we use:
+
+\snippet tutorial-mb-tracker-stereo.cpp Load config file
+
+\note The dynamic cast is necessary to access to the specific methods that are not declared in vpMbTracker.
+
+The following code is used in order to retrieve the intrinsic camera parameters:
+
+\snippet tutorial-mb-tracker-stereo.cpp Get camera parameters
+
+To load the 3D object model, we use:
+
+\snippet tutorial-mb-tracker-stereo.cpp Load cao
+
+We can also use the following setting that enables the display of the features used during the tracking:
+
+\snippet tutorial-mb-tracker-stereo.cpp Set display features
+
+We have to set the transformation matrices between the cameras and the reference camera to be able to compute the control law in a reference camera frame. In the code we consider the left camera with the name "Camera1" as the reference camera. For the right camera with the name "Camera2" we have to set the transformation (\f$ ^{c_{right}}{\bf M}_{c_{left}} \f$).
+This transformation is read from cRightMcLeft.txt file. Since our left and right cameras are not moving, this transformation is constant and has not to be updated in the tracking loop:
+
+\note For the reference camera, the camera transformation matrix has to be specified as an identity homogeneous matrix (no rotation, no translation). By default the vpHomogeneousMatrix constructor builds an identity matrix.
+
+\snippet tutorial-mb-tracker-stereo.cpp Set camera transformation matrix
+
+The initial pose is set by clicking on specific points in the image:
+
+\snippet tutorial-mb-tracker-stereo.cpp Init
+
+The poses for the left and right views have to be declared:
+
+\snippet tutorial-mb-tracker-stereo.cpp cMo
+
+The tracking is done by:
+
+\snippet tutorial-mb-tracker-stereo.cpp Track
+
+The poses for each camera are retrieved with:
+
+\snippet tutorial-mb-tracker-stereo.cpp Get pose
+
+To display the model with the estimated pose, we use:
+
+\snippet tutorial-mb-tracker-stereo.cpp Display
+
+Finally, do not forget to delete the pointers:
+
+\snippet tutorial-mb-tracker-stereo.cpp Cleanup
+
+\section mb_stereo_advanced Advanced
+
+\subsection mb_stereo_moving_cameras How to deal with moving cameras
+
+The principle remains the same than with static cameras. You have to supply the camera transformation matrices to the tracker each time the cameras move and before calling the track method:
+\code
+mapOfCamTrans["Camera1"] = vpHomogeneousMatrix(); //The Camera1 is the reference camera.
+mapOfCamTrans["Camera2"] = get_c2Mc1(); //Get the new transformation between the two cameras.
+tracker.setCameraTransformationMatrix(mapOfCamTrans);
+tracker.track(mapOfImg);
+\endcode
+
+This information can be available through the robot kinematics or using different kind of sensors.
+
+The following video shows the stereo hybrid model-based tracking based on object edges and KLT features located on visible faces. The result of the tracking is then used to servo the Romeo humanoid robot eyes to gaze toward the object. The images were captured by cameras located in the Romeo eyes.
+
+\htmlonly
+<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/IguBxf4VRUc" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+\section mb_stereo_next Next tutorial
+You are now ready to see the next \ref tutorial-tracking-mb-generic.
+
+*/
diff --git a/doc/tutorial/tracking/tutorial-tracking-mb.doc b/doc/tutorial/tracking/tutorial-tracking-mb.doc
new file mode 100644
index 0000000..2543672
--- /dev/null
+++ b/doc/tutorial/tracking/tutorial-tracking-mb.doc
@@ -0,0 +1,928 @@
+/**
+
+\page tutorial-tracking-mb Tutorial: Markerless model-based tracking
+\tableofcontents
+
+\section mb_intro Introduction
+
+ViSP allows simultaneously the tracking of a markerless object using the knowledge of its CAD model while providing its 3D localization (i.e., the object pose expressed in the camera frame) when a calibrated camera is used \cite Comport06b. Considered objects should be modeled by lines, circles or cylinders. The CAD model of the object could be defined in vrml format (except for circles), or in cao format.
+
+To follow this tutorial depending on your interest you should be sure that ViSP was build with the following third-parties:
+- \c OpenCV: Useful if you want to investigate the KLT keypoint tracker implemented in vpMbKltTracker or its hybrid version vpMbEdgeKltTracker. We recommend to install \c OpenCV. This 3rd party may be also useful to consider input videos (mpeg, png, jpeg...).
+- \c libxml: This 3rd party is optional but recommended to read the tracker settings as well as the camera settings from an xml file. More information is given in \ref mb_settings_xml.
+- \c Ogre \c 3D: This 3rd party is optional and could be difficult to instal on OSX and Windows. To begin with the tracker we don't recomend to istall it. \c Ogre \c 3D allows to enable \ref mb_settings_visibility_ogre.
+- \c Coin \c 3D: This 3rd party is also optional and difficult to install. That's why we don't recommend to install \c Coin \c 3D to begin with the tracker. \c Coin \c 3D allows only to consider \ref mb_advanced_wrml instead of the home-made \ref mb_advanced_cao.
+
+Next sections highlight how to use the differents versions of the markerless model-based trackers that are implemented in ViSP.
+
+Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/model-based/generic
+\endcode
+
+\section mb_started Getting started
+
+In ViSP, depending on the visual features that are used three trackers are available:
+- a tracker implemented in vpMbEdgeTracker that consider moving-edges behind the visible lines of the model. This tracker is appropriate to track texture less objects.
+- an other tracker implemented in vpMbKltTracker that consider KLT keypoints that are detected and tracked on each visible face of the model. This tracker is more designed to track textured objects with edges that are not really visible.
+- an hybrid version implemented in vpMbEdgeKltTracker that is able to consider moving-edges and KLT keypoints. This tracker is appropriate to track textured objects with visible edges.
+
+\subsection mb_started_src Example source code
+The following example that comes from tutorial-mb-tracker.cpp allows to track a tea box modeled in cao format using one of the markerless model-based tracker implemented in ViSP.
+
+\include tutorial-mb-tracker.cpp
+
+\note An extension of the previous getting started example is proposed in tutorial-mb-tracker-full.cpp where advanced functionalities such as reading tracker settings from an XML file or visibility computation are implemented.
+
+\note Other tutorials that are specific to a given tracker are provided in tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp.
+
+\subsection mb_started_input Example input data
+
+The previous example uses the following data as input:
+- a video file; "teabox.mpg" is the default video.
+- a cad model that describes the object to track. In our case the file \c "teabox.cao" is the default one. See \ref mb_model section to learn how the teabox is modelled and section \ref mb_advanced_cao to learn how to model an other object.
+- a file with extension "*.init" that contains the 3D coordinates of some points used to compute an initial pose which serves to initialize the tracker. The user has than to click in the image on the corresponding 2D points. The default file is named "teabox.init". The content of this file is detailed in \ref mb_started_src_explained section.
+- an optional image with extension "*.ppm" that may help the user to remember the location of the corresponding 3D points specified in "*.init" file.
+
+
+\subsection mb_started_exe Running the example
+
+Once build, to see the options that are available in the previous source code, just run:
+\code
+$ ./tutorial-mb-tracker --help
+Usage: ./tutorial-mb-tracker-full [--video <video name>] [--model <model name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]
+\endcode
+By default, \c teabox.mpg video and \c teabox.cao model are used as input. Using \c "--tracker" option, you can specify which tracker has to be used:
+- Using vpMbEdgeTracker to track only moving-edges:
+\code
+$ ./tutorial-mb-tracker --tracker 0
+\endcode
+will produce results similar to:
+\htmlonly
+<br>
+<p align="center"><iframe width="420" height="315" src="http://www.youtube.com/embed/b__u_yGEbmc?rel=0" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+- Using vpMbKltTracker to track only keypoints:
+\code
+$ ./tutorial-mb-tracker --tracker 1
+\endcode
+will produce results similar to:
+\htmlonly
+<br>
+<p align="center"><iframe width="420" height="315" src="http://www.youtube.com/embed/eZmUw9r6Idw?rel=0" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+- Using vpMbEdgeKltTracker to track moving-edges and keypoints in an hybrid scheme:
+\code
+$ ./tutorial-mb-tracker --tracker 2
+\endcode
+will produce results similar to:
+\htmlonly
+<br>
+<p align="center"><iframe width="420" height="315" src="http://www.youtube.com/embed/a-RX9NPF2k0?rel=0" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+With this example it is also possible to work on an other data set using "--video" and "--model" command line options. For example, if you run:
+\code
+$ ./tutorial-mb-tracker --video <path1>/myvideo%04.png --model <path2>/myobject.cao.
+\endcode
+it means that the following images will be used as input:
+\code
+<path1>/myvideo0001.png
+<path1>/myvideo0002.png
+...
+<path1>/myvideo0009.png
+<path1>/myvideo0010.png
+...
+\endcode
+and that in \c \<path2\> you have the following data:
+- \c myobject.init: The coordinates of at least four 3D points used for the initialization.
+- \c myobject.cao: The CAD model of the object to track.
+- \c myobject.ppm: An optional image that shows where the user has to click the points defined in \c myobject.init.
+- \c myobject.xml: An optional files that contains the tracker parameters that are specific to the image sequence and that contains also the camera intrinsic parameters obtained by calibration (see \ref tutorial-calibration). This file is handled in tutorial-mb-tracker-full.cpp but not in tutorial-mb-tracker.cpp. That's why since the video \c teabox.mpg was acquired by an other camera than yours, you have to set the camera intrinsic parameters in tutorial-mb-tracker.cpp source code modif [...]
+\snippet tutorial-mb-tracker.cpp Set camera parameters
+
+and build again before using \c "--model" command line option.
+
+\subsection mb_started_src_explained Source code explained
+
+Hereafter is the description of the some lines introduced in the previous example.
+
+First we include the header of the hybrid tracker that includes internally vpMbEdgeTracker and vpMbKltTracker classes.
+\snippet tutorial-mb-tracker.cpp Include
+
+The tracker uses image \c I and the intrinsic camera parameters \c cam as input.
+\snippet tutorial-mb-tracker.cpp Image
+
+As output, it estimates \c cMo, the pose of the object in the camera frame.
+\snippet tutorial-mb-tracker.cpp cMo
+
+Once input image \c teabox.pgm is loaded in \c I, a window is created and initialized with image \c I. Then we create an instance of the tracker depending on \c "--tracker" command line option.
+\snippet tutorial-mb-tracker.cpp Constructor
+
+Then the corresponding tracker settings are initialized. More details are given in \ref mb_settings section.
+\snippet tutorial-mb-tracker.cpp Set parameters
+
+Now we are ready to load the cad model of the object. ViSP supports cad model in cao format or in vrml format. The cao format is a particular format only supported by ViSP. It doesn't require an additional 3rd party rather then vrml format that require Coin 3rd party. We load the cad model in cao format from \c teabox.cao file which complete description is provided in \ref mb_teabox_cao with:
+\snippet tutorial-mb-tracker.cpp Load cao
+
+It is also possible to modify the code to load the cad model in vrml format from \c teabox.wrl file described in \ref mb_teabox_vrml. To this end modify the previous line with:
+\code
+tracker->loadModel(objectname + ".wrl");
+\endcode
+
+Once the model of the object to track is loaded, with the next line the display in the image window of additional drawings in overlay such as the moving edges positions, is then enabled by:
+\snippet tutorial-mb-tracker.cpp Set display
+
+Now we have to initialize the tracker. With the next line we choose to use a user interaction.
+\snippet tutorial-mb-tracker.cpp Init
+
+The user has to click in the image on four vertices with their 3D coordinates defined in the \c "teabox.init" file. The following image shows where the user has to click.
+
+\image html img-teabox-click.jpg Image \c "teabox.ppm" used to help the user to initialize the tracker.
+
+Matched 2D and 3D coordinates are then used to compute an initial pose used to initialize the tracker. Note also that the third optional argument "true" is used here to enable the display of an image that may help the user for the initialization. The name of this image is the same as the \c "*.init" file except the extension that sould be \c ".ppm". In our case it will be \c "teabox.ppm".
+
+The content of \c teabox.init file that defines 3D coordinates of some points of the model used during user intialization is provided hereafter. Note that all the characters after character '#' are considered as comments.
+
+\includelineno teabox.init
+
+We give now the signification of each line of this file:
+- line 1: Number of 3D points that are defined in this file. At least 4 points are required. Four is the minimal number of points requested to compute a pose.
+- line 2: Each point is defined by its 3D coordinates. Here we define the first point with coordinates (0,0,0). In the previous figure it corresponds to vertex 0 of the tea box. This point is also the origin of the frame in which all the points are defined.
+- line 3: 3D coordinates of vertex 3.
+- line 4: 3D coordinates of vertex 2.
+- line 5: 3D coordinates of vertex 5.
+
+Here the user has to click on vertex 0, 3, 2 and 5 in the window that displays image \c I. From the 3D coordinates defined in \c teabox.init and the corresponding 2D coordinates of the vertices obtained by user interaction a pose is computed that is then used to initialize the tracker.
+
+Next, in the infinite while loop, after displaying the next image, we track the object on a new image \c I.
+\snippet tutorial-mb-tracker.cpp Track
+
+The result of the tracking is a pose \c cMo that can be obtained by:
+\snippet tutorial-mb-tracker.cpp Get pose
+
+Next lines are used first to retrieve the camera parameters used by the tracker, then to display the visible part of the cad model using red lines with 2 as thickness, and finally to display the object frame at the estimated position \c cMo. Each axis of the frame are 0.025 meters long. Using vpColor::none indicates that x-axis is displayed in red, y-axis in green, while z-axis in blue. The thickness of the axis is 3.
+\snippet tutorial-mb-tracker.cpp Display
+
+The last lines are here to free the memory allocated for the display and tracker:
+\snippet tutorial-mb-tracker.cpp Cleanup
+
+\section mb_model Teabox CAD model
+
+ViSP supports two different ways to describe CAD models, either in cao or in vrml format.
+- cao format is specific to ViSP. It allows to describe the CAD model of an object using a text file with extension \c .cao.
+- vrml format is supported only if Coin 3rd party is installed. This format allows to describe the CAD model of an object using a text file with extension \c .wrl.
+
+
+\subsection mb_teabox_cao teabox.cao example
+
+The content of the file teabox.cao used in the getting started \ref mb_started_src but also in tutorial-mb-edge-tracker.cpp and in tutorial-mb-hybrid-tracker.cpp examples is given here:
+
+\includelineno teabox.cao
+
+This file describes the model of the tea box corresponding to the next image:
+
+\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in cao format.
+
+We make the choice to describe the faces of the box from the 3D points that correspond to the vertices. We provide now a line by line description of the file. Notice that the characters after the '#' are considered as comments.
+- line 1: Header of the \c .cao file
+- line 3: The model is defined by 8 3D points. Here the 8 points correspond to the 8 vertices of the tea box presented in the previous figure. Thus, next 8 lines define the 3D points coordinates.
+- line 4: 3D point with coordinate (0,0,0) corresponding to vertex 0 of the tea box. This point is also the origin of the frame in which all the 3D points are defined.
+- line 5: 3D point with coordinate (0,0,-0.08) corresponding to vertex 1.
+- line 6 to 11: The other 3D points corresponding to vertices 2 to 7 respectively.
+- line 13: Number of 3D lines defined from two 3D points. It is possible to introduce 3D lines and then use these lines to define faces from these 3D lines. This is particularly useful to define faces from non-closed polygons. For instance, it can be used to specify the tracking of only 3 edges of a rectangle. Notice also that a 3D line that doesn't belong to a face is always visible and consequently always tracked.
+- line 15: Number of faces defined from 3D lines. In our teabox example we decide to define all the faces from 3D points, that is why this value is set to 0.
+- line 17: The number of faces defined by a set of 3D points. Here our teabox has 6 faces. Thus, next 6 lines describe each face from the 3D points defined previously line 4 to 11. Notice here that all the faces defined from 3D points corresponds to closed polygons.
+- line 18: First face defined by 4 3D points, respectively vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
+- line 19: Second face also defined by 4 points, respectively vertices 1,6,5,2 to have a counter clockwise orientation.
+- line 20 to 23: The four other faces of the box.
+- line 25: Number of 3D cylinders describing the model. Since we model a simple box, the number of cylinders is 0.
+- line 27: Number of 3D circles describing the model. For the same reason, the number of circles is 0.
+
+\subsection mb_teabox_cao_triangle teabox-triangle.cao example
+
+The content of the file teabox-triangle.cao used in the tutorial-mb-klt-tracker.cpp example is given here:
+
+\includelineno teabox-triangle.cao
+
+This file describes the model of the tea box corresponding to the next image:
+
+\image html img-teabox-cao-triangle.jpg Index of the vertices used to model the tea box in cao format with triangles.
+
+Until line 15, the content of this file is similar to the one described in
+\ref mb_teabox_cao. Line 17 we specify that the model contains 12 faces. Each face is then described as a triangle.
+
+\note Since some lines of the model (for example the one between points 0 and 2, or 7 and 3...) don't correspond to teabox edges, this CAD model is not suited for moving-edges and hybrid trackers.
+
+\subsection mb_teabox_vrml teabox.wrl example
+
+The content of the \c teabox.wrl file used in tutorial-mb-tracker-full.cpp and tutorial-mb-edge-tracker.cpp when \c teabox.cao is missing is given hereafter. This content is to make into relation with \c teabox.cao described in \ref mb_teabox_cao. As for the cao format, \c teabox.wrl describes first the vertices of the box, then the edges that corresponds to the faces.
+
+\includelineno teabox.wrl
+
+This file describes the model of the tea box corresponding to the next image:
+
+\image html img-teabox-cao.jpg Index of the vertices used to model the tea box in vrml format.
+
+We provide now a line by line description of the file where the faces of the box are defined from the vertices:
+- line 1 to 10: Header of the \c .wrl file.
+- line 13 to 20: 3D coordinates of the 8 tea box vertices.
+- line 34 to 29: Each line describe a face. In this example, a face is defined by 4 vertices. For example, the first face join vertices 0,1,2,3. The orientation of the face is counter clockwise by going from vertex 0 to vertex 1, then 2 and 3. This fixes the orientation of the normal of the face going outside the object.
+
+
+\section mb_settings Tracker settings
+
+\subsection mb_settings_xml Settings from an XML file
+
+
+Instead of setting the tracker parameters from source code, if libxml2 is available it is possible to specify the settings from an XML file. As done in tutorial-mb-tracker-full.cpp example, to read the parameters from an XML file, simply modify the code like:
+\snippet tutorial-mb-tracker-full.cpp Load xml
+The content of the XML file teabox.xml that is considered by default is the following:
+\code
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>8</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ <camera>
+ <u0>325.66776</u0>
+ <v0>243.69727</v0>
+ <px>839.21470</px>
+ <py>839.44555</py>
+ </camera>
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ <near_clipping>0.1</near_clipping>
+ <far_clipping>100</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+</conf>
+\endcode
+
+Depending on the tracker all the XML tags are not useful:
+- \c \<ecm\> tag corresponds to the moving-edges settings. This tag is used by vpMbEdgeTracker and vpMbEdgeKltTracker.
+- \c \<klt\> tag corresponds to the keypoint settings. This tag is used by vpMbKltTracker and vpMbEdgeKltTracker.
+- \c \<camera\> and \c \<face\> tags are used by all the trackers.
+
+\subsection mb_settings_ecm Moving-edges settings
+Moving-edges settings affect the way the visible edges of an object are tracked.
+These settings could be tuned either from XML using \<ecm\> tag as:
+
+\code
+<conf>
+ ...
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>8</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ ...
+</conf>
+\endcode
+
+of from source code using vpMbEdgeTracker::setMovingEdge() method:
+\snippet tutorial-mb-tracker-full.cpp Set moving-edges parameters
+
+Either from xml or from the previous source code you can set:
+- mask_size: defines the size of the convolution mask used to detect an edge.
+- nb_mask: number of mask applied to determine the object contour. The number of mask determines the precision of the normal of the edge for every sample. If precision is 2deg, then there are 360/2 = 180 masks.
+- range_tracking: range on both sides of the reference pixel along the normal of the contour used to track a moving-edge. If the displacement of the tracked object in two successive images is large, you have to increase this parameter.
+- edge_threshold: likelihood threshold used to determined if the moving edge is valid or not.
+- mu1: minimum image contrast allowed to detect a contour.
+- mu2: maximum image contrast allowed to detect a contour.
+- sample_step: minimum distance in pixel between two discretized moving-edges. To increase the number of moving-edges you have to reduce this parameter.
+
+\note Most important parameters are \e range_tracking and \e sample_step.
+
+
+\subsection mb_settings_klt Keypoints settings
+Keypoint settings affect tracking of keypoints on visible faces using KLT.
+These settings could be tuned either from XML using \<klt\> tag as:
+
+\code
+<conf>
+ ...
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ ...
+</conf>
+\endcode
+
+of from source code using vpMbKltTracker::setKltOpencv() and vpMbKltTracker::setMaskBorder() methods:
+\snippet tutorial-mb-tracker-full.cpp Set klt parameters
+
+With the previous parameters you can set:
+- mask_border: erosion number of the mask used on each face.
+- max_features: maximum number of keypoint features to track in the image.
+- window_size: window size used to refine the corner locations.
+- quality: parameter characterizing the minimal accepted quality of image corners. Corners with quality measure less than this parameter are rejected. This means that if you want to have more keypoints on a face, you have to reduce this parameter.
+- min_distance: minimal Euclidean distance between detected corners during keypoint detection stage used to initialize keypoint location.
+- harris: free parameter of the Harris detector.
+- size_block: size of the averaging block used to track the keypoint features.
+- pyramid_lvl: maximal pyramid level. If the level is zero, then no pyramid is computed for the optical flow.
+
+\note Most important parameters are \e min_distance and \e quality.
+
+\subsection mb_settings_cam Camera settings
+Camera settings correspond to the intrinsic camera parameters without distorsion. If images are acquired by a camera that has a large field of view that introduce distorsion, images need to be undistorded before processed by the tracker. The camera parameters are then the one obtained on undistorded images.
+
+Camera settings could be set from XML using \<camera\> tag as:
+\code
+<conf>
+ ...
+ <camera>
+ <u0>325.66776</u0>
+ <v0>243.69727</v0>
+ <px>839.21470</px>
+ <py>839.44555</py>
+ </camera>
+ ...
+</conf>
+\endcode
+of from source code using vpMbTracker::setCameraParameters() method:
+\snippet tutorial-mb-tracker-full.cpp Set camera parameters
+
+As described in vpCameraParameters class, these parameters correspond to \f$(p_x, p_y)\f$ the ratio between the focal length and the size of a pixel, and \f$(u_0, v_0)\f$ the coordinates of the principal point in pixel.
+
+\note The \ref tutorial-calibration explains how to obtain these parameters from a camera calibration stage.
+
+\subsection mb_settings_visibility Visibility settings
+
+An important setting concerns the visibility test that is used to determine if a face is visible or not. Note that moving-edges and keypoints are only tracked on visible faces. Three different visibility tests are implemented; with or without Ogre ray tracing and with or without scanline rendering. The default test is the one without Ogre and scanline. The functions vpMbTracker::setOgreVisibilityTest() and vpMbTracker::setScanLineVisibilityTest() allow to select which test to use.
+
+\subsubsection mb_settings_visibility_default Default visibility based on normals
+Let us now highlight how the default visibility test works. As illustrated in the following figure, the angle \f$ \alpha \f$ between the normal of the face and the line going from the camera to the center of gravity of the face is used to determine if the face is visible.
+
+\image html img-tracker-mb-visibility.jpg Principle of the visibility test used to determine if a face is visible.
+
+When no advanced visibility test is enable (we recall that this is the default behavior), the algorithm that computes the normal of the face is very simple. It makes the assumption that faces are convex and oriented counter clockwise. If we consider two parameters; the angle to determine if a face is appearing \f$ \alpha_{appear} \f$, and the angle to determine if the face is disappearing \f$ \alpha_{disappear} \f$, a face will be considered as visible if \f$ \alpha \leq \alpha_{disapp [...]
+\code
+<conf>
+ ...
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ </face>
+\endcode
+or in the code:
+\snippet tutorial-mb-tracker-full.cpp Set angles
+
+Here the face is considered as appearing if \f$ \alpha \leq 70\f$ degrees, and disappearing if \f$ \alpha > 80\f$ degrees.
+
+\note When these two angle parameters are not set, their default values set to 89 degrees are used.
+
+\subsubsection mb_settings_visibility_ogre Advanced visibility via Ogre3D
+
+The Ogre3D visibility test approach is based on ray tracing. When this test is enabled, the algorithm used to determine the visibility of a face performs (<b>in addition to the previous test based on normals, i.e on the visible faces resulting from the previous test</b>) another test which sets the faces that are <b>partially occluded as non-visible</b>. It can be enabled via:
+\code
+ tracker->setOgreVisibilityTest(true);
+\endcode
+\image html img-tracker-mb-visibility-ogre.png "Ogre visibility test on both polygons."
+
+When using the <b>classical version of the ogre visibility test</b> (which is the default behavior when activating this test), <b>only one ray</b> is used per polygon to test its visibility. As shown on the figure above, this only ray is sent from the camera to the center of gravity of the considered polygon. If the ray intersects another polygon before the considered one, it is set as non-visible. Intersections are computed <b>between the ray and the axis-aligned bounding-box (AABB)</b> [...]
+
+Additionnaly, it is also possible to use a statistical approach during the ray tracing phase in order to improve the visibility results.
+\code
+ tracker->setNbRayCastingAttemptsForVisibility(4);
+ tracker->setGoodNbRayCastingAttemptsRatio(0.70);
+\endcode
+\image html img-tracker-mb-visibility-ogre-advanced.png "Ogre visibility test on the first polygon, using a statistical approach."
+
+Contrary to the classical version of this test, the <b>statistical approach uses multiple rays per polygons</b> (4 in the example above). Each ray is sent randomly toward the considered polygon. If a specified ratio of rays do not have intersected another polygon before the considered one, the polygon is set as visible. In the example above, three ray on four return the first polygon as visible. As the ratio of good matches is more than 70% (which corresponds to the chosen ratio in this [...]
+
+\note Since ViSP 3.0.0 we have introduced vpMbTracker::setOgreShowConfigDialog() method that allows to open the Ogre configuration pannel which can be used to select the renderer. To enable this feature, use:
+
+\code
+ tracker->setOgreShowConfigDialog(true);
+\endcode
+
+\subsubsection mb_settings_visibility_scanline Advanced visibility via Scanline Rendering
+
+Contrary to the visibility test using Ogre3D, this method <b>does not require any additional third-party library</b>. As for the advanced visibility using Ogre3D, <b>this test is applied in addition to the test based on normals (i.e on the faces set as visible during this test) and also in addition to the test with Ogre3D if it has been activated</b>. This test is based on the scanline rendering algorithm and can be enabled via:
+\code
+ tracker->setScanLineVisibilityTest(true);
+\endcode
+\image html img-tracker-mb-visibility-scanline.png "Scanline visibility test on both polygons."
+
+Even if this approach requires a bit <b>more computing power</b>, it is a <b>pixel perfect visibility test</b>. According to the camera point of view, polygons will be <b>decomposed in order to consider only their visible parts</b>. As a result, if we consider the example above, dashed red lines won't be considered during the tracking phase and detected keypoints will be correctly matched with the closest (in term of depth from the camera position) polygon.
+
+\subsection mb_settings_clipping Clipping settings
+
+Additionally to the visibility test described above, it is also possible to use clipping. Firstly, the algorithm removes the faces that are not visibles, according to the visibility test used, then it will also remove the faces or parts of the faces that are out of the clipping planes. As illustrated in the following figure, different clipping planes can be enabled.
+
+\image html img-fov.png Camera field of view and clipping planes.
+
+Let us consider two plane categories: the ones belonging to the field of view or FOV (Left, Right, Up and Down), and the Near and Far clipping planes. The FOV planes can be enabled by:
+
+\snippet tutorial-mb-tracker-full.cpp Set clipping fov
+
+which is equivalent to:
+
+\code
+ tracker->setClipping(vpMbtPolygon::LEFT_CLIPPING
+ | vpMbtPolygon::RIGHT_CLIPPING
+ | vpMbtPolygon::UP_CLIPPING
+ | vpMbtPolygon::DOWN_CLIPPING);
+\endcode
+
+Of course, if the user just wants to activate Right and Up clipping, he will use:
+
+\code
+ tracker->setClipping(vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING);
+\endcode
+
+For the Near and Far clipping it is quite different. Indeed, those planes require clipping distances. Here there are two choices, either the user uses default values and activate them with:
+
+\code
+ tracker->setClipping(vpMbtPolygon::NEAR_CLIPPING | vpMbtPolygon::FAR_CLIPPING);
+\endcode
+
+or the user can specify the distances in meters, which will automatically activate the clipping for those planes:
+
+\snippet tutorial-mb-tracker-full.cpp Set clipping distance
+
+It is also possible to enable them in the XML file. This is done with the following lines:
+
+\code
+<conf>
+ ...
+ <face>
+ ...
+ <near_clipping>0.1</near_clipping>
+ <far_clipping>100.0</far_clipping>
+ <fov_clipping>0</fov_clipping>
+ </face>
+\endcode
+
+Here for simplicity, the user just has the possibility to either activate all the FOV clipping planes or none of them (fov_clipping requires a boolean).
+
+\note When clipping parameters are not set in the XML file, nor in the code, clipping is not used. Usually clipping is not helpful when the oject to track is simple.
+
+
+\section mb_advanced Advanced
+\subsection mb_advanced_model How to manipulate the model
+
+The following code shows how to access to the CAD model
+- to check if a face is visible,
+- to get the name of the face (only with models in \c .cao format for the moment)
+- to check if the level of detail is enable/disable (only with models in \c .cao format for the moment)
+- to access to the coordinates of the 3D points used to model a face
+- from the pose \e cMo estimated by the tracker to compute the coordinates of the 3D points in the image
+
+\code
+ vpMbHiddenFaces<vpMbtPolygon> &faces = tracker.getFaces();
+ std::cout << "Number of faces: " << faces.size() << std::endl;
+ for (unsigned int i=0; i < faces.size(); i++) {
+ std::vector<vpMbtPolygon*> &poly = faces.getPolygon();
+ std::cout << "face " << i << " with index: " << poly[i]->getIndex()
+ << (poly[i]->getName().empty() ? "" : (" with name: " + poly[i]->getName()))
+ << " is " << (poly[i]->isVisible() ? "visible" : "not visible")
+ << " and has " << poly[i]->getNbPoint() << " points"
+ << " and LOD is " << (poly[i]->useLod ? "enabled" : "disabled") << std::endl;
+
+ for (unsigned int j=0; j<poly[i]->getNbPoint(); j++) {
+ vpPoint P = poly[i]->getPoint(j);
+ P.project(cMo);
+ std::cout << " P obj " << j << ": " << P.get_oX() << " " << P.get_oY() << " " << P.get_oZ() << std::endl;
+ std::cout << " P cam" << j << ": " << P.get_X() << " " << P.get_Y() << " " << P.get_Z() << std::endl;
+
+ vpImagePoint iP;
+ vpMeterPixelConversion::convertPoint(cam, P.get_x(), P.get_y(), iP);
+ std::cout << " iP " << j << ": " << iP.get_u() << " " << iP.get_v() << std::endl;
+ }
+ }
+\endcode
+
+
+\subsection mb_advanced_lod Level of detail (LOD)
+
+The level of detail (LOD) consists in introducing additional constraints to the visibility check to determine if the features of a face have to be tracked or not. Two parameters are used
+- the line length (in pixel)
+- the area of the face (in pixel²), that could be closed or not (you can define an open face by adding all the segments without the last one which closes the face)
+
+The tracker allows to enable/disable the level of detail concept using vpMbTracker::setLod() function.
+This example permits to set LOD settings to all elements :
+\code
+tracker.setLod(true);
+tracker.setMinLineLengthThresh(40.0);
+tracker.setMinPolygonAreaThresh(500.0);
+\endcode
+
+This example permits to set LOD settings to specific elements denominated by his name.
+\code
+tracker.setLod(false);
+tracker.setLod(true, "Left line");
+tracker.setLod(true, "Front face");
+tracker.setMinLineLengthThresh(35.0, "Left line");
+tracker.setMinPolygonAreaThresh(120.0, "Front face");
+\endcode
+
+Furthermore, to set a name to a face see \ref mb_advanced_cao_nam.
+
+
+\subsection mb_advanced_cao CAD model in cao format
+
+\note You may be interested to look at \ref tutorial-tracking-mb-CAO-editor that will present some useful tools to handle more conveniently the custom .cao model file:
+
+- one Blender plugin to export a classical CAD model (Collada, Wavefront, Stl, ...) in the ViSP .cao format
+- one Blender plugin to import the ViSP .cao format into Blender
+- a Qt-based application to edit and view a .cao model file to check if the modeling is correct for instance
+
+\subsubsection mb_advanced_cao_lin How to model faces from lines
+The first thing to do is to declare the differents points. Then you define each segment of the face with the index of the start point and with the index of the end point.
+Finally, you define the face with the index of the segments which constitute the face.
+
+\note The way you declare the face segments (clockwise or counter clockwise) will determine the direction of the normal of the face and so will influe on the visibility of the face.
+
+\code
+V1
+# Left wing model
+6 # Number of points
+# 3D points
+-4 -3.8 0.7
+-6 -8.8 0.2
+-12 -21.7 -1.2
+-9 -21.7 -1.2
+ 0.8 -8.8 0.2
+ 4.6 -3.8 0.7
+# 3D lines
+6 # Number of lines
+0 1 # line 0
+1 2
+2 3
+3 4
+4 5
+5 0 # line 5
+# Faces from 3D lines
+1 # Number of faces defined by lines
+6 0 1 2 3 4 5 # face 0: [number of lines] [index of the lines]...
+# Faces from 3D points
+0
+# 3D cylinders
+0
+# 3D circles
+0
+\endcode
+
+
+\subsubsection mb_advanced_cao_cyl How to model cylinders
+The first thing to do is to declare the two points defining the cylinder axis of revolution. Then you declare the cylinder with the index of the points that define the cylinder axis of revolution and with the cylinder radius.
+
+\note For the level of detail, in a case of a cylinder, this is taking into account by using the length of the axis of revolution to determine the visibility.
+
+\image html img-cylinder.png Example of a cylinder.
+
+\code
+V1
+# Cylinder model
+2 # Number of points
+# 3D points
+16.9 0 0.5 # point 0
+-20 0 0.5 # point 1
+# 3D lines
+0
+# Faces from 3D lines
+0
+# Faces from 3D points
+0
+# 3D cylinders
+1 # Number of cylinders
+0 1 2.4 # cylinder 0: [1st point on revolution axis] [2nd point on revolution axis] [radius]
+# 3D circles
+0
+\endcode
+
+
+\subsubsection mb_advanced_cao_cir How to model circles
+The first thing to do is to declare three points: one point for the center of the circle and two points on the circle plane (i.e. not necessary located on the perimeter of the circle but on the plane of the circle). Then you declare your circle with the radius and with index of the three points.
+
+\note The way you declare the two points on the circle plane (clockwise or counter clockwise) will determine the direction of the normal of the circle and so will influe on the visibility of the circle.
+For the level of detail, in a case of a circle, this is taking into account by using the area of the bounding box of the circle to determine the visibility.
+
+\image html img-circle.png Example of a circle.
+
+\code
+V1
+# Circle model
+3 # Number of points
+# 3D points
+-3.4 14.6 1.1 # point 0
+-3.4 15.4 1.1
+-3.4 14.6 1.8 # point 2
+# 3D lines
+0
+# Faces from 3D lines
+0
+# Faces from 3D points
+0
+# 3D cylinders
+0
+# 3D circles
+1 # Number of circles
+0.8 0 2 1 # circle 0: [radius] [circle center] [1st point on circle plane] [2nd point on circle plane]
+\endcode
+
+
+\subsubsection mb_advanced_cao_hie How to create a hierarchical model
+It could be useful to define a complex model instead of using one big model file with all the declaration with different sub-models, each one representing a specific part of the complex model in a specific model file.
+To create a hierarchical model, the first step is to define all the elementary parts and then regroup them.
+
+\image html img-plane-hierarchical-diagram.jpg Example of a possible hierarchical modelling of a plane.
+
+For example, if we want to have a model of a plane, we could represent as elementary parts the left and right wings, the tailplane (which is constituted of some other parts) and a cylinder for the plane fuselage.
+The following lines represent the top model of the plane.
+
+\code
+V1
+# header
+# load the different parts of the plane
+load("wings.cao") # load the left and right wings
+load("tailplane.cao")
+# 3D points
+2 # Number of points
+16.9 0 0.5
+-20 0 0.5
+# 3D lines
+0
+# Faces from 3D lines
+0
+# Faces from 3D points
+0
+# 3D cylinders
+1 # Number of cylinders
+0 1 2.4 # define the plane fuselage as a cylinder
+# 3D circles
+0
+\endcode
+
+\note The path to include another model can be expressed as an absolute or a relative path (relative to the file which includes the model).
+
+
+\subsubsection mb_advanced_cao_nam How to set a name to a face
+To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
+
+It could be useful to give a name for a face in a model in order to easily modify his LOD parameters for example, or to decide if you want to use this face or not during the tracking phase.
+This is done directly in the model file :
+\code
+V1
+# header
+# load the different parts of the plane
+load("wings.cao")
+load("tailplane.cao")
+# 3D points
+5 # Number of points
+16.9 0 0.5
+-20 0 0.5
+-3.4 14.6 1.1
+-3.4 15.4 1.1
+-3.4 14.6 1.8
+# 3D lines
+0
+# Faces from 3D lines
+0
+# Faces from 3D points
+0
+# 3D cylinders
+1 # Number of cylinders
+0 1 2.4 name=plane_fuselage
+# 3D circles
+1 # Number of circles
+0.8 2 4 3 name="right reactor"
+\endcode
+
+\note If the name contains space characters, it must be surrounded by quotes.
+You can give a name to all the elements excepts for points.
+
+
+\subsubsection mb_advanced_cao_lod How to tune the level of detail
+
+As explained in section \ref mb_advanced_lod the parameters of the lod can be set in the source code. They can also be set directly in the configuration file or in the CAD model in cao format.
+
+The following lines show the content of the configuration file :
+
+\code
+<?xml version="1.0"?>
+<conf>
+ <lod>
+ <use_lod>1</use_lod>
+ <min_line_length_threshold>40</min_line_length_threshold>
+ <min_polygon_area_threshold>150</min_polygon_area_threshold>
+ </lod>
+</conf>
+\endcode
+
+In CAD model file, you can specify the LOD settings to the desired elements :
+
+\code
+V1
+# header
+# load the different parts of the plane
+load("wings.cao")
+load("tailplane.cao")
+# 3D points
+5 # number of points
+16.9 0 0.5
+-20 0 0.5
+-3.4 14.6 1.1
+-3.4 15.4 1.1
+-3.4 14.6 1.8
+# 3D lines
+0
+# Faces from 3D lines
+0
+# Faces from 3D points
+0
+# 3D cylinders
+1 # Number of cylinders
+0 1 2.4 name=plane_fuselage useLod=true minLineLengthThreshold=100.0
+# 3D circles
+1 # Number of circles
+0.8 2 4 3 name="right reactor" useLod=true minPolygonAreaThreshold=40.0
+\endcode
+
+\note The order you call the methods to load the configuration file and to load the CAD model in the code will modify the result of the LOD parameters.
+Basically, the LOD settings expressed in configuration file will have effect on all the elements in the CAD model while the LOD settings expressed in CAD model will be specific to an element.
+The natural order would be to load first the configuration file and after the CAD model.
+
+\subsection mb_advanced_wrml CAD model in wrml format
+\subsubsection mb_advanced_wrml_nam How to set a name to a face
+
+To exploit the name of a face in the code, see sections about \ref mb_advanced_lod and \ref mb_advanced_exclude.
+
+When using a wrml file, names are associated with shapes. In the example below (the model of a teabox), as only one shape is defined, all its faces will have the same name: "teabox_name".
+
+\note If you want to set different names for different faces, you have to define them in different shapes.
+
+\code
+#VRML V2.0 utf8
+
+DEF fst_0 Group {
+children [
+
+# Object "teabox"
+
+DEF teabox_name Shape {
+
+geometry DEF cube IndexedFaceSet {
+
+coord Coordinate {
+point [
+0 0 0 ,
+0 0 -0.08,
+0.165 0 -0.08,
+0.165 0 0 ,
+0.165 0.068 0 ,
+0.165 0.068 -0.08,
+0 0.068 -0.08,
+0 0.068 0 ]
+}
+
+coordIndex [
+ 0,1,2,3,-1,
+ 1,6,5,2,-1,
+ 4,5,6,7,-1,
+ 0,3,4,7,-1,
+ 5,4,3,2,-1,
+ 0,7,6,1,-1]}
+}
+
+]
+}
+\endcode
+
+\subsection mb_advanced_exclude How not to consider specific polygons
+
+When using model-based trackers, it is possible not to consider edge tracking or keypoint tracking for specific faces. To do so, <b>the faces you want to consider must have a name</b>.
+
+If you want to enable (default behavior) or disable the edge tracking on specific face it can be done via:
+
+\code
+vpMbEdgeTracker::setUseEdgeTracking("name of the face", boolean);
+\endcode
+
+If the boolean is set to False, the tracking of the edges of the faces that have the given name will be disable. If it is set to True (default behavior), it will be enable.
+
+As for the edge tracking, the same functionnality is also available when using keypoints via:
+
+\code
+vpMbKltTracker::setUseKltTracking("name of the face", boolean);
+\endcode
+
+\section mb_use_case Use case
+
+Hereafter we provide the information to test the tracker on more complex objects. Data-set could be found browsing http://visp-doc.inria.fr/download/mbt-model/
+
+\subsection mb_use_case_cubesat CubeSAT satellite model
+
+In http://visp-doc.inria.fr/download/mbt-model/cubesat.zip you will find the model data set (.obj, .cao, .init, .xml, .ppm) and a video to test the \c CubeSAT object tracking. After unzip in a folder (let say \c /your-path-to-model) you may run the tracker with something similar to:
+
+\code
+$ ./tutorial-mb-tracker-full --video /your-path-to-model/cubesat/video/00%2d.png --model /your-path-to-model/cubesat/cubesat1b.cao
+\endcode
+
+You should be able to obtain these kind of results:
+
+\htmlonly
+<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/i1zRGhZGpLk" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+\subsection mb_use_case_mmicro mmicro model
+
+In http://visp-doc.inria.fr/download/mbt-model/mmicro.zip you will find the model data set (.cao, .wrl, .init, .xml, .ppm) and a video to track the \c mmicro object. After unzip in a folder (let say \c /your-path-to-model) you may run the tracker with something similar to:
+
+\code
+$ ./tutorial-mb-tracker-full --video /your-path-to-model/mmicro/video/mmicro00%2d.png --model /your-path-to-model/mmicro/mmicro.cao
+\endcode
+
+You should be able to obtain these kind of results:
+
+\htmlonly
+<p align="center"><iframe width="560" height="315" src="https://www.youtube.com/embed/UK10KMMJFCI" frameborder="0" allowfullscreen></iframe></p>
+\endhtmlonly
+
+\section mb_known_issues Known issues
+\subsection mb_known_issues_example_with_ogre Model-based trackers examples are not working with Ogre visibility ckeck
+
+If you run mbtEdgeTracking.cpp, mbtKltTracking.cpp or mbtEdgeKltTracking.cpp examples enabling Ogre visibility check (using "-o" option), you may encounter the following issue that was obtained following \ref tutorial-install-win10-msvc12 :
+\code
+C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
+...
+OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
+...
+*** Initializing OIS ***
+\endcode
+and then a wonderful runtime issue as in the next image:
+\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
+
+It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
+\code
+C:\ViSP\ViSP-3.0.0-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
+\endcode
+Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+
+\subsection mb_known_issues_tutorial_with_ogre Model-based trackers tutorials are not working with Ogre visibility ckeck
+
+This issue is similar to \ref mb_known_issues_example_with_ogre. It may occur with tutorial-mb-edge-tracker.cpp, tutorial-mb-klt-tracker.cpp and tutorial-mb-hybrid-tracker.cpp. To make working the tutorials:
+
+- modify the code like:
+\code
+ tracker.setOgreVisibilityTest(true);
+ tracker.setOgreShowConfigDialog(true);
+\endcode
+- build the modified tutorial
+- run the binary. Now the binary should open the Ogre configuration window where you have to select an Ogre renderer that is working on your computer.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+- Press then OK to continue and start the tracking of the object.
+
+\section mb_next Next tutorial
+You are now ready to see the next \ref tutorial-tracking-mb-stereo if you want to extend the tracking on multiple view images or \ref tutorial-tracking-tt.
+
+
+*/
diff --git a/doc/tutorial-tracking-me.doc b/doc/tutorial/tracking/tutorial-tracking-me.doc
similarity index 100%
rename from doc/tutorial-tracking-me.doc
rename to doc/tutorial/tracking/tutorial-tracking-me.doc
diff --git a/doc/tutorial/tracking/tutorial-tracking-tt.doc b/doc/tutorial/tracking/tutorial-tracking-tt.doc
new file mode 100644
index 0000000..428d9bc
--- /dev/null
+++ b/doc/tutorial/tracking/tutorial-tracking-tt.doc
@@ -0,0 +1,253 @@
+/**
+
+\page tutorial-tracking-tt Tutorial: Template tracking
+\tableofcontents
+
+\section tracking_tt_intro Introduction
+
+With ViSP it is possible to track a template using image registration algorithms \cite Dame10c \cite Dame11c. Contrary to common approaches based on visual features, this method allows to be much more robust to scene variations.
+
+In the following sections, we consider the tracking of a pattern. To simplify the source code, the tracking is performed on a single image. The extension to a sequence of images or to images acquired from a camera is easy. To this end see \ref tutorial-grabber.
+
+Note that all the material (source code and video) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/tracking/template-tracker
+\endcode
+
+
+\section tracking_tt Track the painting
+
+The following example that comes from tutorial-template-tracker.cpp allows to track a template using vpTemplateTrackerSSDInverseCompositional class.
+Let us denote that "SSDInverseCompositional" refers to the similarity function used for the image registration. In ViSP, we have implemented, for now, three different similarity functions: the "Sum of Square Differences" (vpTemplateTrackerSSD classes \cite Baker04a), the "Zero-mean Normalized Cross Correlation" (vpTemplateTrackerZNCC classes \cite Irani98a) and the "Mutual Information" (vpTemplateTrackerMI classes \cite Dame10c). All the methods can be used in different ways: Inverse Com [...]
+
+As detailed in \ref warp_tt section, the tracker is able to track a reference template (in our case a painting) and to estimate the transformation between the reference template and its current position. The estimated transformationn could be modeled as:
+- a pure 2D translation (see vpTemplateTrackerWarpTranslation)
+- a 2D translation with an additional scaling factor (see vpTemplateTrackerWarpSRT)
+- an affine transformation (see vpTemplateTrackerWarpAffine)
+- a homography (see vpTemplateTrackerWarpHomography or vpTemplateTrackerWarpHomographySL3).
+
+\include tutorial-template-tracker.cpp
+
+The video below shows the result of the template tracking.
+
+\htmlonly
+<iframe width="420" height="315" src="http://www.youtube.com/embed/hniUcaUSVBM" frameborder="0" allowfullscreen></iframe>
+\endhtmlonly
+
+Hereafter is the description of the new lines introduced in this example.
+
+\snippet tutorial-template-tracker.cpp Include
+
+Here we include the header of the vpTemplateTrackerSSDInverseCompositional class that allows to track the template. Actually, the tracker estimates the displacement of the template in the current image according to its initial pose. The computed displacement can be represented by multiple transformations, also called warps (vpTemplateTrackerWarp classes). In this example, we include the header vpTemplateTrackerWarpHomography class to define the possible transformation of the template as [...]
+
+\snippet tutorial-template-tracker.cpp Construction
+
+Once the tracker is created with the desired warp function, parameters can be tuned to be more consistent with the expected behavior. Depending on these parameters the perfomances of the tracker in terms of processing time and estimation could be affected. Since here we deal with 640 by 480 pixel wide images, the images are significantly subsampled in order to reduce the time of the image processing to be compatible with real-time.
+
+\code
+ tracker.setSampling(2, 2); // Will consider only one pixel from two along rows and columns
+ // to create the reference template
+ tracker.setLambda(0.001); // Gain used in the optimization loop
+ tracker.setIterationMax(200); // Maximum number of iterations for the optimization loop
+ tracker.setPyramidal(2, 1); // First and last level of the pyramid. Full resolution image is at level 0.
+\endcode
+
+The last step of the initialization is to select the template that will be tracked during the sequence.
+
+\snippet tutorial-template-tracker.cpp Init
+
+The vpTemplateTracker classes proposed in ViSP offer you the possibility to defined your template as multiple planar triangles. When calling the previous line, you will have to specify the triangles that define the template.
+
+\image html img-initClickTemplateTracker.png Initialization of the template without Delaunay triangulation.
+
+Let us denote that those triangles don't have to be spatially tied up. However, if you want to track a simple image as in this example, you should initialize the template as on the figure above. Left clicks on point number zero, one and two create the green triangle. Left clicks on point three and four and then right click on point number five create the red triangle and ends the initialization.
+If ViSP is build with OpenCV, we also provide an initialization with automatic triangulation using Delaunay. To use it, you just have to call vpTemplateTracker::initClick(I, true). Then by left clicking on points number zero, one, two, four and right clicking on point number five initializes the tracker as on the image above.
+
+Next, in the infinite while loop, after displaying the next image, we track the object on a new image I.
+
+\snippet tutorial-template-tracker.cpp Track
+
+If you need to get the parameters of the current transformation of the template, it can be done by calling:
+
+\snippet tutorial-template-tracker.cpp Homography
+
+For further information about the warping parameters, see the following \ref warp_tt section.
+
+Then according to the computed transformation obtained from the last call to track() function, next line is used to display the template using red lines.
+
+\snippet tutorial-template-tracker.cpp Display
+
+\subsection warp_tt Warping classes
+
+In the example presented above, we focused on the vpTemplateTrackerWarpHomography warping class which is the most generic transformation available in ViSP for the template trackers. However, if you know that the template you want to track is constrained, other warps might be more suitable.
+
+\b vpTemplateTrackerWarpTranslation
+
+\f$w({\bf x},{\bf p}) = {\bf x} + {\bf t}\f$ with the following estimated parameters \f$ {\bf p} = (t_x, t_y)\f$
+
+This class is the most simple transformation available for the template trackers. It only considers translation on two-axis (x-axis and y-axis).
+
+\b vpTemplateTrackerWarpSRT
+
+\f$w({\bf x},{\bf p}) = (1+s){\bf Rx} + {\bf t}\f$ with \f${\bf p} = (s, \theta, t_x, t_y)\f$
+
+The SRT warp considers a scale factor, a rotation on z-axis and a 2D translation as in vpTemplateTrackerWarpTranslation.
+
+\b vpTemplateTrackerWarpAffine
+
+
+\f$ w({\bf x},{\bf p}) = {\bf Ax} + {\bf t}\f$ with \f${\bf A} = \left( \begin{array}{cc}
+ 1+a_0 & a_2 \\
+ a_1 & 1+a_3
+ \end{array} \right)\f$, \f${\bf t} = \left( \begin{array}{c}
+ t_x \\
+ t_y
+ \end{array} \right)\f$ and the estimated parameters \f${\bf p} = (a_0 ... a_3, t_x, t_y)\f$
+
+The template transformation can also be defined as an affine transformation. This warping function preserves points, straight lines, and planes.
+
+\b vpTemplateTrackerWarpHomography
+
+\f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f$ {\bf H}=\left( \begin{array}{ccc}
+ 1 + p_0 & p_3 & p_6 \\
+ p_1 & 1+p_4 & p_7 \\
+ p_2 & p_5 & 1.0
+ \end{array} \right) \f$ and the estimated parameters \f${\bf p} = (p_0 ... p_7)\f$
+
+As remind, the vpTemplateTrackerWarpHomography estimates the eight parameters of the homography matrix \f$ {\bf H}\f$.
+
+\b vpTemplateTrackerWarpHomographySL3
+
+\f$w({\bf x},{\bf p}) = {\bf Hx}\f$ with \f${\bf p} = (p_0 ... p_7)\f$
+
+The vpTemplateTrackerWarpHomographySL3 warp works exactly the same as the vpTemplateTrackerWarpHomography warp. The only difference is that here, the parameters of the homography are estimated in the SL3 reference frame.
+
+\subsection tune_tt How to tune the tracker
+
+When you want to obtain a perfect pose estimation, it is often time-consuming. However, by tuning the tracker, you can find a good compromise between speed and efficiency. Basically, what will make the difference is the size of the reference template. The more pixels it contains, the more time-consuming it will be. Fortunately, the solutions to avoid this problem are multiple. First of all lets come back on the vpTemplateTracker::setSampling() function.
+
+\code
+tracker.setSampling(4, 4); // Will consider only one pixel from four along rows and columns
+ // to create the reference template.
+\endcode
+
+In the example above, we decided to consider only one pixel from 16 (4 by 4) to create the reference template. Obviously, by increasing those values it will consider much less pixels, which unfortunately decrease the efficiency, but the tracking phase will be much faster.
+
+The tracking phase relies on an iterative algorithm minimizing a cost function. What does it mean? It means this algorithm has, at some point, to stop! Once again, you have the possibility to reduce the number of iterations of the algorithm by taking the risk to fall in a local minimum.
+
+\code
+tracker.setIterationMax(50); // Maximum number of iterations for the optimization loop.
+\endcode
+
+If this is still not enough for you, let's remember that all of our trackers can be used in a pyramidal way. By reducing the number of levels considered by the algorithm, you will consider, once again, much less pixels and be faster.
+
+\code
+tracker.setPyramidal(3, 2); // First and last level of the pyramid
+\endcode
+Note here that when vpTemplateTracker::setPyramidal() function is not used, the pyramidal approach to speed up the algorithm is not used.
+
+Let us denote that if you're using vpTemplateTrackerSSDInverseCompositional or vpTemplateTrackerZNCCInverseCompositional, you also have another interesting option to speed up your tracking phase.
+
+\code
+tracker.setUseTemplateSelect(true);
+\endcode
+
+This function will force the tracker to only consider, in the reference template, the pixels that have an high gradient value. This is another solution to limit the number of considered pixels.
+
+As well as vpTemplateTrackerSSDInverseCompositional::setUseTemplateSelect() or vpTemplateTrackerMIInverseCompositional::setUseTemplateSelect(), another function, only available in vpTemplateTrackerSSDInverseCompositional and vpTemplateTrackerZNCCInverseCompositional is:
+
+\code
+tracker.setThresholdRMS(1e-6);
+\endcode
+
+By increasing this root mean square threshold value, the algorithm will reduce its number of iterations which should also speed up the tracking phase. This function should be used wisely with the vpTemplateTracker::setIterationMax() function.
+
+\subsubsection tuning_tt_mi Example tuning MI tracker
+
+In tutorial-template-tracker.cpp we use vpTemplateTrackerSSDInverseCompositional class that implements the "Sum of Square Differences" as similarity function. To use Mutual Information that is more robust to occlusion and lighting changes, the code needs to be modified, first introducing vpTemplateTrackerMIInverseCompositional header, then instanciating the tracker, and finally setting the paramaters to speed up the MI tracker that is slower than the SSD one:
+
+- **Adding vpTemplateTrackerMIInverseCompositional header** consists in adding the following line at the beginning of the tutorial-template-tracker.cpp example
+\code
+#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
+\endcode
+
+- **Instantiating the MI tracker** consists in replacing the following line
+\code
+ vpTemplateTrackerSSDInverseCompositional tracker(&warp);
+\endcode
+with the following:
+\code
+ vpTemplateTrackerMIInverseCompositional tracker(&warp);
+\endcode
+
+- **Speeding up the MI tracker** could be performed increasing the sampling step to 3 along the rows and the colums and increasing the root mean square threshold value. The following parameters should speed up the tracker:
+\code
+ tracker.setSampling(3, 3);
+ tracker.setLambda(0.001);
+ tracker.setIterationMax(200);
+ tracker.setPyramidal(2, 1);
+ tracker.setThresholdRMS(1e-6);
+\endcode
+
+\subsection points_tt How to get the points of the template
+
+The previous code provided in tutorial-template-tracker.cpp can be modified to get the coordinates of the corners of the triangles that define the zone to track. To this end, as shown in the next lines, before the while loop we first define a reference zone and the corresponding warped zone. Then in the loop, we update the warped zone using the parameters of the warping model that is estimated by the tracker. From the warped zone, we extract all the triangles, and then for each triangles [...]
+
+\code
+ // Instantiate and get the reference zone
+ vpTemplateTrackerZone zone_ref = tracker.getZoneRef();
+ // Instantiate a warped zone
+ vpTemplateTrackerZone zone_warped;
+
+ while(!g.end()){
+ g.acquire(I);
+ vpDisplay::display(I);
+ tracker.track(I);
+
+ tracker.display(I, vpColor::red);
+
+ // Get the estimated parameters
+ vpColVector p = tracker.getp();
+
+ // Update the warped zone given the tracker estimated parameters
+ warp.warpZone(zone_ref, p, zone_warped);
+
+ // Parse all the triangles that describe the zone
+ for (int i=0; i < zone_warped.getNbTriangle(); i++) {
+ vpTemplateTrackerTriangle triangle;
+ // Get a triangle
+ zone_warped.getTriangle(i, triangle);
+ std::vector<vpImagePoint> corners;
+ // Get the 3 triangle corners
+ triangle.getCorners( corners );
+
+ // From here, each corner triangle is available in
+ // corners[0], corners[1] and corners[2]
+
+ // Display a green cross over each corner
+ for(unsigned int j=0; j<corners.size(); j++)
+ vpDisplay::displayCross(I, corners[j], 15, vpColor::green, 2);
+ }
+ vpDisplay::displayRectangle(I, zone_warped.getBoundingBox(), vpColor::orange);
+\endcode
+
+With the last line, we also sho how to get and display an orange rectangle that corresponds to the bounding box of all the triangles that define the zone.
+
+The resulting drawings introduced previously are shown in the next image. Here we initialize the tracker with 2 triangles that are not connex.
+
+\image html img-template-tracker.jpg
+
+
+\section tracking_tt_example More examples
+
+The templateTracker.cpp source code provided in the example/tracking folder allows to test all the template tracker classes that derive from vpTemplateTracker as well as all the warping classes that derive from vpTemplateTrackerWarp.
+
+Once build, in a terminal just run:
+
+\code
+./templateTracker -h
+\endcode
+
+to see which are the command lines options.
+*/
diff --git a/doc/tutorial/tutorial.doc b/doc/tutorial/tutorial.doc
new file mode 100644
index 0000000..d94b031
--- /dev/null
+++ b/doc/tutorial/tutorial.doc
@@ -0,0 +1,182 @@
+/*! \page tutorial_mainpage Tutorials
+This page references all the tutorials.
+
+- \subpage tutorial_ios
+
+- \subpage tutorial_install_pkg
+
+- \subpage tutorial_install_src
+
+- \subpage tutorial_install_crosscompiling
+
+- \subpage tutorial_started
+
+- \subpage tutorial_image
+
+- \subpage tutorial_imgproc
+
+- \subpage tutorial_calib
+
+- \subpage tutorial_tracking
+
+- \subpage tutorial_detection
+
+- \subpage tutorial_computer_vision
+
+- \subpage tutorial_vs
+
+- \subpage tutorial_bridge
+
+- \subpage tutorial_tools
+
+*/
+
+/*! \page tutorial_ios ViSP iOS
+
+This page introduces the user to the way to exploit ViSP with iOS devices.
+
+- \subpage tutorial-install-ios-package <br>In this tutorial you will learn how to install ViSP prebuilt framework for iOS devices.
+- \subpage tutorial-install-iOS <br>In this tutorial you will learn how to install ViSP from source on OSX for iOS project.
+- \subpage tutorial-getting-started-iOS <br>This tutorial shows how to build a project that uses ViSP on iOS devices.
+- \subpage tutorial-image-ios <br>This tutorial shows how to do simple image processing on iOS devices.
+- \subpage tutorial-detection-apriltag-ios <br>This tutorial shows how to do detct an AprilTag marker on iOS devices.
+
+*/
+
+/*! \page tutorial_install_pkg Installation from packages
+This page introduces the user to the way to install ViSP from existing prebuild packages.
+
+- \subpage tutorial-install-ubuntu-package <br>In this first tutorial you will learn how to install ViSP prebuilt library SDK from Ubuntu or Debian packages.
+- \subpage tutorial-install-archlinux-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for Arch Linux.
+- \subpage tutorial-install-osx-homebrew-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK on OSX with Homebrew.
+- \subpage tutorial-install-ros-package <br>In this tutorial you will learn how to install ViSP prebuilt SDK for ROS.
+
+*/
+
+/*! \page tutorial_install_src Installation from source code
+This page introduces the user to the way to install ViSP from source code.
+
+- \subpage tutorial-install-ubuntu <br>In this tutorial you will learn how to install ViSP from source on Linux Ubuntu.
+- \subpage tutorial-install-fedora <br>In this other tutorial you will learn how to install ViSP from source on Linux Fedora.
+- \subpage tutorial-install-centos <br>In this other tutorial you will learn how to install ViSP from source on Linux CentOS.
+- \subpage tutorial-install-opensuse <br>In this other tutorial you will learn how to install ViSP from source on Linux openSUSE.
+- \subpage tutorial-install-raspberry <br>In this tutorial you will learn how to install ViSP from source on Raspberry Pi.
+- \subpage tutorial-install-win7 <br>In this tutorial you will learn how to install ViSP from source on Windows 7 with Visual C++ 2012 (vc11).
+- \subpage tutorial-install-win10-msvc12 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2013 (vc12).
+- \subpage tutorial-install-win10-msvc14 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2015 (vc14).
+- \subpage tutorial-install-win10-msvc15 <br>In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++ 2017 (vc15).
+- \subpage tutorial-install-win10-mingw64 <br>In this tutorial you will learn how to install ViSP from source on Windows 8.1 or Windows 10 with Mingw-w64.
+- \subpage tutorial-install-win10-msvc14-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2015 (vc14).
+- \subpage tutorial-install-win10-msvc15-uwp <br>In this tutorial you will learn how to install ViSP from source on Windows 10 for Universal Windows Platform apps with Visual C++ 2017 (vc15).
+- \subpage tutorial-install-osx-homebrew <br>In this tutorial you will learn how to install ViSP from source on OSX with Homebrew.
+
+*/
+
+/*! \page tutorial_install_crosscompiling Installation from cross-compiling
+This page introduces the user to the way to cross-compile ViSP from source code.
+
+- \subpage tutorial-install-crosscompiling-raspberry <br>In this tutorial you will learn how to cross-compile ViSP on Ubuntu host for Raspberry Pi target.
+- \subpage tutorial-install-crosscompiling-naoqi <br>In this tutorial you will learn how to cross-compile ViSP on Ubuntu host for NAOqi targets (NAO, Romeo, Pepper).
+
+*/
+/*! \page tutorial_started Getting started
+This page introduces the user to the way to start with ViSP.
+
+- \subpage tutorial-getting-started <br>This tutorial shows how to build a project that uses ViSP to read and display an image.
+- \subpage tutorial-getting-started-naoqi <br>This tutorial shows how to build a project that uses ViSP on NAOqi OS running on NAO, Romeo or Pepper humanoid robots.
+- \subpage tutorial-import-visp-into-EclipseIDE <br>This tutorial explains how to import a CMake project in Eclipse IDE.
+- \subpage tutorial-contrib-module <br>This tutorial explains how to exploit ViSP modular architecture in order to introduce a new contrib module.
+
+*/
+
+/*! \page tutorial_image Image manipulation
+This page introduces the user to the way to get, to filter or to render images.
+
+- \subpage tutorial-image-display <br>In this tutorial you will learn how to display an image in a window and how to down scale the window size to be able to display images that are larger than the screen resolution.
+- \subpage tutorial-grabber <br>This tutorial shows how to acquire images from a camera.
+- \subpage tutorial-image-filtering <br>This tutorial shows how to filter an image with ViSP.
+- \subpage tutorial-simu-image <br>This tutorial shows how to project the image of a planar scene to a given camera position.
+
+*/
+
+/*! \page tutorial_imgproc Image processing
+This page introduces some image processing methods.
+
+- \subpage tutorial-imgproc-brightness <br>This tutorial will show you some simple techniques to adjust or improve the brightness / contrast of an image.
+- \subpage tutorial-imgproc-contrast-sharpening <br>This tutorial will show you some simple techniques to improve the contrast and the sharpness of an image.
+- \subpage tutorial-imgproc-autothreshold <br>This tutorial will show you different methods to automatically threshold, binarize an image.
+- \subpage tutorial-imgproc-contour <br>This tutorial will show you how to extract the contours from a binary image.
+- \subpage tutorial-imgproc-connected-components <br>This tutorial will show you how to do a connected-components labeling.
+- \subpage tutorial-imgproc-flood-fill <br>This tutorial will show you how to use the flood fill algorithm.
+- \subpage tutorial-imgproc-count-coins <br>This tutorial will show you how to count the number of coins in an image.
+
+*/
+
+/*! \page tutorial_calib Camera calibration
+This page introduces the user to the way to calibrate a camera.
+
+- \subpage tutorial-calibration <br>This tutorial explains how to calibrate a camera.
+
+*/
+
+/*! \page tutorial_tracking Tracking
+This page introduces the user to the way to track objects in images.
+
+- \subpage tutorial-tracking-blob <br>This tutorial introduces blob tracking and detection.
+- \subpage tutorial-tracking-keypoint <br>This tutorial focuses on keypoint tracking using Kanade-Lucas-Tomasi feature tracker.
+- \subpage tutorial-tracking-me <br>This tutorial focuses on line and ellipse tracking using moving-edges.
+- \subpage tutorial-tracking-mb <br>This tutorial focuses on model-based trackers using either edges, keypoints or and hybrid scheme that uses edges and keypoints.
+- \subpage tutorial-tracking-mb-stereo <br>This tutorial focuses on model-based trackers using stereo cameras.
+- \subpage tutorial-tracking-mb-generic <br>This tutorial focuses on generic model-based trackers.
+- \subpage tutorial-tracking-mb-CAO-editor <br>This tutorial presents the project done during the GSoC 2017: two Blender plugins to import/export .cao model file and a Qt-based .cao editor.
+- \subpage tutorial-tracking-tt <br>This tutorial focuses on template trackers based on image registration approaches.
+
+*/
+
+/*! \page tutorial_detection Detection
+This page introduces the user to the way to detect features or objects in images.
+
+- \subpage tutorial-matching <br>This tutorial shows how to detect and match keypoints.
+- \subpage tutorial-matching-deprecated <br>This tutorial shows how to detect and match SURF keypoints. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
+- \subpage tutorial-detection-barcode <br>This tutorial focuses on bar code (QR code, Data Matrix code) detection.
+- \subpage tutorial-detection-face <br>This tutorial focuses on face detection thanks to OpenCV Haar cascade classifier.
+- \subpage tutorial-detection-object <br>This tutorial shows how to learn keypoints detected on a known object and how to use the matched correspondences to detect and estimate the pose of the object.
+- \subpage tutorial-detection-apriltag <br>This tutorial focuses on AprilTag patterns detection and pose estimation.
+
+*/
+
+/*! \page tutorial_computer_vision Computer vision
+This page introduces the user to the way to estimate a pose or an homography.
+
+- \subpage tutorial-pose-estimation <br>This tutorial focuses on pose estimation from planar or non planar points.
+- \subpage tutorial-pose-estimation-qrcode <br>This tutorial focuses on pose estimation of a QRcode using it's four corners location.
+- \subpage tutorial-homography <br>Here we explain how to estimate an homography from couples of matched points.
+- \subpage tutorial-homography-deprecated <br>Here we explain how to estimate an homography from couples of matched points. It will be deprecated if you use an OpenCV version equal to 2.8.0 or a more recent version.
+
+*/
+
+/*! \page tutorial_vs Visual servoing
+This page introduces the user to the way to achieve a visual servoing.
+
+- \subpage tutorial-ibvs <br>This tutorial explains how to simulate an IBVS.
+- \subpage tutorial-simu-robot-pioneer <br>This tutorial focuses on visual servoing simulation on a unicycle robot. The study case is a Pioneer P3-DX mobile robot equipped with a camera.
+- \subpage tutorial-boost-vs <br>This tutorial explains how to speed up the time to convergence of a visual servo.
+
+*/
+
+/*! \page tutorial_bridge Bridges over other frameworks
+This page introduces the user to the way to bridge other frameworks like OpenCV.
+
+- \subpage tutorial-bridge-opencv <br>This tutorial explicit how to convert OpenCV to/from ViSP structures such as camera parameters, images...
+
+*/
+
+/*! \page tutorial_tools Other tools
+This page introduces the user to other tools that may be useful.
+
+- \subpage tutorial-plotter <br>This tutorial explains how to plot curves in real-time during a visual servo.
+- \subpage tutorial-trace <br>This tutorial explains how to introduce trace in the code that could be enabled for debugging or disabled.
+- \subpage tutorial-multi-threading <br>This tutorial explains how to implement multi-threaded applications to capture images from a camera
+in one thread and display these images in an other thread.
+
+*/
diff --git a/doc/tutorial/unix/tutorial-install-centos.doc b/doc/tutorial/unix/tutorial-install-centos.doc
new file mode 100644
index 0000000..eb7475c
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-centos.doc
@@ -0,0 +1,424 @@
+/**
+
+\page tutorial-install-centos Tutorial: Installation from source for Linux CentOS
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on CentOS. These steps have been tested with CentOS 7.0 (x86_64) and with CentOS 7.2 (x86_64) distro, but should work with any other distribution as well.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_centos_required Install prerequisities
+
+- gcc 4.4.x or later. This can be installed with:
+\code
+$ sudo yum install gcc-c++
+\endcode
+- CMake 2.8.12.2 or higher that could be installed with:
+\code
+$ sudo yum install cmake
+\endcode
+
+
+\section install_centos_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_centos_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+$ sudo yum install opencv-devel
+\endcode
+- libX11 to be able to open a window to display images
+\code
+$ sudo yum install libX11-devel
+\endcode
+- lapack to benefit from optimized mathematical capabilities
+\code
+$ sudo yum install lapack-devel
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+$ sudo yum install libv4l-devel
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ sudo yum install libxml2-devel
+\endcode
+
+\subsection install_centos_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+$ sudo yum install libjpeg-devel libpng-devel
+\endcode
+
+
+\section install_centos_install_visp Install ViSP from source code
+\subsection install_centos_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_centos_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_centos_config Configuring ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-centos-all.jpg Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_centos_build Building ViSP libraries
+
+- To build ViSP libraries proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_centos_doc Building ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ sudo yum install doxygen graphviz
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_centos_dataset Install ViSP dataset
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $VISP_WS/ViSP-images-3.1.0.
+\code
+$ ls $VISP_WS/ViSP-images
+AprilTag circle ellipse iv LICENSE.txt mbt mire-2 video
+calibration cube ellipse-1 Klimt line mire README.md
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_centos_tips Tips and tricks
+
+\subsection install_centos_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_centos_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_centos_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+
+- For example to build the model-based tracker module named mbt, run:
+\code
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_centos_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_demos
+... visp_examples
+... visp_modules
+... visp_tests
+... visp_tutorials
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_centos_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file that contains also build info.
+\code
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-343-gdd1bc6e
+
+ Platform:
+ Timestamp: 2017-09-27T08:04:07Z
+ Host: Linux 3.10.0-514.6.1.el7.x86_64 x86_64
+ CMake: 2.8.12.2
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/gmake
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /usr/bin/c++ (ver 4.8.5)
+ C++ flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ C Compiler: /usr/bin/cc
+ C flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver n/a)
+ Use Lapack (built-in): no
+ Use Eigen3: no
+ Use OpenCV: yes (ver 2.4.5)
+ Use GSL: yes (ver 1.15)
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: no
+ \- Use OIS: no
+ Coin simulator:
+ \- Use Coin3D: no
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: yes (ver 62)
+ Use PNG: yes (ver 1.5.13)
+ \- Use ZLIB: yes (ver 1.2.7)
+ Use OpenCV: yes (ver 2.4.5)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): no
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 2.4.5)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: no
+ Use CMU 1394: no
+ Use V4L2: yes (ver 0.9.5)
+ Use directshow: no
+ Use OpenCV: yes (ver 2.4.5)
+ Use Flycapture: no
+ Use Pylon: no
+
+ RGB-D sensors:
+ Use Realsense: no
+ \- Use PCL: no
+ \- Use VTK: no
+ Use Kinect: no
+ \- Use libfreenect: no
+ \- Use libusb-1: no
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: no
+ Use dmtx: no
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.1)
+
+ Optimization:
+ Use OpenMP: yes
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: no
+
+ Documentation:
+ Use doxygen: no
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+\section install_centos_issues Known issues
+\subsection install_centos_issues_pthread libpthread may be hidden by files in //lib64
+
+- On CentOS 7.0 with cmake 2.8.11, during cmake configuration you may encounter the following issue:
+\code
+CMake Warning at src/CMakeLists.txt:80 (add_library):
+ Cannot generate a safe runtime search path for target visp because files in
+ some directories may conflict with libraries in implicit directories:
+
+ runtime library [libpthread.so] in /usr/lib64 may be hidden by files in:
+ //lib64
+
+ Some of these libraries may not be found correctly.
+\endcode
+- The problem was that libpthread.so exists in /usr/lib64 and in //lib64. In //lib64 it should be a symbolic link to /usr/lib64.
+\code
+$ ls -als //lib64
+ 0 lrwxrwxrwx. 1 root root 9 Feb 4 12:16 //lib64 -> usr/lib64
+$ ls -als //lib64/libpthread*
+140 -rwxr-xr-x. 1 root root 141616 Jan 27 15:13 //lib64/libpthread-2.17.so
+ 4 -rw-r--r--. 1 root root 222 Jan 27 14:42 //lib64/libpthread.so
+ 0 lrwxrwxrwx. 1 root root 18 Feb 4 12:34 //lib64/libpthread.so.0 -> libpthread-2.17.so
+\endcode
+- The fix consists in removing //lib64/libpthread.so and creating a new symbolic link
+\code
+$ cd //lib64
+$ sudo rm libpthread.so
+$ sudo ln -s libpthread-2.17.so libpthread.so
+$ ls -als libpthread*
+140 -rwxr-xr-x. 1 root root 141616 Jan 27 15:13 libpthread-2.17.so
+ 0 lrwxrwxrwx. 1 root root 18 Feb 4 16:09 libpthread.so -> libpthread-2.17.so
+ 0 lrwxrwxrwx. 1 root root 18 Feb 4 12:34 libpthread.so.0 -> libpthread-2.17.so
+\endcode
+
+\subsection install_centos_issues_videoreader vpVideoReader is not able to read mpeg videos
+
+- On CentOS 7.0, vpVideoReader uses OpenCV to read and decode videos. Some examples or tutorials provided in ViSP hang during cv::Capture::open() call. The reason is that OpenCV 2.4.5 cv::Capture seams buggy. This is for example the case if you run:
+\code
+$ ./example/video/videoReader
+\endcode
+- A work arround consists in installing a more recent OpenCV version from source.
+
+\section install_centos_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/unix/tutorial-install-fedora.doc b/doc/tutorial/unix/tutorial-install-fedora.doc
new file mode 100644
index 0000000..e6a29f9
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-fedora.doc
@@ -0,0 +1,410 @@
+/**
+
+\page tutorial-install-fedora Tutorial: Installation from source for Linux Fedora
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Linux Fedora. These steps have been tested for Fedora 21 and Fedora 26 (64 bit) distribution, but should work with any other distribution as well.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_fedora_required Install prerequisities
+
+Prior to build and install ViSP from source, you may install GNU g++ compiler and CMake.
+
+- gcc 4.4.x or later. This can be istalled with:
+\code
+$ su -c "dnf install gcc-c++"
+\endcode
+- CMake 2.8.12.2 or higher that could be installed with:
+\code
+$ su -c "dnf install cmake"
+\endcode
+
+\section install_fedora_3rdparty Install 3rd parties
+
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_fedora_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+$ su -c "dnf install opencv-devel"
+\endcode
+- libX11 to be able to open a window to display images
+\code
+$ su -c "dnf install libX11-devel"
+\endcode
+- lapack and eigen to benefit from optimized mathematical capabilities
+\code
+$ su -c "dnf install lapack-devel eigen3-devel"
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+$ su -c "dnf install libdc1394-devel"
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+$ su -c "dnf install libv4l-devel"
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ su -c "dnf install libxml2-devel"
+\endcode
+- QR code detection
+\code
+$ su -c "dnf install zbar-devel"
+\endcode
+- pthread library
+\code
+$ su -c "dnf install libxcb-devel"
+\endcode
+
+\subsection install_fedora_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- Coin, to be able to support vrml cad model used by the model-based trackers
+\code
+$ su -c "dnf install Coin3-devel"
+\endcode
+- libjpeg, libpng to support jpeg and png images (only useful if OpenCV is not installed)
+\code
+$ su -c "dnf install libjpeg-devel libpng-devel"
+\endcode
+- Ogre 3D if you want to do augmented reality or simulation
+\code
+$ su -c "dnf install ogre-devel ogre-samples ois-devel"
+\endcode
+- Datamatrix code detection
+\code
+$ su -c "dnf install libdmtx-devel"
+\endcode
+
+\section install_fedora_install_visp Install ViSP from source code
+\subsection install_fedora_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_fedora_get_source Getting ViSP source code
+There are different ways to get ViSP source code in this workspace:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_fedora_config Configuring ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-fedora-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_fedora_build Building ViSP libraries
+
+- To build ViSP libraries proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_fedora_doc Building ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ su -c "dnf install doxygen graphviz"
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_fedora_dataset Install ViSP dataset
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $VISP_WS/ViSP-images-3.1.0.
+\code
+$ ls $VISP_WS/ViSP-images-3.1.0
+AprilTag calibration cube ellipse-1 Klimt line mire README.md
+circle ellipse iv LICENSE.txt mbt mire-2 video
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_fedora_tips Tips and tricks
+
+\subsection install_fedora_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_fedora_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_fedora_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_fedora_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ cd $VISP_WS/visp-build
+$ make help | grep visp
+... visp_tutorials
+... visp_tests
+... visp_modules
+... visp_doc
+... visp_examples
+... visp_demos
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_fedora_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file:
+\code
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-343-gdd1bc6ee4
+
+ Platform:
+ Timestamp: 2017-09-27T10:39:21Z
+ Host: Linux 4.12.13-300.fc26.x86_64 x86_64
+ CMake: 3.9.1
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/gmake
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /usr/lib64/ccache/c++ (ver 7.2.1)
+ C++ flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O2 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ C Compiler: /usr/lib64/ccache/cc
+ C flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O2 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver n/a)
+ Use Lapack (built-in): no
+ Use Eigen3: yes (ver 3.3.4)
+ Use OpenCV: yes (ver 3.2.0)
+ Use GSL: yes (ver 2.3)
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: yes (ver 1.9.0)
+ \- Use OIS: yes (ver 1.3.0)
+ Coin simulator:
+ \- Use Coin3D: yes (ver 3.1.3)
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: yes (ver 62)
+ Use PNG: yes (ver 1.6.28)
+ \- Use ZLIB: yes (ver 1.2.11)
+ Use OpenCV: yes (ver 3.2.0)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): no
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 3.2.0)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: yes (ver 2.2.2)
+ Use CMU 1394: no
+ Use V4L2: yes (ver 1.12.5)
+ Use directshow: no
+ Use OpenCV: yes (ver 3.2.0)
+ Use Flycapture: no
+ Use Pylon: no
+
+ RGB-D sensors:
+ Use Realsense: no
+ \- Use PCL: no
+ \- Use VTK: no
+ Use Kinect: no
+ \- Use libfreenect: no
+ \- Use libusb-1: yes (ver 1.0.21)
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: yes (ver 0.20)
+ Use dmtx: yes (ver 0.7.4)
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.4)
+
+ Optimization:
+ Use OpenMP: yes
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: no
+
+ Documentation:
+ Use doxygen: yes
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+
+\section install_fedora_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/unix/tutorial-install-opensuse.doc b/doc/tutorial/unix/tutorial-install-opensuse.doc
new file mode 100644
index 0000000..ebb730e
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-opensuse.doc
@@ -0,0 +1,405 @@
+/**
+
+\page tutorial-install-opensuse Tutorial: Installation from source for Linux openSUSE
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on openSUSE. These steps have been tested for openSUSE 14.1 (x86_64) and openSUSE Leap 42.3 (x86_64) distro, but should work with any other distribution as well.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_opensuse_required Install prerequisities
+
+- gcc 4.4.x or later. This can be installed with:
+\code
+$ sudo zypper install gcc-c++
+\endcode
+- CMake 2.6 or higher that could be installed with:
+\code
+$ sudo zypper install cmake
+\endcode
+- Git if you want to get the source code from http://github.com/lagadic/visp
+\code
+$ sudo zypper install git
+\endcode
+
+\section install_opensuse_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_opensuse_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+$ sudo zypper install opencv-devel
+\endcode
+- libX11 to be able to open a window to display images
+\code
+$ sudo zypper install libX11-devel
+\endcode
+- lapack and eigen to benefit from optimized mathematical capabilities
+\code
+$ sudo zypper install lapack-devel eigen3-devel
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+$ sudo zypper install libv4l-devel
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+$ sudo zypper install libdc1394-devel
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ sudo zypper install libxml2-devel
+\endcode
+- libzbar to be able to detect QR codes
+\code
+$ sudo zypper install libzbar-devel
+\endcode
+- pthread library
+\code
+zypper install pthread-stubs
+\endcode
+
+\subsection install_opensuse_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- libjpeg and libpng to support jpeg and png images (only useful if OpenCV is not installed)
+\code
+$ sudo zypper install libjpeg8-devel libpng-devel
+\endcode
+
+\section install_opensuse_install_visp Install ViSP from source code
+\subsection install_opensuse_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_opensuse_get_source Getting ViSP source code
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_opensuse_config Configuring ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-opensuse-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_opensuse_build Building ViSP libraries
+
+- To build ViSP libraries proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_opensuse_doc Building ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ sudo zypper install doxygen graphviz
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_opensuse_dataset Install ViSP dataset
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $VISP_WS/ViSP-images-3.1.0.
+\code
+$ ls $VISP_WS/ViSP-images-3.1.0
+AprilTag circle ellipse iv LICENSE.txt mbt mire-2 video
+calibration cube ellipse-1 Klimt line mire README.md
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_opensuse_tips Tips and tricks
+
+\subsection install_opensuse_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_opensuse_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_opensuse_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_opensuse_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_modules
+... visp_examples
+... visp_demos
+... visp_tutorials
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_opensuse_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file that contains also build info.
+\code
+$ cat $VISP_WS/visp-build/ViSP-third-party.txt
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-346-g22ae3e680
+
+ Platform:
+ Timestamp: 2017-09-27T13:18:49Z
+ Host: Linux 4.4.76-1-default x86_64
+ CMake: 3.5.2
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/gmake
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /usr/bin/c++ (ver 4.8.5)
+ C++ flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ C Compiler: /usr/bin/cc
+ C flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver n/a)
+ Use Lapack (built-in): no
+ Use Eigen3: yes (ver 3.2.9)
+ Use OpenCV: yes (ver 3.1.0)
+ Use GSL: no
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: no
+ \- Use OIS: no
+ Coin simulator:
+ \- Use Coin3D: no
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: no
+ Use PNG: no
+ \- Use ZLIB: yes (ver 1.2.8)
+ Use OpenCV: yes (ver 3.1.0)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): no
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 3.1.0)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: yes (ver 2.2.3)
+ Use CMU 1394: no
+ Use V4L2: yes (ver 1.8.0)
+ Use directshow: no
+ Use OpenCV: yes (ver 3.1.0)
+ Use Flycapture: no
+ Use Pylon: no
+
+ RGB-D sensors:
+ Use Realsense: no
+ \- Use PCL: no
+ \- Use VTK: no
+ Use Kinect: no
+ \- Use libfreenect: no
+ \- Use libusb-1: no
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: yes (ver 0.10)
+ Use dmtx: no
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.4)
+
+ Optimization:
+ Use OpenMP: yes
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: no
+
+ Documentation:
+ Use doxygen: no
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+\section install_opensuse_issues Known issues
+\subsection install_opensuse_issues_libjpeg libjpeg.so.8 may conflict with libjpeg.so.62
+
+- Note that with openSUSE 12.04 but also with 13.02, \c libjpeg-devel package lead to \c libjpeg62.so installation, that may conflict with \c libjpeg8.so that is also installed. That's why we recommend to not install \c libjpeg-devel, but rather install \c libjpeg8-devel.
+\code
+Linking CXX executable HelloWorld
+/usr/lib64/gcc/x86_64-suse-linux/4.8/../../../../x86_64-suse-linux/bin/ld: warning: libjpeg.so.8, needed by /usr/lib64/libopencv_highgui.so.2.4.9, may conflict with libjpeg.so.62
+\endcode
+
+\section install_opensuse_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/unix/tutorial-install-osx-homebrew.doc b/doc/tutorial/unix/tutorial-install-osx-homebrew.doc
new file mode 100644
index 0000000..db4e2cb
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-osx-homebrew.doc
@@ -0,0 +1,444 @@
+/**
+
+\page tutorial-install-osx-homebrew Tutorial: Installation from source for OSX with Homebrew
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on OSX with Homebrew. These steps have been tested with Mac OS X 10.9.5 Mavericks, with 10.10.3 Yosemite and with 10.12.5 Sierra.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_osx_brew_required Install prerequisities
+
+- First, go to http://brew.sh to install Homebrew.
+
+- Next use homebrew to install additional software.
+\code
+$ brew update
+$ brew install cmake
+\endcode
+
+- You will need to add \c /usr/local/bin to the \c PATH environment var in your \c ~/.bashrc or \c ~/.bash_profile to have Homebrew be at the front of the PATH.
+\code
+$ echo "export PATH=/usr/local/bin:$PATH" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+\section install_osx_brew_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_osx_brew_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following packages.
+
+- OpenCV to get advanced image processing and computer vision features coming with ViSP
+\code
+$ brew install opencv
+\endcode
+
+- lapack and eigen for matrix inversion
+\code
+$ brew install lapack eigen
+\endcode
+
+- libdc1394 to capture images from firewire cameras
+\code
+$ brew install libdc1394
+\endcode
+
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ brew install libxml2
+\endcode
+
+- zbar for QR code detection
+\code
+$ brew install zbar
+\endcode
+
+\subsection install_osx_brew_3rdparty_other Other optional 3rd parties
+
+- If you have an Intel Realsense RGB-D camera (R200, F200, SR300, LR200, RZ300) you may install librealsense 1.12.1 and PCL library using:
+\code
+$ brew install librealsense pcl
+\endcode
+
+- If you have a rather a Basler camera you may donwload and install Pylon SDK following these <a href="https://visp.inria.fr/3rd_pylon/">instructions</a>.
+
+- If lapack 3rd party is not detected during CMake configuration it may be useful to install the Gnu Scientific Library (GSL) to benefit from optimized mathematical capabilities. To this end run the following instruction:
+\code
+$ brew install gsl
+\endcode
+
+\section install_osx_brew_install_visp Install ViSP from source code
+
+\subsection install_osx_brew_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_osx_brew_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> repository using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_osx_brew_config Configuring ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The previous command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd party that will be used. Then to generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-osx-all.jpg Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_osx_brew_build Building ViSP libraries
+
+- To build ViSP proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_osx_brew_doc Building ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ brew install doxygen
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_brew_dataset Install ViSP dataset
+
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data requested to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $HOME/ViSP-images-3.1.0.
+\code
+$ ls $HOME/ViSP-images-3.1.0
+AprilTag README.md cube iv mbt-depth video
+Klimt calibration ellipse line mire
+LICENSE.txt circle ellipse-1 mbt mire-2
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_brew_tips Tips and tricks
+
+\subsection install_brew_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_brew_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_brew_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_brew_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_doc
+... visp_modules
+... visp_tutorials
+... visp_demos
+... visp_tests
+... visp_examples
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_brew_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file that contains also build info.
+\code
+$ cat $VISP_WS/visp-build/ViSP-third-party.txt
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-340-gae7e5f10c-dirty
+
+ Platform:
+ Timestamp: 2017-09-26T20:35:47Z
+ Host: Darwin 16.6.0 x86_64
+ CMake: 3.9.1
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/make
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/c++ (ver 8.1.0.8020042)
+ C++ flags (Release): -Wall -Wextra -std=c++11 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -std=c++11 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ C Compiler: /Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/cc
+ C flags (Release): -Wall -Wextra -std=c++11 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -std=c++11 -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver n/a)
+ Use Lapack (built-in): no
+ Use Eigen3: yes (ver 3.3.4)
+ Use OpenCV: yes (ver 3.3.0)
+ Use GSL: yes (ver 2.3)
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: no
+ \- Use OIS: no
+ Coin simulator:
+ \- Use Coin3D: no
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: yes (ver 90)
+ Use PNG: yes (ver 1.6.31)
+ \- Use ZLIB: yes (ver 1.2.8)
+ Use OpenCV: yes (ver 3.3.0)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): no
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 3.3.0)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: yes (ver 2.2.2)
+ Use CMU 1394: no
+ Use V4L2: no
+ Use directshow: no
+ Use OpenCV: yes (ver 3.3.0)
+ Use Flycapture: no
+ Use Pylon: yes (ver 5.0.5.8973)
+
+ RGB-D sensors:
+ Use Realsense: yes (ver 1.12.1)
+ \- Use PCL: yes (ver 1.8.0)
+ \- Use VTK: yes (ver 8.0.0)
+ Use Kinect: no
+ \- Use libfreenect: no
+ \- Use libusb-1: yes (ver 1.0.21)
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: yes (ver 0.10)
+ Use dmtx: no
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.4)
+
+ Optimization:
+ Use OpenMP: no
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: yes
+
+ Documentation:
+ Use doxygen: yes
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+\section install_osx_brew_issues Kwown issues
+\subsection install_osx_brew_issues_opencv3 /usr/local/lib/pkgconfig is not writable
+
+- During OpenCV installation, if you get the following errors:
+\code
+$ brew install opencv
+Could not symlink lib/pkgconfig/isl.pc
+/usr/local/lib/pkgconfig is not writable.
+
+You can try again using:
+ brew link isl
+...
+\endcode
+it means maybe that you install other softwares without brew in /usr/local.
+A work arround is to change the owner of the corresponding folder like:
+\code
+$ sudo chown <your-user-name> /usr/local/lib/pkgconfig
+\endcode
+
+- If you enter into troubles with the packages you install with brew, a good stating is to run:
+\code
+$ brew doctor
+\endcode
+
+\subsection install_osx_brew_issues_libpng Application built with libpng-1.5.18 but running with 1.6.17
+- If you encounter the following issue
+\code
+$ ./modules/vision/testKeypoint-5
+libpng warning: Application built with libpng-1.5.18 but running with 1.6.17
+error: can't create a png read structure!
+error reading png file
+\endcode
+It means that apparently there is a conflict between libpng version installed by "brew install opencv3" (1.6.17), and the one used by X11/XQuartz (1.5.18).
+A work arround is to turn off libpng usage in ViSP. To configure and build again ViSP without png support:
+\code
+$ ccmake -DUSE_PNG=OFF ../ViSP
+$ make -j4
+\endcode
+An other work arround option is to turn off X11 usage in ViSP. Display capabilities will be then the one from OpenCV. To this end, configure and build again ViSP without X11 support:
+\code
+$ ccmake -DUSE_X11=OFF ../ViSP
+$ make -j4
+\endcode
+
+\section install_osx_brew_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/unix/tutorial-install-raspberry.doc b/doc/tutorial/unix/tutorial-install-raspberry.doc
new file mode 100644
index 0000000..912960d
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-raspberry.doc
@@ -0,0 +1,461 @@
+/**
+
+\page tutorial-install-raspberry Tutorial: Installation from source for Raspberry Pi
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on <a href="http://www.raspberrypi.org/help/what-is-a-raspberry-pi">Raspberry Pi 1 or Pi 2</a> equipped with an optional HD camera module.
+
+\image html img-raspberry-pi.jpg
+
+In a first section we give some useful instructions to start with a Raspberry PI. Then in the second section, we focus on the installation of ViSP from source.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_raspberry Raspberry Pi installation
+
+\subsection install_raspberry_setup Setting up Raspberry Pi
+
+- There are a lot of documentation and tutorial that explain different ways to setup a Raspberry Pi. A good reference is the official page <a href="http://www.raspberrypi.org/">http://www.raspberrypi.org</a>
+
+- We suggest to start with NOOBS (New Out Of the Box Software). Bellow we resume the different steps that are fully described in <a href="http://www.raspberrypi.org/help/noobs-setup/">http://www.raspberrypi.org/help/noobs-setup</a>:
+ - Format you SD card (at least 4Go are requested) on your computer. To this end you can use "GParted partition editor" under Ubuntu.
+ - Download Noobs "Offline and network install" zip file from <a href="http://www.raspberrypi.org/downloads/">http://www.raspberrypi.org/downloads</a>.
+ - Unzip all the files on the SD card.
+ - Insert your SD Card into your Raspberry Pi.
+ - Plug in your keyboard, mouse, monitor and Ethernet cables.
+ - Now plug in the USB power cable to your Pi.
+ - Your Raspberry Pi will boot, and a window will appear with a list of different operating systems that you can install. Select the "recommended Raspbian" checkbox and click on "Install" button.
+ - When the install process has completed, the Raspberry Pi configuration menu (raspi-config) will load. Here you are able to set the time and date for your region and enable a Raspberry Pi camera board, or even create users. You can exit this menu by using Tab on your keyboard to move to Finish.
+
+\subsection install_raspberry_login Logging in
+
+- The default login for Raspbian is username \c pi with the password \c raspberry.
+
+\subsection update Update your Raspbian distribution
+
+- If your Raspberry Pi is connected to Ethernet you can update your Raspbian distribution:
+\code
+$ sudo apt-get update
+$ sudo apt-get upgrade
+\endcode
+
+\subsection install_raspberry_camera Setting up a Raspberry Pi camera
+
+- If you have a Raspberry Pi camera module see <a href="http://www.raspberrypi.org/help/camera-module-setup/">http://www.raspberrypi.org/help/camera-module-setup</a>. To resume, enable the camera using:
+\code
+$ sudo raspi-config
+\endcode
+- Enter in menu "5/ Interfacing Options" to enable the camera.
+- Do a rebooot
+\code
+$ sudo reboot
+\endcode
+
+- Connect again and load the camera module:
+\code
+$ sudo modprobe bcm2835-v4l2
+\endcode
+
+- This will add the following modules:
+\code
+$ lsmod
+Module Size Used by
+bcm2835_v4l2 37611 0
+videobuf2_core 30853 1 bcm2835_v4l2
+v4l2_common 7792 1 bcm2835_v4l2
+videodev 121362 3 bcm2835_v4l2,v4l2_common,videobuf2_core
+\endcode
+
+- To check if the camera is recognized and connected, run:
+\code
+$ v4l2-ctl --list-formats
+\endcode
+
+- After each Raspberry Pi reboot you need to relaunch the previous modprobe command. To load bcm2835-v4l2 module during reboot, you can edit \c /etc/modules file
+\code
+$ sudo nano /etc/modules
+\endcode
+and add a line with the name of the module:
+\code
+bcm2835-v4l2
+\endcode
+
+
+\subsection install_raspberry_startx Start graphical user interface
+
+- To load the graphical user interface, type \c startx and press Enter on your keyboard. This will later allow to use ViSP vpDisplayX or vpDisplayOpenCV classes useful to display images in a X11 window.
+
+\section install_raspberry_visp ViSP installation
+\subsection install_raspberry_visp_prerequisities Install prerequisities
+
+- First you need to install the following packagages (g++, CMake, Git) that are requested to get and build ViSP:
+\code
+$ sudo apt-get install build-essential cmake-curses-gui git
+\endcode
+
+\subsection install_raspberry_visp_3rd_party Install recommended 3rd parties
+
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install the following:
+
+- OpenCV
+\code
+$ sudo apt-get install libopencv-dev
+\endcode
+- libX11 to be able to open a window to display images
+\code
+$ sudo apt-get install libx11-dev
+\endcode
+- lapack and eigen to benefit from optimized mathematical capabilities
+\code
+$ sudo apt-get install liblapack-dev libeigen3-dev
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+$ sudo apt-get install libv4l-dev
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ sudo apt-get install libxml2-dev
+\endcode
+- QR code detection
+\code
+$ sudo apt-get install libzbar-dev
+\endcode
+- pthread library
+\code
+$ sudo apt-get install libpthread-stubs0-dev
+\endcode
+
+\subsection install_raspberry_visp_3rd_party_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+$ sudo apt-get install libjpeg-dev libpng12-dev
+\endcode
+
+\subsection install_raspberry_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_raspberry_visp_get_source Get ViSP source code
+
+There are different ways to get ViSP source code on Raspberry Pi:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_raspberry_visp_config Configure ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-raspberry-all.png Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_raspberry_visp_build Build ViSP libraries
+
+- To build ViSP libraries proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_raspberry_visp_doc Build ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ sudo apt-get install doxygen graphviz texlive-latex-base
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_raspberry_dataset Install ViSP dataset
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $VISP_WS/ViSP-images-3.1.0.
+\code
+$ ls $VISP_WS/ViSP-images-3.1.0
+AprilTag circle ellipse iv LICENSE.txt mbt mire-2 video
+calibration cube ellipse-1 Klimt line mire README.md
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_raspberry_tips Tips and tricks
+
+\subsection install_raspberry_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_raspberry_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_raspberry_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_raspberry_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_modules
+... visp_examples
+... visp_demos
+... visp_tutorials
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_raspberry_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file that contains also build info.
+\code
+$ cat $VISP_WS/visp-build/ViSP-third-party.txt
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-350-gabcba81
+
+ Platform:
+ Timestamp: 2017-09-27T19:27:53Z
+ Host: Linux 4.1.19-v7+ armv7l
+ CMake: 3.0.2
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/make
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /usr/bin/c++ (ver 4.9.2)
+ C++ flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -fPIC -O3 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -fPIC -g
+ C Compiler: /usr/bin/cc
+ C flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -fPIC -O3 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver 3.7.0)
+ Use Lapack (built-in): no
+ Use Eigen3: yes (ver 3.2.2)
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use GSL: yes (ver 1.16)
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: no
+ \- Use OIS: no
+ Coin simulator:
+ \- Use Coin3D: no
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: yes (ver 62)
+ Use PNG: yes (ver 1.2.50)
+ \- Use ZLIB: yes (ver 1.2.8)
+ Use OpenCV: yes (ver 2.4.9.1)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): yes
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: yes (ver 2.2.3)
+ Use CMU 1394: no
+ Use V4L2: yes (ver 1.6.0)
+ Use directshow: no
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use Flycapture: no
+ Use Pylon: no
+
+ RGB-D sensors:
+ Use Realsense: no
+ \- Use PCL: no
+ \- Use VTK: no
+ Use Kinect: no
+ \- Use libfreenect: no
+ \- Use libusb-1: no
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: yes (ver 0.10)
+ Use dmtx: no
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.1)
+
+ Optimization:
+ Use OpenMP: yes
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: no
+
+ Documentation:
+ Use doxygen: no
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+\section install_raspberry_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project on Raspberry Pi or on any other system. Then if you have a Raspberry Pi camera module, you can also follow the \ref tutorial-tracking-blob especially subsection Tracking form v4l2 live cameras.
+
+You may also be interested in \ref tutorial-install-crosscompiling-raspberry if you want to speed up the build process or deploy ViSP on other Rapberry boards.
+*/
diff --git a/doc/tutorial/unix/tutorial-install-ubuntu.doc b/doc/tutorial/unix/tutorial-install-ubuntu.doc
new file mode 100644
index 0000000..9068e0f
--- /dev/null
+++ b/doc/tutorial/unix/tutorial-install-ubuntu.doc
@@ -0,0 +1,416 @@
+/**
+
+\page tutorial-install-ubuntu Tutorial: Installation from source for Linux Ubuntu
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Linux Ubuntu. These steps have been tested with Ubuntu 17.04 (64 bit) distribution, but should work with any other distribution as well.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_ubuntu_required Install prerequisities
+
+Prior to build and install ViSP from source, you may install GNU g++ compiler and CMake.
+
+- gcc 4.4.x or later. This can be installed with:
+\code
+$ sudo apt-get install build-essential
+\endcode
+- CMake 2.8.12.2 or higher that could be installed with:
+\code
+$ sudo apt-get install cmake-curses-gui
+\endcode
+
+\section install_ubuntu_3rdparty Install 3rd parties
+
+ViSP is interfaced with several optional 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>.
+
+\subsection install_ubuntu_3rdparty_recommended Recommended 3rd parties
+
+We recommend to install the following:
+
+- OpenCV
+\code
+$ sudo apt-get install libopencv-dev
+\endcode
+- libX11 to be able to open a window to display images
+\code
+$ sudo apt-get install libx11-dev
+\endcode
+- lapack and eigen to benefit from optimized mathematical capabilities
+\code
+$ sudo apt-get install liblapack-dev libeigen3-dev
+\endcode
+- libdc1394 to grab images from firewire cameras
+\code
+$ sudo apt-get install libdc1394-22-dev
+\endcode
+- libv4l to grab images from usb or analogic cameras
+\code
+$ sudo apt-get install libv4l-dev
+\endcode
+- libxml2 to be able to configure the model-based trackers from xml files
+\code
+$ sudo apt-get install libxml2-dev
+\endcode
+- QR code detection
+\code
+$ sudo apt-get install libzbar-dev
+\endcode
+- pthread library
+\code
+$ sudo apt-get install libpthread-stubs0-dev
+\endcode
+
+\subsection install_ubuntu_3rdparty_other Other optional 3rd parties
+
+We give also the way to install other 3rd party libraries to enable specific capabilities.
+
+- If you have an Intel Realsense RGB-D camera (R200, F200, SR300, LR200, RZ300) you may install librealsense 1.12.1 following these <a href="https://github.com/IntelRealSense/librealsense/blob/v1.12.1/doc/installation.md">installation instructions</a>. Additionnally you may also install PCL library using:
+\code
+$ sudo apt-get install libpcl-dev
+\endcode
+- If you have a Point Grey camera you may donwload and install Flycapture SDK following these <a href="https://visp.inria.fr/3rd_flycapture/">instructions</a>.
+
+- If you have a Basler camera you may donwload and install Pylon SDK following these <a href="https://visp.inria.fr/3rd_pylon/">instructions</a>.
+
+- Coin, to be able to support vrml cad model used by the model-based trackers
+\code
+$ sudo apt-get install libcoin80-dev
+\endcode
+- libjpeg and libpng to support jpeg and png images respectively (only useful if OpenCV is not installed)
+\code
+$ sudo apt-get install libjpeg-dev libpng-dev
+\endcode
+- Ogre 3D if you want to do augmented reality or simulation
+\code
+$ sudo apt-get install libogre-1.9-dev libois-dev
+\endcode
+- Datamatrix code detection
+\code
+$ sudo apt-get install libdmtx-dev
+\endcode
+
+\section install_ubuntu_visp Install ViSP from source code
+\subsection install_ubuntu_ws Create a workspace
+
+- First create a workspace in \c $HOME/visp-ws that will contain ViSP source, build and dataset.
+\code
+$ export VISP_WS=$HOME/visp-ws
+$ mkdir -p $VISP_WS
+\endcode
+
+\subsection install_ubuntu_visp_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once downloaded, uncompress the file using either
+\code
+$ tar xvzf visp-x.y.z.tar.gz -C $VISP_WS
+\endcode
+or
+\code
+$ unzip visp-x.y.z.zip -d $VISP_WS
+\endcode
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once downloaded, uncompress the file using
+\code
+$ tar xvzf visp-snapshot-yyyy-mm-dd.tar.gz -C $VISP_WS
+\endcode
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the following command
+\code
+$ cd $VISP_WS
+$ git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in the directory \c $VISP_WS/visp. The following should be adapted if you downloaded ViSP from a zip or tarball. In that case, the source is rather in something like \c $VISP_WS/visp-x.y.z.
+
+\subsection install_ubuntu_visp_config Configuring ViSP from source
+
+- In the worspace, create first a directory named \c visp-build that will contain all the build material; generated Makefiles, object files, output libraries and binaries.
+\code
+$ mkdir $VISP_WS/visp-build
+\endcode
+
+- Enter the \c visp-build folder and configure the build:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+\endcode
+A more versatile way to configure the build is to use \c ccmake, the CMake GUI:
+\code
+$ ccmake ../visp
+\endcode
+The following image shows that this command allows to configure (just by pressing [c] key) the build in a more advanced way where some options could be easily turned ON/OFF. It allows also to see which are the 3rd parties that will be used. To generate the makefiles, just press [g] key in the ccmake gui.
+\image html img-ccmake-ubuntu-all.jpeg Snapshot of the ccmake \c ../visp command used to configure ViSP.
+
+\subsection install_ubuntu_visp_build Building ViSP libraries
+
+- To build ViSP libraries proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4
+\endcode
+
+\note Installing ViSP is optional, since ViSP could be used as a 3rd party without installation. To proceed with the installation run:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make install
+\endcode
+\note The default install location is set to \c /usr/local. This location could be changed modifying \c CMAKE_INSTALL_PREFIX var:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp -DCMAKE_INSTALL_PREFIX=/usr
+$ sudo make install
+\endcode
+
+\subsection install_ubuntu_visp_doc Building ViSP documentation
+
+- To build ViSP documentation, you have first to install Doxygen package:
+\code
+$ sudo apt-get install doxygen graphviz texlive-latex-base
+\endcode
+Then you can proceed with:
+\code
+$ cd $VISP_WS/visp-build
+$ cmake ../visp
+$ make -j4 visp_doc
+\endcode
+The generated documentation is then available in \c $VISP_WS/visp-build/doc/html/index\.html
+
+- It is also possible to generate a more complete documentation that includes also all the internal classes. This could be achieved setting CMake var \c ENABLE\_FULL\_DOC to \c ON like:
+\code
+$ cmake ../visp -DENABLE_FULL_DOC=ON
+$ make -j4 visp_doc
+\endcode
+
+\section install_ubuntu_dataset Install ViSP dataset
+- Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+\code
+$ unzip ViSP-images-3.1.0.zip -d $VISP_WS
+\endcode
+
+- We suppose now that the data are located in \c $VISP_WS/ViSP-images-3.1.0.
+\code
+$ ls $VISP_WS/ViSP-images-3.1.0
+AprilTag circle ellipse iv LICENSE.txt mbt mire-2 video
+calibration cube ellipse-1 Klimt line mire README.md
+\endcode
+
+- Set \c VISP_INPUT_IMAGE_PATH environment variable to help ViSP examples and tests to find the location of the data set. It is more convenient if this environment variables is automatically added to your bash session every time a new shell is launched:
+\code
+$ echo "export VISP_WS=$HOME/visp-ws" >> ~/.bashrc
+$ echo "export VISP_INPUT_IMAGE_PATH=$VISP_WS/ViSP-images-3.1.0" >> ~/.bashrc
+$ source ~/.bashrc
+\endcode
+
+- From now, you can try to run ViSP examples and tests. For example you can run \c displayX example that should open a windows with Klimt painting image and some overlay drawings:
+\code
+$ cd $VISP_WS/visp-build
+$ ./example/device/display/displayX
+
+A click to close the windows...
+
+A click to display a cross...
+Cross position: 201, 441
+
+A click to exit the program...
+Bye
+\endcode
+
+\section install_ubuntu_tips Tips and tricks
+
+\subsection install_ubuntu_tips_uninstall How to uninstall ViSP
+- After ViSP installation, you can remove installed material using:
+\code
+$ cd $VISP_WS/visp-build
+$ sudo make uninstall
+\endcode
+
+\subsection install_ubuntu_tips_modules How to build only ViSP libraries
+
+- If you want to build only ViSP modules libraries, nor the examples, tutorials and tests:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_modules
+\endcode
+
+\subsection install_ubuntu_tips_module_once How to build a ViSP specific module
+
+- If you want to build a given module and all the dependencies:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_<module_name>
+\endcode
+For example to build the model-based tracker module named mbt, run:
+\code
+$ cd $VISP_WS/visp-build
+$ make -j4 visp_mbt
+\endcode
+
+\subsection install_ubuntu_tips_target Which are the targets that could be run with make ?
+
+- To know which are the target available with \c make:
+\code
+$ make help | grep visp
+... visp_tests
+... visp_modules
+... visp_examples
+... visp_demos
+... visp_tutorials
+... visp_clipper
+... visp_apriltag
+... visp_core
+... visp_gui
+... visp_io
+... visp_klt
+... visp_me
+... visp_sensor
+... visp_ar
+... visp_blob
+... visp_robot
+... visp_visual_features
+... visp_vs
+... visp_vision
+... visp_detection
+... visp_mbt
+... visp_tt
+... visp_tt_mi
+\endcode
+
+\subsection install_ubuntu_tips_3rd_party Which are the 3rd party libraries that are used in ViSP ?
+
+- To see which are the optional 3rd parties that are found during the configuration stage and that will be used by ViSP during the build you can have a look to the text file named \c ViSP-third-party.txt and located in \c $VISP_WS/visp-build. We provide hereafter an example of a possible content of this file that contains also build info.
+\code
+$ cat $VISP_WS/visp-build/ViSP-third-party.txt
+==========================================================
+General configuration information for ViSP 3.1.0
+
+ Version control: 3.0.1-340-gae7e5f10c
+
+ Platform:
+ Timestamp: 2017-09-26T15:06:26Z
+ Host: Linux 4.10.0-26-generic x86_64
+ CMake: 3.7.2
+ CMake generator: Unix Makefiles
+ CMake build tool: /usr/bin/make
+ Configuration: Release
+
+ C/C++:
+ Built as dynamic libs?: yes
+ C++ Compiler: /usr/bin/c++ (ver 6.3.0)
+ C++ flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C++ flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ C Compiler: /usr/bin/cc
+ C flags (Release): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -O3 -DNDEBUG
+ C flags (Debug): -Wall -Wextra -fopenmp -fvisibility=hidden -msse2 -msse3 -mssse3 -fPIC -g
+ Linker flags (Release):
+ Linker flags (Debug):
+
+ ViSP modules:
+ To be built: core gui io klt me sensor ar blob robot visual_features vs vision detection mbt tt tt_mi
+ Disabled: -
+ Disabled by dependency: -
+ Unavailable: -
+
+ Build options:
+ Build deprecated: yes
+ Build with moment combine: no
+
+ Mathematics:
+ Use Lapack/blas: yes (ver 3.7.0)
+ Use Lapack (built-in): no
+ Use Eigen3: yes (ver 3.3.2)
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use GSL: yes (ver 2.3)
+
+ Simulator:
+ Ogre simulator:
+ \- Use Ogre3D: yes (ver 1.9.0)
+ \- Use OIS: yes (ver 1.3.0)
+ Coin simulator:
+ \- Use Coin3D: yes (ver 4.0.0)
+ \- Use SoWin: no
+ \- Use SoXt: no
+ \- Use SoQt: no
+ \- Use Qt4: no
+ \- Use Qt3: no
+
+ Media I/O:
+ Use JPEG: yes (ver 80)
+ Use PNG: yes (ver 1.6.28)
+ \- Use ZLIB: yes (ver 1.2.11)
+ Use OpenCV: yes (ver 2.4.9.1)
+
+ Real robots:
+ Use Afma4: no
+ Use Afma6: no
+ Use Viper650: no
+ Use Viper850: no
+ Use aria (Pioneer): no
+ Use PTU46: no
+ Use Biclops PT: no
+
+ GUI:
+ Use X11: yes
+ Use GTK: no
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use GDI: no
+ Use Direct3D: no
+
+ Cameras:
+ Use DC1394-2.x: yes (ver 2.2.5)
+ Use CMU 1394: no
+ Use V4L2: yes (ver 1.12.3)
+ Use directshow: no
+ Use OpenCV: yes (ver 2.4.9.1)
+ Use Flycapture: no
+ Use Pylon: yes (ver 5.0.9.10389)
+
+ RGB-D sensors:
+ Use Realsense: no
+ \- Use PCL: no
+ \- Use VTK: no
+ Use Kinect: yes
+ \- Use libfreenect: yes (ver 0.5)
+ \- Use libusb-1: yes (ver 0.1.12)
+ \- Use pthread: yes
+
+ F/T sensors:
+ Use atidaq (built-in): no
+ Use comedi: no
+
+ Detection:
+ Use zbar: yes (ver 0.10)
+ Use dmtx: yes (ver 0.7.4)
+ Use AprilTag (built-in): yes (ver 0.9.8)
+
+ Misc:
+ Use Clipper (built-in): yes (ver 6.4.2)
+ Use XML2: yes (ver 2.9.4)
+
+ Optimization:
+ Use OpenMP: yes
+ Use pthread: yes
+ Use pthread (built-in): no
+ Use cxx11: no
+
+ Documentation:
+ Use doxygen: no
+
+ Tests and samples:
+ Tests: yes
+ Demos: yes
+ Examples: yes
+ Tutorials: yes
+
+ Install path: /usr/local
+
+==========================================================
+\endcode
+
+\section install_ubuntu_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial-homography-deprecated.doc b/doc/tutorial/vision/tutorial-homography-deprecated.doc
similarity index 100%
rename from doc/tutorial-homography-deprecated.doc
rename to doc/tutorial/vision/tutorial-homography-deprecated.doc
diff --git a/doc/tutorial-homography.doc b/doc/tutorial/vision/tutorial-homography.doc
similarity index 100%
rename from doc/tutorial-homography.doc
rename to doc/tutorial/vision/tutorial-homography.doc
diff --git a/doc/tutorial-pose-estimation-qrcode.doc b/doc/tutorial/vision/tutorial-pose-estimation-qrcode.doc
similarity index 100%
rename from doc/tutorial-pose-estimation-qrcode.doc
rename to doc/tutorial/vision/tutorial-pose-estimation-qrcode.doc
diff --git a/doc/tutorial-pose-estimation.doc b/doc/tutorial/vision/tutorial-pose-estimation.doc
similarity index 100%
rename from doc/tutorial-pose-estimation.doc
rename to doc/tutorial/vision/tutorial-pose-estimation.doc
diff --git a/doc/tutorial/visual-servo/tutorial-boost-vs.doc b/doc/tutorial/visual-servo/tutorial-boost-vs.doc
new file mode 100644
index 0000000..44d709c
--- /dev/null
+++ b/doc/tutorial/visual-servo/tutorial-boost-vs.doc
@@ -0,0 +1,100 @@
+/**
+
+\page tutorial-boost-vs Tutorial: How to boost your visual servo control law
+\tableofcontents
+
+\section intro_boost_vs Introduction
+
+This tutorial gives some hints to boost your visual servo control law in order to speed up the time to convergence.
+
+Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:
+
+\code
+$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/visual-servoing/ibvs
+\endcode
+
+To illustrate this tutorial let us consider the example tutorial-ibvs-4pts-plotter.cpp introduced in \ref tutorial-ibvs. This example consider an image based visual servoing using four points as visual features.
+
+In the general case, considering \f$ \dot {\bf q} \f$ as the input velocities to the robot controller, the control laws provided in vpServo class lead to the following control law \f$ \dot {\bf q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e}\f$ where the sign is negative for an eye in hand servo and positive for an eye to hand servo, \f$\lambda\f$ is a constant gain, \f$ {\bf \widehat J}_e\f$ is the task Jacobian and \f$\bf e \f$ is the error to regulate to zero. As described in \cite Ch [...]
+
+This behavior is illustrated with the next figure, where we see the exponential decrease of the eight visual features (x and y for each point) and the corresponding six velocities that are applied to the robot controller. As a consequence, velocities are high when the error is important, and very low when the error is small near the convergence. At the beginning, we can also notice velocity discontinuities with velocities varying from zero to high values in one iteration.
+
+\image html img-ibvs-control-law-exponential.png "Convergence in 191 iterations with a constant gain."
+
+This behavior can be reproduced running tutorial-ibvs-4pts-plotter.cpp example. Here after we recall the important lines of code used to compute the control law:
+
+\code
+ vpServo task ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setLambda(0.5); // Set the constant gain value
+ for (unsigned int i = 0 ; i < 4 ; i++) {
+ ...
+ task.addFeature(p[i], pd[i]); // Add visual features to the task
+ }
+ while(1) {
+ for (unsigned int i = 0 ; i < 4 ; i++) {
+ ...
+ vpFeatureBuilder::create(p[i], point[i]); // Update the visual features used in the task
+ }
+ vpColVector v = task.computeControlLaw(); // Compute the control law
+ }
+\endcode
+
+
+\section adaptive_gain Using an adaptive gain
+
+As implemented in tutorial-ibvs-4pts-plotter-gain-adaptive.cpp it is possible to adapt the gain \f$ \lambda \f$ in order to depend on the infinity norm of the task Jacobian. The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time. In that case the gain becomes:
+
+ \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{ \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f]
+
+ where:
+ - \f$ x = ||{\bf e}||_{\infty} \f$ is the infinity norm of the task Jacobian to consider.
+ - \f$\lambda_0 = \lambda(0)\f$ is the gain in 0, that is for very small values of \f$||{\bf e}||\f$
+ - \f$\lambda_\infty = \lambda_{||{\bf e}|| \rightarrow \infty}\lambda(||{\bf e}||)\f$ is the gain
+ to infinity, that is for very high values of \f$||{\bf e}||\f$
+ - \f$\lambda'_0\f$ is the slope of \f$\lambda\f$ at \f$||{\bf e}|| = 0\f$
+
+The impact of the adaptive gain is illustrated in the next figure. During the servo, velocities applied to the controller are higher, especially when the visual error \f${\bf e}\f$ is small. But as in the previous section, using an adaptive gain doesn't insure continuous velocities especially at the first iteration.
+
+\image html img-ibvs-control-law-adaptive.png "Convergence in 91 iterations with an adaptive gain."
+
+This behavior can be reproduced running tutorial-ibvs-4pts-plotter-gain-adaptive.cpp example. Compared to the previous code given in \ref intro and available in tutorial-ibvs-4pts-plotter.cpp, here after we give the new lines of code that were introduced to use an adaptive gain:
+
+\code
+ vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
+ task.setLambda(lambda);
+\endcode
+
+
+\section continuous_adaptive_gain Continuous sequencing
+
+As implemented in tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp it is also possible to ensure continuous sequencing to avoid velocity discontinuities. This behavior is achieved by introducing an additional term to the general form of the control law. This additional term comes from the task sequencing approach described in \cite Mansard07e equation (17). It allows to compute continuous velocities by avoiding abrupt changes in the command.
+
+ The form of the control law considered here is the following:
+
+ \f[
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t)
+ \f]
+
+ where :
+ - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot controller.
+ - the sign of the control law depends on the eye in hand or eye to hand configuration.
+ - \f$\bf J\f$ is the Jacobian of the task.
+ - \f$\bf e = (s-s^*)\f$ is the error to regulate.
+ - \f$t\f$ is the time.
+ - \f$\mu\f$ is a gain. We recommend to set this value to 4.
+ - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
+
+
+The effect of continuous sequencing is illustrated in the next figure where during the first iterations velocities are starting from zero.
+
+\image html img-ibvs-control-law-continuous-adaptive.png "Convergence in 98 iterations with an adaptive gain and continuous sequencing."
+
+This behavior can be reproduced running tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp example. Compared to the previous code given in \ref adaptive_gain and available in tutorial-ibvs-4pts-plotter-gain-adaptive.cpp, here after we give the new line of code that were introduced to ensure continuous sequencing:
+
+\code
+ vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());
+\endcode
+
+*/
diff --git a/doc/tutorial-ibvs.doc b/doc/tutorial/visual-servo/tutorial-ibvs.doc
similarity index 100%
rename from doc/tutorial-ibvs.doc
rename to doc/tutorial/visual-servo/tutorial-ibvs.doc
diff --git a/doc/tutorial-simu-robot-pioneer.doc b/doc/tutorial/visual-servo/tutorial-simu-robot-pioneer.doc
similarity index 100%
rename from doc/tutorial-simu-robot-pioneer.doc
rename to doc/tutorial/visual-servo/tutorial-simu-robot-pioneer.doc
diff --git a/doc/tutorial/windows/tutorial-install-win10-msvc12.doc b/doc/tutorial/windows/tutorial-install-win10-msvc12.doc
new file mode 100644
index 0000000..fa370df
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win10-msvc12.doc
@@ -0,0 +1,328 @@
+/**
+
+\page tutorial-install-win10-msvc12 Tutorial: Installation from source for Windows with Visual C++ 2013 (vc12)
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows 8.1 or 10.0 with Visual C++ 2013. These steps have been tested on Windows 10.0 (64 bit), with CMake 3.9.6 and Visual Studio Express 2013.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win10_msvc12_required Install prerequisities
+
+- Visual Studio C++. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products. In this tutorial we use Visual Studio Express 2013.
+\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
+- CMake that could be download at : http://www.cmake.org. Download the latest release for Windows win64-x64 platform (at the time this tuto was written it was the file \c cmake-3.9.6-win64-x64.msi). To install just double click on the msi file.
+
+\section install_win10_msvc12_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+\section install_win10_msvc12_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="https://visp.inria.fr/software-architecture">complete list is provided here</a>. If you are with Windows 8.1 we recommend to install \ref install_win10_msvc12_3rdparty_gdi. We recommend also to install \ref install_win10_msvc12_3rdparty_opencv. Other 3rd parties like \ref install_win10_msvc12_3rdparty_ogre are optional and should be considered only by expert developers.
+
+
+\subsection install_win10_msvc12_3rdparty_gdi Windows SDK 3rd party
+
+If you are under Windows 8.1 we recommend to install the "Windows Software Development Kit (SDK) for windows 8.1" to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
+
+- This SDK can be downloaded from https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx
+
+\subsection install_win10_msvc12_3rdparty_opencv OpenCV 3rd party
+
+<b>1. Get OpenCV</b>
+
+First you have to get OpenCV:
+- From http://opencv.org/releases.html download OpenCV 3.0.0 for Windows. In our case we got \c opencv-3.0.0.exe Win pack installer.
+- Extract the content of the archive in `C:\workspace`.
+\image html img-win10-extract-opencv.png
+- The installer extracted all the material in `C:\workspace\opencv`.
+\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (vc12) in `C:\workspace\opencv\build\x64\vc12` and also libraries compatible with Visual C++ 2012 (vc11) in `C:\workspace\opencv\build\x64\vc11`. With a more recent OpenCV release you need to build yourself OpenCV from source with Visual C++ 2013 (vc12) as it is done in \ref tutorial-install-win10-msvc15.
+- We strongly recommend to rename this folder to a name that contain OpenCV version like `C:\workspace\opencv-3.0.0`.
+
+<b>2. Complete OpenCV installation</b>
+
+Now you have to complete OpenCV installation setting some environment vars:
+- In order that ViSP detects OpenCV you have to set `OpenCV_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx OpenCV_DIR "C:\workspace\opencv-3.0.0\build"
+\endcode
+where `C:\workspace\opencv-3.0.0\build` is where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc12_env_var.
+
+
+\subsection install_win10_msvc12_3rdparty_realsense librealsense 3rd party
+
+I you have an Intel RealSense Depth camera (F200, SR300, R200, LR200, ZR300, and D400 series cameras), you may install librealsense 1.12.1. Otherwise you can skip this section.
+
+\note There is also librealsense 2.x.y, but ViSP is not compatible yet with this version, that is why we recommend to install the last stable release 1.12.1.
+
+<b>1. Get librealsense 1.12.1</b>
+
+- Get librealsense 1.12.1 source code from github
+
+ C:\> cd C:\workspace
+ C:\> git clone https://github.com/IntelRealSense/librealsense.git librealsense-1.12.1
+ C:\> cd librealsense-1.12.1
+ C:\> git checkout tags/v1.12.1
+
+- Here we suppose that you have librealsense source code in `C:\workspace\librealsense-1.12.1` folder.
+
+<b>2. Configure librealsense 1.12.1</b>
+
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image to set the location of the build to `C:/workspace/librealsense-1.12.1/build-vc12`:
+\image html img-cmake-win10-msvc12-realsense-config.png
+- Click then on "Configure" button and accept the creation of the build directory by a click on "Yes" button.
+- Select then your compiler "Visual Studio 12 2013 Win64" and click on "Finish" button.
+- Modify `CMAKE_INSTALL_PREFIX` var to `C:/workspace/librealsense-1.12.1/build-vc12` folder location.
+\image html img-cmake-win10-msvc12-realsense-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+
+<b>3. Install librealsense 1.12.1</b>
+
+- To build librealsense just double click on `C:/workspace/librealsense-1.12.1/Realsense.sln` solution file. This action will open Realsense project in Visual Studio C++ 12 2013.
+- Select `Release` configuration and build `INSTALL` target
+\image html img-win10-msvc12-realsense-build.png
+- After these steps, librealsense library could be found in `C:/workspace/librealsense-1.12.1/build-vc12/bin/realsense.dll`
+
+<b>4. Complete librealsense 1.12.1 installation</b>
+
+- Set `REALSENSE_DIR` environment variable:
+\code
+C:\> setx REALSENSE_DIR "C:\workspace\librealsense-1.12.1\build-vc12"
+\endcode
+- You have also to add the location of librealsense library in the PATH environment variable following the indications mentioned in \ref install_win10_msvc12_env_var.
+
+\subsection install_win10_msvc12_3rdparty_ogre Ogre3D 3rd party
+
+\warning Ogre3D installation from source code described in this section is reserved to expert users.
+
+If you are interested in augmented reality (see vpAROgre), or if you want to enable advanced visibility computation during model-based tracking (see vpMbTracker::setOgreVisibilityTest() and \ref tutorial-tracking-mb) we recommend to install Ogre3D. You can check if a <a href="http://www.ogre3d.org/download/sdk">prebuild SDK</A> matches your Visual C++ version, and follow the <a href="http://www.ogre3d.org/tikiwiki/Installing+the+Ogre+SDK">SDK installation instructions</a>. You can also i [...]
+
+Ogre source code is hosted on <a href="https://bitbucket.org/sinbad/ogre">BitBucket</a>. On Ogre wiki you will find useful information to <a href="http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Building+Ogre">build and install Ogre from source</a>. Hereafter we give the main steps.
+
+<b>1. Get Ogre prerequisities</b>
+- To build Ogre on Windows you will need to install the DirectX SDK. Since Windows 8, DirectX is no longer a standalone package, rather it is bundled in the Windows 8.x SDK. The latest version is the <a href="https://msdn.microsoft.com/en-us/windows/desktop/bg162891.aspx">Windows 8.1 SDK</a>.
+- To get the Ogre source code but also the source code of all the dependencies hosted on BitBucket and available with Mercurial, you have to install <a href="http://tortoisehg.bitbucket.org">TortoiseHg</a>.
+
+<b>2. Get Ogre dependencies</b>
+- A repository containing Ogre dependencies source code is available <a href="https://bitbucket.org/cabalistic/ogredeps">here at BitBucket</a>.
+- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/cabalistic/ogredeps and as destination \c C:\\Ogre\\ogredeps as in the next image:
+\image html img-win10-ogredeps-tortoisehg.png
+- Click on "Clone" button to get dependencies source code in `C:\workspace\ogredeps`
+
+<b>3. Build Ogre dependencies</b>
+- To build the dependencies, open CMake GUI and set source code location pointing to `C:\workspace\ogredeps` and build location pointing to `C:\workspace\ogredeps\build-vc12`. Select Visual Studio 12 2013 Win64 as generator, click on "Configure" button twice and then on "Generate" button. You should have something similar to the next image:
+\image html img-cmake-win10-msvc12-ogredeps.png
+- Open `C:\workspace\ogredeps\build-vc12\OGREDEPS.sln` solution file with Visual C++.
+- In Visual, select Debug configuration and build INSTALL project:
+\image html img-win10-msvc12-build-ogredeps.png
+\note If the build fails due to the environment variable \c DXSDK_DIR that is not set, see \ref known_issues_win10_msvc12_ogredeps for a work arround.
+- In Visual, select also Release configuration and build again INSTALL project.
+\image html img-win10-msvc12-build-ogredeps-install-release.png
+- Now in `C:\workspace\ogredeps\build-vc12\ogredeps\bin` you will find the `cg.dll` and `OIS.dll` dependencies build in Debug and Release:
+- To finish dependencies installation, set \c OGRE_DEPENDENCIES_DIR environment variable pointing to `C:\workspace\ogredeps\build-vc12\ogredeps`
+\code
+C:\> setx OGRE_DEPENDENCIES_DIR C:\workspace\ogredeps\build-vc12\ogredeps
+\endcode
+
+<b>4. Get Ogre source</b>
+
+- From <a href="http://www.ogre3d.org/download/source">Ogre download page</a>, identify the last release. In our case Ogre 1.10.9 that is on <a href="https://bitbucket.org/sinbad/ogre/branches">BitBucket repository</a> under branch v1-10-9.
+- Open an explorer window, and enter in TortoiseHg Clone menu. Select as source https://bitbucket.org/sinbad/ogre, as destination `C:\workspace\ogre` and select revision v1-10-9 corresponding to the branch of the latest release as presented in the next image:
+\image html img-win10-ogre-tortoisehg.png
+- Click on "Clone" button to get Ogre source code in `C:\workspace\ogre`
+
+<b>5. Build Ogre source</b>
+
+- Close and re-open CMake Gui. Set source code location pointing to `C:\workspace\ogre` and build location pointing to `C:\workspace\ogre\build-vc12`. Select "Visual Studio 12 2013 Win64" as generator, click on "Configure" button and then on "Generate" button. You should have something similar to the next image:
+\image html img-cmake-win10-msvc12-ogre.png
+\note As shown in the previous image, Ogre installation folder is set to `C:\workspace\ogre\build-vc12\sdk`.
+- Open `C:\workspace\ogre\build-vc12\OGRE.sln` solution file with Visual C++.
+- In Visual, select Debug configuration and build INSTALL project.
+- In Visual, select also Release configuration and build again INSTALL project.
+\image html img-win10-msvc12-build-ogre-install-release.png
+- Now in `C:\workspace\ogre\build-vc12\sdk\bin` you will find all the libraries in Debug and Release configuration, including cg and OIS dependencies that were build previously.
+\image html img-win10-ogre-explorer.png
+
+<b>6. Complete Ogre installation</b>
+
+- To finish Ogre installation set `OGRE_DIR` environment variables to `C:\workspace\ogre\build-vc12\sdk` location.
+\code
+C:\> setx OGRE_DIR C:\workspace\ogre\build-vc12\sdk
+\endcode
+
+\section install_win10_msvc12_install_visp Install ViSP from source code
+\subsection install_win10_msvc12_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+\subsection install_win10_msvc12_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution file that will be located in `C:/workspace/visp/visp-build-vc12`.
+
+- Launch CMake (cmake-gui) and complete the source code and binaries location as in the next image.
+\image html img-cmake-win10-msvc12-visp-launch.png
+- Click then on "Configure" button.
+\image html img-cmake-win10-msvc12-visp-create-folder.png
+- Click on "Yes" to create the `C:/workspace/visp/visp-build-vc12` folder.
+- Select then your compiler "Visual Studio 12 2013 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc12-msvc-version.png
+- This will start CMake configuration. As shown in the next image, eigen, Flycapture, GDI (Graphical Device Interface), OpenCV, OpenMP, Pylon, Realsense, XML2 and zbar 3rd parties are automatically detected.
+\image html img-cmake-win10-msvc12-visp-config.png
+\note Installation folder is set to `C:/workspace/visp/visp-build-vc12/install`. If you want to change the installation folder to `C:/Program Files (x86)/ViSP`, make sure that you have administrator privileges to write in that folder before modifying `CMAKE_INSTALL_PREFIX`.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win10-msvc12-visp-config2.png
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win10-msvc12-visp-generate.png
+
+- Once the generation is done, in `C:/workspace/visp/visp-build-vc12` folder you have the Visual Studio \c VISP.sln generated solution file.
+
+
+\subsection install_win10_msvc12_build Building ViSP from source
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc12/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+\image html img-win10-msvc12-visp-open.png
+
+- Enter menu "BUILD/Build Solution" to build ViSP or hit "Ctrl+Shift+B".
+
+- At the end of the build process you should have the following indicating that all the build succeeded.
+\image html img-win10-msvc12-visp-build-end.png
+
+- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
+\image html img-win10-msvc12-visp-install.png
+
+- At the end of the installation, you should have the following.
+\image html img-win10-msvc12-visp-install-end.png
+
+- As shown in the previous image, all the headers but also the generated libraries are copied in `C:/workspace/visp/visp-build-vc12/install` folder; headers and libraries in `include` and `x64/vc14` subfolders respectively.
+
+- This ends ViSP installation with \c Debug configuration.
+
+- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
+\image html img-win10-msvc12-visp-release.png
+
+- Now, as previously, build and install ViSP again.
+
+- At the end, in `C:/workspace/visp/visp-build-vc12/install/x64/vc12/bin` folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+\image html img-win10-msvc12-visp-explorer-install.png
+
+
+
+\subsection install_win10_msvc12_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\visp\visp-build-vc12\install\x64\vc12\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\opencv-3.0.0\build\x64\vc12\bin"
+\endcode
+
+Then to add the path to librealsense 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\librealsense-1.12.1\build-vc12\bin"
+\endcode
+
+If installed, you have also to add the location of Ogre 3rd party libraries to the path. Close and re-open a cmd terminal to run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\ogre\build-vc12\sdk\bin"
+C:\> setx PATH "%PATH%;C:\workspace\ogre\build-vc12\sdk\bin\Release"
+C:\> setx PATH "%PATH%;C:\workspace\ogre\build-vc12\sdk\bin\Debug"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned.
+
+\code
+C:\> echo %PATH%
+\endcode
+
+\section install_win10_msvc12_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named `visp-images-x.y.z.zip`. This archive could be downloaded from https://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download `visp-images-3.1.0.zip` from https://visp.inria.fr/download and uncompress it in your workspace `C:/workspace`.
+\image html img-win10-visp-images.png
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called `VISP_INPUT_IMAGE_PATH`. In our case, this variable should be set to `C:\workspace\visp-images-3.1.0`.
+\code
+C:\> setx VISP_INPUT_IMAGE_PATH C:\workspace\visp-images-3.1.0
+\endcode
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run `C:/worspace/visp/visp-build-vc12/example/device/display/Debug/displayGDI.exe`, open a command window, enter in the right folder, and run:
+
+\image html img-win8.1-cmd-displayGDI.jpg
+
+\section known_issues_win10_msvc12 Known issues
+
+\subsection known_issues_win10_msvc12_ogredeps Unable to build ogredeps: DXSDK_DIR missing
+
+During ogredeps build as described in \ref install_win10_msvc12_3rdparty_ogre installation you may get in Visual C++ the following error when building SDL2 project:
+\code
+error: DIRECTX requires $DXSDK_DIR environment variable to be set
+\endcode
+\image html img-win8.1-msvc-ogredeps-issue.jpg
+
+To fix the issue:
+- Quit Visual C++ and start again CMake over ogredeps project
+- Disable SDL2 dependency build turning \c OGREDEPS_BUILD_SDL2 option off
+\image html img-win8.1-cmake-ogredeps-issue.jpg
+- Configure and generate again the project
+- Open \c C:\\Ogre\\ogredeps\\build\\OGREDEPS.sln solution file with Visual C++ and try a new build as described in \ref install_win10_msvc12_3rdparty_ogre
+
+\subsection known_issues_win10_msvc12_ogre_mbtracker Model-based trackers are not working with Ogre visibility ckeck
+
+If you run \c mbtEdgeTracking.exe, \c mbtKltTracking.exe or \c mbtEdgeKltTracking.exe enabling Ogre visibility check (using "-o" option), you may encounter the following issue:
+\code
+C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o
+...
+OGRE EXCEPTION(6:FileNotFoundException): Cannot locate resource VTFInstancing.cg in resource group General
+...
+*** Initializing OIS ***
+\endcode
+and then a wonderful runtime issue as in the next image:
+\image html img-win8.1-msvc-mbtracker-ogre-issue.jpg
+
+It means maybe that Ogre version is not compatible with DirectX 11. This can be checked adding "-w" option to the command line:
+\code
+C:\ViSP\visp-build\example\Tracking\Debug> mbtEdgeTracking.exe -c -o -w
+\endcode
+Now the binary should open the Ogre configuration window where you have to select "OpenGL Rendering Subsystem" instead of "Direct3D11 Rendering Subsystem". Press then OK to continue and start the tracking of the cube.
+\image html img-win8.1-msvc-mbtracker-ogre-opengl.jpg
+
+\section install_win10_msvc12_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win10-msvc14-uwp.doc b/doc/tutorial/windows/tutorial-install-win10-msvc14-uwp.doc
new file mode 100644
index 0000000..e57f098
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win10-msvc14-uwp.doc
@@ -0,0 +1,159 @@
+/**
+
+\page tutorial-install-win10-msvc14-uwp Tutorial: Installation from source for UWP with Visual C++ 2015 (vc14)
+\tableofcontents
+
+In this tutorial you will learn how to build ViSP framework for Universal Windows Platform (UWP) to include ViSP functionalities in apps developments that target a wide range of devices including PC, mobile, Xbox, HoloLens, IoT, and Surface Hub.
+
+This tutorial has been tested on Windows 10 (64 bit), with CMake 3.7.1 and Visual Studio Community 2015 and is only working since ViSP 3.0.1.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section tutorial_install_msvc14_uwp_required Install required software
+
+- CMake last version that could be download at http://www.cmake.org
+- <a href="https://www.visualstudio.com/downloads/">Visual Studio Community 2015</a>. Make sure that the Universal Windows App Development Tools are selected from the optional features list. Without these tools, you won't be able to create your universal apps. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2015.
+
+\note If you have already Visual Studio 2015 installed and don't know if the Universal Windows App Development Tools are enabled see \ref install_msvc14_uwp_tips_msvc.
+
+After installing Visual Studio software, you need to <a href="https://msdn.microsoft.com/windows/uwp/get-started/enable-your-device-for-development">enable your Windows 10 device</a> for development.
+
+\section install_msvc14_uwp_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+\section install_msvc14_uwp_get_visp_source Get ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z`.
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z`.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+Next we suggest to first build ViSP without OpenCV.
+
+\section install_msvc14_uwp_build_without_opencv Build ViSP
+
+\subsection install_msvc14_uwp_build_ws_10 Build for Windows Store 10.0
+
+- Configure ViSP build for Windows Store 10 targeting a x86 platform, by opening a cmd terminal and running:
+\code
+C:\> cd C:\workspace\visp
+C:\> md visp-build-vc14-uwp-ws-10
+C:\> cd visp-build-vc14-uwp-ws-10
+C:\> cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
+\endcode
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc14-uwp-ws-10/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++.
+
+- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in `C:/workspace/visp/visp-build-vc14-uwp-ws-10/install` folder.
+
+\subsection install_msvc14_uwp_build_ws_8_1 Build for Windows Store 8.1
+
+- Configure ViSP build for Windows Store 8.1 targeting a x86 platform, by opening a cmd terminal and running:
+\code
+C:\> cd C:\workspace\visp
+C:\> md visp-build-vc14-uwp-ws-8.1
+C:\> cd visp-build-vc14-uwp-ws-8.1
+C:\> cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="8.1" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
+\endcode
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc14-uwp-ws-8.1/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++.
+
+- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in `C:/workspace/visp/visp-build-vc14-uwp-ws-8.1/install` folder.
+
+\subsection install_msvc14_uwp_build_wp_8_1 Build for Windows Phone 8.1
+
+- Configure ViSP build for Windows Phone 8.1 targeting a x86 platform, by opening a cmd terminal and running:
+\code
+C:\> cd C:\workspace\visp
+C:\> md visp-build-vc14-uwp-wp-8.1
+C:\> cd visp-build-vc14-uwp-wp-8.1
+C:\> cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsPhone -DCMAKE_SYSTEM_VERSION:String="8.1" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
+\endcode
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc14-uwp-wp-8.1/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++.
+
+- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in `C:/workspace/visp/visp-build-vc14-uwp-wp-8.1/install` folder.
+
+Now if you want that ViSP benefits from OpenCV functionalities, you can follow the next section.
+
+\section install_msvc14_uwp_build_with_opencv Build ViSP with OpenCV
+
+\note The following steps are given for Windows Store 10.0 targetting a x86 platform. They need to be adapted to your use case.
+
+Building ViSP with OpenCV as 3rd party needs first to succeed in building OpenCV for UWP. As of writing on January 2017, the way is to use Microsoft fork <a href="https://github.com/Microsoft/opencv">https://github.com/Microsoft/opencv</a> and branch \c vs2015-samples.
+
+- Get OpenCV source code
+\code
+C:\> md C:\workspace\opencv
+C:\> cd C:\workspace\opencv
+C:\> git clone https://github.com/Microsoft/opencv.git
+C:\> cd opencv
+C:\> git checkout vs2015-samples
+\endcode
+
+- Download <a href="http://visp-doc.inria.fr/download/opencv/patch/opencv-microsoft-branch-vs2015-samples-patch.diff">opencv-microsoft-branch-vs2015-samples-patch.diff</a> patch and save it in `C:\workspace` folder. This patch allows to add \c /WZ compiler option when building highgui and videoio modules.
+- Modify the source code applying the patch
+\code
+C:\> cd C:\workspace\opencv\opencv
+C:\> git apply --ignore-space-change --ignore-whitespace ..\opencv-microsoft-branch-vs2015-samples-patch.diff
+\endcode
+
+- Configure OpenCV build. The following command is for Windows Store 10.0 targetting a x86 platform but could be easily adapted:
+\code
+C:\> cd C:\workspace\opencv
+C:\> md opencv-build-vc14-uwp-ws-10.0
+C:\> cd opencv-build-vc14-uwp-ws-10.0
+C:\> cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 -DBUILD_opencv_ts=OFF ../opencv
+\endcode
+
+- Open `C:/workspace/opencv/opencv-build-vc14-uwp-ws-10/opencv.sln` file with Visual Studio 2015, build and install all the solution
+
+- If everything goes right you will find OpenCV libraries and headers in `C:/workspace/opencv/opencv-build-vc14-uwp-ws-10/install` folder.
+
+Now you can use OpenCV as third-party and configure ViSP in order to use OpenCV. To this end modify the cmake command that was used to \ref install_msvc14_uwp_build_without_opencv by adding `-DOpenCV_DIR=C:\workspace\opencv\opencv-build-vc14-uwp-ws-10\install` option to cmake command line:
+
+\code
+C:\> cd C:\workspace\visp\visp-build-vc14-uwp-ws-10
+C:\> cmake.exe -G "Visual Studio 14 2015" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 -DOpenCV_DIR=C:/workspace/opencv/opencv-build-vc14-uwp-ws-10/install ../visp
+\endcode
+
+As described in \ref install_msvc14_uwp_build_without_opencv, open again ViSP solution file, build and install ViSP.
+
+\section install_msvc14_uwp_tips Tips and tricks
+\subsection install_msvc14_uwp_tips_msvc How to add Universal Windows App Development Tools support
+
+In case you're trying to build Windows Store and Windows Phone 10.0 projects having only default Visual Studio 2015 installation you'll likely get the following error during \c cmake run:
+\code
+A Windows Store component with CMake requires both the Windows Desktop SDK
+as well as the Windows Store '10.0' SDK. Please make sure that you have
+both installed
+\endcode
+
+Resolution consists in:
+- On the Start menu select Settings
+- Select System > Apps & features.
+- Select Microsoft Visual Studio 2015 program, and right click to modify installation
+\image html img-uwp-msvc-modify.png
+- Select Modify
+\image html img-uwp-msvc-modify2.png
+- Enable "Universal Windows App Development Tools" and "Windows 8.1 and Windows Phone 8.0/8.1 Tools"
+\image html img-uwp-msvc-modify3.png
+- Select Update button
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win10-msvc14.doc b/doc/tutorial/windows/tutorial-install-win10-msvc14.doc
new file mode 100644
index 0000000..815da92
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win10-msvc14.doc
@@ -0,0 +1,372 @@
+/**
+
+\page tutorial-install-win10-msvc14 Tutorial: Installation from source for Windows with Visual C++ 2015 (vc14)
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++. These steps have been tested on Windows 10 (64 bit), with CMake 3.10.0 and Visual Studio Community 2015.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win10_msvc14_required Install prerequisities
+
+- Visual C++. Visual Studio Community 2015 that includes Visual C++ could be downloaded from https://www.visualstudio.com. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2015.
+- CMake that could be download at : http://www.cmake.org. Download the latest release for Windows win64-x64 platform (at the time this tuto was written it was the file \c cmake-3.10.0-rc3-win64-x64.msi). To install just double click on the msi file.
+- Windows Software Developement Kit (SDK) for Windows 10. This SDK could be downloaded from https://dev.windows.com/en-US/downloads/windows-10-sdk. This SDK is requested by CMake and allows also to get the Graphical Device Interface (GDI) capabilities. The GDI is used in ViSP to display images in a window thanks to vpDisplayGDI class.
+
+\section install_win10_msvc14_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+\section install_win10_msvc14_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="https://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install \ref install_win10_msvc14_3rdparty_opencv and \ref install_win10_msvc14_3rdparty_xml in the workspace. If you have an Intel Realsense depth camera you may also install \ref install_win10_msvc14_3rdparty_realsense. If your camera is a PointGrey you may install \ref install_win10_msvc14_3rdparty_flycapture, while if your [...]
+
+\subsection install_win10_msvc14_3rdparty_opencv OpenCV 3rd party
+
+<b>1. Get OpenCV</b>
+
+First you have to get OpenCV:
+- From http://opencv.org/releases.html download the latest OpenCV for Windows. In our case we got \c opencv-3.3.1-vc14.exe Win pack installer. The same procedure could be used to consider rather OpenCV 3.3.0 by downloading \c opencv-3.3.0-vc14.exe Win pack installer.
+- Extract the content of the archive in `C:\workspace`.
+\image html img-win10-extract-opencv.png
+- The installer extracted all the material in `C:\workspace\opencv`.
+- We strongly recommend to rename this folder to a name that contain OpenCV version like `C:\workspace\opencv-3.3.1`.
+
+OpenCV 3.3.1 windows installer contains pre-build libraries compatible with Visual C++ 2015 (vc14) in `C:\workspace\opencv-3.3.1\build\x64\vc14`. That is why there is no need to build OpenCV from source for vc14 as it is done in \ref tutorial-install-win10-msvc15.
+
+<b>2. Complete OpenCV installation</b>
+
+Now you have to complete OpenCV installation setting some environment vars:
+- In order that ViSP detects OpenCV you have to set `OpenCV_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx OpenCV_DIR "C:\workspace\opencv-3.3.1\build"
+\endcode
+where `C:\workspace\opencv-3.3.1\build` is where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc14_env_var.
+
+\subsection install_win10_msvc14_3rdparty_xml libxml2 3rd party
+
+<b>1. Get libxml2</b>
+- Download the latest libxml2 release from ftp://xmlsoft.org/libxml2. At the time this tuto was written it was `libxml2-2.9.7.tag.gz` file.
+- Extract the content of the tarball in `C:\workspace`. [Winrar](https://www.win-rar.com) could be used to this end.
+- Here we suppose that you have libxml2 source code in `C:\workspace\libxml2-2.9.7` folder.
+
+<b>2. Install libxml2</b>
+
+- In the Windows "Start" menu, scroll to "Visual Studio 2015", and select "Visual Studio Command Prompt for native x64".
+- To build libxml2 from source enter the following instructions in the Visual Studio Command Prompt:
+\code
+C:\> cd C:\workspace\libxml2-2.9.7\win32
+C:\> cscript configure.js iconv=no ftp=no http=no compiler=msvc prefix=C:\workspace\libxml2-2.9.7\build-vc14
+C:\> nmake /f Makefile.msvc install
+\endcode
+- After these steps, libxml2 library could be found in `C:/workspace/libxml2-2.9.7/build/vc14/bin/libxml2.dll`
+
+<b>3. Complete libxml2 installation</b>
+
+- To complete the installation, set `XML2_DIR` environment variable:
+\code
+C:\> setx XML2_DIR "C:\workspace\libxml2-2.9.7\build-vc14"
+\endcode
+- You have also to add the location of libxml2 library in the PATH environment variable following the indications mentioned in \ref install_win10_msvc14_env_var.
+
+\subsection install_win10_msvc14_3rdparty_realsense librealsense 3rd party
+
+I you have an Intel RealSense Depth camera (F200, SR300, R200, LR200, ZR300, and D400 series cameras), you may install librealsense 1.12.1. Otherwise you can skip this section.
+
+\note There is also librealsense 2.x.y, but ViSP is not compatible yet with this version, that is why we recommend to install the last stable release 1.12.1.
+
+<b>1. Get librealsense 1.12.1</b>
+
+- Get librealsense 1.12.1 source code from github
+
+ C:\> cd C:\workspace
+ C:\> git clone https://github.com/IntelRealSense/librealsense.git librealsense-1.12.1
+ C:\> cd librealsense-1.12.1
+ C:\> git checkout tags/v1.12.1
+
+- Here we suppose that you have librealsense source code in `C:\workspace\librealsense-1.12.1` folder.
+
+<b>2. Configure librealsense 1.12.1</b>
+
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image to set the location of the build to `C:/workspace/librealsense-1.12.1/build-vc14`:
+\image html img-cmake-win10-msvc14-realsense-config.png
+- Click then on "Configure" button and accept the creation of the build directory by a click on "Yes" button.
+- Select then your compiler "Visual Studio 14 2015 Win64" and click on "Finish" button.
+- Modify `CMAKE_INSTALL_PREFIX` var to `C:/workspace/librealsense-1.12.1/build-vc14` folder location.
+\image html img-cmake-win10-msvc14-realsense-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+
+<b>3. Install librealsense 1.12.1</b>
+
+- To build librealsense just double click on `C:/workspace/librealsense-1.12.1/Realsense.sln` solution file. This action will open Realsense project in Visual Studio C++ 14 2015.
+- Select `Release` configuration and build `INSTALL` target
+\image html img-win10-msvc14-realsense-build.png
+- After these steps, librealsense library could be found in `C:/workspace/librealsense-1.12.1/build-vc14/bin/realsense.dll`
+
+<b>4. Complete librealsense 1.12.1 installation</b>
+
+- set `REALSENSE_DIR` environment variable:
+\code
+C:\> setx REALSENSE_DIR "C:\workspace\librealsense-1.12.1\build-vc14"
+\endcode
+- You have also to add the location of librealsense library in the PATH environment variable following the indications mentioned in \ref install_win10_msvc14_env_var.
+
+\subsection install_win10_msvc14_3rdparty_flycapture FlyCapture 3rd party
+
+I you have a PointGrey USB 2.0, USB 3.0, GigE or FireWire camera (DragonFly2, Flea3...), you may install FlyCapture. Otherwise you can skip this section.
+
+Complete installation instructions are given [here](https://www.ptgrey.com/KB/10020). We recap hereafter the main instructions to install FlyCapture SDK under Windows:
+
+- Download the latest FlyCapture2 Full SDK for your camera from Point Grey’s [download site](https://eu.ptgrey.com/support/downloads). On the download page, select your product family and "Windows 10 64 bit" as operating system. At the time this tutorial we written we selected "FlyCapture 2.11.3.164 SDK - Windows (64 bit)" to download `FlyCapture_2.11.3.164_x64.exe` file.
+- Double-click the downloaded software executable and follow the installation instructions.
+- Select the correct driver for the camera when prompted by the installation instructions.
+- Reboot the system.
+
+\subsection install_win10_msvc14_3rdparty_pylon Pylon 3rd party
+
+I you have a Basler FireWire, Camera Link, GigE or USB3.0 camera, you may install Pylon SDK. Otherwise you can skip this section.
+
+To install Pylon:
+- visit [Basler Software Download page](https://www.baslerweb.com/en/support/downloads/software-downloads/) to download and install the SDK corresponding to your platform. At the time this tutorial we written we selected "Pylon 5.0.11 Camera Software Suite Windows" to download `Basler_pylon_5.0.11.10913.exe` file.
+- Double-click on downloaded file and follow the installation instructions.
+- Be carreful to select "Developer" profile during the installation in order to install Pylon SDK headers (.h) besides the dll.
+\image html img-win10-pylon-developer.png
+- Click then on "Next" button.
+- Select how your Basler camera is connected to the computer (USB, GigE, FireWire, Camera Link), then click on "Next" button.
+- Keep the default destination folder as `C:\Program Files\Basler\pylon 5\` and click on "Next" button
+- In the "Ready to install" panel, click on the "Install" button
+- Reboot the system.
+
+\subsection install_win10_msvc14_3rdparty_zbar zbar 3rd party
+
+If you want to work with QR codes (see \ref tutorial-detection-barcode), we recommend to install zbar 3rd party. Otherwise you can skip this section.
+\note Since ViSP 3.1.0 we provide also AprilTag detection capabilities that are available without 3rd party installation (see \ref tutorial-detection-apriltag).
+
+To install zbar you need to build the library from source. There is a fork of the library available from GitHub (<a href="https://github.com/dani4/ZBarWin64">https://github.com/dani4/ZBarWin64</a>) and adapted for compiling under Visual following the steps:
+
+<b>1. Get zbar</b>
+- Get ZBarWin64 source code from <a href="https://github.com/dani4/ZBarWin64">Github repository</a>. Open a command window and hit:
+\code
+C:\> cd C:\workspace
+C:\> git clone https://github.com/dani4/ZBarWin64
+\endcode
+
+<b>2. Build zbar</b>
+- Then open `C:\workspace\ZBarWin64\zbar64.sln` solution file with Visual Studio C++
+- Select x64 configuration and build zbar library in Release
+\image html img-win10-msvc15-zbar-build.png
+
+<b>3. Complete zbar installation</b>
+- To complete the installation, set `ZBAR_DIR` environment variable:
+\code
+C:\> setx ZBAR_DIR "C:\workspace\ZBarWin64"
+\endcode
+- You have also to add the location of zbar and iconv libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc14_env_var.
+
+\subsection install_win10_msvc14_3rdparty_eigen eigen 3rd party
+
+Even if eigen is designed as a template we recommand to install the library with Visual Studio.
+
+<b>1. Get eigen</b>
+
+- Download the latest eigen release from http://eigen.tuxfamily.org. At the time this tutorial was written we downloaded `eigen-eigen-5a0156e40feb.tar.gz` tarball corresponding to eigen 3.3.4.
+- Extract the content of the archive in `C:\workspace`.
+- We recommend to rename `C:\workspace\eigen-eigen-5a0156e40feb` in `C:\workspace\eigen-3.3.4`
+
+<b>2. Configure eigen</b>
+
+To build eigen from source with Visual C++ 2015 (vc14)
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image.
+\image html img-cmake-win10-msvc14-eigen-launch.png
+- Click then on "Configure" button.
+- Click on "Yes" to create the `C:/workspace/eigen-3.3.4/build-vc14` folder.
+- Select then your compiler "Visual Studio 14 2015 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc14-msvc-version.png
+- This will start CMake configuration.
+- Set `CMAKE_INSTALL_PREFIX` var to `C:/workspace/eigen-3.3.4/build-vc14/install` folder location.
+\image html img-cmake-win10-msvc14-eigen-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+- Once the generation is done, in `C:/workspace/eigen-3.3.4/build-vc14` folder you have the Visual Studio `Eigen3.sln` generated solution file.
+
+<b>3. Install eigen</b>
+
+- To install eigen just double click on `C:/workspace/eigen-3.3.4/build-vc14/Eigen3.sln` solution file. This action will open eigen project in Visual Studio C++ 14 2015.
+- Build `INSTALL` target knowing that "Debug" or "Release" configuration doesn't mather
+\image html img-win10-msvc14-eigen-build-install.png
+- After these steps, eigen is installed in `C:/workspace/eigen-3.3.4/build-vc14/install`
+
+<b>4. Complete eigen installation</b>
+
+Now you have to complete eigen installation setting some environment vars:
+- In order that ViSP detects eigen you have to set `EIGEN_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx Eigen3_DIR "C:\workspace\eigen-3.3.4\build-vc15\install"
+\endcode
+where `C:\workspace\eigen-3.3.4\build-vc14\install` is where you have installed eigen. Inside this folder you should have a file named `Eigen3Config.cmake`.
+- There is no need to set `PATH` environment var since eigen is a template that has no library.
+
+
+\section install_win10_msvc14_install_visp Install ViSP from source code
+\subsection install_win10_msvc14_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="https://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- You can also download a <a href="https://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+\subsection install_win10_msvc14_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution file that will be located in `C:/workspace/visp/visp-build-vc14`.
+
+- Launch CMake (cmake-gui) and complete the source code and binaries location as in the next image.
+\image html img-cmake-win10-msvc14-visp-launch.png
+- Click then on "Configure" button.
+\image html img-cmake-win10-msvc14-visp-create-folder.png
+- Click on "Yes" to create the `C:/workspace/visp/visp-build-vc14` folder.
+- Select then your compiler "Visual Studio 14 2015 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc14-msvc-version.png
+- This will start CMake configuration. As shown in the next image, Eigen, Flycapture, GDI (Graphical Device Interface), OpenCV, OpenMP, Pylon, Realsense, XML2 and Zbar 3rd parties are automatically detected.
+\image html img-cmake-win10-msvc14-visp-config.png
+\note Installation folder is set to `C:/workspace/visp/visp-build-vc14/install`. If you want to change the installation folder to `C:/Program Files (x86)/ViSP`, make sure that you have administrator privileges to write in that folder before modifying `CMAKE_INSTALL_PREFIX`.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win10-msvc14-visp-config2.png
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win10-msvc14-visp-generate.png
+
+- Once the generation is done, in `C:/workspace/visp/visp-build-vc14` folder you have the Visual Studio \c VISP.sln generated solution file.
+
+\subsection install_win10_msvc14_build Building ViSP from source
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc14/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+\image html img-win10-msvc14-visp-open.png
+
+- Enter menu "BUILD/Build Solution" to build ViSP or hit "Ctrl+Shift+B".
+\image html img-win10-msvc14-visp-build.png
+
+- At the end of the build process you should have the following indicating that all the build succeeded.
+\image html img-win10-msvc14-visp-build-end.png
+
+- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
+\image html img-win10-msvc14-visp-install.png
+
+- At the end of the installation, you should have the following.
+\image html img-win10-msvc14-visp-install-end.png
+
+- As shown in the previous image, all the headers but also the generated libraries are copied in `C:/workspace/visp/visp-build-vc14/install` folder; headers and libraries in `include` and `x64/vc14` subfolders respectively.
+
+- This ends ViSP installation with \c Debug configuration.
+
+- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
+\image html img-win10-msvc14-visp-release.png
+
+- Now, as previously, build and install ViSP again.
+
+- At the end, in `C:/workspace/visp/visp-build-vc14/install/x64/vc14/bin` folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+\image html img-win10-msvc14-visp-explorer-install.png
+
+
+\subsection install_win10_msvc14_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP library, open a \c cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\visp\visp-build-vc14\install\x64\vc14\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\opencv-3.3.1\build\x64\vc14\bin"
+\endcode
+
+Then to add the path to libxml2 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\libxml2-2.9.7\build-vc14\bin"
+\endcode
+
+Then to add the path to librealsense 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\librealsense-1.12.1\build-vc14\bin"
+\endcode
+
+Then to add the path to zbar 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\ZBarWin64\x64\Release"
+C:\> setx PATH "%PATH%;C:\workspace\ZBarWin64\zbar\libiconv\dll_x64"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned.
+
+\code
+C:\> echo %PATH%
+\endcode
+
+\section install_win10_msvc14_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named `visp-images-x.y.z.zip`. This archive could be downloaded from https://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download `visp-images-3.1.0.zip` from https://visp.inria.fr/download and uncompress it in your workspace `C:/workspace`.
+\image html img-win10-visp-images.png
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called `VISP_INPUT_IMAGE_PATH`. In our case, this variable should be set to `C:\workspace\visp-images-3.1.0`.
+\code
+C:\> setx VISP_INPUT_IMAGE_PATH C:\workspace\visp-images-3.1.0
+\endcode
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run `C:/worspace/visp/visp-build-vc14/example/device/display/Debug/displayGDI.exe`, open a command window, enter in the right folder, and run:
+
+\image html img-win10-msvc14-cmd-displayGDI.png
+
+\section install_win10_msvc14_tip Tip & tricks
+
+\section install_win10_msvc14_tip_msvc_select How to set a specific Visual Studio version
+
+If one of the following command lines are not working:
+
+\code
+C:\> nmake
+C:\> cl
+\endcode
+
+it may possible that you have more then one Visual Studio version installed on your system.
+
+To select Visual Studio 2015 (vc14) you may open a `cmd` terminal, enter in the corresponding Visual Studio folder and run `vcvarsall.bat`:
+
+\code
+C:\> cd C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC
+C:\> vcvarsall.bat
+\endcode
+
+
+\section install_win10_msvc14_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win10-msvc15-uwp.doc b/doc/tutorial/windows/tutorial-install-win10-msvc15-uwp.doc
new file mode 100644
index 0000000..b68d4c5
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win10-msvc15-uwp.doc
@@ -0,0 +1,79 @@
+/**
+
+\page tutorial-install-win10-msvc15-uwp Tutorial: Installation from source for UWP with Visual C++ 2017 (vc15)
+\tableofcontents
+
+In this tutorial you will learn how to build ViSP framework for Universal Windows Platform (UWP) to include ViSP functionalities in apps developments that target a wide range of devices including PC, mobile, Xbox, HoloLens, IoT, and Surface Hub.
+
+This tutorial has been tested on Windows 10 (64 bit), with CMake 3.9.4 and Visual Studio Community 2017 and is only working since ViSP 3.0.1.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section tutorial_install_msvc15_uwp_required Install required software
+
+- CMake that could be download at : http://www.cmake.org. Download the latest release for Windows win64-x64 platform (at the time this tuto was written it was the file \c cmake-3.9.4-win64-x64.msi). To install just double click on the msi file.
+- <a href="https://www.visualstudio.com/downloads/">Visual Studio Community 2017</a>. Make sure that the Universal Windows App Development Tools are selected from the optional features list. Without these tools, you won't be able to create your universal apps. After the installation, start Visual Studio and create an empty C++ project to install the common tools for Visual C++ 2017.
+
+\note If you have already Visual Studio 2017 installed and don't know if the Universal Windows App Development Tools are enabled see \ref install_msvc15_uwp_tips_msvc.
+
+After installing Visual Studio software, you need to <a href="https://msdn.microsoft.com/windows/uwp/get-started/enable-your-device-for-development">enable your Windows 10 device</a> for development.
+
+\section install_msvc15_uwp_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+\section install_msvc15_uwp_get_visp_source Get ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z`.
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z`.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+\section install_msvc15_uwp_build_without_opencv Build ViSP
+
+\subsection install_msvc15_uwp_build_ws_10 Build for Windows Store 10.0
+
+- Configure ViSP build for Windows Store 10 targeting a x86 platform, by opening a cmd terminal and running:
+\code
+C:\> cd C:\workspace\visp
+C:\> md visp-build-vc15-uwp-ws-10
+C:\> cd visp-build-vc15-uwp-ws-10
+C:\> cmake.exe -G "Visual Studio 15 2017" -DCMAKE_SYSTEM_NAME:String=WindowsStore -DCMAKE_SYSTEM_VERSION:String="10.0" -DCMAKE_VS_EFFECTIVE_PLATFORMS:String=x86 ../visp
+\endcode
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc15-uwp-ws-10/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++.
+
+- With Visual Studio build and install the solution. If everything goes right you will find ViSP libraries and headers in `C:/workspace/visp/visp-build-vc15-uwp-ws-10/install` folder.
+
+\section install_msvc15_uwp_tips Tips and tricks
+\subsection install_msvc15_uwp_tips_msvc How to add Universal Windows App Development Tools support
+
+In case you're trying to build Windows Store and Windows Phone 10.0 projects having only default Visual Studio 2017 installation you'll likely get the following error during \c cmake run:
+\code
+A Windows Store component with CMake requires both the Windows Desktop SDK
+as well as the Windows Store '10.0' SDK. Please make sure that you have
+both installed
+\endcode
+
+Resolution consists in:
+- Open the Start menu ("Windows" key)
+- Select "Visual Studio Installer"
+- Under Visual Studio Community 2017 section, click on "Modify"
+- Enable "Universal Windows App Development Tools" and "Windows 8.1 and Windows Phone 8.0/8.1 Tools"
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win10-msvc15.doc b/doc/tutorial/windows/tutorial-install-win10-msvc15.doc
new file mode 100644
index 0000000..3aa6d90
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win10-msvc15.doc
@@ -0,0 +1,379 @@
+/**
+
+\page tutorial-install-win10-msvc15 Tutorial: Installation from source for Windows with Visual C++ 2017 (vc15)
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows 10 with Visual C++. These steps have been tested on Windows 10 (64 bit), with CMake 3.9.4 and Visual Studio Community 2017.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win10_msvc15_required Install prerequisites
+
+- Visual C++. Visual Studio Community 2017 that includes Visual C++ could be downloaded from https://www.visualstudio.com/downloads/. During installation enable "Desktop development with C++" to get "Windows 10 SDK". This SDK allows usage of the Graphical Device Interface (GDI) useful to display images in a window thanks to vpDisplayGDI class.
+- CMake that could be download at : http://www.cmake.org. Download the latest release for Windows win64-x64 platform (at the time this tuto was written it was the file \c cmake-3.9.4-win64-x64.msi). To install just double click on the msi file.
+
+\section install_win10_msvc15_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+\section install_win10_msvc15_3rdparty Install 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="https://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install \ref install_win10_msvc15_3rdparty_opencv and \ref install_win10_msvc15_3rdparty_xml in the workspace. If you have an Intel Realsense depth camera you may also install \ref install_win10_msvc15_3rdparty_realsense. If your camera is a PointGrey you may install \ref install_win10_msvc15_3rdparty_flycapture, while if your [...]
+
+\subsection install_win10_msvc15_3rdparty_opencv OpenCV 3rd party
+
+<b>1. Get OpenCV</b>
+
+First you have to get OpenCV:
+- From http://opencv.org/releases.html download the latest OpenCV for Windows. In our case we got \c opencv-3.3.1-vc14.exe Win pack installer. The same procedure could be used to consider rather OpenCV 3.3.0 by downloading \c opencv-3.3.0-vc14.exe Win pack installer.
+- Extract the content of the archive in `C:\workspace`.
+\image html img-win10-extract-opencv.png
+- The installer extracted all the material in `C:\workspace\opencv`.
+- We strongly recommend to rename this folder to a name that contain OpenCV version like \c C:\\workspace\\opencv-3.3.1.
+
+OpenCV 3.3.1 windows installer contains pre-build libraries compatible with Visual C++ 2015 (vc14) in `C:\workspace\opencv-3.3.1\build\x64\vc14`. Since here you are using Visual C++ 2017 (vc15) you need to build yourself OpenCV from source and install the libraries in `C:\workspace\opencv-3.3.1\build\x64\vc15`.
+
+<b>2. Configure OpenCV</b>
+
+To build OpenCV from source with Visual C++ 2017 (vc15):
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image.
+\image html img-cmake-win10-msvc15-opencv-launch.png
+- Click then on "Configure" button.
+\image html img-cmake-win10-msvc15-opencv-folder.png
+- Click on "Yes" to create the `C:/workspace/opencv-3.3.1/build-vc15` folder.
+- Select then your compiler "Visual Studio 15 2017 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc15-msvc-version.png
+- This will start CMake configuration.
+- Modify `CMAKE_INSTALL_PREFIX` var to `C:/workspace/opencv-3.3.1/build` folder location.
+\image html img-cmake-win10-msvc15-opencv-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win10-msvc15-opencv-generate.png
+- Once the generation is done, in `C:/workspace/opencv-3.3.1/sources/build-vc15` folder you have the Visual Studio \c OpenCV.sln generated solution file.
+
+<b>3. Build OpenCV</b>
+
+- To build OpenCV just double click on `C:/workspace/opencv-3.3.1/sources/build-vc15/OpenCV.sln` solution file. This action will open OpenCV project in Visual Studio C++ 15 2017. As shown in the next image, by default the solution configuration is set to \c Debug.
+- Build `INSTALL` target for `Debug` configuration (on the right panel move the mouse on `INSTALL` target and right click to enter `Build INSTALL`
+\image html img-win10-msvc15-build-opencv-debug.png
+- Select `Release` configuration and build also `INSTALL` target for `Release`
+\image html img-win10-msvc15-build-opencv-release.png
+- After these steps, OpenCV libraries are installed in `C:/workspace/opencv-3.3.1/build/x64/vc15`
+
+<b>4. Complete OpenCV installation</b>
+
+Now you have to complete OpenCV installation setting some environment vars:
+- In order that ViSP detects OpenCV you have to set `OpenCV_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx OpenCV_DIR "C:\workspace\opencv-3.3.1\build"
+\endcode
+where `C:\workspace\opencv-3.3.1\build` is where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc15_env_var.
+
+
+\subsection install_win10_msvc15_3rdparty_xml libxml2 3rd party
+
+<b>1. Get libxml2</b>
+
+- Download the latest libxml2 release from ftp://xmlsoft.org/libxml2. At the time this tuto was written it was `libxml2-2.9.7.tag.gz` file.
+- Extract the content of the tarball in `C:\workspace`. [Winrar](https://www.win-rar.com) could be used to this end.
+- Here we suppose that you have libxml2 source code in `C:\workspace\libxml2-2.9.7` folder.
+
+<b>2. Install libxml2</b>
+
+- In the Windows "Start" menu, scroll to "Visual Studio 2017", and select "Visual Studio Command Prompt for native x64".
+- To build libxml2 from source enter the following instructions in the Visual Studio Command Prompt:
+\code
+C:\> cd C:\workspace\libxml2-2.9.7\win32
+C:\> cscript configure.js iconv=no ftp=no http=no compiler=msvc prefix=C:\workspace\libxml2-2.9.7\build-vc15
+C:\> nmake /f Makefile.msvc install
+\endcode
+- After these steps, libxml2 library could be found in `C:/workspace/libxml2-2.9.7/build/vc15/bin/libxml2.dll`
+
+<b>3. Complete libxml2 installation</b>
+
+- To complete the installation, set `XML2_DIR` environment variable:
+\code
+C:\> setx XML2_DIR "C:\workspace\libxml2-2.9.7\build-vc15"
+\endcode
+- You have also to add the location of libxml2 library in the PATH environment variable following the indications mentioned in \ref install_win10_msvc15_env_var.
+
+\subsection install_win10_msvc15_3rdparty_realsense librealsense 3rd party
+
+I you have an Intel RealSense Depth camera (F200, SR300, R200, LR200, ZR300, and D400 series cameras), you may install librealsense 1.12.1. Otherwise you can skip this section.
+
+\note There is also librealsense 2.x.y, but ViSP is not compatible yet with this version, that is why we recommend to install the last stable release 1.12.1.
+
+<b>1. Get librealsense 1.12.1</b>
+
+- Get librealsense 1.12.1 source code from github
+
+ C:\> cd C:\workspace
+ C:\> git clone https://github.com/IntelRealSense/librealsense.git librealsense-1.12.1
+ C:\> cd librealsense-1.12.1
+ C:\> git checkout tags/v1.12.1
+
+- Here we suppose that you have librealsense source code in `C:\workspace\librealsense-1.12.1` folder.
+
+<b>2. Configure librealsense 1.12.1</b>
+
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image to set the location of the build to `C:/workspace/librealsense-1.12.1/build-vc15`:
+\image html img-cmake-win10-msvc15-realsense-config.png
+- Click then on "Configure" button and accept the creation of the build directory by a click on "Yes" button.
+- Select then your compiler "Visual Studio 15 2017 Win64" and click on "Finish" button.
+- Modify `CMAKE_INSTALL_PREFIX` var to `C:/workspace/librealsense-1.12.1/build-vc15` folder location.
+\image html img-cmake-win10-msvc15-realsense-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+
+<b>3. Install librealsense 1.12.1</b>
+
+- To build librealsense just double click on `C:/workspace/librealsense-1.12.1/Realsense.sln` solution file. This action will open Realsense project in Visual Studio C++ 15 2017.
+- Select `Release` configuration and build `INSTALL` target
+\image html img-win10-msvc15-realsense-build.png
+- After these steps, librealsense library could be found in `C:/workspace/librealsense-1.12.1/build-vc15/bin/realsense.dll`
+
+<b>4. Complete librealsense 1.12.1 installation</b>
+
+- Set `REALSENSE_DIR` environment variable:
+\code
+C:\> setx REALSENSE_DIR "C:\workspace\librealsense-1.12.1\build-vc15"
+\endcode
+- You have also to add the location of librealsense library in the PATH environment variable following the indications mentioned in \ref install_win10_msvc15_env_var.
+
+\subsection install_win10_msvc15_3rdparty_flycapture FlyCapture 3rd party
+
+I you have a PointGrey USB 2.0, USB 3.0, GigE or FireWire camera (DragonFly2, Flea3...), you may install FlyCapture. Otherwise you can skip this section.
+
+Complete installation instructions are given [here](https://www.ptgrey.com/KB/10020). We recap hereafter the main instructions to install FlyCapture SDK under Windows:
+
+- Download the latest FlyCapture2 Full SDK for your camera from Point Grey’s [download site](https://eu.ptgrey.com/support/downloads). On the download page, select your product family and "Windows 10 64 bit" as operating system. At the time this tutorial we written we selected "FlyCapture 2.11.3.164 SDK - Windows (64 bit)" to download `FlyCapture_2.11.3.164_x64.exe` file.
+- Double-click the downloaded software executable and follow the installation instructions.
+- Select the correct driver for the camera when prompted by the installation instructions.
+- Reboot the system.
+
+\subsection install_win10_msvc15_3rdparty_pylon Pylon 3rd party
+
+I you have a Basler FireWire, Camera Link, GigE or USB3.0 camera, you may install Pylon SDK. Otherwise you can skip this section.
+
+To install Pylon:
+- visit [Basler Software Download page](https://www.baslerweb.com/en/support/downloads/software-downloads/) to download and install the SDK corresponding to your platform. At the time this tutorial we written we selected "Pylon 5.0.11 Camera Software Suite Windows" to download `Basler_pylon_5.0.11.10913.exe` file.
+- Double-click on downloaded file and follow the installation instructions.
+- Be carreful to select "Developer" profile during the installation in order to install Pylon SDK headers (.h) besides the dll.
+\image html img-win10-pylon-developer.png
+- Click then on "Next" button.
+- Select how your Basler camera is connected to the computer (USB, GigE, FireWire, Camera Link), then click on "Next" button.
+- Keep the default destination folder as `C:\Program Files\Basler\pylon 5\` and click on "Next" button
+- In the "Ready to install" panel, click on the "Install" button
+- Reboot the system.
+
+\subsection install_win10_msvc15_3rdparty_zbar zbar 3rd party
+
+If you want to work with QR codes (see \ref tutorial-detection-barcode), we recommend to install zbar 3rd party. Otherwise you can skip this section.
+\note Since ViSP 3.1.0 we provide also AprilTag detection capabilities that are available without 3rd party installation (see \ref tutorial-detection-apriltag).
+
+To install zbar you need to build the library from source. There is a fork of the library available from GitHub (<a href="https://github.com/dani4/ZBarWin64">https://github.com/dani4/ZBarWin64</a>) and adapted for compiling under Visual following the steps:
+
+<b>1. Get zbar</b>
+
+- Get ZBarWin64 source code from <a href="https://github.com/dani4/ZBarWin64">Github repository</a>. Open a command window and hit:
+\code
+C:\> cd C:\workspace
+C:\> git clone https://github.com/dani4/ZBarWin64
+\endcode
+
+<b>2. Build zbar</b>
+- Then open `C:\workspace\ZBarWin64\zbar64.sln` solution file with Visual Studio C++
+- Select x64 configuration and build zbar library in Release enter menu "BUILD/Build Solution" or hit "Ctrl+Shift+B".
+\image html img-win10-msvc15-zbar-build.png
+
+<b>3. Complete zbar installation</b>
+- To complete the installation, set `ZBAR_DIR` environment variable:
+\code
+C:\> setx ZBAR_DIR "C:\workspace\ZBarWin64"
+\endcode
+- You have also to add the location of zbar and iconv libraries in the PATH environment variable following the indications mentioned in \ref install_win10_msvc15_env_var.
+
+\subsection install_win10_msvc15_3rdparty_eigen eigen 3rd party
+
+Even if eigen is designed as a template we recommand to install the library with Visual Studio.
+
+<b>1. Get eigen</b>
+
+- Download the latest eigen release from http://eigen.tuxfamily.org. At the time this tutorial was written we downloaded `eigen-eigen-5a0156e40feb.tar.gz` tarball corresponding to eigen 3.3.4.
+- Extract the content of the archive in `C:\workspace`.
+- We recommend to rename `C:\workspace\eigen-eigen-5a0156e40feb` in `C:\workspace\eigen-3.3.4`
+
+<b>2. Configure eigen</b>
+
+To build eigen from source with Visual C++ 2017 (vc15)
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image.
+\image html img-cmake-win10-msvc15-eigen-launch.png
+- Click then on "Configure" button.
+- Click on "Yes" to create the `C:/workspace/eigen-3.3.4/build-vc15` folder.
+- Select then your compiler "Visual Studio 15 2017 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc15-msvc-version.png
+- This will start CMake configuration.
+- Set `CMAKE_INSTALL_PREFIX` var to `C:/workspace/eigen-3.3.4/build-vc15/install` folder location.
+\image html img-cmake-win10-msvc15-eigen-install.png
+- Click again on "Configure" button. All the red lines should disappear.
+- To finish the configuration, click on "Generate" button.
+- Once the generation is done, in `C:/workspace/eigen-3.3.4/build-vc15` folder you have the Visual Studio `Eigen3.sln` generated solution file.
+
+<b>3. Install eigen</b>
+
+- To install eigen just double click on `C:/workspace/eigen-3.3.4/build-vc15/Eigen3.sln` solution file. This action will open eigen project in Visual Studio C++ 15 2017.
+- Build `INSTALL` target knowing that "Debug" or "Release" configuration doesn't mather
+\image html img-win10-msvc15-eigen-build-install.png
+- After these steps, eigen is installed in `C:/workspace/eigen-3.3.4/build-vc15/install`
+
+<b>4. Complete eigen installation</b>
+
+Now you have to complete eigen installation setting some environment vars:
+- In order that ViSP detects eigen you have to set `EIGEN_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx Eigen3_DIR "C:\workspace\eigen-3.3.4\build-vc15\install"
+\endcode
+where `C:\workspace\eigen-3.3.4\build-vc15\install` is where you have installed eigen. Inside this folder you should have a file named `Eigen3Config.cmake`.
+- There is no need to set `PATH` environment var since eigen is a template that has no library.
+
+
+\section install_win10_msvc15_install_visp Install ViSP from source code
+\subsection install_win10_msvc15_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="https://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- You can also download a <a href="https://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+\subsection install_win10_msvc15_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution file that will be located in `C:/workspace/visp/visp-build-vc15`.
+
+- Launch CMake (cmake-gui) and complete the source code and binaries location as in the next image.
+\image html img-cmake-win10-msvc15-visp-launch.png
+- Click then on "Configure" button.
+\image html img-cmake-win10-msvc15-visp-create-folder.png
+- Click on "Yes" to create the `C:/workspace/visp/visp-build-vc15` folder.
+- Select then your compiler "Visual Studio 15 2017 Win64" and click on "Finish" button.
+\image html img-cmake-win10-msvc15-msvc-version.png
+- This will start CMake configuration. As shown in the next image, Eigen, Flycapture, GDI (Graphical Device Interface), OpenCV, OpenMP, Pylon, Realsense, XML2 and Zbar 3rd parties are automatically detected.
+\image html img-cmake-win10-msvc15-visp-config.png
+\note Installation folder is set to `C:/workspace/visp/visp-build-vc15/install`. If you want to change the installation folder to `C:/Program Files (x86)/ViSP`, make sure that you have administrator privileges to write in that folder before modifying `CMAKE_INSTALL_PREFIX`.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win10-msvc15-visp-config2.png
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win10-msvc15-visp-generate.png
+
+- Once the generation is done, in `C:/workspace/visp/visp-build-vc15` folder you have the Visual Studio \c VISP.sln generated solution file.
+
+\subsection install_win10_msvc15_build Building ViSP from source
+
+- To build ViSP just double click on `C:/workspace/visp/visp-build-vc15/VISP.sln` solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+\image html img-win10-msvc15-visp-open.png
+
+- Enter menu "BUILD/Build Solution" to build ViSP or hit "Ctrl+Shift+B".
+\image html img-win10-msvc15-visp-build.png
+
+- At the end of the build process you should have the following indicating that all the build succeeded.
+\image html img-win10-msvc15-visp-build-end.png
+
+- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
+\image html img-win10-msvc15-visp-install.png
+
+- At the end of the installation, you should have the following.
+\image html img-win10-msvc15-visp-install-end.png
+
+- As shown in the previous image, all the headers but also the generated libraries are copied in `C:/workspace/visp/visp-build-vc15/install` folder; headers and libraries in `include` and `x64/vc15` subfolders respectively.
+
+- This ends ViSP installation with \c Debug configuration.
+
+- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
+\image html img-win10-msvc15-visp-release.png
+
+- Now, as previously, build and install ViSP again.
+
+- At the end, in `C:/workspace/visp/visp-build-vc15/install/x64/vc15/bin` folder you have two versions of ViSP DLL libraries corresponding to ViSP modules; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+\image html img-win10-msvc15-visp-explorer-install.png
+
+\subsection install_win10_msvc15_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP library, open a \c cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\visp\visp-build-vc15\install\x64\vc15\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\opencv-3.3.1\build\x64\vc15\bin"
+\endcode
+
+Then to add the path to libxml2 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\libxml2-2.9.7\build-vc15\bin"
+\endcode
+
+Then to add the path to librealsense 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\librealsense-1.12.1\build-vc15\bin"
+\endcode
+
+Then to add the path to zbar 3rd party library location, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\ZBarWin64\x64\Release"
+C:\> setx PATH "%PATH%;C:\workspace\ZBarWin64\zbar\libiconv\dll_x64"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned.
+
+\code
+C:\> echo %PATH%
+\endcode
+
+\section install_win10_msvc15_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named `visp-images-x.y.z.zip`. This archive could be downloaded from https://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download `visp-images-3.1.0.zip` from https://visp.inria.fr/download and uncompress it in your workspace `C:/workspace`.
+\image html img-win10-visp-images.png
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called `VISP_INPUT_IMAGE_PATH`. In our case, this variable should be set to `C:\workspace\visp-images-3.1.0`.
+\code
+C:\> setx VISP_INPUT_IMAGE_PATH C:\workspace\visp-images-3.1.0
+\endcode
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run `C:/worspace/visp/visp-build-vc15/example/device/display/Debug/displayGDI.exe`, open a command window, enter in the right folder, and run:
+
+\image html img-win10-msvc15-cmd-displayGDI.png
+
+\section install_win10_msvc15_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win7-msvc11.doc b/doc/tutorial/windows/tutorial-install-win7-msvc11.doc
new file mode 100644
index 0000000..da95591
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win7-msvc11.doc
@@ -0,0 +1,170 @@
+/**
+
+\page tutorial-install-win7 Tutorial: Installation from source for Windows with Visual C++ 2012 (vc11)
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows with Visual C++. These steps have been tested on Windows 7 (64 bit), with CMake 3.3.2 and Visual Studio 2012 but should work with any other version as well.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win7_required Install prerequisities
+
+- Visual Studio C++. In this tutorial we use Visual Studio 2012. Note that ViSP can also be build with Visual Studio Express that could be downloaded from http://www.microsoft.com/visualstudio/eng/products/visual-studio-express-products
+\note If you want to use Visual Studio Express, you have to install <b>Express 2013 for Windows Desktop</b>. The \b Desktop version is important to work with CMake.
+- CMake 2.8.3 or higher that could be download at : http://www.cmake.org. To install CMake, download the latest binary distribution installer for Windows. You will find it under <a href="https://cmake.org/download/">"Windows (Win32 Installer)"</a>. Install CMake just by double clicking on the binary cmake-x.y.z-win32-x86.exe you downloaded.
+
+
+\section install_win7_3rdparty Install recommended 3rd party
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. We recommend to install the following:
+
+\subsection install_win7_3rdparty_opencv OpenCV 3rd party
+
+We recommend to install OpenCV.
+
+- From http://opencv.org/releases.html download the latest OpenCV for Windows version. In our case we install \c opencv-3.0.0.exe pre-build SDK in \c C:\\OpenCV. The installer opencv-3.0.0.exe extracted all the material in \c C:\\OpenCV\\opencv.
+\note OpenCV 3.0.0 windows installer contains pre-build libraries compatible with Visual C++ 2013 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc12 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc12 for the Win64 compiler) and also libraries compatible with Visual C++ 2012 (in \c C:\\OpenCV\\opencv\\build\\x86\\vc11 and in \c C:\\OpenCV\\opencv\\build\\x64\\vc11 for the Win64 compiler). With any other compiler version you need to build yourself OpenCV from source.
+
+\note There is an <a href="http://docs.opencv.org/doc/tutorials/introduction/windows_install/windows_install.html#windows-install-prebuild">OpenCV tutorial: Installation Using the Pre-build Libraries</a> that may help if you encounter some difficulties.
+
+- Now in order that ViSP detects OpenCV you have to set \c OpenCV_DIR environment variable. Start up a command window (in your "Start" menu click on "Run" and type in \c cmd.exe) and enter:
+\code
+C:\> setx OpenCV_DIR C:\OpenCV\opencv\build
+\endcode
+where \c C:\\OpenCV\\opencv\\build is where you have the build directory (extracted or built). Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win7_env_var.
+
+\section install_win7_install_visp Install ViSP from source code
+\subsection install_win7_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code:
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file using for example <a href="http://www.win-rar.com">WinRAR</a>
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\ViSP> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in a directory denoted \c \<source_dir\>, for example \c C:\\ViSP\\visp
+
+\subsection install_win7_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce a Visual Studio C++ solution that will be located in \c \<binary_dir\>, for example \c C:\\ViSP\\visp-build.
+
+- Launch CMake (cmake-gui) and complete the \c \<source_dir\> and \c \<binary_dir\> locations as in the next image.
+\image html img-cmake-win7-msvc-launch.jpg
+
+- Click then on "Configure" button.
+\image html img-cmake-win7-create-build-folder.jpg
+
+- Click on "Yes" to create the \c C:\\ViSP\\visp-build folder.
+- Select then your compiler, for example here Visual Studio 11 Win64, and click on "Finish" button.
+\image html img-cmake-win7-msvc-version.jpg
+
+- This will start CMake configuration. As shown in the next image, GDI (Graphical Device Interface), OpenCV and OpenMP 3rd parties are automatically detected.
+\image html img-cmake-win7-msvc-config.jpg
+
+- As given in the previous image, note also that the installation folder is set to \c C:\\ViSP\\visp-build\\install.
+\warning If you want to change the installation forder to \c C:/Program \c Files \c (x86)/ViSP, make sure that you have administrator privileges to write in that folder.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win7-msvc-config-end.jpg
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win7-generate.jpg
+
+- Once the generation is done, in `C:/ViSP/visp-build` folder you have the Visual Studio \c VISP.sln generated solution file.
+\image html img-cmake-win7-solution.jpg
+
+\subsection install_win7_build Building ViSP from source
+
+- To build ViSP just double click on \c C:/ViSP/visp-build/VISP.sln solution file. This action will open ViSP project in Visual Studio C++. As shown in the next image, by default, Visual Studio position the solution configuration to \c Debug.
+\image html img-win7-msvc-open.jpg
+
+- Enter menu "BUILD/Build Solution" to build ViSP.
+\image html img-win7-msvc-build.jpg
+
+- At the end of the build process you should have the following indicating that all the build succeeded.
+\image html img-win7-msvc-build-succeed.jpg
+
+- Now to install ViSP, build "INSTALL" project. To this end, apply a left click on "INSTALL" to select the project, then a right click to enter in the "Build" menu.
+\image html img-win7-msvc-install.jpg
+
+- At the end of the installation, you should have the following.
+\image html img-win7-msvc-install-succeed.jpg
+
+- As shown in the previous image, all the headers but also the generated library are copied in \c C:/ViSP/visp-build/install folder.
+
+- This ends ViSP installation with \c Debug configuration.
+
+- We recommend now to do the same with \c Release settings. As shown in the next image, select the \c Release configuration.
+\image html img-win7-msvc-release.jpg
+
+- Now, as previously, build and install ViSP again.
+
+- At the end, in \c C:/ViSP/visp-build/install/bin folder you have two versions of ViSP DLL libraries for each module; the one suffixed by "d" with debug information, the other one optimized with release compiler options.
+\image html img-win7-msvc-install-end.jpg
+
+\subsection install_win7_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP library, open a cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\ViSP\visp-build\install\x64\vc11\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\OpenCV\opencv\build\x64\vc11\bin"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned
+
+\code
+C:\> echo %PATH%
+\endcode
+
+
+\section install_win7_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named `visp-images-x.y.z.zip`. This archive could be downloaded from https://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download `visp-images-3.1.0.zip` from https://visp.inria.fr/download and uncompress it for example in `C:/ViSP`.
+
+\image html img-win7-ViSP-images.jpg
+
+We suppose now that the data are located in `C:/ViSP/visp-images-3.1.0`.
+\code
+C:\> dir ViSP/visp-images-3.1.0
+AprilTag LICENSE.txt calibration cube ellipse-1 line mbt-depth mire-2
+Klimt README.md circle ellipse iv mbt mire video
+\endcode
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called `VISP_INPUT_IMAGE_PATH`. In our case, this variable should be set to `C:\ViSP\visp-images-3.1.0`.
+\code
+C:\> setx VISP_INPUT_IMAGE_PATH C:\ViSP\visp-images-3.1.0
+\endcode
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run `C:/ViSP/ViSP-build/example/device/display/Debug/displayGDI.exe`, open a command window, enter in the right folder, and run:
+
+\image html img-win7-cmd-displayGDI.jpg
+
+\section install_win7_tip Tip and tricks
+
+\subsection install_win7_tip_git How to install Git
+
+We recommand to install Git for Windows from https://git-for-windows.github.io/. This installation allows then to use git in a cmd.exe command dos shell.
+
+\section install_win7_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/doc/tutorial/windows/tutorial-install-win8.1-mingw-w64.doc b/doc/tutorial/windows/tutorial-install-win8.1-mingw-w64.doc
new file mode 100644
index 0000000..e0611eb
--- /dev/null
+++ b/doc/tutorial/windows/tutorial-install-win8.1-mingw-w64.doc
@@ -0,0 +1,319 @@
+/**
+
+\page tutorial-install-win10-mingw64 Tutorial: Installation from source for Windows with Mingw-w64
+\tableofcontents
+
+In this tutorial you will learn how to install ViSP from source on Windows 8.1 with Mingw-w64. These steps have been tested:
+- on Windows 8.1 (64 bit), with CMake 3.3.2 and Mingw-w64 - GCC for Windows 64 & 32 bits
+- on windows 10 (64 bit), with CMake 3.10.0 and Mingw-w64 - GCC for Windows 64 & 32 bits.
+
+\note Concerning ViSP installation, we provide also other \ref tutorial.
+
+\section install_win10_mingw64_required Install prerequisities
+
+\subsection install_win10_mingw64_required_mingw Mingw-w64
+
+Mingw-w64 could be found following http://mingw-w64.org/doku.php/download. From that page, click on download "Win-builds" that will bring you to the page http://mingw-w64.org/doku.php/download/win-builds from where you can access to http://win-builds.org/doku.php/download_and_installation_from_windows. Here simply download and run the package manager, in our case we downloaded `win-builds-1.5.0.exe` file.
+
+- Once downloaded, double click on the the exe file. It will open an installation window.
+- Select x86_64 and MinGW installation folder `C:\mingw` as in the following image:
+\image html img-mingw64-installer-started.jpg
+\note It you want to install MinGW in an other folder, you have to set `MINGW_DIR` environment variable to your installation location in order to allow CMake to detect 3rd party libraries that come with MinGW installer. Fo example, if you install MinGW in `C:\folder\mingw-w64`, setting `MINGW_DIR` is simply done running in a cmd terminal:
+\code
+C:\> setx MINGW_DIR "C:\folder\mingw-w64"
+\endcode
+- Click on "OK" button. It will open an other window:
+\image html img-mingw64-installer-process.jpg
+- Where you can click on "Process" button to start the installation
+- A window shows the progression of the installation. When all the packages are installed you can close all the installation windows.
+- To finish the installation, just add `C:\mingw\bin` folder to the PATH variable. To this end open a cmd terminal and do the following:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\mingw\bin"
+\endcode
+- Close and re-open a cmd terminal, then run again
+\code
+C:\> echo %PATH%
+\endcode
+to check that `C:\mingw\bin` was added.
+
+\subsection install_win10_mingw64_required_cmake CMake
+
+CMake could be download at : http://www.cmake.org.
+- Download the latest release for Windows win64-x64 platform (at the time this tuto was written it was the file \c cmake-3.10.0-rc3-win64-x64.msi). To install just double click on the msi file.
+
+\section install_win10_mingw64_ws Create a workspace
+
+First create a workspace that will contain all ViSP and optional 3rd parties. This workspace is here set to \c C:\\workspace folder, but it could be set to any other location.
+To create the workspace:
+- Open a \c cmd terminal (in your "Start" menu click on "Run" and type in \c cmd.exe) and run:
+\code
+C:\> mkdir C:\workspace
+\endcode
+
+
+\section install_win10_mingw64_3rdparty Install recommended 3rd parties
+
+ViSP is interfaced with several 3rd party libraries. The <a href="http://visp.inria.fr/software-architecture">complete list is provided here</a>. Some of them are automatically detected since they are part of MinGW distribution. Other may be installed from source. We recommend to install the following:
+
+\subsection install_win10_mingw64_3rdparty_opencv OpenCV 3rd party
+
+<b>1. Get OpenCV</b>
+
+First you have to get OpenCV:
+- From http://opencv.org/releases.html download the latest OpenCV for Windows. In our case we got \c opencv-3.3.0-vc14.exe Win pack installer.
+- Extract the content of the archive in `C:\workspace`.
+\image html img-win10-extract-opencv.png
+- The installer extracted all the material in `C:\workspace\opencv`.
+- We strongly recommend to rename this folder to a name that contain OpenCV version like \c C:\\workspace\\opencv-3.3.0.
+
+OpenCV 3.3.0 windows installer contains pre-build libraries compatible with Visual C++ 2015 (vc14) in `C:\workspace\opencv-3.3.0\build\x64\vc14`. Since here you are using MinGW you need to build yourself OpenCV from source and install the libraries in `C:\workspace\opencv-3.3.0\build\x64\mingw`.
+
+<b>2. Configure OpenCV</b>
+
+To build OpenCV from source with MinGW:
+- Launch CMake (cmake-gui), complete the source code and build locations as in the next image.
+\image html img-cmake-win10-mingw-opencv-launch.png
+- Click then on "Configure" button.
+- Click on "Yes" to create `C:\workspace\opencv-3.3.0\sources\build-mingw` folder.
+- Select then "MinGW Makefiles" and click on "Finish" button.
+- Modify "CMAKE_INSTALL_PREFIX" to `C:\workspace\opencv-3.3.0\build` folder. This is the location of the pre-build libraries that come with the installer. Doing that, allows to install OpenCV libraries in a same parent folder.
+\image html img-cmake-win10-mingw-opencv-install.png
+- Click then on "Configure" button to remove the red lines.
+- Click on "Generate" button to generate the Makefiles for MinGW.
+
+<b>3. Build OpenCV</b>
+
+- Open a cmd terminal, enter in the `C:\workspace\opencv-3.3.0\sources\build-mingw` folder and start `mingw32-make`
+\code
+C:\> cd C:\workspace\opencv-3.3.0\sources\build-mingw
+C:\> mingw32-make -j4
+\endcode
+\note Depending on OpenCV version you may encounter the following issues, for which we propose work arrounds; \ref issue_win10_mingw_opencv_precompiled_headers, \ref issue_win10_mingw_opencv_cap_dshow, \ref issue_win10_mingw_opencv_dtor, \ref issue_win10_mingw_opencv_tiff, or \ref issue_win10_mingw_opencv_ipp.
+
+- Now install OpenCV running:
+\code
+C:\> mingw32-make install
+\endcode
+
+<b>4. Complete OpenCV installation</b>
+
+Now you have to complete OpenCV installation setting some environment vars:
+- In order that ViSP detects OpenCV you have to set `OpenCV_DIR` environment variable. Start up a \c cmd terminal and enter:
+\code
+C:\> setx OpenCV_DIR C:\workspace\opencv-3.3.0\build
+\endcode
+where `C:\workspace\opencv-3.3.0\build` is where you have installed OpenCV. Inside this folder you should have a file named \c OpenCVConfig.cmake.
+
+- You have also to add the location of OpenCV libraries in the PATH environment variable following the indications mentioned in \ref install_win10_mingw64_env_var.
+
+\section install_win10_mingw64_install_visp Install ViSP from source code
+\subsection install_win10_mingw64_get_source Getting ViSP source code
+
+There are different ways to get ViSP source code.
+
+- You can download the <a href="http://visp.inria.fr/download">latest release</a> as a zip or a tarball. Once visp-x.y.z.tar.gz or visp-x.y.z.zip is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- You can also download a <a href="http://visp.inria.fr/download#snapshot">daily snapshot</a>. Once visp-snapshot-yyyy-mm-dd.tar.gz is downloaded, uncompress the file in `C:\workspace\visp\visp-x.y.z` using for axample <a href="http://www.win-rar.com">WinRAR</a>.
+
+- Or you get the cutting-edge ViSP from <a href="https://github.com/lagadic/visp">GitHub repository</a> using the git command line (see \ref install_win7_tip_git):
+\code
+C:\> mkdir C:\workspace\visp
+C:\> cd C:\workspace\visp
+C:\> git clone https://github.com/lagadic/visp.git
+\endcode
+
+We suppose now that ViSP source is in `C:\workspace\visp\visp`.
+
+\subsection install_win10_mingw64_config Configuring ViSP from source
+
+The goal of the configuration step is now to use CMake to produce MinGW Makefiles in `C:/workspace/visp/visp-build-mingw`.
+
+- Launch CMake (cmake-gui) and complete the source code and binaries location as in the next image.
+\image html img-cmake-win10-mingw-visp-launch.png
+- Click then on "Configure" button.
+\image html img-cmake-win10-mingw-visp-create-folder.png
+- Click on "Yes" to create the `C:/workspace/visp/visp-build-mingw` folder.
+- Select then your compiler "MinGW Makefiles" and click on "Finish" button.
+\image html img-cmake-win10-mingw-makefiles.png
+- This will start CMake configuration. As shown in the next image, OpenCV, GDI (Graphical Device Interface), libjpeg, libpng, pthread and libxml2 3rd party are automatically detected.
+\image html img-cmake-win10-mingw-visp-config.png
+\note If OpenCV is not detected, you may encounter the following issue \ref issue_win10_mingw_opencv_not_detected.
+
+\note Installation folder is set to `C:/workspace/visp/visp-build-mingw/install`. If you want to change the installation folder to `C:/Program Files (x86)/ViSP`, make sure that you have administrator privileges to write in that folder before modifying `CMAKE_INSTALL_PREFIX`.
+
+- Click then on "Configure" button. All the red lines should disappear.
+\image html img-cmake-win10-mingw-visp-config2.png
+\note The default configuration lead to the creation of a shared library (with \c .dll extension). This is the default configuration that is recommended. If you want to create rather a static library (with \c .lib extension) you have to uncheck the \c BUILD_SHARED_LIBS option to disable DLL creation.
+
+- To finish the configuration, click on "Generate" button.
+\image html img-cmake-win10-mingw-visp-generate.png
+
+- Once the generation is done, in `C:/workspace/visp/visp-build-mingw` folder you have the Makefile file that will be used by MinGW to build the whole project.
+
+\subsection install_win10_mingw64_build Building ViSP from source
+
+- To build ViSP, open a `cmd` terminal, change to `C:\workspace\visp\visp-build-mingw` folder and run `mingw32-make`:
+\code
+C:\> cd C:\workspace\visp\visp-build-mingw
+C:\> mingw32-make -j4
+\endcode
+
+- Now to install ViSP, in the same `cmd` terminal run:
+\code
+C:\> mingw32-make -j4 install
+\endcode
+
+- At the end, in `C:/workspace/visp/visp-build-mingw/install/x64/mingw/bin` folder you will find ViSP DLL libraries corresponding to the build modules.
+\note When `CMAKE_BUILD_TYPE` is set to Debug, the library names are suffixed by "d".
+
+\subsection install_win10_mingw64_env_var Setting up PATH variable
+
+If you built static libraries then you are done. Otherwise, if you follow this tutorial step by step you need to add the bin folders path to the systems path. This is because you will use ViSP and OpenCV libraries in form of "Dynamic-link libraries" (also known as DLL). Inside these are stored all the algorithms and information the libraries contains. The operating system will load them only on demand, during runtime. However, to do this he needs to know where they are. The systems \c PA [...]
+
+To modify the PATH var and add the path to ViSP libraries, open a cmd terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\visp\visp-build-mingw\install\x64\mingw\bin"
+\endcode
+
+Then to add the path to OpenCV 3rd party libraries, close and re-open a cmd-terminal and run:
+\code
+C:\> echo %PATH%
+C:\> setx PATH "%PATH%;C:\workspace\opencv-3.3.0\build\x64\mingw\bin"
+\endcode
+
+Then close and re-open a cmd terminal to check if the PATH var was well positioned
+
+\code
+C:\> echo %PATH%
+\endcode
+
+
+\section install_win10_mingw64_dataset Install ViSP dataset
+
+Some ViSP examples and tests require data (images, video, models) that are not part of ViSP source code but available in a separate archive named \c ViSP-images-x.y.z.zip. This archive could be downloaded from http://visp.inria.fr/download page. We provide here after the way to install these data if you want to run ViSP examples.
+
+Download `visp-images-3.1.0.zip` from http://visp.inria.fr/download and uncompress it in your workspace `C:/workspace`.
+\image html img-win10-visp-images.png
+
+ViSP examples and tests are able to detect automatically the location of the requested data if you position an environment variable called `VISP_INPUT_IMAGE_PATH`. In our case, this variable should be set to `C:\workspace\visp-images-3.1.0`.
+\code
+C:\> setx VISP_INPUT_IMAGE_PATH C:\workspace\visp-images-3.1.0
+\endcode
+
+From now, you can try to run ViSP examples and tests. For example, if you want to run `C:/worspace/visp/visp-build-mingw/example/device/display/Debug/displayGDI.exe`, open a command window, enter in the right folder, and run:
+
+\image html img-win10-mingw-cmd-displayGDI.png
+
+\section issue_win10_mingw Known issues
+
+\subsection issue_win10_mingw_opencv_endianness OpenCV endianness failure during CMake configuration
+
+\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
+
+If you encounter the following issue during CMake configuration
+\image html img-opencv-issue-test-big-endian.jpg
+
+Edit `C:\workspace\opencv-2.y.z\sources\CMakeLists.txt` file, and line 464 replace:
+\code
+test_big_endian(WORDS_BIGENDIAN)
+\endcode
+by:
+\code
+#test_big_endian(WORDS_BIGENDIAN)
+set(WORDS_BIGENDIAN 0)
+\endcode
+
+\subsection issue_win10_mingw_opencv_tiff OpenCV build error: cannot build with tiff support
+
+\note This issue occurs with OpenCV 2.4.10, 2.3.0-beta and 2.3.0 releases.
+
+If you encounter a build issue during libtiff build as given in the next image:
+\image html img-opencv-issue-tiff.jpg
+
+- Open CMake GUI on OpenCV, turn BUILD_TIFF=OFF and also WITH_TIFF=OFF
+- Click on "Configure" button, and then on "Generate" one.
+- Build again OpenCV using
+\code
+cd C:\workspace\opencv-2.y.z\sources\build-mingw
+C:\> mingw32-make
+\endcode
+
+\subsection issue_win10_mingw_opencv_ipp OpenCV link error: cannot find -lRunTmChk
+
+\note This issue occurs with OpenCV 2.3.0-beta and 2.3.0 releases.
+
+The following image shows the link issue that may appear when building OpenCV with MinGW:
+\image html img-opencv-issue-ipp.jpg
+
+A work arround is to configure OpenCV without ipp support turning WITH_IPP=OFF and then trying to build again.
+
+\subsection issue_win10_mingw_opencv_dtor OpenCV build error: struct has virtual functions and accessible non-virtual destructor
+
+This error that occurs with OpenCV 3.0.0 during \c cap_dshow.cpp build is known and reported as an issue in https://github.com/Itseez/opencv/pull/5282/commits.
+\image html img-win8.1-mingw-opencv-issue-dtor.jpg
+
+- The fix consists in modifying \c modules/videoio/src/cap_dshow.cpp by adding near line 96:
+\code
+#ifdef __MINGW32__
+// MinGW does not understand COM interfaces
+#pragma GCC diagnostic ignored "-Wnon-virtual-dtor"
+#endif
+\endcode
+
+\subsection issue_win10_mingw_opencv_not_detected OpenCV not detected with Mingw build
+
+\note This issue occurs with OpenCV 2.4.9, 2.4.10 and 2.3.0-beta.
+
+- To fix this issue, edit `C:\workspace\opencv-2.y.z\sources\cmake\OpenCVConfig.cmake`, and line 89 replace:
+\code
+ if(CMAKE_OPENCV_GCC_TARGET_MACHINE MATCHES "64")
+\endcode
+by:
+\code
+ if(OPENCV_GCC_TARGET_MACHINE MATCHES "64")
+\endcode
+
+- Then open a new cmd terminal to build and install OpenCV again:
+\code
+C:\> cd C:\workspace\opencv-2.y.z\sources\build-mingw
+C:\> mingw32-make install
+\endcode
+
+\subsection issue_win10_mingw_opencv_precompiled_headers windres.exe invalid option --W
+
+\note This issue occurs with OpenCV 3.3.0.
+
+When running `mingw32-make` if you get the following issue:
+\image html img-win10-mingw-opencv-issue-precompiled-headers.png
+
+the workarround consists in:
+- opening cmake-gui and turning `ENABLE_PRECOMPILED_HEADERS` cmake var OFF:
+\image html img-cmake-win10-mingw-opencv-issue-precompiled-headers.png
+- in cmake-gui press "Configure" button, then press "Generate" button
+- then in the `cmd` terminal run again `mingw32-make`
+
+\subsection issue_win10_mingw_opencv_cap_dshow Build error in cap_dshow.cpp
+
+\note This issue occurs with OpenCV 3.3.0.
+
+When running `mingw32-make` if you get the following issue:
+\image html img-win10-mingw-opencv-issue-cap-dshow.png
+
+the workarround consists in:
+- editing `C:/workspace/opencv-3.3.0/sources/modules/videoio/src/cap_dshow.cpp` adding
+\code
+#define NO_DSHOW_STRSAFE
+\endcode
+before the line
+\code
+#include "DShow.h"
+\endcode
+- then in the `cmd` terminal run again `mingw32-make`
+
+\section install_win10_mingw_next Next tutorial
+
+You are now ready to see the next \ref tutorial-getting-started that will show you how to use ViSP as a 3rd party to build your own project.
+
+*/
diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt
index 8ec8e76..79897de 100644
--- a/example/CMakeLists.txt
+++ b/example/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -87,6 +88,7 @@ visp_add_subdirectory(robot-simulator/viper850 REQUIRED_DEPS visp_core visp_vs v
visp_add_subdirectory(servo-afma4 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
visp_add_subdirectory(servo-afma6 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
visp_add_subdirectory(servo-biclops REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
+visp_add_subdirectory(servo-franka REQUIRED_DEPS visp_core visp_robot)
visp_add_subdirectory(servo-pioneer REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
visp_add_subdirectory(servo-ptu46 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_gui)
visp_add_subdirectory(servo-viper650 REQUIRED_DEPS visp_core visp_blob visp_vs visp_robot visp_sensor visp_vision visp_gui)
diff --git a/example/calibration/CMakeLists.txt b/example/calibration/CMakeLists.txt
index 789317d..90213db 100644
--- a/example/calibration/CMakeLists.txt
+++ b/example/calibration/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/calibration/calibrateTsai.cpp b/example/calibration/calibrateTsai.cpp
index 567140e..d3b521b 100644
--- a/example/calibration/calibrateTsai.cpp
+++ b/example/calibration/calibrateTsai.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,85 +36,91 @@
*
*****************************************************************************/
-
/*!
\example calibrateTsai.cpp
- \brief Example of Tsai calibration to estimate extrinsic camera parameters, ie hand-eye homogeneous transformation.
+ \brief Example of Tsai calibration to estimate extrinsic camera parameters,
+ ie hand-eye homogeneous transformation.
*/
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
#include <vector>
#include <visp3/core/vpDebug.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/vision/vpCalibration.h>
-#include <visp3/core/vpExponentialMap.h>
int main()
{
try {
- // We want to calibrate the hand to eye extrinsic camera parameters from 6 couple of poses: cMo and wMe
+ // We want to calibrate the hand to eye extrinsic camera parameters from 6
+ // couple of poses: cMo and wMe
const unsigned int N = 6;
// Input: six couple of poses used as input in the calibration proces
- std::vector<vpHomogeneousMatrix> cMo(N) ; // eye (camera) to object transformation. The object frame is attached to the calibrartion grid
- std::vector<vpHomogeneousMatrix> wMe(N) ; // world to hand (end-effector) transformation
+ std::vector<vpHomogeneousMatrix> cMo(N); // eye (camera) to object
+ // transformation. The object
+ // frame is attached to the
+ // calibrartion grid
+ std::vector<vpHomogeneousMatrix> 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
+ // Initialize an eMc transformation used to produce the simulated input
+ // transformations cMo and wMe
vpTranslationVector etc(0.1, 0.2, 0.3);
vpThetaUVector erc;
- erc[0] = vpMath::rad(10); // 10 deg
+ erc[0] = vpMath::rad(10); // 10 deg
erc[1] = vpMath::rad(-10); // -10 deg
- erc[2] = vpMath::rad(25); // 25 deg
+ erc[2] = vpMath::rad(25); // 25 deg
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::endl;
+ 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::endl;
- vpColVector v_c(6) ; // camera velocity used to produce 6 simulated poses
- for (unsigned int i=0 ; i < N ; i++)
- {
- v_c = 0 ;
- if (i==0) {
+ vpColVector v_c(6); // camera velocity used to produce 6 simulated poses
+ for (unsigned int i = 0; i < N; i++) {
+ v_c = 0;
+ if (i == 0) {
// Initialize first poses
cMo[0].buildFrom(0, 0, 0.5, 0, 0, 0); // z=0.5 m
- wMe[0].buildFrom(0, 0, 0, 0, 0, 0); // Id
- }
- else if (i==1)
- v_c[3] = M_PI/8 ;
- else if (i==2)
- v_c[4] = M_PI/8 ;
- else if (i==3)
- v_c[5] = M_PI/10 ;
- else if (i==4)
- v_c[0] = 0.5 ;
- else if (i==5)
- v_c[1] = 0.8 ;
+ wMe[0].buildFrom(0, 0, 0, 0, 0, 0); // Id
+ } else if (i == 1)
+ v_c[3] = M_PI / 8;
+ else if (i == 2)
+ v_c[4] = M_PI / 8;
+ else if (i == 3)
+ v_c[5] = M_PI / 10;
+ else if (i == 4)
+ v_c[0] = 0.5;
+ else if (i == 5)
+ v_c[1] = 0.8;
- vpHomogeneousMatrix cMc; // camera displacement
- cMc = vpExponentialMap::direct(v_c) ; // Compute the camera displacement due to the velocity applied to the camera
+ vpHomogeneousMatrix cMc; // camera displacement
+ cMc = vpExponentialMap::direct(v_c); // Compute the camera displacement
+ // due to the velocity applied to
+ // the camera
if (i > 0) {
// From the camera displacement cMc, compute the wMe and cMo matrices
- cMo[i] = cMc.inverse() * cMo[i-1];
- wMe[i] = wMe[i-1] * eMc * cMc * eMc.inverse();
+ cMo[i] = cMc.inverse() * cMo[i - 1];
+ wMe[i] = wMe[i - 1] * eMc * cMc * eMc.inverse();
}
}
if (0) {
- for (unsigned int i=0 ; i < N ; i++) {
+ for (unsigned int i = 0; i < N; i++) {
vpHomogeneousMatrix wMo;
wMo = wMe[i] * eMc * cMo[i];
- std::cout << std::endl << "wMo[" << i << "] " << std::endl ;
- std::cout << wMo << std::endl ;
- std::cout << "cMo[" << i << "] " << std::endl ;
- std::cout << cMo[i] << std::endl ;
- std::cout << "wMe[" << i << "] " << std::endl ;
- std::cout << wMe[i] << std::endl ;
+ std::cout << std::endl << "wMo[" << i << "] " << std::endl;
+ std::cout << wMo << std::endl;
+ std::cout << "cMo[" << i << "] " << std::endl;
+ std::cout << cMo[i] << std::endl;
+ std::cout << "wMe[" << i << "] " << std::endl;
+ std::cout << wMe[i] << std::endl;
}
}
@@ -122,18 +129,18 @@ int main()
// Compute the eMc hand to eye transformation from six poses
// - cMo[6]: camera to object poses as six homogeneous transformations
- // - wMe[6]: world to hand (end-effector) poses as six homogeneous transformations
- vpCalibration::calibrationTsai(cMo, wMe, eMc) ;
+ // - wMe[6]: world to hand (end-effector) poses as six homogeneous
+ // transformations
+ vpCalibration::calibrationTsai(cMo, wMe, eMc);
- std::cout << std::endl << "Output: hand to eye calibration result: eMc estimated " << std::endl ;
- std::cout << eMc << 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::endl;
- return 0 ;
- }
- catch(vpException &e) {
+ std::cout << "Theta U rotation: " << vpMath::deg(erc[0]) << " " << vpMath::deg(erc[1]) << " " << vpMath::deg(erc[2])
+ << std::endl;
+ return 0;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
- return 1 ;
+ return 1;
}
}
-
diff --git a/example/calibration/camera_calibration.cpp b/example/calibration/camera_calibration.cpp
index 224d409..6630e7f 100644
--- a/example/calibration/camera_calibration.cpp
+++ b/example/calibration/camera_calibration.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,22 +41,22 @@
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
-#include <opencv2/core/core.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
#include <visp3/vision/vpCalibration.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpVideoReader.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -70,14 +71,13 @@ public:
squareSize = 0.025f;
goodInput = false;
tempo = 1.f;
-
}
- enum Pattern { UNDEFINED, CHESSBOARD, CIRCLES_GRID};
+ enum Pattern { UNDEFINED, CHESSBOARD, CIRCLES_GRID };
- bool read(const std::string &filename) // Read the parameters
+ bool read(const std::string &filename) // Read the parameters
{
// reading configuration file
- if (! vpIoTools::loadConfigFile(filename) )
+ if (!vpIoTools::loadConfigFile(filename))
return false;
vpIoTools::readConfigVar("BoardSize_Width:", boardSize.width);
vpIoTools::readConfigVar("BoardSize_Height:", boardSize.height);
@@ -107,12 +107,14 @@ public:
goodInput = false;
}
- if (input.empty()) // Check for valid input
+ if (input.empty()) // Check for valid input
goodInput = false;
calibrationPattern = UNDEFINED;
- if (patternToUse.compare("CHESSBOARD") == 0) calibrationPattern = CHESSBOARD;
- else if (patternToUse.compare("CIRCLES_GRID") == 0) calibrationPattern = CIRCLES_GRID;
+ if (patternToUse.compare("CHESSBOARD") == 0)
+ calibrationPattern = CHESSBOARD;
+ else if (patternToUse.compare("CIRCLES_GRID") == 0)
+ calibrationPattern = CIRCLES_GRID;
if (calibrationPattern == UNDEFINED) {
std::cerr << " Inexistent camera calibration mode: " << patternToUse << std::endl;
goodInput = false;
@@ -120,11 +122,15 @@ public:
}
public:
- 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
+ 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
bool goodInput;
private:
@@ -132,21 +138,20 @@ private:
};
#endif
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string outputFileName = "camera.xml";
Settings s;
const std::string inputSettingsFile = argc > 1 ? argv[1] : "default.cfg";
- if (! s.read(inputSettingsFile) ) {
+ if (!s.read(inputSettingsFile)) {
std::cout << "Could not open the configuration file: \"" << inputSettingsFile << "\"" << std::endl;
std::cout << std::endl << "Usage: " << argv[0] << " <configuration file>.cfg" << std::endl;
return -1;
}
- if (! s.goodInput)
- {
+ if (!s.goodInput) {
std::cout << "Invalid input detected. Application stopping. " << std::endl;
return -1;
}
@@ -170,16 +175,15 @@ int main(int argc, const char ** argv)
std::vector<vpPoint> model;
std::vector<vpCalibration> calibrator;
- for (int i=0; i< s.boardSize.height; i++) {
- for (int j=0; j< s.boardSize.width; j++) {
- model.push_back( vpPoint(j*s.squareSize, i*s.squareSize, 0) );
+ for (int i = 0; i < s.boardSize.height; i++) {
+ for (int j = 0; j < s.boardSize.width; j++) {
+ model.push_back(vpPoint(j * s.squareSize, i * s.squareSize, 0));
}
}
- long frame_index;
- while(! reader.end()) {
- frame_index = reader.getFrameIndex();
+ while (!reader.end()) {
reader.acquire(I);
+ long frame_index = reader.getFrameIndex();
vpDisplay::display(I);
cv::Mat cvI;
@@ -187,20 +191,22 @@ int main(int argc, const char ** argv)
vpImageConvert::convert(I, cvI);
bool found = false;
- switch( s.calibrationPattern ) // Find feature points on the input format
+ switch (s.calibrationPattern) // Find feature points on the input format
{
case Settings::CHESSBOARD:
- //std::cout << "Use chessboard " << std::endl;
- found = findChessboardCorners( cvI, s.boardSize, pointBuf,
+ // 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_NORMALIZE_IMAGE);
+ 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_NORMALIZE_IMAGE);
+ CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
+ CV_CALIB_CB_NORMALIZE_IMAGE);
#endif
- break;
+ break;
case Settings::CIRCLES_GRID:
- //std::cout << "Use circle grid " << std::endl;
- found = findCirclesGrid( cvI, s.boardSize, pointBuf, cv::CALIB_CB_SYMMETRIC_GRID );
+ // std::cout << "Use circle grid " << std::endl;
+ found = findCirclesGrid(cvI, s.boardSize, pointBuf, cv::CALIB_CB_SYMMETRIC_GRID);
break;
case Settings::UNDEFINED:
default:
@@ -214,23 +220,23 @@ int main(int argc, const char ** argv)
else
std::cout << ", image used as input data" << std::endl;
- if ( found) // If done with success,
+ if (found) // If done with success,
{
std::vector<vpImagePoint> data;
if (s.calibrationPattern == Settings::CHESSBOARD) {
// improve the found corners' coordinate accuracy for chessboard
- cornerSubPix( cvI, pointBuf, cv::Size(11,11),
- cv::Size(-1,-1),
+ 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
}
- char title[20]; sprintf(title, "image %ld", frame_index);
- vpDisplay::setTitle(I, title);
- for (unsigned int i=0; i < pointBuf.size(); i++) {
+ std::stringstream ss;
+ ss << "image " << frame_index;
+ vpDisplay::setTitle(I, ss.str());
+ for (unsigned int i = 0; i < pointBuf.size(); i++) {
vpImagePoint ip(pointBuf[i].y, pointBuf[i].x);
data.push_back(ip);
vpDisplay::displayCross(I, ip, 10, vpColor::red);
@@ -240,7 +246,7 @@ int main(int argc, const char ** argv)
vpCalibration calib;
calib.setLambda(0.5);
calib.clearPoint();
- for (unsigned int i=0; i<model.size(); i++) {
+ 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]);
}
vpHomogeneousMatrix cMo;
@@ -249,12 +255,12 @@ int main(int argc, const char ** argv)
// Set (u0,v0) in the middle of the image
double px = cam.get_px();
double py = cam.get_px();
- double u0 = I.getWidth()/2;
- double v0 = I.getHeight()/2;
+ double u0 = I.getWidth() / 2;
+ double v0 = I.getHeight() / 2;
cam.initPersProjWithoutDistortion(px, py, u0, v0);
if (calib.computeCalibration(vpCalibration::CALIB_VIRTUAL_VS, cMo, cam, false) == 0) {
- //std::cout << "camera parameters: " << cam << std::endl;
+ // std::cout << "camera parameters: " << cam << std::endl;
calibrator.push_back(calib);
}
}
@@ -268,10 +274,9 @@ int main(int argc, const char ** argv)
vpDisplay::displayText(I, 35, 15, "A click to process the next image", vpColor::green);
vpDisplay::flush(I);
vpDisplay::getClick(I);
- }
- else {
+ } else {
vpDisplay::flush(I);
- vpTime::wait(s.tempo*1000);
+ vpTime::wait(s.tempo * 1000);
}
}
@@ -287,7 +292,7 @@ int main(int argc, const char ** argv)
ss_additional_info << "<nb_calibration_images>" << calibrator.size() << "</nb_calibration_images>";
ss_additional_info << "<calibration_pattern_type>";
- switch(s.calibrationPattern) {
+ switch (s.calibrationPattern) {
case Settings::CHESSBOARD:
ss_additional_info << "Chessboard";
break;
@@ -316,19 +321,24 @@ int main(int argc, const char ** argv)
#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 << "A file with the same name exists. Remove it to be able to save the parameters..." << 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;
}
#endif
- }
- else
+ } 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) {
+ 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 << "<with_distortion>" << error << "</with_distortion></global_reprojection_error>";
@@ -336,31 +346,32 @@ int main(int argc, const char ** argv)
#ifdef VISP_HAVE_XML2
vpXmlParserCamera xml;
- 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;
+ 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;
else {
- 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;
+ 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;
}
#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;
+ 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;
- }
- else
+ } else
std::cout << "Calibration with distortion failed." << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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/CMakeLists.txt b/example/coin-simulator/CMakeLists.txt
index e3b5f51..419b38f 100644
--- a/example/coin-simulator/CMakeLists.txt
+++ b/example/coin-simulator/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/coin-simulator/simulateCircle2DCamVelocity.cpp b/example/coin-simulator/simulateCircle2DCamVelocity.cpp
index c97598c..b0693aa 100644
--- a/example/coin-simulator/simulateCircle2DCamVelocity.cpp
+++ b/example/coin-simulator/simulateCircle2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,28 +49,25 @@
from the camera and from an external view using vpSimulator.
*/
-
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-
#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpTime.h>
#include <visp3/ar/vpSimulator.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCircle.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "cdi:h"
+#define GETOPTARGS "cdi:h"
#define SAVE 0
/*!
@@ -89,12 +87,11 @@ Simulation Servo Circle\n\
SYNOPSIS\n\
%s [-i <input image path>] [-d] [-h]\n", name);
-
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/iv/4points.iv\"\n\
+ From this path read \"iv/4points.iv\"\n\
cad model.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -106,8 +103,7 @@ OPTIONS: Default\n\
using the task manager under Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str());
+ Print the help.\n\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -131,16 +127,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'i': ipath = optarg; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath); return false; break;
+ usage(argv[0], optarg, ipath);
+ return false;
+ break;
}
}
@@ -155,35 +160,33 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
return true;
}
-static
-void *mainLoop (void *_simu)
+static void *mainLoop(void *_simu)
{
- vpSimulator *simu = (vpSimulator *)_simu ;
- simu->initMainApplication() ;
+ vpSimulator *simu = static_cast<vpSimulator *>(_simu);
+ simu->initMainApplication();
- vpPoseVector vcMo ;
+ vpPoseVector vcMo;
- vcMo[0] = 0.3 ;
- vcMo[1] = 0.2 ;
- vcMo[2] = 3 ;
- vcMo[3] = 0 ;
- vcMo[4] = vpMath::rad(45) ;
- vcMo[5] = vpMath::rad(40) ;
+ vcMo[0] = 0.3;
+ vcMo[1] = 0.2;
+ vcMo[2] = 3;
+ vcMo[3] = 0;
+ vcMo[4] = vpMath::rad(45);
+ vcMo[5] = vpMath::rad(40);
vpHomogeneousMatrix cMo(vcMo);
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Robot (=camera) location in the world frame
- vpHomogeneousMatrix cMod ;
- cMod[0][3] = 0 ;
- cMod[1][3] = 0 ;
- cMod[2][3] = 1 ;
+ vpHomogeneousMatrix cMod;
+ cMod[0][3] = 0;
+ cMod[1][3] = 0;
+ cMod[2][3] = 1;
- int it =0 ;
- unsigned int pos = 2 ;
- while (pos!=0)
- {
- vpServo task ;
- vpSimulatorCamera robot ;
+ int it = 0;
+ unsigned int pos = 2;
+ while (pos != 0) {
+ vpServo task;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
@@ -191,100 +194,102 @@ void *mainLoop (void *_simu)
// Sets the initial camera location
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
- simu->setCameraPosition(cMo) ;
+ robot.setPosition(wMc);
+ simu->setCameraPosition(cMo);
- if (pos==1) cMod[2][3] = 0.32 ;
+ if (pos == 1)
+ cMod[2][3] = 0.32;
// Sets the circle coordinates in the world frame
- vpCircle circle ;
- circle.setWorldCoordinates(0,0,1,0,0,0,0.1) ;
+ vpCircle circle;
+ circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1);
// Sets the desired position of the visual feature
- vpFeatureEllipse pd ;
- circle.track(cMod) ;
- vpFeatureBuilder::create(pd,circle) ;
+ vpFeatureEllipse pd;
+ circle.track(cMod);
+ vpFeatureBuilder::create(pd, circle);
- // Project : computes the circle coordinates in the camera frame and its 2D coordinates
- // Sets the current position of the visual feature
- vpFeatureEllipse p ;
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle) ;
+ // Project : computes the circle coordinates in the camera frame and its
+ // 2D coordinates Sets the current position of the visual feature
+ vpFeatureEllipse p;
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
// Define the task
// We want an eye-in-hand control law
// Robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// We want to see a circle on a circle
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
// Display task information
- task.print() ;
+ task.print();
vpTime::wait(1000); // Sleep 1s
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Visual servoing loop
- unsigned int itermax ;
- if (pos==2) itermax = 75 ; else itermax = 100 ;
- while(iter++ < itermax)
- {
+ unsigned int itermax;
+ if (pos == 2)
+ itermax = 75;
+ else
+ itermax = 100;
+ while (iter++ < itermax) {
double t = vpTime::measureTimeMs();
- if (iter==1) std::cout << "get the robot position" << std::endl;
- wMc = robot.getPosition() ;
- if (iter==1) std::cout << "new circle position" << std::endl;
- //retrieve x,y and Z of the vpCircle structure
+ if (iter == 1)
+ std::cout << "get the robot position" << std::endl;
+ wMc = robot.getPosition();
+ if (iter == 1)
+ std::cout << "new circle position" << std::endl;
+ // retrieve x,y and Z of the vpCircle structure
cMo = wMc.inverse() * wMo;
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle);
-
- if (iter==1) std::cout << "compute the control law" << std::endl;
- vpColVector v = task.computeControlLaw() ;
- if (iter==1) {
- std::cout << "Task rank: " << task.getTaskRank() <<std::endl ;
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
+
+ if (iter == 1)
+ std::cout << "compute the control law" << std::endl;
+ 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;
}
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
- simu->setCameraPosition(cMo) ;
-
- if(SAVE==1)
- {
- char name[FILENAME_MAX] ;
- sprintf(name,"/tmp/image.%04d.external.png",it) ;
- std::cout << "Save " << name << std::endl ;
- simu->write(name) ;
- sprintf(name,"/tmp/image.%04u.internal.png",iter) ;
- std::cout << "Save " << name << std::endl ;
- simu->write(name) ;
- it++ ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+ simu->setCameraPosition(cMo);
+
+ if (SAVE == 1) {
+ char name[FILENAME_MAX];
+ sprintf(name, "/tmp/image.%04d.external.png", it);
+ std::cout << "Save " << name << std::endl;
+ simu->write(name);
+ sprintf(name, "/tmp/image.%04u.internal.png", iter);
+ std::cout << "Save " << name << std::endl;
+ simu->write(name);
+ it++;
}
// std::cout << "\t\t || s - s* || "
// std::cout << ( task.getError() ).sumSquare() <<std::endl ; ;
vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
-
}
- pos-- ;
+ pos--;
task.kill();
}
- simu->closeMainApplication() ;
+ simu->closeMainApplication();
- void *a=NULL ;
- return a ;
+ void *a = NULL;
+ return a;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -293,16 +298,17 @@ main(int argc, const char ** argv)
std::string filename;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -313,8 +319,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_INPUT_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -322,53 +327,46 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
- vpCameraParameters cam ;
- vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
+ vpCameraParameters cam;
+ vpHomogeneousMatrix fMo;
+ fMo[2][3] = 0;
if (opt_display) {
- vpSimulator simu ;
- simu.initInternalViewer(300, 300) ;
- simu.initExternalViewer(300, 300) ;
+ vpSimulator simu;
+ simu.initInternalViewer(300, 300);
+ simu.initExternalViewer(300, 300);
- vpTime::wait(1000) ;
- simu.setZoomFactor(1.0f) ;
- simu.addAbsoluteFrame() ;
+ vpTime::wait(1000);
+ simu.setZoomFactor(1.0f);
+ simu.addAbsoluteFrame();
// Load the cad model
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/circle.iv");
- simu.load(filename.c_str(),fMo) ;
-
- simu.setInternalCameraParameters(cam) ;
+ filename = vpIoTools::createFilePath(ipath, "iv/circle.iv");
+ simu.load(filename.c_str(), fMo);
- simu.initApplication(&mainLoop) ;
- simu.mainLoop() ;
+ simu.setInternalCameraParameters(cam);
+ simu.initApplication(&mainLoop);
+ simu.mainLoop();
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
#else
-int
-main()
-{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
-
-}
+int main() { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt"); }
#endif
diff --git a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
index 28a0efa..f8dee38 100644
--- a/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
+++ b/example/coin-simulator/simulateFourPoints2DCartesianCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,23 +52,22 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-
#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpTime.h>
#include <visp3/ar/vpSimulator.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "di:h"
+#define GETOPTARGS "di:h"
#define SAVE 0
/*!
@@ -91,7 +91,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/iv/4points.iv\"\n\
+ From this path read \"iv/4points.iv\"\n\
cad model.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -127,16 +127,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'i': ipath = optarg; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath); return false; break;
+ usage(argv[0], optarg, ipath);
+ return false;
+ break;
}
}
@@ -151,149 +160,147 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
return true;
}
-static
-void *mainLoop (void *_simu)
+static void *mainLoop(void *_simu)
{
- vpSimulator *simu = (vpSimulator *)_simu ;
- simu->initMainApplication() ;
+ vpSimulator *simu = static_cast<vpSimulator *>(_simu);
+ simu->initMainApplication();
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// Sets the initial camera location
- vpPoseVector vcMo ;
+ vpPoseVector vcMo;
- vcMo[0] = 0.3 ;
- vcMo[1] = 0.2 ;
- vcMo[2] = 3 ;
- vcMo[3] = 0 ;
- vcMo[4] = vpMath::rad(0) ;
- vcMo[5] = vpMath::rad(40) ;
+ vcMo[0] = 0.3;
+ vcMo[1] = 0.2;
+ vcMo[2] = 3;
+ vcMo[3] = 0;
+ vcMo[4] = vpMath::rad(0);
+ vcMo[5] = vpMath::rad(40);
vpHomogeneousMatrix cMo(vcMo);
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera location in world frame
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
- simu->setCameraPosition(cMo) ;
+ robot.setPosition(wMc);
+ simu->setCameraPosition(cMo);
- simu->getCameraPosition(cMo) ;
+ simu->getCameraPosition(cMo);
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
+ robot.setPosition(wMc);
robot.setMaxTranslationVelocity(4.);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Sets the point coordinates in the world frame
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
- // Project : computes the point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
+ // Project : computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].track(cMo);
// 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
+ 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
// Sets the desired position of the point
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
- pd[0].buildFrom(-0.1,-0.1,1) ;
- pd[1].buildFrom(0.1,-0.1,1) ;
- pd[2].buildFrom(0.1,0.1,1) ;
- pd[3].buildFrom(-0.1,0.1,1) ;
+ pd[0].buildFrom(-0.1, -0.1, 1);
+ pd[1].buildFrom(0.1, -0.1, 1);
+ pd[2].buildFrom(0.1, 0.1, 1);
+ pd[3].buildFrom(-0.1, 0.1, 1);
// Define the task
// We want an eye-in-hand control law
// Articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// We want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
std::cout << "Display task information" << std::endl;
- task.print() ;
+ task.print();
vpTime::wait(1000); // Sleep 1s to ensure that all the thread are initialized
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// visual servo loop
- while(iter++ < 100) {
+ while (iter++ < 100) {
double t = vpTime::measureTimeMs();
- vpColVector v ;
+ vpColVector v;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (int i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ vpFeatureBuilder::create(p[i], point[i]);
}
- v = task.computeControlLaw() ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ v = task.computeControlLaw();
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
- if(SAVE==1)
- {
+ if (SAVE == 1) {
char name[FILENAME_MAX];
- sprintf(name,"/tmp/image.%04u.external.png",iter) ;
- std::cout << name << std::endl ;
- simu->write(name) ;
- sprintf(name,"/tmp/image.%04u.internal.png",iter) ;
- simu->write(name) ;
+ sprintf(name, "/tmp/image.%04u.external.png", iter);
+ std::cout << name << std::endl;
+ simu->write(name);
+ sprintf(name, "/tmp/image.%04u.internal.png", iter);
+ simu->write(name);
}
vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
}
std::cout << "\nDisplay task information" << std::endl;
- task.print() ;
- task.kill() ;
+ task.print();
+ task.kill();
- simu->closeMainApplication() ;
+ simu->closeMainApplication();
- void *a=NULL ;
- return a ;
+ void *a = NULL;
+ return a;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -302,16 +309,17 @@ int main(int argc, const char ** argv)
std::string filename;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -322,8 +330,7 @@ int main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -331,50 +338,45 @@ int main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
- vpCameraParameters cam ;
- vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
+ vpCameraParameters cam;
+ vpHomogeneousMatrix fMo;
+ fMo[2][3] = 0;
if (opt_display) {
- vpSimulator simu ;
- simu.initInternalViewer(300, 300) ;
- simu.initExternalViewer(300, 300) ;
+ vpSimulator simu;
+ simu.initInternalViewer(300, 300);
+ simu.initExternalViewer(300, 300);
- vpTime::wait(1000) ;
- simu.setZoomFactor(1.0f) ;
+ vpTime::wait(1000);
+ simu.setZoomFactor(1.0f);
// Load the cad model
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
- simu.load(filename.c_str()) ;
+ filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
+ simu.load(filename.c_str());
- simu.setInternalCameraParameters(cam) ;
- simu.setExternalCameraParameters(cam) ;
- simu.initApplication(&mainLoop) ;
+ simu.setInternalCameraParameters(cam);
+ simu.setExternalCameraParameters(cam);
+ simu.initApplication(&mainLoop);
- simu.mainLoop() ;
+ simu.mainLoop();
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
-
-}
+int main() { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt"); }
#endif
diff --git a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
index f230397..637b1ad 100644
--- a/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
+++ b/example/coin-simulator/simulateFourPoints2DPolarCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,23 +50,22 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-
#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpTime.h>
#include <visp3/ar/vpSimulator.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePointPolar.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/vs/vpServo.h>
-#define GETOPTARGS "di:h"
+#define GETOPTARGS "di:h"
#define SAVE 0
/*!
@@ -85,11 +85,11 @@ Simulation Servo 4points.\n\
SYNOPSIS\n\
%s [-i <input image path>] [-d] [-h]\n", name);
-fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/iv/4points.iv\"\n\
+ From this path read \"iv/4points.iv\"\n\
cad model.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -125,16 +125,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
{
const char *optarg;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'i': ipath = optarg; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath); return false; break;
+ usage(argv[0], optarg, ipath);
+ return false;
+ break;
}
}
@@ -149,171 +158,171 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &display)
return true;
}
-static
-void *mainLoop (void *_simu)
+static void *mainLoop(void *_simu)
{
- vpSimulator *simu = (vpSimulator *)_simu ;
- simu->initMainApplication() ;
+ vpSimulator *simu = static_cast<vpSimulator *>(_simu);
+ simu->initMainApplication();
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
float sampling_time = 0.040f; // Sampling period in second
robot.setSamplingTime(sampling_time);
robot.setMaxTranslationVelocity(4.);
// Sets the initial camera location
- vpPoseVector vcMo ;
+ vpPoseVector vcMo;
- vcMo[0] = 0. ;
- vcMo[1] = 0. ;
- vcMo[2] = 3 ;
- vcMo[3] = 0 ;
- vcMo[4] = vpMath::rad(0) ;
- vcMo[5] = vpMath::rad(90) ;
+ vcMo[0] = 0.;
+ vcMo[1] = 0.;
+ vcMo[2] = 3;
+ vcMo[3] = 0;
+ vcMo[4] = vpMath::rad(0);
+ vcMo[5] = vpMath::rad(90);
vpHomogeneousMatrix cMo(vcMo);
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera location in world frame
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
- simu->setCameraPosition(cMo) ;
+ robot.setPosition(wMc);
+ simu->setCameraPosition(cMo);
- simu->getCameraPosition(cMo) ;
+ simu->getCameraPosition(cMo);
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
+ robot.setPosition(wMc);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Sets the point coordinates in the world frame
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
-
- // Project : computes the point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++) {
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
+
+ // 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].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
+ 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
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());
+ 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());
}
// Sets the desired position of the point
- vcMo[0] = 0 ;
- vcMo[1] = 0 ;
- vcMo[2] = 1 ;
+ vcMo[0] = 0;
+ vcMo[1] = 0;
+ vcMo[2] = 1;
vcMo[3] = vpMath::rad(0);
vcMo[4] = vpMath::rad(0);
vcMo[5] = vpMath::rad(0);
vpHomogeneousMatrix cMod(vcMo);
- vpFeaturePointPolar pd[4] ;
+ vpFeaturePointPolar pd[4];
vpPoint pointd[4]; // Desired position of the points
- pointd[0].setWorldCoordinates(-0.1,-0.1,0) ;
- pointd[1].setWorldCoordinates(0.1,-0.1,0) ;
- pointd[2].setWorldCoordinates(0.1,0.1,0) ;
- pointd[3].setWorldCoordinates(-0.1,0.1,0) ;
- for (int i=0; i < 4; i ++) {
+ pointd[0].setWorldCoordinates(-0.1, -0.1, 0);
+ pointd[1].setWorldCoordinates(0.1, -0.1, 0);
+ 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].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 the polar coordinates
+ vpFeatureBuilder::create(pd[i], pointd[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, pd[i].get_rho(), pd[i].get_theta(), pd[i].get_Z());
+ 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());
}
// Define the task
// We want an eye-in-hand control law
// Articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// We want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
// Display task information
- task.print() ;
+ task.print();
vpTime::wait(1000); // Sleep 1s
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Visual servo loop
- while(iter++ < 200) {
+ while (iter++ < 200) {
double t = vpTime::measureTimeMs();
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (int i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ vpFeatureBuilder::create(p[i], point[i]);
}
- vpColVector v = task.computeControlLaw() ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ vpColVector v = task.computeControlLaw();
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
- if(SAVE==1)
- {
+ if (SAVE == 1) {
char name[FILENAME_MAX];
- sprintf(name,"/tmp/image.%04u.external.png",iter) ;
- std::cout << name << std::endl ;
- simu->write(name) ;
- sprintf(name,"/tmp/image.%04u.internal.png",iter) ;
- simu->write(name) ;
+ sprintf(name, "/tmp/image.%04u.external.png", iter);
+ std::cout << name << std::endl;
+ simu->write(name);
+ sprintf(name, "/tmp/image.%04u.internal.png", iter);
+ simu->write(name);
}
vpTime::wait(t, sampling_time * 1000); // Wait 40 ms
-
}
// Display task information
- task.print() ;
- task.kill() ;
+ task.print();
+ task.kill();
std::cout << "cMo:\n" << cMo << std::endl;
vpPoseVector pose(cMo);
std::cout << "final pose:\n" << pose.t() << std::endl;
- simu->closeMainApplication() ;
+ simu->closeMainApplication();
- void *a=NULL ;
- return a ;
+ void *a = NULL;
+ return a;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -322,16 +331,17 @@ int main(int argc, const char ** argv)
std::string filename;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -342,8 +352,7 @@ int main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -351,51 +360,45 @@ int main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
- vpCameraParameters cam ;
- vpHomogeneousMatrix fMo ; fMo[2][3] = 0 ;
-
+ vpCameraParameters cam;
+ vpHomogeneousMatrix fMo;
+ fMo[2][3] = 0;
if (opt_display) {
- vpSimulator simu ;
- simu.initInternalViewer(300, 300) ;
- simu.initExternalViewer(300, 300) ;
+ vpSimulator simu;
+ simu.initInternalViewer(300, 300);
+ simu.initExternalViewer(300, 300);
- vpTime::wait(1000) ;
- simu.setZoomFactor(1.0f) ;
+ vpTime::wait(1000);
+ simu.setZoomFactor(1.0f);
// Load the cad model
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
- simu.load(filename.c_str()) ;
+ filename = vpIoTools::createFilePath(ipath, "iv/4points.iv");
+ simu.load(filename.c_str());
- simu.setInternalCameraParameters(cam) ;
- simu.setExternalCameraParameters(cam) ;
- simu.initApplication(&mainLoop) ;
+ simu.setInternalCameraParameters(cam);
+ simu.setExternalCameraParameters(cam);
+ simu.initApplication(&mainLoop);
- simu.mainLoop() ;
+ simu.mainLoop();
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
-
-}
+int main() { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt"); }
#endif
diff --git a/example/device/display/CMakeLists.txt b/example/device/display/CMakeLists.txt
index 58af34f..8870d98 100644
--- a/example/device/display/CMakeLists.txt
+++ b/example/device/display/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -62,7 +63,7 @@ endforeach()
# To run some of these tests don't forget to set VISP_INPUT_IMAGE_PATH
# environment variable to the ViSP test sequences location.
# To get these sequence download ViSP-images.tar.gz from
-# http://www.irisa.fr/lagadic/visp/visp.html
+# https://visp.inria.fr/download/
add_test(displaySequence displaySequence ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(displayGDI displayGDI -c ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(displayD3D displayD3D -c ${OPTION_TO_DESACTIVE_DISPLAY})
diff --git a/example/device/display/displayD3D.cpp b/example/device/display/displayD3D.cpp
old mode 100755
new mode 100644
index 7dd8004..d63744c
--- a/example/device/display/displayD3D.cpp
+++ b/example/device/display/displayD3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,23 +39,23 @@
/*!
\file displayD3D.cpp
- \brief Read an image on the disk, display it using vpDisplayD3D class, display some
- features (line, circle, caracters) in overlay and finaly write the image and
- the overlayed features in an image on the disk.
+ \brief Read an image on the disk, display it using vpDisplayD3D class,
+ display some features (line, circle, caracters) in overlay and finaly write
+ the image and the overlayed features in an image on the disk.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if ( defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_D3D9))
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
/*!
\example displayD3D.cpp
@@ -66,7 +67,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:h"
+#define GETOPTARGS "cdi:o:h"
/*!
@@ -97,7 +98,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -119,12 +120,11 @@ OPTIONS: Default\n\
Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
- if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
- }
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
}
/*!
@@ -142,23 +142,35 @@ 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,
- std::string user, bool &display)
+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;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg; break;
- case 'o': opath = optarg; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'o':
+ opath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath, opath, user); return false; break;
+ usage(argv[0], optarg, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -173,8 +185,7 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -187,24 +198,23 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Set the default output path
opt_opath = "C:\\temp";
-
// Get the user login name
vpIoTools::getUserName(username);
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_opath,
- opt_click_allowed, username, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -221,11 +231,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -236,8 +244,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -245,24 +252,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
// Create a display using X11
vpDisplayD3D display;
@@ -270,92 +276,91 @@ main(int argc, const char ** argv)
if (opt_display) {
// For this grey level image, open a X11 display at position 100,100
// in the screen, and with title "X11 display"
- display.init(I, 100, 100, "X11 display") ;
+ display.init(I, 100, 100, "X11 display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 10,10 in the
// image. The lines are 10 pixels long
- ip.set_i( 100 );
- ip.set_j( 10 );
+ ip.set_i(100);
+ ip.set_j(10);
- vpDisplay::displayCross(I, ip, 20, vpColor::red) ;
+ vpDisplay::displayCross(I, ip, 20, vpColor::red);
// Display in overlay horizontal red lines
- for (unsigned i=0 ; i < I.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned i = 0; i < I.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
// Display a ligne in the diagonal
- ip1.set_i( -10 );
- ip1.set_j( -10 );
- ip2.set_i( I.getHeight() + 10 );
- ip2.set_j( I.getWidth() + 10 );
+ ip1.set_i(-10);
+ ip1.set_j(-10);
+ ip2.set_i(I.getHeight() + 10);
+ ip2.set_j(I.getWidth() + 10);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
// Display in overlay vertical green dot lines
- for (unsigned i=0 ; i < I.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green) ;
+ for (unsigned i = 0; i < I.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green);
}
// Display a rectangle
- ip.set_i( I.getHeight() - 45 );
- ip.set_j( -10 );
- vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange) ;
+ ip.set_i(I.getHeight() - 45);
+ ip.set_j(-10);
+ vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue) ;
-
- // Display in overlay some circles. The position of the center is 200, 200
- // the radius is increased by 20 pixels for each circle
-
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 80 );
- ip.set_j( 80 );
- vpDisplay::displayCircle(I, ip, 20+i, vpColor::yellow) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue);
+
+ // Display in overlay some circles. The position of the center is 200,
+ // 200 the radius is increased by 20 pixels for each circle
+
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(80);
+ ip.set_j(80);
+ vpDisplay::displayCircle(I, ip, 20 + i, vpColor::yellow);
}
- ip.set_i( -10 );
- ip.set_j( 300 );
- vpDisplay::displayCircle(I, ip, 100,vpColor::yellow) ;
+ ip.set_i(-10);
+ ip.set_j(300);
+ vpDisplay::displayCircle(I, ip, 100, vpColor::yellow);
// 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) ;
- //Flush the display
+ ip.set_i(85);
+ ip.set_j(100);
+ vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow);
+ // Flush the display
vpDisplay::flush(I);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Close the display
@@ -363,11 +368,11 @@ main(int argc, const char ** argv)
}
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk and convert it to a color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgba, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgba, filename);
// Create a new display
vpDisplayD3D displayRGBa;
@@ -378,60 +383,55 @@ main(int argc, const char ** argv)
displayRGBa.init(Irgba, 100, 100, "X11 color display");
// Display the color image
- vpDisplay::display(Irgba) ;
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ vpDisplay::flush(Irgba);
- // If click is allowed, wait for a blocking mouse click to display a cross
- // at the clicked pixel position
+ // If click is allowed, wait for a blocking mouse click to display a
+ // cross at the clicked pixel position
if (opt_click_allowed) {
std::cout << "\nA click to display 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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(Irgba, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
- }
- else {
- ip.set_i( 10 );
- ip.set_j( 20 );
+ } else {
+ ip.set_i(10);
+ ip.set_j(20);
// Display a red cross at position i, j (i correspond to the row
// and j to the column coordinates in the image)
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
-
}
// Flush the display. Sometimes the display content is
// bufferized. Force to display the content that has been bufferized.
vpDisplay::flush(Irgba);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(Irgba, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(Irgba, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a blocking mouse click to exit.
if (opt_click_allowed) {
std::cout << "\nA click to exit the program..." << std::endl;
- vpDisplay::getClick(Irgba) ;
+ vpDisplay::getClick(Irgba);
std::cout << "Bye" << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("Direct 3D library is not available...");
-}
+int main() { vpERROR_TRACE("Direct 3D library is not available..."); }
#endif
diff --git a/example/device/display/displayGDI.cpp b/example/device/display/displayGDI.cpp
old mode 100755
new mode 100644
index 2e7d23f..66f9c05
--- a/example/device/display/displayGDI.cpp
+++ b/example/device/display/displayGDI.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,23 +38,23 @@
/*!
\file displayGDI.cpp
- \brief Read an image on the disk, display it using vpDisplayGDI class, display some
- features (line, circle, caracters) in overlay and finaly write the image and
- the overlayed features in an image on the disk.
+ \brief Read an image on the disk, display it using vpDisplayGDI class,
+ display some features (line, circle, caracters) in overlay and finaly write
+ the image and the overlayed features in an image on the disk.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if ( defined(_WIN32) && defined(VISP_HAVE_GDI) )
+#if (defined(_WIN32) && defined(VISP_HAVE_GDI))
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
/*!
\example displayGDI.cpp
@@ -65,7 +66,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:h"
+#define GETOPTARGS "cdi:o:h"
/*!
@@ -96,7 +97,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -118,12 +119,10 @@ OPTIONS: Default\n\
Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
}
/*!
@@ -141,23 +140,35 @@ 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,
- std::string user, bool &display)
+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;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg; break;
- case 'o': opath = optarg; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'o':
+ opath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath, opath, user); return false; break;
+ usage(argv[0], optarg, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -172,8 +183,7 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -186,11 +196,12 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Set the default output path
@@ -200,9 +211,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -219,11 +229,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -234,8 +242,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -243,24 +250,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
// Create a display using X11
vpDisplayGDI display;
@@ -268,92 +274,91 @@ main(int argc, const char ** argv)
if (opt_display) {
// For this grey level image, open a X11 display at position 100,100
// in the screen, and with title "X11 display"
- display.init(I, 100, 100, "X11 display") ;
+ display.init(I, 100, 100, "X11 display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 10,10 in the
// image. The lines are 10 pixels long
- ip.set_i( 100 );
- ip.set_j( 10 );
+ ip.set_i(100);
+ ip.set_j(10);
- vpDisplay::displayCross(I, ip, 20, vpColor::red) ;
+ vpDisplay::displayCross(I, ip, 20, vpColor::red);
// Display in overlay horizontal red lines
- for (unsigned i=0 ; i < I.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned i = 0; i < I.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
// Display a ligne in the diagonal
- ip1.set_i( -10 );
- ip1.set_j( -10 );
- ip2.set_i( I.getHeight() + 10 );
- ip2.set_j( I.getWidth() + 10 );
+ ip1.set_i(-10);
+ ip1.set_j(-10);
+ ip2.set_i(I.getHeight() + 10);
+ ip2.set_j(I.getWidth() + 10);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
// Display in overlay vertical green dot lines
- for (unsigned i=0 ; i < I.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green) ;
+ for (unsigned i = 0; i < I.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green);
}
// Display a rectangle
- ip.set_i( I.getHeight() - 45 );
- ip.set_j( -10 );
- vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange) ;
+ ip.set_i(I.getHeight() - 45);
+ ip.set_j(-10);
+ vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue) ;
-
- // Display in overlay some circles. The position of the center is 200, 200
- // the radius is increased by 20 pixels for each circle
-
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 80 );
- ip.set_j( 80 );
- vpDisplay::displayCircle(I, ip, 20+i, vpColor::yellow) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue);
+
+ // Display in overlay some circles. The position of the center is 200,
+ // 200 the radius is increased by 20 pixels for each circle
+
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(80);
+ ip.set_j(80);
+ vpDisplay::displayCircle(I, ip, 20 + i, vpColor::yellow);
}
- ip.set_i( -10 );
- ip.set_j( 300 );
- vpDisplay::displayCircle(I, ip, 100,vpColor::yellow) ;
+ ip.set_i(-10);
+ ip.set_j(300);
+ vpDisplay::displayCircle(I, ip, 100, vpColor::yellow);
// 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) ;
- //Flush the display
+ ip.set_i(85);
+ ip.set_j(100);
+ vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow);
+ // Flush the display
vpDisplay::flush(I);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Close the display
@@ -361,11 +366,11 @@ main(int argc, const char ** argv)
}
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk and convert it to a color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgba, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgba, filename);
// Create a new display
vpDisplayGDI displayRGBa;
@@ -376,60 +381,55 @@ main(int argc, const char ** argv)
displayRGBa.init(Irgba, 100, 100, "X11 color display");
// Display the color image
- vpDisplay::display(Irgba) ;
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ vpDisplay::flush(Irgba);
- // If click is allowed, wait for a blocking mouse click to display a cross
- // at the clicked pixel position
+ // If click is allowed, wait for a blocking mouse click to display a
+ // cross at the clicked pixel position
if (opt_click_allowed) {
std::cout << "\nA click to display 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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(Irgba, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
- }
- else {
- ip.set_i( 10 );
- ip.set_j( 20 );
+ } else {
+ ip.set_i(10);
+ ip.set_j(20);
// Display a red cross at position i, j (i correspond to the row
// and j to the column coordinates in the image)
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
-
}
// Flush the display. Sometimes the display content is
// bufferized. Force to display the content that has been bufferized.
vpDisplay::flush(Irgba);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(Irgba, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(Irgba, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a blocking mouse click to exit.
if (opt_click_allowed) {
std::cout << "\nA click to exit the program..." << std::endl;
- vpDisplay::getClick(Irgba) ;
+ vpDisplay::getClick(Irgba);
std::cout << "Bye" << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("GDI display only works under windows...");
-}
+int main() { vpERROR_TRACE("GDI display only works under windows..."); }
#endif
diff --git a/example/device/display/displayGTK.cpp b/example/device/display/displayGTK.cpp
index c80ecc9..66eaab5 100644
--- a/example/device/display/displayGTK.cpp
+++ b/example/device/display/displayGTK.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,27 +39,26 @@
/*!
\file displayGTK.cpp
- \brief Read an image on the disk, display it using vpDisplayGTK class, display some
- features (line, circle, caracters) in overlay and finaly write the image and
- the overlayed features in an image on the disk.
+ \brief Read an image on the disk, display it using vpDisplayGTK class,
+ display some features (line, circle, caracters) in overlay and finaly write
+ the image and the overlayed features in an image on the disk.
*/
-
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#ifdef VISP_HAVE_GTK
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
/*!
\example displayGTK.cpp
@@ -70,7 +70,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:h"
+#define GETOPTARGS "cdi:o:h"
/*!
@@ -99,7 +99,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -121,8 +121,7 @@ OPTIONS: Default\n\
using the task manager under Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -145,23 +144,35 @@ 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,
- std::string user, bool &display)
+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;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg; break;
- case 'o': opath = optarg; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'o':
+ opath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg, ipath, opath, user); return false; break;
+ usage(argv[0], optarg, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -176,8 +187,7 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -190,14 +200,15 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -208,9 +219,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -227,11 +237,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -242,8 +250,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -251,24 +258,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
// Create a display using X11
vpDisplayGTK display;
@@ -276,92 +282,91 @@ main(int argc, const char ** argv)
if (opt_display) {
// For this grey level image, open a X11 display at position 100,100
// in the screen, and with title "X11 display"
- display.init(I, 100, 100, "X11 display") ;
+ display.init(I, 100, 100, "X11 display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 10,10 in the
// image. The lines are 10 pixels long
- ip.set_i( 100 );
- ip.set_j( 10 );
+ ip.set_i(100);
+ ip.set_j(10);
- vpDisplay::displayCross(I, ip, 20, vpColor::red) ;
+ vpDisplay::displayCross(I, ip, 20, vpColor::red);
// Display in overlay horizontal red lines
- for (unsigned i=0 ; i < I.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned i = 0; i < I.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
// Display a ligne in the diagonal
- ip1.set_i( -10 );
- ip1.set_j( -10 );
- ip2.set_i( I.getHeight() + 10 );
- ip2.set_j( I.getWidth() + 10 );
+ ip1.set_i(-10);
+ ip1.set_j(-10);
+ ip2.set_i(I.getHeight() + 10);
+ ip2.set_j(I.getWidth() + 10);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
// Display in overlay vertical green dot lines
- for (unsigned i=0 ; i < I.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green) ;
+ for (unsigned i = 0; i < I.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green);
}
// Display a rectangle
- ip.set_i( I.getHeight() - 45 );
- ip.set_j( -10 );
- vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange) ;
+ ip.set_i(I.getHeight() - 45);
+ ip.set_j(-10);
+ vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue) ;
-
- // Display in overlay some circles. The position of the center is 200, 200
- // the radius is increased by 20 pixels for each circle
-
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 80 );
- ip.set_j( 80 );
- vpDisplay::displayCircle(I, ip, 20+i, vpColor::yellow) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue);
+
+ // Display in overlay some circles. The position of the center is 200,
+ // 200 the radius is increased by 20 pixels for each circle
+
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(80);
+ ip.set_j(80);
+ vpDisplay::displayCircle(I, ip, 20 + i, vpColor::yellow);
}
- ip.set_i( -10 );
- ip.set_j( 300 );
- vpDisplay::displayCircle(I, ip, 100,vpColor::yellow) ;
+ ip.set_i(-10);
+ ip.set_j(300);
+ vpDisplay::displayCircle(I, ip, 100, vpColor::yellow);
// 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) ;
- //Flush the display
+ ip.set_i(85);
+ ip.set_j(100);
+ vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow);
+ // Flush the display
vpDisplay::flush(I);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Close the display
@@ -369,11 +374,11 @@ main(int argc, const char ** argv)
}
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk and convert it to a color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgba, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgba, filename);
// Create a new display
vpDisplayGTK displayRGBa;
@@ -384,61 +389,56 @@ main(int argc, const char ** argv)
displayRGBa.init(Irgba, 100, 100, "X11 color display");
// Display the color image
- vpDisplay::display(Irgba) ;
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ vpDisplay::flush(Irgba);
- // If click is allowed, wait for a blocking mouse click to display a cross
- // at the clicked pixel position
+ // If click is allowed, wait for a blocking mouse click to display a
+ // cross at the clicked pixel position
if (opt_click_allowed) {
std::cout << "\nA click to display 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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(Irgba, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
- }
- else {
- ip.set_i( 10 );
- ip.set_j( 20 );
+ } else {
+ ip.set_i(10);
+ ip.set_j(20);
// Display a red cross at position i, j (i correspond to the row
// and j to the column coordinates in the image)
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
-
}
// Flush the display. Sometimes the display content is
// bufferized. Force to display the content that has been bufferized.
vpDisplay::flush(Irgba);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(Irgba, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(Irgba, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a blocking mouse click to exit.
if (opt_click_allowed) {
std::cout << "\nA click to exit the program..." << std::endl;
- vpDisplay::getClick(Irgba) ;
+ vpDisplay::getClick(Irgba);
std::cout << "Bye" << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 e061900..a1970d2 100644
--- a/example/device/display/displayOpenCV.cpp
+++ b/example/device/display/displayOpenCV.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,17 +44,17 @@
*/
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if defined(VISP_HAVE_OPENCV)
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
@@ -67,11 +68,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:p: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,
- bool &click_allowed, std::string user, bool &display);
+#define GETOPTARGS "cdi:o:p:h"
/*!
@@ -100,7 +97,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -122,14 +119,12 @@ OPTIONS: Default\n\
using the task manager under Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -150,22 +145,35 @@ 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, std::string user, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -180,8 +188,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -194,14 +201,15 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -212,9 +220,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -231,11 +238,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -246,8 +251,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -255,24 +259,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
// Create a display using X11
vpDisplayOpenCV display;
@@ -280,92 +283,91 @@ main(int argc, const char ** argv)
if (opt_display) {
// For this grey level image, open a X11 display at position 100,100
// in the screen, and with title "X11 display"
- display.init(I, 100, 100, "X11 display") ;
+ display.init(I, 100, 100, "X11 display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 10,10 in the
// image. The lines are 10 pixels long
- ip.set_i( 100 );
- ip.set_j( 10 );
+ ip.set_i(100);
+ ip.set_j(10);
- vpDisplay::displayCross(I, ip, 20, vpColor::red) ;
+ vpDisplay::displayCross(I, ip, 20, vpColor::red);
// Display in overlay horizontal red lines
- for (unsigned i=0 ; i < I.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned i = 0; i < I.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
// Display a ligne in the diagonal
- ip1.set_i( -10 );
- ip1.set_j( -10 );
- ip2.set_i( I.getHeight() + 10 );
- ip2.set_j( I.getWidth() + 10 );
+ ip1.set_i(-10);
+ ip1.set_j(-10);
+ ip2.set_i(I.getHeight() + 10);
+ ip2.set_j(I.getWidth() + 10);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
// Display in overlay vertical green dot lines
- for (unsigned i=0 ; i < I.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green) ;
+ for (unsigned i = 0; i < I.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green);
}
// Display a rectangle
- ip.set_i( I.getHeight() - 45 );
- ip.set_j( -10 );
- vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange) ;
+ ip.set_i(I.getHeight() - 45);
+ ip.set_j(-10);
+ vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue) ;
-
- // Display in overlay some circles. The position of the center is 200, 200
- // the radius is increased by 20 pixels for each circle
-
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 80 );
- ip.set_j( 80 );
- vpDisplay::displayCircle(I, ip, 20+i, vpColor::yellow) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue);
+
+ // Display in overlay some circles. The position of the center is 200,
+ // 200 the radius is increased by 20 pixels for each circle
+
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(80);
+ ip.set_j(80);
+ vpDisplay::displayCircle(I, ip, 20 + i, vpColor::yellow);
}
- ip.set_i( -10 );
- ip.set_j( 300 );
- vpDisplay::displayCircle(I, ip, 100,vpColor::yellow) ;
+ ip.set_i(-10);
+ ip.set_j(300);
+ vpDisplay::displayCircle(I, ip, 100, vpColor::yellow);
// 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) ;
- //Flush the display
+ ip.set_i(85);
+ ip.set_j(100);
+ vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow);
+ // Flush the display
vpDisplay::flush(I);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Close the display
@@ -373,11 +375,11 @@ main(int argc, const char ** argv)
}
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk and convert it to a color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgba, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgba, filename);
// Create a new display
vpDisplayOpenCV displayRGBa;
@@ -388,62 +390,56 @@ main(int argc, const char ** argv)
displayRGBa.init(Irgba, 100, 100, "X11 color display");
// Display the color image
- vpDisplay::display(Irgba) ;
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ vpDisplay::flush(Irgba);
- // If click is allowed, wait for a blocking mouse click to display a cross
- // at the clicked pixel position
+ // If click is allowed, wait for a blocking mouse click to display a
+ // cross at the clicked pixel position
if (opt_click_allowed) {
std::cout << "\nA click to display 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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(Irgba, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
- }
- else {
- ip.set_i( 10 );
- ip.set_j( 20 );
+ } else {
+ ip.set_i(10);
+ ip.set_j(20);
// Display a red cross at position i, j (i correspond to the row
// and j to the column coordinates in the image)
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
-
}
// Flush the display. Sometimes the display content is
// bufferized. Force to display the content that has been bufferized.
vpDisplay::flush(Irgba);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(Irgba, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(Irgba, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a blocking mouse click to exit.
if (opt_click_allowed) {
std::cout << "\nA click to exit the program..." << std::endl;
- vpDisplay::getClick(Irgba) ;
+ vpDisplay::getClick(Irgba);
std::cout << "Bye" << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 e36790b..309c1c3 100644
--- a/example/device/display/displaySequence.cpp
+++ b/example/device/display/displaySequence.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,24 +47,22 @@
*/
-
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <iomanip>
+#include <sstream>
#include <stdio.h>
#include <stdlib.h>
-#include <sstream>
-#include <iomanip>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpTime.h>
@@ -78,12 +77,12 @@
*/
// List of allowed command line options
-#define GETOPTARGS "di:p:hf:n:s:w"
+#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, unsigned &step, bool &display, bool &wait);
+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);
/*!
@@ -98,8 +97,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
\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\
@@ -112,11 +111,11 @@ SYNOPSIS\n\
[-w] [-d] [-h]\n \
", name);
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <test image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ From this path read \"cube/image.%%04d.pgm\"\n\
images. These images come from ViSP-images-x.y.z.tar.gz\n\
available on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -153,8 +152,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);
+ Print the help.\n\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -180,27 +178,44 @@ 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, bool &display, bool &wait)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'w': wait = true; break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step);
- return false; break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'w':
+ wait = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
- return false; break;
+ return false;
+ break;
}
}
@@ -215,8 +230,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -231,20 +245,21 @@ main(int argc, const char ** argv)
bool opt_display = true;
bool opt_wait = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_display, opt_wait) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_display, opt_wait) ==
+ false) {
+ exit(-1);
}
- if ( ! opt_display )
+ if (!opt_display)
opt_wait = false; // turn off the waiting
// Get the option values
@@ -255,8 +270,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -264,15 +278,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -282,14 +294,13 @@ main(int argc, const char ** argv)
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
unsigned iter = opt_first;
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -300,20 +311,19 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// Read the PGM image named "filename" on the disk, and put the
@@ -322,22 +332,18 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ try {
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option, " << std::endl
- << " or your -p " << opt_ppath << " option " <<std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable"
- << std::endl;
+ << " or your -p " << opt_ppath << " option " << std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl;
exit(-1);
}
@@ -352,32 +358,31 @@ main(int argc, const char ** argv)
// We open a window using either X11 or GTK or GDI.
// Its size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
// double tms_1 = vpTime::measureTimeMs() ;
- unsigned niter=0 ;
+ unsigned niter = 0;
// this is the loop over the image sequence
- while (iter < opt_first + opt_nimages*opt_step) {
- double tms = vpTime::measureTimeMs() ;
+ while (iter < opt_first + opt_nimages * opt_step) {
+ double tms = vpTime::measureTimeMs();
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
@@ -386,39 +391,32 @@ main(int argc, const char ** argv)
vpImageIo::read(I, filename);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
-
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
}
if (opt_wait) {
std::cout << "A click in the image to continue..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
- }
- else {
+ vpDisplay::getClick(I);
+ } else {
// Synchronise the loop to 40 ms
- vpTime::wait(tms, 40) ;
+ vpTime::wait(tms, 40);
}
- niter++ ;
+ niter++;
- iter += opt_step ;
+ iter += opt_step;
}
// double tms_2 = vpTime::measureTimeMs() ;
// double tms_total = tms_2 - tms_1 ;
// std::cout << "Total Time : "<< tms_total<<std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 c7b5b46..4d49be0 100644
--- a/example/device/display/displayX.cpp
+++ b/example/device/display/displayX.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,16 +45,16 @@
*/
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#ifdef VISP_HAVE_X11
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
@@ -67,11 +68,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:p: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, bool &click_allowed,
- std::string user, bool &display);
+#define GETOPTARGS "cdi:o:p:h"
/*!
@@ -100,7 +97,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -122,14 +119,12 @@ OPTIONS: Default\n\
using the task manager under Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -151,21 +146,34 @@ OPTIONS: Default\n\
*/
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed,
- std::string user, bool &display)
+ const std::string &user, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -180,8 +188,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -194,14 +201,15 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -212,9 +220,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -231,11 +238,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -246,8 +251,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -255,24 +259,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
// Create a display using X11
vpDisplayX display;
@@ -280,92 +283,91 @@ main(int argc, const char ** argv)
if (opt_display) {
// For this grey level image, open a X11 display at position 100,100
// in the screen, and with title "X11 display"
- display.init(I, 100, 100, "X11 display") ;
+ display.init(I, 100, 100, "X11 display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 10,10 in the
// image. The lines are 10 pixels long
- ip.set_i( 100 );
- ip.set_j( 10 );
+ ip.set_i(100);
+ ip.set_j(10);
- vpDisplay::displayCross(I, ip, 20, vpColor::red) ;
+ vpDisplay::displayCross(I, ip, 20, vpColor::red);
// Display in overlay horizontal red lines
- for (unsigned i=0 ; i < I.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned i = 0; i < I.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
// Display a ligne in the diagonal
- ip1.set_i( -10 );
- ip1.set_j( -10 );
- ip2.set_i( I.getHeight() + 10 );
- ip2.set_j( I.getWidth() + 10 );
+ ip1.set_i(-10);
+ ip1.set_j(-10);
+ ip2.set_i(I.getHeight() + 10);
+ ip2.set_j(I.getWidth() + 10);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
// Display in overlay vertical green dot lines
- for (unsigned i=0 ; i < I.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green) ;
+ for (unsigned i = 0; i < I.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green);
}
// Display a rectangle
- ip.set_i( I.getHeight() - 45 );
- ip.set_j( -10 );
- vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange) ;
+ ip.set_i(I.getHeight() - 45);
+ ip.set_j(-10);
+ vpDisplay::displayRectangle(I, ip, 60, 80, vpColor::orange);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue) ;
-
- // Display in overlay some circles. The position of the center is 200, 200
- // the radius is increased by 20 pixels for each circle
-
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 80 );
- ip.set_j( 80 );
- vpDisplay::displayCircle(I, ip, 20+i, vpColor::yellow) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue);
+
+ // Display in overlay some circles. The position of the center is 200,
+ // 200 the radius is increased by 20 pixels for each circle
+
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(80);
+ ip.set_j(80);
+ vpDisplay::displayCircle(I, ip, 20 + i, vpColor::yellow);
}
- ip.set_i( -10 );
- ip.set_j( 300 );
- vpDisplay::displayCircle(I, ip, 100,vpColor::yellow) ;
+ ip.set_i(-10);
+ ip.set_j(300);
+ vpDisplay::displayCircle(I, ip, 100, vpColor::yellow);
// 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) ;
- //Flush the display
+ ip.set_i(85);
+ ip.set_j(100);
+ vpDisplay::displayText(I, ip, "ViSP is a marvelous software", vpColor::yellow);
+ // Flush the display
vpDisplay::flush(I);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Close the display
@@ -373,11 +375,11 @@ main(int argc, const char ** argv)
}
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk and convert it to a color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgba, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgba, filename);
// Create a new display
vpDisplayX displayRGBa;
@@ -388,62 +390,56 @@ main(int argc, const char ** argv)
displayRGBa.init(Irgba, 100, 100, "X11 color display");
// Display the color image
- vpDisplay::display(Irgba) ;
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ vpDisplay::flush(Irgba);
- // If click is allowed, wait for a blocking mouse click to display a cross
- // at the clicked pixel position
+ // If click is allowed, wait for a blocking mouse click to display a
+ // cross at the clicked pixel position
if (opt_click_allowed) {
std::cout << "\nA click to display 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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(Irgba, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
- }
- else {
- ip.set_i( 10 );
- ip.set_j( 20 );
+ } else {
+ ip.set_i(10);
+ ip.set_j(20);
// Display a red cross at position i, j (i correspond to the row
// and j to the column coordinates in the image)
std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(Irgba, ip, 15, vpColor::red);
-
}
// Flush the display. Sometimes the display content is
// bufferized. Force to display the content that has been bufferized.
vpDisplay::flush(Irgba);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(Irgba, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image and the
+ // overlay
+ vpDisplay::getImage(Irgba, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_color.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a blocking mouse click to exit.
if (opt_click_allowed) {
std::cout << "\nA click to exit the program..." << std::endl;
- vpDisplay::getClick(Irgba) ;
+ vpDisplay::getClick(Irgba);
std::cout << "Bye" << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 abac169..029bc3f 100644
--- a/example/device/display/displayXMulti.cpp
+++ b/example/device/display/displayXMulti.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,20 +45,20 @@
the overlayed features in an image on the disk.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#ifdef VISP_HAVE_X11
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <string.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
/*!
\example displayXMulti.cpp
@@ -70,11 +71,7 @@
*/
// 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, bool &click_allowed,
- std::string user, bool &display);
+#define GETOPTARGS "cdi:o:h"
/*!
@@ -103,8 +100,8 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -125,11 +122,10 @@ OPTIONS: Default\n\
using the task manager under Windows.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -152,21 +148,34 @@ OPTIONS: Default\n\
*/
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, bool &click_allowed,
- std::string user, bool &display)
+ const std::string &user, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -181,8 +190,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -195,14 +203,15 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -213,9 +222,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_opath, opt_click_allowed, username, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -232,11 +240,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(odirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << odirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -247,8 +253,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -256,49 +261,40 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create two color images
- vpImage<vpRGBa> I1, I2 ;
+ vpImage<vpRGBa> I1, I2;
vpImagePoint ip, ip1, ip2;
try {
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I1, filename) ;
- }
- catch(...)
- {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I1, filename);
+ } catch (...) {
+ 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 {
// Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(I2, filename) ;
- }
- catch(...)
- {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(I2, filename);
+ } catch (...) {
+ 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);
}
@@ -308,56 +304,54 @@ main(int argc, const char ** argv)
if (opt_display) {
// Attach image 1 to display 1
- display1.init(I1, 0, 0,"X11 Display 1...") ;
+ display1.init(I1, 0, 0, "X11 Display 1...");
// Attach image 2 to display 2
- display2.init(I2, 200, 200,"X11 Display 2...") ;
+ display2.init(I2, 200, 200, "X11 Display 2...");
// Display the images
- vpDisplay::display(I1) ;
- vpDisplay::display(I2) ;
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
// In the first display, display in overlay horizontal red lines
- for (unsigned int i=0 ; i < I1.getHeight() ; i+=20) {
- ip1.set_i( i );
- ip1.set_j( 0 );
- ip2.set_i( i );
- ip2.set_j( I1.getWidth() );
- vpDisplay::displayLine(I1, ip1, ip2, vpColor::red) ;
+ for (unsigned int i = 0; i < I1.getHeight(); i += 20) {
+ ip1.set_i(i);
+ ip1.set_j(0);
+ ip2.set_i(i);
+ ip2.set_j(I1.getWidth());
+ vpDisplay::displayLine(I1, ip1, ip2, vpColor::red);
}
// In the first display, display in overlay vertical green dot lines
- for (unsigned int i=0 ; i < I1.getWidth() ; i+=20) {
- ip1.set_i( 0 );
- ip1.set_j( i );
- ip2.set_i( I1.getWidth() );
- ip2.set_j( i );
- vpDisplay::displayDotLine(I1, ip1, ip2, vpColor::green) ;
+ for (unsigned int i = 0; i < I1.getWidth(); i += 20) {
+ ip1.set_i(0);
+ ip1.set_j(i);
+ ip2.set_i(I1.getWidth());
+ ip2.set_j(i);
+ vpDisplay::displayDotLine(I1, ip1, ip2, vpColor::green);
}
// In the first display, display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I1, ip1, ip2, vpColor::blue) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I1, ip1, ip2, vpColor::blue);
// In the first display, display in overlay some circles. The
// position of the center is 200, 200 the radius is increased by 20
// pixels for each circle
- for (unsigned int i=0 ; i < 100 ; i+=20) {
- ip.set_i( 200 );
- ip.set_j( 200 );
- vpDisplay::displayCircle(I1, ip, 20+i,vpColor::yellow) ;
+ for (unsigned int i = 0; i < 100; i += 20) {
+ ip.set_i(200);
+ ip.set_j(200);
+ vpDisplay::displayCircle(I1, ip, 20 + i, vpColor::yellow);
}
// 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) ;
-
- //Flush displays. The displays must be flushed to show the overlay.
- //without this line, nothing will be displayed.
+ ip.set_i(100);
+ ip.set_j(100);
+ 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.
vpDisplay::flush(I1);
vpDisplay::flush(I2);
@@ -366,51 +360,48 @@ main(int argc, const char ** argv)
if (opt_click_allowed) {
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)
+ // (i correspond to the row and j to the column coordinates in the
+ // image)
vpDisplay::getClick(I1, ip);
// Display a red cross on the click pixel position
std::cout << "Cross position: " << ip << std::endl;
- vpDisplay::displayCross(I1, ip, 15,vpColor::red);
+ vpDisplay::displayCross(I1, ip, 15, vpColor::red);
vpDisplay::flush(I1);
- }
- else {
- ip.set_i( 50 );
- ip.set_j( 50 );
+ } else {
+ ip.set_i(50);
+ ip.set_j(50);
// Display a red cross at position ip in the first display
- std::cout << "Cross position: " << ip<< std::endl;
+ std::cout << "Cross position: " << ip << std::endl;
vpDisplay::displayCross(I1, ip, 15, vpColor::red);
vpDisplay::flush(I1);
}
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
- // Updates the color image with the original loaded image 1 and the overlay
- vpDisplay::getImage(I1, Ioverlay) ;
+ vpImage<vpRGBa> Ioverlay;
+ // Updates the color image with the original loaded image 1 and the
+ // overlay
+ vpDisplay::getImage(I1, Ioverlay);
// Write the color image on the disk
filename = vpIoTools::createFilePath(odirname, "Klimt_grey.overlay.ppm");
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
if (opt_click_allowed) {
- std::cout << "\nA click in the second display to close the windows and exit..." << std::endl;
+ std::cout << "\nA click in the second display to close the windows "
+ "and exit..."
+ << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I2) ;
+ vpDisplay::getClick(I2);
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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/CMakeLists.txt b/example/device/framegrabber/CMakeLists.txt
index 7c43c9f..d417d58 100644
--- a/example/device/framegrabber/CMakeLists.txt
+++ b/example/device/framegrabber/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -51,6 +52,7 @@ set(example_cpp
grabFlyCapture.cpp
grabOpenCV.cpp
grabRealSense.cpp
+ grabRealSense2.cpp
grabV4l2MultiCpp11Thread.cpp
)
@@ -61,7 +63,7 @@ endif()
if(VISP_HAVE_PCL)
# Add specific build flag to turn off warnings coming from PCL 3rd party
- visp_set_source_file_compile_flag(grabRealSense.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal)
+ visp_set_source_file_compile_flag(grabRealSense.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal -Wno-unused-parameter -Wno-pessimizing-move)
endif()
foreach(cpp ${example_cpp})
diff --git a/example/device/framegrabber/grab1394CMU.cpp b/example/device/framegrabber/grab1394CMU.cpp
index 3826b29..025681a 100644
--- a/example/device/framegrabber/grab1394CMU.cpp
+++ b/example/device/framegrabber/grab1394CMU.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,34 +36,32 @@
*
*****************************************************************************/
-
/*!
\file grab1394CMU.cpp
\brief Video capture example based on CMU 1394 Digital Camera SDK.
*/
+#include <iostream>
#include <stdio.h>
#include <stdlib.h>
-#include <iostream>
#include <visp3/core/vpConfig.h>
-#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpTime.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
#define GRAB_COLOR
// List of allowed command line options
-#define GETOPTARGS "dhn:o:"
+#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);
+bool getOptions(int argc, const char **argv, bool &display, unsigned int &nframes, bool &save, std::string &opath);
/*!
@@ -100,7 +99,7 @@ OPTIONS: Default\n\
Print the help.\n\
\n", nframes, opath.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -120,25 +119,32 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': display = false; break;
+ case 'd':
+ display = false;
+ break;
case 'n':
- nframes = (unsigned int)atoi(optarg_); break;
+ nframes = (unsigned int)atoi(optarg_);
+ break;
case 'o':
save = true;
- opath = optarg_; break;
- case 'h': usage(argv[0], NULL, nframes, opath); return false; break;
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, nframes, opath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, nframes, opath);
- return false; break;
+ return false;
+ break;
}
}
@@ -153,30 +159,28 @@ bool getOptions(int argc, const char **argv, bool &display,
return true;
}
-
/*!
\example grab1394CMU.cpp
- Video capture example based on CMU 1394 Digital Camera SDK using vp1394CMUGrabber class.
- Display the images using the GDI or OpenCV display.
+ Video capture example based on CMU 1394 Digital Camera SDK using
+ vp1394CMUGrabber class. Display the images using the GDI or OpenCV display.
*/
#if defined(VISP_HAVE_CMU1394)
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
bool opt_display = true;
unsigned nframes = 50;
bool save = false;
- // Declare an image. It size is not defined yet. It will be defined when the
- // image will acquired the first time.
+// Declare an image. It size is not defined yet. It will be defined when the
+// image will acquired the first time.
#ifdef GRAB_COLOR
vpImage<vpRGBa> I; // This is a color image (in RGBa format)
#else
vpImage<unsigned char> I; // This is a B&W image
#endif
- // Set default output image name for saving
+// Set default output image name for saving
#ifdef GRAB_COLOR
// Color images will be saved in PGM P6 format
std::string opath = "C:/temp/I%04d.ppm";
@@ -187,52 +191,51 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_display, nframes, save, opath) == false) {
- exit (-1);
+ exit(-1);
}
// Create the grabber
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);
+ 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))
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
- // Creates a display
+// Creates a display
#if defined VISP_HAVE_OPENCV
vpDisplayOpenCV display;
#elif defined VISP_HAVE_GDI
vpDisplayGDI display;
#endif
if (opt_display) {
- display.init(I,100,100,"DirectShow Framegrabber");
+ display.init(I, 100, 100, "DirectShow Framegrabber");
}
#endif
try {
- double tbegin=0, ttotal=0;
+ double tbegin = 0, ttotal = 0;
ttotal = 0;
tbegin = vpTime::measureTimeMs();
// Loop for image acquisition and display
for (unsigned i = 0; i < nframes; i++) {
- //Acquires an RGBa image
+ // Acquires an RGBa image
g.acquire(I);
-#if (defined (VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
if (opt_display) {
- //Displays the grabbed rgba image
- vpDisplay::display(I);
+ // Displays the grabbed rgba image
+ vpDisplay::display(I);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false)) // A click to exit
break;
@@ -253,9 +256,8 @@ 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;
- }
- catch(vpException &e) {
+ std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -267,4 +269,3 @@ int main()
return 0;
}
#endif
-
diff --git a/example/device/framegrabber/grab1394Two.cpp b/example/device/framegrabber/grab1394Two.cpp
index 951d116..7604d72 100644
--- a/example/device/framegrabber/grab1394Two.cpp
+++ b/example/device/framegrabber/grab1394Two.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file grab1394Two.cpp
@@ -49,54 +49,42 @@
vp1394TwoGrabber was tested with MF-033C and F-131B Marlin cameras.
*/
-
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
#include <iostream>
-#include <sstream>
#include <list>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#if defined(VISP_HAVE_DC1394)
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpTime.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpRGBa.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#define GRAB_CxOLOR
-
// List of allowed command line options
-#define GETOPTARGS "b:c:df:g:hH:L:mn:io:p:rsT:v:W:"
+#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, 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 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);
/*!
@@ -113,11 +101,10 @@ void read_options(int argc, const char **argv, bool &multi, unsigned int &camera
\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);
@@ -224,10 +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());
}
/*!
@@ -270,81 +255,85 @@ OPTIONS Default\n\
\param panControl_is_set : Indicates if the pan contraol register
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.
*/
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
case 'c':
- camera = (unsigned int)atoi(optarg_); break;
+ camera = (unsigned int)atoi(optarg_);
+ break;
case 'd':
- display = false; break;
+ display = false;
+ break;
case 'f':
framerate_is_set = true;
- framerate = (vp1394TwoGrabber::vp1394TwoFramerateType) atoi(optarg_);
+ framerate = (vp1394TwoGrabber::vp1394TwoFramerateType)atoi(optarg_);
break;
case 'g':
colorcoding_is_set = true;
- colorcoding = (vp1394TwoGrabber::vp1394TwoColorCodingType) atoi(optarg_);
+ colorcoding = (vp1394TwoGrabber::vp1394TwoColorCodingType)atoi(optarg_);
break;
case 'H':
- roi_height = (unsigned int) atoi(optarg_); break;
+ roi_height = (unsigned int)atoi(optarg_);
+ break;
case 'i':
- verbose_info = true; break;
+ verbose_info = true;
+ break;
case 'L':
- roi_left = (unsigned int) atoi(optarg_); break;
+ roi_left = (unsigned int)atoi(optarg_);
+ break;
case 'm':
- multi = true; break;
+ multi = true;
+ break;
case 'n':
- nframes = (unsigned int)atoi(optarg_); break;
+ nframes = (unsigned int)atoi(optarg_);
+ break;
case 'o':
save = true;
- opath = optarg_; break;
+ opath = optarg_;
+ break;
case 'b':
ringbuffersize_is_set = true;
- ringbuffersize = (unsigned int) atoi(optarg_); break;
+ ringbuffersize = (unsigned int)atoi(optarg_);
+ break;
case 'p':
- panControl = (unsigned int) atoi(optarg_);
+ panControl = (unsigned int)atoi(optarg_);
panControl_is_set = true;
break;
case 'r':
- reset = true; break;
+ reset = true;
+ break;
case 's':
- verbose_settings = true; break;
+ verbose_settings = true;
+ break;
case 'T':
- roi_top = (unsigned int) atoi(optarg_); break;
+ roi_top = (unsigned int)atoi(optarg_);
+ break;
case 'v':
videomode_is_set = true;
- videomode = (vp1394TwoGrabber::vp1394TwoVideoModeType) atoi(optarg_);
+ videomode = (vp1394TwoGrabber::vp1394TwoVideoModeType)atoi(optarg_);
break;
case 'W':
- roi_width = (unsigned int) atoi(optarg_); break;
+ roi_width = (unsigned int)atoi(optarg_);
+ break;
case 'h':
case '?':
- usage(argv[0], NULL, camera, nframes, opath,
- roi_left, roi_top, roi_width, roi_height, ringbuffersize,
+ usage(argv[0], NULL, camera, nframes, opath, roi_left, roi_top, roi_width, roi_height, ringbuffersize,
panControl);
exit(0);
break;
@@ -353,8 +342,7 @@ void read_options(int argc, const char **argv, bool &multi, unsigned int &camera
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);
@@ -370,10 +358,9 @@ void read_options(int argc, const char **argv, bool &multi, unsigned int &camera
the libdc1394-2.0.0 driver. Display these images using X11 or GTK.
*/
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- try {
+ try {
unsigned int camera = 0;
bool multi = false;
bool verbose_info = false;
@@ -395,20 +382,14 @@ main(int argc, const char ** argv)
bool panControl_is_set = false;
// Format 7 roi
- unsigned int roi_left=0, roi_top=0, roi_width=0, roi_height=0;
+ unsigned int roi_left = 0, roi_top = 0, roi_width = 0, roi_height = 0;
// 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);
@@ -418,7 +399,7 @@ main(int argc, const char ** argv)
// F033C) a tempo of 1s is requested after a bus reset.
vpTime::wait(1000); // Wait 1000 ms
}
-
+
// Number of cameras connected on the bus
unsigned int ncameras = 0;
g.getNumCameras(ncameras);
@@ -434,7 +415,7 @@ main(int argc, const char ** argv)
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);
+ return (0);
}
}
if (camera >= ncameras) {
@@ -443,25 +424,24 @@ main(int argc, const char ** argv)
std::cout << "It is not possible to select camera " << camera << std::endl;
std::cout << "Check your -c <camera> command line option." << std::endl;
g.close();
- return(0);
+ return (0);
}
if (multi) {
camera = 0; // to over write a bad option usage
- }
- else {
+ } else {
ncameras = 1; // acquisition from only one camera
}
// Offset is used to set the correspondancy between and image and the
- // camera. For example, images comming from camera (i+offset) are available
- // in I[i]
+ // camera. For example, images comming from camera (i+offset) are
+ // available in I[i]
offset = camera;
// Display information for each camera
if (verbose_info || verbose_settings) {
- for (unsigned int i=0; i < ncameras; i ++) {
+ for (unsigned int i = 0; i < ncameras; i++) {
- g.setCamera(i+offset);
+ g.setCamera(i + offset);
if (verbose_info)
g.printCameraInfo();
@@ -484,67 +464,54 @@ 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
+ 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;
+ << "---- * 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)){
+ 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) {
vp1394TwoGrabber::vp1394TwoColorCodingType supcoding;
supcoding = *it_lcoding;
- if ( (curmode == supmode) && (supcoding == curcoding) )
- std::cout << " * "
- << vp1394TwoGrabber::colorCoding2string(supcoding)
- << " (-g " << (int)supcoding << ")" << std::endl;
+ if ((curmode == supmode) && (supcoding == curcoding))
+ 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 {
+ } else {
// Parse the list of supported framerates for a supported mode
g.getFramerateSupported(supmode, lfps);
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;
+ if ((curmode == supmode) && (supfps == curfps))
+ 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;
@@ -560,8 +527,7 @@ main(int argc, const char ** argv)
if (videomode_is_set) {
g.setCamera(camera);
g.setVideoMode(videomode);
- }
- else {
+ } else {
// get The actual video mode
g.setCamera(camera);
g.getVideoMode(videomode);
@@ -583,75 +549,71 @@ main(int argc, const char ** argv)
g.setFormat7ROI(roi_left, roi_top, roi_width, roi_height);
// Array to know if color images or grey level images are acquired
- bool *grab_color = new bool [ncameras];
+ bool *grab_color = new bool[ncameras];
#ifdef VISP_HAVE_X11
// allocate a display for each camera to consider
vpDisplayX *d = NULL;
if (display)
- d = new vpDisplayX [ncameras];
+ d = new vpDisplayX[ncameras];
#endif
// allocate an Grey and color image for each camera to consider
- vpImage<vpRGBa> *Ic = new vpImage<vpRGBa> [ncameras];
- vpImage<unsigned char> *Ig = new vpImage<unsigned char> [ncameras];
+ vpImage<vpRGBa> *Ic = new vpImage<vpRGBa>[ncameras];
+ vpImage<unsigned char> *Ig = new vpImage<unsigned char>[ncameras];
// Do a first acquisition to initialise the display
- for (unsigned int i=0; i < ncameras; i ++) {
+ for (unsigned int i = 0; i < ncameras; i++) {
// Set the active camera on the bus
- g.setCamera(i+offset);
+ g.setCamera(i + offset);
// Ask each camera to know if color images or grey level images are
// acquired
grab_color[i] = g.isColor();
// Acquire the first image
if (grab_color[i]) {
g.acquire(Ic[i]);
- std::cout << "Image size for camera " << i+offset << " : width: "
- << Ic[i].getWidth() << " height: " << Ic[i].getHeight()
- << std::endl;
+ std::cout << "Image size for camera " << i + offset << " : width: " << Ic[i].getWidth()
+ << " height: " << Ic[i].getHeight() << std::endl;
#ifdef VISP_HAVE_X11
if (display) {
// Initialise the display
char title[100];
- sprintf(title, "Images captured by camera %u", i+offset);
- d[i].init(Ic[i], (int)(100+i*50), (int)(100+i*50), title) ;
+ sprintf(title, "Images captured by camera %u", i + offset);
+ d[i].init(Ic[i], (int)(100 + i * 50), (int)(100 + i * 50), title);
vpDisplay::display(Ic[i]);
vpDisplay::flush(Ic[i]);
}
#endif
- }
- else {
+ } else {
g.acquire(Ig[i]);
- std::cout << "Image size for camera " << i+offset << " : width: "
- << Ig[i].getWidth() << " height: " << Ig[i].getHeight()
- << std::endl;
+ std::cout << "Image size for camera " << i + offset << " : width: " << Ig[i].getWidth()
+ << " height: " << Ig[i].getHeight() << std::endl;
#ifdef VISP_HAVE_X11
if (display) {
// Initialise the display
char title[100];
- sprintf(title, "Images captured by camera %u", i+offset);
- d[i].init(Ig[i], (int)(100+i*50), (int)(100+i*50), title) ;
+ sprintf(title, "Images captured by camera %u", i + offset);
+ d[i].init(Ig[i], (int)(100 + i * 50), (int)(100 + i * 50), title);
vpDisplay::display(Ig[i]);
vpDisplay::flush(Ig[i]);
}
#endif
-
}
}
// Main loop for single or multi-camera acquisition and display
std::cout << "Capture in process..." << std::endl;
- double tbegin=0, ttotal=0;
+ double tbegin = 0, ttotal = 0;
ttotal = 0;
tbegin = vpTime::measureTimeMs();
for (unsigned int i = 0; i < nframes; i++) {
for (unsigned int c = 0; c < ncameras; c++) {
// Set the active camera on the bus
- g.setCamera(c+offset);
+ g.setCamera(c + offset);
// Acquire an image
if (grab_color[c]) {
g.acquire(Ic[c]);
@@ -662,8 +624,7 @@ main(int argc, const char ** argv)
vpDisplay::flush(Ic[c]);
}
#endif
- }
- else {
+ } else {
g.acquire(Ig[c]);
#ifdef VISP_HAVE_X11
if (display) {
@@ -672,17 +633,15 @@ main(int argc, const char ** argv)
vpDisplay::flush(Ig[c]);
}
#endif
-
}
if (save) {
char buf[FILENAME_MAX];
- sprintf(buf, opath.c_str(), c+offset, i);
+ sprintf(buf, opath.c_str(), c + offset, i);
std::string filename(buf);
std::cout << "Write: " << filename << std::endl;
if (grab_color[c]) {
vpImageIo::write(Ic[c], filename);
- }
- else {
+ } else {
vpImageIo::write(Ig[c], filename);
}
}
@@ -695,34 +654,32 @@ 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();
// Free memory
- delete [] Ic;
- delete [] Ig;
- delete [] grab_color;
+ delete[] Ic;
+ delete[] Ig;
+ delete[] grab_color;
#ifdef VISP_HAVE_X11
if (display)
- delete [] d;
+ delete[] d;
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
- "You should install libdc1394-2 to use this example.") ;
+ "You should install libdc1394-2 to use this example.");
}
#endif
diff --git a/example/device/framegrabber/grabDirectShow.cpp b/example/device/framegrabber/grabDirectShow.cpp
index b6c64be..63cfe06 100644
--- a/example/device/framegrabber/grabDirectShow.cpp
+++ b/example/device/framegrabber/grabDirectShow.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,20 +48,19 @@
*/
-#if defined (VISP_HAVE_DIRECTSHOW)
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+#if defined(VISP_HAVE_DIRECTSHOW)
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-
-#include <visp3/sensor/vpDirectShowGrabber.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpTime.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
// List of allowed command line options
-#define GETOPTARGS "dhn:o:"
+#define GETOPTARGS "dhn:o:"
/*!
@@ -98,7 +98,7 @@ OPTIONS: Default\n\
Print the help.\n\
\n", nframes, opath.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -118,25 +118,32 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'd': display = false; break;
+ case 'd':
+ display = false;
+ break;
case 'n':
- nframes = atoi(optarg); break;
+ nframes = atoi(optarg);
+ break;
case 'o':
save = true;
- opath = optarg; break;
- case 'h': usage(argv[0], NULL, nframes, opath); return false; break;
+ opath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, nframes, opath);
+ return false;
+ break;
default:
usage(argv[0], optarg, nframes, opath);
- return false; break;
+ return false;
+ break;
}
}
@@ -151,32 +158,30 @@ bool getOptions(int argc, const char **argv, bool &display,
return true;
}
-
/*!
\example grabDirectShow.cpp
Example of framegrabbing using vpDirectShowGrabber class.
- Grab grey level images using DirectShow frame grabbing capabilities. Display the
- images using the GTK or GDI display.
+ Grab grey level images using DirectShow frame grabbing capabilities. Display
+ the images using the GTK or GDI display.
*/
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
unsigned nframes = 50;
bool save = false;
- // Declare an image. It size is not defined yet. It will be defined when the
- // image will acquired the first time.
+// Declare an image. It size is not defined yet. It will be defined when the
+// image will acquired the first time.
#ifdef GRAB_COLOR
vpImage<vpRGBa> I; // This is a color image (in RGBa format)
#else
vpImage<unsigned char> I; // This is a B&W image
#endif
- // Set default output image name for saving
+// Set default output image name for saving
#ifdef GRAB_COLOR
// Color images will be saved in PGM P6 format
std::string opath = "C:/temp/I%04d.ppm";
@@ -187,15 +192,14 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_display, nframes, save, opath) == false) {
- exit (-1);
+ exit(-1);
}
// Create the grabber
- vpDirectShowGrabber* grabber = new vpDirectShowGrabber();
+ vpDirectShowGrabber *grabber = new vpDirectShowGrabber();
- //test if a camera is connected
- if(grabber->getDeviceNumber() == 0)
- {
- vpCTRACE << "there is no camera detected on your computer." << std::endl ;
+ // test if a camera is connected
+ if (grabber->getDeviceNumber() == 0) {
+ vpCTRACE << "there is no camera detected on your computer." << std::endl;
grabber->close();
exit(0);
}
@@ -205,11 +209,9 @@ 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
+// Creates a display
#if defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
@@ -217,20 +219,20 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- display.init(I,100,100,"DirectShow Framegrabber");
+ display.init(I, 100, 100, "DirectShow Framegrabber");
}
- double tbegin=0, ttotal=0;
+ double tbegin = 0, ttotal = 0;
ttotal = 0;
tbegin = vpTime::measureTimeMs();
// Loop for image acquisition and display
for (unsigned i = 0; i < nframes; i++) {
- //Acquires an RGBa image
+ // Acquires an RGBa image
grabber->acquire(I);
if (opt_display) {
- //Displays the grabbed rgba image
+ // Displays the grabbed rgba image
vpDisplay::display(I);
vpDisplay::flush(I);
}
@@ -249,31 +251,20 @@ 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;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-#else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+#else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-int
-main()
-{
- vpTRACE("GDI or GTK is not available...") ;
-}
+int main() { vpTRACE("GDI or GTK is not available..."); }
#endif // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-#else // defined (VISP_HAVE_DIRECTSHOW)
-int
-main()
-{
- vpTRACE("DirectShow is not available...") ;
-}
-#endif // defined (VISP_HAVE_DIRECTSHOW)
-
-
+#else // defined (VISP_HAVE_DIRECTSHOW)
+int main() { vpTRACE("DirectShow is not available..."); }
+#endif // defined (VISP_HAVE_DIRECTSHOW)
diff --git a/example/device/framegrabber/grabDirectShowMulti.cpp b/example/device/framegrabber/grabDirectShowMulti.cpp
index b500a15..214bf7c 100644
--- a/example/device/framegrabber/grabDirectShowMulti.cpp
+++ b/example/device/framegrabber/grabDirectShowMulti.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -53,22 +54,20 @@
#include <iostream>
#include <sstream>
+#if defined(VISP_HAVE_DIRECTSHOW)
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-#if defined (VISP_HAVE_DIRECTSHOW)
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-
-#include <visp3/sensor/vpDirectShowGrabber.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpTime.h>
-
+#include <visp3/sensor/vpDirectShowGrabber.h>
// List of allowed command line options
//#define GETOPTARGS "dhn:o:"
-#define GETOPTARGS "c:df:hmn:io:st:?"
+#define GETOPTARGS "c:df:hmn:io:st:?"
#define GRAB_COLOR
@@ -83,13 +82,12 @@ 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);
+ if (badparam)
+ fprintf(stderr, "\nERREUR: Bad parameter [%s]\n", badparam);
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
Acquire images using DirectShow (under Windows only) and display\n\
it using GTK or the windows GDI if GTK is not available.\n\
For a given camera, mediatype (or video mode) as well as framerate\n\
@@ -111,10 +109,9 @@ OPTIONS Default\n\
\n\
-f [%%d] \n\
Framerate to set for the active camera.\n\
- You can select the active camera using -c option.\n",
- name);
+ You can select the active camera using -c option.\n", name);
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
-c [%%u] %u\n\
Active camera identifier.\n\
Zero is for the first camera found on the bus.\n\
@@ -144,10 +141,9 @@ OPTIONS Default\n\
\n\
-h \n\
Print the help.\n\
-\n",
- camera, nframes, opath.c_str());
+\n", camera, nframes, opath.c_str());
- exit(0);
+ exit(0);
}
/*!
@@ -175,305 +171,291 @@ 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;
- /*
- * Lecture des options.
- */
-
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS,&optarg)) > 1) {
- switch (c) {
+ const char *optarg;
+ int c;
+ /*
+ * Lecture des options.
+ */
+
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
+ switch (c) {
case 'c':
- camera = atoi(optarg); break;
+ camera = atoi(optarg);
+ break;
case 'd':
- display = false; break;
+ display = false;
+ break;
case 'f':
framerate_is_set = true;
- framerate = atoi(optarg); break;
+ framerate = atoi(optarg);
+ break;
case 'i':
- verbose_info = true; break;
+ verbose_info = true;
+ break;
case 'm':
- multi = true; break;
+ multi = true;
+ break;
case 'n':
- nframes = atoi(optarg); break;
+ nframes = atoi(optarg);
+ break;
case 'o':
save = true;
- opath = optarg; break;
+ opath = optarg;
+ break;
case 's':
- verbose_settings = true; break;
+ verbose_settings = true;
+ break;
case 't':
mediatype_is_set = true;
- mediatypeID = atoi(optarg); break;
+ mediatypeID = atoi(optarg);
+ break;
default:
usage(argv[0], NULL, camera, nframes, opath);
break;
- }
- }
+ }
+ }
- if ((c == 1) || (c == -1)) {
+ if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL, camera, nframes, opath);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg << std::endl << std::endl;
- }
+ }
}
-
/*!
\example grabDirectShowMulti.cpp
Example of framegrabbing using vpDirectShowGrabber class.
-Grab grey level images using DirectShow frame grabbing capabilities. Display the
-images using the GTK or GDI display.
+Grab grey level images using DirectShow frame grabbing capabilities. Display
+the images using the GTK or GDI display.
*/
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- try {
- unsigned int camera = 0;
- bool multi = false;
- bool verbose_info = false;
- bool verbose_settings = false;
- bool display = true;
- unsigned int nframes = 50;
- bool mediatype_is_set = false;
- unsigned int mediatypeID;
- bool framerate_is_set = false;
- double framerate;
- bool save = false;
+ try {
+ unsigned int camera = 0;
+ bool multi = false;
+ bool verbose_info = false;
+ bool verbose_settings = false;
+ bool display = true;
+ unsigned int nframes = 50;
+ bool mediatype_is_set = false;
+ unsigned int mediatypeID;
+ bool framerate_is_set = false;
+ double framerate;
+ bool save = false;
#ifdef GRAB_COLOR
- vpImage<vpRGBa> *I;
- std::string opath = "C:/temp/I%d-%04d.ppm";
+ vpImage<vpRGBa> *I;
+ std::string opath = "C:/temp/I%d-%04d.ppm";
#else
- vpImage<unsigned char> *I;
- std::string opath = "C:/temp/I%d-%04d.pgm";
+ vpImage<unsigned char> *I;
+ std::string opath = "C:/temp/I%d-%04d.pgm";
#endif
#if defined VISP_HAVE_GDI
- vpDisplayGDI *d;
+ vpDisplayGDI *d;
#elif defined VISP_HAVE_GTK
- vpDisplayGTK *d;
+ vpDisplayGTK *d;
#endif
- 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
- vpDirectShowGrabber *g;
- g = new vpDirectShowGrabber();
- unsigned int ncameras = g->getDeviceNumber();
- // Check the consistancy of the options
- 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 << "cameras on the bus." << std::endl;
- g->close();
+ 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
+ vpDirectShowGrabber *g;
+ g = new vpDirectShowGrabber();
+ unsigned int ncameras = g->getDeviceNumber();
+ // Check the consistancy of the options
+ 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 << "cameras on the bus." << std::endl;
+ g->close();
delete g;
- return(0);
- }
- }
- 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 << "Check your -c <camera> command line option." << std::endl;
- g->close();
+ return (0);
+ }
+ }
+ 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 << "Check your -c <camera> command line option." << std::endl;
+ g->close();
delete g;
- return(0);
- }
- if (multi) {
- camera = 0; // to over write a bad option usage
- //reinitialize the grabbers with the right number of devices (one grabber per device)
- delete [] g;
- g = new vpDirectShowGrabber[ncameras];
- for(unsigned int i=0; i<ncameras ; i++)
- {
- g[i].open();
- }
-
- }
- else {
- ncameras = 1; // acquisition from only one camera
- delete [] g;
- g = new vpDirectShowGrabber[1];
- g[0].open();
- g[0].setDevice(camera);
-
- }
-
- // allocate an image and display for each camera to consider
+ return (0);
+ }
+ if (multi) {
+ camera = 0; // to over write a bad option usage
+ // reinitialize the grabbers with the right number of devices (one
+ // grabber per device)
+ delete[] g;
+ g = new vpDirectShowGrabber[ncameras];
+ for (unsigned int i = 0; i < ncameras; i++) {
+ g[i].open();
+ }
+
+ } else {
+ ncameras = 1; // acquisition from only one camera
+ delete[] g;
+ g = new vpDirectShowGrabber[1];
+ g[0].open();
+ g[0].setDevice(camera);
+ }
+
+// allocate an image and display for each camera to consider
#ifdef GRAB_COLOR
- I = new vpImage<vpRGBa> [ncameras];
+ I = new vpImage<vpRGBa>[ncameras];
#else
- I = new vpImage<unsigned char> [ncameras];
+ I = new vpImage<unsigned char>[ncameras];
#endif
- if (display)
+ if (display)
#ifdef VISP_HAVE_GDI
- d = new vpDisplayGDI [ncameras];
+ d = new vpDisplayGDI[ncameras];
#else
- d = new vpDisplayGTK [ncameras];
+ d = new vpDisplayGTK[ncameras];
#endif
// If required modify camera settings
- if (mediatype_is_set) {
- g[0].setMediaType(mediatypeID);
- }
-
- if (framerate_is_set) {
- for(unsigned int i=0; i<ncameras ; i++)
- {
- unsigned int c;
- if (multi) c = i;
- else c = camera;
- std::cout<<"camera " << c <<std::endl;
- if (!g[i].setFramerate(framerate))
- std::cout << "Set Framerate failed !!" <<std::endl<< std::endl;
- }
- }
-
- // Display information for each camera
- if (verbose_info || verbose_settings) {
-
- std::cout << "----------------------------------------------------------" << std::endl;
- std::cout << "---- Device List : " << std::endl;
- std::cout << "----------------------------------------------------------" << std::endl;
- g[0].displayDevices();
- for (unsigned i=0; i < ncameras; i ++) {
- unsigned int c;
- if (multi) c = i;
- else c = camera;
-
- if (verbose_info){
+ if (mediatype_is_set) {
+ g[0].setMediaType(mediatypeID);
+ }
+
+ if (framerate_is_set) {
+ for (unsigned int i = 0; i < ncameras; i++) {
+ unsigned int c;
+ if (multi)
+ c = i;
+ else
+ c = camera;
+ std::cout << "camera " << c << std::endl;
+ if (!g[i].setFramerate(framerate))
+ std::cout << "Set Framerate failed !!" << std::endl << std::endl;
+ }
+ }
+
+ // Display information for each camera
+ if (verbose_info || verbose_settings) {
+
+ std::cout << "----------------------------------------------------------" << std::endl;
+ std::cout << "---- Device List : " << std::endl;
+ std::cout << "----------------------------------------------------------" << std::endl;
+ g[0].displayDevices();
+ for (unsigned i = 0; i < ncameras; i++) {
+ unsigned int c;
+ if (multi)
+ c = i;
+ else
+ c = camera;
+
+ if (verbose_info) {
unsigned int width, height;
g[i].getFormat(width, height, framerate);
- std::cout << "----------------------------------------------------------"
- << std::endl
+ std::cout << "----------------------------------------------------------" << std::endl
<< "---- MediaType and framerate currently used by device " << std::endl
- << "---- (or camera) " << c << std::endl
+ << "---- (or camera) " << c << std::endl
<< "---- Current MediaType : " << g[i].getMediaType() << std::endl
- << "---- Current format : " << width <<" x "<< height <<" at "<< framerate << " fps" << 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
+ 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 <mediatype>."
- << std::endl
- << "----------------------------------------------------------"
- << std::endl;
- g[i].getStreamCapabilities();
- }
-
- }
- delete [] g;
- delete [] I;
+ << "---- option -t <mediatype>." << std::endl
+ << "----------------------------------------------------------" << std::endl;
+ g[i].getStreamCapabilities();
+ }
+ }
+ delete[] g;
+ delete[] I;
if (display)
- delete [] d;
-
- return 0;
- }
-
-
- // Do a first acquisition to initialise the display
- for (unsigned int i=0; i < ncameras; i ++) {
- // Acquire the first image
- g[i].acquire(I[i]);
- unsigned int c;
- if (multi) c = i;
- else c = camera;
-
- std::cout << "Image size for camera " << c << " : width: "
- << I[i].getWidth() << " height: " << I[i].getHeight() << std::endl;
-
- if (display) {
- // Initialise the display
- char title[100];
- sprintf(title, "Images captured by camera %u", c );
- d[i].init(I[i], 100+i*50, 100+i*50, title) ;
- }
- }
-
- // Main loop for single or multi-camera acquisition and display
- std::cout << "Capture in process..." << std::endl;
-
- double tbegin=0, ttotal=0;
-
- ttotal = 0;
- tbegin = vpTime::measureTimeMs();
- for (unsigned i = 0; i < nframes; i++) {
- for (unsigned c = 0; c < ncameras; c++) {
- // Acquire an image
- g[c].acquire(I[c]);
- if (display) {
- // Display the last image acquired
- vpDisplay::display(I[c]);
- vpDisplay::flush(I[c]);
- }
- if (save) {
- char buf[FILENAME_MAX];
- sprintf(buf, opath.c_str(), c, i);
- std::string filename(buf);
- std::cout << "Write: " << filename << std::endl;
+ delete[] d;
+
+ return 0;
+ }
+
+ // Do a first acquisition to initialise the display
+ for (unsigned int i = 0; i < ncameras; i++) {
+ // Acquire the first image
+ g[i].acquire(I[i]);
+ unsigned int c;
+ if (multi)
+ c = i;
+ else
+ c = camera;
+
+ std::cout << "Image size for camera " << c << " : width: " << I[i].getWidth() << " height: " << I[i].getHeight()
+ << std::endl;
+
+ if (display) {
+ // Initialise the display
+ char title[100];
+ sprintf(title, "Images captured by camera %u", c);
+ d[i].init(I[i], 100 + i * 50, 100 + i * 50, title);
+ }
+ }
+
+ // Main loop for single or multi-camera acquisition and display
+ std::cout << "Capture in process..." << std::endl;
+
+ double tbegin = 0, ttotal = 0;
+
+ ttotal = 0;
+ tbegin = vpTime::measureTimeMs();
+ for (unsigned i = 0; i < nframes; i++) {
+ for (unsigned c = 0; c < ncameras; c++) {
+ // Acquire an image
+ g[c].acquire(I[c]);
+ if (display) {
+ // Display the last image acquired
+ vpDisplay::display(I[c]);
+ vpDisplay::flush(I[c]);
+ }
+ if (save) {
+ char buf[FILENAME_MAX];
+ sprintf(buf, opath.c_str(), c, i);
+ std::string filename(buf);
+ std::cout << "Write: " << filename << std::endl;
vpImageIo::write(I[c], filename);
- }
- }
+ }
+ }
double tend = vpTime::measureTimeMs();
double tloop = tend - tbegin;
- tbegin = tend;
- std::cout << "loop time: " << tloop << " ms" << std::endl;
- ttotal += tloop;
- }
+ tbegin = tend;
+ std::cout << "loop time: " << tloop << " ms" << std::endl;
+ 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 loop time: " << ttotal / nframes << " ms" << std::endl;
+ std::cout << "Mean frequency: " << 1000. / (ttotal / nframes) << " fps" << std::endl;
- // Release the framegrabber
- delete [] g;
+ // Release the framegrabber
+ delete[] g;
- // Free memory
- delete [] I;
- if (display)
- delete [] d;
+ // Free memory
+ delete[] I;
+ if (display)
+ delete[] d;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-#else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+#else // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-int
-main()
-{
- vpTRACE("GDI or GTK is not available...") ;
-}
+int main() { vpTRACE("GDI or GTK is not available..."); }
#endif // (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
-#else // defined (VISP_HAVE_DIRECTSHOW)
-int
-main()
-{
- vpTRACE("DirectShow is not available...") ;
-}
-#endif // defined (VISP_HAVE_DIRECTSHOW)
-
+#else // defined (VISP_HAVE_DIRECTSHOW)
+int main() { vpTRACE("DirectShow is not available..."); }
+#endif // defined (VISP_HAVE_DIRECTSHOW)
diff --git a/example/device/framegrabber/grabDisk.cpp b/example/device/framegrabber/grabDisk.cpp
index 19392d8..f312734 100644
--- a/example/device/framegrabber/grabDisk.cpp
+++ b/example/device/framegrabber/grabDisk.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,38 +37,37 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
-#include <visp3/io/vpDiskGrabber.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpTime.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpDiskGrabber.h>
+#include <visp3/io/vpParseArgv.h>
/*!
\file grabDisk.cpp
- \brief Example of image sequence reading from the disk using vpDiskGrabber class.
+ \brief Example of image sequence reading from the disk using vpDiskGrabber
+ class.
The sequence is made of separate images. Each image corresponds to a PGM
file.
*/
// List of allowed command line options
-#define GETOPTARGS "b:de:f:i:hn:s:z:"
+#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, 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);
+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);
/*
@@ -84,8 +84,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ba
\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, unsigned int nimages, int step, unsigned int nzero)
+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\
Read an image sequence from the disk. Display it using X11 or GTK.\n\
@@ -101,7 +101,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ From this path read \"cube/image.%%04d.pgm\"\n\
images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -136,9 +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);
@@ -161,29 +159,47 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'b': basename = optarg_; break;
- case 'd': display = false; break;
- case 'e': ext = optarg_; break;
- case 'f': first = atoi(optarg_); break;
- case 'i': ipath = optarg_; break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = atoi(optarg_); break;
- case 'z': nzero = (unsigned) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, basename, ext, first, nimages,
- step, nzero); return false; break;
+ case 'b':
+ basename = optarg_;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'e':
+ ext = optarg_;
+ break;
+ case 'f':
+ first = atoi(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = atoi(optarg_);
+ break;
+ case 'z':
+ nzero = (unsigned)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, basename, ext, first, nimages, step, nzero);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, basename, ext, first, nimages, step, nzero);
- return false; break;
+ return false;
+ break;
}
}
@@ -198,7 +214,6 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ba
return true;
}
-
/*!
\example grabDisk.cpp
@@ -208,13 +223,13 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ba
images. Each image corresponds to a PGM file. Display these images using X11
or GTK.
*/
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
std::string opt_ipath;
std::string ipath;
- std::string opt_basename = "ViSP-images/cube/image.";
+ std::string opt_basename = "cube/image.";
std::string opt_ext = "pgm";
bool opt_display = true;
@@ -223,17 +238,18 @@ int main(int argc, const char ** argv)
int opt_step = 1;
unsigned int opt_nzero = 4;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_basename, opt_ext, opt_first, opt_nimages, opt_step, opt_nzero,
+ opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -244,8 +260,7 @@ int main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -253,24 +268,20 @@ int main(int argc, const char ** argv)
}
// 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);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
-
+ vpImage<unsigned char> I;
// Declare a framegrabber able to read a sequence of successive
// images from the disk
@@ -291,13 +302,12 @@ int main(int argc, const char ** argv)
g.setExtension(opt_ext.c_str());
// Open the framegrabber by loading the first image of the sequence
- g.open(I) ;
+ 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
+// We open a window using either X11 or GDI.
+// Its size is automatically defined by the image (I) size
#if defined(VISP_HAVE_X11)
vpDisplayX display(I);
#elif defined(VISP_HAVE_GDI)
@@ -307,49 +317,42 @@ int main(int argc, const char ** argv)
#endif
if (opt_display) {
- display.init(I,100,100,"Disk Framegrabber");
+ display.init(I, 100, 100, "Disk Framegrabber");
// display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
unsigned cpt = 1;
// this is the loop over the image sequence
- while(cpt ++ < opt_nimages)
- {
+ while (cpt++ < opt_nimages) {
double tms = vpTime::measureTimeMs();
// read the image and then increment the image counter so that the next
// call to acquire(I) will get the next image
- g.acquire(I) ;
+ g.acquire(I);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Synchronise the loop to 40 ms
- vpTime::wait(tms, 40) ;
+ vpTime::wait(tms, 40);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 32a67c8..c57175d 100644
--- a/example/device/framegrabber/grabFlyCapture.cpp
+++ b/example/device/framegrabber/grabFlyCapture.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,16 +51,16 @@
#if defined(VISP_HAVE_FLYCAPTURE)
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vpFlyCaptureGrabber.h>
-#define GETOPTARGS "cdhi:n:o:"
+#define GETOPTARGS "cdhi:n:o:"
/*!
@@ -71,9 +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\
@@ -102,7 +101,7 @@ OPTIONS: Default\n\
\n", icamera, opath.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -123,25 +122,36 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click = false; break;
- case 'd': display = false; break;
- case 'i': icamera = (unsigned int)atoi(optarg_); break;
+ case 'c':
+ click = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ icamera = (unsigned int)atoi(optarg_);
+ break;
case 'o':
save = true;
- opath = optarg_; break;
- case 'h': usage(argv[0], NULL, icamera, opath); return false; break;
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, icamera, opath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, icamera, opath);
- return false; break;
+ return false;
+ break;
}
}
@@ -157,9 +167,9 @@ bool getOptions(int argc, const char **argv, bool &display, bool &click,
}
// usage: binary <device name>
-// device name: 0 is the default to dial with the first camera,
+// device name: 0 is the default to dial with the first camera,
// 1 to dial with a second camera attached to the computer
-int main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -167,12 +177,11 @@ int main(int argc, const char** argv)
bool opt_save = false;
unsigned int opt_icamera = 0;
std::string opt_opath = "I%04d.pgm";
- //vpImage<vpRGBa> I; // for color images
+ // vpImage<vpRGBa> I; // for color images
vpImage<unsigned char> 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;
}
@@ -180,7 +189,7 @@ 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 << "Camera serial: " << g.getCameraSerial(g.getCameraIndex()) << std::endl;
std::cout << "Image size : " << I.getWidth() << " " << I.getHeight() << std::endl;
vpDisplay *display = NULL;
@@ -196,7 +205,7 @@ int main(int argc, const char** argv)
#endif
}
- for(;;) {
+ for (;;) {
g.acquire(I); // get a new frame from camera
if (opt_save) {
@@ -214,29 +223,25 @@ int main(int argc, const char** argv)
if (opt_click && opt_display) {
if (vpDisplay::getClick(I, false) == true)
break;
- }
- else {
+ } else {
static unsigned int cpt = 0;
- if (cpt ++ == 10)
+ if (cpt++ == 10)
break;
}
}
if (display)
delete display;
- // The camera connection will be closed automatically in vpFlyCapture destructor
+ // The camera connection will be closed automatically in vpFlyCapture
+ // destructor
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return 0;
}
}
#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 67bd1c9..7ee7430 100644
--- a/example/device/framegrabber/grabOpenCV.cpp
+++ b/example/device/framegrabber/grabOpenCV.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,16 +51,15 @@
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/sensor/vpOpenCVGrabber.h>
-
// usage: binary <device name>
-// device name: 0 is the default to dial with the first camera,
+// device name: 0 is the default to dial with the first camera,
// 1 to dial with a second camera attached to the computer
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
try {
int device = 0;
@@ -75,28 +75,27 @@ int main(int argc, char** argv)
cap.set(CV_CAP_PROP_FRAME_WIDTH, 640);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
#endif
- if(!cap.isOpened()) // check if we succeeded
+ if (!cap.isOpened()) // check if we succeeded
return -1;
cv::Mat frame;
- int i=0;
- while ((i++ < 100) && !cap.read(frame)) {}; // warm up camera by skiping unread frames
+ int i = 0;
+ while ((i++ < 100) && !cap.read(frame)) {
+ }; // warm up camera by skiping unread frames
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<vpRGBa> I; // for color images
+ // vpImage<vpRGBa> I; // for color images
vpImage<unsigned char> I; // for gray images
vpImageConvert::convert(frame, I);
vpDisplayOpenCV d(I);
- for(;;) {
+ for (;;) {
cap >> frame; // get a new frame from camera
// Convert the image in ViSP format and display it
@@ -106,19 +105,16 @@ int main(int argc, char** argv)
if (vpDisplay::getClick(I, false)) // a click to exit
break;
}
- // the camera will be deinitialized automatically in VideoCapture destructor
+ // the camera will be deinitialized automatically in VideoCapture
+ // destructor
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int main()
-{
- std::cout << "OpenCV is not available..." << std::endl;
-}
+int main() { std::cout << "OpenCV is not available..." << std::endl; }
#endif
diff --git a/example/device/framegrabber/grabRealSense.cpp b/example/device/framegrabber/grabRealSense.cpp
index 9c5c6c9..f2eeb58 100644
--- a/example/device/framegrabber/grabRealSense.cpp
+++ b/example/device/framegrabber/grabRealSense.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,51 +44,45 @@
#include <iostream>
-#include <visp3/sensor/vpRealSense.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpMutex.h>
#include <visp3/core/vpThread.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense.h>
#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
-// Using a thread to display the pointcloud with PCL produces a segfault on OSX
-#if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX
-# if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
-# define USE_THREAD
-# endif
+// Using a thread to display the pointcloud with PCL produces a segfault on
+// OSX
+#if (!defined(__APPLE__) && !defined(__MACH__)) // Not OSX
+#if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
+#define USE_THREAD
+#endif
#endif
#ifdef VISP_HAVE_PCL
-# include <pcl/visualization/cloud_viewer.h>
-# include <pcl/visualization/pcl_visualizer.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
#endif
#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<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *)args);
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
- viewer->setBackgroundColor (0, 0, 0);
- viewer->addCoordinateSystem (1.0);
- viewer->initCameraParameters ();
- viewer->setPosition(640+80, 480+80);
- viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setPosition(640 + 80, 480 + 80);
+ viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
viewer->setSize(640, 480);
t_CaptureState capture_state_;
@@ -99,18 +94,17 @@ vpThread::Return displayPointcloudFunction(vpThread::Args args)
if (capture_state_ == capture_started) {
static bool update = false;
- if (! update) {
- viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ if (!update) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(pointcloud_, rgb, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
update = true;
- }
- else {
- viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZRGB>(pointcloud_, rgb, "sample cloud");
}
- viewer->spinOnce (10);
+ viewer->spinOnce(10);
}
- } while(capture_state_ != capture_stopped);
+ } while (capture_state_ != capture_stopped);
std::cout << "End of point cloud display thread" << std::endl;
return 0;
@@ -124,22 +118,30 @@ int main()
#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
try {
vpRealSense rs;
- //rs.setDeviceBySerialNumber("541142003219");
-
+ 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::endl;
- std::cout << rs.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
+ 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::endl;
- vpImage<vpRGBa> color((unsigned int) rs.getIntrinsics(rs::stream::color).height, (unsigned int) rs.getIntrinsics(rs::stream::color).width);
- vpImage<uint16_t> infrared;
- vpImage<unsigned char> infrared_display((unsigned int) rs.getIntrinsics(rs::stream::infrared).height, (unsigned int) rs.getIntrinsics(rs::stream::infrared).width);;
- vpImage<uint16_t> depth;
- vpImage<vpRGBa> depth_display((unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width);
+ vpImage<vpRGBa> color((unsigned int)rs.getIntrinsics(rs::stream::color).height,
+ (unsigned int)rs.getIntrinsics(rs::stream::color).width);
+
+ vpImage<unsigned char> infrared_display((unsigned int)rs.getIntrinsics(rs::stream::infrared).height,
+ (unsigned int)rs.getIntrinsics(rs::stream::infrared).width);
+ vpImage<uint16_t> infrared_y16;
+ rs::device *dev = rs.getHandler();
+ bool use_infrared_y16 = dev->get_stream_format(rs::stream::infrared) == rs::format::y16;
+
+ vpImage<vpRGBa> depth_display((unsigned int)rs.getIntrinsics(rs::stream::depth).height,
+ (unsigned int)rs.getIntrinsics(rs::stream::depth).width);
+ vpImage<uint16_t> depth(depth_display.getHeight(), depth_display.getWidth());
#ifdef VISP_HAVE_PCL
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
@@ -147,12 +149,12 @@ int main()
#ifdef USE_THREAD
vpThread thread_display_pointcloud(displayPointcloudFunction, (vpThread::Args)&pointcloud);
#else
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
- viewer->setBackgroundColor (0, 0, 0);
- viewer->addCoordinateSystem (1.0);
- viewer->initCameraParameters ();
- viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->addCoordinateSystem(1.0);
+ viewer->initCameraParameters();
+ viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
#endif
#else
@@ -161,19 +163,31 @@ 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 dd(depth_display, 10, color.getHeight()+80, "Depth 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;
#endif
- while(1) {
+ while (1) {
double t = vpTime::measureTimeMs();
- rs.acquire(color, infrared, depth, pointcloud);
+
+ if (use_infrared_y16) {
+ rs.acquire(color, infrared_y16, depth, pointcloud);
+ vpImageConvert::convert(infrared_y16, infrared_display);
+ } else {
+#ifdef VISP_HAVE_PCL
+ 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,
+ (unsigned char *)infrared_display.bitmap);
+#endif
+ }
#ifdef VISP_HAVE_PCL
#ifdef USE_THREAD
@@ -183,21 +197,19 @@ int main()
}
#else
static bool update = false;
- if (! update) {
- viewer->addPointCloud<pcl::PointXYZRGB> (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<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
- }
-
- viewer->spinOnce (10);
+ if (!update) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(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<pcl::PointXYZRGB>(pointcloud, rgb, "sample cloud");
+ }
+
+ viewer->spinOnce(10);
#endif
#endif
- vpImageConvert::convert(infrared, infrared_display);
vpImageConvert::createDepthHistogram(depth, depth_display);
vpDisplay::display(color);
@@ -205,7 +217,8 @@ 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) || vpDisplay::getClick(depth_display, false)) {
+ if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared_display, false) ||
+ vpDisplay::getClick(depth_display, false)) {
break;
}
vpDisplay::flush(color);
@@ -227,22 +240,21 @@ int main()
#endif
rs.close();
- }
- catch(const vpException &e) {
- std::cerr << "RealSense error " << e.getStringMessage() << std::endl;
- }
- catch(const rs::error & e) {
- std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "): " << e.what() << std::endl;
- }
- catch(const std::exception & e) {
+ } 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;
+ } catch (const std::exception &e) {
std::cerr << e.what() << std::endl;
}
#elif !defined(VISP_HAVE_REALSENSE)
std::cout << "Install RealSense SDK to make this test working" << std::endl;
#elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
- std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) to make this test working" << std::endl;
+ std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) "
+ "to make this test working"
+ << std::endl;
#endif
return 0;
}
-
diff --git a/example/device/framegrabber/grabRealSense2.cpp b/example/device/framegrabber/grabRealSense2.cpp
new file mode 100644
index 0000000..78ee471
--- /dev/null
+++ b/example/device/framegrabber/grabRealSense2.cpp
@@ -0,0 +1,244 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Acquisition with RealSense RGB-D sensor and librealsense2.
+ *
+ *****************************************************************************/
+
+/*!
+ \example grabRealSense2.cpp
+ This example shows how to retrieve data from a RealSense RGB-D sensor with
+ librealsense2.
+*/
+
+#include <iostream>
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <thread>
+
+namespace
+{
+// Global variables
+pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+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) {}
+
+ void run()
+ {
+ std::string date = vpTime::getDateTime();
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date));
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setPosition(640 + 80, 480 + 80);
+ viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
+ viewer->setSize(640, 480);
+
+ bool init = true;
+ bool local_update = false, local_cancelled = false;
+ while (!local_cancelled) {
+ {
+ std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
+
+ if (lock.owns_lock()) {
+ local_update = update_pointcloud;
+ update_pointcloud = false;
+ local_cancelled = cancelled;
+
+ if (local_update) {
+ if (m_colorMode) {
+ local_pointcloud_color = pointcloud_color->makeShared();
+ } else {
+ local_pointcloud = pointcloud->makeShared();
+ }
+ }
+ }
+ }
+
+ if (local_update && !local_cancelled) {
+ local_update = false;
+
+ if (init) {
+ if (m_colorMode) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
+ "RGB sample cloud");
+ } else {
+ viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ }
+ init = false;
+ } else {
+ if (m_colorMode) {
+ viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ }
+ }
+ }
+
+ viewer->spinOnce(5);
+ }
+
+ std::cout << "End of point cloud display thread" << std::endl;
+ }
+
+private:
+ bool m_colorMode;
+ std::mutex &m_mutex;
+};
+}
+#endif
+
+int main()
+{
+ try {
+ vpRealSense2 rs;
+ rs2::config config;
+ config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+ 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::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<vpRGBa> color((unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).height,
+ (unsigned int)rs.getIntrinsics(RS2_STREAM_COLOR).width);
+ vpImage<unsigned char> infrared((unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).height,
+ (unsigned int)rs.getIntrinsics(RS2_STREAM_INFRARED).width);
+ vpImage<vpRGBa> depth_display((unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).height,
+ (unsigned int)rs.getIntrinsics(RS2_STREAM_DEPTH).width);
+ vpImage<uint16_t> depth(depth_display.getHeight(), depth_display.getWidth());
+
+#ifdef VISP_HAVE_PCL
+ std::mutex mutex;
+ ViewerWorker viewer(true, mutex);
+ std::thread viewer_thread(&ViewerWorker::run, &viewer);
+#endif
+
+#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");
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI dc(color, 10, 10, "Color image");
+ vpDisplayGDI di(infrared, color.getWidth() + 80, 10, "Infrared image");
+ vpDisplayGDI dd(depth_display, 10, color.getHeight() + 80, "Depth image");
+#endif
+
+ while (true) {
+ double t = vpTime::measureTimeMs();
+
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ 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);
+#endif
+
+ vpImageConvert::createDepthHistogram(depth, depth_display);
+
+ vpDisplay::display(color);
+ vpDisplay::display(infrared);
+ vpDisplay::display(depth_display);
+
+ vpDisplay::displayText(color, 15, 15, "Click to quit", vpColor::red);
+ if (vpDisplay::getClick(color, false) || vpDisplay::getClick(infrared, false) ||
+ vpDisplay::getClick(depth_display, false)) {
+ break;
+ }
+ vpDisplay::flush(color);
+ vpDisplay::flush(infrared);
+ vpDisplay::flush(depth_display);
+
+ std::cout << "Loop time: " << vpTime::measureTimeMs() - t << std::endl;
+ }
+
+ std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
+
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ cancelled = true;
+ }
+
+ viewer_thread.join();
+#endif
+
+ } catch (const vpException &e) {
+ std::cerr << "RealSense error " << e.what() << std::endl;
+ } catch (const std::exception &e) {
+ std::cerr << e.what() << std::endl;
+ }
+
+ return 0;
+}
+#else
+int main()
+{
+#if !defined(VISP_HAVE_REALSENSE2)
+ 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;
+#endif
+ return 0;
+}
+#endif
diff --git a/example/device/framegrabber/grabV4l2.cpp b/example/device/framegrabber/grabV4l2.cpp
index c3ec818..e48eda3 100644
--- a/example/device/framegrabber/grabV4l2.cpp
+++ b/example/device/framegrabber/grabV4l2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,10 +37,9 @@
*
*****************************************************************************/
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
/*!
\file grabV4l2.cpp
@@ -49,19 +49,19 @@
#ifdef VISP_HAVE_V4L2
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpTime.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vpV4l2Grabber.h>
// List of allowed command line options
-#define GETOPTARGS "df:i:hn:o:p:s:t:v:x"
+#define GETOPTARGS "df:i:hn:o:p:s:t:v:x"
typedef enum {
grey_image = 0, // for ViSP unsigned char grey images
@@ -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,9 +144,8 @@ 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, opath.c_str());
+ Print the help.\n\n", device, fps, input, pixelformat, vpV4l2Grabber::V4L2_MAX_FORMAT - 1, image_type, scale, niter,
+ opath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -174,43 +172,61 @@ 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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': display = false; break;
- case 'f': fps = (unsigned) atoi(optarg_); break;
- case 'i': input = (unsigned) atoi(optarg_); break;
- case 'n': niter = atol(optarg_); break;
+ case 'd':
+ display = false;
+ break;
+ case 'f':
+ fps = (unsigned)atoi(optarg_);
+ break;
+ case 'i':
+ input = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ niter = atol(optarg_);
+ break;
case 'o':
save = true;
- opath = optarg_; break;
- case 'p': pixelformat = (vpV4l2Grabber::vpV4l2PixelFormatType) atoi(optarg_); break;
- case 's': scale = (unsigned) atoi(optarg_); break;
- case 't': image_type = (vpImage_type) atoi(optarg_); break;
- case 'v': sprintf(device, "%s", optarg_); break;
- case 'x': verbose = true; break;
- case 'h': usage(argv[0], NULL, fps, input, scale, niter,
- device, pixelformat, image_type, opath);
- return false; break;
+ opath = optarg_;
+ break;
+ case 'p':
+ pixelformat = (vpV4l2Grabber::vpV4l2PixelFormatType)atoi(optarg_);
+ break;
+ case 's':
+ scale = (unsigned)atoi(optarg_);
+ break;
+ case 't':
+ image_type = (vpImage_type)atoi(optarg_);
+ break;
+ case 'v':
+ sprintf(device, "%s", optarg_);
+ break;
+ case 'x':
+ verbose = true;
+ break;
+ case 'h':
+ 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); return false; break;
+ usage(argv[0], optarg_, fps, input, scale, niter, device, pixelformat, image_type, opath);
+ return false;
+ break;
}
}
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;
@@ -228,8 +244,7 @@ bool getOptions(int argc, const char **argv, unsigned &fps, unsigned &input,
device. Only grabbing of grey level images is possible in this
example. Display these images using X11 or GTK.
*/
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
unsigned int opt_fps = 25;
@@ -248,18 +263,17 @@ 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,
+ 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);
+ exit(-1);
}
// Declare an image, this is a gray level image (unsigned char) and
// an other one that is a color image. There size is not defined
// yet. It will be defined when the image will acquired the first
// time.
- vpImage<unsigned char> Ig ; // grey level image
- vpImage<vpRGBa> Ic ; // color image
+ vpImage<unsigned char> Ig; // grey level image
+ vpImage<vpRGBa> Ic; // color image
// Creates the grabber
vpV4l2Grabber g;
@@ -276,24 +290,20 @@ main(int argc, const char ** argv)
g.setFramerate(vpV4l2Grabber::framerate_50fps);
if (opt_image_type == grey_image) {
// Open the framegrabber with the specified settings on grey images
- g.open(Ig) ;
+ g.open(Ig);
// Acquire an image
- g.acquire(Ig) ;
- std::cout << "Grey image size: width : " << Ig.getWidth() << " height: "
- << Ig.getHeight() << std::endl;
- }
- else {
+ g.acquire(Ig);
+ 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) ;
+ g.open(Ic);
// Acquire an image
- g.acquire(Ic) ;
- std::cout << "Color image size: width : " << Ic.getWidth() << " height: "
- << Ic.getHeight() << std::endl;
+ g.acquire(Ic);
+ std::cout << "Color image size: width : " << Ic.getWidth() << " height: " << Ic.getHeight() << std::endl;
}
-
- // We open a window using either X11 or GTK.
- // Its size is automatically defined by the image (I) size
+// We open a window using either X11 or GTK.
+// Its size is automatically defined by the image (I) size
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -307,39 +317,36 @@ main(int argc, const char ** argv)
// therefore is is no longuer necessary to make a reference to the
// display variable.
if (opt_image_type == grey_image) {
- display.init(Ig, 100, 100, "V4L2 grey images framegrabbing") ;
- vpDisplay::display(Ig) ;
- vpDisplay::flush(Ig) ;
- }
- else {
- display.init(Ic, 100, 100, "V4L2 color images framegrabbing") ;
- vpDisplay::display(Ic) ;
- vpDisplay::flush(Ic) ;
+ display.init(Ig, 100, 100, "V4L2 grey images framegrabbing");
+ vpDisplay::display(Ig);
+ vpDisplay::flush(Ig);
+ } else {
+ display.init(Ic, 100, 100, "V4L2 color images framegrabbing");
+ vpDisplay::display(Ic);
+ vpDisplay::flush(Ic);
}
}
// Acquisition loop
long cpt = 1;
- while(cpt ++ < opt_iter)
- {
+ while (cpt++ < opt_iter) {
// Measure the initial time of an iteration
double t = vpTime::measureTimeMs();
// Acquire the image
if (opt_image_type == grey_image) {
- g.acquire(Ig) ;
+ g.acquire(Ig);
if (opt_display) {
// Display the image
- vpDisplay::display(Ig) ;
+ vpDisplay::display(Ig);
// Flush the display
- vpDisplay::flush(Ig) ;
+ vpDisplay::flush(Ig);
}
- }
- else {
- g.acquire(Ic) ;
+ } else {
+ g.acquire(Ic);
if (opt_display) {
// Display the image
- vpDisplay::display(Ic) ;
+ vpDisplay::display(Ic);
// Flush the display
- vpDisplay::flush(Ic) ;
+ vpDisplay::flush(Ic);
}
}
@@ -350,8 +357,7 @@ main(int argc, const char ** argv)
std::cout << "Write: " << filename << std::endl;
if (opt_image_type == grey_image) {
vpImageIo::write(Ig, filename);
- }
- else {
+ } else {
vpImageIo::write(Ic, filename);
}
}
@@ -362,24 +368,14 @@ main(int argc, const char ** argv)
g.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpTRACE("X11 or GTK display are not available") ;
-}
+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 d4c8991..b5added 100644
--- a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp
+++ b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp
@@ -7,30 +7,30 @@
with C++11 threads.
*/
-#if defined(VISP_HAVE_CPP11_COMPATIBILITY) && defined(VISP_HAVE_V4L2) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) )
+#if defined(VISP_HAVE_CPP11_COMPATIBILITY) && defined(VISP_HAVE_V4L2) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK))
+#include <condition_variable>
#include <iostream>
#include <limits>
-#include <queue>
#include <mutex>
+#include <queue>
#include <thread>
-#include <condition_variable>
-#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpVideoWriter.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#define GETOPTARGS "d:oh"
-#define GETOPTARGS "d:oh"
-
-
-namespace {
+namespace
+{
void usage(const char *name, const char *badparam)
{
@@ -49,30 +49,35 @@ OPTIONS: \n\
Save each stream in a dedicated folder.\n\
\n\
-h \n\
- Print the help.\n\n",
- name);
+ Print the help.\n\n", name);
if (badparam)
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;
- int c;
+ const char **argv1 = (const char **)argv;
+ int c;
while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'd': deviceCount = (unsigned int) atoi(optarg); break;
- case 'o': saveVideo = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'd':
+ deviceCount = (unsigned int)atoi(optarg);
+ break;
+ case 'o':
+ saveVideo = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg);
- return false; break;
+ return false;
+ break;
}
}
@@ -87,38 +92,45 @@ bool getOptions(int argc, char **argv,
return true;
}
-//Code adapted from the original author Dan Mašek to be compatible with ViSP image
-class FrameQueue {
+// Code adapted from the original author Dan Mašek to be compatible with ViSP
+// image
+class FrameQueue
+{
public:
struct cancelled {
};
- FrameQueue() : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits<size_t>::max()), m_mutex() {
+ FrameQueue()
+ : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits<size_t>::max()), m_mutex()
+ {
}
- void cancel() {
+ void cancel()
+ {
std::lock_guard<std::mutex> lock(m_mutex);
m_cancelled = true;
m_cond.notify_all();
}
- //Push the image to save in the queue (FIFO)
- void push(const vpImage<vpRGBa> &image) {
+ // Push the image to save in the queue (FIFO)
+ void push(const vpImage<vpRGBa> &image)
+ {
std::lock_guard<std::mutex> lock(m_mutex);
m_queueColor.push(image);
- //Pop extra images in the queue
- while(m_queueColor.size() > m_maxQueueSize) {
+ // Pop extra images in the queue
+ while (m_queueColor.size() > m_maxQueueSize) {
m_queueColor.pop();
}
m_cond.notify_one();
}
- //Pop the image to save from the queue (FIFO)
- vpImage<vpRGBa> pop() {
+ // Pop the image to save from the queue (FIFO)
+ vpImage<vpRGBa> pop()
+ {
std::unique_lock<std::mutex> lock(m_mutex);
while (m_queueColor.empty()) {
@@ -139,9 +151,7 @@ public:
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;
@@ -151,21 +161,22 @@ private:
std::mutex m_mutex;
};
-
-class StorageWorker {
+class StorageWorker
+{
public:
- 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) {
+ 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)
+ {
}
- //Thread main loop
- void run() {
+ // Thread main loop
+ void run()
+ {
vpImage<vpRGBa> O_color(m_height, m_width);
vpVideoWriter writer;
- if(!m_filename.empty()) {
+ if (!m_filename.empty()) {
writer.setFileName(m_filename);
writer.open(O_color);
}
@@ -174,7 +185,7 @@ public:
for (;;) {
vpImage<vpRGBa> image(m_queue.pop());
- if(!m_filename.empty()) {
+ if (!m_filename.empty()) {
writer.saveFrame(image);
}
}
@@ -189,8 +200,8 @@ private:
unsigned int m_height;
};
-
-class ShareImage {
+class ShareImage
+{
private:
bool m_cancelled;
@@ -203,23 +214,25 @@ public:
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() {
- if(m_pImgData != NULL) {
- delete []m_pImgData;
+ virtual ~ShareImage()
+ {
+ if (m_pImgData != NULL) {
+ delete[] m_pImgData;
}
}
- void cancel() {
+ void cancel()
+ {
std::lock_guard<std::mutex> lock(m_mutex);
m_cancelled = true;
m_cond.notify_all();
}
- //Get the image to display
- void getImage(unsigned char * const imageData, const unsigned int totalSize) {
+ // Get the image to display
+ void getImage(unsigned char *const imageData, const unsigned int totalSize)
+ {
std::unique_lock<std::mutex> lock(m_mutex);
if (m_cancelled) {
@@ -232,61 +245,64 @@ public:
throw cancelled();
}
- //Copy to imageData
- if(totalSize <= m_totalSize) {
- memcpy(imageData, m_pImgData, totalSize*sizeof(unsigned char));
+ // Copy to imageData
+ if (totalSize <= m_totalSize) {
+ memcpy(imageData, m_pImgData, totalSize * sizeof(unsigned char));
} else {
std::cerr << "totalSize <= m_totalSize !" << std::endl;
}
}
- bool isCancelled() {
+ bool isCancelled()
+ {
std::lock_guard<std::mutex> lock(m_mutex);
return m_cancelled;
}
- //Set the image to display
- void setImage(const unsigned char * const imageData, const unsigned int totalSize) {
+ // Set the image to display
+ void setImage(const unsigned char *const imageData, const unsigned int totalSize)
+ {
std::lock_guard<std::mutex> lock(m_mutex);
- if(m_pImgData == NULL || m_totalSize != totalSize) {
+ if (m_pImgData == NULL || m_totalSize != totalSize) {
m_totalSize = totalSize;
- if(m_pImgData != NULL) {
- delete []m_pImgData;
+ if (m_pImgData != NULL) {
+ delete[] m_pImgData;
}
m_pImgData = new unsigned char[m_totalSize];
}
- //Copy from imageData
- memcpy(m_pImgData, imageData, m_totalSize*sizeof(unsigned char));
+ // Copy from imageData
+ memcpy(m_pImgData, imageData, m_totalSize * sizeof(unsigned char));
m_cond.notify_one();
}
};
-
-void capture(vpV4l2Grabber * const pGrabber, ShareImage &share_image) {
+void capture(vpV4l2Grabber *const pGrabber, ShareImage &share_image)
+{
vpImage<vpRGBa> local_img;
- //Open the camera stream
+ // Open the camera stream
pGrabber->open(local_img);
- while(true) {
- if(share_image.isCancelled()) {
+ while (true) {
+ if (share_image.isCancelled()) {
break;
}
pGrabber->acquire(local_img);
- //Update share_image
- share_image.setImage((unsigned char *) local_img.bitmap, local_img.getSize()*4);
+ // Update share_image
+ 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) {
+ const unsigned int deviceId, ShareImage &share_image, FrameQueue &queue, const bool save)
+{
vpImage<vpRGBa> local_img(height, width);
#if defined VISP_HAVE_X11
@@ -295,7 +311,7 @@ void display(const unsigned int width, const unsigned int height, const int win_
vpDisplayGTK display;
#endif
- //Init Display
+ // Init Display
std::stringstream ss;
ss << "Camera stream " << deviceId;
display.init(local_img, win_x, win_y, ss.str());
@@ -303,20 +319,22 @@ void display(const unsigned int width, const unsigned int height, const int win_
try {
vpMouseButton::vpMouseButtonType button;
- vpImage<unsigned char> I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width);;
- vpImage<unsigned char> I_red_gaussian(height, width), I_green_gaussian(height, width), I_blue_gaussian(height, width);
+ vpImage<unsigned char> I_red(height, width), I_green(height, width), I_blue(height, width), I_alpha(height, width);
+ ;
+ vpImage<unsigned char> I_red_gaussian(height, width), I_green_gaussian(height, width),
+ I_blue_gaussian(height, width);
vpImage<double> I_red_gaussian_double, I_green_gaussian_double, I_blue_gaussian_double;
bool exit = false, gaussian_blur = false;
- while(!exit) {
+ while (!exit) {
double t = vpTime::measureTimeMs();
- //Get image
- share_image.getImage((unsigned char *) local_img.bitmap, local_img.getSize()*4);
+ // Get image
+ 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) {
- //Split channels
+ // Apply gaussian blur to simulate a computation on the image
+ if (gaussian_blur) {
+ // Split channels
vpImageConvert::split(local_img, &I_red, &I_green, &I_blue, &I_alpha);
vpImageConvert::convert(I_red, I_red_gaussian_double);
vpImageConvert::convert(I_green, I_green_gaussian_double);
@@ -344,39 +362,38 @@ void display(const unsigned int width, const unsigned int height, const int win_
vpDisplay::flush(local_img);
- if(save) {
+ if (save) {
queue.push(local_img);
}
- if(vpDisplay::getClick(local_img, button, false)) {
- switch(button) {
- case vpMouseButton::button3:
- gaussian_blur = !gaussian_blur;
- break;
+ if (vpDisplay::getClick(local_img, button, false)) {
+ switch (button) {
+ case vpMouseButton::button3:
+ gaussian_blur = !gaussian_blur;
+ break;
- default:
- exit = true;
- break;
+ default:
+ exit = true;
+ break;
}
}
}
- } catch(ShareImage::cancelled &) {
+ } catch (ShareImage::cancelled &) {
std::cout << "Cancelled!" << std::endl;
}
share_image.cancel();
}
-} //Namespace
-
+} // Namespace
int main(int argc, char *argv[])
{
unsigned int deviceCount = 1;
- unsigned int cameraScale = 1; //640x480
+ unsigned int cameraScale = 1; // 640x480
bool saveVideo = false;
- //Read the command line options
+ // Read the command line options
if (!getOptions(argc, argv, deviceCount, saveVideo)) {
return (-1);
}
@@ -384,7 +401,7 @@ int main(int argc, char *argv[])
std::vector<vpV4l2Grabber *> grabbers;
const unsigned int offsetX = 100, offsetY = 100;
- for(unsigned int devicedId = 0; devicedId < deviceCount; devicedId++) {
+ for (unsigned int devicedId = 0; devicedId < deviceCount; devicedId++) {
try {
vpV4l2Grabber *pGrabber = new vpV4l2Grabber;
std::stringstream ss;
@@ -393,7 +410,7 @@ int main(int argc, char *argv[])
pGrabber->setScale(cameraScale);
grabbers.push_back(pGrabber);
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Exception: " << e.what() << std::endl;
}
}
@@ -404,28 +421,23 @@ int main(int argc, char *argv[])
std::vector<std::thread> capture_threads;
std::vector<std::thread> display_threads;
- //Synchronized queues for each camera stream
+ // Synchronized queues for each camera stream
std::vector<FrameQueue> save_queues(grabbers.size());
std::vector<StorageWorker> storages;
std::vector<std::thread> storage_threads;
std::string parent_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
- if(saveVideo) {
- std::cout << "Create parent_directory: " << parent_directory << std::endl;
- vpIoTools::makeDirectory(parent_directory);
- }
-
- 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]) );
+ 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]));
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 );
+ // 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);
- if(saveVideo) {
+ if (saveVideo) {
std::stringstream ss;
ss << parent_directory << "/Camera_Stream" << deviceId;
std::cout << "Create directory: " << ss.str() << std::endl;
@@ -433,43 +445,44 @@ 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());
}
}
- if(saveVideo) {
- for(auto& s : storages) {
- //Start the storage thread for the current camera stream
+ if (saveVideo) {
+ for (auto &s : storages) {
+ // Start the storage thread for the current camera stream
storage_threads.emplace_back(&StorageWorker::run, &s);
}
}
- //Join all the worker threads, waiting for them to finish
- for(auto& ct : capture_threads) {
+ // Join all the worker threads, waiting for them to finish
+ for (auto &ct : capture_threads) {
ct.join();
}
- for (auto& dt : display_threads) {
+ for (auto &dt : display_threads) {
dt.join();
}
- //Clean first the grabbers to avoid camera problems when cancelling the storage threads in the terminal
- for(auto& g : grabbers) {
+ // Clean first the grabbers to avoid camera problems when cancelling the
+ // storage threads in the terminal
+ for (auto &g : grabbers) {
delete g;
}
- if(saveVideo) {
+ if (saveVideo) {
std::cout << "\nWaiting for finishing thread to write images..." << std::endl;
}
// We're done reading, cancel all the queues
- for (auto& qu : save_queues) {
+ for (auto &qu : save_queues) {
qu.cancel();
}
- //Join all the worker threads, waiting for them to finish
- for (auto& st : storage_threads) {
+ // Join all the worker threads, waiting for them to finish
+ for (auto &st : storage_threads) {
st.join();
}
@@ -480,8 +493,9 @@ int main(int argc, char *argv[])
int main()
{
- std::cout << "Warning: This example need to be build with cxx11 compiler flags, v4l2 and x11 or gtk 3rd partiess. " << std::endl;
+ std::cout << "Warning: This example need to be build with cxx11 compiler "
+ "flags, v4l2 and x11 or gtk 3rd partiess. "
+ << std::endl;
return 0;
}
#endif
-
diff --git a/example/device/kinect/CMakeLists.txt b/example/device/kinect/CMakeLists.txt
index f189bad..b7cfc50 100644
--- a/example/device/kinect/CMakeLists.txt
+++ b/example/device/kinect/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/device/kinect/kinectAcquisition.cpp b/example/device/kinect/kinectAcquisition.cpp
index f7e7d5d..bf03d70 100644
--- a/example/device/kinect/kinectAcquisition.cpp
+++ b/example/device/kinect/kinectAcquisition.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,40 +36,40 @@
*
*****************************************************************************/
-
/*!
\example kinectAcquisition.cpp
- \brief Example that shows how to acquire depth map and RGB images from a kinect device, and show the warped RGB frame
+ \brief Example that shows how to acquire depth map and RGB images from a
+ kinect device, and show the warped RGB frame
*/
-
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
#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 <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpKinect.h>
-#include <visp3/core/vpTime.h>
-int main() {
+int main()
+{
try {
- // Init Kinect
+// Init Kinect
#ifdef VISP_HAVE_LIBFREENECT_OLD
- // This is the way to initialize Freenect with an old version of libfreenect packages under ubuntu lucid 10.04
+ // This is the way to initialize Freenect with an old version of
+ // libfreenect packages under ubuntu lucid 10.04
Freenect::Freenect<vpKinect> freenect;
- vpKinect & kinect = freenect.createDevice(0);
+ vpKinect &kinect = freenect.createDevice(0);
#else
Freenect::Freenect freenect;
- vpKinect & kinect = freenect.createDevice<vpKinect>(0);
+ vpKinect &kinect = freenect.createDevice<vpKinect>(0);
#endif
// Set tilt angle in degrees
@@ -77,17 +78,21 @@ int main() {
kinect.setTiltDegrees(angle);
}
- // Init display
+// Init display
#if 1
- kinect.start(vpKinect::DMAP_MEDIUM_RES); // Start acquisition thread with a depth map resolution of 480x640
- vpImage<unsigned char> Idmap(480,640);//for medium resolution
- vpImage<float> dmap(480,640);//for medium resolution
+ kinect.start(vpKinect::DMAP_MEDIUM_RES); // Start acquisition thread with
+ // a depth map resolution of
+ // 480x640
+ vpImage<unsigned char> Idmap(480, 640); // for medium resolution
+ vpImage<float> dmap(480, 640); // for medium resolution
#else
- kinect.start(vpKinect::DMAP_LOW_RES); // Start acquisition thread with a depth map resolution of 240x320 (default resolution)
- vpImage<unsigned char> Idmap(240,320);//for low resolution
- vpImage<float> dmap(240,320);//for low resolution
+ kinect.start(vpKinect::DMAP_LOW_RES); // Start acquisition thread with a
+ // depth map resolution of 240x320
+ // (default resolution)
+ vpImage<unsigned char> Idmap(240, 320); // for low resolution
+ vpImage<float> dmap(240, 320); // for low resolution
#endif
- vpImage<vpRGBa> Irgb(480,640),Iwarped(480,640);
+ vpImage<vpRGBa> Irgb(480, 640), Iwarped(480, 640);
#if defined VISP_HAVE_X11
vpDisplayX display, displayRgb, displayRgbWarped;
@@ -105,15 +110,14 @@ int main() {
vpDisplayGDI displayRgbWarped;
#endif
- display.init(Idmap, 100, 200,"Depth map");
- displayRgb.init(Irgb, 900, 200,"Color Image");
- displayRgbWarped.init(Iwarped,900,700,"Warped Color Image");
+ display.init(Idmap, 100, 200, "Depth map");
+ displayRgb.init(Irgb, 900, 200, "Color Image");
+ displayRgbWarped.init(Iwarped, 900, 700, "Warped Color Image");
// 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);
@@ -123,20 +127,18 @@ int main() {
vpDisplay::display(Irgb);
vpDisplay::flush(Irgb);
- //Warped RGB image:
- kinect.warpRGBFrame(Irgb,dmap, Iwarped);
+ // Warped RGB image:
+ kinect.warpRGBFrame(Irgb, dmap, Iwarped);
vpDisplay::display(Iwarped);
vpDisplay::flush(Iwarped);
}
std::cout << "Stop acquisition" << std::endl;
kinect.stop(); // Stop acquisition thread
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
@@ -144,20 +146,15 @@ int main() {
#else
-int
-main()
+int main()
{
- std::cout << "You should install a video device (X11, GTK, OpenCV, GDI) to run this example" << std::endl;
+ std::cout << "You should install a video device (X11, GTK, OpenCV, GDI) to "
+ "run this example"
+ << std::endl;
}
#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/CMakeLists.txt b/example/device/laserscanner/CMakeLists.txt
index 7c6b8e7..f0ec56f 100644
--- a/example/device/laserscanner/CMakeLists.txt
+++ b/example/device/laserscanner/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/device/laserscanner/SickLDMRS-Acq.cpp b/example/device/laserscanner/SickLDMRS-Acq.cpp
index ff0ba15..9ec84aa 100644
--- a/example/device/laserscanner/SickLDMRS-Acq.cpp
+++ b/example/device/laserscanner/SickLDMRS-Acq.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,21 +36,19 @@
*
*****************************************************************************/
-
/*!
\example SickLDMRS-Acq.cpp
\brief Example that shows how to acquire Sick LD-MRS laser
- measurements.
+ measurements.
\warning For the moment, this example is only working on UNIX
platforms since the Sick LD-MRS driver was not ported to Windows.
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/sensor/vpSickLDMRS.h>
#include <visp3/io/vpParseArgv.h>
-
+#include <visp3/sensor/vpSickLDMRS.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
@@ -63,31 +62,28 @@ int main()
laser.setup();
unsigned long int iter = 0;
- for ( ; ; ) {
+ for (;;) {
double t1 = vpTime::measureTimeMs();
vpLaserScan laserscan[4];
if (laser.measure(laserscan) == false)
continue;
- iter ++;
- std::cout << "iter: " << iter << " time: "
- << vpTime::measureTimeMs() - t1 << " ms" << std::endl;
+ iter++;
+ std::cout << "iter: " << iter << " time: " << vpTime::measureTimeMs() - t1 << " ms" << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-#else // #ifdef UNIX
+#else // #ifdef UNIX
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 6070a45..d1dcb19 100644
--- a/example/device/laserscanner/SickLDMRS-Process.cpp
+++ b/example/device/laserscanner/SickLDMRS-Process.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\example SickLDMRS-Process.cpp
@@ -43,7 +43,7 @@
This example shows by multithreading how to:
- acquire Sick LD-MRS laser measurements
- - how to process the data in order to display the laser scans and how
+ - how to process the data in order to display the laser scans and how
to save the scan data in a file
- and how to acquire and display images from a firewire camera if connected.
@@ -58,21 +58,22 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpImagePoint.h>
-#include <visp3/sensor/vpSickLDMRS.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpDisplay.h>
+#include <visp3/sensor/vpSickLDMRS.h>
#ifdef VISP_HAVE_MODULE_GUI
-# include <visp3/gui/vpDisplayX.h>
-# include <visp3/gui/vpDisplayGDI.h>
-# include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#endif
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpIoTools.h>
-
-#if ( !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) ) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
+
+#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
@@ -95,19 +96,27 @@ void *laser_display_and_save_loop(void *)
unsigned int height = map.getHeight();
vpImagePoint O; // Beam origin
O.set_i(height);
- O.set_j(width/2.);
+ O.set_j(width / 2.);
vpScanPoint p;
vpColor color[4]; // one color per layer
char filename[FILENAME_MAX];
std::ofstream fdscan;
vpLaserScan laserscan[4];
- for (int layer=0; layer<4; layer++) {
+ for (int layer = 0; layer < 4; layer++) {
switch (layer) {
- case 0: color[layer] = vpColor::red; break;
- case 1: color[layer] = vpColor::green; break;
- case 2: color[layer] = vpColor::blue; break;
- case 3: color[layer] = vpColor::yellow; break;
+ case 0:
+ color[layer] = vpColor::red;
+ break;
+ case 1:
+ color[layer] = vpColor::green;
+ break;
+ case 2:
+ color[layer] = vpColor::blue;
+ break;
+ case 3:
+ color[layer] = vpColor::yellow;
+ break;
}
}
@@ -120,20 +129,20 @@ void *laser_display_and_save_loop(void *)
#elif defined VISP_HAVE_GTK
display = new vpDisplayGTK;
#endif
- display->init (map, 10, 10, "Laser scan");
+ display->init(map, 10, 10, "Laser scan");
#endif
unsigned int iter = 0;
- for ( ; ; ) {
-
-#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
+ for (;;) {
+
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK))
vpDisplay::display(map);
#endif
-
+
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_lock(&shm_mutex);
#endif
- for (int layer=0; layer < 4; layer++)
+ for (int layer = 0; layer < 4; layer++)
laserscan[layer] = shm_laserscan[layer];
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock(&shm_mutex);
@@ -141,45 +150,42 @@ void *laser_display_and_save_loop(void *)
// std::cout << "laser start timestamp "
// << laserscan[0].getStartTimestamp() - time_offset << std::endl;
-
+
// Parse the four layers
- for (int layer=0; layer<4; layer++) {
- if (! ((0x1<<layer) & layerToDisplay)) {
- std::cout << "Layer " << layer+1 << " is not displayed" << std::endl;
+ for (int layer = 0; layer < 4; layer++) {
+ if (!((0x1 << layer) & layerToDisplay)) {
+ std::cout << "Layer " << layer + 1 << " is not displayed" << std::endl;
continue;
}
-
+
std::vector<vpScanPoint> pointsLayer = laserscan[layer].getScanPoints();
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
- << "# Data : \"radial distance (m)\" \"horizontal angle (rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z (m)\""
+ 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
+ << "# Data : \"radial distance (m)\" \"horizontal angle "
+ "(rad)\" \"vertical angle (rad)\" \"X (m)\" \"Y (m)\" \"Z "
+ "(m)\""
<< std::endl;
}
-
- vpImagePoint E; // Beam echo
- double resolution = 5; // 100 pixels = 1 meter - increase this value to see better near info
+ vpImagePoint E; // Beam echo
+ double resolution = 5; // 100 pixels = 1 meter - increase this value to
+ // see better near info
// std::cout << "display layer " << layer << " nb points: "
// << pointsLayer.size() << std::endl;
- for (unsigned int i=0; i<pointsLayer.size(); i++) {
+ 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()));
- //std::cout << "E: " << E << std::endl;
-#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined (VISP_HAVE_GTK) )
+ E.set_i(height - resolution * p.getRadialDist() * cos(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))
vpDisplay::displayLine(map, O, E, color[layer]);
#endif
if (save) {
@@ -191,12 +197,12 @@ 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 ++;
- //std::cout << "display time: " << vpTime::measureTimeMs() - t1 << std::endl;
-
+ iter++;
+ // std::cout << "display time: " << vpTime::measureTimeMs() - t1 <<
+ // std::endl;
}
delete display;
return NULL;
@@ -212,7 +218,7 @@ void *laser_acq_loop(void *)
vpLaserScan laserscan[4];
unsigned int iter = 0;
- for ( ; ; ) {
+ for (;;) {
double t1 = vpTime::measureTimeMs();
if (laser.measure(laserscan) == false)
continue;
@@ -220,15 +226,14 @@ void *laser_acq_loop(void *)
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_lock(&shm_mutex);
#endif
- for (int layer=0; layer < 4; layer++)
+ for (int layer = 0; layer < 4; layer++)
shm_laserscan[layer] = laserscan[layer];
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_unlock(&shm_mutex);
#endif
- iter ++;
+ iter++;
std::cout << "laser acq time: " << vpTime::measureTimeMs() - t1 << std::endl;
-
}
return NULL;
@@ -239,7 +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)
@@ -250,7 +255,7 @@ void *camera_acq_and_display_loop(void *)
vpImage<unsigned char> I; // Create a gray level image container
vpImage<unsigned char> Q; // Create a quarter size gray level image container
- g.acquire(I); // Acquire an image
+ g.acquire(I); // Acquire an image
I.quarterSizeImage(Q);
vpDisplay *display = NULL;
@@ -262,24 +267,24 @@ void *camera_acq_and_display_loop(void *)
#elif defined VISP_HAVE_GTK
display = new vpDisplayGTK;
#endif
- display->init (Q, 320, 10, "Camera");
+ display->init(Q, 320, 10, "Camera");
#endif
// Create a file with cameraimage time stamps
std::ofstream fdimage_ts;
if (save) {
std::string filename = output_path + "/image_timestamp.txt";
- fdimage_ts.open( filename.c_str() );
+ fdimage_ts.open(filename.c_str());
fdimage_ts << "# [image name] [time stamp in second]" << std::endl;
}
unsigned iter = 0;
char filename[FILENAME_MAX];
uint64_t timestamp;
uint32_t id;
- for ( ; ; ) {
+ for (;;) {
dc1394video_frame_t *frame = g.dequeue(I, timestamp, id); // Acquire an image
I.quarterSizeImage(Q);
- double image_timestamp = timestamp/1000000. - time_offset;
+ double image_timestamp = timestamp / 1000000. - time_offset;
std::cout << "camera timestamp: " << image_timestamp << " s " << std::endl;
if (save) {
// Set the image filename
@@ -287,28 +292,25 @@ void *camera_acq_and_display_loop(void *)
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
g.enqueue(frame);
- iter ++;
+ iter++;
}
delete display;
if (save) {
fdimage_ts.close();
}
- }
- catch(...) {
-
+ } catch (...) {
}
#endif
return NULL;
}
-
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
output_path = "data";
@@ -317,38 +319,32 @@ int main(int argc, const char ** argv)
try {
// Create a directory with name "username"
vpIoTools::makeDirectory(output_path);
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot create " << output_path << " directory" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
// Parse the command line to set the variables
- vpParseArgv::vpArgvInfo argTable[] =
- {
- {"-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."
- },
- {"-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}
- } ;
+ vpParseArgv::vpArgvInfo argTable[] = {
+ {"-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."},
+ {"-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}};
// Read the command line options
- if(vpParseArgv::parse(&argc, argv, argTable,
- vpParseArgv::ARGV_NO_LEFTOVERS |
- vpParseArgv::ARGV_NO_ABBREV |
- vpParseArgv::ARGV_NO_DEFAULTS)) {
- return (false);
+ if (vpParseArgv::parse(&argc, argv, argTable,
+ vpParseArgv::ARGV_NO_LEFTOVERS | vpParseArgv::ARGV_NO_ABBREV |
+ vpParseArgv::ARGV_NO_DEFAULTS)) {
+ return (EXIT_FAILURE);
}
time_offset = vpTime::measureTimeSecond();
@@ -364,11 +360,10 @@ int main(int argc, const char ** argv)
pthread_join(thread_laser_display, 0);
#endif
- return 0;
- }
- catch(vpException &e) {
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
- return 1;
+ return EXIT_FAILURE;
}
}
@@ -377,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/CMakeLists.txt b/example/device/light/CMakeLists.txt
index c8e6f2c..2b1a6f0 100644
--- a/example/device/light/CMakeLists.txt
+++ b/example/device/light/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/device/light/ringLight.cpp b/example/device/light/ringLight.cpp
index f056f6e..5f8eba7 100644
--- a/example/device/light/ringLight.cpp
+++ b/example/device/light/ringLight.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,23 +42,21 @@
Shows how to activates the ring light.
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#if defined VISP_HAVE_PARPORT
-#include <stdlib.h>
-#include <stdio.h>
#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/robot/vpRingLight.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpRingLight.h>
// List of allowed command line options
-#define GETOPTARGS "d:hn:ot:"
+#define GETOPTARGS "d:hn:ot:"
/*!
@@ -106,10 +105,9 @@ OPTIONS: Default\n\
Print the help.\n\n", nsec, nmsec, nsec);
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -128,18 +126,29 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &on, int &nsec, double &nmsec)
{
const char *optarg;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'n': nsec = atoi(optarg); break;
- case 'o': on = true; break;
- case 't': nmsec = atof(optarg); break;
- case 'h': usage(argv[0], NULL, nsec, nmsec); return false; break;
+ case 'n':
+ nsec = atoi(optarg);
+ break;
+ case 'o':
+ on = true;
+ break;
+ case 't':
+ nmsec = atof(optarg);
+ break;
+ case 'h':
+ usage(argv[0], NULL, nsec, nmsec);
+ return false;
+ break;
default:
- usage(argv[0], optarg, nsec, nmsec); return false; break;
+ usage(argv[0], optarg, nsec, nmsec);
+ return false;
+ break;
}
}
@@ -159,22 +168,21 @@ bool getOptions(int argc, const char **argv, bool &on, int &nsec, double &nmsec)
Send a data to the parallel port.
*/
-int
-main(int argc, const char **argv)
+int main(int argc, const char **argv)
{
try {
bool on = false;
- int nsec = 5; // Time while the ring light is turned on
+ int nsec = 5; // Time while the ring light is turned on
double nmsec = 0; // Pulse duration
// Read the command line options
if (getOptions(argc, argv, on, nsec, nmsec) == false) {
- exit (-1);
+ exit(-1);
}
vpRingLight light;
- //if (nmsec == 0.)
+ // if (nmsec == 0.)
if (std::fabs(nmsec) <= std::numeric_limits<double>::epsilon())
light.pulse();
else
@@ -182,33 +190,30 @@ main(int argc, const char **argv)
if (on) {
printf("Turn on ring light\n");
- light.on(); // Turn the ring light on
+ light.on(); // Turn the ring light on
vpTime::wait(nsec * 1000); // Wait 5 s
- light.off(); // and then turn the ring light off
- }
- else {
+ light.off(); // and then turn the ring light off
+ } else {
printf("Send a pulse to activate the ring light\n");
light.pulse();
}
- }
- catch (vpParallelPortException &e) {
- switch(e.getCode()) {
+ } catch (vpParallelPortException &e) {
+ switch (e.getCode()) {
case vpParallelPortException::opening:
- printf("Can't open the parallel port to access to the ring light device\n");
+ printf("Can't open the parallel port to access to the ring light "
+ "device\n");
break;
case vpParallelPortException::closing:
printf("Can't close the parallel port\n");
break;
}
- }
- catch(...) {
+ } catch (...) {
printf("An error occurs...\n");
}
return 0;
}
#else
-int
-main()
+int main()
{
vpTRACE("Sorry, for the moment, vpRingLight class works only on unix...");
return 0;
diff --git a/example/direct-visual-servoing/CMakeLists.txt b/example/direct-visual-servoing/CMakeLists.txt
index 50e68e3..e2a2e6b 100644
--- a/example/direct-visual-servoing/CMakeLists.txt
+++ b/example/direct-visual-servoing/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/direct-visual-servoing/photometricVisualServoing.cpp b/example/direct-visual-servoing/photometricVisualServoing.cpp
old mode 100755
new mode 100644
index cdf5138..767d121
--- a/example/direct-visual-servoing/photometricVisualServoing.cpp
+++ b/example/direct-visual-servoing/photometricVisualServoing.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,41 +40,39 @@
Implemented from \cite Collewet08c.
*/
-
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpTime.h>
#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/visual_features/vpFeatureLuminance.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/visual_features/vpFeatureLuminance.h>
-#include <visp3/robot/vpImageSimulator.h>
#include <stdlib.h>
-#define Z 1
+#include <visp3/robot/vpImageSimulator.h>
+#define Z 1
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:n:h"
+#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);
+bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display, int &niter);
/*!
@@ -97,7 +96,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/doisneau/doisneau.jpg\"\n\
+ From this path read \"doisneau/doisneau.jpg\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -114,8 +113,7 @@ OPTIONS: Default\n\
Number of iterations.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), niter);
+ Print the help.\n", ipath.c_str(), niter);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -134,23 +132,34 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'n': niter = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, niter); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'n':
+ niter = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, niter);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, niter);
- return false; break;
+ return false;
+ break;
}
}
@@ -165,10 +174,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -179,16 +185,16 @@ main(int argc, const char ** argv)
bool opt_display = true;
int opt_niter = 400;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
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);
}
@@ -200,8 +206,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -209,23 +214,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
- vpImage<unsigned char> Itexture ;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(Itexture,filename) ;
+ vpImage<unsigned char> Itexture;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(Itexture, filename);
vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
+ for (int i = 0; i < 4; i++)
+ X[i].resize(3);
// Top left corner
X[0][0] = -0.3;
X[0][1] = -0.215;
@@ -241,33 +246,33 @@ main(int argc, const char ** argv)
X[2][1] = 0.215;
X[2][2] = 0;
- //Bottom left corner
+ // Bottom left corner
X[3][0] = -0.3;
X[3][1] = 0.215;
X[3][2] = 0;
vpImageSimulator sim;
- sim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION) ;
+ sim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);
sim.init(Itexture, X);
vpCameraParameters cam(870, 870, 160, 120);
// ----------------------------------------------------------
// Create the framegraber (here a simulated image)
- vpImage<unsigned char> I(240,320,0) ;
- vpImage<unsigned char> Id ;
+ vpImage<unsigned char> I(240, 320, 0);
+ vpImage<unsigned char> Id;
- //camera desired position
- vpHomogeneousMatrix cdMo ;
- cdMo[2][3] = 1 ;
+ // camera desired position
+ vpHomogeneousMatrix cdMo;
+ cdMo[2][3] = 1;
- //set the robot at the desired position
- sim.setCameraPosition(cdMo) ;
- sim.getImage(I,cam); // and aquire the image Id
- Id = I ;
+ // set the robot at the desired position
+ sim.setCameraPosition(cdMo);
+ sim.getImage(I, cam); // and aquire the image Id
+ Id = I;
- // display the image
+// display the image
#if defined VISP_HAVE_X11
vpDisplayX d;
#elif defined VISP_HAVE_GDI
@@ -278,15 +283,15 @@ 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") ;
+ d.init(I, 20, 10, "Photometric visual servoing : s");
vpDisplay::display(I);
vpDisplay::flush(I);
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
#endif
@@ -294,34 +299,34 @@ main(int argc, const char ** argv)
// position the robot at the initial position
// ----------------------------------------------------------
- //camera desired position
- vpHomogeneousMatrix cMo ;
- cMo.buildFrom(0,0,1.2,vpMath::rad(15),vpMath::rad(-5),vpMath::rad(20));
+ // camera desired position
+ vpHomogeneousMatrix cMo;
+ cMo.buildFrom(0, 0, 1.2, vpMath::rad(15), vpMath::rad(-5), vpMath::rad(20));
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
- //set the robot at the desired position
- sim.setCameraPosition(cMo) ;
- I =0 ;
- sim.getImage(I,cam); // and aquire the image Id
+ // set the robot at the desired position
+ sim.setCameraPosition(cMo);
+ I = 0;
+ sim.getImage(I, cam); // and aquire the image Id
-#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)
if (opt_display) {
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
if (opt_display && opt_click_allowed) {
std::cout << "Click in the image to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
-#endif
+#endif
- vpImage<unsigned char> Idiff ;
- Idiff = I ;
+ vpImage<unsigned char> Idiff;
+ Idiff = I;
- vpImageTools::imageDifference(I,Id,Idiff) ;
+ vpImageTools::imageDifference(I, Id, Idiff);
- // Affiche de l'image de difference
+// Affiche de l'image de difference
#if defined VISP_HAVE_X11
vpDisplayX d1;
#elif defined VISP_HAVE_GDI
@@ -329,11 +334,11 @@ main(int argc, const char ** argv)
#elif defined VISP_HAVE_GTK
vpDisplayGTK d1;
#endif
-#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)
if (opt_display) {
- d1.init(Idiff, 40+(int)I.getWidth(), 10, "photometric visual servoing : s-s* ") ;
- vpDisplay::display(Idiff) ;
- vpDisplay::flush(Idiff) ;
+ d1.init(Idiff, 40 + (int)I.getWidth(), 10, "photometric visual servoing : s-s* ");
+ vpDisplay::display(Idiff);
+ vpDisplay::flush(Idiff);
}
#endif
// create the robot (here a simulated free flying camera)
@@ -349,98 +354,98 @@ main(int argc, const char ** argv)
// current visual feature built from the image
// (actually, this is the image...)
- vpFeatureLuminance sI ;
- sI.init( I.getHeight(), I.getWidth(), Z) ;
- sI.setCameraParameters(cam) ;
- sI.buildFrom(I) ;
+ vpFeatureLuminance sI;
+ sI.init(I.getHeight(), I.getWidth(), Z);
+ sI.setCameraParameters(cam);
+ sI.buildFrom(I);
// desired visual feature built from the image
- vpFeatureLuminance sId ;
- sId.init(I.getHeight(), I.getWidth(), Z) ;
- sId.setCameraParameters(cam) ;
- sId.buildFrom(Id) ;
+ vpFeatureLuminance sId;
+ sId.init(I.getHeight(), I.getWidth(), Z);
+ sId.setCameraParameters(cam);
+ sId.buildFrom(Id);
// Matrice d'interaction, Hessien, erreur,...
- vpMatrix Lsd; // matrice d'interaction a la position desiree
- vpMatrix Hsd; // hessien a la position desiree
- vpMatrix H ; // Hessien utilise pour le levenberg-Marquartd
- vpColVector error ; // Erreur I-I*
+ vpMatrix Lsd; // matrice d'interaction a la position desiree
+ vpMatrix Hsd; // hessien a la position desiree
+ vpMatrix H; // Hessien utilise pour le levenberg-Marquartd
+ vpColVector error; // Erreur I-I*
// Compute the interaction matrix
// link the variation of image intensity to camera motion
// here it is computed at the desired position
- sId.interaction(Lsd) ;
+ sId.interaction(Lsd);
// Compute the Hessian H = L^TL
- Hsd = Lsd.AtA() ;
+ Hsd = Lsd.AtA();
// Compute the Hessian diagonal for the Levenberg-Marquartd
// optimization process
- unsigned int n = 6 ;
- vpMatrix diagHsd(n,n) ;
+ unsigned int n = 6;
+ vpMatrix diagHsd(n, n);
diagHsd.eye(n);
- for(unsigned int i = 0 ; i < n ; i++) diagHsd[i][i] = Hsd[i][i];
+ for (unsigned int i = 0; i < n; i++)
+ diagHsd[i][i] = Hsd[i][i];
// ------------------------------------------------------
// Control law
- double lambda ; //gain
- vpColVector e ;
- vpColVector v ; // camera velocity send to the robot
+ double lambda; // gain
+ vpColVector e;
+ vpColVector v; // camera velocity send to the robot
// ----------------------------------------------------------
// Minimisation
- double mu ; // mu = 0 : Gauss Newton ; mu != 0 : LM
+ double mu; // mu = 0 : Gauss Newton ; mu != 0 : LM
double lambdaGN;
- mu = 0.01;
- lambda = 30 ;
+ mu = 0.01;
+ lambda = 30;
lambdaGN = 30;
// set a velocity control mode
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// ----------------------------------------------------------
- int iter = 1;
- int iterGN = 90 ; // swicth to Gauss Newton after iterGN iterations
+ int iter = 1;
+ int iterGN = 90; // swicth to Gauss Newton after iterGN iterations
double normeError = 0;
do {
- std::cout << "--------------------------------------------" << iter++ << std::endl ;
+ std::cout << "--------------------------------------------" << iter++ << std::endl;
// Acquire the new image
- sim.setCameraPosition(cMo) ;
- sim.getImage(I,cam) ;
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
+ sim.setCameraPosition(cMo);
+ sim.getImage(I, cam);
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
#endif
- vpImageTools::imageDifference(I,Id,Idiff) ;
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
+ vpImageTools::imageDifference(I, Id, Idiff);
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_GTK)
if (opt_display) {
- vpDisplay::display(Idiff) ;
- vpDisplay::flush(Idiff) ;
+ vpDisplay::display(Idiff);
+ vpDisplay::flush(Idiff);
}
#endif
// Compute current visual feature
- sI.buildFrom(I) ;
+ sI.buildFrom(I);
// compute current error
- sI.error(sId,error) ;
+ sI.error(sId, error);
normeError = (error.sumSquare());
- std::cout << "|e| "<<normeError <<std::endl ;
+ std::cout << "|e| " << normeError << std::endl;
// double t = vpTime::measureTimeMs() ;
// ---------- Levenberg Marquardt method --------------
{
- if (iter > iterGN)
- {
- mu = 0.0001 ;
+ if (iter > iterGN) {
+ mu = 0.0001;
lambda = lambdaGN;
}
@@ -449,27 +454,25 @@ main(int argc, const char ** argv)
H = ((mu * diagHsd) + Hsd).inverseByLU();
}
// compute the control law
- e = H * Lsd.t() *error ;
+ e = H * Lsd.t() * error;
- v = - lambda*e;
+ v = -lambda * e;
}
- std::cout << "lambda = " << lambda << " mu = " << mu ;
+ std::cout << "lambda = " << lambda << " mu = " << mu;
std::cout << " |Tc| = " << sqrt(v.sumSquare()) << std::endl;
// send the robot velocity
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- }
- while(normeError > 10000 && iter < opt_niter);
+ } while (normeError > 10000 && iter < opt_niter);
- v = 0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/homography/CMakeLists.txt b/example/homography/CMakeLists.txt
index 07d2896..e418154 100644
--- a/example/homography/CMakeLists.txt
+++ b/example/homography/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/homography/homographyHLM2DObject.cpp b/example/homography/homographyHLM2DObject.cpp
index 6bdc2b2..e0fe47e 100644
--- a/example/homography/homographyHLM2DObject.cpp
+++ b/example/homography/homographyHLM2DObject.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,34 +39,33 @@
/*!
\file homographyHLM2DObject.cpp
- \brief Example of the HLM (Malis) homography estimation algorithm with a planar object using vpHomography class.
+ \brief Example of the HLM (Malis) homography estimation algorithm with a
+ planar object using vpHomography class.
*/
-
-
/*!
\example homographyHLM2DObject.cpp
- Example of the HLM (Malis) homography estimation algorithm with a planar object using vpHomography class.
+ Example of the HLM (Malis) homography estimation algorithm with a planar
+ object using vpHomography class.
*/
-
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
+#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <stdlib.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
#define L 0.1
#define nbpt 5
@@ -95,7 +95,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -112,15 +112,19 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,140 +139,131 @@ bool getOptions(int argc, const char **argv)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpPoint P[nbpt] ; // Point to be tracked
- std::vector<double> xa(nbpt), ya(nbpt) ;
- std::vector<double> xb(nbpt), yb(nbpt) ;
+ vpPoint P[nbpt]; // Point to be tracked
+ std::vector<double> xa(nbpt), ya(nbpt);
+ std::vector<double> xb(nbpt), yb(nbpt);
- vpPoint aP[nbpt] ; // Point to be tracked
- vpPoint bP[nbpt] ; // Point to be tracked
+ vpPoint aP[nbpt]; // Point to be tracked
+ vpPoint bP[nbpt]; // Point to be tracked
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(2*L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,3*L, 0 ) ;
- P[4].setWorldCoordinates(0,0, 0 ) ;
+ P[0].setWorldCoordinates(-L, -L, 0);
+ P[1].setWorldCoordinates(2 * L, -L, 0);
+ P[2].setWorldCoordinates(L, L, 0);
+ P[3].setWorldCoordinates(-L, 3 * L, 0);
+ P[4].setWorldCoordinates(0, 0, 0);
/*
P[5].setWorldCoordinates(10,20, 0 ) ;
P[6].setWorldCoordinates(-10,12, 0 ) ;
*/
- vpHomogeneousMatrix bMo(0,0,1, 0,0,0) ;
- vpHomogeneousMatrix aMb(1,0,0.0,vpMath::rad(10),0,vpMath::rad(40)) ;
- vpHomogeneousMatrix aMo =aMb*bMo ;
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(aMo) ;
- aP[i] = P[i] ;
- xa[i] = P[i].get_x() ;
- ya[i] = P[i].get_y() ;
+ vpHomogeneousMatrix bMo(0, 0, 1, 0, 0, 0);
+ vpHomogeneousMatrix aMb(1, 0, 0.0, vpMath::rad(10), 0, vpMath::rad(40));
+ vpHomogeneousMatrix aMo = aMb * bMo;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(aMo);
+ aP[i] = P[i];
+ xa[i] = P[i].get_x();
+ ya[i] = P[i].get_y();
}
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(bMo) ;
- bP[i] = P[i] ;
- xb[i] = P[i].get_x() ;
- yb[i] = P[i].get_y() ;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(bMo);
+ bP[i] = P[i];
+ xb[i] = P[i].get_x();
+ yb[i] = P[i].get_y();
}
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "aMb "<<std::endl <<aMb << std::endl ;
- std::cout << "-------------------------------" <<std::endl ;
- vpHomography aHb ;
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "aMb " << std::endl << aMb << std::endl;
+ std::cout << "-------------------------------" << std::endl;
+ vpHomography aHb;
- vpHomography::HLM(xb, yb, xa, ya, true, aHb) ;
+ 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 ;
+ aHb /= aHb[2][2];
+ std::cout << "aHb computed using the Malis paralax algorithm: \n" << aHb << std::endl;
- vpRotationMatrix aRb ;
- vpTranslationVector aTb ;
- vpColVector n ;
+ vpRotationMatrix aRb;
+ vpTranslationVector aTb;
+ vpColVector n;
- std::cout << "-------------------------------" <<std::endl ;
+ std::cout << "-------------------------------" << std::endl;
std::cout << "extract R, T and n " << std::endl;
- aHb.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
-
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "Compare with built homography H = R + t/d " << std::endl ;
- vpPlane bp(0,0,1,1) ;
- vpHomography aHb_built(aMb,bp) ;
- std::cout << "aHb built from the displacement " << std::endl ;
- std::cout << std::endl <<aHb_built/aHb_built[2][2] << std::endl ;
-
- aHb_built.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" << std::endl ;
- std::cout << "test if ap = aHb bp" << std::endl ;
-
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- 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 <<") = (" ;
- p = aHb*bP[i] ;
- std::cout << p.get_x() /p.get_w()<<", "<< p.get_y()/ p.get_w() <<")"<<std::endl ;
+ aHb.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "Compare with built homography H = R + t/d " << std::endl;
+ vpPlane bp(0, 0, 1, 1);
+ vpHomography aHb_built(aMb, bp);
+ std::cout << "aHb built from the displacement " << std::endl;
+ std::cout << std::endl << aHb_built / aHb_built[2][2] << std::endl;
+
+ aHb_built.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "test if ap = aHb bp" << std::endl;
+
+ for (unsigned int i = 0; i < nbpt; i++) {
+ 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 << ") = (";
+ p = aHb * bP[i];
+ std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl;
}
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "test displacement" << std::endl ;
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "test displacement" << std::endl;
- std::list<vpRotationMatrix> laRb ;
- std::list<vpTranslationVector> laTb ;
- std::list<vpColVector> lnb ;
+ std::list<vpRotationMatrix> laRb;
+ std::list<vpTranslationVector> laTb;
+ std::list<vpColVector> 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<vpRotationMatrix>::const_iterator it_laRb = laRb.begin();
std::list<vpTranslationVector>::const_iterator it_laTb = laTb.begin();
std::list<vpColVector>::const_iterator it_lnb = lnb.begin();
- int k =1 ;
- while (it_lnb != lnb.end())
- {
- std::cout << "Solution " << k++ << std::endl ;
+ int k = 1;
+ while (it_lnb != lnb.end()) {
+ std::cout << "Solution " << k++ << std::endl;
aRb = *it_laRb;
aTb = *it_laTb;
n = *it_lnb;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- ++ it_laRb;
- ++ it_laTb;
- ++ it_lnb;
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ ++it_laRb;
+ ++it_laTb;
+ ++it_lnb;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/homography/homographyHLM3DObject.cpp b/example/homography/homographyHLM3DObject.cpp
index b8b757b..95d4f63 100644
--- a/example/homography/homographyHLM3DObject.cpp
+++ b/example/homography/homographyHLM3DObject.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,32 +39,33 @@
/*!
\file homographyHLM3DObject.cpp
- \brief Example of the HLM (Malis) homography estimation algorithm with a 3D object using vpHomography class.
+ \brief Example of the HLM (Malis) homography estimation algorithm with a 3D
+ object using vpHomography class.
*/
/*!
\example homographyHLM3DObject.cpp
- Example of the HLM (Malis) homography estimation algorithm with a 3D object using vpHomography class.
+ Example of the HLM (Malis) homography estimation algorithm with a 3D object
+ using vpHomography class.
*/
-
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
+#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <stdlib.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
#define L 0.1
#define nbpt 11
@@ -94,7 +96,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -111,15 +113,19 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -134,108 +140,102 @@ bool getOptions(int argc, const char **argv)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpPoint P[nbpt] ; // Point to be tracked
- std::vector<double> xa(nbpt), ya(nbpt) ;
- std::vector<double> xb(nbpt), yb(nbpt) ;
-
- vpPoint aP[nbpt] ; // Point to be tracked
- vpPoint bP[nbpt] ; // Point to be tracked
-
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(2*L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,3*L, 0 ) ;
- P[4].setWorldCoordinates(0,0, L ) ;
- P[5].setWorldCoordinates(L,-2*L, L ) ;
- P[6].setWorldCoordinates(L,-4*L, 2*L ) ;
- P[7].setWorldCoordinates(-2*L,-L, -L ) ;
- P[8].setWorldCoordinates(-5*L,-5*L, L ) ;
- P[9].setWorldCoordinates(-2*L,+3*L, 2*L ) ;
- 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 aMo =aMb*bMo ;
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(aMo) ;
- aP[i] = P[i] ;
- xa[i] = P[i].get_x() ;
- ya[i] = P[i].get_y() ;
+ vpPoint P[nbpt]; // Point to be tracked
+ std::vector<double> xa(nbpt), ya(nbpt);
+ std::vector<double> xb(nbpt), yb(nbpt);
+
+ vpPoint aP[nbpt]; // Point to be tracked
+ vpPoint bP[nbpt]; // Point to be tracked
+
+ P[0].setWorldCoordinates(-L, -L, 0);
+ P[1].setWorldCoordinates(2 * L, -L, 0);
+ P[2].setWorldCoordinates(L, L, 0);
+ P[3].setWorldCoordinates(-L, 3 * L, 0);
+ P[4].setWorldCoordinates(0, 0, L);
+ P[5].setWorldCoordinates(L, -2 * L, L);
+ P[6].setWorldCoordinates(L, -4 * L, 2 * L);
+ P[7].setWorldCoordinates(-2 * L, -L, -L);
+ P[8].setWorldCoordinates(-5 * L, -5 * L, L);
+ P[9].setWorldCoordinates(-2 * L, +3 * L, 2 * L);
+ 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 aMo = aMb * bMo;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(aMo);
+ aP[i] = P[i];
+ xa[i] = P[i].get_x();
+ ya[i] = P[i].get_y();
}
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(bMo) ;
- bP[i] = P[i] ;
- xb[i] = P[i].get_x() ;
- yb[i] = P[i].get_y() ;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(bMo);
+ bP[i] = P[i];
+ xb[i] = P[i].get_x();
+ yb[i] = P[i].get_y();
}
- vpRotationMatrix aRb ;
- vpTranslationVector aTb ;
- vpColVector n ;
- std::cout << "-------------------------------" << std::endl ;
- 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 ;
-
- aHb_built.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "aMb "<<std::endl <<aMb << std::endl ;
- std::cout << "-------------------------------" <<std::endl ;
- vpHomography aHb ;
-
- vpHomography::HLM(xb, yb, xa, ya, false, aHb) ;
-
- std::cout << "aHb computed using the Malis paralax algorithm" << std::endl ;
- aHb /= aHb[2][2] ;
- std::cout << std::endl << aHb<< std::endl ;
-
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "extract R, T and n " << std::endl ;
- aHb.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "test if ap = aHb bp" << std::endl ;
-
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- 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 <<") = (" ;
- p = aHb*bP[i] ;
- std::cout << p.get_x() /p.get_w()<<", "<< p.get_y()/ p.get_w() <<")"<<std::endl ;
+ vpRotationMatrix aRb;
+ vpTranslationVector aTb;
+ vpColVector n;
+ std::cout << "-------------------------------" << std::endl;
+ 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;
+
+ aHb_built.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "aMb " << std::endl << aMb << std::endl;
+ std::cout << "-------------------------------" << std::endl;
+ vpHomography aHb;
+
+ vpHomography::HLM(xb, yb, xa, ya, false, aHb);
+
+ std::cout << "aHb computed using the Malis paralax algorithm" << std::endl;
+ aHb /= aHb[2][2];
+ std::cout << std::endl << aHb << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "extract R, T and n " << std::endl;
+ aHb.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "test if ap = aHb bp" << std::endl;
+
+ for (unsigned int i = 0; i < nbpt; i++) {
+ 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 << ") = (";
+ p = aHb * bP[i];
+ std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/homography/homographyHartleyDLT2DObject.cpp b/example/homography/homographyHartleyDLT2DObject.cpp
index 7a90e14..d5c5d6e 100644
--- a/example/homography/homographyHartleyDLT2DObject.cpp
+++ b/example/homography/homographyHartleyDLT2DObject.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,22 +50,20 @@
*/
-
-
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
+#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <stdlib.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
#define L 0.1
#define nbpt 5
@@ -95,7 +94,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -113,15 +112,19 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -136,105 +139,99 @@ bool getOptions(int argc, const char **argv)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpPoint P[nbpt] ; // Point to be tracked
+ vpPoint P[nbpt]; // Point to be tracked
std::vector<double> xa(nbpt), ya(nbpt), xb(nbpt), yb(nbpt);
- vpPoint aP[nbpt] ; // Point to be tracked
- vpPoint bP[nbpt] ; // Point to be tracked
+ vpPoint aP[nbpt]; // Point to be tracked
+ vpPoint bP[nbpt]; // Point to be tracked
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(2*L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,3*L, 0 ) ;
- P[4].setWorldCoordinates(0,0, 0 ) ;
+ P[0].setWorldCoordinates(-L, -L, 0);
+ P[1].setWorldCoordinates(2 * L, -L, 0);
+ P[2].setWorldCoordinates(L, L, 0);
+ P[3].setWorldCoordinates(-L, 3 * L, 0);
+ P[4].setWorldCoordinates(0, 0, 0);
/*
P[5].setWorldCoordinates(10,20, 0 ) ;
P[6].setWorldCoordinates(-10,12, 0 ) ;
*/
- vpHomogeneousMatrix bMo(0,0,1, 0,0,0) ;
- vpHomogeneousMatrix aMb(1,0,0.0,vpMath::rad(10),0,vpMath::rad(40)) ;
- vpHomogeneousMatrix aMo =aMb*bMo ;
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(aMo) ;
- aP[i] = P[i] ;
- xa[i] = P[i].get_x() ;
- ya[i] = P[i].get_y() ;
+ vpHomogeneousMatrix bMo(0, 0, 1, 0, 0, 0);
+ vpHomogeneousMatrix aMb(1, 0, 0.0, vpMath::rad(10), 0, vpMath::rad(40));
+ vpHomogeneousMatrix aMo = aMb * bMo;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(aMo);
+ aP[i] = P[i];
+ xa[i] = P[i].get_x();
+ ya[i] = P[i].get_y();
}
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(bMo) ;
- bP[i] = P[i] ;
- xb[i] = P[i].get_x() ;
- yb[i] = P[i].get_y() ;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(bMo);
+ bP[i] = P[i];
+ xb[i] = P[i].get_x();
+ yb[i] = P[i].get_y();
}
- std::cout << "-------------------------------" <<std::endl ;
- std::cout << "aMb "<<std::endl <<aMb << std::endl ;
- std::cout << "-------------------------------" <<std::endl ;
- vpHomography aHb ;
-
- vpHomography::DLT(xb, yb, xa, ya, aHb, true) ;
-
- vpTRACE("aHb computed using the DLT algorithm") ;
- aHb /= aHb[2][2] ;
- std::cout << std::endl << aHb<< std::endl ;
-
- vpRotationMatrix aRb ;
- vpTranslationVector aTb ;
- vpColVector n ;
-
- std::cout << "-------------------------------" <<std::endl ;
- vpTRACE("extract R, T and n ") ;
- aHb.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" <<std::endl ;
- vpTRACE("Compare with built homoraphy H = R + t/d ") ;
- vpPlane bp(0,0,1,1) ;
- vpHomography aHb_built(aMb,bp) ;
- vpTRACE( "aHb built from the displacement ") ;
- std::cout << std::endl <<aHb_built/aHb_built[2][2] << std::endl ;
-
- aHb_built.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation: aRb" <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" <<std::endl ;
- vpTRACE("test if ap = aHb bp") ;
-
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- 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 <<") = (" ;
- p = aHb*bP[i] ;
- std::cout << p.get_x() /p.get_w()<<", "<< p.get_y()/ p.get_w() <<")"<<std::endl ;
+ std::cout << "-------------------------------" << std::endl;
+ std::cout << "aMb " << std::endl << aMb << std::endl;
+ std::cout << "-------------------------------" << std::endl;
+ vpHomography aHb;
+
+ vpHomography::DLT(xb, yb, xa, ya, aHb, true);
+
+ vpTRACE("aHb computed using the DLT algorithm");
+ aHb /= aHb[2][2];
+ std::cout << std::endl << aHb << std::endl;
+
+ vpRotationMatrix aRb;
+ vpTranslationVector aTb;
+ vpColVector n;
+
+ std::cout << "-------------------------------" << std::endl;
+ vpTRACE("extract R, T and n ");
+ aHb.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ vpTRACE("Compare with built homoraphy H = R + t/d ");
+ vpPlane bp(0, 0, 1, 1);
+ vpHomography aHb_built(aMb, bp);
+ vpTRACE("aHb built from the displacement ");
+ std::cout << std::endl << aHb_built / aHb_built[2][2] << std::endl;
+
+ aHb_built.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation: aRb" << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
+ vpTRACE("test if ap = aHb bp");
+
+ for (unsigned int i = 0; i < nbpt; i++) {
+ 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 << ") = (";
+ p = aHb * bP[i];
+ std::cout << p.get_x() / p.get_w() << ", " << p.get_y() / p.get_w() << ")" << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/homography/homographyRansac2DObject.cpp b/example/homography/homographyRansac2DObject.cpp
index fc53d17..08744dc 100644
--- a/example/homography/homographyRansac2DObject.cpp
+++ b/example/homography/homographyRansac2DObject.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,7 +44,6 @@
*/
-
/*!
\example homographyRansac2DObject.cpp
@@ -52,23 +52,22 @@
*/
-
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
+#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <stdlib.h>
#include <visp3/core/vpRansac.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -95,7 +94,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -112,15 +111,19 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,33 +138,31 @@ bool getOptions(int argc, const char **argv)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- double L=0.1;
+ double L = 0.1;
unsigned int nbpt = 11;
- std::vector<vpPoint> P(nbpt); // Point to be tracked
+ std::vector<vpPoint> P(nbpt); // Point to be tracked
std::vector<double> xa(nbpt), ya(nbpt), xb(nbpt), yb(nbpt);
- P[0].setWorldCoordinates(-L,-L, 0 ) ; // inlier
- P[1].setWorldCoordinates(2*L,-L, 0 ) ; // inlier
- P[2].setWorldCoordinates(L,L, 0 ) ; // inlier
- P[3].setWorldCoordinates(-L,3*L, 0 ) ; // inlier
- P[4].setWorldCoordinates(0,0, L ) ;
- P[5].setWorldCoordinates(L,-2*L, L ) ;
- P[6].setWorldCoordinates(L,-4*L, 2*L ) ;
- P[7].setWorldCoordinates(-2*L,-L, -3*L ) ;
- P[8].setWorldCoordinates(-5*L,-5*L, 0 ) ; // inlier
- P[9].setWorldCoordinates(-2*L,+3*L, 4*L ) ;
- P[10].setWorldCoordinates(-2*L,-0.5*L, 0 ) ; // inlier
+ P[0].setWorldCoordinates(-L, -L, 0); // inlier
+ P[1].setWorldCoordinates(2 * L, -L, 0); // inlier
+ P[2].setWorldCoordinates(L, L, 0); // inlier
+ P[3].setWorldCoordinates(-L, 3 * L, 0); // inlier
+ P[4].setWorldCoordinates(0, 0, L);
+ P[5].setWorldCoordinates(L, -2 * L, L);
+ P[6].setWorldCoordinates(L, -4 * L, 2 * L);
+ P[7].setWorldCoordinates(-2 * L, -L, -3 * L);
+ P[8].setWorldCoordinates(-5 * L, -5 * L, 0); // inlier
+ P[9].setWorldCoordinates(-2 * L, +3 * L, 4 * L);
+ P[10].setWorldCoordinates(-2 * L, -0.5 * L, 0); // inlier
std::vector<bool> inliers_ground_truth(nbpt, false);
inliers_ground_truth[0] = true;
@@ -171,64 +172,61 @@ main(int argc, const char ** argv)
inliers_ground_truth[8] = true;
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 aMo =aMb*bMo ;
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(aMo) ;
- xa[i] = P[i].get_x() ;
- ya[i] = P[i].get_y() ;
+ vpHomogeneousMatrix bMo(0, 0, 1, 0, 0, 0);
+ 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);
+ xa[i] = P[i].get_x();
+ ya[i] = P[i].get_y();
}
- for(unsigned int i=0 ; i < nbpt ; i++)
- {
- P[i].project(bMo) ;
- xb[i] = P[i].get_x() ;
- yb[i] = P[i].get_y() ;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ P[i].project(bMo);
+ xb[i] = P[i].get_x();
+ yb[i] = P[i].get_y();
}
- std::cout << "-------------------------------" <<std::endl ;
+ std::cout << "-------------------------------" << std::endl;
- vpRotationMatrix aRb ;
- vpTranslationVector aTb ;
- vpColVector n ;
+ vpRotationMatrix aRb;
+ vpTranslationVector aTb;
+ vpColVector n;
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 ;
-
- aHb_built.computeDisplacement(aRb, aTb, n) ;
- std::cout << "Rotation aRb: " <<std::endl ;
- std::cout << aRb << std::endl ;
- std::cout << "Translation: aTb" <<std::endl;
- std::cout << (aTb).t() <<std::endl ;
- std::cout << "Normal to the plane: n" <<std::endl;
- std::cout << (n).t() <<std::endl ;
-
- std::cout << "-------------------------------" <<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;
+
+ aHb_built.computeDisplacement(aRb, aTb, n);
+ std::cout << "Rotation aRb: " << std::endl;
+ std::cout << aRb << std::endl;
+ std::cout << "Translation: aTb" << std::endl;
+ std::cout << (aTb).t() << std::endl;
+ std::cout << "Normal to the plane: n" << std::endl;
+ std::cout << (n).t() << std::endl;
+
+ std::cout << "-------------------------------" << std::endl;
vpHomography aHb;
std::vector<bool> inliers;
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 << "aHb estimated using ransac:\n" << aHb << std::endl;
std::cout << "Inliers indexes (should be 0,1,2,3,8,10): ";
- for (unsigned int i=0; i< inliers.size(); i++)
- if (inliers[i]) std::cout << i << ",";
+ for (unsigned int i = 0; i < inliers.size(); i++)
+ if (inliers[i])
+ std::cout << i << ",";
std::cout << std::endl;
if (inliers == inliers_ground_truth) {
std::cout << "Ransac estimation succeed" << std::endl;
return 0;
- }
- else {
+ } else {
std::cout << "Ransac estimation fails" << std::endl;
return 1;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/image/CMakeLists.txt b/example/image/CMakeLists.txt
index 5189061..284ddb8 100644
--- a/example/image/CMakeLists.txt
+++ b/example/image/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/image/imageDiskRW.cpp b/example/image/imageDiskRW.cpp
index 702adee..80a05b5 100644
--- a/example/image/imageDiskRW.cpp
+++ b/example/image/imageDiskRW.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -53,20 +54,15 @@
write in a directory that does no exist
*/
-
-
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+#define GETOPTARGS "i:o:h"
/*!
@@ -93,7 +89,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -106,11 +102,10 @@ OPTIONS: Default\n\
Klimt_grey.pgm output image is written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -126,19 +121,28 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -153,10 +157,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -167,23 +168,24 @@ main(int argc, const char ** argv)
std::string filename;
std::string username;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << " imageDiskRW.cpp" <<std::endl << 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 << " 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;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -195,7 +197,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -212,11 +214,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(dirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << dirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -227,8 +227,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -236,41 +235,40 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
/////////////////////////////////////////////////////////////////////
// First we wanted to have gray level image (8bits)
- // vpImage is a template class you can declare vpImage of ... everything...
- vpImage<unsigned char> I ;
+ // vpImage is a template class you can declare vpImage of ...
+ // everything...
+ vpImage<unsigned char> I;
// Although I is a gray level image you can read and write
// color image. Obviously the color will be translated as a gray level
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
vpImageIo::read(I, filename);
filename = vpIoTools::createFilePath(dirname, "IoPPM.Klimt_char.ppm");
- vpImageIo::write(I, filename) ;
+ vpImageIo::write(I, filename);
// test io error
// if the image you want to read on the disk does not exist
// an exception is thrown
- //Try to load a non existing image
+ // Try to load a non existing image
try {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
- vpImageIo::read(I,filename) ;
- }
- catch(vpException &e) {
+ 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;
}
@@ -278,51 +276,46 @@ main(int argc, const char ** argv)
// or where you are not allowd to write.
try {
filename = vpIoTools::createFilePath(dirname, "directory-that-does-not-exist/Klimt.ppm");
- vpImageIo::write(I,filename) ;
- }
- catch(vpException &e) {
+ 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<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// read write unsigned char ppm image.
// Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
vpImageIo::read(Irgba, filename);
// Write the content of the color image on the disk
filename = vpIoTools::createFilePath(dirname, "IoPGM.Klimt_rgba.ppm");
- vpImageIo::write(Irgba, filename) ;
+ vpImageIo::write(Irgba, filename);
// test io error
try {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
- vpImageIo::read(Irgba,filename) ;
- }
- catch(vpException &e) {
+ 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;
}
// test io error
try {
filename = vpIoTools::createFilePath(dirname, "directory-that-does-not-exist/Klimt.ppm");
- vpImageIo::write(Irgba,filename) ;
+ vpImageIo::write(Irgba, filename);
}
- catch(vpException &e) {
+ catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/key-point/CMakeLists.txt b/example/key-point/CMakeLists.txt
index 85e8c70..64bbb99 100644
--- a/example/key-point/CMakeLists.txt
+++ b/example/key-point/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/key-point/fernClassifier.cpp b/example/key-point/fernClassifier.cpp
index 94753d7..fc897bf 100644
--- a/example/key-point/fernClassifier.cpp
+++ b/example/key-point/fernClassifier.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,8 @@
/*!
\file fernClassifier.cpp
- \brief Detection of points of interests and matching using a Fern classifier.
+ \brief Detection of points of interests and matching using a Fern
+ classifier.
*/
/*!
@@ -49,28 +51,29 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \
+ (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+#include <iomanip>
#include <iostream>
#include <stdlib.h>
-#include <visp3/vision/vpFernClassifier.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
-#include <iomanip>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpFernClassifier.h>
+#include <visp3/vision/vpHomography.h>
-#define GETOPTARGS "hlcdb:i:p"
+#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);
/*!
@@ -97,7 +100,7 @@ OPTIONS: \n\
\n\
-i <input image path> \n\
Set image input path.\n\
- From this path read \"ViSP-images/line/image.%%04d.pgm\"\n\
+ From this path read \"line/image.%%04d.pgm\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -121,7 +124,6 @@ OPTIONS: \n\
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
}
/*!
@@ -140,24 +142,40 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'l': isLearning = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
- case 'b': dataFile = optarg_; break;
- case 'p': displayPoints = true; break;
- case 'i': ipath = optarg_; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'l':
+ isLearning = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'b':
+ dataFile = optarg_;
+ break;
+ case 'p':
+ displayPoints = true;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -172,10 +190,7 @@ bool getOptions(int argc, const char **argv, bool &isLearning, std::string& data
return true;
}
-
-
-int
-main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
bool isLearning = false;
@@ -190,22 +205,23 @@ main(int argc, const char** argv)
std::string dirname;
std::string filename;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty()){
+ if (!env_ipath.empty()) {
ipath = env_ipath;
}
// Read the command line options
- if (getOptions(argc, argv,
- isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) == false) {
- exit (-1);
+ if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) ==
+ false) {
+ exit(-1);
}
// Get the option values
- if (!opt_ipath.empty()){
+ if (!opt_ipath.empty()) {
ipath = opt_ipath;
}
@@ -213,8 +229,7 @@ main(int argc, const char** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -222,25 +237,22 @@ main(int argc, const char** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare two images, these are gray level images (unsigned char)
- vpImage <unsigned char> I;
- vpImage <unsigned char> Iref;
-
+ vpImage<unsigned char> I;
+ vpImage<unsigned char> Iref;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
unsigned iter = 0; // Image number
@@ -255,23 +267,19 @@ main(int argc, const char** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
std::cout << "Load: " << filename << std::endl;
- vpImageIo::read(Iref, filename) ;
+ vpImageIo::read(Iref, filename);
I = Iref;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
@@ -294,98 +302,96 @@ main(int argc, const char** argv)
// declare a planar object detector
vpFernClassifier fern;
- if(isLearning){
- if(opt_display){
- displayRef.init(Iref, 100, 100, "Reference image") ;
+ if (isLearning) {
+ if (opt_display) {
+ displayRef.init(Iref, 100, 100, "Reference image");
vpDisplay::display(Iref);
vpDisplay::flush(Iref);
}
vpImagePoint corners[2];
- if (opt_display && opt_click_allowed){
- std::cout << "Click on the top left and the bottom right corners to define the reference plane" << std::endl;
- for (int i=0 ; i < 2 ; i++){
+ if (opt_display && opt_click_allowed) {
+ std::cout << "Click on the top left and the bottom right corners to "
+ "define the reference plane"
+ << std::endl;
+ for (int i = 0; i < 2; i++) {
vpDisplay::getClick(Iref, corners[i]);
std::cout << corners[i] << std::endl;
}
- }
- else{
- corners[0].set_ij(1,1);
- corners[1].set_ij(I.getHeight()-2,I.getWidth()-2);
+ } else {
+ corners[0].set_ij(1, 1);
+ corners[1].set_ij(I.getHeight() - 2, I.getWidth() - 2);
}
if (opt_display) {
- //Display the rectangle which defines the part of the image where the reference points are computed.
+ // 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::flush(Iref);
}
- if (opt_click_allowed){
+ if (opt_click_allowed) {
std::cout << "Click on the image to continue" << std::endl;
vpDisplay::getClick(Iref);
}
vpRect roi(corners[0], corners[1]);
- std::cout << "> train the classifier on the selected plane. (may take up to several minutes)." << std::endl;
- if(opt_display) {
+ std::cout << "> train the classifier on the selected plane. (may take "
+ "up to several minutes)."
+ << std::endl;
+ if (opt_display) {
vpDisplay::display(Iref);
vpDisplay::flush(Iref);
}
- try{
+ try {
fern.buildReference(Iref, roi);
- }
- catch(vpException &e){
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
- }
- catch(...){
+ } catch (...) {
std::cout << "unknown error, line " << __LINE__ << std::endl;
}
- try{
+ try {
fern.record(objectName, dataFile);
- }
- catch(vpException &e){
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
- }
- catch(...){
+ } catch (...) {
std::cout << "unknown error, line " << __LINE__ << std::endl;
}
std::cout << __LINE__ << std::endl;
- }
- else{
- if(!vpIoTools::checkFilename(dataFile)){
- vpERROR_TRACE("cannot load the database with the specified name. Has the object been learned with the -l option? ");
+ } else {
+ if (!vpIoTools::checkFilename(dataFile)) {
+ vpERROR_TRACE("cannot load the database with the specified name. Has "
+ "the object been learned with the -l option? ");
exit(-1);
}
- try{
+ try {
// load a previously recorded file
fern.load(dataFile, objectName);
- }
- catch(...){
- vpERROR_TRACE("cannot load the database with the specified name. Has the object been learned with the -l option? ");
+ } catch (...) {
+ vpERROR_TRACE("cannot load the database with the specified name. Has "
+ "the object been learned with the -l option? ");
exit(-1);
}
}
-
- if(opt_display){
- display.init(I, 110 + (int)Iref.getWidth(), 100, "Current image") ;
+ if (opt_display) {
+ display.init(I, 110 + (int)Iref.getWidth(), 100, "Current image");
vpDisplay::display(I);
vpDisplay::flush(I);
}
- if (opt_display && opt_click_allowed){
+ 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);
}
- for ( ; ; ) {
+ for (;;) {
// acquire a new image
iter++;
- if(iter >= 80){
+ if (iter >= 80) {
break;
}
s.str("");
@@ -394,52 +400,48 @@ main(int argc, const char** argv)
// read the image
vpImageIo::read(I, filename);
- if(opt_display){
+ if (opt_display) {
vpDisplay::display(I);
- if(isLearning)
+ if (isLearning)
vpDisplay::display(Iref);
}
- double t0 = vpTime::measureTimeMs ();
+ double t0 = vpTime::measureTimeMs();
// detection of the reference image
unsigned int nbpts;
- try{
+ try {
nbpts = fern.matchPoint(I);
- }
- catch(vpException &e){
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
return -1;
- }
- catch(...){
+ } catch (...) {
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){
+ if (opt_display) {
fern.display(Iref, I, 7);
vpDisplay::flush(I);
- if(isLearning)
+ if (isLearning)
vpDisplay::flush(Iref);
- if(vpDisplay::getClick(I, false)){
+ if (vpDisplay::getClick(I, false)) {
break;
}
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+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 558412b..5918738 100644
--- a/example/key-point/keyPointSurf.cpp
+++ b/example/key-point/keyPointSurf.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,30 +48,29 @@
Tracking of Surf key-points.
*/
-
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
-#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \
+ defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
#include <visp3/vision/vpKeyPointSurf.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
@@ -96,7 +96,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/line/image.%%04d.pgm\"\n\
+ From this path read \"line/image.%%04d.pgm\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -110,8 +110,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -132,18 +131,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -158,9 +167,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -171,17 +178,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -192,8 +199,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -201,25 +207,24 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> Iref ;
- vpImage<unsigned char> Icur ;
+ vpImage<unsigned char> Iref;
+ vpImage<unsigned char> Icur;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
unsigned int iter = 0; // Image number
@@ -234,27 +239,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(Iref, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(Iref, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display[2];
#elif defined VISP_HAVE_GTK
@@ -264,44 +265,40 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display[0].init(Iref, 100, 100,"Display reference image") ;
- vpDisplay::display(Iref) ;
- vpDisplay::flush(Iref) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ display[0].init(Iref, 100, 100, "Display reference image");
+ vpDisplay::display(Iref);
+ vpDisplay::flush(Iref);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
vpImagePoint corners[2];
- if (opt_display && opt_click_allowed)
- {
- std::cout << "Click on the top left and the bottom right corners to define the part of the image where the reference points will be computed" << std::endl;
- for (unsigned int i=0 ; i < 2 ; i++)
- {
+ if (opt_display && opt_click_allowed) {
+ std::cout << "Click on the top left and the bottom right corners to "
+ "define the part of the image where the reference points "
+ "will be computed"
+ << std::endl;
+ for (unsigned int i = 0; i < 2; i++) {
vpDisplay::getClick(Iref, corners[i]);
std::cout << corners[i] << std::endl;
}
- }
- else
- {
- corners[0].set_ij(156,209);
- corners[1].set_ij(272,378);
+ } else {
+ corners[0].set_ij(156, 209);
+ corners[1].set_ij(272, 378);
}
- if (opt_display)
- {
- //Display the rectangle which defines the part of the image where the reference points are computed.
+ 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::flush(Iref);
}
- if (opt_click_allowed)
- {
+ if (opt_click_allowed) {
std::cout << "Click on the image to continue" << std::endl;
vpDisplay::getClick(Iref);
}
@@ -312,30 +309,25 @@ main(int argc, const char ** argv)
height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- //Computes the reference points
+ // Computes the reference points
/* nbrRef = */ surf.buildReference(Iref, corners[0], height, width);
- unsigned int nbrPair = 0;
-
vpImageIo::read(Icur, filename);
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display[1].init(Icur, (int)(100+Iref.getWidth()), 100,"Display current image") ;
- vpDisplay::display(Icur) ;
- vpDisplay::flush(Icur) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, "Display current image");
+ vpDisplay::display(Icur);
+ vpDisplay::flush(Icur);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- for (iter = 1 ; iter < 30 ; iter++)
- {
- std::cout <<"----------------------------------------------------------"<<std::endl;
+ for (iter = 1; iter < 30; iter++) {
+ std::cout << "----------------------------------------------------------" << std::endl;
// set the new image name
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
@@ -344,38 +336,36 @@ main(int argc, const char ** argv)
vpImageIo::read(Icur, filename);
if (opt_display) {
// Display the image
- vpDisplay::display(Iref) ;
- vpDisplay::display(Icur) ;
+ vpDisplay::display(Iref);
+ vpDisplay::display(Icur);
}
- nbrPair = surf.matchPoint(Icur);
- std::cout << "Number of matched point : " << nbrPair <<std::endl;
+ unsigned int nbrPair = surf.matchPoint(Icur);
+ std::cout << "Number of matched point : " << nbrPair << std::endl;
- if (opt_display)
- {
+ if (opt_display) {
// Display the matched features
surf.display(Iref, Icur, 7);
vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::red);
- vpDisplay::flush(Iref) ;
- vpDisplay::flush(Icur) ;
+ vpDisplay::flush(Iref);
+ vpDisplay::flush(Icur);
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+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 opencv_nonfree component...");
+ vpERROR_TRACE("You do not have 1.1.0 <= OpenCV < 3.0.0 that contains "
+ "opencv_nonfree component...");
#endif
}
diff --git a/example/key-point/planarObjectDetector.cpp b/example/key-point/planarObjectDetector.cpp
index 8869b7d..8bc6039 100644
--- a/example/key-point/planarObjectDetector.cpp
+++ b/example/key-point/planarObjectDetector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,30 +50,31 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#if ((defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+#if ((defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI)) && \
+ (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
+#include <iomanip>
#include <iostream>
#include <stdlib.h>
-#include <visp3/vision/vpPlanarObjectDetector.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpTime.h>
-#include <iomanip>
-#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/vision/vpHomography.h>
+#include <visp3/vision/vpPlanarObjectDetector.h>
-#define GETOPTARGS "hlcdb:i:p"
+#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);
/*!
@@ -99,7 +101,7 @@ OPTIONS: \n\
\n\
-i <input image path> \n\
Set image input path.\n\
- From this path read \"ViSP-images/line/image.%%04d.pgm\"\n\
+ From this path read \"line/image.%%04d.pgm\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -126,7 +128,6 @@ OPTIONS: \n\
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
}
/*!
@@ -145,24 +146,40 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'l': isLearning = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
- case 'b': dataFile = optarg_; break;
- case 'p': displayPoints = true; break;
- case 'i': ipath = optarg_; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'l':
+ isLearning = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'b':
+ dataFile = optarg_;
+ break;
+ case 'p':
+ displayPoints = true;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -177,10 +194,7 @@ bool getOptions(int argc, const char **argv, bool &isLearning, std::string& data
return true;
}
-
-
-int
-main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
bool isLearning = false;
@@ -195,22 +209,23 @@ main(int argc, const char** argv)
std::string dirname;
std::string filename;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty()){
+ if (!env_ipath.empty()) {
ipath = env_ipath;
}
// Read the command line options
- if (getOptions(argc, argv,
- isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) == false) {
- exit (-1);
+ if (getOptions(argc, argv, isLearning, dataFile, opt_click_allowed, opt_display, displayPoints, opt_ipath) ==
+ false) {
+ exit(-1);
}
// Get the option values
- if (!opt_ipath.empty()){
+ if (!opt_ipath.empty()) {
ipath = opt_ipath;
}
@@ -218,8 +233,7 @@ main(int argc, const char** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -227,23 +241,22 @@ main(int argc, const char** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Declare two images, these are gray level images (unsigned char)
- vpImage <unsigned char> I;
- vpImage <unsigned char> Iref;
+ vpImage<unsigned char> I;
+ vpImage<unsigned char> Iref;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
unsigned iter = 0; // Image number
@@ -258,23 +271,19 @@ main(int argc, const char** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
std::cout << "Load: " << filename << std::endl;
- vpImageIo::read(Iref, filename) ;
+ vpImageIo::read(Iref, filename);
I = Iref;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
@@ -298,82 +307,85 @@ main(int argc, const char** argv)
vpPlanarObjectDetector planar;
vpImagePoint corners[2];
- if(isLearning){
- if(opt_display){
- displayRef.init(Iref, 100, 100, "Reference image") ;
+ if (isLearning) {
+ if (opt_display) {
+ displayRef.init(Iref, 100, 100, "Reference image");
vpDisplay::display(Iref);
vpDisplay::flush(Iref);
}
- if (opt_display && opt_click_allowed){
- std::cout << "Click on the top left and the bottom right corners to define the reference plane" << std::endl;
- for (int i=0 ; i < 2 ; i++){
+ if (opt_display && opt_click_allowed) {
+ std::cout << "Click on the top left and the bottom right corners to "
+ "define the reference plane"
+ << std::endl;
+ for (int i = 0; i < 2; i++) {
vpDisplay::getClick(Iref, corners[i]);
std::cout << corners[i] << std::endl;
}
- }
- else{
- corners[0].set_ij(50, I.getWidth()-100);// small ROI for the automated test
- corners[1].set_ij(I.getHeight()-100, I.getWidth()-2);
+ } else {
+ 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.
+ // 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::flush(Iref);
}
- if (opt_click_allowed){
+ if (opt_click_allowed) {
std::cout << "Click on the image to continue" << std::endl;
vpDisplay::getClick(Iref);
}
vpRect roi(corners[0], corners[1]);
- std::cout << "> train the classifier on the selected plane (may take up to several minutes)." << std::endl;
- if(opt_display) {
+ std::cout << "> train the classifier on the selected plane (may take "
+ "up to several minutes)."
+ << std::endl;
+ if (opt_display) {
vpDisplay::display(Iref);
vpDisplay::flush(Iref);
}
- double t0 = vpTime::measureTimeMs ();
+ double t0 = vpTime::measureTimeMs();
planar.buildReference(Iref, roi);
- std::cout << "build reference in " << vpTime::measureTimeMs () - t0 << " ms" << std::endl;
- t0 = vpTime::measureTimeMs ();
+ 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;
- }
- else{
- if(!vpIoTools::checkFilename(dataFile)){
- vpERROR_TRACE("cannot load the database with the specified name. Has the object been learned with the -l option? ");
+ 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 "
+ "the object been learned with the -l option? ");
exit(-1);
}
- try{
+ try {
// load a previously recorded file
planar.load(dataFile, objectName);
- }
- catch(...){
- vpERROR_TRACE("cannot load the database with the specified name. Has the object been learned with the -l option? ");
+ } catch (...) {
+ vpERROR_TRACE("cannot load the database with the specified name. Has "
+ "the object been learned with the -l option? ");
exit(-1);
}
}
- if(opt_display){
- display.init(I, 110 + (int)Iref.getWidth(), 100, "Current image") ;
+ if (opt_display) {
+ display.init(I, 110 + (int)Iref.getWidth(), 100, "Current image");
vpDisplay::display(I);
vpDisplay::flush(I);
}
- if (opt_display && opt_click_allowed){
+ 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);
}
- for ( ; ; ) {
+ for (;;) {
// acquire a new image
iter++;
- if(iter >= 80){
+ if (iter >= 80) {
break;
}
s.str("");
@@ -382,54 +394,51 @@ main(int argc, const char** argv)
// read the image
vpImageIo::read(I, filename);
- if(opt_display){
+ if (opt_display) {
vpDisplay::display(I);
}
- double t0 = vpTime::measureTimeMs ();
+ 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){
+ if (isDetected) {
vpHomography H;
planar.getHomography(H);
std::cout << " > computed homography:" << std::endl << H << std::endl;
- if(opt_display){
- if(isLearning){
+ if (opt_display) {
+ if (isLearning) {
vpDisplay::display(Iref);
vpDisplay::displayRectangle(Iref, corners[0], corners[1], vpColor::green);
planar.display(Iref, I, displayPoints);
vpDisplay::flush(Iref);
- }else{
+ } else {
planar.display(I, displayPoints);
}
}
- }
- else{
+ } else {
std::cout << " > reference is not detected in the image" << std::endl;
}
- if(opt_display){
+ if (opt_display) {
vpDisplay::flush(I);
- if(vpDisplay::getClick(I, false)){
+ if (vpDisplay::getClick(I, false)) {
break;
}
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+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/CMakeLists.txt b/example/manual/CMakeLists.txt
index 894ba00..c9053cb 100644
--- a/example/manual/CMakeLists.txt
+++ b/example/manual/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -59,8 +60,8 @@ set(example_cpp
if(VISP_HAVE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- visp_set_source_file_compile_flag(ogre/HelloWorldOgre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(ogre/HelloWorldOgreAdvanced.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(ogre/HelloWorldOgre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(ogre/HelloWorldOgreAdvanced.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
foreach(cpp ${example_cpp})
diff --git a/example/manual/geometric-features/manGeometricFeatures.cpp b/example/manual/geometric-features/manGeometricFeatures.cpp
index 5fc5f64..90d154d 100644
--- a/example/manual/geometric-features/manGeometricFeatures.cpp
+++ b/example/manual/geometric-features/manGeometricFeatures.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,24 +49,23 @@
*/
-
#include <visp3/core/vpDebug.h>
#include <visp3/io/vpImageIo.h>
-// For 2D image
+// For 2D image
#include <visp3/core/vpImage.h>
// Video device interface
#include <visp3/core/vpDisplay.h>
#include <visp3/gui/vpDisplayGTK.h>
// For frame transformation and projection
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
// Needed geometric features
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpLine.h>
-#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpSphere.h>
#include <iostream>
@@ -74,39 +74,42 @@ int main()
{
#ifdef VISP_HAVE_GTK
try {
- std::cout << "ViSP geometric features display example" <<std::endl;
+ std::cout << "ViSP geometric features display example" << std::endl;
unsigned int height = 288;
unsigned int width = 384;
- vpImage<unsigned char> I(height,width);
+ vpImage<unsigned char> I(height, width);
I = 255; // I is a white image
// create a display window
vpDisplayGTK display;
// initialize a display attached to image I
- display.init(I,100,100,"ViSP geometric features display");
+ display.init(I, 100, 100, "ViSP geometric features display");
// camera parameters to digitalize the image plane
- vpCameraParameters cam(600,600,width/2,height/2); // px,py,u0,v0
+ vpCameraParameters cam(600, 600, width / 2, height / 2); // px,py,u0,v0
// pose of the camera with reference to the scene
- vpTranslationVector t(0,0,1);
- vpRxyzVector rxyz(-M_PI/4,0,0);
+ vpTranslationVector t(0, 0, 1);
+ vpRxyzVector rxyz(-M_PI / 4, 0, 0);
vpRotationMatrix R(rxyz);
vpHomogeneousMatrix cMo(t, R);
// scene building, geometric features definition
vpPoint point;
- point.setWorldCoordinates(0,0,0);// (X0=0,Y0=0,Z0=0)
+ point.setWorldCoordinates(0, 0, 0); // (X0=0,Y0=0,Z0=0)
vpLine line;
- line.setWorldCoordinates(1,1,0,0,0,0,1,0); // planes:(X+Y=0)&(Z=0)
+ line.setWorldCoordinates(1, 1, 0, 0, 0, 0, 1, 0); // planes:(X+Y=0)&(Z=0)
vpCylinder cylinder;
- cylinder.setWorldCoordinates(1,-1,0,0,0,0,0.1); // alpha=1,beta=-1,gamma=0,
+ cylinder.setWorldCoordinates(1, -1, 0, 0, 0, 0,
+ 0.1); // alpha=1,beta=-1,gamma=0,
// X0=0,Y0=0,Z0=0,R=0.1
vpCircle circle;
- circle.setWorldCoordinates(0,0,1,0,0,0,0.1); // plane:(Z=0),X0=0,Y0=0,Z=0,R=0.1
+ circle.setWorldCoordinates(0, 0, 1, 0, 0, 0,
+ 0.1); // plane:(Z=0),X0=0,Y0=0,Z=0,R=0.1
vpSphere sphere;
- sphere.setWorldCoordinates(0,0,0,0.1); // X0=0,Y0=0,Z0=0,R=0.1
+ sphere.setWorldCoordinates(0, 0, 0, 0.1); // X0=0,Y0=0,Z0=0,R=0.1
- // change frame to be the camera frame and project features in the image plane
+ // change frame to be the camera frame and project features in the image
+ // plane
point.project(cMo);
line.project(cMo);
cylinder.project(cMo);
@@ -116,24 +119,23 @@ int main()
// display the scene
vpDisplay::display(I); // display I
// draw the projections of the 3D geometric features in the image plane.
- point.display(I,cam,vpColor::black); // draw a black cross over I
- line.display(I,cam,vpColor::blue); // draw a blue line over I
- cylinder.display(I,cam,vpColor::red); // draw two red lines over I
- circle.display(I,cam,vpColor::orange); // draw an orange ellipse over I
- sphere.display(I,cam,vpColor::black); // draw a black ellipse over I
+ point.display(I, cam, vpColor::black); // draw a black cross over I
+ line.display(I, cam, vpColor::blue); // draw a blue line over I
+ cylinder.display(I, cam, vpColor::red); // draw two red lines over I
+ circle.display(I, cam, vpColor::orange); // draw an orange ellipse over I
+ sphere.display(I, cam, vpColor::black); // draw a black ellipse over I
- vpDisplay::flush(I); // flush the display buffer
+ vpDisplay::flush(I); // flush the display buffer
std::cout << "A click in the display to exit" << std::endl;
vpDisplay::getClick(I); // wait for a click in the display to exit
// save the drawing
vpImage<vpRGBa> Ic;
- vpDisplay::getImage(I,Ic);
- std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image "<< std::endl;
+ vpDisplay::getImage(I, Ic);
+ std::cout << "ViSP creates \"./geometricFeatures.ppm\" B&W image " << std::endl;
vpImageIo::write(Ic, "./geometricFeatures.ppm");
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/manual/hello-world/Autotools/HelloWorld.cpp b/example/manual/hello-world/Autotools/HelloWorld.cpp
index 09062b9..7cee909 100644
--- a/example/manual/hello-world/Autotools/HelloWorld.cpp
+++ b/example/manual/hello-world/Autotools/HelloWorld.cpp
@@ -4,16 +4,15 @@
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
-
int main()
{
- std::cout << "ViSP Hello World example" <<std::endl;
+ std::cout << "ViSP Hello World example" << std::endl;
vpImage<unsigned char> I(288, 384);
I = 128;
- std::cout << "ViSP creates \"./myimage.pgm\" B&W image " <<std::endl;
+ std::cout << "ViSP creates \"./myimage.pgm\" B&W image " << std::endl;
vpImageIo::write(I, "./myimage.pgm");
return 0;
diff --git a/example/manual/hello-world/Autotools/have_visp.m4 b/example/manual/hello-world/Autotools/have_visp.m4
index 1a2d181..945fbc7 100644
--- a/example/manual/hello-world/Autotools/have_visp.m4
+++ b/example/manual/hello-world/Autotools/have_visp.m4
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/manual/hello-world/CMake/HelloWorld.cpp b/example/manual/hello-world/CMake/HelloWorld.cpp
index 8f3a20c..3a148c3 100644
--- a/example/manual/hello-world/CMake/HelloWorld.cpp
+++ b/example/manual/hello-world/CMake/HelloWorld.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,10 +37,10 @@
*****************************************************************************/
#include <iostream>
+#include <limits>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpThetaUVector.h>
-#include <limits>
int main()
{
@@ -47,7 +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<double>::epsilon(),0.);
+ vpRotationMatrix R(vpMath::rad(0.), vpMath::rad(180) + 100 * std::numeric_limits<double>::epsilon(), 0.);
// Extract the theta U angles from a rotation matrix
tu.buildFrom(R);
@@ -59,8 +60,7 @@ int main()
// Print the transpose row vector
std::cout << tu_t << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/manual/hello-world/Makefile/HelloWorld.cpp b/example/manual/hello-world/Makefile/HelloWorld.cpp
index 09062b9..7cee909 100644
--- a/example/manual/hello-world/Makefile/HelloWorld.cpp
+++ b/example/manual/hello-world/Makefile/HelloWorld.cpp
@@ -4,16 +4,15 @@
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
-
int main()
{
- std::cout << "ViSP Hello World example" <<std::endl;
+ std::cout << "ViSP Hello World example" << std::endl;
vpImage<unsigned char> I(288, 384);
I = 128;
- std::cout << "ViSP creates \"./myimage.pgm\" B&W image " <<std::endl;
+ std::cout << "ViSP creates \"./myimage.pgm\" B&W image " << std::endl;
vpImageIo::write(I, "./myimage.pgm");
return 0;
diff --git a/example/manual/image-manipulation/manDisplay.cpp b/example/manual/image-manipulation/manDisplay.cpp
index e63f83d..3dca565 100644
--- a/example/manual/image-manipulation/manDisplay.cpp
+++ b/example/manual/image-manipulation/manDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,25 +44,26 @@
*/
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpColor.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
try {
// Create a grey level image
- vpImage<vpRGBa> I ;
+ vpImage<vpRGBa> I;
// Create image points for pixel coordinates
vpImagePoint ip, ip1, ip2;
// Load a grey image from the disk. Klimt.ppm image is part of the ViSP
- // image data set available from http://www.irisa.fr/lagadic/visp/download.html
+ // image data set available from
+ // http://www.irisa.fr/lagadic/visp/download.html
std::string filename = "./Klimt.ppm";
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
#ifdef VISP_HAVE_GTK
// Create a display using GTK
@@ -69,84 +71,81 @@ int main()
// For this grey level image, open a GTK display at position 100,100
// in the screen, and with title "GTK display"
- display.init(I, 100, 100, "GTK display") ;
+ display.init(I, 100, 100, "GTK display");
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Display in overlay a red cross at position 100,10 in the
// image. The lines are 20 pixels long
- ip.set_i( 200 );
- ip.set_j( 200 );
- vpDisplay::displayCross(I, ip, 20, vpColor::red, 3) ;
+ ip.set_i(200);
+ ip.set_j(200);
+ vpDisplay::displayCross(I, ip, 20, vpColor::red, 3);
// Display in overlay a horizontal red line
- ip1.set_i( 10 );
- ip1.set_j( 0 );
- ip2.set_i( 10 );
- ip2.set_j( I.getWidth() );
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red, 3) ;
+ ip1.set_i(10);
+ ip1.set_j(0);
+ ip2.set_i(10);
+ ip2.set_j(I.getWidth());
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red, 3);
// Display in overlay a vertical green dot line
- ip1.set_i( 0 );
- ip1.set_j( 20 );
- ip2.set_i( I.getWidth() );
- ip2.set_j( 20 );
- vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green, 3) ;
+ ip1.set_i(0);
+ ip1.set_j(20);
+ ip2.set_i(I.getWidth());
+ ip2.set_j(20);
+ vpDisplay::displayDotLine(I, ip1, ip2, vpColor::green, 3);
// Display in overlay a blue arrow
- ip1.set_i( 0 );
- ip1.set_j( 0 );
- ip2.set_i( 100 );
- ip2.set_j( 100 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue, 8, 4, 3) ;
+ ip1.set_i(0);
+ ip1.set_j(0);
+ ip2.set_i(100);
+ ip2.set_j(100);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::blue, 8, 4, 3);
// Display in overlay some circles. The position of the center is 200, 200
// the radius is increased by 20 pixels for each circle
- for (unsigned i=0 ; i < 5 ; i++) {
- ip.set_i( 200 );
- ip.set_j( 200 );
- vpDisplay::displayCircle(I, ip, 20*i, vpColor::white, false, 3) ;
+ for (unsigned i = 0; i < 5; i++) {
+ ip.set_i(200);
+ ip.set_j(200);
+ vpDisplay::displayCircle(I, ip, 20 * i, vpColor::white, false, 3);
}
// Display in overlay a rectangle.
// The position of the top left corner is 300, 200.
// The width is 200. The height is 100.
- ip.set_i( 280 );
- ip.set_j( 150 );
- vpDisplay::displayRectangle(I, ip, 270, 30,vpColor::purple, false, 3) ;
+ ip.set_i(280);
+ ip.set_j(150);
+ vpDisplay::displayRectangle(I, ip, 270, 30, vpColor::purple, false, 3);
// 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) ;
- //Flush the display : without this line nothing will appear on the screen
+ ip.set_i(300);
+ ip.set_j(160);
+ 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);
// Create a color image
- vpImage<vpRGBa> Ioverlay ;
+ vpImage<vpRGBa> Ioverlay;
// Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
filename = "./Klimt.overlay.ppm";
- vpImageIo::write(Ioverlay, filename) ;
+ vpImageIo::write(Ioverlay, filename);
// If click is allowed, wait for a mouse click to close the display
std::cout << "\nA click to close the windows..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
// Close the display
vpDisplay::close(I);
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
-}
+}
diff --git a/example/manual/image-manipulation/manGrab1394-2.cpp b/example/manual/image-manipulation/manGrab1394-2.cpp
index 84c45d0..fc1a7d7 100644
--- a/example/manual/image-manipulation/manGrab1394-2.cpp
+++ b/example/manual/image-manipulation/manGrab1394-2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,12 +49,10 @@
*/
-
-
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpImage.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <stdlib.h>
-#include <stdio.h>
int main()
{
@@ -62,33 +61,31 @@ int main()
unsigned int ncameras; // Number of cameras on the bus
vp1394TwoGrabber g;
g.getNumCameras(ncameras);
- vpImage<unsigned char> *I = new vpImage<unsigned char> [ncameras];
+ vpImage<unsigned char> *I = new vpImage<unsigned char>[ncameras];
// If the first camera supports vpVIDEO_MODE_640x480_YUV422 video mode
g.setCamera(0);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_YUV422);
// If all cameras support 30 fps acquisition
- for (unsigned int camera=0; camera < ncameras; camera ++) {
+ for (unsigned int camera = 0; camera < ncameras; camera++) {
g.setCamera(camera);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
}
- for ( ; ; ) {
- for (unsigned int camera=0; camera < ncameras; camera ++) {
+ for (;;) {
+ for (unsigned int camera = 0; camera < ncameras; camera++) {
// Acquire successively images from the different cameras
g.setCamera(camera);
g.acquire(I[camera]);
}
}
- delete [] I;
+ delete[] I;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#endif
-
}
diff --git a/example/manual/image-manipulation/manGrabDirectShow.cpp b/example/manual/image-manipulation/manGrabDirectShow.cpp
index bc823ca..41e237b 100644
--- a/example/manual/image-manipulation/manGrabDirectShow.cpp
+++ b/example/manual/image-manipulation/manGrabDirectShow.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -59,8 +60,8 @@ int main()
vpImage<unsigned char> I; // Grey level image
#ifdef VISP_HAVE_DIRECTSHOW
- vpDirectShowGrabber g; // Create the grabber
- if(g.getDeviceNumber() == 0) //test if a camera is connected
+ vpDirectShowGrabber g; // Create the grabber
+ if (g.getDeviceNumber() == 0) // test if a camera is connected
{
g.close();
return -1;
@@ -68,18 +69,16 @@ int main()
g.open(); // Initialize the grabber
- g.setImageSize(640,480); // If the camera supports 640x480 image size
- g.setFramerate(30); // If the camera supports 30fps framerate
+ g.setImageSize(640, 480); // If the camera supports 640x480 image size
+ g.setFramerate(30); // If the camera supports 30fps framerate
- for ( ; ; )
+ for (;;)
g.acquire(I); // Acquire an image
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
-
}
diff --git a/example/manual/image-manipulation/manGrabDisk.cpp b/example/manual/image-manipulation/manGrabDisk.cpp
index 831a5e8..6f10129 100644
--- a/example/manual/image-manipulation/manGrabDisk.cpp
+++ b/example/manual/image-manipulation/manGrabDisk.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -77,19 +78,17 @@ int main()
g.setExtension("pgm");
// Open the framegrabber by loading the first image of the sequence
- g.open(I) ;
+ g.open(I);
// this is the loop over the image sequence
- for(int cpt = 0; cpt < 100; cpt++)
- {
+ for (int cpt = 0; cpt < 100; cpt++) {
// read the image and then increment the image counter so that the next
// call to acquire(I) will get the next image
- g.acquire(I) ;
+ g.acquire(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/manual/image-manipulation/manGrabOpenCV.cpp b/example/manual/image-manipulation/manGrabOpenCV.cpp
index 37357c2..5d8cc68 100644
--- a/example/manual/image-manipulation/manGrabOpenCV.cpp
+++ b/example/manual/image-manipulation/manGrabOpenCV.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,20 +45,21 @@
#include <visp3/core/vpImage.h>
#include <visp3/sensor/vpOpenCVGrabber.h>
-int main(){
+int main()
+{
vpImage<unsigned char> I; // Grey level image
-
+
#ifdef VISP_HAVE_OPENCV
- vpOpenCVGrabber g; // Create the grabber
-
- g.open(); // Initialize the grabber
-
+ vpOpenCVGrabber g; // Create the grabber
+
+ g.open(); // Initialize the grabber
+
g.setWidth(640);
- g.setHeight(480); // If the camera supports 640x480 image size
- g.setFramerate(30); // If the camera supports 30fps framerate
-
- while(1)
- g.acquire(I); // Acquire an image
+ g.setHeight(480); // If the camera supports 640x480 image size
+ g.setFramerate(30); // If the camera supports 30fps framerate
+
+ while (1)
+ g.acquire(I); // Acquire an image
#endif
}
diff --git a/example/manual/image-manipulation/manGrabV4l2.cpp b/example/manual/image-manipulation/manGrabV4l2.cpp
index 6c47c40..6ef9396 100644
--- a/example/manual/image-manipulation/manGrabV4l2.cpp
+++ b/example/manual/image-manipulation/manGrabV4l2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -63,13 +64,12 @@ int main()
g.setHeight(576); // Acquired images are 576 height
g.setNBuffers(3); // 3 ring buffers to ensure real-time acquisition
g.open(I); // Open the grabber
- for ( ; ; )
- g.acquire(I); // Acquire a 768x576 grey image
+ for (;;)
+ g.acquire(I); // Acquire a 768x576 grey image
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/manual/moments/manServoMomentsSimple.cpp b/example/manual/moments/manServoMomentsSimple.cpp
index 51d0647..09594c7 100644
--- a/example/manual/moments/manServoMomentsSimple.cpp
+++ b/example/manual/moments/manServoMomentsSimple.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,142 +38,147 @@
/*!
\example manServoMomentsSimple.cpp
- Minimalist example of moment-based visual servoing with polygon and a simple robot
+ Minimalist example of moment-based visual servoing with polygon and a simple
+ robot
*/
-
#include <visp3/core/vpPoint.h> //the basic tracker
+#include <iostream> //some console output
+#include <limits>
#include <vector> //store the polygon
-#include <visp3/core/vpMomentObject.h> //transmit the polygon to the object
+#include <visp3/core/vpException.h>
#include <visp3/core/vpMomentCommon.h> //update the common database with the object
-#include <visp3/visual_features/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
-#include <visp3/vs/vpServo.h> //visual servoing task
-#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/core/vpMomentObject.h> //transmit the polygon to the object
#include <visp3/core/vpPlane.h>
-#include <visp3/core/vpException.h>
-#include <limits>
-#include <iostream> //some console output
-//this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane form
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h> //init the feature database using the information about moment dependencies
+#include <visp3/vs/vpServo.h> //visual servoing task
+// this function converts the plane defined by the cMo to 1/Z=Ax+By+C plane
+// form
-void cMoToABC(vpHomogeneousMatrix& cMo, double& A,double& B, double& C);
+void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C);
-void cMoToABC(vpHomogeneousMatrix& cMo, double& A,double& B, double& C)
+void cMoToABC(vpHomogeneousMatrix &cMo, double &A, double &B, double &C)
{
- vpPlane pl;
- pl.setABCD(0,0,1.0,0);
+ vpPlane pl;
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
- std::cout << "Invalid position:" << std::endl;
- std::cout << cMo << std::endl;
- std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
- throw vpException(vpException::divideByZeroError,"invalid position!");
- }
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
+ if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
+ std::cout << "Invalid position:" << std::endl;
+ std::cout << cMo << std::endl;
+ std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
+ throw vpException(vpException::divideByZeroError, "invalid position!");
+ }
+ A = -pl.getA() / pl.getD();
+ B = -pl.getB() / pl.getD();
+ C = -pl.getC() / pl.getD();
}
int main()
{
try {
- double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
- double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
- double A,B,C,Ad,Bd,Cd;
+ double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
+ double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
+ double A, B, C, Ad, Bd, Cd;
int nbpoints = 8;
- std::vector<vpPoint> vec_p,vec_p_d; // vectors that contain the vertices of the contour polygon
+ std::vector<vpPoint> 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
- cMoToABC(cMo,A,B,C);
- cMoToABC(cdMo,Ad,Bd,Cd);
+ cMoToABC(cMo, A, B, C);
+ cMoToABC(cdMo, Ad, Bd, Cd);
// Define source and destination polygons
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
p.track(cMo);
vec_p.push_back(p);
- p.track(cdMo) ;
+ p.track(cdMo);
vec_p_d.push_back(p);
}
- 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
+ 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
- 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
+ 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
- //init classic moment primitives (for source)
- vpMomentCommon mdb_cur(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst)); //Init classic features
+ // init classic moment primitives (for source)
+ 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)
+ // update+compute moment primitives from object (for destination)
mdb_dst.updateAll(dst);
- //update+compute features (+interaction matrixes) from plane
- fmdb_dst.updateAll(Ad,Bd,Cd);
+ // update+compute features (+interaction matrixes) from plane
+ fmdb_dst.updateAll(Ad, Bd, Cd);
- //define visual servoing task
+ // define visual servoing task
vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
- task.setLambda(1) ;
+ task.setLambda(1);
- 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(),
+ 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.getFeatureAlpha(),fmdb_dst.getFeatureAlpha());
+ task.addFeature(fmdb_cur.getFeatureAlpha(), fmdb_dst.getFeatureAlpha());
- vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst,0,0,1.);
+ vpBasicFeature *al = new vpFeatureMomentAlpha(mdb_dst, 0, 0, 1.);
al->init();
al->error(*al);
- //param robot
- vpSimulatorCamera robot ;
+ // param robot
+ vpSimulatorCamera robot;
float sampling_time = 0.010f; // Sampling period in seconds
robot.setSamplingTime(sampling_time);
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
- do{
+ do {
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
vec_p.clear();
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cMo);
vec_p.push_back(p);
}
- cMoToABC(cMo,A,B,C);
+ cMoToABC(cMo, A, B, C);
cur.fromVector(vec_p);
- //update+compute moment primitives from object (for source)
+ // update+compute moment primitives from object (for source)
mdb_cur.updateAll(cur);
- //update+compute features (+interaction matrixes) from plane
- fmdb_cur.updateAll(A,B,C);
+ // update+compute features (+interaction matrixes) from plane
+ fmdb_cur.updateAll(A, B, C);
vpColVector v = task.computeControlLaw();
task.print();
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
double t = vpTime::measureTimeMs();
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
- } while(( task.getError() ).sumSquare()>0.005);
- std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
+ } while ((task.getError()).sumSquare() > 0.005);
+ std::cout << "final error=" << (task.getError()).sumSquare() << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/manual/ogre/HelloWorldOgre.cpp b/example/manual/ogre/HelloWorldOgre.cpp
index c950df0..e90a91b 100644
--- a/example/manual/ogre/HelloWorldOgre.cpp
+++ b/example/manual/ogre/HelloWorldOgre.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,18 +44,18 @@
#include <iostream>
-#include <visp3/sensor/vpOpenCVGrabber.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/ar/vpAROgre.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/ar/vpAROgre.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
int main()
{
try {
-#if defined(VISP_HAVE_OGRE)
+#if defined(VISP_HAVE_OGRE)
#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
// Image to stock gathered data
@@ -62,7 +63,7 @@ int main()
// the background texture used in Ogre renderer will be also in color.
vpImage<vpRGBa> I;
- // Now we try to find an available framegrabber
+// Now we try to find an available framegrabber
#if defined(VISP_HAVE_V4L2)
// Video for linux 2 grabber
vpV4l2Grabber grabber;
@@ -76,7 +77,7 @@ int main()
#elif defined(VISP_HAVE_OPENCV)
// OpenCV to gather images
cv::VideoCapture grabber(0); // open the default camera
- if(!grabber.isOpened()) { // check if we succeeded
+ if (!grabber.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -90,7 +91,7 @@ int main()
double py = 565;
double u0 = I.getWidth() / 2;
double v0 = I.getHeight() / 2;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
// The matrix with our pose
// Defines the pose of the object in the camera frame
vpHomogeneousMatrix cMo;
@@ -102,8 +103,8 @@ int main()
// Initialisation
// Here we load the requested plugins specified in the "plugins.cfg" file
// and the resources specified in the "resources.cfg" file
- // These two files can be found respectively in ViSP_HAVE_OGRE_PLUGINS_PATH
- // and ViSP_HAVE_OGRE_RESOURCES_PATH folders
+ // These two files can be found respectively in
+ // ViSP_HAVE_OGRE_PLUGINS_PATH and ViSP_HAVE_OGRE_RESOURCES_PATH folders
ogre.init(I);
// Create a basic scene
@@ -116,15 +117,15 @@ int main()
// Here we use a mesh included in
// the installation files : a robot.
// -----------------------------------
- // Here we load the "robot.mesh" model that is found thanks to the resources locations
- // specified in the "resources.cfg" file
+ // Here we load the "robot.mesh" model that is found thanks to the
+ // resources locations specified in the "resources.cfg" file
ogre.load("Robot", "robot.mesh");
// Modify robot scale and orientation
// - downscale the size to have something completly visible in the image
// - 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
@@ -132,8 +133,8 @@ int main()
std::cout << "cMo:\n" << cMo << std::endl;
// Rendering loop, ended with on escape
- while(ogre.continueRendering()){
- // Acquire a new image
+ while (ogre.continueRendering()) {
+// Acquire a new image
#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394)
grabber.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -144,7 +145,7 @@ int main()
// ...
// cMo updated
// Display the robot at the position specified by cMo with vpAROgre
- ogre.display(I,cMo);
+ ogre.display(I, cMo);
}
#else
std::cout << "You need an available framegrabber to run this example" << std::endl;
@@ -153,12 +154,10 @@ int main()
std::cout << "You need Ogre3D to run this example" << std::endl;
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
diff --git a/example/manual/ogre/HelloWorldOgreAdvanced.cpp b/example/manual/ogre/HelloWorldOgreAdvanced.cpp
index 9591c0d..383963b 100644
--- a/example/manual/ogre/HelloWorldOgreAdvanced.cpp
+++ b/example/manual/ogre/HelloWorldOgreAdvanced.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,13 +44,13 @@
#include <iostream>
-#include <visp3/sensor/vpOpenCVGrabber.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/ar/vpAROgre.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/ar/vpAROgre.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
#if defined(VISP_HAVE_OGRE)
@@ -59,11 +60,11 @@ class vpAROgreAdvanced : public vpAROgre
{
private:
// Animation attribute
- Ogre::AnimationState * mAnimationState;
+ 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;
@@ -73,32 +74,33 @@ protected:
void createScene()
{
// Create the Entity
- Ogre::Entity* robot = mSceneMgr->createEntity("Robot", "robot.mesh");
+ Ogre::Entity *robot = mSceneMgr->createEntity("Robot", "robot.mesh");
// Attach robot to scene graph
- Ogre::SceneNode* RobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot");
- //RobotNode->setPosition((Ogre::Real)-0.3, (Ogre::Real)0.2, (Ogre::Real)0);
+ Ogre::SceneNode *RobotNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Robot");
+ // RobotNode->setPosition((Ogre::Real)-0.3, (Ogre::Real)0.2,
+ // (Ogre::Real)0);
RobotNode->attachObject(robot);
- RobotNode->scale((Ogre::Real)0.001,(Ogre::Real)0.001,(Ogre::Real)0.001);
+ RobotNode->scale((Ogre::Real)0.001, (Ogre::Real)0.001, (Ogre::Real)0.001);
RobotNode->pitch(Ogre::Degree(180));
RobotNode->yaw(Ogre::Degree(-90));
// The animation
// Set the good animation
- mAnimationState = robot->getAnimationState( "Idle" );
+ mAnimationState = robot->getAnimationState("Idle");
// Start over when finished
- mAnimationState->setLoop( true );
+ mAnimationState->setLoop(true);
// Animation enabled
- mAnimationState->setEnabled( true );
+ mAnimationState->setEnabled(true);
}
- bool customframeEnded( const Ogre::FrameEvent& evt)
+ bool customframeEnded(const Ogre::FrameEvent &evt)
{
// Update animation
// To move, we add it the time since last frame
- mAnimationState->addTime( evt.timeSinceLastFrame );
+ mAnimationState->addTime(evt.timeSinceLastFrame);
return true;
}
-};// End of vpAROgreAdvanced class definition
+}; // End of vpAROgreAdvanced class definition
#endif
#endif
@@ -106,7 +108,7 @@ protected:
int main()
{
try {
-#if defined(VISP_HAVE_OGRE)
+#if defined(VISP_HAVE_OGRE)
#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
// Image to store gathered data
@@ -115,7 +117,7 @@ int main()
// level.
vpImage<unsigned char> I;
- // Now we try to find an available framegrabber
+// Now we try to find an available framegrabber
#if defined(VISP_HAVE_V4L2)
// Video for linux 2 grabber
vpV4l2Grabber grabber;
@@ -135,7 +137,7 @@ int main()
#elif defined(VISP_HAVE_OPENCV)
// OpenCV to gather images
cv::VideoCapture grabber(0); // open the default camera
- if(!grabber.isOpened()) { // check if we succeeded
+ if (!grabber.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -149,7 +151,7 @@ int main()
double py = 565;
double u0 = I.getWidth() / 2;
double v0 = I.getHeight() / 2;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
// The matrix with our pose
vpHomogeneousMatrix cMo;
cMo[2][3] = 0.5; // Z = 0.5 meter
@@ -160,8 +162,8 @@ int main()
ogre.init(I);
// Rendering loop
- while(ogre.continueRendering()){
- // Acquire a new image
+ while (ogre.continueRendering()) {
+// Acquire a new image
#if defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_DC1394)
grabber.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -181,12 +183,10 @@ int main()
std::cout << "You need Ogre3D to run this example" << std::endl;
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
diff --git a/example/manual/simulation/manServo4PointsDisplay.cpp b/example/manual/simulation/manServo4PointsDisplay.cpp
old mode 100755
new mode 100644
index df05035..4e7e6a4
--- a/example/manual/simulation/manServo4PointsDisplay.cpp
+++ b/example/manual/simulation/manServo4PointsDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,52 +52,50 @@
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpTime.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/core/vpIoTools.h>
int main()
{
try {
//////////////////////////////////////////
// sets the initial camera location
- vpHomogeneousMatrix cMo(0.3,0.2,3,
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(40)) ;
+ vpHomogeneousMatrix cMo(0.3, 0.2, 3, vpMath::rad(0), vpMath::rad(0), vpMath::rad(40));
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
///////////////////////////////////
// initialize the robot
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
robot.setSamplingTime(0.04); // 40ms
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
+ robot.setPosition(wMc);
- //initialize the camera parameters
- vpCameraParameters cam(800,800,240,180);
+ // initialize the camera parameters
+ vpCameraParameters cam(800, 800, 240, 180);
- //Image definition
- unsigned int height = 360 ;
- unsigned int width = 480 ;
- vpImage<unsigned char> I(height,width);
+ // Image definition
+ unsigned int height = 360;
+ unsigned int width = 480;
+ vpImage<unsigned char> I(height, width);
- //Display initialization
+// Display initialization
#if defined(VISP_HAVE_X11)
vpDisplayX disp;
#elif defined(VISP_HAVE_GTK)
@@ -106,97 +105,96 @@ int main()
#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV disp;
#endif
- disp.init(I,100,100,"Simulation display");
+ disp.init(I, 100, 100, "Simulation display");
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
// sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+ vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
+ // computes the 3D point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].project(cMo_d);
// creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
-
+ vpFeaturePoint pd[4];
+ for (int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
///////////////////////////////////////
// Current visual features initialization
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo) ;
+ // computes the 3D point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].project(cMo);
// creates the associated features
- vpFeaturePoint p[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],point[i]) ;
-
+ vpFeaturePoint p[4];
+ for (int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(p[i], point[i]);
/////////////////////////////////
// Task defintion
- vpServo task ;
+ vpServo task;
// we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
// Print the current information about the task
task.print();
-
////////////////////////////////////////////////
// The control loop
int k = 0;
- while(k++ < 200){
+ while (k++ < 200) {
double t = vpTime::measureTimeMs();
// Display the image background
vpDisplay::display(I);
// Update the current features
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].project(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (int i = 0; i < 4; i++) {
+ point[i].project(cMo);
+ vpFeatureBuilder::create(p[i], point[i]);
}
// Display the task features (current and desired)
- vpServoDisplay::display(task,cam,I);
+ vpServoDisplay::display(task, cam, I);
vpDisplay::flush(I);
// Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// Compute the control law
- vpColVector v = task.computeControlLaw() ;
+ vpColVector v = task.computeControlLaw();
- // Send the computed velocity to the robot and compute the new robot position
+ // Send the computed velocity to the robot and compute the new robot
+ // position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
@@ -205,21 +203,16 @@ int main()
task.print();
// Wait 40 ms
- vpTime::wait(t,40);
+ vpTime::wait(t, 40);
}
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{ vpTRACE("You should install GTK") ;
-
-}
+int main() { vpTRACE("You should install GTK"); }
#endif
diff --git a/example/manual/simulation/manSimu4Dots.cpp b/example/manual/simulation/manSimu4Dots.cpp
old mode 100755
new mode 100644
index c407c75..8f52c5f
--- a/example/manual/simulation/manSimu4Dots.cpp
+++ b/example/manual/simulation/manSimu4Dots.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,99 +39,96 @@
/*!
\file manSimu4Dots.cpp
- \brief Visual servoing experiment on 4 points with a visualization and image generation
- from the camera and from an external view using vpSimulator.
+ \brief Visual servoing experiment on 4 points with a visualization and image
+ generation from the camera and from an external view using vpSimulator.
*/
/*!
\example manSimu4Dots.cpp
- Visual servoing experiment on 4 points with a visualization and image generation
- from the camera and from an external view using vpSimulator.
+ Visual servoing experiment on 4 points with a visualization and image
+ generation from the camera and from an external view using vpSimulator.
*/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-
#if (defined(VISP_HAVE_COIN3D_AND_GUI) && (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)))
-#include <visp3/core/vpImage.h>
+#include <visp3/ar/vpSimulator.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpTime.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/ar/vpSimulator.h>
+#include <visp3/core/vpTime.h>
#if defined(VISP_HAVE_X11)
-# include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayX.h>
#elif defined(VISP_HAVE_GDI)
-# include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGDI.h>
#elif defined(VISP_HAVE_GTK)
-# include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayGTK.h>
#endif
-// You may have strange compiler issues using the simulator based on SoQt
+// You may have strange compiler issues using the simulator based on SoQt
// and the vpDisplayGTK. In that case prefer to use another display like
// vpDisplayX under linux or vpDisplayGDI under Windows
-#include <visp3/core/vpMath.h>
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/blob/vpDot2.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/core/vpIoTools.h>
-static
-void *mainLoop (void *_simu)
+static void *mainLoop(void *_simu)
{
// pointer copy of the vpSimulator instance
- vpSimulator *simu = (vpSimulator *)_simu ;
+ vpSimulator *simu = static_cast<vpSimulator *> _simu;
// Simulation initialization
- simu->initMainApplication() ;
+ simu->initMainApplication();
///////////////////////////////////
// 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
///////////////////////////////////
// Initialize the robot
- vpSimulatorCamera robot ;
- robot.setSamplingTime(0.04); // 40ms
+ vpSimulatorCamera robot;
+ robot.setSamplingTime(0.04); // 40ms
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
+ robot.setPosition(wMc);
// Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
// Initialize the camera parameters
- vpCameraParameters cam ;
+ vpCameraParameters cam;
simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
// sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+ vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
+ // computes the 3D point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].project(cMo_d);
// creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
+ vpFeaturePoint pd[4];
+ for (int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
///////////////////////////////////////
// Current visual features initialization
@@ -138,9 +136,9 @@ void *mainLoop (void *_simu)
unsigned int width = simu->getInternalWidth();
// Create a greyscale image
- vpImage<unsigned char> I(height,width);
+ vpImage<unsigned char> I(height, width);
- //Display initialization
+// Display initialization
#if defined(VISP_HAVE_X11)
vpDisplayX disp;
#elif defined(VISP_HAVE_GDI)
@@ -148,7 +146,7 @@ void *mainLoop (void *_simu)
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK disp;
#endif
- disp.init(I,100,100,"Simulation display");
+ disp.init(I, 100, 100, "Simulation display");
// disp(I);
// Get the current image
vpTime::wait(500); // wait to be sure the image is generated
@@ -161,41 +159,39 @@ void *mainLoop (void *_simu)
// Initialize the four dots tracker
std::cout << "A click in the four dots clockwise. " << std::endl;
vpDot2 dot[4];
- vpFeaturePoint p[4];
- for (int i = 0 ; i < 4 ; i++)
- {
+ vpFeaturePoint p[4];
+ for (int i = 0; i < 4; i++) {
dot[i].setGraphics(true);
// Call for a click
std::cout << "A click in the dot " << i << std::endl;
dot[i].initTracking(I);
- // Create the associated feature
- vpFeatureBuilder::create(p[i],cam,dot[i]);
+ // Create the associated feature
+ vpFeatureBuilder::create(p[i], cam, dot[i]);
// flush the display
vpDisplay::flush(I);
- }
-
+ }
/////////////////////////////////
// Task defintion
- vpServo task ;
+ vpServo task;
// we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
// Print the current information about the task
task.print();
@@ -204,99 +200,92 @@ void *mainLoop (void *_simu)
////////////////////////////////////////////////
// The control loop
int k = 0;
- while(k++ < 200){
+ while (k++ < 200) {
double t = vpTime::measureTimeMs();
-
+
// Get the current internal camera view and display it
simu->getInternalImage(I);
vpDisplay::display(I);
// Track the four dots and update the associated visual features
- for (int i = 0 ; i < 4 ; i++)
- {
- dot[i].track(I) ;
- vpFeatureBuilder::create(p[i],cam,dot[i]) ;
+ for (int i = 0; i < 4; i++) {
+ dot[i].track(I);
+ vpFeatureBuilder::create(p[i], cam, dot[i]);
}
// Display the desired and current visual features
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
vpDisplay::flush(I);
-
+
// Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+
// Compute the control law
- vpColVector v = task.computeControlLaw() ;
-
- // Send the computed velocity to the robot and compute the new robot position
+ vpColVector v = task.computeControlLaw();
+
+ // Send the computed velocity to the robot and compute the new robot
+ // position
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
-
+
// Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
// Wait 40 ms
- vpTime::wait(t,40);
- }
+ vpTime::wait(t, 40);
+ }
// Print information about the task
task.print();
task.kill();
- simu->closeMainApplication() ;
+ simu->closeMainApplication();
- void *a=NULL ;
- return a ;
+ void *a = NULL;
+ return a;
}
-
-int
-main()
+int main()
{
try {
- vpSimulator simu ;
+ vpSimulator simu;
// Internal view initialization : view from the robot camera
- simu.initInternalViewer(480, 360) ;
+ simu.initInternalViewer(480, 360);
// External view initialization : view from an external camera
- simu.initExternalViewer(300, 300) ;
+ simu.initExternalViewer(300, 300);
// Inernal camera paramters initialization
- vpCameraParameters cam(800,800,240,180) ;
- simu.setInternalCameraParameters(cam) ;
+ vpCameraParameters cam(800, 800, 240, 180);
+ simu.setInternalCameraParameters(cam);
- vpTime::wait(500) ;
+ vpTime::wait(500);
// Load the scene
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
std::string ipath = vpIoTools::getViSPImagesDataPath();
std::string filename = "./4points.iv";
// Set the default input path
- if (! ipath.empty())
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
+ 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());
// Run the main loop
- simu.initApplication(&mainLoop) ;
+ simu.initApplication(&mainLoop);
// Run the simulator
- simu.mainLoop() ;
+ simu.mainLoop();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{ vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt") ;
-
-}
+int main() { vpTRACE("You should install Coin3D and SoQT or SoWin or SoXt"); }
#endif
diff --git a/example/manual/simulation/manSimu4Points.cpp b/example/manual/simulation/manSimu4Points.cpp
old mode 100755
new mode 100644
index a58f162..5902091
--- a/example/manual/simulation/manSimu4Points.cpp
+++ b/example/manual/simulation/manSimu4Points.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,108 +52,105 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-
#if (defined(VISP_HAVE_COIN3D_AND_GUI))
-#include <visp3/core/vpImage.h>
+#include <visp3/ar/vpSimulator.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpTime.h>
-#include <visp3/ar/vpSimulator.h>
-
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
-static
-void *mainLoop (void *_simu)
+static void *mainLoop(void *_simu)
{
// pointer copy of the vpSimulator instance
- vpSimulator *simu = (vpSimulator *)_simu ;
+ vpSimulator *simu = static_cast<vpSimulator *>(_simu);
// Simulation initialization
- simu->initMainApplication() ;
+ simu->initMainApplication();
/////////////////////////////////////////
// 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
///////////////////////////////////
// Initialize the robot
- vpSimulatorCamera robot ;
- robot.setSamplingTime(0.04); // 40ms
+ vpSimulatorCamera robot;
+ robot.setSamplingTime(0.04); // 40ms
wMc = wMo * cMo.inverse();
- robot.setPosition(wMc) ;
+ robot.setPosition(wMc);
// Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
// Initialize the camera parameters
- vpCameraParameters cam ;
- simu->getCameraParameters(cam);
+ vpCameraParameters cam;
+ simu->getCameraParameters(cam);
////////////////////////////////////////
// Desired visual features initialization
// sets the points coordinates in the object frame (in meter)
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
// sets the desired camera location
- vpHomogeneousMatrix cMo_d(0,0,1,0,0,0) ;
+ vpHomogeneousMatrix cMo_d(0, 0, 1, 0, 0, 0);
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo_d) ;
+ // computes the 3D point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].project(cMo_d);
// creates the associated features
- vpFeaturePoint pd[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i],point[i]) ;
+ vpFeaturePoint pd[4];
+ for (int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
///////////////////////////////////////
// Current visual features initialization
- // computes the 3D point coordinates in the camera frame and its 2D coordinates
- for (int i = 0 ; i < 4 ; i++)
- point[i].project(cMo) ;
+ // computes the 3D point coordinates in the camera frame and its 2D
+ // coordinates
+ for (int i = 0; i < 4; i++)
+ point[i].project(cMo);
// creates the associated features
- vpFeaturePoint p[4] ;
- for (int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],point[i]) ;
-
+ vpFeaturePoint p[4];
+ for (int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(p[i], point[i]);
/////////////////////////////////
// Task defintion
- vpServo task ;
+ vpServo task;
// we want an eye-in-hand control law ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the gain
- task.setLambda(1.0) ;
+ task.setLambda(1.0);
// Print the current information about the task
task.print();
@@ -160,93 +158,84 @@ void *mainLoop (void *_simu)
////////////////////////////////////////////////
// The control loop
int k = 0;
- while(k++ < 200){
+ while (k++ < 200) {
double t = vpTime::measureTimeMs();
-
-
+
// Update the current features
- for (int i = 0 ; i < 4 ; i++)
- {
- point[i].project(cMo) ;
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (int i = 0; i < 4; i++) {
+ point[i].project(cMo);
+ vpFeatureBuilder::create(p[i], point[i]);
}
-
+
// Update the robot Jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+
// Compute the control law
- vpColVector v = task.computeControlLaw() ;
-
- // Send the computed velocity to the robot and compute the new robot position
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ vpColVector v = task.computeControlLaw();
+
+ // Send the computed velocity to the robot and compute the new robot
+ // position
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
-
+
// Send the robot position to the visualizator
- simu->setCameraPosition(cMo) ;
+ simu->setCameraPosition(cMo);
// Print the current information about the task
task.print();
-
+
// Wait 40 ms
- vpTime::wait(t,40);
- }
+ vpTime::wait(t, 40);
+ }
task.kill();
- simu->closeMainApplication() ;
-
+ simu->closeMainApplication();
- void *a=NULL ;
- return a ;
+ void *a = NULL;
+ return a;
// return (void *);
}
-
-int
-main()
+int main()
{
try {
- vpSimulator simu ;
+ vpSimulator simu;
// Internal view initialization : view from the robot camera
- simu.initInternalViewer(480, 360) ;
+ simu.initInternalViewer(480, 360);
// External view initialization : view from an external camera
- simu.initExternalViewer(300, 300) ;
+ simu.initExternalViewer(300, 300);
// Inernal camera paramters initialization
- vpCameraParameters cam(800,800,240,180) ;
- simu.setInternalCameraParameters(cam) ;
+ vpCameraParameters cam(800, 800, 240, 180);
+ simu.setInternalCameraParameters(cam);
- vpTime::wait(1000) ;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ vpTime::wait(1000);
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
std::string ipath = vpIoTools::getViSPImagesDataPath();
std::string filename = "./4points.iv";
// Set the default input path
- if (! ipath.empty())
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/iv/4points.iv");
+ 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());
// Run the main loop
- simu.initApplication(&mainLoop) ;
+ simu.initApplication(&mainLoop);
// Run the simulator
- simu.mainLoop() ;
+ simu.mainLoop();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpTRACE("You should install Coin3D and/or GTK") ;
-}
+int main() { vpTRACE("You should install Coin3D and/or GTK"); }
#endif
diff --git a/example/math/BSpline.cpp b/example/math/BSpline.cpp
index 9ec963a..31dab07 100644
--- a/example/math/BSpline.cpp
+++ b/example/math/BSpline.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,33 +47,32 @@
Describe a curve thanks to a BSpline.
*/
-
-
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpBSpline.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
+#include <visp3/io/vpImageIo.h>
#ifdef VISP_HAVE_MODULE_GUI
-# include <visp3/gui/vpDisplayGTK.h>
-# include <visp3/gui/vpDisplayGDI.h>
-# include <visp3/gui/vpDisplayOpenCV.h>
-# include <visp3/gui/vpDisplayD3D.h>
-# include <visp3/gui/vpDisplayX.h> // Should be after #include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h> // Should be after #include <visp3/gui/vpDisplayOpenCV.h>
#endif
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
#include <cstdlib>
#include <stdlib.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
-#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9)
+#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"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -109,7 +109,6 @@ OPTIONS: Default\n\
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
-
/*!
Set the program options.
@@ -125,17 +124,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -150,26 +157,23 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
- if (getOptions(argc, argv, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I(540,480);
+ vpImage<unsigned char> I(540, 480);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#ifdef VISP_HAVE_MODULE_GUI
#if defined VISP_HAVE_X11
@@ -186,9 +190,9 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display image") ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ display.init(I, 100, 100, "Display image");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
#endif
@@ -208,91 +212,89 @@ main(int argc, const char ** argv)
std::list<vpImagePoint> controlPoints;
vpImagePoint pt;
- pt.set_ij(50,300);
+ pt.set_ij(50, 300);
controlPoints.push_back(pt);
- pt.set_ij(100,130);
+ pt.set_ij(100, 130);
controlPoints.push_back(pt);
- pt.set_ij(150,400);
+ pt.set_ij(150, 400);
controlPoints.push_back(pt);
- pt.set_ij(200,370);
+ pt.set_ij(200, 370);
controlPoints.push_back(pt);
- pt.set_ij(250,120);
+ pt.set_ij(250, 120);
controlPoints.push_back(pt);
- pt.set_ij(300,250);
+ pt.set_ij(300, 250);
controlPoints.push_back(pt);
- pt.set_ij(350,200);
+ pt.set_ij(350, 200);
controlPoints.push_back(pt);
- pt.set_ij(400,300);
+ pt.set_ij(400, 300);
controlPoints.push_back(pt);
bSpline.set_p(2);
bSpline.set_knots(knots);
bSpline.set_controlPoints(controlPoints);
- std::cout << "The parameters are :" <<std::endl;
- std::cout << "p : " << bSpline.get_p() <<std::endl;
- std::cout << "" <<std::endl;
- std::cout << "The knot vector :" <<std::endl;
+ std::cout << "The parameters are :" << std::endl;
+ std::cout << "p : " << bSpline.get_p() << std::endl;
+ std::cout << "" << std::endl;
+ std::cout << "The knot vector :" << std::endl;
std::list<double> knots_cur;
bSpline.get_knots(knots_cur);
- unsigned int i_display=0;
- for(std::list<double>::const_iterator it=knots_cur.begin(); it!=knots_cur.end(); ++it, ++i_display){
+ unsigned int i_display = 0;
+ for (std::list<double>::const_iterator it = knots_cur.begin(); it != knots_cur.end(); ++it, ++i_display) {
std::cout << i_display << " ---> " << *it << std::endl;
}
- std::cout << "The control points are :" <<std::endl;
+ std::cout << "The control points are :" << std::endl;
std::list<vpImagePoint> controlPoints_cur;
bSpline.get_controlPoints(controlPoints_cur);
- i_display=0;
- for(std::list<vpImagePoint>::const_iterator it=controlPoints_cur.begin(); it!=controlPoints_cur.end(); ++it, ++i_display){
+ i_display = 0;
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints_cur.begin(); it != controlPoints_cur.end();
+ ++it, ++i_display) {
std::cout << i_display << " ---> " << *it << std::endl;
}
- unsigned int i = bSpline.findSpan(5/2.0);
- std::cout << "The knot interval number for the value u = 5/2 is : " << i <<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;
vpBasisFunction *N = NULL;
- N = bSpline.computeBasisFuns(5/2.0);
+ N = bSpline.computeBasisFuns(5 / 2.0);
std::cout << "The nonvanishing basis functions N(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < bSpline.get_p()+1; j++)
+ 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);
+ N2 = bSpline.computeDersBasisFuns(5 / 2.0, 2);
std::cout << "The first derivatives of the basis functions N'(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < bSpline.get_p()+1; j++)
+ 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;
- for (unsigned int j = 0; j < bSpline.get_p()+1; j++)
+ for (unsigned int j = 0; j < bSpline.get_p() + 1; j++)
std::cout << N2[2][j].value << std::endl;
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
double u = 0.0;
- while (u <= 5)
- {
+ while (u <= 5) {
pt = bSpline.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,vpColor::red);
- u+=0.01;
+ vpDisplay::displayCross(I, pt, 4, vpColor::red);
+ u += 0.01;
}
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!= controlPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) {
vpDisplay::displayCross(I, *it, 4, vpColor::green);
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
vpDisplay::getClick(I);
}
- if (N != NULL) delete[] N;
- if (N2 != NULL)
- {
+ if (N != NULL)
+ delete[] N;
+ if (N2 != NULL) {
for (unsigned int j = 0; j <= 2; j++)
delete[] N2[j];
delete[] N2;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -301,12 +303,9 @@ main(int argc, const char ** argv)
#else
int main()
{
- std::cout << "This example requires a video device. "
- << std::endl
- << "You should install X11, GTK, OpenCV, GDI or Direct3D"
- << std::endl
- << "to be able to execute this example."
- << std::endl;
+ std::cout << "This example requires a video device. " << std::endl
+ << "You should install X11, GTK, OpenCV, GDI or Direct3D" << std::endl
+ << "to be able to execute this example." << std::endl;
return 0;
}
#endif
diff --git a/example/math/CMakeLists.txt b/example/math/CMakeLists.txt
index 731f652..2af3cf4 100644
--- a/example/math/CMakeLists.txt
+++ b/example/math/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/math/exponentialMap.cpp b/example/math/exponentialMap.cpp
index 97563e2..7154ec7 100644
--- a/example/math/exponentialMap.cpp
+++ b/example/math/exponentialMap.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,18 +42,15 @@
Test some vpExponentialMap functionalities.
*/
-
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationVector.h>
-#include <visp3/core/vpThetaUVector.h>
-#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpTranslationVector.h>
-
-int
-main()
+int main()
{
try {
vpTranslationVector t;
@@ -71,9 +69,9 @@ main()
vpColVector v(6); // Velocity vector [t, thetaU]^t
- v[0] = t[0]; // t_x
- v[1] = t[1]; // t_y
- v[2] = t[2]; // t_z
+ v[0] = t[0]; // t_x
+ v[1] = t[1]; // t_y
+ v[2] = t[2]; // t_z
v[3] = tu[0]; // ThetaU_x
v[4] = tu[1]; // ThetaU_y
v[5] = tu[2]; // ThetaU_z
@@ -95,8 +93,7 @@ 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
@@ -112,18 +109,15 @@ 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) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/moments/image/CMakeLists.txt b/example/moments/image/CMakeLists.txt
index b22e376..58cb0bc 100644
--- a/example/moments/image/CMakeLists.txt
+++ b/example/moments/image/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/moments/image/servoMomentImage.cpp b/example/moments/image/servoMomentImage.cpp
index 64ff4e7..61bb9e9 100644
--- a/example/moments/image/servoMomentImage.cpp
+++ b/example/moments/image/servoMomentImage.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,89 +44,90 @@
#define PRINT_CONDITION_NUMBER
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMomentCommon.h>
-#include <visp3/visual_features/vpFeatureMomentCommon.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPlane.h>
#include <visp3/core/vpPoseVector.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
int main()
{
- std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
+ std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
+ "not available."
+ << std::endl;
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) && !defined(VISP_HAVE_GTK)
+#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 << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
+ std::cout << "You should install one of the following third-party library: "
+ "X11, OpenCV, GDI, GTK."
+ << std::endl;
}
#else
-//setup robot parameters
+// setup robot parameters
void paramRobot();
-//update moment objects and interface
+// update moment objects and interface
void refreshScene(vpMomentObject &obj);
-//initialize scene in the interface
+// initialize scene in the interface
void initScene();
-//initialize the moment features
+// initialize the moment features
void initFeatures();
-void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
-void execute(unsigned int nbIter); //launch the simulation
+void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo);
+void execute(unsigned int nbIter); // launch the simulation
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
double error();
-void planeToABC(vpPlane& pl, double& A,double& B, double& C);
+void planeToABC(vpPlane &pl, double &A, double &B, double &C);
void paramRobot();
-void init_visp_plot(vpPlot& );
+void init_visp_plot(vpPlot &);
int main()
{
try {
- //intial pose
- 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)));
+ // intial pose
+ 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)));
- //init the simulation
- init(cMo,cdMo);
+ // init the simulation
+ init(cMo, cdMo);
execute(1500);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
-//init the right display
+// init the right display
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_OPENCV
@@ -138,28 +140,30 @@ vpDisplayD3D displayInt;
vpDisplayGTK displayInt;
#endif
-//start and destination positioning matrices
+// start and destination positioning matrices
vpHomogeneousMatrix cMo;
vpHomogeneousMatrix cdMo;
-vpSimulatorCamera robot;//robot used in this simulation
-vpImage<vpRGBa> Iint(480,640, 0);//internal image used for interface display
-vpServo task; //servoing task
-vpCameraParameters cam;//robot camera parameters
-double _error; //current error
-vpImageSimulator imsim;//image simulator used to simulate the perspective-projection camera
-
-//several images used in the simulation
-vpImage<unsigned char> cur_img(480,640, 0);
-vpImage<unsigned char> src_img(480,640, 0);
-vpImage<unsigned char> dst_img(480,640, 0);
-vpImage<vpRGBa> start_img(480,640, 0);
-vpServo::vpServoIteractionMatrixType interaction_type; //current or desired
-//source and destination objects for moment manipulation
+vpSimulatorCamera robot; // robot used in this simulation
+vpImage<vpRGBa> Iint(480, 640, 0); // internal image used for interface
+ // display
+vpServo task; // servoing task
+vpCameraParameters cam; // robot camera parameters
+double _error; // current error
+vpImageSimulator imsim; // image simulator used to simulate the
+ // perspective-projection camera
+
+// several images used in the simulation
+vpImage<unsigned char> cur_img(480, 640, 0);
+vpImage<unsigned char> src_img(480, 640, 0);
+vpImage<unsigned char> dst_img(480, 640, 0);
+vpImage<vpRGBa> start_img(480, 640, 0);
+vpServo::vpServoIteractionMatrixType interaction_type; // current or desired
+// source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
-//moment sets and their corresponding features
+// moment sets and their corresponding features
vpMomentCommon *moments;
vpMomentCommon *momentsDes;
vpFeatureMomentCommon *featureMoments;
@@ -167,10 +171,11 @@ vpFeatureMomentCommon *featureMomentsDes;
using namespace std;
-
-void initScene(){
+void initScene()
+{
vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
+ for (int i = 0; i < 4; i++)
+ X[i].resize(3);
X[0][0] = -0.2;
X[0][1] = -0.1;
X[0][2] = 0;
@@ -186,119 +191,129 @@ void initScene(){
X[3][0] = -0.2;
X[3][1] = 0.1;
X[3][2] = 0;
- //init source and destination images
- vpImage<unsigned char> tmp_img(480,640,255);
- vpImage<vpRGBa> tmp_start_img(480,640,vpRGBa(255,0,0));
+ // init source and destination images
+ vpImage<unsigned char> tmp_img(480, 640, 255);
+ vpImage<vpRGBa> tmp_start_img(480, 640, vpRGBa(255, 0, 0));
vpImageSimulator imsim_start;
- imsim_start.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION) ;
+ imsim_start.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);
imsim_start.init(tmp_start_img, X);
imsim_start.setCameraPosition(cdMo);
- imsim_start.getImage(start_img,cam);
+ imsim_start.getImage(start_img, cam);
- imsim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION) ;
+ imsim.setInterpolationType(vpImageSimulator::BILINEAR_INTERPOLATION);
imsim.init(tmp_img, X);
imsim.setCameraPosition(cMo);
- imsim.getImage(src_img,cam);
+ imsim.getImage(src_img, cam);
src.setType(vpMomentObject::DENSE_FULL_OBJECT);
- src.fromImage(src_img,128,cam);
-
+ src.fromImage(src_img, 128, cam);
dst.setType(vpMomentObject::DENSE_FULL_OBJECT);
imsim.setCameraPosition(cdMo);
- imsim.getImage(dst_img,cam);
- dst.fromImage(dst_img,128,cam);
-
+ imsim.getImage(dst_img, cam);
+ dst.fromImage(dst_img, 128, cam);
}
-void refreshScene(vpMomentObject &obj){
+void refreshScene(vpMomentObject &obj)
+{
cur_img = 0;
imsim.setCameraPosition(cMo);
- imsim.getImage(cur_img,cam);
- obj.fromImage(cur_img,128,cam);
+ imsim.getImage(cur_img, cam);
+ obj.fromImage(cur_img, 128, cam);
}
-void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
+void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo)
{
- cMo = _cMo; //init source matrix
- cdMo = _cdMo; //init destination matrix
+ 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") ;
+ displayInt.init(Iint, 700, 0, "Visual servoing with moments");
- paramRobot(); //set up robot parameters
+ paramRobot(); // set up robot parameters
task.setServo(vpServo::EYEINHAND_CAMERA);
- initScene(); //initialize graphical scene (for interface)
- initFeatures();//initialize moment features
+ initScene(); // initialize graphical scene (for interface)
+ initFeatures(); // initialize moment features
}
-void initFeatures(){
- //A,B,C parameters of source and destination plane
- double A; double B; double C;
- double Ad; double Bd; double Cd;
- //init main object: using moments up to order 5
-
- //Initializing values from regular plane (with ax+by+cz=d convention)
+void initFeatures()
+{
+ // A,B,C parameters of source and destination plane
+ double A;
+ double B;
+ double C;
+ double Ad;
+ double Bd;
+ double Cd;
+ // init main object: using moments up to order 5
+
+ // Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
- planeToABC(pl,Ad,Bd,Cd);
+ planeToABC(pl, Ad, Bd, Cd);
- //extracting initial position (actually we only care about Zdst)
+ // extracting initial position (actually we only care about Zdst)
vpTranslationVector vec;
cdMo.extract(vec);
- ///////////////////////////// initializing moments and features /////////////////////////////////
- //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
- 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);
- //same thing with common features
+ ///////////////////////////// initializing moments and features
+ ////////////////////////////////////
+ // 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),
+ 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);
moments->updateAll(src);
momentsDes->updateAll(dst);
- featureMoments->updateAll(A,B,C);
- featureMomentsDes->updateAll(Ad,Bd,Cd);
+ featureMoments->updateAll(A, B, C);
+ featureMomentsDes->updateAll(Ad, Bd, Cd);
- //setup the interaction type
- task.setInteractionMatrixType(interaction_type) ;
- //////////////////////////////////add useful features to task//////////////////////////////
- task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
- task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
- //the moments are different in case of a symmetric object
- task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
- task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
+ // setup the interaction type
+ task.setInteractionMatrixType(interaction_type);
+ //////////////////////////////////add useful features to
+ /// task//////////////////////////////
+ task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized());
+ task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn());
+ // the moments are different in case of a symmetric object
+ task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(),
+ (1 << 10) | (1 << 11));
+ task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
task.setLambda(1.);
}
-void execute(unsigned int nbIter){
+void execute(unsigned int nbIter)
+{
vpPlot ViSP_plot;
- init_visp_plot(ViSP_plot); // Initialize plot object
+ init_visp_plot(ViSP_plot); // Initialize plot object
- //init main object: using moments up to order 6
+ // init main object: using moments up to order 6
vpMomentObject obj(6);
- //setting object type (disrete, continuous[form polygon])
+ // setting object type (disrete, continuous[form polygon])
obj.setType(vpMomentObject::DENSE_FULL_OBJECT);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
vpDisplay::display(Iint);
vpDisplay::flush(Iint);
- unsigned int iter=0;
- double t=0;
+ unsigned int iter = 0;
vpHomogeneousMatrix wMo; // Set to identity
vpHomogeneousMatrix wMc; // Camera position in the world frame
@@ -312,73 +327,73 @@ void execute(unsigned int nbIter){
vpColVector err_features;
///////////////////SIMULATION LOOP/////////////////////////////
- while(iter++<nbIter ){
+ while (iter++ < nbIter) {
- vpColVector v ;
- t = vpTime::measureTimeMs();
- //get the cMo
+ vpColVector v;
+ double t = vpTime::measureTimeMs();
+ // get the cMo
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- currentpose.buildFrom(cMo); // For plot
- //setup the plane in A,B,C style
+ currentpose.buildFrom(cMo); // For plot
+ // setup the plane in A,B,C style
vpPlane pl;
- double A,B,C;
- pl.setABCD(0,0,1.0,0);
+ double A, B, C;
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- //track points, draw points and add refresh our object
+ // track points, draw points and add refresh our object
refreshScene(obj);
- //this is the most important thing to do: update our moments
+ // this is the most important thing to do: update our moments
moments->updateAll(obj);
- //and update our features. Do it in that order. Features need to use the information computed by moments
- featureMoments->updateAll(A,B,C);
- //some graphics again
+ // and update our features. Do it in that order. Features need to use the
+ // information computed by moments
+ featureMoments->updateAll(A, B, C);
+ // some graphics again
imsim.setCameraPosition(cMo);
Iint = start_img;
- imsim.getImage(Iint,cam);
- vpDisplay::display(Iint) ;
+ imsim.getImage(Iint, cam);
+ vpDisplay::display(Iint);
vpDisplay::flush(Iint);
if (iter == 1)
- vpDisplay::getClick(Iint) ;
- v = task.computeControlLaw() ;
- //pilot robot using position control. The displacement is t*v with t=10ms step
- //robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
+ vpDisplay::getClick(Iint);
+ v = task.computeControlLaw();
+ // pilot robot using position control. The displacement is t*v with t=10ms
+ // step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
err_features = task.error;
- std::cout<<" || s - s* || = "<<task.error.sumSquare()<<std::endl;
+ std::cout << " || s - s* || = " << task.error.sumSquare() << std::endl;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
- ViSP_plot.plot(0,iter, v);
- ViSP_plot.plot(1,iter,currentpose); // Plot the velocities
- ViSP_plot.plot(2, iter,err_features); //cMo as translations and theta_u
+ ViSP_plot.plot(0, iter, v);
+ ViSP_plot.plot(1, iter, currentpose); // Plot the velocities
+ ViSP_plot.plot(2, iter, err_features); // cMo as translations and theta_u
- _error = ( task.getError() ).sumSquare();
+ _error = (task.getError()).sumSquare();
#if defined(PRINT_CONDITION_NUMBER)
/*
- * Condition number of interaction matrix
- */
- vpMatrix Linteraction = task.L;
- vpMatrix tmpry,U;
- vpColVector singularvals;
- Linteraction.svd(singularvals, tmpry);
- double condno = static_cast<double>(singularvals.getMaxValue()/singularvals.getMinValue());
- std::cout<<"Condition Number: "<<condno<<std::endl;
- #endif
-
+ * Condition number of interaction matrix
+ */
+ vpMatrix Linteraction = task.L;
+ vpMatrix tmpry, U;
+ vpColVector singularvals;
+ Linteraction.svd(singularvals, tmpry);
+ double condno = static_cast<double>(singularvals.getMaxValue() / singularvals.getMinValue());
+ std::cout << "Condition Number: " << condno << std::endl;
+#endif
}
task.kill();
vpTRACE("\n\nClick in the internal view window to end...");
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
delete moments;
delete momentsDes;
@@ -386,72 +401,66 @@ void execute(unsigned int nbIter){
delete featureMomentsDes;
}
-void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
-double error(){return _error;}
-
-
+void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
+double error() { return _error; }
-void planeToABC(vpPlane& pl, double& A,double& B, double& C){
- if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
- std::cout << "Invalid position:" << std::endl;
- std::cout << cMo << std::endl;
- std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
- throw vpException(vpException::divideByZeroError,"invalid position!");
- }
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
+void planeToABC(vpPlane &pl, double &A, double &B, double &C)
+{
+ if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
+ std::cout << "Invalid position:" << std::endl;
+ std::cout << cMo << std::endl;
+ std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
+ throw vpException(vpException::divideByZeroError, "invalid position!");
+ }
+ A = -pl.getA() / pl.getD();
+ B = -pl.getB() / pl.getD();
+ C = -pl.getC() / pl.getD();
}
-void paramRobot(){
- cam = vpCameraParameters(640,480,320,240);
-}
+void paramRobot() { cam = vpCameraParameters(640, 480, 320, 240); }
+
+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
+
+ 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};
+
+ for (unsigned int p = 0; p < NbGraphs; p++) {
+ ViSP_plot.initGraph(p, NbCurves_in_graph[p]);
+ for (unsigned int c = 0; c < NbCurves_in_graph[p]; c++)
+ ViSP_plot.setColor(p, c, Colors[c]);
+ }
-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
-
- 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
- };
-
- for (unsigned int p = 0; p<NbGraphs; p++) {
- ViSP_plot.initGraph(p,NbCurves_in_graph[p]);
- for (unsigned int c = 0; c<NbCurves_in_graph[p]; c++)
- ViSP_plot.setColor(p,c,Colors[c]);
- }
-
- ViSP_plot.setTitle(0,"Robot velocities");
- ViSP_plot.setLegend(0, 0, "v_x");
- ViSP_plot.setLegend(0, 1, "v_y");
- ViSP_plot.setLegend(0, 2, "v_z");
- ViSP_plot.setLegend(0, 3, "w_x");
- ViSP_plot.setLegend(0, 4, "w_y");
- ViSP_plot.setLegend(0, 5, "w_z");
-
- ViSP_plot.setTitle(1,"Camera pose cMo");
- ViSP_plot.setLegend(1, 0, "tx");
- ViSP_plot.setLegend(1, 1, "ty");
- ViSP_plot.setLegend(1, 2, "tz");
- ViSP_plot.setLegend(1, 3, "tu_x");
- ViSP_plot.setLegend(1, 4, "tu_y");
- ViSP_plot.setLegend(1, 5, "tu_z");
-
- ViSP_plot.setTitle(2,"Error in visual features: ");
- ViSP_plot.setLegend(2, 0, "x_n");
- ViSP_plot.setLegend(2, 1, "y_n");
- ViSP_plot.setLegend(2, 2, "a_n");
- ViSP_plot.setLegend(2, 3, "sx");
- ViSP_plot.setLegend(2, 4, "sy");
- ViSP_plot.setLegend(2, 5, "alpha");
+ ViSP_plot.setTitle(0, "Robot velocities");
+ ViSP_plot.setLegend(0, 0, "v_x");
+ ViSP_plot.setLegend(0, 1, "v_y");
+ ViSP_plot.setLegend(0, 2, "v_z");
+ ViSP_plot.setLegend(0, 3, "w_x");
+ ViSP_plot.setLegend(0, 4, "w_y");
+ ViSP_plot.setLegend(0, 5, "w_z");
+
+ ViSP_plot.setTitle(1, "Camera pose cMo");
+ ViSP_plot.setLegend(1, 0, "tx");
+ ViSP_plot.setLegend(1, 1, "ty");
+ ViSP_plot.setLegend(1, 2, "tz");
+ ViSP_plot.setLegend(1, 3, "tu_x");
+ ViSP_plot.setLegend(1, 4, "tu_y");
+ ViSP_plot.setLegend(1, 5, "tu_z");
+
+ ViSP_plot.setTitle(2, "Error in visual features: ");
+ ViSP_plot.setLegend(2, 0, "x_n");
+ ViSP_plot.setLegend(2, 1, "y_n");
+ ViSP_plot.setLegend(2, 2, "a_n");
+ ViSP_plot.setLegend(2, 3, "sx");
+ ViSP_plot.setLegend(2, 4, "sy");
+ ViSP_plot.setLegend(2, 5, "alpha");
}
#endif
diff --git a/example/moments/points/CMakeLists.txt b/example/moments/points/CMakeLists.txt
index ca31505..5f91eb5 100644
--- a/example/moments/points/CMakeLists.txt
+++ b/example/moments/points/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/moments/points/servoMomentPoints.cpp b/example/moments/points/servoMomentPoints.cpp
index 934002a..cb72b8f 100644
--- a/example/moments/points/servoMomentPoints.cpp
+++ b/example/moments/points/servoMomentPoints.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,82 +42,83 @@
Example of moment-based visual servoing with Images
*/
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMomentCommon.h>
-#include <visp3/visual_features/vpFeatureMomentCommon.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpPlane.h>
-
+#include <visp3/vs/vpServo.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
int main()
{
- std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
+ std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
+ "not available."
+ << std::endl;
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) && !defined(VISP_HAVE_GTK)
+#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 << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
+ std::cout << "You should install one of the following third-party library: "
+ "X11, OpenCV, GDI, GTK."
+ << std::endl;
}
#else
-//setup robot parameters
+// setup robot parameters
void paramRobot();
-//update moment objects and interface
+// update moment objects and interface
void refreshScene(vpMomentObject &obj);
-//initialize scene in the interface
+// initialize scene in the interface
void initScene();
-//initialize the moment features
+// initialize the moment features
void initFeatures();
-void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
-void execute(unsigned int nbIter); //launch the simulation
+void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo);
+void execute(unsigned int nbIter); // launch the simulation
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
double error();
-void planeToABC(vpPlane& pl, double& A,double& B, double& C);
+void planeToABC(vpPlane &pl, double &A, double &B, double &C);
void paramRobot();
-void removeJointLimits(vpSimulatorAfma6& robot);
+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));
- //Desired pose
- vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)));
+ try { // intial pose
+ 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)));
- //init and run the simulation
- init(cMo,cdMo);
+ // init and run the simulation
+ init(cMo, cdMo);
execute(1500);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-//init the right display
+// init the right display
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_OPENCV
@@ -129,186 +131,199 @@ vpDisplayD3D displayInt;
vpDisplayGTK displayInt;
#endif
-//start and destination positioning matrices
+// start and destination positioning matrices
vpHomogeneousMatrix cMo;
vpHomogeneousMatrix cdMo;
-vpSimulatorAfma6 robot(false);//robot used in this simulation
-vpImage<vpRGBa> Iint(480,640, 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
-
-//moment sets and their corresponding features
+vpSimulatorAfma6 robot(false); // robot used in this simulation
+vpImage<vpRGBa> Iint(480, 640,
+ 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
+
+// moment sets and their corresponding features
vpMomentCommon *moments;
vpMomentCommon *momentsDes;
vpFeatureMomentCommon *featureMoments;
vpFeatureMomentCommon *featureMomentsDes;
-//source and destination objects for moment manipulation
+// source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
-
-void initScene(){
+void initScene()
+{
std::vector<vpPoint> src_pts;
std::vector<vpPoint> dst_pts;
- double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
- double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
+ double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
+ double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i]/20,y[i]/20,0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i] / 20, y[i] / 20, 0.0);
+ p.track(cMo);
src_pts.push_back(p);
}
src.setType(vpMomentObject::DISCRETE);
src.fromVector(src_pts);
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i]/20,y[i]/20,0.0);
- p.track(cdMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i] / 20, y[i] / 20, 0.0);
+ p.track(cdMo);
dst_pts.push_back(p);
}
dst.setType(vpMomentObject::DISCRETE);
dst.fromVector(dst_pts);
-
}
-void initFeatures(){
- //A,B,C parameters of source and destination plane
- double A; double B; double C;
- double Ad; double Bd; double Cd;
- //init main object: using moments up to order 6
-
- //Initializing values from regular plane (with ax+by+cz=d convention)
+void initFeatures()
+{
+ // A,B,C parameters of source and destination plane
+ double A;
+ double B;
+ double C;
+ double Ad;
+ double Bd;
+ double Cd;
+ // init main object: using moments up to order 6
+
+ // Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
- planeToABC(pl,Ad,Bd,Cd);
+ planeToABC(pl, Ad, Bd, Cd);
- //extracting initial position (actually we only care about Zdst)
+ // extracting initial position (actually we only care about Zdst)
vpTranslationVector vec;
cdMo.extract(vec);
- ///////////////////////////// initializing moments and features /////////////////////////////////
- //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
- moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
- momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
- //same thing with common features
+ ///////////////////////////// initializing moments and features
+ ////////////////////////////////////
+ // 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),
+ vpMomentCommon::getAlpha(dst), vec[2]);
+ momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst),
+ vpMomentCommon::getAlpha(dst), vec[2]);
+ // same thing with common features
featureMoments = new vpFeatureMomentCommon(*moments);
featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
moments->updateAll(src);
momentsDes->updateAll(dst);
- featureMoments->updateAll(A,B,C);
- featureMomentsDes->updateAll(Ad,Bd,Cd);
+ featureMoments->updateAll(A, B, C);
+ featureMomentsDes->updateAll(Ad, Bd, Cd);
- //setup the interaction type
- task.setInteractionMatrixType(interaction_type) ;
- //////////////////////////////////add useful features to task//////////////////////////////
- task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
- task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
- task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 3) | (1 << 5));
- task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
+ // 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(),
+ (1 << 3) | (1 << 5));
+ task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
- task.setLambda(1.) ;
+ task.setLambda(1.);
}
-
-void refreshScene(vpMomentObject &obj){
- //double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
- //double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
- double x[8] = { 1,3, 4,-1 ,-3,-2,-1,1};
- double y[8] = { 0,1, 4, 4, -2,-2, 1,0};
+void refreshScene(vpMomentObject &obj)
+{
+ // double x[8] = { 0.05,0.15, 0.2,-0.05 ,-0.15,-0.1,-0.05,0.05};
+ // double y[8] = { 0,0.05, 0.2, 0.2, -0.1,-0.1, 0.05,0};
+ double x[8] = {1, 3, 4, -1, -3, -2, -1, 1};
+ double y[8] = {0, 1, 4, 4, -2, -2, 1, 0};
int nbpoints = 8;
std::vector<vpPoint> cur_pts;
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i]/20,y[i]/20,0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i] / 20, y[i] / 20, 0.0);
+ p.track(cMo);
cur_pts.push_back(p);
}
obj.fromVector(cur_pts);
}
-void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
+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") ;
+ displayInt.init(Iint, 700, 0, "Visual servoing with moments");
- paramRobot(); //set up robot parameters
+ paramRobot(); // set up robot parameters
task.setServo(vpServo::EYEINHAND_CAMERA);
- initScene(); //initialize graphical scene (for interface)
- initFeatures();//initialize moment features
+ initScene(); // initialize graphical scene (for interface)
+ initFeatures(); // initialize moment features
}
-void execute(unsigned int nbIter){
- //init main object: using moments up to order 6
+void execute(unsigned int nbIter)
+{
+ // init main object: using moments up to order 6
vpMomentObject obj(6);
- //setting object type (disrete, continuous[form polygon])
+ // setting object type (disrete, continuous[form polygon])
obj.setType(vpMomentObject::DISCRETE);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
- unsigned int iter=0;
+ unsigned int iter = 0;
///////////////////SIMULATION LOOP/////////////////////////////
- while(iter++<nbIter ){
- vpColVector v ;
- //get the cMo
+ while (iter++ < nbIter) {
+ vpColVector v;
+ // get the cMo
cMo = robot.get_cMo();
- //setup the plane in A,B,C style
+ // setup the plane in A,B,C style
vpPlane pl;
- double A,B,C;
- pl.setABCD(0,0,1.0,0);
+ double A, B, C;
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- //track points, draw points and add refresh our object
+ // track points, draw points and add refresh our object
refreshScene(obj);
- //this is the most important thing to do: update our moments
+ // this is the most important thing to do: update our moments
moments->updateAll(obj);
- //and update our features. Do it in that order. Features need to use the information computed by moments
- featureMoments->updateAll(A,B,C);
+ // and update our features. Do it in that order. Features need to use the
+ // information computed by moments
+ featureMoments->updateAll(A, B, C);
- vpDisplay::display(Iint) ;
+ vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
if (iter == 1)
- vpDisplay::getClick(Iint) ;
- v = task.computeControlLaw() ;
-
- //pilot robot using position control. The displacement is t*v with t=10ms step
- //robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
- robot.setVelocity(vpRobot::CAMERA_FRAME,v);
+ vpDisplay::getClick(Iint);
+ v = task.computeControlLaw();
+ // pilot robot using position control. The displacement is t*v with t=10ms
+ // step robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- _error = ( task.getError() ).sumSquare();
+ _error = (task.getError()).sumSquare();
}
task.kill();
vpTRACE("\n\nClick in the internal view window to end...");
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
delete moments;
delete momentsDes;
@@ -316,7 +331,7 @@ void execute(unsigned int nbIter){
delete featureMomentsDes;
}
-void removeJointLimits(vpSimulatorAfma6& robot_)
+void removeJointLimits(vpSimulatorAfma6 &robot_)
{
vpColVector limMin(6);
vpColVector limMax(6);
@@ -334,41 +349,44 @@ void removeJointLimits(vpSimulatorAfma6& robot_)
limMax[4] = vpMath::rad(3600);
limMax[5] = vpMath::rad(3600);
- robot_.setJointLimit(limMin,limMax);
+ robot_.setJointLimit(limMin, limMax);
robot_.setMaxRotationVelocity(99999);
robot_.setMaxTranslationVelocity(999999);
}
-void planeToABC(vpPlane& pl, double& A,double& B, double& C){
- if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
- std::cout << "Invalid position:" << std::endl;
- std::cout << cMo << std::endl;
- std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
- throw vpException(vpException::divideByZeroError,"invalid position!");
- }
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
+void planeToABC(vpPlane &pl, double &A, double &B, double &C)
+{
+ if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
+ std::cout << "Invalid position:" << std::endl;
+ std::cout << cMo << std::endl;
+ std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
+ throw vpException(vpException::divideByZeroError, "invalid position!");
+ }
+ A = -pl.getA() / pl.getD();
+ B = -pl.getB() / pl.getD();
+ C = -pl.getC() / pl.getD();
}
-void paramRobot(){
+void paramRobot()
+{
/*Initialise the robot and especially the camera*/
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
- robot.setCurrentViewColor(vpColor(150,150,150));
- robot.setDesiredViewColor(vpColor(200,200,200));
+ 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.setConstantSamplingTimeMode(true);
- /*Initialise the position of the object relative to the pose of the robot's camera*/
+ /*Initialise the position of the object relative to the pose of the robot's
+ * camera*/
robot.initialiseObjectRelativeToCamera(cMo);
/*Set the desired position (for the displaypart)*/
robot.setDesiredCameraPosition(cdMo);
- robot.getCameraParameters(cam,Iint);
+ robot.getCameraParameters(cam, Iint);
}
-void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
-double error(){return _error;}
+void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
+double error() { return _error; }
#endif
diff --git a/example/moments/polygon/CMakeLists.txt b/example/moments/polygon/CMakeLists.txt
index 65e78ef..fc7ef7d 100644
--- a/example/moments/polygon/CMakeLists.txt
+++ b/example/moments/polygon/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/moments/polygon/servoMomentPolygon.cpp b/example/moments/polygon/servoMomentPolygon.cpp
index 59c00d0..0ffae75 100644
--- a/example/moments/polygon/servoMomentPolygon.cpp
+++ b/example/moments/polygon/servoMomentPolygon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,81 +41,83 @@
Example of moment-based visual servoing with Images
*/
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
#include <iostream>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMomentCommon.h>
-#include <visp3/visual_features/vpFeatureMomentCommon.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpPlane.h>
+#include <visp3/vs/vpServo.h>
#if !defined(_WIN32) && !defined(VISP_HAVE_PTHREAD)
// Robot simulator used in this example is not available
int main()
{
- std::cout << "Can't run this example since vpSimulatorAfma6 capability is not available." << std::endl;
+ std::cout << "Can't run this example since vpSimulatorAfma6 capability is "
+ "not available."
+ << std::endl;
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) && !defined(VISP_HAVE_GTK)
+#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 << "You should install one of the following third-party library: X11, OpenCV, GDI, GTK." << std::endl;
+ std::cout << "You should install one of the following third-party library: "
+ "X11, OpenCV, GDI, GTK."
+ << std::endl;
}
#else
-//setup robot parameters
+// setup robot parameters
void paramRobot();
-//update moment objects and interface
+// update moment objects and interface
void refreshScene(vpMomentObject &obj);
-//initialize scene in the interface
+// initialize scene in the interface
void initScene();
-//initialize the moment features
+// initialize the moment features
void initFeatures();
-void init(vpHomogeneousMatrix& cMo, vpHomogeneousMatrix& cdMo);
-void execute(unsigned int nbIter); //launch the simulation
+void init(vpHomogeneousMatrix &cMo, vpHomogeneousMatrix &cdMo);
+void execute(unsigned int nbIter); // launch the simulation
void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type);
double error();
-void planeToABC(vpPlane& pl, double& A,double& B, double& C);
+void planeToABC(vpPlane &pl, double &A, double &B, double &C);
void paramRobot();
-void removeJointLimits(vpSimulatorAfma6& robot);
+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));
- //Desired pose
- vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
+ try { // intial pose
+ 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)));
- //init and run the simulation
- init(cMo,cdMo);
+ // init and run the simulation
+ init(cMo, cdMo);
execute(1500);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-//init the right display
+// init the right display
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
#elif defined VISP_HAVE_OPENCV
@@ -127,22 +130,24 @@ vpDisplayD3D displayInt;
vpDisplayGTK displayInt;
#endif
-//start and destination positioning matrices
+// start and destination positioning matrices
vpHomogeneousMatrix cMo;
vpHomogeneousMatrix cdMo;
-vpSimulatorAfma6 robot(false);//robot used in this simulation
-vpImage<vpRGBa> Iint(480,640, 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
-
-//source and destination objects for moment manipulation
+vpSimulatorAfma6 robot(false); // robot used in this simulation
+vpImage<vpRGBa> Iint(480, 640,
+ 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
+
+// source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
-//moment sets and their corresponding features
+// moment sets and their corresponding features
vpMomentCommon *moments;
vpMomentCommon *momentsDes;
vpFeatureMomentCommon *featureMoments;
@@ -150,160 +155,174 @@ vpFeatureMomentCommon *featureMomentsDes;
using namespace std;
-
-void initScene(){
+void initScene()
+{
vector<vpPoint> src_pts;
vector<vpPoint> dst_pts;
- double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
- double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
+ double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
+ double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
int nbpoints = 4;
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cMo);
src_pts.push_back(p);
}
src.setType(vpMomentObject::DENSE_POLYGON);
src.fromVector(src_pts);
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cdMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cdMo);
dst_pts.push_back(p);
}
dst.setType(vpMomentObject::DENSE_POLYGON);
dst.fromVector(dst_pts);
-
}
-void refreshScene(vpMomentObject &obj){
- double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
- double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
+void refreshScene(vpMomentObject &obj)
+{
+ double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
+ double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
int nbpoints = 5;
vector<vpPoint> cur_pts;
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cMo);
cur_pts.push_back(p);
}
obj.fromVector(cur_pts);
}
-void init(vpHomogeneousMatrix& _cMo, vpHomogeneousMatrix& _cdMo)
+void init(vpHomogeneousMatrix &_cMo, vpHomogeneousMatrix &_cdMo)
{
cMo = _cMo;
cdMo = _cdMo;
interaction_type = vpServo::CURRENT;
- displayInt.init(Iint,700,0, "Visual servoing with moments") ;
+ displayInt.init(Iint, 700, 0, "Visual servoing with moments");
- paramRobot(); //set up robot parameters
+ paramRobot(); // set up robot parameters
task.setServo(vpServo::EYEINHAND_CAMERA);
- initScene(); //initialize graphical scene (for interface)
- initFeatures();//initialize moment features
+ initScene(); // initialize graphical scene (for interface)
+ initFeatures(); // initialize moment features
}
-void initFeatures(){
- //A,B,C parameters of source and destination plane
- double A; double B; double C;
- double Ad; double Bd; double Cd;
- //init main object: using moments up to order 6
-
- //Initializing values from regular plane (with ax+by+cz=d convention)
+void initFeatures()
+{
+ // A,B,C parameters of source and destination plane
+ double A;
+ double B;
+ double C;
+ double Ad;
+ double Bd;
+ double Cd;
+ // init main object: using moments up to order 6
+
+ // Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
- planeToABC(pl,Ad,Bd,Cd);
+ planeToABC(pl, Ad, Bd, Cd);
- //extracting initial position (actually we only care about Zdst)
+ // extracting initial position (actually we only care about Zdst)
vpTranslationVector vec;
cdMo.extract(vec);
- ///////////////////////////// initializing moments and features /////////////////////////////////
- //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
- moments = new vpMomentCommon(vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
- momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
- //same thing with common features
+ ///////////////////////////// initializing moments and features
+ ////////////////////////////////////
+ // 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),
+ vpMomentCommon::getAlpha(dst), vec[2]);
+ momentsDes = new vpMomentCommon(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst),
+ vpMomentCommon::getAlpha(dst), vec[2]);
+ // same thing with common features
featureMoments = new vpFeatureMomentCommon(*moments);
featureMomentsDes = new vpFeatureMomentCommon(*momentsDes);
moments->updateAll(src);
momentsDes->updateAll(dst);
- featureMoments->updateAll(A,B,C);
- featureMomentsDes->updateAll(Ad,Bd,Cd);
-
- //setup the interaction type
- task.setInteractionMatrixType(interaction_type) ;
- //////////////////////////////////add useful features to task//////////////////////////////
- task.addFeature(featureMoments->getFeatureGravityNormalized(),featureMomentsDes->getFeatureGravityNormalized());
- task.addFeature(featureMoments->getFeatureAn(),featureMomentsDes->getFeatureAn());
- //the moments are different in case of a symmetric object
- task.addFeature(featureMoments->getFeatureCInvariant(),featureMomentsDes->getFeatureCInvariant(),(1 << 10) | (1 << 11));
- task.addFeature(featureMoments->getFeatureAlpha(),featureMomentsDes->getFeatureAlpha());
-
- task.setLambda(0.4) ;
+ featureMoments->updateAll(A, B, C);
+ featureMomentsDes->updateAll(Ad, Bd, Cd);
+
+ // setup the interaction type
+ task.setInteractionMatrixType(interaction_type);
+ //////////////////////////////////add useful features to
+ /// task//////////////////////////////
+ task.addFeature(featureMoments->getFeatureGravityNormalized(), featureMomentsDes->getFeatureGravityNormalized());
+ task.addFeature(featureMoments->getFeatureAn(), featureMomentsDes->getFeatureAn());
+ // the moments are different in case of a symmetric object
+ task.addFeature(featureMoments->getFeatureCInvariant(), featureMomentsDes->getFeatureCInvariant(),
+ (1 << 10) | (1 << 11));
+ task.addFeature(featureMoments->getFeatureAlpha(), featureMomentsDes->getFeatureAlpha());
+
+ task.setLambda(0.4);
}
-void execute(unsigned int nbIter){
- //init main object: using moments up to order 5
+void execute(unsigned int nbIter)
+{
+ // init main object: using moments up to order 5
vpMomentObject obj(6);
- //setting object type (disrete, continuous[form polygon])
+ // setting object type (disrete, continuous[form polygon])
obj.setType(vpMomentObject::DENSE_POLYGON);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
- unsigned int iter=0;
- double t=0;
+ unsigned int iter = 0;
+
///////////////////SIMULATION LOOP/////////////////////////////
- while(iter++<nbIter ){
- vpColVector v ;
- t = vpTime::measureTimeMs();
- //get the cMo
+ while (iter++ < nbIter) {
+ vpColVector v;
+ double t = vpTime::measureTimeMs();
+ // get the cMo
cMo = robot.get_cMo();
- //setup the plane in A,B,C style
+ // setup the plane in A,B,C style
vpPlane pl;
- double A,B,C;
- pl.setABCD(0,0,1.0,0);
+ double A, B, C;
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- //track points, draw points and add refresh our object
+ // track points, draw points and add refresh our object
refreshScene(obj);
- //this is the most important thing to do: update our moments
+ // this is the most important thing to do: update our moments
moments->updateAll(obj);
- //and update our features. Do it in that order. Features need to use the information computed by moments
- featureMoments->updateAll(A,B,C);
+ // and update our features. Do it in that order. Features need to use the
+ // information computed by moments
+ featureMoments->updateAll(A, B, C);
- vpDisplay::display(Iint) ;
+ vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
if (iter == 1)
- vpDisplay::getClick(Iint) ;
- v = task.computeControlLaw() ;
+ vpDisplay::getClick(Iint);
+ v = task.computeControlLaw();
- //pilot robot using position control. The displacement is t*v with t=10ms step
- robot.setPosition(vpRobot::CAMERA_FRAME,0.01*v);
+ // pilot robot using position control. The displacement is t*v with t=10ms
+ // step
+ robot.setPosition(vpRobot::CAMERA_FRAME, 0.01 * v);
- vpTime::wait(t,10);
- _error = ( task.getError() ).sumSquare();
+ vpTime::wait(t, 10);
+ _error = (task.getError()).sumSquare();
}
task.kill();
vpTRACE("\n\nClick in the internal view window to end...");
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
delete moments;
delete momentsDes;
@@ -311,10 +330,10 @@ void execute(unsigned int nbIter){
delete featureMomentsDes;
}
-void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type){interaction_type=type;}
-double error(){return _error;}
+void setInteractionMatrixType(vpServo::vpServoIteractionMatrixType type) { interaction_type = type; }
+double error() { return _error; }
-void removeJointLimits(vpSimulatorAfma6& robot_)
+void removeJointLimits(vpSimulatorAfma6 &robot_)
{
vpColVector limMin(6);
vpColVector limMax(6);
@@ -332,35 +351,38 @@ void removeJointLimits(vpSimulatorAfma6& robot_)
limMax[4] = vpMath::rad(3600);
limMax[5] = vpMath::rad(3600);
- robot_.setJointLimit(limMin,limMax);
+ robot_.setJointLimit(limMin, limMax);
}
-void planeToABC(vpPlane& pl, double& A,double& B, double& C){
- if(fabs(pl.getD())<std::numeric_limits<double>::epsilon()){
- std::cout << "Invalid position:" << std::endl;
- std::cout << cMo << std::endl;
- std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
- throw vpException(vpException::divideByZeroError,"invalid position!");
- }
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
+void planeToABC(vpPlane &pl, double &A, double &B, double &C)
+{
+ if (fabs(pl.getD()) < std::numeric_limits<double>::epsilon()) {
+ std::cout << "Invalid position:" << std::endl;
+ std::cout << cMo << std::endl;
+ std::cout << "Cannot put plane in the form 1/Z=Ax+By+C." << std::endl;
+ throw vpException(vpException::divideByZeroError, "invalid position!");
+ }
+ A = -pl.getA() / pl.getD();
+ B = -pl.getB() / pl.getD();
+ C = -pl.getC() / pl.getD();
}
-void paramRobot(){
+void paramRobot()
+{
/*Initialise the robot and especially the camera*/
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
- robot.setCurrentViewColor(vpColor(150,150,150));
- robot.setDesiredViewColor(vpColor(200,200,200));
+ 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);
- /*Initialise the position of the object relative to the pose of the robot's camera*/
+ /*Initialise the position of the object relative to the pose of the robot's
+ * camera*/
robot.initialiseObjectRelativeToCamera(cMo);
/*Set the desired position (for the displaypart)*/
robot.setDesiredCameraPosition(cdMo);
- robot.getCameraParameters(cam,Iint);
+ robot.getCameraParameters(cam, Iint);
}
#endif
diff --git a/example/ogre-simulator/AROgre.cpp b/example/ogre-simulator/AROgre.cpp
index 02c8eb3..5fbd7f2 100644
--- a/example/ogre-simulator/AROgre.cpp
+++ b/example/ogre-simulator/AROgre.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,39 +42,41 @@
Example of augmented reality based on Ogre3D.
*/
-
-
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
-//#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)))
-#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_OGRE) && (defined(VISP_HAVE_OPENCV) ||
+// 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) || \
+ (defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))))
//#if defined(VISP_HAVE_X11) && ! defined(APPLE)
-#if defined(VISP_HAVE_X11) && ! (defined(__APPLE__) && defined(__MACH__))
+#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
// produce an error on OSX: ‘typedef int Cursor’
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
// used on APPLE platforms
-# include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpImagePoint.h>
+#include <visp3/ar/vpAROgre.h>
#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/ar/vpAROgre.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/vision/vpPose.h>
// List of allowed command line options
-#define GETOPTARGS "ci:p:h"
+#define GETOPTARGS "ci:p:h"
/*!
@@ -100,7 +103,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
+ \"mire-2/image.%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -122,8 +125,7 @@ OPTIONS: Default\n\
execution of this program without humain intervention.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), ppath.c_str());
+ Print the help.\n", ipath.c_str(), ppath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -141,23 +143,31 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'i': ipath = optarg; break;
- case 'p': ppath = optarg; break;
- case 'h': usage(argv[0], NULL, ipath, ppath);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'p':
+ ppath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath);
+ return false;
+ break;
default:
usage(argv[0], optarg, ipath, ppath);
- return false; break;
+ return false;
+ break;
}
}
@@ -172,56 +182,55 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
return true;
}
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
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)
- : vpAROgre(mcam, width, height){
+ vpAROgreExample(const vpCameraParameters &mcam = vpCameraParameters(), unsigned int width = 640,
+ unsigned int height = 480, const char *resourcePath = NULL)
+ : vpAROgre(mcam, width, height)
+ {
// Direction vectors
- if (resourcePath) mResourcePath = resourcePath;
- std::cout << "mResourcePath: " << mResourcePath<< std::endl;
- vecDevant = Ogre::Vector3(0,-1,0);
+ if (resourcePath)
+ mResourcePath = resourcePath;
+ std::cout << "mResourcePath: " << mResourcePath << std::endl;
+ vecDevant = Ogre::Vector3(0, -1, 0);
robot = NULL;
mAnimationState = NULL;
}
-
-protected :
+protected:
// Attributes
// Vector to move
Ogre::Vector3 vecDevant;
// Animation attribute
- Ogre::AnimationState * mAnimationState;
+ Ogre::AnimationState *mAnimationState;
// The entity representing the robot
- Ogre::Entity* robot;
+ Ogre::Entity *robot;
// Our scene will just be a plane
void createScene()
{
// Lumieres
- 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
+ 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);
- //Ombres
+ // 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->scale((Ogre::Real)0.001, (Ogre::Real)0.001, (Ogre::Real)0.001);
RobotNode->pitch(Ogre::Degree(90));
RobotNode->yaw(Ogre::Degree(-90));
robot->setCastShadows(true);
@@ -229,76 +238,80 @@ protected :
// Add an animation
// Set the good animation
- mAnimationState = robot->getAnimationState( "Idle" );
+ mAnimationState = robot->getAnimationState("Idle");
// Start over when finished
- mAnimationState->setLoop( true );
+ mAnimationState->setLoop(true);
// Animation enabled
- mAnimationState->setEnabled( true );
+ mAnimationState->setEnabled(true);
// Add a ground
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::Entity* ent = mSceneMgr->createEntity("Entitesol", "sol");
- Ogre::SceneNode* PlaneNode = mSceneMgr->getRootSceneNode()->createChildSceneNode("Entitesol");
+ 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");
PlaneNode->attachObject(ent);
ent->setMaterialName("Examples/GrassFloor");
}
- bool customframeEnded(const Ogre::FrameEvent& evt) {
+ bool customframeEnded(const Ogre::FrameEvent &evt)
+ {
// Update animation
// To move, we add it the time since last frame
- mAnimationState->addTime( evt.timeSinceLastFrame );
+ mAnimationState->addTime(evt.timeSinceLastFrame);
return true;
}
#ifdef VISP_HAVE_OIS
- bool processInputEvent(const Ogre::FrameEvent& /*evt*/) {
+ bool processInputEvent(const Ogre::FrameEvent & /*evt*/)
+ {
mKeyboard->capture();
Ogre::Matrix3 rotmy;
- double angle = -M_PI/8;
- if(mKeyboard->isKeyDown(OIS::KC_ESCAPE))
+ double angle = -M_PI / 8;
+ if (mKeyboard->isKeyDown(OIS::KC_ESCAPE))
return false;
- // Event telling that we will have to move, setting the animation to "walk", if false, annimation goes to "Idle"
+ // Event telling that we will have to move, setting the animation to
+ // "walk", if false, annimation goes to "Idle"
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);
+ if (mKeyboard->isKeyDown(OIS::KC_Z) || mKeyboard->isKeyDown(OIS::KC_UP)) {
+ 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)), (Ogre::Real)cos(-angle),0,
- 0,0,1);
- vecDevant=vecDevant*rotmy;
+ 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)));
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);
- vecDevant=vecDevant*rotmy;
+ 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;
}
// Play the right animation
- if(event){
+ if (event) {
mAnimationState = robot->getAnimationState("Walk");
- }
- else mAnimationState = robot->getAnimationState( "Idle" );
+ } else
+ mAnimationState = robot->getAnimationState("Idle");
// Start over when finished
- mAnimationState->setLoop( true );
+ mAnimationState->setLoop(true);
// Animation enabled
- mAnimationState->setEnabled( true );
+ mAnimationState->setEnabled(true);
return true;
}
@@ -307,20 +320,20 @@ protected :
/*!
This function computes a pose from four black points.
- Here to keep dimensions coherency you will need those four dots to be situated at (-7,6,0),(7,6,0),(7,-6,0),(-7,-6,0) (unit = cm) in your real world
+ Here to keep dimensions coherency you will need those four dots to be
+ 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<unsigned char> &I,
- vpPose * mPose, vpDot2 *md, vpImagePoint *mcog,
- vpHomogeneousMatrix *cMo, vpPoint *mP,
- const bool &opt_click_allowed)
+void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I, vpPose *mPose, vpDot2 *md,
+ vpImagePoint *mcog, vpHomogeneousMatrix *cMo, vpPoint *mP, const bool &opt_click_allowed)
{
// ---------------------------------------------------
// Code inspired from ViSP example of camera pose
// ----------------------------------------------------
bool opt_display = true;
-
+
//#if defined(VISP_HAVE_X11) && ! defined(APPLE)
-#if defined(VISP_HAVE_X11) && ! (defined(__APPLE__) && defined(__MACH__))
+#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
// produce an error on OSX: ‘typedef int Cursor’
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
@@ -335,101 +348,105 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
#elif defined VISP_HAVE_D3D9
vpDisplayD3D display;
#endif
- for (unsigned int i=0 ; i < 4 ; i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
if (opt_display) {
- md[i].setGraphics(true) ;
- }
- else {
- md[i].setGraphics(false) ;
+ md[i].setGraphics(true);
+ } else {
+ md[i].setGraphics(false);
}
}
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I,100,100,"Preliminary Pose Calculation");
+ display.init(I, 100, 100, "Preliminary Pose Calculation");
// display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
- return ;
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
+ return;
}
}
- std::cout<<"************************************************************************************"<<std::endl;
- std::cout<<"*************************** Preliminary Pose Calculation ***************************"<<std::endl;
- std::cout<<"****************************** Click on the 4 dots *******************************"<<std::endl;
- std::cout<<"********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), Dot4 : (-x,y,0)**********"<<std::endl;
- std::cout<<"************************************************************************************"<<std::endl;
-
- try{
+ std::cout << "*************************************************************"
+ "***********************"
+ << std::endl;
+ std::cout << "*************************** Preliminary Pose Calculation "
+ "***************************"
+ << std::endl;
+ std::cout << "****************************** Click on the 4 dots "
+ "*******************************"
+ << std::endl;
+ std::cout << "********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), "
+ "Dot4 : (-x,y,0)**********"
+ << std::endl;
+ std::cout << "*************************************************************"
+ "***********************"
+ << std::endl;
+
+ try {
vpImagePoint ip[4];
- if (! opt_click_allowed) {
- ip[0].set_i( 265 );
- ip[0].set_j( 93 );
- ip[1].set_i( 248 );
- ip[1].set_j( 242 );
- ip[2].set_i( 166 );
- ip[2].set_j( 215 );
- ip[3].set_i( 178 );
- ip[3].set_j( 85 );
+ if (!opt_click_allowed) {
+ ip[0].set_i(265);
+ ip[0].set_j(93);
+ ip[1].set_i(248);
+ ip[1].set_j(242);
+ ip[2].set_i(166);
+ ip[2].set_j(215);
+ ip[3].set_i(178);
+ ip[3].set_j(85);
}
- for(unsigned int i=0;i<4;i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// by using setGraphics, we request to see the edges of the dot
// in red on the screen.
// It uses the overlay image plane.
// The default of this setting is that it is time consumming
- md[i].setGraphics(true) ;
+ md[i].setGraphics(true);
md[i].setGrayLevelPrecision(0.7);
md[i].setSizePrecision(0.5);
- for(unsigned int j = 0;j<i;j++)
- md[j].display(I) ;
+ for (unsigned int j = 0; j < i; j++)
+ md[j].display(I);
// flush the display buffer
vpDisplay::flush(I);
- try{
+ try {
if (opt_click_allowed)
md[i].initTracking(I);
else
md[i].initTracking(I, ip[i]);
- }
- catch(...){
+ } catch (...) {
}
mcog[i] = md[i].getCog();
// an expcetion is thrown by the track method if
// - dot is lost
// - the number of pixel is too small
- // - too many pixels are detected (this is usual when a "big" specularity
+ // - too many pixels are detected (this is usual when a "big"
+ // specularity
// occurs. The threshold can be modified using the
// setNbMaxPoint(int) method
if (opt_display) {
- md[i].display(I) ;
+ md[i].display(I);
// flush the display buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
}
- }
- catch(vpException &e){
- vpERROR_TRACE("Error while tracking dots") ;
+ } catch (vpException &e) {
+ vpERROR_TRACE("Error while tracking dots");
vpCTRACE << e;
return;
}
-
- if (opt_display)
- {
+
+ if (opt_display) {
// display a red cross (size 10) in the image at the dot center
// of gravity location
//
@@ -442,67 +459,65 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
// Alternatively, to avoid this problem another set of member have
// been defined in the vpDisplay class.
// If the method name is postfixe with _uv the specification is :
- // vpDisplay::displayCross_uv(Image, column index, row index, size, color)
-
- for (unsigned int i=0 ; i < 4 ; i++)
- vpDisplay::displayCross(I, mcog[i], 10, vpColor::red) ;
-
+ // vpDisplay::displayCross_uv(Image, column index, row index, size,
+ // color)
+
+ for (unsigned int i = 0; i < 4; i++)
+ vpDisplay::displayCross(I, mcog[i], 10, vpColor::red);
+
// flush the X11 buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
-
+
// --------------------------------------------------------
// Now we will compute the pose
// --------------------------------------------------------
// the list of point is cleared (if that's not done before)
- mPose->clearPoint() ;
-
+ mPose->clearPoint();
+
// we set the 3D points coordinates (in meter !) in the object/world frame
- double l=0.06 ;
- double L=0.07 ;
- mP[0].setWorldCoordinates(-L,-l, 0 ) ; // (X,Y,Z)
- mP[1].setWorldCoordinates(L,-l, 0 ) ;
- mP[2].setWorldCoordinates(L,l, 0 ) ;
- mP[3].setWorldCoordinates(-L,l, 0 ) ;
-
+ double l = 0.06;
+ double L = 0.07;
+ mP[0].setWorldCoordinates(-L, -l, 0); // (X,Y,Z)
+ mP[1].setWorldCoordinates(L, -l, 0);
+ mP[2].setWorldCoordinates(L, l, 0);
+ mP[3].setWorldCoordinates(-L, l, 0);
+
// pixel-> meter conversion
- for (unsigned int i=0 ; i < 4 ; i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
// u[i]. v[i] are expressed in pixel
// conversion in meter is achieved using
// x = (u-u0)/px
// y = (v-v0)/py
// where px, py, u0, v0 are the intrinsic camera parameters
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x,y) ;
- mP[i].set_x(x) ;
- mP[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x, y);
+ mP[i].set_x(x);
+ mP[i].set_y(y);
}
-
-
+
// The pose structure is build, we put in the point list the set of point
// here both 2D and 3D world coordinates are known
- for (unsigned int i=0 ; i < 4 ; i++)
- {
- mPose->addPoint(mP[i]) ; // and added to the pose computation point list
+ for (unsigned int i = 0; i < 4; i++) {
+ mPose->addPoint(mP[i]); // and added to the pose computation point list
}
-
+
// compute the initial pose using Dementhon method followed by a non linear
// minimisation method
-
+
// Pose by Lagrange it provides an initialization of the pose
- mPose->computePose(vpPose::LAGRANGE, *cMo) ;
+ mPose->computePose(vpPose::LAGRANGE, *cMo);
// the pose is now refined using the virtual visual servoing approach
// Warning: cMo needs to be initialized otherwise it may diverge
- mPose->computePose(vpPose::VIRTUAL_VS, *cMo) ;
-
+ mPose->computePose(vpPose::VIRTUAL_VS, *cMo);
+
// Display breifly just to have a glimpse a the ViSP pose
// while(cpt<500){
- if( opt_display ){
+ if (opt_display) {
// Display the computed pose
- mPose->display(I,*cMo,*mcam, 0.05, vpColor::red) ;
- vpDisplay::flush(I) ;
+ mPose->display(I, *cMo, *mcam, 0.05, vpColor::red);
+ vpDisplay::flush(I);
vpTime::wait(800);
}
}
@@ -510,7 +525,7 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
#endif
int main(int argc, const char **argv)
-{
+{
try {
std::string env_ipath;
std::string opt_ipath;
@@ -520,16 +535,17 @@ int main(int argc, const char **argv)
std::string filename;
bool opt_click_allowed = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -540,8 +556,7 @@ int main(int argc, const char **argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -549,15 +564,13 @@ int main(int argc, const char **argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -566,25 +579,25 @@ int main(int argc, const char **argv)
std::ostringstream s;
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image.%04d.pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
filename = opt_ppath;
}
- //We will read a sequence of images
+ // We will read a sequence of images
vpVideoReader grabber;
grabber.setFirstFrameIndex(1);
grabber.setFileName(filename.c_str());
- // Grey level image associated to a display in the initial pose computation
+ // Grey level image associated to a display in the initial pose
+ // computation
vpImage<unsigned char> Idisplay;
// Grey level image to track points
vpImage<unsigned char> I;
@@ -609,33 +622,28 @@ int main(int argc, const char **argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
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();
// Associate the grabber to the RGBa image
grabber.open(IC);
mcam.init(mcamTmp);
- }
- catch(...)
- {
+ } catch (...) {
// an exception is thrown if an exception from readPGM has been caught
// here this will result in the end of the program
// Note that another error message has been printed from readPGM
// to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << 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);
}
@@ -647,32 +655,31 @@ int main(int argc, const char **argv)
double t0 = vpTime::measureTimeMs();
// Rendering loop
- while(ogre.continueRendering() && !grabber.end()) {
+ while (ogre.continueRendering() && !grabber.end()) {
// Acquire a frame
grabber.acquire(IC);
// Convert it to a grey level image for tracking purpose
- vpImageConvert::convert(IC,I);
+ vpImageConvert::convert(IC, I);
// kill the point list
- mPose.clearPoint() ;
+ mPose.clearPoint();
// track the dot
- for (int i=0 ; i < 4 ; i++)
- {
+ for (int i = 0; i < 4; i++) {
// track the point
- md[i].track(I, mcog[i]) ;
+ md[i].track(I, mcog[i]);
md[i].setGrayLevelPrecision(0.90);
// pixel->meter conversion
{
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y) ;
- mP[i].set_x(x) ;
- mP[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y);
+ mP[i].set_x(x);
+ mP[i].set_y(y);
}
// and added to the pose computation point list
- mPose.addPoint(mP[i]) ;
+ mPose.addPoint(mP[i]);
}
// the pose structure has been updated
@@ -682,35 +689,33 @@ int main(int argc, const char **argv)
mPose.computePose(vpPose::VIRTUAL_VS, cMo);
// Display with ogre
- ogre.display(IC,cMo);
+ ogre.display(IC, cMo);
// Wait so that the video does not go too fast
double t1 = vpTime::measureTimeMs();
- std::cout << "\r> " << 1000 / (t1 - t0) << " fps" ;
+ std::cout << "\r> " << 1000 / (t1 - t0) << " fps";
t0 = t1;
}
// Close the grabber
grabber.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
- }
- catch(Ogre::Exception &e) {
+ } catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
return 1;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
}
#else // VISP_HAVE_OGRE && VISP_HAVE_DISPLAY
-int
-main()
-{
- std::cout << "You should install Ogre3D or a display (GTK or OpenCV...) to run this example..." << std::endl;
+int main()
+{
+ std::cout << "You should install Ogre3D or a display (GTK or OpenCV...) to "
+ "run this example..."
+ << std::endl;
}
#endif
diff --git a/example/ogre-simulator/AROgreBasic.cpp b/example/ogre-simulator/AROgreBasic.cpp
index 26a8969..b8774e0 100644
--- a/example/ogre-simulator/AROgreBasic.cpp
+++ b/example/ogre-simulator/AROgreBasic.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,39 +42,41 @@
Very simple example of augmented reality based on Ogre3D.
*/
-
-
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
-//#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)))
-#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_OGRE) && (defined(VISP_HAVE_OPENCV) ||
+// 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) || \
+ (defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))))
//#if defined(VISP_HAVE_X11) && ! defined(APPLE)
-#if defined(VISP_HAVE_X11) && ! (defined(__APPLE__) && defined(__MACH__))
+#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
// produce an error on OSX: ‘typedef int Cursor’
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
// used on APPLE platforms
-# include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpImagePoint.h>
+#include <visp3/ar/vpAROgre.h>
#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/io/vpVideoReader.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/ar/vpAROgre.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/vision/vpPose.h>
// List of allowed command line options
-#define GETOPTARGS "ci:p:h"
+#define GETOPTARGS "ci:p:h"
/*!
@@ -99,7 +102,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
+ \"mire-2/image.%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -121,8 +124,7 @@ OPTIONS: Default\n\
execution of this program without humain intervention.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), ppath.c_str());
+ Print the help.\n", ipath.c_str(), ppath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -140,23 +142,31 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'i': ipath = optarg; break;
- case 'p': ppath = optarg; break;
- case 'h': usage(argv[0], NULL, ipath, ppath);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'i':
+ ipath = optarg;
+ break;
+ case 'p':
+ ppath = optarg;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath);
+ return false;
+ break;
default:
usage(argv[0], optarg, ipath, ppath);
- return false; break;
+ return false;
+ break;
}
}
@@ -173,25 +183,25 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
/*!
This function computes a pose from four black points.
- Here to keep dimensions coherency you will need those four dots to be situated at (-7,6,0),(7,6,0),(7,-6,0),(-7,-6,0) (unit = cm) in your real world
+ Here to keep dimensions coherency you will need those four dots to be
+ 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<unsigned char> &I,
- vpPose * mPose, vpDot2 *md, vpImagePoint *mcog,
- vpHomogeneousMatrix *cMo, vpPoint *mP,
- const bool &opt_click_allowed)
+void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I, vpPose *mPose, vpDot2 *md,
+ vpImagePoint *mcog, vpHomogeneousMatrix *cMo, vpPoint *mP, const bool &opt_click_allowed)
{
// ---------------------------------------------------
// Code inspired from ViSP example of camera pose
// ----------------------------------------------------
bool opt_display = true;
-
+
//#if defined(VISP_HAVE_X11) && ! defined(APPLE)
-#if defined(VISP_HAVE_X11) && ! (defined(__APPLE__) && defined(__MACH__))
+#if defined(VISP_HAVE_X11) && !(defined(__APPLE__) && defined(__MACH__))
// produce an error on OSX: ‘typedef int Cursor’
// /usr/X11R6/include/X11/X.h:108: error: ‘Cursor’ has a previous
// declaration as ‘typedef XID Cursor’. That's why it should not be
// used on APPLE platforms
- vpDisplayX display;
+ vpDisplayX display;
#elif defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_GDI
@@ -202,105 +212,108 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
vpDisplayD3D display;
#endif
- for (unsigned int i=0 ; i < 4 ; i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
if (opt_display) {
- md[i].setGraphics(true) ;
- }
- else {
- md[i].setGraphics(false) ;
+ md[i].setGraphics(true);
+ } else {
+ md[i].setGraphics(false);
}
}
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I,100,100,"Preliminary Pose Calculation");
+ display.init(I, 100, 100, "Preliminary Pose Calculation");
// display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
- return ;
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
+ return;
}
}
- std::cout<<"************************************************************************************"<<std::endl;
- std::cout<<"*************************** Preliminary Pose Calculation ***************************"<<std::endl;
- std::cout<<"****************************** Click on the 4 dots *******************************"<<std::endl;
- std::cout<<"********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), Dot4 : (-x,y,0)**********"<<std::endl;
- std::cout<<"************************************************************************************"<<std::endl;
-
- try{
+ std::cout << "*************************************************************"
+ "***********************"
+ << std::endl;
+ std::cout << "*************************** Preliminary Pose Calculation "
+ "***************************"
+ << std::endl;
+ std::cout << "****************************** Click on the 4 dots "
+ "*******************************"
+ << std::endl;
+ std::cout << "********Dot1 : (-x,-y,0), Dot2 : (x,-y,0), Dot3 : (x,y,0), "
+ "Dot4 : (-x,y,0)**********"
+ << std::endl;
+ std::cout << "*************************************************************"
+ "***********************"
+ << std::endl;
+
+ try {
vpImagePoint ip[4];
- if (! opt_click_allowed) {
- ip[0].set_i( 265 );
- ip[0].set_j( 93 );
- ip[1].set_i( 248 );
- ip[1].set_j( 242 );
- ip[2].set_i( 166 );
- ip[2].set_j( 215 );
- ip[3].set_i( 178 );
- ip[3].set_j( 85 );
+ if (!opt_click_allowed) {
+ ip[0].set_i(265);
+ ip[0].set_j(93);
+ ip[1].set_i(248);
+ ip[1].set_j(242);
+ ip[2].set_i(166);
+ ip[2].set_j(215);
+ ip[3].set_i(178);
+ ip[3].set_j(85);
}
-
- for(unsigned int i=0;i<4;i++) {
+
+ for (unsigned int i = 0; i < 4; i++) {
// by using setGraphics, we request to see the edges of the dot
// in red on the screen.
// It uses the overlay image plane.
// The default of this setting is that it is time consumming
- md[i].setGraphics(true) ;
+ md[i].setGraphics(true);
md[i].setGrayLevelPrecision(0.7);
md[i].setSizePrecision(0.5);
- for(unsigned int j = 0;j<i;j++)
- md[j].display(I) ;
+ for (unsigned int j = 0; j < i; j++)
+ md[j].display(I);
// flush the display buffer
vpDisplay::flush(I);
- try{
+ try {
if (opt_click_allowed) {
md[i].initTracking(I);
- //std::cout << "click " << i << " " << md[i] << std::endl;
- }
- else {
+ // std::cout << "click " << i << " " << md[i] << std::endl;
+ } else {
md[i].initTracking(I, ip[i]);
}
- }
- catch(...){
+ } catch (...) {
}
mcog[i] = md[i].getCog();
// an expcetion is thrown by the track method if
// - dot is lost
// - the number of pixel is too small
- // - too many pixels are detected (this is usual when a "big" specularity
+ // - too many pixels are detected (this is usual when a "big"
+ // specularity
// occurs. The threshold can be modified using the
// setNbMaxPoint(int) method
if (opt_display) {
- md[i].display(I) ;
+ md[i].display(I);
// flush the display buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
}
- }
- catch(vpException &e){
- vpERROR_TRACE("Error while tracking dots") ;
+ } catch (vpException &e) {
+ vpERROR_TRACE("Error while tracking dots");
vpCTRACE << e;
return;
}
-
- if (opt_display)
- {
+
+ if (opt_display) {
// display a red cross (size 10) in the image at the dot center
// of gravity location
//
@@ -313,74 +326,71 @@ void computeInitialPose(vpCameraParameters *mcam, vpImage<unsigned char> &I,
// Alternatively, to avoid this problem another set of member have
// been defined in the vpDisplay class.
// If the method name is postfixe with _uv the specification is :
- // vpDisplay::displayCross_uv(Image, column index, row index, size, color)
-
- for (unsigned int i=0 ; i < 4 ; i++)
- vpDisplay::displayCross(I, mcog[i], 10, vpColor::red) ;
-
+ // vpDisplay::displayCross_uv(Image, column index, row index, size,
+ // color)
+
+ for (unsigned int i = 0; i < 4; i++)
+ vpDisplay::displayCross(I, mcog[i], 10, vpColor::red);
+
// flush the X11 buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
-
+
// --------------------------------------------------------
// Now we will compute the pose
// --------------------------------------------------------
// the list of point is cleared (if that's not done before)
- mPose->clearPoint() ;
-
+ mPose->clearPoint();
+
// we set the 3D points coordinates (in meter !) in the object/world frame
- double l=0.06 ;
- double L=0.07 ;
- mP[0].setWorldCoordinates(-L,-l, 0 ) ; // (X,Y,Z)
- mP[1].setWorldCoordinates(L,-l, 0 ) ;
- mP[2].setWorldCoordinates(L,l, 0 ) ;
- mP[3].setWorldCoordinates(-L,l, 0 ) ;
-
+ double l = 0.06;
+ double L = 0.07;
+ mP[0].setWorldCoordinates(-L, -l, 0); // (X,Y,Z)
+ mP[1].setWorldCoordinates(L, -l, 0);
+ mP[2].setWorldCoordinates(L, l, 0);
+ mP[3].setWorldCoordinates(-L, l, 0);
+
// pixel-> meter conversion
- for (unsigned int i=0 ; i < 4 ; i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
// u[i]. v[i] are expressed in pixel
// conversion in meter is achieved using
// x = (u-u0)/px
// y = (v-v0)/py
// where px, py, u0, v0 are the intrinsic camera parameters
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x,y) ;
- mP[i].set_x(x) ;
- mP[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(*mcam, mcog[i], x, y);
+ mP[i].set_x(x);
+ mP[i].set_y(y);
}
-
-
+
// The pose structure is build, we put in the point list the set of point
// here both 2D and 3D world coordinates are known
- for (unsigned int i=0 ; i < 4 ; i++)
- {
- mPose->addPoint(mP[i]) ; // and added to the pose computation point list
+ for (unsigned int i = 0; i < 4; i++) {
+ mPose->addPoint(mP[i]); // and added to the pose computation point list
}
-
+
// compute the initial pose using Dementhon method followed by a non linear
// minimisation method
-
+
// Pose by Lagrange it provides an initialization of the pose
- mPose->computePose(vpPose::LAGRANGE, *cMo) ;
+ mPose->computePose(vpPose::LAGRANGE, *cMo);
// the pose is now refined using the virtual visual servoing approach
// Warning: cMo needs to be initialized otherwise it may diverge
- mPose->computePose(vpPose::VIRTUAL_VS, *cMo) ;
-
+ mPose->computePose(vpPose::VIRTUAL_VS, *cMo);
+
// Display breifly just to have a glimpse a the ViSP pose
// while(cpt<500){
- if( opt_display ){
+ if (opt_display) {
// Display the computed pose
- mPose->display(I,*cMo,*mcam, 0.05, vpColor::red) ;
- vpDisplay::flush(I) ;
+ mPose->display(I, *cMo, *mcam, 0.05, vpColor::red);
+ vpDisplay::flush(I);
vpTime::wait(1000);
}
}
-
int main(int argc, const char **argv)
-{
+{
try {
std::string env_ipath;
std::string opt_ipath;
@@ -390,17 +400,17 @@ int main(int argc, const char **argv)
std::string filename;
bool opt_click_allowed = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -411,8 +421,7 @@ int main(int argc, const char **argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -420,15 +429,13 @@ int main(int argc, const char **argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -444,25 +451,25 @@ int main(int argc, const char **argv)
std::ostringstream s;
// char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image.%04d.pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
filename = opt_ppath;
}
- //We will read a sequence of images
+ // We will read a sequence of images
vpVideoReader grabber;
grabber.setFirstFrameIndex(1);
grabber.setFileName(filename.c_str());
- // Grey level image associated to a display in the initial pose computation
+ // Grey level image associated to a display in the initial pose
+ // computation
vpImage<unsigned char> Idisplay;
// Grey level image to track points
vpImage<unsigned char> I;
@@ -487,33 +494,28 @@ int main(int argc, const char **argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
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();
// Associate the grabber to the RGBa image
grabber.open(IC);
mcam.init(mcamTmp);
- }
- catch(...)
- {
+ } catch (...) {
// an exception is thrown if an exception from readPGM has been caught
// here this will result in the end of the program
// Note that another error message has been printed from readPGM
// to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << 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);
}
@@ -522,42 +524,42 @@ int main(int argc, const char **argv)
// Initialize it
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.setScale("Robot", 0.001f, 0.001f, 0.001f);
+ ogre.setRotation("Robot", vpRotationMatrix(vpRxyzVector(M_PI / 2, -M_PI / 2, 0)));
// Add an optional point light source
- Ogre::Light * light = ogre.getSceneManager()->createLight();
- light->setDiffuseColour(1, 1, 1); // scaled RGB values
+ Ogre::Light *light = ogre.getSceneManager()->createLight();
+ light->setDiffuseColour(1, 1, 1); // scaled RGB values
light->setSpecularColour(1, 1, 1); // scaled RGB values
light->setPosition(-5, -5, 10);
light->setType(Ogre::Light::LT_POINT);
// Rendering loop
- while(ogre.continueRendering() && !grabber.end()) {
+ while (ogre.continueRendering() && !grabber.end()) {
// Acquire a frame
grabber.acquire(IC);
// Convert it to a grey level image for tracking purpose
- vpImageConvert::convert(IC,I);
+ vpImageConvert::convert(IC, I);
// kill the point list
- mPose.clearPoint() ;
+ mPose.clearPoint();
// track the dot
- for (int i=0 ; i < 4 ; i++) {
+ for (int i = 0; i < 4; i++) {
// track the point
- md[i].track(I, mcog[i]) ;
+ md[i].track(I, mcog[i]);
md[i].setGrayLevelPrecision(0.90);
// pixel->meter conversion
{
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y) ;
- mP[i].set_x(x) ;
- mP[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(mcam, mcog[i], x, y);
+ mP[i].set_x(x);
+ mP[i].set_y(y);
}
// and added to the pose computation point list
- mPose.addPoint(mP[i]) ;
+ mPose.addPoint(mP[i]);
}
// the pose structure has been updated
@@ -567,7 +569,7 @@ int main(int argc, const char **argv)
mPose.computePose(vpPose::VIRTUAL_VS, cMo);
// Display with ogre
- ogre.display(IC,cMo);
+ ogre.display(IC, cMo);
// Wait so that the video does not go too fast
vpTime::wait(15);
@@ -575,24 +577,22 @@ int main(int argc, const char **argv)
// Close the grabber
grabber.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
- }
- catch(Ogre::Exception &e) {
+ } catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
return 1;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
}
#else // VISP_HAVE_OGRE && VISP_HAVE_DISPLAY
-int
-main()
-{
- std::cout << "You should install Ogre3D or a display (GTK or OpenCV...) to run this example..." << std::endl;
+int main()
+{
+ std::cout << "You should install Ogre3D or a display (GTK or OpenCV...) to "
+ "run this example..."
+ << std::endl;
}
#endif
diff --git a/example/ogre-simulator/CMakeLists.txt b/example/ogre-simulator/CMakeLists.txt
index 00f957a..5196f38 100644
--- a/example/ogre-simulator/CMakeLists.txt
+++ b/example/ogre-simulator/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -51,7 +52,7 @@ foreach(cpp ${example_cpp})
if(VISP_HAVE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/example/parse-argv/CMakeLists.txt b/example/parse-argv/CMakeLists.txt
index 3931d4b..23b03c6 100644
--- a/example/parse-argv/CMakeLists.txt
+++ b/example/parse-argv/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/parse-argv/parse-argv1.cpp b/example/parse-argv/parse-argv1.cpp
index ca57ef7..74a5d92 100644
--- a/example/parse-argv/parse-argv1.cpp
+++ b/example/parse-argv/parse-argv1.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,23 +42,20 @@
\brief Parsing command line arguments.
*/
-
/*!
\example parse-argv1.cpp
Example of command line parsing.
*/
-
-
+#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpDebug.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
-#include <iomanip>
// List of allowed command line options
-#define GETOPTARGS "d:f:i:h"
+#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);
@@ -94,11 +92,10 @@ OPTIONS: Default\n\
A double value.\n\
\n\
-h\n\
- Print the help.\n\n",
- i_val, f_val, d_val);
+ Print the help.\n\n", i_val, f_val, d_val);
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -118,17 +115,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, int &i_val, float &f_val, double &d_val)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'd': d_val = atof(optarg_); break;
- case 'f': f_val = (float) atof(optarg_); break;
- case 'i': i_val = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, i_val, f_val, d_val); return false; break;
+ case 'd':
+ d_val = atof(optarg_);
+ break;
+ case 'f':
+ f_val = (float)atof(optarg_);
+ break;
+ case 'i':
+ i_val = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, i_val, f_val, d_val);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, i_val, f_val, d_val); return false; break;
+ usage(argv[0], optarg_, i_val, f_val, d_val);
+ return false;
+ break;
}
}
@@ -143,15 +151,14 @@ bool getOptions(int argc, const char **argv, int &i_val, float &f_val, double &d
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
using ::std::cout;
using ::std::endl;
- int i_val = 3;
- float f_val = 3.14f;
+ int i_val = 3;
+ float f_val = 3.14f;
double d_val = 3.1415;
// Read the command line options
@@ -163,12 +170,10 @@ 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) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
diff --git a/example/parse-argv/parse-argv2.cpp b/example/parse-argv/parse-argv2.cpp
index 3527d3f..bb5b137 100644
--- a/example/parse-argv/parse-argv2.cpp
+++ b/example/parse-argv/parse-argv2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,54 +42,44 @@
\brief Parsing command line arguments.
*/
-
/*!
\example parse-argv2.cpp
Example of command line parsing.
*/
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
#include <visp3/core/vpDebug.h>
#include <visp3/io/vpParseArgv.h>
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
using ::std::cout;
using ::std::endl;
- bool bool_val = false;
- int int_val = 3;
- long long_val = 33333333;
- float float_val = 3.14f;
+ bool bool_val = false;
+ int int_val = 3;
+ long long_val = 33333333;
+ float float_val = 3.14f;
double double_val = 3.1415;
- char *string_val = NULL;
+ char *string_val = NULL;
- vpParseArgv::vpArgvInfo argTable[] =
- {
- {"-bool", vpParseArgv::ARGV_CONSTANT, 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}
- } ;
+ 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}};
// 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,12 +94,10 @@ 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) {
+ } catch (const vpException &e) {
std::cout << "Catch a ViSP exception: " << e.getStringMessage() << std::endl;
return 1;
}
diff --git a/example/pose-estimation/CMakeLists.txt b/example/pose-estimation/CMakeLists.txt
index 7c7ce27..eab7ded 100644
--- a/example/pose-estimation/CMakeLists.txt
+++ b/example/pose-estimation/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/pose-estimation/poseVirtualVS.cpp b/example/pose-estimation/poseVirtualVS.cpp
index cd6d675..fea3583 100644
--- a/example/pose-estimation/poseVirtualVS.cpp
+++ b/example/pose-estimation/poseVirtualVS.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -58,39 +59,36 @@
computation.
*/
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <visp3/vision/vpPose.h>
#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-
+#include <visp3/vision/vpPose.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:p:hf:n:s:"
+#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, unsigned &step,
- bool &click_allowed, bool &display);
+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);
/*!
@@ -105,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\
@@ -120,7 +118,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ \"cube/image.%%04d.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -152,8 +150,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(),ppath.c_str(), first, nimages, step);
+ Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -178,27 +175,44 @@ 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, unsigned &step,
- bool &click_allowed, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
- return false; break;
+ return false;
+ break;
}
}
@@ -213,11 +227,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-
-
-
-int
-main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -232,25 +242,29 @@ main(int argc, const char** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- int i ;
+ int i;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << " poseVirtualVS.cpp" <<std::endl << 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 << " Example of dots tracking in an image sequence and pose "
+ "computation"
+ << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
+ opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -261,82 +275,75 @@ main(int argc, const char** argv)
// the input path comming from the command line option
if (opt_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<< std::endl
- << " use personal images" <<std::endl << std::endl;
+ << " Use -p <personal image path> option if you want to " << std::endl
+ << " use personal images" << std::endl
+ << std::endl;
exit(-1);
}
-
-
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
- unsigned iter = opt_first ;
+ unsigned iter = opt_first;
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
// therefore the program will return you an error :
- // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file /ViSP-images/cube/image.0001.pgm
+ // !! vpImageIoPnm.cpp: readPGM(#210) :couldn't read file
+ // /ViSP-images/cube/image.0001.pgm
// !! poseExample.cpp: main(#95) :Error while reading the image
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images-x.y.z.tar.gz"
// directory name
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// define the vpDot structure, here 4 dots will tracked
- vpDot d[4] ;
+ vpDot d[4];
- for (i=0 ; i < 4 ; i++)
- {
+ for (i = 0; i < 4; i++) {
// by using setGraphics, we request to see the all the pixel of the dot
// in green on the screen.
// It uses the overlay image plane.
// The default of this setting is that it is time consumming
if (opt_display) {
- d[i].setGraphics(true) ;
- }
- else {
- d[i].setGraphics(false) ;
+ d[i].setGraphics(true);
+ } else {
+ d[i].setGraphics(false);
}
}
@@ -346,37 +353,29 @@ main(int argc, const char** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
- vpImageIo::read(I,filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
+ try {
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
if (opt_ppath.empty()) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ 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;
- }
- else {
- std::cerr << std::endl
- << "ERROR:" << 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);
-
}
- // We open a window using either the X11 or GTK or GDI window manager
- // it will be located in 100,100 and titled "tracking using vpDot"
- // its size is automatically defined by the image (I) size
+// We open a window using either the X11 or GTK or GDI window manager
+// it will be located in 100,100 and titled "tracking using vpDot"
+// its size is automatically defined by the image (I) size
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -388,24 +387,22 @@ main(int argc, const char** argv)
#endif
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I,100,100,"tracking using vpDot");
+ display.init(I, 100, 100, "tracking using vpDot");
// display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
}
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 ;
- for (i=0 ; i < 4 ; i++)
- {
+ 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
// dot is expected dot location can also be specified
@@ -413,48 +410,47 @@ main(int argc, const char** argv)
// d.initTracking(I,ip) where ip is the image point from
// where the dot need to be searched.
- d[i].initTracking(I) ;
+ d[i].initTracking(I);
// track the dot and returns its coordinates in the image
// results are given in float since many many are usually considered
//
// an expcetion is thrown by the track method if
// - dot is lost
// - the number of pixel is too small
- // - too many pixels are detected (this is usual when a "big" specularity
+ // - too many pixels are detected (this is usual when a "big"
+ // specularity
// occurs. The threshold can be modified using the
// setMaxDotSize() method
- d[i].track(I, cog[i]) ;
- vpDisplay::flush(I) ;
+ d[i].track(I, cog[i]);
+ vpDisplay::flush(I);
}
- }
- else{
- cog[0].set_u( 194 );
- cog[0].set_v( 88 );
- d[0].initTracking(I, cog[0] );
+ } else {
+ cog[0].set_u(194);
+ cog[0].set_v(88);
+ d[0].initTracking(I, cog[0]);
d[0].track(I, cog[0]);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- cog[1].set_u( 225 );
- cog[1].set_v( 84 );
+ cog[1].set_u(225);
+ cog[1].set_v(84);
d[1].initTracking(I, cog[1]);
d[1].track(I, cog[1]);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- cog[2].set_u( 242 );
- cog[2].set_v( 114 );
+ cog[2].set_u(242);
+ cog[2].set_v(114);
d[2].initTracking(I, cog[2]);
d[2].track(I, cog[2]);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- cog[3].set_u( 212 );
- cog[3].set_v( 131 );
+ cog[3].set_u(212);
+ cog[3].set_v(131);
d[3].initTracking(I, cog[3]);
d[3].track(I, cog[3]);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- if (opt_display)
- {
+ if (opt_display) {
// display a red cross (size 10) in the image at the dot center
// of gravity location
@@ -463,18 +459,20 @@ main(int argc, const char** argv)
// in the vpDisplay class member's when pixel coordinates
// are considered the first element is the row index and the second
// is the column index:
- // vpDisplay::displayCross(Image, row index, column index, size, color)
- // therefore u and v are inverted wrt to the vpDot specification
+ // vpDisplay::displayCross(Image, row index, column index, size,
+ // color) therefore u and v are inverted wrt to the vpDot
+ // specification
// Alternatively, to avoid this problem another set of member have
// been defined in the vpDisplay class.
// If the method name is postfixe with _uv the specification is :
- // vpDisplay::displayCross_uv(Image, column index, row index, size, color)
+ // vpDisplay::displayCross_uv(Image, column index, row index, size,
+ // color)
- for (i=0 ; i < 4 ; i++)
- vpDisplay::displayCross(I, cog[i], 10, vpColor::red) ;
+ for (i = 0; i < 4; i++)
+ vpDisplay::displayCross(I, cog[i], 10, vpColor::red);
// flush the X11 buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// --------------------------------------------------------
@@ -482,159 +480,150 @@ main(int argc, const char** argv)
//
// The pose will be contained in an homogeneous matrix cMo
- vpHomogeneousMatrix cMo ;
-
+ vpHomogeneousMatrix cMo;
// We need a structure that content both the 3D coordinates of the point
- // in the object frame and the 2D coordinates of the point expressed in meter
- // the vpPoint class is ok for that
- vpPoint P[4] ;
+ // in the object frame and the 2D coordinates of the point expressed in
+ // meter the vpPoint class is ok for that
+ vpPoint P[4];
- // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
- vpPose pose ;
+ // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x,
+ // y) )
+ vpPose pose;
// the list of point is cleared (if that's not done before)
- pose.clearPoint() ;
+ pose.clearPoint();
// we set the 3D points coordinates (in meter !) in the object/world frame
- double L=0.04 ;
- P[0].setWorldCoordinates(-L,-L, 0 ) ; // (X,Y,Z)
- P[1].setWorldCoordinates(L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-L,L, 0 ) ;
-
+ double L = 0.04;
+ P[0].setWorldCoordinates(-L, -L, 0); // (X,Y,Z)
+ P[1].setWorldCoordinates(L, -L, 0);
+ P[2].setWorldCoordinates(L, L, 0);
+ P[3].setWorldCoordinates(-L, L, 0);
// set the camera intrinsic parameters
// see more details about the model in vpCameraParameters
- double px = 600 ;
- double py = 600 ;
- double u0 = 192 ;
- double v0 = 144 ;
- vpCameraParameters cam(px,py,u0,v0) ;
+ double px = 600;
+ double py = 600;
+ double u0 = 192;
+ double v0 = 144;
+ vpCameraParameters cam(px, py, u0, v0);
// pixel-> meter conversion
- for (i=0 ; i < 4 ; i++)
- {
+ for (i = 0; i < 4; i++) {
// u[i]. v[i] are expressed in pixel
// conversion in meter is achieved using
// x = (u-u0)/px
// y = (v-v0)/py
// where px, py, u0, v0 are the intrinsic camera parameters
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(cam, cog[i], x,y) ;
- P[i].set_x(x) ;
- P[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
}
-
// The pose structure is build, we put in the point list the set of point
// here both 2D and 3D world coordinates are known
- for (i=0 ; i < 4 ; i++)
- {
- pose.addPoint(P[i]) ; // and added to the pose computation point list
+ for (i = 0; i < 4; i++) {
+ pose.addPoint(P[i]); // and added to the pose computation point list
}
- // compute the initial pose using Dementhon method followed by a non linear
- // minimisation method
+ // compute the initial pose using Dementhon method followed by a non
+ // linear minimisation method
// Pose by Lagrange it provides an initialization of the pose
- pose.computePose(vpPose::LAGRANGE, cMo) ;
+ pose.computePose(vpPose::LAGRANGE, cMo);
// the pose is now refined using the virtual visual servoing approach
// Warning: cMo needs to be initialized otherwise it may diverge
- pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
- if( opt_display ){
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+ if (opt_display) {
// display the compute pose
- pose.display(I,cMo,cam, 0.05, vpColor::red) ;
- vpDisplay::flush(I) ;
+ pose.display(I, cMo, cam, 0.05, vpColor::red);
+ vpDisplay::flush(I);
}
// Covariance Matrix Computation
// Uncomment if you want to compute the covariance matrix.
- // pose.setCovarianceComputation(true); //Important if you want tracker.getCovarianceMatrix() to work.
+ // pose.setCovarianceComputation(true); //Important if you want
+ // tracker.getCovarianceMatrix() to work.
unsigned niter = 0;
// this is the loop over the image sequence
- while (iter < opt_nimages)
- {
+ while (iter < opt_nimages) {
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf( cfilename, opt_ppath.c_str(), iter) ;
- filename = cfilename ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
+ filename = cfilename;
}
// read the image
- vpImageIo::read(I,filename) ;
- if (opt_display){
+ vpImageIo::read(I, filename);
+ if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
-
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
}
// kill the point list
- pose.clearPoint() ;
+ pose.clearPoint();
// track the dot
- for (i=0 ; i < 4 ; i++)
- {
+ for (i = 0; i < 4; i++) {
// track the point
- d[i].track(I, cog[i]) ;
- if (opt_display){
+ d[i].track(I, cog[i]);
+ if (opt_display) {
// display point location
- vpDisplay::displayCross(I, cog[i], 10,vpColor::red) ;
+ vpDisplay::displayCross(I, cog[i], 10, vpColor::red);
}
// pixel->meter conversion
{
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(cam, cog[i], x, y) ;
- P[i].set_x(x) ;
- P[i].set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(cam, cog[i], x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
}
// and added to the pose computation point list
- pose.addPoint(P[i]) ;
+ pose.addPoint(P[i]);
}
// the pose structure has been updated
-
// the pose is now updated using the virtual visual servoing approach
// Dementhon or lagrange is no longuer necessary, pose at the
// previous iteration is sufficient
- pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
if (opt_display) {
// display the compute pose
- pose.display(I,cMo,cam, 0.05, vpColor::red) ;
+ pose.display(I, cMo, cam, 0.05, vpColor::red);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Covariance Matrix Display
// Uncomment if you want to print the covariance matrix.
- // Make sure pose.setCovarianceComputation(true) has been called (uncomment below).
- // std::cout << pose.getCovarianceMatrix() << std::endl << std::endl;
+ // Make sure pose.setCovarianceComputation(true) has been called
+ // (uncomment below). std::cout << pose.getCovarianceMatrix() <<
+ // std::endl << std::endl;
- niter++ ;
+ niter++;
- iter += opt_step ;
+ iter += opt_step;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV functionalities to display images...");
+ vpERROR_TRACE("You do not have X11, GTK, GDI or OpenCV functionalities to "
+ "display images...");
}
#endif
-
diff --git a/example/robot-simulator/afma6/CMakeLists.txt b/example/robot-simulator/afma6/CMakeLists.txt
index 2d15c1a..c3e4064 100644
--- a/example/robot-simulator/afma6/CMakeLists.txt
+++ b/example/robot-simulator/afma6/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
index 663eea0..e6795a5 100644
--- a/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/afma6/servoSimuAfma6FourPoints2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuAfma6FourPoints2DCamVelocity.cpp
@@ -53,36 +53,37 @@
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#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
// installed. On Windows, we use the native threading capabilities.
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorAfma6.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -137,17 +138,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -162,202 +171,203 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
+ std::string appveyor_threading = "";
try {
- bool opt_click_allowed = true;
- bool opt_display = true;
+ appveyor_threading = vpIoTools::getenv("APPVEYOR_THREADING");
+ } catch (...) {
+ }
- // Read the command line options
- if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
+ if (appveyor_threading == "true") {
+ try {
+ bool opt_click_allowed = true;
+ bool opt_display = true;
- // We open two displays, one for the internal camera view, the other one for
- // the external view, using either X11, GTK or GDI.
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit(-1);
+ }
+
+// We open two displays, one for the internal camera view, the other one for
+// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
- vpDisplayX displayInt;
+ vpDisplayX displayInt;
#elif defined VISP_HAVE_GDI
- vpDisplayGDI displayInt;
+ vpDisplayGDI displayInt;
#elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV displayInt;
+ vpDisplayOpenCV displayInt;
#endif
- vpImage<unsigned char> Iint(480, 640, 255);
+ vpImage<unsigned char> Iint(480, 640, 255);
- if (opt_display) {
- // open a display for the visualization
- displayInt.init(Iint,700,0, "Internal view") ;
- }
+ if (opt_display) {
+ // open a display for the visualization
+ displayInt.init(Iint, 700, 0, "Internal view");
+ }
- vpServo task;
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << 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));
-
- // sets the point coordinates in the object frame
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.045,-0.045,0) ;
- point[3].setWorldCoordinates(-0.045,0.045,0) ;
- point[2].setWorldCoordinates(0.045,0.045,0) ;
- point[1].setWorldCoordinates(0.045,-0.045,0) ;
-
- // computes the point coordinates in the camera frame and its 2D coordinates
- for (unsigned int i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
-
- // 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
-
- // 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)));
-
- // Projection of the points
- for (unsigned int i = 0 ; i < 4 ; i++)
- point[i].track(cdMo);
-
- for (unsigned int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i], point[i]);
-
- // define the task
- // - we want an eye-in-hand control law
- // - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_CAMERA);
- task.setInteractionMatrixType(vpServo::DESIRED) ;
-
- // we want to see a point on a point
- for (unsigned int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
-
- // set the gain
- task.setLambda(0.8) ;
-
- // Declaration of the robot
- vpSimulatorAfma6 robot(opt_display);
-
- // Initialise the robot and especially the camera
- 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);
-
- // Initialise the position of the object relative to the pose of the robot's camera
- robot.initialiseObjectRelativeToCamera(cMo);
-
- // Set the desired position (for the displaypart)
- robot.setDesiredCameraPosition(cdMo);
-
- // Get the internal robot's camera parameters
- vpCameraParameters cam;
- robot.getCameraParameters(cam,Iint);
-
- if (opt_display)
- {
- //Get the internal view
- vpDisplay::display(Iint);
- robot.getInternalView(Iint);
- vpDisplay::flush(Iint);
- }
+ vpServo task;
- // Display task information
- task.print() ;
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << std::endl;
+ std::cout << "----------------------------------------------" << std::endl;
+ std::cout << std::endl;
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- while(iter++<500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ // sets the initial camera location
+ vpHomogeneousMatrix cMo(-0.05, -0.05, 0.7, vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
- // Get the Time at the beginning of the loop
- double t = vpTime::measureTimeMs();
+ // sets the point coordinates in the object frame
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.045, -0.045, 0);
+ point[3].setWorldCoordinates(-0.045, 0.045, 0);
+ point[2].setWorldCoordinates(0.045, 0.045, 0);
+ point[1].setWorldCoordinates(0.045, -0.045, 0);
- // Get the current pose of the camera
- cMo = robot.get_cMo();
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ for (unsigned int i = 0; i < 4; i++)
+ point[i].track(cMo);
- if (iter==1) {
- std::cout <<"Initial robot position with respect to the object frame:\n";
- cMo.print();
- }
+ // 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
- // new point position
- for (unsigned int i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- // retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p[i],point[i]) ;
- }
+ // sets the desired position of the feature point s*
+ vpFeaturePoint pd[4];
- if (opt_display)
- {
- // Get the internal view and display it
- vpDisplay::display(Iint) ;
+ // Desired pose
+ 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++)
+ point[i].track(cdMo);
+
+ for (unsigned int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
+
+ // define the task
+ // - we want an eye-in-hand control law
+ // - articular velocity are computed
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED);
+
+ // we want to see a point on a point
+ for (unsigned int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
+
+ // set the gain
+ task.setLambda(0.8);
+
+ // Declaration of the robot
+ vpSimulatorAfma6 robot(opt_display);
+
+ // Initialise the robot and especially the camera
+ 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);
+
+ // Initialise the position of the object relative to the pose of the
+ // robot's camera
+ robot.initialiseObjectRelativeToCamera(cMo);
+
+ // Set the desired position (for the displaypart)
+ robot.setDesiredCameraPosition(cdMo);
+
+ // Get the internal robot's camera parameters
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, Iint);
+
+ if (opt_display) {
+ // Get the internal view
+ vpDisplay::display(Iint);
robot.getInternalView(Iint);
vpDisplay::flush(Iint);
}
- if (opt_display && opt_click_allowed && iter == 1)
- {
- // suppressed for automate test
- std::cout << "Click in the internal view window to continue..." << std::endl;
- vpDisplay::getClick(Iint) ;
- }
+ // Display task information
+ task.print();
- // compute the control law
- v = task.computeControlLaw() ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ while (iter++ < 500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
- // send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ // Get the Time at the beginning of the loop
+ double t = vpTime::measureTimeMs();
- std::cout << "|| s - s* || " << ( task.getError() ).sumSquare() <<std::endl ;
+ // Get the current pose of the camera
+ cMo = robot.get_cMo();
- // The main loop has a duration of 10 ms at minimum
- vpTime::wait(t,10);
- }
+ if (iter == 1) {
+ std::cout << "Initial robot position with respect to the object frame:\n";
+ cMo.print();
+ }
+
+ // new point position
+ for (unsigned int i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ // retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p[i], point[i]);
+ }
+
+ if (opt_display) {
+ // Get the internal view and display it
+ vpDisplay::display(Iint);
+ robot.getInternalView(Iint);
+ vpDisplay::flush(Iint);
+ }
- // Display task information
- task.print() ;
- task.kill();
+ if (opt_display && opt_click_allowed && iter == 1) {
+ // suppressed for automate test
+ std::cout << "Click in the internal view window to continue..." << std::endl;
+ vpDisplay::getClick(Iint);
+ }
- std::cout <<"Final robot position with respect to the object frame:\n";
- cMo.print();
+ // compute the control law
+ v = task.computeControlLaw();
- if (opt_display && opt_click_allowed)
- {
- // suppressed for automate test
- std::cout << "Click in the internal view window to end..." << std::endl;
- vpDisplay::getClick(Iint) ;
+ // send the camera velocity to the controller
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+ std::cout << "|| s - s* || " << (task.getError()).sumSquare() << std::endl;
+
+ // The main loop has a duration of 10 ms at minimum
+ vpTime::wait(t, 10);
+ }
+
+ // Display task information
+ task.print();
+ task.kill();
+
+ std::cout << "Final robot position with respect to the object frame:\n";
+ cMo.print();
+
+ if (opt_display && opt_click_allowed) {
+ // suppressed for automate test
+ std::cout << "Click in the internal view window to end..." << std::endl;
+ vpDisplay::getClick(Iint);
+ }
+ return 0;
+ } catch (vpException &e) {
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
+ return 1;
}
- return 0;
- }
- catch(vpException &e) {
- std::cout << "Catch a ViSP exception: " << e << std::endl;
- return 1;
}
+ return 0;
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
+ vpERROR_TRACE("You do not have X11, OpenCV or GDI display functionalities "
+ "or threading capabilities...");
}
#endif
diff --git a/example/robot-simulator/camera/CMakeLists.txt b/example/robot-simulator/camera/CMakeLists.txt
index d99ec99..c00bd03 100644
--- a/example/robot-simulator/camera/CMakeLists.txt
+++ b/example/robot-simulator/camera/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
index e23af9f..18197a9 100644
--- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,28 +47,28 @@
- an eye-in-hand control law,
- where velocities are computed in the camera frame.
- To compute the camera velocities, we use here the vpServo class.
-
+ To compute the camera velocities, we use here the vpServo class.
+
This example is to make into relation with
servoSimu3D_cdMc_CamVelocity.cpp where the current visual feature is
\f$s=({^{c^*}}{\bf t}_c, \; \theta u_{{^{c^*}}{\bf R}_c})\f$.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -113,15 +114,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -136,13 +141,12 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
@@ -156,19 +160,17 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
- logdirname ="C:/temp/" + username;
+ logdirname = "C:/temp/" + username;
#else
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -179,111 +181,113 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : 3D visual servoing " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : 3D visual servoing " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// Sets the initial camera location
- vpPoseVector c_r_o(// Translation tx,ty,tz
- 0.1, 0.2, 2,
- // ThetaU rotation
- vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
+ vpPoseVector c_r_o( // Translation tx,ty,tz
+ 0.1, 0.2, 2,
+ // ThetaU rotation
+ vpMath::rad(20), vpMath::rad(10), vpMath::rad(50));
// From the camera pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cMo(c_r_o) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Set the robot initial position
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo; // Compute the position of the object in the world frame
// Sets the desired camera location
- vpPoseVector cd_r_o(// Translation tx,ty,tz
- 0, 0, 1,
- // ThetaU rotation
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
+ vpPoseVector cd_r_o( // Translation tx,ty,tz
+ 0, 0, 1,
+ // ThetaU rotation
+ vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// From the camera desired pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ vpHomogeneousMatrix cdMo(cd_r_o);
- // Compute the transformation from the initial camera position to the desired one
- vpHomogeneousMatrix cMcd ;
- cMcd = cMo*cdMo.inverse() ;
+ // Compute the transformation from the initial camera position to the
+ // desired one
+ vpHomogeneousMatrix cMcd;
+ cMcd = cMo * cdMo.inverse();
// Build the 3D translation feature: ctc*
- vpFeatureTranslation t(vpFeatureTranslation::cMcd) ;
- t.buildFrom(cMcd) ;
+ vpFeatureTranslation t(vpFeatureTranslation::cMcd);
+ t.buildFrom(cMcd);
// Build the 3D rotation feature: thetaU_cRc*
vpFeatureThetaU tu(vpFeatureThetaU::cRcd); // current feature
- tu.buildFrom(cMcd) ;
+ tu.buildFrom(cMcd);
// Sets the desired rotation (always zero !) since s is the
// rotation that the camera has to achieve. Here s* = (0, 0)^T
- vpFeatureTranslation td(vpFeatureTranslation::cMcd) ;
+ vpFeatureTranslation td(vpFeatureTranslation::cMcd);
vpFeatureThetaU tud(vpFeatureThetaU::cRcd); // desired feature
// Define the task
// - we want an eye-in-hand control law
// - the robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we use here the interaction matrix computed with the current
// features
task.setInteractionMatrixType(vpServo::CURRENT);
// Add the current and desired visual features
- task.addFeature(t,td) ; // 3D translation
- task.addFeature(tu,tud) ; // 3D rotation theta u
+ task.addFeature(t, td); // 3D translation
+ task.addFeature(tu, tud); // 3D rotation theta u
// - set the constant gain to 1.0
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Start the visual servoing loop. We stop the servo after 200 iterations
- while(iter++ < 200) {
- std::cout << "------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new displacement to achieve
- cMcd = cMo*cdMo.inverse() ;
+ cMcd = cMo * cdMo.inverse();
// Update the current visual features
- t.buildFrom(cMcd) ;
- tu.buildFrom(cMcd) ;
+ t.buildFrom(cMcd);
+ tu.buildFrom(cMcd);
// Compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display task information
- if (iter==1) task.print() ;
+ if (iter == 1)
+ task.print();
// Send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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;
+ flog << v.t() << " " << (task.getError()).t() << std::endl;
}
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
@@ -291,10 +295,8 @@ main(int argc, const char ** argv)
// Close the log file
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
index 3a2c8c2..76f5862 100644
--- a/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cMcd_CamVelocityWithoutVpServo.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,9 +48,9 @@
- where velocities are computed in the camera frame.
Considering the visual feature, the interaction matrix to consider
- is \f[{\bf L}_s = \left[ \begin{array}{cc} -I_3 & [{^{c}}{\bf t}_{c^*}]_\times\\ 0
- & {\bf Lw} \end{array} \right]\f] with \f[ {\bf Lw} = -I_3 +
- \frac{\theta}{2} \; [u]_\times - \left(1 - \frac{sinc \theta}{sinc^2
+ is \f[{\bf L}_s = \left[ \begin{array}{cc} -I_3 & [{^{c}}{\bf
+ t}_{c^*}]_\times\\ 0 & {\bf Lw} \end{array} \right]\f] with \f[ {\bf Lw} =
+ -I_3 + \frac{\theta}{2} \; [u]_\times - \left(1 - \frac{sinc \theta}{sinc^2
\frac{\theta}{2}}\right) [u]^2_\times \f]
The camera velocity skew is given by:
@@ -74,24 +75,24 @@
This example is to make into relation with
servoSimu3D_cMcd_CamVelocity.cpp where vpServo and vpFeature
- classes are used.
+ classes are used.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -137,15 +138,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -160,13 +165,12 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
@@ -180,19 +184,17 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
- logdirname ="C:/temp/" + username;
+ logdirname = "C:/temp/" + username;
#else
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -203,58 +205,58 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : 3D visual servoing " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : 3D visual servoing " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// Sets the initial camera location
- vpPoseVector c_r_o(// Translation tx,ty,tz
- 0.1, 0.2, 2,
- // ThetaU rotation
- vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
+ vpPoseVector c_r_o( // Translation tx,ty,tz
+ 0.1, 0.2, 2,
+ // ThetaU rotation
+ vpMath::rad(20), vpMath::rad(10), vpMath::rad(50));
// From the camera pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cMo(c_r_o) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Set the robot initial position
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo; // Compute the position of the object in the world frame
// Sets the desired camera location
- vpPoseVector cd_r_o(// Translation tx,ty,tz
- 0, 0, 1,
- // ThetaU rotation
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
+ vpPoseVector cd_r_o( // Translation tx,ty,tz
+ 0, 0, 1,
+ // ThetaU rotation
+ vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// From the camera desired pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ vpHomogeneousMatrix cdMo(cd_r_o);
vpHomogeneousMatrix cMcd; // Transformation between current and desired camera frame
- vpRotationMatrix cRcd; // Rotation 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;
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Start the visual servoing loop. We stop the servo after 200 iterations
- while(iter++ < 200) {
- std::cout << "------------------------------------" << iter <<std::endl ;
+ while (iter++ < 200) {
+ std::cout << "------------------------------------" << iter << std::endl;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new displacement to achieve
- cMcd = cMo*cdMo.inverse() ;
+ cMcd = cMo * cdMo.inverse();
// Extract the translation vector ctc* which is the current
// translational visual feature.
@@ -264,25 +266,25 @@ main(int argc, const char ** argv)
vpThetaUVector tu_cRcd(cMcd);
// Create the identity matrix
- vpMatrix I(3,3);
+ vpMatrix I(3, 3);
I.eye();
// Compute the camera translational velocity
vpColVector v(3);
- v = lambda * ( I - vpColVector::skew(tu_cRcd) ) * ctcd;
+ v = lambda * (I - vpColVector::skew(tu_cRcd)) * ctcd;
// Compute the camera rotational velocity
vpColVector w(3);
w = lambda * tu_cRcd;
// Update the complete camera velocity vector
vpColVector velocity(6);
- for (unsigned int i=0; i<3; i++) {
- velocity[i] = v[i]; // Translational velocity
- velocity[i+3] = w[i]; // Rotational velocity
+ for (unsigned int i = 0; i < 3; i++) {
+ velocity[i] = v[i]; // Translational velocity
+ velocity[i + 3] = w[i]; // Rotational velocity
}
// Send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, velocity) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, velocity);
// Retrieve the error (s-s*)
std::cout << "|| s - s* || = " << ctcd.t() << " " << tu_cRcd.t() << std::endl;
@@ -294,10 +296,8 @@ main(int argc, const char ** argv)
// Close the log file
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
index 478c265..ce241f2 100644
--- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,7 +47,7 @@
- an eye-in-hand control law,
- where velocities are computed in the camera frame.
- To compute the camera velocities, we use here the vpServo class.
+ To compute the camera velocities, we use here the vpServo class.
This example is to make into relation with
servoSimu3D_cMcd_CamVelocity.cpp where the current visual feature is
@@ -55,20 +56,20 @@
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -114,15 +115,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -137,13 +142,12 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
@@ -157,19 +161,17 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
- logdirname ="C:/temp/" + username;
+ logdirname = "C:/temp/" + username;
#else
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -180,109 +182,110 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpServo task ;
- vpSimulatorCamera robot ;
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : 3D visual servoing " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ vpServo task;
+ vpSimulatorCamera robot;
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : 3D visual servoing " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// Sets the initial camera location
- vpPoseVector c_r_o(// Translation tx,ty,tz
- 0.1, 0.2, 2,
- // ThetaU rotation
- vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
+ vpPoseVector c_r_o( // Translation tx,ty,tz
+ 0.1, 0.2, 2,
+ // ThetaU rotation
+ vpMath::rad(20), vpMath::rad(10), vpMath::rad(50));
// From the camera pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cMo(c_r_o) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Set the robot initial position
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo; // Compute the position of the object in the world frame
// Sets the desired camera location
- vpPoseVector cd_r_o(// Translation tx,ty,tz
- 0, 0, 1,
- // ThetaU rotation
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
+ vpPoseVector cd_r_o( // Translation tx,ty,tz
+ 0, 0, 1,
+ // ThetaU rotation
+ vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// From the camera desired pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ vpHomogeneousMatrix cdMo(cd_r_o);
- // Compute the homogeneous transformation from the desired camera position to the initial one
- vpHomogeneousMatrix cdMc ;
- cdMc = cdMo*cMo.inverse() ;
+ // Compute the homogeneous transformation from the desired camera position
+ // to the initial one
+ vpHomogeneousMatrix cdMc;
+ cdMc = cdMo * cMo.inverse();
// Build the current visual features s = (c*tc, thetaU_c*Rc)^T
- vpFeatureTranslation t(vpFeatureTranslation::cdMc) ;
+ vpFeatureTranslation t(vpFeatureTranslation::cdMc);
vpFeatureThetaU tu(vpFeatureThetaU::cdRc); // current feature
- t.buildFrom(cdMc) ;
- tu.buildFrom(cdMc) ;
+ t.buildFrom(cdMc);
+ tu.buildFrom(cdMc);
// Sets the desired rotation (always zero !) since s is the
// rotation that the camera has to achieve. Here s* = (0, 0)^T
- vpFeatureTranslation td(vpFeatureTranslation::cdMc) ;
+ vpFeatureTranslation td(vpFeatureTranslation::cdMc);
vpFeatureThetaU tud(vpFeatureThetaU::cdRc); // desired feature
// Define the task
// - we want an eye-in-hand control law
// - the robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we use here the interaction matrix computed with the
// current features
task.setInteractionMatrixType(vpServo::CURRENT);
// Add the current and desired visual features
- task.addFeature(t,td) ; // 3D translation
- task.addFeature(tu,tud) ; // 3D rotation
+ task.addFeature(t, td); // 3D translation
+ task.addFeature(tu, tud); // 3D rotation
// - set the constant gain to 1.0
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Start the visual servoing loop. We stop the servo after 200 iterations
- while(iter++ < 200) {
- std::cout << "-----------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "-----------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new displacement to achieve
- cdMc = cdMo*cMo.inverse() ;
+ cdMc = cdMo * cMo.inverse();
// Update the current visual features
- t.buildFrom(cdMc) ;
- tu.buildFrom(cdMc) ;
+ t.buildFrom(cdMc);
+ tu.buildFrom(cdMc);
// Compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display task information
- if (iter==1) task.print() ;
+ if (iter == 1)
+ task.print();
// Send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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;
+ flog << v.t() << " " << (task.getError()).t() << std::endl;
}
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
@@ -290,10 +293,8 @@ main(int argc, const char ** argv)
// Close the log file
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
index 5b0c28b..81e632a 100644
--- a/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
+++ b/example/robot-simulator/camera/servoSimu3D_cdMc_CamVelocityWithoutVpServo.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -70,22 +71,20 @@
*/
-
-
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <string>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpThetaUVector.h>
#include <visp3/core/vpTranslationVector.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -131,15 +130,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -154,13 +157,12 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -173,9 +175,9 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
- logdirname ="C:/temp/" + username;
+ logdirname = "C:/temp/" + username;
#else
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
@@ -183,10 +185,8 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -197,59 +197,58 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
- 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 << " Simulation " << std::endl ;
- std::cout << " task : 3D visual servoing " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : 3D visual servoing " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// Sets the initial camera location
- vpPoseVector c_r_o(// Translation tx,ty,tz
- 0.1, 0.2, 2,
- // ThetaU rotation
- vpMath::rad(20), vpMath::rad(10), vpMath::rad(50) ) ;
+ vpPoseVector c_r_o( // Translation tx,ty,tz
+ 0.1, 0.2, 2,
+ // ThetaU rotation
+ vpMath::rad(20), vpMath::rad(10), vpMath::rad(50));
// From the camera pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cMo(c_r_o) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Set the robot initial position
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo; // Compute the position of the object in the world frame
// Sets the desired camera location
- vpPoseVector cd_r_o(// Translation tx,ty,tz
- 0, 0, 1,
- // ThetaU rotation
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
+ vpPoseVector cd_r_o( // Translation tx,ty,tz
+ 0, 0, 1,
+ // ThetaU rotation
+ vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
// From the camera desired pose build the corresponding homogeneous matrix
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ 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
+ 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;
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// Start the visual servoing loop. We stop the servo after 200 iterations
- while(iter++ < 200) {
- std::cout << "-----------------------------------" << iter <<std::endl ;
+ while (iter++ < 200) {
+ std::cout << "-----------------------------------" << iter << std::endl;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new displacement to achieve
- cdMc = cdMo*cMo.inverse() ;
+ cdMc = cdMo * cMo.inverse();
// Extract the translation vector c*tc which is the current
// translational visual feature.
@@ -270,13 +269,13 @@ main(int argc, const char ** argv)
// Update the complete camera velocity vector
vpColVector velocity(6);
- for (unsigned int i=0; i<3; i++) {
- velocity[i] = v[i]; // Translational velocity
- velocity[i+3] = w[i]; // Rotational velocity
+ for (unsigned int i = 0; i < 3; i++) {
+ velocity[i] = v[i]; // Translational velocity
+ velocity[i + 3] = w[i]; // Rotational velocity
}
// Send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, velocity) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, velocity);
// Retrieve the error (s-s*)
std::cout << "|| s - s* || = " << cdtc.t() << " " << tu_cdRc.t() << std::endl;
@@ -287,10 +286,8 @@ main(int argc, const char ** argv)
// Close the log file
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
index d5c06b3..80c8469 100644
--- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -53,20 +54,20 @@
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCircle.h>
-#include <visp3/visual_features/vpFeatureEllipse.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -112,15 +113,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,106 +140,102 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
-
- 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 ;
+ vpServo task;
+ vpSimulatorCamera robot;
+ 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;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo; // Compute the position of the object in the world frame
- vpHomogeneousMatrix cMod ;
- cMod[0][3] = 0 ;
- cMod[1][3] = 0 ;
- cMod[2][3] = 1 ;
+ vpHomogeneousMatrix cMod;
+ cMod[0][3] = 0;
+ cMod[1][3] = 0;
+ cMod[2][3] = 1;
// sets the circle coordinates in the world frame
- vpCircle circle ;
- circle.setWorldCoordinates(0,0,1,0,0,0,0.1) ;
+ vpCircle circle;
+ circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1);
// sets the desired position of the visual feature
- vpFeatureEllipse pd ;
- circle.track(cMod) ;
- vpFeatureBuilder::create(pd,circle) ;
+ vpFeatureEllipse pd;
+ circle.track(cMod);
+ vpFeatureBuilder::create(pd, circle);
- // project : computes the circle coordinates in the camera frame and its 2D coordinates
+ // project : computes the circle coordinates in the camera frame and its
+ // 2D coordinates
// sets the current position of the visual feature
- vpFeatureEllipse p ;
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle) ;
+ vpFeatureEllipse p;
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we want to see a circle on a circle
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// - set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new circle position: retrieve x,y and Z of the vpCircle structure
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle);
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
// compute the control law
- v = task.computeControlLaw() ;
- std::cout << "task rank: " << task.getTaskRank() <<std::endl ;
+ v = task.computeControlLaw();
+ std::cout << "task rank: " << task.getTaskRank() << std::endl;
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
index b993cd7..785497e 100644
--- a/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuCircle2DCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,32 +47,32 @@
- display the camera view.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCircle.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -109,7 +110,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -129,17 +130,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -154,9 +163,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -164,12 +171,12 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<unsigned char> I(512,512,0) ;
+ vpImage<unsigned char> I(512, 512, 0);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -181,126 +188,116 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Camera view...") ;
+ display.init(I, 100, 100, "Camera 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
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- double px, py ; px = py = 600 ;
- double u0, v0 ; u0 = v0 = 256 ;
+ double px, py;
+ px = py = 600;
+ double u0, v0;
+ u0 = v0 = 256;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ 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) ;
+ 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 ;
- circle.setWorldCoordinates(0,0,1,
- 0,0,0,
- 0.1) ;
+ vpCircle circle;
+ circle.setWorldCoordinates(0, 0, 1, 0, 0, 0, 0.1);
// sets the desired position of the visual feature
- vpFeatureEllipse pd ;
- circle.track(cMod) ;
- vpFeatureBuilder::create(pd,circle) ;
+ vpFeatureEllipse pd;
+ circle.track(cMod);
+ vpFeatureBuilder::create(pd, circle);
- // project : computes the circle coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- vpFeatureEllipse p ;
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle) ;
+ // project : computes the circle coordinates in the camera frame and its
+ // 2D coordinates sets the current position of the visual feature
+ vpFeatureEllipse p;
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::DESIRED) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED);
// - we want to see a circle on a circle
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// - set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new circle position
// retrieve x,y and Z of the vpCircle structure
- circle.track(cMo) ;
- vpFeatureBuilder::create(p,circle);
- circle.print() ;
- p.print() ;
+ circle.track(cMo);
+ vpFeatureBuilder::create(p, circle);
+ circle.print();
+ p.print();
if (opt_display) {
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
}
// compute the control law
- v = task.computeControlLaw() ;
- std::cout << "task rank: " << task.getTaskRank() <<std::endl ;
+ v = task.computeControlLaw();
+ std::cout << "task rank: " << task.getTaskRank() << std::endl;
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
if (opt_display && opt_click_allowed) {
std::cout << "Click in the camera view window to end..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#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 c1a957f..d3abcd5 100644
--- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,32 +47,32 @@
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCylinder.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -109,7 +110,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
/*!
@@ -126,17 +127,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -151,9 +160,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -161,12 +168,12 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<unsigned char> I(512,512,255) ;
+ vpImage<unsigned char> I(512, 512, 255);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -178,163 +185,155 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Camera view...") ;
+ display.init(I, 100, 100, "Camera 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
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- double px, py ; px = py = 600 ;
- double u0, v0 ; u0 = v0 = 256 ;
+ double px, py;
+ px = py = 600;
+ double u0, v0;
+ u0 = v0 = 256;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ 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) ;
+ 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
- 0,0,0, // point of the axis
- 0.1) ; // radius
+ vpCylinder cylinder(0, 1, 0, // direction
+ 0, 0, 0, // point of the axis
+ 0.1); // radius
// sets the desired position of the visual feature
- cylinder.track(cMod) ;
- cylinder.print() ;
-
- vpFeatureLine ld[2] ;
- int i ;
- for(i=0 ; i < 2 ; i++)
- vpFeatureBuilder::create(ld[i],cylinder,i) ;
-
- // computes the cylinder coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- cylinder.track(cMo) ;
- cylinder.print() ;
-
- vpFeatureLine l[2] ;
- for(i=0 ; i < 2 ; i++)
- {
- vpFeatureBuilder::create(l[i],cylinder,i) ;
- l[i].print() ;
+ cylinder.track(cMod);
+ cylinder.print();
+
+ vpFeatureLine ld[2];
+ int i;
+ for (i = 0; i < 2; i++)
+ vpFeatureBuilder::create(ld[i], cylinder, i);
+
+ // computes the cylinder coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ cylinder.track(cMo);
+ cylinder.print();
+
+ vpFeatureLine l[2];
+ for (i = 0; i < 2; i++) {
+ vpFeatureBuilder::create(l[i], cylinder, i);
+ l[i].print();
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- // task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ // task.setInteractionMatrixType(vpServo::CURRENT,
+ // vpServo::PSEUDO_INVERSE) ;
// it can also be interesting to test these possibilities
- // task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE) ;
- task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
- //task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
+ // task.setInteractionMatrixType(vpServo::MEAN,
+ // vpServo::PSEUDO_INVERSE) ;
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
+ // task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
// - we want to see 2 lines on 2 lines
- task.addFeature(l[0],ld[0]) ;
- task.addFeature(l[1],ld[1]) ;
+ task.addFeature(l[0], ld[0]);
+ task.addFeature(l[1], ld[1]);
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
// Display task information
- task.print() ;
+ task.print();
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// - set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- do
- {
- std::cout << "---------------------------------------------" << iter++ <<std::endl ;
- vpColVector v ;
+ do {
+ std::cout << "---------------------------------------------" << iter++ << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
// retrieve x,y and Z of the vpLine structure
- cylinder.track(cMo) ;
+ cylinder.track(cMo);
// cylinder.print() ;
- for(i=0 ; i < 2 ; i++)
- {
- vpFeatureBuilder::create(l[i],cylinder,i) ;
+ for (i = 0; i < 2; i++) {
+ vpFeatureBuilder::create(l[i], cylinder, i);
// l[i].print() ;
}
if (opt_display) {
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
}
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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) ;
- }
- while(( task.getError() ).sumSquare() > 1e-9) ;
+ } while ((task.getError()).sumSquare() > 1e-9);
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#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 46c9950..98e8f99 100644
--- a/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp
+++ b/example/robot-simulator/camera/servoSimuCylinder2DCamVelocityDisplaySecondaryTask.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,37 +46,37 @@
- camera velocities are computed,
- display internal camera view and an external view.
- This example illustrates in one hand a classical visual servoing with a cylinder.
- And in the other hand it illustrates the behaviour of the robot when adding a secondary task.
+ This example illustrates in one hand a classical visual servoing with a
+ cylinder. And in the other hand it illustrates the behaviour of the robot
+ when adding a secondary task.
*/
-
#include <visp3/core/vpConfig.h>
-#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 <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpCylinder.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -130,17 +131,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -155,9 +164,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -165,13 +172,13 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<unsigned char> Iint(512,512,0) ;
- vpImage<unsigned char> Iext(512,512,0) ;
+ vpImage<unsigned char> Iint(512, 512, 0);
+ vpImage<unsigned char> Iext(512, 512, 0);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
@@ -187,265 +194,260 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
- // 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") ;
+ try {
+ // 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");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
- vpDisplay::flush(Iint) ;
- vpDisplay::flush(Iext) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
+ vpDisplay::flush(Iint);
+ vpDisplay::flush(Iext);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- vpProjectionDisplay externalview ;
+ vpProjectionDisplay externalview;
- //Set the camera parameters
- double px, py ; px = py = 600 ;
- double u0, v0 ; u0 = v0 = 256 ;
+ // Set the camera parameters
+ double px, py;
+ px = py = 600;
+ double u0, v0;
+ u0 = v0 = 256;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ 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) ;
+ 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
- 0,0,0, // point of the axis
- 0.1) ; // radius
+ vpCylinder cylinder(0, 1, 0, // direction
+ 0, 0, 0, // point of the axis
+ 0.1); // radius
- externalview.insert(cylinder) ;
+ externalview.insert(cylinder);
// sets the desired position of the visual feature
- cylinder.track(cMod) ;
- cylinder.print() ;
-
- //Build the desired line features thanks to the cylinder and especially its paramaters in the image frame
- vpFeatureLine ld[2] ;
- int i ;
- for(i=0 ; i < 2 ; i++)
- vpFeatureBuilder::create(ld[i],cylinder,i) ;
-
- // computes the cylinder coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- cylinder.track(cMo) ;
- cylinder.print() ;
-
- //Build the current line features thanks to the cylinder and especially its paramaters in the image frame
- vpFeatureLine l[2] ;
- for(i=0 ; i < 2 ; i++)
- {
- vpFeatureBuilder::create(l[i],cylinder,i) ;
- l[i].print() ;
+ cylinder.track(cMod);
+ cylinder.print();
+
+ // Build the desired line features thanks to the cylinder and especially
+ // its paramaters in the image frame
+ vpFeatureLine ld[2];
+ int i;
+ for (i = 0; i < 2; i++)
+ vpFeatureBuilder::create(ld[i], cylinder, i);
+
+ // computes the cylinder coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ cylinder.track(cMo);
+ cylinder.print();
+
+ // Build the current line features thanks to the cylinder and especially
+ // its paramaters in the image frame
+ vpFeatureLine l[2];
+ for (i = 0; i < 2; i++) {
+ vpFeatureBuilder::create(l[i], cylinder, i);
+ l[i].print();
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::DESIRED,vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
// it can also be interesting to test these possibilities
- // task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE) ;
- // task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE) ;
- // task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
+ // task.setInteractionMatrixType(vpServo::CURRENT,vpServo::PSEUDO_INVERSE)
+ // ; task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE)
+ // ; task.setInteractionMatrixType(vpServo::CURRENT,
+ // vpServo::PSEUDO_INVERSE) ;
// task.setInteractionMatrixType(vpServo::DESIRED, vpServo::TRANSPOSE) ;
// task.setInteractionMatrixType(vpServo::CURRENT, vpServo::TRANSPOSE) ;
// we want to see 2 lines on 2 lines
- task.addFeature(l[0],ld[0]) ;
- task.addFeature(l[1],ld[1]) ;
+ task.addFeature(l[0], ld[0]);
+ 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) ;
- externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
- vpDisplay::flush(Iint) ;
- vpDisplay::flush(Iext) ;
+ vpServoDisplay::display(task, cam, Iint);
+ externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
+ vpDisplay::flush(Iint);
+ vpDisplay::flush(Iext);
// Display task information
- task.print() ;
+ task.print();
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the internal camera view window to start..." << std::endl;
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
}
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// The first loop is needed to reach the desired position
- do
- {
- std::cout << "---------------------------------------------" << iter++ <<std::endl ;
- vpColVector v ;
+ do {
+ std::cout << "---------------------------------------------" << iter++ << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
// retrieve x,y and Z of the vpLine structure
- // Compute the parameters of the cylinder in the camera frame and in the image frame
- cylinder.track(cMo) ;
-
- //Build the current line features thanks to the cylinder and especially its paramaters in the image frame
- for(i=0 ; i < 2 ; i++)
- {
- vpFeatureBuilder::create(l[i],cylinder,i) ;
+ // Compute the parameters of the cylinder in the camera frame and in the
+ // image frame
+ cylinder.track(cMo);
+
+ // Build the current line features thanks to the cylinder and especially
+ // its paramaters in the image frame
+ for (i = 0; i < 2; i++) {
+ vpFeatureBuilder::create(l[i], cylinder, i);
}
// Display the current scene
if (opt_display) {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
- vpServoDisplay::display(task,cam,Iint) ;
- externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
+ vpServoDisplay::display(task, cam, Iint);
+ externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
- std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ;
- }
- while(( task.getError() ).sumSquare() > 1e-9) ;
-
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- // Second loop is to compute the control law while taking into account the secondary task.
- // In this example the secondary task is cut in four steps.
- // The first one consists in impose a movement of the robot along the x axis of the object frame with a velocity of 0.5.
- // The second one consists in impose a movement of the robot along the y axis of the object frame with a velocity of 0.5.
- // The third one consists in impose a movement of the robot along the x axis of the object frame with a velocity of -0.5.
- // The last one consists in impose a movement of the robot along the y axis of the object frame with a velocity of -0.5.
+ 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
+ // secondary task. In this example the secondary task is cut in four
+ // steps. The first one consists in impose a movement of the robot along
+ // the x axis of the object frame with a velocity of 0.5. The second one
+ // consists in impose a movement of the robot along the y axis of the
+ // object frame with a velocity of 0.5. The third one consists in impose a
+ // movement of the robot along the x axis of the object frame with a
+ // velocity of -0.5. The last one consists in impose a movement of the
+ // robot along the y axis of the object frame with a velocity of -0.5.
// Each steps is made during 200 iterations.
- vpColVector e1(6) ; e1 = 0 ;
- vpColVector e2(6) ; e2 = 0 ;
- vpColVector proj_e1 ;
- vpColVector proj_e2 ;
+ vpColVector e1(6);
+ e1 = 0;
+ vpColVector e2(6);
+ e2 = 0;
+ vpColVector proj_e1;
+ vpColVector proj_e2;
iter = 0;
double rapport = 0;
double vitesse = 0.5;
unsigned int tempo = 800;
- while(iter < tempo)
- {
- vpColVector v ;
+ while (iter < tempo) {
+ vpColVector v;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
- cylinder.track(cMo) ;
+ cylinder.track(cMo);
- for(i=0 ; i < 2 ; i++)
- {
- vpFeatureBuilder::create(l[i],cylinder,i) ;
+ for (i = 0; i < 2; i++) {
+ vpFeatureBuilder::create(l[i], cylinder, i);
}
- if (opt_display)
- {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
- vpServoDisplay::display(task,cam,Iint) ;
- externalview.display(Iext,cextMo, cMo, cam, vpColor::red);
+ if (opt_display) {
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
+ vpServoDisplay::display(task, cam, Iint);
+ externalview.display(Iext, cextMo, cMo, cam, vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- if ( iter%tempo < 200 /*&& iter%tempo >= 0*/)
- {
+ if (iter % tempo < 200 /*&& iter%tempo >= 0*/) {
e2 = 0;
- e1[0] = fabs(vitesse) ;
+ e1[0] = fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
+ rapport = vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
}
- if ( iter%tempo < 400 && iter%tempo >= 200)
- {
+ if (iter % tempo < 400 && iter % tempo >= 200) {
e1 = 0;
- e2[1] = fabs(vitesse) ;
+ e2[1] = fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
- if ( iter%tempo < 600 && iter%tempo >= 400)
- {
+ if (iter % tempo < 600 && iter % tempo >= 400) {
e2 = 0;
- e1[0] = -fabs(vitesse) ;
+ e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = -vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
+ rapport = -vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
}
- if ( iter%tempo < 800 && iter%tempo >= 600)
- {
+ if (iter % tempo < 800 && iter % tempo >= 600) {
e1 = 0;
- e2[1] = -fabs(vitesse) ;
+ e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = -vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = -vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
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;
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
@@ -454,9 +456,6 @@ main(int argc, const char ** argv)
#else
#include <iostream>
-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 8681595..ee3b885 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,20 +51,20 @@
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -95,8 +96,8 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
- fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -113,15 +114,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -136,137 +141,133 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- int i ;
- vpServo task ;
- vpSimulatorCamera robot ;
-
+ int i;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// sets the initial camera location with respect to the object
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the point coordinates in the object frame
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-1,-1,0) ;
- point[1].setWorldCoordinates(1,-1,0) ;
- point[2].setWorldCoordinates(1,1,0) ;
- point[3].setWorldCoordinates(-1,1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-1, -1, 0);
+ point[1].setWorldCoordinates(1, -1, 0);
+ point[2].setWorldCoordinates(1, 1, 0);
+ point[3].setWorldCoordinates(-1, 1, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
- for (i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ for (i = 0; i < 4; i++)
+ point[i].track(cMo);
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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] ;
+ vpFeaturePoint pd[4];
- pd[0].buildFrom(-0.1,-0.1, 1) ;
- pd[1].buildFrom( 0.1,-0.1, 1) ;
- pd[2].buildFrom( 0.1, 0.1, 1) ;
- pd[3].buildFrom(-0.1, 0.1, 1) ;
+ pd[0].buildFrom(-0.1, -0.1, 1);
+ pd[1].buildFrom(0.1, -0.1, 1);
+ pd[2].buildFrom(0.1, 0.1, 1);
+ pd[3].buildFrom(-0.1, 0.1, 1);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::MEAN) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::MEAN);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<1500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 1500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// Set the Jacobian (expressed in the end-effector frame)
// since q is modified eJe is modified
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update new point position and corresponding features
- for (i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- //retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ // retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p[i], point[i]);
}
- // since vpServo::MEAN interaction matrix is used, we need also to update the desired features at each iteration
- pd[0].buildFrom(-0.1,-0.1, 1) ;
- pd[1].buildFrom( 0.1,-0.1, 1) ;
- pd[2].buildFrom( 0.1, 0.1, 1) ;
- pd[3].buildFrom(-0.1, 0.1, 1) ;
+ // since vpServo::MEAN interaction matrix is used, we need also to
+ // update the desired features at each iteration
+ pd[0].buildFrom(-0.1, -0.1, 1);
+ pd[1].buildFrom(0.1, -0.1, 1);
+ pd[2].buildFrom(0.1, 0.1, 1);
+ pd[3].buildFrom(-0.1, 0.1, 1);
// compute the control law ") ;
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller ") ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
index 974c9cf..18bb512 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -54,29 +55,29 @@
#include <visp3/core/vpConfig.h>
-#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 <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -131,17 +132,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -156,8 +165,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_click_allowed = true;
@@ -165,11 +173,11 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- // We open two displays, one for the internal camera view, the other one for
- // the external view, using either X11, GTK or GDI.
+// We open two displays, one for the internal camera view, the other one for
+// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
@@ -186,163 +194,161 @@ main(int argc, const char ** argv)
// open a display for the visualization
- vpImage<unsigned char> Iint(300, 300, 0) ;
- vpImage<unsigned char> Iext(300, 300, 0) ;
+ vpImage<unsigned char> Iint(300, 300, 0);
+ vpImage<unsigned char> Iext(300, 300, 0);
if (opt_display) {
- displayInt.init(Iint,0,0, "Internal view") ;
- displayExt.init(Iext,330,000, "External view") ;
-
+ displayInt.init(Iint, 0, 0, "Internal view");
+ displayExt.init(Iext, 330, 000, "External view");
}
- vpProjectionDisplay externalview ;
+ vpProjectionDisplay externalview;
- double px, py ; px = py = 500 ;
- double u0, v0 ; u0 = 150, v0 = 160 ;
+ double px, py;
+ px = py = 500;
+ double u0, v0;
+ u0 = 150, v0 = 160;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- int i ;
- vpServo task ;
- vpSimulatorCamera robot ;
+ int i;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << std::endl ;
- std::cout << "----------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << 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) ;
+ 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] ;
- point[0].setWorldCoordinates(-0.1,-0.1,0) ;
- point[1].setWorldCoordinates(0.1,-0.1,0) ;
- point[2].setWorldCoordinates(0.1,0.1,0) ;
- point[3].setWorldCoordinates(-0.1,0.1,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
+ point[3].setWorldCoordinates(-0.1, 0.1, 0);
- for (i = 0 ; i < 4 ; i++)
- externalview.insert(point[i]) ;
+ for (i = 0; i < 4; i++)
+ externalview.insert(point[i]);
- // computes the point coordinates in the camera frame and its 2D coordinates
- for (i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ for (i = 0; i < 4; i++)
+ point[i].track(cMo);
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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] ;
+ vpFeaturePoint pd[4];
- pd[0].buildFrom(-0.1,-0.1, 1) ;
- pd[1].buildFrom( 0.1,-0.1, 1) ;
- pd[2].buildFrom( 0.1, 0.1, 1) ;
- pd[3].buildFrom(-0.1, 0.1, 1) ;
+ pd[0].buildFrom(-0.1, -0.1, 1);
+ pd[1].buildFrom(0.1, -0.1, 1);
+ pd[2].buildFrom(0.1, 0.1, 1);
+ pd[3].buildFrom(-0.1, 0.1, 1);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::MEAN) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::MEAN);
// Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information " ) ;
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// Set the Jacobian (expressed in the end-effector frame)
// since q is modified eJe is modified
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update new point position and corresponding features
- for (i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- //retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p[i],point[i]) ;
+ for (i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ // retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p[i], point[i]);
}
- // since vpServo::MEAN interaction matrix is used, we need also to update the desired features at each iteration
- pd[0].buildFrom(-0.1,-0.1, 1) ;
- pd[1].buildFrom( 0.1,-0.1, 1) ;
- pd[2].buildFrom( 0.1, 0.1, 1) ;
- pd[3].buildFrom(-0.1, 0.1, 1) ;
+ // since vpServo::MEAN interaction matrix is used, we need also to
+ // update the desired features at each iteration
+ pd[0].buildFrom(-0.1, -0.1, 1);
+ pd[1].buildFrom(0.1, -0.1, 1);
+ pd[2].buildFrom(0.1, 0.1, 1);
+ pd[3].buildFrom(-0.1, 0.1, 1);
if (opt_display) {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
- vpServoDisplay::display(task,cam,Iint) ;
- externalview.display(Iext,cextMo, cMo, cam, vpColor::green) ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
+ vpServoDisplay::display(task, cam, Iint);
+ externalview.display(Iext, cextMo, cMo, cam, vpColor::green);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
- std::cout <<"Final robot position with respect to the object frame:\n";
+ std::cout << "Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed) {
// suppressed for automate test
std::cout << "\n\nClick in the internal view window to end..." << std::endl;
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
@@ -350,9 +356,6 @@ main(int argc, const char ** argv)
#else
#include <iostream>
-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 98987e2..c14b22f 100644
--- a/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuFourPoints2DPolarCamVelocityDisplay.cpp
@@ -53,35 +53,35 @@
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePointPolar.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -136,17 +136,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -161,8 +169,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Log file creation in /tmp/$USERNAME/log.dat
@@ -178,9 +185,9 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
#if defined(_WIN32)
- logdirname ="C:/temp/" + username;
+ logdirname = "C:/temp/" + username;
#else
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
#endif
// Test if the output path exist. If no try to create it
@@ -188,10 +195,8 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -202,17 +207,16 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
-
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- // We open two displays, one for the internal camera view, the other one for
- // the external view, using either X11, GTK or GDI.
+// We open two displays, one for the internal camera view, the other one for
+// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX displayInt;
vpDisplayX displayExt;
@@ -229,132 +233,116 @@ main(int argc, const char ** argv)
// open a display for the visualization
- vpImage<unsigned char> Iint(300, 300, 0) ;
- vpImage<unsigned char> Iext(300, 300, 0) ;
+ vpImage<unsigned char> Iint(300, 300, 0);
+ vpImage<unsigned char> Iext(300, 300, 0);
if (opt_display) {
- displayInt.init(Iint,0,0, "Internal view") ;
- displayExt.init(Iext,330,000, "External view") ;
-
+ displayInt.init(Iint, 0, 0, "Internal view");
+ displayExt.init(Iext, 330, 000, "External view");
}
- vpProjectionDisplay externalview ;
-
- double px, py ; px = py = 500 ;
- double u0, v0 ; u0 = 150, v0 = 160 ;
-
- vpCameraParameters cam(px,py,u0,v0);
-
- int i ;
- vpServo task ;
- vpSimulatorCamera robot ;
-
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << std::endl ;
- std::cout << "----------------------------------------------" << std::endl ;
- std::cout << std::endl ;
-
- // #define TRANS_Z_PURE
- // #define TRANS_X_PURE
- // #define ROT_Z_PURE
- // #define ROT_X_PURE
+ vpProjectionDisplay externalview;
+
+ double px, py;
+ px = py = 500;
+ double u0, v0;
+ u0 = 150, v0 = 160;
+
+ vpCameraParameters cam(px, py, u0, v0);
+
+ int i;
+ vpServo task;
+ vpSimulatorCamera robot;
+
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << std::endl;
+ std::cout << "----------------------------------------------" << std::endl;
+ std::cout << std::endl;
+
+// #define TRANS_Z_PURE
+// #define TRANS_X_PURE
+// #define ROT_Z_PURE
+// #define ROT_X_PURE
#define COMPLEX
- //#define PROBLEM
+//#define PROBLEM
#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));
+ // 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));
// 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
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ 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] ;
- point[0].setWorldCoordinates(-0.25,-0.25,0) ;
- point[1].setWorldCoordinates(0.25,-0.25,0) ;
- point[2].setWorldCoordinates(0.25,0.25,0) ;
- point[3].setWorldCoordinates(-0.25,0.25,0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.25, -0.25, 0);
+ point[1].setWorldCoordinates(0.25, -0.25, 0);
+ point[2].setWorldCoordinates(0.25, 0.25, 0);
+ point[3].setWorldCoordinates(-0.25, 0.25, 0);
- for (i = 0 ; i < 4 ; i++)
- externalview.insert(point[i]) ;
+ for (i = 0; i < 4; i++)
+ externalview.insert(point[i]);
// sets the desired position of the feature point s*"
- vpFeaturePointPolar pd[4] ;
+ vpFeaturePointPolar pd[4];
// computes the point coordinates in the desired camera frame and
// its 2D coordinates
- for (i = 0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
point[i].track(cMod);
// Computes the polar coordinates from the image point
// cartesian coordinates
- vpFeatureBuilder::create(pd[i],point[i]);
+ vpFeatureBuilder::create(pd[i], point[i]);
}
-
// computes the point coordinates in the camera frame and its 2D
// coordinates
- for (i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
+ for (i = 0; i < 4; i++)
+ point[i].track(cMo);
// sets the desired position of the point
- vpFeaturePointPolar p[4] ;
- for (i = 0 ; i < 4 ; i++) {
+ vpFeaturePointPolar p[4];
+ for (i = 0; i < 4; i++) {
// retrieve x,y and Z of the vpPoint structure to initialize the
// visual feature
vpFeatureBuilder::create(p[i], point[i]);
@@ -363,74 +351,70 @@ main(int argc, const char ** argv)
// Define the task;
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
// task.setInteractionMatrixType(vpServo::MEAN) ;
// task.setInteractionMatrixType(vpServo::DESIRED) ;
- task.setInteractionMatrixType(vpServo::CURRENT) ;
-
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- for (i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// set the gain
- task.setLambda(1) ;
-
+ task.setLambda(1);
std::cout << "\nDisplay task information: " << std::endl;
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 200) {
- std::cout << "---------------------------------------------"
- << iter <<std::endl ;
- vpColVector v ;
-
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// Set the Jacobian (expressed in the end-effector frame)
// Since q is modified eJe is modified
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// Compute new point position
- for (i = 0 ; i < 4 ; i++) {
- point[i].track(cMo) ;
+ for (i = 0; i < 4; i++) {
+ point[i].track(cMo);
// retrieve x,y and Z of the vpPoint structure to compute the feature
- vpFeatureBuilder::create(p[i],point[i]) ;
+ vpFeatureBuilder::create(p[i], point[i]);
}
if (opt_display) {
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
- vpServoDisplay::display(task,cam,Iint) ;
- externalview.display(Iext,cextMo, cMo, cam, vpColor::green);
+ vpServoDisplay::display(task, cam, Iint);
+ externalview.display(Iext, cextMo, cMo, cam, vpColor::green);
vpDisplay::flush(Iint);
vpDisplay::flush(Iext);
}
// Compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- if (iter==1) {
+ if (iter == 1) {
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
}
task.print(vpServo::FEATURE_CURRENT);
@@ -441,72 +425,63 @@ 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 ;
+ // 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;
// Save current visual feature s = (rho,theta)
- for (i = 0 ; i < 4 ; i++) {
- flog << p[i].get_rho() << " " << p[i].get_theta() << " ";
+ for (i = 0; i < 4; i++) {
+ flog << p[i].get_rho() << " " << p[i].get_theta() << " ";
}
// Save current position of the points
- for (i = 0 ; i < 4 ; i++) {
- flog << point[i].get_x() << " " << point[i].get_y() << " ";
+ for (i = 0; i < 4; i++) {
+ flog << point[i].get_x() << " " << point[i].get_y() << " ";
}
flog << std::endl;
if (iter == 1) {
vpImagePoint ip;
- ip.set_i( 10 );
- ip.set_j( 10 );
+ 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);
+ vpDisplay::displayText(Iint, ip, "A click to continue...", vpColor::red);
vpDisplay::flush(Iint);
vpDisplay::getClick(Iint);
}
-
}
-
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
- std::cout <<"Final robot position with respect to the object frame:\n";
+ std::cout << "Final robot position with respect to the object frame:\n";
cMo.print();
if (opt_display && opt_click_allowed) {
// suppressed for automate test
std::cout << "\n\nClick in the internal view to end..." << std::endl;
- vpDisplay::getClick(Iint) ;
+ vpDisplay::getClick(Iint);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#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 9c04cde..52032fd 100644
--- a/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuLine2DCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuLine2DCamVelocityDisplay.cpp
Servo a line:
@@ -45,32 +45,32 @@
- display the camera view.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpLine.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -126,17 +126,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -151,9 +159,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -161,12 +167,12 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<unsigned char> I(512,512,0) ;
+ vpImage<unsigned char> I(512, 512, 0);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -178,49 +184,47 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Camera view...") ;
+ display.init(I, 100, 100, "Camera 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
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- double px, py ; px = py = 600 ;
- double u0, v0 ; u0 = v0 = 256 ;
+ double px, py;
+ px = py = 600;
+ double u0, v0;
+ u0 = v0 = 256;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ 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;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
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) ;
- vpColVector plane2(4) ;
- plane1[0] = 0; // z = 0
+ vpColVector plane1(4);
+ vpColVector plane2(4);
+ plane1[0] = 0; // z = 0
plane1[1] = 0;
plane1[2] = 1;
plane1[3] = 0;
@@ -229,101 +233,94 @@ main(int argc, const char ** argv)
plane2[2] = 0;
plane2[3] = 0;
- vpLine line ;
- line.setWorldCoordinates(plane1, plane2) ;
+ vpLine line;
+ line.setWorldCoordinates(plane1, plane2);
// sets the desired position of the visual feature
- line.track(cMod) ;
- line.print() ;
+ line.track(cMod);
+ line.print();
- vpFeatureLine ld ;
- vpFeatureBuilder::create(ld,line) ;
+ vpFeatureLine ld;
+ vpFeatureBuilder::create(ld, line);
- // computes the line coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- line.track(cMo) ;
- line.print() ;
+ // computes the line coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ line.track(cMo);
+ line.print();
- vpFeatureLine l ;
- vpFeatureBuilder::create(l,line) ;
- l.print() ;
+ vpFeatureLine l;
+ vpFeatureBuilder::create(l, line);
+ l.print();
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a line on a line
- task.addFeature(l,ld) ;
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ task.addFeature(l, ld);
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information " ) ;
- task.print() ;
+ task.print();
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position
- line.track(cMo) ;
+ line.track(cMo);
// retrieve x,y and Z of the vpLine structure
- vpFeatureBuilder::create(l,line);
+ vpFeatureBuilder::create(l, line);
if (opt_display) {
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
}
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#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 983ba48..acfb0ed 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity1.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,19 +45,19 @@
- no display.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -103,15 +104,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -126,17 +131,16 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
// sets the initial camera location
vpHomogeneousMatrix cMo;
@@ -152,16 +156,17 @@ main(int argc, const char ** argv)
// sets the point coordinates in the world frame
vpPoint point(0, 0, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
point.track(cMo);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point); //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
vpFeaturePoint pd;
- pd.buildFrom(0,0,1); // buildFrom(x,y,Z) ;
+ pd.buildFrom(0, 0, 1); // buildFrom(x,y,Z) ;
// define the task
// - we want an eye-in-hand control law
@@ -169,49 +174,46 @@ main(int argc, const char ** argv)
task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a point on a point
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<100)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 100) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new point position
- point.track(cMo) ;
- //retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p,point);
+ point.track(cMo);
+ // retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p, point);
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
index f6a9441..f3a815b 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,28 +44,28 @@
- velocity computed in the camera frame,
- no display.
- wrt. servoSimuPoint2DCamVelocity1.cpp only the type of control law is modified. This
- illustrates the need for Jacobian update and Twist transformation matrix
- initialization.
+ wrt. servoSimuPoint2DCamVelocity1.cpp only the type of control law is
+ modified. This illustrates the need for Jacobian update and Twist
+ transformation matrix initialization.
Interaction matrix is computed as the mean of the current and desired
interaction matrix.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -111,15 +112,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -134,27 +139,25 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
-
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// sets the initial camera location
vpHomogeneousMatrix cMo;
@@ -169,80 +172,80 @@ main(int argc, const char ** argv)
// sets the point coordinates in the world frame
vpPoint point(0, 0, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
point.track(cMo);
// sets the current position of the visual feature
vpFeaturePoint p;
- vpFeatureBuilder::create(p, point); //retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
vpFeaturePoint pd;
- pd.buildFrom(0,0,1);
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::MEAN) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::MEAN);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<100)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 100) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// Set the Jacobian (expressed in the end-effector frame)
// since q is modified eJe is modified
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// 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
+ 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
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
index c4a2e39..133134c 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DCamVelocity3.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuPoint2DCamVelocity3.cpp
Servo a point:
@@ -44,26 +44,26 @@
- velocity computed in the camera frame,
- no display.
- wrt. servoSimuPoint2DCamVelocity1.cpp only the type of control law is modified. This
- illustrates the need for Jacobian update and Twist transformation matrix
- initialization, only the X coordinate is selected.
+ wrt. servoSimuPoint2DCamVelocity1.cpp only the type of control law is
+ modified. This illustrates the need for Jacobian update and Twist
+ transformation matrix initialization, only the X coordinate is selected.
Only the X coordinate is selected (test the selection process).
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -111,15 +111,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -134,25 +138,24 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
std::cout << std::endl;
// sets the initial camera location
@@ -168,79 +171,77 @@ main(int argc, const char ** argv)
// sets the point coordinates in the world frame
vpPoint point(0, 0, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
point.track(cMo);
// sets the current position of the visual feature
vpFeaturePoint p;
- vpFeatureBuilder::create(p,point); //retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
// sets the desired position of the visual feature
vpFeaturePoint pd;
- pd.buildFrom(0,0,1); // buildFrom(x,y,Z) ;
+ pd.buildFrom(0, 0, 1); // buildFrom(x,y,Z) ;
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
// Set the position of the camera in the end-effector frame
- vpHomogeneousMatrix cMe ;
- vpVelocityTwistMatrix cVe(cMe) ;
- task.set_cVe(cVe) ;
+ vpHomogeneousMatrix cMe;
+ vpVelocityTwistMatrix cVe(cMe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- task.addFeature(p,pd, vpFeaturePoint::selectX()) ;
+ task.addFeature(p, pd, vpFeaturePoint::selectX());
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 100)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 100) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// Set the Jacobian (expressed in the end-effector frame)
// since q is modified eJe is modified
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new point position
- point.track(cMo) ;
- vpFeatureBuilder::create(p,point) ; //retrieve x,y and Z of the vpPoint structure
+ point.track(cMo);
+ vpFeatureBuilder::create(p, point); // retrieve x,y and Z of the vpPoint structure
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
index deb36f5..e19a1ac 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity1.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuPoint2DhalfCamVelocity1.cpp
Simulation of a 2 1/2 D visual servoing (theta U):
@@ -47,23 +47,22 @@
*/
-
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpGenericFeature.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -94,7 +93,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -112,15 +111,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,44 +138,41 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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;
+ 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;
// 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) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
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
vpPoint point(0, 0, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
point.track(cMo);
vpPoint pointd(0, 0, 0);
@@ -181,29 +181,29 @@ main(int argc, const char ** argv)
// 1st feature (x,y)
// want to it at (0,0)
vpFeaturePoint p;
- vpFeatureBuilder::create(p,point);
+ vpFeatureBuilder::create(p, point);
vpFeaturePoint pd;
- vpFeatureBuilder::create(pd,pointd);
+ vpFeatureBuilder::create(pd, pointd);
//------------------------------------------------------------------
// 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
+ vpFeaturePoint3D Z;
+ 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
+ vpFeaturePoint3D Zd;
+ vpFeatureBuilder::create(Zd, pointd); // retrieve x,y and Z of the vpPoint structure
//------------------------------------------------------------------
// 3rd feature ThetaU
// compute the rotation that the camera has to achieve
- vpHomogeneousMatrix cdMc ;
- cdMc = cdMo*cMo.inverse() ;
+ vpHomogeneousMatrix cdMc;
+ cdMc = cdMo * cMo.inverse();
- vpFeatureThetaU tu(vpFeatureThetaU::cdRc) ;
- tu.buildFrom(cdMc) ;
+ vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
+ tu.buildFrom(cdMc);
// sets the desired rotation (always zero !)
// since s is the rotation that the camera has to achieve
@@ -212,55 +212,52 @@ main(int argc, const char ** argv)
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
- task.addFeature(p,pd) ;
- task.addFeature(Z,Zd,vpFeaturePoint3D::selectZ()) ;
- task.addFeature(tu) ;
+ task.addFeature(p, pd);
+ task.addFeature(Z, Zd, vpFeaturePoint3D::selectZ());
+ task.addFeature(tu);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update the feature
- point.track(cMo) ;
- vpFeatureBuilder::create(p,point) ;
- vpFeatureBuilder::create(Z,point) ;
+ point.track(cMo);
+ vpFeatureBuilder::create(p, point);
+ vpFeatureBuilder::create(Z, point);
- cdMc = cdMo*cMo.inverse() ;
- tu.buildFrom(cdMc) ;
+ cdMc = cdMo * cMo.inverse();
+ tu.buildFrom(cdMc);
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller ") ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
- std::cout << "Final camera location:\n " << cMo << std::endl ;
+ std::cout << "Final camera location:\n " << cMo << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
index 66f4a69..770858a 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuPoint2DhalfCamVelocity2.cpp
Simulation of a 2 1/2 D visual servoing (x,y,log Z, theta U)
@@ -46,23 +46,22 @@
- no display.
*/
-
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpGenericFeature.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -108,15 +107,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -131,20 +134,19 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- 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;
+ 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;
// In this example we will simulate a visual servoing task.
// In simulation, we have to define the scene frane Ro and the
@@ -152,36 +154,32 @@ main(int argc, const char ** argv)
// The camera location is given by an homogenous matrix cMo that
// describes the position of the camera in the scene frame.
- vpServo task ;
+ vpServo task;
// 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) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// We define a robot
// The vpSimulatorCamera implements a simple moving that is juste defined
// by its location cMo
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// Now that the current camera position has been defined,
// 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)) ;
- vpHomogeneousMatrix cdMo(cd_r_o) ;
-
+ vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
+ vpHomogeneousMatrix cdMo(cd_r_o);
//----------------------------------------------------------------------
// A 2 1/2 D visual servoing can be defined by
@@ -192,7 +190,6 @@ main(int argc, const char ** argv)
// Let us now defined the current value of these features
-
// since we simulate we have to define a 3D point that will
// forward-projected to define the current position x,y of the
// reference point
@@ -211,7 +208,8 @@ main(int argc, const char ** argv)
vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame and its
// 2D coordinates cP and then p
- // computes the point coordinates in the camera frame and its 2D coordinates" ) ;
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates" ) ;
point.track(cMo);
// We also defined (again by forward projection) the desired position
@@ -227,13 +225,13 @@ main(int argc, const char ** argv)
// for a point (x,y) Visp implements the vpFeaturePoint class.
// we no defined a feature for x,y (and for (x*,y*))
- vpFeaturePoint p,pd ;
+ vpFeaturePoint p, pd;
// and we initialized the vector s=(x,y) of p from the tracker P
// Z coordinates in p is also initialized, it will be used to compute
// the interaction matrix
- vpFeatureBuilder::create(p,point) ;
- vpFeatureBuilder::create(pd,pointd) ;
+ vpFeatureBuilder::create(p, point);
+ vpFeatureBuilder::create(pd, pointd);
//------------------------------------------------------------------
// Second feature log (Z/Zd)
@@ -247,28 +245,27 @@ main(int argc, const char ** argv)
// the interaction matrix Ls : .setInteractionMatrix(...)
// log(Z/Zd) is then a size 1 vector logZ
- vpGenericFeature logZ(1) ;
+ vpGenericFeature logZ(1);
// initialized to s = log(Z/Zd)
// Let us note that here we use the point P and Pd, it's not necessary
// to forward project twice (it's already done)
- logZ.set_s(log(point.get_Z()/pointd.get_Z())) ;
+ logZ.set_s(log(point.get_Z() / pointd.get_Z()));
// This visual has to be regulated to zero
//------------------------------------------------------------------
// 3rd feature ThetaU
- // The thetaU feature is defined, tu represents the rotation that the camera
- // has to realized.
- // the complete displacement is then defined by:
+ // The thetaU feature is defined, tu represents the rotation that the
+ // camera has to realized. the complete displacement is then defined by:
//------------------------------------------------------------------
- vpHomogeneousMatrix cdMc ;
+ vpHomogeneousMatrix cdMc;
// compute the rotation that the camera has to achieve
- cdMc = cdMo*cMo.inverse() ;
+ cdMc = cdMo * cMo.inverse();
// from this displacement, we extract the rotation cdRc represented by
// the angle theta and the rotation axis u
- vpFeatureThetaU tu(vpFeatureThetaU::cdRc) ;
- tu.buildFrom(cdMc) ;
+ vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
+ tu.buildFrom(cdMc);
// This visual has to be regulated to zero
// sets the desired rotation (always zero !)
@@ -282,77 +279,74 @@ main(int argc, const char ** argv)
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// we choose to control the robot in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current value of s
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ task.setInteractionMatrixType(vpServo::CURRENT);
// we build the task by "stacking" the visual feature
// previously defined
- task.addFeature(p,pd) ;
- task.addFeature(logZ) ;
- task.addFeature(tu) ;
+ task.addFeature(p, pd);
+ task.addFeature(logZ);
+ task.addFeature(tu);
// addFeature(X,Xd) means X should be regulated to Xd
// addFeature(X) means that X should be regulated to 0
// some features such as vpFeatureThetaU MUST be regulated to zero
// (otherwise, it will results in an error at exectution level)
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
//------------------------------------------------------------------
// An now the closed loop
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update the feature
- point.track(cMo) ;
- vpFeatureBuilder::create(p,point) ;
+ point.track(cMo);
+ vpFeatureBuilder::create(p, point);
- cdMc = cdMo*cMo.inverse() ;
- tu.buildFrom(cdMc) ;
+ cdMc = cdMo * cMo.inverse();
+ tu.buildFrom(cdMc);
// there is no feature for logZ, we explicitely build
// the related interaction matrix") ;
- logZ.set_s(log(point.get_Z()/pointd.get_Z())) ;
- vpMatrix LlogZ(1,6) ;
- LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0 ;
- LlogZ[0][2] = -1/p.get_Z() ;
- LlogZ[0][3] = -p.get_y() ;
- LlogZ[0][4] = p.get_x() ;
+ logZ.set_s(log(point.get_Z() / pointd.get_Z()));
+ vpMatrix LlogZ(1, 6);
+ LlogZ[0][0] = LlogZ[0][1] = LlogZ[0][5] = 0;
+ LlogZ[0][2] = -1 / p.get_Z();
+ LlogZ[0][3] = -p.get_y();
+ LlogZ[0][4] = p.get_x();
- logZ.setInteractionMatrix(LlogZ) ;
+ logZ.setInteractionMatrix(LlogZ);
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller ") ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
// Final camera location
- std::cout << cMo << std::endl ;
+ std::cout << cMo << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
index 03a6846..0c6cf54 100644
--- a/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint2DhalfCamVelocity3.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuPoint2DhalfCamVelocity3.cpp
Simulation of a 2 1/2 D visual servoing (x,y, t,theta u_z)
@@ -46,23 +46,22 @@
- no display.
*/
-
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpGenericFeature.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -108,15 +107,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -131,20 +134,19 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- 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;
+ 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;
// In this example we will simulate a visual servoing task.
// In simulation, we have to define the scene frane Ro and the
@@ -152,35 +154,32 @@ main(int argc, const char ** argv)
// The camera location is given by an homogenous matrix cMo that
// describes the position of the camera in the scene frame.
- vpServo task ;
+ vpServo task;
// 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) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// We define a robot
// The vpSimulatorCamera implements a simple moving that is juste defined
// by its location cMo
- vpSimulatorCamera robot ;
+ vpSimulatorCamera robot;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// Now that the current camera position has been defined,
// 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)) ;
- vpHomogeneousMatrix cdMo(cd_r_o) ;
+ vpPoseVector cd_r_o(0, 0, 1, vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
+ vpHomogeneousMatrix cdMo(cd_r_o);
//----------------------------------------------------------------------
// A 2 1/2 D visual servoing can be defined by
@@ -191,7 +190,6 @@ main(int argc, const char ** argv)
// Let us now defined the current value of these features
-
// since we simulate we have to define a 3D point that will
// forward-projected to define the current position x,y of the
// reference point
@@ -207,16 +205,17 @@ main(int argc, const char ** argv)
//------------------------------------------------------------------
// sets the point coordinates in the world frame
- vpPoint P(0, 0, 0) ;
+ vpPoint P(0, 0, 0);
// computes the P coordinates in the camera frame and its
// 2D coordinates cP and then p
- // computes the point coordinates in the camera frame and its 2D coordinates
- P.track(cMo) ;
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ P.track(cMo);
// We also defined (again by forward projection) the desired position
// of this point according to the desired camera position
- vpPoint Pd(0, 0, 0) ;
- Pd.track(cdMo) ;
+ vpPoint Pd(0, 0, 0);
+ Pd.track(cdMo);
// Nevertheless, a vpPoint is not a feature, this is just a "tracker"
// from which the feature are built
@@ -226,33 +225,33 @@ main(int argc, const char ** argv)
// for a point (x,y) Visp implements the vpFeaturePoint class.
// we no defined a feature for x,y (and for (x*,y*))
- vpFeaturePoint p,pd ;
+ vpFeaturePoint p, pd;
// and we initialized the vector s=(x,y) of p from the tracker P
// Z coordinates in p is also initialized, it will be used to compute
// the interaction matrix
- vpFeatureBuilder::create(p,P) ;
- vpFeatureBuilder::create(pd,Pd) ;
+ vpFeatureBuilder::create(p, P);
+ vpFeatureBuilder::create(pd, Pd);
// This visual has to be regulated to zero
//------------------------------------------------------------------
// 2nd feature ThetaUz and 3rd feature t
- // The thetaU feature is defined, tu represents the rotation that the camera
- // has to realized. t the translation.
- // the complete displacement is then defined by:
+ // The thetaU feature is defined, tu represents the rotation that the
+ // camera has to realized. t the translation. the complete displacement is
+ // then defined by:
//------------------------------------------------------------------
- vpHomogeneousMatrix cdMc ;
+ vpHomogeneousMatrix cdMc;
// compute the rotation that the camera has to achieve
- cdMc = cdMo*cMo.inverse() ;
+ cdMc = cdMo * cMo.inverse();
// from this displacement, we extract the rotation cdRc represented by
// the angle theta and the rotation axis u
- vpFeatureThetaU tuz(vpFeatureThetaU::cdRc) ;
- tuz.buildFrom(cdMc) ;
+ vpFeatureThetaU tuz(vpFeatureThetaU::cdRc);
+ tuz.buildFrom(cdMc);
// And the translations
- vpFeatureTranslation t(vpFeatureTranslation::cdMc) ;
- t.buildFrom(cdMc) ;
+ vpFeatureTranslation t(vpFeatureTranslation::cdMc);
+ t.buildFrom(cdMc);
// This visual has to be regulated to zero
@@ -267,15 +266,15 @@ main(int argc, const char ** argv)
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// we choose to control the robot in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current value of s
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ task.setInteractionMatrixType(vpServo::CURRENT);
// we build the task by "stacking" the visual feature
// previously defined
- task.addFeature(t) ;
- task.addFeature(p,pd) ;
- task.addFeature(tuz,vpFeatureThetaU::TUz) ; //selection of TUz
+ task.addFeature(t);
+ task.addFeature(p, pd);
+ task.addFeature(tuz, vpFeatureThetaU::TUz); // selection of TUz
// addFeature(X,Xd) means X should be regulated to Xd
// addFeature(X) means that X should be regulated to 0
@@ -283,52 +282,49 @@ main(int argc, const char ** argv)
// (otherwise, it will results in an error at exectution level)
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information " ) ;
- task.print() ;
+ task.print();
//------------------------------------------------------------------
// An now the closed loop
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// update the feature
- P.track(cMo) ;
- vpFeatureBuilder::create(p,P) ;
+ P.track(cMo);
+ vpFeatureBuilder::create(p, P);
- cdMc = cdMo*cMo.inverse() ;
- tuz.buildFrom(cdMc) ;
- t.buildFrom(cdMc) ;
+ cdMc = cdMo * cMo.inverse();
+ tuz.buildFrom(cdMc);
+ t.buildFrom(cdMc);
// compute the control law: v = -lambda L^+(s-sd)
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
// Final camera location
- std::cout << "Final camera location: \n" << cMo << std::endl ;
+ std::cout << "Final camera location: \n" << cMo << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
index 1416445..9485b45 100644
--- a/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuPoint3DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuPoint3DCamVelocity.cpp
Simulation of a 3D visual servoing:
@@ -47,20 +47,19 @@
*/
-
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeaturePoint3D.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -107,15 +106,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -130,101 +133,97 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a 3D point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a 3D point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the point coordinates in the world frame
- vpPoint point(0,0,0) ;
+ vpPoint point(0, 0, 0);
// computes the point coordinates in the camera frame
- point.track(cMo) ;
+ point.track(cMo);
- std::cout << "Point coordinates in the camera frame: " << point.cP.t() ;
+ std::cout << "Point coordinates in the camera frame: " << point.cP.t();
- vpFeaturePoint3D p ;
- p.buildFrom(point) ;
+ vpFeaturePoint3D p;
+ p.buildFrom(point);
// sets the desired position of the point
- vpFeaturePoint3D pd ;
- pd.set_XYZ(0,0,1) ;
+ vpFeaturePoint3D pd;
+ pd.set_XYZ(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a point on a point
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// set the gain") ;
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new point position
- point.track(cMo) ;
- p.buildFrom(point) ;
+ point.track(cMo);
+ p.buildFrom(point);
// std::cout << p.cP.t() ;
// std::cout << (p.get_s()).t() ;
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
index 3c06aed..9319fe8 100644
--- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuSphere2DCamVelocity.cpp
Servo a sphere:
@@ -46,20 +46,20 @@
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/core/vpSphere.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -106,15 +106,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -129,102 +133,99 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ vpSimulatorCamera robot;
- 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;
+ 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;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
- vpHomogeneousMatrix cMod ;
- cMod[0][3] = 0 ;
- cMod[1][3] = 0 ;
- cMod[2][3] = 1 ;
+ vpHomogeneousMatrix cMod;
+ cMod[0][3] = 0;
+ cMod[1][3] = 0;
+ cMod[2][3] = 1;
// sets the sphere coordinates in the world frame
- vpSphere sphere ;
- sphere.setWorldCoordinates(0,0,0,0.1) ;
+ vpSphere sphere;
+ sphere.setWorldCoordinates(0, 0, 0, 0.1);
// sets the desired position of the visual feature
- vpFeatureEllipse pd ;
- sphere.track(cMod) ;
- vpFeatureBuilder::create(pd,sphere) ;
+ vpFeatureEllipse pd;
+ sphere.track(cMod);
+ vpFeatureBuilder::create(pd, sphere);
- // computes the sphere coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- vpFeatureEllipse p ;
- sphere.track(cMo) ;
- vpFeatureBuilder::create(p,sphere) ;
+ // computes the sphere coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ vpFeatureEllipse p;
+ sphere.track(cMo);
+ vpFeatureBuilder::create(p, sphere);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a sphere on a sphere
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new sphere position: retrieve x,y and Z of the vpSphere structure
- sphere.track(cMo) ;
- vpFeatureBuilder::create(p,sphere);
+ sphere.track(cMo);
+ vpFeatureBuilder::create(p, sphere);
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- std::cout << "Task rank: " << task.getTaskRank() << std::endl ;
+ std::cout << "Task rank: " << task.getTaskRank() << std::endl;
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
diff --git a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
index 79b897d..a35095b 100644
--- a/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
+++ b/example/robot-simulator/camera/servoSimuSphere2DCamVelocitySecondaryTask.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuSphere2DCamVelocitySecondaryTask.cpp
Servo a sphere:
@@ -47,20 +47,20 @@
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/core/vpSphere.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
@@ -108,15 +108,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -131,117 +135,113 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
-
- 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 ;
+ vpServo task;
+ vpSimulatorCamera robot;
+ 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;
// sets the initial camera location
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
- vpHomogeneousMatrix cMod ;
- cMod[0][3] = 0 ;
- cMod[1][3] = 0 ;
- cMod[2][3] = 1 ;
+ vpHomogeneousMatrix cMod;
+ cMod[0][3] = 0;
+ cMod[1][3] = 0;
+ cMod[2][3] = 1;
// sets the sphere coordinates in the world frame
- vpSphere sphere ;
- sphere.setWorldCoordinates(0,0,0,0.1) ;
+ vpSphere sphere;
+ sphere.setWorldCoordinates(0, 0, 0, 0.1);
// sets the desired position of the visual feature
- vpFeatureEllipse pd ;
- sphere.track(cMod) ;
- vpFeatureBuilder::create(pd,sphere) ;
+ vpFeatureEllipse pd;
+ sphere.track(cMod);
+ vpFeatureBuilder::create(pd, sphere);
- // computes the sphere coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- vpFeatureEllipse p ;
- sphere.track(cMo) ;
- vpFeatureBuilder::create(p,sphere) ;
+ // computes the sphere coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ vpFeatureEllipse p;
+ sphere.track(cMo);
+ vpFeatureBuilder::create(p, sphere);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// we want to see a sphere on a sphere
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
// exit(1) ;
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new sphere position: retrieve x,y and Z of the vpSphere structure
- sphere.track(cMo) ;
- vpFeatureBuilder::create(p,sphere);
+ sphere.track(cMo);
+ vpFeatureBuilder::create(p, sphere);
- vpColVector de2dt(6) ;
- de2dt[2] = 1 ; // should be zero in (I-WpW)de2dt
- de2dt[5] = 0.01 ; // should be ok
- de2dt[0] = 0.01 ; // should generate a motion on (I-WpW)de2dt[4]
+ vpColVector de2dt(6);
+ de2dt[2] = 1; // should be zero in (I-WpW)de2dt
+ de2dt[5] = 0.01; // should be ok
+ de2dt[0] = 0.01; // should generate a motion on (I-WpW)de2dt[4]
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- std::cout << "de2dt :"<< de2dt.t() << std::endl;
- vpColVector sec ;
- sec = task.secondaryTask(de2dt) ;
- std::cout << "(I-WpW)de2dt :"<< sec.t() << std::endl;
+ std::cout << "de2dt :" << de2dt.t() << std::endl;
+ vpColVector sec;
+ sec = task.secondaryTask(de2dt);
+ std::cout << "(I-WpW)de2dt :" << sec.t() << std::endl;
- if (iter>20) v += sec ;
+ if (iter > 20)
+ v += sec;
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
index d890354..7f87faa 100644
--- a/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
+++ b/example/robot-simulator/camera/servoSimuSquareLine2DCamVelocityDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,33 +45,32 @@
- display the camera view.
*/
-
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpLine.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -126,17 +126,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -151,9 +159,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -161,12 +167,12 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- vpImage<unsigned char> I(512,512,0) ;
+ vpImage<unsigned char> I(512, 512, 0);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -178,163 +184,152 @@ main(int argc, const char ** argv)
#endif
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Camera view...") ;
+ display.init(I, 100, 100, "Camera 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
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
// Set the camera parameters
- double px, py ; px = py = 600 ;
- double u0, v0 ; u0 = v0 = 256 ;
+ double px, py;
+ px = py = 600;
+ double u0, v0;
+ u0 = v0 = 256;
- vpCameraParameters cam(px,py,u0,v0);
+ vpCameraParameters cam(px, py, u0, v0);
- vpServo task ;
- vpSimulatorCamera robot ;
+ vpServo task;
+ 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;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
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;
// sets the line coordinates (2 planes) in the world frame
- vpLine line[4] ;
- line[0].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
- line[1].setWorldCoordinates(0,1,0,0.05,0,0,1,0);
- line[2].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
- line[3].setWorldCoordinates(0,1,0,-0.05,0,0,1,0);
+ vpLine line[4];
+ line[0].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
+ line[1].setWorldCoordinates(0, 1, 0, 0.05, 0, 0, 1, 0);
+ line[2].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
+ line[3].setWorldCoordinates(0, 1, 0, -0.05, 0, 0, 1, 0);
- vpFeatureLine ld[4] ;
- vpFeatureLine l[4] ;
+ vpFeatureLine ld[4];
+ vpFeatureLine l[4];
// sets the desired position of the visual feature
- for(int i = 0; i < nbline; i++)
- {
- line[i].track(cMod) ;
- line[i].print() ;
+ for (int i = 0; i < nbline; i++) {
+ line[i].track(cMod);
+ line[i].print();
- vpFeatureBuilder::create(ld[i],line[i]) ;
+ vpFeatureBuilder::create(ld[i], line[i]);
}
- // computes the line coordinates in the camera frame and its 2D coordinates
- // sets the current position of the visual feature
- for(int i = 0; i < nbline; i++)
- {
- line[i].track(cMo) ;
- line[i].print() ;
+ // computes the line coordinates in the camera frame and its 2D
+ // coordinates sets the current position of the visual feature
+ for (int i = 0; i < nbline; i++) {
+ line[i].track(cMo);
+ line[i].print();
- vpFeatureBuilder::create(l[i],line[i]) ;
- l[i].print() ;
+ vpFeatureBuilder::create(l[i], line[i]);
+ l[i].print();
}
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
- //It could be also interesting to test the following tasks
- //task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- //task.setInteractionMatrixType(vpServo::MEAN, vpServo::PSEUDO_INVERSE);
+ // It could be also interesting to test the following tasks
+ // task.setInteractionMatrixType(vpServo::DESIRED,
+ // vpServo::PSEUDO_INVERSE); task.setInteractionMatrixType(vpServo::MEAN,
+ // vpServo::PSEUDO_INVERSE);
// we want to see a four lines on four lines
- for(int i = 0; i < nbline; i++)
- task.addFeature(l[i],ld[i]) ;
+ for (int i = 0; i < nbline; i++)
+ task.addFeature(l[i], ld[i]);
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
// set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
if (opt_display && opt_click_allowed) {
std::cout << "\n\nClick in the camera view window to start..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++<200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new line position: retrieve x,y and Z of the vpLine structure
- for(int i = 0; i < nbline; i++)
- {
- line[i].track(cMo) ;
- vpFeatureBuilder::create(l[i],line[i]);
+ for (int i = 0; i < nbline; i++) {
+ line[i].track(cMo);
+ vpFeatureBuilder::create(l[i], line[i]);
}
if (opt_display) {
- vpDisplay::display(I) ;
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
}
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
- std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<std::endl ; ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ std::cout << "|| s - s* || = " << (task.getError()).sumSquare() << std::endl;
+ ;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nClick in the camera view window to end..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#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 33b6279..1e05ac6 100644
--- a/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp
+++ b/example/robot-simulator/camera/servoSimuThetaUCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuThetaUCamVelocity.cpp
Simulation of a visual servoing using theta U visual features:
@@ -49,19 +49,19 @@
- no display.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "h"
+#define GETOPTARGS "h"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv);
/*!
@@ -106,15 +106,19 @@ Set the program options.
bool getOptions(int argc, const char **argv)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -129,98 +133,90 @@ bool getOptions(int argc, const char **argv)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
// Read the command line options
if (getOptions(argc, argv) == false) {
- exit (-1);
+ exit(-1);
}
- vpServo task ;
- vpSimulatorCamera robot ;
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo using theta U visual feature " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ vpServo task;
+ vpSimulatorCamera robot;
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo using theta U visual feature " << 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) ;
+ vpHomogeneousMatrix cMo(c_r_o);
// Compute the position of the object in the world frame
vpHomogeneousMatrix wMc, wMo;
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
wMo = wMc * cMo;
// sets the desired camera location
- vpPoseVector cd_r_o(0,0,1,
- vpMath::rad(0),vpMath::rad(0),vpMath::rad(0)) ;
- vpHomogeneousMatrix cdMo(cd_r_o) ;
-
+ 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
- vpHomogeneousMatrix cdMc ;
- cdMc = cdMo*cMo.inverse() ;
- vpFeatureThetaU tu(vpFeatureThetaU::cdRc) ;
- tu.buildFrom(cdMc) ;
+ vpHomogeneousMatrix cdMc;
+ cdMc = cdMo * cMo.inverse();
+ vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
+ tu.buildFrom(cdMc);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::DESIRED) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED);
- task.addFeature(tu) ;
+ task.addFeature(tu);
// - set the gain
- task.setLambda(1) ;
+ task.setLambda(1);
// Display task information
- task.print() ;
+ task.print();
- unsigned int iter=0 ;
+ unsigned int iter = 0;
// loop
- while(iter++ < 200)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ while (iter++ < 200) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
// get the robot position
- robot.getPosition(wMc) ;
+ robot.getPosition(wMc);
// Compute the position of the camera wrt the object frame
cMo = wMc.inverse() * wMo;
// new rotation to achieve
- cdMc = cdMo*cMo.inverse() ;
- tu.buildFrom(cdMc) ;
+ cdMc = cdMo * cMo.inverse();
+ tu.buildFrom(cdMc);
// compute the control law
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ 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
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/example/robot-simulator/viper850/CMakeLists.txt b/example/robot-simulator/viper850/CMakeLists.txt
index 935e2e8..59e777c 100644
--- a/example/robot-simulator/viper850/CMakeLists.txt
+++ b/example/robot-simulator/viper850/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
index d324583..32289d9 100644
--- a/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
+++ b/example/robot-simulator/viper850/servoSimuViper850FourPoints2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\example servoSimuViper850FourPoints2DCamVelocity.cpp
@@ -53,36 +53,37 @@
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#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
// installed. On Windows, we use the native threading capabilities.
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpSimulatorViper850.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -120,7 +121,7 @@ OPTIONS: Default\n\
Print the help.\n");
if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
/*!
@@ -137,17 +138,25 @@ Set the program options.
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -162,204 +171,203 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
+ std::string appveyor_threading = "";
try {
- bool opt_click_allowed = true;
- bool opt_display = true;
+ appveyor_threading = vpIoTools::getenv("APPVEYOR_THREADING");
+ } catch (...) {
+ }
- // Read the command line options
- if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
- }
+ if (appveyor_threading == "true") {
+ try {
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit(-1);
+ }
- // We open two displays, one for the internal camera view, the other one for
- // the external view, using either X11, GTK or GDI.
+// We open two displays, one for the internal camera view, the other one for
+// the external view, using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
- vpDisplayX displayInt;
+ vpDisplayX displayInt;
#elif defined VISP_HAVE_GDI
- vpDisplayGDI displayInt;
+ vpDisplayGDI displayInt;
#elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV displayInt;
+ vpDisplayOpenCV displayInt;
#endif
- // open a display for the visualization
-
- vpImage<unsigned char> Iint(480, 640, 255);
+ // open a display for the visualization
- if (opt_display) {
- displayInt.init(Iint,700,0, "Internal view") ;
- }
+ vpImage<unsigned char> Iint(480, 640, 255);
- vpServo task;
+ if (opt_display) {
+ displayInt.init(Iint, 700, 0, "Internal view");
+ }
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo 4 points " << std::endl ;
- std::cout << "----------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ vpServo task;
- // sets the initial camera location
- vpHomogeneousMatrix cMo(-0.05,-0.05,0.7,
- vpMath::rad(10), vpMath::rad(10), vpMath::rad(-30));
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo 4 points " << 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));
- // sets the point coordinates in the object frame
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.045,-0.045,0) ;
- point[3].setWorldCoordinates(-0.045,0.045,0) ;
- point[2].setWorldCoordinates(0.045,0.045,0) ;
- point[1].setWorldCoordinates(0.045,-0.045,0) ;
+ // sets the point coordinates in the object frame
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.045, -0.045, 0);
+ point[3].setWorldCoordinates(-0.045, 0.045, 0);
+ point[2].setWorldCoordinates(0.045, 0.045, 0);
+ point[1].setWorldCoordinates(0.045, -0.045, 0);
- // computes the point coordinates in the camera frame and its 2D coordinates
- for (unsigned int i = 0 ; i < 4 ; i++)
- point[i].track(cMo) ;
+ // computes the point coordinates in the camera frame and its 2D
+ // coordinates
+ for (unsigned int i = 0; i < 4; i++)
+ point[i].track(cMo);
- // 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
+ // 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
- // sets the desired position of the feature point s*
- vpFeaturePoint pd[4] ;
+ // 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)));
+ // Desired pose
+ 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++)
- point[i].track(cdMo);
+ // Projection of the points
+ for (unsigned int i = 0; i < 4; i++)
+ point[i].track(cdMo);
- for (unsigned int i = 0 ; i < 4 ; i++)
- vpFeatureBuilder::create(pd[i], point[i]);
+ for (unsigned int i = 0; i < 4; i++)
+ vpFeatureBuilder::create(pd[i], point[i]);
- // define the task
- // - we want an eye-in-hand control law
- // - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_CAMERA);
- task.setInteractionMatrixType(vpServo::DESIRED) ;
+ // define the task
+ // - we want an eye-in-hand control law
+ // - articular velocity are computed
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED);
- // - we want to see a point on a point
- for (unsigned int i = 0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ // - we want to see a point on a point
+ for (unsigned int i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
- // set the gain
- task.setLambda(0.8) ;
+ // set the gain
+ task.setLambda(0.8);
- // Declaration of the robot
- vpSimulatorViper850 robot(opt_display);
+ // Declaration of the robot
+ vpSimulatorViper850 robot(opt_display);
- // Initialise the robot and especially the camera
- robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA,vpCameraParameters::perspectiveProjWithoutDistortion);
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ // Initialise the robot and especially the camera
+ 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);
+ // Initialise the object for the display part
+ robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
- // Initialise the position of the object relative to the pose of the robot's camera
- robot.initialiseObjectRelativeToCamera(cMo);
+ // Initialise the position of the object relative to the pose of the
+ // robot's camera
+ robot.initialiseObjectRelativeToCamera(cMo);
- // Set the desired position (for the display part)
- robot.setDesiredCameraPosition(cdMo);
+ // Set the desired position (for the display part)
+ robot.setDesiredCameraPosition(cdMo);
- // Get the internal robot's camera parameters
- vpCameraParameters cam;
- robot.getCameraParameters(cam,Iint);
+ // Get the internal robot's camera parameters
+ vpCameraParameters cam;
+ robot.getCameraParameters(cam, Iint);
- if (opt_display)
- {
- //Get the internal view
- vpDisplay::display(Iint);
- robot.getInternalView(Iint);
- vpDisplay::flush(Iint);
- }
+ if (opt_display) {
+ // Get the internal view
+ vpDisplay::display(Iint);
+ robot.getInternalView(Iint);
+ vpDisplay::flush(Iint);
+ }
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
- unsigned int iter=0 ;
- // loop
- while(iter++<500)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
- vpColVector v ;
+ unsigned int iter = 0;
+ // loop
+ while (iter++ < 500) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
+ vpColVector v;
- //Get the Time at the beginning of the loop
- double t = vpTime::measureTimeMs();
+ // Get the Time at the beginning of the loop
+ double t = vpTime::measureTimeMs();
- //Get the current pose of the camera
- cMo = robot.get_cMo();
+ // Get the current pose of the camera
+ cMo = robot.get_cMo();
- if (iter==1) {
- std::cout <<"Initial robot position with respect to the object frame:\n";
- cMo.print();
- }
+ if (iter == 1) {
+ std::cout << "Initial robot position with respect to the object frame:\n";
+ cMo.print();
+ }
- // new point position
- for (unsigned int i = 0 ; i < 4 ; i++)
- {
- point[i].track(cMo) ;
- //retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p[i],point[i]) ;
- }
+ // new point position
+ for (unsigned int i = 0; i < 4; i++) {
+ point[i].track(cMo);
+ // retrieve x,y and Z of the vpPoint structure
+ vpFeatureBuilder::create(p[i], point[i]);
+ }
- if (opt_display)
- {
- // Get the internal view and display it
- vpDisplay::display(Iint) ;
- robot.getInternalView(Iint);
- vpDisplay::flush(Iint);
- }
+ if (opt_display) {
+ // Get the internal view and display it
+ vpDisplay::display(Iint);
+ robot.getInternalView(Iint);
+ vpDisplay::flush(Iint);
+ }
- if (opt_display && opt_click_allowed && iter == 1)
- {
- // suppressed for automate test
- std::cout << "Click in the internal view window to continue..." << std::endl;
- vpDisplay::getClick(Iint) ;
- }
+ if (opt_display && opt_click_allowed && iter == 1) {
+ // suppressed for automate test
+ std::cout << "Click in the internal view window to continue..." << std::endl;
+ vpDisplay::getClick(Iint);
+ }
- // compute the control law
- v = task.computeControlLaw() ;
+ // compute the control law
+ v = task.computeControlLaw();
- // send the camera velocity to the controller
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ // 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);
- }
+ // The main loop has a duration of 10 ms at minimum
+ vpTime::wait(t, 10);
+ }
- // Display task information
- task.print() ;
- task.kill();
+ // Display task information
+ task.print();
+ task.kill();
- std::cout <<"Final robot position with respect to the object frame:\n";
- cMo.print();
+ std::cout << "Final robot position with respect to the object frame:\n";
+ cMo.print();
- if (opt_display && opt_click_allowed)
- {
- // suppressed for automate test
- std::cout << "Click in the internal view window to end..." << std::endl;
- vpDisplay::getClick(Iint) ;
+ if (opt_display && opt_click_allowed) {
+ // suppressed for automate test
+ std::cout << "Click in the internal view window to end..." << std::endl;
+ vpDisplay::getClick(Iint);
+ }
+ return 0;
+ } catch (vpException &e) {
+ std::cout << "Catch a ViSP exception: " << e << std::endl;
+ return 1;
}
- return 0;
- }
- catch(vpException &e) {
- std::cout << "Catch a ViSP exception: " << e << std::endl;
- return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV or GDI display functionalities or threading capabilities...");
+ vpERROR_TRACE("You do not have X11, OpenCV or GDI display functionalities "
+ "or threading capabilities...");
}
#endif
diff --git a/example/servo-afma4/CMakeLists.txt b/example/servo-afma4/CMakeLists.txt
index 93e81d8..32ad313 100644
--- a/example/servo-afma4/CMakeLists.txt
+++ b/example/servo-afma4/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/servo-afma4/moveAfma4.cpp b/example/servo-afma4/moveAfma4.cpp
index d2ab7bb..2ba8ef9 100644
--- a/example/servo-afma4/moveAfma4.cpp
+++ b/example/servo-afma4/moveAfma4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -56,14 +57,14 @@
#ifdef VISP_HAVE_AFMA4
-#include <unistd.h>
#include <stdlib.h>
+#include <unistd.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpRobotAfma4.h>
// List of allowed command line options
-#define GETOPTARGS "mh"
+#define GETOPTARGS "mh"
/*!
@@ -94,10 +95,9 @@ OPTIONS: Default\n\
Print the help.\n\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -114,15 +114,22 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &control)
{
const char *optarg;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'm': control = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'm':
+ control = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
- usage(argv[0], optarg); return false; break;
+ usage(argv[0], optarg);
+ return false;
+ break;
}
}
@@ -137,22 +144,20 @@ bool getOptions(int argc, const char **argv, bool &control)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- try
- {
+ try {
bool control = true; // Turn on the robot control by applying positions
// and velocities to the robot.
// Read the command line options
if (getOptions(argc, argv, control) == false) {
- exit (-1);
+ exit(-1);
}
- vpRobotAfma4 robot ;
+ vpRobotAfma4 robot;
- vpColVector qd(robot.njoint) ;
- vpColVector q(robot.njoint) ;
+ vpColVector qd(robot.njoint);
+ vpColVector q(robot.njoint);
//
// Position control in articular
@@ -164,18 +169,18 @@ main(int argc, const char ** argv)
std::cout << "Position control: in articular..." << std::endl;
std::cout << " position to reach: " << qd.t() << std::endl;
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
if (control)
- robot.setPosition(vpRobot::ARTICULAR_FRAME, qd) ;
- sleep(1) ;
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, qd);
+ sleep(1);
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ std::cout << " measured position: " << q.t();
+ sleep(1);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q) ;
- std::cout << " measured position: " << q.t() ;
- sleep(1) ;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+#if 0
//
// Velocity control in articular
//
@@ -214,39 +219,37 @@ main(int argc, const char ** argv)
if (control)
robot.setVelocity(vpRobot::ARTICULAR_FRAME, q) ;
sleep(5) ;
-
+#endif
//
// Velocity control in camera frame
//
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "Velocity control: in camera frame..." << std::endl;
- q.resize(2) ;
+ q.resize(6);
q = 0.0;
- q[0] = vpMath::rad(2) ; // rotation arround vertical axis
+ q[0] = vpMath::rad(2); // rotation arround vertical axis
std::cout << " rx rotation: " << q[0] << std::endl;
if (control)
- robot.setVelocity(vpRobot::CAMERA_FRAME, q) ;
- sleep(5) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, q);
+ sleep(5);
- q.resize(2) ;
+ q.resize(6);
q = 0.0;
- q[1] = vpMath::rad(2) ; // rotation arround vertical axis
+ q[1] = vpMath::rad(2); // rotation arround vertical axis
std::cout << " ry rotation: " << q[1] << std::endl;
if (control)
- robot.setVelocity(vpRobot::CAMERA_FRAME, q) ;
- sleep(5) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, q);
+ sleep(5);
std::cout << "The end" << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
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 f1023e7..51f40b3 100644
--- a/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp
+++ b/example/servo-afma4/servoAfma4Point2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,7 +49,6 @@
*/
-
/*!
\example servoAfma4Point2DArtVelocity.cpp
@@ -59,33 +59,31 @@
*/
-
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/robot/vpRobotAfma4.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -93,8 +91,7 @@
#include <visp3/blob/vpDot.h>
-int
-main()
+int main()
{
try {
// Log file creation in /tmp/$USERNAME/log.dat
@@ -109,17 +106,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -131,144 +126,139 @@ main()
std::ofstream flog(logfilename.c_str());
// vpRobotAfma4 robot ;
- vpRobotAfma4 robot ;
+ vpRobotAfma4 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
// exit(1) ;
- 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 << " 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;
+ 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 << " Use of the Afma4 robot " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
-
- vpDot dot ;
+ vpDot dot;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- //robot.getCameraParameters (cam, I);
-
- 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
+ // robot.getCameraParameters (cam, I);
- p.set_Z(1) ;
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ 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
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t articular velocity are computed") ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ p.set_Z(1);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t articular velocity are computed");
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpTRACE("Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
+ vpTRACE("Set the position of the camera in the end-effector frame ");
+ vpHomogeneousMatrix cMe;
// robot.get_cMe(cMe) ;
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// vpDisplay::getClick(I) ;
- vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.8);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.8) ;
+ vpTRACE("Display task information ");
+ task.print();
- vpTRACE("Display task information " ) ;
- task.print() ;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the cog of the dot
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
-
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
// std::cout << (vpMatrix)cVe*eJe << std::endl ;
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -278,8 +268,7 @@ 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;
@@ -289,36 +278,34 @@ 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 expressed
- // in meters in the camera frame
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
flog << task.getError() << std::endl;
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ // vpTRACE("\t\t || s - s* || = %f ", ( task.getError()
+ // ).sumSquare()) ;
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
index ce79220..b2649e4 100644
--- a/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
+++ b/example/servo-afma4/servoAfma4Point2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,8 +49,6 @@
*/
-
-
/*!
\example servoAfma4Point2DCamVelocity.cpp
@@ -60,29 +59,27 @@
*/
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/robot/vpRobotAfma4.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -90,8 +87,7 @@
#include <visp3/blob/vpDot.h>
-int
-main()
+int main()
{
try {
// Log file creation in /tmp/$USERNAME/log.dat
@@ -106,17 +102,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -127,115 +121,110 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpRobotAfma4 robot ;
- vpServo task ;
+ vpRobotAfma4 robot;
+ vpServo task;
-
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- vpDot dot ;
+ vpDot dot;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
// Get the cog of the dot
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
+ vpCameraParameters cam;
- vpCameraParameters cam ;
-
- 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
-
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ 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
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.8) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
+ vpTRACE("\t set the gain");
+ task.setLambda(0.8);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the cog of the dot
vpImagePoint cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -245,8 +234,7 @@ 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;
@@ -256,40 +244,36 @@ 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 expressed
- // in meters in the camera frame
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
flog << task.getError() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
-
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
index e9913d5..eddd152 100644
--- a/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
+++ b/example/servo-afma4/servoAfma4Point2DCamVelocityKalman.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,52 +47,43 @@
freedom). The velocity is computed in the camera frame. The visual
feature is the center of gravity of a point.
- In this example we estimate the velocity of the target in order to reduce the
- tracking error when the target is moving. The velocity of the target is
+ In this example we estimate the velocity of the target in order to reduce
+ the tracking error when the target is moving. The velocity of the target is
filtered by a Kalman filter with a constant velocity state model, or a
constant acceleration state model.
*/
-
-
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA4) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA4) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/robot/vpRobotAfma4.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
-#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/blob/vpDot2.h>
+#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpAdaptiveGain.h>
-#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
-
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
// List of allowed command line options
-#define GETOPTARGS "hK:l:"
+#define GETOPTARGS "hK:l:"
-typedef enum {
- K_NONE,
- K_VELOCITY,
- K_ACCELERATION
-} KalmanType;
+typedef enum { K_NONE, K_VELOCITY, K_ACCELERATION } KalmanType;
/*!
@@ -102,8 +94,7 @@ typedef enum {
\param kalman : Kalman state model selector.
*/
-void usage(const char *name, const char *badparam,
- KalmanType &kalman)
+void usage(const char *name, const char *badparam, KalmanType &kalman)
{
fprintf(stdout, "\n\
Tests a control law with the following characteristics:\n\
@@ -127,13 +118,12 @@ OPTIONS: Default\n\
2: acceleration model\n\
\n\
-h\n\
- Print the help.\n", (int) kalman);
+ Print the help.\n", (int)kalman);
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -143,35 +133,37 @@ Set the program options.
\param argc : Command line number of parameters.
\param argv : Array of command line parameters.
\param kalman : Kalman state model selector.
- \param doAdaptativeGain : If true use an adaptive gain. Otherwise,
+ \param doAdaptativeGain : If true use an adaptive gain. Otherwise,
the gain is constant.
\param lambda : Constant visual servo gain.
\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;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
switch (c) {
- case 'K':
- kalman = (KalmanType) atoi(optarg);
+ case 'K':
+ kalman = (KalmanType)atoi(optarg);
break;
- case 'l':
+ case 'l':
doAdaptativeGain = false;
- lambda.initFromConstant( atof(optarg) );
+ lambda.initFromConstant(atof(optarg));
+ break;
+ case 'h':
+ usage(argv[0], NULL, kalman);
+ return false;
break;
- case 'h': usage(argv[0], NULL, kalman);
- return false; break;
default:
usage(argv[0], optarg, kalman);
- return false; break;
+ return false;
+ break;
}
}
@@ -186,12 +178,11 @@ bool getOptions(int argc, const char **argv, KalmanType &kalman,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
KalmanType opt_kalman = K_NONE;
- vpAdaptiveGain lambda; // Gain de la commande
+ vpAdaptiveGain lambda; // Gain de la commande
bool doAdaptativeGain = true; // Compute adaptative gain
lambda.initStandard(4, 0.2, 40);
int opt_cam_frequency = 60; // 60 Hz
@@ -213,17 +204,15 @@ main(int argc, const char ** argv)
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -234,40 +223,48 @@ main(int argc, const char ** argv)
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
- switch(opt_cam_frequency) {
- case 15: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15); break;
- case 30: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30); break;
- case 60: g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60); break;
+ switch (opt_cam_frequency) {
+ case 15:
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_15);
+ break;
+ case 30:
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
+ break;
+ case 60:
+ g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
+ break;
}
- g.open(I) ;
+ g.open(I);
- for (int i=0; i < 10; i++) // 10 acquisition to warm up the camera
- g.acquire(I) ;
+ for (int i = 0; i < 10; i++) // 10 acquisition to warm up the camera
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << "Task : servo a point \n" << 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 << "Task : servo a point \n" << std::endl;
// Kalman filtering
- switch(opt_kalman) {
+ switch (opt_kalman) {
case K_NONE:
std::cout << "Servo with no target motion compensation (see -K option)\n";
break;
@@ -280,20 +277,20 @@ main(int argc, const char ** argv)
<< "with constant acceleration modelization (see -K option)\n";
break;
}
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- vpDot2 dot ;
+ vpDot2 dot;
std::cout << "Click on the dot..." << std::endl;
dot.setGraphics(true);
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog;
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpRobotAfma4 robot ;
+ vpRobotAfma4 robot;
double px = 1000;
double py = 1000;
@@ -303,24 +300,24 @@ main(int argc, const char ** argv)
vpCameraParameters cam(px, py, u0, v0);
// Sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p, cam, dot) ;
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, cam, dot);
// Sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we want to see a point on a point
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// - set the gain
- task.setLambda(lambda) ;
+ task.setLambda(lambda);
// Display task information
// task.print() ;
@@ -329,7 +326,7 @@ main(int argc, const char ** argv)
//!---------------------------Init Kalman Filter--------------------------
//--------------------------------------------------------------------------
- //!Initialize filter
+ //! Initialize filter
vpLinearKalmanFilterInstantiation kalman;
// Initialize the kalman filter
@@ -339,15 +336,15 @@ main(int argc, const char ** argv)
vpColVector sigma_measure(nsignal);
unsigned int state_size = 0; // Kalman state vector size
- switch(opt_kalman) {
- case K_VELOCITY: {
+ switch (opt_kalman) {
+ case K_VELOCITY: {
// Set the constant velocity state model used for the filtering
kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
state_size = kalman.getStateSize();
- sigma_state.resize(state_size*nsignal);
+ 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
+ sigma_measure = 0.05; // Same measure variance for all the signals
+ double dummy = 0; // non used parameter dt for the velocity state model
kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
break;
@@ -356,32 +353,31 @@ main(int argc, const char ** argv)
// Set the constant acceleration state model used for the filtering
kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel);
state_size = kalman.getStateSize();
- sigma_state.resize(state_size*nsignal);
+ sigma_state.resize(state_size * nsignal);
sigma_state = 0.00001; // Same variance for all the signals
- sigma_measure = 0.05; // Same measure variance for all the signals
- double dt = 1./opt_cam_frequency;
- kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt );
+ sigma_measure = 0.05; // Same measure variance for all the signals
+ double dt = 1. / opt_cam_frequency;
+ kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
break;
}
default:
break;
}
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- int iter = 0 ;
+ int iter = 0;
double t_1, Tv_0;
vpColVector vm(6), vm_0(6);
vpColVector v(6), v1(6), v2(6); // robot velocities
- //task error
+ // task error
vpColVector err(2), err_0(2), err_1(2);
vpColVector dedt_filt(2), dedt_mes(2);
-
t_1 = vpTime::measureTimeMs(); // t_1: time at previous iter
- Tv_0 = 0;
+ Tv_0 = 0;
//
// Warning: In all varaible names,
@@ -390,10 +386,11 @@ main(int argc, const char ** argv)
// _2 means the value for the previous previous iteration (t=-2)
//
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
double t_0 = vpTime::measureTimeMs(); // t_0: current time
// Temps de la boucle d'asservissement
- double Tv = (double)(t_0 - t_1) / 1000.0; //temps d'une iteration en s !
+ double Tv = (double)(t_0 - t_1) / 1000.0; // temps d'une iteration en s
+ // !
// std::cout << "time iter : " << Tv << std::endl;
// Update time for next iteration
@@ -402,17 +399,17 @@ main(int argc, const char ** argv)
vm = robot.getVelocity(vpRobot::CAMERA_FRAME);
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
vpImagePoint cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
@@ -427,25 +424,24 @@ main(int argc, const char ** argv)
Tv_0 = Tv;
// Compute the visual servoing skew vector
- v1 = task.computeControlLaw() ;
+ v1 = task.computeControlLaw();
err = task.error;
- //!terme correctif : de/dt = Delta s / Delta t - L*vc
- if (iter==0){
+ //! terme correctif : de/dt = Delta s / Delta t - L*vc
+ if (iter == 0) {
err_0 = 0;
err_1 = 0;
dedt_mes = 0;
dedt_filt = 0;
- }
- else{
+ } else {
err_1 = err_0;
err_0 = err;
- dedt_mes = (err_0 - err_1)/(Tv_1) - task.J1*vm_0;
+ dedt_mes = (err_0 - err_1) / (Tv_1)-task.J1 * vm_0;
}
//! pour corriger pb de iter = 1
- if (iter <= 1){
+ if (iter <= 1) {
dedt_mes = 0;
}
@@ -453,30 +449,29 @@ main(int argc, const char ** argv)
//----------------------- Kalman Filter Equations ----------------------
//----------------------------------------------------------------------
// Kalman filtering
- switch(opt_kalman) {
+ switch (opt_kalman) {
case K_NONE:
dedt_filt = 0;
break;
case K_VELOCITY:
case K_ACCELERATION:
kalman.filter(dedt_mes);
- for (unsigned int i=0; i < nsignal; i++) {
- dedt_filt[i] = kalman.Xest[i*state_size];
+ for (unsigned int i = 0; i < nsignal; i++) {
+ dedt_filt[i] = kalman.Xest[i * state_size];
}
break;
}
//! Modified control law
vpMatrix J1p = task.getTaskJacobianPseudoInverse();
- v2 = - J1p*dedt_filt;
+ v2 = -J1p * dedt_filt;
// std::cout << "task J1p: " << J1p.t() << std::endl ;
// std::cout << "dedt_filt: " << dedt_filt.t() << std::endl ;
v = v1 + v2;
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
-
+ vpServoDisplay::display(task, cam, I);
// std::cout << "v2 : " << v2.t() << std::endl ;
// std::cout << "v1 : " << v1.t() << std::endl ;
@@ -484,7 +479,7 @@ main(int argc, const char ** argv)
// std::cout << "v : " << v.t();
// Apply the camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// Save loop time
flog << Tv_0 << " ";
@@ -492,17 +487,15 @@ 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 expressed
- // in meters in the camera frame
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
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] << " ";
@@ -515,32 +508,28 @@ main(int argc, const char ** argv)
// Flush the display
vpDisplay::flush(I);
- iter ++;
-
+ iter++;
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
return 1;
}
-
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma4 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/CMakeLists.txt b/example/servo-afma6/CMakeLists.txt
index c904bbf..957552f 100644
--- a/example/servo-afma6/CMakeLists.txt
+++ b/example/servo-afma6/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
index 403888d..1a41a61 100644
--- a/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma62DhalfCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,9 +40,11 @@
/*!
\file servoAfma62DhalfCamVelocity.cpp
- \brief Example of eye-in-hand control law. We control here a real robot, the Afma6
- robot (cartesian robot, with 6 degrees of freedom). The velocity is computed
- in the camera frame. Visual features are given thanks to four lines and are the x and y coordinates of the rectangle center, log(Z/Z*) the current depth relative to the desired depth and the thetau rotations.
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is
+ computed in the camera frame. Visual features are given thanks to four lines
+ and are the x and y coordinates of the rectangle center, log(Z/Z*) the
+ current depth relative to the desired depth and the thetau rotations.
*/
@@ -50,38 +53,38 @@
Example of eye-in-hand control law. We control here a real robot, the Afma6
robot (cartesian robot, with 6 degrees of freedom). The velocity is computed
- in the camera frame. Visual features are given thanks to four lines and are the x and y coordinates of the rectangle center, log(Z/Z*) the current depth relative to the desired depth and the thetau rotations.
+ in the camera frame. Visual features are given thanks to four lines and are
+ the x and y coordinates of the rectangle center, log(Z/Z*) the current depth
+ relative to the desired depth and the thetau rotations.
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpGenericFeature.h>
-#include <visp3/core/vpLine.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/vision/vpPose.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -90,74 +93,70 @@
#include <visp3/vs/vpServoDisplay.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
-
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
- vpServo task ;
+ vpServo task;
- vpRobotAfma6 robot ;
- //robot.move("zero.pos") ;
+ vpRobotAfma6 robot;
+ // robot.move("zero.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
-
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a line " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
-
- int nbline =4 ;
- int nbpoint =4 ;
-
-
- vpTRACE("sets the desired position of the visual feature ") ;
- vpPoint pointd[nbpoint]; //position of the fours corners
- vpPoint pointcd; //position of the center of the square
+ robot.getCameraParameters(cam, I);
+
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a line " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
+
+ int nbline = 4;
+ int nbpoint = 4;
+
+ vpTRACE("sets the desired position of the visual feature ");
+ vpPoint pointd[nbpoint]; // position of the fours corners
+ vpPoint pointcd; // position of the center of the square
vpFeaturePoint pd;
- double L=0.05 ;
- pointd[0].setWorldCoordinates(L,-L, 0 ) ;
- pointd[1].setWorldCoordinates(L,L, 0 ) ;
- pointd[2].setWorldCoordinates(-L,L, 0 ) ;
- pointd[3].setWorldCoordinates(-L,-L, 0 ) ;
+ double L = 0.05;
+ pointd[0].setWorldCoordinates(L, -L, 0);
+ pointd[1].setWorldCoordinates(L, L, 0);
+ pointd[2].setWorldCoordinates(-L, L, 0);
+ pointd[3].setWorldCoordinates(-L, -L, 0);
- //The coordinates in the object frame of the point used as a feature ie the center of the square
- pointcd.setWorldCoordinates(0, 0, 0 ) ;
+ // The coordinates in the object frame of the point used as a feature ie
+ // the center of the square
+ pointcd.setWorldCoordinates(0, 0, 0);
- //The desired homogeneous matrix.
- vpHomogeneousMatrix cMod(0,0,0.4,0,0,vpMath::rad(10));
+ // The desired homogeneous matrix.
+ vpHomogeneousMatrix cMod(0, 0, 0.4, 0, 0, vpMath::rad(10));
pointd[0].project(cMod);
pointd[1].project(cMod);
@@ -166,229 +165,219 @@ main()
pointcd.project(cMod);
- vpFeatureBuilder::create(pd,pointcd);
+ vpFeatureBuilder::create(pd, pointcd);
- vpTRACE("Initialization of the tracking") ;
- vpMeLine line[nbline] ;
+ vpTRACE("Initialization of the tracking");
+ vpMeLine line[nbline];
vpPoint point[nbpoint];
- int i ;
+ int i;
- vpMe me ;
- me.setRange(10) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(50000) ;
+ vpMe me;
+ me.setRange(10);
+ me.setPointsToTrack(100);
+ me.setThreshold(50000);
me.setSampleStep(10);
- //Initialize the tracking. Define the four lines to track
- for (i=0 ; i < nbline ; i++)
- {
- line[i].setMe(&me) ;
+ // Initialize the tracking. Define the four lines to track
+ for (i = 0; i < nbline; i++) {
+ line[i].setMe(&me);
- line[i].initTracking(I) ;
- line[i].track(I) ;
+ line[i].initTracking(I);
+ line[i].track(I);
}
// Compute the position of the four corners. The goal is to
// compute the pose
vpImagePoint ip;
- for (i=0 ; i < nbline ; i++)
- {
- double x=0, y=0;
+ 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);
}
- vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- point[i].set_x(x) ;
- point[i].set_y(y) ;
+ point[i].set_x(x);
+ point[i].set_y(y);
}
- //Compute the pose cMo
- vpPose pose ;
- pose.clearPoint() ;
- vpHomogeneousMatrix cMo ;
+ // Compute the pose cMo
+ vpPose pose;
+ pose.clearPoint();
+ vpHomogeneousMatrix cMo;
- point[0].setWorldCoordinates(L,-L, 0 ) ;
- point[1].setWorldCoordinates(L,L, 0 ) ;
- point[2].setWorldCoordinates(-L,L, 0 ) ;
- point[3].setWorldCoordinates(-L,-L, 0 ) ;
+ point[0].setWorldCoordinates(L, -L, 0);
+ point[1].setWorldCoordinates(L, L, 0);
+ point[2].setWorldCoordinates(-L, L, 0);
+ point[3].setWorldCoordinates(-L, -L, 0);
- for (i=0 ; i < nbline ; i++)
- {
- pose.addPoint(point[i]) ; // and added to the pose computation point list
+ for (i = 0; i < nbline; i++) {
+ pose.addPoint(point[i]); // and added to the pose computation point list
}
- pose.computePose(vpPose::LAGRANGE, cMo) ;
+ pose.computePose(vpPose::LAGRANGE, cMo);
pose.computePose(vpPose::VIRTUAL_VS, cMo);
+ vpTRACE("sets the current position of the visual feature ");
- vpTRACE("sets the current position of the visual feature ") ;
-
- //The first features are the position in the camera frame x and y of the square center
- vpPoint pointc; //The current position of the center of the square
- double xc = (point[0].get_x()+point[2].get_x())/2;
- double yc = (point[0].get_y()+point[2].get_y())/2;
+ // The first features are the position in the camera frame x and y of the
+ // square center
+ vpPoint pointc; // The current position of the center of the square
+ double xc = (point[0].get_x() + point[2].get_x()) / 2;
+ double yc = (point[0].get_y() + point[2].get_y()) / 2;
pointc.set_x(xc);
pointc.set_y(yc);
vpFeaturePoint p;
pointc.project(cMo);
- vpFeatureBuilder::create(p,pointc);
+ vpFeatureBuilder::create(p, pointc);
- //The second feature is the depth of the current square center relative to the depth of the desired square center.
+ // 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.
- vpHomogeneousMatrix cdMc ;
- cdMc = cMod*cMo.inverse() ;
+ // The last three features are the rotations thetau between the current
+ // pose and the desired pose.
+ vpHomogeneousMatrix cdMc;
+ cdMc = cMod * cMo.inverse();
vpFeatureThetaU tu(vpFeatureThetaU::cdRc);
- tu.buildFrom(cdMc) ;
+ tu.buildFrom(cdMc);
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
- task.addFeature(logZ) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
+ task.addFeature(logZ);
task.addFeature(tu);
+ vpTRACE("\t set the gain");
+ task.setLambda(0.2);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.2) ;
-
+ vpTRACE("Display task information ");
+ task.print();
- vpTRACE("Display task information " ) ;
- task.print() ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
+ vpImage<vpRGBa> Ic;
+ double lambda_av = 0.05;
+ double alpha = 0.05;
+ double beta = 3;
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
- vpImage<vpRGBa> Ic ;
- double lambda_av =0.05;
- double alpha = 0.05 ;
- double beta =3 ;
-
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- pose.clearPoint() ;
+ pose.clearPoint();
- //Track the lines and find the current position of the corners
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
+ // Track the lines and find the current position of the corners
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
- line[i].display(I,vpColor::green);
+ line[i].display(I, vpColor::green);
- double x=0, y=0;
+ 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);
}
- vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
point[i].set_x(x);
point[i].set_y(y);
- pose.addPoint(point[i]) ;
+ pose.addPoint(point[i]);
}
- //Compute the pose
- pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
+ // Compute the pose
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
- //Update the two first features x and y (position of the square center)
- xc = (point[0].get_x()+point[2].get_x())/2;
- yc = (point[0].get_y()+point[2].get_y())/2;
+ // Update the two first features x and y (position of the square
+ // center)
+ xc = (point[0].get_x() + point[2].get_x()) / 2;
+ yc = (point[0].get_y() + point[2].get_y()) / 2;
pointc.set_x(xc);
pointc.set_y(yc);
pointc.project(cMo);
- vpFeatureBuilder::create(p,pointc);
- //Print the current and the desired position of the center of the square
- //Print the desired position of the four corners
- p.display(cam, I, vpColor::green) ;
- pd.display(cam, I, vpColor::red) ;
- for (i = 0; i < nbpoint; i++) 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()));
-
- //Update the last three features
- cdMc = cMod*cMo.inverse() ;
- tu.buildFrom(cdMc) ;
-
- //Adaptive gain
- double gain ;
+ vpFeatureBuilder::create(p, pointc);
+ // Print the current and the desired position of the center of the
+ // square Print the desired position of the four corners
+ p.display(cam, I, vpColor::green);
+ pd.display(cam, I, vpColor::red);
+ for (i = 0; i < nbpoint; i++)
+ 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()));
+
+ // Update the last three features
+ cdMc = cMod * cMo.inverse();
+ tu.buildFrom(cdMc);
+
+ // Adaptive gain
+ double gain;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
}
- task.setLambda(gain) ;
+ task.setLambda(gain);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- vpDisplay::flush(I) ;
- std::cout << v.sumSquare() <<std::endl ;
- if (iter==0) vpDisplay::getClick(I) ;
- if (v.sumSquare() > 0.5)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- vpDisplay::getClick(I) ;
- }
+ vpDisplay::flush(I);
+ std::cout << v.sumSquare() << std::endl;
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ if (v.sumSquare() > 0.5) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ vpDisplay::getClick(I);
+ }
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
index 587fbbd..b799376 100644
--- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,30 +51,28 @@
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpMath.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/core/vpCylinder.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -81,186 +80,176 @@
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- vpServo task ;
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
-
-
- int i ;
- int nbline =2 ;
- vpMeLine line[nbline] ;
-
- vpMe me ;
- me.setRange(10) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(30000) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ vpServo task;
+
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
+
+ int i;
+ int nbline = 2;
+ vpMeLine line[nbline];
+
+ vpMe me;
+ me.setRange(10);
+ me.setPointsToTrack(100);
+ me.setThreshold(30000);
me.setSampleStep(10);
- //Initialize the tracking of the two edges of the cylinder
- for (i=0 ; i < nbline ; i++)
- {
- line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
- line[i].setMe(&me) ;
+ // Initialize the tracking of the two edges of the cylinder
+ for (i = 0; i < nbline; i++) {
+ line[i].setDisplay(vpMeSite::RANGE_RESULT);
+ line[i].setMe(&me);
- line[i].initTracking(I) ;
- line[i].track(I) ;
+ line[i].initTracking(I);
+ line[i].track(I);
}
- vpRobotAfma6 robot ;
- //robot.move("zero.pos") ;
+ vpRobotAfma6 robot;
+ // robot.move("zero.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureLine p[nbline] ;
- for (i=0 ; i < nbline ; i++)
- vpFeatureBuilder::create(p[i],cam, line[i]) ;
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureLine p[nbline];
+ for (i = 0; i < nbline; i++)
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- vpTRACE("sets the desired position of the visual feature ") ;
- vpCylinder cyld(0,1,0,0,0,0,0.04);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpCylinder cyld(0, 1, 0, 0, 0, 0, 0.04);
- vpHomogeneousMatrix cMo(0,0,0.4,0,0,vpMath::rad(0));
+ vpHomogeneousMatrix cMo(0, 0, 0.4, 0, 0, vpMath::rad(0));
cyld.project(cMo);
- vpFeatureLine pd[nbline] ;
- vpFeatureBuilder::create(pd[0],cyld,vpCylinder::line1);
- vpFeatureBuilder::create(pd[1],cyld,vpCylinder::line2);
-
- //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
- //Another way to have the coordinates of the desired features is to learn them before executing the program.
- pd[0].setRhoTheta(-fabs(pd[0].getRho()),0);
- pd[1].setRhoTheta(-fabs(pd[1].getRho()),M_PI);
-
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpFeatureLine pd[nbline];
+ vpFeatureBuilder::create(pd[0], cyld, vpCylinder::line1);
+ vpFeatureBuilder::create(pd[1], cyld, vpCylinder::line2);
+
+ // Those lines are needed to keep the conventions define in vpMeLine
+ // (Those in vpLine are less restrictive) Another way to have the
+ // coordinates of the desired features is to learn them before executing
+ // the program.
+ pd[0].setRhoTheta(-fabs(pd[0].getRho()), 0);
+ pd[1].setRhoTheta(-fabs(pd[1].getRho()), M_PI);
+
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpTRACE("\t we want to see a two lines on two lines..") ;
- std::cout << std::endl ;
- for (i=0 ; i < nbline ; i++)
- task.addFeature(p[i],pd[i]) ;
-
- vpTRACE("\t set the gain") ;
- task.setLambda(0.2) ;
+ vpTRACE("\t we want to see a two lines on two lines..");
+ std::cout << std::endl;
+ for (i = 0; i < nbline; i++)
+ task.addFeature(p[i], pd[i]);
+ vpTRACE("\t set the gain");
+ task.setLambda(0.2);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
-
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
- vpImage<vpRGBa> Ic ;
- double lambda_av =0.05;
- double alpha = 0.2 ;
- double beta =3 ;
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
+ vpImage<vpRGBa> Ic;
+ double lambda_av = 0.05;
+ double alpha = 0.2;
+ double beta = 3;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- //Track the two edges and update the features
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
- line[i].display(I, vpColor::red) ;
+ // Track the two edges and update the features
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
+ line[i].display(I, vpColor::red);
- vpFeatureBuilder::create(p[i],cam,line[i]);
- vpTRACE("%f %f ",line[i].getRho(), line[i].getTheta()) ;
+ vpFeatureBuilder::create(p[i], cam, line[i]);
+ vpTRACE("%f %f ", line[i].getRho(), line[i].getTheta());
- p[i].display(cam, I, vpColor::red) ;
- pd[i].display(cam, I, vpColor::green) ;
- }
+ p[i].display(cam, I, vpColor::red);
+ pd[i].display(cam, I, vpColor::green);
+ }
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- //Adaptative gain
- double gain ;
+ // Adaptative gain
+ double gain;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
}
- task.setLambda(gain) ;
-
- v = task.computeControlLaw() ;
-
- if (iter==0) vpDisplay::getClick(I) ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ task.setLambda(gain);
+
+ v = task.computeControlLaw();
+
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
index 4dff8bc..96b19a9 100644
--- a/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
+++ b/example/servo-afma6/servoAfma6Cylinder2DCamVelocitySecondaryTask.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -54,30 +55,28 @@
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/core/vpMath.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/core/vpCylinder.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -85,285 +84,269 @@
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
-
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpServo task ;
+ vpServo task;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
+ int i;
+ int nbline = 2;
+ vpMeLine line[nbline];
- int i ;
- int nbline =2 ;
- vpMeLine line[nbline] ;
-
- vpMe me ;
- me.setRange(20) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(2000) ;
+ vpMe me;
+ me.setRange(20);
+ me.setPointsToTrack(100);
+ me.setThreshold(2000);
me.setSampleStep(10);
- //Initialize the tracking of the two edges of the cylinder
- for (i=0 ; i < nbline ; i++)
- {
- line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
- line[i].setMe(&me) ;
+ // Initialize the tracking of the two edges of the cylinder
+ for (i = 0; i < nbline; i++) {
+ line[i].setDisplay(vpMeSite::RANGE_RESULT);
+ line[i].setMe(&me);
- line[i].initTracking(I) ;
- line[i].track(I) ;
+ line[i].initTracking(I);
+ line[i].track(I);
}
- vpRobotAfma6 robot ;
- //robot.move("zero.pos") ;
+ vpRobotAfma6 robot;
+ // robot.move("zero.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureLine p[nbline] ;
- for (i=0 ; i < nbline ; i++)
- vpFeatureBuilder::create(p[i],cam, line[i]) ;
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureLine p[nbline];
+ for (i = 0; i < nbline; i++)
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- vpTRACE("sets the desired position of the visual feature ") ;
- vpCylinder cyld(0,1,0,0,0,0,0.04);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpCylinder cyld(0, 1, 0, 0, 0, 0, 0.04);
- vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
+ vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
cyld.project(cMo);
- vpFeatureLine pd[nbline] ;
- vpFeatureBuilder::create(pd[0],cyld,vpCylinder::line1);
- vpFeatureBuilder::create(pd[1],cyld,vpCylinder::line2);
-
- //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
- //Another way to have the coordinates of the desired features is to learn them before executing the program.
- pd[0].setRhoTheta(-fabs(pd[0].getRho()),0);
- pd[1].setRhoTheta(-fabs(pd[1].getRho()),M_PI);
-
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpFeatureLine pd[nbline];
+ vpFeatureBuilder::create(pd[0], cyld, vpCylinder::line1);
+ vpFeatureBuilder::create(pd[1], cyld, vpCylinder::line2);
+
+ // Those lines are needed to keep the conventions define in vpMeLine
+ // (Those in vpLine are less restrictive) Another way to have the
+ // coordinates of the desired features is to learn them before executing
+ // the program.
+ pd[0].setRhoTheta(-fabs(pd[0].getRho()), 0);
+ pd[1].setRhoTheta(-fabs(pd[1].getRho()), M_PI);
+
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- for (i=0 ; i < nbline ; i++)
- task.addFeature(p[i],pd[i]) ;
-
- vpTRACE("\t set the gain") ;
- task.setLambda(0.3) ;
-
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ for (i = 0; i < nbline; i++)
+ task.addFeature(p[i], pd[i]);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.3);
+ vpTRACE("Display task information ");
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
- vpImage<vpRGBa> Ic ;
- double lambda_av =0.05;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
+ vpImage<vpRGBa> Ic;
+ double lambda_av = 0.05;
double alpha = 0.02;
- double beta =3;
+ double beta = 3;
double erreur = 1;
-
- //First loop to reach the convergence position
- while(erreur > 0.00001)
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ // First loop to reach the convergence position
+ while (erreur > 0.00001) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- //Track the two edges and update the features
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
- line[i].display(I, vpColor::red) ;
+ // Track the two edges and update the features
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
+ line[i].display(I, vpColor::red);
- vpFeatureBuilder::create(p[i],cam,line[i]);
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- p[i].display(cam, I, vpColor::red) ;
- pd[i].display(cam, I, vpColor::green) ;
- }
+ p[i].display(cam, I, vpColor::red);
+ pd[i].display(cam, I, vpColor::green);
+ }
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- //Adaptative gain
- double gain ;
+ // Adaptative gain
+ double gain;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
}
- task.setLambda(gain) ;
+ task.setLambda(gain);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- if (iter==0) vpDisplay::getClick(I) ;
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- erreur = ( task.getError() ).sumSquare();
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ erreur = (task.getError()).sumSquare();
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
/**********************************************************************************************/
- //Second loop is to compute the control while taking into account the secondary task.
- vpColVector e1(6) ; e1 = 0 ;
- vpColVector e2(6) ; e2 = 0 ;
- vpColVector proj_e1 ;
- vpColVector proj_e2 ;
+ // Second loop is to compute the control while taking into account the
+ // secondary task.
+ vpColVector e1(6);
+ e1 = 0;
+ vpColVector e2(6);
+ e2 = 0;
+ vpColVector proj_e1;
+ vpColVector proj_e2;
iter = 0;
double rapport = 0;
double vitesse = 0.02;
unsigned int tempo = 1200;
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- //Track the two edges and update the features
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
- line[i].display(I, vpColor::red) ;
+ // Track the two edges and update the features
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
+ line[i].display(I, vpColor::red);
- vpFeatureBuilder::create(p[i],cam,line[i]);
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- p[i].display(cam, I, vpColor::red) ;
- pd[i].display(cam, I, vpColor::green) ;
- }
+ p[i].display(cam, I, vpColor::red);
+ pd[i].display(cam, I, vpColor::green);
+ }
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- //Compute the new control law corresponding to the secondary task
- if ( iter%tempo < 400 /*&& iter%tempo >= 0*/)
- {
+ // Compute the new control law corresponding to the secondary task
+ if (iter % tempo < 400 /*&& iter%tempo >= 0*/) {
e2 = 0;
- e1[0] = fabs(vitesse) ;
+ e1[0] = fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
- if ( iter == 199 ) iter+=200; //This line is needed to make on ly an half turn during the first cycle
+ rapport = vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
+ if (iter == 199)
+ iter += 200; // This line is needed to make on ly an half turn
+ // during the first cycle
}
- if ( iter%tempo < 600 && iter%tempo >= 400)
- {
+ if (iter % tempo < 600 && iter % tempo >= 400) {
e1 = 0;
- e2[1] = fabs(vitesse) ;
+ e2[1] = fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
- if ( iter%tempo < 1000 && iter%tempo >= 600)
- {
+ if (iter % tempo < 1000 && iter % tempo >= 600) {
e2 = 0;
- e1[0] = -fabs(vitesse) ;
+ e1[0] = -fabs(vitesse);
proj_e1 = task.secondaryTask(e1);
- rapport = -vitesse/proj_e1[0];
- proj_e1 *= rapport ;
- v += proj_e1 ;
+ rapport = -vitesse / proj_e1[0];
+ proj_e1 *= rapport;
+ v += proj_e1;
}
- if ( iter%tempo < 1200 && iter%tempo >= 1000)
- {
+ if (iter % tempo < 1200 && iter % tempo >= 1000) {
e1 = 0;
- e2[1] = -fabs(vitesse) ;
+ e2[1] = -fabs(vitesse);
proj_e2 = task.secondaryTask(e2);
- rapport = -vitesse/proj_e2[1];
- proj_e2 *= rapport ;
- v += proj_e2 ;
+ rapport = -vitesse / proj_e2[1];
+ proj_e2 *= rapport;
+ v += proj_e2;
}
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
-
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
index 90158c1..68c590a 100644
--- a/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Ellipse2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,17 +38,15 @@
*
*****************************************************************************/
-
/*!
\file servoAfma6Ellipse2DCamVelocity.cpp
- \brief Example of eye-in-hand control law. We control here a real robot, the Afma6
- robot (cartesian robot, with 6 degrees of freedom). The velocity is computed
- in the camera frame. The used visual feature is a circle.
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is
+ computed in the camera frame. The used visual feature is a circle.
*/
-
/*!
\example servoAfma6Ellipse2DCamVelocity.cpp
@@ -57,26 +56,25 @@
*/
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -86,168 +84,157 @@
#include <visp3/blob/vpDot.h>
-
-
-int
-main()
+int main()
{
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
- g.acquire(I) ;
+ g.open(I);
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- vpDot dot ;
+ vpDot dot;
- dot.setMaxDotSize(0.30) ; // Max dot size is 30 % of the image size
+ dot.setMaxDotSize(0.30); // Max dot size is 30 % of the image size
// dot.setGraphics(true) ;
- dot.setComputeMoments(true) ;
+ dot.setComputeMoments(true);
std::cout << "Click on an ellipse..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- dot.track(I) ;
+ dot.track(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
// Update camera parameters
- robot.getCameraParameters (cam, I);
-
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureEllipse c ;
- vpFeatureBuilder::create(c, cam, dot) ;
-
-
- std::cout << " Learning 0/1 " <<std::endl ;
- int learning ;
- std::cin >> learning ;
- char name[FILENAME_MAX] ;
- sprintf(name,"dat/ellipse.dat") ;
- if (learning ==1)
- {
+ robot.getCameraParameters(cam, I);
+
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureEllipse c;
+ vpFeatureBuilder::create(c, cam, dot);
+
+ std::cout << " Learning 0/1 " << std::endl;
+ int learning;
+ std::cin >> learning;
+ char name[FILENAME_MAX];
+ sprintf(name, "dat/ellipse.dat");
+ if (learning == 1) {
// save the object position
- vpTRACE("Save the location of the object in a file dat/ellipse.dat") ;
- std::ofstream f(name) ;
- f << c.get_s().t() ;
- f.close() ;
- exit(1) ;
+ vpTRACE("Save the location of the object in a file dat/ellipse.dat");
+ std::ofstream f(name);
+ f << c.get_s().t();
+ f.close();
+ exit(1);
}
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeatureEllipse cd;
+ std::ifstream f("dat/ellipse.dat");
+ double x, y, mu20, mu11, mu02;
+ f >> x;
+ f >> y;
+ f >> mu20;
+ f >> mu11;
+ f >> mu02;
+ f.close();
+ cd.buildFrom(x, y, mu20, mu11, mu02);
+ cd.setABC(0, 0, 10);
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeatureEllipse cd ;
- std::ifstream f("dat/ellipse.dat") ;
- double x,y,mu20,mu11,mu02 ;
- f >> x ; f >> y ; f >> mu20 ; f >> mu11 ; f >> mu02 ;
- f.close() ;
- cd.buildFrom(x,y,mu20,mu11,mu02) ;
- cd.setABC(0,0,10) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
+ task.addFeature(c, cd);
- task.addFeature(c,cd) ;
+ task.setLambda(0.01);
- task.setLambda(0.01) ;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
- unsigned int iter=0 ;
- double lambda_av =0.01;
- double alpha = 0.1 ; //1 ;
- double beta =3 ; //3 ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ unsigned int iter = 0;
+ double lambda_av = 0.01;
+ double alpha = 0.1; // 1 ;
+ double beta = 3; // 3 ;
std::cout << "alpha 0.7" << std::endl;
- std::cin >> alpha ;
+ std::cin >> alpha;
std::cout << "beta 5" << std::endl;
- std::cin >> beta ;
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter++ <<std::endl ;
+ std::cin >> beta;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter++ << std::endl;
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- dot.track(I) ;
+ dot.track(I);
// Get the dot cog
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
- vpFeatureBuilder::create(c,cam, dot);
+ vpFeatureBuilder::create(c, cam, dot);
// Compute the adaptative gain (speed up the convergence)
- double gain ;
- if (iter>2)
- {
+ double gain;
+ if (iter > 2) {
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
- }
- else gain = lambda_av ;
-
-
- vpTRACE("%f %f", ( task.getError() ).sumSquare(), gain) ;
- task.setLambda(gain) ;
- vpColVector v ;
- v = task.computeControlLaw() ;
- std::cout <<"rank " << task.getTaskRank() << std::endl ;
- vpServoDisplay::display(task,cam,I) ;
- std::cout << v.t() ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
- vpDisplay::flush(I) ;
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ } else
+ gain = lambda_av;
+
+ vpTRACE("%f %f", (task.getError()).sumSquare(), gain);
+ task.setLambda(gain);
+ vpColVector v;
+ v = task.computeControlLaw();
+ std::cout << "rank " << task.getTaskRank() << std::endl;
+ vpServoDisplay::display(task, cam, I);
+ std::cout << v.t();
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+ vpDisplay::flush(I);
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
index 7e1dc3e..636e7da 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,7 +48,6 @@
*/
-
/*!
\example servoAfma6FourPoints2DArtVelocity.cpp
@@ -57,33 +57,31 @@
*/
-
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -91,8 +89,7 @@
#include <visp3/blob/vpDot.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -106,17 +103,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -128,164 +123,160 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- // Define the square CAD model
- // Square dimention
+// Define the square CAD model
+// Square dimention
#define L 0.075
- // Distance between the camera and the square at the desired
- // position after visual servoing convergence
+// Distance between the camera and the square at the desired
+// position after visual servoing convergence
#define D 0.5
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- 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 << " 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 << std::endl ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ 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 << " 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 << std::endl;
- vpDot dot[4] ;
+ 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) ;
+ for (i = 0; i < 4; i++) {
+ dot[i].initTracking(I);
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// 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
+ 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
// sets the desired position of the visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
- pd[0].buildFrom(-L,-L,D) ;
- pd[1].buildFrom(L,-L,D) ;
- pd[2].buildFrom(L,L,D) ;
- pd[3].buildFrom(-L,L,D) ;
+ pd[0].buildFrom(-L, -L, D);
+ pd[1].buildFrom(L, -L, D);
+ pd[2].buildFrom(L, L, D);
+ pd[3].buildFrom(-L, L, D);
// We want to see a point on a point
- std::cout << std::endl ;
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ std::cout << std::endl;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.2) ;
+ task.setLambda(0.2);
// Display task information
- task.print() ;
+ task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
+ task.print();
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- task.set_cVe(cVe) ;
- task.print() ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ task.set_cVe(cVe);
+ task.print();
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
- task.print() ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Get the dot cog
cog = dot[i].getCog();
// Display a green cross at the center of gravity position in the
// image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- exit(1) ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ exit(1);
}
// Update the point feature from the dot location
- for (i=0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],cam, dot[i]);
+ for (i = 0; i < 4; i++)
+ vpFeatureBuilder::create(p[i], cam, dot[i]);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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 @@ 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,40 +296,37 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ // vpTRACE("\t\t || s - s* || = %f ", ( task.getError()
+ //).sumSquare()) ;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
-
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
index 6505166..9190185 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,13 +44,12 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is
- computed in the camera frame. Visual features are the image coordinates of 4
- vpDot2 points. The interaction matrix is computed using the current visual
+ computed in the camera frame. Visual features are the image coordinates of
+ 4 vpDot2 points. The interaction matrix is computed using the current visual
features.
*/
-
/*!
\example servoAfma6FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -61,42 +61,39 @@
*/
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/blob/vpDot.h>
-#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpIoTools.h>
// Exception
#include <visp3/core/vpException.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
@@ -122,35 +119,32 @@
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+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
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpRotationMatrix cRo;
vpPose pose;
vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
+ for (int i = 0; i < ndot; i++) {
- double x=0, y=0;
+ double x = 0, y = 0;
cog = dot[i].getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ; //pixel to meter conversion
+ vpPixelMeterConversion::convertPoint(cam, cog, x,
+ y); // pixel to meter conversion
// std::cout << "point cam: " << i << x << " " << y << std::endl;
- point[i].set_x(x) ;//projection perspective p
- point[i].set_y(y) ;
- pose.addPoint(point[i]) ;
+ point[i].set_x(x); // projection perspective p
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
// std::cout << "point " << i << std::endl;
// point[i].print();
-
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
- //compute the pose for a given method
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
+ // compute the pose for a given method
// cMo_dementhon.extract(cto);
// cMo_dementhon.extract(cRo);
// cro.buildFrom(cRo);
@@ -168,7 +162,7 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
// << "cdro[2] = vpMath::rad(" << vpMath::deg(cro[2]) << ");\n"
// << std::endl;
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
// cMo_lagrange.extract(cto);
// cMo_lagrange.extract(cRo);
// cro.buildFrom(cRo);
@@ -187,18 +181,18 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
// cout << "Lagrange residual term: " << residual_lagrange <<endl ;
// Select the best pose to initialize the lowe pose computation
- if (residual_lagrange < residual_dementhon) //on garde le cMo
+ if (residual_lagrange < residual_dementhon) // on garde le cMo
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
- // cout <<"------------------------------------------------------------"<<endl
- }
- else { // init = false; use of the previous pose to initialise LOWE
+ // cout
+ // <<"------------------------------------------------------------"<<endl
+ } else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
- pose.computePose(vpPose::LOWE, cMo) ;
+ pose.computePose(vpPose::LOWE, cMo);
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
@@ -218,8 +212,7 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
// std::cout << cMo << std::endl ;
}
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -235,17 +228,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -256,96 +247,93 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- g.acquire(I) ;
+ g.acquire(I);
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " 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 << 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 << " 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 << std::endl;
-
- vpDot2 dot[4] ;
+ vpDot2 dot[4];
vpImagePoint cog;
- 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) ;
+ 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();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters::vpCameraParametersProjType
- projModel = vpCameraParameters::perspectiveProjWithDistortion;
+ vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithDistortion;
vpRobotAfma6 robot;
// Load the end-effector to camera frame transformation obtained
// using a camera intrinsic model with distortion
robot.init(vpAfma6::TOOL_CCMOP, projModel);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.7); // tz = 0.7 meter
- vpRxyzVector cro(vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // No rotations
- vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ vpRxyzVector cro(vpMath::rad(0), vpMath::rad(0),
+ vpMath::rad(0)); // No rotations
+ vpRotationMatrix cRo(cro); // Build the rotation matrix
+ cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
@@ -353,46 +341,46 @@ main()
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// - Interaction matrix is computed with the current visual features
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
// We want to see a point on a point
- std::cout << std::endl ;
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ std::cout << std::endl;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.1) ;
+ task.setLambda(0.1);
// Display task information
- task.print() ;
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- // Initialise the pose using Lagrange and Dementhon methods, chose the best
- // estimated pose (either Lagrange or Dementhon) and than compute the pose
- // using LOWE method with Lagrange or Dementhon pose as initialisation.
- // compute_pose(point, dot, 4, cam, cMo, cto, cro, true);
+ // Initialise the pose using Lagrange and Dementhon methods, chose the
+ // best estimated pose (either Lagrange or Dementhon) and than compute the
+ // pose using LOWE method with Lagrange or Dementhon pose as
+ // initialisation. compute_pose(point, dot, 4, cam, cMo, cto, cro, true);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Get the dot cog
cog = dot[i].getCog();
// Display a green cross at the center of gravity position in the
// image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
// During the servo, we compute the pose using LOWE method. For the
@@ -400,12 +388,12 @@ main()
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
@@ -413,21 +401,20 @@ main()
// Printing on stdout concerning task information
// task.print() ;
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
vpServoDisplay::display(task, cam, I);
// Apply the computed camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -437,8 +424,7 @@ 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;
@@ -448,47 +434,44 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t()<< " "; // s-s* for points
+ // 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 points
// 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
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...) {
- flog.close() ; // Close the log file
+ } catch (...) {
+ flog.close(); // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
-
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
index 2f0b641..7d4b0a4 100644
--- a/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
+++ b/example/servo-afma6/servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,13 +44,12 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is
- computed in the camera frame. Visual features are the image coordinates of 4
- vpDot2 points. The interaction matrix is computed using the desired visual
+ computed in the camera frame. Visual features are the image coordinates of
+ 4 vpDot2 points. The interaction matrix is computed using the desired visual
features.
*/
-
/*!
\example servoAfma6FourPoints2DCamVelocityInteractionDesired.cpp
@@ -61,42 +61,39 @@
*/
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/blob/vpDot.h>
-#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/core/vpIoTools.h>
// Exception
#include <visp3/core/vpException.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -110,17 +107,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -131,98 +126,95 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- g.acquire(I) ;
+ g.acquire(I);
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " Use of the Afma6 robot " << std::endl ;
- std::cout << " Interaction matrix computed with the desired features " << 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 << " Use of the Afma6 robot " << 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 << 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] ;
+ vpDot2 dot[4];
vpImagePoint cog;
- 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) ;
+ 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();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
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
robot.init(vpAfma6::TOOL_CCMOP, projModel);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.7); // tz = 0.7 meter
- vpRxyzVector cro(vpMath::rad(0), vpMath::rad(0), vpMath::rad(0)); // No rotations
- vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ vpRxyzVector cro(vpMath::rad(0), vpMath::rad(0),
+ vpMath::rad(0)); // No rotations
+ vpRotationMatrix cRo(cro); // Build the rotation matrix
+ cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
@@ -230,65 +222,64 @@ main()
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
// - Interaction matrix is computed with the desired visual features
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
// We want to see a point on a point
- std::cout << std::endl ;
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ std::cout << std::endl;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.4) ;
+ task.setLambda(0.4);
// Display task information
- task.print() ;
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Get the dot cog
cog = dot[i].getCog();
// Display a green cross at the center of gravity position in the
// image
- vpDisplay::displayCross(I,cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
// Printing on stdout concerning task information
// task.print() ;
// Update the point feature from the dot location
- for (i=0 ; i < 4 ; i++)
+ for (i = 0; i < 4; i++)
vpFeatureBuilder::create(p[i], cam, dot[i]);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
vpServoDisplay::display(task, cam, I);
// Apply the computed camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -297,9 +288,8 @@ main()
// - qvel[0], qvel[1], qvel[2] correspond to measured camera translation
// 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] << " ";
+ // velocities in rad/s
+ flog << qvel[0] << " " << qvel[1] << " " << qvel[2] << " " << qvel[3] << " " << qvel[4] << " " << qvel[5] << " ";
// Get the measured joint positions of the robot
vpColVector q;
@@ -309,41 +299,38 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...) {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
-
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
index dd15e80..c1eb50c 100644
--- a/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Line2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,7 +45,6 @@
computed in the camera frame. The visual feature is a line.
*/
-
/*!
\example servoAfma6Line2DCamVelocity.cpp
@@ -53,27 +53,25 @@
in the camera frame. The visual feature is a line.
*/
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -81,159 +79,147 @@
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpServo task ;
+ vpServo task;
+ 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a line " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a line " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ vpMeLine line;
-
- vpMeLine line ;
-
- vpMe me ;
- me.setRange(10) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(100000) ;
+ vpMe me;
+ me.setRange(10);
+ me.setPointsToTrack(100);
+ me.setThreshold(100000);
me.setSampleStep(10);
- line.setDisplay(vpMeSite::RANGE_RESULT) ;
+ line.setDisplay(vpMeSite::RANGE_RESULT);
+ line.setMe(&me);
- line.setMe(&me) ;
+ // Initialize the tracking. Define the line to track.
+ line.initTracking(I);
+ line.track(I);
- //Initialize the tracking. Define the line to track.
- line.initTracking(I) ;
- line.track(I) ;
-
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
// robot.move("pos-init.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureLine p ;
- vpFeatureBuilder::create(p,cam, line) ;
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureLine p;
+ vpFeatureBuilder::create(p, cam, line);
- vpTRACE("sets the desired position of the visual feature ") ;
+ vpTRACE("sets the desired position of the visual feature ");
vpLine lined;
- lined.setWorldCoordinates(1,0,0,0,0,0,1,0);
- vpHomogeneousMatrix cMo(0,0,0.3,0,0,vpMath::rad(0));
+ lined.setWorldCoordinates(1, 0, 0, 0, 0, 0, 1, 0);
+ vpHomogeneousMatrix cMo(0, 0, 0.3, 0, 0, vpMath::rad(0));
lined.project(cMo);
lined.setRho(-fabs(lined.getRho()));
lined.setTheta(0);
- vpFeatureLine pd ;
- vpFeatureBuilder::create(pd,lined);
-
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpFeatureLine pd;
+ vpFeatureBuilder::create(pd, lined);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.2) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
+ vpTRACE("\t set the gain");
+ task.setLambda(0.2);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
-
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
-
- //Track the line
- line.track(I) ;
- line.display(I, vpColor::red) ;
-
- //Update the current line feature
- vpFeatureBuilder::create(p,cam,line);
-
- //displqy the current and the desired features
- p.display(cam, I, vpColor::red) ;
- pd.display(cam, I, vpColor::green) ;
-
- v = task.computeControlLaw() ;
-
- vpDisplay::flush(I) ;
- if (iter==0) vpDisplay::getClick(I) ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ g.acquire(I);
+ vpDisplay::display(I);
+
+ // Track the line
+ line.track(I);
+ line.display(I, vpColor::red);
+
+ // Update the current line feature
+ vpFeatureBuilder::create(p, cam, line);
+
+ // displqy the current and the desired features
+ p.display(cam, I, vpColor::red);
+ pd.display(cam, I, vpColor::green);
+
+ v = task.computeControlLaw();
+
+ vpDisplay::flush(I);
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
index 4c670a1..0a195c5 100644
--- a/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Point2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,7 +49,6 @@
*/
-
/*!
\example servoAfma6Point2DArtVelocity.cpp
@@ -58,33 +58,31 @@
*/
-
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -92,8 +90,7 @@
#include <visp3/blob/vpDot.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -107,17 +104,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -129,143 +124,139 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
// exit(1) ;
- 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 << " 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;
+ 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 << " Use of the Afma6 robot " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- vpDot dot ;
+ vpDot dot;
vpImagePoint cog;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
// Get the dot cog
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- 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
+ 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
- p.set_Z(1) ;
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ p.set_Z(1);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t articular velocity are computed") ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t articular velocity are computed");
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
-
- vpTRACE("Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
+ vpTRACE("Set the position of the camera in the end-effector frame ");
+ vpHomogeneousMatrix cMe;
// robot.get_cMe(cMe) ;
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// vpDisplay::getClick(I) ;
- vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
+ vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.8) ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.8);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the dot cog
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
// std::cout << (vpMatrix)cVe*eJe << std::endl ;
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -275,8 +266,7 @@ 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;
@@ -286,38 +276,35 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
- vpDisplay::flush(I) ;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
+ vpDisplay::flush(I);
- // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ // vpTRACE("\t\t || s - s* || = %f ", ( task.getError()
+ // ).sumSquare()) ;
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
index 98d51c8..1d7db67 100644
--- a/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Point2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,8 +49,6 @@
*/
-
-
/*!
\example servoAfma6Point2DCamVelocity.cpp
@@ -59,34 +58,31 @@
*/
-
-
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -100,17 +96,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -121,106 +115,104 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpDot dot ;
+ vpDot dot;
vpImagePoint cog;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
+ vpFeaturePoint p;
// retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// - set the constant gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
// Now the robot will be controlled in velocity
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the dot cog
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -230,8 +222,7 @@ 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;
@@ -241,42 +232,37 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
-
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
index cbab4e5..83c0af0 100644
--- a/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
+++ b/example/servo-afma6/servoAfma6Points2DCamVelocityEyeToHand.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,366 +46,332 @@
*/
-
/*!
\example servoAfma6Points2DCamVelocityEyeToHand.cpp
- Example of a eye-to-hand control law. We control here a real robot, the Afma6
- robot (cartesian robot, with 6 degrees of freedom). The robot is controlled
- in the camera frame.
+ Example of a eye-to-hand control law. We control here a real robot, the
+ Afma6 robot (cartesian robot, with 6 degrees of freedom). The robot is
+ controlled in the camera frame.
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <list>
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <list>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
#define SAVE 0
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpException.h>
-#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/vision/vpPose.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
#define L 0.006
#define D 0
int main()
{
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpCameraParameters cam ;
- vpImage<unsigned char> I ;
- int i ;
+ vpCameraParameters cam;
+ vpImage<unsigned char> I;
+ int i;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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;
+ 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;
- int nbPoint =7 ;
+ int nbPoint = 7;
- vpDot dot[nbPoint] ;
+ vpDot dot[nbPoint];
vpImagePoint cog;
- for (i=0 ; i < nbPoint ; i++)
- {
- dot[i].initTracking(I) ;
- dot[i].setGraphics(true) ;
- dot[i].track(I) ;
- vpDisplay::flush(I) ;
- dot[i].setGraphics(false) ;
+ for (i = 0; i < nbPoint; i++) {
+ dot[i].initTracking(I);
+ dot[i].setGraphics(true);
+ dot[i].track(I);
+ vpDisplay::flush(I);
+ dot[i].setGraphics(false);
}
// Compute the pose 3D model
- vpPoint point[nbPoint] ;
- point[0].setWorldCoordinates(-2*L,D, -3*L) ;
- point[1].setWorldCoordinates(0,D, -3*L) ;
- point[2].setWorldCoordinates(2*L,D, -3*L) ;
+ vpPoint point[nbPoint];
+ point[0].setWorldCoordinates(-2 * L, D, -3 * L);
+ point[1].setWorldCoordinates(0, D, -3 * L);
+ point[2].setWorldCoordinates(2 * L, D, -3 * L);
- point[3].setWorldCoordinates(-L,D,-L) ;
- point[4].setWorldCoordinates(L,D, -L) ;
- point[5].setWorldCoordinates(L,D, L) ;
- point[6].setWorldCoordinates(-L,D, L) ;
+ point[3].setWorldCoordinates(-L, D, -L);
+ point[4].setWorldCoordinates(L, D, -L);
+ point[5].setWorldCoordinates(L, D, L);
+ point[6].setWorldCoordinates(-L, D, L);
- vpRobotAfma6 robot ;
+ vpRobotAfma6 robot;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpHomogeneousMatrix cMo, cdMo ;
- vpPose pose ;
- pose.clearPoint() ;
- for (i=0 ; i < nbPoint ; i++)
- {
+ vpHomogeneousMatrix cMo, cdMo;
+ vpPose pose;
+ pose.clearPoint();
+ for (i = 0; i < nbPoint; i++) {
cog = dot[i].getCog();
- double x=0, y=0;
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
- point[i].set_x(x) ;
- point[i].set_y(y) ;
- pose.addPoint(point[i]) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
+ point[i].set_x(x);
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
}
- // compute the initial pose using Dementhon method followed by a non linear
- // minimisation method
- pose.computePose(vpPose::DEMENTHON_LOWE, cMo) ;
+ // compute the initial pose using Dementhon method followed by a non
+ // linear minimisation method
+ pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
-
- std::cout << cMo << std::endl ;
- cMo.print() ;
+ std::cout << cMo << std::endl;
+ cMo.print();
/*------------------------------------------------------------------
-- Learning the desired position
-- or reading the desired position
------------------------------------------------------------------
*/
- std::cout << " Learning 0/1 " <<std::endl ;
- char name[FILENAME_MAX] ;
- sprintf(name,"cdMo.dat") ;
- int learning ;
- std::cin >> learning ;
- if (learning ==1)
- {
+ std::cout << " Learning 0/1 " << std::endl;
+ char name[FILENAME_MAX];
+ sprintf(name, "cdMo.dat");
+ int learning;
+ std::cin >> learning;
+ if (learning == 1) {
// save the object position
- vpTRACE("Save the location of the object in a file cdMo.dat") ;
- std::ofstream f(name) ;
- cMo.save(f) ;
- f.close() ;
- exit(1) ;
+ vpTRACE("Save the location of the object in a file cdMo.dat");
+ std::ofstream f(name);
+ cMo.save(f);
+ f.close();
+ exit(1);
}
-
{
- vpTRACE("Loading desired location from cdMo.dat") ;
- std::ifstream f("cdMo.dat") ;
- cdMo.load(f) ;
- f.close() ;
+ vpTRACE("Loading desired location from cdMo.dat");
+ std::ifstream f("cdMo.dat");
+ cdMo.load(f);
+ f.close();
}
- vpFeaturePoint p[nbPoint], pd[nbPoint] ;
+ vpFeaturePoint p[nbPoint], pd[nbPoint];
// set the desired position of the point by forward projection using
// the pose cdMo
- for (i=0 ; i < nbPoint ; i++)
- {
- vpColVector cP, p ;
- point[i].changeFrame(cdMo, cP) ;
- point[i].projection(cP, p) ;
+ for (i = 0; i < nbPoint; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cdMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
}
-
-
//------------------------------------------------------------------
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYETOHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::CURRENT) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYETOHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::CURRENT);
-
- for (i=0 ; i < nbPoint ; i++)
- {
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < nbPoint; i++) {
+ task.addFeature(p[i], pd[i]);
}
-
- vpTRACE("Display task information " ) ;
- task.print() ;
-
+ vpTRACE("Display task information ");
+ task.print();
//------------------------------------------------------------------
-
- double convergence_threshold = 0.00; //025 ;
- vpDisplay::getClick(I) ;
+ double convergence_threshold = 0.00; // 025 ;
+ vpDisplay::getClick(I);
//-------------------------------------------------------------
- double error =1 ;
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
- vpColVector v ; // computed robot velocity
-
+ double error = 1;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ vpColVector v; // computed robot velocity
// position of the object in the effector frame
- vpHomogeneousMatrix oMcamrobot ;
- oMcamrobot[0][3] = -0.05 ;
+ vpHomogeneousMatrix oMcamrobot;
+ oMcamrobot[0][3] = -0.05;
- vpImage<vpRGBa> Ic ;
- int it = 0 ;
+ vpImage<vpRGBa> Ic;
+ int it = 0;
- double lambda_av =0.1;
- double alpha = 1 ; //1 ;
- double beta =3 ; //3 ;
+ double lambda_av = 0.1;
+ double alpha = 1; // 1 ;
+ double beta = 3; // 3 ;
std::cout << "alpha 0.7" << std::endl;
- std::cin >> alpha ;
+ std::cin >> alpha;
std::cout << "beta 5" << std::endl;
- std::cin >> beta ;
- std::list<vpImagePoint> Lcog ;
+ std::cin >> beta;
+ std::list<vpImagePoint> Lcog;
vpImagePoint ip;
- while(error > convergence_threshold)
- {
- 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) ;
- ip.set_i( 280 );
- ip.set_j( 150 );
- vpDisplay::displayText(I, ip, "IRISA-INRIA Rennes, Lagadic project", vpColor::green) ;
- try
- {
- for (i=0 ; i < nbPoint ; i++)
- {
- dot[i].track(I) ;
- Lcog.push_back( dot[i].getCog() );
- }
- }
- catch(...)
- {
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- exit(1) ;
- }
+ while (error > convergence_threshold) {
+ 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);
+ ip.set_i(280);
+ ip.set_j(150);
+ vpDisplay::displayText(I, ip, "IRISA-INRIA Rennes, Lagadic project", vpColor::green);
+ try {
+ for (i = 0; i < nbPoint; i++) {
+ dot[i].track(I);
+ Lcog.push_back(dot[i].getCog());
+ }
+ } catch (...) {
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ exit(1);
+ }
// compute the initial pose using a non linear minimisation method
- pose.clearPoint() ;
+ pose.clearPoint();
- for (i=0 ; i < nbPoint ; i++)
- {
- double x=0, y=0;
- cog = dot[i].getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
- point[i].set_x(x) ;
- point[i].set_y(y) ;
+ for (i = 0; i < nbPoint; i++) {
+ double x = 0, y = 0;
+ cog = dot[i].getCog();
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
+ point[i].set_x(x);
+ point[i].set_y(y);
- vpColVector cP ;
- point[i].changeFrame(cdMo, cP) ;
+ vpColVector cP;
+ point[i].changeFrame(cdMo, cP);
- p[i].set_x(x) ;
- p[i].set_y(y) ;
- p[i].set_Z(cP[2]) ;
+ p[i].set_x(x);
+ p[i].set_y(y);
+ p[i].set_Z(cP[2]);
- pose.addPoint(point[i]) ;
+ pose.addPoint(point[i]);
- point[i].display(I,cMo,cam, vpColor::green) ;
- point[i].display(I,cdMo,cam, vpColor::blue) ;
- }
- pose.computePose(vpPose::LOWE, cMo) ;
- vpDisplay::flush(I) ;
+ point[i].display(I, cMo, cam, vpColor::green);
+ point[i].display(I, cdMo, cam, vpColor::blue);
+ }
+ pose.computePose(vpPose::LOWE, cMo);
+ vpDisplay::flush(I);
//! set up the Jacobian
- vpHomogeneousMatrix cMe, camrobotMe ;
- robot.get_cMe(camrobotMe) ;
- cMe = cMo *oMcamrobot * camrobotMe ;
-
+ vpHomogeneousMatrix cMe, camrobotMe;
+ robot.get_cMe(camrobotMe);
+ cMe = cMo * oMcamrobot * camrobotMe;
- task.set_cVe(cMe) ;
-
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ task.set_cVe(cMe);
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// Compute the adaptative gain (speed up the convergence)
- double gain ;
- if (iter>2)
- {
+ double gain;
+ if (iter > 2) {
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
- }
- else gain = lambda_av ;
- if (SAVE==1)
- gain = gain/5 ;
+ } else
+ gain = lambda_av;
+ if (SAVE == 1)
+ gain = gain / 5;
- vpTRACE("%f %f %f %f %f",alpha, beta, lambda_av, ( task.getError() ).sumSquare(), gain) ;
- task.setLambda(gain) ;
+ vpTRACE("%f %f %f %f %f", alpha, beta, lambda_av, (task.getError()).sumSquare(), gain);
+ task.setLambda(gain);
-
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// display points trajectory
- for (std::list<vpImagePoint>::const_iterator it_cog = Lcog.begin(); it_cog != Lcog.end(); ++it_cog)
- {
- vpDisplay::displayPoint(I, *it_cog, vpColor::red) ;
+ for (std::list<vpImagePoint>::const_iterator it_cog = Lcog.begin(); it_cog != Lcog.end(); ++it_cog) {
+ vpDisplay::displayPoint(I, *it_cog, vpColor::red);
}
- vpServoDisplay::display(task,cam,I) ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ vpServoDisplay::display(task, cam, I);
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
- error = ( task.getError() ).sumSquare() ;
- std::cout << "|| s - s* || = "<< error<<std::endl ;
+ error = (task.getError()).sumSquare();
+ std::cout << "|| s - s* || = " << error << std::endl;
- if (error>7)
- {
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- exit(1) ;
- }
+ if (error > 7) {
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ exit(1);
+ }
// display the pose
// pose.display(I,cMo,cam, 0.04, vpColor::red) ;
// display the pose
// pose.display(I,cdMo,cam, 0.04, vpColor::blue) ;
- if ((SAVE==1) && (iter %3==0))
- {
+ if ((SAVE == 1) && (iter % 3 == 0)) {
- vpDisplay::getImage(I,Ic) ;
- sprintf(name,"/tmp/marchand/image.%04d.ppm",it++) ;
- vpImageIo::write(Ic,name) ;
- }
+ vpDisplay::getImage(I, Ic);
+ sprintf(name, "/tmp/marchand/image.%04d.ppm", it++);
+ vpImageIo::write(Ic, name);
+ }
}
- v = 0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- vpDisplay::getClick(I) ;
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ vpDisplay::getClick(I);
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
index 49848f3..87f447c 100644
--- a/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6Segment2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,34 +47,32 @@
*/
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
#include <stdlib.h>
#include <vector>
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -87,17 +86,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -108,140 +105,134 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- try
- {
- vpServo task ;
+ try {
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- std::vector<vpDot> dot_d(2), dot(2) ;
- vpFeatureSegment seg_d,seg ;
+ std::vector<vpDot> dot_d(2), dot(2);
+ vpFeatureSegment seg_d, seg;
vpImagePoint cog;
- vpRobotAfma6 robot ;
- vpCameraParameters cam ;
+ vpRobotAfma6 robot;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
std::cout << "define the initial segment" << std::endl;
- for(std::vector<vpDot>::iterator i = dot.begin();i!=dot.end(); ++i){
+ for (std::vector<vpDot>::iterator i = dot.begin(); i != dot.end(); ++i) {
std::cout << "Click on a dot..." << std::endl;
- i->initTracking(I) ;
+ i->initTracking(I);
cog = i->getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- }
- vpFeatureBuilder::create(seg, cam, dot[0], dot[1]);
- seg.display(cam,I,vpColor::red);
+ }
+ vpFeatureBuilder::create(seg, cam, dot[0], dot[1]);
+ seg.display(cam, I, vpColor::red);
vpDisplay::flush(I);
std::cout << "define the destination segment" << std::endl;
- for(std::vector<vpDot>::iterator i = dot_d.begin();i!=dot_d.end(); ++i){
+ for (std::vector<vpDot>::iterator i = dot_d.begin(); i != dot_d.end(); ++i) {
vpImagePoint ip;
- vpDisplay::getClick(I,ip);
+ vpDisplay::getClick(I, ip);
*i = vpDot(ip);
- vpDisplay::displayCross(I, ip, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, ip, 10, vpColor::green);
}
vpFeatureBuilder::create(seg_d, cam, dot_d[0], dot_d[1]);
- seg_d.setZ1( 1. );
- seg_d.setZ2( 1. );
+ seg_d.setZ1(1.);
+ seg_d.setZ2(1.);
- seg_d.display(cam,I);
+ seg_d.display(cam, I);
vpDisplay::flush(I);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::DESIRED);
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED);
// - we want to see both segments
- task.addFeature(seg, seg_d) ;
+ task.addFeature(seg, seg_d);
// - set the constant gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
// Now the robot will be controlled in velocity
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- for(std::vector<vpDot>::iterator i = dot.begin();i!=dot.end(); ++i){
- i->track(I) ;
+ for (std::vector<vpDot>::iterator i = dot.begin(); i != dot.end(); ++i) {
+ i->track(I);
}
// Update the segment feature from the dot locations
vpFeatureBuilder::create(seg, cam, dot[0], dot[1]);
-
- vpColVector v ;
+
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature segments in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
-
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// Save feature error (s-s*) for the feature segment. For this feature
// segments, we have 4 errors (Xc,Yc,l,alpha).
- flog << ( task.getError() ).t() << std::endl;
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
-
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
index 09be8a7..3861a9c 100644
--- a/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6SquareLines2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,9 +40,10 @@
/*!
\file servoAfma6SquareLines2DCamVelocity.cpp
- \brief Example of eye-in-hand control law. We control here a real robot, the Afma6
- robot (cartesian robot, with 6 degrees of freedom). The velocity is computed
- in the camera frame. Visual features are the four lines of a square.
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ Afma6 robot (cartesian robot, with 6 degrees of freedom). The velocity is
+ computed in the camera frame. Visual features are the four lines of a
+ square.
*/
@@ -54,30 +56,28 @@
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -87,212 +87,202 @@
#include <visp3/blob/vpDot.h>
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
- vpServo task ;
-
+ vpServo task;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a line " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a line " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- int i ;
- int nbline =4 ;
+ int i;
+ int nbline = 4;
- vpMeLine line[nbline] ;
+ vpMeLine line[nbline];
- vpMe me ;
- me.setRange(10) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(50000) ;
+ vpMe me;
+ me.setRange(10);
+ me.setPointsToTrack(100);
+ me.setThreshold(50000);
me.setSampleStep(10);
- //Initialize the tracking. Define the four lines to track.
- for (i=0 ; i < nbline ; i++)
- {
- line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
- line[i].setMe(&me) ;
+ // Initialize the tracking. Define the four lines to track.
+ for (i = 0; i < nbline; i++) {
+ line[i].setDisplay(vpMeSite::RANGE_RESULT);
+ line[i].setMe(&me);
- line[i].initTracking(I) ;
- line[i].track(I) ;
+ line[i].initTracking(I);
+ line[i].track(I);
}
- vpRobotAfma6 robot ;
- //robot.move("zero.pos") ;
+ vpRobotAfma6 robot;
+ // robot.move("zero.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureLine p[nbline] ;
- for (i=0 ; i < nbline ; i++)
- vpFeatureBuilder::create(p[i],cam, line[i]) ;
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureLine p[nbline];
+ for (i = 0; i < nbline; i++)
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- vpTRACE("sets the desired position of the visual feature ") ;
+ vpTRACE("sets the desired position of the visual feature ");
vpLine lined[nbline];
- lined[0].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
- lined[1].setWorldCoordinates(0,1,0,0.05,0,0,1,0);
- lined[2].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
- lined[3].setWorldCoordinates(0,1,0,-0.05,0,0,1,0);
+ lined[0].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
+ lined[1].setWorldCoordinates(0, 1, 0, 0.05, 0, 0, 1, 0);
+ lined[2].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
+ lined[3].setWorldCoordinates(0, 1, 0, -0.05, 0, 0, 1, 0);
- vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
+ vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
lined[0].project(cMo);
lined[1].project(cMo);
lined[2].project(cMo);
lined[3].project(cMo);
- //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
- //Another way to have the coordinates of the desired features is to learn them before executing the program.
+ // Those lines are needed to keep the conventions define in vpMeLine
+ // (Those in vpLine are less restrictive) Another way to have the
+ // coordinates of the desired features is to learn them before executing
+ // the program.
lined[0].setRho(-fabs(lined[0].getRho()));
lined[0].setTheta(0);
lined[1].setRho(-fabs(lined[1].getRho()));
- lined[1].setTheta(M_PI/2);
+ lined[1].setTheta(M_PI / 2);
lined[2].setRho(-fabs(lined[2].getRho()));
lined[2].setTheta(M_PI);
lined[3].setRho(-fabs(lined[3].getRho()));
- lined[3].setTheta(-M_PI/2);
+ lined[3].setTheta(-M_PI / 2);
- vpFeatureLine pd[nbline] ;
+ vpFeatureLine pd[nbline];
- vpFeatureBuilder::create(pd[0],lined[0]);
- vpFeatureBuilder::create(pd[1],lined[1]);
- vpFeatureBuilder::create(pd[2],lined[2]);
- vpFeatureBuilder::create(pd[3],lined[3]);
+ vpFeatureBuilder::create(pd[0], lined[0]);
+ vpFeatureBuilder::create(pd[1], lined[1]);
+ vpFeatureBuilder::create(pd[2], lined[2]);
+ vpFeatureBuilder::create(pd[3], lined[3]);
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- for (i=0 ; i < nbline ; i++)
- task.addFeature(p[i],pd[i]) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ for (i = 0; i < nbline; i++)
+ task.addFeature(p[i], pd[i]);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.2) ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.2);
+ vpTRACE("Display task information ");
+ task.print();
- vpTRACE("Display task information " ) ;
- task.print() ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
+ vpImage<vpRGBa> Ic;
+ double lambda_av = 0.05;
+ double alpha = 0.05;
+ double beta = 3;
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
- vpImage<vpRGBa> Ic ;
- double lambda_av =0.05;
- double alpha = 0.05 ;
- double beta =3 ;
-
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- //Track the lines and update the features
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
- line[i].display(I, vpColor::red) ;
+ // Track the lines and update the features
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
+ line[i].display(I, vpColor::red);
- vpFeatureBuilder::create(p[i],cam,line[i]);
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- p[i].display(cam, I, vpColor::red) ;
- pd[i].display(cam, I, vpColor::green) ;
- }
+ p[i].display(cam, I, vpColor::red);
+ pd[i].display(cam, I, vpColor::green);
+ }
- double gain ;
+ double gain;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
}
- task.setLambda(gain) ;
+ task.setLambda(gain);
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- if (iter==0) vpDisplay::getClick(I) ;
- if (v.sumSquare() > 0.5)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- vpDisplay::getClick(I) ;
- }
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ if (v.sumSquare() > 0.5) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ vpDisplay::getClick(I);
+ }
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
index d505930..e8d21bf 100644
--- a/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
+++ b/example/servo-afma6/servoAfma6TwoLines2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,7 +48,6 @@
*/
-
/*!
\example servoAfma6TwoLines2DCamVelocity.cpp
@@ -58,30 +58,28 @@
*/
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#if (defined (VISP_HAVE_AFMA6) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpLine.h>
+#include <visp3/core/vpMath.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -89,194 +87,185 @@
#include <visp3/core/vpException.h>
#include <visp3/vs/vpServoDisplay.h>
-int
-main()
+int main()
{
- try
- {
- vpImage<unsigned char> I ;
+ try {
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpServo task ;
+ vpServo task;
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- int i ;
- int nbline =2 ;
+ int i;
+ int nbline = 2;
- vpMeLine line[nbline] ;
+ vpMeLine line[nbline];
- vpMe me ;
- me.setRange(10) ;
- me.setPointsToTrack(100) ;
- me.setThreshold(50000) ;
+ vpMe me;
+ me.setRange(10);
+ me.setPointsToTrack(100);
+ me.setThreshold(50000);
me.setSampleStep(10);
- //Initialize the tracking. Define the two lines to track
- vpTRACE("The two lines to track must be parallels ") ;
- //vpTRACE("The two lines to track must be perpendicular ") ;
- for (i=0 ; i < nbline ; i++)
- {
- line[i].setDisplay(vpMeSite::RANGE_RESULT) ;
- line[i].setMe(&me) ;
+ // Initialize the tracking. Define the two lines to track
+ vpTRACE("The two lines to track must be parallels ");
+ // vpTRACE("The two lines to track must be perpendicular ") ;
+ for (i = 0; i < nbline; i++) {
+ line[i].setDisplay(vpMeSite::RANGE_RESULT);
+ line[i].setMe(&me);
- line[i].initTracking(I) ;
- line[i].track(I) ;
+ line[i].initTracking(I);
+ line[i].track(I);
}
- vpRobotAfma6 robot ;
- //robot.move("zero.pos") ;
+ vpRobotAfma6 robot;
+ // robot.move("zero.pos") ;
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
- vpTRACE("sets the current position of the visual feature ") ;
- vpFeatureLine p[nbline] ;
- for (i=0 ; i < nbline ; i++)
- vpFeatureBuilder::create(p[i],cam, line[i]) ;
+ vpTRACE("sets the current position of the visual feature ");
+ vpFeatureLine p[nbline];
+ for (i = 0; i < nbline; i++)
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- vpTRACE("sets the desired position of the visual feature ") ;
+ vpTRACE("sets the desired position of the visual feature ");
vpLine lined[2];
- lined[0].setWorldCoordinates(1,0,0,-0.05,0,0,1,0);
- lined[1].setWorldCoordinates(1,0,0,0.05,0,0,1,0);
+ lined[0].setWorldCoordinates(1, 0, 0, -0.05, 0, 0, 1, 0);
+ lined[1].setWorldCoordinates(1, 0, 0, 0.05, 0, 0, 1, 0);
- vpHomogeneousMatrix cMo(0,0,0.5,0,0,vpMath::rad(0));
+ vpHomogeneousMatrix cMo(0, 0, 0.5, 0, 0, vpMath::rad(0));
lined[0].project(cMo);
lined[1].project(cMo);
- //Those lines are needed to keep the conventions define in vpMeLine (Those in vpLine are less restrictive)
- //Another way to have the coordinates of the desired features is to learn them before executing the program.
+ // Those lines are needed to keep the conventions define in vpMeLine
+ // (Those in vpLine are less restrictive) Another way to have the
+ // coordinates of the desired features is to learn them before executing
+ // the program.
lined[0].setRho(-fabs(lined[0].getRho()));
lined[0].setTheta(0);
lined[1].setRho(-fabs(lined[1].getRho()));
lined[1].setTheta(M_PI);
- vpFeatureLine pd[nbline] ;
- vpFeatureBuilder::create(pd[0],lined[0]);
- vpFeatureBuilder::create(pd[1],lined[1]);
+ vpFeatureLine pd[nbline];
+ vpFeatureBuilder::create(pd[0], lined[0]);
+ vpFeatureBuilder::create(pd[1], lined[1]);
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t robot is controlled in the camera frame") ;
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t robot is controlled in the camera frame");
+ task.setServo(vpServo::EYEINHAND_CAMERA);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- for (i=0 ; i < nbline ; i++)
- task.addFeature(p[i],pd[i]) ;
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ for (i = 0; i < nbline; i++)
+ task.addFeature(p[i], pd[i]);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.2) ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.2);
+ vpTRACE("Display task information ");
+ task.print();
- vpTRACE("Display task information " ) ;
- task.print() ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
+ vpColVector v;
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
-
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
- vpColVector v ;
-
- vpImage<vpRGBa> Ic ;
- double lambda_av =0.05;
+ vpImage<vpRGBa> Ic;
+ double lambda_av = 0.05;
double alpha = 0.2;
- double beta =3;
+ double beta = 3;
- for ( ; ; )
- {
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ for (;;) {
+ std::cout << "---------------------------------------------" << iter << std::endl;
try {
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- //Track the lines and update the features
- for (i=0 ; i < nbline ; i++)
- {
- line[i].track(I) ;
- line[i].display(I, vpColor::red) ;
+ // Track the lines and update the features
+ for (i = 0; i < nbline; i++) {
+ line[i].track(I);
+ line[i].display(I, vpColor::red);
- vpFeatureBuilder::create(p[i],cam,line[i]);
+ vpFeatureBuilder::create(p[i], cam, line[i]);
- p[i].display(cam, I, vpColor::red) ;
- pd[i].display(cam, I, vpColor::green) ;
- }
+ p[i].display(cam, I, vpColor::red);
+ pd[i].display(cam, I, vpColor::green);
+ }
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- //Adaptative gain
- double gain ;
+ // Adaptative gain
+ double gain;
{
if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- gain = lambda_av ;
- else
- {
- gain = alpha * exp (-beta * ( task.getError() ).sumSquare() ) + lambda_av ;
+ gain = lambda_av;
+ else {
+ gain = alpha * exp(-beta * (task.getError()).sumSquare()) + lambda_av;
}
}
- task.setLambda(gain) ;
-
- v = task.computeControlLaw() ;
-
- if (iter==0) vpDisplay::getClick(I) ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ task.setLambda(gain);
+
+ v = task.computeControlLaw();
+
+ if (iter == 0)
+ vpDisplay::getClick(I);
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
+ v = 0;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ robot.stopMotion();
+ exit(1);
}
- catch(...)
- {
- v =0 ;
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- robot.stopMotion() ;
- exit(1) ;
- }
-
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
iter++;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-biclops/CMakeLists.txt b/example/servo-biclops/CMakeLists.txt
index ba60606..78023a5 100644
--- a/example/servo-biclops/CMakeLists.txt
+++ b/example/servo-biclops/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/servo-biclops/moveBiclops.cpp b/example/servo-biclops/moveBiclops.cpp
index 4b5d2bd..ea8747a 100644
--- a/example/servo-biclops/moveBiclops.cpp
+++ b/example/servo-biclops/moveBiclops.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,13 +37,12 @@
/*!
\file moveBiclops.cpp
- \brief Example of a real robot control, the biclops robot (pan-tilt turret) by
- Traclabs. The robot is controlled first in position, then in velocity.
+ \brief Example of a real robot control, the biclops robot (pan-tilt turret)
+ by Traclabs. The robot is controlled first in position, then in velocity.
See http://www.traclabs.com/tracbiclops.htm for more details.
*/
-
/*!
\example moveBiclops.cpp
@@ -52,20 +52,18 @@
See http://www.traclabs.com/tracbiclops.htm for more details.
*/
-
-
-#include <visp3/io/vpParseArgv.h>
+#include <stdlib.h>
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpColVector.h>
#include <visp3/core/vpTime.h>
-#include <stdlib.h>
+#include <visp3/io/vpParseArgv.h>
#ifdef VISP_HAVE_BICLOPS
#include <visp3/robot/vpRobotBiclops.h>
// List of allowed command line options
-#define GETOPTARGS "c:h"
+#define GETOPTARGS "c:h"
/*
@@ -88,11 +86,10 @@ SYNOPSIS\n\
fprintf(stdout, "\n\
OPTIONS: Default\n\
-c <Biclops configuration file> %s\n\
- Sets the biclops robot configuration file.\n\n",
- conf.c_str());
+ Sets the biclops robot configuration file.\n\n", conf.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -104,22 +101,29 @@ Set the program options.
\param argc : Command line number of parameters.
\param argv : Array of command line parameters.
\param conf : Biclops configuration file.
-
+
\return false if the program has to be stopped, true otherwise.
*/
-bool getOptions(int argc, const char **argv, std::string& conf)
+bool getOptions(int argc, const char **argv, std::string &conf)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': conf = optarg_; break;
- case 'h': usage(argv[0], NULL, conf); return false; break;
+ case 'c':
+ conf = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, conf);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, conf); return false; break;
+ usage(argv[0], optarg_, conf);
+ return false;
+ break;
}
}
@@ -134,164 +138,151 @@ bool getOptions(int argc, const char **argv, std::string& conf)
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
// Read the command line options
if (getOptions(argc, argv, opt_conf) == false) {
- exit (-1);
+ exit(-1);
}
try {
vpRobotBiclops robot(opt_conf.c_str());
- vpColVector q (vpBiclops::ndof) ; // desired position
- vpColVector qdot (vpBiclops::ndof) ; // desired velocity
- vpColVector qm (vpBiclops::ndof) ; // measured position
- vpColVector qm_dot(vpBiclops::ndof) ; // measured velocity
+ vpColVector q(vpBiclops::ndof); // desired position
+ vpColVector qdot(vpBiclops::ndof); // desired velocity
+ vpColVector qm(vpBiclops::ndof); // measured position
+ vpColVector qm_dot(vpBiclops::ndof); // measured velocity
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
q = 0;
q[0] = vpMath::rad(-10);
q[1] = vpMath::rad(-20);
std::cout << "Set position in the articular frame: "
<< " pan: " << vpMath::deg(q[0]) << " deg"
- << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl ;
- robot.setPositioningVelocity(30.) ;
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
+ robot.setPositioningVelocity(30.);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ 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 ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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);
std::cout << "Set position in the articular frame: "
<< " pan: " << vpMath::deg(q[0]) << " deg"
- << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl ;
- robot.setPositioningVelocity(10) ;
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ << " tilt: " << vpMath::deg(q[1]) << " deg" << std::endl;
+ robot.setPositioningVelocity(10);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ 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 ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
- << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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;
// qdot[0] = vpMath::rad(0.1) ;
- qdot[1] = vpMath::rad(25) ;
+ qdot[1] = vpMath::rad(25);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
- << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
+ << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
- //waits 5000ms
+ // waits 5000ms
vpTime::wait(5000.0);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
- << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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;
// qdot[0] = vpMath::rad(0.1) ;
- qdot[1] = -vpMath::rad(25) ;
+ qdot[1] = -vpMath::rad(25);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
- << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
+ << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
- //waits 3000 ms
+ // waits 3000 ms
vpTime::wait(3000.0);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
- << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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;
// qdot[0] = vpMath::rad(0.1) ;
- qdot[1] = vpMath::rad(10) ;
+ qdot[1] = vpMath::rad(10);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
- << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
+ << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
- //waits 2000 ms
+ // waits 2000 ms
vpTime::wait(2000.0);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
- << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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;
qdot[0] = vpMath::rad(-5);
- //qdot[1] = vpMath::rad(-5);
+ // qdot[1] = vpMath::rad(-5);
std::cout << "Set articular frame velocity "
<< " pan: " << vpMath::deg(qdot[0]) << " deg/s"
- << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
+ << " tilt: " << vpMath::deg(qdot[1]) << " deg/s" << std::endl;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
- //waits 2000 ms
+ // waits 2000 ms
vpTime::wait(2000.0);
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
std::cout << "Position in the articular frame: "
<< " pan: " << vpMath::deg(qm[0]) << " deg"
- << " tilt: " << vpMath::deg(qm[1]) << " deg" << std::endl ;
- robot.getVelocity(vpRobot::ARTICULAR_FRAME, qm) ;
+ << " 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 ;
- }
- catch(vpException &e) {
+ << " pan: " << vpMath::deg(qm[0]) << " tilt: " << vpMath::deg(qm[1]) << std::endl;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
-
}
#else
-int
-main()
+int main()
{
vpERROR_TRACE("You do not have a biclops robot connected to your computer...");
- return 0;
+ return 0;
}
#endif
diff --git a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
index e86bd43..347fee5 100644
--- a/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
+++ b/example/servo-biclops/servoBiclopsPoint2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,56 +41,51 @@
/*!
\file servoBiclopsPoint2DArtVelocity.cpp
- \brief Example of eye-in-hand control law. We control here a real robot, the biclops
- robot (pan-tilt head provided by Traclabs). The velocity is
- computed in articular. The visual feature is the center of gravity of a
- point.
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ biclops robot (pan-tilt head provided by Traclabs). The velocity is computed
+ in articular. The visual feature is the center of gravity of a point.
*/
-
/*!
\example servoBiclopsPoint2DArtVelocity.cpp
- Example of eye-in-hand control law. We control here a real robot, the biclops
- robot (pan-tilt head provided by Traclabs). The velocity is
- computed in articular. The visual feature is the center of gravity of a
- point.
+ Example of eye-in-hand control law. We control here a real robot, the
+ biclops robot (pan-tilt head provided by Traclabs). The velocity is computed
+ in articular. The visual feature is the center of gravity of a point.
*/
-
-
-#include <visp3/core/vpTime.h>
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
#include <signal.h>
#include <stdlib.h>
-#if ( defined (VISP_HAVE_BICLOPS) && (defined (VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)) )
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h> // Debug trace
+#include <visp3/core/vpTime.h>
+#if (defined(VISP_HAVE_BICLOPS) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_DIRECTSHOW)))
#ifdef VISP_HAVE_PTHREAD
-# include <pthread.h>
+#include <pthread.h>
#endif
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/sensor/vpDirectShowGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpDirectShowGrabber.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/blob/vpDot.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/robot/vpRobotBiclops.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot.h>
// Exception
#include <visp3/core/vpException.h>
@@ -98,18 +94,17 @@
pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
#endif
-void signalCtrC( int /* signumber */)
+void signalCtrC(int /* signumber */)
{
#ifdef VISP_HAVE_PTHREAD
- pthread_mutex_unlock( &mutexEndLoop );
+ pthread_mutex_unlock(&mutexEndLoop);
#endif
vpTime::wait(10);
vpTRACE("Ctrl-C pressed...");
}
-
// List of allowed command line options
-#define GETOPTARGS "c:d:h"
+#define GETOPTARGS "c:d:h"
/*!
@@ -122,7 +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\
@@ -144,7 +139,7 @@ OPTIONS: Default\n\
it writes biclops.txt file.\n", conf.c_str(), debugdir.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -161,19 +156,28 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': conf = optarg_; break;
- case 'd': debugdir = optarg_; break;
- case 'h': usage(argv[0], NULL, conf, debugdir, user); return false; break;
+ case 'c':
+ conf = optarg_;
+ break;
+ case 'd':
+ debugdir = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, conf, debugdir, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, conf, debugdir, user); return false; break;
+ usage(argv[0], optarg_, conf, debugdir, user);
+ return false;
+ break;
}
}
@@ -188,35 +192,32 @@ bool getOptions(int argc, const char **argv, std::string& conf, std::string &deb
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- try{
+ try {
#ifdef VISP_HAVE_PTHREAD
- pthread_mutex_lock( &mutexEndLoop );
+ pthread_mutex_lock(&mutexEndLoop);
#endif
- signal( SIGINT,&signalCtrC );
+ signal(SIGINT, &signalCtrC);
- //default unix configuration file path
+ // default unix configuration file path
std::string opt_conf = "/usr/share/BiclopsDefault.cfg";
std::string username;
std::string debugdir;
std::string opt_debugdir;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_debugdir = "/tmp";
#elif defined(_WIN32)
@@ -227,8 +228,8 @@ main(int argc, const char ** argv)
vpIoTools::getUserName(username);
// Read the command line options
- if (getOptions(argc, argv, opt_conf, opt_debugdir , username) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_conf, opt_debugdir, username) == false) {
+ exit(-1);
}
// Get the option value
@@ -243,11 +244,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(dirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, opt_conf, debugdir, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << dirname << std::endl;
std::cerr << " Check your -d " << debugdir << " option " << std::endl;
exit(-1);
@@ -259,16 +258,17 @@ main(int argc, const char ** argv)
sprintf(filename, "%s/biclops.txt", debugdir.c_str());
FILE *fd = fopen(filename, "w");
- vpRobotBiclops robot(opt_conf.c_str()) ;
+ vpRobotBiclops robot(opt_conf.c_str());
robot.setDenavitHartenbergModel(vpBiclops::DH2);
{
- vpColVector q(2); q=0;
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- robot.setPosition( vpRobot::ARTICULAR_FRAME,q );
+ vpColVector q(2);
+ q = 0;
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
}
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
#if defined VISP_HAVE_DC1394
vp1394TwoGrabber g;
@@ -276,164 +276,152 @@ main(int argc, const char ** argv)
vpDirectShowGrabber g;
#endif
- g.open(I) ;
+ g.open(I);
- try{
- g.acquire(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught") ;
- return(-1) ;
+ try {
+ g.acquire(I);
+ } catch (...) {
+ vpERROR_TRACE(" Error caught");
+ return (-1);
}
- // We open a window using either X11 or GTK or GDI.
- // Its size is automatically defined by the image (I) size
+// We open a window using either X11 or GTK or GDI.
+// Its size is automatically defined by the image (I) size
#if defined VISP_HAVE_X11
- vpDisplayX display(I, 100, 100,"Display X...") ;
+ vpDisplayX display(I, 100, 100, "Display X...");
#elif defined VISP_HAVE_GTK
- vpDisplayGTK display(I, 100, 100,"Display GTK...") ;
+ vpDisplayGTK display(I, 100, 100, "Display GTK...");
#elif defined(_WIN32)
- vpDisplayGDI display(I, 100, 100,"Display GDI...") ;
+ vpDisplayGDI display(I, 100, 100, "Display GDI...");
#endif
- try{
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught") ;
- return(-1) ;
+ try {
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE(" Error caught");
+ return (-1);
}
+ vpServo task;
- vpServo task ;
+ vpDot dot;
- vpDot dot ;
-
- try{
+ try {
std::cout << "Click on a dot to initialize the tracking..." << std::endl;
dot.setGraphics(true);
- dot.initTracking(I) ;
+ dot.initTracking(I);
dot.track(I);
- vpERROR_TRACE("after dot.initTracking(I) ") ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught") ;
- return(-1) ;
+ vpERROR_TRACE("after dot.initTracking(I) ");
+ } catch (...) {
+ vpERROR_TRACE(" Error caught");
+ return (-1);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
- p.set_Z(1) ;
+ p.set_Z(1);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
-
- vpTRACE("Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
+ vpTRACE("Set the position of the camera in the end-effector frame ");
+ vpHomogeneousMatrix cMe;
// robot.get_cMe(cMe) ;
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
std::cout << "Click in the image to start the servoing..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// set the gain
- task.setLambda(0.2) ;
+ task.setLambda(0.2);
// Display task information
- task.print() ;
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
#ifdef VISP_HAVE_PTHREAD
- while( 0 != pthread_mutex_trylock( &mutexEndLoop ) )
+ while (0 != pthread_mutex_trylock(&mutexEndLoop))
#else
- for ( ; ; )
+ for (;;)
#endif
{
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ std::cout << "---------------------------------------------" << iter << std::endl;
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- dot.track(I) ;
+ dot.track(I);
// vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(),
// 10,vpColor::green) ;
-
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// get the jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// std::cout << (vpMatrix)cVe*eJe << std::endl ;
- vpColVector v ;
- v = task.computeControlLaw() ;
+ vpColVector v;
+ v = task.computeControlLaw();
- vpServoDisplay::display(task,cam,I) ;
- vpDisplay::flush(I) ;
+ vpServoDisplay::display(task, cam, I);
+ vpDisplay::flush(I);
- std::cout << "v: " << v.t() ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ 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());
}
}
std::cout << "Display task information " << std::endl;
- task.print() ;
+ task.print();
task.kill();
fclose(fd);
- } catch (...) { vpERROR_TRACE("Throw uncatched..."); }
-
+ } catch (...) {
+ vpERROR_TRACE("Throw uncatched...");
+ }
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You don't have a biclops head connected to your computer or 1394 framegrabbing capabilities...");
+ vpERROR_TRACE("You don't have a biclops head connected to your computer or "
+ "1394 framegrabbing capabilities...");
}
#endif
diff --git a/example/servo-franka/CMakeLists.txt b/example/servo-franka/CMakeLists.txt
new file mode 100644
index 0000000..0eda484
--- /dev/null
+++ b/example/servo-franka/CMakeLists.txt
@@ -0,0 +1,66 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+project(example-servo-franka)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_robot)
+
+set(example_cpp
+ franka_echo_robot_state.cpp
+ franka_execute_trajectory.cpp
+ franka_generate_cartesian_pose_motion.cpp
+ franka_generate_cartesian_velocity_motion.cpp
+ franka_generate_consecutive_motions.cpp
+ franka_generate_joint_position_motion.cpp
+ franka_generate_joint_velocity_motion.cpp
+ franka_grasp_object.cpp
+ franka_joint_impedance_control.cpp
+ franka_joint_point_to_point_motion.cpp
+ franka_motion_with_control.cpp
+ franka_print_joint_positions.cpp
+)
+
+foreach(cpp ${example_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "examples")
+ endif()
+endforeach()
+
diff --git a/example/servo-franka/LICENCE.txt b/example/servo-franka/LICENCE.txt
new file mode 100644
index 0000000..d9a10c0
--- /dev/null
+++ b/example/servo-franka/LICENCE.txt
@@ -0,0 +1,176 @@
+ Apache License
+ Version 2.0, January 2004
+ http://www.apache.org/licenses/
+
+ TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+ 1. Definitions.
+
+ "License" shall mean the terms and conditions for use, reproduction,
+ and distribution as defined by Sections 1 through 9 of this document.
+
+ "Licensor" shall mean the copyright owner or entity authorized by
+ the copyright owner that is granting the License.
+
+ "Legal Entity" shall mean the union of the acting entity and all
+ other entities that control, are controlled by, or are under common
+ control with that entity. For the purposes of this definition,
+ "control" means (i) the power, direct or indirect, to cause the
+ direction or management of such entity, whether by contract or
+ otherwise, or (ii) ownership of fifty percent (50%) or more of the
+ outstanding shares, or (iii) beneficial ownership of such entity.
+
+ "You" (or "Your") shall mean an individual or Legal Entity
+ exercising permissions granted by this License.
+
+ "Source" form shall mean the preferred form for making modifications,
+ including but not limited to software source code, documentation
+ source, and configuration files.
+
+ "Object" form shall mean any form resulting from mechanical
+ transformation or translation of a Source form, including but
+ not limited to compiled object code, generated documentation,
+ and conversions to other media types.
+
+ "Work" shall mean the work of authorship, whether in Source or
+ Object form, made available under the License, as indicated by a
+ copyright notice that is included in or attached to the work
+ (an example is provided in the Appendix below).
+
+ "Derivative Works" shall mean any work, whether in Source or Object
+ form, that is based on (or derived from) the Work and for which the
+ editorial revisions, annotations, elaborations, or other modifications
+ represent, as a whole, an original work of authorship. For the purposes
+ of this License, Derivative Works shall not include works that remain
+ separable from, or merely link (or bind by name) to the interfaces of,
+ the Work and Derivative Works thereof.
+
+ "Contribution" shall mean any work of authorship, including
+ the original version of the Work and any modifications or additions
+ to that Work or Derivative Works thereof, that is intentionally
+ submitted to Licensor for inclusion in the Work by the copyright owner
+ or by an individual or Legal Entity authorized to submit on behalf of
+ the copyright owner. For the purposes of this definition, "submitted"
+ means any form of electronic, verbal, or written communication sent
+ to the Licensor or its representatives, including but not limited to
+ communication on electronic mailing lists, source code control systems,
+ and issue tracking systems that are managed by, or on behalf of, the
+ Licensor for the purpose of discussing and improving the Work, but
+ excluding communication that is conspicuously marked or otherwise
+ designated in writing by the copyright owner as "Not a Contribution."
+
+ "Contributor" shall mean Licensor and any individual or Legal Entity
+ on behalf of whom a Contribution has been received by Licensor and
+ subsequently incorporated within the Work.
+
+ 2. Grant of Copyright License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ copyright license to reproduce, prepare Derivative Works of,
+ publicly display, publicly perform, sublicense, and distribute the
+ Work and such Derivative Works in Source or Object form.
+
+ 3. Grant of Patent License. Subject to the terms and conditions of
+ this License, each Contributor hereby grants to You a perpetual,
+ worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+ (except as stated in this section) patent license to make, have made,
+ use, offer to sell, sell, import, and otherwise transfer the Work,
+ where such license applies only to those patent claims licensable
+ by such Contributor that are necessarily infringed by their
+ Contribution(s) alone or by combination of their Contribution(s)
+ with the Work to which such Contribution(s) was submitted. If You
+ institute patent litigation against any entity (including a
+ cross-claim or counterclaim in a lawsuit) alleging that the Work
+ or a Contribution incorporated within the Work constitutes direct
+ or contributory patent infringement, then any patent licenses
+ granted to You under this License for that Work shall terminate
+ as of the date such litigation is filed.
+
+ 4. Redistribution. You may reproduce and distribute copies of the
+ Work or Derivative Works thereof in any medium, with or without
+ modifications, and in Source or Object form, provided that You
+ meet the following conditions:
+
+ (a) You must give any other recipients of the Work or
+ Derivative Works a copy of this License; and
+
+ (b) You must cause any modified files to carry prominent notices
+ stating that You changed the files; and
+
+ (c) You must retain, in the Source form of any Derivative Works
+ that You distribute, all copyright, patent, trademark, and
+ attribution notices from the Source form of the Work,
+ excluding those notices that do not pertain to any part of
+ the Derivative Works; and
+
+ (d) If the Work includes a "NOTICE" text file as part of its
+ distribution, then any Derivative Works that You distribute must
+ include a readable copy of the attribution notices contained
+ within such NOTICE file, excluding those notices that do not
+ pertain to any part of the Derivative Works, in at least one
+ of the following places: within a NOTICE text file distributed
+ as part of the Derivative Works; within the Source form or
+ documentation, if provided along with the Derivative Works; or,
+ within a display generated by the Derivative Works, if and
+ wherever such third-party notices normally appear. The contents
+ of the NOTICE file are for informational purposes only and
+ do not modify the License. You may add Your own attribution
+ notices within Derivative Works that You distribute, alongside
+ or as an addendum to the NOTICE text from the Work, provided
+ that such additional attribution notices cannot be construed
+ as modifying the License.
+
+ You may add Your own copyright statement to Your modifications and
+ may provide additional or different license terms and conditions
+ for use, reproduction, or distribution of Your modifications, or
+ for any such Derivative Works as a whole, provided Your use,
+ reproduction, and distribution of the Work otherwise complies with
+ the conditions stated in this License.
+
+ 5. Submission of Contributions. Unless You explicitly state otherwise,
+ any Contribution intentionally submitted for inclusion in the Work
+ by You to the Licensor shall be under the terms and conditions of
+ this License, without any additional terms or conditions.
+ Notwithstanding the above, nothing herein shall supersede or modify
+ the terms of any separate license agreement you may have executed
+ with Licensor regarding such Contributions.
+
+ 6. Trademarks. This License does not grant permission to use the trade
+ names, trademarks, service marks, or product names of the Licensor,
+ except as required for reasonable and customary use in describing the
+ origin of the Work and reproducing the content of the NOTICE file.
+
+ 7. Disclaimer of Warranty. Unless required by applicable law or
+ agreed to in writing, Licensor provides the Work (and each
+ Contributor provides its Contributions) on an "AS IS" BASIS,
+ WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+ implied, including, without limitation, any warranties or conditions
+ of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+ PARTICULAR PURPOSE. You are solely responsible for determining the
+ appropriateness of using or redistributing the Work and assume any
+ risks associated with Your exercise of permissions under this License.
+
+ 8. Limitation of Liability. In no event and under no legal theory,
+ whether in tort (including negligence), contract, or otherwise,
+ unless required by applicable law (such as deliberate and grossly
+ negligent acts) or agreed to in writing, shall any Contributor be
+ liable to You for damages, including any direct, indirect, special,
+ incidental, or consequential damages of any character arising as a
+ result of this License or out of the use or inability to use the
+ Work (including but not limited to damages for loss of goodwill,
+ work stoppage, computer failure or malfunction, or any and all
+ other commercial damages or losses), even if such Contributor
+ has been advised of the possibility of such damages.
+
+ 9. Accepting Warranty or Additional Liability. While redistributing
+ the Work or Derivative Works thereof, You may choose to offer,
+ and charge a fee for, acceptance of support, warranty, indemnity,
+ or other liability obligations and/or rights consistent with this
+ License. However, in accepting such obligations, You may act only
+ on Your own behalf and on Your sole responsibility, not on behalf
+ of any other Contributor, and only if You agree to indemnify,
+ defend, and hold each Contributor harmless for any liability
+ incurred by, or claims asserted against, such Contributor by reason
+ of your accepting any such warranty or additional liability.
+
+ END OF TERMS AND CONDITIONS
diff --git a/example/servo-franka/franka_echo_robot_state.cpp b/example/servo-franka/franka_echo_robot_state.cpp
new file mode 100644
index 0000000..6c26301
--- /dev/null
+++ b/example/servo-franka/franka_echo_robot_state.cpp
@@ -0,0 +1,50 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_echo_robot_state.cpp
+ * An example showing how to continuously read the robot state.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./echo_robot_state <robot-hostname>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ size_t count = 0;
+ robot.read([&count](const franka::RobotState &robot_state) {
+ // Printing to std::cout adds a delay. This is acceptable for a read
+ // loop such as this, but should not be done in a control loop.
+ std::cout << robot_state << std::endl;
+ return count++ < 100;
+ });
+
+ std::cout << "Done." << std::endl;
+ } catch (franka::Exception const &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..a98661f
--- /dev/null
+++ b/example/servo-franka/franka_execute_trajectory.cpp
@@ -0,0 +1,108 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <iterator>
+#include <vector>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_execute_trajectory.cpp
+ * An example showing how to execute a joint trajectory loaded from a CSV
+ * file.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
+{
+ ostream << "[";
+ std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
+ std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
+ ostream << "]";
+ return ostream;
+}
+
+int main(int argc, char **argv)
+{
+ if (argc != 4) {
+ std::cerr << "Usage: " << argv[0] << " <robot-hostname> <trajectory-csv> <output>" << std::endl;
+ return -1;
+ }
+
+ std::cout << "Loading csv trajectory" << std::endl;
+ std::fstream csv_file_stream;
+ csv_file_stream.open(argv[2], std::fstream::in);
+
+ std::vector<std::array<double, 7> > samples;
+ while (csv_file_stream) {
+ std::array<double, 7> q;
+ char delimiter;
+ for (int i = 0; i < 7; i++) {
+ csv_file_stream >> q[i] >> delimiter;
+ }
+ samples.push_back(q);
+ }
+ std::cout << "Read " << samples.size() << " samples" << std::endl;
+
+ std::vector<franka::RobotState> states;
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ // 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 {
+ states.push_back(robot_state);
+
+ index += time_step.toMSec();
+
+ if (index >= samples.size() - 1) {
+ return franka::MotionFinished(franka::JointPositions(samples.back()));
+ }
+ return samples[index];
+ });
+ } catch (const franka::ControlException &e) {
+ std::cout << e.what() << std::endl;
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "Logging results to file: " << argv[3] << std::endl;
+ std::fstream output_stream;
+ output_stream.open(argv[3], std::fstream::out);
+ for (size_t s = 0; s < states.size(); s++) {
+ output_stream << "Sample: #" << s << std::endl;
+ output_stream << "q_d: \t" << samples[s] << std::endl;
+ output_stream << "Robot state:" << std::endl;
+ output_stream << "q: \t" << states[s].q << std::endl;
+ output_stream << "q_d: \t" << states[s].q_d << std::endl;
+ output_stream << "dq: \t" << states[s].dq << std::endl;
+ output_stream << "______" << std::endl;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..a392c61
--- /dev/null
+++ b/example/servo-franka/franka_generate_cartesian_pose_motion.cpp
@@ -0,0 +1,73 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_generate_cartesian_pose_motion.cpp
+ * An example showing how to generate a Cartesian motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./generate_cartesian_pose_motion <robot-hostname>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ 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 {
+ time += time_step.toSec();
+
+ double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time));
+ double delta_x = radius * std::sin(angle);
+ double delta_z = radius * (std::cos(angle) - 1);
+
+ std::array<double, 16> new_pose = initial_pose;
+ new_pose[12] += delta_x;
+ new_pose[14] += delta_z;
+
+ if (time >= 10.0) {
+ std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
+ return franka::MotionFinished(new_pose);
+ }
+ return new_pose;
+ });
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..c056e31
--- /dev/null
+++ b/example/servo-franka/franka_generate_cartesian_velocity_motion.cpp
@@ -0,0 +1,81 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_generate_cartesian_velocity_motion.cpp
+ * An example showing how to generate a Cartesian velocity motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./generate_cartesian_velocity_motion <robot-hostname>" << std::endl;
+ return -1;
+ }
+ try {
+ franka::Robot robot(argv[1]);
+
+ // Set additional parameters always before the control loop, NEVER in the
+ // control loop! Set the joint impedance.
+ robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
+
+ // Set the collision behavior.
+ std::array<double, 7> lower_torque_thresholds_nominal{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}};
+ std::array<double, 7> upper_torque_thresholds_nominal{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
+ std::array<double, 7> lower_torque_thresholds_acceleration{{25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}};
+ std::array<double, 7> upper_torque_thresholds_acceleration{{35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}};
+ std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
+ std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}};
+ std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}};
+ std::array<double, 6> 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 {
+ 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 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;
+ return franka::MotionFinished(output);
+ }
+ return output;
+ });
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..f64df6f
--- /dev/null
+++ b/example/servo-franka/franka_generate_consecutive_motions.cpp
@@ -0,0 +1,79 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_generate_consecutive_motions.cpp
+ * An example showing how to execute consecutive motions with error recovery.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front and to the side of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./generate_consecutive_motions <robot-hostname>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ for (int i = 0; i < 5; i++) {
+ std::cout << "Executing motion." << std::endl;
+ try {
+ 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 {
+ 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));
+
+ 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);
+ }
+ return velocities;
+ });
+ } catch (const franka::ControlException &e) {
+ std::cout << e.what() << std::endl;
+ std::cout << "Running error recovery..." << std::endl;
+ robot.automaticErrorRecovery();
+ }
+ }
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ std::cout << "Finished." << std::endl;
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..d4c71f2
--- /dev/null
+++ b/example/servo-franka/franka_generate_joint_position_motion.cpp
@@ -0,0 +1,70 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_generate_joint_position_motion.cpp
+ * An example showing how to generate a joint position motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./generate_joint_position_motion <robot-hostname>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ auto initial_position = robot.readOnce().q_d;
+ double time = 0.0;
+ 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}};
+
+ if (time >= 10.0) {
+ std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
+ return franka::MotionFinished(output);
+ }
+ return output;
+ });
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..1c5e8ae
--- /dev/null
+++ b/example/servo-franka/franka_generate_joint_velocity_motion.cpp
@@ -0,0 +1,69 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_generate_joint_velocity_motion.cpp
+ * An example showing how to generate a joint velocity motion.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./generate_joint_velocity_motion <robot-hostname>" << std::endl;
+ return -1;
+ }
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ 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 {
+ 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));
+
+ 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;
+ return franka::MotionFinished(velocities);
+ }
+ return velocities;
+ });
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..dc60262
--- /dev/null
+++ b/example/servo-franka/franka_grasp_object.cpp
@@ -0,0 +1,83 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <sstream>
+#include <string>
+#include <thread>
+
+#include <franka/exception.h>
+#include <franka/gripper.h>
+
+/**
+ * @example franka_grasp_object.cpp
+ * An example showing how to control FRANKA's gripper.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ if (argc != 4) {
+ std::cerr << "Usage: ./grasp_object <gripper-hostname> <homing> <object-width>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Gripper gripper(argv[1]);
+ double grasping_width = std::stod(argv[3]);
+
+ std::stringstream ss(argv[2]);
+ bool homing;
+ if (!(ss >> homing)) {
+ std::cerr << "<homing> can be 0 or 1." << std::endl;
+ return -1;
+ }
+
+ if (homing) {
+ // Do a homing in order to estimate the maximum grasping width with the
+ // current fingers.
+ gripper.homing();
+ }
+
+ // 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;
+ return -1;
+ }
+
+ // Grasp the object.
+ if (!gripper.grasp(grasping_width, 0.1, 300)) {
+ std::cout << "Failed to grasp object." << std::endl;
+ return -1;
+ }
+
+ // Wait 3s and check afterwards, if the object is still grasped.
+ std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(3000));
+
+ gripper_state = gripper.readOnce();
+ if (!gripper_state.is_grasped) {
+ std::cout << "Object lost." << std::endl;
+ return -1;
+ }
+
+ std::cout << "Grasped object, will release it now." << std::endl;
+ gripper.stop();
+ } catch (franka::Exception const &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..b4feb37
--- /dev/null
+++ b/example/servo-franka/franka_joint_impedance_control.cpp
@@ -0,0 +1,225 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <array>
+#include <atomic>
+#include <cmath>
+#include <functional>
+#include <iterator>
+#include <mutex>
+#include <thread>
+
+#include <franka/duration.h>
+#include <franka/exception.h>
+#include <franka/model.h>
+#include <franka/robot.h>
+
+namespace
+{
+template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
+{
+ ostream << "[";
+ std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
+ std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
+ ostream << "]";
+ return ostream;
+}
+} // anonymous namespace
+
+/**
+ * @example franka_joint_impedance_control.cpp
+ * An example showing a joint impedance type control that executes a Cartesian
+ * motion in the shape of a circle. The example illustrates how to use the
+ * internal inverse kinematics to map a Cartesian trajectory to joint space.
+ * The joint space target is tracked by an impedance control that additionally
+ * compensates coriolis terms using the libfranka model library. This example
+ * also serves to compare commanded vs. measured torques. The results are
+ * printed from a separate thread to avoid blocking print functions in the
+ * real-time loop.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+int main(int argc, char **argv)
+{
+ // Check whether the required arguments were passed.
+ if (argc != 5) {
+ std::cerr << "Usage: ./" << argv[0] << " <robot-hostname>"
+ << " <radius in [m]>"
+ << " <vel_max in [m/s]>"
+ << " <print_rate in [Hz]>" << std::endl;
+ return -1;
+ }
+
+ // Set and initialize trajectory parameters.
+ const double radius = std::stod(argv[2]);
+ const double vel_max = std::stod(argv[3]);
+ const double acceleration_time = 2.0;
+ double vel_current = 0.0;
+ double angle = 0.0;
+
+ double time = 0.0;
+ const double run_time = 20.0;
+
+ // Set print rate for comparing commanded vs. measured torques.
+ double print_rate = std::stod(argv[4]);
+ if (print_rate < 0.0) {
+ std::cerr << "print_rate too small, must be >= 0.0" << std::endl;
+ return -1;
+ }
+
+ // Initialize data fields for the print thread.
+ struct {
+ std::mutex mutex;
+ bool has_data;
+ std::array<double, 7> tau_d_last;
+ franka::RobotState robot_state;
+ std::array<double, 7> gravity;
+ } print_data{};
+ std::atomic_bool running{true};
+
+ // Start print thread.
+ 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<int>((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) {
+ std::array<double, 7> tau_error{};
+ double error_rms(0.0);
+ std::array<double, 7> tau_d_actual{};
+ 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();
+ }
+ // 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
+ << "-----------------------" << std::endl;
+ print_data.has_data = false;
+ print_data.mutex.unlock();
+ }
+ }
+ });
+
+ try {
+ // Connect to robot.
+ 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}});
+
+ // Load the kinematics and dynamics model.
+ franka::Model model = robot.loadModel();
+
+ // Read the initial pose to start the motion from there.
+ std::array<double, 16> initial_pose = robot.readOnce().O_T_EE;
+
+ // Define callback function to send Cartesian pose goals to get inverse
+ // kinematics solved.
+ std::function<franka::CartesianPose(const franka::RobotState &, franka::Duration)> 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);
+ }
+ if (vel_current > 0.0 && time > run_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);
+
+ // Compute new angle for our circular trajectory.
+ angle += period.toSec() * vel_current / std::fabs(radius);
+ if (angle > 2 * M_PI) {
+ angle -= 2 * M_PI;
+ }
+
+ // Compute relative y and z positions of desired pose.
+ double delta_y = radius * (1 - std::cos(angle));
+ double delta_z = radius * std::sin(angle);
+ franka::CartesianPose pose_desired = initial_pose;
+ pose_desired.O_T_EE[13] += delta_y;
+ pose_desired.O_T_EE[14] += delta_z;
+
+ // Send desired pose.
+ if (time >= run_time + acceleration_time) {
+ running = false;
+ return franka::MotionFinished(pose_desired);
+ }
+
+ return pose_desired;
+ };
+
+ // Set gains for the joint impedance control.
+ // Stiffness
+ const std::array<double, 7> k_gains = {{1000.0, 1000.0, 1000.0, 1000.0, 500.0, 300.0, 100.0}};
+ // Damping
+ const std::array<double, 7> 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<franka::Torques(const franka::RobotState &, franka::Duration)> 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<double, 7> 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<double, 7> 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];
+ }
+
+ // Update data to print.
+ if (print_data.mutex.try_lock()) {
+ print_data.has_data = true;
+ print_data.robot_state = state;
+ print_data.tau_d_last = tau_d;
+ print_data.gravity = model.gravity(state, 0.0, {{0.0, 0.0, 0.0}});
+ print_data.mutex.unlock();
+ }
+
+ // Send torque command.
+ return tau_d;
+ };
+
+ // Start real-time control loop.
+ robot.control(impedance_control_callback, cartesian_pose_callback);
+
+ } catch (const franka::Exception &ex) {
+ running = false;
+ std::cerr << ex.what() << std::endl;
+ }
+
+ if (print_thread.joinable()) {
+ print_thread.join();
+ }
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..30f2349
--- /dev/null
+++ b/example/servo-franka/franka_joint_point_to_point_motion.cpp
@@ -0,0 +1,216 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+#include <vector>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_joint_point_to_point_motion.cpp
+ * An example showing how to generate a joint pose motion to a goal position.
+ * Adapted from: Wisama Khalil and Etienne Dombre. 2002. Modeling,
+ * Identification and Control of Robots (Kogan Page Science Paper edition).
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+constexpr double kDeltaQMotionFinished = 1e-6;
+
+inline int sgn(double x)
+{
+ if (x == 0) {
+ return 0;
+ }
+ return (x > 0) ? 1 : -1;
+}
+
+std::array<double, 7> add(const std::array<double, 7> &a, const std::array<double, 7> &b)
+{
+ std::array<double, 7> result;
+ for (size_t i = 0; i < a.size(); i++) {
+ result[i] = a[i] + b[i];
+ }
+ return result;
+}
+
+std::array<double, 7> subtract(const std::array<double, 7> &a, const std::array<double, 7> &b)
+{
+ std::array<double, 7> result;
+ for (size_t i = 0; i < a.size(); i++) {
+ result[i] = a[i] - b[i];
+ }
+ return result;
+}
+
+bool calculateDesiredValues(double t, const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
+ const std::array<double, 7> &t_1, const std::array<double, 7> &t_2,
+ const std::array<double, 7> &t_f, const std::array<double, 7> &q_1,
+ std::array<double, 7> *delta_q_d);
+
+void calculateSynchronizedValues(const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
+ const std::array<double, 7> &ddq_max_start, const std::array<double, 7> &ddq_max_goal,
+ std::array<double, 7> *dq_max_sync, std::array<double, 7> *t_1_sync,
+ std::array<double, 7> *t_2_sync, std::array<double, 7> *t_f_sync,
+ std::array<double, 7> *q_1);
+
+int main(int argc, char **argv)
+{
+ if (argc != 10) {
+ std::cerr << "Usage: ./generate_joint_pose_motion "
+ << "<robot-hostname> <goal-position> <speed-factor>" << std::endl
+ << "speed-factor must be between zero and one." << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+ std::array<double, 7> q_goal;
+ for (size_t i = 0; i < 7; i++) {
+ q_goal[i] = std::stod(argv[i + 2]);
+ }
+ double speed_factor = std::stod(argv[9]);
+
+ // 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}});
+
+ std::array<double, 7> q_start = robot.readOnce().q_d;
+
+ std::array<double, 7> dq_max{{2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5}};
+ std::array<double, 7> ddq_max_start{{5, 5, 5, 5, 5, 5, 5}};
+ std::array<double, 7> ddq_max_goal{{5, 5, 5, 5, 5, 5, 5}};
+ for (size_t i = 0; i < 7; i++) {
+ dq_max[i] = speed_factor * dq_max[i];
+ ddq_max_start[i] = speed_factor * ddq_max_start[i];
+ ddq_max_goal[i] = speed_factor * ddq_max_goal[i];
+ }
+
+ double time = 0.0;
+
+ std::array<double, 7> dq_max_sync{};
+ std::array<double, 7> t_1_sync{};
+ std::array<double, 7> t_2_sync{};
+ std::array<double, 7> t_f_sync{};
+ std::array<double, 7> q_1{};
+ std::array<double, 7> 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<double, 7> 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;
+ return -1;
+ }
+
+ return 0;
+}
+
+bool calculateDesiredValues(double t, const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
+ const std::array<double, 7> &t_1, const std::array<double, 7> &t_2,
+ const std::array<double, 7> &t_f, const std::array<double, 7> &q_1,
+ std::array<double, 7> *delta_q_d)
+{
+ std::array<int, 7> sign_delta_q;
+ std::array<double, 7> t_d = subtract(t_2, t_1);
+ std::array<double, 7> delta_t_2 = subtract(t_f, t_2);
+ std::array<bool, 7> joint_motion_finished{};
+
+ for (size_t i = 0; i < 7; i++) {
+ sign_delta_q[i] = sgn(delta_q[i]);
+ if (std::abs(delta_q[i]) < kDeltaQMotionFinished) {
+ (*delta_q_d)[i] = 0;
+ 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);
+ } 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];
+ } 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; });
+}
+
+void calculateSynchronizedValues(const std::array<double, 7> &delta_q, const std::array<double, 7> &dq_max,
+ const std::array<double, 7> &ddq_max_start, const std::array<double, 7> &ddq_max_goal,
+ std::array<double, 7> *dq_max_sync, std::array<double, 7> *t_1_sync,
+ std::array<double, 7> *t_2_sync, std::array<double, 7> *t_f_sync,
+ std::array<double, 7> *q_1)
+{
+ std::array<double, 7> dq_max_reach = dq_max;
+ std::array<double, 7> t_f{};
+ std::array<double, 7> delta_t_2{};
+ std::array<double, 7> t_1{};
+ std::array<double, 7> delta_t_2_sync{};
+ int sign_delta_q[7];
+ 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]) /
+ (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];
+ }
+ }
+
+ double max_t_f = *std::max_element(t_f.begin(), t_f.end());
+ for (size_t i = 0; i < 7; i++) {
+ if (std::abs(delta_q[i]) > kDeltaQMotionFinished) {
+ double a = 1.5 / 2.0 * (ddq_max_goal[i] + ddq_max_start[i]);
+ double b = -1.0 * max_t_f * ddq_max_goal[i] * ddq_max_start[i];
+ double c = std::abs(delta_q[i]) * ddq_max_goal[i] * ddq_max_start[i];
+ double delta = b * b - 4.0 * a * c;
+ (*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_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]);
+ }
+ }
+}
+
+#else
+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
new file mode 100644
index 0000000..ed64fd2
--- /dev/null
+++ b/example/servo-franka/franka_motion_with_control.cpp
@@ -0,0 +1,183 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <cmath>
+#include <iomanip>
+#include <iostream>
+#include <vector>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/robot.h>
+
+/**
+ * @example franka_motion_with_control.cpp
+ * An example showing how to use a joint velocity motion generator and torque
+ * control.
+ *
+ * @warning Before executing this example, make sure there is enough space in
+ * front of the robot.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+namespace
+{
+
+class Controller
+{
+public:
+ Controller(size_t dq_filter_size, const std::array<double, 7> &K_P, // NOLINT
+ const std::array<double, 7> &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);
+ }
+
+ inline franka::Torques step(const franka::RobotState &state)
+ {
+ updateDQFilter(state);
+
+ std::array<double, 7> 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));
+ }
+ return tau_J_d;
+ }
+
+ void updateDQFilter(const franka::RobotState &state)
+ {
+ 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_;
+ }
+
+ double getDQFiltered(size_t index) const
+ {
+ double value = 0;
+ for (size_t i = index; i < 7 * dq_filter_size_; i += 7) {
+ value += dq_buffer_.get()[i];
+ }
+ return value / dq_filter_size_;
+ }
+
+private:
+ size_t dq_current_filter_position_;
+ size_t dq_filter_size_;
+
+ const std::array<double, 7> K_P_; // NOLINT
+ const std::array<double, 7> K_D_; // NOLINT
+
+ std::array<double, 7> dq_d_;
+ std::unique_ptr<double[]> dq_buffer_;
+};
+
+std::vector<double> generateTrajectory(double a_max)
+{
+ // Generating a motion with smooth velocity and acceleration.
+ // Squared sine is used for the acceleration/deceleration phase.
+ std::vector<double> 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]
+ // obtained during the speed up
+ // and slow down [rad/s^2]
+ double a = 0; // [rad/s^2]
+ double v = 0; // [rad/s]
+ double t = 0; // [s]
+ while (t < (2 * kAccelerationTime + kConstantVelocityTime)) {
+ if (t <= kAccelerationTime) {
+ a = pow(sin(t * M_PI / kAccelerationTime), 2) * a_max;
+ } else if (t <= (kAccelerationTime + kConstantVelocityTime)) {
+ a = 0;
+ } else {
+ const double deceleration_time =
+ (kAccelerationTime + kConstantVelocityTime) - t; // time spent in the deceleration phase
+ a = -pow(sin(deceleration_time * M_PI / kAccelerationTime), 2) * a_max;
+ }
+ v += a * kTimeStep;
+ t += kTimeStep;
+ trajectory.push_back(v);
+ }
+ return trajectory;
+}
+
+} // anonymous namespace
+
+int main(int argc, char **argv)
+{
+ if (argc != 7) {
+ std::cerr << "Usage: ./" << argv[0] << " <robot-hostname>"
+ << " <filter size>"
+ << " <K_P>"
+ << " <K_D>"
+ << " <joint>"
+ << " <a_max>" << std::endl;
+ return -1;
+ }
+
+ size_t filter_size = std::stoul(argv[2]);
+ std::array<double, 7> K_P; // NOLINT
+ std::array<double, 7> K_D; // NOLINT
+ for (size_t i = 0; i < 7; i++) {
+ K_P[i] = std::stod(argv[3]);
+ K_D[i] = std::stod(argv[4]);
+ }
+
+ 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 << "dq filter size: " << filter_size << std::endl;
+ Controller controller(filter_size, K_P, K_D);
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ // 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}});
+
+ size_t index = 0;
+ int joint_number = std::stoi(argv[5]);
+ std::vector<double> 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;
+ });
+ } catch (const franka::Exception &e) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+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
new file mode 100644
index 0000000..491cfdc
--- /dev/null
+++ b/example/servo-franka/franka_print_joint_positions.cpp
@@ -0,0 +1,57 @@
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <iostream>
+#include <iterator>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_FRANKA
+
+#include <franka/exception.h>
+#include <franka/model.h>
+
+/**
+ * @example franka_print_joint_positions.cpp
+ * An example showing how to use the model library.
+ *
+ * This example is part of libfranka FCI C++ API:
+ * https://frankaemika.github.io/libfranka See
+ * https://frankaemika.github.io/docs for more details.
+ */
+
+template <class T, size_t N> std::ostream &operator<<(std::ostream &ostream, const std::array<T, N> &array)
+{
+ ostream << "[";
+ std::copy(array.cbegin(), array.cend() - 1, std::ostream_iterator<T>(ostream, ","));
+ std::copy(array.cend() - 1, array.cend(), std::ostream_iterator<T>(ostream));
+ ostream << "]";
+ return ostream;
+}
+
+int main(int argc, char **argv)
+{
+ if (argc != 2) {
+ std::cerr << "Usage: ./print_joint_positions <robot-hostname>" << std::endl;
+ return -1;
+ }
+
+ try {
+ franka::Robot robot(argv[1]);
+
+ franka::RobotState state = robot.readOnce();
+
+ franka::Model model(robot.loadModel());
+ 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) {
+ std::cout << e.what() << std::endl;
+ return -1;
+ }
+
+ return 0;
+}
+
+#else
+int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
+#endif
diff --git a/example/servo-pioneer/CMakeLists.txt b/example/servo-pioneer/CMakeLists.txt
index 452c232..8869836 100644
--- a/example/servo-pioneer/CMakeLists.txt
+++ b/example/servo-pioneer/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -55,6 +56,7 @@ foreach(cpp ${example_cpp})
if(VISP_HAVE_PIONEER)
# Add specific build flag to turn off warnings coming from libaria 3rd party
visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-type-limits)
endif()
if(COMMAND visp_add_dependency)
diff --git a/example/servo-pioneer/movePioneer.cpp b/example/servo-pioneer/movePioneer.cpp
index ab64edd..d0a1b51 100644
--- a/example/servo-pioneer/movePioneer.cpp
+++ b/example/servo-pioneer/movePioneer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,7 +45,8 @@
#ifndef VISP_HAVE_PIONEER
int main()
{
- std::cout << "\nThis example requires Aria 3rd party library. You should install it.\n"
+ std::cout << "\nThis example requires Aria 3rd party library. You should "
+ "install it.\n"
<< std::endl;
return 0;
}
@@ -55,9 +57,9 @@ int main()
\example movePioneer.cpp example showing how to connect and send
direct basic motion commands to a Pioneer mobile robot.
- WARNING: this program does no sensing or avoiding of obstacles, the robot WILL
- collide with any objects in the way! Make sure the robot has about 2-3
- meters of free space around it before starting the program.
+ WARNING: this program does no sensing or avoiding of obstacles, the robot
+ WILL collide with any objects in the way! Make sure the robot has about
+ 2-3 meters of free space around it before starting the program.
This program will work either with the MobileSim simulator or on a real
robot's onboard computer. (Or use -remoteHost to connect to a wireless
@@ -66,8 +68,10 @@ 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"
+ 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;
@@ -76,20 +80,17 @@ int main(int argc, char **argv)
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
- // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
- // and then loads parameter files for this robot.
+ // ArRobotConnector connects to the robot, get some initial data from it
+ // such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
- if(!robotConnector.connectRobot())
- {
+ if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
- if(parser.checkHelpAndWarnUnparsed())
- {
+ if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
- if (!Aria::parseArgs())
- {
+ if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
@@ -101,13 +102,12 @@ int main(int argc, char **argv)
// Robot velocities
vpColVector v(2), v_mes(2);
- for (int i=0; i < 100; i++)
- {
+ for (int i = 0; i < 100; i++) {
double t = vpTime::measureTimeMs();
v = 0;
- v[0] = i/1000.; // Translational velocity in m/s
- //v[1] = vpMath::rad(i/5.); // Rotational velocity in rad/sec
+ v[0] = i / 1000.; // Translational velocity in m/s
+ // v[1] = vpMath::rad(i/5.); // Rotational velocity in rad/sec
robot.setVelocity(vpRobot::REFERENCE_FRAME, v);
v_mes = robot.getVelocity(vpRobot::REFERENCE_FRAME);
@@ -126,7 +126,9 @@ int main(int argc, char **argv)
ArUtil::sleep(1000);
robot.lock();
- ArLog::log(ArLog::Normal, "simpleMotionCommands: Pose=(%.2f,%.2f,%.2f), Trans. Vel=%.2f, Rot. Vel=%.2f, Battery=%.2fV",
+ 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.unlock();
@@ -139,12 +141,10 @@ int main(int argc, char **argv)
// exit
ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#endif
-
diff --git a/example/servo-pioneer/servoPioneerPanSegment3D.cpp b/example/servo-pioneer/servoPioneerPanSegment3D.cpp
index aea7cb1..7df14ef 100644
--- a/example/servo-pioneer/servoPioneerPanSegment3D.cpp
+++ b/example/servo-pioneer/servoPioneerPanSegment3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,23 +39,23 @@
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
-#include <visp3/robot/vpRobotBiclops.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureSegment.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpPioneerPan.h>
+#include <visp3/robot/vpRobotBiclops.h>
+#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/robot/vpPioneerPan.h>
-#include <visp3/gui/vpPlot.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
#define USE_REAL_ROBOT
#define USE_PLOTTER
@@ -63,16 +64,18 @@
/*!
\example servoPioneerPanSegment3D.cpp
- Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a segment.
- The segment consists in two horizontal dots. The current visual features that are used are
- \f${\bf s} = (x_n, l_n, \alpha)\f$. The desired one are \f${\bf s^*} = (0, l_n*, 0)\f$, with:
+ Example that shows how to control the Pioneer mobile robot by IBVS visual
+ servoing with respect to a segment. The segment consists in two horizontal
+ dots. The current visual features that are used are \f${\bf s} = (x_n, l_n,
+ \alpha)\f$. The desired one are \f${\bf s^*} = (0, l_n*, 0)\f$, with:
- \f$x_n\f$ the normalized abscisse of the point corresponding to segment
- \f$l_n\f$ the normalized segment length
- \f$\alpha\f$ the segment orientation.
- The degrees of freedom that are controlled are \f$(v_x, w_z, \dot{q})\f$, the translational and rotational
- velocity of the mobile platform at point M located at the middle between the two wheels, the head pan
- velocity respectively.
+ The degrees of freedom that are controlled are \f$(v_x, w_z, \dot{q})\f$,
+ the translational and rotational velocity of the mobile platform at point M
+ located at the middle between the two wheels, the head pan velocity
+ respectively.
The depth of the points is estimated from the surface of the blob.
@@ -85,16 +88,19 @@ int main(int argc, char **argv)
try {
vpImage<unsigned char> I; // Create a gray level image container
double lambda = 0.1;
- // Scale parameter used to estimate the depth Z of the blob from its 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
-
- // 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 segment
- double Y_d = -.11; // Desired distance along Y between camera and segment.
+ // Scale parameter used to estimate the depth Z of the blob from its
+ // 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
+
+ // 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
+ // segment
+ 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)
@@ -108,38 +114,36 @@ int main(int argc, char **argv)
// Move to the initial position
vpColVector q(2);
- q=0;
+ q = 0;
// q[0] = vpMath::rad(63);
- // q[1] = vpMath::rad(12); // introduce a tilt angle to compensate camera sphere tilt so that the camera is parallel to the plane
+ // q[1] = vpMath::rad(12); // introduce a tilt angle to compensate camera
+ // sphere tilt so that the camera is parallel to the plane
- biclops.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- biclops.setPosition( vpRobot::ARTICULAR_FRAME, q );
- //biclops.setPositioningVelocity(50);
+ biclops.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ biclops.setPosition(vpRobot::ARTICULAR_FRAME, q);
+ // biclops.setPositioningVelocity(50);
biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
qm_pan = qm[0];
// Now the head will be controlled in velocity
- biclops.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ biclops.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Initialize the pioneer robot
vpRobotPioneer pioneer;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
- // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
- // and then loads parameter files for this robot.
+ // ArRobotConnector connects to the robot, get some initial data from it
+ // such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &pioneer);
- if(!robotConnector.connectRobot())
- {
+ if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the pioneer robot.");
- if(parser.checkHelpAndWarnUnparsed())
- {
+ if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
- if (!Aria::parseArgs())
- {
+ if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
@@ -148,22 +152,25 @@ int main(int argc, char **argv)
pioneer.useSonar(false); // disable the sonar device usage
// Wait 3 sec to be sure that the low level Aria thread used to control
- // the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
- // between the velocity send to the robot and the velocity that is really applied
- // to the wheels.
+ // the robot is started. Without this delay we experienced a delay
+ // (arround 2.2 sec) between the velocity send to the robot and the
+ // velocity that is really applied to the wheels.
sleep(3);
std::cout << "Pioneer robot connected" << std::endl;
#endif
- vpPioneerPan robot_pan; // Generic robot that computes the velocities for the pioneer and the biclops head
+ vpPioneerPan robot_pan; // Generic robot that computes the velocities for
+ // the pioneer and the biclops head
- // Camera parameters. In this experiment we don't need a precise calibration of the camera
+ // Camera parameters. In this experiment we don't need a precise
+ // calibration of the camera
vpCameraParameters cam;
- // Create the camera framegrabber
+// Create the camera framegrabber
#if defined(VISP_HAVE_V4L2)
- // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
+ // Create a grabber based on v4l2 third party lib (for usb cameras under
+ // Linux)
vpV4l2Grabber g;
g.setScale(1);
g.setInput(0);
@@ -172,14 +179,16 @@ int main(int argc, char **argv)
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_DC1394)
- // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
+ // Create a grabber based on libdc1394-2.x third party lib (for firewire
+ // cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#elif defined(VISP_HAVE_CMU1394)
- // Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
+ // Create a grabber based on CMU 1394 third party lib (for firewire
+ // cameras under windows)
vp1394CMUGrabber g;
g.setVideoMode(0, 5); // 640x480 MONO8
g.setFramerate(4); // 30 Hz
@@ -191,7 +200,7 @@ int main(int argc, char **argv)
// Acquire an image from the grabber
g.acquire(I);
- // Create an image viewer
+// Create an image viewer
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
@@ -202,24 +211,25 @@ int main(int argc, char **argv)
// The 3D segment consists in two horizontal dots
vpDot2 dot[2];
- for (int i=0; i <2; i++)
- {
+ 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].setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
+ 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
dot[i].initTracking(I);
vpDisplay::flush(I);
}
vpServo task;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
- task.setLambda(lambda) ;
- vpVelocityTwistMatrix cVe ; // keep to identity
- cVe = robot_pan.get_cVe() ;
- task.set_cVe(cVe) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
+ task.setLambda(lambda);
+ vpVelocityTwistMatrix cVe; // keep to identity
+ cVe = robot_pan.get_cVe();
+ task.set_cVe(cVe);
std::cout << "cVe: \n" << cVe << std::endl;
@@ -228,56 +238,57 @@ int main(int argc, char **argv)
// Update the robot jacobian that depends on the pan position
robot_pan.set_eJe(qm_pan);
// Get the robot jacobian
- eJe = robot_pan.get_eJe() ;
- task.set_eJe(eJe) ;
+ eJe = robot_pan.get_eJe();
+ task.set_eJe(eJe);
std::cout << "eJe: \n" << eJe << std::endl;
// Define a 3D horizontal segment an its cordinates in the image plane
vpPoint P[2];
- P[0].setWorldCoordinates(-L/2, 0, 0);
- P[1].setWorldCoordinates( L/2, 0, 0);
+ P[0].setWorldCoordinates(-L / 2, 0, 0);
+ P[1].setWorldCoordinates(L / 2, 0, 0);
// Define the desired camera position
- vpHomogeneousMatrix cMo(0, Y_d, Z_d, 0, 0, 0); // Here we are in front of the segment
- for (int i=0; i <2; i++)
- {
+ vpHomogeneousMatrix cMo(0, Y_d, Z_d, 0, 0,
+ 0); // Here we are in front of the segment
+ for (int i = 0; i < 2; i++) {
P[i].changeFrame(cMo);
- P[i].project(); // Here the x,y parameters obtained by perspective projection are computed
+ P[i].project(); // Here the x,y parameters obtained by perspective
+ // projection are computed
}
// Estimate the depth of the segment extremity points
double surface[2];
double Z[2]; // Depth of the segment points
- for (int i=0; i<2; i++)
- {
- // Surface of the blob estimated from the image moment m00 and converted in meters
- surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
+ for (int i = 0; i < 2; i++) {
+ // Surface of the blob estimated from the image moment m00 and converted
+ // in meters
+ surface[i] = 1. / sqrt(dot[i].m00 / (cam.get_px() * cam.get_py()));
// Initial depth of the blob
- Z[i] = coef * surface[i] ;
+ Z[i] = coef * surface[i];
}
// Use here a feature segment builder
- vpFeatureSegment s_segment(normalized), s_segment_d(normalized); // From the segment feature we use only alpha
+ vpFeatureSegment s_segment(normalized),
+ s_segment_d(normalized); // From the segment feature we use only alpha
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
// Set the desired feature
vpFeatureBuilder::create(s_segment_d, P[0], P[1]);
- s_segment.setZ1( P[0].get_Z() ); // Desired depth
- s_segment.setZ2( P[1].get_Z() );
+ s_segment.setZ1(P[0].get_Z()); // Desired depth
+ 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
+ // Create a window (500 by 500) at position (700, 10) with two graphics
vpPlot graph(2, 500, 500, 700, 10, "Curves...");
- //The first graphic contains 3 curve and the second graphic contains 3 curves
- graph.initGraph(0,3);
- graph.initGraph(1,3);
+ // The first graphic contains 3 curve and the second graphic contains 3
+ // curves
+ graph.initGraph(0, 3);
+ graph.initGraph(1, 3);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setLegend(0, 0, "vx");
@@ -290,11 +301,9 @@ int main(int argc, char **argv)
vpColVector v; // vz, wx
- try
- {
+ try {
unsigned int iter = 0;
- while(1)
- {
+ while (1) {
#ifdef USE_REAL_ROBOT
// Get the new pan position
biclops.getPosition(vpRobot::ARTICULAR_FRAME, qm);
@@ -307,26 +316,27 @@ int main(int argc, char **argv)
vpDisplay::display(I);
// Display the desired position of the segment
- for (int i=0; i<2; i++)
+ for (int i = 0; i < 2; i++)
P[i].display(I, cam, vpColor::red, 3);
// Does the blob tracking
- for (int i=0; i<2; i++)
+ for (int i = 0; i < 2; i++)
dot[i].track(I);
- for (int i=0; i<2; i++)
- {
- // Surface of the blob estimated from the image moment m00 and converted in meters
- surface[i] = 1./sqrt(dot[i].m00/(cam.get_px()*cam.get_py()));
+ for (int i = 0; i < 2; i++) {
+ // Surface of the blob estimated from the image moment m00 and
+ // converted in meters
+ surface[i] = 1. / sqrt(dot[i].m00 / (cam.get_px() * cam.get_py()));
// Initial depth of the blob
- Z[i] = coef * surface[i] ;
+ Z[i] = coef * surface[i];
}
// Update the features
vpFeatureBuilder::create(s_segment, cam, dot[0], dot[1]);
- // Update the depth of the point. Useful only if current interaction matrix is used
- // when task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) is set
+ // Update the depth of the point. Useful only if current interaction
+ // matrix is used when task.setInteractionMatrixType(vpServo::CURRENT,
+ // vpServo::PSEUDO_INVERSE) is set
s_segment.setZ1(Z[0]);
s_segment.setZ2(Z[1]);
@@ -340,22 +350,24 @@ int main(int argc, char **argv)
// Update the jacobian that will be used to compute the control law
task.set_eJe(eJe);
- // Compute the control law. Velocities are computed in the mobile robot reference frame
+ // Compute the control law. Velocities are computed in the mobile
+ // robot reference frame
v = task.computeControlLaw();
// std::cout << "-----" << std::endl;
// std::cout << "v: " << v.t() << std::endl;
// std::cout << "error: " << task.getError().t() << std::endl;
- // std::cout << "L:\n " << task.getInteractionMatrix() << std::endl;
- // std::cout << "eJe:\n " << task.get_eJe() << std::endl;
- // std::cout << "cVe:\n " << task.get_cVe() << std::endl;
- // std::cout << "L_cVe_eJe:\n" << task.getInteractionMatrix() * task.get_cVe() * task.get_eJe() << std::endl;
- // task.print() ;
+ // std::cout << "L:\n " << task.getInteractionMatrix() <<
+ // std::endl; std::cout << "eJe:\n " << task.get_eJe() <<
+ // std::endl; std::cout << "cVe:\n " << task.get_cVe() <<
+ // std::endl; std::cout << "L_cVe_eJe:\n" <<
+ // 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;
#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
@@ -368,28 +380,27 @@ 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 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) ;
+ biclops.setVelocity(vpRobot::ARTICULAR_FRAME, v_biclops);
#endif
- // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
+ // 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::flush(I);
// A click in the viewer to exit
- if ( vpDisplay::getClick(I, false) )
+ if (vpDisplay::getClick(I, false))
break;
- iter ++;
- //break;
+ iter++;
+ // break;
}
- }
- catch(...)
- {
+ } catch (...) {
}
#ifdef USE_REAL_ROBOT
@@ -401,10 +412,9 @@ int main(int argc, char **argv)
#endif
// Kill the servo task
- task.print() ;
+ task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
index 51c7bd3..7bcfac8 100644
--- a/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
+++ b/example/servo-pioneer/servoPioneerPoint2DDepth.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,52 +39,56 @@
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotPioneer.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureDepth.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpRobotPioneer.h>
#include <visp3/sensor/vp1394CMUGrabber.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpOpenCVGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)
+#if defined(VISP_HAVE_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
+#define TEST_COULD_BE_ACHIEVED
#endif
#endif
#endif
#undef VISP_HAVE_OPENCV // To use a firewire camera
-#undef VISP_HAVE_V4L2 // To use a firewire camera
+#undef VISP_HAVE_V4L2 // To use a firewire camera
/*!
\example servoPioneerPoint2DDepth.cpp
- Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a blob.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- - x the abscisse of the point corresponding to the blob center of gravity measured at each iteration,
+ Example that shows how to control the Pioneer mobile robot by IBVS visual
+ servoing with respect to a blob. The current visual features that are used
+ are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ - x the abscisse of the point corresponding to the blob center of gravity
+ measured at each iteration,
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
The value of x is measured thanks to a blob tracker.
- The value of Z is estimated from the surface of the blob that is proportional to the depth Z.
+ The value of Z is estimated from the surface of the blob that is
+ proportional to the depth Z.
*/
#ifdef TEST_COULD_BE_ACHIEVED
@@ -93,50 +98,49 @@ int main(int argc, char **argv)
vpImage<unsigned char> I; // Create a gray level image container
double depth = 1.;
double lambda = 0.6;
- double coef = 1./6.77; // Scale parameter used to estimate the depth Z of the blob from its surface
+ double coef = 1. / 6.77; // Scale parameter used to estimate the depth Z
+ // of the blob from its surface
vpRobotPioneer robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
- // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
- // and then loads parameter files for this robot.
+ // ArRobotConnector connects to the robot, get some initial data from it
+ // such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
- if(!robotConnector.connectRobot())
- {
+ if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
- if(parser.checkHelpAndWarnUnparsed())
- {
+ if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
- if (!Aria::parseArgs())
- {
+ if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
}
// Wait 3 sec to be sure that the low level Aria thread used to control
- // the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
- // between the velocity send to the robot and the velocity that is really applied
- // to the wheels.
+ // the robot is started. Without this delay we experienced a delay
+ // (arround 2.2 sec) between the velocity send to the robot and the
+ // velocity that is really applied to the wheels.
vpTime::sleepMs(3000);
std::cout << "Robot connected" << std::endl;
- // Camera parameters. In this experiment we don't need a precise calibration of the camera
+ // Camera parameters. In this experiment we don't need a precise
+ // calibration of the camera
vpCameraParameters cam;
- // Create the camera framegrabber
+// Create the camera framegrabber
#if defined(VISP_HAVE_OPENCV)
int device = 1;
std::cout << "Use device: " << device << std::endl;
cv::VideoCapture g(device); // open the default camera
g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
- if(!g.isOpened()) // check if we succeeded
+ if (!g.isOpened()) // check if we succeeded
return -1;
cv::Mat frame;
g >> frame; // get a new frame from camera
@@ -145,7 +149,8 @@ int main(int argc, char **argv)
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_V4L2)
- // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
+ // Create a grabber based on v4l2 third party lib (for usb cameras under
+ // Linux)
vpV4l2Grabber g;
g.setScale(1);
g.setInput(0);
@@ -154,14 +159,16 @@ int main(int argc, char **argv)
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_DC1394)
- // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
+ // Create a grabber based on libdc1394-2.x third party lib (for firewire
+ // cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#elif defined(VISP_HAVE_CMU1394)
- // Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
+ // Create a grabber based on CMU 1394 third party lib (for firewire
+ // cameras under windows)
vp1394CMUGrabber g;
g.setVideoMode(0, 5); // 640x480 MONO8
g.setFramerate(4); // 30 Hz
@@ -170,7 +177,7 @@ int main(int argc, char **argv)
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#endif
- // Acquire an image from the grabber
+// Acquire an image from the grabber
#if defined(VISP_HAVE_OPENCV)
g >> frame; // get a new frame from camera
vpImageConvert::convert(frame, I);
@@ -178,7 +185,7 @@ int main(int argc, char **argv)
g.acquire(I);
#endif
- // Create an image viewer
+// Create an image viewer
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
@@ -191,28 +198,31 @@ 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.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
+ 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
dot.initTracking(I);
vpDisplay::flush(I);
vpServo task;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
- task.setLambda(lambda) ;
- vpVelocityTwistMatrix cVe ;
- cVe = robot.get_cVe() ;
- task.set_cVe(cVe) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
+ task.setLambda(lambda);
+ vpVelocityTwistMatrix cVe;
+ cVe = robot.get_cVe();
+ task.set_cVe(cVe);
std::cout << "cVe: \n" << cVe << std::endl;
vpMatrix eJe;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
std::cout << "eJe: \n" << eJe << std::endl;
- // Current and desired visual feature associated to the x coordinate of the point
+ // Current and desired visual feature associated to the x coordinate of
+ // the point
vpFeaturePoint s_x, s_xd;
// Create the current x visual feature
@@ -222,30 +232,33 @@ int main(int argc, char **argv)
s_xd.buildFrom(0, 0, depth);
// Add the feature
- task.addFeature(s_x, s_xd) ;
+ task.addFeature(s_x, s_xd);
// Create the current log(Z/Z*) visual feature
vpFeatureDepth s_Z, s_Zd;
- // Surface of the blob estimated from the image moment m00 and converted in meters
- double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
+ // Surface of the blob estimated from the image moment m00 and converted
+ // in meters
+ double surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
double Z, Zd;
// Initial depth of the blob in from of the camera
- Z = coef * surface ;
- // Desired depth Z* of the blob. This depth is learned and equal to the initial depth
+ Z = coef * surface;
+ // Desired depth Z* of the blob. This depth is learned and equal to the
+ // initial depth
Zd = Z;
std::cout << "Z " << Z << std::endl;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
- s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z,
+ 0); // log(Z/Z*) = 0 that's why the last parameter is 0
+ s_Zd.buildFrom(s_x.get_x(), s_x.get_y(), Zd,
+ 0); // log(Z/Z*) = 0 that's why the last parameter is 0
// Add the feature
- task.addFeature(s_Z, s_Zd) ;
+ task.addFeature(s_Z, s_Zd);
vpColVector v; // vz, wx
- while(1)
- {
- // Acquire a new image
+ while (1) {
+// Acquire a new image
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
g >> frame; // get a new frame from camera
vpImageConvert::convert(frame, I);
@@ -260,32 +273,34 @@ int main(int argc, char **argv)
// Update the current x feature
vpFeatureBuilder::create(s_x, cam, dot);
- // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
- surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
- Z = coef * surface ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
+ // Update log(Z/Z*) feature. Since the depth Z change, we need to update
+ // the intection matrix
+ surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
+ Z = coef * surface;
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
- robot.get_cVe(cVe) ;
- task.set_cVe(cVe) ;
+ robot.get_cVe(cVe);
+ task.set_cVe(cVe);
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
- // Compute the control law. Velocities are computed in the mobile robot reference frame
- v = task.computeControlLaw() ;
+ // Compute the control law. Velocities are computed in the mobile robot
+ // 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);
- // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
+ // Draw a vertical line which corresponds to the desired x coordinate of
+ // the dot cog
vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
vpDisplay::flush(I);
// A click in the viewer to exit
- if ( vpDisplay::getClick(I, false) )
+ if (vpDisplay::getClick(I, false))
break;
}
@@ -296,17 +311,13 @@ int main(int argc, char **argv)
robot.waitForRunExit();
// Kill the servo task
- task.print() ;
+ task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 27036ef..0aecc1c 100644
--- a/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp
+++ b/example/servo-pioneer/servoPioneerPoint2DDepthWithoutVpServo.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,52 +37,56 @@
*****************************************************************************/
#include <iostream>
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotPioneer.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureDepth.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpRobotPioneer.h>
#include <visp3/sensor/vp1394CMUGrabber.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpOpenCVGrabber.h>
#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
-#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
+#define TEST_COULD_BE_ACHIEVED
#endif
#endif
#endif
#undef VISP_HAVE_OPENCV // To use a firewire camera
-#undef VISP_HAVE_V4L2 // To use a firewire camera
+#undef VISP_HAVE_V4L2 // To use a firewire camera
/*!
\example servoPioneerPoint2DDepthWithoutVpServo.cpp
- Example that shows how to control the Pioneer mobile robot by IBVS visual servoing with respect to a blob.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- - x the abscisse of the point corresponding to the blob center of gravity measured at each iteration,
+ Example that shows how to control the Pioneer mobile robot by IBVS visual
+ servoing with respect to a blob. The current visual features that are used
+ are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ - x the abscisse of the point corresponding to the blob center of gravity
+ measured at each iteration,
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
The value of x is measured thanks to a blob tracker.
- The value of Z is estimated from the surface of the blob that is proportional to the depth Z.
+ The value of Z is estimated from the surface of the blob that is
+ proportional to the depth Z.
*/
#ifdef TEST_COULD_BE_ACHIEVED
@@ -91,50 +96,49 @@ int main(int argc, char **argv)
vpImage<unsigned char> I; // Create a gray level image container
double depth = 1.;
double lambda = 0.6;
- double coef = 1./6.77; // Scale parameter used to estimate the depth Z of the blob from its surface
+ double coef = 1. / 6.77; // Scale parameter used to estimate the depth Z
+ // of the blob from its surface
vpRobotPioneer robot;
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
- // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
- // and then loads parameter files for this robot.
+ // ArRobotConnector connects to the robot, get some initial data from it
+ // such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, &robot);
- if(!robotConnector.connectRobot())
- {
+ if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot.");
- if(parser.checkHelpAndWarnUnparsed())
- {
+ if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
- if (!Aria::parseArgs())
- {
+ if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
}
// Wait 3 sec to be sure that the low level Aria thread used to control
- // the robot is started. Without this delay we experienced a delay (arround 2.2 sec)
- // between the velocity send to the robot and the velocity that is really applied
- // to the wheels.
+ // the robot is started. Without this delay we experienced a delay
+ // (arround 2.2 sec) between the velocity send to the robot and the
+ // velocity that is really applied to the wheels.
vpTime::sleepMs(3000);
std::cout << "Robot connected" << std::endl;
- // Camera parameters. In this experiment we don't need a precise calibration of the camera
+ // Camera parameters. In this experiment we don't need a precise
+ // calibration of the camera
vpCameraParameters cam;
- // Create the camera framegrabber
+// Create the camera framegrabber
#if defined(VISP_HAVE_OPENCV)
int device = 1;
std::cout << "Use device: " << device << std::endl;
cv::VideoCapture g(device); // open the default camera
g.set(CV_CAP_PROP_FRAME_WIDTH, 640);
g.set(CV_CAP_PROP_FRAME_HEIGHT, 480);
- if(!g.isOpened()) // check if we succeeded
+ if (!g.isOpened()) // check if we succeeded
return -1;
cv::Mat frame;
g >> frame; // get a new frame from camera
@@ -143,7 +147,8 @@ int main(int argc, char **argv)
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_V4L2)
- // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
+ // Create a grabber based on v4l2 third party lib (for usb cameras under
+ // Linux)
vpV4l2Grabber g;
g.setScale(1);
g.setInput(0);
@@ -152,14 +157,16 @@ int main(int argc, char **argv)
// Logitec sphere parameters
cam.initPersProjWithoutDistortion(558, 555, 312, 210);
#elif defined(VISP_HAVE_DC1394)
- // Create a grabber based on libdc1394-2.x third party lib (for firewire cameras under Linux)
+ // Create a grabber based on libdc1394-2.x third party lib (for firewire
+ // cameras under Linux)
vp1394TwoGrabber g(false);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_30);
// AVT Pike 032C parameters
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#elif defined(VISP_HAVE_CMU1394)
- // Create a grabber based on CMU 1394 third party lib (for firewire cameras under windows)
+ // Create a grabber based on CMU 1394 third party lib (for firewire
+ // cameras under windows)
vp1394CMUGrabber g;
g.setVideoMode(0, 5); // 640x480 MONO8
g.setFramerate(4); // 30 Hz
@@ -168,7 +175,7 @@ int main(int argc, char **argv)
cam.initPersProjWithoutDistortion(800, 795, 320, 216);
#endif
- // Acquire an image from the grabber
+// Acquire an image from the grabber
#if defined(VISP_HAVE_OPENCV)
g >> frame; // get a new frame from camera
vpImageConvert::convert(frame, I);
@@ -176,7 +183,7 @@ int main(int argc, char **argv)
g.acquire(I);
#endif
- // Create an image viewer
+// Create an image viewer
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 10, 10, "Current frame");
#elif defined(VISP_HAVE_GDI)
@@ -189,13 +196,16 @@ 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.setEllipsoidBadPointsPercentage(0.5); // to be accept 50% of bad inner and outside points with bad gray level
+ 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
dot.initTracking(I);
vpDisplay::flush(I);
- // Current and desired visual feature associated to the x coordinate of the point
+ // Current and desired visual feature associated to the x coordinate of
+ // the point
vpFeaturePoint s_x, s_xd;
// Create the current x visual feature
@@ -207,37 +217,40 @@ int main(int argc, char **argv)
// Create the current log(Z/Z*) visual feature
vpFeatureDepth s_Z;
- // Surface of the blob estimated from the image moment m00 and converted in meters
- double surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
+ // Surface of the blob estimated from the image moment m00 and converted
+ // in meters
+ double surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
double Z, Zd;
// Initial depth of the blob in from of the camera
- Z = coef * surface ;
- // Desired depth Z* of the blob. This depth is learned and equal to the initial depth
+ Z = coef * surface;
+ // Desired depth Z* of the blob. This depth is learned and equal to the
+ // initial depth
Zd = Z;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z , 0); // log(Z/Z*) = 0 that's why the last parameter is 0
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z,
+ 0); // log(Z/Z*) = 0 that's why the last parameter is 0
vpMatrix L_Z = s_Z.interaction();
vpVelocityTwistMatrix cVe = robot.get_cVe();
vpMatrix eJe; // pioneer jacobian
robot.get_eJe(eJe);
- vpMatrix L; // Interaction matrix
+ vpMatrix L; // Interaction matrix
L.stack(L_x); // constant since build with the desired feature
- L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
+ L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that
+ // evolves at each iteration
vpColVector v; // vz, wx
vpFeatureDepth s_Zd;
s_Zd.buildFrom(0, 0, 1, 0); // The value of s* is 0 with Z=1 meter.
- while(1)
- {
- // Acquire a new image
+ while (1) {
+// Acquire a new image
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- g >> frame; // get a new frame from camera
- vpImageConvert::convert(frame, I);
+ g >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
#else
- g.acquire(I);
+ g.acquire(I);
#endif
// Set the image as background of the viewer
vpDisplay::display(I);
@@ -247,37 +260,40 @@ int main(int argc, char **argv)
// Update the current x feature
vpFeatureBuilder::create(s_x, cam, dot);
- // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
- surface = 1./sqrt(dot.m00/(cam.get_px()*cam.get_py()));
- Z = coef * surface ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
+ // Update log(Z/Z*) feature. Since the depth Z change, we need to update
+ // the intection matrix
+ surface = 1. / sqrt(dot.m00 / (cam.get_px() * cam.get_py()));
+ Z = coef * surface;
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
L_Z = s_Z.interaction();
// Update the global interaction matrix
vpMatrix L;
L.stack(L_x); // constant since build with the desired feature
- L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that evolves at each iteration
+ L.stack(L_Z); // not constant since it corresponds to log(Z/Z*) that
+ // evolves at each iteration
// Update the global error s-s*
vpColVector error;
- error.stack( s_x.error( s_xd, vpFeaturePoint::selectX() ) );
- error.stack( s_Z.error( s_Zd ) );
+ error.stack(s_x.error(s_xd, vpFeaturePoint::selectX()));
+ error.stack(s_Z.error(s_Zd));
- // Compute the control law. Velocities are computed in the mobile robot reference frame
+ // Compute the control law. Velocities are computed in the mobile robot
+ // 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);
- // Draw a vertical line which corresponds to the desired x coordinate of the dot cog
+ // Draw a vertical line which corresponds to the desired x coordinate of
+ // the dot cog
vpDisplay::displayLine(I, 0, 320, 479, 320, vpColor::red);
vpDisplay::flush(I);
// A click in the viewer to exit
- if ( vpDisplay::getClick(I, false) )
+ if (vpDisplay::getClick(I, false))
break;
}
@@ -286,15 +302,11 @@ int main(int argc, char **argv)
// wait for the thread to stop
robot.waitForRunExit();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#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 bdd6008..c90649c 100644
--- a/example/servo-pioneer/sonarPioneerReader.cpp
+++ b/example/servo-pioneer/sonarPioneerReader.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,20 +38,21 @@
#include <iostream>
-#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpTime.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/robot/vpRobotPioneer.h> // Include before vpDisplayX to avoid build issues
#ifndef VISP_HAVE_PIONEER
int main()
{
- std::cout << "\nThis example requires Aria 3rd party library. You should install it.\n"
+ std::cout << "\nThis example requires Aria 3rd party library. You should "
+ "install it.\n"
<< std::endl;
return 0;
}
@@ -61,16 +63,17 @@ ArSonarDevice sonar;
vpRobotPioneer *robot;
#if defined(VISP_HAVE_X11)
vpDisplayX *d;
-#elif defined (VISP_HAVE_GDI)
+#elif defined(VISP_HAVE_GDI)
vpDisplayGDI *d;
#endif
vpImage<unsigned char> I;
static bool isInitialized = false;
-static int half_size = 256*2;
+static int half_size = 256 * 2;
void sonarPrinter(void)
{
- fprintf(stdout, "in sonarPrinter()\n"); fflush(stdout);
+ fprintf(stdout, "in sonarPrinter()\n");
+ fflush(stdout);
double scale = (double)half_size / (double)sonar.getMaxRange();
/*
@@ -108,24 +111,24 @@ void sonarPrinter(void)
double end_angle = 45;
range = sonar.currentReadingPolar(start_angle, end_angle, &angle);
printf(" front quadrant: %5.0f ", range);
- //if (range != sonar.getMaxRange())
+ // if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
- //if (isInitialized && range != sonar.getMaxRange())
- if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
- {
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
+ // if (isInitialized && range != sonar.getMaxRange())
+ if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
double x = range * cos(vpMath::rad(angle)); // position of the obstacle in the sensor frame
double y = range * sin(vpMath::rad(angle));
- // Conversion in pixels so that the robot frame is in the middle of the image
+ // Conversion in pixels so that the robot frame is in the middle of the
+ // image
double j = -y * scale + half_size; // obstacle position
double i = -x * scale + half_size;
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);
}
@@ -133,21 +136,21 @@ void sonarPrinter(void)
range = sonar.currentReadingPolar(-135, -45, &angle);
printf(" right quadrant: %5.0f ", range);
- //if (range != sonar.getMaxRange())
+ // if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(45, 135, &angle);
printf(" left quadrant: %5.0f ", range);
- //if (range != sonar.getMaxRange())
+ // if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
range = sonar.currentReadingPolar(-135, 135, &angle);
printf(" back quadrant: %5.0f ", range);
- //if (range != sonar.getMaxRange())
+ // if (range != sonar.getMaxRange())
if (std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
printf("%3.0f ", angle);
printf("\n");
@@ -156,11 +159,9 @@ void sonarPrinter(void)
* example to show how get all sonar sensor data
*/
ArSensorReading *reading;
- for (int sensor = 0; sensor < robot->getNumSonar(); sensor++)
- {
+ for (int sensor = 0; sensor < robot->getNumSonar(); sensor++) {
reading = robot->getSonarReading(sensor);
- if (reading != NULL)
- {
+ if (reading != NULL) {
angle = reading->getSensorTh();
range = reading->getRange();
double sx = reading->getSensorX(); // position of the sensor in the robot frame
@@ -170,31 +171,32 @@ void sonarPrinter(void)
double x = sx + ox; // position of the obstacle in the robot frame
double y = sy + oy;
- // Conversion in pixels so that the robot frame is in the middle of the image
+ // Conversion in pixels so that the robot frame is in the middle of the
+ // image
double sj = -sy * scale + half_size; // sensor position
double si = -sx * scale + half_size;
double j = -y * scale + half_size; // obstacle position
double i = -x * scale + half_size;
- // printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor, reading->getSensorX(),
- // reading->getSensorY(), reading->getSensorTh(), reading->getRange());
+// printf("%d x: %.1f y: %.1f th: %.1f d: %d\n", sensor,
+// reading->getSensorX(),
+// reading->getSensorY(), reading->getSensorTh(),
+// reading->getRange());
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
- //if (isInitialized && range != sonar.getMaxRange())
- if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon())
- {
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
+ // if (isInitialized && range != sonar.getMaxRange())
+ if (isInitialized && std::fabs(range - sonar.getMaxRange()) > std::numeric_limits<double>::epsilon()) {
vpDisplay::displayLine(I, si, sj, i, j, vpColor::blue, 2);
vpDisplay::displayCross(I, si, sj, 7, vpColor::blue);
char legend[15];
- sprintf(legend, "%d: %1.2fm", sensor, float(range)/1000);
- vpDisplay::displayCharString(I, i-7, j+7, legend, vpColor::blue);
+ sprintf(legend, "%d: %1.2fm", sensor, float(range) / 1000);
+ vpDisplay::displayCharString(I, i - 7, j + 7, legend, vpColor::blue);
}
#endif
}
-
}
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
if (isInitialized)
vpDisplay::flush(I);
#endif
@@ -215,20 +217,17 @@ int main(int argc, char **argv)
robot = new vpRobotPioneer;
- // ArRobotConnector connects to the robot, get some initial data from it such as type and name,
- // and then loads parameter files for this robot.
+ // ArRobotConnector connects to the robot, get some initial data from it
+ // such as type and name, and then loads parameter files for this robot.
ArRobotConnector robotConnector(&parser, robot);
- if(!robotConnector.connectRobot())
- {
+ if (!robotConnector.connectRobot()) {
ArLog::log(ArLog::Terse, "Could not connect to the robot");
- if(parser.checkHelpAndWarnUnparsed())
- {
+ if (parser.checkHelpAndWarnUnparsed()) {
Aria::logOptions();
Aria::exit(1);
}
}
- if (!Aria::parseArgs())
- {
+ if (!Aria::parseArgs()) {
Aria::logOptions();
Aria::shutdown();
return false;
@@ -236,16 +235,15 @@ int main(int argc, char **argv)
std::cout << "Robot connected" << std::endl;
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
// Create a display to show sensor data
- if (isInitialized == false)
- {
- I.resize((unsigned int)half_size*2, (unsigned int)half_size*2);
+ if (isInitialized == false) {
+ I.resize((unsigned int)half_size * 2, (unsigned int)half_size * 2);
I = 255;
#if defined(VISP_HAVE_X11)
d = new vpDisplayX;
-#elif defined (VISP_HAVE_GDI)
+#elif defined(VISP_HAVE_GDI)
d = new vpDisplayGDI;
#endif
d->init(I, -1, -1, "Sonar range data");
@@ -263,8 +261,7 @@ int main(int argc, char **argv)
// Robot velocities
vpColVector v_mes(2);
- for (int i=0; i < 1000; i++)
- {
+ for (int i = 0; i < 1000; i++) {
double t = vpTime::measureTimeMs();
v_mes = robot->getVelocity(vpRobot::REFERENCE_FRAME);
@@ -273,7 +270,7 @@ int main(int argc, char **argv)
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)
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
if (isInitialized) {
// A mouse click to exit
// Before exiting save the last sonar image
@@ -296,10 +293,8 @@ int main(int argc, char **argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
exit(-1);
}
@@ -324,14 +319,17 @@ int main(int argc, char **argv)
ArUtil::sleep(1000);
robot->lock();
- 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());
+ 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->unlock();
std::cout << "Ending robot thread..." << std::endl;
robot->stopRunning();
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI)
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
if (isInitialized) {
if (d != NULL)
delete d;
@@ -346,12 +344,10 @@ int main(int argc, char **argv)
// exit
ArLog::log(ArLog::Normal, "simpleMotionCommands: Exiting.");
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#endif
-
diff --git a/example/servo-ptu46/CMakeLists.txt b/example/servo-ptu46/CMakeLists.txt
index d5dcdc5..e3b603a 100644
--- a/example/servo-ptu46/CMakeLists.txt
+++ b/example/servo-ptu46/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/servo-ptu46/movePtu46.cpp b/example/servo-ptu46/movePtu46.cpp
index 00d6de3..1f5c811 100644
--- a/example/servo-ptu46/movePtu46.cpp
+++ b/example/servo-ptu46/movePtu46.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,12 +38,11 @@
/*!
\file movePtu46.cpp
- \brief Example of a real robot control, the ptu-46 robot (pan-tilt turret). The
- robot is controlled first in position, then in velocity.
+ \brief Example of a real robot control, the ptu-46 robot (pan-tilt turret).
+ The robot is controlled first in position, then in velocity.
*/
-
/*!
\example movePtu46.cpp
@@ -53,42 +53,38 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
+#include <unistd.h>
#endif
-
-
#ifdef VISP_HAVE_PTU46
#include <visp3/robot/vpRobotPtu46.h>
-int
-main()
+int main()
{
- try
- {
- vpRobotPtu46 robot ;
- vpColVector q(2) ;
+ try {
+ vpRobotPtu46 robot;
+ vpColVector q(2);
- vpERROR_TRACE(" ") ;
+ vpERROR_TRACE(" ");
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
q = 0;
vpCTRACE << "Set position in the articular frame: " << q.t();
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
q[0] = vpMath::rad(10);
q[1] = vpMath::rad(20);
vpCTRACE << "Set position in the articular frame: " << q.t();
- robot.setPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
- vpColVector qm(2) ;
- robot.getPosition(vpRobot::ARTICULAR_FRAME, qm) ;
- vpCTRACE << "Position in the articular frame " << qm.t() ;
+ vpColVector qm(2);
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, qm);
+ vpCTRACE << "Position in the articular frame " << qm.t();
- vpColVector qdot(2) ;
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ vpColVector qdot(2);
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
#if 0
qdot = 0 ;
qdot[0] = vpMath::rad(10) ;
@@ -107,35 +103,31 @@ main()
sleep(2) ;
#endif
- qdot = 0 ;
+ qdot = 0;
// qdot[0] = vpMath::rad(0.1) ;
- qdot[1] = vpMath::rad(10) ;
- vpCTRACE << "Set articular frame velocity " << qdot.t() ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
- sleep(2) ;
+ qdot[1] = vpMath::rad(10);
+ vpCTRACE << "Set articular frame velocity " << qdot.t();
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
+ sleep(2);
- qdot = 0 ;
+ qdot = 0;
qdot[0] = vpMath::rad(-5);
- //qdot[1] = vpMath::rad(-5);
+ // qdot[1] = vpMath::rad(-5);
- vpCTRACE << "Set articular frame velocity " << qdot.t() ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot) ;
- sleep(2) ;
- }
- catch (...)
- {
+ vpCTRACE << "Set articular frame velocity " << qdot.t();
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, qdot);
+ sleep(2);
+ } catch (...) {
std::cout << "Sorry PtU46 not available ..." << std::endl;
}
return 0;
-
}
#else
-int
-main()
+int main()
{
vpERROR_TRACE("You do not have a ptu-46 robot connected to your computer...");
- return 0;
+ return 0;
}
#endif
diff --git a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
index 9b9a77b..840e313 100644
--- a/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
+++ b/example/servo-ptu46/servoPtu46Point2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,14 +41,13 @@
/*!
\file servoPtu46Point2DArtVelocity.cpp
- \brief Example of eye-in-hand control law. We control here a real robot, the ptu-46
- robot (pan-tilt head provided by Directed Perception). The velocity is
- computed in articular. The visual feature is the center of gravity of a
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ ptu-46 robot (pan-tilt head provided by Directed Perception). The velocity
+ is computed in articular. The visual feature is the center of gravity of a
point.
*/
-
/*!
\example servoPtu46Point2DArtVelocity.cpp
@@ -60,29 +60,27 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
+#include <unistd.h>
#endif
#include <signal.h>
-
-
-#if (defined(VISP_HAVE_PTU46) & defined (VISP_HAVE_DC1394) )
+#if (defined(VISP_HAVE_PTU46) & defined(VISP_HAVE_DC1394))
#ifdef VISP_HAVE_PTHREAD
-# include <pthread.h>
+#include <pthread.h>
#endif
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/robot/vpRobotPtu46.h>
@@ -92,195 +90,181 @@
#include <visp3/blob/vpDot2.h>
-
#ifdef VISP_HAVE_PTHREAD
pthread_mutex_t mutexEndLoop = PTHREAD_MUTEX_INITIALIZER;
#endif
-void signalCtrC( int signumber )
+void signalCtrC(int signumber)
{
(void)(signumber);
#ifdef VISP_HAVE_PTHREAD
- pthread_mutex_unlock( &mutexEndLoop );
+ pthread_mutex_unlock(&mutexEndLoop);
#endif
- usleep( 1000*10 );
+ usleep(1000 * 10);
vpTRACE("Ctrl-C pressed...");
}
-int
-main()
+int main()
{
- 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 << " Simulation " << std::endl ;
- std::cout << " task : servo a point " << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- 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 << " Simulation " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- try{
+ try {
#ifdef VISP_HAVE_PTHREAD
- pthread_mutex_lock( &mutexEndLoop );
+ pthread_mutex_lock(&mutexEndLoop);
#endif
- signal( SIGINT,&signalCtrC );
+ signal(SIGINT, &signalCtrC);
- vpRobotPtu46 robot ;
+ vpRobotPtu46 robot;
{
- vpColVector q(2); q=0;
- robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- robot.setPosition( vpRobot::ARTICULAR_FRAME,q );
+ vpColVector q(2);
+ q = 0;
+ robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
}
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vp1394TwoGrabber g;
- g.open(I) ;
+ g.open(I);
- try{
- g.acquire(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught") ;
- return(-1) ;
+ try {
+ g.acquire(I);
+ } catch (...) {
+ vpERROR_TRACE(" Error caught");
+ return (-1);
}
+ vpDisplayX display(I, 100, 100, "testDisplayX.cpp ");
+ vpTRACE(" ");
- vpDisplayX display(I,100,100,"testDisplayX.cpp ") ;
- vpTRACE(" ") ;
-
- try{
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught") ;
- return(-1) ;
+ try {
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE(" Error caught");
+ return (-1);
}
- vpServo task ;
+ vpServo task;
- vpDot2 dot ;
+ vpDot2 dot;
- try{
- vpERROR_TRACE("start dot.initTracking(I) ") ;
+ try {
+ vpERROR_TRACE("start dot.initTracking(I) ");
vpImagePoint germ;
- vpDisplay::getClick( I, germ );
+ vpDisplay::getClick(I, germ);
dot.setCog(germ);
- vpDEBUG_TRACE(25,"Click!");
- //dot.initTracking(I) ;
+ vpDEBUG_TRACE(25, "Click!");
+ // dot.initTracking(I) ;
dot.track(I);
- vpERROR_TRACE("after dot.initTracking(I) ") ;
- }
- catch(...)
- {
- vpERROR_TRACE(" Error caught ") ;
- return(-1) ;
+ vpERROR_TRACE("after dot.initTracking(I) ");
+ } catch (...) {
+ vpERROR_TRACE(" Error caught ");
+ return (-1);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
- 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
+ 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
- p.set_Z(1) ;
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ p.set_Z(1);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t articular velocity are computed") ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t articular velocity are computed");
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
-
- vpTRACE("Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
+ vpTRACE("Set the position of the camera in the end-effector frame ");
+ vpHomogeneousMatrix cMe;
// robot.get_cMe(cMe) ;
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
-
- vpDisplay::getClick(I) ;
- vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
-
-
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.1) ;
+ vpDisplay::getClick(I);
+ vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.1);
+ vpTRACE("Display task information ");
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- unsigned int iter=0 ;
- vpTRACE("\t loop") ;
+ unsigned int iter = 0;
+ vpTRACE("\t loop");
#ifdef VISP_HAVE_PTHREAD
- while( 0 != pthread_mutex_trylock( &mutexEndLoop ) )
+ while (0 != pthread_mutex_trylock(&mutexEndLoop))
#else
- for ( ; ; )
+ for (;;)
#endif
{
- std::cout << "---------------------------------------------" << iter <<std::endl ;
+ std::cout << "---------------------------------------------" << iter << std::endl;
- g.acquire(I) ;
- vpDisplay::display(I) ;
+ g.acquire(I);
+ vpDisplay::display(I);
- dot.track(I) ;
+ dot.track(I);
// vpDisplay::displayCross(I,(int)dot.I(), (int)dot.J(),
// 10,vpColor::green) ;
-
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// get the jacobian
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// std::cout << (vpMatrix)cVe*eJe << std::endl ;
- vpColVector v ;
- v = task.computeControlLaw() ;
+ vpColVector v;
+ v = task.computeControlLaw();
- vpServoDisplay::display(task,cam,I) ;
- std::cout << v.t() ;
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
- vpDisplay::flush(I) ;
+ vpServoDisplay::display(task, cam, I);
+ std::cout << v.t();
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
+ vpDisplay::flush(I);
- vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ vpTRACE("\t\t || s - s* || = %f ", (task.getError()).sumSquare());
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ vpTRACE("Display task information ");
+ task.print();
task.kill();
- } catch (...) { vpERROR_TRACE("Trow uncatched..."); }
+ } catch (...) {
+ vpERROR_TRACE("Trow uncatched...");
+ }
}
-
#else
-int
-main()
+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/CMakeLists.txt b/example/servo-viper650/CMakeLists.txt
index 014ceb3..ce6a7ec 100644
--- a/example/servo-viper650/CMakeLists.txt
+++ b/example/servo-viper650/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -75,6 +76,9 @@ set(example_cpp
if(NOT DEFINED WINRT_8_1)
list(APPEND example_cpp servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp)
list(APPEND example_cpp servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp)
+ list(APPEND example_cpp servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp)
+
+ list(APPEND example_data "${CMAKE_CURRENT_SOURCE_DIR}/SR300-eMc.cnf")
endif()
foreach(cpp ${example_cpp})
@@ -83,3 +87,10 @@ foreach(cpp ${example_cpp})
visp_add_dependency(${cpp} "examples")
endif()
endforeach()
+
+if(NOT DEFINED WINRT_8_1)
+ # Copy the data files to the same location than the target
+ foreach(data ${example_data})
+ visp_copy_data(servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp ${data})
+ endforeach()
+endif()
diff --git a/example/servo-viper650/SR300-eMc.cnf b/example/servo-viper650/SR300-eMc.cnf
new file mode 100644
index 0000000..2aa8808
--- /dev/null
+++ b/example/servo-viper650/SR300-eMc.cnf
@@ -0,0 +1,11 @@
+# Start with any number of consecutive lines
+# beginning with the symbol '#'
+#
+# The 3 following lines contain the name of the camera,
+# the rotation parameters of the geometric transformation
+# using the Euler angles in degrees with convention XYZ and
+# the translation parameters expressed in meters
+CAMERA SR300
+eMc_ROT_XYZ -1.094521437 -0.8733872716 -87.95535141
+eMc_TRANS_XYZ -0.03321048274 0.00218700917 0.02981391035
+
diff --git a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
index 60d634e..b76f610 100644
--- a/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
+++ b/example/servo-viper650/servoViper650FourPoints2DArtVelocityInteractionCurrent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,37 +43,36 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Viper S650 robot (arm with 6 degrees of freedom). The velocities resulting
from visual servo are here joint velocities. Visual features are the image
- coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by 10cm
- square.
+ coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by
+ 10cm square.
-*/
+ The device used to acquire images is a firewire camera (PointGrey Flea2)
+ Camera extrinsic (eMc) and intrinsic parameters are retrieved from the robot
+ low level driver that is not public.
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h>
+*/
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11)
+
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
@@ -81,78 +81,57 @@
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
- their corresponding 2D coordinates \e dot. The pose is computed using a Lowe
- non linear method.
+ their corresponding 2D coordinates \e dot.
\param point : 3D coordinates of the points.
\param dot : 2D coordinates of the points.
- \param ndot : Number of points or dots used for the pose estimation.
-
\param cam : Intrinsic camera parameters.
\param cMo : Homogeneous matrix in output describing the transformation
between the camera and object frame.
- \param cto : Translation in ouput extracted from \e cMo.
-
- \param cro : Rotation in ouput extracted from \e cMo.
-
\param init : Indicates if the we have to estimate an initial pose with
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+void compute_pose(std::vector<vpPoint> &point, std::vector<vpDot2> &dot, vpCameraParameters cam,
+ vpHomogeneousMatrix &cMo, bool init)
{
- vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
- vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
- vpRotationMatrix cRo;
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method
+ vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method
vpPose pose;
- vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
-
- double x=0, y=0;
- 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_y(y) ;
- pose.addPoint(point[i]) ;
+
+ for (size_t i = 0; i < point.size(); i++) {
+
+ double x = 0, y = 0;
+ 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_y(y);
+ pose.addPoint(point[i]);
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
- // 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
- if (residual_lagrange < residual_dementhon)
+ if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
-
}
- else { // init = false; use of the previous pose to initialise LOWE
- cRo.buildFrom(cro);
- cMo.buildFrom(cto, cRo);
- }
- pose.computePose(vpPose::LOWE, cMo) ;
- cMo.extract(cto);
- cMo.extract(cRo);
- cro.buildFrom(cRo);
+
+ pose.computePose(vpPose::LOWE, cMo);
}
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -166,19 +145,17 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -188,194 +165,170 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper650 robot ;
+ 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);
+ std::cout << "Camera extrinsic parameters (eMc): \n" << eMc << std::endl;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
-
-#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
-#endif
+ g.acquire(I);
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplayX display(I, 100, 100, "Current image");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " 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 << std::endl ;
+ std::vector<vpDot2> dot(4);
- 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) ;
- dot[i].initTracking(I) ;
- cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ for (size_t i = 0; i < dot.size(); i++) {
+ dot[i].setGraphics(true);
+ dot[i].initTracking(I);
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
-
- std::cout << "Camera parameters: \n" << cam << std::endl;
+ robot.getCameraParameters(cam, I);
+ std::cout << "Camera intrinsic parameters: \n" << cam << std::endl;
// 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
+ 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
// Set the position of the square target in a frame which origin is
// centered in the middle of the square
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ std::vector<vpPoint> point(4);
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
- // Initialise a desired pose to compute s*, the desired 2D point features
+ // Compute target initial pose
vpHomogeneousMatrix cMo;
- vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
- vpRxyzVector cro(vpMath::rad(0), vpMath::rad(10), vpMath::rad(20));
- vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ compute_pose(point, dot, cam, cMo, true);
+ 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
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo_d, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (size_t i = 0; i < dot.size(); i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.3) ;
-
- // Display task information
- task.print() ;
+ task.setLambda(0.3);
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- task.set_cVe(cVe) ;
- task.print() ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ task.set_cVe(cVe);
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
- task.print() ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ 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) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (size_t i = 0; i < dot.size(); i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
- cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- return(1) ;
+ } catch (...) {
+ std::cout << "Error detected while tracking visual features.." << std::endl;
+ break;
}
// During the servo, we compute the pose using LOWE method. For the
// initial pose used in the non linear minimisation we use the pose
// computed at the previous iteration.
- compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
+ compute_pose(point, dot, cam, cMo, false);
- for (i=0 ; i < 4 ; i++) {
+ for (size_t i = 0; i < dot.size(); i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
- vpColVector v ;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ vpColVector v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -385,8 +338,7 @@ 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;
@@ -396,40 +348,42 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
+
+ vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
+ if (vpDisplay::getClick(I, false))
+ break;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ // std::cout << "\t\t || s - s* || = " << ( task.getError()
+ // ).sumSquare() << std::endl;
}
- vpTRACE("Display task information " ) ;
- task.print() ;
+ std::cout << "Display task information: " << std::endl;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (const vpException &e) {
+ flog.close(); // Close the log file
+ std::cout << "Catched an exception: " << e.getMessage() << std::endl;
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
-
+ std::cout << "You do not have an Viper650 robot or a firewire framegrabber "
+ "connected to your computer..."
+ << std::endl;
}
#endif
diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp
new file mode 100644
index 0000000..6deedb0
--- /dev/null
+++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp
@@ -0,0 +1,377 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * tests the control law
+ * eye-in-hand control
+ * velocity computed in the camera frame
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+/*!
+ \example servoViper650FourPoints2DCamVelocityInteractionCurrent-SR300.cpp
+
+ \brief Example of eye-in-hand control law. We control here a real robot, the
+ Viper S650 robot (arm with 6 degrees of freedom). The velocity is
+ computed in camera frame. The inverse jacobian that converts cartesian
+ velocities in joint velocities is implemented in the robot low level
+ controller. Visual features are the image coordinates of 4 points. The
+ target is made of 4 dots arranged as a 10cm by 10cm square.
+
+ The device used to acquire images is a Realsense SR300 device.
+
+ Camera extrinsic (eMc) parameters are read from SR300-eMc.cnf file located
+ besides this code.
+
+ Camera intrinsic parameters are retrieved from the Realsense SDK.
+
+*/
+
+#include <fstream>
+#include <iostream>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_X11)
+
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/sensor/vpRealSense.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
+
+#define L 0.05 // to deal with a 10cm by 10cm square
+
+/*!
+
+ Compute the pose \e cMo from the 3D coordinates of the points \e point and
+ their corresponding 2D coordinates \e dot.
+
+ \param point : 3D coordinates of the points.
+
+ \param dot : 2D coordinates of the points.
+
+ \param cam : Intrinsic camera parameters.
+
+ \param cMo : Homogeneous matrix in output describing the transformation
+ between the camera and object frame.
+
+ \param init : Indicates if the we have to estimate an initial pose with
+ Lagrange or Dementhon methods.
+
+*/
+void compute_pose(std::vector<vpPoint> &point, std::vector<vpDot2> &dot, vpCameraParameters cam,
+ vpHomogeneousMatrix &cMo, bool init)
+{
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method
+ vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method
+ vpPose pose;
+
+ for (size_t i = 0; i < point.size(); i++) {
+
+ double x = 0, y = 0;
+ 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_y(y);
+ pose.addPoint(point[i]);
+ }
+
+ if (init == true) {
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
+ // Compute and return the residual expressed in meter for the pose matrix
+ double residual_dementhon = pose.computeResidual(cMo_dementhon);
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
+ double residual_lagrange = pose.computeResidual(cMo_lagrange);
+
+ // Select the best pose to initialize the lowe pose computation
+ if (residual_lagrange < residual_dementhon)
+ cMo = cMo_lagrange;
+ else
+ cMo = cMo_dementhon;
+ }
+
+ pose.computePose(vpPose::LOWE, cMo);
+}
+
+int main()
+{
+ // Log file creation in /tmp/$USERNAME/log.dat
+ // This file contains by line:
+ // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
+ // - the 6 mesured joint velocities (m/s, rad/s)
+ // - the 6 mesured joint positions (m, rad)
+ // - the 8 values of s - s*
+ std::string username;
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Create a log filename to save velocities...
+ std::string logdirname;
+ logdirname = "/tmp/" + username;
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(logdirname) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(logdirname);
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << logdirname << std::endl;
+ return (-1);
+ }
+ }
+ std::string logfilename;
+ logfilename = logdirname + "/log.dat";
+
+ // Open the log file name
+ std::ofstream flog(logfilename.c_str());
+
+ try {
+ vpRobotViper650 robot;
+
+ // Load the end-effector to camera frame transformation from SR300-eMc.cnf
+ // file
+ robot.init(vpRobotViper650::TOOL_CUSTOM, "./SR300-eMc.cnf");
+ vpHomogeneousMatrix eMc;
+ robot.get_eMc(eMc);
+ std::cout << "Camera extrinsic parameters (eMc): \n" << eMc << std::endl;
+
+ vpServo task;
+
+ vpImage<unsigned char> I;
+
+ vpRealSense g;
+ // Enable the RealSense device to acquire only color images with size
+ // 640x480
+ g.setEnableStream(rs::stream::color, true);
+ 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.open();
+
+ // Update camera parameters
+ vpCameraParameters cam =
+ g.getCameraParameters(rs::stream::color, vpCameraParameters::perspectiveProjWithDistortion);
+ std::cout << "Camera intrinsic parameters: \n" << cam << std::endl;
+
+ g.acquire(I);
+
+ vpDisplayX display(I, 100, 100, "Current image");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ std::vector<vpDot2> dot(4);
+
+ 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);
+ dot[i].initTracking(I);
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
+ vpDisplay::flush(I);
+ }
+
+ // 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
+
+ // Set the position of the square target in a frame which origin is
+ // centered in the middle of the square
+ std::vector<vpPoint> point(4);
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
+
+ // Compute target initial pose
+ vpHomogeneousMatrix cMo;
+ compute_pose(point, dot, cam, cMo, true);
+ 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
+
+ // Sets the desired position of the 2D visual feature
+ vpFeaturePoint pd[4];
+ // Compute the desired position of the features from the desired pose
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo_d, cP);
+ point[i].projection(cP, p);
+
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
+ pd[i].set_Z(cP[2]);
+ }
+
+ // We want to see a point on a point
+ for (size_t i = 0; i < dot.size(); i++)
+ task.addFeature(p[i], pd[i]);
+
+ // Set the proportional gain
+ task.setLambda(0.3);
+
+ // Define the task
+ // - we want an eye-in-hand control law
+ // - camera velocities are computed
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
+ task.print();
+
+ // 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;
+ for (;;) {
+ // Acquire a new image from the camera
+ g.acquire(I);
+
+ // Display this image
+ vpDisplay::display(I);
+
+ try {
+ // For each point...
+ for (size_t i = 0; i < dot.size(); i++) {
+ // Achieve the tracking of the dot in the image
+ dot[i].track(I);
+ // Display a green cross at the center of gravity position in the
+ // image
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
+ }
+ } catch (...) {
+ std::cout << "Error detected while tracking visual features.." << std::endl;
+ break;
+ }
+
+ // During the servo, we compute the pose using a non linear method. For
+ // the initial pose used in the non linear minimisation we use the pose
+ // computed at the previous iteration.
+ compute_pose(point, dot, cam, cMo, false);
+
+ for (size_t i = 0; i < dot.size(); i++) {
+ // Update the point feature from the dot location
+ vpFeatureBuilder::create(p[i], cam, dot[i]);
+ // Set the feature Z coordinate from the pose
+ vpColVector cP;
+ point[i].changeFrame(cMo, cP);
+
+ p[i].set_Z(cP[2]);
+ }
+
+ // Compute the visual servoing skew vector
+ vpColVector v = task.computeControlLaw();
+
+ // Display the current and desired feature points in the image display
+ vpServoDisplay::display(task, cam, I);
+
+ // Apply the computed joint velocities to the robot
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+
+ // 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] << " ";
+
+ // Get the measured joint velocities of the robot
+ vpColVector qvel;
+ robot.getVelocity(vpRobot::ARTICULAR_FRAME, qvel);
+ // Save measured joint velocities of the robot in the log file:
+ // - qvel[0], qvel[1], qvel[2] correspond to measured joint translation
+ // 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] << " ";
+
+ // Get the measured joint positions of the robot
+ vpColVector q;
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ // Save measured joint positions of the robot in the log file
+ // - q[0], q[1], q[2] correspond to measured joint translation
+ // 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] << " ";
+
+ // 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() << std::endl;
+
+ vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
+ if (vpDisplay::getClick(I, false))
+ break;
+
+ // Flush the display
+ vpDisplay::flush(I);
+
+ // std::cout << "\t\t || s - s* || = " << ( task.getError()
+ // ).sumSquare() << std::endl;
+ }
+
+ std::cout << "Display task information: " << std::endl;
+ task.print();
+ task.kill();
+ flog.close(); // Close the log file
+ return 0;
+ } catch (const vpException &e) {
+ flog.close(); // Close the log file
+ std::cout << "Catch an exception: " << e.getMessage() << std::endl;
+ return 0;
+ }
+}
+
+#else
+int main()
+{
+ std::cout << "You do not have an Viper650 robot or a RealSense device "
+ "connected to your computer..."
+ << std::endl;
+}
+
+#endif
diff --git a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
index 426bb59..d9f566b 100644
--- a/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-viper650/servoViper650FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,122 +42,101 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Viper S650 robot (arm with 6 degrees of freedom). The velocity is
- computed in camera frame. The inverse jacobian that converts cartesian velocities
- in joint velocities is implemented in the robot low level controller.
- Visual features are the image coordinates of 4 points. The target is made of 4 dots
- arranged as a 10cm by 10cm square.
+ computed in the camera frame. The inverse jacobian that converts cartesian
+ velocities in joint velocities is implemented in the robot low level
+ controller. Visual features are the image coordinates of 4 points. The
+ target is made of 4 dots arranged as a 10cm by 10cm square.
-*/
+ The device used to acquire images is a firewire camera (PointGrey Flea2)
+ Camera extrinsic (eMc) and intrinsic parameters are retrieved from the robot
+ low level driver that is not public.
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h>
+*/
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11)
+
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
- their corresponding 2D coordinates \e dot. The pose is computed using a Lowe
- non linear method.
+ their corresponding 2D coordinates \e dot.
\param point : 3D coordinates of the points.
\param dot : 2D coordinates of the points.
- \param ndot : Number of points or dots used for the pose estimation.
-
\param cam : Intrinsic camera parameters.
\param cMo : Homogeneous matrix in output describing the transformation
between the camera and object frame.
- \param cto : Translation in ouput extracted from \e cMo.
-
- \param cro : Rotation in ouput extracted from \e cMo.
-
\param init : Indicates if the we have to estimate an initial pose with
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+void compute_pose(std::vector<vpPoint> &point, std::vector<vpDot2> &dot, vpCameraParameters cam,
+ vpHomogeneousMatrix &cMo, bool init)
{
- vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
- vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
- vpRotationMatrix cRo;
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon method
+ vpHomogeneousMatrix cMo_lagrange; // computed pose with lagrange method
vpPose pose;
- vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
-
- double x=0, y=0;
- 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_y(y) ;
- pose.addPoint(point[i]) ;
+
+ for (size_t i = 0; i < point.size(); i++) {
+
+ double x = 0, y = 0;
+ 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_y(y);
+ pose.addPoint(point[i]);
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
- // 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
- if (residual_lagrange < residual_dementhon)
+ if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
-
- }
- else { // init = false; use of the previous pose to initialise LOWE
- cRo.buildFrom(cro);
- cMo.buildFrom(cto, cRo);
}
- pose.computePose(vpPose::LOWE, cMo) ;
- cMo.extract(cto);
- cMo.extract(cRo);
- cro.buildFrom(cRo);
+
+ pose.computePose(vpPose::LOWE, cMo);
}
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
- // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
+ // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 mesured joint velocities (m/s, rad/s)
// - the 6 mesured joint positions (m, rad)
// - the 8 values of s - s*
@@ -166,19 +146,17 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -188,177 +166,152 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper650 robot ;
+ 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);
+ std::cout << "Camera extrinsic parameters (eMc): \n" << eMc << std::endl;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
-#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
-#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
-#endif
+ vpDisplayX display(I, 100, 100, "Current image");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ std::vector<vpDot2> dot(4);
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- 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 << " Use of the Viper650 robot " << 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;
-
- for (i=0 ; i < 4 ; i++) {
- dot[i].setGraphics(true) ;
- dot[i].initTracking(I) ;
- cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ 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);
+ dot[i].initTracking(I);
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
-
- std::cout << "Camera parameters: \n" << cam << std::endl;
+ robot.getCameraParameters(cam, I);
+ std::cout << "Camera intrinsic parameters: \n" << cam << std::endl;
// 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
+ 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
// Set the position of the square target in a frame which origin is
// centered in the middle of the square
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ std::vector<vpPoint> point(4);
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
- // Initialise a desired pose to compute s*, the desired 2D point features
+ // Compute target initial pose
vpHomogeneousMatrix cMo;
- vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
- vpRxyzVector cro(vpMath::rad(0), vpMath::rad(10), vpMath::rad(10));
- vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ compute_pose(point, dot, cam, cMo, true);
+ 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
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo_d, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (size_t i = 0; i < dot.size(); i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.3) ;
-
- // Display task information
- task.print() ;
+ task.setLambda(0.3);
// Define the task
// - we want an eye-in-hand control law
- // - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
-
+ // - camera velocities are computed
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ 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) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (size_t i = 0; i < dot.size(); i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
- cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpImagePoint cog = dot[i].getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- return(1) ;
+ } catch (...) {
+ std::cout << "Error detected while tracking visual features.." << std::endl;
+ break;
}
- // During the servo, we compute the pose using LOWE method. For the
- // initial pose used in the non linear minimisation we use the pose
+ // During the servo, we compute the pose using a non linear method. For
+ // the initial pose used in the non linear minimisation we use the pose
// computed at the previous iteration.
- compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
+ compute_pose(point, dot, cam, cMo, false);
- for (i=0 ; i < 4 ; i++) {
+ for (size_t i = 0; i < dot.size(); i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
- vpColVector v ;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ vpColVector v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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] << " ";
+ // 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] << " ";
// Get the measured joint velocities of the robot
vpColVector qvel;
@@ -368,8 +321,7 @@ 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;
@@ -379,40 +331,44 @@ 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 expressed
- // in meters in the camera frame
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
flog << task.getError() << std::endl;
+ vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
+ if (vpDisplay::getClick(I, false))
+ break;
+
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // vpTRACE("\t\t || s - s* || = %f ", ( task.getError() ).sumSquare()) ;
+ // std::cout << "\t\t || s - s* || = " << ( task.getError()
+ // ).sumSquare() << std::endl;
}
+ robot.stopMotion();
+
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (const vpException &e) {
+ flog.close(); // Close the log file
+ std::cout << "Catched an exception: " << e.getMessage() << std::endl;
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
-
+ std::cout << "You do not have an Viper650 robot or a firewire framegrabber "
+ "connected to your computer..."
+ << std::endl;
}
#endif
diff --git a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
index 7815c57..7fb569d 100644
--- a/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
+++ b/example/servo-viper650/servoViper650Point2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,51 +39,49 @@
*
*****************************************************************************/
-
/*!
\example servoViper650Point2DCamVelocity.cpp
Example of eye-in-hand control law. We control here a real robot, the
ADEPT Viper 650 robot (arm, with 6 degrees of freedom). The velocity is
- computed in the camera frame. The visual feature is the center of gravity of a
- point.
+ computed in the camera frame. The visual feature is the center of gravity of
+ a point.
-*/
+ The device used to acquire images is a firewire camera (PointGrey Flea2)
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h> // Debug trace
+ Camera extrinsic (eMc) and intrinsic parameters are retrieved from the robot
+ low level driver that is not public.
+
+*/
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER650) && defined (VISP_HAVE_DC1394) && defined (VISP_HAVE_X11))
+#include <visp3/core/vpConfig.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpMath.h>
+#if defined(VISP_HAVE_VIPER650) && defined(VISP_HAVE_DC1394) && defined(VISP_HAVE_X11)
+
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
- // - the 6 computed joint velocities (m/s, rad/s) to achieve the task
+ // - the 6 computed camera velocities (m/s, rad/s) to achieve the task
// - the 6 mesured joint velocities (m/s, rad/s)
// - the 6 mesured joint positions (m, rad)
// - the 2 values of s - s*
@@ -92,17 +91,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -114,11 +111,11 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper650 robot ;
+ vpRobotViper650 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
@@ -130,103 +127,95 @@ main()
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0);
g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
#endif
- g.open(I) ;
-
-
- vpDisplayX display(I, (int)(100+I.getWidth()+30), 200, "Current image") ;
+ g.open(I);
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplayX display(I, 100, 100, "Current image");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpDot2 dot ;
- vpImagePoint cog;
+ vpDot2 dot;
dot.setGraphics(true);
- for (int i=0; i< 10; i++)
- g.acquire(I) ;
+ for (int i = 0; i < 10; i++) // warm up the camera
+ g.acquire(I);
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
- cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpImagePoint cog = dot.getCog();
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
+ vpFeaturePoint p;
// retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// - set the constant gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
// Now the robot will be controlled in velocity
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- vpColVector v ;
- for ( ; ; ) {
- try {
- // Acquire a new image from the camera
- g.acquire(I) ;
+ 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);
- // Display this image
- vpDisplay::display(I) ;
+ // Display this image
+ vpDisplay::display(I);
+ try {
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
+ } catch (...) {
+ std::cout << "Error detected while tracking visual features.." << std::endl;
+ break;
+ }
- // Get the dot cog
- cog = dot.getCog();
+ // Get the dot cog
+ cog = dot.getCog();
- // Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ // Display a green cross at the center of gravity position in the image
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
- // Update the point feature from the dot location
- vpFeatureBuilder::create(p, cam, dot);
+ // Update the point feature from the dot location
+ vpFeatureBuilder::create(p, cam, dot);
- // Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ // Compute the visual servoing skew vector
+ vpColVector v = task.computeControlLaw();
- // Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ // Display the current and desired feature points in the image display
+ vpServoDisplay::display(task, cam, I);
- // Apply the computed camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- }
- catch(...) {
- std::cout << "Tracking failed... Stop the robot." << std::endl;
- v = 0;
- // Stop robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- return 0;
- }
+ // Apply the computed camera velocities to the robot
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -236,8 +225,7 @@ 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;
@@ -247,41 +235,44 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl; // s-s* for point
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl; // s-s* for point
- // Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::displayText(I, 10, 10, "Click to quit...", vpColor::red);
+ if (vpDisplay::getClick(I, false))
+ break;
+ // Flush the display
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ robot.stopMotion();
+
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (const vpException &e) {
+ flog.close(); // Close the log file
+ std::cout << "Catched an exception: " << e.getMessage() << std::endl;
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber connected to your computer...");
+ std::cout << "You do not have an Viper650 robot or a firewire framegrabber "
+ "connected to your computer..."
+ << std::endl;
}
#endif
diff --git a/example/servo-viper850/CMakeLists.txt b/example/servo-viper850/CMakeLists.txt
index 59e1706..0433d86 100644
--- a/example/servo-viper850/CMakeLists.txt
+++ b/example/servo-viper850/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
index c433261..28e0c54 100644
--- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionCurrent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,43 +43,41 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Viper S850 robot (arm with 6 degrees of freedom). The velocities resulting
from visual servo are here joint velocities. Visual features are the image
- coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by 10cm
- square.
+ coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by
+ 10cm square.
*/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
@@ -104,56 +103,50 @@
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+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
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpRotationMatrix cRo;
vpPose pose;
vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
+ for (int i = 0; i < ndot; i++) {
- double x=0, y=0;
+ double x = 0, y = 0;
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_y(y) ;
- pose.addPoint(point[i]) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x,
+ y); // pixel to meter conversion
+ point[i].set_x(x); // projection perspective p
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
// 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
- if (residual_lagrange < residual_dementhon)
+ if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
- }
- else { // init = false; use of the previous pose to initialise LOWE
+ } else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
- pose.computePose(vpPose::LOWE, cMo) ;
+ pose.computePose(vpPose::LOWE, cMo);
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
}
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -167,19 +160,17 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -189,157 +180,152 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper850 robot ;
+ 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 ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " 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 << 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 << " 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 << std::endl;
-
- vpDot2 dot[4] ;
+ 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) ;
- dot[i].initTracking(I) ;
+ for (i = 0; i < 4; i++) {
+ dot[i].setGraphics(true);
+ dot[i].initTracking(I);
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
cam.printParameters();
-
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
vpRxyzVector cro(vpMath::rad(0), vpMath::rad(10), vpMath::rad(20));
vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.3) ;
+ task.setLambda(0.3);
// Display task information
- task.print() ;
+ task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
+ task.print();
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- task.set_cVe(cVe) ;
- task.print() ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ task.set_cVe(cVe);
+ task.print();
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
- task.print() ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- return(1) ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ return (1);
}
// During the servo, we compute the pose using LOWE method. For the
@@ -347,38 +333,37 @@ main()
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -388,8 +373,7 @@ 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;
@@ -399,39 +383,37 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() << std::endl;
+ // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
+ // std::endl;
}
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
index 790a7e5..b5d4df8 100644
--- a/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DArtVelocityInteractionDesired.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,42 +43,40 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Viper S850 robot (arm with 6 degrees of freedom). The velocities resulting
from visual servo are here joint velocities. Visual features are the image
- coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by 10cm
- square.
+ coordinates of 4 points. The target is made of 4 dots arranged as a 10cm by
+ 10cm square.
*/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -91,19 +90,17 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -113,173 +110,168 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- // Define the square CAD model
- // Square dimention
- //#define L 0.075
+// Define the square CAD model
+// Square dimention
+//#define L 0.075
#define L 0.05
- // Distance between the camera and the square at the desired
- // position after visual servoing convergence
+// Distance between the camera and the square at the desired
+// position after visual servoing convergence
#define D 0.5
- vpRobotViper850 robot ;
+ 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 ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- 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 << " 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 << std::endl ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ 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 << " 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 << std::endl;
- vpDot dot[4] ;
+ 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) ;
- dot[i].initTracking(I) ;
+ for (i = 0; i < 4; i++) {
+ dot[i].setGraphics(true);
+ dot[i].initTracking(I);
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
cam.printParameters();
// 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
+ 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
// sets the desired position of the visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
- pd[0].buildFrom(-L,-L,D) ;
- pd[1].buildFrom(L,-L,D) ;
- pd[2].buildFrom(L,L,D) ;
- pd[3].buildFrom(-L,L,D) ;
+ pd[0].buildFrom(-L, -L, D);
+ pd[1].buildFrom(L, -L, D);
+ pd[2].buildFrom(L, L, D);
+ pd[3].buildFrom(-L, L, D);
// We want to see a point on a point
- std::cout << std::endl ;
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ std::cout << std::endl;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.4) ;
+ task.setLambda(0.4);
// Display task information
- task.print() ;
+ task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
+ task.print();
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- task.set_cVe(cVe) ;
- task.print() ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ task.set_cVe(cVe);
+ task.print();
// Set the Jacobian (expressed in the end-effector frame)
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
- task.print() ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- exit(1) ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ exit(1);
}
// Update the point feature from the dot location
- for (i=0 ; i < 4 ; i++)
- vpFeatureBuilder::create(p[i],cam, dot[i]);
+ for (i = 0; i < 4; i++)
+ vpFeatureBuilder::create(p[i], cam, dot[i]);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -289,8 +281,7 @@ 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;
@@ -300,39 +291,37 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() << std::endl;
+ // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
+ // std::endl;
}
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
index d0f319d..82afa04 100644
--- a/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
+++ b/example/servo-viper850/servoViper850FourPoints2DCamVelocityInteractionCurrent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,45 +42,43 @@
\brief Example of eye-in-hand control law. We control here a real robot, the
Viper S850 robot (arm with 6 degrees of freedom). The velocity is
- computed in camera frame. The inverse jacobian that converts cartesian velocities
- in joint velocities is implemented in the robot low level controller.
- Visual features are the image coordinates of 4 points. The target is made of 4 dots
- arranged as a 10cm by 10cm square.
+ computed in camera frame. The inverse jacobian that converts cartesian
+ velocities in joint velocities is implemented in the robot low level
+ controller. Visual features are the image coordinates of 4 points. The
+ target is made of 4 dots arranged as a 10cm by 10cm square.
*/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/blob/vpDot2.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
@@ -105,54 +104,50 @@
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+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
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpRotationMatrix cRo;
vpPose pose;
vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
+ for (int i = 0; i < ndot; i++) {
- double x=0, y=0;
+ double x = 0, y = 0;
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_y(y) ;
- pose.addPoint(point[i]) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x,
+ y); // pixel to meter conversion
+ point[i].set_x(x); // projection perspective p
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
// 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
- if (residual_lagrange < residual_dementhon)
+ if (residual_lagrange < residual_dementhon)
cMo = cMo_lagrange;
else
cMo = cMo_dementhon;
- }
- else { // init = false; use of the previous pose to initialise LOWE
+ } else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
- pose.computePose(vpPose::LOWE, cMo) ;
+ pose.computePose(vpPose::LOWE, cMo);
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
}
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -166,19 +161,17 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -188,147 +181,141 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper850 robot ;
+ 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 ;
+ vpServo task;
- vpImage<unsigned char> I ;
- int i ;
+ vpImage<unsigned char> I;
+ int i;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
-
- 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 << " 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 << std::endl ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ 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 << " 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 << std::endl;
- vpDot2 dot[4] ;
+ 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) ;
- dot[i].initTracking(I) ;
+ for (i = 0; i < 4; i++) {
+ dot[i].setGraphics(true);
+ dot[i].initTracking(I);
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
cam.printParameters();
-
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
vpRxyzVector cro(vpMath::rad(10), vpMath::rad(30), vpMath::rad(20));
vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.3) ;
+ task.setLambda(0.3);
// Display task information
- task.print() ;
+ task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
-
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
- return(1) ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
+ return (1);
}
// During the servo, we compute the pose using LOWE method. For the
@@ -336,31 +323,30 @@ main()
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -370,8 +356,7 @@ 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;
@@ -381,39 +366,37 @@ 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 expressed
- // in meters in the camera frame
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
flog << task.getError() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() << std::endl;
+ // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
+ // std::endl;
}
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850FourPointsKinect.cpp b/example/servo-viper850/servoViper850FourPointsKinect.cpp
index cff4e96..9013349 100644
--- a/example/servo-viper850/servoViper850FourPointsKinect.cpp
+++ b/example/servo-viper850/servoViper850FourPointsKinect.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,42 +41,40 @@
\example servoViper850FourPointsKinect.cpp
\brief Example of eye-in-hand control law. We control here a real robot, the
- Viper850 robot (cartesian robot, with 6 degrees of freedom). A kinect is attached to the hand.
- The velocity is computed in the kinect camera frame. Visual features are the image
- coordinates of 4 points.
+ Viper850 robot (cartesian robot, with 6 degrees of freedom). A kinect is
+ attached to the hand. The velocity is computed in the kinect camera frame.
+ Visual features are the image coordinates of 4 points.
*/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES) )
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES))
-
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/sensor/vpKinect.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -84,7 +83,6 @@
#include <visp3/blob/vpDot2.h>
#define L 0.05 // to deal with a 10cm by 10cm square
-
/*!
Compute the pose \e cMo from the 3D coordinates of the points \e point and
@@ -110,33 +108,31 @@
Lagrange or Dementhon methods.
*/
-void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
- vpCameraParameters cam,
- vpHomogeneousMatrix &cMo,
- vpTranslationVector &cto,
- vpRxyzVector &cro, bool init)
+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
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
vpRotationMatrix cRo;
vpPose pose;
vpImagePoint cog;
- for (int i=0; i < ndot; i ++) {
+ for (int i = 0; i < ndot; i++) {
- double x=0, y=0;
+ double x = 0, y = 0;
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_y(y) ;
- pose.addPoint(point[i]) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x,
+ y); // pixel to meter conversion
+ point[i].set_x(x); // projection perspective p
+ point[i].set_y(y);
+ pose.addPoint(point[i]);
}
if (init == true) {
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
// Compute and return the residual expressed in meter for the pose matrix
// 'cMo'
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// Select the best pose to initialize the lowe pose computation
@@ -145,12 +141,11 @@ void compute_pose(vpPoint point[], vpDot2 dot[], int ndot,
else
cMo = cMo_dementhon;
- }
- else { // init = false; use of the previous pose to initialise LOWE
+ } else { // init = false; use of the previous pose to initialise LOWE
cRo.buildFrom(cro);
cMo.buildFrom(cto, cRo);
}
- pose.computePose(vpPose::LOWE, cMo) ;
+ pose.computePose(vpPose::LOWE, cMo);
cMo.extract(cto);
cMo.extract(cRo);
cro.buildFrom(cRo);
@@ -170,19 +165,17 @@ int main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
- return(-1);
+ return (-1);
}
}
std::string logfilename;
@@ -192,26 +185,26 @@ int main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper850 robot ;
+ 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 ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
vpImage<vpRGBa> Irgb;
- int i ;
+ int i;
#ifdef VISP_HAVE_LIBFREENECT_OLD
- // This is the way to initialize Freenect with an old version of libfreenect packages under ubuntu lucid 10.04
+ // This is the way to initialize Freenect with an old version of
+ // libfreenect packages under ubuntu lucid 10.04
Freenect::Freenect<vpKinect> freenect;
- vpKinect & kinect = freenect.createDevice(0);
+ vpKinect &kinect = freenect.createDevice(0);
#else
Freenect::Freenect freenect;
- vpKinect & kinect = freenect.createDevice<vpKinect>(0);
+ vpKinect &kinect = freenect.createDevice<vpKinect>(0);
#endif
kinect.start(vpKinect::DMAP_LOW_RES);
@@ -219,125 +212,123 @@ int main()
vpImageConvert::convert(Irgb, I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- 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 << " 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 << 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 << " 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 << std::endl;
- vpDot2 dot[4] ;
+ 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) ;
+ for (i = 0; i < 4; i++) {
+ dot[i].initTracking(I);
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
}
// Get Kinect Camera Parameters
- vpCameraParameters cam ;
- //kinect.getRGBCamParameters(cam);
+ vpCameraParameters cam;
+ // kinect.getRGBCamParameters(cam);
robot.getCameraParameters(cam, I);
cam.printParameters();
// 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
+ vpFeaturePoint p[4];
+ for (i = 0; i < 4; i++)
+ 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
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-L, -L, 0) ;
- point[1].setWorldCoordinates( L, -L, 0) ;
- point[2].setWorldCoordinates( L, L, 0) ;
- point[3].setWorldCoordinates(-L, L, 0) ;
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-L, -L, 0);
+ point[1].setWorldCoordinates(L, -L, 0);
+ point[2].setWorldCoordinates(L, L, 0);
+ point[3].setWorldCoordinates(-L, L, 0);
// Initialise a desired pose to compute s*, the desired 2D point features
vpHomogeneousMatrix cMo;
vpTranslationVector cto(0, 0, 0.5); // tz = 0.5 meter
vpRxyzVector cro(vpMath::rad(0), vpMath::rad(0), vpMath::rad(0));
vpRotationMatrix cRo(cro); // Build the rotation matrix
- cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
+ cMo.buildFrom(cto, cRo); // Build the homogeneous matrix
// Sets the desired position of the 2D visual feature
- vpFeaturePoint pd[4] ;
+ vpFeaturePoint pd[4];
// Compute the desired position of the features from the desired pose
- for (int i=0; i < 4; i ++) {
- vpColVector cP, p ;
- point[i].changeFrame(cMo, cP) ;
- point[i].projection(cP, p) ;
+ for (int i = 0; i < 4; i++) {
+ vpColVector cP, p;
+ point[i].changeFrame(cMo, cP);
+ point[i].projection(cP, p);
- pd[i].set_x(p[0]) ;
- pd[i].set_y(p[1]) ;
+ pd[i].set_x(p[0]);
+ pd[i].set_y(p[1]);
pd[i].set_Z(cP[2]);
}
// We want to see a point on a point
- for (i=0 ; i < 4 ; i++)
- task.addFeature(p[i],pd[i]) ;
+ for (i = 0; i < 4; i++)
+ task.addFeature(p[i], pd[i]);
// Set the proportional gain
- task.setLambda(0.5) ;
+ task.setLambda(0.5);
// Display task information
- task.print() ;
+ task.print();
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE) ;
- task.print() ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::CURRENT, vpServo::PSEUDO_INVERSE);
+ task.print();
// Initialise the velocity control of the robot
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the kinect
kinect.getRGB(Irgb);
vpImageConvert::convert(Irgb, I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
try {
// For each point...
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Achieve the tracking of the dot in the image
- dot[i].track(I) ;
+ dot[i].track(I);
// Display a green cross at the center of gravity position in the
// image
cog = dot[i].getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
}
- }
- catch(...) {
- flog.close() ; // Close the log file
- vpTRACE("Error detected while tracking visual features") ;
- robot.stopMotion() ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpTRACE("Error detected while tracking visual features");
+ robot.stopMotion();
kinect.stop();
- return(1) ;
+ return (1);
}
// During the servo, we compute the pose using LOWE method. For the
@@ -345,31 +336,30 @@ int main()
// computed at the previous iteration.
compute_pose(point, dot, 4, cam, cMo, cto, cro, false);
- for (i=0 ; i < 4 ; i++) {
+ for (i = 0; i < 4; i++) {
// Update the point feature from the dot location
vpFeatureBuilder::create(p[i], cam, dot[i]);
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task,cam,I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// 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;
@@ -379,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;
@@ -390,31 +379,29 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl;
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl;
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() << std::endl;
+ // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
+ // std::endl;
}
kinect.stop();
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
@@ -422,7 +409,8 @@ int main()
#else
int main()
{
- vpERROR_TRACE("You do not have a Viper robot or a kinect connected to your computer...");
+ vpERROR_TRACE("You do not have a Viper robot or a kinect connected to your "
+ "computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
index 3cdb454..3122dce 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,7 @@
/*!
\example servoViper850Point2DArtVelocity-jointAvoidance-basic.cpp
- Joint limits avoidance by stopping the motion on axis near the joint limits.
+ Joint limits avoidance by stopping the motion on axis near the joint limits.
Implemented from section III.B in \cite Chaumette01c.
@@ -50,102 +51,108 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
+#include <cmath> // std::fabs
#include <fstream>
+#include <iostream>
+#include <limits> // numeric_limits
#include <sstream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/gui/vpPlot.h>
-
-int
-main()
+int main()
{
try {
- vpRobotViper850 robot ;
+ vpRobotViper850 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
- double Tloop = 1./60.f;
+ double Tloop = 1. / 60.f;
vp1394TwoGrabber::vp1394TwoFramerateType fps;
g.getFramerate(fps);
- switch(fps) {
- case vp1394TwoGrabber::vpFRAMERATE_15 : Tloop = 1.f/15.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_30 : Tloop = 1.f/30.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_60 : Tloop = 1.f/60.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_120: Tloop = 1.f/120.f; break;
- default: break;
+ switch (fps) {
+ case vp1394TwoGrabber::vpFRAMERATE_15:
+ Tloop = 1.f / 15.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_30:
+ Tloop = 1.f / 30.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_60:
+ Tloop = 1.f / 60.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_120:
+ Tloop = 1.f / 120.f;
+ break;
+ default:
+ break;
}
- std::cout << "Tloop: " << Tloop << std::endl;
+ std::cout << "Tloop: " << Tloop << std::endl;
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,800,100,"Current image") ;
+ vpDisplayX display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,800,100,"Current image") ;
+ vpDisplayOpenCV display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,800,100,"Current image") ;
+ vpDisplayGTK display(I, 800, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpColVector jointMin(6), jointMax(6) ;
+ vpColVector jointMin(6), jointMax(6);
jointMin = robot.getJointMin();
jointMax = robot.getJointMax();
- vpColVector Qmin(6), tQmin(6) ;
- vpColVector Qmax(6), tQmax(6) ;
+ vpColVector Qmin(6), tQmin(6);
+ vpColVector Qmax(6), tQmax(6);
vpColVector Qmiddle(6);
- vpColVector data(10) ;
+ vpColVector data(10);
- double rho = 0.25 ;
- for (unsigned int i=0 ; i < 6 ; i++)
- {
- Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
- Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
+ double rho = 0.25;
+ for (unsigned int i = 0; i < 6; i++) {
+ Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
+ Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
}
- Qmiddle = (Qmin + Qmax) /2.;
- double rho1 = 0.1 ;
-
- for (unsigned int i=0 ; i < 6 ; i++) {
- tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
- tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
+ Qmiddle = (Qmin + Qmax) / 2.;
+ double rho1 = 0.1;
+
+ for (unsigned int i = 0; i < 6; i++) {
+ tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
+ tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
}
- vpColVector q(6) ;
+ vpColVector q(6);
// Create a window with two graphics
// - first graphic to plot q(t), Qmin, Qmax, tQmin and tQmax
@@ -158,21 +165,21 @@ main()
plot.initGraph(1, 6);
// For the first graphic :
- // - along the x axis the expected values are between 0 and 200 and
- // the step is 1
+ // - along the x axis the expected values are between 0 and 200 and
+ // the step is 1
// - along the y axis the expected values are between -1.2 and 1.2 and the
// step is 0.1
- plot.initRange(0,0,200,1,-1.2,1.2,0.1);
+ plot.initRange(0, 0, 200, 1, -1.2, 1.2, 0.1);
plot.setTitle(0, "Joint behavior");
- plot.initRange(1,0,200,1,-0.01,0.01,0.05);
+ plot.initRange(1, 0, 200, 1, -0.01, 0.01, 0.05);
plot.setTitle(1, "Joint velocity");
// For the first graphic, set the curves legend
char legend[10];
- for (unsigned int i=0; i < 6; i++) {
- sprintf(legend, "q%u", i+1);
- plot.setLegend(0, i, legend);
- sprintf(legend, "q%u", i+1);
+ for (unsigned int i = 0; i < 6; i++) {
+ sprintf(legend, "q%u", i + 1);
+ plot.setLegend(0, i, legend);
+ sprintf(legend, "q%u", i + 1);
plot.setLegend(1, i, legend);
}
plot.setLegend(0, 6, "tQmin");
@@ -181,102 +188,102 @@ main()
plot.setLegend(0, 9, "Qmax");
// Set the curves color
- plot.setColor(0, 0, vpColor::red);
- plot.setColor(0, 1, vpColor::green);
- plot.setColor(0, 2, vpColor::blue);
- plot.setColor(0, 3, vpColor::orange);
- plot.setColor(0, 4, vpColor(0, 128, 0));
- plot.setColor(0, 5, vpColor::cyan);
- for (unsigned int i= 6; i < 10; i++)
+ plot.setColor(0, 0, vpColor::red);
+ plot.setColor(0, 1, vpColor::green);
+ plot.setColor(0, 2, vpColor::blue);
+ plot.setColor(0, 3, vpColor::orange);
+ plot.setColor(0, 4, vpColor(0, 128, 0));
+ plot.setColor(0, 5, vpColor::cyan);
+ for (unsigned int i = 6; i < 10; i++)
plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
// Set the curves color
- plot.setColor(1, 0, vpColor::red);
- plot.setColor(1, 1, vpColor::green);
- plot.setColor(1, 2, vpColor::blue);
- plot.setColor(1, 3, vpColor::orange);
- plot.setColor(1, 4, vpColor(0, 128, 0));
- plot.setColor(1, 5, vpColor::cyan);
- vpDot2 dot ;
-
+ plot.setColor(1, 0, vpColor::red);
+ plot.setColor(1, 1, vpColor::green);
+ plot.setColor(1, 2, vpColor::blue);
+ plot.setColor(1, 3, vpColor::orange);
+ plot.setColor(1, 4, vpColor(0, 128, 0));
+ plot.setColor(1, 5, vpColor::cyan);
+ vpDot2 dot;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
- p.set_Z(1) ;
+ p.set_Z(1);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// - Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// - we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// - set the gain
double lambda = 0.8;
- // set to -1 to suppress the lambda used in the vpServo::computeControlLaw()
- task.setLambda(-1) ;
+ // set to -1 to suppress the lambda used in the
+ // vpServo::computeControlLaw()
+ task.setLambda(-1);
// Display task information " ) ;
- task.print() ;
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
int iter = 0;
double t_1 = vpTime::measureTimeMs();
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
- iter ++;
-
+ for (;;) {
+ iter++;
+
double t_0 = vpTime::measureTimeMs(); // t_0: current time
// Update loop time in second
double Tv = (double)(t_0 - t_1) / 1000.0;
std::cout << "Tv: " << Tv << std::endl;
-
+
// Update time for next iteration
t_1 = t_0;
-
+
// Acquire a new image from the camera
dc1394video_frame_t *frame = g.dequeue(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Get the measured joint positions of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -285,126 +292,126 @@ main()
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
-
- vpColVector prim_task ;
- vpColVector e2(6) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
+
+ vpColVector prim_task;
+ vpColVector e2(6);
// Compute the visual servoing skew vector
- prim_task = task.computeControlLaw() ;
+ prim_task = task.computeControlLaw();
vpColVector qpre(6);
- qpre = q ;
- qpre += -lambda*prim_task*(4*Tloop) ;
+ qpre = q;
+ qpre += -lambda * prim_task * (4 * Tloop);
// Identify the joints near the limits
- vpColVector pb(6) ; pb = 0 ;
- unsigned int npb =0 ;
- for (unsigned int i=0 ; i < 6 ;i++) {
+ vpColVector pb(6);
+ pb = 0;
+ unsigned int npb = 0;
+ for (unsigned int i = 0; i < 6; i++) {
if (q[i] < tQmin[i])
- if (fabs(Qmin[i]-q[i]) > fabs(Qmin[i]-qpre[i])) {
- pb[i] = 1 ; npb++ ;
- std::cout << "Joint " << i << " near limit " << std::endl ;
+ if (fabs(Qmin[i] - q[i]) > fabs(Qmin[i] - qpre[i])) {
+ pb[i] = 1;
+ npb++;
+ std::cout << "Joint " << i << " near limit " << std::endl;
}
- if (q[i]>tQmax[i]) {
- if (fabs(Qmax[i]-q[i]) > fabs(Qmax[i]-qpre[i])) {
- pb[i] = 1 ; npb++ ;
- std::cout << "Joint " << i << " near limit " << std::endl ;
+ if (q[i] > tQmax[i]) {
+ if (fabs(Qmax[i] - q[i]) > fabs(Qmax[i] - qpre[i])) {
+ pb[i] = 1;
+ npb++;
+ std::cout << "Joint " << i << " near limit " << std::endl;
}
}
}
- vpColVector a0 ;
+ vpColVector a0;
vpMatrix J1 = task.getTaskJacobian();
vpMatrix kernelJ1;
J1.kernel(kernelJ1);
- unsigned int dimKernelL = kernelJ1.getCols() ;
+ unsigned int dimKernelL = kernelJ1.getCols();
if (npb != 0) {
// Build linear system a0*E = S
- vpMatrix E(npb, dimKernelL) ;
- vpColVector S(npb) ;
+ vpMatrix E(npb, dimKernelL);
+ vpColVector S(npb);
- unsigned int k=0 ;
+ unsigned int k = 0;
- for (unsigned int j=0 ; j < 6 ; j++) // j is the joint
- //if (pb[j]==1) {
- if (std::fabs(pb[j]-1) <= std::numeric_limits<double>::epsilon()) {
- for (unsigned int i=0 ; i < dimKernelL ; i++)
- E[k][i] = kernelJ1[j][i] ;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < dimKernelL; i++)
+ E[k][i] = kernelJ1[j][i];
- S[k] = -prim_task[j] ;
- k++ ;
+ S[k] = -prim_task[j];
+ k++;
}
- vpMatrix Ep ;
- //vpTRACE("nbp %d", npb);
- Ep = E.t()*(E*E.t()).pseudoInverse() ;
- a0 = Ep*S ;
-
- e2 = (kernelJ1*a0) ;
- //cout << "e2 " << e2.t() ;
- }
- else {
+ vpMatrix Ep;
+ // vpTRACE("nbp %d", npb);
+ Ep = E.t() * (E * E.t()).pseudoInverse();
+ a0 = Ep * S;
+
+ e2 = (kernelJ1 * a0);
+ // cout << "e2 " << e2.t() ;
+ } else {
e2 = 0;
}
- // std::cout << "e2: " << e2.t() << std::endl;
+ // std::cout << "e2: " << e2.t() << std::endl;
- vpColVector v ;
+ vpColVector v;
v = -lambda * (prim_task + e2);
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
{
// Add the material to plot curves
// q normalized between (entre -1 et 1)
- for (unsigned int i=0 ; i < 6 ; i++) {
- data[i] = (q[i] - Qmiddle[i]) ;
- data[i] /= (Qmax[i] - Qmin[i]) ;
- data[i]*=2 ;
+ for (unsigned int i = 0; i < 6; i++) {
+ data[i] = (q[i] - Qmiddle[i]);
+ data[i] /= (Qmax[i] - Qmin[i]);
+ 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[8] = -1 ; data[9] = 1 ;
+ 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) ;
+ vpDisplay::flush(I);
// Synchronize the loop with the image frame rate
- vpTime::wait(t_0, 1000.*Tloop);
+ vpTime::wait(t_0, 1000. * Tloop);
// Release the ring buffer used for the last image to start a new acq
g.enqueue(frame);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
index a52d646..775a6f4 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,7 @@
/*!
\example servoViper850Point2DArtVelocity-jointAvoidance-gpa.cpp
- Joint limits avoidance using a gradient projection approach.
+ Joint limits avoidance using a gradient projection approach.
Implemented from \cite Marchand96f and section II.B in \cite Chaumette01c.
*/
@@ -49,87 +50,84 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/gui/vpPlot.h>
-
-int
-main()
+int main()
{
try {
- vpRobotViper850 robot ;
+ vpRobotViper850 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,800,100,"Current image") ;
+ vpDisplayX display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,800,100,"Current image") ;
+ vpDisplayOpenCV display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,800,100,"Current image") ;
+ vpDisplayGTK display(I, 800, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpColVector jointMin(6), jointMax(6) ;
+ vpColVector jointMin(6), jointMax(6);
jointMin = robot.getJointMin();
jointMax = robot.getJointMax();
- vpColVector Qmin(6), tQmin(6) ;
- vpColVector Qmax(6), tQmax(6) ;
+ vpColVector Qmin(6), tQmin(6);
+ vpColVector Qmax(6), tQmax(6);
vpColVector Qmiddle(6);
- vpColVector data(10) ;
+ vpColVector data(10);
- double rho = 0.15 ;
- for (unsigned int i=0 ; i < 6 ; i++)
- {
- Qmin[i] = jointMin[i] + 0.5*rho*(jointMax[i]-jointMin[i]) ;
- Qmax[i] = jointMax[i] - 0.5*rho*(jointMax[i]-jointMin[i]) ;
+ double rho = 0.15;
+ for (unsigned int i = 0; i < 6; i++) {
+ Qmin[i] = jointMin[i] + 0.5 * rho * (jointMax[i] - jointMin[i]);
+ Qmax[i] = jointMax[i] - 0.5 * rho * (jointMax[i] - jointMin[i]);
}
- Qmiddle = (Qmin + Qmax) /2.;
- double rho1 = 0.1 ;
-
- for (unsigned int i=0 ; i < 6 ; i++) {
- tQmin[i]=Qmin[i]+ 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
- tQmax[i]=Qmax[i]- 0.5*(rho1)*(Qmax[i]-Qmin[i]) ;
+ Qmiddle = (Qmin + Qmax) / 2.;
+ double rho1 = 0.1;
+
+ for (unsigned int i = 0; i < 6; i++) {
+ tQmin[i] = Qmin[i] + 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
+ tQmax[i] = Qmax[i] - 0.5 * (rho1) * (Qmax[i] - Qmin[i]);
}
- vpColVector q(6) ;
+ vpColVector q(6);
// Create a window with two graphics
// - first graphic to plot q(t), Qmin, Qmax, tQmin and tQmax
@@ -142,16 +140,15 @@ main()
// The second graphic contains 1 curve, the cost function h_s
plot.initGraph(1, 1);
-
// For the first graphic :
- // - along the x axis the expected values are between 0 and 200
- // - along the y axis the expected values are between -1.2 and 1.2
+ // - along the x axis the expected values are between 0 and 200
+ // - along the y axis the expected values are between -1.2 and 1.2
plot.initRange(0, 0., 200., -1.2, 1.2);
plot.setTitle(0, "Joint behavior");
// For the second graphic :
- // - along the x axis the expected values are between 0 and 200 and
- // the step is 1
+ // - along the x axis the expected values are between 0 and 200 and
+ // the step is 1
// - along the y axis the expected values are between 0 and 0.0001 and the
// step is 0.00001
plot.initRange(1, 0., 200., 0., 1e-4);
@@ -159,8 +156,8 @@ main()
// For the first graphic, set the curves legend
char legend[10];
- for (unsigned int i=0; i < 6; i++) {
- sprintf(legend, "q%u", i+1);
+ for (unsigned int i = 0; i < 6; i++) {
+ sprintf(legend, "q%u", i + 1);
plot.setLegend(0, i, legend);
}
plot.setLegend(0, 6, "tQmin");
@@ -169,89 +166,89 @@ main()
plot.setLegend(0, 9, "Qmax");
// Set the curves color
- plot.setColor(0, 0, vpColor::red);
- plot.setColor(0, 1, vpColor::green);
- plot.setColor(0, 2, vpColor::blue);
- plot.setColor(0, 3, vpColor::orange);
- plot.setColor(0, 4, vpColor(0, 128, 0));
- plot.setColor(0, 5, vpColor::cyan);
- for (unsigned int i= 6; i < 10; i++)
+ plot.setColor(0, 0, vpColor::red);
+ plot.setColor(0, 1, vpColor::green);
+ plot.setColor(0, 2, vpColor::blue);
+ plot.setColor(0, 3, vpColor::orange);
+ plot.setColor(0, 4, vpColor(0, 128, 0));
+ plot.setColor(0, 5, vpColor::cyan);
+ for (unsigned int i = 6; i < 10; i++)
plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
// For the second graphic, set the curves legend
plot.setLegend(1, 0, "h_s");
- double beta = 1;
+ double beta = 1;
// Set the amplitude of the control law due to the secondary task
std::cout << " Give the parameters beta (1) : ";
- std::cin >> beta ;
+ std::cin >> beta;
- vpDot2 dot ;
+ vpDot2 dot;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
- p.set_Z(1) ;
+ p.set_Z(1);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// - Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// - we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// - set the gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
// Display task information " ) ;
- task.print() ;
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
int iter = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
- iter ++;
+ for (;;) {
+ iter++;
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Get the measured joint positions of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -260,87 +257,87 @@ main()
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
-
- vpColVector prim_task ;
- vpColVector e2(6) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
+
+ vpColVector prim_task;
+ vpColVector e2(6);
// Compute the visual servoing skew vector
- prim_task = task.computeControlLaw() ;
+ prim_task = task.computeControlLaw();
- vpColVector sec_task(6) ;
- double h_s = 0 ;
+ vpColVector sec_task(6);
+ double h_s = 0;
{
// joint limit avoidance with secondary task
vpColVector de2dt(6);
- de2dt = 0 ;
- e2 = 0 ;
- for (unsigned int i=0 ; i < 6 ; i++)
- {
- double S = 0 ;
- if (q[i] > tQmax[i]) S = q[i] - tQmax[i] ;
- if (q[i] < tQmin[i]) S = q[i] - tQmin[i] ;
- double D = (Qmax[i]-Qmin[i]) ;
- h_s += vpMath::sqr(S)/D ;
- e2[i] = S/D ;
+ de2dt = 0;
+ e2 = 0;
+ for (unsigned int i = 0; i < 6; i++) {
+ double S = 0;
+ if (q[i] > tQmax[i])
+ S = q[i] - tQmax[i];
+ if (q[i] < tQmin[i])
+ S = q[i] - tQmin[i];
+ double D = (Qmax[i] - Qmin[i]);
+ h_s += vpMath::sqr(S) / D;
+ e2[i] = S / D;
}
- h_s = beta*h_s/2.0 ; // cost function
- e2 *= beta ;
+ h_s = beta * h_s / 2.0; // cost function
+ e2 *= beta;
// std::cout << e2.t() << std::endl;
std::cout << "Cost function h_s: " << h_s << std::endl;
- sec_task = task.secondaryTask(e2, de2dt) ;
+ sec_task = task.secondaryTask(e2, de2dt);
}
- vpColVector v ;
+ vpColVector v;
v = prim_task + sec_task;
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
{
// Add the material to plot curves
// q normalized between (entre -1 et 1)
- for (unsigned int i=0 ; i < 6 ; i++) {
- data[i] = (q[i] - Qmiddle[i]) ;
- data[i] /= (Qmax[i] - Qmin[i]) ;
- data[i]*=2 ;
+ for (unsigned int i = 0; i < 6; i++) {
+ data[i] = (q[i] - Qmiddle[i]);
+ data[i] /= (Qmax[i] - Qmin[i]);
+ 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[8] = -1 ; data[9] = 1 ;
- plot.plot(0, iter, data); // plot q, Qmin, Qmax, tQmin, tQmax
+ 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, 0, iter, h_s); // plot the cost function
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch (vpException &e)
- {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an Viper 850 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an Viper 850 robot or a firewire "
+ "framegrabber connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
index e1d5eee..b5cc6f4 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,90 +43,91 @@
/*!
\example servoViper850Point2DArtVelocity-jointAvoidance-large.cpp
- Joint limits avoidance using a secondary task for joint limit avoidance \cite Marey:2010b using the
- new large projection operator (see equation(24) in the paper \cite Marey:2010).
+ Joint limits avoidance using a secondary task for joint limit avoidance
+ \cite Marey:2010b using the new large projection operator (see equation(24)
+ in the paper \cite Marey:2010).
*/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/gui/vpPlot.h>
-
-int
-main()
+int main()
{
try {
- vpRobotViper850 robot ;
+ vpRobotViper850 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,800,100,"Current image") ;
+ vpDisplayX display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,800,100,"Current image") ;
+ vpDisplayOpenCV display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,800,100,"Current image") ;
+ vpDisplayGTK display(I, 800, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpColVector jointMin(6), jointMax(6) ;
+ vpColVector jointMin(6), jointMax(6);
jointMin = robot.getJointMin();
jointMax = robot.getJointMax();
vpColVector Qmiddle(6);
- vpColVector data(12) ;
+ vpColVector data(12);
- Qmiddle = (jointMin + jointMax) /2.;
+ Qmiddle = (jointMin + jointMax) / 2.;
// double rho1 = 0.1 ;
-
+
double rho = 0.1;
double rho1 = 0.3;
- vpColVector q(6) ;
+ vpColVector q(6);
// Create a window with two graphics
- // - first graphic to plot q(t), Qmin, Qmax, Ql0min, Ql1min, Ql0max and Ql1max
+ // - first graphic to plot q(t), Qmin, Qmax, Ql0min, Ql1min, Ql0max and
+ // Ql1max
vpPlot plot(2);
- // The first graphic contains 12 data to plot: q(t), Low Limits, Upper Limits, ql0min, ql1min, ql0max and ql1max
+ // The first graphic contains 12 data to plot: q(t), Low Limits, Upper
+ // Limits, ql0min, ql1min, ql0max and ql1max
plot.initGraph(0, 12);
// The second graphic contains the values of the secondaty task velocities
plot.initGraph(1, 6);
@@ -141,8 +143,8 @@ main()
// For the first and second graphic, set the curves legend
char legend[10];
- for (unsigned int i=0; i < 6; i++) {
- sprintf(legend, "q%u", i+1);
+ for (unsigned int i = 0; i < 6; i++) {
+ sprintf(legend, "q%u", i + 1);
plot.setLegend(0, i, legend);
plot.setLegend(1, i, legend);
}
@@ -160,76 +162,76 @@ main()
plot.setColor(0, 3, vpColor::orange);
plot.setColor(0, 4, vpColor(0, 128, 0));
plot.setColor(0, 5, vpColor::cyan);
- for (unsigned int i= 6; i < 12; i++)
+ for (unsigned int i = 6; i < 12; i++)
plot.setColor(0, i, vpColor::black); // for Q and tQ [min,max]
- vpColVector sec_task(6) ;
+ vpColVector sec_task(6);
+
+ vpDot2 dot;
- vpDot2 dot ;
-
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,cam, dot) ; //retrieve x,y and Z of the vpPoint structure
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, cam, dot); // retrieve x,y and Z of the vpPoint structure
- p.set_Z(1) ;
+ p.set_Z(1);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// Define the task
// - we want an eye-in-hand control law
// - articular velocity are computed
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// - Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
// - we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ std::cout << std::endl;
+ task.addFeature(p, pd);
// - set the gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
// Display task information " ) ;
- task.print() ;
+ task.print();
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
int iter = 0;
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
- iter ++;
+ for (;;) {
+ iter++;
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Get the measured joint positions of the robot
robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -238,76 +240,74 @@ main()
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
- vpColVector prim_task ;
+ vpColVector prim_task;
// Compute the visual servoing skew vector
- prim_task = task.computeControlLaw() ;
+ prim_task = task.computeControlLaw();
// Compute the secondary task for the joint limit avoidance
sec_task = task.secondaryTaskJointLimitAvoidance(q, prim_task, jointMin, jointMax, rho, rho1);
- vpColVector v ;
+ vpColVector v;
v = prim_task + sec_task;
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
{
// Add the material to plot curves
// q normalized between (entre -1 et 1)
- for (unsigned int i=0 ; i < 6 ; i++) {
- data[i] = (q[i] - Qmiddle[i]) ;
- data[i] /= (jointMax[i] - jointMin[i]) ;
- data[i]*=2 ;
+ for (unsigned int i = 0; i < 6; i++) {
+ data[i] = (q[i] - Qmiddle[i]);
+ data[i] /= (jointMax[i] - jointMin[i]);
+ data[i] *= 2;
}
data[6] = -1.0;
data[7] = 1.0;
unsigned int joint = 2;
- double tQmin_l0 = jointMin[joint] + rho *(jointMax[joint] - jointMin[joint]);
- double tQmax_l0 = jointMax[joint] - rho *(jointMax[joint] - jointMin[joint]);
-
- double tQmin_l1 = tQmin_l0 - rho * rho1 * (jointMax[joint] - jointMin[joint]);
- double tQmax_l1 = tQmax_l0 + rho * rho1 * (jointMax[joint] - jointMin[joint]);
-
- data[8] = 2*(tQmin_l0 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
- data[9] = 2*(tQmax_l0 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
- data[10] = 2*(tQmin_l1 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
- data[11] = 2*(tQmax_l1 - Qmiddle[joint])/(jointMax[joint] - jointMin[joint]);
- plot.plot(0, iter, data); // plot q(t), Low Limits, Upper Limits, ql0min, ql1min, ql0max and ql1max
- plot.plot(1, iter, sec_task); //plot secondary task velocities
+ double tQmin_l0 = jointMin[joint] + rho * (jointMax[joint] - jointMin[joint]);
+ double tQmax_l0 = jointMax[joint] - rho * (jointMax[joint] - jointMin[joint]);
+
+ double tQmin_l1 = tQmin_l0 - rho * rho1 * (jointMax[joint] - jointMin[joint]);
+ double tQmax_l1 = tQmax_l0 + rho * rho1 * (jointMax[joint] - jointMin[joint]);
+
+ data[8] = 2 * (tQmin_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
+ data[9] = 2 * (tQmax_l0 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
+ data[10] = 2 * (tQmin_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
+ data[11] = 2 * (tQmax_l1 - Qmiddle[joint]) / (jointMax[joint] - jointMin[joint]);
+ plot.plot(0, iter, data); // plot q(t), Low Limits, Upper Limits,
+ // ql0min, ql1min, ql0max and ql1max
+ plot.plot(1, iter, sec_task); // plot secondary task velocities
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Display task information
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch (...)
- {
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
index b4d9a9b..1749ffe 100644
--- a/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
+++ b/example/servo-viper850/servoViper850Point2DArtVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,36 +43,36 @@
\example servoViper850Point2DArtVelocity.cpp
Example of eye-in-hand control law. We control here a real robot, the Viper
- S850 Adept robot (arm with 6 degrees of freedom). The velocity is computed in
- articular. The visual feature is the center of gravity of a point.
+ S850 Adept robot (arm with 6 degrees of freedom). The velocity is computed
+ in articular. The visual feature is the center of gravity of a point.
*/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
// Exception
#include <visp3/core/vpException.h>
@@ -79,8 +80,7 @@
#include <visp3/blob/vpDot2.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -94,17 +94,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -116,141 +114,136 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper850 robot ;
+ vpRobotViper850 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
g.setFramerate(vp1394TwoGrabber::vpFRAMERATE_60);
- g.open(I) ;
+ g.open(I);
- g.acquire(I) ;
+ g.acquire(I);
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,800,100,"Current image") ;
+ vpDisplayX display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,800,100,"Current image") ;
+ vpDisplayOpenCV display(I, 800, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,800,100,"Current image") ;
+ vpDisplayGTK display(I, 800, 100, "Current image");
#endif
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
// exit(1) ;
- 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 << " 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;
+ 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 << " Use of the Afma6 robot " << std::endl;
+ std::cout << " task : servo a point " << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
-
- vpDot2 dot ;
+ vpDot2 dot;
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
vpImagePoint cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
-
- 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
+ robot.getCameraParameters(cam, I);
- p.set_Z(1) ;
- vpTRACE("sets the desired position of the visual feature ") ;
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ 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
- vpTRACE("define the task") ;
- vpTRACE("\t we want an eye-in-hand control law") ;
- vpTRACE("\t articular velocity are computed") ;
- task.setServo(vpServo::EYEINHAND_L_cVe_eJe) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ p.set_Z(1);
+ vpTRACE("sets the desired position of the visual feature ");
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
+ vpTRACE("define the task");
+ vpTRACE("\t we want an eye-in-hand control law");
+ vpTRACE("\t articular velocity are computed");
+ task.setServo(vpServo::EYEINHAND_L_cVe_eJe);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
- vpTRACE("Set the position of the camera in the end-effector frame ") ;
- vpHomogeneousMatrix cMe ;
+ vpTRACE("Set the position of the camera in the end-effector frame ");
+ vpHomogeneousMatrix cMe;
// robot.get_cMe(cMe) ;
- vpVelocityTwistMatrix cVe ;
- robot.get_cVe(cVe) ;
- std::cout << cVe <<std::endl ;
- task.set_cVe(cVe) ;
+ vpVelocityTwistMatrix cVe;
+ robot.get_cVe(cVe);
+ std::cout << cVe << std::endl;
+ task.set_cVe(cVe);
// vpDisplay::getClick(I) ;
- vpTRACE("Set the Jacobian (expressed in the end-effector frame)") ;
- vpMatrix eJe ;
- robot.get_eJe(eJe) ;
- task.set_eJe(eJe) ;
+ vpTRACE("Set the Jacobian (expressed in the end-effector frame)");
+ vpMatrix eJe;
+ robot.get_eJe(eJe);
+ task.set_eJe(eJe);
+ vpTRACE("\t we want to see a point on a point..");
+ std::cout << std::endl;
+ task.addFeature(p, pd);
- vpTRACE("\t we want to see a point on a point..") ;
- std::cout << std::endl ;
- task.addFeature(p,pd) ;
+ vpTRACE("\t set the gain");
+ task.setLambda(0.8);
- vpTRACE("\t set the gain") ;
- task.setLambda(0.8) ;
+ vpTRACE("Display task information ");
+ task.print();
- vpTRACE("Display task information " ) ;
- task.print() ;
-
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- for ( ; ; ) {
+ for (;;) {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
cog = dot.getCog();
// Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
-
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
// Get the jacobian of the robot
- robot.get_eJe(eJe) ;
- // Update this jacobian in the task structure. It will be used to compute
- // the velocity skew (as an articular velocity)
- // qdot = -lambda * L^+ * cVe * eJe * (s-s*)
- task.set_eJe(eJe) ;
+ robot.get_eJe(eJe);
+ // Update this jacobian in the task structure. It will be used to
+ // compute the velocity skew (as an articular velocity) qdot = -lambda *
+ // L^+ * cVe * eJe * (s-s*)
+ task.set_eJe(eJe);
// std::cout << (vpMatrix)cVe*eJe << std::endl ;
- vpColVector v ;
+ vpColVector v;
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed joint velocities to the robot
- robot.setVelocity(vpRobot::ARTICULAR_FRAME, v) ;
+ robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
// 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;
@@ -260,8 +253,7 @@ 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;
@@ -271,39 +263,36 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl; // s-s* for point
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl; // s-s* for point
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() << std::endl;
+ // std::cout << "|| s - s* || = " << ( task.getError() ).sumSquare() <<
+ // std::endl;
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
std::cout << "Display task information: " << std::endl;
- task.print() ;
+ task.print();
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
-
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have an afma6 robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
index da2b53a..26ae028 100644
--- a/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
+++ b/example/servo-viper850/servoViper850Point2DCamVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,50 +39,47 @@
*
*****************************************************************************/
-
/*!
\example servoViper850Point2DCamVelocity.cpp
Example of eye-in-hand control law. We control here a real robot, the
ADEPT Viper 850 robot (arm, with 6 degrees of freedom). The velocity is
- computed in the camera frame. The visual feature is the center of gravity of a
- point.
+ computed in the camera frame. The visual feature is the center of gravity of
+ a point.
*/
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
-
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -95,17 +93,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -117,11 +113,11 @@ main()
std::ofstream flog(logfilename.c_str());
try {
- vpRobotViper850 robot ;
+ vpRobotViper850 robot;
- vpServo task ;
+ vpServo task;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
@@ -133,108 +129,107 @@ main()
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0);
g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
#endif
- g.open(I) ;
+ 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) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpDot2 dot ;
+ vpDot2 dot;
vpImagePoint cog;
dot.setGraphics(true);
- for (int i=0; i< 10; i++)
- g.acquire(I) ;
+ for (int i = 0; i < 10; i++)
+ g.acquire(I);
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
+ vpFeaturePoint p;
// retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// - set the constant gain
- task.setLambda(0.8) ;
+ task.setLambda(0.8);
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
// Now the robot will be controlled in velocity
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- vpColVector v ;
- for ( ; ; ) {
+ vpColVector v;
+ for (;;) {
try {
// Acquire a new image from the camera
- g.acquire(I) ;
+ g.acquire(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the dot cog
cog = dot.getCog();
- // Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ // Display a green cross at the center of gravity position in the
+ // image
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
// Compute the visual servoing skew vector
- v = task.computeControlLaw() ;
+ v = task.computeControlLaw();
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
- }
- catch(...) {
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
+ } catch (...) {
std::cout << "Tracking failed... Stop the robot." << std::endl;
v = 0;
// Stop robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
return 0;
}
// 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;
@@ -244,8 +239,7 @@ 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;
@@ -255,40 +249,37 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl; // s-s* for point
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl; // s-s* for point
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
- vpERROR_TRACE(" Test failed") ;
+ } catch (...) {
+ flog.close(); // Close the log file
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
index cd8ebd4..0a421ff 100644
--- a/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
+++ b/example/servo-viper850/servoViper850Point2DCamVelocityKalman.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,14 +39,13 @@
*
*****************************************************************************/
-
/*!
\example servoViper850Point2DCamVelocityKalman.cpp
Example of eye-in-hand control law. We control here a real robot, the ADEPT
- Viper 850 robot (arm, with 6 degrees of freedom). The velocity is computed in
- the camera frame. The visual feature is the center of gravity of a point. We
- use here a linear Kalman filter with a constant velocity state model to
+ Viper 850 robot (arm, with 6 degrees of freedom). The velocity is computed
+ in the camera frame. The visual feature is the center of gravity of a point.
+ We use here a linear Kalman filter with a constant velocity state model to
estimate the moving target motion.
*/
@@ -53,37 +53,36 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h> // Debug trace
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#if (defined (VISP_HAVE_VIPER850) && defined (VISP_HAVE_DC1394))
+#if (defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394))
-#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpException.h>
-#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/blob/vpDot2.h>
+#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpAdaptiveGain.h>
-#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
-int
-main()
+int main()
{
// Log file creation in /tmp/$USERNAME/log.dat
// This file contains by line:
@@ -97,17 +96,15 @@ main()
// Create a log filename to save velocities...
std::string logdirname;
- logdirname ="/tmp/" + username;
+ logdirname = "/tmp/" + username;
// Test if the output path exist. If no try to create it
if (vpIoTools::checkDirectory(logdirname) == false) {
try {
// Create the dirname
vpIoTools::makeDirectory(logdirname);
- }
- catch (...) {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << logdirname << std::endl;
exit(-1);
}
@@ -118,12 +115,12 @@ main()
// Open the log file name
std::ofstream flog(logfilename.c_str());
- vpServo task ;
-
+ vpServo task;
+
try {
// Initialize linear Kalman filter
vpLinearKalmanFilterInstantiation kalman;
-
+
// Initialize the kalman filter
unsigned int nsignal = 2; // The two values of dedt
double rho = 0.3;
@@ -133,17 +130,16 @@ main()
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
+ 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
kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dummy);
// Initialize the robot
- vpRobotViper850 robot ;
-
+ vpRobotViper850 robot;
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
bool reset = false;
vp1394TwoGrabber g(reset);
@@ -155,91 +151,100 @@ main()
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_FORMAT7_0);
g.setColorCoding(vp1394TwoGrabber::vpCOLOR_CODING_MONO8);
#endif
- g.open(I) ;
+ g.open(I);
- double Tloop = 1./80.f;
+ double Tloop = 1. / 80.f;
vp1394TwoGrabber::vp1394TwoFramerateType fps;
g.getFramerate(fps);
- switch(fps) {
- case vp1394TwoGrabber::vpFRAMERATE_15 : Tloop = 1.f/15.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_30 : Tloop = 1.f/30.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_60 : Tloop = 1.f/60.f; break;
- case vp1394TwoGrabber::vpFRAMERATE_120: Tloop = 1.f/120.f; break;
- default: break;
+ switch (fps) {
+ case vp1394TwoGrabber::vpFRAMERATE_15:
+ Tloop = 1.f / 15.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_30:
+ Tloop = 1.f / 30.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_60:
+ Tloop = 1.f / 60.f;
+ break;
+ case vp1394TwoGrabber::vpFRAMERATE_120:
+ Tloop = 1.f / 120.f;
+ break;
+ default:
+ break;
}
#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) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpDot2 dot ;
+ vpDot2 dot;
vpImagePoint cog;
dot.setGraphics(true);
- for (int i=0; i< 10; i++)
- g.acquire(I) ;
+ for (int i = 0; i < 10; i++)
+ g.acquire(I);
std::cout << "Click on a dot..." << std::endl;
- dot.initTracking(I) ;
+ dot.initTracking(I);
cog = dot.getCog();
- vpDisplay::displayCross(I, cog, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::blue);
vpDisplay::flush(I);
- vpCameraParameters cam ;
+ vpCameraParameters cam;
// Update camera parameters
- robot.getCameraParameters (cam, I);
+ robot.getCameraParameters(cam, I);
// sets the current position of the visual feature
- vpFeaturePoint p ;
+ vpFeaturePoint p;
// retrieve x,y and Z of the vpPoint structure
- vpFeatureBuilder::create(p,cam, dot);
+ vpFeatureBuilder::create(p, cam, dot);
// sets the desired position of the visual feature
- vpFeaturePoint pd ;
- pd.buildFrom(0,0,1) ;
+ vpFeaturePoint pd;
+ pd.buildFrom(0, 0, 1);
// define the task
// - we want an eye-in-hand control law
// - robot is controlled in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA) ;
- task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE) ;
+ task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setInteractionMatrixType(vpServo::DESIRED, vpServo::PSEUDO_INVERSE);
// - we want to see a point on a point
- task.addFeature(p,pd) ;
+ task.addFeature(p, pd);
// - set the constant gain
- vpAdaptiveGain lambda;
+ vpAdaptiveGain lambda;
lambda.initStandard(4, 0.2, 30);
- task.setLambda(lambda) ;
+ task.setLambda(lambda);
- // Display task information
- task.print() ;
+ // Display task information
+ task.print();
// Now the robot will be controlled in velocity
- robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL) ;
+ robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
std::cout << "\nHit CTRL-C to stop the loop...\n" << std::flush;
- vpColVector v, v1, v2 ;
+ vpColVector v, v1, v2;
int iter = 0;
vpColVector vm(6);
double t_0, t_1, Tv;
vpColVector err(2), err_1(2);
- vpColVector dedt_filt(2), dedt_mes(2);
+ vpColVector dedt_filt(2), dedt_mes(2);
dc1394video_frame_t *frame = NULL;
-
+
t_1 = vpTime::measureTimeMs();
- for ( ; ; ) {
+ for (;;) {
try {
t_0 = vpTime::measureTimeMs(); // t_0: current time
@@ -255,34 +260,34 @@ main()
frame = g.dequeue(I);
// Display this image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
// Achieve the tracking of the dot in the image
- dot.track(I) ;
+ dot.track(I);
// Get the dot cog
cog = dot.getCog();
- // Display a green cross at the center of gravity position in the image
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ // Display a green cross at the center of gravity position in the
+ // image
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// Update the point feature from the dot location
vpFeatureBuilder::create(p, cam, dot);
// Compute the visual servoing skew vector
- v1 = task.computeControlLaw() ;
+ v1 = task.computeControlLaw();
// Get the error ||s-s*||
err = task.getError();
- //!terme correctif : de/dt = Delta s / Delta t - L*vc
- if (iter==0){
+ //! terme correctif : de/dt = Delta s / Delta t - L*vc
+ if (iter == 0) {
err_1 = 0;
dedt_mes = 0;
- }
- else{
+ } else {
vpMatrix J1 = task.getTaskJacobian();
- dedt_mes = (err - err_1)/(Tv) - J1 *vm;
+ dedt_mes = (err - err_1) / (Tv)-J1 * vm;
err_1 = err;
}
@@ -291,35 +296,34 @@ main()
dedt_mes = 0;
kalman.filter(dedt_mes);
// Get the filtered values
- for (unsigned int i=0; i < nsignal; i++) {
- dedt_filt[i] = kalman.Xest[i*state_size];
+ for (unsigned int i = 0; i < nsignal; i++) {
+ dedt_filt[i] = kalman.Xest[i * state_size];
}
if (iter < 2)
dedt_filt = 0;
vpMatrix J1p = task.getTaskJacobianPseudoInverse();
- v2 = - J1p*dedt_filt;
+ v2 = -J1p * dedt_filt;
// Update the robot camera velocity
v = v1 + v2;
// Display the current and desired feature points in the image display
- vpServoDisplay::display(task, cam, I) ;
+ vpServoDisplay::display(task, cam, I);
// Apply the computed camera velocities to the robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- iter ++;
+ iter++;
// Synchronize the loop with the image frame rate
- vpTime::wait(t_0, 1000.*Tloop);
+ vpTime::wait(t_0, 1000. * Tloop);
// Release the ring buffer used for the last image to start a new acq
g.enqueue(frame);
- }
- catch(...) {
+ } catch (...) {
std::cout << "Tracking failed... Stop the robot." << std::endl;
v = 0;
// Stop robot
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
// Kill the task
task.kill();
return 0;
@@ -328,8 +332,7 @@ 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;
@@ -339,8 +342,7 @@ 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;
@@ -350,42 +352,39 @@ 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 expressed
- // in meters in the camera frame
- flog << ( task.getError() ).t() << std::endl; // s-s* for point
+ // point, we have 2 errors (along x and y axis). This error is
+ // expressed in meters in the camera frame
+ flog << (task.getError()).t() << std::endl; // s-s* for point
// Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- flog.close() ; // Close the log file
+ flog.close(); // Close the log file
// Display task information
- task.print() ;
+ task.print();
// Kill the task
task.kill();
return 0;
- }
- catch (...)
- {
- flog.close() ; // Close the log file
+ } catch (...) {
+ flog.close(); // Close the log file
// Kill the task
task.kill();
- vpERROR_TRACE(" Test failed") ;
+ vpERROR_TRACE(" Test failed");
return 0;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber connected to your computer...");
+ vpERROR_TRACE("You do not have a Viper robot or a firewire framegrabber "
+ "connected to your computer...");
}
#endif
diff --git a/example/tools/CMakeLists.txt b/example/tools/CMakeLists.txt
index 20d8333..39ed786 100644
--- a/example/tools/CMakeLists.txt
+++ b/example/tools/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/tools/histogram.cpp b/example/tools/histogram.cpp
index 369ba35..a1d868c 100644
--- a/example/tools/histogram.cpp
+++ b/example/tools/histogram.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,26 +42,20 @@
\brief Histogram manipulation.
*/
-
-
-
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHistogram.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpHistogram.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <iomanip>
+#include <sstream>
#include <stdio.h>
#include <stdlib.h>
-#include <sstream>
-#include <iomanip>
// List of allowed command line options
-#define GETOPTARGS "i:o:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+#define GETOPTARGS "i:o:h"
/*!
\example histogram.cpp
@@ -96,7 +91,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -108,14 +103,12 @@ OPTIONS: Default\n\
\"histogram.txt\" is saved.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -130,19 +123,28 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -157,8 +159,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -169,14 +170,15 @@ main(int argc, const char ** argv)
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -188,7 +190,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -205,11 +207,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(dirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << dirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -220,8 +220,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -229,27 +228,26 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Load a grey image from the disk
filename = ipath;
if (opt_ipath.empty())
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
unsigned char distance = 60;
vpHistogram h;
@@ -258,14 +256,14 @@ main(int argc, const char ** argv)
h.calculate(I);
// Save the histogram
- filename = dirname + vpIoTools::path("/histogram.txt");
+ filename = dirname + vpIoTools::path("/histogram.txt");
std::cout << "Save the histogram in: " << filename << std::endl;
h.write(filename);
// Smooth the histogram
h.smooth();
// Save the histogram
- filename = dirname + vpIoTools::path("/histogram_smoothed.txt");
+ filename = dirname + vpIoTools::path("/histogram_smoothed.txt");
std::cout << "Save the smoothed histogram in: " << filename << std::endl;
h.write(filename);
@@ -278,8 +276,7 @@ main(int argc, const char ** argv)
vpTRACE("List of peaks");
vpTRACE("Nb peaks: %d", nbpeaks);
if (nbpeaks) {
- for(std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it)
- {
+ for (std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it) {
vpHistogramPeak p = *it;
vpTRACE("Peak: gray level: %d value: %d", p.getLevel(), p.getValue());
}
@@ -292,8 +289,7 @@ main(int argc, const char ** argv)
vpTRACE("Sorted list of peaks");
vpTRACE("Nb peaks: %d", nbpeaks);
if (nbpeaks) {
- for(std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it)
- {
+ for (std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it) {
vpHistogramPeak p = *it;
vpTRACE("Peak: gray level: %d value: %d", p.getLevel(), p.getValue());
}
@@ -304,10 +300,8 @@ main(int argc, const char ** argv)
nbpeaks = h.getPeaks(distance, peak1, peak2);
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(),
+ } else {
+ vpTRACE("Bimodal histogram: main peak1: %d-%d second peak2: %d-%d", peak1.getLevel(), peak1.getValue(),
peak2.getLevel(), peak2.getValue());
}
@@ -315,8 +309,7 @@ main(int argc, const char ** argv)
vpHistogramValey valey;
if (h.getValey(peak1, peak2, valey) == false) {
vpTRACE("No valey found...");
- }
- else {
+ } else {
vpTRACE("Valey: %d-%d", valey.getLevel(), valey.getValue());
}
@@ -326,21 +319,15 @@ 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());
- }
- else if (ret == 0x10) {
- vpTRACE("No 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("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue());
- }
- else if (ret == 0x01) {
- vpTRACE("No left valey for peak %d-%d...",
- peak1.getLevel(), peak1.getValue());
+ } else if (ret == 0x01) {
+ 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());
+ } else if (ret == 0x11) {
+ vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue());
vpTRACE("Right valey: %d-%d", valeyr.getLevel(), valeyr.getValue());
}
}
@@ -348,21 +335,15 @@ 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());
- }
- else if (ret == 0x10) {
- vpTRACE("No 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("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue());
- }
- else if (ret == 0x01) {
- vpTRACE("No left valey for peak %d-%d...",
- peak2.getLevel(), peak2.getValue());
+ } else if (ret == 0x01) {
+ 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());
+ } else if (ret == 0x11) {
+ vpTRACE("Left valey: %d-%d", valeyl.getLevel(), valeyl.getValue());
vpTRACE("Right valey: %d-%d", valeyr.getLevel(), valeyr.getValue());
}
}
@@ -373,16 +354,12 @@ main(int argc, const char ** argv)
vpHistogramPeak peakl, peakr;
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());
+ } 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());
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/example/tools/keyboard.cpp b/example/tools/keyboard.cpp
index 6386864..6c43657 100644
--- a/example/tools/keyboard.cpp
+++ b/example/tools/keyboard.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,34 +46,31 @@
#include <visp3/core/vpDebug.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-#include <stdio.h>
#include <iostream>
#include <signal.h>
+#include <stdio.h>
#include <visp3/io/vpKeyboard.h>
-
-int
-main()
+int main()
{
std::cout << "Push some characters on the keyboard..." << std::endl;
printf("Hit 'q' or 'Q' to stop the loop ...\n");
vpKeyboard keyboard;
std::cout << "Start the keyboard scrutation..." << std::endl;
- for ( ; ; ) {
+ for (;;) {
if (keyboard.kbhit()) {
- int c = keyboard.getchar () ;
+ int c = keyboard.getchar();
printf("You hit key: %d '%c'\n", c, c);
if (c == 'q' || c == 'Q') {
printf("You hit key: %d %c we stop the loop\n", c, c);
- break ;
+ break;
}
}
// My job is here
-
}
std::cout << "Enter an integer: ";
@@ -82,8 +80,7 @@ main()
return 0;
}
#else
-int
-main()
+int main()
{
vpTRACE("Sorry, for the moment, vpKeyboard class works only on unix...");
return 0;
diff --git a/example/tools/parallelPort.cpp b/example/tools/parallelPort.cpp
index 0439750..830db42 100644
--- a/example/tools/parallelPort.cpp
+++ b/example/tools/parallelPort.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,16 +46,16 @@
#include <visp3/core/vpDebug.h>
#if defined VISP_HAVE_PARPORT
-#include <stdlib.h>
-#include <stdio.h>
#include <iostream>
#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/io/vpParallelPort.h>
#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
-#define GETOPTARGS "d:h"
+#define GETOPTARGS "d:h"
/*!
@@ -81,14 +82,12 @@ OPTIONS: Default\n\
Value should be in [0:255].\n\
\n\
-h\n\
- Print the help.\n\n",
- data);
+ Print the help.\n\n", data);
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -105,7 +104,7 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, unsigned char &data)
{
const char *optarg;
- int c;
+ int c;
int value;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg)) > 1) {
@@ -116,19 +115,22 @@ 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;
+ } else {
+ data = (unsigned char)value;
}
break;
}
- case 'h': usage(argv[0], NULL, data); return false; break;
+ case 'h':
+ usage(argv[0], NULL, data);
+ return false;
+ break;
default:
- usage(argv[0], optarg, data); return false; break;
+ usage(argv[0], optarg, data);
+ return false;
+ break;
}
}
@@ -148,15 +150,14 @@ bool getOptions(int argc, const char **argv, unsigned char &data)
Send a data to the parallel port.
*/
-int
-main(int argc, const char **argv)
+int main(int argc, const char **argv)
{
// data to send to the parallel port
unsigned char data = 0;
// Read the command line options
if (getOptions(argc, argv, data) == false) {
- exit (-1);
+ exit(-1);
}
try {
@@ -165,10 +166,8 @@ main(int argc, const char **argv)
printf("Send data \"%d\" to the parallel port\n", data);
parport.sendData(data);
-
- }
- catch (vpParallelPortException &e) {
- switch(e.getCode()) {
+ } catch (vpParallelPortException &e) {
+ switch (e.getCode()) {
case vpParallelPortException::opening:
printf("Can't open the parallel port\n");
break;
@@ -176,15 +175,13 @@ main(int argc, const char **argv)
printf("Can't close the parallel port\n");
break;
}
- }
- catch(...) {
+ } catch (...) {
printf("An error occurs...\n");
}
return 0;
}
#else
-int
-main()
+int main()
{
vpTRACE("Sorry, for the moment, vpParallelPort class works only on unix...");
return 0;
diff --git a/example/tools/plot2d.cpp b/example/tools/plot2d.cpp
index 3188f8d..e569c22 100644
--- a/example/tools/plot2d.cpp
+++ b/example/tools/plot2d.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\example plot2d.cpp
@@ -45,10 +45,10 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpPlot.h>
-int main ()
+int main()
{
#if defined(VISP_HAVE_DISPLAY)
try {
@@ -57,66 +57,65 @@ int main ()
// Change the default font
// plot.setFont("-misc-fixed-bold-r-semicondensed--0-0-75-75-c-0-iso8859-10");
- //Initialize the number of curve for each graphic
- plot.initGraph(0,1);
- plot.initGraph(1,1);
+ // Initialize the number of curve for each graphic
+ plot.initGraph(0, 1);
+ plot.initGraph(1, 1);
- //Set the color of the curves
- plot.setColor(0,0,vpColor::green);
- plot.setColor(1,0,vpColor::red);
+ // Set the color of the curves
+ plot.setColor(0, 0, vpColor::green);
+ plot.setColor(1, 0, vpColor::red);
- //Set the titles of the graphic
+ // Set the titles of the graphic
char title[40];
- strncpy( title, "cos function", 40 );
- plot.setTitle(0,title);
- strncpy( title, "sin function", 40 );
+ strncpy(title, "cos function", 40);
+ plot.setTitle(0, title);
+ strncpy(title, "sin function", 40);
plot.setTitle(1, title);
- //Set the legend of each curves
+ // Set the legend of each curves
char legend[40];
- strncpy( legend, "cos x", 40 );
- plot.setLegend(0,0,legend);
- strncpy( legend, "sin x", 40 );
- plot.setLegend(1,0, legend);
+ strncpy(legend, "cos x", 40);
+ plot.setLegend(0, 0, legend);
+ strncpy(legend, "sin x", 40);
+ plot.setLegend(1, 0, legend);
- //Set the x axis legend of each curves
+ // Set the x axis legend of each curves
char unit[40];
- strncpy( unit, "x", 40 );
- plot.setUnitX(0,unit);
- strncpy( unit, "x", 40 );
- plot.setUnitX(1,unit);
-
- //Set the y axis legend of each curves
- strncpy( unit, "y", 40 );
- plot.setUnitY(0,unit);
- strncpy( unit, "y", 40 );
- plot.setUnitY(1,unit);
-
- //Plot the cosinus and sinus functions
+ strncpy(unit, "x", 40);
+ plot.setUnitX(0, unit);
+ strncpy(unit, "x", 40);
+ plot.setUnitX(1, unit);
+
+ // Set the y axis legend of each curves
+ strncpy(unit, "y", 40);
+ plot.setUnitY(0, unit);
+ strncpy(unit, "y", 40);
+ plot.setUnitY(1, unit);
+
+ // Plot the cosinus and sinus functions
double i = 0;
- while(i <= 20*2*M_PI)
- {
+ while (i <= 20 * 2 * M_PI) {
double co = cos(i);
double si = sin(i);
- plot.plot(0,0,i,co);
- plot.plot(1,0,i,si);
- i+=0.1;
+ plot.plot(0, 0, i, co);
+ plot.plot(1, 0, i, si);
+ i += 0.1;
}
vpDisplay::getClick(plot.I);
- //Save the datas as text files
+ // Save the datas as text files
plot.saveData(0, "dataCos.txt", "# ");
plot.saveData(1, "dataSin.txt", "# ");
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#else
- std::cout << "Plot functionalities are not avalaible since no display is available." << std::endl;
+ std::cout << "Plot functionalities are not avalaible since no display is "
+ "available."
+ << std::endl;
#endif
-
}
diff --git a/example/tools/plot3d.cpp b/example/tools/plot3d.cpp
old mode 100755
new mode 100644
index 9083b28..ed89004
--- a/example/tools/plot3d.cpp
+++ b/example/tools/plot3d.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\example plot3d.cpp
@@ -46,32 +46,32 @@
#include <visp3/core/vpConfig.h>
#include <visp3/gui/vpPlot.h>
-int main ()
+int main()
{
#if defined(VISP_HAVE_DISPLAY)
try {
- //Create a window with one graphic
+ // Create a window with one graphic
vpPlot plot(1);
// Change the default font
- //plot.setFont("-misc-fixed-bold-r-semicondensed--0-0-75-75-c-0-iso8859-10");
+ // plot.setFont("-misc-fixed-bold-r-semicondensed--0-0-75-75-c-0-iso8859-10");
- //The graphic contains 2 curves
- plot.initGraph(0,2);
+ // The graphic contains 2 curves
+ plot.initGraph(0, 2);
- //Set the graphic parameters
+ // Set the graphic parameters
plot.setTitle(0, "First graphic");
plot.setUnitX(0, "time (s)");
plot.setUnitY(0, "y");
plot.setUnitZ(0, "z");
- plot.setLegend(0,0, "y^2+z^2=1 and y(0) = 1");
- plot.setLegend(0,1, "y^2+z^2=1 and y(0) = -1");
- plot.setColor(0,0,vpColor::red);
- plot.setColor(0,1,vpColor::green);
+ plot.setLegend(0, 0, "y^2+z^2=1 and y(0) = 1");
+ plot.setLegend(0, 1, "y^2+z^2=1 and y(0) = -1");
+ plot.setColor(0, 0, vpColor::red);
+ plot.setColor(0, 1, vpColor::green);
double x = 0;
double y = 1;
- double z = 0 ;
+ double z = 0;
double dx = 0.08;
double dy = 0.04;
double zsign = 1.0;
@@ -80,44 +80,45 @@ int main ()
std::cout << "Hit CTRL-C to or right mouse button to exit..." << std::endl;
bool end = false;
- while( !end ) {
+ while (!end) {
if (iter < 300) {
- //y*y+z*z = 1
+ // y*y+z*z = 1
if (fabs(y) < 1.0)
- z = sqrt(1.0-y*y);
- else z = 0;
+ z = sqrt(1.0 - y * y);
+ else
+ z = 0;
- //Add points to the graphic
- if (plot.plot(0,0, x, y, z*zsign) == vpMouseButton::button3)
+ // Add points to the graphic
+ if (plot.plot(0, 0, x, y, z * zsign) == vpMouseButton::button3)
end = true;
- if (plot.plot(0,1, x, -y, -z*zsign) == vpMouseButton::button3)
+ if (plot.plot(0, 1, x, -y, -z * zsign) == vpMouseButton::button3)
end = true;
x += dx;
- if (fabs(y) >= 1.0 )
+ if (fabs(y) >= 1.0)
dy = -dy;
y += dy;
- if (fabs(y) >= 1.0 )
+ if (fabs(y) >= 1.0)
zsign = -zsign;
- }
- else {
+ } else {
// Tip: to allows modifying the point of view with the mouse we
// plot always the last point
- if (plot.plot(0,0, x, y,z*zsign) == vpMouseButton::button3)
+ if (plot.plot(0, 0, x, y, z * zsign) == vpMouseButton::button3)
end = true;
- if (plot.plot(0,1, x, -y,-z*zsign) == vpMouseButton::button3)
+ if (plot.plot(0, 1, x, -y, -z * zsign) == vpMouseButton::button3)
end = true;
}
- iter ++;
+ iter++;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
#else
- std::cout << "Plot functionalities are not avalaible since no display is available." << std::endl;
+ std::cout << "Plot functionalities are not avalaible since no display is "
+ "available."
+ << std::endl;
#endif
}
diff --git a/example/tracking/CMakeLists.txt b/example/tracking/CMakeLists.txt
index af2780c..7394814 100644
--- a/example/tracking/CMakeLists.txt
+++ b/example/tracking/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -28,7 +29,7 @@
# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
#
# Description:
-# ViSP overall configuration file.
+# ViSP overall configuration file.
#
# Authors:
# Fabien Spindler
@@ -46,6 +47,9 @@ set(example_cpp
mbtEdgeKltMultiTracking.cpp
mbtEdgeMultiTracking.cpp
mbtEdgeTracking.cpp
+ mbtGenericTracking.cpp
+ mbtGenericTracking2.cpp
+ mbtGenericTrackingDepth.cpp
mbtKltTracking.cpp
mbtKltMultiTracking.cpp
templateTracker.cpp
@@ -60,12 +64,15 @@ set(example_cpp
)
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(mbtEdgeKltTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(mbtEdgeKltMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(mbtEdgeMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(mbtEdgeTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(mbtKltTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(mbtKltMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(mbtEdgeKltTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtEdgeKltMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtEdgeMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtEdgeTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtKltTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtKltMultiTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtGenericTracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtGenericTracking2.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(mbtGenericTrackingDepth.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
foreach(cpp ${example_cpp})
@@ -87,17 +94,41 @@ add_test(mbtKltTracking-cao mbtKltTracking -c -f ${OPTION_TO_DESACTIVE_DISPL
add_test(mbtEdgeTracking-cao-nocyl mbtEdgeTracking -c -f -C ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(mbtEdgeKltTracking-cao-nocyl mbtEdgeKltTracking -c -f -C ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtEdgeTracking-cao-scanline mbtEdgeTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtEdgeKltTracking-cao-scanline mbtEdgeKltTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtKltTracking-cao-scanline mbtKltTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtEdgeTracking-cao-scanline mbtEdgeTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+add_test(mbtEdgeKltTracking-cao-scanline mbtEdgeKltTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+add_test(mbtKltTracking-cao-scanline mbtKltTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
add_test(mbtEdgeMultiTracking-cao mbtEdgeMultiTracking -c -f ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(mbtEdgeKltMultiTracking-cao mbtEdgeKltMultiTracking -c -f ${OPTION_TO_DESACTIVE_DISPLAY})
add_test(mbtKltMultiTracking-cao mbtKltMultiTracking -c -f ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtEdgeMultiTracking-cao-scanline mbtEdgeMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtEdgeKltMultiTracking-cao-scanline mbtEdgeKltMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
-add_test(mbtKltMultiTracking-cao-scanline mbtKltMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtEdgeMultiTracking-cao-scanline mbtEdgeMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+add_test(mbtEdgeKltMultiTracking-cao-scanline mbtEdgeKltMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+add_test(mbtKltMultiTracking-cao-scanline mbtKltMultiTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+
+add_test(mbtGenericTracking mbtGenericTracking -c ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-cao mbtGenericTracking -c -f ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-cao-nocyl mbtGenericTracking -c -f -C ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-cao-scanline mbtGenericTracking -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+
+add_test(mbtGenericTracking-map mbtGenericTracking2 -c ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-map-cao mbtGenericTracking2 -c -f ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-map-cao-nocyl mbtGenericTracking2 -c -f -C ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTracking-map-cao-scanline mbtGenericTracking2 -c -f -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 20)
+
+add_test(mbtGenericTrackingDepth-edge-dense mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY})
+add_test(mbtGenericTrackingDepth-edge-dense-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -e 14)
+add_test(mbtGenericTrackingDepth-klt-dense mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY} -t 2)
+add_test(mbtGenericTrackingDepth-klt-dense-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -t 2 -e 14)
+add_test(mbtGenericTrackingDepth-hybrid-dense mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY} -t 3)
+add_test(mbtGenericTrackingDepth-hybrid-dense-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -t 3 -e 14)
+
+add_test(mbtGenericTrackingDepth-edge-normal mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY} -T 4)
+add_test(mbtGenericTrackingDepth-edge-normal-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -T 4 -e 14)
+add_test(mbtGenericTrackingDepth-klt-normal mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY} -T 4 -t 2)
+add_test(mbtGenericTrackingDepth-klt-normal-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -T 4 -t 2 -e 14)
+add_test(mbtGenericTrackingDepth-hybrid-normal mbtGenericTrackingDepth -c ${OPTION_TO_DESACTIVE_DISPLAY} -T 4 -t 3)
+add_test(mbtGenericTrackingDepth-hybrid-normal-scanline mbtGenericTrackingDepth -c -l ${OPTION_TO_DESACTIVE_DISPLAY} -T 4 -t 3 -e 14)
#disable test with Ogre otherwise we get an Ogre exception with Ogre and remote access:
#terminate called after throwing an instance of 'Ogre::RenderingAPIException'
diff --git a/example/tracking/mbtEdgeKltMultiTracking.cpp b/example/tracking/mbtEdgeKltMultiTracking.cpp
index c02acf8..ee8fe68 100644
--- a/example/tracking/mbtEdgeKltMultiTracking.cpp
+++ b/example/tracking/mbtEdgeKltMultiTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,30 +40,31 @@
/*!
\example mbtEdgeKltMultiTracking.cpp
- \brief Example of Hybrid Tracking of MBT and MBT KTL on an image sequence containing a cube.
+ \brief Example of Hybrid Tracking of MBT and MBT KTL on an image sequence
+ containing a cube.
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
-#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 <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfColwvp"
-
+#define GETOPTARGS "x:m:i:n:de:chtfColwvp"
void usage(const char *name, const char *badparam)
{
@@ -71,16 +73,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -95,6 +96,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -109,7 +113,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optional picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the moving edges and Klt points. \n\
@@ -133,7 +137,7 @@ OPTIONS: \n\
-v\n\
Compute covariance matrix.\n\
\n\
- -v\n\
+ -p\n\
Compute gradient projection error.\n\
\n\
-h \n\
@@ -143,36 +147,70 @@ 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, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError)
+ 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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayFeatures = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'C': trackCylinder = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'p': projectionError = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -187,8 +225,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -200,6 +237,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayFeatures = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -212,27 +250,26 @@ main(int argc, const char ** argv)
bool projectionError = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -242,35 +279,34 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
+ } else {
std::string modelFileCao;
std::string modelFileWrl;
- if(trackCylinder){
- modelFileCao = "ViSP-images/mbt/cube_and_cylinder.cao";
- modelFileWrl = "ViSP-images/mbt/cube_and_cylinder.wrl";
- }else{
- modelFileCao = "ViSP-images/mbt/cube.cao";
- modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
}
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -278,12 +314,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -297,26 +331,29 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I1, I2;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I1);
I2 = I1;
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I1);
I2 = I1;
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display1, display2;
#elif defined VISP_HAVE_GDI
@@ -330,13 +367,12 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
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);
@@ -348,8 +384,8 @@ main(int argc, const char ** argv)
vpHomogeneousMatrix c1Mo, c2Mo;
vpCameraParameters cam1, cam2;
- // Initialise the tracker: camera parameters, moving edge and KLT settings
-#if defined (VISP_HAVE_XML2)
+// Initialise the tracker: camera parameters, moving edge and KLT settings
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile, configFile);
#else
@@ -378,15 +414,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam1, cam2);
tracker.setMovingEdge(me);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// Specify the clipping to
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
// Display the moving edges, and the Klt points
@@ -410,12 +448,11 @@ main(int argc, const char ** argv)
tracker.getCameraParameters(cam1, cam2);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I1,false)){
+ 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::flush(I1) ;
+ vpDisplay::flush(I1);
}
}
@@ -424,23 +461,22 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I1, I2, initFile, initFile, true);
tracker.getPose(c1Mo, c2Mo);
// display the 3D model at the given pose
tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red);
- }
- else
- {
+ } 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);
tracker.initFromPose(I1, I2, c1Moi, c2Moi);
}
- //track the model
+ // track the model
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
@@ -449,8 +485,7 @@ main(int argc, const char ** argv)
vpDisplay::flush(I2);
}
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I1);
I2 = I1;
@@ -468,7 +503,7 @@ main(int argc, const char ** argv)
vpDisplay::display(I2);
}
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile, configFile);
#else
// By setting the parameters:
@@ -496,15 +531,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam1, cam2);
tracker.setMovingEdge(me);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// Specify the clipping to
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam1, cam2);
@@ -517,22 +554,23 @@ 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);
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
if (opt_display) {
// display the 3D model
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);
+ vpDisplay::displayFrame(I1, c1Mo, cam1, 0.05);
+ vpDisplay::displayFrame(I2, c2Mo, cam2, 0.05);
}
}
@@ -544,37 +582,40 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
}
- if(projectionError) {
+ if (projectionError) {
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;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I1);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbEdgeKltTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#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)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbEdgeKltTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -584,7 +625,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
return 0;
}
diff --git a/example/tracking/mbtEdgeKltTracking.cpp b/example/tracking/mbtEdgeKltTracking.cpp
index ef12794..15fb96d 100644
--- a/example/tracking/mbtEdgeKltTracking.cpp
+++ b/example/tracking/mbtEdgeKltTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,35 +39,31 @@
/*!
\example mbtEdgeKltTracking.cpp
- \brief Example of Hybrid Tracking of MBT and MBT KTL on an image sequence containing a cube.
+ \brief Example of Hybrid Tracking of MBT and MBT KTL on an image sequence
+ containing a cube.
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
-#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 <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbEdgeKltTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfColwvp"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
- std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError);
+#define GETOPTARGS "x:m:i:n:de:chtfColwvp"
void usage(const char *name, const char *badparam)
{
@@ -75,16 +72,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -99,6 +95,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -113,7 +112,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optionnal picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the moving edges and Klt points. \n\
@@ -137,7 +136,7 @@ OPTIONS: \n\
-v\n\
Compute covariance matrix.\n\
\n\
- -v\n\
+ -p\n\
Compute gradient projection error.\n\
\n\
-h \n\
@@ -147,36 +146,70 @@ 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, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError)
+ 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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayFeatures = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'C': trackCylinder = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'p': projectionError = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -191,8 +224,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -204,6 +236,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayFeatures = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -216,27 +249,26 @@ main(int argc, const char ** argv)
bool projectionError = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -246,35 +278,34 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
+ } else {
std::string modelFileCao;
std::string modelFileWrl;
- if(trackCylinder){
- modelFileCao = "ViSP-images/mbt/cube_and_cylinder.cao";
- modelFileWrl = "ViSP-images/mbt/cube_and_cylinder.wrl";
- }else{
- modelFileCao = "ViSP-images/mbt/cube.cao";
- modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
}
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -282,12 +313,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -301,24 +330,27 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I);
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I);
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GDI
@@ -332,12 +364,11 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
- display.init(I, 100, 100, "Test tracking") ;
+ display.init(I, 100, 100, "Test tracking");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
@@ -345,8 +376,8 @@ main(int argc, const char ** argv)
vpHomogeneousMatrix cMo;
vpCameraParameters cam;
- // Initialise the tracker: camera parameters, moving edge and KLT settings
-#if defined (VISP_HAVE_XML2)
+// Initialise the tracker: camera parameters, moving edge and KLT settings
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile);
#else
@@ -374,15 +405,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam);
tracker.setMovingEdge(me);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// Specify the clipping to
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
// Display the moving edges, and the Klt points
@@ -406,9 +439,8 @@ main(int argc, const char ** argv)
tracker.getCameraParameters(cam);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I,false)){
+ 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::flush(I);
@@ -421,30 +453,28 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I, initFile, true);
tracker.getPose(cMo);
// display the 3D model at the given pose
- tracker.display(I,cMo, cam, vpColor::red);
- }
- else
- {
+ tracker.display(I, cMo, cam, vpColor::red);
+ } else {
vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
tracker.initFromPose(I, cMoi);
}
- //track the model
+ // track the model
tracker.track(I);
tracker.getPose(cMo);
if (opt_display)
vpDisplay::flush(I);
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I);
// display the image
@@ -457,7 +487,7 @@ main(int argc, const char ** argv)
if (opt_display)
vpDisplay::display(I);
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile);
#else
// By setting the parameters:
@@ -484,15 +514,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam);
tracker.setMovingEdge(me);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// Specify the clipping to
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam);
@@ -505,30 +537,31 @@ 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) {
-// // display the 3D model
-// tracker.display(I, cMo, cam, vpColor::darkRed);
-// // display the frame
-// vpDisplay::displayFrame (I, cMo, cam, 0.05);
-//// if (opt_click_allowed) {
-//// vpDisplay::flush(I);
-//// vpDisplay::getClick(I);
-//// }
-// }
+ // if (opt_display) {
+ // // display the 3D model
+ // tracker.display(I, cMo, cam, vpColor::darkRed);
+ // // display the frame
+ // vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ //// if (opt_click_allowed) {
+ //// vpDisplay::flush(I);
+ //// vpDisplay::getClick(I);
+ //// }
+ // }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
// display the 3D model
tracker.display(I, cMo, cam, vpColor::darkRed);
// display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ vpDisplay::displayFrame(I, cMo, cam, 0.05);
}
}
@@ -540,36 +573,39 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
}
- if(projectionError) {
+ if (projectionError) {
std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl;
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
+
+ std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbEdgeKltTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
- // Cleanup memory allocated by Coin library used to load a vrml model in vpMbEdgeKltTracker::loadModel()
- // We clean only if Coin was used.
- if(! cao3DModel)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbEdgeKltTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -579,7 +615,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
return 0;
}
diff --git a/example/tracking/mbtEdgeMultiTracking.cpp b/example/tracking/mbtEdgeMultiTracking.cpp
index 197a89d..a79556a 100644
--- a/example/tracking/mbtEdgeMultiTracking.cpp
+++ b/example/tracking/mbtEdgeMultiTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,8 @@
/*!
\example mbtEdgeMultiTracking.cpp
- \brief Example of model based tracking on an image sequence containing a cube.
+ \brief Example of model based tracking on an image sequence containing a
+ cube.
*/
#include <iostream>
@@ -50,21 +52,20 @@
#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbEdgeMultiTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfColwvp"
-
+#define GETOPTARGS "x:m:i:n:de:chtfColwvp"
void usage(const char *name, const char *badparam)
{
@@ -73,16 +74,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -97,6 +97,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -111,7 +114,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optional picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the moving edges. \n\
@@ -135,7 +138,7 @@ OPTIONS: \n\
-v\n\
Compute covariance matrix.\n\
\n\
- -v\n\
+ -p\n\
Compute gradient projection error.\n\
\n\
-h \n\
@@ -145,36 +148,70 @@ 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, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError)
+ 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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayFeatures = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'C': trackCylinder = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'p': projectionError = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -189,8 +226,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -202,6 +238,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayFeatures = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -214,28 +251,26 @@ main(int argc, const char ** argv)
bool projectionError = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -245,35 +280,34 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
+ } else {
std::string modelFileCao;
std::string modelFileWrl;
- if(trackCylinder){
- modelFileCao = "ViSP-images/mbt/cube_and_cylinder.cao";
- modelFileWrl = "ViSP-images/mbt/cube_and_cylinder.wrl";
- }else{
- modelFileCao = "ViSP-images/mbt/cube.cao";
- modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
}
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -281,12 +315,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -300,26 +332,29 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I1, I2;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I1);
I2 = I1;
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I1);
I2 = I1;
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display1, display2;
#elif defined VISP_HAVE_GDI
@@ -333,13 +368,12 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
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);
@@ -349,10 +383,10 @@ main(int argc, const char ** argv)
vpMbEdgeMultiTracker tracker(2);
vpHomogeneousMatrix c1Mo, c2Mo;
-
+
// Initialise the tracker: camera parameters, moving edge and KLT settings
vpCameraParameters cam1, cam2;
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile, configFile);
#else
@@ -376,10 +410,13 @@ main(int argc, const char ** argv)
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
- // Display the moving edges, see documentation for the signification of the colours
+ // Display the moving edges, see documentation for the signification of
+ // the colours
tracker.setDisplayFeatures(displayFeatures);
// Tells if the tracker has to use Ogre3D for visibility tests
@@ -406,12 +443,11 @@ main(int argc, const char ** argv)
tracker.setCameraTransformationMatrix(mapOfCamTrans);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I1,false)){
+ 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::flush(I1) ;
+ vpDisplay::flush(I1);
vpTime::wait(100);
}
}
@@ -421,23 +457,22 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I1, I2, initFile, initFile, true);
tracker.getPose(c1Mo, c2Mo);
// display the 3D model at the given pose
tracker.display(I1, I2, c1Mo, c2Mo, cam1, cam2, vpColor::red);
- }
- else
- {
+ } 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);
tracker.initFromPose(I1, I2, c1Moi, c2Moi);
}
- //track the model
+ // track the model
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
@@ -446,8 +481,7 @@ main(int argc, const char ** argv)
vpDisplay::flush(I2);
}
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I1);
I2 = I1;
@@ -466,7 +500,7 @@ main(int argc, const char ** argv)
vpDisplay::display(I2);
}
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile, configFile);
#else
// By setting the parameters:
@@ -489,7 +523,9 @@ main(int argc, const char ** argv)
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam1, cam2);
@@ -502,14 +538,15 @@ 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);
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
if (opt_display) {
@@ -529,11 +566,11 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
}
- if(projectionError) {
+ if (projectionError) {
std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl;
}
@@ -541,26 +578,28 @@ main(int argc, const char ** argv)
vpDisplay::flush(I2);
}
+ std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I1);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbEdgeTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#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)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbEdgeTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
-
+
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -575,4 +614,3 @@ int main()
}
#endif
-
diff --git a/example/tracking/mbtEdgeTracking.cpp b/example/tracking/mbtEdgeTracking.cpp
index ccdced8..299a2e4 100644
--- a/example/tracking/mbtEdgeTracking.cpp
+++ b/example/tracking/mbtEdgeTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,8 @@
/*!
\example mbtEdgeTracking.cpp
- \brief Example of model based tracking on an image sequence containing a cube.
+ \brief Example of model based tracking on an image sequence containing a
+ cube.
*/
#include <iostream>
@@ -49,26 +51,20 @@
#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfColwvp"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
- std::string &initFile, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError);
+#define GETOPTARGS "x:m:i:n:de:chtfColwvp"
void usage(const char *name, const char *badparam)
{
@@ -77,16 +73,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -101,6 +96,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -115,7 +113,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optionnal picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the moving edges. \n\
@@ -139,7 +137,7 @@ OPTIONS: \n\
-v\n\
Compute covariance matrix.\n\
\n\
- -v\n\
+ -p\n\
Compute gradient projection error.\n\
\n\
-h \n\
@@ -149,36 +147,70 @@ 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, bool &displayFeatures, bool &click_allowed, bool &display,
- bool& cao3DModel, bool& trackCylinder, bool &useOgre, bool &showOgreConfigDialog,
- bool &useScanline, bool &computeCovariance, bool &projectionError)
+ 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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayFeatures = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'C': trackCylinder = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'p': projectionError = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -193,8 +225,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -206,6 +237,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayFeatures = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -218,28 +250,26 @@ main(int argc, const char ** argv)
bool projectionError = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayFeatures,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -249,35 +279,34 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
+ } else {
std::string modelFileCao;
std::string modelFileWrl;
- if(trackCylinder){
- modelFileCao = "ViSP-images/mbt/cube_and_cylinder.cao";
- modelFileWrl = "ViSP-images/mbt/cube_and_cylinder.wrl";
- }else{
- modelFileCao = "ViSP-images/mbt/cube.cao";
- modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
}
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -285,12 +314,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -304,24 +331,27 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I);
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I);
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GDI
@@ -335,21 +365,20 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
- display.init(I, 100, 100, "Test tracking") ;
+ display.init(I, 100, 100, "Test tracking");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
vpMbEdgeTracker tracker;
vpHomogeneousMatrix cMo;
-
+
// Initialise the tracker: camera parameters, moving edge and KLT settings
vpCameraParameters cam;
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile);
#else
@@ -372,10 +401,13 @@ main(int argc, const char ** argv)
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
- // Display the moving edges, see documentation for the signification of the colours
+ // Display the moving edges, see documentation for the signification of
+ // the colours
tracker.setDisplayFeatures(displayFeatures);
// Tells if the tracker has to use Ogre3D for visibility tests
@@ -396,12 +428,11 @@ main(int argc, const char ** argv)
tracker.getCameraParameters(cam);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I,false)){
+ 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::flush(I) ;
+ vpDisplay::flush(I);
}
}
@@ -410,30 +441,28 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I, initFile, true);
tracker.getPose(cMo);
// display the 3D model at the given pose
- tracker.display(I,cMo, cam, vpColor::red);
- }
- else
- {
+ tracker.display(I, cMo, cam, vpColor::red);
+ } else {
vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
tracker.initFromPose(I, cMoi);
}
- //track the model
+ // track the model
tracker.track(I);
tracker.getPose(cMo);
if (opt_display)
vpDisplay::flush(I);
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I);
// display the image
@@ -446,7 +475,7 @@ main(int argc, const char ** argv)
if (opt_display)
vpDisplay::display(I);
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile);
#else
// By setting the parameters:
@@ -468,7 +497,9 @@ main(int argc, const char ** argv)
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(0.90);
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
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
tracker.loadModel(modelFile);
tracker.setCameraParameters(cam);
@@ -481,30 +512,31 @@ 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) {
-// // display the 3D model
-// tracker.display(I, cMo, cam, vpColor::darkRed);
-// // display the frame
-// vpDisplay::displayFrame (I, cMo, cam, 0.05);
-//// if (opt_click_allowed) {
-//// vpDisplay::flush(I);
-//// vpDisplay::getClick(I);
-//// }
-// }
+ // if (opt_display) {
+ // // display the 3D model
+ // tracker.display(I, cMo, cam, vpColor::darkRed);
+ // // display the frame
+ // vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ //// if (opt_click_allowed) {
+ //// vpDisplay::flush(I);
+ //// vpDisplay::getClick(I);
+ //// }
+ // }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
// display the 3D model
tracker.display(I, cMo, cam, vpColor::darkRed);
// display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ vpDisplay::displayFrame(I, cMo, cam, 0.05);
}
}
@@ -516,37 +548,39 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
}
- if(projectionError) {
+ if (projectionError) {
std::cout << "Projection error: " << tracker.getProjectionError() << std::endl << std::endl;
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
+ std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbEdgeTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
- // Cleanup memory allocated by Coin library used to load a vrml model in vpMbEdgeTracker::loadModel()
- // We clean only if Coin was used.
- if(! cao3DModel)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbEdgeTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
-
+
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -561,4 +595,3 @@ int main()
}
#endif
-
diff --git a/example/tracking/mbtGenericTracking.cpp b/example/tracking/mbtGenericTracking.cpp
new file mode 100644
index 0000000..cdc9681
--- /dev/null
+++ b/example/tracking/mbtGenericTracking.cpp
@@ -0,0 +1,672 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Example of Hybrid Tracking of MBT and MBT KTL.
+ *
+ * Authors:
+ * Aurelien Yol
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \example mbtGenericTracking.cpp
+
+ \brief Example of tracking with vpGenericTracker on an image sequence
+ containing a cube.
+*/
+
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbGenericTracker.h>
+
+#define GETOPTARGS "x:m:i:n:de:chtfColwvpT:"
+
+#define USE_XML 0
+
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Example of tracking based on the 3D model.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <test image path>] [-x <config file>]\n\
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n\
+ [-T <tracker type>]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+ -i <input image path> \n\
+ Set image input path.\n\
+ From this path read images \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
+ images come from ViSP-images-x.y.z.tar.gz available \n\
+ on the ViSP website.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behavior than using\n\
+ this option.\n\
+\n\
+ -x <config file> \n\
+ Set the config file (the xml file) to use.\n\
+ The config file is used to specify the parameters of the tracker.\n\
+\n\
+ -m <model name> \n\
+ Specify the name of the file of the model\n\
+ The model can either be a vrml model (.wrl) or a .cao file.\n\
+\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
+ -f \n\
+ Do not use the vrml model, use the .cao one. These two models are \n\
+ equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
+ website. However, the .cao model allows to use the 3d model based tracker \n\
+ without Coin.\n\
+\n\
+ -C \n\
+ Track only the cube (not the cylinder). In this case the models files are\n\
+ cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
+ cube_and_cylinder.wrl.\n\
+\n\
+ -n <initialisation file base name> \n\
+ Base name of the initialisation file. The file will be 'base_name'.init .\n\
+ This base name is also used for the optional picture specifying where to \n\
+ click (a .ppm picture).\n\
+\n\
+ -t \n\
+ Turn off the display of the the moving edges and Klt points. \n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+\n\
+ -o\n\
+ Use Ogre3D for visibility tests\n\
+\n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
+\n\
+ -l\n\
+ Use the scanline for visibility tests.\n\
+\n\
+ -v\n\
+ Compute covariance matrix.\n\
+\n\
+ -p\n\
+ Compute gradient projection error.\n\
+\n\
+ -T <tracker type>\n\
+ Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (EdgeKlt)>).\n\
+\n\
+ -h \n\
+ Print the help.\n\n");
+
+ if (badparam)
+ 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)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'T':
+ trackerType = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string opt_configFile;
+ std::string opt_modelFile;
+ std::string modelFile;
+ std::string opt_initFile;
+ std::string initFile;
+ long opt_lastFrame = -1;
+ bool displayFeatures = true;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+ bool cao3DModel = false;
+ bool trackCylinder = true;
+ bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
+ bool computeCovariance = false;
+ bool projectionError = false;
+ int trackerType = vpMbGenericTracker::EDGE_TRACKER;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures,
+ opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
+ useScanline, computeCovariance, projectionError, trackerType)) {
+ return EXIT_FAILURE;
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+
+ return EXIT_FAILURE;
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
+ else
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
+
+#if defined(VISP_HAVE_XML2) && USE_XML
+ std::string configFile;
+ if (!opt_configFile.empty())
+ configFile = opt_configFile;
+ else if (!opt_ipath.empty())
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
+ else
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
+#endif
+
+ if (!opt_modelFile.empty()) {
+ modelFile = opt_modelFile;
+ } else {
+ std::string modelFileCao;
+ std::string modelFileWrl;
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
+ }
+
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
+ modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
+ } else {
+#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;
+ modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
+#endif
+ }
+ } else {
+ if (cao3DModel) {
+ modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
+ } else {
+#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;
+ modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
+#endif
+ }
+ }
+ }
+
+ if (!opt_initFile.empty())
+ initFile = opt_initFile;
+ else if (!opt_ipath.empty())
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
+ else
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
+
+ vpImage<unsigned char> I1, I2;
+ vpVideoReader reader;
+
+ reader.setFileName(ipath);
+ try {
+ reader.open(I1);
+ I2 = I1;
+ } catch (...) {
+ std::cerr << "Cannot open sequence: " << ipath << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
+ reader.acquire(I1);
+ I2 = I1;
+
+// initialise a display
+#if defined VISP_HAVE_X11
+ vpDisplayX display1, display2;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display1, display2;
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display1, display2;
+#elif defined VISP_HAVE_D3D9
+ vpDisplayD3D display1, display2;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display1, display2;
+#else
+ opt_display = false;
+#endif
+ if (opt_display) {
+#if (defined VISP_HAVE_DISPLAY)
+ 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)");
+#endif
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ }
+
+ vpMbTracker *tracker = new vpMbGenericTracker(2, trackerType);
+ vpHomogeneousMatrix c1Mo, c2Mo;
+ vpCameraParameters cam1, cam2;
+
+// Initialise the tracker: camera parameters, moving edge and KLT settings
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // From the xml file
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile, configFile);
+#else
+ // By setting the parameters:
+ cam1.initPersProjWithoutDistortion(547, 542, 338, 234);
+ cam2.initPersProjWithoutDistortion(547, 542, 338, 234);
+
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(7);
+ me.setThreshold(5000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ vpKltOpencv klt;
+ klt.setMaxFeatures(10000);
+ klt.setWindowSize(5);
+ klt.setQuality(0.01);
+ klt.setMinDistance(5);
+ klt.setHarrisFreeParameter(0.01);
+ klt.setBlockSize(3);
+ klt.setPyramidLevels(3);
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(klt);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
+#endif
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(me);
+
+ tracker->setAngleAppear(vpMath::rad(65));
+ tracker->setAngleDisappear(vpMath::rad(75));
+
+ // Specify the clipping to
+ tracker->setNearClippingDistance(0.01);
+ tracker->setFarClippingDistance(0.90);
+ 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
+
+ // Display the moving edges, and the Klt points
+ tracker->setDisplayFeatures(displayFeatures);
+
+ // Tells if the tracker has to use Ogre3D for visibility tests
+ tracker->setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker->setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker->setScanLineVisibilityTest(useScanline);
+
+ // Tells if the tracker has to compute the covariance matrix
+ tracker->setCovarianceComputation(computeCovariance);
+
+ // Tells if the tracker has to compute the projection error
+ tracker->setProjectionErrorComputation(projectionError);
+
+ // Retrieve the camera parameters from the tracker
+ dynamic_cast<vpMbGenericTracker *>(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::flush(I1);
+ }
+ }
+
+ // Load the 3D model (either a vrml file or a .cao file)
+ tracker->loadModel(modelFile);
+
+ // Initialise the tracker by clicking on the image
+ // This function looks for
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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<vpMbGenericTracker *>(tracker)->initClick(I1, I2, initFile, initFile, true);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+ // display the 3D model at the given pose
+ dynamic_cast<vpMbGenericTracker *>(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<vpMbGenericTracker *>(tracker)->initFromPose(I1, I2, c1Moi, c2Moi);
+ }
+
+ // track the model
+ dynamic_cast<vpMbGenericTracker *>(tracker)->track(I1, I2);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+
+ if (opt_display) {
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ }
+
+ bool quit = false, click = false;
+ while (!reader.end() && !quit) {
+ // acquire a new image
+ reader.acquire(I1);
+ I2 = I1;
+ // display the image
+ if (opt_display) {
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+
+ std::stringstream ss;
+ ss << "Num frame: " << reader.getFrameIndex() << "/" << reader.getLastFrameIndex();
+ vpDisplay::displayText(I1, 40, 20, ss.str(), vpColor::red);
+ }
+
+ // Test to reset the tracker
+ if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
+ std::cout << "----------Test reset tracker----------" << std::endl;
+ if (opt_display) {
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+ }
+
+ tracker->resetTracker();
+#if defined(VISP_HAVE_XML2) && USE_XML
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile, configFile);
+#else
+ // By setting the parameters:
+ cam1.initPersProjWithoutDistortion(547, 542, 338, 234);
+ cam2.initPersProjWithoutDistortion(547, 542, 338, 234);
+
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(7);
+ me.setThreshold(5000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ vpKltOpencv klt;
+ klt.setMaxFeatures(10000);
+ klt.setWindowSize(5);
+ klt.setQuality(0.01);
+ klt.setMinDistance(5);
+ klt.setHarrisFreeParameter(0.01);
+ klt.setBlockSize(3);
+ klt.setPyramidLevels(3);
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(klt);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
+#endif
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(me);
+ tracker->setAngleAppear(vpMath::rad(65));
+ tracker->setAngleDisappear(vpMath::rad(75));
+
+ // Specify the clipping to
+ tracker->setNearClippingDistance(0.01);
+ tracker->setFarClippingDistance(0.90);
+ 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<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
+ tracker->setOgreVisibilityTest(useOgre);
+ tracker->setScanLineVisibilityTest(useScanline);
+ tracker->setCovarianceComputation(computeCovariance);
+ tracker->setProjectionErrorComputation(projectionError);
+ dynamic_cast<vpMbGenericTracker *>(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);
+ std::cout << "Test set pose" << std::endl;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(I1, I2, c1Mo, c2Mo);
+ }
+
+ // track the object: stop tracking from frame 40 to 50
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ dynamic_cast<vpMbGenericTracker *>(tracker)->track(I1, I2);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+ if (opt_display) {
+ // display the 3D model
+ dynamic_cast<vpMbGenericTracker *>(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);
+ }
+ }
+
+ if (opt_click_allowed) {
+ vpDisplay::displayText(I1, 10, 10, "Click to quit", vpColor::red);
+ vpMouseButton::vpMouseButtonType button;
+ if (vpDisplay::getClick(I1, button, click)) {
+ switch (button) {
+ case vpMouseButton::button1:
+ quit = !click;
+ break;
+
+ case vpMouseButton::button3:
+ click = !click;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ if (computeCovariance) {
+ std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
+ }
+
+ if (projectionError) {
+ 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;
+
+ if (opt_click_allowed && !quit) {
+ vpDisplay::getClick(I1);
+ }
+ reader.close();
+
+ delete tracker;
+ tracker = NULL;
+
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbGenericTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+#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)
+ SoDB::finish();
+#endif
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+
+#else
+
+int main()
+{
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/example/tracking/mbtGenericTracking2.cpp b/example/tracking/mbtGenericTracking2.cpp
new file mode 100644
index 0000000..4200b58
--- /dev/null
+++ b/example/tracking/mbtGenericTracking2.cpp
@@ -0,0 +1,760 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Example of Hybrid Tracking of MBT and MBT KTL.
+ *
+ * Authors:
+ * Aurelien Yol
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \example mbtGenericTracking2.cpp
+
+ \brief Example of tracking with vpGenericTracker on an image sequence
+ containing a cube.
+*/
+
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbGenericTracker.h>
+
+#define GETOPTARGS "x:m:i:n:de:chtfColwvpT:"
+
+#define USE_XML 0
+
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Example of tracking based on the 3D model.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <test image path>] [-x <config file>]\n\
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-C] [-o] [-w] [-l] [-v] [-p]\n\
+ [-T <tracker type>]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: \n\
+ -i <input image path> \n\
+ Set image input path.\n\
+ From this path read images \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
+ images come from ViSP-images-x.y.z.tar.gz available \n\
+ on the ViSP website.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behavior than using\n\
+ this option.\n\
+\n\
+ -x <config file> \n\
+ Set the config file (the xml file) to use.\n\
+ The config file is used to specify the parameters of the tracker.\n\
+\n\
+ -m <model name> \n\
+ Specify the name of the file of the model\n\
+ The model can either be a vrml model (.wrl) or a .cao file.\n\
+\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
+ -f \n\
+ Do not use the vrml model, use the .cao one. These two models are \n\
+ equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
+ website. However, the .cao model allows to use the 3d model based tracker \n\
+ without Coin.\n\
+\n\
+ -C \n\
+ Track only the cube (not the cylinder). In this case the models files are\n\
+ cube.cao or cube.wrl instead of cube_and_cylinder.cao and \n\
+ cube_and_cylinder.wrl.\n\
+\n\
+ -n <initialisation file base name> \n\
+ Base name of the initialisation file. The file will be 'base_name'.init .\n\
+ This base name is also used for the optional picture specifying where to \n\
+ click (a .ppm picture).\n\
+\n\
+ -t \n\
+ Turn off the display of the the moving edges and Klt points. \n\
+\n\
+ -d \n\
+ Turn off the display.\n\
+\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+\n\
+ -o\n\
+ Use Ogre3D for visibility tests\n\
+\n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
+\n\
+ -l\n\
+ Use the scanline for visibility tests.\n\
+\n\
+ -v\n\
+ Compute covariance matrix.\n\
+\n\
+ -p\n\
+ Compute gradient projection error.\n\
+\n\
+ -T <tracker type>\n\
+ Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (EdgeKlt)>).\n\
+\n\
+ -h \n\
+ Print the help.\n\n");
+
+ if (badparam)
+ 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)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayFeatures = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'C':
+ trackCylinder = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 'T':
+ trackerType = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string opt_configFile;
+ std::string opt_modelFile;
+ std::string modelFile;
+ std::string opt_initFile;
+ std::string initFile;
+ long opt_lastFrame = -1;
+ bool displayFeatures = true;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+ bool cao3DModel = false;
+ bool trackCylinder = true;
+ bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
+ bool computeCovariance = false;
+ bool projectionError = false;
+ int trackerType = vpMbGenericTracker::EDGE_TRACKER;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, opt_lastFrame, displayFeatures,
+ opt_click_allowed, opt_display, cao3DModel, trackCylinder, useOgre, showOgreConfigDialog,
+ useScanline, computeCovariance, projectionError, trackerType)) {
+ return EXIT_FAILURE;
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+
+ return EXIT_FAILURE;
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
+ else
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
+
+#if defined(VISP_HAVE_XML2) && USE_XML
+ std::string configFile;
+ if (!opt_configFile.empty())
+ configFile = opt_configFile;
+ else if (!opt_ipath.empty())
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
+ else
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
+#endif
+
+ if (!opt_modelFile.empty()) {
+ modelFile = opt_modelFile;
+ } else {
+ std::string modelFileCao;
+ std::string modelFileWrl;
+ if (trackCylinder) {
+ modelFileCao = "mbt/cube_and_cylinder.cao";
+ modelFileWrl = "mbt/cube_and_cylinder.wrl";
+ } else {
+ modelFileCao = "mbt/cube.cao";
+ modelFileWrl = "mbt/cube.wrl";
+ }
+
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
+ modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
+ } else {
+#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;
+ modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
+#endif
+ }
+ } else {
+ if (cao3DModel) {
+ modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
+ } else {
+#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;
+ modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
+#endif
+ }
+ }
+ }
+
+ if (!opt_initFile.empty())
+ initFile = opt_initFile;
+ else if (!opt_ipath.empty())
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
+ else
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
+
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ vpImage<unsigned char> I1, I2, I3;
+ vpVideoReader reader;
+
+ reader.setFileName(ipath);
+ try {
+ reader.open(I1);
+ I2 = I1;
+ I3 = I1;
+ } catch (...) {
+ std::cerr << "Cannot open sequence: " << ipath << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
+ reader.acquire(I1);
+ I2 = I1;
+ I3 = I1;
+
+ mapOfImages["Camera1"] = &I1;
+ mapOfImages["Camera2"] = &I2;
+ mapOfImages["Camera3"] = &I3;
+
+// initialise a display
+#if defined VISP_HAVE_X11
+ vpDisplayX display1, display2, display3;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display1, display2, display3;
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display1, display2, display3;
+#elif defined VISP_HAVE_D3D9
+ vpDisplayD3D display1, display2, display3;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display1, display2, display3;
+#else
+ opt_display = false;
+#endif
+
+ if (opt_display) {
+#if (defined VISP_HAVE_DISPLAY)
+ display1.setDownScalingFactor(vpDisplay::SCALE_AUTO);
+ display2.setDownScalingFactor(vpDisplay::SCALE_AUTO);
+ 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)");
+#endif
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+ vpDisplay::display(I3);
+
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ vpDisplay::flush(I3);
+ }
+
+ vpMbTracker *tracker = new vpMbGenericTracker(3, trackerType);
+ std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
+ std::map<std::string, vpCameraParameters> mapOfCameraParams;
+
+// Initialise the tracker: camera parameters, moving edge and KLT settings
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // From the xml file
+ std::map<std::string, std::string> mapOfConfigFiles;
+ mapOfConfigFiles["Camera1"] = configFile;
+ mapOfConfigFiles["Camera2"] = configFile;
+ mapOfConfigFiles["Camera3"] = configFile;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
+#else
+ // By setting the parameters:
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(547, 542, 338, 234);
+ mapOfCameraParams["Camera1"] = cam;
+ mapOfCameraParams["Camera2"] = cam;
+ mapOfCameraParams["Camera3"] = cam;
+
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(7);
+ me.setThreshold(5000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ std::map<std::string, vpMe> mapOfMe;
+ mapOfMe["Camera1"] = me;
+ mapOfMe["Camera2"] = me;
+ mapOfMe["Camera3"] = me;
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ vpKltOpencv klt;
+ klt.setMaxFeatures(10000);
+ klt.setWindowSize(5);
+ klt.setQuality(0.01);
+ klt.setMinDistance(5);
+ klt.setHarrisFreeParameter(0.01);
+ klt.setBlockSize(3);
+ klt.setPyramidLevels(3);
+ std::map<std::string, vpKltOpencv> mapOfKlt;
+ mapOfKlt["Camera1"] = klt;
+ mapOfKlt["Camera2"] = klt;
+ mapOfKlt["Camera3"] = klt;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
+#endif
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
+ tracker->setAngleAppear(vpMath::rad(65));
+ tracker->setAngleDisappear(vpMath::rad(75));
+
+ // Specify the clipping to
+ tracker->setNearClippingDistance(0.01);
+ tracker->setFarClippingDistance(0.90);
+
+ std::map<std::string, unsigned int> mapOfClippingFlags;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
+ for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin(); it != mapOfClippingFlags.end();
+ ++it) {
+ it->second = (it->second | vpMbtPolygon::FOV_CLIPPING);
+ }
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setClipping(mapOfClippingFlags);
+// tracker->setClipping(tracker->getClipping() | vpMbtPolygon::LEFT_CLIPPING
+// | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
+#endif
+
+ // Display the moving edges, and the Klt points
+ tracker->setDisplayFeatures(displayFeatures);
+
+ // Tells if the tracker has to use Ogre3D for visibility tests
+ tracker->setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker->setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker->setScanLineVisibilityTest(useScanline);
+
+ // Tells if the tracker has to compute the covariance matrix
+ tracker->setCovarianceComputation(computeCovariance);
+
+ // Tells if the tracker has to compute the projection error
+ tracker->setProjectionErrorComputation(projectionError);
+
+ // Retrieve the camera parameters from the tracker
+ dynamic_cast<vpMbGenericTracker *>(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::flush(I1);
+ }
+ }
+
+ // Load the 3D model (either a vrml file or a .cao file)
+ tracker->loadModel(modelFile);
+
+ // Initialise the tracker by clicking on the image
+ // This function looks for
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
+ std::map<std::string, std::string> mapOfInitFiles;
+ mapOfInitFiles["Camera1"] = initFile;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(mapOfImages, mapOfInitFiles, true);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
+
+ // display the 3D model at the given pose
+ dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfImages, mapOfCameraPoses, mapOfCameraParams,
+ vpColor::red);
+ } else {
+ vpHomogeneousMatrix c1Moi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
+ std::map<std::string, vpHomogeneousMatrix> mapOfInitPoses;
+ mapOfInitPoses["Camera1"] = c1Moi;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->initFromPose(mapOfImages, mapOfInitPoses);
+ }
+
+ // track the model
+ dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
+
+ if (opt_display) {
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ vpDisplay::flush(I3);
+ }
+
+ bool quit = false, click = false;
+ while (!reader.end() && !quit) {
+ // acquire a new image
+ reader.acquire(I1);
+ I2 = I1;
+ I3 = I1;
+ mapOfImages["Camera1"] = &I1;
+ mapOfImages["Camera2"] = &I2;
+ mapOfImages["Camera3"] = &I3;
+
+ // display the image
+ if (opt_display) {
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+ vpDisplay::display(I3);
+
+ std::stringstream ss;
+ ss << "Num frame: " << reader.getFrameIndex() << "/" << reader.getLastFrameIndex();
+ vpDisplay::displayText(I1, 40, 20, ss.str(), vpColor::red);
+ }
+
+ // Test to reset the tracker
+ if (reader.getFrameIndex() == reader.getFirstFrameIndex() + 10) {
+ std::cout << "----------Test reset tracker----------" << std::endl;
+ if (opt_display) {
+ vpDisplay::display(I1);
+ vpDisplay::display(I2);
+ vpDisplay::display(I3);
+ }
+
+ tracker->resetTracker();
+#if defined(VISP_HAVE_XML2) && USE_XML
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(mapOfConfigFiles);
+#else
+ // By setting the parameters:
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(547, 542, 338, 234);
+ mapOfCameraParams["Camera1"] = cam;
+ mapOfCameraParams["Camera2"] = cam;
+ mapOfCameraParams["Camera3"] = cam;
+
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(7);
+ me.setThreshold(5000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ std::map<std::string, vpMe> mapOfMe;
+ mapOfMe["Camera1"] = me;
+ mapOfMe["Camera2"] = me;
+ mapOfMe["Camera3"] = me;
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ vpKltOpencv klt;
+ klt.setMaxFeatures(10000);
+ klt.setWindowSize(5);
+ klt.setQuality(0.01);
+ klt.setMinDistance(5);
+ klt.setHarrisFreeParameter(0.01);
+ klt.setBlockSize(3);
+ klt.setPyramidLevels(3);
+ std::map<std::string, vpKltOpencv> mapOfKlt;
+ mapOfKlt["Camera1"] = klt;
+ mapOfKlt["Camera2"] = klt;
+ mapOfKlt["Camera3"] = klt;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(mapOfKlt);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
+#endif
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(mapOfMe);
+ tracker->setAngleAppear(vpMath::rad(65));
+ tracker->setAngleDisappear(vpMath::rad(75));
+
+ // Specify the clipping to
+ tracker->setNearClippingDistance(0.01);
+ tracker->setFarClippingDistance(0.90);
+
+ std::map<std::string, unsigned int> mapOfClippingFlags;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getClipping(mapOfClippingFlags);
+ for (std::map<std::string, unsigned int>::iterator it = mapOfClippingFlags.begin();
+ it != mapOfClippingFlags.end(); ++it) {
+ it->second = (it->second | vpMbtPolygon::FOV_CLIPPING);
+ }
+
+ dynamic_cast<vpMbGenericTracker *>(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<vpMbGenericTracker *>(tracker)->setCameraParameters(mapOfCameraParams);
+ tracker->setOgreVisibilityTest(useOgre);
+ tracker->setScanLineVisibilityTest(useScanline);
+ tracker->setCovarianceComputation(computeCovariance);
+ tracker->setProjectionErrorComputation(projectionError);
+ dynamic_cast<vpMbGenericTracker *>(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);
+ std::map<std::string, vpHomogeneousMatrix> mapOfSetPoses;
+ mapOfSetPoses["Camera1"] = c1Moi;
+
+ std::cout << "Test set pose" << std::endl;
+ dynamic_cast<vpMbGenericTracker *>(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<vpMbGenericTracker *>(tracker)->track(mapOfImages);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(mapOfCameraPoses);
+ if (opt_display) {
+ // display the 3D model
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ std::map<std::string, const vpImage<unsigned char> *> mapOfSubImages;
+ mapOfSubImages["Camera1"] = &I1;
+ mapOfSubImages["Camera2"] = &I2;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->display(mapOfSubImages, mapOfCameraPoses, mapOfCameraParams,
+ vpColor::red, 3);
+ } else {
+ dynamic_cast<vpMbGenericTracker *>(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);
+ }
+ }
+
+ if (opt_click_allowed) {
+ vpDisplay::displayText(I1, 10, 10, "Click to quit", vpColor::red);
+ vpMouseButton::vpMouseButtonType button;
+ if (vpDisplay::getClick(I1, button, click)) {
+ switch (button) {
+ case vpMouseButton::button1:
+ quit = !click;
+ break;
+
+ case vpMouseButton::button3:
+ click = !click;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ if (computeCovariance) {
+ std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
+ }
+
+ if (projectionError) {
+ std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
+ }
+
+ vpDisplay::flush(I1);
+ vpDisplay::flush(I2);
+ vpDisplay::flush(I3);
+ }
+
+ std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
+ std::cout << "\nFinal poses, c1Mo:\n"
+ << mapOfCameraPoses["Camera1"] << "\nc2Mo:\n"
+ << mapOfCameraPoses["Camera2"] << "\nc3Mo:\n"
+ << mapOfCameraPoses["Camera3"] << std::endl;
+
+ if (opt_click_allowed && !quit) {
+ vpDisplay::getClick(I1);
+ }
+ reader.close();
+
+ delete tracker;
+ tracker = NULL;
+
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbGenericTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+#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)
+ SoDB::finish();
+#endif
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+
+#else
+
+int main()
+{
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
+ return 0;
+}
+
+#endif
diff --git a/example/tracking/mbtGenericTrackingDepth.cpp b/example/tracking/mbtGenericTrackingDepth.cpp
new file mode 100644
index 0000000..dc02639
--- /dev/null
+++ b/example/tracking/mbtGenericTrackingDepth.cpp
@@ -0,0 +1,865 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Example of tracking with vpGenericTracker on Castel.
+ *
+ *****************************************************************************/
+
+/*!
+ \example mbtGenericTrackingDepth.cpp
+
+ \brief Example of tracking with vpGenericTracker on Castel.
+*/
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_MODULE_MBT) && defined(VISP_HAVE_DISPLAY)
+
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbGenericTracker.h>
+
+#define GETOPTARGS "x:X:m:M:i:n:dchfolwvpt:T:e:"
+
+#define USE_XML 1
+#define USE_SMALL_DATASET 1 // small depth dataset in ViSP-images
+
+namespace
+{
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+ Example of tracking with vpGenericTracker.\n\
+ \n\
+ SYNOPSIS\n\
+ %s [-i <test image path>] [-x <config file>] [-X <config file depth>]\n\
+ [-m <model name>] [-M <model name depth>] [-n <initialisation file base name>]\n\
+ [-f] [-c] [-d] [-h] [-o] [-w] [-l] [-v] [-p]\n\
+ [-t <tracker type>] [-T <tracker type>] [-e <last frame index>]\n", name);
+
+ fprintf(stdout, "\n\
+ OPTIONS: \n\
+ -i <input image path> \n\
+ Set image input path.\n\
+ These images come from ViSP-images-x.y.z.tar.gz available \n\
+ on the ViSP website.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behavior than using\n\
+ this option.\n\
+ \n\
+ -x <config file> \n\
+ Set the config file (the xml file) to use.\n\
+ The config file is used to specify the parameters of the tracker.\n\
+ \n\
+ -X <config file> \n\
+ Set the config file (the xml file) to use for the depth sensor.\n\
+ The config file is used to specify the parameters of the tracker.\n\
+ \n\
+ -m <model name> \n\
+ Specify the name of the file of the model.\n\
+ The model can either be a vrml model (.wrl) or a .cao file.\n\
+ \n\
+ -M <model name> \n\
+ Specify the name of the file of the model for the depth sensor.\n\
+ The model can either be a vrml model (.wrl) or a .cao file.\n\
+ \n\
+ -n <initialisation file base name> \n\
+ Base name of the initialisation file. The file will be 'base_name'.init .\n\
+ This base name is also used for the optional picture specifying where to \n\
+ click (a .ppm picture).\n\
+ \n\
+ -f \n\
+ Turn off the display of the the moving edges and Klt points. \n\
+ \n\
+ -d \n\
+ Turn off the display.\n\
+ \n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without human intervention.\n\
+ \n\
+ -o\n\
+ Use Ogre3D for visibility tests\n\
+ \n\
+ -w\n\
+ When Ogre3D is enable [-o] show Ogre3D configuration dialog that allows to set the renderer.\n\
+ \n\
+ -l\n\
+ Use the scanline for visibility tests.\n\
+ \n\
+ -v\n\
+ Compute covariance matrix.\n\
+ \n\
+ -p\n\
+ Compute gradient projection error.\n\
+ \n\
+ -t <tracker type>\n\
+ Set tracker type (<1 (Edge)>, <2 (KLT)>, <3 (both)>) for color sensor.\n\
+ \n\
+ -T <tracker type>\n\
+ Set tracker type (<4 (Depth normal)>, <8 (Depth dense)>, <12 (both)>) for depth sensor.\n\
+ \n\
+ -e <last frame index>\n\
+ Specify the index of the last frame. Once reached, the tracking is stopped.\n\
+ \n\
+ -h \n\
+ Print the help.\n\n");
+
+ if (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)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'X':
+ configFile_depth = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'M':
+ modelFile_depth = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 'f':
+ displayFeatures = false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'p':
+ projectionError = true;
+ break;
+ case 't':
+ trackerType = atoi(optarg_);
+ break;
+ case 'T':
+ tracker_type_depth = atoi(optarg_);
+ break;
+ case 'e':
+ lastFrame = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+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 coeffs[5]; /**< Distortion coefficients */
+};
+
+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);
+
+ x = ux;
+ y = uy;
+
+ point[0] = depth * x;
+ point[1] = depth * y;
+ point[2] = depth;
+}
+
+bool read_data(const unsigned int cpt, const std::string &input_directory, vpImage<unsigned char> &I,
+ vpImage<uint16_t> &I_depth_raw, std::vector<vpColVector> &pointcloud, unsigned int &pointcloud_width,
+ unsigned int &pointcloud_height)
+{
+ char buffer[256];
+
+ // Read image
+ std::stringstream ss;
+ ss << input_directory << "/image_%04d.pgm";
+ sprintf(buffer, ss.str().c_str(), cpt);
+ std::string filename_image = buffer;
+
+ if (!vpIoTools::checkFilename(filename_image)) {
+ std::cerr << "Cannot read: " << filename_image << std::endl;
+ return false;
+ }
+ vpImageIo::read(I, filename_image);
+
+ // Read raw depth
+ ss.str("");
+ ss << input_directory << "/depth_image_%04d.bin";
+ 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);
+ if (!file_depth.is_open()) {
+ return false;
+ }
+
+ unsigned int height = 0, width = 0;
+ file_depth.read((char *)(&height), sizeof(height));
+ file_depth.read((char *)(&width), sizeof(width));
+ I_depth_raw.resize(height, width);
+
+ uint16_t depth_value = 0;
+ for (unsigned int i = 0; i < height; i++) {
+ for (unsigned int j = 0; j < width; j++) {
+ file_depth.read((char *)(&depth_value), sizeof(depth_value));
+ I_depth_raw[i][j] = depth_value;
+ }
+ }
+
+ // Transform pointcloud
+ pointcloud_width = width;
+ pointcloud_height = height;
+ pointcloud.resize((size_t)width * height);
+
+ // Only for Creative SR300
+ const float depth_scale = 0.000124986647f;
+ rs_intrinsics depth_intrinsic;
+ depth_intrinsic.ppx = 311.484558f;
+ depth_intrinsic.ppy = 246.283234f;
+ depth_intrinsic.fx = 476.053619f;
+ depth_intrinsic.fy = 476.053497f;
+ depth_intrinsic.coeffs[0] = 0.165056542f;
+ depth_intrinsic.coeffs[1] = -0.0508309528f;
+ depth_intrinsic.coeffs[2] = 0.00435937941f;
+ depth_intrinsic.coeffs[3] = 0.00541406544f;
+ depth_intrinsic.coeffs[4] = 0.250085592f;
+
+ for (unsigned int i = 0; i < height; i++) {
+ for (unsigned int j = 0; j < width; j++) {
+ 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);
+
+ vpColVector data_3D(3);
+ data_3D[0] = point[0];
+ data_3D[1] = point[1];
+ data_3D[2] = point[2];
+
+ pointcloud[(size_t)(i * width + j)] = data_3D;
+ }
+ }
+
+ return true;
+}
+
+void loadConfiguration(vpMbTracker *const tracker,
+ const std::string &
+#if defined(VISP_HAVE_XML2) && USE_XML
+ configFile
+#endif
+ ,
+ const std::string &
+#if defined(VISP_HAVE_XML2) && USE_XML
+ configFile_depth
+#endif
+)
+{
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // From the xml file
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadConfigFile(configFile, configFile_depth);
+#else
+ // Edge
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setMovingEdge(me);
+
+// Klt
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ vpKltOpencv klt;
+ klt.setMaxFeatures(10000);
+ klt.setWindowSize(5);
+ klt.setQuality(0.01);
+ klt.setMinDistance(5);
+ klt.setHarrisFreeParameter(0.02);
+ klt.setBlockSize(3);
+ klt.setPyramidLevels(3);
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltOpencv(klt);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setKltMaskBorder(5);
+#endif
+
+ // Depth
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalFeatureEstimationMethod(
+ vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationMethod(2);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacMaxIter(200);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalPclPlaneEstimationRansacThreshold(0.001);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setDepthNormalSamplingStep(2, 2);
+
+ dynamic_cast<vpMbGenericTracker *>(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);
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
+
+ tracker->setAngleAppear(vpMath::rad(70));
+ tracker->setAngleDisappear(vpMath::rad(80));
+
+ // Specify the clipping to
+ tracker->setNearClippingDistance(0.01);
+ tracker->setFarClippingDistance(2.0);
+ 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
+}
+}
+
+int main(int argc, const char **argv)
+{
+ {
+ // Test TukeyEstimator
+ {
+ vpMbtTukeyEstimator<double> tukey_estimator;
+ std::vector<double> residues;
+ residues.push_back(0.5);
+ residues.push_back(0.1);
+ residues.push_back(0.15);
+ residues.push_back(0.14);
+ residues.push_back(0.12);
+ std::vector<double> weights(5, 1);
+
+ 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 << std::endl;
+ }
+
+ {
+ vpMbtTukeyEstimator<float> tukey_estimator;
+ std::vector<float> residues;
+ residues.push_back(0.5f);
+ residues.push_back(0.1f);
+ residues.push_back(0.15f);
+ residues.push_back(0.14f);
+ residues.push_back(0.12f);
+ std::vector<float> weights(5, 1);
+
+ 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 << std::endl;
+ }
+ }
+
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string ipath;
+ std::string opt_configFile;
+ std::string opt_configFile_depth;
+ std::string opt_modelFile;
+ std::string opt_modelFile_depth;
+ std::string opt_initFile;
+ std::string initFile;
+ bool displayFeatures = true;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+ bool useOgre = false;
+ bool showOgreConfigDialog = false;
+ bool useScanline = false;
+ bool computeCovariance = false;
+ bool projectionError = false;
+ int trackerType_image = vpMbGenericTracker::EDGE_TRACKER;
+ int trackerType_depth = vpMbGenericTracker::DEPTH_DENSE_TRACKER;
+ int opt_lastFrame = -1;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (!getOptions(argc, argv, opt_ipath, opt_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 (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."
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+#endif
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+
+ return EXIT_FAILURE;
+ }
+
+ // Get the option values
+ 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");
+ if (!vpIoTools::checkDirectory(dir_path)) {
+ std::cerr << "ViSP-images does not contain the folder: " << dir_path << "!" << std::endl;
+ return EXIT_SUCCESS;
+ }
+
+ std::string configFile, configFile_depth;
+ if (!opt_configFile.empty())
+ configFile = opt_configFile;
+ else
+ 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");
+
+ 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");
+#else
+ 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");
+
+ 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);
+
+ if (use_vrml) {
+#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 "
+ "build with Coin3D 3rd party"
+ << std::endl;
+ return EXIT_FAILURE;
+#endif
+ }
+
+ if (!opt_initFile.empty())
+ initFile = opt_initFile;
+ else
+ initFile = vpIoTools::createFilePath(!opt_ipath.empty() ? opt_ipath : env_ipath, "mbt-depth/castel/chateau.init");
+
+ vpImage<unsigned char> I, I_depth;
+ vpImage<uint16_t> I_depth_raw;
+ std::vector<vpColVector> pointcloud;
+ unsigned int 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;
+ }
+
+ vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
+
+// initialise a display
+#if defined VISP_HAVE_X11
+ vpDisplayX display1, display2;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display1, display2;
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display1, display2;
+#elif defined VISP_HAVE_D3D9
+ vpDisplayD3D display1, display2;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display1, display2;
+#else
+ opt_display = false;
+#endif
+ if (opt_display) {
+#if (defined VISP_HAVE_DISPLAY)
+ 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)");
+#endif
+ vpDisplay::display(I);
+ vpDisplay::display(I_depth);
+ vpDisplay::flush(I);
+ vpDisplay::flush(I_depth);
+ }
+
+ std::vector<int> trackerTypes(2);
+ trackerTypes[0] = trackerType_image;
+ trackerTypes[1] = trackerType_depth;
+ vpMbTracker *tracker = new vpMbGenericTracker(trackerTypes);
+ vpHomogeneousMatrix c1Mo, c2Mo;
+ vpCameraParameters cam1, cam2;
+
+ loadConfiguration(tracker, configFile, configFile_depth);
+
+ 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");
+ {
+ std::ifstream depth_M_color_file(depth_M_color_filename.c_str());
+ depth_M_color.load(depth_M_color_file);
+ std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrices;
+ mapOfCameraTransformationMatrices["Camera2"] = depth_M_color;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrices);
+ }
+
+ // Display the moving edges, and the Klt points
+ tracker->setDisplayFeatures(displayFeatures);
+
+ // Tells if the tracker has to use Ogre3D for visibility tests
+ tracker->setOgreVisibilityTest(useOgre);
+ if (useOgre)
+ tracker->setOgreShowConfigDialog(showOgreConfigDialog);
+
+ // Tells if the tracker has to use the scanline visibility tests
+ tracker->setScanLineVisibilityTest(useScanline);
+
+ // Tells if the tracker has to compute the covariance matrix
+ tracker->setCovarianceComputation(computeCovariance);
+
+ // Tells if the tracker has to compute the projection error
+ tracker->setProjectionErrorComputation(projectionError);
+
+ // Retrieve the camera parameters from the tracker
+ dynamic_cast<vpMbGenericTracker *>(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::flush(I);
+ }
+ }
+
+ // Load the 3D model (either a vrml file or a .cao file)
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile, modelFile_depth);
+
+ if (opt_display && opt_click_allowed) {
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ mapOfImages["Camera1"] = &I;
+ mapOfImages["Camera2"] = &I_depth;
+ std::map<std::string, std::string> mapOfInitFiles;
+ mapOfInitFiles["Camera1"] = initFile;
+
+ // Initialise the tracker by clicking on the image
+ dynamic_cast<vpMbGenericTracker *>(tracker)->initClick(mapOfImages, mapOfInitFiles, true);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+ // display the 3D model at the given pose
+ dynamic_cast<vpMbGenericTracker *>(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<vpMbGenericTracker *>(tracker)->initFromPose(I, I_depth, c1Moi, c2Moi);
+ }
+
+ // track the model
+ {
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ mapOfImages["Camera1"] = &I;
+ std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
+ mapOfPointclouds["Camera2"] = &pointcloud;
+ std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
+ mapOfWidths["Camera2"] = pointcloud_width;
+ mapOfHeights["Camera2"] = pointcloud_height;
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
+ }
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+
+ if (opt_display) {
+ vpDisplay::flush(I);
+ vpDisplay::flush(I_depth);
+ }
+
+ bool quit = false, click = false;
+ unsigned int frame_index = 0;
+ std::vector<double> time_vec;
+ 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);
+
+ if (opt_display) {
+ vpDisplay::display(I);
+ vpDisplay::display(I_depth);
+
+ std::stringstream ss;
+ ss << "Num frame: " << frame_index;
+ vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
+ }
+
+ // Test reset the tracker
+ if (frame_index == 10) {
+ std::cout << "----------Test reset tracker----------" << std::endl;
+ if (opt_display) {
+ vpDisplay::display(I);
+ vpDisplay::display(I_depth);
+ }
+
+ tracker->resetTracker();
+
+ loadConfiguration(tracker, configFile, configFile_depth);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->loadModel(modelFile, modelFile_depth);
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setCameraParameters(cam1, cam2);
+ tracker->setOgreVisibilityTest(useOgre);
+ tracker->setScanLineVisibilityTest(useScanline);
+ tracker->setCovarianceComputation(computeCovariance);
+ tracker->setProjectionErrorComputation(projectionError);
+ dynamic_cast<vpMbGenericTracker *>(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);
+#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);
+#endif
+ std::cout << "Test set pose" << std::endl;
+ dynamic_cast<vpMbGenericTracker *>(tracker)->setPose(I, I_depth, c1Mo, c2Mo);
+ }
+
+#if USE_SMALL_DATASET
+ // track the object: stop tracking from frame 15 to 20
+ if (frame_index < 15 || frame_index >= 20) {
+#else
+ // track the object: stop tracking from frame 30 to 50
+ if (frame_index < 30 || frame_index >= 50) {
+#endif
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ mapOfImages["Camera1"] = &I;
+ std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
+ mapOfPointclouds["Camera2"] = &pointcloud;
+ std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
+ mapOfWidths["Camera2"] = pointcloud_width;
+ mapOfHeights["Camera2"] = pointcloud_height;
+
+ double t = vpTime::measureTimeMs();
+ dynamic_cast<vpMbGenericTracker *>(tracker)->track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
+ t = vpTime::measureTimeMs() - t;
+ time_vec.push_back(t);
+
+ dynamic_cast<vpMbGenericTracker *>(tracker)->getPose(c1Mo, c2Mo);
+
+ if (opt_display) {
+ // display the 3D model
+ dynamic_cast<vpMbGenericTracker *>(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);
+ // computation time
+ std::stringstream ss;
+ ss << "Computation time: " << t << " ms";
+ vpDisplay::displayText(I, 60, 20, ss.str(), vpColor::red);
+ }
+ }
+
+ if (opt_click_allowed) {
+ vpDisplay::displayText(I, 10, 10, "Click to quit", vpColor::red);
+ vpMouseButton::vpMouseButtonType button;
+ if (vpDisplay::getClick(I, button, click)) {
+ switch (button) {
+ case vpMouseButton::button1:
+ quit = !click;
+ break;
+
+ case vpMouseButton::button3:
+ click = !click;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ if (computeCovariance) {
+ std::cout << "Covariance matrix: \n" << tracker->getCovarianceMatrix() << std::endl << std::endl;
+ }
+
+ if (projectionError) {
+ std::cout << "Projection error: " << tracker->getProjectionError() << std::endl << std::endl;
+ }
+
+ vpDisplay::flush(I);
+ vpDisplay::flush(I_depth);
+
+ frame_index++;
+ }
+
+ 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"
+ << std::endl;
+
+ if (opt_click_allowed && !quit) {
+ vpDisplay::getClick(I);
+ }
+
+ delete tracker;
+ tracker = NULL;
+
+#if defined(VISP_HAVE_XML2) && USE_XML
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbGenericTracker::loadConfigFile()
+ vpXmlParser::cleanup();
+#endif
+
+#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)
+ SoDB::finish();
+#endif
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+
+#else
+int main()
+{
+ std::cerr << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
+ return EXIT_SUCCESS;
+}
+#endif
diff --git a/example/tracking/mbtKltMultiTracking.cpp b/example/tracking/mbtKltMultiTracking.cpp
index b12f683..2b74b8e 100644
--- a/example/tracking/mbtKltMultiTracking.cpp
+++ b/example/tracking/mbtKltMultiTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,24 +46,24 @@
#include <iostream>
#include <visp3/core/vpConfig.h>
-#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 <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbKltMultiTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfolwv"
-
+#define GETOPTARGS "x:m:i:n:de:chtfolwv"
void usage(const char *name, const char *badparam)
{
@@ -71,16 +72,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l] [-v]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>] \n\
+ [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l] [-v]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -95,6 +95,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -104,7 +107,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optional picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the klt points. \n\
@@ -135,33 +138,63 @@ 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, bool &displayKltPoints, bool &click_allowed, bool &display,
- bool& cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance)
+ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayKltPoints = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayKltPoints = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -176,8 +209,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -189,6 +221,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayKltPoints = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -199,27 +232,26 @@ main(int argc, const char ** argv)
bool computeCovariance = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayKltPoints,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -229,28 +261,27 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
- std::string modelFileCao = "ViSP-images/mbt/cube.cao";
- std::string modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ } else {
+ std::string modelFileCao = "mbt/cube.cao";
+ std::string modelFileWrl = "mbt/cube.wrl";
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -258,12 +289,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -277,26 +306,29 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I1, I2;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I1);
I2 = I1;
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I1);
I2 = I1;
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display1, display2;
#elif defined VISP_HAVE_GDI
@@ -310,13 +342,12 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
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);
@@ -329,7 +360,7 @@ main(int argc, const char ** argv)
// Load tracker config file (camera parameters and moving edge settings)
vpCameraParameters cam1, cam2;
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile, configFile);
#else
@@ -348,15 +379,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam1, cam2);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// 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::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
// Display the klt points
@@ -377,9 +410,8 @@ main(int argc, const char ** argv)
tracker.getCameraParameters(cam1, cam2);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I1,false)){
+ 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::flush(I1);
@@ -392,23 +424,22 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I1, I2, initFile, initFile, true);
tracker.getPose(c1Mo, c2Mo);
// display the 3D model at the given pose
- tracker.display(I1, I2,c1Mo, c2Mo, cam1, cam2, vpColor::red);
- }
- else
- {
+ 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);
tracker.initFromPose(I1, I2, c1Moi, c2Moi);
}
- //track the model
+ // track the model
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
@@ -417,8 +448,7 @@ main(int argc, const char ** argv)
vpDisplay::flush(I2);
}
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I1);
I2 = I1;
@@ -436,7 +466,7 @@ main(int argc, const char ** argv)
vpDisplay::display(I2);
}
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile, configFile);
#else
// By setting the parameters:
@@ -454,15 +484,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam1, cam2);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// 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::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to 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);
tracker.setCameraParameters(cam1, cam2);
@@ -474,22 +506,23 @@ 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);
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I1, I2);
tracker.getPose(c1Mo, c2Mo);
if (opt_display) {
// display the 3D model
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);
+ vpDisplay::displayFrame(I1, c1Mo, cam1, 0.05);
+ vpDisplay::displayFrame(I2, c2Mo, cam2, 0.05);
}
}
@@ -501,34 +534,37 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
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;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I1);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbKltTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#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)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbKltTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -538,9 +574,10 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
return 0;
-
}
#endif
diff --git a/example/tracking/mbtKltTracking.cpp b/example/tracking/mbtKltTracking.cpp
index e8bdaf1..0199f39 100644
--- a/example/tracking/mbtKltTracking.cpp
+++ b/example/tracking/mbtKltTracking.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,28 +45,24 @@
#include <iostream>
#include <visp3/core/vpConfig.h>
-#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 <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbKltTracker.h>
-#define GETOPTARGS "x:m:i:n:dchtfolwv"
-
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &configFile, std::string &modelFile,
- std::string &initFile, bool &displayKltPoints, bool &click_allowed, bool &display,
- bool& cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance);
+#define GETOPTARGS "x:m:i:n:de:chtfolwv"
void usage(const char *name, const char *badparam)
{
@@ -74,16 +71,15 @@ Example of tracking based on the 3D model.\n\
\n\
SYNOPSIS\n\
%s [-i <test image path>] [-x <config file>]\n\
- [-m <model name>] [-n <initialisation file base name>]\n\
- [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l] [-v]",
- name );
+ [-m <model name>] [-n <initialisation file base name>] [-e <last frame index>]\n\
+ [-t] [-c] [-d] [-h] [-f] [-o] [-w] [-l] [-v]\n", name);
fprintf(stdout, "\n\
OPTIONS: \n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mbt/cube/image%%04d.ppm\". These \n\
+ \"mbt/cube/image%%04d.ppm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -98,6 +94,9 @@ OPTIONS: \n\
Specify the name of the file of the model\n\
The model can either be a vrml model (.wrl) or a .cao file.\n\
\n\
+ -e <last frame index> \n\
+ Specify the index of the last frame. Once reached, the tracking is stopped\n\
+\n\
-f \n\
Do not use the vrml model, use the .cao one. These two models are \n\
equivalent and comes from ViSP-images-x.y.z.tar.gz available on the ViSP\n\
@@ -107,7 +106,7 @@ OPTIONS: \n\
-n <initialisation file base name> \n\
Base name of the initialisation file. The file will be 'base_name'.init .\n\
This base name is also used for the optionnal picture specifying where to \n\
- click (a .ppm picture).\
+ click (a .ppm picture).\n\
\n\
-t \n\
Turn off the display of the the klt points. \n\
@@ -138,33 +137,63 @@ 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, bool &displayKltPoints, bool &click_allowed, bool &display,
- bool& cao3DModel, bool &useOgre, bool &showOgreConfigDialog, bool &useScanline, bool &computeCovariance)
+ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'x': configFile = optarg_; break;
- case 'm': modelFile = optarg_; break;
- case 'n': initFile = optarg_; break;
- case 't': displayKltPoints = false; break;
- case 'f': cao3DModel = true; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'o': useOgre = true; break;
- case 'l': useScanline = true; break;
- case 'w': showOgreConfigDialog = true; break;
- case 'v': computeCovariance = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'e':
+ lastFrame = atol(optarg_);
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'x':
+ configFile = optarg_;
+ break;
+ case 'm':
+ modelFile = optarg_;
+ break;
+ case 'n':
+ initFile = optarg_;
+ break;
+ case 't':
+ displayKltPoints = false;
+ break;
+ case 'f':
+ cao3DModel = true;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'o':
+ useOgre = true;
+ break;
+ case 'l':
+ useScanline = true;
+ break;
+ case 'w':
+ showOgreConfigDialog = true;
+ break;
+ case 'v':
+ computeCovariance = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -179,8 +208,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &co
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -192,6 +220,7 @@ main(int argc, const char ** argv)
std::string modelFile;
std::string opt_initFile;
std::string initFile;
+ long opt_lastFrame = -1;
bool displayKltPoints = true;
bool opt_click_allowed = true;
bool opt_display = true;
@@ -202,27 +231,26 @@ main(int argc, const char ** argv)
bool computeCovariance = false;
bool quit = false;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (!getOptions(argc, argv, opt_ipath, opt_configFile, opt_modelFile, opt_initFile, displayKltPoints,
+ 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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -232,28 +260,27 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mbt/cube/image%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube/image%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mbt/cube/image%04d.pgm");
if (!opt_configFile.empty())
configFile = opt_configFile;
else if (!opt_ipath.empty())
- configFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube.xml");
else
- configFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ configFile = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- if (!opt_modelFile.empty()){
+ if (!opt_modelFile.empty()) {
modelFile = opt_modelFile;
- }else{
- std::string modelFileCao = "ViSP-images/mbt/cube.cao";
- std::string modelFileWrl = "ViSP-images/mbt/cube.wrl";
+ } else {
+ std::string modelFileCao = "mbt/cube.cao";
+ std::string modelFileWrl = "mbt/cube.wrl";
- if(!opt_ipath.empty()){
- if(cao3DModel){
+ if (!opt_ipath.empty()) {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileWrl);
#else
@@ -261,12 +288,10 @@ main(int argc, const char ** argv)
modelFile = vpIoTools::createFilePath(opt_ipath, modelFileCao);
#endif
}
- }
- else{
- if(cao3DModel){
+ } else {
+ if (cao3DModel) {
modelFile = vpIoTools::createFilePath(env_ipath, modelFileCao);
- }
- else{
+ } else {
#ifdef VISP_HAVE_COIN3D
modelFile = vpIoTools::createFilePath(env_ipath, modelFileWrl);
#else
@@ -280,24 +305,27 @@ main(int argc, const char ** argv)
if (!opt_initFile.empty())
initFile = opt_initFile;
else if (!opt_ipath.empty())
- initFile = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(opt_ipath, "mbt/cube");
else
- initFile = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ initFile = vpIoTools::createFilePath(env_ipath, "mbt/cube");
vpImage<unsigned char> I;
vpVideoReader reader;
reader.setFileName(ipath);
- try{
+ try {
reader.open(I);
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
+ if (opt_lastFrame > 1 && opt_lastFrame < reader.getLastFrameIndex())
+ reader.setLastFrameIndex(opt_lastFrame);
+
reader.acquire(I);
- // initialise a display
+// initialise a display
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GDI
@@ -311,12 +339,11 @@ main(int argc, const char ** argv)
#else
opt_display = false;
#endif
- if (opt_display)
- {
+ if (opt_display) {
#if (defined VISP_HAVE_DISPLAY)
- display.init(I, 100, 100, "Test tracking") ;
+ display.init(I, 100, 100, "Test tracking");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
@@ -325,7 +352,7 @@ main(int argc, const char ** argv)
// Load tracker config file (camera parameters and moving edge settings)
vpCameraParameters cam;
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
// From the xml file
tracker.loadConfigFile(configFile);
#else
@@ -343,15 +370,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// 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::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
+// tracker.setClipping(tracker.getClipping() | vpMbtPolygon::LEFT_CLIPPING |
+// vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING |
+// vpMbtPolygon::DOWN_CLIPPING); // Equivalent to FOV_CLIPPING
#endif
// Display the klt points
@@ -372,12 +401,11 @@ main(int argc, const char ** argv)
tracker.getCameraParameters(cam);
// Loop to position the cube
- if (opt_display && opt_click_allowed)
- {
- while(!vpDisplay::getClick(I,false)){
+ 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::flush(I) ;
+ vpDisplay::flush(I);
}
}
@@ -386,30 +414,28 @@ main(int argc, const char ** argv)
// Initialise the tracker by clicking on the image
// This function looks for
- // - a ./cube/cube.init file that defines the 3d coordinates (in meter, in the object basis) of the points used for the initialisation
- // - 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)
- {
+ // - a ./cube/cube.init file that defines the 3d coordinates (in meter,
+ // in the object basis) of the points used for the initialisation
+ // - 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) {
tracker.initClick(I, initFile, true);
tracker.getPose(cMo);
// display the 3D model at the given pose
- tracker.display(I,cMo, cam, vpColor::red);
- }
- else
- {
+ tracker.display(I, cMo, cam, vpColor::red);
+ } else {
vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
tracker.initFromPose(I, cMoi);
}
- //track the model
+ // track the model
tracker.track(I);
tracker.getPose(cMo);
if (opt_display)
vpDisplay::flush(I);
- while (!reader.end())
- {
+ while (!reader.end()) {
// acquire a new image
reader.acquire(I);
// display the image
@@ -422,7 +448,7 @@ main(int argc, const char ** argv)
if (opt_display)
vpDisplay::display(I);
tracker.resetTracker();
-#if defined (VISP_HAVE_XML2)
+#if defined(VISP_HAVE_XML2)
tracker.loadConfigFile(configFile);
#else
// By setting the parameters:
@@ -439,15 +465,17 @@ main(int argc, const char ** argv)
tracker.setCameraParameters(cam);
tracker.setKltOpencv(klt);
- tracker.setAngleAppear( vpMath::rad(65) );
- tracker.setAngleDisappear( vpMath::rad(75) );
- tracker.setMaskBorder(5);
+ tracker.setKltMaskBorder(5);
+ tracker.setAngleAppear(vpMath::rad(65));
+ tracker.setAngleDisappear(vpMath::rad(75));
// 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::LEFT_CLIPPING | vpMbtPolygon::RIGHT_CLIPPING | vpMbtPolygon::UP_CLIPPING | vpMbtPolygon::DOWN_CLIPPING); // Equivalent to 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);
tracker.setCameraParameters(cam);
@@ -459,30 +487,31 @@ 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) {
-// // display the 3D model
-// tracker.display(I, cMo, cam, vpColor::darkRed);
-// // display the frame
-// vpDisplay::displayFrame (I, cMo, cam, 0.05);
-//// if (opt_click_allowed) {
-//// vpDisplay::flush(I);
-//// vpDisplay::getClick(I);
-//// }
-// }
+ // if (opt_display) {
+ // // display the 3D model
+ // tracker.display(I, cMo, cam, vpColor::darkRed);
+ // // display the frame
+ // vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ //// if (opt_click_allowed) {
+ //// vpDisplay::flush(I);
+ //// vpDisplay::getClick(I);
+ //// }
+ // }
}
// track the object: stop tracking from frame 40 to 50
- if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 || reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
+ if (reader.getFrameIndex() - reader.getFirstFrameIndex() < 40 ||
+ reader.getFrameIndex() - reader.getFirstFrameIndex() >= 50) {
tracker.track(I);
tracker.getPose(cMo);
if (opt_display) {
// display the 3D model
tracker.display(I, cMo, cam, vpColor::darkRed);
// display the frame
- vpDisplay::displayFrame (I, cMo, cam, 0.05);
+ vpDisplay::displayFrame(I, cMo, cam, 0.05);
}
}
@@ -494,33 +523,36 @@ main(int argc, const char ** argv)
}
}
- if(computeCovariance) {
+ if (computeCovariance) {
std::cout << "Covariance matrix: \n" << tracker.getCovarianceMatrix() << std::endl << std::endl;
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
+
+ std::cout << "Reached last frame: " << reader.getFrameIndex() << std::endl;
+
if (opt_click_allowed && !quit) {
vpDisplay::getClick(I);
}
reader.close();
-#if defined (VISP_HAVE_XML2)
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
+#if defined(VISP_HAVE_XML2)
+ // Cleanup memory allocated by xml library used to parse the xml config
+ // file in vpMbKltTracker::loadConfigFile()
vpXmlParser::cleanup();
#endif
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
- // Cleanup memory allocated by Coin library used to load a vrml model in vpMbKltTracker::loadModel()
- // We clean only if Coin was used.
- if(! cao3DModel)
+ // Cleanup memory allocated by Coin library used to load a vrml model in
+ // vpMbKltTracker::loadModel() We clean only if Coin was used.
+ if (!cao3DModel)
SoDB::finish();
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -530,9 +562,10 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run this example." << std::endl;
+ std::cout << "visp_mbt, visp_gui modules and OpenCV are required to run "
+ "this example."
+ << std::endl;
return 0;
-
}
#endif
diff --git a/example/tracking/templateTracker.cpp b/example/tracking/templateTracker.cpp
index 482c5b5..efb1072 100644
--- a/example/tracking/templateTracker.cpp
+++ b/example/tracking/templateTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,44 +47,44 @@
#include <iostream>
#include <visp3/core/vpConfig.h>
-#if defined(VISP_HAVE_MODULE_TT) && defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_MODULE_TT) && defined(VISP_HAVE_DISPLAY)
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/tt/vpTemplateTrackerSSD.h>
+#include <visp3/tt/vpTemplateTrackerSSDESM.h>
#include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
#include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
-#include <visp3/tt/vpTemplateTrackerSSDESM.h>
#include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
#include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
#include <visp3/tt/vpTemplateTrackerWarpAffine.h>
#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
#include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
+#include <visp3/tt/vpTemplateTrackerWarpRT.h>
#include <visp3/tt/vpTemplateTrackerWarpSRT.h>
#include <visp3/tt/vpTemplateTrackerWarpTranslation.h>
-#include <visp3/tt/vpTemplateTrackerWarpRT.h>
#ifdef VISP_HAVE_MODULE_TT_MI
-# include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
-# include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
-# include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
-# include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIESM.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
#endif
-#define GETOPTARGS "cdhi:l:pt:w:"
+#define GETOPTARGS "cdhi:l:pt:w:"
#ifndef DOXYGEN_SHOULD_SKIP_THIS
typedef enum {
@@ -116,13 +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);
+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)
{
fprintf(stdout, "\n\
Example of template tracking.\n\
@@ -130,14 +131,14 @@ Example of template tracking.\n\
SYNOPSIS\n\
%s [-i <test image path>] [-c] [-d] [-p] \n\
[-w <warp type>] [-t <tracker type>] \n\
- [-l <last frame number>] [-h]\n", name );
+ [-l <last frame number>] [-h]\n", name);
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> \n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/cube/image%%04d.pgm\". These \n\
+ \"cube/image%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -165,8 +166,8 @@ 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, (int)WARP_TRANSLATION, (int)WARP_RT);
+ %d : Translation\n\n", (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT,
+ (int)WARP_TRANSLATION, (int)WARP_RT);
#else
fprintf(stdout, "\n\
-w <warp type=[0,1,2,3,4]> %d\n\
@@ -176,8 +177,8 @@ 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, (int)WARP_TRANSLATION);
+ %d : Translation\n\n", (int)warp_type, (int)WARP_AFFINE, (int)WARP_HOMOGRAPHY, (int)WARP_HOMOGRAPHY_SL3, (int)WARP_SRT,
+ (int)WARP_TRANSLATION);
#endif
#ifdef VISP_HAVE_MODULE_TT_MI
@@ -194,12 +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\
@@ -211,14 +210,12 @@ 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\
+ fprintf(stdout, "\n\
-p\n\
Enable pyramidal tracking.\n\
\n\
@@ -229,27 +226,44 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL, warp_type, tracker_type, last_frame); return false; break;
- case 'i': ipath = optarg_; break;
- case 'l': last_frame = (long)atoi(optarg_); break;
- case 'p': pyramidal = true; break;
- case 't': tracker_type = (TrackerType)atoi(optarg_); break;
- case 'w': warp_type = (WarpType)atoi(optarg_); break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL, warp_type, tracker_type, last_frame);
+ return false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'l':
+ last_frame = (long)atoi(optarg_);
+ break;
+ case 'p':
+ pyramidal = true;
+ break;
+ case 't':
+ tracker_type = (TrackerType)atoi(optarg_);
+ break;
+ case 'w':
+ warp_type = (WarpType)atoi(optarg_);
+ break;
default:
usage(argv[0], optarg_, warp_type, tracker_type, last_frame);
- return false; break;
+ return false;
+ break;
}
}
@@ -262,7 +276,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
if (tracker_type >= TRACKER_LAST) {
usage(argv[0], NULL, warp_type, tracker_type, last_frame);
std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument -t <tracker type> with \"tracker type\"=" << (int)tracker_type << std::endl << std::endl;
+ std::cerr << " Bad argument -t <tracker type> with \"tracker type\"=" << (int)tracker_type << std::endl
+ << std::endl;
return false;
}
if ((c == 1) || (c == -1)) {
@@ -276,8 +291,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -290,26 +304,25 @@ main(int argc, const char ** argv)
WarpType opt_warp_type = WARP_AFFINE;
long opt_last_frame = 30;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
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);
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() ){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
<< std::endl;
@@ -319,9 +332,9 @@ main(int argc, const char ** argv)
// Get the option values
if (!opt_ipath.empty())
- ipath = vpIoTools::createFilePath(opt_ipath, "ViSP-images/mire-2/image.%04d.pgm");
+ ipath = vpIoTools::createFilePath(opt_ipath, "mire-2/image.%04d.pgm");
else
- ipath = vpIoTools::createFilePath(env_ipath, "ViSP-images/mire-2/image.%04d.pgm");
+ ipath = vpIoTools::createFilePath(env_ipath, "mire-2/image.%04d.pgm");
vpImage<unsigned char> I;
vpVideoReader reader;
@@ -329,18 +342,17 @@ main(int argc, const char ** argv)
reader.setFileName(ipath.c_str());
reader.setFirstFrameIndex(1);
reader.setLastFrameIndex(opt_last_frame);
- try{
+ try {
reader.open(I);
- }catch(...){
+ } catch (...) {
std::cout << "Cannot open sequence: " << ipath << std::endl;
return -1;
}
reader.acquire(I);
vpDisplay *display = NULL;
- if (opt_display)
- {
- // initialise a display
+ if (opt_display) {
+// initialise a display
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
@@ -355,43 +367,77 @@ main(int argc, const char ** argv)
opt_display = false;
#endif
#if (defined VISP_HAVE_DISPLAY)
- display->init(I, 100, 100, "Test tracking") ;
+ display->init(I, 100, 100, "Test tracking");
#endif
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
vpTemplateTrackerWarp *warp = NULL;
- switch(opt_warp_type) {
- case WARP_AFFINE: warp = new vpTemplateTrackerWarpAffine; break;
- case WARP_HOMOGRAPHY: warp = new vpTemplateTrackerWarpHomography; break;
- case WARP_HOMOGRAPHY_SL3: warp = new vpTemplateTrackerWarpHomographySL3; break;
- case WARP_SRT: warp = new vpTemplateTrackerWarpSRT; break;
- case WARP_TRANSLATION: warp = new vpTemplateTrackerWarpTranslation; break;
+ switch (opt_warp_type) {
+ case WARP_AFFINE:
+ warp = new vpTemplateTrackerWarpAffine;
+ break;
+ case WARP_HOMOGRAPHY:
+ warp = new vpTemplateTrackerWarpHomography;
+ break;
+ case WARP_HOMOGRAPHY_SL3:
+ warp = new vpTemplateTrackerWarpHomographySL3;
+ break;
+ case WARP_SRT:
+ warp = new vpTemplateTrackerWarpSRT;
+ break;
+ case WARP_TRANSLATION:
+ warp = new vpTemplateTrackerWarpTranslation;
+ break;
#ifdef VISP_HAVE_MODULE_TT_MI
- case WARP_RT: warp = new vpTemplateTrackerWarpRT; break;
+ case WARP_RT:
+ warp = new vpTemplateTrackerWarpRT;
+ break;
#endif
- default: return 0;
+ default:
+ return 0;
}
vpTemplateTracker *tracker = NULL;
- switch(opt_tracker_type) {
- case TRACKER_SSD_ESM: tracker = new vpTemplateTrackerSSDESM(warp); break;
- case TRACKER_SSD_FORWARD_ADDITIONAL: tracker = new vpTemplateTrackerSSDForwardAdditional(warp); break;
- case TRACKER_SSD_FORWARD_COMPOSITIONAL: tracker = new vpTemplateTrackerSSDForwardCompositional(warp); break;
- case TRACKER_SSD_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerSSDInverseCompositional(warp); break;
- case TRACKER_ZNCC_FORWARD_ADDITIONEL: tracker = new vpTemplateTrackerZNCCForwardAdditional(warp); break;
- case TRACKER_ZNCC_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerZNCCInverseCompositional(warp); break;
+ switch (opt_tracker_type) {
+ case TRACKER_SSD_ESM:
+ tracker = new vpTemplateTrackerSSDESM(warp);
+ break;
+ case TRACKER_SSD_FORWARD_ADDITIONAL:
+ tracker = new vpTemplateTrackerSSDForwardAdditional(warp);
+ break;
+ case TRACKER_SSD_FORWARD_COMPOSITIONAL:
+ tracker = new vpTemplateTrackerSSDForwardCompositional(warp);
+ break;
+ case TRACKER_SSD_INVERSE_COMPOSITIONAL:
+ tracker = new vpTemplateTrackerSSDInverseCompositional(warp);
+ break;
+ case TRACKER_ZNCC_FORWARD_ADDITIONEL:
+ tracker = new vpTemplateTrackerZNCCForwardAdditional(warp);
+ break;
+ case TRACKER_ZNCC_INVERSE_COMPOSITIONAL:
+ tracker = new vpTemplateTrackerZNCCInverseCompositional(warp);
+ break;
#ifdef VISP_HAVE_MODULE_TT_MI
- case TRACKER_MI_ESM: tracker = new vpTemplateTrackerMIESM(warp); break;
- case TRACKER_MI_FORWARD_ADDITIONAL: tracker = new vpTemplateTrackerMIForwardAdditional(warp); break;
- case TRACKER_MI_FORWARD_COMPOSITIONAL: tracker = new vpTemplateTrackerMIForwardCompositional(warp); break;
- case TRACKER_MI_INVERSE_COMPOSITIONAL: tracker = new vpTemplateTrackerMIInverseCompositional(warp); break;
+ case TRACKER_MI_ESM:
+ tracker = new vpTemplateTrackerMIESM(warp);
+ break;
+ case TRACKER_MI_FORWARD_ADDITIONAL:
+ tracker = new vpTemplateTrackerMIForwardAdditional(warp);
+ break;
+ case TRACKER_MI_FORWARD_COMPOSITIONAL:
+ tracker = new vpTemplateTrackerMIForwardCompositional(warp);
+ break;
+ case TRACKER_MI_INVERSE_COMPOSITIONAL:
+ tracker = new vpTemplateTrackerMIInverseCompositional(warp);
+ break;
#endif
- default: return 0;
+ default:
+ return 0;
}
- tracker->setSampling(2,2);
+ tracker->setSampling(2, 2);
tracker->setLambda(0.001);
tracker->setThresholdGradient(60.);
tracker->setIterationMax(800);
@@ -404,28 +450,33 @@ main(int argc, const char ** argv)
else {
std::vector<vpImagePoint> v_ip;
vpImagePoint ip;
- ip.set_ij(166, 54); v_ip.push_back(ip);
- ip.set_ij(284, 55); v_ip.push_back(ip);
- ip.set_ij(259, 284); v_ip.push_back(ip); // ends the first triangle
- ip.set_ij(259, 284); v_ip.push_back(ip); // start the second triangle
- ip.set_ij(149, 240); v_ip.push_back(ip);
- ip.set_ij(167, 58); v_ip.push_back(ip);
+ ip.set_ij(166, 54);
+ v_ip.push_back(ip);
+ ip.set_ij(284, 55);
+ v_ip.push_back(ip);
+ ip.set_ij(259, 284);
+ v_ip.push_back(ip); // ends the first triangle
+ ip.set_ij(259, 284);
+ v_ip.push_back(ip); // start the second triangle
+ ip.set_ij(149, 240);
+ v_ip.push_back(ip);
+ ip.set_ij(167, 58);
+ v_ip.push_back(ip);
tracker->initFromPoints(I, v_ip, false);
}
- while (! reader.end())
- {
- std::cout << "Process image number " << reader.getFrameIndex() << std::endl;
+ while (!reader.end()) {
// Acquire a new image
reader.acquire(I);
+ std::cout << "Process image number " << reader.getFrameIndex() << std::endl;
// Display the image
vpDisplay::display(I);
// Track the template
tracker->track(I);
// Simulate a re-init
- if (reader.getFrameIndex() == 10){
+ if (reader.getFrameIndex() == 10) {
std::cout << "re-init simulation" << std::endl;
if (opt_click_allowed)
vpDisplay::getClick(I);
@@ -436,39 +487,44 @@ main(int argc, const char ** argv)
vpDisplay::displayText(I, 10, 10, "Re-init simulation", vpColor::red);
vpDisplay::flush(I);
tracker->initClick(I, delaunay);
- }
- else {
+ } else {
std::vector<vpImagePoint> v_ip;
vpImagePoint ip;
- ip.set_ij(146, 60); v_ip.push_back(ip);
- ip.set_ij(254, 74); v_ip.push_back(ip);
- ip.set_ij(228, 288); v_ip.push_back(ip); // ends the first triangle
- ip.set_ij(228, 288); v_ip.push_back(ip); // start the second triangle
- ip.set_ij(126, 242); v_ip.push_back(ip);
- ip.set_ij(146, 60); v_ip.push_back(ip);
+ ip.set_ij(146, 60);
+ v_ip.push_back(ip);
+ ip.set_ij(254, 74);
+ v_ip.push_back(ip);
+ ip.set_ij(228, 288);
+ v_ip.push_back(ip); // ends the first triangle
+ ip.set_ij(228, 288);
+ v_ip.push_back(ip); // start the second triangle
+ ip.set_ij(126, 242);
+ v_ip.push_back(ip);
+ ip.set_ij(146, 60);
+ v_ip.push_back(ip);
tracker->initFromPoints(I, v_ip, false);
}
}
- // Display the template
+// Display the template
#if 1
tracker->display(I, vpColor::red, 3);
#else
vpTemplateTrackerZone zoneWarped_, zoneRef_ = tracker->getZoneRef();
vpTemplateTrackerWarp *warp_ = tracker->getWarp();
- vpColVector p_= tracker->getp();
+ vpColVector p_ = tracker->getp();
warp_->warpZone(zoneRef_, p_, zoneWarped_);
zoneWarped_.display(I, vpColor::red, 3);
zoneRef_.display(I, vpColor::green, 3);
#endif
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
if (opt_click_allowed) {
- vpDisplay::displayText(I, 10,10, "A click to exit...", vpColor::red);
- vpDisplay::flush(I) ;
- vpDisplay::getClick(I) ;
+ vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red);
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I);
}
reader.close();
if (display)
@@ -478,9 +534,7 @@ main(int argc, const char ** argv)
delete tracker;
return 0;
- }
- catch(vpException &e)
- {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return -1;
}
diff --git a/example/tracking/trackDot.cpp b/example/tracking/trackDot.cpp
index f0ef2af..9db368e 100644
--- a/example/tracking/trackDot.cpp
+++ b/example/tracking/trackDot.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,41 +43,41 @@
\brief test dot tracking on an image sequence using vpDot.
*/
-
/*!
\example trackDot.cpp
Example of dot tracking on an image sequence using vpDot.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/blob/vpDot.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/blob/vpDot.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
// List of allowed command line options
-#define GETOPTARGS "cdf:i:n:p:s:h"
+#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, unsigned &step, bool &click_allowed, bool &display);
+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);
/*!
@@ -91,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\
@@ -107,7 +108,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
+ \"mire-2/image.%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -141,8 +142,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), ppath.c_str(), first, nimages, step);
+ Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -164,27 +164,44 @@ 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, bool &click_allowed, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
- return false; break;
+ return false;
+ break;
}
}
@@ -199,9 +216,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -216,17 +231,18 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
+ opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -237,8 +253,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -246,15 +261,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -264,14 +277,13 @@ main(int argc, const char ** argv)
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
unsigned iter = opt_first;
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -282,21 +294,20 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
@@ -306,27 +317,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -339,27 +346,26 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
// by using setGraphics, we request to see the all the pixel of the dot
- vpDot d ;
+ vpDot d;
if (opt_display) {
// by using setGraphics, we request to see the all the pixel of the dot
// in green on the screen.
// It uses the overlay image plane.
// The default of this setting is that it is time consumming
- d.setGraphics(true) ;
- }
- else {
- d.setGraphics(false) ;
+ d.setGraphics(true);
+ } else {
+ d.setGraphics(false);
}
// we also request to compute the dot moment m00, m10, m01, m11, m20, m02
d.setComputeMoments(true);
@@ -370,28 +376,26 @@ main(int argc, const char ** argv)
// if no other parameters are given to the iniTracking(..) method
// a right mouse click on the dot is expected
std::cout << "Click on a white dot you want to track..." << std::endl;
- d.initTracking(I) ;
- }
- else {
+ d.initTracking(I);
+ } else {
// dot location can also be specified explicitely in the
// initTracking method : d.initTracking(I,ip) where ip is the
// image point from which the dot is searched
vpImagePoint ip;
- ip.set_u( 160 );
- ip.set_v( 212 );
- d.initTracking(I, ip) ;
+ ip.set_u(160);
+ ip.set_v(212);
+ d.initTracking(I, ip);
}
vpImagePoint cog;
- while (iter < opt_first + opt_nimages*opt_step) {
+ while (iter < opt_first + opt_nimages * opt_step) {
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// read the image
@@ -400,20 +404,17 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
std::cout << "Tracking on image: " << filename << std::endl;
// track the dot
double time = vpTime::measureTimeMs();
- d.track(I) ;
+ 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 << "Moments:" << std::endl;
@@ -431,7 +432,7 @@ main(int argc, const char ** argv)
if (0) {
std::list<vpImagePoint> edges = d.getEdges();
std::list<vpImagePoint>::const_iterator it;
- for(it = edges.begin(); it != edges.end(); ++it) {
+ for (it = edges.begin(); it != edges.end(); ++it) {
vpDisplay::displayPoint(I, *it, vpColor::blue);
}
}
@@ -439,22 +440,20 @@ main(int argc, const char ** argv)
if (opt_display) {
// display a red cross (size 10) in the image at the dot center
// of gravity location
- vpDisplay::displayCross(I, cog, 10, vpColor::red) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::red);
// flush the X11 buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- iter ++;
+ iter++;
}
-
if (opt_display && opt_click_allowed) {
std::cout << "\nA click to exit..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -464,7 +463,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_blob module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_blob module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackDot2.cpp b/example/tracking/trackDot2.cpp
index 2fe3877..9ef41d8 100644
--- a/example/tracking/trackDot2.cpp
+++ b/example/tracking/trackDot2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,36 +42,37 @@
\brief Example of dot tracking on an image sequence using vpDot2.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/blob/vpDot2.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/blob/vpDot2.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
// List of allowed command line options
-#define GETOPTARGS "cdf:i:n:p:s:h"
+#define GETOPTARGS "cdf:i:n:p:s:h"
-//int gsl_warnings_off;
+// 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, unsigned &step, bool &click_allowed, bool &display);
+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
Example of dot tracking on an image sequence using vpDot2.
@@ -90,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\
@@ -106,7 +108,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
+ \"mire-2/image.%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -140,8 +142,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), ppath.c_str(), first, nimages, step);
+ Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -163,27 +164,44 @@ 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, bool &click_allowed, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
- return false; break;
+ return false;
+ break;
}
}
@@ -198,8 +216,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -214,17 +231,18 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
+ opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -235,8 +253,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -244,15 +261,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -262,14 +277,13 @@ main(int argc, const char ** argv)
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
unsigned iter = opt_first;
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -280,21 +294,20 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
@@ -304,27 +317,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -337,23 +346,23 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
// define the vpDot structure.
- // vpDot and vpDot2 correspond to two different algorithms designed to track
- // a dot. vpDot is based on recurse connex componants (all the pixels of the
- // dot are parsed), while vpDot2 is based on freeman chain code (only the
- // contour of the dot is parsed)
+ // vpDot and vpDot2 correspond to two different algorithms designed to
+ // track a dot. vpDot is based on recurse connex componants (all the
+ // pixels of the dot are parsed), while vpDot2 is based on freeman chain
+ // code (only the contour of the dot is parsed)
- vpDot2 d ;
+ vpDot2 d;
vpImagePoint cog;
if (opt_display) {
@@ -362,15 +371,14 @@ main(int argc, const char ** argv)
// It uses the overlay image plane.
// The default of this setting is that it is time consumming
- d.setGraphics(true) ;
- }
- else {
+ d.setGraphics(true);
+ } else {
- d.setGraphics(false) ;
+ d.setGraphics(false);
}
- // We want to track an ellipsoid shape. If you want to track a non ellipsoid
- // object, use d.setEllipsoidShape(0);
- // we also request to compute the dot moment m00, m10, m01, m11, m20, m02
+ // We want to track an ellipsoid shape. If you want to track a non
+ // ellipsoid object, use d.setEllipsoidShape(0); we also request to
+ // compute the dot moment m00, m10, m01, m11, m20, m02
d.setComputeMoments(true);
d.setGrayLevelPrecision(0.90);
@@ -381,21 +389,19 @@ main(int argc, const char ** argv)
// point from which the dot is searched
if (opt_display && opt_click_allowed) {
- std::cout << "Click on a dot to track it."<< std::endl;
- d.initTracking(I) ;
- }
- else {
+ std::cout << "Click on a dot to track it." << std::endl;
+ d.initTracking(I);
+ } else {
vpImagePoint ip;
- ip.set_u( 160 );
- ip.set_v( 212 );
- d.initTracking(I, ip) ;
+ ip.set_u(160);
+ ip.set_v(212);
+ d.initTracking(I, ip);
}
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 << " 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;
@@ -417,15 +423,14 @@ main(int argc, const char ** argv)
std::cout << " gray level precision: " << d.getGrayLevelPrecision() << std::endl;
}
- while (iter < opt_first + opt_nimages*opt_step) {
+ while (iter < opt_first + opt_nimages * opt_step) {
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// read the image
@@ -440,19 +445,17 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
std::cout << "Tracking on image: " << filename << std::endl;
double time = vpTime::measureTimeMs();
- d.track(I) ;
+ 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 << " 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;
@@ -471,37 +474,35 @@ 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) {
std::list<vpImagePoint> edges;
d.getEdges(edges);
std::list<vpImagePoint>::const_iterator it;
- for(it = edges.begin(); it != edges.end(); ++it) {
+ for (it = edges.begin(); it != edges.end(); ++it) {
vpDisplay::displayPoint(I, *it, vpColor::blue);
}
}
// display a green cross (size 10) in the image at the dot center
// of gravity location
- vpDisplay::displayCross(I, cog, 10, vpColor::green) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
// flush the X11 buffer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
- iter ++;
+ iter++;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nA click to exit..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -511,7 +512,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackDot2WithAutoDetection.cpp b/example/tracking/trackDot2WithAutoDetection.cpp
index 6670378..28c214b 100644
--- a/example/tracking/trackDot2WithAutoDetection.cpp
+++ b/example/tracking/trackDot2WithAutoDetection.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,37 +47,37 @@
Example of auto detection of dots using vpDot2.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/blob/vpDot2.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/blob/vpDot2.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:p:f:n:s:S:G:E:h"
+#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, 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);
+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 &click_allowed, bool &display);
/*!
@@ -95,9 +96,9 @@ 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, double ellipsoidShapePrecision)
+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\
Test auto detection of dots using vpDot2.\n\
@@ -113,7 +114,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\"\n\
+ \"mire-2/image.%%04d.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -157,9 +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\
@@ -195,41 +195,63 @@ grayLevelPrecision, ellipsoidShapePrecision );
\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 &click_allowed, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'S': sizePrecision = atof(optarg_);break;
- case 'G': grayLevelPrecision = atof(optarg_);break;
- case 'E': ellipsoidShapePrecision = atof(optarg_);break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step,
- sizePrecision,grayLevelPrecision,ellipsoidShapePrecision);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'S':
+ sizePrecision = atof(optarg_);
+ break;
+ case 'G':
+ grayLevelPrecision = atof(optarg_);
+ break;
+ case 'E':
+ ellipsoidShapePrecision = atof(optarg_);
+ break;
+ case 'h':
+ 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);
- return false; break;
+ usage(argv[0], optarg_, ipath, ppath, first, nimages, step, sizePrecision, grayLevelPrecision,
+ ellipsoidShapePrecision);
+ return false;
+ break;
}
}
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;
@@ -238,9 +260,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -258,18 +278,18 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step,opt_sizePrecision,opt_grayLevelPrecision,
- opt_ellipsoidShapePrecision, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ 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);
}
// Get the option values
@@ -280,8 +300,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -289,31 +308,28 @@ main(int argc, const char ** argv)
}
// 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);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ 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);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
std::ostringstream s;
char cfilename[FILENAME_MAX];
unsigned iter = opt_first; // Image number
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -324,21 +340,20 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// Read the PGM image named "filename" on the disk, and put the
@@ -347,29 +362,24 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or your -p " << opt_ppath << " option " <<std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or your -p " << opt_ppath << " option " << std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
-
- // We open a window using either GTK, X11 or GDI.
+// We open a window using either GTK, X11 or GDI.
#if defined VISP_HAVE_GTK
vpDisplayGTK display;
#elif defined VISP_HAVE_X11
@@ -382,32 +392,31 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
// Dot declaration
- vpDot2 d ;
+ vpDot2 d;
d.setGraphics(true);
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) ;
+ d.initTracking(I);
if (opt_display) {
vpImagePoint cog;
cog = d.getCog();
- vpDisplay::displayCross(I, cog, 10,vpColor::green) ;
- vpDisplay::flush(I) ;
+ vpDisplay::displayCross(I, cog, 10, vpColor::green);
+ vpDisplay::flush(I);
}
d.setSizePrecision(opt_sizePrecision);
d.setEllipsoidShapePrecision(opt_ellipsoidShapePrecision);
@@ -415,13 +424,12 @@ main(int argc, const char ** argv)
printf(" width : %lf\n", d.getWidth());
printf(" height: %lf\n", d.getHeight());
printf(" area: %lf\n", d.getArea());
- printf(" gray level min: %d\n", d.getGrayLevelMin());
- printf(" gray level max: %d\n", d.getGrayLevelMax());
+ printf(" gray level min: %u\n", d.getGrayLevelMin());
+ 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());
- }
- else{
+ } else {
// Set dot characteristics for the auto detection
d.setGraphics(true);
d.setWidth(15.0);
@@ -434,18 +442,16 @@ main(int argc, const char ** argv)
d.setEllipsoidShapePrecision(opt_ellipsoidShapePrecision);
}
- while (iter < opt_first + opt_nimages*opt_step)
- {
+ while (iter < opt_first + opt_nimages * opt_step) {
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// read the image
@@ -453,54 +459,51 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
std::cout << "Search dots in image" << filename << std::endl;
std::list<vpDot2> list_d;
- d.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), list_d) ;
+ d.searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), list_d);
- if( list_d.empty() ) {
+ if (list_d.empty()) {
std::cout << "Dot auto detection did not work." << std::endl;
- return(-1) ;
- }
- else {
+ return (-1);
+ } else {
std::cout << std::endl << list_d.size() << " dots are detected" << std::endl;
if (opt_display) {
- int i=0;
+ int i = 0;
// Parse all founded dots for display
- for (std::list<vpDot2>::const_iterator it = list_d.begin(); it != list_d.end(); ++ it)
- {
+ for (std::list<vpDot2>::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) ;
+ vpDisplay::displayCross(I, cog, 16, vpColor::blue, 3);
}
vpDisplay::flush(I);
}
}
- // If click is allowed, wait for a mouse click to launch the next iteration
+ // If click is allowed, wait for a mouse click to launch the next
+ // iteration
if (opt_display && opt_click_allowed) {
std::cout << "\nA click to continue..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- iter += opt_step ;
+ iter += opt_step;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nA click to exit..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -510,7 +513,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackKltOpencv.cpp b/example/tracking/trackKltOpencv.cpp
index 4269e75..b24d38d 100644
--- a/example/tracking/trackKltOpencv.cpp
+++ b/example/tracking/trackKltOpencv.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,35 +42,36 @@
\brief Example of KLT tracking using OpenCV library.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
#include <vector>
-#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)
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
-#include <visp3/klt/vpKltOpencv.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/klt/vpKltOpencv.h>
// List of allowed command line options
-#define GETOPTARGS "cdf:i:n:p:s:h"
+#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, unsigned &step, bool &click_allowed, bool &display);
+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
@@ -91,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\
@@ -107,7 +109,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/mire-2/image.%%04d.pgm\". These \n\
+ \"mire-2/image.%%04d.pgm\". These \n\
images come from ViSP-images-x.y.z.tar.gz available \n\
on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -141,8 +143,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str(), ppath.c_str(), first, nimages, step);
+ Print the help.\n", ipath.c_str(), ppath.c_str(), first, nimages, step);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -164,27 +165,44 @@ 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, bool &click_allowed, bool &display)
+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_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step);
- return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step);
- return false; break;
+ return false;
+ break;
}
}
@@ -199,8 +217,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -215,17 +232,18 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_click_allowed,
+ opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -236,8 +254,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -245,15 +262,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -263,9 +278,9 @@ main(int argc, const char ** argv)
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> vpI ; // This is a ViSP image used for display only
+ vpImage<unsigned char> vpI; // This is a ViSP image used for display only
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage * cvI = NULL; // This is an OpenCV IPL image used by the tracker
+ IplImage *cvI = NULL; // This is an OpenCV IPL image used by the tracker
#else
cv::Mat cvI;
#endif
@@ -274,8 +289,7 @@ main(int argc, const char ** argv)
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -286,20 +300,19 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2");
+ dirname = vpIoTools::createFilePath(ipath, "mire-2");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
@@ -309,29 +322,25 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
std::cout << "Load: " << filename << std::endl;
// Load a ViSP image used for the display
- vpImageIo::read(vpI, filename) ;
+ vpImageIo::read(vpI, filename);
vpImageConvert::convert(vpI, cvI);
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -344,27 +353,27 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(vpI, 100, 100,"Display...") ;
+ display.init(vpI, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(vpI) ;
- vpDisplay::flush(vpI) ;
+ vpDisplay::display(vpI);
+ vpDisplay::flush(vpI);
}
// KLT tracker
vpKltOpencv tracker;
// Event manager
- //tracker.setOnNewFeature(&newFeature);
- //tracker.setOnFeatureLost(&lostFeature);
- //tracker.setIsFeatureValid(&isValid);
+ // tracker.setOnNewFeature(&newFeature);
+ // tracker.setOnFeatureLost(&lostFeature);
+ // tracker.setIsFeatureValid(&isValid);
// Tracker parameters
tracker.setTrackerId(1);
- //tracker.setOnMeasureFeature(&modifyFeature);
+ // tracker.setOnMeasureFeature(&modifyFeature);
tracker.setMaxFeatures(200);
tracker.setWindowSize(10);
tracker.setQuality(0.01);
@@ -383,19 +392,18 @@ main(int argc, const char ** argv)
}
// tracking is now initialized. We can start the tracker.
- while (iter < opt_first + opt_nimages*opt_step) {
+ while (iter < opt_first + opt_nimages * opt_step) {
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// read the image
- vpImageIo::read(vpI, filename) ;
+ vpImageIo::read(vpI, filename);
vpImageConvert::convert(vpI, cvI);
// track the dot and returns its coordinates in the image
@@ -406,50 +414,45 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display the image
- vpDisplay::display(vpI) ;
+ vpDisplay::display(vpI);
}
std::cout << "Tracking on image: " << filename << std::endl;
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
tracker.display(vpI, vpColor::red);
- vpDisplay::flush(vpI) ;
+ vpDisplay::flush(vpI);
}
iter += opt_step;
}
if (opt_display && opt_click_allowed) {
std::cout << "\nA click to exit..." << std::endl;
// Wait for a blocking mouse click
- vpDisplay::getClick(vpI) ;
+ vpDisplay::getClick(vpI);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("You do not have OpenCV functionalities...");
-}
+int main() { vpERROR_TRACE("You do not have OpenCV functionalities..."); }
#endif
#else
#include <iostream>
int main()
{
- std::cout << "visp_klt module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_klt module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
-
#endif
diff --git a/example/tracking/trackMeCircle.cpp b/example/tracking/trackMeCircle.cpp
index f16c309..a22c0d9 100644
--- a/example/tracking/trackMeCircle.cpp
+++ b/example/tracking/trackMeCircle.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,27 +50,28 @@
#include <visp3/core/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpColor.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/me/vpMeEllipse.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/me/vpMeEllipse.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
@@ -95,7 +97,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/circle/circle.pgm\"\n\
+ From this path read \"circle/circle.pgm\"\n\
image. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -109,8 +111,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -131,18 +132,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -157,9 +168,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -170,17 +179,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -191,8 +200,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -200,25 +208,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/circle");
+ dirname = vpIoTools::createFilePath(ipath, "circle");
// Build the name of the image file
filename = vpIoTools::createFilePath(dirname, "circle.pgm");
@@ -229,28 +235,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
-
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -263,32 +264,31 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
- vpMeEllipse E1 ;
+ vpMeEllipse E1;
- vpMe me ;
- me.setRange(20) ;
- me.setSampleStep(2) ;
- me.setPointsToTrack(60) ;
- me.setThreshold(15000) ;
+ vpMe me;
+ me.setRange(20);
+ me.setSampleStep(2);
+ me.setPointsToTrack(60);
+ me.setThreshold(15000);
- E1.setMe(&me) ;
- E1.setDisplay(vpMeSite::RANGE_RESULT) ;
+ E1.setMe(&me);
+ E1.setDisplay(vpMeSite::RANGE_RESULT);
// If click is allowed, wait for a mouse click to select the points
// on the ellipse
if (opt_display && opt_click_allowed) {
- E1.initTracking(I) ;
- }
- else {
+ E1.initTracking(I);
+ } else {
// Create a list of points to automate the test
std::vector<vpImagePoint> ip;
ip.push_back(vpImagePoint(39, 136));
@@ -297,29 +297,28 @@ main(int argc, const char ** argv)
ip.push_back(vpImagePoint(132, 72));
ip.push_back(vpImagePoint(145, 134));
- E1.initTracking(I, ip) ;
+ E1.initTracking(I, ip);
}
if (opt_display) {
- E1.display(I, vpColor::green) ;
+ E1.display(I, vpColor::green);
vpDisplay::flush(I);
}
- vpTRACE("sample step %f ",E1.getMe()->getSampleStep()) ;
+ vpTRACE("sample step %f ", E1.getMe()->getSampleStep());
std::cout << "Tracking on image: " << filename << std::endl;
- E1.track(I) ;
+ E1.track(I);
if (opt_display) {
vpDisplay::flush(I);
}
if (opt_display && opt_click_allowed) {
std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- std::cout <<"------------------------------------------------------------"<<std::endl;
+ std::cout << "------------------------------------------------------------" << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -329,7 +328,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackMeEllipse.cpp b/example/tracking/trackMeEllipse.cpp
index ccd5f24..19e606f 100644
--- a/example/tracking/trackMeEllipse.cpp
+++ b/example/tracking/trackMeEllipse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,32 +48,32 @@
Tracking of an ellipse using vpMe.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpColor.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/me/vpMeEllipse.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/me/vpMeEllipse.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
@@ -99,7 +100,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/ellipse-1/image.%%04d.pgm\"\n\
+ \"ellipse-1/image.%%04d.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -112,8 +113,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -134,18 +134,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -160,9 +170,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -173,16 +181,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -193,8 +202,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -202,25 +210,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image is
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/ellipse-1");
+ dirname = vpIoTools::createFilePath(ipath, "ellipse-1");
// Build the name of the image file
unsigned int iter = 1; // Image number
@@ -235,27 +241,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
+ vpImageIo::read(I, filename);
+ } catch (...) {
// an exception is thrown if an exception from readPGM has been caught
// here this will result in the end of the program
// Note that another error message has been printed from readPGM
// to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << 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);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -268,28 +270,28 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
- vpMeEllipse E1 ;
+ vpMeEllipse E1;
- vpMe me ;
- me.setRange(20) ;
- me.setSampleStep(2) ;
- me.setPointsToTrack(60) ;
- me.setThreshold(15000) ;
+ vpMe me;
+ me.setRange(20);
+ me.setSampleStep(2);
+ me.setPointsToTrack(60);
+ me.setThreshold(15000);
- E1.setMe(&me) ;
- E1.setDisplay(vpMeSite::RANGE_RESULT) ;
+ E1.setMe(&me);
+ E1.setDisplay(vpMeSite::RANGE_RESULT);
if (opt_click_allowed)
- E1.initTracking(I) ;
+ E1.initTracking(I);
else {
// Create a list of points to automate the test
std::vector<vpImagePoint> ip;
@@ -299,22 +301,21 @@ main(int argc, const char ** argv)
ip.push_back(vpImagePoint(243, 164));
ip.push_back(vpImagePoint(195, 329));
- E1.initTracking(I, ip) ;
+ E1.initTracking(I, ip);
}
if (opt_display) {
- E1.display(I, vpColor::green) ;
+ E1.display(I, vpColor::green);
}
- vpERROR_TRACE("sample step %f ",E1.getMe()->getSampleStep()) ;
- E1.track(I) ;
+ vpERROR_TRACE("sample step %f ", E1.getMe()->getSampleStep());
+ E1.track(I);
if (opt_display && opt_click_allowed) {
std::cout << "A click to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- std::cout <<"------------------------------------------------------------"<<std::endl;
+ std::cout << "------------------------------------------------------------" << std::endl;
-
- for (iter = 1 ; iter < 51 ; iter++) // initially : iter < 1500
+ for (iter = 1; iter < 51; iter++) // initially : iter < 1500
{
// set the new image name
s.str("");
@@ -325,23 +326,22 @@ main(int argc, const char ** argv)
vpImageIo::read(I, filename);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
- E1.track(I) ;
+ E1.track(I);
if (opt_display) {
- E1.display(I,vpColor::green) ;
- vpDisplay::flush(I) ;
+ E1.display(I, vpColor::green);
+ vpDisplay::flush(I);
}
}
if (opt_display && opt_click_allowed) {
std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -351,7 +351,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackMeLine.cpp b/example/tracking/trackMeLine.cpp
index 5482a16..f075d63 100644
--- a/example/tracking/trackMeLine.cpp
+++ b/example/tracking/trackMeLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,35 +48,36 @@
Tracking of a line using vpMe.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpColor.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/me/vpMeLine.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureLine.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
@@ -101,7 +103,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/line/image.%%04d.pgm\"\n\
+ From this path read \"line/image.%%04d.pgm\"\n\
images. \n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -115,8 +117,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -137,18 +138,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -163,9 +174,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -176,18 +185,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -198,8 +206,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -207,24 +214,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/line");
+ dirname = vpIoTools::createFilePath(ipath, "line");
// Build the name of the image file
unsigned int iter = 1; // Image number
@@ -239,27 +245,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -272,54 +274,53 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
- vpMeLine L1 ;
+ vpMeLine L1;
- vpMe me ;
- me.setRange(15) ;
- me.setPointsToTrack(160) ;
- me.setThreshold(15000) ;
+ vpMe me;
+ me.setRange(15);
+ me.setPointsToTrack(160);
+ me.setThreshold(15000);
- L1.setMe(&me) ;
- L1.setDisplay(vpMeSite::RANGE_RESULT) ;
+ L1.setMe(&me);
+ L1.setDisplay(vpMeSite::RANGE_RESULT);
if (opt_display && opt_click_allowed)
- L1.initTracking(I) ;
+ L1.initTracking(I);
else {
vpImagePoint ip1, ip2;
- ip1.set_i( 96 );
- ip1.set_j( 191 );
- ip2.set_i( 122 );
- ip2.set_j( 211 );
- L1.initTracking(I, ip1, ip2) ;
+ ip1.set_i(96);
+ ip1.set_j(191);
+ ip2.set_i(122);
+ ip2.set_j(211);
+ L1.initTracking(I, ip1, ip2);
}
if (opt_display)
- L1.display(I, vpColor::green) ;
+ L1.display(I, vpColor::green);
- L1.track(I) ;
+ L1.track(I);
if (opt_display && opt_click_allowed) {
std::cout << "A click to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- std::cout <<"----------------------------------------------------------"<<std::endl;
+ std::cout << "----------------------------------------------------------" << std::endl;
- vpFeatureLine l ;
+ vpFeatureLine l;
- vpCameraParameters cam ;
- vpImage<vpRGBa> Ic ;
- for (iter = 1 ; iter < 30 ; iter++)
- {
- std::cout <<"----------------------------------------------------------"<<std::endl;
+ vpCameraParameters cam;
+ vpImage<vpRGBa> Ic;
+ for (iter = 1; iter < 30; iter++) {
+ std::cout << "----------------------------------------------------------" << std::endl;
// set the new image name
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
@@ -328,32 +329,31 @@ main(int argc, const char ** argv)
vpImageIo::read(I, filename);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
std::cout << "Tracking on image: " << filename << std::endl;
- L1.track(I) ;
+ L1.track(I);
- vpTRACE("L1 : %f %f", L1.getRho(), vpMath::deg(L1.getTheta())) ;
- vpFeatureBuilder::create(l,cam,L1) ;
- vpTRACE("L1 : %f %f", l.getRho(), vpMath::deg(l.getTheta())) ;
+ vpTRACE("L1 : %f %f", L1.getRho(), vpMath::deg(L1.getTheta()));
+ vpFeatureBuilder::create(l, cam, L1);
+ vpTRACE("L1 : %f %f", l.getRho(), vpMath::deg(l.getTheta()));
if (opt_display) {
- L1.display(I,vpColor::green) ;
- vpDisplay::flush(I) ;
+ L1.display(I, vpColor::green);
+ vpDisplay::flush(I);
if (opt_click_allowed) {
std::cout << "A click to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
}
}
if (opt_display && opt_click_allowed) {
std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -364,7 +364,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/tracking/trackMeNurbs.cpp b/example/tracking/trackMeNurbs.cpp
index e02587d..1152750 100644
--- a/example/tracking/trackMeNurbs.cpp
+++ b/example/tracking/trackMeNurbs.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,32 +49,33 @@
Tracking of a nurbs using vpMe.
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpColor.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-#include <visp3/me/vpMeNurbs.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/me/vpMeNurbs.h>
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
@@ -100,7 +102,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read images \n\
- \"ViSP-images/ellipse-1/image.%%04d.pgm\"\n\
+ \"ellipse-1/image.%%04d.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -113,8 +115,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -135,18 +136,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -161,9 +172,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, bool &click_all
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -173,17 +182,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -194,8 +203,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -203,34 +211,32 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image is
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Set the path location of the image sequence
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/ellipse-1/image.%04d.pgm");
+ filename = vpIoTools::createFilePath(ipath, "ellipse-1/image.%04d.pgm");
// Build the name of the image file
vpVideoReader reader;
- //Initialize the reader and get the first frame.
+ // Initialize the reader and get the first frame.
reader.setFileName(filename.c_str());
reader.setFirstFrameIndex(1);
reader.open(I);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -243,83 +249,77 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
- vpMeNurbs nurbs ;
+ vpMeNurbs nurbs;
- vpMe me ;
- me.setRange(30) ;
- me.setSampleStep(5) ;
- me.setPointsToTrack(60) ;
- me.setThreshold(15000) ;
+ vpMe me;
+ me.setRange(30);
+ me.setSampleStep(5);
+ me.setPointsToTrack(60);
+ me.setThreshold(15000);
nurbs.setMe(&me);
- nurbs.setDisplay(vpMeSite::RANGE_RESULT) ;
+ nurbs.setDisplay(vpMeSite::RANGE_RESULT);
nurbs.setNbControlPoints(14);
- if (opt_click_allowed)
- {
+ if (opt_click_allowed) {
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
- {
+ } else {
// Create a list of points to automate the test
std::list<vpImagePoint> list;
- list.push_back(vpImagePoint(178,357));
- list.push_back(vpImagePoint(212,287));
- list.push_back(vpImagePoint(236,210));
+ list.push_back(vpImagePoint(178, 357));
+ list.push_back(vpImagePoint(212, 287));
+ list.push_back(vpImagePoint(236, 210));
list.push_back(vpImagePoint(240, 118));
list.push_back(vpImagePoint(210, 40));
- nurbs.initTracking(I, list) ;
+ nurbs.initTracking(I, list);
}
if (opt_display) {
- nurbs.display(I, vpColor::green) ;
+ nurbs.display(I, vpColor::green);
}
- nurbs.track(I) ;
+ nurbs.track(I);
if (opt_display && opt_click_allowed) {
std::cout << "A click to continue..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
- std::cout <<"------------------------------------------------------------"<<std::endl;
+ std::cout << "------------------------------------------------------------" << std::endl;
- for (int iter = 1 ; iter < 40 ; iter++)
- {
- //read the image
- reader.getFrame(I,iter);
+ for (int iter = 1; iter < 40; iter++) {
+ // read the image
+ reader.getFrame(I, iter);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
+ vpDisplay::display(I);
}
- //Track the nurbs
- nurbs.track(I) ;
-
+ // Track the nurbs
+ nurbs.track(I);
if (opt_display) {
- nurbs.display(I,vpColor::green) ;
- vpDisplay::flush(I) ;
+ nurbs.display(I, vpColor::green);
+ vpDisplay::flush(I);
vpTime::wait(100);
}
}
if (opt_display && opt_click_allowed) {
std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return 0;
}
@@ -329,7 +329,9 @@ main(int argc, const char ** argv)
int main()
{
- std::cout << "visp_me module or X11, GTK, GDI or OpenCV display functionalities are required..." << std::endl;
+ std::cout << "visp_me module or X11, GTK, GDI or OpenCV display "
+ "functionalities are required..."
+ << std::endl;
}
#endif
diff --git a/example/video/CMakeLists.txt b/example/video/CMakeLists.txt
index c4c4ffe..3c56857 100644
--- a/example/video/CMakeLists.txt
+++ b/example/video/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/video/imageSequenceReader.cpp b/example/video/imageSequenceReader.cpp
index e2a925f..a1faecc 100644
--- a/example/video/imageSequenceReader.cpp
+++ b/example/video/imageSequenceReader.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,25 +46,25 @@
Reading an image sequence using vpVideoReader class.
*/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/io/vpVideoReader.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#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"
+#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, bool &display);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, int &first, bool &click_allowed,
+ bool &display);
/*!
@@ -89,7 +90,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input images path> %s\n\
Set ViSP-images input path.\n\
- From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ From this path read \"cube/image.%%04d.pgm\"\n\
images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -112,11 +113,10 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), ppath.c_str());
+ Print the help.\n\n", ipath.c_str(), ppath.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -134,23 +134,38 @@ 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 &display)
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, int &first, bool &click_allowed,
+ bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, ppath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, ppath); return false; break;
+ usage(argv[0], optarg_, ipath, ppath);
+ return false;
+ break;
}
}
@@ -165,10 +180,7 @@ bool getOptions(int argc, const char **argv,
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -180,24 +192,24 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << " videoImageSequenceReader.cpp" <<std::endl << 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 << " reading an image sequence" << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -208,8 +220,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -217,42 +228,38 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " video path where test images are located." << std::endl << std::endl;
+ << " video path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
/////////////////////////////////////////////////////////////////////
+ // vpImage is a template class you can declare vpImage of ...
+ // everything...
+ vpImage<vpRGBa> I;
- // vpImage is a template class you can declare vpImage of ... everything...
- vpImage<vpRGBa> I ;
-
- //Create the video Reader
+ // Create the video Reader
vpVideoReader reader;
- if (opt_ppath.empty())
- {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/mire-2/image.%04d.pgm");
- }
- else
- {
+ if (opt_ppath.empty()) {
+ filename = vpIoTools::createFilePath(ipath, "mire-2/image.%04d.pgm");
+ } else {
filename.assign(opt_ppath);
}
- //Initialize the reader and get the first frame.
+ // Initialize the reader and get the first frame.
reader.setFileName(filename);
reader.setFirstFrameIndex(opt_first);
reader.open(I);
+ std::cout << "Current image number: " << reader.getFrameIndex() << std::endl;
- // We open a window using either X11, GTK, GDI or OpenCV.
+// We open a window using either X11, GTK, GDI or OpenCV.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -265,66 +272,61 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display video frame") ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ display.init(I, 100, 100, "Display video frame");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
- if (opt_display && opt_click_allowed)
- {
- std::cout << "Click on the image to read and display the second frame" << std::endl;
+ if (opt_display && opt_click_allowed) {
+ std::cout << "Click in the image to read and display the second frame" << std::endl;
vpDisplay::getClick(I);
}
- reader.getFrame(I,opt_first+1);
+ reader.getFrame(I, opt_first + 1);
+ std::cout << "Current image number (should be " << opt_first + 1 << "): " << reader.getFrameIndex() << std::endl;
- if (opt_display)
- {
- vpDisplay::display(I) ;
+ if (opt_display) {
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
std::cout << "Click on the image to read and display the last frame" << std::endl;
vpDisplay::getClick(I);
}
- reader.getFrame(I,reader.getLastFrameIndex());
+ reader.getFrame(I, reader.getLastFrameIndex());
+ std::cout << "Current image number (should be " << reader.getLastFrameIndex() << "): " << reader.getFrameIndex()
+ << std::endl;
- if (opt_display)
- {
- vpDisplay::display(I) ;
+ if (opt_display) {
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
std::cout << "Click to see the video" << std::endl;
vpDisplay::getClick(I);
}
int lastFrame = reader.getLastFrameIndex();
- for (int i = opt_first; i <= lastFrame; i++)
- {
- reader.getFrame(I,i);
- if (opt_display)
- {
- vpDisplay::display(I) ;
+ for (int i = opt_first; i <= lastFrame; i++) {
+ reader.getFrame(I, i);
+ std::cout << "Current image number: " << reader.getFrameIndex() << std::endl;
+ if (opt_display) {
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
}
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
std::cout << "Click to exit the test" << std::endl;
vpDisplay::getClick(I);
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -332,9 +334,7 @@ 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 3894bf5..52eda69 100644
--- a/example/video/videoReader.cpp
+++ b/example/video/videoReader.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,25 +47,25 @@
*/
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/io/vpVideoReader.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#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"
+#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);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed,
+ bool &display);
/*!
@@ -90,7 +91,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input video path> %s\n\
Set video input path.\n\
- From this path read \"ViSP-images/video/video.mpeg\"\n\
+ From this path read \"video/video.mpeg\"\n\
video.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -109,11 +110,10 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), ppath.c_str());
+ Print the help.\n\n", ipath.c_str(), ppath.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -130,22 +130,34 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, ppath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, ipath, ppath); return false; break;
+ usage(argv[0], optarg_, ipath, ppath);
+ return false;
+ break;
}
}
@@ -160,10 +172,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -174,25 +183,24 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- 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;
+ std::cout << " videoReader.cpp" << std::endl << std::endl;
+ std::cout << " reading a video file" << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -203,8 +211,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -212,42 +219,37 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " video path where test images are located." << std::endl << std::endl;
+ << " video path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
/////////////////////////////////////////////////////////////////////
+ // vpImage is a template class you can declare vpImage of ...
+ // everything...
+ vpImage<vpRGBa> I;
- // vpImage is a template class you can declare vpImage of ... everything...
- vpImage<vpRGBa> I ;
-
- //Create the video Reader
+ // Create the video Reader
vpVideoReader reader;
- if (opt_ppath.empty())
- {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/video/cube.mpeg");
- }
- else
- {
+ if (opt_ppath.empty()) {
+ filename = vpIoTools::createFilePath(ipath, "video/cube.mpeg");
+ } else {
filename.assign(opt_ppath);
}
- //Initialize the reader and get the first frame.
+ // Initialize the reader and get the first frame.
std::cout << "Process video in " << filename << std::endl;
reader.setFileName(filename);
reader.open(I);
- // We open a window using either X11, GTK, GDI or OpenCV.
+// We open a window using either X11, GTK, GDI or OpenCV.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -260,15 +262,15 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display video frame") ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ display.init(I, 100, 100, "Display video frame");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
// if (opt_display && opt_click_allowed)
// {
- // std::cout << "Click on the image to read and display the last key frame" << std::endl;
- // vpDisplay::getClick(I);
+ // std::cout << "Click on the image to read and display the last key
+ // frame" << std::endl; vpDisplay::getClick(I);
// }
//
// reader.getFrame(I,reader.getLastFrameIndex());
@@ -279,29 +281,25 @@ main(int argc, const char ** argv)
// vpDisplay::flush(I);
// }
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
std::cout << "Click to see the video" << std::endl;
vpDisplay::getClick(I);
}
- while (! reader.end() ) {
- std::cout << "Read frame: " << reader.getFrameIndex() << std::endl;
+ while (!reader.end()) {
reader.acquire(I);
- if (opt_display)
- {
- vpDisplay::display(I) ;
+ std::cout << "Display frame: " << reader.getFrameIndex() << std::endl;
+ if (opt_display) {
+ vpDisplay::display(I);
vpDisplay::flush(I);
}
}
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
std::cout << "Click to exit this example" << std::endl;
vpDisplay::getClick(I);
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -309,8 +307,7 @@ 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/CMakeLists.txt b/example/wireframe-simulator/CMakeLists.txt
index fd22367..33fda54 100644
--- a/example/wireframe-simulator/CMakeLists.txt
+++ b/example/wireframe-simulator/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/example/wireframe-simulator/wireframeSimulator.cpp b/example/wireframe-simulator/wireframeSimulator.cpp
index fc3770f..53302bc 100644
--- a/example/wireframe-simulator/wireframeSimulator.cpp
+++ b/example/wireframe-simulator/wireframeSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,20 +45,20 @@
#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpWireFrameSimulator.h>
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
#ifdef VISP_HAVE_DISPLAY
@@ -97,7 +98,6 @@ OPTIONS: Default\n\
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
-
/*!
Set the program options.
@@ -113,17 +113,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &display, bool &click)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -138,9 +146,7 @@ bool getOptions(int argc, const char **argv, bool &display, bool &click)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -148,20 +154,20 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_display, opt_click) == false) {
- exit (-1);
+ exit(-1);
}
/*
Three vpImage are created : one for the main camera and the others
for two external cameras
*/
- vpImage<vpRGBa> Iint(480,640,255);
- vpImage<vpRGBa> Iext1(480,640,255);
- vpImage<vpRGBa> Iext2(480,640,255);
+ vpImage<vpRGBa> Iint(480, 640, 255);
+ vpImage<vpRGBa> Iext1(480, 640, 255);
+ vpImage<vpRGBa> Iext2(480, 640, 255);
- /*
- Create a display for each different cameras.
- */
+/*
+Create a display for each different cameras.
+*/
#if defined VISP_HAVE_X11
vpDisplayX display[3];
#elif defined VISP_HAVE_OPENCV
@@ -176,12 +182,12 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(Iint, 100, 100,"The internal view") ;
- display[1].init(Iext1, 100, 100,"The first external view") ;
- display[2].init(Iext2, 100, 100,"The second external view") ;
- vpDisplay::setWindowPosition (Iint, 0, 0);
- vpDisplay::setWindowPosition (Iext1, 700, 0);
- vpDisplay::setWindowPosition (Iext2, 0, 550);
+ display[0].init(Iint, 100, 100, "The internal view");
+ display[1].init(Iext1, 100, 100, "The first external view");
+ display[2].init(Iext2, 100, 100, "The second external view");
+ vpDisplay::setWindowPosition(Iint, 0, 0);
+ vpDisplay::setWindowPosition(Iext1, 700, 0);
+ vpDisplay::setWindowPosition(Iext2, 0, 550);
vpDisplay::display(Iint);
vpDisplay::flush(Iint);
vpDisplay::display(Iext1);
@@ -190,22 +196,27 @@ main(int argc, const char ** argv)
vpDisplay::flush(Iext2);
}
- //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);
+ // 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);
- //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)));
+ // 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)));
- //Declaration of the simulator
+ // Declaration of the simulator
vpWireFrameSimulator sim;
/*
- Set the scene. It enables to choose the shape of the object and the shape of the desired object which is
- displayed in the main camera view. It exists several objects in ViSP. See the html documentation of the
+ Set the scene. It enables to choose the shape of the object and the shape
+ of the desired object which is displayed in the main camera view. It
+ exists several objects in ViSP. See the html documentation of the
simulator class to have the complete list.
-
- Note : if you don't want to have a desired object displayed in the main camera view you can use the initObject Method.
-
- 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.
+
+ Note : if you don't want to have a desired object displayed in the main
+ camera view you can use the initObject Method.
+
+ 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);
@@ -218,37 +229,38 @@ main(int argc, const char ** argv)
sim.setDesiredViewColor(vpColor::red);
sim.setCameraColor(vpColor::green);
/*
- Set the current and the desired position of the camera relative to the object.
+ Set the current and the desired position of the camera relative to the
+ object.
*/
- sim.setCameraPositionRelObj(cMo) ;
+ sim.setCameraPositionRelObj(cMo);
sim.setDesiredCameraPosition(cdMo);
/*
- Set the main external camera's position relative to the world reference frame.
- More information about the different frames are given in the html documentation.
+ Set the main external camera's position relative to the world reference
+ 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);
/*
Set the parameters of the cameras (internal and external)
*/
- vpCameraParameters camera(1000,1000,320,240);
+ vpCameraParameters camera(1000, 1000, 320, 240);
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
+ if (opt_display) {
+ // Get the view of the internal camera
sim.getInternalImage(Iint);
- //Get the view of the main external camera
+ // Get the view of the main external camera
sim.getExternalImage(Iext1);
- //Get the view of an external camera that you can positionned thanks
- //to a vpHomogeneousMatrix which describes the position of the camera
- //relative to the world reference frame.
- sim.getExternalImage(Iext2,camoMw);
- //Display the views.
+ // Get the view of an external camera that you can positionned thanks
+ // to a vpHomogeneousMatrix which describes the position of the camera
+ // relative to the world reference frame.
+ sim.getExternalImage(Iext2, camoMw);
+ // Display the views.
vpDisplay::flush(Iint);
vpDisplay::flush(Iext1);
@@ -256,38 +268,41 @@ main(int argc, const char ** argv)
}
std::cout << std::endl;
- std::cout << "Here are presented the effect of the basic functions of the simulator" << std::endl;
+ std::cout << "Here are presented the effect of the basic functions of "
+ "the simulator"
+ << std::endl;
std::cout << std::endl;
- if (opt_display)
- {
+ if (opt_display) {
if (opt_click) {
- std::cout << "Click on the internal view window to continue. the object will move. The external cameras are fixed. The main camera moves too because the homogeneous matrix cMo didn't change." << std::endl;
+ std::cout << "Click on the internal view window to continue. the "
+ "object will move. The external cameras are fixed. The "
+ "main camera moves too because the homogeneous matrix "
+ "cMo didn't change."
+ << std::endl;
vpDisplay::getClick(Iint);
}
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext1) ;
- vpDisplay::display(Iext2) ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext1);
+ vpDisplay::display(Iext2);
}
/*
To move the object you have to define a vpHomogeneousMatrix which gives
the position of the object relative to the world refrenece frame.
*/
- vpHomogeneousMatrix mov(0.05,0.05,0.2,vpMath::rad(10),0,0);
+ vpHomogeneousMatrix mov(0.05, 0.05, 0.2, vpMath::rad(10), 0, 0);
sim.set_fMo(mov);
-
- if (opt_display)
- {
- //Get the view of the internal camera
+ if (opt_display) {
+ // Get the view of the internal camera
sim.getInternalImage(Iint);
- //Get the view of the main external camera
+ // Get the view of the main external camera
sim.getExternalImage(Iext1);
- //Get the view of an external camera that you can positionned thanks
- //to a vpHomogeneousMatrix which describes the position of the camera
- //relative to the world reference frame.
- sim.getExternalImage(Iext2,camoMw);
- //Display the views.
+ // Get the view of an external camera that you can positionned thanks
+ // to a vpHomogeneousMatrix which describes the position of the camera
+ // relative to the world reference frame.
+ sim.getExternalImage(Iext2, camoMw);
+ // Display the views.
vpDisplay::flush(Iint);
vpDisplay::flush(Iext1);
@@ -295,45 +310,50 @@ main(int argc, const char ** argv)
}
std::cout << std::endl;
- if (opt_display)
- {
+ if (opt_display) {
if (opt_click) {
std::cout << "Click on the internal view window to continue" << std::endl;
vpDisplay::getClick(Iint);
}
}
std::cout << std::endl;
- std::cout << "Now you can move the main external camera. Click inside the corresponding window with one of the three buttons of your mouse and move the pointer." << std::endl;
+ std::cout << "Now you can move the main external camera. Click inside "
+ "the corresponding window with one of the three buttons of "
+ "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;
/*
- To move the main external camera you need a loop containing the getExternalImage method. This functionnality is only available for the main external camera.
+ To move the main external camera you need a loop containing the
+ getExternalImage method. This functionnality is only available for the
+ main external camera.
*/
- if (opt_display && opt_click)
- {
- while (!vpDisplay::getClick(Iint, false))
- {
- vpDisplay::display(Iext1) ;
+ if (opt_display && opt_click) {
+ while (!vpDisplay::getClick(Iint, false)) {
+ vpDisplay::display(Iext1);
sim.getExternalImage(Iext1);
vpDisplay::flush(Iext1);
}
}
std::cout << std::endl;
- std::cout << "You have seen the main capabilities of the simulator. Other specific functionalities are available. Please refers to the html documentation to access the list of all functions" << std::endl;
+ std::cout << "You have seen the main capabilities of the simulator. "
+ "Other specific functionalities are available. Please "
+ "refers to the html documentation to access the list of all "
+ "functions"
+ << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
+int main()
{
- vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display functionalities...");
+ vpERROR_TRACE("You do not have X11, OpenCV, GDI, D3D9 or GTK display "
+ "functionalities...");
}
#endif
diff --git a/macros/have_visp.m4 b/macros/have_visp.m4
index 1a2d181..945fbc7 100644
--- a/macros/have_visp.m4
+++ b/macros/have_visp.m4
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/CMakeLists.txt b/modules/CMakeLists.txt
index a1676aa..0d48380 100644
--- a/modules/CMakeLists.txt
+++ b/modules/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/ar/CMakeLists.txt b/modules/ar/CMakeLists.txt
index d55bdf8..4985ca2 100644
--- a/modules/ar/CMakeLists.txt
+++ b/modules/ar/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -95,7 +96,17 @@ if(USE_OGRE)
list(APPEND opt_incs ${inc_})
endif()
endforeach()
- list(APPEND opt_libs ${OGRE_LIBRARIES})
+ if(WIN32)
+ foreach(lib_ ${OGRE_LIBRARIES})
+ if(${lib_} MATCHES "^Ogre")
+ list(APPEND opt_libs "${OGRE_LIBRARY_DIRS}/${lib_}.lib")
+ else()
+ list(APPEND opt_libs ${lib_})
+ endif()
+ endforeach()
+ else()
+ list(APPEND opt_libs ${OGRE_LIBRARIES})
+ endif()
endif(USE_OGRE)
if(USE_OIS AND USE_OGRE)
@@ -180,7 +191,7 @@ vp_glob_module_sources()
if(USE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- vp_set_source_file_compile_flag(src/ogre-simulator/vpAROgre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ vp_set_source_file_compile_flag(src/ogre-simulator/vpAROgre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
vp_module_include_directories(${opt_incs})
diff --git a/modules/ar/include/visp3/ar/vpAR.h b/modules/ar/include/visp3/ar/vpAR.h
index 9b1db6b..4d75cf9 100644
--- a/modules/ar/include/visp3/ar/vpAR.h
+++ b/modules/ar/include/visp3/ar/vpAR.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -14,7 +15,7 @@
* Edition License.
*
* See http://visp.inria.fr for more information.
- *
+ *
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
@@ -23,7 +24,7 @@
*
* If you have questions regarding the use of this file, please contact
* Inria at visp at inria.fr
- *
+ *
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
/*!
\file vpAR.h
@@ -49,7 +49,6 @@
*/
-
#ifndef vpAR_HH
#define vpAR_HH
@@ -58,9 +57,9 @@
#ifdef VISP_HAVE_COIN3D_AND_GUI
// visp
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/ar/vpSimulator.h>
@@ -73,7 +72,7 @@
\class vpAR
\ingroup group_ar_renderer
-
+
\brief Implementation of an augmented reality viewer using Coin3D 3rd party.
Thus to be enabled this class requires Coin3D 3rd party installation.
@@ -81,18 +80,18 @@
This class can be used to display an image behind the internal view
of the simulator used for augmented reality application.
-
+
\warning This class is only available if Coin3D and one of the GUI
(SoWin, SoXT, SoQt) are installed.
The code below shows how to use the class.
\code
-#include <visp3/core/vpConfig.h>
#include <visp3/ar/vpAR.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpImage.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
#ifdef VISP_HAVE_COIN3D_AND_GUI
static void *mainloopfunction(void *_simu)
@@ -126,8 +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);
@@ -145,19 +145,16 @@ class VISP_EXPORT vpAR : public vpSimulator
{
private:
-
bool background;
- public:
-
-
- virtual ~vpAR() ;
- void initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type = grayImage) ;
- void setImage(vpImage<unsigned char> &I) ;
- void setImage(vpImage<vpRGBa> &I) ;
-
-} ;
+public:
+ vpAR() : background(false){};
+ virtual ~vpAR();
+ void initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type = grayImage);
+ void setImage(vpImage<unsigned char> &I);
+ void setImage(vpImage<vpRGBa> &I);
+};
#endif
#endif
diff --git a/modules/ar/include/visp3/ar/vpAROgre.h b/modules/ar/include/visp3/ar/vpAROgre.h
index 1fea701..42775a7 100644
--- a/modules/ar/include/visp3/ar/vpAROgre.h
+++ b/modules/ar/include/visp3/ar/vpAROgre.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -57,8 +58,8 @@
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
@@ -66,11 +67,11 @@
#include <OgreFrameListener.h>
#ifdef VISP_HAVE_OIS
-# include <OIS.h>
+#include <OIS.h>
#endif
/*!
- \class vpAROgre
+ \class vpAROgre
\ingroup group_ar_renderer
@@ -83,62 +84,63 @@
you will need to initialise it with the parameters of your camera, then each
frame you will need to compute a pose for your camera and give it to the
application. With that information and the image to be shown in background
- it will set up the 3D scene correspondingly.
+ it will set up the 3D scene correspondingly.
*/
-class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEventListener
+class VISP_EXPORT vpAROgre : public Ogre::FrameListener,
+ public Ogre::WindowEventListener
#ifdef VISP_HAVE_OIS
- , public OIS::KeyListener
+ ,
+ public OIS::KeyListener
#endif
-{
- public:
- vpAROgre(const vpCameraParameters &cam = vpCameraParameters(),
- unsigned int width = 0, unsigned int height = 0,
- const char* resourcePath =
+{
+public:
+ vpAROgre(const vpCameraParameters &cam = vpCameraParameters(), unsigned int width = 0, unsigned int height = 0,
+ const char *resourcePath =
#ifdef VISP_HAVE_OGRE_RESOURCES_PATH
- VISP_HAVE_OGRE_RESOURCES_PATH,
+ VISP_HAVE_OGRE_RESOURCES_PATH,
#else
- ".",
+ ".",
#endif
- const char* pluginsPath =
+ const char *pluginsPath =
#ifdef VISP_HAVE_OGRE_PLUGINS_PATH
- VISP_HAVE_OGRE_PLUGINS_PATH
+ VISP_HAVE_OGRE_PLUGINS_PATH
#else
- "."
+ "."
#endif
- );
-
+ );
+
virtual ~vpAROgre(void);
-
+
/*!
Add optional resource location. Since a resource file cannot be always
- sufficient to manage multiple location media (depending on the computer and
- the executable path), this method may be used to add such paths.
-
- \warning To be effective, this method must be called before the init() one.
-
- \param resourceLocation : The resource location (it may be a folder or a zip
- file).
+ sufficient to manage multiple location media (depending on the computer
+ and the executable path), this method may be used to add such paths.
+
+ \warning To be effective, this method must be called before the init()
+ one.
+
+ \param resourceLocation : The resource location (it may be a folder or a
+ zip file).
*/
- inline void addResource(const std::string& resourceLocation){
+ inline void addResource(const std::string &resourceLocation)
+ {
mOptionnalResourceLocation.push_back(resourceLocation);
}
-
+
void addRotation(const std::string &sceneName, const vpRotationMatrix &wRo);
-
+
bool continueRendering(void);
-
- virtual bool customframeStarted(const Ogre::FrameEvent& evt);
- virtual bool customframeEnded(const Ogre::FrameEvent& evt);
-
- virtual void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMw);
+ virtual bool customframeStarted(const Ogre::FrameEvent &evt);
+
+ virtual bool customframeEnded(const Ogre::FrameEvent &evt);
+
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw);
- virtual void display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMw);
-
- inline Ogre::Camera* getCamera(){ return mCamera; }
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw);
+
+ inline Ogre::Camera *getCamera() { return mCamera; }
/*!
Get the far distance for clipping.
@@ -153,40 +155,40 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEven
\return Near clipping value.
*/
inline double getNearClippingDistance() const { return mNearClipping; }
-
- vpTranslationVector getPosition(const std::string &sceneName)const;
-
+
+ vpTranslationVector getPosition(const std::string &sceneName) const;
+
void getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo);
-
- inline Ogre::SceneManager* getSceneManager(){ return mSceneMgr; }
-
- virtual void init(vpImage<unsigned char> &I, bool bufferedKeys=false, bool hidden=false);
- virtual void init(vpImage<vpRGBa> &I, bool bufferedKeys=false, bool hidden=false);
-
+
+ inline Ogre::SceneManager *getSceneManager() { return mSceneMgr; }
+
+ virtual void init(vpImage<unsigned char> &I, bool bufferedKeys = false, bool hidden = false);
+ virtual void init(vpImage<vpRGBa> &I, bool bufferedKeys = false, bool hidden = false);
+
/*!
Test if the window is hidden or not.
-
- \warning True if the window is hidden, false otherwise.
+
+ \warning True if the window is hidden, false otherwise.
*/
- bool isWindowHidden(){ return windowHidden; }
-
+ bool isWindowHidden() { return windowHidden; }
+
#ifdef VISP_HAVE_OIS
/**
* Default event handler
*/
- virtual bool keyPressed( const OIS::KeyEvent & /*e*/) { return true; }
+ virtual bool keyPressed(const OIS::KeyEvent & /*e*/) { return true; }
/**
* Default event handler
*/
- virtual bool keyReleased( const OIS::KeyEvent & /*e*/) { return true; }
+ virtual bool keyReleased(const OIS::KeyEvent & /*e*/) { return true; }
#endif
void load(const std::string &entityName, const std::string &model);
bool renderOneFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw);
-
+
bool renderOneFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw);
-
+
void setCameraParameters(const vpCameraParameters &cameraP);
/*!
@@ -194,7 +196,8 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEven
\param dist : Far clipping value.
*/
- void setFarClippingDistance(const double &dist){
+ void setFarClippingDistance(const double &dist)
+ {
mFarClipping = dist;
updateCameraProjection();
}
@@ -204,92 +207,89 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEven
\param dist : Near clipping value.
*/
- void setNearClippingDistance(const double &dist){
+ void setNearClippingDistance(const double &dist)
+ {
mNearClipping = dist;
updateCameraProjection();
}
-
+
/*!
Set the plugins path used to locate the plugins.cfg file.
-
- By default, this path is set to VISP_HAVE_OGRE_PLUGINS_PATH defined in vpConfig.h
- \warning To be effective, this method must be called before the init() one.
-
+ By default, this path is set to VISP_HAVE_OGRE_PLUGINS_PATH defined in
+ vpConfig.h
+
+ \warning To be effective, this method must be called before the init()
+ one.
+
\param pluginsPath : The new plugins path (must not have a terminate /).
*/
- inline void setPluginsPath(const char* pluginsPath){
- mPluginsPath = pluginsPath;
- }
-
+ inline void setPluginsPath(const char *pluginsPath) { mPluginsPath = pluginsPath; }
+
void setPosition(const std::string &sceneName, const vpTranslationVector &wTo);
void setPosition(const std::string &sceneName, const vpHomogeneousMatrix &wMo);
-
+
/*!
Set the resource path used to locate the resource.cfg file.
-
- By default, this path is set to VISP_HAVE_OGRE_RESOURCES_PATH defined in vpConfig.h
- \warning To be effective, this method must be called before the init() one.
-
+ By default, this path is set to VISP_HAVE_OGRE_RESOURCES_PATH defined in
+ vpConfig.h
+
+ \warning To be effective, this method must be called before the init()
+ one.
+
\param resourcePath : The new resource path (must not have a terminate /).
*/
- inline void setResourcePath(const char* resourcePath){
- mResourcePath = resourcePath;
- }
-
+ inline void setResourcePath(const char *resourcePath) { mResourcePath = resourcePath; }
+
void setRotation(const std::string &sceneName, const vpRotationMatrix &wRo);
-
+
void setScale(const std::string &sceneName, const float factorx, const float factory, const float factorz);
-
+
/*!
Enable/Disable the appearance of the config dialog on startup.
-
- \warning To be effective, this method must be called before the init() one.
-
- \param showConfigDialog : if true, shows the dialog window (used to set the
- display options)
+
+ \warning To be effective, this method must be called before the init()
+ one.
+
+ \param showConfigDialog : if true, shows the dialog window (used to set
+ the display options)
*/
- inline void setShowConfigDialog(const bool showConfigDialog){
- mshowConfigDialog = showConfigDialog;
- }
-
+ inline void setShowConfigDialog(const bool showConfigDialog) { mshowConfigDialog = showConfigDialog; }
+
void setVisibility(const std::string &sceneName, bool isVisible);
-
+
/*!
Set the name of the window.
-
+
\warning Has to be called before initialisation.
-
+
\param n : Name of the window.
*/
- inline void setWindowName(const Ogre::String &n){
- name = n;
- }
+ inline void setWindowName(const Ogre::String &n) { name = n; }
/*!
Set the window position in the screen.
-
- \throw vpException::notInitialised if the window has not been created (using
- the init() method).
-
- \param win_x : x coordinate of the new top left corner of the window in the
- screen.
- \param win_y : y coordinate of the new top left corner of the window in the
- screen.
+
+ \throw vpException::notInitialised if the window has not been created
+ (using the init() method).
+
+ \param win_x : x coordinate of the new top left corner of the window in
+ the screen. \param win_y : y coordinate of the new top left corner of the
+ window in the screen.
*/
- inline void setWindowPosition(const unsigned int win_x, const unsigned int win_y){
- if(mWindow == NULL){
+ inline void setWindowPosition(const unsigned int win_x, const unsigned int win_y)
+ {
+ if (mWindow == NULL) {
throw vpException(vpException::notInitialized, "Window not initialised, cannot set its position");
}
mWindow->reposition(static_cast<int>(win_x), static_cast<int>(win_y));
}
-
- virtual void windowClosed(Ogre::RenderWindow* rw);
-
- protected:
- virtual void init(bool bufferedKeys=false, bool hidden=false);
+ virtual void windowClosed(Ogre::RenderWindow *rw);
+
+protected:
+ virtual void init(bool bufferedKeys = false, bool hidden = false);
virtual void createCamera(void);
/**
@@ -302,34 +302,35 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEven
/*!
Update the 3D scene
-
+
\return Always true.
*/
- virtual bool updateScene(const Ogre::FrameEvent & /*evt*/) {return true;};
+ virtual bool updateScene(const Ogre::FrameEvent & /*evt*/) { return true; };
/*!
Check for keyboard, mouse and joystick inputs.
-
+
\return Always true.
*/
- virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) {return true;};
+ virtual bool processInputEvent(const Ogre::FrameEvent & /*evt*/) { return true; };
/*!
Clean up the 3D scene.
-
+
\return Always true.
*/
- virtual bool destroyScene(void) {
- if(!mSceneMgr)
- return false;
-
- mSceneMgr->destroyAllCameras ();
- mSceneMgr->clearScene();
- mRoot->destroySceneManager(mSceneMgr);
- return true;
+ virtual bool destroyScene(void)
+ {
+ if (!mSceneMgr)
+ return false;
+
+ mSceneMgr->destroyAllCameras();
+ mSceneMgr->clearScene();
+ mRoot->destroySceneManager(mSceneMgr);
+ return true;
}
-
- virtual void updateCameraParameters (const vpHomogeneousMatrix &cMo);
+
+ virtual void updateCameraParameters(const vpHomogeneousMatrix &cMo);
virtual void updateCameraProjection(void);
@@ -337,57 +338,57 @@ class VISP_EXPORT vpAROgre : public Ogre::FrameListener, public Ogre::WindowEven
virtual void updateBackgroundTexture(const vpImage<vpRGBa> &I);
-
- private:
-
+private:
void createBackground(vpImage<unsigned char> &I);
void createBackground(vpImage<vpRGBa> &I);
- bool frameStarted(const Ogre::FrameEvent& evt);
+ bool frameStarted(const Ogre::FrameEvent &evt);
- bool frameEnded( const Ogre::FrameEvent& evt);
+ bool frameEnded(const Ogre::FrameEvent &evt);
- bool stopTest(const Ogre::FrameEvent& evt);
+ bool stopTest(const Ogre::FrameEvent &evt);
- protected:
+protected:
// Attributes
- Ogre::String name; /**Name of th Window*/
-
+ Ogre::String name; /**Name of th Window*/
+
// OGRE 3D System
- Ogre::Root* mRoot; /** Application's root */
- Ogre::Camera* mCamera; /** Camera */
- Ogre::SceneManager* mSceneMgr; /** Scene manager */
- Ogre::RenderWindow* mWindow; /** Display window */
- Ogre::String mResourcePath; /** Path to resources.cfg */
- Ogre::String mPluginsPath; /** Path to plugins.cfg */
+ Ogre::Root *mRoot; /** Application's root */
+ Ogre::Camera *mCamera; /** Camera */
+ Ogre::SceneManager *mSceneMgr; /** Scene manager */
+ Ogre::RenderWindow *mWindow; /** Display window */
+ Ogre::String mResourcePath; /** Path to resources.cfg */
+ Ogre::String mPluginsPath; /** Path to plugins.cfg */
#ifdef VISP_HAVE_OIS
// OIS Input manager and devices
- OIS::InputManager* mInputManager;
- OIS::Keyboard* mKeyboard;
+ OIS::InputManager *mInputManager;
+ OIS::Keyboard *mKeyboard;
#endif
// ViSP AR System
- bool keepOn; /** Has the application received a signal to stop(false) or not (true) */
- vpImage<vpRGBa> mImageRGBA; /** vpImage to store grabbed image */
- vpImage<unsigned char> mImage; /** vpImage to store grabbed image */
- Ogre::HardwarePixelBufferSharedPtr mPixelBuffer; /** Pointer to the pixel buffer */
- Ogre::Rectangle2D* mBackground; /** Background image */
- unsigned int mBackgroundHeight; /** Height of the acquired image */
- unsigned int mBackgroundWidth; /** Width of the acquired image */
- unsigned int mWindowHeight; /** Height of the window */
- unsigned int mWindowWidth; /** Width of the window */
- bool windowHidden; /** Is window hidden */
+ bool keepOn; /** Has the application received a signal to stop(false) or not
+ (true) */
+ vpImage<vpRGBa> mImageRGBA; /** vpImage to store grabbed image */
+ vpImage<unsigned char> mImage; /** vpImage to store grabbed image */
+ Ogre::HardwarePixelBufferSharedPtr mPixelBuffer; /** Pointer to the pixel buffer */
+ Ogre::Rectangle2D *mBackground; /** Background image */
+ unsigned int mBackgroundHeight; /** Height of the acquired image */
+ unsigned int mBackgroundWidth; /** Width of the acquired image */
+ unsigned int mWindowHeight; /** Height of the window */
+ unsigned int mWindowWidth; /** Width of the window */
+ bool windowHidden; /** Is window hidden */
// Camera calculations
- double mNearClipping; /** Near Clipping Distance **/
- double mFarClipping; /** Far Clipping Distance **/
- vpCameraParameters mcam; /** The intrinsic camera parameters */
-
- bool mshowConfigDialog; /** if true, shows the dialog window (used to set the display options) */
-
- std::list<std::string> mOptionnalResourceLocation; /** Optional resource location (used to load mesh and material) */
+ double mNearClipping; /** Near Clipping Distance **/
+ double mFarClipping; /** Far Clipping Distance **/
+ vpCameraParameters mcam; /** The intrinsic camera parameters */
+
+ bool mshowConfigDialog; /** if true, shows the dialog window (used to set
+ the display options) */
+ std::list<std::string> mOptionnalResourceLocation; /** Optional resource location (used to
+ load mesh and material) */
};
#endif // VISP_HAVE_OGRE
diff --git a/modules/ar/include/visp3/ar/vpSimulator.h b/modules/ar/include/visp3/ar/vpSimulator.h
index 20f6118..b5d6b1a 100644
--- a/modules/ar/include/visp3/ar/vpSimulator.h
+++ b/modules/ar/include/visp3/ar/vpSimulator.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -59,8 +60,9 @@
the Qt includes to solve this problem.
*/
+#include <Inventor/SoOffscreenRenderer.h>
+#include <Inventor/fields/SoSFTime.h>
#include <Inventor/nodes/SoBaseColor.h>
-#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoCone.h>
#include <Inventor/nodes/SoCube.h>
#include <Inventor/nodes/SoImage.h>
@@ -68,25 +70,24 @@
#include <Inventor/nodes/SoPerspectiveCamera.h>
#include <Inventor/nodes/SoRotationXYZ.h>
#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoTranslation.h>
#include <Inventor/sensors/SoTimerSensor.h>
-#include <Inventor/SoOffscreenRenderer.h>
-#include <Inventor/fields/SoSFTime.h>
#include <Inventor/threads/SbThread.h>
// visp
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
/*!
\class vpSimulator
\ingroup group_ar_simulator
-
+
\brief Implementation of a simulator based on Coin3d (www.coin3d.org).
The simulator uses the vpViewer class.
@@ -99,65 +100,62 @@ class VISP_EXPORT vpSimulator
{
protected:
//! perform some initialization
- void init() ;
+ void init();
//! perform some destruction
- void kill() ;
+ void kill();
public:
//! constructor
- vpSimulator() ;
- virtual ~vpSimulator() ;
+ vpSimulator();
+ virtual ~vpSimulator();
protected:
- //! main Widget
+//! main Widget
#if defined(VISP_HAVE_SOWIN)
- HWND mainWindow ;
+ HWND mainWindow;
#elif defined(VISP_HAVE_SOQT)
- QWidget * mainWindow ;
+ QWidget *mainWindow;
#elif defined(VISP_HAVE_SOXT)
- Widget mainWindow ;
+ Widget mainWindow;
#endif
- bool mainWindowInitialized ;
+ bool mainWindowInitialized;
//! open the SoGui application
- void initSoApplication() ;
+ void initSoApplication();
public:
- typedef enum {
- grayImage,
- colorImage
- } vpImageType ;
- vpImageType typeImage ;
+ typedef enum { grayImage, colorImage } vpImageType;
+ vpImageType typeImage;
- GLubyte * image_background;
+ GLubyte *image_background;
//! activate the mainloop
- virtual void mainLoop() ;
+ virtual void mainLoop();
protected:
//! view from the camera
- vpViewer *internalView ;
+ vpViewer *internalView;
//! view from an external camera
- vpViewer *externalView ;
+ vpViewer *externalView;
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) ;
+ void initExternalViewer(const unsigned int nlig, const unsigned int ncol);
protected:
//! thread with the main program
- SbThread * mainThread;
- //pthread_t mainThread;
+ SbThread *mainThread;
+ // pthread_t mainThread;
public:
//! begin the main program
- void initApplication(void *(*start_routine)(void *)) ;
- void initApplication(void *(*start_routine)(void *), void* data);
+ void initApplication(void *(*start_routine)(void *));
+ void initApplication(void *(*start_routine)(void *), void *data);
//! perform some initialization in the main program thread
- void initMainApplication() ;
- void closeMainApplication() ;
+ void initMainApplication();
+ void closeMainApplication();
//----------------------------------------------------
// scene description
@@ -169,14 +167,14 @@ protected:
public:
/*!
- Return the width of the internal view.
-
+ Return the width of the internal view.
+
\return The width of the internal view.
*/
unsigned int getInternalWidth() const { return internal_width; }
/*!
- Return the height of the internal view.
-
+ Return the height of the internal view.
+
\return The height of the internal view.
*/
unsigned int getInternalHeight() const { return internal_height; }
@@ -191,115 +189,109 @@ protected:
SoSeparator *externalRoot;
//! internal camera
- SoPerspectiveCamera *internalCamera ;
+ SoPerspectiveCamera *internalCamera;
//! external camera
- SoPerspectiveCamera *externalCamera ;
+ SoPerspectiveCamera *externalCamera;
//! internal camera position
- SoTransform *internalCameraPosition ;
+ SoTransform *internalCameraPosition;
//! external camera position
- SoTransform *extrenalCameraPosition ;
+ SoTransform *extrenalCameraPosition;
//! representation of the camera in the external view
SoSeparator *internalCameraObject;
//! initialize the scene graph
- void initSceneGraph() ;
+ void initSceneGraph();
- //!Add a new object in the scene graph ad a given location
- void addObject(SoSeparator * object, const vpHomogeneousMatrix &fMo,
- SoSeparator * root) ;
+ //! Add a new object in the scene graph ad a given location
+ void addObject(SoSeparator *object, const vpHomogeneousMatrix &fMo, SoSeparator *root);
-public :
- //!Add a new object in the scene graph ad a given location
- void addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo) ;
+public:
+ //! Add a new object in the scene graph ad a given location
+ void addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo);
public:
//! display the scene (handle with care)
- void redraw() ;
+ void redraw();
//! load an iv file
- void load(const char *file_name) ;
+ void load(const char *file_name);
//! load an iv file, set the location of this scene
- void load(const char *iv_filename,const vpHomogeneousMatrix &fMo) ;
+ void load(const char *iv_filename, const vpHomogeneousMatrix &fMo);
//! save the scene in an iv file
- void save(const char *name,bool binary=false) ;
+ void save(const char *name, bool binary = false);
//! Add the representation of the absolute frame
- void addAbsoluteFrame (float zoom=1) ;
+ void addAbsoluteFrame(float zoom = 1);
//! Add the representation of a frame
- void addFrame (const vpHomogeneousMatrix &fMo, float zoom=1) ;
+ void addFrame(const vpHomogeneousMatrix &fMo, float zoom = 1);
//! set the size of the camera/frame
- void setZoomFactor (const float zoom) ;
+ void setZoomFactor(const float zoom);
protected:
- float zoomFactor ;
+ float zoomFactor;
//---------------------------------------------------
// camera description
protected:
- bool cameraPositionInitialized ;
+ bool cameraPositionInitialized;
//! internal camera position
- vpHomogeneousMatrix cMf ;
+ vpHomogeneousMatrix cMf;
//! internal camera parameters
- vpCameraParameters internalCameraParameters ;
+ vpCameraParameters internalCameraParameters;
//! internal camera parameters
- vpCameraParameters externalCameraParameters ;
+ vpCameraParameters externalCameraParameters;
public:
//! set the camera position (from an homogeneous matrix)
- void setCameraPosition(vpHomogeneousMatrix &cMf) ;
+ void setCameraPosition(vpHomogeneousMatrix &cMf);
//! get the camera position (from an homogeneous matrix)
- void getCameraPosition(vpHomogeneousMatrix &_cMf) { _cMf = cMf ; }
+ void getCameraPosition(vpHomogeneousMatrix &_cMf) { _cMf = cMf; }
//! modify the position of the camera in the scene graph
- void moveInternalCamera(vpHomogeneousMatrix &cMf) ;
+ void moveInternalCamera(vpHomogeneousMatrix &cMf);
//! set internal camera parameters
- void setInternalCameraParameters(vpCameraParameters &cam) ;
+ void setInternalCameraParameters(vpCameraParameters &cam);
//! set external camera parameters
- void setExternalCameraParameters(vpCameraParameters &cam) ;
+ void setExternalCameraParameters(vpCameraParameters &cam);
//! get the external camera position
- void getExternalCameraPosition(vpHomogeneousMatrix &cMf) ;
+ void getExternalCameraPosition(vpHomogeneousMatrix &cMf);
- //! get an Image of the internal view
- void getInternalImage(vpImage<unsigned char> &I) ;
- //! get an Image of the internal view
- void getInternalImage(vpImage<vpRGBa> &I) ;
+ //! get an Image of the internal view
+ void getInternalImage(vpImage<unsigned char> &I);
+ //! get an Image of the internal view
+ void getInternalImage(vpImage<vpRGBa> &I);
/* --- Off screen rendering --- */
-
+
void changeZoomFactor(const float zoom, const int index);
+
public:
- typedef enum { INTERNAL, EXTERNAL } vpSimulatorViewType ;
+ typedef enum { INTERNAL, EXTERNAL } vpSimulatorViewType;
#ifdef VISP_HAVE_MODULE_IO
- void write(const char * fileName);
+ void write(const char *fileName);
#endif
protected:
- SbTime * realtime ;
- SoOffscreenRenderer * offScreenRenderer ;
- void offScreenRendering (vpSimulatorViewType view=vpSimulator::EXTERNAL,
- int * width = NULL,
- int * height = NULL);
+ SbTime *realtime;
+ SoOffscreenRenderer *offScreenRenderer;
+ void offScreenRendering(vpSimulatorViewType view = vpSimulator::EXTERNAL, int *width = NULL, int *height = NULL);
+
public:
//! image of the internal view
- unsigned char * bufferView;
+ unsigned char *bufferView;
+
+ //! Flag to protect the read and write of the framebuffer (between the
+ //! simulator and the viewer).
+ int get;
- //! Flag to protect the read and write of the framebuffer (between the simulator and the viewer).
- int get ;
public:
//! get the image corresponding to the internal view
- unsigned char* getBufferedOffScreenRenderer()
- {
- return bufferView;
- }
+ unsigned char *getBufferedOffScreenRenderer() { return bufferView; }
//! get the size of the internal view
- void getSizeInternalView(int& width, int& height);
+ 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
#endif
diff --git a/modules/ar/include/visp3/ar/vpSimulatorException.h b/modules/ar/include/visp3/ar/vpSimulatorException.h
index 704e497..9ee96c4 100644
--- a/modules/ar/include/visp3/ar/vpSimulatorException.h
+++ b/modules/ar/include/visp3/ar/vpSimulatorException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,15 @@
*
*****************************************************************************/
-
#ifndef __vpSimulatorException_H
#define __vpSimulatorException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpSimulatorException.h
\brief error that can be emited by the vpSimulator class and its derivates
@@ -52,12 +53,15 @@
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -66,24 +70,23 @@
*/
class VISP_EXPORT vpSimulatorException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpSimulator member
- */
- enum errorSimulatorCodeEnum
- {
- ioError,
- noFileNameError,
- notInitializedError,
- windowSizeNotInitializedError,
- badInitializationError
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpSimulator member
+ */
+ enum errorSimulatorCodeEnum {
+ ioError,
+ noFileNameError,
+ notInitializedError,
+ windowSizeNotInitializedError,
+ badInitializationError
+ };
- public:
- vpSimulatorException (const int id, const char* format, ...);
- vpSimulatorException (const int id, const std::string & msg);
- vpSimulatorException (const int id);
+public:
+ vpSimulatorException(const int id, const char *format, ...);
+ vpSimulatorException(const int id, const std::string &msg);
+ explicit vpSimulatorException(const int id);
};
#endif
diff --git a/modules/ar/include/visp3/ar/vpViewer.h b/modules/ar/include/visp3/ar/vpViewer.h
index 6d5c38f..2d0214d 100644
--- a/modules/ar/include/visp3/ar/vpViewer.h
+++ b/modules/ar/include/visp3/ar/vpViewer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,11 @@
*
*****************************************************************************/
-
#ifndef vpViewer_HH
#define vpViewer_HH
/*!
\file vpViewer.h
-
+
Viewer used by the simulator. Under Windows, the viewer is based
either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or
SoXt.
@@ -56,18 +56,18 @@
#if defined(VISP_HAVE_SOWIN)
- #include <Inventor/Win/SoWin.h>
- #include <Inventor/Win/viewers/SoWinExaminerViewer.h>
+#include <Inventor/Win/SoWin.h>
+#include <Inventor/Win/viewers/SoWinExaminerViewer.h>
#elif defined(VISP_HAVE_SOQT)
- #include <Inventor/Qt/SoQt.h>
- #include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
+#include <Inventor/Qt/SoQt.h>
+#include <Inventor/Qt/viewers/SoQtExaminerViewer.h>
#elif defined(VISP_HAVE_SOXT)
-
- #include <Inventor/Xt/SoXt.h>
- #include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
+
+#include <Inventor/Xt/SoXt.h>
+#include <Inventor/Xt/viewers/SoXtExaminerViewer.h>
#endif
@@ -93,9 +93,9 @@
#endif
// visp
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
/*!
\class vpViewer
@@ -108,7 +108,7 @@
\warning This class is only available if Coin3D and one of the GUI
(SoWin, SoXT, SoQt) are installed.
-
+
*/
class vpSimulator;
@@ -122,45 +122,41 @@ class VISP_EXPORT vpViewer : public SoXtExaminerViewer
#endif
{
- friend class vpSimulator ;
+ friend class vpSimulator;
public:
- typedef enum{
- internalView,
- externalView
- } vpViewerType ;
+ typedef enum { internalView, externalView } vpViewerType;
#if defined(VISP_HAVE_SOWIN)
- vpViewer(HWND parent, vpSimulator *simu,vpViewerType viewerType);
+ vpViewer(HWND parent, vpSimulator *simu, vpViewerType type);
#elif defined(VISP_HAVE_SOQT)
- vpViewer(QWidget * parent, vpSimulator *simu,vpViewerType viewerType);
+ vpViewer(QWidget *parent, vpSimulator *simu, vpViewerType type);
#elif defined(VISP_HAVE_SOXT)
- vpViewer(Widget parent, vpSimulator *simu,vpViewerType viewerType);
+ vpViewer(Widget parent, vpSimulator *simu, vpViewerType type);
#endif
virtual ~vpViewer();
- void resize(int x, int y, bool fixed = false) ;
+ void resize(int x, int y, bool fixed = false);
virtual void actualRedraw(void);
private:
-
- vpViewerType viewerType;
- vpSimulator *simu ;
- SbBool processSoEvent(const SoEvent * const event) ;
+ vpViewerType viewerType;
+ vpSimulator *simu;
+ SbBool processSoEvent(const SoEvent *const event);
#if defined(VISP_HAVE_SOWIN)
- static HWND init(const char * appname) {return SoWin::init(appname);};
- static void mainLoop() {SoWin::mainLoop();};
- static void exitMainLoop() {SoWin::exitMainLoop();};
+ static HWND init(const char *appname) { return SoWin::init(appname); };
+ static void mainLoop() { SoWin::mainLoop(); };
+ static void exitMainLoop() { SoWin::exitMainLoop(); };
#elif defined(VISP_HAVE_SOQT)
- static QWidget * init(const char * appname) {return SoQt::init(appname);};
- static void mainLoop() { SoQt::mainLoop();};
- static void exitMainLoop() {SoQt::exitMainLoop();};
+ static QWidget *init(const char *appname) { return SoQt::init(appname); };
+ static void mainLoop() { SoQt::mainLoop(); };
+ static void exitMainLoop() { SoQt::exitMainLoop(); };
#elif defined(VISP_HAVE_SOXT)
- static Widget init(const char * appname) {return SoXt::init(appname);};
- static void mainLoop() { SoXt::mainLoop();};
- static void exitMainLoop() {SoXt::exitMainLoop();};
+ static Widget init(const char *appname) { return SoXt::init(appname); };
+ static void mainLoop() { SoXt::mainLoop(); };
+ static void exitMainLoop() { SoXt::exitMainLoop(); };
#endif
};
-#endif //VISP_HAVE_COIN3D_AND_GUI
+#endif // VISP_HAVE_COIN3D_AND_GUI
#endif
diff --git a/modules/ar/src/coin-simulator/vpAR.cpp b/modules/ar/src/coin-simulator/vpAR.cpp
index 1ea36de..4b584f0 100644
--- a/modules/ar/src/coin-simulator/vpAR.cpp
+++ b/modules/ar/src/coin-simulator/vpAR.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,7 +40,8 @@
/*!
\file vpAR.cpp
- \brief class used to display an image behind the internal view of the simulator. Used for augmented reality applications.
+ \brief class used to display an image behind the internal view of the
+ simulator. Used for augmented reality applications.
*/
#include <visp3/core/vpConfig.h>
@@ -49,136 +51,109 @@
#include <visp3/ar/vpAR.h>
#include <visp3/core/vpTime.h>
-
/* Objets OIV. */
-#include <Inventor/nodes/SoCone.h> /* Objet cone. */
-#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
-#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
-#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoCone.h> /* Objet cone. */
+#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
-#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
-#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
+#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
+#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
-#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
-#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
-#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
-#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
#include <Inventor/actions/SoWriteAction.h>
-
-
+#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
+#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
+#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
+#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
+#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
/*!
- Basic Destructor that calls the kill() method of the vpSimulator class.
+ Basic Destructor that calls the kill() method of the vpSimulator
+ class.
*/
-vpAR::~vpAR()
-{
- kill() ;
-}
-
+vpAR::~vpAR() { kill(); }
/*!
- Initialisation of the internal view of the simulator.
+ Initialisation of the internal view of the simulator.
- \param width : Width of the internal view.
- \param height : Height of the internal view.
- \param type : Type of background image ie gray scaled or color.
+ \param width : Width of the internal view.
+ \param height : Height of the internal view.
+ \param type : Type of background image ie gray scaled or color.
*/
-void
-vpAR::initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type)
+void vpAR::initInternalViewer(const unsigned int width, const unsigned int height, vpImageType type)
{
- vpSimulator::initInternalViewer(width,height) ;
-
+ vpSimulator::initInternalViewer(width, height);
// no image is loaded
- background = false ;
+ background = false;
- if ( image_background != NULL)
- {
- free(image_background) ;
- image_background = NULL ;
+ if (image_background != NULL) {
+ free(image_background);
+ image_background = NULL;
}
typeImage = type;
if (typeImage == grayImage)
- image_background =(GLubyte *)
- malloc(internal_width*internal_height*sizeof(GLubyte)) ;
+ 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));
}
-
-
/*!
- Set the background image and turn it to deal with the frame of OpenGL.
+ Set the background image and turn it to deal with the frame of OpenGL.
- \param I : Gray scaled image for the background.
+ \param I : Gray scaled image for the background.
*/
// Grey pictures SetBackGroundImage
-void
-vpAR::setImage(vpImage<unsigned char> &I)
+void vpAR::setImage(vpImage<unsigned char> &I)
{
- if ((internal_width != I.getWidth()) ||
- (internal_height != I.getHeight()))
- {
- vpERROR_TRACE("The image size is different from the view size ");
- throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
- }
-
-
- background = true ;
-
+ 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");
+ }
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- //le repere image open GL est en bas a gauche donc l'image serait inverse
- image_background[i*I.getWidth()+j] = I[I.getHeight()-i-1][j] ;
+ background = true;
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ // le repere image open GL est en bas a gauche donc l'image serait
+ // inverse
+ image_background[i * I.getWidth() + j] = I[I.getHeight() - i - 1][j];
}
-
-
/*!
- Set the background image and turn it to deal with the frame of OpenGL.
+ Set the background image and turn it to deal with the frame of OpenGL.
- \param I : Color image for the background.
+ \param I : Color image for the background.
*/
// Color pictures SetBackGroundImage
-void
-vpAR::setImage(vpImage<vpRGBa> &I)
+void vpAR::setImage(vpImage<vpRGBa> &I)
{
- if ((internal_width != I.getWidth()) ||
- (internal_height != I.getHeight()))
- {
- vpERROR_TRACE("The image size is different from the view size ");
- throw(vpException(vpException::dimensionError),"The image size is different from the view size") ;
- }
-
+ if ((internal_width != I.getWidth()) || (internal_height != I.getHeight())) {
+ vpERROR_TRACE("The image size is different from the view size ");
+ throw(vpException(vpException::dimensionError), "The image size is different from the view size");
+ }
- background = true ;
+ background = true;
- for (unsigned int i=0 ; i <I.getHeight() ; i++)
- {
- unsigned int k=0;
- for (unsigned int j=0 ; j <I.getWidth() ; j++)
- //le repere image open GL est en bas a gauche donc l'image serait inverse
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ unsigned int k = 0;
+ for (unsigned int j = 0; j < I.getWidth(); j++)
+ // le repere image open GL est en bas a gauche donc l'image serait inverse
{
- image_background[i*I.getWidth()*3+k+0]=I[I.getHeight()-i-1][j].R ;
- image_background[i*I.getWidth()*3+k+1]=I[I.getHeight()-i-1][j].G ;
- image_background[i*I.getWidth()*3+k+2]=I[I.getHeight()-i-1][j].B ;
- k+=3;
+ image_background[i * I.getWidth() * 3 + k + 0] = I[I.getHeight() - i - 1][j].R;
+ image_background[i * I.getWidth() * 3 + k + 1] = I[I.getHeight() - i - 1][j].G;
+ image_background[i * I.getWidth() * 3 + k + 2] = I[I.getHeight() - i - 1][j].B;
+ k += 3;
}
}
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_ar.a(vpAR.cpp.o) has no symbols
-void dummy_vpAR() {};
+void dummy_vpAR(){};
#endif
diff --git a/modules/ar/src/coin-simulator/vpSimulator.cpp b/modules/ar/src/coin-simulator/vpSimulator.cpp
index 4d28cf3..aa2a635 100644
--- a/modules/ar/src/coin-simulator/vpSimulator.cpp
+++ b/modules/ar/src/coin-simulator/vpSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,56 +52,62 @@
#include <visp3/core/vpImage.h>
#ifdef VISP_HAVE_MODULE_IO
-# include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
#endif
/* Objets OIV. */
-#include <Inventor/nodes/SoCone.h> /* Objet cone. */
-#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
-#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
-#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoCone.h> /* Objet cone. */
+#include <Inventor/nodes/SoCoordinate3.h> /* Liste de points. */
+#include <Inventor/nodes/SoCylinder.h> /* Objet cylindre. */
#include <Inventor/nodes/SoIndexedFaceSet.h> /* Liste de face. */
-#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
-#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+#include <Inventor/nodes/SoPointLight.h> /* Objet lumiere ponctuelle. */
+#include <Inventor/nodes/SoRotationXYZ.h> /* Transfo rotation simple. */
+#include <Inventor/nodes/SoScale.h> /* Trasnfo mise a l'echelle. */
+#include <Inventor/nodes/SoTranslation.h> /* Trasnfo translation. */
-#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
-#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
-#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
-#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
-#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
#include <Inventor/actions/SoWriteAction.h>
-
-
-
+#include <Inventor/nodes/SoDirectionalLight.h> /* Objet lumiere directionnelle*/
+#include <Inventor/nodes/SoDrawStyle.h> /* Style de rendu. */
+#include <Inventor/nodes/SoEnvironment.h> /* Eclairage ambiant. */
+#include <Inventor/nodes/SoGroup.h> /* Groupement de noeuds (sans separation)*/
+#include <Inventor/nodes/SoMaterial.h> /* Matiere (couleur) des objets. */
// Positions of all of the vertices:
//
-static float pyramidVertexes [5][3] =
- {
- {0.33f, 0.33f, 0.f},
- {-0.33f, 0.33f, 0.f},
- {-0.33f, -0.33f, 0.f},
- {0.33f, -0.33f, 0.f},
-
- {0.f, 0.f, -1.0f}
- };
-
-
-static int32_t pyramidFaces[] =
- {
- 0, 1, 2, 3, SO_END_FACE_INDEX, // top face
-
- 0, 1, 4, SO_END_FACE_INDEX, // 4 faces about top
- 1, 2, 4, SO_END_FACE_INDEX,
- 2, 3, 4, SO_END_FACE_INDEX,
- 3, 0, 4, SO_END_FACE_INDEX,
- };
-
+static float pyramidVertexes[5][3] = {{0.33f, 0.33f, 0.f},
+ {-0.33f, 0.33f, 0.f},
+ {-0.33f, -0.33f, 0.f},
+ {0.33f, -0.33f, 0.f},
+
+ {0.f, 0.f, -1.0f}};
+
+static int32_t pyramidFaces[] = {
+ 0,
+ 1,
+ 2,
+ 3,
+ SO_END_FACE_INDEX, // top face
+
+ 0,
+ 1,
+ 4,
+ SO_END_FACE_INDEX, // 4 faces about top
+ 1,
+ 2,
+ 4,
+ SO_END_FACE_INDEX,
+ 2,
+ 3,
+ 4,
+ SO_END_FACE_INDEX,
+ 3,
+ 0,
+ 4,
+ SO_END_FACE_INDEX,
+};
// Routine to create a scene graph representing a dodecahedron
-SoSeparator *
-makePyramide()
+SoSeparator *makePyramide()
{
SoSeparator *result = new SoSeparator;
result->ref();
@@ -112,8 +119,8 @@ makePyramide()
// Define the IndexedFaceSet, with indices into the vertices:
SoIndexedFaceSet *myFaceSet = new SoIndexedFaceSet;
- myFaceSet->coordIndex.setValues (0, 21, (const int32_t*)pyramidFaces);
- result->addChild (myFaceSet);
+ myFaceSet->coordIndex.setValues(0, 21, (const int32_t *)pyramidFaces);
+ result->addChild(myFaceSet);
result->unrefNoDelete();
return result;
@@ -125,10 +132,7 @@ makePyramide()
* est <radius>, et celui de la fleche <radius> * 5.
* La fleche est oriente selon l'axe Y.
*/
-static SoSeparator *
-createArrow (float longueur,
- float proportionFleche,
- float radius)
+static SoSeparator *createArrow(float longueur, float proportionFleche, float radius)
{
SoSeparator *fleche = new SoSeparator;
fleche->ref();
@@ -138,59 +142,55 @@ createArrow (float longueur,
SoTranslation *posePointe = new SoTranslation;
SoCone *pointe = new SoCone;
- float l_cylindre = longueur * ( 1 - proportionFleche);
+ float l_cylindre = longueur * (1 - proportionFleche);
float l_cone = longueur * proportionFleche;
float radius_cylindre = radius;
float radius_cone = radius * 5;
- line->radius.setValue (radius_cylindre);
- line->height.setValue (l_cylindre);
-
- poseCylindre->translation.setValue (0, l_cylindre / 2, 0);
- posePointe->translation.setValue (0.0, l_cylindre / 2 + l_cone / 2, 0);
+ line->radius.setValue(radius_cylindre);
+ line->height.setValue(l_cylindre);
- pointe->bottomRadius.setValue (radius_cone);
- pointe->height.setValue (l_cone);
+ poseCylindre->translation.setValue(0, l_cylindre / 2, 0);
+ posePointe->translation.setValue(0.0, l_cylindre / 2 + l_cone / 2, 0);
+ pointe->bottomRadius.setValue(radius_cone);
+ pointe->height.setValue(l_cone);
- fleche->addChild (poseCylindre);
- fleche->addChild (line);
- fleche->addChild (posePointe);
- fleche->addChild (pointe);
+ fleche->addChild(poseCylindre);
+ fleche->addChild(line);
+ fleche->addChild(posePointe);
+ fleche->addChild(pointe);
return fleche;
}
-
/*
Cree un objet repere dans un noeud separator, et le renvoie.
\return : code d'erreur, SIMU_CODE_OK si tout s'est bien passe.
*/
-#define LONGUEUR_FLECHE 1.0f
-#define RAYON_FLECHE 0.002f
-#define PROPORTION_FLECHE 0.1f
-
-SoSeparator *
-createFrame (float longueurFleche = LONGUEUR_FLECHE ,
- float proportionFleche = PROPORTION_FLECHE,
- float radiusFleche = RAYON_FLECHE)
+#define LONGUEUR_FLECHE 1.0f
+#define RAYON_FLECHE 0.002f
+#define PROPORTION_FLECHE 0.1f
+
+SoSeparator *createFrame(float longueurFleche = LONGUEUR_FLECHE, float proportionFleche = PROPORTION_FLECHE,
+ float radiusFleche = RAYON_FLECHE)
{
- vpDEBUG_TRACE (15, "# Entree.");
+ vpDEBUG_TRACE(15, "# Entree.");
SoSeparator *frame = new SoSeparator;
- frame-> ref ();
+ frame->ref();
SoRotationXYZ *rotationY_X = new SoRotationXYZ;
rotationY_X->axis = SoRotationXYZ::Z;
- rotationY_X->angle.setValue ((float)(- M_PI / 2));
+ rotationY_X->angle.setValue((float)(-M_PI / 2));
SoRotationXYZ *rotationX_Y = new SoRotationXYZ;
rotationX_Y->axis = SoRotationXYZ::Z;
- rotationX_Y->angle.setValue ((float)(M_PI / 2));
+ rotationX_Y->angle.setValue((float)(M_PI / 2));
SoRotationXYZ *rotationY_Z = new SoRotationXYZ;
rotationY_Z->axis = SoRotationXYZ::X;
- rotationY_Z->angle.setValue ((float)(M_PI / 2));
+ rotationY_Z->angle.setValue((float)(M_PI / 2));
SoMaterial *rouge = new SoMaterial;
rouge->diffuseColor.setValue(1.0, 0.0, 0.0);
@@ -204,33 +204,30 @@ 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);
- frame->addChild (fleche);
- frame->addChild (vert);
- frame->addChild (rotationX_Y);
- frame->addChild (fleche);
- frame->addChild (bleu);
- frame->addChild (rotationY_Z);
- frame->addChild (fleche);
+ frame->addChild(rouge);
+ frame->addChild(rotationY_X);
+ frame->addChild(fleche);
+ frame->addChild(vert);
+ frame->addChild(rotationX_Y);
+ frame->addChild(fleche);
+ frame->addChild(bleu);
+ frame->addChild(rotationY_Z);
+ frame->addChild(fleche);
- frame-> unrefNoDelete ();
+ frame->unrefNoDelete();
- vpDEBUG_TRACE (15, "# Sortie.");
+ vpDEBUG_TRACE(15, "# Sortie.");
return frame;
}
-SoSeparator *
-createCameraObject (const float zoomFactor = 1.0)
+SoSeparator *createCameraObject(const float zoomFactor = 1.0)
{
- vpDEBUG_TRACE (15, "# Entree.");
+ vpDEBUG_TRACE(15, "# Entree.");
- SoSeparator * cam = new SoSeparator;
- cam->ref ();
+ SoSeparator *cam = new SoSeparator;
+ cam->ref();
SoMaterial *myMaterial = new SoMaterial;
myMaterial->diffuseColor.setValue(1.0, 0.0, 0.0);
@@ -239,57 +236,55 @@ createCameraObject (const float zoomFactor = 1.0)
SoScale *taille = new SoScale;
{
float zoom = 0.1f * zoomFactor;
- taille->scaleFactor.setValue (zoom, zoom, zoom);
+ taille->scaleFactor.setValue(zoom, zoom, zoom);
}
SoMaterial *couleurBlanc = new SoMaterial;
couleurBlanc->diffuseColor.setValue(1.0, 1.0, 1.0);
couleurBlanc->emissiveColor.setValue(1.0, 1.0, 1.0);
- SoDrawStyle * filDeFer = new SoDrawStyle;
- filDeFer->style.setValue (SoDrawStyle::LINES);
- filDeFer->lineWidth.setValue (1);
+ SoDrawStyle *filDeFer = new SoDrawStyle;
+ filDeFer->style.setValue(SoDrawStyle::LINES);
+ filDeFer->lineWidth.setValue(1);
- SoSeparator * cone = new SoSeparator;
+ SoSeparator *cone = new SoSeparator;
cone->ref();
- cone->addChild (makePyramide());
- cone->addChild (couleurBlanc);
- cone->addChild (filDeFer);
- cone->addChild (makePyramide());
+ cone->addChild(makePyramide());
+ cone->addChild(couleurBlanc);
+ cone->addChild(filDeFer);
+ cone->addChild(makePyramide());
cone->unrefNoDelete();
cam->addChild(myMaterial);
cam->addChild(taille);
cam->addChild(cone);
- cam->addChild(createFrame(2.0f,0.1f,0.01f));
+ cam->addChild(createFrame(2.0f, 0.1f, 0.01f));
// cam->unref() ;
- vpDEBUG_TRACE (15, "# Sortie.");
+ vpDEBUG_TRACE(15, "# Sortie.");
return cam;
}
-
//--------------------------------------------------------------
-void
-vpSimulator::init()
+void vpSimulator::init()
{
internal_width = 200;
- internal_height= 200;
+ internal_height = 200;
external_width = 200;
- external_height= 200;
+ external_height = 200;
- mainWindowInitialized = false ;
- internalView = NULL ;
- externalView = NULL ;
- image_background = NULL ;
+ mainWindowInitialized = false;
+ internalView = NULL;
+ externalView = NULL;
+ image_background = NULL;
- zoomFactor = 1 ;
- cameraPositionInitialized = false ;
+ zoomFactor = 1;
+ cameraPositionInitialized = false;
// write image process
- realtime=NULL ;
- offScreenRenderer = NULL ;
+ realtime = NULL;
+ offScreenRenderer = NULL;
bufferView = NULL;
- get = 1 ;
+ get = 1;
typeImage = grayImage;
mainThread = NULL;
scene = NULL;
@@ -301,25 +296,31 @@ vpSimulator::init()
extrenalCameraPosition = NULL;
internalCameraObject = NULL;
#if defined(VISP_HAVE_SOWIN)
- // mainWindow = ?;
+// mainWindow = ?;
#elif defined(VISP_HAVE_SOQT)
mainWindow = NULL;
#elif defined(VISP_HAVE_SOXT)
- // mainWindow = ?;
+// mainWindow = ?;
#endif
-
}
-void
-vpSimulator::kill()
+void vpSimulator::kill()
{
- if (internalView !=NULL) {delete internalView ; internalView = NULL;}
- if (externalView !=NULL) {delete externalView ; externalView = NULL;}
- if (bufferView!=NULL) {delete[] bufferView ; bufferView = NULL;}
+ if (internalView != NULL) {
+ delete internalView;
+ internalView = NULL;
+ }
+ if (externalView != NULL) {
+ delete externalView;
+ externalView = NULL;
+ }
+ if (bufferView != NULL) {
+ delete[] bufferView;
+ bufferView = NULL;
+ }
if (image_background != NULL) {
- free (image_background);
+ free(image_background);
image_background = NULL;
}
-
}
vpSimulator::vpSimulator()
@@ -331,33 +332,25 @@ 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() ;
+ vpSimulator::init();
}
-vpSimulator::~vpSimulator()
-{
- vpSimulator::kill() ;
-}
+vpSimulator::~vpSimulator() { vpSimulator::kill(); }
-void
-vpSimulator::initSoApplication()
+void vpSimulator::initSoApplication()
{
mainWindow = vpViewer::init("");
- mainWindowInitialized = true ;
+ mainWindowInitialized = true;
}
-void
-vpSimulator::initSceneGraph()
+void vpSimulator::initSceneGraph()
{
this->scene = new SoSeparator;
this->internalRoot = new SoSeparator;
@@ -368,384 +361,343 @@ vpSimulator::initSceneGraph()
this->externalRoot->ref();
// define the camera SoPerspectiveCamera
- this->internalCamera = new SoPerspectiveCamera ;
- this->externalCamera = new SoPerspectiveCamera ;
-
-
+ this->internalCamera = new SoPerspectiveCamera;
+ this->externalCamera = new SoPerspectiveCamera;
+
this->internalCameraPosition = new SoTransform;
this->internalCameraObject = createCameraObject(zoomFactor);
internalCamera->farDistance.setValue(100);
internalCamera->nearDistance.setValue(0.0001f);
-
-
// link between camera and internal root
- this->internalRoot->addChild (this->internalCamera);
- this->internalRoot->addChild (this->scene);
-
- this->externalRoot->addChild (this->externalCamera);
- this->externalRoot->addChild (this->scene);
+ this->internalRoot->addChild(this->internalCamera);
+ this->internalRoot->addChild(this->scene);
+ this->externalRoot->addChild(this->externalCamera);
+ this->externalRoot->addChild(this->scene);
- SoSeparator * camera = new SoSeparator;
+ SoSeparator *camera = new SoSeparator;
camera->ref();
- camera->addChild (this->internalCameraPosition);
- camera->addChild (this->internalCameraObject);
- this->externalRoot->addChild (camera);
+ camera->addChild(this->internalCameraPosition);
+ camera->addChild(this->internalCameraObject);
+ this->externalRoot->addChild(camera);
-
- //this->externalRoot->addChild (internalCameraPosition);
+ // this->externalRoot->addChild (internalCameraPosition);
// this->externalRoot->addChild (internalCameraObject);
- SoCube *cube = new SoCube ;
- cube->width=0.01f ;
- cube->depth=0.01f ;
- cube->height=0.01f ;
+ SoCube *cube = new SoCube;
+ cube->width = 0.01f;
+ cube->depth = 0.01f;
+ cube->height = 0.01f;
- this->externalRoot->addChild (cube);
+ this->externalRoot->addChild(cube);
- if (realtime==NULL)
- {
+ if (realtime == NULL) {
SoDB::enableRealTimeSensor(FALSE);
SoSceneManager::enableRealTimeUpdate(FALSE);
- realtime = (SbTime *) SoDB::getGlobalField("realTime");
+ realtime = (SbTime *)SoDB::getGlobalField("realTime");
realtime->setValue(0.0);
-
}
-
-
}
-
-
/*!
- \brief Define the zoom factor used to define the size of the objects (frame,
+ \brief Define the zoom factor used to define the size of the objects (frame,
camera, ...)
\param zoom: zoom factor of the objects. By default, 1.
*/
-void
-vpSimulator::setZoomFactor (const float zoom)
+void vpSimulator::setZoomFactor(const float zoom)
{
zoomFactor = zoom;
static bool firstTime = true;
- if(firstTime){
+ if (firstTime) {
SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+ taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
this->scene->addChild(taille);
firstTime = false;
- }
- else{
- SoScale * taille = (SoScale*)this->scene->getChild(0);
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+ } else {
+ SoScale *taille = (SoScale *)this->scene->getChild(0);
+ taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
}
}
/*!
- \brief Change the zoom factor associated to the child given by index.
- In order to create multiple zoom factor for multiple object to display,
- objects loaded the load() function, you have to know the index of the scale
- object associated to.
-
- Usually, if you define the main zoom factor (for example for the frames) and
- then load two differents objects, You can change the zoom factor of all the
+ \brief Change the zoom factor associated to the child given by index.
+ In order to create multiple zoom factor for multiple object to display,
+ objects loaded the load() function, you have to know the index of the scale
+ object associated to.
+
+ Usually, if you define the main zoom factor (for example for the frames) and
+ then load two differents objects, You can change the zoom factor of all the
objects using: changeZoomFactor(newZoom, 0)
-
- If you want to change the zoom factor of the first object, use changeZoomFactor(newZoom, 1)
-
+
+ If you want to change the zoom factor of the first object, use
+ changeZoomFactor(newZoom, 1)
+
And for the second object, use changeZoomFactor(newZoom, 3)
-
- \param zoomFactor : the new zoom use to specify the apparent size of the object
- \param index : the index of the Scale object to modify (see comments)
+
+ \param zoomFactor : the new zoom use to specify the apparent size of the
+ object \param index : the index of the Scale object to modify (see comments)
*/
-void
-vpSimulator::changeZoomFactor(const float zoomFactor, const int index)
+void vpSimulator::changeZoomFactor(const float zoomFactor, const int index)
{
- SoScale * taille = (SoScale*)this->scene->getChild(index);
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
-// this->setZoomFactor(zoomFactor);
+ SoScale *taille = (SoScale *)this->scene->getChild(index);
+ taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
+ // this->setZoomFactor(zoomFactor);
}
-void
-vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height)
+void vpSimulator::initInternalViewer(const unsigned int width, const unsigned int height)
{
internal_width = width;
internal_height = height;
- if (mainWindowInitialized==false)
- {
- initSoApplication() ;
- initSceneGraph() ;
+ if (mainWindowInitialized == false) {
+ initSoApplication();
+ initSceneGraph();
}
- internalView = new vpViewer(mainWindow, this,vpViewer::internalView);
+ internalView = new vpViewer(mainWindow, this, vpViewer::internalView);
// set the scene to render from this view
internalView->setSceneGraph(internalRoot);
// set the title
- internalView->setTitle("Internal camera view") ;
+ internalView->setTitle("Internal camera view");
- //If the view mode is on, user events will be caught and used to influence
- //the camera position / orientation. in this viewer we do not want that,
- //we set it to false
+ // If the view mode is on, user events will be caught and used to influence
+ // the camera position / orientation. in this viewer we do not want that,
+ // we set it to false
internalView->setViewing(false);
// Turn the viewer decorations
- internalView->setDecoration(false) ;
+ internalView->setDecoration(false);
+
+ internalView->resize((int)width, (int)height, true);
- internalView->resize((int)width, (int)height, true) ;
-
// open the window
internalView->show();
- bufferView = new unsigned char[3*width*height] ;
-
+ 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;
external_height = height;
- if (mainWindowInitialized==false)
- {
- initSoApplication() ;
- initSceneGraph() ;
+ if (mainWindowInitialized == false) {
+ initSoApplication();
+ initSceneGraph();
}
- externalView = new vpViewer(mainWindow,this, vpViewer::externalView);
+ externalView = new vpViewer(mainWindow, this, vpViewer::externalView);
// set the scene to render this view
externalView->setSceneGraph(externalRoot);
// set the title
- externalView->setTitle("External View") ;
- externalView->resize((int)width, (int)height, false) ;
+ externalView->setTitle("External View");
+ externalView->resize((int)width, (int)height, false);
// the goal here is to see all the scene and not to determine
// a manual viewpoint
- externalView->viewAll ();
+ externalView->viewAll();
// open the window
externalView->show();
}
-void
-vpSimulator::setInternalCameraParameters(vpCameraParameters &_cam)
+void vpSimulator::setInternalCameraParameters(vpCameraParameters &_cam)
{
- internalCameraParameters = _cam ;
-
+ internalCameraParameters = _cam;
float px = (float)_cam.get_px();
float py = (float)_cam.get_py();
- float v = internal_height/(2.f*py);
+ float v = internal_height / (2.f * py);
- internalCamera->ref() ;
- internalCamera->heightAngle = 2*atan(v);
- internalCamera->aspectRatio=(internal_width/internal_height)*(px/py);
- internalCamera->nearDistance = 0.001f ;
+ internalCamera->ref();
+ internalCamera->heightAngle = 2 * atan(v);
+ internalCamera->aspectRatio = (internal_width / internal_height) * (px / py);
+ internalCamera->nearDistance = 0.001f;
internalCamera->farDistance = 1000;
- internalCamera->unrefNoDelete() ;
+ internalCamera->unrefNoDelete();
}
-void
-vpSimulator::setExternalCameraParameters(vpCameraParameters &_cam)
+void vpSimulator::setExternalCameraParameters(vpCameraParameters &_cam)
{
-// SoPerspectiveCamera *camera ;
-// camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
- externalCameraParameters = _cam ;
+ // SoPerspectiveCamera *camera ;
+ // camera = (SoPerspectiveCamera *)this->externalView->getCamera() ;
+ externalCameraParameters = _cam;
float px = (float)_cam.get_px();
float py = (float)_cam.get_py();
- float v = external_height/(2*py);
+ float v = external_height / (2 * py);
- externalCamera->ref() ;
- externalCamera->heightAngle = 2*atan(v);
- externalCamera->aspectRatio=(external_width/external_height)*(px/py);
- externalCamera->nearDistance = 0.001f ;
+ externalCamera->ref();
+ externalCamera->heightAngle = 2 * atan(v);
+ externalCamera->aspectRatio = (external_width / external_height) * (px / py);
+ externalCamera->nearDistance = 0.001f;
externalCamera->farDistance = 1000;
- externalCamera->unrefNoDelete() ;
-
+ externalCamera->unrefNoDelete();
}
-void
-vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf)
+void vpSimulator::getExternalCameraPosition(vpHomogeneousMatrix &cMf)
{
-/* SoCamera *camera ;
- camera = this->externalView->getCamera() ;*/
- SoSFVec3f position = externalCamera->position ;
+ /* SoCamera *camera ;
+ camera = this->externalView->getCamera() ;*/
+ SoSFVec3f position = externalCamera->position;
// get the rotation
- SoSFRotation orientation = externalCamera->orientation;
- SbVec3f axis ; float angle ;
- orientation.getValue(axis,angle) ;
- SbRotation rotation(axis,angle) ;
+ SoSFRotation orientation = externalCamera->orientation;
+ SbVec3f axis;
+ float angle;
+ orientation.getValue(axis, angle);
+ SbRotation rotation(axis, angle);
// get the translation
- SbVec3f t ;
- t = position.getValue() ;
+ SbVec3f t;
+ t = position.getValue();
- SbMatrix matrix ;
- matrix.setRotate(rotation) ;
+ SbMatrix matrix;
+ matrix.setRotate(rotation);
- vpHomogeneousMatrix fMc ;
+ vpHomogeneousMatrix fMc;
SbMatrix rotX;
- rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
- matrix.multLeft (rotX);
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- fMc[j][i]=matrix[(int)i][(int)j];
- fMc[0][3] = t[0] ;
- fMc[1][3] = t[1] ;
- fMc[2][3] = t[2] ;
-
- cMf = fMc.inverse() ;
-}
+ rotX.setRotate(SbRotation(SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
+ matrix.multLeft(rotX);
+ for (unsigned int i = 0; i < 4; i++)
+ for (unsigned int j = 0; j < 4; j++)
+ fMc[j][i] = matrix[(int)i][(int)j];
+ fMc[0][3] = t[0];
+ fMc[1][3] = t[1];
+ fMc[2][3] = t[2];
+ cMf = fMc.inverse();
+}
-void
-vpSimulator::setCameraPosition(vpHomogeneousMatrix &_cMf)
+void vpSimulator::setCameraPosition(vpHomogeneousMatrix &_cMf)
{
- cameraPositionInitialized = true ;
- cMf = _cMf ;
+ cameraPositionInitialized = true;
+ cMf = _cMf;
}
-void
-vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf)
+void vpSimulator::moveInternalCamera(vpHomogeneousMatrix &cMf)
{
SbMatrix matrix;
SbRotation rotCam;
SbMatrix rotX;
- rotX.setRotate (SbRotation (SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- matrix[(int)j][(int)i]=(float)cMf[i][j];
+ rotX.setRotate(SbRotation(SbVec3f(1.0f, 0.0f, 0.0f), (float)M_PI));
+ for (unsigned int i = 0; i < 4; i++)
+ for (unsigned int j = 0; j < 4; j++)
+ matrix[(int)j][(int)i] = (float)cMf[i][j];
- matrix= matrix.inverse();
- matrix.multLeft (rotX);
+ matrix = matrix.inverse();
+ matrix.multLeft(rotX);
rotCam.setValue(matrix);
-
- internalCamera->ref() ;
+ internalCamera->ref();
internalCamera->orientation.setValue(rotCam);
- internalCamera->position.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
- internalCamera->unref() ;
+ internalCamera->position.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
+ internalCamera->unref();
- rotX.setRotate (SbRotation (SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
- matrix.multLeft (rotX);
+ rotX.setRotate(SbRotation(SbVec3f(-1.0f, 0.0f, 0.0f), (float)M_PI));
+ matrix.multLeft(rotX);
rotCam.setValue(matrix);
- internalCameraPosition->ref() ;
+ internalCameraPosition->ref();
internalCameraPosition->rotation.setValue(rotCam);
- internalCameraPosition->translation.setValue(matrix[3][0],matrix[3][1],matrix[3][2]);
- internalCameraPosition->unref() ;
+ internalCameraPosition->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
+ internalCameraPosition->unref();
}
-
/*! this function MUST NOT be called from a thread where the vpSimulator and
its mainloop are not
*/
-void
-vpSimulator::redraw()
+void vpSimulator::redraw()
{
// if (this->cameraPositionInitialized==true)
{
- if (this->externalView != NULL)
- {
- this->externalView->render() ; //call actualRedraw()
+ if (this->externalView != NULL) {
+ this->externalView->render(); // call actualRedraw()
// vpHomogeneousMatrix c ;
// getExternalCameraPosition(c) ;
}
- if (this->internalView != NULL)
- {
- this->moveInternalCamera(this->cMf) ;
- this->internalView->render() ; //call actualRedraw()
+ if (this->internalView != NULL) {
+ this->moveInternalCamera(this->cMf);
+ this->internalView->render(); // call actualRedraw()
}
}
}
// This function is called 20 times each second.
-static void
-timerSensorCallback(void *data , SoSensor *)
+static void timerSensorCallback(void *data, SoSensor *)
{
- vpSimulator * simulator = (vpSimulator *)data ;
-
- simulator->redraw() ;
+ vpSimulator *simulator = (vpSimulator *)data;
+ simulator->redraw();
}
-
-void
-vpSimulator::mainLoop()
+void vpSimulator::mainLoop()
{
- if (mainWindowInitialized==false)
- {
- vpERROR_TRACE("main window is not opened ") ;
+ if (mainWindowInitialized == false) {
+ vpERROR_TRACE("main window is not opened ");
}
- vpTime::wait(1000) ;
+ vpTime::wait(1000);
// Timer sensor
- SoTimerSensor * timer = new SoTimerSensor(timerSensorCallback, (void *)this);
+ SoTimerSensor *timer = new SoTimerSensor(timerSensorCallback, (void *)this);
timer->setInterval(0.01);
timer->schedule();
- vpViewer::mainLoop() ;
+ vpViewer::mainLoop();
}
-
//-----------------------------------------------------------------
// scene stuff
//-----------------------------------------------------------------
-//!loading the virtual scene
-void
-vpSimulator::load(const char *file_name)
+//! loading the virtual scene
+void vpSimulator::load(const char *file_name)
{
SoInput input;
- if (!input.openFile(file_name))
- {
- vpERROR_TRACE("Erreur cannot open file %s",file_name);
+ if (!input.openFile(file_name)) {
+ vpERROR_TRACE("Erreur cannot open file %s", file_name);
}
- SoSeparator *newscene=SoDB::readAll(&input);
- newscene->ref() ;
- if (newscene==NULL)
- {
- vpERROR_TRACE("Error while reading %s",file_name);
+ SoSeparator *newscene = SoDB::readAll(&input);
+ newscene->ref();
+ if (newscene == NULL) {
+ vpERROR_TRACE("Error while reading %s", file_name);
}
-
+
SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoomFactor, zoomFactor, zoomFactor);
+ taille->scaleFactor.setValue(zoomFactor, zoomFactor, zoomFactor);
-// newscene->addChild(taille);
+ // newscene->addChild(taille);
-// std::cout << "this->scene->getNumChildren() = " << this->scene->getNumChildren() << std::endl;
+ // std::cout << "this->scene->getNumChildren() = " <<
+ // this->scene->getNumChildren() << std::endl;
this->scene->addChild(taille);
this->scene->addChild(newscene);
- newscene->unref() ;
-
+ newscene->unref();
}
-
-void
-vpSimulator::save(const char *name,bool binary)
+void vpSimulator::save(const char *name, bool binary)
{
// get a pointer to the object "name"
- SoOutput output ;
- output.openFile(name) ;
-
- if (binary==true) output.setBinary(TRUE) ;
+ SoOutput output;
+ output.openFile(name);
- SoWriteAction writeAction(&output) ;
- writeAction.apply(scene) ;
- output.closeFile() ;
+ if (binary == true)
+ output.setBinary(TRUE);
+ SoWriteAction writeAction(&output);
+ writeAction.apply(scene);
+ output.closeFile();
}
/*!
@@ -753,30 +705,28 @@ vpSimulator::save(const char *name,bool binary)
\param fMo : desired position of the frame
\param zoom : Zoom factor.
*/
-void
-vpSimulator::addFrame (const vpHomogeneousMatrix &fMo, float zoom)
+void vpSimulator::addFrame(const vpHomogeneousMatrix &fMo, float zoom)
{
SoScale *taille = new SoScale;
- taille->scaleFactor.setValue (zoom, zoom, zoom);
+ taille->scaleFactor.setValue(zoom, zoom, zoom);
- SoSeparator * frame = new SoSeparator;
+ SoSeparator *frame = new SoSeparator;
frame->ref();
frame->addChild(taille);
- frame->addChild(createFrame (LONGUEUR_FLECHE*zoom, PROPORTION_FLECHE*zoom, RAYON_FLECHE*zoom));
- this->addObject(frame, fMo, externalRoot) ;
+ frame->addChild(createFrame(LONGUEUR_FLECHE * zoom, PROPORTION_FLECHE * zoom, RAYON_FLECHE * zoom));
+ this->addObject(frame, fMo, externalRoot);
// frame->unref();
}
/*!
\brief Add the representation of the absolute frame
-
+
\param zoom : Zoom factor.
*/
-void
-vpSimulator::addAbsoluteFrame (float zoom)
+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));
}
/*!
@@ -784,60 +734,44 @@ 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;
- SoSeparator * newObject;
+ SoSeparator *newObject;
- if (! in.openFile (iv_filename))
- {
- vpERROR_TRACE ("Erreur lors de la lecture du fichier %s.", iv_filename);
+ if (!in.openFile(iv_filename)) {
+ vpERROR_TRACE("Erreur lors de la lecture du fichier %s.", iv_filename);
}
- newObject = SoDB::readAll (&in);
- if (NULL == newObject)
- {
- vpERROR_TRACE ("Problem reading data for file <%s>.", iv_filename);
+ newObject = SoDB::readAll(&in);
+ if (NULL == newObject) {
+ vpERROR_TRACE("Problem reading data for file <%s>.", iv_filename);
}
- try
- {
- this->addObject (newObject, fMo) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error adding object from file <%s> ",iv_filename) ;
- throw ;
+ try {
+ this->addObject(newObject, fMo);
+ } catch (...) {
+ vpERROR_TRACE("Error adding object from file <%s> ", iv_filename);
+ throw;
}
-
}
-
/*!
\brief Add a new object in the scene graph
\param newObject : pointer toward the new object
\param fMo : position of the object wrt the reference frame
*/
-void
-vpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo)
+void vpSimulator::addObject(SoSeparator *newObject, const vpHomogeneousMatrix &fMo)
{
- try
- {
- this->addObject(newObject, fMo , scene);
- }
- catch(...)
- {
- vpERROR_TRACE("Error adding object in scene graph ") ;
- throw ;
+ try {
+ this->addObject(newObject, fMo, scene);
+ } catch (...) {
+ vpERROR_TRACE("Error adding object in scene graph ");
+ throw;
}
}
-
-
-
-
/*!
\brief Add an object in a sub scene graph
\param object : pointer toward the new object
@@ -845,100 +779,83 @@ vpSimulator::addObject(SoSeparator * newObject, const vpHomogeneousMatrix &fMo)
\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 ;
- for (unsigned int i=0 ; i <4 ;i++){
- for (unsigned int j=0 ; j < 4 ; j++){
- if (i==j){
- if (fabs(fMo[i][j] -1) > 1e-6) identity=false ;
- }
- else{
- if (fabs(fMo[i][j]) > 1e-6) identity=false ;
+ bool identity = true;
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 0; j < 4; j++) {
+ if (i == j) {
+ if (fabs(fMo[i][j] - 1) > 1e-6)
+ identity = false;
+ } else {
+ if (fabs(fMo[i][j]) > 1e-6)
+ identity = false;
}
}
}
- if (identity==true)
- {
- root->addChild (object);
- }
- else
- {
+ if (identity == true) {
+ root->addChild(object);
+ } else {
SbMatrix matrix;
SbRotation rotation;
- for(unsigned int i=0;i<4;i++)
- for(unsigned int j=0;j<4;j++)
- matrix[(int)j][(int)i]=(float)fMo[i][j];
+ for (unsigned int i = 0; i < 4; i++)
+ for (unsigned int j = 0; j < 4; j++)
+ matrix[(int)j][(int)i] = (float)fMo[i][j];
// matrix= matrix.inverse();
rotation.setValue(matrix);
SoTransform *displacement = new SoTransform;
- SoSeparator *newNode = new SoSeparator;
+ SoSeparator *newNode = new SoSeparator;
displacement->rotation.setValue(rotation);
- displacement->translation.setValue(matrix[3][0],
- matrix[3][1],
- matrix[3][2]);
-
- root->addChild (newNode);
- newNode->addChild (displacement);
- newNode->addChild (object);
+ displacement->translation.setValue(matrix[3][0], matrix[3][1], matrix[3][2]);
+ root->addChild(newNode);
+ newNode->addChild(displacement);
+ newNode->addChild(object);
}
}
-
//! init the main program thread
-void
-vpSimulator::initApplication(void *(*start_routine)(void *))
+void vpSimulator::initApplication(void *(*start_routine)(void *))
{
- //pthread_create (&mainThread, NULL, start_routine, (void *)this);
- mainThread = SbThread::create (start_routine, (void *)this);
+ // pthread_create (&mainThread, NULL, start_routine, (void *)this);
+ mainThread = SbThread::create(start_routine, (void *)this);
}
/*!
Set the function used for the simulation loop and the data to pass to this
- function. As the data are represented using a generic pointer, care should be
- taken to ensure there is no memory corruption.
+ function. As the data are represented using a generic pointer, care should
+ be taken to ensure there is no memory corruption.
\param start_routine : A pointer to the function used as a main simulation
loop for the simulation.
\param data : The data to pass to the main loop.
*/
-void
-vpSimulator::initApplication(void *(*start_routine)(void *), void* data)
+void vpSimulator::initApplication(void *(*start_routine)(void *), void *data)
{
- mainThread = SbThread::create (start_routine, (void *)data);
+ mainThread = SbThread::create(start_routine, (void *)data);
}
//! performed some initialization in the main program thread
//! should be locate at the beginning of the main program
-void
-vpSimulator::initMainApplication()
+void vpSimulator::initMainApplication()
{
- //pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
- //pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
- vpTime::wait(1000) ;
+ // pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, NULL );
+ // pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);
+ vpTime::wait(1000);
}
//! performed some thread destruction in the main program thread
//! should be locate at the end of the main program
-void
-vpSimulator::closeMainApplication()
+void vpSimulator::closeMainApplication()
{
- vpViewer::exitMainLoop() ;
- //pthread_exit (NULL);
+ vpViewer::exitMainLoop();
+ // pthread_exit (NULL);
}
-
-
-
-
/* Initialise le SoOffScreenRenderer si necessaire, puis realise le rendu.
* Quand la fonction rend la main, le buffer est pret et n'a plus qu'a etre
* enregistre ou passe a l'utilisateur.
@@ -949,49 +866,38 @@ 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);
- SoNode * thisroot;
+ SbVec2s size(320, 200);
+ SoNode *thisroot;
{
- if (view==vpSimulator::INTERNAL)
- {
- size = this ->internalView ->getViewportRegion().getWindowSize();
- thisroot = this ->internalView->getSceneManager()->getSceneGraph() ;
- }
- else
- {
- size = this ->externalView ->getViewportRegion().getWindowSize();
- thisroot = this ->externalView->getSceneManager()->getSceneGraph() ;
+ if (view == vpSimulator::INTERNAL) {
+ size = this->internalView->getViewportRegion().getWindowSize();
+ thisroot = this->internalView->getSceneManager()->getSceneGraph();
+ } else {
+ size = this->externalView->getViewportRegion().getWindowSize();
+ thisroot = this->externalView->getSceneManager()->getSceneGraph();
}
}
SbViewportRegion myViewPort(size);
// Creation du rendu si necessaire.
- if (NULL == this ->offScreenRenderer)
- {
- //Init du SoOffscreenRenderer
- this ->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
- }
- else
- {
+ if (NULL == this->offScreenRenderer) {
+ // Init du SoOffscreenRenderer
+ this->offScreenRenderer = new SoOffscreenRenderer(myViewPort);
+ } else {
// Redefini le view port
- this ->offScreenRenderer ->setViewportRegion (myViewPort);
+ this->offScreenRenderer->setViewportRegion(myViewPort);
}
// Rendu offscreen
- if (! this ->offScreenRenderer ->render(thisroot))
- {
+ if (!this->offScreenRenderer->render(thisroot)) {
vpERROR_TRACE("La scene n'a pas pu etre rendue offscreen.");
- delete this ->offScreenRenderer;
- this ->offScreenRenderer = NULL;
- }
- else
- {
-
+ delete this->offScreenRenderer;
+ this->offScreenRenderer = NULL;
+ } else {
/*
if (view==vpSimulator::INTERNAL)
@@ -1005,17 +911,17 @@ vpSimulator::offScreenRendering(vpSimulatorViewType view, int * width, int * hei
bufferView[i] = this ->offScreenRenderer->getBuffer()[i];
}
}*/
-
}
// exit(1) ;
- if (NULL != width) { * width = size [0]; }
- if (NULL != height) { * height = size [1]; }
-
-
+ if (NULL != width) {
+ *width = size[0];
+ }
+ if (NULL != height) {
+ *height = size[1];
+ }
}
-
/* Enregistre l'image de vue interne ou externe dans un fichier RGB.
* Effectue le rendu dans un buffer plutot qu'a l'ecran, puis sauvegarde
* ce buffer au format PS (copie directe).
@@ -1026,74 +932,72 @@ vpSimulator::offScreenRendering(vpSimulatorViewType view, int * width, int * hei
*/
#ifdef VISP_HAVE_MODULE_IO
-void
-vpSimulator::write (const char * fileName)
+void vpSimulator::write(const char *fileName)
{
- while (get==0) { vpTRACE("%d ",get); }
- get =2 ;
+ while (get == 0) {
+ vpTRACE("%d ", get);
+ }
+ get = 2;
/* FILE *fp = fopen(fileName, "w");
fprintf(fp,"P6 \n %d %d \n 255",internal_width,internal_height) ;
- fwrite(bufferView, sizeof(unsigned char), internal_width*internal_height*3, fp) ;*/
- vpImage<vpRGBa> I(internal_height, internal_width) ;
-
-
- for(unsigned int i=0 ; i < internal_height ; i++)
- for(unsigned int j=0 ; j < internal_width ; j++)
- {
- unsigned char r,g,b ;
- unsigned int index = 3*((internal_height-i-1)* internal_width + j );
- r = *(bufferView+index);
- g = *(bufferView+index+1);
- b = *(bufferView+index+2);
- I[i][j].R =r ;
- I[i][j].G =g ;
- I[i][j].B =b ;
+ fwrite(bufferView, sizeof(unsigned char),
+ internal_width*internal_height*3, fp) ;*/
+ vpImage<vpRGBa> I(internal_height, internal_width);
+
+ for (unsigned int i = 0; i < internal_height; i++)
+ for (unsigned int j = 0; j < internal_width; j++) {
+ unsigned char r, g, b;
+ unsigned int index = 3 * ((internal_height - i - 1) * internal_width + j);
+ r = *(bufferView + index);
+ g = *(bufferView + index + 1);
+ b = *(bufferView + index + 2);
+ I[i][j].R = r;
+ I[i][j].G = g;
+ I[i][j].B = b;
}
- vpImageIo::write(I,fileName) ;
+ vpImageIo::write(I, fileName);
// fclose (fp);
- get =1 ;
+ get = 1;
}
#endif
-void
-vpSimulator::getSizeInternalView(int& width, int& height)
+void vpSimulator::getSizeInternalView(int &width, int &height)
{
- SbVec2s size = this ->internalView ->getViewportRegion().getWindowSize();
- width = size [0];
+ SbVec2s size = this->internalView->getViewportRegion().getWindowSize();
+ width = size[0];
height = size[1];
}
-/*!
+/*!
Make a copy of the current internal view
\param I : destination image
*/
-void
-vpSimulator::getInternalImage(vpImage<vpRGBa> &I)
+void vpSimulator::getInternalImage(vpImage<vpRGBa> &I)
{
- //while (get==0) {;}
- get =2 ;
- I.resize(internal_height,internal_width) ;
- vpImageConvert::RGBToRGBa(bufferView,(unsigned char*)I.bitmap,internal_width,internal_height,true);
- get =1 ;
+ // while (get==0) {;}
+ get = 2;
+ I.resize(internal_height, internal_width);
+ vpImageConvert::RGBToRGBa(bufferView, (unsigned char *)I.bitmap, internal_width, internal_height, true);
+ get = 1;
}
-/*!
+/*!
Make a copy of the current internal view
\param I : destination image
*/
-void
-vpSimulator::getInternalImage(vpImage<unsigned char> &I)
+void vpSimulator::getInternalImage(vpImage<unsigned char> &I)
{
- //while (get==0) {;}
- get =2 ;
- I.resize(internal_height,internal_width) ;
- vpImageConvert::RGBToGrey(bufferView,I.bitmap,internal_width,internal_height,true);
- get =1 ;
+ // while (get==0) {;}
+ get = 2;
+ I.resize(internal_height, internal_width);
+ vpImageConvert::RGBToGrey(bufferView, I.bitmap, internal_width, internal_height, true);
+ get = 1;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no symbols
-void dummy_vpSimulator() {};
+// Work arround to avoid warning: libvisp_ar.a(vpSimulator.cpp.o) has no
+// symbols
+void dummy_vpSimulator(){};
#endif
diff --git a/modules/ar/src/coin-simulator/vpViewer.cpp b/modules/ar/src/coin-simulator/vpViewer.cpp
index e611477..7316886 100644
--- a/modules/ar/src/coin-simulator/vpViewer.cpp
+++ b/modules/ar/src/coin-simulator/vpViewer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,33 +38,31 @@
/*!
\file vpViewer.cpp
Viewer used by the simulator. Under Windows, the viewer is
- based either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or SoXt.
+ based either on SoWin or SoQt. Under Unix, the viewer is based on SoQt or
+ SoXt.
*/
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_COIN3D_AND_GUI
-#include <visp3/ar/vpViewer.h>
#include <visp3/ar/vpSimulator.h>
-
+#include <visp3/ar/vpViewer.h>
#include <Inventor/events/SoKeyboardEvent.h>
#include <Inventor/nodes/SoEventCallback.h>
-
#if defined(VISP_HAVE_SOWIN)
-vpViewer::vpViewer(HWND parent, vpSimulator *_simu, vpViewerType viewerType):
- SoWinExaminerViewer(parent,(char *)NULL,false)
+vpViewer::vpViewer(HWND parent, vpSimulator *_simu, vpViewerType type)
+ : viewerType(type), SoWinExaminerViewer(parent, (char *)NULL, false)
#elif defined(VISP_HAVE_SOQT)
-vpViewer::vpViewer(QWidget * parent, vpSimulator *_simu, vpViewerType viewerType) :
- SoQtExaminerViewer(parent,(char *)NULL,false)
+vpViewer::vpViewer(QWidget *parent, vpSimulator *_simu, vpViewerType type)
+ : viewerType(type), SoQtExaminerViewer(parent, (char *)NULL, false)
#elif defined(VISP_HAVE_SOXT)
-vpViewer::vpViewer(Widget parent, vpSimulator *_simu, vpViewerType viewerType):
- SoXtExaminerViewer(parent,(char *)NULL,false)
+vpViewer::vpViewer(Widget parent, vpSimulator *_simu, vpViewerType type)
+ : viewerType(type), SoXtExaminerViewer(parent, (char *)NULL, false)
#endif
{
- this->simu = _simu ;
- this->viewerType = viewerType;
+ this->simu = _simu;
// Coin should not clear the pixel-buffer, so the background image
// is not removed.
@@ -72,46 +71,37 @@ vpViewer::vpViewer(Widget parent, vpSimulator *_simu, vpViewerType viewerType):
setAutoRedraw(false);
}
-vpViewer::~vpViewer()
-{
+vpViewer::~vpViewer() {}
-}
-
-void
-vpViewer::actualRedraw(void)
+void vpViewer::actualRedraw(void)
{
{
- const SbViewportRegion vp = this->getViewportRegion();
- SbVec2s origin = vp.getViewportOriginPixels();
- SbVec2s size = vp.getViewportSizePixels();
- glViewport(origin[0], origin[1], size[0], size[1]);
-
- const SbColor col = this->getBackgroundColor();
- glClearColor(col[0], col[1], col[2], 0.0f);
- glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
-
- // this should be used only with the vpAR:vpSimulator
- // to diplay an image background
- if (simu->image_background != NULL)
- {
- glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
- if (simu->typeImage == vpSimulator::grayImage)
- glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_LUMINANCE,
- GL_UNSIGNED_BYTE,
- simu->image_background );
- else
- glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_RGB,
- GL_UNSIGNED_BYTE,
- simu->image_background );
-
- glEnable(GL_DEPTH_TEST);
- glClear(GL_DEPTH_BUFFER_BIT); // clear the z-buffer
- glClearDepth(100.0); // Profondeur du Z-Buf
- }
- // Render normal scenegraph.
+ const SbViewportRegion vp = this->getViewportRegion();
+ SbVec2s origin = vp.getViewportOriginPixels();
+ SbVec2s size = vp.getViewportSizePixels();
+ glViewport(origin[0], origin[1], size[0], size[1]);
+
+ const SbColor col = this->getBackgroundColor();
+ glClearColor(col[0], col[1], col[2], 0.0f);
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ // this should be used only with the vpAR:vpSimulator
+ // to diplay an image background
+ if (simu->image_background != NULL) {
+ glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+ if (simu->typeImage == vpSimulator::grayImage)
+ glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(), (GLenum)GL_LUMINANCE,
+ GL_UNSIGNED_BYTE, simu->image_background);
+ else
+ glDrawPixels((GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(), (GLenum)GL_RGB,
+ GL_UNSIGNED_BYTE, simu->image_background);
+
+ glEnable(GL_DEPTH_TEST);
+ glClear(GL_DEPTH_BUFFER_BIT); // clear the z-buffer
+ glClearDepth(100.0); // Profondeur du Z-Buf
+ }
+// Render normal scenegraph.
#if defined(VISP_HAVE_SOWIN)
SoWinExaminerViewer::actualRedraw();
#elif defined(VISP_HAVE_SOQT)
@@ -119,17 +109,14 @@ vpViewer::actualRedraw(void)
#elif defined(VISP_HAVE_SOXT)
SoXtExaminerViewer::actualRedraw();
#endif
- glSwapBuffers() ;
- if(viewerType == vpViewer::internalView){
- simu->get = 0 ;
- glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(),
- (GLenum)GL_RGB,
- GL_UNSIGNED_BYTE,
- simu->bufferView ) ;
- simu->get =1 ;
+ glSwapBuffers();
+ if (viewerType == vpViewer::internalView) {
+ simu->get = 0;
+ glReadPixels(0, 0, (GLsizei)simu->getInternalWidth(), (GLsizei)simu->getInternalHeight(), (GLenum)GL_RGB,
+ GL_UNSIGNED_BYTE, simu->bufferView);
+ simu->get = 1;
}
- }
-
+ }
}
/*!
@@ -140,15 +127,15 @@ vpViewer::actualRedraw(void)
*/
void
-#if defined(VISP_HAVE_SOWIN) || defined (VISP_HAVE_SOQT)
+#if defined(VISP_HAVE_SOWIN) || defined(VISP_HAVE_SOQT)
vpViewer::resize(int x, int y, bool fixed)
#else
vpViewer::resize(int x, int y, bool /*fixed*/)
#endif
{
- SbVec2s size(x,y) ;
+ SbVec2s size(x, y);
setSize(size);
- setGLSize(size) ;
+ setGLSize(size);
#if defined(VISP_HAVE_SOWIN)
HWND parent = getParentWidget();
@@ -159,19 +146,18 @@ 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);
- if(fixed){
+ MoveWindow(parent, rcWindow.left, rcWindow.top, x + ptDiff.x, y + ptDiff.y, TRUE);
+ if (fixed) {
DWORD dwStyle = GetWindowLong(parent, GWL_STYLE);
dwStyle &= ~(WS_SIZEBOX);
- SetWindowLong(parent, GWL_STYLE,dwStyle);
+ SetWindowLong(parent, GWL_STYLE, dwStyle);
}
#elif defined(VISP_HAVE_SOQT)
- if(fixed){
- QWidget * parent = getParentWidget();
+ if (fixed) {
+ QWidget *parent = getParentWidget();
parent->setFixedSize(x, y);
}
-#endif
+#endif
}
/*!
@@ -182,67 +168,59 @@ vpViewer::resize(int x, int y, bool /*fixed*/)
*/
-SbBool
-vpViewer::processSoEvent(const SoEvent * const event)
+SbBool vpViewer::processSoEvent(const SoEvent *const event)
{
- if ( this->isViewing() &&
- event->getTypeId() == SoKeyboardEvent::getClassTypeId() )
- {
- SoKeyboardEvent * kbevent = (SoKeyboardEvent *) event;
- switch ( kbevent->getKey() ) {
+ if (this->isViewing() && event->getTypeId() == SoKeyboardEvent::getClassTypeId()) {
+ SoKeyboardEvent *kbevent = (SoKeyboardEvent *)event;
+ switch (kbevent->getKey()) {
case SoKeyboardEvent::H:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- std::cout << "H : this help "<<std::endl ;
- std::cout << "M : get and save the external camera location (matrix)"<<std::endl;
- std::cout << "V : get and save the external camera location (vector)"<<std::endl;
- std::cout << "M : load camera location (vector)"<<std::endl;
- std::cout << "P : get external camera location and set the internal one"<<std::endl;
+ if (kbevent->getState() == SoButtonEvent::DOWN) {
+ std::cout << "H : this help " << std::endl;
+ std::cout << "M : get and save the external camera location (matrix)" << std::endl;
+ std::cout << "V : get and save the external camera location (vector)" << std::endl;
+ std::cout << "M : load camera location (vector)" << std::endl;
+ std::cout << "P : get external camera location and set the internal one" << std::endl;
}
return TRUE;
case SoKeyboardEvent::M:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- std::ofstream f("cMf.dat") ;
- cMf.save(f) ;
- f.close() ;
+ if (kbevent->getState() == SoButtonEvent::DOWN) {
+ vpHomogeneousMatrix cMf;
+ simu->getExternalCameraPosition(cMf);
+ std::ofstream f("cMf.dat");
+ cMf.save(f);
+ f.close();
}
return TRUE;
case SoKeyboardEvent::V:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- vpPoseVector vcMf(cMf) ;
- std::ofstream f("vcMf.dat") ;
- vcMf.save(f) ;
- f.close() ;
+ if (kbevent->getState() == SoButtonEvent::DOWN) {
+ vpHomogeneousMatrix cMf;
+ simu->getExternalCameraPosition(cMf);
+ vpPoseVector vcMf(cMf);
+ std::ofstream f("vcMf.dat");
+ vcMf.save(f);
+ f.close();
}
return TRUE;
case SoKeyboardEvent::L:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpPoseVector vcMf;
- std::ifstream f("vcMf.dat") ;
- vcMf.load(f) ;
- f.close() ;
- vpHomogeneousMatrix cMf(vcMf) ;
- simu->setCameraPosition(cMf) ;
- simu->moveInternalCamera(cMf) ;
+ if (kbevent->getState() == SoButtonEvent::DOWN) {
+ vpPoseVector vcMf;
+ std::ifstream f("vcMf.dat");
+ vcMf.load(f);
+ f.close();
+ vpHomogeneousMatrix cMf(vcMf);
+ simu->setCameraPosition(cMf);
+ simu->moveInternalCamera(cMf);
}
return TRUE;
case SoKeyboardEvent::P:
- if ( kbevent->getState() == SoButtonEvent::DOWN )
- {
- vpHomogeneousMatrix cMf ;
- simu->getExternalCameraPosition(cMf) ;
- vpPoseVector vcMf(cMf) ;
- vcMf.print() ;
- simu->setCameraPosition(cMf) ;
- simu->moveInternalCamera(cMf) ;
+ if (kbevent->getState() == SoButtonEvent::DOWN) {
+ vpHomogeneousMatrix cMf;
+ simu->getExternalCameraPosition(cMf);
+ vpPoseVector vcMf(cMf);
+ vcMf.print();
+ simu->setCameraPosition(cMf);
+ simu->moveInternalCamera(cMf);
}
return TRUE;
default:
@@ -250,16 +228,15 @@ vpViewer::processSoEvent(const SoEvent * const event)
}
}
#if defined(VISP_HAVE_SOWIN)
- return SoWinExaminerViewer::processSoEvent(event);
+ return SoWinExaminerViewer::processSoEvent(event);
#elif defined(VISP_HAVE_SOQT)
- return SoQtExaminerViewer::processSoEvent(event);
+ return SoQtExaminerViewer::processSoEvent(event);
#elif defined(VISP_HAVE_SOXT)
- return SoXtExaminerViewer::processSoEvent(event);
+ return SoXtExaminerViewer::processSoEvent(event);
#endif
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_ar.a(vpViewer.cpp.o) has no symbols
-void dummy_vpViewer() {};
+void dummy_vpViewer(){};
#endif
diff --git a/modules/ar/src/ogre-simulator/vpAROgre.cpp b/modules/ar/src/ogre-simulator/vpAROgre.cpp
index 9ee5855..48c1fe9 100644
--- a/modules/ar/src/ogre-simulator/vpAROgre.cpp
+++ b/modules/ar/src/ogre-simulator/vpAROgre.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,13 +46,14 @@
*/
-#include "visp3/core/vpConfig.h"
+#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_OGRE
-#include "visp3/ar/vpAROgre.h"
-#include "visp3/core/vpIoTools.h"
+#include <visp3/ar/vpAROgre.h>
+#include <visp3/core/vpIoTools.h>
+#include <OgreRectangle2D.h>
/*!
Constructor.
@@ -68,19 +70,17 @@
telling Ogre where to look for renderer plugins.
*/
-vpAROgre::vpAROgre(const vpCameraParameters &cam,
- unsigned int width, unsigned int height,
- const char *resourcePath, const char *pluginsPath)
- : name("ViSP - Augmented Reality"),mRoot(0), mCamera(0), mSceneMgr(0), mWindow(0),
- mResourcePath(resourcePath), mPluginsPath(pluginsPath),
+vpAROgre::vpAROgre(const vpCameraParameters &cam, unsigned int width, unsigned int height, const char *resourcePath,
+ const char *pluginsPath)
+ : name("ViSP - Augmented Reality"), mRoot(0), mCamera(0), mSceneMgr(0), mWindow(0), mResourcePath(resourcePath),
+ mPluginsPath(pluginsPath),
#ifdef VISP_HAVE_OIS
- mInputManager(0), mKeyboard(0),
+ mInputManager(0), mKeyboard(0),
#endif
- keepOn(true), // When created no reason to stop displaying
- mImageRGBA(), mImage(), mPixelBuffer(NULL), mBackground(NULL), mBackgroundHeight(0),
- mBackgroundWidth(0), mWindowHeight(height), mWindowWidth(width), windowHidden(false),
- mNearClipping(0.001), mFarClipping(200), mcam(cam), mshowConfigDialog(true),
- mOptionnalResourceLocation()
+ 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()
{
}
@@ -113,24 +113,23 @@ vpAROgre::vpAROgre(const vpCameraParameters &cam,
*/
void vpAROgre::init(vpImage<unsigned char> &I,
- bool
+ bool
#ifdef VISP_HAVE_OIS
- bufferedKeys
+ bufferedKeys
#endif
- ,bool hidden
- )
+ ,
+ bool hidden)
{
mBackgroundWidth = I.getWidth();
mBackgroundHeight = I.getHeight();
init(
#ifdef VISP_HAVE_OIS
- bufferedKeys,
+ bufferedKeys,
#else
false,
#endif
- hidden
- );
+ hidden);
// Create the background image which will come from the grabber
createBackground(I);
}
@@ -164,24 +163,23 @@ void vpAROgre::init(vpImage<unsigned char> &I,
*/
void vpAROgre::init(vpImage<vpRGBa> &I,
- bool
+ bool
#ifdef VISP_HAVE_OIS
- bufferedKeys
+ bufferedKeys
#endif
- ,bool hidden
- )
+ ,
+ bool hidden)
{
mBackgroundWidth = I.getWidth();
mBackgroundHeight = I.getHeight();
init(
#ifdef VISP_HAVE_OIS
- bufferedKeys,
+ bufferedKeys,
#else
- false,
+ false,
#endif
- hidden
- );
+ hidden);
// Create the background image which will come from the grabber
createBackground(I);
}
@@ -212,44 +210,43 @@ void vpAROgre::init(vpImage<vpRGBa> &I,
*/
void vpAROgre::init(bool
#ifdef VISP_HAVE_OIS
- bufferedKeys
+ bufferedKeys
#endif
- ,bool hidden
- )
+ ,
+ bool hidden)
{
// Create the root
// mPluginsPath may contain more than one folder location separated by ";"
bool pluginsFileExists = false;
std::string pluginFile;
std::vector<std::string> plugingsPaths = vpIoTools::splitChain(std::string(mPluginsPath), std::string(";"));
- for (size_t i=0; i<plugingsPaths.size(); i++) {
+ for (size_t i = 0; i < plugingsPaths.size(); i++) {
#if defined(NDEBUG) || !defined(_WIN32)
- pluginFile = plugingsPaths[i]+"/plugins.cfg";
+ pluginFile = plugingsPaths[i] + "/plugins.cfg";
#else
- pluginFile = plugingsPaths[i]+"/plugins_d.cfg";
+ pluginFile = plugingsPaths[i] + "/plugins_d.cfg";
#endif
- if(vpIoTools::checkFilename(pluginFile)) {
+ if (vpIoTools::checkFilename(pluginFile)) {
pluginsFileExists = true;
break;
}
}
- if (! pluginsFileExists) {
+ if (!pluginsFileExists) {
std::string errorMsg = std::string("Error: the requested plugins file \"")
#if defined(NDEBUG) || !defined(_WIN32)
- + std::string("plugins.cfg")
+ + std::string("plugins.cfg")
#else
- + std::string("plugins_d.cfg")
+ + 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));
+ throw(vpException(vpException::ioError, errorMsg));
}
std::cout << "######################### Load plugin file: " << pluginFile << std::endl;
- if(Ogre::Root::getSingletonPtr() == NULL)
+ if (Ogre::Root::getSingletonPtr() == NULL)
mRoot = new Ogre::Root(pluginFile, "ogre.cfg", "Ogre.log");
else
mRoot = Ogre::Root::getSingletonPtr();
@@ -269,21 +266,20 @@ void vpAROgre::init(bool
bool resourcesFileExists = false;
std::string resourceFile;
std::vector<std::string> resourcesPaths = vpIoTools::splitChain(std::string(mResourcePath), std::string(";"));
- for (size_t i=0; i<resourcesPaths.size(); i++) {
- resourceFile = resourcesPaths[i]+"/resources.cfg";
- if(vpIoTools::checkFilename(resourceFile)) {
+ for (size_t i = 0; i < resourcesPaths.size(); i++) {
+ resourceFile = resourcesPaths[i] + "/resources.cfg";
+ if (vpIoTools::checkFilename(resourceFile)) {
resourcesFileExists = true;
break;
}
}
- if (! resourcesFileExists) {
- std::string errorMsg = std::string("Error: the requested resource file \"resources.cfg\"")
- + std::string("doesn't exist in ")
- + std::string(mResourcePath);
+ if (!resourcesFileExists) {
+ std::string errorMsg = std::string("Error: the requested resource file \"resources.cfg\"") +
+ std::string("doesn't exist in ") + std::string(mResourcePath);
std::cout << errorMsg << std::endl;
- throw (vpException(vpException::ioError, errorMsg));
+ throw(vpException(vpException::ioError, errorMsg));
}
std::cout << "######################### Load resource file: " << resourceFile << std::endl;
Ogre::ConfigFile cf;
@@ -293,41 +289,39 @@ void vpAROgre::init(bool
Ogre::ConfigFile::SectionIterator seci = cf.getSectionIterator();
Ogre::String secName, typeName, archName;
- while (seci.hasMoreElements())
- {
- secName = seci.peekNextKey();
- Ogre::ConfigFile::SettingsMultiMap *settings = seci.getNext();
- Ogre::ConfigFile::SettingsMultiMap::iterator i;
- for (i = settings->begin(); i != settings->end(); ++i)
- {
- typeName = i->first;
- archName = i->second;
- Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
- archName, typeName, secName);
- }
+ while (seci.hasMoreElements()) {
+ secName = seci.peekNextKey();
+ Ogre::ConfigFile::SettingsMultiMap *settings = seci.getNext();
+ Ogre::ConfigFile::SettingsMultiMap::iterator i;
+ for (i = settings->begin(); i != settings->end(); ++i) {
+ typeName = i->first;
+ archName = i->second;
+ Ogre::ResourceGroupManager::getSingleton().addResourceLocation(archName, typeName, secName);
}
- std::cout << "##################### add resources" << std::endl;
- //Add optionnal resources (given by the user).
- for(std::list<std::string>::const_iterator iter = mOptionnalResourceLocation.begin(); iter != mOptionnalResourceLocation.end(); ++iter){
- Ogre::ResourceGroupManager::getSingleton().addResourceLocation(*iter, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ }
+ std::cout << "##################### add resources" << std::endl;
+ // Add optionnal resources (given by the user).
+ for (std::list<std::string>::const_iterator iter = mOptionnalResourceLocation.begin();
+ iter != mOptionnalResourceLocation.end(); ++iter) {
+ Ogre::ResourceGroupManager::getSingleton().addResourceLocation(
+ *iter, "FileSystem", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
}
// Create the window
bool canInit = true;
- if(mshowConfigDialog){
+ if (mshowConfigDialog) {
mRoot->restoreConfig();
- if(!mRoot->showConfigDialog())
+ if (!mRoot->showConfigDialog())
canInit = false;
- }
- else{
- if(!mRoot->restoreConfig())
+ } else {
+ if (!mRoot->restoreConfig())
canInit = false;
}
- if(!mRoot->isInitialised()){
- if(!canInit){ //We set the default renderer system
- const Ogre::RenderSystemList& lRenderSystemList = mRoot->getAvailableRenderers();
- if( lRenderSystemList.size() == 0 )
+ if (!mRoot->isInitialised()) {
+ if (!canInit) { // We set the default renderer system
+ const Ogre::RenderSystemList &lRenderSystemList = mRoot->getAvailableRenderers();
+ if (lRenderSystemList.size() == 0)
throw "ConfigDialog aborted"; // Exit the application on cancel
Ogre::RenderSystem *lRenderSystem = lRenderSystemList.at(0);
@@ -343,39 +337,37 @@ void vpAROgre::init(bool
Ogre::ConfigOptionMap config = mRoot->getRenderSystem()->getConfigOptions();
Ogre::ConfigOptionMap::const_iterator it = config.begin();
- while( it != config.end() ){
+ while (it != config.end()) {
Ogre::String leftconf = (*it).first;
Ogre::String rightconf = (*it).second.currentValue;
- if(leftconf == "Video Mode"){
- if(canInit) {
+ if (leftconf == "Video Mode") {
+ if (canInit) {
std::stringstream ss(rightconf.c_str());
std::string dummy;
ss >> mWindowWidth >> dummy >> mWindowHeight;
if (ss.fail())
std::cout << "Cannot read Ogre video mode" << std::endl;
- }
- else{
- if(mWindowWidth == 0 && mWindowHeight == 0){
+ } else {
+ if (mWindowWidth == 0 && mWindowHeight == 0) {
mWindowWidth = mBackgroundWidth;
mWindowHeight = mBackgroundHeight;
}
}
- }
- else if( leftconf == "Full Screen" ){
- if(canInit){
- if(rightconf == "Yes") fullscreen = true;
+ } else if (leftconf == "Full Screen") {
+ if (canInit) {
+ if (rightconf == "Yes")
+ fullscreen = true;
}
- }
- else
+ } else
misc[leftconf] = rightconf;
++it;
}
// With Ogre version >= 1.8.1 we hide the window
- if( hidden ){
-#if ( OGRE_VERSION >= (1 << 16 | 8 << 8 | 1) )
+ if (hidden) {
+#if (OGRE_VERSION >= (1 << 16 | 8 << 8 | 1))
misc["hidden"] = "true";
windowHidden = true;
#endif
@@ -400,8 +392,8 @@ void vpAROgre::init(bool
createCamera();
// Create a viewport
- Ogre::Viewport* viewPort = mWindow->addViewport(mCamera);
-// Ogre::Viewport* viewPort = mCamera->getViewport();
+ Ogre::Viewport *viewPort = mWindow->addViewport(mCamera);
+ // Ogre::Viewport* viewPort = mCamera->getViewport();
viewPort->setClearEveryFrame(true);
// Set the projection parameters to match the camera intrinsic parameters
updateCameraProjection();
@@ -426,29 +418,32 @@ void vpAROgre::init(bool
mWindow->getCustomAttribute("WINDOW", &windowHnd);
windowHndStr << windowHnd;
pl.insert(std::make_pair(std::string("WINDOW"), windowHndStr.str()));
- // Let the user use the keyboard elsewhere
+// Let the user use the keyboard elsewhere
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
pl.insert(std::make_pair(std::string("x11_keyboard_grab"), std::string("false")));
#endif
- mInputManager = OIS::InputManager::createInputSystem( pl );
+ mInputManager = OIS::InputManager::createInputSystem(pl);
- //Create all devices
+ // Create all devices
// Here we only consider the keyboard input
- mKeyboard = static_cast<OIS::Keyboard*>(mInputManager->createInputObject( OIS::OISKeyboard, bufferedKeys ));
- if ( !bufferedKeys ) mKeyboard->setEventCallback ( this);
+ mKeyboard = static_cast<OIS::Keyboard *>(mInputManager->createInputObject(OIS::OISKeyboard, bufferedKeys));
+ if (!bufferedKeys)
+ mKeyboard->setEventCallback(this);
#endif
// Initialise a render to texture to be able to retrieve a screenshot
- Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
- mWindow->getWidth(),mWindow->getHeight(), 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
-
-
-
-// Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual("rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
-// 640,480, 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
- Ogre::RenderTexture* RTarget = Texture->getBuffer()->getRenderTarget();
- /*Ogre::Viewport* Viewport =*/ RTarget->addViewport(mCamera);
+ Ogre::TexturePtr Texture = Ogre::TextureManager::getSingleton().createManual(
+ "rtf", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, mWindow->getWidth(),
+ mWindow->getHeight(), 0, Ogre::PF_R8G8B8A8, Ogre::TU_RENDERTARGET);
+
+ // Ogre::TexturePtr Texture =
+ // Ogre::TextureManager::getSingleton().createManual("rtf",
+ // Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,Ogre::TEX_TYPE_2D,
+ // 640,480, 0, Ogre::PF_R8G8B8A8,
+ // Ogre::TU_RENDERTARGET);
+ Ogre::RenderTexture *RTarget = Texture->getBuffer()->getRenderTarget();
+ /*Ogre::Viewport* Viewport =*/RTarget->addViewport(mCamera);
RTarget->getViewport(0)->setClearEveryFrame(true);
RTarget->getViewport(0)->setOverlaysEnabled(false);
}
@@ -463,14 +458,14 @@ vpAROgre::~vpAROgre(void)
// Close OIS
closeOIS();
- if ( mWindow) {
+ if (mWindow) {
Ogre::WindowEventUtilities::removeWindowEventListener(mWindow, this);
windowClosed(mWindow);
}
// Delete root
- if(Ogre::Root::getSingletonPtr() && !Ogre::Root::getSingletonPtr()->getSceneManagerIterator().hasMoreElements()) {
- if(mRoot) {
+ if (Ogre::Root::getSingletonPtr() && !Ogre::Root::getSingletonPtr()->getSceneManagerIterator().hasMoreElements()) {
+ if (mRoot) {
delete mRoot;
}
}
@@ -482,13 +477,12 @@ vpAROgre::~vpAROgre(void)
\param evt : Frame event to process.
\return False if the program must be stopped.
*/
-bool vpAROgre::stopTest(const Ogre::FrameEvent& evt)
+bool vpAROgre::stopTest(const Ogre::FrameEvent &evt)
{
// Always keep this part
- if(keepOn){
+ if (keepOn) {
return updateScene(evt);
- }
- else
+ } else
return keepOn;
}
@@ -501,7 +495,7 @@ bool vpAROgre::stopTest(const Ogre::FrameEvent& evt)
\param evt : Frame event to process.
\return True if everything went well.
*/
-bool vpAROgre::frameStarted(const Ogre::FrameEvent& evt)
+bool vpAROgre::frameStarted(const Ogre::FrameEvent &evt)
{
// custom method telling what to do at the beginning of each frame
bool result = customframeStarted(evt);
@@ -511,25 +505,28 @@ bool vpAROgre::frameStarted(const Ogre::FrameEvent& evt)
processInputEvent(evt);
// See if we have to stop rendering
- if(result) return stopTest(evt);
- else return result;
+ if (result)
+ return stopTest(evt);
+ else
+ return result;
}
-
/*!
Function telling what to do after each frame.
Calls customframeEnded() and see if the program is to be stopped.
\param evt : Frame event to process.
\return True if everything went well.
*/
-bool vpAROgre::frameEnded(const Ogre::FrameEvent& evt)
+bool vpAROgre::frameEnded(const Ogre::FrameEvent &evt)
{
// custom method telling what to do at the end of each frame
bool result = customframeEnded(evt);
// See if we have to stop rendering
- if(result) return stopTest(evt);
- else return result;
+ if (result)
+ return stopTest(evt);
+ else
+ return result;
}
/*!
@@ -540,27 +537,27 @@ bool vpAROgre::frameEnded(const Ogre::FrameEvent& evt)
\param evt : Frame event to process
\return True if everything went well
*/
-bool vpAROgre::customframeStarted(const Ogre::FrameEvent& /*evt*/)
+bool vpAROgre::customframeStarted(const Ogre::FrameEvent & /*evt*/)
{
// See if window was closed
- if(mWindow->isClosed()) return false;
+ if (mWindow->isClosed())
+ return false;
#ifdef VISP_HAVE_OIS
// Get keyboard input
mKeyboard->capture();
- if(mKeyboard->isKeyDown(OIS::KC_ESCAPE))
+ if (mKeyboard->isKeyDown(OIS::KC_ESCAPE))
return false;
#endif
return true;
}
-
/*!
Function telling what to do after each frame.
\param evt : Frame event to process.
\return True if everything went well.
*/
-bool vpAROgre::customframeEnded(const Ogre::FrameEvent& /*evt*/){return true;}
+bool vpAROgre::customframeEnded(const Ogre::FrameEvent & /*evt*/) { return true; }
/*!
@@ -572,10 +569,11 @@ bool vpAROgre::customframeEnded(const Ogre::FrameEvent& /*evt*/){return true;}
\param rw : RenderWindow to close.
*/
-void vpAROgre::windowClosed(Ogre::RenderWindow* rw)
+void vpAROgre::windowClosed(Ogre::RenderWindow *rw)
{
- //Only close for window that created OIS (the main window in these demos)
- if( rw == mWindow ) closeOIS();
+ // Only close for window that created OIS (the main window in these demos)
+ if (rw == mWindow)
+ closeOIS();
}
/*!
@@ -583,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<unsigned char> &I,
- const vpHomogeneousMatrix &cMw)
+bool vpAROgre::renderOneFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw)
{
// Update the background to match the situation
updateBackgroundTexture(I);
@@ -601,8 +598,7 @@ bool vpAROgre::renderOneFrame(const vpImage<unsigned char> &I,
\param I : RGBa image to show in background.
\param cMw : Camera pose as an homogeneous matrix.
*/
-bool vpAROgre::renderOneFrame(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMw)
+bool vpAROgre::renderOneFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw)
{
// Update the background to match the situation
updateBackgroundTexture(I);
@@ -619,15 +615,13 @@ bool vpAROgre::renderOneFrame(const vpImage<vpRGBa> &I,
\param I : Grey level image to show in background.
\param cMw : Camera pose as an homogeneous matrix.
*/
-void vpAROgre::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMw)
+void vpAROgre::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMw)
{
// Display on Ogre Window
- if(renderOneFrame(I,cMw)){
+ if (renderOneFrame(I, cMw)) {
mWindow->update();
keepOn = true;
- }
- else
+ } else
keepOn = false;
}
@@ -639,11 +633,10 @@ void vpAROgre::display(const vpImage<unsigned char> &I,
void vpAROgre::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw)
{
// Display on Ogre Window
- if(renderOneFrame(I,cMw)){
+ if (renderOneFrame(I, cMw)) {
mWindow->update();
keepOn = true;
- }
- else
+ } else
keepOn = false;
}
@@ -651,18 +644,12 @@ void vpAROgre::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMw)
Ask the program if we can continue to render.
\return True if nothing stopped the rendering loop.
*/
-bool vpAROgre::continueRendering(void)
-{
- return keepOn;
-}
+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.
@@ -682,8 +669,7 @@ 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);
@@ -695,7 +681,7 @@ void vpAROgre::setPosition(const std::string &sceneName,
\param sceneName : Name of the SceneNode in the scene graph.
\return The position of the node.
*/
-vpTranslationVector vpAROgre::getPosition(const std::string &sceneName)const
+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]);
@@ -711,10 +697,9 @@ void vpAROgre::setRotation(const std::string &sceneName, const vpRotationMatrix
// 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);
}
@@ -727,10 +712,9 @@ void vpAROgre::setRotation(const std::string &sceneName, const vpRotationMatrix
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);
}
@@ -775,7 +759,7 @@ void vpAROgre::setVisibility(const std::string &sceneName, bool isVisible)
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));
}
@@ -783,75 +767,69 @@ void vpAROgre::setScale(const std::string &sceneName, const float factorx, const
/*!
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.
- \param I : This parameter is here only used to initialize a grey level background.
+ \param I : This parameter is here only used to initialize a grey level
+ background.
*/
void vpAROgre::createBackground(vpImage<unsigned char> & /* I */)
{
// Create a rectangle to show the incoming images from the camera
- mBackground = new Ogre::Rectangle2D(true); // true = textured
+ mBackground = 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().setDefaultAnisotropy(1);
// Dynamic texture
- // If we are using opengl we can boost a little bit performances with a dynamic texture
- if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- Ogre::PF_BYTE_L,
- Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
- }
- else{
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- Ogre::PF_BYTE_L,
- Ogre::TU_DEFAULT);
+ // If we are using opengl we can boost a little bit performances with a
+ // dynamic texture
+ if (mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
+ Ogre::TextureManager::getSingleton().createManual(
+ "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D,
+ mBackgroundWidth, // width
+ mBackgroundHeight, // height
+ 0, // num of mip maps
+ Ogre::PF_BYTE_L, Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
+ } else {
+ Ogre::TextureManager::getSingleton().createManual(
+ "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D,
+ mBackgroundWidth, // width
+ mBackgroundHeight, // height
+ 0, // num of mip maps
+ Ogre::PF_BYTE_L, Ogre::TU_DEFAULT);
}
// Pointer to the dynamic texture
Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
-//#else
-// ;
-//#endif
+ //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+ // .dynamicCast<Ogre::Texture>();// Get the pixel buffer
+ //#else
+ // ;
+ //#endif
mPixelBuffer = dynTexPtr->getBuffer();
// Material to apply the texture to the background
- Ogre::MaterialPtr Backgroundmaterial
- = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Material>();
-//#else
-// ;
-//#endif
+ Ogre::MaterialPtr Backgroundmaterial = Ogre::MaterialManager::getSingleton().create(
+ "BackgroundMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+ // .dynamicCast<Ogre::Material>();
+ //#else
+ // ;
+ //#endif
Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
Backgroundtechnique->createPass();
Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
- mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
+ mBackground->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
@@ -862,73 +840,69 @@ void vpAROgre::createBackground(vpImage<unsigned char> & /* I */)
/*!
Create a color background to show the real scene.
- \param I : This parameter is here only used to initialize a color background.
+ \param I : This parameter is here only used to initialize a color
+ background.
*/
void vpAROgre::createBackground(vpImage<vpRGBa> & /* I */)
{
// Create a rectangle to show the incoming images from the camera
- mBackground = new Ogre::Rectangle2D(true); // true = textured
+ mBackground = 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().setDefaultAnisotropy(1);
// Dynamic texture
- // If we are using opengl we can boost a little bit performances with a dynamic texture
- if(mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- //Ogre::PF_BYTE_RGBA,
- Ogre::PF_BYTE_BGRA,
- Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
- }
- else{ // As that texture does not seem to work properly with direct3D we use a default texture
- Ogre::TextureManager::getSingleton().createManual("BackgroundTexture",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- mBackgroundWidth,//width
- mBackgroundHeight,//height
- 0, // num of mip maps
- //Ogre::PF_BYTE_RGBA,
- Ogre::PF_BYTE_BGRA,
- Ogre::TU_DEFAULT);
+ // If we are using opengl we can boost a little bit performances with a
+ // dynamic texture
+ if (mRoot->getRenderSystem()->getName() == "OpenGL Rendering Subsystem") {
+ Ogre::TextureManager::getSingleton().createManual(
+ "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D,
+ mBackgroundWidth, // width
+ mBackgroundHeight, // height
+ 0, // num of mip maps
+ // Ogre::PF_BYTE_RGBA,
+ Ogre::PF_BYTE_BGRA, Ogre::TU_DYNAMIC_WRITE_ONLY_DISCARDABLE);
+ } else { // As that texture does not seem to work properly with direct3D we
+ // use a default texture
+ Ogre::TextureManager::getSingleton().createManual(
+ "BackgroundTexture", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D,
+ mBackgroundWidth, // width
+ mBackgroundHeight, // height
+ 0, // num of mip maps
+ // Ogre::PF_BYTE_RGBA,
+ Ogre::PF_BYTE_BGRA, Ogre::TU_DEFAULT);
}
-
// Pointer to the dynamic texture
- Ogre::TexturePtr dynTexPtr =
- Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();// Get the pixel buffer
-//#else
-// ;
-//#endif
+ Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("BackgroundTexture");
+ //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+ // .dynamicCast<Ogre::Texture>();// Get the pixel buffer
+ //#else
+ // ;
+ //#endif
// Get the pixel buffer
mPixelBuffer = dynTexPtr->getBuffer();
// Material to apply the texture to the background
- Ogre::MaterialPtr Backgroundmaterial
- = Ogre::MaterialManager::getSingleton().create("BackgroundMaterial",
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Material>();
-//#else
-// ;
-//#endif
+ Ogre::MaterialPtr Backgroundmaterial = Ogre::MaterialManager::getSingleton().create(
+ "BackgroundMaterial", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+ // .dynamicCast<Ogre::Material>();
+ //#else
+ // ;
+ //#endif
Ogre::Technique *Backgroundtechnique = Backgroundmaterial->createTechnique();
Backgroundtechnique->createPass();
Backgroundmaterial->getTechnique(0)->getPass(0)->setLightingEnabled(false);
Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthCheckEnabled(false); // Background
Backgroundmaterial->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false); // Background
Backgroundmaterial->getTechnique(0)->getPass(0)->createTextureUnitState("BackgroundTexture");
- mBackground->setMaterial("BackgroundMaterial"); // Attach the material to the rectangle
+ mBackground->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
@@ -946,13 +920,12 @@ void vpAROgre::createBackground(vpImage<vpRGBa> & /* I */)
void vpAROgre::closeOIS(void)
{
#ifdef VISP_HAVE_OIS
- if( mInputManager )
- {
- mInputManager->destroyInputObject( mKeyboard );
+ if (mInputManager) {
+ mInputManager->destroyInputObject(mKeyboard);
- OIS::InputManager::destroyInputSystem(mInputManager);
- mInputManager = 0;
- }
+ OIS::InputManager::destroyInputSystem(mInputManager);
+ mInputManager = 0;
+ }
#endif
}
@@ -963,22 +936,21 @@ void vpAROgre::closeOIS(void)
// http://strawlab.org/2011/11/05/augmented-reality-with-OpenGL/
void vpAROgre::updateCameraProjection(void)
{
- if(mCamera != 0){
- Ogre::Real f,n,f_m_n,f_p_n,px,py,u0,v0;
- f = (Ogre::Real)(mFarClipping); // Far clip distance
- n = (Ogre::Real)(mNearClipping); // Near clip distance
- f_m_n = (Ogre::Real)(f-n);
- f_p_n = (Ogre::Real)(f+n);
- px = (Ogre::Real)mcam.get_px();
- py = (Ogre::Real)mcam.get_py();
- u0 = (Ogre::Real)mcam.get_u0();
- v0 = (Ogre::Real)mcam.get_v0();
- Ogre::Matrix4 Projection
- = Ogre::Matrix4( (Ogre::Real)(2.0*px/mBackgroundWidth), 0, (Ogre::Real)(1.0 - 2.0*(u0/mBackgroundWidth)), 0,
- 0, (Ogre::Real)(2.0*py/mBackgroundHeight), (Ogre::Real)(-1.0 + 2.0*(v0/mBackgroundHeight)),0,
- 0, 0, (Ogre::Real)(-1.0*f_p_n/f_m_n), (Ogre::Real)(-2.0*f*n/f_m_n),
- 0, 0, -1.0, 0);
- mCamera->setCustomProjectionMatrix(true, Projection);
+ if (mCamera != 0) {
+ Ogre::Real f, n, f_m_n, f_p_n, px, py, u0, v0;
+ f = (Ogre::Real)(mFarClipping); // Far clip distance
+ n = (Ogre::Real)(mNearClipping); // Near clip distance
+ f_m_n = (Ogre::Real)(f - n);
+ f_p_n = (Ogre::Real)(f + n);
+ px = (Ogre::Real)mcam.get_px();
+ py = (Ogre::Real)mcam.get_py();
+ u0 = (Ogre::Real)mcam.get_u0();
+ v0 = (Ogre::Real)mcam.get_v0();
+ Ogre::Matrix4 Projection = Ogre::Matrix4(
+ (Ogre::Real)(2.0 * px / mBackgroundWidth), 0, (Ogre::Real)(1.0 - 2.0 * (u0 / mBackgroundWidth)), 0, 0,
+ (Ogre::Real)(2.0 * py / mBackgroundHeight), (Ogre::Real)(-1.0 + 2.0 * (v0 / mBackgroundHeight)), 0, 0, 0,
+ (Ogre::Real)(-1.0 * f_p_n / f_m_n), (Ogre::Real)(-2.0 * f * n / f_m_n), 0, 0, -1.0, 0);
+ mCamera->setCustomProjectionMatrix(true, Projection);
}
}
@@ -987,15 +959,16 @@ void vpAROgre::updateCameraProjection(void)
*/
void vpAROgre::updateBackgroundTexture(const vpImage<unsigned char> &I)
{
- // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
- // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
- // performance than HBL_NORMAL
+ // Inspired from Ogre wiki :
+ // http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures Lock the pixel
+ // buffer and get a pixel box. HBL_DISCARD is to use for best performance
+ // than HBL_NORMAL
mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
+ const Ogre::PixelBox &pixelBox = mPixelBuffer->getCurrentLock();
// Buffer data
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
+ Ogre::uint8 *pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
// Fill in the data in the grey level texture
- memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth);
+ memcpy(pDest, I.bitmap, mBackgroundHeight * mBackgroundWidth);
// Unlock the pixel buffer
mPixelBuffer->unlock();
@@ -1006,31 +979,32 @@ void vpAROgre::updateBackgroundTexture(const vpImage<unsigned char> &I)
*/
void vpAROgre::updateBackgroundTexture(const vpImage<vpRGBa> &I)
{
- // Inspired from Ogre wiki : http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures
- // Lock the pixel buffer and get a pixel box. HBL_DISCARD is to use for best
- // performance than HBL_NORMAL
+ // Inspired from Ogre wiki :
+ // http://www.ogre3d.org/tikiwiki/Creating+dynamic+textures Lock the pixel
+ // buffer and get a pixel box. HBL_DISCARD is to use for best performance
+ // than HBL_NORMAL
mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD); // Lock the buffer
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
+ const Ogre::PixelBox &pixelBox = mPixelBuffer->getCurrentLock();
// Buffer data
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
- // Fill in the data in the grey level texture
+ Ogre::uint8 *pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
+// Fill in the data in the grey level texture
#if 1 // if texture in BGRa format
- for(unsigned int i=0; i<mBackgroundHeight; i++){
- for(unsigned int j=0; j<mBackgroundWidth; j++){
+ for (unsigned int i = 0; i < mBackgroundHeight; i++) {
+ for (unsigned int j = 0; j < mBackgroundWidth; j++) {
// Color Image
-// *pDest++=I[i][mBackgroundWidth-j].B; // Blue component
-// *pDest++=I[i][mBackgroundWidth-j].G; // Green component
-// *pDest++=I[i][mBackgroundWidth-j].R; // Red component
+ // *pDest++=I[i][mBackgroundWidth-j].B; // Blue component
+ // *pDest++=I[i][mBackgroundWidth-j].G; // Green component
+ // *pDest++=I[i][mBackgroundWidth-j].R; // Red component
- *pDest++=I[i][j].B; // Blue component
- *pDest++=I[i][j].G; // Green component
- *pDest++=I[i][j].R; // Red component
+ *pDest++ = I[i][j].B; // Blue component
+ *pDest++ = I[i][j].G; // Green component
+ *pDest++ = I[i][j].R; // Red component
- *pDest++ = 255; // Alpha component
+ *pDest++ = 255; // Alpha component
}
}
#else // if texture in RGBa format which is the format of the input image
- memcpy(pDest, I.bitmap, mBackgroundHeight*mBackgroundWidth*sizeof(vpRGBa));
+ memcpy(pDest, I.bitmap, mBackgroundHeight * mBackgroundWidth * sizeof(vpRGBa));
#endif
// Unlock the pixel buffer
@@ -1040,15 +1014,17 @@ void vpAROgre::updateBackgroundTexture(const vpImage<vpRGBa> &I)
/*!
Update Camera parameters from a pose calculation.
*/
-void vpAROgre::updateCameraParameters (const vpHomogeneousMatrix &cMw)
+void vpAROgre::updateCameraParameters(const vpHomogeneousMatrix &cMw)
{
- // The matrix is given to Ogre with some changes to fit with the world projection
+ // The matrix is given to Ogre with some changes to fit with the world
+ // projection
Ogre::Matrix4 ModelView
-// = Ogre::Matrix4( (Ogre::Real)-cMo[0][0], (Ogre::Real)-cMo[0][1], (Ogre::Real)-cMo[0][2], (Ogre::Real)-cMo[0][3],
- = Ogre::Matrix4( (Ogre::Real)cMw[0][0], (Ogre::Real)cMw[0][1], (Ogre::Real)cMw[0][2], (Ogre::Real)cMw[0][3],
- (Ogre::Real)-cMw[1][0], (Ogre::Real)-cMw[1][1], (Ogre::Real)-cMw[1][2], (Ogre::Real)-cMw[1][3],
- (Ogre::Real)-cMw[2][0], (Ogre::Real)-cMw[2][1], (Ogre::Real)-cMw[2][2], (Ogre::Real)-cMw[2][3],
- (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)1);
+ // = Ogre::Matrix4( (Ogre::Real)-cMo[0][0], (Ogre::Real)-cMo[0][1],
+ // (Ogre::Real)-cMo[0][2], (Ogre::Real)-cMo[0][3],
+ = Ogre::Matrix4((Ogre::Real)cMw[0][0], (Ogre::Real)cMw[0][1], (Ogre::Real)cMw[0][2], (Ogre::Real)cMw[0][3],
+ (Ogre::Real)-cMw[1][0], (Ogre::Real)-cMw[1][1], (Ogre::Real)-cMw[1][2], (Ogre::Real)-cMw[1][3],
+ (Ogre::Real)-cMw[2][0], (Ogre::Real)-cMw[2][1], (Ogre::Real)-cMw[2][2], (Ogre::Real)-cMw[2][3],
+ (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)0, (Ogre::Real)1);
mCamera->setCustomViewMatrix(true, ModelView);
}
@@ -1060,46 +1036,43 @@ void vpAROgre::updateCameraParameters (const vpHomogeneousMatrix &cMw)
*/
void vpAROgre::getRenderingOutput(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo)
{
- updateCameraParameters(cMo);
- Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("rtf");
-//#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
-// .dynamicCast<Ogre::Texture>();
-//#else
-// ;
-//#endif
- Ogre::RenderTexture* RTarget = dynTexPtr->getBuffer()->getRenderTarget();
- mWindow->update();
- RTarget->update();
- if(I.getHeight() != mWindow->getHeight() || I.getWidth() != mWindow->getWidth()){
- I.resize(mWindow->getHeight(), mWindow->getWidth());
- }
- Ogre::HardwarePixelBufferSharedPtr mPixelBuffer = dynTexPtr->getBuffer();
- mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
- const Ogre::PixelBox& pixelBox = mPixelBuffer->getCurrentLock();
- dynTexPtr->getBuffer()->blitToMemory(pixelBox);
- Ogre::uint8* pDest = static_cast<Ogre::uint8*>(pixelBox.data);
+ updateCameraParameters(cMo);
+ Ogre::TexturePtr dynTexPtr = Ogre::TextureManager::getSingleton().getByName("rtf");
+ //#if ( OGRE_VERSION >= (1 << 16 | 9 << 8 | 0) )
+ // .dynamicCast<Ogre::Texture>();
+ //#else
+ // ;
+ //#endif
+ Ogre::RenderTexture *RTarget = dynTexPtr->getBuffer()->getRenderTarget();
+ mWindow->update();
+ RTarget->update();
+ if (I.getHeight() != mWindow->getHeight() || I.getWidth() != mWindow->getWidth()) {
+ I.resize(mWindow->getHeight(), mWindow->getWidth());
+ }
+ Ogre::HardwarePixelBufferSharedPtr mPixelBuffer = dynTexPtr->getBuffer();
+ mPixelBuffer->lock(Ogre::HardwareBuffer::HBL_DISCARD);
+ const Ogre::PixelBox &pixelBox = mPixelBuffer->getCurrentLock();
+ dynTexPtr->getBuffer()->blitToMemory(pixelBox);
+ Ogre::uint8 *pDest = static_cast<Ogre::uint8 *>(pixelBox.data);
#if 1 // if texture in BGRa format
- for(unsigned int i=0; i<I.getHeight(); i++){
- for(unsigned int j=0; j<I.getWidth(); j++){
- // Color Image
- I[i][j].B = *pDest++; // Blue component
- I[i][j].G = *pDest++; // Green component
- I[i][j].R = *pDest++; // Red component
- I[i][j].A = *pDest++; // Alpha component
- }
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ // Color Image
+ I[i][j].B = *pDest++; // Blue component
+ I[i][j].G = *pDest++; // Green component
+ I[i][j].R = *pDest++; // Red component
+ I[i][j].A = *pDest++; // Alpha component
}
+ }
#else // if texture in RGBa format which is the format of the input image
- memcpy(I.bitmap, pDest, I.getHeight()*I.getWidth()*sizeof(vpRGBa));
+ memcpy(I.bitmap, pDest, I.getHeight() * I.getWidth() * sizeof(vpRGBa));
#endif
- // Unlock the pixel buffer
- mPixelBuffer->unlock();
-
+ // Unlock the pixel buffer
+ mPixelBuffer->unlock();
}
-
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_ar.a(vpAROgre.cpp.o) has no symbols
-void dummy_vpAROgre() {};
+void dummy_vpAROgre(){};
#endif
-
diff --git a/modules/ar/src/vpSimulatorException.cpp b/modules/ar/src/vpSimulatorException.cpp
index 2edc1af..221ad76 100644
--- a/modules/ar/src/vpSimulatorException.cpp
+++ b/modules/ar/src/vpSimulatorException.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,17 +43,15 @@
#include <visp3/ar/vpSimulatorException.h>
-vpSimulatorException::vpSimulatorException (const int id, const char* format, ...)
+vpSimulatorException::vpSimulatorException(const int id, const char *format, ...)
{
this->code = id;
va_list args;
va_start(args, format);
setMessage(format, args);
- va_end (args);
+ 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/CMakeLists.txt b/modules/core/CMakeLists.txt
index 3673579..b2186c4 100644
--- a/modules/core/CMakeLists.txt
+++ b/modules/core/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -223,7 +224,14 @@ if(USE_YARP)
add_definitions(${YARP_DEFINES})
endif(USE_YARP)
-# Math: gsl, lapack, OpenCV
+# Math: eigen3, gsl, lapack, OpenCV
+if(USE_EIGEN3)
+ if(EIGEN3_INCLUDE_DIRS)
+ list(APPEND opt_incs ${EIGEN3_INCLUDE_DIRS})
+ else()
+ list(APPEND opt_incs ${EIGEN3_INCLUDE_DIR})
+ endif()
+endif()
if(USE_GSL)
list(APPEND opt_incs ${GSL_INCLUDE_DIRS})
list(APPEND opt_libs ${GSL_LIBRARIES})
@@ -251,16 +259,21 @@ if(MSVC)
# warning C4996: 'gethostbyname': Use getaddrinfo() or GetAddrInfoW() instead
# warning C4996: 'inet_ntoa': Use inet_ntop() or InetNtop() instead
# warning C4996: 'inet_addr': Use inet_pton() or InetPton() instead
- vp_set_source_file_compile_flag(src/tools/network/vpClient.cpp /wd4996)
- vp_set_source_file_compile_flag(src/tools/network/vpServer.cpp /wd4996)
- vp_set_source_file_compile_flag(src/tools/network/vpNetwork.cpp /wd4996)
+ vp_set_source_file_compile_flag(src/tools/network/vpClient.cpp /wd"4996")
+ vp_set_source_file_compile_flag(src/tools/network/vpServer.cpp /wd"4996")
+ vp_set_source_file_compile_flag(src/tools/network/vpNetwork.cpp /wd"4996")
if(BUILD_TESTS)
- vp_set_source_file_compile_flag(test/network/testClient.cpp /wd4996)
- vp_set_source_file_compile_flag(test/network/testServer.cpp /wd4996)
+ vp_set_source_file_compile_flag(test/network/testClient.cpp /wd"4996")
+ vp_set_source_file_compile_flag(test/network/testServer.cpp /wd"4996")
endif()
endif()
-vp_add_module(core)
+if(USE_EIGEN3)
+ vp_set_source_file_compile_flag(src/math/matrix/vpMatrix_svd.cpp -Wno-float-equal -Wno-strict-overflow -Wno-misleading-indentation -Wno-int-in-bool-context)
+ vp_set_source_file_compile_flag(src/math/matrix/vpMatrix_lu.cpp -Wno-float-equal -Wno-strict-overflow -Wno-misleading-indentation -Wno-int-in-bool-context)
+endif()
+
+vp_add_module(core PRIVATE_OPTIONAL ${LAPACK_LIBRARIES})
vp_source_group("Src" FILES "${VISP_MODULE_visp_core_BINARY_DIR}/version_string.inc")
diff --git a/modules/core/include/visp3/core/vpArray2D.h b/modules/core/include/visp3/core/vpArray2D.h
index ef7988d..7e34150 100644
--- a/modules/core/include/visp3/core/vpArray2D.h
+++ b/modules/core/include/visp3/core/vpArray2D.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,14 +38,14 @@
#ifndef __vpArray2D_h_
#define __vpArray2D_h_
+#include <fstream>
#include <iostream>
-#include <ostream>
+#include <limits>
#include <math.h>
+#include <ostream>
+#include <sstream>
#include <stdlib.h>
#include <string.h>
-#include <fstream>
-#include <sstream>
-#include <limits>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
@@ -53,21 +54,20 @@
\class vpArray2D
\ingroup group_core_matrices
- \brief Implementation of a generic 2D array used as vase class of matrices and vectors.
+ \brief Implementation of a generic 2D array used as vase class of matrices
+ and vectors.
- This class implements a 2D array as a template class and all the basic functionalities
- common to matrices and vectors.
- More precisely:
+ This class implements a 2D array as a template class and all the basic
+ functionalities common to matrices and vectors. More precisely:
- concerning matrices, vpMatrix but also specific containers such as twist
- (vpVelocityTwistMatrix and vpForceTwistMatrix), homogeneous (vpHomogeneousMatrix),
- rotation (vpRotationMatrix) and homography (vpHomography) matrices inherit from
- vpArray2D<double>.
- - concerning vectors, vpColVector, vpRowVector but also specific containers describing
- the pose (vpPoseVector) and the rotation (vpRotationVector) inherit also from
- vpArray2D<double>.
+ (vpVelocityTwistMatrix and vpForceTwistMatrix), homogeneous
+ (vpHomogeneousMatrix), rotation (vpRotationMatrix) and homography
+ (vpHomography) matrices inherit from vpArray2D<double>.
+ - concerning vectors, vpColVector, vpRowVector but also specific containers
+ describing the pose (vpPoseVector) and the rotation (vpRotationVector)
+ inherit also from vpArray2D<double>.
*/
-template<class Type>
-class vpArray2D
+template <class Type> class vpArray2D
{
protected:
//! Number of rows in the array
@@ -88,17 +88,14 @@ public:
Basic constructor of a 2D array.
Number of columns and rows are set to zero.
*/
- vpArray2D<Type>()
- : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
- {}
+ vpArray2D<Type>() : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL) {}
/*!
Copy constructor of a 2D array.
*/
- vpArray2D<Type>(const vpArray2D<Type> & A)
- : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ vpArray2D<Type>(const vpArray2D<Type> &A) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
{
- resize(A.rowNum, A.colNum);
- memcpy(data, A.data, rowNum*colNum*sizeof(Type));
+ resize(A.rowNum, A.colNum, false, false);
+ memcpy(data, A.data, rowNum * colNum * sizeof(Type));
}
/*!
Constructor that initializes a 2D array with 0.
@@ -106,8 +103,7 @@ public:
\param r : Array number of rows.
\param c : Array number of columns.
*/
- vpArray2D<Type>(unsigned int r, unsigned int c)
- : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ vpArray2D<Type>(unsigned int r, unsigned int c) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
{
resize(r, c);
}
@@ -118,10 +114,9 @@ public:
\param c : Array number of columns.
\param val : Each element of the array is set to \e val.
*/
- vpArray2D<Type>(unsigned int r, unsigned int c, Type val)
- : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
+ vpArray2D<Type>(unsigned int r, unsigned int c, Type val) : rowNum(0), colNum(0), rowPtrs(NULL), dsize(0), data(NULL)
{
- resize(r, c);
+ resize(r, c, false, false);
*this = val;
}
/*!
@@ -129,14 +124,14 @@ public:
*/
virtual ~vpArray2D<Type>()
{
- if (data != NULL ) {
+ if (data != NULL) {
free(data);
- data=NULL;
+ data = NULL;
}
- if (rowPtrs!=NULL) {
+ if (rowPtrs != NULL) {
free(rowPtrs);
- rowPtrs=NULL ;
+ rowPtrs = NULL;
}
rowNum = colNum = dsize = 0;
}
@@ -144,16 +139,23 @@ public:
/** @name Inherited functionalities from vpArray2D */
//@{
- Type getMinValue() const;
+ /*!
+ * Return the number of columns of the 2D array.
+ * \sa getRows(), size()
+ */
+ inline unsigned int getCols() const { return colNum; }
Type getMaxValue() const;
- //! Return the number of rows of the 2D array
- inline unsigned int getRows() const { return rowNum ;}
- //! Return the number of columns of the 2D array
- inline unsigned int getCols() const { return colNum; }
+ Type getMinValue() const;
+
+ /*!
+ * Return the number of rows of the 2D array.
+ * \sa getCols(), size()
+ */
+ inline unsigned int getRows() const { return rowNum; }
//! Return the number of elements of the 2D array.
- inline unsigned int size() const { return colNum*rowNum; }
+ inline unsigned int size() const { return colNum * rowNum; }
/*!
Set the size of the array and initialize all the values to zero.
@@ -163,68 +165,71 @@ public:
after resize. If false, the initial values from the common part of the
array (common part between old and new version of the array) are kept.
Default value is true.
+ \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)
+ 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) {
- memset(this->data, 0, this->dsize*sizeof(Type));
+ memset(this->data, 0, this->dsize * sizeof(Type));
}
- }
- else {
- const bool recopyNeeded = (ncols != this ->colNum);
- Type * copyTmp = NULL;
- unsigned int rowTmp = 0, colTmp=0;
+ } else {
+ bool recopy = !flagNullify && recopy_; // priority to flagNullify
+ const bool recopyNeeded = (ncols != this->colNum && this->colNum > 0 && ncols > 0 && (!flagNullify || recopy));
+ Type *copyTmp = NULL;
+ unsigned int rowTmp = 0, colTmp = 0;
// Recopy case per case is required if number of cols has changed;
// structure of Type array is not the same in this case.
if (recopyNeeded && this->data != NULL) {
copyTmp = new Type[this->dsize];
- memcpy (copyTmp, this ->data, sizeof(Type)*this->dsize);
- rowTmp=this->rowNum; colTmp=this->colNum;
+ memcpy(copyTmp, this->data, sizeof(Type) * this->dsize);
+ rowTmp = this->rowNum;
+ colTmp = this->colNum;
}
// Reallocation of this->data array
- this->dsize = nrows*ncols;
- this->data = (Type*)realloc(this->data, this->dsize*sizeof(Type));
+ this->dsize = nrows * ncols;
+ this->data = (Type *)realloc(this->data, this->dsize * sizeof(Type));
if ((NULL == this->data) && (0 != this->dsize)) {
- if (copyTmp != NULL) delete [] copyTmp;
- throw(vpException(vpException::memoryAllocationError,
- "Memory allocation error when allocating 2D array data")) ;
+ if (copyTmp != NULL)
+ delete[] copyTmp;
+ throw(vpException(vpException::memoryAllocationError, "Memory allocation error when allocating 2D array data"));
}
- this->rowPtrs = (Type**)realloc (this->rowPtrs, nrows*sizeof(Type*));
+ this->rowPtrs = (Type **)realloc(this->rowPtrs, nrows * sizeof(Type *));
if ((NULL == this->rowPtrs) && (0 != this->dsize)) {
- if (copyTmp != NULL) delete [] copyTmp;
+ if (copyTmp != NULL)
+ delete[] copyTmp;
throw(vpException(vpException::memoryAllocationError,
- "Memory allocation error when allocating 2D array rowPtrs")) ;
+ "Memory allocation error when allocating 2D array rowPtrs"));
}
// Update rowPtrs
{
- Type **t_= rowPtrs;
- for (unsigned int i=0; i<dsize; i+=ncols) {
+ Type **t_ = rowPtrs;
+ for (unsigned int i = 0; i < dsize; i += ncols) {
*t_++ = this->data + i;
}
}
- this->rowNum = nrows; this->colNum = ncols;
+ this->rowNum = nrows;
+ this->colNum = ncols;
// Recopy of this->data array values or nullify
if (flagNullify) {
- memset(this->data,0,this->dsize*sizeof(Type));
- }
- else if (recopyNeeded && this->rowPtrs != NULL) {
+ memset(this->data, 0, this->dsize * sizeof(Type));
+ } else if (recopyNeeded && this->rowPtrs != NULL) {
// Recopy...
- const unsigned int minRow = (this->rowNum<rowTmp)?this->rowNum:rowTmp;
- const unsigned int minCol = (this->colNum<colTmp)?this->colNum:colTmp;
- for (unsigned int i=0; i<this->rowNum; ++i) {
- for (unsigned int j=0; j<this->colNum; ++j) {
+ const unsigned int minRow = (this->rowNum < rowTmp) ? this->rowNum : rowTmp;
+ const unsigned int minCol = (this->colNum < colTmp) ? this->colNum : colTmp;
+ for (unsigned int i = 0; i < this->rowNum; ++i) {
+ for (unsigned int j = 0; j < this->colNum; ++j) {
if ((minRow > i) && (minCol > j)) {
- (*this)[i][j] = copyTmp [i*colTmp+j];
- }
- else {
+ (*this)[i][j] = copyTmp[i * colTmp + j];
+ } else {
(*this)[i][j] = 0;
}
}
@@ -232,14 +237,14 @@ public:
}
if (copyTmp != NULL)
- delete [] copyTmp;
+ delete[] copyTmp;
}
}
//! Set all the elements of the array to \e x.
- vpArray2D<Type> & operator=(Type x)
+ vpArray2D<Type> &operator=(Type x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] = x;
return *this;
@@ -248,37 +253,38 @@ public:
/*!
Copy operator of a 2D array.
*/
- vpArray2D<Type> & operator=(const vpArray2D<Type> & A)
+ vpArray2D<Type> &operator=(const vpArray2D<Type> &A)
{
- resize(A.rowNum, A.colNum);
- memcpy(data, A.data, rowNum*colNum*sizeof(Type));
+ resize(A.rowNum, A.colNum, false, false);
+ memcpy(data, A.data, rowNum * colNum * sizeof(Type));
return *this;
}
//! Set element \f$A_{ij} = x\f$ using A[i][j] = x
inline Type *operator[](unsigned int i) { return rowPtrs[i]; }
//! Get element \f$x = A_{ij}\f$ using x = A[i][j]
- inline Type *operator[](unsigned int i) const {return rowPtrs[i];}
+ inline Type *operator[](unsigned int i) const { return rowPtrs[i]; }
/*!
\relates vpArray2D
- Writes the given array to the output stream and returns a reference to the output stream.
+ Writes the given array to the output stream and returns a reference to the
+ output stream.
*/
friend std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
{
- if (A.data == NULL)
+ if (A.data == NULL || A.size() == 0)
return s;
std::ios_base::fmtflags original_flags = s.flags();
- s.precision(10) ;
- for (unsigned int i=0;i<A.getRows();i++) {
- for (unsigned int j=0;j<A.getCols() -1;j++){
+ s.precision(10);
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols() - 1; j++) {
s << A[i][j] << " ";
}
// We don't add " " after the last row element
- s << A[i][A.getCols() -1];
+ s << A[i][A.getCols() - 1];
// We don't add a \n char on the end of the last array line
- if (i < A.getRows()-1)
+ if (i < A.getRows() - 1)
s << std::endl;
}
@@ -286,36 +292,39 @@ public:
return s;
}
+
+ vpArray2D<Type> hadamard(const vpArray2D<Type> &m) const;
//@}
//---------------------------------
// Inherited array I/O Static Public Member Functions
//---------------------------------
- /** @name Inherited I/O from vpArray2D with Static Public Member Functions */
+ /** @name Inherited I/O from vpArray2D with Static Public Member Functions
+ */
//@{
/*!
Load a matrix from a file.
\param filename : Absolute file name.
\param A : Array to be loaded
- \param binary : If true the matrix is loaded from a binary file, else from a text file.
- \param header : Header of the file is loaded in this parameter.
+ \param binary : If true the matrix is loaded from a binary file, else from
+ a text file. \param header : Header of the file is loaded in this
+ parameter.
\return Returns true if success.
\sa save()
*/
- static bool load(const std::string &filename, vpArray2D<Type> &A,
- const bool binary = false, char *header = NULL)
+ static bool load(const std::string &filename, vpArray2D<Type> &A, const bool binary = false, char *header = NULL)
{
std::fstream file;
if (!binary)
file.open(filename.c_str(), std::fstream::in);
else
- file.open(filename.c_str(), std::fstream::in|std::fstream::binary);
+ file.open(filename.c_str(), std::fstream::in | std::fstream::binary);
- if(!file) {
+ if (!file) {
file.close();
return false;
}
@@ -329,25 +338,26 @@ public:
file.getline(line, 256);
std::string prefix("# ");
std::string line_(line);
- if (line_.compare(0, 2, "# ") == 0) {
+ if (line_.compare(0, 2, prefix.c_str()) == 0) {
// Line is a comment
- // If we are not on the first line, we should add "\n" to the end of the previous line
+ // If we are not on the first line, we should add "\n" to the end of
+ // the previous line
if (pos)
h += "\n";
h += line_.substr(2); // Remove "# "
- }
- else {
+ } else {
// rewind before the line
- file.seekg (pos, file.beg);
+ file.seekg(pos, file.beg);
headerIsDecoded = true;
}
- } while(! headerIsDecoded);
+ } while (!headerIsDecoded);
if (header != NULL) {
-#if !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());
+ _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
#endif
}
@@ -355,45 +365,44 @@ public:
file >> rows;
file >> cols;
- if (rows >= (std::numeric_limits<unsigned int>::max)()
- || cols >= (std::numeric_limits<unsigned int>::max)())
+ if (rows >= (std::numeric_limits<unsigned int>::max)() || cols >= (std::numeric_limits<unsigned int>::max)())
throw vpException(vpException::badValue, "Array exceed the max size.");
- A.resize(rows,cols);
+ A.resize(rows, cols);
Type value;
- for(unsigned int i = 0; i < rows; i++) {
- for(unsigned int j = 0; j < cols; j++) {
+ for (unsigned int i = 0; i < rows; i++) {
+ for (unsigned int j = 0; j < cols; j++) {
file >> value;
A[i][j] = value;
}
}
- }
- else {
- char c='0';
+ } else {
+ char c = '0';
std::string h;
// Decode header until '\0' char that ends the header string
while ((c != '\0')) {
- file.read(&c,1);
- h+=c;
+ file.read(&c, 1);
+ h += c;
}
if (header != NULL) {
-#if !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());
+ _snprintf_s(header, h.size() + 1, _TRUNCATE, "%s", h.c_str());
#endif
}
unsigned int rows, cols;
- file.read((char*)&rows, sizeof(unsigned int));
- file.read((char*)&cols, sizeof(unsigned int));
- A.resize(rows,cols);
+ file.read((char *)&rows, sizeof(unsigned int));
+ file.read((char *)&cols, sizeof(unsigned int));
+ A.resize(rows, cols);
Type value;
- for(unsigned int i = 0; i < rows; i++) {
- for(unsigned int j = 0; j < cols; j++) {
- file.read((char*)&value, sizeof(Type));
+ for (unsigned int i = 0; i < rows; i++) {
+ for (unsigned int j = 0; j < cols; j++) {
+ file.read((char *)&value, sizeof(Type));
A[i][j] = value;
}
}
@@ -420,39 +429,37 @@ public:
file.open(filename.c_str(), std::fstream::in);
- if(!file) {
+ if (!file) {
file.close();
return false;
}
- unsigned int rows = 0,cols = 0;
+ unsigned int rows = 0, cols = 0;
std::string h;
- std::string line,subs;
+ std::string line, subs;
bool inheader = true;
- unsigned int i=0, j;
+ unsigned int i = 0, j;
unsigned int lineStart = 0;
- while ( getline (file,line) ) {
- if(inheader) {
- if(rows == 0 && line.compare(0,5,"rows:") == 0) {
+ while (getline(file, line)) {
+ if (inheader) {
+ if (rows == 0 && line.compare(0, 5, "rows:") == 0) {
std::stringstream ss(line);
ss >> subs;
ss >> rows;
- }
- else if (cols == 0 && line.compare(0,5,"cols:") == 0) {
+ } else if (cols == 0 && line.compare(0, 5, "cols:") == 0) {
std::stringstream ss(line);
ss >> subs;
ss >> cols;
- }
- else if (line.compare(0,5,"data:") == 0)
+ } else if (line.compare(0, 5, "data:") == 0)
inheader = false;
else
h += line + "\n";
- }
- else {
- // if i == 0, we just got out of the header: initialize matrix dimensions
- if(i == 0) {
- if(rows == 0 || cols == 0) {
+ } else {
+ // if i == 0, we just got out of the header: initialize matrix
+ // dimensions
+ if (i == 0) {
+ if (rows == 0 || cols == 0) {
file.close();
return false;
}
@@ -462,7 +469,7 @@ public:
}
std::stringstream ss(line.substr(lineStart, line.find("]") - lineStart));
j = 0;
- while(getline(ss, subs, ','))
+ while (getline(ss, subs, ','))
A[i][j++] = atof(subs.c_str());
i++;
}
@@ -470,10 +477,11 @@ public:
if (header != NULL) {
std::string h_ = h.substr(0, h.size() - 1); // Remove last '\n' char
-#if !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());
+ _snprintf_s(header, h_.size() + 1, _TRUNCATE, "%s", h_.c_str());
#endif
}
@@ -486,8 +494,9 @@ public:
\param filename : Absolute file name.
\param A : Array to be saved.
- \param binary : If true the matrix is saved in a binary file, else a text file.
- \param header : Optional line that will be saved at the beginning of the file.
+ \param binary : If true the matrix is saved in a binary file, else a text
+ file. \param header : Optional line that will be saved at the beginning of
+ the file.
\return Returns true if success.
@@ -496,17 +505,17 @@ public:
\sa load()
*/
- static bool save(const std::string &filename, const vpArray2D<Type> &A,
- const bool binary = false, const char *header = "")
+ static bool save(const std::string &filename, const vpArray2D<Type> &A, const bool binary = false,
+ const char *header = "")
{
std::fstream file;
if (!binary)
file.open(filename.c_str(), std::fstream::out);
else
- file.open(filename.c_str(), std::fstream::out|std::fstream::binary);
+ file.open(filename.c_str(), std::fstream::out | std::fstream::binary);
- if(!file) {
+ if (!file) {
file.close();
return false;
}
@@ -523,21 +532,21 @@ public:
file << std::endl;
file << A.getRows() << "\t" << A.getCols() << std::endl;
file << A << std::endl;
- }
- else {
+ } else {
int headerSize = 0;
- while (header[headerSize] != '\0') headerSize++;
- file.write(header, headerSize+1);
+ while (header[headerSize] != '\0')
+ headerSize++;
+ file.write(header, headerSize + 1);
unsigned int matrixSize;
matrixSize = A.getRows();
- file.write((char*)&matrixSize, sizeof(unsigned int));
+ file.write((char *)&matrixSize, sizeof(unsigned int));
matrixSize = A.getCols();
- file.write((char*)&matrixSize, sizeof(unsigned int));
+ file.write((char *)&matrixSize, sizeof(unsigned int));
Type value;
- for(unsigned int i = 0; i < A.getRows(); i++) {
- for(unsigned int j = 0; j < A.getCols(); j++) {
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols(); j++) {
value = A[i][j];
- file.write((char*)&value, sizeof(Type));
+ file.write((char *)&value, sizeof(Type));
}
}
}
@@ -550,7 +559,8 @@ public:
\param filename : absolute file name.
\param A : array to be saved in the file.
- \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
+ \param header : optional lines that will be saved at the beginning of the
+ file. Should be YAML-formatted and will adapt to the indentation if any.
\return Returns true if success.
@@ -558,9 +568,8 @@ public:
\code
vpArray2D<double> M(3,4);
vpArray2D::saveYAML("matrix.yml", M, "example: a YAML-formatted header");
- vpArray2D::saveYAML("matrixIndent.yml", M, "example:\n - a YAML-formatted header\n - with inner indentation");
- \endcode
- Content of matrix.yml:
+ vpArray2D::saveYAML("matrixIndent.yml", M, "example:\n - a YAML-formatted
+ header\n - with inner indentation"); \endcode Content of matrix.yml:
\code
example: a YAML-formatted header
rows: 3
@@ -591,7 +600,7 @@ public:
file.open(filename.c_str(), std::fstream::out);
- if(!file) {
+ if (!file) {
file.close();
return false;
}
@@ -602,10 +611,10 @@ public:
bool checkIndent = true;
while (header[i] != '\0') {
file << header[i];
- if(checkIndent) {
+ if (checkIndent) {
if (inIndent) {
- if(header[i] == ' ')
- indent += " ";
+ if (header[i] == ' ')
+ indent += " ";
else if (indent.length() > 0)
checkIndent = false;
}
@@ -617,20 +626,19 @@ public:
i++;
}
- if(i != 0)
+ if (i != 0)
file << std::endl;
file << "rows: " << A.getRows() << std::endl;
file << "cols: " << A.getCols() << std::endl;
- if(indent.length() == 0)
+ if (indent.length() == 0)
indent = " ";
file << "data: " << std::endl;
unsigned int j;
- for(i=0;i<A.getRows();++i)
- {
+ for (i = 0; i < A.getRows(); ++i) {
file << indent << "- [";
- for(j=0;j<A.getCols()-1;++j)
+ for (j = 0; j < A.getCols() - 1; ++j)
file << A[i][j] << ", ";
file << A[i][j] << "]" << std::endl;
}
@@ -644,16 +652,14 @@ public:
/*!
Return the array min value.
*/
-template<class Type>
-Type
-vpArray2D<Type>::getMinValue() const
+template <class Type> Type vpArray2D<Type>::getMinValue() const
{
Type *dataptr = data;
Type min = *dataptr;
dataptr++;
- for (unsigned int i = 0; i < dsize-1; i++)
- {
- if (*dataptr < min) min = *dataptr;
+ for (unsigned int i = 0; i < dsize - 1; i++) {
+ if (*dataptr < min)
+ min = *dataptr;
dataptr++;
}
return min;
@@ -662,19 +668,39 @@ vpArray2D<Type>::getMinValue() const
/*!
Return the array max value.
*/
-template<class Type>
-Type
-vpArray2D<Type>::getMaxValue() const
+template <class Type> Type vpArray2D<Type>::getMaxValue() const
{
Type *dataptr = data;
Type max = *dataptr;
dataptr++;
- for (unsigned int i = 0; i < dsize-1; i++)
- {
- if (*dataptr > max) max = *dataptr;
+ for (unsigned int i = 0; i < dsize - 1; i++) {
+ if (*dataptr > max)
+ max = *dataptr;
dataptr++;
}
return max;
}
+/*!
+ Compute the Hadamard product (element wise matrix multiplication).
+ \param m : Second matrix;
+ \return m1.hadamard(m2) The Hadamard product : \f$ m1 \circ m2 = (m1 \circ
+ m2)_{i,j} = (m1)_{i,j} (m2)_{i,j} \f$
+*/
+template <class Type> vpArray2D<Type> vpArray2D<Type>::hadamard(const vpArray2D<Type> &m) const
+{
+ if (m.getRows() != rowNum || m.getCols() != colNum) {
+ throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
+ }
+
+ vpArray2D<Type> out;
+ out.resize(rowNum, colNum, false);
+
+ for (unsigned int i = 0; i < dsize; i++) {
+ out.data[i] = data[i] * m.data[i];
+ }
+
+ return out;
+}
+
#endif
diff --git a/modules/core/include/visp3/core/vpBSpline.h b/modules/core/include/visp3/core/vpBSpline.h
index ad39514..61a522e 100644
--- a/modules/core/include/visp3/core/vpBSpline.h
+++ b/modules/core/include/visp3/core/vpBSpline.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -32,7 +33,7 @@
*
* Authors:
* Nicolas Melchior
- *
+ *
*****************************************************************************/
#ifndef vpBSpline_H
@@ -45,22 +46,23 @@
#include <visp3/core/vpImagePoint.h>
-#include <vector>
#include <list>
+#include <vector>
#include <visp3/core/vpMatrix.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
Structure that defines a B-Spline basis function \f$ N_{i,p}^k(u) \f$.
-
- - i is the number of the knot interval in which the basis function is computed.
+
+ - i is the number of the knot interval in which the basis function is
+ computed.
- p is the degree of the B-Spline basis function.
- u is the "point" point of the curve where the basis function is computed.
- k indicates which kth derivative is computed.
- value is the numerical value of \f$ N_{i,p}^k(u) \f$.
*/
-typedef struct vpBasisFunction{
+typedef struct vpBasisFunction {
unsigned int i;
unsigned int p;
double u;
@@ -77,101 +79,112 @@ typedef struct vpBasisFunction{
The different parameters are :
- - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1}, i = 0, ...,m \f$.
- To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
+ - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i =
+ 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1}, i = 0, ...,m \f$. To
+ define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} ,
+ ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real
+ numbers and p is the degree of the B-Spline basis functions.
- The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
\f[ N_{i,0}(u) = \left\{\begin{array}{cc}
1 & \mbox{if } u_i \leq u \leq u_{i+1} \\ 0 & else
\end{array}\right.\f]
- \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
-
- where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
+ \f[ N_{i,p}(u) =
+ \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
+
+ where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis
+ functions.
- - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
+ - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$
+ (i,j) \f$ of a point in an image.
- It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
- \f[ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)\f]
+ It is possible to compute the coordinates of a point corresponding to the
+ knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula : \f[ C(u) =
+ \sum_{i=0}^n (N_{i,p}(u)P_i)\f]
- You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
+ You can find much more information about the B-Splines and the
+ implementation of all the methods in the Nurbs Book.
*/
class VISP_EXPORT vpBSpline
{
- public/*protected*/:
- //!Vector wich contains the control points
- std::vector<vpImagePoint> controlPoints;
- //! Vector which contain the knots \f$ {u0, ..., um} \f$
- std::vector<double> knots;
- //! Degree of the B-Spline basis functions.
- unsigned int p;
- //! Vector wich contains the points used during the interpolation method.
- std::vector<vpImagePoint> crossingPoints;
-
- public:
-
- vpBSpline();
- vpBSpline(const vpBSpline &bspline);
- virtual ~vpBSpline();
-
- /*!
- Gets the degree of the B-Spline.
-
- \return the degree of the B-Spline.
- */
- inline unsigned int get_p() const {return p;}
+public /*protected*/:
+ //! Vector wich contains the control points
+ std::vector<vpImagePoint> controlPoints;
+ //! Vector which contain the knots \f$ {u0, ..., um} \f$
+ std::vector<double> knots;
+ //! Degree of the B-Spline basis functions.
+ unsigned int p;
+ //! Vector wich contains the points used during the interpolation method.
+ std::vector<vpImagePoint> crossingPoints;
+
+public:
+ vpBSpline();
+ vpBSpline(const vpBSpline &bspline);
+ virtual ~vpBSpline();
+
+ /*!
+ Gets the degree of the B-Spline.
+
+ \return the degree of the B-Spline.
+ */
+ inline unsigned int get_p() const { return p; }
/*!
Gets all the control points.
- \param list : A std::list containing the coordinates of the control points.
+ \param list : A std::list containing the coordinates of the control
+ points.
*/
- inline void get_controlPoints(std::list<vpImagePoint> &list) const {
+ inline void get_controlPoints(std::list<vpImagePoint> &list) const
+ {
list.clear();
for (unsigned int i = 0; i < controlPoints.size(); i++)
- list.push_back(*(&(controlPoints[0])+i));
- }
+ list.push_back(*(&(controlPoints[0]) + i));
+ }
/*!
Gets all the knots.
\param list : A std::list containing the value of the knots.
*/
- inline void get_knots(std::list<double> &list) const {
+ inline void get_knots(std::list<double> &list) const
+ {
list.clear();
for (unsigned int i = 0; i < knots.size(); i++)
- list.push_back(*(&(knots[0])+i));
- }
+ list.push_back(*(&(knots[0]) + i));
+ }
/*!
Gets all the crossing points (used in the interpolation method)
- \param list : A std::list containing the coordinates of the crossing points.
+ \param list : A std::list containing the coordinates of the crossing
+ points.
*/
- inline void get_crossingPoints(std::list<vpImagePoint> &list) const {
+ inline void get_crossingPoints(std::list<vpImagePoint> &list) const
+ {
list.clear();
for (unsigned int i = 0; i < crossingPoints.size(); i++)
- list.push_back(*(&(crossingPoints[0])+i));
- }
+ list.push_back(*(&(crossingPoints[0]) + i));
+ }
-
- /*!
- Sets the degree of the B-Spline.
-
- \param degree : the degree of the B-Spline.
- */
- inline void set_p(unsigned int degree) {this->p = degree;}
+ /*!
+ Sets the degree of the B-Spline.
+\param degree : the degree of the B-Spline.
+ */
+ inline void set_p(unsigned int degree) { this->p = degree; }
/*!
Sets all the control points.
\param list : A std::list containing the coordinates of the control points
*/
- inline void set_controlPoints(const std::list<vpImagePoint> &list) {
+ inline void set_controlPoints(const std::list<vpImagePoint> &list)
+ {
controlPoints.clear();
- for(std::list<vpImagePoint>::const_iterator it = list.begin(); it!=list.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = list.begin(); it != list.end(); ++it) {
controlPoints.push_back(*it);
}
}
@@ -181,9 +194,10 @@ class VISP_EXPORT vpBSpline
\param list : A std::list containing the value of the knots.
*/
- inline void set_knots(const std::list<double> &list) {
+ inline void set_knots(const std::list<double> &list)
+ {
knots.clear();
- for(std::list<double>::const_iterator it = list.begin(); it!=list.end(); ++it){
+ for (std::list<double>::const_iterator it = list.begin(); it != list.end(); ++it) {
knots.push_back(*it);
}
}
@@ -191,29 +205,35 @@ class VISP_EXPORT vpBSpline
/*!
Sets all the crossing points (used in the interpolation method)
- \param list : A std::list containing the coordinates of the crossing points
+ \param list : A std::list containing the coordinates of the crossing
+ points
*/
- inline void set_crossingPoints(const std::list<vpImagePoint> &list) {
+ inline void set_crossingPoints(const std::list<vpImagePoint> &list)
+ {
crossingPoints.clear();
- for(std::list<vpImagePoint>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = list.begin(); it != list.end(); ++it) {
crossingPoints.push_back(*it);
}
}
- static unsigned int findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots);
- unsigned int findSpan(double u);
+ static unsigned int findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots);
+ unsigned int findSpan(double u);
- static vpBasisFunction* computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots);
- vpBasisFunction* computeBasisFuns(double u);
+ static vpBasisFunction *computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p,
+ std::vector<double> &l_knots);
+ vpBasisFunction *computeBasisFuns(double u);
- static vpBasisFunction** computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots);
- vpBasisFunction** computeDersBasisFuns(double u, unsigned int der);
+ static vpBasisFunction **computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots);
+ vpBasisFunction **computeDersBasisFuns(double u, unsigned int der);
- static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
- vpImagePoint computeCurvePoint(double u);
+ static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints);
+ vpImagePoint computeCurvePoint(double u);
- static vpImagePoint* computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
- vpImagePoint* computeCurveDers(double u, unsigned int der);
+ static vpImagePoint *computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints);
+ vpImagePoint *computeCurveDers(double u, unsigned int der);
};
#endif
diff --git a/modules/core/include/visp3/core/vpCPUFeatures.h b/modules/core/include/visp3/core/vpCPUFeatures.h
new file mode 100644
index 0000000..cc72b5f
--- /dev/null
+++ b/modules/core/include/visp3/core/vpCPUFeatures.h
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * CPU features (hardware capabilities).
+ *
+ *****************************************************************************/
+
+#ifndef __vpCPUFeatures_h_
+#define __vpCPUFeatures_h_
+
+/*!
+ \file vpCPUFeatures.h
+ \brief Check CPU features (hardware capabilities).
+*/
+
+#include <visp3/core/vpConfig.h>
+
+/*!
+ \ingroup group_core_cpu_features
+ \brief Check CPU features (hardware capabilities).
+
+ The example below shows how to check or get CPU capabilities.
+
+ \code
+#include <visp3/core/vpCPUFeatures.h>
+
+int main()
+{
+ std::cout << "checkSSE2: " << vpCPUFeatures::checkSSE2() << std::endl;
+ std::cout << "CPU info: " << vpCPUFeatures::printCPUInfo() << std::endl;
+ return 0;
+}
+ \endcode
+*/
+
+namespace vpCPUFeatures
+{
+VISP_EXPORT bool checkSSE2();
+VISP_EXPORT bool checkSSE3();
+VISP_EXPORT bool checkSSSE3();
+VISP_EXPORT bool checkSSE41();
+VISP_EXPORT bool checkSSE42();
+VISP_EXPORT bool checkAVX();
+VISP_EXPORT bool checkAVX2();
+VISP_EXPORT void printCPUInfo();
+}
+
+#endif
diff --git a/modules/core/include/visp3/core/vpCameraParameters.h b/modules/core/include/visp3/core/vpCameraParameters.h
index f494ae1..5523eea 100644
--- a/modules/core/include/visp3/core/vpCameraParameters.h
+++ b/modules/core/include/visp3/core/vpCameraParameters.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,8 +37,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpCameraParameters.h
\brief Declaration of the vpCameraParameters class.
@@ -50,10 +49,10 @@
#include <vector>
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMatrix.h>
/*!
\class vpCameraParameters
@@ -62,8 +61,9 @@
\brief Generic class defining intrinsic camera parameters.
- Let us define the pinhole camera model implemented in ViSP. In this model \cite Marchand16a, a scene view is formed
- by projecting 3D points into the image plane using a perspective transformation.
+ Let us define the pinhole camera model implemented in ViSP. In this model
+ \cite Marchand16a, a scene view is formed by projecting 3D points into the
+ image plane using a perspective transformation.
\f[
\left[ \begin{array}{c}
@@ -87,11 +87,13 @@
- \f$(X_c,Y_c,Z_c)\f$ are the coordinates of a 3D point in the camera frame
- \f$(u,v)\f$ are the coordinates in pixels of the projected 3D point
- - \f$(u_0,v_0)\f$ are the coordinates of the principal point (the intersection of the optical
- axes with the image plane) that is usually near the image center
- - \f$p_x\f$ (resp \f$p_y\f$) is the ratio between the focal length of the lens \f$f\f$
- in meters and the size of the pixel \f$l_x\f$ in meters: \f$p_x=f/l_x\f$ (resp, \f$l_y\f$
- being the height of a pixel, \f$p_y=f/l_y\f$).
+ - \f$(u_0,v_0)\f$ are the coordinates of the principal point (the
+ intersection of the optical axes with the image plane) that is usually near
+ the image center
+ - \f$p_x\f$ (resp \f$p_y\f$) is the ratio between the focal length of the
+ lens \f$f\f$ in meters and the size of the pixel \f$l_x\f$ in meters:
+ \f$p_x=f/l_x\f$ (resp, \f$l_y\f$ being the height of a pixel,
+ \f$p_y=f/l_y\f$).
When \f$Z_c \neq 0\f$, the previous equation si equivalent to the following:
\f[
@@ -103,7 +105,8 @@
\end{array}
\f]
- Real lenses usually have some radial distortion. So, the above model is extended as:
+ Real lenses usually have some radial distortion. So, the above model is
+ extended as:
\f[
\begin{array}{lcl}
@@ -117,10 +120,12 @@
\end{array}
\f]
- where \f$k_{ud}\f$ is the first order radial distorsion. Higher order distorsion coefficients are not considered in ViSP.
+ where \f$k_{ud}\f$ is the first order radial distorsion. Higher order
+ distorsion coefficients are not considered in ViSP.
- Now in ViSP we consider also the inverse transformation, where from pixel coordinates we want to compute their
- normalized coordinates in the image plane. Previous equations could be written like:
+ Now in ViSP we consider also the inverse transformation, where from pixel
+ coordinates we want to compute their normalized coordinates in the image
+ plane. Previous equations could be written like:
\f[
\begin{array}{lcl}
@@ -140,72 +145,78 @@
\end{array}
\f]
- Finally, in ViSP the main intrinsic camera parameters are \f$(p_x, p_y)\f$ the ratio
- between the focal length and the size of a pixel, and \f$(u_0,
+ Finally, in ViSP the main intrinsic camera parameters are \f$(p_x, p_y)\f$
+ the ratio between the focal length and the size of a pixel, and \f$(u_0,
v_0)\f$ the coordinates of the principal point in pixel. The lens
distortion can also be considered by two additional parameters
\f$(k_{ud}, k_{du})\f$.
\note The \ref tutorial-calibration shows how to calibrate a camera
- to estimate the parameters corresponding to the model implemented in this class.
+ to estimate the parameters corresponding to the model implemented in this
+ class.
- \note Note also that \ref tutorial-bridge-opencv gives the correspondance between
- ViSP and OpenCV camera modelization.
+ \note Note also that \ref tutorial-bridge-opencv gives the correspondance
+ between ViSP and OpenCV camera modelization.
\note The conversion from pixel coordinates \f$(u,v)\f$ in the normalized
space \f$(x,y)\f$ is implemented in vpPixelMeterConversion, whereas
the conversion from normalized coordinates into pixel is implemented
in vpMeterPixelConversion.
- From a practical point of view, two kinds of camera modelisation are implemented in this class:
+ From a practical point of view, two kinds of camera modelisation are
+ implemented in this class:
- <b>1. Camera parameters for a perspective projection without distortion model</b>
+ <b>1. Camera parameters for a perspective projection without distortion
+ model</b>
In this modelisation, only \f$u_0,v_0,p_x,p_y\f$ parameters are considered.
Initialization of such a model can be done using:
- - initPersProjWithoutDistortion() that allows to set \f$u_0,v_0,p_x,p_y\f$ parameters;
- - initFromFov() that computes the parameters from an image size and a camera field of view.
+ - initPersProjWithoutDistortion() that allows to set \f$u_0,v_0,p_x,p_y\f$
+ parameters;
+ - initFromFov() that computes the parameters from an image size and a camera
+ field of view.
- <b>2. Camera parameters for a perspective projection with distortion model</b>
+ <b>2. Camera parameters for a perspective projection with distortion
+ model</b>
In this modelisation, all the parameters \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$
are considered. Initialization of such a model can be done using:
- - initPersProjWithDistortion() that allows to set \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$ parameters;
+ - initPersProjWithDistortion() that allows to set
+ \f$u_0,v_0,p_x,p_y,k_{ud},k_{du}\f$ parameters;
- The selection of the camera model (without or with distorsion) is done during
- vpCameraParameters initialisation.
+ The selection of the camera model (without or with distorsion) is done
+ during vpCameraParameters initialisation.
- Here an example of camera initialisation, for a model without distortion. A complete example is given in initPersProjWithoutDistortion().
- \code
- double px = 600;
- double py = 600;
- double u0 = 320;
- double v0 = 240;
+ Here an example of camera initialisation, for a model without distortion. A
+ complete example is given in initPersProjWithoutDistortion().
+
+\code
+ double px = 600; double py = 600; double u0 = 320; double v0 = 240;
// Create a camera parameter container
vpCameraParameters cam;
- // Camera initialization with a perspective projection without distortion model
+ // Camera initialization with a perspective projection without distortion
+ // model
cam.initPersProjWithoutDistortion(px,py,u0,v0);
// It is also possible to print the current camera parameters
std::cout << cam << std::endl;
- \endcode
+\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;
+ Here an example of camera initialisation, for a model with distortion. A
+ complete example is given in initPersProjWithDistortion().
+
+\code
+ double px = 600; double py = 600;
+ double u0 = 320; double v0 = 240;
+ double kud = -0.19; double kdu = 0.20;
// Create a camera parameter container
vpCameraParameters cam;
- // Camera initialization with a perspective projection without distortion model
- cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
- \endcode
+ // Camera initialization with a perspective projection without distortion
+ model cam.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
+\endcode
The code below shows how to know the currently used projection model:
\code
@@ -222,44 +233,44 @@ class VISP_EXPORT vpCameraParameters
{
friend class vpMeterPixelConversion;
friend class vpPixelMeterConversion;
-public :
- typedef enum{
- perspectiveProjWithoutDistortion, //!< Perspective projection without distortion model
- perspectiveProjWithDistortion //!< Perspective projection with distortion model
- } vpCameraParametersProjType ;
-
-
- //generic functions
- vpCameraParameters() ;
- vpCameraParameters(const vpCameraParameters &c) ;
- vpCameraParameters(const double px, const double py,
- const double u0, const double v0) ;
- vpCameraParameters(const double px, const double py,
- const double u0, const double v0,
- const double kud, const double kdu) ;
-
- vpCameraParameters& operator =(const vpCameraParameters &c) ;
- virtual ~vpCameraParameters() ;
-
- void init() ;
- void init(const vpCameraParameters &c) ;
- void initFromCalibrationMatrix(const vpMatrix& _K);
+
+public:
+ typedef enum {
+ perspectiveProjWithoutDistortion, //!< Perspective projection without
+ //!< distortion model
+ perspectiveProjWithDistortion //!< Perspective projection with distortion
+ //!< model
+ } vpCameraParametersProjType;
+
+ // generic functions
+ vpCameraParameters();
+ vpCameraParameters(const vpCameraParameters &c);
+ vpCameraParameters(const double px, const double py, const double u0, const double v0);
+ vpCameraParameters(const double px, const double py, const double u0, const double v0, const double kud,
+ const double kdu);
+
+ vpCameraParameters &operator=(const vpCameraParameters &c);
+ virtual ~vpCameraParameters();
+
+ void init();
+ void init(const vpCameraParameters &c);
+ void initFromCalibrationMatrix(const vpMatrix &_K);
void initFromFov(const unsigned int &w, const unsigned int &h, const double &hfov, const double &vfov);
void initPersProjWithoutDistortion(const double px, const double py, const double u0, const double v0);
- void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0,
- const double kud,const double kdu) ;
-
+ void initPersProjWithDistortion(const double px, const double py, const double u0, const double v0, const double kud,
+ const double kdu);
+
/*!
Specify if the fov has been computed.
-
+
\sa computeFov()
-
+
\return True if the fov has been computed, False otherwise.
*/
inline bool isFovComputed() const { return isFov; }
-
+
void computeFov(const unsigned int &w, const unsigned int &h);
-
+
/*!
Get the horizontal angle in radian of the field of view.
@@ -267,8 +278,11 @@ public :
\sa computeFov(), getVerticalFovAngle()
*/
- inline double getHorizontalFovAngle() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() won't be significant.");
+ inline double getHorizontalFovAngle() const
+ {
+ if (!isFov)
+ vpTRACE("Warning: The FOV is not computed, getHorizontalFovAngle() "
+ "won't be significant.");
return m_hFovAngle;
}
@@ -279,44 +293,49 @@ public :
\sa computeFov(), getHorizontalFovAngle()
*/
- inline double getVerticalFovAngle() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't be significant.");
+ inline double getVerticalFovAngle() const
+ {
+ if (!isFov)
+ vpTRACE("Warning: The FOV is not computed, getVerticalFovAngle() won't "
+ "be significant.");
return m_vFovAngle;
}
-
+
/*!
- Get the list of the normals corresponding to planes describing the field of view.
+ Get the list of the normals corresponding to planes describing the field
+ of view.
- vector[0] : Left Normal.
- vector[1] : Right Normal.
- vector[2] : Up Normal.
- vector[3] : Down Normal.
-
+
\sa computeFov()
-
+
\return List of the normals.
*/
- inline std::vector<vpColVector> getFovNormals() const {
- if(!isFov) vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be significant.");
- return fovNormals;
+ inline std::vector<vpColVector> getFovNormals() const
+ {
+ if (!isFov)
+ vpTRACE("Warning: The FOV is not computed, getFovNormals() won't be "
+ "significant.");
+ return fovNormals;
}
-
+
inline double get_px() const { return px; }
- inline double get_px_inverse() const {return inv_px; }
- inline double get_py_inverse() const {return inv_py; }
+ inline double get_px_inverse() const { return inv_px; }
+ inline double get_py_inverse() const { return inv_py; }
inline double get_py() const { return py; }
inline double get_u0() const { return u0; }
inline double get_v0() const { return v0; }
inline double get_kud() const { return kud; }
inline double get_kdu() const { return kdu; }
-
- inline vpCameraParametersProjType get_projModel() const {
- return projModel;
- }
-
+
+ inline vpCameraParametersProjType get_projModel() const { return projModel; }
+
vpMatrix get_K() const;
vpMatrix get_K_inverse() const;
- void printParameters() ;
+ void printParameters();
friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpCameraParameters &cam);
private:
@@ -326,24 +345,23 @@ private:
static const double DEFAULT_PY_PARAMETER;
static const double DEFAULT_KUD_PARAMETER;
static const double DEFAULT_KDU_PARAMETER;
- static const vpCameraParametersProjType DEFAULT_PROJ_TYPE;
-
-
- double px, py ; //!< pixel size
- double u0, v0 ; //!< principal point
- double kud ; //!< radial distortion (from undistorted to distorted)
- double kdu ; //!< radial distortion (from distorted to undistorted)
-
- unsigned int width ; //!< Width of the image used for the fov computation
- unsigned int height ; //!< Height of the image used for the fov computation
- bool isFov ; //!< Boolean to specify if the fov has been computed
- double m_hFovAngle ; //!< Field of view horizontal angle
- double m_vFovAngle ; //!< Field of view vertical angle
- std::vector<vpColVector> fovNormals ; //!< Normals of the planes describing the fov
-
- double inv_px, inv_py;
-
- vpCameraParametersProjType projModel ; //!< used projection model
-} ;
+ static const vpCameraParametersProjType DEFAULT_PROJ_TYPE;
+
+ double px, py; //!< pixel size
+ double u0, v0; //!< principal point
+ double kud; //!< radial distortion (from undistorted to distorted)
+ double kdu; //!< radial distortion (from distorted to undistorted)
+
+ unsigned int width; //!< Width of the image used for the fov computation
+ unsigned int height; //!< Height of the image used for the fov computation
+ bool isFov; //!< Boolean to specify if the fov has been computed
+ double m_hFovAngle; //!< Field of view horizontal angle
+ double m_vFovAngle; //!< Field of view vertical angle
+ std::vector<vpColVector> fovNormals; //!< Normals of the planes describing the fov
+
+ double inv_px, inv_py;
+
+ vpCameraParametersProjType projModel; //!< used projection model
+};
#endif
diff --git a/modules/core/include/visp3/core/vpCircle.h b/modules/core/include/visp3/core/vpCircle.h
index 0c60ef8..87c3952 100644
--- a/modules/core/include/visp3/core/vpCircle.h
+++ b/modules/core/include/visp3/core/vpCircle.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,11 +44,11 @@
#ifndef vpCircle_hh
#define vpCircle_hh
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <math.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
/*!
\class vpCircle
@@ -57,62 +58,45 @@
class VISP_EXPORT vpCircle : public vpForwardProjection
{
public:
- void init() ;
- vpCircle() ;
- vpCircle(const vpColVector& oP) ;
- vpCircle(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
- virtual ~vpCircle() ;
-
-
- void setWorldCoordinates(const vpColVector& oP) ;
- void setWorldCoordinates(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
-
-
- double getA() const { return cP[0] ; }
- double getB() const{ return cP[1] ; }
- double getC() const { return cP[2] ; }
-
- double getX() const { return cP[3] ; }
- double getY() const { return cP[4] ; }
- double getZ() const{ return cP[5] ; }
-
- double getR() const { return cP[6] ; }
-
-
-
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
-
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- vpCircle *duplicate() const ;
+ 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);
+ virtual ~vpCircle();
+
+ void setWorldCoordinates(const vpColVector &oP);
+ void setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
+ const double Z0, const double R);
+
+ double getA() const { return cP[0]; }
+ double getB() const { return cP[1]; }
+ double getC() const { return cP[2]; }
+
+ double getX() const { return cP[3]; }
+ double getY() const { return cP[4]; }
+ double getZ() const { return cP[5]; }
+
+ double getR() const { return cP[6]; }
+
+ void projection();
+ void projection(const vpColVector &cP, vpColVector &p);
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP);
+ void changeFrame(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green,
+ const unsigned int thickness = 1);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
+ vpCircle *duplicate() const;
//###################
// Static Functions
//###################
- public:
- static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j);
-
-} ;
-
+public:
+ static void computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
+ const double &theta, double &i, double &j);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpClient.h b/modules/core/include/visp3/core/vpClient.h
index a9d3873..36551d8 100644
--- a/modules/core/include/visp3/core/vpClient.h
+++ b/modules/core/include/visp3/core/vpClient.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,27 +42,27 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
-#include <visp3/core/vpRequest.h>
#include <visp3/core/vpNetwork.h>
+#include <visp3/core/vpRequest.h>
#include <visp3/core/vpTime.h>
-
/*!
\class vpClient
-
+
\ingroup group_core_network
\brief This class represents a Transmission Control Protocol (TCP) client.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
on one computer to another program on another computer.
Exemple of client's code, receiving and sending basic message
- It corresponds to the client used in the first exemple of vpServer class' documentation:
-
+ It corresponds to the client used in the first exemple of vpServer class'
+documentation:
+
\code
-#include <visp3/core/vpClient.h>
#include <iostream>
+#include <visp3/core/vpClient.h>
int main(int argc, char **argv)
{
@@ -76,43 +77,47 @@ int main(int argc, char **argv)
while(1)
{
- if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
+ // 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;
-
- if(client.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
+ 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;
}
\endcode
-
+
Exemple of client's code, sending a vpImage on request form.
- It correspond to the server used in the second exemple of vpServer class' documentation.
-
+ It correspond to the server used in the second exemple of vpServer class'
+documentation.
+
\code
+#include <iostream>
#include <visp3/core/vpClient.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
-#include <iostream>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
#include "vpRequestImage.h" //See vpRequest class documentation
int main(int argc, char **argv)
{
-#if defined(VISP_HAVE_V4L2)
+#if defined(VISP_HAVE_V4L2)
std::string servername = "localhost";
unsigned int port = 35000;
-
+
vpImage<unsigned char> I; // Create a gray level image container
-
- // Create a grabber based on v4l2 third party lib (for usb cameras under Linux)
+
+ // Create a grabber based on v4l2 third party lib (for usb cameras under
+ // Linux)
vpV4l2Grabber g;
g.setScale(1);
g.setInput(0);
@@ -136,10 +141,10 @@ int main(int argc, char **argv)
double t = vpTime::measureTimeMs();
// Acquire a new image
g.acquire(I);
-
+
vpDisplay::display(I);
vpDisplay::flush(I);
-
+
client.sendAndEncodeRequest(reqImage);
// A click in the viewer to exit
@@ -151,7 +156,7 @@ int main(int argc, char **argv)
#endif
}
\endcode
-
+
\sa vpClient
\sa vpRequest
\sa vpNetwork
@@ -162,51 +167,51 @@ private:
//######## PARAMETERS ########
//# #
//############################
-
- unsigned int numberOfAttempts;
-
+
+ unsigned int numberOfAttempts;
+
//######## Private Functions ########
//# #
//###################################
-
- bool connectServer(vpNetwork::vpReceptor &serv);
-
+
+ bool connectServer(vpNetwork::vpReceptor &serv);
+
public:
- vpClient();
- virtual ~vpClient();
-
- bool connectToHostname(const std::string &hostname, const unsigned int &port_serv);
- bool connectToIP(const std::string &ip, const unsigned int &port_serv);
-
- void deconnect(const unsigned int &index = 0);
+ vpClient();
+ virtual ~vpClient();
+
+ bool connectToHostname(const std::string &hostname, const unsigned int &port_serv);
+ bool connectToIP(const std::string &ip, const unsigned int &port_serv);
+
+ void deconnect(const unsigned int &index = 0);
/*!
Get the actual number of attempts to connect to the server.
-
+
\sa vpClient::setNumberOfAttempts()
\return Actual number of attempts.
*/
- unsigned int getNumberOfAttempts(){ return numberOfAttempts; }
-
+ unsigned int getNumberOfAttempts() { return numberOfAttempts; }
+
/*!
Get the number of server that the client is connected on.
\return Number of servers.
*/
- unsigned int getNumberOfServers(){ return (unsigned int)receptor_list.size(); }
-
- void print();
-
+ unsigned int getNumberOfServers() { return (unsigned int)receptor_list.size(); }
+
+ void print();
+
/*!
Set the number of attempts to connect to the server.
-
+
\sa vpClient::getNumberOfAttempts()
\param nb : Number of attempts.
*/
- void setNumberOfAttempts(const unsigned int &nb){ numberOfAttempts = nb; }
-
- void stop();
+ void setNumberOfAttempts(const unsigned int &nb) { numberOfAttempts = nb; }
+
+ void stop();
};
#endif
diff --git a/modules/core/include/visp3/core/vpColVector.h b/modules/core/include/visp3/core/vpColVector.h
index 3a7bc22..8243cd0 100644
--- a/modules/core/include/visp3/core/vpColVector.h
+++ b/modules/core/include/visp3/core/vpColVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,16 +36,14 @@
*
*****************************************************************************/
-
-
#ifndef vpColVector_H
#define vpColVector_H
#include <visp3/core/vpArray2D.h>
-#include <visp3/core/vpRowVector.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpRotationVector.h>
+#include <visp3/core/vpRowVector.h>
class vpMatrix;
class vpRowVector;
@@ -64,36 +63,44 @@ class vpPoseVector;
\brief Implementation of column vector and the associated operations.
- This class provides a data structure for a column vector that contains values of double.
- It contains also some functions to achieve a set of operations on these vectors.
+ This class provides a data structure for a column vector that contains
+ values of double. It contains also some functions to achieve a set of
+ operations on these vectors.
The vpColVector class is derived from vpArray2D<double>.
*/
class VISP_EXPORT vpColVector : public vpArray2D<double>
{
- friend class vpMatrix;
-public:
+ friend class vpMatrix;
- //! Basic constructor that creates an empty 0-size column vector.
+public:
+ //! Basic constructor that creates an empty 0-size column vector.
vpColVector() : vpArray2D<double>() {}
- //! Construct a column vector of size n. All the elements are initialized to zero.
- vpColVector(unsigned int n) : vpArray2D<double>(n,1){}
+ //! Construct a column vector of size n. \warning Elements are not
+ //! initialized. If you want to set an initial value use
+ //! vpColVector(unsigned int, double).
+ explicit vpColVector(unsigned int n) : vpArray2D<double>(n, 1) {}
//! Construct a column vector of size n. Each element is set to \e val.
- vpColVector(unsigned int n, double val) : vpArray2D<double>(n, 1, val){}
- //! Copy constructor that allows to construct a column vector from an other one.
+ vpColVector(unsigned int n, double val) : vpArray2D<double>(n, 1, val) {}
+ //! Copy constructor that allows to construct a column vector from an other
+ //! one.
vpColVector(const vpColVector &v) : vpArray2D<double>(v) {}
- vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) ;
- //! Constructor that initialize a column vector from a 3-dim (Euler or \f$\theta {\bf u}\f$)
- //! or 4-dim (quaternion) rotation vector.
+ vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows);
+ //! Constructor that initialize a column vector from a 3-dim (Euler or
+ //! \f$\theta {\bf u}\f$) or 4-dim (quaternion) rotation vector.
vpColVector(const vpRotationVector &v);
//! Constructor that initialize a column vector from a 6-dim pose vector.
vpColVector(const vpPoseVector &p);
- //! Constructor that initialize a column vector from a 3-dim translation vector.
+ //! Constructor that initialize a column vector from a 3-dim translation
+ //! vector.
vpColVector(const vpTranslationVector &t);
vpColVector(const vpMatrix &M);
vpColVector(const vpMatrix &M, unsigned int j);
vpColVector(const std::vector<double> &v);
vpColVector(const std::vector<float> &v);
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ vpColVector(vpColVector &&v);
+#endif
/*!
Destructor.
*/
@@ -105,27 +112,28 @@ public:
*/
void clear()
{
- if (data != NULL ) {
+ if (data != NULL) {
free(data);
- data=NULL;
+ data = NULL;
}
- if (rowPtrs!=NULL) {
+ if (rowPtrs != NULL) {
free(rowPtrs);
- rowPtrs=NULL ;
+ rowPtrs = NULL;
}
rowNum = colNum = dsize = 0;
}
- std::ostream & cppPrint(std::ostream & os, const std::string &matrixName="A", bool octet = false) const;
- std::ostream & csvPrint(std::ostream & os) const;
+ std::ostream &cppPrint(std::ostream &os, const std::string &matrixName = "A", bool octet = false) const;
+ std::ostream &csvPrint(std::ostream &os) const;
/*!
Convert a column vector containing angles in degrees into radians.
\sa rad2deg()
*/
- inline void deg2rad() {
- double d2r = M_PI/180.0;
+ inline void deg2rad()
+ {
+ double d2r = M_PI / 180.0;
(*this) *= d2r;
}
@@ -133,9 +141,10 @@ public:
double euclideanNorm() const;
/*!
Extract a sub-column vector from a column vector.
- \param r : Index of the row corresponding to the first element of the vector to extract.
- \param colsize : Size of the vector to extract.
- \exception vpException::fatalError If the vector to extract is not contained in the original one.
+ \param r : Index of the row corresponding to the first element of the
+ vector to extract. \param colsize : Size of the vector to extract.
+ \exception vpException::fatalError If the vector to extract is not
+ contained in the original one.
\code
vpColVector v1;
@@ -148,30 +157,33 @@ public:
*/
vpColVector extract(unsigned int r, unsigned int colsize) const
{
- if (r >= rowNum || r+colsize > rowNum) {
+ if (r >= rowNum || r + colsize > rowNum) {
throw(vpException(vpException::fatalError,
- "Cannot extract a (%dx1) column vector from a (%dx1) column vector starting at index %d",
+ "Cannot extract a (%dx1) column vector from a (%dx1) "
+ "column vector starting at index %d",
colsize, rowNum, r));
}
return vpColVector(*this, r, colsize);
}
+ vpColVector hadamard(const vpColVector &v) const;
+
double infinityNorm() const;
void init(const vpColVector &v, unsigned int r, unsigned int nrows);
void insert(unsigned int i, const vpColVector &v);
void insert(const vpColVector &v, unsigned int i);
- std::ostream & maplePrint(std::ostream & os) const;
- std::ostream & matlabPrint(std::ostream & os) const;
+ std::ostream &maplePrint(std::ostream &os) const;
+ std::ostream &matlabPrint(std::ostream &os) const;
- vpColVector &normalize() ;
- vpColVector &normalize(vpColVector &x) const ;
+ vpColVector &normalize();
+ vpColVector &normalize(vpColVector &x) const;
//! Operator that allows to set a value of an element \f$v_i\f$: v[i] = x
- inline double &operator[](unsigned int n) { return *(data + n); }
+ 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);
@@ -181,9 +193,12 @@ public:
vpColVector &operator=(const std::vector<double> &v);
vpColVector &operator=(const std::vector<float> &v);
vpColVector &operator=(double x);
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ vpColVector &operator=(vpColVector &&v);
+#endif
double operator*(const vpColVector &x) const;
- vpMatrix operator*(const vpRowVector &v) const;
+ vpMatrix operator*(const vpRowVector &v) const;
vpColVector operator*(const double x) const;
vpColVector &operator*=(double x);
@@ -201,19 +216,20 @@ public:
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.
\sa deg2rad()
*/
- inline void rad2deg() {
- double r2d = 180.0/M_PI;
+ inline void rad2deg()
+ {
+ double r2d = 180.0 / M_PI;
(*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.
@@ -222,10 +238,7 @@ public:
\param flagNullify : If true, set the data to zero.
\exception vpException::fatalError When \e ncols is not equal to 1.
*/
- void resize(const unsigned int i, const bool flagNullify = true)
- {
- vpArray2D<double>::resize(i, 1, flagNullify);
- }
+ void resize(const unsigned int i, const bool flagNullify = true) { vpArray2D<double>::resize(i, 1, flagNullify); }
/*!
Resize the column vector to a \e nrows-dimension vector.
This function can only be used with \e ncols = 1.
@@ -240,7 +253,8 @@ public:
{
if (ncols != 1)
throw(vpException(vpException::fatalError,
- "Cannot resize a column vector to a (%dx%d) dimension vector that has more than one column",
+ "Cannot resize a column vector to a (%dx%d) "
+ "dimension vector that has more than one column",
nrows, ncols));
vpArray2D<double>::resize(nrows, ncols, flagNullify);
}
@@ -255,33 +269,30 @@ public:
void transpose(vpRowVector &v) const;
/*!
- Compute and return the cross product of two 3-dimension vectors: \f$a \times b\f$.
- \param a : 3-dimension column vector.
- \param b : 3-dimension column vector.
- \return The cross product \f$a \times b\f$.
+ Compute and return the cross product of two 3-dimension vectors: \f$a
+ \times b\f$. \param a : 3-dimension column vector. \param b : 3-dimension
+ column vector. \return The cross product \f$a \times b\f$.
- \exception vpException::dimensionError If the vectors dimension is not equal to 3.
+ \exception vpException::dimensionError If the vectors dimension is not
+ equal to 3.
*/
- inline static vpColVector cross(const vpColVector &a, const vpColVector &b)
- {
- return crossProd(a,b);
- }
- static vpColVector crossProd(const vpColVector &a, const vpColVector &b) ;
+ inline static vpColVector cross(const vpColVector &a, const vpColVector &b) { return crossProd(a, b); }
+ static vpColVector crossProd(const vpColVector &a, const vpColVector &b);
- static double dotProd(const vpColVector &a, const vpColVector &b) ;
- static vpColVector invSort(const vpColVector &v) ;
- static double median(const vpColVector &v) ;
- static double mean(const vpColVector &v) ;
+ static double dotProd(const vpColVector &a, const vpColVector &b);
+ static vpColVector invSort(const vpColVector &v);
+ static double median(const vpColVector &v);
+ static double mean(const vpColVector &v);
// Compute the skew matrix [v]x
static vpMatrix skew(const vpColVector &v);
- static vpColVector sort(const vpColVector &v) ;
+ static vpColVector sort(const vpColVector &v);
static vpColVector stack(const vpColVector &A, const vpColVector &B);
static void stack(const vpColVector &A, const vpColVector &B, vpColVector &C);
- static double stdev(const vpColVector &v, const bool useBesselCorrection=false);
+ static double stdev(const vpColVector &v, const bool useBesselCorrection = false);
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
@@ -298,33 +309,38 @@ public:
*/
vp_deprecated vpColVector rows(unsigned int first_row, unsigned int last_row) const
{
- return vpColVector(*this, first_row-1, last_row-first_row+1);
+ return vpColVector(*this, first_row - 1, last_row - first_row + 1);
}
/*!
\deprecated You should rather use eye()
*/
- vp_deprecated void setIdentity(const double & val=1.0) ;
+ vp_deprecated void setIdentity(const double &val = 1.0);
/*!
\deprecated You should rather use stack(const vpColVector &)
*/
vp_deprecated void stackMatrices(const vpColVector &r) { stack(r); }
/*!
- \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B)
+ \deprecated You should rather use stack(const vpColVector &A, const
+ vpColVector &B)
*/
vp_deprecated static vpColVector stackMatrices(const vpColVector &A, const vpColVector &B) { return stack(A, B); }
/*!
- \deprecated You should rather use stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
+ \deprecated You should rather use stack(const vpColVector &A, const
+ vpColVector &B, vpColVector &C)
*/
- vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C) { stack(A, B, C); }
+ 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
};
#ifndef DOXYGEN_SHOULD_SKIP_THIS
VISP_EXPORT
#endif
-vpColVector operator*(const double &x, const vpColVector &v) ;
+vpColVector operator*(const double &x, const vpColVector &v);
#endif
diff --git a/modules/core/include/visp3/core/vpColor.h b/modules/core/include/visp3/core/vpColor.h
index 503dc6c..6666f7c 100644
--- a/modules/core/include/visp3/core/vpColor.h
+++ b/modules/core/include/visp3/core/vpColor.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,11 +37,9 @@
*
*****************************************************************************/
-
#ifndef vpColor_hh
#define vpColor_hh
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpRGBa.h>
@@ -66,11 +65,11 @@
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
int main()
{
@@ -117,35 +116,53 @@ int main()
}
\endcode
-*/
+*/
class VISP_EXPORT vpColor : public vpRGBa
{
- public:
+public:
/*! Predefined colors identifier. */
typedef enum {
- id_black=0, /*!< Identifier associated to the predefined vpColor::black color. */
- id_white, /*!< Identifier associated to the predefined vpColor::white color. */
- id_lightGray, /*!< Identifier associated to the predefined vpColor::lightGray color. */
- id_gray, /*!< Identifier associated to the predefined vpColor::gray color. */
- id_darkGray, /*!< Identifier associated to the predefined vpColor::darkGray color. */
- id_lightRed, /*!< Identifier associated to the predefined vpColor::lightRed color. */
- id_red, /*!< Identifier associated to the predefined vpColor::red color. */
- id_darkRed, /*!< Identifier associated to the predefined vpColor::darkRed color. */
- id_lightGreen, /*!< Identifier associated to the predefined vpColor::lightGreen color. */
- id_green, /*!< Identifier associated to the predefined vpColor::green color. */
- id_darkGreen, /*!< Identifier associated to the predefined vpColor::darkGreen color. */
- id_lightBlue, /*!< Identifier associated to the predefined vpColor::lightBlue color. */
- id_blue, /*!< Identifier associated to the predefined vpColor::blue color. */
- id_darkBlue, /*!< Identifier associated to the predefined vpColor::darkBlue color. */
- id_yellow, /*!< Identifier associated to the predefined vpColor::yellow color. */
- id_cyan, /*!< Identifier associated to the predefined vpColor::cyan color. */
- id_orange, /*!< Identifier associated to the predefined vpColor::orange color. */
- id_purple, /*!< Identifier associated to the predefined vpColor::purple color. */
-
- id_unknown /*!< Identifier associated to unknowned
- colors. By unknowned, we mean not a predefined
- color. This identifier can also be used to know
- the number of predefined colors. */
+ id_black = 0, /*!< Identifier associated to the predefined vpColor::black
+ color. */
+ id_white, /*!< Identifier associated to the predefined vpColor::white
+ color. */
+ id_lightGray, /*!< Identifier associated to the predefined
+ vpColor::lightGray color. */
+ id_gray, /*!< Identifier associated to the predefined vpColor::gray color.
+ */
+ id_darkGray, /*!< Identifier associated to the predefined
+ vpColor::darkGray color. */
+ id_lightRed, /*!< Identifier associated to the predefined
+ vpColor::lightRed color. */
+ id_red, /*!< Identifier associated to the predefined vpColor::red color.
+ */
+ id_darkRed, /*!< Identifier associated to the predefined vpColor::darkRed
+ color. */
+ id_lightGreen, /*!< Identifier associated to the predefined
+ vpColor::lightGreen color. */
+ id_green, /*!< Identifier associated to the predefined vpColor::green
+ color. */
+ id_darkGreen, /*!< Identifier associated to the predefined
+ vpColor::darkGreen color. */
+ id_lightBlue, /*!< Identifier associated to the predefined
+ vpColor::lightBlue color. */
+ id_blue, /*!< Identifier associated to the predefined vpColor::blue color.
+ */
+ id_darkBlue, /*!< Identifier associated to the predefined
+ vpColor::darkBlue color. */
+ id_yellow, /*!< Identifier associated to the predefined vpColor::yellow
+ color. */
+ id_cyan, /*!< Identifier associated to the predefined vpColor::cyan color.
+ */
+ id_orange, /*!< Identifier associated to the predefined vpColor::orange
+ color. */
+ id_purple, /*!< Identifier associated to the predefined vpColor::purple
+ color. */
+
+ id_unknown /*!< Identifier associated to unknowned
+ colors. By unknowned, we mean not a predefined
+ color. This identifier can also be used to know
+ the number of predefined colors. */
} vpColorIdentifier;
@@ -154,61 +171,58 @@ class VISP_EXPORT vpColor : public vpRGBa
RGB values. */
/* Predefined colors. */
- static const vpColor black ;
- static const vpColor white ;
- static const vpColor lightGray;
- static const vpColor gray;
- static const vpColor darkGray;
- static const vpColor lightRed;
- static const vpColor red ;
- static const vpColor darkRed;
- static const vpColor lightGreen;
- static const vpColor green;
- static const vpColor darkGreen;
- static const vpColor lightBlue;
- static const vpColor blue ;
- static const vpColor darkBlue;
- static const vpColor yellow ;
- static const vpColor cyan;
- static const vpColor orange;
- static const vpColor purple;
- static const vpColor none;
+ static const vpColor black;
+ static const vpColor white;
+ static const vpColor lightGray;
+ static const vpColor gray;
+ static const vpColor darkGray;
+ static const vpColor lightRed;
+ static const vpColor red;
+ static const vpColor darkRed;
+ static const vpColor lightGreen;
+ static const vpColor green;
+ static const vpColor darkGreen;
+ static const vpColor lightBlue;
+ static const vpColor blue;
+ static const vpColor darkBlue;
+ static const vpColor yellow;
+ static const vpColor cyan;
+ static const vpColor orange;
+ static const vpColor purple;
+ static const vpColor none;
static const unsigned int nbColors;
static const vpColor allColors[];
/*!
- Default constructor. All the colors components are set to zero.
+ Default constructor. All the colors components are set to zero.
The color identifier is set to vpColor::id_unknown to indicate
that this color is not a predefined one.
- */
- inline vpColor()
- : vpRGBa(), id(id_unknown)
- {};
+ */
+ inline vpColor() : vpRGBa(), id(id_unknown){};
/*!
Construct a color from its RGB values.
-
+
\param r : Red component.
\param g : Green component.
\param b : Blue component.
\param cid : The color identifier to indicate if this color is or
not a predefined one.
- */
+ */
inline vpColor(unsigned char r, unsigned char g, unsigned char b,
- vpColor::vpColorIdentifier cid=vpColor::id_unknown)
- : vpRGBa(r, g, b), id(cid)
- {};
+ vpColor::vpColorIdentifier cid = vpColor::id_unknown)
+ : vpRGBa(r, g, b), id(cid){};
/*! Default destructor. */
- inline virtual ~vpColor() {};
+ inline virtual ~vpColor(){};
- friend VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 );
- friend VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 );
+ friend VISP_EXPORT bool operator==(const vpColor &c1, const vpColor &c2);
+ friend VISP_EXPORT bool operator!=(const vpColor &c1, const vpColor &c2);
/*!
Set a color from its RGB values.
-
+
\param r : Red component.
\param g : Green component.
\param b : Blue component.
@@ -216,8 +230,9 @@ class VISP_EXPORT vpColor : public vpRGBa
The color identifier is set to vpColor::id_unknown to indicate
that this color is not a predefined one.
- */
- inline void setColor(unsigned char r, unsigned char g, unsigned char b) {
+ */
+ inline void setColor(unsigned char r, unsigned char g, unsigned char b)
+ {
this->R = r;
this->G = g;
this->B = b;
@@ -231,57 +246,57 @@ 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);
-/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white. */
-vpColor const __declspec(selectany) vpColor::white = vpColor(255, 255, 255, id_white);
-/*!< Predefined light gray color with R=G=B=64 and identifier vpColor::id_lightGray. */
-vpColor const __declspec(selectany) vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
+vpColor const __declspec(selectany) vpColor::black = vpColor(0, 0, 0, id_black);
+/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white.
+ */
+vpColor const __declspec(selectany) vpColor::white = vpColor(255, 255, 255, id_white);
+/*!< Predefined light gray color with R=G=B=64 and identifier
+ * vpColor::id_lightGray. */
+vpColor const __declspec(selectany) vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
/*!< Predefined gray color with R=G=B=128 and identifier vpColor::id_gray. */
-vpColor const __declspec(selectany) vpColor::gray = vpColor(128, 128, 128, id_gray);
-/*!< Predefined dark gray color with R=G=B=192 and identifier vpColor::id_darkGray. */
-vpColor const __declspec(selectany) vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
+vpColor const __declspec(selectany) vpColor::gray = vpColor(128, 128, 128, id_gray);
+/*!< Predefined dark gray color with R=G=B=192 and identifier
+ * vpColor::id_darkGray. */
+vpColor const __declspec(selectany) vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
/*!< Predefined light red color with R= 255 and G=B=140 and identifier
vpColor::id_lightRed. */
-vpColor const __declspec(selectany) vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
+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);
+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);
/*!< 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);
@@ -294,25 +309,24 @@ 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 61beef7..20306d0 100644
--- a/modules/core/include/visp3/core/vpConvert.h
+++ b/modules/core/include/visp3/core/vpConvert.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef __vpConvert_h__
#define __vpConvert_h__
@@ -48,8 +48,8 @@
#include <visp3/core/vpConfig.h>
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- #include <opencv2/core/core.hpp>
- #include <opencv2/features2d/features2d.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
#endif
#include <visp3/core/vpImagePoint.h>
@@ -68,25 +68,29 @@ public:
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<cv::KeyPoint> &from, std::vector<vpImagePoint> &to);
static void convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to);
static void convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to);
- static void convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
- static void convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame=false);
+ static void convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to,
+ const bool cameraFrame = false);
+ static void convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to,
+ const bool cameraFrame = false);
static void convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to);
static void convertToOpenCV(const vpImagePoint &from, cv::Point2f &to);
static void convertToOpenCV(const vpImagePoint &from, cv::Point2d &to);
- static void convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame=false);
- static void convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame=false);
+ static void convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame = false);
+ static void convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame = false);
static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to);
static void convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to);
- static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame=false);
- static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame=false);
+ static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to,
+ const bool cameraFrame = false);
+ static void convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to,
+ const bool cameraFrame = false);
private:
static vpImagePoint keyPointToVpImagePoint(const cv::KeyPoint &keypoint);
@@ -106,7 +110,6 @@ private:
static cv::Point3d vpObjectPointToPoint3d(const vpPoint &point);
#endif
-
};
#endif
diff --git a/modules/core/include/visp3/core/vpCylinder.h b/modules/core/include/visp3/core/vpCylinder.h
index 7731d60..ef700bd 100644
--- a/modules/core/include/visp3/core/vpCylinder.h
+++ b/modules/core/include/visp3/core/vpCylinder.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpCylinder.h
\brief class that defines what is a cylinder
@@ -44,8 +44,8 @@
#ifndef vpCylinder_hh
#define vpCylinder_hh
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <math.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpForwardProjection.h>
@@ -56,8 +56,8 @@
\brief Class that defines what is a cylinder.
A cylinder may be represented by the equation:
- \f$ (X - X_0)^2 + (Y - Y_0)^2 + (Z - Z_0)^2 - (A \; X + B \; Y + C \; Z)^2 - R^2 = 0 \f$
- with
+ \f$ (X - X_0)^2 + (Y - Y_0)^2 + (Z - Z_0)^2 - (A \; X + B \; Y + C \; Z)^2 -
+ R^2 = 0 \f$ with
\f$
\left\{ \begin{array}{l}
@@ -66,16 +66,18 @@
\end{array} \right.
\f$
- where \f$R\f$ is the radius of the cylinder, \f$A, B, C\f$ are the coordinates of
- its direction vector and \f$X_0, Y_0, Z_0\f$ are the coordinates
- of the nearest point belonging to the cylinder axis from the projection center.
+ where \f$R\f$ is the radius of the cylinder, \f$A, B, C\f$ are the
+ coordinates of its direction vector and \f$X_0, Y_0, Z_0\f$ are the
+ coordinates of the nearest point belonging to the cylinder axis from the
+ projection center.
- Setting the cylinder parameters is achieved throw the constructors with parameters
- or the setWorldCoordinates() methods.
+ Setting the cylinder parameters is achieved throw the constructors with
+ parameters or the setWorldCoordinates() methods.
- Considering the set of parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame, cylinder coordinates expressed in the camera
- frame are obtained using changeFrame().
+ Considering the set of parameters \f$^{o}{\bf P} =
+ ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$ expressed in the world
+ frame, cylinder coordinates expressed in the camera frame are obtained using
+ changeFrame().
The projection of a cylinder on the image plane is (for
non-degenerated cases) a set of two straight lines with equation:
@@ -87,111 +89,99 @@
\end{array} \right.
\f$
- The projection is achieved using projection() methods. The methods getRho1(), getTheta1() and
- getRho2(), getTheta2() allow to access to the projected line parameters.
+ The projection is achieved using projection() methods. The methods
+ getRho1(), getTheta1() and getRho2(), getTheta2() allow to access to the
+ projected line parameters.
*/
class VISP_EXPORT vpCylinder : public vpForwardProjection
{
public:
- typedef enum
- {
+ typedef enum {
line1, /*!< First limb of the cylinder. */
line2 /*!< Second limb of the cylinder. */
} vpLineCylinderType;
- vpCylinder() ;
- vpCylinder(const vpColVector& oP) ;
- vpCylinder(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R) ;
- virtual ~vpCylinder() ;
+ 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);
+ virtual ~vpCylinder();
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP);
+ void changeFrame(const vpHomogeneousMatrix &cMo);
double computeZ(const double x, const double y) const;
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
+ void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green,
+ const unsigned int thickness = 1);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
- vpCylinder *duplicate() const ;
+ vpCylinder *duplicate() const;
/*!
Return the \f$\rho_1\f$ parameter of the line corresponding to the
projection of the cylinder in the image plane.
\sa getTheta1()
*/
- double getRho1() const { return p[0] ; }
+ double getRho1() const { return p[0]; }
/*!
Return the \f$\theta_1\f$ parameter of the line corresponding to the
projection of the cylinder in the image plane.
\sa getRho1()
*/
- double getTheta1() const { return p[1] ; }
+ double getTheta1() const { return p[1]; }
/*!
Return the \f$\rho_2\f$ parameter of the line corresponding to the
projection of the cylinder in the image plane.
\sa getTheta2()
*/
- double getRho2() const { return p[2] ; }
+ double getRho2() const { return p[2]; }
/*!
Return the \f$\theta_2\f$ parameter of the line corresponding to the
projection of the cylinder in the image plane.
\sa getRho2()
*/
- double getTheta2() const { return p[3] ; }
+ double getTheta2() const { return p[3]; }
/*!
Return cylinder \f$A\f$ parameter expressed in the camera frame.
*/
- double getA() const { return cP[0] ; }
+ double getA() const { return cP[0]; }
/*!
Return cylinder \f$B\f$ parameter expressed in the camera frame.
*/
- double getB() const { return cP[1] ; }
+ double getB() const { return cP[1]; }
/*!
Return cylinder \f$C\f$ parameter expressed in the camera frame.
*/
- double getC() const { return cP[2] ; }
+ double getC() const { return cP[2]; }
/*!
Return cylinder \f$X_0\f$ parameter expressed in the camera frame.
*/
- double getX() const { return cP[3] ; }
+ double getX() const { return cP[3]; }
/*!
Return cylinder \f$Y_0\f$ parameter expressed in the camera frame.
*/
- double getY() const { return cP[4] ; }
+ double getY() const { return cP[4]; }
/*!
Return cylinder \f$Z_0\f$ parameter expressed in the camera frame.
*/
- double getZ() const { return cP[5] ; }
+ double getZ() const { return cP[5]; }
/*!
Return cylinder \f$R\f$ parameter corresponding to the cylinder radius.
*/
- double getR() const { return cP[6] ; }
-
- void init() ;
+ double getR() const { return cP[6]; }
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
+ void init();
- 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 projection();
+ void projection(const vpColVector &cP, vpColVector &p);
+ void setWorldCoordinates(const vpColVector &oP);
+ void setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
+ const double Z0, const double R);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpDebug.h b/modules/core/include/visp3/core/vpDebug.h
index 1b3d794..51aac47 100644
--- a/modules/core/include/visp3/core/vpDebug.h
+++ b/modules/core/include/visp3/core/vpDebug.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -30,14 +31,17 @@
* Description:
* Debug and trace macro.
*
- * - TRACING: vpTRACE and vpERROR_TRACE work like printf with carreer return at the end of the string.
- * vpCERROR et vpCTRACE work like the C++ output streams std::cout and std::cerr.
- * - DEBUGING: vpDEBUG_TRACE(niv) and vpDERROR_TRACE(niv), work like printf, but print only if the
- * tracing level niv is greater than the debug level VP_DEBUG_MODE.
- * vpCDEBUG(niv) work like the C++ output stream std::cout.
- * vpDEBUG_ENABLE(niv) is equal to 1 if the debug level niv is greater than the debug mode
+ * - TRACING: vpTRACE and vpERROR_TRACE work like printf with carreer
+ *return at the end of the string. vpCERROR et vpCTRACE work like the C++
+ *output streams std::cout and std::cerr.
+ * - DEBUGING: vpDEBUG_TRACE(niv) and vpDERROR_TRACE(niv), work like
+ *printf, but print only if the tracing level niv is greater than the debug
+ *level VP_DEBUG_MODE. vpCDEBUG(niv) work like the C++ output
+ *stream std::cout. vpDEBUG_ENABLE(niv) is equal to 1 if the
+ *debug level niv is greater than the debug mode
* VP_DEBUG_MODE, 0 else.
- * - PROG DEFENSIVE: DEFENSIF(a) is equal to a if defensive mode is active, 0 else.
+ * - PROG DEFENSIVE: DEFENSIF(a) is equal to a if defensive mode is active,
+ *0 else.
*
* Authors:
* Nicolas Mansard, Bruno Renier
@@ -47,23 +51,21 @@
#ifndef __VP_DEBUG_HH
#define __VP_DEBUG_HH
-#include <visp3/core/vpConfig.h>
-#include <stdio.h>
-#include <stdarg.h>
#include <iostream>
-
+#include <stdarg.h>
+#include <stdio.h>
+#include <visp3/core/vpConfig.h>
#if defined(_WIN32)
-# ifndef __FUNCTION__
-# define __FUNCTION__ " "
-# endif
+#ifndef __FUNCTION__
+#define __FUNCTION__ " "
+#endif
#endif
#ifndef VP_DEBUG_MODE
-# define VP_DEBUG_MODE 0
+#define VP_DEBUG_MODE 0
#endif
-
/*!
\class vpTraceOutput
@@ -134,126 +136,123 @@ int main()
*/
class vpTraceOutput
{
- private:
- const char* currentFile; //Name of the file to use in the displays
- const char* currentFunc; //Name of the function to use in the displays
- int currentLine; //Line to use in the displays
-
- //if true, output to std::cerr/stderr else std::cout/stdout
- bool err;
- //string to display before anything else
- const char* header;
-
- public:
- /*!
- Constructor.
- \param file Should be the name of the file where this constructor was called.
- \param line Should be the line in file where this constructor was called.
- \param func Should be the name of the function where this constructor was called.
- \param error If true, writes to the error stream.
- \param s String to print before any other message (acts like a header).
- \note Call the constructor with something like vpTraceOutput(__FILE__,__LINE__, __FUNCTION__).
- */
- vpTraceOutput(const char* file, int line, const char* func, bool error=false, const char * s=NULL) :
- currentFile(file),
- currentFunc(func),
- currentLine(line),
- err(error),
- header(s)
- {}
-
- /*!
- Displays a string if the debug level is inferior to VP_DEBUG_MODE.
- \param level Level of this message.
- \param format Formating string.
- */
- void operator()(int level, const char* format, ...)
- {
- //if the level is inferior to VP_DEBUG_MODE
- if(VP_DEBUG_MODE >= level)
- {
- //gets the variable list of arguments
- va_list args;
- va_start(args, format);
-
- if (err)
- std::cerr << "(L" << level << ") " ;
- else
- std::cout << "(L" << level << ") " ;
-
- //calls display with it
- display(format, args);
-
- va_end(args);
- }
- }
-
- /*!
- Displays a string.
- \param format Formating string.
- */
- void operator()(const char* format, ...)
- {
- //gets the variable list of arguments
+private:
+ const char *currentFile; // Name of the file to use in the displays
+ const char *currentFunc; // Name of the function to use in the displays
+ int currentLine; // Line to use in the displays
+
+ // if true, output to std::cerr/stderr else std::cout/stdout
+ bool err;
+ // string to display before anything else
+ const char *header;
+
+public:
+ /*!
+ Constructor.
+ \param file Should be the name of the file where this constructor was
+ called. \param line Should be the line in file where this constructor was
+ called. \param func Should be the name of the function where this
+ constructor was called. \param error If true, writes to the error stream.
+ \param s String to print before any other message (acts like a header).
+ \note Call the constructor with something like
+ vpTraceOutput(__FILE__,__LINE__, __FUNCTION__).
+ */
+ vpTraceOutput(const char *file, int line, const char *func, bool error = false, const char *s = NULL)
+ : currentFile(file), currentFunc(func), currentLine(line), err(error), header(s)
+ {
+ }
+
+ /*!
+ Displays a string if the debug level is inferior to VP_DEBUG_MODE.
+ \param level Level of this message.
+ \param format Formating string.
+ */
+ void operator()(int level, const char *format, ...)
+ {
+ // if the level is inferior to VP_DEBUG_MODE
+ if (VP_DEBUG_MODE >= level) {
+ // gets the variable list of arguments
va_list args;
va_start(args, format);
-#ifdef VP_DEBUG
- std::cout<<"(L0) ";
-#endif
+ if (err)
+ std::cerr << "(L" << level << ") ";
+ else
+ std::cout << "(L" << level << ") ";
- //calls display with it
+ // calls display with it
display(format, args);
va_end(args);
}
+ }
+
+ /*!
+ Displays a string.
+ \param format Formating string.
+ */
+ void operator()(const char *format, ...)
+ {
+ // gets the variable list of arguments
+ va_list args;
+ va_start(args, format);
- /*!
-
- Displays a message to either stdout or
- stderr (based on error boolean).
-
- \param format Formating string.
- \param args List of arguments.
-
- */
- void display(const char* format, va_list args)
- {
- //if we want to write to std::cerr/stderr
- if(err)
- {
- //first writes the header if there is one
- if(header != NULL) std::cerr<<header;
- //then writes the recorded namefile, function and line
- std::cerr << "!!\t" << currentFile << ": " <<currentFunc << "(#" << currentLine << ") : " ;
- //and finally writes the message passed to () operator.
- vfprintf (stderr, format, args);
- fprintf (stderr, "\n");
- //flushes the buffer
- fflush (stderr);
- }
- else
- {
- //first writes the header if there is one
- if(header != NULL) std::cout<<header;
- //then writes the recorded namefile, function and line
- std::cout <<currentFile << ": " << currentFunc << "(#" << currentLine << ") : " ;
- //and finally writes the message passed to () operator.
- vprintf (format, args);
- printf ("\n");
- //flushes the buffer
- fflush (stdout);
- }
- }
+#ifdef VP_DEBUG
+ std::cout << "(L0) ";
+#endif
+ // calls display with it
+ display(format, args);
+
+ va_end(args);
+ }
+
+ /*!
+
+ Displays a message to either stdout or
+ stderr (based on error boolean).
+
+ \param format Formating string.
+ \param args List of arguments.
+
+ */
+ void display(const char *format, va_list args)
+ {
+ // if we want to write to std::cerr/stderr
+ if (err) {
+ // first writes the header if there is one
+ if (header != NULL)
+ std::cerr << header;
+ // then writes the recorded namefile, function and line
+ std::cerr << "!!\t" << currentFile << ": " << currentFunc << "(#" << currentLine << ") : ";
+ // and finally writes the message passed to () operator.
+ vfprintf(stderr, format, args);
+ fprintf(stderr, "\n");
+ // flushes the buffer
+ fflush(stderr);
+ } else {
+ // first writes the header if there is one
+ if (header != NULL)
+ std::cout << header;
+ // then writes the recorded namefile, function and line
+ std::cout << currentFile << ": " << currentFunc << "(#" << currentLine << ") : ";
+ // and finally writes the message passed to () operator.
+ vprintf(format, args);
+ printf("\n");
+ // flushes the buffer
+ fflush(stdout);
+ }
+ }
};
+/* -------------------------------------------------------------------------
+ */
+/* --- vpTRACE IN/OUT FONCTION ---------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
-/* ------------------------------------------------------------------------- */
-/* --- vpTRACE IN/OUT FONCTION --------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-#ifdef VP_TRACE // Activate the trace mode
+#ifdef VP_TRACE // Activate the trace mode
/*!
\ingroup group_core_debug
@@ -271,10 +270,9 @@ int main()
}
\endcode
- \sa vpOUT_FCT
+ \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
@@ -292,20 +290,23 @@ int main()
}
\endcode
- \sa vpIN_FCT
+ \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
-inline void vpIN_FCT (const char * /* a */, ...){}
-inline void vpOUT_FCT (const char * /* a */, ...){}
+inline void vpIN_FCT(const char * /* a */, ...) {}
+inline void vpOUT_FCT(const char * /* a */, ...) {}
#endif // #ifdef VP_TRACE
-/* -------------------------------------------------------------------------- */
-/* --- vpTRACE -------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- vpTRACE --------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
#ifdef VP_TRACE
@@ -336,7 +337,6 @@ int main()
*/
#define vpCTRACE std::cout << "(L0) " << __FILE__ << ": " << __FUNCTION__ << "(#" << __LINE__ << ") : "
-
/*!
\ingroup group_core_debug
Used to display error messages on the error stream (C++).
@@ -362,7 +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
@@ -388,7 +390,7 @@ int main()
\sa vpTRACE()
*/
-#define vpERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
+#define vpERROR_TRACE (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, true))
/*!
\ingroup group_core_debug
@@ -411,23 +413,30 @@ int main()
\sa vpCTRACE(), vpERROR_TRACE()
*/
-#define vpTRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
+#define vpTRACE (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false))
#else // #ifdef VP_TRACE
-#define vpCTRACE if(false) std::cout // Warning C4127
-#define vpCERROR if(false) std::cerr // Warning C4127
+#define vpCTRACE \
+ if (false) \
+ std::cout // Warning C4127
+#define vpCERROR \
+ if (false) \
+ std::cerr // Warning C4127
-inline void vpERROR_TRACE (const char * /* a */, ...){}
-inline void vpERROR_TRACE (int /* level */, const char * /* a */, ...){}
-inline void vpTRACE (const char * /* a */, ...){}
-inline void vpTRACE (int /* level */, const char * /* a */, ...){}
+inline void vpERROR_TRACE(const char * /* a */, ...) {}
+inline void vpERROR_TRACE(int /* level */, const char * /* a */, ...) {}
+inline void vpTRACE(const char * /* a */, ...) {}
+inline void vpTRACE(int /* level */, const char * /* a */, ...) {}
#endif // #ifdef VP_TRACE
-/* ------------------------------------------------------------------------- */
-/* --- VP_DEBUG ------------------------------------------------------------ */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- VP_DEBUG ------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
#ifdef VP_DEBUG
@@ -452,7 +461,7 @@ int main()
\sa vpDEBUG_TRACE()
*/
-#define vpDERROR_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, true))
+#define vpDERROR_TRACE (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, true))
/*!
\ingroup group_core_debug
@@ -475,7 +484,7 @@ int main()
\sa vpDERROR_TRACE()
*/
-#define vpDEBUG_TRACE (vpTraceOutput( __FILE__,__LINE__, __FUNCTION__, false))
+#define vpDEBUG_TRACE (vpTraceOutput(__FILE__, __LINE__, __FUNCTION__, false))
/*!
\ingroup group_core_debug
@@ -499,14 +508,17 @@ 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
- vpDEBUG_ENABLE(level) is equal to 1 if the debug level \e level is greater than
- the debug mode VP_DEBUG_MODE, 0 else.
+ vpDEBUG_ENABLE(level) is equal to 1 if the debug level \e level is greater
+than the debug mode VP_DEBUG_MODE, 0 else.
\code
#define VP_DEBUG // Activate the debug mode
@@ -520,31 +532,35 @@ int main()
std::cout << "Debug level 1 active: " << vpDEBUG_ENABLE(1) << std::endl;
std::cout << "Debug level 2 active: " << vpDEBUG_ENABLE(2) << std::endl;
std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
-}
+}
\endcode
*/
#define vpDEBUG_ENABLE(level) (VP_DEBUG_MODE >= level)
#else // #ifdef VP_DEBUG
-inline void vpDERROR_TRACE(const char * /* a */, ...){}
-inline void vpDEBUG_TRACE(const char * /* a */, ...){}
-inline void vpDERROR_TRACE(int /* level */, const char * /* a */, ...){}
-inline void vpDEBUG_TRACE(int /* level */, const char * /* a */, ...){}
+inline void vpDERROR_TRACE(const char * /* a */, ...) {}
+inline void vpDEBUG_TRACE(const char * /* a */, ...) {}
+inline void vpDERROR_TRACE(int /* level */, const char * /* a */, ...) {}
+inline void vpDEBUG_TRACE(int /* level */, const char * /* a */, ...) {}
-#define vpCDEBUG(level) if(false) std::cout // Warning C4127
-#define vpDEBUG_ENABLE(level) (false) // Warning C4127
+#define vpCDEBUG(level) \
+ if (false) \
+ std::cout // Warning C4127
+#define vpDEBUG_ENABLE(level) (false) // Warning C4127
#endif // #ifdef VP_DEBUG
-/* -------------------------------------------------------------------------- */
-/* --- DEFENSIF ------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- DEFENSIF -------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
#ifdef VP_DEFENSIF
-#define DEFENSIF(a) (a)
+#define DEFENSIF(a) (a)
#else
-#define DEFENSIF(a) (0)
-#endif /*#ifdef DEFENSIF*/
-
+#define DEFENSIF(a) (0)
+#endif /*#ifdef DEFENSIF*/
#endif /* #ifdef __DEBUG_HH */
diff --git a/modules/core/include/visp3/core/vpDisplay.h b/modules/core/include/visp3/core/vpDisplay.h
index 6818ebe..c01b46f 100644
--- a/modules/core/include/visp3/core/vpDisplay.h
+++ b/modules/core/include/visp3/core/vpDisplay.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,20 +37,19 @@
*
*****************************************************************************/
-
#ifndef vpDisplay_h
#define vpDisplay_h
-#include <string>
#include <sstream>
+#include <string>
-#include <visp3/core/vpImage.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColor.h>
-#include <visp3/core/vpMouseButton.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMouseButton.h>
+#include <visp3/core/vpRect.h>
/*!
\file vpDisplay.h
@@ -72,12 +72,12 @@
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -132,27 +132,29 @@ 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);
// Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
+ std::cout << "Check keyboard events..." << std::endl;
char key[10]; sprintf(key, "\0");
bool ret;
for (int i=0; i< 200; i++) {
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
- std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
+ if (ret)
+ std::cout << "keyboard event: key: " << "\"" << key
+ << "\"" << std::endl;
vpTime::wait(40);
}
// Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
+ std::cout << "Wait for a keyboard event..." << std::endl;
ret = vpDisplay::getKeyboardEvent(I, key, true);
std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
+ if (ret)
std::cout << "key: " << "\"" << key << "\"" << std::endl;
// Wait for a click in the display window
@@ -169,31 +171,43 @@ int main()
class VISP_EXPORT vpDisplay
{
public:
- //! Values that could be applied to a display to down scale the size of the display.
+ //! 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_DEFAULT /*!< Display and image have the same size. Similar to vpDisplay::SCALE_1. */
+ 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;
-protected :
+protected:
//! display has been initialized
- bool m_displayHasBeenInitialized ;
+ bool m_displayHasBeenInitialized;
//! display position
- int m_windowXPosition ;
+ int m_windowXPosition;
//! display position
- int m_windowYPosition ;
- unsigned int m_width ;
- unsigned int m_height ;
+ int m_windowYPosition;
+ unsigned int m_width;
+ unsigned int m_height;
std::string m_title;
unsigned int m_scale;
vpScaleType m_scaleType;
@@ -201,38 +215,39 @@ protected :
void setScale(vpScaleType scaleType, unsigned int width, unsigned int height);
public:
- vpDisplay() ;
- vpDisplay(const vpDisplay& d) ;
+ vpDisplay();
+ vpDisplay(const vpDisplay &d);
virtual ~vpDisplay();
/** @name Inherited functionalities from vpDisplay */
//@{
unsigned int computeAutoScale(unsigned int width, unsigned int height);
/*!
- Return the value of the down scale factor applied to the image in order to reduce the size of the window used to display the image.
+ Return the value of the down scale factor applied to the image in order to
+ reduce the size of the window used to display the image.
*/
- unsigned int getDownScalingFactor() { return m_scale; };
+ unsigned int getDownScalingFactor() { return m_scale; }
/*!
Return the display height.
\sa getWidth()
*/
- inline unsigned int getHeight() const { return m_height ; }
+ inline unsigned int getHeight() const { return m_height; }
/*!
Return the display width.
\sa getHeight()
*/
- inline unsigned int getWidth() const { return m_width ; }
+ inline unsigned int getWidth() const { return m_width; }
/*!
- Return the position (along the horizontal axis) on the screen of the display window.
- \sa getWindowYPosition()
+ Return the position (along the horizontal axis) on the screen of the
+ display window. \sa getWindowYPosition()
*/
- int getWindowXPosition() const { return m_windowXPosition; };
+ int getWindowXPosition() const { return m_windowXPosition; }
/*!
- Return the position (along the vertical axis) on the screen of the display window.
- \sa getWindowXPosition()
+ Return the position (along the vertical axis) on the screen of the display
+ window. \sa getWindowXPosition()
*/
- int getWindowYPosition() const { return m_windowYPosition; };
+ int getWindowYPosition() const { return m_windowYPosition; }
/*!
Check if the display has been initialised
@@ -241,8 +256,8 @@ public:
*/
inline bool isInitialised() { return m_displayHasBeenInitialized; }
virtual void setDownScalingFactor(unsigned int scale);
- void setDownScalingFactor(vpScaleType scaleType);
- //@}
+ virtual void setDownScalingFactor(vpScaleType scaleType);
+//@}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/** @name vpDisplay pure virtual functions */
@@ -251,11 +266,11 @@ public:
Set the window backgroud to \e color.
\param color : Background color.
*/
- virtual void clearDisplay(const vpColor &color=vpColor::white) =0 ;
+ virtual void clearDisplay(const vpColor &color = vpColor::white) = 0;
/*!
Close the window.
*/
- virtual void closeDisplay() =0;
+ virtual void closeDisplay() = 0;
/*!
Display an arrow from image point \e ip1 to image point \e ip2.
@@ -266,7 +281,8 @@ public:
\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.
@@ -278,7 +294,7 @@ public:
\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.
@@ -288,7 +304,8 @@ public:
\param thickness : Thickness of the circle. This parameter is only useful
when \e fill is set to false.
*/
- virtual void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1) =0;
+ virtual void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1) = 0;
/*!
Display a cross at the image point \e ip location.
\param ip : Cross location.
@@ -296,7 +313,8 @@ public:
\param color : Cross color.
\param thickness : Thickness of the lines used to display the cross.
*/
- virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1) =0;
+ virtual void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness = 1) = 0;
/*!
Display a dashed line from image point \e ip1 to image point \e ip2.
\param ip1 : Initial image point.
@@ -304,7 +322,8 @@ public:
\param color : Line color.
\param thickness : Dashed line thickness.
*/
- virtual void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) =0;
+ virtual void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness = 1) = 0;
/*!
Display a line from image point \e ip1 to image point \e ip2.
\param ip1 : Initial image point.
@@ -312,7 +331,8 @@ public:
\param color : Line color.
\param thickness : Line thickness.
*/
- virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) =0;
+ virtual void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness = 1) = 0;
/*!
Display the gray level image \e I (8bits).
@@ -325,7 +345,7 @@ public:
\sa init(), closeDisplay()
*/
- virtual void displayImage(const vpImage<unsigned char> &I) =0 ;
+ virtual void displayImage(const vpImage<unsigned char> &I) = 0;
/*!
Display the color image \e I in RGBa format (32bits).
@@ -338,10 +358,12 @@ public:
\sa init(), closeDisplay()
*/
- virtual void displayImage(const vpImage<vpRGBa> &I) =0 ;
+ virtual void displayImage(const vpImage<vpRGBa> &I) = 0;
- virtual void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
- virtual void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0 ;
+ virtual void displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height) = 0;
+ virtual void displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height) = 0;
/*!
Display a point at the image point \e ip location.
@@ -349,7 +371,7 @@ public:
\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
@@ -366,7 +388,7 @@ public:
false.
*/
virtual void displayRectangle(const vpImagePoint &topLeft, unsigned int width, unsigned int height,
- const vpColor &color, bool fill = false, unsigned int thickness=1)=0 ;
+ 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.
@@ -380,8 +402,8 @@ public:
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.
@@ -395,20 +417,20 @@ public:
false.
*/
- virtual void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness=1)=0 ;
+ virtual void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1) = 0;
/*!
Flushes the display.
- It's necessary to use this function to see the results of any drawing.
- */
- virtual void flushDisplay() =0;
-
+ It's necessary to use this function to see the results of any drawing.
+ */
+ virtual void flushDisplay() = 0;
+
/*!
Flushes the display.
- It's necessary to use this function to see the results of any drawing.
- */
- virtual void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) =0;
-
+ It's necessary to use this function to see the results of any drawing.
+ */
+ virtual void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) = 0;
/* Simple interface with the mouse event */
@@ -421,13 +443,13 @@ public:
- When set to false, returns true only if a mouse button is
pressed, otherwise returns false.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
- virtual bool getClick(bool blocking=true) =0;
+ virtual bool getClick(bool blocking = true) = 0;
/*!
Wait for a click from one of the mouse button and get the position
@@ -438,22 +460,22 @@ public:
\param blocking [in] : true for a blocking behaviour waiting a mouse
button click, false for a non blocking behaviour.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
- virtual bool getClick(vpImagePoint &ip, bool blocking=true) =0;
+ virtual bool getClick(vpImagePoint &ip, bool blocking = true) = 0;
/*!
Wait for a mouse button click and get the position of the clicked
pixel. The button used to click is also set.
-
+
\param ip [out] : The coordinates of the clicked image point.
\param button [out] : The button used to click.
- \param blocking [in] :
+ \param blocking [in] :
- When set to true, this method waits until a mouse button is
pressed and then returns always true.
- When set to false, returns true only if a mouse button is
@@ -463,7 +485,7 @@ public:
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
@@ -477,37 +499,37 @@ public:
\param blocking [in] : true for a blocking behaviour waiting a mouse
button click, false for a non blocking behaviour.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
\sa getClick(vpImagePoint &, vpMouseButton::vpMouseButtonType &, bool)
- */
- virtual bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true) =0;
+ */
+ virtual bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true) = 0;
/*!
Get a keyboard event.
-
+
\param blocking [in] : Blocking behavior.
- When set to true, this method waits until a key is
pressed and then returns always true.
- When set to false, returns true only if a key is
pressed, otherwise returns false.
- \return
- - true if a key was pressed. This is always the case if blocking is set
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
to \e true.
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
- virtual bool getKeyboardEvent(bool blocking=true) =0;
+ virtual bool getKeyboardEvent(bool blocking = true) = 0;
/*!
-
+
Get a keyboard event.
-
+
\param blocking [in] : Blocking behavior.
- When set to true, this method waits until a key is
pressed and then returns always true.
@@ -517,50 +539,50 @@ public:
\param key [out]: If possible, an ISO Latin-1 character
corresponding to the keyboard key.
- \return
- - true if a key was pressed. This is always the case if blocking is set
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
to \e true.
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
- virtual bool getKeyboardEvent(std::string &key, bool blocking=true) =0;
+ virtual bool getKeyboardEvent(std::string &key, bool blocking = true) = 0;
/*!
Get the coordinates of the mouse pointer.
-
+
\param ip [out] : The coordinates of the mouse pointer.
-
+
\return true if a pointer motion event was received, false otherwise.
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
- virtual bool getPointerMotionEvent (vpImagePoint &ip) =0;
-
+ virtual bool getPointerMotionEvent(vpImagePoint &ip) = 0;
+
/*!
Get the coordinates of the mouse pointer.
-
+
\param ip [out] : The coordinates of the mouse pointer.
-
+
\return true.
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
- virtual bool getPointerPosition (vpImagePoint &ip) =0;
+ virtual bool getPointerPosition(vpImagePoint &ip) = 0;
/*!
Gets the screen vertical resolution in pixel.
*/
- virtual unsigned int getScreenHeight() =0;
+ virtual unsigned int getScreenHeight() = 0;
/*!
Gets the screen resolution in pixel.
\param width, height : Screen resolution in pixels.
*/
- virtual void getScreenSize(unsigned int &width, unsigned int &height) =0;
+ virtual void getScreenSize(unsigned int &width, unsigned int &height) = 0;
/*!
Gets the screen horizontal resolution in pixel.
*/
- virtual unsigned int getScreenWidth() =0;
+ virtual unsigned int getScreenWidth() = 0;
/*!
Initialize the display (size, position and title) of a gray level image.
@@ -570,7 +592,7 @@ public:
\param y : Vertical position of the upper/left window corner.
\param title : Window title.
*/
- virtual void init(vpImage<unsigned char> &I, int x=-1, int y=-1, const std::string &title="") =0 ;
+ virtual void init(vpImage<unsigned char> &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.
@@ -580,7 +602,7 @@ public:
\param y : Vertical position of the upper/left window corner.
\param title : Window title.
*/
- virtual void init(vpImage<vpRGBa> &I, int x=-1, int y=-1, const std::string &title="") =0 ;
+ virtual void init(vpImage<vpRGBa> &I, int x = -1, int y = -1, const std::string &title = "") = 0;
/*!
Initialize the display size, position and title.
@@ -593,11 +615,11 @@ public:
The following example shows how to use this function
\code
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/io/vpImageIo.h>
int main()
@@ -642,7 +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
@@ -657,12 +679,12 @@ int main()
\sa displayCharString()
*/
- virtual void setFont(const std::string &font) =0;
+ virtual void setFont(const std::string &font) = 0;
/*!
Set the window title.
\param title : Window title.
*/
- virtual void setTitle(const std::string &title) =0;
+ virtual void setTitle(const std::string &title) = 0;
/*!
Set the window position in the screen.
@@ -670,61 +692,102 @@ int main()
\param y : Vertical position of the upper/left window corner.
*/
- virtual void setWindowPosition(int x, int y) = 0 ;
- //@}
+ virtual void setWindowPosition(int x, int y) = 0;
+//@}
#endif // ifndef DOXYGEN_SHOULD_SKIP_THIS
-
/*!
@name Static public vpDisplay functionalities on gray level images.
*/
//@{
- static void close(vpImage<unsigned char> &I) ;
- static void display(const vpImage<unsigned char> &I) ;
- static void displayArrow(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1) ;
- static void displayArrow(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1) ;
- static void displayCamera(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness);
- static void displayCharString(const vpImage<unsigned char> &I, const vpImagePoint &ip, const char *string, const vpColor &color) ;
- static void displayCharString(const vpImage<unsigned char> &I, int i, int j, const char *string, const vpColor &color) ;
- static void displayCircle(const vpImage<unsigned char> &I, const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayCircle(const vpImage<unsigned char> &I, int i, int j, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayCross(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- static void displayCross(const vpImage<unsigned char> &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness=1) ;
- static void displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1);
- static void displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, const double &theta1, const double &theta2, bool use_centered_moments, const vpColor &color, unsigned int thickness=1);
- static void displayFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0,0)) ;
- static void displayLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
- static void displayLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness=1) ;
- static void displayPoint(const vpImage<unsigned char> &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1) ;
- static void displayPoint(const vpImage<unsigned char> &I, int i, int j, const vpColor &color, unsigned int thickness=1) ;
- static void displayPolygon(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip, const vpColor &color, unsigned int thickness=1) ;
- static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I, const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint ¢er, float angle, unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I, int i, int j, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<unsigned char> &I, unsigned int i, unsigned int j, float angle, unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness=1);
- static void displayROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
- static void displayText(const vpImage<unsigned char> &I, const vpImagePoint &ip, const std::string &s, const vpColor &color) ;
- static void displayText(const vpImage<unsigned char> &I, int i, int j, const std::string &s, const vpColor &color) ;
-
- static void flush(const vpImage<unsigned char> &I) ;
- static void flushROI(const vpImage<unsigned char> &I,const vpRect &roi) ;
-
- static bool getClick(const vpImage<unsigned char> &I, bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClick(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClickUp(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClickUp(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
+ static void close(vpImage<unsigned char> &I);
+ static void display(const vpImage<unsigned char> &I);
+ static void displayArrow(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2,
+ unsigned int thickness = 1);
+ static void displayArrow(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2,
+ const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2,
+ unsigned int thickness = 1);
+ static void displayCamera(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness);
+ static void displayCharString(const vpImage<unsigned char> &I, const vpImagePoint &ip, const char *string,
+ const vpColor &color);
+ static void displayCharString(const vpImage<unsigned char> &I, int i, int j, const char *string,
+ const vpColor &color);
+ static void displayCircle(const vpImage<unsigned char> &I, const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill = false, unsigned int thickness = 1);
+ static void displayCircle(const vpImage<unsigned char> &I, int i, int j, unsigned int radius, const vpColor &color,
+ bool fill = false, unsigned int thickness = 1);
+ static void displayCross(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayCross(const vpImage<unsigned char> &I, int i, int j, unsigned int size, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &ips,
+ const bool closeTheShape, const vpColor &color, unsigned int thickness = 1);
+ static void displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, const double &theta1, const double &theta2,
+ bool use_centered_moments, const vpColor &color, unsigned int thickness = 1);
+ static void displayFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, double size, const vpColor &color = vpColor::none,
+ unsigned int thickness = 1, const vpImagePoint &offset = vpImagePoint(0, 0));
+ static void displayLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayLine(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &ips,
+ const bool closeTheShape, const vpColor &color, unsigned int thickness = 1);
+ static void displayPoint(const vpImage<unsigned char> &I, const vpImagePoint &ip, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayPoint(const vpImage<unsigned char> &I, int i, int j, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayPolygon(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft, unsigned int width,
+ unsigned int height, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft,
+ const vpImagePoint &bottomRight, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, const vpRect &rectangle, const vpColor &color,
+ bool fill = false, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint ¢er, float angle,
+ unsigned int width, unsigned int height, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, int i, int j, unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<unsigned char> &I, unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayROI(const vpImage<unsigned char> &I, const vpRect &roi);
+ static void displayText(const vpImage<unsigned char> &I, const vpImagePoint &ip, const std::string &s,
+ const vpColor &color);
+ static void displayText(const vpImage<unsigned char> &I, int i, int j, const std::string &s, const vpColor &color);
+
+ static void flush(const vpImage<unsigned char> &I);
+ static void flushROI(const vpImage<unsigned char> &I, const vpRect &roi);
+
+ static bool getClick(const vpImage<unsigned char> &I, bool blocking = true);
+ static bool getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, bool blocking = true);
+ static bool getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking = true);
+ static bool getClick(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ static bool getClickUp(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking = true);
+ static bool getClickUp(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button,
+ bool blocking = true);
static unsigned int getDownScalingFactor(const vpImage<unsigned char> &I);
- static void getImage(const vpImage<unsigned char> &Is, vpImage<vpRGBa> &Id) ;
- static bool getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking=true);
- static bool getKeyboardEvent(const vpImage<unsigned char> &I, std::string &key, bool blocking=true);
- static bool getKeyboardEvent(const vpImage<unsigned char> &I, char *key, bool blocking=true);
- static bool getPointerMotionEvent (const vpImage<unsigned char> &I, vpImagePoint &ip);
+ static void getImage(const vpImage<unsigned char> &Is, vpImage<vpRGBa> &Id);
+ static bool getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking = true);
+ static bool getKeyboardEvent(const vpImage<unsigned char> &I, std::string &key, bool blocking = true);
+ static bool getKeyboardEvent(const vpImage<unsigned char> &I, char *key, bool blocking = true);
+ static bool getPointerMotionEvent(const vpImage<unsigned char> &I, vpImagePoint &ip);
static bool getPointerPosition(const vpImage<unsigned char> &I, vpImagePoint &ip);
static void setBackground(const vpImage<unsigned char> &I, const vpColor &color);
@@ -737,53 +800,89 @@ int main()
@name Static public vpDisplay functionalities on 32 bits color images.
*/
//@{
- static void close(vpImage<vpRGBa> &I) ;
- static void display(const vpImage<vpRGBa> &I) ;
- static void displayArrow(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1) ;
- static void displayArrow(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color=vpColor::white, unsigned int w=4, unsigned int h=2, unsigned int thickness=1) ;
- static void displayCamera(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness);
- static void displayCharString(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const char *string, const vpColor &color) ;
- static void displayCharString(const vpImage<vpRGBa> &I, int i, int j, const char *string, const vpColor &color) ;
- static void displayCircle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayCircle(const vpImage<vpRGBa> &I, int i, int j, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayCross(const vpImage<vpRGBa> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- static void displayCross(const vpImage<vpRGBa> &I, int i, int j, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
- static void displayDotLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness=1) ;
- static void displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color, unsigned int thickness=1);
- static void displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1, const double &coef2, const double &coef3, const double &theta1, const double &theta2, bool use_centered_moments, const vpColor &color, unsigned int thickness=1);
- static void displayFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, vpImagePoint offset=vpImagePoint(0,0)) ;
- static void displayLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
- static void displayLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color, unsigned int thickness=1) ;
- static void displayPoint(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const vpColor &color, unsigned int thickness=1) ;
- static void displayPoint(const vpImage<vpRGBa> &I, int i, int j, const vpColor &color, unsigned int thickness=1) ;
- static void displayPolygon(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip, const vpColor &color, unsigned int thickness=1) ;
- static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I, const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, float angle, unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I, int i, int j, unsigned int width, unsigned int height, const vpColor &color, bool fill = false, unsigned int thickness=1);
- static void displayRectangle(const vpImage<vpRGBa> &I, unsigned int i, unsigned int j, float angle, unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness=1);
- static void displayROI(const vpImage<vpRGBa> &I,const vpRect &roi) ;
- static void displayText(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const std::string &s, const vpColor &color) ;
- static void displayText(const vpImage<vpRGBa> &I, int i, int j, const std::string &s, const vpColor &color) ;
-
- static void flush(const vpImage<vpRGBa> &I) ;
- static void flushROI(const vpImage<vpRGBa> &I,const vpRect &roi) ;
-
- static bool getClick(const vpImage<vpRGBa> &I, bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClick(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClickUp(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
- static bool getClickUp(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking=true) ;
+ static void close(vpImage<vpRGBa> &I);
+ static void display(const vpImage<vpRGBa> &I);
+ static void displayArrow(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2,
+ unsigned int thickness = 1);
+ static void displayArrow(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2,
+ const vpColor &color = vpColor::white, unsigned int w = 4, unsigned int h = 2,
+ unsigned int thickness = 1);
+ static void displayCamera(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ double size, const vpColor &color, unsigned int thickness);
+ static void displayCharString(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const char *string,
+ const vpColor &color);
+ static void displayCharString(const vpImage<vpRGBa> &I, int i, int j, const char *string, const vpColor &color);
+ static void displayCircle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, unsigned int radius,
+ const vpColor &color, bool fill = false, unsigned int thickness = 1);
+ static void displayCircle(const vpImage<vpRGBa> &I, int i, int j, unsigned int radius, const vpColor &color,
+ bool fill = false, unsigned int thickness = 1);
+ static void displayCross(const vpImage<vpRGBa> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayCross(const vpImage<vpRGBa> &I, int i, int j, unsigned int size, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayDotLine(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &ips, const bool closeTheShape,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, bool use_centered_moments, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, const double &theta1, const double &theta2,
+ bool use_centered_moments, const vpColor &color, unsigned int thickness = 1);
+ static void displayFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ double size, const vpColor &color = vpColor::none, unsigned int thickness = 1,
+ const vpImagePoint &offset = vpImagePoint(0, 0));
+ static void displayLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayLine(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &ips, const bool closeTheShape,
+ const vpColor &color, unsigned int thickness = 1);
+ static void displayPoint(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayPoint(const vpImage<vpRGBa> &I, int i, int j, const vpColor &color, unsigned int thickness = 1);
+ static void displayPolygon(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLeft, unsigned int width,
+ unsigned int height, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLeft, const vpImagePoint &bottomRight,
+ const vpColor &color, bool fill = false, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, const vpRect &rectangle, const vpColor &color,
+ bool fill = false, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, float angle, unsigned int width,
+ unsigned int height, const vpColor &color, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, int i, int j, unsigned int width, unsigned int height,
+ const vpColor &color, bool fill = false, unsigned int thickness = 1);
+ static void displayRectangle(const vpImage<vpRGBa> &I, unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height, const vpColor &color,
+ unsigned int thickness = 1);
+ static void displayROI(const vpImage<vpRGBa> &I, const vpRect &roi);
+ static void displayText(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const std::string &s, const vpColor &color);
+ static void displayText(const vpImage<vpRGBa> &I, int i, int j, const std::string &s, const vpColor &color);
+
+ static void flush(const vpImage<vpRGBa> &I);
+ static void flushROI(const vpImage<vpRGBa> &I, const vpRect &roi);
+
+ static bool getClick(const vpImage<vpRGBa> &I, bool blocking = true);
+ static bool getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, bool blocking = true);
+ static bool getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking = true);
+ static bool getClick(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ static bool getClickUp(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking = true);
+ static bool getClickUp(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
static unsigned int getDownScalingFactor(const vpImage<vpRGBa> &I);
- static void getImage(const vpImage<vpRGBa> &Is, vpImage<vpRGBa> &Id) ;
- static bool getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking=true);
- static bool getKeyboardEvent(const vpImage<vpRGBa> &I, std::string &key, bool blocking=true);
- static bool getKeyboardEvent(const vpImage<vpRGBa> &I, char *key, bool blocking=true);
- static bool getPointerMotionEvent (const vpImage<vpRGBa> &I, vpImagePoint &ip);
- static bool getPointerPosition (const vpImage<vpRGBa> &I, vpImagePoint &ip);
+ static void getImage(const vpImage<vpRGBa> &Is, vpImage<vpRGBa> &Id);
+ static bool getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking = true);
+ static bool getKeyboardEvent(const vpImage<vpRGBa> &I, std::string &key, bool blocking = true);
+ static bool getKeyboardEvent(const vpImage<vpRGBa> &I, char *key, bool blocking = true);
+ static bool getPointerMotionEvent(const vpImage<vpRGBa> &I, vpImagePoint &ip);
+ static bool getPointerPosition(const vpImage<vpRGBa> &I, vpImagePoint &ip);
static void setBackground(const vpImage<vpRGBa> &I, const vpColor &color);
static void setFont(const vpImage<vpRGBa> &I, const std::string &font);
@@ -791,10 +890,9 @@ int main()
static void setWindowPosition(const vpImage<vpRGBa> &I, int winx, int winy);
//@}
- private:
+private:
//! get the window pixmap and put it in vpRGBa image
virtual void getImage(vpImage<vpRGBa> &I) = 0;
-
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpDisplayException.h b/modules/core/include/visp3/core/vpDisplayException.h
index fae5b5f..5f870f2 100644
--- a/modules/core/include/visp3/core/vpDisplayException.h
+++ b/modules/core/include/visp3/core/vpDisplayException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,30 +36,31 @@
*
*****************************************************************************/
-
-
#ifndef __vpDisplayException_H
#define __vpDisplayException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpDisplayException.h
\brief error that can be emited by the vpDisplay class and its derivates
*/
/* Classes standards. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpDisplayException
@@ -67,42 +69,34 @@
*/
class VISP_EXPORT vpDisplayException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpDisplay member
- */
- enum errorDisplayCodeEnum
- {
- notInitializedError,
- cannotOpenWindowError,
- connexionError,
- XWindowsError,
- GTKWindowsError,
- colorAllocError,
- depthNotSupportedError
- } ;
-
- public:
- vpDisplayException(const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpDisplay member
+ */
+ enum errorDisplayCodeEnum {
+ notInitializedError,
+ cannotOpenWindowError,
+ connexionError,
+ XWindowsError,
+ GTKWindowsError,
+ colorAllocError,
+ depthNotSupportedError
+ };
- vpDisplayException (const int id, const std::string & msg)
- : vpException(id, msg)
- {
- }
+public:
+ vpDisplayException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
- vpDisplayException (const int id)
- : vpException(id)
- {
- }
+ vpDisplayException(const int id, const std::string &msg) : vpException(id, msg) {}
+ explicit vpDisplayException(const int id) : vpException(id) {}
};
#endif
diff --git a/modules/core/include/visp3/core/vpException.h b/modules/core/include/visp3/core/vpException.h
index ee4e3ca..0153059 100644
--- a/modules/core/include/visp3/core/vpException.h
+++ b/modules/core/include/visp3/core/vpException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/* \file vpException.h
\brief error that can be emited by the vp class and its derivates
*/
@@ -43,7 +43,6 @@
#ifndef __vpException_H
#define __vpException_H
-
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -51,15 +50,14 @@
#include <visp3/core/vpConfig.h>
/* Classes standards. */
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
#include <stdarg.h>
+#include <string> /* Classe string. */
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-
/*!
\class vpException
\ingroup group_core_debug
@@ -72,64 +70,63 @@
*/
class VISP_EXPORT vpException : public std::exception
{
- protected:
-
- //! Contains the error code, see the errorCodeEnum table for details.
- int code;
-
- //! Contains an error message (can be empty)
- std::string message;
-
- //! Set the message container
- void setMessage(const char* format, va_list args);
-
- //! forbid the empty constructor (protected)
- vpException(): code(notInitialized), message("") { };
-
- public:
-
- enum generalExceptionEnum
- {
- memoryAllocationError,
- memoryFreeError,
- functionNotImplementedError,
- ioError,
- cannotUseConstructorError,
- notImplementedError,
- divideByZeroError,
- dimensionError,
- fatalError,
- badValue, /*!< Used to indicate that a value is not in the allowed range. */
- notInitialized /*!< Used to indicate that a parameter is not initialized. */
- } ;
-
- vpException (const int code, const char* format, va_list args);
- vpException (const int code, const char* format, ...);
- vpException (const int code, const std::string & msg);
- vpException (const int code);
-
- /*!
- Basic destructor. Do nothing but implemented to fit the inheritance from
- std::exception
- */
- virtual ~vpException() throw() {}
-
- /** @name Inherited functionalities from vpException */
- //@{
- //! Send the object code.
- int getCode (void);
-
- //! Send a reference (constant) related the error message (can be empty).
- const std::string &getStringMessage (void) const;
- //! send a pointer on the array of \e char related to the error string.
- //! Cannot be \e NULL.
- const char *getMessage (void) const;
- //@}
-
- //! Print the error structure.
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpException & art);
-
- const char* what () const throw();
+protected:
+ //! Contains the error code, see the errorCodeEnum table for details.
+ int code;
+
+ //! Contains an error message (can be empty)
+ std::string message;
+
+ //! Set the message container
+ void setMessage(const char *format, va_list args);
+
+ //! forbid the empty constructor (protected)
+ vpException() : code(notInitialized), message(""){};
+
+public:
+ enum generalExceptionEnum {
+ memoryAllocationError,
+ memoryFreeError,
+ functionNotImplementedError,
+ ioError,
+ cannotUseConstructorError,
+ notImplementedError,
+ divideByZeroError,
+ dimensionError,
+ fatalError,
+ badValue, /*!< Used to indicate that a value is not in the allowed range.
+ */
+ notInitialized /*!< Used to indicate that a parameter is not initialized.
+ */
+ };
+
+ vpException(const int code, const char *format, va_list args);
+ vpException(const int code, const char *format, ...);
+ vpException(const int code, const std::string &msg);
+ explicit vpException(const int code);
+
+ /*!
+ Basic destructor. Do nothing but implemented to fit the inheritance from
+ std::exception
+ */
+ virtual ~vpException() throw() {}
+
+ /** @name Inherited functionalities from vpException */
+ //@{
+ //! Send the object code.
+ int getCode(void);
+
+ //! Send a reference (constant) related the error message (can be empty).
+ const std::string &getStringMessage(void) const;
+ //! send a pointer on the array of \e char related to the error string.
+ //! Cannot be \e NULL.
+ const char *getMessage(void) const;
+ //@}
+
+ //! Print the error structure.
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpException &art);
+
+ const char *what() const throw();
};
#endif /* #ifndef __vpException_H */
diff --git a/modules/core/include/visp3/core/vpExponentialMap.h b/modules/core/include/visp3/core/vpExponentialMap.h
index f4d8a6b..661f792 100644
--- a/modules/core/include/visp3/core/vpExponentialMap.h
+++ b/modules/core/include/visp3/core/vpExponentialMap.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,13 +42,11 @@
\brief Provides exponential map computation
*/
-
-
#ifndef vpExponentialMap_h
#define vpExponentialMap_h
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
/*!
\class vpExponentialMap
@@ -56,10 +55,11 @@
\brief Direct or inverse exponential map computation.
- The exponential map is the relationship between the velocity of a moving body
- and its pose. The exponential map transforms exponentially the velocity skew vector
- \f$ \bf v \f$ applied during a given time \f$\Delta t\f$ to its corresponding
- pose. The exponential map is usually written using homogeneous matrices as:
+ The exponential map is the relationship between the velocity of a moving
+ body and its pose. The exponential map transforms exponentially the velocity
+ skew vector \f$ \bf v \f$ applied during a given time \f$\Delta t\f$ to its
+ corresponding pose. The exponential map is usually written using homogeneous
+ matrices as:
\f[ {\bf M}_{t+\Delta t} = {\bf M}_{t} \exp^{({\bf v}, \Delta t)} \f]
where \f${\bf M}_{t}\f$ is a pose before applied velocity and
@@ -68,17 +68,18 @@
This class allows to compute the direct or the inverse exponential map.
- The direct exponential map allows to determine the displacement
- \f$ \exp^{({\bf v}, \Delta t)} \f$ from a velocity vector skew \f$ \bf v \f$
- applied during a sampling time \f$\Delta t\f$. With direct() the sampling
- time is set to 1 second. With direct(const vpColVector &, const double &)
- the sampling time can be set to an other value where the second argument
- is \f$ \Delta t \f$.
+ \f$ \exp^{({\bf v}, \Delta t)} \f$ from a velocity vector skew \f$ \bf v
+ \f$ applied during a sampling time \f$\Delta t\f$. With direct() the
+ sampling time is set to 1 second. With direct(const vpColVector &, const
+ double &) the sampling time can be set to an other value where the second
+ argument is \f$ \Delta t \f$.
- - The inverse exponential map allows to compute a velocity skew vector \f$ \bf v \f$
- from a displacement measured during a time interval \f$ \Delta t \f$.
- With inverse() the time interval also called sampling time is set to 1 second.
- With inverse(const vpHomogeneousMatrix &, const double &) the sampling time
- can be set to an other value where the second argument is \f$ \Delta t \f$.
+ - The inverse exponential map allows to compute a velocity skew vector \f$
+ \bf v \f$ from a displacement measured during a time interval \f$ \Delta t
+ \f$. With inverse() the time interval also called sampling time is set to 1
+ second. With inverse(const vpHomogeneousMatrix &, const double &) the
+ sampling time can be set to an other value where the second argument is \f$
+ \Delta t \f$.
The displacement is represented as an homogeneous matrix implemented in
vpHomogeneousMatrix. Velocities \f$ \bf v \f$ are represented as a
diff --git a/modules/core/include/visp3/core/vpFeatureDisplay.h b/modules/core/include/visp3/core/vpFeatureDisplay.h
index 0591a55..f3f8413 100644
--- a/modules/core/include/visp3/core/vpFeatureDisplay.h
+++ b/modules/core/include/visp3/core/vpFeatureDisplay.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureDisplay_H
#define vpFeatureDisplay_H
@@ -45,9 +45,9 @@
\brief interface with the image for feature display
*/
-//Color / image / display
-#include <visp3/core/vpImage.h>
+// Color / image / display
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
// Meter/pixel conversion
@@ -63,54 +63,30 @@ class VISP_EXPORT vpFeatureDisplay
{
public:
- static void displayCylinder(double rho1,double theta1,
- double rho2,double theta2,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayCylinder(double rho1,double theta1,
- double rho2,double theta2,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
+ static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
+ static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
- static void displayEllipse(double x,double y,
- double mu20, double mu11, double m02,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
+ static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
- static void displayEllipse(double x,double y,
- double mu20, double mu11, double m02,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
+ static void displayEllipse(double x, double y, double mu20, double mu11, double m02, const vpCameraParameters &cam,
+ const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
- static void displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayLine(double rho,double theta,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
+ static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
+ static void displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
- static void displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
- static void displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1) ;
+ static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
+ static void displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
};
#endif
diff --git a/modules/core/include/visp3/core/vpForceTwistMatrix.h b/modules/core/include/visp3/core/vpForceTwistMatrix.h
index 249fca1..f000c2c 100644
--- a/modules/core/include/visp3/core/vpForceTwistMatrix.h
+++ b/modules/core/include/visp3/core/vpForceTwistMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,7 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Twist transformation matrix that allows to transform forces from one
+ * Twist transformation matrix that allows to transform forces from one
* frame to an other.
*
* Authors:
@@ -44,23 +45,19 @@
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRotationMatrix.h>
-
/*!
\class vpForceTwistMatrix
\ingroup group_core_transformations
- \brief Implementation of a force/torque twist matrix and operations on such kind of matrices.
-
- Class that consider the particular case of twist
- transformation matrix that allows to transform a force/troque vector
- from one frame to an other.
+ This class derived from vpArray2D<double> implements the 6 by 6 matrix which
+transforms force/torque from one frame to another. This matrix is also called
+force/torque twist transformation matrix.
- The vpForceTwistMatrix class is derived from vpArray2D<double>.
-
- The twist transformation matrix that allows to transform the
- force/torque vector expressed at frame \f${\cal F}_b\f$ into the
- frame \f${\cal F}_a\f$ is a 6 by 6 matrix defined as
+ The full force/torque twist transformation matrix allows to compute the
+force/torque at point <em>a</em> expressed in frame <em>a</em> knowing its
+ force/torque at point <em>b</em> expressed in frame <em>b</em>. This matrix
+is defined as:
\f[
^a{\bf F}_b = \left[ \begin{array}{cc}
@@ -70,9 +67,21 @@
\right]
\f]
- \f$ ^a{\bf R}_b \f$ is a rotation matrix and
+ where \f$ ^a{\bf R}_b \f$ is a rotation matrix and
\f$ ^a{\bf t}_b \f$ is a translation vector.
+ When the point where the velocity is expressed doesn't change, the matrix
+becomes block diagonal. It allows than to compute the force/torque at point
+ <em>b</em> expressed in frame <em>a</em> knowing its
+ force/torque at point <em>b</em> expressed in frame <em>b</em> :
+ \f[
+ ^a{\bf F}_b = \left[ \begin{array}{cc}
+ ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
+ {\bf 0}_{3\times 3} & ^a{\bf R}_b
+ \end{array}
+ \right]
+ \f]
+
The code belows shows for example how to convert a force/torque skew
from probe frame to a sensor frame.
@@ -82,17 +91,21 @@
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
+ sFp.buildFrom(sMp);
+
+ // 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;
@@ -101,51 +114,55 @@ int main()
*/
class VISP_EXPORT vpForceTwistMatrix : public vpArray2D<double>
{
- public:
+public:
// basic constructor
vpForceTwistMatrix();
// copy constructor
- vpForceTwistMatrix(const vpForceTwistMatrix &F) ;
+ vpForceTwistMatrix(const vpForceTwistMatrix &F);
// constructor from an homogeneous transformation
- vpForceTwistMatrix(const vpHomogeneousMatrix &M) ;
+ explicit vpForceTwistMatrix(const vpHomogeneousMatrix &M, bool full = true);
- // Construction from Translation and rotation (ThetaU parameterization)
- vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
// Construction from Translation and rotation (matrix parameterization)
- vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- vpForceTwistMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
+ 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 vpRotationMatrix &R);
+ vpForceTwistMatrix(const vpThetaUVector &thetau);
+
/*!
Destructor.
*/
- virtual ~vpForceTwistMatrix() {};
+ 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) ;
+ vpForceTwistMatrix buildFrom(const vpRotationMatrix &R);
+ vpForceTwistMatrix buildFrom(const vpThetaUVector &thetau);
// Basic initialisation (identity)
- void eye() ;
+ void eye();
- vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const ;
- vpMatrix operator*(const vpMatrix &M) const ;
+ vpForceTwistMatrix operator*(const vpForceTwistMatrix &F) const;
+ vpMatrix operator*(const vpMatrix &M) const;
- vpColVector operator*(const vpColVector &H) const ;
+ vpColVector operator*(const vpColVector &H) const;
// copy operator from vpMatrix (handle with care)
vpForceTwistMatrix &operator=(const vpForceTwistMatrix &H);
- int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+ 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.
+ 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;
@@ -162,13 +179,13 @@ class VISP_EXPORT vpForceTwistMatrix : public vpArray2D<double>
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
+ vp_deprecated void init(){};
/*!
\deprecated You should rather use eye().
*/
vp_deprecated void setIdentity();
- //@}
+//@}
#endif
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpForwardProjection.h b/modules/core/include/visp3/core/vpForwardProjection.h
index 02951b9..3ff442e 100644
--- a/modules/core/include/visp3/core/vpForwardProjection.h
+++ b/modules/core/include/visp3/core/vpForwardProjection.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,13 +44,12 @@
\brief class that defines what is a generic geometric feature
*/
+#include <visp3/core/vpColor.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpTracker.h>
-#include <visp3/core/vpColor.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-
/*!
\class vpForwardProjection
\brief Class that defines what is a generic geometric feature.
@@ -61,23 +61,24 @@
- in the camera frame \e cP. These parameters are located in the public
attribute vpTracker::cP.
- in the image plane \e p. These parameters are located in the public
- attribute vpTracker::p. They correspond to normalized coordinates
+ attribute vpTracker::p. They correspond to normalized coordinates
of the feature expressed in meters.
*/
class VISP_EXPORT vpForwardProjection : public vpTracker
{
public:
- /*!
- Feature coordinates expressed in the object frame, also called world frame.
+ /*!
+ Feature coordinates expressed in the object frame, also called world
+ frame.
*/
- vpColVector oP ;
+ vpColVector oP;
public:
- /*!
+ /*!
Default initialisation of the feature parameters:
- in the object frame: \e oP
- in the camera frame: \e cP
- - in the image plane: \e p.
+ - in the image plane: \e p.
*/
virtual void init() = 0;
@@ -85,7 +86,7 @@ public:
virtual ~vpForwardProjection() { ; }
public:
- /*!
+ /*!
Sets the parameters which define the feature in the object frame.
\param oP : Feature parameters expressed in the object frame used
@@ -94,7 +95,7 @@ public:
*/
virtual void setWorldCoordinates(const vpColVector &oP) = 0;
- /*!
+ /*!
Computes the features parameters in the camera frame (\e cP) thanks
to the parameters given in the object frame
@@ -109,11 +110,11 @@ public:
\param cP : The vector which will contain the feature parameters
expressed in the camera frame.
-
+
With this method, the vpTracker::cP public attribute is not updated.
-
+
*/
- virtual void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP)=0;
+ virtual void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) = 0;
/*!
Computes the features parameters in the camera frame (\e cP) thanks
@@ -130,8 +131,8 @@ public:
The features parameters in the camera frame (cP) are updated in
the vpTracker::cP public attribute.
*/
- virtual void changeFrame(const vpHomogeneousMatrix &cMo) = 0 ;
- /*!
+ virtual void changeFrame(const vpHomogeneousMatrix &cMo) = 0;
+ /*!
Computes the feature parameters in the image plane from the
parameters expressed in the camera frame.
@@ -140,9 +141,9 @@ public:
\param p [output] : Feature parameters expressed in the image plane.
*/
- virtual void projection(const vpColVector &cP, vpColVector &p) =0 ;
+ virtual void projection(const vpColVector &cP, vpColVector &p) = 0;
- /*!
+ /*!
Computes the feature parameters in the image plane. These
parameters are than updated in the vpTracker::p public attribute.
@@ -151,14 +152,14 @@ public:
to be updated before the call of this method. For that, a call to
changeFrame(const vpHomogeneousMatrix &) is requested.
*/
- virtual void projection() =0 ;
+ virtual void projection() = 0;
- void project() ;
- void project(const vpHomogeneousMatrix &cMo) ;
- void track(const vpHomogeneousMatrix &cMo) ;
+ void project();
+ void project(const vpHomogeneousMatrix &cMo);
+ void track(const vpHomogeneousMatrix &cMo);
/*!
-
+
Displays the feature in the image \e I thanks to the 2D feature
parameters in the image plane (vpTracker::p) and the camera
parameters which enable to convert the features from meter to pixel.
@@ -171,16 +172,14 @@ public:
\param color : The desired color to display the line in the image.
\param thickness : Thickness of the feature representation.
*/
- virtual void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) =0;
+ virtual void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1) = 0;
/*!
Displays the feature in the image \e I thanks to the features in
- the object frame (vpForwardProjection::oP), the homogeneous matrix relative
- to the pose between the object frame and the camera frame and the
+ the object frame (vpForwardProjection::oP), the homogeneous matrix
+ relative to the pose between the object frame and the camera frame and the
camera parameters which enable to convert the features from meter
to pixel.
@@ -195,38 +194,31 @@ public:
\param color : The desired color to display the line in the image.
\param thickness : Thickness of the feature representation.
*/
- virtual void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) =0;
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1) = 0;
- virtual void print() const ;
+ virtual void print() const;
/*!
Create an object with the same type.
*/
- virtual vpForwardProjection *duplicate() const = 0 ;
+ virtual vpForwardProjection *duplicate() const = 0;
public:
/*!
Used for memory issue especially in the vpServo class.
*/
- typedef enum
- {
- user,
- vpDisplayForwardProjection
- } vpForwardProjectionDeallocatorType;
+ typedef enum { user, vpDisplayForwardProjection } vpForwardProjectionDeallocatorType;
private:
- vpForwardProjectionDeallocatorType deallocate ;
+ vpForwardProjectionDeallocatorType deallocate;
+
public:
vpForwardProjection() : oP(), deallocate(user) {}
- void setDeallocate(vpForwardProjectionDeallocatorType d) { deallocate = d ; }
- vpForwardProjectionDeallocatorType getDeallocate() { return deallocate ; }
-} ;
-
+ void setDeallocate(vpForwardProjectionDeallocatorType d) { deallocate = d; }
+ vpForwardProjectionDeallocatorType getDeallocate() { return deallocate; }
+};
#endif
diff --git a/modules/core/include/visp3/core/vpFrameGrabber.h b/modules/core/include/visp3/core/vpFrameGrabber.h
index ac7ecfb..7d741c6 100644
--- a/modules/core/include/visp3/core/vpFrameGrabber.h
+++ b/modules/core/include/visp3/core/vpFrameGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef vpFrameGrabber_hh
#define vpFrameGrabber_hh
@@ -63,8 +62,8 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
int main()
{
@@ -101,38 +100,37 @@ int main()
*/
class VISP_EXPORT vpFrameGrabber
{
-public :
- bool init ; //!< Set to true if the frame grabber has been initialized.
+public:
+ bool init; //!< Set to true if the frame grabber has been initialized.
protected:
- unsigned int height ; //!< Number of rows in the image.
- unsigned int width ; //!< Number of columns in the image.
+ unsigned int height; //!< Number of rows in the image.
+ unsigned int width; //!< Number of columns in the image.
public:
/** @name Inherited functionalities from vpFramegrabber */
//@{
//! Return the number of rows in the image.
- inline unsigned int getHeight() const { return height ; }
+ inline unsigned int getHeight() const { return height; }
//! Return the number of columns in the image.
- inline unsigned int getWidth() const { return width ; }
+ inline unsigned int getWidth() const { return width; }
//@}
public:
- vpFrameGrabber() : init(false), height(0), width(0) {};
+ vpFrameGrabber() : init(false), height(0), width(0){};
virtual ~vpFrameGrabber() { ; }
- virtual void open(vpImage<unsigned char> &I) =0 ;
- virtual void open(vpImage<vpRGBa> &I) =0 ;
-
- virtual void acquire(vpImage<unsigned char> &I) =0 ;
- virtual void acquire(vpImage<vpRGBa> &I) =0 ;
+ virtual void open(vpImage<unsigned char> &I) = 0;
+ virtual void open(vpImage<vpRGBa> &I) = 0;
+ virtual void acquire(vpImage<unsigned char> &I) = 0;
+ virtual void acquire(vpImage<vpRGBa> &I) = 0;
/*!
This virtual function is used to de-allocate
the memory used by a specific frame grabber
*/
- virtual void close() =0 ;
-} ;
+ virtual void close() = 0;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpFrameGrabberException.h b/modules/core/include/visp3/core/vpFrameGrabberException.h
index 42be1f3..d577669 100644
--- a/modules/core/include/visp3/core/vpFrameGrabberException.h
+++ b/modules/core/include/visp3/core/vpFrameGrabberException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,68 +29,66 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Exceptions that can be emited by the vpFrameGrabber class and its derivates.
+ * Exceptions that can be emited by the vpFrameGrabber class and its
+ *derivates.
*
* Authors:
* Eric Marchand
*
*****************************************************************************/
-
#ifndef __vpFrameGrabberException_H
#define __vpFrameGrabberException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpFrameGrabberException.h
- \brief error that can be emited by the vpFrameGrabber class and its derivates
+ \brief error that can be emited by the vpFrameGrabber class and its
+ derivates
*/
/* Classes standards. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
-/*!
- \brief Error that can be emited by the vpFrameGrabber class and its derivates
+/*!
+ \brief Error that can be emited by the vpFrameGrabber class and its
+ derivates
*/
class VISP_EXPORT vpFrameGrabberException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpFrameGrabber member
- */
- enum errorFrameGrabberCodeEnum
- {
- settingError,
- initializationError,
- otherError
- } ;
-
- public:
- vpFrameGrabberException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpFrameGrabberException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpFrameGrabberException (const int id)
- : vpException(id){ ; }
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpFrameGrabber member
+ */
+ enum errorFrameGrabberCodeEnum { settingError, initializationError, otherError };
+public:
+ vpFrameGrabberException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpFrameGrabberException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpFrameGrabberException(const int id) : vpException(id) { ; }
};
#endif /* #ifndef __vpFrameGrabberException_H */
diff --git a/modules/core/include/visp3/core/vpGEMM.h b/modules/core/include/visp3/core/vpGEMM.h
index cc689c6..f05ffc5 100644
--- a/modules/core/include/visp3/core/vpGEMM.h
+++ b/modules/core/include/visp3/core/vpGEMM.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,358 +36,390 @@
*
*****************************************************************************/
-
#ifndef __VP_GEMM__
#define __VP_GEMM__
#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpException.h>
-const vpArray2D<double> null(0,0);
+const vpArray2D<double> null(0, 0);
/*!
Enumeration of the operations applied on matrices in vpGEMM() function.
-
- Operations are :
+
+ Operations are :
- VP_GEMM_A_T to use the transpose matrix of A instead of the matrix A
- VP_GEMM_B_T to use the transpose matrix of B instead of the matrix B
- VP_GEMM_C_T to use the transpose matrix of C instead of the matrix C
-
+
\relates vpArray2D
*/
typedef enum {
- VP_GEMM_A_T=1, //! Use A^T instead of A
- VP_GEMM_B_T=2, //! Use B^T instead of B
- VP_GEMM_C_T=4, //! Use C^T instead of C
+ VP_GEMM_A_T = 1, //! Use A^T instead of A
+ VP_GEMM_B_T = 2, //! Use B^T instead of B
+ VP_GEMM_C_T = 4, //! Use C^T instead of C
} vpGEMMmethod;
-template<unsigned int>
-inline void GEMMsize(const vpArray2D<double> & /*A*/,const vpArray2D<double> & /*B*/, unsigned int &/*Arows*/, unsigned int &/*Acols*/, unsigned int &/*Brows*/, unsigned int &/*Bcols*/)
-{}
+template <unsigned int>
+inline void GEMMsize(const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, unsigned int & /*Arows*/,
+ unsigned int & /*Acols*/, unsigned int & /*Brows*/, unsigned int & /*Bcols*/)
+{
+}
-template<>
-void inline GEMMsize<0>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+void inline GEMMsize<0>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getRows();
- Bcols= B.getCols();
+ Arows = A.getRows();
+ Acols = A.getCols();
+ Brows = B.getRows();
+ Bcols = B.getCols();
}
-template<>
-inline void GEMMsize<1>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<1>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getRows();
- Bcols= B.getCols();
+ Arows = A.getCols();
+ Acols = A.getRows();
+ Brows = B.getRows();
+ Bcols = B.getCols();
}
-template<>
-inline void GEMMsize<2>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<2>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getCols();
- Bcols= B.getRows();
+ Arows = A.getRows();
+ Acols = A.getCols();
+ Brows = B.getCols();
+ Bcols = B.getRows();
}
-template<>
-inline void GEMMsize<3>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<3>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getCols();
- Bcols= B.getRows();
+ Arows = A.getCols();
+ Acols = A.getRows();
+ Brows = B.getCols();
+ Bcols = B.getRows();
}
-template<>
-inline void GEMMsize<4>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<4>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getRows();
- Bcols= B.getCols();
+ Arows = A.getRows();
+ Acols = A.getCols();
+ Brows = B.getRows();
+ Bcols = B.getCols();
}
-template<>
-inline void GEMMsize<5>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<5>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getRows();
- Bcols= B.getCols();
+ Arows = A.getCols();
+ Acols = A.getRows();
+ Brows = B.getRows();
+ Bcols = B.getCols();
}
-template<>
-inline void GEMMsize<6>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<6>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getRows();
- Acols= A.getCols();
- Brows= B.getCols();
- Bcols= B.getRows();
+ Arows = A.getRows();
+ Acols = A.getCols();
+ Brows = B.getCols();
+ Bcols = B.getRows();
}
-template<>
-inline void GEMMsize<7>(const vpArray2D<double> & A,const vpArray2D<double> & B, unsigned int &Arows, unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
+template <>
+inline void GEMMsize<7>(const vpArray2D<double> &A, const vpArray2D<double> &B, unsigned int &Arows,
+ unsigned int &Acols, unsigned int &Brows, unsigned int &Bcols)
{
- Arows= A.getCols();
- Acols= A.getRows();
- Brows= B.getCols();
- Bcols= B.getRows();
+ Arows = A.getCols();
+ Acols = A.getRows();
+ Brows = B.getCols();
+ Bcols = B.getRows();
}
-template<unsigned int>
-inline void GEMM1(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,vpArray2D<double> &/*D*/){}
+template <unsigned int>
+inline void GEMM1(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/,
+ const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,
+ vpArray2D<double> & /*D*/)
+{
+}
-template<>
-inline void GEMM1<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+template <>
+inline void GEMM1<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[n][c] * alpha;
+ D[r][c] = sum;
}
}
-template<>
-inline void GEMM1<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+template <>
+inline void GEMM1<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[n][c] * alpha;
+ D[r][c] = sum;
}
}
-template<>
-inline void GEMM1<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+template <>
+inline void GEMM1<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[c][n] * alpha;
+ D[r][c] = sum;
}
}
-template<>
-inline void GEMM1<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha,vpArray2D<double> &D)
+template <>
+inline void GEMM1<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[c][n] * alpha;
+ D[r][c] = sum;
}
}
-template<unsigned int>
-inline void GEMM2(const unsigned int &/*Arows*/,const unsigned int &/*Brows*/, const unsigned int &/*Bcols*/, const vpArray2D<double> & /*A*/,const vpArray2D<double> & /*B*/, const double & /*alpha*/, const vpArray2D<double> & /*C*/ , const double &/*beta*/, vpArray2D<double> &/*D*/)
-{}
+template <unsigned int>
+inline void GEMM2(const unsigned int & /*Arows*/, const unsigned int & /*Brows*/, const unsigned int & /*Bcols*/,
+ const vpArray2D<double> & /*A*/, const vpArray2D<double> & /*B*/, const double & /*alpha*/,
+ const vpArray2D<double> & /*C*/, const double & /*beta*/, vpArray2D<double> & /*D*/)
+{
+}
-template<>
-inline void GEMM2<0>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<0>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum+C[r][c]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[n][c] * alpha;
+ D[r][c] = sum + C[r][c] * beta;
}
}
-template<>
-inline void GEMM2<1>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<1>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum+C[r][c]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[n][c] * alpha;
+ D[r][c] = sum + C[r][c] * beta;
}
}
-template<>
-inline void GEMM2<2>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<2>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum+C[r][c]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[c][n] * alpha;
+ D[r][c] = sum + C[r][c] * beta;
}
}
-template<>
-inline void GEMM2<3>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<3>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum+C[r][c]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[c][n] * alpha;
+ D[r][c] = sum + C[r][c] * beta;
}
}
-
-template<>
-inline void GEMM2<4>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<4>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[n][c]*alpha;
- D[r][c]=sum+C[c][r]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[n][c] * alpha;
+ D[r][c] = sum + C[c][r] * beta;
}
}
-template<>
-inline void GEMM2<5>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<5>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[n][c]*alpha;
- D[r][c]=sum+C[c][r]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[n][c] * alpha;
+ D[r][c] = sum + C[c][r] * beta;
}
-
}
-template<>
-inline void GEMM2<6>(const unsigned int &Arows,const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<6>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[r][n]*B[c][n]*alpha;
- D[r][c]=sum+C[c][r]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[r][n] * B[c][n] * alpha;
+ D[r][c] = sum + C[c][r] * beta;
}
}
-template<>
-inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols, const vpArray2D<double> & A,const vpArray2D<double> & B, const double & alpha, const vpArray2D<double> & C , const double &beta, vpArray2D<double> &D)
+template <>
+inline void GEMM2<7>(const unsigned int &Arows, const unsigned int &Brows, const unsigned int &Bcols,
+ const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
- for(unsigned int r=0;r<Arows;r++)
- for(unsigned int c=0;c<Bcols;c++){
- double sum=0;
- for(unsigned int n=0;n<Brows;n++)
- sum+=A[n][r]*B[c][n]*alpha;
- D[r][c]=sum+C[c][r]*beta;
+ for (unsigned int r = 0; r < Arows; r++)
+ for (unsigned int c = 0; c < Bcols; c++) {
+ double sum = 0;
+ for (unsigned int n = 0; n < Brows; n++)
+ sum += A[n][r] * B[c][n] * alpha;
+ D[r][c] = sum + C[c][r] * beta;
}
}
-template<unsigned int T>
-inline void vpTGEMM(const vpArray2D<double> & A, const vpArray2D<double> & B, const double & alpha ,const vpArray2D<double> & C, const double & beta, vpArray2D<double> & D)
+template <unsigned int T>
+inline void vpTGEMM(const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D)
{
unsigned int Arows;
unsigned int Acols;
unsigned int Brows;
unsigned int Bcols;
-
- GEMMsize<T>(A,B,Arows,Acols,Brows,Bcols);
-
- try {
- if ((Arows != D.getRows()) || (Bcols != D.getCols())) D.resize(Arows,Bcols);
- }
- catch(...) {
- throw ;
+
+ GEMMsize<T>(A, B, Arows, Acols, Brows, Bcols);
+
+ try {
+ if ((Arows != D.getRows()) || (Bcols != D.getCols()))
+ D.resize(Arows, Bcols);
+ } catch (...) {
+ throw;
}
-
+
if (Acols != Brows) {
- throw(vpException(vpException::dimensionError,
- "In vpGEMM, cannot multiply (%dx%d) matrix by (%dx%d) matrix",
- Arows, Acols, Brows, Bcols)) ;
+ 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 (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<T>(Arows,Brows,Bcols,A,B,alpha,C,beta,D);
- }else{
- GEMM1<T>(Arows,Brows,Bcols,A,B,alpha,D);
+
+ GEMM2<T>(Arows, Brows, Bcols, A, B, alpha, C, beta, D);
+ } else {
+ GEMM1<T>(Arows, Brows, Bcols, A, B, alpha, D);
}
-
}
/*!
This function performs generalized matrix multiplication:
D = alpha*op(A)*op(B) + beta*op(C), where op(X) is X or X^T.
- Operation on A, B and C matrices is described by enumeration vpGEMMmethod().
-
+ Operation on A, B and C matrices is described by enumeration
+ vpGEMMmethod().
+
For example, to compute D = alpha*A^T*B^T+beta*C we need to call :
\code
vpGEMM(A, B, alpha, C, beta, D, VP_GEMM_A_T + VP_GEMM_B_T);
\endcode
-
+
If C is not used, vpGEMM must be called using an empty array \e null.
Thus to compute D = alpha*A^T*B, we have to call:
\code
vpGEMM(A, B, alpha, null, 0, D, VP_GEMM_B_T);
\endcode
-
- \exception vpException::incorrectMatrixSizeError if the sizes of the matrices
- do not allow the operations.
-
+
+ \exception vpException::incorrectMatrixSizeError if the sizes of the
+ matrices do not allow the operations.
+
\param A : An array that could be a vpMatrix.
\param B : An array that could be a vpMatrix.
\param alpha : A scalar.
\param C : An array that could be a vpMatrix.
\param beta : A scalar.
\param D : The resulting array that could be a vpMatrix.
- \param ops : A scalar describing operation applied on the matrices. Possible values
- are the one defined in vpGEMMmethod(): VP_GEMM_A_T, VP_GEMM_B_T, VP_GEMM_C_T.
-
+ \param ops : A scalar describing operation applied on the matrices.
+ Possible values are the one defined in vpGEMMmethod(): VP_GEMM_A_T,
+ VP_GEMM_B_T, VP_GEMM_C_T.
+
\relates vpArray2D
-
-*/
-inline void vpGEMM(const vpArray2D<double> & A, const vpArray2D<double> & B,
- const double & alpha, const vpArray2D<double> & C,
- const double & beta, vpArray2D<double> & D, const unsigned int &ops=0)
+
+*/
+inline void vpGEMM(const vpArray2D<double> &A, const vpArray2D<double> &B, const double &alpha,
+ const vpArray2D<double> &C, const double &beta, vpArray2D<double> &D, const unsigned int &ops = 0)
{
- switch(ops){
- case 0 :
- vpTGEMM<0>( A, B, alpha , C, beta, D);
+ switch (ops) {
+ case 0:
+ vpTGEMM<0>(A, B, alpha, C, beta, D);
break;
- case 1 :
- vpTGEMM<1>( A, B, alpha , C, beta, D);
+ case 1:
+ vpTGEMM<1>(A, B, alpha, C, beta, D);
break;
- case 2 :
- vpTGEMM<2>( A, B, alpha , C, beta, D);
+ case 2:
+ vpTGEMM<2>(A, B, alpha, C, beta, D);
break;
- case 3 :
- vpTGEMM<3>( A, B, alpha , C, beta, D);
+ case 3:
+ vpTGEMM<3>(A, B, alpha, C, beta, D);
break;
- case 4 :
- vpTGEMM<4>( A, B, alpha , C, beta, D);
+ case 4:
+ vpTGEMM<4>(A, B, alpha, C, beta, D);
break;
- case 5 :
- vpTGEMM<5>( A, B, alpha , C, beta, D);
+ case 5:
+ vpTGEMM<5>(A, B, alpha, C, beta, D);
break;
- case 6 :
- vpTGEMM<6>( A, B, alpha , C, beta, D);
+ case 6:
+ vpTGEMM<6>(A, B, alpha, C, beta, D);
break;
- case 7 :
- vpTGEMM<7>( A, B, alpha , C, beta, D);
+ case 7:
+ 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 1dd89a2..7440f68 100644
--- a/modules/core/include/visp3/core/vpGaussRand.h
+++ b/modules/core/include/visp3/core/vpGaussRand.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpGaussRand_hh
#define vpGaussRand_hh
@@ -49,8 +49,8 @@
The algorithms and notations used are described in \cite Gentle:2004.
- The code below shows how to use the random generator to get values that have their mean equal to
- 10 with a standart deviation equal to 0.5.
+ The code below shows how to use the random generator to get values that have
+their mean equal to 10 with a standart deviation equal to 0.5.
\code
#include <iostream>
@@ -80,9 +80,10 @@ noise 8: 9.62742
noise 9: 9.37701
\endcode
- Note that the previous example produces always the same "random" results. To produce real random
- values, you need to initialize the random generator with different values using seed(). For example,
- this could be done using the current time. The code becomes:
+ Note that the previous example produces always the same "random" results. To
+produce real random values, you need to initialize the random generator with
+different values using seed(). For example, this could be done using the
+current time. The code becomes:
\code
#include <iostream>
@@ -118,13 +119,12 @@ noise 9: 10.2391
*/
class VISP_EXPORT vpGaussRand : public vpUniRand
{
-private :
+private:
double mean;
double sigma;
double gaussianDraw();
public:
-
/*!
Default noise generator constructor.
*/
@@ -138,7 +138,9 @@ public:
\param noise_seed : Seed of the noise
*/
vpGaussRand(const double sigma_val, const double mean_val, const long noise_seed = 0)
- : vpUniRand(noise_seed), mean(mean_val), sigma(sigma_val) {}
+ : vpUniRand(noise_seed), mean(mean_val), sigma(sigma_val)
+ {
+ }
/*!
Set the standard deviation and mean for gaussian noise.
@@ -146,7 +148,8 @@ public:
\param sigma_val : New standard deviation sigma.
\param mean_val : New mean value.
*/
- void setSigmaMean(const double sigma_val, const double mean_val) {
+ void setSigmaMean(const double sigma_val, const double mean_val)
+ {
this->mean = mean_val;
this->sigma = sigma_val;
}
@@ -156,16 +159,12 @@ public:
\param seed_val : New seed.
*/
- void seed(const long seed_val) {
- x=seed_val;
- }
+ void seed(const long seed_val) { x = seed_val; }
/*!
Return a random value from the Gaussian noise generator.
*/
- double operator()() {
- return sigma*gaussianDraw()+mean;
- }
+ double operator()() { return sigma * gaussianDraw() + mean; }
};
#endif
diff --git a/modules/core/include/visp3/core/vpHinkley.h b/modules/core/include/visp3/core/vpHinkley.h
index c6334b7..f88aacb 100644
--- a/modules/core/include/visp3/core/vpHinkley.h
+++ b/modules/core/include/visp3/core/vpHinkley.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -94,23 +95,23 @@
*/
class VISP_EXPORT vpHinkley
{
- public:
+public:
/*! \enum vpHinkleyJumpType
Indicates if a jump is detected by the Hinkley test.
*/
typedef enum {
- noJump, /*!< No jump is detected by the Hinkley test. */
+ noJump, /*!< No jump is detected by the Hinkley test. */
downwardJump, /*!< A downward jump is detected by the Hinkley test. */
- upwardJump /*!< An upward jump is detected by the Hinkley test. */
+ upwardJump /*!< An upward jump is detected by the Hinkley test. */
} vpHinkleyJumpType;
- public:
+public:
vpHinkley();
~vpHinkley();
vpHinkley(double alpha, double delta);
void init();
- void init(double alpha, double delta) ;
+ void init(double alpha, double delta);
void setDelta(double delta);
void setAlpha(double alpha);
@@ -118,47 +119,49 @@ class VISP_EXPORT vpHinkley
vpHinkleyJumpType testUpwardJump(double signal);
vpHinkleyJumpType testDownUpwardJump(double signal);
- static void print(vpHinkleyJumpType jump) ;
+ static void print(vpHinkleyJumpType jump);
/*!
\return The mean value \f$m_0\f$ of the signal \f$ s(t) \f$.
*/
- inline double getMean() {return mean;}
+ inline double getMean() { return mean; }
/*!
- \return The value of \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 + \frac{\delta}{2})\f$
+ \return The value of \f$S_k = \sum_{t=0}^{k} (s(t) - m_0 +
+ \frac{\delta}{2})\f$
*/
- inline double getSk() {return Sk;}
+ inline double getSk() { return Sk; }
/*!
\return The value of \f$M_k\f$, the maximum value of \f$S_k\f$.
*/
- inline double getMk() {return Mk;}
+ inline double getMk() { return Mk; }
/*!
\return The value of \f$T_k = \sum_{t=0}^{k} (s(t) - m_0 -
\frac{\delta}{2})\f$
*/
- inline double getTk() {return Tk;}
+ inline double getTk() { return Tk; }
/*!
\return The value of \f$N_k\f$, the maximum value of \f$T_k\f$.
*/
- inline double getNk() {return Nk;}
- private:
+ inline double getNk() { return Nk; }
+
+private:
void computeMean(double signal);
void computeSk(double signal);
void computeMk();
void computeTk(double signal);
void computeNk();
- private:
+private:
double dmin2;
double alpha;
- int nsignal; // Signal length
- double mean; // Signal mean value
+ int nsignal; // Signal length
+ double mean; // Signal mean value
double Sk;
double Mk;
double Tk;
diff --git a/modules/core/include/visp3/core/vpHistogram.h b/modules/core/include/visp3/core/vpHistogram.h
index 8266b36..930ea82 100644
--- a/modules/core/include/visp3/core/vpHistogram.h
+++ b/modules/core/include/visp3/core/vpHistogram.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpHistogram.h
\brief Declaration of the vpHistogram class.
@@ -49,13 +48,13 @@
#include <sstream>
-#include <visp3/core/vpImage.h>
+#include <visp3/core/vpColor.h>
#include <visp3/core/vpHistogramPeak.h>
#include <visp3/core/vpHistogramValey.h>
-#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp3/core/vpList.h>
+#include <visp3/core/vpList.h>
#endif
#include <list>
@@ -77,7 +76,7 @@
vpHistogramValey valey;
vpHistogram h(I);
h.smooth(); // Filter the histogram values
- vpHistogramPeak peakl, peakr; // Two highest peaks in the histogram
+ vpHistogramPeak peakl, peakr; // Two highest peaks in the histogram
// - peakl: Peak on the left
// - peakr: Peak on the right
@@ -86,7 +85,7 @@
threshold = valey.getLevel();
\endcode
- has the same behaviour than this one:
+ has the same behaviour than this one:
\code
vpImage<unsigned char> I;
@@ -94,13 +93,13 @@
unsigned char dist = 60;
vpHistogram h(I);
h.smooth(); // Filter the histogram values
- vpHistogramPeak peak1, peak2; // Two highest peaks in the histogram
+ vpHistogramPeak peak1, peak2; // Two highest peaks in the histogram
// - peak1: Highest peak
// - peakr: Second highest peak
// Get the two highest peaks
h.getPeaks(dist, peak1, peak2);
-
+
// Get the valey between the two highest peaks
vpHistogramValey valey;
h.getValey(peak1, peak2, valey);
@@ -115,10 +114,10 @@ class VISP_EXPORT vpHistogram
public:
vpHistogram();
vpHistogram(const vpHistogram &h);
- vpHistogram(const vpImage<unsigned char> &I);
+ explicit vpHistogram(const vpImage<unsigned char> &I);
virtual ~vpHistogram();
- vpHistogram & operator=(const vpHistogram &h);
+ vpHistogram &operator=(const vpHistogram &h);
/*!
@@ -130,8 +129,8 @@ public:
\code
vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
+
+ vpHistogram h;
h.calculate(I); // Histogram of the gray level image
// Print the histogram values
@@ -160,8 +159,8 @@ public:
\code
vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
+
+ vpHistogram h;
h.calculate(I); // Histogram of the gray level image
// Print the histogram values
@@ -172,7 +171,7 @@ public:
*/
inline unsigned operator()(const unsigned char level) const
{
- if(level < size) {
+ if (level < size) {
return histogram[level];
}
@@ -190,8 +189,8 @@ public:
\code
vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
+
+ vpHistogram h;
h.calculate(I); // Histogram of the gray level image
// Print the histogram values
@@ -202,7 +201,7 @@ public:
*/
inline unsigned get(const unsigned char level) const
{
- if(level < size) {
+ if (level < size) {
return histogram[level];
}
@@ -220,7 +219,7 @@ public:
\param value : Number of pixels having the gray level.
\code
- vpHistogram h;
+ vpHistogram h;
// Set histogram values
for (int i=0; i < h.getSize(); i ++)
@@ -230,7 +229,7 @@ public:
*/
inline void set(const unsigned char level, unsigned int value)
{
- if(level < size) {
+ if (level < size) {
histogram[level] = value;
} else {
std::stringstream ss;
@@ -239,32 +238,23 @@ public:
}
};
- void calculate(const vpImage<unsigned char> &I, const unsigned int nbins=256, const unsigned int nbThreads=1);
-
- void display(const vpImage<unsigned char> &I, const vpColor &color=vpColor::white, const unsigned int thickness=2,
- const unsigned int maxValue_=0);
-
- void smooth(const unsigned int fsize = 3);
- unsigned getPeaks(std::list<vpHistogramPeak> & peaks);
- unsigned getPeaks(unsigned char dist,
- vpHistogramPeak & peak1,
- vpHistogramPeak & peak2);
- bool getPeaks(unsigned char dist,
- vpHistogramPeak & peakl,
- vpHistogramPeak & peakr,
- vpHistogramValey & valey);
- unsigned getValey(std::list<vpHistogramValey> & valey);
- bool getValey(const vpHistogramPeak & peak1,
- const vpHistogramPeak & peak2,
- vpHistogramValey & valey);
- unsigned getValey(unsigned char dist,
- const vpHistogramPeak & peak,
- vpHistogramValey & valeyl,
- vpHistogramValey & valeyr);
- unsigned sort(std::list<vpHistogramPeak> & peaks);
-
- bool write(const std::string &filename);
- bool write(const char *filename);
+ void calculate(const vpImage<unsigned char> &I, const unsigned int nbins = 256, const unsigned int nbThreads = 1);
+
+ void display(const vpImage<unsigned char> &I, const vpColor &color = vpColor::white, const unsigned int thickness = 2,
+ const unsigned int maxValue_ = 0);
+
+ void smooth(const unsigned int fsize = 3);
+ unsigned getPeaks(std::list<vpHistogramPeak> &peaks);
+ unsigned getPeaks(unsigned char dist, vpHistogramPeak &peak1, vpHistogramPeak &peak2);
+ bool getPeaks(unsigned char dist, vpHistogramPeak &peakl, vpHistogramPeak &peakr, vpHistogramValey &valey);
+ unsigned getValey(std::list<vpHistogramValey> &valey);
+ bool getValey(const vpHistogramPeak &peak1, const vpHistogramPeak &peak2, vpHistogramValey &valey);
+ unsigned getValey(unsigned char dist, const vpHistogramPeak &peak, vpHistogramValey &valeyl,
+ vpHistogramValey &valeyr);
+ unsigned sort(std::list<vpHistogramPeak> &peaks);
+
+ bool write(const std::string &filename);
+ bool write(const char *filename);
/*!
Get the histogram size.
@@ -274,10 +264,7 @@ public:
\sa getValues()
*/
- inline unsigned getSize() const
- {
- return size;
- };
+ inline unsigned getSize() const { return size; };
/*!
@@ -288,8 +275,8 @@ public:
\code
vpImage<unsigned char> I; // A gray level image
-
- vpHistogram h;
+
+ vpHistogram h;
h.calculate(I); // Histogram of the gray level image
// Print the histogram values
@@ -300,10 +287,7 @@ public:
\sa getSize()
*/
- inline unsigned * getValues()
- {
- return histogram;
- };
+ inline unsigned *getValues() { return histogram; };
private:
void init(unsigned size = 256);
@@ -312,8 +296,6 @@ private:
unsigned size; // Histogram size (max allowed 256)
};
-
-
#endif
/*
diff --git a/modules/core/include/visp3/core/vpHistogramPeak.h b/modules/core/include/visp3/core/vpHistogramPeak.h
index 4cae198..2197215 100644
--- a/modules/core/include/visp3/core/vpHistogramPeak.h
+++ b/modules/core/include/visp3/core/vpHistogramPeak.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpHistogramPeak.h
\brief Declaration of the vpHistogramPeak class.
@@ -65,15 +64,15 @@
class VISP_EXPORT vpHistogramPeak
{
-public :
+public:
vpHistogramPeak();
vpHistogramPeak(unsigned char level, unsigned value);
- vpHistogramPeak(const vpHistogramPeak & p);
+ vpHistogramPeak(const vpHistogramPeak &p);
/*! Destructor that does nothing. */
virtual ~vpHistogramPeak() {}
- vpHistogramPeak & operator=(const vpHistogramPeak &p);
+ vpHistogramPeak &operator=(const vpHistogramPeak &p);
bool operator==(const vpHistogramPeak &p) const;
/*!
@@ -86,10 +85,7 @@ public :
\sa setValue(), set()
*/
- inline void setLevel(unsigned char lvl)
- {
- this->level = lvl;
- };
+ inline void setLevel(unsigned char lvl) { this->level = lvl; };
/*!
Set the peak number of pixels having a same gray level. To set the
@@ -100,10 +96,7 @@ public :
\sa setLevel(), set()
*/
- inline void setValue(unsigned val)
- {
- this->value = val;
- };
+ inline void setValue(unsigned val) { this->value = val; };
/*!
Set the peak gray \e level and number of pixels at this gray level.
@@ -115,11 +108,11 @@ public :
*/
inline void set(unsigned char lvl, unsigned val)
- {
- this->level = lvl;
- this->value = val;
- };
-
+ {
+ this->level = lvl;
+ this->value = val;
+ };
+
/*!
Get the peak gray \e level. The number of pixels having this
@@ -130,10 +123,7 @@ public :
\sa getValue()
*/
- inline unsigned char getLevel() const
- {
- return level;
- };
+ inline unsigned char getLevel() const { return level; };
/*!
Get the peak number of pixels having a same gray level. The
@@ -144,23 +134,18 @@ public :
\sa getLevel()
*/
- inline unsigned getValue() const
- {
- return value;
- };
+ inline unsigned getValue() const { return value; };
//---------------------------------
// 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 :
+protected:
unsigned char level; //! Gray level ot the peak.
- unsigned value; //! Number of pixels on the gray level.
+ unsigned value; //! Number of pixels on the gray level.
};
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/core/include/visp3/core/vpHistogramValey.h b/modules/core/include/visp3/core/vpHistogramValey.h
index 5dafb2e..59a8371 100644
--- a/modules/core/include/visp3/core/vpHistogramValey.h
+++ b/modules/core/include/visp3/core/vpHistogramValey.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpHistogramValey.h
\brief Declaration of the vpHistogramValey class.
@@ -63,20 +62,17 @@
class VISP_EXPORT vpHistogramValey : vpHistogramPeak
{
- public:
- vpHistogramValey() :
- vpHistogramPeak() {};
-
- vpHistogramValey(unsigned char lvl, unsigned val) :
- vpHistogramPeak(lvl, val) {};
+public:
+ vpHistogramValey() : vpHistogramPeak(){};
- vpHistogramValey(const vpHistogramValey & v) :
- vpHistogramPeak(v) {};
+ vpHistogramValey(unsigned char lvl, unsigned val) : vpHistogramPeak(lvl, val){};
+
+ vpHistogramValey(const vpHistogramValey &v) : vpHistogramPeak(v){};
/*! Destructor that does nothing. */
virtual ~vpHistogramValey() {}
- vpHistogramValey & operator=(const vpHistogramValey &v);
+ vpHistogramValey &operator=(const vpHistogramValey &v);
bool operator==(const vpHistogramValey &v) const;
/*!
@@ -89,10 +85,7 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak
\sa setValue(), set()
*/
- inline void setLevel(unsigned char lvl)
- {
- this->level = lvl;
- };
+ inline void setLevel(unsigned char lvl) { this->level = lvl; };
/*!
Set the valey number of pixels having a same gray level. To set the
@@ -103,10 +96,7 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak
\sa setPosition(), set()
*/
- inline void setValue(unsigned val)
- {
- this->value = val;
- };
+ inline void setValue(unsigned val) { this->value = val; };
/*!
Set the valey gray \e level and number of pixels at this location.
@@ -118,11 +108,11 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak
*/
inline void set(unsigned char lvl, unsigned val)
- {
- this->level = lvl;
- this->value = val;
- };
-
+ {
+ this->level = lvl;
+ this->value = val;
+ };
+
/*!
Get the valey gray \e level. The number of pixels having this
@@ -133,10 +123,7 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak
\sa getValue()
*/
- inline unsigned char getLevel() const
- {
- return level;
- };
+ inline unsigned char getLevel() const { return level; };
/*!
Get the valey number of pixels having a same gray level. The
@@ -147,15 +134,12 @@ class VISP_EXPORT vpHistogramValey : vpHistogramPeak
\sa getLevel()
*/
- inline unsigned getValue() const
- {
- return value;
- };
+ inline unsigned getValue() const { return value; };
//---------------------------------
// 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 c8f3531..5d157ca 100644
--- a/modules/core/include/visp3/core/vpHomogeneousMatrix.h
+++ b/modules/core/include/visp3/core/vpHomogeneousMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,14 +36,11 @@
*
*****************************************************************************/
-
-
/*!
\file vpHomogeneousMatrix.h
\brief Definition and computation on the homogeneous matrices
*/
-
#ifndef VPHOMOGENEOUSMATRIX_HH
#define VPHOMOGENEOUSMATRIX_HH
@@ -55,8 +53,8 @@ class vpThetaUVector;
class vpQuaternionVector;
class vpPoint;
-#include <vector>
#include <fstream>
+#include <vector>
#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpRotationMatrix.h>
@@ -69,7 +67,8 @@ class vpPoint;
\ingroup group_core_transformations
- \brief Implementation of an homogeneous matrix and operations on such kind of matrices.
+ \brief Implementation of an homogeneous matrix and operations on such kind
+ of matrices.
The class provides a data structure for the homogeneous matrices
as well as a set of operations on these matrices.
@@ -92,30 +91,30 @@ class vpPoint;
*/
class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
{
- public:
+public:
vpHomogeneousMatrix();
- vpHomogeneousMatrix(const vpHomogeneousMatrix &M) ;
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu) ;
- vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q) ;
- vpHomogeneousMatrix(const vpPoseVector &p) ;
- vpHomogeneousMatrix(const std::vector<float> &v);
- vpHomogeneousMatrix(const std::vector<double> &v);
- vpHomogeneousMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
+ vpHomogeneousMatrix(const vpHomogeneousMatrix &M);
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpRotationMatrix &R);
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpThetaUVector &tu);
+ vpHomogeneousMatrix(const vpTranslationVector &t, const vpQuaternionVector &q);
+ explicit vpHomogeneousMatrix(const vpPoseVector &p);
+ explicit vpHomogeneousMatrix(const std::vector<float> &v);
+ explicit vpHomogeneousMatrix(const std::vector<double> &v);
+ vpHomogeneousMatrix(const double tx, const double ty, const double tz, const double tux, const double tuy,
+ const double tuz);
/*!
Destructor.
*/
- virtual ~vpHomogeneousMatrix() {};
+ virtual ~vpHomogeneousMatrix(){};
- void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R) ;
- void buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu) ;
- void buildFrom(const vpTranslationVector &t, const vpQuaternionVector& q) ;
- void buildFrom(const vpPoseVector &p) ;
- void buildFrom(const std::vector<float> &v) ;
- void buildFrom(const std::vector<double> &v) ;
- void buildFrom(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
+ void buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R);
+ void buildFrom(const vpTranslationVector &t, const vpThetaUVector &tu);
+ void buildFrom(const vpTranslationVector &t, const vpQuaternionVector &q);
+ void buildFrom(const vpPoseVector &p);
+ void buildFrom(const std::vector<float> &v);
+ void buildFrom(const std::vector<double> &v);
+ void buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy,
+ const double tuz);
void convert(std::vector<float> &M);
void convert(std::vector<double> &M);
@@ -125,31 +124,31 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
vpColVector getCol(const unsigned int j) const;
vpRotationMatrix getRotationMatrix() const;
- vpThetaUVector getThetaUVector() const ;
+ vpThetaUVector getThetaUVector() const;
vpTranslationVector getTranslationVector() const;
// Invert the homogeneous matrix.
- vpHomogeneousMatrix inverse() const ;
+ vpHomogeneousMatrix inverse() const;
// Invert the homogeneous matrix.
void inverse(vpHomogeneousMatrix &Mi) const;
// Test if the rotational part of the matrix is a rotation matrix.
- bool isAnHomogeneousMatrix() const ;
+ bool isAnHomogeneousMatrix() const;
- void insert(const vpRotationMatrix &R) ;
- void insert(const vpThetaUVector &tu) ;
- void insert(const vpTranslationVector &t) ;
- void insert(const vpQuaternionVector &t) ;
+ void insert(const vpRotationMatrix &R);
+ void insert(const vpThetaUVector &tu);
+ void insert(const vpTranslationVector &t);
+ void insert(const vpQuaternionVector &t);
void extract(vpRotationMatrix &R) const;
void extract(vpThetaUVector &tu) const;
void extract(vpTranslationVector &t) const;
- void extract(vpQuaternionVector& q) const;
+ void extract(vpQuaternionVector &q) const;
// Load an homogeneous matrix from a file
- void load(std::ifstream &f) ;
+ void load(std::ifstream &f);
// Save an homogeneous matrix in a file
- void save(std::ofstream &f) const ;
+ void save(std::ofstream &f) const;
vpHomogeneousMatrix &operator=(const vpHomogeneousMatrix &M);
vpHomogeneousMatrix operator*(const vpHomogeneousMatrix &M) const;
@@ -168,8 +167,7 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
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;
@@ -186,15 +184,14 @@ class VISP_EXPORT vpHomogeneousMatrix : public vpArray2D<double>
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
+ vp_deprecated void init(){};
/*!
\deprecated You should rather use eye().
*/
vp_deprecated void setIdentity();
- //@}
+//@}
#endif
-
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpImage.h b/modules/core/include/visp3/core/vpImage.h
index d41ec4c..598a5de 100644
--- a/modules/core/include/visp3/core/vpImage.h
+++ b/modules/core/include/visp3/core/vpImage.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpImage.h
\brief Image handling.
@@ -51,12 +51,12 @@
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpRGBa.h>
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
-# include <visp3/core/vpThread.h>
+#include <visp3/core/vpThread.h>
#endif
#include <fstream>
+#include <iomanip> // std::setw
#include <iostream>
-#include <iomanip> // std::setw
#include <math.h>
#include <string.h>
@@ -112,40 +112,43 @@ 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 Type>
-class vpImage; // forward declare to make function declaration possible
+// Ref: http://en.cppreference.com/w/cpp/language/friend#Template_friends
+template <class Type> class vpImage; // forward declare to make function declaration possible
// declarations
-template<class Type>
-std::ostream& operator<<(std::ostream&, const vpImage<Type>&);
+template <class Type> std::ostream &operator<<(std::ostream &, const vpImage<Type> &);
+
+std::ostream &operator<<(std::ostream &, const vpImage<unsigned char> &);
+std::ostream &operator<<(std::ostream &, const vpImage<char> &);
+std::ostream &operator<<(std::ostream &, const vpImage<float> &);
+std::ostream &operator<<(std::ostream &, const vpImage<double> &);
-std::ostream& operator<<(std::ostream&, const vpImage<unsigned char>&);
-std::ostream& operator<<(std::ostream&, const vpImage<char>&);
-std::ostream& operator<<(std::ostream&, const vpImage<float>&);
-std::ostream& operator<<(std::ostream&, const vpImage<double>&);
+template <class Type> void swap(vpImage<Type> &first, vpImage<Type> &second);
-template<class Type>
-class vpImage
+template <class Type> class vpImage
{
friend class vpImageConvert;
public:
- Type *bitmap ; //!< points toward the bitmap
- vpDisplay *display ;
+ Type *bitmap; //!< points toward the bitmap
+ vpDisplay *display;
//! constructor
- vpImage() ;
+ vpImage();
//! copy constructor
- vpImage(const vpImage<Type>&);
+ vpImage(const vpImage<Type> &);
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ //! move constructor
+ vpImage(vpImage<Type> &&);
+#endif
//! constructor set the size of the image
- vpImage(unsigned int height, unsigned int width) ;
+ vpImage(unsigned int height, unsigned int width);
//! constructor set the size of the image and init all the pixel
- vpImage(unsigned int height, unsigned int width, Type value) ;
+ 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() ;
+ virtual ~vpImage();
/** @name Inherited functionalities from vpImage */
//@{
@@ -163,7 +166,7 @@ public:
\sa getWidth()
*/
- inline unsigned int getCols() const { return width ; }
+ inline unsigned int getCols() const { return width; }
/*!
Get the image height.
@@ -172,13 +175,13 @@ public:
\sa getWidth()
*/
- inline unsigned int getHeight() const { return height; }
+ inline unsigned int getHeight() const { return height; }
// Return the maximum value within the bitmap
- Type getMaxValue() const ;
+ Type getMaxValue() const;
// Return the minumum value within the bitmap
- Type getMinValue() const ;
- //Look for the minumum and the maximum value within the bitmap
+ Type getMinValue() const;
+ // Look for the minumum and the maximum value within the bitmap
void getMinMaxValue(Type &min, Type &max) const;
/*!
@@ -191,7 +194,7 @@ public:
\sa getWidth(), getHeight()
*/
- inline unsigned int getNumberOfPixel() const{ return npixels; }
+ inline unsigned int getNumberOfPixel() const { return npixels; }
/*!
@@ -201,7 +204,7 @@ public:
\sa getHeight()
*/
- inline unsigned int getRows() const { return height ; }
+ inline unsigned int getRows() const { return height; }
/*!
Get the image size.
@@ -209,7 +212,7 @@ public:
\sa getWidth(), getHeight()
*/
- inline unsigned int getSize() const { return width*height ; }
+ inline unsigned int getSize() const { return width * height; }
// Gets the value of a pixel at a location with bilinear interpolation.
Type getValue(double i, double j) const;
@@ -223,51 +226,44 @@ public:
\sa getHeight()
*/
- inline unsigned int getWidth() const { return width; }
+ inline unsigned int getWidth() const { return width; }
// Returns a new image that's half size of the current image
void halfSizeImage(vpImage<Type> &res) const;
//! Set the size of the image
- void init(unsigned int height, unsigned int width) ;
+ void init(unsigned int height, unsigned int width);
//! Set the size of the image
- void init(unsigned int height, unsigned int width, Type value) ;
+ 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 insert(const vpImage<Type> &src, const vpImagePoint topLeft);
+ void init(Type *const array, const unsigned int height, const unsigned int width, const bool copyData = false);
+ void insert(const vpImage<Type> &src, const vpImagePoint &topLeft);
//------------------------------------------------------------------
// Acces to the image
//! operator[] allows operation like I[i] = x.
- inline Type *operator[]( const unsigned int i) { return row[i];}
- inline Type *operator[]( const int i) { return row[i];}
+ inline Type *operator[](const unsigned int i) { return row[i]; }
+ inline Type *operator[](const int i) { return row[i]; }
//! operator[] allows operation like x = I[i]
- inline const Type *operator[](unsigned int i) const { return row[i];}
- inline const Type *operator[](int i) const { return row[i];}
+ inline const Type *operator[](unsigned int i) const { return row[i]; }
+ inline const Type *operator[](int i) const { return row[i]; }
/*!
- Get the value of an image point with coordinates (i, j), with i the row position and j
- the column position.
+ Get the value of an image point with coordinates (i, j), with i the row
+ position and j the column position.
\return Value of the image point (i, j).
*/
- inline Type operator()(const unsigned int i, const unsigned int j) const
- {
- return bitmap[i*width+j] ;
- }
+ 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.
+ 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.
@@ -281,10 +277,10 @@ public:
*/
inline Type operator()(const vpImagePoint &ip) const
{
- unsigned int i = (unsigned int) ip.get_i();
- unsigned int j = (unsigned int) ip.get_j();
+ unsigned int i = (unsigned int)ip.get_i();
+ unsigned int j = (unsigned int)ip.get_j();
- return bitmap[i*width+j] ;
+ return bitmap[i * width + j];
}
/*!
Set the value of an image point.
@@ -297,28 +293,28 @@ public:
*/
inline void operator()(const vpImagePoint &ip, const Type &v)
{
- unsigned int i = (unsigned int) ip.get_i();
- unsigned int j = (unsigned int) ip.get_j();
+ unsigned int i = (unsigned int)ip.get_i();
+ unsigned int j = (unsigned int)ip.get_j();
- bitmap[i*width+j] = v ;
+ bitmap[i * width + j] = v;
}
vpImage<Type> operator-(const vpImage<Type> &B);
//! Copy operator
- vpImage<Type>& operator=(const vpImage<Type> &I);
+ vpImage<Type> &operator=(vpImage<Type> other);
- vpImage<Type>& operator=(const Type &v);
+ vpImage<Type> &operator=(const Type &v);
bool operator==(const vpImage<Type> &I);
bool operator!=(const vpImage<Type> &I);
- friend std::ostream& operator<< <> (std::ostream &s, const vpImage<Type> &I);
- friend std::ostream& operator<<(std::ostream &s, const vpImage<unsigned char> &I);
- friend std::ostream& operator<<(std::ostream &s, const vpImage<char> &I);
- friend std::ostream& operator<<(std::ostream &s, const vpImage<float> &I);
- friend std::ostream& operator<<(std::ostream &s, const vpImage<double> &I);
+ friend std::ostream &operator<<<>(std::ostream &s, const vpImage<Type> &I);
+ friend std::ostream &operator<<(std::ostream &s, const vpImage<unsigned char> &I);
+ friend std::ostream &operator<<(std::ostream &s, const vpImage<char> &I);
+ friend std::ostream &operator<<(std::ostream &s, const vpImage<float> &I);
+ friend std::ostream &operator<<(std::ostream &s, const vpImage<double> &I);
// Perform a look-up table transformation
- void performLut(const Type (&lut)[256], const unsigned int nbThreads=1);
+ void performLut(const Type (&lut)[256], const unsigned int nbThreads = 1);
// Returns a new image that's a quarter size of the current image
void quarterSizeImage(vpImage<Type> &res) const;
@@ -326,37 +322,39 @@ public:
// set the size of the image without initializing it.
void resize(const unsigned int h, const unsigned int w);
// set the size of the image and initialize it.
- void resize(const unsigned int h, const unsigned int w, const Type val);
+ void resize(const unsigned int h, const unsigned int w, const Type &val);
void sub(const vpImage<Type> &B, vpImage<Type> &C);
void sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C);
void subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Type> &sampled) const;
+ friend void swap<>(vpImage<Type> &first, vpImage<Type> &second);
+
//@}
private:
- unsigned int npixels ; ///! number of pixel in the image
- unsigned int width ; ///! number of columns
- unsigned int height ; ///! number of rows
- Type **row ; //!< points the row pointer array
+ unsigned int npixels; ///! number of pixel in the image
+ unsigned int width; ///! number of columns
+ unsigned int height; ///! number of rows
+ Type **row; ///! points the row pointer array
};
-template<class Type>
-std::ostream& operator<<(std::ostream &s, const vpImage<Type> &I) {
+template <class Type> std::ostream &operator<<(std::ostream &s, const vpImage<Type> &I)
+{
if (I.bitmap == NULL) {
return s;
}
for (unsigned int i = 0; i < I.getHeight(); i++) {
- for (unsigned int j = 0; j < I.getWidth()-1; j++) {
+ for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
s << I[i][j] << " ";
}
// We don't add " " after the last column element
- s << I[i][I.getWidth() -1];
+ s << I[i][I.getWidth() - 1];
// We don't add a \n character at the end of the last row line
- if (i < I.getHeight()-1) {
+ if (i < I.getHeight() - 1) {
s << std::endl;
}
}
@@ -364,7 +362,8 @@ std::ostream& operator<<(std::ostream &s, const vpImage<Type> &I) {
return s;
}
-inline std::ostream& operator<<(std::ostream &s, const vpImage<unsigned char> &I) {
+inline std::ostream &operator<<(std::ostream &s, const vpImage<unsigned char> &I)
+{
if (I.bitmap == NULL) {
return s;
}
@@ -372,15 +371,15 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<unsigned char> &I
std::ios_base::fmtflags original_flags = s.flags();
for (unsigned int i = 0; i < I.getHeight(); i++) {
- for (unsigned int j = 0; j < I.getWidth()-1; j++) {
+ for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
s << std::setw(3) << static_cast<unsigned>(I[i][j]) << " ";
}
// We don't add " " after the last column element
- s << std::setw(3) << static_cast<unsigned>(I[i][I.getWidth() -1]);
+ s << std::setw(3) << static_cast<unsigned>(I[i][I.getWidth() - 1]);
// We don't add a \n character at the end of the last row line
- if (i < I.getHeight()-1) {
+ if (i < I.getHeight() - 1) {
s << std::endl;
}
}
@@ -389,7 +388,8 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<unsigned char> &I
return s;
}
-inline std::ostream& operator<<(std::ostream &s, const vpImage<char> &I) {
+inline std::ostream &operator<<(std::ostream &s, const vpImage<char> &I)
+{
if (I.bitmap == NULL) {
return s;
}
@@ -397,15 +397,15 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<char> &I) {
std::ios_base::fmtflags original_flags = s.flags();
for (unsigned int i = 0; i < I.getHeight(); i++) {
- for (unsigned int j = 0; j < I.getWidth()-1; j++) {
- s <<std::setw(4) << static_cast<int>(I[i][j]) << " ";
+ for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
+ s << std::setw(4) << static_cast<int>(I[i][j]) << " ";
}
// We don't add " " after the last column element
- s << std::setw(4) << static_cast<int>(I[i][I.getWidth() -1]);
+ s << std::setw(4) << static_cast<int>(I[i][I.getWidth() - 1]);
// We don't add a \n character at the end of the last row line
- if (i < I.getHeight()-1) {
+ if (i < I.getHeight() - 1) {
s << std::endl;
}
}
@@ -414,24 +414,25 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<char> &I) {
return s;
}
-inline std::ostream& operator<<(std::ostream &s, const vpImage<float> &I) {
+inline std::ostream &operator<<(std::ostream &s, const vpImage<float> &I)
+{
if (I.bitmap == NULL) {
return s;
}
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++) {
+ for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
s << I[i][j] << " ";
}
// We don't add " " after the last column element
- s << I[i][I.getWidth() -1];
+ s << I[i][I.getWidth() - 1];
// We don't add a \n character at the end of the last row line
- if (i < I.getHeight()-1) {
+ if (i < I.getHeight() - 1) {
s << std::endl;
}
}
@@ -440,24 +441,25 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<float> &I) {
return s;
}
-inline std::ostream& operator<<(std::ostream &s, const vpImage<double> &I) {
+inline std::ostream &operator<<(std::ostream &s, const vpImage<double> &I)
+{
if (I.bitmap == NULL) {
return s;
}
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++) {
+ for (unsigned int j = 0; j < I.getWidth() - 1; j++) {
s << I[i][j] << " ";
}
// We don't add " " after the last column element
- s << I[i][I.getWidth() -1];
+ s << I[i][I.getWidth() - 1];
// We don't add a \n character at the end of the last row line
- if (i < I.getHeight()-1) {
+ if (i < I.getHeight() - 1) {
s << std::endl;
}
}
@@ -466,150 +468,146 @@ inline std::ostream& operator<<(std::ostream &s, const vpImage<double> &I) {
return s;
}
-
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
-namespace {
- struct ImageLut_Param_t {
- unsigned int m_start_index;
- unsigned int m_end_index;
-
- unsigned char m_lut[256];
- unsigned char *m_bitmap;
+namespace
+{
+struct ImageLut_Param_t {
+ unsigned int m_start_index;
+ unsigned int m_end_index;
- ImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {
- }
+ unsigned char m_lut[256];
+ unsigned char *m_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) {
- }
- };
+ ImageLut_Param_t() : m_start_index(0), m_end_index(0), m_lut(), m_bitmap(NULL) {}
- vpThread::Return performLutThread(vpThread::Args args) {
- ImageLut_Param_t *imageLut_param = ( (ImageLut_Param_t *) args );
- unsigned int start_index = imageLut_param->m_start_index;
- unsigned int end_index = imageLut_param->m_end_index;
-
- unsigned char *bitmap = imageLut_param->m_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)
+ {
+ }
+};
- unsigned char *ptrStart = bitmap + start_index;
- unsigned char *ptrEnd = bitmap + end_index;
- unsigned char *ptrCurrent = ptrStart;
+vpThread::Return performLutThread(vpThread::Args args)
+{
+ ImageLut_Param_t *imageLut_param = static_cast<ImageLut_Param_t *>(args);
+ unsigned int start_index = imageLut_param->m_start_index;
+ unsigned int end_index = imageLut_param->m_end_index;
+ unsigned char *bitmap = imageLut_param->m_bitmap;
-// while(ptrCurrent != ptrEnd) {
-// *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
-// ++ptrCurrent;
-// }
+ unsigned char *ptrStart = bitmap + start_index;
+ unsigned char *ptrEnd = bitmap + end_index;
+ unsigned char *ptrCurrent = ptrStart;
- if(end_index - start_index >= 8) {
- //Unroll loop version
- for(; ptrCurrent <= ptrEnd - 8;) {
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ // while(ptrCurrent != ptrEnd) {
+ // *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ // ++ptrCurrent;
+ // }
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ if (end_index - start_index >= 8) {
+ // Unroll loop version
+ for (; ptrCurrent <= ptrEnd - 8;) {
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
- ++ptrCurrent;
- }
- }
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
- for(; ptrCurrent != ptrEnd; ++ptrCurrent) {
*ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
+ ++ptrCurrent;
}
+ }
- return 0;
+ for (; ptrCurrent != ptrEnd; ++ptrCurrent) {
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent];
}
+ return 0;
+}
- struct ImageLutRGBa_Param_t {
- unsigned int m_start_index;
- unsigned int m_end_index;
+struct ImageLutRGBa_Param_t {
+ unsigned int m_start_index;
+ unsigned int m_end_index;
- vpRGBa m_lut[256];
- unsigned char *m_bitmap;
+ 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) {
- }
- };
-
- vpThread::Return performLutRGBaThread(vpThread::Args args) {
- ImageLutRGBa_Param_t *imageLut_param = ( (ImageLutRGBa_Param_t *) args );
- unsigned int start_index = imageLut_param->m_start_index;
- unsigned int end_index = imageLut_param->m_end_index;
-
- unsigned char *bitmap = imageLut_param->m_bitmap;
-
- unsigned char *ptrStart = bitmap + start_index*4;
- unsigned char *ptrEnd = bitmap + end_index*4;
- unsigned char *ptrCurrent = ptrStart;
-
-
- if(end_index - start_index >= 4*2) {
- //Unroll loop version
- for(; ptrCurrent <= ptrEnd - 4*2;) {
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
- ptrCurrent++;
-
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
- ptrCurrent++;
- *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
- ptrCurrent++;
- }
- }
+ 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<ImageLutRGBa_Param_t *>(args);
+ unsigned int start_index = imageLut_param->m_start_index;
+ unsigned int end_index = imageLut_param->m_end_index;
+
+ unsigned char *bitmap = imageLut_param->m_bitmap;
+
+ unsigned char *ptrStart = bitmap + start_index * 4;
+ unsigned char *ptrEnd = bitmap + end_index * 4;
+ unsigned char *ptrCurrent = ptrStart;
- while(ptrCurrent != ptrEnd) {
+ if (end_index - start_index >= 4 * 2) {
+ // Unroll loop version
+ for (; ptrCurrent <= ptrEnd - 4 * 2;) {
*ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
ptrCurrent++;
-
*ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
ptrCurrent++;
-
*ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
ptrCurrent++;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
+ ptrCurrent++;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
+ ptrCurrent++;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
+ ptrCurrent++;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
+ ptrCurrent++;
*ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
ptrCurrent++;
}
+ }
+
+ while (ptrCurrent != ptrEnd) {
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].R;
+ ptrCurrent++;
+
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].G;
+ ptrCurrent++;
- return 0;
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].B;
+ ptrCurrent++;
+
+ *ptrCurrent = imageLut_param->m_lut[*ptrCurrent].A;
+ ptrCurrent++;
}
+
+ return 0;
+}
}
#endif
-
/*!
\brief Image initialisation
@@ -623,25 +621,14 @@ namespace {
\sa vpImage::init(h, w)
*/
-template<class Type>
-void
-vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
+template <class Type> void vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
{
- try
- {
- init(h,w) ;
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
+ init(h, w);
- for (unsigned int i=0 ; i < npixels ; i++)
- bitmap[i] = value ;
+ for (unsigned int i = 0; i < npixels; i++)
+ bitmap[i] = value;
}
-
/*!
\brief Image initialization
@@ -659,54 +646,45 @@ vpImage<Type>::init(unsigned int h, unsigned int w, Type value)
\exception vpException::memoryAllocationError
*/
-template<class Type>
-void
-vpImage<Type>::init(unsigned int h, unsigned int w)
+template <class Type> void vpImage<Type>::init(unsigned int h, unsigned int w)
{
if (h != this->height) {
- if (row != NULL) {
- vpDEBUG_TRACE(10,"Destruction row[]");
- delete [] row;
+ if (row != NULL) {
+ vpDEBUG_TRACE(10, "Destruction row[]");
+ delete[] row;
row = NULL;
}
}
- if ((h != this->height) || (w != this->width))
- {
+ if ((h != this->height) || (w != this->width)) {
if (bitmap != NULL) {
- vpDEBUG_TRACE(10,"Destruction bitmap[]") ;
- delete [] bitmap;
+ vpDEBUG_TRACE(10, "Destruction bitmap[]");
+ delete[] bitmap;
bitmap = NULL;
}
}
- this->width = w ;
+ this->width = w;
this->height = h;
- npixels=width*height;
-
- if (bitmap == NULL) bitmap = new Type[npixels] ;
+ npixels = width * height;
- // vpERROR_TRACE("Allocate bitmap %p",bitmap) ;
if (bitmap == NULL)
- {
- vpERROR_TRACE("cannot allocate bitmap ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate bitmap ")) ;
+ bitmap = new Type[npixels];
+
+ if (bitmap == NULL) {
+ throw(vpException(vpException::memoryAllocationError, "cannot allocate bitmap "));
}
- if (row == NULL) row = new Type*[height] ;
-// vpERROR_TRACE("Allocate row %p",row) ;
if (row == NULL)
- {
- vpERROR_TRACE("cannot allocate row ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate row ")) ;
+ row = new Type *[height];
+ if (row == NULL) {
+ throw(vpException(vpException::memoryAllocationError, "cannot allocate row "));
}
- unsigned int i ;
- for ( i =0 ; i < height ; i++)
- row[i] = bitmap + i*width ;
+ unsigned int i;
+ for (i = 0; i < height; i++)
+ row[i] = bitmap + i * width;
}
/*!
@@ -717,57 +695,57 @@ vpImage<Type>::init(unsigned int h, unsigned int w)
\param array : Image data stored as a continuous array in memory
\param h : Image height.
\param w : Image width.
- \param copyData : If false (by default) only the memory address is copied, otherwise the data are copied.
+ \param copyData : If false (by default) only the memory address is copied,
+ otherwise the data are copied.
\exception vpException::memoryAllocationError
*/
-template<class Type>
-void
-vpImage<Type>::init(Type * const array, const unsigned int h, const unsigned int w, const bool copyData)
+template <class Type>
+void vpImage<Type>::init(Type *const array, const unsigned int h, const unsigned int w, const bool copyData)
{
if (h != this->height) {
- if (row != NULL) {
- delete [] row;
+ if (row != NULL) {
+ delete[] row;
row = NULL;
}
}
- //Delete bitmap if copyData==false, otherwise only if the dimension differs
- if ( (copyData && ((h != this->height) || (w != this->width))) || !copyData ) {
+ // Delete bitmap if copyData==false, otherwise only if the dimension differs
+ if ((copyData && ((h != this->height) || (w != this->width))) || !copyData) {
if (bitmap != NULL) {
- delete [] bitmap;
+ delete[] bitmap;
bitmap = NULL;
}
}
- this->width = w ;
+ this->width = w;
this->height = h;
- npixels = width*height;
+ npixels = width * height;
- if(copyData) {
- if (bitmap == NULL) bitmap = new Type[npixels];
+ if (copyData) {
+ if (bitmap == NULL)
+ 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
- memcpy(bitmap, array, (size_t) (npixels * sizeof(Type)));
+ // Copy the image data
+ memcpy(bitmap, array, (size_t)(npixels * sizeof(Type)));
} else {
- //Copy the address of the array in the bitmap
+ // Copy the address of the array in the bitmap
bitmap = array;
}
- if (row == NULL) row = new Type*[height];
+ 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++) {
- row[i] = bitmap + i*width;
+ for (unsigned int i = 0; i < height; i++) {
+ row[i] = bitmap + i * width;
}
}
@@ -789,18 +767,11 @@ vpImage<Type>::init(Type * const array, const unsigned int h, const unsigned int
\sa vpImage::init(height, width)
*/
-template<class Type>
+template <class Type>
vpImage<Type>::vpImage(unsigned int h, unsigned int w)
: bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
- try
- {
- init(h,w,0) ;
- }
- catch(...)
- {
- throw ;
- }
+ init(h, w, 0);
}
/*!
@@ -820,19 +791,11 @@ vpImage<Type>::vpImage(unsigned int h, unsigned int w)
\sa vpImage::init(height, width, value)
*/
-template<class Type>
-vpImage<Type>::vpImage (unsigned int h, unsigned int w, Type value)
+template <class Type>
+vpImage<Type>::vpImage(unsigned int h, unsigned int w, Type value)
: bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
- try
- {
- init(h,w,value) ;
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
+ init(h, w, value);
}
/*!
@@ -843,24 +806,18 @@ vpImage<Type>::vpImage (unsigned int h, unsigned int w, Type value)
\param array : Image data stored as a continuous array in memory.
\param h : Image height.
\param w : Image width.
- \param copyData : If false (by default) only the memory address is copied, otherwise the data are copied.
+ \param copyData : If false (by default) only the memory address is copied,
+ otherwise the data are copied.
\return MEMORY_FAULT if memory allocation is impossible, else OK
\sa vpImage::init(array, height, width)
*/
-template<class Type>
-vpImage<Type>::vpImage (Type * const array, const unsigned int h, const unsigned int w, const bool copyData)
+template <class Type>
+vpImage<Type>::vpImage(Type *const array, const unsigned int h, const unsigned int w, const bool copyData)
: bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
- try
- {
- init(array, h, w, copyData);
- }
- catch(vpException &)
- {
- throw ;
- }
+ init(array, h, w, copyData);
}
/*!
@@ -872,9 +829,7 @@ vpImage<Type>::vpImage (Type * const array, const unsigned int h, const unsigned
\sa vpImage::resize(height, width) for memory allocation
*/
-template<class Type>
-vpImage<Type>::vpImage()
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+template <class Type> vpImage<Type>::vpImage() : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
}
@@ -898,20 +853,7 @@ vpImage<Type>::vpImage()
\sa init(unsigned int, unsigned int)
*/
-template<class Type>
-void
-vpImage<Type>::resize(unsigned int h, unsigned int w)
-{
- try
- {
- init(h, w) ;
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
+template <class Type> void vpImage<Type>::resize(unsigned int h, unsigned int w) { init(h, w); }
/*!
\brief resize the image : Image initialization
@@ -932,21 +874,7 @@ vpImage<Type>::resize(unsigned int h, unsigned int w)
\sa init(unsigned int, unsigned int)
*/
-template<class Type>
-void
-vpImage<Type>::resize(unsigned int h, unsigned int w, const Type val)
-{
- try
- {
- init(h, w, val) ;
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
-}
-
+template <class Type> void vpImage<Type>::resize(unsigned int h, unsigned int w, const Type &val) { init(h, w, val); }
/*!
\brief Destructor : Memory de-allocation
@@ -954,30 +882,23 @@ vpImage<Type>::resize(unsigned int h, unsigned int w, const Type val)
\warning does not deallocate memory for display and video
*/
-template<class Type>
-void
-vpImage<Type>::destroy()
+template <class Type> void vpImage<Type>::destroy()
{
- // vpERROR_TRACE("Deallocate ") ;
-
+ // vpERROR_TRACE("Deallocate ");
- if (bitmap!=NULL)
- {
- // vpERROR_TRACE("Deallocate bitmap memory %p",bitmap) ;
-// vpDEBUG_TRACE(20,"Deallocate bitmap memory %p",bitmap) ;
- delete [] bitmap ;
+ if (bitmap != NULL) {
+ // vpERROR_TRACE("Deallocate bitmap memory %p",bitmap);
+ // vpDEBUG_TRACE(20,"Deallocate bitmap memory %p",bitmap);
+ delete[] bitmap;
bitmap = NULL;
}
-
- if (row!=NULL)
- {
- // vpERROR_TRACE("Deallocate row memory %p",row) ;
-// vpDEBUG_TRACE(20,"Deallocate row memory %p",row) ;
- delete [] row ;
+ if (row != NULL) {
+ // vpERROR_TRACE("Deallocate row memory %p",row);
+ // vpDEBUG_TRACE(20,"Deallocate row memory %p",row);
+ delete[] row;
row = NULL;
}
-
}
/*!
@@ -986,48 +907,48 @@ vpImage<Type>::destroy()
\warning does not deallocate memory for display and video
*/
-template<class Type>
-vpImage<Type>::~vpImage()
+template <class Type> vpImage<Type>::~vpImage() { destroy(); }
+
+/*!
+ Copy constructor
+*/
+template <class Type>
+vpImage<Type>::vpImage(const vpImage<Type> &I) : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
{
- destroy() ;
+ resize(I.getHeight(), I.getWidth());
+ memcpy(bitmap, I.bitmap, I.npixels * sizeof(Type));
}
-
-
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
/*!
- Copy constructor
+ Move constructor
*/
-template<class Type>
-vpImage<Type>::vpImage(const vpImage<Type>& I)
- : bitmap(NULL), display(NULL), npixels(0), width(0), height(0), row(NULL)
+template <class Type>
+vpImage<Type>::vpImage(vpImage<Type> &&I)
+ : bitmap(I.bitmap), display(I.display), npixels(I.npixels), width(I.width), height(I.height), row(I.row)
{
- try
- {
- resize(I.getHeight(),I.getWidth());
- memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
- for (unsigned int i =0 ; i < this->height ; i++) row[i] = bitmap + i*this->width ;
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
+ I.bitmap = NULL;
+ I.display = NULL;
+ I.npixels = 0;
+ I.width = 0;
+ I.height = 0;
+ I.row = NULL;
}
+#endif
/*!
\brief Return the maximum value within the bitmap
\sa getMinValue()
*/
-template<class Type>
-Type vpImage<Type>::getMaxValue() const
+template <class Type> Type vpImage<Type>::getMaxValue() const
{
- Type m = bitmap[0] ;
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i]>m) m = bitmap[i] ;
+ Type m = bitmap[0];
+ for (unsigned int i = 0; i < npixels; i++) {
+ if (bitmap[i] > m)
+ m = bitmap[i];
}
- return m ;
+ return m;
}
/*!
@@ -1035,102 +956,58 @@ Type vpImage<Type>::getMaxValue() const
\sa getMaxValue()
*/
-template<class Type>
-Type vpImage<Type>::getMinValue() const
+template <class Type> Type vpImage<Type>::getMinValue() const
{
- Type m = bitmap[0];
- for (unsigned int i=0 ; i < npixels ; i++)
- if (bitmap[i]<m) m = bitmap[i] ;
- return m ;
+ Type m = bitmap[0];
+ for (unsigned int i = 0; i < npixels; i++)
+ if (bitmap[i] < m)
+ m = bitmap[i];
+ return m;
}
-
/*!
\brief Look for the minimum and the maximum value within the bitmap
\sa getMaxValue()
\sa getMinValue()
*/
-template<class Type>
-void vpImage<Type>::getMinMaxValue(Type &min, Type &max) const
+template <class Type> void vpImage<Type>::getMinMaxValue(Type &min, Type &max) const
{
- min = max = bitmap[0];
- for (unsigned int i=0 ; i < npixels ; i++)
- {
- if (bitmap[i]<min) min = bitmap[i] ;
- if (bitmap[i]>max) max = bitmap[i] ;
+ min = max = bitmap[0];
+ for (unsigned int i = 0; i < npixels; i++) {
+ if (bitmap[i] < min)
+ min = bitmap[i];
+ if (bitmap[i] > max)
+ max = bitmap[i];
}
}
/*!
\brief Copy operator
*/
-template<class Type>
-vpImage<Type> & vpImage<Type>::operator=(const vpImage<Type> &I)
+template <class Type> vpImage<Type> &vpImage<Type>::operator=(vpImage<Type> other)
{
- /* we first have to set the initial values of the image because resize function calls init function that test the actual size of the image */
- if(bitmap != NULL){
- delete[] bitmap;
- bitmap = NULL ;
- }
+ swap(*this, other);
+ // Swap back display pointer if it was not null
+ // vpImage<unsigned char> I2(480, 640);
+ // vpDisplayX d(I2);
+ // I2 = I1; //copy only the data
+ if (other.display != NULL)
+ display = other.display;
- if(row != NULL){
- delete[] row;
- row = NULL ;
- }
- this->width = I.width;
- this->height = I.height;
- this->npixels = I.npixels;
- try
- {
- if(I.npixels != 0)
- {
- if (bitmap == NULL){
- bitmap = new Type[npixels] ;
- }
-
- if (bitmap == NULL){
- vpERROR_TRACE("cannot allocate bitmap ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate bitmap ")) ;
- }
-
- if (row == NULL){
- row = new Type*[height] ;
- }
- if (row == NULL){
- vpERROR_TRACE("cannot allocate row ") ;
- throw(vpException(vpException::memoryAllocationError,
- "cannot allocate row ")) ;
- }
-
- memcpy(bitmap, I.bitmap, I.npixels*sizeof(Type)) ;
-
- for (unsigned int i=0; i<this->height; i++){
- row[i] = bitmap + i*this->width;
- }
- }
- }
- catch(vpException &)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
- return (* this);
+ return *this;
}
-
/*!
- \brief = operator : Set all the element of the bitmap to a given value \e v.
- \f$ A = v <=> A[i][j] = v \f$
+ \brief = operator : Set all the element of the bitmap to a given value \e
+ v. \f$ A = v <=> A[i][j] = v \f$
\warning = must be defined for \f$ <\f$ Type \f$ > \f$
*/
-template<class Type>
-vpImage<Type>& vpImage<Type>::operator=(const Type &v)
+template <class Type> vpImage<Type> &vpImage<Type>::operator=(const Type &v)
{
- for (unsigned int i=0 ; i < npixels ; i++)
- bitmap[i] = v ;
+ for (unsigned int i = 0; i < npixels; i++)
+ bitmap[i] = v;
return *this;
}
@@ -1140,43 +1017,42 @@ vpImage<Type>& vpImage<Type>::operator=(const Type &v)
\return true if the images are the same, false otherwise.
*/
-template<class Type>
-bool vpImage<Type>::operator==(const vpImage<Type> &I)
+template <class Type> bool vpImage<Type>::operator==(const vpImage<Type> &I)
{
if (this->width != I.getWidth())
return false;
if (this->height != I.getHeight())
return false;
-// printf("wxh: %dx%d bitmap: %p I.bitmap %p\n", width, height, bitmap, I.bitmap);
- for (unsigned int i=0 ; i < npixels ; i++)
- {
+ // printf("wxh: %dx%d bitmap: %p I.bitmap %p\n", width, height, bitmap,
+ // I.bitmap);
+ for (unsigned int i = 0; i < npixels; i++) {
if (bitmap[i] != I.bitmap[i]) {
-// std::cout << "differ for pixel " << i << " (" << i%this->height << ", " << i - i%this->height << ")" << std::endl;
+ // std::cout << "differ for pixel " << i << " (" << i%this->height
+ // << ", " << i - i%this->height << ")" << std::endl;
return false;
}
}
- return true ;
+ return true;
}
/*!
Compare two images.
\return true if the images are different, false if they are the same.
*/
-template<class Type>
-bool vpImage<Type>::operator!=(const vpImage<Type> &I)
+template <class Type> bool vpImage<Type>::operator!=(const vpImage<Type> &I)
{
-// if (this->width != I.getWidth())
-// return true;
-// if (this->height != I.getHeight())
-// return true;
-
-// for (unsigned int i=0 ; i < npixels ; i++)
-// {
-// if (bitmap[i] != I.bitmap[i])
-// return true;
-// }
-// return false ;
+ // if (this->width != I.getWidth())
+ // return true;
+ // if (this->height != I.getHeight())
+ // return true;
+
+ // for (unsigned int i=0 ; i < npixels ; i++)
+ // {
+ // if (bitmap[i] != I.bitmap[i])
+ // return true;
+ // }
+ // return false;
return !(*this == I);
}
@@ -1205,11 +1081,10 @@ int main()
\sa sub(const vpImage<Type> &, const vpImage<Type> &, vpImage<Type> &) to
avoid matrix allocation for each use.
*/
-template<class Type>
-vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
+template <class Type> vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
{
vpImage<Type> C;
- sub(*this,B,C);
+ sub(*this, B, C);
return C;
}
@@ -1217,17 +1092,15 @@ vpImage<Type> vpImage<Type>::operator-(const vpImage<Type> &B)
Insert an image into another one.
It is possible to insert the image \f$ src \f$ into the calling vpImage.
- You can set the point in the destination image where the top left corner of the \f$ src \f$ image will belocated.
+ You can set the point in the destination image where the top left corner of
+ the \f$ src \f$ image will belocated.
\param src : Image to insert
- \param topLeft : Upper/left coordinates in the image where the image \e src is inserted in the destination image.
+ \param topLeft : Upper/left coordinates in the image where the image \e src
+ is inserted in the destination image.
*/
-template<class Type>
-void vpImage<Type>::insert(const vpImage<Type> &src, const vpImagePoint topLeft)
+template <class Type> void vpImage<Type>::insert(const vpImage<Type> &src, const vpImagePoint &topLeft)
{
- Type* srcBitmap;
- Type* destBitmap;
-
int itl = (int)topLeft.get_i();
int jtl = (int)topLeft.get_j();
@@ -1265,12 +1138,11 @@ void vpImage<Type>::insert(const vpImage<Type> &src, const vpImagePoint topLeft)
else
hsize = src_h - src_ibegin;
- for (int i = 0; i < hsize; i++)
- {
- srcBitmap = src.bitmap + ((src_ibegin+i)*src_w+src_jbegin);
- destBitmap = this->bitmap + ((dest_ibegin+i)*dest_w+dest_jbegin);
+ 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);
- memcpy(destBitmap, srcBitmap, (size_t)wsize*sizeof(Type));
+ memcpy(destBitmap, srcBitmap, (size_t)wsize * sizeof(Type));
}
}
@@ -1304,16 +1176,14 @@ void vpImage<Type>::insert(const vpImage<Type> &src, const vpImagePoint topLeft)
\sa subsample()
*/
-template<class Type>
-void
-vpImage<Type>::halfSizeImage(vpImage<Type> &res) const
+template <class Type> void vpImage<Type>::halfSizeImage(vpImage<Type> &res) const
{
- unsigned int h = height/2;
- unsigned int w = width/2;
+ unsigned int h = height / 2;
+ unsigned int w = width / 2;
res.resize(h, w);
- for(unsigned int i = 0; i < h; i++)
- for(unsigned int j = 0; j < w; j++)
- res[i][j] = (*this)[i<<1][j<<1];
+ for (unsigned int i = 0; i < h; i++)
+ for (unsigned int j = 0; j < w; j++)
+ res[i][j] = (*this)[i << 1][j << 1];
}
/*!
@@ -1333,16 +1203,15 @@ vpImage<Type>::halfSizeImage(vpImage<Type> &res) const
vpImageIo::write(I2, "myHalfSizeImage.pgm");
\endcode
*/
-template<class Type>
-void
-vpImage<Type>::subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Type> &sampled) const
+template <class Type>
+void vpImage<Type>::subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Type> &sampled) const
{
- unsigned int h = height/v_scale;
- unsigned int w = width/h_scale;
+ unsigned int h = height / v_scale;
+ unsigned int w = width / h_scale;
sampled.resize(h, w);
- for(unsigned int i = 0; i < h; i++)
- for(unsigned int j = 0; j < w; j++)
- sampled[i][j] = (*this)[i*v_scale][j*h_scale];
+ for (unsigned int i = 0; i < h; i++)
+ for (unsigned int j = 0; j < w; j++)
+ sampled[i][j] = (*this)[i * v_scale][j * h_scale];
}
/*!
@@ -1369,16 +1238,14 @@ vpImage<Type>::subsample(unsigned int v_scale, unsigned int h_scale, vpImage<Typ
*/
-template<class Type>
-void
-vpImage<Type>::quarterSizeImage(vpImage<Type> &res) const
+template <class Type> void vpImage<Type>::quarterSizeImage(vpImage<Type> &res) const
{
- unsigned int h = height/4;
- unsigned int w = width/4;
+ unsigned int h = height / 4;
+ unsigned int w = width / 4;
res.resize(h, w);
- for(unsigned int i = 0; i < h; i++)
- for(unsigned int j = 0; j < w; j++)
- res[i][j] = (*this)[i<<2][j<<2];
+ for (unsigned int i = 0; i < h; i++)
+ for (unsigned int j = 0; j < w; j++)
+ res[i][j] = (*this)[i << 2][j << 2];
}
/*!
@@ -1414,18 +1281,16 @@ vpImage<Type>::quarterSizeImage(vpImage<Type> &res) const
See halfSizeImage(vpImage<Type> &) for an example of pyramid construction.
*/
-template<class Type>
-void
-vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
+template <class Type> void vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
{
- int h = height*2;
- int w = width*2;
+ int h = height * 2;
+ int w = width * 2;
res.resize(h, w);
- for(int i = 0; i < h; i++)
- for(int j = 0; j < w; j++)
- res[i][j] = (*this)[i>>1][j>>1];
+ for (int i = 0; i < h; i++)
+ for (int j = 0; j < w; j++)
+ res[i][j] = (*this)[i >> 1][j >> 1];
/*
A B C
@@ -1435,30 +1300,27 @@ vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
B E G I are interpolated pixels
*/
- //interpolate pixels B and I
- for(int i = 0; i < h; i += 2)
- for(int j = 1; j < w - 1; j += 2)
- res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
- + (*this)[i>>1][(j>>1) + 1]));
-
- //interpolate pixels E and G
- for(int i = 1; i < h - 1; i += 2)
- for(int j = 0; j < w; j += 2)
- res[i][j] = (Type)(0.5 * ((*this)[i>>1][j>>1]
- + (*this)[(i>>1)+1][j>>1]));
-
- //interpolate pixel F
- for(int i = 1; i < h - 1; i += 2)
- for(int j = 1; j < w - 1; j += 2)
- res[i][j] = (Type)(0.25 * ((*this)[i>>1][j>>1]
- + (*this)[i>>1][(j>>1)+1]
- + (*this)[(i>>1)+1][j>>1]
- + (*this)[(i>>1)+1][(j>>1)+1]));
+ // interpolate pixels B and I
+ for (int i = 0; i < h; i += 2)
+ for (int j = 1; j < w - 1; j += 2)
+ res[i][j] = (Type)(0.5 * ((*this)[i >> 1][j >> 1] + (*this)[i >> 1][(j >> 1) + 1]));
+
+ // interpolate pixels E and G
+ for (int i = 1; i < h - 1; i += 2)
+ for (int j = 0; j < w; j += 2)
+ res[i][j] = (Type)(0.5 * ((*this)[i >> 1][j >> 1] + (*this)[(i >> 1) + 1][j >> 1]));
+
+ // interpolate pixel F
+ for (int i = 1; i < h - 1; i += 2)
+ for (int j = 1; j < w - 1; j += 2)
+ res[i][j] = (Type)(0.25 * ((*this)[i >> 1][j >> 1] + (*this)[i >> 1][(j >> 1) + 1] +
+ (*this)[(i >> 1) + 1][j >> 1] + (*this)[(i >> 1) + 1][(j >> 1) + 1]));
}
/*!
- Retrieves pixel value from an image containing values of type \e Type with sub-pixel accuracy.
+ Retrieves pixel value from an image containing values of type \e Type with
+ sub-pixel accuracy.
Gets the value of a sub-pixel with coordinates (i,j) with bilinear
interpolation. If location is out of bounds, then return the value of the
@@ -1473,8 +1335,7 @@ vpImage<Type>::doubleSizeImage(vpImage<Type> &res)
of the image.
*/
-template<class Type>
-Type vpImage<Type>::getValue(double i, double j) const
+template <class Type> Type vpImage<Type>::getValue(double i, double j) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1483,9 +1344,7 @@ Type vpImage<Type>::getValue(double i, double j) const
jround = (unsigned int)floor(j);
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (i > height - 1)
@@ -1494,18 +1353,18 @@ Type vpImage<Type>::getValue(double i, double j) const
if (j > width - 1)
j = (double)(width - 1);
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = i - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = j - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ 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);
}
@@ -1526,8 +1385,7 @@ Type vpImage<Type>::getValue(double i, double j) const
of the image.
*/
-template<>
-inline double vpImage<double>::getValue(double i, double j) const
+template <> inline double vpImage<double>::getValue(double i, double j) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1536,9 +1394,7 @@ inline double vpImage<double>::getValue(double i, double j) const
jround = (unsigned int)floor(j);
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (i > height - 1)
@@ -1547,24 +1403,22 @@ inline double vpImage<double>::getValue(double i, double j) const
if (j > width - 1)
j = (double)(width - 1);
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = i - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = j - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround + 1][jround] * rratio) * cfrac +
+ ((double)row[iround][jround + 1] * rfrac + (double)row[iround + 1][jround + 1] * rratio) * cratio;
return value;
}
-template<>
-inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
+template <> inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1573,9 +1427,7 @@ inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
jround = (unsigned int)floor(j);
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (i > height - 1)
@@ -1584,28 +1436,30 @@ inline vpRGBa vpImage<vpRGBa>::getValue(double i, double j) const
if (j > width - 1)
j = (double)(width - 1);
- double rratio = i - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = j - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = i - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = j - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
- double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
- + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
- double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
- + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
- double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
- + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
- return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
+ 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));
}
/*!
-Retrieves pixel value from an image containing values of type \e Type with sub-pixel accuracy.
+Retrieves pixel value from an image containing values of type \e Type with
+sub-pixel accuracy.
Gets the value of a sub-pixel with coordinates (i,j) with bilinear
interpolation. If location is out of bounds, then return the value of the
@@ -1619,8 +1473,7 @@ closest pixel.
of the image.
*/
-template<class Type>
-inline Type vpImage<Type>::getValue(vpImagePoint &ip) const
+template <class Type> inline Type vpImage<Type>::getValue(vpImagePoint &ip) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1629,9 +1482,7 @@ inline Type vpImage<Type>::getValue(vpImagePoint &ip) const
jround = (unsigned int)floor(ip.get_j());
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (ip.get_i() > height - 1)
@@ -1640,23 +1491,22 @@ inline Type vpImage<Type>::getValue(vpImagePoint &ip) const
if (ip.get_j() > width - 1)
ip.set_j((double)(width - 1));
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = ip.get_i() - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = ip.get_j() - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ 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);
}
-template<>
-inline double vpImage<double>::getValue(vpImagePoint &ip) const
+template <> inline double vpImage<double>::getValue(vpImagePoint &ip) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1665,9 +1515,7 @@ inline double vpImage<double>::getValue(vpImagePoint &ip) const
jround = (unsigned int)floor(ip.get_j());
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (ip.get_i() > height - 1)
@@ -1676,24 +1524,22 @@ inline double vpImage<double>::getValue(vpImagePoint &ip) const
if (ip.get_j() > width - 1)
ip.set_j((double)(width - 1));
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = ip.get_i() - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = ip.get_j() - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
-
- double value = ((double)row[iround][jround] * rfrac + (double)row[iround+1][jround] * rratio)*cfrac
- + ((double)row[iround][jround+1]*rfrac + (double)row[iround+1][jround+1] * rratio)*cratio;
+ double value = ((double)row[iround][jround] * rfrac + (double)row[iround + 1][jround] * rratio) * cfrac +
+ ((double)row[iround][jround + 1] * rfrac + (double)row[iround + 1][jround + 1] * rratio) * cratio;
return value;
}
-template<>
-inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
+template <> inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
{
unsigned int iround, jround;
double rfrac, cfrac;
@@ -1702,9 +1548,7 @@ inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
jround = (unsigned int)floor(ip.get_j());
if (iround >= height || jround >= width) {
- vpERROR_TRACE("Pixel outside the image") ;
- throw(vpException(vpImageException::notInTheImage,
- "Pixel outside the image"));
+ throw(vpException(vpImageException::notInTheImage, "Pixel outside the image"));
}
if (ip.get_i() > height - 1)
@@ -1713,23 +1557,24 @@ inline vpRGBa vpImage<vpRGBa>::getValue(vpImagePoint &ip) const
if (ip.get_j() > width - 1)
ip.set_j((double)(width - 1));
- double rratio = ip.get_i() - (double) iround;
- if(rratio < 0)
- rratio=-rratio;
- double cratio = ip.get_j() - (double) jround;
- if(cratio < 0)
- cratio=-cratio;
+ double rratio = ip.get_i() - (double)iround;
+ if (rratio < 0)
+ rratio = -rratio;
+ double cratio = ip.get_j() - (double)jround;
+ if (cratio < 0)
+ cratio = -cratio;
rfrac = 1.0f - rratio;
cfrac = 1.0f - cratio;
- double valueR = ((double)row[iround][jround].R * rfrac + (double)row[iround+1][jround].R * rratio)*cfrac
- + ((double)row[iround][jround+1].R * rfrac + (double)row[iround+1][jround+1].R * rratio)*cratio;
- double valueG = ((double)row[iround][jround].G * rfrac + (double)row[iround+1][jround].G * rratio)*cfrac
- + ((double)row[iround][jround+1].G* rfrac + (double)row[iround+1][jround+1].G * rratio)*cratio;
- double valueB = ((double)row[iround][jround].B * rfrac + (double)row[iround+1][jround].B * rratio)*cfrac
- + ((double)row[iround][jround+1].B*rfrac + (double)row[iround+1][jround+1].B * rratio)*cratio;
- return vpRGBa((unsigned char)vpMath::round(valueR),(unsigned char)vpMath::round(valueG),(unsigned char)vpMath::round(valueB));
+ 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));
}
/*!
@@ -1761,33 +1606,23 @@ int main()
\sa operator-()
*/
-template<class Type>
-void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
+template <class Type> void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
{
- try
- {
- if ((this->getHeight() != C.getHeight())
- || (this->getWidth() != C.getWidth()))
+ try {
+ if ((this->getHeight() != C.getHeight()) || (this->getWidth() != C.getWidth()))
C.resize(this->getHeight(), this->getWidth());
- }
- catch(vpException &me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
+ } catch (vpException &me) {
+ std::cout << me << std::endl;
+ throw;
}
- if ( (this->getWidth() != B.getWidth())||(this->getHeight() != B.getHeight()))
- {
- vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
- throw(vpException(vpException::memoryAllocationError,
- "vpImage mismatch in vpImage/vpImage substraction ")) ;
+ 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++)
- {
- *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i) ;
+ for (unsigned int i = 0; i < this->getWidth() * this->getHeight(); i++) {
+ *(C.bitmap + i) = *(bitmap + i) - *(B.bitmap + i);
}
}
@@ -1802,34 +1637,23 @@ void vpImage<Type>::sub(const vpImage<Type> &B, vpImage<Type> &C)
\sa operator-()
*/
-template<class Type>
-void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B,
- vpImage<Type> &C)
+template <class Type> void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B, vpImage<Type> &C)
{
- try
- {
- if ((A.getHeight() != C.getHeight())
- || (A.getWidth() != C.getWidth()))
+ try {
+ if ((A.getHeight() != C.getHeight()) || (A.getWidth() != C.getWidth()))
C.resize(A.getHeight(), A.getWidth());
- }
- catch(vpException &me)
- {
- vpERROR_TRACE("Error caught") ;
- std::cout << me << std::endl ;
- throw ;
+ } catch (vpException &me) {
+ std::cout << me << std::endl;
+ throw;
}
- if ( (A.getWidth() != B.getWidth())||(A.getHeight() != B.getHeight()))
- {
- vpERROR_TRACE("\n\t\t vpImage mismatch in vpImage/vpImage substraction") ;
- throw(vpException(vpException::memoryAllocationError,
- "vpImage mismatch in vpImage/vpImage substraction ")) ;
+ if ((A.getWidth() != B.getWidth()) || (A.getHeight() != B.getHeight())) {
+ throw(vpException(vpException::memoryAllocationError, "vpImage mismatch in vpImage/vpImage substraction "));
}
- for (unsigned int i=0;i<A.getWidth()*A.getHeight();i++)
- {
- *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i) ;
+ for (unsigned int i = 0; i < A.getWidth() * A.getHeight(); i++) {
+ *(C.bitmap + i) = *(A.bitmap + i) - *(B.bitmap + i);
}
}
@@ -1838,92 +1662,88 @@ void vpImage<Type>::sub(const vpImage<Type> &A, const vpImage<Type> &B,
\warning This generic method is not implemented. You should rather use the
instantiated methods for unsigned char and vpRGBa images.
- \sa vpImage<unsigned char>::performLut(const unsigned char (&)[256], const unsigned int)
- \sa vpImage<vpRGBa char>::performLut(const vpRGBa (&)[256], const unsigned int)
+ \sa vpImage<unsigned char>::performLut(const unsigned char (&)[256], const
+ unsigned int) \sa vpImage<vpRGBa char>::performLut(const vpRGBa (&)[256],
+ const unsigned int)
*/
-template<class Type>
-void vpImage<Type>::performLut(const Type (&)[256], const unsigned int)
+template <class Type> void vpImage<Type>::performLut(const Type (&)[256], const unsigned int)
{
-// vpTRACE("Not implemented");
std::cerr << "Not implemented !" << std::endl;
}
/*!
- Modify the intensities of a grayscale image using the look-up table passed in parameter.
+ Modify the intensities of a grayscale image using the look-up table passed
+ in parameter.
- \param lut : Look-up table (unsigned char array of size=256) which maps each intensity to his new value.
- \param nbThreads : Number of threads to use for the computation.
+ \param lut : Look-up table (unsigned char array of size=256) which maps each
+ intensity to his new value. \param nbThreads : Number of threads to use for
+ the computation.
*/
-template<>
-inline void vpImage<unsigned char>::performLut(const unsigned char (&lut)[256], const unsigned int nbThreads) {
- unsigned int size = getWidth()*getHeight();
- unsigned char *ptrStart = (unsigned char*) bitmap;
+template <>
+inline void vpImage<unsigned char>::performLut(const unsigned char (&lut)[256], const unsigned int nbThreads)
+{
+ unsigned int size = getWidth() * getHeight();
+ unsigned char *ptrStart = (unsigned char *)bitmap;
unsigned char *ptrEnd = ptrStart + size;
unsigned char *ptrCurrent = ptrStart;
-
bool use_single_thread = (nbThreads == 0 || nbThreads == 1);
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
use_single_thread = true;
#endif
- if(!use_single_thread && getSize() <= nbThreads) {
+ if (!use_single_thread && getSize() <= nbThreads) {
use_single_thread = true;
}
+ if (use_single_thread) {
+ // Single thread
- if(use_single_thread) {
- //Single thread
-
- while(ptrCurrent != ptrEnd) {
+ while (ptrCurrent != ptrEnd) {
*ptrCurrent = lut[*ptrCurrent];
++ptrCurrent;
}
} else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
- //Multi-threads
+ // Multi-threads
std::vector<vpThread *> threadpool;
std::vector<ImageLut_Param_t *> imageLutParams;
- ImageLut_Param_t *imageLut_param = NULL;
- vpThread *imageLut_thread = NULL;
-
unsigned int image_size = getSize();
unsigned int step = image_size / nbThreads;
- unsigned int last_step = image_size - step * (nbThreads-1);
+ unsigned int last_step = image_size - step * (nbThreads - 1);
- for(unsigned int index = 0; index < nbThreads; index++) {
- unsigned int start_index = index*step;
- unsigned int end_index = (index+1)*step;
+ for (unsigned int index = 0; index < nbThreads; index++) {
+ unsigned int start_index = index * step;
+ unsigned int end_index = (index + 1) * step;
- if(index == nbThreads-1) {
- end_index = start_index+last_step;
+ if (index == nbThreads - 1) {
+ end_index = start_index + last_step;
}
- imageLut_param = new ImageLut_Param_t(start_index, end_index, bitmap);
- memcpy(imageLut_param->m_lut, lut, 256*sizeof(unsigned char));
+ 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
- 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);
}
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
// Wait until thread ends up
threadpool[cpt]->join();
}
-
- //Delete
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ // Delete
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
delete threadpool[cpt];
}
- for(size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
+ for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
delete imageLutParams[cpt];
}
#endif
@@ -1931,32 +1751,32 @@ inline void vpImage<unsigned char>::performLut(const unsigned char (&lut)[256],
}
/*!
- Modify the intensities of a color image using the look-up table passed in parameter.
+ Modify the intensities of a color image using the look-up table passed in
+ parameter.
- \param lut : Look-up table (vpRGBa array of size=256) which maps each intensity to his new value.
- \param nbThreads : Number of threads to use for the computation.
+ \param lut : Look-up table (vpRGBa array of size=256) which maps each
+ intensity to his new value. \param nbThreads : Number of threads to use for
+ the computation.
*/
-template<>
-inline void vpImage<vpRGBa>::performLut(const vpRGBa (&lut)[256], const unsigned int nbThreads) {
- unsigned int size = getWidth()*getHeight();
- unsigned char *ptrStart = (unsigned char*) bitmap;
- unsigned char *ptrEnd = ptrStart + size*4;
+template <> inline void vpImage<vpRGBa>::performLut(const vpRGBa (&lut)[256], const unsigned int nbThreads)
+{
+ unsigned int size = getWidth() * getHeight();
+ unsigned char *ptrStart = (unsigned char *)bitmap;
+ unsigned char *ptrEnd = ptrStart + size * 4;
unsigned char *ptrCurrent = ptrStart;
-
bool use_single_thread = (nbThreads == 0 || nbThreads == 1);
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
use_single_thread = true;
#endif
- if(!use_single_thread && getSize() <= nbThreads) {
+ if (!use_single_thread && getSize() <= nbThreads) {
use_single_thread = true;
}
-
- if(use_single_thread) {
- //Single thread
- while(ptrCurrent != ptrEnd) {
+ if (use_single_thread) {
+ // Single thread
+ while (ptrCurrent != ptrEnd) {
*ptrCurrent = lut[*ptrCurrent].R;
++ptrCurrent;
@@ -1971,51 +1791,58 @@ inline void vpImage<vpRGBa>::performLut(const vpRGBa (&lut)[256], const unsigned
}
} else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
- //Multi-threads
+ // Multi-threads
std::vector<vpThread *> threadpool;
std::vector<ImageLutRGBa_Param_t *> imageLutParams;
- ImageLutRGBa_Param_t *imageLut_param = NULL;
- vpThread *imageLut_thread = NULL;
-
unsigned int image_size = getSize();
unsigned int step = image_size / nbThreads;
- unsigned int last_step = image_size - step * (nbThreads-1);
+ unsigned int last_step = image_size - step * (nbThreads - 1);
- for(unsigned int index = 0; index < nbThreads; index++) {
- unsigned int start_index = index*step;
- unsigned int end_index = (index+1)*step;
+ for (unsigned int index = 0; index < nbThreads; index++) {
+ unsigned int start_index = index * step;
+ unsigned int end_index = (index + 1) * step;
- if(index == nbThreads-1) {
- end_index = start_index+last_step;
+ if (index == nbThreads - 1) {
+ end_index = start_index + last_step;
}
- imageLut_param = new ImageLutRGBa_Param_t(start_index, end_index, (unsigned char *) bitmap);
- memcpy(imageLut_param->m_lut, lut, 256*sizeof(vpRGBa));
+ 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
- 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);
}
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
// Wait until thread ends up
threadpool[cpt]->join();
}
-
- //Delete
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ // Delete
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
delete threadpool[cpt];
}
- for(size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
+ for (size_t cpt = 0; cpt < imageLutParams.size(); cpt++) {
delete imageLutParams[cpt];
}
#endif
}
}
+template <class Type> void swap(vpImage<Type> &first, vpImage<Type> &second)
+{
+ using std::swap;
+ swap(first.bitmap, second.bitmap);
+ swap(first.display, second.display);
+ swap(first.npixels, second.npixels);
+ swap(first.width, second.width);
+ swap(first.height, second.height);
+ swap(first.row, second.row);
+}
+
#endif
diff --git a/modules/core/include/visp3/core/vpImageConvert.h b/modules/core/include/visp3/core/vpImageConvert.h
index 09d7560..6516574 100644
--- a/modules/core/include/visp3/core/vpImageConvert.h
+++ b/modules/core/include/visp3/core/vpImageConvert.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,36 +50,39 @@
// image
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpImage.h>
// color
#include <visp3/core/vpRGBa.h>
#ifdef VISP_HAVE_OPENCV
-# if (VISP_HAVE_OPENCV_VERSION >= 0x030000) // Require opencv >= 3.0.0
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# include <opencv2/imgproc/imgproc.hpp>
-# elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) // Require opencv >= 2.4.8
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# include <opencv2/imgproc/imgproc.hpp>
-# elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/core/core.hpp>
-# include <opencv2/legacy/legacy.hpp>
-# include <opencv2/highgui/highgui.hpp>
-# include <opencv2/highgui/highgui_c.h>
-# else
-# include <highgui.h>
-# endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000) // Require opencv >= 3.0.0
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020408) // Require opencv >= 2.4.8
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui_c.h>
+#include <opencv2/legacy/legacy.hpp>
+#else
+#include <highgui.h>
+#endif
#endif
#ifdef VISP_HAVE_YARP
-# include <yarp/sig/Image.h>
+#include <yarp/sig/Image.h>
#endif
#if defined(_WIN32)
-# include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
+#include <windows.h>
#endif
/*!
@@ -99,12 +103,13 @@ class VISP_EXPORT vpImageConvert
public:
static void createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<vpRGBa> &dest_rgba);
- static void convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> & dest) ;
- static void convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> & dest) ;
+ static void createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<unsigned char> &dest_depth);
+ static void convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> &dest);
+ static void convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> &dest);
static void convert(const vpImage<float> &src, vpImage<unsigned char> &dest);
static void convert(const vpImage<unsigned char> &src, vpImage<float> &dest);
-
+
static void convert(const vpImage<double> &src, vpImage<unsigned char> &dest);
static void convert(const vpImage<unsigned char> &src, vpImage<double> &dest);
@@ -116,55 +121,42 @@ public:
\param src : source image.
\param dest : destination image.
*/
- template< typename Type>
- static void convert(const vpImage<Type> &src, vpImage<Type> & dest)
- {
- dest = src;
- }
+ template <typename Type> static void convert(const vpImage<Type> &src, vpImage<Type> &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<vpRGBa> & dest, bool flip = false) ;
- static void convert(const IplImage* src, vpImage<unsigned char> & dest, bool flip = false) ;
- static void convert(const vpImage<vpRGBa> & src, IplImage *&dest) ;
- static void convert(const vpImage<unsigned char> & src, IplImage* &dest) ;
-# if VISP_HAVE_OPENCV_VERSION >= 0x020100
- static void convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool flip = false);
- static void convert(const cv::Mat& src, vpImage<unsigned char>& dest, const bool flip = false);
- static void convert(const vpImage<vpRGBa> & src, cv::Mat& dest) ;
- static void convert(const vpImage<unsigned char> & src, cv::Mat& dest, const bool copyData = true) ;
-# endif
+ static void convert(const IplImage *src, vpImage<vpRGBa> &dest, bool flip = false);
+ static void convert(const IplImage *src, vpImage<unsigned char> &dest, bool flip = false);
+ static void convert(const vpImage<vpRGBa> &src, IplImage *&dest);
+ static void convert(const vpImage<unsigned char> &src, IplImage *&dest);
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ static void convert(const cv::Mat &src, vpImage<vpRGBa> &dest, const bool flip = false);
+ static void convert(const cv::Mat &src, vpImage<unsigned char> &dest, const bool flip = false);
+ static void convert(const vpImage<vpRGBa> &src, cv::Mat &dest);
+ static void convert(const vpImage<unsigned char> &src, cv::Mat &dest, const bool copyData = true);
+#endif
#endif
-
+
#ifdef VISP_HAVE_YARP
- static void convert(const vpImage<unsigned char> & src,
- yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData = true) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
- vpImage<unsigned char> & dest,const bool copyData = true ) ;
-
-
- static void convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData = true) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
- vpImage<vpRGBa> & dest,const bool copyData = true) ;
-
- static void convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest) ;
- static void convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src,
- vpImage<vpRGBa> & dest) ;
+ static void convert(const vpImage<unsigned char> &src, yarp::sig::ImageOf<yarp::sig::PixelMono> *dest,
+ const bool copyData = true);
+ static void convert(const yarp::sig::ImageOf<yarp::sig::PixelMono> *src, vpImage<unsigned char> &dest,
+ const bool copyData = true);
+
+ static void convert(const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgba> *dest,
+ const bool copyData = true);
+ static void convert(const yarp::sig::ImageOf<yarp::sig::PixelRgba> *src, vpImage<vpRGBa> &dest,
+ const bool copyData = true);
+
+ static void convert(const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgb> *dest);
+ static void convert(const yarp::sig::ImageOf<yarp::sig::PixelRgb> *src, vpImage<vpRGBa> &dest);
#endif
-
- static void split(const vpImage<vpRGBa> &src,
- vpImage<unsigned char>* pR,
- vpImage<unsigned char>* pG,
- vpImage<unsigned char>* pB,
- vpImage<unsigned char>* pa = NULL);
-
- static void merge(const vpImage<unsigned char> *R,
- const vpImage<unsigned char> *G,
- const vpImage<unsigned char> *B,
- const vpImage<unsigned char> *a,
- vpImage<vpRGBa> &RGBa);
+
+ static void split(const vpImage<vpRGBa> &src, vpImage<unsigned char> *pR, vpImage<unsigned char> *pG,
+ vpImage<unsigned char> *pB, vpImage<unsigned char> *pa = NULL);
+
+ static void merge(const vpImage<unsigned char> *R, const vpImage<unsigned char> *G, const vpImage<unsigned char> *B,
+ const vpImage<unsigned char> *a, vpImage<vpRGBa> &RGBa);
/*!
Converts a yuv pixel value in rgb format.
@@ -175,141 +167,106 @@ public:
\param r Red component from the YUV coding format. This value is computed
using:
\f[ r = 0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128) \f]
- \param g Green component from the YUV coding format. This value is computed
- using:
- \f[g = 0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128) \f]
- \param b Blue component from the YUV coding format. This value is computed
- using:
- \f[b = 0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128) \f]
+ \param g Green component from the YUV coding format. This value is
+ computed using: \f[g = 0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128)
+ \f] \param b Blue component from the YUV coding format. This value is
+ computed using: \f[b = 0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128)
+ \f]
*/
- static inline void YUVToRGB(unsigned char y,
- unsigned char u,
- unsigned char v,
- unsigned char &r,
- unsigned char &g,
- unsigned char &b)
- {
- double dr, dg, db;
- dr = floor(0.9999695*y - 0.0009508*(u-128) + 1.1359061*(v-128));
- dg = floor(0.9999695*y - 0.3959609*(u-128) - 0.5782955*(v-128));
- db = floor(0.9999695*y + 2.04112*(u-128) - 0.0016314*(v-128));
-
- dr = dr < 0. ? 0. : dr;
- dg = dg < 0. ? 0. : dg;
- db = db < 0. ? 0. : db;
- dr = dr > 255. ? 255. : dr;
- dg = dg > 255. ? 255. : dg;
- db = db > 255. ? 255. : db;
-
- r = (unsigned char) dr;
- g = (unsigned char) dg;
- b = (unsigned char) db;
- }
- static void YUYVToRGBa(unsigned char* yuyv, unsigned char* rgba,
- unsigned int width, unsigned int height);
- static void YUYVToRGB(unsigned char* yuyv, unsigned char* rgb,
- unsigned int width, unsigned int height);
- static void YUYVToGrey(unsigned char* yuyv, unsigned char* grey,
- unsigned int size);
- static void YUV411ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV411ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV411ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
- static void YUV422ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV422ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV422ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
- static void YUV420ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YUV420ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void YUV420ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
-
- static void YUV444ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int size);
- static void YUV444ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int size);
- static void YUV444ToGrey(unsigned char* yuv,
- unsigned char* grey, unsigned int size);
-
- static void YV12ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YV12ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void YVU9ToRGBa(unsigned char* yuv,
- unsigned char* rgba, unsigned int width, unsigned int height);
- static void YVU9ToRGB(unsigned char* yuv,
- unsigned char* rgb, unsigned int width, unsigned int height);
- static void RGBToRGBa(unsigned char* rgb,
- unsigned char* rgba, unsigned int size);
- static void RGBaToRGB(unsigned char* rgba,
- unsigned char* rgb, unsigned int size);
-
- static void RGBToGrey(unsigned char* rgb, unsigned char* grey, unsigned int size);
- static void RGBaToGrey(unsigned char* rgba, unsigned char* grey, unsigned int size);
-
- static void RGBToRGBa(unsigned char * 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, bool flip = false);
-
- static void GreyToRGBa(unsigned char* grey,
- unsigned char* rgba, unsigned int size);
- static void GreyToRGB(unsigned char* grey,
- unsigned char* rgb, unsigned int size);
-
- static void BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip=false);
-
- static void BGRToGrey(unsigned char * bgr, unsigned char * grey,
- unsigned int width, unsigned int height, bool flip=false);
-
- static void YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCbCrToRGBa (unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCrCbToRGB(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCrCbToRGBa(unsigned char *ycbcr, unsigned char *rgb,
- unsigned int size);
- static void YCbCrToGrey(unsigned char *ycbcr, unsigned char *grey,
- unsigned int size);
- static void MONO16ToGrey(unsigned char *grey16, unsigned char *grey,
- unsigned int size);
- static void MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba,
- unsigned int size);
-
+ static inline void YUVToRGB(unsigned char y, unsigned char u, unsigned char v, unsigned char &r, unsigned char &g,
+ unsigned char &b)
+ {
+ double dr, dg, db;
+ dr = floor(0.9999695 * y - 0.0009508 * (u - 128) + 1.1359061 * (v - 128));
+ dg = floor(0.9999695 * y - 0.3959609 * (u - 128) - 0.5782955 * (v - 128));
+ db = floor(0.9999695 * y + 2.04112 * (u - 128) - 0.0016314 * (v - 128));
+
+ dr = dr < 0. ? 0. : dr;
+ dg = dg < 0. ? 0. : dg;
+ db = db < 0. ? 0. : db;
+ dr = dr > 255. ? 255. : dr;
+ dg = dg > 255. ? 255. : dg;
+ db = db > 255. ? 255. : db;
+
+ r = (unsigned char)dr;
+ g = (unsigned char)dg;
+ b = (unsigned char)db;
+ }
+ static void YUYVToRGBa(unsigned char *yuyv, unsigned char *rgba, unsigned int width, unsigned int height);
+ static void YUYVToRGB(unsigned char *yuyv, unsigned char *rgb, unsigned int width, unsigned int height);
+ static void YUYVToGrey(unsigned char *yuyv, unsigned char *grey, unsigned int size);
+ static void YUV411ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size);
+ static void YUV411ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size);
+ static void YUV411ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size);
+ static void YUV422ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size);
+ static void YUV422ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size);
+ static void YUV422ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size);
+ static void YUV420ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height);
+ static void YUV420ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height);
+ static void YUV420ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size);
+
+ static void YUV444ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int size);
+ static void YUV444ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int size);
+ static void YUV444ToGrey(unsigned char *yuv, unsigned char *grey, unsigned int size);
+
+ static void YV12ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height);
+ static void YV12ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height);
+ static void YVU9ToRGBa(unsigned char *yuv, unsigned char *rgba, unsigned int width, unsigned int height);
+ static void YVU9ToRGB(unsigned char *yuv, unsigned char *rgb, unsigned int width, unsigned int height);
+ static void RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int size);
+ static void RGBaToRGB(unsigned char *rgba, unsigned char *rgb, unsigned int size);
+
+ static void RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int size);
+ static void RGBaToGrey(unsigned char *rgba, unsigned char *grey, unsigned int size);
+
+ static void RGBToRGBa(unsigned char *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,
+ bool flip = false);
+
+ static void GreyToRGBa(unsigned char *grey, unsigned char *rgba, unsigned int size);
+ static void GreyToRGB(unsigned char *grey, unsigned char *rgb, unsigned int size);
+
+ static void BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height,
+ bool flip = false);
+
+ static void BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height,
+ bool flip = false);
+
+ static void YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsigned int size);
+ static void YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgb, unsigned int size);
+ static void YCrCbToRGB(unsigned char *ycbcr, unsigned char *rgb, unsigned int size);
+ static void YCrCbToRGBa(unsigned char *ycbcr, unsigned char *rgb, unsigned int size);
+ static void YCbCrToGrey(unsigned char *ycbcr, unsigned char *grey, unsigned int size);
+ static void MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size);
+ static void MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size);
+
static void HSVToRGBa(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
- const unsigned int size);
+ const unsigned int size);
static void HSVToRGBa(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
- unsigned char *rgba, const unsigned int size);
+ unsigned char *rgba, const unsigned int size);
static void RGBaToHSV(const unsigned char *rgba, double *hue, double *saturation, double *value,
- const unsigned int size);
+ const unsigned int size);
static void RGBaToHSV(const unsigned char *rgba, unsigned char *hue, unsigned char *saturation, unsigned char *value,
- const unsigned int size);
+ const unsigned int size);
static void HSVToRGB(const double *hue, const double *saturation, const double *value, unsigned char *rgb,
- const unsigned int size);
+ const unsigned int size);
static void HSVToRGB(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
- unsigned char *rgb, const unsigned int size);
+ unsigned char *rgb, const unsigned int size);
static void RGBToHSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
- const unsigned int size);
+ const unsigned int size);
static void RGBToHSV(const unsigned char *rgb, unsigned char *hue, unsigned char *saturation, unsigned char *value,
- const unsigned int size);
+ const unsigned int size);
private:
static void computeYCbCrLUT();
static void HSV2RGB(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
- const unsigned int size, const unsigned int step);
- static void RGB2HSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
- const unsigned int size, const unsigned int step);
+ 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;
@@ -317,14 +274,12 @@ private:
static int vpCgb[256];
static int vpCgr[256];
static int vpCbb[256];
-
-} ;
+};
#endif
-
/*
-* Local variables:
-* c-basic-offset: 2
-* End:
-*/
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/core/include/visp3/core/vpImageException.h b/modules/core/include/visp3/core/vpImageException.h
index 20f8c16..2ed8673 100644
--- a/modules/core/include/visp3/core/vpImageException.h
+++ b/modules/core/include/visp3/core/vpImageException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,11 +39,12 @@
#ifndef __vpImageException_H
#define __vpImageException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpImageException.h
\brief error that can be emited by the vpImage class and its derivates
@@ -52,12 +54,15 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -67,33 +72,30 @@
*/
class VISP_EXPORT vpImageException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpImage member
- */
- enum errorImageCodeEnum
- {
- ioError,
- noFileNameError,
- notInitializedError,
- incorrectInitializationError,
- notInTheImage
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpImage member
+ */
+ enum errorImageCodeEnum {
+ ioError,
+ noFileNameError,
+ notInitializedError,
+ incorrectInitializationError,
+ notInTheImage
+ };
- public:
- vpImageException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpImageException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpImageException (const int id)
- : vpException(id){ ; }
+public:
+ vpImageException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpImageException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpImageException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/core/include/visp3/core/vpImageFilter.h b/modules/core/include/visp3/core/vpImageFilter.h
index 5fd07ed..6e10e5c 100644
--- a/modules/core/include/visp3/core/vpImageFilter.h
+++ b/modules/core/include/visp3/core/vpImageFilter.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef vpImageFilter_H
#define vpImageFilter_H
@@ -48,8 +47,8 @@
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageException.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <fstream>
#include <iostream>
@@ -68,11 +67,8 @@ class VISP_EXPORT vpImageFilter
{
public:
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- static void canny(const vpImage<unsigned char>& I,
- vpImage<unsigned char>& Ic,
- const unsigned int gaussianFilterSize,
- const double thresholdCanny,
- const unsigned int apertureSobel);
+ static void canny(const vpImage<unsigned char> &I, vpImage<unsigned char> &Ic, const unsigned int gaussianFilterSize,
+ const double thresholdCanny, const unsigned int apertureSobel);
#endif
/*!
@@ -82,13 +78,11 @@ public:
\param r : coordinates (row) of the pixel
\param c : coordinates (column) of the pixel
*/
- template<class T>
- static double derivativeFilterX(const vpImage<T> &I,
- const unsigned int r, const unsigned int c)
+ template <class T> static double derivativeFilterX(const vpImage<T> &I, const unsigned int r, const unsigned int c)
{
- return (2047.0 *(I[r][c+1] - I[r][c-1])
- +913.0 *(I[r][c+2] - I[r][c-2])
- +112.0 *(I[r][c+3] - I[r][c-3]))/8418.0;
+ 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,13 +92,11 @@ public:
\param r : coordinates (row) of the pixel
\param c : coordinates (column) of the pixel
*/
- template<class T>
- static double derivativeFilterY(const vpImage<T> &I,
- const unsigned int r, const unsigned int c)
+ template <class T> static double derivativeFilterY(const vpImage<T> &I, const unsigned int r, const unsigned int c)
{
- return (2047.0 *(I[r+1][c] - I[r-1][c])
- +913.0 *(I[r+2][c] - I[r-2][c])
- +112.0 *(I[r+3][c] - I[r-3][c]))/8418.0;
+ 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;
}
/*!
@@ -113,291 +105,267 @@ public:
\param I : Image to filter
\param r : coordinates (row) of the pixel
\param c : coordinates (column) of the pixel
- \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
- \param size : size of the filter
+ \param filter : coefficients of the filter to be initialized using
+ vpImageFilter::getGaussianDerivativeKernel(). \param size : size of the
+ filter
\sa vpImageFilter::getGaussianDerivativeKernel()
*/
- template<class T>
- static double derivativeFilterX(const vpImage<T> &I,
- const unsigned int r, const unsigned int c,
- const double *filter, const unsigned int size)
+ template <class T>
+ static double derivativeFilterX(const vpImage<T> &I, const unsigned int r, const unsigned int c, const double *filter,
+ const unsigned int size)
{
unsigned int i;
double result;
result = 0;
- for(i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] - I[r][c-i]) ;
+ for (i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r][c + i] - I[r][c - i]);
}
return result;
}
-
-
/*!
Apply a size x 1 Derivative Filter in Y to an image pixel.
\param I : Image to filter
\param r : coordinates (row) of the pixel
\param c : coordinates (column) of the pixel
- \param filter : coefficients of the filter to be initialized using vpImageFilter::getGaussianDerivativeKernel().
- \param size : size of the filter
+ \param filter : coefficients of the filter to be initialized using
+ vpImageFilter::getGaussianDerivativeKernel(). \param size : size of the
+ filter
\sa vpImageFilter::getGaussianDerivativeKernel()
*/
- template<class T>
- static double derivativeFilterY(const vpImage<T> &I,
- const unsigned int r, const unsigned int c,
- const double *filter, const unsigned int size)
+ template <class T>
+ static double derivativeFilterY(const vpImage<T> &I, const unsigned int r, const unsigned int c, const double *filter,
+ const unsigned int size)
{
unsigned int i;
double result;
result = 0;
- for(i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] - I[r-i][c]) ;
+ for (i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r + i][c] - I[r - i][c]);
}
return result;
}
- static void filter(const vpImage<double> &I,
- vpImage<double>& Iu,
- vpImage<double>& Iv,
- const vpMatrix& M,
- const bool convolve=false);
+ static void filter(const vpImage<double> &I, vpImage<double> &Iu, vpImage<double> &Iv, const vpMatrix &M,
+ const bool convolve = false);
- static void filter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpMatrix& M,
- const bool convolve=false);
+ static void filter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpMatrix &M,
+ const bool convolve = false);
- static void sepFilter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpColVector& kernelH,
- const vpColVector& kernelV);
+ static void sepFilter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpColVector &kernelH,
+ const vpColVector &kernelV);
- static void filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size);
- static void filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size);
+ static void filter(const vpImage<unsigned char> &I, vpImage<double> &GI, const double *filter, unsigned int size);
+ static void filter(const vpImage<double> &I, vpImage<double> &GI, const double *filter, unsigned int size);
static inline unsigned char filterGaussXPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
{
- return (unsigned char)((1.*I[i][j-2]+4.*I[i][j-1]+6.*I[i][j]+4.*I[i][j+1]+1.*I[i][j+2])/16.);
+ return (unsigned char)((1. * I[i][j - 2] + 4. * I[i][j - 1] + 6. * I[i][j] + 4. * I[i][j + 1] + 1. * I[i][j + 2]) /
+ 16.);
}
static inline unsigned char filterGaussYPyramidal(const vpImage<unsigned char> &I, unsigned int i, unsigned int j)
{
- return (unsigned char)((1.*I[i-2][j]+4.*I[i-1][j]+6.*I[i][j]+4.*I[i+1][j]+1.*I[i+2][j])/16.);
+ return (unsigned char)((1. * I[i - 2][j] + 4. * I[i - 1][j] + 6. * I[i][j] + 4. * I[i + 1][j] + 1. * I[i + 2][j]) /
+ 16.);
}
- static void filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static void filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
+ static void filterX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
+ static void filterX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
- static inline double filterX(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterX(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
-
- static inline double filterXLeftBorder(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterXLeftBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c>i)
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (c > i)
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
else
- result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
+ result += filter[i] * (I[r][c + i] + I[r][i - c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterXRightBorder(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterXRightBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c+i<I.getWidth())
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (c + i < I.getWidth())
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
else
- result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
+ result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1] + I[r][c - i]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterX(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterX(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterXLeftBorder(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterXLeftBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c>i)
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (c > i)
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
else
- result += filter[i]*(I[r][c+i] + I[r][i-c]) ;
+ result += filter[i] * (I[r][c + i] + I[r][i - c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterXRightBorder(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterXRightBorder(const vpImage<double> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(c+i<I.getWidth())
- result += filter[i]*(I[r][c+i] + I[r][c-i]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (c + i < I.getWidth())
+ result += filter[i] * (I[r][c + i] + I[r][c - i]);
else
- result += filter[i]*(I[r][2*I.getWidth()-c-i-1] + I[r][c-i]) ;
+ result += filter[i] * (I[r][2 * I.getWidth() - c - i - 1] + I[r][c - i]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static void filterY(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static void filterY(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size);
- static inline double filterY(const vpImage<unsigned char> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static void filterY(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
+ static void filterY(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
+ static inline double filterY(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- double static inline filterYTopBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ double static inline filterYTopBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r>i)
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (r > i)
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
else
- result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
+ result += filter[i] * (I[r + i][c] + I[i - r][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- double static inline filterYBottomBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ double static inline filterYBottomBorder(const vpImage<unsigned char> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r+i<I.getHeight())
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (r + i < I.getHeight())
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
else
- result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
+ result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c] + I[r - i][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterYTopBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ static inline double filterYTopBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r>i)
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (r > i)
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
else
- result += filter[i]*(I[r+i][c] + I[i-r][c]) ;
+ result += filter[i] * (I[r + i][c] + I[i - r][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterYBottomBorder(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,unsigned int size)
+ static inline double filterYBottomBorder(const vpImage<double> &I, unsigned int r, unsigned int c,
+ const double *filter, unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- if(r+i<I.getHeight())
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ if (r + i < I.getHeight())
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
else
- result += filter[i]*(I[2*I.getHeight()-r-i-1][c] + I[r-i][c]) ;
+ result += filter[i] * (I[2 * I.getHeight() - r - i - 1][c] + I[r - i][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static inline double filterY(const vpImage<double> &I,
- unsigned int r, unsigned int c,
- const double *filter,unsigned int size)
+ static inline double filterY(const vpImage<double> &I, unsigned int r, unsigned int c, const double *filter,
+ unsigned int size)
{
double result;
result = 0;
- for(unsigned int i=1; i<=(size-1)/2; i++)
- {
- result += filter[i]*(I[r+i][c] + I[r-i][c]) ;
+ for (unsigned int i = 1; i <= (size - 1) / 2; i++) {
+ result += filter[i] * (I[r + i][c] + I[r - i][c]);
}
- return result+filter[0]*I[r][c];
+ return result + filter[0] * I[r][c];
}
- static void gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size=7, double sigma=0., bool normalize=true);
- static void gaussianBlur(const vpImage<double> &I, vpImage<double>& GI, unsigned int size=7, double sigma=0., bool normalize=true);
+ static void gaussianBlur(const vpImage<unsigned char> &I, vpImage<double> &GI, unsigned int size = 7,
+ double sigma = 0., bool normalize = true);
+ static void gaussianBlur(const vpImage<double> &I, vpImage<double> &GI, unsigned int size = 7, double sigma = 0.,
+ bool normalize = true);
/*!
Apply a 5x5 Gaussian filter to an image pixel.
@@ -405,44 +373,39 @@ public:
\param r : coordinates (row) of the pixel
\param c : coordinates (column) of the pixel
*/
- template<class T>
- static double gaussianFilter(const vpImage<T> & fr,
- const unsigned int r, const unsigned int c)
+ template <class T> static double gaussianFilter(const vpImage<T> &fr, const unsigned int r, const unsigned int c)
{
- //filter Gaussien
- return (
- 15.0 * fr[r][c]
- + 12.0 * ( fr[r-1][c] + fr[r][c-1] + fr[r+1][c] + fr[r][c+1] )
- + 9.0 * ( fr[r-1][c-1] + fr[r+1][c-1] + fr[r-1][c+1] + fr[r+1][c+1])
- + 5.0 * ( fr[r-2][c] + fr[r][c-2] + fr[r+2][c] + fr[r][c+2] )
- + 4.0 * ( fr[r-2][c+1] + fr[r-2][c-1] + fr[r-1][c-2] + fr[r+1][c-2] +
- fr[r+2][c-1] + fr[r+2][c+1] + fr[r-1][c+2] + fr[r+1][c+2] )
- + 2.0 * ( fr[r-2][c-2] + fr[r+2][c-2] + fr[r-2][c+2] + fr[r+2][c+2] )
- )
- /159.0;
+ // filter Gaussien
+ return (15.0 * fr[r][c] + 12.0 * (fr[r - 1][c] + fr[r][c - 1] + fr[r + 1][c] + fr[r][c + 1]) +
+ 9.0 * (fr[r - 1][c - 1] + fr[r + 1][c - 1] + fr[r - 1][c + 1] + fr[r + 1][c + 1]) +
+ 5.0 * (fr[r - 2][c] + fr[r][c - 2] + fr[r + 2][c] + fr[r][c + 2]) +
+ 4.0 * (fr[r - 2][c + 1] + fr[r - 2][c - 1] + fr[r - 1][c - 2] + fr[r + 1][c - 2] + fr[r + 2][c - 1] +
+ fr[r + 2][c + 1] + fr[r - 1][c + 2] + fr[r + 1][c + 2]) +
+ 2.0 * (fr[r - 2][c - 2] + fr[r + 2][c - 2] + fr[r - 2][c + 2] + fr[r + 2][c + 2])) /
+ 159.0;
}
- //operation pour pyramide gaussienne
- static void getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
- static void getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
- static void getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI);
-
- static void getGaussianKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
- static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma=0., bool normalize=true);
-
- //fonction renvoyant le gradient en X de l'image I pour traitement pyramidal => dimension /2
- static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx);
- static void getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
- static void getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter, unsigned int size);
- static void getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel,
- const double *gaussianDerivativeKernel, unsigned int size);
-
- //fonction renvoyant le gradient en Y de l'image I
- static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy);
- static void getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
- static void getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter, unsigned int size);
- static void getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel,
- const double *gaussianDerivativeKernel,unsigned int size);
+ // operation pour pyramide gaussienne
+ static void getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
+ static void getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
+ static void getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI);
+
+ static void getGaussianKernel(double *filter, unsigned int size, double sigma = 0., bool normalize = true);
+ static void getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma = 0., bool normalize = true);
+
+ // fonction renvoyant le gradient en X de l'image I pour traitement
+ // pyramidal => dimension /2
+ static void getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx);
+ static void getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
+ static void getGradX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size);
+ static void getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel, unsigned int size);
+
+ // fonction renvoyant le gradient en Y de l'image I
+ static void getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy);
+ static void getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *filter, unsigned int size);
+ static void getGradY(const vpImage<double> &I, vpImage<double> &dIy, const double *filter, unsigned int size);
+ static void getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel, unsigned int size);
};
-
#endif
diff --git a/modules/core/include/visp3/core/vpImageMorphology.h b/modules/core/include/visp3/core/vpImageMorphology.h
index aebf02e..e8fe262 100644
--- a/modules/core/include/visp3/core/vpImageMorphology.h
+++ b/modules/core/include/visp3/core/vpImageMorphology.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpImageMorphology_H
#define vpImageMorphology_H
@@ -73,22 +73,21 @@ public:
typedef enum {
CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
right, up, down) */
- CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
- right, up, down, and the 4 pixels located on the diagonal) */
+ CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
+ right, up, down, and the 4 pixels located on the
+ diagonal) */
} vpConnexityType;
public:
- template<class Type>
- static void erosion(vpImage<Type> &I, Type value, Type value_out,
- vpConnexityType connexity = CONNEXITY_4);
+ template <class Type>
+ static void erosion(vpImage<Type> &I, Type value, Type value_out, vpConnexityType connexity = CONNEXITY_4);
- template<class Type>
- static void dilatation(vpImage<Type> &I, Type value, Type value_out,
- vpConnexityType connexity = CONNEXITY_4);
+ template <class Type>
+ static void dilatation(vpImage<Type> &I, Type value, Type value_out, vpConnexityType connexity = CONNEXITY_4);
static void erosion(vpImage<unsigned char> &I, const vpConnexityType &connexity = CONNEXITY_4);
static void dilatation(vpImage<unsigned char> &I, const vpConnexityType &connexity = CONNEXITY_4);
-} ;
+};
/*!
@@ -107,18 +106,15 @@ public:
\sa dilatation()
*/
-template<class Type>
-void vpImageMorphology::erosion(vpImage<Type> &I,
- Type value,
- Type value_out,
- vpConnexityType connexity)
+template <class Type>
+void vpImageMorphology::erosion(vpImage<Type> &I, Type value, Type value_out, vpConnexityType connexity)
{
- if(I.getSize() == 0) {
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
- vpImage<Type> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<Type> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -127,7 +123,7 @@ void vpImageMorphology::erosion(vpImage<Type> &I,
}
} else {
J[i][0] = value;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = value;
}
}
@@ -135,32 +131,26 @@ void vpImageMorphology::erosion(vpImage<Type> &I,
if (connexity == CONNEXITY_4) {
for (unsigned int i = 0; i < I.getHeight(); i++) {
for (unsigned int j = 0; j < I.getWidth(); j++) {
- if (J[i+1][j+1] == value) {
+ if (J[i + 1][j + 1] == value) {
// Consider 4 neighbors
- if ((J[i][j+1] == value_out) || //Top
- (J[i+2][j+1] == value_out) || //Bottom
- (J[i+1][j] == value_out) || //Left
- (J[i+1][j+2] == value_out)) { //Right
+ if ((J[i][j + 1] == value_out) || // Top
+ (J[i + 2][j + 1] == value_out) || // Bottom
+ (J[i + 1][j] == value_out) || // Left
+ (J[i + 1][j + 2] == value_out)) { // Right
I[i][j] = value_out;
}
}
}
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < I.getHeight(); i++) {
for (unsigned int j = 0; j < I.getWidth(); j++) {
- if (J[i+1][j+1] == value) {
+ 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) )
- I[i][j] = 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;
}
}
}
@@ -184,18 +174,15 @@ void vpImageMorphology::erosion(vpImage<Type> &I,
\sa erosion()
*/
-template<class Type>
-void vpImageMorphology::dilatation(vpImage<Type> &I,
- Type value,
- Type value_out,
- vpConnexityType connexity)
+template <class Type>
+void vpImageMorphology::dilatation(vpImage<Type> &I, Type value, Type value_out, vpConnexityType connexity)
{
- if(I.getSize() == 0) {
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
- vpImage<Type> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<Type> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -204,7 +191,7 @@ void vpImageMorphology::dilatation(vpImage<Type> &I,
}
} else {
J[i][0] = value_out;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = value_out;
}
}
@@ -212,32 +199,26 @@ void vpImageMorphology::dilatation(vpImage<Type> &I,
if (connexity == CONNEXITY_4) {
for (unsigned int i = 0; i < I.getHeight(); i++) {
for (unsigned int j = 0; j < I.getWidth(); j++) {
- if (J[i+1][j+1] == value_out) {
+ if (J[i + 1][j + 1] == value_out) {
// Consider 4 neighbors
- if ((J[i][j+1] == value) || //Top
- (J[i+2][j+1] == value) || //Bottom
- (J[i+1][j] == value) || //Left
- (J[i+1][j+2] == value)) { //Right
+ if ((J[i][j + 1] == value) || // Top
+ (J[i + 2][j + 1] == value) || // Bottom
+ (J[i + 1][j] == value) || // Left
+ (J[i + 1][j + 2] == value)) { // Right
I[i][j] = value;
}
}
}
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < I.getHeight(); i++) {
for (unsigned int j = 0; j < I.getWidth(); j++) {
- if (J[i+1][j+1] == value_out) {
+ 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) ) {
- I[i][j] = 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;
}
}
}
@@ -246,7 +227,6 @@ void vpImageMorphology::dilatation(vpImage<Type> &I,
}
#endif
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/core/include/visp3/core/vpImagePoint.h b/modules/core/include/visp3/core/vpImagePoint.h
index ebcb2ab..91a6c2e 100644
--- a/modules/core/include/visp3/core/vpImagePoint.h
+++ b/modules/core/include/visp3/core/vpImagePoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,9 +49,9 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <ostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#include <vector>
class vpRect;
@@ -65,7 +66,7 @@ class vpRect;
\warning If you want to define a point thanks to its coordinates
given in meter in the object frame, the camera frame or the image
- plane, you have to use the class vpPoint.
+ plane, you have to use the class vpPoint.
In this class, the 2D coordinates are not necessary integer
values. It is easy to manipulate the given coordinates in the two
@@ -84,10 +85,9 @@ class vpRect;
set_j(const double j) and set_u(const double u).</B>
*/
-
class VISP_EXPORT vpImagePoint
{
- public:
+public:
/*!
Default constructor that initialize the coordinates of the image
point to zero.
@@ -112,7 +112,8 @@ class VISP_EXPORT vpImagePoint
/*!
Copy operator.
*/
- inline const vpImagePoint& operator=(const vpImagePoint &ip) {
+ inline vpImagePoint &operator=(const vpImagePoint &ip)
+ {
this->i = ip.i;
this->j = ip.j;
return *this;
@@ -121,31 +122,34 @@ class VISP_EXPORT vpImagePoint
/*!
Move operator.
*/
- inline const vpImagePoint& operator=(const vpImagePoint &&ip) {
+ inline vpImagePoint &operator=(const vpImagePoint &&ip)
+ {
this->i = std::move(ip.i);
this->j = std::move(ip.j);
return *this;
}
#endif
- vpImagePoint& operator+=(const vpImagePoint &ip);
+ vpImagePoint &operator+=(const vpImagePoint &ip);
/*!
Operator -=.
*/
- inline vpImagePoint& operator-=(const vpImagePoint &ip) {
+ inline vpImagePoint &operator-=(const vpImagePoint &ip)
+ {
this->i -= ip.i;
this->j -= ip.j;
return *this;
}
- vpImagePoint& operator/=(const double scale);
+ vpImagePoint &operator/=(const double scale);
/*!
Operator *=.
*/
- inline vpImagePoint& operator*=(const double scale) {
+ inline vpImagePoint &operator*=(const double scale)
+ {
this->i *= scale;
this->j *= scale;
return *this;
@@ -160,7 +164,7 @@ class VISP_EXPORT vpImagePoint
\sa set_j(), set_u(), set_v()
*/
- inline void set_i(const double ii) { this->i = ii ; }
+ inline void set_i(const double ii) { this->i = ii; }
/*!
@@ -171,7 +175,7 @@ class VISP_EXPORT vpImagePoint
\sa set_i(), set_u(), set_v()
*/
- inline void set_j(const double jj) { this->j = jj ; }
+ inline void set_j(const double jj) { this->j = jj; }
/*!
@@ -182,9 +186,10 @@ class VISP_EXPORT vpImagePoint
\sa set_i(), set_j(), set_u(), set_v()
*/
- inline void set_ij(const double ii, const double jj) {
- this->i = ii ;
- this->j = jj ;
+ inline void set_ij(const double ii, const double jj)
+ {
+ this->i = ii;
+ this->j = jj;
}
/*!
@@ -196,7 +201,7 @@ class VISP_EXPORT vpImagePoint
\sa get_j(), get_u(), get_v()
*/
- inline double get_i() const { return i ; }
+ inline double get_i() const { return i; }
/*!
@@ -207,7 +212,7 @@ class VISP_EXPORT vpImagePoint
\sa get_i(), get_u(), get_v()
*/
- inline double get_j() const { return j ; }
+ inline double get_j() const { return j; }
/*!
@@ -218,7 +223,7 @@ class VISP_EXPORT vpImagePoint
\sa set_i(), set_j(), set_v()
*/
- inline void set_u(const double u) { j = u ; }
+ inline void set_u(const double u) { j = u; }
/*!
@@ -229,7 +234,7 @@ class VISP_EXPORT vpImagePoint
\sa set_i(), set_j(), set_u()
*/
- inline void set_v(const double v) { i = v ; }
+ inline void set_v(const double v) { i = v; }
/*!
@@ -240,9 +245,10 @@ class VISP_EXPORT vpImagePoint
\sa set_i(), set_j(), set_u(), set_v()
*/
- inline void set_uv(const double u, const double v) {
- this->i = v ;
- this->j = u ;
+ inline void set_uv(const double u, const double v)
+ {
+ this->i = v;
+ this->j = u;
}
/*!
@@ -254,7 +260,7 @@ class VISP_EXPORT vpImagePoint
\sa get_i(), get_j(), get_v()
*/
- inline double get_u() const { return j ; }
+ inline double get_u() const { return j; }
/*!
@@ -265,7 +271,7 @@ class VISP_EXPORT vpImagePoint
\sa get_i(), get_j(), get_u()
*/
- inline double get_v() const { return i; }
+ inline double get_v() const { return i; }
/*!
@@ -276,12 +282,13 @@ class VISP_EXPORT vpImagePoint
\return the distance between the two points.
*/
- static double distance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
- return(sqrt(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j())));}
+ static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
+ {
+ return (sqrt(vpMath::sqr(iP1.get_i() - iP2.get_i()) + vpMath::sqr(iP1.get_j() - iP2.get_j())));
+ }
+ static vpRect getBBox(const std::vector<vpImagePoint> &ipVec);
- static vpRect getBBox(const std::vector<vpImagePoint>& ipVec);
-
/*!
Compute the distance \f$ |iP1 - iP2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
@@ -291,29 +298,30 @@ class VISP_EXPORT vpImagePoint
\return the distance between the two points.
*/
- static double sqrDistance (const vpImagePoint &iP1, const vpImagePoint &iP2) {
- return(vpMath::sqr(iP1.get_i()-iP2.get_i())+vpMath::sqr(iP1.get_j()-iP2.get_j()));}
-
-
- bool inRectangle( const vpRect &rect ) const;
-
- friend VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint &ip2 );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset );
- friend VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const 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;
+ static double sqrDistance(const vpImagePoint &iP1, const vpImagePoint &iP2)
+ {
+ return (vpMath::sqr(iP1.get_i() - iP2.get_i()) + vpMath::sqr(iP1.get_j() - iP2.get_j()));
+ }
+
+ bool inRectangle(const vpRect &rect) const;
+
+ friend VISP_EXPORT bool operator==(const vpImagePoint &ip1, const vpImagePoint &ip2);
+ friend VISP_EXPORT bool operator!=(const vpImagePoint &ip1, const vpImagePoint &ip2);
+ friend VISP_EXPORT vpImagePoint operator+=(const vpImagePoint &ip1, const vpImagePoint &ip2);
+ friend VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const vpImagePoint &ip2);
+ friend VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const int offset);
+ friend VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const 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;
};
#endif
diff --git a/modules/core/include/visp3/core/vpImageTools.h b/modules/core/include/visp3/core/vpImageTools.h
index 8d91a5a..25420c1 100644
--- a/modules/core/include/visp3/core/vpImageTools.h
+++ b/modules/core/include/visp3/core/vpImageTools.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpImageTools_H
#define vpImageTools_H
@@ -50,13 +50,13 @@
#include <visp3/core/vpImage.h>
#ifdef VISP_HAVE_PTHREAD
-# include <pthread.h>
+#include <pthread.h>
#endif
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImageException.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRect.h>
-#include <visp3/core/vpCameraParameters.h>
#include <fstream>
#include <iostream>
@@ -81,141 +81,113 @@ public:
INTERPOLATION_CUBIC /*!< Bi-cubic interpolation. */
};
- template<class Type>
- static inline void binarise(vpImage<Type> &I,
- Type threshold1, Type threshold2,
- Type value1, Type value2, Type value3, const bool useLUT=true);
- static void changeLUT(vpImage<unsigned char>& I,
- unsigned char A,
- unsigned char newA,
- unsigned char B,
+ template <class Type>
+ static inline void binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2, Type value3,
+ const bool useLUT = true);
+ static void changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char newA, unsigned char B,
unsigned char newB);
- template<class Type>
- static void crop(const vpImage<Type> &I,
- double roi_top, double roi_left,
- unsigned int roi_height, unsigned int roi_width,
- vpImage<Type> &crop, unsigned int v_scale=1, unsigned int h_scale=1);
-
- template<class Type>
- static void crop(const vpImage<Type> &I,
- const vpImagePoint &topLeft,
- unsigned int roi_height, unsigned int roi_width,
- vpImage<Type> &crop, unsigned int v_scale=1, unsigned int h_scale=1);
- template<class Type>
- static void crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale=1, unsigned int h_scale=1);
- template<class Type>
- static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, vpImage<Type> &crop,
- unsigned int v_scale=1, unsigned int h_scale=1);
+ template <class Type>
+ static void crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
+ unsigned int roi_width, vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
- template<class Type>
- static void flip(const vpImage<Type> &I, vpImage<Type> &newI);
+ template <class Type>
+ static void crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height, unsigned int roi_width,
+ vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
+ template <class Type>
+ static void crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale = 1,
+ unsigned int h_scale = 1);
+ template <class Type>
+ static void crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
+ vpImage<Type> &crop, unsigned int v_scale = 1, unsigned int h_scale = 1);
- template<class Type>
- static void flip(vpImage<Type> &I);
+ template <class Type> static void flip(const vpImage<Type> &I, vpImage<Type> &newI);
+ template <class Type> static void flip(vpImage<Type> &I);
- static void imageDifference(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
+ static void imageDifference(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
vpImage<unsigned char> &Idiff);
- static void imageDifference(const vpImage<vpRGBa> &I1,
- const vpImage<vpRGBa> &I2,
- vpImage<vpRGBa> &Idiff);
+ static void imageDifference(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
- static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
+ static void imageDifferenceAbsolute(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
vpImage<unsigned char> &Idiff);
- static void imageDifferenceAbsolute(const vpImage<vpRGBa> &I1,
- const vpImage<vpRGBa> &I2,
- vpImage<vpRGBa> &Idiff);
-
- static void imageAdd(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires,
- const bool saturate=false);
-
- static void imageSubtract(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires,
- const bool saturate=false);
-
- template<class Type>
- static void resize(const vpImage<Type> &I,
- vpImage<Type> &Ires,
- const unsigned int width, const unsigned int height,
- const vpImageInterpolationType &method=INTERPOLATION_NEAREST);
-
- template<class Type>
- static void resize(const vpImage<Type> &I,
- vpImage<Type> &Ires,
- const vpImageInterpolationType &method=INTERPOLATION_NEAREST);
-
- template<class Type>
- static void undistort(const vpImage<Type> &I,
- const vpCameraParameters &cam,
- vpImage<Type> &newI);
+ static void imageDifferenceAbsolute(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff);
+
+ static void imageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, vpImage<unsigned char> &Ires,
+ const bool saturate = false);
+
+ static void imageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Ires, const bool saturate = false);
+
+ template <class Type>
+ static void resize(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int width, const unsigned int height,
+ const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
+
+ template <class Type>
+ static void resize(const vpImage<Type> &I, vpImage<Type> &Ires,
+ const vpImageInterpolationType &method = INTERPOLATION_NEAREST);
+
+ template <class Type>
+ static void undistort(const vpImage<Type> &I, const vpCameraParameters &cam, vpImage<Type> &newI);
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
@name Deprecated functions
*/
//@{
- template<class Type>
- vp_deprecated static void createSubImage(const vpImage<Type> &I,
- unsigned int i_sub, unsigned int j_sub,
- unsigned int nrow_sub, unsigned int ncol_sub,
- vpImage<Type> &S);
+ template <class Type>
+ vp_deprecated static void createSubImage(const vpImage<Type> &I, unsigned int i_sub, unsigned int j_sub,
+ unsigned int nrow_sub, unsigned int ncol_sub, vpImage<Type> &S);
- template<class Type>
+ template <class Type>
vp_deprecated static void createSubImage(const vpImage<Type> &I, const vpRect &rect, vpImage<Type> &S);
- //@}
+//@}
#endif
private:
- //Cubic interpolation
- static float cubicHermite (const float A, const float B, const float C, const float D, const float t);
+ // Cubic interpolation
+ static float cubicHermite(const float A, const float B, const float C, const float D, const float t);
- template<class Type>
- static Type getPixelClamped(const vpImage<Type> &I, const float u, const float v);
+ template <class Type> static Type getPixelClamped(const vpImage<Type> &I, const float u, const float v);
- //Linear interpolation
+ // Linear interpolation
static float lerp(const float A, const float B, const float t);
- template<class Type>
+ template <class Type>
static void resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i, const unsigned int j,
const float u, const float v, const float xFrac, const float yFrac);
- template<class Type>
+ template <class Type>
static void resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i, const unsigned int j,
const float u, const float v, const float xFrac, const float yFrac);
- template<class Type>
+ template <class Type>
static void resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i, const unsigned int j,
const float u, const float v);
-} ;
+};
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
Crop a region of interest (ROI) in an image.
\deprecated This fonction is deprecated. You should rather use
- crop(const vpImage<Type> &, unsigned int, unsigned int, unsigned int, unsigned int, vpImage<Type> &).
+ crop(const vpImage<Type> &, unsigned int, unsigned int, unsigned int,
+ unsigned int, vpImage<Type> &).
\param I : Input image from which a sub image will be extracted.
- \param roi_top : ROI vertical position of the upper/left corner in the input image.
- \param roi_left : ROI horizontal position of the upper/left corner in the input image.
- \param roi_height : Cropped image height corresponding to the ROI height.
- \param roi_width : Cropped image width corresponding to the ROI height.
- \param crop : Cropped image.
-
- \sa crop(const vpImage<Type> &, unsigned int, unsigned int, unsigned int, unsigned int, vpImage<Type> &)
+ \param roi_top : ROI vertical position of the upper/left corner in the input
+ image. \param roi_left : ROI horizontal position of the upper/left corner
+ in the input image. \param roi_height : Cropped image height corresponding
+ to the ROI height. \param roi_width : Cropped image width corresponding to
+ the ROI height. \param crop : Cropped image.
+
+ \sa crop(const vpImage<Type> &, unsigned int, unsigned int, unsigned int,
+ unsigned int, vpImage<Type> &)
*/
-template<class Type>
-void vpImageTools::createSubImage(const vpImage<Type> &I,
- unsigned int roi_top, unsigned int roi_left,
- unsigned int roi_height, unsigned int roi_width,
- vpImage<Type> &crop)
+template <class Type>
+void vpImageTools::createSubImage(const vpImage<Type> &I, unsigned int roi_top, unsigned int roi_left,
+ unsigned int roi_height, unsigned int roi_width, vpImage<Type> &crop)
{
vpImageTools::crop(I, roi_top, roi_left, roi_height, roi_width, crop);
}
@@ -235,10 +207,7 @@ void vpImageTools::createSubImage(const vpImage<Type> &I,
\sa crop(const vpImage<Type> &, const vpRect &, vpImage<Type> &)
*/
-template<class Type>
-void vpImageTools::createSubImage(const vpImage<Type> &I,
- const vpRect &roi,
- vpImage<Type> &crop)
+template <class Type> void vpImageTools::createSubImage(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop)
{
vpImageTools::crop(I, roi, crop);
}
@@ -246,68 +215,68 @@ void vpImageTools::createSubImage(const vpImage<Type> &I,
#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
- Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image.
+ Crop a region of interest (ROI) in an image. The ROI coordinates and
+ dimension are defined in the original image.
- Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image.
+ Setting \e v_scale and \e h_scale to values different from 1 allows also to
+ subsample the cropped image.
\param I : Input image from which a sub image will be extracted.
- \param roi_top : ROI vertical position of the upper/left corner in the input image.
- \param roi_left : ROI horizontal position of the upper/left corner in the input image.
- \param roi_height : Cropped image height corresponding to the ROI height.
- \param roi_width : Cropped image width corresponding to the ROI height.
- \param crop : Cropped image.
- \param v_scale [in] : Vertical subsampling factor applied to the ROI.
- \param h_scale [in] : Horizontal subsampling factor applied to the ROI.
+ \param roi_top : ROI vertical position of the upper/left corner in the input
+ image. \param roi_left : ROI horizontal position of the upper/left corner
+ in the input image. \param roi_height : Cropped image height corresponding
+ to the ROI height. \param roi_width : Cropped image width corresponding to
+ the ROI height. \param crop : Cropped image. \param v_scale [in] : Vertical
+ subsampling factor applied to the ROI. \param h_scale [in] : Horizontal
+ subsampling factor applied to the ROI.
\sa crop(const vpImage<Type> &, const vpRect &, vpImage<Type> &)
*/
-template<class Type>
-void vpImageTools::crop(const vpImage<Type> &I,
- double roi_top, double roi_left,
- unsigned int roi_height, unsigned int roi_width,
- vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
+template <class Type>
+void vpImageTools::crop(const vpImage<Type> &I, double roi_top, double roi_left, unsigned int roi_height,
+ unsigned int roi_width, vpImage<Type> &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_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));
unsigned int i_min_u = (unsigned int)i_min;
unsigned int j_min_u = (unsigned int)j_min;
- unsigned int r_width = (unsigned int)(j_max-j_min);
- unsigned int r_height = (unsigned int)(i_max-i_min);
+ unsigned int r_width = (unsigned int)(j_max - j_min);
+ unsigned int r_height = (unsigned int)(i_max - i_min);
- crop.resize(r_height, r_width) ;
+ crop.resize(r_height, r_width);
if (v_scale == 1 && h_scale == 1) {
- for (unsigned int i=0 ; i < r_height ; i++) {
- void *src = (void *)(I[i+i_min_u] + j_min_u);
+ for (unsigned int i = 0; i < r_height; i++) {
+ void *src = (void *)(I[i + i_min_u] + j_min_u);
void *dst = (void *)crop[i];
- memcpy(dst, src, r_width*sizeof(Type));
+ memcpy(dst, src, r_width * sizeof(Type));
}
- }
- else if (h_scale == 1) {
- for (unsigned int i=0 ; i < r_height ; i++) {
- void *src = (void *)(I[(i + i_min_u)*v_scale]+j_min_u);
+ } else if (h_scale == 1) {
+ for (unsigned int i = 0; i < r_height; i++) {
+ void *src = (void *)(I[(i + i_min_u) * v_scale] + j_min_u);
void *dst = (void *)crop[i];
- memcpy(dst, src, r_width*sizeof(Type));
+ memcpy(dst, src, r_width * sizeof(Type));
}
- }
- else {
- for (unsigned int i=0 ; i < r_height ; i++) {
- for (unsigned int j=0 ; j < r_width ; j++) {
- crop[i][j] = I[(i + i_min_u)*v_scale][(j + j_min_u)*h_scale];
+ } else {
+ for (unsigned int i = 0; i < r_height; i++) {
+ for (unsigned int j = 0; j < r_width; j++) {
+ crop[i][j] = I[(i + i_min_u) * v_scale][(j + j_min_u) * h_scale];
}
}
}
}
/*!
- Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image.
+ Crop a region of interest (ROI) in an image. The ROI coordinates and
+ dimension are defined in the original image.
- Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image.
+ Setting \e v_scale and \e h_scale to values different from 1 allows also to
+ subsample the cropped image.
\param I : Input image from which a sub image will be extracted.
\param topLeft : ROI position of the upper/left corner in the input image.
@@ -320,19 +289,19 @@ void vpImageTools::crop(const vpImage<Type> &I,
\sa crop(const vpImage<Type> &, const vpRect &, vpImage<Type> &)
*/
-template<class Type>
-void vpImageTools::crop(const vpImage<Type> &I,
- const vpImagePoint &topLeft,
- unsigned int roi_height, unsigned int roi_width,
- vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
+template <class Type>
+void vpImageTools::crop(const vpImage<Type> &I, const vpImagePoint &topLeft, unsigned int roi_height,
+ unsigned int roi_width, vpImage<Type> &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);
}
/*!
- Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image.
+ Crop a region of interest (ROI) in an image. The ROI coordinates and
+ dimension are defined in the original image.
- Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image.
+ Setting \e v_scale and \e h_scale to values different from 1 allows also to
+ subsample the cropped image.
\param I : Input image from which a sub image will be extracted.
@@ -344,62 +313,65 @@ void vpImageTools::crop(const vpImage<Type> &I,
\param h_scale [in] : Horizontal subsampling factor applied to the ROI.
*/
-template<class Type>
-void vpImageTools::crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &crop, unsigned int v_scale, unsigned int h_scale)
+template <class Type>
+void vpImageTools::crop(const vpImage<Type> &I, const vpRect &roi, vpImage<Type> &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);
}
/*!
- Crop a region of interest (ROI) in an image. The ROI coordinates and dimension are defined in the original image.
+ Crop a region of interest (ROI) in an image. The ROI coordinates and
+ dimension are defined in the original image.
- Setting \e v_scale and \e h_scale to values different from 1 allows also to subsample the cropped image.
+ Setting \e v_scale and \e h_scale to values different from 1 allows also to
+ subsample the cropped image.
- \param bitmap : Pointer to the input image from which a sub image will be extracted.
- \param width, height : Size of the input image.
+ \param bitmap : Pointer to the input image from which a sub image will be
+ extracted. \param width, height : Size of the input image.
- \param roi : Region of interest corresponding to the cropped part of the image.
+ \param roi : Region of interest corresponding to the cropped part of the
+ image.
\param crop : Cropped image.
\param v_scale [in] : Vertical subsampling factor applied to the ROI.
\param h_scale [in] : Horizontal subsampling factor applied to the ROI.
*/
-template<class Type>
-void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi, vpImage<Type> &crop,
- unsigned int v_scale, unsigned int h_scale)
+template <class Type>
+void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigned int height, const vpRect &roi,
+ vpImage<Type> &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_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));
unsigned int i_min_u = (unsigned int)i_min;
unsigned int j_min_u = (unsigned int)j_min;
- unsigned int r_width = (unsigned int)(j_max-j_min);
- unsigned int r_height = (unsigned int)(i_max-i_min);
+ unsigned int r_width = (unsigned int)(j_max - j_min);
+ unsigned int r_height = (unsigned int)(i_max - i_min);
- crop.resize(r_height, r_width) ;
+ crop.resize(r_height, r_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));
+ for (unsigned int i = 0; i < r_height; i++) {
+ 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));
+ 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) );
+ } 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 *dst = (void *)crop[i];
- memcpy(dst, src, r_width*sizeof(Type));
+ 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;
- for (unsigned int j=0 ; j < r_width ; j++) {
- void *src = (void *)(bitmap + (i_src + j*h_scale)*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;
+ 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];
memcpy(dst, src, sizeof(Type));
}
@@ -419,23 +391,25 @@ void vpImageTools::crop(const unsigned char *bitmap, unsigned int width, unsigne
- Pixels whose values are greater than \e threshold2 are set to \e value3
*/
-template<class Type>
-inline void vpImageTools::binarise(vpImage<Type> &I,
- Type threshold1, Type threshold2,
- Type value1, Type value2, Type value3, const bool useLUT)
+template <class Type>
+inline void vpImageTools::binarise(vpImage<Type> &I, Type threshold1, Type threshold2, Type value1, Type value2,
+ Type value3, const bool useLUT)
{
- if(useLUT) {
+ if (useLUT) {
std::cerr << "LUT not available for this type ! Will use the iteration method." << std::endl;
}
Type v;
Type *p = I.bitmap;
- Type *pend = I.bitmap + I.getWidth()*I.getHeight();
- for (; p < pend; p ++) {
+ Type *pend = I.bitmap + I.getWidth() * I.getHeight();
+ for (; p < pend; p++) {
v = *p;
- if (v < threshold1) *p = value1;
- else if (v > threshold2) *p = value3;
- else *p = value2;
+ if (v < threshold1)
+ *p = value1;
+ else if (v > threshold2)
+ *p = value3;
+ else
+ *p = value2;
}
}
@@ -451,28 +425,29 @@ inline void vpImageTools::binarise(vpImage<Type> &I,
- Pixels whose values are greater than \e threshold2 are set to \e value3
*/
-template<>
-inline void vpImageTools::binarise(vpImage<unsigned char> &I,
- unsigned char threshold1, unsigned char threshold2,
+template <>
+inline void vpImageTools::binarise(vpImage<unsigned char> &I, unsigned char threshold1, unsigned char threshold2,
unsigned char value1, unsigned char value2, unsigned char value3, const bool useLUT)
{
- if(useLUT) {
- //Construct the LUT
+ if (useLUT) {
+ // Construct the LUT
unsigned char lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i] = i < threshold1 ? value1 : (i > threshold2 ? value3 : value2);
}
I.performLut(lut);
} else {
- unsigned char v;
unsigned char *p = I.bitmap;
- unsigned char *pend = I.bitmap + I.getWidth()*I.getHeight();
- for (; p < pend; p ++) {
- v = *p;
- if (v < threshold1) *p = value1;
- else if (v > threshold2) *p = value3;
- else *p = value2;
+ unsigned char *pend = I.bitmap + I.getWidth() * I.getHeight();
+ for (; p < pend; p++) {
+ unsigned char v = *p;
+ if (v < threshold1)
+ *p = value1;
+ else if (v > threshold2)
+ *p = value3;
+ else
+ *p = value2;
}
}
}
@@ -480,8 +455,7 @@ inline void vpImageTools::binarise(vpImage<unsigned char> &I,
#ifdef VISP_HAVE_PTHREAD
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-template<class Type>
-class vpUndistortInternalType
+template <class Type> class vpUndistortInternalType
{
public:
Type *src;
@@ -491,15 +465,13 @@ public:
vpCameraParameters cam;
unsigned int nthreads;
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<Type> &u) {
- *this = u;
- };
- vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u) {
+ vpUndistortInternalType(const vpUndistortInternalType<Type> &u) { *this = u; };
+ vpUndistortInternalType &operator=(const vpUndistortInternalType<Type> &u)
+ {
src = u.src;
dst = u.dst;
width = u.width;
@@ -514,14 +486,12 @@ public:
static void *vpUndistort_threaded(void *arg);
};
-
-template<class Type>
-void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
+template <class Type> void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
{
- vpUndistortInternalType<Type> *undistortSharedData = (vpUndistortInternalType<Type>*)arg;
- int offset = (int)undistortSharedData->threadid;
- int width = (int)undistortSharedData->width;
- int height = (int)undistortSharedData->height;
+ vpUndistortInternalType<Type> *undistortSharedData = static_cast<vpUndistortInternalType<Type> *>(arg);
+ int offset = (int)undistortSharedData->threadid;
+ int width = (int)undistortSharedData->width;
+ int height = (int)undistortSharedData->height;
int nthreads = (int)undistortSharedData->nthreads;
double u0 = undistortSharedData->cam.get_u0();
@@ -530,57 +500,57 @@ void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
double py = undistortSharedData->cam.get_py();
double kud = undistortSharedData->cam.get_kud();
- double invpx = 1.0/px;
- double invpy = 1.0/py;
+ double invpx = 1.0 / px;
+ double invpy = 1.0 / py;
double kud_px2 = kud * invpx * invpx;
double kud_py2 = kud * invpy * invpy;
- Type *dst = undistortSharedData->dst+(height/nthreads*offset)*width;
+ Type *dst = undistortSharedData->dst + (height / nthreads * offset) * width;
Type *src = undistortSharedData->src;
- for (double v = height/nthreads*offset;v < height/nthreads*(offset+1) ; v++) {
- double deltav = v - v0;
- //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
+ for (double v = height / nthreads * offset; v < height / nthreads * (offset + 1); v++) {
+ double deltav = v - v0;
+ // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
double fr1 = 1.0 + kud_py2 * deltav * deltav;
- for (double u = 0 ; u < width ; u++) {
- //computation of u,v : corresponding pixel coordinates in I.
- double deltau = u - u0;
- //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
+ for (double u = 0; u < width; u++) {
+ // computation of u,v : corresponding pixel coordinates in I.
+ double deltau = u - u0;
+ // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
double fr2 = fr1 + kud_px2 * deltau * deltau;
double u_double = deltau * fr2 + u0;
double v_double = deltav * fr2 + v0;
- //computation of the bilinear interpolation
-
- //declarations
- int u_round = (int) (u_double);
- int v_round = (int) (v_double);
- if (u_round < 0.f) u_round = -1;
- if (v_round < 0.f) v_round = -1;
- double du_double = (u_double) - (double) u_round;
- double dv_double = (v_double) - (double) v_round;
+ // computation of the bilinear interpolation
+
+ // declarations
+ int u_round = (int)(u_double);
+ int v_round = (int)(v_double);
+ if (u_round < 0.f)
+ u_round = -1;
+ if (v_round < 0.f)
+ v_round = -1;
+ double du_double = (u_double) - (double)u_round;
+ double dv_double = (v_double) - (double)v_round;
Type v01;
Type v23;
- if ( (0 <= u_round) && (0 <= v_round) &&
- (u_round < ((width) - 1)) && (v_round < ((height) - 1)) ) {
- //process interpolation
- const Type* _mp = &src[v_round*width+u_round];
+ if ((0 <= u_round) && (0 <= v_round) && (u_round < ((width)-1)) && (v_round < ((height)-1))) {
+ // process interpolation
+ const Type *_mp = &src[v_round * width + u_round];
v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
_mp += width;
v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
*dst = (Type)(v01 + ((v23 - v01) * dv_double));
- }
- else {
+ } else {
*dst = 0;
}
dst++;
}
}
- pthread_exit((void*) 0);
+ pthread_exit((void *)0);
return NULL;
}
#endif // DOXYGEN_SHOULD_SKIP_THIS
@@ -605,10 +575,8 @@ void *vpUndistortInternalType<Type>::vpUndistort_threaded(void *arg)
- On "Rhea"(Intel Core 2 Extreme X6800 2.93GHz, 2Go RAM)
or "Charon"(Intel Xeon 3 GHz, 2Go RAM) : ~8 ms for a 640x480 image.
*/
-template<class Type>
-void vpImageTools::undistort(const vpImage<Type> &I,
- const vpCameraParameters &cam,
- vpImage<Type> &undistI)
+template <class Type>
+void vpImageTools::undistort(const vpImage<Type> &I, const vpCameraParameters &cam, vpImage<Type> &undistI)
{
#ifdef VISP_HAVE_PTHREAD
//
@@ -621,7 +589,7 @@ void vpImageTools::undistort(const vpImage<Type> &I,
double kud = cam.get_kud();
- //if (kud == 0) {
+ // if (kud == 0) {
if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
// There is no need to undistort the image
undistI = I;
@@ -630,38 +598,36 @@ void vpImageTools::undistort(const vpImage<Type> &I,
unsigned int nthreads = 2;
pthread_attr_t attr;
- pthread_t *callThd = new pthread_t [nthreads];
+ pthread_t *callThd = new pthread_t[nthreads];
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
vpUndistortInternalType<Type> *undistortSharedData;
- undistortSharedData = new vpUndistortInternalType<Type> [nthreads];
+ undistortSharedData = new vpUndistortInternalType<Type>[nthreads];
- for(unsigned int i=0;i<nthreads;i++) {
+ for (unsigned int i = 0; i < nthreads; i++) {
// Each thread works on a different set of data.
// vpTRACE("create thread %d", i);
- undistortSharedData[i].src = I.bitmap;
- undistortSharedData[i].dst = undistI.bitmap;
- undistortSharedData[i].width = I.getWidth();
- undistortSharedData[i].height = I.getHeight();
- undistortSharedData[i].cam = cam;
+ undistortSharedData[i].src = I.bitmap;
+ undistortSharedData[i].dst = undistI.bitmap;
+ undistortSharedData[i].width = I.getWidth();
+ undistortSharedData[i].height = I.getHeight();
+ undistortSharedData[i].cam = cam;
undistortSharedData[i].nthreads = nthreads;
undistortSharedData[i].threadid = i;
- pthread_create( &callThd[i], &attr,
- &vpUndistortInternalType<Type>::vpUndistort_threaded,
- &undistortSharedData[i]);
+ pthread_create(&callThd[i], &attr, &vpUndistortInternalType<Type>::vpUndistort_threaded, &undistortSharedData[i]);
}
pthread_attr_destroy(&attr);
/* Wait on the other threads */
- for(unsigned int i=0;i<nthreads;i++) {
+ for (unsigned int i = 0; i < nthreads; i++) {
// vpTRACE("join thread %d", i);
- pthread_join( callThd[i], NULL);
+ pthread_join(callThd[i], NULL);
}
- delete [] callThd;
- delete [] undistortSharedData;
-#else // VISP_HAVE_PTHREAD
+ delete[] callThd;
+ delete[] undistortSharedData;
+#else // VISP_HAVE_PTHREAD
//
// optimized version without pthreads
//
@@ -676,58 +642,58 @@ void vpImageTools::undistort(const vpImage<Type> &I,
double py = cam.get_py();
double kud = cam.get_kud();
- //if (kud == 0) {
+ // if (kud == 0) {
if (std::fabs(kud) <= std::numeric_limits<double>::epsilon()) {
// There is no need to undistort the image
undistI = I;
return;
}
- double invpx = 1.0/px;
- double invpy = 1.0/py;
+ double invpx = 1.0 / px;
+ double invpy = 1.0 / py;
double kud_px2 = kud * invpx * invpx;
double kud_py2 = kud * invpy * invpy;
Type *dst = undistI.bitmap;
- for (double v = 0;v < height ; v++) {
- double deltav = v - v0;
- //double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
+ for (double v = 0; v < height; v++) {
+ double deltav = v - v0;
+ // double fr1 = 1.0 + kd * (vpMath::sqr(deltav * invpy));
double fr1 = 1.0 + kud_py2 * deltav * deltav;
- for (double u = 0 ; u < width ; u++) {
- //computation of u,v : corresponding pixel coordinates in I.
- double deltau = u - u0;
- //double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
+ for (double u = 0; u < width; u++) {
+ // computation of u,v : corresponding pixel coordinates in I.
+ double deltau = u - u0;
+ // double fr2 = fr1 + kd * (vpMath::sqr(deltau * invpx));
double fr2 = fr1 + kud_px2 * deltau * deltau;
double u_double = deltau * fr2 + u0;
double v_double = deltav * fr2 + v0;
- //printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
-
- //computation of the bilinear interpolation
-
- //declarations
- int u_round = (int) (u_double);
- int v_round = (int) (v_double);
- if (u_round < 0.f) u_round = -1;
- if (v_round < 0.f) v_round = -1;
- double du_double = (u_double) - (double) u_round;
- double dv_double = (v_double) - (double) v_round;
- Type v01;
- Type v23;
- if ( (0 <= u_round) && (0 <= v_round) &&
- (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1)) ) {
- //process interpolation
- const Type* _mp = &I[(unsigned int)v_round][(unsigned int)u_round];
+ // printf("[%g][%g] %g %g : ", u, v, u_double, v_double );
+
+ // computation of the bilinear interpolation
+
+ // declarations
+ int u_round = (int)(u_double);
+ int v_round = (int)(v_double);
+ if (u_round < 0.f)
+ u_round = -1;
+ if (v_round < 0.f)
+ v_round = -1;
+ double du_double = (u_double) - (double)u_round;
+ double dv_double = (v_double) - (double)v_round;
+ Type v01;
+ Type v23;
+ if ((0 <= u_round) && (0 <= v_round) && (u_round < (((int)width) - 1)) && (v_round < (((int)height) - 1))) {
+ // process interpolation
+ const Type *_mp = &I[(unsigned int)v_round][(unsigned int)u_round];
v01 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
_mp += width;
v23 = (Type)(_mp[0] + ((_mp[1] - _mp[0]) * du_double));
*dst = (Type)(v01 + ((v23 - v01) * dv_double));
- //printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
- }
- else {
+ // printf("R %d G %d B %d\n", dst->R, dst->G, dst->B);
+ } else {
*dst = 0;
}
dst++;
@@ -735,9 +701,6 @@ void vpImageTools::undistort(const vpImage<Type> &I,
}
#endif // VISP_HAVE_PTHREAD
-
-
-
#if 0
// non optimized version
int width = I.getWidth();
@@ -776,24 +739,19 @@ void vpImageTools::undistort(const vpImage<Type> &I,
\param newI : Output image which is the flipped input image.
*/
-template<class Type>
-void vpImageTools::flip(const vpImage<Type> &I,
- vpImage<Type> &newI)
+template <class Type> void vpImageTools::flip(const vpImage<Type> &I, vpImage<Type> &newI)
{
- unsigned int height = 0, width = 0;
+ unsigned int height = 0, width = 0;
- height = I.getHeight();
- width = I.getWidth();
- newI.resize(height, width);
+ height = I.getHeight();
+ width = I.getWidth();
+ newI.resize(height, width);
- for (unsigned int i = 0; i < height; i++)
- {
- memcpy(newI.bitmap+i*width, I.bitmap+(height-1-i)*width,
- width*sizeof(Type));
- }
+ for (unsigned int i = 0; i < height; i++) {
+ memcpy(newI.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
+ }
}
-
/*!
Flip vertically the input image.
@@ -801,8 +759,8 @@ void vpImageTools::flip(const vpImage<Type> &I,
The following example shows how to use this function:
\code
-#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageTools.h>
#include <visp3/io/vpImageIo.h>
int main()
@@ -820,68 +778,79 @@ 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
*/
-template<class Type>
-void vpImageTools::flip(vpImage<Type> &I)
+template <class Type> void vpImageTools::flip(vpImage<Type> &I)
{
- unsigned int height = 0, width = 0;
- unsigned int i = 0;
- vpImage<Type> Ibuf;
-
- height = I.getHeight();
- width = I.getWidth();
- Ibuf.resize(1, width);
-
- for ( i = 0; i < height/2; i++)
- {
- memcpy(Ibuf.bitmap, I.bitmap+i*width,
- width*sizeof(Type));
-
- memcpy(I.bitmap+i*width, I.bitmap+(height-1-i)*width,
- width*sizeof(Type));
- memcpy(I.bitmap+(height-1-i)*width, Ibuf.bitmap,
- width*sizeof(Type));
- }
+ unsigned int height = 0, width = 0;
+ unsigned int i = 0;
+ vpImage<Type> Ibuf;
+
+ height = I.getHeight();
+ width = I.getWidth();
+ Ibuf.resize(1, width);
+
+ for (i = 0; i < height / 2; i++) {
+ memcpy(Ibuf.bitmap, I.bitmap + i * width, width * sizeof(Type));
+
+ memcpy(I.bitmap + i * width, I.bitmap + (height - 1 - i) * width, width * sizeof(Type));
+ memcpy(I.bitmap + (height - 1 - i) * width, Ibuf.bitmap, width * sizeof(Type));
+ }
}
-template<class Type>
-Type vpImageTools::getPixelClamped(const vpImage<Type> &I, const float u, const float v) {
- unsigned int j = std::min(std::max(0u, (unsigned int) u), I.getWidth()-1);
- unsigned int i = std::min(std::max(0u, (unsigned int) v), I.getHeight()-1);
+template <class Type> Type vpImageTools::getPixelClamped(const vpImage<Type> &I, const float u, const float v)
+{
+ unsigned int i, j;
+ if (u < 0.)
+ j = 0;
+ else if (u > (float)I.getWidth() - 1.)
+ j = I.getWidth() - 1;
+ else
+ j = (unsigned int)u;
+
+ if (v < 0.)
+ i = 0;
+ else if (v > (float)I.getHeight() - 1.)
+ i = I.getHeight() - 1;
+ else
+ i = (unsigned int)v;
return I[i][j];
}
-// Reference: http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
-template<class Type> void
-vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i, const unsigned int j,
- const float u, const float v, const float xFrac, const float yFrac) {
+// Reference:
+// http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
+template <class Type>
+void vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &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);
- Type p01 = getPixelClamped(I, u+0, v-1);
- Type p02 = getPixelClamped(I, u+1, v-1);
- Type p03 = getPixelClamped(I, u+2, v-1);
+ Type p00 = getPixelClamped(I, u - 1, v - 1);
+ Type p01 = getPixelClamped(I, u + 0, v - 1);
+ Type p02 = getPixelClamped(I, u + 1, v - 1);
+ Type p03 = getPixelClamped(I, u + 2, v - 1);
// 2nd row
- Type p10 = getPixelClamped(I, u-1, v+0);
- Type p11 = getPixelClamped(I, u+0, v+0);
- Type p12 = getPixelClamped(I, u+1, v+0);
- Type p13 = getPixelClamped(I, u+2, v+0);
+ Type p10 = getPixelClamped(I, u - 1, v + 0);
+ Type p11 = getPixelClamped(I, u + 0, v + 0);
+ Type p12 = getPixelClamped(I, u + 1, v + 0);
+ Type p13 = getPixelClamped(I, u + 2, v + 0);
// 3rd row
- Type p20 = getPixelClamped(I, u-1, v+1);
- Type p21 = getPixelClamped(I, u+0, v+1);
- Type p22 = getPixelClamped(I, u+1, v+1);
- Type p23 = getPixelClamped(I, u+2, v+1);
+ Type p20 = getPixelClamped(I, u - 1, v + 1);
+ Type p21 = getPixelClamped(I, u + 0, v + 1);
+ Type p22 = getPixelClamped(I, u + 1, v + 1);
+ Type p23 = getPixelClamped(I, u + 2, v + 1);
// 4th row
- Type p30 = getPixelClamped(I, u-1, v+2);
- Type p31 = getPixelClamped(I, u+0, v+2);
- Type p32 = getPixelClamped(I, u+1, v+2);
- Type p33 = getPixelClamped(I, u+2, v+2);
+ Type p30 = getPixelClamped(I, u - 1, v + 2);
+ Type p31 = getPixelClamped(I, u + 0, v + 2);
+ Type p32 = getPixelClamped(I, u + 1, v + 2);
+ Type p33 = getPixelClamped(I, u + 2, v + 2);
float col0 = cubicHermite(p00, p01, p02, p03, xFrac);
float col1 = cubicHermite(p10, p11, p12, p13, xFrac);
@@ -891,58 +860,66 @@ vpImageTools::resizeBicubic(const vpImage<Type> &I, vpImage<Type> &Ires, const u
Ires[i][j] = vpMath::saturate<Type>(value);
}
-template<> inline void
-vpImageTools::resizeBicubic(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, const unsigned int i, const unsigned int j,
- const float u, const float v, const float xFrac, const float yFrac) {
+template <>
+inline void vpImageTools::resizeBicubic(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &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);
- vpRGBa p01 = getPixelClamped(I, u+0, v-1);
- vpRGBa p02 = getPixelClamped(I, u+1, v-1);
- vpRGBa p03 = getPixelClamped(I, u+2, v-1);
+ vpRGBa p00 = getPixelClamped(I, u - 1, v - 1);
+ vpRGBa p01 = getPixelClamped(I, u + 0, v - 1);
+ vpRGBa p02 = getPixelClamped(I, u + 1, v - 1);
+ vpRGBa p03 = getPixelClamped(I, u + 2, v - 1);
// 2nd row
- vpRGBa p10 = getPixelClamped(I, u-1, v+0);
- vpRGBa p11 = getPixelClamped(I, u+0, v+0);
- vpRGBa p12 = getPixelClamped(I, u+1, v+0);
- vpRGBa p13 = getPixelClamped(I, u+2, v+0);
+ vpRGBa p10 = getPixelClamped(I, u - 1, v + 0);
+ vpRGBa p11 = getPixelClamped(I, u + 0, v + 0);
+ vpRGBa p12 = getPixelClamped(I, u + 1, v + 0);
+ vpRGBa p13 = getPixelClamped(I, u + 2, v + 0);
// 3rd row
- vpRGBa p20 = getPixelClamped(I, u-1, v+1);
- vpRGBa p21 = getPixelClamped(I, u+0, v+1);
- vpRGBa p22 = getPixelClamped(I, u+1, v+1);
- vpRGBa p23 = getPixelClamped(I, u+2, v+1);
+ vpRGBa p20 = getPixelClamped(I, u - 1, v + 1);
+ vpRGBa p21 = getPixelClamped(I, u + 0, v + 1);
+ vpRGBa p22 = getPixelClamped(I, u + 1, v + 1);
+ vpRGBa p23 = getPixelClamped(I, u + 2, v + 1);
// 4th row
- vpRGBa p30 = getPixelClamped(I, u-1, v+2);
- vpRGBa p31 = getPixelClamped(I, u+0, v+2);
- vpRGBa p32 = getPixelClamped(I, u+1, v+2);
- vpRGBa p33 = getPixelClamped(I, u+2, v+2);
+ vpRGBa p30 = getPixelClamped(I, u - 1, v + 2);
+ vpRGBa p31 = getPixelClamped(I, u + 0, v + 2);
+ vpRGBa p32 = getPixelClamped(I, u + 1, v + 2);
+ 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<unsigned char>(value);
+ ((unsigned char *)&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
}
}
-template<class Type> void
-vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &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;
+template <class Type>
+void vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &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;
- unsigned int u1 = std::min(I.getWidth()-1, (unsigned int) u+1);
+ unsigned int u1 = (std::min)(I.getWidth() - 1, (unsigned int)u + 1);
unsigned int v1 = v0;
unsigned int u2 = u0;
- unsigned int v2 = std::min(I.getHeight()-1, (unsigned int) v+1);
+ unsigned int v2 = (std::min)(I.getHeight() - 1, (unsigned int)v + 1);
- unsigned int u3 = std::min(I.getWidth()-1, (unsigned int) u+1);
- unsigned int v3 = std::min(I.getHeight()-1, (unsigned int) v+1);
+ unsigned int u3 = (std::min)(I.getWidth() - 1, (unsigned int)u + 1);
+ unsigned int v3 = (std::min)(I.getHeight() - 1, (unsigned int)v + 1);
float col0 = lerp(I[v0][u0], I[v1][u1], xFrac);
float col1 = lerp(I[v2][u2], I[v3][u3], xFrac);
@@ -951,81 +928,89 @@ vpImageTools::resizeBilinear(const vpImage<Type> &I, vpImage<Type> &Ires, const
Ires[i][j] = vpMath::saturate<Type>(value);
}
-template<> inline void
-vpImageTools::resizeBilinear(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &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;
+template <>
+inline void vpImageTools::resizeBilinear(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &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;
- unsigned int u1 = std::min(I.getWidth()-1, (unsigned int) u+1);
+ unsigned int u1 = (std::min)(I.getWidth() - 1, (unsigned int)u + 1);
unsigned int v1 = v0;
unsigned int u2 = u0;
- unsigned int v2 = std::min(I.getHeight()-1, (unsigned int) v+1);
+ unsigned int v2 = (std::min)(I.getHeight() - 1, (unsigned int)v + 1);
- unsigned int u3 = std::min(I.getWidth()-1, (unsigned int) u+1);
- unsigned int v3 = std::min(I.getHeight()-1, (unsigned int) v+1);
+ unsigned int u3 = (std::min)(I.getWidth() - 1, (unsigned int)u + 1);
+ 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<unsigned char>(value);
+ ((unsigned char *)&Ires[i][j])[c] = vpMath::saturate<unsigned char>(value);
}
}
-template<class Type> void
-vpImageTools::resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i, const unsigned int j,
- const float u, const float v) {
+template <class Type>
+void vpImageTools::resizeNearest(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int i,
+ const unsigned int j, const float u, const float v)
+{
Ires[i][j] = getPixelClamped(I, u, v);
}
/*!
- Resize the image using one interpolation method (by default it uses the nearest neighbor interpolation).
+ Resize the image using one interpolation method (by default it uses the
+ nearest neighbor interpolation).
\param I : Input image.
- \param Ires : Output image resized.
- \param width : Resize width.
- \param height : Resize height.
+ \param Ires : Output image resized to \e width, \e height.
+ \param width : Resized width.
+ \param height : Resized height.
\param method : Interpolation method.
*/
-template<class Type> void
-vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int width, const unsigned int height, const vpImageInterpolationType &method) {
+template <class Type>
+void vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, const unsigned int width,
+ const unsigned int height, const vpImageInterpolationType &method)
+{
Ires.resize(height, width);
vpImageTools::resize(I, Ires, method);
}
/*!
- Resize the image using one interpolation method (by default it uses the nearest neighbor interpolation).
+ Resize the image using one interpolation method (by default it uses the
+ nearest neighbor interpolation).
\param I : Input image.
- \param Ires : Output image resized (you have to init the image \e Ires at the desired size).
- \param method : Interpolation method.
+ \param Ires : Output image resized (you have to init the image \e Ires at
+ the desired size). \param method : Interpolation method.
*/
-template<class Type> void
-vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, const vpImageInterpolationType &method) {
+template <class Type>
+void vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, const vpImageInterpolationType &method)
+{
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;
}
- float scaleY = (I.getHeight() - 1) / (float) (Ires.getHeight() - 1);
- float scaleX = (I.getWidth() - 1) / (float) (Ires.getWidth() - 1);
+ float scaleY = (I.getHeight() - 1) / (float)(Ires.getHeight() - 1);
+ float scaleX = (I.getWidth() - 1) / (float)(Ires.getWidth() - 1);
if (method == INTERPOLATION_NEAREST) {
- scaleY = I.getHeight() / (float) (Ires.getHeight() - 1);
- scaleX = I.getWidth() / (float) (Ires.getWidth() - 1);
+ scaleY = I.getHeight() / (float)(Ires.getHeight() - 1);
+ scaleX = I.getWidth() / (float)(Ires.getWidth() - 1);
}
for (unsigned int i = 0; i < Ires.getHeight(); i++) {
float v = i * scaleY;
- float yFrac = v - (int) v;
+ float yFrac = v - (int)v;
for (unsigned int j = 0; j < Ires.getWidth(); j++) {
float u = j * scaleX;
- float xFrac = u - (int) u;
+ float xFrac = u - (int)u;
if (method == INTERPOLATION_NEAREST) {
resizeNearest(I, Ires, i, j, u, v);
@@ -1039,10 +1024,3 @@ vpImageTools::resize(const vpImage<Type> &I, vpImage<Type> &Ires, const vpImageI
}
#endif
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/modules/core/include/visp3/core/vpIoException.h b/modules/core/include/visp3/core/vpIoException.h
index 99400af..db24d32 100644
--- a/modules/core/include/visp3/core/vpIoException.h
+++ b/modules/core/include/visp3/core/vpIoException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,15 @@
*
*****************************************************************************/
-
#ifndef __vpIoException_H
#define __vpIoException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\file vpIoException.h
@@ -56,9 +57,12 @@
#include <iostream>
#include <string>
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpIoException
@@ -67,33 +71,29 @@
*/
class VISP_EXPORT vpIoException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpIo member.
- */
- enum error
- {
- invalidDirectoryName, /*! Directory name is invalid. */
- cantCreateDirectory, /*! Unable to create a directory. */
- cantGetUserName, /*! User name is not available. */
- cantGetenv /*! Cannot get environment variable value. */
- } ;
-
- public:
- vpIoException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpIoException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpIoException (const int id)
- : vpException(id){ ; }
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpIo member.
+ */
+ enum error {
+ invalidDirectoryName, /*! Directory name is invalid. */
+ cantCreateDirectory, /*! Unable to create a directory. */
+ cantGetUserName, /*! User name is not available. */
+ cantGetenv /*! Cannot get environment variable value. */
+ };
+public:
+ vpIoException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpIoException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpIoException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/core/include/visp3/core/vpIoTools.h b/modules/core/include/visp3/core/vpIoTools.h
index 4039b6d..836470b 100644
--- a/modules/core/include/visp3/core/vpIoTools.h
+++ b/modules/core/include/visp3/core/vpIoTools.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpIoTools_HH
#define vpIoTools_HH
@@ -48,8 +48,8 @@
#include <iostream>
#include <sstream>
-#include <string>
#include <stdlib.h>
+#include <string>
#include <vector>
#include <visp3/core/vpColor.h>
@@ -87,8 +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();
@@ -106,8 +106,8 @@ int main()
}
\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" :
+ 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
@@ -133,14 +133,18 @@ int main()
// 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/"
}
@@ -152,7 +156,7 @@ class VISP_EXPORT vpIoTools
{
public:
- static const std::string& getBuildInformation();
+ static const std::string &getBuildInformation();
static void getUserName(std::string &username);
static std::string getUserName();
static std::string getenv(const char *env);
@@ -171,30 +175,32 @@ public:
static bool remove(const std::string &filename);
static bool rename(const char *oldfilename, const char *newfilename);
static bool rename(const std::string &oldfilename, const std::string &newfilename);
-
- static std::string path(const char * pathname);
+
+ static std::string path(const char *pathname);
static std::string path(const std::string &pathname);
-
+
/*!
- Define the directory separator character, backslash ('\') for windows platform or slash ('/') otherwise.
+ Define the directory separator character, backslash ('\') for windows
+ platform or slash ('/') otherwise.
*/
static const char separator =
- #if defined(_WIN32)
- '\\';
- #else
- '/';
- #endif
+#if defined(_WIN32)
+ '\\';
+#else
+ '/';
+#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 bool isAbsolutePathname(const std::string& pathname);
- static bool isSamePathname(const std::string& pathname1, const std::string& pathname2);
- static std::pair<std::string, std::string> splitDrive(const std::string& pathname);
- static std::vector<std::string> splitChain(const std::string & chain, const std::string & sep);
+ static std::string getParent(const std::string &pathname);
+ static std::string createFilePath(const std::string &parent, const std::string &child);
+ static bool isAbsolutePathname(const std::string &pathname);
+ static bool isSamePathname(const std::string &pathname1, const std::string &pathname2);
+ static std::pair<std::string, std::string> splitDrive(const std::string &pathname);
+ static std::vector<std::string> splitChain(const std::string &chain, const std::string &sep);
+ static std::vector<std::string> getDirFiles(const std::string &dirname);
/*!
@name Configuration file parsing
@@ -209,32 +215,29 @@ public:
static bool readConfigVar(const std::string &var, bool &value);
static bool readConfigVar(const std::string &var, std::string &value);
static bool readConfigVar(const std::string &var, vpColor &value);
- static bool readConfigVar(const std::string &var, vpArray2D<double> &value,
- const unsigned int &nCols = 0,
+ static bool readConfigVar(const std::string &var, vpArray2D<double> &value, const unsigned int &nCols = 0,
const unsigned int &nRows = 0);
-
+
// construct experiment filename & path
static void setBaseName(const std::string &s);
static void setBaseDir(const std::string &dir);
- static void addNameElement(const std::string &strTrue,
- const bool &cond=true,
- const std::string &strFalse="");
+ static void addNameElement(const std::string &strTrue, const bool &cond = true, const std::string &strFalse = "");
static void addNameElement(const std::string &strTrue, const double &val);
static std::string getBaseName();
static std::string getFullName();
-
+
// write files
static void saveConfigFile(const bool &actuallySave = true);
static void createBaseNamePath(const bool &empty = false);
//@}
- protected:
+protected:
static std::string baseName;
static std::string baseDir;
static std::string configFile;
static std::vector<std::string> configVars;
static std::vector<std::string> configValues;
-} ;
-
+ static int mkdir_p(const char *path, const int mode);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpKalmanFilter.h b/modules/core/include/visp3/core/vpKalmanFilter.h
index 71d413f..64926e0 100644
--- a/modules/core/include/visp3/core/vpKalmanFilter.h
+++ b/modules/core/include/visp3/core/vpKalmanFilter.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,8 +40,8 @@
#ifndef vpKalmanFilter_h
#define vpKalmanFilter_h
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
#include <math.h>
@@ -61,13 +62,14 @@
{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
\f]
where \f${\bf x}_{k}\f$ is the unknown state at iteration \f$k\f$.
-
+
The measurement equation is given by:
\f[
{\bf z}_k = {\bf H} {\bf x}_k + {\bf r}_k
\f]
- where \f${\bf z}_{k}\f$ is the measure (also named observation) at iteration \f$k\f$.
-
+ where \f${\bf z}_{k}\f$ is the measure (also named observation) at iteration
+ \f$k\f$.
+
The predicted state is obtained by:
\f[
{\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}
@@ -107,40 +109,38 @@
*/
class VISP_EXPORT vpKalmanFilter
{
-protected :
- //bool init_done ;
+protected:
+ // bool init_done ;
- //! Filter step or iteration. When set to zero, initialize the filter.
- long iter ;
+ //! Filter step or iteration. When set to zero, initialize the filter.
+ long iter;
//! Size of the state vector \f${\bf x}_k\f$.
- unsigned int size_state ;
+ unsigned int size_state;
//! Size of the measure vector \f${\bf z}_k\f$.
- unsigned int size_measure ;
+ unsigned int size_measure;
//! Number of signal to filter.
- unsigned int nsignal ;
+ unsigned int nsignal;
- //! When set to true, print the content of internal variables during filtering() and prediction().
+ //! When set to true, print the content of internal variables during
+ //! filtering() and prediction().
bool verbose_mode;
public:
- vpKalmanFilter() ;
- vpKalmanFilter(unsigned int n_signal) ;
- vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
+ vpKalmanFilter();
+ explicit vpKalmanFilter(unsigned int n_signal);
+ vpKalmanFilter(unsigned int size_state, unsigned int size_measure, unsigned int n_signal);
/*! Destructor that does noting. */
- virtual ~vpKalmanFilter() {};
+ virtual ~vpKalmanFilter(){};
/*!
Set the number of signal to filter.
*/
- void setNumberOfSignal(unsigned int n_signal)
- {
- this->nsignal = n_signal;
- }
+ void setNumberOfSignal(unsigned int n_signal) { this->nsignal = n_signal; }
// int init() { return init_done ; }
- void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal) ;
- void prediction() ;
- void filtering(const vpColVector &z) ;
+ void init(unsigned int size_state, unsigned int size_measure, unsigned int n_signal);
+ void prediction();
+ void filtering(const vpColVector &z);
/*!
Return the size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
*/
@@ -156,67 +156,66 @@ public:
/*!
Return the iteration number.
*/
- long getIteration() { return iter ; }
+ long getIteration() { return iter; }
/*!
Sets the verbose mode.
- \param on : If true, activates the verbose mode which consists in printing the Kalman
- filter internal values.
+ \param on : If true, activates the verbose mode which consists in printing
+ the Kalman filter internal values.
*/
- void verbose(bool on) { verbose_mode = on;};
+ void verbose(bool on) { verbose_mode = on; };
public:
/*!
- The updated state estimate \f${\bf x}_{k \mid k} \f$ where
- \f${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k
+ The updated state estimate \f${\bf x}_{k \mid k} \f$ where
+ \f${\bf x}_{k \mid k} = {\bf x}_{k \mid k-1} + {\bf W}_k
\left[ {\bf z}_k - {\bf H x}_{k \mid k-1} \right]\f$.
*/
- vpColVector Xest ;
+ vpColVector Xest;
/*!
- The predicted state \f${\bf x}_{k \mid k-1} \f$ where
+ The predicted state \f${\bf x}_{k \mid k-1} \f$ where
\f$ {\bf x}_{k|k-1} = {\bf F}_{k-1} {\bf x}_{k-1\mid k-1}\f$.
*/
- vpColVector Xpre ;
- //! Transition matrix \f${\bf F}\f$ that describes the evolution of the state.
- vpMatrix F ;
+ vpColVector Xpre;
+ //! Transition matrix \f${\bf F}\f$ that describes the evolution of the
+ //! state.
+ vpMatrix F;
- //! Matrix \f${\bf H}\f$ that describes the evolution of the measurements.
- vpMatrix H ;
+ //! Matrix \f${\bf H}\f$ that describes the evolution of the measurements.
+ vpMatrix H;
//! Measurement noise covariance matrix \f${\bf R}\f$.
- vpMatrix R ;
+ vpMatrix R;
//! Process noise covariance matrix \f${\bf Q}\f$.
- vpMatrix Q ;
+ vpMatrix Q;
/*! Sampling time \f$\Delta t\f$ in second between two succesive
iterations. Only used in some specific state models implemented
in vpLinearKalmanFilterInstantiation.*/
- double dt ;
+ double dt;
/*!
- The state prediction covariance \f${\bf P}_{k \mid k-1} \f$ where
- \f$ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
+ The state prediction covariance \f${\bf P}_{k \mid k-1} \f$ where
+ \f$ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf
+ F}^T_{k-1}
+ {\bf Q}_k\f$.
*/
- vpMatrix Ppre ;
+ vpMatrix Ppre;
- /*!
+ /*!
The updated covariance of the state \f${\bf P}_{k \mid k}\f$
where \f${\bf P}_{k \mid k} = \left({\bf I - W}_k {\bf H}
- \right) {\bf P}_{k \mid k-1}\f$.
+ \right) {\bf P}_{k \mid k-1}\f$.
*/
- vpMatrix Pest ;
+ vpMatrix Pest;
protected:
-
- /*!
+ /*!
Filter gain \f${\bf W}_k\f$ where \f$ {\bf W}_k = {\bf P}_{k
\mid k-1} {\bf H}^T \left[ {\bf H P}_{k \mid k-1} {\bf H}^T + {\bf
R}_k \right]^{-1}\f$.
*/
- vpMatrix W ;
+ vpMatrix W;
//! Identity matrix \f$ \bf I\f$.
- vpMatrix I ;
-} ;
-
-
+ vpMatrix I;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpLine.h b/modules/core/include/visp3/core/vpLine.h
index 7ff74b3..5f1acda 100644
--- a/modules/core/include/visp3/core/vpLine.h
+++ b/modules/core/include/visp3/core/vpLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpLine_H
#define vpLine_H
@@ -44,8 +44,8 @@
\brief class that defines what is a line
*/
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpForwardProjection.h>
@@ -57,7 +57,7 @@
camera frame and the image plane. All the parameters
must be set in meter.
- \par Object and camera frame parametrization:
+ \par Object and camera frame parametrization:
In the 3D frames, the object frame parameters (\e oP) and the camera
frame parameters (\e cP), the line is defined as the intersection
between two plans. Thus, the parameters which define the line are
@@ -84,7 +84,7 @@
in the vpTracker::cP public attribute, where \e cP is a vector
defined as: \f[ cP = \left[\begin{array}{c}A1_c \\ B1_c \\ C1_c \\
D1_c \\ A2_c \\ B2_c \\ C2_c \\ D2_c \end{array}\right] \f]
-
+
\par Image plane parametrization:
In the image plane, the line is defined thanks to its 2D equation.
\f[ x \; cos(\theta) + y \; sin(\theta) -\rho = 0 \f] Here \f$ x
@@ -106,10 +106,9 @@ class VISP_EXPORT vpLine : public vpForwardProjection
{
public:
+ void init();
- void init() ;
-
- vpLine() ;
+ vpLine();
//! Destructor
virtual ~vpLine() { ; }
@@ -122,7 +121,7 @@ public:
\sa setTheta()
*/
- void setRho(const double rho) { p[0] = rho ; };
+ void setRho(const double rho) { p[0] = rho; };
/*!
Sets the \f$ \theta \f$ angle value used to define the line in the
@@ -132,7 +131,7 @@ public:
\sa setRho()
*/
- void setTheta(const double theta) { p[1] = theta ;};
+ void setTheta(const double theta) { p[1] = theta; };
/*!
@@ -144,7 +143,7 @@ public:
\sa getRho()
*/
- double getTheta() const { return p[1] ; }
+ double getTheta() const { return p[1]; }
/*!
Gets the \f$ \rho \f$ value corresponding to one of the
@@ -155,41 +154,27 @@ public:
\sa getTheta()
*/
- double getRho() const { return p[0] ; }
-
-
-
- void setWorldCoordinates(const double &A1, const double &B1,
- const double &C1, const double &D1,
- const double &A2, const double &B2,
- const double &C2, const double &D2) ;
-
-
- void setWorldCoordinates(const vpColVector &oP1,
- const vpColVector &oP2) ;
-
+ double getRho() const { return p[0]; }
- void setWorldCoordinates(const vpColVector &oP) ;
+ void setWorldCoordinates(const double &A1, const double &B1, const double &C1, const double &D1, const double &A2,
+ const double &B2, const double &C2, const double &D2);
+ void setWorldCoordinates(const vpColVector &oP1, const vpColVector &oP2);
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
+ void setWorldCoordinates(const vpColVector &oP);
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
+ void projection();
+ void projection(const vpColVector &cP, vpColVector &p);
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP);
+ void changeFrame(const vpHomogeneousMatrix &cMo);
- vpLine *duplicate() const ;
-} ;
+ void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green,
+ const unsigned int thickness = 1);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
+ vpLine *duplicate() const;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h
index 7a8efe0..8d67e01 100644
--- a/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h
+++ b/modules/core/include/visp3/core/vpLinearKalmanFilterInstantiation.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,87 +52,75 @@
/*!
\class vpLinearKalmanFilterInstantiation
\ingroup group_core_kalman
- \brief This class provides an implementation of some specific linear Kalman filters.
+ \brief This class provides an implementation of some specific linear Kalman
+ filters.
*/
class VISP_EXPORT vpLinearKalmanFilterInstantiation : public vpKalmanFilter
{
- public:
- /*!
+public:
+ /*!
Selector used to set the Kalman filter state model.
- */
+ */
typedef enum {
/*! Consider the state as a constant velocity model with white
noise. Measures available are the succesive positions of the
target. To know more about this state model, see
initStateConstVel_MeasurePos(). */
- stateConstVel_MeasurePos,
+ stateConstVel_MeasurePos,
/*! Consider the state as a constant velocity model with colored noise
measurements as acceleration terms. Measured available are the
velocities of the target. To know more about this state model,
see initStateConstVelWithColoredNoise_MeasureVel(). */
- stateConstVelWithColoredNoise_MeasureVel,
+ stateConstVelWithColoredNoise_MeasureVel,
/*! Consider the state as a constant acceleration model with colored noise
measurements as acceleration terms. Measured available are the
velocities of the target. To know more about this state model,
see initStateConstAccWithColoredNoise_MeasureVel(). */
stateConstAccWithColoredNoise_MeasureVel,
/*! Used to indicate that the state model is not initialized. */
- unknown
+ unknown
} vpStateModel;
-
+
/*!
Default linear Kalman filter.
-
+
By default the state model is unknown and set to
vpLinearKalmanFilterInstantiation::unknown.
*/
- vpLinearKalmanFilterInstantiation() : model(unknown)
- {
- };
+ vpLinearKalmanFilterInstantiation() : model(unknown){};
/*! Destructor that does nothng. */
- virtual ~vpLinearKalmanFilterInstantiation() {};
+ virtual ~vpLinearKalmanFilterInstantiation(){};
/*!
Return the current state model.
*/
- inline vpStateModel getStateModel() {
- return model;
- }
+ inline vpStateModel getStateModel() { return model; }
void filter(vpColVector &z);
-
+
/*! @name Generic linear filter initializer */
//@{
inline void setStateModel(vpStateModel model);
- void initFilter(unsigned int nsignal, vpColVector &sigma_state,
- vpColVector &sigma_measure, double rho, double dt);
+ 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:
+protected:
vpStateModel model;
-
-} ;
+};
/*!
Set the Kalman state model. Depending on the state model, we set
@@ -148,14 +137,17 @@ 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
*/
-void vpLinearKalmanFilterInstantiation::setStateModel(vpStateModel mdl) {
+void vpLinearKalmanFilterInstantiation::setStateModel(vpStateModel mdl)
+{
this->model = mdl;
- switch(model) {
+ switch (model) {
case stateConstVel_MeasurePos:
case stateConstVelWithColoredNoise_MeasureVel:
size_state = 2;
@@ -172,5 +164,4 @@ void vpLinearKalmanFilterInstantiation::setStateModel(vpStateModel mdl) {
}
}
-
#endif
diff --git a/modules/core/include/visp3/core/vpList.h b/modules/core/include/visp3/core/vpList.h
index a8addce..e62055f 100644
--- a/modules/core/include/visp3/core/vpList.h
+++ b/modules/core/include/visp3/core/vpList.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,11 +37,9 @@
*
*****************************************************************************/
-
#ifndef VP_LIST_H
#define VP_LIST_H
-
/*!
\file vpList.h
\brief Definition of the list managment class
@@ -58,30 +57,29 @@
\class vpListElement
\brief Each element of a list
*/
-template <class type>
-class vpListElement
-{
-//private:
-// vpListElement(const vpListElement &)
-// : prev(NULL), next(NULL), val()
-// {
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// }
-// vpListElement &operator=(const vpListElement &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
+template <class type> class vpListElement
+{
+ // private:
+ // vpListElement(const vpListElement &)
+ // : prev(NULL), next(NULL), val()
+ // {
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!");
+ // }
+ // vpListElement &operator=(const vpListElement &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
public:
- vpListElement() : prev(NULL), next(NULL), val() {};
+ vpListElement() : prev(NULL), next(NULL), val(){};
vpListElement<type> *prev; ///! pointer to the previous element in the list
vpListElement<type> *next; ///! pointer to the next element in the list
type val; ///! value of the element
-} ;
+};
#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-
/*!
\class vpList
\brief Provide simple list management
@@ -112,16 +110,13 @@ public:
\endverbatim
*/
-
-
-template <class type>
-class vpList
+template <class type> class vpList
{
- private:
- void init() ;
- public:
+private:
+ void init();
- unsigned int nb; ///! number of items in the List
+public:
+ unsigned int nb; ///! number of items in the List
/*!
\brief the first virtual item in the list
\verbatim
@@ -145,79 +140,73 @@ class vpList
^
\endverbatim
*/
- vpListElement<type> *cur; // the current element
- public:
- vpList() ; // constr.
- vpList(const vpList& l); // cloning
- virtual ~vpList(); // destr.
-
- inline void next(void) ; // current element's successor ( cur = cur->next )
- inline void previous(void) ; // current element's predecessor ( cur = cur->pred )
- inline void front(void) ; // go to the front of the List (cur = first)
- inline void end(void) ; // go back to the end of the List ( cur = last )
- inline bool outside(void) const; // test whether we are outside the List
+ vpListElement<type> *cur; // the current element
+public:
+ vpList(); // constr.
+ vpList(const vpList &l); // cloning
+ virtual ~vpList(); // destr.
- bool empty(void) const; // tests whether the List is empty
+ 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
- inline type& value(void); // returns the current element value
- inline const type& value(void) const; // returns the current element value
+ bool empty(void) const; // tests whether the List is empty
- void suppress(void); // deletes the current item
- void kill(); // empties the List
+ inline type &value(void); // returns the current element value
+ inline const type &value(void) const; // returns the current element value
- void display() ; // displays the content of the list
- void print() {display() ;} // displays the content of the list
+ void suppress(void); // deletes the current item
+ void kill(); // empties the List
+ void display(); // displays the content of the list
+ void print() { display(); } // displays the content of the list
- inline void addRight(const type& el); // inserts an element on the right
- inline void addLeft(const type& el); // inserts an element on the left
- inline void modify(const type& el); // modifies thevalue field of the curr. el.
- inline void addRight(type& el); // inserts an element on the right
- inline void addLeft(type& el); // inserts an element on the left
- inline void swapLeft(); // Switch the current element with the element on the left
- inline void swapRight(); // Switch the current element with the element on the right
- inline unsigned int nbElement(void); // returns the number of items currently in the list
- inline unsigned int nbElements(void); // returns the number of items currently in the list
+ inline void addRight(const type &el); // inserts an element on the right
+ inline void addLeft(const type &el); // inserts an element on the left
+ inline void modify(const type &el); // modifies thevalue field of the curr. el.
+ inline void addRight(type &el); // inserts an element on the right
+ inline void addLeft(type &el); // inserts an element on the left
+ inline void swapLeft(); // Switch the current element with the element on the left
+ inline void swapRight(); // Switch the current element with the element on the right
+ inline unsigned int nbElement(void); // returns the number of items currently in the list
+ inline unsigned int nbElements(void); // returns the number of items currently in the list
- vpList<type>& operator=(const vpList<type>& l);
- inline void operator+=(vpList<type>& l);
- inline void operator+=(const type& l);
+ vpList<type> &operator=(const vpList<type> &l);
+ inline void operator+=(vpList<type> &l);
+ inline void operator+=(const type &l);
// Other non fundamental member (may be somehow useful)
bool nextOutside(void) const; // test whether we are outside the List
- bool previousOutside(void) const;// test whether we are outside the List
-
-
- type& previousValue(void); // returns the previous element value
- type& nextValue(void); // returns the next element value
- type& firstValue(void) ;
- type& lastValue(void) ;
-
+ bool previousOutside(void) const; // test whether we are outside the List
+ type &previousValue(void); // returns the previous element value
+ type &nextValue(void); // returns the next element value
+ type &firstValue(void);
+ type &lastValue(void);
};
-
/*!
\brief initialization, Create an empty list
\verbatim
init() --> [*, *]
\endverbatim */
-template<class type>
-void vpList<type>::init()
+template <class type> void vpList<type>::init()
{
- vpListElement<type> *x=new vpListElement<type>;
- vpListElement<type> *y=new vpListElement<type> ;
+ vpListElement<type> *x = new vpListElement<type>;
+ vpListElement<type> *y = new vpListElement<type>;
- first = x ;
- last = y ;
+ first = x;
+ last = y;
- x->prev = NULL ;
- x->next = y ;
- y->prev = x ;
- y->next =NULL ;
+ x->prev = NULL;
+ x->next = y;
+ y->prev = x;
+ y->next = NULL;
- cur = x ;
- nb = 0 ;
+ cur = x;
+ nb = 0;
}
/*!
@@ -227,45 +216,29 @@ void vpList<type>::init()
\endverbatim
\sa init()
*/
-template<class type>
-vpList<type>::vpList() : nb(0), first(NULL), last(NULL), cur(NULL)
-{
- init() ;
-}
-
-
+template <class type> vpList<type>::vpList() : nb(0), first(NULL), last(NULL), cur(NULL) { init(); }
/*!
\brief vpList destructor
\sa kill()
*/
-template<class type>
-vpList<type>::~vpList()
+template <class type> vpList<type>::~vpList()
{
- kill() ;
+ kill();
- /*if (first != NULL) */ delete first ;
- /*if (last != NULL) */ delete last ;
+ /*if (first != NULL) */ delete first;
+ /*if (last != NULL) */ delete last;
}
/*!
\brief return the number of element in the list
*/
-template<class type>
-unsigned int vpList<type>::nbElement(void)
-{
- return(nb) ;
-}
+template <class type> unsigned int vpList<type>::nbElement(void) { return (nb); }
/*!
\brief return the number of element in the list
*/
-template<class type>
-unsigned int vpList<type>::nbElements(void)
-{
- return(nb) ;
-}
-
+template <class type> unsigned int vpList<type>::nbElements(void) { return (nb); }
/*!
\brief position the current element on the next one
@@ -274,12 +247,7 @@ unsigned int vpList<type>::nbElements(void)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::next(void)
-{
- cur = cur->next ;
-}
-
+template <class type> void vpList<type>::next(void) { cur = cur->next; }
/*!
\brief position the current element on the previous one
@@ -288,11 +256,7 @@ void vpList<type>::next(void)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::previous(void)
-{
- cur = cur->prev ;
-}
+template <class type> void vpList<type>::previous(void) { cur = cur->prev; }
/*!
\brief return the value of the current element
@@ -302,11 +266,7 @@ void vpList<type>::previous(void)
^
\endverbatim
*/
-template<class type>
-type& vpList<type>::value(void)
-{
- return(cur->val) ;
-}
+template <class type> type &vpList<type>::value(void) { return (cur->val); }
/*!
\brief return the value of the current element using a const ref.
@@ -316,11 +276,7 @@ type& vpList<type>::value(void)
^
\endverbatim
*/
-template<class type>
-const type& vpList<type>::value(void) const
-{
- return(cur->val) ;
-}
+template <class type> const type &vpList<type>::value(void) const { return (cur->val); }
/*!
\brief return the value of the previous element
@@ -330,11 +286,7 @@ const type& vpList<type>::value(void) const
^
\endverbatim
*/
-template<class type>
-type& vpList<type>::previousValue(void)
-{
- return(cur->prev->val) ;
-}
+template <class type> type &vpList<type>::previousValue(void) { return (cur->prev->val); }
/*!
\brief return the value of the next element
@@ -343,13 +295,7 @@ type& vpList<type>::previousValue(void)
^
\endverbatim
*/
-template<class type>
-type& vpList<type>::nextValue(void)
-{
- return(cur->next->val) ;
-}
-
-
+template <class type> type &vpList<type>::nextValue(void) { return (cur->next->val); }
/*!
\brief return the first element of the list
@@ -357,25 +303,14 @@ type& vpList<type>::nextValue(void)
[*, a, b, c, d, *] --> firstValue() return a
\endverbatim
*/
-template<class type>
-type& vpList<type>::firstValue(void)
-{
- return(first->next->val) ;
-}
-
-
+template <class type> type &vpList<type>::firstValue(void) { return (first->next->val); }
/*!\brief return the last element of the list
\verbatim
[*, a, b, c, d, *] --> lastValue() return d
\endverbatim
*/
-template<class type>
-type& vpList<type>::lastValue(void)
-{
- return(last->prev->val) ;
-}
-
+template <class type> type &vpList<type>::lastValue(void) { return (last->prev->val); }
/*!
\brief Position the current element on the first element of the list
@@ -385,11 +320,7 @@ type& vpList<type>::lastValue(void)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::front(void)
-{
- cur = first->next ;
-}
+template <class type> void vpList<type>::front(void) { cur = first->next; }
/*!
\brief Position the current element on the last element of the list
@@ -399,11 +330,7 @@ void vpList<type>::front(void)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::end(void)
-{
- cur = last->prev ;
-}
+template <class type> void vpList<type>::end(void) { cur = last->prev; }
/*!
\brief Test if the list is empty
@@ -413,11 +340,7 @@ void vpList<type>::end(void)
[*, *] --> empty return true
\endverbatim
*/
-template<class type>
-bool vpList<type>::empty(void) const
-{
- return((first->next == last) &&( first == last->prev)) ;
-}
+template <class type> bool vpList<type>::empty(void) const { return ((first->next == last) && (first == last->prev)); }
/*!
\brief Test if the current element is outside the list
@@ -430,12 +353,7 @@ bool vpList<type>::empty(void) const
^ or ^
\endverbatim
*/
-template<class type>
-bool vpList<type>::outside(void) const
-{
-
- return((cur==first)||(cur==last)) ;
-}
+template <class type> bool vpList<type>::outside(void) const { return ((cur == first) || (cur == last)); }
/*!
\brief Test if the next element is outside the list
@@ -446,13 +364,11 @@ bool vpList<type>::outside(void) const
^
\endverbatim
*/
-template<class type>
-bool vpList<type>::nextOutside(void) const
+template <class type> bool vpList<type>::nextOutside(void) const
{
- return((cur->next==first)||(cur->next==last)) ;
+ return ((cur->next == first) || (cur->next == last));
}
-
/*!
\brief Test if the previous element is outside the list
(ie if the current element is the firts one)
@@ -462,13 +378,11 @@ bool vpList<type>::nextOutside(void) const
^
\endverbatim
*/
-template<class type>
-bool vpList<type>::previousOutside(void) const
+template <class type> bool vpList<type>::previousOutside(void) const
{
- return((cur->prev==first)||(cur->prev==last)) ;
+ return ((cur->prev == first) || (cur->prev == last));
}
-
/*!
\brief add a new element in the list, at the right of the current one
@@ -479,29 +393,25 @@ bool vpList<type>::previousOutside(void) const
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::addRight(const type& v)
+template <class type> void vpList<type>::addRight(const type &v)
{
- vpListElement<type> *x=new vpListElement<type>;
+ vpListElement<type> *x = new vpListElement<type>;
- x->val = v ;
- if (empty())
- {
- cur = first ;
+ x->val = v;
+ if (empty()) {
+ cur = first;
+ } else {
+ if (outside())
+ std::cout << "vpList: outside with addRight " << std::endl;
}
- else
- {
- if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
- }
- cur->next->prev = x ;
- x->next = cur->next ;
- x->prev = cur ;
- cur->next = x ;
- cur = x ;
- nb++ ;
+ cur->next->prev = x;
+ x->next = cur->next;
+ x->prev = cur;
+ cur->next = x;
+ cur = x;
+ nb++;
}
-
/*!
\brief add a new element in the list, at the left of the current one
@@ -512,28 +422,24 @@ void vpList<type>::addRight(const type& v)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::addLeft(const type& v)
+template <class type> void vpList<type>::addLeft(const type &v)
{
- vpListElement<type> *x=new vpListElement<type>;
+ vpListElement<type> *x = new vpListElement<type>;
- x->val = v ;
+ x->val = v;
- if (empty())
- {
- cur = last ;
+ if (empty()) {
+ cur = last;
+ } else {
+ if (outside())
+ std::cout << "vpList: outside with addLeft " << std::endl;
}
- else
- {
- if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
- }
- x->next = cur ;
- x->prev = cur->prev ;
- cur->prev->next = x ;
- cur->prev = x ;
- cur = x ;
- nb++ ;
-
+ x->next = cur;
+ x->prev = cur->prev;
+ cur->prev->next = x;
+ cur->prev = x;
+ cur = x;
+ nb++;
}
/*!
@@ -546,29 +452,25 @@ void vpList<type>::addLeft(const type& v)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::addRight(type& v)
+template <class type> void vpList<type>::addRight(type &v)
{
- vpListElement<type> *x=new vpListElement<type>;
+ vpListElement<type> *x = new vpListElement<type>;
- x->val = v ;
- if (empty())
- {
- cur = first ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addRight " << std::endl ;
+ x->val = v;
+ if (empty()) {
+ cur = first;
+ } else {
+ if (outside())
+ std::cout << "vpList: outside with addRight " << std::endl;
}
- cur->next->prev = x ;
- x->next = cur->next ;
- x->prev = cur ;
- cur->next = x ;
- cur = x ;
- nb++ ;
+ cur->next->prev = x;
+ x->next = cur->next;
+ x->prev = cur;
+ cur->next = x;
+ cur = x;
+ nb++;
}
-
/*!
\brief add a new element in the list, at the left of the current one
@@ -579,28 +481,24 @@ void vpList<type>::addRight(type& v)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::addLeft(type& v)
+template <class type> void vpList<type>::addLeft(type &v)
{
- vpListElement<type> *x=new vpListElement<type>;
+ vpListElement<type> *x = new vpListElement<type>;
- x->val = v ;
+ x->val = v;
- if (empty())
- {
- cur = last ;
- }
- else
- {
- if (outside()) std::cout << "vpList: outside with addLeft " << std::endl ;
+ if (empty()) {
+ cur = last;
+ } else {
+ if (outside())
+ std::cout << "vpList: outside with addLeft " << std::endl;
}
- x->next = cur ;
- x->prev = cur->prev ;
- cur->prev->next = x ;
- cur->prev = x ;
- cur = x ;
- nb++ ;
-
+ x->next = cur;
+ x->prev = cur->prev;
+ cur->prev->next = x;
+ cur->prev = x;
+ cur = x;
+ nb++;
}
/*!
@@ -611,11 +509,7 @@ void vpList<type>::addLeft(type& v)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::modify(const type& v)
-{
- cur->val = v ;
-}
+template <class type> void vpList<type>::modify(const type &v) { cur->val = v; }
/*!
\brief Switch the current element with the element on the left
@@ -625,11 +519,9 @@ void vpList<type>::modify(const type& v)
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::swapLeft()
+template <class type> void vpList<type>::swapLeft()
{
- if (cur->prev != first)
- {
+ if (cur->prev != first) {
cur->prev->prev->next = cur;
cur->next->prev = cur->prev;
@@ -637,17 +529,15 @@ void vpList<type>::swapLeft()
vpListElement<type> *prevTmp;
nextTmp = cur->next;
- prevTmp = cur->prev;
+ prevTmp = cur->prev;
cur->next = cur->prev;
cur->prev = cur->prev->prev;
prevTmp->prev = cur;
prevTmp->next = nextTmp;
- }
- else
- {
- std::cout << "vpList: previous element is outside (swapLeft) " << std::endl ;
+ } else {
+ std::cout << "vpList: previous element is outside (swapLeft) " << std::endl;
}
}
@@ -659,11 +549,9 @@ void vpList<type>::swapLeft()
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::swapRight()
+template <class type> void vpList<type>::swapRight()
{
- if (cur->next != last)
- {
+ if (cur->next != last) {
cur->prev->next = cur->next;
cur->next->next->prev = cur;
@@ -671,17 +559,15 @@ void vpList<type>::swapRight()
vpListElement<type> *prevTmp;
nextTmp = cur->next;
- prevTmp = cur->prev;
+ prevTmp = cur->prev;
cur->next = nextTmp->next;
cur->prev = nextTmp;
nextTmp->prev = prevTmp;
nextTmp->next = cur;
- }
- else
- {
- std::cout << "vpList: next element is outside (swapRight) " << std::endl ;
+ } else {
+ std::cout << "vpList: next element is outside (swapRight) " << std::endl;
}
}
@@ -693,20 +579,15 @@ void vpList<type>::swapRight()
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::kill()
+template <class type> void vpList<type>::kill()
{
- front() ;
- while (!empty())
- {
- suppress() ;
+ front();
+ while (!empty()) {
+ suppress();
}
-
}
-
-
/*!
\brief suppress the current element
@@ -717,50 +598,43 @@ void vpList<type>::kill()
^ ^
\endverbatim
*/
-template<class type>
-void vpList<type>::suppress(void)
+template <class type> void vpList<type>::suppress(void)
{
- vpListElement<type> *x ;
-
- cur->prev->next = cur->next ;
- cur->next->prev = cur->prev ;
- x = cur ;
- cur = cur->next ;
-
- if (x!=NULL) delete x ;
+ vpListElement<type> *x;
- nb-- ;
+ cur->prev->next = cur->next;
+ cur->next->prev = cur->prev;
+ x = cur;
+ cur = cur->next;
+ if (x != NULL)
+ delete x;
+ nb--;
}
-
-
-
/*!
\brief Copy constructor const
\param l : the list to copy
*/
-template<class type>
-vpList<type>& vpList<type>::operator=(const vpList<type>& l)
+template <class type> vpList<type> &vpList<type>::operator=(const vpList<type> &l)
{
- type x ;
- vpListElement<type> *e ;
-
- kill() ;
- e = l.first->next ;
- front() ;
- while (e!=l.last)
- {
- x = e->val ;
- addRight(x) ;
- e = e->next ;
+ type x;
+ vpListElement<type> *e;
+
+ kill();
+ e = l.first->next;
+ front();
+ while (e != l.last) {
+ x = e->val;
+ addRight(x);
+ e = e->next;
}
- nb = l.nb ;
- cur = first->next ;
+ nb = l.nb;
+ cur = first->next;
return *this;
}
@@ -773,18 +647,16 @@ vpList<type>& vpList<type>::operator=(const vpList<type>& l)
^
\endverbatim
*/
-template<class type>
-void vpList<type>::operator+=(vpList<type>& l)
+template <class type> void vpList<type>::operator+=(vpList<type> &l)
{
- type x ;
-
- l.front() ;
- end() ;
- while (!l.outside())
- {
- x = l.value() ;
- addRight(x) ;
- l.next() ;
+ type x;
+
+ l.front();
+ end();
+ while (!l.outside()) {
+ x = l.value();
+ addRight(x);
+ l.next();
}
}
@@ -796,46 +668,39 @@ void vpList<type>::operator+=(vpList<type>& l)
^
\endverbatim
*/
-template<class type>
-void vpList<type>::operator += (const type& l)
+template <class type> void vpList<type>::operator+=(const type &l)
{
- end() ;
- addRight(l) ;
+ end();
+ addRight(l);
}
-
/*!
\brief copy constructor
-
+
\param l : the list to copy
*/
-template<class type>
-vpList<type>::vpList(const vpList<type>& l)
- : nb(0), first(NULL), last(NULL), cur(NULL)
+template <class type> vpList<type>::vpList(const vpList<type> &l) : nb(0), first(NULL), last(NULL), cur(NULL)
{
- init() ;
+ init();
*this = l;
}
/*!
\brief Print (std::cout) all the element of the list
*/
-template<class type>
-void vpList<type>::display()
-{
- unsigned int k = 1 ;
- front() ;
- while(!outside()) {
- std::cout<<k<<" ---> "<<value()<<std::endl ;
- next() ;
- k++ ;
+template <class type> void vpList<type>::display()
+{
+ unsigned int k = 1;
+ front();
+ while (!outside()) {
+ std::cout << k << " ---> " << value() << std::endl;
+ next();
+ k++;
}
- std::cout<< std::endl << std::endl ;
+ std::cout << std::endl << std::endl;
}
-#endif /* #ifndef VP_LIST_H */
-
-
+#endif /* #ifndef VP_LIST_H */
/*
* Local variables:
diff --git a/modules/core/include/visp3/core/vpMath.h b/modules/core/include/visp3/core/vpMath.h
index 8735990..99c81be 100644
--- a/modules/core/include/visp3/core/vpMath.h
+++ b/modules/core/include/visp3/core/vpMath.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,39 +42,37 @@
the C mathematics library (math.h)
*/
-
-
#ifndef vpMATH_HH
#define vpMATH_HH
#include <visp3/core/vpConfig.h>
-#include <math.h>
-#include <limits>
-#include <climits>
#include <algorithm>
+#include <climits>
+#include <limits>
+#include <math.h>
#include <vector>
-#if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
-# include <cmath>
+#if defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN) || defined(VISP_HAVE_FUNC_ISINF) || \
+ defined(VISP_HAVE_FUNC_STD_ISINF) || defined(VISP_HAVE_FUNC_STD_ROUND)
+#include <cmath>
#endif
-#if defined(_WIN32) // Not defined in Microsoft math.h
+#if defined(_WIN32) // Not defined in Microsoft math.h
-# ifndef M_PI
-# define M_PI 3.14159265358979323846
-# endif
-
-# ifndef M_PI_2
-# define M_PI_2 (M_PI/2.0)
-# endif
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
-# ifndef M_PI_4
-# define M_PI_4 (M_PI/4.0)
-# endif
+#ifndef M_PI_2
+#define M_PI_2 (M_PI / 2.0)
+#endif
+#ifndef M_PI_4
+#define M_PI_4 (M_PI / 4.0)
#endif
+#endif
/*!
\class vpMath
@@ -86,50 +85,48 @@
class VISP_EXPORT vpMath
{
- public:
-
+public:
/*!
Convert an angle in radians into degrees.
\param rad : Angle in radians.
\return Angle converted in degrees.
*/
- static inline double deg(double rad) { return (rad*180.0)/M_PI ; }
+ static inline double deg(double rad) { return (rad * 180.0) / M_PI; }
/*!
Convert an angle in degrees into radian.
\param deg : Angle in degrees.
\return Angle converted in radian.
*/
- static inline double rad(double deg) { return (deg*M_PI)/180.0 ; }
+ static inline double rad(double deg) { return (deg * M_PI) / 180.0; }
/*!
Compute x square value.
\return Square value \f$ x^2 \f$.
*/
- static inline double sqr(double x) { return x*x ; }
+ static inline double sqr(double x) { return x * x; }
// factorial of x
- static inline double fact(unsigned int x) ;
+ static inline double fact(unsigned int x);
// combinaison
- static inline long double comb(unsigned int n, unsigned int p) ;
+ static inline long double comb(unsigned int n, unsigned int p);
// round x to the nearest integer
- static inline int round(const double x) ;
+ static inline int round(const double x);
// return the sign of x (+-1)
- static inline int (sign)(double x) ;
+ static inline int(sign)(double x);
// test if a number equals 0 (with threshold value)
- static inline bool nul(double x, double s=0.001);
+ static inline bool nul(double x, double s = 0.001);
// test if two numbers are equals (with a user defined threshold)
- static inline bool equal(double x, double y, double s=0.001);
+ static inline bool equal(double x, double y, double s = 0.001);
// test if a number is greater than another (with a user defined threshold)
- static inline bool greater(double x, double y, double s=0.001);
-
+ static inline bool greater(double x, double y, double s = 0.001);
/*!
Find the maximum between two numbers (or other).
@@ -137,10 +134,7 @@ class VISP_EXPORT vpMath
\param b : Second number.
\return The maximum of the two numbers.
*/
- template <class Type> static Type maximum(const Type& a, const Type& b)
- {
- return (a > b) ? a : b;
- }
+ template <class Type> static Type maximum(const Type &a, const Type &b) { return (a > b) ? a : b; }
/*!
Find the minimum between two numbers (or other).
@@ -148,30 +142,23 @@ class VISP_EXPORT vpMath
\param b : Second number.
\return The minimum of the two numbers.
*/
- template <class Type> static Type minimum(const Type& a, const Type& b)
- {
- return (a < b) ? a : b;
- }
+ template <class Type> static Type minimum(const Type &a, const Type &b) { return (a < b) ? a : b; }
/*!
Find the absolute value of a number (or other).
\param x : The number.
\return The absolute value of x
*/
- template <class Type> static Type abs(const Type& x)
- {
- return (x < 0) ? -x : x;
- }
-
+ template <class Type> static Type abs(const Type &x) { return (x < 0) ? -x : x; }
// sinus cardinal
- static double sinc(double x) ;
- static double sinc(double sinx, double x) ;
- static double mcosc(double cosx, double x) ;
- static double msinc(double sinx, double x) ;
+ static double sinc(double x);
+ static double sinc(double sinx, double x);
+ static double mcosc(double cosx, double x);
+ static double msinc(double sinx, double x);
// sigmoid
- static inline double sigmoid(double x, double x0=0.,double x1=1., double n=12.);
+ static inline double sigmoid(double x, double x0 = 0., double x1 = 1., double n = 12.);
/*!
Exchange two numbers.
@@ -179,7 +166,7 @@ class VISP_EXPORT vpMath
\param a First number to exchange.
\param b Second number to exchange
*/
- template <class Type> static void swap(Type& a, Type& b)
+ template <class Type> static void swap(Type &a, Type &b)
{
Type tmp = b;
b = a;
@@ -189,29 +176,27 @@ class VISP_EXPORT vpMath
static bool isNaN(const double value);
static bool isInf(const double value);
- template<typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
- template<typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(unsigned char v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(char v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(unsigned short v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(short v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(unsigned v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(int v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(float v) { return _Tp(v); }
+ template <typename _Tp> static inline _Tp saturate(double v) { return _Tp(v); }
static double getMean(const std::vector<double> &v);
static double getMedian(const std::vector<double> &v);
- static double getStdev(const std::vector<double> &v, const bool useBesselCorrection=false);
+ static double getStdev(const std::vector<double> &v, const bool useBesselCorrection = false);
static int modulo(const int a, const int n);
- private:
+private:
static const double ang_min_sinc;
static const double ang_min_mc;
};
-
-
-//Begining of the inline functions definition
+// Begining of the inline functions definition
/*!
Computes and returns x!
@@ -219,8 +204,9 @@ class VISP_EXPORT vpMath
*/
double vpMath::fact(unsigned int x)
{
- if ( (x == 1) || (x == 0)) return 1;
- return x * fact(x-1);
+ if ((x == 1) || (x == 0))
+ return 1;
+ return x * fact(x - 1);
}
/*!
@@ -233,11 +219,11 @@ double vpMath::fact(unsigned int x)
*/
long double vpMath::comb(unsigned int n, unsigned int p)
{
- if (n == p) return 1;
- return fact(n)/ (fact(n-p) * fact(p));
+ if (n == p)
+ return 1;
+ return fact(n) / (fact(n - p) * fact(p));
}
-
/*!
Round x to the nearest integer.
@@ -249,12 +235,13 @@ long double vpMath::comb(unsigned int n, unsigned int p)
int vpMath::round(const double x)
{
#if defined(VISP_HAVE_FUNC_ROUND)
- //:: to design the global namespace and avoid to call recursively vpMath::round
+ //:: to design the global namespace and avoid to call recursively
+ // vpMath::round
return (int)::round(x);
#elif defined(VISP_HAVE_FUNC_STD_ROUND)
return (int)std::round(x);
#else
- return (x > 0.0) ? ((int) floor(x + 0.5)) : ((int) ceil(x - 0.5));
+ return (x > 0.0) ? ((int)floor(x + 0.5)) : ((int)ceil(x - 0.5));
#endif
}
@@ -266,13 +253,13 @@ int vpMath::round(const double x)
*/
int
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-(
+ (
#endif
- vpMath::sign
+ vpMath::sign
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-)
+ )
#endif
-(double x)
+ (double x)
{
if (fabs(x) < std::numeric_limits<double>::epsilon())
return 0;
@@ -291,10 +278,7 @@ int
\return true if \f$ | x | < s \f$.
*/
-bool vpMath::nul(double x, double s)
-{
- return(fabs(x)<s);
-}
+bool vpMath::nul(double x, double s) { return (fabs(x) < s); }
/*!
Compares \f$ | x - y | \f$ to \f$ s \f$.
@@ -303,10 +287,7 @@ bool vpMath::nul(double x, double s)
\param s : Tolerance threshold.
\return true if \f$ | x - y | < s \f$.
*/
-bool vpMath::equal(double x, double y, double s)
-{
- return( nul(x-y, s) );
-}
+bool vpMath::equal(double x, double y, double s) { return (nul(x - y, s)); }
/*!
Compares \f$ x \f$ to \f$ y - s \f$.
@@ -315,176 +296,175 @@ bool vpMath::equal(double x, double y, double s)
\param s : Tolerance threshold.
\return true if \f$ x > y - s \f$.
*/
-bool vpMath::greater(double x, double y, double s)
-{
- return(x>(y-s));
-}
+bool vpMath::greater(double x, double y, double s) { return (x > (y - s)); }
/*!
- Sigmoid function between [x0,x1] with \f$ s(x)=0 if x\le x0\f$ and \f$ s(x)=1 if x \ge x1 \f$
- \param x : Value of x.
- \param x0 : Lower bound (default 0).
+ Sigmoid function between [x0,x1] with \f$ s(x)=0 if x\le x0\f$ and \f$ s(x)=1
+if x \ge x1 \f$ \param x : Value of x. \param x0 : Lower bound (default 0).
\param x1 : Upper bound (default 1).
\param n : Degree of the exponential (default 12).
\return Sigmoid value \f$1/(1+exp(-n*((x-x0)/(x1-x0)-0.5)))\f$
*/
-double vpMath::sigmoid(double x, double x0,double x1, double n)
+double vpMath::sigmoid(double x, double x0, double x1, double n)
{
- if(x < x0)
- return 0.;
- else if(x > x1)
- return 1.;
- double l0 = 1./(1.+exp(0.5*n));
- double l1 = 1./(1.+exp(-0.5*n));
- return (1./(1.+exp(-n*((x-x0)/(x1-x0)-0.5)))-l0)/(l1-l0);
+ if (x < x0)
+ return 0.;
+ else if (x > x1)
+ return 1.;
+ double l0 = 1. / (1. + exp(0.5 * n));
+ double l1 = 1. / (1. + exp(-0.5 * n));
+ return (1. / (1. + exp(-n * ((x - x0) / (x1 - x0) - 0.5))) - l0) / (l1 - l0);
}
-//unsigned char
-template<> inline unsigned char vpMath::saturate<unsigned char>(char v) {
+// unsigned char
+template <> inline unsigned char vpMath::saturate<unsigned char>(char v)
+{
// On big endian arch like powerpc, char implementation is unsigned
// with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
// leading to (int)(char -127) = 129.
// On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
// (int)(char -127) = -127.
if (std::numeric_limits<char>::is_signed)
- return (unsigned char) ((std::max)((int) v, 0));
+ return (unsigned char)(((std::max))((int)v, 0));
else
- return (unsigned char) ((unsigned int) v > SCHAR_MAX ? 0 : v);
+ return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
}
-template<> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v) {
- return (unsigned char) (std::min)((unsigned int) v, (unsigned int) UCHAR_MAX);
+template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned short v)
+{
+ return (unsigned char)((std::min))((unsigned int)v, (unsigned int)UCHAR_MAX);
}
-template<> inline unsigned char vpMath::saturate<unsigned char>(int v) {
- return (unsigned char) ((unsigned int) v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
+template <> inline unsigned char vpMath::saturate<unsigned char>(int v)
+{
+ return (unsigned char)((unsigned int)v <= UCHAR_MAX ? v : v > 0 ? UCHAR_MAX : 0);
}
-template<> inline unsigned char vpMath::saturate<unsigned char>(short v) {
- return saturate<unsigned char> ((int) v);
-}
+template <> inline unsigned char vpMath::saturate<unsigned char>(short v) { return saturate<unsigned char>((int)v); }
-template<> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v) {
- return (unsigned char) (std::min)(v, (unsigned int) UCHAR_MAX);
+template <> inline unsigned char vpMath::saturate<unsigned char>(unsigned int v)
+{
+ return (unsigned char)((std::min))(v, (unsigned int)UCHAR_MAX);
}
-template<> inline unsigned char vpMath::saturate<unsigned char>(float v) {
+template <> inline unsigned char vpMath::saturate<unsigned char>(float v)
+{
int iv = vpMath::round(v);
- return saturate<unsigned char> (iv);
+ return saturate<unsigned char>(iv);
}
-template<> inline unsigned char vpMath::saturate<unsigned char>(double v) {
+template <> inline unsigned char vpMath::saturate<unsigned char>(double v)
+{
int iv = vpMath::round(v);
- return saturate<unsigned char> (iv);
+ return saturate<unsigned char>(iv);
}
-//char
-template<> inline char vpMath::saturate<char>(unsigned char v) {
- return (char) (std::min)((int) v, SCHAR_MAX);
-}
+// char
+template <> inline char vpMath::saturate<char>(unsigned char v) { return (char)((std::min))((int)v, SCHAR_MAX); }
-template<> inline char vpMath::saturate<char>(unsigned short v) {
- return (char) (std::min)((unsigned int) v, (unsigned int) SCHAR_MAX);
+template <> inline char vpMath::saturate<char>(unsigned short v)
+{
+ return (char)((std::min))((unsigned int)v, (unsigned int)SCHAR_MAX);
}
-template<> inline char vpMath::saturate<char>(int v) {
- return (char) ((unsigned int) (v - SCHAR_MIN) <= (unsigned int) UCHAR_MAX ? v :
- v > 0 ? SCHAR_MAX : SCHAR_MIN);
+template <> inline char vpMath::saturate<char>(int v)
+{
+ return (char)((unsigned int)(v - SCHAR_MIN) <= (unsigned int)UCHAR_MAX ? v : v > 0 ? SCHAR_MAX : SCHAR_MIN);
}
-template<> inline char vpMath::saturate<char>(short v) {
- return saturate<char>((int) v);
-}
+template <> inline char vpMath::saturate<char>(short v) { return saturate<char>((int)v); }
-template<> inline char vpMath::saturate<char>(unsigned int v) {
- return (char) (std::min)(v, (unsigned int) SCHAR_MAX);
+template <> inline char vpMath::saturate<char>(unsigned int v)
+{
+ return (char)((std::min))(v, (unsigned int)SCHAR_MAX);
}
-template<> inline char vpMath::saturate<char>(float v) {
+template <> inline char vpMath::saturate<char>(float v)
+{
int iv = vpMath::round(v);
return saturate<char>(iv);
}
-template<> inline char vpMath::saturate<char>(double v) {
+template <> inline char vpMath::saturate<char>(double v)
+{
int iv = vpMath::round(v);
return saturate<char>(iv);
}
-//unsigned short
-template<> inline unsigned short vpMath::saturate<unsigned short>(char v) {
+// unsigned short
+template <> inline unsigned short vpMath::saturate<unsigned short>(char v)
+{
// On big endian arch like powerpc, char implementation is unsigned
// with CHAR_MIN=0, CHAR_MAX=255 and SCHAR_MIN=-128, SCHAR_MAX=127
// leading to (int)(char -127) = 129.
// On little endian arch, CHAR_MIN=-127 and CHAR_MAX=128 leading to
// (int)(char -127) = -127.
if (std::numeric_limits<char>::is_signed)
- return (unsigned char) ((std::max)((int) v, 0));
+ return (unsigned char)(((std::max))((int)v, 0));
else
- return (unsigned char) ((unsigned int) v > SCHAR_MAX ? 0 : v);
+ return (unsigned char)((unsigned int)v > SCHAR_MAX ? 0 : v);
}
-template<> inline unsigned short vpMath::saturate<unsigned short>(short v) {
- return (unsigned short) (std::max)((int) v, 0);
+template <> inline unsigned short vpMath::saturate<unsigned short>(short v)
+{
+ return (unsigned short)((std::max))((int)v, 0);
}
-template<> inline unsigned short vpMath::saturate<unsigned short>(int v) {
- return (unsigned short) ((unsigned int) v <= (unsigned int) USHRT_MAX ? v :
- v > 0 ? USHRT_MAX : 0);
+template <> inline unsigned short vpMath::saturate<unsigned short>(int v)
+{
+ return (unsigned short)((unsigned int)v <= (unsigned int)USHRT_MAX ? v : v > 0 ? USHRT_MAX : 0);
}
-template<> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v) {
- return (unsigned short) (std::min)(v, (unsigned int) USHRT_MAX);
+template <> inline unsigned short vpMath::saturate<unsigned short>(unsigned int v)
+{
+ return (unsigned short)((std::min))(v, (unsigned int)USHRT_MAX);
}
-template<> inline unsigned short vpMath::saturate<unsigned short>(float v) {
+template <> inline unsigned short vpMath::saturate<unsigned short>(float v)
+{
int iv = vpMath::round(v);
return vpMath::saturate<unsigned short>(iv);
}
-template<> inline unsigned short vpMath::saturate<unsigned short>(double v) {
+template <> inline unsigned short vpMath::saturate<unsigned short>(double v)
+{
int iv = vpMath::round(v);
return vpMath::saturate<unsigned short>(iv);
}
-//short
-template<> inline short vpMath::saturate<short>(unsigned short v) {
- return (short) (std::min)((int) v, SHRT_MAX);
-}
-template<> inline short vpMath::saturate<short>(int v) {
- return (short) ((unsigned int) (v - SHRT_MIN) <= (unsigned int) USHRT_MAX ? v :
- v > 0 ? SHRT_MAX : SHRT_MIN);
+// short
+template <> inline short vpMath::saturate<short>(unsigned short v) { return (short)((std::min))((int)v, SHRT_MAX); }
+template <> inline short vpMath::saturate<short>(int v)
+{
+ return (short)((unsigned int)(v - SHRT_MIN) <= (unsigned int)USHRT_MAX ? v : v > 0 ? SHRT_MAX : SHRT_MIN);
}
-template<> inline short vpMath::saturate<short>(unsigned int v) {
- return (short) (std::min)(v, (unsigned int) SHRT_MAX);
+template <> inline short vpMath::saturate<short>(unsigned int v)
+{
+ return (short)((std::min))(v, (unsigned int)SHRT_MAX);
}
-template<> inline short vpMath::saturate<short>(float v) {
+template <> inline short vpMath::saturate<short>(float v)
+{
int iv = vpMath::round(v);
return vpMath::saturate<short>(iv);
}
-template<> inline short vpMath::saturate<short>(double v) {
+template <> inline short vpMath::saturate<short>(double v)
+{
int iv = vpMath::round(v);
return vpMath::saturate<short>(iv);
}
-//int
-template<> inline int vpMath::saturate<int>(float v) {
- return vpMath::round(v);
-}
+// int
+template <> inline int vpMath::saturate<int>(float v) { return vpMath::round(v); }
-template<> inline int vpMath::saturate<int>(double v) {
- return vpMath::round(v);
-}
+template <> inline int vpMath::saturate<int>(double v) { return vpMath::round(v); }
-//unsigned int
-// (Comment from OpenCV) we intentionally do not clip negative numbers, to make -1 become 0xffffffff etc.
-template<> inline unsigned int vpMath::saturate<unsigned int>(float v) {
- return (unsigned int) vpMath::round(v);
-}
+// unsigned int
+// (Comment from OpenCV) we intentionally do not clip negative numbers, to
+// make -1 become 0xffffffff etc.
+template <> inline unsigned int vpMath::saturate<unsigned int>(float v) { return (unsigned int)vpMath::round(v); }
-template<> inline unsigned int vpMath::saturate<unsigned int>(double v) {
- return (unsigned int) vpMath::round(v);
-}
+template <> inline unsigned int vpMath::saturate<unsigned int>(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 7ee56f4..0b00138 100644
--- a/modules/core/include/visp3/core/vpMatrix.h
+++ b/modules/core/include/visp3/core/vpMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,18 +39,18 @@
#ifndef vpMatrix_H
#define vpMatrix_H
+#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <visp3/core/vpTime.h>
-#include <visp3/core/vpArray2D.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpForceTwistMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/core/vpForceTwistMatrix.h>
#ifdef VISP_HAVE_GSL
-# include <gsl/gsl_math.h>
-# include <gsl/gsl_eigen.h>
+#include <gsl/gsl_eigen.h>
+#include <gsl/gsl_math.h>
#endif
#include <iostream>
@@ -69,45 +70,52 @@ class vpForceTwistMatrix;
these matrices.
*/
-
-
/*!
\class vpMatrix
\ingroup group_core_matrices
\brief Implementation of a matrix and operations on matrices.
- This class may benefit from Lapack or GSL optional 3rd parties that are used especially
- for pseudo-inverse. Concerning Lapack optional 3rd party, installation instructions are provide
- here https://visp.inria.fr/3rd_lapack. For optional GSL, installation instructions are provide
- here https://visp.inria.fr/3rd_gsl.
+ This class needs one of the following third-party to compute matrix inverse,
+ pseudo-inverse, singular value decomposition, determinant:
+ - If Lapack is installed and detected by ViSP, this 3rd party is used by
+ vpMatrix. Installation instructions are provided here
+ https://visp.inria.fr/3rd_lapack;
+ - else if Eigen3 is installed and detected by ViSP, this 3rd party is used
+ by vpMatrix. Installation instructions are provided here
+ https://visp.inria.fr/3rd_eigen;
+ - else if OpenCV is installed and detected by ViSP, this 3rd party is used,
+ Installation instructions are provided here
+ https://visp.inria.fr/3rd_opencv;
+ - else if GSL is installed and detected by ViSP, we use this other 3rd
+ party. Installation instructions are provided here
+ https://visp.inria.fr/3rd_gsl.
+ - If none of these previous 3rd parties is installed, we use by default a
+ Lapack built-in version.
vpMatrix class provides a data structure for the matrices as well
as a set of operations on these matrices.
The vpMatrix class is derived from vpArray2D<double>.
- \warning Note the matrix in the class (*this) will be noted A in the comment
-
- \ingroup libmath
-
- \sa vpRowVector, vpColVector, vpHomogeneousMatrix, vpRotationMatrix,
- vpTwistMatrix, vpHomography
+ \sa vpArray2D, vpRowVector, vpColVector, vpHomogeneousMatrix,
+ vpRotationMatrix, vpVelocityTwistMatrix, vpForceTwistMatrix, vpHomography
*/
class VISP_EXPORT vpMatrix : public vpArray2D<double>
{
- public:
+public:
/*!
Method used to compute the determinant of a square matrix.
\sa det()
*/
typedef enum {
- LU_DECOMPOSITION /*!< LU decomposition method. */
+ LU_DECOMPOSITION /*!< LU decomposition method. */
} vpDetMethod;
- public:
+public:
/*!
- Basic constructor of a matrix of double. Number of columns and rows are zero.
+ Basic constructor of a matrix of double. Number of columns and rows are
+ zero.
*/
vpMatrix() : vpArray2D<double>(0, 0) {}
/*!
@@ -125,20 +133,26 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
\param val : Each element of the matrix is set to \e val.
*/
vpMatrix(unsigned int r, unsigned int c, double val) : vpArray2D<double>(r, c, val) {}
- vpMatrix(const vpMatrix &M, unsigned int r, unsigned int c,
- unsigned int nrows, unsigned int ncols) ;
+ 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
+ Create a matrix from a 2D array that could be one of the following
+ container that inherit from vpArray2D such as vpMatrix, vpRotationMatrix,
+ vpHomogeneousMatrix, vpPoseVector, vpColVector, vpRowVector...
+
+ The following example shows how to create a matrix from an homogeneous
+ matrix:
+\code
+vpRotationMatrix R;
+vpMatrix M(R);
+\endcode
*/
- vpMatrix(const vpArray2D<double>& A) : vpArray2D<double>(A) {}
+ vpMatrix(const vpArray2D<double> &A) : vpArray2D<double>(A) {}
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ vpMatrix(const vpMatrix &A) : vpArray2D<double>(A) {}
+
+ vpMatrix(vpMatrix &&A);
+#endif
//! Destructor (Memory de-allocation)
virtual ~vpMatrix() {}
@@ -149,14 +163,14 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
*/
void clear()
{
- if (data != NULL ) {
+ if (data != NULL) {
free(data);
- data=NULL;
+ data = NULL;
}
- if (rowPtrs!=NULL) {
+ if (rowPtrs != NULL) {
free(rowPtrs);
- rowPtrs=NULL ;
+ rowPtrs = NULL;
}
rowNum = colNum = dsize = 0;
}
@@ -170,18 +184,22 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
void diag(const vpColVector &A);
// Initialize an identity matrix n-by-n
void eye();
- void eye(unsigned int n) ;
+ void eye(unsigned int n);
// Initialize an identity matrix m-by-n
- void eye(unsigned int m, unsigned int n) ;
+ void eye(unsigned int m, unsigned int n);
//@}
-
+
//---------------------------------
// Assignment
//---------------------------------
/** @name Assignment operators */
//@{
- vpMatrix &operator<<(double*);
+ vpMatrix &operator<<(double *);
vpMatrix &operator=(const vpArray2D<double> &A);
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ vpMatrix &operator=(const vpMatrix &A);
+ vpMatrix &operator=(vpMatrix &&A);
+#endif
vpMatrix &operator=(const double x);
//@}
@@ -190,29 +208,30 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//-------------------------------------------------
/** @name Stacking */
//@{
- // Stack the matrix A below the current one, copy if not initialized this = [ this A ]^T
+ // Stack the matrix A below the current one, copy if not initialized this =
+ // [ this A ]^T
void stack(const vpMatrix &A);
void stack(const vpRowVector &r);
// Stacks columns of a matrix in a vector
- void stackColumns(vpColVector &out );
+ void stackColumns(vpColVector &out);
// Stacks columns of a matrix in a vector
vpColVector stackColumns();
// Stacks columns of a matrix in a vector
- void stackRows(vpRowVector &out );
+ void stackRows(vpRowVector &out);
// Stacks columns of a matrix in a vector
vpRowVector stackRows();
//@}
//---------------------------------
- // Matrix insertion with Static Public Member Functions
+ // Matrix insertion
//---------------------------------
/** @name Matrix insertion */
//@{
// Insert matrix A in the current matrix at the given position (r, c).
- void insert(const vpMatrix&A, const unsigned int r, const unsigned int c);
+ void insert(const vpMatrix &A, const unsigned int r, const unsigned int c);
//@}
//-------------------------------------------------
@@ -220,10 +239,11 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//-------------------------------------------------
/** @name Columns, rows, sub-matrices extraction */
//@{
- vpRowVector getRow(const unsigned int i) const;
- vpRowVector getRow(const unsigned int i, const unsigned int j_begin, const unsigned int size) const;
+ 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;
+ 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);
//@}
@@ -232,6 +252,27 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//---------------------------------
/** @name Matrix operations */
//@{
+ // return the determinant of the matrix.
+ double det(vpDetMethod method = LU_DECOMPOSITION) const;
+ double detByLU() const;
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_EIGEN3
+ double detByLUEigen3() const;
+#endif
+#ifdef VISP_HAVE_GSL
+ double detByLUGsl() const;
+#endif
+#ifdef VISP_HAVE_LAPACK
+ double detByLULapack() const;
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ double detByLUOpenCV() const;
+#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ // Compute the exponential matrix of a square matrix
+ vpMatrix expm() const;
+
// operation A = A + B
vpMatrix &operator+=(const vpMatrix &B);
// operation A = A - B
@@ -240,7 +281,8 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
vpMatrix operator*(const vpRotationMatrix &R) const;
vpMatrix operator*(const vpVelocityTwistMatrix &V) const;
vpMatrix operator*(const vpForceTwistMatrix &V) const;
- // operation t_out = A * t (A is unchanged, t and t_out are translation vectors)
+ // operation t_out = A * t (A is unchanged, t and t_out are translation
+ // vectors)
vpTranslationVector operator*(const vpTranslationVector &tv) const;
vpColVector operator*(const vpColVector &v) const;
vpMatrix operator+(const vpMatrix &B) const;
@@ -268,24 +310,25 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
*/
double sum() const;
double sumSquare() const;
- // return the determinant of the matrix.
- double det(vpDetMethod method = LU_DECOMPOSITION) const;
-
- //Compute the exponential matrix of a square matrix
- vpMatrix expm() const;
+
+ //-------------------------------------------------
+ // Hadamard product
+ //-------------------------------------------------
+ /** @name Hadamard product */
+ vpMatrix hadamard(const vpMatrix &m) const;
//-------------------------------------------------
// Kronecker product
//-------------------------------------------------
/** @name Kronecker product */
- //@{
- // Compute Kronecker product matrix
- void kron(const vpMatrix &m1, vpMatrix &out) const;
-
- // Compute Kronecker product matrix
- vpMatrix kron(const vpMatrix &m1) const;
+ //@{
+ // Compute Kronecker product matrix
+ void kron(const vpMatrix &m1, vpMatrix &out) const;
+
+ // Compute Kronecker product matrix
+ vpMatrix kron(const vpMatrix &m1) const;
//@}
-
+
//-------------------------------------------------
// Transpose
//-------------------------------------------------
@@ -295,8 +338,8 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
vpMatrix t() const;
// Compute the transpose C = A^T
- vpMatrix transpose()const;
- void transpose(vpMatrix & C )const;
+ vpMatrix transpose() const;
+ void transpose(vpMatrix &C) const;
vpMatrix AAt() const;
void AAt(vpMatrix &B) const;
@@ -310,83 +353,116 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//-------------------------------------------------
/** @name Matrix inversion */
//@{
+ // inverse matrix A using the LU decomposition
+ vpMatrix inverseByLU() const;
+
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- //! LU Decomposition
- void LUDcmp(unsigned int* perm, int& d);
- //! solve AX = B using the LU Decomposition
- void LUBksb(unsigned int* perm, vpColVector& b);
+#if defined(VISP_HAVE_EIGEN3)
+ vpMatrix inverseByLUEigen3() const;
+#endif
+#if defined(VISP_HAVE_GSL)
+ vpMatrix inverseByLUGsl() const;
+#endif
+#if defined(VISP_HAVE_LAPACK)
+ vpMatrix inverseByLULapack() const;
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpMatrix inverseByLUOpenCV() const;
+#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-#endif // doxygen should skip this
- // inverse matrix A using the LU decomposition
- vpMatrix inverseByLU() const;
-#if defined(VISP_HAVE_LAPACK_C)
- // inverse matrix A using the Cholesky decomposition (only for real symmetric matrices)
+ // inverse matrix A using the Cholesky decomposition (only for real
+ // symmetric matrices)
vpMatrix inverseByCholesky() const;
- //lapack implementation of inverse by Cholesky
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#if defined(VISP_HAVE_LAPACK)
vpMatrix inverseByCholeskyLapack() const;
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ vpMatrix inverseByCholeskyOpenCV() const;
+#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
// inverse matrix A using the QR decomposition
vpMatrix inverseByQR() const;
- //lapack implementation of inverse by QR
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#if defined(VISP_HAVE_LAPACK)
vpMatrix inverseByQRLapack() const;
#endif
- //! Compute the pseudo inverse of the matrix using the SVD.
- vpMatrix pseudoInverse(double svThreshold=1e-6) const;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank
- unsigned int pseudoInverse(vpMatrix &Ap, double svThreshold=1e-6) const;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value
- unsigned int pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold=1e-6) const ;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value, image
- unsigned int pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &ImA,
- vpMatrix &ImAt) const ;
- //! Compute the pseudo inverse of the matrix using the SVD.
- //! return the rank and the singular value, image, kernel.
- unsigned int pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &ImA,
- vpMatrix &ImAt,
- vpMatrix &kerA) const ;
- //@}
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
- //-------------------------------------------------
- // SVD decomposition
- //-------------------------------------------------
+ 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;
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- void svdFlake(vpColVector& w, vpMatrix& v);
- void svdNr(vpColVector& w, vpMatrix& v);
-#ifdef VISP_HAVE_GSL
- void svdGsl(vpColVector& w, vpMatrix& v);
+#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;
#endif
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
- void svdOpenCV(vpColVector& w, vpMatrix& v);
+#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;
#endif
-#ifdef VISP_HAVE_LAPACK_C
- void svdLapack(vpColVector& W, vpMatrix& V);
+#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;
#endif
- //! solve AX=B using the SVD decomposition
- void SVBksb(const vpColVector& w,
- const vpMatrix& v,
- const vpColVector& b, vpColVector& x);
+#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;
#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ //@}
+
+ //-------------------------------------------------
+ // SVD decomposition
+ //-------------------------------------------------
/** @name SVD decomposition */
//@{
- // singular value decomposition SVD
-
- void svd(vpColVector& w, vpMatrix& v);
+ double cond() const;
+ unsigned int kernel(vpMatrix &kerAt, double svThreshold = 1e-6) const;
// solve Ax=B using the SVD decomposition (usage A = solveBySVD(B,x) )
- void solveBySVD(const vpColVector &B, vpColVector &x) const ;
+ void solveBySVD(const vpColVector &B, vpColVector &x) const;
// solve Ax=B using the SVD decomposition (usage x=A.solveBySVD(B))
- vpColVector solveBySVD(const vpColVector &B) const ;
+ vpColVector solveBySVD(const vpColVector &B) const;
- unsigned int kernel(vpMatrix &KerA, double svThreshold=1e-6) const;
- double cond() const;
+ // singular value decomposition SVD
+ void svd(vpColVector &w, vpMatrix &V);
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_EIGEN3
+ void svdEigen3(vpColVector &w, vpMatrix &V);
+#endif
+#ifdef VISP_HAVE_GSL
+ void svdGsl(vpColVector &w, vpMatrix &V);
+#endif
+#ifdef VISP_HAVE_LAPACK
+ void svdLapack(vpColVector &w, vpMatrix &V);
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ void svdOpenCV(vpColVector &w, vpMatrix &V);
+#endif
+#endif
//@}
//-------------------------------------------------
@@ -414,12 +490,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//---------------------------------
/** @name Printing */
//@{
- 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() <<" " ; }
+ 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() << " "; }
//@}
//------------------------------------------------------------------
@@ -432,7 +508,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
/** @name Setting a diagonal matrix with Static Public Member Functions */
//@{
// Create a diagonal matrix with the element of a vector DAii = Ai
- static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA) ;
+ static void createDiagonalMatrix(const vpColVector &A, vpMatrix &DA);
//@}
//---------------------------------
@@ -441,9 +517,10 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
/** @name Matrix insertion with Static Public Member Functions */
//@{
// Insert matrix B in matrix A at the given position (r, c).
- static vpMatrix insert(const vpMatrix &A,const vpMatrix &B, const unsigned int r, const unsigned int c) ;
- // Insert matrix B in matrix A (not modified) at the given position (r, c), the result is given in matrix C.
- static void insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c) ;
+ static vpMatrix insert(const vpMatrix &A, const vpMatrix &B, const unsigned int r, const unsigned int c);
+ // Insert matrix B in matrix A (not modified) at the given position (r, c),
+ // the result is given in matrix C.
+ static void insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C, const unsigned int r, const unsigned int c);
//---------------------------------
// Stacking with Static Public Member Functions
@@ -451,12 +528,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
/** @name Stacking with Static Public Member Functions */
//@{
// Juxtapose to matrices C = [ A B ]
- static vpMatrix juxtaposeMatrices(const vpMatrix &A,const vpMatrix &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) ;
+ static vpMatrix stack(const vpMatrix &A, const vpMatrix &B);
+ static vpMatrix stack(const vpMatrix &A, const vpRowVector &r);
// Stack two matrices C = [ A B ]^T
static void stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
@@ -470,7 +547,8 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
//@{
static void add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C);
static void add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C);
- static void add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C);
+ static void 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);
@@ -488,10 +566,10 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
/** @name Kronecker product with Static Public Member Functions */
//@{
// Compute Kronecker product matrix
- static void kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out);
+ static void kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out);
// Compute Kronecker product matrix
- static vpMatrix kron(const vpMatrix &m1, const vpMatrix &m2 );
+ static vpMatrix kron(const vpMatrix &m1, const vpMatrix &m2);
//@}
//---------------------------------
@@ -500,9 +578,12 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
/** @name Covariance computation with Static Public Member Functions */
//@{
static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b);
- static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &w);
- static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W);
- static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls);
+ static vpMatrix computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b,
+ const vpMatrix &w);
+ static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
+ const vpMatrix &Ls, const vpMatrix &W);
+ static vpMatrix computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
+ const vpMatrix &Ls);
//@}
//---------------------------------
@@ -515,19 +596,21 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
\param filename : absolute file name.
\param M : matrix to be loaded.
- \param binary :If true the matrix is loaded from a binary file, else from a text file.
- \param header : Header of the file is loaded in this parameter
+ \param binary :If true the matrix is loaded from a binary file, else from
+ a text file. \param header : Header of the file is loaded in this
+ parameter
\return Returns true if no problem appends.
*/
- static inline bool loadMatrix(const std::string &filename, vpArray2D<double> &M,
- const bool binary = false, char *header = NULL)
+ static inline bool loadMatrix(const std::string &filename, vpArray2D<double> &M, const bool binary = false,
+ char *header = NULL)
{
return vpArray2D<double>::load(filename, M, binary, header);
}
/*!
- Load a matrix from a YAML-formatted file. This function overloads vpArray2D::loadYAML().
+ Load a matrix from a YAML-formatted file. This function overloads
+ vpArray2D::loadYAML().
\param filename : absolute file name.
\param M : matrix to be loaded from the file.
@@ -545,26 +628,29 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
\param filename : absolute file name.
\param M : matrix to be saved.
- \param binary : If true the matrix is save in a binary file, else a text file.
- \param header : optional line that will be saved at the beginning of the file.
+ \param binary : If true the matrix is save in a binary file, else a text
+ file. \param header : optional line that will be saved at the beginning of
+ the file.
\return Returns true if no problem appends.
- Warning : If you save the matrix as in a text file the precision is less than if you save it in a binary file.
+ Warning : If you save the matrix as in a text file the precision is less
+ than if you save it in a binary file.
*/
- static inline bool saveMatrix(const std::string &filename, const vpArray2D<double> &M,
- const bool binary = false,
+ static inline bool saveMatrix(const std::string &filename, const vpArray2D<double> &M, const bool binary = false,
const char *header = "")
{
return vpArray2D<double>::save(filename, M, binary, header);
}
/*!
- Save a matrix in a YAML-formatted file. This function overloads vpArray2D::saveYAML().
+ Save a matrix in a YAML-formatted file. This function overloads
+ vpArray2D::saveYAML().
\param filename : absolute file name.
\param M : matrix to be saved in the file.
- \param header : optional lines that will be saved at the beginning of the file. Should be YAML-formatted and will adapt to the indentation if any.
+ \param header : optional lines that will be saved at the beginning of the
+ file. Should be YAML-formatted and will adapt to the indentation if any.
\return Returns true if success.
@@ -573,8 +659,7 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
{
return vpArray2D<double>::saveYAML(filename, M, header);
}
- //@}
-
+//@}
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
@@ -582,61 +667,75 @@ class VISP_EXPORT vpMatrix : public vpArray2D<double>
*/
//@{
/*!
- \deprecated Only provided for compatibilty with ViSP previous releases. This function does nothing.
+ \deprecated Only provided for compatibilty with ViSP previous releases.
+ This function does nothing.
*/
- vp_deprecated void init() { }
+ vp_deprecated void init() {}
+
/*!
\deprecated You should rather use stack(const vpMatrix &A)
*/
vp_deprecated void stackMatrices(const vpMatrix &A) { stack(A); }
/*!
- \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix &B)
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix
+ &B)
*/
- vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, const vpMatrix &B) { return vpMatrix::stack(A, B); }
+ 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)
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix
+ &B, vpMatrix &C)
*/
- vp_deprecated static void stackMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C) { vpMatrix::stack(A, B, C); }
+ 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)
+ \deprecated You should rather use stack(const vpMatrix &A, const vpMatrix
+ &B)
*/
vp_deprecated static vpMatrix stackMatrices(const vpMatrix &A, const vpRowVector &B);
/*!
- \deprecated You should rather use stack(const vpMatrix &A, const vpRowVector &B, vpMatrix &C)
+ \deprecated You should rather use stack(const vpMatrix &A, const
+ vpRowVector &B, vpMatrix &C)
*/
vp_deprecated static void stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &C);
/*!
- \deprecated You should rather use vpColVector::stack(const vpColVector &A, const vpColVector &B)
+ \deprecated You should rather use vpColVector::stack(const vpColVector
+ &A, const vpColVector &B)
*/
vp_deprecated static vpMatrix stackMatrices(const vpColVector &A, const vpColVector &B);
/*!
- \deprecated You should rather use vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector &C)
+ \deprecated You should rather use vpColVector::stack(const vpColVector
+ &A, const vpColVector &B, vpColVector &C)
*/
vp_deprecated static void stackMatrices(const vpColVector &A, const vpColVector &B, vpColVector &C);
/*!
\deprecated You should rather use diag(const double &)
*/
- vp_deprecated void setIdentity(const double & val=1.0) ;
+ vp_deprecated void setIdentity(const double &val = 1.0);
vp_deprecated vpRowVector row(const unsigned int i);
vp_deprecated vpColVector column(const unsigned int j);
- //@}
+//@}
#endif
- private:
- double detByLU() const;
- static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, vpMatrix &Js, vpColVector &deltaP);
+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);
+#endif
+ static void computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls,
+ vpMatrix &Js, vpColVector &deltaP);
};
-
//////////////////////////////////////////////////////////////////////////
#ifndef DOXYGEN_SHOULD_SKIP_THIS
VISP_EXPORT
#endif
-vpMatrix operator*(const double &x, const vpMatrix &A) ;
+vpMatrix operator*(const double &x, const vpMatrix &A);
#endif
diff --git a/modules/core/include/visp3/core/vpMatrixException.h b/modules/core/include/visp3/core/vpMatrixException.h
index e0d3ebd..325f4b5 100644
--- a/modules/core/include/visp3/core/vpMatrixException.h
+++ b/modules/core/include/visp3/core/vpMatrixException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,11 +39,12 @@
#ifndef __vpMatrixException_H
#define __vpMatrixException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* Classes standards. */
//
@@ -50,12 +52,15 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpMatrixException
@@ -64,44 +69,41 @@
*/
class VISP_EXPORT vpMatrixException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpMatrix member
- */
- enum errorCodeEnum
- {
- //! error returns by a constructor
- constructionError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedError,
- //! index out of range
- outOfRangeError,
- //! iterative algorithm doesn't converge (ex SVD)
- convergencyError,
- incorrectMatrixSizeError,
- forbiddenOperatorError,
- subMatrixError,
- matrixError,
- rankDeficient
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpMatrix member
+ */
+ enum errorCodeEnum {
+ //! error returns by a constructor
+ constructionError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedError,
+ //! index out of range
+ outOfRangeError,
+ //! iterative algorithm doesn't converge (ex SVD)
+ convergencyError,
+ incorrectMatrixSizeError,
+ forbiddenOperatorError,
+ subMatrixError,
+ matrixError,
+ rankDeficient
+ };
- public:
- vpMatrixException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpMatrixException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpMatrixException (const int id)
- : vpException(id){ ; }
- // vpMatrixException() : vpException() { ;}
+public:
+ vpMatrixException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpMatrixException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpMatrixException(const int id) : vpException(id) { ; }
+ // vpMatrixException() : vpException() { ;}
};
#endif
diff --git a/modules/core/include/visp3/core/vpMeterPixelConversion.h b/modules/core/include/visp3/core/vpMeterPixelConversion.h
index 2adec55..c443843 100644
--- a/modules/core/include/visp3/core/vpMeterPixelConversion.h
+++ b/modules/core/include/visp3/core/vpMeterPixelConversion.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,11 +37,9 @@
*
*****************************************************************************/
-
#ifndef vpMeterPixelConversion_H
#define vpMeterPixelConversion_H
-
/*!
\file vpMeterPixelConversion.h
\brief meter to pixel conversion
@@ -67,89 +66,81 @@
class VISP_EXPORT vpMeterPixelConversion
{
public:
- static void convertEllipse(const vpCameraParameters &cam,
- const vpCircle &circle, vpImagePoint ¢er,
- double &mu20_p, double &mu11_p, double &mu02_p);
+ static void convertEllipse(const vpCameraParameters &cam, const vpCircle &circle, vpImagePoint ¢er,
+ double &mu20_p, double &mu11_p, double &mu02_p);
- static void convertLine(const vpCameraParameters &cam,
- const double &rho_m, const double &theta_m,
- double &rho_p, double &theta_p) ;
+ static void convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m, double &rho_p,
+ double &theta_p);
-/*!
+ /*!
- \brief Point coordinates conversion from normalized coordinates
- \f$(x,y)\f$ in meter to pixel coordinates \f$(u,v)\f$.
+ \brief Point coordinates conversion from normalized coordinates
+ \f$(x,y)\f$ in meter to pixel coordinates \f$(u,v)\f$.
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
- \param cam : camera parameters.
- \param x : input coordinate in meter along image plane x-axis.
- \param y : input coordinate in meter along image plane y-axis.
- \param u : output coordinate in pixels along image horizontal axis.
- \param v : output coordinate in pixels along image vertical axis.
+ \param cam : camera parameters.
+ \param x : input coordinate in meter along image plane x-axis.
+ \param y : input coordinate in meter along image plane y-axis.
+ \param u : output coordinate in pixels along image horizontal axis.
+ \param v : output coordinate in pixels along image vertical axis.
- \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
- perspective projection without distortion.
+ \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
+ perspective projection without distortion.
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
- distortion.
-*/
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0
+ \f$ with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
+ distortion.
+ */
- inline static void
- convertPoint(const vpCameraParameters &cam,
- const double &x, const double &y,
- double &u, double &v)
+ inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,x,y,u,v);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,x,y,u,v);
+ switch (cam.projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ convertPointWithoutDistortion(cam, x, y, u, v);
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ convertPointWithDistortion(cam, x, y, u, v);
break;
- }
+ }
}
-/*!
+ /*!
- \brief Point coordinates conversion from normalized coordinates
- \f$(x,y)\f$ in meter to pixel coordinates.
+ \brief Point coordinates conversion from normalized coordinates
+ \f$(x,y)\f$ in meter to pixel coordinates.
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
- \param cam : camera parameters.
- \param x : input coordinate in meter along image plane x-axis.
- \param y : input coordinate in meter along image plane y-axis.
- \param iP : output coordinates in pixels.
+ \param cam : camera parameters.
+ \param x : input coordinate in meter along image plane x-axis.
+ \param y : input coordinate in meter along image plane y-axis.
+ \param iP : output coordinates in pixels.
- In the frame (u,v) the result is given by:
+ In the frame (u,v) the result is given by:
- \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
- perspective projection without distortion.
+ \f$ u = x*p_x + u_0 \f$ and \f$ v = y*p_y + v_0 \f$ in the case of
+ perspective projection without distortion.
- \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
- with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
- distortion.
-*/
+ \f$ u = x*p_x*(1+k_{ud}*r^2)+u_0 \f$ and \f$ v = y*p_y*(1+k_{ud}*r^2)+v_0
+ \f$ with \f$ r^2 = x^2+y^2 \f$ in the case of perspective projection with
+ distortion.
+ */
- inline static void
- convertPoint(const vpCameraParameters &cam,
- const double &x, const double &y,
- vpImagePoint &iP)
+ inline static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, vpImagePoint &iP)
{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,x,y,iP);
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,x,y,iP);
+ switch (cam.projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ convertPointWithoutDistortion(cam, x, y, iP);
break;
- }
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ convertPointWithDistortion(cam, x, y, iP);
+ break;
+ }
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -162,13 +153,11 @@ public:
\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 ;
+ u = x * cam.px + cam.u0;
+ v = y * cam.py + cam.v0;
}
/*!
@@ -181,13 +170,11 @@ public:
\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 );
+ iP.set_u(x * cam.px + cam.u0);
+ iP.set_v(y * cam.py + cam.v0);
}
/*!
@@ -206,14 +193,12 @@ public:
\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;
- v = cam.v0 + cam.py*y*r2;
+ double r2 = 1. + cam.kud * (x * x + y * y);
+ u = cam.u0 + cam.px * x * r2;
+ v = cam.v0 + cam.py * y * r2;
}
/*!
@@ -232,18 +217,14 @@ public:
\f$ v = y*p_y*(1+k_{ud}*r^2)+v_0 \f$
with \f$ r^2 = x^2+y^2 \f$
*/
- inline static void
- convertPointWithDistortion(const vpCameraParameters &cam,
- const double &x, const double &y,
- vpImagePoint &iP)
+ inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &x, const double &y,
+ vpImagePoint &iP)
{
- double r2 = 1.+cam.kud*(x*x+y*y);
- iP.set_u( cam.u0 + cam.px*x*r2 );
- iP.set_v( cam.v0 + cam.py*y*r2 );
+ double r2 = 1. + cam.kud * (x * x + y * y);
+ iP.set_u(cam.u0 + cam.px * x * r2);
+ iP.set_v(cam.v0 + cam.py * y * r2);
}
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-} ;
+};
#endif
-
-
diff --git a/modules/core/include/visp3/core/vpMoment.h b/modules/core/include/visp3/core/vpMoment.h
index 34bf3b7..b46256a 100644
--- a/modules/core/include/visp3/core/vpMoment.h
+++ b/modules/core/include/visp3/core/vpMoment.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,17 +44,15 @@
#ifndef __MOMENT_H__
#define __MOMENT_H__
-#include <vector>
#include <iostream>
+#include <vector>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-
class vpMomentDatabase;
class vpMomentObject;
-
/*!
\class vpMoment
@@ -61,9 +60,10 @@ class vpMomentObject;
\brief This class defines shared methods/attributes for 2D moments.
- All moments or combination of moments in the moments module are based on this class.
- A moment uses a vpMomentObject object to access all useful information.
- Moment values are obtained by a 4-step process common for all moment types:
+ All moments or combination of moments in the moments module are based on
+ this class. A moment uses a vpMomentObject object to access all useful
+ information. Moment values are obtained by a 4-step process common for all
+ moment types:
- Declaration.
\code
vpMoment moment;
@@ -81,17 +81,21 @@ class vpMomentObject;
std::vector<double> values = moment.get();
\endcode
- A moment may also be linked to a vpMomentDatabase. Moments linked to a database are able to access each others values.
- Some moments can be computed only if they are linked to a a database containing their dependencies.
+ A moment may also be linked to a vpMomentDatabase. Moments linked to a
+ database are able to access each others values. Some moments can be computed
+ only if they are linked to a a database containing their dependencies.
Linking to a database is done using the vpMoment::linkTo(...) method.
- There are no constraints about format of the array returned by vpMoment::get(); any implementation is fine.
+ There are no constraints about format of the array returned by
+ vpMoment::get(); any implementation is fine.
- Each moment must have a string name by implementing the char* vpMoment::name() method which allows to identify the moment in the database.
- Each moment must also implement a compute method describing how to obtain its values from the object.
+ Each moment must have a string name by implementing the char*
+ vpMoment::name() method which allows to identify the moment in the database.
+ Each moment must also implement a compute method describing how to obtain
+ its values from the object.
- \attention Order of moment computation DOES matter: when you compute a moment using vpMoment::compute(),
- all moment dependencies must be computed.
+ \attention Order of moment computation DOES matter: when you compute a
+ moment using vpMoment::compute(), all moment dependencies must be computed.
We recall that implemented moments are:
- vpMomentAlpha
- vpMomentArea
@@ -106,8 +110,8 @@ class vpMomentObject;
class VISP_EXPORT vpMoment
{
private:
- vpMomentObject* object;
- vpMomentDatabase* moments;
+ vpMomentObject *object;
+ vpMomentDatabase *moments;
char _name[255];
protected:
@@ -116,20 +120,21 @@ protected:
Returns the linked moment database.
\return the moment database
*/
- inline vpMomentDatabase& getMoments() const { return *moments; }
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMoment(const vpMoment &)
-// : object(NULL), moments(NULL), values()
-// {
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// }
-// vpMoment &operator=(const vpMoment &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
-//#endif
+ inline vpMomentDatabase &getMoments() const { return *moments; }
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMoment(const vpMoment &)
+ // : object(NULL), moments(NULL), values()
+ // {
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!");
+ // }
+ // vpMoment &operator=(const vpMoment &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
vpMoment();
@@ -141,18 +146,18 @@ public:
/** @name Inherited functionalities from vpMoment */
//@{
- virtual void compute()=0;
- inline const vpMomentObject& getObject() const { return *object;}
+ virtual void compute() = 0;
+ inline const vpMomentObject &getObject() const { return *object; }
/*!
Returns all values computed by the moment.
\return vector of values
*/
- const std::vector<double>& get() const { return values;}
- void linkTo(vpMomentDatabase& moments);
- virtual const char* name() const = 0;
- virtual void printDependencies(std::ostream& os) const;
- void update(vpMomentObject& object);
+ const std::vector<double> &get() const { return values; }
+ void linkTo(vpMomentDatabase &moments);
+ virtual const char *name() const = 0;
+ 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 b48743d..c9a746f 100644
--- a/modules/core/include/visp3/core/vpMomentAlpha.h
+++ b/modules/core/include/visp3/core/vpMomentAlpha.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,27 +51,34 @@
\ingroup group_core_moments
- \brief This class defines the orientation of the object inside the plane parallel to the object.
+ \brief This class defines the orientation of the object inside the plane
+parallel to the object.
- In general the value of the moment is computed in \f$ [-\pi/2 .. \pi/2] \f$ interval by the
- formula \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$.
+ In general the value of the moment is computed in \f$ [-\pi/2 .. \pi/2] \f$
+interval by the formula \f$ \alpha = \frac{1}{2}
+arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$.
- To obtain a \f$ [-\pi .. \pi] \f$ precision for non symetric object, you have to specify a
- reference information. This reference information is an alpha computed using the previous
- formula in \f$ [-\pi/2 .. \pi/2] \f$. Obtaining this precision comes from third-order centered
- moments and this reference information.
+ To obtain a \f$ [-\pi .. \pi] \f$ precision for non symetric object, you
+have to specify a reference information. This reference information is an
+alpha computed using the previous formula in \f$ [-\pi/2 .. \pi/2] \f$.
+Obtaining this precision comes from third-order centered moments and this
+reference information.
- Therefore there are two modes for vpMomentAlpha and one constructor per mode:
+ Therefore there are two modes for vpMomentAlpha and one constructor per
+mode:
- Reference mode (using the empty constructor vpMomentAlpha()):
- The vpMomentAlpha doesn't need any additionnal information, it will compute its values from
- available moments in \f$ [-\pi/2 .. \pi/2] \f$.
- - Relative mode (using non-empty constructor vpMomentAlpha(std::vector<double>&, double)):
- The vpMomentAlpha is computed in \f$ [-\pi .. \pi] \f$ from the available moments and the reference information.
- By knowing the reference, it may distinguish in-plane rotations of \f$ \alpha \f$ from rotations of \f$ \alpha + \pi \f$.
-
- The following code demonstrates a calculation of a reference alpha and then uses this alpha to estimate the orientation
- of the same object after performing a 180 degrees rotation.
- Therefore the first and second alpha should have opposite values.
+ The vpMomentAlpha doesn't need any additionnal information, it will compute
+its values from available moments in \f$ [-\pi/2 .. \pi/2] \f$.
+ - Relative mode (using non-empty constructor
+vpMomentAlpha(std::vector<double>&, double)): The vpMomentAlpha is computed in
+\f$ [-\pi .. \pi] \f$ from the available moments and the reference
+information. By knowing the reference, it may distinguish in-plane rotations
+of \f$ \alpha \f$ from rotations of \f$ \alpha + \pi \f$.
+
+ The following code demonstrates a calculation of a reference alpha and then
+uses this alpha to estimate the orientation of the same object after
+performing a 180 degrees rotation. Therefore the first and second alpha should
+have opposite values.
\code
#include <visp3/core/vpMomentObject.h>
@@ -89,22 +97,28 @@ void print (double i) { std::cout << i << "\t";}
int main()
{
vpPoint p;
- std::vector<vpPoint> vec_p; // vector that contains the vertices of the contour polygon
+ // vector that contains the vertices of the contour polygon
+ std::vector<vpPoint> vec_p;
- p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
+ // coordinates in meters in the image plane (vertex 1)
+ p.set_x(1); p.set_y(1);
vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ // coordinates in meters in the image plane (vertex 2)
+ p.set_x(2); p.set_y(2);
vec_p.push_back(p);
- p.set_x(-3); p.set_y(0); // coordinates in meters in the image plane (vertex 3)
+ // coordinates in meters in the image plane (vertex 3)
+ p.set_x(-3); p.set_y(0);
vec_p.push_back(p);
- p.set_x(-3); p.set_y(-1); // coordinates in meters in the image plane (vertex 4)
+ // 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
+ // 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
@@ -149,7 +163,6 @@ int main()
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.
@@ -177,7 +190,6 @@ int main()
return 0;
}
-
\endcode
This program outputs:
\code
@@ -193,17 +205,18 @@ Shortcuts for quickly getting those references exist in vpMomentCommon.
This moment depends on vpMomentCentered.
*/
-class VISP_EXPORT vpMomentAlpha : public vpMoment {
+class VISP_EXPORT vpMomentAlpha : public vpMoment
+{
private:
bool isRef;
bool symmetric;
std::vector<double> ref;
double alphaRef;
-public:
+public:
vpMomentAlpha();
- vpMomentAlpha(std::vector<double>& ref, double alphaRef);
- virtual ~vpMomentAlpha() {};
+ vpMomentAlpha(const std::vector<double> &ref, double alphaRef);
+ virtual ~vpMomentAlpha(){};
void compute();
/*!
@@ -213,7 +226,7 @@ public:
/*!
Moment name.
*/
- const char* name() const {return "vpMomentAlpha";}
+ const char *name() const { return "vpMomentAlpha"; }
inline bool is_ref() const
{
@@ -231,8 +244,8 @@ public:
return false;
}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& v);
- void printDependencies(std::ostream& os) const;
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAlpha &v);
+ void printDependencies(std::ostream &os) const;
};
#endif
diff --git a/modules/core/include/visp3/core/vpMomentArea.h b/modules/core/include/visp3/core/vpMomentArea.h
index 188d34d..dcb8180 100644
--- a/modules/core/include/visp3/core/vpMomentArea.h
+++ b/modules/core/include/visp3/core/vpMomentArea.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,7 @@
#include <visp3/core/vpMoment.h>
class vpMomentObject;
-class vpMomentCentered; // Required for discrete case of vpMomentObject
+class vpMomentCentered; // Required for discrete case of vpMomentObject
/*!
\class vpMomentArea
@@ -50,19 +51,20 @@ class vpMomentCentered; // Required for discrete case of vpMomentObject
\brief Class handling the surface moment.
*/
-class VISP_EXPORT vpMomentArea : public vpMoment {
+class VISP_EXPORT vpMomentArea : public vpMoment
+{
public:
vpMomentArea();
- virtual ~vpMomentArea() {};
+ virtual ~vpMomentArea(){};
/** @name Inherited functionalities from vpMomentArea */
//@{
void compute();
//! Moment name.
- const char* name() const {return "vpMomentArea";}
- void printDependencies(std::ostream& os) const;
+ 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 b5da806..3029cd3 100644
--- a/modules/core/include/visp3/core/vpMomentAreaNormalized.h
+++ b/modules/core/include/visp3/core/vpMomentAreaNormalized.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,23 +52,32 @@ class vpMomentCentered;
\ingroup group_core_moments
- \brief Class handling the normalized surface moment that is invariant in scale and used to estimate depth.
+ \brief Class handling the normalized surface moment that is invariant in
+scale and used to estimate depth.
This moment depends on vpMomentCentered.
The idea behind vpMomentAreaNormalized is described in \cite Tahri05z.
- During a visual servoing process, a vpMomentAreaNormalized will converge towards the desired depth when the current surface will converge to the destination surface.
- It is defined as follows: \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$ where \e a is the current surface and \e a* the destination surface.
- Consequently, the vpMomentAreaNormalized needs to have information about the desired depth \e Z* and the desired surface \e a*.
+ During a visual servoing process, a vpMomentAreaNormalized will converge
+towards the desired depth when the current surface will converge to the
+destination surface. It is defined as follows: \f$ a_n=Z^*
+\sqrt{\frac{a^*}{a}} \f$ where \e a is the current surface and \e a* the
+destination surface. Consequently, the vpMomentAreaNormalized needs to have
+information about the desired depth \e Z* and the desired surface \e a*.
\warning About the order of the object.
- The surface (refered to as \e a in the above paragraph) depends of the nature of the object.
- - In case of a continuous object (when vpMomentObject::getType() is vpMomentObject::DENSE_FULL_OBJECT or vpMomentObject::DENSE_POLYGON) \f$a=m_{00}\f$.
- - In case of a discrete object (when vpMomentObject::getType() is vpMomentObject::DISCRETE) \f$a=\mu_{20}+\mu_{02}\f$.
-
- Therefore, a vpMomentObject has to be of minimum order 2 in order to compute a vpMomentAreaNormalized moment in the discrete case
- and of minimum order 0 in continous cases.
+ The surface (refered to as \e a in the above paragraph) depends of the
+nature of the object.
+ - In case of a continuous object (when vpMomentObject::getType() is
+vpMomentObject::DENSE_FULL_OBJECT or vpMomentObject::DENSE_POLYGON)
+\f$a=m_{00}\f$.
+ - In case of a discrete object (when vpMomentObject::getType() is
+vpMomentObject::DISCRETE) \f$a=\mu_{20}+\mu_{02}\f$.
+
+ Therefore, a vpMomentObject has to be of minimum order 2 in order to compute
+a vpMomentAreaNormalized moment in the discrete case and of minimum order 0 in
+continous cases.
This example shows a computation in the discrete case.
\code
@@ -125,29 +135,31 @@ int main()
An:1.41421
\endcode
*/
-class VISP_EXPORT vpMomentAreaNormalized : public vpMoment {
- private:
- double desiredSurface;
- double desiredDepth;
- public:
+class VISP_EXPORT vpMomentAreaNormalized : public vpMoment
+{
+private:
+ double desiredSurface;
+ double desiredDepth;
+
+public:
vpMomentAreaNormalized(double desiredSurface, double desiredDepth);
- virtual ~vpMomentAreaNormalized() {};
- void compute();
- /*!
- Retrieves the desired depth \e Z* as specified in the constructor.
- */
- double getDesiredDepth() const { return desiredDepth; }
- /*!
- Retrieves the desired surface \e a* as specified in the constructor.
- */
- double getDesiredSurface() const { return desiredSurface; }
-
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentAreaNormalized";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& v);
- void printDependencies(std::ostream& os) const;
+ virtual ~vpMomentAreaNormalized(){};
+ void compute();
+ /*!
+ Retrieves the desired depth \e Z* as specified in the constructor.
+ */
+ double getDesiredDepth() const { return desiredDepth; }
+ /*!
+ Retrieves the desired surface \e a* as specified in the constructor.
+ */
+ double getDesiredSurface() const { return desiredSurface; }
+
+ /*!
+ Moment name.
+ */
+ const char *name() const { return "vpMomentAreaNormalized"; }
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAreaNormalized &v);
+ void printDependencies(std::ostream &os) const;
};
#endif
diff --git a/modules/core/include/visp3/core/vpMomentBasic.h b/modules/core/include/visp3/core/vpMomentBasic.h
index 451e6b9..7041c82 100644
--- a/modules/core/include/visp3/core/vpMomentBasic.h
+++ b/modules/core/include/visp3/core/vpMomentBasic.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,35 +52,39 @@
\ingroup group_core_moments
\brief This class defines the 2D basic moment \f$m_{ij}\f$.
- This class is a wrapper for vpMomentObject wich allows to use it as a moment.
- This enables all common operations such as adding to a vpMomentDatabase.
+ This class is a wrapper for vpMomentObject wich allows to use it as a
+ moment. This enables all common operations such as adding to a
+ vpMomentDatabase.
In the case of a dense object \e O, 2D basic moments are defined by:
\f[m_{ij} = \int \int_{O} x^j y^j dx dy\f]
- In the case of a discrete set of \e n points, the basic moments are defined by
- \f[m_{ij} = \sum_{k=1}^{n} x_k^j y_k^j \f]
+ In the case of a discrete set of \e n points, the basic moments are defined
+ by \f[m_{ij} = \sum_{k=1}^{n} x_k^j y_k^j \f]
The selection of a dense or discrete object is done in vpMomentObject.
- The maximum order of vpMomentBasic is the same as that of vpMomentObject (ie: \f$i+j \leq \f$ vpMomentObject::getOrder).
+ The maximum order of vpMomentBasic is the same as that of vpMomentObject
+ (ie: \f$i+j \leq \f$ vpMomentObject::getOrder).
- Note that in the continuous case, the basic moment \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
- In the discrete case, it is the number of discrete points \f$n\f$.
+ Note that in the continuous case, the basic moment \f$m_{00}\f$ corresponds
+ to the surface \f$a\f$ of the object. In the discrete case, it is the number
+ of discrete points \f$n\f$.
*/
-class VISP_EXPORT vpMomentBasic : public vpMoment {
- public:
+class VISP_EXPORT vpMomentBasic : public vpMoment
+{
+public:
vpMomentBasic();
virtual ~vpMomentBasic(){};
- void compute();
- const std::vector<double>& get() const;
- double get (unsigned int i,unsigned int j) const;
- /*!
- Moment name.
- */
- const char* name() const { return "vpMomentBasic";}
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentBasic& v);
- void printDependencies(std::ostream& os) const;
+ void compute();
+ const std::vector<double> &get() const;
+ double get(unsigned int i, unsigned int j) const;
+ /*!
+ Moment name.
+ */
+ const char *name() const { return "vpMomentBasic"; }
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentBasic &v);
+ void printDependencies(std::ostream &os) const;
};
#endif
diff --git a/modules/core/include/visp3/core/vpMomentCInvariant.h b/modules/core/include/visp3/core/vpMomentCInvariant.h
index d7add9b..6d32820 100644
--- a/modules/core/include/visp3/core/vpMomentCInvariant.h
+++ b/modules/core/include/visp3/core/vpMomentCInvariant.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Descriptor for various invariants used to drive space roations around X and Y axis.
+ * Descriptor for various invariants used to drive space roations around X and
+ *Y axis.
*
* Authors:
* Filip Novotny
@@ -36,7 +38,8 @@
*****************************************************************************/
/*!
\file vpMomentCInvariant.h
- \brief Descriptor for various invariants used to drive space roations around X and Y axis.
+ \brief Descriptor for various invariants used to drive space roations around
+ X and Y axis.
*/
#ifndef __MOMENTCINVARIANT_H__
#define __MOMENTCINVARIANT_H__
@@ -52,19 +55,26 @@ class vpMomentBasic;
\ingroup group_core_moments
- This class defines several 2D (translation+rotation+scale) invariants for both symmetric and non-symmetric objects.
- These moment-based invariants are described in the following papers \cite Chaumette04a, \cite Tahri05z.
+ This class defines several 2D (translation+rotation+scale) invariants for
+both symmetric and non-symmetric objects. These moment-based invariants are
+described in the following papers \cite Chaumette04a, \cite Tahri05z.
- The descriptions for the invariants \f$C_1\f$ to \f$C_{10}\f$ can be found in \cite Chaumette04a and for invariants \f$P_x\f$,\f$P_y\f$,\f$S_x\f$,\f$S_y\f$ in \cite Tahri05z.
+ The descriptions for the invariants \f$C_1\f$ to \f$C_{10}\f$ can be found
+in \cite Chaumette04a and for invariants
+\f$P_x\f$,\f$P_y\f$,\f$S_x\f$,\f$S_y\f$ in \cite Tahri05z.
- These invariants are classicaly used in visual servoing to control the out-of-plane rotations.
- The C-type or P-type invariants are used for non-symmetric objects whereas the S-type invariants are used for symmetric objects.
+ These invariants are classicaly used in visual servoing to control the
+out-of-plane rotations. The C-type or P-type invariants are used for
+non-symmetric objects whereas the S-type invariants are used for symmetric
+objects.
- For most cases of non-symmetric objects, (\f$C_4\f$,\f$C_6\f$) or (\f$P_x\f$,\f$P_y\f$) couples are widely used to control x and y rotations.
+ For most cases of non-symmetric objects, (\f$C_4\f$,\f$C_6\f$) or
+(\f$P_x\f$,\f$P_y\f$) couples are widely used to control x and y rotations.
For symmetric objects \f$S_x\f$ and \f$S_y\f$ are the only choice.
- There are 14 translation+rotation+scale invariants (10 C-type, 2 P-type and 2 S-type) that can be accessed from by vpMomentCInvariant::get
- or any of the get shortcuts.
+ There are 14 translation+rotation+scale invariants (10 C-type, 2 P-type and
+2 S-type) that can be accessed from by vpMomentCInvariant::get or any of the
+get shortcuts.
The example below shows how to retrieve the \f$C_2\f$ invariant:
\code
@@ -93,13 +103,15 @@ int main()
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<const vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
+ const vpMomentCInvariant& C
+ = static_cast<const vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
if(success)
std::cout << C.get(1) << std:: endl; // print C2 invariant
else
@@ -109,163 +121,167 @@ int main()
}
\endcode
-vpMomentCInvariant depends on vpMomentCentered (see vpMomentDatabase and vpMomentCommon).
+vpMomentCInvariant depends on vpMomentCentered (see vpMomentDatabase and
+vpMomentCommon).
*/
-class VISP_EXPORT vpMomentCInvariant : public vpMoment {
- private:
- std::vector<double> I;
- std::vector<double> II;
- std::vector<double> c;
- std::vector<double> s;
- double K;
- void computeI(const vpMomentCentered& momentCentered, std::vector<double>& I);
-
- /* To calculate Sx and Sy from normalized moments */
- void calcSxSy(double& sx, double& sy) const;
- void calcSxSyNormalized(double& sx, double& sy) const;
- std::vector<double> cn; // same as s above but calculated from normalized moments
- std::vector<double> sn; // same as c above but calculated from normalized moments
- double In1; // same as I1 in Sx,Sy formulae but calculated from normalized moments
- bool flg_sxsynormalization_;
-
- public:
-
- vpMomentCInvariant(bool flg_sxsynormalization = false);
- virtual ~vpMomentCInvariant() {};
-
- /*!
- Shorcut for getting the value of \f$C_1\f$.
- */
- double C1() const { return values[0]; }
- /*!
- Shorcut for getting the value of \f$C_2\f$.
- */
- double C2() const { return values[1]; }
- /*!
- Shorcut for getting the value of \f$C_3\f$.
- */
- double C3() const { return values[2]; }
- /*!
- Shorcut for getting the value of \f$C_4\f$.
- */
- double C4() const { return values[3]; }
- /*!
- Shorcut for getting the value of \f$C_5\f$.
- */
- double C5() const { return values[4]; }
- /*!
- Shorcut for getting the value of \f$C_6\f$.
- */
- double C6() const { return values[5]; }
- /*!
- Shorcut for getting the value of \f$C_7\f$.
- */
- double C7() const { return values[6]; }
- /*!
- Shorcut for getting the value of \f$C_8\f$.
- */
- double C8() const { return values[7]; }
- /*!
- Shorcut for getting the value of \f$C_9\f$.
- */
- double C9() const { return values[8]; }
- /*!
- Shorcut for getting the value of \f$C_{10}\f$.
- */
- double C10() const { return values[9]; }
-
- void compute();
-
- /*!
- Gets the desired invariant.
- \param i given index. For invariants from C1 to C10 the corresponding index is from 0 to 9. For \f$S_x\f$,\f$S_y\f$ the indexes are 10,11 and for \f$P_x\f$,\f$P_y\f$ they are 12,13.
- */
- double get(unsigned int i) const { return values[i]; }
-
- /*!
- Access to partial invariant c (see [2]).
- */
- double getC(unsigned int i) const {return c[i];}
- /*!
- Access to partial invariants. The index convention is the same as in [1].
- */
- double getI(unsigned int index) const {return I[index];}
-
- /*!
- Print the moment invariants used to obtain the actual visual features
- */
- void printInvariants(std::ostream& os) const;
-
- /*!
- Access to partial invariant I (see [2]).
- */
- double getII(unsigned int i) const {return II[i];}
- /*!
- Access to partial invariant K (see [2]).
- */
- double getK() const {return K;}
-
- /*!
- Access to partial invariant S (see [2]).
- */
- double getS(unsigned int i) const {return s[i];}
-
- /*!
- Moment name.
- */
- const char* name() const {return "vpMomentCInvariant";}
-
- /*!
- Print partial invariant.
- */
- void printI(unsigned int index);
-
- /*!
- Shorcut for getting the value of \f$P_x\f$.
- */
- double Px(){ return values[12]; }
- /*!
- Shorcut for getting the value of\f$P_y\f$.
- */
- double Py(){ return values[13]; }
-
- /*!
- Shorcut for getting the value of \f$S_x\f$.
- */
- double Sx() const { return values[10]; }
- /*!
- Shorcut for getting the value of \f$S_y\f$.
- */
- double Sy() const { return values[11]; }
-
- /*!
- * Getters for I
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getIn1() const {return In1;}
-
- /*!
- * Getter for c
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getCN(unsigned int i) const {return cn[i];}
-
- /*!
- * Getter for s
- * (calculated from normalized 2nd and 3ord order moments)
- */
- double getSN(unsigned int i) const {return sn[i];}
-
- /*!
- * To know if Sx and Sy were calculated from normalized moments or not
- */
- bool isSxSyfromNormalizedMoments() const {return flg_sxsynormalization_;};
-
- /*!
- * To get all the invariant values as a whole.
- */
- inline const std::vector<double>& getMomentVector() const { return values; }
-
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCInvariant& v);
+class VISP_EXPORT vpMomentCInvariant : public vpMoment
+{
+private:
+ std::vector<double> I;
+ std::vector<double> II;
+ std::vector<double> c;
+ std::vector<double> s;
+ double K;
+ void computeI(const vpMomentCentered &momentCentered, std::vector<double> &I);
+
+ /* To calculate Sx and Sy from normalized moments */
+ void calcSxSy(double &sx, double &sy) const;
+ void calcSxSyNormalized(double &sx, double &sy) const;
+ std::vector<double> cn; // same as s above but calculated from normalized moments
+ std::vector<double> sn; // same as c above but calculated from normalized moments
+ double In1; // same as I1 in Sx,Sy formulae but calculated from normalized
+ // moments
+ bool flg_sxsynormalization_;
+
+public:
+ explicit vpMomentCInvariant(bool flg_sxsynormalization = false);
+ virtual ~vpMomentCInvariant(){};
+
+ /*!
+ Shorcut for getting the value of \f$C_1\f$.
+ */
+ double C1() const { return values[0]; }
+ /*!
+ Shorcut for getting the value of \f$C_2\f$.
+ */
+ double C2() const { return values[1]; }
+ /*!
+ Shorcut for getting the value of \f$C_3\f$.
+ */
+ double C3() const { return values[2]; }
+ /*!
+ Shorcut for getting the value of \f$C_4\f$.
+ */
+ double C4() const { return values[3]; }
+ /*!
+ Shorcut for getting the value of \f$C_5\f$.
+ */
+ double C5() const { return values[4]; }
+ /*!
+ Shorcut for getting the value of \f$C_6\f$.
+ */
+ double C6() const { return values[5]; }
+ /*!
+ Shorcut for getting the value of \f$C_7\f$.
+ */
+ double C7() const { return values[6]; }
+ /*!
+ Shorcut for getting the value of \f$C_8\f$.
+ */
+ double C8() const { return values[7]; }
+ /*!
+ Shorcut for getting the value of \f$C_9\f$.
+ */
+ double C9() const { return values[8]; }
+ /*!
+ Shorcut for getting the value of \f$C_{10}\f$.
+ */
+ double C10() const { return values[9]; }
+
+ void compute();
+
+ /*!
+ Gets the desired invariant.
+ \param i given index. For invariants from C1 to C10 the corresponding
+ index is from 0 to 9. For \f$S_x\f$,\f$S_y\f$ the indexes are 10,11 and
+ for \f$P_x\f$,\f$P_y\f$ they are 12,13.
+ */
+ double get(unsigned int i) const { return values[i]; }
+
+ /*!
+ Access to partial invariant c (see [2]).
+ */
+ double getC(unsigned int i) const { return c[i]; }
+ /*!
+ Access to partial invariants. The index convention is the same as in [1].
+ */
+ double getI(unsigned int index) const { return I[index]; }
+
+ /*!
+ Print the moment invariants used to obtain the actual visual features
+ */
+ void printInvariants(std::ostream &os) const;
+
+ /*!
+ Access to partial invariant I (see [2]).
+ */
+ double getII(unsigned int i) const { return II[i]; }
+ /*!
+ Access to partial invariant K (see [2]).
+ */
+ double getK() const { return K; }
+
+ /*!
+ Access to partial invariant S (see [2]).
+ */
+ double getS(unsigned int i) const { return s[i]; }
+
+ /*!
+ Moment name.
+ */
+ const char *name() const { return "vpMomentCInvariant"; }
+
+ /*!
+ Print partial invariant.
+ */
+ void printI(unsigned int index);
+
+ /*!
+ Shorcut for getting the value of \f$P_x\f$.
+ */
+ double Px() { return values[12]; }
+ /*!
+ Shorcut for getting the value of\f$P_y\f$.
+ */
+ double Py() { return values[13]; }
+
+ /*!
+ Shorcut for getting the value of \f$S_x\f$.
+ */
+ double Sx() const { return values[10]; }
+ /*!
+ Shorcut for getting the value of \f$S_y\f$.
+ */
+ double Sy() const { return values[11]; }
+
+ /*!
+ * Getters for I
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getIn1() const { return In1; }
+
+ /*!
+ * Getter for c
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getCN(unsigned int i) const { return cn[i]; }
+
+ /*!
+ * Getter for s
+ * (calculated from normalized 2nd and 3ord order moments)
+ */
+ double getSN(unsigned int i) const { return sn[i]; }
+
+ /*!
+ * To know if Sx and Sy were calculated from normalized moments or not
+ */
+ bool isSxSyfromNormalizedMoments() const { return flg_sxsynormalization_; };
+
+ /*!
+ * To get all the invariant values as a whole.
+ */
+ inline const std::vector<double> &getMomentVector() const { return values; }
+
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCInvariant &v);
};
#endif
diff --git a/modules/core/include/visp3/core/vpMomentCentered.h b/modules/core/include/visp3/core/vpMomentCentered.h
index e10d250..ea76e0d 100644
--- a/modules/core/include/visp3/core/vpMomentCentered.h
+++ b/modules/core/include/visp3/core/vpMomentCentered.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,69 +53,76 @@ class vpMomentObject;
\ingroup group_core_moments
- \brief This class defines the double-indexed centered moment descriptor \f$\mu_{ij}\f$.
+ \brief This class defines the double-indexed centered moment descriptor
+ \f$\mu_{ij}\f$.
In the case of a dense object \e O, centered moments are defined by:
\f[\mu_{ij}= \int \int_{O} (x_k-x_g)^j (y_k-y_g)^j\f]
- In the case of a discrete set of \e n points, centered moments are defined by:
- \f[\mu_{ij}= \sum_{k=1}^{n} (x_k-x_g)^j (y_k-y_g)^j\f]
+ In the case of a discrete set of \e n points, centered moments are defined
+ by: \f[\mu_{ij}= \sum_{k=1}^{n} (x_k-x_g)^j (y_k-y_g)^j\f]
where \f$(x_g,y_g)\f$ are the coordinates of the center of gravity.
- The centered moments are computed from the object at the highest possible order.
- For example if the vpMomentObject is defined up to order 5, vpMomentCentered will be too.
+ The centered moments are computed from the object at the highest possible
+ order. For example if the vpMomentObject is defined up to order 5,
+ vpMomentCentered will be too.
- Values of vpMomentCentered may be accessed by one of the two vpMomentCentered::get methods.
- When using vpMomentCentered::get (), the format of the return vector is the following:
- \f$ \mu_{ij} \f$ is stored at vpMomentCentered::get ()[j* (vpMomentObject::getOrder () +1)+i]
+ Values of vpMomentCentered may be accessed by one of the two
+ vpMomentCentered::get methods. When using vpMomentCentered::get (), the
+ format of the return vector is the following: \f$ \mu_{ij} \f$ is stored at
+ vpMomentCentered::get ()[j* (vpMomentObject::getOrder () +1)+i]
vpMomentCentered depends on vpMomentGravityCenter.
*/
-class VISP_EXPORT vpMomentCentered : public vpMoment {
+class VISP_EXPORT vpMomentCentered : public vpMoment
+{
public:
-
vpMomentCentered();
- virtual ~vpMomentCentered() {};
+ virtual ~vpMomentCentered(){};
void compute();
- double get(unsigned int i,unsigned int j) const;
+ double get(unsigned int i, unsigned int j) const;
- inline const std::vector<double>& get() const;
+ inline const std::vector<double> &get() const;
/*!
Moment name.
*/
- inline const char* name() const {return "vpMomentCentered";}
+ inline const char *name() const { return "vpMomentCentered"; }
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& v);
- void printWithIndices(std::ostream& os) const;
- void printDependencies(std::ostream& os) const;
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCentered &v);
+ void printWithIndices(std::ostream &os) const;
+ void printDependencies(std::ostream &os) const;
protected:
-
void set(unsigned int i, unsigned int j, double value);
-
};
/*!
- Returns all centered moment values \f$\mu_{ij}\f$ with \f$i+j \leq order\f$ where order is the object's order.
+ Returns all centered moment values \f$\mu_{ij}\f$ with \f$i+j \leq order\f$
+where order is the object's order.
- \return Vector of moment values. To access \f$\mu_{ij}\f$, you have to read vpMomentObject::get()[j*order+i].
+ \return Vector of moment values. To access \f$\mu_{ij}\f$, you have to read
+vpMomentObject::get()[j*order+i].
For example, if the maximal order is 3, the following values are provided:
+
\code
u00 u10 u20 u01 u11 u21 u02 u12 u12 u30 u03
\endcode
- To have a better reading of the moments you can picture them as a triangular matrix:
+
+ 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
+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:
+ To access for example to the centered moment \f$\mu_{12}\f$, you should use
+ this kind of code:
+
\code
vpMomentCentered mc;
//[...]
@@ -124,10 +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<double>& vpMomentCentered::get() const
-{
- return vpMoment::get();
-}
-
+inline const std::vector<double> &vpMomentCentered::get() const { return vpMoment::get(); }
#endif
diff --git a/modules/core/include/visp3/core/vpMomentCommon.h b/modules/core/include/visp3/core/vpMomentCommon.h
index 97d2dc3..e983262 100644
--- a/modules/core/include/visp3/core/vpMomentCommon.h
+++ b/modules/core/include/visp3/core/vpMomentCommon.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,15 +42,15 @@
#ifndef VPCOMMONMOMENTS_H
#define VPCOMMONMOMENTS_H
-#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentAlpha.h>
+#include <visp3/core/vpMomentArea.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentBasic.h>
-#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentCInvariant.h>
#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentGravityCenterNormalized.h>
-#include <visp3/core/vpMomentAreaNormalized.h>
-#include <visp3/core/vpMomentCInvariant.h>
-#include <visp3/core/vpMomentAlpha.h>
-#include <visp3/core/vpMomentArea.h>
#include <vector>
@@ -72,23 +73,32 @@ class vpMomentObject;
- vpMomentAlpha
- vpMomentArea
- There is no need to do the linkTo operations manually nor is it necessary to care about the order of moment computation.
+ There is no need to do the linkTo operations manually nor is it necessary
+ to care about the order of moment computation.
- This class carries an vpMomentCommon::updateAll() method capable of updating AND computing moments from an object (see 4-step process in vpMoment).
- The moments computed by this class are classical moments used in moment-based visual servoing.
- For more information see \cite Tahri05z.
+ This class carries an vpMomentCommon::updateAll() method capable of
+ updating AND computing moments from an object (see 4-step process in
+ vpMoment). The moments computed by this class are classical moments used in
+ moment-based visual servoing. For more information see \cite Tahri05z.
- To initialize this moment set the user needs to compute the following things:
- - the Mu3 value set: set of third-order centered moments computed for a reference object. (\f$\mu_{ij}$ with $i+j = 3\f$ ).
- These values allow the system to save the reference angular position and to perform planar rotations of more than 180 degrees if needed.
+ To initialize this moment set the user needs to compute the following
+ things:
+ - the Mu3 value set: set of third-order centered moments computed for a
+ reference object. (\f$\mu_{ij}$ with $i+j = 3\f$ ). These values allow the
+ system to save the reference angular position and to perform planar
+ rotations of more than 180 degrees if needed.
- the destination depth.
- - the surface of the destination object in the end of the visual servoing process.
- - the reference alpha: angular position of the object used to obtain the Mu3 set.
+ - the surface of the destination object in the end of the visual servoing
+ process.
+ - the reference alpha: angular position of the object used to obtain the
+ Mu3 set.
- Shortcuts for each of these prerequisites are provided by this class except depth (methods
- vpMomentCommon::getMu3(), vpMomentCommon::getSurface(), vpMomentCommon::getAlpha()).
+ Shortcuts for each of these prerequisites are provided by this class
+ except depth (methods vpMomentCommon::getMu3(),
+ vpMomentCommon::getSurface(), vpMomentCommon::getAlpha()).
- \attention Make sure your object is at least of order 5 when using this pre-filled database.
+ \attention Make sure your object is at least of order 5 when using this
+ pre-filled database.
*/
class VISP_EXPORT vpMomentCommon : public vpMomentDatabase
@@ -99,32 +109,36 @@ private:
vpMomentCentered momentCentered;
vpMomentGravityCenterNormalized momentGravityNormalized;
vpMomentAreaNormalized momentSurfaceNormalized;
- vpMomentCInvariant* momentCInvariant;
+ vpMomentCInvariant *momentCInvariant;
vpMomentAlpha momentAlpha;
vpMomentArea momentArea;
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMomentCommon(const vpMomentCommon &)
-// : vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), momentGravityNormalized(),
-// momentSurfaceNormalized(0, 0), momentCInvariant(), momentAlpha(), momentArea()
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMomentCommon &operator=(const vpMomentCommon &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMomentCommon(const vpMomentCommon &)
+ // : vpMomentDatabase(), momentBasic(), momentGravity(),
+ // momentCentered(), momentGravityNormalized(),
+ // momentSurfaceNormalized(0, 0), momentCInvariant(), momentAlpha(),
+ // momentArea()
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMomentCommon &operator=(const vpMomentCommon &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ=1.0, bool flg_sxsyfromnormalized=false);
+ vpMomentCommon(double dstSurface, const std::vector<double> &ref, double refAlpha, double dstZ = 1.0,
+ bool flg_sxsyfromnormalized = false);
virtual ~vpMomentCommon();
- static double getAlpha(vpMomentObject& object);
- static std::vector<double> getMu3(vpMomentObject& object);
- static double getSurface(vpMomentObject& object);
+ static double getAlpha(vpMomentObject &object);
+ static std::vector<double> getMu3(vpMomentObject &object);
+ static double getSurface(vpMomentObject &object);
- void updateAll(vpMomentObject& object);
+ void updateAll(vpMomentObject &object);
};
#endif // VPCOMMONMOMENTS_H
diff --git a/modules/core/include/visp3/core/vpMomentDatabase.h b/modules/core/include/visp3/core/vpMomentDatabase.h
index 146ffe5..4ac5e7d 100644
--- a/modules/core/include/visp3/core/vpMomentDatabase.h
+++ b/modules/core/include/visp3/core/vpMomentDatabase.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,9 +44,9 @@
#include <visp3/core/vpImage.h>
-#include <map>
-#include <iostream>
#include <cstring>
+#include <iostream>
+#include <map>
class vpMoment;
class vpMomentObject;
@@ -55,15 +56,20 @@ class vpMomentObject;
\ingroup group_core_moments
- \brief This class allows to register all vpMoments so they can access each other according to their dependencies.
+ \brief This class allows to register all vpMoments so they can access each
+other according to their dependencies.
- Sometimes, a moment needs to have access to other moment's values to be computed.
- For example vpMomentCentered needs additionnal information about the gravity center vpMomentGravityCenter in order to compute the moment's value from a vpMomentObject.
- This gravity center should be stored in a vpMomentDatabase where it can be accessed.
+ Sometimes, a moment needs to have access to other moment's values to be
+computed. For example vpMomentCentered needs additionnal information about the
+gravity center vpMomentGravityCenter in order to compute the moment's value
+from a vpMomentObject. This gravity center should be stored in a
+vpMomentDatabase where it can be accessed.
- All moments in a database can access each other freely at any time. They can also verify if a moment is present in the database or not.
- Here is a example of a dependency between two moments using a vpMomentDatabase:
- \code
+ All moments in a database can access each other freely at any time. They can
+also verify if a moment is present in the database or not. Here is a example
+of a dependency between two moments using a vpMomentDatabase:
+
+\code
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMomentGravityCenter.h>
@@ -81,75 +87,77 @@ int main()
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)
+ // 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;
vpMomentGravityCenter g; // declaration of gravity center
vpMomentCentered mc; // mc containts centered moments
-
+
g.linkTo(db); //add gravity center to database
mc.linkTo(db); //centered moments depend on gravity, add them to the
- //database to grant access
-
+ //database to grant access
+
db.updateAll(obj); // All of the moments must be updated, not just mc
-
+
//There is no global compute method since the order of compute calls
//depends on the implementation
g.compute(); // compute the moment
mc.compute(); //compute centered moments AFTER gravity center
-
+
std::cout << "Gravity center: " << g << std:: endl; // print gravity center moment
std::cout << "Centered moments: " << mc << std:: endl; // print centered moment
-
+
return 0;
}
\endcode
The following code outputs:
\code
-Gravity center:
+Gravity center:
Xg=1.5, Yg=1.5
-Centered moments:
-2 0
+Centered moments:
+2 0
0 x
\endcode
- A moment is identified in the database by it's vpMoment::name method. Consequently, a database can contain at most one moment of each type.
- Often it is useful to update all moments with the same object. Shortcuts (vpMomentDatabase::updateAll) are provided for that matter.
+ A moment is identified in the database by it's vpMoment::name method.
+Consequently, a database can contain at most one moment of each type. Often it
+is useful to update all moments with the same object. Shortcuts
+(vpMomentDatabase::updateAll) are provided for that matter.
*/
-class VISP_EXPORT vpMomentDatabase{
- private:
+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;
- }
- };
+ struct cmp_str {
+ bool operator()(char const *a, char const *b) const { return std::strcmp(a, b) < 0; }
+ };
#endif
- std::map<const char*,vpMoment*,cmp_str> moments;
- void add(vpMoment& moment, const char* name);
- public:
- vpMomentDatabase() : moments() {}
- virtual ~vpMomentDatabase() {}
-
- /** @name Inherited functionalities from vpMomentDatabase */
- //@{
- const vpMoment& get(const char* type, bool& found) const;
- /*!
- Get the first element in the database.
- May be useful in case an unnamed object is present but is the only element in the database.
- \return the first element in the database.
- */
- vpMoment& get_first(){return *(moments.begin()->second);}
-
- virtual void updateAll(vpMomentObject& object);
- //@}
-
- friend class vpMoment;
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& v);
+ std::map<const char *, vpMoment *, cmp_str> moments;
+ void add(vpMoment &moment, const char *name);
+
+public:
+ vpMomentDatabase() : moments() {}
+ virtual ~vpMomentDatabase() {}
+
+ /** @name Inherited functionalities from vpMomentDatabase */
+ //@{
+ const vpMoment &get(const char *type, bool &found) const;
+ /*!
+ Get the first element in the database.
+ May be useful in case an unnamed object is present but is the only element
+ in the database. \return the first element in the database.
+ */
+ vpMoment &get_first() { return *(moments.begin()->second); }
+
+ virtual void updateAll(vpMomentObject &object);
+ //@}
+
+ friend class vpMoment;
+ 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 4a20ff1..55fe47b 100644
--- a/modules/core/include/visp3/core/vpMomentGravityCenter.h
+++ b/modules/core/include/visp3/core/vpMomentGravityCenter.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpMomentGravityCenter.h
- \brief 2D Gravity Center moment descriptor (usually described by the pair Xg,Yg).
+ \brief 2D Gravity Center moment descriptor (usually described by the pair
+ Xg,Yg).
*/
#ifndef __MOMENTGRAVITYCENTER_H__
#define __MOMENTGRAVITYCENTER_H__
@@ -52,11 +54,13 @@ 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$.
+ This moment can be computed from scratch (no need to compute any different
+ moments before computing this). It gives access to both coordinates of the
+ gravity center \f$x_g\f$ and \f$y_g\f$.
- These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}} \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$
- \code
+ These coordinates are defined as follows: \f$x_g = \frac{m_{01}}{m_{00}}
+ \f$,\f$y_g = \frac{m_{10}}{m_{00}} \f$
+\code
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMomentGravityCenter.h>
@@ -106,16 +110,16 @@ Xg=0.0166667, Yg=-0.00833333
\endcode
*/
-class VISP_EXPORT vpMomentGravityCenter : public vpMoment {
+class VISP_EXPORT vpMomentGravityCenter : public vpMoment
+{
public:
-
vpMomentGravityCenter();
- virtual ~vpMomentGravityCenter() {};
+ virtual ~vpMomentGravityCenter(){};
/** @name Inherited functionalities from vpMomentGravityCenter */
//@{
void compute();
- const std::vector<double>& get() const;
+ const std::vector<double> &get() const;
/*!
Shortcut function to retrieve \f$x_g\f$.
\return The first gravity center coordinate.
@@ -129,10 +133,10 @@ public:
/*!
The class's string name.
*/
- const char* name() const {return "vpMomentGravityCenter";}
- void printDependencies(std::ostream& os) const;
+ 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 8441cd2..9359684 100644
--- a/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h
+++ b/modules/core/include/visp3/core/vpMomentGravityCenterNormalized.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+ * 2D normalized gravity center moment descriptor (usually described by the
+ *pair Xn,Yn)
*
* Authors:
* Filip Novotny
@@ -36,13 +38,14 @@
*****************************************************************************/
/*!
\file vpMomentGravityCenterNormalized.h
- \brief 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+ \brief 2D normalized gravity center moment descriptor (usually described by
+ the pair Xn,Yn)
*/
#ifndef __MOMENTGRAVITYNORMALIZED_H__
#define __MOMENTGRAVITYNORMALIZED_H__
-#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentGravityCenter.h>
class vpMomentObject;
/*!
@@ -52,20 +55,23 @@ class vpMomentObject;
\brief Class describing 2D normalized gravity center moment.
- Centered and normalized gravity center moment is defined as follows: \f$(x_n,y_n)\f$ where
- \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$.
+ Centered and normalized gravity center moment is defined as follows:
+ \f$(x_n,y_n)\f$ where \f$x_n = x_g a_n\f$ and \f$y_n = y_g a_n\f$.
- vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get access to \f$a_n\f$ and on vpMomentGravityCenter to get access to \f$(x_g,y_g)\f$ .
+ vpMomentGravityCenterNormalized depends on vpMomentAreaNormalized to get
+ access to \f$a_n\f$ and on vpMomentGravityCenter to get access to
+ \f$(x_g,y_g)\f$ .
*/
-class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter {
- public:
+class VISP_EXPORT vpMomentGravityCenterNormalized : public vpMomentGravityCenter
+{
+public:
vpMomentGravityCenterNormalized();
- virtual ~vpMomentGravityCenterNormalized() {};
+ virtual ~vpMomentGravityCenterNormalized(){};
void compute();
//! 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);
+ const char *name() const { return "vpMomentGravityCenterNormalized"; }
+ void printDependencies(std::ostream &os) const;
+ 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 456ff63..9e1f1ed 100644
--- a/modules/core/include/visp3/core/vpMomentObject.h
+++ b/modules/core/include/visp3/core/vpMomentObject.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,12 +42,12 @@
#ifndef __MOMENTOBJECT_H__
#define __MOMENTOBJECT_H__
+#include <cstdlib>
+#include <utility>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpMoment.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-#include <cstdlib>
-#include <utility>
class vpCameraParameters;
@@ -57,40 +58,52 @@ class vpCameraParameters;
\brief Class for generic objects.
- It contains all basic moments often described by \f$m_{ij}\f$ of order \f$i+j\f$ going from \f$m_{00}\f$ to the order used as parameter in vpMomentObject() constructor.
- All other moments implemented in ViSP (gravity center, alpha orientation, centered moments...) use this moment object as a combination of its different values.
+ It contains all basic moments often described by \f$m_{ij}\f$ of order
+\f$i+j\f$ going from \f$m_{00}\f$ to the order used as parameter in
+vpMomentObject() constructor. All other moments implemented in ViSP (gravity
+center, alpha orientation, centered moments...) use this moment object as a
+combination of its different values.
- When constructing a vpMomentObject() you need first to specify the maximum used moment order as parameter.
+ When constructing a vpMomentObject() you need first to specify the maximum
+used moment order as parameter.
- Then there are three ways to initialize a vpMomentObject. Firstly using fromImage() you can considerer
- a dense object \e O defined by a binary image. Secondly, as described in fromVector() you can also define a dense object \e O by a closed contour.
- In these two cases, 2D basic moments are defined by:
+ Then there are three ways to initialize a vpMomentObject. Firstly using
+fromImage() you can considerer a dense object \e O defined by a binary image.
+Secondly, as described in fromVector() you can also define a dense object \e O
+by a closed contour. In these two cases, 2D basic moments are defined by:
\f[m_{ij} = \int \int_{O} x^i y^j dx dy\f]
- Lastly, as presented in fromVector() you can consider a discrete set of \e n points. In that last case, the basic moments are defined by
- \f[m_{ij} = \sum_{k=1}^{n} x_k^i y_k^j \f]
+ Lastly, as presented in fromVector() you can consider a discrete set of \e n
+points. In that last case, the basic moments are defined by \f[m_{ij} =
+\sum_{k=1}^{n} x_k^i y_k^j \f]
With setType() method you can specify the object type.
- \attention Be careful with the object order. When you specify a maximum order in the vpMomentObject::vpMomentObject constructor (see its detailed description),
- it will compute all moment orders up to the order you specified. If you want to access the values \f$ m_{ij} \f$ with the vpMomentObject::get method, you can
- do object.get()[j*(order+1)+i].
+ \attention Be careful with the object order. When you specify a maximum
+order in the vpMomentObject::vpMomentObject constructor (see its detailed
+description), it will compute all moment orders up to the order you specified.
+If you want to access the values \f$ m_{ij} \f$ with the vpMomentObject::get
+method, you can do object.get()[j*(order+1)+i].
A few tips about which orders to use in different situations:
- - moment based visual servoing: use vpMomentObject(6). This will compute moment values up to order 6 which will enable vpFeatureMoments up to order 5 which is the maximum order required for common moments.
- - computing gravity center: use vpMomentObject(1). You only need \f$ m_{00},m_{01},m_{10} \f$. You should compute moments up to order 1.
- - computing gravity center interaction matrix (vpFeatureMomentGravityCenter): use vpMomentObject(2). This will compute moment values till order 2 since they are needed for the interaction matrix of vpFeatureMoments of order 1.
-
-
- The following example shows how to create a moment object from 4 discrete points
- locate on a plane one meter in front of the camera. It shows also how to get the basic
- moments that are computed and how to compute other classical moments such as the gravity
- center or the centered moments.
- \code
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentCommon.h>
-#include <visp3/core/vpPoint.h>
+ - moment based visual servoing: use vpMomentObject(6). This will compute
+moment values up to order 6 which will enable vpFeatureMoments up to order 5
+which is the maximum order required for common moments.
+ - computing gravity center: use vpMomentObject(1). You only need \f$
+m_{00},m_{01},m_{10} \f$. You should compute moments up to order 1.
+ - computing gravity center interaction matrix
+(vpFeatureMomentGravityCenter): use vpMomentObject(2). This will compute
+moment values till order 2 since they are needed for the interaction matrix of
+vpFeatureMoments of order 1.
+
+
+ The following example shows how to create a moment object from 4 discrete
+points locate on a plane one meter in front of the camera. It shows also how
+to get the basic moments that are computed and how to compute other classical
+moments such as the gravity center or the centered moments. \code #include
+<visp3/core/vpMomentCommon.h> #include <visp3/core/vpMomentObject.h> #include
+<visp3/core/vpPoint.h>
int main()
{
@@ -103,30 +116,32 @@ int main()
vec_p.push_back( vpPoint(-0.2,-0.15, 0.0) ); // values in meters
// These points are observed by a camera
- vpHomogeneousMatrix cMo(0, 0, 1, 0, 0, 0); // We set the camera to be 1m far the object
+ vpHomogeneousMatrix cMo(0, 0, 1, 0, 0, 0); // We set the camera to be 1m far
+the object
// ... update cMo from an image processing
- // Apply the perspective projection to update the points coordinates in the camera plane
- for(unsigned int i=0; i<vec_p.size(); ++i)
- vec_p[i].project(cMo);
+ // Apply the perspective projection to update the points coordinates in the
+camera plane for(unsigned int i=0; i<vec_p.size(); ++i) vec_p[i].project(cMo);
std::cout << "Considered points: " << std::endl;
for(unsigned int i=0; i<vec_p.size(); ++i)
- std::cout << "point " << i << ": " << vec_p[i].get_x() << ", " << vec_p[i].get_y() << std::endl;
+ std::cout << "point " << i << ": " << vec_p[i].get_x() << ", " <<
+vec_p[i].get_y() << std::endl;
// Define an image moment object from the previous points
vpMomentObject obj(5); // use moments up to order 5
- obj.setType(vpMomentObject::DISCRETE); // initialize the object as constituted by discrete points
- obj.fromVector(vec_p); // init the object from the points
+ obj.setType(vpMomentObject::DISCRETE); // initialize the object as
+constituted by discrete points obj.fromVector(vec_p); // init the object from
+the points
// --- Access the computed moments by querying the moment object
// 1. Getting a vector of doubles
std::vector<double> moment = obj.get();
- std::cout << std::endl << "Basic moment available (from vector of doubles) " << std::endl;
- for(unsigned int k=0; k<=obj.getOrder(); k++) {
- for(unsigned int l=0; l<(obj.getOrder()+1)-k; l++){
- std::cout << "m" << l << k << "=" << moment[k*(momobj.getOrder()+1)+ l] << "\t";
+ std::cout << std::endl << "Basic moment available (from vector of doubles) "
+<< std::endl; for(unsigned int k=0; k<=obj.getOrder(); k++) { for(unsigned int
+l=0; l<(obj.getOrder()+1)-k; l++){ std::cout << "m" << l << k << "=" <<
+moment[k*(momobj.getOrder()+1)+ l] << "\t";
}
std::cout<<std::endl;
}
@@ -136,26 +151,24 @@ int main()
std::cout << obj << std::endl;
// 3. Directly indexing the moment object
- std::cout << std::endl << "Direct acces to some basic moments: " << std::endl;
- std::cout << "m00: " << obj.get(0, 0) << std::endl;
- std::cout << "m10: " << obj.get(1, 0) << std::endl;
- std::cout << "m01: " << obj.get(0, 1) << std::endl;
- std::cout << "m22: " << obj.get(2, 2) << std::endl;
- std::cout << "m20: " << obj.get(2, 0) << std::endl;
- std::cout << "m02: " << obj.get(0, 2) << std::endl;
+ std::cout << std::endl << "Direct acces to some basic moments: " <<
+std::endl; std::cout << "m00: " << obj.get(0, 0) << std::endl; std::cout <<
+"m10: " << obj.get(1, 0) << std::endl; std::cout << "m01: " << obj.get(0, 1)
+<< std::endl; std::cout << "m22: " << obj.get(2, 2) << std::endl; std::cout <<
+"m20: " << obj.get(2, 0) << std::endl; std::cout << "m02: " << obj.get(0, 2)
+<< std::endl;
// Get common moments computed using basic moments
double m00 = vpMomentCommon::getSurface(obj); // surface = m00
double alpha = vpMomentCommon::getAlpha(obj); // orientation
- std::vector<double> mu_3 = vpMomentCommon::getMu3(obj); // centered moment up to 3rd order
+ std::vector<double> mu_3 = vpMomentCommon::getMu3(obj); // centered moment
+up to 3rd order
- std::cout << std::endl << "Common moments computed using basic moments:" << std::endl;
- std::cout << "Surface: " << m00 << std::endl;
- std::cout << "Alpha: " << alpha << std::endl;
- std::cout << "Centered moments (mu03, mu12, mu21, mu30): ";
- for(unsigned int i=0; i<mu_3.size(); ++i)
- std::cout << mu_3[i] << " ";
- std::cout << std::endl;
+ std::cout << std::endl << "Common moments computed using basic moments:" <<
+std::endl; std::cout << "Surface: " << m00 << std::endl; std::cout << "Alpha:
+" << alpha << std::endl; std::cout << "Centered moments (mu03, mu12, mu21,
+mu30): "; for(unsigned int i=0; i<mu_3.size(); ++i) std::cout << mu_3[i] << "
+"; std::cout << std::endl;
return 0;
}
@@ -197,98 +210,105 @@ 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
+\endcode
- Note that in the continuous case, the moment object \f$m_{00}\f$ corresponds to the surface \f$a\f$ of the object.
- In the discrete case, it is the number of discrete points \f$n\f$.
+ Note that in the continuous case, the moment object \f$m_{00}\f$ corresponds
+to the surface \f$a\f$ of the object. In the discrete case, it is the number
+of discrete points \f$n\f$.
*/
class VISP_EXPORT vpMomentObject
{
public:
-
/*!
Type of object that will be considered.
*/
- typedef enum{
- DENSE_FULL_OBJECT = 0, /*!< A set of points (typically from an image) which are interpreted as being dense. */
- DENSE_POLYGON = 1, /*!< A set of points (stored in clockwise order) describing a polygon. It will be treated as dense. */
- DISCRETE = 2, /*!< A cloud of points. Treated as discrete. */
+ typedef enum {
+ DENSE_FULL_OBJECT = 0, /*!< A set of points (typically from an image)
+ which are interpreted as being dense. */
+ DENSE_POLYGON = 1, /*!< A set of points (stored in clockwise order)
+ describing a polygon. It will be treated as dense.
+ */
+ DISCRETE = 2, /*!< A cloud of points. Treated as discrete. */
} vpObjectType;
/*!
Type of camera image background.
*/
- typedef enum{
- BLACK = 0, /*! Black background */
- WHITE = 1, /*! No functionality as of now */
- } vpCameraImgBckGrndType;
+ typedef enum {
+ BLACK = 0, /*! Black background */
+ WHITE = 1, /*! No functionality as of now */
+ } vpCameraImgBckGrndType;
- bool flg_normalize_intensity; // To scale the intensity of each individual pixel in the image by the maximum intensity value present in it
+ bool flg_normalize_intensity; // To scale the intensity of each individual
+ // pixel in the image by the maximum intensity
+ // value present in it
// Constructors
- vpMomentObject(unsigned int order);
- vpMomentObject(const vpMomentObject& srcobj);
+ explicit vpMomentObject(unsigned int order);
+ vpMomentObject(const vpMomentObject &srcobj);
/*!
Virtual destructor to allow polymorphic usage.
For instance,
\code
- vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
+ vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER);
\endcode
+ where vpWeightedMomentObject is child class of vpMomentObject
*/
virtual ~vpMomentObject();
- void fromImage(const vpImage<unsigned char>& image,unsigned char threshold, const vpCameraParameters& cam); // Binary version
- void fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam, vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size = true); // Photometric version
+ void fromImage(const vpImage<unsigned char> &image, unsigned char threshold,
+ const vpCameraParameters &cam); // Binary version
+ void fromImage(const vpImage<unsigned char> &image, const vpCameraParameters &cam, vpCameraImgBckGrndType bg_type,
+ bool normalize_with_pix_size = true); // Photometric version
- void fromVector(std::vector<vpPoint>& points);
- const std::vector<double>& get() const;
- double get(unsigned int i,unsigned int j) const;
+ void fromVector(std::vector<vpPoint> &points);
+ const std::vector<double> &get() const;
+ double get(unsigned int i, unsigned int j) const;
/*!
\return The type of object that is considered.
*/
- vpObjectType getType() const {return type;}
+ vpObjectType getType() const { return type; }
/*!
- \return The maximal order. The basic moments \f$m_{ij}\f$ that will be computed
- are for \f$i+j \in [0:\mbox{order}]\f$.
+ \return The maximal order. The basic moments \f$m_{ij}\f$ that will be
+ computed are for \f$i+j \in [0:\mbox{order}]\f$.
*/
- unsigned int getOrder() const {return order-1;}
+ unsigned int getOrder() const { return order - 1; }
// Constructor helpers
void init(unsigned int orderinp);
- void init(const vpMomentObject& objin);
+ 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.
*/
- void setType(vpObjectType input_type){this->type=input_type;}
+ void setType(vpObjectType input_type) { this->type = input_type; }
/*!
Converts the raw moments contained in vpMomentObject to a vpMatrix
\param momobj : A vpMomentObject
*/
- static vpMatrix convertTovpMatrix(const vpMomentObject& momobj);
+ static vpMatrix convertTovpMatrix(const vpMomentObject &momobj);
protected:
unsigned int order;
vpObjectType type;
std::vector<double> values;
- void set(unsigned int i, unsigned int j, const double& value_ij);
- void cacheValues(std::vector<double>& cache,double x, double y);
+ void set(unsigned int i, unsigned int j, const double &value_ij);
+ void cacheValues(std::vector<double> &cache, double x, double y);
private:
- void cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized);
- double calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points);
-
+ void cacheValues(std::vector<double> &cache, double x, double y, double IntensityNormalized);
+ double calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint> &points);
};
#endif
diff --git a/modules/core/include/visp3/core/vpMouseButton.h b/modules/core/include/visp3/core/vpMouseButton.h
index 21a209b..cf03a57 100644
--- a/modules/core/include/visp3/core/vpMouseButton.h
+++ b/modules/core/include/visp3/core/vpMouseButton.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpMouseButton_h
#define vpMouseButton_h
@@ -52,10 +52,10 @@ public:
typedef enum {
button1 = 1, /*!< Mouse left button. */
button2 = 2, /*!< Mouse middle button, or roll. */
- button3 = 3, /*!< Mouse right button. */
- none = 0 /*!< No button. */
- } vpMouseButtonType ;
-} ;
+ button3 = 3, /*!< Mouse right button. */
+ none = 0 /*!< No button. */
+ } vpMouseButtonType;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpMutex.h b/modules/core/include/visp3/core/vpMutex.h
index 5ede3ca..c082034 100644
--- a/modules/core/include/visp3/core/vpMutex.h
+++ b/modules/core/include/visp3/core/vpMutex.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,19 +36,21 @@
*
*****************************************************************************/
-
#ifndef __vpMutex_h_
#define __vpMutex_h_
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
#if defined(VISP_HAVE_PTHREAD)
-# include <pthread.h>
+#include <pthread.h>
#elif defined(_WIN32)
-# include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
+#include <windows.h>
#endif
/*!
@@ -58,8 +61,9 @@
Class that allows protection by mutex.
- This class implements native pthread functionalities if available, of native Windows threading
- capabilities if pthread is not available under Windows.
+ This class implements native pthread functionalities if available, of
+ native Windows threading capabilities if pthread is not available under
+ Windows.
An example of vpMutex usage is given in testMutex.cpp.
@@ -67,19 +71,20 @@
\sa vpScopedLock
*/
-class vpMutex {
+class vpMutex
+{
public:
- vpMutex() : m_mutex() {
+ vpMutex() : m_mutex()
+ {
#if defined(VISP_HAVE_PTHREAD)
- pthread_mutex_init( &m_mutex, NULL );
+ pthread_mutex_init(&m_mutex, NULL);
#elif defined(_WIN32)
-# ifdef WINRT_8_1
+#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
+#else
+ 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;
@@ -87,32 +92,32 @@ public:
}
#endif
}
- void lock() {
+ void lock()
+ {
#if defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock( &m_mutex );
+ pthread_mutex_lock(&m_mutex);
#elif defined(_WIN32)
DWORD dwWaitResult;
-# ifdef WINRT_8_1
+#ifdef WINRT_8_1
dwWaitResult = WaitForSingleObjectEx(m_mutex, INFINITE, FALSE);
-# else
- dwWaitResult = WaitForSingleObject(
- m_mutex, // handle to mutex
- INFINITE); // no time-out interval
-# endif
+#else
+ dwWaitResult = WaitForSingleObject(m_mutex, // handle to mutex
+ INFINITE); // no time-out interval
+#endif
if (dwWaitResult == WAIT_FAILED)
- std::cout << "lock() error: " << GetLastError() << std::endl;
+ std::cout << "lock() error: " << GetLastError() << std::endl;
#endif
}
- void unlock() {
+ void unlock()
+ {
#if defined(VISP_HAVE_PTHREAD)
- pthread_mutex_unlock( &m_mutex );
+ pthread_mutex_unlock(&m_mutex);
#elif defined(_WIN32)
- // Release ownership of the mutex object
- if (!ReleaseMutex(m_mutex))
- {
- // Handle error.
- std::cout << "unlock() error: " << GetLastError() << std::endl;
- }
+ // Release ownership of the mutex object
+ if (!ReleaseMutex(m_mutex)) {
+ // Handle error.
+ std::cout << "unlock() error: " << GetLastError() << std::endl;
+ }
#endif
}
@@ -124,10 +129,12 @@ public:
\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 <visp3/core/vpMutex.h>
+ 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 <visp3/core/vpMutex.h>
int main()
{
@@ -163,29 +170,23 @@ int main()
class vpScopedLock
{
private:
- vpMutex & _mutex;
+ vpMutex &_mutex;
-// private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpScopedLock &operator=(const vpScopedLock &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpScopedLock &operator=(const vpScopedLock &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
//! Constructor that locks the mutex.
- vpScopedLock(vpMutex & mutex)
- : _mutex(mutex)
- {
- _mutex.lock();
- }
+ vpScopedLock(vpMutex &mutex) : _mutex(mutex) { _mutex.lock(); }
//! Destructor that unlocks the mutex.
- ~vpScopedLock()
- {
- _mutex.unlock();
- }
+ ~vpScopedLock() { _mutex.unlock(); }
};
+
private:
#if defined(VISP_HAVE_PTHREAD)
pthread_mutex_t m_mutex;
diff --git a/modules/core/include/visp3/core/vpNetwork.h b/modules/core/include/visp3/core/vpNetwork.h
index 2bebaef..2c866d3 100644
--- a/modules/core/include/visp3/core/vpNetwork.h
+++ b/modules/core/include/visp3/core/vpNetwork.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,40 +42,41 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpRequest.h>
-#include <vector>
+#include <iostream>
#include <stdio.h>
#include <string.h>
-#include <iostream>
+#include <vector>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
-# include <sys/socket.h>
-# include <netinet/in.h>
-# include <arpa/inet.h>
-# include <netdb.h>
+#include <arpa/inet.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
#else
-# include<io.h>
+#include <io.h>
//# include<winsock.h>
-# include<winsock2.h>
+#include <winsock2.h>
//# pragma comment(lib, "ws2_32.lib") // Done by CMake in main CMakeLists.txt
#endif
#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
-# include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
#endif
-/*!
+/*!
\class vpNetwork
-
+
\ingroup group_core_network
\brief This class represents a Transmission Control Protocol (TCP) network.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
on one computer to another program on another computer.
-
+
\warning This class shouldn't be used directly. You better use vpClient and
- vpServer to simulate your network. Some exemples are provided in these classes.
+ vpServer to simulate your network. Some exemples are provided in these
+ classes.
\sa vpServer
\sa vpNetwork
@@ -83,26 +85,26 @@ class VISP_EXPORT vpNetwork
{
protected:
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- struct vpReceptor{
+ struct vpReceptor {
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketFileDescriptorReceptor;
- socklen_t receptorAddressSize;
+ int socketFileDescriptorReceptor;
+ socklen_t receptorAddressSize;
#else
- SOCKET socketFileDescriptorReceptor;
- int receptorAddressSize;
+ SOCKET socketFileDescriptorReceptor;
+ int receptorAddressSize;
#endif
- struct sockaddr_in receptorAddress;
- std::string receptorIP;
+ struct sockaddr_in receptorAddress;
+ std::string receptorIP;
vpReceptor() : socketFileDescriptorReceptor(0), receptorAddressSize(), receptorAddress(), receptorIP() {}
};
-
- struct vpEmitter{
- struct sockaddr_in emitterAddress;
+
+ struct vpEmitter {
+ struct sockaddr_in emitterAddress;
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketFileDescriptorEmitter;
+ int socketFileDescriptorEmitter;
#else
- SOCKET socketFileDescriptorEmitter;
+ SOCKET socketFileDescriptorEmitter;
#endif
vpEmitter() : emitterAddress(), socketFileDescriptorEmitter(0)
{
@@ -117,139 +119,135 @@ protected:
//######## PARAMETERS ########
//# #
//############################
-
- vpEmitter emitter;
+
+ vpEmitter emitter;
std::vector<vpReceptor> receptor_list;
- fd_set readFileDescriptor;
+ fd_set readFileDescriptor;
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int socketMax;
+ int socketMax;
#else
- SOCKET socketMax;
+ SOCKET socketMax;
#endif
-
- //Message Handling
- std::vector<vpRequest*> request_list;
-
- unsigned int max_size_message;
- std::string separator;
- std::string beginning;
- std::string end;
- std::string param_sep;
-
- std::string currentMessageReceived;
-
- struct timeval tv;
- long tv_sec;
- long tv_usec;
-
- bool verboseMode;
-
+
+ // Message Handling
+ std::vector<vpRequest *> request_list;
+
+ unsigned int max_size_message;
+ std::string separator;
+ std::string beginning;
+ std::string end;
+ std::string param_sep;
+
+ std::string currentMessageReceived;
+
+ struct timeval tv;
+ long tv_sec;
+ long tv_usec;
+
+ bool verboseMode;
+
private:
-
- std::vector<int> _handleRequests();
- int _handleFirstRequest();
-
- void _receiveRequest();
- void _receiveRequestFrom(const unsigned int &receptorEmitting);
- int _receiveRequestOnce();
- int _receiveRequestOnceFrom(const unsigned int &receptorEmitting);
-
+ std::vector<int> _handleRequests();
+ int _handleFirstRequest();
+
+ void _receiveRequest();
+ void _receiveRequestFrom(const unsigned int &receptorEmitting);
+ int _receiveRequestOnce();
+ int _receiveRequestOnceFrom(const unsigned int &receptorEmitting);
+
public:
+ vpNetwork();
+ virtual ~vpNetwork();
+
+ void addDecodingRequest(vpRequest *);
+
+ int getReceptorIndex(const char *name);
- vpNetwork();
- virtual ~vpNetwork();
-
- void addDecodingRequest(vpRequest *);
-
- int getReceptorIndex(const char *name);
-
/*!
Get the Id of the request at the index ind.
\param ind : Index of the request.
-
+
\return Id of the request.
*/
- std::string getRequestIdFromIndex(const int &ind){
- if(ind >= (int)request_list.size() || ind < 0)
- return "";
- return request_list[(unsigned)ind]->getId();
- }
-
+ std::string getRequestIdFromIndex(const int &ind)
+ {
+ if (ind >= (int)request_list.size() || ind < 0)
+ return "";
+ return request_list[(unsigned)ind]->getId();
+ }
+
/*!
Get the maximum size that the emitter can receive (in request mode).
-
+
\sa vpNetwork::setMaxSizeReceivedMessage()
\return Acutal max size value.
*/
- unsigned int getMaxSizeReceivedMessage(){ return max_size_message; }
-
- void print(const char *id = "");
-
- template<typename T>
- int receive(T* object, const unsigned int &sizeOfObject = sizeof(T));
- template<typename T>
- int receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject = sizeof(T));
-
- std::vector<int> receiveRequest();
- std::vector<int> receiveRequestFrom(const unsigned int &receptorEmitting);
- int receiveRequestOnce();
- int receiveRequestOnceFrom(const unsigned int &receptorEmitting);
-
- std::vector<int> receiveAndDecodeRequest();
- std::vector<int> receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting);
- int receiveAndDecodeRequestOnce();
- int receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting);
-
- void removeDecodingRequest(const char *);
-
- template<typename T>
- int send(T* object, const int unsigned &sizeOfObject = sizeof(T));
- template<typename T>
- int sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject = sizeof(T));
-
- int sendRequest(vpRequest &req);
- int sendRequestTo(vpRequest &req, const unsigned int &dest);
-
- int sendAndEncodeRequest(vpRequest &req);
- int sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest);
-
+ unsigned int getMaxSizeReceivedMessage() { return max_size_message; }
+
+ void print(const char *id = "");
+
+ template <typename T> int receive(T *object, const unsigned int &sizeOfObject = sizeof(T));
+ template <typename T>
+ int receiveFrom(T *object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject = sizeof(T));
+
+ std::vector<int> receiveRequest();
+ std::vector<int> receiveRequestFrom(const unsigned int &receptorEmitting);
+ int receiveRequestOnce();
+ int receiveRequestOnceFrom(const unsigned int &receptorEmitting);
+
+ std::vector<int> receiveAndDecodeRequest();
+ std::vector<int> receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting);
+ int receiveAndDecodeRequestOnce();
+ int receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting);
+
+ void removeDecodingRequest(const char *);
+
+ template <typename T> int send(T *object, const int unsigned &sizeOfObject = sizeof(T));
+ template <typename T> int sendTo(T *object, const unsigned int &dest, const unsigned int &sizeOfObject = sizeof(T));
+
+ int sendRequest(vpRequest &req);
+ int sendRequestTo(vpRequest &req, const unsigned int &dest);
+
+ int sendAndEncodeRequest(vpRequest &req);
+ int sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest);
+
/*!
Change the maximum size that the emitter can receive (in request mode).
-
+
\sa vpNetwork::getMaxSizeReceivedMessage()
\param s : new maximum size value.
*/
- void setMaxSizeReceivedMessage(const unsigned int &s){ max_size_message = s;}
-
+ void setMaxSizeReceivedMessage(const unsigned int &s) { max_size_message = s; }
+
/*!
- Change the time the emitter spend to check if he receives a message from a receptor.
- Initially this value is set to 10usec.
-
+ Change the time the emitter spend to check if he receives a message from a
+ receptor. Initially this value is set to 10usec.
+
\sa vpNetwork::setTimeoutUSec()
\param sec : new value in second.
*/
- void setTimeoutSec(const long &sec){ tv_sec = sec; }
-
+ void setTimeoutSec(const long &sec) { tv_sec = sec; }
+
/*!
- Change the time the emitter spend to check if he receives a message from a receptor.
- Initially this value is set to 10usec.
-
+ Change the time the emitter spend to check if he receives a message from a
+ receptor. Initially this value is set to 10usec.
+
\sa vpNetwork::setTimeoutSec()
\param usec : new value in micro second.
*/
- void setTimeoutUSec(const long &usec){ tv_usec = usec; }
-
+ void setTimeoutUSec(const long &usec) { tv_usec = usec; }
+
/*!
Set the verbose mode.
-
+
\param mode : Change the verbose mode. True to turn on, False to turn off.
*/
- void setVerbose(const bool &mode){ verboseMode = mode; }
+ void setVerbose(const bool &mode) { verboseMode = mode; }
};
//######## Definition of Template Functions ########
@@ -257,118 +255,115 @@ public:
//##################################################
/*!
- Receives a object. The size of the received object is suppose to be the size of the type of the object.
- Note that a received message can correspond to a deconnection signal.
-
- \warning Using this function means that you know what kind of object you are suppose to receive,
- and when you are suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
+ Receives a object. The size of the received object is suppose to be the size
+ of the type of the object. Note that a received message can correspond to a
+ deconnection signal.
+
+ \warning Using this function means that you know what kind of object you are
+ suppose to receive, and when you are suppose to receive. If the emitter has
+ several receptors. It might be a problem, and in that case you better use
+ the "request" option.
+
\sa vpNetwork::receiveRequest()
- \sa vpNetwork::receiveRequestOnce()
+ \sa vpNetwork::receiveRequestOnce()
\sa vpNetwork::receiveAndDecodeRequest()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\param object : Received object.
\param sizeOfObject : Size of the received object.
-
+
\return the number of bytes received, or -1 if an error occured.
*/
-template<typename T>
-int vpNetwork::receive(T* object, const unsigned int &sizeOfObject)
+template <typename T> int vpNetwork::receive(T *object, const unsigned int &sizeOfObject)
{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No receptor" );
+ if (receptor_list.size() == 0) {
+ if (verboseMode)
+ vpTRACE("No receptor");
return -1;
}
-
+
tv.tv_sec = tv_sec;
#if TARGET_OS_IPHONE
tv.tv_usec = (int)tv_usec;
#else
tv.tv_usec = tv_usec;
#endif
-
- FD_ZERO(&readFileDescriptor);
-
- for(unsigned int i=0; i<receptor_list.size(); i++){
- FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
- if(i == 0)
+ FD_ZERO(&readFileDescriptor);
+
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
+ FD_SET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor);
+
+ if (i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ if (socketMax < receptor_list[i].socketFileDescriptorReceptor)
socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- 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){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
+
+ if (value == -1) {
+ if (verboseMode)
+ vpERROR_TRACE("Select error");
return -1;
- }
- else if(value == 0){
- //Timeout
+ } else if (value == 0) {
+ // Timeout
return 0;
- }
- else{
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
+ } 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);
+ 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)
- {
+ if (numbytes <= 0) {
std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
+ receptor_list.erase(receptor_list.begin() + (int)i);
return numbytes;
}
-
+
break;
}
}
}
-
+
return numbytes;
}
/*!
Receives a object from a receptor, by specifying its size or not.
Note that a received message can correspond to a deconnection signal.
-
- \warning Using this function means that you know what kind of object you are suppose to receive,
- and when you are suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" mode.
-
+
+ \warning Using this function means that you know what kind of object you are
+ suppose to receive, and when you are suppose to receive. If the emitter has
+ several receptors. It might be a problem, and in that case you better use
+ the "request" mode.
+
\sa vpNetwork::getReceptorIndex()
\sa vpNetwork::receiveRequestFrom()
- \sa vpNetwork::receiveRequestOnceFrom()
+ \sa vpNetwork::receiveRequestOnceFrom()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
\param object : Received object.
\param receptorEmitting : Index of the receptor emitting the message.
\param sizeOfObject : Size of the received object.
-
+
\return the number of bytes received, or -1 if an error occured.
*/
-template<typename T>
-int vpNetwork::receiveFrom(T* object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject)
+template <typename T>
+int vpNetwork::receiveFrom(T *object, const unsigned int &receptorEmitting, const unsigned int &sizeOfObject)
{
- if(receptor_list.size() == 0 || receptorEmitting > (unsigned int)receptor_list.size()-1 )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index" );
+ if (receptor_list.size() == 0 || receptorEmitting > (unsigned int)receptor_list.size() - 1) {
+ if (verboseMode)
+ vpTRACE("No receptor at the specified index");
return -1;
}
-
+
tv.tv_sec = tv_sec;
#if TARGET_OS_IPHONE
tv.tv_usec = (int)tv_usec;
@@ -377,50 +372,51 @@ int vpNetwork::receiveFrom(T* object, const unsigned int &receptorEmitting, cons
#endif
FD_ZERO(&readFileDescriptor);
-
+
socketMax = receptor_list[receptorEmitting].socketFileDescriptorReceptor;
- FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor);
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
+ FD_SET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor);
+
+ int value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv);
int numbytes = 0;
-
- if(value == -1){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
+
+ if (value == -1) {
+ if (verboseMode)
+ vpERROR_TRACE("Select error");
return -1;
- }
- else if(value == 0){
- //timeout
+ } else if (value == 0) {
+ // timeout
return 0;
- }
- else{
- if(FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor)){
+ } else {
+ if (FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor)) {
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes = recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, sizeOfObject, 0);
+ numbytes =
+ recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char *)(void *)object, sizeOfObject, 0);
#else
- numbytes = recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, (char*)(void*)object, (int)sizeOfObject, 0);
+ numbytes = recv((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,
+ (char *)(void *)object, (int)sizeOfObject, 0);
#endif
- if(numbytes <= 0)
- {
- std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
+ if (numbytes <= 0) {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr)
+ << std::endl;
+ receptor_list.erase(receptor_list.begin() + (int)receptorEmitting);
return numbytes;
}
}
}
-
+
return numbytes;
}
/*!
- Send an object. The size of the received object is suppose to be the size of its type.
- Note that sending object containing pointers, virtual methods, etc, won't probably work.
-
- \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
- and when it is suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
+ Send an object. The size of the received object is suppose to be the size of
+ its type. Note that sending object containing pointers, virtual methods,
+ etc, won't probably work.
+
+ \warning Using this function means that, in the other side of the network,
+ it knows what kind of object it is suppose to receive, and when it is
+ suppose to receive. If the emitter has several receptors. It might be a
+ problem, and in that case you better use the "request" option.
+
\sa vpNetwork::sendTo()
\sa vpNetwork::sendRequest()
\sa vpNetwork::sendRequestTo()
@@ -429,19 +425,17 @@ int vpNetwork::receiveFrom(T* object, const unsigned int &receptorEmitting, cons
\param object : Received object.
\param sizeOfObject : Size of the object
-
+
\return The number of bytes sent, or -1 if an error happened.
*/
-template<typename T>
-int vpNetwork::send(T* object, const unsigned int &sizeOfObject)
+template <typename T> int vpNetwork::send(T *object, const unsigned int &sizeOfObject)
{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No receptor !" );
+ if (receptor_list.size() == 0) {
+ if (verboseMode)
+ vpTRACE("No receptor !");
return 0;
}
-
+
int flags = 0;
//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
#if defined(__linux__)
@@ -449,23 +443,22 @@ int vpNetwork::send(T* object, const unsigned int &sizeOfObject)
#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);
+ 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
-
}
/*!
Send an object. The size has to be specified.
-
- \warning Using this function means that, in the other side of the network, it knows what kind of object it is suppose to receive,
- and when it is suppose to receive.
- If the emitter has several receptors. It might be a problem, and in that case you better use the
- "request" option.
-
+
+ \warning Using this function means that, in the other side of the network,
+ it knows what kind of object it is suppose to receive, and when it is
+ suppose to receive. If the emitter has several receptors. It might be a
+ problem, and in that case you better use the "request" option.
+
\sa vpNetwork::getReceptorIndex()
\sa vpNetwork::send()
\sa vpNetwork::sendRequest()
@@ -476,19 +469,17 @@ int vpNetwork::send(T* object, const unsigned int &sizeOfObject)
\param object : Object to send.
\param dest : Index of the receptor that you are sending the object.
\param sizeOfObject : Size of the object.
-
+
\return The number of bytes sent, or -1 if an error happened.
*/
-template<typename T>
-int vpNetwork::sendTo(T* object, const unsigned int &dest, const unsigned int &sizeOfObject)
+template <typename T> int vpNetwork::sendTo(T *object, const unsigned int &dest, const unsigned int &sizeOfObject)
{
- if(receptor_list.size() == 0 || dest > (unsigned int)receptor_list.size()-1 )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index." );
+ if (receptor_list.size() == 0 || dest > (unsigned int)receptor_list.size() - 1) {
+ if (verboseMode)
+ vpTRACE("No receptor at the specified index.");
return 0;
}
-
+
int flags = 0;
//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
#if defined(__linux__)
@@ -496,11 +487,11 @@ int vpNetwork::sendTo(T* object, const unsigned int &dest, const unsigned int &s
#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);
+ 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/vpNoise.h b/modules/core/include/visp3/core/vpNoise.h
index 0c79306..1010ef7 100644
--- a/modules/core/include/visp3/core/vpNoise.h
+++ b/modules/core/include/visp3/core/vpNoise.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpNoise_hh
#define vpNoise_hh
diff --git a/modules/core/include/visp3/core/vpPixelMeterConversion.h b/modules/core/include/visp3/core/vpPixelMeterConversion.h
index 0efb670..721ece3 100644
--- a/modules/core/include/visp3/core/vpPixelMeterConversion.h
+++ b/modules/core/include/visp3/core/vpPixelMeterConversion.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,17 +40,16 @@
#ifndef vpPixelMeterConversion_H
#define vpPixelMeterConversion_H
-
/*!
\file vpPixelMeterConversion.h
\brief pixel to meter conversion
*/
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpException.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
/*!
\class vpPixelMeterConversion
@@ -65,191 +65,171 @@
class VISP_EXPORT vpPixelMeterConversion
{
public:
-
-/*!
- \brief Point coordinates conversion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param u : input coordinate in pixels along image horizontal axis.
- \param v : input coordinate in pixels along image vertical axis.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
- perspective projection without distortion.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
- projection with distortion.
-*/
-inline static void
-convertPoint(const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,u,v,x,y);
+ /*!
+ \brief Point coordinates conversion from pixel coordinates
+ \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param u : input coordinate in pixels along image horizontal axis.
+ \param v : input coordinate in pixels along image vertical axis.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of
+ perspective projection with distortion.
+ */
+ inline static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
+ {
+ switch (cam.projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ convertPointWithoutDistortion(cam, u, v, x, y);
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,u,v,x,y);
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ convertPointWithDistortion(cam, u, v, x, y);
break;
- }
-}
-
-/*!
- \brief Point coordinates conversion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- The used formula depends on the projection model of the camera. To
- know the currently used projection model use
- vpCameraParameter::get_projModel()
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
- perspective projection without distortion.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of perspective
- projection with distortion.
-*/
-inline static void
-convertPoint(const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- switch(cam.projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- convertPointWithoutDistortion(cam,iP,x,y);
+ }
+ }
+
+ /*!
+ \brief Point coordinates conversion from pixel coordinates
+ Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
+
+ The used formula depends on the projection model of the camera. To
+ know the currently used projection model use
+ vpCameraParameter::get_projModel()
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates
+ are given by :
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$ in the case of
+ perspective projection without distortion.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2+((v-v_0)/p_y)^2 \f$ in the case of
+ perspective projection with distortion.
+ */
+ inline static void convertPoint(const vpCameraParameters &cam, const vpImagePoint &iP, double &x, double &y)
+ {
+ switch (cam.projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ convertPointWithoutDistortion(cam, iP, x, y);
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- convertPointWithDistortion(cam,iP,x,y);
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ convertPointWithDistortion(cam, iP, x, y);
break;
- }
-}
-
-/*!
- \brief Point coordinates conversion without distortion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param u : input coordinate in pixels along image horizontal axis.
- \param v : input coordinate in pixels along image vertical axis.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
-*/
-inline static void
-convertPointWithoutDistortion(
- const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- x = (u - cam.u0)*cam.inv_px ;
- y = (v - cam.v0)*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion without distortion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
-*/
-inline static void
-convertPointWithoutDistortion(
- const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- x = (iP.get_u() - cam.u0)*cam.inv_px ;
- y = (iP.get_v() - cam.v0)*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion with distortion from pixel coordinates
- \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param u : input coordinate in pixels along image horizontal axis.
- \param v : input coordinate in pixels along image vertical axis.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
-*/
-inline static void
-convertPointWithDistortion(
- const vpCameraParameters &cam,
- const double &u, const double &v,
- double &x, double &y)
-{
- double r2 = 1.+cam.kdu*(vpMath::sqr((u - cam.u0)*cam.inv_px) +
- vpMath::sqr((v-cam.v0)*cam.inv_py));
- x = (u - cam.u0)*r2*cam.inv_px ;
- y = (v - cam.v0)*r2*cam.inv_py ;
-}
-
-/*!
- \brief Point coordinates conversion with distortion from pixel coordinates
- Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
-
- \param cam : camera parameters.
- \param iP : input coordinates in pixels.
- \param x : output coordinate in meter along image plane x-axis.
- \param y : output coordinate in meter along image plane y-axis.
-
- Thanks to the pixel coordinates in the frame (u,v), the meter coordinates are given by :
-
- \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
- \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
- with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
-*/
-inline static void
-convertPointWithDistortion(
- const vpCameraParameters &cam,
- const vpImagePoint &iP,
- double &x, double &y)
-{
- double r2 = 1.+cam.kdu*(vpMath::sqr((iP.get_u() - cam.u0)*cam.inv_px) +
- vpMath::sqr((iP.get_v()-cam.v0)*cam.inv_py));
- x = (iP.get_u() - cam.u0)*r2*cam.inv_px ;
- y = (iP.get_v() - cam.v0)*r2*cam.inv_py ;
-}
+ }
+ }
+
+ /*!
+ \brief Point coordinates conversion without distortion from pixel
+ coordinates \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param u : input coordinate in pixels along image horizontal axis.
+ \param v : input coordinate in pixels along image vertical axis.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
+ */
+ inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const double &u, const double &v,
+ double &x, double &y)
+ {
+ x = (u - cam.u0) * cam.inv_px;
+ y = (v - cam.v0) * cam.inv_py;
+ }
+
+ /*!
+ \brief Point coordinates conversion without distortion from pixel
+ coordinates Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in
+ meter.
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates
+ are given by :
+
+ \f$ x = (u-u_0)/p_x \f$ and \f$ y = (v-v_0)/p_y \f$
+ */
+ inline static void convertPointWithoutDistortion(const vpCameraParameters &cam, const vpImagePoint &iP, double &x,
+ double &y)
+ {
+ x = (iP.get_u() - cam.u0) * cam.inv_px;
+ y = (iP.get_v() - cam.v0) * cam.inv_py;
+ }
+
+ /*!
+ \brief Point coordinates conversion with distortion from pixel coordinates
+ \f$(u,v)\f$ to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param u : input coordinate in pixels along image horizontal axis.
+ \param v : input coordinate in pixels along image vertical axis.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
+ */
+ inline static void convertPointWithDistortion(const vpCameraParameters &cam, const double &u, const double &v,
+ double &x, double &y)
+ {
+ double r2 = 1. + cam.kdu * (vpMath::sqr((u - cam.u0) * cam.inv_px) + vpMath::sqr((v - cam.v0) * cam.inv_py));
+ x = (u - cam.u0) * r2 * cam.inv_px;
+ y = (v - cam.v0) * r2 * cam.inv_py;
+ }
+
+ /*!
+ \brief Point coordinates conversion with distortion from pixel coordinates
+ Coordinates in pixel to normalized coordinates \f$(x,y)\f$ in meter.
+
+ \param cam : camera parameters.
+ \param iP : input coordinates in pixels.
+ \param x : output coordinate in meter along image plane x-axis.
+ \param y : output coordinate in meter along image plane y-axis.
+
+ Thanks to the pixel coordinates in the frame (u,v), the meter coordinates
+ are given by :
+
+ \f$ x = (u-u_0)*(1+k_{du}*r^2)/p_x \f$ and
+ \f$ y = (v-v_0)*(1+k_{du}*r^2)/p_y \f$
+ with \f$ r^2=((u - u_0)/p_x)^2 + ((v-v_0)/p_y)^2 \f$
+ */
+ inline static void convertPointWithDistortion(const vpCameraParameters &cam, const vpImagePoint &iP, double &x,
+ double &y)
+ {
+ double r2 = 1. + cam.kdu * (vpMath::sqr((iP.get_u() - cam.u0) * cam.inv_px) +
+ vpMath::sqr((iP.get_v() - cam.v0) * cam.inv_py));
+ x = (iP.get_u() - cam.u0) * r2 * cam.inv_px;
+ y = (iP.get_v() - cam.v0) * r2 * cam.inv_py;
+ }
//! line coordinates conversion (rho,theta)
- static void convertLine(const vpCameraParameters &cam,
- const double &rho_p, const double &theta_p,
- double &rho_m, double &theta_m) ;
+ static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m,
+ double &theta_m);
-
- static void convertMoment(const vpCameraParameters &cam,
- unsigned int order,
- const vpMatrix &moment_pixel,
- vpMatrix &moment_meter) ;
-} ;
+ static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel,
+ vpMatrix &moment_meter);
+};
#endif
/*
@@ -257,4 +237,3 @@ convertPointWithDistortion(
* c-basic-offset: 2
* End:
*/
-
diff --git a/modules/core/include/visp3/core/vpPlane.h b/modules/core/include/visp3/core/vpPlane.h
index 9630e03..0315ab6 100644
--- a/modules/core/include/visp3/core/vpPlane.h
+++ b/modules/core/include/visp3/core/vpPlane.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,12 @@
*
*****************************************************************************/
-
#ifndef vpPlane_hh
#define vpPlane_hh
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
/*!
\class vpPlane
@@ -60,38 +60,34 @@ class VISP_EXPORT vpPlane
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
// for backward compatibility
- public:
+public:
#else
- private:
+private:
#endif
- double A,B,C,D ;
+ double A, B, C, D;
public:
- typedef enum {
- object_frame,
- camera_frame
- } vpPlaneFrame;
- vpPlane() ;
- vpPlane(const vpPlane& P) ;
- vpPlane(const double A, const double B,const double C,const double D) ;
- vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame=camera_frame) ;
- vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame=camera_frame) ;
- void init(const vpPoint& P, const vpPoint& Q, const vpPoint& R, vpPlaneFrame frame=camera_frame) ;
- void init(const vpColVector& P, const vpColVector &n) ;
- void init(const vpPlane& P) ;
+ typedef enum { object_frame, camera_frame } vpPlaneFrame;
+ vpPlane();
+ vpPlane(const vpPlane &P);
+ vpPlane(const double A, const double B, const double C, const double D);
+ vpPlane(const vpPoint &P, const vpColVector &n, vpPlaneFrame frame = camera_frame);
+ vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame = camera_frame);
+ void init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame frame = camera_frame);
+ void init(const vpColVector &P, const vpColVector &n);
+ void init(const vpPlane &P);
// SET the parameter
/*! Set plane parameter A. */
- inline void setA(const double a) { this->A = a ; }
+ inline void setA(const double a) { this->A = a; }
/*! Set plane parameter B. */
- inline void setB(const double b) { this->B = b ; }
+ inline void setB(const double b) { this->B = b; }
/*! Set plane parameter C. */
- inline void setC(const double c) { this->C = c ; }
+ inline void setC(const double c) { this->C = c; }
/*! Set plane parameter D. */
- inline void setD(const double d) { this->D = 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;
@@ -99,17 +95,17 @@ public:
this->D = d;
}
- vpPlane& operator =(const vpPlane& f) ;
+ vpPlane &operator=(const vpPlane &f);
// GET information
/*! \return The value of the plane parameter A. */
- double getA() const { return A ; }
+ double getA() const { return A; }
/*! \return The value of the plane parameter B. */
- double getB() const { return B ; }
+ double getB() const { return B; }
/*! \return The value of the plane parameter C. */
- double getC() const { return C ; }
+ double getC() const { return C; }
/*! \return The value of the plane parameter D. */
- double getD() const { return D ; }
+ double getD() const { return D; }
/*!
@@ -117,13 +113,14 @@ public:
corresponding to the plane parameters.
*/
- inline vpColVector getABCD() const {
+ inline vpColVector getABCD() const
+ {
vpColVector n(4);
- n[0]=A;
- n[1]=B;
- n[2]=C;
- n[3]=D;
-
+ n[0] = A;
+ n[1] = B;
+ n[2] = C;
+ n[3] = D;
+
return n;
}
/*!
@@ -136,33 +133,29 @@ public:
\sa getABCD()
*/
- inline vpColVector abcd() const {
+ inline vpColVector abcd() const
+ {
vpColVector n(4);
- n[0]=A;
- n[1]=B;
- n[2]=C;
- n[3]=D;
-
+ n[0] = A;
+ n[1] = B;
+ n[2] = C;
+ n[3] = D;
+
return n;
}
vpColVector getNormal() const;
void getNormal(vpColVector &n) const;
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p);
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpPlane &p);
// Operation with Plane
- void projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const ;
+ 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 ;
+ double getIntersection(const vpColVector &M1, vpColVector &H) const;
void changeFrame(const vpHomogeneousMatrix &cMo);
-
-} ;
-
-
+};
#endif
diff --git a/modules/core/include/visp3/core/vpPoint.h b/modules/core/include/visp3/core/vpPoint.h
index 7a8a182..9e09430 100644
--- a/modules/core/include/visp3/core/vpPoint.h
+++ b/modules/core/include/visp3/core/vpPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpPoint_H
#define vpPoint_H
@@ -46,10 +46,9 @@
class vpHomogeneousMatrix;
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpForwardProjection.h>
#include <visp3/core/vpColor.h>
-
+#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpMatrix.h>
/*!
\class vpPoint
@@ -63,32 +62,23 @@ public:
//! Basic constructor.
vpPoint();
vpPoint(double oX, double oY, double oZ);
- vpPoint(const vpColVector &P);
- vpPoint(const std::vector<double> &P);
+ explicit vpPoint(const vpColVector &P);
+ explicit vpPoint(const std::vector<double> &P);
//! Destructor.
virtual ~vpPoint() {}
public:
-
// Compute the 3D coordinates _cP (camera frame)
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP) ;
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP);
void changeFrame(const vpHomogeneousMatrix &cMo);
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- vpPoint *duplicate() const ;
+ void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green,
+ const unsigned int thickness = 1);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
+ vpPoint *duplicate() const;
// Get coordinates
double get_X() const;
@@ -103,18 +93,19 @@ public:
double get_y() const;
double get_w() const;
- void getWorldCoordinates(double& oX, double& oY, double& oZ);
+ void getWorldCoordinates(double &oX, double &oY, double &oZ);
void getWorldCoordinates(vpColVector &P);
vpColVector getWorldCoordinates(void);
void getWorldCoordinates(std::vector<double> &P);
//! Basic construction.
- void init() ;
+ void init();
- friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, const vpPoint& vpp);
- vpPoint& operator=(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 the camera frame _cP, output : the 2D coordinates _p.
+ //! Projection onto the image plane of a point. Input: the 3D coordinates in
+ //! the camera frame _cP, output : the 2D coordinates _p.
void projection(const vpColVector &_cP, vpColVector &_p);
void projection();
diff --git a/modules/core/include/visp3/core/vpPolygon.h b/modules/core/include/visp3/core/vpPolygon.h
index b26f007..64db072 100644
--- a/modules/core/include/visp3/core/vpPolygon.h
+++ b/modules/core/include/visp3/core/vpPolygon.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,15 +41,15 @@
#ifndef vpPolygon_h
#define vpPolygon_h
-#include <vector>
#include <list>
+#include <vector>
-#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpRect.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpRect.h>
/*!
\class vpPolygon
@@ -58,8 +59,8 @@
A polygon is internally represented by N 2D points.
By default three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint
- class documentation for more details about the frame) are used \f$ (0,0) \f$,
- \f$ (1,0) \f$ and \f$ (0,1) \f$.
+ class documentation for more details about the frame) are used \f$ (0,0)
+\f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
The code bellow shows how to manipulate a polygon.
\code
@@ -82,7 +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;
@@ -90,7 +92,9 @@ int main()
// 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;
}
@@ -98,106 +102,102 @@ int main()
*/
class VISP_EXPORT vpPolygon
{
- protected:
- //! Collection of image points containing the corners.
- std::vector<vpImagePoint> _corners;
- //! Center of the polygon. It is automatically computed when the corners are set.
- vpImagePoint _center;
- //! Area of the polygon.
- double _area;
- //! Flag set to true when the polygon is a good polygon (ie. it has more than two corners) or false otherwise.
- bool _goodPoly;
- //! Bounding box containing the polygon.
- vpRect _bbox;
-
- public:
- enum PointInPolygonMethod {
- PnPolySegmentIntersection, /*!< Legacy Point In Polygon test. */
- PnPolyRayCasting /*!< Point In Polygon test using ray casting method (faster). */
- };
-
- vpPolygon();
- vpPolygon(const std::vector<vpImagePoint>& corners);
- vpPolygon(const std::list<vpImagePoint>& corners);
- vpPolygon(const vpPolygon &poly);
- virtual ~vpPolygon();
-
- vpPolygon &operator=(const vpPolygon& poly);
-
- void buildFrom(const std::vector<vpImagePoint>& corners);
- void buildFrom(const std::list<vpImagePoint>& corners);
- void buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam);
-
- unsigned int getSize() const;
- void initClick(const vpImage<unsigned char>& I);
-
- bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method=PnPolyRayCasting) const;
-
- void display(const vpImage<unsigned char>& I, const vpColor& color, unsigned int thickness=1) const;
-
- /*!
- Get the corners of the polygon.
-
- \return A reference to the corners.
- */
- const std::vector<vpImagePoint>& getCorners() const {
- return _corners;
- }
-
-
- /*!
- Return the area of the polygon.
- The area is computed when the polygon is built from the corners.
-
- \return The area of the polygon.
- */
- inline double getArea() const{
- return this->_area;
- }
-
- /*!
- Return the center of the polygon.
- The center is computed when the polygon is built from the corners.
-
- \return The area of the polygon.
- */
- inline vpImagePoint getCenter() const{
- return this->_center;
- }
-
- /*!
- Return the bounding box. The bounding box is the smallest rectangle
- containing all the polygon.
-
- \return The bounding box of the polygon.
- */
- inline vpRect getBoundingBox() const {
- return _bbox;
- }
-
- protected:
- void init(const std::vector<vpImagePoint>& corners);
- void init(const std::list<vpImagePoint>& corners);
- void updateArea();
- void updateCenter();
- void updateBoundingBox();
-
- private:
- bool testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4) const;
- void precalcValuesPnPoly();
-
- std::vector<double> m_PnPolyConstants;
- std::vector<double> m_PnPolyMultiples;
+protected:
+ //! Collection of image points containing the corners.
+ std::vector<vpImagePoint> _corners;
+ //! Center of the polygon. It is automatically computed when the corners are
+ //! set.
+ vpImagePoint _center;
+ //! Area of the polygon.
+ double _area;
+ //! Flag set to true when the polygon is a good polygon (ie. it has more
+ //! than two corners) or false otherwise.
+ bool _goodPoly;
+ //! Bounding box containing the polygon.
+ vpRect _bbox;
+
+public:
+ enum PointInPolygonMethod {
+ PnPolySegmentIntersection, /*!< Legacy Point In Polygon test. */
+ PnPolyRayCasting /*!< Point In Polygon test using ray casting method
+ (faster). */
+ };
+
+ vpPolygon();
+ explicit vpPolygon(const std::vector<vpImagePoint> &corners);
+ explicit vpPolygon(const std::list<vpImagePoint> &corners);
+ vpPolygon(const vpPolygon &poly);
+ virtual ~vpPolygon();
+
+ vpPolygon &operator=(const vpPolygon &poly);
+
+ void buildFrom(const std::vector<vpImagePoint> &corners);
+ void buildFrom(const std::list<vpImagePoint> &corners);
+ void buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam);
+
+ unsigned int getSize() const;
+ void initClick(const vpImage<unsigned char> &I, unsigned int size = 5, const vpColor &color = vpColor::red,
+ unsigned int thickness = 1);
+ void initClick(const vpImage<vpRGBa> &I, unsigned int size = 5, const vpColor &color = vpColor::red,
+ unsigned int thickness = 1);
+
+ bool isInside(const vpImagePoint &iP, const PointInPolygonMethod &method = PnPolyRayCasting) const;
+
+ void display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness = 1) const;
+
+ /*!
+ Get the corners of the polygon.
+
+ \return A reference to the corners.
+ */
+ const std::vector<vpImagePoint> &getCorners() const { return _corners; }
+
+ /*!
+ Return the area of the polygon.
+ The area is computed when the polygon is built from the corners.
+
+ \return The area of the polygon.
+ */
+ inline double getArea() const { return this->_area; }
+
+ /*!
+ Return the center of the polygon.
+ The center is computed when the polygon is built from the corners.
+
+ \return The area of the polygon.
+ */
+ inline vpImagePoint getCenter() const { return this->_center; }
+
+ /*!
+ Return the bounding box. The bounding box is the smallest rectangle
+ containing all the polygon.
+
+ \return The bounding box of the polygon.
+ */
+ inline vpRect getBoundingBox() const { return _bbox; }
+
+protected:
+ void init(const std::vector<vpImagePoint> &corners);
+ void init(const std::list<vpImagePoint> &corners);
+ void updateArea();
+ void updateCenter();
+ void updateBoundingBox();
+
+private:
+ bool testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
+ const vpImagePoint &ip4) const;
+ void precalcValuesPnPoly();
+
+ std::vector<double> m_PnPolyConstants;
+ std::vector<double> m_PnPolyMultiples;
//###################
// Static Functions
//###################
- public:
- static bool isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j, const PointInPolygonMethod &method=PnPolyRayCasting);
- private:
- static bool intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i, const double &j, const double &i_test, const double &j_test);
+public:
+ static bool isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
+ const PointInPolygonMethod &method = PnPolyRayCasting);
};
#endif
-
diff --git a/modules/core/include/visp3/core/vpPolygon3D.h b/modules/core/include/visp3/core/vpPolygon3D.h
index e2a7980..47d2504 100644
--- a/modules/core/include/visp3/core/vpPolygon3D.h
+++ b/modules/core/include/visp3/core/vpPolygon3D.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,10 +46,9 @@
#include <vector>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpMeterPixelConversion.h>
-
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPoint.h>
/*!
\class vpPolygon3D
@@ -59,158 +59,164 @@
class VISP_EXPORT vpPolygon3D
{
public:
- typedef enum
- {
- NO_CLIPPING = 0,
- NEAR_CLIPPING = 1,
- FAR_CLIPPING = 2,
- LEFT_CLIPPING = 4,
- RIGHT_CLIPPING = 8,
- UP_CLIPPING = 16,
- DOWN_CLIPPING = 32,
- FOV_CLIPPING = 60,
- ALL_CLIPPING = 63
- } vpPolygon3DClippingType;
-
+ typedef enum {
+ NO_CLIPPING = 0,
+ NEAR_CLIPPING = 1,
+ FAR_CLIPPING = 2,
+ LEFT_CLIPPING = 4,
+ RIGHT_CLIPPING = 8,
+ UP_CLIPPING = 16,
+ DOWN_CLIPPING = 32,
+ FOV_CLIPPING = 60,
+ ALL_CLIPPING = 63
+ } vpPolygon3DClippingType;
+
public:
//! Number of points used to define the polygon.
unsigned int nbpt;
- //! Number of corners inside the image during the last call to getNbCornerInsideImage
+ //! Number of corners inside the image during the last call to
+ //! getNbCornerInsideImage
unsigned int nbCornersInsidePrev;
//! corners in the object frame
vpPoint *p;
//! Region of interest clipped
- std::vector<std::pair<vpPoint,unsigned int> > polyClipped;
+ std::vector<std::pair<vpPoint, unsigned int> > polyClipped;
//! Clipping flag
unsigned int clippingFlag;
//! Distance for near clipping
double distNearClip;
//! Distance for near clipping
double distFarClip;
-
-private:
- bool getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const vpColVector &normal,
- const unsigned int &flag);
-
- bool getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const unsigned int &flag, const double &distance);
-
-public:
- vpPolygon3D() ;
- vpPolygon3D(const vpPolygon3D& mbtp) ;
- virtual ~vpPolygon3D() ;
-
- void addPoint(const unsigned int n, const vpPoint &P) ;
-
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
-
- void computePolygonClipped(const vpCameraParameters &cam = vpCameraParameters());
-
+
+private:
+ bool getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo, const vpColVector &normal,
+ const unsigned int &flag);
+
+ bool getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo, const unsigned int &flag,
+ const double &distance);
+
+public:
+ vpPolygon3D();
+ vpPolygon3D(const vpPolygon3D &mbtp);
+ virtual ~vpPolygon3D();
+
+ void addPoint(const unsigned int n, const vpPoint &P);
+
+ void changeFrame(const vpHomogeneousMatrix &cMo);
+
+ void computePolygonClipped(const vpCameraParameters &cam = vpCameraParameters());
+
/*!
Get the clipping used.
-
+
\sa vpPolygon3DClipping
-
+
\return Clipping flags.
- */
- inline unsigned int getClipping() const { return clippingFlag; }
-
- /*!
- Get the far distance for clipping.
-
- \return Far clipping value.
*/
- inline double getFarClippingDistance() const { return distFarClip; }
-
+ inline unsigned int getClipping() const { return clippingFlag; }
+
+ /*!
+ Get the far distance for clipping.
+
+ \return Far clipping value.
+ */
+ inline double getFarClippingDistance() const { return distFarClip; }
+
/*!
Return the number of corners.
\return number of corner of the face
*/
- inline unsigned int getNbPoint() const {return nbpt ;}
+ inline unsigned int getNbPoint() const { return nbpt; }
/*!
Return the number of corners at the previous computation.
\return number of corner of the face at the previous computation
*/
- inline unsigned int getNbCornerInsidePrevImage() const { return nbCornersInsidePrev; }
+ inline unsigned int getNbCornerInsidePrevImage() const { return nbCornersInsidePrev; }
+
+ unsigned int getNbCornerInsideImage(const vpImage<unsigned char> &I, const vpCameraParameters &cam);
- unsigned int getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam);
-
/*!
Get the near distance for clipping.
-
+
\return Near clipping value.
*/
- inline double getNearClippingDistance() const { return distNearClip; }
-
- vpPoint & getPoint(const unsigned int _index);
-
+ inline double getNearClippingDistance() const { return distNearClip; }
+
+ vpPoint &getPoint(const unsigned int _index);
+
std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam);
- std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo);
+ std::vector<vpImagePoint> getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo);
+
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi);
- void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi);
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi, const vpHomogeneousMatrix &cMo);
- void getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint>&roi, const vpHomogeneousMatrix &cMo);
-
- void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi);
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi);
- void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo);
+ void getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi,
+ const vpHomogeneousMatrix &cMo);
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
/*!
@name Deprecated functions
*/
//@{
- vp_deprecated void getRoiClipped(std::vector<vpPoint> &points);
- //@}
+ vp_deprecated void getRoiClipped(std::vector<vpPoint> &points);
+//@}
#endif
- void getPolygonClipped(std::vector<std::pair<vpPoint,unsigned int> > &poly);
+ void getPolygonClipped(std::vector<std::pair<vpPoint, unsigned int> > &poly);
- void getPolygonClipped(std::vector<vpPoint> &poly);
+ void getPolygonClipped(std::vector<vpPoint> &poly);
- vpPolygon3D& operator=(const vpPolygon3D& mbtp) ;
+ vpPolygon3D &operator=(const vpPolygon3D &mbtp);
/*!
Specify which clipping to use.
-
+
\sa vpPolygon3DClipping
-
+
\param flags : New clipping flags.
*/
- inline void setClipping(const unsigned int &flags) { clippingFlag = flags; }
-
+ inline void setClipping(const unsigned int &flags) { clippingFlag = flags; }
+
/*!
Set the far distance for clipping.
-
+
\param dist : Far clipping value.
*/
- inline void setFarClippingDistance(const double &dist) { distFarClip = dist; clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);}
+ inline void setFarClippingDistance(const double &dist)
+ {
+ distFarClip = dist;
+ clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);
+ }
+
+ virtual void setNbPoint(const unsigned int nb);
- virtual void setNbPoint(const unsigned int nb) ;
-
/*!
Set the near distance for clipping.
-
+
\param dist : Near clipping value.
*/
- inline void setNearClippingDistance(const double &dist) { distNearClip = dist; clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);}
-
+ inline void setNearClippingDistance(const double &dist)
+ {
+ distNearClip = dist;
+ clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);
+ }
+
public:
- static void getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo,
- const unsigned int &clippingFlags, const vpCameraParameters &cam = vpCameraParameters(),
- const double &znear = 0.001, const double &zfar = 100 );
- static void getMinMaxRoi(const std::vector<vpImagePoint> &roi, int & i_min, int &i_max, int &j_min, int &j_max);
- static bool roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners);
+ static void getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut,
+ const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
+ const vpCameraParameters &cam = vpCameraParameters(), const double &znear = 0.001,
+ const double &zfar = 100);
+ static void getMinMaxRoi(const std::vector<vpImagePoint> &roi, int &i_min, int &i_max, int &j_min, int &j_max);
+ static bool roiInsideImage(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &corners);
};
#endif
-
diff --git a/modules/core/include/visp3/core/vpPoseVector.h b/modules/core/include/visp3/core/vpPoseVector.h
index 88df81c..27fbf45 100644
--- a/modules/core/include/visp3/core/vpPoseVector.h
+++ b/modules/core/include/visp3/core/vpPoseVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
#ifndef vpPOSEVECTOR_H
#define vpPOSEVECTOR_H
@@ -56,10 +56,9 @@ class vpThetaUVector;
class vpRowVector;
#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-
/*!
\class vpPoseVector
@@ -68,8 +67,8 @@ class vpRowVector;
\brief Implementation of a pose vector and operations on poses.
- The vpPose class implements a complete representation of every rigid motion in the
- euclidian space.
+ The vpPose class implements a complete representation of every rigid motion
+ in the euclidian space.
The vpPose class is derived from vpArray2D<double>.
@@ -83,8 +82,8 @@ class vpRowVector;
axis and \f$\theta \bf u \f$, the axis-angle representation of the
rotation \f$^{a}\bf{R}_{b}\f$ between these frames.
- Translations are expressed in meters, while the angles in the \f$\theta {\bf u}\f$
- axis-angle representation are expressed in radians.
+ Translations are expressed in meters, while the angles in the \f$\theta {\bf
+ u}\f$ axis-angle representation are expressed in radians.
To know more about the \f$\theta \bf u\f$ rotation representation,
see vpThetaUVector documentation.
@@ -94,45 +93,40 @@ class VISP_EXPORT vpPoseVector : public vpArray2D<double>
{
public:
// constructor
- vpPoseVector() ;
+ 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
- vpPoseVector(const vpHomogeneousMatrix& M) ;
+ explicit vpPoseVector(const vpHomogeneousMatrix &M);
// constructor convert a translation and a "thetau" vector into a pose
- vpPoseVector(const vpTranslationVector& tv,
- const vpThetaUVector& tu) ;
+ vpPoseVector(const vpTranslationVector &tv, const vpThetaUVector &tu);
// constructor convert a translation and a rotation matrix into a pose
- vpPoseVector(const vpTranslationVector& tv,
- const vpRotationMatrix& R) ;
+ vpPoseVector(const vpTranslationVector &tv, const vpRotationMatrix &R);
/*!
Destructor.
*/
- virtual ~vpPoseVector() {};
+ virtual ~vpPoseVector(){};
- vpPoseVector buildFrom(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz) ;
+ 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) ;
+ 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;
void extract(vpTranslationVector &tv) const;
- void extract(vpQuaternionVector& q) const;
+ void extract(vpQuaternionVector &q) const;
vpRotationMatrix getRotationMatrix() const;
vpThetaUVector getThetaUVector() const;
vpTranslationVector getTranslationVector() const;
// Load an homogeneous matrix from a file
- void load(std::ifstream &f) ;
+ void load(std::ifstream &f);
/*!
Set the value of an element of the pose vector: r[i] = x.
@@ -158,7 +152,7 @@ public:
\endcode
*/
- inline double &operator [](unsigned int i) { return *(data + i); }
+ inline double &operator[](unsigned int i) { return *(data + i); }
/*!
Get the value of an element of the pose vector: x = r[i].
@@ -166,7 +160,7 @@ public:
\code
vpPoseVector r(1, 2, 3, M_PI, -M_PI, 0);
-
+
double tx,ty,tz; // Translation
double tux, tuy,tuz; // Theta u rotation
tx = r[0];
@@ -177,19 +171,18 @@ public:
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;
@@ -198,9 +191,8 @@ public:
};
// 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 save(std::ofstream &f) const;
+ void set(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz);
vpRowVector t() const;
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
@@ -212,9 +204,9 @@ public:
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
- //@}
+ vp_deprecated void init(){};
+//@}
#endif
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpQuaternionVector.h b/modules/core/include/visp3/core/vpQuaternionVector.h
index de2e58c..d28226a 100644
--- a/modules/core/include/visp3/core/vpQuaternionVector.h
+++ b/modules/core/include/visp3/core/vpQuaternionVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef __QUATERNIONVECTOR_H__
#define __QUATERNIONVECTOR_H__
@@ -53,12 +52,11 @@
#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpThetaUVector.h>
-
/*!
\class vpQuaternionVector
\ingroup group_core_transformations
-
+
\brief Implementation of a rotation vector as quaternion angle
minimal representation.
@@ -73,30 +71,31 @@
or buildFrom() method.
It also defines common operations on a quaternion such as:
- - multiplication (scalar and quaternion)
- - addition
- - substraction.
+ - multiplication (scalar and quaternion)
+ - addition
+ - substraction.
*/
class VISP_EXPORT vpQuaternionVector : public vpRotationVector
{
-private:
+private:
static const double minimum;
+
public:
vpQuaternionVector();
vpQuaternionVector(const vpQuaternionVector &q);
- vpQuaternionVector(const double qx, const double qy, const double qz,const double qw) ;
- vpQuaternionVector(const vpRotationMatrix &R);
- vpQuaternionVector(const vpThetaUVector& tu) ;
- vpQuaternionVector(const vpColVector &q);
+ 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);
//! Destructor.
- virtual ~vpQuaternionVector() {};
+ virtual ~vpQuaternionVector(){};
- vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw) ;
- vpQuaternionVector buildFrom(const vpRotationMatrix& R);
- vpQuaternionVector buildFrom(const vpThetaUVector& tu);
- void set(const double x, const double y, const double z, const double w) ;
+ vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw);
+ vpQuaternionVector buildFrom(const vpRotationMatrix &R);
+ vpQuaternionVector buildFrom(const vpThetaUVector &tu);
+ void set(const double x, const double y, const double z, const double w);
double x() const;
double y() const;
@@ -115,6 +114,6 @@ public:
vpQuaternionVector inverse() const;
double magnitude() const;
void normalize();
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpRGBa.h b/modules/core/include/visp3/core/vpRGBa.h
index c0dee53..79cb146 100644
--- a/modules/core/include/visp3/core/vpRGBa.h
+++ b/modules/core/include/visp3/core/vpRGBa.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpRGBa_h
#define vpRGBa_h
@@ -56,9 +56,9 @@
Class that defines a RGBa 32 bits structure that is used to build color
images. RGBa stands for red green blue alpha color space.
- The alpha channel is normally used as an opacity channel. If a pixel has a value
- of 0 in its alpha channel, it is fully transparent, whereas a value of 255 in the alpha
- channel gives a fully opaque pixel.
+ The alpha channel is normally used as an opacity channel. If a pixel has a
+ value of 0 in its alpha channel, it is fully transparent, whereas a value of
+ 255 in the alpha channel gives a fully opaque pixel.
By default the alpha channel is set to vpRGBa::alpha_default.
\sa vpImage
@@ -66,45 +66,42 @@
class VISP_EXPORT vpRGBa
{
public:
- enum AlphaDefault {
- alpha_default = 255
- };
+ enum AlphaDefault { alpha_default = 255 };
/*!
Basic constructor.
-
+
Build a black value.
-
+
*/
inline vpRGBa() : R(0), G(0), B(0), A(vpRGBa::alpha_default) {}
-
+
/*!
Constructor.
-
+
Initialize the color with R, G, B, A values.
-
+
\param r : Red value.
\param g : Green value.
\param b : Blue value.
\param a : Additional value.
-
- */
- inline vpRGBa(const unsigned char &r, const unsigned char &g,
- const unsigned char &b, const unsigned char &a=0)
- : R(r), G(g), B(b), A(a) {}
+ */
+ inline vpRGBa(const unsigned char &r, const unsigned char &g, const unsigned char &b, const unsigned char &a = 0)
+ : R(r), G(g), B(b), A(a)
+ {
+ }
/*!
Constructor.
-
+
Initialize all the R, G, B, A components to \e v.
-
+
\param v : Value to set.
-
+
*/
inline vpRGBa(const unsigned char &v) : R(v), G(v), B(v), A(v) {}
-
/*!
Copy constructor.
*/
@@ -117,23 +114,21 @@ public:
G=v[1]
B=v[2]
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 becomes 16 that does break a lot of things arround image conversions
+ // We cannot add here the following destructor without changing the
+ // hypothesis that the size of this class is 4. With the destructor it
+ // becomes 16 that does break a lot of things arround image conversions
// virtual ~vpRGBa() {}; // Not to implement
- vpRGBa & operator=(const unsigned char &v) ;
- vpRGBa & operator=(const vpRGBa &v) ;
+ vpRGBa &operator=(const unsigned char &v);
+ vpRGBa &operator=(const vpRGBa &v);
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- vpRGBa & operator=(const vpRGBa &&v);
+ vpRGBa &operator=(const vpRGBa &&v);
#endif
- vpRGBa & operator=(const vpColVector &v) ;
+ vpRGBa &operator=(const vpColVector &v);
bool operator==(const vpRGBa &v);
bool operator!=(const vpRGBa &v);
@@ -147,17 +142,15 @@ public:
bool operator<(const vpRGBa &v) const;
bool operator>(const vpRGBa &v) const;
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRGBa& rgba);
-
- public:
- unsigned char R ; //!< Red component.
- unsigned char G ; //!< Green component.
- unsigned char B ; //!< Blue component.
- unsigned char A ; //!< Additionnal component.
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRGBa &rgba);
- friend VISP_EXPORT vpRGBa operator*(const double &x, const vpRGBa &rgb);
+public:
+ unsigned char R; //!< Red component.
+ unsigned char G; //!< Green component.
+ unsigned char B; //!< Blue component.
+ unsigned char A; //!< Additionnal component.
-} ;
+ friend VISP_EXPORT vpRGBa operator*(const double &x, const vpRGBa &rgb);
+};
#endif
-
diff --git a/modules/core/include/visp3/core/vpRansac.h b/modules/core/include/visp3/core/vpRansac.h
index d654d82..47931c8 100644
--- a/modules/core/include/visp3/core/vpRansac.h
+++ b/modules/core/include/visp3/core/vpRansac.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpRansac.h
@@ -45,18 +45,16 @@
#ifndef vpRANSAC_HH
#define vpRANSAC_HH
-
-
-#include <visp3/core/vpUniRand.h> // random number generation
-#include <visp3/core/vpDebug.h> // debug and trace
+#include <ctime>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h> // debug and trace
#include <visp3/core/vpMath.h>
-#include <ctime>
+#include <visp3/core/vpUniRand.h> // random number generation
/*!
\class vpRansac
\ingroup group_core_robust
-
- \brief This class is a generic implementation of the Ransac algorithm. It
+
+ \brief This class is a generic implementation of the Ransac algorithm. It
cannot be used alone.
RANSAC is described in \cite Fischler81 and \cite Hartley01a.
@@ -71,18 +69,12 @@
\sa vpHomography
*/
-template <class vpTransformation>
-class vpRansac
+template <class vpTransformation> class vpRansac
{
public:
- static bool ransac(unsigned int npts,
- vpColVector &x,
- unsigned int s, double t,
- vpColVector &model,
- vpColVector &inliers,
- int consensus = 1000,
- double not_used = 0.0,
- const int maxNbumbersOfTrials = 10000);
+ 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);
};
/*!
@@ -116,45 +108,39 @@ public:
*/
template <class vpTransformation>
-bool
-vpRansac<vpTransformation>::ransac(unsigned int npts, vpColVector &x,
- unsigned int s, double t,
- vpColVector &M,
- vpColVector &inliers,
- int consensus,
- double not_used,
- const int maxNbumbersOfTrials)
+bool vpRansac<vpTransformation>::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; */
/* if (s == 4) isplanar = true; */
/* else isplanar = false; */
(void)not_used;
- double eps = 1e-6 ;
- double p = 0.99; // Desired probability of choosing at least one sample
+ double eps = 1e-6;
+ double p = 0.99; // Desired probability of choosing at least one sample
// free from outliers
- int maxTrials = maxNbumbersOfTrials; // Maximum number of trials before we give up.
- int maxDataTrials = 1000; // Max number of attempts to select a non-degenerate
+ 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)
+ if (s < 4)
s = 4;
// Sentinel value allowing detection of solution failure.
- bool solutionFind = false ;
- vpColVector bestM ;
+ bool solutionFind = false;
+ vpColVector bestM;
int trialcount = 0;
- int bestscore = -1;
- double N = 1; // Dummy initialisation for number of trials.
+ int bestscore = -1;
+ double N = 1; // Dummy initialisation for number of trials.
- vpUniRand random((const long)time(NULL)) ;
- vpColVector bestinliers ;
- unsigned int *ind = new unsigned int [s] ;
+ vpUniRand random((const long)time(NULL));
+ vpColVector bestinliers;
+ unsigned int *ind = new unsigned int[s];
int numiter = 0;
int ninliers = 0;
double residual = 0.0;
- while(( N > trialcount) && (consensus > bestscore))
- {
+ while ((N > trialcount) && (consensus > bestscore)) {
// Select at random s datapoints to form a trial model, M.
// In selecting these points we have to check that they are not in
// a degenerate configuration.
@@ -162,94 +148,87 @@ vpRansac<vpTransformation>::ransac(unsigned int npts, vpColVector &x,
bool degenerate = true;
int count = 1;
- while ( degenerate == true)
- {
+ while (degenerate == true) {
// Generate s random indicies in the range 1..npts
- for (unsigned int i=0 ; i < s ; i++)
- ind[i] = (unsigned int)ceil(random()*npts) -1;
+ for (unsigned int i = 0; i < s; i++)
+ ind[i] = (unsigned int)ceil(random() * npts) - 1;
// Test that these points are not a degenerate configuration.
- degenerate = vpTransformation::degenerateConfiguration(x,ind) ;
+ degenerate = vpTransformation::degenerateConfiguration(x, ind);
// degenerate = feval(degenfn, x(:,ind));
// Safeguard against being stuck in this loop forever
count = count + 1;
- if (count > maxDataTrials) {
- delete [] ind;
+ if (count > maxDataTrials) {
+ delete[] ind;
vpERROR_TRACE("Unable to select a nondegenerate data set");
throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
- //return false; //Useless after a throw() function
+ // return false; //Useless after a throw() function
}
}
// Fit model to this random selection of data points.
- vpTransformation::computeTransformation(x,ind, M);
+ vpTransformation::computeTransformation(x, ind, M);
- vpColVector d ;
+ vpColVector d;
// Evaluate distances between points and model.
- vpTransformation::computeResidual(x, M, d) ;
+ vpTransformation::computeResidual(x, M, d);
// Find the indices of points that are inliers to this model.
residual = 0.0;
- ninliers =0 ;
- for (unsigned int i=0 ; i < npts ; i++)
- {
+ ninliers = 0;
+ for (unsigned int i = 0; i < npts; i++) {
double resid = fabs(d[i]);
- if (resid < t)
- {
- inliers[i] = 1 ;
- ninliers++ ;
+ if (resid < t) {
+ inliers[i] = 1;
+ ninliers++;
residual += fabs(d[i]);
- }
- else inliers[i] = 0;
+ } else
+ inliers[i] = 0;
}
- if (ninliers > bestscore) // Largest set of inliers so far...
+ if (ninliers > bestscore) // Largest set of inliers so far...
{
- bestscore = ninliers; // Record data for this model
+ bestscore = ninliers; // Record data for this model
bestinliers = inliers;
bestM = M;
- solutionFind = true ;
+ solutionFind = true;
// Update estimate of N, the number of trials to ensure we pick,
// with probability p, a data set with no outliers.
- double fracinliers = (double)ninliers / (double)npts;
+ double fracinliers = (double)ninliers / (double)npts;
- double pNoOutliers = 1 - pow(fracinliers,static_cast<int>(s));
+ double pNoOutliers = 1 - pow(fracinliers, static_cast<int>(s));
- pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
- pNoOutliers = vpMath::minimum(1-eps, pNoOutliers);// Avoid division by 0.
- N = (log(1-p)/log(pNoOutliers));
+ pNoOutliers = vpMath::maximum(eps, pNoOutliers); // Avoid division by -Inf
+ pNoOutliers = vpMath::minimum(1 - eps, pNoOutliers); // Avoid division by 0.
+ N = (log(1 - p) / log(pNoOutliers));
}
- trialcount = trialcount+1;
+ trialcount = trialcount + 1;
// Safeguard against being stuck in this loop forever
- if (trialcount > maxTrials)
- {
- vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
- break ;
+ if (trialcount > maxTrials) {
+ vpTRACE("ransac reached the maximum number of %d trials", maxTrials);
+ break;
}
numiter++;
}
- if (solutionFind==true) // We got a solution
+ if (solutionFind == true) // We got a solution
{
M = bestM;
inliers = bestinliers;
- }
- else
- {
+ } else {
vpTRACE("ransac was unable to find a useful solution");
M = 0;
}
- if(ninliers > 0)
+ if (ninliers > 0)
residual /= ninliers;
- delete [] ind;
+ delete[] ind;
return true;
}
-
#endif
diff --git a/modules/core/include/visp3/core/vpRect.h b/modules/core/include/visp3/core/vpRect.h
index 2ae800f..ec677a6 100644
--- a/modules/core/include/visp3/core/vpRect.h
+++ b/modules/core/include/visp3/core/vpRect.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,9 @@
*
*****************************************************************************/
-
#ifndef vpRect_h
#define vpRect_h
-
-
-
/*!
\class vpRect
\ingroup group_core_geometry
@@ -73,39 +70,37 @@
*/
-#include <vector>
#include <cassert>
+#include <vector>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImagePoint.h>
-
class VISP_EXPORT vpRect
{
public:
-
vpRect();
vpRect(double left, double top, double width, double height);
vpRect(const vpImagePoint &topLeft, double width, double height);
vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight);
- vpRect(const vpRect& r);
- vpRect(const std::vector<vpImagePoint> &ip);
+ vpRect(const vpRect &r);
+ explicit vpRect(const std::vector<vpImagePoint> &ip);
-
- vpRect &operator=(const vpRect& r);
+ vpRect &operator=(const vpRect &r);
/*!
- Returns the bottom coordinate of the rectangle.
+ Returns the bottom coordinate of the rectangle.
\sa getRight()
*/
inline double getBottom() const { return (this->top + this->height - 1.0); }
/*!
- Returns the bottom-right coordinate of the rectangle.
+ Returns the bottom-right coordinate of the rectangle.
\sa getTopLeft(), getBottom(), getRight()
*/
- inline vpImagePoint getBottomRight() const {
+ inline vpImagePoint getBottomRight() const
+ {
vpImagePoint bottomRight;
- bottomRight.set_u( getRight() );
- bottomRight.set_v( getBottom() );
+ bottomRight.set_u(getRight());
+ bottomRight.set_v(getBottom());
return bottomRight;
}
@@ -120,9 +115,10 @@ public:
\sa moveCenter()
*/
- inline void getCenter(double & x, double & y) const {
- x = this->left + this->width / 2.0 - 0.5;
- y = this->top + this->height / 2.0 - 0.5;
+ inline void getCenter(double &x, double &y) const
+ {
+ x = this->left + this->width / 2.0 - 0.5;
+ y = this->top + this->height / 2.0 - 0.5;
}
/*!
@@ -135,14 +131,15 @@ public:
\sa moveCenter()
*/
- inline vpImagePoint getCenter() const {
+ inline vpImagePoint getCenter() const
+ {
vpImagePoint center;
- center.set_u( this->left + this->width / 2.0 - 0.5 );
- center.set_v( this->top + this->height / 2.0 - 0.5 );
+ center.set_u(this->left + this->width / 2.0 - 0.5);
+ center.set_v(this->top + this->height / 2.0 - 0.5);
return center;
}
- /*!
+ /*!
Returns the height of the rectangle. The height includes both the
top and bottom edges, i.e. height = bottom - top + 1.
@@ -152,14 +149,14 @@ public:
inline double getHeight() const { return this->height; }
/*!
- Returns the left coordinate of the rectangle.
+ Returns the left coordinate of the rectangle.
\sa getTopLeft(), getRight()
*/
- inline double getLeft() const { return this->left; }
+ inline double getLeft() const { return this->left; }
/*!
- Returns the right coordinate of the rectangle.
+ Returns the right coordinate of the rectangle.
\sa getLeft()
*/
inline double getRight() const { return (this->left + this->width - 1.0); }
@@ -171,49 +168,50 @@ public:
inline double getSize() const { return (this->width * this->height); }
/*!
- Returns the top coordinate of the rectangle.
+ Returns the top coordinate of the rectangle.
\sa getTopLeft(), getBottom()
*/
- inline double getTop() const { return this->top; }
+ inline double getTop() const { return this->top; }
/*!
- Returns the top-left position of the rectangle.
+ Returns the top-left position of the rectangle.
\sa getBottomRight(), getTop(), getLeft()
*/
- inline vpImagePoint getTopLeft() const {
+ inline vpImagePoint getTopLeft() const
+ {
vpImagePoint topLeft;
- topLeft.set_u( this->left );
- topLeft.set_v( this->top );
- return topLeft;
+ topLeft.set_u(this->left);
+ topLeft.set_v(this->top);
+ return topLeft;
}
/*!
Returns the width of the rectangle. The width includes both the
left and right edges, i.e. width = right - left + 1.
-
+
\sa getHeight()
*/
- inline double getWidth() const { return this->width; }
+ inline double getWidth() const { return this->width; }
/*!
Returns true if the point belongs to the rectangle.
*/
- bool isInside( const vpImagePoint &ip ) const;
+ bool isInside(const vpImagePoint &ip) const;
bool operator==(const vpRect &r) const;
bool operator!=(const vpRect &r) const;
- vpRect& operator&=(const vpRect &r);
+ 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);
- void set(const vpRect& r);
+ void set(const vpRect &r);
void set(const std::vector<vpImagePoint> &ip);
/*!
@@ -231,8 +229,9 @@ public:
\sa setTopLeft()
*/
- inline void setBottomRight(const vpImagePoint &bottomRight) {
- this->height = bottomRight.get_v() - this->top + 1.0;
+ inline void setBottomRight(const vpImagePoint &bottomRight)
+ {
+ this->height = bottomRight.get_v() - this->top + 1.0;
this->width = bottomRight.get_u() - this->left + 1.0;
}
@@ -242,7 +241,8 @@ public:
\sa setWidth()
*/
- inline void setHeight(double h) {
+ inline void setHeight(double h)
+ {
assert(h > 0);
this->height = h;
}
@@ -260,17 +260,17 @@ public:
Sets the coordinates of the rectangle's top left corner to
(left, top), and its size to (width, height).
- \param l : horizontal position of the rectangle upper/left corner position.
- \param t : vertical position of the rectangle upper/left corner position.
- \param w : rectangle width.
- \param h : rectangle height.
+ \param l : horizontal position of the rectangle upper/left corner
+ position. \param t : vertical position of the rectangle upper/left corner
+ position. \param w : rectangle width. \param h : rectangle height.
*/
- inline void setRect(double l, double t, double w, double h) {
- this->left = l;
- this->top = t;
- this->width = w;
- this->height = h;
+ inline void setRect(double l, double t, double w, double h)
+ {
+ this->left = l;
+ this->top = t;
+ this->width = w;
+ this->height = h;
}
/*!
@@ -298,9 +298,10 @@ public:
\sa setBottomRight()
*/
- inline void setTopLeft(const vpImagePoint &topLeft) {
- this->left = topLeft.get_u();
- this->top = topLeft.get_v();
+ inline void setTopLeft(const vpImagePoint &topLeft)
+ {
+ this->left = topLeft.get_u();
+ this->top = topLeft.get_v();
}
/*!
@@ -309,31 +310,34 @@ public:
\sa setHeight()
*/
- inline void setWidth(double w) {
+ inline void setWidth(double w)
+ {
assert(w > 0);
this->width = w;
}
-
+
/*!
Sets the center point of the rectangle to (\e x, \e y), leaving
the size unchanged.
\sa getCenter()
*/
- inline void moveCenter(double x, double y) {
- this->left = x - this->width/2 + 0.5;
- this->top = y - this->height/2 + 0.5;
+ inline void moveCenter(double x, double y)
+ {
+ this->left = x - this->width / 2 + 0.5;
+ this->top = y - this->height / 2 + 0.5;
}
-
+
/*!
Center the rectangle to the image point given as parameter, leaving
the size unchanged.
\sa getCenter()
*/
- inline void moveCenter(const vpImagePoint ¢er) {
- this->left = center.get_u() - this->width/2 + 0.5;
- this->top = center.get_v() - this->height/2 + 0.5;
+ inline void moveCenter(const vpImagePoint ¢er)
+ {
+ this->left = center.get_u() - this->width / 2 + 0.5;
+ this->top = center.get_v() - this->height / 2 + 0.5;
}
private:
diff --git a/modules/core/include/visp3/core/vpRequest.h b/modules/core/include/visp3/core/vpRequest.h
index 26d9cb1..b93698f 100644
--- a/modules/core/include/visp3/core/vpRequest.h
+++ b/modules/core/include/visp3/core/vpRequest.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,7 +49,7 @@
/*!
\class vpRequest
-
+
\ingroup group_core_network
\brief This the request that will transit on the network
@@ -57,9 +58,9 @@
First parameter : Height of the image.
Second parameter : Width of the image.
Thirs parameter : Bitmap of the image (not compress).
-
+
Here is the header of the vpRequestImage class.
-
+
\code
#ifndef vpRequestImage_H
#define vpRequestImage_H
@@ -71,21 +72,21 @@ class vpRequestImage : public vpRequest
{
private:
vpImage<unsigned char> *I;
-
+
public:
vpRequestImage();
vpRequestImage(vpImage<unsigned char> *);
~vpRequestImage();
-
+
virtual void encode();
virtual void decode();
};
#endif
\endcode
-
+
Here is the definition of the vpRequestImage class.
-
+
\code
#include <vpRequestImage.h>
@@ -101,28 +102,28 @@ vpRequestImage::vpRequestImage(vpImage<unsigned char> *Im){
vpRequestImage::~vpRequestImage(){}
void vpRequestImage::encode(){
- clear();
-
+ clear();
+
unsigned int h = I->getHeight();
- unsigned int w = I->getWidth();
+ unsigned int w = I->getWidth();
addParameterObject(&h);
addParameterObject(&w);
addParameterObject(I->bitmap,h*w*sizeof(unsigned char));
}
-
+
void vpRequestImage::decode(){
if(listOfParams.size() == 3){
unsigned int w, h;
memcpy((void*)&h, (void*)listOfParams[0].c_str(), sizeof(unsigned int));
memcpy((void*)&w, (void*)listOfParams[1].c_str(), sizeof(unsigned int));
-
+
I->resize(h,w);
memcpy((void*)I->bitmap,(void*)listOfParams[2].c_str(),w*h*sizeof(unsigned char));
}
}
\endcode
-
+
\sa vpClient
\sa vpServer
\sa vpNetwork
@@ -130,107 +131,103 @@ void vpRequestImage::decode(){
class VISP_EXPORT vpRequest
{
protected:
- std::string request_id;
- std::vector<std::string> listOfParams;
-
+ std::string request_id;
+ std::vector<std::string> listOfParams;
+
public:
- vpRequest();
- virtual ~vpRequest();
-
- void addParameter(char *params);
- void addParameter(std::string ¶ms);
- void addParameter(std::vector<std::string> &listOfparams);
- template<typename T>
- void addParameterObject(T * params, const int &sizeOfObject = sizeof(T));
-
+ vpRequest();
+ virtual ~vpRequest();
+
+ void addParameter(char *params);
+ void addParameter(std::string ¶ms);
+ void addParameter(std::vector<std::string> &listOfparams);
+ template <typename T> void addParameterObject(T *params, const int &sizeOfObject = sizeof(T));
+
/*!
Decode the parameters of the request (Funtion that has to be redifined).
-
+
\sa vpRequest::encode()
*/
- virtual void decode() = 0;
-
+ virtual void decode() = 0;
+
/*!
Clear the parameters of the request.
*/
- void clear(){ listOfParams.clear(); }
-
+ void clear() { listOfParams.clear(); }
+
/*!
Encode the parameters of the request (Funtion that has to be redifined).
-
+
\sa vpRequest::decode()
*/
- virtual void encode() = 0;
-
+ virtual void encode() = 0;
+
/*!
Accessor on the parameters.
-
+
\return Parameter at the index i.
- */
- inline std::string& operator[](const unsigned int &i) { return listOfParams[i];}
+ */
+ 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.
-
+
\sa vpRequest::setId()
-
+
\return ID of the request.
*/
- std::string getId(){ return request_id; }
-
+ std::string getId() { return request_id; }
+
/*!
Change the ID of the request.
-
+
\sa vpRequest::getId()
-
+
\param id : new ID.
*/
- void setId(const char *id){ request_id = id; }
-
+ void setId(const char *id) { request_id = id; }
+
/*!
Get the number of parameters.
-
+
\return Number of parameters.
*/
- unsigned int size(){ return (unsigned int)listOfParams.size(); }
+ unsigned int size() { return (unsigned int)listOfParams.size(); }
};
-
//######## Definition of Template Functions ########
//# #
//##################################################
-
/*!
Add an object as parameter of the request.
-
- \warning Only simple object can be sent unless you know its size.
- Sending object containing pointers, virtual methods, etc, won't probably work.
- Unless the size is well defined...
-
+
+ \warning Only simple object can be sent unless you know its size.
+ Sending object containing pointers, virtual methods, etc, won't probably
+ work. Unless the size is well defined...
+
\sa vpRequest::addParameter()
\param params : Object to add.
\param sizeOfObject : Size of the object.
*/
-template<typename T>
-void vpRequest::addParameterObject(T * params, const int &sizeOfObject)
+template <typename T> void vpRequest::addParameterObject(T *params, const int &sizeOfObject)
{
- if(sizeOfObject != 0){
- char *tempS = new char [sizeOfObject];
- memcpy((void*)tempS, (void*)params, sizeOfObject);
+ if (sizeOfObject != 0) {
+ char *tempS = new char[sizeOfObject];
+ memcpy((void *)tempS, (void *)params, sizeOfObject);
std::string returnVal(tempS, (size_t)sizeOfObject);
-
+
listOfParams.push_back(returnVal);
- delete [] tempS;
+ delete[] tempS;
}
}
diff --git a/modules/core/include/visp3/core/vpRobust.h b/modules/core/include/visp3/core/vpRobust.h
index 1e38a5d..b2624e9 100644
--- a/modules/core/include/visp3/core/vpRobust.h
+++ b/modules/core/include/visp3/core/vpRobust.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,16 +41,13 @@
\file vpRobust.h
*/
-
-
#ifndef CROBUST_HH
#define CROBUST_HH
-#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
-
/*!
\class vpRobust
\ingroup group_core_robust
@@ -59,27 +57,20 @@
*/
class VISP_EXPORT vpRobust
{
-
+
public:
//! Enumeration of influence functions
- typedef enum
- {
- TUKEY,
- CAUCHY,
- // MCLURE,
- HUBER
- } vpRobustEstimatorType;
-
-private:
+ typedef enum { TUKEY, CAUCHY, HUBER } vpRobustEstimatorType;
- //!Normalized residue
- vpColVector normres;
- //!Sorted normalized Residues
+private:
+ //! Normalized residue
+ vpColVector normres;
+ //! Sorted normalized Residues
vpColVector sorted_normres;
- //!Sorted residues
+ //! Sorted residues
vpColVector sorted_residues;
- //!Noise threshold
+ //! Noise threshold
double NoiseThreshold;
//!
double sig_prev;
@@ -91,81 +82,66 @@ private:
unsigned int size;
public:
-
- //!Default Constructor
- vpRobust(unsigned int n_data);
+ //! Default Constructor
+ explicit vpRobust(unsigned int n_data);
vpRobust();
vpRobust(const vpRobust &other);
- //!Destructor
+ //! Destructor
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);
+ vpRobust &operator=(const vpRobust &other);
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- vpRobust & operator=(const vpRobust &&other);
+ vpRobust &operator=(const vpRobust &&other);
#endif
- //!Resize containers for sort methods
+ //! Resize containers for sort methods
void resize(unsigned int n_data);
- //! Set iteration
- void setIteration(const unsigned int iter){it=iter;}
-
+ //! Set iteration
+ void setIteration(const unsigned int iter) { it = iter; }
+
/*!
Set maximal noise threshold.
\param noise_threshold : Maximal noise threshold.
*/
- inline void setThreshold(const double noise_threshold) {
- NoiseThreshold=noise_threshold;
- }
+ inline void setThreshold(const double noise_threshold) { NoiseThreshold = noise_threshold; }
//! Simult Mestimator
vpColVector simultMEstimator(vpColVector &residues);
-//public :
-//double residualMedian ;
-//double normalizedResidualMedian ;
-// private:
-// double median(const vpColVector &x);
-// double median(const vpColVector &x, vpColVector &weights);
-
- private:
- //!Compute normalized median
- double computeNormalizedMedian(vpColVector &all_normres,
- const vpColVector &residues,
- const vpColVector &all_residues,
- const vpColVector &weights
- );
+ // public :
+ // double residualMedian ;
+ // double normalizedResidualMedian ;
+ // private:
+ // double median(const vpColVector &x);
+ // double median(const vpColVector &x, vpColVector &weights);
+private:
+ //! Compute normalized median
+ double computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues, const vpColVector &all_residues,
+ const vpColVector &weights);
//! Calculate various scale estimates
- double scale(vpRobustEstimatorType method, vpColVector &x);
double simultscale(vpColVector &x);
-
//---------------------------------
// Partial derivative of loss function with respect to the residue
//---------------------------------
/** @name PsiFunctions */
//@{
- //! Tuckey influence function
+ //! Tuckey influence function
void psiTukey(double sigma, vpColVector &x, vpColVector &w);
- //! Caucht influence function
+ //! Caucht influence function
void psiCauchy(double sigma, vpColVector &x, vpColVector &w);
- //! McLure influence function
- void psiMcLure(double sigma, vpColVector &x, vpColVector &w);
- //! Huber influence function
+ //! Huber influence function
void psiHuber(double sigma, vpColVector &x, vpColVector &w);
//@}
@@ -175,8 +151,8 @@ public:
//---------------------------------
// Constrained Partial derivative of loss function with respect to the scale
- //---------------------------------
- /** @name Constrained Chi Functions */
+ //---------------------------------
+ /** @name Constrained Chi Functions */
//@{
//! Constrained Chi Function
double constrainedChi(vpRobustEstimatorType method, double x);
@@ -184,10 +160,10 @@ public:
double constrainedChiTukey(double x);
//! Constrained Chi Cauchy Function
double constrainedChiCauchy(double x);
- //! Constrained Chi Huber Function
+ //! Constrained Chi Huber Function
double constrainedChiHuber(double x);
- //@}
-
+//@}
+
#if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
//---------------------------------
// Mathematic functions used to calculate the Expectation
@@ -199,13 +175,18 @@ public:
void gser(double *gamser, double a, double x, double *gln);
void gcf(double *gammcf, double a, double x, double *gln);
double gammln(double xx);
- //@}
+//@}
#endif
-
+
/** @name Sort function */
//@{
//! Swap two value
- void exch(double &A, double &B){swap = A; A = B; B = swap;}
+ void exch(double &A, double &B)
+ {
+ swap = A;
+ A = B;
+ B = swap;
+ }
//! Sort function using partition method
int partition(vpColVector &a, int l, int r);
//! Sort the vector and select a value in the sorted vector
diff --git a/modules/core/include/visp3/core/vpRotationMatrix.h b/modules/core/include/visp3/core/vpRotationMatrix.h
index 75995fe..82d7f75 100644
--- a/modules/core/include/visp3/core/vpRotationMatrix.h
+++ b/modules/core/include/visp3/core/vpRotationMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpROTATIONMATRIX_H
#define vpROTATIONMATRIX_H
@@ -44,22 +44,23 @@
\brief Class that consider the particular case of rotation matrix
*/
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpArray2D.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpQuaternionVector.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpRzyxVector.h>
#include <visp3/core/vpRzyzVector.h>
#include <visp3/core/vpThetaUVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpQuaternionVector.h>
-#include <visp3/core/vpPoseVector.h>
/*!
\class vpRotationMatrix
\ingroup group_core_transformations
- \brief Implementation of a rotation matrix and operations on such kind of matrices.
+ \brief Implementation of a rotation matrix and operations on such kind of
+ matrices.
The vpRotationMatrix considers the particular case of
a rotation matrix.
@@ -72,26 +73,26 @@ class VISP_EXPORT vpRotationMatrix : public vpArray2D<double>
public:
vpRotationMatrix();
vpRotationMatrix(const vpRotationMatrix &R);
- vpRotationMatrix(const vpHomogeneousMatrix &M);
- vpRotationMatrix(const vpThetaUVector &r);
- vpRotationMatrix(const vpPoseVector &p);
- vpRotationMatrix(const vpRzyzVector &r);
- vpRotationMatrix(const vpRxyzVector &r);
- vpRotationMatrix(const vpRzyxVector &r);
- vpRotationMatrix(const vpQuaternionVector& q);
- vpRotationMatrix(const double tux, const double tuy, const double tuz);
+ explicit vpRotationMatrix(const vpHomogeneousMatrix &M);
+ explicit vpRotationMatrix(const vpThetaUVector &r);
+ explicit vpRotationMatrix(const vpPoseVector &p);
+ explicit vpRotationMatrix(const vpRzyzVector &r);
+ explicit vpRotationMatrix(const vpRxyzVector &r);
+ explicit vpRotationMatrix(const vpRzyxVector &r);
+ explicit vpRotationMatrix(const vpQuaternionVector &q);
+ vpRotationMatrix(const double tux, const double tuy, const double tuz);
/*!
Destructor.
*/
- virtual ~vpRotationMatrix() {};
+ virtual ~vpRotationMatrix(){};
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M);
- vpRotationMatrix buildFrom(const vpThetaUVector &v) ;
+ vpRotationMatrix buildFrom(const vpThetaUVector &v);
vpRotationMatrix buildFrom(const vpPoseVector &p);
vpRotationMatrix buildFrom(const vpRzyzVector &v);
vpRotationMatrix buildFrom(const vpRxyzVector &v);
vpRotationMatrix buildFrom(const vpRzyxVector &v);
- vpRotationMatrix buildFrom(const vpQuaternionVector& q);
+ vpRotationMatrix buildFrom(const vpQuaternionVector &q);
vpRotationMatrix buildFrom(const double tux, const double tuy, const double tuz);
void eye();
@@ -102,7 +103,7 @@ public:
vpRotationMatrix inverse() const;
void inverse(vpRotationMatrix &R) const;
- bool isARotationMatrix() const ;
+ bool isARotationMatrix() const;
// copy operator from vpRotationMatrix
vpRotationMatrix &operator=(const vpRotationMatrix &R);
@@ -119,15 +120,14 @@ public:
vpRotationMatrix operator*(const double x) const;
vpRotationMatrix &operator*=(const double x);
- void printVector() ;
+ void printVector();
/*!
This function is not applicable to a rotation matrix that is always a
3-by-3 matrix.
\exception vpException::fatalError When this function is called.
*/
- void resize(const unsigned int nrows, const unsigned int ncols,
- const bool flagNullify = true)
+ void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify = true)
{
(void)nrows;
(void)ncols;
@@ -137,7 +137,7 @@ public:
// transpose
vpRotationMatrix t() const;
-
+
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
@name Deprecated functions
@@ -147,12 +147,12 @@ public:
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
+ vp_deprecated void init(){};
/*!
\deprecated You should rather use eye().
*/
vp_deprecated void setIdentity();
- //@}
+//@}
#endif
private:
@@ -162,6 +162,6 @@ private:
#ifndef DOXYGEN_SHOULD_SKIP_THIS
VISP_EXPORT
#endif
-vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R) ;
+vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R);
#endif
diff --git a/modules/core/include/visp3/core/vpRotationVector.h b/modules/core/include/visp3/core/vpRotationVector.h
index 42e4333..748edc1 100644
--- a/modules/core/include/visp3/core/vpRotationVector.h
+++ b/modules/core/include/visp3/core/vpRotationVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -64,23 +65,28 @@ class vpColVector;
(cannot be used as is !) consisting in three or four angles.
The vpRotationVector class is derived from vpArray2D<double>.
- The vpRotationVector class is also the base class of specific rotations vectors such as
- vpThetaUVector, vpRxyzVector, vpRzyxVector, vpRzyzVector and vpQuaternionVector.
+ The vpRotationVector class is also the base class of specific rotations
+vectors such as vpThetaUVector, vpRxyzVector, vpRzyxVector, vpRzyzVector and
+vpQuaternionVector.
- The code below shows how this class can be used to manipulate a vpRxyzVector.
+ The code below shows how this class can be used to manipulate a
+vpRxyzVector.
\code
#include <iostream>
-#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpRxyzVector.h>
-int main()
+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;
double rx = r[0]; // Get the value of the angle around x axis
@@ -92,29 +98,24 @@ int main()
*/
class VISP_EXPORT vpRotationVector : public vpArray2D<double>
-{
+{
public:
//! Constructor that constructs a 0-size rotation vector.
- vpRotationVector()
- : vpArray2D<double>()
- {}
+ vpRotationVector() : vpArray2D<double>() {}
- //! Constructor that constructs a vector of size n and initialize all values to zero.
- vpRotationVector(const unsigned int n)
- : vpArray2D<double>(n, 1)
- {}
+ //! Constructor that constructs a vector of size n and initialize all values
+ //! to zero.
+ explicit vpRotationVector(const unsigned int n) : vpArray2D<double>(n, 1) {}
/*!
Copy operator.
*/
- vpRotationVector(const vpRotationVector &v)
- : vpArray2D<double>(v)
- {}
+ vpRotationVector(const vpRotationVector &v) : vpArray2D<double>(v) {}
/*!
Destructor.
*/
- virtual ~vpRotationVector() {};
+ virtual ~vpRotationVector(){};
/** @name Inherited functionalities from vpRotationVector */
//@{
@@ -123,12 +124,12 @@ public:
Operator that allows to set the value of an element of the rotation
vector: r[i] = value
*/
- inline double &operator [](unsigned int i) { return *(data + i); }
+ inline double &operator[](unsigned int i) { return *(data + i); }
/*!
Operator that allows to get the value of an element of the rotation
vector: value = r[i]
*/
- inline const double &operator [](unsigned int i) const { return *(data+i); }
+ inline const double &operator[](unsigned int i) const { return *(data + i); }
/*!
Affectation of two vectors.
@@ -136,9 +137,8 @@ public:
vpRotationVector &operator=(const vpRotationVector &v)
{
resize(v.size(), 1);
- for (unsigned int i=0; i<v.size(); i++)
- {
- data[i] = v.data[i] ;
+ for (unsigned int i = 0; i < v.size(); i++) {
+ data[i] = v.data[i];
}
return *this;
}
@@ -150,12 +150,11 @@ public:
vpRowVector t() const;
//@}
-} ;
+};
#ifndef DOXYGEN_SHOULD_SKIP_THIS
VISP_EXPORT
#endif
-vpColVector operator*(const double &x, const vpRotationVector &v) ;
+vpColVector operator*(const double &x, const vpRotationVector &v);
#endif
-
diff --git a/modules/core/include/visp3/core/vpRowVector.h b/modules/core/include/visp3/core/vpRowVector.h
index bed2155..4b51edd 100644
--- a/modules/core/include/visp3/core/vpRowVector.h
+++ b/modules/core/include/visp3/core/vpRowVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,8 +43,8 @@
#include <visp3/core/vpArray2D.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
class vpMatrix;
class vpColVector;
@@ -62,8 +63,9 @@ class vpColVector;
\brief Implementation of row vector and the associated operations.
- This class provides a data structure for a row vector that contains values of double.
- It contains also some functions to achieve a set of operations on these vectors.
+ This class provides a data structure for a row vector that contains values
+ of double. It contains also some functions to achieve a set of operations on
+ these vectors.
The vpRowVector class is derived from vpArray2D<double>.
*/
@@ -71,14 +73,16 @@ class VISP_EXPORT vpRowVector : public vpArray2D<double>
{
public:
//! Basic constructor that creates an empty 0-size row vector.
- vpRowVector() : vpArray2D<double>() {};
- //! Construct a row vector of size n. All the elements are initialized to zero.
- vpRowVector(unsigned int n) : vpArray2D<double>(1, n){};
+ vpRowVector() : vpArray2D<double>() {}
+ //! Construct a row vector of size n. All the elements are initialized to
+ //! zero.
+ explicit vpRowVector(unsigned int n) : vpArray2D<double>(1, n) {}
//! Construct a row vector of size n. Each element is set to \e val.
- vpRowVector(unsigned int n, double val) : vpArray2D<double>(1, n, val){};
- //! Copy constructor that allows to construct a row vector from an other one.
- vpRowVector(const vpRowVector &v) : vpArray2D<double>(v) {};
- vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) ;
+ vpRowVector(unsigned int n, double val) : vpArray2D<double>(1, n, val) {}
+ //! Copy constructor that allows to construct a row vector from an other
+ //! one.
+ vpRowVector(const vpRowVector &v) : vpArray2D<double>(v) {}
+ vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols);
vpRowVector(const vpMatrix &M);
vpRowVector(const vpMatrix &M, unsigned int i);
vpRowVector(const std::vector<double> &v);
@@ -86,7 +90,7 @@ public:
/*!
Destructor.
*/
- virtual ~vpRowVector() {};
+ virtual ~vpRowVector() {}
/*!
Removes all elements from the vector (which are destroyed),
@@ -94,27 +98,28 @@ public:
*/
void clear()
{
- if (data != NULL ) {
+ if (data != NULL) {
free(data);
- data=NULL;
+ data = NULL;
}
- if (rowPtrs!=NULL) {
+ if (rowPtrs != NULL) {
free(rowPtrs);
- rowPtrs=NULL ;
+ rowPtrs = NULL;
}
rowNum = colNum = dsize = 0;
}
- std::ostream & cppPrint(std::ostream & os, const std::string &matrixName="A", bool octet = false) const;
- std::ostream & csvPrint(std::ostream & os) const;
+ std::ostream &cppPrint(std::ostream &os, const std::string &matrixName = "A", bool octet = false) const;
+ std::ostream &csvPrint(std::ostream &os) const;
/*!
Convert a column vector containing angles in degrees into radians.
\sa rad2deg()
*/
- inline void deg2rad() {
- double d2r = M_PI/180.0;
+ inline void deg2rad()
+ {
+ double d2r = M_PI / 180.0;
(*this) *= d2r;
}
@@ -122,9 +127,10 @@ public:
double euclideanNorm() const;
/*!
Extract a sub-row vector from a row vector.
- \param c : Index of the column corresponding to the first element of the vector to extract.
- \param rowsize : Size of the vector to extract.
- \exception vpException::fatalError If the vector to extract is not contained in the original one.
+ \param c : Index of the column corresponding to the first element of the
+ vector to extract. \param rowsize : Size of the vector to extract.
+ \exception vpException::fatalError If the vector to extract is not
+ contained in the original one.
\code
vpRowVector r1;
@@ -137,9 +143,10 @@ public:
*/
vpRowVector extract(unsigned int c, unsigned int rowsize) const
{
- if (c >= colNum || c+rowsize > colNum) {
+ if (c >= colNum || c + rowsize > colNum) {
throw(vpException(vpException::fatalError,
- "Cannot extract a (1x%d) row vector from a (1x%d) row vector starting at index %d",
+ "Cannot extract a (1x%d) row vector from a (1x%d) "
+ "row vector starting at index %d",
rowsize, colNum, c));
}
@@ -149,16 +156,16 @@ public:
void init(const vpRowVector &v, unsigned int c, unsigned int ncols);
void insert(unsigned int i, const vpRowVector &v);
- std::ostream & maplePrint(std::ostream & os) const;
- std::ostream & matlabPrint(std::ostream & os) const;
+ std::ostream &maplePrint(std::ostream &os) const;
+ std::ostream &matlabPrint(std::ostream &os) const;
- vpRowVector &normalize() ;
- vpRowVector &normalize(vpRowVector &x) const ;
+ vpRowVector &normalize();
+ vpRowVector &normalize(vpRowVector &x) const;
//! Operator that allows to set a value of an element \f$v_i\f$: v[i] = x
- inline double &operator[](unsigned int n) { return *(data+n); }
+ 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);
@@ -167,7 +174,7 @@ public:
vpRowVector &operator=(const std::vector<float> &v);
vpRowVector &operator=(const double x);
- double operator*(const vpColVector &x) const;
+ double operator*(const vpColVector &x) const;
vpRowVector operator*(const vpMatrix &M) const;
vpRowVector operator*(const double x) const;
vpRowVector &operator*=(double x);
@@ -184,19 +191,20 @@ public:
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()
*/
- inline void rad2deg() {
- double r2d = 180.0/M_PI;
+ inline void rad2deg()
+ {
+ double r2d = 180.0 / M_PI;
(*this) *= r2d;
}
- void reshape(vpMatrix & M,const unsigned int &nrows,const unsigned int &ncols);
- vpMatrix reshape(const unsigned int &nrows,const unsigned int &ncols);
-
+ void reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols);
+ vpMatrix reshape(const unsigned int &nrows, const unsigned int &ncols);
+
/*! Modify the size of the row vector.
\param i : Size of the vector. This value corresponds to the vector number
of columns.
@@ -221,10 +229,11 @@ public:
{
if (nrows != 1)
throw(vpException(vpException::fatalError,
- "Cannot resize a row vector to a (%dx%d) dimension vector that has more than one row",
+ "Cannot resize a row vector to a (%dx%d) dimension "
+ "vector that has more than one row",
nrows, ncols));
vpArray2D<double>::resize(nrows, ncols, flagNullify);
- };
+ }
void stack(const double &d);
void stack(const vpRowVector &v);
@@ -235,11 +244,11 @@ public:
vpColVector transpose() const;
void transpose(vpColVector &v) const;
- static double mean(const vpRowVector &v) ;
- static double median(const vpRowVector &v) ;
+ 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 double stdev(const vpRowVector &v, const bool useBesselCorrection = false);
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
@@ -250,29 +259,33 @@ public:
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
+ vp_deprecated void init() {}
/*!
\deprecated You should rather use stack(const vpRowVector &)
*/
- vp_deprecated void stackMatrices(const vpRowVector &r) { stack(r); };
+ vp_deprecated void stackMatrices(const vpRowVector &r) { stack(r); }
/*!
- \deprecated You should rather use stack(const vpRowVector &A, const vpRowVector &B)
+ \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)
+ \deprecated You should rather use stack(const vpRowVector &A, const
+ vpRowVector &B, vpRowVector &C)
*/
- vp_deprecated static void stackMatrices(const vpRowVector &A, const vpRowVector &B, vpRowVector &C) { stack(A, B, C); };
+ vp_deprecated static void stackMatrices(const vpRowVector &A, const vpRowVector &B, vpRowVector &C)
+ {
+ stack(A, B, C);
+ }
/*!
\deprecated You should rather use eye()
*/
- vp_deprecated void setIdentity(const double & val=1.0) ;
- //@}
+ vp_deprecated void setIdentity(const double &val = 1.0);
+//@}
#endif
-
};
-VISP_EXPORT vpRowVector operator*(const double &x, const vpRowVector &v) ;
+VISP_EXPORT vpRowVector operator*(const double &x, const vpRowVector &v);
#endif
diff --git a/modules/core/include/visp3/core/vpRxyzVector.h b/modules/core/include/visp3/core/vpRxyzVector.h
index be91eb9..30966d1 100644
--- a/modules/core/include/visp3/core/vpRxyzVector.h
+++ b/modules/core/include/visp3/core/vpRxyzVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
#ifndef vpRxyzVECTOR_H
#define vpRxyzVECTOR_H
@@ -50,9 +50,8 @@
Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
*/
-
-#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRotationVector.h>
class vpRotationVector;
class vpRotationMatrix;
@@ -67,10 +66,12 @@ class vpThetaUVector;
minimal representation.
Class that consider the case of the Euler
- \f$(\varphi,\theta,\psi)\f$ angle using the x-y-z convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
- rotation angles around the \f$x\f$, \f$y\f$ and \f$z\f$ axis.
+ \f$(\varphi,\theta,\psi)\f$ angle using the x-y-z convention, where
+\f$(\varphi,\theta,\psi)\f$ are respectively the rotation angles around the
+\f$x\f$, \f$y\f$ and \f$z\f$ axis.
- \f[R_{xyz}(\varphi,\theta,\psi) = R_x(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
+ \f[R_{xyz}(\varphi,\theta,\psi) = R_x(\varphi) \; R_y(\theta) \;
+R_z(\psi)\f]
with
@@ -102,9 +103,10 @@ class vpThetaUVector;
R_{xyz}(\varphi,\theta,\psi) = \left(
\begin{array}{ccc}
\cos\theta \cos\psi & -\cos\theta \sin\psi & \sin\theta \\
- \sin\varphi \sin\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \sin\theta \sin\psi +\cos\varphi\cos\psi & -\sin\varphi \cos\theta \\
- -\cos\varphi \sin\theta \cos\psi + \sin\varphi\sin\psi & \cos\varphi \sin\theta \sin\psi +\sin\varphi\cos\psi & \cos\varphi \cos\theta
- \end{array}
+ \sin\varphi \sin\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi
+\sin\theta \sin\psi +\cos\varphi\cos\psi & -\sin\varphi \cos\theta \\
+ -\cos\varphi \sin\theta \cos\psi + \sin\varphi\sin\psi & \cos\varphi
+\sin\theta \sin\psi +\sin\varphi\cos\psi & \cos\varphi \cos\theta \end{array}
\right)
\f]
@@ -126,7 +128,7 @@ int main()
vpRxyzVector rxyz;
// Initialise the Euler angles
- rxyz[0] = vpMath::rad( 45.f); // phi angle in rad around x axis
+ rxyz[0] = vpMath::rad( 45.f); // phi angle in rad around x axis
rxyz[1] = vpMath::rad(-30.f); // theta angle in rad around y axis
rxyz[2] = vpMath::rad( 90.f); // psi angle in rad around z axis
@@ -138,12 +140,12 @@ int main()
// Print the extracted Euler angles. Values are the same than the
// one used for initialization
- std::cout << rxyz;
+ std::cout << rxyz;
// Since the rotation vector is 3 values column vector, the
// transpose operation produce a row vector.
vpRowVector rxyz_t = rxyz.t();
-
+
// Print the transpose row vector
std::cout << rxyz_t << std::endl;
}
@@ -153,31 +155,31 @@ int main()
class VISP_EXPORT vpRxyzVector : public vpRotationVector
{
- public:
+public:
vpRxyzVector();
vpRxyzVector(const vpRxyzVector &rxyz);
vpRxyzVector(const double phi, const double theta, const double psi);
// initialize a Rxyz vector from a rotation matrix
- vpRxyzVector(const vpRotationMatrix& R) ;
+ explicit vpRxyzVector(const vpRotationMatrix &R);
// initialize a Rxyz vector from a ThetaU vector
- vpRxyzVector(const vpThetaUVector& tu) ;
- vpRxyzVector(const vpColVector &rxyz);
+ explicit vpRxyzVector(const vpThetaUVector &tu);
+ explicit vpRxyzVector(const vpColVector &rxyz);
//! Destructor.
- virtual ~vpRxyzVector() {};
+ virtual ~vpRxyzVector(){};
// convert a rotation matrix into Rxyz vector
- vpRxyzVector buildFrom(const vpRotationMatrix& R) ;
+ vpRxyzVector buildFrom(const vpRotationMatrix &R);
// convert a ThetaU vector into a Rxyz vector
- vpRxyzVector buildFrom(const vpThetaUVector& tu) ;
+ vpRxyzVector buildFrom(const vpThetaUVector &tu);
void buildFrom(const double phi, const double theta, const double psi);
vpRxyzVector &operator=(const vpColVector &rxyz);
- vpRxyzVector &operator=(double x) ;
-} ;
+ vpRxyzVector &operator=(double x);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpRzyxVector.h b/modules/core/include/visp3/core/vpRzyxVector.h
index ccdf0e1..68afcb8 100644
--- a/modules/core/include/visp3/core/vpRzyxVector.h
+++ b/modules/core/include/visp3/core/vpRzyxVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -64,10 +65,12 @@ class vpThetaUVector;
minimal representation.
Class that consider the case of the Euler
- \f$(\varphi,\theta,\psi)\f$ angle using the z-y-x convention, where \f$(\varphi,\theta,\psi)\f$ are respectively the
- rotation angles around the \f$z\f$, \f$y\f$ and \f$x\f$ axis.
+ \f$(\varphi,\theta,\psi)\f$ angle using the z-y-x convention, where
+\f$(\varphi,\theta,\psi)\f$ are respectively the rotation angles around the
+\f$z\f$, \f$y\f$ and \f$x\f$ axis.
- \f[R_{zyx}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_x(\psi)\f]
+ \f[R_{zyx}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \;
+R_x(\psi)\f]
with
@@ -92,7 +95,7 @@ class vpThetaUVector;
0 &\cos \psi & -\sin\psi \\
0 &\sin \psi & \cos\psi \\
\end{array}
- \right)
+ \right)
\f]
The rotation matrix corresponding to the z-y-x convention is given by:
@@ -100,8 +103,11 @@ class vpThetaUVector;
\f[
R_{zyx}(\varphi,\theta,\psi) = \left(
\begin{array}{ccc}
- \cos\varphi \cos\theta & -\sin\varphi \cos\psi + \cos\varphi\sin\theta\sin\psi & \sin\varphi \sin\psi +\cos\varphi\sin\theta\cos\psi \\
- \sin\varphi \cos\theta & \cos\varphi\cos\psi + \sin\varphi\sin\theta \sin\psi & -\cos\varphi \sin\psi +\sin\varphi\sin\theta\cos\psi \\
+ \cos\varphi \cos\theta & -\sin\varphi \cos\psi +
+\cos\varphi\sin\theta\sin\psi & \sin\varphi \sin\psi
++\cos\varphi\sin\theta\cos\psi \\
+ \sin\varphi \cos\theta & \cos\varphi\cos\psi + \sin\varphi\sin\theta
+\sin\psi & -\cos\varphi \sin\psi +\sin\varphi\sin\theta\cos\psi \\
-\sin\theta & \cos\theta \sin\psi & \cos\theta \cos\psi
\end{array}
\right)
@@ -124,7 +130,7 @@ int main()
vpRzyxVector rzyx;
// Initialise the Euler angles
- rzyx[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
+ rzyx[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
rzyx[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
rzyx[2] = vpMath::rad( 90.f); // psi angle in rad/s around x axis
@@ -136,12 +142,12 @@ int main()
// Print the extracted Euler angles. Values are the same than the
// one used for initialization
- std::cout << rzyx;
+ std::cout << rzyx;
// Since the rotation vector is 3 values column vector, the
// transpose operation produce a row vector.
vpRowVector rzyx_t = rzyx.t();
-
+
// Print the transpose row vector
std::cout << rzyx_t << std::endl;
}
@@ -157,25 +163,25 @@ public:
vpRzyxVector(const double phi, const double theta, const double psi);
// initialize a Rzyx vector from a rotation matrix
- vpRzyxVector(const vpRotationMatrix& R) ;
+ explicit vpRzyxVector(const vpRotationMatrix &R);
// initialize a Rzyx vector from a ThetaU vector
- vpRzyxVector(const vpThetaUVector& tu) ;
- vpRzyxVector(const vpColVector &rzyx);
+ explicit vpRzyxVector(const vpThetaUVector &tu);
+ explicit vpRzyxVector(const vpColVector &rzyx);
//! Destructor.
- virtual ~vpRzyxVector() {};
+ virtual ~vpRzyxVector(){};
// convert a rotation matrix into Rzyx vector
- vpRzyxVector buildFrom(const vpRotationMatrix& R) ;
+ vpRzyxVector buildFrom(const vpRotationMatrix &R);
// convert a ThetaU vector into a Rzyx vector
- vpRzyxVector buildFrom(const vpThetaUVector& R) ;
+ vpRzyxVector buildFrom(const vpThetaUVector &R);
void buildFrom(const double phi, const double theta, const double psi);
vpRzyxVector &operator=(const vpColVector &rzyx);
- vpRzyxVector &operator=(double x) ;
-} ;
+ vpRzyxVector &operator=(double x);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpRzyzVector.h b/modules/core/include/visp3/core/vpRzyzVector.h
index 0ad0153..1205706 100644
--- a/modules/core/include/visp3/core/vpRzyzVector.h
+++ b/modules/core/include/visp3/core/vpRzyzVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
#ifndef vpRzyzVector_h
#define vpRzyzVector_h
@@ -52,8 +52,8 @@
class vpRotationMatrix;
class vpThetaUVector;
-#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRotationVector.h>
/*!
\class vpRzyzVector
@@ -68,7 +68,8 @@ class vpThetaUVector;
\f$(\varphi,\theta,\psi)\f$ are respectively the rotation angles
around the \f$z\f$, \f$y\f$ and \f$z\f$ axis.
- \f[R_{zyz}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \; R_z(\psi)\f]
+ \f[R_{zyz}(\varphi,\theta,\psi) = R_z(\varphi) \; R_y(\theta) \;
+R_z(\psi)\f]
with
@@ -95,19 +96,21 @@ class vpThetaUVector;
\end{array}
\right)
\f]
-
+
The rotation matrix corresponding to the z-y-z convention is given by:
\f[
R_{zyz}(\varphi,\theta,\psi) = \left(
\begin{array}{ccc}
- \cos\varphi \cos\theta \cos\psi - \sin\varphi\sin\psi & -\cos\varphi \cos\theta \sin\psi -\sin\varphi\cos\psi & \cos\varphi \sin\theta \\
- \sin\varphi \cos\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi \cos\theta \sin\psi +\cos\varphi\cos\psi & \sin\varphi \sin\theta \\
+ \cos\varphi \cos\theta \cos\psi - \sin\varphi\sin\psi & -\cos\varphi
+\cos\theta \sin\psi -\sin\varphi\cos\psi & \cos\varphi \sin\theta \\
+ \sin\varphi \cos\theta \cos\psi + \cos\varphi\sin\psi & -\sin\varphi
+\cos\theta \sin\psi +\cos\varphi\cos\psi & \sin\varphi \sin\theta \\
-\sin\theta \cos\psi & \sin\theta \sin\psi & \cos\theta
\end{array}
\right)
\f]
-
+
The vpRzyzVector class is derived from vpRotationVector.
The code below shows first how to initialize this representation of
@@ -125,7 +128,7 @@ int main()
vpRzyzVector rzyz;
// Initialise the Euler angles
- rzyz[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
+ rzyz[0] = vpMath::rad( 45.f); // phi angle in rad/s around z axis
rzyz[1] = vpMath::rad(-30.f); // theta angle in rad/s around y axis
rzyz[2] = vpMath::rad( 90.f); // psi angle in rad/s around z axis
@@ -137,12 +140,12 @@ int main()
// Print the extracted Euler angles. Values are the same than the
// one used for initialization
- std::cout << rzyz;
+ std::cout << rzyz;
// Since the rotation vector is 3 values column vector, the
// transpose operation produce a row vector.
vpRowVector rzyz_t = rzyz.t();
-
+
// Print the transpose row vector
std::cout << rzyz_t << std::endl;
}
@@ -150,31 +153,31 @@ int main()
*/
class VISP_EXPORT vpRzyzVector : public vpRotationVector
{
- public:
+public:
vpRzyzVector();
vpRzyzVector(const vpRzyzVector &rzyz);
// initialize a Rzyz vector from a rotation matrix
- vpRzyzVector(const vpRotationMatrix& R);
+ explicit vpRzyzVector(const vpRotationMatrix &R);
// initialize a Rzyz vector from a ThetaU vector
- vpRzyzVector(const vpThetaUVector& tu);
+ explicit vpRzyzVector(const vpThetaUVector &tu);
vpRzyzVector(const double phi, const double theta, const double psi);
- vpRzyzVector(const vpColVector &rzyz);
+ explicit vpRzyzVector(const vpColVector &rzyz);
//! Destructor.
- virtual ~vpRzyzVector() {};
+ virtual ~vpRzyzVector(){};
// convert a rotation matrix into Rzyz vector
- vpRzyzVector buildFrom(const vpRotationMatrix& R) ;
+ vpRzyzVector buildFrom(const vpRotationMatrix &R);
// convert a ThetaU vector into a Rzyz vector
- vpRzyzVector buildFrom(const vpThetaUVector& R) ;
+ vpRzyzVector buildFrom(const vpThetaUVector &R);
void buildFrom(const double phi, const double theta, const double psi);
vpRzyzVector &operator=(const vpColVector &rzyz);
vpRzyzVector &operator=(double x);
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpScale.h b/modules/core/include/visp3/core/vpScale.h
index 7042962..59b9358 100644
--- a/modules/core/include/visp3/core/vpScale.h
+++ b/modules/core/include/visp3/core/vpScale.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
+ * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density
+ *estimation.
*
* Authors:
* Andrew Comport
@@ -43,19 +45,16 @@
/*!
* \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
*/
// ==========================================================
-
#ifndef VPSCALE_HH
#define VPSCALE_HH
-#include <visp3/core/vpColVector.h>
#include <math.h>
-
+#include <visp3/core/vpColVector.h>
/*!
\class vpScale
@@ -78,10 +77,9 @@ private:
unsigned int dimension;
public:
-
//! Constructor
vpScale();
- vpScale(double kernel_bandwidth, unsigned int dim=1);
+ vpScale(double kernel_bandwidth, unsigned int dim = 1);
//! Destructor
virtual ~vpScale(void);
@@ -91,7 +89,6 @@ public:
double KernelDensity_EPANECHNIKOV(vpColVector &X);
double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n);
-
};
#endif
diff --git a/modules/core/include/visp3/core/vpServer.h b/modules/core/include/visp3/core/vpServer.h
index 9cde44f..8147351 100644
--- a/modules/core/include/visp3/core/vpServer.h
+++ b/modules/core/include/visp3/core/vpServer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,70 +44,72 @@
#include <visp3/core/vpException.h>
#include <visp3/core/vpNetwork.h>
-
/*!
\class vpServer
\ingroup group_core_network
-
+
\brief This class represents a Transmission Control Protocol (TCP) server.
-
- TCP provides reliable, ordered delivery of a stream of bytes from a program
+
+ TCP provides reliable, ordered delivery of a stream of bytes from a program
on one computer to another program on another computer.
Exemple of server's code, receiving and sending basic message.
- It corresponds to the client used in the first exemple of vpClient class'
+ It corresponds to the client used in the first exemple of vpClient class'
documentation.
-
+
\code
-#include <visp3/core/vpServer.h>
#include <iostream>
+#include <visp3/core/vpServer.h>
int main(int argc,const char** argv)
{
int port = 35000;
vpServer serv(port); //Launch the server on localhost
serv.start();
-
+
bool run = true;
int val;
-
+
while(run){
serv.checkForConnections();
-
+
if(serv.getNumberOfClients() > 0)
{
- if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
- std::cout << "Error while receiving" << std::endl;
+ // 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
+ // 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;
}
}
-
+
return 0;
}
\endcode
-
+
Exemple of server's code, receiving a vpImage on request form.
- It correspond to the client used in the second exemple of vpClient class' documentation.
-
+ It correspond to the client used in the second exemple of vpClient class'
+documentation.
+
\code
#include <visp3/core/vpServer.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
#include "vpRequestImage.h" //See vpRequest class documentation
int main(int argc,const char** argv)
{
int port = 35000;
-
+
std::cout << "Port: " << port << std::endl;
vpServer serv(port);
serv.start();
@@ -118,27 +121,27 @@ int main(int argc,const char** argv)
#endif
vpImage<unsigned char> I;
-
+
vpRequestImage reqImage(&I);
serv.addDecodingRequest(&reqImage);
-
+
bool run = true;
-
+
while(run){
serv.checkForConnections();
-
+
if(serv.getNumberOfClients() > 0)
{
int index = serv.receiveAndDecodeRequestOnce();
std::string id = serv.getRequestIdFromIndex(index);
-
+
if(id == reqImage.getId())
{
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
if (! display.isInitialised() )
display.init(I, -1, -1, "Remote display");
#endif
-
+
vpDisplay::display(I) ;
vpDisplay::flush(I);
@@ -148,11 +151,11 @@ int main(int argc,const char** argv)
}
}
}
-
+
return 0;
}
\endcode
-
+
\sa vpClient
\sa vpRequest
\sa vpNetwork
@@ -160,62 +163,60 @@ int main(int argc,const char** argv)
class VISP_EXPORT vpServer : public vpNetwork
{
private:
-
//######## PARAMETERS ########
//# #
//############################
- std::string adress;
- int port;
- bool started;
+ std::string adress;
+ int port;
+ bool started;
unsigned int max_clients;
-
+
public:
-
vpServer();
- vpServer(const int &port);
- vpServer(const std::string &adress_serv,const int &port_serv);
-
- virtual ~vpServer();
-
- bool checkForConnections();
-
+ explicit vpServer(const int &port);
+ vpServer(const std::string &adress_serv, const int &port_serv);
+
+ virtual ~vpServer();
+
+ bool checkForConnections();
+
/*!
Check if the server is started.
-
+
\sa vpServer::start()
\return True if the server is started, false otherwise.
*/
- bool isStarted(){ return started; }
-
+ bool isStarted() { return started; }
+
/*!
Get the maximum number of clients that can be connected to the server.
-
+
\sa vpServer::setMaxNumberOfClients()
\return Maximum number of clients.
*/
- unsigned int getMaxNumberOfClients(){ return max_clients; }
-
+ unsigned int getMaxNumberOfClients() { return max_clients; }
+
/*!
Get the number of clients connected to the server.
\return Number of clients connected.
*/
- unsigned int getNumberOfClients(){ return (unsigned int)receptor_list.size(); }
-
- void print();
-
- bool start();
-
+ unsigned int getNumberOfClients() { return (unsigned int)receptor_list.size(); }
+
+ void print();
+
+ bool start();
+
/*!
Set the maximum number of clients that can be connected to the server.
-
+
\sa vpServer::getMaxNumberOfClients()
\param l : Maximum number of clients.
*/
- void setMaxNumberOfClients(unsigned int &l){ max_clients = l; }
+ void setMaxNumberOfClients(unsigned int &l) { max_clients = l; }
};
#endif
diff --git a/modules/core/include/visp3/core/vpSphere.h b/modules/core/include/visp3/core/vpSphere.h
index 59944d6..4a68445 100644
--- a/modules/core/include/visp3/core/vpSphere.h
+++ b/modules/core/include/visp3/core/vpSphere.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,13 +44,12 @@
#ifndef vpSphere_hh
#define vpSphere_hh
-
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
-#include <visp3/core/vpForwardProjection.h>
#include <math.h>
+#include <visp3/core/vpForwardProjection.h>
/*!
\class vpSphere
\ingroup group_core_geometry
@@ -60,51 +60,40 @@
class VISP_EXPORT vpSphere : public vpForwardProjection
{
public:
- void init() ;
- vpSphere() ;
- virtual ~vpSphere() ;
+ void init();
+ vpSphere();
+ virtual ~vpSphere();
public:
- vpSphere(const vpColVector& oP) ;
- vpSphere(const double X0, const double Y0,
- const double Z0,
- const double R) ;
-
-
- void setWorldCoordinates(const vpColVector& oP) ;
- void setWorldCoordinates(const double X0, const double Y0,
- const double Z0,
- const double R) ;
+ explicit vpSphere(const vpColVector &oP);
+ vpSphere(const double X0, const double Y0, const double Z0, const double R);
- double get_x() const { return p[0] ; }
- double get_y() const { return p[1] ; }
- double get_mu20() const { return p[2] ; }
- double get_mu11() const { return p[3] ; }
- double get_mu02() const { return p[4] ; }
+ void setWorldCoordinates(const vpColVector &oP);
+ void setWorldCoordinates(const double X0, const double Y0, const double Z0, const double R);
- double getX() const { return cP[0] ; }
- double getY() const { return cP[1] ; }
- double getZ() const { return cP[2] ; }
+ double get_x() const { return p[0]; }
+ double get_y() const { return p[1]; }
+ double get_mu20() const { return p[2]; }
+ double get_mu11() const { return p[3]; }
+ double get_mu02() const { return p[4]; }
- double getR() const { return cP[3] ; }
+ double getX() const { return cP[0]; }
+ double getY() const { return cP[1]; }
+ double getZ() const { return cP[2]; }
- void projection() ;
- void projection(const vpColVector &cP, vpColVector &p) ;
- void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) ;
- void changeFrame(const vpHomogeneousMatrix &cMo) ;
+ double getR() const { return cP[3]; }
- void display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
- void display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color=vpColor::green,
- const unsigned int thickness=1) ;
+ void projection();
+ void projection(const vpColVector &cP, vpColVector &p);
+ void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP);
+ void changeFrame(const vpHomogeneousMatrix &cMo);
- vpSphere *duplicate() const ;
-} ;
+ void display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color = vpColor::green,
+ const unsigned int thickness = 1);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color = vpColor::green, const unsigned int thickness = 1);
+ vpSphere *duplicate() const;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpSubColVector.h b/modules/core/include/visp3/core/vpSubColVector.h
index d2ed86c..eeb69d1 100644
--- a/modules/core/include/visp3/core/vpSubColVector.h
+++ b/modules/core/include/visp3/core/vpSubColVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -61,26 +62,24 @@
class VISP_EXPORT vpSubColVector : public vpColVector
{
-private :
+private:
//! Copy constructor unavaible
- vpSubColVector(const vpSubColVector& /* m */);
-
-protected :
+ vpSubColVector(const vpSubColVector & /* m */);
+protected:
//! Number of row of parent vpColvector at initialization
unsigned int pRowNum;
//! Parent vpColvector
vpColVector *parent;
public:
-
vpSubColVector();
- vpSubColVector(vpColVector &v, const unsigned int & offset,const unsigned int & nrows);
+ 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 ea51fcf..f38378e 100644
--- a/modules/core/include/visp3/core/vpSubMatrix.h
+++ b/modules/core/include/visp3/core/vpSubMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,6 @@
#include <visp3/core/vpMatrix.h>
-
/*!
\file vpSubMatrix.h
@@ -59,44 +59,44 @@
\sa vpMatrix vpColvector vpRowVector
*/
-class VISP_EXPORT vpSubMatrix : public vpMatrix{
+class VISP_EXPORT vpSubMatrix : public vpMatrix
+{
+
+private:
+ //! Eye method unavailable
+ void eye(unsigned int n);
+ //! Eye method unavailable
+ void eye(unsigned int m, unsigned int n);
+ //! Copy constructor unavailable
+ vpSubMatrix(const vpSubMatrix & /* m */);
+
+protected:
+ unsigned int pRowNum;
+ unsigned int pColNum;
+ vpMatrix *parent;
+
+public:
+ //! Default constructor
+ vpSubMatrix();
+ //! Constructor
+ vpSubMatrix(vpMatrix &m, const unsigned int &row, const unsigned int &col, const unsigned int &nrows,
+ const unsigned int &ncols);
+ //! Destructor
+ ~vpSubMatrix();
+
+ //! Initialisation of vpMatrix
+ void init(vpMatrix &m, const unsigned int &row, const unsigned int &col, const unsigned int &nrows,
+ const unsigned int &ncols);
- private :
- //! Eye method unavailable
- void eye(unsigned int n);
- //! Eye method unavailable
- void eye(unsigned int m, unsigned int n);
- //! Copy constructor unavailable
- vpSubMatrix(const vpSubMatrix& /* m */);
-
- protected :
-
- unsigned int pRowNum;
- unsigned int pColNum;
- vpMatrix *parent;
-
- public:
+ //! Check is parent vpRowVector has changed since initialization
+ void checkParentStatus() const;
- //!Default constructor
- vpSubMatrix();
- //!Constructor
- vpSubMatrix(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
- //!Destructor
- ~vpSubMatrix();
-
- //! Initialisation of vpMatrix
- void init(vpMatrix &m, const unsigned int & row, const unsigned int &col , const unsigned int & nrows , const unsigned int & ncols);
-
- //!Check is parent vpRowVector has changed since initialization
- void checkParentStatus() const;
-
- //! Operation such as subA = subB
- vpSubMatrix & operator=(const vpSubMatrix &B);
- //! Operation such as subA = B
- vpSubMatrix & operator=(const vpMatrix &B);
- //! Operation such as subA = x
- vpSubMatrix & operator=(const double &x);
-
+ //! Operation such as subA = subB
+ vpSubMatrix &operator=(const vpSubMatrix &B);
+ //! Operation such as subA = B
+ vpSubMatrix &operator=(const vpMatrix &B);
+ //! Operation such as subA = x
+ vpSubMatrix &operator=(const double &x);
};
#endif
diff --git a/modules/core/include/visp3/core/vpSubRowVector.h b/modules/core/include/visp3/core/vpSubRowVector.h
index 9f44b89..951cd58 100644
--- a/modules/core/include/visp3/core/vpSubRowVector.h
+++ b/modules/core/include/visp3/core/vpSubRowVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,6 @@
#include <visp3/core/vpRowVector.h>
-
/*!
\file vpSubRowVector.h
@@ -63,31 +63,29 @@
class VISP_EXPORT vpSubRowVector : public vpRowVector
{
-private :
+private:
//! Copy constructor unavaible
- vpSubRowVector(const vpSubRowVector& /* m */);
-
-protected :
+ vpSubRowVector(const vpSubRowVector & /* m */);
+protected:
//! Number of row of parent vpColvector at initialization
unsigned int pColNum;
//! Parent vpColvector
vpRowVector *parent;
public:
-
vpSubRowVector();
- vpSubRowVector(vpRowVector &v, const unsigned int & offset,const unsigned int & ncols);
+ 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);
- vpSubRowVector & operator=(const vpMatrix &B);
- vpSubRowVector & operator=(const double &x);
+ vpSubRowVector &operator=(const vpSubRowVector &B);
+ vpSubRowVector &operator=(const vpRowVector &B);
+ vpSubRowVector &operator=(const vpMatrix &B);
+ vpSubRowVector &operator=(const double &x);
};
#endif
diff --git a/modules/core/include/visp3/core/vpThetaUVector.h b/modules/core/include/visp3/core/vpThetaUVector.h
index f4e9959..9d4d8c9 100644
--- a/modules/core/include/visp3/core/vpThetaUVector.h
+++ b/modules/core/include/visp3/core/vpThetaUVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpTHETAUVECTOR_H
#define vpTHETAUVECTOR_H
@@ -56,21 +56,20 @@ class vpRotationVector;
class vpQuaternionVector;
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpRotationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpRzyxVector.h>
-#include <visp3/core/vpQuaternionVector.h>
-
/*!
\class vpThetaUVector
\ingroup group_core_transformations
- \brief Implementation of a rotation vector as \f$\theta {\bf u}\f$ axis-angle
- minimal representation.
+ \brief Implementation of a rotation vector as \f$\theta {\bf u}\f$
+axis-angle minimal representation.
Class that consider the case of the \f$\theta {\bf u}\f$
parameterization for the rotation.
@@ -78,8 +77,8 @@ class vpQuaternionVector;
The vpThetaUVector class is derived from vpRotationVector.
The \f$\theta {\bf u}\f$ representation is one of the minimal
- representation of a rotation matrix, where
- \f${\bf u} = (u_{x} \; u_{y} \; u_{z})^{\top}\f$
+ representation of a rotation matrix, where
+ \f${\bf u} = (u_{x} \; u_{y} \; u_{z})^{\top}\f$
is a unit vector representing the rotation
axis and \f$\theta\f$ is the rotation angle.
@@ -87,15 +86,15 @@ class vpQuaternionVector;
rotation matrix \f${\bf R}\f$ using the Rodrigues formula:
\f[
- {\bf R} = {\bf I}_{3} + (1 - \cos{ \theta}) \; {\bf u u}^{\top} + \sin{ \theta} \; [{\bf u}]_{\times}
- \f]
+ {\bf R} = {\bf I}_{3} + (1 - \cos{ \theta}) \; {\bf u u}^{\top} + \sin{
+\theta} \; [{\bf u}]_{\times} \f]
with \f${\bf I}_{3}\f$ the identity matrix of dimension
\f$3\times3\f$ and \f$[{\bf u}]_{\times}\f$ the skew matrix:
\f[
[{\bf u}]_{\times} = \left(
- \begin{array}{ccc}
+ \begin{array}{ccc}
0 & -u_{z} & u_{y} \\
u_{z} & 0 & -u_{x} \\
-u_{y} & u_{x} & 0
@@ -103,7 +102,7 @@ class vpQuaternionVector;
\right)
\f]
From the implementation point of view, it is nothing more than an
- array of three floats.
+ array of three floats.
The code below shows first how to initialize a \f$\theta {\bf u}\f$
vector, than how to contruct a rotation matrix from a vpThetaUVector
@@ -121,9 +120,9 @@ int main()
vpThetaUVector tu;
// Initialise the theta U rotation vector
- tu[0] = vpMath::rad( 45.f);
- tu[1] = vpMath::rad(-30.f);
- tu[2] = vpMath::rad( 90.f);
+ tu[0] = vpMath::rad( 45.f);
+ tu[1] = vpMath::rad(-30.f);
+ tu[2] = vpMath::rad( 90.f);
// Construct a rotation matrix from the theta U angles
vpRotationMatrix R(tu);
@@ -133,12 +132,12 @@ int main()
// Print the extracted theta U angles. Values are the same than the
// one used for initialization
- std::cout << tu;
+ std::cout << tu;
// Since the rotation vector is 3 values column vector, the
// transpose operation produce a row vector.
vpRowVector tu_t = tu.t();
-
+
// Print the transpose row vector
std::cout << tu_t << std::endl;
}
@@ -147,7 +146,6 @@ int main()
class VISP_EXPORT vpThetaUVector : public vpRotationVector
{
private:
-
static const double minimum;
public:
@@ -155,47 +153,47 @@ public:
vpThetaUVector(const vpThetaUVector &tu);
// constructor initialize a Theta U vector from a homogeneous matrix
- vpThetaUVector(const vpHomogeneousMatrix & M) ;
+ explicit vpThetaUVector(const vpHomogeneousMatrix &M);
// constructor initialize a Theta U vector from a pose vector
- vpThetaUVector(const vpPoseVector & p) ;
+ explicit vpThetaUVector(const vpPoseVector &p);
// constructor initialize a Theta U vector from a rotation matrix
- vpThetaUVector(const vpRotationMatrix& R) ;
+ explicit vpThetaUVector(const vpRotationMatrix &R);
// constructor initialize a Theta U vector from a RzyxVector
- vpThetaUVector(const vpRzyxVector& rzyx) ;
+ explicit vpThetaUVector(const vpRzyxVector &rzyx);
// constructor initialize a Theta U vector from a RzyzVector
- vpThetaUVector(const vpRzyzVector& rzyz) ;
+ explicit vpThetaUVector(const vpRzyzVector &rzyz);
// constructor initialize a Theta U vector from a RxyzVector
- vpThetaUVector(const vpRxyzVector& rxyz) ;
- vpThetaUVector(const vpQuaternionVector& q) ;
+ explicit vpThetaUVector(const vpRxyzVector &rxyz);
+ explicit vpThetaUVector(const vpQuaternionVector &q);
vpThetaUVector(const double tux, const double tuy, const double tuz);
- vpThetaUVector(const vpColVector &tu);
+ explicit vpThetaUVector(const vpColVector &tu);
//! Destructor.
- virtual ~vpThetaUVector() {};
+ virtual ~vpThetaUVector(){};
// convert an homogeneous matrix into Theta U vector
- vpThetaUVector buildFrom(const vpHomogeneousMatrix& M) ;
+ vpThetaUVector buildFrom(const vpHomogeneousMatrix &M);
// convert a pose vector into Theta U vector
- vpThetaUVector buildFrom(const vpPoseVector& p) ;
+ vpThetaUVector buildFrom(const vpPoseVector &p);
// convert a rotation matrix into Theta U vector
- vpThetaUVector buildFrom(const vpRotationMatrix& R) ;
+ vpThetaUVector buildFrom(const vpRotationMatrix &R);
// convert an Rzyx vector into Theta U vector
- vpThetaUVector buildFrom(const vpRzyxVector &rzyx) ;
+ vpThetaUVector buildFrom(const vpRzyxVector &rzyx);
// convert an Rzyz vector into Theta U vector
- vpThetaUVector buildFrom(const vpRzyzVector &zyz) ;
+ vpThetaUVector buildFrom(const vpRzyzVector &zyz);
// convert an Rxyz vector into Theta U vector
- vpThetaUVector buildFrom(const vpRxyzVector &xyz) ;
- vpThetaUVector buildFrom(const vpQuaternionVector &q) ;
+ vpThetaUVector buildFrom(const vpRxyzVector &xyz);
+ vpThetaUVector buildFrom(const vpQuaternionVector &q);
void buildFrom(const double tux, const double tuy, const double tuz);
// extract the angle and the axis from the ThetaU representation
- void extract( double &theta, vpColVector &u) const;
+ void extract(double &theta, vpColVector &u) const;
double getTheta() const;
vpColVector getU() const;
vpThetaUVector &operator=(const vpColVector &tu);
- vpThetaUVector &operator=(double x) ;
-} ;
+ vpThetaUVector &operator=(double x);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpThread.h b/modules/core/include/visp3/core/vpThread.h
index deed5f5..26af611 100644
--- a/modules/core/include/visp3/core/vpThread.h
+++ b/modules/core/include/visp3/core/vpThread.h
@@ -7,10 +7,13 @@
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
#if defined(VISP_HAVE_PTHREAD)
-# include <pthread.h>
-# include <string.h>
+#include <pthread.h>
+#include <string.h>
#elif defined(_WIN32)
-# include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
+#include <windows.h>
#endif
/*!
@@ -19,12 +22,14 @@
\ingroup group_core_threading
Class to represent individual threads of execution.
- This class implements native pthread functionalities if available, or native Windows threading
- capabilities if pthread is not available under Windows.
+ This class implements native pthread functionalities if available, or
+ native Windows threading capabilities if pthread is not available under
+ Windows.
- There are two examples implemented in testMutex.cpp and testThread.cpp to show how to use this class.
- The content of test-thread.cpp that hightlights the main functionalities of this class is given hereafter:
- \snippet testThread.cpp Code
+ There are two examples implemented in testMutex.cpp and testThread.cpp to
+ show how to use this class. The content of test-thread.cpp that hightlights
+ the main functionalities of this class is given hereafter: \snippet
+ testThread.cpp Code
More examples are provided in \ref tutorial-multi-threading.
*/
@@ -39,53 +44,49 @@ public:
#elif defined(_WIN32)
typedef LPVOID Args;
typedef DWORD Return;
- typedef LPTHREAD_START_ROUTINE Fn;
- //typedef DWORD (*Fn)(Args);
- typedef HANDLE Handle;
+ typedef LPTHREAD_START_ROUTINE Fn;
+ // typedef DWORD (*Fn)(Args);
+ typedef HANDLE Handle;
#endif
/*!
- Default constructor that does nothing. To attach a function to this thread
- of execution you need to call create().
+ Default constructor that does nothing. To attach a function to this
+ thread of execution you need to call create().
*/
- vpThread() : m_handle(), m_isCreated(false), m_isJoinable(false)
- {
- }
+ vpThread() : m_handle(), m_isCreated(false), m_isJoinable(false) {}
/*!
- Construct a thread object that represents a new joinable thread of execution.
- The new thread of execution calls \e fn passing \e args as arguments.
- \param fn : A pointer to a function.
- \param args : Arguments passed to the call to \e fn (if any).
+ Construct a thread object that represents a new joinable thread of
+ execution. The new thread of execution calls \e fn passing \e args as
+ 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);
}
/*!
- Creates a thread object that represents a new joinable thread of execution.
- \param fn : A pointer to a function.
- \param args : Arguments passed to the call to \e fn (if any).
+ Creates a thread object that represents a new joinable thread of
+ execution. \param fn : A pointer to a function. \param args : Arguments
+ passed to the call to \e fn (if any).
*/
- void create(vpThread::Fn fn, vpThread::Args args=NULL)
+ void create(vpThread::Fn fn, vpThread::Args args = NULL)
{
if (m_isCreated)
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
+ 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
#endif
m_isJoinable = true;
@@ -103,13 +104,13 @@ public:
#endif
}
-
/*!
This function return when the thread execution has completed.
- This blocks the execution of the thread that calls this function until the function
- called on construction returns (if it hasn't yet).
+ This blocks the execution of the thread that calls this function until
+ the function called on construction returns (if it hasn't yet).
- After a call to this function, the thread object becomes non-joinable and can be destroyed safely.
+ After a call to this function, the thread object becomes non-joinable and
+ can be destroyed safely.
\sa joinable()
*/
@@ -119,23 +120,21 @@ public:
#if defined(VISP_HAVE_PTHREAD)
pthread_join(m_handle, NULL);
#elif defined(_WIN32)
-# if defined(WINRT_8_1)
+#if defined(WINRT_8_1)
WaitForSingleObjectEx(m_handle, INFINITE, FALSE);
-# else
+#else
WaitForSingleObject(m_handle, INFINITE);
-# endif
+#endif
#endif
m_isJoinable = false;
}
}
/*!
- Returns a value used to access implementation-specific information associated to the thread.
+ Returns a value used to access implementation-specific information
+ associated to the thread.
*/
- Handle getHandle()
- {
- return m_handle;
- }
+ Handle getHandle() { return m_handle; }
/*!
Returns whether the thread object is joinable.
@@ -146,14 +145,11 @@ public:
\sa join()
*/
- bool joinable()
- {
- return m_isJoinable;
- }
+ bool joinable() { return m_isJoinable; }
protected:
- Handle m_handle; //!< Thread handle
- bool m_isCreated; //!< Indicates if the thread is created
+ Handle m_handle; //!< Thread handle
+ bool m_isCreated; //!< Indicates if the thread is created
bool m_isJoinable; //!< Indicates if the thread is joinable
};
diff --git a/modules/core/include/visp3/core/vpTime.h b/modules/core/include/visp3/core/vpTime.h
index d58cac8..905b136 100644
--- a/modules/core/include/visp3/core/vpTime.h
+++ b/modules/core/include/visp3/core/vpTime.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpTime_h
#define vpTime_h
@@ -45,8 +45,8 @@
\brief Time management and measurement
*/
-#include <string>
#include <iostream>
+#include <string>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
@@ -75,14 +75,14 @@ int main()
namespace vpTime
{
- 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() ;
- VISP_EXPORT double measureTimeMicros() ;
- VISP_EXPORT void sleepMs(double t);
- VISP_EXPORT int wait(double t0, double t) ;
- VISP_EXPORT void wait(double t) ;
+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();
+VISP_EXPORT double measureTimeMicros();
+VISP_EXPORT void sleepMs(double t);
+VISP_EXPORT int wait(double t0, double t);
+VISP_EXPORT void wait(double t);
};
#endif
diff --git a/modules/core/include/visp3/core/vpTracker.h b/modules/core/include/visp3/core/vpTracker.h
index d4e8749..263d891 100644
--- a/modules/core/include/visp3/core/vpTracker.h
+++ b/modules/core/include/visp3/core/vpTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef vpTracker_H
#define vpTracker_H
@@ -45,10 +44,9 @@
\brief Class that defines what is a generic tracker.
*/
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-
+#include <visp3/core/vpImage.h>
/*!
\class vpTracker
@@ -59,7 +57,7 @@
- in the camera frame \e cP. These parameters are located in the public
attribute vpTracker::cP.
- in the image plane \e p. These parameters are located in the public
- attribute vpTracker::p. They correspond to normalized coordinates
+ attribute vpTracker::p. They correspond to normalized coordinates
of the feature expressed in meters.
*/
@@ -70,32 +68,31 @@ public:
Feature coordinates expressed in the image plane \e p. They correspond
to 2D normalized coordinates expressed in meters.
*/
- vpColVector p ;
+ vpColVector p;
/*!
- Feature coordinates expressed in the camera frame \e cP.
+ Feature coordinates expressed in the camera frame \e cP.
*/
- vpColVector cP ;
+ vpColVector cP;
/*!
Flag used to indicate if the feature parameters \e cP expressed
in the camera frame are available.
*/
- bool cPAvailable ;
+ bool cPAvailable;
public:
//! Default initialization.
- void init() ;
+ void init();
//! Default constructor.
- vpTracker() ;
+ vpTracker();
//! Copy constructor.
- vpTracker(const vpTracker &tracker) ;
+ vpTracker(const vpTracker &tracker);
//! Copy operator.
vpTracker &operator=(const vpTracker &tracker);
//! Destructor.
virtual ~vpTracker() { ; }
-} ;
-
+};
#endif
diff --git a/modules/core/include/visp3/core/vpTrackingException.h b/modules/core/include/visp3/core/vpTrackingException.h
index 42f249d..c9e24f4 100644
--- a/modules/core/include/visp3/core/vpTrackingException.h
+++ b/modules/core/include/visp3/core/vpTrackingException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,27 +36,30 @@
*
*****************************************************************************/
-
#ifndef __vpTrackingException_H
#define __vpTrackingException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpTrackingException.h
\brief error that can be emited by the vpTracker class and its derivates
*/
/* Classes standards. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpTrackingException
@@ -64,35 +68,31 @@
*/
class VISP_EXPORT vpTrackingException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpTracking member
- */
- enum errorTrackingCodeEnum
- {
- featureLostError,
-
- // Moving edges
- notEnoughPointError,
- initializationError,
- fatalError
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpTracking member
+ */
+ enum errorTrackingCodeEnum {
+ featureLostError,
- public:
- vpTrackingException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpTrackingException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpTrackingException (const int id)
- : vpException(id){ ; }
+ // Moving edges
+ notEnoughPointError,
+ initializationError,
+ fatalError
+ };
+public:
+ vpTrackingException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpTrackingException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpTrackingException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/core/include/visp3/core/vpTranslationVector.h b/modules/core/include/visp3/core/vpTranslationVector.h
index 32aade6..8a16db2 100644
--- a/modules/core/include/visp3/core/vpTranslationVector.h
+++ b/modules/core/include/visp3/core/vpTranslationVector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,8 +37,6 @@
*
*****************************************************************************/
-
-
#ifndef vpTRANSLATIONVECTOR_H
#define vpTRANSLATIONVECTOR_H
@@ -47,16 +46,15 @@
*/
#include <visp3/core/vpArray2D.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoseVector.h>
-
/*!
\class vpTranslationVector
\ingroup group_core_transformations
-
+
\brief Class that consider the case of a translation vector.
Let us denote \f$^{a}{\bf t}_{b} = [t_x,t_y,t_z]^\top\f$ the translation
@@ -75,7 +73,7 @@
int main()
{
- vpTranslationVector t; // Translation vector
+ vpTranslationVector t; // Translation vector
// Initialization of the translation vector
t[0] = 0.2; // tx = 0.2 meters
@@ -93,58 +91,56 @@ int main()
class VISP_EXPORT vpTranslationVector : public vpArray2D<double>
{
public:
-
/*!
Default constructor.
The translation vector is initialized to zero.
*/
- vpTranslationVector() : vpArray2D<double>(3, 1) {};
- vpTranslationVector(const double tx, const double ty, const double tz) ;
+ vpTranslationVector() : vpArray2D<double>(3, 1){};
+ vpTranslationVector(const double tx, const double ty, const double tz);
vpTranslationVector(const vpTranslationVector &tv);
- vpTranslationVector(const vpHomogeneousMatrix &M);
- vpTranslationVector(const vpPoseVector &p);
- vpTranslationVector(const vpColVector &v);
+ explicit vpTranslationVector(const vpHomogeneousMatrix &M);
+ explicit vpTranslationVector(const vpPoseVector &p);
+ explicit vpTranslationVector(const vpColVector &v);
- 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) ;
+ 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);
double euclideanNorm() const;
// operators
// translation vectors additions c = a + b (a, b unchanged)
- vpTranslationVector operator+(const vpTranslationVector &tv) const ;
+ vpTranslationVector operator+(const vpTranslationVector &tv) const;
vpTranslationVector operator+(const vpColVector &v) const;
// translation vectors substraction c = a - b (a, b unchanged)
- vpTranslationVector operator-(const vpTranslationVector &tv) const ;
+ vpTranslationVector operator-(const vpTranslationVector &tv) const;
// negate t = -a (t is unchanged)
- vpTranslationVector operator-() const ;
- vpMatrix operator*(const vpRowVector &v) const;
+ vpTranslationVector operator-() const;
+ vpMatrix operator*(const vpRowVector &v) const;
// b = x * a (x=scalar)
vpTranslationVector operator*(const double x) const;
- vpTranslationVector & operator*=(double x);
+ vpTranslationVector &operator*=(double x);
vpTranslationVector operator/(const double x) const;
- vpTranslationVector & operator/=(double x);
+ vpTranslationVector &operator/=(double x);
// Copy operator. Allow operation such as A = v
vpTranslationVector &operator=(const vpColVector &tv);
vpTranslationVector &operator=(const vpTranslationVector &tv);
- vpTranslationVector &operator=(double x) ;
+ vpTranslationVector &operator=(double x);
//! Operator that allows to set a value of an element \f$t_i\f$: t[i] = x
- inline double &operator [](unsigned int n) { return *(data + n); }
+ 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;
@@ -152,19 +148,18 @@ public:
throw(vpException(vpException::fatalError, "Cannot resize a translation vector"));
};
- void set(const double tx, const double ty, const double tz) ;
+ void set(const double tx, const double ty, const double tz);
// Skew Symmetric matrix
- vpMatrix skew() const ;
+ vpMatrix skew() const;
double sumSquare() const;
vpRowVector t() const;
- static vpTranslationVector cross(const vpTranslationVector &a,
- const vpTranslationVector &b) ;
- static vpMatrix skew(const vpTranslationVector &tv) ;
- static void skew(const vpTranslationVector &tv, vpMatrix &M) ;
-} ;
+ static vpTranslationVector cross(const vpTranslationVector &a, const vpTranslationVector &b);
+ static vpMatrix skew(const vpTranslationVector &tv);
+ static void skew(const vpTranslationVector &tv, vpMatrix &M);
+};
#endif
diff --git a/modules/core/include/visp3/core/vpTriangle.h b/modules/core/include/visp3/core/vpTriangle.h
index 7e784a6..c1a0bc3 100644
--- a/modules/core/include/visp3/core/vpTriangle.h
+++ b/modules/core/include/visp3/core/vpTriangle.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,10 +44,12 @@
\class vpTriangle
\ingroup group_core_geometry
\brief Defines a 2D triangle.
-
+
A triangle is internally represented by three 2D points.
-
- By default the three coordinates in the \f$ (i,j) \f$ frame (see vpImagePoint class documentation for more details about the frame.) are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
+
+ By default the three coordinates in the \f$ (i,j) \f$ frame (see
+ vpImagePoint class documentation for more details about the frame.) are \f$
+ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
*/
#include <visp3/core/vpImagePoint.h>
@@ -54,60 +57,57 @@
class VISP_EXPORT vpTriangle
{
- private:
- bool goodTriange;
- vpImagePoint S1;
- double uvinv00;
- double uvinv01;
- double uvinv10;
- double uvinv11;
- double ptempo0;
- double ptempo1;
- double area;
- vpImagePoint apex1;
- vpImagePoint apex2;
- vpImagePoint apex3;
-
- public:
- vpTriangle();
- vpTriangle(const vpTriangle &tri);
- virtual ~vpTriangle();
-
- vpTriangle &operator=(const vpTriangle& tri);
-
- vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
- void buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
- bool inTriangle(const vpImagePoint &iP, double threshold = 0.00001);
-
- /*!
- Get the apexes of the triangle.
-
- \param iP1 : first apex.
- \param iP2 : second apex.
- \param iP3 : third apex.
- */
- void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, vpImagePoint &iP3) const {
- iP1 = apex1;
- iP2 = apex2;
- iP3 = apex3;}
-
-
- /*!
- Return the area of the triangle.
- The area is computed when the triangle is built from three image points.
-
- \return The area of the triangle.
- */
- inline double getArea() const{
- return this->area;
- }
-
- private:
- void init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
-
+private:
+ bool goodTriange;
+ vpImagePoint S1;
+ double uvinv00;
+ double uvinv01;
+ double uvinv10;
+ double uvinv11;
+ double ptempo0;
+ double ptempo1;
+ double area;
+ vpImagePoint apex1;
+ vpImagePoint apex2;
+ vpImagePoint apex3;
+
+public:
+ vpTriangle();
+ vpTriangle(const vpTriangle &tri);
+ virtual ~vpTriangle();
+
+ vpTriangle &operator=(const vpTriangle &tri);
+
+ vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
+
+ void buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
+
+ bool inTriangle(const vpImagePoint &iP, double threshold = 0.00001);
+
+ /*!
+ Get the apexes of the triangle.
+
+ \param iP1 : first apex.
+ \param iP2 : second apex.
+ \param iP3 : third apex.
+ */
+ void getTriangleApexes(vpImagePoint &iP1, vpImagePoint &iP2, vpImagePoint &iP3) const
+ {
+ iP1 = apex1;
+ iP2 = apex2;
+ iP3 = apex3;
+ }
+
+ /*!
+ Return the area of the triangle.
+ The area is computed when the triangle is built from three image points.
+
+ \return The area of the triangle.
+ */
+ inline double getArea() const { return this->area; }
+
+private:
+ void init(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3);
};
#endif
-
diff --git a/modules/core/include/visp3/core/vpUDPClient.h b/modules/core/include/visp3/core/vpUDPClient.h
new file mode 100644
index 0000000..1102054
--- /dev/null
+++ b/modules/core/include/visp3/core/vpUDPClient.h
@@ -0,0 +1,188 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * UDP Client
+ *
+ *****************************************************************************/
+
+#ifndef __vpUDPClient_h__
+#define __vpUDPClient_h__
+
+#include <visp3/core/vpConfig.h>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#else
+#include <winsock2.h>
+#endif
+
+#include <visp3/core/vpException.h>
+
+#define VP_MAX_UDP_PAYLOAD 508
+
+/*!
+ \class vpUDPClient
+
+ \ingroup group_core_network
+
+ \brief This class implements a basic (IPv4) User Datagram Protocol (UDP)
+client.
+
+ More information <a href="https://tools.ietf.org/html/rfc768">here</a>,
+ <a
+href="https://www.beej.us/guide/bgnet/output/html/singlepage/bgnet.html">here</a>
+ or <a
+href="https://www.ibm.com/support/knowledgecenter/en/SSLTBW_2.1.0/com.ibm.zos.v2r1.hala001/samples.htm">here</a>:
+ <blockquote>
+ This User Datagram Protocol (UDP) is defined to make available a
+ datagram mode of packet-switched computer communication in the
+ environment of an interconnected set of computer networks. This
+ protocol assumes that the Internet Protocol (IP) [1] is used as the
+ underlying protocol.
+
+ This protocol provides a procedure for application programs to send
+ messages to other programs with a minimum of protocol mechanism. The
+ protocol is transaction oriented, and delivery and duplicate protection
+ are not guaranteed. Applications requiring ordered reliable delivery of
+ streams of data should use the Transmission Control Protocol (TCP) [2].
+ </blockquote>
+
+ Example of a client's code, sending a basic message and receiving the
+ server answer:
+
+ \code
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpUDPClient.h>
+
+int main() {
+ try {
+ std::string servername = "127.0.0.1";
+ unsigned int port = 50037;
+ vpUDPClient client(servername, port);
+
+ while (true) {
+ std::cout << "Enter the message to send:" << std::endl;
+ std::string msg = "";
+ std::getline(std::cin, msg);
+ if (client.send(msg) != (int) msg.size())
+ std::cerr << "Error client.send()!" << std::endl;
+ if (client.receive(msg))
+ std::cout << "Receive from the server: " << msg << std::endl;
+ }
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+ \endcode
+
+ If you want to send a complex data type, you can either send the ASCII
+representation or send directly the byte data. In the last case, you should
+have to handle that both the server and the client have the same data type
+representation. Be careful also with the endianness of the network / host.
+
+ Here an example using a structure of data, assuming that both the server and
+the client have the same architecture (probably you should write your own
+serialization / deserialization functions for the data you want to send /
+receive):
+
+ \code
+#include <cstdlib>
+#include <iostream>
+#include <cstring>
+#include <visp3/core/vpUDPClient.h>
+
+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) {}
+};
+
+int main() {
+ try {
+ 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;
+ if (client.receive(msg)) {
+ data_type.double_val = *reinterpret_cast<const double *>(msg.c_str());
+ data_type.int_val
+ = *reinterpret_cast<const int *>(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;
+ return EXIT_FAILURE;
+ }
+}
+ \endcode
+
+ \sa vpUDPServer
+*/
+class VISP_EXPORT vpUDPClient
+{
+public:
+ vpUDPClient(const std::string &hostname, const int port);
+ ~vpUDPClient();
+
+ int receive(std::string &msg, const int timeoutMs = 0);
+ int send(const std::string &msg);
+
+private:
+ 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
+ int m_socketFileDescriptor;
+#else
+ SOCKET m_socketFileDescriptor;
+ WSADATA m_wsa;
+#endif
+
+ void init(const std::string &hostname, const int port);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpUDPServer.h b/modules/core/include/visp3/core/vpUDPServer.h
new file mode 100644
index 0000000..bfdced4
--- /dev/null
+++ b/modules/core/include/visp3/core/vpUDPServer.h
@@ -0,0 +1,220 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * UDP Server
+ *
+ *****************************************************************************/
+
+#ifndef __vpUDPServer_h__
+#define __vpUDPServer_h__
+
+#include <visp3/core/vpConfig.h>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#else
+#include <winsock2.h>
+#endif
+
+#include <visp3/core/vpException.h>
+
+#define VP_MAX_UDP_PAYLOAD 508
+
+/*!
+ \class vpUDPServer
+
+ \ingroup group_core_network
+
+ \brief This class implements a basic (IPv4) User Datagram Protocol (UDP)
+server.
+
+ More information <a href="https://tools.ietf.org/html/rfc768">here</a>,
+ <a
+href="https://www.beej.us/guide/bgnet/output/html/singlepage/bgnet.html">here</a>
+ or <a
+href="https://www.ibm.com/support/knowledgecenter/en/SSLTBW_2.1.0/com.ibm.zos.v2r1.hala001/samples.htm">here</a>:
+ <blockquote>
+ This User Datagram Protocol (UDP) is defined to make available a
+ datagram mode of packet-switched computer communication in the
+ environment of an interconnected set of computer networks. This
+ protocol assumes that the Internet Protocol (IP) [1] is used as the
+ underlying protocol.
+
+ This protocol provides a procedure for application programs to send
+ messages to other programs with a minimum of protocol mechanism. The
+ protocol is transaction oriented, and delivery and duplicate protection
+ are not guaranteed. Applications requiring ordered reliable delivery of
+ streams of data should use the Transmission Control Protocol (TCP) [2].
+ </blockquote>
+
+ Example of a server's code, receiving a basic message and sending an
+ echo message to the client:
+
+ \code
+#include <cstdlib>
+#include <iostream>
+#include <iterator>
+#include <sstream>
+#include <vector>
+#include <visp3/core/vpUDPServer.h>
+
+int main() {
+ try {
+ int port = 50037;
+ vpUDPServer server(port);
+
+ while (true) {
+ 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;
+
+ //Get address and port
+ std::istringstream iss(hostInfo);
+ std::vector<std::string> tokens;
+ std::copy(std::istream_iterator<std::string>(iss),
+ std::istream_iterator<std::string>(),
+ std::back_inserter(tokens));
+ server.send("Echo: " + msg, tokens[1], atoi(tokens[2].c_str()));
+ } else if (res == 0) {
+ std::cout << "Receive timeout" << std::endl;
+ } else {
+ std::cerr << "Error server.receive()!" << std::endl;
+ }
+ }
+
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+ \endcode
+
+ If you want to send a complex data type, you can either send the ASCII
+representation or send directly the byte data. In the last case, you should
+have to handle that both the server and the client have the same data type
+representation. Be careful also with the endianness of the network / host.
+
+ Here an example using a structure of data, assuming that both the server and
+the client have the same architecture (probably you should write your own
+serialization / deserialization functions for the data you want to send /
+receive):
+
+ \code
+#include <cstdlib>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <sstream>
+#include <vector>
+#include <visp3/core/vpUDPServer.h>
+
+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) {}
+};
+
+int main() {
+ try {
+ int port = 50037;
+ vpUDPServer server(port);
+
+ std::string msg = "", hostInfo = "";
+ 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 << " from: " << hostInfo << std::endl;
+
+ //Get address and port
+ std::istringstream iss(hostInfo);
+ std::vector<std::string> tokens;
+ std::copy(std::istream_iterator<std::string>(iss),
+ std::istream_iterator<std::string>(),
+ 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));
+
+ server.send(msg, tokens[1], atoi(tokens[2].c_str()));
+ }
+
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
+ \endcode
+
+ \sa vpUDPServer
+*/
+class VISP_EXPORT vpUDPServer
+{
+public:
+ vpUDPServer(const int port);
+ vpUDPServer(const std::string &hostname, const int port);
+ ~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);
+
+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
+ int m_socketFileDescriptor;
+#else
+ SOCKET m_socketFileDescriptor;
+ WSADATA m_wsa;
+#endif
+
+ void init(const std::string &hostname, const int port);
+};
+
+#endif
diff --git a/modules/core/include/visp3/core/vpUniRand.h b/modules/core/include/visp3/core/vpUniRand.h
index e20b93b..c84aa2f 100644
--- a/modules/core/include/visp3/core/vpUniRand.h
+++ b/modules/core/include/visp3/core/vpUniRand.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -63,31 +64,32 @@ int main()
*/
class VISP_EXPORT vpUniRand
{
- long a;
- long m; //2^31-1
- long q; //integer part of m/a
- long r;//r=m mod a
- double normalizer; //we use a normalizer > m to ensure ans will never be 1 (it is the case if x = 739806647)
+ long a;
+ long m; // 2^31-1
+ long q; // integer part of m/a
+ long r; // r=m mod a
+ double normalizer; // we use a normalizer > m to ensure ans will never be 1
+ // (it is the case if x = 739806647)
private:
void draw0();
+
protected:
long x;
double draw1();
public:
//! Default constructor.
- vpUniRand(const long seed = 0)
- : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), x((seed)? seed : 739806647)
- {}
+ explicit vpUniRand(const long seed = 0)
+ : a(16807), m(2147483647), q(127773), r(2836), normalizer(2147484721.0), x((seed) ? seed : 739806647)
+ {
+ }
//! Default destructor.
- virtual ~vpUniRand() {};
+ virtual ~vpUniRand(){};
//! Operator that allows to get a random value.
- double operator()() {
- return draw1();
- }
+ double operator()() { return draw1(); }
};
#endif
diff --git a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h
index ab93e82..923d800 100644
--- a/modules/core/include/visp3/core/vpVelocityTwistMatrix.h
+++ b/modules/core/include/visp3/core/vpVelocityTwistMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,9 +41,9 @@
#define vpVelocityRwistMatrix_h
#include <visp3/core/vpArray2D.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRotationMatrix.h>
class vpHomogeneousMatrix;
@@ -53,17 +54,13 @@ class vpColVector;
\ingroup group_core_transformations
- \brief Implementation of a velocity twist matrix and operations on such kind of matrices.
-
- Class that consider the particular case of twist
- transformation matrix that allows to transform a velocity skew from
- one frame to an other.
+ This class derived from vpArray2D<double> implements the 6 by 6 matrix which
+transforms velocities from one frame to another. This matrix is also called
+velocity twist transformation matrix.
- The vpVelocityTwistMatrix class is derived from vpArray2D<double>.
-
- A twist transformation matrix is a 6x6 matrix that express a velocity in frame <em>a</em> knowing
- velocity in <em>b</em>. This matrix is defined as:
- \f[
+ The full velocity twist transformation matrix allows to compute the velocity
+at point <em>a</em> expressed in frame <em>a</em> knowing its velocity at
+point <em>b</em> expressed in frame <em>b</em>. This matrix is defined as: \f[
^a{\bf V}_b = \left[\begin{array}{cc}
^a{\bf R}_b & [^a{\bf t}_b]_\times \; ^a{\bf R}_b\\
{\bf 0}_{3\times 3} & ^a{\bf R}_b
@@ -74,10 +71,21 @@ class vpColVector;
where \f$ ^a{\bf R}_b \f$ is a rotation matrix and
\f$ ^a{\bf t}_b \f$ is a translation vector.
- The vpVelocityTwistMatrix is derived from vpArray2D.
+ When the point where the velocity is expressed doesn't change, the matrix
+becomes block diagonal. It allows than to compute the velocity at point
+ <em>b</em> expressed in frame <em>a</em> knowing its
+ velocity at point <em>b</em> expressed in frame <em>b</em> :
+ \f[
+ ^a{\bf V}_b = \left[\begin{array}{cc}
+ ^a{\bf R}_b & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & ^a{\bf R}_b
+ \end{array}
+ \right]
+ \f]
- The code belows shows for example how to convert a velocity skew
- from camera frame to a fix frame.
+ The code below shows how to convert a velocity skew
+ expressed at the origin of the camera frame into the origin of the fix frame
+using the full velocity twist matrix.
\code
#include <visp3/core/vpColVector.h>
@@ -85,17 +93,18 @@ class vpColVector;
int main()
{
- vpVelocityTwistMatrix fVc; // Twist transformation matrix from fix to camera frame
+ vpVelocityTwistMatrix fVc; // Twist transformation matrix from fix to camera
+frame
vpHomogeneousMatrix fMc; // Fix to camera frame transformation
// ... fMc need here to be initialized
- fVc.buildFrom(fMc);
-
- vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
+ fVc.buildFrom(fMc);
+
+ vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
// ... c_v should here have an initial value
- vpColVector f_v(6); // Velocity in the fix frame: vx,vy,vz,wx,wy,wz
+ vpColVector f_v(6); // Velocity in the fix frame: vx,vy,vz,wx,wy,wz
// Compute the velocity in the fix frame
f_v = fVc * c_v;
@@ -106,56 +115,58 @@ class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D<double>
{
friend class vpMatrix;
- public:
+public:
// basic constructor
vpVelocityTwistMatrix();
// copy constructor
vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V);
// constructor from an homogeneous transformation
- vpVelocityTwistMatrix(const vpHomogeneousMatrix &M);
+ explicit vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, bool full = true);
- // Construction from Translation and rotation (ThetaU parameterization)
- vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau) ;
// Construction from Translation and rotation (matrix parameterization)
vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R);
- vpVelocityTwistMatrix(const double tx, const double ty, const double tz,
- const double tux, const double tuy, const double tuz);
+ // 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 vpRotationMatrix &R);
+ vpVelocityTwistMatrix(const vpThetaUVector &thetau);
+
/*!
Destructor.
*/
- virtual ~vpVelocityTwistMatrix() {};
-
+ virtual ~vpVelocityTwistMatrix(){};
- vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R);
- vpVelocityTwistMatrix buildFrom(const vpTranslationVector &t,
- const vpThetaUVector &thetau);
- vpVelocityTwistMatrix buildFrom(const vpHomogeneousMatrix &M) ;
+ 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);
- void extract( vpRotationMatrix &R) const;
+ void extract(vpRotationMatrix &R) const;
void extract(vpTranslationVector &t) const;
// Basic initialisation (identity)
- void eye() ;
+ void eye();
- vpVelocityTwistMatrix inverse() const ;
+ vpVelocityTwistMatrix inverse() const;
void inverse(vpVelocityTwistMatrix &V) const;
- vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const ;
- vpMatrix operator*(const vpMatrix &M) const ;
- vpColVector operator*(const vpColVector &v) const ;
+ vpVelocityTwistMatrix operator*(const vpVelocityTwistMatrix &V) const;
+ vpMatrix operator*(const vpMatrix &M) const;
+ vpColVector operator*(const vpColVector &v) const;
vpVelocityTwistMatrix &operator=(const vpVelocityTwistMatrix &V);
- int print(std::ostream& s, unsigned int length, char const* intro=0) const;
+ 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.
+ 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;
@@ -172,13 +183,13 @@ class VISP_EXPORT vpVelocityTwistMatrix : public vpArray2D<double>
\deprecated Provided only for compat with previous releases.
This function does nothing.
*/
- vp_deprecated void init() {};
+ vp_deprecated void init(){};
/*!
\deprecated You should rather use eye().
*/
vp_deprecated void setIdentity();
- //@}
+//@}
#endif
-} ;
+};
#endif
diff --git a/modules/core/include/visp3/core/vpXmlParser.h b/modules/core/include/visp3/core/vpXmlParser.h
index 2fbf033..8da542f 100644
--- a/modules/core/include/visp3/core/vpXmlParser.h
+++ b/modules/core/include/visp3/core/vpXmlParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,7 +43,7 @@
\file vpXmlParser.h
\brief Tools to simplify the creation of xml parser based on the libXML2
*/
-
+
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_XML2
@@ -51,36 +52,36 @@
#include <libxml/parser.h>
-#include <string>
-#include <sstream>
#include <iomanip>
-#include <typeinfo>
#include <map>
+#include <sstream>
#include <string.h>
+#include <string>
+#include <typeinfo>
-
/*!
\class vpXmlParser
- \brief This class intends to simplify the creation of xml parser based on the
- libxml2 third party library.
-
- This class can be useful to manage external data parameters (for example for
+ \brief This class intends to simplify the creation of xml parser based on
+ the libxml2 third party library.
+
+ This class can be useful to manage external data parameters (for example for
configuration of an experiment, ...).
- \warning This class is only available if libxml2 is installed and detected by ViSP.
- Installation instructions are provided here https://visp.inria.fr/3rd_xml2.
+ \warning This class is only available if libxml2 is installed and detected
+ by ViSP. Installation instructions are provided here
+ https://visp.inria.fr/3rd_xml2.
- In order to use this class, you have to create a new class inheriting from this one.
- In the child class, you have to implement the methods:
+ In order to use this class, you have to create a new class inheriting from
+ this one. In the child class, you have to implement the methods:
- writeMainClass()
- readMainClass()
-
- These two methods depends on the data to parse, and must not be directly called
- (they are called from the parse() and the save() methods).
-
+
+ These two methods depends on the data to parse, and must not be directly
+ called (they are called from the parse() and the save() methods).
+
Following is an example of implementation for the document:
-
+
\code
<config>
<range>5</range>
@@ -88,9 +89,9 @@
<size_filter>3</size_filter>
</config>
\endcode
-
+
A class to parse this document is declared as follows:
-
+
\code
class vpDataParser: public vpXmlParser
{
@@ -105,64 +106,65 @@
step,
size_filter
}dataToParse
-
+
vpDataParser(){
nodeMap["config"] = config;
nodeMap["range"] = range;
nodeMap["step"] = step;
nodeMap["size_filter"] = size_filter;
}
-
+
virtual void writeMainClass(xmlNodePtr node);
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
-
+
// additionals methods specific to the data to parse
// such as: accessors
}
\endcode
-
+
The readMainClass function implementation is:
-
+
\code
- void
+ 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<std::string, int>::iterator iter= this->nodeMap.find((char*)tmpNode->name);
- if(iter == nodeMap.end()){
+ std::map<std::string, int>::iterator iter = this->nodeMap.find((char*)tmpNode->name);
+ if(iter == nodeMap.end()) {
continue;
}
switch (iter->second){
case range:
- this->m_range = xmlReadIntChild(doc, tmpNode);
- break;
+ 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;
}
}
}
}
\endcode
-
- Data can now be accessed through the internal variables of the class vpDataParser.
-
- To store the data in a xml file, the function save has to be called. This
+
+ Data can now be accessed through the internal variables of the class
+ vpDataParser.
+
+ To store the data in a xml file, the function save has to be called. This
function needs the implementation of the writeMainClass function.
-
+
For example,
-
+
\code
- void
+ void
vpDataParser::writeMainClass(xmlNodePtr node)
{
xmlWriteIntChild(node, "range", m_range);
@@ -170,51 +172,51 @@
xmlWriteIntChild(node, "size_filter", m_size_filter);
}
\endcode
-
+
*/
class VISP_EXPORT vpXmlParser
-{
+{
protected:
/** @name Protected Member Functions Inherited from vpXmlParser */
//@{
/*!
pure virtual method used to read the document.
- As the content of the function depends on the structure of the file to read,
- data name, data types and data values, it has to be reimplemented for every
- type of filenam
+ As the content of the function depends on the structure of the file to
+ read, data name, data types and data values, it has to be reimplemented
+ for every type of filenam
\param doc : a pointer representing the document
\param node : the root node of the document
*/
- virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node)=0;
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node) = 0;
/*!
pure virtual method used to write the document.
- As the content of the function depends on the structure of the file to read,
- data name and data types, it has to be reimplemented for every
- type of file to parse.
+ As the content of the function depends on the structure of the file to
+ read, data name and data types, it has to be reimplemented for every type
+ of file to parse.
\param node : the root node of the document
*/
- virtual void writeMainClass(xmlNodePtr node)=0;
-
- bool xmlReadBoolChild (xmlDocPtr doc, xmlNodePtr node);
- char* xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node);
- double xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node);
- float xmlReadFloatChild (xmlDocPtr doc, xmlNodePtr node);
- int xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node);
- 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 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);
+ virtual void writeMainClass(xmlNodePtr node) = 0;
+
+ bool xmlReadBoolChild(xmlDocPtr doc, xmlNodePtr node);
+ char *xmlReadCharChild(xmlDocPtr doc, xmlNodePtr node);
+ double xmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node);
+ float xmlReadFloatChild(xmlDocPtr doc, xmlNodePtr node);
+ int xmlReadIntChild(xmlDocPtr doc, xmlNodePtr node);
+ 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 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);
//@}
protected:
@@ -232,18 +234,18 @@ public:
/** @name Public Member Functions Inherited from vpXmlParser */
//@{
vpXmlParser();
- vpXmlParser(const vpXmlParser& _twin);
+ vpXmlParser(const vpXmlParser &_twin);
virtual ~vpXmlParser();
- /* virtual */ void parse(const std::string& filename);
- /* virtual */ void save(const std::string& filename, const bool append=false);
+ /* virtual */ void parse(const std::string &filename);
+ /* virtual */ void save(const std::string &filename, const bool append = false);
/*!
- Set the map describing the data to parse. This map stores the name of each
+ Set the map describing the data to parse. This map stores the name of each
node and an associated key used to simplify the parsing of the file.
-
+
If the following file want to be parsed:
-
+
\code
<config>
<range>5</range>
@@ -251,9 +253,9 @@ public:
<size_filter>3</size_filter>
</config>
\endcode
-
+
The following map has to be declared:
-
+
\code
std::map dataToParse;
dataToParse["config"] = 0;
@@ -261,59 +263,54 @@ public:
dataToParse["step"] = 2;
dataToParse["size_filter"] = 3;
\endcode
-
- Or, you can use keyzord instead of number as key but it implies to declare
+
+ Or, you can use keyzord instead of number as key but it implies to declare
in the child class an enumeration type of the name. For example:
-
+
\code
typedef enum{
config,
range,
step,
size_filter} data_enum;
-
+
std::map dataToParse;
dataToParse["config"] = config;
dataToParse["range"] = range;
dataToParse["step"] = step;
dataToParse["size_filter"] = size_filter;
\endcode
-
+
\param _map : the map describing the data to parse
*/
- void setMap(const std::map<std::string, int>& _map){ nodeMap = _map;}
-
+ void setMap(const std::map<std::string, int> &_map) { nodeMap = _map; }
+
/*!
set the name of the main tag
-
- The main tag corresponds to the name of the root node
-
+
+ The main tag corresponds to the name of the root node
+
\param tag : name of the root node of the document
*/
- inline void setMainTag(const std::string& tag){ main_tag = tag;}
+ inline void setMainTag(const std::string &tag) { main_tag = tag; }
//@}
/** @name Static Public Member Functions Inherited from vpXmlParser */
//@{
/*!
As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
- to clean up memory allocated by the xml2 library itself, the user should call
- xmlCleanupParser() only when the process has finished using the xml2 library.
- In case of doubt abstain from calling this function or do it just before calling exit()
- to avoid leak reports from valgrind ! That's why
- in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
- function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
- just before exit().
+ to clean up memory allocated by the xml2 library itself, the user should
+ call xmlCleanupParser() only when the process has finished using the xml2
+ library. In case of doubt abstain from calling this function or do it just
+ before calling exit() to avoid leak reports from valgrind ! That's why in
+ ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the
+ static function vpXmlParser::cleanup() that calls xmlCleanupParser() that
+ could be called just before exit().
*/
- static void cleanup()
- {
- xmlCleanupParser();
- }
+ static void cleanup() { xmlCleanupParser(); }
//@}
-
};
-
#endif /* VISP_HAVE_XML2 */
#endif
diff --git a/modules/core/include/visp3/core/vpXmlParserCamera.h b/modules/core/include/visp3/core/vpXmlParserCamera.h
index 3674dd3..af47c71 100644
--- a/modules/core/include/visp3/core/vpXmlParserCamera.h
+++ b/modules/core/include/visp3/core/vpXmlParserCamera.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpXmlParserCamera.h
\brief Declaration of the vpXmlParserCamera class.
@@ -44,7 +43,6 @@
*/
-
#ifndef vpXMLPARSERCAMERA_H
#define vpXMLPARSERCAMERA_H
@@ -52,10 +50,10 @@
#ifdef VISP_HAVE_XML2
+#include <libxml/xmlmemory.h> /* Functions of libxml. */
#include <string>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpXmlParser.h>
-#include <libxml/xmlmemory.h> /* Functions of libxml. */
/*!
\class vpXmlParserCamera
@@ -64,8 +62,9 @@
\brief XML parser to load and save intrinsic camera parameters.
- \warning This class is only available if libxml2 is installed and detected by ViSP.
- Installation instructions are provided here https://visp.inria.fr/3rd_xml2.
+ \warning This class is only available if libxml2 is installed and detected
+by ViSP. Installation instructions are provided here
+https://visp.inria.fr/3rd_xml2.
To have a complete description of the camera parameters and the
corresponding projection model implemented in ViSP, see
@@ -112,15 +111,16 @@ int main()
#ifdef VISP_HAVE_XML2
vpXmlParserCamera p; // Create a XML parser
- vpCameraParameters::vpCameraParametersProjType projModel; // Projection model
+ vpCameraParameters::vpCameraParametersProjType projModel; // Projection
+model
// Use a perspective projection model without distortion
projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
- // Parse the xml file "myXmlFile.xml" to find the intrinsic camera
+ // Parse the xml file "myXmlFile.xml" to find the intrinsic camera
// parameters of the camera named "myCamera" for the image sizes 640x480,
// for the projection model projModel. The size of the image is optional
// if camera parameters are given only for one image size.
if (p.parse(cam, "myXmlFile.xml", "myCamera", projModel,640,480) != vpXmlParserCamera::SEQUENCE_OK) {
- std::cout << "Cannot found myCamera" << std::endl;
+ std::cout << "Cannot found myCamera" << std::endl;
}
// cout the parameters
@@ -155,10 +155,10 @@ int main()
int main()
{
- // Create a camera parameter container. We want to set these parameters
- // for a 320x240 image, and we want to use the perspective projection
+ // Create a camera parameter container. We want to set these parameters
+ // for a 320x240 image, and we want to use the perspective projection
// modelisation without distortion.
- vpCameraParameters cam;
+ vpCameraParameters cam;
// Set the principal point coordinates (u0,v0)
double u0 = 162.3;
@@ -166,7 +166,7 @@ int main()
// Set the pixel ratio (px, py)
double px = 563.2;
double py = 564.1;
-
+
// Set the camera parameters for a model without distortion
cam.initPersProjWithoutDistortion(px, py, u0, v0);
@@ -185,43 +185,37 @@ int main()
\endcode
*/
-class VISP_EXPORT vpXmlParserCamera: public vpXmlParser
+class VISP_EXPORT vpXmlParserCamera : public vpXmlParser
{
public:
+ /* --- XML Code------------------------------------------------------------
+ */
+ typedef enum {
+ CODE_XML_BAD = -1,
+ CODE_XML_OTHER,
+ CODE_XML_CAMERA,
+ CODE_XML_CAMERA_NAME,
+ CODE_XML_HEIGHT,
+ CODE_XML_WIDTH,
+ CODE_XML_SUBSAMPLING_WIDTH,
+ CODE_XML_SUBSAMPLING_HEIGHT,
+ CODE_XML_FULL_HEIGHT,
+ CODE_XML_FULL_WIDTH,
+ CODE_XML_MODEL,
+ CODE_XML_MODEL_TYPE,
+ CODE_XML_U0,
+ CODE_XML_V0,
+ CODE_XML_PX,
+ CODE_XML_PY,
+ CODE_XML_KUD,
+ CODE_XML_KDU,
+ CODE_XML_ADDITIONAL_INFO
+ } vpXmlCodeType;
+
+ typedef enum { SEQUENCE_OK, SEQUENCE_ERROR } vpXmlCodeSequenceType;
- /* --- XML Code------------------------------------------------------------ */
- typedef enum
- {
- CODE_XML_BAD = -1,
- CODE_XML_OTHER,
- CODE_XML_CAMERA,
- CODE_XML_CAMERA_NAME,
- CODE_XML_HEIGHT,
- CODE_XML_WIDTH,
- CODE_XML_SUBSAMPLING_WIDTH,
- CODE_XML_SUBSAMPLING_HEIGHT,
- CODE_XML_FULL_HEIGHT,
- CODE_XML_FULL_WIDTH,
- CODE_XML_MODEL,
- CODE_XML_MODEL_TYPE,
- CODE_XML_U0,
- CODE_XML_V0,
- CODE_XML_PX,
- CODE_XML_PY,
- CODE_XML_KUD,
- CODE_XML_KDU,
- CODE_XML_ADDITIONAL_INFO
- } vpXmlCodeType;
-
- typedef enum
- {
- SEQUENCE_OK ,
- SEQUENCE_ERROR
- } vpXmlCodeSequenceType;
-
-private :
-
+private:
vpCameraParameters camera;
std::string camera_name;
unsigned int image_width;
@@ -231,129 +225,93 @@ private :
unsigned int full_width;
unsigned int full_height;
- //! Allowed size difference between input image and data from the xml parser to handle minor differences (ex. FORMAT7 can creates 648*488 images).
+ //! Allowed size difference between input image and data from the xml parser
+ //! to handle minor differences (ex. FORMAT7 can creates 648*488 images).
static const int allowedPixelDiffOnImageSize = 15;
public:
-
vpXmlParserCamera();
- vpXmlParserCamera(vpXmlParserCamera& twinParser);
+ vpXmlParserCamera(vpXmlParserCamera &twinParser);
//! Default destructor.
- virtual ~vpXmlParserCamera(){}
+ virtual ~vpXmlParserCamera() {}
// get/set functions
- std::string getCameraName(){return this->camera_name;}
- vpCameraParameters getCameraParameters(){return this->camera;}
- unsigned int getHeight(){ return this->image_height; }
- unsigned int getSubsampling_width(){return this->subsampling_width;}
- unsigned int getSubsampling_height(){return this->subsampling_height;}
- unsigned int getWidth(){ return this->image_width; }
-
- 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, 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 setWidth(const unsigned int width){ this->image_width = width ; }
+ std::string getCameraName() { return this->camera_name; }
+ vpCameraParameters getCameraParameters() { return this->camera; }
+ unsigned int getHeight() { return this->image_height; }
+ unsigned int getSubsampling_width() { return this->subsampling_width; }
+ unsigned int getSubsampling_height() { return this->subsampling_height; }
+ unsigned int getWidth() { return this->image_width; }
+
+ 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,
+ 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 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 unsigned int subsampling_height = 0);
-
- int count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- int read_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const vpCameraParameters::vpCameraParametersProjType &projModel,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- xmlNodePtr find_camera (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- xmlNodePtr find_additional_info (xmlNodePtr node);
-
- vpXmlCodeSequenceType read_camera_model (xmlDocPtr doc, xmlNodePtr node,
- vpCameraParameters &camera);
-
- int read_camera_header (xmlDocPtr doc, xmlNodePtr node,
- const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
-
- static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
- void myXmlReadIntChild (xmlDocPtr doc,
- xmlNodePtr node,
- int &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadDoubleChild (xmlDocPtr doc,
- xmlNodePtr node,
- double &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadCharChild (xmlDocPtr doc,
- xmlNodePtr node,
- char **res);
- int write (xmlNodePtr node, const std::string& camera_name,
- const unsigned int image_width = 0,
- const unsigned int image_height = 0,
- const unsigned int subsampling_width = 0,
- const unsigned int subsampling_height = 0);
+ int read(xmlDocPtr doc, xmlNodePtr node, const std::string &camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width = 0,
+ const unsigned int image_height = 0, const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ int count(xmlDocPtr doc, xmlNodePtr node, const std::string &camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width = 0,
+ const unsigned int image_height = 0, const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ int read_camera(xmlDocPtr doc, xmlNodePtr node, const std::string &camera_name,
+ const vpCameraParameters::vpCameraParametersProjType &projModel, const unsigned int image_width = 0,
+ const unsigned int image_height = 0, const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
+
+ xmlNodePtr find_camera(xmlDocPtr doc, xmlNodePtr node, const std::string &camera_name,
+ const unsigned int image_width = 0, const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0, const unsigned int subsampling_height = 0);
+
+ xmlNodePtr find_additional_info(xmlNodePtr node);
+
+ vpXmlCodeSequenceType read_camera_model(xmlDocPtr doc, xmlNodePtr node, vpCameraParameters &camera);
+
+ int read_camera_header(xmlDocPtr doc, xmlNodePtr node, const std::string &camera_name,
+ const unsigned int image_width = 0, const unsigned int image_height = 0,
+ const unsigned int subsampling_width = 0, const unsigned int subsampling_height = 0);
+
+ static vpXmlCodeSequenceType str2xmlcode(char *str, vpXmlCodeType &res);
+ void myXmlReadIntChild(xmlDocPtr doc, xmlNodePtr node, int &res, vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node, double &res, vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadCharChild(xmlDocPtr doc, xmlNodePtr node, char **res);
+ int write(xmlNodePtr node, const std::string &camera_name, const unsigned int image_width = 0,
+ const unsigned int image_height = 0, const unsigned int subsampling_width = 0,
+ const unsigned int subsampling_height = 0);
int write_camera(xmlNodePtr node_camera);
-
-private:
+private:
/*!
-
+
\param 2doc : a pointer representing the document
\param node : the root node of the document
*/
- virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
-
+ virtual void readMainClass(xmlDocPtr, xmlNodePtr){};
+
/*!
-
+
\param node2 : the root node of the document
*/
- virtual void writeMainClass(xmlNodePtr ){};
-
+ virtual void writeMainClass(xmlNodePtr){};
};
-#endif //VISP_HAVE_XML2
+#endif // VISP_HAVE_XML2
#endif
diff --git a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h
index 9747f7b..5077b57 100644
--- a/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h
+++ b/modules/core/include/visp3/core/vpXmlParserHomogeneousMatrix.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,7 +39,8 @@
/*!
\file vpXmlParserHomogeneousMatrix.h
\brief Declaration of the vpXmlParserHomogeneousMatrix class.
- Class vpXmlParserHomogeneousMatrix allowed to load and save Homogeneous Matrixes in a file XML
+ Class vpXmlParserHomogeneousMatrix allowed to load and save Homogeneous
+ Matrixes in a file XML
*/
@@ -49,10 +51,10 @@
#ifdef VISP_HAVE_XML2
+#include <libxml/xmlmemory.h> /* Functions of libxml. */
#include <string>
-#include <visp3/core/vpXmlParser.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <libxml/xmlmemory.h> /* Functions of libxml. */
+#include <visp3/core/vpXmlParser.h>
/*!
\class vpXmlParserHomogeneousMatrix
@@ -61,11 +63,12 @@
\brief XML parser to load and save an homogeneous matrix in a file.
- \warning This class is only available if libxml2 is installed and detected by ViSP.
- Installation instructions are provided here https://visp.inria.fr/3rd_xml2.
+ \warning This class is only available if libxml2 is installed and detected
+by ViSP. Installation instructions are provided here
+https://visp.inria.fr/3rd_xml2.
- To have a complete description of the homogeneous matrix implemented in ViSP, see
- vpHomogeneousMatrix.
+ To have a complete description of the homogeneous matrix implemented in
+ViSP, see vpHomogeneousMatrix.
Example of an XML file "homogeneous_matrixes.xml" containing a Pose vector
that will be converted in an homogeneous matrix:
@@ -81,9 +84,8 @@
<tx>1.00</tx>
<ty>1.30</ty>
<tz>3.50</tz>
- <!--Rotational vector expressed in angle axis representation with values in radians -->
- <theta_ux>0.20</theta_ux>
- <theta_uy>0.30</theta_uy>
+ <!--Rotational vector expressed in angle axis representation with values
+in radians --> <theta_ux>0.20</theta_ux> <theta_uy>0.30</theta_uy>
<theta_uz>0.50</theta_uz>
</values>
</homogeneous_transformation>
@@ -160,14 +162,13 @@ int main(int argc, char* argv[])
\endcode
*/
-class VISP_EXPORT vpXmlParserHomogeneousMatrix: public vpXmlParser
+class VISP_EXPORT vpXmlParserHomogeneousMatrix : public vpXmlParser
{
public:
-
- /* --- XML Code------------------------------------------------------------ */
- typedef enum
- {
+ /* --- XML Code------------------------------------------------------------
+ */
+ typedef enum {
CODE_XML_BAD = -1,
CODE_XML_OTHER,
CODE_XML_M,
@@ -181,82 +182,60 @@ public:
CODE_XML_TUZ
} vpXmlCodeType;
- typedef enum
- {
- SEQUENCE_OK,
- SEQUENCE_ERROR
- } vpXmlCodeSequenceType;
-
-private :
+ typedef enum { SEQUENCE_OK, SEQUENCE_ERROR } vpXmlCodeSequenceType;
+private:
vpHomogeneousMatrix m_M;
std::string m_name;
public:
-
vpXmlParserHomogeneousMatrix();
- vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix& twinParser);
+ vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneousMatrix &twinParser);
//! Default destructor.
- virtual ~vpXmlParserHomogeneousMatrix(){}
+ virtual ~vpXmlParserHomogeneousMatrix() {}
// get/set functions
- vpHomogeneousMatrix getHomogeneousMatrix() const {return this->m_M;}
- std::string getHomogeneousMatrixName() const {return this->m_name;}
+ vpHomogeneousMatrix getHomogeneousMatrix() const { return this->m_M; }
+ std::string getHomogeneousMatrixName() const { return this->m_name; }
- vpXmlParserHomogeneousMatrix& operator =(const vpXmlParserHomogeneousMatrix& twinparser);
+ vpXmlParserHomogeneousMatrix &operator=(const vpXmlParserHomogeneousMatrix &twinparser);
int parse(vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
int save(const vpHomogeneousMatrix &M, const std::string &filename, const std::string &name);
- void setHomogeneousMatrixName(const std::string& name){
- this->m_name = name;
- }
+ void setHomogeneousMatrixName(const std::string &name) { this->m_name = name; }
private:
- int read (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- int count (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- int read_matrix (xmlDocPtr doc, xmlNodePtr node,
- const std::string& name);
-
- vpXmlCodeSequenceType read_values (xmlDocPtr doc, xmlNodePtr node,
- vpHomogeneousMatrix &M);
-
- static vpXmlCodeSequenceType str2xmlcode (char * str, vpXmlCodeType & res);
- void myXmlReadIntChild (xmlDocPtr doc,
- xmlNodePtr node,
- int &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadDoubleChild (xmlDocPtr doc,
- xmlNodePtr node,
- double &res,
- vpXmlCodeSequenceType &code_error);
-
- void myXmlReadCharChild (xmlDocPtr doc,
- xmlNodePtr node,
- char **res);
- int write (xmlNodePtr node, const std::string& name);
+ int read(xmlDocPtr doc, xmlNodePtr node, const std::string &name);
-private:
+ int count(xmlDocPtr doc, xmlNodePtr node, const std::string &name);
+ int read_matrix(xmlDocPtr doc, xmlNodePtr node, const std::string &name);
+
+ vpXmlCodeSequenceType read_values(xmlDocPtr doc, xmlNodePtr node, vpHomogeneousMatrix &M);
+
+ static vpXmlCodeSequenceType str2xmlcode(char *str, vpXmlCodeType &res);
+ void myXmlReadIntChild(xmlDocPtr doc, xmlNodePtr node, int &res, vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node, double &res, vpXmlCodeSequenceType &code_error);
+
+ void myXmlReadCharChild(xmlDocPtr doc, xmlNodePtr node, char **res);
+ int write(xmlNodePtr node, const std::string &name);
+
+private:
/*!
\param 2doc : a pointer representing the document
\param node : the root node of the document
*/
- virtual void readMainClass(xmlDocPtr , xmlNodePtr ){};
-
+ virtual void readMainClass(xmlDocPtr, xmlNodePtr){};
+
/*!
-
+
\param node2 : the root node of the document
*/
- virtual void writeMainClass(xmlNodePtr ){};
-
+ virtual void writeMainClass(xmlNodePtr){};
};
-#endif //VISP_HAVE_XML2
+#endif // VISP_HAVE_XML2
#endif
diff --git a/modules/core/src/camera/vpCameraParameters.cpp b/modules/core/src/camera/vpCameraParameters.cpp
index 3199b81..14c4a60 100644
--- a/modules/core/src/camera/vpCameraParameters.cpp
+++ b/modules/core/src/camera/vpCameraParameters.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\file vpCameraParameters.cpp
\brief Definition of the vpCameraParameters class member functions.
@@ -44,15 +44,15 @@
*/
+#include <cmath>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <sstream>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <cmath>
-#include <limits>
-#include <iostream>
-#include <sstream>
-#include <iomanip>
const double vpCameraParameters::DEFAULT_PX_PARAMETER = 600.0;
const double vpCameraParameters::DEFAULT_PY_PARAMETER = 600.0;
@@ -60,8 +60,7 @@ 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 =
+const vpCameraParameters::vpCameraParametersProjType vpCameraParameters::DEFAULT_PROJ_TYPE =
vpCameraParameters::perspectiveProjWithoutDistortion;
/*!
@@ -71,32 +70,24 @@ 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() ;
+ init();
}
/*!
Copy constructor
*/
vpCameraParameters::vpCameraParameters(const vpCameraParameters &c)
- :
- px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER),
- u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
- kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER),
- width(0), height(0),
- isFov(false), m_hFovAngle(0), m_vFovAngle(0), fovNormals(),
- inv_px(1./DEFAULT_PX_PARAMETER), inv_py(1./DEFAULT_PY_PARAMETER),
+ : 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) ;
+ init(c);
}
/*!
@@ -106,18 +97,14 @@ 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, 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),
+vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py, const double cam_u0,
+ const double cam_v0)
+ : px(DEFAULT_PX_PARAMETER), py(DEFAULT_PY_PARAMETER), u0(DEFAULT_U0_PARAMETER), v0(DEFAULT_V0_PARAMETER),
+ kud(DEFAULT_KUD_PARAMETER), kdu(DEFAULT_KDU_PARAMETER), width(0), height(0), isFov(false), m_hFovAngle(0),
+ m_vFovAngle(0), fovNormals(), inv_px(1. / DEFAULT_PX_PARAMETER), inv_py(1. / DEFAULT_PY_PARAMETER),
projModel(DEFAULT_PROJ_TYPE)
{
- initPersProjWithoutDistortion(cam_px,cam_py,cam_u0,cam_v0) ;
+ initPersProjWithoutDistortion(cam_px, cam_py, cam_u0, cam_v0);
}
/*!
@@ -129,48 +116,38 @@ vpCameraParameters::vpCameraParameters(const double cam_px, const double cam_py,
\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);
}
/*!
\brief basic initialization with the default parameters
*/
-void
-vpCameraParameters::init()
-{
- if (fabs(this->px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+void vpCameraParameters::init()
+{
+ if (fabs(this->px) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- if (fabs(this->py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+ if (fabs(this->py) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- this->inv_px = 1./this->px;
- this->inv_py = 1./this->py;
+ this->inv_px = 1. / this->px;
+ this->inv_py = 1. / this->py;
}
/*!
Initialization with specific parameters using perpective projection without
distortion model.
- \param cam_px,cam_py : the ratio between the focal length and the size of a pixel.
- \param cam_u0,cam_v0 : principal point coordinates in pixels.
+ \param cam_px,cam_py : the ratio between the focal length and the size of a
+pixel. \param cam_u0,cam_v0 : principal point coordinates in pixels.
The following sample code shows how to use this function:
\code
@@ -189,7 +166,8 @@ int main()
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;
+ << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle())
+ << " degrees)" << std::endl;
}
\endcode
It produces the following output:
@@ -202,33 +180,28 @@ Field of view (horizontal: 56.145 and vertical: 43.6028 degrees)
\endcode
*/
-void
-vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py,
- const double cam_u0, const double cam_v0)
+void vpCameraParameters::initPersProjWithoutDistortion(const double cam_px, const double cam_py, const double cam_u0,
+ const double cam_v0)
{
- this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion ;
-
- this->px = cam_px ;
- this->py = cam_py ;
- this->u0 = cam_u0 ;
- this->v0 = cam_v0 ;
- this->kud = 0 ;
- this->kdu = 0 ;
-
- if (fabs(px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+ this->projModel = vpCameraParameters::perspectiveProjWithoutDistortion;
+
+ this->px = cam_px;
+ this->py = cam_py;
+ this->u0 = cam_u0;
+ this->v0 = cam_v0;
+ this->kud = 0;
+ this->kdu = 0;
+
+ if (fabs(px) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- if (fabs(py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+ if (fabs(py) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- this->inv_px = 1./px;
- this->inv_py = 1./py;
+ this->inv_px = 1. / px;
+ this->inv_py = 1. / py;
}
/*!
@@ -258,7 +231,8 @@ int main()
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;
+ << " and vertical: " << vpMath::deg(cam.getVerticalFovAngle())
+ << " degrees)" << std::endl;
}
\endcode
It produces the following output:
@@ -269,36 +243,31 @@ Camera parameters for perspective projection with distortion:
kud = -0.19
kdu = 0.2
-Field of view (horizontal: 56.14497387 and vertical: 43.60281897 degrees) \endcode
+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 ;
-
- this->px = cam_px ;
- this->py = cam_py ;
- this->u0 = cam_u0 ;
- this->v0 = cam_v0 ;
- this->kud = cam_kud ;
- this->kdu = cam_kdu ;
-
- if (fabs(px)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+ this->projModel = vpCameraParameters::perspectiveProjWithDistortion;
+
+ this->px = cam_px;
+ this->py = cam_py;
+ this->u0 = cam_u0;
+ this->v0 = cam_v0;
+ this->kud = cam_kud;
+ this->kdu = cam_kdu;
+
+ if (fabs(px) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- if (fabs(py)<1e-6)
- {
- vpERROR_TRACE("Camera parameter px = 0") ;
- throw(vpException(vpException::divideByZeroError,
- "Camera parameter px = 0")) ;
+ if (fabs(py) < 1e-6) {
+ vpERROR_TRACE("Camera parameter px = 0");
+ throw(vpException(vpException::divideByZeroError, "Camera parameter px = 0"));
}
- this->inv_px = 1./px;
- this->inv_py = 1./py;
+ this->inv_px = 1. / px;
+ this->inv_py = 1. / py;
}
/*!
@@ -306,50 +275,41 @@ vpCameraParameters::initPersProjWithDistortion(const double cam_px, const double
nothing much to destroy...
*/
-vpCameraParameters::~vpCameraParameters()
-{
-}
+vpCameraParameters::~vpCameraParameters() {}
/*!
initialization from another vpCameraParameters object
*/
-void
-vpCameraParameters::init(const vpCameraParameters &c)
-{
- *this = c ;
-}
-
+void vpCameraParameters::init(const vpCameraParameters &c) { *this = c; }
/*!
- initialise the camera from a calibration matrix.
+ initialise the camera from a calibration matrix.
Using a calibration matrix leads to a camera without distortion
-
+
The K matrix in parameters must be like:
-
+
\f$ K = \left(\begin{array}{ccc}
p_x & 0 & u_0 \\
0 & p_y & v_0 \\
0 & 0 & 1
\end{array} \right) \f$
-
+
\param _K : the 3by3 calibration matrix
*/
-void
-vpCameraParameters::initFromCalibrationMatrix(const vpMatrix& _K)
+void vpCameraParameters::initFromCalibrationMatrix(const vpMatrix &_K)
{
- if(_K.getRows() != 3 || _K.getCols() != 3 ){
+ if (_K.getRows() != 3 || _K.getCols() != 3) {
throw vpException(vpException::dimensionError, "bad size for calibration matrix");
}
- if( std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()){
+ if (std::fabs(_K[2][2] - 1.0) > std::numeric_limits<double>::epsilon()) {
throw vpException(vpException::badValue, "bad value: K[2][2] must be equal to 1");
}
- initPersProjWithoutDistortion (_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
+ initPersProjWithoutDistortion(_K[0][0], _K[1][1], _K[0][2], _K[1][2]);
}
/*!
- Initialize the camera model without distorsion from the image dimension and the camera field of view.
- \param w : Image width.
- \param h : Image height.
+ Initialize the camera model without distorsion from the image dimension and
+the camera field of view. \param w : Image width. \param h : Image height.
\param hfov : Camera horizontal field of view angle expressed in radians.
\param vfov : Camera vertical field of view angle expressed in radians.
@@ -380,67 +340,65 @@ 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.;
- v0 = (double)h/2.;
- px = u0 / tan(hfov/2);
- py = v0 / tan(vfov/2);
+ u0 = (double)w / 2.;
+ v0 = (double)h / 2.;
+ px = u0 / tan(hfov / 2);
+ py = v0 / tan(vfov / 2);
kud = 0;
kdu = 0;
- inv_px = 1./px;
- inv_py = 1./py;
+ inv_px = 1. / px;
+ inv_py = 1. / py;
computeFov(w, h);
}
/*!
copy operator
*/
-vpCameraParameters&
- vpCameraParameters::operator=(const vpCameraParameters& cam)
+vpCameraParameters &vpCameraParameters::operator=(const vpCameraParameters &cam)
{
- projModel = cam.projModel ;
- px = cam.px ;
- py = cam.py ;
- u0 = cam.u0 ;
- v0 = cam.v0 ;
- kud = cam.kud ;
- kdu = cam.kdu ;
-
- inv_px = cam.inv_px;
+ projModel = cam.projModel;
+ px = cam.px;
+ py = cam.py;
+ u0 = cam.u0;
+ v0 = cam.v0;
+ kud = cam.kud;
+ kdu = cam.kdu;
+
+ inv_px = cam.inv_px;
inv_py = cam.inv_py;
-
+
isFov = cam.isFov;
m_hFovAngle = cam.m_hFovAngle;
m_vFovAngle = cam.m_vFovAngle;
width = cam.width;
height = cam.height;
fovNormals = cam.fovNormals;
-
- return *this ;
+
+ return *this;
}
/*!
Compute angles and normals of the FOV.
-
+
\param w : Width of the image
\param h : Height of the image.
*/
-void
-vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
+void vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
{
- if( !isFov && w != width && h != height && w != 0 && h != 0){
+ if ((!isFov || w != width || h != height) && w != 0 && h != 0) {
fovNormals = std::vector<vpColVector>(4);
-
+
isFov = true;
- double hFovAngle = atan(((double)w - u0) * ( 1.0 / px ));
- double vFovAngle = atan(( v0 ) * ( 1.0 / py ));
- double minushFovAngle = atan(( u0 ) * ( 1.0 / px ));
- double minusvFovAngle = atan(((double)h - v0) * ( 1.0 / py ));
-
+ double hFovAngle = atan(((double)w - u0) * (1.0 / px));
+ double vFovAngle = atan((v0) * (1.0 / py));
+ double minushFovAngle = atan((u0) * (1.0 / px));
+ double minusvFovAngle = atan(((double)h - v0) * (1.0 / py));
+
width = w;
height = h;
@@ -448,8 +406,8 @@ vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
n = 0;
n[0] = 1.0;
- vpRotationMatrix Rleft(0,-minushFovAngle,0);
- vpRotationMatrix Rright(0,hFovAngle,0);
+ vpRotationMatrix Rleft(0, -minushFovAngle, 0);
+ vpRotationMatrix Rright(0, hFovAngle, 0);
vpColVector nLeft, nRight;
@@ -462,8 +420,8 @@ vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
n = 0;
n[1] = 1.0;
- vpRotationMatrix Rup(vFovAngle,0,0);
- vpRotationMatrix Rdown(-minusvFovAngle,0,0);
+ vpRotationMatrix Rup(vFovAngle, 0, 0);
+ vpRotationMatrix Rdown(-minusvFovAngle, 0, 0);
vpColVector nUp, nDown;
@@ -478,7 +436,6 @@ vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
}
}
-
/*!
Return the camera matrix \f$K\f$ given by:
@@ -490,17 +447,16 @@ vpCameraParameters::computeFov(const unsigned int &w, const unsigned int &h)
\sa get_K_inverse()
*/
-vpMatrix
-vpCameraParameters::get_K() const
+vpMatrix vpCameraParameters::get_K() const
{
vpMatrix K(3, 3, 0.);
- K[0][0] = px ;
- K[1][1] = py ;
- K[0][2] = u0 ;
- K[1][2] = v0 ;
- K[2][2] = 1.0 ;
+ K[0][0] = px;
+ K[1][1] = py;
+ K[0][2] = u0;
+ K[1][2] = v0;
+ K[2][2] = 1.0;
- return K;
+ return K;
}
/*!
Return the inverted camera matrix \f$K^{-1}\f$ given by:
@@ -513,47 +469,42 @@ vpCameraParameters::get_K() const
\sa get_K()
*/
-vpMatrix
-vpCameraParameters::get_K_inverse() const
+vpMatrix vpCameraParameters::get_K_inverse() const
{
vpMatrix K_inv(3, 3, 0.);
- K_inv[0][0] = inv_px ;
- K_inv[1][1] = inv_py ;
- K_inv[0][2] = -u0*inv_px ;
- K_inv[1][2] = -v0*inv_py ;
- K_inv[2][2] = 1.0 ;
+ K_inv[0][0] = inv_px;
+ K_inv[1][1] = inv_py;
+ K_inv[0][2] = -u0 * inv_px;
+ K_inv[1][2] = -v0 * inv_py;
+ K_inv[2][2] = 1.0;
return K_inv;
}
-
/*!
Print the camera parameters on the standard output
\sa operator<<(std::ostream &, const vpCameraParameters &)
*/
-void
-vpCameraParameters::printParameters()
+void vpCameraParameters::printParameters()
{
- std::ios::fmtflags original_flags( std::cout.flags() );
- switch(projModel){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- std::cout.precision(10);
- std::cout << "Camera parameters for perspective projection without distortion:"
- << std::endl ;
- std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
- std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
- break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- std::cout.precision(10);
- std::cout << "Camera parameters for perspective projection with distortion:"
- << std::endl ;
- std::cout << " px = " << px <<"\t py = "<< py << std::endl ;
- std::cout << " u0 = " << u0 <<"\t v0 = "<< v0 << std::endl ;
- std::cout << " kud = " << kud << std::endl ;
- std::cout << " kdu = " << kdu << std::endl ;
- break;
- }
+ std::ios::fmtflags original_flags(std::cout.flags());
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ std::cout.precision(10);
+ std::cout << "Camera parameters for perspective projection without distortion:" << std::endl;
+ std::cout << " px = " << px << "\t py = " << py << std::endl;
+ std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
+ break;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ std::cout.precision(10);
+ std::cout << "Camera parameters for perspective projection with distortion:" << std::endl;
+ std::cout << " px = " << px << "\t py = " << py << std::endl;
+ std::cout << " u0 = " << u0 << "\t v0 = " << v0 << std::endl;
+ std::cout << " kud = " << kud << std::endl;
+ std::cout << " kdu = " << kdu << std::endl;
+ break;
+ }
// Restore ostream format
std::cout.flags(original_flags);
}
@@ -566,31 +517,23 @@ vpCameraParameters::printParameters()
*/
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 ;
+ switch (cam.get_projModel()) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ os << "Camera parameters for perspective projection without distortion:" << std::endl;
+ os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
+ os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion:
std::ios_base::fmtflags original_flags = os.flags();
os.precision(10);
- os << "Camera parameters for perspective projection with distortion:"
- << std::endl ;
- os << " px = " << cam.get_px() <<"\t py = "<< cam.get_py()
- << std::endl ;
- os << " u0 = " << cam.get_u0() <<"\t v0 = "<< cam.get_v0()
- << std::endl ;
- os << " kud = " << cam.get_kud() << std::endl ;
- os << " kdu = " << cam.get_kdu() << std::endl ;
+ os << "Camera parameters for perspective projection with distortion:" << std::endl;
+ os << " px = " << cam.get_px() << "\t py = " << cam.get_py() << std::endl;
+ os << " u0 = " << cam.get_u0() << "\t v0 = " << cam.get_v0() << std::endl;
+ os << " kud = " << cam.get_kud() << std::endl;
+ os << " kdu = " << cam.get_kdu() << std::endl;
os.flags(original_flags); // restore os to standard state
break;
}
return os;
}
-
-
diff --git a/modules/core/src/camera/vpMeterPixelConversion.cpp b/modules/core/src/camera/vpMeterPixelConversion.cpp
index ff23780..e34cea3 100644
--- a/modules/core/src/camera/vpMeterPixelConversion.cpp
+++ b/modules/core/src/camera/vpMeterPixelConversion.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,45 +42,42 @@
\brief meter to pixel conversion
*/
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMeterPixelConversion.h>
//! Line coordinates conversion (rho,theta).
-void
-vpMeterPixelConversion::convertLine(const vpCameraParameters &cam,
- const double &rho_m, const double &theta_m,
- double &rho_p, double &theta_p)
+void vpMeterPixelConversion::convertLine(const vpCameraParameters &cam, const double &rho_m, const double &theta_m,
+ double &rho_p, double &theta_p)
{
- double co = cos(theta_m) ;
- double si = sin(theta_m) ;
- double d = sqrt(vpMath::sqr(cam.py*co) + vpMath::sqr(cam.px*si)) ;
+ double co = cos(theta_m);
+ double si = sin(theta_m);
+ double d = sqrt(vpMath::sqr(cam.py * co) + vpMath::sqr(cam.px * si));
- if (fabs(d)<1e-6)
- {
- vpERROR_TRACE("division by zero") ;
- throw(vpException(vpException::divideByZeroError,
- "division by zero")) ;
+ if (fabs(d) < 1e-6) {
+ vpERROR_TRACE("division by zero");
+ throw(vpException(vpException::divideByZeroError, "division by zero"));
}
- theta_p = atan2(cam.px*si, cam.py*co) ;
- rho_p = (cam.px*cam.py*rho_m + cam.u0*cam.py*co + cam.v0*cam.px*si) ;
- rho_p /= d ;
+ theta_p = atan2(cam.px * si, cam.py * co);
+ rho_p = (cam.px * cam.py * rho_m + cam.u0 * cam.py * co + cam.v0 * cam.px * si);
+ rho_p /= d;
}
/*!
Converts an ellipse with parameters expressed in meter in the image
plane in parameters expressed in the image in pixels.
- The ellipse is here represented by its parameters \f$x_c,y_c,\mu_{20}, \mu_{11}, \mu_{02}\f$.
+ The ellipse is here represented by its parameters \f$x_c,y_c,\mu_{20},
+ \mu_{11}, \mu_{02}\f$.
\param cam [in]: Intrinsic camera parameters.
- \param circle [in]: 3D circle with parameters circle.p[] expressed in meters in the image plane.
- \param center [out]: Center of the corresponding ellipse in the image with coordinates
- expressed in pixels.
- \param mu20_p,mu11_p,mu02_p [out]: Centered moments expressed in pixels.
+ \param circle [in]: 3D circle with parameters circle.p[] expressed in meters
+ in the image plane. \param center [out]: Center of the corresponding ellipse
+ in the image with coordinates expressed in pixels. \param
+ mu20_p,mu11_p,mu02_p [out]: Centered moments expressed in pixels.
The following code shows how to use this function:
\code
@@ -91,10 +89,8 @@ vpMeterPixelConversion::convertLine(const vpCameraParameters &cam,
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];
@@ -105,8 +101,7 @@ vpMeterPixelConversion::convertEllipse(const vpCameraParameters &cam,
// Convert from meter to pixels
vpMeterPixelConversion::convertPoint(cam, xc_m, yc_m, center);
- mu20_p = mu20_m*vpMath::sqr(cam.get_px());
- mu11_p = mu11_m*cam.get_px()*cam.get_py();
- mu02_p = mu02_m*vpMath::sqr(cam.get_py());
+ mu20_p = mu20_m * vpMath::sqr(cam.get_px());
+ mu11_p = mu11_m * cam.get_px() * cam.get_py();
+ mu02_p = mu02_m * vpMath::sqr(cam.get_py());
}
-
diff --git a/modules/core/src/camera/vpPixelMeterConversion.cpp b/modules/core/src/camera/vpPixelMeterConversion.cpp
index 5415948..02587b5 100644
--- a/modules/core/src/camera/vpPixelMeterConversion.cpp
+++ b/modules/core/src/camera/vpPixelMeterConversion.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,88 +41,69 @@
\file vpPixelMeterConversion.cpp
\brief Pixel to meter conversion.
*/
-#include<visp3/core/vpPixelMeterConversion.h>
-#include<visp3/core/vpCameraParameters.h>
-#include<visp3/core/vpException.h>
-#include<visp3/core/vpMath.h>
-#include<visp3/core/vpDebug.h>
-
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPixelMeterConversion.h>
//! line coordinates conversion (rho,theta)
-void
-vpPixelMeterConversion::convertLine(const vpCameraParameters &cam,
- const double &rho_p, const double &theta_p,
- double &rho_m, double &theta_m)
+void vpPixelMeterConversion::convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p,
+ double &rho_m, double &theta_m)
{
- double co = cos(theta_p) ;
- double si = sin(theta_p) ;
- double d = vpMath::sqr(cam.px*co)+vpMath::sqr(cam.py*si) ;
+ double co = cos(theta_p);
+ double si = sin(theta_p);
+ double d = vpMath::sqr(cam.px * co) + vpMath::sqr(cam.py * si);
- if (fabs(d)<1e-6)
- {
- vpERROR_TRACE("division by zero") ;
- throw(vpException(vpException::divideByZeroError,
- "division by zero")) ;
+ if (fabs(d) < 1e-6) {
+ vpERROR_TRACE("division by zero");
+ throw(vpException(vpException::divideByZeroError, "division by zero"));
}
- theta_m = atan2(si*cam.py, co*cam.px) ;
- rho_m = (rho_p - cam.u0*co-cam.v0*si)/sqrt(d) ;
+ theta_m = atan2(si * cam.py, co * cam.px);
+ rho_m = (rho_p - cam.u0 * co - cam.v0 * si) / sqrt(d);
}
-
-void
-vpPixelMeterConversion::convertMoment(const vpCameraParameters &cam,
- unsigned int order,
- const vpMatrix &moment_pixel,
- vpMatrix &moment_meter)
+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 ;
+ 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
- {
- m[p][q] = 0; // initialisation e 0
- for(unsigned int r=0; r<=p; r++) // somme externe
- for(unsigned int t=0; t<=q; t++) // somme interne
- {
- m[p][q] +=
- static_cast<double>(vpMath::comb(p, r))
- * static_cast<double>(vpMath::comb(q, t))
- * pow(xc, (int)(p-r)) * pow(yc, (int)(q-t))
- * moment_pixel[r][t];
-
- }
- }
-
+ for (unsigned int p = 0; p < order; p++) // iteration en X
+ for (unsigned int q = 0; q < order; q++) // iteration en Y
+ if (p + q == k) // on est bien dans la matrice triangulaire superieure
+ {
+ m[p][q] = 0; // initialisation e 0
+ for (unsigned int r = 0; r <= p; r++) // somme externe
+ for (unsigned int t = 0; t <= q; t++) // somme interne
+ {
+ m[p][q] += static_cast<double>(vpMath::comb(p, r)) * static_cast<double>(vpMath::comb(q, t)) *
+ pow(xc, (int)(p - r)) * pow(yc, (int)(q - t)) * moment_pixel[r][t];
+ }
+ }
}
- for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
- for (unsigned int p=0 ; p<order; p++)
- for (unsigned int q=0; q<order; q++)
- if (p+q==k)
- {
- m[p][q] *= pow(cam.inv_px,(int)(1+p)) * pow(cam.inv_py,(int)(1+q));
- }
-
- for (unsigned int k=0; k<order; k++) // iteration correspondant e l'ordre du moment
- for (unsigned int p=0 ; p<order; p++)
- for (unsigned int q=0; q<order; q++)
- if (p+q==k)
- {
- moment_meter[p][q] = m[p][q];
- }
-
+ for (unsigned int k = 0; k < order; k++) // iteration correspondant e l'ordre du moment
+ for (unsigned int p = 0; p < order; p++)
+ for (unsigned int q = 0; q < order; q++)
+ if (p + q == k) {
+ m[p][q] *= pow(cam.inv_px, (int)(1 + p)) * pow(cam.inv_py, (int)(1 + q));
+ }
+
+ for (unsigned int k = 0; k < order; k++) // iteration correspondant e l'ordre du moment
+ for (unsigned int p = 0; p < order; p++)
+ for (unsigned int q = 0; q < order; q++)
+ if (p + q == k) {
+ moment_meter[p][q] = m[p][q];
+ }
}
-
/*
* Local variables:
* c-basic-offset: 2
* End:
*/
-
diff --git a/modules/core/src/camera/vpXmlParserCamera.cpp b/modules/core/src/camera/vpXmlParserCamera.cpp
index f88f829..81dcd97 100644
--- a/modules/core/src/camera/vpXmlParserCamera.cpp
+++ b/modules/core/src/camera/vpXmlParserCamera.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpXmlParserCamera.cpp
\brief Definition of the vpXmlParserCamera class member functions.
@@ -49,53 +49,52 @@
#include <string.h>
#include <visp3/core/vpDebug.h>
-/* -------------------------------------------------------------------------- */
-/* --- LABEL XML ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-#define LABEL_XML_ROOT "root"
-#define LABEL_XML_CAMERA "camera"
-#define LABEL_XML_CAMERA_NAME "name"
-#define LABEL_XML_WIDTH "image_width"
-#define LABEL_XML_HEIGHT "image_height"
-#define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
-#define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
-#define LABEL_XML_FULL_WIDTH "full_width"
-#define LABEL_XML_FULL_HEIGHT "full_height"
-#define LABEL_XML_MODEL "model"
-#define LABEL_XML_MODEL_TYPE "type"
-#define LABEL_XML_U0 "u0"
-#define LABEL_XML_V0 "v0"
-#define LABEL_XML_PX "px"
-#define LABEL_XML_PY "py"
-#define LABEL_XML_KUD "kud"
-#define LABEL_XML_KDU "kdu"
-
-#define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
-#define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
-
-#define LABEL_XML_ADDITIONAL_INFO "additional_information"
+/* --------------------------------------------------------------------------
+ */
+/* --- LABEL XML ------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
+
+#define LABEL_XML_ROOT "root"
+#define LABEL_XML_CAMERA "camera"
+#define LABEL_XML_CAMERA_NAME "name"
+#define LABEL_XML_WIDTH "image_width"
+#define LABEL_XML_HEIGHT "image_height"
+#define LABEL_XML_SUBSAMPLING_WIDTH "subsampling_width"
+#define LABEL_XML_SUBSAMPLING_HEIGHT "subsampling_height"
+#define LABEL_XML_FULL_WIDTH "full_width"
+#define LABEL_XML_FULL_HEIGHT "full_height"
+#define LABEL_XML_MODEL "model"
+#define LABEL_XML_MODEL_TYPE "type"
+#define LABEL_XML_U0 "u0"
+#define LABEL_XML_V0 "v0"
+#define LABEL_XML_PX "px"
+#define LABEL_XML_PY "py"
+#define LABEL_XML_KUD "kud"
+#define LABEL_XML_KDU "kdu"
+
+#define LABEL_XML_MODEL_WITHOUT_DISTORTION "perspectiveProjWithoutDistortion"
+#define LABEL_XML_MODEL_WITH_DISTORTION "perspectiveProjWithDistortion"
+
+#define LABEL_XML_ADDITIONAL_INFO "additional_information"
/*!
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)
{
}
/*!
Copy constructor
\param twinParser : parser object to copy
*/
-vpXmlParserCamera::vpXmlParserCamera(vpXmlParserCamera& twinParser)
- : vpXmlParser(twinParser),
- camera(), camera_name(), image_width(0), image_height(0),
- subsampling_width(0), subsampling_height(0), full_width(0), full_height(0)
+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)
{
- this->camera = twinParser.camera;
- this->camera_name = twinParser.camera_name;
this->image_width = twinParser.image_width;
this->image_height = twinParser.image_height;
this->subsampling_width = twinParser.subsampling_width;
@@ -109,8 +108,8 @@ 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;
this->image_width = twinParser.image_width;
@@ -119,7 +118,7 @@ vpXmlParserCamera::operator =(const vpXmlParserCamera& twinParser) {
this->subsampling_height = twinParser.subsampling_height;
this->full_width = twinParser.full_width;
this->full_height = twinParser.full_height;
- return *this ;
+ return *this;
}
/*!
@@ -136,32 +135,27 @@ vpXmlParserCamera::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;
doc = xmlParseFile(filename.c_str());
- if (doc == NULL)
- {
+ if (doc == NULL) {
return SEQUENCE_ERROR;
}
node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
+ if (node == NULL) {
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
- int ret = this ->read (doc, node, cam_name, projModel, im_width, im_height);
+ int ret = this->read(doc, node, cam_name, projModel, im_width, im_height);
- cam = camera ;
+ cam = camera;
xmlFreeDoc(doc);
@@ -176,17 +170,17 @@ vpXmlParserCamera::parse(vpCameraParameters &cam, const std::string &filename,
camera parameters. Set as "" if the camera name is not ambiguous.
\param im_width : width of image on which camera calibration was performed.
Set as 0 if not ambiguous.
- \param im_height : height of the image on which camera calibration was performed.
- Set as 0 if not ambiguous.
- \param additionalInfo : Additional information added in the saved xml file. The content
- of this string should be in xml format.
+ \param im_height : height of the image on which camera calibration was
+performed. Set as 0 if not ambiguous. \param additionalInfo : Additional
+information added in the saved xml file. The content of this string should be
+in xml format.
\return error code.
A typical usage would be the following:
\code
-#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/core/vpTime.h>
+#include <visp3/core/vpXmlParserCamera.h>
int main()
{
@@ -211,81 +205,76 @@ int main()
</camera>
\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,
- const std::string &additionalInfo)
+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);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
- xmlDocSetRootElement(doc,node);
- xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
- "This file stores intrinsic camera parameters used\n"
- " in the vpCameraParameters Class of ViSP available\n"
- " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
- " It can be read with the parse method of\n"
- " the vpXmlParserCamera class.");
- xmlAddChild(node,node_tmp);
+ 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.");
+ xmlAddChild(node, node_tmp);
}
node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
+ if (node == NULL) {
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
this->camera = cam;
- int nbCamera = count(doc, node, cam_name,cam.get_projModel(),
- im_width, im_height);
- if( nbCamera > 0){
-// vpCERROR << nbCamera
-// << " set(s) of camera parameters is(are) already "<< std::endl
-// << "available in the file with your specifications : "<< std::endl
-// << "precise the grabber parameters or delete manually"<< std::endl
-// << "the previous one."<<std::endl;
+ int nbCamera = count(doc, node, cam_name, cam.get_projModel(), im_width, im_height);
+ if (nbCamera > 0) {
+ // vpCERROR << nbCamera
+ // << " set(s) of camera parameters is(are) already "<<
+ // std::endl
+ // << "available in the file with your specifications : "<<
+ // std::endl
+ // << "precise the grabber parameters or delete manually"<<
+ // std::endl
+ // << "the previous one."<<std::endl;
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
nodeCamera = find_camera(doc, node, cam_name, im_width, im_height);
- if(nodeCamera == NULL){
+ if (nodeCamera == NULL) {
write(node, cam_name, im_width, im_height);
- }
- else{
+ } else {
write_camera(nodeCamera);
}
- if(!additionalInfo.empty()) {
- //Get camera node pointer
+ if (!additionalInfo.empty()) {
+ // Get camera node pointer
nodeCamera = find_camera(doc, node, cam_name, im_width, im_height);
- //Additional information provided by the user
+ // Additional information provided by the user
xmlNodePtr nodeAdditionalInfo = find_additional_info(nodeCamera);
- if(nodeAdditionalInfo == NULL) {
- //Create the additional information node
- xmlNodePtr node_comment = xmlNewComment((xmlChar*)"Additional information");
- xmlAddChild(nodeCamera,node_comment);
+ if (nodeAdditionalInfo == NULL) {
+ // Create the additional information node
+ 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
+ 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));
@@ -303,8 +292,6 @@ vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filena
return SEQUENCE_OK;
}
-
-
/*!
Read camera parameters from a XML file.
@@ -323,14 +310,10 @@ vpXmlParserCamera::save(const vpCameraParameters &cam, const std::string &filena
\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;
@@ -338,11 +321,10 @@ vpXmlParserCamera::read (xmlDocPtr doc, xmlNodePtr node,
vpXmlCodeSequenceType back = SEQUENCE_OK;
unsigned int nbCamera = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
@@ -361,23 +343,21 @@ vpXmlParserCamera::read (xmlDocPtr doc, xmlNodePtr node,
}
*/
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;
+ } else
+ back = SEQUENCE_ERROR;
}
- if (nbCamera == 0){
+ if (nbCamera == 0) {
back = SEQUENCE_ERROR;
- vpCERROR << "No camera parameters is available" << std::endl
- << "with your specifications" << std::endl;
- }
- else if(nbCamera > 1){
+ vpCERROR << "No camera parameters is available" << std::endl << "with your specifications" << std::endl;
+ } else if (nbCamera > 1) {
back = SEQUENCE_ERROR;
- vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
- << "with your specifications : " << std::endl
- << "precise your choice..." << std::endl;
+ vpCERROR << nbCamera << " sets of camera parameters are available" << std::endl
+ << "with your specifications : " << std::endl
+ << "precise your choice..." << std::endl;
}
return back;
@@ -401,24 +381,19 @@ vpXmlParserCamera::read (xmlDocPtr doc, xmlNodePtr node,
\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;
int nbCamera = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
}
/*
@@ -435,10 +410,9 @@ vpXmlParserCamera::count (xmlDocPtr doc, xmlNodePtr node,
break;
}
*/
- if (prop== CODE_XML_CAMERA) {
- if (SEQUENCE_OK == this->read_camera (doc, node, cam_name, projModel,
- im_width, im_height,
- subsampl_width, subsampl_height))
+ if (prop == CODE_XML_CAMERA) {
+ if (SEQUENCE_OK ==
+ this->read_camera(doc, node, cam_name, projModel, im_width, im_height, subsampl_width, subsampl_height))
nbCamera++;
}
}
@@ -465,22 +439,17 @@ vpXmlParserCamera::count (xmlDocPtr doc, xmlNodePtr node,
\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;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
}
/*
@@ -497,10 +466,9 @@ vpXmlParserCamera::find_camera (xmlDocPtr doc, xmlNodePtr node,
break;
}
*/
- if(prop == CODE_XML_CAMERA){
- if (SEQUENCE_OK == this->read_camera_header(doc, node, cam_name,
- im_width, im_height,
- subsampl_width, subsampl_height))
+ if (prop == CODE_XML_CAMERA) {
+ if (SEQUENCE_OK ==
+ this->read_camera_header(doc, node, cam_name, im_width, im_height, subsampl_width, subsampl_height))
return node;
}
}
@@ -516,8 +484,8 @@ vpXmlParserCamera::find_camera (xmlDocPtr doc, xmlNodePtr node,
\return additional information node.
*/
-xmlNodePtr
-vpXmlParserCamera::find_additional_info(xmlNodePtr node) {
+xmlNodePtr vpXmlParserCamera::find_additional_info(xmlNodePtr node)
+{
vpXmlCodeType prop;
for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
@@ -525,12 +493,12 @@ vpXmlParserCamera::find_additional_info(xmlNodePtr node) {
continue;
}
- if (SEQUENCE_OK != str2xmlcode((char*) (node->name), prop)) {
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
}
if (prop == CODE_XML_ADDITIONAL_INFO) {
- //We found the node
+ // We found the node
return node;
}
}
@@ -557,20 +525,16 @@ 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. */
std::string camera_name_tmp = "";
- unsigned int image_height_tmp = 0 ;
- unsigned int image_width_tmp = 0 ;
+ unsigned int image_height_tmp = 0;
+ unsigned int image_width_tmp = 0;
unsigned int subsampling_width_tmp = 0;
unsigned int subsampling_height_tmp = 0;
// unsigned int full_width_tmp = 0;
@@ -580,21 +544,18 @@ vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
bool projModelFound = false;
vpXmlCodeSequenceType back = SEQUENCE_OK;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
// vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
-
- switch (prop)
- {
+ switch (prop) {
case CODE_XML_CAMERA_NAME: {
- char * val_char = xmlReadCharChild(doc, node);
+ char *val_char = xmlReadCharChild(doc, node);
camera_name_tmp = val_char;
std::cout << "Found camera with name: \"" << camera_name_tmp << "\"" << std::endl;
xmlFree(val_char);
@@ -613,17 +574,17 @@ vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
case CODE_XML_SUBSAMPLING_HEIGHT:
subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
break;
- // case CODE_XML_FULL_WIDTH:
- // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
+ // case CODE_XML_FULL_WIDTH:
+ // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
- // case CODE_XML_FULL_HEIGHT:
- // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
+ // case CODE_XML_FULL_HEIGHT:
+ // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
case CODE_XML_MODEL:
back = read_camera_model(doc, node, cam_tmp_model);
- if(cam_tmp_model.get_projModel() == projModel){
+ if (cam_tmp_model.get_projModel() == projModel) {
cam_tmp = cam_tmp_model;
projModelFound = true;
}
@@ -648,26 +609,26 @@ vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
back = SEQUENCE_ERROR;
break;
}
-
}
- // Create a specific test for subsampling_width and subsampling_height to ensure that division by zero is not possible in the next test
+ // Create a specific test for subsampling_width and subsampling_height to
+ // ensure that division by zero is not possible in the next test
bool test_subsampling_width = true;
bool test_subsampling_height = true;
if (subsampling_width) {
- test_subsampling_width = (abs((int)subsampl_width - (int)subsampling_width_tmp) < (allowedPixelDiffOnImageSize * (int)(subsampling_width_tmp / subsampling_width)));
+ 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) &&
+ if (!((projModelFound == true) && (cam_name == camera_name_tmp) &&
(abs((int)im_width - (int)image_width_tmp) < allowedPixelDiffOnImageSize || im_width == 0) &&
(abs((int)im_height - (int)image_height_tmp) < allowedPixelDiffOnImageSize || im_height == 0) &&
- (test_subsampling_width)&&
- (test_subsampling_height))){
+ (test_subsampling_width) && (test_subsampling_height))) {
back = SEQUENCE_ERROR;
- }
- else{
+ } else {
this->camera = cam_tmp;
this->camera_name = camera_name_tmp;
this->image_width = image_width_tmp;
@@ -698,44 +659,36 @@ vpXmlParserCamera::read_camera (xmlDocPtr doc, xmlNodePtr node,
\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. */
std::string camera_name_tmp = "";
- unsigned int image_height_tmp = 0 ;
- unsigned int image_width_tmp = 0 ;
+ unsigned int image_height_tmp = 0;
+ unsigned int image_width_tmp = 0;
unsigned int subsampling_width_tmp = 0;
unsigned int subsampling_height_tmp = 0;
// unsigned int full_width_tmp = 0;
// unsigned int full_height_tmp = 0;
vpXmlCodeSequenceType back = SEQUENCE_OK;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
// vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
-
- switch (prop)
- {
- case CODE_XML_CAMERA_NAME:{
- char * val_char = xmlReadCharChild(doc, node);
+ switch (prop) {
+ case CODE_XML_CAMERA_NAME: {
+ char *val_char = xmlReadCharChild(doc, node);
camera_name_tmp = val_char;
xmlFree(val_char);
- }break;
+ } break;
case CODE_XML_WIDTH:
image_width_tmp = xmlReadUnsignedIntChild(doc, node);
@@ -750,13 +703,13 @@ read_camera_header (xmlDocPtr doc, xmlNodePtr node,
case CODE_XML_SUBSAMPLING_HEIGHT:
subsampling_height_tmp = xmlReadUnsignedIntChild(doc, node);
break;
- // case CODE_XML_FULL_WIDTH:
- // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
+ // case CODE_XML_FULL_WIDTH:
+ // full_width_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
- // case CODE_XML_FULL_HEIGHT:
- // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
- // break;
+ // case CODE_XML_FULL_HEIGHT:
+ // full_height_tmp = xmlReadUnsignedIntChild(doc, node);
+ // break;
case CODE_XML_MODEL:
break;
@@ -781,13 +734,10 @@ 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_width == subsampling_width_tmp || subsampl_width == 0) &&
+ (subsampl_height == subsampling_height_tmp || subsampl_height == 0))) {
back = SEQUENCE_ERROR;
}
return back;
@@ -803,16 +753,15 @@ 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;
vpXmlCodeType prop;
/* read value in the XML file. */
- char* model_type = NULL;
+ char *model_type = NULL;
double u0 = cam_tmp.get_u0();
double v0 = cam_tmp.get_v0();
double px = cam_tmp.get_px();
@@ -822,26 +771,24 @@ vpXmlParserCamera::read_camera_model (xmlDocPtr doc, xmlNodePtr node,
vpXmlCodeSequenceType back = SEQUENCE_OK;
int validation = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
// vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
- switch (prop)
- {
- case CODE_XML_MODEL_TYPE:{
- if(model_type != NULL){
+ switch (prop) {
+ case CODE_XML_MODEL_TYPE: {
+ if (model_type != NULL) {
xmlFree(model_type);
}
model_type = xmlReadCharChild(doc, node);
nb++;
validation = validation | 0x01;
- }break;
+ } break;
case CODE_XML_U0:
u0 = xmlReadDoubleChild(doc, node);
nb++;
@@ -890,34 +837,30 @@ vpXmlParserCamera::read_camera_model (xmlDocPtr doc, xmlNodePtr node,
}
}
- if(model_type == NULL) {
+ if (model_type == NULL) {
vpERROR_TRACE("projection model type doesn't match with any known model !");
return SEQUENCE_ERROR;
}
- if( !strcmp(model_type,LABEL_XML_MODEL_WITHOUT_DISTORTION)){
- if (nb != 5 || validation != 0x1F)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
+ if (!strcmp(model_type, LABEL_XML_MODEL_WITHOUT_DISTORTION)) {
+ if (nb != 5 || validation != 0x1F) {
+ vpCERROR << "ERROR in 'model' field:\n";
vpCERROR << "it must contain 5 parameters\n";
xmlFree(model_type);
return SEQUENCE_ERROR;
}
- cam_tmp.initPersProjWithoutDistortion(px,py,u0,v0) ;
- }
- else if( !strcmp(model_type,LABEL_XML_MODEL_WITH_DISTORTION)){
- if (nb != 7 || validation != 0x7F)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
+ cam_tmp.initPersProjWithoutDistortion(px, py, u0, v0);
+ } else if (!strcmp(model_type, LABEL_XML_MODEL_WITH_DISTORTION)) {
+ if (nb != 7 || validation != 0x7F) {
+ vpCERROR << "ERROR in 'model' field:\n";
vpCERROR << "it must contain 7 parameters\n";
xmlFree(model_type);
return SEQUENCE_ERROR;
}
- cam_tmp.initPersProjWithDistortion(px,py,u0,v0,kud,kdu);
- }
- else{
+ cam_tmp.initPersProjWithDistortion(px, py, u0, v0, kud, kdu);
+ } else {
vpERROR_TRACE("projection model type doesn't match with any known model !");
xmlFree(model_type);
@@ -945,11 +888,9 @@ 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,
- const unsigned int subsampl_height)
+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;
@@ -957,59 +898,59 @@ write (xmlNodePtr node, const std::string& cam_name,
xmlNodePtr node_camera;
// <camera>
- node_camera = xmlNewNode(NULL,(xmlChar*)LABEL_XML_CAMERA);
- xmlAddChild(node,node_camera);
+ node_camera = xmlNewNode(NULL, (xmlChar *)LABEL_XML_CAMERA);
+ xmlAddChild(node, node_camera);
{
//<name>
- if(!cam_name.empty()){
- node_tmp = xmlNewComment((xmlChar*)"Name of the camera");
- xmlAddChild(node_camera,node_tmp);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_CAMERA_NAME,
- (xmlChar*)cam_name.c_str());
+ if (!cam_name.empty()) {
+ node_tmp = xmlNewComment((xmlChar *)"Name of the camera");
+ xmlAddChild(node_camera, node_tmp);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_CAMERA_NAME, (xmlChar *)cam_name.c_str());
}
- if(im_width != 0 || im_height != 0){
+ if (im_width != 0 || im_height != 0) {
char str[11];
//<image_width>
- node_tmp = xmlNewComment((xmlChar*)"Size of the image on which camera calibration was performed");
- xmlAddChild(node_camera,node_tmp);
+ node_tmp = xmlNewComment((xmlChar *)"Size of the image on which camera "
+ "calibration was performed");
+ xmlAddChild(node_camera, node_tmp);
- sprintf(str,"%u",im_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_WIDTH,(xmlChar*)str);
+ sprintf(str, "%u", im_width);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_WIDTH, (xmlChar *)str);
//<image_height>
- sprintf(str,"%u",im_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_HEIGHT,(xmlChar*)str);
- if(subsampling_width != 0 || subsampling_height != 0){
- node_tmp = xmlNewComment((xmlChar*)"Subsampling used to obtain the current size of the image.");
- xmlAddChild(node_camera,node_tmp);
+ sprintf(str, "%u", im_height);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_HEIGHT, (xmlChar *)str);
+ if (subsampling_width != 0 || subsampling_height != 0) {
+ node_tmp = xmlNewComment((xmlChar *)"Subsampling used to obtain the "
+ "current size of the image.");
+ xmlAddChild(node_camera, node_tmp);
//<subsampling_width>
- sprintf(str,"%u",subsampl_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_WIDTH,
- (xmlChar*)str);
+ sprintf(str, "%u", subsampl_width);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_SUBSAMPLING_WIDTH, (xmlChar *)str);
//<subsampling_height>
- sprintf(str,"%u",subsampl_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_SUBSAMPLING_HEIGHT,
- (xmlChar*)str);
- node_tmp = xmlNewComment((xmlChar*)"The full size is the sensor size actually used to grab the image. full_width = subsampling_width * image_width");
- xmlAddChild(node_camera,node_tmp);
+ sprintf(str, "%u", subsampl_height);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_SUBSAMPLING_HEIGHT, (xmlChar *)str);
+ node_tmp = xmlNewComment((xmlChar *)"The full size is the sensor size actually used to "
+ "grab the image. full_width = subsampling_width * "
+ "image_width");
+ xmlAddChild(node_camera, node_tmp);
//<full_width>
- sprintf(str,"%u",im_width*subsampl_width);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_WIDTH,
- (xmlChar*)str);
+ sprintf(str, "%u", im_width * subsampl_width);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_WIDTH, (xmlChar *)str);
//<full_height>
- sprintf(str,"%u",im_height*subsampl_height);
- xmlNewTextChild(node_camera,NULL,(xmlChar*)LABEL_XML_FULL_HEIGHT,
- (xmlChar*)str);
+ sprintf(str, "%u", im_height * subsampl_height);
+ xmlNewTextChild(node_camera, NULL, (xmlChar *)LABEL_XML_FULL_HEIGHT, (xmlChar *)str);
}
}
- node_tmp = xmlNewComment((xmlChar*)"Intrinsic camera parameters computed for each projection model");
+ node_tmp = xmlNewComment((xmlChar *)"Intrinsic camera parameters "
+ "computed for each projection model");
- xmlAddChild(node_camera,node_tmp);
+ xmlAddChild(node_camera, node_tmp);
back = write_camera(node_camera);
}
@@ -1022,88 +963,86 @@ write (xmlNodePtr node, const std::string& cam_name,
\return error code.
*/
-int vpXmlParserCamera::
-write_camera(xmlNodePtr node_camera){
+int vpXmlParserCamera::write_camera(xmlNodePtr node_camera)
+{
xmlNodePtr node_model;
xmlNodePtr node_tmp;
int back = SEQUENCE_OK;
- switch(camera.get_projModel()){
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ switch (camera.get_projModel()) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
//<model>
- node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
- xmlAddChild(node_camera,node_model);
- {
- char str[21];
- node_tmp = xmlNewComment((xmlChar*)"Projection model type");
- xmlAddChild(node_model,node_tmp);
-
- //<type>without_distortion</type>
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
- (xmlChar*)LABEL_XML_MODEL_WITHOUT_DISTORTION);
-
- node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
- xmlAddChild(node_model,node_tmp);
- //<px>
- sprintf(str,"%.10f",camera.get_px());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
- //<py>
- sprintf(str,"%.10f",camera.get_py());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
-
- node_tmp = xmlNewComment((xmlChar*)"Principal point");
- xmlAddChild(node_model,node_tmp);
-
- //<u0>
- sprintf(str,"%.10f",camera.get_u0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
- //<v0>
- sprintf(str,"%.10f",camera.get_v0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
- }
+ node_model = xmlNewNode(NULL, (xmlChar *)LABEL_XML_MODEL);
+ xmlAddChild(node_camera, node_model);
+ {
+ char str[21];
+ node_tmp = xmlNewComment((xmlChar *)"Projection model type");
+ xmlAddChild(node_model, node_tmp);
+
+ //<type>without_distortion</type>
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_MODEL_TYPE, (xmlChar *)LABEL_XML_MODEL_WITHOUT_DISTORTION);
+
+ node_tmp = xmlNewComment((xmlChar *)"Pixel ratio");
+ xmlAddChild(node_model, node_tmp);
+ //<px>
+ sprintf(str, "%.10f", camera.get_px());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, (xmlChar *)str);
+ //<py>
+ sprintf(str, "%.10f", camera.get_py());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, (xmlChar *)str);
+
+ node_tmp = xmlNewComment((xmlChar *)"Principal point");
+ xmlAddChild(node_model, node_tmp);
+
+ //<u0>
+ sprintf(str, "%.10f", camera.get_u0());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, (xmlChar *)str);
+ //<v0>
+ sprintf(str, "%.10f", camera.get_v0());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, (xmlChar *)str);
+ }
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion:
//<model>
- node_model = xmlNewNode(NULL,(xmlChar*)LABEL_XML_MODEL);
- xmlAddChild(node_camera,node_model);
- {
- char str[21];
- node_tmp = xmlNewComment((xmlChar*)"Projection model type");
- xmlAddChild(node_model,node_tmp);
- //<type>with_distortion</type>
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_MODEL_TYPE,
- (xmlChar*)LABEL_XML_MODEL_WITH_DISTORTION);
-
- node_tmp = xmlNewComment((xmlChar*)"Pixel ratio");
- xmlAddChild(node_model,node_tmp);
- //<px>
- sprintf(str,"%.10f",camera.get_px());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PX,(xmlChar*)str);
- //<py>
- sprintf(str,"%.10f",camera.get_py());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_PY,(xmlChar*)str);
-
- node_tmp = xmlNewComment((xmlChar*)"Principal point");
- xmlAddChild(node_model,node_tmp);
- //<u0>
- sprintf(str,"%.10f",camera.get_u0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_U0,(xmlChar*)str);
- //<v0>
- sprintf(str,"%.10f",camera.get_v0());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_V0,(xmlChar*)str);
-
- //<kud>
- node_tmp = xmlNewComment((xmlChar*)"Undistorted to distorted distortion parameter");
- xmlAddChild(node_model,node_tmp);
- sprintf(str,"%.10f",camera.get_kud());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KUD,(xmlChar*)str);
-
- //<kud>
- node_tmp = xmlNewComment((xmlChar*)"Distorted to undistorted distortion parameter");
- xmlAddChild(node_model,node_tmp);
- sprintf(str,"%.10f",camera.get_kdu());
- xmlNewTextChild(node_model,NULL,(xmlChar*)LABEL_XML_KDU,(xmlChar*)str);
- }
+ node_model = xmlNewNode(NULL, (xmlChar *)LABEL_XML_MODEL);
+ xmlAddChild(node_camera, node_model);
+ {
+ char str[21];
+ node_tmp = xmlNewComment((xmlChar *)"Projection model type");
+ xmlAddChild(node_model, node_tmp);
+ //<type>with_distortion</type>
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_MODEL_TYPE, (xmlChar *)LABEL_XML_MODEL_WITH_DISTORTION);
+
+ node_tmp = xmlNewComment((xmlChar *)"Pixel ratio");
+ xmlAddChild(node_model, node_tmp);
+ //<px>
+ sprintf(str, "%.10f", camera.get_px());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PX, (xmlChar *)str);
+ //<py>
+ sprintf(str, "%.10f", camera.get_py());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_PY, (xmlChar *)str);
+
+ node_tmp = xmlNewComment((xmlChar *)"Principal point");
+ xmlAddChild(node_model, node_tmp);
+ //<u0>
+ sprintf(str, "%.10f", camera.get_u0());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_U0, (xmlChar *)str);
+ //<v0>
+ sprintf(str, "%.10f", camera.get_v0());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_V0, (xmlChar *)str);
+
+ //<kud>
+ node_tmp = xmlNewComment((xmlChar *)"Undistorted to distorted distortion parameter");
+ xmlAddChild(node_model, node_tmp);
+ sprintf(str, "%.10f", camera.get_kud());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_KUD, (xmlChar *)str);
+
+ //<kud>
+ node_tmp = xmlNewComment((xmlChar *)"Distorted to undistorted distortion parameter");
+ xmlAddChild(node_model, node_tmp);
+ sprintf(str, "%.10f", camera.get_kdu());
+ xmlNewTextChild(node_model, NULL, (xmlChar *)LABEL_XML_KDU, (xmlChar *)str);
+ }
break;
}
return back;
@@ -1117,84 +1056,48 @@ 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;
// DEBUG_TRACE (9, "# Entree :str=%s.", str);
- if (! strcmp (str, LABEL_XML_CAMERA))
- {
+ if (!strcmp(str, LABEL_XML_CAMERA)) {
val_int = CODE_XML_CAMERA;
- }
- else if (! strcmp (str, LABEL_XML_CAMERA_NAME))
- {
+ } else if (!strcmp(str, LABEL_XML_CAMERA_NAME)) {
val_int = CODE_XML_CAMERA_NAME;
- }
- else if (! strcmp (str, LABEL_XML_MODEL))
- {
+ } else if (!strcmp(str, LABEL_XML_MODEL)) {
val_int = CODE_XML_MODEL;
- }
- else if (! strcmp (str, LABEL_XML_MODEL_TYPE))
- {
+ } else if (!strcmp(str, LABEL_XML_MODEL_TYPE)) {
val_int = CODE_XML_MODEL_TYPE;
- }
- else if (! strcmp (str, LABEL_XML_WIDTH))
- {
+ } else if (!strcmp(str, LABEL_XML_WIDTH)) {
val_int = CODE_XML_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_HEIGHT))
- {
+ } else if (!strcmp(str, LABEL_XML_HEIGHT)) {
val_int = CODE_XML_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_SUBSAMPLING_WIDTH))
- {
+ } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_WIDTH)) {
val_int = CODE_XML_SUBSAMPLING_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_SUBSAMPLING_HEIGHT))
- {
+ } else if (!strcmp(str, LABEL_XML_SUBSAMPLING_HEIGHT)) {
val_int = CODE_XML_SUBSAMPLING_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_FULL_WIDTH))
- {
+ } else if (!strcmp(str, LABEL_XML_FULL_WIDTH)) {
val_int = CODE_XML_FULL_WIDTH;
- }
- else if (! strcmp (str, LABEL_XML_FULL_HEIGHT))
- {
+ } else if (!strcmp(str, LABEL_XML_FULL_HEIGHT)) {
val_int = CODE_XML_FULL_HEIGHT;
- }
- else if (! strcmp (str, LABEL_XML_U0))
- {
+ } else if (!strcmp(str, LABEL_XML_U0)) {
val_int = CODE_XML_U0;
- }
- else if (! strcmp (str, LABEL_XML_V0))
- {
+ } else if (!strcmp(str, LABEL_XML_V0)) {
val_int = CODE_XML_V0;
- }
- else if (! strcmp (str, LABEL_XML_PX))
- {
+ } else if (!strcmp(str, LABEL_XML_PX)) {
val_int = CODE_XML_PX;
- }
- else if (! strcmp (str, LABEL_XML_PY))
- {
+ } else if (!strcmp(str, LABEL_XML_PY)) {
val_int = CODE_XML_PY;
- }
- else if (! strcmp (str, LABEL_XML_KUD))
- {
+ } else if (!strcmp(str, LABEL_XML_KUD)) {
val_int = CODE_XML_KUD;
- }
- else if (! strcmp (str, LABEL_XML_KDU))
- {
+ } else if (!strcmp(str, LABEL_XML_KDU)) {
val_int = CODE_XML_KDU;
- }
- else if (! strcmp (str, LABEL_XML_ADDITIONAL_INFO))
- {
+ } else if (!strcmp(str, LABEL_XML_ADDITIONAL_INFO)) {
val_int = CODE_XML_ADDITIONAL_INFO;
- }
- else
- {
+ } else {
val_int = CODE_XML_OTHER;
}
res = val_int;
@@ -1202,6 +1105,7 @@ vpXmlParserCamera::str2xmlcode (char * str, vpXmlCodeType & res)
return back;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has no symbols
-void dummy_vpXmlParserCamera() {};
-#endif //VISP_HAVE_XML2
+// Work arround to avoid warning: libvisp_core.a(vpXmlParserCamera.cpp.o) has
+// no symbols
+void dummy_vpXmlParserCamera(){};
+#endif // VISP_HAVE_XML2
diff --git a/modules/core/src/display/vpColor.cpp b/modules/core/src/display/vpColor.cpp
index 700f94d..9a235d3 100644
--- a/modules/core/src/display/vpColor.cpp
+++ b/modules/core/src/display/vpColor.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,53 +39,57 @@
#include <visp3/core/vpColor.h>
-// FS: Sould be improved to avoid the #if preprocessor line. Not a good idea to define colors in static.
+// FS: Sould be improved to avoid the #if preprocessor line. Not a good idea
+// to define colors in static.
// See also vpColor.h where things need to be improved.
//#if !defined(visp_EXPORTS)
#if !defined(VISP_USE_MSVC) || (defined(VISP_USE_MSVC) && !defined(VISP_BUILD_SHARED_LIBS))
/*!< Predefined black color with R=G=B=0 and identifier vpColor::id_black. */
-vpColor const vpColor::black = vpColor(0, 0, 0, id_black);
-/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white. */
-vpColor const vpColor::white = vpColor(255, 255, 255, id_white);
-/*!< Predefined light gray color with R=G=B=64 and identifier vpColor::id_lightGray. */
-vpColor const vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
+vpColor const vpColor::black = vpColor(0, 0, 0, id_black);
+/*!< Predefined white color with R=G=B=255 and identifier vpColor::id_white.
+ */
+vpColor const vpColor::white = vpColor(255, 255, 255, id_white);
+/*!< Predefined light gray color with R=G=B=64 and identifier
+ * vpColor::id_lightGray. */
+vpColor const vpColor::lightGray = vpColor(192, 192, 192, id_lightGray);
/*!< Predefined gray color with R=G=B=128 and identifier vpColor::id_gray. */
-vpColor const vpColor::gray = vpColor(128, 128, 128, id_gray);
-/*!< Predefined dark gray color with R=G=B=192 and identifier vpColor::id_darkGray. */
-vpColor const vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
+vpColor const vpColor::gray = vpColor(128, 128, 128, id_gray);
+/*!< Predefined dark gray color with R=G=B=192 and identifier
+ * vpColor::id_darkGray. */
+vpColor const vpColor::darkGray = vpColor(64, 64, 64, id_darkGray);
/*!< Predefined light red color with R= 255 and G=B=140 and identifier
vpColor::id_lightRed. */
-vpColor const vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
+vpColor const vpColor::lightRed = vpColor(255, 140, 140, id_lightRed);
/*!< Predefined red color with R=255 and G=B=0 and identifier
vpColor::id_red. */
-vpColor const vpColor::red = vpColor(255, 0, 0, id_red);
+vpColor const vpColor::red = vpColor(255, 0, 0, id_red);
/*!< Predefined dark red color with R= 128 and G=B=0 and identifier
vpColor::id_darkRed. */
-vpColor const vpColor::darkRed = vpColor(128, 0, 0, id_darkRed);
+vpColor const vpColor::darkRed = vpColor(128, 0, 0, id_darkRed);
/*!< Predefined light green color with G= 255 and R=B=140 and identifier
vpColor::id_lightGreen. */
-vpColor const vpColor::lightGreen = vpColor(140, 255, 140, id_lightGreen);
+vpColor const vpColor::lightGreen = vpColor(140, 255, 140, id_lightGreen);
/*!< Predefined green color with G=255 and R=B=0 and identifier
vpColor::id_green. */
-vpColor const vpColor::green = vpColor(0, 255, 0, id_green);
+vpColor const vpColor::green = vpColor(0, 255, 0, id_green);
/*!< Predefined dark green color with G= 128 and R=B=0 and identifier
vpColor::id_darkGreen. */
-vpColor const vpColor::darkGreen = vpColor(0, 128, 0, id_darkGreen);
+vpColor const vpColor::darkGreen = vpColor(0, 128, 0, id_darkGreen);
/*!< Predefined light blue color with B= 255 and R=G=140 and identifier
vpColor::id_lightBlue. */
-vpColor const vpColor::lightBlue = vpColor(140, 140, 255, id_lightBlue);
+vpColor const vpColor::lightBlue = vpColor(140, 140, 255, id_lightBlue);
/*!< Predefined blue color with R=G=0 and B=255 and identifier
vpColor::id_blue. */
-vpColor const vpColor::blue = vpColor(0, 0, 255, id_blue);
+vpColor const vpColor::blue = vpColor(0, 0, 255, id_blue);
/*!< Predefined dark blue color with B= 128 and R=G=0 and identifier
vpColor::id_darkBlue. */
-vpColor const vpColor::darkBlue = vpColor(0, 0, 128, id_darkBlue);
+vpColor const vpColor::darkBlue = vpColor(0, 0, 128, id_darkBlue);
/*!< Predefined yellow color with R=G=255 and B=0 and identifier
vpColor::id_yellow. */
vpColor const vpColor::yellow = vpColor(255, 255, 0, id_yellow);
/*!< Predefined cyan color with R=0 and G=B=255 and identifier
vpColor::id_cyan. */
-vpColor const vpColor::cyan = vpColor(0, 255, 255, id_cyan);
+vpColor const vpColor::cyan = vpColor(0, 255, 255, id_cyan);
/*!< Predefined orange color with R=255, G=165 and B=0 and identifier
vpColor::id_orange. */
vpColor const vpColor::orange = vpColor(255, 165, 0, id_orange);
@@ -97,28 +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.
@@ -127,8 +131,9 @@ vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpC
\param c1,c2 : Color to compare.
*/
-VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 ) {
- return ( ( c1.R == c2.R ) && ( c1.G == c2.G ) && ( c1.B == c2.B) );
+VISP_EXPORT bool operator==(const vpColor &c1, const vpColor &c2)
+{
+ return ((c1.R == c2.R) && (c1.G == c2.G) && (c1.B == c2.B));
}
/*!
@@ -139,6 +144,7 @@ VISP_EXPORT bool operator==( const vpColor &c1, const vpColor &c2 ) {
\param c1,c2 : Color to compare.
*/
-VISP_EXPORT bool operator!=( const vpColor &c1, const vpColor &c2 ) {
- return ( ( c1.R != c2.R ) || ( c1.G != c2.G ) || ( c1.B == c2.B) );
+VISP_EXPORT bool operator!=(const vpColor &c1, const vpColor &c2)
+{
+ return ((c1.R != c2.R) || (c1.G != c2.G) || (c1.B == c2.B));
}
diff --git a/modules/core/src/display/vpDisplay.cpp b/modules/core/src/display/vpDisplay.cpp
index 262533b..d7df91e 100644
--- a/modules/core/src/display/vpDisplay.cpp
+++ b/modules/core/src/display/vpDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,9 +43,9 @@
#include <visp3/core/vpDisplayException.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
/*!
\file vpDisplay.cpp
@@ -55,8 +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)
{
}
@@ -64,27 +65,16 @@ vpDisplay::vpDisplay()
Copy constructor.
*/
vpDisplay::vpDisplay(const vpDisplay &d)
- : m_displayHasBeenInitialized(false), m_windowXPosition(0), m_windowYPosition(0),
- m_width(0), m_height(0), m_title(), m_scale(1)
+ : 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;
}
/*!
Destructor that desallocates memory.
*/
-vpDisplay::~vpDisplay()
-{
- m_displayHasBeenInitialized = false ;
-}
+vpDisplay::~vpDisplay() { m_displayHasBeenInitialized = false; }
/*!
Get the window pixmap and put it in vpRGBa image.
@@ -92,12 +82,12 @@ vpDisplay::~vpDisplay()
The code below shows how to use this method.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -136,11 +126,11 @@ int main()
vpDisplay::flush(I);
// Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
std::string ofilename("overlay.ppm");
- vpImageIo::write(Ioverlay, ofilename) ;
+ vpImageIo::write(Ioverlay, ofilename);
// Wait for a click in the display window
vpDisplay::getClick(I);
@@ -151,15 +141,11 @@ int main()
}
\endcode
*/
-void
-vpDisplay::getImage(const vpImage<unsigned char> &Isrc, vpImage<vpRGBa> &Idest )
+void vpDisplay::getImage(const vpImage<unsigned char> &Isrc, vpImage<vpRGBa> &Idest)
{
- if ( Isrc.display != NULL )
- {
- ( Isrc.display )->getImage ( Idest ) ;
- }
- else
- {
+ if (Isrc.display != NULL) {
+ (Isrc.display)->getImage(Idest);
+ } else {
vpImageConvert::convert(Isrc, Idest);
}
}
@@ -170,12 +156,12 @@ vpDisplay::getImage(const vpImage<unsigned char> &Isrc, vpImage<vpRGBa> &Idest
The code below shows how to use this method.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -214,11 +200,11 @@ int main()
vpDisplay::flush(I);
// Updates the color image with the original loaded image and the overlay
- vpDisplay::getImage(I, Ioverlay) ;
+ vpDisplay::getImage(I, Ioverlay);
// Write the color image on the disk
std::string ofilename("overlay.ppm");
- vpImageIo::write(Ioverlay, ofilename) ;
+ vpImageIo::write(Ioverlay, ofilename);
// Wait for a click in the display window
vpDisplay::getClick(I);
@@ -229,44 +215,42 @@ int main()
}
\endcode
*/
-void
-vpDisplay::getImage(const vpImage<vpRGBa> &Isrc, vpImage<vpRGBa> &Idest)
+void vpDisplay::getImage(const vpImage<vpRGBa> &Isrc, vpImage<vpRGBa> &Idest)
{
- if ( Isrc.display != NULL )
- {
- ( Isrc.display )->getImage ( Idest ) ;
- }
- else {
+ if (Isrc.display != NULL) {
+ (Isrc.display)->getImage(Idest);
+ } else {
Idest = Isrc;
}
}
/*!
- Set the down scale factor applied to the image in order to reduce the display size.
- \param scale : Scale factor applied to display a rescaled image.
+ Set the down scale factor applied to the image in order to reduce the
+ display size. \param scale : Scale factor applied to display a rescaled
+ image.
*/
void vpDisplay::setDownScalingFactor(unsigned int scale)
{
- if (! m_displayHasBeenInitialized)
+ 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;
}
}
/*!
- * Computes the down scaling factor that should be applied to the window size to display
- * the image given the resolution of the screen.
- * \param width, height : Image size.
- * \return
+ * Computes the down scaling factor that should be applied to the window size
+ * 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 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;
}
@@ -277,7 +261,7 @@ void vpDisplay::setScale(vpScaleType scaleType, unsigned int width, unsigned int
{
switch (scaleType) {
case vpDisplay::SCALE_AUTO:
- setDownScalingFactor( computeAutoScale(width, height) );
+ setDownScalingFactor(computeAutoScale(width, height));
break;
case vpDisplay::SCALE_DEFAULT:
case vpDisplay::SCALE_1:
@@ -318,8 +302,8 @@ void vpDisplay::setScale(vpScaleType scaleType, unsigned int width, unsigned int
This method has to be called before display initialization.
\code
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
int main()
{
@@ -329,7 +313,8 @@ int main()
#elif defined VISP_HAVE_GDI
vpDisplayGDI d;
#endif
- d.setDownScalingFactor(vpDisplay::SCALE_4); // Display in a 160 by 120 windows size
+ // Display in a 160 by 120 windows size
+ d.setDownScalingFactor(vpDisplay::SCALE_4);
d.init(I);
vpDisplay::display(I);
vpDisplay::flush(I);
@@ -339,6 +324,6 @@ int main()
*/
void vpDisplay::setDownScalingFactor(vpScaleType scaleType)
{
- if (! m_displayHasBeenInitialized)
+ if (!m_displayHasBeenInitialized)
m_scaleType = scaleType;
}
diff --git a/modules/core/src/display/vpDisplay_impl.h b/modules/core/src/display/vpDisplay_impl.h
index 342e9e9..db55142 100644
--- a/modules/core/src/display/vpDisplay_impl.h
+++ b/modules/core/src/display/vpDisplay_impl.h
@@ -1,193 +1,171 @@
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
-template <class Type> void
-vp_display_close(vpImage<Type> &I)
+template <class Type> void vp_display_close(vpImage<Type> &I)
{
- if ( I.display != NULL )
- {
- ( I.display )->closeDisplay() ;
+ if (I.display != NULL) {
+ (I.display)->closeDisplay();
I.display = NULL;
}
-
}
-template <class Type> void
-vp_display_display(const vpImage<Type> &I)
+template <class Type> void vp_display_display(const vpImage<Type> &I)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayImage ( I ) ;
+ if (I.display != NULL) {
+ (I.display)->displayImage(I);
}
-
}
-template <class Type> void
-vp_display_display_arrow(const vpImage<Type> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int w,unsigned int h, unsigned int thickness )
+template <class Type>
+void vp_display_display_arrow(const vpImage<Type> &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 ) ;
+ if (I.display != NULL) {
+ (I.display)->displayArrow(ip1, ip2, color, w, h, thickness);
}
}
-template <class Type> void
-vp_display_display_arrow(const vpImage<Type> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness)
+template <class Type>
+void vp_display_display_arrow(const vpImage<Type> &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 )
- {
+ if (I.display != NULL) {
vpImagePoint ip1, ip2;
ip1.set_i(i1);
ip1.set_j(j1);
ip2.set_i(i2);
ip2.set_j(j2);
- ( I.display )->displayArrow ( ip1, ip2, color, w, h, thickness ) ;
+ (I.display)->displayArrow(ip1, ip2, color, w, h, thickness);
}
}
-template <class Type> void
-vp_display_display_camera(const vpImage<Type> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
+template <class Type>
+void vp_display_display_camera(const vpImage<Type> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ double size, const vpColor &color, unsigned int thickness)
{
// used by display
- double halfSize = size/2.0;
+ double halfSize = size / 2.0;
vpPoint pt[5];
- pt[0].setWorldCoordinates ( -halfSize,-halfSize,0.0 );
- pt[1].setWorldCoordinates ( halfSize,-halfSize,0.0 );
- pt[2].setWorldCoordinates ( halfSize,halfSize,0.0 );
- pt[3].setWorldCoordinates ( -halfSize,halfSize,0.0 );
- pt[4].setWorldCoordinates ( 0.0,0.0,-size );
+ pt[0].setWorldCoordinates(-halfSize, -halfSize, 0.0);
+ pt[1].setWorldCoordinates(halfSize, -halfSize, 0.0);
+ pt[2].setWorldCoordinates(halfSize, halfSize, 0.0);
+ pt[3].setWorldCoordinates(-halfSize, halfSize, 0.0);
+ pt[4].setWorldCoordinates(0.0, 0.0, -size);
for (int i = 0; i < 5; i++)
- pt[i].track ( cMo ) ;
+ pt[i].track(cMo);
vpImagePoint ip, ip_1, ip0;
- vpMeterPixelConversion::convertPoint ( cam, pt[4].p[0], pt[4].p[1], ip0);
+ vpMeterPixelConversion::convertPoint(cam, pt[4].p[0], pt[4].p[1], ip0);
- for (int i = 0; i < 4; i++)
- {
- vpMeterPixelConversion::convertPoint ( cam, pt[i].p[0], pt[i].p[1], ip_1);
- vpMeterPixelConversion::convertPoint ( cam, pt[(i+1)%4].p[0], pt[(i+1)%4].p[1], ip);
- vpDisplay::displayLine ( I, ip_1, ip, color, thickness);
- vpDisplay::displayLine ( I, ip0, ip_1, color, thickness);
+ for (int i = 0; i < 4; i++) {
+ vpMeterPixelConversion::convertPoint(cam, pt[i].p[0], pt[i].p[1], ip_1);
+ vpMeterPixelConversion::convertPoint(cam, pt[(i + 1) % 4].p[0], pt[(i + 1) % 4].p[1], ip);
+ vpDisplay::displayLine(I, ip_1, ip, color, thickness);
+ vpDisplay::displayLine(I, ip0, ip_1, color, thickness);
}
}
-
-
-template <class Type> void
-vp_display_display_char_string(const vpImage<Type> &I, const vpImagePoint &ip,
- const char *string, const vpColor &color )
+template <class Type>
+void vp_display_display_char_string(const vpImage<Type> &I, const vpImagePoint &ip, const char *string,
+ const vpColor &color)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, string, color ) ;
+ if (I.display != NULL) {
+ (I.display)->displayCharString(ip, string, color);
}
}
-template <class Type> void
-vp_display_display_char_string(const vpImage<Type> &I, int i, int j,
- const char *string, const vpColor &color)
+template <class Type>
+void vp_display_display_char_string(const vpImage<Type> &I, int i, int j, const char *string, const vpColor &color)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
+ ip.set_i(i);
+ ip.set_j(j);
- ( I.display )->displayCharString ( ip, string, color ) ;
+ (I.display)->displayCharString(ip, string, color);
}
}
-template <class Type> void
-vp_display_display_circle(const vpImage<Type> &I, const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+template <class Type>
+void vp_display_display_circle(const vpImage<Type> &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 ) ;
+ if (I.display != NULL) {
+ (I.display)->displayCircle(center, radius, color, fill, thickness);
}
}
-template <class Type> void
-vp_display_display_circle(const vpImage<Type> &I, int i, int j, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+template <class Type>
+void vp_display_display_circle(const vpImage<Type> &I, int i, int j, unsigned int radius, const vpColor &color,
+ bool fill, unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
+ ip.set_i(i);
+ ip.set_j(j);
- ( I.display )->displayCircle ( ip, radius, color, fill, thickness ) ;
+ (I.display)->displayCircle(ip, radius, color, fill, thickness);
}
}
-template <class Type> void
-vp_display_display_cross(const vpImage<Type> &I, const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness)
+template <class Type>
+void vp_display_display_cross(const vpImage<Type> &I, const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ if (I.display != NULL) {
+ (I.display)->displayCross(ip, size, color, thickness);
}
}
-template <class Type> void
-vp_display_display_cross(const vpImage<Type> &I, int i, int j, unsigned int size,
- const vpColor &color, unsigned int thickness)
+template <class Type>
+void vp_display_display_cross(const vpImage<Type> &I, int i, int j, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
+ ip.set_i(i);
+ ip.set_j(j);
- ( I.display )->displayCross ( ip, size, color, thickness ) ;
+ (I.display)->displayCross(ip, size, color, thickness);
}
}
-template <class Type> void
-vp_display_display_dot_line(const vpImage<Type> &I, const vpImagePoint &ip1,
- const vpImagePoint &ip2, const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_dot_line(const vpImage<Type> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ if (I.display != NULL) {
+ (I.display)->displayDotLine(ip1, ip2, color, thickness);
}
}
-template <class Type> void
-vp_display_display_dot_line(const vpImage<Type> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_dot_line(const vpImage<Type> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayDotLine ( ip1, ip2, color, thickness );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
+ (I.display)->displayDotLine(ip1, ip2, color, thickness);
}
}
-template <class Type> void
-vp_display_display_ellipse(const vpImage<Type> &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)
+template <class Type>
+void vp_display_display_ellipse(const vpImage<Type> &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 )
- {
+ if (I.display != NULL) {
double j1, i1;
vpImagePoint iP11;
double j2, i2;
vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
- double a=0., b=0., e=0.;
+ j1 = j2 = i1 = i2 = 0;
+ double a = 0., b = 0., e = 0.;
double mu20_p = coef1;
double mu11_p = coef2;
@@ -196,20 +174,18 @@ vp_display_display_ellipse(const vpImage<Type> &I, const vpImagePoint ¢er,
if (use_centered_moments) {
if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
+ double val_p = sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p) / 2);
+ b = sqrt((mu20_p + mu02_p - val_p) / 2);
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
+ e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
e = atan(e);
- }
- else {
+ } else {
a = sqrt(mu20_p);
b = sqrt(mu02_p);
e = 0.;
}
- }
- else {
+ } else {
a = coef1;
b = coef2;
e = coef3;
@@ -218,263 +194,236 @@ vp_display_display_ellipse(const vpImage<Type> &I, const vpImagePoint ¢er,
// Approximation of the circumference of an ellipse:
// [Ramanujan, S., "Modular Equations and Approximations to ,"
// Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
- double t = (a-b)/(a+b);
- double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
+ double t = (a - b) / (a + b);
+ double circumference = M_PI * (a + b) * (1 + 3 * vpMath::sqr(t) / (10 + sqrt(4 - 3 * vpMath::sqr(t))));
- int nbpoints = (int)(floor(circumference/5));
+ int nbpoints = (int)(floor(circumference / 5));
if (nbpoints < 10)
nbpoints = 10;
- double incr = 2*M_PI / nbpoints ; // angle increment
+ double incr = 2 * M_PI / nbpoints; // angle increment
double smallalpha = theta1;
- double highalpha = theta2;
+ double highalpha = theta2;
double ce = cos(e);
double se = sin(e);
- double k = smallalpha ;
- j1 = a *cos(k) ; // equation of an ellipse
- i1 = b *sin(k) ; // equation of an ellipse
+ double k = smallalpha;
+ j1 = a * cos(k); // equation of an ellipse
+ i1 = b * sin(k); // equation of an ellipse
// (i1,j1) are the coordinates on the origin centered ellipse ;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + ce *j1 - se *i1 );
- iP11.set_i ( center.get_i() + se *j1 + ce *i1 );
+ iP11.set_j(center.get_j() + ce * j1 - se * i1);
+ iP11.set_i(center.get_i() + se * j1 + ce * i1);
- while (k+incr<highalpha+incr)
- {
- j2 = a *cos(k+incr) ; // equation of an ellipse
- i2 = b *sin(k+incr) ; // equation of an ellipse
+ while (k + incr < highalpha + incr) {
+ j2 = a * cos(k + incr); // equation of an ellipse
+ i2 = b * sin(k + incr); // equation of an ellipse
// to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + ce *j2 - se *i2 );
- iP22.set_i ( center.get_i() + se *j2 + ce *i2 );
+ iP22.set_j(center.get_j() + ce * j2 - se * i2);
+ iP22.set_i(center.get_i() + se * j2 + ce * i2);
- ( I.display )->displayLine(iP11, iP22, color, thickness) ;
+ (I.display)->displayLine(iP11, iP22, color, thickness);
iP11 = iP22;
- k += incr ;
+ k += incr;
}
}
}
-template <class Type> void
-vp_display_display_frame(const vpImage<Type> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color,
- unsigned int thickness, vpImagePoint offset)
+template <class Type>
+void vp_display_display_frame(const vpImage<Type> &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);
- vpPoint x(size, 0.0, 0.0);
- vpPoint y( 0.0, size, 0.0);
- vpPoint z( 0.0, 0.0, size);
+ vpPoint o(0.0, 0.0, 0.0);
+ vpPoint x(size, 0.0, 0.0);
+ vpPoint y(0.0, size, 0.0);
+ vpPoint z(0.0, 0.0, size);
- o.track ( cMo ) ;
- x.track ( cMo ) ;
- y.track ( cMo ) ;
- z.track ( cMo ) ;
+ o.track(cMo);
+ x.track(cMo);
+ y.track(cMo);
+ z.track(cMo);
vpImagePoint ipo, ip1;
- if ( color == vpColor::none )
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+ if (color == vpColor::none) {
+ vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::red, 4*thickness, 2*thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, vpColor::red, 4 * thickness, 2 * thickness, thickness);
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::green, 4*thickness, 2*thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, vpColor::green, 4 * thickness, 2 * thickness, thickness);
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, vpColor::blue, 4*thickness, 2*thickness, thickness) ;
- }
- else
- {
- vpMeterPixelConversion::convertPoint ( cam, o.p[0], o.p[1], ipo) ;
+ vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, vpColor::blue, 4 * thickness, 2 * thickness, thickness);
+ } else {
+ vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
- vpMeterPixelConversion::convertPoint ( cam, x.p[0], x.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
- vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
- vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
- vpDisplay::displayArrow ( I, ipo+offset, ip1+offset, color, 4*thickness, 2*thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ip1);
+ vpDisplay::displayArrow(I, ipo + offset, ip1 + offset, color, 4 * thickness, 2 * thickness, thickness);
}
}
-template <class Type> void
-vp_display_display_line(const vpImage<Type> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_line(const vpImage<Type> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ if (I.display != NULL) {
+ (I.display)->displayLine(ip1, ip2, color, thickness);
}
}
-template <class Type> void
-vp_display_display_line(const vpImage<Type> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_line(const vpImage<Type> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip1, ip2;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
- ( I.display )->displayLine ( ip1, ip2, color, thickness );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
+ (I.display)->displayLine(ip1, ip2, color, thickness);
}
}
-template <class Type> void
-vp_display_display_point(const vpImage<Type> &I, const vpImagePoint &ip,
- const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_point(const vpImage<Type> &I, const vpImagePoint &ip, const vpColor &color,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayPoint ( ip, color, thickness ) ;
+ if (I.display != NULL) {
+ (I.display)->displayPoint(ip, color, thickness);
}
}
-template <class Type> void
-vp_display_display_point(const vpImage<Type> &I, int i, int j,
- const vpColor &color, unsigned int thickness )
+template <class Type>
+void vp_display_display_point(const vpImage<Type> &I, int i, int j, const vpColor &color, unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
- ( I.display )->displayPoint ( ip, color, thickness ) ;
+ ip.set_i(i);
+ ip.set_j(j);
+ (I.display)->displayPoint(ip, color, thickness);
}
}
-template <class Type> void
-vp_display_display_polygon(const vpImage<Type> &I, const std::vector<vpImagePoint> &vip,
- const vpColor &color, unsigned int thickness)
+template <class Type>
+void vp_display_display_polygon(const vpImage<Type> &I, const std::vector<vpImagePoint> &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 );
+ 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);
}
}
-template <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I, const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &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 ) ;
+ if (I.display != NULL) {
+ (I.display)->displayRectangle(topLeft, width, height, color, fill, thickness);
}
}
-template <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I, const vpRect &rectangle,
- const vpColor &color, bool fill, unsigned int thickness )
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &I, const vpRect &rectangle, const vpColor &color, bool fill,
+ unsigned int thickness)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayRectangle ( rectangle, color, fill, thickness ) ;
+ if (I.display != NULL) {
+ (I.display)->displayRectangle(rectangle, color, fill, thickness);
}
}
-template <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I, const vpImagePoint ¢er,
- float angle, unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &I, const vpImagePoint ¢er, float angle, unsigned int width,
+ unsigned int height, const vpColor &color, unsigned int thickness)
{
- if (I.display != NULL)
- {
+ if (I.display != NULL) {
double i = center.get_i();
double j = center.get_j();
- //A, B, C, D, corners of the rectangle clockwise
+ // A, B, C, D, corners of the rectangle clockwise
vpImagePoint ipa, ipb, ipc, ipd;
double cosinus = cos(angle);
double sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+ ipa.set_u(j + 0.5 * width * cosinus + 0.5 * height * sinus);
+ ipa.set_v(i + 0.5 * width * sinus - 0.5 * height * cosinus);
+ ipb.set_u(j + 0.5 * width * cosinus - 0.5 * height * sinus);
+ ipb.set_v(i + 0.5 * width * sinus + 0.5 * height * cosinus);
+ ipc.set_u(j - 0.5 * width * cosinus - 0.5 * height * sinus);
+ ipc.set_v(i - 0.5 * width * sinus + 0.5 * height * cosinus);
+ ipd.set_u(j - 0.5 * width * cosinus + 0.5 * height * sinus);
+ ipd.set_v(i - 0.5 * width * sinus - 0.5 * height * cosinus);
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ (I.display)->displayLine(I, ipa, ipb, color, thickness);
+ (I.display)->displayLine(I, ipa, ipd, color, thickness);
+ (I.display)->displayLine(I, ipc, ipb, color, thickness);
+ (I.display)->displayLine(I, ipc, ipd, color, thickness);
}
}
-template <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I,
- const vpImagePoint &topLeft, const vpImagePoint &bottomRight,
- const vpColor &color, bool fill, unsigned int thickness)
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &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 ) ;
+ if (I.display != NULL) {
+ (I.display)->displayRectangle(topLeft, bottomRight, color, fill, thickness);
}
}
-template <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I,
- int i, int j, unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &I, int i, int j, unsigned int width, unsigned int height,
+ const vpColor &color, bool fill, unsigned int thickness)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint topLeft;
- topLeft.set_i( i );
- topLeft.set_j( j );
+ 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 <class Type> void
-vp_display_display_rectangle(const vpImage<Type> &I,
- unsigned int i, unsigned int j, float angle, unsigned int width, unsigned int height,
- const vpColor &color,unsigned int thickness)
+template <class Type>
+void vp_display_display_rectangle(const vpImage<Type> &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
+ if (I.display != NULL) {
+ // A, B, C, D, corners of the rectangle clockwise
vpImagePoint ipa, ipb, ipc, ipd;
float cosinus = cos(angle);
float sinus = sin(angle);
- ipa.set_u(j + 0.5*width*cosinus + 0.5*height*sinus);
- ipa.set_v(i + 0.5*width*sinus - 0.5*height*cosinus);
- ipb.set_u(j + 0.5*width*cosinus - 0.5*height*sinus);
- ipb.set_v(i + 0.5*width*sinus + 0.5*height*cosinus);
- ipc.set_u(j - 0.5*width*cosinus - 0.5*height*sinus);
- ipc.set_v(i - 0.5*width*sinus + 0.5*height*cosinus);
- ipd.set_u(j - 0.5*width*cosinus + 0.5*height*sinus);
- ipd.set_v(i - 0.5*width*sinus - 0.5*height*cosinus);
+ ipa.set_u(j + 0.5 * width * cosinus + 0.5 * height * sinus);
+ ipa.set_v(i + 0.5 * width * sinus - 0.5 * height * cosinus);
+ ipb.set_u(j + 0.5 * width * cosinus - 0.5 * height * sinus);
+ ipb.set_v(i + 0.5 * width * sinus + 0.5 * height * cosinus);
+ ipc.set_u(j - 0.5 * width * cosinus - 0.5 * height * sinus);
+ ipc.set_v(i - 0.5 * width * sinus + 0.5 * height * cosinus);
+ ipd.set_u(j - 0.5 * width * cosinus + 0.5 * height * sinus);
+ ipd.set_v(i - 0.5 * width * sinus - 0.5 * height * cosinus);
- ( I.display )->displayLine(I, ipa, ipb, color, thickness);
- ( I.display )->displayLine(I, ipa, ipd, color, thickness);
- ( I.display )->displayLine(I, ipc, ipb, color, thickness);
- ( I.display )->displayLine(I, ipc, ipd, color, thickness);
+ (I.display)->displayLine(I, ipa, ipb, color, thickness);
+ (I.display)->displayLine(I, ipa, ipd, color, thickness);
+ (I.display)->displayLine(I, ipc, ipb, color, thickness);
+ (I.display)->displayLine(I, ipc, ipd, color, thickness);
}
}
-template <class Type> void
-vp_display_display_roi(const vpImage<Type> &I, const vpRect &roi)
+template <class Type> void vp_display_display_roi(const vpImage<Type> &I, const vpRect &roi)
{
- vpImagePoint topLeft;
double top = floor(roi.getTop());
double left = floor(roi.getLeft());
double roiheight = floor(roi.getHeight());
@@ -482,199 +431,161 @@ vp_display_display_roi(const vpImage<Type> &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 ) ;
+ if (I.display != NULL) {
+ (I.display)->displayImageROI(I, vpImagePoint(top, left), (unsigned int)roiwidth, (unsigned int)roiheight);
}
}
-template <class Type> void
-vp_display_flush(const vpImage<Type> &I)
+template <class Type> void vp_display_flush(const vpImage<Type> &I)
{
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplay() ;
+ if (I.display != NULL) {
+ (I.display)->flushDisplay();
}
}
-template <class Type> void
-vp_display_flush_roi(const vpImage<Type> &I, const vpRect &roi)
+template <class Type> void vp_display_flush_roi(const vpImage<Type> &I, const vpRect &roi)
{
- if ( I.display != NULL )
- {
- ( I.display )->flushDisplayROI(roi.getTopLeft(),(unsigned int)roi.getWidth(),(unsigned int)roi.getHeight()) ;
+ if (I.display != NULL) {
+ (I.display)->flushDisplayROI(roi.getTopLeft(), (unsigned int)roi.getWidth(), (unsigned int)roi.getHeight());
}
}
-template <class Type> bool
-vp_display_get_click(const vpImage<Type> &I, bool blocking)
+template <class Type> bool vp_display_get_click(const vpImage<Type> &I, bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getClick(blocking) ;
+ if (I.display != NULL) {
+ return (I.display)->getClick(blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_click(const vpImage<Type> &I, vpImagePoint &ip, bool blocking)
+template <class Type> bool vp_display_get_click(const vpImage<Type> &I, vpImagePoint &ip, bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, blocking ) ;
+ if (I.display != NULL) {
+ return (I.display)->getClick(ip, blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_click(const vpImage<Type> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking)
+template <class Type>
+bool vp_display_get_click(const vpImage<Type> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getClick ( ip, button, blocking ) ;
+ if (I.display != NULL) {
+ return (I.display)->getClick(ip, button, blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_click_up(const vpImage<Type> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking )
+template <class Type>
+bool vp_display_get_click_up(const vpImage<Type> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getClickUp ( ip, button, blocking ) ;
+ if (I.display != NULL) {
+ return (I.display)->getClickUp(ip, button, blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_keyboard_event(const vpImage<Type> &I, bool blocking)
+template <class Type> bool vp_display_get_keyboard_event(const vpImage<Type> &I, bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( blocking ) ;
+ if (I.display != NULL) {
+ return (I.display)->getKeyboardEvent(blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_keyboard_event(const vpImage<Type> &I, std::string &key, bool blocking)
+template <class Type> bool vp_display_get_keyboard_event(const vpImage<Type> &I, std::string &key, bool blocking)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getKeyboardEvent ( key, blocking ) ;
+ if (I.display != NULL) {
+ return (I.display)->getKeyboardEvent(key, blocking);
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_keyboard_event(const vpImage<Type> &I, char *key, bool blocking)
+template <class Type> bool vp_display_get_keyboard_event(const vpImage<Type> &I, char *key, bool blocking)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
std::string str;
- bool ret = ( I.display )->getKeyboardEvent ( str, blocking );
+ bool ret = (I.display)->getKeyboardEvent(str, blocking);
sprintf(key, "%s", str.c_str());
return ret;
}
- return false ;
+ return false;
}
-template <class Type> bool
-vp_display_get_pointer_motion_event(const vpImage<Type> &I, vpImagePoint &ip)
+template <class Type> bool vp_display_get_pointer_motion_event(const vpImage<Type> &I, vpImagePoint &ip)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerMotionEvent ( ip ) ;
+ if (I.display != NULL) {
+ return (I.display)->getPointerMotionEvent(ip);
}
return false;
}
-template <class Type> bool
-vp_display_get_pointer_position(const vpImage<Type> &I, vpImagePoint &ip)
+template <class Type> bool vp_display_get_pointer_position(const vpImage<Type> &I, vpImagePoint &ip)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getPointerPosition ( ip ) ;
+ if (I.display != NULL) {
+ return (I.display)->getPointerPosition(ip);
}
return false;
}
-template <class Type> void
-vp_display_set_background(const vpImage<Type> &I, const vpColor &color)
+template <class Type> void vp_display_set_background(const vpImage<Type> &I, const vpColor &color)
{
- if ( I.display != NULL )
- {
- ( I.display )->clearDisplay ( color ) ;
+ if (I.display != NULL) {
+ (I.display)->clearDisplay(color);
}
}
-template <class Type> void
-vp_display_display_text(const vpImage<Type> &I, const vpImagePoint &ip,
- const std::string &s, const vpColor &color )
+template <class Type>
+void vp_display_display_text(const vpImage<Type> &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
{
- if ( I.display != NULL )
- {
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ if (I.display != NULL) {
+ (I.display)->displayCharString(ip, s.c_str(), color);
}
}
-template <class Type> void
-vp_display_display_text(const vpImage<Type> &I, int i, int j,
- const std::string &s, const vpColor &color)
+template <class Type>
+void vp_display_display_text(const vpImage<Type> &I, int i, int j, const std::string &s, const vpColor &color)
{
- if ( I.display != NULL )
- {
+ if (I.display != NULL) {
vpImagePoint ip;
- ip.set_i( i );
- ip.set_j( j );
+ ip.set_i(i);
+ ip.set_j(j);
- ( I.display )->displayCharString ( ip, s.c_str(), color ) ;
+ (I.display)->displayCharString(ip, s.c_str(), color);
}
}
-template <class Type> void
-vp_display_set_font(const vpImage<Type> &I, const std::string &fontname )
+template <class Type> void vp_display_set_font(const vpImage<Type> &I, const std::string &fontname)
{
- if ( I.display != NULL )
- {
- ( I.display )->setFont ( fontname) ;
+ if (I.display != NULL) {
+ (I.display)->setFont(fontname);
}
}
-
-template <class Type> void
-vp_display_set_title(const vpImage<Type> &I, const std::string &windowtitle)
+template <class Type> void vp_display_set_title(const vpImage<Type> &I, const std::string &windowtitle)
{
- if ( I.display != NULL )
- {
- ( I.display )->setTitle ( windowtitle ) ;
+ if (I.display != NULL) {
+ (I.display)->setTitle(windowtitle);
}
}
-template <class Type> void
-vp_display_set_window_position(const vpImage<Type> &I, int winx, int winy)
+template <class Type> void vp_display_set_window_position(const vpImage<Type> &I, int winx, int winy)
{
- if ( I.display != NULL )
- {
- ( I.display )->setWindowPosition ( winx, winy ) ;
+ if (I.display != NULL) {
+ (I.display)->setWindowPosition(winx, winy);
}
}
-template <class Type> unsigned int
-vp_display_get_down_scaling_factor(const vpImage<Type> &I)
+template <class Type> unsigned int vp_display_get_down_scaling_factor(const vpImage<Type> &I)
{
- if ( I.display != NULL )
- {
- return ( I.display )->getDownScalingFactor() ;
- }
- else {
- throw(vpException(vpException::fatalError,"Cannot get the down scaling factor: Display is not initialized"));
+ if (I.display != NULL) {
+ return (I.display)->getDownScalingFactor();
+ } else {
+ 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 4f1f3dd..3c75d09 100644
--- a/modules/core/src/display/vpDisplay_rgba.cpp
+++ b/modules/core/src/display/vpDisplay_rgba.cpp
@@ -3,17 +3,14 @@
#include "vpDisplay_impl.h"
//************************************************************************
-// Modifications done in this file should be reported in all vpDisplay_*.cpp files
-// that implement other types (unsigned char, vpRGB, vpRGBa)
+// Modifications done in this file should be reported in all vpDisplay_*.cpp
+// files that implement other types (unsigned char, vpRGB, vpRGBa)
//************************************************************************
/*!
Close the display attached to I.
*/
-void vpDisplay::close(vpImage<vpRGBa> &I)
-{
- vp_display_close(I);
-}
+void vpDisplay::close(vpImage<vpRGBa> &I) { vp_display_close(I); }
/*!
Display an arrow from image point \e ip1 to image point \e ip2.
@@ -23,9 +20,8 @@ void vpDisplay::close(vpImage<vpRGBa> &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<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness )
+void vpDisplay::displayArrow(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness)
{
vp_display_display_arrow(I, ip1, ip2, color, w, h, thickness);
}
@@ -40,9 +36,8 @@ vpDisplay::displayArrow(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const
\param w,h : Width and height of the arrow.
\param thickness : Thickness of the lines used to display the arrow.
*/
-void
-vpDisplay::displayArrow(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness)
+void vpDisplay::displayArrow(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int w, unsigned int h, unsigned int thickness)
{
vp_display_display_arrow(I, i1, j1, i2, j2, color, w, h, thickness);
}
@@ -60,9 +55,8 @@ vpDisplay::displayArrow(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2
\param color : Color used to display the camera in the image.
\param thickness : Thickness of the graphics drawing.
*/
-void
-vpDisplay::displayCamera(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
+void vpDisplay::displayCamera(const vpImage<vpRGBa> &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,9 +74,8 @@ vpDisplay::displayCamera(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cM
\sa setFont(), displayText()
*/
-void
-vpDisplay::displayCharString(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
- const char *string, const vpColor &color )
+void vpDisplay::displayCharString(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const char *string,
+ const vpColor &color)
{
vp_display_display_char_string(I, ip, string, color);
}
@@ -100,9 +93,7 @@ vpDisplay::displayCharString(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
\sa setFont(), displayText()
*/
-void
-vpDisplay::displayCharString(const vpImage<vpRGBa> &I, int i, int j,
- const char *string, const vpColor &color)
+void vpDisplay::displayCharString(const vpImage<vpRGBa> &I, int i, int j, const char *string, const vpColor &color)
{
vp_display_display_char_string(I, i, j, string, color);
}
@@ -117,9 +108,8 @@ vpDisplay::displayCharString(const vpImage<vpRGBa> &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<vpRGBa> &I, const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayCircle(const vpImage<vpRGBa> &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);
}
@@ -134,9 +124,8 @@ vpDisplay::displayCircle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, u
\param thickness : Thickness of the circle. This parameter is only useful
when \e fill is set to false.
*/
-void
-vpDisplay::displayCircle(const vpImage<vpRGBa> &I, int i, int j, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayCircle(const vpImage<vpRGBa> &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);
}
@@ -149,8 +138,8 @@ vpDisplay::displayCircle(const vpImage<vpRGBa> &I, int i, int j, unsigned int r
\param color : Cross color.
\param thickness : Thickness of the lines used to display the cross.
*/
-void vpDisplay::displayCross(const vpImage<vpRGBa> &I, const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayCross(const vpImage<vpRGBa> &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,8 @@ void vpDisplay::displayCross(const vpImage<vpRGBa> &I, const vpImagePoint &ip, u
\param color : Cross color.
\param thickness : Thickness of the lines used to display the cross.
*/
-void vpDisplay::displayCross(const vpImage<vpRGBa> &I, int i, int j, unsigned int size,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayCross(const vpImage<vpRGBa> &I, int i, int j, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_cross(I, i, j, size, color, thickness);
}
@@ -176,8 +165,8 @@ void vpDisplay::displayCross(const vpImage<vpRGBa> &I, int i, int j, unsigned in
\param color : Line color.
\param thickness : Dashed line thickness.
*/
-void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1,
- const vpImagePoint &ip2, const vpColor &color, unsigned int thickness )
+void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness)
{
vp_display_display_dot_line(I, ip1, ip2, color, thickness);
}
@@ -190,28 +179,51 @@ void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1
\param color : Line color.
\param thickness : Dashed line thickness.
*/
-void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayDotLine(const vpImage<vpRGBa> &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);
}
/*!
+ Display the dashed lines formed by the list of image points
+ \param I : The image associated to the display.
+ \param ips : List of image points.
+ \param closeTheShape : If true, display a dashed line from the first and
+ last image points. \param color : Line color. \param thickness : Dashed line
+ thickness.
+*/
+void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &ips, const bool closeTheShape,
+ const vpColor &color, unsigned int thickness)
+{
+ if (ips.size() <= 1)
+ return;
+
+ for (size_t i = 0; i < ips.size() - 1; i++)
+ vp_display_display_dot_line(I, ips[i], ips[i + 1], color, thickness);
+
+ if (closeTheShape)
+ vp_display_display_dot_line(I, ips.front(), ips.back(), color, thickness);
+}
+
+/*!
Display an ellipse from its parameters expressed in pixels.
\param I : Image to consider.
\param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+ \param coef1, coef2, coef3 : Depending on the parameter \e
+ use_centered_moments these parameters are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the
+ ellipse in radians: \f$a, b, e\f$. \param use_centered_moments : When false,
+ the parameters coef1, coef2, coef3 are the parameters \f$a, b, e\f$. When
+ true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we
+ compute the parameters \e a, \e b and \e e from the centered moments. \param
+ color : Drawings color. \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to
+ its parametric representation:
\f[ \left(\begin{array}{c}
u_\theta \\
@@ -229,43 +241,48 @@ void vpDisplay::displayDotLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2,
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.
+ The following example shows how to use for example this function to display
+ the result of a tracking.
\code
vpMeEllipse ellipse;
...
vpDisplay::display(I);
- ellipse.track(I) ;
+ ellipse.track(I);
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
- true, vpColor::orange, 1);
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
+ ellipse.get_mu11(), ellipse.get_mu02(), true, vpColor::orange, 1);
vpDisplay::flush(I);
\endcode
*/
-void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments, const vpColor &color, unsigned int thickness)
+void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, bool use_centered_moments,
+ const vpColor &color, unsigned int thickness)
{
- vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
+ vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color,
+ thickness);
}
/*!
Display an ellipse from its parameters expressed in pixels.
\param I : Image to consider.
\param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
- If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
+ \param coef1, coef2, coef3 : Depending on the parameter \e
+ use_centered_moments these parameters are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the
+ ellipse in radians: \f$a, b, e\f$. \param theta1, theta2 : Angles
+ \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the
+ ellipse. If theta1=0 and theta2=vpMath::rad(360) all the ellipse is
+ displayed. \param use_centered_moments : When false, the parameters coef1,
+ coef2, coef3 are the parameters \f$a, b, e\f$. When true, the parameters
+ coef1, coef2, coef3 are rather the centered moments \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e
+ a, \e b and \e e from the centered moments. \param color : Drawings color.
\param thickness : Drawings thickness.
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to
+ its parametric representation:
\f[ \left(\begin{array}{c}
u_\theta \\
@@ -283,24 +300,24 @@ void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint &cen
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.
+ The following example shows how to use for example this function to display
+ the result of a tracking.
\code
vpMeEllipse ellipse;
...
vpDisplay::display(I);
- ellipse.track(I) ;
+ ellipse.track(I);
vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
ellipse.get_mu11(), ellipse.get_mu02(),
- ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
- true, vpColor::orange, 1);
+ ellipse.getSmallestAngle(),
+ ellipse.getHighestAngle(), true, vpColor::orange, 1);
vpDisplay::flush(I);
\endcode
*/
-void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &theta1, const double &theta2, bool use_centered_moments,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, const double &theta1, const double &theta2,
+ bool use_centered_moments, const vpColor &color, unsigned int thickness)
{
vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, use_centered_moments, color, thickness);
}
@@ -317,12 +334,11 @@ void vpDisplay::displayEllipse(const vpImage<vpRGBa> &I, const vpImagePoint &cen
\param size : Size of the object frame.
\param color : Color used to display the frame in the image.
\param thickness : the thickness of the line.
- \param offset : Offset in pixels applied to the frame origin location in the image.
+ \param offset : Offset in pixels applied to the frame origin location in the
+ image.
*/
-void
-vpDisplay::displayFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color,
- unsigned int thickness, vpImagePoint offset)
+void vpDisplay::displayFrame(const vpImage<vpRGBa> &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);
}
@@ -335,7 +351,7 @@ vpDisplay::displayFrame(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo
\param thickness : Line thickness.
*/
void vpDisplay::displayLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness )
+ const vpColor &color, unsigned int thickness)
{
vp_display_display_line(I, ip1, ip2, color, thickness);
}
@@ -348,21 +364,41 @@ void vpDisplay::displayLine(const vpImage<vpRGBa> &I, const vpImagePoint &ip1, c
\param color : Line color.
\param thickness : Line thickness.
*/
-void vpDisplay::displayLine(const vpImage<vpRGBa> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayLine(const vpImage<vpRGBa> &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);
}
/*!
+ Display the lines formed by the list of image points.
+ \param I : The image associated to the display.
+ \param ips : List of image points.
+ \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<vpRGBa> &I, const std::vector<vpImagePoint> &ips, const bool closeTheShape,
+ const vpColor &color, unsigned int thickness)
+{
+ if (ips.size() <= 1)
+ return;
+
+ for (size_t i = 0; i < ips.size() - 1; i++)
+ vp_display_display_line(I, ips[i], ips[i + 1], color, thickness);
+
+ if (closeTheShape)
+ vp_display_display_line(I, ips.front(), ips.back(), color, thickness);
+}
+
+/*!
Display a point at the image point \e ip location.
\param I : The image associated to the display.
\param ip : Point location.
\param color : Point color.
\param thickness : Thickness of the point
*/
-void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_point(I, ip, color, thickness);
}
@@ -374,11 +410,9 @@ void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
\param color : Point color.
\param thickness : Thickness of the point
*/
-void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, int i, int j,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, int i, int j, const vpColor &color, unsigned int thickness)
{
vp_display_display_point(I, i, j, color, thickness);
-
}
/*!
@@ -388,9 +422,8 @@ void vpDisplay::displayPoint(const vpImage<vpRGBa> &I, int i, int j,
\param color : Line color.
\param thickness : Line thickness.
*/
-void
-vpDisplay::displayPolygon(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayPolygon(const vpImage<vpRGBa> &I, const std::vector<vpImagePoint> &vip, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_polygon(I, vip, color, thickness);
}
@@ -409,10 +442,8 @@ vpDisplay::displayPolygon(const vpImage<vpRGBa> &I, const std::vector<vpImagePoi
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &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);
}
@@ -431,10 +462,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint &topLef
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
- int i, int j, unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, int i, int j, unsigned int width, unsigned int height,
+ const vpColor &color, bool fill, unsigned int thickness)
{
vp_display_display_rectangle(I, i, j, width, height, color, fill, thickness);
}
@@ -452,9 +481,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpRect &rectangle,
- const vpColor &color, bool fill, unsigned int thickness )
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpRect &rectangle, const vpColor &color, bool fill,
+ unsigned int thickness)
{
vp_display_display_rectangle(I, rectangle, color, fill, thickness);
}
@@ -472,10 +500,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpRect &rectangle,
\param thickness : Thickness of the four lines used to display the
rectangle.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er,
- float angle, unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &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);
}
@@ -494,10 +520,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, const vpImagePoint ¢er
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
- const vpImagePoint &topLeft, const vpImagePoint &bottomRight,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &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);
}
@@ -515,10 +539,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
\param thickness : Thickness of the four lines used to display the
rectangle.
*/
-void
-vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
- unsigned int i, unsigned int j, float angle, unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<vpRGBa> &I, unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness)
{
vp_display_display_rectangle(I, i, j, angle, width, height, color, thickness);
}
@@ -535,9 +557,8 @@ vpDisplay::displayRectangle(const vpImage<vpRGBa> &I,
\sa setFont()
*/
-void
-vpDisplay::displayText(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
- const std::string &s, const vpColor &color )
+void vpDisplay::displayText(const vpImage<vpRGBa> &I, const vpImagePoint &ip, const std::string &s,
+ const vpColor &color)
{
vp_display_display_text(I, ip, s, color);
}
@@ -554,9 +575,7 @@ vpDisplay::displayText(const vpImage<vpRGBa> &I, const vpImagePoint &ip,
\sa setFont()
*/
-void
-vpDisplay::displayText(const vpImage<vpRGBa> &I, int i, int j,
- const std::string &s, const vpColor &color)
+void vpDisplay::displayText(const vpImage<vpRGBa> &I, int i, int j, const std::string &s, const vpColor &color)
{
vp_display_display_text(I, i, j, s, color);
}
@@ -569,11 +588,11 @@ vpDisplay::displayText(const vpImage<vpRGBa> &I, int i, int j,
to show the overlay. Because it's time spending, use it parcimoniously.
\code
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGDI.h>
int main() {
vpImage<vpRGBa> I(240, 380);
@@ -590,10 +609,7 @@ int main() {
\sa flushROI()
*/
-void vpDisplay::flush(const vpImage<vpRGBa> &I)
-{
- vp_display_flush(I);
-}
+void vpDisplay::flush(const vpImage<vpRGBa> &I) { vp_display_flush(I); }
/*!
Flushes the output buffer associated to image \e I display.
@@ -604,10 +620,7 @@ void vpDisplay::flush(const vpImage<vpRGBa> &I)
\sa flush()
*/
-void vpDisplay::flushROI(const vpImage<vpRGBa> &I, const vpRect &roi)
-{
- vp_display_flush_roi(I, roi);
-}
+void vpDisplay::flushROI(const vpImage<vpRGBa> &I, const vpRect &roi) { vp_display_flush_roi(I, roi); }
/*!
Display image \e I.
@@ -620,22 +633,13 @@ void vpDisplay::flushROI(const vpImage<vpRGBa> &I, const vpRect &roi)
\sa init(), close()
*/
-void
-vpDisplay::display(const vpImage<vpRGBa> &I)
-{
- vp_display_display(I);
-}
+void vpDisplay::display(const vpImage<vpRGBa> &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.
+ 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<vpRGBa> &I, const vpRect &roi)
-{
- vp_display_display_roi(I, roi);
-}
+void vpDisplay::displayROI(const vpImage<vpRGBa> &I, const vpRect &roi) { vp_display_display_roi(I, roi); }
/*!
Wait for a click from one of the mouse button.
@@ -654,10 +658,7 @@ vpDisplay::displayROI(const vpImage<vpRGBa> &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<vpRGBa> &I, bool blocking)
-{
- return vp_display_get_click(I, blocking);
-}
+bool vpDisplay::getClick(const vpImage<vpRGBa> &I, bool blocking) { return vp_display_get_click(I, blocking); }
/*!
Wait for a click from one of the mouse button and get the position
@@ -702,8 +703,8 @@ bool vpDisplay::getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, bool blocki
button is pressed, the location of the mouse pointer is updated in
\e ip.
*/
-bool vpDisplay::getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking)
+bool vpDisplay::getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
return vp_display_get_click(I, ip, button, blocking);
}
@@ -724,9 +725,7 @@ bool vpDisplay::getClick(const vpImage<vpRGBa> &I, vpImagePoint &ip,
\return true if a mouse button is pressed, false otherwise.
*/
-bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
+bool vpDisplay::getClick(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
vpImagePoint ip;
return vpDisplay::getClick(I, ip, button, blocking);
@@ -752,9 +751,8 @@ bool vpDisplay::getClick ( const vpImage<vpRGBa> &I,
button is released, the location of the mouse pointer is updated in
\e ip.
*/
-bool
-vpDisplay::getClickUp(const vpImage<vpRGBa> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking )
+bool vpDisplay::getClickUp(const vpImage<vpRGBa> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
return vp_display_get_click_up(I, ip, button, blocking);
}
@@ -775,8 +773,7 @@ vpDisplay::getClickUp(const vpImage<vpRGBa> &I, vpImagePoint &ip,
\return true if a mouse button is released, false otherwise.
*/
-bool vpDisplay::getClickUp(const vpImage<vpRGBa> &I,
- vpMouseButton::vpMouseButtonType& button, bool blocking)
+bool vpDisplay::getClickUp(const vpImage<vpRGBa> &I, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
vpImagePoint ip;
return vpDisplay::getClickUp(I, ip, button, blocking);
@@ -802,11 +799,11 @@ bool vpDisplay::getClickUp(const vpImage<vpRGBa> &I,
Below you will find an example showing how to use this method.
\code
#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -852,8 +849,7 @@ int main()
do {
bool event = vpDisplay::getKeyboardEvent(I, false);
if (event) {
- std::cout << "A keyboard event was detected" << std::endl;
- cpt_event ++;
+ std::cout << "A keyboard event was detected" << std::endl; cpt_event ++;
}
vpTime::wait(5); // wait 5 ms
@@ -865,8 +861,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking)
{
return vp_display_get_keyboard_event(I, blocking);
}
@@ -893,11 +888,11 @@ vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, bool blocking)
Below you will find an example showing how to use this method.
\code
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -957,8 +952,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, std::string &key, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, std::string &key, bool blocking)
{
return vp_display_get_keyboard_event(I, key, blocking);
}
@@ -985,11 +979,11 @@ vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, std::string &key, bool blo
Below you will find an example showing how to use this method.
\code
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -1049,8 +1043,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, char *key, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, char *key, bool blocking)
{
return vp_display_get_keyboard_event(I, key, blocking);
}
@@ -1064,8 +1057,7 @@ vpDisplay::getKeyboardEvent(const vpImage<vpRGBa> &I, char *key, bool blocking)
\return true if a pointer motion event was received, false otherwise.
*/
-bool
-vpDisplay::getPointerMotionEvent(const vpImage<vpRGBa> &I, vpImagePoint &ip)
+bool vpDisplay::getPointerMotionEvent(const vpImage<vpRGBa> &I, vpImagePoint &ip)
{
return vp_display_get_pointer_motion_event(I, ip);
}
@@ -1079,8 +1071,7 @@ vpDisplay::getPointerMotionEvent(const vpImage<vpRGBa> &I, vpImagePoint &ip)
\return true.
*/
-bool
-vpDisplay::getPointerPosition(const vpImage<vpRGBa> &I, vpImagePoint &ip)
+bool vpDisplay::getPointerPosition(const vpImage<vpRGBa> &I, vpImagePoint &ip)
{
return vp_display_get_pointer_position(I, ip);
}
@@ -1094,11 +1085,7 @@ vpDisplay::getPointerPosition(const vpImage<vpRGBa> &I, vpImagePoint &ip)
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-void
-vpDisplay::setBackground(const vpImage<vpRGBa> &I, const vpColor &color)
-{
- vp_display_set_background(I, color);
-}
+void vpDisplay::setBackground(const vpImage<vpRGBa> &I, const vpColor &color) { vp_display_set_background(I, color); }
/*!
Set the font of a text printed in the display overlay. To print a
@@ -1113,11 +1100,7 @@ vpDisplay::setBackground(const vpImage<vpRGBa> &I, const vpColor &color)
\sa displayText()
*/
-void
-vpDisplay::setFont(const vpImage<vpRGBa> &I, const std::string &fontname)
-{
- vp_display_set_font(I, fontname);
-}
+void vpDisplay::setFont(const vpImage<vpRGBa> &I, const std::string &fontname) { vp_display_set_font(I, fontname); }
/*!
Set the windows title.
@@ -1126,8 +1109,7 @@ vpDisplay::setFont(const vpImage<vpRGBa> &I, const std::string &fontname)
\param I : Image associated to the display window.
\param windowtitle : Window title.
*/
-void
-vpDisplay::setTitle(const vpImage<vpRGBa> &I, const std::string &windowtitle)
+void vpDisplay::setTitle(const vpImage<vpRGBa> &I, const std::string &windowtitle)
{
vp_display_set_title(I, windowtitle);
}
@@ -1136,28 +1118,24 @@ vpDisplay::setTitle(const vpImage<vpRGBa> &I, const std::string &windowtitle)
Set the window position in the screen.
\param I : Image associated to the display window.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-void
-vpDisplay::setWindowPosition(const vpImage<vpRGBa> &I, int winx, int winy )
+void vpDisplay::setWindowPosition(const vpImage<vpRGBa> &I, int winx, int winy)
{
vp_display_set_window_position(I, winx, winy);
}
/*!
- Return the value of the down scale factor applied to the image in order to reduce
- the size of the window used to display the image.
+ Return the value of the down scale factor applied to the image in order to
+ reduce the size of the window used to display the image.
\param I : Image associated to the display window.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-unsigned int
-vpDisplay::getDownScalingFactor(const vpImage<vpRGBa> &I)
-{
- return vp_display_get_down_scaling_factor(I);
-}
+unsigned int vpDisplay::getDownScalingFactor(const vpImage<vpRGBa> &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 dd4d5a7..a55a2a9 100644
--- a/modules/core/src/display/vpDisplay_uchar.cpp
+++ b/modules/core/src/display/vpDisplay_uchar.cpp
@@ -3,17 +3,14 @@
#include "vpDisplay_impl.h"
//************************************************************************
-// Modifications done in this file should be reported in all vpDisplay_*.cpp files
-// that implement other types (unsigned char, vpRGB, vpRGBa)
+// Modifications done in this file should be reported in all vpDisplay_*.cpp
+// files that implement other types (unsigned char, vpRGB, vpRGBa)
//************************************************************************
/*!
Close the display attached to I.
*/
-void vpDisplay::close(vpImage<unsigned char> &I)
-{
- vp_display_close(I);
-}
+void vpDisplay::close(vpImage<unsigned char> &I) { vp_display_close(I); }
/*!
Display an arrow from image point \e ip1 to image point \e ip2.
@@ -23,9 +20,8 @@ void vpDisplay::close(vpImage<unsigned char> &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<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness )
+void vpDisplay::displayArrow(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness)
{
vp_display_display_arrow(I, ip1, ip2, color, w, h, thickness);
}
@@ -40,9 +36,8 @@ vpDisplay::displayArrow(const vpImage<unsigned char> &I, const vpImagePoint &ip1
\param w,h : Width and height of the arrow.
\param thickness : Thickness of the lines used to display the arrow.
*/
-void
-vpDisplay::displayArrow(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int w, unsigned int h, unsigned int thickness)
+void vpDisplay::displayArrow(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2, const vpColor &color,
+ unsigned int w, unsigned int h, unsigned int thickness)
{
vp_display_display_arrow(I, i1, j1, i2, j2, color, w, h, thickness);
}
@@ -60,9 +55,8 @@ vpDisplay::displayArrow(const vpImage<unsigned char> &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<unsigned char> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color, unsigned int thickness)
+void vpDisplay::displayCamera(const vpImage<unsigned char> &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,9 +74,8 @@ vpDisplay::displayCamera(const vpImage<unsigned char> &I, const vpHomogeneousMat
\sa setFont(), displayText()
*/
-void
-vpDisplay::displayCharString(const vpImage<unsigned char> &I, const vpImagePoint &ip,
- const char *string, const vpColor &color )
+void vpDisplay::displayCharString(const vpImage<unsigned char> &I, const vpImagePoint &ip, const char *string,
+ const vpColor &color)
{
vp_display_display_char_string(I, ip, string, color);
}
@@ -100,9 +93,8 @@ vpDisplay::displayCharString(const vpImage<unsigned char> &I, const vpImagePoint
\sa setFont(), displayText()
*/
-void
-vpDisplay::displayCharString(const vpImage<unsigned char> &I, int i, int j,
- const char *string, const vpColor &color)
+void vpDisplay::displayCharString(const vpImage<unsigned char> &I, int i, int j, const char *string,
+ const vpColor &color)
{
vp_display_display_char_string(I, i, j, string, color);
}
@@ -117,9 +109,8 @@ vpDisplay::displayCharString(const vpImage<unsigned char> &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<unsigned char> &I, const vpImagePoint ¢er, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayCircle(const vpImage<unsigned char> &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);
}
@@ -134,9 +125,8 @@ vpDisplay::displayCircle(const vpImage<unsigned char> &I, const vpImagePoint &ce
\param thickness : Thickness of the circle. This parameter is only useful
when \e fill is set to false.
*/
-void
-vpDisplay::displayCircle(const vpImage<unsigned char> &I, int i, int j, unsigned int radius,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayCircle(const vpImage<unsigned char> &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);
}
@@ -163,8 +153,8 @@ void vpDisplay::displayCross(const vpImage<unsigned char> &I, const vpImagePoint
\param color : Cross color.
\param thickness : Thickness of the lines used to display the cross.
*/
-void vpDisplay::displayCross(const vpImage<unsigned char> &I, int i, int j, unsigned int size,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayCross(const vpImage<unsigned char> &I, int i, int j, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_cross(I, i, j, size, color, thickness);
}
@@ -176,8 +166,8 @@ void vpDisplay::displayCross(const vpImage<unsigned char> &I, int i, int j, unsi
\param color : Line color.
\param thickness : Dashed line thickness.
*/
-void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1,
- const vpImagePoint &ip2, const vpColor &color, unsigned int thickness )
+void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ const vpColor &color, unsigned int thickness)
{
vp_display_display_dot_line(I, ip1, ip2, color, thickness);
}
@@ -190,28 +180,51 @@ void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, const vpImagePoi
\param color : Line color.
\param thickness : Dashed line thickness.
*/
-void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayDotLine(const vpImage<unsigned char> &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);
}
/*!
+ Display dashed lines for the list of image points.
+ \param I : The image associated to the display.
+ \param ips : List of image points.
+ \param closeTheShape : If true, display a dashed line from the first and
+ last image points. \param color : Line color. \param thickness : Dashed line
+ thickness.
+*/
+void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &ips,
+ const bool closeTheShape, const vpColor &color, unsigned int thickness)
+{
+ if (ips.size() <= 1)
+ return;
+
+ for (size_t i = 0; i < ips.size() - 1; i++)
+ vp_display_display_dot_line(I, ips[i], ips[i + 1], color, thickness);
+
+ if (closeTheShape)
+ vp_display_display_dot_line(I, ips.front(), ips.back(), color, thickness);
+}
+
+/*!
Display an ellipse from its parameters expressed in pixels.
\param I : Image to consider.
\param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
- \param thickness : Drawings thickness.
-
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+ \param coef1, coef2, coef3 : Depending on the parameter \e
+ use_centered_moments these parameters are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the
+ ellipse in radians: \f$a, b, e\f$. \param use_centered_moments : When false,
+ the parameters coef1, coef2, coef3 are the parameters \f$a, b, e\f$. When
+ true, the parameters coef1, coef2, coef3 are rather the centered moments
+ \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we
+ compute the parameters \e a, \e b and \e e from the centered moments. \param
+ color : Drawings color. \param thickness : Drawings thickness.
+
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to
+ its parametric representation:
\f[ \left(\begin{array}{c}
u_\theta \\
@@ -229,43 +242,48 @@ void vpDisplay::displayDotLine(const vpImage<unsigned char> &I, int i1, int j1,
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.
+ The following example shows how to use for example this function to display
+ the result of a tracking.
\code
vpMeEllipse ellipse;
...
vpDisplay::display(I);
- ellipse.track(I) ;
+ ellipse.track(I);
- vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(), ellipse.get_mu11(), ellipse.get_mu02(),
- true, vpColor::orange, 1);
+ vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
+ ellipse.get_mu11(), ellipse.get_mu02(), true, vpColor::orange, 1);
vpDisplay::flush(I);
\endcode
*/
-void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- bool use_centered_moments, const vpColor &color, unsigned int thickness)
+void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, bool use_centered_moments,
+ const vpColor &color, unsigned int thickness)
{
- vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color, thickness);
+ vpDisplay::displayEllipse(I, center, coef1, coef2, coef3, 0., vpMath::rad(360), use_centered_moments, color,
+ thickness);
}
/*!
Display an ellipse from its parameters expressed in pixels.
\param I : Image to consider.
\param center : Center \f$(u_c, v_c)\f$ of the ellipse.
- \param coef1, coef2, coef3 : Depending on the parameter \e use_centered_moments these parameters
- are:
- - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11}, \mu_{02}\f$;
- - the major and minor axis lenght in pixels and the excentricity of the ellipse in radians: \f$a, b, e\f$.
- \param theta1, theta2 : Angles \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the ellipse.
- If theta1=0 and theta2=vpMath::rad(360) all the ellipse is displayed.
- \param use_centered_moments : When false, the parameters coef1, coef2, coef3
- are the parameters \f$a, b, e\f$. When true, the parameters coef1, coef2, coef3 are rather the centered moments
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e a, \e b and \e e
- from the centered moments.
- \param color : Drawings color.
+ \param coef1, coef2, coef3 : Depending on the parameter \e
+ use_centered_moments these parameters are:
+ - the centered moments expressed in pixels: \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$;
+ - the major and minor axis lenght in pixels and the excentricity of the
+ ellipse in radians: \f$a, b, e\f$. \param theta1, theta2 : Angles
+ \f$(\theta_1, \theta_2)\f$ in radians used to select a portion of the
+ ellipse. If theta1=0 and theta2=vpMath::rad(360) all the ellipse is
+ displayed. \param use_centered_moments : When false, the parameters coef1,
+ coef2, coef3 are the parameters \f$a, b, e\f$. When true, the parameters
+ coef1, coef2, coef3 are rather the centered moments \f$\mu_{20}, \mu_{11},
+ \mu_{02}\f$ expressed in pixels. In that case, we compute the parameters \e
+ a, \e b and \e e from the centered moments. \param color : Drawings color.
\param thickness : Drawings thickness.
- All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to its parametric representation:
+ All the points \f$(u_\theta,v_\theta)\f$ on the ellipse are drawn thanks to
+ its parametric representation:
\f[ \left(\begin{array}{c}
u_\theta \\
@@ -283,24 +301,23 @@ void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoi
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.
+ The following example shows how to use for example this function to display
+ the result of a tracking.
\code
vpMeEllipse ellipse;
...
vpDisplay::display(I);
- ellipse.track(I) ;
+ ellipse.track(I);
vpDisplay::displayEllipse(I, ellipse.getCenter(), ellipse.get_mu20(),
ellipse.get_mu11(), ellipse.get_mu02(),
- ellipse.getSmallestAngle(), ellipse.getHighestAngle(),
- true, vpColor::orange, 1);
- vpDisplay::flush(I);
+ ellipse.getSmallestAngle(),
+ ellipse.getHighestAngle(), true, vpColor::orange, 1); vpDisplay::flush(I);
\endcode
*/
-void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er,
- const double &coef1, const double &coef2, const double &coef3,
- const double &theta1, const double &theta2, bool use_centered_moments,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &coef1,
+ const double &coef2, const double &coef3, const double &theta1, const double &theta2,
+ bool use_centered_moments, const vpColor &color, unsigned int thickness)
{
vp_display_display_ellipse(I, center, coef1, coef2, coef3, theta1, theta2, use_centered_moments, color, thickness);
}
@@ -317,12 +334,12 @@ void vpDisplay::displayEllipse(const vpImage<unsigned char> &I, const vpImagePoi
\param size : Size of the object frame.
\param color : Color used to display the frame in the image.
\param thickness : the thickness of the line.
- \param offset : Offset in pixels applied to the frame origin location in the image.
+ \param offset : Offset in pixels applied to the frame origin location in the
+ image.
*/
-void
-vpDisplay::displayFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, double size, const vpColor &color,
- unsigned int thickness, vpImagePoint offset)
+void vpDisplay::displayFrame(const vpImage<unsigned char> &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);
}
@@ -335,7 +352,7 @@ vpDisplay::displayFrame(const vpImage<unsigned char> &I, const vpHomogeneousMatr
\param thickness : Line thickness.
*/
void vpDisplay::displayLine(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness )
+ const vpColor &color, unsigned int thickness)
{
vp_display_display_line(I, ip1, ip2, color, thickness);
}
@@ -348,21 +365,41 @@ void vpDisplay::displayLine(const vpImage<unsigned char> &I, const vpImagePoint
\param color : Line color.
\param thickness : Line thickness.
*/
-void vpDisplay::displayLine(const vpImage<unsigned char> &I, int i1, int j1, int i2, int j2,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayLine(const vpImage<unsigned char> &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);
}
/*!
+ Display the lines formed by the list of image points.
+ \param I : The image associated to the display.
+ \param ips : List of image points.
+ \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<unsigned char> &I, const std::vector<vpImagePoint> &ips,
+ const bool closeTheShape, const vpColor &color, unsigned int thickness)
+{
+ if (ips.size() <= 1)
+ return;
+
+ for (size_t i = 0; i < ips.size() - 1; i++)
+ vp_display_display_line(I, ips[i], ips[i + 1], color, thickness);
+
+ if (closeTheShape)
+ vp_display_display_line(I, ips.front(), ips.back(), color, thickness);
+}
+
+/*!
Display a point at the image point \e ip location.
\param I : The image associated to the display.
\param ip : Point location.
\param color : Point color.
\param thickness : Thickness of the point
*/
-void vpDisplay::displayPoint(const vpImage<unsigned char> &I, const vpImagePoint &ip,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayPoint(const vpImage<unsigned char> &I, const vpImagePoint &ip, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_point(I, ip, color, thickness);
}
@@ -374,11 +411,10 @@ void vpDisplay::displayPoint(const vpImage<unsigned char> &I, const vpImagePoint
\param color : Point color.
\param thickness : Thickness of the point
*/
-void vpDisplay::displayPoint(const vpImage<unsigned char> &I, int i, int j,
- const vpColor &color, unsigned int thickness )
+void vpDisplay::displayPoint(const vpImage<unsigned char> &I, int i, int j, const vpColor &color,
+ unsigned int thickness)
{
vp_display_display_point(I, i, j, color, thickness);
-
}
/*!
@@ -388,9 +424,8 @@ void vpDisplay::displayPoint(const vpImage<unsigned char> &I, int i, int j,
\param color : Line color.
\param thickness : Line thickness.
*/
-void
-vpDisplay::displayPolygon(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayPolygon(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
+ const vpColor &color, unsigned int thickness)
{
vp_display_display_polygon(I, vip, color, thickness);
}
@@ -409,10 +444,8 @@ vpDisplay::displayPolygon(const vpImage<unsigned char> &I, const std::vector<vpI
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint &topLeft, unsigned int width,
+ unsigned int height, const vpColor &color, bool fill, unsigned int thickness)
{
vp_display_display_rectangle(I, topLeft, width, height, color, fill, thickness);
}
@@ -431,10 +464,8 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
- int i, int j, unsigned int width, unsigned int height,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &I, int i, int j, unsigned int width, unsigned int height,
+ const vpColor &color, bool fill, unsigned int thickness)
{
vp_display_display_rectangle(I, i, j, width, height, color, fill, thickness);
}
@@ -452,9 +483,8 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpRect &rectangle,
- const vpColor &color, bool fill, unsigned int thickness )
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpRect &rectangle, const vpColor &color,
+ bool fill, unsigned int thickness)
{
vp_display_display_rectangle(I, rectangle, color, fill, thickness);
}
@@ -472,10 +502,8 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpRect &recta
\param thickness : Thickness of the four lines used to display the
rectangle.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint ¢er,
- float angle, unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint ¢er, float angle,
+ unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness)
{
vp_display_display_rectangle(I, center, angle, width, height, color, thickness);
}
@@ -494,10 +522,9 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I, const vpImagePoint
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
- const vpImagePoint &topLeft, const vpImagePoint &bottomRight,
- const vpColor &color, bool fill, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &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);
}
@@ -515,10 +542,8 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
\param thickness : Thickness of the four lines used to display the
rectangle.
*/
-void
-vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
- unsigned int i, unsigned int j, float angle, unsigned int width, unsigned int height,
- const vpColor &color, unsigned int thickness)
+void vpDisplay::displayRectangle(const vpImage<unsigned char> &I, unsigned int i, unsigned int j, float angle,
+ unsigned int width, unsigned int height, const vpColor &color, unsigned int thickness)
{
vp_display_display_rectangle(I, i, j, angle, width, height, color, thickness);
}
@@ -535,9 +560,8 @@ vpDisplay::displayRectangle(const vpImage<unsigned char> &I,
\sa setFont()
*/
-void
-vpDisplay::displayText(const vpImage<unsigned char> &I, const vpImagePoint &ip,
- const std::string &s, const vpColor &color )
+void vpDisplay::displayText(const vpImage<unsigned char> &I, const vpImagePoint &ip, const std::string &s,
+ const vpColor &color)
{
vp_display_display_text(I, ip, s, color);
}
@@ -554,9 +578,7 @@ vpDisplay::displayText(const vpImage<unsigned char> &I, const vpImagePoint &ip,
\sa setFont()
*/
-void
-vpDisplay::displayText(const vpImage<unsigned char> &I, int i, int j,
- const std::string &s, const vpColor &color)
+void vpDisplay::displayText(const vpImage<unsigned char> &I, int i, int j, const std::string &s, const vpColor &color)
{
vp_display_display_text(I, i, j, s, color);
}
@@ -569,11 +591,11 @@ vpDisplay::displayText(const vpImage<unsigned char> &I, int i, int j,
to show the overlay. Because it's time spending, use it parcimoniously.
\code
-#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGDI.h>
int main() {
vpImage<unsigned char> I(240, 380);
@@ -590,10 +612,7 @@ int main() {
\sa flushROI()
*/
-void vpDisplay::flush(const vpImage<unsigned char> &I)
-{
- vp_display_flush(I);
-}
+void vpDisplay::flush(const vpImage<unsigned char> &I) { vp_display_flush(I); }
/*!
Flushes the output buffer associated to image \e I display.
@@ -604,10 +623,7 @@ void vpDisplay::flush(const vpImage<unsigned char> &I)
\sa flush()
*/
-void vpDisplay::flushROI(const vpImage<unsigned char> &I, const vpRect &roi)
-{
- vp_display_flush_roi(I, roi);
-}
+void vpDisplay::flushROI(const vpImage<unsigned char> &I, const vpRect &roi) { vp_display_flush_roi(I, roi); }
/*!
Display image \e I.
@@ -620,22 +636,13 @@ void vpDisplay::flushROI(const vpImage<unsigned char> &I, const vpRect &roi)
\sa init(), close()
*/
-void
-vpDisplay::display(const vpImage<unsigned char> &I)
-{
- vp_display_display(I);
-}
+void vpDisplay::display(const vpImage<unsigned char> &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.
+ 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<unsigned char> &I, const vpRect &roi)
-{
- vp_display_display_roi(I, roi);
-}
+void vpDisplay::displayROI(const vpImage<unsigned char> &I, const vpRect &roi) { vp_display_display_roi(I, roi); }
/*!
Wait for a click from one of the mouse button.
@@ -654,10 +661,7 @@ vpDisplay::displayROI(const vpImage<unsigned char> &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<unsigned char> &I, bool blocking)
-{
- return vp_display_get_click(I, blocking);
-}
+bool vpDisplay::getClick(const vpImage<unsigned char> &I, bool blocking) { return vp_display_get_click(I, blocking); }
/*!
Wait for a click from one of the mouse button and get the position
@@ -702,8 +706,8 @@ bool vpDisplay::getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, bool
button is pressed, the location of the mouse pointer is updated in
\e ip.
*/
-bool vpDisplay::getClick(const vpImage<unsigned char> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking)
+bool vpDisplay::getClick(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
return vp_display_get_click(I, ip, button, blocking);
}
@@ -724,9 +728,7 @@ bool vpDisplay::getClick(const vpImage<unsigned char> &I, vpImagePoint &ip,
\return true if a mouse button is pressed, false otherwise.
*/
-bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType& button,
- bool blocking)
+bool vpDisplay::getClick(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
vpImagePoint ip;
return vpDisplay::getClick(I, ip, button, blocking);
@@ -752,9 +754,8 @@ bool vpDisplay::getClick ( const vpImage<unsigned char> &I,
button is released, the location of the mouse pointer is updated in
\e ip.
*/
-bool
-vpDisplay::getClickUp(const vpImage<unsigned char> &I, vpImagePoint &ip,
- vpMouseButton::vpMouseButtonType& button, bool blocking )
+bool vpDisplay::getClickUp(const vpImage<unsigned char> &I, vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button,
+ bool blocking)
{
return vp_display_get_click_up(I, ip, button, blocking);
}
@@ -775,8 +776,7 @@ vpDisplay::getClickUp(const vpImage<unsigned char> &I, vpImagePoint &ip,
\return true if a mouse button is released, false otherwise.
*/
-bool vpDisplay::getClickUp(const vpImage<unsigned char> &I,
- vpMouseButton::vpMouseButtonType& button, bool blocking)
+bool vpDisplay::getClickUp(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button, bool blocking)
{
vpImagePoint ip;
return vpDisplay::getClickUp(I, ip, button, blocking);
@@ -802,11 +802,11 @@ bool vpDisplay::getClickUp(const vpImage<unsigned char> &I,
Below you will find an example showing how to use this method.
\code
#include <visp3/core/vpConfig.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -865,8 +865,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking)
{
return vp_display_get_keyboard_event(I, blocking);
}
@@ -893,11 +892,11 @@ vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, bool blocking)
Below you will find an example showing how to use this method.
\code
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -957,8 +956,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, std::string &key, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, std::string &key, bool blocking)
{
return vp_display_get_keyboard_event(I, key, blocking);
}
@@ -985,11 +983,11 @@ vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, std::string &key, b
Below you will find an example showing how to use this method.
\code
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
int main()
{
@@ -1049,8 +1047,7 @@ int main()
}
\endcode
*/
-bool
-vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, char *key, bool blocking)
+bool vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, char *key, bool blocking)
{
return vp_display_get_keyboard_event(I, key, blocking);
}
@@ -1064,8 +1061,7 @@ vpDisplay::getKeyboardEvent(const vpImage<unsigned char> &I, char *key, bool blo
\return true if a pointer motion event was received, false otherwise.
*/
-bool
-vpDisplay::getPointerMotionEvent(const vpImage<unsigned char> &I, vpImagePoint &ip)
+bool vpDisplay::getPointerMotionEvent(const vpImage<unsigned char> &I, vpImagePoint &ip)
{
return vp_display_get_pointer_motion_event(I, ip);
}
@@ -1079,8 +1075,7 @@ vpDisplay::getPointerMotionEvent(const vpImage<unsigned char> &I, vpImagePoint &
\return true.
*/
-bool
-vpDisplay::getPointerPosition(const vpImage<unsigned char> &I, vpImagePoint &ip)
+bool vpDisplay::getPointerPosition(const vpImage<unsigned char> &I, vpImagePoint &ip)
{
return vp_display_get_pointer_position(I, ip);
}
@@ -1094,8 +1089,7 @@ vpDisplay::getPointerPosition(const vpImage<unsigned char> &I, vpImagePoint &ip)
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-void
-vpDisplay::setBackground(const vpImage<unsigned char> &I, const vpColor &color)
+void vpDisplay::setBackground(const vpImage<unsigned char> &I, const vpColor &color)
{
vp_display_set_background(I, color);
}
@@ -1113,8 +1107,7 @@ vpDisplay::setBackground(const vpImage<unsigned char> &I, const vpColor &color)
\sa displayText()
*/
-void
-vpDisplay::setFont(const vpImage<unsigned char> &I, const std::string &fontname)
+void vpDisplay::setFont(const vpImage<unsigned char> &I, const std::string &fontname)
{
vp_display_set_font(I, fontname);
}
@@ -1126,8 +1119,7 @@ vpDisplay::setFont(const vpImage<unsigned char> &I, const std::string &fontname)
\param I : Image associated to the display window.
\param windowtitle : Window title.
*/
-void
-vpDisplay::setTitle(const vpImage<unsigned char> &I, const std::string &windowtitle)
+void vpDisplay::setTitle(const vpImage<unsigned char> &I, const std::string &windowtitle)
{
vp_display_set_title(I, windowtitle);
}
@@ -1136,28 +1128,27 @@ vpDisplay::setTitle(const vpImage<unsigned char> &I, const std::string &windowti
Set the window position in the screen.
\param I : Image associated to the display window.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-void
-vpDisplay::setWindowPosition(const vpImage<unsigned char> &I, int winx, int winy )
+void vpDisplay::setWindowPosition(const vpImage<unsigned char> &I, int winx, int winy)
{
vp_display_set_window_position(I, winx, winy);
}
/*!
- Return the value of the down scale factor applied to the image in order to reduce
- the size of the window used to display the image.
+ Return the value of the down scale factor applied to the image in order to
+ reduce the size of the window used to display the image.
\param I : Image associated to the display window.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
-unsigned int
-vpDisplay::getDownScalingFactor(const vpImage<unsigned char> &I)
+unsigned int vpDisplay::getDownScalingFactor(const vpImage<unsigned char> &I)
{
return vp_display_get_down_scaling_factor(I);
}
diff --git a/modules/core/src/display/vpFeatureDisplay.cpp b/modules/core/src/display/vpFeatureDisplay.cpp
index cc48b3a..d285416 100644
--- a/modules/core/src/display/vpFeatureDisplay.cpp
+++ b/modules/core/src/display/vpFeatureDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,8 +53,6 @@
#include <visp3/core/vpImagePoint.h>
-
-
/*!
\param x,y : Point coordinates in meters.
\param cam : Camera intrinsic parameters.
@@ -62,24 +61,18 @@
\param color : Color to use to display the feature
\param thickness : Thickness of the feature representation.
*/
-void vpFeatureDisplay::displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness)
+void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color, unsigned int thickness)
{
- try{
+ try {
vpImagePoint ip; // pixel coordinates in float
- vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
+ vpMeterPixelConversion::convertPoint(cam, x, y, ip);
- vpDisplay::displayCross(I, ip, 15, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ vpDisplay::displayCross(I, ip, 15, color, thickness);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
}
/*!
\param rho, theta : Line parameters.
@@ -88,50 +81,43 @@ 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<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness )
+void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam,
+ const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
{
-
- try{
+ try {
// x cos(theta) + y sin(theta) - rho = 0
- double rhop,thetap ;
- vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
+ double rhop, thetap;
+ vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
// u cos(thetap) + v sin(thetap) - rhop = 0
- double co = cos(thetap) ;
- double si = sin(thetap) ;
- double c = -rhop ;
+ double co = cos(thetap);
+ double si = sin(thetap);
+ double c = -rhop;
// vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
// double u1,v1,u2,v2 ;
- double a = si ;
- double b = co ;
+ double a = si;
+ double b = co;
vpImagePoint ip1, ip2;
if (fabs(a) < fabs(b)) {
- ip1.set_ij(0, (-c)/b);
+ ip1.set_ij(0, (-c) / b);
double h = I.getHeight() - 1;
- ip2.set_ij(h, (-c - a*h)/b);
+ ip2.set_ij(h, (-c - a * h) / b);
vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- else {
- ip1.set_ij((-c)/a, 0);
- double w = I.getWidth()-1;
- ip2.set_ij((-c - b*w)/a, w);
+ } else {
+ ip1.set_ij((-c) / a, 0);
+ double w = I.getWidth() - 1;
+ ip2.set_ij((-c - b * w) / a, w);
vpDisplay::displayLine(I, ip1, ip2, color, thickness);
}
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
@@ -142,28 +128,22 @@ 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<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness)
+void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
+ const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color, unsigned int thickness)
{
- try
- {
- displayLine(rho1, theta1, cam, I, color, thickness) ;
- displayLine(rho2, theta2, cam, I, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ displayLine(rho1, theta1, cam, I, color, thickness);
+ displayLine(rho2, theta2, cam, I, color, thickness);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
\param x, y, mu20, mu11, mu02 : Ellipse parameters where:
- - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
- along the horizontal and vertical axis in the image.
+ - \f$(x,y)\f$ are the normalized coordinates of the ellipse center,
+ respectively along the horizontal and vertical axis in the image.
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
\param cam : Camera intrinsic parameters.
\param I : Image.
@@ -172,68 +152,61 @@ 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<unsigned char> &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<unsigned char> &I,
+ const vpColor &color, unsigned int thickness)
{
try {
- unsigned int number_of_points = 45 ;
- const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
- unsigned int i = 0 ;
+ unsigned int number_of_points = 45;
+ const double incr = 2 * M_PI / (double)number_of_points; // angle increment
+ unsigned int i = 0;
- double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
- double a, b, e ;
+ double s = sqrt(vpMath::sqr(mu20 - mu02) + 4 * mu11 * mu11);
+ double a, b, e;
- //if (fabs(mu11)<1e-6) e =0 ;
+ // if (fabs(mu11)<1e-6) e =0 ;
if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
- e = 0 ;
+ e = 0;
a = sqrt(mu20);
b = sqrt(mu02);
- }
- else {
- e = (mu02-mu20+s)/(2*mu11) ;
- a = sqrt( (mu02+mu20+s)/2.0) ;
- b = sqrt( (mu02+mu20-s)/2.0) ;
+ } else {
+ e = (mu02 - mu20 + s) / (2 * mu11);
+ a = sqrt((mu02 + mu20 + s) / 2.0);
+ b = sqrt((mu02 + mu20 - s) / 2.0);
}
- double e1 = atan(e) ;
+ double e1 = atan(e);
- double k = 0.0 ;
+ double k = 0.0;
- double ce = cos(e1) ;
- double se = sin(e1) ;
+ double ce = cos(e1);
+ double se = sin(e1);
- double x2 = 0;
- double y2 =0;
+ double x2 = 0;
+ double y2 = 0;
vpImagePoint ip1, ip2;
- for( i = 0; i < number_of_points+2 ; i++)
- {
- double x1 = a *cos(k) ; // equation of an ellipse
- double y1 = b *sin(k) ; // equation of an ellipse
- double x11 = x + ce *x1 - se *y1 ;
- double y11 = y + se *x1 + ce *y1 ;
+ for (i = 0; i < number_of_points + 2; i++) {
+ double x1 = a * cos(k); // equation of an ellipse
+ double y1 = b * sin(k); // equation of an ellipse
+ double x11 = x + ce * x1 - se * y1;
+ double y11 = y + se * x1 + ce * y1;
vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
if (i > 1) {
- ip2.set_u( x2 );
- ip2.set_v( y2 );
+ ip2.set_u(x2);
+ ip2.set_v(y2);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
}
ip2 = ip1;
- y2 = y1 ;
- x2 = x1 ;
- k += incr ;
+ y2 = y1;
+ x2 = x1;
+ k += incr;
} // end for loop
- }
- catch(vpException &e)
- {
+ } catch (vpException &e) {
throw(e);
}
}
@@ -246,24 +219,18 @@ void vpFeatureDisplay::displayEllipse(double x,double y,
\param color : Color to use to display the feature
\param thickness : Thickness of the feature representation.
*/
-void vpFeatureDisplay::displayPoint(double x,double y,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness)
+void vpFeatureDisplay::displayPoint(double x, double y, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ const vpColor &color, unsigned int thickness)
{
- try{
+ try {
vpImagePoint ip; // pixel coordinates in float
- vpMeterPixelConversion::convertPoint(cam, x, y, ip) ;
+ vpMeterPixelConversion::convertPoint(cam, x, y, ip);
- vpDisplay::displayCross(I, ip, 15, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ vpDisplay::displayCross(I, ip, 15, color, thickness);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
}
/*!
@@ -273,50 +240,43 @@ 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<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness )
+void vpFeatureDisplay::displayLine(double rho, double theta, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ const vpColor &color, unsigned int thickness)
{
-
- try{
+ try {
// x cos(theta) + y sin(theta) - rho = 0
- double rhop,thetap ;
- vpMeterPixelConversion::convertLine(cam,rho,theta,rhop,thetap) ;
+ double rhop, thetap;
+ vpMeterPixelConversion::convertLine(cam, rho, theta, rhop, thetap);
// u cos(thetap) + v sin(thetap) - rhop = 0
- double co = cos(thetap) ;
- double si = sin(thetap) ;
- double c = -rhop ;
+ double co = cos(thetap);
+ double si = sin(thetap);
+ double c = -rhop;
// vpTRACE("rhop %f %f ",rhop, atan2(si,co)) ;
// double u1,v1,u2,v2 ;
- double a = si ;
- double b = co ;
+ double a = si;
+ double b = co;
vpImagePoint ip1, ip2;
if (fabs(a) < fabs(b)) {
- ip1.set_ij(0, (-c)/b);
+ ip1.set_ij(0, (-c) / b);
double h = I.getHeight() - 1;
- ip2.set_ij(h, (-c - a*h)/b);
+ ip2.set_ij(h, (-c - a * h) / b);
vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- }
- else {
- ip1.set_ij((-c)/a, 0);
- double w = I.getWidth()-1;
- ip2.set_ij((-c - b*w)/a, w);
+ } else {
+ ip1.set_ij((-c) / a, 0);
+ double w = I.getWidth() - 1;
+ ip2.set_ij((-c - b * w) / a, w);
vpDisplay::displayLine(I, ip1, ip2, color, thickness);
}
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
@@ -327,29 +287,23 @@ 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<vpRGBa> &I,
- const vpColor &color,
+void vpFeatureDisplay::displayCylinder(double rho1, double theta1, double rho2, double theta2,
+ const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
unsigned int thickness)
{
- try
- {
- displayLine(rho1, theta1, cam, I, color, thickness) ;
- displayLine(rho2, theta2, cam, I, color, thickness) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ displayLine(rho1, theta1, cam, I, color, thickness);
+ displayLine(rho2, theta2, cam, I, color, thickness);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
\param x, y, mu20, mu11, mu02 : Ellipse parameters where:
- - \f$(x,y)\f$ are the normalized coordinates of the ellipse center, respectively
- along the horizontal and vertical axis in the image.
+ - \f$(x,y)\f$ are the normalized coordinates of the ellipse center,
+ respectively along the horizontal and vertical axis in the image.
- \f$\mu_{20}, \mu_{11}, \mu_{02}\f$ are the centered moments.
\param cam : Camera intrinsic parameters.
\param I : Image.
@@ -358,68 +312,61 @@ 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<vpRGBa> &I,
- const vpColor &color,
+void vpFeatureDisplay::displayEllipse(double x, double y, double mu20, double mu11, double mu02,
+ const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
unsigned int thickness)
{
try {
- unsigned int number_of_points = 45 ;
- const double incr = 2 * M_PI/(double)number_of_points ; // angle increment
- unsigned int i = 0 ;
+ unsigned int number_of_points = 45;
+ const double incr = 2 * M_PI / (double)number_of_points; // angle increment
+ unsigned int i = 0;
- double s = sqrt(vpMath::sqr(mu20-mu02)+4*mu11*mu11) ;
- double a, b, e ;
+ double s = sqrt(vpMath::sqr(mu20 - mu02) + 4 * mu11 * mu11);
+ double a, b, e;
- //if (fabs(mu11)<1e-6) e =0 ;
+ // if (fabs(mu11)<1e-6) e =0 ;
if (fabs(mu11) < std::numeric_limits<double>::epsilon()) {
- e = 0 ;
+ e = 0;
a = sqrt(mu20);
b = sqrt(mu02);
- }
- else {
- e = (mu02-mu20+s)/(2*mu11) ;
- a = sqrt( (mu02+mu20+s)/2.0) ;
- b = sqrt( (mu02+mu20-s)/2.0) ;
+ } else {
+ e = (mu02 - mu20 + s) / (2 * mu11);
+ a = sqrt((mu02 + mu20 + s) / 2.0);
+ b = sqrt((mu02 + mu20 - s) / 2.0);
}
- double e1 = atan(e) ;
+ double e1 = atan(e);
- double k = 0.0 ;
+ double k = 0.0;
- double ce = cos(e1) ;
- double se = sin(e1) ;
+ double ce = cos(e1);
+ double se = sin(e1);
- double x2 = 0;
- double y2 =0;
+ double x2 = 0;
+ double y2 = 0;
vpImagePoint ip1, ip2;
- for( i = 0; i < number_of_points+2 ; i++)
- {
- double x1 = a *cos(k) ; // equation of an ellipse
- double y1 = b *sin(k) ; // equation of an ellipse
- double x11 = x + ce *x1 - se *y1 ;
- double y11 = y + se *x1 + ce *y1 ;
+ for (i = 0; i < number_of_points + 2; i++) {
+ double x1 = a * cos(k); // equation of an ellipse
+ double y1 = b * sin(k); // equation of an ellipse
+ double x11 = x + ce * x1 - se * y1;
+ double y11 = y + se * x1 + ce * y1;
vpMeterPixelConversion::convertPoint(cam, x11, y11, ip1);
if (i > 1) {
- ip2.set_u( x2 );
- ip2.set_v( y2 );
+ ip2.set_u(x2);
+ ip2.set_v(y2);
- vpDisplay::displayLine(I, ip1, ip2, color, thickness) ;
+ vpDisplay::displayLine(I, ip1, ip2, color, thickness);
}
ip2 = ip1;
- y2 = y1 ;
- x2 = x1 ;
- k += incr ;
+ y2 = y1;
+ x2 = x1;
+ k += incr;
} // end for loop
- }
- catch(vpException &e)
- {
+ } catch (vpException &e) {
throw(e);
}
}
diff --git a/modules/core/src/image/vpImageConvert.cpp b/modules/core/src/image/vpImageConvert.cpp
index 0dc586d..41f6adc 100644
--- a/modules/core/src/image/vpImageConvert.cpp
+++ b/modules/core/src/image/vpImageConvert.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,27 +43,26 @@
\brief Convert image types
*/
-
-#include <sstream>
#include <map>
+#include <sstream>
// image
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpImageConvert.h>
#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
-# include <emmintrin.h>
-# define VISP_HAVE_SSE2 1
-
-# if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
-# include <pmmintrin.h>
-# define VISP_HAVE_SSE3 1
-# endif
-# if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
-# include <tmmintrin.h>
-# define VISP_HAVE_SSSE3 1
-# endif
-#endif
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+#if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <pmmintrin.h>
+#define VISP_HAVE_SSE3 1
+#endif
+#if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <tmmintrin.h>
+#define VISP_HAVE_SSSE3 1
+#endif
+#endif
bool vpImageConvert::YCbCrLUTcomputed = false;
int vpImageConvert::vpCrr[256];
@@ -70,19 +70,17 @@ int vpImageConvert::vpCgb[256];
int vpImageConvert::vpCgr[256];
int vpImageConvert::vpCbb[256];
-
/*!
Convert a vpImage\<unsigned char\> to a vpImage\<vpRGBa\>.
Tha alpha component is set to vpRGBa::alpha_default.
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> & dest)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
+ 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());
}
/*!
@@ -90,34 +88,30 @@ vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<vpRGBa> & des
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> & dest)
+void vpImageConvert::convert(const vpImage<vpRGBa> &src, vpImage<unsigned char> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
+ dest.resize(src.getHeight(), src.getWidth());
RGBaToGrey((unsigned char *)src.bitmap, dest.bitmap, src.getHeight() * src.getWidth());
}
-
/*!
- Convert a vpImage\<float\> to a vpImage\<unsigend char\> by renormalizing between 0 and 255.
- \param src : source image
- \param dest : destination image
+ Convert a vpImage\<float\> to a vpImage\<unsigend char\> by renormalizing
+ between 0 and 255. \param src : source image \param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<float> &src, vpImage<unsigned char> &dest)
+void vpImageConvert::convert(const vpImage<float> &src, vpImage<unsigned char> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
- unsigned int max_xy = src.getWidth()*src.getHeight();
+ dest.resize(src.getHeight(), src.getWidth());
+ unsigned int max_xy = src.getWidth() * src.getHeight();
float min, max;
- src.getMinMaxValue(min,max);
-
+ src.getMinMaxValue(min, max);
+
for (unsigned int i = 0; i < max_xy; i++) {
float val = 255.f * (src.bitmap[i] - min) / (max - min);
- if(val < 0)
+ if (val < 0)
dest.bitmap[i] = 0;
- else if(val > 255)
+ else if (val > 255)
dest.bitmap[i] = 255;
else
dest.bitmap[i] = (unsigned char)val;
@@ -129,33 +123,30 @@ Convert a vpImage\<unsigned char\> to a vpImage\<float\> by basic casting.
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<float> &dest)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<float> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
- for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
+ dest.resize(src.getHeight(), src.getWidth());
+ for (unsigned int i = 0; i < src.getHeight() * src.getWidth(); i++)
dest.bitmap[i] = (float)src.bitmap[i];
}
/*!
-Convert a vpImage\<double\> to a vpImage\<unsigned char\> by renormalizing between 0 and 255.
-\param src : source image
-\param dest : destination image
+Convert a vpImage\<double\> to a vpImage\<unsigned char\> by renormalizing
+between 0 and 255. \param src : source image \param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<double> &src, vpImage<unsigned char> &dest)
+void vpImageConvert::convert(const vpImage<double> &src, vpImage<unsigned char> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
- unsigned int max_xy = src.getWidth()*src.getHeight();
+ dest.resize(src.getHeight(), src.getWidth());
+ unsigned int max_xy = src.getWidth() * src.getHeight();
double min, max;
- src.getMinMaxValue(min,max);
-
+ src.getMinMaxValue(min, max);
+
for (unsigned int i = 0; i < max_xy; i++) {
double val = 255. * (src.bitmap[i] - min) / (max - min);
- if(val < 0)
+ if (val < 0)
dest.bitmap[i] = 0;
- else if(val > 255)
+ else if (val > 255)
dest.bitmap[i] = 255;
else
dest.bitmap[i] = (unsigned char)val;
@@ -167,12 +158,11 @@ Convert a vpImage\<uint16_t> to a vpImage\<unsigned char\>.
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<uint16_t> &src, vpImage<unsigned char> &dest)
+void vpImageConvert::convert(const vpImage<uint16_t> &src, vpImage<unsigned char> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
+ dest.resize(src.getHeight(), src.getWidth());
- for (unsigned int i=0; i< src.getSize(); i++)
+ for (unsigned int i = 0; i < src.getSize(); i++)
dest.bitmap[i] = (src.bitmap[i] >> 8);
}
@@ -181,12 +171,11 @@ Convert a vpImage\<unsigned char> to a vpImage\<uint16_t\>.
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<uint16_t> &dest)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<uint16_t> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
+ dest.resize(src.getHeight(), src.getWidth());
- for (unsigned int i=0; i< src.getSize(); i++)
+ for (unsigned int i = 0; i < src.getSize(); i++)
dest.bitmap[i] = (src.bitmap[i] << 8);
}
@@ -195,42 +184,41 @@ Convert a vpImage\<unsigned char\> to a vpImage\<double\> by basic casting.
\param src : source image
\param dest : destination image
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<double> &dest)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, vpImage<double> &dest)
{
- dest.resize(src.getHeight(), src.getWidth()) ;
- for (unsigned int i = 0; i < src.getHeight()*src.getWidth(); i++)
+ dest.resize(src.getHeight(), src.getWidth());
+ for (unsigned int i = 0; i < src.getHeight() * src.getWidth(); i++)
dest.bitmap[i] = (double)src.bitmap[i];
}
/*!
- Create depth histogram as a color image.
- Tha alpha component of the resulting image is set to vpRGBa::alpha_default.
- \param src_depth : source image corresponding to depth.
- \param dest_rgba : destination image containing the color histogram.
+ Convert the input 16-bits depth image to a color depth image. The input
+ depth value is assigned a color value proportional to its frequency. Tha
+ alpha component of the resulting image is set to vpRGBa::alpha_default.
+ \param src_depth : input 16-bits depth image.
+ \param dest_rgba : output color depth image.
*/
-void
-vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<vpRGBa> &dest_rgba)
+void vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<vpRGBa> &dest_rgba)
{
dest_rgba.resize(src_depth.getHeight(), src_depth.getWidth());
static uint32_t histogram[0x10000];
memset(histogram, 0, sizeof(histogram));
- for(unsigned int i = 0; i < src_depth.getSize(); ++i) ++histogram[src_depth.bitmap[i]];
- for(int i = 2; i < 0x10000; ++i) histogram[i] += histogram[i-1]; // Build a cumulative histogram for the indices in [1,0xFFFF]
- for(unsigned int i = 0; i < src_depth.getSize(); ++i)
- {
+ for (unsigned int i = 0; i < src_depth.getSize(); ++i)
+ ++histogram[src_depth.bitmap[i]];
+ for (int i = 2; i < 0x10000; ++i)
+ histogram[i] += histogram[i - 1]; // Build a cumulative histogram for the
+ // indices in [1,0xFFFF]
+
+ for (unsigned int i = 0; i < src_depth.getSize(); ++i) {
uint16_t d = src_depth.bitmap[i];
- if(d)
- {
+ if (d) {
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;
dest_rgba.bitmap[i].A = vpRGBa::alpha_default;
- }
- else
- {
+ } else {
dest_rgba.bitmap[i].R = 20;
dest_rgba.bitmap[i].G = 5;
dest_rgba.bitmap[i].B = 0;
@@ -239,6 +227,35 @@ vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage
}
}
+/*!
+ Convert the input 16-bits depth image to a 8-bits depth image. The input
+ depth value is assigned a value proportional to its frequency. \param
+ src_depth : input 16-bits depth image. \param dest_depth : output grayscale
+ depth image.
+*/
+void vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage<unsigned char> &dest_depth)
+{
+ dest_depth.resize(src_depth.getHeight(), src_depth.getWidth());
+ static uint32_t histogram2[0x10000];
+ memset(histogram2, 0, sizeof(histogram2));
+
+ for (unsigned int i = 0; i < src_depth.getSize(); ++i)
+ ++histogram2[src_depth.bitmap[i]];
+ for (int i = 2; i < 0x10000; ++i)
+ histogram2[i] += histogram2[i - 1]; // Build a cumulative histogram for
+ // the indices in [1,0xFFFF]
+
+ 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
+ dest_depth.bitmap[i] = f;
+ } else {
+ dest_depth.bitmap[i] = 0;
+ }
+ }
+}
+
#ifdef VISP_HAVE_OPENCV
// Deprecated: will be removed with OpenCV transcient from C to C++ api
/*!
@@ -252,7 +269,8 @@ vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage
OpenCV documentation, or http://opencvlibrary.sourceforge.net/CxCore
for the specific IplImage structure documentation.
- If the input image has only 1 or 3 channels, the alpha channel is set to vpRGBa::alpha_default.
+ If the input image has only 1 or 3 channels, the alpha channel is set to
+vpRGBa::alpha_default.
\warning This function is only available if OpenCV was detected during
the configuration step.
@@ -264,8 +282,8 @@ vpImageConvert::createDepthHistogram(const vpImage<uint16_t> &src_depth, vpImage
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -286,8 +304,7 @@ int main()
}
\endcode
*/
-void
-vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
+void vpImageConvert::convert(const IplImage *src, vpImage<vpRGBa> &dest, bool flip)
{
int nChannel = src->nChannels;
int depth = src->depth;
@@ -296,42 +313,37 @@ vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
int widthStep = src->widthStep;
int lineStep = (flip) ? 1 : 0;
- if(nChannel == 3 && depth == 8){
+ if (nChannel == 3 && depth == 8) {
dest.resize((unsigned int)height, (unsigned int)width);
- //starting source address
- unsigned char* input = (unsigned char*)src->imageData;
- unsigned char* beginOutput = (unsigned char*)dest.bitmap;
+ // starting source address
+ unsigned char *input = (unsigned char *)src->imageData;
+ unsigned char *beginOutput = (unsigned char *)dest.bitmap;
- for(int i=0 ; i < height ; i++)
- {
+ 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;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line+2);
- *(output++) = *(line+1);
+ 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);
*(output++) = *(line);
*(output++) = vpRGBa::alpha_default;
- line+=3;
+ line += 3;
}
- //go to the next line
- input+=widthStep;
+ // go to the next line
+ input += widthStep;
}
- }
- else if(nChannel == 1 && depth == 8 ){
+ } else if (nChannel == 1 && depth == 8) {
dest.resize((unsigned int)height, (unsigned int)width);
- //starting source address
- unsigned char *input = (unsigned char*)src->imageData;
- unsigned char *beginOutput = (unsigned char*)dest.bitmap;
+ // starting source address
+ unsigned char *input = (unsigned char *)src->imageData;
+ unsigned char *beginOutput = (unsigned char *)dest.bitmap;
- for(int i=0 ; i < height ; i++)
- {
+ 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;
- for(int j=0 ; j < width ; j++)
- {
+ 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);
*output++ = *(line);
@@ -339,8 +351,8 @@ vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
line++;
}
- //go to the next line
- input+=widthStep;
+ // go to the next line
+ input += widthStep;
}
}
}
@@ -366,8 +378,8 @@ vpImageConvert::convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -388,8 +400,7 @@ int main()
}
\endcode
*/
-void
-vpImageConvert::convert(const IplImage* src, vpImage<unsigned char> &dest, bool flip)
+void vpImageConvert::convert(const IplImage *src, vpImage<unsigned char> &dest, bool flip)
{
int nChannel = src->nChannels;
int depth = src->depth;
@@ -398,51 +409,44 @@ vpImageConvert::convert(const IplImage* src, vpImage<unsigned char> &dest, bool
int widthStep = src->widthStep;
int lineStep = (flip) ? 1 : 0;
- if (flip == false)
- {
- if(widthStep == width){
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- memcpy(dest.bitmap, src->imageData,
- (size_t)(height*width));
+ if (flip == false) {
+ if (widthStep == width) {
+ if (nChannel == 1 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ memcpy(dest.bitmap, src->imageData, (size_t)(height * width));
}
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- BGRToGrey((unsigned char*)src->imageData,dest.bitmap, (unsigned int)width, (unsigned int)height,false);
+ if (nChannel == 3 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ BGRToGrey((unsigned char *)src->imageData, dest.bitmap, (unsigned int)width, (unsigned int)height, false);
}
- }
- else{
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- for (int i =0 ; i < height ; i++){
- memcpy(dest.bitmap+i*width, src->imageData + i*widthStep,
- (size_t)width);
+ } else {
+ if (nChannel == 1 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ for (int i = 0; i < height; i++) {
+ memcpy(dest.bitmap + i * width, src->imageData + i * widthStep, (size_t)width);
}
}
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- for (int i = 0 ; i < height ; i++){
- BGRToGrey((unsigned char*)src->imageData + i*widthStep,
- dest.bitmap + i*width, (unsigned int)width, 1, false);
+ if (nChannel == 3 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ for (int i = 0; i < height; i++) {
+ BGRToGrey((unsigned char *)src->imageData + i * widthStep, dest.bitmap + i * width, (unsigned int)width, 1,
+ false);
}
}
}
- }
- else
- {
- if(nChannel == 1 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- unsigned char* beginOutput = (unsigned char*)dest.bitmap;
- for (int i =0 ; i < height ; i++){
- memcpy(beginOutput + lineStep * ( 4 * width * ( height - 1 - i ) ) , src->imageData + i*widthStep,
- (size_t)width);
+ } else {
+ if (nChannel == 1 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ unsigned char *beginOutput = (unsigned char *)dest.bitmap;
+ for (int i = 0; i < height; i++) {
+ memcpy(beginOutput + lineStep * (4 * width * (height - 1 - i)), src->imageData + i * widthStep, (size_t)width);
}
}
- if(nChannel == 3 && depth == 8){
- dest.resize((unsigned int)height, (unsigned int)width) ;
- //for (int i = 0 ; i < height ; i++){
- BGRToGrey((unsigned char*)src->imageData /*+ i*widthStep*/,
- dest.bitmap /*+ i*width*/, (unsigned int)width, (unsigned int)height/*1*/, true);
+ if (nChannel == 3 && depth == 8) {
+ dest.resize((unsigned int)height, (unsigned int)width);
+ // for (int i = 0 ; i < height ; i++){
+ BGRToGrey((unsigned char *)src->imageData /*+ i*widthStep*/, dest.bitmap /*+ i*width*/, (unsigned int)width,
+ (unsigned int)height /*1*/, true);
//}
}
}
@@ -468,8 +472,8 @@ vpImageConvert::convert(const IplImage* src, vpImage<unsigned char> &dest, bool
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -492,46 +496,42 @@ int main()
}
\endcode
*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> & src, IplImage *&dest)
+void vpImageConvert::convert(const vpImage<vpRGBa> &src, IplImage *&dest)
{
int height = (int)src.getHeight();
- int width = (int)src.getWidth();
+ int width = (int)src.getWidth();
CvSize size = cvSize(width, height);
int depth = 8;
int channels = 3;
- if (dest != NULL){
- if(dest->nChannels != channels || dest->depth != depth
- || dest->height != height || dest->width != width){
- if(dest->nChannels != 0) cvReleaseImage(&dest);
- dest = cvCreateImage( size, depth, channels );
+ if (dest != NULL) {
+ if (dest->nChannels != channels || dest->depth != depth || dest->height != height || dest->width != width) {
+ if (dest->nChannels != 0)
+ cvReleaseImage(&dest);
+ dest = cvCreateImage(size, depth, channels);
}
- }
- else dest = cvCreateImage( size, depth, channels );
+ } else
+ dest = cvCreateImage(size, depth, channels);
+ // starting source address
+ unsigned char *input = (unsigned char *)src.bitmap; // rgba image
+ unsigned char *output = (unsigned char *)dest->imageData; // bgr image
- //starting source address
- unsigned char * input = (unsigned char*)src.bitmap;//rgba image
- unsigned char * output = (unsigned char*)dest->imageData;//bgr image
-
- int j=0;
- int i=0;
+ int j = 0;
+ int i = 0;
int widthStep = dest->widthStep;
- for(i=0 ; i < height ; i++)
- {
- output = (unsigned char*)dest->imageData + i*widthStep;
+ for (i = 0; i < height; i++) {
+ output = (unsigned char *)dest->imageData + i * widthStep;
unsigned char *line = input;
- for( j=0 ; j < width ; j++)
- {
- *output++ = *(line+2); //B
- *output++ = *(line+1); //G
- *output++ = *(line); //R
+ for (j = 0; j < width; j++) {
+ *output++ = *(line + 2); // B
+ *output++ = *(line + 1); // G
+ *output++ = *(line); // R
- line+=4;
+ line += 4;
}
- //go to the next line
- input+=4*width;
+ // go to the next line
+ input += 4 * width;
}
}
@@ -555,8 +555,8 @@ vpImageConvert::convert(const vpImage<vpRGBa> & src, IplImage *&dest)
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -579,33 +579,31 @@ int main()
}
\endcode
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> & src, IplImage* &dest)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, IplImage *&dest)
{
unsigned int height = src.getHeight();
- unsigned int width = src.getWidth();
+ unsigned int width = src.getWidth();
CvSize size = cvSize((int)width, (int)height);
int depth = 8;
int channels = 1;
- if (dest != NULL){
- if(dest->nChannels != channels || dest->depth != depth
- || dest->height != (int) height || dest->width != (int) width){
- if(dest->nChannels != 0) cvReleaseImage(&dest);
- dest = cvCreateImage( size, depth, channels );
+ if (dest != NULL) {
+ if (dest->nChannels != channels || dest->depth != depth || dest->height != (int)height ||
+ dest->width != (int)width) {
+ if (dest->nChannels != 0)
+ cvReleaseImage(&dest);
+ dest = cvCreateImage(size, depth, channels);
}
- }
- else dest = cvCreateImage( size, depth, channels );
+ } else
+ dest = cvCreateImage(size, depth, channels);
unsigned int widthStep = (unsigned int)dest->widthStep;
- if ( width == widthStep){
- memcpy(dest->imageData,src.bitmap, width*height);
- }
- else{
- //copying each line taking account of the widthStep
- for (unsigned int i =0 ; i < height ; i++){
- memcpy(dest->imageData + i*widthStep, src.bitmap + i*width,
- width);
+ if (width == widthStep) {
+ memcpy(dest->imageData, src.bitmap, width * height);
+ } else {
+ // copying each line taking account of the widthStep
+ for (unsigned int i = 0; i < height; i++) {
+ memcpy(dest->imageData + i * widthStep, src.bitmap + i * width, width);
}
}
}
@@ -619,14 +617,15 @@ vpImageConvert::convert(const vpImage<unsigned char> & src, IplImage* &dest)
http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
for the specific Mat structure documentation.
- Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
- method, only cv::Mat with a depth equal to 8 and a channel between 1 and 3 are
- converted.
+ Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool
+flip) method, only cv::Mat with a depth equal to 8 and a channel between 1 and
+3 are converted.
- If the input image is of type CV_8UC1 or CV_8UC3, the alpha channel is set to vpRGBa::alpha_default.
+ If the input image is of type CV_8UC1 or CV_8UC3, the alpha channel is set
+to vpRGBa::alpha_default.
- \warning This function is only available if OpenCV (version 2.1.0 or greater)
- was detected during the configuration step.
+ \warning This function is only available if OpenCV (version 2.1.0 or
+greater) was detected during the configuration step.
\param src : Source image in OpenCV format.
\param dest : Destination image in ViSP format.
@@ -635,9 +634,9 @@ vpImageConvert::convert(const vpImage<unsigned char> & src, IplImage* &dest)
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -655,50 +654,49 @@ int main()
}
\endcode
*/
-void
-vpImageConvert::convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool flip)
+void vpImageConvert::convert(const cv::Mat &src, vpImage<vpRGBa> &dest, const bool flip)
{
- if(src.type() == CV_8UC4){
+ if (src.type() == CV_8UC4) {
dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
vpRGBa rgbaVal;
- for(unsigned int i=0; i<dest.getRows(); ++i)
- for(unsigned int j=0; j<dest.getCols(); ++j){
+ for (unsigned int i = 0; i < dest.getRows(); ++i)
+ for (unsigned int j = 0; j < dest.getCols(); ++j) {
cv::Vec4b tmp = src.at<cv::Vec4b>((int)i, (int)j);
rgbaVal.R = tmp[2];
rgbaVal.G = tmp[1];
rgbaVal.B = tmp[0];
rgbaVal.A = tmp[3];
- if(flip)
- dest[dest.getRows()-i-1][j] = rgbaVal;
+ if (flip)
+ dest[dest.getRows() - i - 1][j] = rgbaVal;
else
dest[i][j] = rgbaVal;
}
- }else if(src.type() == CV_8UC3){
+ } else if (src.type() == CV_8UC3) {
dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
vpRGBa rgbaVal;
rgbaVal.A = vpRGBa::alpha_default;
- for(unsigned int i=0; i<dest.getRows(); ++i){
- for(unsigned int j=0; j<dest.getCols(); ++j){
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ for (unsigned int j = 0; j < dest.getCols(); ++j) {
cv::Vec3b tmp = src.at<cv::Vec3b>((int)i, (int)j);
rgbaVal.R = tmp[2];
rgbaVal.G = tmp[1];
rgbaVal.B = tmp[0];
- if(flip){
- dest[dest.getRows()-i-1][j] = rgbaVal;
- }else{
+ if (flip) {
+ dest[dest.getRows() - i - 1][j] = rgbaVal;
+ } else {
dest[i][j] = rgbaVal;
}
}
}
- }else if(src.type() == CV_8UC1){
+ } else if (src.type() == CV_8UC1) {
dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
vpRGBa rgbaVal;
- for(unsigned int i=0; i<dest.getRows(); ++i){
- for(unsigned int j=0; j<dest.getCols(); ++j){
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ for (unsigned int j = 0; j < dest.getCols(); ++j) {
rgbaVal = src.at<unsigned char>((int)i, (int)j);
- if(flip){
- dest[dest.getRows()-i-1][j] = rgbaVal;
- }else{
+ if (flip) {
+ dest[dest.getRows() - i - 1][j] = rgbaVal;
+ } else {
dest[i][j] = rgbaVal;
}
}
@@ -714,9 +712,9 @@ vpImageConvert::convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool fl
http://opencv.willowgarage.com/documentation/cpp/core_basic_structures.html
for the specific Mat structure documentation.
- Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool flip)
- method, only Mat with a depth equal to 8 and a channel between 1 and 3 are
- converted.
+ Similarily to the convert(const IplImage* src, vpImage<vpRGBa> & dest, bool
+flip) method, only Mat with a depth equal to 8 and a channel between 1 and 3
+are converted.
\warning This function is only available if OpenCV was detected during
the configuration step.
@@ -728,8 +726,8 @@ vpImageConvert::convert(const cv::Mat& src, vpImage<vpRGBa>& dest, const bool fl
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -748,41 +746,38 @@ int main()
\endcode
*/
-void
-vpImageConvert::convert(const cv::Mat& src, vpImage<unsigned char>& dest, const bool flip)
+void vpImageConvert::convert(const cv::Mat &src, vpImage<unsigned char> &dest, const bool flip)
{
- if(src.type() == CV_8UC1){
+ if (src.type() == CV_8UC1) {
dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- if(src.isContinuous() && !flip){
- memcpy(dest.bitmap, src.data, (size_t)(src.rows*src.cols));
- }
- else{
- if(flip){
- for(unsigned int i=0; i<dest.getRows(); ++i){
- memcpy(dest.bitmap+i*dest.getCols(), src.data+(dest.getRows()-i-1)*src.step1(), (size_t)src.step);
+ if (src.isContinuous() && !flip) {
+ memcpy(dest.bitmap, src.data, (size_t)(src.rows * src.cols));
+ } else {
+ if (flip) {
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ memcpy(dest.bitmap + i * dest.getCols(), src.data + (dest.getRows() - i - 1) * src.step1(), (size_t)src.step);
}
- }else{
- for(unsigned int i=0; i<dest.getRows(); ++i){
- memcpy(dest.bitmap+i*dest.getCols(), src.data+i*src.step1(), (size_t)src.step);
+ } else {
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ memcpy(dest.bitmap + i * dest.getCols(), src.data + i * src.step1(), (size_t)src.step);
}
}
}
- }else if(src.type() == CV_8UC3){
+ } else if (src.type() == CV_8UC3) {
dest.resize((unsigned int)src.rows, (unsigned int)src.cols);
- if(src.isContinuous() /*&& !flip*/){
- BGRToGrey((unsigned char*)src.data, (unsigned char*)dest.bitmap, (unsigned int)src.cols, (unsigned int)src.rows, flip);
- }
- else{
- if(flip){
- for(unsigned int i=0; i<dest.getRows(); ++i){
- BGRToGrey((unsigned char*)src.data+i*src.step1(),
- (unsigned char*)dest.bitmap+(dest.getRows()-i-1)*dest.getCols(),
+ if (src.isContinuous() /*&& !flip*/) {
+ BGRToGrey((unsigned char *)src.data, (unsigned char *)dest.bitmap, (unsigned int)src.cols, (unsigned int)src.rows,
+ flip);
+ } else {
+ if (flip) {
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ BGRToGrey((unsigned char *)src.data + i * src.step1(),
+ (unsigned char *)dest.bitmap + (dest.getRows() - i - 1) * dest.getCols(),
(unsigned int)dest.getCols(), 1, false);
}
- }else{
- for(unsigned int i=0; i<dest.getRows(); ++i){
- BGRToGrey((unsigned char*)src.data+i*src.step1(),
- (unsigned char*)dest.bitmap+i*dest.getCols(),
+ } else {
+ for (unsigned int i = 0; i < dest.getRows(); ++i) {
+ BGRToGrey((unsigned char *)src.data + i * src.step1(), (unsigned char *)dest.bitmap + i * dest.getCols(),
(unsigned int)dest.getCols(), 1, false);
}
}
@@ -790,7 +785,6 @@ vpImageConvert::convert(const cv::Mat& src, vpImage<unsigned char>& dest, const
}
}
-
/*!
Convert a vpImage\<unsigned char\> to a cv::Mat.
@@ -808,8 +802,8 @@ vpImageConvert::convert(const cv::Mat& src, vpImage<unsigned char>& dest, const
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -830,16 +824,15 @@ int main()
\endcode
*/
-void
-vpImageConvert::convert(const vpImage<vpRGBa> & src, cv::Mat& dest)
+void vpImageConvert::convert(const vpImage<vpRGBa> &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);
cv::Mat out[] = {dest, alpha};
- int from_to[] = { 0,2, 1,1, 2,0, 3,3 };
+ int from_to[] = {0, 2, 1, 1, 2, 0, 3, 3};
cv::mixChannels(&vpToMat, 1, out, 2, from_to, 4);
}
@@ -856,14 +849,14 @@ vpImageConvert::convert(const vpImage<vpRGBa> & src, cv::Mat& dest)
\param src : source image
\param dest : destination image
- \param copyData : if true, the image is copied and modification in one object
- will not modified the other.
+ \param copyData : if true, the image is copied and modification in one
+object will not modified the other.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -884,14 +877,13 @@ int main()
\endcode
*/
-void
-vpImageConvert::convert(const vpImage<unsigned char> & src, cv::Mat& dest, const bool copyData)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, cv::Mat &dest, const bool copyData)
{
- if(copyData){
- cv::Mat tmpMap((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
+ if (copyData) {
+ cv::Mat tmpMap((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void *)src.bitmap);
dest = tmpMap.clone();
- }else{
- dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void*)src.bitmap);
+ } else {
+ dest = cv::Mat((int)src.getRows(), (int)src.getCols(), CV_8UC1, (void *)src.bitmap);
}
}
@@ -900,20 +892,23 @@ vpImageConvert::convert(const vpImage<unsigned char> & src, cv::Mat& dest, const
#ifdef VISP_HAVE_YARP
/*!
- Convert a vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
+ Convert a vpImage\<unsigned char\> to a
+yarp::sig::ImageOf\<yarp::sig::PixelMono\>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
\param src : Source image in ViSP format.
\param dest : Destination image in YARP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+ \param copyData : Set to true to copy all the image content. If false we
+only update the image pointer.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -921,8 +916,8 @@ int main()
vpImage<unsigned char> I; // A mocochrome image
// Read an image on a disk
vpImageIo::read(I, "image.pgm");
-
- yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelMono >();
+
+ yarp::sig::ImageOf< yarp::sig::PixelMono > *Iyarp = new yarp::sig::ImageOf<yarp::sig::PixelMono >();
// Convert the vpImage\<unsigned char\> to a yarp::sig::ImageOf\<yarp::sig::PixelMono\>
vpImageConvert::convert(I, Iyarp);
@@ -931,33 +926,34 @@ int main()
}
\endcode
*/
-void vpImageConvert::convert(const vpImage<unsigned char> & src,
- yarp::sig::ImageOf< yarp::sig::PixelMono > *dest, const bool copyData)
+void vpImageConvert::convert(const vpImage<unsigned char> &src, yarp::sig::ImageOf<yarp::sig::PixelMono> *dest,
+ const bool copyData)
{
- if(copyData)
- {
- dest->resize(src.getWidth(),src.getHeight());
- memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth());
- }
- else
+ if (copyData) {
+ dest->resize(src.getWidth(), src.getHeight());
+ memcpy(dest->getRawImage(), src.bitmap, src.getHeight() * src.getWidth());
+ } else
dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
}
/*!
- Convert a yarp::sig::ImageOf\<yarp::sig::PixelMono\> to a vpImage\<unsigned char\>
+ Convert a yarp::sig::ImageOf\<yarp::sig::PixelMono\> to a vpImage\<unsigned
+char\>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
\param src : Source image in YARP format.
\param dest : Destination image in ViSP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+ \param copyData : Set to true to copy all the image content. If false we
+only update the image pointer.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
#if defined(VISP_HAVE_YARP)
#include <yarp/sig/ImageFile.h>
@@ -966,10 +962,10 @@ void vpImageConvert::convert(const vpImage<unsigned char> & src,
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<yarp::sig::PixelMono >();
// Read an image on a disk
yarp::sig::file::read(*Iyarp, "image.pgm");
-
+
// Convert the yarp::sig::ImageOf<yarp::sig::PixelMono> to a vpImage<unsigned char>
vpImage<unsigned char> I;
vpImageConvert::convert(Iyarp, I);
@@ -979,12 +975,12 @@ int main()
}
\endcode
*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *src,
- vpImage<unsigned char> & dest,const bool copyData)
+void vpImageConvert::convert(const yarp::sig::ImageOf<yarp::sig::PixelMono> *src, vpImage<unsigned char> &dest,
+ const bool copyData)
{
- dest.resize(src->height(),src->width());
- if(copyData)
- memcpy(dest.bitmap, src->getRawImage(), src->height()*src->width()*sizeof(yarp::sig::PixelMono));
+ dest.resize(src->height(), src->width());
+ if (copyData)
+ memcpy(dest.bitmap, src->getRawImage(), src->height() * src->width() * sizeof(yarp::sig::PixelMono));
else
dest.bitmap = src->getRawImage();
}
@@ -992,19 +988,21 @@ void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelMono > *s
/*!
Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgba>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
\param src : Source image in ViSP format.
\param dest : Destination image in YARP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
+ \param copyData : Set to true to copy all the image content. If false we
+only update the image pointer.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -1012,8 +1010,8 @@ int main()
vpImage<vpRGBa> I; // A color image
// Read an image on a disk
vpImageIo::read(I,"image.jpg");
-
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgba >();
+
+ yarp::sig::ImageOf< yarp::sig::PixelRgba > *Iyarp = new yarp::sig::ImageOf<yarp::sig::PixelRgba >();
// Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgba>
vpImageConvert::convert(I,Iyarp);
@@ -1021,34 +1019,35 @@ int main()
#endif
}
\endcode
-*/
-void vpImageConvert::convert(const vpImage<vpRGBa> & src,
- yarp::sig::ImageOf< yarp::sig::PixelRgba > *dest, const bool copyData)
+*/
+void vpImageConvert::convert(const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgba> *dest,
+ const bool copyData)
{
- if(copyData){
- dest->resize(src.getWidth(),src.getHeight());
- memcpy(dest->getRawImage(), src.bitmap, src.getHeight()*src.getWidth()*sizeof(vpRGBa));
- }
- else
+ if (copyData) {
+ dest->resize(src.getWidth(), src.getHeight());
+ memcpy(dest->getRawImage(), src.bitmap, src.getHeight() * src.getWidth() * sizeof(vpRGBa));
+ } else
dest->setExternal(src.bitmap, (int)src.getCols(), (int)src.getRows());
}
/*!
Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgba> to a vpImage\<vpRGBa\>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
\param src : Source image in YARP format.
\param dest : Destination image in ViSP format.
- \param copyData : Set to true to copy all the image content. If false we only update the image pointer.
-
+ \param copyData : Set to true to copy all the image content. If false we
+only update the image pointer.
+
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
#if defined(VISP_HAVE_YARP)
#include <yarp/sig/ImageFile.h>
@@ -1057,10 +1056,10 @@ void vpImageConvert::convert(const vpImage<vpRGBa> & src,
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<yarp::sig::PixelRgba >();
// Read an image on a disk
yarp::sig::file::read(*Iyarp,"image.pgm");
-
+
// Convert the yarp::sig::ImageOf<yarp::sig::PixelRgba> to a vpImage<vpRGBa>
vpImage<vpRGBa> I;
vpImageConvert::convert(Iyarp,I);
@@ -1070,21 +1069,22 @@ int main()
}
\endcode
*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *src,
- vpImage<vpRGBa> & dest,const bool copyData)
+void vpImageConvert::convert(const yarp::sig::ImageOf<yarp::sig::PixelRgba> *src, vpImage<vpRGBa> &dest,
+ const bool copyData)
{
- dest.resize(src->height(),src->width());
- if(copyData)
- memcpy(dest.bitmap, src->getRawImage(),src->height()*src->width()*sizeof(yarp::sig::PixelRgba));
+ dest.resize(src->height(), src->width());
+ if (copyData)
+ memcpy(dest.bitmap, src->getRawImage(), src->height() * src->width() * sizeof(yarp::sig::PixelRgba));
else
- dest.bitmap = (vpRGBa*)src->getRawImage();
+ dest.bitmap = static_cast<vpRGBa *>(src->getRawImage());
}
/*!
Convert a vpImage\<vpRGBa\> to a yarp::sig::ImageOf\<yarp::sig::PixelRgb>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
\param src : Source image in ViSP format.
\param dest : Destination image in YARP format.
@@ -1092,9 +1092,9 @@ void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgba > *s
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -1102,8 +1102,8 @@ int main()
vpImage<vpRGBa> I; // A color image
// Read an image on a disk
vpImageIo::read(I,"image.jpg");
-
- yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf< yarp::sig::PixelRgb >();
+
+ yarp::sig::ImageOf< yarp::sig::PixelRgb > *Iyarp = new yarp::sig::ImageOf<yarp::sig::PixelRgb >();
// Convert the vpImage<vpRGBa> to a yarp::sig::ImageOf<yarp::sig::PixelRgb>
vpImageConvert::convert(I,Iyarp);
@@ -1112,14 +1112,14 @@ int main()
}
\endcode
*/
-void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< yarp::sig::PixelRgb > *dest)
+void vpImageConvert::convert(const vpImage<vpRGBa> &src, yarp::sig::ImageOf<yarp::sig::PixelRgb> *dest)
{
- dest->resize(src.getWidth(),src.getHeight());
- for(unsigned int i = 0 ; i < src.getRows() ; i++){
- for(unsigned int j = 0 ; j < src.getWidth() ; j++){
- dest->pixel(j,i).r = src[i][j].R;
- dest->pixel(j,i).g = src[i][j].G;
- dest->pixel(j,i).b = src[i][j].B;
+ dest->resize(src.getWidth(), src.getHeight());
+ for (unsigned int i = 0; i < src.getRows(); i++) {
+ for (unsigned int j = 0; j < src.getWidth(); j++) {
+ dest->pixel(j, i).r = src[i][j].R;
+ dest->pixel(j, i).g = src[i][j].G;
+ dest->pixel(j, i).b = src[i][j].B;
}
}
}
@@ -1127,8 +1127,9 @@ void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< ya
/*!
Convert a yarp::sig::ImageOf\<yarp::sig::PixelRgb> to a vpImage\<vpRGBa\>
- A yarp::sig::Image is a YARP image class. See http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for
- the YARP image class documentation.
+ A yarp::sig::Image is a YARP image class. See
+http://eris.liralab.it/yarpdoc/df/d15/classyarp_1_1sig_1_1Image.html for the
+YARP image class documentation.
The alpha component of the resulting image is set to vpRGBa::alpha_default.
@@ -1138,9 +1139,9 @@ void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< ya
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/io/vpImageIo.h>
#if defined(VISP_HAVE_YARP)
#include <yarp/sig/ImageFile.h>
@@ -1149,10 +1150,10 @@ void vpImageConvert::convert(const vpImage<vpRGBa> & src, yarp::sig::ImageOf< ya
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<yarp::sig::PixelRgb >();
// Read an image on a disk
yarp::sig::file::read(*Iyarp,"image.pgm");
-
+
// Convert the yarp::sig::ImageOf<yarp::sig::PixelRgb> to a vpImage<vpRGBa>
vpImage<vpRGBa> I;
vpImageConvert::convert(Iyarp,I);
@@ -1162,14 +1163,14 @@ int main()
}
\endcode
*/
-void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *src, vpImage<vpRGBa> & dest)
+void vpImageConvert::convert(const yarp::sig::ImageOf<yarp::sig::PixelRgb> *src, vpImage<vpRGBa> &dest)
{
- dest.resize(src->height(),src->width());
- for(int i = 0 ; i < src->height() ; i++){
- for(int j = 0 ; j < src->width() ; j++){
- dest[i][j].R = src->pixel(j,i).r;
- dest[i][j].G = src->pixel(j,i).g;
- dest[i][j].B = src->pixel(j,i).b;
+ dest.resize(src->height(), src->width());
+ for (int i = 0; i < src->height(); i++) {
+ for (int j = 0; j < src->width(); j++) {
+ dest[i][j].R = src->pixel(j, i).r;
+ dest[i][j].G = src->pixel(j, i).g;
+ dest[i][j].B = src->pixel(j, i).b;
dest[i][j].A = vpRGBa::alpha_default;
}
}
@@ -1177,8 +1178,13 @@ void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *sr
#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.
Destination rgba memory area has to be allocated before.
@@ -1187,8 +1193,7 @@ void vpImageConvert::convert(const yarp::sig::ImageOf< yarp::sig::PixelRgb > *sr
\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;
@@ -1243,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;
@@ -1296,52 +1300,59 @@ 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;
+ unsigned int i = 0, j = 0;
- while( j < size*2)
- {
+ while (j < size * 2) {
grey[i++] = yuyv[j];
- grey[i++] = yuyv[j+2];
- j+=4;
+ grey[i++] = yuyv[j + 2];
+ j += 4;
}
}
-
/*!
- Convert YUV411 (u y1 y2 v y3 y4) images into RGBa images. The alpha component of the converted
- image is set to vpRGBa::alpha_default.
+ Convert YUV411 (u y1 y2 v y3 y4) images into RGBa images. The alpha
+ 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;
- for(unsigned int i = size / 4; i; i--) {
- int U = (int)((*yuv++ - 128) * 0.354);
- int U5 = 5*U;
- int Y0 = *yuv++;
- int Y1 = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int Y2 = *yuv++;
- int Y3 = *yuv++;
- int UV = - U - V;
+ for (unsigned int i = size / 4; i; i--) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y0 = *yuv++;
+ int Y1 = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int Y2 = *yuv++;
+ int Y3 = *yuv++;
+ int UV = -U - V;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
int R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1350,13 +1361,22 @@ void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1365,13 +1385,22 @@ void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1380,13 +1409,22 @@ void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1395,43 +1433,41 @@ void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
}
#else
// tres tres lent ....
- unsigned int i=0,j=0;
+ unsigned int i = 0, j = 0;
unsigned char r, g, b;
- while( j < numpixels*3/2)
- {
-
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
-
- YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
-
- YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
-
- YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
-
- j+=6;
+ while (j < numpixels * 3 / 2) {
+
+ YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+
+ YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+
+ YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+
+ YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+
+ j += 6;
}
#endif
-
}
/*!
@@ -1442,29 +1478,38 @@ void vpImageConvert::YUV411ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
\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
// std::cout << "call optimized convertYUV422ToRGBa()" << std::endl;
- for( unsigned int i = size / 2; i; i-- ) {
- int U = (int)((*yuv++ - 128) * 0.354);
- int U5 = 5*U;
- int Y0 = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int Y1 = *yuv++;
- int UV = - U - V;
+ for (unsigned int i = size / 2; i; i--) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y0 = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int Y1 = *yuv++;
+ int UV = -U - V;
//---
int R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1473,13 +1518,22 @@ void vpImageConvert::YUV422ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1489,27 +1543,25 @@ void vpImageConvert::YUV422ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
#else
// tres tres lent ....
- unsigned int i=0,j=0;
+ unsigned int i = 0, j = 0;
unsigned char r, g, b;
- while( j < size*2)
- {
-
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
-
- YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
- rgba[i] = r;
- rgba[i+1] = g;
- rgba[i+2] = b;
- rgba[i+3] = vpRGBa::alpha_default;
- i+=4;
- j+=4;
-
+ while (j < size * 2) {
+
+ YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+
+ YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b);
+ rgba[i] = r;
+ rgba[i + 1] = g;
+ rgba[i + 2] = b;
+ rgba[i + 3] = vpRGBa::alpha_default;
+ i += 4;
+ j += 4;
}
#endif
}
@@ -1520,19 +1572,18 @@ 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)
- {
- grey[i ] = yuv[j+1];
- grey[i+1] = yuv[j+2];
- grey[i+2] = yuv[j+4];
- grey[i+3] = yuv[j+5];
+ unsigned int i = 0, j = 0;
+ while (j < size * 3 / 2) {
+ grey[i] = yuv[j + 1];
+ grey[i + 1] = yuv[j + 2];
+ grey[i + 2] = yuv[j + 4];
+ grey[i + 3] = yuv[j + 5];
- i+=4;
+ i += 4;
- j+=6;
+ j += 6;
}
}
@@ -1544,28 +1595,37 @@ void vpImageConvert::YUV411ToGrey(unsigned char* yuv, unsigned char* grey, unsig
\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;
- for( unsigned int i = size / 2; i; i-- ) {
- int U = (int)((*yuv++ - 128) * 0.354);
- int U5 = 5*U;
- int Y0 = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int Y1 = *yuv++;
- int UV = - U - V;
+ for (unsigned int i = size / 2; i; i--) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y0 = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int Y1 = *yuv++;
+ int UV = -U - V;
//---
int R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1573,41 +1633,47 @@ void vpImageConvert::YUV422ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb++ = (unsigned char)B;
-
}
#else
// tres tres lent ....
- unsigned int i=0,j=0;
+ unsigned int i = 0, j = 0;
unsigned char r, g, b;
- while( j < size*2)
- {
+ while (j < size * 2) {
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+2], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+3], yuv[j], yuv[j+2], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
- j+=4;
+ YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 2], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+ YUVToRGB(yuv[j + 3], yuv[j], yuv[j + 2], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+ j += 4;
}
#endif
}
@@ -1620,15 +1686,14 @@ void vpImageConvert::YUV422ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
\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;
+ unsigned int i = 0, j = 0;
- while( j < size*2)
- {
- grey[i++] = yuv[j+1];
- grey[i++] = yuv[j+3];
- j+=4;
+ while (j < size * 2) {
+ grey[i++] = yuv[j + 1];
+ grey[i++] = yuv[j + 3];
+ j += 4;
}
}
@@ -1638,33 +1703,42 @@ 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;
- for(unsigned int i = size / 4; i; i--) {
- int U = (int)((*yuv++ - 128) * 0.354);
- int U5 = 5*U;
- int Y0 = *yuv++;
- int Y1 = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int Y2 = *yuv++;
- int Y3 = *yuv++;
- int UV = - U - V;
+ for (unsigned int i = size / 4; i; i--) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y0 = *yuv++;
+ int Y1 = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int Y2 = *yuv++;
+ int Y3 = *yuv++;
+ int UV = -U - V;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
int R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1672,13 +1746,22 @@ void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1686,13 +1769,22 @@ void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1700,13 +1792,22 @@ void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1715,44 +1816,40 @@ void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
#else
// tres tres lent ....
- unsigned int i=0,j=0;
+ unsigned int i = 0, j = 0;
unsigned char r, g, b;
- while( j < size*3/2)
- {
- YUVToRGB (yuv[j+1], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+2], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+4], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
-
- YUVToRGB (yuv[j+5], yuv[j], yuv[j+3], r, g, b);
- rgb[i] = r;
- rgb[i+1] = g;
- rgb[i+2] = b;
- i+=3;
- //TRACE("r= %d g=%d b=%d", r, g, b);
-
- j+=6;
+ while (j < size * 3 / 2) {
+ YUVToRGB(yuv[j + 1], yuv[j], yuv[j + 3], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+
+ YUVToRGB(yuv[j + 2], yuv[j], yuv[j + 3], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+
+ YUVToRGB(yuv[j + 4], yuv[j], yuv[j + 3], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+
+ YUVToRGB(yuv[j + 5], yuv[j], yuv[j + 3], r, g, b);
+ rgb[i] = r;
+ rgb[i + 1] = g;
+ rgb[i + 2] = b;
+ i += 3;
+ // TRACE("r= %d g=%d b=%d", r, g, b);
+
+ j += 6;
}
#endif
-
}
-
-
/*!
Convert YUV420 [Y(NxM), U(N/2xM/2), V(N/2xM/2)] image into RGBa image.
@@ -1760,43 +1857,49 @@ void vpImageConvert::YUV411ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
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;
int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iU = yuv + size;
- unsigned char* iV = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
+ unsigned int size = width * height;
+ unsigned char *iU = yuv + size;
+ unsigned char *iV = yuv + 5 * size / 4;
+ for (unsigned int i = 0; i < height / 2; i++) {
+ for (unsigned int j = 0; j < width / 2; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv + width - 1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv - width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1805,29 +1908,47 @@ void vpImageConvert::YUV420ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba + 4*width-7;
+ rgba = rgba + 4 * width - 7;
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -1836,22 +1957,31 @@ void vpImageConvert::YUV420ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba -4*width+1;
+ rgba = rgba - 4 * width + 1;
}
- yuv+=width;
- rgba+=4*width;
+ yuv += width;
+ rgba += 4 * width;
}
}
/*!
@@ -1859,44 +1989,49 @@ 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;
int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iU = yuv + size;
- unsigned char* iV = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
+ unsigned int size = width * height;
+ unsigned char *iU = yuv + size;
+ unsigned char *iV = yuv + 5 * size / 4;
+ for (unsigned int i = 0; i < height / 2; i++) {
+ for (unsigned int j = 0; j < width / 2; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv + width - 1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv - width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1904,28 +2039,46 @@ void vpImageConvert::YUV420ToRGB(unsigned char* yuv,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb + 3*width-5;
+ rgb = rgb + 3 * width - 5;
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -1933,21 +2086,30 @@ void vpImageConvert::YUV420ToRGB(unsigned char* yuv,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb -3*width+1;
+ rgb = rgb - 3 * width + 1;
}
- yuv+=width;
- rgb+=3*width;
+ yuv += width;
+ rgb += 3 * width;
}
}
@@ -1956,10 +2118,9 @@ void vpImageConvert::YUV420ToRGB(unsigned char* yuv,
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++)
- {
+ for (unsigned int i = 0; i < size; i++) {
*grey++ = *yuv++;
}
}
@@ -1970,29 +2131,37 @@ void vpImageConvert::YUV420ToGrey(unsigned char* yuv, unsigned char* grey, unsig
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);
- int U5 = 5*U;
- int Y = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int UV = - U - V;
+ for (unsigned int i = 0; i < size; i++) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int UV = -U - V;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
int R = Y + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2005,29 +2174,37 @@ void vpImageConvert::YUV444ToRGBa(unsigned char* yuv, unsigned char* rgba, unsig
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);
- int U5 = 5*U;
- int Y = *yuv++;
- int V = (int)((*yuv++ - 128) * 0.707);
- int V2 = 2*V;
- int UV = - U - V;
+ for (unsigned int i = 0; i < size; i++) {
+ int U = (int)((*yuv++ - 128) * 0.354);
+ int U5 = 5 * U;
+ int Y = *yuv++;
+ int V = (int)((*yuv++ - 128) * 0.707);
+ int V2 = 2 * V;
+ int UV = -U - V;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
int R = Y + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
int G = Y + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
int B = Y + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2040,11 +2217,10 @@ void vpImageConvert::YUV444ToRGB(unsigned char* yuv, unsigned char* rgb, unsigne
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++)
- {
+ for (unsigned int i = 0; i < size; i++) {
*grey++ = *yuv;
yuv = yuv + 3;
}
@@ -2057,43 +2233,49 @@ void vpImageConvert::YUV444ToGrey(unsigned char* yuv, unsigned char* grey, unsig
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;
int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
+ unsigned int size = width * height;
+ unsigned char *iV = yuv + size;
+ unsigned char *iU = yuv + 5 * size / 4;
+ for (unsigned int i = 0; i < height / 2; i++) {
+ for (unsigned int j = 0; j < width / 2; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv + width - 1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv - width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2102,29 +2284,47 @@ void vpImageConvert::YV12ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = 0;
- rgba = rgba + 4*width-7;
+ rgba = rgba + 4 * width - 7;
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2133,22 +2333,31 @@ void vpImageConvert::YV12ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba -4*width+1;
+ rgba = rgba - 4 * width + 1;
}
- yuv+=width;
- rgba+=4*width;
+ yuv += width;
+ rgba += 4 * width;
}
}
/*!
@@ -2156,43 +2365,49 @@ 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;
int Y0, Y1, Y2, Y3;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 5*size/4;
- for(unsigned int i = 0; i<height/2; i++)
- {
- for(unsigned int j = 0; j < width/2 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv;
- yuv = yuv+width-1;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv-width+1;
+ unsigned int size = width * height;
+ unsigned char *iV = yuv + size;
+ unsigned char *iU = yuv + 5 * size / 4;
+ for (unsigned int i = 0; i < height / 2; i++) {
+ for (unsigned int j = 0; j < width / 2; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv;
+ yuv = yuv + width - 1;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv - width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2200,28 +2415,46 @@ void vpImageConvert::YV12ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb + 3*width-5;
+ rgb = rgb + 3 * width - 5;
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2229,21 +2462,30 @@ void vpImageConvert::YV12ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb -3*width+1;
+ rgb = rgb - 3 * width + 1;
}
- yuv+=width;
- rgb+=3*width;
+ yuv += width;
+ rgb += 3 * width;
}
}
@@ -2254,57 +2496,63 @@ 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;
- int Y0, Y1, Y2, Y3,Y4, Y5, Y6, Y7,Y8, Y9, Y10, Y11,Y12, Y13, Y14, Y15;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 17*size/16;
- for(unsigned int i = 0; i<height/4; i++)
- {
- for(unsigned int j = 0; j < width/4 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv++;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv+width-3;
- Y4 = *yuv++;
- Y5 = *yuv++;
- Y6 = *yuv++;
- Y7 = *yuv;
- yuv = yuv+width-3;
- Y8 = *yuv++;
- Y9 = *yuv++;
- Y10 = *yuv++;
- Y11 = *yuv;
- yuv = yuv+width-3;
- Y12 = *yuv++;
- Y13 = *yuv++;
- Y14 = *yuv++;
- Y15 = *yuv;
- yuv = yuv-3*width+1;
+ int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15;
+ unsigned int size = width * height;
+ unsigned char *iV = yuv + size;
+ unsigned char *iU = yuv + 17 * size / 16;
+ for (unsigned int i = 0; i < height / 4; i++) {
+ for (unsigned int j = 0; j < width / 4; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv + width - 3;
+ Y4 = *yuv++;
+ Y5 = *yuv++;
+ Y6 = *yuv++;
+ Y7 = *yuv;
+ yuv = yuv + width - 3;
+ Y8 = *yuv++;
+ Y9 = *yuv++;
+ Y10 = *yuv++;
+ Y11 = *yuv;
+ yuv = yuv + width - 3;
+ Y12 = *yuv++;
+ Y13 = *yuv++;
+ Y14 = *yuv++;
+ Y15 = *yuv;
+ yuv = yuv - 3 * width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2313,13 +2561,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2328,13 +2585,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2343,28 +2609,46 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba + 4*width-15;
+ rgba = rgba + 4 * width - 15;
R = Y4 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y4 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y4 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2373,13 +2657,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y5 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y5 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y5 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2388,13 +2681,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y6 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y6 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y6 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2403,28 +2705,46 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y7 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y7 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y7 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba + 4*width-15;
+ rgba = rgba + 4 * width - 15;
R = Y8 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y8 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y8 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2433,13 +2753,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y9 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y9 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y9 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2448,13 +2777,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y10 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y10 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y10 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2463,28 +2801,46 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y11 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y11 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y11 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba + 4*width-15;
+ rgba = rgba + 4 * width - 15;
R = Y12 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y12 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y12 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2493,13 +2849,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y13 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y13 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y13 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2508,13 +2873,22 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y14 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y14 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y14 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
@@ -2523,22 +2897,31 @@ void vpImageConvert::YVU9ToRGBa(unsigned char* yuv, unsigned char* rgba,
//---
R = Y15 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y15 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y15 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgba++ = (unsigned char)R;
*rgba++ = (unsigned char)G;
*rgba++ = (unsigned char)B;
*rgba = vpRGBa::alpha_default;
- rgba = rgba -12*width+1;
+ rgba = rgba - 12 * width + 1;
}
- yuv+=3*width;
- rgba+=12*width;
+ yuv += 3 * width;
+ rgba += 12 * width;
}
}
/*!
@@ -2546,57 +2929,63 @@ 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;
int Y0, Y1, Y2, Y3, Y4, Y5, Y6, Y7, Y8, Y9, Y10, Y11, Y12, Y13, Y14, Y15;
- unsigned int size = width*height;
- unsigned char* iV = yuv + size;
- unsigned char* iU = yuv + 17*size/16;
- for(unsigned int i = 0; i<height/4; i++)
- {
- for(unsigned int j = 0; j < width/4 ; j++)
- {
- U = (int)((*iU++ - 128) * 0.354);
- U5 = 5*U;
- V = (int)((*iV++ - 128) * 0.707);
- V2 = 2*V;
- UV = - U - V;
- Y0 = *yuv++;
- Y1 = *yuv++;
- Y2 = *yuv++;
- Y3 = *yuv;
- yuv = yuv+width-3;
- Y4 = *yuv++;
- Y5 = *yuv++;
- Y6 = *yuv++;
- Y7 = *yuv;
- yuv = yuv+width-3;
- Y8 = *yuv++;
- Y9 = *yuv++;
- Y10 = *yuv++;
- Y11 = *yuv;
- yuv = yuv+width-3;
- Y12 = *yuv++;
- Y13 = *yuv++;
- Y14 = *yuv++;
- Y15 = *yuv;
- yuv = yuv-3*width+1;
+ unsigned int size = width * height;
+ unsigned char *iV = yuv + size;
+ unsigned char *iU = yuv + 17 * size / 16;
+ for (unsigned int i = 0; i < height / 4; i++) {
+ for (unsigned int j = 0; j < width / 4; j++) {
+ U = (int)((*iU++ - 128) * 0.354);
+ U5 = 5 * U;
+ V = (int)((*iV++ - 128) * 0.707);
+ V2 = 2 * V;
+ UV = -U - V;
+ Y0 = *yuv++;
+ Y1 = *yuv++;
+ Y2 = *yuv++;
+ Y3 = *yuv;
+ yuv = yuv + width - 3;
+ Y4 = *yuv++;
+ Y5 = *yuv++;
+ Y6 = *yuv++;
+ Y7 = *yuv;
+ yuv = yuv + width - 3;
+ Y8 = *yuv++;
+ Y9 = *yuv++;
+ Y10 = *yuv++;
+ Y11 = *yuv;
+ yuv = yuv + width - 3;
+ Y12 = *yuv++;
+ Y13 = *yuv++;
+ Y14 = *yuv++;
+ Y15 = *yuv;
+ yuv = yuv - 3 * width + 1;
// Original equations
// R = Y + 1.402 V
// G = Y - 0.344 U - 0.714 V
// B = Y + 1.772 U
R = Y0 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y0 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y0 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2604,13 +2993,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y1 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y1 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y1 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2618,13 +3016,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y2 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y2 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y2 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2632,27 +3039,45 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y3 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y3 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y3 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
+ rgb = rgb + 3 * width - 11;
R = Y4 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y4 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y4 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2660,13 +3085,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y5 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y5 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y5 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2674,13 +3108,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y6 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y6 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y6 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2688,27 +3131,45 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y7 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y7 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y7 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
+ rgb = rgb + 3 * width - 11;
R = Y8 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y8 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y8 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2716,13 +3177,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y9 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y9 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y9 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2730,13 +3200,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y10 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y10 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y10 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2744,27 +3223,45 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y11 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y11 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y11 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb = (unsigned char)B;
- rgb = rgb + 3*width-11;
+ rgb = rgb + 3 * width - 11;
R = Y12 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y12 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y12 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2772,13 +3269,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y13 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y13 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y13 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2786,13 +3292,22 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y14 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y14 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y14 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
@@ -2800,21 +3315,30 @@ void vpImageConvert::YVU9ToRGB(unsigned char* yuv, unsigned char* rgb,
//---
R = Y15 + V2;
- if ((R >> 8) > 0) R = 255; else if (R < 0) R = 0;
+ if ((R >> 8) > 0)
+ R = 255;
+ else if (R < 0)
+ R = 0;
G = Y15 + UV;
- if ((G >> 8) > 0) G = 255; else if (G < 0) G = 0;
+ if ((G >> 8) > 0)
+ G = 255;
+ else if (G < 0)
+ G = 0;
B = Y15 + U5;
- if ((B >> 8) > 0) B = 255; else if (B < 0) B = 0;
+ if ((B >> 8) > 0)
+ B = 255;
+ else if (B < 0)
+ B = 0;
*rgb++ = (unsigned char)R;
*rgb++ = (unsigned char)G;
*rgb++ = (unsigned char)B;
- rgb = rgb -9*width+1;
+ rgb = rgb - 9 * width + 1;
}
- yuv+=3*width;
- rgb+=9*width;
+ yuv += 3 * width;
+ rgb += 9 * width;
}
}
@@ -2825,16 +3349,16 @@ 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;
+ unsigned char *pt_end = rgb + 3 * size;
unsigned char *pt_output = rgba;
- while(pt_input != pt_end) {
- *(pt_output++) = *(pt_input++) ; // R
- *(pt_output++) = *(pt_input++) ; // G
- *(pt_output++) = *(pt_input++) ; // B
+ while (pt_input != pt_end) {
+ *(pt_output++) = *(pt_input++); // R
+ *(pt_output++) = *(pt_input++); // G
+ *(pt_output++) = *(pt_input++); // B
*(pt_output++) = vpRGBa::alpha_default; // A
}
}
@@ -2846,17 +3370,17 @@ void vpImageConvert::RGBToRGBa(unsigned char* rgb, unsigned char* rgba, unsigned
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;
+ unsigned char *pt_end = rgba + 4 * size;
unsigned char *pt_output = rgb;
- while(pt_input != pt_end) {
- *(pt_output++) = *(pt_input++) ; // R
- *(pt_output++) = *(pt_input++) ; // G
- *(pt_output++) = *(pt_input++) ; // B
- pt_input++ ;
+ while (pt_input != pt_end) {
+ *(pt_output++) = *(pt_input++); // R
+ *(pt_output++) = *(pt_input++); // G
+ *(pt_output++) = *(pt_input++); // B
+ pt_input++;
}
}
/*!
@@ -2865,132 +3389,94 @@ void vpImageConvert::RGBaToRGB(unsigned char* rgba, unsigned char* rgb, unsigned
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
+ checkSSSE3 = false;
+#endif
+
+ if (checkSSSE3) {
#if VISP_HAVE_SSSE3
- unsigned int i = 0;
+ unsigned int i = 0;
+
+ 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ for (; i <= size - 16; i += 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)));
- 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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- for(; i <= size - 16; i+=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)) );
-
- rgb += 48;
- grey += 16;
+ _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;
+ }
}
- }
- for(; i < size; i++) {
- *grey = (unsigned char) (0.2126 * (*rgb)
- + 0.7152 * (*(rgb + 1))
- + 0.0722 * (*(rgb + 2)) );
+ for (; i < size; i++) {
+ *grey = (unsigned char)(0.2126 * (*rgb) + 0.7152 * (*(rgb + 1)) + 0.0722 * (*(rgb + 2)));
- rgb += 3;
- ++grey;
- }
-#else
- unsigned char *pt_input = rgb;
- unsigned char* pt_end = rgb + size*3;
- unsigned char *pt_output = grey;
-
- while(pt_input != pt_end) {
- *pt_output = (unsigned char) (0.2126 * (*pt_input)
- + 0.7152 * (*(pt_input + 1))
- + 0.0722 * (*(pt_input + 2)) );
- pt_input += 3;
- pt_output ++;
- }
+ rgb += 3;
+ ++grey;
+ }
#endif
+ } else {
+ unsigned char *pt_input = rgb;
+ unsigned char *pt_end = rgb + size * 3;
+ unsigned char *pt_output = grey;
+
+ while (pt_input != pt_end) {
+ *pt_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2)));
+ pt_input += 3;
+ pt_output++;
+ }
+ }
}
/*!
@@ -2999,117 +3485,91 @@ void vpImageConvert::RGBToGrey(unsigned char* rgb, unsigned char* grey, unsigned
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
+ checkSSSE3 = false;
+#endif
+
+ if (checkSSSE3) {
#if VISP_HAVE_SSSE3
- unsigned int i = 0;
+ unsigned int i = 0;
+
+ 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ 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)));
+
+ // 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));
- 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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- 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)
- ));
-
- //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)) );
-
- rgba += 64;
- grey += 16;
+ 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;
+ }
}
- }
- for(; i < size; i++) {
- *grey = (unsigned char) (0.2126 * (*rgba)
- + 0.7152 * (*(rgba + 1))
- + 0.0722 * (*(rgba + 2)) );
+ for (; i < size; i++) {
+ *grey = (unsigned char)(0.2126 * (*rgba) + 0.7152 * (*(rgba + 1)) + 0.0722 * (*(rgba + 2)));
- rgba += 4;
- ++grey;
- }
-#else
- unsigned char *pt_input = rgba;
- unsigned char* pt_end = rgba + size*4;
- unsigned char *pt_output = grey;
-
- while(pt_input != pt_end) {
- *pt_output = (unsigned char) (0.2126 * (*pt_input)
- + 0.7152 * (*(pt_input + 1))
- + 0.0722 * (*(pt_input + 2)) );
- pt_input += 4;
- pt_output ++;
- }
+ rgba += 4;
+ ++grey;
+ }
#endif
+ } else {
+ unsigned char *pt_input = rgba;
+ unsigned char *pt_end = rgba + size * 4;
+ unsigned char *pt_output = grey;
+
+ while (pt_input != pt_end) {
+ *pt_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2)));
+ pt_input += 4;
+ pt_output++;
+ }
+ }
}
/*!
@@ -3117,21 +3577,20 @@ void vpImageConvert::RGBaToGrey(unsigned char* rgba, unsigned char* grey, unsign
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;
unsigned char *pt_output = rgba;
- while(pt_input != pt_end) {
- unsigned char p = *pt_input ;
- *(pt_output ) = p ; // R
- *(pt_output + 1) = p ; // G
- *(pt_output + 2) = p ; // B
- *(pt_output + 3) = vpRGBa::alpha_default ; // A
+ while (pt_input != pt_end) {
+ unsigned char p = *pt_input;
+ *(pt_output) = p; // R
+ *(pt_output + 1) = p; // G
+ *(pt_output + 2) = p; // B
+ *(pt_output + 3) = vpRGBa::alpha_default; // A
- pt_input ++;
+ pt_input++;
pt_output += 4;
}
}
@@ -3140,56 +3599,52 @@ vpImageConvert::GreyToRGBa(unsigned char* grey, unsigned char* rgba, unsigned in
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;
+ unsigned char *pt_end = grey + size;
unsigned char *pt_output = rgb;
- while(pt_input != pt_end) {
- unsigned char p = *pt_input ;
- *(pt_output ) = p ; // R
- *(pt_output + 1) = p ; // G
- *(pt_output + 2) = p ; // B
+ while (pt_input != pt_end) {
+ unsigned char p = *pt_input;
+ *(pt_output) = p; // R
+ *(pt_output + 1) = p; // G
+ *(pt_output + 2) = p; // B
- pt_input ++;
+ pt_input++;
pt_output += 3;
}
}
-
/*!
- Converts a BGR image to RGBa. The alpha component is set to vpRGBa::alpha_default.
+ Converts a BGR image to RGBa. The alpha component is set to
+ vpRGBa::alpha_default.
Flips the image verticaly if needed.
Assumes that rgba is already resized.
*/
-void
-vpImageConvert::BGRToRGBa(unsigned char * bgr, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip)
+void vpImageConvert::BGRToRGBa(unsigned char *bgr, unsigned char *rgba, unsigned int width, unsigned int height,
+ bool flip)
{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+ // if we have to flip the image, we start from the end last scanline so the
+ // step is negative
+ int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3);
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? (bgr+(width*height*3)+lineStep) : bgr;
+ // starting source address = last line if we need to flip the image
+ unsigned char *src = (flip) ? (bgr + (width * height * 3) + lineStep) : bgr;
- for(unsigned int i=0 ; i < height ; i++)
- {
+ for (unsigned int i = 0; i < height; i++) {
unsigned char *line = src;
- for(unsigned int j=0 ; j < width ; j++)
- {
- *rgba++ = *(line+2);
- *rgba++ = *(line+1);
- *rgba++ = *(line+0);
+ for (unsigned int j = 0; j < width; j++) {
+ *rgba++ = *(line + 2);
+ *rgba++ = *(line + 1);
+ *rgba++ = *(line + 0);
*rgba++ = vpRGBa::alpha_default;
- line+=3;
+ line += 3;
}
- //go to the next line
- src+=lineStep;
+ // go to the next line
+ src += lineStep;
}
}
@@ -3198,270 +3653,222 @@ 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, bool flip)
+void vpImageConvert::BGRToGrey(unsigned char *bgr, unsigned char *grey, unsigned int width, unsigned int height,
+ bool flip)
{
-#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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- //Coefficients RGB to Gray
- // const __m128i coeff_R = _mm_set_epi8(
- // 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1
- // );
- // const __m128i coeff_G = _mm_set_epi8(
- // 183, -1, 183, -1, 183, -1, 183, -1, 183, -1, 183, -1, 183, -1, 183, -1
- // );
- // const __m128i coeff_B = _mm_set_epi8(
- // 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1
- // );
- // const __m128i coeff_R = _mm_set_epi16(
- // 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2
- // );
- // const __m128i coeff_G = _mm_set_epi16(
- // 23434*2, 23434*2, 23434*2, 23434*2, 23434*2, 23434*2, 23434*2, 23434*2
- // );
- // 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
- );
-
- if(flip) {
- int i = ((int) height) - 1;
- int lineStep = -(int) (width*3);
- bgr = bgr + (width * (height-1) * 3);
-
- unsigned char *linePtr = bgr;
- unsigned char r,g,b;
-
- if(width >= 16) {
- for(; i >= 0; i--) {
- unsigned int j = 0;
-
- 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)) );
+ bool checkSSSE3 = vpCPUFeatures::checkSSSE3();
+#if !VISP_HAVE_SSSE3
+ checkSSSE3 = false;
+#endif
- bgr += 48;
- grey += 16;
+ 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // Coefficients RGB to Gray
+ // const __m128i coeff_R = _mm_set_epi8(
+ // 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1, 54, -1
+ // );
+ // const __m128i coeff_G = _mm_set_epi8(
+ // 183, -1, 183, -1, 183, -1, 183, -1, 183, -1, 183, -1, 183, -1,
+ // 183, -1
+ // );
+ // const __m128i coeff_B = _mm_set_epi8(
+ // 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1, 18, -1
+ // );
+ // const __m128i coeff_R = _mm_set_epi16(
+ // 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2, 6969*2
+ // );
+ // const __m128i coeff_G = _mm_set_epi16(
+ // 23434*2, 23434*2, 23434*2, 23434*2, 23434*2, 23434*2, 23434*2,
+ // 23434*2
+ // );
+ // 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);
+
+ if (flip) {
+ int i = ((int)height) - 1;
+ int lineStep = -(int)(width * 3);
+ bgr = bgr + (width * (height - 1) * 3);
+
+ unsigned char *linePtr = bgr;
+ unsigned char r, g, b;
+
+ if (width >= 16) {
+ for (; i >= 0; i--) {
+ unsigned int j = 0;
+
+ 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)));
+
+ bgr += 48;
+ grey += 16;
+ }
+
+ for (; j < width; j++) {
+ b = *(bgr++);
+ g = *(bgr++);
+ r = *(bgr++);
+ *grey++ = (unsigned char)(0.2126 * r + 0.7152 * g + 0.0722 * b);
+ }
+
+ linePtr += lineStep;
+ bgr = linePtr;
}
+ }
- for(; j < width; j++) {
+ for (; i >= 0; i--) {
+ for (unsigned int j = 0; j < width; j++) {
b = *(bgr++);
g = *(bgr++);
r = *(bgr++);
- *grey++ = (unsigned char) ( 0.2126 * r + 0.7152 * g + 0.0722 * b);
+ *grey++ = (unsigned char)(0.2126 * r + 0.7152 * g + 0.0722 * b);
}
linePtr += lineStep;
bgr = linePtr;
}
- }
-
- for(; i >= 0; i--) {
- for(unsigned int j = 0; j < width; j++) {
- b = *(bgr++);
- g = *(bgr++);
- r = *(bgr++);
- *grey++ = (unsigned char) ( 0.2126 * r + 0.7152 * g + 0.0722 * b);
- }
-
- linePtr += lineStep;
- bgr = linePtr;
- }
- } else {
- unsigned int i = 0;
- unsigned int size = width * height;
+ } else {
+ unsigned int i = 0;
+ unsigned int size = width * height;
- if(size >= 16) {
- for(; i <= size - 16; i+=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));
+ if (size >= 16) {
+ for (; i <= size - 16; i += 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 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 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 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)
- ));
+ 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)) );
+ _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;
+ bgr += 48;
+ grey += 16;
+ }
}
- }
- for(; i < size; i++) {
- *grey = (unsigned char) (0.2126 * (*(bgr + 2))
- + 0.7152 * (*(bgr + 1))
- + 0.0722 * (*bgr) );
+ for (; i < size; i++) {
+ *grey = (unsigned char)(0.2126 * (*(bgr + 2)) + 0.7152 * (*(bgr + 1)) + 0.0722 * (*bgr));
- bgr += 3;
- ++grey;
+ bgr += 3;
+ ++grey;
+ }
}
- }
-#else
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? bgr+(width*height*3)+lineStep : bgr;
+#endif
+ } else {
+ // if we have to flip the image, we start from the end last scanline so
+ // the step is negative
+ int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3);
+
+ // starting source address = last line if we need to flip the image
+ unsigned char *src = (flip) ? bgr + (width * height * 3) + lineStep : bgr;
+
+ 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));
+ line += 3;
+ }
- 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)) ;
- line+=3;
+ // go to the next line
+ src += lineStep;
}
-
- //go to the next line
- src+=lineStep;
}
-#endif
}
/*!
- Converts a RGB image to RGBa. Alpha component is set to vpRGBa::alpha_default.
+ Converts a RGB image to RGBa. Alpha component is set to
+ vpRGBa::alpha_default.
Flips the image verticaly if needed.
Assumes that rgba is already resized.
*/
-void
-vpImageConvert::RGBToRGBa(unsigned char * rgb, unsigned char * rgba,
- unsigned int width, unsigned int height, bool flip)
+void vpImageConvert::RGBToRGBa(unsigned char *rgb, unsigned char *rgba, unsigned int width, unsigned int height,
+ bool flip)
{
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+ // if we have to flip the image, we start from the end last scanline so the
+ // step is negative
+ int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3);
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? (rgb+(width*height*3)+lineStep) : rgb;
+ // starting source address = last line if we need to flip the image
+ unsigned char *src = (flip) ? (rgb + (width * height * 3) + lineStep) : rgb;
- unsigned int j=0;
- unsigned int i=0;
+ unsigned int j = 0;
+ unsigned int i = 0;
- for(i=0 ; i < height ; i++)
- {
- unsigned char * line = src;
- for( j=0 ; j < width ; j++)
- {
+ for (i = 0; i < height; i++) {
+ unsigned char *line = src;
+ for (j = 0; j < width; j++) {
*rgba++ = *(line++);
*rgba++ = *(line++);
*rgba++ = *(line++);
*rgba++ = vpRGBa::alpha_default;
}
- //go to the next line
- src+=lineStep;
+ // go to the next line
+ src += lineStep;
}
}
@@ -3470,201 +3877,161 @@ 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, bool flip)
+void vpImageConvert::RGBToGrey(unsigned char *rgb, unsigned char *grey, unsigned int width, unsigned int height,
+ bool flip)
{
- if(flip) {
-#if VISP_HAVE_SSSE3
- int i = ((int) height) - 1;
- int lineStep = -(int) (width*3);
- rgb = rgb + (width * (height-1) * 3);
-
- unsigned char *linePtr = rgb;
- unsigned char r,g,b;
-
- 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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- //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
- );
-
- for(; i >= 0; i--) {
- unsigned int j = 0;
-
- 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)) );
+ if (flip) {
+ bool checkSSSE3 = vpCPUFeatures::checkSSSE3();
+#if !VISP_HAVE_SSSE3
+ checkSSSE3 = false;
+#endif
- rgb += 48;
- grey += 16;
+ if (checkSSSE3) {
+#if VISP_HAVE_SSSE3
+ int i = ((int)height) - 1;
+ int lineStep = -(int)(width * 3);
+ rgb = rgb + (width * (height - 1) * 3);
+
+ unsigned char *linePtr = rgb;
+ unsigned char r, g, b;
+
+ 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ // 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);
+
+ for (; i >= 0; i--) {
+ unsigned int j = 0;
+
+ 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)));
+
+ rgb += 48;
+ grey += 16;
+ }
+
+ for (; j < width; j++) {
+ r = *(rgb++);
+ g = *(rgb++);
+ b = *(rgb++);
+ *grey++ = (unsigned char)(0.2126 * r + 0.7152 * g + 0.0722 * b);
+ }
+
+ linePtr += lineStep;
+ rgb = linePtr;
}
+ }
- for(; j < width; j++) {
+ for (; i >= 0; i--) {
+ for (unsigned int j = 0; j < width; j++) {
r = *(rgb++);
g = *(rgb++);
b = *(rgb++);
- *grey++ = (unsigned char) ( 0.2126 * r + 0.7152 * g + 0.0722 * b);
+ *grey++ = (unsigned char)(0.2126 * r + 0.7152 * g + 0.0722 * b);
}
linePtr += lineStep;
rgb = linePtr;
}
- }
+#endif
+ } else {
+ // if we have to flip the image, we start from the end last scanline so
+ // the step is negative
+ int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3);
- for(; i >= 0; i--) {
- for(unsigned int j = 0; j < width; j++) {
- r = *(rgb++);
- g = *(rgb++);
- b = *(rgb++);
- *grey++ = (unsigned char) ( 0.2126 * r + 0.7152 * g + 0.0722 * b);
- }
+ // starting source address = last line if we need to flip the image
+ unsigned char *src = (flip) ? rgb + (width * height * 3) + lineStep : rgb;
- linePtr += lineStep;
- rgb = linePtr;
- }
-#else
- //if we have to flip the image, we start from the end last scanline so the
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
-
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? rgb+(width*height*3)+lineStep : rgb;
-
- unsigned int j=0;
- unsigned int i=0;
-
- unsigned r,g,b;
-
- for(i=0 ; i < height ; i++)
- {
- unsigned char * line = src;
- for( j=0 ; j < width ; j++)
- {
- r = *(line++);
- g = *(line++);
- b = *(line++);
- *grey++ = (unsigned char)( 0.2126 * r + 0.7152 * g + 0.0722 * b) ;
- }
+ unsigned int j = 0;
+ unsigned int i = 0;
+
+ unsigned r, g, b;
- //go to the next line
- src+=lineStep;
+ for (i = 0; i < height; i++) {
+ unsigned char *line = src;
+ for (j = 0; j < width; j++) {
+ r = *(line++);
+ g = *(line++);
+ b = *(line++);
+ *grey++ = (unsigned char)(0.2126 * r + 0.7152 * g + 0.0722 * b);
+ }
+
+ // go to the next line
+ src += lineStep;
+ }
}
-#endif
} else {
- RGBToGrey(rgb, grey, width*height);
+ RGBToGrey(rgb, grey, width * height);
}
}
/*!
-
Compute the look up table useful for YCbCr conversions.
-
*/
void vpImageConvert::computeYCbCrLUT()
{
if (YCbCrLUTcomputed == false) {
int index = 256;
- while (index-- ) {
+ while (index--) {
int aux = index - 128;
- vpImageConvert::vpCrr[index] = (int)( 364.6610 * aux) >> 8;
- vpImageConvert::vpCgb[index] = (int)( -89.8779 * aux) >> 8;
+ vpImageConvert::vpCrr[index] = (int)(364.6610 * aux) >> 8;
+ vpImageConvert::vpCgb[index] = (int)(-89.8779 * aux) >> 8;
vpImageConvert::vpCgr[index] = (int)(-185.8154 * aux) >> 8;
- vpImageConvert::vpCbb[index] = (int)( 460.5724 * aux) >> 8;
+ vpImageConvert::vpCbb[index] = (int)(460.5724 * aux) >> 8;
}
YCbCrLUTcomputed = true;
}
}
-
/*!
Convert an image from YCbCr 4:2:2 (Y0 Cb01 Y1 Cr01 Y2 Cb23 Y3 ...) to RGB
@@ -3710,12 +4077,9 @@ void vpImageConvert::YCbCrToRGB(unsigned char *ycbcr, unsigned char *rgb, unsign
vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
- *pt_rgb++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgb++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgb++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (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;
}
@@ -3770,19 +4134,15 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi
vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
- *pt_rgba++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgba++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgba++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (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;
}
}
-
/*!
Convert an image from YCrCb 4:2:2 (Y0 Cr01 Y1 Cb01 Y2 Cr23 Y3 ...) to grey
@@ -3799,15 +4159,14 @@ void vpImageConvert::YCbCrToRGBa(unsigned char *ycbcr, unsigned char *rgba, unsi
- 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;
+ unsigned int i = 0, j = 0;
- while( j < size*2)
- {
+ while (j < size * 2) {
grey[i++] = yuv[j];
- grey[i++] = yuv[j+2];
- j+=4;
+ grey[i++] = yuv[j + 2];
+ j += 4;
}
}
@@ -3855,12 +4214,9 @@ void vpImageConvert::YCrCbToRGB(unsigned char *ycrcb, unsigned char *rgb, unsign
vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
- *pt_rgb++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgb++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgb++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgb++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (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;
}
@@ -3913,12 +4269,9 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi
vpDEBUG_TRACE(5, "[%d] R: %d G: %d B: %d\n", size, val_r, val_g, val_b);
- *pt_rgba++ = (val_r < 0) ? 0u :
- ((val_r > 255) ? 255u : (unsigned char)val_r); // Red component.
- *pt_rgba++ = (val_g < 0) ? 0u :
- ((val_g > 255) ? 255u : (unsigned char)val_g); // Green component.
- *pt_rgba++ = (val_b < 0) ? 0u :
- ((val_b > 255) ? 255u : (unsigned char)val_b); // Blue component.
+ *pt_rgba++ = (val_r < 0) ? 0u : ((val_r > 255) ? 255u : (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;
@@ -3938,8 +4291,8 @@ void vpImageConvert::YCrCbToRGBa(unsigned char *ycrcb, unsigned char *rgba, unsi
\code
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -3960,19 +4313,16 @@ int main()
}
\endcode
*/
-void vpImageConvert::split(const vpImage<vpRGBa> &src,
- vpImage<unsigned char>* pR,
- vpImage<unsigned char>* pG,
- vpImage<unsigned char>* pB,
- vpImage<unsigned char>* pa)
+void vpImageConvert::split(const vpImage<vpRGBa> &src, vpImage<unsigned char> *pR, vpImage<unsigned char> *pG,
+ vpImage<unsigned char> *pB, vpImage<unsigned char> *pa)
{
size_t n = src.getNumberOfPixel();
unsigned int height = src.getHeight();
- unsigned int width = src.getWidth();
- unsigned char* input;
- unsigned char* dst ;
+ unsigned int width = src.getWidth();
+ unsigned char *input;
+ unsigned char *dst;
- vpImage<unsigned char>* tabChannel[4];
+ vpImage<unsigned char> *tabChannel[4];
/* incrsrc[0] = 0; //init
incrsrc[1] = 0; //step after the first used channel
@@ -3985,31 +4335,40 @@ void vpImageConvert::split(const vpImage<vpRGBa> &src,
tabChannel[2] = pB;
tabChannel[3] = pa;
- size_t i; /* ordre */
- for(unsigned int j = 0;j < 4;j++){
- if(tabChannel[j]!=NULL){
- if(tabChannel[j]->getHeight() != height ||
- tabChannel[j]->getWidth() != width){
- tabChannel[j]->resize(height,width);
+ size_t i; /* ordre */
+ for (unsigned int j = 0; j < 4; j++) {
+ if (tabChannel[j] != NULL) {
+ if (tabChannel[j]->getHeight() != height || tabChannel[j]->getWidth() != width) {
+ tabChannel[j]->resize(height, width);
}
- dst = (unsigned char*)tabChannel[j]->bitmap;
+ dst = (unsigned char *)tabChannel[j]->bitmap;
- input = (unsigned char*)src.bitmap+j;
+ input = (unsigned char *)src.bitmap + j;
i = 0;
-#if 1 //optimization
- if (n >= 4) { /* boucle deroulee lsize fois */
+#if 1 // optimization
+ if (n >= 4) { /* boucle deroulee lsize fois */
n -= 3;
for (; i < n; i += 4) {
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
- *dst = *input; input += 4; dst++;
+ *dst = *input;
+ input += 4;
+ dst++;
+ *dst = *input;
+ input += 4;
+ dst++;
+ *dst = *input;
+ input += 4;
+ dst++;
+ *dst = *input;
+ input += 4;
+ dst++;
}
n += 3;
}
#endif
for (; i < n; i++) {
- *dst = *input; input += 4; dst ++;
+ *dst = *input;
+ input += 4;
+ dst++;
}
}
}
@@ -4023,54 +4382,52 @@ void vpImageConvert::split(const vpImage<vpRGBa> &src,
\param a : Alpha channel.
\param RGBa : Destination RGBa image.
*/
-void vpImageConvert::merge(const vpImage<unsigned char> *R,
- const vpImage<unsigned char> *G,
- const vpImage<unsigned char> *B,
- const vpImage<unsigned char> *a,
- vpImage<vpRGBa> &RGBa) {
- //Check if the input channels have all the same dimensions
+void vpImageConvert::merge(const vpImage<unsigned char> *R, const vpImage<unsigned char> *G,
+ const vpImage<unsigned char> *B, const vpImage<unsigned char> *a, vpImage<vpRGBa> &RGBa)
+{
+ // Check if the input channels have all the same dimensions
std::map<unsigned int, unsigned int> mapOfWidths, mapOfHeights;
- if(R != NULL) {
+ if (R != NULL) {
mapOfWidths[R->getWidth()]++;
mapOfHeights[R->getHeight()]++;
}
- if(G != NULL) {
+ if (G != NULL) {
mapOfWidths[G->getWidth()]++;
mapOfHeights[G->getHeight()]++;
}
- if(B != NULL) {
+ if (B != NULL) {
mapOfWidths[B->getWidth()]++;
mapOfHeights[B->getHeight()]++;
}
- if(a != NULL) {
+ if (a != NULL) {
mapOfWidths[a->getWidth()]++;
mapOfHeights[a->getHeight()]++;
}
- if(mapOfWidths.size() == 1 && mapOfHeights.size() == 1) {
+ if (mapOfWidths.size() == 1 && mapOfHeights.size() == 1) {
unsigned int width = mapOfWidths.begin()->first;
unsigned int height = mapOfHeights.begin()->first;
RGBa.resize(height, width);
- unsigned int size = width*height;
- for(unsigned int i = 0; i < size; i++) {
- if(R != NULL) {
+ unsigned int size = width * height;
+ for (unsigned int i = 0; i < size; i++) {
+ if (R != NULL) {
RGBa.bitmap[i].R = R->bitmap[i];
}
- if(G != NULL) {
+ if (G != NULL) {
RGBa.bitmap[i].G = G->bitmap[i];
}
- if(B != NULL) {
+ if (B != NULL) {
RGBa.bitmap[i].B = B->bitmap[i];
}
- if(a != NULL) {
+ if (a != NULL) {
RGBa.bitmap[i].A = a->bitmap[i];
}
}
@@ -4091,12 +4448,12 @@ void vpImageConvert::merge(const vpImage<unsigned char> *R,
*/
void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, unsigned int size)
{
- int i = (((int)size)<<1)-1;
- int j = (int)size-1;
+ int i = (((int)size) << 1) - 1;
+ int j = (int)size - 1;
while (i >= 0) {
int y = grey16[i--];
- grey[j--] = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
+ grey[j--] = static_cast<unsigned char>((y + (grey16[i--] << 8)) >> 8);
}
}
@@ -4114,12 +4471,12 @@ void vpImageConvert::MONO16ToGrey(unsigned char *grey16, unsigned char *grey, un
*/
void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, unsigned int size)
{
- int i = (((int)size)<<1)-1;
- int j = (int)(size*4-1);
+ int i = (((int)size) << 1) - 1;
+ int j = (int)(size * 4 - 1);
while (i >= 0) {
int y = grey16[i--];
- unsigned char v = static_cast<unsigned char>( (y+(grey16[i--]<<8))>>8 );
+ unsigned char v = static_cast<unsigned char>((y + (grey16[i--] << 8)) >> 8);
rgba[j--] = vpRGBa::alpha_default;
rgba[j--] = v;
rgba[j--] = v;
@@ -4128,8 +4485,9 @@ void vpImageConvert::MONO16ToRGBa(unsigned char *grey16, unsigned char *rgba, un
}
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++) {
+ 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<double>::epsilon())) {
@@ -4144,12 +4502,12 @@ void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, cons
h = 0.0;
}
- double f = h - (int) h;
+ double f = h - (int)h;
double p = v * (1.0 - s);
double q = v * (1.0 - s * f);
double t = v * (1.0 - s * (1.0 - f));
- switch ((int) h) {
+ switch ((int)h) {
case 0:
hue = v;
saturation = t;
@@ -4180,7 +4538,7 @@ void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, cons
value = v;
break;
- default: //case 5:
+ default: // case 5:
hue = v;
saturation = p;
value = q;
@@ -4188,31 +4546,32 @@ void vpImageConvert::HSV2RGB(const double *hue_, const double *saturation_, cons
}
}
- rgb[i*step] = (unsigned char) vpMath::round(hue * 255.0);
- rgb[i*step + 1] = (unsigned char) vpMath::round(saturation * 255.0);
- rgb[i*step + 2] = (unsigned char) vpMath::round(value * 255.0);
+ rgb[i * step] = (unsigned char)vpMath::round(hue * 255.0);
+ rgb[i * step + 1] = (unsigned char)vpMath::round(saturation * 255.0);
+ rgb[i * step + 2] = (unsigned char)vpMath::round(value * 255.0);
if (step == 4) // alpha
- rgb[i*step + 3] = vpRGBa::alpha_default;
+ rgb[i * step + 3] = vpRGBa::alpha_default;
}
}
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++) {
+ const unsigned int size, const unsigned int step)
+{
+ for (unsigned int i = 0; i < size; i++) {
double red, green, blue;
double h, s, v;
double min, max;
- red = rgb[i*step] / 255.0;
- green = rgb[i*step + 1] / 255.0;
- blue = rgb[i*step + 2] / 255.0;
+ red = rgb[i * step] / 255.0;
+ green = rgb[i * step + 1] / 255.0;
+ blue = rgb[i * step + 2] / 255.0;
if (red > green) {
- max = (std::max)(red, blue);
- min = (std::min)(green, blue);
+ max = ((std::max))(red, blue);
+ min = ((std::min))(green, blue);
} else {
- max = (std::max)(green, blue);
- min = (std::min)(red, blue);
+ max = ((std::max))(green, blue);
+ min = ((std::min))(red, blue);
}
v = max;
@@ -4261,11 +4620,13 @@ void vpImageConvert::RGB2HSV(const unsigned char *rgb, double *hue, double *satu
\param hue : Array of hue values (range between [0 - 1]).
\param saturation : Array of saturation values (range between [0 - 1]).
\param value : Array of value values (range between [0 - 1]).
- \param rgba : RGBa array values (with alpha channel set to zero) converted from HSV color space.
- \param size : The total image size or the number of pixels.
+ \param rgba : RGBa array values (with alpha channel set to zero) converted
+ from HSV color space. \param size : The total image size or the number of
+ pixels.
*/
void vpImageConvert::HSVToRGBa(const double *hue, const double *saturation, const double *value, unsigned char *rgba,
- const unsigned int size) {
+ const unsigned int size)
+{
vpImageConvert::HSV2RGB(hue, saturation, value, rgba, size, 4);
}
@@ -4277,15 +4638,17 @@ void vpImageConvert::HSVToRGBa(const double *hue, const double *saturation, cons
\param hue : Array of hue values (range between [0 - 255]).
\param saturation : Array of saturation values (range between [0 - 255]).
\param value : Array of value values (range between [0 - 255]).
- \param rgba : RGBa array values (with alpha channel set to zero) converted from HSV color space.
- \param size : The total image size or the number of pixels.
+ \param rgba : RGBa array values (with alpha channel set to zero) converted
+ from HSV color space. \param size : The total image size or the number of
+ pixels.
*/
void vpImageConvert::HSVToRGBa(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
- unsigned char *rgba, const unsigned int size) {
- for(unsigned int i = 0; i < size; i++) {
+ unsigned char *rgba, const unsigned int size)
+{
+ for (unsigned int i = 0; i < size; i++) {
double h = hue[i] / 255.0, s = saturation[i] / 255.0, v = value[i] / 255.0;
- vpImageConvert::HSVToRGBa(&h, &s, &v, (rgba + i*4), 1);
+ vpImageConvert::HSVToRGBa(&h, &s, &v, (rgba + i * 4), 1);
}
}
@@ -4294,13 +4657,15 @@ void vpImageConvert::HSVToRGBa(const unsigned char *hue, const unsigned char *sa
The alpha channel is not used.
\param rgba : RGBa array values.
- \param hue : Array of hue values converted from RGB color space (range between [0 - 1]).
- \param saturation : Array of saturation values converted from RGB color space (range between [0 - 1]).
- \param value : Array of value values converted from RGB color space (range between [0 - 1]).
- \param size : The total image size or the number of pixels.
+ \param hue : Array of hue values converted from RGB color space (range
+ between [0 - 1]). \param saturation : Array of saturation values converted
+ from RGB color space (range between [0 - 1]). \param value : Array of value
+ values converted from RGB color space (range between [0 - 1]). \param size :
+ The total image size or the number of pixels.
*/
void vpImageConvert::RGBaToHSV(const unsigned char *rgba, double *hue, double *saturation, double *value,
- const unsigned int size) {
+ const unsigned int size)
+{
vpImageConvert::RGB2HSV(rgba, hue, saturation, value, size, 4);
}
@@ -4309,20 +4674,22 @@ void vpImageConvert::RGBaToHSV(const unsigned char *rgba, double *hue, double *s
The alpha channel is not used.
\param rgba : RGBa array values.
- \param hue : Array of hue values converted from RGB color space (range between [0 - 255]).
- \param saturation : Array of saturation values converted from RGB color space (range between [0 - 255]).
- \param value : Array of value values converted from RGB color space (range between [0 - 255]).
+ \param hue : Array of hue values converted from RGB color space (range
+ between [0 - 255]). \param saturation : Array of saturation values converted
+ from RGB color space (range between [0 - 255]). \param value : Array of
+ value values converted from RGB color space (range between [0 - 255]).
\param size : The total image size or the number of pixels.
*/
void vpImageConvert::RGBaToHSV(const unsigned char *rgba, unsigned char *hue, unsigned char *saturation,
- unsigned char *value, const unsigned int size) {
- for(unsigned int i = 0; i < size; i++) {
+ unsigned char *value, const unsigned int size)
+{
+ for (unsigned int i = 0; i < size; i++) {
double h, s, v;
- vpImageConvert::RGBaToHSV((rgba + i*4), &h, &s, &v, 1);
+ vpImageConvert::RGBaToHSV((rgba + i * 4), &h, &s, &v, 1);
- hue[i] = (unsigned char) (255.0 * h);
- saturation[i] = (unsigned char) (255.0 * s);
- value[i] = (unsigned char) (255.0 * v);
+ hue[i] = (unsigned char)(255.0 * h);
+ saturation[i] = (unsigned char)(255.0 * s);
+ value[i] = (unsigned char)(255.0 * v);
}
}
@@ -4336,7 +4703,8 @@ void vpImageConvert::RGBaToHSV(const unsigned char *rgba, unsigned char *hue, un
\param size : The total image size or the number of pixels.
*/
void vpImageConvert::HSVToRGB(const double *hue, const double *saturation, const double *value, unsigned char *rgb,
- const unsigned int size) {
+ const unsigned int size)
+{
vpImageConvert::HSV2RGB(hue, saturation, value, rgb, size, 3);
}
@@ -4350,11 +4718,12 @@ void vpImageConvert::HSVToRGB(const double *hue, const double *saturation, const
\param size : The total image size or the number of pixels.
*/
void vpImageConvert::HSVToRGB(const unsigned char *hue, const unsigned char *saturation, const unsigned char *value,
- unsigned char *rgb, const unsigned int size) {
- for(unsigned int i = 0; i < size; i++) {
+ unsigned char *rgb, const unsigned int size)
+{
+ for (unsigned int i = 0; i < size; i++) {
double h = hue[i] / 255.0, s = saturation[i] / 255.0, v = value[i] / 255.0;
- vpImageConvert::HSVToRGB(&h, &s, &v, (rgb + i*3), 1);
+ vpImageConvert::HSVToRGB(&h, &s, &v, (rgb + i * 3), 1);
}
}
@@ -4362,13 +4731,15 @@ void vpImageConvert::HSVToRGB(const unsigned char *hue, const unsigned char *sat
Converts an array of RGB to an array of hue, saturation, value values.
\param rgb : RGB array values.
- \param hue : Array of hue values converted from RGB color space (range between [0 - 1]).
- \param saturation : Array of saturation values converted from RGB color space (range between [0 - 1]).
- \param value : Array of value values converted from RGB color space (range between [0 - 1]).
- \param size : The total image size or the number of pixels.
+ \param hue : Array of hue values converted from RGB color space (range
+ between [0 - 1]). \param saturation : Array of saturation values converted
+ from RGB color space (range between [0 - 1]). \param value : Array of value
+ values converted from RGB color space (range between [0 - 1]). \param size :
+ The total image size or the number of pixels.
*/
void vpImageConvert::RGBToHSV(const unsigned char *rgb, double *hue, double *saturation, double *value,
- const unsigned int size) {
+ const unsigned int size)
+{
vpImageConvert::RGB2HSV(rgb, hue, saturation, value, size, 3);
}
@@ -4376,20 +4747,22 @@ void vpImageConvert::RGBToHSV(const unsigned char *rgb, double *hue, double *sat
Converts an array of RGB to an array of hue, saturation, value values.
\param rgb : RGB array values.
- \param hue : Array of hue values converted from RGB color space (range between [0 - 255]).
- \param saturation : Array of saturation values converted from RGB color space (range between [0 - 255]).
- \param value : Array of value values converted from RGB color space (range between [0 - 255]).
+ \param hue : Array of hue values converted from RGB color space (range
+ between [0 - 255]). \param saturation : Array of saturation values converted
+ from RGB color space (range between [0 - 255]). \param value : Array of
+ value values converted from RGB color space (range between [0 - 255]).
\param size : The total image size or the number of pixels.
*/
-void vpImageConvert::RGBToHSV(const unsigned char *rgb, unsigned char *hue, unsigned char *saturation, unsigned char *value,
- const unsigned int size) {
- for(unsigned int i = 0; i < size; i++) {
+void vpImageConvert::RGBToHSV(const unsigned char *rgb, unsigned char *hue, unsigned char *saturation,
+ unsigned char *value, const unsigned int size)
+{
+ for (unsigned int i = 0; i < size; i++) {
double h, s, v;
- vpImageConvert::RGBToHSV((rgb + i*3), &h, &s, &v, 1);
+ vpImageConvert::RGBToHSV((rgb + i * 3), &h, &s, &v, 1);
- hue[i] = (unsigned char) (255.0 * h);
- saturation[i] = (unsigned char) (255.0 * s);
- value[i] = (unsigned char) (255.0 * v);
+ hue[i] = (unsigned char)(255.0 * h);
+ saturation[i] = (unsigned char)(255.0 * s);
+ value[i] = (unsigned char)(255.0 * v);
}
}
diff --git a/modules/core/src/image/vpImageFilter.cpp b/modules/core/src/image/vpImageFilter.cpp
index 386e086..df63b71 100644
--- a/modules/core/src/image/vpImageFilter.cpp
+++ b/modules/core/src/image/vpImageFilter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,17 +36,16 @@
*
*****************************************************************************/
-#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-# include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-# include <opencv2/imgproc/imgproc_c.h>
+#include <opencv2/imgproc/imgproc_c.h>
#elif defined(VISP_HAVE_OPENCV)
-# include <cv.h>
+#include <cv.h>
#endif
-
/*!
Apply a filter to an image.
\param I : Image to filter
@@ -59,7 +59,8 @@
\sum_{y=0}^{\textbf{kernel\_h}}
\sum_{x=0}^{\textbf{kernel\_w}}
\textbf{M} \left( x,y \right ) \times
- \textbf{I} \left( u-\frac{\textbf{kernel\_w}}{2}+x,v-\frac{\textbf{kernel\_h}}{2}+y \right)
+ \textbf{I} \left(
+ u-\frac{\textbf{kernel\_w}}{2}+x,v-\frac{\textbf{kernel\_h}}{2}+y \right)
\f]
The convolution is almost the same operation:
\f[
@@ -67,28 +68,26 @@
\sum_{y=0}^{\textbf{kernel\_h}}
\sum_{x=0}^{\textbf{kernel\_w}}
\textbf{M} \left( x,y \right ) \times
- \textbf{I} \left( u+\frac{\textbf{kernel\_w}}{2}-x,v+\frac{\textbf{kernel\_h}}{2}-y \right)
+ \textbf{I} \left(
+ u+\frac{\textbf{kernel\_w}}{2}-x,v+\frac{\textbf{kernel\_h}}{2}-y \right)
\f]
Only pixels in the input image fully covered by the kernel are considered.
*/
-void
-vpImageFilter::filter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpMatrix& M,
- const bool convolve) {
+void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double> &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;
+ unsigned int half_size_y = size_y / 2, half_size_x = size_x / 2;
- If.resize(I.getHeight(),I.getWidth(), 0.0);
+ If.resize(I.getHeight(), I.getWidth(), 0.0);
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 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++) {
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
+ 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
conv += M[a][b] * val;
}
}
@@ -96,13 +95,13 @@ vpImageFilter::filter(const vpImage<unsigned char> &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 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++) {
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
+ 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
corr += M[a][b] * val;
}
}
@@ -115,8 +114,8 @@ vpImageFilter::filter(const vpImage<unsigned char> &I,
/*!
Apply a filter to an image:
\f[
- \textbf{I}_u = \textbf{M} \ast \textbf{I} \textbf{ and } \textbf{I}_v = \textbf{M}^t \ast \textbf{I}
- \f]
+ \textbf{I}_u = \textbf{M} \ast \textbf{I} \textbf{ and } \textbf{I}_v =
+ \textbf{M}^t \ast \textbf{I} \f]
\param I : Image to filter
\param Iu : Filtered image along the horizontal axis (u = columns).
@@ -124,27 +123,24 @@ vpImageFilter::filter(const vpImage<unsigned char> &I,
\param M : Filter kernel.
\param convolve : If true, perform a convolution otherwise a correlation.
*/
-void
-vpImageFilter::filter(const vpImage<double> &I,
- vpImage<double>& Iu,
- vpImage<double>& Iv,
- const vpMatrix& M,
- const bool convolve) {
+void vpImageFilter::filter(const vpImage<double> &I, vpImage<double> &Iu, vpImage<double> &Iv, const vpMatrix &M,
+ const bool convolve)
+{
unsigned int size = M.getRows();
- unsigned int half_size = size/2;
+ unsigned int half_size = size / 2;
- Iu.resize(I.getHeight(),I.getWidth(), 0.0);
- Iv.resize(I.getHeight(),I.getWidth(), 0.0);
+ Iu.resize(I.getHeight(), I.getWidth(), 0.0);
+ Iv.resize(I.getHeight(), I.getWidth(), 0.0);
if (convolve) {
- for (unsigned int v = half_size ; v < I.getHeight()-half_size ; v++) {
- for (unsigned int u = half_size ; u < I.getWidth()-half_size ; u++) {
+ for (unsigned int v = half_size; v < I.getHeight() - half_size; v++) {
+ for (unsigned int u = half_size; u < I.getWidth() - half_size; u++) {
double conv_u = 0;
double conv_v = 0;
- for(unsigned int a = 0 ; a < size ; a++ ) {
- for(unsigned int b = 0 ; b < size ; b++ ) {
- double val = I[v+half_size-a][u+half_size-b]; //Convolution
+ 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
conv_u += M[a][b] * val;
conv_v += M[b][a] * val;
}
@@ -154,14 +150,14 @@ vpImageFilter::filter(const vpImage<double> &I,
}
}
} else {
- for (unsigned int v = half_size ; v < I.getHeight()-half_size ; v++) {
- for (unsigned int u = half_size ; u < I.getWidth()-half_size ; u++) {
+ for (unsigned int v = half_size; v < I.getHeight() - half_size; v++) {
+ for (unsigned int u = half_size; u < I.getWidth() - half_size; u++) {
double conv_u = 0;
double conv_v = 0;
- for(unsigned int a = 0 ; a < size ; a++ ) {
- for(unsigned int b = 0 ; b < size ; b++ ) {
- double val = I[v-half_size+a][u-half_size+b]; //Correlation
+ 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
conv_u += M[a][b] * val;
conv_v += M[b][a] * val;
}
@@ -215,41 +211,41 @@ vpImageFilter::filter(const vpImage<double> &I,
\right ] \ast I
\right )
\f]
- Using two separable kernels reduce the number of operations and can be faster for large kernels.
+ Using two separable kernels reduce the number of operations and can be
+ faster for large kernels.
\param I : Image to filter
\param If : Filtered image.
\param kernelH : Separable kernel (performed first).
\param kernelV : Separable kernel (performed last).
- \note Only pixels in the input image fully covered by the kernel are considered.
+ \note Only pixels in the input image fully covered by the kernel are
+ considered.
*/
-void
-vpImageFilter::sepFilter(const vpImage<unsigned char> &I,
- vpImage<double>& If,
- const vpColVector& kernelH,
- const vpColVector &kernelV) {
+void vpImageFilter::sepFilter(const vpImage<unsigned char> &I, vpImage<double> &If, const vpColVector &kernelH,
+ const vpColVector &kernelV)
+{
unsigned int size = kernelH.size();
- unsigned int half_size = size/2;
+ unsigned int half_size = size / 2;
- If.resize(I.getHeight(),I.getWidth(), 0.0);
- vpImage<double> I_filter(I.getHeight(),I.getWidth(), 0.0);
+ If.resize(I.getHeight(), I.getWidth(), 0.0);
+ vpImage<double> I_filter(I.getHeight(), I.getWidth(), 0.0);
for (unsigned int i = 0; i < I.getHeight(); i++) {
- for (unsigned int j = half_size; j < I.getWidth()-half_size; j++) {
+ for (unsigned int j = half_size; j < I.getWidth() - half_size; j++) {
double conv = 0.0;
for (unsigned int a = 0; a < kernelH.size(); a++) {
- conv += kernelH[a] * I[i][j+half_size-a];
+ conv += kernelH[a] * I[i][j + half_size - a];
}
I_filter[i][j] = conv;
}
}
- for (unsigned int i = half_size; i < I.getHeight()-half_size; i++) {
+ for (unsigned int i = half_size; i < I.getHeight() - half_size; i++) {
for (unsigned int j = 0; j < I.getWidth(); j++) {
double conv = 0.0;
for (unsigned int a = 0; a < kernelV.size(); a++) {
- conv += kernelV[a] * I_filter[i+half_size-a][j];
+ conv += kernelV[a] * I_filter[i + half_size - a][j];
}
If[i][j] = conv;
@@ -286,7 +282,7 @@ int main()
//Apply the Canny edge operator and set the Icanny image.
vpImageFilter::canny(Isrc, Icanny, gaussianFilterSize, thresholdCanny, apertureSobel);
#endif
- return (0);
+ return (0);
}
\endcode
@@ -298,17 +294,14 @@ int main()
greater than this value are marked as an edge).
\param apertureSobel : Size of the mask for the Sobel operator (odd number).
*/
-void
-vpImageFilter:: canny(const vpImage<unsigned char>& Isrc,
- vpImage<unsigned char>& Ires,
- const unsigned int gaussianFilterSize,
- const double thresholdCanny,
- const unsigned int apertureSobel)
+void vpImageFilter::canny(const vpImage<unsigned char> &Isrc, vpImage<unsigned char> &Ires,
+ const unsigned int gaussianFilterSize, const double thresholdCanny,
+ const unsigned int apertureSobel)
{
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage* img_ipl = NULL;
+ IplImage *img_ipl = NULL;
vpImageConvert::convert(Isrc, img_ipl);
- IplImage* edges_ipl;
+ IplImage *edges_ipl;
edges_ipl = cvCreateImage(cvSize(img_ipl->width, img_ipl->height), img_ipl->depth, img_ipl->nChannels);
cvSmooth(img_ipl, img_ipl, CV_GAUSSIAN, (int)gaussianFilterSize, (int)gaussianFilterSize, 0, 0);
@@ -330,114 +323,97 @@ vpImageFilter:: canny(const vpImage<unsigned char>& Isrc,
/*!
Apply a separable filter.
*/
-void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double>& GI, const double *filter,unsigned int size)
+void vpImageFilter::filter(const vpImage<unsigned char> &I, vpImage<double> &GI, const double *filter,
+ unsigned int size)
{
- vpImage<double> GIx ;
- filterX(I, GIx,filter,size);
- filterY(GIx, GI,filter,size);
+ vpImage<double> GIx;
+ filterX(I, GIx, filter, size);
+ filterY(GIx, GI, filter, size);
GIx.destroy();
}
/*!
Apply a separable filter.
*/
-void vpImageFilter::filter(const vpImage<double> &I, vpImage<double>& GI, const double *filter,unsigned int size)
+void vpImageFilter::filter(const vpImage<double> &I, vpImage<double> &GI, const double *filter, unsigned int size)
{
- vpImage<double> GIx ;
- filterX(I, GIx,filter,size);
- filterY(GIx, GI,filter,size);
+ vpImage<double> GIx;
+ filterX(I, GIx, filter, size);
+ filterY(GIx, GI, filter, size);
GIx.destroy();
}
-void vpImageFilter::filterX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+void vpImageFilter::filterX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter,
+ unsigned int size)
{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
+ dIx.resize(I.getHeight(), I.getWidth());
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::filterXLeftBorder(I, i, j, filter, size);
+ // dIx[i][j]=0;
}
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
+ for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::filterX(I, i, j, filter, size);
}
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
+ for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
+ dIx[i][j] = vpImageFilter::filterXRightBorder(I, i, j, filter, size);
+ // dIx[i][j]=0;
}
}
}
-void vpImageFilter::filterX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+void vpImageFilter::filterX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size)
{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXLeftBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
+ dIx.resize(I.getHeight(), I.getWidth());
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::filterXLeftBorder(I, i, j, filter, size);
+ // dIx[i][j]=0;
}
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::filterX(I,i,j,filter,size);
+ for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::filterX(I, i, j, filter, size);
}
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=vpImageFilter::filterXRightBorder(I,i,j,filter,size);
- //dIx[i][j]=0;
+ for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
+ dIx[i][j] = vpImageFilter::filterXRightBorder(I, i, j, filter, size);
+ // dIx[i][j]=0;
}
}
}
-void vpImageFilter::filterY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+void vpImageFilter::filterY(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *filter,
+ unsigned int size)
{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
+ dIy.resize(I.getHeight(), I.getWidth());
+ for (unsigned int i = 0; i < (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::filterYTopBorder(I, i, j, filter, size);
}
}
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
+ for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::filterY(I, i, j, filter, size);
}
}
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
+ for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::filterYBottomBorder(I, i, j, filter, size);
}
}
}
-void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double> &dIy, const double *filter, unsigned int size)
{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYTopBorder(I,i,j,filter,size);
+ dIy.resize(I.getHeight(), I.getWidth());
+ for (unsigned int i = 0; i < (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::filterYTopBorder(I, i, j, filter, size);
}
}
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterY(I,i,j,filter,size);
+ for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::filterY(I, i, j, filter, size);
}
}
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::filterYBottomBorder(I,i,j,filter,size);
+ 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);
}
}
}
@@ -447,17 +423,20 @@ void vpImageFilter::filterY(const vpImage<double> &I, vpImage<double>& dIy, cons
\param I : Input image.
\param GI : Filtered image.
\param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or
+ negative, it is computed from filter size as sigma = (size-1)/6. \param
+ normalize : Flag indicating whether to normalize the filter coefficients or
+ not.
*/
-void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double>& GI, unsigned int size, double sigma, bool normalize)
+void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double> &GI, unsigned int size, double sigma,
+ bool normalize)
{
- double *fg=new double[(size+1)/2] ;
- vpImageFilter::getGaussianKernel(fg, size, sigma, normalize) ;
- vpImage<double> GIx ;
- vpImageFilter::filterX(I, GIx,fg,size);
- vpImageFilter::filterY(GIx, GI,fg,size);
+ double *fg = new double[(size + 1) / 2];
+ vpImageFilter::getGaussianKernel(fg, size, sigma, normalize);
+ vpImage<double> GIx;
+ vpImageFilter::filterX(I, GIx, fg, size);
+ vpImageFilter::filterY(GIx, GI, fg, size);
GIx.destroy();
delete[] fg;
}
@@ -467,17 +446,20 @@ void vpImageFilter::gaussianBlur(const vpImage<unsigned char> &I, vpImage<double
\param I : Input double image.
\param GI : Filtered image.
\param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+ \param sigma : Gaussian standard deviation. If it is equal to zero or
+ negative, it is computed from filter size as sigma = (size-1)/6. \param
+ normalize : Flag indicating whether to normalize the filter coefficients or
+ not.
*/
-void vpImageFilter::gaussianBlur(const vpImage<double> &I, vpImage<double>& GI, unsigned int size, double sigma, bool normalize)
+void vpImageFilter::gaussianBlur(const vpImage<double> &I, vpImage<double> &GI, unsigned int size, double sigma,
+ bool normalize)
{
- double *fg=new double[(size+1)/2] ;
- vpImageFilter::getGaussianKernel(fg, size, sigma, normalize) ;
- vpImage<double> GIx ;
- vpImageFilter::filterX(I, GIx,fg,size);
- vpImageFilter::filterY(GIx, GI,fg,size);
+ double *fg = new double[(size + 1) / 2];
+ vpImageFilter::getGaussianKernel(fg, size, sigma, normalize);
+ vpImage<double> GIx;
+ vpImageFilter::filterX(I, GIx, fg, size);
+ vpImageFilter::filterY(GIx, GI, fg, size);
GIx.destroy();
delete[] fg;
}
@@ -485,226 +467,195 @@ void vpImageFilter::gaussianBlur(const vpImage<double> &I, vpImage<double>& GI,
/*!
Return the coefficients of a Gaussian filter.
- \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
- The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
- \param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+ \param filter : Pointer to the filter kernel that should refer to a
+ (size+1)/2 array. The first value refers to the central coefficient, the
+ next one to the right coefficients. Left coefficients could be deduced by
+ symmetry. \param size : Filter size. This value should be odd. \param sigma
+ : Gaussian standard deviation. If it is equal to zero or negative, it is
+ computed from filter size as sigma = (size-1)/6. \param normalize : Flag
+ indicating whether to normalize the filter coefficients or not.
*/
void vpImageFilter::getGaussianKernel(double *filter, unsigned int size, double sigma, bool normalize)
{
- if (size%2 != 1)
- throw (vpImageException(vpImageException::incorrectInitializationError,
- "Bad Gaussian filter size"));
+ if (size % 2 != 1)
+ throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));
- if (sigma<= 0)
- sigma = (size-1)/6.0;
+ if (sigma <= 0)
+ sigma = (size - 1) / 6.0;
- int middle = (int)(size-1)/2;
+ 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));
+ for (int i = 0; i <= middle; i++) {
+ filter[i] = (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2));
}
if (normalize) {
- //renormalization
- double sum=0;
- for(int i=1; i<=middle; i++)
- {
- sum += 2*filter[i] ;
+ // renormalization
+ double sum = 0;
+ for (int i = 1; i <= middle; i++) {
+ sum += 2 * filter[i];
}
sum += filter[0];
- for(int i=0; i<=middle; i++)
- {
- filter[i] = filter[i]/sum;
+ for (int i = 0; i <= middle; i++) {
+ filter[i] = filter[i] / sum;
}
}
}
/*!
- Return the coefficients of a Gaussian derivative filter that may be used to compute spatial image derivatives after applying a Gaussian blur.
-
- \param filter : Pointer to the filter kernel that should refer to a (size+1)/2 array.
- The first value refers to the central coefficient, the next one to the right coefficients. Left coefficients could be deduced by symmetry.
- \param size : Filter size. This value should be odd.
- \param sigma : Gaussian standard deviation. If it is equal to zero or negative, it is computed from filter size as sigma = (size-1)/6.
- \param normalize : Flag indicating whether to normalize the filter coefficients or not.
+ Return the coefficients of a Gaussian derivative filter that may be used to
+ compute spatial image derivatives after applying a Gaussian blur.
+
+ \param filter : Pointer to the filter kernel that should refer to a
+ (size+1)/2 array. The first value refers to the central coefficient, the
+ next one to the right coefficients. Left coefficients could be deduced by
+ symmetry. \param size : Filter size. This value should be odd. \param sigma
+ : Gaussian standard deviation. If it is equal to zero or negative, it is
+ computed from filter size as sigma = (size-1)/6. \param normalize : Flag
+ indicating whether to normalize the filter coefficients or not.
*/
void vpImageFilter::getGaussianDerivativeKernel(double *filter, unsigned int size, double sigma, bool normalize)
{
- if (size%2 != 1)
- throw (vpImageException(vpImageException::incorrectInitializationError,
- "Bad Gaussian filter size"));
+ if (size % 2 != 1)
+ throw(vpImageException(vpImageException::incorrectInitializationError, "Bad Gaussian filter size"));
- if (sigma<= 0)
- sigma = (size-1)/6.0;
+ if (sigma <= 0)
+ sigma = (size - 1) / 6.0;
- int middle = (int)(size-1)/2;
+ int middle = (int)(size - 1) / 2;
double sigma2 = vpMath::sqr(sigma);
filter[0] = 0.;
- for(int i=1; i<= middle; i++)
- {
- filter[i] = -(1./(sigma*sqrt(2.*M_PI)))*(exp(-((i+1)*(i+1))/(2.*sigma2))-exp(-((i-1)*(i-1))/(2.*sigma2)))/2.;
+ for (int i = 1; i <= middle; i++) {
+ filter[i] = -(1. / (sigma * sqrt(2. * M_PI))) *
+ (exp(-((i + 1) * (i + 1)) / (2. * sigma2)) - exp(-((i - 1) * (i - 1)) / (2. * sigma2))) / 2.;
}
if (normalize) {
- double sum=0;
- for(int i=1; i<=middle; i++)
- {
- sum += 2.*(1./(sigma*sqrt(2.*M_PI)))*exp(-(i*i)/(2.*sigma2));
+ double sum = 0;
+ for (int i = 1; i <= middle; i++) {
+ sum += 2. * (1. / (sigma * sqrt(2. * M_PI))) * exp(-(i * i) / (2. * sigma2));
}
- sum += (1./(sigma*sqrt(2.*M_PI))) ;
+ sum += (1. / (sigma * sqrt(2. * M_PI)));
- for(int i=1; i<=middle; i++)
- {
- filter[i] = filter[i]/sum;
+ for (int i = 1; i <= middle; i++) {
+ filter[i] = filter[i] / sum;
}
}
}
-
-void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx)
+void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx)
{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- //dIx=0;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < 3 ; j++)
- {
- dIx[i][j]=0;
+ dIx.resize(I.getHeight(), I.getWidth());
+ // dIx=0;
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ dIx[i][j] = 0;
}
- for (unsigned int j=3 ; j < I.getWidth()-3 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j);
+ for (unsigned int j = 3; j < I.getWidth() - 3; j++) {
+ dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j);
}
- for (unsigned int j=I.getWidth()-3 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
+ for (unsigned int j = I.getWidth() - 3; j < I.getWidth(); j++) {
+ dIx[i][j] = 0;
}
}
}
-void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy)
+void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy)
{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- //dIy=0;
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ dIy.resize(I.getHeight(), I.getWidth());
+ // dIy=0;
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = 0;
}
}
- for (unsigned int i=3 ; i < I.getHeight()-3 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j);
+ for (unsigned int i = 3; i < I.getHeight() - 3; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j);
}
}
- for (unsigned int i=I.getHeight()-3 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ for (unsigned int i = I.getHeight() - 3; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = 0;
}
}
}
-void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+void vpImageFilter::getGradX(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *filter,
+ unsigned int size)
{
- dIx.resize(I.getHeight(),I.getWidth()) ;
+ dIx.resize(I.getHeight(), I.getWidth());
//#pragma omp parallel for
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=0;
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < (size - 1) / 2; j++) {
+ dIx[i][j] = 0;
}
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
+ for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j, filter, size);
}
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
+ for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
+ dIx[i][j] = 0;
}
}
}
-void vpImageFilter::getGradX(const vpImage<double> &I, vpImage<double>& dIx, const double *filter,unsigned int size)
+void vpImageFilter::getGradX(const vpImage<double> &I, vpImage<double> &dIx, const double *filter, unsigned int size)
{
- dIx.resize(I.getHeight(),I.getWidth()) ;
- //dIx=0;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < (size-1)/2 ; j++)
- {
- dIx[i][j]=0;
+ dIx.resize(I.getHeight(), I.getWidth());
+ // dIx=0;
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < (size - 1) / 2; j++) {
+ dIx[i][j] = 0;
}
- for (unsigned int j=(size-1)/2 ; j < I.getWidth()-(size-1)/2 ; j++)
- {
- dIx[i][j]=vpImageFilter::derivativeFilterX(I,i,j,filter,size);
+ for (unsigned int j = (size - 1) / 2; j < I.getWidth() - (size - 1) / 2; j++) {
+ dIx[i][j] = vpImageFilter::derivativeFilterX(I, i, j, filter, size);
}
- for (unsigned int j=I.getWidth()-(size-1)/2 ; j < I.getWidth() ; j++)
- {
- dIx[i][j]=0;
+ for (unsigned int j = I.getWidth() - (size - 1) / 2; j < I.getWidth(); j++) {
+ dIx[i][j] = 0;
}
}
}
-void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+void vpImageFilter::getGradY(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *filter,
+ unsigned int size)
{
- dIy.resize(I.getHeight(),I.getWidth()) ;
+ dIy.resize(I.getHeight(), I.getWidth());
//#pragma omp parallel for
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ for (unsigned int i = 0; i < (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = 0;
}
}
//#pragma omp parallel for
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
+ for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j, filter, size);
}
}
//#pragma omp parallel for
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ for (unsigned int i = I.getHeight() - (size - 1) / 2; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = 0;
}
}
}
-void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double>& dIy, const double *filter,unsigned int size)
+void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double> &dIy, const double *filter, unsigned int size)
{
- dIy.resize(I.getHeight(),I.getWidth()) ;
- //dIy=0;
- for (unsigned int i=0 ; i < (size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ dIy.resize(I.getHeight(), I.getWidth());
+ // dIy=0;
+ for (unsigned int i = 0; i < (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = 0;
}
}
- for (unsigned int i=(size-1)/2 ; i < I.getHeight()-(size-1)/2 ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=vpImageFilter::derivativeFilterY(I,i,j,filter,size);
+ for (unsigned int i = (size - 1) / 2; i < I.getHeight() - (size - 1) / 2; i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ dIy[i][j] = vpImageFilter::derivativeFilterY(I, i, j, filter, size);
}
}
- for (unsigned int i=I.getHeight()-(size-1)/2 ; i < I.getHeight() ; i++)
- {
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- dIy[i][j]=0;
+ 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;
}
}
}
@@ -713,14 +664,17 @@ void vpImageFilter::getGradY(const vpImage<double> &I, vpImage<double>& dIy, con
Compute the gradient along X after applying a gaussian filter along Y.
\param I : Input image
\param dIx : Gradient along X.
- \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
- \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
- \param size : Size of the Gaussian and Gaussian derivative kernels.
+ \param gaussianKernel : Gaussian kernel which values should be computed
+ using vpImageFilter::getGaussianKernel(). \param gaussianDerivativeKernel :
+ Gaussian derivative kernel which values should be computed using
+ vpImageFilter::getGaussianDerivativeKernel(). \param size : Size of the
+ Gaussian and Gaussian derivative kernels.
*/
-void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIx, const double *gaussianKernel, const double *gaussianDerivativeKernel, unsigned int size)
+void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIx, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel, unsigned int size)
{
vpImage<double> GIy;
- vpImageFilter::filterY(I, GIy, gaussianKernel, size);
+ vpImageFilter::filterY(I, GIy, gaussianKernel, size);
vpImageFilter::getGradX(GIy, dIx, gaussianDerivativeKernel, size);
}
@@ -728,51 +682,54 @@ void vpImageFilter::getGradXGauss2D(const vpImage<unsigned char> &I, vpImage<dou
Compute the gradient along Y after applying a gaussian filter along X.
\param I : Input image
\param dIy : Gradient along Y.
- \param gaussianKernel : Gaussian kernel which values should be computed using vpImageFilter::getGaussianKernel().
- \param gaussianDerivativeKernel : Gaussian derivative kernel which values should be computed using vpImageFilter::getGaussianDerivativeKernel().
- \param size : Size of the Gaussian and Gaussian derivative kernels.
+ \param gaussianKernel : Gaussian kernel which values should be computed
+ using vpImageFilter::getGaussianKernel(). \param gaussianDerivativeKernel :
+ Gaussian derivative kernel which values should be computed using
+ vpImageFilter::getGaussianDerivativeKernel(). \param size : Size of the
+ Gaussian and Gaussian derivative kernels.
*/
-void vpImageFilter::getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double>& dIy, const double *gaussianKernel, const double *gaussianDerivativeKernel,unsigned int size)
+void vpImageFilter::getGradYGauss2D(const vpImage<unsigned char> &I, vpImage<double> &dIy, const double *gaussianKernel,
+ const double *gaussianDerivativeKernel, unsigned int size)
{
vpImage<double> GIx;
- vpImageFilter::filterX(I, GIx, gaussianKernel, size);
+ vpImageFilter::filterX(I, GIx, gaussianKernel, size);
vpImageFilter::getGradY(GIx, dIy, gaussianDerivativeKernel, size);
}
-//operation pour pyramide gaussienne
-void vpImageFilter::getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+// operation pour pyramide gaussienne
+void vpImageFilter::getGaussPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{
vpImage<unsigned char> GIx;
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
cv::Mat imgsrc, imgdest;
vpImageConvert::convert(I, imgsrc);
- cv::pyrDown( imgsrc, imgdest, cv::Size((int)I.getWidth()/2,(int)I.getHeight()/2));
+ 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);
- vpImageConvert::convert(I,imgsrc);
- cvPyrDown( imgsrc, imgdest);
- vpImageConvert::convert(imgdest,GI);
+ IplImage *imgsrc = NULL; // cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
+ IplImage *imgdest = NULL; // cvCreateImage(cvGetSize(imgign), IPL_DEPTH_8U, 1);
+ imgsrc = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
+ imgdest = cvCreateImage(cvSize((int)I.getWidth() / 2, (int)I.getHeight() / 2), IPL_DEPTH_8U, 1);
+ vpImageConvert::convert(I, imgsrc);
+ cvPyrDown(imgsrc, imgdest);
+ vpImageConvert::convert(imgdest, GI);
cvReleaseImage(&imgsrc);
cvReleaseImage(&imgdest);
- //vpImage<unsigned char> sGI;sGI=GI;
+// vpImage<unsigned char> sGI;sGI=GI;
#else
- vpImageFilter::getGaussXPyramidal(I,GIx);
- vpImageFilter::getGaussYPyramidal(GIx,GI);
+ vpImageFilter::getGaussXPyramidal(I, GIx);
+ vpImageFilter::getGaussYPyramidal(GIx, GI);
#endif
}
-void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{
#if 0
GI.resize(I.getHeight(),(int)((I.getWidth()+1.)/2.)) ;
@@ -786,50 +743,42 @@ void vpImageFilter::getGaussXPyramidal(const vpImage<unsigned char> &I, vpImage<
GI[i][(int)((I.getWidth()+1.)/2.)-1]=I[i][2*((int)((I.getWidth()+1.)/2.)-1)];
}
#else
- unsigned int w = I.getWidth()/2;
+ unsigned int w = I.getWidth() / 2;
- GI.resize(I.getHeight(), w) ;
- for (unsigned int i=0 ; i < I.getHeight() ; i++)
- {
- GI[i][0]=I[i][0];
- for (unsigned int j=1 ; j < w-1 ; j++)
- {
- GI[i][j]=vpImageFilter::filterGaussXPyramidal(I,i,2*j);
+ GI.resize(I.getHeight(), w);
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ GI[i][0] = I[i][0];
+ for (unsigned int j = 1; j < w - 1; j++) {
+ GI[i][j] = vpImageFilter::filterGaussXPyramidal(I, i, 2 * j);
}
- GI[i][w-1]=I[i][2*w-1];
+ GI[i][w - 1] = I[i][2 * w - 1];
}
#endif
}
-void vpImageFilter::getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char>& GI)
+void vpImageFilter::getGaussYPyramidal(const vpImage<unsigned char> &I, vpImage<unsigned char> &GI)
{
#ifdef ORIG
- GI.resize((int)((I.getHeight()+1.)/2.),I.getWidth()) ;
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- GI[0][j]=I[0][j];
- for (unsigned int i=1 ; i < ((I.getHeight()+1.)/2.)-1 ; i++)
- {
- GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
+ GI.resize((int)((I.getHeight() + 1.) / 2.), I.getWidth());
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ GI[0][j] = I[0][j];
+ for (unsigned int i = 1; i < ((I.getHeight() + 1.) / 2.) - 1; i++) {
+ GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j);
}
- GI[(int)((I.getHeight()+1.)/2.)-1][j]=I[2*((int)((I.getHeight()+1.)/2.)-1)][j];
+ GI[(int)((I.getHeight() + 1.) / 2.) - 1][j] = I[2 * ((int)((I.getHeight() + 1.) / 2.) - 1)][j];
}
#else
- unsigned int h = I.getHeight()/2;
+ unsigned int h = I.getHeight() / 2;
- GI.resize(h, I.getWidth()) ;
- for (unsigned int j=0 ; j < I.getWidth() ; j++)
- {
- GI[0][j]=I[0][j];
- for (unsigned int i=1 ; i < h-1 ; i++)
- {
- GI[i][j]=vpImageFilter::filterGaussYPyramidal(I,2*i,j);
+ GI.resize(h, I.getWidth());
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ GI[0][j] = I[0][j];
+ for (unsigned int i = 1; i < h - 1; i++) {
+ GI[i][j] = vpImageFilter::filterGaussYPyramidal(I, 2 * i, j);
}
- GI[h-1][j]=I[2*h-1][j];
+ GI[h - 1][j] = I[2 * h - 1][j];
}
#endif
}
-
-
diff --git a/modules/core/src/image/vpImageMorphology.cpp b/modules/core/src/image/vpImageMorphology.cpp
index 3e1dfda..531034e 100644
--- a/modules/core/src/image/vpImageMorphology.cpp
+++ b/modules/core/src/image/vpImageMorphology.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,29 +36,30 @@
*
*****************************************************************************/
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpImageMorphology.h>
#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
-# include <emmintrin.h>
-# define VISP_HAVE_SSE2 1
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
#endif
-
/*!
Erode a grayscale image using the given structuring element.
- The gray-scale erosion of \f$ A \left( x, y \right) \f$ by \f$ B \left (x, y \right) \f$ is defined as:
- \f[
- \left ( A \ominus B \right ) \left( x,y \right) = \textbf{min} \left \{ A \left ( x+x', y+y' \right ) -
- B \left ( x', y'\right ) | \left ( x', y'\right ) \subseteq D_B \right \}
- \f]
- where \f$ D_B \f$ is the domain of the structuring element \f$ B \f$ and \f$ A \left( x,y \right) \f$ is assumed
- to be \f$ + \infty \f$ outside the domain of the image.
-
- In our case, gray-scale erosion is performed with a flat structuring element \f$ \left( B \left( x,y \right) = 0 \right) \f$.
- Gray-scale erosion using such a structuring element is equivalent to a local-minimum operator:
- \f[
- \left ( A \ominus B \right ) \left( x,y \right) = \textbf{min} \left \{ A \left ( x+x', y+y' \right ) | \left ( x', y'\right ) \subseteq D_B \right \}
+ The gray-scale erosion of \f$ A \left( x, y \right) \f$ by \f$ B \left (x, y
+ \right) \f$ is defined as: \f[ \left ( A \ominus B \right ) \left( x,y
+ \right) = \textbf{min} \left \{ A \left ( x+x', y+y' \right ) - B \left (
+ x', y'\right ) | \left ( x', y'\right ) \subseteq D_B \right \} \f] where
+ \f$ D_B \f$ is the domain of the structuring element \f$ B \f$ and \f$ A
+ \left( x,y \right) \f$ is assumed to be \f$ + \infty \f$ outside the domain
+ of the image.
+
+ In our case, gray-scale erosion is performed with a flat structuring element
+ \f$ \left( B \left( x,y \right) = 0 \right) \f$. Gray-scale erosion using
+ such a structuring element is equivalent to a local-minimum operator: \f[
+ \left ( A \ominus B \right ) \left( x,y \right) = \textbf{min} \left \{ A
+ \left ( x+x', y+y' \right ) | \left ( x', y'\right ) \subseteq D_B \right \}
\f]
\param I : Image to process.
@@ -65,15 +67,16 @@
\sa dilatation(vpImage<unsigned char> &, const vpConnexityType &)
*/
-void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType &connexity) {
- if(I.getSize() == 0) {
+void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType &connexity)
+{
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
const unsigned char null_value = 255;
- vpImage<unsigned char> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<unsigned char> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -82,30 +85,32 @@ void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType
}
} else {
J[i][0] = null_value;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = null_value;
}
}
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
for (unsigned int i = 0; i < I.getHeight(); i++) {
unsigned int j = 0;
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
#if VISP_HAVE_SSE2
- if (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])) );
-
- _mm_storeu_si128( (__m128i *) (ptr_curr_I + j), m );
+ 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])));
+
+ _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m);
}
}
#endif
@@ -113,37 +118,46 @@ void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType
for (; j < I.getWidth(); j++) {
unsigned char min_value = null_value;
for (int k = 0; k < 5; k++) {
- min_value = std::min(min_value, *(ptr_curr_J + j + offset[k]));
+ min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = min_value;
}
}
} else {
- //CONNEXITY_8
- unsigned int offset[9] = {0, 1, 2, J.getWidth(), J.getWidth()+1, J.getWidth()+2,
- J.getWidth()*2, J.getWidth()*2 + 1, J.getWidth()*2 + 2};
+ // CONNEXITY_8
+ unsigned int offset[9] = {0,
+ 1,
+ 2,
+ J.getWidth(),
+ J.getWidth() + 1,
+ J.getWidth() + 2,
+ J.getWidth() * 2,
+ J.getWidth() * 2 + 1,
+ J.getWidth() * 2 + 2};
+#if VISP_HAVE_SSE2
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#endif
for (unsigned int i = 0; i < I.getHeight(); i++) {
unsigned int j = 0;
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
#if VISP_HAVE_SSE2
- if (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])) );
-
- _mm_storeu_si128( (__m128i *) (ptr_curr_I + j), m );
+ 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])));
+
+ _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m);
}
}
#endif
@@ -151,7 +165,7 @@ void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType
for (; j < I.getWidth(); j++) {
unsigned char min_value = null_value;
for (int k = 0; k < 9; k++) {
- min_value = std::min(min_value, *(ptr_curr_J + j + offset[k]));
+ min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = min_value;
@@ -163,18 +177,19 @@ void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType
/*!
Dilate a grayscale image using the given structuring element.
- The gray-scale dilatation of \f$ A \left( x, y \right) \f$ by \f$ B \left (x, y \right) \f$ is defined as:
- \f[
- \left ( A \oplus B \right ) \left( x,y \right) = \textbf{max} \left \{ A \left ( x-x', y-y' \right ) +
- B \left ( x', y'\right ) | \left ( x', y'\right ) \subseteq D_B \right \}
- \f]
- where \f$ D_B \f$ is the domain of the structuring element \f$ B \f$ and \f$ A \left( x,y \right) \f$ is assumed
- to be \f$ - \infty \f$ outside the domain of the image.
-
- In our case, gray-scale erosion is performed with a flat structuring element \f$ \left( B \left( x,y \right) = 0 \right) \f$.
- Gray-scale erosion using such a structuring element is equivalent to a local-maximum operator:
- \f[
- \left ( A \oplus B \right ) \left( x,y \right) = \textbf{max} \left \{ A \left ( x-x', y-y' \right ) | \left ( x', y'\right ) \subseteq D_B \right \}
+ The gray-scale dilatation of \f$ A \left( x, y \right) \f$ by \f$ B \left
+ (x, y \right) \f$ is defined as: \f[ \left ( A \oplus B \right ) \left( x,y
+ \right) = \textbf{max} \left \{ A \left ( x-x', y-y' \right ) + B \left (
+ x', y'\right ) | \left ( x', y'\right ) \subseteq D_B \right \} \f] where
+ \f$ D_B \f$ is the domain of the structuring element \f$ B \f$ and \f$ A
+ \left( x,y \right) \f$ is assumed to be \f$ - \infty \f$ outside the domain
+ of the image.
+
+ In our case, gray-scale erosion is performed with a flat structuring element
+ \f$ \left( B \left( x,y \right) = 0 \right) \f$. Gray-scale erosion using
+ such a structuring element is equivalent to a local-maximum operator: \f[
+ \left ( A \oplus B \right ) \left( x,y \right) = \textbf{max} \left \{ A
+ \left ( x-x', y-y' \right ) | \left ( x', y'\right ) \subseteq D_B \right \}
\f]
\param I : Image to process.
@@ -182,15 +197,16 @@ void vpImageMorphology::erosion(vpImage<unsigned char> &I, const vpConnexityType
\sa erosion(vpImage<unsigned char> &, const vpConnexityType &)
*/
-void vpImageMorphology::dilatation(vpImage<unsigned char> &I, const vpConnexityType &connexity) {
- if(I.getSize() == 0) {
+void vpImageMorphology::dilatation(vpImage<unsigned char> &I, const vpConnexityType &connexity)
+{
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
const unsigned char null_value = 0;
- vpImage<unsigned char> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<unsigned char> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -199,30 +215,32 @@ void vpImageMorphology::dilatation(vpImage<unsigned char> &I, const vpConnexityT
}
} else {
J[i][0] = null_value;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = null_value;
}
}
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
for (unsigned int i = 0; i < I.getHeight(); i++) {
unsigned int j = 0;
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
#if VISP_HAVE_SSE2
- if (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])) );
-
- _mm_storeu_si128( (__m128i *) (ptr_curr_I + j), m );
+ 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])));
+
+ _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m);
}
}
#endif
@@ -230,37 +248,46 @@ void vpImageMorphology::dilatation(vpImage<unsigned char> &I, const vpConnexityT
for (; j < I.getWidth(); j++) {
unsigned char max_value = null_value;
for (int k = 0; k < 5; k++) {
- max_value = std::max(max_value, *(ptr_curr_J + j + offset[k]));
+ max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = max_value;
}
}
} else {
- //CONNEXITY_8
- unsigned int offset[9] = {0, 1, 2, J.getWidth(), J.getWidth()+1, J.getWidth()+2,
- J.getWidth()*2, J.getWidth()*2 + 1, J.getWidth()*2 + 2};
+ // CONNEXITY_8
+ unsigned int offset[9] = {0,
+ 1,
+ 2,
+ J.getWidth(),
+ J.getWidth() + 1,
+ J.getWidth() + 2,
+ J.getWidth() * 2,
+ J.getWidth() * 2 + 1,
+ J.getWidth() * 2 + 2};
+#if VISP_HAVE_SSE2
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#endif
for (unsigned int i = 0; i < I.getHeight(); i++) {
unsigned int j = 0;
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
#if VISP_HAVE_SSE2
- if (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])) );
-
- _mm_storeu_si128( (__m128i *) (ptr_curr_I + j), m );
+ 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])));
+
+ _mm_storeu_si128((__m128i *)(ptr_curr_I + j), m);
}
}
#endif
@@ -268,7 +295,7 @@ void vpImageMorphology::dilatation(vpImage<unsigned char> &I, const vpConnexityT
for (; j < I.getWidth(); j++) {
unsigned char max_value = null_value;
for (int k = 0; k < 9; k++) {
- max_value = std::max(max_value, *(ptr_curr_J + j + offset[k]));
+ max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = max_value;
diff --git a/modules/core/src/image/vpImagePoint.cpp b/modules/core/src/image/vpImagePoint.cpp
index 9a255b6..c1b4c16 100644
--- a/modules/core/src/image/vpImagePoint.cpp
+++ b/modules/core/src/image/vpImagePoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpRect.h>
@@ -44,25 +44,24 @@
/*!
Check if an image point belongs to a rectangle.
-
+
\param rect : the rectangle.
-
+
\return Returns true if the point belongs to the rectangle.
*/
-bool vpImagePoint::inRectangle( const vpRect &rect ) const
+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());
}
/*!
Operator +=.
- This operator can be used to compute the center of gravity of a set of image points.
+ This operator can be used to compute the center of gravity of a set of image
+points.
\code
#include <iostream>
@@ -71,21 +70,22 @@ bool vpImagePoint::inRectangle( const vpRect &rect ) const
int main()
{
-std::vector<vpImagePoint> ip(2);
+ std::vector<vpImagePoint> 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<ip.size(); i++)
- cog += ip[i];
-cog /= ip.size();
-std::cout << "cog: " << cog << std::endl;
+ vpImagePoint cog(0,0);
+ for(unsigned int i=0; i<ip.size(); i++)
+ cog += ip[i];
+ cog /= ip.size();
+ std::cout << "cog: " << cog << std::endl;
}
\endcode
*/
-vpImagePoint& vpImagePoint::operator+=(const vpImagePoint &ip) {
+vpImagePoint &vpImagePoint::operator+=(const vpImagePoint &ip)
+{
this->i += ip.i;
this->j += ip.j;
return *this;
@@ -95,29 +95,31 @@ vpImagePoint& vpImagePoint::operator+=(const vpImagePoint &ip) {
Operator /=.
- This operator can be used to compute the center of gravity of a set of image points.
- \code
+ This operator can be used to compute the center of gravity of a set of image
+points.
+\code
#include <iostream>
#include <vector>
#include <visp3/core/vpImagePoint.h>
int main()
{
-std::vector<vpImagePoint> ip(2);
+ std::vector<vpImagePoint> 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<ip.size(); i++)
- cog += ip[i];
-cog /= ip.size();
-std::cout << "cog: " << cog << std::endl;
+ vpImagePoint cog(0,0);
+ for(unsigned int i=0; i<ip.size(); i++)
+ cog += ip[i];
+ cog /= ip.size();
+ std::cout << "cog: " << cog << std::endl;
}
\endcode
*/
-vpImagePoint& vpImagePoint::operator/=(const double scale) {
+vpImagePoint &vpImagePoint::operator/=(const double scale)
+{
this->i /= scale;
this->j /= scale;
return *this;
@@ -129,19 +131,18 @@ vpImagePoint& vpImagePoint::operator/=(const double scale) {
Returns true if ip1 and ip2 are equal; otherwire returns false.
*/
-VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- //return ( ( ip1.get_i() == ip2.get_i() ) && ( ip1.get_j() == ip2.get_j() ) );
+VISP_EXPORT bool operator==(const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ // return ( ( ip1.get_i() == ip2.get_i() ) && ( ip1.get_j() == ip2.get_j() )
+ // );
double i1 = ip1.get_i();
double j1 = ip1.get_j();
double i2 = ip2.get_i();
double j2 = ip2.get_j();
- return (
- ( std::fabs(i1-i2) <= std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(j1-j2) <= std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
- );
+ return ((std::fabs(i1 - i2) <= std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(j1 - j2) <= std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits<double>::epsilon()));
}
/*!
@@ -151,18 +152,17 @@ VISP_EXPORT bool operator==( const vpImagePoint &ip1, const vpImagePoint &ip2 )
Returns true if ip1 and ip2 are different; otherwire returns true.
*/
-VISP_EXPORT bool operator!=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- //return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() ) );
+VISP_EXPORT bool operator!=(const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ // return ( ( ip1.get_i() != ip2.get_i() ) || ( ip1.get_j() != ip2.get_j() )
+ // );
double i1 = ip1.get_i();
double j1 = ip1.get_j();
double i2 = ip2.get_i();
double j2 = ip2.get_j();
- return (
- ( std::fabs(i1-i2) > std::fabs(vpMath::maximum(i1, i2))*std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(j1-j2) > std::fabs(vpMath::maximum(j1, j2))*std::numeric_limits<double>::epsilon() )
- );
+ return ((std::fabs(i1 - i2) > std::fabs(vpMath::maximum(i1, i2)) * std::numeric_limits<double>::epsilon()) ||
+ (std::fabs(j1 - j2) > std::fabs(vpMath::maximum(j1, j2)) * std::numeric_limits<double>::epsilon()));
}
/*!
@@ -172,8 +172,9 @@ 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 ) {
- return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
+VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ return (vpImagePoint(ip1.get_i() + ip2.get_i(), ip1.get_j() + ip2.get_j()));
}
/*!
@@ -182,8 +183,9 @@ VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const vpImagePoint
Returns a vpImagePoint wich is the sum of \f$ ip1 \f$ and \f$ ip2 \f$.
*/
-VISP_EXPORT vpImagePoint operator+=( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- return ( vpImagePoint(ip1.get_i()+ip2.get_i(), ip1.get_j()+ip2.get_j()));
+VISP_EXPORT vpImagePoint operator+=(const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ return (vpImagePoint(ip1.get_i() + ip2.get_i(), ip1.get_j() + ip2.get_j()));
}
/*!
@@ -205,8 +207,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const int offset ) {
- return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
+VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const int offset)
+{
+ return (vpImagePoint(ip1.get_i() + offset, ip1.get_j() + offset));
}
/*!
@@ -229,8 +232,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const unsigned int offset ) {
- return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
+VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const unsigned int offset)
+{
+ return (vpImagePoint(ip1.get_i() + offset, ip1.get_j() + offset));
}
/*!
@@ -253,19 +257,22 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator+( const vpImagePoint &ip1, const double offset ) {
- return ( vpImagePoint(ip1.get_i()+offset, ip1.get_j()+offset));
+VISP_EXPORT vpImagePoint operator+(const vpImagePoint &ip1, const double offset)
+{
+ return (vpImagePoint(ip1.get_i() + offset, ip1.get_j() + offset));
}
/*!
\relates vpImagePoint
- Returns a vpImagePoint wich is the difference between \f$ ip1 \f$ and \f$ ip2 \f$.
+ Returns a vpImagePoint wich is the difference between \f$ ip1 \f$ and \f$
+ ip2 \f$.
*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const vpImagePoint &ip2 ) {
- return ( vpImagePoint(ip1.get_i()-ip2.get_i(), ip1.get_j()-ip2.get_j()));
+VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const vpImagePoint &ip2)
+{
+ return (vpImagePoint(ip1.get_i() - ip2.get_i(), ip1.get_j() - ip2.get_j()));
}
/*!
@@ -287,8 +294,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const int offset ) {
- return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
+VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const int offset)
+{
+ return (vpImagePoint(ip1.get_i() - offset, ip1.get_j() - offset));
}
/*!
@@ -310,8 +318,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const unsigned int offset ) {
- return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
+VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const unsigned int offset)
+{
+ return (vpImagePoint(ip1.get_i() - offset, ip1.get_j() - offset));
}
/*!
@@ -334,8 +343,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator-( const vpImagePoint &ip1, const double offset ) {
- return ( vpImagePoint(ip1.get_i()-offset, ip1.get_j()-offset));
+VISP_EXPORT vpImagePoint operator-(const vpImagePoint &ip1, const double offset)
+{
+ return (vpImagePoint(ip1.get_i() - offset, ip1.get_j() - offset));
}
/*!
@@ -357,8 +367,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator*( const vpImagePoint &ip1, const double scale ) {
- return ( vpImagePoint(ip1.get_i()*scale, ip1.get_j()*scale));
+VISP_EXPORT vpImagePoint operator*(const vpImagePoint &ip1, const double scale)
+{
+ return (vpImagePoint(ip1.get_i() * scale, ip1.get_j() * scale));
}
/*!
@@ -380,8 +391,9 @@ int main()
}
\endcode
*/
-VISP_EXPORT vpImagePoint operator/( const vpImagePoint &ip1, const double scale ) {
- return ( vpImagePoint(ip1.get_i()/scale, ip1.get_j()/scale));
+VISP_EXPORT vpImagePoint operator/(const vpImagePoint &ip1, const double scale)
+{
+ return (vpImagePoint(ip1.get_i() / scale, ip1.get_j() / scale));
}
/*!
@@ -416,7 +428,7 @@ int main()
Image point with coordinates: 10, 11.1
\endverbatim
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImagePoint &ip)
{
os << ip.get_i() << ", " << ip.get_j();
return os;
@@ -427,7 +439,7 @@ VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImagePoint& ip)
* @param ipVec : Vector of input image points.
* @return Bounding box of the points.
*/
-vpRect vpImagePoint::getBBox(const std::vector<vpImagePoint>& ipVec)
+vpRect vpImagePoint::getBBox(const std::vector<vpImagePoint> &ipVec)
{
vpRect rec(ipVec);
diff --git a/modules/core/src/image/vpImageTools.cpp b/modules/core/src/image/vpImageTools.cpp
index b050dc5..33bbca3 100644
--- a/modules/core/src/image/vpImageTools.cpp
+++ b/modules/core/src/image/vpImageTools.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,14 +36,14 @@
*
*****************************************************************************/
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpImageTools.h>
#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
-# include <emmintrin.h>
-# define VISP_HAVE_SSE2 1
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
#endif
-
/*!
Change the look up table (LUT) of an image. Considering pixel gray
level values \f$ l \f$ in the range \f$[A, B]\f$, this method allows
@@ -73,8 +74,8 @@
thresholding this image at level 127 can be done by:
\code
-#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageTools.h>
#include <visp3/io/vpImageIo.h>
int main()
@@ -99,24 +100,20 @@ int main()
\endcode
*/
-void vpImageTools::changeLUT(vpImage<unsigned char>& I,
- unsigned char A,
- unsigned char A_star,
- unsigned char B,
+void vpImageTools::changeLUT(vpImage<unsigned char> &I, unsigned char A, unsigned char A_star, unsigned char B,
unsigned char B_star)
{
// Test if input values are valid
if (B <= A) {
- vpERROR_TRACE("Bad gray levels") ;
- throw (vpImageException(vpImageException::incorrectInitializationError ,
- "Bad gray levels"));
+ vpERROR_TRACE("Bad gray levels");
+ throw(vpImageException(vpImageException::incorrectInitializationError, "Bad gray levels"));
}
unsigned char v;
- double factor = (double)(B_star - A_star)/(double)(B - A);
+ double factor = (double)(B_star - A_star) / (double)(B - A);
- for (unsigned int i=0 ; i < I.getHeight(); i++)
- for (unsigned int j=0 ; j < I.getWidth(); j++) {
+ for (unsigned int i = 0; i < I.getHeight(); i++)
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
v = I[i][j];
if (v <= A)
@@ -124,7 +121,7 @@ void vpImageTools::changeLUT(vpImage<unsigned char>& I,
else if (v >= B)
I[i][j] = B_star;
else
- I[i][j] = (unsigned char)(A_star + factor*(v-A));
+ I[i][j] = (unsigned char)(A_star + factor * (v - A));
}
}
@@ -140,30 +137,27 @@ void vpImageTools::changeLUT(vpImage<unsigned char>& I,
\param I2 : The second image.
\param Idiff : The result of the difference.
*/
-void vpImageTools::imageDifference(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
+void vpImageTools::imageDifference(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
vpImage<unsigned char> &Idiff)
{
- if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
- {
- throw (vpException(vpException::dimensionError, "The two images have not the same size"));
+ if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
+ throw(vpException(vpException::dimensionError, "The two images have not the same size"));
}
if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
Idiff.resize(I1.getHeight(), I1.getWidth());
- unsigned int n = I1.getHeight() * I1.getWidth() ;
- for (unsigned int b = 0; b < n ; b++)
- {
+ 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));
}
}
/*!
- Compute the signed difference between the two images I1 and I2 RGB components for
- visualization issue : Idiff = I1-I2. The fourth component named A is not compared.
- It is set to 0 in the resulting difference image.
+ Compute the signed difference between the two images I1 and I2 RGB
+ components for visualization issue : Idiff = I1-I2. The fourth component
+ named A is not compared. It is set to 0 in the resulting difference image.
- pixels with a null difference are set to R=128, G=128, B=128.
- A negative difference implies a pixel R, G, B value < 128
@@ -173,30 +167,28 @@ void vpImageTools::imageDifference(const vpImage<unsigned char> &I1,
\param I2 : The second image.
\param Idiff : The result of the difference between RGB components.
*/
-void vpImageTools::imageDifference(const vpImage<vpRGBa> &I1,
- const vpImage<vpRGBa> &I2,
- vpImage<vpRGBa> &Idiff)
+void vpImageTools::imageDifference(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &Idiff)
{
- 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()));
+ 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()));
}
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++)
- {
+ unsigned int n = I1.getHeight() * I1.getWidth();
+ for (unsigned int b = 0; b < n; b++) {
int diffR = I1.bitmap[b].R - I2.bitmap[b].R + 128;
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));
}
}
@@ -210,22 +202,18 @@ void vpImageTools::imageDifference(const vpImage<vpRGBa> &I1,
\param I2 : The second image.
\param Idiff : The result of the difference.
*/
-void
-vpImageTools::imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Idiff)
+void vpImageTools::imageDifferenceAbsolute(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &Idiff)
{
- if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth()))
- {
- throw (vpException(vpException::dimensionError, "The two images do not have the same size"));
+ if ((I1.getHeight() != I2.getHeight()) || (I1.getWidth() != I2.getWidth())) {
+ throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Idiff.getHeight()) || (I1.getWidth() != Idiff.getWidth()))
Idiff.resize(I1.getHeight(), I1.getWidth());
- unsigned int n = I1.getHeight() * I1.getWidth() ;
- for (unsigned int b = 0; b < n ; b++)
- {
+ unsigned int n = I1.getHeight() * I1.getWidth();
+ for (unsigned int b = 0; b < n; b++) {
int diff = I1.bitmap[b] - I2.bitmap[b];
Idiff.bitmap[b] = diff;
}
@@ -244,30 +232,25 @@ vpImageTools::imageDifferenceAbsolute(const vpImage<unsigned char> &I1,
\param I2 : The second image.
\param Idiff : The result of the difference between RGB components.
*/
-void
-vpImageTools::imageDifferenceAbsolute(const vpImage<vpRGBa> &I1,
- const vpImage<vpRGBa> &I2,
- vpImage<vpRGBa> &Idiff)
+void vpImageTools::imageDifferenceAbsolute(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &I2, vpImage<vpRGBa> &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()))
Idiff.resize(I1.getHeight(), I1.getWidth());
- unsigned int n = I1.getHeight() * I1.getWidth() ;
- for (unsigned int b = 0; b < n ; b++)
- {
+ unsigned int n = I1.getHeight() * I1.getWidth();
+ for (unsigned int b = 0; b < n; b++) {
int diffR = I1.bitmap[b].R - I2.bitmap[b].R;
int diffG = I1.bitmap[b].G - I2.bitmap[b].G;
int diffB = I1.bitmap[b].B - I2.bitmap[b].B;
- //int diffA = I1.bitmap[b].A - I2.bitmap[b].A;
+ // int diffA = I1.bitmap[b].A - I2.bitmap[b].A;
Idiff.bitmap[b].R = diffR;
Idiff.bitmap[b].G = diffG;
Idiff.bitmap[b].B = diffB;
- //Idiff.bitmap[b].A = diffA;
+ // Idiff.bitmap[b].A = diffA;
Idiff.bitmap[b].A = 0;
}
}
@@ -278,41 +261,39 @@ vpImageTools::imageDifferenceAbsolute(const vpImage<vpRGBa> &I1,
\param I1 : The first image.
\param I2 : The second image.
\param Ires : \f$ Ires = I1 + I2 \f$
- \param saturate : If true, saturate the result to [0 ; 255] using vpMath::saturate, otherwise overflow may occur.
+ \param saturate : If true, saturate the result to [0 ; 255] using
+ vpMath::saturate, otherwise overflow may occur.
*/
-void
-vpImageTools::imageAdd(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires,
- const bool saturate)
+void vpImageTools::imageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &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"));
+ throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
Ires.resize(I1.getHeight(), I1.getWidth());
}
- unsigned char *ptr_I1 = I1.bitmap;
- unsigned char *ptr_I2 = I2.bitmap;
+ unsigned char *ptr_I1 = I1.bitmap;
+ unsigned char *ptr_I2 = I2.bitmap;
unsigned char *ptr_Ires = Ires.bitmap;
unsigned int cpt = 0;
#if VISP_HAVE_SSE2
- if (Ires.getSize() >= 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);
+ if (vpCPUFeatures::checkSSE2() && Ires.getSize() >= 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);
- _mm_storeu_si128( (__m128i*) ptr_Ires, vres );
+ _mm_storeu_si128((__m128i *)ptr_Ires, vres);
}
}
#endif
for (; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
- *ptr_Ires = saturate ? vpMath::saturate<unsigned char>( (short int) *ptr_I1 + (short int) *ptr_I2 ) : *ptr_I1 + *ptr_I2;
+ *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2;
}
}
@@ -322,57 +303,57 @@ vpImageTools::imageAdd(const vpImage<unsigned char> &I1,
\param I1 : The first image.
\param I2 : The second image.
\param Ires : \f$ Ires = I1 - I2 \f$
- \param saturate : If true, saturate the result to [0 ; 255] using vpMath::saturate, otherwise overflow may occur.
+ \param saturate : If true, saturate the result to [0 ; 255] using
+ vpMath::saturate, otherwise overflow may occur.
*/
-void
-vpImageTools::imageSubtract(const vpImage<unsigned char> &I1,
- const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires,
- const bool saturate)
+void vpImageTools::imageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ vpImage<unsigned char> &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"));
+ throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
Ires.resize(I1.getHeight(), I1.getWidth());
}
- unsigned char *ptr_I1 = I1.bitmap;
- unsigned char *ptr_I2 = I2.bitmap;
+ unsigned char *ptr_I1 = I1.bitmap;
+ unsigned char *ptr_I2 = I2.bitmap;
unsigned char *ptr_Ires = Ires.bitmap;
unsigned int cpt = 0;
#if VISP_HAVE_SSE2
- if (Ires.getSize() >= 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);
+ if (vpCPUFeatures::checkSSE2() && Ires.getSize() >= 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);
- _mm_storeu_si128( (__m128i*) ptr_Ires, vres );
+ _mm_storeu_si128((__m128i *)ptr_Ires, vres);
}
}
#endif
for (; cpt < Ires.getSize(); cpt++, ++ptr_I1, ++ptr_I2, ++ptr_Ires) {
- *ptr_Ires = saturate ? vpMath::saturate<unsigned char>( (short int) *ptr_I1 - (short int) *ptr_I2 ) : *ptr_I1 - *ptr_I2;
+ *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((short int)*ptr_I1 - (short int)*ptr_I2) : *ptr_I1 - *ptr_I2;
}
}
-// Reference: http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
-// t is a value that goes from 0 to 1 to interpolate in a C1 continuous way across uniformly sampled data points.
-// when t is 0, this will return B. 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 a = (-A + 3.0f*B - 3.0f*C + D) / 2.0f;
- float b = A + 2.0f*C - (5.0f*B + D) / 2.0f;
+// Reference:
+// http://blog.demofox.org/2015/08/15/resizing-images-with-bicubic-interpolation/
+// t is a value that goes from 0 to 1 to interpolate in a C1 continuous way
+// across uniformly sampled data points. when t is 0, this will return B.
+// 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 a = (-A + 3.0f * B - 3.0f * C + D) / 2.0f;
+ float b = A + 2.0f * C - (5.0f * B + D) / 2.0f;
float c = (-A + C) / 2.0f;
float d = B;
- return a*t*t*t + b*t*t + c*t + d;
+ 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 9a29146..bbebf77 100644
--- a/modules/core/src/image/vpRGBa.cpp
+++ b/modules/core/src/image/vpRGBa.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,20 +43,17 @@
images (it define a RGB 32 bits structure, fourth byte is not used - yet -)
*/
-
-#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
-
+#include <visp3/core/vpRGBa.h>
/*!
Copy operator (from an unsigned char value)
\param v : Input color ( R = G = B = v )
*/
-vpRGBa &
-vpRGBa::operator=(const unsigned char &v)
+vpRGBa &vpRGBa::operator=(const unsigned char &v)
{
this->R = v;
this->G = v;
@@ -67,8 +65,7 @@ vpRGBa::operator=(const unsigned char &v)
/*!
Copy operator.
*/
-vpRGBa &
-vpRGBa::operator=(const vpRGBa &v)
+vpRGBa &vpRGBa::operator=(const vpRGBa &v)
{
this->R = v.R;
this->G = v.G;
@@ -81,8 +78,7 @@ vpRGBa::operator=(const vpRGBa &v)
/*!
Move operator.
*/
-vpRGBa &
-vpRGBa::operator=(const vpRGBa &&v)
+vpRGBa &vpRGBa::operator=(const vpRGBa &&v)
{
this->R = std::move(v.R);
this->G = std::move(v.G);
@@ -101,11 +97,10 @@ vpRGBa::operator=(const vpRGBa &&v)
\exception vpException::dimensionError : If v is not a 4 four
dimention vector.
*/
-vpRGBa &
-vpRGBa::operator=(const vpColVector &v)
+vpRGBa &vpRGBa::operator=(const vpColVector &v)
{
if (v.getRows() != 4) {
- vpERROR_TRACE("Bad vector dimension ") ;
+ vpERROR_TRACE("Bad vector dimension ");
throw(vpException(vpException::dimensionError, "Bad vector dimension "));
}
R = (unsigned char)v[0];
@@ -131,25 +126,21 @@ bool vpRGBa::operator==(const vpRGBa &v)
if (A != v.A)
return false;
- return true ;
+ return true;
}
/*!
Compare two color pixels.
\return true if the images are different, false if they are the same.
*/
-bool vpRGBa::operator!=(const vpRGBa &v)
-{
- return (R != v.R || G != v.G || B != v.B || A != v.A);
-}
+bool vpRGBa::operator!=(const vpRGBa &v) { return (R != v.R || G != v.G || B != v.B || A != v.A); }
/*!
Substraction operator : "this" - v.
\param v : Color to substract to the current object "this".
\return "this" - v
*/
-vpColVector
-vpRGBa::operator-(const vpRGBa &v) const
+vpColVector vpRGBa::operator-(const vpRGBa &v) const
{
vpColVector n(4); // new color
n[0] = (double)R - (double)v.R;
@@ -165,14 +156,13 @@ vpRGBa::operator-(const vpRGBa &v) const
\return "this" + v
\warning in case of overflow : e.g. 128+128 returns 0 for all 4 channels
*/
-vpRGBa
-vpRGBa::operator+(const vpRGBa &v) const
+vpRGBa vpRGBa::operator+(const vpRGBa &v) const
{
vpRGBa n; // new color
- n.R = static_cast<unsigned char>( R + v.R );
- n.G = static_cast<unsigned char>( G + v.G );
- n.B = static_cast<unsigned char>( B + v.B );
- n.A = static_cast<unsigned char>( A + v.A );
+ n.R = static_cast<unsigned char>(R + v.R);
+ n.G = static_cast<unsigned char>(G + v.G);
+ n.B = static_cast<unsigned char>(B + v.B);
+ n.A = static_cast<unsigned char>(A + v.A);
return n;
}
@@ -181,8 +171,7 @@ vpRGBa::operator+(const vpRGBa &v) const
\param v : Color to substract to the current object "this".
\return "this" - v
*/
-vpColVector
-vpRGBa::operator-(const vpColVector &v) const
+vpColVector vpRGBa::operator-(const vpColVector &v) const
{
vpColVector n(4); // new color
n[0] = R - v[0];
@@ -197,8 +186,7 @@ vpRGBa::operator-(const vpColVector &v) const
\param v : Color to add to the current object "this".
\return "this" + v
*/
-vpColVector
-vpRGBa::operator+(const vpColVector &v) const
+vpColVector vpRGBa::operator+(const vpColVector &v) const
{
vpColVector n(4); // new color
n[0] = R + v[0];
@@ -213,8 +201,7 @@ vpRGBa::operator+(const vpColVector &v) const
\param v : Value to multiply.
\return v * "this"
*/
-vpColVector
-vpRGBa::operator*(const float &v) const
+vpColVector vpRGBa::operator*(const float &v) const
{
vpColVector n(4);
n[0] = R * v;
@@ -229,8 +216,7 @@ vpRGBa::operator*(const float &v) const
\param v : Value to multiply.
\return v * "this"
*/
-vpColVector
-vpRGBa::operator*(const double &v) const
+vpColVector vpRGBa::operator*(const double &v) const
{
vpColVector n(4);
n[0] = R * v;
@@ -240,28 +226,23 @@ vpRGBa::operator*(const double &v) const
return n;
}
-bool
-vpRGBa::operator<(const vpRGBa &v) const
+bool vpRGBa::operator<(const vpRGBa &v) const
{
- double gray1 = 0.2126*R+0.7152*G+0.0722*B;
- double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
+ double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B;
+ double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B;
- return (gray1 < gray2);
+ return (gray1 < gray2);
}
-bool
-vpRGBa::operator>(const vpRGBa &v) const
+bool vpRGBa::operator>(const vpRGBa &v) const
{
- double gray1 = 0.2126*R+0.7152*G+0.0722*B;
- double gray2 = 0.2126*v.R+0.7152*v.G+0.0722*v.B;
+ double gray1 = 0.2126 * R + 0.7152 * G + 0.0722 * B;
+ double gray2 = 0.2126 * v.R + 0.7152 * v.G + 0.0722 * v.B;
- return (gray1 > gray2);
+ return (gray1 > gray2);
}
-vpRGBa operator*(const double &x, const vpRGBa &rgb)
-{
- return rgb*x;
-}
+vpRGBa operator*(const double &x, const vpRGBa &rgb) { return rgb * x; }
/*!
@@ -272,7 +253,7 @@ vpRGBa operator*(const double &x, const vpRGBa &rgb)
coordinates are separated by a comma.
The following code prints the intensity of the pixel in the middle of the image:
- \code
+\code
#include <visp3/core/vpImage.h>
int main()
@@ -285,7 +266,7 @@ int main()
}
\endcode
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRGBa& rgba)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRGBa &rgba)
{
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 ef4eb3b..b385266 100644
--- a/modules/core/src/math/kalman/vpKalmanFilter.cpp
+++ b/modules/core/src/math/kalman/vpKalmanFilter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\file vpKalmanFilter.cpp
\brief Generic kalman filtering implementation.
@@ -49,47 +49,49 @@
/*!
Initialize the Kalman filter.
-
- \param size_state_vector : Size of the state vector \f${\bf x}_{k}\f$ for one signal.
+
+ \param size_state_vector : Size of the state vector \f${\bf x}_{k}\f$ for
+ one signal.
\param size_measure_vector : Size of the measure vector \f${\bf z}_{k}\f$
for one signal.
\param n_signal : Number of signal to filter.
*/
-void
-vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector,
- unsigned int n_signal)
+void vpKalmanFilter::init(unsigned int size_state_vector, unsigned int size_measure_vector, unsigned int n_signal)
{
this->size_state = size_state_vector;
- this->size_measure = size_measure_vector ;
- this->nsignal = n_signal ;
- F.resize(size_state*nsignal, size_state*nsignal) ;
- H.resize(size_measure*nsignal, size_state*nsignal) ;
+ this->size_measure = size_measure_vector;
+ this->nsignal = n_signal;
+ F.resize(size_state * nsignal, size_state * nsignal);
+ H.resize(size_measure * nsignal, size_state * nsignal);
- R.resize(size_measure*nsignal, size_measure*nsignal) ;
- Q.resize(size_state*nsignal, size_state*nsignal) ;
+ R.resize(size_measure * nsignal, size_measure * nsignal);
+ Q.resize(size_state * nsignal, size_state * nsignal);
- Xest.resize(size_state*nsignal) ; Xest = 0;
- Xpre.resize(size_state*nsignal) ; Xpre = 0 ;
+ Xest.resize(size_state * nsignal);
+ Xest = 0;
+ Xpre.resize(size_state * nsignal);
+ Xpre = 0;
- Pest.resize(size_state*nsignal, size_state*nsignal) ; Pest = 0 ;
+ Pest.resize(size_state * nsignal, size_state * nsignal);
+ Pest = 0;
- I.resize(size_state*nsignal, size_state*nsignal) ;
+ I.resize(size_state * nsignal, size_state * nsignal);
// init_done = false ;
- iter = 0 ;
- dt = -1 ;
+ iter = 0;
+ dt = -1;
}
/*!
Construct a default Kalman filter.
The verbose mode is by default desactivated.
-
+
*/
vpKalmanFilter::vpKalmanFilter()
- : iter(0), size_state(0), size_measure(0), nsignal(0), verbose_mode(false),
- Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
+ : 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()
{
}
@@ -97,12 +99,12 @@ vpKalmanFilter::vpKalmanFilter()
Construct a default Kalman filter by setting the number of signal to filter.
The verbose mode is by default desactivated.
-
+
\param n_signal : Number of signal to filter.
*/
vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
- : iter(0), size_state(0), size_measure(0), nsignal(n_signal), verbose_mode(false),
- Xest(), Xpre(), F(), H(), R(), Q(), dt(-1), Ppre(), Pest(), W(), I()
+ : 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()
{
}
@@ -110,8 +112,9 @@ vpKalmanFilter::vpKalmanFilter(unsigned int n_signal)
Construct a Kalman filter.
The verbose mode is by default desactivated.
-
- \param size_state_vector : Size of the state vector \f${\bf x}_{(k)}\f$ for one signal.
+
+ \param size_state_vector : Size of the state vector \f${\bf x}_{(k)}\f$ for
+ one signal.
\param size_measure_vector : Size of the measure vector \f${\bf z}_{(k)}\f$
for one signal.
@@ -119,10 +122,10 @@ 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()
+ : 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) ;
+ init(size_state_vector, size_measure_vector, n_signal);
}
/*!
@@ -134,47 +137,45 @@ vpKalmanFilter::vpKalmanFilter(unsigned int size_state_vector, unsigned int size
\f]
and the state prediction covariance by
\f[
- {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf F}^T_{k-1}
+ {\bf P}_{k \mid k-1} = {\bf F}_{k-1} {\bf P}_{k-1 \mid k-1} {\bf
+ F}^T_{k-1}
+ {\bf Q}_k
\f]
*/
-void
-vpKalmanFilter::prediction()
+void vpKalmanFilter::prediction()
{
- if (Xest.getRows() != size_state*nsignal) {
- std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()
- <<" " << size_state*nsignal<< std::endl ;
+ if (Xest.getRows() != size_state * nsignal) {
+ std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows() << " " << size_state * nsignal << std::endl;
std::cout << " Error : Filter non initialized " << std::endl;
- exit(1) ;
+ exit(1);
}
-// if (!init_done) {
-// std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<" " << size_state<< std::endl ;
-// std::cout << " Error : Filter non initialized " << std::endl;
-// exit(1) ;
-// return;
-// }
-
+ // if (!init_done) {
+ // std::cout << " in vpKalmanFilter::prediction()" << Xest.getRows()<<"
+ // " << size_state<< std::endl ; std::cout << " Error : Filter non
+ // initialized " << std::endl; exit(1) ; return;
+ // }
+
if (verbose_mode) {
- std::cout << "F = " << std::endl << F << std::endl ;
- std::cout << "Xest = "<< std::endl << Xest << std::endl ;
+ std::cout << "F = " << std::endl << F << std::endl;
+ std::cout << "Xest = " << std::endl << Xest << std::endl;
}
// Prediction
// Bar-Shalom 5.2.3.2
- Xpre = F*Xest ;
+ Xpre = F * Xest;
if (verbose_mode) {
- std::cout << "Xpre = "<< std::endl << Xpre << std::endl ;
- std::cout << "Q = "<< std::endl << Q << std::endl ;
- std::cout << "Pest " << std::endl << Pest << std::endl ;
+ std::cout << "Xpre = " << std::endl << Xpre << std::endl;
+ std::cout << "Q = " << std::endl << Q << std::endl;
+ std::cout << "Pest " << std::endl << Pest << std::endl;
}
// Bar-Shalom 5.2.3.5
- Ppre = F*Pest*F.t() + Q ;
+ Ppre = F * Pest * F.t() + Q;
// Matrice de covariance de l'erreur de prediction
- if (verbose_mode)
- std::cout << "Ppre " << std::endl << Ppre << std::endl ;
+ if (verbose_mode)
+ std::cout << "Ppre " << std::endl << Ppre << std::endl;
}
/*!
@@ -182,7 +183,8 @@ vpKalmanFilter::prediction()
Update the Kalman filter by applying the filtering equations and
increment the filter iteration (vpKalmanFilter::iter).
- \param z : Measure (or observation) \f${\bf z}_k\f$ provided at iteration \f$k\f$.
+ \param z : Measure (or observation) \f${\bf z}_k\f$ provided at iteration
+ \f$k\f$.
The filtering equation is given by:
\f[
@@ -196,10 +198,8 @@ vpKalmanFilter::prediction()
\f]
and where the updated covariance of the state is given by
\f[
- {\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k \mid k-1}
- \f]
- or in a symetric form
- \f[
+ {\bf P}_{k \mid k} = \left({\bf I} - {\bf W}_k {\bf H} \right) {\bf P}_{k
+ \mid k-1} \f] or in a symetric form \f[
{\bf P}_{k \mid k} = {\bf P}_{k \mid k-1} - {\bf W}_k {\bf S}_k {\bf W}^T_k
\f]
with
@@ -208,39 +208,37 @@ vpKalmanFilter::prediction()
\f]
*/
-void
-vpKalmanFilter::filtering(const vpColVector &z)
+void vpKalmanFilter::filtering(const vpColVector &z)
{
if (verbose_mode)
- std::cout << "z " << std::endl << z << std::endl ;
+ std::cout << "z " << std::endl << z << std::endl;
// Bar-Shalom 5.2.3.11
- vpMatrix S = H*Ppre*H.t() + R ;
+ vpMatrix S = H * Ppre * H.t() + R;
if (verbose_mode)
- std::cout << "S " << std::endl << S << std::endl ;
+ std::cout << "S " << std::endl << S << std::endl;
- W = (Ppre * H.t())* (S).inverseByLU() ;
+ W = (Ppre * H.t()) * (S).inverseByLU();
if (verbose_mode)
- std::cout << "W " << std::endl << W << std::endl ;
+ std::cout << "W " << std::endl << W << std::endl;
// Bar-Shalom 5.2.3.15
- Pest = Ppre - W*S*W.t() ;
+ Pest = Ppre - W * S * W.t();
if (verbose_mode)
- std::cout << "Pest " << std::endl << Pest << std::endl ;
+ std::cout << "Pest " << std::endl << Pest << std::endl;
if (0) {
// Bar-Shalom 5.2.3.16
// numeriquement plus stable que 5.2.3.15
- vpMatrix Pestinv ;
- Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU()*H ;
- Pest = Pestinv.inverseByLU() ;
+ vpMatrix Pestinv;
+ Pestinv = Ppre.inverseByLU() + H.t() * R.inverseByLU() * H;
+ Pest = Pestinv.inverseByLU();
}
// Bar-Shalom 5.2.3.12 5.2.3.13 5.2.3.7
- Xest = Xpre + (W*(z - (H*Xpre))) ;
+ Xest = Xpre + (W * (z - (H * Xpre)));
if (verbose_mode)
- std::cout << "Xest " << std::endl << Xest << std::endl ;
-
- iter++ ;
-}
+ std::cout << "Xest " << std::endl << Xest << std::endl;
+ iter++;
+}
#if 0
diff --git a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
index ffe1117..51caa27 100644
--- a/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
+++ b/modules/core/src/math/kalman/vpLinearKalmanFilterInstantiation.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,9 +42,9 @@
\brief Implementation of some specific Kalman filters.
*/
-#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
#include <math.h>
#include <stdlib.h>
@@ -60,7 +61,7 @@
\warning It is requiered to set the state model before using this method.
\param n_signal : Number of signal to filter.
-
+
\param sigma_state : Vector that contains the variance of the state
noise. The dimension of this vector is equal to the state vector
\f${\bf x}\f$ size multiplied by the number of signal to
@@ -73,14 +74,14 @@
to filter. Values are used to initialize the \f${\bf R}\f$ measure
covariance matrix.
- \param rho : Degree of correlation between successive accelerations. Values
+ \param rho : Degree of correlation between successive accelerations. Values
are in [0:1[.
\param delta_t : Sampling time \f$\Delta t\f$ expressed is
second. Depending on the filter modelization, this value may not be
used. This is for example the case for the
- vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel model
- implemented in initStateConstVelWithColoredNoise_MeasureVel().
+ vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel
+model implemented in initStateConstVelWithColoredNoise_MeasureVel().
\exception vpException::badValue : Bad rho value wich is not in [0:1[.
@@ -96,10 +97,10 @@
int main()
{
vpLinearKalmanFilterInstantiation kalman;
- // Select a constant velocity state model with colored noise
+ // Select a constant velocity state model with colored noise
// Measures are velocities
kalman.setStateModel(vpLinearKalmanFilterInstantiation::stateConstVelWithColoredNoise_MeasureVel);
-
+
// Initialise the filter for a one dimension signal
int signal = 1;
vpColVector sigma_state(2); // State vector size is 2
@@ -132,14 +133,14 @@ int main()
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
-
+
// Initialize the filter parameters:
- // - Firstly, the state variance
+ // - Firstly, the state variance
int size = state_size*nsignal;
vpColVector sigma_state(size);
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
+ // - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
@@ -161,7 +162,7 @@ int main()
// Compute the filtering and the prediction
kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
+ // Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
@@ -170,28 +171,22 @@ int main()
}
\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")) ;
+ vpERROR_TRACE("Kalman state model is not set");
+ throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
}
}
@@ -210,16 +205,16 @@ vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
\right.
\f]
- The terms \f${w_1}_{(k)}\f$ and \f${w_2}_{(k)}\f$ account for deviations from the assumed
- constant velocity trajectory. They are assumed zero-mean, white,
- mutually uncorrelated, stationary random variable with variance
- \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$.
+ The terms \f${w_1}_{(k)}\f$ and \f${w_2}_{(k)}\f$ account for deviations
+ from the assumed constant velocity trajectory. They are assumed zero-mean,
+ white, mutually uncorrelated, stationary random variable with variance
+ \f$\sigma^2_{Q_1}\f$ and \f$\sigma^2_{Q_2}\f$.
We recall that the recursive state evolution equation is given by
\f[
{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
\f]
-
+
From this state model, the transition matrix \f${\bf F}\f$ and the
state covariance matrix \f${\bf Q}\f$ are given by:
@@ -232,8 +227,8 @@ vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
\end{array}
\right]
\f]
-
- and
+
+ and
\f[
{\bf Q} = \sigma^2_Q
@@ -251,7 +246,7 @@ vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
\left[
\begin{array}{c}
z_{(0)}\\
- 0
+ 0
\end{array}
\right]
\f]
@@ -268,22 +263,20 @@ vpLinearKalmanFilterInstantiation::initFilter(unsigned int n_signal,
\f]
The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
+ matrix is given by:
\f[
{\bf P_{(0|0)}} =
\left[ \begin{array}{cc}
- \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\
- \frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t + \frac{\sigma^2_R}{2 \Delta t^2}
- \end{array}
- \right]
- \f]
+ \sigma^2_R & \frac{\sigma^2_R}{2 \Delta t}\\
+ \frac{\sigma^2_R}{2 \Delta t} & \frac{2}{3}\sigma^2_Q \Delta t +
+ \frac{\sigma^2_R}{2 \Delta t^2} \end{array} \right] \f]
\param n_signal : Number of signal to filter.
- \param sigma_state : Vector that fix the variance of the state covariance matrix
- \f$[\sigma^2_Q \; 0]^T\f$. The dimension of
- this vector is 2 multiplied by the number of signal to filter.
+ \param sigma_state : Vector that fix the variance of the state covariance
+ matrix \f$[\sigma^2_Q \; 0]^T\f$. The dimension of this vector is 2
+ multiplied by the number of signal to filter.
\param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
noise. The dimension of this vector is equal to the number of signal
@@ -292,11 +285,8 @@ 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);
@@ -306,45 +296,45 @@ vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_s
iter = 0;
Pest = 0;
Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
- this->dt = delta_t ;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
+ this->dt = delta_t;
- double dt2 = dt*dt ;
- double dt3 = dt2*dt ;
+ double dt2 = dt * dt;
+ double dt3 = dt2 * dt;
- for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
+ for (unsigned int i = 0; i < size_measure * n_signal; i++) {
// State model
// | 1 dt |
// F = | |
// | 0 1 |
- F[2*i][2*i] = 1 ;
- F[2*i][2*i+1] = dt ;
- F[2*i+1][2*i+1] = 1 ;
+ F[2 * i][2 * i] = 1;
+ F[2 * i][2 * i + 1] = dt;
+ F[2 * i + 1][2 * i + 1] = 1;
// Measure model
- H[i][2*i] = 1 ;
- H[i][2*i+1] = 0 ;
+ H[i][2 * i] = 1;
+ H[i][2 * i + 1] = 0;
- double sR = sigma_measure[i] ;
- double sQ = sigma_state[2*i] ; // sigma_state[2*i+1] is not used
+ double sR = sigma_measure[i];
+ double sQ = sigma_state[2 * i]; // sigma_state[2*i+1] is not used
- // Measure noise
- R[i][i] = sR ;
+ // Measure noise
+ R[i][i] = sR;
// State covariance matrix 6.2.2.12
- Q[2*i][2*i] = sQ * dt3/3;
- Q[2*i][2*i+1] = sQ * dt2/2;
- Q[2*i+1][2*i] = sQ * dt2/2;
- Q[2*i+1][2*i+1] = sQ * dt;
-
- Pest[2*i][2*i] = sR ;
- Pest[2*i][2*i+1] = sR/(2*dt) ;
- Pest[2*i+1][2*i] = sR/(2*dt) ;
- Pest[2*i+1][2*i+1] = sQ*2*dt/3.0+ sR/(2*dt2) ;
+ Q[2 * i][2 * i] = sQ * dt3 / 3;
+ Q[2 * i][2 * i + 1] = sQ * dt2 / 2;
+ Q[2 * i + 1][2 * i] = sQ * dt2 / 2;
+ Q[2 * i + 1][2 * i + 1] = sQ * dt;
+
+ Pest[2 * i][2 * i] = sR;
+ Pest[2 * i][2 * i + 1] = sR / (2 * dt);
+ Pest[2 * i + 1][2 * i] = sR / (2 * dt);
+ Pest[2 * i + 1][2 * i + 1] = sQ * 2 * dt / 3.0 + sR / (2 * dt2);
}
}
@@ -366,7 +356,7 @@ vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_s
\end{array}
\right.
\f]
-
+
The term \f$w_{(k)}\f$ account for deviations from the assumed
constant velocity trajectory. It is assumed zero-mean, white,
mutually uncorrelated, stationary random variable with variance
@@ -377,7 +367,7 @@ vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_s
\f[
{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
\f]
-
+
From this state model, the transition matrix \f${\bf F}\f$ and the
state covariance matrix \f${\bf Q}\f$ are given by:
@@ -390,8 +380,8 @@ vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_s
\end{array}
\right]
\f]
-
- and
+
+ and
\f[
{\bf Q} =
@@ -423,34 +413,34 @@ vpLinearKalmanFilterInstantiation::initStateConstVel_MeasurePos(unsigned int n_s
\left[
\begin{array}{c}
z_{(0)}\\
- 0
+ 0
\end{array}
\right]
\f]
The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
+ matrix is given by:
\f[
{\bf P_{(0|0)}} =
\left[ \begin{array}{cc}
- \sigma^2_R & 0\\
- 0 & \sigma^2_Q/(1-\rho^2)
- \end{array}
- \right]
+ \sigma^2_R & 0\\
+ 0 & \sigma^2_Q/(1-\rho^2)
+ \end{array}
+ \right]
\f]
-
+
\param n_signal : Number of signal to filter.
- \param sigma_state : Vector that fix the variance of the state covariance matrix
- \f$[0 \; \sigma^2_Q]^T\f$. The dimension of
- this vector is 2 multiplied by the number of signal to filter.
+ \param sigma_state : Vector that fix the variance of the state covariance
+matrix \f$[0 \; \sigma^2_Q]^T\f$. The dimension of this vector is 2 multiplied
+by the number of signal to filter.
\param sigma_measure : Variance \f$\sigma^2_R\f$ of the measurement
noise. The dimension of this vector is equal to the number of signal
to filter.
- \param rho : Degree of correlation between successive accelerations. Values
+ \param rho : Degree of correlation between successive accelerations. Values
are in [0:1[.
\exception vpException::badValue : Bad rho value wich is not in [0:1[.
@@ -467,13 +457,13 @@ int main()
vpLinearKalmanFilterInstantiation kalman;
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
-
+
// Initialize the filter parameters:
- // - Firstly, the state variance
+ // - Firstly, the state variance
vpColVector sigma_state(4); // 4 = 2 for the state size x 2 signal
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[3] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
+ // - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
@@ -492,7 +482,7 @@ int main()
// Compute the filtering and the prediction
kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
+ // Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
@@ -501,15 +491,14 @@ int main()
}
\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) ;
- throw(vpException(vpException::badValue, "Bad rho value")) ;
+ vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
+ throw(vpException(vpException::badValue, "Bad rho value"));
}
setStateModel(stateConstVelWithColoredNoise_MeasureVel);
@@ -519,41 +508,41 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
iter = 0;
Pest = 0;
Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
- for (unsigned int i=0; i < size_measure*n_signal ; i++ ) {
+ for (unsigned int i = 0; i < size_measure * n_signal; i++) {
// State model
// | 1 1 |
// F = | |
// | 0 rho |
- F[2*i][2*i] = 1 ;
- F[2*i][2*i+1] = 1 ;
- F[2*i+1][2*i+1] = rho ;
+ F[2 * i][2 * i] = 1;
+ F[2 * i][2 * i + 1] = 1;
+ F[2 * i + 1][2 * i + 1] = rho;
// Measure model
- H[i][2*i] = 1 ;
- H[i][2*i+1] = 0 ;
+ H[i][2 * i] = 1;
+ H[i][2 * i + 1] = 0;
- double sR = sigma_measure[i] ;
- double sQ = sigma_state[2*i+1] ; // sigma_state[2*i] is not used
+ double sR = sigma_measure[i];
+ double sQ = sigma_state[2 * i + 1]; // sigma_state[2*i] is not used
- // Measure noise
- R[i][i] = sR ;
+ // Measure noise
+ R[i][i] = sR;
// State covariance matrix
- Q[2*i][2*i] = 0 ;
- Q[2*i][2*i+1] = 0;
- Q[2*i+1][2*i] = 0;
- Q[2*i+1][2*i+1] = sQ ;
-
- Pest[2*i][2*i] = sR ;
- Pest[2*i][2*i+1] = 0. ;
- Pest[2*i+1][2*i] = 0 ;
- Pest[2*i+1][2*i+1] = sQ/(1-rho*rho) ;
+ Q[2 * i][2 * i] = 0;
+ Q[2 * i][2 * i + 1] = 0;
+ Q[2 * i + 1][2 * i] = 0;
+ Q[2 * i + 1][2 * i + 1] = sQ;
+
+ Pest[2 * i][2 * i] = sR;
+ Pest[2 * i][2 * i + 1] = 0.;
+ Pest[2 * i + 1][2 * i] = 0;
+ Pest[2 * i + 1][2 * i + 1] = sQ / (1 - rho * rho);
}
}
@@ -576,7 +565,7 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
\end{array}
\right.
\f]
-
+
The terms \f$w_{1(k)}\f$ and \f$w_{2(k)}\f$ account for deviations
from the assumed constant acceleration trajectory. They are assumed
zero-mean, white, mutually uncorrelated, stationary random variable
@@ -588,7 +577,7 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
\f[
{\bf x}_k= {\bf F}_{k-1} {\bf x}_{k-1} + {\bf w}_{k-1} \\
\f]
-
+
From this state model, the transition matrix \f${\bf F}\f$ and the
state covariance matrix \f${\bf Q}\f$ are given by:
@@ -602,8 +591,8 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
\end{array}
\right]
\f]
-
- and
+
+ and
\f[
{\bf Q} =
@@ -621,9 +610,9 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
z_{(k)} = {\bf H} {\bf x}_{(k)} + r_{(k)}
\f]
- where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the velocity and \f$r_{(k)}\f$
- is the measurement noise, assumed zero-mean, white mutually
- uncorrelated stationary random variables with variance
+ where \f${\bf H} = [1 \; 0 \; 0]\f$, \f$z_{(k)}\f$ is the measure of the
+velocity and \f$r_{(k)}\f$ is the measurement noise, assumed zero-mean, white
+mutually uncorrelated stationary random variables with variance
\f$\sigma^2_R\f$, giving the covariance matrix:
\f[
@@ -643,20 +632,18 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
\f]
The initial value \f$P_{(0|0)}\f$ of the prediction covariance
- matrix is given by:
+ matrix is given by:
\f[
{\bf P_{(0|0)}} =
\left[ \begin{array}{ccc}
- \sigma^2_R & 0 & \sigma^2_R / \Delta t\\
+ \sigma^2_R & 0 & \sigma^2_R / \Delta t\\
0 & \sigma^2_{Q_1}/(1-\rho^2) & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t \\
- \sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t & (2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2
- \end{array}
- \right]
- \f]
-
+ \sigma^2_R / \Delta t & -\rho \sigma^2_{Q_1} / (1-\rho^2)\Delta t &
+(2\sigma^2_R +\sigma^2_{Q_1}/(1-\rho^2) )/\Delta t^2 \end{array} \right] \f]
+
\param n_signal : Number of signal to filter.
-
+
\param sigma_state : Vector that fix the variance of the state
covariance matrix \f$[0 \; \sigma^2_{Q_1} \;
\sigma^2_{Q_2}]^T\f$. The dimension of this vector is 3 multiplied
@@ -666,7 +653,7 @@ vpLinearKalmanFilterInstantiation::initStateConstVelWithColoredNoise_MeasureVel(
noise. The dimension of this vector is equal to the number of signal
to filter.
- \param rho : Degree of correlation between successive accelerations. Values
+ \param rho : Degree of correlation between successive accelerations. Values
are in [0:1[.
\param delta_t : Sampling time \f$\Delta t\f$ expressed is second.
@@ -685,15 +672,15 @@ int main()
vpLinearKalmanFilterInstantiation kalman;
// Filter the x and y velocities of a target (2 signals are to consider)
int nsignal = 2;
-
+
// Initialize the filter parameters:
- // - Firstly, the state variance
+ // - Firstly, the state variance
vpColVector sigma_state(6); // 6 = 3 for the state size x 2 signal
sigma_state[1] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[2] = 0.001; // Variance on the acceleration for the 1st signal (x)
sigma_state[4] = 0.002; // Variance on the acceleration for the 2nd signal (y)
sigma_state[5] = 0.002; // Variance on the acceleration for the 2nd signal (y)
- // - Secondly, the measures variance
+ // - Secondly, the measures variance
vpColVector sigma_measure(nsignal); // 2 velocity measures available
sigma_measure[0] = 0.03; // Variance on the x velocity measure
sigma_measure[1] = 0.06; // Variance on the y velocity measure
@@ -714,7 +701,7 @@ int main()
// Compute the filtering and the prediction
kalman.filter(vm);
- // Print the estimation of the velocities (1st value of the state vector)
+ // Print the estimation of the velocities (1st value of the state vector)
std::cout << "Estimated x velocity: kalman.Xest[0]" << std::endl;
std::cout << "Estimated y velocity: kalman.Xest[kalman.getStateSize()]"
<< std::endl;
@@ -724,16 +711,14 @@ int main()
\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) ;
- throw(vpException(vpException::badValue, "Bad rho value")) ;
+ vpERROR_TRACE("Bad rho value %g; should be in [0:1[", rho);
+ throw(vpException(vpException::badValue, "Bad rho value"));
}
setStateModel(stateConstAccWithColoredNoise_MeasureVel);
@@ -742,50 +727,50 @@ vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(
iter = 0;
Pest = 0;
Xest = 0;
- F = 0;
- H = 0;
- R = 0;
- Q = 0;
+ F = 0;
+ H = 0;
+ R = 0;
+ Q = 0;
this->dt = delta_t;
// initialise les matrices decrivant les modeles
- for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
+ for (unsigned int i = 0; i < size_measure * nsignal; i++) {
// State model
// | 1 1 dt |
// F = | o rho 0 |
// | 0 0 1 |
- F[3*i][3*i] = 1 ;
- F[3*i][3*i+1] = 1 ;
- F[3*i][3*i+2] = dt ;
- F[3*i+1][3*i+1] = rho ;
- F[3*i+2][3*i+2] = 1 ;
+ F[3 * i][3 * i] = 1;
+ F[3 * i][3 * i + 1] = 1;
+ F[3 * i][3 * i + 2] = dt;
+ F[3 * i + 1][3 * i + 1] = rho;
+ F[3 * i + 2][3 * i + 2] = 1;
// Measure model
- H[i][3*i] = 1 ;
- H[i][3*i+1] = 0 ;
- H[i][3*i+2] = 0 ;
+ H[i][3 * i] = 1;
+ H[i][3 * i + 1] = 0;
+ H[i][3 * i + 2] = 0;
- double sR = sigma_measure[i] ;
- double sQ1 = sigma_state[3*i+1] ;
- double sQ2 = sigma_state[3*i+2] ;
+ double sR = sigma_measure[i];
+ double sQ1 = sigma_state[3 * i + 1];
+ double sQ2 = sigma_state[3 * i + 2];
- // Measure noise
- R[i][i] = sR ;
+ // Measure noise
+ R[i][i] = sR;
// State covariance matrix
- Q[3*i+1][3*i+1] = sQ1;
- Q[3*i+2][3*i+2] = sQ2;
-
- Pest[3*i][3*i] = sR ;
- Pest[3*i][3*i+1] = 0. ;
- Pest[3*i][3*i+2] = sR/dt ;
- Pest[3*i+1][3*i+1] = sQ1/(1-rho*rho) ;
- Pest[3*i+1][3*i+2] = -rho*sQ1/((1-rho*rho)*dt) ;
- Pest[3*i+2][3*i+2] = (2*sR+sQ1/(1-rho*rho))/(dt*dt) ;
+ Q[3 * i + 1][3 * i + 1] = sQ1;
+ Q[3 * i + 2][3 * i + 2] = sQ2;
+
+ Pest[3 * i][3 * i] = sR;
+ Pest[3 * i][3 * i + 1] = 0.;
+ Pest[3 * i][3 * i + 2] = sR / dt;
+ Pest[3 * i + 1][3 * i + 1] = sQ1 / (1 - rho * rho);
+ Pest[3 * i + 1][3 * i + 2] = -rho * sQ1 / ((1 - rho * rho) * dt);
+ Pest[3 * i + 2][3 * i + 2] = (2 * sR + sQ1 / (1 - rho * rho)) / (dt * dt);
// complete the lower triangle
- Pest[3*i+1][3*i] = Pest[3*i][3*i+1];
- Pest[3*i+2][3*i] = Pest[3*i][3*i+2];
- Pest[3*i+2][3*i+1] = Pest[3*i+1][3*i+2];
+ Pest[3 * i + 1][3 * i] = Pest[3 * i][3 * i + 1];
+ Pest[3 * i + 2][3 * i] = Pest[3 * i][3 * i + 2];
+ Pest[3 * i + 2][3 * i + 1] = Pest[3 * i + 1][3 * i + 2];
}
}
@@ -793,22 +778,20 @@ vpLinearKalmanFilterInstantiation::initStateConstAccWithColoredNoise_MeasureVel(
Do the filtering and prediction of the measure signal.
- \param z : Measures \f${\bf z}_k\f$ used to initialise the filter. The dimension of
- this vector is equal to the number of signal to filter (given by
- getNumberOfSignal()) multiplied by the size of the measure vector
- (given by getMeasureSize()) .
-
+ \param z : Measures \f${\bf z}_k\f$ used to initialise the filter. The
+ dimension of this vector is equal to the number of signal to filter (given
+ by getNumberOfSignal()) multiplied by the size of the measure vector (given
+ by getMeasureSize()) .
+
\exception vpException::notInitialized : If the filter is not
initialized. To initialize the filter see initFilter().
*/
-void
-vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
+void vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
{
if (nsignal < 1) {
- vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter") ;
- throw(vpException(vpException::notInitialized,
- "Bad signal number")) ;
+ vpERROR_TRACE("Bad signal number. You need to initialize the Kalman filter");
+ throw(vpException(vpException::notInitialized, "Bad signal number"));
}
// Specific initialization of the filter that depends on the state model
@@ -818,37 +801,35 @@ vpLinearKalmanFilterInstantiation::filter(vpColVector &z)
case stateConstVel_MeasurePos:
case stateConstVelWithColoredNoise_MeasureVel:
case stateConstAccWithColoredNoise_MeasureVel:
- for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
- Xest[size_state*i] = z[i] ;
+ for (unsigned int i = 0; i < size_measure * nsignal; i++) {
+ Xest[size_state * i] = z[i];
}
prediction();
// init_done = true;
break;
case unknown:
- vpERROR_TRACE("Kalman state model is not set") ;
- throw(vpException(vpException::notInitialized, "Kalman state model is not set")) ;
+ vpERROR_TRACE("Kalman state model is not set");
+ throw(vpException(vpException::notInitialized, "Kalman state model is not set"));
break;
}
- iter ++;
+ iter++;
return;
- }
- else if (iter == 1) {
+ } else if (iter == 1) {
if (model == stateConstVel_MeasurePos) {
- for (unsigned int i=0; i < size_measure*nsignal ; i++ ) {
- double z_prev = Xest[size_state*i]; // Previous mesured position
- // std::cout << "Mesure pre: " << z_prev << std::endl;
- Xest[size_state*i] = z[i] ;
- Xest[size_state*i+1] = (z[i] - z_prev) / dt ;
+ for (unsigned int i = 0; i < size_measure * nsignal; i++) {
+ double z_prev = Xest[size_state * i]; // Previous mesured position
+ // std::cout << "Mesure pre: " << z_prev << std::endl;
+ Xest[size_state * i] = z[i];
+ Xest[size_state * i + 1] = (z[i] - z_prev) / dt;
}
prediction();
- iter ++;
-
+ iter++;
+
return;
}
}
filtering(z);
prediction();
-
}
diff --git a/modules/core/src/math/matrix/vpColVector.cpp b/modules/core/src/math/matrix/vpColVector.cpp
index d7e39aa..b97d726 100644
--- a/modules/core/src/math/matrix/vpColVector.cpp
+++ b/modules/core/src/math/matrix/vpColVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,47 +36,43 @@
*
*****************************************************************************/
-
/*!
\file vpColVector.cpp
\brief Class that provides a data structure for the column vectors as well
as a set of operations on these vectors
*/
+#include <assert.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <math.h>
+#include <sstream>
#include <stdio.h>
-#include <string.h>
#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#include <string.h>
-#include <math.h>
-#include <sstream>
-#include <assert.h>
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpRotationVector.h>
#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
-# include <emmintrin.h>
-# define VISP_HAVE_SSE2 1
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
#endif
-
//! Operator that allows to add two column vectors.
-vpColVector
-vpColVector::operator+(const vpColVector &v) const
+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())) ;
+ if (getRows() != v.getRows()) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
+ v.getRows()));
}
vpColVector r(rowNum);
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
r[i] = (*this)[i] + v[i];
return r;
}
@@ -84,11 +81,14 @@ 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
+ \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;
+ v[0] = 4;
+ v[1] = 5;
+ v[2] = 6;
vpTranslationVector t2;
t2 = v + t1;
@@ -97,50 +97,45 @@ vpColVector::operator+(const vpColVector &v) const
\endcode
*/
-vpTranslationVector
-vpColVector::operator+(const vpTranslationVector &t) 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;
- for (unsigned int i=0;i<3;i++) s[i] = (*this)[i]+t[i] ;
+ for (unsigned int i = 0; i < 3; i++)
+ s[i] = (*this)[i] + t[i];
return s;
}
//! Operator that allows to add two column vectors.
-vpColVector &
-vpColVector::operator+=(vpColVector v)
+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())) ;
+ if (getRows() != v.getRows()) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx1) column vector to (%dx1) column vector", getRows(),
+ v.getRows()));
}
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] += v[i];
return (*this);
}
//! Operator that allows to substract two column vectors.
-vpColVector &
-vpColVector::operator-=(vpColVector v)
+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())) ;
+ if (getRows() != v.getRows()) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (%dx1) column vector to (%dx1) column vector",
+ getRows(), v.getRows()));
}
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] -= v[i];
return (*this);
}
-
/*!
Operator that performs the dot product between two column vectors.
@@ -148,18 +143,18 @@ vpColVector::operator-=(vpColVector v)
\sa dotProd()
*/
-double
-vpColVector::operator*(const vpColVector &v) const
+double vpColVector::operator*(const vpColVector &v) const
{
if (size() != v.size()) {
throw(vpException(vpException::dimensionError,
- "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
+ "Cannot compute the dot product between column vectors "
+ "with different dimensions (%d) and (%d)",
size(), v.size()));
}
- double r = 0 ;
+ double r = 0;
- for (unsigned int i=0;i<rowNum;i++)
- r += (*this)[i] * v[i];
+ for (unsigned int i = 0; i < rowNum; i++)
+ r += (*this)[i] * v[i];
return r;
}
@@ -175,8 +170,8 @@ vpColVector::operator*(const vpColVector &v) const
vpMatrix vpColVector::operator*(const vpRowVector &v) const
{
vpMatrix M(rowNum, v.getCols());
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<v.getCols(); j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < v.getCols(); j++) {
M[i][j] = (*this)[i] * v[j];
}
}
@@ -186,14 +181,15 @@ vpMatrix vpColVector::operator*(const vpRowVector &v) const
//! operator substraction of two vectors V = A-v
vpColVector vpColVector::operator-(const vpColVector &m) const
{
- if (getRows() != m.getRows() ) {
+ if (getRows() != m.getRows()) {
throw(vpException(vpException::dimensionError,
- "Bad size during vpColVector (%dx1) and vpColVector (%dx1) substraction",
- getRows(), m.getRows())) ;
+ "Bad size during vpColVector (%dx1) and vpColVector "
+ "(%dx1) substraction",
+ getRows(), m.getRows()));
}
vpColVector v(rowNum);
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
v[i] = (*this)[i] - m[i];
return v;
}
@@ -202,16 +198,16 @@ vpColVector vpColVector::operator-(const vpColVector &m) const
Construct a column vector from a part of an input column vector \e v.
\param v : Input column vector used for initialization.
- \param r : row index in \e v that corresponds to the first element of the column vector to contruct.
- \param nrows : Number of rows of the constructed column vector.
+ \param r : row index in \e v that corresponds to the first element of the
+ column vector to contruct. \param nrows : Number of rows of the constructed
+ column vector.
The sub-vector starting from v[r] element and ending on v[r+nrows-1] element
is used to initialize the contructed column vector.
\sa init()
*/
-vpColVector::vpColVector (const vpColVector &v, unsigned int r, unsigned int nrows)
- : vpArray2D<double>(nrows, 1)
+vpColVector::vpColVector(const vpColVector &v, unsigned int r, unsigned int nrows) : vpArray2D<double>(nrows, 1)
{
init(v, r, nrows);
}
@@ -220,8 +216,10 @@ vpColVector::vpColVector (const vpColVector &v, unsigned int r, unsigned int nro
Initialize the column vector from a part of an input column vector \e v.
\param v : Input column vector used for initialization.
- \param r : row index in \e v that corresponds to the first element of the column vector to contruct.
- \param nrows : Number of rows of the constructed column vector.
+ \param r : row index in \e v that corresponds to the first element of the
+ column vector to contruct.
+ \param nrows : Number of rows of the constructed
+ column vector.
The sub-vector starting from v[r] element and ending on v[r+nrows-1] element
is used to initialize the contructed column vector.
@@ -251,89 +249,97 @@ 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 ;
+ unsigned int rnrows = r + nrows;
if (rnrows > v.getRows())
- throw(vpException(vpException::dimensionError,
- "Bad row dimension (%d > %d) used to initialize vpColVector",
- rnrows, v.getRows()));
- resize(nrows);
+ 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
- return; // Noting to do
- for (unsigned int i=r ; i < rnrows; i++)
- (*this)[i-r] = v[i];
+ return; // Nothing to do
+ for (unsigned int i = r; i < rnrows; i++)
+ (*this)[i - r] = v[i];
}
-vpColVector::vpColVector (const vpRotationVector &v)
- : vpArray2D<double>(v.size(), 1)
+vpColVector::vpColVector(const vpRotationVector &v) : vpArray2D<double>(v.size(), 1)
{
- for (unsigned int i=0; i< v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = v[i];
}
-vpColVector::vpColVector (const vpPoseVector &p)
- : vpArray2D<double>(p.size(), 1)
+vpColVector::vpColVector(const vpPoseVector &p) : vpArray2D<double>(p.size(), 1)
{
- for (unsigned int i=0; i< p.size(); i++)
+ for (unsigned int i = 0; i < p.size(); i++)
(*this)[i] = p[i];
}
-vpColVector::vpColVector (const vpTranslationVector &v)
- : vpArray2D<double>(v.size(), 1)
+vpColVector::vpColVector(const vpTranslationVector &v) : vpArray2D<double>(v.size(), 1)
{
- for (unsigned int i=0; i< v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = v[i];
}
//! Constructor that take column j of matrix M.
-vpColVector::vpColVector (const vpMatrix &M, unsigned int j)
- : vpArray2D<double>(M.getRows(), 1)
+vpColVector::vpColVector(const vpMatrix &M, unsigned int j) : vpArray2D<double>(M.getRows(), 1)
{
- for(unsigned int i=0; i< M.getCols(); i++)
+ for (unsigned int i = 0; i < M.getCols(); i++)
(*this)[i] = M[i][j];
}
-
+
/*!
Constructor that creates a column vector from a m-by-1 matrix \e M.
- \exception vpException::dimensionError If the matrix is not a m-by-1 matrix.
+ \exception vpException::dimensionError If the matrix is not a m-by-1
+ matrix.
*/
-vpColVector::vpColVector (const vpMatrix &M)
- : vpArray2D<double>(M.getRows(), 1)
+vpColVector::vpColVector(const vpMatrix &M) : vpArray2D<double>(M.getRows(), 1)
{
- if(M.getCols()!=1) {
- throw(vpException(vpException::dimensionError,
- "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
- M.getRows(), M.getRows(), M.getCols())) ;
+ if (M.getCols() != 1) {
+ throw(vpException(vpException::dimensionError, "Cannot construct a (%dx1) row vector from a (%dx%d) matrix",
+ M.getRows(), M.getRows(), M.getCols()));
}
- for(unsigned int i=0; i< M.getRows(); i++)
+ for (unsigned int i = 0; i < M.getRows(); i++)
(*this)[i] = M[i][0];
}
/*!
Constructor that creates a column vector from a std vector of double.
*/
-vpColVector::vpColVector (const std::vector<double> &v)
- : vpArray2D<double>((unsigned int)v.size(), 1)
+vpColVector::vpColVector(const std::vector<double> &v) : vpArray2D<double>((unsigned int)v.size(), 1)
{
- for(unsigned int i=0; i< v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = v[i];
}
/*!
Constructor that creates a column vector from a std vector of float.
*/
-vpColVector::vpColVector (const std::vector<float> &v)
- : vpArray2D<double>((unsigned int)v.size(), 1)
+vpColVector::vpColVector(const std::vector<float> &v) : vpArray2D<double>((unsigned int)v.size(), 1)
{
- for(unsigned int i=0; i< v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = (double)(v[i]);
}
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+vpColVector::vpColVector(vpColVector &&v) : vpArray2D<double>()
+{
+ rowNum = v.rowNum;
+ colNum = v.colNum;
+ rowPtrs = v.rowPtrs;
+ dsize = v.dsize;
+ data = v.data;
+
+ v.rowNum = 0;
+ v.colNum = 0;
+ v.rowPtrs = NULL;
+ v.dsize = 0;
+ v.data = NULL;
+}
+#endif
+
/*!
Operator that allows to negate all the column vector elements.
@@ -346,26 +352,21 @@ vpColVector::vpColVector (const std::vector<float> &v)
*/
vpColVector vpColVector::operator-() const
{
- vpColVector A ;
- try {
- A.resize(rowNum) ;
- }
- catch(vpException &/*e*/)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
+ vpColVector A;
+ A.resize(rowNum, false);
- double *vd = A.data ; double *d = data ;
+ double *vd = A.data;
+ double *d = data;
- for (unsigned int i=0; i<rowNum; i++)
- *(vd++)= - (*d++);
+ for (unsigned int i = 0; i < rowNum; i++)
+ *(vd++) = -(*d++);
return A;
}
/*!
- Operator that allows to multiply each element of a column vector by a scalar.
+ Operator that allows to multiply each element of a column vector by a
+ scalar.
\param x : The scalar.
@@ -387,15 +388,17 @@ vpColVector vpColVector::operator*(double x) const
{
vpColVector v(rowNum);
- double *vd = v.data ; double *d = data ;
+ double *vd = v.data;
+ double *d = data;
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
*(vd++) = (*d++) * x;
return v;
}
/*!
- Operator that allows to multiply each element of a column vector by a scalar.
+ Operator that allows to multiply each element of a column vector by a
+ scalar.
\param x : The scalar.
@@ -413,7 +416,7 @@ vpColVector vpColVector::operator*(double x) const
*/
vpColVector &vpColVector::operator*=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] *= x;
return (*this);
}
@@ -437,7 +440,7 @@ vpColVector &vpColVector::operator*=(double x)
*/
vpColVector &vpColVector::operator/=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] /= x;
return (*this);
}
@@ -465,9 +468,10 @@ vpColVector vpColVector::operator/(double x) const
{
vpColVector v(rowNum);
- double *vd = v.data ; double *d = data ;
+ double *vd = v.data;
+ double *d = data;
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
*(vd++) = (*d++) / x;
return v;
}
@@ -479,20 +483,13 @@ 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()));
- }
-
- try {
- resize(M.getRows());
- }
- catch(...) {
- throw ;
+ if (M.getCols() != 1) {
+ throw(vpException(vpException::dimensionError, "Cannot transform a (%dx%d) matrix into a column vector",
+ M.getRows(), M.getCols()));
}
- memcpy(data, M.data, rowNum*sizeof(double)) ;
+ resize(M.getRows(), false);
+ memcpy(data, M.data, rowNum * sizeof(double));
return (*this);
}
@@ -500,38 +497,32 @@ vpColVector &vpColVector::operator=(const vpMatrix &M)
/*!
Initialize a row vector from a standard vector of double.
*/
-vpColVector & vpColVector::operator=(const std::vector<double> &v)
+vpColVector &vpColVector::operator=(const std::vector<double> &v)
{
- resize((unsigned int)v.size());
- for(unsigned int i=0; i<v.size(); i++)
+ resize((unsigned int)v.size(), false);
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = v[i];
return *this;
}
/*!
Initialize a row vector from a standard vector of double.
*/
-vpColVector & vpColVector::operator=(const std::vector<float> &v)
+vpColVector &vpColVector::operator=(const std::vector<float> &v)
{
- resize((unsigned int)v.size());
- for(unsigned int i=0; i<v.size(); i++)
+ resize((unsigned int)v.size(), false);
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = (float)v[i];
return *this;
}
vpColVector &vpColVector::operator=(const vpColVector &v)
{
- unsigned int k = v.rowNum ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(...)
- {
- throw ;
- }
+ unsigned int k = v.rowNum;
+ if (rowNum != k) {
+ resize(k, false);
}
- memcpy(data, v.data, rowNum*sizeof(double)) ;
+ memcpy(data, v.data, rowNum * sizeof(double));
return *this;
}
@@ -540,18 +531,12 @@ vpColVector &vpColVector::operator=(const vpColVector &v)
*/
vpColVector &vpColVector::operator=(const vpTranslationVector &tv)
{
- unsigned int k = tv.getRows() ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(...)
- {
- throw ;
- }
+ unsigned int k = tv.getRows();
+ if (rowNum != k) {
+ resize(k, false);
}
- memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ memcpy(data, tv.data, rowNum * sizeof(double));
return *this;
}
/*!
@@ -559,18 +544,12 @@ vpColVector &vpColVector::operator=(const vpTranslationVector &tv)
*/
vpColVector &vpColVector::operator=(const vpRotationVector &rv)
{
- unsigned int k = rv.getRows() ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(...)
- {
- throw ;
- }
+ unsigned int k = rv.getRows();
+ if (rowNum != k) {
+ resize(k, false);
}
- memcpy(data, rv.data, rowNum*sizeof(double)) ;
+ memcpy(data, rv.data, rowNum * sizeof(double));
return *this;
}
/*!
@@ -578,18 +557,12 @@ vpColVector &vpColVector::operator=(const vpRotationVector &rv)
*/
vpColVector &vpColVector::operator=(const vpPoseVector &p)
{
- unsigned int k = p.getRows() ;
- if (rowNum != k){
- try {
- resize(k);
- }
- catch(...)
- {
- throw ;
- }
+ unsigned int k = p.getRows();
+ if (rowNum != k) {
+ resize(k, false);
}
- memcpy(data, p.data, rowNum*sizeof(double)) ;
+ memcpy(data, p.data, rowNum * sizeof(double));
return *this;
}
@@ -613,7 +586,7 @@ int main()
A: 0 1 2 3 4
\endcode
*/
-vpColVector & vpColVector::operator<<(const vpColVector &v)
+vpColVector &vpColVector::operator<<(const vpColVector &v)
{
*this = v;
return *this;
@@ -643,10 +616,10 @@ int main()
A: 0 1 2 3 4
\endcode
*/
-vpColVector & vpColVector::operator<<( double *x )
+vpColVector &vpColVector::operator<<(double *x)
{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
rowPtrs[i][j] = *x++;
}
}
@@ -654,14 +627,38 @@ vpColVector & vpColVector::operator<<( double *x )
}
//! Set each element of the column vector to x.
-vpColVector & vpColVector::operator=(double x)
+vpColVector &vpColVector::operator=(double x)
+{
+ double *d = data;
+
+ for (unsigned int i = 0; i < rowNum; i++)
+ *(d++) = x;
+ return *this;
+}
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+vpColVector &vpColVector::operator=(vpColVector &&other)
{
- double *d = data ;
+ if (this != &other) {
+ free(data);
+ free(rowPtrs);
+
+ rowNum = other.rowNum;
+ colNum = other.colNum;
+ rowPtrs = other.rowPtrs;
+ dsize = other.dsize;
+ data = other.data;
+
+ other.rowNum = 0;
+ other.colNum = 0;
+ other.rowPtrs = NULL;
+ other.dsize = 0;
+ other.data = NULL;
+ }
- for (unsigned int i=0;i<rowNum;i++)
- *(d++)= x ;
return *this;
}
+#endif
/*!
Transpose the column vector. The resulting vector becomes a row vector.
@@ -669,7 +666,7 @@ vpColVector & vpColVector::operator=(double x)
vpRowVector vpColVector::t() const
{
vpRowVector v(rowNum);
- memcpy(v.data, data, rowNum*sizeof(double)) ;
+ memcpy(v.data, data, rowNum * sizeof(double));
return v;
}
@@ -677,20 +674,13 @@ vpRowVector vpColVector::t() const
Transpose the column vector. The resulting vector becomes a row vector.
\sa t()
*/
-vpRowVector vpColVector::transpose() const
-{
- return t();
-}
+vpRowVector vpColVector::transpose() const { return t(); }
/*!
Transpose the column vector. The resulting vector \e v becomes a row vector.
\sa t()
*/
-void vpColVector::transpose(vpRowVector &v) const
-{
- v = t();
-}
-
+void vpColVector::transpose(vpRowVector &v) const { v = t(); }
/*!
\relates vpColVector
@@ -698,42 +688,42 @@ void vpColVector::transpose(vpRowVector &v) const
*/
vpColVector operator*(const double &x, const vpColVector &v)
{
- vpColVector vout ;
- vout = v*x ;
- return vout ;
+ vpColVector vout;
+ vout = v * x;
+ return vout;
}
/*!
Compute end return the dot product of two column vectors:
- \f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of both vectors.
+ \f[ a \cdot b = \sum_{i=0}^n a_i * b_i\f] where \e n is the dimension of
+ both vectors.
\exception vpException::dimensionError If the vector dimension differ.
*/
-double
-vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
+double vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{
- if (a.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute the dot product: first vector empty")) ;
+ if (a.data == NULL) {
+ throw(vpException(vpException::fatalError, "Cannot compute the dot product: first vector empty"));
}
- if (b.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute the dot product: second vector empty")) ;
+ if (b.data == NULL) {
+ throw(vpException(vpException::fatalError, "Cannot compute the dot product: second vector empty"));
}
if (a.size() != b.size()) {
throw(vpException(vpException::dimensionError,
- "Cannot compute the dot product between column vectors with different dimensions (%d) and (%d)",
+ "Cannot compute the dot product between column vectors "
+ "with different dimensions (%d) and (%d)",
a.size(), b.size()));
}
- double *ad = a.data ; double *bd = b.data ;
+ double *ad = a.data;
+ double *bd = b.data;
- double c = 0 ;
- for (unsigned int i=0 ; i < a.getRows() ; i++)
- c += *(ad++)* *(bd++) ;
+ double c = 0;
+ for (unsigned int i = 0; i < a.getRows(); i++)
+ c += *(ad++) * *(bd++);
// vpMatrix c = (a.t() * b);
// return c[0][0];
- return c ;
+ return c;
}
/*!
@@ -743,13 +733,12 @@ vpColVector::dotProd(const vpColVector &a, const vpColVector &b)
{\bf x}_i = \frac{{\bf x}_i}{\sqrt{\sum_{i=1}^{n}x^2_i}}
\f]
*/
- vpColVector &vpColVector::normalize(vpColVector &x) const
- {
- x = x/sqrt(x.sumSquare());
-
- return x;
- }
+vpColVector &vpColVector::normalize(vpColVector &x) const
+{
+ x = x / sqrt(x.sumSquare());
+ return x;
+}
/*!
Normalise the vector:
@@ -763,9 +752,9 @@ vpColVector &vpColVector::normalize()
double sum_square = sumSquare();
- //if (sum != 0.0)
+ // if (sum != 0.0)
if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
- *this /= sqrt(sum_square) ;
+ *this /= sqrt(sum_square);
// If sum = 0, we have a nul vector. So we return just.
return *this;
@@ -775,68 +764,58 @@ vpColVector &vpColVector::normalize()
Return a column vector with elements of \e v that are reverse sorted.
\sa sort()
*/
-vpColVector
-vpColVector::invSort(const vpColVector &v)
+vpColVector vpColVector::invSort(const vpColVector &v)
{
- if (v.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot sort content of column vector: vector empty")) ;
+ if (v.data == NULL) {
+ throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
}
- vpColVector tab ;
- tab = v ;
- unsigned int nb_permutation = 1 ;
- unsigned int i = 0 ;
- while (nb_permutation !=0 )
- {
- nb_permutation = 0 ;
- for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
- {
- if ((tab[j]>tab[j-1]))
- {
- double tmp = tab[j] ;
- tab[j] = tab[j-1] ;
- tab[j-1] = tmp ;
- nb_permutation++ ;
+ vpColVector tab;
+ tab = v;
+ unsigned int nb_permutation = 1;
+ unsigned int i = 0;
+ while (nb_permutation != 0) {
+ nb_permutation = 0;
+ for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) {
+ if ((tab[j] > tab[j - 1])) {
+ double tmp = tab[j];
+ tab[j] = tab[j - 1];
+ tab[j - 1] = tmp;
+ nb_permutation++;
}
}
- i++ ;
+ i++;
}
- return tab ;
+ return tab;
}
/*!
Return a column vector with elements of \e v that are sorted.
\sa invSort()
*/
-vpColVector
-vpColVector::sort(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")) ;
+ if (v.data == NULL) {
+ throw(vpException(vpException::fatalError, "Cannot sort content of column vector: vector empty"));
}
- vpColVector tab ;
- tab = v ;
- unsigned int nb_permutation = 1 ;
- unsigned int i = 0 ;
- while (nb_permutation !=0 )
- {
- nb_permutation = 0 ;
- for (unsigned int j = v.getRows()-1 ; j >= i+1 ; j--)
- {
- if ((tab[j]<tab[j-1]))
- {
- double tmp = tab[j] ;
- tab[j] = tab[j-1] ;
- tab[j-1] = tmp ;
- nb_permutation++ ;
+ vpColVector tab;
+ tab = v;
+ unsigned int nb_permutation = 1;
+ unsigned int i = 0;
+ while (nb_permutation != 0) {
+ nb_permutation = 0;
+ for (unsigned int j = v.getRows() - 1; j >= i + 1; j--) {
+ if ((tab[j] < tab[j - 1])) {
+ double tmp = tab[j];
+ tab[j] = tab[j - 1];
+ tab[j - 1] = tmp;
+ nb_permutation++;
}
}
- i++ ;
+ i++;
}
- return tab ;
+ return tab;
}
/*!
@@ -857,8 +836,8 @@ vpColVector::sort(const vpColVector &v)
*/
void vpColVector::stack(const double &d)
{
- this->resize(rowNum+1,false);
- (*this)[rowNum-1] = d;
+ this->resize(rowNum + 1, false);
+ (*this)[rowNum - 1] = d;
}
/*!
@@ -880,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.
@@ -949,13 +925,13 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector
}
// General case
- C.resize(nrA + nrB);
+ C.resize(nrA + nrB, false);
- for (unsigned int i=0; i<nrA; i++)
+ for (unsigned int i = 0; i < nrA; i++)
C[i] = A[i];
- for (unsigned int i=0; i<nrB; i++)
- C[nrA+i] = B[i];
+ for (unsigned int i = 0; i < nrB; i++)
+ C[nrA + i] = B[i];
}
/*!
@@ -963,37 +939,31 @@ void vpColVector::stack(const vpColVector &A, const vpColVector &B, vpColVector
*/
double vpColVector::mean(const vpColVector &v)
{
- if (v.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute column vector mean: vector empty")) ;
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute column vector mean: vector empty"));
}
- //Use directly sum() function
+ // Use directly sum() function
double mean = v.sum();
- //Old code used
-// double *vd = v.data ;
-// for (unsigned int i=0 ; i < v.getRows() ; i++)
-// mean += *(vd++) ;
+ // Old code used
+ // double *vd = v.data;
+ // for (unsigned int i=0 ; i < v.getRows() ; i++)
+ // mean += *(vd++);
- return mean/v.getRows();
+ return mean / v.getRows();
}
/*!
Compute the median value of all the elements of the vector.
*/
-double
-vpColVector::median(const vpColVector &v)
+double vpColVector::median(const vpColVector &v)
{
- if (v.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute column vector median: vector empty")) ;
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute column vector median: vector empty"));
}
- std::vector<double> vectorOfDoubles(v.size());
- for(unsigned int i = 0; i < v.size(); i++) {
- vectorOfDoubles[i] = v[i];
- }
+ std::vector<double> vectorOfDoubles(v.data, v.data + v.rowNum);
return vpMath::getMedian(vectorOfDoubles);
}
@@ -1001,12 +971,10 @@ 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) {
- throw(vpException(vpException::fatalError,
- "Cannot compute column vector stdev: vector empty")) ;
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute column vector stdev: vector empty"));
}
double mean_value = mean(v);
@@ -1014,47 +982,41 @@ vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection)
unsigned int i = 0;
#if VISP_HAVE_SSE2
- __m128d v_sub, v_mul, v_sum = _mm_setzero_pd();
- //Compilation error with:
- //clang version 3.5.0 (tags/RELEASE_350/final)
- //Target: x86_64-unknown-linux-gnu
- //Apple LLVM version 6.0 (clang-600.0.54) (based on LLVM 3.5svn)
- //Target: x86_64-apple-darwin13.4.0
- //error: use of undeclared identifier '_mm_set_pd1'; did you mean '_mm_set_ps1'?
-// __m128d v_mean = _mm_set_pd1(mean_value);
- __m128d v_mean = _mm_set_pd(mean_value, mean_value);
-
- if(v.getRows() >= 4) {
- for(; i <= v.getRows()- 4; i+=4) {
- v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i), v_mean);
- v_mul = _mm_mul_pd(v_sub, v_sub);
- v_sum = _mm_add_pd(v_mul, v_sum);
-
- v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i + 2), v_mean);
- v_mul = _mm_mul_pd(v_sub, v_sub);
- v_sum = _mm_add_pd(v_mul, v_sum);
+ if (vpCPUFeatures::checkSSE2()) {
+ __m128d v_sub, v_mul, v_sum = _mm_setzero_pd();
+ __m128d v_mean = _mm_set1_pd(mean_value);
+
+ if (v.getRows() >= 4) {
+ for (; i <= v.getRows() - 4; i += 4) {
+ v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i), v_mean);
+ v_mul = _mm_mul_pd(v_sub, v_sub);
+ v_sum = _mm_add_pd(v_mul, v_sum);
+
+ v_sub = _mm_sub_pd(_mm_loadu_pd(v.data + i + 2), v_mean);
+ v_mul = _mm_mul_pd(v_sub, v_sub);
+ v_sum = _mm_add_pd(v_mul, v_sum);
+ }
}
- }
- double res[2];
- _mm_storeu_pd(res, v_sum);
+ double res[2];
+ _mm_storeu_pd(res, v_sum);
- sum_squared_diff = res[0]+res[1];
-
- //Old code used before SSE
+ sum_squared_diff = res[0] + res[1];
+ }
+// Old code used before SSE
//#else
// for(unsigned int i = 0; i < v.size(); i++) {
// sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
// }
#endif
- for(; i < v.getRows(); i++) {
- sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
+ for (; i < v.getRows(); i++) {
+ sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
}
- double divisor = (double) v.size();
- if(useBesselCorrection && v.size() > 1) {
- divisor = divisor-1;
+ double divisor = (double)v.size();
+ if (useBesselCorrection && v.size() > 1) {
+ divisor = divisor - 1;
}
return std::sqrt(sum_squared_diff / divisor);
@@ -1074,22 +1036,26 @@ vpColVector::stdev(const vpColVector &v, const bool useBesselCorrection)
\param v : Input vector used to compute the skew symmetric matrix.
*/
-vpMatrix
-vpColVector::skew(const vpColVector &v)
+vpMatrix vpColVector::skew(const vpColVector &v)
{
- vpMatrix M ;
+ 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) ;
- M[0][0] = 0 ; M[0][1] = -v[2] ; M[0][2] = v[1] ;
- M[1][0] = v[2] ; M[1][1] = 0 ; M[1][2] = -v[0] ;
- M[2][0] = -v[1] ; M[2][1] = v[0] ; M[2][2] = 0 ;
+ M.resize(3, 3, false, false);
+ M[0][0] = 0;
+ M[0][1] = -v[2];
+ M[0][2] = v[1];
+ M[1][0] = v[2];
+ M[1][1] = 0;
+ M[1][2] = -v[0];
+ M[2][0] = -v[1];
+ M[2][1] = v[0];
+ M[2][2] = 0;
- return M ;
+ return M;
}
/*!
@@ -1099,20 +1065,21 @@ vpColVector::skew(const vpColVector &v)
\param b : 3-dimension column vector.
\return The cross product \f$a \times b\f$.
- \exception vpException::dimensionError If the vectors dimension is not equal to 3.
+ \exception vpException::dimensionError If the vectors dimension is not equal
+ to 3.
*/
vpColVector vpColVector::crossProd(const vpColVector &a, const vpColVector &b)
{
if (a.getRows() != 3 || b.getRows() != 3) {
throw(vpException(vpException::dimensionError,
- "Cannot compute the cross product between column vector with dimension %d and %d",
+ "Cannot compute the cross product between column "
+ "vector with dimension %d and %d",
a.getRows(), b.getRows()));
}
return vpColVector::skew(a) * b;
}
-
/*!
Reshape the column vector in a matrix.
\param nrows : number of rows of the matrix
@@ -1134,7 +1101,8 @@ vpMatrix vpColVector::reshape(const unsigned int &nrows, const unsigned int &nco
\param nrows : number of rows of the matrix.
\param ncols : number of columns of the matrix.
- \exception vpException::dimensionError If the matrix and the column vector have not the same size.
+ \exception vpException::dimensionError If the matrix and the column vector
+have not the same size.
The following example shows how to use this method.
\code
@@ -1184,27 +1152,22 @@ remat:
*/
void vpColVector::reshape(vpMatrix &M, const unsigned int &nrows, const unsigned int &ncols)
{
- if(dsize!=nrows*ncols) {
- throw(vpException(vpException::dimensionError,
- "Cannot reshape (%dx1) column vector in (%dx%d) matrix",
- rowNum, M.getRows(), M.getCols())) ;
- }
- try {
- if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
- }
- catch(...) {
- throw ;
+ if (dsize != nrows * ncols) {
+ 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);
- for(unsigned int j =0; j< ncols; j++)
- for(unsigned int i =0; i< nrows; i++)
- M[i][j]=data[j*nrows+i];
+ for (unsigned int j = 0; j < ncols; j++)
+ for (unsigned int i = 0; i < nrows; i++)
+ M[i][j] = data[j * nrows + i];
}
/*!
Insert a column vector.
- \param i : Index of the first element to introduce. This index starts from 0.
- \param v : Column vector to insert.
+ \param i : Index of the first element to introduce. This index starts from
+0. \param v : Column vector to insert.
The following example shows how to use this function:
\code
@@ -1235,10 +1198,12 @@ v: 0 10 11 3
*/
void vpColVector::insert(unsigned int i, const vpColVector &v)
{
- if (i+v.size() > this->size())
+ if (i + v.size() > this->size())
throw(vpException(vpException::dimensionError, "Unable to insert a column vector"));
- for (unsigned int j=0; j < v.size(); j++)
- (*this)[i+j] = v[j];
+
+ if (data != NULL && v.data != NULL && v.rowNum > 0) {
+ memcpy(data + i, v.data, sizeof(double) * v.rowNum);
+ }
}
/*!
@@ -1260,88 +1225,88 @@ void vpColVector::insert(unsigned int i, const vpColVector &v)
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &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;
unsigned int m = getRows();
unsigned int n = 1;
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
+ for (unsigned int i = 0; i < m; ++i) {
oss.str("");
oss << (*this)[i];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[i];
oss.str(ossFixed.str());
}
- values[i]=oss.str();
- size_type thislen=values[i].size();
- size_type p=values[i].find('.');
+ values[i] = oss.str();
+ size_type thislen = values[i].size();
+ size_type p = values[i].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
-
}
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- size_type p=values[i].find('.');
+ for (unsigned int i = 0; i < m; i++) {
+ s << " ";
+ size_type p = values[i].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[i].substr(0,p).c_str();
+ s << values[i].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[i].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[i].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
- s <<std::endl;
+ s << std::endl;
}
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
/*!
@@ -1355,30 +1320,31 @@ double vpColVector::sum() const
unsigned int i = 0;
#if VISP_HAVE_SSE2
- __m128d v_sum1 = _mm_setzero_pd(), v_sum2 = _mm_setzero_pd(), v_sum;
+ if (vpCPUFeatures::checkSSE2()) {
+ __m128d v_sum1 = _mm_setzero_pd(), v_sum2 = _mm_setzero_pd(), v_sum;
- if(rowNum >= 4) {
- for(; i <= rowNum- 4; i+=4) {
- v_sum1 = _mm_add_pd(_mm_loadu_pd(data + i), v_sum1);
- v_sum2 = _mm_add_pd(_mm_loadu_pd(data + i + 2), v_sum2);
+ if (rowNum >= 4) {
+ for (; i <= rowNum - 4; i += 4) {
+ v_sum1 = _mm_add_pd(_mm_loadu_pd(data + i), v_sum1);
+ v_sum2 = _mm_add_pd(_mm_loadu_pd(data + i + 2), v_sum2);
+ }
}
- }
- v_sum = _mm_add_pd(v_sum1, v_sum2);
+ v_sum = _mm_add_pd(v_sum1, v_sum2);
- double res[2];
- _mm_storeu_pd(res, v_sum);
+ double res[2];
+ _mm_storeu_pd(res, v_sum);
- sum = res[0]+res[1];
-
- //Old code used before SSE
+ sum = res[0] + res[1];
+ }
+// Old code used before SSE
//#else
// for (unsigned int i=0;i<rowNum;i++) {
// sum += rowPtrs[i][0];
// }
#endif
- for(; i < rowNum; i++) {
+ for (; i < rowNum; i++) {
sum += (*this)[i];
}
@@ -1386,7 +1352,8 @@ double vpColVector::sum() const
}
/*!
- Return the sum square of all the elements \f$v_{i}\f$ of the column vector v(m).
+ Return the sum square of all the elements \f$v_{i}\f$ of the column vector
+ v(m).
\return The value \f[\sum{i=0}^{m} v_i^{2}\f].
*/
@@ -1396,25 +1363,26 @@ double vpColVector::sumSquare() const
unsigned int i = 0;
#if VISP_HAVE_SSE2
- __m128d v_mul1, v_mul2;
- __m128d v_sum = _mm_setzero_pd();
+ if (vpCPUFeatures::checkSSE2()) {
+ __m128d v_mul1, v_mul2;
+ __m128d v_sum = _mm_setzero_pd();
- 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));
+ 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_sum = _mm_add_pd(v_mul1, v_sum);
- v_sum = _mm_add_pd(v_mul2, v_sum);
+ v_sum = _mm_add_pd(v_mul1, v_sum);
+ v_sum = _mm_add_pd(v_mul2, v_sum);
+ }
}
- }
-
- double res[2];
- _mm_storeu_pd(res, v_sum);
- sum_square = res[0]+res[1];
+ double res[2];
+ _mm_storeu_pd(res, v_sum);
- //Old code used before SSE
+ sum_square = res[0] + res[1];
+ }
+// Old code used before SSE
//#else
// for (unsigned int i=0;i<rowNum;i++) {
// double x=rowPtrs[i][0];
@@ -1422,7 +1390,7 @@ double vpColVector::sumSquare() const
// }
#endif
- for(; i < rowNum; i++) {
+ for (; i < rowNum; i++) {
sum_square += (*this)[i] * (*this)[i];
}
@@ -1437,22 +1405,56 @@ double vpColVector::sumSquare() const
*/
double vpColVector::euclideanNorm() const
{
- //Use directly sumSquare() function
+ // Use directly sumSquare() function
double norm = sumSquare();
- //Old code used
-// for (unsigned int i=0;i<dsize;i++) {
-// double x = *(data +i); norm += x*x;
-// }
+ // Old code used
+ // for (unsigned int i=0;i<dsize;i++) {
+ // double x = *(data +i); norm += x*x;
+ // }
return sqrt(norm);
}
/*!
+ Compute the Hadamard product (element wise vector multiplication).
+ \param v : Second vector;
+ \return v1.hadamard(v2) The kronecker product : \f$ v1 \circ v2 = (v1 \circ
+ v2)_{i} = (v1)_{i} (v2)_{i} \f$
+*/
+vpColVector vpColVector::hadamard(const vpColVector &v) const
+{
+ if (v.getRows() != rowNum || v.getCols() != colNum) {
+ throw(vpException(vpException::dimensionError, "Hadamard product: bad dimensions!"));
+ }
+
+ vpColVector out;
+ out.resize(rowNum, false);
+
+ unsigned int i = 0;
+
+#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));
+ _mm_storeu_pd(out.data + i, vout);
+ }
+ }
+#endif
+
+ for (; i < dsize; i++) {
+ out.data[i] = data[i] * v.data[i];
+ }
+
+ return out;
+}
+
+/*!
Compute and return the infinity norm \f$ {||x||}_{\infty} =
max\left({\mid x_{i} \mid}\right) \f$ with \f$i \in
- \{0, ..., m-1\}\f$ where \e m is the vector size and \f$x_i\f$ an element of the vector.
+ \{0, ..., m-1\}\f$ where \e m is the vector size and \f$x_i\f$ an element of
+ the vector.
\return The infinity norm if the matrix is initialized, 0 otherwise.
@@ -1460,9 +1462,9 @@ double vpColVector::euclideanNorm() const
*/
double vpColVector::infinityNorm() const
{
- double norm=0.0;
- for (unsigned int i=0;i<rowNum;i++){
- double x = fabs ( (*this)[i] ) ;
+ double norm = 0.0;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double x = fabs((*this)[i]);
if (x > norm) {
norm = x;
}
@@ -1496,26 +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) {
- for (unsigned int i=0; i < this->getRows(); ++ i) {
-
- if (! octet) {
+ if (!octet) {
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;
+ } 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;
}
}
}
@@ -1549,11 +1545,11 @@ int main()
2
\endcode
*/
-std::ostream & vpColVector::csvPrint(std::ostream & os) const
+std::ostream &vpColVector::csvPrint(std::ostream &os) const
{
- for (unsigned int i=0; i < this->getRows(); ++ i) {
- os << (*this)[i];
-
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
+ os << (*this)[i];
+
os << std::endl;
}
return os;
@@ -1584,12 +1580,12 @@ v = ([
\endcode
that could be copy/paste in Maple.
*/
-std::ostream & vpColVector::maplePrint(std::ostream & os) const
+std::ostream &vpColVector::maplePrint(std::ostream &os) const
{
os << "([ " << std::endl;
- for (unsigned int i=0; i < this->getRows(); ++ i) {
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
os << "[";
- os << (*this)[i] << ", ";
+ os << (*this)[i] << ", ";
os << "]," << std::endl;
}
os << "])" << std::endl;
@@ -1632,13 +1628,16 @@ v =
>>
\endcode
*/
-std::ostream & vpColVector::matlabPrint(std::ostream & os) const
+std::ostream &vpColVector::matlabPrint(std::ostream &os) const
{
os << "[ ";
- for (unsigned int i=0; i < this->getRows(); ++ i) {
- os << (*this)[i] << ", ";
- if (this ->getRows() != i+1) { os << ";" << std::endl; }
- else { os << "]" << std::endl; }
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
+ os << (*this)[i] << ", ";
+ if (this->getRows() != i + 1) {
+ os << ";" << std::endl;
+ } else {
+ os << "]" << std::endl;
+ }
}
return os;
};
@@ -1647,7 +1646,8 @@ std::ostream & vpColVector::matlabPrint(std::ostream & os) const
/*!
\deprecated You should rather use insert(unsigned int, const vpColVector &).
- Insert column vector \e v at the given position \e r in the current column vector.
+ Insert column vector \e v at the given position \e r in the current column
+ vector.
\warning Throw vpMatrixException::incorrectMatrixSizeError if the
dimensions of the matrices do not allow the operation.
@@ -1659,7 +1659,7 @@ std::ostream & vpColVector::matlabPrint(std::ostream & os) const
*/
void vpColVector::insert(const vpColVector &v, const unsigned int r, const unsigned int c)
{
- (void) c;
+ (void)c;
insert(r, v);
}
#endif // defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
diff --git a/modules/core/src/math/matrix/vpMatrix.cpp b/modules/core/src/math/matrix/vpMatrix.cpp
index 9d1a172..a48ea09 100644
--- a/modules/core/src/math/matrix/vpMatrix.cpp
+++ b/modules/core/src/math/matrix/vpMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,25 +35,22 @@
* Eric Marchand
*
*****************************************************************************/
-
-
-
/*!
\file vpMatrix.cpp
\brief Definition of the vpMatrix class
*/
-#include <stdlib.h>
-#include <stdio.h>
-#include <string.h>
-#include <vector>
-#include <sstream>
#include <algorithm>
#include <assert.h>
+#include <cmath> // std::fabs
#include <fstream>
+#include <limits> // numeric_limits
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
#include <string>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <vector>
#include <visp3/core/vpConfig.h>
@@ -60,35 +58,157 @@
#include <gsl/gsl_linalg.h>
#endif
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpException.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTranslationVector.h>
+
+#define USE_SSE_CODE 1
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+#endif
+
+#if VISP_HAVE_SSE2 && USE_SSE_CODE
+#define USE_SSE 1
+#endif
-//Prototypes of specific functions
+// 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,
+ vpMatrix &Ap, unsigned int &rank)
+{
+ vpMatrix a1(ncols, nrows);
+
+ // compute the highest singular value and the rank of h
+ double maxsv = 0;
+ for (unsigned int i = 0; i < ncols; i++) {
+ if (fabs(sv[i]) > maxsv)
+ maxsv = fabs(sv[i]);
+ }
+
+ rank = 0;
+
+ for (unsigned int i = 0; i < ncols; i++) {
+ if (fabs(sv[i]) > maxsv * svThreshold) {
+ rank++;
+ }
+
+ for (unsigned int j = 0; j < nrows; j++) {
+ a1[i][j] = 0.0;
+
+ for (unsigned int k = 0; k < ncols; k++) {
+ if (fabs(sv[k]) > maxsv * svThreshold) {
+ a1[i][j] += v[i][k] * a[j][k] / sv[k];
+ }
+ }
+ }
+ }
+ if (nrows_orig >= ncols_orig)
+ Ap = a1;
+ else
+ Ap = a1.t();
+}
+
+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);
+
+ // compute the highest singular value and the rank of h
+ double maxsv = fabs(sv[0]);
+
+ rank = 0;
+
+ for (unsigned int i = 0; i < ncols_orig; i++) {
+ if (fabs(sv[i]) > maxsv * svThreshold) {
+ rank++;
+ }
+
+ for (unsigned int j = 0; j < nrows_orig; j++) {
+ // Ap[i][j] = 0.0;
+
+ for (unsigned int k = 0; k < ncols_orig; k++) {
+ if (fabs(sv[k]) > maxsv * svThreshold) {
+ Ap[i][j] += V[i][k] * U[j][k] / sv[k];
+ }
+ }
+ }
+ }
+
+ // Compute im(A) and im(At)
+ imA.resize(nrows_orig, rank);
+ imAt.resize(ncols_orig, rank);
+
+ for (unsigned int i = 0; i < nrows_orig; i++) {
+ for (unsigned int j = 0; j < rank; j++) {
+ imA[i][j] = U[i][j];
+ }
+ }
+
+ for (unsigned int i = 0; i < ncols_orig; i++) {
+ for (unsigned int j = 0; j < rank; j++) {
+ imAt[i][j] = V[i][j];
+ }
+ }
+
+ kerAt.resize(ncols_orig - rank, ncols_orig);
+ if (rank != ncols_orig) {
+ 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<double>::epsilon())) {
+ for (unsigned int i = 0; i < V.getRows(); i++) {
+ kerAt[k][i] = V[i][j];
+ }
+ k++;
+ }
+ }
+ }
+}
/*!
Construct a matrix as a sub-matrix of the input matrix \e M.
- \sa init(const vpMatrix &M, unsigned int r, unsigned int c, unsigned int nrows, unsigned int ncols)
+ \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<double>()
{
if (((r + nrows) > M.rowNum) || ((c + ncols) > M.colNum)) {
throw(vpException(vpException::dimensionError,
- "Cannot construct a sub matrix (%dx%d) starting at position (%d,%d) that is not contained in the original matrix (%dx%d)",
- nrows, ncols, r, c, M.rowNum, M.colNum)) ;
+ "Cannot construct a sub matrix (%dx%d) starting at "
+ "position (%d,%d) that is not contained in the "
+ "original matrix (%dx%d)",
+ nrows, ncols, r, c, M.rowNum, M.colNum));
}
init(M, r, c, nrows, ncols);
}
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+vpMatrix::vpMatrix(vpMatrix &&A) : vpArray2D<double>()
+{
+ rowNum = A.rowNum;
+ colNum = A.colNum;
+ rowPtrs = A.rowPtrs;
+ dsize = A.dsize;
+ data = A.data;
+
+ A.rowNum = 0;
+ A.colNum = 0;
+ A.rowPtrs = NULL;
+ A.dsize = 0;
+ A.data = NULL;
+}
+#endif
+
/*!
Initialize the matrix from a part of an input matrix \e M.
@@ -98,8 +218,8 @@ vpMatrix::vpMatrix(const vpMatrix &M,
\param nrows : Number of rows of the matrix that should be initialized.
\param ncols : Number of columns of the matrix that should be initialized.
- The sub-matrix starting from M[r][c] element and ending on M[r+nrows-1][c+ncols-1] element
- is used to initialize the matrix.
+ The sub-matrix starting from M[r][c] element and ending on
+M[r+nrows-1][c+ncols-1] element is used to initialize the matrix.
The following code shows how to use this function:
\code
@@ -132,56 +252,103 @@ N [2,3]=
1 2 3
6 7 8
\endcode
+
+ \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 ;
+ 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()));
- resize(nrows, ncols);
+ 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
- return; // Noting to do
- for (unsigned int i=r ; i < rnrows; i++)
- for (unsigned int j=c ; j < cncols; j++)
- (*this)[i-r][j-c] = M[i][j] ;
+ return; // Noting to do
+ for (unsigned int i = 0; i < nrows; i++) {
+ memcpy((*this)[i], &M[i + r][c], ncols * sizeof(double));
+ }
}
/*!
- Set an n-by-n matrix to identity with ones on the diagonal and zeros
- else where.
-*/
-void
-vpMatrix::eye(unsigned int n)
+ Extract a sub matrix from a matrix \e M.
+
+ \param r : row index in matrix \e M.
+ \param c : column index in matrix \e M.
+ \param nrows : Number of rows of the matrix that should be extracted.
+ \param ncols : Number of columns of the matrix that should be extracted.
+
+ The following code shows how to use this function:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
{
- try {
- eye(n, n);
+ vpMatrix M(4,5);
+ int val = 0;
+ for(size_t i=0; i<M.getRows(); i++) {
+ for(size_t j=0; j<M.getCols(); j++) {
+ M[i][j] = val++;
+ }
}
- catch(...) {
- throw ;
+ M.print (std::cout, 4, "M ");
+ vpMatrix N = M.extract(0, 1, 2, 3);
+ N.print (std::cout, 4, "N ");
+}
+ \endcode
+ It produces the following output:
+ \code
+M [4,5]=
+ 0 1 2 3 4
+ 5 6 7 8 9
+ 10 11 12 13 14
+ 15 16 17 18 19
+N [2,3]=
+ 1 2 3
+ 6 7 8
+ \endcode
+
+ \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
+{
+ 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()));
+ if (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++) {
+ memcpy(M[i], &(*this)[i + r][c], ncols * sizeof(double));
}
+
+ return M;
}
/*!
+ Set an n-by-n matrix to identity with ones on the diagonal and zeros
+ else where.
+*/
+void vpMatrix::eye(unsigned int n) { eye(n, n); }
+
+/*!
Set an m-by-n matrix to identity with ones on the diagonal and zeros
else where.
*/
-void
-vpMatrix::eye(unsigned int m, unsigned int n)
+void vpMatrix::eye(unsigned int m, unsigned int n)
{
- try {
- resize(m,n) ;
- }
- catch(...) {
- throw ;
- }
+ resize(m, n);
eye();
}
@@ -190,13 +357,14 @@ vpMatrix::eye(unsigned int m, unsigned int n)
Set an m-by-n matrix to identity with ones on the diagonal and zeros
else where.
*/
-void
-vpMatrix::eye()
+void vpMatrix::eye()
{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
- if (i == j) (*this)[i][j] = 1.0;
- else (*this)[i][j] = 0;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
+ if (i == j)
+ (*this)[i][j] = 1.0;
+ else
+ (*this)[i][j] = 0;
}
}
}
@@ -206,33 +374,26 @@ vpMatrix::eye()
*/
vpMatrix vpMatrix::t() const
{
- vpMatrix At ;
+ vpMatrix At;
- try {
- At.resize(colNum, rowNum);
- }
- catch(...)
- {
- throw ;
- }
+ At.resize(colNum, rowNum, false, false);
- for (unsigned int i=0;i<rowNum;i++) {
- double *coli = (*this)[i] ;
- for (unsigned int j=0;j<colNum;j++)
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double *coli = (*this)[i];
+ for (unsigned int j = 0; j < colNum; j++)
At[j][i] = coli[j];
}
return At;
}
-
/*!
Compute and return the transpose of the matrix.
\sa t()
*/
-vpMatrix vpMatrix::transpose()const
+vpMatrix vpMatrix::transpose() const
{
- vpMatrix At ;
+ vpMatrix At;
transpose(At);
return At;
}
@@ -242,28 +403,21 @@ vpMatrix vpMatrix::transpose()const
\param At (output) : Resulting transpose matrix.
\sa t()
*/
-void vpMatrix::transpose(vpMatrix & At ) const
+void vpMatrix::transpose(vpMatrix &At) const
{
- try {
- At.resize(colNum,rowNum);
- }
- catch(...)
- {
- throw ;
- }
+ At.resize(colNum, rowNum, false, false);
size_t A_step = colNum;
- double ** AtRowPtrs = At.rowPtrs;
+ double **AtRowPtrs = At.rowPtrs;
- for( unsigned int i = 0; i < colNum; i++ ) {
- double * row_ = AtRowPtrs[i];
- double * col = rowPtrs[0]+i;
- for( unsigned int j = 0; j < rowNum; j++, col+=A_step )
- *(row_++)=*col;
+ for (unsigned int i = 0; i < colNum; i++) {
+ double *row_ = AtRowPtrs[i];
+ double *col = rowPtrs[0] + i;
+ for (unsigned int j = 0; j < rowNum; j++, col += A_step)
+ *(row_++) = *col;
}
}
-
/*!
Computes the \f$AA^T\f$ operation \f$B = A*A^T\f$
\return \f$A*A^T\f$
@@ -291,28 +445,23 @@ vpMatrix vpMatrix::AAt() const
*/
void vpMatrix::AAt(vpMatrix &B) const
{
- try {
- if ((B.rowNum != rowNum) || (B.colNum != rowNum)) B.resize(rowNum,rowNum);
- }
- catch(...)
- {
- throw ;
- }
+ if ((B.rowNum != rowNum) || (B.colNum != rowNum))
+ B.resize(rowNum, rowNum, false, false);
// compute A*A^T
- for(unsigned int i=0;i<rowNum;i++){
- for(unsigned int j=i;j<rowNum;j++){
- double *pi = rowPtrs[i];// row i
- double *pj = rowPtrs[j];// row j
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = i; j < rowNum; j++) {
+ double *pi = rowPtrs[i]; // row i
+ double *pj = rowPtrs[j]; // row j
// sum (row i .* row j)
- double ssum=0;
- for(unsigned int k=0; k < colNum ;k++)
- ssum += *(pi++)* *(pj++);
+ double ssum = 0;
+ for (unsigned int k = 0; k < colNum; k++)
+ ssum += *(pi++) * *(pj++);
- B[i][j]=ssum; //upper triangle
- if(i!=j)
- B[j][i]=ssum; //lower triangle
+ B[i][j] = ssum; // upper triangle
+ if (i != j)
+ B[j][i] = ssum; // lower triangle
}
}
}
@@ -330,44 +479,43 @@ void vpMatrix::AAt(vpMatrix &B) const
*/
void vpMatrix::AtA(vpMatrix &B) const
{
- try {
- if ((B.rowNum != colNum) || (B.colNum != colNum)) B.resize(colNum,colNum);
- }
- catch(...)
- {
- throw ;
- }
+ if ((B.rowNum != colNum) || (B.colNum != colNum))
+ B.resize(colNum, colNum, false, false);
- unsigned int i,j,k;
+#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
+ double alpha = 1.0;
+ double beta = 0.0;
+ char transa = 'n';
+ char transb = 't';
+
+ 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;
double *ptr;
- for (i=0;i<colNum;i++)
- {
- double *Bi = B[i] ;
- for (j=0;j<i;j++)
- {
- ptr=data;
- s = 0 ;
- for (k=0;k<rowNum;k++)
- {
- s +=(*(ptr+i)) * (*(ptr+j));
- ptr+=colNum;
+ for (i = 0; i < colNum; i++) {
+ double *Bi = B[i];
+ for (j = 0; j < i; j++) {
+ ptr = data;
+ s = 0;
+ for (k = 0; k < rowNum; k++) {
+ s += (*(ptr + i)) * (*(ptr + j));
+ ptr += colNum;
}
- *Bi++ = s ;
+ *Bi++ = s;
B[j][i] = s;
}
- ptr=data;
- s = 0 ;
- for (k=0;k<rowNum;k++)
- {
- s +=(*(ptr+i)) * (*(ptr+i));
- ptr+=colNum;
+ ptr = data;
+ s = 0;
+ for (k = 0; k < rowNum; k++) {
+ s += (*(ptr + i)) * (*(ptr + i));
+ ptr += colNum;
}
*Bi = s;
}
+#endif
}
-
/*!
Compute the AtA operation such as \f$B = A^T*A\f$
\return \f$A^T*A\f$
@@ -384,54 +532,81 @@ vpMatrix vpMatrix::AtA() const
/*!
Copy operator that allows to convert on of the following container that
- inherit from vpArray2D such as vpMatrix, vpRotationMatrix, vpHomogeneousMatrix,
- vpPoseVector, vpColVector, vpRowVector... into a vpMatrix.
+ inherit from vpArray2D such as vpMatrix, vpRotationMatrix,
+ vpHomogeneousMatrix, vpPoseVector, vpColVector, vpRowVector... into a
+ vpMatrix.
\param A : 2D array to be copied.
- The following example shows how to create a matrix from an homogeneous matrix:
- \code
- vpRotationMatrix R;
- vpMatrix M = R;
- \endcode
+ The following example shows how to create a matrix from an homogeneous
+ matrix:
+\code
+vpRotationMatrix R;
+vpMatrix M = R;
+\endcode
*/
-vpMatrix &
-vpMatrix::operator=(const vpArray2D<double> &A)
+vpMatrix &vpMatrix::operator=(const vpArray2D<double> &A)
{
- try {
- resize(A.getRows(), A.getCols()) ;
- }
- catch(...) {
- throw ;
- }
+ resize(A.getRows(), A.getCols(), false, false);
+
+ memcpy(data, A.data, dsize * sizeof(double));
+
+ return *this;
+}
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+vpMatrix &vpMatrix::operator=(const vpMatrix &A)
+{
+ resize(A.getRows(), A.getCols(), false);
+
+ memcpy(data, A.data, dsize * sizeof(double));
+
+ return *this;
+}
+
+vpMatrix &vpMatrix::operator=(vpMatrix &&other)
+{
+ if (this != &other) {
+ free(data);
+ free(rowPtrs);
+
+ rowNum = other.rowNum;
+ colNum = other.colNum;
+ rowPtrs = other.rowPtrs;
+ dsize = other.dsize;
+ data = other.data;
- memcpy(data, A.data, dsize*sizeof(double));
+ other.rowNum = 0;
+ other.colNum = 0;
+ other.rowPtrs = NULL;
+ other.dsize = 0;
+ other.data = NULL;
+ }
return *this;
}
+#endif
//! Set all the element of the matrix A to \e x.
-vpMatrix &
-vpMatrix::operator=(double x)
+vpMatrix &vpMatrix::operator=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] = x;
return *this;
}
-
/*!
- Assigment from an array of double. This method has to be used carefully since
- the array allocated behind \e x pointer should have the same dimension than the matrix.
+ Assigment from an array of double. This method has to be used carefully
+ since the array allocated behind \e x pointer should have the same dimension
+ than the matrix.
*/
-vpMatrix &
-vpMatrix::operator<<( double *x )
+vpMatrix &vpMatrix::operator<<(double *x)
{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
rowPtrs[i][j] = *x++;
}
}
@@ -474,25 +649,19 @@ int main()
0 0 3
\endcode
*/
-void
-vpMatrix::diag(const vpColVector &A)
+void vpMatrix::diag(const vpColVector &A)
{
- unsigned int rows = A.getRows() ;
- try {
- this->resize(rows,rows) ;
- }
- catch(...) {
- throw ;
- }
- (*this) = 0 ;
- for (unsigned int i=0 ; i< rows ; i++ )
- (* this)[i][i] = A[i] ;
+ unsigned int rows = A.getRows();
+ this->resize(rows, rows);
+
+ (*this) = 0;
+ for (unsigned int i = 0; i < rows; i++)
+ (*this)[i][i] = A[i];
}
/*!
-
- Set the matrix as a diagonal matrix where each element on the diagonal is set to \e val.
- Elements that are not on the diagonal are set to 0.
+ Set the matrix as a diagonal matrix where each element on the diagonal is
+set to \e val. Elements that are not on the diagonal are set to 0.
\param val : Value to set.
@@ -520,16 +689,14 @@ int main()
0 0 2 0
\endcode
*/
-void
-vpMatrix::diag(const double &val)
+void vpMatrix::diag(const double &val)
{
(*this) = 0;
unsigned int min_ = (rowNum < colNum) ? rowNum : colNum;
- for (unsigned int i=0 ; i< min_ ; i++ )
- (* this)[i][i] = val;
+ for (unsigned int i = 0; i < min_; i++)
+ (*this)[i][i] = val;
}
-
/*!
Create a diagonal matrix with the element of a vector \f$ DA_{ii} = A_i \f$.
@@ -541,43 +708,35 @@ vpMatrix::diag(const double &val)
\sa diag()
*/
-void
-vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
+void vpMatrix::createDiagonalMatrix(const vpColVector &A, vpMatrix &DA)
{
- unsigned int rows = A.getRows() ;
- try {
- DA.resize(rows,rows) ;
- }
- catch(...)
- {
- throw ;
- }
- DA =0 ;
- for (unsigned int i=0 ; i< rows ; i++ )
- DA[i][i] = A[i] ;
+ unsigned int rows = A.getRows();
+ DA.resize(rows, rows);
+
+ for (unsigned int i = 0; i < rows; i++)
+ DA[i][i] = A[i];
}
/*!
Operator that allows to multiply a matrix by a translation vector.
The matrix should be of dimension (3x3)
*/
-vpTranslationVector
-vpMatrix::operator*(const vpTranslationVector &tv) const
+vpTranslationVector 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++) t_out[j]=0 ;
+ for (unsigned int j = 0; j < 3; j++)
+ t_out[j] = 0;
- for (unsigned int j=0;j<3;j++) {
- double tj = tv[j] ; // optimization em 5/12/2006
- for (unsigned int i=0;i<3;i++) {
- t_out[i]+=rowPtrs[i][j] * tj;
+ for (unsigned int j = 0; j < 3; j++) {
+ double tj = tv[j]; // optimization em 5/12/2006
+ for (unsigned int i = 0; i < 3; i++) {
+ t_out[i] += rowPtrs[i][j] * tj;
}
}
return t_out;
@@ -587,8 +746,7 @@ vpMatrix::operator*(const vpTranslationVector &tv) const
Operation w = A * v (matrix A is unchanged, v and w are column vectors).
\sa multMatrixVector() to avoid matrix allocation for each use.
*/
-vpColVector
-vpMatrix::operator*(const vpColVector &v) const
+vpColVector vpMatrix::operator*(const vpColVector &v) const
{
vpColVector v_out;
vpMatrix::multMatrixVector(*this, v, v_out);
@@ -606,25 +764,29 @@ vpMatrix::operator*(const vpColVector &v) const
void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVector &w)
{
if (A.colNum != v.getRows()) {
- throw(vpException(vpException::dimensionError,
- "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
- A.getRows(), A.getCols(), v.getRows())) ;
+ throw(vpException(vpException::dimensionError, "Cannot multiply a (%dx%d) matrix by a (%d) column vector",
+ A.getRows(), A.getCols(), v.getRows()));
}
- try {
- if (A.rowNum != w.rowNum) w.resize(A.rowNum);
- }
- catch(...) {
- throw ;
- }
+ if (A.rowNum != w.rowNum)
+ w.resize(A.rowNum, false);
+
+#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
+ double alpha = 1.0;
+ double beta = 0.0;
+ 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);
+#else
w = 0.0;
- for (unsigned int j=0;j<A.colNum;j++) {
- double vj = v[j] ; // optimization em 5/12/2006
- for (unsigned int i=0;i<A.rowNum;i++) {
- w[i]+=A.rowPtrs[i][j] * vj;
+ for (unsigned int j = 0; j < A.colNum; j++) {
+ double vj = v[j]; // optimization em 5/12/2006
+ for (unsigned int i = 0; i < A.rowNum; i++) {
+ w[i] += A.rowPtrs[i][j] * vj;
}
}
+#endif
}
//---------------------------------
@@ -642,40 +804,43 @@ void vpMatrix::multMatrixVector(const vpMatrix &A, const vpColVector &v, vpColVe
*/
void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
- try {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(...) {
- throw ;
- }
+ if ((A.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)
+ double alpha = 1.0;
+ 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);
+#else
// 5/12/06 some "very" simple optimization to avoid indexation
unsigned int BcolNum = B.colNum;
unsigned int BrowNum = B.rowNum;
- unsigned int i,j,k;
+ unsigned int i, j, k;
double **BrowPtrs = B.rowPtrs;
- for (i=0;i<A.rowNum;i++)
- {
+ for (i = 0; i < A.rowNum; i++) {
double *rowptri = A.rowPtrs[i];
double *ci = C[i];
- for (j=0;j<BcolNum;j++)
- {
+ for (j = 0; j < BcolNum; j++) {
double s = 0;
- for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ for (k = 0; k < BrowNum; k++)
+ s += rowptri[k] * BrowPtrs[k][j];
ci[j] = s;
}
}
+#endif
}
/*!
- \warning This function is provided for compat with previous releases. You should
- rather use the functionalities provided in vpRotationMatrix class.
+ \warning This function is provided for compat with previous releases. You
+ should rather use the functionalities provided in vpRotationMatrix class.
Operation C = A * B.
@@ -690,31 +855,31 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpRotationMat
{
if (A.colNum != 3 || A.rowNum != 3 || B.colNum != 3 || B.rowNum != 3) {
throw(vpException(vpException::dimensionError,
- "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
- A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
+ "rotation matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols()));
}
// 5/12/06 some "very" simple optimization to avoid indexation
unsigned int BcolNum = B.colNum;
unsigned int BrowNum = B.rowNum;
- unsigned int i,j,k;
+ unsigned int i, j, k;
double **BrowPtrs = B.rowPtrs;
- for (i=0;i<A.rowNum;i++)
- {
+ for (i = 0; i < A.rowNum; i++) {
double *rowptri = A.rowPtrs[i];
double *ci = C[i];
- for (j=0;j<BcolNum;j++)
- {
+ for (j = 0; j < BcolNum; j++) {
double s = 0;
- for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ for (k = 0; k < BrowNum; k++)
+ s += rowptri[k] * BrowPtrs[k][j];
ci[j] = s;
}
}
}
/*!
- \warning This function is provided for compat with previous releases. You should
- rather use the functionalities provided in vpHomogeneousMatrix class.
+ \warning This function is provided for compat with previous releases. You
+ should rather use the functionalities provided in vpHomogeneousMatrix class.
Operation C = A * B.
@@ -729,31 +894,31 @@ void vpMatrix::mult2Matrices(const vpMatrix &A, const vpMatrix &B, vpHomogeneous
{
if (A.colNum != 4 || A.rowNum != 4 || B.colNum != 4 || B.rowNum != 4) {
throw(vpException(vpException::dimensionError,
- "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a rotation matrix",
- A.getRows(), A.getCols(), B.getRows(), B.getCols())) ;
+ "Cannot multiply (%dx%d) matrix by (%dx%d) matrix as a "
+ "rotation matrix",
+ A.getRows(), A.getCols(), B.getRows(), B.getCols()));
}
// 5/12/06 some "very" simple optimization to avoid indexation
unsigned int BcolNum = B.colNum;
unsigned int BrowNum = B.rowNum;
- unsigned int i,j,k;
+ unsigned int i, j, k;
double **BrowPtrs = B.rowPtrs;
- for (i=0;i<A.rowNum;i++)
- {
+ for (i = 0; i < A.rowNum; i++) {
double *rowptri = A.rowPtrs[i];
double *ci = C[i];
- for (j=0;j<BcolNum;j++)
- {
+ for (j = 0; j < BcolNum; j++) {
double s = 0;
- for (k=0;k<BrowNum;k++) s += rowptri[k] * BrowPtrs[k][j];
+ for (k = 0; k < BrowNum; k++)
+ s += rowptri[k] * BrowPtrs[k][j];
ci[j] = s;
}
}
}
/*!
- \warning This function is provided for compat with previous releases. You should
- rather use multMatrixVector() that is more explicit.
+ \warning This function is provided for compat with previous releases. You
+ should rather use multMatrixVector() that is more explicit.
Operation C = A * B.
@@ -776,7 +941,7 @@ vpMatrix vpMatrix::operator*(const vpMatrix &B) const
{
vpMatrix C;
- vpMatrix::mult2Matrices(*this,B,C);
+ vpMatrix::mult2Matrices(*this, B, C);
return C;
}
@@ -788,22 +953,20 @@ 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);
unsigned int RcolNum = R.getCols();
unsigned int RrowNum = R.getRows();
- for (unsigned int i=0;i<rowNum;i++)
- {
+ for (unsigned int i = 0; i < rowNum; i++) {
double *rowptri = rowPtrs[i];
double *ci = C[i];
- for (unsigned int j=0;j<RcolNum;j++)
- {
+ for (unsigned int j = 0; j < RcolNum; j++) {
double s = 0;
- for (unsigned int k=0;k<RrowNum;k++) s += rowptri[k] * R[k][j];
+ for (unsigned int k = 0; k < RrowNum; k++)
+ s += rowptri[k] * R[k][j];
ci[j] = s;
}
}
@@ -817,25 +980,65 @@ 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 (3x3) velocity twist matrix",
- rowNum, colNum)) ;
+ throw(vpException(vpException::dimensionError, "Cannot multiply (%dx%d) matrix by (6x6) velocity twist matrix",
+ rowNum, colNum));
}
- vpMatrix M(rowNum, 6);
+ vpMatrix M;
+ M.resize(rowNum, 6, false, false);
- unsigned int VcolNum = V.getCols();
- unsigned int VrowNum = V.getRows();
- for (unsigned int i=0;i<rowNum;i++)
- {
- double *rowptri = rowPtrs[i];
- double *ci = M[i];
- for (unsigned int j=0;j<VcolNum;j++)
- {
- double s = 0;
- for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
- ci[j] = s;
+#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
+ double alpha = 1.0;
+ 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);
+#else
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#endif
+
+ if (checkSSE2) {
+#if USE_SSE
+ vpMatrix V_trans(6, 6);
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ V_trans[i][j] = V[j][i];
+ }
+ }
+
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double *rowptri = rowPtrs[i];
+ double *ci = M[i];
+
+ 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])));
+ }
+
+ double v_tmp[2];
+ _mm_storeu_pd(v_tmp, v_mul);
+ ci[j] = v_tmp[0] + v_tmp[1];
+ }
+ }
+#endif
+ } else {
+ unsigned int VcolNum = V.getCols();
+ unsigned int VrowNum = V.getRows();
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double *rowptri = rowPtrs[i];
+ double *ci = M[i];
+ for (unsigned int j = 0; j < VcolNum; j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < VrowNum; k++)
+ s += rowptri[k] * V[k][j];
+ ci[j] = s;
+ }
}
}
+#endif
return M;
}
@@ -846,22 +1049,20 @@ 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 (3x3) 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);
unsigned int VcolNum = V.getCols();
unsigned int VrowNum = V.getRows();
- for (unsigned int i=0;i<rowNum;i++)
- {
+ for (unsigned int i = 0; i < rowNum; i++) {
double *rowptri = rowPtrs[i];
double *ci = M[i];
- for (unsigned int j=0;j<VcolNum;j++)
- {
+ for (unsigned int j = 0; j < VcolNum; j++) {
double s = 0;
- for (unsigned int k=0;k<VrowNum;k++) s += rowptri[k] * V[k][j];
+ for (unsigned int k = 0; k < VrowNum; k++)
+ s += rowptri[k] * V[k][j];
ci[j] = s;
}
}
@@ -870,37 +1071,33 @@ vpMatrix vpMatrix::operator*(const vpForceTwistMatrix &V) const
}
/*!
-Operation C = A*wA + B*wB
+Operation C = A*wA + B*wB
The result is placed in the third parameter C and not returned.
-A new matrix won't be allocated for every use of the function
+A new matrix won't be allocated for every use of the function
(Speed gain if used many times with the same result matrix size).
\sa operator+()
*/
-void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const vpMatrix &B,const double &wB, vpMatrix &C){
- try
- {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(...) {
- throw ;
- }
+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())) ;
+ if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
+ A.getCols(), B.getRows(), B.getCols()));
}
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
- for (unsigned int i=0;i<A.rowNum;i++)
- for(unsigned int j=0;j<A.colNum;j++)
- CrowPtrs[i][j] = wB*BrowPtrs[i][j]+wA*ArowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++)
+ for (unsigned int j = 0; j < A.colNum; j++)
+ CrowPtrs[i][j] = wB * BrowPtrs[i][j] + wA * ArowPtrs[i][j];
}
/*!
@@ -913,34 +1110,29 @@ void vpMatrix::add2WeightedMatrices(const vpMatrix &A, const double &wA, const v
\sa operator+()
*/
void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
-{
- try {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum,B.colNum);
- }
- catch(...) {
- throw ;
- }
+{
+ if ((A.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())) ;
+ if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
+ A.getCols(), B.getRows(), B.getCols()));
}
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
- for (unsigned int i=0;i<A.rowNum;i++) {
- for(unsigned int j=0;j<A.colNum;j++) {
- CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++) {
+ for (unsigned int j = 0; j < A.colNum; j++) {
+ CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
}
}
}
/*!
- \warning This function is provided for compat with previous releases. You should
- rather use the functionalities provided in vpColVector class.
+ \warning This function is provided for compat with previous releases. You
+ should rather use the functionalities provided in vpColVector class.
Operation C = A + B.
@@ -952,26 +1144,21 @@ void vpMatrix::add2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
*/
void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
- try {
- if ((A.rowNum != C.rowNum) || (B.colNum != C.colNum)) C.resize(A.rowNum);
- }
- catch(...) {
- throw ;
- }
+ if ((A.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())) ;
+ if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix with (%dx%d) matrix", A.getRows(),
+ A.getCols(), B.getRows(), B.getCols()));
}
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
- for (unsigned int i=0;i<A.rowNum;i++) {
- for(unsigned int j=0;j<A.colNum;j++) {
- CrowPtrs[i][j] = BrowPtrs[i][j]+ArowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++) {
+ for (unsigned int j = 0; j < A.colNum; j++) {
+ CrowPtrs[i][j] = BrowPtrs[i][j] + ArowPtrs[i][j];
}
}
}
@@ -983,14 +1170,13 @@ void vpMatrix::add2Matrices(const vpColVector &A, const vpColVector &B, vpColVec
vpMatrix vpMatrix::operator+(const vpMatrix &B) const
{
vpMatrix C;
- vpMatrix::add2Matrices(*this,B,C);
+ vpMatrix::add2Matrices(*this, B, C);
return C;
}
-
/*!
- \warning This function is provided for compat with previous releases. You should
- rather use the functionalities provided in vpColVector class.
+ \warning This function is provided for compat with previous releases. You
+ should rather use the functionalities provided in vpColVector class.
Operation C = A - B on column vectors.
@@ -998,32 +1184,28 @@ vpMatrix vpMatrix::operator+(const vpMatrix &B) const
A new vector won't be allocated for every use of the function
(speed gain if used many times with the same result matrix size).
- \exception vpException::dimensionError If A and B vectors have not the same size.
+ \exception vpException::dimensionError If A and B vectors have not the same
+ size.
\sa vpColVector::operator-()
*/
void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVector &C)
{
- try {
- if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum);
- }
- catch(...) {
- throw ;
- }
+ if ((A.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())) ;
+ if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
+ A.getCols(), B.getRows(), B.getCols()));
}
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
- for (unsigned int i=0;i<A.rowNum;i++) {
- for(unsigned int j=0;j<A.colNum;j++) {
- CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++) {
+ for (unsigned int j = 0; j < A.colNum; j++) {
+ CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
}
}
}
@@ -1035,32 +1217,28 @@ void vpMatrix::sub2Matrices(const vpColVector &A, const vpColVector &B, vpColVec
A new matrix won't be allocated for every use of the function
(speed gain if used many times with the same result matrix size).
- \exception vpException::dimensionError If A and B matrices have not the same size.
+ \exception vpException::dimensionError If A and B matrices have not the same
+ size.
\sa operator-()
*/
void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
- try {
- if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
- }
- catch(...) {
- throw ;
- }
+ if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
+ 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())) ;
+ if ((A.colNum != B.getCols()) || (A.rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", A.getRows(),
+ A.getCols(), B.getRows(), B.getCols()));
}
- double ** ArowPtrs=A.rowPtrs;
- double ** BrowPtrs=B.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
- for (unsigned int i=0;i<A.rowNum;i++) {
- for(unsigned int j=0;j<A.colNum;j++) {
- CrowPtrs[i][j] = ArowPtrs[i][j]-BrowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++) {
+ for (unsigned int j = 0; j < A.colNum; j++) {
+ CrowPtrs[i][j] = ArowPtrs[i][j] - BrowPtrs[i][j];
}
}
}
@@ -1072,7 +1250,7 @@ void vpMatrix::sub2Matrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
vpMatrix vpMatrix::operator-(const vpMatrix &B) const
{
vpMatrix C;
- vpMatrix::sub2Matrices(*this,B,C);
+ vpMatrix::sub2Matrices(*this, B, C);
return C;
}
@@ -1080,33 +1258,31 @@ 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())) ;
+ if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot add (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
+ B.getRows(), B.getCols()));
}
- double ** BrowPtrs=B.rowPtrs;
+ double **BrowPtrs = B.rowPtrs;
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] += BrowPtrs[i][j];
return *this;
}
//! Operation A = A - B
-vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
+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())) ;
+ if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (%dx%d) matrix to (%dx%d) matrix", rowNum, colNum,
+ B.getRows(), B.getCols()));
}
- double ** BrowPtrs=B.rowPtrs;
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+ double **BrowPtrs = B.rowPtrs;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] -= BrowPtrs[i][j];
return *this;
@@ -1123,42 +1299,34 @@ vpMatrix & vpMatrix::operator-=(const vpMatrix &B)
*/
void vpMatrix::negateMatrix(const vpMatrix &A, vpMatrix &C)
{
- try {
- if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum)) C.resize(A.rowNum,A.colNum);
- }
- catch(...) {
- throw ;
- }
+ if ((A.rowNum != C.rowNum) || (A.colNum != C.colNum))
+ C.resize(A.rowNum, A.colNum, false, false);
- double ** ArowPtrs=A.rowPtrs;
- double ** CrowPtrs=C.rowPtrs;
+ double **ArowPtrs = A.rowPtrs;
+ double **CrowPtrs = C.rowPtrs;
// t0=vpTime::measureTimeMicros();
- for (unsigned int i=0;i<A.rowNum;i++)
- for(unsigned int j=0;j<A.colNum;j++)
- CrowPtrs[i][j]= -ArowPtrs[i][j];
+ for (unsigned int i = 0; i < A.rowNum; i++)
+ for (unsigned int j = 0; j < A.colNum; j++)
+ CrowPtrs[i][j] = -ArowPtrs[i][j];
}
/*!
Operation C = -A (A is unchanged).
\sa negateMatrix() to avoid matrix allocation for each use.
*/
-vpMatrix vpMatrix::operator-() const //negate
+vpMatrix vpMatrix::operator-() const // negate
{
vpMatrix C;
- vpMatrix::negateMatrix(*this,C);
+ vpMatrix::negateMatrix(*this, C);
return C;
}
-
-double
-vpMatrix::sum() const
+double vpMatrix::sum() const
{
- double s=0.0;
- for (unsigned int i=0;i<rowNum;i++)
- {
- for(unsigned int j=0;j<colNum;j++)
- {
+ double s = 0.0;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
s += rowPtrs[i][j];
}
}
@@ -1166,14 +1334,10 @@ vpMatrix::sum() const
return s;
}
-
//---------------------------------
// Matrix/vector operations.
//---------------------------------
-
-
-
//---------------------------------
// Matrix/real operations.
//---------------------------------
@@ -1182,18 +1346,18 @@ vpMatrix::sum() const
\relates vpMatrix
Allow to multiply a scalar by a matrix.
*/
-vpMatrix operator*(const double &x,const vpMatrix &B)
+vpMatrix operator*(const double &x, const vpMatrix &B)
{
vpMatrix C(B.getRows(), B.getCols());
- unsigned int Brow = B.getRows() ;
- unsigned int Bcol = B.getCols() ;
+ unsigned int Brow = B.getRows();
+ unsigned int Bcol = B.getCols();
- for (unsigned int i=0;i<Brow;i++)
- for(unsigned int j=0;j<Bcol;j++)
- C[i][j]= B[i][j]*x;
+ for (unsigned int i = 0; i < Brow; i++)
+ for (unsigned int j = 0; j < Bcol; j++)
+ C[i][j] = B[i][j] * x;
- return C ;
+ return C;
}
/*!
@@ -1202,59 +1366,52 @@ vpMatrix operator*(const double &x,const vpMatrix &B)
*/
vpMatrix vpMatrix::operator*(double x) const
{
- vpMatrix M(rowNum,colNum);
+ vpMatrix M(rowNum, colNum);
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- M[i][j]= rowPtrs[i][j]*x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ M[i][j] = rowPtrs[i][j] * x;
return M;
}
//! Cij = Aij / x (A is unchanged)
-vpMatrix vpMatrix::operator/(double x) const
+vpMatrix vpMatrix::operator/(double x) const
{
vpMatrix C;
- try {
- C.resize(rowNum,colNum);
- }
- catch(...) {
- throw ;
- }
+ C.resize(rowNum, colNum, false, false);
- //if (x == 0) {
+ // if (x == 0) {
if (std::fabs(x) <= std::numeric_limits<double>::epsilon()) {
throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
}
- double xinv = 1/x ;
+ double xinv = 1 / x;
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- C[i][j]=rowPtrs[i][j]*xinv;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ C[i][j] = rowPtrs[i][j] * xinv;
return C;
}
-
//! Add x to all the element of the matrix : Aij = Aij + x
-vpMatrix & vpMatrix::operator+=(double x)
+vpMatrix &vpMatrix::operator+=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]+=x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ rowPtrs[i][j] += x;
return *this;
}
-
//! Substract x to all the element of the matrix : Aij = Aij - x
-vpMatrix & vpMatrix::operator-=(double x)
+vpMatrix &vpMatrix::operator-=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]-=x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ rowPtrs[i][j] -= x;
return *this;
}
@@ -1263,27 +1420,27 @@ vpMatrix & vpMatrix::operator-=(double x)
Operator that allows to multiply all the elements of a matrix
by a scalar.
*/
-vpMatrix & vpMatrix::operator*=(double x)
+vpMatrix &vpMatrix::operator*=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]*=x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ rowPtrs[i][j] *= x;
return *this;
}
//! Divide all the element of the matrix by x : Aij = Aij / x
-vpMatrix & vpMatrix::operator/=(double x)
+vpMatrix &vpMatrix::operator/=(double x)
{
- //if (x == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
+ // if (x == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon())
throw vpException(vpException::divideByZeroError, "Divide matrix by zero scalar");
- double xinv = 1/x ;
+ double xinv = 1 / x;
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]*=xinv;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ rowPtrs[i][j] *= xinv;
return *this;
}
@@ -1292,72 +1449,91 @@ vpMatrix & vpMatrix::operator/=(double x)
// Matrix Operation
//----------------------------------------------------------------
-
-
-
-
-
-
-/*!
+/*!
Stacks columns of a matrix in a vector.
\param out : a vpColVector.
*/
-void vpMatrix::stackColumns(vpColVector &out ){
-
- try {
- if ((out.rowNum != colNum*rowNum) || (out.colNum != 1)) out.resize(rowNum);
- }
- catch(...) {
- throw ;
- }
+void vpMatrix::stackColumns(vpColVector &out)
+{
+ if ((out.rowNum != colNum * rowNum) || (out.colNum != 1))
+ out.resize(colNum * rowNum, false, false);
- double *optr=out.data;
- for(unsigned int j =0;j<colNum ; j++){
- for(unsigned int i =0;i<rowNum ; i++){
- *(optr++)=rowPtrs[i][j];
+ double *optr = out.data;
+ for (unsigned int j = 0; j < colNum; j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ *(optr++) = rowPtrs[i][j];
}
}
}
/*!
Stacks columns of a matrix in a vector.
- \return a vpColVector.
+ \return a vpColVector.
*/
vpColVector vpMatrix::stackColumns()
{
- vpColVector out(colNum*rowNum);
+ vpColVector out(colNum * rowNum);
stackColumns(out);
return out;
}
-/*!
+/*!
Stacks rows of a matrix in a vector
\param out : a vpRowVector.
*/
void vpMatrix::stackRows(vpRowVector &out)
{
- try {
- if ((out.getRows() != 1) || (out.getCols() != colNum*rowNum)) out.resize(rowNum);
- }
- catch(...) {
- throw ;
- }
+ if ((out.getRows() != 1) || (out.getCols() != colNum * rowNum))
+ out.resize(colNum * rowNum, false, false);
- double *mdata=data;
- double *optr=out.data;
- for(unsigned int i =0;i<dsize ; i++){
- *(optr++)=*(mdata++);
+ double *mdata = data;
+ double *optr = out.data;
+ for (unsigned int i = 0; i < dsize; i++) {
+ *(optr++) = *(mdata++);
}
}
-/*!
+/*!
Stacks rows of a matrix in a vector.
\return a vpRowVector.
*/
vpRowVector vpMatrix::stackRows()
{
- vpRowVector out(colNum*rowNum);
- stackRows(out );
- return out;
+ vpRowVector out(colNum * rowNum);
+ stackRows(out);
+ return out;
+}
+
+/*!
+ Compute the Hadamard product (element wise matrix multiplication).
+ \param m : Second matrix;
+ \return m1.hadamard(m2) The Hadamard product : \f$ m1 \circ m2 = (m1 \circ
+ m2)_{i,j} = (m1)_{i,j} (m2)_{i,j} \f$
+*/
+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"));
+ }
+
+ vpMatrix out;
+ out.resize(rowNum, colNum, false);
+
+ unsigned int i = 0;
+
+#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));
+ _mm_storeu_pd(out.data + i, vout);
+ }
+ }
+#endif
+
+ for (; i < dsize; i++) {
+ out.data[i] = data[i] * m.data[i];
+ }
+
+ return out;
}
/*!
@@ -1366,45 +1542,40 @@ vpRowVector vpMatrix::stackRows()
\param m2 : vpMatrix;
\param out : The kronecker product : \f$ m1 \otimes m2 \f$
*/
-void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2 , vpMatrix &out)
+void vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2, vpMatrix &out)
{
- unsigned int r1= m1.getRows();
- unsigned int c1= m1.getCols();
- unsigned int r2= m2.getRows();
- unsigned int c2= m2.getCols();
+ unsigned int r1 = m1.getRows();
+ unsigned int c1 = m1.getCols();
+ unsigned int r2 = m2.getRows();
+ unsigned int c2 = m2.getCols();
- if (r1*r2 !=out.rowNum || c1*c2!= out.colNum )
- {
- vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix") ;
- throw(vpException(vpException::dimensionError,
- "In Kronecker product bad dimension of output matrix"));
+ if (r1 * r2 != out.rowNum || c1 * c2 != out.colNum) {
+ vpERROR_TRACE("Kronecker prodect bad dimension of output vpMatrix");
+ throw(vpException(vpException::dimensionError, "In Kronecker product bad dimension of output matrix"));
}
- for(unsigned int r =0;r<r1 ; r++){
- for(unsigned int c =0;c<c1 ; c++){
+ for (unsigned int r = 0; r < r1; r++) {
+ for (unsigned int c = 0; c < c1; c++) {
double alpha = m1[r][c];
double *m2ptr = m2[0];
- unsigned int roffset= r*r2;
- unsigned int coffset= c*c2;
- for(unsigned int rr =0;rr<r2 ; rr++){
- for(unsigned int cc =0;cc<c2 ;cc++){
- out[roffset+rr][coffset+cc]= alpha* *(m2ptr++);
+ unsigned int roffset = r * r2;
+ unsigned int coffset = c * c2;
+ for (unsigned int rr = 0; rr < r2; rr++) {
+ for (unsigned int cc = 0; cc < c2; cc++) {
+ out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
}
}
}
}
-
}
/*!
Compute Kronecker product matrix.
\param m : vpMatrix.
- \param out : If m1.kron(m2) out contains the kronecker product's result : \f$ m1 \otimes m2 \f$.
+ \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.
@@ -1414,22 +1585,22 @@ void vpMatrix::kron(const vpMatrix &m , vpMatrix &out) const
*/
vpMatrix vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2)
{
- unsigned int r1= m1.getRows();
- unsigned int c1= m1.getCols();
- unsigned int r2= m2.getRows();
- unsigned int c2= m2.getCols();
+ unsigned int r1 = m1.getRows();
+ unsigned int c1 = m1.getCols();
+ unsigned int r2 = m2.getRows();
+ unsigned int c2 = m2.getCols();
- vpMatrix out(r1*r2,c1*c2);
+ vpMatrix out(r1 * r2, c1 * c2);
- for(unsigned int r =0;r<r1 ; r++){
- for(unsigned int c =0;c<c1 ; c++){
+ for (unsigned int r = 0; r < r1; r++) {
+ for (unsigned int c = 0; c < c1; c++) {
double alpha = m1[r][c];
double *m2ptr = m2[0];
- unsigned int roffset= r*r2;
- unsigned int coffset= c*c2;
- for(unsigned int rr =0;rr<r2 ; rr++){
- for(unsigned int cc =0;cc<c2 ;cc++){
- out[roffset+rr ][coffset+cc]= alpha* *(m2ptr++);
+ unsigned int roffset = r * r2;
+ unsigned int coffset = c * c2;
+ for (unsigned int rr = 0; rr < r2; rr++) {
+ for (unsigned int cc = 0; cc < c2; cc++) {
+ out[roffset + rr][coffset + cc] = alpha * *(m2ptr++);
}
}
}
@@ -1437,16 +1608,12 @@ vpMatrix vpMatrix::kron(const vpMatrix &m1, const vpMatrix &m2)
return out;
}
-
/*!
Compute Kronecker product matrix.
\param m : vpMatrix;
\return m1.kron(m2) The kronecker product : \f$ m1 \otimes m2 \f$
*/
-vpMatrix vpMatrix::kron(const vpMatrix &m) const
-{
- return kron(*this,m);
-}
+vpMatrix vpMatrix::kron(const vpMatrix &m) const { return kron(*this, m); }
/*!
@@ -1468,17 +1635,17 @@ int main()
{
vpMatrix A(3,3);
-A[0][0] = 4.64;
-A[0][1] = 0.288;
-A[0][2] = -0.384;
+A[0][0] = 4.64;
+A[0][1] = 0.288;
+A[0][2] = -0.384;
-A[1][0] = 0.288;
-A[1][1] = 7.3296;
-A[1][2] = 2.2272;
+A[1][0] = 0.288;
+A[1][1] = 7.3296;
+A[1][2] = 2.2272;
-A[2][0] = -0.384;
-A[2][1] = 2.2272;
-A[2][2] = 6.0304;
+A[2][0] = -0.384;
+A[2][1] = 2.2272;
+A[2][2] = 6.0304;
vpColVector X(3), B(3);
B[0] = 1;
@@ -1488,9 +1655,9 @@ B[2] = 3;
A.solveBySVD(B, X);
// Obtained values of X
-// X[0] = 0.2468;
-// X[1] = 0.120782;
-// X[2] = 0.468587;
+// X[0] = 0.2468;
+// X[1] = 0.120782;
+// X[2] = 0.468587;
std::cout << "X:\n" << X << std::endl;
}
@@ -1498,12 +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; }
/*!
@@ -1525,17 +1687,17 @@ int main()
{
vpMatrix A(3,3);
-A[0][0] = 4.64;
-A[0][1] = 0.288;
-A[0][2] = -0.384;
+A[0][0] = 4.64;
+A[0][1] = 0.288;
+A[0][2] = -0.384;
-A[1][0] = 0.288;
-A[1][1] = 7.3296;
-A[1][2] = 2.2272;
+A[1][0] = 0.288;
+A[1][1] = 7.3296;
+A[1][2] = 2.2272;
-A[2][0] = -0.384;
-A[2][1] = 2.2272;
-A[2][2] = 6.0304;
+A[2][0] = -0.384;
+A[2][1] = 2.2272;
+A[2][2] = 6.0304;
vpColVector X(3), B(3);
B[0] = 1;
@@ -1544,9 +1706,9 @@ B[2] = 3;
X = A.solveBySVD(B);
// Obtained values of X
-// X[0] = 0.2468;
-// X[1] = 0.120782;
-// X[2] = 0.468587;
+// X[0] = 0.2468;
+// X[1] = 0.120782;
+// X[2] = 0.468587;
std::cout << "X:\n" << X << std::endl;
}
@@ -1562,592 +1724,1994 @@ vpColVector vpMatrix::solveBySVD(const vpColVector &B) const
return X;
}
-
/*!
- Singular value decomposition (SVD).
+ Matrix singular value decomposition (SVD).
+
+ This function calls the first following function that is available:
+ - svdLapack() if Lapack 3rd party is installed
+ - svdEigen3() if Eigen3 3rd party is installed
+ - svdOpenCV() if OpenCV 3rd party is installed
+ - svdGsl() if GSL 3rd party is installed.
+
+ If none of these previous 3rd parties is installed, we use by default
+svdLapack() with a Lapack built-in version.
+
+ Given matrix \f$M\f$, this function computes it singular value decomposition
+such as
\f[ M = U \Sigma V^{\top} \f]
- \warning Destructive method wrt. to the matrix \f$ M \f$ to
- decompose. You should make a COPY of that matrix if needed not to
- CHANGE.
+ \warning This method is destructive wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed.
- \param w : Vector of singular values. \f$ \Sigma = diag(w) \f$.
+ \param w : Vector of singular values: \f$ \Sigma = diag(w) \f$.
- \param v : Matrix \f$ V \f$.
+ \param V : Matrix \f$ V \f$.
\return Matrix \f$ U \f$.
- \warning If the GNU Scientific Library (GSL) third party library is used to compute the SVD
- decomposition, the singular values \f$ \Sigma_{i,i} \f$ are ordered in decreasing
- fashion in \e w. This is not the case, if the GSL is not detected by ViSP.
+ \note The singular values are ordered in decreasing
+ fashion in \e w. It means that the highest singular value is in \e w[0].
Here an example of SVD decomposition of a non square Matrix M.
\code
-#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrix.h>
int main()
{
vpMatrix M(3,2);
- M[0][0] = 1;
- M[1][0] = 2;
- M[2][0] = 0.5;
-
- M[0][1] = 6;
- M[1][1] = 8 ;
- M[2][1] = 9 ;
+ M[0][0] = 1; M[0][1] = 6;
+ M[1][0] = 2; M[1][1] = 8;
+ M[2][0] = 0.5; M[2][1] = 9;
- vpMatrix v;
vpColVector w;
- vpMatrix Mrec;
- vpMatrix Sigma;
+ vpMatrix V, Sigma, U = M;
- M.svd(w, v);
- // Here M is modified and is now equal to U
+ U.svd(w, V);
// Construct the diagonal matrix from the singular values
Sigma.diag(w);
- // Reconstruct the initial matrix M using the decomposition
- Mrec = M * Sigma * v.t();
+ // Reconstruct the initial matrix using the decomposition
+ vpMatrix Mrec = U * Sigma * V.t();
// Here, Mrec is obtained equal to the initial value of M
- // Mrec[0][0] = 1;
- // Mrec[1][0] = 2;
- // Mrec[2][0] = 0.5;
- // Mrec[0][1] = 6;
- // Mrec[1][1] = 8 ;
- // Mrec[2][1] = 9 ;
+ // Mrec[0][0] = 1; Mrec[0][1] = 6;
+ // Mrec[1][0] = 2; Mrec[1][1] = 8;
+ // Mrec[2][0] = 0.5; Mrec[2][1] = 9;
std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
}
-\endcode
+ \endcode
+ \sa svdLapack(), svdEigen3(), svdOpenCV(), svdGsl()
*/
-void
-vpMatrix::svd(vpColVector& w, vpMatrix& v)
+void vpMatrix::svd(vpColVector &w, vpMatrix &V)
{
-#if 1 /* no verification */
- {
- w.resize( this->getCols() );
- v.resize( this->getCols(), this->getCols() );
-
-#if defined (VISP_HAVE_LAPACK_C)
- svdLapack(w,v);
+#if defined(VISP_HAVE_LAPACK)
+ svdLapack(w, V);
+#elif defined(VISP_HAVE_EIGEN3)
+ svdEigen3(w, V);
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
- svdOpenCV(w,v);
-#elif defined (VISP_HAVE_GSL) /* be careful of the copy below */
- svdGsl(w,v) ;
+ svdOpenCV(w, V);
+#elif defined(VISP_HAVE_GSL)
+ svdGsl(w, V);
#else
- svdNr(w,v) ;
+ (void)w;
+ (void)V;
+ throw(vpException(vpException::fatalError, "Cannot compute SVD. Install Lapack, Eigen3, OpenCV or GSL 3rd party"));
#endif
+}
- //svdNr(w,v) ;
- }
-#else /* verification of the SVD */
- {
- int pb = 0;
- unsigned int i,j,k,nrows,ncols;
- vpMatrix A, Asvd;
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ and return the rank r of the matrix.
- A = (*this); /* copy because svd is destructive */
+ \note By default, this function uses Lapack 3rd party. It is also possible
+to use a specific 3rd party suffixing this function name with one of the
+following 3rd party names (Lapack, Eigen3, OpenCV or Gsl).
- w.resize( this->getCols() );
- v.resize( this->getCols(), this->getCols() );
-#ifdef VISP_HAVE_GSL /* be careful of the copy above */
- svdGsl(w,v) ;
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ unsigned int rank = A.pseudoInverse(A_p);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+}
+ \endcode
+
+ Once build, the previous example produces the following output:
+ \code
+A: [2,3]=
+ 2 3 5
+ -4 2 3
+A^+ (pseudo-inverse): [3,2]=
+ 0.117899 -0.190782
+ 0.065380 0.039657
+ 0.113612 0.052518
+Rank: 2
+ \endcode
+*/
+unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
+{
+#if defined(VISP_HAVE_LAPACK)
+ return pseudoInverseLapack(Ap, svThreshold);
+#elif defined(VISP_HAVE_EIGEN3)
+ return pseudoInverseEigen3(Ap, svThreshold);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ return pseudoInverseOpenCV(Ap, svThreshold);
+#elif defined(VISP_HAVE_GSL)
+ return pseudoInverseGsl(Ap, svThreshold);
#else
- svdNr(w,v) ;
-#endif
- //svdNr(w,v) ;
-
- nrows = A.getRows();
- ncols = A.getCols();
- Asvd.resize(nrows,ncols);
-
- for (i = 0 ; i < nrows ; i++)
- {
- for (j = 0 ; j < ncols ; j++)
- {
- Asvd[i][j] = 0.0;
- for (k=0 ; k < ncols ; k++) Asvd[i][j] += (*this)[i][k]*w[k]*v[j][k];
- }
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(A[i][j]-Asvd[i][j]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in SVD\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Asvd : " << std::endl << Asvd << std::endl;
- }
- // else printf("SVD ok ;-)\n"); /* It's so good... */
- }
+ (void)Ap;
+ (void)svThreshold;
+ throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
+ "Install Lapack, Eigen3, OpenCV "
+ "or GSL 3rd party"));
#endif
}
+
/*!
- Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
- \param Ap : The pseudo inverse \f$ A^+ \f$.
- \param svThreshold : Threshold used to test the singular values.
- \return Return the rank of the matrix A
+ Compute and return the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n
+matrix \f$\bf A\f$.
+
+ \note By default, this function uses Lapack 3rd party. It is also possible
+to use a specific 3rd party suffixing this function name with one of the
+following 3rd party names (Lapack, Eigen3, OpenCV or Gsl).
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p = A.pseudoInverse();
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+}
+ \endcode
+
+ Once build, the previous example produces the following output:
+ \code
+A: [2,3]=
+ 2 3 5
+ -4 2 3
+A^+ (pseudo-inverse): [3,2]=
+ 0.117899 -0.190782
+ 0.065380 0.039657
+ 0.113612 0.052518
+ \endcode
+
*/
+vpMatrix vpMatrix::pseudoInverse(double svThreshold) const
+{
+#if defined(VISP_HAVE_LAPACK)
+ return pseudoInverseLapack(svThreshold);
+#elif defined(VISP_HAVE_EIGEN3)
+ return pseudoInverseEigen3(svThreshold);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ return pseudoInverseOpenCV(svThreshold);
+#elif defined(VISP_HAVE_GSL)
+ return pseudoInverseGsl(svThreshold);
+#else
+ (void)w;
+ (void)V;
+ throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
+ "Install Lapack, Eigen3, OpenCV "
+ "or GSL 3rd party"));
+#endif
+}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#if defined(VISP_HAVE_LAPACK)
+/*!
+ Compute and return the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n
+matrix \f$\bf A\f$ using Lapack 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap, double svThreshold) const
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p = A.pseudoInverseLapack();
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+}
+ \endcode
+
+ \sa pseudoInverse(double) const
+*/
+vpMatrix vpMatrix::pseudoInverseLapack(double svThreshold) const
{
- vpColVector sv ;
- return pseudoInverse(Ap, sv, svThreshold) ;
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+
+ vpMatrix Ap(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdLapack(sv, V);
+
+ unsigned int rank;
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return Ap;
}
/*!
- Compute and return the pseudo inverse of a n-by-m matrix : \f$ A^+ \f$
- \param svThreshold : Threshold used to test the singular values.
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ and return the rank r of the matrix using Lapack 3rd party.
- \return Pseudo inverse of the matrix.
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
- Here an example to compute the inverse of a n-by-n matrix. If the
- matrix is n-by-n it is also possible to use inverseByLU().
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
-\code
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
#include <visp3/core/vpMatrix.h>
int main()
{
- vpMatrix A(4,4);
+ vpMatrix A(2, 3);
- A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
- A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
- A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
- A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
- // Compute the inverse
- vpMatrix A_1; // A^-1
- A_1 = A.pseudoInverse();
- std::cout << "Inverse by pseudo inverse: \n" << A_1 << std::endl;
+ vpMatrix A_p;
+ unsigned int rank = A.pseudoInverseLapack(A_p);
- std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
}
-\endcode
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdLapack(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values and return the rank r of the matrix using
+Lapack 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ vpColVector sv;
+ unsigned int rank = A.pseudoInverseLapack(A_p, sv);
- \sa inverseByLU()
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, vpColVector &, double) const
*/
-vpMatrix
-vpMatrix::pseudoInverse(double svThreshold) const
+unsigned int vpMatrix::pseudoInverseLapack(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
{
- vpMatrix Ap ;
- vpColVector sv ;
- pseudoInverse(Ap, sv, svThreshold) ;
- return Ap ;
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ sv.resize(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdLapack(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
}
/*!
- Compute the pseudo inverse of the matrix \f$Ap = A^+\f$
- \param Ap : The pseudo inverse \f$ A^+ \f$.
- \param sv : Singular values.
- \param svThreshold : Threshold used to test the singular values.
- \return Return the rank of the matrix A
-*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$, \f$\mbox{Im}(A^T)\f$ and
+\f$\mbox{Ker}(A)\f$ and return the rank r of the matrix using Lapack 3rd
+party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather
+inverseByLU(), inverseByCholesky(), or inverseByQR() that are kwown as faster.
+
+ Using singular value decomposition, we have:
+
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf
+V^\top}_{n\times n} \f] \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | &
+ \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix
+\f$A\f$ singular values.
+
+ This equation could be reformulated in a minimal way:
+ \f[
+ {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix
+\f$A\f$ first r singular values.
+
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+= { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
+
+ \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
+
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
+
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
{
- vpMatrix imA, imAt ;
- return pseudoInverse(Ap, sv, svThreshold, imA, imAt) ;
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpColVector sv;
+ vpMatrix A_p, imA, imAt, kerAt;
+ unsigned int rank = A.pseudoInverseLapack(A_p, sv, 1e-6, imA, imAt, kerAt);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+
+ if (kerAt.size()) {
+ kerAt.t().print(std::cout, 10, "Ker(A): ");
+ }
+ else {
+ std::cout << "Ker(A) empty " << std::endl;
+ }
+
+ // Reconstruct matrix A from ImA, ImAt, KerAt
+ vpMatrix S(rank, A.getCols());
+ for(unsigned int i = 0; i< rank; i++)
+ S[i][i] = sv[i];
+ vpMatrix Vt(A.getCols(), A.getCols());
+ Vt.insert(imAt.t(), 0, 0);
+ Vt.insert(kerAt, rank, 0);
+ (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:");
}
+ \endcode
+
+ \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 nrows = getRows();
+ unsigned int ncols = getCols();
+ unsigned int rank;
+ vpMatrix U, V;
+ vpColVector sv_;
+
+ if (nrows < ncols) {
+ U.resize(ncols, ncols);
+ sv.resize(nrows);
+ } else {
+ U.resize(nrows, ncols);
+ sv.resize(ncols);
+ }
+
+ U.insert(*this, 0, 0);
+ U.svdLapack(sv_, V);
+
+ 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
+ for (unsigned int i = 0; i < sv.size(); i++)
+ sv[i] = sv_[i];
+
+ return rank;
+}
+#endif
+#if defined(VISP_HAVE_EIGEN3)
+/*!
+ Compute and return the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n
+matrix \f$\bf A\f$ using Eigen3 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p = A.pseudoInverseEigen3();
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+}
+ \endcode
+
+ \sa pseudoInverse(double)
+*/
+vpMatrix vpMatrix::pseudoInverseEigen3(double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+
+ vpMatrix Ap(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdEigen3(sv, V);
+
+ unsigned int rank;
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return Ap;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ and return the rank r of the matrix using Eigen3 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ unsigned int rank = A.pseudoInverseEigen3(A_p);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdEigen3(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values and return the rank r of the matrix using
+Eigen3 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ vpColVector sv;
+ unsigned int rank = A.pseudoInverseEigen3(A_p, sv);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, vpColVector &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseEigen3(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ sv.resize(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdEigen3(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$, \f$\mbox{Im}(A^T)\f$ and
+\f$\mbox{Ker}(A)\f$ and return the rank r of the matrix using Eigen3 3rd
+party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather
+inverseByLU(), inverseByCholesky(), or inverseByQR() that are kwown as faster.
+
+ Using singular value decomposition, we have:
+
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf
+V^\top}_{n\times n} \f] \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | &
+ \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix
+\f$A\f$ singular values.
+
+ This equation could be reformulated in a minimal way:
+ \f[
+ {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix
+\f$A\f$ first r singular values.
+
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+= { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
+
+ \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
+
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
+
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpColVector sv;
+ vpMatrix A_p, imA, imAt, kerAt;
+ unsigned int rank = A.pseudoInverseEigen3(A_p, sv, 1e-6, imA, imAt, kerAt);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+
+ if (kerAt.size()) {
+ kerAt.t().print(std::cout, 10, "Ker(A): ");
+ }
+ else {
+ std::cout << "Ker(A) empty " << std::endl;
+ }
+
+ // Reconstruct matrix A from ImA, ImAt, KerAt
+ vpMatrix S(rank, A.getCols());
+ for(unsigned int i = 0; i< rank; i++)
+ S[i][i] = sv[i];
+ vpMatrix Vt(A.getCols(), A.getCols());
+ Vt.insert(imAt.t(), 0, 0);
+ Vt.insert(kerAt, rank, 0);
+ (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:");
+}
+ \endcode
+
+ \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 nrows = getRows();
+ unsigned int ncols = getCols();
+ unsigned int rank;
+ vpMatrix U, V;
+ vpColVector sv_;
+
+ if (nrows < ncols) {
+ U.resize(ncols, ncols);
+ sv.resize(nrows);
+ } else {
+ U.resize(nrows, ncols);
+ sv.resize(ncols);
+ }
+
+ U.insert(*this, 0, 0);
+ U.svdEigen3(sv_, V);
+
+ 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
+ for (unsigned int i = 0; i < sv.size(); i++)
+ sv[i] = sv_[i];
+
+ return rank;
+}
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+/*!
+ Compute and return the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n
+matrix \f$\bf A\f$ using OpenCV 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p = A.pseudoInverseEigen3();
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+}
+ \endcode
+
+ \sa pseudoInverse(double) const
+*/
+vpMatrix vpMatrix::pseudoInverseOpenCV(double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+
+ vpMatrix Ap(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdOpenCV(sv, V);
+
+ unsigned int rank;
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return Ap;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ and return the rank r of the matrix using OpenCV 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ unsigned int rank = A.pseudoInverseOpenCV(A_p);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdOpenCV(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values and return the rank r of the matrix using
+OpenCV 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ vpColVector sv;
+ unsigned int rank = A.pseudoInverseOpenCV(A_p, sv);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, vpColVector &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseOpenCV(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ sv.resize(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdOpenCV(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$, \f$\mbox{Im}(A^T)\f$ and
+\f$\mbox{Ker}(A)\f$ and return the rank r of the matrix using OpenCV 3rd
+party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather
+inverseByLU(), inverseByCholesky(), or inverseByQR() that are kwown as faster.
+
+ Using singular value decomposition, we have:
+
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf
+V^\top}_{n\times n} \f] \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | &
+ \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix
+\f$A\f$ singular values.
+
+ This equation could be reformulated in a minimal way:
+ \f[
+ {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix
+\f$A\f$ first r singular values.
+
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+= { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
+
+ \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
+
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
+
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpColVector sv;
+ vpMatrix A_p, imA, imAt, kerAt;
+ unsigned int rank = A.pseudoInverseOpenCV(A_p, sv, 1e-6, imA, imAt, kerAt);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+
+ if (kerAt.size()) {
+ kerAt.t().print(std::cout, 10, "Ker(A): ");
+ }
+ else {
+ std::cout << "Ker(A) empty " << std::endl;
+ }
+
+ // Reconstruct matrix A from ImA, ImAt, KerAt
+ vpMatrix S(rank, A.getCols());
+ for(unsigned int i = 0; i< rank; i++)
+ S[i][i] = sv[i];
+ vpMatrix Vt(A.getCols(), A.getCols());
+ Vt.insert(imAt.t(), 0, 0);
+ Vt.insert(kerAt, rank, 0);
+ (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:");
+}
+ \endcode
+
+ \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 nrows = getRows();
+ unsigned int ncols = getCols();
+ unsigned int rank;
+ vpMatrix U, V;
+ vpColVector sv_;
+
+ if (nrows < ncols) {
+ U.resize(ncols, ncols);
+ sv.resize(nrows);
+ } else {
+ U.resize(nrows, ncols);
+ sv.resize(ncols);
+ }
+
+ U.insert(*this, 0, 0);
+ U.svdOpenCV(sv_, V);
+
+ 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
+ for (unsigned int i = 0; i < sv.size(); i++)
+ sv[i] = sv_[i];
+
+ return rank;
+}
+#endif
+#if defined(VISP_HAVE_GSL)
+/*!
+ Compute and return the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n
+matrix \f$\bf A\f$ using GSL 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p = A.pseudoInverseGsl();
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+}
+ \endcode
+
+ \sa pseudoInverse(double) const
+*/
+vpMatrix vpMatrix::pseudoInverseGsl(double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+
+ vpMatrix Ap(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdGsl(sv, V);
+
+ unsigned int rank;
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return Ap;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ and return the rank r of the matrix using GSL 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ unsigned int rank = A.pseudoInverseGsl(A_p);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ vpColVector sv(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdGsl(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values and return the rank r of the matrix using GSL
+3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
+
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpMatrix A_p;
+ vpColVector sv;
+ unsigned int rank = A.pseudoInverseGsl(A_p, sv);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, vpColVector &, double) const
+*/
+unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+{
+ unsigned int nrows, ncols;
+ unsigned int nrows_orig = getRows();
+ unsigned int ncols_orig = getCols();
+ unsigned int rank;
+
+ Ap.resize(ncols_orig, nrows_orig);
+
+ if (nrows_orig >= ncols_orig) {
+ nrows = nrows_orig;
+ ncols = ncols_orig;
+ } else {
+ nrows = ncols_orig;
+ ncols = nrows_orig;
+ }
+
+ vpMatrix U(nrows, ncols);
+ vpMatrix V(ncols, ncols);
+ sv.resize(ncols);
+
+ if (nrows_orig >= ncols_orig)
+ U = *this;
+ else
+ U = (*this).t();
+
+ U.svdGsl(sv, V);
+
+ compute_pseudo_inverse(U, sv, V, nrows, ncols, nrows_orig, ncols_orig, svThreshold, Ap, rank);
+
+ return rank;
+}
+
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$, \f$\mbox{Im}(A^T)\f$ and
+\f$\mbox{Ker}(A)\f$ and return the rank r of the matrix using GSL 3rd party.
+
+ \warning To inverse a square n-by-n matrix, you have to use rather
+inverseByLU(), inverseByCholesky(), or inverseByQR() that are kwown as faster.
+
+ Using singular value decomposition, we have:
+
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf
+V^\top}_{n\times n} \f] \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | &
+ \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix
+\f$A\f$ singular values.
+
+ This equation could be reformulated in a minimal way:
+ \f[
+ {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+
+ where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix
+\f$A\f$ first r singular values.
+
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+= { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
+
+ \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$.
+
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
+
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
+
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
+
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
+
+ \return The rank r of the matrix \f$\bf A\f$.
+
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
+
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(2, 3);
+
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
+
+ A.print(std::cout, 10, "A: ");
+
+ vpColVector sv;
+ vpMatrix A_p, imA, imAt, kerAt;
+ unsigned int rank = A.pseudoInverseGsl(A_p, sv, 1e-6, imA, imAt, kerAt);
+
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+
+ if (kerAt.size()) {
+ kerAt.t().print(std::cout, 10, "Ker(A): ");
+ }
+ else {
+ std::cout << "Ker(A) empty " << std::endl;
+ }
+
+ // Reconstruct matrix A from ImA, ImAt, KerAt
+ vpMatrix S(rank, A.getCols());
+ for(unsigned int i = 0; i< rank; i++)
+ S[i][i] = sv[i];
+ vpMatrix Vt(A.getCols(), A.getCols());
+ Vt.insert(imAt.t(), 0, 0);
+ Vt.insert(kerAt, rank, 0);
+ (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:");
+}
+ \endcode
+
+ \sa pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &,
+vpMatrix &) const
+*/
+unsigned int vpMatrix::pseudoInverseGsl(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
+ vpMatrix &imAt, vpMatrix &kerA) const
+{
+ unsigned int nrows = getRows();
+ unsigned int ncols = getCols();
+ unsigned int rank;
+ vpMatrix U, V;
+ vpColVector sv_;
+
+ if (nrows < ncols) {
+ U.resize(ncols, ncols);
+ sv.resize(nrows);
+ } else {
+ U.resize(nrows, ncols);
+ sv.resize(ncols);
+ }
-/*!
- Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
+ U.insert(*this, 0, 0);
+ U.svdGsl(sv_, V);
- Pseudo inverse, kernel and image are computed using the SVD decomposition.
+ compute_pseudo_inverse(U, sv_, V, nrows, ncols, svThreshold, Ap, rank, imA, imAt, kerA);
- A is an m x n matrix,
- if m >=n the svd works on A other wise it works on \f$A^T\f$.
+ // Remove singular values equal to to that correspond to the lines of 0
+ // introduced when m < n
+ for (unsigned int i = 0; i < sv.size(); i++)
+ sv[i] = sv_[i];
- Therefore if m>=n we have
+ return rank;
+}
+#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
- \f[
- {\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
- \f]
- \f[
- {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
- \mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
- \left[
- \begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
- \f]
- where
- Im(A) is an m x r matrix (r is the rank of A) and
- Im(A^T) is an r x n matrix
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values and return the rank r of the matrix.
- \param Ap : The pseudo inverse \f$ A^+ \f$.
- \param sv : Singular values.
- \param svThreshold : Threshold used to test the singular values.
- \param imAt : Image A^T
- \param imA: Image A
- \return Return the rank of the matrix A
+ \note By default, this function uses Lapack 3rd party. It is also possible
+to use a specific 3rd party suffixing this function name with one of the
+following 3rd party names (Lapack, Eigen3, OpenCV or Gsl).
-*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &imA,
- vpMatrix &imAt) const
-{
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
- unsigned int i, j, k ;
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
- unsigned int nrows, ncols;
- unsigned int nrows_orig = getRows() ;
- unsigned int ncols_orig = getCols() ;
- Ap.resize(ncols_orig,nrows_orig) ;
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
- if (nrows_orig >= ncols_orig)
- {
- nrows = nrows_orig;
- ncols = ncols_orig;
- }
- else
- {
- nrows = ncols_orig;
- ncols = nrows_orig;
- }
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
- vpMatrix a(nrows,ncols) ;
- vpMatrix a1(ncols,nrows);
- vpMatrix v(ncols,ncols) ;
- sv.resize(ncols) ;
+ \return The rank r of the matrix \f$\bf A\f$.
- if (nrows_orig >= ncols_orig) a = *this;
- else a = (*this).t();
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
- a.svd(sv,v);
+ \code
+#include <visp3/core/vpMatrix.h>
- // compute the highest singular value and the rank of h
- double maxsv = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+int main()
+{
+ vpMatrix A(2, 3);
- unsigned int rank = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
- /*------------------------------------------------------- */
- for (i = 0 ; i < ncols ; i++)
- {
- for (j = 0 ; j < nrows ; j++)
- {
- a1[i][j] = 0.0;
+ A.print(std::cout, 10, "A: ");
- for (k=0 ; k < ncols ; k++)
- if (fabs(sv[k]) > maxsv*svThreshold)
- {
- a1[i][j] += v[i][k]*a[j][k]/sv[k];
- }
- }
- }
- if (nrows_orig >= ncols_orig) Ap = a1;
- else Ap = a1.t();
+ vpMatrix A_p;
+ vpColVector sv;
+ unsigned int rank = A.pseudoInverse(A_p, sv);
- if (nrows_orig >= ncols_orig)
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = v[i][j] ;
-
- // compute dim A
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = a[i][j] ;
- }
- else
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = a[i][j] ;
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = v[i][j] ;
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+}
+ \endcode
- }
+ Once build, the previous example produces the following output:
+ \code
+A: [2,3]=
+ 2 3 5
+ -4 2 3
+A^+ (pseudo-inverse): [3,2]=
+ 0.117899 -0.190782
+ 0.065380 0.039657
+ 0.113612 0.052518
+Rank: 2
+Singular values: 6.874359351 4.443330227
+ \endcode
+*/
+unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold) const
+{
+#if defined(VISP_HAVE_LAPACK)
+ return pseudoInverseLapack(Ap, sv, svThreshold);
+#elif defined(VISP_HAVE_EIGEN3)
+ return pseudoInverseEigen3(Ap, sv, svThreshold);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ return pseudoInverseOpenCV(Ap, sv, svThreshold);
+#elif defined(VISP_HAVE_GSL)
+ return pseudoInverseGsl(Ap, sv, svThreshold);
+#else
+ (void)Ap;
+ (void)sv;
+ (void)svThreshold;
+ throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
+ "Install Lapack, Eigen3, OpenCV "
+ "or GSL 3rd party"));
+#endif
+}
-#if 0 // debug
- {
- int pb = 0;
- vpMatrix A, ApA, AAp, AApA, ApAAp ;
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$ and \f$\mbox{Im}(A^T)\f$
+and return the rank r of the matrix.
- nrows = nrows_orig;
- ncols = ncols_orig;
+ See pseudoInverse(vpMatrix &, vpColVector &, double, vpMatrix &, vpMatrix &,
+vpMatrix &) const for a complete description of this function.
- A.resize(nrows,ncols) ;
- A = *this ;
+ \warning To inverse a square n-by-n matrix, you have to use rather one of
+the following functions inverseByLU(), inverseByQR(), inverseByCholesky() that
+are kwown as faster.
- ApA = Ap * A;
- AApA = A * ApA;
- ApAAp = ApA * Ap;
- AAp = A * Ap;
+ \param Ap : The Moore-Penros pseudo inverse \f$ A^+ \f$.
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in pseudo inverse\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Ap : " << std::endl << Ap << std::endl;
- std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
- std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
- std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
- std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
- }
- // else printf("Ap OK ;-) \n");
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
- }
-#endif
+ \param svThreshold : Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
- // std::cout << v << std::endl ;
- return rank ;
-}
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
+ \return The rank r of the matrix \f$\bf A\f$.
-/*!
- Compute the pseudo inverse of the matrix \f$Ap = A^+\f$ along with Ker A, Ker \f$A^T\f$, Im A and Im \f$A^T\f$
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
- Pseudo inverse, kernel and image are computed using the SVD decomposition.
+ \code
+#include <visp3/core/vpMatrix.h>
- A is an m x n matrix,
- if m >=n the svd works on A other wise it works on \f$A^T\f$.
+int main()
+{
+ vpMatrix A(2, 3);
- Therefore if m>=n we have
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
-\f[
-{\bf A}_{m\times n} = {\bf U}_{m\times m} {\bf S}_{m\times n} {\bf V^\top}_{n\times n}
-\f]
-\f[
-{\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} {\bf A} & | &
-\mbox{Ker} {\bf A^\top} \end{array} \right] {\bf S}
-\left[
-\begin{array}{c} (\mbox{Im} {\bf A^\top})^\top \\ (\mbox{Ker}{\bf A})^\top \end{array}\right]
-\f]
-where
-Im(A) is an m x r matrix (r is the rank of A) and
-Im(A^T) is an r x n matrix
+ A.print(std::cout, 10, "A: ");
+ vpMatrix A_p;
+ vpColVector sv;
+ vpMatrix imA, imAt;
+ unsigned int rank = A.pseudoInverse(A_p, sv, 1e-6, imA, imAt);
- \param Ap : The pseudo inverse \f$ A^+ \f$.
- \param sv : Singular values.
- \param svThreshold : Threshold used to test the singular values.
- \param imA: Image A
- \param imAt : Image A^T
- \param kerA : null space of A
- \return Return the rank of the matrix A
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+}
+ \endcode
+ Once build, the previous example produces the following output:
+ \code
+A: [2,3]=
+ 2 3 5
+ -4 2 3
+A^+ (pseudo-inverse): [3,2]=
+ 0.117899 -0.190782
+ 0.065380 0.039657
+ 0.113612 0.052518
+Rank: 2
+Singular values: 6.874359351 4.443330227
+Im(A): [2,2]=
+ 0.81458 -0.58003
+ 0.58003 0.81458
+Im(A^T): [3,2]=
+ -0.100515 -0.994397
+ 0.524244 -0.024967
+ 0.845615 -0.102722
+ \endcode
*/
-unsigned int
-vpMatrix::pseudoInverse(vpMatrix &Ap,
- vpColVector &sv, double svThreshold,
- vpMatrix &imA,
- vpMatrix &imAt,
- vpMatrix &kerA) const
+unsigned int vpMatrix::pseudoInverse(vpMatrix &Ap, vpColVector &sv, double svThreshold, vpMatrix &imA,
+ vpMatrix &imAt) const
{
+ vpMatrix kerAt;
+ return pseudoInverse(Ap, sv, svThreshold, imA, imAt, kerAt);
+}
- unsigned int i, j, k ;
-
- unsigned int nrows, ncols;
- unsigned int nrows_orig = getRows() ;
- unsigned int ncols_orig = getCols() ;
- Ap.resize(ncols_orig,nrows_orig) ;
+/*!
+ Compute the Moore-Penros pseudo inverse \f$A^+\f$ of a m-by-n matrix \f$\bf
+A\f$ along with singular values, \f$\mbox{Im}(A)\f$, \f$\mbox{Im}(A^T)\f$ and
+\f$\mbox{Ker}(A)\f$ and return the rank r of the matrix.
- if (nrows_orig >= ncols_orig)
- {
- nrows = nrows_orig;
- ncols = ncols_orig;
- }
- else
- {
- nrows = ncols_orig;
- ncols = nrows_orig;
- }
+ \note By default, this function uses Lapack 3rd party. It is also possible
+to use a specific 3rd party suffixing this function name with one of the
+following 3rd party names (Lapack, Eigen3, OpenCV or Gsl).
- vpMatrix a(nrows,ncols) ;
- vpMatrix a1(ncols,nrows);
- vpMatrix v(ncols,ncols) ;
- sv.resize(ncols) ;
+ \warning To inverse a square n-by-n matrix, you have to use rather
+inverseByLU(), inverseByCholesky(), or inverseByQR() that are kwown as faster.
- if (nrows_orig >= ncols_orig) a = *this;
- else a = (*this).t();
+ Using singular value decomposition, we have:
- a.svd(sv,v);
+ \f[
+ {\bf A}_{m\times n} = {\bf U}_{m\times m} \; {\bf S}_{m\times n} \; {\bf
+V^\top}_{n\times n} \f] \f[
+ {\bf A}_{m\times n} = \left[\begin{array}{ccc}\mbox{Im} ({\bf A}) & | &
+ \mbox{Ker} ({\bf A}^\top) \end{array} \right] {\bf S}_{m\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
- // compute the highest singular value and the rank of h
- double maxsv = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+ where the diagonal of \f${\bf S}_{m\times n}\f$ corresponds to the matrix
+\f$A\f$ singular values.
- unsigned int rank = 0 ;
- for (i=0 ; i < ncols ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+ This equation could be reformulated in a minimal way:
+ \f[
+ {\bf A}_{m\times n} = \mbox{Im} ({\bf A}) \; {\bf S}_{r\times n}
+ \left[
+ \begin{array}{c} \left[\mbox{Im} ({\bf A}^\top)\right]^\top \\
+ \\
+ \hline \\
+ \left[\mbox{Ker}({\bf A})\right]^\top \end{array}\right]
+ \f]
+ where the diagonal of \f${\bf S}_{r\times n}\f$ corresponds to the matrix
+\f$A\f$ first r singular values.
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+= { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
- /*------------------------------------------------------- */
- for (i = 0 ; i < ncols ; i++)
- {
- for (j = 0 ; j < nrows ; j++)
- {
- a1[i][j] = 0.0;
+ \param Ap: The Moore-Penros pseudo inverse \f$ {\bf A}^+ \f$.
- for (k=0 ; k < ncols ; k++)
- if (fabs(sv[k]) > maxsv*svThreshold)
- {
- a1[i][j] += v[i][k]*a[j][k]/sv[k];
- }
- }
- }
- if (nrows_orig >= ncols_orig) Ap = a1;
- else Ap = a1.t();
+ \param sv: Vector corresponding to matrix \f$A\f$ singular values. The size
+of this vector is equal to min(m, n).
- if (nrows_orig >= ncols_orig)
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = v[i][j] ;
-
- // compute dim A
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = a[i][j] ;
- }
- else
- {
- // compute dim At
- imAt.resize(ncols_orig,rank) ;
- for (i=0 ; i < ncols_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imAt[i][j] = a[i][j] ;
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
- imA.resize(nrows_orig,rank) ;
- for (i=0 ; i < nrows_orig ; i++)
- for (j=0 ; j < rank ; j++)
- imA[i][j] = v[i][j] ;
+ \param imA: \f$\mbox{Im}({\bf A})\f$ that is a m-by-r matrix.
- }
+ \param imAt: \f$\mbox{Im}({\bf A}^T)\f$ that is n-by-r matrix.
- vpMatrix cons(ncols_orig, ncols_orig);
- cons = 0;
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
- for (j = 0; j < ncols_orig; j++)
- {
- for (i = 0; i < ncols_orig; i++)
- {
- if (fabs(sv[i]) <= maxsv*svThreshold)
- {
- cons[i][j] = v[j][i];
- }
- }
- }
+ \return The rank r of the matrix \f$\bf A\f$.
- vpMatrix Ker (ncols_orig-rank, ncols_orig);
- k = 0;
- for (j = 0; j < ncols_orig ; j++)
- {
- //if ( cons.row(j+1).sumSquare() != 0)
- if ( std::fabs(cons.getRow(j).sumSquare()) > std::numeric_limits<double>::epsilon())
- {
- for (i = 0; i < cons.getCols(); i++)
- Ker[k][i] = cons[j][i];
+ Here an example to compute the pseudo-inverse of a 2-by-3 matrix.
- k++;
- }
- }
- kerA = Ker;
+ \code
+#include <visp3/core/vpMatrix.h>
-#if 0 // debug
- {
- int pb = 0;
- vpMatrix A, ApA, AAp, AApA, ApAAp ;
+int main()
+{
+ vpMatrix A(2, 3);
- nrows = nrows_orig;
- ncols = ncols_orig;
+ A[0][0] = 2; A[0][1] = 3; A[0][2] = 5;
+ A[1][0] = -4; A[1][1] = 2; A[1][2] = 3;
- A.resize(nrows,ncols) ;
- A = *this ;
+ A.print(std::cout, 10, "A: ");
- ApA = Ap * A;
- AApA = A * ApA;
- ApAAp = ApA * Ap;
- AAp = A * Ap;
+ vpColVector sv;
+ vpMatrix A_p, imA, imAt, kerAt;
+ unsigned int rank = A.pseudoInverse(A_p, sv, 1e-6, imA, imAt, kerAt);
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(AApA[i][j]-A[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(ApAAp[i][j]-Ap[i][j]) > 1e-6) pb = 1;
- }
- for (i=0;i<nrows;i++)
- {
- for (j=0;j<nrows;j++) if (fabs(AAp[i][j]-AAp[j][i]) > 1e-6) pb = 1;
- }
- for (i=0;i<ncols;i++)
- {
- for (j=0;j<ncols;j++) if (fabs(ApA[i][j]-ApA[j][i]) > 1e-6) pb = 1;
- }
- if (pb == 1)
- {
- printf("pb in pseudo inverse\n");
- std::cout << " A : " << std::endl << A << std::endl;
- std::cout << " Ap : " << std::endl << Ap << std::endl;
- std::cout << " A - AApA : " << std::endl << A - AApA << std::endl;
- std::cout << " Ap - ApAAp : " << std::endl << Ap - ApAAp << std::endl;
- std::cout << " AAp - (AAp)^T : " << std::endl << AAp - AAp.t() << std::endl;
- std::cout << " ApA - (ApA)^T : " << std::endl << ApA - ApA.t() << std::endl;
- std::cout << " KerA : " << std::endl << kerA << std::endl;
- }
- // else printf("Ap OK ;-) \n");
+ A_p.print(std::cout, 10, "A^+ (pseudo-inverse): ");
+ std::cout << "Rank: " << rank << std::endl;
+ std::cout << "Singular values: " << sv.t() << std::endl;
+ imA.print(std::cout, 10, "Im(A): ");
+ imAt.print(std::cout, 10, "Im(A^T): ");
+ if (kerAt.size()) {
+ kerAt.t().print(std::cout, 10, "Ker(A): ");
}
-#endif
+ else {
+ std::cout << "Ker(A) empty " << std::endl;
+ }
+
+ // Reconstruct matrix A from ImA, ImAt, KerAt
+ vpMatrix S(rank, A.getCols());
+ for(unsigned int i = 0; i< rank; i++)
+ S[i][i] = sv[i];
+ vpMatrix Vt(A.getCols(), A.getCols());
+ Vt.insert(imAt.t(), 0, 0);
+ Vt.insert(kerAt, rank, 0);
+ (imA * S * Vt).print(std::cout, 10, "Im(A) * S * [Im(A^T) | Ker(A)]^T:");
+}
+ \endcode
- // std::cout << v << std::endl ;
- return rank ;
+ Once build, the previous example produces the following output:
+ \code
+A: [2,3]=
+ 2 3 5
+ -4 2 3
+A^+ (pseudo-inverse): [3,2]=
+ 0.117899 -0.190782
+ 0.065380 0.039657
+ 0.113612 0.052518
+Rank: 2
+Singular values: 6.874359351 4.443330227
+Im(A): [2,2]=
+ 0.81458 -0.58003
+ 0.58003 0.81458
+Im(A^T): [3,2]=
+ -0.100515 -0.994397
+ 0.524244 -0.024967
+ 0.845615 -0.102722
+Ker(A): [3,1]=
+ -0.032738
+ -0.851202
+ 0.523816
+Im(A) * S * [Im(A^T) | Ker(A)]^T:[2,3]=
+ 2 3 5
+ -4 2 3
+ \endcode
+*/
+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);
+#elif defined(VISP_HAVE_EIGEN3)
+ return pseudoInverseEigen3(Ap, sv, svThreshold, imA, imAt, kerAt);
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+ return pseudoInverseOpenCV(Ap, sv, svThreshold, imA, imAt, kerAt);
+#elif defined(VISP_HAVE_GSL)
+ return pseudoInverseGsl(Ap, sv, svThreshold, imA, imAt, kerAt);
+#else
+ (void)Ap;
+ (void)sv;
+ (void)svThreshold;
+ (void)imA;
+ (void)imAt;
+ (void)kerAt;
+ throw(vpException(vpException::fatalError, "Cannot compute pseudo-inverse. "
+ "Install Lapack, Eigen3, OpenCV "
+ "or GSL 3rd party"));
+#endif
}
/*!
Extract a column vector from a matrix.
\warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If col=0, the first column is extracted.
- \param i_begin : Index of the row that gives the location of the first element of the column vector to extract.
- \param column_size : Size of the column vector to extract.
- \return The extracted column vector.
+ \param j : Index of the column to extract. If col=0, the first column is
+extracted. \param i_begin : Index of the row that gives the location of the
+first element of the column vector to extract. \param column_size : Size of
+the column vector to extract. \return The extracted column vector.
The following example shows how to use this function:
\code
@@ -2181,22 +3745,21 @@ 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"));
vpColVector c(column_size);
- for (unsigned int i=0 ; i < column_size ; i++)
- c[i] = (*this)[i_begin+i][j];
+ for (unsigned int i = 0; i < column_size; i++)
+ c[i] = (*this)[i_begin + i][j];
return c;
}
/*!
Extract a column vector from a matrix.
\warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If j=0, the first column is extracted.
- \return The extracted column vector.
+ \param j : Index of the column to extract. If j=0, the first column is
+extracted. \return The extracted column vector.
The following example shows how to use this function:
\code
@@ -2231,14 +3794,13 @@ column vector:
13
\endcode
*/
-vpColVector
-vpMatrix::getCol(const unsigned int j) const
+vpColVector vpMatrix::getCol(const unsigned int j) const
{
if (j >= getCols())
throw(vpException(vpException::dimensionError, "Unable to extract a column vector from the matrix"));
unsigned int nb_rows = getRows();
vpColVector c(nb_rows);
- for (unsigned int i=0 ; i < nb_rows ; i++)
+ for (unsigned int i = 0; i < nb_rows; i++)
c[i] = (*this)[i][j];
return c;
}
@@ -2278,15 +3840,18 @@ Row vector:
4 5 6 7
\endcode
*/
-vpRowVector
-vpMatrix::getRow(const unsigned int i) const
+vpRowVector vpMatrix::getRow(const unsigned int i) const
{
if (i >= getRows())
throw(vpException(vpException::dimensionError, "Unable to extract a row vector from the matrix"));
- unsigned int nb_cols = getCols();
- vpRowVector r( nb_cols );
- for (unsigned int j=0 ; j < nb_cols ; j++)
- r[j] = (*this)[i][j];
+
+ vpRowVector r;
+ r.resize(colNum, false);
+
+ if (r.data != NULL && data != NULL && colNum > 0) {
+ memcpy(r.data, data + i * colNum, sizeof(double) * colNum);
+ }
+
return r;
}
@@ -2294,9 +3859,9 @@ vpMatrix::getRow(const unsigned int i) const
Extract a row vector from a matrix.
\warning All the indexes start from 0 in this function.
\param i : Index of the row to extract. If i=0, the first row is extracted.
- \param j_begin : Index of the column that gives the location of the first element of the row vector to extract.
- \param row_size : Size of the row vector to extract.
- \return The extracted row vector.
+ \param j_begin : Index of the column that gives the location of the first
+element of the row vector to extract. \param row_size : Size of the row vector
+to extract. \return The extracted row vector.
The following example shows how to use this function:
\code
@@ -2327,19 +3892,19 @@ 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"));
vpRowVector r(row_size);
- for (unsigned int j=0 ; j < row_size ; j++)
- r[j] = (*this)[i][j_begin+i];
+ for (unsigned int j = 0; j < row_size; j++)
+ r[j] = (*this)[i][j_begin + i];
return r;
}
/*!
- Stack matrix \e B to the end of matrix \e A and return the resulting matrix [ A B ]^T
+ Stack matrix \e B to the end of matrix \e A and return the resulting matrix
+ [ A B ]^T
\param A : Upper matrix.
\param B : Lower matrix.
@@ -2347,23 +3912,18 @@ vpMatrix::getRow(const unsigned int i, const unsigned int j_begin, const unsigne
\warning A and B must have the same number of columns.
*/
-vpMatrix
-vpMatrix::stack(const vpMatrix &A, const vpMatrix &B)
+vpMatrix vpMatrix::stack(const vpMatrix &A, const vpMatrix &B)
{
- vpMatrix C ;
+ vpMatrix C;
- try{
- vpMatrix::stack(A, B, C) ;
- }
- catch(...) {
- throw ;
- }
+ vpMatrix::stack(A, B, C);
- return C ;
+ return C;
}
/*!
- Stack row vector \e r to matrix \e A and return the resulting matrix [ A r ]^T
+ Stack row vector \e r to matrix \e A and return the resulting matrix [ A r
+ ]^T
\param A : Upper matrix.
\param r : Lower matrix.
@@ -2371,99 +3931,103 @@ vpMatrix::stack(const vpMatrix &A, const vpMatrix &B)
\warning \e A and \e r must have the same number of columns.
*/
-vpMatrix
-vpMatrix::stack(const vpMatrix &A, const vpRowVector &r)
+vpMatrix vpMatrix::stack(const vpMatrix &A, const vpRowVector &r)
{
- vpMatrix C ;
+ vpMatrix C;
- try{
- vpMatrix::stack(A, r, C) ;
- }
- catch(...) {
- throw ;
- }
+ vpMatrix::stack(A, r, C);
- return C ;
+ return C;
}
/*!
- Stack matrix \e B to the end of matrix \e A and return the resulting matrix in \e C.
+ Stack matrix \e B to the end of matrix \e A and return the resulting matrix
+ in \e C.
\param A : Upper matrix.
\param B : Lower matrix.
\param C : Stacked matrix C = [ A B ]^T
- \warning A and B must have the same number of columns.
+ \warning A and B must have the same number of columns. A and C, B and C must
+ be two different objects.
*/
-void
-vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
+void vpMatrix::stack(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
{
- unsigned int nra = A.getRows() ;
- unsigned int nrb = B.getRows() ;
+ unsigned int nra = A.getRows();
+ unsigned int nrb = B.getRows();
- if (nra !=0)
+ 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()));
}
+ }
- try {
- C.resize(nra+nrb,B.getCols() ) ;
+ if (A.data != NULL && A.data == C.data) {
+ std::cerr << "A and C must be two different objects!" << std::endl;
+ return;
}
- catch(...) {
- throw ;
+
+ if (B.data != NULL && B.data == C.data) {
+ std::cerr << "B and C must be two different objects!" << std::endl;
+ return;
}
- unsigned int i,j ;
- for (i=0 ; i < nra ; i++) {
- for (j=0 ; j < A.getCols() ; j++)
- C[i][j] = A[i][j] ;
+ C.resize(nra + nrb, B.getCols(), false, false);
+
+ if (C.data != NULL && A.data != NULL && A.size() > 0) {
+ // Copy A in C
+ memcpy(C.data, A.data, sizeof(double) * A.size());
}
- for (i=0 ; i < nrb ; i++) {
- for (j=0 ; j < B.getCols() ; j++) {
- C[i+nra][j] = B[i][j] ;
- }
+ if (C.data != NULL && B.data != NULL && B.size() > 0) {
+ // Copy B in C
+ memcpy(C.data + A.size(), B.data, sizeof(double) * B.size());
}
}
/*!
- Stack row vector \e v to the end of matrix \e A and return the resulting matrix in \e C.
+ Stack row vector \e v to the end of matrix \e A and return the resulting
+ matrix in \e C.
\param A : Upper matrix.
\param r : Lower row vector.
\param C : Stacked matrix C = [ A r ]^T
- \warning A and r must have the same number of columns.
+ \warning A and r must have the same number of columns. A and C must be two
+ different objects.
*/
-void
-vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
+void vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
{
- unsigned int nra = A.getRows() ;
+ unsigned int nra = A.getRows();
- if (nra !=0)
+ 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()));
}
+ }
- try {
- C.resize(nra+1,r.getCols() ) ;
+ if (A.data != NULL && A.data == C.data) {
+ std::cerr << "A and C must be two different objects!" << std::endl;
+ return;
}
- catch(...) {
- throw ;
+
+ if (r.size() == 0) {
+ C = A;
+ return;
}
- unsigned int i,j ;
- for (i=0 ; i < nra ; i++) {
- for (j=0 ; j < A.getCols() ; j++)
- C[i][j] = A[i][j] ;
+ C.resize(nra + 1, r.getCols(), false, false);
+
+ if (C.data != NULL && A.data != NULL && A.size() > 0) {
+ // Copy A in C
+ memcpy(C.data, A.data, sizeof(double) * A.size());
}
- for (j=0 ; j < r.getCols() ; j++) {
- C[nra][j] = r[j] ;
+ if (C.data != NULL && r.data != NULL && r.size() > 0) {
+ // Copy r in C
+ memcpy(C.data + A.size(), r.data, sizeof(double) * r.size());
}
}
@@ -2476,22 +4040,16 @@ vpMatrix::stack(const vpMatrix &A, const vpRowVector &r, vpMatrix &C)
\param c : Index of the column where to add the matrix.
\return Matrix with B insert in A.
- \warning Throw exception if the sizes of the matrices do not allow the insertion.
+ \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 ;
+ vpMatrix C;
- try{
- insert(A,B, C, r, c) ;
- }
- catch(...) {
- throw;
- }
+ insert(A, B, C, r, c);
- return C ;
+ return C;
}
/*!
@@ -2507,32 +4065,22 @@ 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() ) ){
- try {
- C.resize(A.getRows(),A.getCols() ) ;
- }
- catch(...) {
- throw ;
- }
- for(unsigned int i=0; i<A.getRows(); i++){
- for(unsigned int j=0; j<A.getCols(); j++){
- if(i >= r && i < (r + B.getRows()) && j >= c && j < (c+B.getCols())){
- C[i][j] = B[i-r][j-c];
- }
- else{
- C[i][j] = A[i][j];
- }
+ 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())) {
+ C[i][j] = B[i - r][j - c];
+ } else {
+ C[i][j] = A[i][j];
}
}
- }
- else{
- throw vpException(vpException::dimensionError,
- "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
+ }
+ } 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);
}
}
@@ -2546,21 +4094,15 @@ vpMatrix::insert(const vpMatrix &A, const vpMatrix &B, vpMatrix &C,
\param B : Right matrix.
\return Juxtaposed matrix C = [ A B ]
- \warning A and B must have the same number of column
+ \warning A and B must have the same number of rows.
*/
-vpMatrix
-vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
+vpMatrix vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B)
{
- vpMatrix C ;
+ vpMatrix C;
- try{
- juxtaposeMatrices(A,B, C) ;
- }
- catch(...) {
- throw ;
- }
+ juxtaposeMatrices(A, B, C);
- return C ;
+ return C;
}
/*!
@@ -2575,38 +4117,29 @@ 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() ;
+ unsigned int nca = A.getCols();
+ unsigned int ncb = B.getCols();
- if (nca !=0)
+ 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()));
}
+ }
- try {
- C.resize(B.getRows(),nca+ncb) ;
- }
- catch(...) {
- throw ;
- }
+ if (B.getRows() == 0 || nca + ncb == 0) {
+ std::cerr << "B.getRows() == 0 || nca+ncb == 0" << std::endl;
+ return;
+ }
- unsigned int i,j ;
- for (i=0 ; i < C.getRows(); i++)
- for (j=0 ; j < nca ; j++)
- C[i][j] = A[i][j] ;
+ C.resize(B.getRows(), nca + ncb, false, false);
- for (i=0 ; i < C.getRows() ; i++)
- for (j=0 ; j < ncb ; j++){
- C[i][nca+j] = B[i][j] ;
- }
+ C.insert(A, 0, 0);
+ C.insert(B, 0, nca);
}
-
//--------------------------------------------------------------------
// Output
//--------------------------------------------------------------------
@@ -2630,93 +4163,93 @@ vpMatrix::juxtaposeMatrices(const vpMatrix &A, const vpMatrix &B, vpMatrix &C)
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &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;
unsigned int m = getRows();
unsigned int n = getCols();
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
- for (unsigned int j=0;j<n;++j){
+ for (unsigned int i = 0; i < m; ++i) {
+ for (unsigned int j = 0; j < n; ++j) {
oss.str("");
oss << (*this)[i][j];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[i][j];
oss.str(ossFixed.str());
}
- values[i*n+j]=oss.str();
- size_type thislen=values[i*n+j].size();
- size_type p=values[i*n+j].find('.');
+ values[i * n + j] = oss.str();
+ size_type thislen = values[i * n + j].size();
+ size_type p = values[i * n + j].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
}
}
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- for (unsigned int j=0;j<n;j++){
- size_type p=values[i*n+j].find('.');
+ for (unsigned int i = 0; i < m; i++) {
+ s << " ";
+ for (unsigned int j = 0; j < n; j++) {
+ size_type p = values[i * n + j].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[i*n+j].substr(0,p).c_str();
+ s << values[i * n + j].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[i*n+j].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[i * n + j].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
}
- s <<std::endl;
+ s << std::endl;
}
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
-
/*!
Print using Matlab syntax, to copy/paste in Matlab later.
@@ -2753,18 +4286,21 @@ M =
>>
\endcode
*/
-std::ostream & vpMatrix::matlabPrint(std::ostream & os) const
+std::ostream &vpMatrix::matlabPrint(std::ostream &os) const
{
os << "[ ";
- for (unsigned int i=0; i < this->getRows(); ++ i) {
- for (unsigned int j=0; j < this ->getCols(); ++ j) {
- os << (*this)[i][j] << ", ";
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[i][j] << ", ";
+ }
+ if (this->getRows() != i + 1) {
+ os << ";" << std::endl;
+ } else {
+ os << "]" << std::endl;
}
- if (this ->getRows() != i+1) { os << ";" << std::endl; }
- else { os << "]" << std::endl; }
}
return os;
-};
+}
/*!
Print using Maple syntax, to copy/paste in Maple later.
@@ -2794,19 +4330,19 @@ M = ([
that could be copy/paste in Maple.
*/
-std::ostream & vpMatrix::maplePrint(std::ostream & os) const
+std::ostream &vpMatrix::maplePrint(std::ostream &os) const
{
os << "([ " << std::endl;
- for (unsigned int i=0; i < this->getRows(); ++ i) {
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
os << "[";
- for (unsigned int j=0; j < this->getCols(); ++ j) {
- os << (*this)[i][j] << ", ";
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[i][j] << ", ";
}
os << "]," << std::endl;
}
os << "])" << std::endl;
return os;
-};
+}
/*!
Print/save a matrix in csv format.
@@ -2835,19 +4371,18 @@ int main()
3, 4, 5
\endcode
*/
-std::ostream & vpMatrix::csvPrint(std::ostream & os) const
+std::ostream &vpMatrix::csvPrint(std::ostream &os) const
{
- for (unsigned int i=0; i < this->getRows(); ++ i) {
- for (unsigned int j=0; j < this->getCols(); ++ j) {
- os << (*this)[i][j];
- if (!(j==(this->getCols()-1)))
+ for (unsigned int i = 0; i < this->getRows(); ++i) {
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[i][j];
+ if (!(j == (this->getCols() - 1)))
os << ", ";
}
os << std::endl;
}
return os;
-};
-
+}
/*!
Print to be used as part of a C++ code later.
@@ -2885,87 +4420,49 @@ 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;
-
- 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;
- }
- 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 << "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;
+ } 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 << std::endl;
}
return os;
-};
+}
/*!
- Compute the determinant of the matrix using the LU Decomposition.
-
- \return The determinant of the matrix if the matrix is square, 0 otherwise.
-
- See the Numerical Recipes in C page 43 for further explanations.
+ Stack A at the end of the current matrix, or copy if the matrix has no
+ dimensions : this = [ this A ]^T.
*/
-
-double vpMatrix::detByLU() const
+void vpMatrix::stack(const vpMatrix &A)
{
- double det_ = 0;
-
- // Test wether the matrix is squred
- if (rowNum == colNum)
- {
- // create a temporary matrix that will be modified by LUDcmp
- vpMatrix tmp(*this);
-
- // using th LUdcmp based on NR codes
- // it modified the tmp matrix in a special structure of type :
- // b11 b12 b13 b14
- // a21 b22 b23 b24
- // a21 a32 b33 b34
- // a31 a42 a43 b44
-
- unsigned int * perm = new unsigned int[rowNum]; // stores the permutations
- int d; // +- 1 fi the number of column interchange is even or odd
- tmp.LUDcmp(perm, d);
- delete[]perm;
-
- // compute the determinant that is the product of the eigen values
- det_ = (double) d;
- for(unsigned int i=0;i<rowNum;i++)
- {
- det_*=tmp[i][i];
+ if (rowNum == 0) {
+ *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()));
}
+
+ unsigned int rowNumOld = rowNum;
+ resize(rowNum + A.getRows(), colNum, false, false);
+ insert(A, rowNumOld, 0);
}
- else {
- throw(vpException(vpException::fatalError,
- "Cannot compute LU decomposition on a non square matrix (%dx%d)",
- rowNum, colNum)) ;
- }
- return det_ ;
}
-
-
/*!
- Stack A at the end of the current matrix, or copy if the matrix has no dimensions : this = [ this A ]^T.
+ Stack row vector \e r at the end of the current matrix, or copy if the
+matrix has no dimensions : this = [ this r ]^T.
Here an example for a robot velocity log :
\code
@@ -2978,25 +4475,29 @@ for(unsigned int i = 0;i<100;i++)
}
\endcode
*/
-void vpMatrix::stack(const vpMatrix &A)
-{
- if(rowNum == 0)
- *this = A;
- else
- *this = vpMatrix::stack(*this, A);
-}
-
-/*!
- Stack row vector \e r at the end of the current matrix, or copy if the matrix has no dimensions : this = [ this r ]^T.
-*/
void vpMatrix::stack(const vpRowVector &r)
{
- if(rowNum == 0)
+ if (rowNum == 0) {
*this = r;
- else
- *this = vpMatrix::stack(*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()));
+ }
+
+ if (r.size() == 0) {
+ return;
+ }
+
+ unsigned int oldSize = size();
+ resize(rowNum + 1, colNum, false, false);
+ if (data != NULL && r.data != NULL && r.size() > 0) {
+ // Copy r in data
+ memcpy(data + oldSize, r.data, sizeof(double) * r.size());
+ }
+ }
+}
/*!
Insert matrix A at the given position in the current matrix.
@@ -3008,25 +4509,22 @@ 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 ){
- // recopy matrix A in the current one, does not call static function to avoid initialisation and recopy of matrix
- for(unsigned int i=r; i<(r+A.getRows()); i++){
- for(unsigned int j=c; j<(c+A.getCols()); j++){
- (*this)[i][j] = A[i-r][j-c];
+ if ((r + A.getRows()) <= rowNum && (c + A.getCols()) <= colNum) {
+ 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);
}
}
- }
- else{
- throw vpException(vpException::dimensionError,
- "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
+ } else {
+ throw vpException(vpException::dimensionError, "Cannot insert (%dx%d) matrix in (%dx%d) matrix at position (%d,%d)",
A.getRows(), A.getCols(), rowNum, colNum, r, c);
}
}
-
/*!
Compute the eigenvalues of a n-by-n real symmetric matrix.
@@ -3037,7 +4535,8 @@ void vpMatrix::insert(const vpMatrix&A, const unsigned int r,
\exception vpException::dimensionError If the matrix is not square.
\exception vpException::fatalError If the matrix is not symmetric.
- \exception vpException::functionNotImplementedError If the GSL library is not detected.
+ \exception vpException::functionNotImplementedError If the GSL library is
+not detected.
Here an example:
\code
@@ -3063,64 +4562,62 @@ int main()
\sa eigenValues(vpColVector &, vpMatrix &)
-*/
+*/
vpColVector vpMatrix::eigenValues() const
{
if (rowNum != colNum) {
- throw(vpException(vpException::dimensionError,
- "Cannot compute eigen values on a non square matrix (%dx%d)",
- rowNum, colNum)) ;
+ 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 */
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
{
// Check if the matrix is symetric: At - A = 0
vpMatrix At_A = (*this).t() - (*this);
- for (unsigned int i=0; i < rowNum; i++) {
- for (unsigned int j=0; j < rowNum; j++) {
- //if (At_A[i][j] != 0) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < rowNum; j++) {
+ // if (At_A[i][j] != 0) {
if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
- throw(vpException(vpException::fatalError,
- "Cannot compute eigen values on a non symetric matrix")) ;
+ throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
}
}
}
vpColVector evalue(rowNum); // Eigen values
- gsl_vector *eval = gsl_vector_alloc (rowNum);
- gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
+ gsl_vector *eval = gsl_vector_alloc(rowNum);
+ gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
- gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
+ gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
- unsigned int Atda = (unsigned int)m->tda ;
- for (unsigned int i=0 ; i < rowNum ; i++){
- unsigned int k = i*Atda ;
- for (unsigned int j=0 ; j < colNum ; j++)
- m->data[k+j] = (*this)[i][j] ;
+ unsigned int Atda = (unsigned int)m->tda;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ unsigned int k = i * Atda;
+ for (unsigned int j = 0; j < colNum; j++)
+ m->data[k + j] = (*this)[i][j];
}
- gsl_eigen_symmv (m, eval, evec, w);
+ gsl_eigen_symmv(m, eval, evec, w);
- gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
+ gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
- for (unsigned int i=0; i < rowNum; i++) {
- evalue[i] = gsl_vector_get (eval, i);
+ for (unsigned int i = 0; i < rowNum; i++) {
+ evalue[i] = gsl_vector_get(eval, i);
}
- gsl_eigen_symmv_free (w);
- gsl_vector_free (eval);
- gsl_matrix_free (m);
- gsl_matrix_free (evec);
+ gsl_eigen_symmv_free(w);
+ gsl_vector_free(eval);
+ gsl_matrix_free(m);
+ gsl_matrix_free(evec);
return evalue;
}
#else
{
- throw(vpException(vpException::functionNotImplementedError,
- "Eigen values computation is not implemented. You should install GSL rd party")) ;
+ throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
+ "should install GSL rd party"));
}
-#endif
+#endif
}
/*!
@@ -3136,7 +4633,8 @@ vpColVector vpMatrix::eigenValues() const
\exception vpException::dimensionError If the matrix is not square.
\exception vpException::fatalError If the matrix is not symmetric.
- \exception vpException::functionNotImplementedError If the GSL library is not detected.
+ \exception vpException::functionNotImplementedError If the GSL library is
+not detected.
Here an example:
\code
@@ -3175,28 +4673,26 @@ int main()
\sa eigenValues()
*/
-#ifdef VISP_HAVE_GSL /* be careful of the copy below */
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
void vpMatrix::eigenValues(vpColVector &evalue, vpMatrix &evector) const
#else
void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */) const
#endif
{
if (rowNum != colNum) {
- throw(vpException(vpException::dimensionError,
- "Cannot compute eigen values on a non square matrix (%dx%d)",
- rowNum, colNum)) ;
+ 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 */
+#ifdef VISP_HAVE_GSL /* be careful of the copy below */
{
// Check if the matrix is symetric: At - A = 0
vpMatrix At_A = (*this).t() - (*this);
- for (unsigned int i=0; i < rowNum; i++) {
- for (unsigned int j=0; j < rowNum; j++) {
- //if (At_A[i][j] != 0) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < rowNum; j++) {
+ // if (At_A[i][j] != 0) {
if (std::fabs(At_A[i][j]) > std::numeric_limits<double>::epsilon()) {
- throw(vpException(vpException::fatalError,
- "Cannot compute eigen values on a non symetric matrix")) ;
+ throw(vpException(vpException::fatalError, "Cannot compute eigen values on a non symetric matrix"));
}
}
}
@@ -3205,118 +4701,117 @@ void vpMatrix::eigenValues(vpColVector & /* evalue */, vpMatrix & /* evector */)
evalue.resize(rowNum);
evector.resize(rowNum, colNum);
- gsl_vector *eval = gsl_vector_alloc (rowNum);
- gsl_matrix *evec = gsl_matrix_alloc (rowNum, colNum);
+ gsl_vector *eval = gsl_vector_alloc(rowNum);
+ gsl_matrix *evec = gsl_matrix_alloc(rowNum, colNum);
- gsl_eigen_symmv_workspace * w = gsl_eigen_symmv_alloc (rowNum);
+ gsl_eigen_symmv_workspace *w = gsl_eigen_symmv_alloc(rowNum);
gsl_matrix *m = gsl_matrix_alloc(rowNum, colNum);
- unsigned int Atda = (unsigned int)m->tda ;
- for (unsigned int i=0 ; i < rowNum ; i++){
- unsigned int k = i*Atda ;
- for (unsigned int j=0 ; j < colNum ; j++)
- m->data[k+j] = (*this)[i][j] ;
+ unsigned int Atda = (unsigned int)m->tda;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ unsigned int k = i * Atda;
+ for (unsigned int j = 0; j < colNum; j++)
+ m->data[k + j] = (*this)[i][j];
}
- gsl_eigen_symmv (m, eval, evec, w);
+ gsl_eigen_symmv(m, eval, evec, w);
- gsl_eigen_symmv_sort (eval, evec, GSL_EIGEN_SORT_ABS_ASC);
+ gsl_eigen_symmv_sort(eval, evec, GSL_EIGEN_SORT_ABS_ASC);
- for (unsigned int i=0; i < rowNum; i++) {
- evalue[i] = gsl_vector_get (eval, i);
+ for (unsigned int i = 0; i < rowNum; i++) {
+ evalue[i] = gsl_vector_get(eval, i);
}
- Atda = (unsigned int)evec->tda ;
- for (unsigned int i=0; i < rowNum; i++) {
- unsigned int k = i*Atda ;
- for (unsigned int j=0; j < rowNum; j++) {
- evector[i][j] = evec->data[k+j];
+ Atda = (unsigned int)evec->tda;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ unsigned int k = i * Atda;
+ for (unsigned int j = 0; j < rowNum; j++) {
+ evector[i][j] = evec->data[k + j];
}
}
- gsl_eigen_symmv_free (w);
- gsl_vector_free (eval);
- gsl_matrix_free (m);
- gsl_matrix_free (evec);
+ gsl_eigen_symmv_free(w);
+ gsl_vector_free(eval);
+ gsl_matrix_free(m);
+ gsl_matrix_free(evec);
}
#else
{
- throw(vpException(vpException::functionNotImplementedError,
- "Eigen values computation is not implemented. You should install GSL rd party")) ;
+ throw(vpException(vpException::functionNotImplementedError, "Eigen values computation is not implemented. You "
+ "should install GSL rd party"));
}
-#endif
+#endif
}
-
/*!
- Function to compute the null space (the kernel) of the interaction matrix A which is not full rank.
- The null space ( the kernel ) of a matrix A is defined as Null(A) = Ker(A) = {X : A*X =0}.
+ Function to compute the null space (the kernel) of a m-by-n matrix \f$\bf
+ A\f$.
+
+ The null space of a matrix \f$\bf A\f$ is defined as \f$\mbox{Ker}({\bf A})
+ = { {\bf X} : {\bf A}*{\bf X} = {\bf 0}}\f$.
- \param kerA : The matrix to contain the null space (kernel) of A defined by the row vectors (A*KerA.t()=0)
- \param svThreshold : Specify the used threshold in the svd(...) function (a function to compute the singular value decomposition)
+ \param kerAt: The matrix that contains the null space (kernel) of \f$\bf
+ A\f$ defined by the matrix \f${\bf X}^T\f$. If matrix \f$\bf A\f$ is full
+ rank, the dimension of \c kerAt is (0, n), otherwise the dimension is (n-r,
+ n). This matrix is thus the transpose of \f$\mbox{Ker}({\bf A})\f$.
- \return the rank of the matrix.
+ \param svThreshold: Threshold used to test the singular values. If
+ a singular value is lower than this threshold we consider that the
+ matrix is not full rank.
+
+ \return The rank r of the matrix.
*/
-unsigned int
-vpMatrix::kernel(vpMatrix &kerA, double svThreshold) const
+unsigned int vpMatrix::kernel(vpMatrix &kerAt, double svThreshold) const
{
- unsigned int i, j ;
- unsigned int nbline = getRows() ;
- unsigned int nbcol = getCols() ;
+ unsigned int nbline = getRows();
+ unsigned int nbcol = getCols();
- vpMatrix A ; // Copy of the matrix, SVD function is destructive
- vpColVector sv(nbcol) ; // singular values
- vpMatrix v(nbcol,nbcol) ; // V matrix of singular value decomposition
+ vpMatrix U; // Copy of the matrix, SVD function is destructive
+ vpColVector sv(nbcol); // singular values
+ vpMatrix V(nbcol, nbcol); // V matrix of singular value decomposition
// Copy and resize matrix to have at least as many rows as columns
- // kernel is computed in svd method only if the matrix has more rows than columns
+ // kernel is computed in svd method only if the matrix has more rows than
+ // columns
- if (nbline < nbcol) A.resize(nbcol,nbcol) ;
- else A.resize(nbline,nbcol) ;
+ if (nbline < nbcol)
+ U.resize(nbcol, nbcol);
+ else
+ U.resize(nbline, nbcol);
- for (i=0 ; i < nbline ; i++)
- {
- for (j=0 ; j < nbcol ; j++)
- {
- A[i][j] = (*this)[i][j] ;
- }
- }
+ U.insert(*this, 0, 0);
- A.svd(sv,v);
+ U.svd(sv, V);
// Compute the highest singular value and rank of the matrix
- double maxsv = 0 ;
- for (i=0 ; i < nbcol ; i++)
- if (fabs(sv[i]) > maxsv) maxsv = fabs(sv[i]) ;
+ double maxsv = 0;
+ for (unsigned int i = 0; i < nbcol; i++) {
+ if (fabs(sv[i]) > maxsv) {
+ maxsv = fabs(sv[i]);
+ }
+ }
- unsigned int rank = 0 ;
- for (i=0 ; i < nbcol ; i++)
- if (fabs(sv[i]) > maxsv*svThreshold) rank++ ;
+ unsigned int rank = 0;
+ for (unsigned int i = 0; i < nbcol; i++) {
+ if (fabs(sv[i]) > maxsv * svThreshold) {
+ rank++;
+ }
+ }
- if (rank != nbcol)
- {
- vpMatrix Ker(nbcol-rank,nbcol) ;
- unsigned int k = 0 ;
- for (j = 0 ; j < nbcol ; j++)
- {
- //if( v.col(j) in kernel and non zero )
- if ( (fabs(sv[j]) <= maxsv*svThreshold) && (std::fabs(v.getCol(j).sumSquare()) > std::numeric_limits<double>::epsilon()))
- {
- // Ker.Row(k) = v.Col(j) ;
- for (i=0 ; i < v.getRows() ; i++)
- {
- Ker[k][i] = v[i][j];
+ kerAt.resize(nbcol - rank, nbcol);
+ if (rank != nbcol) {
+ 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<double>::epsilon())) {
+ for (unsigned int i = 0; i < V.getRows(); i++) {
+ kerAt[k][i] = V[i][j];
}
k++;
}
}
- kerA = Ker ;
- }
- else
- {
- kerA.resize(0,0);
}
- return rank ;
+ return rank;
}
/*!
@@ -3342,23 +4837,23 @@ 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.det(vpMatrix::LU_DECOMPOSITION ) << 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
*/
double vpMatrix::det(vpDetMethod method) const
{
- double det_ = 0;
+ double det = 0.;
- if ( method == LU_DECOMPOSITION )
- {
- det_ = this->detByLU();
+ if (method == LU_DECOMPOSITION) {
+ det = this->detByLU();
}
- return (det_);
+ return (det);
}
/*!
@@ -3367,30 +4862,26 @@ double vpMatrix::det(vpDetMethod method) const
\return Return the exponential matrix.
-*/
-vpMatrix
-vpMatrix::expm() 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 ));
- }
- else
- {
+ if (colNum != rowNum) {
+ throw(vpException(vpException::dimensionError, "Cannot compute the exponential of a non square (%dx%d) matrix",
+ rowNum, colNum));
+ } else {
#ifdef VISP_HAVE_GSL
size_t size_ = rowNum * colNum;
- double *b = new double [size_];
- for (size_t i=0; i< size_; i++)
+ double *b = new double[size_];
+ for (size_t i = 0; i < size_; i++)
b[i] = 0.;
- gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
+ gsl_matrix_view m = gsl_matrix_view_array(this->data, rowNum, colNum);
gsl_matrix_view em = gsl_matrix_view_array(b, rowNum, colNum);
gsl_linalg_exponential_ss(&m.matrix, &em.matrix, 0);
- //gsl_matrix_fprintf(stdout, &em.matrix, "%g");
+ // gsl_matrix_fprintf(stdout, &em.matrix, "%g");
vpMatrix expA(rowNum, colNum);
memcpy(expA.data, b, size_ * sizeof(double));
- delete [] b;
+ delete[] b;
return expA;
#else
vpMatrix _expE(rowNum, colNum);
@@ -3409,45 +4900,42 @@ vpMatrix::expm() const
int p = 1;
double nA = 0;
- for (unsigned int i = 0; i < rowNum;i++)
- {
+ for (unsigned int i = 0; i < rowNum; i++) {
double sum = 0;
- for (unsigned int j=0; j < colNum; j++)
- {
+ for (unsigned int j = 0; j < colNum; j++) {
sum += fabs((*this)[i][j]);
}
- if (sum>nA||i==0)
- {
- nA=sum;
+ if (sum > nA || i == 0) {
+ nA = sum;
}
}
/* f = */ frexp(nA, &e);
- //double s = (0 > e+1)?0:e+1;
- double s = e+1;
-
- double sca = 1.0 / pow(2.0,s);
- exp=sca*exp;
- _expX=*this;
- _expE=c*exp+_eye;
- _expD=-c*exp+_eye;
- for (int k=2;k<=q;k++)
- {
- c = c * ((double)(q-k+1)) / ((double)(k*(2*q-k+1)));
- _expcX=exp*_expX;
- _expX=_expcX;
- _expcX=c*_expX;
- _expE=_expE+_expcX;
- if (p) _expD=_expD+_expcX;
- else _expD=_expD- _expcX;
+ // double s = (0 > e+1)?0:e+1;
+ double s = e + 1;
+
+ double sca = 1.0 / pow(2.0, s);
+ exp = sca * exp;
+ _expX = *this;
+ _expE = c * exp + _eye;
+ _expD = -c * exp + _eye;
+ for (int k = 2; k <= q; k++) {
+ c = c * ((double)(q - k + 1)) / ((double)(k * (2 * q - k + 1)));
+ _expcX = exp * _expX;
+ _expX = _expcX;
+ _expcX = c * _expX;
+ _expE = _expE + _expcX;
+ if (p)
+ _expD = _expD + _expcX;
+ else
+ _expD = _expD - _expcX;
p = !p;
}
- _expX=_expD.inverseByLU();
- exp=_expX*_expE;
- for (int k=1;k<=s;k++)
- {
- _expE=exp*exp;
- exp=_expE;
+ _expX = _expD.inverseByLU();
+ exp = _expX * _expE;
+ for (int k = 1; k <= s; k++) {
+ _expE = exp * exp;
+ exp = _expE;
}
return exp;
#endif
@@ -3457,14 +4945,14 @@ vpMatrix::expm() const
/**************************************************************************************************************/
/**************************************************************************************************************/
-
-//Specific functions
+// Specific functions
/*
input:: matrix M(nCols,nRows), nCols > 3, nRows > 3 , nCols == nRows.
output:: the complement matrix of the element (rowNo,colNo).
-This is the matrix obtained from M after elimenating the row rowNo and column colNo
+This is the matrix obtained from M after elimenating the row rowNo and column
+colNo
example:
1 2 3
@@ -3475,27 +4963,26 @@ subblock(M, 1, 1) give the matrix 7 9
*/
vpMatrix subblock(const vpMatrix &M, unsigned int col, unsigned int row)
{
- vpMatrix M_comp(M.getRows()-1,M.getCols()-1);
+ vpMatrix M_comp(M.getRows() - 1, M.getCols() - 1);
- for ( unsigned int i = 0 ; i < col ; i++)
- {
- for ( unsigned int j = 0 ; j < row ; j++)
- M_comp[i][j]=M[i][j];
- for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
- M_comp[i][j-1]=M[i][j];
+ for (unsigned int i = 0; i < col; i++) {
+ for (unsigned int j = 0; j < row; j++)
+ M_comp[i][j] = M[i][j];
+ for (unsigned int j = row + 1; j < M.getRows(); j++)
+ M_comp[i][j - 1] = M[i][j];
}
- for ( unsigned int i = col+1 ; i < M.getCols(); i++)
- {
- for ( unsigned int j = 0 ; j < row ; j++)
- M_comp[i-1][j]=M[i][j];
- for ( unsigned int j = row+1 ; j < M.getRows() ; j++)
- M_comp[i-1][j-1]=M[i][j];
+ for (unsigned int i = col + 1; i < M.getCols(); i++) {
+ for (unsigned int j = 0; j < row; j++)
+ M_comp[i - 1][j] = M[i][j];
+ for (unsigned int j = row + 1; j < M.getRows(); j++)
+ M_comp[i - 1][j - 1] = M[i][j];
}
return M_comp;
}
/*!
- \return The condition number, the ratio of the largest singular value of the matrix to the smallest.
+ \return The condition number, the ratio of the largest singular value of
+ the matrix to the smallest.
*/
double vpMatrix::cond() const
{
@@ -3506,14 +4993,15 @@ double vpMatrix::cond() const
M = *this;
M.svd(w, v);
- double min=w[0];
- double max=w[0];
- for(unsigned int i=0;i<M.getCols();i++)
- {
- if(min>w[i])min=w[i];
- if(max<w[i])max=w[i];
- }
- return max/min;
+ double min = w[0];
+ double max = w[0];
+ for (unsigned int i = 0; i < M.getCols(); i++) {
+ if (min > w[i])
+ min = w[i];
+ if (max < w[i])
+ max = w[i];
+ }
+ return max / min;
}
/*!
@@ -3524,26 +5012,24 @@ double vpMatrix::cond() const
*/
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() ));
+ if (H.getCols() != H.getRows()) {
+ throw(vpException(vpException::dimensionError, "Cannot compute HLM on a non square matrix (%dx%d)", H.getRows(),
+ H.getCols()));
}
- HLM.resize(H.getRows(), H.getCols());
+ HLM.resize(H.getRows(), H.getCols(), false, false);
- for(unsigned int i=0;i<H.getCols();i++)
- {
- for(unsigned int j=0;j<H.getCols();j++)
- {
- HLM[i][j]=H[i][j];
- if(i==j)
- HLM[i][j]+= alpha*H[i][j];
+ for (unsigned int i = 0; i < H.getCols(); i++) {
+ for (unsigned int j = 0; j < H.getCols(); j++) {
+ HLM[i][j] = H[i][j];
+ if (i == j)
+ HLM[i][j] += alpha * H[i][j];
}
}
}
/*!
- Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{A_{ij}^2}} \f$.
+ Compute and return the Euclidean norm \f$ ||x|| = \sqrt{ \sum{A_{ij}^2}}
+ \f$.
\return The Euclidean norm if the matrix is initialized, 0 otherwise.
@@ -3551,10 +5037,10 @@ void vpMatrix::computeHLM(const vpMatrix &H, const double &alpha, vpMatrix &HLM)
*/
double vpMatrix::euclideanNorm() const
{
- double norm=0.0;
- for (unsigned int i=0;i<dsize;i++) {
- double x = *(data +i);
- norm += x*x;
+ double norm = 0.0;
+ for (unsigned int i = 0; i < dsize; i++) {
+ double x = *(data + i);
+ norm += x * x;
}
return sqrt(norm);
@@ -3572,11 +5058,11 @@ double vpMatrix::euclideanNorm() const
*/
double vpMatrix::infinityNorm() const
{
- double norm=0.0;
- for (unsigned int i=0;i<rowNum;i++){
+ double norm = 0.0;
+ for (unsigned int i = 0; i < rowNum; i++) {
double x = 0;
- for (unsigned int j=0; j<colNum;j++){
- x += fabs (*(*(rowPtrs + i)+j)) ;
+ for (unsigned int j = 0; j < colNum; j++) {
+ x += fabs(*(*(rowPtrs + i) + j));
}
if (x > norm) {
norm = x;
@@ -3586,19 +5072,20 @@ double vpMatrix::infinityNorm() const
}
/*!
- Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m, n)\f$.
+ Return the sum square of all the \f$A_{ij}\f$ elements of the matrix \f$A(m,
+ n)\f$.
\return The value \f$\sum A_{ij}^{2}\f$.
*/
double vpMatrix::sumSquare() const
{
- double sum_square=0.0;
- double x ;
+ double sum_square = 0.0;
+ double x;
- for (unsigned int i=0;i<rowNum;i++) {
- for(unsigned int j=0;j<colNum;j++) {
- x=rowPtrs[i][j];
- sum_square += x*x;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
+ x = rowPtrs[i][j];
+ sum_square += x * x;
}
}
@@ -3616,15 +5103,9 @@ void vpMatrix::stackMatrices(const vpColVector &A, const vpColVector &B, vpColVe
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().
@@ -3632,13 +5113,13 @@ void vpMatrix::stackMatrices(const vpMatrix &A, const vpRowVector &B, vpMatrix &
Return the i-th row of the matrix.
\warning notice row(1) is the 0th row.
*/
-vpRowVector
-vpMatrix::row(const unsigned int i)
+vpRowVector vpMatrix::row(const unsigned int i)
{
- vpRowVector c(getCols()) ;
+ vpRowVector c(getCols());
- for (unsigned int j =0 ; j < getCols() ; j++) c[j] = (*this)[i-1][j] ;
- return c ;
+ for (unsigned int j = 0; j < getCols(); j++)
+ c[j] = (*this)[i - 1][j];
+ return c;
}
/*!
@@ -3648,13 +5129,13 @@ vpMatrix::row(const unsigned int i)
\warning notice column(1) is the 0-th column.
\param j : Index of the column to extract.
*/
-vpColVector
-vpMatrix::column(const unsigned int j)
+vpColVector vpMatrix::column(const unsigned int j)
{
- vpColVector c(getRows()) ;
+ vpColVector c(getRows());
- for (unsigned int i =0 ; i < getRows() ; i++) c[i] = (*this)[i][j-1] ;
- return c ;
+ for (unsigned int i = 0; i < getRows(); i++)
+ c[i] = (*this)[i][j - 1];
+ return c;
}
/*!
@@ -3663,14 +5144,14 @@ vpMatrix::column(const unsigned int j)
Set the matrix diagonal elements to \e val.
More generally set M[i][i] = val.
*/
-void
-vpMatrix::setIdentity(const double & val)
+void vpMatrix::setIdentity(const double &val)
{
- for (unsigned int i=0;i<rowNum;i++)
- for (unsigned int j=0;j<colNum;j++)
- if (i==j) (*this)[i][j] = val ;
- else (*this)[i][j] = 0;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ if (i == j)
+ (*this)[i][j] = val;
+ else
+ (*this)[i][j] = 0;
}
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
-
diff --git a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp
index 479f500..c412566 100644
--- a/modules/core/src/math/matrix/vpMatrix_cholesky.cpp
+++ b/modules/core/src/math/matrix/vpMatrix_cholesky.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,9 +38,9 @@
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
// Exception
#include <visp3/core/vpException.h>
@@ -48,38 +49,30 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-#ifdef VISP_HAVE_LAPACK_C
-extern "C" void dpotrf_ (char *uplo, int *n, double *a, int *lda, int *info);
-extern "C" int dpotri_(char *uplo, int *n, double *a, int *lda, int *info);
-
-vpMatrix vpMatrix::inverseByCholeskyLapack() const{
- int rowNum_ = (int)this->getRows();
- int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
- int info;
-
- vpMatrix A = *this;
- dpotrf_((char*)"L",&rowNum_,A.data,&lda,&info);
-
- if(info!=0)
- std::cout << "cholesky:dpotrf_:error" << std::endl;
-
- dpotri_((char*)"L",&rowNum_,A.data,&lda,&info);
- if(info!=0){
- std::cout << "cholesky:dpotri_:error" << std::endl;
- throw vpMatrixException::badValue;
- }
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+#include <opencv2/core/core.hpp>
+#endif
- for(unsigned int i=0;i<A.getRows();i++)
- for(unsigned int j=0;j<A.getCols();j++)
- if(i>j) A[i][j] = A[j][i];
+#ifdef VISP_HAVE_LAPACK
+#ifdef VISP_HAVE_LAPACK_BUILT_IN
+typedef long int integer;
+#else
+typedef int integer;
+#endif
- return A;
-}
+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
/*!
Compute the inverse of a n-by-n matrix using the Cholesky decomposition.
- The matrix must be real and symmetric.
- Only available if lapack is installed.
+ The matrix must be real symmetric positive defined.
+
+ This function calls the first following function that is available:
+ - inverseByCholeskyLapack() if Lapack 3rd party is installed
+ - inverseByLUOpenCV() if OpenCV 3rd party is installed.
+
+ If none of these 3rd parties is installed we use a Lapack built-in version.
\return The inverse matrix.
@@ -109,20 +102,142 @@ int main()
\sa pseudoInverse()
*/
-vpMatrix
-vpMatrix::inverseByCholesky() const
+vpMatrix vpMatrix::inverseByCholesky() const
{
+#ifdef VISP_HAVE_LAPACK
+ return inverseByCholeskyLapack();
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ return inverseByCholeskyOpenCV();
+#else
+ throw(vpException(vpException::fatalError, "Cannot inverse matrix by "
+ "Cholesky. Install Lapack or "
+ "OpenCV 3rd party"));
+#endif
+}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
+#ifdef VISP_HAVE_LAPACK
+/*!
+ Compute the inverse of a n-by-n matrix using the Cholesky decomposition with
+Lapack 3rd party. The matrix must be real symmetric positive defined.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ unsigned int n = 4;
+ vpMatrix A(n, n);
+ vpMatrix I;
+ I.eye(4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Make matrix symmetric positive
+ A = 0.5*(A+A.t());
+ A = A + n*I;
+
+ // Compute the inverse
+ vpMatrix A_1 = A.inverseByCholeskyLapack();
+ std::cout << "Inverse by Cholesky (Lapack): \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa inverseByCholesky(), inverseByCholeskyOpenCV()
+*/
+vpMatrix vpMatrix::inverseByCholeskyLapack() const
+{
+ if (rowNum != colNum) {
+ throw(vpMatrixException(vpMatrixException::matrixError, "Cannot inverse a non-square matrix (%ux%u) by Cholesky",
+ rowNum, colNum));
}
- return inverseByCholeskyLapack();
+
+ integer rowNum_ = (integer)this->getRows();
+ 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_()"));
+
+ dpotri_((char *)"L", &rowNum_, A.data, &lda, &info);
+ if (info != 0) {
+ std::cout << "cholesky:dpotri_:error" << std::endl;
+ throw vpMatrixException::badValue;
+ }
+
+ for (unsigned int i = 0; i < A.getRows(); i++)
+ for (unsigned int j = 0; j < A.getCols(); j++)
+ if (i > j)
+ A[i][j] = A[j][i];
+
+ return A;
+}
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+/*!
+ Compute the inverse of a n-by-n matrix using the Cholesky decomposition with
+OpenCV 3rd party. The matrix must be real symmetric positive defined.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ unsigned int n = 4;
+ vpMatrix A(n, n);
+ vpMatrix I;
+ I.eye(4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Make matrix symmetric positive
+ A = 0.5*(A+A.t());
+ A = A + n*I;
+
+ // Compute the inverse
+ vpMatrix A_1 = A.inverseByCholeskyOpenCV();
+ std::cout << "Inverse by Cholesky (OpenCV): \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
}
+ \endcode
+
+ \sa inverseByCholesky(), inverseByCholeskyLapack()
+*/
+vpMatrix vpMatrix::inverseByCholeskyOpenCV() const
+{
+ if (rowNum != colNum) {
+ throw(
+ vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by Cholesky", rowNum, colNum));
+ }
-#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpMatrixLapack.cpp.o) has no symbols
-void dummy_vpMatrixLapack() {};
+ cv::Mat M(rowNum, colNum, CV_64F, this->data);
+ cv::Mat Minv = M.inv(cv::DECOMP_CHOLESKY);
+
+ vpMatrix A(rowNum, colNum);
+ memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
+
+ return A;
+}
#endif
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/core/src/math/matrix/vpMatrix_covariance.cpp b/modules/core/src/math/matrix/vpMatrix_covariance.cpp
index f0fb6d7..f698393 100644
--- a/modules/core/src/math/matrix/vpMatrix_covariance.cpp
+++ b/modules/core/src/math/matrix/vpMatrix_covariance.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,77 +36,84 @@
*
*****************************************************************************/
-#include <limits> // numeric_limits
#include <cmath> // std::fabs()
+#include <limits> // numeric_limits
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMatrixException.h>
-
+#include <visp3/core/vpTranslationVector.h>
/*!
- Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
- Ax = b
-
+ Compute the covariance matrix of the parameters x from a least squares
+ minimisation defined as: Ax = b
+
\param A : Matrix A from Ax = b.
-
+
\param x : Vector x from Ax = b corresponding to the parameters to estimate.
-
+
\param b : Vector b from Ax = b.
*/
vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b)
{
-// double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To consider OLS Estimate for sigma
+ // double denom = ((double)(A.getRows()) - (double)(A.getCols())); // To
+ // consider OLS Estimate for sigma
double denom = ((double)(A.getRows())); // To consider MLE Estimate for sigma
- if(denom <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
+ if (denom <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError,
+ "Impossible to compute covariance matrix: not enough data");
-// double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be equivalent to line bellow.
+ // double sigma2 = ( ((b.t())*b) - ( (b.t())*A*x ) ); // Should be
+ // equivalent to line bellow.
double sigma2 = (b - (A * x)).t() * (b - (A * x));
sigma2 /= denom;
- return (A.t()*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
+ return (A.t() * A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
}
/*!
- Compute the covariance matrix of the parameters x from a least squares minimisation defined as:
- WAx = Wb
-
+ Compute the covariance matrix of the parameters x from a least squares
+ minimisation defined as: WAx = Wb
+
\param A : Matrix A from WAx = Wb.
-
- \param x : Vector x from WAx = Wb corresponding to the parameters to estimate.
-
+
+ \param x : Vector x from WAx = Wb corresponding to the parameters to
+ estimate.
+
\param b : Vector b from WAx = Wb.
-
+
\param W : Diagonal weigths matrix from WAx = Wb.
*/
-vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b, const vpMatrix &W)
+vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector &x, const vpColVector &b,
+ const vpMatrix &W)
{
double denom = 0.0;
- vpMatrix W2(W.getCols(),W.getCols());
- for(unsigned int i = 0 ; i < W.getCols() ; i++){
- denom += W[i][i];
- W2[i][i] = W[i][i]*W[i][i];
+ vpMatrix W2(W.getCols(), W.getCols());
+ for (unsigned int i = 0; i < W.getCols(); i++) {
+ denom += W[i][i];
+ W2[i][i] = W[i][i] * W[i][i];
}
- if(denom <= std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::divideByZeroError, "Impossible to compute covariance matrix: not enough data");
+ if (denom <= std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::divideByZeroError,
+ "Impossible to compute covariance matrix: not enough data");
-// double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should be equivalent to line bellow.
- double sigma2 = (W * b - (W * A * x)).t() * (W*b - (W * A * x));
+ // double sigma2 = ( ((W*b).t())*W*b - ( ((W*b).t())*W*A*x ) ); // Should
+ // be equivalent to line bellow.
+ double sigma2 = (W * b - (W * A * x)).t() * (W * b - (W * A * x));
sigma2 /= denom;
- return (A.t()*(W2)*A).pseudoInverse(A.getCols()*std::numeric_limits<double>::epsilon())*sigma2;
+ return (A.t() * (W2)*A).pseudoInverse(A.getCols() * std::numeric_limits<double>::epsilon()) * sigma2;
}
/*!
Compute the covariance matrix of an image-based virtual visual servoing.
- This assumes the optimization has been done via v = Ls.pseudoInverse() * DeltaS.
+ This assumes the optimization has been done via v = Ls.pseudoInverse() *
+ DeltaS.
\param cMo : Pose matrix that has been computed with the v.
@@ -113,103 +121,107 @@ vpMatrix vpMatrix::computeCovarianceMatrix(const vpMatrix &A, const vpColVector
\param Ls : interaction matrix used in v = Ls.pseudoInverse() * DeltaS
*/
-vpMatrix
-vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls)
+vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
+ const vpMatrix &Ls)
{
vpMatrix Js;
vpColVector deltaP;
- vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
+ vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
- return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS);
+ return vpMatrix::computeCovarianceMatrix(Js, deltaP, deltaS);
}
/*!
Compute the covariance matrix of an image-based virtual visual servoing.
- This assumes the optimization has been done via v = (W * Ls).pseudoInverse() * W * DeltaS.
+ This assumes the optimization has been done via v = (W * Ls).pseudoInverse()
+ * W * DeltaS.
\param cMo : Pose matrix that has been computed with the v.
- \param deltaS : Error vector used in v = (W * Ls).pseudoInverse() * W * DeltaS.
+ \param deltaS : Error vector used in v = (W * Ls).pseudoInverse() * W *
+ DeltaS.
- \param Ls : interaction matrix used in v = (W * Ls).pseudoInverse() * W * DeltaS.
+ \param Ls : interaction matrix used in v = (W * Ls).pseudoInverse() * W *
+ DeltaS.
\param W : Weight matrix used in v = (W * Ls).pseudoInverse() * W * DeltaS.
*/
-vpMatrix
-vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS, const vpMatrix &Ls, const vpMatrix &W)
+vpMatrix vpMatrix::computeCovarianceMatrixVVS(const vpHomogeneousMatrix &cMo, const vpColVector &deltaS,
+ const vpMatrix &Ls, const vpMatrix &W)
{
vpMatrix Js;
vpColVector deltaP;
- vpMatrix::computeCovarianceMatrixVVS(cMo,deltaS,Ls,Js,deltaP);
+ vpMatrix::computeCovarianceMatrixVVS(cMo, deltaS, Ls, Js, deltaP);
- return vpMatrix::computeCovarianceMatrix(Js,deltaP,deltaS,W);
+ 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);
- LpInv = 0;
- LpInv[0][0] = -1.0;
- LpInv[1][1] = -1.0;
- LpInv[2][2] = -1.0;
-
- vpTranslationVector ctoInit;
-
- cMo.extract(ctoInit);
- vpMatrix ctoInitSkew = ctoInit.skew();
-
- vpThetaUVector thetau;
- cMo.extract(thetau);
-
- vpColVector tu(3);
- for(unsigned int i = 0 ; i < 3 ; i++)
- tu[i] = thetau[i];
-
- double theta = sqrt(tu.sumSquare()) ;
-
-// vpMatrix Lthetau(3,3);
- vpMatrix LthetauInvAnalytic(3,3);
- vpMatrix I3(3,3);
- I3.eye();
-// Lthetau = -I3;
- LthetauInvAnalytic = -I3;
-
- if(theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon())
- {
- // Computing [theta/2 u]_x
- vpColVector theta2u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- theta2u[i] = tu[i]/2.0 ;
- }
- vpMatrix theta2u_skew = vpColVector::skew(theta2u);
-
- vpColVector u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- u[i] = tu[i]/theta ;
- }
- vpMatrix u_skew = vpColVector::skew(u);
-
-// Lthetau += (theta2u_skew - (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
- LthetauInvAnalytic += -(vpMath::sqr(vpMath::sinc(theta/2.0)) * theta2u_skew - (1.0-vpMath::sinc(theta))*u_skew*u_skew);
+ // building Lp
+ vpMatrix LpInv(6, 6);
+ LpInv = 0;
+ LpInv[0][0] = -1.0;
+ LpInv[1][1] = -1.0;
+ LpInv[2][2] = -1.0;
+
+ vpTranslationVector ctoInit;
+
+ cMo.extract(ctoInit);
+ vpMatrix ctoInitSkew = ctoInit.skew();
+
+ vpThetaUVector thetau;
+ cMo.extract(thetau);
+
+ vpColVector tu(3);
+ for (unsigned int i = 0; i < 3; i++)
+ tu[i] = thetau[i];
+
+ double theta = sqrt(tu.sumSquare());
+
+ // vpMatrix Lthetau(3,3);
+ vpMatrix LthetauInvAnalytic(3, 3);
+ vpMatrix I3(3, 3);
+ I3.eye();
+ // Lthetau = -I3;
+ LthetauInvAnalytic = -I3;
+
+ if (theta / (2.0 * M_PI) > std::numeric_limits<double>::epsilon()) {
+ // Computing [theta/2 u]_x
+ vpColVector theta2u(3);
+ for (unsigned int i = 0; i < 3; i++) {
+ theta2u[i] = tu[i] / 2.0;
+ }
+ vpMatrix theta2u_skew = vpColVector::skew(theta2u);
+
+ vpColVector u(3);
+ for (unsigned int i = 0; i < 3; i++) {
+ u[i] = tu[i] / theta;
}
+ vpMatrix u_skew = vpColVector::skew(u);
+
+ // Lthetau += (theta2u_skew -
+ // (1.0-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*u_skew*u_skew);
+ LthetauInvAnalytic +=
+ -(vpMath::sqr(vpMath::sinc(theta / 2.0)) * theta2u_skew - (1.0 - vpMath::sinc(theta)) * u_skew * u_skew);
+ }
-// vpMatrix LthetauInv = Lthetau.inverseByLU();
+ // vpMatrix LthetauInv = Lthetau.inverseByLU();
- ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
+ ctoInitSkew = ctoInitSkew * LthetauInvAnalytic;
- for(unsigned int a = 0 ; a < 3 ; a++)
- for(unsigned int b = 0 ; b < 3 ; b++)
- LpInv[a][b+3] = ctoInitSkew[a][b];
+ for (unsigned int a = 0; a < 3; a++)
+ for (unsigned int b = 0; b < 3; b++)
+ LpInv[a][b + 3] = ctoInitSkew[a][b];
- for(unsigned int a = 0 ; a < 3 ; a++)
- for(unsigned int b = 0 ; b < 3 ; b++)
- LpInv[a+3][b+3] = LthetauInvAnalytic[a][b];
+ for (unsigned int a = 0; a < 3; a++)
+ for (unsigned int b = 0; b < 3; b++)
+ LpInv[a + 3][b + 3] = LthetauInvAnalytic[a][b];
- // Building Js
- Js = Ls * LpInv;
+ // Building Js
+ Js = Ls * LpInv;
- // building deltaP
- deltaP = (Js).pseudoInverse(Js.getRows()*std::numeric_limits<double>::epsilon()) * deltaS;
+ // building deltaP
+ deltaP = (Js).pseudoInverse(Js.getRows() * std::numeric_limits<double>::epsilon()) * deltaS;
}
diff --git a/modules/core/src/math/matrix/vpMatrix_lu.cpp b/modules/core/src/math/matrix/vpMatrix_lu.cpp
index b16f4d4..93a1c4b 100644
--- a/modules/core/src/math/matrix/vpMatrix_lu.cpp
+++ b/modules/core/src/math/matrix/vpMatrix_lu.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,169 +38,304 @@
#include <visp3/core/vpConfig.h>
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
-// Exception
-#include <visp3/core/vpException.h>
-#include <visp3/core/vpMatrixException.h>
+#ifdef VISP_HAVE_EIGEN3
+#include <Eigen/LU>
+#endif
-// Debug trace
-#include <visp3/core/vpDebug.h>
+#ifdef VISP_HAVE_GSL
+#include <gsl/gsl_linalg.h>
+#include <gsl/gsl_permutation.h>
+#endif
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#ifdef VISP_HAVE_LAPACK
+#ifdef VISP_HAVE_LAPACK_BUILT_IN
+typedef long int integer;
+#else
+typedef int integer;
+#endif
-#define TINY 1.0e-20;
+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
+#include <opencv2/core/core.hpp>
+#endif
+
+// Exception
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
/*--------------------------------------------------------------------
LU Decomposition related functions
-------------------------------------------------------------------- */
/*!
- \brief Performed the LU decomposition
+ Compute the inverse of a n-by-n matrix using the LU decomposition.
+
+ This function calls the first following function that is available:
+ - inverseByLULapack() if Lapack 3rd party is installed
+ - inverseByLUEigen3() if Eigen3 3rd party is installed
+ - inverseByLUOpenCV() if OpenCV 3rd party is installed
+ - inverseByLUGsl() if GSL 3rd party is installed.
+
+ If none of these previous 3rd parties is installed, we use by default
+inverseByLULapack() with a Lapack built-in version.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1 = A.inverseByLU();
+
+ std::cout << "Inverse by LU ";
+#if defined(VISP_HAVE_LAPACK)
+ std::cout << "(using Lapack)";
+#elif defined(VISP_HAVE_EIGEN3)
+ std::cout << "(using Eigen3)";
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ std::cout << "(using OpenCV)";
+#elif defined (VISP_HAVE_GSL)
+ std::cout << "(using GSL)";
+#endif
+ std::cout << ": \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa inverseByLULapack(), inverseByLUEigen3(), inverseByLUOpenCV(),
+inverseByLUGsl(), pseudoInverse()
+*/
+vpMatrix vpMatrix::inverseByLU() const
+{
+#if defined(VISP_HAVE_LAPACK)
+ return inverseByLULapack();
+#elif defined(VISP_HAVE_EIGEN3)
+ return inverseByLUEigen3();
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ return inverseByLUOpenCV();
+#elif defined(VISP_HAVE_GSL)
+ return inverseByLUGsl();
+#else
+ throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Eigen3, "
+ "Lapack, OpenCV or GSL 3rd party"));
+#endif
+}
+
+/*!
+ Compute the determinant of a square matrix using the LU decomposition.
- Given a matrix A (n x n), this routine replaces it by the LU decomposition of
- a rowwise permutation of itself. A is output, arranged as in equation
- (2.3.14) of the NRC ; perm (n) is an output vector that records the row
- permutation effected by the partial pivoting; d is output as 1 depending on
- whether the number of row interchanges was even or odd, respectively.
+ This function calls the first following function that is available:
+ - detByLULapack() if Lapack 3rd party is installed
+ - detByLUEigen3() if Eigen3 3rd party is installed
+ - detByLUOpenCV() if OpenCV 3rd party is installed
+ - detByLUGsl() if GSL 3rd party is installed.
- \warning Destructive wrt. A
+ If none of these previous 3rd parties is installed, we use by default
+detByLULapack() with a Lapack built-in version.
- \sa This routine is used in combination with LUDksb to solve linear equations
- or invert a matrix.
+ \return The determinant of the matrix if the matrix is square.
- This function is extracted from the NRC
+ \code
+#include <iostream>
- */
+#include <visp3/core/vpMatrix.h>
-void
-vpMatrix::LUDcmp(unsigned int *perm, int& d)
+int main()
{
- unsigned int n = rowNum;
-
- unsigned int i,imax=0,j,k;
- double big,dum,sum_,temp;
- vpColVector vv(n);
-
- d=1;
- for (i=0;i<n;i++) {
- big=0.0;
- for (j=0;j<n;j++)
- if ((temp=fabs(rowPtrs[i][j])) > big) big=temp;
- //if (big == 0.0)
- if (std::fabs(big) <= std::numeric_limits<double>::epsilon())
- {
- //vpERROR_TRACE("Singular vpMatrix in LUDcmp") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Singular vpMatrix in LUDcmp")) ;
- }
- vv[i]=1.0/big;
- }
- for (j=0;j<n;j++) {
- for (i=0;i<j;i++) {
- sum_=rowPtrs[i][j];
- for (k=0;k<i;k++) sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
- rowPtrs[i][j]=sum_;
- }
- big=0.0;
- for (i=j;i<n;i++) {
- sum_=rowPtrs[i][j];
- for (k=0;k<j;k++)
- sum_ -= rowPtrs[i][k]*rowPtrs[k][j];
- rowPtrs[i][j]=sum_;
- if ( (dum=vv[i]*fabs(sum_)) >= big) {
- big=dum;
- imax=i;
- }
- }
- if (j != imax) {
- for (k=0;k<n;k++) {
- dum=rowPtrs[imax][k];
- rowPtrs[imax][k]=rowPtrs[j][k];
- rowPtrs[j][k]=dum;
- }
- d *= -1;
- vv[imax]=vv[j];
- }
- perm[j]=imax;
- //if (rowPtrs[j][j] == 0.0)
- if (std::fabs(rowPtrs[j][j]) <= std::numeric_limits<double>::epsilon())
- rowPtrs[j][j]=TINY;
- if (j != n) {
- dum=1.0/(rowPtrs[j][j]);
- for (i=j+1;i<n;i++) rowPtrs[i][j] *= dum;
- }
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by default method : " << A.det() << std::endl;
+ std:: cout << "Determinant by LU decomposition : " << A.detByLU() << std::endl;
+}
+ \endcode
+ \sa detByLULapack(), detByLUEigen3(), detByLUOpenCV(), detByLUGsl()
+*/
+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]));
+ } else {
+#if defined(VISP_HAVE_LAPACK)
+ return detByLULapack();
+#elif defined(VISP_HAVE_EIGEN3)
+ return detByLUEigen3();
+#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ return detByLUOpenCV();
+#elif defined(VISP_HAVE_GSL)
+ return detByLUGsl();
+#else
+ throw(vpException(vpException::fatalError, "Cannot compute matrix determinant. Install Lapack, "
+ "Eigen3, OpenCV or GSL 3rd party"));
+#endif
}
}
-#undef TINY
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#if defined(VISP_HAVE_GSL)
/*!
- \brief Solve linear system AX = B using LU decomposition
+ Compute the inverse of a n-by-n matrix using the LU decomposition with GSL
+3rd party.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
- Solves the set of n linear equations AX = B. Here A (n x n) is input, not
- as the matrix A but rather as its LU decomposition, determined by the routine
- ludcmp. perm (n) is input as the permutation vector returned by
- ludcmp. b (n) is input as the right-hand side vector B, and returns with
- the solution vector X. A and perm are not modified by this routine and can
- be left in place for successive calls with different right-hand sides b. This
- routine takes into account the possibility that b will begin with many zero
- elements, so it is efficient for use in matrix inversion.
+int main()
+{
+ vpMatrix A(4,4);
- \sa This function must be used with LUDcmp
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
- \sa LUDsolve and solveByLUD are more intuitive and direct to use
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByLUGsl();
+ std::cout << "Inverse by LU (GSL): \n" << A_1 << std::endl;
- This function is extracted from the NRC
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+ \sa inverseByLU(), inverseByLUEigen3(), inverseByLULapack(),
+inverseByLUOpenCV()
*/
-void vpMatrix::LUBksb(unsigned int *perm, vpColVector& b)
+vpMatrix vpMatrix::inverseByLUGsl() const
{
- unsigned int n = rowNum;
-
- unsigned int ii=0;
- double sum_;
- bool flag = false;
- unsigned int i;
-
- for (i=0;i<n;i++) {
- unsigned int ip=perm[i];
- sum_=b[ip];
- b[ip]=b[i];
- if (flag) {
- for (unsigned int j=ii;j<=i-1;j++) sum_ -= rowPtrs[i][j]*b[j];
- }
- //else if (sum_) {
- else if (std::fabs(sum_) > std::numeric_limits<double>::epsilon()) {
- ii=i;
- flag = true;
- }
- b[i]=sum_;
+ if (rowNum != colNum) {
+ throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
}
- // for (int i=n-1;i>=0;i--) {
- // sum_=b[i];
- // for (int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
- // b[i]=sum_/rowPtrs[i][i];
- // }
- i=n;
- do {
- i --;
-
- sum_=b[i];
- for (unsigned int j=i+1;j<n;j++) sum_ -= rowPtrs[i][j]*b[j];
- b[i]=sum_/rowPtrs[i][i];
- } while(i != 0);
+
+ gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
+
+ // copy the input matrix to ensure the function doesn't modify its content
+ unsigned int tda = (unsigned int)A->tda;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ unsigned int k = i * tda;
+ for (unsigned int j = 0; j < colNum; j++)
+ A->data[k + j] = (*this)[i][j];
+ }
+
+ vpMatrix Ainv(rowNum, colNum);
+
+ gsl_matrix inverse;
+ inverse.size1 = rowNum;
+ inverse.size2 = colNum;
+ inverse.tda = inverse.size2;
+ inverse.data = Ainv.data;
+ inverse.owner = 0;
+ inverse.block = 0;
+
+ gsl_permutation *p = gsl_permutation_alloc(rowNum);
+ int s;
+
+ // Do the LU decomposition on A and use it to solve the system
+ gsl_linalg_LU_decomp(A, p, &s);
+ gsl_linalg_LU_invert(A, p, &inverse);
+
+ gsl_permutation_free(p);
+ gsl_matrix_free(A);
+
+ return Ainv;
}
-#endif // doxygen should skip this
/*!
- Compute the inverse of a n-by-n matrix using the LU decomposition.
+ Compute the determinant of a square matrix using the LU decomposition with
+GSL 3rd party.
+
+ \return The determinant of the matrix if the matrix is square.
+
+ \code
+#include <iostream>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by LU decomposition (GSL): " << A.detByLUGsl() << std::endl;
+}
+ \endcode
+ \sa detByLU(), detByLUEigen3(), detByLUOpenCV(), detByLULapack()
+*/
+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));
+ }
+
+ gsl_matrix *A = gsl_matrix_alloc(rowNum, colNum);
+
+ // copy the input matrix to ensure the function doesn't modify its content
+ unsigned int tda = (unsigned int)A->tda;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ unsigned int k = i * tda;
+ for (unsigned int j = 0; j < colNum; j++)
+ A->data[k + j] = (*this)[i][j];
+ }
+
+ gsl_permutation *p = gsl_permutation_alloc(rowNum);
+ int s;
+
+ // Do the LU decomposition on A and use it to solve the system
+ gsl_linalg_LU_decomp(A, p, &s);
+ det = gsl_linalg_LU_det(A, s);
+
+ gsl_permutation_free(p);
+ gsl_matrix_free(A);
+
+ return det;
+}
+#endif
+
+#ifdef VISP_HAVE_LAPACK
+/*!
+ Compute the inverse of a n-by-n matrix using the LU decomposition with
+Lapack 3rd party.
\return The inverse matrix.
@@ -218,60 +354,286 @@ int main()
// Compute the inverse
vpMatrix A_1; // A^-1
- A_1 = A.inverseByLU();
- std::cout << "Inverse by LU: \n" << A_1 << std::endl;
+ A_1 = A.inverseByLULapack();
+ std::cout << "Inverse by LU (Lapack): \n" << A_1 << std::endl;
std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
}
\endcode
- \sa pseudoInverse()
+ \sa inverseByLU(), inverseByLUEigen3(), inverseByLUGsl(),
+inverseByLUOpenCV()
*/
-vpMatrix
-vpMatrix::inverseByLU() const
+vpMatrix vpMatrix::inverseByLULapack() const
{
- unsigned int i,j;
+ if (rowNum != colNum) {
+ throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
+ }
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
+ integer dim = (integer)rowNum;
+ integer lda = dim;
+ integer info;
+ integer lwork = dim * dim;
+ integer *ipiv = new integer[dim + 1];
+ double *work = new double[lwork];
+
+ vpMatrix A = *this;
+
+ dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
+ if (info) {
+ delete[] ipiv;
+ delete[] work;
+ throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
}
- vpMatrix B(rowNum, rowNum), X(rowNum, rowNum);
- vpMatrix V(rowNum, rowNum);
- vpColVector W(rowNum);
+ dgetri_(&dim, A.data, &dim, &ipiv[1], work, &lwork, &info);
+
+ delete[] ipiv;
+ delete[] work;
+
+ return A;
+}
+
+/*!
+ Compute the determinant of a square matrix using the LU decomposition with
+GSL 3rd party.
+
+ \return The determinant of the matrix if the matrix is square.
+
+ \code
+#include <iostream>
- for (i=0; i<rowNum; i++) {
- for (j=0; j<rowNum; j++) {
- B[i][j] = (i == j) ? 1 : 0;
- }
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by LU decomposition (Lapack): " << A.detByLULapack() << std::endl;
+}
+ \endcode
+ \sa detByLU(), detByLUEigen3(), detByLUGsl(), detByLUOpenCV()
+*/
+double vpMatrix::detByLULapack() const
+{
+ if (rowNum != colNum) {
+ throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
+ rowNum, colNum));
}
- vpMatrix A(rowNum, rowNum);
- A = *this;
+ integer dim = (integer)rowNum;
+ integer lda = dim;
+ integer info;
+ integer *ipiv = new integer[dim + 1];
- unsigned int *perm = new unsigned int[rowNum];
+ vpMatrix A = *this;
- try {
- int p;
- A.LUDcmp(perm, p);
+ dgetrf_(&dim, &dim, A.data, &lda, &ipiv[1], &info);
+ if (info) {
+ delete[] ipiv;
+ throw(vpException(vpException::fatalError, "Lapack LU decomposition failed; info=%d", info));
}
- catch(vpException &e) {
- delete [] perm;
- throw(e);
+
+ double det = A[0][0];
+ for (unsigned int i = 1; i < rowNum; i++) {
+ det *= A[i][i];
+ }
+
+ double sign = 1.;
+ for (int i = 1; i <= dim; i++) {
+ if (ipiv[i] != i)
+ sign = -sign;
}
- vpColVector c_tmp(rowNum) ;
- for (j=1; j<=rowNum; j++)
- {
- c_tmp =0 ; c_tmp[j-1] = 1 ;
- A.LUBksb(perm, c_tmp);
- for (unsigned int k=0 ; k < c_tmp.getRows() ; k++)
- B[k][j-1] = c_tmp[k] ;
+ det *= sign;
+
+ delete[] ipiv;
+
+ return det;
+}
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+/*!
+ Compute the inverse of a n-by-n matrix using the LU decomposition with
+OpenCV 3rd party.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByLUOpenCV();
+ std::cout << "Inverse by LU (OpenCV): \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa inverseByLU(), inverseByLUEigen3(), inverseByLUGsl(),
+inverseByLULapack()
+*/
+vpMatrix vpMatrix::inverseByLUOpenCV() const
+{
+ if (rowNum != colNum) {
+ throw(vpException(vpException::fatalError, "Cannot inverse a non square matrix (%ux%u) by LU", rowNum, colNum));
}
- delete [] perm;
- return B;
+
+ cv::Mat M(rowNum, colNum, CV_64F, this->data);
+
+ cv::Mat Minv = M.inv(cv::DECOMP_LU);
+
+ vpMatrix A(rowNum, colNum);
+ memcpy(A.data, Minv.data, (size_t)(8 * Minv.rows * Minv.cols));
+
+ return A;
+}
+
+/*!
+ Compute the determinant of a n-by-n matrix using the LU decomposition with
+OpenCV 3rd party.
+
+ \return Determinant of the matrix.
+
+ \code
+#include <iostream>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by LU decomposition (OpenCV): " << A.detByLUOpenCV() << std::endl;
+}
+ \endcode
+ \sa detByLU(), detByLUEigen3(), detByLUGsl(), detByLULapack()
+*/
+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));
+ }
+
+ cv::Mat M(rowNum, colNum, CV_64F, this->data);
+ det = cv::determinant(M);
+
+ return (det);
+}
+#endif
+
+#if defined(VISP_HAVE_EIGEN3)
+
+/*!
+ Compute the inverse of a n-by-n matrix using the LU decomposition with
+Eigen3 3rd party.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1; // A^-1
+ A_1 = A.inverseByLUEigen3();
+ std::cout << "Inverse by LU (Eigen3): \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa inverseByLU(), inverseByLULapack(), inverseByLUOpenCV(),
+inverseByLUGsl()
+*/
+vpMatrix vpMatrix::inverseByLUEigen3() const
+{
+ if (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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
+ this->getCols());
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > A_(A.data, this->getRows(),
+ this->getCols());
+
+ A_ = M.inverse();
+
+ return A;
+}
+
+/*!
+ Compute the determinant of a square matrix using the LU decomposition with
+Eigen3 3rd party.
+
+ \return The determinant of the matrix if the matrix is square.
+
+ \code
+#include <iostream>
+
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(3,3);
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.;
+ A[1][0] = 1/3.; A[1][1] = 1/4.; A[1][2] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/7.; A[2][2] = 1/8.;
+ std::cout << "Initial matrix: \n" << A << std::endl;
+
+ // Compute the determinant
+ std:: cout << "Determinant by LU decomposition (Eigen3): " << A.detByLUEigen3() << std::endl;
+}
+ \endcode
+ \sa detByLU(), detByLUOpenCV(), detByLULapack()
+*/
+double vpMatrix::detByLUEigen3() const
+{
+ if (rowNum != colNum) {
+ throw(vpException(vpException::fatalError, "Cannot compute matrix determinant of a non square matrix (%ux%u)",
+ rowNum, colNum));
+ }
+
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
+ this->getCols());
+
+ return M.determinant();
}
+#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/core/src/math/matrix/vpMatrix_mul.cpp b/modules/core/src/math/matrix/vpMatrix_mul.cpp
new file mode 100644
index 0000000..2b0193b
--- /dev/null
+++ b/modules/core/src/math/matrix/vpMatrix_mul.cpp
@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * BLAS subroutines.
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpMatrix.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#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 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,
+ 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);
+}
+
+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);
+}
+
+#else
+// Work arround to avoid warning LNK4221: This object file does not define any
+// previously undefined public symbols
+void dummy_vpMatrix_blas(){};
+#endif
+
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/core/src/math/matrix/vpMatrix_qr.cpp b/modules/core/src/math/matrix/vpMatrix_qr.cpp
index 123b753..4e54e52 100644
--- a/modules/core/src/math/matrix/vpMatrix_qr.cpp
+++ b/modules/core/src/math/matrix/vpMatrix_qr.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,12 @@
*
*****************************************************************************/
-#include <algorithm> // for std::min and std::max
+#include <algorithm> // for (std::min) and (std::max)
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
// Exception
#include <visp3/core/vpException.h>
@@ -49,148 +50,198 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-int allocate_work(double** work);
-
-#ifdef VISP_HAVE_LAPACK_C
-extern "C" int dgeqrf_(int *m, int *n, double*a, int *lda, double *tau, double *work, int *lwork, int *info);
-extern "C" int dormqr_(char *side, char *trans, int *m, int *n,
- int *k, double *a, int *lda, double *tau, double *c__,
- int *ldc, double *work, int *lwork, int *info);
-extern "C" int dorgqr_(int *, int *, int *, double *, int *,
- double *, double *, int *, int *);
-extern "C" int dtrtri_(char *uplo, char *diag, int *n, double *a, int *lda, int *info);
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#ifdef VISP_HAVE_LAPACK
+#ifdef VISP_HAVE_LAPACK_BUILT_IN
+typedef long int integer;
+#else
+typedef int integer;
#endif
-int allocate_work(double** work)
+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);
+
+int allocate_work(double **work);
+
+int allocate_work(double **work)
{
unsigned int dimWork = (unsigned int)((*work)[0]);
- delete[] *work;
+ delete[] * work;
*work = new double[dimWork];
return (int)dimWork;
}
-#ifdef VISP_HAVE_LAPACK_C
-vpMatrix vpMatrix::inverseByQRLapack() const{
- int rowNum_ = (int)this->getRows();
- int colNum_ = (int)this->getCols();
- int lda = (int)rowNum_; //lda is the number of rows because we don't use a submatrix
- int dimTau = std::min(rowNum_,colNum_);
- int dimWork = -1;
+#endif
+
+#ifdef VISP_HAVE_LAPACK
+/*!
+ Compute the inverse of a n-by-n matrix using the QR decomposition with
+Lapack 3rd party.
+
+ \return The inverse matrix.
+
+ Here an example:
+ \code
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix A(4,4);
+
+ A[0][0] = 1/1.; A[0][1] = 1/2.; A[0][2] = 1/3.; A[0][3] = 1/4.;
+ A[1][0] = 1/5.; A[1][1] = 1/3.; A[1][2] = 1/3.; A[1][3] = 1/5.;
+ A[2][0] = 1/6.; A[2][1] = 1/4.; A[2][2] = 1/2.; A[2][3] = 1/6.;
+ A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
+
+ // Compute the inverse
+ vpMatrix A_1 = A.inverseByQRLapack();
+ std::cout << "Inverse by QR: \n" << A_1 << std::endl;
+
+ std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
+}
+ \endcode
+
+ \sa inverseByQR()
+*/
+vpMatrix vpMatrix::inverseByQRLapack() const
+{
+ if (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 dimTau = (std::min)(rowNum_, colNum_);
+ integer dimWork = -1;
double *tau = new double[dimTau];
double *work = new double[1];
- int info;
+ integer info;
vpMatrix C;
vpMatrix A = *this;
- try{
- //1) Extract householder reflections (useful to compute Q) and R
- dgeqrf_(
- &rowNum_, //The number of rows of the matrix A. M >= 0.
- &colNum_, //The number of columns of the matrix A. N >= 0.
- A.data, /*On entry, the M-by-N matrix A.
- On exit, the elements on and above the diagonal of the array
- contain the min(M,N)-by-N upper trapezoidal matrix R (R is
- upper triangular if m >= n); the elements below the diagonal,
- with the array TAU, represent the orthogonal matrix Q as a
- product of min(m,n) elementary reflectors.
- */
- &lda, //The leading dimension of the array A. LDA >= max(1,M).
- tau, /*Dimension (min(M,N))
- The scalar factors of the elementary reflectors
- */
- work, //Internal working array. dimension (MAX(1,LWORK))
- &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
- &info //status
- );
-
- if(info != 0){
+ try {
+ // 1) Extract householder reflections (useful to compute Q) and R
+ dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
+ &colNum_, // The number of columns of the matrix A. N >= 0.
+ A.data, /*On entry, the M-by-N matrix A.
+ On exit, the elements on and above the diagonal of
+ the array contain the min(M,N)-by-N upper trapezoidal
+ matrix R (R is upper triangular if m >= n); the
+ elements below the diagonal, with the array TAU,
+ represent the orthogonal matrix Q as a product of
+ min(m,n) elementary reflectors.
+ */
+ &lda, // The leading dimension of the array A. LDA >= max(1,M).
+ tau, /*Dimension (min(M,N))
+ The scalar factors of the elementary reflectors
+ */
+ work, // Internal working array. dimension (MAX(1,LWORK))
+ &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
+ &info // status
+ );
+
+ if (info != 0) {
std::cout << "dgeqrf_:Preparation:" << -info << "th element had an illegal value" << std::endl;
throw vpMatrixException::badValue;
}
dimWork = allocate_work(&work);
- dgeqrf_(
- &rowNum_, //The number of rows of the matrix A. M >= 0.
- &colNum_, //The number of columns of the matrix A. N >= 0.
- A.data, /*On entry, the M-by-N matrix A.
- On exit, the elements on and above the diagonal of the array
- contain the min(M,N)-by-N upper trapezoidal matrix R (R is
- upper triangular if m >= n); the elements below the diagonal,
- with the array TAU, represent the orthogonal matrix Q as a
- product of min(m,n) elementary reflectors.
- */
- &lda, //The leading dimension of the array A. LDA >= max(1,M).
- tau, /*Dimension (min(M,N))
- The scalar factors of the elementary reflectors
+ dgeqrf_(&rowNum_, // The number of rows of the matrix A. M >= 0.
+ &colNum_, // The number of columns of the matrix A. N >= 0.
+ A.data, /*On entry, the M-by-N matrix A.
+ On exit, the elements on and above the diagonal of
+ the array contain the min(M,N)-by-N upper trapezoidal
+ matrix R (R is upper triangular if m >= n); the
+ elements below the diagonal, with the array TAU,
+ represent the orthogonal matrix Q as a product of
+ min(m,n) elementary reflectors.
*/
- work, //Internal working array. dimension (MAX(1,LWORK))
- &dimWork, //The dimension of the array WORK. LWORK >= max(1,N).
- &info //status
- );
-
-
- if(info != 0){
+ &lda, // The leading dimension of the array A. LDA >= max(1,M).
+ tau, /*Dimension (min(M,N))
+ The scalar factors of the elementary reflectors
+ */
+ work, // Internal working array. dimension (MAX(1,LWORK))
+ &dimWork, // The dimension of the array WORK. LWORK >= max(1,N).
+ &info // status
+ );
+
+ if (info != 0) {
std::cout << "dgeqrf_:" << -info << "th element had an illegal value" << std::endl;
throw vpMatrixException::badValue;
}
- //A now contains the R matrix in its upper triangular (in lapack convention)
+ // A now contains the R matrix in its upper triangular (in lapack
+ // convention)
C = A;
- //2) Invert R
- dtrtri_((char*)"U",(char*)"N",&dimTau,C.data,&lda,&info);
- if(info!=0){
- if(info < 0)
- std::cout << "dtrtri_:"<< -info << "th element had an illegal value" << std::endl;
- else if(info > 0){
- std::cout << "dtrtri_:R("<< info << "," <<info << ")"<< " is exactly zero. The triangular matrix is singular and its inverse can not be computed." << std::endl;
+ // 2) Invert R
+ dtrtri_((char *)"U", (char *)"N", &dimTau, C.data, &lda, &info);
+ if (info != 0) {
+ if (info < 0)
+ std::cout << "dtrtri_:" << -info << "th element had an illegal value" << std::endl;
+ else if (info > 0) {
+ std::cout << "dtrtri_:R(" << info << "," << info << ")"
+ << " is exactly zero. The triangular matrix is singular "
+ "and its inverse can not be computed."
+ << std::endl;
std::cout << "R=" << std::endl << C << std::endl;
}
throw vpMatrixException::badValue;
}
- //3) Zero-fill R^-1
- //the matrix is upper triangular for lapack but lower triangular for visp
- //we fill it with zeros above the diagonal (where we don't need the values)
- for(unsigned int i=0;i<C.getRows();i++)
- for(unsigned int j=0;j<C.getRows();j++)
- if(j>i) C[i][j] = 0.;
+ // 3) Zero-fill R^-1
+ // the matrix is upper triangular for lapack but lower triangular for visp
+ // we fill it with zeros above the diagonal (where we don't need the
+ // values)
+ for (unsigned int i = 0; i < C.getRows(); i++)
+ for (unsigned int j = 0; j < C.getRows(); j++)
+ if (j > i)
+ C[i][j] = 0.;
dimWork = -1;
- int ldc = lda;
-
- //4) Transpose Q and left-multiply it by R^-1
- //get R^-1*tQ
- //C contains R^-1
- //A contains Q
- dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
- if(info != 0){
- std::cout << "dormqr_:Preparation"<< -info << "th element had an illegal value" << std::endl;
+ integer ldc = lda;
+
+ // 4) Transpose Q and left-multiply it by R^-1
+ // get R^-1*tQ
+ // C contains R^-1
+ // A contains Q
+ dormqr_((char *)"R", (char *)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork,
+ &info);
+ if (info != 0) {
+ std::cout << "dormqr_:Preparation" << -info << "th element had an illegal value" << std::endl;
throw vpMatrixException::badValue;
}
dimWork = allocate_work(&work);
- dormqr_((char*)"R", (char*)"T", &rowNum_, &colNum_, &dimTau, A.data, &lda, tau, C.data, &ldc, work, &dimWork, &info);
+ 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;
+ if (info != 0) {
+ std::cout << "dormqr_:" << -info << "th element had an illegal value" << std::endl;
throw vpMatrixException::badValue;
}
delete[] tau;
delete[] work;
- }catch(vpMatrixException&){
+ } catch (vpMatrixException &) {
delete[] tau;
delete[] work;
throw;
}
return C;
-
}
#endif
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
Compute the inverse of a n-by-n matrix using the QR decomposition.
- Only available if lapack is installed.
+ Only available if Lapack 3rd party is installed. If Lapack is not installed
+we use a Lapack built-in version.
\return The inverse matrix.
@@ -208,31 +259,20 @@ int main()
A[3][0] = 1/7.; A[3][1] = 1/5.; A[3][2] = 1/6.; A[3][3] = 1/7.;
// Compute the inverse
- vpMatrix A_1; // A^-1
- A_1 = A.inverseByQR();
+ vpMatrix A_1 = A.inverseByQR();
std::cout << "Inverse by QR: \n" << A_1 << std::endl;
std::cout << "A*A^-1: \n" << A * A_1 << std::endl;
}
\endcode
- \sa pseudoInverse()
+ \sa inverseByLU(), inverseByCholesky()
*/
-
-#if defined(VISP_HAVE_LAPACK_C)
-vpMatrix
-vpMatrix::inverseByQR() const
+vpMatrix vpMatrix::inverseByQR() const
{
-
- if ( rowNum != colNum)
- {
- vpERROR_TRACE("\n\t\tCannot invert a non-square vpMatrix") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "Cannot invert a non-square vpMatrix")) ;
- }
-#ifdef VISP_HAVE_LAPACK_C
+#ifdef VISP_HAVE_LAPACK
return inverseByQRLapack();
+#else
+ throw(vpException(vpException::fatalError, "Cannot inverse matrix by QR. Install Lapack 3rd party"));
#endif
}
-
-#endif
diff --git a/modules/core/src/math/matrix/vpMatrix_svd.cpp b/modules/core/src/math/matrix/vpMatrix_svd.cpp
index 651e4b9..06c33dc 100644
--- a/modules/core/src/math/matrix/vpMatrix_svd.cpp
+++ b/modules/core/src/math/matrix/vpMatrix_svd.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,569 +36,241 @@
*
*****************************************************************************/
-#include <visp3/core/vpConfig.h>
-
-#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMatrixException.h>
-#include <visp3/core/vpDebug.h>
-
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
#include <iostream>
+#include <limits> // numeric_limits
-/*---------------------------------------------------------------------
-
-SVD related functions
-
----------------------------------------------------------------------*/
-
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static double pythag(double a, double b)
-{
- double absa, absb;
- absa = fabs(a);
- absb = fabs(b);
- if (absa > absb) return absa*sqrt(1.0+vpMath::sqr(absb/absa));
- //else return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
- else return (std::fabs(absb) <= std::numeric_limits<double>::epsilon() ? 0.0 : absb*sqrt(1.0+vpMath::sqr(absa/absb)));
-}
+#ifdef VISP_HAVE_EIGEN3
+#include <Eigen/SVD>
+#endif
-#ifdef SIGN
-#undef SIGN
+#ifdef VISP_HAVE_GSL
+#include <gsl/gsl_linalg.h>
#endif
-#define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
-/*!
- \brief Singular value decomposition
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+#include <opencv2/core/core.hpp>
+#endif
- Given a matrix A (m x n) this routine compute its sngular value decomposition
- A = U W V^T. The matrice U replace A on output. the diagonal matrix of
- singular value is output as a vector W (n). The matrix V (not the transpose
- V^T) is output as V (n x n)
+#ifdef VISP_HAVE_LAPACK
+#ifdef VISP_HAVE_LAPACK_BUILT_IN
+typedef long int integer;
+#else
+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);
- \warning Destructive wrt A
- \warning
+#include <stdio.h>
+#include <string.h>
+#endif
+/*---------------------------------------------------------------------
+SVD related functions
- \sa SVD for a more intuitive use
+---------------------------------------------------------------------*/
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
- This function is extracted from the NRC
+/*!
-*/
+ Singular value decomposition (SVD) using OpenCV 3rd party.
-#define MAX_ITER_SVD 50
-void vpMatrix::svdNr(vpColVector& W, vpMatrix& V)
-{
+ Given matrix \f$M\f$, this function computes it singular value decomposition
+such as
- unsigned int m = rowNum;
- unsigned int n = colNum;
- double epsilon = 10*std::numeric_limits<double>::epsilon();
+ \f[ M = U \Sigma V^{\top} \f]
- unsigned int flag,i,its,j,jj,k,l=0,nm=0;
- double c,f,h,s,x,y,z;
- double anorm=0.0,g=0.0,scale=0.0;
+ \warning This method is destructive wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed.
- // So that the original NRC code (using 1..n indexing) can be used
- // This should be considered as a temporary fix.
- double **a = new double*[m+1];
- double **v = new double*[n+1];
- // double **w = W.rowPtrs;
- // w--;
+ \param w : Vector of singular values: \f$ \Sigma = diag(w) \f$.
- double *w = new double[n+1];
- for (i=0;i<n;i++) w[i+1] = 0.0;
+ \param V : Matrix \f$ V \f$.
- for (i=1;i<=m;i++) {
- a[i] = this->rowPtrs[i-1]-1;
- }
- for (i=1;i<=n;i++) {
- v[i] = V.rowPtrs[i-1]-1;
- }
+ \return Matrix \f$ U \f$.
- if (m < n)
- {
- delete[] w;
- delete[] a;
- delete[] v;
- vpERROR_TRACE("\n\t\tSVDcmp: You must augment A with extra zero rows") ;
- throw(vpMatrixException(vpMatrixException::matrixError,
- "\n\t\tSVDcmp: You must augment A with "
- "extra zero rows")) ;
- }
- double* rv1=new double[n+1];
-
- for (i=1;i<=n;i++) {
- l=i+1;
- rv1[i]=scale*g;
- g=s=scale=0.0;
- if (i <= m) {
- for (k=i;k<=m;k++) scale += fabs(a[k][i]);
- //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
- if ((std::fabs(scale) > epsilon)/* || (fabs(scale) > EPS_SVD)*/) {
- for (k=i;k<=m;k++) {
- a[k][i] /= scale;
- s += a[k][i]*a[k][i];
- }
- f=a[i][i];
- g = -SIGN(sqrt(s),f);
- h=f*g-s;
- a[i][i]=f-g;
- if (i != n) {
- for (j=l;j<=n;j++) {
- for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
- f=s/h;
- for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
- }
- }
- for (k=i;k<=m;k++) a[k][i] *= scale;
- }
- }
- w[i]=scale*g;
- g=s=scale=0.0;
- if (i <= m && i != n) {
- for (k=l;k<=n;k++) scale += fabs(a[i][k]);
- //if ((scale != 0.0) || (fabs(scale) > EPS_SVD)) {
- if ((std::fabs(scale) > epsilon) /*|| (fabs(scale) > EPS_SVD)*/) {
- for (k=l;k<=n;k++) {
- a[i][k] /= scale;
- s += a[i][k]*a[i][k];
- }
- f=a[i][l];
- g = -SIGN(sqrt(s),f);
- h=f*g-s;
- a[i][l]=f-g;
- for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
- if (i != m) {
- for (j=l;j<=m;j++) {
- for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
- for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
- }
- }
- for (k=l;k<=n;k++) a[i][k] *= scale;
- }
- }
- anorm=vpMath::maximum(anorm,(fabs(w[i])+fabs(rv1[i])));
- }
- for (i=n;i>=1;i--) {
- if (i < n) {
- //if ((g) || (fabs(g) > EPS_SVD)) {
- if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
- for (j=l;j<=n;j++)
- v[j][i]=(a[i][j]/a[i][l])/g;
- for (j=l;j<=n;j++) {
- for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
- for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
- }
- }
- for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
- }
- v[i][i]=1.0;
- g=rv1[i];
- l=i;
- }
- for (i=n;i>=1;i--) {
- l=i+1;
- g=w[i];
- if (i < n)
- for (j=l;j<=n;j++) a[i][j]=0.0;
- //if ((g != 0.0) || (fabs(g) > EPS_SVD)) {
- if ((std::fabs(g) > epsilon) /*|| (fabs(g) > EPS_SVD)*/) {
- g=1.0/g;
- if (i != n) {
- for (j=l;j<=n;j++) {
- for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
- f=(s/a[i][i])*g;
- for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
- }
- }
- for (j=i;j<=m;j++) a[j][i] *= g;
- } else {
- for (j=i;j<=m;j++) a[j][i]=0.0;
- }
- ++a[i][i];
- }
- for (k=n;k>=1;k--) {
- for (its=1;its<=MAX_ITER_SVD;its++) {
- flag=1;
- for (l=k;l>=1;l--) {
- nm=l-1;
- //if ((fabs(rv1[l])+anorm == anorm) || (fabs(rv1[l]) <= EPS_SVD)) {
- if ((std::fabs(rv1[l]) <= epsilon) /*|| (fabs(rv1[l]) <= EPS_SVD)*/) {
- flag=0;
- break;
- }
- //if ((fabs(w[nm])+anorm == anorm) || (fabs(w[nm]) <= EPS_SVD)) break;
- if ((std::fabs(w[nm]) <= epsilon) /*|| (fabs(w[nm]) <= EPS_SVD)*/) break;
- }
- if (flag) {
- c=0.0;
- s=1.0;
- for (i=l;i<=k;i++) {
- f=s*rv1[i];
- //if ((fabs(f)+anorm != anorm) || (fabs(f) <= EPS_SVD)) {
- if ((std::fabs(f) > epsilon) /*|| (fabs(f) <= EPS_SVD)*/) {
- g=w[i];
- h=pythag(f,g);
- w[i]=h;
- h=1.0/h;
- c=g*h;
- s=(-f*h);
- for (j=1;j<=m;j++) {
- y=a[j][nm];
- z=a[j][i];
- a[j][nm]=y*c+z*s;
- a[j][i]=z*c-y*s;
- }
- }
- }
- }
- z=w[k];
- if (l == k) {
- if (z < 0.0) {
- w[k] = -z;
- for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
- }
- break;
- }
- if (its == MAX_ITER_SVD)
- {
- for (i=0;i<n;i++) W[i] = w[i+1];
-
- vpERROR_TRACE("\n\t\t No convergence in SVDcmp ") ;
- std::cout << *this <<std::endl ;
- // throw(vpMatrixException(vpMatrixException::matrixError,
- // "\n\t\t No convergence in SVDcmp ")) ;
- }
- x=w[l];
- nm=k-1;
- y=w[nm];
- g=rv1[nm];
- h=rv1[k];
- f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
- g=pythag(f,1.0);
- f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
- c=s=1.0;
- for (j=l;j<=nm;j++) {
- i=j+1;
- g=rv1[i];
- y=w[i];
- h=s*g;
- g=c*g;
- z=pythag(f,h);
- rv1[j]=z;
- if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
- c=f/z;
- s=h/z;
- }
- f=x*c+g*s;
- g=g*c-x*s;
- h=y*s;
- y=y*c;
- for (jj=1;jj<=n;jj++) {
- x=v[jj][j];
- z=v[jj][i];
- v[jj][j]=x*c+z*s;
- v[jj][i]=z*c-x*s;
- }
- z=pythag(f,h);
- w[j]=z;
- //if ((z != 0.0) || (fabs(z) > EPS_SVD)) {
- if ((std::fabs(z) > epsilon) /*|| (fabs(z) > EPS_SVD)*/) {
- z=1.0/z;
- c=f*z;
- s=h*z;
- }
- f=(c*g)+(s*y);
- x=(c*y)-(s*g);
- for (jj=1;jj<=m;jj++) {
- y=a[jj][j];
- z=a[jj][i];
- a[jj][j]=y*c+z*s;
- a[jj][i]=z*c-y*s;
- }
- }
- rv1[l]=0.0;
- rv1[k]=f;
- w[k]=x;
- }
- }
- for (i=0;i<n;i++) W[i] = w[i+1];
+ \note The singular values are ordered in decreasing
+ fashion in \e w. It means that the highest singular value is in \e w[0].
+ Here an example of SVD decomposition of a non square Matrix M.
- delete[] w;
- delete[] rv1;
- delete[] a;
- delete[] v;
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+int main()
+{
+ vpMatrix M(3,2);
+ M[0][0] = 1;
+ M[1][0] = 2;
+ M[2][0] = 0.5;
+
+ M[0][1] = 6;
+ M[1][1] = 8 ;
+ M[2][1] = 9 ;
+
+ vpMatrix V;
+ vpColVector w;
+ vpMatrix Mrec;
+ vpMatrix Sigma;
+
+ M.svdOpenCV(w, V);
+ // Here M is modified and is now equal to U
+
+ // Construct the diagonal matrix from the singular values
+ Sigma.diag(w);
+
+ // Reconstruct the initial matrix M using the decomposition
+ Mrec = M * Sigma * V.t();
+
+ // Here, Mrec is obtained equal to the initial value of M
+ // Mrec[0][0] = 1;
+ // Mrec[1][0] = 2;
+ // Mrec[2][0] = 0.5;
+ // Mrec[0][1] = 6;
+ // Mrec[1][1] = 8 ;
+ // Mrec[2][1] = 9 ;
+
+ std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
}
+ \endcode
-#undef SIGN
-#undef PYTHAG
-
-/*!
- \brief solve a linear system AX = B using an SVD decomposition
-
- Solves AX = B for a vector X, where A is am matrix m x n, w a vector (n) and
- v a matrix (n x n) as returned by SVDcmp. m and n are the dimensions of A,
- and will be equal for square matrices. b (m) is the input right-hand
- side. x (n) is the output solution vector. No input quantities are
- destroyed, so the routine may be called sequentially with different b's.
-
- \warning not to be used directly
-
- \sa to be used with svd first
-
- \sa solveBySVD and SVDsolve for a more intuitive solution of AX=B problem
+ \sa svd(), svdEigen3(), svdLapack(), svdGsl()
*/
-
-
-void vpMatrix::SVBksb( const vpColVector& w,
- const vpMatrix& v,
- const vpColVector& b, vpColVector& x)
+void vpMatrix::svdOpenCV(vpColVector &w, vpMatrix &V)
{
- unsigned int m = this->rowNum;
- unsigned int n = this->colNum;
- double** u = rowPtrs;
-
- unsigned int jj,j,i;
- double s,*tmp;
-
- tmp=new double[n];
- for (j=0;j<n;j++) {
- s=0.0;
- //if (w[j])
- if (std::fabs(w[j]) > std::numeric_limits<double>::epsilon())
- {
- for (i=0;i<m;i++) s += u[i][j]*b[i];
- s /= w[j];
- }
- tmp[j]=s;
- }
- for (j=0;j<n;j++) {
- s=0.0;
- for (jj=0;jj<n;jj++) s += v[j][jj]*tmp[jj];
- x[j]=s;
- }
- delete [] tmp;
+ int rows = (int)this->getRows();
+ int cols = (int)this->getCols();
+ cv::Mat m(rows, cols, CV_64F, this->data);
+ cv::SVD opencvSVD(m);
+ cv::Mat opencvV = opencvSVD.vt;
+ cv::Mat opencvW = opencvSVD.w;
+ V.resize((unsigned int)opencvV.rows, (unsigned int)opencvV.cols);
+ w.resize((unsigned int)(opencvW.rows * opencvW.cols));
+
+ memcpy(V.data, opencvV.data, (size_t)(8 * opencvV.rows * opencvV.cols));
+ V = V.transpose();
+ memcpy(w.data, opencvW.data, (size_t)(8 * opencvW.rows * opencvW.cols));
+ this->resize((unsigned int)opencvSVD.u.rows, (unsigned int)opencvSVD.u.cols);
+ memcpy(this->data, opencvSVD.u.data, (size_t)(8 * opencvSVD.u.rows * opencvSVD.u.cols));
}
-#define TOL 1.0e-5
+#endif
+#ifdef VISP_HAVE_LAPACK
/*!
- \brief Compute the SVD decomposition
- Computes the singular value decomposition of the matrix, U.
- The contents of U are replaced such that A = U*S*V' where A represents
- the initial value of U.
- S is understood to have only room for ncol elements.
- The matrix V may be NULL, in which case, no values are returned for V.
+ Singular value decomposition (SVD) using Lapack 3rd party.
- This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
- for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
+ Given matrix \f$M\f$, this function computes it singular value decomposition
+such as
- Gary William Flake
- http://research.yahoo.com/~flakeg/nodelib/html/
- http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html (not valid)
+ \f[ M = U \Sigma V^{\top} \f]
- \sa SVDcmp and SVDksb
-*/
-
-#define TOLERANCE 1.0e-7
-
-static
-void svd_internal_use(double *U, double *S, double *V,
- unsigned int nRow, unsigned int nCol)
-{
- unsigned int i, j, k, EstColRank, RotCount, SweepCount, slimit;
- double eps, e2, tol, vt, p, x0, y0, q, r, c0, s0, d1, d2;
-
- eps = TOLERANCE;
- slimit = nCol / 4;
- if (slimit < 6.0)
- slimit = 6;
- SweepCount = 0;
- e2 = 10.0 * nRow * eps * eps;
- tol = eps * .1;
- EstColRank = nCol;
- if(V)
- for (i = 0; i < nCol; i++)
- for (j = 0; j < nCol; j++) {
- V[nCol * i + j] = 0.0;
- V[nCol * i + i] = 1.0;
- }
- RotCount = EstColRank * (EstColRank - 1) / 2;
- while (RotCount != 0 && SweepCount <= slimit) {
- RotCount = EstColRank * (EstColRank - 1) / 2;
- SweepCount++;
- for (j = 0; j < EstColRank - 1; j++) {
- for (k = j + 1; k < EstColRank; k++) {
- p = q = r = 0.0;
- for (i = 0; i < nRow; i++) {
- x0 = U[nCol * i + j];
- y0 = U[nCol * i + k];
- p += x0 * y0;
- q += x0 * x0;
- r += y0 * y0;
- }
- S[j] = q;
- S[k] = r;
- if (q >= r) {
- if (q <= e2 * S[0] || fabs(p) <= tol * q)
- RotCount--;
- else {
- p /= q;
- r = 1 - r / q;
- vt = sqrt(4 * p * p + r * r);
- c0 = sqrt(fabs(.5 * (1 + r / vt)));
- s0 = p / (vt * c0);
- for (i = 0; i < nRow; i++) {
- d1 = U[nCol * i + j];
- d2 = U[nCol * i + k];
- U[nCol * i + j] = d1 * c0 + d2 * s0;
- U[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- if(V)
- for (i = 0; i < nCol; i++) {
- d1 = V[nCol * i + j];
- d2 = V[nCol * i + k];
- V[nCol * i + j] = d1 * c0 + d2 * s0;
- V[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- }
- }
- else {
- p /= r;
- q = q / r - 1;
- vt = sqrt(4 * p * p + q * q);
- s0 = sqrt(fabs(.5 * (1 - q / vt)));
- if (p < 0)
- s0 = -s0;
- c0 = p / (vt * s0);
- for (i = 0; i < nRow; i++) {
- d1 = U[nCol * i + j];
- d2 = U[nCol * i + k];
- U[nCol * i + j] = d1 * c0 + d2 * s0;
- U[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- if(V)
- for (i = 0; i < nCol; i++) {
- d1 = V[nCol * i + j];
- d2 = V[nCol * i + k];
- V[nCol * i + j] = d1 * c0 + d2 * s0;
- V[nCol * i + k] = -d1 * s0 + d2 * c0;
- }
- }
- }
- }
- while (EstColRank >= 3 && S[(EstColRank - 1)] <= S[0] * tol + tol * tol)
- EstColRank--;
- }
- for(i = 0; i < nCol; i++)
- S[i] = sqrt(S[i]);
- for(i = 0; i < nCol; i++)
- for(j = 0; j < nRow; j++)
- U[nCol * j + i] = U[nCol * j + i] / S[i];
-}
-
-/*!
- \brief Singular value decomposition (other function)
-
- Given a matrix A (m x n) this routine compute its singular value decomposition
- A = U W V^T. The matrice U replace A on output. the diagonal matrix of
- singular value is output as a vector W (n). The matrix V (not the transpose
- V^T) is output as V (n x n)
+ \warning This method is destructive wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed.
+ \param w : Vector of singular values: \f$ \Sigma = diag(w) \f$.
- \warning Destructive wrt A
- \warning
+ \param V : Matrix \f$ V \f$.
+ \return Matrix \f$ U \f$.
- \sa SVD for a more intuitive use
+ \note The singular values are ordered in decreasing
+ fashion in \e w. It means that the highest singular value is in \e w[0].
- This SVD routine is based on pgs 30-48 of "Compact Numerical Methods
- for Computers" by J.C. Nash (1990), used to compute the pseudoinverse.
+ Here an example of SVD decomposition of a non square Matrix M.
- http://www.neci.nec.com/homepages/flake/nodelib/html/svd.html
- http://labs.yahoo.com/~flakeg/nodelib/html/svd.html
-
- \sa SVDcmp and SVDksb
-
-*/
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
-void vpMatrix::svdFlake(vpColVector &W, vpMatrix &V)
+int main()
{
-
-
- svd_internal_use(data, W.data, V.data, getRows(), getCols());
+ vpMatrix M(3,2);
+ M[0][0] = 1;
+ M[1][0] = 2;
+ M[2][0] = 0.5;
+
+ M[0][1] = 6;
+ M[1][1] = 8 ;
+ M[2][1] = 9 ;
+
+ vpMatrix V;
+ vpColVector w;
+ vpMatrix Mrec;
+ vpMatrix Sigma;
+
+ M.svdLapack(w, V);
+ // Here M is modified and is now equal to U
+
+ // Construct the diagonal matrix from the singular values
+ Sigma.diag(w);
+
+ // Reconstruct the initial matrix M using the decomposition
+ Mrec = M * Sigma * V.t();
+
+ // Here, Mrec is obtained equal to the initial value of M
+ // Mrec[0][0] = 1;
+ // Mrec[1][0] = 2;
+ // Mrec[2][0] = 0.5;
+ // Mrec[0][1] = 6;
+ // Mrec[1][1] = 8 ;
+ // Mrec[2][1] = 9 ;
+
+ std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
}
+ \endcode
+ \sa svd(), svdEigen3(), svdOpenCV(), svdGsl()
+*/
+void vpMatrix::svdLapack(vpColVector &w, vpMatrix &V)
+{
+ w.resize(this->getCols());
+ V.resize(this->getCols(), this->getCols());
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/core/core.hpp>
-void vpMatrix::svdOpenCV(vpColVector& w, vpMatrix& v){
- int rows = (int)this->getRows();
- int cols = (int)this->getCols();
- cv::Mat m(rows, cols, CV_64F, this->data);
- cv::SVD opencvSVD(m);
- cv::Mat opencvV = opencvSVD.vt;
- cv::Mat opencvW = opencvSVD.w;
- v.resize((unsigned int)opencvV.rows, (unsigned int)opencvV.cols);
- w.resize((unsigned int)(opencvW.rows*opencvW.cols));
-
- memcpy(v.data, opencvV.data, (size_t)(8*opencvV.rows*opencvV.cols));
- v=v.transpose();
- memcpy(w.data, opencvW.data, (size_t)(8*opencvW.rows*opencvW.cols));
- this->resize((unsigned int)opencvSVD.u.rows, (unsigned int)opencvSVD.u.cols);
- memcpy(this->data,opencvSVD.u.data, (size_t)(8*opencvSVD.u.rows*opencvSVD.u.cols));
-}
-
-#endif
-
-#ifdef VISP_HAVE_LAPACK_C
-extern "C" int dgesdd_(char *jobz, int *m, int *n, double *a, int *lda, double *s, double *u, int *ldu, double *vt, int *ldvt, double *work, int *lwork, int *iwork, int *info);
-#include <stdio.h>
-#include <string.h>
-
-void vpMatrix::svdLapack(vpColVector& W, vpMatrix& V){
- /* unsigned */ int m = static_cast<int>(this->getCols()), n = static_cast<int>(this->getRows()), lda = m, ldu = m, ldvt = std::min(m,n);
- int info, lwork;
+ integer m = (integer)(this->getCols());
+ integer n = (integer)(this->getRows());
+ integer lda = m;
+ integer ldu = m;
+ integer ldvt = (std::min)(m, n);
+ integer info, lwork;
double wkopt;
- double* work;
-
- int* iwork = new int[8*static_cast<unsigned int>(std::min(n,m))];
-
- double *s = W.data;
- double* a = new double[static_cast<unsigned int>(lda*n)];
- memcpy(a,this->data,this->getRows()*this->getCols()*sizeof(double));
- double* u = V.data;
- double* vt = this->data;
+ double *work;
+ integer *iwork = new integer[8 * static_cast<integer>((std::min)(n, m))];
+ double *s = w.data;
+ double *a = new double[static_cast<unsigned int>(lda * n)];
+ memcpy(a, this->data, this->getRows() * this->getCols() * sizeof(double));
+ double *u = V.data;
+ double *vt = this->data;
lwork = -1;
- dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info );
+ dgesdd_((char *)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, &wkopt, &lwork, iwork, &info);
lwork = (int)wkopt;
work = new double[static_cast<unsigned int>(lwork)];
- dgesdd_( (char*)"S", &m, &n, a, &lda, s, u, &ldu, vt, &ldvt, work, &lwork, iwork, &info );
-
- if( info > 0 ) {
- vpTRACE("The algorithm computing SVD failed to converge.");
- throw(vpMatrixException(vpMatrixException::fatalError,
- "The algorithm computing SVD failed to converge.")) ;
+ 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."));
}
- V=V.transpose();
+ V = V.transpose();
delete[] work;
delete[] iwork;
delete[] a;
@@ -605,73 +278,81 @@ void vpMatrix::svdLapack(vpColVector& W, vpMatrix& V){
#endif
#ifdef VISP_HAVE_GSL
-#include <gsl/gsl_linalg.h>
-void
-vpMatrix::svdGsl(vpColVector& w, vpMatrix& v)
-{
-
-#if 0
- // premier test avec la gsl 1. on recopie...
- int i,j ;
-
- int nc = getCols() ;
- int nr = getRows() ;
- gsl_matrix *A = gsl_matrix_alloc(nr, nc) ;
-
- int Atda = A->tda ;
- for (i=0 ; i < nr ; i++)
- {
- int k = i*Atda ;
- for (j=0 ; j < nc ; j++)
- A->data[k+j] = (*this)[i][j] ;
- }
- // gsl_matrix_set(A,i,j,(*this)[i][j]) ;
+/*!
- gsl_matrix *V = gsl_matrix_alloc(nc, nc) ;
- gsl_vector *S = gsl_vector_alloc(nc) ;
- gsl_vector *work = gsl_vector_alloc(nc) ;
+ Singular value decomposition (SVD) using GSL 3rd party.
- gsl_linalg_SV_decomp(A,V,S, work) ;
-// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
+ Given matrix \f$M\f$, this function computes it singular value decomposition
+such as
+ \f[ M = U \Sigma V^{\top} \f]
- //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
- // autremement (surement !)
+ \warning This method is destructive wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed.
- Atda = A->tda ;
- for (i=0 ; i < nr ; i++)
- for (j=0 ; j < nc ; j++)
- (*this)[i][j] = gsl_matrix_get(A,i,j) ;
+ \param w : Vector of singular values: \f$ \Sigma = diag(w) \f$.
- int Vtda = V->tda ;
- for (i=0 ; i < nc ; i++)
- {
- int k = i*Vtda ;
- for (j=0 ; j < nc ; j++)
- v[i][j] = V->data[k+j] ;
- }
+ \param V : Matrix \f$ V \f$.
- for (j=0 ; j < nc ; j++)
- w[j] = gsl_vector_get(S,j) ;
+ \return Matrix \f$ U \f$.
+ \note The singular values are ordered in decreasing
+ fashion in \e w. It means that the highest singular value is in \e w[0].
- gsl_matrix_free(V) ;
- gsl_matrix_free(A) ;
- gsl_vector_free(S) ;
- gsl_vector_free(work) ;
+ Here an example of SVD decomposition of a non square Matrix M.
-#else //optimisation Anthony 20/03/2008
-
- unsigned int nc = getCols() ;
- unsigned int nr = getRows() ;
- gsl_vector *work = gsl_vector_alloc(nc) ;
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
-// gsl_linalg_SV_decomp_jacobi(A,V,S) ;
+int main()
+{
+ vpMatrix M(3,2);
+ M[0][0] = 1;
+ M[1][0] = 2;
+ M[2][0] = 0.5;
+
+ M[0][1] = 6;
+ M[1][1] = 8 ;
+ M[2][1] = 9 ;
+
+ vpMatrix V;
+ vpColVector w;
+ vpMatrix Mrec;
+ vpMatrix Sigma;
+
+ M.svdGsl(w, V);
+ // Here M is modified and is now equal to U
+
+ // Construct the diagonal matrix from the singular values
+ Sigma.diag(w);
+
+ // Reconstruct the initial matrix M using the decomposition
+ Mrec = M * Sigma * V.t();
+
+ // Here, Mrec is obtained equal to the initial value of M
+ // Mrec[0][0] = 1;
+ // Mrec[1][0] = 2;
+ // Mrec[2][0] = 0.5;
+ // Mrec[0][1] = 6;
+ // Mrec[1][1] = 8 ;
+ // Mrec[2][1] = 9 ;
+
+ std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
+}
+ \endcode
+ \sa svd(), svdEigen3(), svdOpenCV(), svdLapack()
+*/
+void vpMatrix::svdGsl(vpColVector &w, vpMatrix &V)
+{
+ w.resize(this->getCols());
+ V.resize(this->getCols(), this->getCols());
- //l'acces par gsl_matrix_get est tres lourd, voir si on peut pas faire
- // autremement (surement !)
+ unsigned int nc = getCols();
+ unsigned int nr = getRows();
+ gsl_vector *work = gsl_vector_alloc(nc);
gsl_matrix A;
A.size1 = nr;
@@ -680,34 +361,114 @@ vpMatrix::svdGsl(vpColVector& w, vpMatrix& v)
A.data = this->data;
A.owner = 0;
A.block = 0;
-
- gsl_matrix V;
- V.size1 = nc;
- V.size2 = nc;
- V.tda = V.size2;
- V.data = v.data;
- V.owner = 0;
- V.block = 0;
-
+
+ gsl_matrix V_;
+ V_.size1 = nc;
+ V_.size2 = nc;
+ V_.tda = V_.size2;
+ V_.data = V.data;
+ V_.owner = 0;
+ V_.block = 0;
+
gsl_vector S;
S.size = nc;
S.stride = 1;
S.data = w.data;
S.owner = 0;
S.block = 0;
-
- gsl_linalg_SV_decomp(&A,&V,&S, work) ;
-
- gsl_vector_free(work) ;
-#endif
+ gsl_linalg_SV_decomp(&A, &V_, &S, work);
+
+ gsl_vector_free(work);
}
#endif // # #GSL
+#ifdef VISP_HAVE_EIGEN3
+/*!
+
+ Singular value decomposition (SVD) using Eigen3 3rd party.
-#undef TOL
-#undef TOLERANCE
+ Given matrix \f$M\f$, this function computes it singular value decomposition
+such as
-#undef MAX_ITER_SVD
+ \f[ M = U \Sigma V^{\top} \f]
+
+ \warning This method is destructive wrt. to the matrix \f$ M \f$ to
+ decompose. You should make a COPY of that matrix if needed.
+
+ \param w : Vector of singular values: \f$ \Sigma = diag(w) \f$.
+
+ \param V : Matrix \f$ V \f$.
+
+ \return Matrix \f$ U \f$.
+
+ \note The singular values are ordered in decreasing
+ fashion in \e w. It means that the highest singular value is in \e w[0].
+
+ Here an example of SVD decomposition of a non square Matrix M.
+
+\code
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+
+int main()
+{
+ vpMatrix M(3,2);
+ M[0][0] = 1;
+ M[1][0] = 2;
+ M[2][0] = 0.5;
+
+ M[0][1] = 6;
+ M[1][1] = 8 ;
+ M[2][1] = 9 ;
+
+ vpMatrix V;
+ vpColVector w;
+ vpMatrix Mrec;
+ vpMatrix Sigma;
+
+ M.svdEigen3(w, V);
+ // Here M is modified and is now equal to U
+
+ // Construct the diagonal matrix from the singular values
+ Sigma.diag(w);
+
+ // Reconstruct the initial matrix M using the decomposition
+ Mrec = M * Sigma * V.t();
+
+ // Here, Mrec is obtained equal to the initial value of M
+ // Mrec[0][0] = 1;
+ // Mrec[1][0] = 2;
+ // Mrec[2][0] = 0.5;
+ // Mrec[0][1] = 6;
+ // Mrec[1][1] = 8 ;
+ // Mrec[2][1] = 9 ;
+
+ std::cout << "Reconstructed M matrix: \n" << Mrec << std::endl;
+}
+ \endcode
+
+ \sa svd(), svdLapack(), svdOpenCV(), svdGsl()
+*/
+void vpMatrix::svdEigen3(vpColVector &w, vpMatrix &V)
+{
+ w.resize(this->getCols());
+ V.resize(this->getCols(), this->getCols());
+
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > M(this->data, this->getRows(),
+ this->getCols());
+
+ Eigen::JacobiSVD<Eigen::MatrixXd> svd(M, Eigen::ComputeThinU | Eigen::ComputeThinV);
+
+ Eigen::Map<Eigen::VectorXd> w_(w.data, w.size());
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > V_(V.data, V.getRows(),
+ V.getCols());
+ Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> > U_(this->data, this->getRows(),
+ this->getCols());
+ w_ = svd.singularValues();
+ V_ = svd.matrixV();
+ U_ = svd.matrixU();
+}
+#endif
-#endif // doxygen should skip this
+#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/core/src/math/matrix/vpRowVector.cpp b/modules/core/src/math/matrix/vpRowVector.cpp
index 36e7c66..0a027c1 100644
--- a/modules/core/src/math/matrix/vpRowVector.cpp
+++ b/modules/core/src/math/matrix/vpRowVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,40 +36,37 @@
*
*****************************************************************************/
-
/*!
\file vpRowVector.cpp
\brief Definition of vpRowVector class member
*/
-#include <string.h>
-#include <stdlib.h>
-#include <sstream>
#include <assert.h>
#include <cmath>
+#include <sstream>
+#include <stdlib.h>
+#include <string.h>
#include <visp3/core/vpArray2D.h>
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpException.h>
-#include <visp3/core/vpRowVector.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpRowVector.h>
//! Copy operator. Allow operation such as A = v
-vpRowVector & vpRowVector::operator=(const vpRowVector &v)
+vpRowVector &vpRowVector::operator=(const vpRowVector &v)
{
- unsigned int k = v.colNum ;
- if (colNum != k){
+ unsigned int k = v.colNum;
+ if (colNum != k) {
try {
resize(k);
- }
- catch(...)
- {
+ } catch (...) {
throw;
}
}
- memcpy(data, v.data, colNum*sizeof(double)) ;
+ memcpy(data, v.data, colNum * sizeof(double));
return *this;
}
@@ -77,49 +75,49 @@ vpRowVector & vpRowVector::operator=(const vpRowVector &v)
Initialize a row vector from a 1-by-n size matrix.
\warning Handled with care m should be a 1 column matrix.
- \exception vpException::dimensionError If the matrix is not a 1-by-n dimension matrix.
+ \exception vpException::dimensionError If the matrix is not a 1-by-n
+ dimension matrix.
*/
-vpRowVector & vpRowVector::operator=(const vpMatrix &M)
+vpRowVector &vpRowVector::operator=(const vpMatrix &M)
{
- if (M.getRows() != 1 ) {
- throw(vpException(vpException::dimensionError,
- "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
- M.getCols(), M.getRows(), M.getCols())) ;
+ if (M.getRows() != 1) {
+ throw(vpException(vpException::dimensionError, "Cannot initialize a (1x%d) row vector from a (%dx%d) matrix",
+ M.getCols(), M.getRows(), M.getCols()));
}
if (M.getCols() != colNum)
resize(M.getCols());
- memcpy(data, M.data, colNum*sizeof(double)) ;
+ memcpy(data, M.data, colNum * sizeof(double));
return *this;
}
/*!
Initialize a row vector from a standard vector of double.
*/
-vpRowVector & vpRowVector::operator=(const std::vector<double> &v)
+vpRowVector &vpRowVector::operator=(const std::vector<double> &v)
{
resize((unsigned int)v.size());
- for(unsigned int i=0; i<v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = v[i];
return *this;
}
/*!
Initialize a row vector from a standard vector of double.
*/
-vpRowVector & vpRowVector::operator=(const std::vector<float> &v)
+vpRowVector &vpRowVector::operator=(const std::vector<float> &v)
{
resize((unsigned int)v.size());
- for(unsigned int i=0; i<v.size(); i++)
+ for (unsigned int i = 0; i < v.size(); i++)
(*this)[i] = (float)v[i];
return *this;
}
//! Initialize each element of the vector with \e x.
-vpRowVector & vpRowVector::operator=(double x)
+vpRowVector &vpRowVector::operator=(double x)
{
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<colNum; j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
rowPtrs[i][j] = x;
}
}
@@ -143,15 +141,14 @@ vpRowVector & vpRowVector::operator=(double x)
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())) ;
+ if (getCols() != nelements) {
+ throw(vpException(vpException::dimensionError, "Cannot multiply (1x%d) row vector by (%dx1) column vector", colNum,
+ x.getRows()));
}
double scalar = 0.0;
- for (unsigned int i=0; i<nelements; i++) {
+ for (unsigned int i = 0; i < nelements; i++) {
scalar += (*this)[i] * x[i];
}
return scalar;
@@ -162,8 +159,8 @@ double vpRowVector::operator*(const vpColVector &x) const
\param M : Matrix.
- \warning The number of elements of the row vector must be equal to the number
- of rows of the matrix.
+ \warning The number of elements of the row vector must be equal to the
+ number of rows of the matrix.
\exception vpException::dimensionError If the number of elements of the
row vector is not equal to the number of rows of the matrix.
@@ -176,21 +173,20 @@ 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;
- for (unsigned int i=0;i<colNum;i++) {
- double bi = data[i] ; // optimization em 5/12/2006
- for (unsigned int j=0;j<M.getCols();j++) {
- c[j]+=bi*M[i][j];
+ for (unsigned int i = 0; i < colNum; i++) {
+ double bi = data[i]; // optimization em 5/12/2006
+ for (unsigned int j = 0; j < M.getCols(); j++) {
+ c[j] += bi * M[i][j];
}
}
- return c ;
+ return c;
}
/*!
@@ -216,9 +212,10 @@ vpRowVector vpRowVector::operator*(double x) const
{
vpRowVector v(colNum);
- double *vd = v.data ; double *d = data ;
+ double *vd = v.data;
+ double *d = data;
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
*(vd++) = (*d++) * x;
return v;
}
@@ -242,7 +239,7 @@ vpRowVector vpRowVector::operator*(double x) const
*/
vpRowVector &vpRowVector::operator*=(double x)
{
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
(*this)[i] *= x;
return (*this);
}
@@ -270,9 +267,10 @@ vpRowVector vpRowVector::operator/(double x) const
{
vpRowVector v(colNum);
- double *vd = v.data ; double *d = data ;
+ double *vd = v.data;
+ double *d = data;
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
*(vd++) = (*d++) / x;
return v;
}
@@ -297,7 +295,7 @@ vpRowVector vpRowVector::operator/(double x) const
*/
vpRowVector &vpRowVector::operator/=(double x)
{
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
(*this)[i] /= x;
return (*this);
}
@@ -316,10 +314,11 @@ vpRowVector vpRowVector::operator-() const
{
vpRowVector A(colNum);
- double *vd = A.data ; double *d = data ;
+ double *vd = A.data;
+ double *d = data;
- for (unsigned int i=0; i<colNum; i++)
- *(vd++)= - (*d++);
+ for (unsigned int i = 0; i < colNum; i++)
+ *(vd++) = -(*d++);
return A;
}
@@ -330,15 +329,14 @@ 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())) ;
+ if (getCols() != m.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(),
+ m.getCols()));
}
- vpRowVector v(colNum) ;
+ vpRowVector v(colNum);
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
v[i] = (*this)[i] - m[i];
return v;
}
@@ -349,51 +347,48 @@ 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())) ;
+ if (getCols() != v.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
+ v.getCols()));
}
- vpRowVector r(colNum) ;
+ vpRowVector r(colNum);
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
r[i] = (*this)[i] + v[i];
return r;
}
/*!
Operator that allows to add two row vectors that have the same size.
- \exception vpException::dimensionError If the size of the two vectors differ.
+ \exception vpException::dimensionError If the size of the two vectors
+ differ.
*/
-vpRowVector &
-vpRowVector::operator+=(vpRowVector v)
+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())) ;
+ if (getCols() != v.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot add (1x%d) row vector to (1x%d) row vector", getCols(),
+ v.getCols()));
}
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
(*this)[i] += v[i];
return (*this);
}
/*!
Operator that allows to substract two row vectors that have the same size.
- \exception vpException::dimensionError If the size of the two vectors differ.
+ \exception vpException::dimensionError If the size of the two vectors
+ differ.
*/
-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())) ;
+ if (getCols() != v.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot substract (1x%d) row vector to (1x%d) row vector", getCols(),
+ v.getCols()));
}
- for (unsigned int i=0;i<colNum;i++)
+ for (unsigned int i = 0; i < colNum; i++)
(*this)[i] -= v[i];
return (*this);
}
@@ -419,7 +414,7 @@ A: 0 1 2 3 4
\endcode
*/
-vpRowVector & vpRowVector::operator<<(const vpRowVector &v)
+vpRowVector &vpRowVector::operator<<(const vpRowVector &v)
{
*this = v;
return *this;
@@ -431,7 +426,7 @@ vpRowVector & vpRowVector::operator<<(const vpRowVector &v)
vpColVector vpRowVector::t() const
{
vpColVector v(colNum);
- memcpy(v.data, data, colNum*sizeof(double)) ;
+ memcpy(v.data, data, colNum * sizeof(double));
return v;
}
@@ -439,63 +434,53 @@ vpColVector vpRowVector::t() const
Transpose the row vector. The resulting vector becomes a column vector.
\sa t()
*/
-vpColVector vpRowVector::transpose() const
-{
- return t();
-}
+vpColVector vpRowVector::transpose() const { return t(); }
/*!
Transpose the row vector. The resulting vector \e v becomes a column vector.
\sa t()
*/
-void vpRowVector::transpose(vpColVector &v) const
-{
- v = t();
-}
+void vpRowVector::transpose(vpColVector &v) const { v = t(); }
/*!
Constructor that creates a row vector corresponding to row \e i
of matrix \e M.
*/
-vpRowVector::vpRowVector (const vpMatrix &M, unsigned int i)
- : vpArray2D<double>(1, M.getCols())
+vpRowVector::vpRowVector(const vpMatrix &M, unsigned int i) : vpArray2D<double>(1, M.getCols())
{
- for(unsigned int j=0; j< M.getCols(); j++)
+ for (unsigned int j = 0; j < M.getCols(); j++)
(*this)[j] = M[i][j];
}
/*!
Constructor that creates a row vector from a 1-by-n matrix \e M.
- \exception vpException::dimensionError If the matrix is not a 1-by-n matrix.
+ \exception vpException::dimensionError If the matrix is not a 1-by-n
+ matrix.
*/
-vpRowVector::vpRowVector (const vpMatrix &M)
- : vpArray2D<double>(1, M.getCols())
+vpRowVector::vpRowVector(const vpMatrix &M) : vpArray2D<double>(1, M.getCols())
{
- if(M.getRows()!=1) {
- throw(vpException(vpException::dimensionError,
- "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
- M.getCols(), M.getRows(), M.getCols())) ;
+ if (M.getRows() != 1) {
+ throw(vpException(vpException::dimensionError, "Cannot construct a (1x%d) row vector from a (%dx%d) matrix",
+ M.getCols(), M.getRows(), M.getCols()));
}
- for(unsigned int j=0; j< M.getCols(); j++)
+ for (unsigned int j = 0; j < M.getCols(); j++)
(*this)[j] = M[0][j];
}
/*!
Constructor that creates a row vector from a std vector of double.
*/
-vpRowVector::vpRowVector (const std::vector<double> &v)
- : vpArray2D<double>(1, (unsigned int)v.size())
+vpRowVector::vpRowVector(const std::vector<double> &v) : vpArray2D<double>(1, (unsigned int)v.size())
{
- for(unsigned int j=0; j< v.size(); j++)
+ for (unsigned int j = 0; j < v.size(); j++)
(*this)[j] = v[j];
}
/*!
Constructor that creates a row vector from a std vector of float.
*/
-vpRowVector::vpRowVector (const std::vector<float> &v)
- : vpArray2D<double>(1, (unsigned int)v.size())
+vpRowVector::vpRowVector(const std::vector<float> &v) : vpArray2D<double>(1, (unsigned int)v.size())
{
- for(unsigned int j=0; j< v.size(); j++)
+ for (unsigned int j = 0; j < v.size(); j++)
(*this)[j] = (double)(v[j]);
}
@@ -503,22 +488,23 @@ vpRowVector::vpRowVector (const std::vector<float> &v)
Construct a row vector from a part of an input row vector \e v.
\param v : Input row vector used for initialization.
- \param c : column index in \e v that corresponds to the first element of the row vector to contruct.
- \param ncols : Number of columns of the constructed row vector.
+ \param c : column index in \e v that corresponds to the first element of the
+ row vector to contruct. \param ncols : Number of columns of the constructed
+ row vector.
The sub-vector starting from v[c] element and ending on v[c+ncols-1] element
is used to initialize the contructed row vector.
\sa init()
*/
-vpRowVector::vpRowVector (const vpRowVector &v, unsigned int c, unsigned int ncols)
- : vpArray2D<double>(1, ncols)
+vpRowVector::vpRowVector(const vpRowVector &v, unsigned int c, unsigned int ncols) : vpArray2D<double>(1, ncols)
{
init(v, c, ncols);
}
/*!
- Normalise the vector given as input parameter and return the normalized vector:
+ Normalise the vector given as input parameter and return the normalized
+ vector:
\f[
{\bf x} = \frac{{\bf x}}{\sqrt{\sum_{i=1}^{n}x^2_i}}
@@ -527,12 +513,11 @@ vpRowVector::vpRowVector (const vpRowVector &v, unsigned int c, unsigned int nco
*/
vpRowVector &vpRowVector::normalize(vpRowVector &x) const
{
- x = x/sqrt(x.sumSquare());
+ x = x / sqrt(x.sumSquare());
return x;
}
-
/*!
Normalise the vector modifying the vector as:
@@ -545,7 +530,7 @@ vpRowVector &vpRowVector::normalize()
{
double sum_square = sumSquare();
if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon()) {
- *this /= sqrt(sum_square) ;
+ *this /= sqrt(sum_square);
}
// If sum = 0, we have a nul vector. So we return just.
@@ -558,13 +543,14 @@ vpRowVector &vpRowVector::normalize()
\param ncols : number of columns of the matrix.
\return The resulting matrix.
- \exception vpException::dimensionError If the matrix and the row vector have not the same size.
+ \exception vpException::dimensionError If the matrix and the row vector have
+ not the same size.
\sa reshape(vpMatrix &, const unsigned int &, const unsigned int &)
*/
vpMatrix vpRowVector::reshape(const unsigned int &nrows, const unsigned int &ncols)
{
- vpMatrix M(nrows,ncols);
+ vpMatrix M(nrows, ncols);
reshape(M, nrows, ncols);
return M;
}
@@ -575,7 +561,8 @@ vpMatrix vpRowVector::reshape(const unsigned int &nrows, const unsigned int &nco
\param nrows : number of rows of the matrix.
\param ncols : number of columns of the matrix.
- \exception vpException::dimensionError If the matrix and the row vector have not the same size.
+ \exception vpException::dimensionError If the matrix and the row vector have
+not the same size.
The following example shows how to use this method.
\code
@@ -613,26 +600,25 @@ remat:
*/
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())) ;
+ if (dsize != nrows * ncols) {
+ throw(vpException(vpException::dimensionError, "Cannot reshape (1x%d) row vector in (%dx%d) matrix", colNum,
+ M.getRows(), M.getCols()));
}
try {
- if ((M.getRows() != nrows) || (M.getCols() != ncols)) M.resize(nrows,ncols);
- }
- catch(...) {
- throw ;
+ if ((M.getRows() != nrows) || (M.getCols() != ncols))
+ M.resize(nrows, ncols);
+ } catch (...) {
+ throw;
}
- for(unsigned int i =0; i< nrows; i++)
- for(unsigned int j =0; j< ncols; j++)
- M[i][j]=data[i*ncols+j];
+ for (unsigned int i = 0; i < nrows; i++)
+ for (unsigned int j = 0; j < ncols; j++)
+ M[i][j] = data[i * ncols + j];
}
/*!
Insert a row vector.
- \param i : Index of the first element to introduce. This index starts from 0.
- \param v : Row vector to insert.
+ \param i : Index of the first element to introduce. This index starts from
+0. \param v : Row vector to insert.
The following example shows how to use this function:
\code
@@ -662,12 +648,13 @@ v: 0 10 11 3
*/
void vpRowVector::insert(unsigned int i, const vpRowVector &v)
{
- if (i+v.size() > this->size())
+ if (i + v.size() > this->size())
throw(vpException(vpException::dimensionError,
- "Unable to insert (1x%d) row vector in (1x%d) row vector at position (%d)",
+ "Unable to insert (1x%d) row vector in (1x%d) row "
+ "vector at position (%d)",
v.getCols(), colNum, i));
- for (unsigned int j=0; j < v.size(); j++)
- (*this)[i+j] = v[j];
+ for (unsigned int j = 0; j < v.size(); j++)
+ (*this)[i + j] = v[j];
}
/*!
@@ -688,8 +675,8 @@ void vpRowVector::insert(unsigned int i, const vpRowVector &v)
*/
void vpRowVector::stack(const double &d)
{
- this->resize(colNum+1,false);
- (*this)[colNum-1] = d;
+ this->resize(colNum + 1, false);
+ (*this)[colNum - 1] = d;
}
/*!
@@ -711,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.
@@ -786,11 +770,11 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector
// General case
C.resize(nrA + nrB);
- for (unsigned int i=0; i<nrA; i++)
+ for (unsigned int i = 0; i < nrA; i++)
C[i] = A[i];
- for (unsigned int i=0; i<nrB; i++)
- C[nrA+i] = B[i];
+ for (unsigned int i = 0; i < nrB; i++)
+ C[nrA + i] = B[i];
}
/*!
@@ -798,9 +782,8 @@ void vpRowVector::stack(const vpRowVector &A, const vpRowVector &B, vpRowVector
*/
double vpRowVector::mean(const vpRowVector &v)
{
- if (v.data == NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute mean value of an empty row vector"));
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
}
double mean = 0;
@@ -814,18 +797,13 @@ double vpRowVector::mean(const vpRowVector &v)
/*!
Compute the median value of all the elements of the vector.
*/
-double
-vpRowVector::median(const vpRowVector &v)
+double vpRowVector::median(const vpRowVector &v)
{
- if (v.data==NULL) {
- throw(vpException(vpException::fatalError,
- "Cannot compute mean value of an empty row vector"));
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
}
- std::vector<double> vectorOfDoubles(v.size());
- for(unsigned int i = 0; i < v.size(); i++) {
- vectorOfDoubles[i] = v[i];
- }
+ std::vector<double> vectorOfDoubles(v.data, v.data + v.colNum);
return vpMath::getMedian(vectorOfDoubles);
}
@@ -833,23 +811,21 @@ 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) {
- throw(vpException(vpException::fatalError,
- "Cannot compute mean value of an empty row vector"));
+ if (v.data == NULL || v.size() == 0) {
+ throw(vpException(vpException::dimensionError, "Cannot compute mean value of an empty row vector"));
}
double mean_value = mean(v);
double sum_squared_diff = 0.0;
- for(unsigned int i = 0; i < v.size(); i++) {
- sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
+ for (unsigned int i = 0; i < v.size(); i++) {
+ sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
}
- double divisor = (double) v.size();
- if(useBesselCorrection && v.size() > 1) {
- divisor = divisor-1;
+ double divisor = (double)v.size();
+ if (useBesselCorrection && v.size() > 1) {
+ divisor = divisor - 1;
}
return std::sqrt(sum_squared_diff / divisor);
@@ -874,98 +850,97 @@ vpRowVector::stdev(const vpRowVector &v, const bool useBesselCorrection)
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &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;
unsigned int m = 1;
unsigned int n = getCols();
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int j=0;j<n;++j){
+ for (unsigned int j = 0; j < n; ++j) {
oss.str("");
oss << (*this)[j];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[j];
oss.str(ossFixed.str());
}
- values[j]=oss.str();
- size_type thislen=values[j].size();
- size_type p=values[j].find('.');
+ values[j] = oss.str();
+ size_type thislen = values[j].size();
+ size_type p = values[j].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
}
-
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- s <<" ";
- for (unsigned int j=0;j<n;j++){
- size_type p=values[j].find('.');
+ s << " ";
+ for (unsigned int j = 0; j < n; j++) {
+ size_type p = values[j].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[j].substr(0,p).c_str();
+ s << values[j].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[j].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[j].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
}
- s <<std::endl;
-
+ s << std::endl;
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
/*!
Allows to multiply a scalar by row vector.
*/
-vpRowVector operator*(const double &x,const vpRowVector &v)
+vpRowVector operator*(const double &x, const vpRowVector &v)
{
- vpRowVector vout ;
- vout = v*x ;
- return vout ;
+ vpRowVector vout;
+ vout = v * x;
+ return vout;
}
/*!
@@ -975,9 +950,9 @@ vpRowVector operator*(const double &x,const vpRowVector &v)
*/
double vpRowVector::sum() const
{
- double sum=0.0;
+ double sum = 0.0;
- for (unsigned int j=0;j<colNum;j++) {
+ for (unsigned int j = 0; j < colNum; j++) {
sum += rowPtrs[0][j];
}
@@ -985,17 +960,18 @@ double vpRowVector::sum() const
}
/*!
- Return the sum square of all the elements \f$v_{i}\f$ of the row vector v(n).
+ Return the sum square of all the elements \f$v_{i}\f$ of the row vector
+ v(n).
\return The sum square value: \f$\sum_{j=0}^{n} v_j^{2}\f$.
*/
double vpRowVector::sumSquare() const
{
- double sum_square=0.0;
+ double sum_square = 0.0;
- for (unsigned int j=0;j<colNum;j++) {
- double x=rowPtrs[0][j];
- sum_square += x*x;
+ for (unsigned int j = 0; j < colNum; j++) {
+ double x = rowPtrs[0][j];
+ sum_square += x * x;
}
return sum_square;
@@ -1008,9 +984,10 @@ double vpRowVector::sumSquare() const
*/
double vpRowVector::euclideanNorm() const
{
- double norm=0.0;
- for (unsigned int i=0;i<dsize;i++) {
- double x = *(data +i); norm += x*x;
+ double norm = 0.0;
+ for (unsigned int i = 0; i < dsize; i++) {
+ double x = *(data + i);
+ norm += x * x;
}
return sqrt(norm);
@@ -1020,8 +997,9 @@ double vpRowVector::euclideanNorm() const
Initialize the row vector from a part of an input row vector \e v.
\param v : Input row vector used for initialization.
- \param c : column index in \e v that corresponds to the first element of the row vector to contruct.
- \param ncols : Number of columns of the constructed row vector.
+ \param c : column index in \e v that corresponds to the first element of the
+row vector to contruct. \param ncols : Number of columns of the constructed
+row vector.
The sub-vector starting from v[c] element and ending on v[c+ncols-1] element
is used to initialize the contructed row vector.
@@ -1050,20 +1028,18 @@ 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 ;
+ unsigned int cncols = c + ncols;
if (cncols > v.getCols())
- throw(vpException(vpException::dimensionError,
- "Bad column dimension (%d > %d) used to initialize vpRowVector",
+ throw(vpException(vpException::dimensionError, "Bad column dimension (%d > %d) used to initialize vpRowVector",
cncols, v.getCols()));
resize(ncols);
if (this->rowPtrs == NULL) // Fix coverity scan: explicit null dereferenced
- return; // Noting to do
- for (unsigned int i=0 ; i < ncols; i++)
- (*this)[i] = v[i+c];
+ return; // Noting to do
+ for (unsigned int i = 0; i < ncols; i++)
+ (*this)[i] = v[i + c];
}
/*!
@@ -1096,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;
-
- for (unsigned int j=0; j < this ->getCols(); ++ j) {
- if (! octet) {
- 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 << "vpRowVector " << matrixName << " (" << this->getCols() << "); " << std::endl;
+
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ if (!octet) {
+ 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;
}
}
}
@@ -1144,11 +1114,11 @@ int main()
0, 1, 2
\endcode
*/
-std::ostream & vpRowVector::csvPrint(std::ostream & os) const
+std::ostream &vpRowVector::csvPrint(std::ostream &os) const
{
- for (unsigned int j=0; j < this->getCols(); ++ j) {
- os << (*this)[j];
- if (!(j==(this->getCols()-1)))
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[j];
+ if (!(j == (this->getCols() - 1)))
os << ", ";
}
os << std::endl;
@@ -1178,12 +1148,12 @@ r = ([
\endcode
that could be copy/paste in Maple.
*/
-std::ostream & vpRowVector::maplePrint(std::ostream & os) const
+std::ostream &vpRowVector::maplePrint(std::ostream &os) const
{
os << "([ " << std::endl;
os << "[";
- for (unsigned int j=0; j < this->getCols(); ++ j) {
- os << (*this)[j] << ", ";
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[j] << ", ";
}
os << "]," << std::endl;
os << "])" << std::endl;
@@ -1220,11 +1190,11 @@ r =
>>
\endcode
*/
-std::ostream & vpRowVector::matlabPrint(std::ostream & os) const
+std::ostream &vpRowVector::matlabPrint(std::ostream &os) const
{
os << "[ ";
- for (unsigned int j=0; j < this ->getCols(); ++ j) {
- os << (*this)[j] << ", ";
+ for (unsigned int j = 0; j < this->getCols(); ++j) {
+ os << (*this)[j] << ", ";
}
os << "]" << std::endl;
return os;
diff --git a/modules/core/src/math/matrix/vpSubColVector.cpp b/modules/core/src/math/matrix/vpSubColVector.cpp
index 62cd1bb..95fc28a 100644
--- a/modules/core/src/math/matrix/vpSubColVector.cpp
+++ b/modules/core/src/math/matrix/vpSubColVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,22 +38,19 @@
#include <stdlib.h>
-#include <visp3/core/vpSubColVector.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpSubColVector.h>
//! Default constructor that creates an empty vector.
-vpSubColVector::vpSubColVector()
- : vpColVector(), pRowNum(0), parent(NULL)
-{
-}
+vpSubColVector::vpSubColVector() : vpColVector(), pRowNum(0), parent(NULL) {}
/*!
Construct a sub-column vector from a parent column vector.
\param v : parent column vector.
- \param offset : offset where the sub-column vector starts in the parent column vector.
- \param nrows : size of the sub-column vector.
+ \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);
@@ -61,115 +59,113 @@ vpSubColVector::vpSubColVector(vpColVector &v, const unsigned int & offset, cons
/*!
Initialize a sub-column vector from a parent column vector.
\param v : parent column vector.
- \param offset : offset where the sub-column vector starts in the parent column vector.
- \param nrows : size of the sub-column vector.
+ \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 sub-column vector from an empty parent column vector")) ;
+ throw(vpException(vpException::fatalError, "Cannot initialize a "
+ "sub-column vector from an "
+ "empty parent column vector"));
}
-
- if(offset+nrows<=v.getRows()){
- data=v.data+offset;
-
- rowNum=nrows;
+
+ if (offset + nrows <= v.getRows()) {
+ data = v.data + offset;
+
+ rowNum = nrows;
colNum = 1;
-
- pRowNum=v.getRows();
- parent=&v;
-
- if(rowPtrs){
+
+ pRowNum = v.getRows();
+ parent = &v;
+
+ if (rowPtrs) {
free(rowPtrs);
}
-
- rowPtrs=(double**)malloc( parent->getRows() * sizeof(double*));
- for(unsigned int i=0;i<nrows;i++)
- rowPtrs[i]=v.data+i+offset;
-
- dsize = rowNum ;
+
+ rowPtrs = (double **)malloc(parent->getRows() * sizeof(double *));
+ for (unsigned int i = 0; i < nrows; i++)
+ rowPtrs[i] = v.data + i + offset;
+
+ dsize = rowNum;
} else {
- throw(vpException(vpException::dimensionError,
- "Cannot create a sub-column vector that is not completely containt in the parrent column vector")) ;
+ throw(vpException(vpException::dimensionError, "Cannot create a sub-column vector that is not "
+ "completely containt in the parrent column vector"));
}
}
//! Destructor that set the pointer to the parrent column vector to NULL.
-vpSubColVector::~vpSubColVector(){
- data=NULL ;
-}
+vpSubColVector::~vpSubColVector() { data = NULL; }
/*!
This method can be used to detect if the parent column vector
always exits or its size have not changed.
If this not the case an exception is thrown.
*/
-void vpSubColVector::checkParentStatus() const{
+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")) ;
+ if (pRowNum != parent->getRows()) {
+ throw(vpException(vpException::dimensionError, "The size of the parent sub-column vector has changed"));
}
}
/*!
- Allow to initialize a sub-column vector from an other one using operation A = B.
- Notice that the sub-column vector is not resized to the dimension of \e B.
+ Allow to initialize a sub-column vector from an other one using operation A
+ = B. Notice that the sub-column vector is not resized to the dimension of \e
+ B.
\param B : a sub-column vector.
*/
-vpSubColVector & vpSubColVector::operator=(const vpSubColVector &B)
+vpSubColVector &vpSubColVector::operator=(const vpSubColVector &B)
{
- if ( rowNum != B.getRows()) {
+ if (rowNum != B.getRows()) {
throw(vpException(vpException::dimensionError,
- "Cannot initialize (%dx1) sub-column vector from (%dx1) sub-column vector",
- rowNum, B.getRows())) ;
+ "Cannot initialize (%dx1) sub-column vector from "
+ "(%dx1) sub-column vector",
+ rowNum, B.getRows()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+ pRowNum = B.pRowNum;
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i];
return *this;
}
/*!
- Allow to initialize a sub-column vector from a column vector using operation A = B.
- Notice that the sub-column vector is not resized to the dimension of \e B.
- \param B : a column vector.
+ Allow to initialize a sub-column vector from a column vector using operation
+ A = B. Notice that the sub-column vector is not resized to the dimension of
+ \e B. \param B : a column vector.
*/
-vpSubColVector & vpSubColVector::operator=(const vpColVector &B)
+vpSubColVector &vpSubColVector::operator=(const vpColVector &B)
{
- if ( rowNum != B.getRows()) {
+ if (rowNum != B.getRows()) {
throw(vpException(vpException::dimensionError,
- "Cannot initialize (%dx1) sub-column vector from (%dx1) column vector",
- rowNum, B.getRows())) ;
+ "Cannot initialize (%dx1) sub-column vector from "
+ "(%dx1) column vector",
+ rowNum, B.getRows()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i];
-
+
return *this;
}
/*!
- Allow to initialize a sub-column vector from a m-by-1 matrix using operation A = B.
- Notice that the sub-column vector is not resized to the dimension of \e B.
- \param B : a matrix of size m-by-1.
+ Allow to initialize a sub-column vector from a m-by-1 matrix using operation
+ A = B. Notice that the sub-column vector is not resized to the dimension of
+ \e B. \param B : a matrix of size m-by-1.
*/
-vpSubColVector & vpSubColVector::operator=(const vpMatrix &B)
+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())) ;
+ if ((B.getCols() != 1) || (rowNum != B.getRows())) {
+ throw(vpException(vpException::dimensionError, "Cannot initialize (%dx1) sub-column vector from (%dx%d) matrix",
+ rowNum, B.getRows(), B.getCols()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i][1];
return *this;
}
@@ -178,9 +174,9 @@ vpSubColVector & vpSubColVector::operator=(const vpMatrix &B)
Set all the elements of the sub-column vector to \e x.
\param x : a scalar value.
*/
-vpSubColVector & vpSubColVector::operator=(const double &x)
+vpSubColVector &vpSubColVector::operator=(const double &x)
{
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = x;
return *this;
}
@@ -190,18 +186,16 @@ vpSubColVector & vpSubColVector::operator=(const double &x)
*/
vpSubColVector &vpSubColVector::operator=(const vpTranslationVector &tv)
{
- unsigned int k = tv.getRows() ;
- if (rowNum != k){
+ unsigned int k = tv.getRows();
+ if (rowNum != k) {
try {
resize(k);
- }
- catch(...)
- {
- throw ;
+ } catch (...) {
+ throw;
}
}
- memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ memcpy(data, tv.data, rowNum * sizeof(double));
return *this;
}
/*!
@@ -209,18 +203,16 @@ vpSubColVector &vpSubColVector::operator=(const vpTranslationVector &tv)
*/
vpSubColVector &vpSubColVector::operator=(const vpRotationVector &rv)
{
- unsigned int k = rv.getRows() ;
- if (rowNum != k){
+ unsigned int k = rv.getRows();
+ if (rowNum != k) {
try {
resize(k);
- }
- catch(...)
- {
- throw ;
+ } catch (...) {
+ throw;
}
}
- memcpy(data, rv.data, rowNum*sizeof(double)) ;
+ memcpy(data, rv.data, rowNum * sizeof(double));
return *this;
}
/*!
@@ -228,17 +220,15 @@ vpSubColVector &vpSubColVector::operator=(const vpRotationVector &rv)
*/
vpSubColVector &vpSubColVector::operator=(const vpPoseVector &p)
{
- unsigned int k = p.getRows() ;
- if (rowNum != k){
+ unsigned int k = p.getRows();
+ if (rowNum != k) {
try {
resize(k);
- }
- catch(...)
- {
- throw ;
+ } catch (...) {
+ throw;
}
}
- memcpy(data, p.data, rowNum*sizeof(double)) ;
+ memcpy(data, p.data, rowNum * sizeof(double));
return *this;
}
diff --git a/modules/core/src/math/matrix/vpSubMatrix.cpp b/modules/core/src/math/matrix/vpSubMatrix.cpp
index 17c0bdd..7ed25dd 100644
--- a/modules/core/src/math/matrix/vpSubMatrix.cpp
+++ b/modules/core/src/math/matrix/vpSubMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,16 +36,13 @@
*
*****************************************************************************/
-#include <visp3/core/vpSubMatrix.h>
+#include <stdlib.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpMatrixException.h>
-#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
+#include <visp3/core/vpSubMatrix.h>
-vpSubMatrix::vpSubMatrix()
- : pRowNum(0), pColNum(0), parent(NULL)
-{
-}
+vpSubMatrix::vpSubMatrix() : pRowNum(0), pColNum(0), parent(NULL) {}
/*!
\brief Constructor
@@ -55,10 +53,10 @@ vpSubMatrix::vpSubMatrix()
\param ncols : number of columns of the sub matrix
*/
vpSubMatrix::vpSubMatrix(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset,
- const unsigned int & nrows, const unsigned int & ncols)
+ const unsigned int &nrows, const unsigned int &ncols)
: pRowNum(0), pColNum(0), parent(NULL)
{
- init(m,row_offset,col_offset,nrows,ncols);
+ init(m, row_offset, col_offset, nrows, ncols);
}
/*!
@@ -69,50 +67,49 @@ vpSubMatrix::vpSubMatrix(vpMatrix &m, const unsigned int &row_offset, const unsi
\param nrows : number of rows of the sub matrix
\param ncols : number of columns of the sub matrix
*/
-void vpSubMatrix::init(vpMatrix &m, const unsigned int &row_offset, const unsigned int &col_offset , const unsigned int & nrows , const unsigned int & ncols){
-
- if(! m.data){
- vpERROR_TRACE("\n\t\t SubMatrix parent matrix is not allocated") ;
- throw(vpMatrixException(vpMatrixException::subMatrixError,
- "\n\t\t SubMatrix parent matrix is not allocated")) ;
- }
-
- if(row_offset+nrows <= m.getRows() && col_offset+ncols <= m.getCols()){
- data=m.data;
- parent =&m;
+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"));
+ }
+
+ if (row_offset + nrows <= m.getRows() && col_offset + ncols <= m.getCols()) {
+ data = m.data;
+ parent = &m;
rowNum = nrows;
colNum = ncols;
- pRowNum=m.getRows();
- pColNum=m.getCols();
-
- if(rowPtrs)
+ pRowNum = m.getRows();
+ pColNum = m.getCols();
+
+ if (rowPtrs)
free(rowPtrs);
-
- rowPtrs=(double**) malloc(nrows * sizeof(double*));
- for(unsigned int r=0;r<nrows;r++)
- rowPtrs[r]= m.data+col_offset+(r+row_offset)*pColNum;
-
- dsize = pRowNum*pColNum ;
- }else{
- vpERROR_TRACE("Submatrix cannot be contain in parent matrix") ;
- throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,"Submatrix cannot be contain in parent matrix")) ;
+
+ rowPtrs = (double **)malloc(nrows * sizeof(double *));
+ for (unsigned int r = 0; r < nrows; r++)
+ rowPtrs[r] = m.data + col_offset + (r + row_offset) * pColNum;
+
+ dsize = pRowNum * pColNum;
+ } else {
+ throw(
+ vpMatrixException(vpMatrixException::incorrectMatrixSizeError, "Submatrix cannot be contain in parent matrix"));
}
}
/*!
- \brief This method can be used to detect if the parent matrix
+ \brief This method can be used to detect if the parent matrix
always exits or its size have not changed and throw an exception is not
*/
-void vpSubMatrix::checkParentStatus() const {
- if(!data){
- vpERROR_TRACE("\n\t\t vpSubMatrix parent vpMatrix has been destroyed");
+void vpSubMatrix::checkParentStatus() const
+{
+ if (!data) {
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix parent vpMatrix has been destroyed")) ;
+ "vpSubMatrix parent vpMatrix has been destroyed"));
}
- if(pRowNum!=parent->getRows() || pColNum!=parent->getCols()){
- vpERROR_TRACE("\n\t\t vpSubMatrix size of parent vpMatrix has been changed");
+ if (pRowNum != parent->getRows() || pColNum != parent->getCols()) {
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix size of parent vpMatrix has been changed")) ;
+ "vpSubMatrix size of parent vpMatrix has been changed"));
}
}
@@ -120,59 +117,60 @@ void vpSubMatrix::checkParentStatus() const {
\brief Operation A = B
\param B : a matrix
*/
-vpSubMatrix & vpSubMatrix::operator=(const vpMatrix &B){
-
- if ((colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
+vpSubMatrix &vpSubMatrix::operator=(const vpMatrix &B)
+{
+
+ if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
+ "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix"));
}
-
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] = B[i][j];
-
- return *this;
+ }
+
+ return *this;
}
/*!
\brief Operation A = B
\param B : a subMatrix
*/
-vpSubMatrix & vpSubMatrix::operator=(const vpSubMatrix &B){
-
- if ((colNum != B.getCols())||(rowNum != B.getRows()))
- {
- vpERROR_TRACE("\n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix") ;
+vpSubMatrix &vpSubMatrix::operator=(const vpSubMatrix &B)
+{
+
+ if ((colNum != B.getCols()) || (rowNum != B.getRows())) {
throw(vpMatrixException(vpMatrixException::incorrectMatrixSizeError,
- "\n\t\t \n\t\t vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix")) ;
+ "vpSubMatrix mismatch in operator vpSubMatrix=vpMatrix"));
}
-
-
- double ** BrowPtrs=B.rowPtrs;
-
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+
+ pRowNum = B.pRowNum;
+ pColNum = B.pColNum;
+ parent = B.parent;
+
+ double **BrowPtrs = B.rowPtrs;
+
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] = BrowPtrs[i][j];
-
- return *this;
+ }
+
+ return *this;
}
/*!
\brief Operation A = x
\param x : a scalar
*/
-vpSubMatrix & vpSubMatrix::operator=(const double &x){
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
+vpSubMatrix &vpSubMatrix::operator=(const double &x)
+{
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < colNum; j++)
rowPtrs[i][j] = x;
-
- return *this;
-}
-
+ }
-vpSubMatrix::~vpSubMatrix()
-{
- data=NULL;
+ return *this;
}
+
+vpSubMatrix::~vpSubMatrix() { data = NULL; }
diff --git a/modules/core/src/math/matrix/vpSubRowVector.cpp b/modules/core/src/math/matrix/vpSubRowVector.cpp
index f753eb8..c095759 100644
--- a/modules/core/src/math/matrix/vpSubRowVector.cpp
+++ b/modules/core/src/math/matrix/vpSubRowVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,22 +38,19 @@
#include <stdlib.h>
-#include <visp3/core/vpSubRowVector.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpSubRowVector.h>
//! Default constructor that creates an empty vector.
-vpSubRowVector::vpSubRowVector()
- : vpRowVector(), pColNum(0), parent(NULL)
-{
-}
+vpSubRowVector::vpSubRowVector() : vpRowVector(), pColNum(0), parent(NULL) {}
/*!
Construct a sub-row vector from a parent row vector.
\param v : parent row vector.
- \param offset : offset where the sub-row vector starts in the parent row vector.
- \param ncols : size of the sub-row vector.
+ \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);
@@ -61,43 +59,42 @@ vpSubRowVector::vpSubRowVector(vpRowVector &v, const unsigned int & offset,const
/*!
Initialize a sub-row vector from a parent row vector.
\param v : parent row vector.
- \param offset : offset where the sub-row vector starts in the parent row vector.
- \param ncols : size of the sub-row vector.
+ \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 vector from an empty parent row vector")) ;
+ throw(vpException(vpException::fatalError, "Cannot initialize a sub-row "
+ "vector from an empty parent "
+ "row vector"));
}
-
- if(offset+ncols<=v.getCols()){
- data=v.data+offset;
-
- rowNum=1;
- colNum = ncols;
-
- pColNum=v.getCols();
- parent=&v;
-
- if(rowPtrs)
- free(rowPtrs);
-
- rowPtrs=(double**) malloc(1 * sizeof(double*));
- for(unsigned int i=0;i<1;i++)
- rowPtrs[i]=v.data+i+offset;
-
- dsize = colNum ;
+
+ if (offset + ncols <= v.getCols()) {
+ data = v.data + offset;
+
+ rowNum = 1;
+ colNum = ncols;
+
+ pColNum = v.getCols();
+ parent = &v;
+
+ if (rowPtrs)
+ free(rowPtrs);
+
+ rowPtrs = (double **)malloc(1 * sizeof(double *));
+ for (unsigned int i = 0; i < 1; i++)
+ rowPtrs[i] = v.data + i + offset;
+
+ dsize = colNum;
} else {
- throw(vpException(vpException::dimensionError,
- "Cannot create a sub-row vector that is not completely containt in the parrent row vector")) ;
+ throw(vpException(vpException::dimensionError, "Cannot create a sub-row vector that is not completely "
+ "containt in the parrent row vector"));
}
}
//! Destructor that set the pointer to the parrent row vector to NULL.
-vpSubRowVector::~vpSubRowVector(){
- data=NULL ;
-}
+vpSubRowVector::~vpSubRowVector() { data = NULL; }
/*!
This method can be used to detect if the parent row vector
@@ -106,54 +103,51 @@ vpSubRowVector::~vpSubRowVector(){
*/
void vpSubRowVector::checkParentStatus() const
{
- if(!data){
- throw(vpException(vpException::fatalError,
- "The parent of the current sub-row vector has been destroyed")) ;
+ if (!data) {
+ throw(vpException(vpException::fatalError, "The parent of the current sub-row vector has been destroyed"));
}
- if(pColNum!=parent->getCols()){
- throw(vpException(vpException::dimensionError,
- "The size of the parent sub-row vector has changed")) ;
+ if (pColNum != parent->getCols()) {
+ throw(vpException(vpException::dimensionError, "The size of the parent sub-row vector has changed"));
}
}
/*!
- Allow to initialize a sub-row vector from an other one using operation A = B.
- Notice that the sub-row vector is not resized to the dimension of \e B.
+ Allow to initialize a sub-row vector from an other one using operation A =
+ B. Notice that the sub-row vector is not resized to the dimension of \e B.
\param B : a sub-row vector.
*/
-vpSubRowVector & vpSubRowVector::operator=(const vpSubRowVector &B)
+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())) ;
+ if (colNum != B.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot initialize (1x%d) sub-row vector from (1x%d) sub-row vector",
+ colNum, B.getCols()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+ pColNum = B.pColNum;
+ parent = B.parent;
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i];
return *this;
}
/*!
- Allow to initialize a sub-row vector from a row vector using operation A = B.
- Notice that the sub-row vector is not resized to the dimension of \e B.
+ Allow to initialize a sub-row vector from a row vector using operation A =
+ B. Notice that the sub-row vector is not resized to the dimension of \e B.
\param B : a row vector.
*/
-vpSubRowVector & vpSubRowVector::operator=(const vpRowVector &B)
+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())) ;
+ if (colNum != B.getCols()) {
+ throw(vpException(vpException::dimensionError, "Cannot initialize (1x%d) sub-row vector from (1x%d) row vector",
+ colNum, B.getCols()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i];
-
- return *this;
+
+ return *this;
}
/*!
@@ -162,15 +156,14 @@ vpSubRowVector & vpSubRowVector::operator=(const vpRowVector &B)
\param B : a matrix of size 1-by-n.
*/
-vpSubRowVector & vpSubRowVector::operator=(const vpMatrix &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())) ;
+ if ((B.getRows() != 1) || (colNum != B.getCols())) {
+ throw(vpException(vpException::dimensionError, "Cannot initialize (1x%d) sub-column vector from (%dx%d) matrix",
+ colNum, B.getRows(), B.getCols()));
}
-
- for (unsigned int i=0;i<rowNum;i++)
+
+ for (unsigned int i = 0; i < rowNum; i++)
data[i] = B[i][1];
return *this;
}
@@ -178,9 +171,9 @@ vpSubRowVector & vpSubRowVector::operator=(const vpMatrix &B)
Set all the elements of the sub-row vector to \e x.
\param x : a scalar value.
*/
-vpSubRowVector & vpSubRowVector::operator=(const double &x)
+vpSubRowVector &vpSubRowVector::operator=(const double &x)
{
- for (unsigned int i=0;i<rowNum;i++)
- data[i] = x;
- return *this;
+ for (unsigned int i = 0; i < rowNum; i++)
+ data[i] = x;
+ return *this;
}
diff --git a/modules/core/src/math/misc/vpHinkley.cpp b/modules/core/src/math/misc/vpHinkley.cpp
index 8629251..415e0db 100644
--- a/modules/core/src/math/misc/vpHinkley.cpp
+++ b/modules/core/src/math/misc/vpHinkley.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,16 +45,16 @@
*/
-#include <visp3/core/vpHinkley.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHinkley.h>
//#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
+#include <cmath> // std::fabs
+#include <iostream>
+#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
/* VP_DEBUG_MODE fixed by configure:
1:
@@ -61,7 +62,6 @@
3: Print data
*/
-
/*!
Constructor.
@@ -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,7 +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)
{
}
@@ -112,8 +109,7 @@ vpHinkley::vpHinkley(double alpha_val, double delta_val)
\sa setAlpha(), setDelta()
*/
-void
-vpHinkley::init(double alpha_val, double delta_val)
+void vpHinkley::init(double alpha_val, double delta_val)
{
init();
@@ -126,9 +122,7 @@ vpHinkley::init(double alpha_val, double delta_val)
Destructor.
*/
-vpHinkley::~vpHinkley()
-{
-}
+vpHinkley::~vpHinkley() {}
/*!
@@ -139,7 +133,7 @@ vpHinkley::~vpHinkley()
void vpHinkley::init()
{
nsignal = 0;
- mean = 0.0;
+ mean = 0.0;
Sk = 0;
Mk = 0;
@@ -156,10 +150,7 @@ void vpHinkley::init()
\sa setAlpha()
*/
-void vpHinkley::setDelta(double delta)
-{
- dmin2 = delta / 2;
-}
+void vpHinkley::setDelta(double delta) { dmin2 = delta / 2; }
/*!
@@ -168,10 +159,7 @@ void vpHinkley::setDelta(double delta)
\sa setDelta()
*/
-void vpHinkley::setAlpha(double alpha_val)
-{
- this->alpha = alpha_val;
-}
+void vpHinkley::setAlpha(double alpha_val) { this->alpha = alpha_val; }
/*!
@@ -188,28 +176,28 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal)
vpHinkleyJumpType jump = noJump;
- nsignal ++; // Signal length
+ nsignal++; // Signal length
- if (nsignal == 1) mean = signal;
+ if (nsignal == 1)
+ mean = signal;
// Calcul des variables cumulees
computeSk(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)
jump = downwardJump;
#ifdef VP_DEBUG
- if (VP_DEBUG_MODE >=2) {
- switch(jump) {
+ if (VP_DEBUG_MODE >= 2) {
+ switch (jump) {
case noJump:
std::cout << "noJump " << std::endl;
- break;
+ break;
case downwardJump:
std::cout << "downWardJump " << std::endl;
break;
@@ -222,10 +210,12 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownwardJump(double signal)
computeMean(signal);
- if (jump == downwardJump) {
+ if (jump == downwardJump) {
vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
- Sk = 0; Mk = 0; nsignal = 0;
+ Sk = 0;
+ Mk = 0;
+ nsignal = 0;
}
return (jump);
@@ -246,17 +236,17 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
vpHinkleyJumpType jump = noJump;
- nsignal ++; // Signal length
+ nsignal++; // Signal length
- if (nsignal == 1) mean = signal;
+ if (nsignal == 1)
+ mean = signal;
// Calcul des variables cumulees
computeTk(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)
@@ -264,10 +254,10 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
#ifdef VP_DEBUG
if (VP_DEBUG_MODE >= 2) {
- switch(jump) {
+ switch (jump) {
case noJump:
std::cout << "noJump " << std::endl;
- break;
+ break;
case downwardJump:
std::cout << "downWardJump " << std::endl;
break;
@@ -279,10 +269,12 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testUpwardJump(double signal)
#endif
computeMean(signal);
- if (jump == upwardJump) {
+ if (jump == upwardJump) {
vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
- Tk = 0; Nk = 0; nsignal = 0;
+ Tk = 0;
+ Nk = 0;
+ nsignal = 0;
}
return (jump);
@@ -303,9 +295,10 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
vpHinkleyJumpType jump = noJump;
- nsignal ++; // Signal length
+ nsignal++; // Signal length
- if (nsignal == 1) mean = signal;
+ if (nsignal == 1)
+ mean = signal;
// Calcul des variables cumulees
computeSk(signal);
@@ -314,10 +307,8 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
computeMk();
computeNk();
- vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2
- << " signal: " << signal
- << " Sk: " << Sk << " Mk: " << Mk
- << " Tk: " << Tk << " Nk: " << Nk << std::endl;
+ vpCDEBUG(2) << "alpha: " << alpha << " dmin2: " << dmin2 << " signal: " << signal << " Sk: " << Sk << " Mk: " << Mk
+ << " Tk: " << Tk << " Nk: " << Nk << std::endl;
// teste si les variables cumulees excedent le seuil
if ((Mk - Sk) > alpha)
@@ -327,10 +318,10 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
#ifdef VP_DEBUG
if (VP_DEBUG_MODE >= 2) {
- switch(jump) {
+ switch (jump) {
case noJump:
std::cout << "noJump " << std::endl;
- break;
+ break;
case downwardJump:
std::cout << "downWardJump " << std::endl;
break;
@@ -346,7 +337,11 @@ vpHinkley::vpHinkleyJumpType vpHinkley::testDownUpwardJump(double signal)
if ((jump == upwardJump) || (jump == downwardJump)) {
vpCDEBUG(2) << "\n*** Reset the Hinkley test ***\n";
- Sk = 0; Mk = 0; Tk = 0; Nk = 0; nsignal = 0;
+ Sk = 0;
+ Mk = 0;
+ Tk = 0;
+ Nk = 0;
+ nsignal = 0;
// Debut modif FS le 03/09/2003
mean = signal;
// Fin modif FS le 03/09/2003
@@ -370,15 +365,13 @@ void vpHinkley::computeMean(double signal)
// apres un saut, la moyenne a tendance a "deriver". Pour reduire ces
// derives de la moyenne, elle n'est remise a jour avec la valeur
// courante du signal que si un debut de saut potentiel n'est pas detecte.
- //if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
- if ( ( std::fabs(Mk-Sk) <= std::fabs(vpMath::maximum(Mk,Sk))*std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(Tk-Nk) <= std::fabs(vpMath::maximum(Tk,Nk))*std::numeric_limits<double>::epsilon() ) )
- // Fin modif FS le 03/09/2003
+ // if ( ((Mk-Sk) == 0) && ((Tk-Nk) == 0) )
+ if ((std::fabs(Mk - Sk) <= std::fabs(vpMath::maximum(Mk, Sk)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(Tk - Nk) <= std::fabs(vpMath::maximum(Tk, Nk)) * std::numeric_limits<double>::epsilon()))
+ // Fin modif FS le 03/09/2003
- // Mise a jour de la moyenne.
+ // Mise a jour de la moyenne.
mean = (mean * (nsignal - 1) + signal) / (nsignal);
-
}
/*!
@@ -400,7 +393,8 @@ void vpHinkley::computeSk(double signal)
*/
void vpHinkley::computeMk()
{
- if (Sk > Mk) Mk = Sk;
+ if (Sk > Mk)
+ Mk = Sk;
}
/*!
@@ -421,26 +415,24 @@ void vpHinkley::computeTk(double signal)
*/
void vpHinkley::computeNk()
{
- if (Tk < Nk) Nk = Tk;
+ if (Tk < Nk)
+ Nk = Tk;
}
void vpHinkley::print(vpHinkley::vpHinkleyJumpType jump)
{
- switch(jump)
- {
- case noJump :
- std::cout << " No jump detected " << std::endl ;
- break ;
- case downwardJump :
- std::cout << " Jump downward detected " << std::endl ;
- break ;
- case upwardJump :
- std::cout << " Jump upward detected " << std::endl ;
- break ;
- default:
- std::cout << " Jump detected " << std::endl ;
- break ;
-
+ switch (jump) {
+ case noJump:
+ std::cout << " No jump detected " << std::endl;
+ break;
+ case downwardJump:
+ std::cout << " Jump downward detected " << std::endl;
+ break;
+ case upwardJump:
+ std::cout << " Jump upward detected " << std::endl;
+ break;
+ default:
+ std::cout << " Jump detected " << std::endl;
+ break;
}
}
-
diff --git a/modules/core/src/math/misc/vpMath.cpp b/modules/core/src/math/misc/vpMath.cpp
index 3c0e6f7..f4364c2 100644
--- a/modules/core/src/math/misc/vpMath.cpp
+++ b/modules/core/src/math/misc/vpMath.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,42 +42,40 @@
the C mathematics library (math.h)
*/
-#include <stdint.h>
-#include <numeric>
-#include <functional>
#include <cmath>
+#include <functional>
+#include <numeric>
+#include <stdint.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpMath.h>
#if defined(VISP_HAVE_FUNC__ISNAN)
-# include <float.h>
+#include <float.h>
#endif
-#if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
-# if defined _MSC_VER || defined __BORLANDC__
+#if !(defined(VISP_HAVE_FUNC_ISNAN) || defined(VISP_HAVE_FUNC_STD_ISNAN)) || \
+ !(defined(VISP_HAVE_FUNC_ISINF) || defined(VISP_HAVE_FUNC_STD_ISINF))
+#if defined _MSC_VER || defined __BORLANDC__
typedef __int64 int64;
typedef unsigned __int64 uint64;
-# else
+#else
typedef int64_t int64;
typedef uint64_t uint64;
-# endif
+#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-typedef union Cv64suf
-{
-// int64 i; //Unused variable, should be harmless to comment it
+typedef union Cv64suf {
+ // int64 i; //Unused variable, should be harmless to comment it
uint64 u;
double f;
-}
-Cv64suf;
+} Cv64suf;
#endif
#endif
const double vpMath::ang_min_sinc = 1.0e-8;
const double vpMath::ang_min_mc = 2.5e-4;
-
/*!
Check whether a double number is not a number (NaN) or not.
\param value : Double number to check.
@@ -91,23 +90,22 @@ bool vpMath::isNaN(const double value)
#elif defined(VISP_HAVE_FUNC__ISNAN)
return (_isnan(value) != 0);
#else
- #if 0
+#if 0
//This trick should work for any compiler which claims to use IEEE floating point.
//Do not work with g++ and -ffast-math option.
return (value != value);
- #else
- //Taken from OpenCV source code CvIsNan()
- Cv64suf ieee754;
- ieee754.f = value;
- return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) +
- ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
- #endif
+#else
+ // Taken from OpenCV source code CvIsNan()
+ Cv64suf ieee754;
+ ieee754.f = value;
+ return (((unsigned)(ieee754.u >> 32) & 0x7fffffff) + ((unsigned)ieee754.u != 0) > 0x7ff00000) != 0;
+#endif
#endif
}
/*!
- Returns whether a double is an infinity value (either positive infinity or negative infinity).
- \param value : Double number to check.
- \return Return true if value is infinity.
+ Returns whether a double is an infinity value (either positive infinity or
+ negative infinity). \param value : Double number to check. \return Return
+ true if value is infinity.
*/
bool vpMath::isInf(const double value)
{
@@ -118,11 +116,10 @@ bool vpMath::isInf(const double value)
#elif defined(VISP_HAVE_FUNC__FINITE)
return !_finite(value);
#else
- //Taken from OpenCV source code CvIsInf()
+ // 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
}
@@ -137,8 +134,10 @@ bool vpMath::isInf(const double value)
*/
double vpMath::mcosc(double cosx, double x)
{
- if (fabs(x) < ang_min_mc) return 0.5 ;
- else return ((1.0-cosx)/x/x) ;
+ if (fabs(x) < ang_min_mc)
+ return 0.5;
+ else
+ return ((1.0 - cosx) / x / x);
}
/*!
@@ -153,8 +152,10 @@ double vpMath::mcosc(double cosx, double x)
*/
double vpMath::msinc(double sinx, double x)
{
- if (fabs(x) < ang_min_mc) return (1./6.0) ;
- else return ((1.0-sinx/x)/x/x) ;
+ if (fabs(x) < ang_min_mc)
+ return (1. / 6.0);
+ else
+ return ((1.0 - sinx / x) / x / x);
}
/*!
@@ -168,8 +169,10 @@ double vpMath::msinc(double sinx, double x)
*/
double vpMath::sinc(double x)
{
- if (fabs(x) < ang_min_sinc) return 1.0 ;
- else return sin(x)/x ;
+ if (fabs(x) < ang_min_sinc)
+ return 1.0;
+ else
+ return sin(x) / x;
}
/*!
@@ -183,8 +186,10 @@ double vpMath::sinc(double x)
*/
double vpMath::sinc(double sinx, double x)
{
- if (fabs(x) < ang_min_sinc) return 1.0 ;
- else return (sinx/x) ;
+ if (fabs(x) < ang_min_sinc)
+ return 1.0;
+ else
+ return (sinx / x);
}
/*!
@@ -196,7 +201,7 @@ double vpMath::sinc(double sinx, double x)
*/
double vpMath::getMean(const std::vector<double> &v)
{
- if(v.empty()) {
+ if (v.empty()) {
throw vpException(vpException::notInitialized, "Empty vector !");
}
@@ -204,7 +209,7 @@ double vpMath::getMean(const std::vector<double> &v)
double sum = std::accumulate(v.begin(), v.end(), 0.0);
- return sum / (double) size;
+ return sum / (double)size;
}
/*!
@@ -214,48 +219,50 @@ double vpMath::getMean(const std::vector<double> &v)
\return The median value.
*/
-double vpMath::getMedian(const std::vector<double> &v) {
- if(v.empty()) {
+double vpMath::getMedian(const std::vector<double> &v)
+{
+ if (v.empty()) {
throw vpException(vpException::notInitialized, "Empty vector !");
}
- double median = 0.0;
std::vector<double> v_copy = v;
size_t size = v_copy.size();
- if(size % 2 == 0) {
- sort(v_copy.begin(), v_copy.end());
- median = (v_copy[size / 2 - 1] + v_copy[size / 2]) / 2.0;
+ size_t n = size / 2;
+ std::nth_element(v_copy.begin(), v_copy.begin() + n, v_copy.end());
+ double val_n = v_copy[n];
+
+ if (size % 2 == 1) {
+ return val_n;
} else {
- std::nth_element(v_copy.begin(), v_copy.begin() + (int) (size/2), v_copy.end());
- median = v_copy[size/2];
+ std::nth_element(v_copy.begin(), v_copy.begin() + n - 1, v_copy.end());
+ return 0.5 * (val_n + v_copy[n - 1]);
}
-
- return median;
}
/*!
Compute the standard deviation value for a vector of double.
\param v : Vector of double values.
- \param useBesselCorrection : If true, the Bessel correction is used (normalize by N-1).
+ \param useBesselCorrection : If true, the Bessel correction is used
+ (normalize by N-1).
\return The standard deviation value.
*/
-double vpMath::getStdev(const std::vector<double> &v, const bool useBesselCorrection) {
- if(v.empty()) {
+double vpMath::getStdev(const std::vector<double> &v, const bool useBesselCorrection)
+{
+ if (v.empty()) {
throw vpException(vpException::notInitialized, "Empty vector !");
}
double mean = getMean(v);
std::vector<double> diff(v.size());
- std::transform(v.begin(), v.end(), diff.begin(),
- std::bind2nd(std::minus<double>(), mean));
+ std::transform(v.begin(), v.end(), diff.begin(), std::bind2nd(std::minus<double>(), mean));
double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
- double divisor = (double) v.size();
- if(useBesselCorrection && v.size() > 1) {
- divisor = divisor-1;
+ double divisor = (double)v.size();
+ if (useBesselCorrection && v.size() > 1) {
+ divisor = divisor - 1;
}
return std::sqrt(sq_sum / divisor);
@@ -271,6 +278,4 @@ double vpMath::getStdev(const std::vector<double> &v, const bool useBesselCorrec
\return The modified modulo of a mod n.
*/
-int vpMath::modulo(const int a, const int n) {
- return ((a % n) + n) % n;
-}
+int vpMath::modulo(const int a, const int n) { return ((a % n) + n) % n; }
diff --git a/modules/core/src/math/random-generator/vpGaussRand.cpp b/modules/core/src/math/random-generator/vpGaussRand.cpp
index 1b9afcb..493d7f9 100644
--- a/modules/core/src/math/random-generator/vpGaussRand.cpp
+++ b/modules/core/src/math/random-generator/vpGaussRand.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,28 +46,27 @@
To adjust to some other distribution, multiply by the standard deviation and
add the mean. Box-Muller method
*/
-double
-vpGaussRand::gaussianDraw()
+double vpGaussRand::gaussianDraw()
{
static bool AlreadyDone = false;
static double x2;
if (AlreadyDone) {
- AlreadyDone=false;
+ AlreadyDone = false;
return x2;
}
else {
- double v1=0, v2=0, rsq=0;
+ double v1 = 0, v2 = 0, rsq = 0;
do {
- v1=2*draw1()-1;
- v2=2*draw1()-1;
- rsq=v1*v1+v2*v2;
+ v1 = 2 * draw1() - 1;
+ v2 = 2 * draw1() - 1;
+ rsq = v1 * v1 + v2 * v2;
} while (rsq >= 1);
- double fac=sqrt(-2*log(rsq)/rsq);
- x2=v2*fac;
- AlreadyDone=true;
- return v1*fac;
+ double fac = sqrt(-2 * log(rsq) / rsq);
+ x2 = v2 * fac;
+ AlreadyDone = true;
+ return v1 * fac;
}
}
diff --git a/modules/core/src/math/random-generator/vpUniRand.cpp b/modules/core/src/math/random-generator/vpUniRand.cpp
index 8453f8d..b42605f 100644
--- a/modules/core/src/math/random-generator/vpUniRand.cpp
+++ b/modules/core/src/math/random-generator/vpUniRand.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,16 +40,16 @@
#include <visp3/core/vpUniRand.h>
/*!
- Minimal random number generator of Park and Miller \cite Park:1988. Returns a
- uniform random deviate between 0.0 and 1.0.
+ Minimal random number generator of Park and Miller \cite Park:1988. Returns
+ a uniform random deviate between 0.0 and 1.0.
*/
-inline void
-vpUniRand::draw0()
+inline void vpUniRand::draw0()
{
- long k= x/q;//temp value for computing without overflow
- x = a*(x-k*q)-k*r;
- if (x < 0) x += m; //compute x without overflow
+ long k = x / q; // temp value for computing without overflow
+ x = a * (x - k * q) - k * r;
+ if (x < 0)
+ x += m; // compute x without overflow
}
/*!
@@ -58,40 +59,39 @@ vpUniRand::draw0()
shuffle. Returns a uniform random deviate between 0.0 and 1.0 (exclusive of
the endpoint values).
*/
-double
-vpUniRand::draw1()
+double vpUniRand::draw1()
{
- const long ntab = 33; //we work on a 32 elements array.
- //the 33rd one is actually the first value of y.
- const long modulo = ntab-2;
+ const long ntab = 33; // we work on a 32 elements array.
+ // the 33rd one is actually the first value of y.
+ const long modulo = ntab - 2;
static long y = 0;
static long T[ntab];
- long j; //index of T
+ long j; // index of T
- //step 0
- if (!y) { //first time
- for(j = 0; j < ntab; j++) {
+ // step 0
+ if (!y) { // first time
+ for (j = 0; j < ntab; j++) {
draw0();
- T[j]=x;
- } //compute table T
- y=T[ntab-1];
+ T[j] = x;
+ } // compute table T
+ y = T[ntab - 1];
}
- //step 1
- j = y & modulo; //compute modulo ntab+1 (the first element is the 0th)
+ // step 1
+ j = y & modulo; // compute modulo ntab+1 (the first element is the 0th)
- //step 3
- y=T[j];
- double ans = (double)y/normalizer;
+ // step 3
+ y = T[j];
+ double ans = (double)y / normalizer;
- //step 4
- //generate x(k+i) and set y=x(k+i)
+ // step 4
+ // generate x(k+i) and set y=x(k+i)
draw0();
- //refresh T[j];
- T[j]=x;
+ // refresh T[j];
+ T[j] = x;
return ans;
}
diff --git a/modules/core/src/math/robust/vpRobust.cpp b/modules/core/src/math/robust/vpRobust.cpp
index 36d4309..45def4d 100644
--- a/modules/core/src/math/robust/vpRobust.cpp
+++ b/modules/core/src/math/robust/vpRobust.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,25 +41,21 @@
\file vpRobust.cpp
*/
-
-
-
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpRobust.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <stdio.h>
-#include <string.h>
#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <string.h>
+#include <visp3/core/vpRobust.h>
#define vpITMAX 100
#define vpEPS 3.0e-7
#define vpCST 1
-
// ===================================================================
/*!
\brief Constructor.
@@ -70,8 +67,8 @@ vpRobust::vpRobust(unsigned int n_data)
{
vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
- normres.resize(n_data);
- sorted_normres.resize(n_data);
+ normres.resize(n_data);
+ sorted_normres.resize(n_data);
sorted_residues.resize(n_data);
// NoiseThreshold=0.0017; //Can not be more accurate than 1 pixel
}
@@ -87,15 +84,12 @@ vpRobust::vpRobust()
/*!
Copy constructor.
*/
-vpRobust::vpRobust(const vpRobust &other)
-{
- *this = other;
-}
+vpRobust::vpRobust(const vpRobust &other) { *this = other; }
/*!
Copy operator.
*/
-vpRobust & vpRobust::operator=(const vpRobust &other)
+vpRobust &vpRobust::operator=(const vpRobust &other)
{
normres = other.normres;
sorted_normres = other.sorted_normres;
@@ -112,7 +106,7 @@ vpRobust & vpRobust::operator=(const vpRobust &other)
/*!
Move operator.
*/
-vpRobust & vpRobust::operator=(const vpRobust &&other)
+vpRobust &vpRobust::operator=(const vpRobust &&other)
{
normres = std::move(other.normres);
sorted_normres = std::move(other.sorted_normres);
@@ -131,15 +125,15 @@ vpRobust & vpRobust::operator=(const vpRobust &&other)
\param n_data : size of input data vector.
*/
-void vpRobust::resize(unsigned int n_data){
+void vpRobust::resize(unsigned int n_data)
+{
- if(n_data!=size){
- normres.resize(n_data);
- sorted_normres.resize(n_data);
- sorted_residues.resize(n_data);
- size=n_data;
+ if (n_data != size) {
+ normres.resize(n_data);
+ sorted_normres.resize(n_data);
+ sorted_residues.resize(n_data);
+ size = n_data;
}
-
}
// ===================================================================
@@ -157,12 +151,12 @@ void vpRobust::resize(unsigned int n_data){
- TUKEY : \f$ \rho(r_i, C) = \left\{
\begin{array}{ll}
- \frac{r_i^6}{6} - \frac{C^2r_i^4}{2} +\frac{C^4r_i^2}{2} & \mbox{if} |r_i| < C \\
- \frac{1}{6} C^6 & \mbox{else} \end{array}
- \right.
- \f$
- with influence function \f$ \psi(r_i, C) = \left\{
- \begin{array}{ll} r_i(r_i^2-C^2)^2 & \mbox{if} |r_i| < C \\ 0 & \mbox{else} \end{array} \right. \f$ where \f$C=4.7 \hat{\sigma} \f$ and with \f$ \hat{\sigma} = 1.48{Med}_i(|r_i - {Med}_j(r_j)|) \f$
+ \frac{r_i^6}{6} - \frac{C^2r_i^4}{2} +\frac{C^4r_i^2}{2} & \mbox{if} |r_i| <
+ C \\ \frac{1}{6} C^6 & \mbox{else} \end{array} \right. \f$ with influence
+ function \f$ \psi(r_i, C) = \left\{ \begin{array}{ll} r_i(r_i^2-C^2)^2 &
+ \mbox{if} |r_i| < C \\ 0 & \mbox{else} \end{array} \right. \f$ where
+ \f$C=4.7 \hat{\sigma} \f$ and with \f$ \hat{\sigma} = 1.48{Med}_i(|r_i -
+ {Med}_j(r_j)|) \f$
- CAUCHY :
@@ -179,203 +173,158 @@ 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
- double normmedian=0; // Normalized median
- double sigma=0;// Standard Deviation
+ double med = 0; // median
+ double normmedian = 0; // Normalized median
+ double sigma = 0; // Standard Deviation
// resize vector only if the size of residue vector has changed
unsigned int n_data = residues.getRows();
- resize(n_data);
-
+ resize(n_data);
+
sorted_residues = residues;
-
- unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
+
+ unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
// Calculate median
- med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
- //residualMedian = med ;
+ med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
+ // residualMedian = med ;
// Normalize residues
- for(unsigned int i=0; i<n_data; i++)
- {
- normres[i] = (fabs(residues[i]- med));
- sorted_normres[i] = (fabs(sorted_residues[i]- med));
-
+ for (unsigned int i = 0; i < n_data; i++) {
+ normres[i] = (fabs(residues[i] - med));
+ sorted_normres[i] = (fabs(sorted_residues[i] - med));
}
// Calculate MAD
- normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
- //normalizedResidualMedian = normmedian ;
+ 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
+ sigma = 1.4826 * normmedian; // median Absolute Deviation
// Set a minimum threshold for sigma
// (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
}
- switch (method)
- {
- case TUKEY :
- {
- psiTukey(sigma, normres,weights);
-
- vpCDEBUG(2) << "Tukey's function computed" << std::endl;
- break ;
+ switch (method) {
+ case TUKEY: {
+ psiTukey(sigma, normres, weights);
- }
- case CAUCHY :
- {
- psiCauchy(sigma, normres,weights);
- break ;
- }
- /* case MCLURE :
- {
- psiMcLure(sigma, normres);
- break ;
- }*/
- case HUBER :
- {
- psiHuber(sigma, normres,weights);
- break ;
- }
+ vpCDEBUG(2) << "Tukey's function computed" << std::endl;
+ break;
+ }
+ case CAUCHY: {
+ psiCauchy(sigma, normres, weights);
+ break;
+ }
+ case HUBER: {
+ psiHuber(sigma, normres, weights);
+ break;
+ }
}
}
-
-
-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
- double sigma=0;// Standard Deviation
+ double normmedian = 0; // Normalized median
+ double sigma = 0; // Standard Deviation
unsigned int n_all_data = all_residues.getRows();
- vpColVector all_normres(n_all_data);
-
- // compute median with the residues vector, return all_normres which are the normalized all_residues vector.
- normmedian = computeNormalizedMedian(all_normres,residues,all_residues,weights);
+ vpColVector all_normres(n_all_data);
+ // compute median with the residues vector, return all_normres which are the
+ // normalized all_residues vector.
+ normmedian = computeNormalizedMedian(all_normres, residues, all_residues, weights);
// 1.48 keeps scale estimate consistent for a normal probability dist.
- sigma = 1.4826*normmedian; // Median Absolute Deviation
+ sigma = 1.4826 * normmedian; // Median Absolute Deviation
// Set a minimum threshold for sigma
// (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
}
+ switch (method) {
+ case TUKEY: {
+ psiTukey(sigma, all_normres, weights);
- switch (method)
- {
- case TUKEY :
- {
- psiTukey(sigma, all_normres,weights);
-
- vpCDEBUG(2) << "Tukey's function computed" << std::endl;
- break ;
-
- }
- case CAUCHY :
- {
- psiCauchy(sigma, all_normres,weights);
- break ;
- }
- /* case MCLURE :
- {
- psiMcLure(sigma, all_normres);
- break ;
- }*/
- case HUBER :
- {
- psiHuber(sigma, all_normres,weights);
- break ;
- }
-
-
+ vpCDEBUG(2) << "Tukey's function computed" << std::endl;
+ break;
+ }
+ case CAUCHY: {
+ psiCauchy(sigma, all_normres, weights);
+ break;
+ }
+ case HUBER: {
+ psiHuber(sigma, all_normres, weights);
+ break;
+ }
};
}
-
-
-double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
- const vpColVector &residues,
- const vpColVector &all_residues,
- const vpColVector & weights
- )
+double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues,
+ const vpColVector &all_residues, const vpColVector &weights)
{
- double med=0;
- double normmedian=0;
+ double med = 0;
+ double normmedian = 0;
unsigned int n_all_data = all_residues.getRows();
unsigned int n_data = residues.getRows();
-
+
// resize vector only if the size of residue vector has changed
resize(n_data);
-
+
sorted_residues = residues;
vpColVector no_null_weight_residues;
no_null_weight_residues.resize(n_data);
-
- //all_normres.resize(n_all_data); // Normalized Residue
- //vpColVector sorted_normres(n_data); // Normalized Residue
- //vpColVector sorted_residues = residues;
- //vpColVector sorted_residues;
-
-
- unsigned int index =0;
- for(unsigned int j=0;j<n_data;j++)
- {
- //if(weights[j]!=0)
- if(std::fabs(weights[j]) > std::numeric_limits<double>::epsilon())
- {
- no_null_weight_residues[index]=residues[j];
+
+ // all_normres.resize(n_all_data); // Normalized Residue
+ // vpColVector sorted_normres(n_data); // Normalized Residue
+ // vpColVector sorted_residues = residues;
+ // vpColVector sorted_residues;
+
+ unsigned int index = 0;
+ for (unsigned int j = 0; j < n_data; j++) {
+ // if(weights[j]!=0)
+ if (std::fabs(weights[j]) > std::numeric_limits<double>::epsilon()) {
+ no_null_weight_residues[index] = residues[j];
index++;
}
}
sorted_residues.resize(index);
- memcpy(sorted_residues.data,no_null_weight_residues.data,index*sizeof(double));
- n_data=index;
+ 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*/);
+
+ unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
+ med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
unsigned int i;
// Normalize residues
- for(i=0; i<n_all_data; i++)
- {
- all_normres[i] = (fabs(all_residues[i]- med));
+ for (i = 0; i < n_all_data; i++) {
+ all_normres[i] = (fabs(all_residues[i] - med));
}
- for(i=0; i<n_data; i++)
- {
- sorted_normres[i] = (fabs(sorted_residues[i]- med));
+ for (i = 0; i < n_data; i++) {
+ sorted_normres[i] = (fabs(sorted_residues[i] - med));
}
// MAD calculated only on first iteration
- //normmedian = Median(normres, weights);
- //normmedian = Median(normres);
- normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+ // normmedian = Median(normres, weights);
+ // normmedian = Median(normres);
+ normmedian = select(sorted_normres, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
return normmedian;
}
@@ -389,145 +338,70 @@ double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
* \return Returns a Column Vector of weights associated to each residue
*/
// ===================================================================
-vpColVector
-vpRobust::simultMEstimator(vpColVector &residues)
+vpColVector vpRobust::simultMEstimator(vpColVector &residues)
{
-
- double med=0; // Median
- double sigma=0; // Standard Deviation
+
+ double med = 0; // Median
+ double sigma = 0; // Standard Deviation
unsigned int n_data = residues.getRows();
vpColVector norm_res(n_data); // Normalized Residue
vpColVector w(n_data);
-
- vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
- << std::endl;
+
+ vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data << std::endl;
// Calculate Median
- unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
- med = select(residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
+ unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
+ med = select(residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
// Normalize residues
- for(unsigned int i=0; i<n_data; i++)
- norm_res[i] = (fabs(residues[i]- med));
+ for (unsigned int i = 0; i < n_data; i++)
+ norm_res[i] = (fabs(residues[i] - med));
// Check for various methods.
// For Huber compute Simultaneous scale estimate
// For Others use MAD calculated on first iteration
- if(it==0)
- {
- double normmedian = select(norm_res, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/); // Normalized Median
+ if (it == 0) {
+ 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
- {
+ sigma = 1.4826 * normmedian; // Median Absolute Deviation
+ } else {
// compute simultaneous scale estimate
sigma = simultscale(residues);
}
// Set a minimum threshold for sigma
// (when sigma reaches the level of noise in the image)
- if(sigma < NoiseThreshold)
- {
- sigma= NoiseThreshold;
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
}
vpCDEBUG(2) << "MAD and C computed" << std::endl;
- psiHuber(sigma, norm_res,w);
+ psiHuber(sigma, norm_res, w);
sig_prev = sigma;
return w;
}
-double
-vpRobust::scale(vpRobustEstimatorType method, vpColVector &x)
+double vpRobust::simultscale(vpColVector &x)
{
- unsigned int p = 6; //Number of parameters to be estimated.
+ unsigned int p = 6; // Number of parameters to be estimated.
unsigned int n = x.getRows();
- double sigma2=0;
- /* long */ double Expectation=0;
- /* long */ double Sum_chi=0;
-
- for(unsigned int i=0; i<n; i++)
- {
- double chiTmp = constrainedChi(method, x[i]);
-#if defined(VISP_HAVE_FUNC_STD_ERFC)
- Expectation += chiTmp*std::erfc(chiTmp);
-#elif defined(VISP_HAVE_FUNC_ERFC)
- Expectation += chiTmp*erfc(chiTmp);
-#else
- Expectation += chiTmp*(1-erf(chiTmp));
-#endif
-
- Sum_chi += chiTmp;
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
-#if defined(VISP_HAVE_FUNC_STD_ERFC)
- std::cout << "erf = " << std::erfc(chiTmp) << std::endl;
-#elif defined(VISP_HAVE_FUNC_ERFC)
- std::cout << "erf = " << erfc(chiTmp) << std::endl;
-#else
- std::cout << "erf = " << (1-erf(chiTmp)) << std::endl;
-#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;
-#if defined(VISP_HAVE_FUNC_STD_ERFC)
- 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;
-#endif
- //getchar();
- }
-#endif
-#endif
- }
+ double sigma2 = 0;
+ /* long */ double Expectation = 0;
+ /* long */ double Sum_chi = 0;
-
- sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
-
-#ifdef VP_DEBUG
-#if VP_DEBUG_MODE == 3
- {
- std::cout << "Expectation = " << Expectation << std::endl;
- std::cout << "Sum chi = " << Sum_chi << std::endl;
- std::cout << "sig_prev" << sig_prev << std::endl;
- std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
- }
-#endif
-#endif
-
- return sqrt(fabs(sigma2));
-
-}
-
-
-double
-vpRobust::simultscale(vpColVector &x)
-{
- unsigned int p = 6; //Number of parameters to be estimated.
- unsigned int n = x.getRows();
- double sigma2=0;
- /* long */ double Expectation=0;
- /* long */ double Sum_chi=0;
-
- for(unsigned int i=0; i<n; i++)
- {
+ for (unsigned int i = 0; i < n; i++) {
double chiTmp = simult_chi_huber(x[i]);
#if defined(VISP_HAVE_FUNC_STD_ERFC)
- Expectation += chiTmp*std::erfc(chiTmp);
+ Expectation += chiTmp * std::erfc(chiTmp);
#elif defined(VISP_HAVE_FUNC_ERFC)
- Expectation += chiTmp*erfc(chiTmp);
+ Expectation += chiTmp * erfc(chiTmp);
#else
- Expectation += chiTmp*(1-erf(chiTmp));
+ Expectation += chiTmp * (1 - erf(chiTmp));
#endif
Sum_chi += chiTmp;
@@ -539,26 +413,25 @@ vpRobust::simultscale(vpColVector &x)
#elif defined(VISP_HAVE_FUNC_ERFC)
std::cout << "erf = " << erfc(chiTmp) << std::endl;
#else
- std::cout << "erf = " << (1-erf(chiTmp)) << std::endl;
+ std::cout << "erf = " << (1 - erf(chiTmp)) << std::endl;
#endif
- std::cout << "x[i] = " << x[i] <<std::endl;
+ std::cout << "x[i] = " << x[i] << std::endl;
std::cout << "chi = " << chiTmp << std::endl;
- std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
+ std::cout << "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;
+ 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();
+ // getchar();
}
#endif
#endif
}
-
- sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
+ sigma2 = Sum_chi * vpMath::sqr(sig_prev) / ((n - p) * Expectation);
#ifdef VP_DEBUG
#if VP_DEBUG_MODE == 3
@@ -572,68 +445,60 @@ vpRobust::simultscale(vpColVector &x)
#endif
return sqrt(fabs(sigma2));
-
}
-double
-vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
+double vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
{
- switch (method)
- {
- case TUKEY :
+ switch (method) {
+ case TUKEY:
return constrainedChiTukey(x);
- case CAUCHY :
+ case CAUCHY:
return constrainedChiCauchy(x);
- case HUBER :
+ case HUBER:
return constrainedChiHuber(x);
};
return -1;
-
}
-
-double
-vpRobust::constrainedChiTukey(double x)
+double vpRobust::constrainedChiTukey(double x)
{
- double sct=0;
- double s=sig_prev;
- //double epsillon=0.5;
-
- if(fabs(x) <= 4.7*sig_prev)
- {
- 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));
- }
- else
- sct = -1/s;
+ double sct = 0;
+ double s = sig_prev;
+ // double epsillon=0.5;
+
+ if (fabs(x) <= 4.7 * sig_prev) {
+ 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));
+ } else
+ sct = -1 / s;
return sct;
}
-
-double
-vpRobust::constrainedChiCauchy(double x)
+double vpRobust::constrainedChiCauchy(double x)
{
double sct = 0;
- //double u = x/sig_prev;
+ // double u = x/sig_prev;
double s = sig_prev;
double b = 2.3849;
- sct = -1*(vpMath::sqr(x)*b)/(s*(vpMath::sqr(s*b)+vpMath::sqr(x)));
+ sct = -1 * (vpMath::sqr(x) * b) / (s * (vpMath::sqr(s * b) + vpMath::sqr(x)));
return sct;
}
-double
-vpRobust::constrainedChiHuber(double x)
+double vpRobust::constrainedChiHuber(double x)
{
- double sct=0;
- double u = x/sig_prev;
- double c = 1.2107; //1.345;
+ double sct = 0;
+ double u = x / sig_prev;
+ double c = 1.2107; // 1.345;
- if(fabs(u) <= c)
+ if (fabs(u) <= c)
sct = vpMath::sqr(u);
else
sct = vpMath::sqr(c);
@@ -641,21 +506,17 @@ vpRobust::constrainedChiHuber(double x)
return sct;
}
-double
-vpRobust::simult_chi_huber(double x)
+double vpRobust::simult_chi_huber(double x)
{
- double sct=0;
- double u = x/sig_prev;
- double c = 1.2107; //1.345;
+ double sct = 0;
+ double u = x / sig_prev;
+ double c = 1.2107; // 1.345;
- if(fabs(u) <= c)
- {
- //sct = 0.5*vpMath::sqr(u);
+ if (fabs(u) <= c) {
+ // sct = 0.5*vpMath::sqr(u);
sct = vpMath::sqr(u);
- }
- else
- {
- //sct = 0.5*vpMath::sqr(c);
+ } else {
+ // sct = 0.5*vpMath::sqr(c);
sct = vpMath::sqr(c);
}
@@ -670,32 +531,28 @@ vpRobust::simult_chi_huber(double x)
\param weights : weight vector
*/
-void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
+void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector &weights)
{
unsigned int n_data = x.getRows();
- double cst_const = vpCST*4.6851;
+ double cst_const = vpCST * 4.6851;
- for(unsigned int i=0; i<n_data; i++)
- {
- //if(sig==0 && weights[i]!=0)
- if(std::fabs(sig) <= std::numeric_limits<double>::epsilon() && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- weights[i]=1;
+ for (unsigned int i = 0; i < n_data; i++) {
+ // if(sig==0 && weights[i]!=0)
+ if (std::fabs(sig) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
+ weights[i] = 1;
continue;
}
- double xi_sig = x[i]/sig;
+ double xi_sig = x[i] / sig;
- //if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
- if((std::fabs(xi_sig)<=(cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- weights[i] = vpMath::sqr(1-vpMath::sqr(xi_sig/cst_const));
- //w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
- }
- else
- {
- //Outlier - could resize list of points tracked here?
+ // if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
+ if ((std::fabs(xi_sig) <= (cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
+ weights[i] = vpMath::sqr(1 - vpMath::sqr(xi_sig / cst_const));
+ // w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
+ } else {
+ // Outlier - could resize list of points tracked here?
weights[i] = 0;
}
}
@@ -710,19 +567,17 @@ void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
*/
void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
{
- double c = 1.2107; //1.345;
+ double c = 1.2107; // 1.345;
unsigned int n_data = x.getRows();
- for(unsigned int i=0; i<n_data; i++)
- {
- //if(weights[i]!=0)
- if(std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
- {
- double xi_sig = x[i]/sig;
- if(fabs(xi_sig)<=c)
- weights[i] = 1;
+ for (unsigned int i = 0; i < n_data; i++) {
+ // if(weights[i]!=0)
+ if (std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
+ double xi_sig = x[i] / sig;
+ if (fabs(xi_sig) <= c)
+ weights[i] = 1;
else
- weights[i] = c/fabs(xi_sig);
+ weights[i] = c / fabs(xi_sig);
}
}
}
@@ -738,45 +593,11 @@ void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
{
unsigned int n_data = x.getRows();
- double const_sig = 2.3849*sig;
-
- //Calculate Cauchy's equation
- for(unsigned int i=0; i<n_data; i++)
- {
- weights[i] = 1/(1+vpMath::sqr(x[i]/(const_sig)));
+ double const_sig = 2.3849 * sig;
- // If one coordinate is an outlier the other is too!
- // w[i] < 0.01 is a threshold to be set
- /*if(w[i] < 0.01)
- {
- if(i%2 == 0)
- {
- w[i+1] = w[i];
- i++;
- }
- else
- w[i-1] = w[i];
- }*/
- }
-}
-
-
-/*!
- \brief calculation of McLure's influence function
-
- \param sigma : sigma parameters
- \param x : normalized residue vector
- \param weights : weight vector
-*/
-void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
-{
- unsigned int n_data = r.getRows();
-
- //McLure's function
- for(unsigned int i=0; i<n_data; i++)
- {
- weights[i] = 1/(vpMath::sqr(1+vpMath::sqr(r[i]/sig)));
- //w[i] = 2*mad/vpMath::sqr((mad+r[i]*r[i]));//odobez
+ // Calculate Cauchy's equation
+ for (unsigned int i = 0; i < n_data; i++) {
+ weights[i] = 1 / (1 + vpMath::sqr(x[i] / (const_sig)));
// If one coordinate is an outlier the other is too!
// w[i] < 0.01 is a threshold to be set
@@ -793,25 +614,26 @@ void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
}
}
-
/*!
\brief partition function
\param a : vector to be sorted
\param l : first value to be considered
\param r : last value to be considered
*/
-int
-vpRobust::partition(vpColVector &a, int l, int r)
+int vpRobust::partition(vpColVector &a, int l, int r)
{
- int i = l-1;
+ int i = l - 1;
int j = r;
double v = a[(unsigned int)r];
- for (;;)
- {
- while (a[(unsigned int)++i] < v) ;
- while (v < a[(unsigned int)--j]) if (j == l) break;
- if (i >= j) break;
+ for (;;) {
+ while (a[(unsigned int)++i] < v)
+ ;
+ while (v < a[(unsigned int)--j])
+ if (j == l)
+ break;
+ if (i >= j)
+ break;
exch(a[(unsigned int)i], a[(unsigned int)j]);
}
exch(a[(unsigned int)i], a[(unsigned int)r]);
@@ -825,69 +647,54 @@ vpRobust::partition(vpColVector &a, int l, int r)
\param r : last value to be considered
\param k : value to be selected
*/
-double
-vpRobust::select(vpColVector &a, int l, int r, int k)
+double vpRobust::select(vpColVector &a, int l, int r, int k)
{
- while (r > l)
- {
+ while (r > l) {
int i = partition(a, l, r);
- if (i >= k) r = i-1;
- if (i <= k) l = i+1;
+ if (i >= k)
+ r = i - 1;
+ if (i <= k)
+ l = i + 1;
}
return a[(unsigned 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)
+double vpRobust::gammp(double a, double x)
{
- double gamser=0.,gammcf=0.,gln;
+ double gamser = 0., gammcf = 0., gln;
if (x < 0.0 || a <= 0.0)
std::cout << "Invalid arguments in routine GAMMP";
- if (x < (a+1.0))
- {
- gser(&gamser,a,x,&gln);
+ if (x < (a + 1.0)) {
+ gser(&gamser, a, x, &gln);
return gamser;
- }
- else
- {
- gcf(&gammcf,a,x,&gln);
- return 1.0-gammcf;
+ } else {
+ gcf(&gammcf, a, x, &gln);
+ return 1.0 - gammcf;
}
}
-void
-vpRobust::gser(double *gamser, double a, double x, double *gln)
+void vpRobust::gser(double *gamser, double a, double x, double *gln)
{
- *gln=gammln(a);
- if (x <= 0.0)
- {
+ *gln = gammln(a);
+ if (x <= 0.0) {
if (x < 0.0)
std::cout << "x less than 0 in routine GSER";
- *gamser=0.0;
+ *gamser = 0.0;
return;
- }
- else
- {
- double ap=a;
- double sum=1.0/a;
+ } else {
+ double ap = a;
+ double sum = 1.0 / a;
double del = sum;
- for (int n=1; n<=vpITMAX; n++)
- {
+ for (int n = 1; n <= vpITMAX; n++) {
ap += 1.0;
- del *= x/ap;
+ del *= x / ap;
sum += del;
- if (fabs(del) < fabs(sum)*vpEPS)
- {
- *gamser=sum*exp(-x+a*log(x)-(*gln));
+ if (fabs(del) < fabs(sum) * vpEPS) {
+ *gamser = sum * exp(-x + a * log(x) - (*gln));
return;
}
}
@@ -896,61 +703,52 @@ vpRobust::gser(double *gamser, double a, double x, double *gln)
}
}
-void
-vpRobust::gcf(double *gammcf, double a, double x, double *gln)
+void vpRobust::gcf(double *gammcf, double a, double x, double *gln)
{
- double gold=0.0,g,fac=1.0,b1=1.0;
- double b0=0.0,a1,a0=1.0;
-
- *gln=gammln(a);
- a1=x;
- for (int n=1; n<=vpITMAX; n++)
- {
- double an=(double) n;
- double ana=an-a;
- a0=(a1+a0*ana)*fac;
- b0=(b1+b0*ana)*fac;
- double anf=an*fac;
- a1=x*a0+anf*a1;
- b1=x*b0+anf*b1;
- //if (a1)
- if (std::fabs(a1) > std::numeric_limits<double>::epsilon())
- {
- fac=1.0/a1;
- g=b1*fac;
- if (fabs((g-gold)/g) < vpEPS)
- {
- *gammcf=exp(-x+a*log(x)-(*gln))*g;
+ double gold = 0.0, g, fac = 1.0, b1 = 1.0;
+ double b0 = 0.0, a1, a0 = 1.0;
+
+ *gln = gammln(a);
+ a1 = x;
+ for (int n = 1; n <= vpITMAX; n++) {
+ double an = (double)n;
+ double ana = an - a;
+ a0 = (a1 + a0 * ana) * fac;
+ b0 = (b1 + b0 * ana) * fac;
+ double anf = an * fac;
+ a1 = x * a0 + anf * a1;
+ b1 = x * b0 + anf * b1;
+ // if (a1)
+ if (std::fabs(a1) > std::numeric_limits<double>::epsilon()) {
+ fac = 1.0 / a1;
+ g = b1 * fac;
+ if (fabs((g - gold) / g) < vpEPS) {
+ *gammcf = exp(-x + a * log(x) - (*gln)) * g;
return;
}
- gold=g;
+ gold = g;
}
}
std::cout << "a too large, vpITMAX too small in routine GCF";
}
-double
-vpRobust::gammln(double xx)
+double vpRobust::gammln(double xx)
{
- double x,tmp,ser;
- static double cof[6]={76.18009173,-86.50532033,24.01409822,
- -1.231739516,0.120858003e-2,-0.536382e-5};
-
- x=xx-1.0;
- tmp=x+5.5;
- tmp -= (x+0.5)*log(tmp);
- ser=1.0;
- for (int j=0; j<=5; j++)
- {
+ double x, tmp, ser;
+ static double cof[6] = {76.18009173, -86.50532033, 24.01409822, -1.231739516, 0.120858003e-2, -0.536382e-5};
+
+ x = xx - 1.0;
+ tmp = x + 5.5;
+ tmp -= (x + 0.5) * log(tmp);
+ ser = 1.0;
+ for (int j = 0; j <= 5; j++) {
x += 1.0;
- ser += cof[j]/x;
+ ser += cof[j] / x;
}
- return -tmp+log(2.50662827465*ser);
+ return -tmp + log(2.50662827465 * ser);
}
#endif
-
-
// double
// vpRobust::median(const vpColVector &v)
// {
@@ -959,10 +757,11 @@ vpRobust::gammln(double xx)
// int n = v.getRows() ;
// vpColVector infsup(n) ;
// vpColVector eq(n) ;
-//
+//
// for (i=0;i<n;i++)
// {
-// // We compute the number of elements superior to the current value (sup)
+// // We compute the number of elements superior to the current value
+// (sup)
// // the number of elements inferior (inf) to the current value and
// // the number of elements equal to the current value (eq)
// inf = sup = 0;
@@ -976,30 +775,31 @@ vpRobust::gammln(double xx)
// }
// }
// // We compute then difference between inf and sup
-// // the median should be for |inf-sup| = 0 (1 if an even number of element)
+// // the median should be for |inf-sup| = 0 (1 if an even number of
+// element)
// // which means that there are the same number of element in the array
// // that are greater and smaller that this value.
// infsup[i] = abs(inf-sup);
// }
-//
+//
// // seek for the smaller value of |inf-sup| (should be 0 or 1)
// int imin = 0 ; // index of the median in the array
// //double eqmax = 0 ; // count of equal values
// // min cannot be greater than the number of element
// double min = n;
-//
+//
// // number of medians
// int mediancount = 0;
// // array of medians
// int *medianindex = new int[n];
-//
+//
// for (i=0; i<n; i++)
// {
// if(infsup[i] < min)
// {
// min = infsup[i];
// imin = i ;
-//
+//
// //reset count of median values
// mediancount=0;
// medianindex[mediancount]=i;
@@ -1010,7 +810,7 @@ vpRobust::gammln(double xx)
// medianindex[mediancount]=i;
// }
// }
-//
+//
// // Choose smalest data to be the median
// /*for(i=0; i<mediancount+1; i++)
// {
@@ -1025,12 +825,12 @@ vpRobust::gammln(double xx)
// //if(v[medianindex[i]] < v[imin])
// // imin = medianindex[i];
// }*/
-//
+//
// // return the median
// delete []medianindex;
// return(v[imin]);
// }
-//
+//
// // Calculate median only for the residues which have
// // not be rejected. i.e. weight=0
// double
@@ -1041,12 +841,13 @@ vpRobust::gammln(double xx)
// int n = v.getRows() ;
// vpColVector infsup(n) ;
// vpColVector eq(n) ;
-//
+//
// for (i=0;i<n;i++)
// {
// if(weights[i]!=0)
// {
-// // We compute the number of elements superior to the current value (sup)
+// // We compute the number of elements superior to the current value
+// (sup)
// // the number of elements inferior (inf) to the current value and
// // the number of elements equal to the current value (eq)
// inf = sup = 0;
@@ -1060,19 +861,20 @@ vpRobust::gammln(double xx)
// }
// }
// // We compute then difference between inf and sup
-// // the median should be for |inf-sup| = 0 (1 if an even number of element)
+// // the median should be for |inf-sup| = 0 (1 if an even number of
+// element)
// // which means that there are the same number of element in the array
// // that are greater and smaller that this value.
// infsup[i] = abs(inf-sup);
// }
// }
-//
+//
// // seek for the smaller value of |inf-sup| (should be 0 or 1)
// int imin = 0 ; // index of the median in the array
// //double eqmax = 0 ; // count of equal values
// // min cannot be greater than the number of element
// double min = n;
-//
+//
// for (i=0; i<n; i++)
// {
// if(weights[i]!=0)
@@ -1084,12 +886,11 @@ vpRobust::gammln(double xx)
// }
// }
// }
-//
+//
// // return the median
// return(v[imin]);
// }
-
#undef vpITMAX
#undef vpEPS
#undef vpCST
diff --git a/modules/core/src/math/robust/vpScale.cpp b/modules/core/src/math/robust/vpScale.cpp
index 43b79e3..3bb3a3c 100644
--- a/modules/core/src/math/robust/vpScale.cpp
+++ b/modules/core/src/math/robust/vpScale.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
+ * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density
+ *estimation.
*
* Authors:
* Andrew Comport
@@ -39,24 +41,17 @@
\file vpScale.cpp
*/
-
-
-
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpScale.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#define DEBUG_LEVEL2 0
-
-
-
//! Constructor
-vpScale::vpScale()
- : bandwidth(0.02), dimension(1)
+vpScale::vpScale() : bandwidth(0.02), dimension(1)
{
#if (DEBUG_LEVEL2)
std::cout << "vpScale constructor reached" << std::endl;
@@ -64,12 +59,10 @@ vpScale::vpScale()
#if (DEBUG_LEVEL2)
std::cout << "vpScale constructor finished" << std::endl;
#endif
-
}
//! Constructor
-vpScale::vpScale(double kernel_bandwidth, unsigned int dim)
- : bandwidth(kernel_bandwidth), dimension(dim)
+vpScale::vpScale(double kernel_bandwidth, unsigned int dim) : bandwidth(kernel_bandwidth), dimension(dim)
{
#if (DEBUG_LEVEL2)
@@ -81,175 +74,150 @@ vpScale::vpScale(double kernel_bandwidth, unsigned int dim)
}
//! Destructor
-vpScale::~vpScale()
-{
-}
+vpScale::~vpScale() {}
// Calculate the modes of the density for the distribution
// and their associated errors
-double
-vpScale::MeanShift(vpColVector &error)
+double vpScale::MeanShift(vpColVector &error)
{
- unsigned int n = error.getRows()/dimension;
+ unsigned int n = error.getRows() / dimension;
vpColVector density(n);
vpColVector density_gradient(n);
vpColVector mean_shift(n);
- unsigned int increment=1;
+ unsigned int increment = 1;
// choose smallest error as start point
- unsigned int i=0;
- while(error[i]<0 && error[i]<error[i+1])
+ unsigned int i = 0;
+ while (error[i] < 0 && error[i] < error[i + 1])
i++;
// Do mean shift until no shift
- while(increment >= 1 && i<n)
- {
- increment=0;
+ while (increment >= 1 && i < n) {
+ increment = 0;
density[i] = KernelDensity(error, i);
density_gradient[i] = KernelDensityGradient(error, i);
- mean_shift[i]=vpMath::sqr(bandwidth)*density_gradient[i]/((dimension+2)*density[i]);
+ mean_shift[i] = vpMath::sqr(bandwidth) * density_gradient[i] / ((dimension + 2) * density[i]);
double tmp_shift = mean_shift[i];
// Do mean shift
- while(tmp_shift>0 && tmp_shift>error[i]-error[i+1])
- {
+ while (tmp_shift > 0 && tmp_shift > error[i] - error[i + 1]) {
i++;
increment++;
- tmp_shift-=(error[i]-error[i-1]);
+ tmp_shift -= (error[i] - error[i - 1]);
}
}
return error[i];
-
}
// Calculate the density of each point in the error vector
// Requires ordered set of errors
-double
-vpScale::KernelDensity(vpColVector &error, unsigned int position)
+double vpScale::KernelDensity(vpColVector &error, unsigned int position)
{
- unsigned int n = error.getRows()/dimension;
- double density=0;
+ unsigned int n = error.getRows() / dimension;
+ double density = 0;
double Ke = 1;
- unsigned int j=position;
+ unsigned int j = position;
vpColVector X(dimension);
-
// Use each error in the bandwidth to calculate
// the local density of error i
// First treat larger errors
- //while(Ke !=0 && j<=n)
- while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j<=n)
- {
- //Create vector of errors corresponding to each dimension of a feature
- for(unsigned int i=0; i<dimension; i++)
- {
- X[i]=(error[position]-error[j])/bandwidth;
+ // while(Ke !=0 && j<=n)
+ while (std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j <= n) {
+ // Create vector of errors corresponding to each dimension of a feature
+ for (unsigned int i = 0; i < dimension; i++) {
+ X[i] = (error[position] - error[j]) / bandwidth;
position++;
j++;
}
- position-=dimension; // reset position
+ position -= dimension; // reset position
Ke = KernelDensity_EPANECHNIKOV(X);
- density+=Ke;
+ density += Ke;
}
Ke = 1;
- j=position;
+ j = position;
// Then treat smaller errors
- //while(Ke !=0 && j>=dimension)
- while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j>=dimension)
- {
- //Create vector of errors corresponding to each dimension of a feature
- for(unsigned int i=0; i<dimension; i++)
- {
- X[i]=(error[position]-error[j])/bandwidth;
+ // while(Ke !=0 && j>=dimension)
+ while (std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j >= dimension) {
+ // Create vector of errors corresponding to each dimension of a feature
+ for (unsigned int i = 0; i < dimension; i++) {
+ X[i] = (error[position] - error[j]) / bandwidth;
position++;
j--;
}
- position-=dimension; // reset position
+ position -= dimension; // reset position
Ke = KernelDensity_EPANECHNIKOV(X);
- density+=Ke;
+ density += Ke;
}
- density*=1/(n*bandwidth);
+ density *= 1 / (n * bandwidth);
return density;
-
}
-double
-vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
+double vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
{
- unsigned int n = error.getRows()/dimension;
- double density_gradient=0;
- double sum_delta=0;
- double delta=0;
- int nx=0;
+ unsigned int n = error.getRows() / dimension;
+ double density_gradient = 0;
+ double sum_delta = 0;
+ double delta = 0;
+ int nx = 0;
double inside_kernel = 1;
- unsigned int j=position;
+ unsigned int j = position;
// Use each error in the bandwidth to calculate
// the local density gradient
// First treat larger errors than current
- //while(inside_kernel !=0 && j<=n)
- while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j<=n)
- {
- delta = error[position]-error[j];
- if(vpMath::sqr(delta/bandwidth)<1)
- {
+ // while(inside_kernel !=0 && j<=n)
+ while (std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j <= n) {
+ delta = error[position] - error[j];
+ if (vpMath::sqr(delta / bandwidth) < 1) {
inside_kernel = 1;
- sum_delta+=error[j]-error[position];
+ sum_delta += error[j] - error[position];
j++;
nx++;
- }
- else
+ } else
inside_kernel = 0;
}
inside_kernel = 1;
- j=position;
+ j = position;
// Then treat smaller errors than current
- //while(inside_kernel !=0 && j>=dimension)
- while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j>=dimension)
- {
- delta = error[position]-error[j];
- if(vpMath::sqr(delta/bandwidth)<1)
- {
+ // while(inside_kernel !=0 && j>=dimension)
+ while (std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j >= dimension) {
+ delta = error[position] - error[j];
+ if (vpMath::sqr(delta / bandwidth) < 1) {
inside_kernel = 1;
- sum_delta+=error[j]-error[position];
+ sum_delta += error[j] - error[position];
j--;
nx++;
- }
- else
+ } else
inside_kernel = 0;
}
density_gradient = KernelDensityGradient_EPANECHNIKOV(sum_delta, n);
-
return density_gradient;
-
}
-
-//Epanechnikov_kernel for an d dimensional Euclidian space R^d
-double
-vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X)
+// Epanechnikov_kernel for an d dimensional Euclidian space R^d
+double vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X)
{
- double XtX= X*X;
- double c; // Volume of an n dimensional unit sphere
+ double XtX = X * X;
+ double c; // Volume of an n dimensional unit sphere
- switch (dimension)
- {
+ switch (dimension) {
case 1:
c = 2;
break;
@@ -257,30 +225,26 @@ vpScale::KernelDensity_EPANECHNIKOV(vpColVector &X)
c = M_PI;
break;
case 3:
- c = 4*M_PI/3;
+ c = 4 * M_PI / 3;
break;
default:
std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
exit(1);
}
- if(XtX < 1)
- return 1/(2*c)*(dimension+2)*(1-XtX);
+ if (XtX < 1)
+ return 1 / (2 * c) * (dimension + 2) * (1 - XtX);
else
return 0;
-
}
-
-//Epanechnikov_kernel for an d dimensional Euclidian space R^d
-double
-vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
+// Epanechnikov_kernel for an d dimensional Euclidian space R^d
+double vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
{
- double c; // Volume of an n dimensional unit sphere
+ double c; // Volume of an n dimensional unit sphere
- switch (dimension)
- {
+ switch (dimension) {
case 1:
c = 2;
break;
@@ -288,15 +252,14 @@ vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
c = M_PI;
break;
case 3:
- c = 4*M_PI/3;
+ c = 4 * M_PI / 3;
break;
default:
std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
exit(1);
}
- //return sumX*(dimension+2)/(n*pow(bandwidth, (double)dimension)*c*vpMath::sqr(bandwidth));
- return sumX*(dimension+2)/(n*bandwidth*c*vpMath::sqr(bandwidth));
-
+ // return sumX*(dimension+2)/(n*pow(bandwidth,
+ // (double)dimension)*c*vpMath::sqr(bandwidth));
+ return sumX * (dimension + 2) / (n * bandwidth * c * vpMath::sqr(bandwidth));
}
-
diff --git a/modules/core/src/math/spline/vpBSpline.cpp b/modules/core/src/math/spline/vpBSpline.cpp
index aafb749..61b32cc 100644
--- a/modules/core/src/math/spline/vpBSpline.cpp
+++ b/modules/core/src/math/spline/vpBSpline.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,7 @@
/*!
Basic constructor.
-
+
The degree \f$ p \f$ of the B-Spline basis functions is set to 3 to
compute cubic B-Spline.
*/
@@ -52,126 +53,123 @@ vpBSpline::vpBSpline()
/*!
Copy constructor.
-
+
*/
vpBSpline::vpBSpline(const vpBSpline &bspline)
- : controlPoints(), knots(), p(3), // By default : p=3 for clubic spline
- crossingPoints()
+ : controlPoints(bspline.controlPoints), knots(bspline.knots), p(bspline.p), // By default : p=3 for clubic spline
+ crossingPoints(bspline.crossingPoints)
{
- controlPoints = bspline.controlPoints;
- knots = bspline.knots;
- p = bspline.p;
- crossingPoints = bspline.crossingPoints;
}
/*!
Basic destructor.
*/
-vpBSpline::~vpBSpline()
-{
-}
+vpBSpline::~vpBSpline() {}
/*!
- Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$ l_u \in [u_i, u_{i+1}[ \f$
-
- Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\} \f$ with \f$ p \f$ is equal to 1.
+ Find the knot interval in which the parameter \f$ l_u \f$ lies. Indeed \f$
+ l_u \in [u_i, u_{i+1}[ \f$
+
+ Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\}
+ \f$ with \f$ p \f$ is equal to 1.
- For \f$ l_u \f$ equal to 0.5 the method will retun 1.
- - For \f$ l_u \f$ equal to 2.5 the method will retun 3.
+ - For \f$ l_u \f$ equal to 2.5 the method will retun 3.
- For \f$ l_u \f$ equal to 3 the method will retun 3.
-
+
\param l_u : The knot whose knot interval is seeked.
\param l_p : Degree of the B-Spline basis functions.
\param l_knots : The knot vector
-
+
\return the number of the knot interval in which \f$ l_u \f$ lies.
*/
-unsigned int
-vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
+unsigned int vpBSpline::findSpan(double l_u, unsigned int l_p, std::vector<double> &l_knots)
{
- unsigned int m = (unsigned int)l_knots.size()-1;
+ unsigned int m = (unsigned int)l_knots.size() - 1;
- if(l_u > l_knots.back())
- {
- //vpTRACE("l_u higher than the maximum value in the knot vector : %lf",l_u);
- return((unsigned int)(m-l_p-1));
+ if (l_u > l_knots.back()) {
+ // vpTRACE("l_u higher than the maximum value in the knot vector :
+ // %lf",l_u);
+ return ((unsigned int)(m - l_p - 1));
}
- //if (l_u == l_knots.back())
- if (std::fabs(l_u - l_knots.back()) <= std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
- return((unsigned int)(m-l_p-1));
+ // if (l_u == l_knots.back())
+ if (std::fabs(l_u - l_knots.back()) <=
+ std::fabs(vpMath::maximum(l_u, l_knots.back())) * std::numeric_limits<double>::epsilon())
+ return ((unsigned int)(m - l_p - 1));
double low = l_p;
- double high = m-l_p;
- double middle = (low+high)/2.0;
-
- while (l_u < l_knots[(unsigned int)vpMath::round(middle)] || l_u >= l_knots[(unsigned int)vpMath::round(middle+1)])
- {
- if(l_u < l_knots[(unsigned int)vpMath::round(middle)]) high = middle;
- else low = middle;
- middle = (low+high)/2.0;
+ double high = m - l_p;
+ double middle = (low + high) / 2.0;
+
+ while (l_u < l_knots[(unsigned int)vpMath::round(middle)] ||
+ l_u >= l_knots[(unsigned int)vpMath::round(middle + 1)]) {
+ if (l_u < l_knots[(unsigned int)vpMath::round(middle)])
+ high = middle;
+ else
+ low = middle;
+ middle = (low + high) / 2.0;
}
return (unsigned int)middle;
}
-
/*!
- Find the knot interval in which the parameter \f$ u \f$ lies. Indeed \f$ u \in [u_i, u_{i+1}[ \f$
-
- Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\} \f$ with \f$ p \f$ is equal to 1.
+ Find the knot interval in which the parameter \f$ u \f$ lies. Indeed \f$ u
+ \in [u_i, u_{i+1}[ \f$
+
+ Example : The knot vector is the following \f$ U = \{0, 0 , 1 , 2 ,3 , 3\}
+ \f$ with \f$ p \f$ is equal to 1.
- For \f$ u \f$ equal to 0.5 the method will retun 1.
- - For \f$ u \f$ equal to 2.5 the method will retun 3.
+ - For \f$ u \f$ equal to 2.5 the method will retun 3.
- For \f$ u \f$ equal to 3 the method will retun 3.
-
+
\param u : The knot whose knot interval is seeked.
-
+
\return the number of the knot interval in which \f$ u \f$ lies.
*/
-unsigned int
-vpBSpline::findSpan(double u)
-{
- return findSpan( u, p, knots);
-}
-
+unsigned int vpBSpline::findSpan(double u) { return findSpan(u, p, knots); }
/*!
- Compute the nonvanishing basis functions at \f$ l_u \f$ which is in the \f$ l_i \f$ th knot interval. All the basis functions are stored in an array such as :
-
- N = \f$ N_{l_i,0}(l_u) \f$, \f$ N_{l_i-1,1}(l_u) \f$, \f$ N_{l_i,1}(l_u) \f$, ... , \f$ N_{l_i-k,k}(l_u) \f$, ..., \f$ N_{l_i,k}(l_u) \f$, ... , \f$ N_{l_i-p,p}(l_u) \f$, ... , \f$ N_{l_i,p}(l_u) \f$
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the B-Spline basis functions.
- \param l_knots : The knot vector
-
- \return An array containing the nonvanishing basis functions at \f$ l_u \f$. The size of the array is \f$ l_p +1 \f$.
+ Compute the nonvanishing basis functions at \f$ l_u \f$ which is in the \f$
+ l_i \f$ th knot interval. All the basis functions are stored in an array
+ such as :
+
+ N = \f$ N_{l_i,0}(l_u) \f$, \f$ N_{l_i-1,1}(l_u) \f$, \f$ N_{l_i,1}(l_u)
+ \f$, ... , \f$ N_{l_i-k,k}(l_u) \f$, ..., \f$ N_{l_i,k}(l_u) \f$, ... , \f$
+ N_{l_i-p,p}(l_u) \f$, ... , \f$ N_{l_i,p}(l_u) \f$
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the B-Spline basis functions. \param l_knots :
+ The knot vector
+
+ \return An array containing the nonvanishing basis functions at \f$ l_u \f$.
+ The size of the array is \f$ l_p +1 \f$.
*/
-vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots)
+vpBasisFunction *vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsigned int l_p,
+ std::vector<double> &l_knots)
{
- vpBasisFunction* N = new vpBasisFunction[l_p+1];
+ vpBasisFunction *N = new vpBasisFunction[l_p + 1];
N[0].value = 1.0;
- double *left = new double[l_p+1];
- double *right = new double[l_p+1];
+ double *left = new double[l_p + 1];
+ double *right = new double[l_p + 1];
double temp = 0.0;
- for(unsigned int j = 1; j <= l_p; j++)
- {
- left[j] = l_u - l_knots[l_i+1-j];
- right[j] = l_knots[l_i+j] - l_u;
+ for (unsigned int j = 1; j <= l_p; j++) {
+ left[j] = l_u - l_knots[l_i + 1 - j];
+ right[j] = l_knots[l_i + j] - l_u;
double saved = 0.0;
- for (unsigned int r = 0; r < j; r++)
- {
- temp = N[r].value / (right[r+1]+left[j-r]);
- N[r].value = saved +right[r+1]*temp;
- saved = left[j-r]*temp;
+ for (unsigned int r = 0; r < j; r++) {
+ temp = N[r].value / (right[r + 1] + left[j - r]);
+ N[r].value = saved + right[r + 1] * temp;
+ saved = left[j - r] * temp;
}
N[j].value = saved;
}
- for(unsigned int j = 0; j < l_p+1; j++)
- {
- N[j].i = l_i-l_p+j;
+ for (unsigned int j = 0; j < l_p + 1; j++) {
+ N[j].i = l_i - l_p + j;
N[j].p = l_p;
N[j].u = l_u;
N[j].k = 0;
@@ -183,90 +181,95 @@ vpBasisFunction* vpBSpline::computeBasisFuns(double l_u, unsigned int l_i, unsig
return N;
}
-
/*!
- Compute the nonvanishing basis functions at \f$ u \f$. All the basis functions are stored in an array such as :
-
- N = \f$ N_{i,0}(u) \f$, \f$ N_{i-1,1}(u) \f$, \f$ N_{i,1}(u) \f$, ... , \f$ N_{i-k,k}(u) \f$, ..., \f$ N_{i,k}(u) \f$, ... , \f$ N_{i-p,p}(u) \f$, ... , \f$ N_{i,p}(u) \f$
-
+ Compute the nonvanishing basis functions at \f$ u \f$. All the basis
+ functions are stored in an array such as :
+
+ N = \f$ N_{i,0}(u) \f$, \f$ N_{i-1,1}(u) \f$, \f$ N_{i,1}(u) \f$, ... , \f$
+ N_{i-k,k}(u) \f$, ..., \f$ N_{i,k}(u) \f$, ... , \f$ N_{i-p,p}(u) \f$, ... ,
+ \f$ N_{i,p}(u) \f$
+
where i the number of the knot interval in which \f$ u \f$ lies.
-
+
\param u : A real number which is between the extrimities of the knot vector
-
- \return An array containing the nonvanishing basis functions at \f$ u \f$. The size of the array is \f$ p +1 \f$.
+
+ \return An array containing the nonvanishing basis functions at \f$ u \f$.
+ The size of the array is \f$ p +1 \f$.
*/
-vpBasisFunction* vpBSpline::computeBasisFuns(double u)
+vpBasisFunction *vpBSpline::computeBasisFuns(double u)
{
unsigned int i = findSpan(u);
- return computeBasisFuns(u, i, p ,knots);
+ return computeBasisFuns(u, i, p, knots);
}
-
/*!
- Compute the nonzero basis functions and their derivatives until the \f$ l_der \f$ th derivative. All the functions are computed at l_u.
-
+ Compute the nonzero basis functions and their derivatives until the \f$
+ l_der \f$ th derivative. All the functions are computed at l_u.
+
\warning \f$ l_der \f$ must be under or equal \f$ l_p \f$.
-
- The result is given as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
-
+
+ The result is given as an array of size l_der+1 x l_p+1. The kth line
+ corresponds to the kth basis functions derivatives.
+
The formula to compute the kth derivative at \f$ u \f$ is :
-
- \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the B-Spline basis functions.
- \param l_der : The last derivative to be computed.
- \param l_knots : The knot vector
-
- \return the basis functions and their derivatives as an array of size l_der+1 x l_p+1. The kth line corresponds to the kth basis functions derivatives.
-
- Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
+
+ \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} -
+ \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
+ p \f$ is the degree of the B-Spline basis function.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the B-Spline basis functions. \param l_der : The
+ last derivative to be computed. \param l_knots : The knot vector
+
+ \return the basis functions and their derivatives as an array of size
+ l_der+1 x l_p+1. The kth line corresponds to the kth basis functions
+ derivatives.
+
+ Example : return[0] is the list of the 0th derivatives ie the basis
+ functions. return[k] is the list of the kth derivatives.
*/
-vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots)
+vpBasisFunction **vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots)
{
- vpBasisFunction** N;
- N = new vpBasisFunction*[l_der+1];
- for(unsigned int j = 0; j <= l_der; j++)
- N[j] = new vpBasisFunction[l_p+1];
+ vpBasisFunction **N;
+ N = new vpBasisFunction *[l_der + 1];
+ for (unsigned int j = 0; j <= l_der; j++)
+ N[j] = new vpBasisFunction[l_p + 1];
- vpMatrix a(2,l_p+1);
- vpMatrix ndu(l_p+1,l_p+1);
+ vpMatrix a(2, l_p + 1);
+ vpMatrix ndu(l_p + 1, l_p + 1);
ndu[0][0] = 1.0;
- double *left = new double[l_p+1];
- double *right = new double[l_p+1];
+ double *left = new double[l_p + 1];
+ double *right = new double[l_p + 1];
double temp = 0.0;
- for(unsigned int j = 1; j <= l_p; j++)
- {
- left[j] = l_u - l_knots[l_i+1-j];
- right[j] = l_knots[l_i+j] - l_u;
+ for (unsigned int j = 1; j <= l_p; j++) {
+ left[j] = l_u - l_knots[l_i + 1 - j];
+ right[j] = l_knots[l_i + j] - l_u;
double saved = 0.0;
- for (unsigned int r = 0; r < j; r++)
- {
- ndu[j][r] = right[r+1]+left[j-r];
- temp = ndu[r][j-1]/ndu[j][r];
- ndu[r][j] = saved + right[r+1]*temp;
- saved = left[j-r]*temp;
+ for (unsigned int r = 0; r < j; r++) {
+ ndu[j][r] = right[r + 1] + left[j - r];
+ temp = ndu[r][j - 1] / ndu[j][r];
+ ndu[r][j] = saved + right[r + 1] * temp;
+ saved = left[j - r] * temp;
}
ndu[j][j] = saved;
}
- for(unsigned int j = 0; j <= l_p; j++)
- {
+ for (unsigned int j = 0; j <= l_p; j++) {
N[0][j].value = ndu[j][l_p];
- N[0][j].i = l_i-l_p+j;
+ N[0][j].i = l_i - l_p + j;
N[0][j].p = l_p;
N[0][j].u = l_u;
N[0][j].k = 0;
}
-
- if( l_der > l_p)
- {
+
+ if (l_der > l_p) {
vpTRACE("l_der must be under or equal to l_p");
l_der = l_p;
}
@@ -274,62 +277,56 @@ vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i,
double d;
int rk;
unsigned int pk;
- unsigned int j1,j2;
+ unsigned int j1, j2;
- for (unsigned int r = 0; r <= l_p; r++)
- {
+ for (unsigned int r = 0; r <= l_p; r++) {
unsigned int s1 = 0;
unsigned int s2 = 1;
a[0][0] = 1.0;
- for(unsigned int k = 1; k <= l_der; k++)
- {
+ for (unsigned int k = 1; k <= l_der; k++) {
d = 0.0;
- rk = (int)(r-k);
- pk = l_p-k;
- if(r >= k)
- {
- a[s2][0] = a[s1][0]/ndu[pk+1][rk];
- d = a[s2][0]*ndu[(unsigned int)rk][pk];
+ rk = (int)(r - k);
+ pk = l_p - k;
+ if (r >= k) {
+ a[s2][0] = a[s1][0] / ndu[pk + 1][rk];
+ d = a[s2][0] * ndu[(unsigned int)rk][pk];
}
- if(rk >= -1)
+ if (rk >= -1)
j1 = 1;
else
j1 = (unsigned int)(-rk);
- if(r-1 <= pk)
- j2 = k-1;
+ if (r - 1 <= pk)
+ j2 = k - 1;
else
- j2 = l_p-r;
+ j2 = l_p - r;
- for(unsigned int j =j1; j<= j2; j++)
- {
- a[s2][j] = (a[s1][j]-a[s1][j-1])/ndu[pk+1][(unsigned int)rk+j];
- d += a[s2][j]*ndu[(unsigned int)rk+j][pk];
+ for (unsigned int j = j1; j <= j2; j++) {
+ a[s2][j] = (a[s1][j] - a[s1][j - 1]) / ndu[pk + 1][(unsigned int)rk + j];
+ d += a[s2][j] * ndu[(unsigned int)rk + j][pk];
}
- if(r <= pk)
- {
- a[s2][k] = -a[s1][k-1]/ndu[pk+1][r];
- d += a[s2][k]*ndu[r][pk];
+ if (r <= pk) {
+ a[s2][k] = -a[s1][k - 1] / ndu[pk + 1][r];
+ d += a[s2][k] * ndu[r][pk];
}
N[k][r].value = d;
- N[k][r].i = l_i-l_p+r;
+ N[k][r].i = l_i - l_p + r;
N[k][r].p = l_p;
N[k][r].u = l_u;
N[k][r].k = k;
- s1 = (s1+1)%2;
- s2 = (s2+1)%2;
+ s1 = (s1 + 1) % 2;
+ s2 = (s2 + 1) % 2;
}
}
double r = l_p;
- for ( unsigned int k = 1; k <= l_der; k++ )
- {
+ for (unsigned int k = 1; k <= l_der; k++) {
for (unsigned int j = 0; j <= l_p; j++)
- N[k][j].value *= r;
- r *= (l_p-k);
+ N[k][j].value *= r;
+ r *= (l_p - k);
}
delete[] left;
@@ -338,56 +335,60 @@ vpBasisFunction** vpBSpline::computeDersBasisFuns(double l_u, unsigned int l_i,
return N;
}
-
/*!
- Compute the nonzero basis functions and their derivatives until the \f$ der \f$ th derivative. All the functions are computed at u.
-
+ Compute the nonzero basis functions and their derivatives until the \f$ der
+ \f$ th derivative. All the functions are computed at u.
+
\warning \f$ der \f$ must be under or equal \f$ p \f$.
-
- The result is given as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
-
+
+ The result is given as an array of size der+1 x p+1. The kth line
+ corresponds to the kth basis functions derivatives.
+
The formula to compute the kth derivative at \f$ u \f$ is :
-
- \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} - \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
+
+ \f[ N_{i,p}^{(k)}(u) =p \left( \frac{N_{i,p-1}^{(k-1)}}{u_{i+p}-u_i} -
+ \frac{N_{i+1,p-1}^{(k-1)}}{u_{i+p+1}-u_{i+1}} \right) \f]
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
+ p \f$ is the degree of the B-Spline basis function.
+
\param u : A real number which is between the extrimities of the knot vector
\param der : The last derivative to be computed.
-
- \return the basis functions and their derivatives as an array of size der+1 x p+1. The kth line corresponds to the kth basis functions derivatives.
-
- Example : return[0] is the list of the 0th derivatives ie the basis functions. return[k] is the list of the kth derivatives.
+
+ \return the basis functions and their derivatives as an array of size der+1
+ x p+1. The kth line corresponds to the kth basis functions derivatives.
+
+ Example : return[0] is the list of the 0th derivatives ie the basis
+ functions. return[k] is the list of the kth derivatives.
*/
-vpBasisFunction** vpBSpline::computeDersBasisFuns(double u, unsigned int der)
+vpBasisFunction **vpBSpline::computeDersBasisFuns(double u, unsigned int der)
{
unsigned int i = findSpan(u);
- return computeDersBasisFuns(u, i, p , der, knots);
+ return computeDersBasisFuns(u, i, p, der, knots);
}
-
/*!
- Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i) \f$ corresponding to the knot \f$ u \f$.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the B-Spline basis functions.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
-
+ Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)
+ \f$ corresponding to the knot \f$ u \f$.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the B-Spline basis functions. \param l_knots :
+ The knot vector \param l_controlPoints : the list of control points.
+
return the coordinates of a point corresponding to the knot \f$ u \f$.
*/
-vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
+vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints)
{
- vpBasisFunction* N = computeBasisFuns(l_u, l_i, l_p, l_knots);
+ vpBasisFunction *N = computeBasisFuns(l_u, l_i, l_p, l_knots);
vpImagePoint pt;
double ic = 0;
double jc = 0;
- for(unsigned int j = 0; j <= l_p; j++)
- {
- ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i();
- jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j();
+ for (unsigned int j = 0; j <= l_p; j++) {
+ ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i();
+ jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j();
}
pt.set_i(ic);
@@ -398,25 +399,24 @@ vpImagePoint vpBSpline::computeCurvePoint(double l_u, unsigned int l_i, unsigned
return pt;
}
-
/*!
- Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i) \f$ corresponding to the knot \f$ u \f$.
-
+ Compute the coordinates of a point \f$ C(u) = \sum_{i=0}^n (N_{i,p}(u)P_i)
+ \f$ corresponding to the knot \f$ u \f$.
+
\param u : A real number which is between the extrimities of the knot vector
-
+
return the coordinates of a point corresponding to the knot \f$ u \f$.
*/
vpImagePoint vpBSpline::computeCurvePoint(double u)
{
- vpBasisFunction* N = computeBasisFuns(u);
+ vpBasisFunction *N = computeBasisFuns(u);
vpImagePoint pt;
double ic = 0;
double jc = 0;
- for(unsigned int j = 0; j <= p; j++)
- {
- ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i();
- jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j();
+ for (unsigned int j = 0; j <= p; j++) {
+ ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i();
+ jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j();
}
pt.set_i(ic);
@@ -427,98 +427,95 @@ vpImagePoint vpBSpline::computeCurvePoint(double u)
return pt;
}
-
/*!
- Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} \f$.
-
+ Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der}
+ \f$.
+
The formula used is the following :
-
+
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)P_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the B-Spline basis functions.
- \param l_der : The last derivative to be computed.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
-
- \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , l_der \f$. The kth derivative is in the kth cell of the array.
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
+ p \f$ is the degree of the B-Spline basis function.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the B-Spline basis functions. \param l_der : The
+ last derivative to be computed. \param l_knots : The knot vector \param
+ l_controlPoints : the list of control points.
+
+ \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u)
+ \f$ for \f$ k = 0, ... , l_der \f$. The kth derivative is in the kth cell of
+ the array.
*/
-vpImagePoint* vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
+vpImagePoint *vpBSpline::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints)
{
- vpImagePoint *derivate = new vpImagePoint[l_der+1];
- vpBasisFunction** N;
+ vpImagePoint *derivate = new vpImagePoint[l_der + 1];
+ vpBasisFunction **N;
N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
unsigned int du;
- if (l_p < l_der)
- {
+ if (l_p < l_der) {
vpTRACE("l_der must be under or equal to l_p");
du = l_p;
- }
- else du = l_der;
-
- for(unsigned int k = 0; k <= du; k++)
- {
- derivate[k].set_ij(0.0,0.0);
- for(unsigned int j = 0; j<= l_p; j++)
- {
- derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i());
- derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j());
+ } else
+ du = l_der;
+
+ for (unsigned int k = 0; k <= du; k++) {
+ derivate[k].set_ij(0.0, 0.0);
+ for (unsigned int j = 0; j <= l_p; j++) {
+ derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i());
+ derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j());
}
}
-
- for(unsigned int j = 0; j <= l_der; j++)
+ for (unsigned int j = 0; j <= l_der; j++)
delete[] N[j];
delete[] N;
-
return derivate;
}
-
/*!
Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$.
-
+
The formula used is the following :
-
+
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)P_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$ p \f$ is the degree of the B-Spline basis function.
-
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies and \f$
+ p \f$ is the degree of the B-Spline basis function.
+
\param u : A real number which is between the extrimities of the knot vector
\param der : The last derivative to be computed.
-
- \return an array of size der+1 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of the array.
+
+ \return an array of size der+1 containing the coordinates \f$ C^{(k)}(u) \f$
+ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of the
+ array.
*/
-vpImagePoint* vpBSpline::computeCurveDers(double u, unsigned int der)
+vpImagePoint *vpBSpline::computeCurveDers(double u, unsigned int der)
{
- vpImagePoint *derivate = new vpImagePoint[der+1];
- vpBasisFunction** N;
+ vpImagePoint *derivate = new vpImagePoint[der + 1];
+ vpBasisFunction **N;
N = computeDersBasisFuns(u, der);
unsigned int du;
- if (p < der)
- {
+ if (p < der) {
vpTRACE("der must be under or equal to p");
du = p;
- }
- else du = der;
-
- for(unsigned int k = 0; k <= du; k++)
- {
- derivate[k].set_ij(0.0,0.0);
- for(unsigned int j = 0; j<= p; j++)
- {
- derivate[k].set_i( derivate[k].get_i() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i());
- derivate[k].set_j( derivate[k].get_j() + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j());
+ } else
+ du = der;
+
+ for (unsigned int k = 0; k <= du; k++) {
+ derivate[k].set_ij(0.0, 0.0);
+ for (unsigned int j = 0; j <= p; j++) {
+ derivate[k].set_i(derivate[k].get_i() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i());
+ derivate[k].set_j(derivate[k].get_j() + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j());
}
}
- for(unsigned int j = 0; j <= der; j++)
+ for (unsigned int j = 0; j <= der; j++)
delete[] N[j];
delete[] N;
diff --git a/modules/core/src/math/transformation/vpExponentialMap.cpp b/modules/core/src/math/transformation/vpExponentialMap.cpp
index de188e1..43c14ef 100644
--- a/modules/core/src/math/transformation/vpExponentialMap.cpp
+++ b/modules/core/src/math/transformation/vpExponentialMap.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,53 +39,49 @@
#include <visp3/core/vpExponentialMap.h>
-
/*!
- Compute the exponential map. The inverse function is inverse(). The sampling
- time is here set to 1 second. To use an other value you should use
+ Compute the exponential map. The inverse function is inverse(). The
+ sampling time is here set to 1 second. To use an other value you should use
direct(const vpColVector &, const double &).
\param v : Instantaneous velocity skew represented by a 6 dimension
- vector \f$ {\bf v} = [v, \omega] \f$ where \f$ v \f$ is a translation velocity vector
- and \f$ \omega \f$ is a rotation velocity vector.
+ vector \f$ {\bf v} = [v, \omega] \f$ where \f$ v \f$ is a translation
+ velocity vector and \f$ \omega \f$ is a rotation velocity vector.
\return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
- velocity \f$ \bf v \f$, where \f${\bf M} = \exp^{({\bf v})} \f$ is the displacement
- of the object when the velocity \f$ \bf v \f$ is applied during 1 second.
+ velocity \f$ \bf v \f$, where \f${\bf M} = \exp^{({\bf v})} \f$ is the
+ displacement of the object when the velocity \f$ \bf v \f$ is applied during
+ 1 second.
\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); }
/*!
Compute the exponential map. The inverse function is inverse().
\param v : Instantaneous velocity skew represented by a 6 dimension
- vector \f$ {\bf v} = [v, \omega] \f$ where \f$ v \f$ is a translation velocity vector
- and \f$ \omega \f$ is a rotation velocity vector.
+ vector \f$ {\bf v} = [v, \omega] \f$ where \f$ v \f$ is a translation
+ velocity vector and \f$ \omega \f$ is a rotation velocity vector.
\param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
velocity \f$ \bf v \f$ is applied.
\return An homogeneous matrix \f$ \bf M \f$ computed from an instantaneous
- velocity \f$ \bf v \f$, where \f${\bf M} = \exp^{({\bf v},\Delta t)} \f$ is the displacement
- of the object when the velocity \f$ \bf v \f$ is applied during \f$\Delta t\f$ seconds.
+ velocity \f$ \bf v \f$, where \f${\bf M} = \exp^{({\bf v},\Delta t)} \f$ is
+ the displacement of the object when the velocity \f$ \bf v \f$ is applied
+ during \f$\Delta t\f$ seconds.
\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 ;
- vpRotationMatrix rd ;
- vpTranslationVector dt ;
+ double theta, si, co, sinc, mcosc, msinc;
+ vpThetaUVector u;
+ vpRotationMatrix rd;
+ vpTranslationVector dt;
vpColVector v_dt = v * delta_t;
@@ -93,73 +90,68 @@ vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
u[2] = v_dt[5];
rd.buildFrom(u);
- theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
+ theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
si = sin(theta);
co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
- msinc = vpMath::msinc(si,theta);
+ sinc = vpMath::sinc(si, theta);
+ mcosc = vpMath::mcosc(co, theta);
+ msinc = vpMath::msinc(si, theta);
- dt[0] = v_dt[0]*(sinc + u[0]*u[0]*msinc)
- + v_dt[1]*(u[0]*u[1]*msinc - u[2]*mcosc)
- + v_dt[2]*(u[0]*u[2]*msinc + u[1]*mcosc);
+ dt[0] = v_dt[0] * (sinc + u[0] * u[0] * msinc) + v_dt[1] * (u[0] * u[1] * msinc - u[2] * mcosc) +
+ v_dt[2] * (u[0] * u[2] * msinc + u[1] * mcosc);
- dt[1] = v_dt[0]*(u[0]*u[1]*msinc + u[2]*mcosc)
- + v_dt[1]*(sinc + u[1]*u[1]*msinc)
- + v_dt[2]*(u[1]*u[2]*msinc - u[0]*mcosc);
+ dt[1] = v_dt[0] * (u[0] * u[1] * msinc + u[2] * mcosc) + v_dt[1] * (sinc + u[1] * u[1] * msinc) +
+ v_dt[2] * (u[1] * u[2] * msinc - u[0] * mcosc);
- dt[2] = v_dt[0]*(u[0]*u[2]*msinc - u[1]*mcosc)
- + v_dt[1]*(u[1]*u[2]*msinc + u[0]*mcosc)
- + v_dt[2]*(sinc + u[2]*u[2]*msinc);
+ dt[2] = v_dt[0] * (u[0] * u[2] * msinc - u[1] * mcosc) + v_dt[1] * (u[1] * u[2] * msinc + u[0] * mcosc) +
+ v_dt[2] * (sinc + u[2] * u[2] * msinc);
- vpHomogeneousMatrix Delta ;
- Delta.insert(rd) ;
- Delta.insert(dt) ;
+ vpHomogeneousMatrix Delta;
+ Delta.insert(rd);
+ Delta.insert(dt);
- if (0) // test new version wrt old version
+ if (0) // test new version wrt old version
{
// old version
- unsigned int i,j;
+ unsigned int i, j;
double s;
// double u[3];
// vpRotationMatrix rd ;
// vpTranslationVector dt ;
- s = sqrt(v_dt[3]*v_dt[3] + v_dt[4]*v_dt[4] + v_dt[5]*v_dt[5]);
- if (s > 1.e-15)
- {
- for (i=0;i<3;i++) u[i] = v_dt[3+i]/s;
+ s = sqrt(v_dt[3] * v_dt[3] + v_dt[4] * v_dt[4] + v_dt[5] * v_dt[5]);
+ if (s > 1.e-15) {
+ for (i = 0; i < 3; i++)
+ u[i] = v_dt[3 + i] / s;
double sinu = sin(s);
double cosi = cos(s);
- double mcosi = 1-cosi;
- rd[0][0] = cosi + mcosi*u[0]*u[0];
- rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
- rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
- rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
- rd[1][1] = cosi + mcosi*u[1]*u[1];
- rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
- rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
- rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
- rd[2][2] = cosi + mcosi*u[2]*u[2];
-
- dt[0] = v_dt[0]*(sinu/s + u[0]*u[0]*(1-sinu/s))
- + v_dt[1]*(u[0]*u[1]*(1-sinu/s)-u[2]*mcosi/s)
- + v_dt[2]*(u[0]*u[2]*(1-sinu/s)+u[1]*mcosi/s);
-
- dt[1] = v_dt[0]*(u[0]*u[1]*(1-sinu/s)+u[2]*mcosi/s)
- + v_dt[1]*(sinu/s + u[1]*u[1]*(1-sinu/s))
- + v_dt[2]*(u[1]*u[2]*(1-sinu/s)-u[0]*mcosi/s);
-
- dt[2] = v_dt[0]*(u[0]*u[2]*(1-sinu/s)-u[1]*mcosi/s)
- + v_dt[1]*(u[1]*u[2]*(1-sinu/s)+u[0]*mcosi/s)
- + v_dt[2]*(sinu/s + u[2]*u[2]*(1-sinu/s));
- }
- else
- {
- for (i=0;i<3;i++)
- {
- for(j=0;j<3;j++) rd[i][j] = 0.0;
+ double mcosi = 1 - cosi;
+ rd[0][0] = cosi + mcosi * u[0] * u[0];
+ rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
+ rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
+ rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
+ rd[1][1] = cosi + mcosi * u[1] * u[1];
+ rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
+ rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
+ rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
+ rd[2][2] = cosi + mcosi * u[2] * u[2];
+
+ dt[0] = v_dt[0] * (sinu / s + u[0] * u[0] * (1 - sinu / s)) +
+ v_dt[1] * (u[0] * u[1] * (1 - sinu / s) - u[2] * mcosi / s) +
+ v_dt[2] * (u[0] * u[2] * (1 - sinu / s) + u[1] * mcosi / s);
+
+ dt[1] = v_dt[0] * (u[0] * u[1] * (1 - sinu / s) + u[2] * mcosi / s) +
+ v_dt[1] * (sinu / s + u[1] * u[1] * (1 - sinu / s)) +
+ v_dt[2] * (u[1] * u[2] * (1 - sinu / s) - u[0] * mcosi / s);
+
+ dt[2] = v_dt[0] * (u[0] * u[2] * (1 - sinu / s) - u[1] * mcosi / s) +
+ v_dt[1] * (u[1] * u[2] * (1 - sinu / s) + u[0] * mcosi / s) +
+ v_dt[2] * (sinu / s + u[2] * u[2] * (1 - sinu / s));
+ } else {
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++)
+ rd[i][j] = 0.0;
rd[i][i] = 1.0;
dt[i] = v_dt[i];
}
@@ -167,18 +159,17 @@ vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
// end old version
// Test of the new version
- vpHomogeneousMatrix Delta_old ;
- Delta_old.insert(rd) ;
- Delta_old.insert(dt) ;
+ vpHomogeneousMatrix Delta_old;
+ Delta_old.insert(rd);
+ Delta_old.insert(dt);
int pb = 0;
- for (i=0;i<4;i++)
- {
- for(j=0;j<4;j++)
- if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5) pb = 1;
+ for (i = 0; i < 4; i++) {
+ for (j = 0; j < 4; j++)
+ if (fabs(Delta[i][j] - Delta_old[i][j]) > 1.e-5)
+ pb = 1;
}
- if (pb == 1)
- {
+ if (pb == 1) {
printf("pb vpHomogeneousMatrix::expMap\n");
std::cout << " Delta : " << std::endl << Delta << std::endl;
std::cout << " Delta_old : " << std::endl << Delta_old << std::endl;
@@ -186,117 +177,100 @@ vpExponentialMap::direct(const vpColVector &v, const double &delta_t)
// end of the test
}
- return Delta ;
+ return Delta;
}
/*!
- Computes an instantaneous velocity skew from an homogeneous matrix. The inverse
- function is the exponential map, see direct().
+ Computes an instantaneous velocity skew from an homogeneous matrix. The
+ inverse function is the exponential map, see direct().
- \param M : An homogeneous matrix corresponding to the displacement of an object
- during 1 second.
+ \param M : An homogeneous matrix corresponding to the displacement of an
+ object during 1 second.
- \return Instantaneous velocity skew \f$ \bf v \f$ represented by a 6 dimension
- vector \f$ [v, \omega] \f$ where \f$ v \f$ is a translation velocity vector
- and \f$ \omega \f$ is a rotation velocity vector.
+ \return Instantaneous velocity skew \f$ \bf v \f$ represented by a 6
+ dimension vector \f$ [v, \omega] \f$ where \f$ v \f$ is a translation
+ velocity vector and \f$ \omega \f$ is a rotation velocity vector.
\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); }
/*!
Compute an instantaneous velocity from an homogeneous matrix. The inverse
function is the exponential map, see direct().
- \param M : An homogeneous matrix corresponding to the displacement of an object
- during \f$\Delta t\f$ seconds.
+ \param M : An homogeneous matrix corresponding to the displacement of an
+ object during \f$\Delta t\f$ seconds.
\param delta_t : Sampling time \f$ \Delta t \f$. Time during which the
displacement is applied.
- \return Instantaneous velocity skew \f$ \bf v \f$ represented by a 6 dimension
- vector \f$ [v, \omega] \f$ where \f$ v \f$ is a translation velocity vector
- and \f$ \omega \f$ is a rotation velocity vector.
+ \return Instantaneous velocity skew \f$ \bf v \f$ represented by a 6
+ dimension vector \f$ [v, \omega] \f$ where \f$ v \f$ is a translation
+ velocity vector and \f$ \omega \f$ is a rotation velocity vector.
\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;
- double theta,si,co,sinc,mcosc,msinc,det;
- vpThetaUVector u ;
- vpRotationMatrix Rd,a;
- vpTranslationVector dt ;
+ double theta, si, co, sinc, mcosc, msinc, det;
+ vpThetaUVector u;
+ vpRotationMatrix Rd, a;
+ vpTranslationVector dt;
M.extract(Rd);
u.buildFrom(Rd);
- for (i=0;i<3;i++) v[3+i] = u[i];
+ for (i = 0; i < 3; i++)
+ v[3 + i] = u[i];
- theta = sqrt(u[0]*u[0] + u[1]*u[1] + u[2]*u[2]);
+ theta = sqrt(u[0] * u[0] + u[1] * u[1] + u[2] * u[2]);
si = sin(theta);
co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
- msinc = vpMath::msinc(si,theta);
+ sinc = vpMath::sinc(si, theta);
+ mcosc = vpMath::mcosc(co, theta);
+ msinc = vpMath::msinc(si, theta);
// a below is not a pure rotation matrix, even if not so far from
// the Rodrigues formula : sinc I + (1-sinc)/t^2 VV^T + (1-cos)/t^2 [V]_X
// with V = t.U
- a[0][0] = sinc + u[0]*u[0]*msinc;
- a[0][1] = u[0]*u[1]*msinc - u[2]*mcosc;
- a[0][2] = u[0]*u[2]*msinc + u[1]*mcosc;
-
- a[1][0] = u[0]*u[1]*msinc + u[2]*mcosc;
- a[1][1] = sinc + u[1]*u[1]*msinc;
- a[1][2] = u[1]*u[2]*msinc - u[0]*mcosc;
-
- a[2][0] = u[0]*u[2]*msinc - u[1]*mcosc;
- a[2][1] = u[1]*u[2]*msinc + u[0]*mcosc;
- a[2][2] = sinc + u[2]*u[2]*msinc;
-
- det = a[0][0]*a[1][1]*a[2][2] + a[1][0]*a[2][1]*a[0][2]
- + a[0][1]*a[1][2]*a[2][0] - a[2][0]*a[1][1]*a[0][2]
- - a[1][0]*a[0][1]*a[2][2] - a[0][0]*a[2][1]*a[1][2];
-
- if (fabs(det) > 1.e-5)
- {
- v[0] = (M[0][3]*a[1][1]*a[2][2]
- + M[1][3]*a[2][1]*a[0][2]
- + M[2][3]*a[0][1]*a[1][2]
- - M[2][3]*a[1][1]*a[0][2]
- - M[1][3]*a[0][1]*a[2][2]
- - M[0][3]*a[2][1]*a[1][2])/det;
- v[1] = (a[0][0]*M[1][3]*a[2][2]
- + a[1][0]*M[2][3]*a[0][2]
- + M[0][3]*a[1][2]*a[2][0]
- - a[2][0]*M[1][3]*a[0][2]
- - a[1][0]*M[0][3]*a[2][2]
- - a[0][0]*M[2][3]*a[1][2])/det;
- v[2] = (a[0][0]*a[1][1]*M[2][3]
- + a[1][0]*a[2][1]*M[0][3]
- + a[0][1]*M[1][3]*a[2][0]
- - a[2][0]*a[1][1]*M[0][3]
- - a[1][0]*a[0][1]*M[2][3]
- - a[0][0]*a[2][1]*M[1][3])/det;
- }
- else
- {
- v[0] = M[0][3];
- v[1] = M[1][3];
- v[2] = M[2][3];
+ a[0][0] = sinc + u[0] * u[0] * msinc;
+ a[0][1] = u[0] * u[1] * msinc - u[2] * mcosc;
+ a[0][2] = u[0] * u[2] * msinc + u[1] * mcosc;
+
+ a[1][0] = u[0] * u[1] * msinc + u[2] * mcosc;
+ a[1][1] = sinc + u[1] * u[1] * msinc;
+ a[1][2] = u[1] * u[2] * msinc - u[0] * mcosc;
+
+ a[2][0] = u[0] * u[2] * msinc - u[1] * mcosc;
+ a[2][1] = u[1] * u[2] * msinc + u[0] * mcosc;
+ a[2][2] = sinc + u[2] * u[2] * msinc;
+
+ det = a[0][0] * a[1][1] * a[2][2] + a[1][0] * a[2][1] * a[0][2] + a[0][1] * a[1][2] * a[2][0] -
+ a[2][0] * a[1][1] * a[0][2] - a[1][0] * a[0][1] * a[2][2] - a[0][0] * a[2][1] * a[1][2];
+
+ if (fabs(det) > 1.e-5) {
+ v[0] = (M[0][3] * a[1][1] * a[2][2] + M[1][3] * a[2][1] * a[0][2] + M[2][3] * a[0][1] * a[1][2] -
+ M[2][3] * a[1][1] * a[0][2] - M[1][3] * a[0][1] * a[2][2] - M[0][3] * a[2][1] * a[1][2]) /
+ det;
+ v[1] = (a[0][0] * M[1][3] * a[2][2] + a[1][0] * M[2][3] * a[0][2] + M[0][3] * a[1][2] * a[2][0] -
+ a[2][0] * M[1][3] * a[0][2] - a[1][0] * M[0][3] * a[2][2] - a[0][0] * M[2][3] * a[1][2]) /
+ det;
+ v[2] = (a[0][0] * a[1][1] * M[2][3] + a[1][0] * a[2][1] * M[0][3] + a[0][1] * M[1][3] * a[2][0] -
+ a[2][0] * a[1][1] * M[0][3] - a[1][0] * a[0][1] * M[2][3] - a[0][0] * a[2][1] * M[1][3]) /
+ det;
+ } else {
+ v[0] = M[0][3];
+ v[1] = M[1][3];
+ v[2] = M[2][3];
}
// Apply the sampling time to the computed velocity
v /= delta_t;
- return(v);
+ return (v);
}
diff --git a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp
index 6819509..47e37c7 100644
--- a/modules/core/src/math/transformation/vpForceTwistMatrix.cpp
+++ b/modules/core/src/math/transformation/vpForceTwistMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,7 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Twist transformation matrix that allows to transform forces from one
+ * Twist transformation matrix that allows to transform forces from one
* frame to an other.
*
* Authors:
@@ -36,13 +37,12 @@
*
*****************************************************************************/
-#include <sstream>
#include <assert.h>
+#include <sstream>
-#include <visp3/core/vpForceTwistMatrix.h>
-#include <visp3/core/vpException.h>
#include <visp3/core/vpDebug.h>
-
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpForceTwistMatrix.h>
/*!
\file vpForceTwistMatrix.cpp
@@ -52,17 +52,15 @@
transform a force/torque skew from one frame to an other.
*/
-
/*!
Copy operator.
\param M : Force/torque twist matrix to copy.
*/
-vpForceTwistMatrix &
-vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
+vpForceTwistMatrix &vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
{
- for (int i=0; i<6; i++) {
- for (int j=0; j<6; j++) {
+ for (int i = 0; i < 6; i++) {
+ for (int j = 0; j < 6; j++) {
rowPtrs[i][j] = M.rowPtrs[i][j];
}
}
@@ -70,17 +68,15 @@ vpForceTwistMatrix::operator=(const vpForceTwistMatrix &M)
return *this;
}
-
/*!
Initialize the force/torque 6 by 6 twist matrix to identity.
*/
-void
-vpForceTwistMatrix::eye()
+void vpForceTwistMatrix::eye()
{
- for (unsigned int i=0 ; i < 6 ; i++) {
- for (unsigned int j=0 ; j < 6; j++) {
- if (i==j)
- (*this)[i][j] = 1.0 ;
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ if (i == j)
+ (*this)[i][j] = 1.0;
else
(*this)[i][j] = 0.0;
}
@@ -90,11 +86,7 @@ vpForceTwistMatrix::eye()
/*!
Initialize a force/torque twist transformation matrix to identity.
*/
-vpForceTwistMatrix::vpForceTwistMatrix()
- : vpArray2D<double>(6,6)
-{
- eye() ;
-}
+vpForceTwistMatrix::vpForceTwistMatrix() : vpArray2D<double>(6, 6) { eye(); }
/*!
@@ -103,98 +95,166 @@ vpForceTwistMatrix::vpForceTwistMatrix()
\param F : Force/torque twist matrix used as initializer.
*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F)
- : vpArray2D<double>(6,6)
-{
- *this = F ;
-}
+vpForceTwistMatrix::vpForceTwistMatrix(const vpForceTwistMatrix &F) : vpArray2D<double>(6, 6) { *this = F; }
/*!
- Initialize a force/torque twist transformation matrix from an
- homogeneous matrix. Given the homogeneous transformation
-
- \f[ ^a{\bf M}_b = \left(\begin{array}{cc}
- ^a{\bf R}_b & ^a{\bf t}_b\\
- 0 & 1
- \end{array} \right) \f]
-
- the force/torque twist matrix is given by :
+ Initialize a force/torque twist transformation matrix from an homogeneous
+ matrix \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
+ \\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
+ \param M : Homogeneous matrix \f$\bf M\f$ used to initialize the twist
+ transformation matrix.
+ \param full : Boolean used to indicate which matrix should be filled.
+ - When set to true, use the complete force/torque skew transformation:
\f[
- ^a{\bf F}_b = \left[\begin{array}{cc}
- ^a{\bf R}_b & {\bf 0}_{3\times 3}\\
- {[^a{\bf t}_b]}_{\times} \; ^a{\bf R}_b & ^a{\bf R}_b
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+ - When set to false, use the block diagonal velocity skew transformation:
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
\end{array}
\right]
\f]
-
- \param M : Homogeneous matrix \f$^a{\bf M}_b\f$ used to initialize the twist
- transformation matrix.
*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M)
- : vpArray2D<double>(6,6)
+vpForceTwistMatrix::vpForceTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D<double>(6, 6)
{
- buildFrom(M);
+ if (full)
+ buildFrom(M);
+ else
+ buildFrom(M.getRotationMatrix());
}
/*!
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
+ Initialize a force/torque twist transformation matrix from a translation
+ vector \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
\param t : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
+
+ \param thetau : \f$\theta u\f$ rotation vector used to initialize \f$R\f$.
*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t,
- const vpThetaUVector &thetau)
- : vpArray2D<double>(6,6)
+vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau)
+ : vpArray2D<double>(6, 6)
{
- buildFrom(t, thetau) ;
+ buildFrom(t, thetau);
}
/*!
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation matrix R.
+ Initialize a force/torque block diagonal twist transformation matrix from a
+ rotation vector with \f$\theta u \f$ parametrization.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+
+ \param thetau : \f$\theta u\f$ rotation vector used to initialize \f$R\f$.
+
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpThetaUVector &thetau) : vpArray2D<double>(6, 6) { buildFrom(thetau); }
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation
+ vector \e t and a rotation matrix \e R.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
\param t : Translation vector.
-
+
\param R : Rotation matrix.
*/
-vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t,
- const vpRotationMatrix &R)
- : vpArray2D<double>(6,6)
+vpForceTwistMatrix::vpForceTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R)
+ : vpArray2D<double>(6, 6)
{
- buildFrom(t, R) ;
+ buildFrom(t, R);
}
/*!
- Initialize a force/torque twist transformation matrix from a translation vector
- \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
+ Initialize a force/torque block diagonal twist transformation matrix from a
+ rotation matrix \e R.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+
+ \param R : Rotation matrix.
+
+*/
+vpForceTwistMatrix::vpForceTwistMatrix(const vpRotationMatrix &R) : vpArray2D<double>(6, 6) { buildFrom(R); }
+
+/*!
+
+ Initialize a force/torque twist transformation matrix from a translation
+ vector \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta
+ {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
\param tx,ty,tz : Translation vector in meters.
- \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
+ \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, const double tuy, const double tuz)
- : vpArray2D<double>(6,6)
+vpForceTwistMatrix::vpForceTwistMatrix(const double tx, const double ty, const double tz, const double tux,
+ const double tuy, const double tuz)
+ : vpArray2D<double>(6, 6)
{
- vpTranslationVector T(tx,ty,tz) ;
- vpThetaUVector tu(tux,tuy,tuz) ;
- buildFrom(T,tu) ;
+ vpTranslationVector T(tx, ty, tz);
+ vpThetaUVector tu(tux, tuy, tuz);
+ buildFrom(T, tu);
}
/*!
- Operator that allows to multiply a skew transformation matrix by an
- other skew transformation matrix.
+ Operator that allows to multiply a force/torque twist transformation matrix
+by an other force/torque skew transformation matrix.
\code
#include <visp3/core/vpForceTwistMatrix.h>
@@ -209,46 +269,43 @@ int main()
\endcode
*/
-vpForceTwistMatrix
-vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const
+vpForceTwistMatrix vpForceTwistMatrix::operator*(const vpForceTwistMatrix &F) const
{
- vpForceTwistMatrix Fout ;
-
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
- s +=rowPtrs[i][k] * F.rowPtrs[k][j];
- Fout[i][j] = s ;
+ vpForceTwistMatrix Fout;
+
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < 6; k++)
+ s += rowPtrs[i][k] * F.rowPtrs[k][j];
+ Fout[i][j] = s;
}
}
return Fout;
}
/*!
- Operator that allows to multiply a skew transformation matrix by a matrix.
+ Operator that allows to multiply a force/torque skew transformation matrix
+ by a matrix.
- \exception vpException::dimensionError If M is not a 6 rows
+ \exception vpException::dimensionError : If \f$\bf M\f$ is not a 6 rows
dimension matrix.
*/
-vpMatrix
-vpForceTwistMatrix::operator*(const vpMatrix &M) const
+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()));
-
+ "Cannot multiply (6x6) force/torque twist matrix by a (%dx%d) matrix", M.getRows(), M.getCols()));
}
- vpMatrix p(6, M.getCols()) ;
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<M.getCols();j++) {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
+ vpMatrix p(6, M.getCols());
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < M.getCols(); j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < 6; k++)
s += rowPtrs[i][k] * M[k][j];
- p[i][j] = s ;
+ p[i][j] = s;
}
}
return p;
@@ -256,30 +313,31 @@ vpForceTwistMatrix::operator*(const vpMatrix &M) const
/*!
- Operator that allows to multiply a skew transformation matrix by a
- column vector.
+ Operator that allows to multiply a force/torque skew transformation matrix
+by a column vector.
- \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x, \tau_y, \tau_z] \f$.
+ \param H : Force/torque skew vector \f${\bf H} = [f_x, f_y, f_z, \tau_x,
+\tau_y, \tau_z] \f$.
- For example, this operator can be used to convert a force/torque skew from sensor
- frame into the probe frame :
+ For example, this operator can be used to convert a force/torque skew from
+sensor frame into the probe frame :
- \f$^p{\bf H} = ^p{\bf F}_s \; ^s{\bf H}\f$
+ \f[{^p}{\bf H}_{p} = {^p}{\bf F}_s \; {^s}{\bf H}_s\f]
The example below shows how to handle that transformation.
-
+
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpForceTwistMatrix.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
vpColVector sH = robot.getForceTorque(sH); // Get the force/torque measures
-#endif
+#endif
// Set the transformation from sensor frame to the probe frame
vpHomogeneousMatrix pMs;
@@ -291,7 +349,7 @@ int main()
// Compute the resulting force/torque in the probe frame
vpColVector pH(6); // Force/torque in the probe frame
pH = pFs * sH;
-
+
return 0;
}
\endcode
@@ -300,102 +358,181 @@ int main()
dimension vector.
*/
-vpColVector
-vpForceTwistMatrix::operator*(const vpColVector &H) const
+vpColVector vpForceTwistMatrix::operator*(const vpColVector &H) const
{
vpColVector Hout(6);
if (6 != H.getRows()) {
- throw (vpException(vpException::dimensionError,
- "Cannot multiply a (6x6) force/torque twist matrix by a %d dimension column vector",
- H.getRows()));
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (6x6) force/torque twist matrix by "
+ "a %d dimension column vector",
+ H.getRows()));
}
Hout = 0.0;
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
- Hout[i]+=rowPtrs[i][j] * H[j];
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ Hout[i] += rowPtrs[i][j] * H[j];
}
}
-
- return Hout ;
-}
+ return Hout;
+}
/*!
Build a force/torque twist transformation matrix from a translation vector
- \e t and a rotation matrix R.
+ \e t and a rotation matrix \e R.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
\param t : Translation vector.
-
+
+ \param R : Rotation matrix.
+
+*/
+vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
+{
+ vpMatrix skewaR = t.skew(t) * R;
+
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ (*this)[i][j] = R[i][j];
+ (*this)[i + 3][j + 3] = R[i][j];
+ (*this)[i + 3][j] = skewaR[i][j];
+ }
+ }
+ return (*this);
+}
+
+/*!
+
+ Build a block diagonal force/torque twist transformation matrix from a
+ rotation matrix \e R.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+
\param R : Rotation matrix.
*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R)
+vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpRotationMatrix &R)
{
- unsigned int i, j;
- vpMatrix skewaR = t.skew(t)*R ;
-
- for (i=0 ; i < 3 ; i++) {
- for (j=0 ; j < 3 ; j++) {
- (*this)[i][j] = R[i][j] ;
- (*this)[i+3][j+3] = R[i][j] ;
- (*this)[i+3][j] = skewaR[i][j] ;
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ (*this)[i][j] = R[i][j];
+ (*this)[i + 3][j + 3] = R[i][j];
+ (*this)[i + 3][j] = 0;
}
}
- return (*this) ;
+ return (*this);
}
/*!
- Initialize a force/torque twist transformation matrix from a translation vector
- \e t and a rotation vector with \f$\theta u \f$ parametrization.
+ Initialize a force/torque twist transformation matrix from a translation
+ vector \e t and a rotation vector with \f$\theta u \f$ parametrization.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
\param tv : Translation vector.
-
- \param thetau : \f$\theta {\bf u}\f$ rotation vector.
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector used to initialise
+ \f$\bf R \f$.
*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv,
- const vpThetaUVector &thetau)
+vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpTranslationVector &tv, const vpThetaUVector &thetau)
{
- vpRotationMatrix R ;
- R.buildFrom(thetau) ;
- buildFrom(tv,R) ;
- return (*this) ;
+ buildFrom(tv, vpRotationMatrix(thetau));
+ return (*this);
}
+/*!
+
+ Initialize a force/torque block diagonal twist transformation matrix from a
+ rotation vector with \f$\theta u \f$ parametrization.
+
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector used to initialise
+ \f$\bf R \f$.
+
+*/
+vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpThetaUVector &thetau)
+{
+ buildFrom(vpRotationMatrix(thetau));
+ return (*this);
+}
/*!
- Initialize a force/torque twist transformation matrix from an homogeneous matrix
- \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
+ Initialize a force/torque twist transformation matrix from an homogeneous
+ matrix \f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
\\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
- \param M : Homogeneous matrix \f$M\f$ used to initialize the force/torque twist
+ \param M : Homogeneous matrix \f$M\f$ used to initialize the velocity twist
transformation matrix.
-
+ \param full : Boolean used to indicate which matrix should be filled.
+ - When set to true, use the complete force/torque skew transformation:
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {[{\bf t}]}_{\times} \; {\bf R} & {\bf R}
+ \end{array}
+ \right]
+ \f]
+ - When set to false, use the block diagonal velocity skew transformation:
+ \f[
+ {\bf F} = \left[
+ \begin{array}{cc}
+ {\bf R} & {\bf 0}_{3 \times 3} \\
+ {{\bf 0}_{3 \times 3}} & {\bf R}
+ \end{array}
+ \right]
+ \f]
*/
-vpForceTwistMatrix
-vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
+vpForceTwistMatrix vpForceTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full)
{
- vpTranslationVector tv ;
- vpRotationMatrix R ;
- M.extract(R) ;
- M.extract(tv) ;
-
- buildFrom(tv, R) ;
- return (*this) ;
+ if (full)
+ buildFrom(M.getTranslationVector(), M.getRotationMatrix());
+ else
+ buildFrom(M.getRotationMatrix());
+
+ return (*this);
}
/*!
- Pretty print a velocity twist matrix. The data are tabulated.
+ Pretty print a force/torque twist matrix. The data are tabulated.
The common widths before and after the decimal point
are set with respect to the parameter maxlen.
@@ -412,90 +549,91 @@ 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;
unsigned int m = getRows();
unsigned int n = getCols();
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
- for (unsigned int j=0;j<n;++j){
+ for (unsigned int i = 0; i < m; ++i) {
+ for (unsigned int j = 0; j < n; ++j) {
oss.str("");
oss << (*this)[i][j];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[i][j];
oss.str(ossFixed.str());
}
- values[i*n+j]=oss.str();
- size_type thislen=values[i*n+j].size();
- size_type p=values[i*n+j].find('.');
+ values[i * n + j] = oss.str();
+ size_type thislen = values[i * n + j].size();
+ size_type p = values[i * n + j].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
}
}
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- for (unsigned int j=0;j<n;j++){
- size_type p=values[i*n+j].find('.');
+ for (unsigned int i = 0; i < m; i++) {
+ s << " ";
+ for (unsigned int j = 0; j < n; j++) {
+ size_type p = values[i * n + j].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[i*n+j].substr(0,p).c_str();
+ s << values[i * n + j].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[i*n+j].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[i * n + j].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
}
- s <<std::endl;
+ s << std::endl;
}
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
@@ -506,10 +644,6 @@ vpForceTwistMatrix::print(std::ostream& s, unsigned int length, char const* intr
Set the twist transformation matrix to identity.
\sa eye()
*/
-void
-vpForceTwistMatrix::setIdentity()
-{
- eye() ;
-}
+void vpForceTwistMatrix::setIdentity() { eye(); }
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
diff --git a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp
index 2db7fe3..548f172 100644
--- a/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp
+++ b/modules/core/src/math/transformation/vpHomogeneousMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpHomogeneousMatrix.cpp
\brief Defines vpHomogeneousMatrix class. Class that consider
@@ -43,46 +43,39 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpQuaternionVector.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpException.h>
+#include <visp3/core/vpQuaternionVector.h>
/*!
- Construct an homogeneous matrix from a translation vector and quaternion rotation vector.
+ 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<double>(4, 4)
{
- buildFrom(t,q);
+ buildFrom(t, q);
(*this)[3][3] = 1.;
}
/*!
Default constructor that initialize an homogeneous matrix as identity.
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix()
- : vpArray2D<double>(4, 4)
-{
- eye() ;
-}
+vpHomogeneousMatrix::vpHomogeneousMatrix() : vpArray2D<double>(4, 4) { eye(); }
/*!
- Copy constructor that initialize an homogeneous matrix from another homogeneous matrix.
+ Copy constructor that initialize an homogeneous matrix from another
+ homogeneous matrix.
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M)
- : vpArray2D<double>(4, 4)
-{
- *this = M;
-}
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(4, 4) { *this = M; }
/*!
- Construct an homogeneous matrix from a translation vector and \f$\theta {\bf u}\f$ rotation vector.
+ 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<double>(4, 4)
{
buildFrom(t, tu);
@@ -90,10 +83,10 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
}
/*!
- Construct an homogeneous matrix from a translation vector and a rotation matrix.
+ 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<double>(4, 4)
{
insert(R);
@@ -104,16 +97,16 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const vpTranslationVector &t,
/*!
Construct an homogeneous matrix from a pose vector.
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p)
- : vpArray2D<double>(4, 4)
+vpHomogeneousMatrix::vpHomogeneousMatrix(const vpPoseVector &p) : vpArray2D<double>(4, 4)
{
- buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]) ;
+ buildFrom(p[0], p[1], p[2], p[3], p[4], p[5]);
(*this)[3][3] = 1.;
}
/*!
Construct an homogeneous matrix from a vector of float.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+ \param v : Vector of 12 or 16 values corresponding to the values of the
+homogeneous matrix.
The following example shows how to use this function:
\code
@@ -149,16 +142,16 @@ M:
0 0 0 1
\endcode
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v)
- : vpArray2D<double>(4, 4)
+vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<float> &v) : vpArray2D<double>(4, 4)
{
- buildFrom(v) ;
+ buildFrom(v);
(*this)[3][3] = 1.;
}
/*!
Construct an homogeneous matrix from a vector of double.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+ \param v : Vector of 12 or 16 values corresponding to the values of the
+homogeneous matrix.
The following example shows how to use this function:
\code
@@ -194,23 +187,19 @@ M:
0 0 0 1
\endcode
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v)
- : vpArray2D<double>(4, 4)
+vpHomogeneousMatrix::vpHomogeneousMatrix(const std::vector<double> &v) : vpArray2D<double>(4, 4)
{
- buildFrom(v) ;
+ buildFrom(v);
(*this)[3][3] = 1.;
}
/*!
- Construct an homogeneous matrix from a translation vector \f${\bf t}=(t_x, t_y, t_z)^T\f$
- and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ rotation vector.
+ Construct an homogeneous matrix from a translation vector \f${\bf t}=(t_x,
+ t_y, t_z)^T\f$ and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta
+ u_z)^T\f$ rotation vector.
*/
-vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz)
+vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx, const double ty, const double tz, const double tux,
+ const double tuy, const double tuz)
: vpArray2D<double>(4, 4)
{
buildFrom(tx, ty, tz, tux, tuy, tuz);
@@ -221,72 +210,63 @@ vpHomogeneousMatrix::vpHomogeneousMatrix(const double tx,
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) ;
+ insert(tu);
+ insert(t);
}
/*!
Build an homogeneous matrix from a translation vector
and a rotation matrix.
*/
-void
-vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R)
+void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
{
- insert(R) ;
- insert(t) ;
+ insert(R);
+ insert(t);
}
/*!
Build an homogeneous matrix from a pose vector.
*/
-void
-vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
+void vpHomogeneousMatrix::buildFrom(const vpPoseVector &p)
{
vpTranslationVector tv(p[0], p[1], p[2]);
vpThetaUVector tu(p[3], p[4], p[5]);
- insert(tu) ;
- insert(tv) ;
+ insert(tu);
+ insert(tv);
}
/*!
Build an homogeneous matrix from a translation vector
and a quaternion rotation vector.
*/
-void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t,
- const vpQuaternionVector &q)
+void vpHomogeneousMatrix::buildFrom(const vpTranslationVector &t, const vpQuaternionVector &q)
{
insert(t);
insert(q);
}
/*!
- Build an homogeneous matrix from a translation vector \f${\bf t}=(t_x, t_y, t_z)^T\f$
- and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ rotation vector.
+ Build an homogeneous matrix from a translation vector \f${\bf t}=(t_x, t_y,
+ t_z)^T\f$ and a \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$
+ rotation vector.
*/
-void
-vpHomogeneousMatrix::buildFrom(const double tx,
- const double ty,
- const double tz,
- const double tux,
- const double tuy,
- const double tuz)
+void vpHomogeneousMatrix::buildFrom(const double tx, const double ty, const double tz, const double tux,
+ const double tuy, const double tuz)
{
- vpRotationMatrix R(tux, tuy, tuz) ;
- vpTranslationVector t(tx, ty, tz) ;
+ vpRotationMatrix R(tux, tuy, tuz);
+ vpTranslationVector t(tx, ty, tz);
- insert(R) ;
- insert(t) ;
+ insert(R);
+ insert(t);
}
/*!
Build an homogeneous matrix from a vector of float.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+ \param v : Vector of 12 or 16 values corresponding to the values of the
+homogeneous matrix.
The following example shows how to use this function:
\code
@@ -323,20 +303,20 @@ M:
0 0 0 1
\endcode
*/
-void
-vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
+void vpHomogeneousMatrix::buildFrom(const std::vector<float> &v)
{
if (v.size() != 12 && v.size() != 16) {
throw(vpException(vpException::dimensionError, "Cannot convert std::vector<float> to vpHomogeneousMatrix"));
}
- for (unsigned int i=0; i < 12; i++)
+ for (unsigned int i = 0; i < 12; i++)
this->data[i] = (double)v[i];
}
/*!
Build an homogeneous matrix from a vector of double.
- \param v : Vector of 12 or 16 values corresponding to the values of the homogeneous matrix.
+ \param v : Vector of 12 or 16 values corresponding to the values of the
+homogeneous matrix.
The following example shows how to use this function:
\code
@@ -373,14 +353,13 @@ M:
0 0 0 1
\endcode
*/
-void
-vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
+void vpHomogeneousMatrix::buildFrom(const std::vector<double> &v)
{
if (v.size() != 12 && v.size() != 16) {
throw(vpException(vpException::dimensionError, "Cannot convert std::vector<double> to vpHomogeneousMatrix"));
}
- for (unsigned int i=0; i < 12; i++)
+ for (unsigned int i = 0; i < 12; i++)
this->data[i] = v[i];
}
@@ -389,11 +368,10 @@ vpHomogeneousMatrix::buildFrom(const std::vector<double> &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++) {
+ for (int i = 0; i < 4; i++) {
+ for (int j = 0; j < 4; j++) {
rowPtrs[i][j] = M.rowPtrs[i][j];
}
}
@@ -412,31 +390,30 @@ int main()
// Initialize aMb and bMc...
// Compute aMc * bMc
- vpHomogeneousMatrix aMc = aMb * bMc;
+ vpHomogeneousMatrix aMc = aMb * bMc;
}
\endcode
*/
-vpHomogeneousMatrix
-vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
+vpHomogeneousMatrix vpHomogeneousMatrix::operator*(const vpHomogeneousMatrix &M) const
{
vpHomogeneousMatrix p;
- vpRotationMatrix R1, R2, R ;
- vpTranslationVector T1, T2 , T;
+ vpRotationMatrix R1, R2, R;
+ vpTranslationVector T1, T2, T;
- extract(T1) ;
- M.extract(T2) ;
+ extract(T1);
+ M.extract(T2);
- extract (R1) ;
- M.extract (R2) ;
+ extract(R1);
+ M.extract(R2);
- R = R1*R2 ;
+ R = R1 * R2;
- T = R1*T2 + T1 ;
+ T = R1 * T2 + T1;
- p.insert(T) ;
- p.insert(R) ;
+ p.insert(T);
+ p.insert(R);
return p;
}
@@ -458,33 +435,34 @@ int main()
\endcode
*/
-vpHomogeneousMatrix &
-vpHomogeneousMatrix::operator*=(const vpHomogeneousMatrix &M)
+vpHomogeneousMatrix &vpHomogeneousMatrix::operator*=(const vpHomogeneousMatrix &M)
{
(*this) = (*this) * M;
return (*this);
}
/*!
- Operator that allow to multiply an homogeneous matrix by a 4-dimension column vector.
+ Operator that allow to multiply an homogeneous matrix by a 4-dimension
+ column vector.
- \exception vpException::dimensionError : If the vector \e v is not a 4-dimension vector.
+ \exception vpException::dimensionError : If the vector \e v is not a
+ 4-dimension vector.
*/
-vpColVector
-vpHomogeneousMatrix::operator*(const vpColVector &v) const
+vpColVector vpHomogeneousMatrix::operator*(const vpColVector &v) const
{
if (v.getRows() != 4) {
throw(vpException(vpException::dimensionError,
- "Cannot multiply a (4x4) homogeneous matrix by a (%dx1) column vector",
+ "Cannot multiply a (4x4) homogeneous matrix by a "
+ "(%dx1) column vector",
v.getRows()));
}
vpColVector p(rowNum);
p = 0.0;
- for (unsigned int j=0;j<4;j++) {
- for (unsigned int i=0;i<4;i++) {
- p[i]+=rowPtrs[i][j] * v[j];
+ for (unsigned int j = 0; j < 4; j++) {
+ for (unsigned int i = 0; i < 4; i++) {
+ p[i] += rowPtrs[i][j] * v[j];
}
}
@@ -492,62 +470,66 @@ vpHomogeneousMatrix::operator*(const vpColVector &v) const
}
/*!
- From the coordinates of the point in camera frame b and the transformation between
- camera frame a and camera frame b computes the coordinates of the point in camera frame a.
+ From the coordinates of the point in camera frame b and the transformation
+ between camera frame a and camera frame b computes the coordinates of the
+ point in camera frame a.
\param bP : 3D coordinates of the point in camera frame bP.
- \return A point with 3D coordinates in the camera frame a. The coordinates in the world or object
- frame are set to the same coordinates than the one in the camera frame.
+ \return A point with 3D coordinates in the camera frame a. The coordinates
+ in the world or object frame are set to the same coordinates than the one in
+ the camera frame.
*/
-vpPoint vpHomogeneousMatrix::operator*(const vpPoint& bP) const
+vpPoint vpHomogeneousMatrix::operator*(const vpPoint &bP) const
{
- vpPoint aP ;
+ vpPoint aP;
- vpColVector v(4),v1(4) ;
+ vpColVector v(4), v1(4);
- v[0] = bP.get_X() ;
- v[1] = bP.get_Y() ;
- v[2] = bP.get_Z() ;
- v[3] = bP.get_W() ;
+ v[0] = bP.get_X();
+ v[1] = bP.get_Y();
+ v[2] = bP.get_Z();
+ v[3] = bP.get_W();
- v1[0] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2]+ (*this)[0][3]*v[3] ;
- v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2]+ (*this)[1][3]*v[3] ;
- v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2]+ (*this)[2][3]*v[3] ;
- v1[3] = (*this)[3][0]*v[0] + (*this)[3][1]*v[1]+ (*this)[3][2]*v[2]+ (*this)[3][3]*v[3] ;
+ v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2] + (*this)[0][3] * v[3];
+ v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2] + (*this)[1][3] * v[3];
+ v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2] + (*this)[2][3] * v[3];
+ v1[3] = (*this)[3][0] * v[0] + (*this)[3][1] * v[1] + (*this)[3][2] * v[2] + (*this)[3][3] * v[3];
- v1 /= v1[3] ;
+ v1 /= v1[3];
// v1 = M*v ;
- aP.set_X(v1[0]) ;
- aP.set_Y(v1[1]) ;
- aP.set_Z(v1[2]) ;
- aP.set_W(v1[3]) ;
+ aP.set_X(v1[0]);
+ aP.set_Y(v1[1]);
+ aP.set_Z(v1[2]);
+ aP.set_W(v1[3]);
- aP.set_oX(v1[0]) ;
- aP.set_oY(v1[1]) ;
- aP.set_oZ(v1[2]) ;
- aP.set_oW(v1[3]) ;
+ aP.set_oX(v1[0]);
+ aP.set_oY(v1[1]);
+ aP.set_oZ(v1[2]);
+ aP.set_oW(v1[3]);
- return aP ;
+ return aP;
}
/*!
- Since a translation vector could be seen as the origin point of a frame, this function computes
- the new coordinates of a translation vector after applying an homogeneous transformation.
+ Since a translation vector could be seen as the origin point of a frame,
+ this function computes the new coordinates of a translation vector after
+ applying an homogeneous transformation.
\param t : Translation vector seen as the 3D coordinates of a point.
- \return A translation vector that contains the new 3D coordinates after applying the homogeneous transformation.
+ \return A translation vector that contains the new 3D coordinates after
+ applying the homogeneous transformation.
*/
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 ;
+ return t_out;
}
/*********************************************************************/
@@ -556,42 +538,38 @@ vpTranslationVector vpHomogeneousMatrix::operator*(const vpTranslationVector &t)
Test if the 3x3 rotational part of the homogeneous matrix is really
a rotation matrix.
*/
-bool
-vpHomogeneousMatrix::isAnHomogeneousMatrix() const
+bool vpHomogeneousMatrix::isAnHomogeneousMatrix() const
{
- vpRotationMatrix R ;
- extract(R) ;
+ vpRotationMatrix R;
+ extract(R);
- return R.isARotationMatrix() ;
+ return R.isARotationMatrix();
}
/*!
Extract the rotational matrix from the homogeneous matrix.
\param R : rotational component as a rotation matrix.
*/
-void
-vpHomogeneousMatrix::extract(vpRotationMatrix &R) const
+void vpHomogeneousMatrix::extract(vpRotationMatrix &R) const
{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- R[i][j] = (*this)[i][j] ;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ R[i][j] = (*this)[i][j];
}
/*!
- Extract the translation vector from the homogeneous matrix.
+ Extract the translation vector from the homogeneous matrix.
*/
-void
-vpHomogeneousMatrix::extract(vpTranslationVector &t) const
+void vpHomogeneousMatrix::extract(vpTranslationVector &t) const
{
- t[0] = (*this)[0][3] ;
- t[1] = (*this)[1][3] ;
- t[2] = (*this)[2][3] ;
+ t[0] = (*this)[0][3];
+ t[1] = (*this)[1][3];
+ t[2] = (*this)[2][3];
}
/*!
Extract the rotation as a \f$\theta \bf u\f$ vector.
*/
-void
-vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
+void vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
{
vpRotationMatrix R;
(*this).extract(R);
@@ -601,8 +579,7 @@ vpHomogeneousMatrix::extract(vpThetaUVector &tu) const
/*!
Extract the rotation as a quaternion.
*/
-void
-vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
+void vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
{
vpRotationMatrix R;
(*this).extract(R);
@@ -612,48 +589,42 @@ vpHomogeneousMatrix::extract(vpQuaternionVector &q) const
/*!
Insert the rotational component of the homogeneous matrix.
*/
-void
-vpHomogeneousMatrix::insert(const vpRotationMatrix &R)
+void vpHomogeneousMatrix::insert(const vpRotationMatrix &R)
{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- (*this)[i][j] = R[i][j] ;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ (*this)[i][j] = R[i][j];
}
-/*!
+/*!
Insert the rotational component of the homogeneous matrix from a
\f$theta {\bf u}\f$ rotation vector.
*/
-void
-vpHomogeneousMatrix::insert(const vpThetaUVector &tu)
+void vpHomogeneousMatrix::insert(const vpThetaUVector &tu)
{
- vpRotationMatrix R(tu) ;
- insert(R) ;
+ vpRotationMatrix R(tu);
+ insert(R);
}
/*!
Insert the translational component in a homogeneous matrix.
*/
-void
-vpHomogeneousMatrix::insert(const vpTranslationVector &t)
+void vpHomogeneousMatrix::insert(const vpTranslationVector &t)
{
- (*this)[0][3] = t[0] ;
- (*this)[1][3] = t[1] ;
- (*this)[2][3] = t[2] ;
+ (*this)[0][3] = t[0];
+ (*this)[1][3] = t[1];
+ (*this)[2][3] = t[2];
}
-/*!
+/*!
Insert the rotational component of the homogeneous matrix from a
quaternion rotation vector.
*/
-void
-vpHomogeneousMatrix::insert(const vpQuaternionVector &q){
- insert(vpRotationMatrix(q));
-}
+void vpHomogeneousMatrix::insert(const vpQuaternionVector &q) { insert(vpRotationMatrix(q)); }
/*!
Invert the homogeneous matrix
@@ -667,23 +638,24 @@ vpHomogeneousMatrix::insert(const vpQuaternionVector &q){
{\bf 0}_{1\times 3} & 1
\end{array}
\right]\f$
-
+
*/
-vpHomogeneousMatrix
-vpHomogeneousMatrix::inverse() const
+vpHomogeneousMatrix vpHomogeneousMatrix::inverse() const
{
- vpHomogeneousMatrix Mi ;
-
- vpRotationMatrix R ; extract(R) ;
- vpTranslationVector T ; extract(T) ;
+ vpHomogeneousMatrix Mi;
- vpTranslationVector RtT ; RtT = -(R.t()*T) ;
+ vpRotationMatrix R;
+ extract(R);
+ vpTranslationVector T;
+ extract(T);
+ vpTranslationVector RtT;
+ RtT = -(R.t() * T);
- Mi.insert(R.t()) ;
- Mi.insert(RtT) ;
+ Mi.insert(R.t());
+ Mi.insert(RtT);
- return Mi ;
+ return Mi;
}
/*!
@@ -691,15 +663,15 @@ vpHomogeneousMatrix::inverse() const
*/
void vpHomogeneousMatrix::eye()
{
- (*this)[0][0] = 1 ;
- (*this)[1][1] = 1 ;
- (*this)[2][2] = 1 ;
- (*this)[3][3] = 1 ;
+ (*this)[0][0] = 1;
+ (*this)[1][1] = 1;
+ (*this)[2][2] = 1;
+ (*this)[3][3] = 1;
- (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0 ;
- (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0 ;
- (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0 ;
- (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0 ;
+ (*this)[0][1] = (*this)[0][2] = (*this)[0][3] = 0;
+ (*this)[1][0] = (*this)[1][2] = (*this)[1][3] = 0;
+ (*this)[2][0] = (*this)[2][1] = (*this)[2][3] = 0;
+ (*this)[3][0] = (*this)[3][1] = (*this)[3][2] = 0;
}
/*!
@@ -716,15 +688,10 @@ 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.
+ Write an homogeneous matrix in an output file stream.
\param f : Output file stream. The homogeneous matrix is saved as a
4 by 4 matrix.
@@ -737,33 +704,29 @@ vpHomogeneousMatrix::inverse(vpHomogeneousMatrix &M) const
vpRxyzVector r(M_PI, 0, -M_PI/4.);
vpRotationMatrix R(r);
vpHomogeneousMatrix M(t, R);
-
+
// Save the content of the matrix in "homogeneous.dat"
- std::ofstream f("homogeneous.dat");
+ std::ofstream f("homogeneous.dat");
M.save(f);
\endcode
\sa load()
*/
-void
-vpHomogeneousMatrix::save(std::ofstream &f) const
+void vpHomogeneousMatrix::save(std::ofstream &f) const
{
- if (! f.fail()) {
- f << *this ;
- }
- else {
- throw(vpException(vpException::ioError,
- "Cannot save homogeneous matrix: ostream not open")) ;
+ if (!f.fail()) {
+ f << *this;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot save homogeneous matrix: ostream not open"));
}
}
-
/*!
Read an homogeneous matrix from an input file stream. The
homogeneous matrix is considered as a 4 by 4 matrix.
- \param f : Input file stream.
+ \param f : Input file stream.
The code below shows how to get an homogeneous matrix from a file.
@@ -776,28 +739,24 @@ vpHomogeneousMatrix::save(std::ofstream &f) const
\sa save()
*/
-void
-vpHomogeneousMatrix::load(std::ifstream &f)
+void vpHomogeneousMatrix::load(std::ifstream &f)
{
- if (! f.fail()) {
- for (unsigned int i=0 ; i < 4 ; i++) {
- for (unsigned int j=0 ; j < 4 ; j++) {
- f >> (*this)[i][j] ;
+ if (!f.fail()) {
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 0; j < 4; j++) {
+ f >> (*this)[i][j];
}
}
- }
- else {
- throw(vpException(vpException::ioError,
- "Cannot laad homogeneous matrix: ifstream not open")) ;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot laad homogeneous matrix: ifstream not open"));
}
}
//! Print the matrix as a pose vector \f$({\bf t}^T \theta {\bf u}^T)\f$
-void
-vpHomogeneousMatrix::print() const
+void vpHomogeneousMatrix::print() const
{
- vpPoseVector r(*this) ;
- std::cout << r.t() ;
+ vpPoseVector r(*this);
+ std::cout << r.t();
}
/*!
@@ -807,7 +766,7 @@ vpHomogeneousMatrix::print() const
void vpHomogeneousMatrix::convert(std::vector<float> &M)
{
M.resize(12);
- for(unsigned int i=0; i < 12; i++)
+ for (unsigned int i = 0; i < 12; i++)
M[i] = (float)(this->data[i]);
}
@@ -818,7 +777,7 @@ void vpHomogeneousMatrix::convert(std::vector<float> &M)
void vpHomogeneousMatrix::convert(std::vector<double> &M)
{
M.resize(12);
- for(unsigned int i=0; i < 12; i++)
+ for (unsigned int i = 0; i < 12; i++)
M[i] = this->data[i];
}
@@ -843,8 +802,8 @@ vpRotationMatrix vpHomogeneousMatrix::getRotationMatrix() const
}
/*!
- Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part of the
- homogeneous transformation.
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part
+ of the homogeneous transformation.
*/
vpThetaUVector vpHomogeneousMatrix::getThetaUVector() const
{
@@ -856,8 +815,8 @@ vpThetaUVector vpHomogeneousMatrix::getThetaUVector() const
/*!
Extract a column vector from an homogeneous matrix.
\warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If j=0, the first column is extracted.
- \return The extracted column vector.
+ \param j : Index of the column to extract. If j=0, the first column is
+extracted. \return The extracted column vector.
The following example shows how to use this function:
\code
@@ -881,15 +840,13 @@ Last column:
0
\endcode
*/
-vpColVector
-vpHomogeneousMatrix::getCol(const unsigned int j) const
+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++)
+ for (unsigned int i = 0; i < nb_rows; i++)
c[i] = (*this)[i][j];
return c;
}
@@ -902,10 +859,6 @@ vpHomogeneousMatrix::getCol(const unsigned int j) const
Set homogeneous matrix to identity.
\sa eye()
*/
-void
-vpHomogeneousMatrix::setIdentity()
-{
- eye() ;
-}
+void vpHomogeneousMatrix::setIdentity() { eye(); }
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
diff --git a/modules/core/src/math/transformation/vpPoseVector.cpp b/modules/core/src/math/transformation/vpPoseVector.cpp
index 9e8958e..8dfbf27 100644
--- a/modules/core/src/math/transformation/vpPoseVector.cpp
+++ b/modules/core/src/math/transformation/vpPoseVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,38 +38,34 @@
*
*****************************************************************************/
-
/*!
\file vpPoseVector.cpp
\brief Pose vector.
*/
-#include <sstream>
#include <assert.h>
+#include <sstream>
-#include <visp3/core/vpPoseVector.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpException.h>
-
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpPoseVector.h>
/*!
-
- Default constructor that construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
- u]^\top\f$ where \f$ \theta \bf u\f$ is a rotation vector \f$[\theta
- u_x, \theta u_y, \theta u_z]^\top\f$ and \f$ \bf t \f$ is a
+
+ Default constructor that construct a 6 dimension pose vector \f$ [\bf t,
+ \theta \bf u]^\top\f$ where \f$ \theta \bf u\f$ is a rotation vector
+ \f$[\theta u_x, \theta u_y, \theta u_z]^\top\f$ and \f$ \bf t \f$ is a
translation vector \f$[t_x, t_y, t_z]^\top\f$.
The pose vector is initialized to zero.
*/
-vpPoseVector::vpPoseVector()
- : vpArray2D<double>(6, 1)
-{}
+vpPoseVector::vpPoseVector() : vpArray2D<double>(6, 1) {}
-/*!
+/*!
Construct a 6 dimension pose vector \f$ [\bf{t}, \theta
\bf{u}]^\top\f$ from 3 translations and 3 \f$ \theta \bf{u}\f$
@@ -83,11 +80,7 @@ vpPoseVector::vpPoseVector()
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<double>(6, 1)
{
@@ -100,7 +93,7 @@ vpPoseVector::vpPoseVector(const double tx,
(*this)[5] = tuz;
}
-/*!
+/*!
Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
u]^\top\f$ from a translation vector \f$ \bf tv \f$ and a \f$\theta
@@ -110,14 +103,12 @@ vpPoseVector::vpPoseVector(const double tx,
\param tu : \f$\theta \bf u\f$ rotation vector.
*/
-vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
- const vpThetaUVector& tu)
- : vpArray2D<double>(6, 1)
+vpPoseVector::vpPoseVector(const vpTranslationVector &tv, const vpThetaUVector &tu) : vpArray2D<double>(6, 1)
{
- buildFrom(tv, tu) ;
+ buildFrom(tv, tu);
}
-/*!
+/*!
Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
u]^\top\f$ from a translation vector \f$ \bf t \f$ and a rotation
@@ -129,14 +120,12 @@ vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
u\f$ vector is extracted to initialise the pose vector.
*/
-vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
- const vpRotationMatrix& R)
- : vpArray2D<double>(6, 1)
+vpPoseVector::vpPoseVector(const vpTranslationVector &tv, const vpRotationMatrix &R) : vpArray2D<double>(6, 1)
{
- buildFrom(tv, R) ;
+ buildFrom(tv, R);
}
-/*!
+/*!
Construct a 6 dimension pose vector \f$ [\bf t, \theta \bf
u]^\top\f$ from an homogeneous matrix \f$ \bf M \f$.
@@ -146,11 +135,7 @@ vpPoseVector::vpPoseVector(const vpTranslationVector& tv,
initialize the pose vector.
*/
-vpPoseVector::vpPoseVector(const vpHomogeneousMatrix& M)
- : vpArray2D<double>(6, 1)
-{
- buildFrom(M) ;
-}
+vpPoseVector::vpPoseVector(const vpHomogeneousMatrix &M) : vpArray2D<double>(6, 1) { buildFrom(M); }
/*!
@@ -167,9 +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;
@@ -196,9 +180,8 @@ 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, const double tuy, const double tuz)
+vpPoseVector vpPoseVector::buildFrom(const double tx, const double ty, const double tz, const double tux,
+ const double tuy, const double tuz)
{
(*this)[0] = tx;
(*this)[1] = ty;
@@ -207,7 +190,7 @@ vpPoseVector::buildFrom(const double tx, const double ty, const double tz,
(*this)[3] = tux;
(*this)[4] = tuy;
(*this)[5] = tuz;
- return *this ;
+ return *this;
}
/*!
@@ -221,16 +204,17 @@ vpPoseVector::buildFrom(const double tx, const double ty, const double tz,
\return The build pose vector.
*/
-vpPoseVector
-vpPoseVector::buildFrom(const vpHomogeneousMatrix& M)
+vpPoseVector vpPoseVector::buildFrom(const vpHomogeneousMatrix &M)
{
- vpRotationMatrix R ; M.extract(R) ;
- vpTranslationVector tv ; M.extract(tv) ;
- buildFrom(tv,R) ;
- return *this ;
+ vpRotationMatrix R;
+ M.extract(R);
+ vpTranslationVector tv;
+ M.extract(tv);
+ buildFrom(tv, R);
+ return *this;
}
-/*!
+/*!
Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$
from a translation vector \f$ \bf t \f$ and a \f$\theta \bf u\f$
@@ -241,18 +225,16 @@ 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] ;
- (*this)[i+3] = tu[i] ;
+ for (unsigned int i = 0; i < 3; i++) {
+ (*this)[i] = tv[i];
+ (*this)[i + 3] = tu[i];
}
- return *this ;
+ return *this;
}
-/*!
+/*!
Build a 6 dimension pose vector \f$ [\bf t, \theta \bf u]^\top\f$
from a translation vector \f$ \bf t \f$ and a rotation matrix \f$
@@ -265,22 +247,19 @@ 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) ;
+ vpThetaUVector tu;
+ tu.buildFrom(R);
- buildFrom(tv, tu) ;
- return *this ;
+ buildFrom(tv, tu);
+ return *this;
}
/*!
Extract the translation vector from the homogeneous matrix.
*/
-void
-vpPoseVector::extract(vpTranslationVector &tv) const
+void vpPoseVector::extract(vpTranslationVector &tv) const
{
tv[0] = (*this)[0];
tv[1] = (*this)[1];
@@ -290,8 +269,7 @@ vpPoseVector::extract(vpTranslationVector &tv) const
/*!
Extract the rotation as a \f$\theta \bf u\f$ vector.
*/
-void
-vpPoseVector::extract(vpThetaUVector &tu) const
+void vpPoseVector::extract(vpThetaUVector &tu) const
{
tu[0] = (*this)[3];
tu[1] = (*this)[4];
@@ -300,8 +278,7 @@ vpPoseVector::extract(vpThetaUVector &tu) const
/*!
Extract the rotation as a quaternion vector.
*/
-void
-vpPoseVector::extract(vpQuaternionVector &q) const
+void vpPoseVector::extract(vpQuaternionVector &q) const
{
vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
q.buildFrom(R);
@@ -309,14 +286,10 @@ 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.
+ Return the translation vector that corresponds to the translation part of
+ the pose vector.
*/
vpTranslationVector vpPoseVector::getTranslationVector() const
{
@@ -330,13 +303,13 @@ vpTranslationVector vpPoseVector::getTranslationVector() const
*/
vpRotationMatrix vpPoseVector::getRotationMatrix() const
{
- vpRotationMatrix R((*this)[0], (*this)[1], (*this)[2]);
+ vpRotationMatrix R((*this)[3], (*this)[4], (*this)[5]);
return R;
}
/*!
- Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part of the
- pose vector.
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation part
+ of the pose vector.
*/
vpThetaUVector vpPoseVector::getThetaUVector() const
{
@@ -365,40 +338,38 @@ vpThetaUVector vpPoseVector::getThetaUVector() const
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &A)
*/
-void
-vpPoseVector::print() const
+void vpPoseVector::print() const
{
- for (unsigned int i =0 ; i < 6 ; i++)
- if (i<3) std::cout << (*this)[i] <<" " ;
- else std::cout << vpMath::deg((*this)[i]) <<" " ;
- std::cout <<std::endl ;
+ for (unsigned int i = 0; i < 6; i++)
+ if (i < 3)
+ std::cout << (*this)[i] << " ";
+ else
+ std::cout << vpMath::deg((*this)[i]) << " ";
+ std::cout << std::endl;
}
/*!
Save the pose vector in the output file stream.
- \param f : Output file stream. Should be open before entering in this method.
+ \param f : Output file stream. Should be open before entering in this
+ method.
\exception vpException::ioError : If the output stream is not open.
\sa load()
*/
-void
-vpPoseVector::save(std::ofstream &f) const
+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")) ;
+ if (!f.fail()) {
+ f << *this;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot save the pose vector: ofstream not openned"));
}
}
-
/*!
- Read a pose vector from an input file stream.
+ Read a pose vector from an input file stream.
\param f : The input file stream..Should be open before entering in
this method.
@@ -407,17 +378,14 @@ vpPoseVector::save(std::ofstream &f) const
\sa save()
*/
-void
-vpPoseVector::load(std::ifstream &f)
+void vpPoseVector::load(std::ifstream &f)
{
- if (! f.fail()) {
- for (unsigned int i=0 ; i < 6 ; i++) {
- f >> (*this)[i] ;
+ if (!f.fail()) {
+ for (unsigned int i = 0; i < 6; i++) {
+ f >> (*this)[i];
}
- }
- else {
- throw(vpException(vpException::ioError,
- "Cannot read pose vector: ifstream not openned")) ;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot read pose vector: ifstream not openned"));
}
}
@@ -428,7 +396,7 @@ vpPoseVector::load(std::ifstream &f)
vpRowVector vpPoseVector::t() const
{
vpRowVector v(rowNum);
- memcpy(v.data, data, rowNum*sizeof(double)) ;
+ memcpy(v.data, data, rowNum * sizeof(double));
return v;
}
@@ -451,86 +419,86 @@ vpRowVector vpPoseVector::t() const
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &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;
unsigned int m = getRows();
unsigned int n = 1;
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
+ for (unsigned int i = 0; i < m; ++i) {
oss.str("");
oss << (*this)[i];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[i];
oss.str(ossFixed.str());
}
- values[i]=oss.str();
- size_type thislen=values[i].size();
- size_type p=values[i].find('.');
+ values[i] = oss.str();
+ size_type thislen = values[i].size();
+ size_type p = values[i].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
-
}
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- size_type p=values[i].find('.');
+ for (unsigned int i = 0; i < m; i++) {
+ s << " ";
+ size_type p = values[i].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[i].substr(0,p).c_str();
+ s << values[i].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[i].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[i].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
- s <<std::endl;
+ s << std::endl;
}
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
diff --git a/modules/core/src/math/transformation/vpQuaternionVector.cpp b/modules/core/src/math/transformation/vpQuaternionVector.cpp
index 0ac833b..1426c30 100644
--- a/modules/core/src/math/transformation/vpQuaternionVector.cpp
+++ b/modules/core/src/math/transformation/vpQuaternionVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpQuaternionVector.h>
-#include <visp3/core/vpMath.h>
+#include <algorithm>
#include <stdio.h>
#include <string.h>
-#include <algorithm>
-
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpQuaternionVector.h>
// minimum value of sine
const double vpQuaternionVector::minimum = 0.0001;
@@ -52,44 +51,35 @@ const double vpQuaternionVector::minimum = 0.0001;
*/
/*! Default constructor that initialize all the 4 angles to zero. */
-vpQuaternionVector::vpQuaternionVector()
- : vpRotationVector(4)
-{}
+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_)
- : vpRotationVector(4)
+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++)
+ for (unsigned int i = 0; i < 4; i++)
data[i] = q[i];
}
-/*!
+/*!
Constructs a quaternion from a rotation matrix.
\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
@@ -97,12 +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,13 +96,12 @@ 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;
- data[2]=qz;
- data[3]=qw;
+ data[0] = qx;
+ data[1] = qy;
+ data[2] = qz;
+ data[3] = qw;
}
/*!
Manually change values of a quaternion.
@@ -128,9 +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;
@@ -142,15 +124,14 @@ vpQuaternionVector::buildFrom(const double qx, const double qy,
input.
\return Quaternion vector.
*/
-vpQuaternionVector
-vpQuaternionVector::buildFrom(const vpThetaUVector& tu)
+vpQuaternionVector vpQuaternionVector::buildFrom(const vpThetaUVector &tu)
{
- vpRotationMatrix R(tu) ;
- buildFrom(R) ;
+ vpRotationMatrix R(tu);
+ buildFrom(R);
- return *this ;
+ return *this;
}
-/*!
+/*!
Quaternion addition.
Adds two quaternions. Addition is component-wise.
@@ -158,10 +139,10 @@ vpQuaternionVector::buildFrom(const vpThetaUVector& tu)
\param q : quaternion to add.
*/
vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) const
-{
- return vpQuaternionVector(x()+q.x(), y()+q.y(), z()+q.z(), w()+q.w());
+{
+ return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w());
}
-/*!
+/*!
Quaternion substraction.
Substracts a quaternion from another. Substraction is component-wise.
@@ -170,43 +151,43 @@ vpQuaternionVector vpQuaternionVector::operator+(const vpQuaternionVector &q) co
*/
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
{
- return vpQuaternionVector(l*x(),l*y(),l*z(),l*w());
+ return vpQuaternionVector(l * x(), l * y(), l * z(), l * w());
}
//! 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());
+ w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
+ w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
+ w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
}
//! Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
vpQuaternionVector vpQuaternionVector::operator/(const double l) const
{
- if(vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
+ if (vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
throw vpException(vpException::fatalError, "Division by scalar l==0 !");
}
- return vpQuaternionVector(x()/l,y()/l,z()/l,w()/l);
+ return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l);
}
/*!
- Copy operator that initializes a quaternion vector from a 4-dimension column vector \e q.
+ Copy operator that initializes a quaternion vector from a 4-dimension column
+vector \e q.
- \param q : 4-dimension vector containing the values of the quaternion vector.
+ \param q : 4-dimension vector containing the values of the quaternion
+vector.
\code
#include <visp3/core/vpQuaternionVector.h>
@@ -227,21 +208,21 @@ 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++)
+ for (unsigned int i = 0; i < 4; i++)
data[i] = q[i];
return *this;
}
-/*!
+/*!
Constructs a quaternion from a rotation matrix.
-
+
\param R : Rotation matrix.
*/
-vpQuaternionVector
-vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
+vpQuaternionVector vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
{
vpThetaUVector tu(R);
vpColVector u;
@@ -251,7 +232,7 @@ vpQuaternionVector::buildFrom(const vpRotationMatrix &R)
theta *= 0.5;
double sinTheta_2 = sin(theta);
- set( u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta) );
+ set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
return *this;
}
@@ -260,20 +241,19 @@ 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.
\return The inverse quaternion.
*/
-vpQuaternionVector vpQuaternionVector::inverse() const {
+vpQuaternionVector vpQuaternionVector::inverse() const
+{
vpQuaternionVector q_inv;
- double mag_square = w()*w() + x()*x() + y()*y() + z()*z();
- if(!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
+ double mag_square = w() * w() + x() * x() + y() * y() + z() * z();
+ if (!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
q_inv = this->conjugate() / mag_square;
} else {
std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
@@ -287,25 +267,24 @@ 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.
*/
-void vpQuaternionVector::normalize() {
+void vpQuaternionVector::normalize()
+{
double mag = magnitude();
- if(!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
- set( x()/mag, y()/mag, z()/mag, w()/mag );
+ if (!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
+ set(x() / mag, y() / mag, z() / mag, w() / mag);
}
}
//! Returns x-component of the quaternion.
-double vpQuaternionVector::x() const {return data[0];}
+double vpQuaternionVector::x() const { return data[0]; }
//! Returns y-component of the quaternion.
-double vpQuaternionVector::y() const {return data[1];}
+double vpQuaternionVector::y() const { return data[1]; }
//! Returns z-component of the quaternion.
-double vpQuaternionVector::z() const {return data[2];}
+double vpQuaternionVector::z() const { return data[2]; }
//! Returns w-component of the quaternion.
-double vpQuaternionVector::w() const {return data[3];}
+double vpQuaternionVector::w() const { return data[3]; }
diff --git a/modules/core/src/math/transformation/vpRotationMatrix.cpp b/modules/core/src/math/transformation/vpRotationMatrix.cpp
index a57e764..172e54f 100644
--- a/modules/core/src/math/transformation/vpRotationMatrix.cpp
+++ b/modules/core/src/math/transformation/vpRotationMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,33 +42,30 @@
the particular case of rotation matrix
*/
-
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
// Rotation classes
#include <visp3/core/vpRotationMatrix.h>
-
// Exception
#include <visp3/core/vpException.h>
// Debug trace
-#include <visp3/core/vpDebug.h>
#include <math.h>
+#include <visp3/core/vpDebug.h>
const double vpRotationMatrix::threshold = 1e-6;
/*!
Initialize the rotation matrix as identity.
-
+
\sa setIdentity()
*/
-void
-vpRotationMatrix::eye()
+void vpRotationMatrix::eye()
{
- for (unsigned int i=0 ; i < 3 ; i++) {
- for (unsigned int j=0 ; j < 3; j++) {
- if (i==j)
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ if (i == j)
(*this)[i][j] = 1.0;
else
(*this)[i][j] = 0.0;
@@ -84,11 +82,10 @@ vpRotationMatrix::eye()
vpRotationMatrix R2 = R1;
\endcode
*/
-vpRotationMatrix &
-vpRotationMatrix::operator=(const vpRotationMatrix &R)
+vpRotationMatrix &vpRotationMatrix::operator=(const vpRotationMatrix &R)
{
- for (unsigned int i=0; i<3; i++) {
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
rowPtrs[i][j] = R.rowPtrs[i][j];
}
}
@@ -107,28 +104,28 @@ vpRotationMatrix::operator=(const vpRotationMatrix &R)
vpRotationMatrix R = M;
\endcode
- \exception vpException::fatalError If the input matrix is not a rotation matrix.
+ \exception vpException::fatalError If the input matrix is not a rotation
+ matrix.
\sa isARotationMatrix()
*/
-vpRotationMatrix &
-vpRotationMatrix::operator=(const vpMatrix &M)
+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",
+ if ((M.getCols() != 3) && (M.getRows() != 3)) {
+ throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
M.getRows(), M.getCols()));
}
- for (unsigned int i=0; i<3; i++) {
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
(*this)[i][j] = M[i][j];
}
}
if (isARotationMatrix() == false) {
- throw(vpException(vpException::fatalError,
- "Cannot set a rotation matrix from a matrix that is not a rotation matrix"));
+ throw(vpException(vpException::fatalError, "Cannot set a rotation matrix "
+ "from a matrix that is not a "
+ "rotation matrix"));
}
return *this;
@@ -137,22 +134,21 @@ vpRotationMatrix::operator=(const vpMatrix &M)
/*!
Compute the product between two rotation matrices.
*/
-vpRotationMatrix
-vpRotationMatrix::operator*(const vpRotationMatrix &R) const
+vpRotationMatrix vpRotationMatrix::operator*(const vpRotationMatrix &R) const
{
- vpRotationMatrix p ;
-
- for (unsigned int i=0;i<3;i++) {
- for (unsigned int j=0;j<3;j++) {
- double s =0 ;
- for (unsigned int k=0;k<3;k++)
- s +=rowPtrs[i][k] * R.rowPtrs[k][j];
- p[i][j] = s ;
+ vpRotationMatrix p;
+
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < 3; k++)
+ s += rowPtrs[i][k] * R.rowPtrs[k][j];
+ p[i][j] = s;
}
}
return p;
}
-/*!
+/*!
Operator that allows to multiply a rotation matrix by a 3-by-3 matrix.
Allows for example to multiply a rotation matrix by a skew matrix.
\code
@@ -166,22 +162,20 @@ vpRotationMatrix::operator*(const vpRotationMatrix &R) const
a 3-by-3 dimension matrix.
*/
-vpMatrix
-vpRotationMatrix::operator*(const vpMatrix &M) 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",
+ throw(vpException(vpException::dimensionError, "Cannot set a (3x3) rotation matrix from a (%dx%d) matrix",
M.getRows(), M.getCols()));
}
- vpMatrix p(3, 3) ;
-
- for (unsigned int i=0;i<3;i++) {
- for (unsigned int j=0;j<3;j++) {
- double s =0 ;
- for (unsigned int k=0;k<3;k++)
- s +=(*this)[i][k] * M[k][j];
- p[i][j] = s ;
+ vpMatrix p(3, 3);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < 3; k++)
+ s += (*this)[i][k] * M[k][j];
+ p[i][j] = s;
}
}
return p;
@@ -201,8 +195,8 @@ vpRotationMatrix::operator*(const vpMatrix &M) const
The code below shows how to use this operator.
\code
-#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRotationMatrix.h>
int main()
{
@@ -210,25 +204,25 @@ int main()
vpRotationMatrix R;
p2 = R * p1;
-
+
return 0;
}
\endcode
*/
-vpColVector
-vpRotationMatrix::operator*(const vpColVector &v) const
+vpColVector vpRotationMatrix::operator*(const vpColVector &v) const
{
if (v.getRows() != 3) {
- throw (vpException(vpException::dimensionError,
- "Cannot multiply a (3x3) rotation matrix by a %d dimension column vector",
- v.getRows()));
+ throw(vpException(vpException::dimensionError,
+ "Cannot multiply a (3x3) rotation matrix by a %d "
+ "dimension column vector",
+ v.getRows()));
}
vpColVector v_out(3);
- for (unsigned int j=0;j<colNum;j++) {
- double vj = v[j] ; // optimization em 5/12/2006
- for (unsigned int i=0;i<rowNum;i++) {
+ for (unsigned int j = 0; j < colNum; j++) {
+ double vj = v[j]; // optimization em 5/12/2006
+ for (unsigned int i = 0; i < rowNum; i++) {
v_out[i] += rowPtrs[i][j] * vj;
}
}
@@ -236,20 +230,19 @@ vpRotationMatrix::operator*(const vpColVector &v) const
return v_out;
}
-
/*!
- Multiply a rotation matrix by a translation vector and return the resulting translation vector.
+ 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 ;
+ vpTranslationVector p;
- for (unsigned int j=0;j<3;j++)
+ for (unsigned int j = 0; j < 3; j++)
p[j] = 0;
- for (unsigned int j=0;j<3;j++) {
- for (unsigned int i=0;i<3;i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ for (unsigned int i = 0; i < 3; i++) {
p[i] += rowPtrs[i][j] * tv[j];
}
}
@@ -265,9 +258,9 @@ vpRotationMatrix vpRotationMatrix::operator*(double x) const
{
vpRotationMatrix R;
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- R[i][j]= rowPtrs[i][j]*x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ R[i][j] = rowPtrs[i][j] * x;
return R;
}
@@ -276,11 +269,11 @@ vpRotationMatrix vpRotationMatrix::operator*(double x) const
Operator that allows to multiply all the elements of a rotation matrix
by a scalar.
*/
-vpRotationMatrix & vpRotationMatrix::operator*=(double x)
+vpRotationMatrix &vpRotationMatrix::operator*=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
- for(unsigned int j=0;j<colNum;j++)
- rowPtrs[i][j]*=x;
+ for (unsigned int i = 0; i < rowNum; i++)
+ for (unsigned int j = 0; j < colNum; j++)
+ rowPtrs[i][j] *= x;
return *this;
}
@@ -290,152 +283,128 @@ vpRotationMatrix & vpRotationMatrix::operator*=(double x)
/*!
Test if the rotation matrix is really a rotation matrix.
*/
-bool
-vpRotationMatrix::isARotationMatrix() const
+bool vpRotationMatrix::isARotationMatrix() const
{
- unsigned int i,j ;
- bool isRotation = true ;
+ unsigned int i, j;
+ bool isRotation = true;
// test R^TR = Id ;
- vpRotationMatrix RtR = (*this).t()*(*this) ;
- for (i=0 ; i < 3 ; i++) {
- for (j=0 ; j < 3 ; j++) {
- if (i==j) {
- if (fabs(RtR[i][j]-1) > threshold) isRotation = false ;
- }
- else {
- if (fabs(RtR[i][j]) > threshold) isRotation = false ;
+ vpRotationMatrix RtR = (*this).t() * (*this);
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ if (i == j) {
+ if (fabs(RtR[i][j] - 1) > threshold)
+ isRotation = false;
+ } else {
+ if (fabs(RtR[i][j]) > threshold)
+ isRotation = false;
}
}
}
// test if it is a basis
// test || Ci || = 1
- for (i=0 ; i < 3 ; i++) {
- if ((sqrt(vpMath::sqr(RtR[0][i]) +
- vpMath::sqr(RtR[1][i]) +
- vpMath::sqr(RtR[2][i])) - 1) > threshold) isRotation = false ;
+ for (i = 0; i < 3; i++) {
+ if ((sqrt(vpMath::sqr(RtR[0][i]) + vpMath::sqr(RtR[1][i]) + vpMath::sqr(RtR[2][i])) - 1) > threshold)
+ isRotation = false;
}
// test || Ri || = 1
- for (i=0 ; i < 3 ; i++) {
- if ((sqrt(vpMath::sqr(RtR[i][0]) +
- vpMath::sqr(RtR[i][1]) +
- vpMath::sqr(RtR[i][2])) - 1) > threshold) isRotation = false ;
+ for (i = 0; i < 3; i++) {
+ if ((sqrt(vpMath::sqr(RtR[i][0]) + vpMath::sqr(RtR[i][1]) + vpMath::sqr(RtR[i][2])) - 1) > threshold)
+ isRotation = false;
}
// test if the basis is orthogonal
- return isRotation ;
+ return isRotation;
}
-
/*!
Default constructor that initialise a 3-by-3 rotation matrix to identity.
*/
-vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3,3)
-{
- eye();
-}
-
+vpRotationMatrix::vpRotationMatrix() : vpArray2D<double>(3, 3) { eye(); }
/*!
- Copy contructor that construct a 3-by-3 rotation matrix from another rotation matrix.
+ Copy contructor that construct a 3-by-3 rotation matrix from another
+ rotation matrix.
*/
-vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3,3)
-{
- (*this) = M ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpRotationMatrix &M) : vpArray2D<double>(3, 3) { (*this) = M; }
/*!
Construct a 3-by-3 rotation matrix from an homogeneous matrix.
*/
-vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3,3)
-{
- buildFrom(M);
-}
+vpRotationMatrix::vpRotationMatrix(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 3) { buildFrom(M); }
/*!
- Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}\f$ angle representation.
+ Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}\f$ angle
+ representation.
*/
-vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3,3)
-{
- buildFrom(tu) ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpThetaUVector &tu) : vpArray2D<double>(3, 3) { buildFrom(tu); }
/*!
Construct a 3-by-3 rotation matrix from a pose vector.
*/
-vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3,3)
-{
- buildFrom(p) ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpPoseVector &p) : vpArray2D<double>(3, 3) { buildFrom(p); }
/*!
- Construct a 3-by-3 rotation matrix from \f$ R(z,y,z) \f$ Euler angle representation.
+ Construct a 3-by-3 rotation matrix from \f$ R(z,y,z) \f$ Euler angle
+ representation.
*/
-vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3,3)
-{
- buildFrom(euler) ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpRzyzVector &euler) : vpArray2D<double>(3, 3) { buildFrom(euler); }
/*!
- Construct a 3-by-3 rotation matrix from \f$ R(x,y,z) \f$ Euler angle representation.
+ Construct a 3-by-3 rotation matrix from \f$ R(x,y,z) \f$ Euler angle
+ representation.
*/
-vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3,3)
-{
- buildFrom(Rxyz) ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpRxyzVector &Rxyz) : vpArray2D<double>(3, 3) { buildFrom(Rxyz); }
/*!
- Construct a 3-by-3 rotation matrix from \f$ R(z,y,x) \f$ Euler angle representation.
+ Construct a 3-by-3 rotation matrix from \f$ R(z,y,x) \f$ Euler angle
+ representation.
*/
-vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3,3)
-{
- buildFrom(Rzyx) ;
-}
+vpRotationMatrix::vpRotationMatrix(const vpRzyxVector &Rzyx) : vpArray2D<double>(3, 3) { buildFrom(Rzyx); }
/*!
- Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ angle representation.
+ Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}=(\theta u_x,
+ \theta u_y, \theta u_z)^T\f$ angle representation.
*/
-vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3,3)
+vpRotationMatrix::vpRotationMatrix(const double tux, const double tuy, const double tuz) : vpArray2D<double>(3, 3)
{
- buildFrom(tux, tuy, tuz) ;
+ buildFrom(tux, tuy, tuz);
}
/*!
Construct a 3-by-3 rotation matrix from quaternion angle representation.
*/
-vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector& q) : vpArray2D<double>(3,3)
-{
- buildFrom(q);
-}
+vpRotationMatrix::vpRotationMatrix(const vpQuaternionVector &q) : vpArray2D<double>(3, 3) { buildFrom(q); }
/*!
- Return the rotation matrix transpose which is also the inverse of the rotation matrix.
+ Return the rotation matrix transpose which is also the inverse of the
+ rotation matrix.
\sa inverse()
*/
-vpRotationMatrix
-vpRotationMatrix::t() const
+vpRotationMatrix vpRotationMatrix::t() const
{
- vpRotationMatrix Rt ;
+ vpRotationMatrix Rt;
- unsigned int i,j;
- for (i=0;i<3;i++)
- for (j=0;j<3;j++)
+ unsigned int i, j;
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
Rt[j][i] = (*this)[i][j];
return Rt;
}
/*!
- Return the rotation matrix inverse which is also the transpose of the rotation matrix.
+ Return the rotation matrix inverse which is also the transpose of the
+ rotation matrix.
\sa t()
*/
vpRotationMatrix vpRotationMatrix::inverse() const
{
- vpRotationMatrix Ri = (*this).t() ;
+ vpRotationMatrix Ri = (*this).t();
- return Ri ;
+ return Ri;
}
/*!
@@ -455,74 +424,70 @@ int main()
}
\endcode
*/
-void
-vpRotationMatrix::inverse(vpRotationMatrix &R) const
-{
- R = inverse();
-}
+void vpRotationMatrix::inverse(vpRotationMatrix &R) const { R = inverse(); }
/*!
- Print to std::cout the rotation matrix as a \f$ \theta {\bf u} \f$ angle representation vector.
+ Print to std::cout the rotation matrix as a \f$ \theta {\bf u} \f$ angle
+ representation vector.
*/
-void
-vpRotationMatrix::printVector()
+void vpRotationMatrix::printVector()
{
- vpThetaUVector tu(*this) ;
+ vpThetaUVector tu(*this);
- for (unsigned int i=0; i<3; i++)
- std::cout << tu[i] << " " ;
+ for (unsigned int i = 0; i < 3; i++)
+ std::cout << tu[i] << " ";
- std::cout << std::endl ;
+ std::cout << std::endl;
}
-
/*!
- Transform a \f$ \theta {\bf u}\f$ angle representation into a rotation matrix.
+ Transform a \f$ \theta {\bf u}\f$ angle representation into a rotation
+ matrix.
The rotation is computed using :
\f[
- R = \cos{ \theta} \; {I}_{3} + (1 - \cos{ \theta}) \; u u^{T} + \sin{ \theta} \; [u]_\times
- \f]
+ R = \cos{ \theta} \; {I}_{3} + (1 - \cos{ \theta}) \; u u^{T} + \sin{
+ \theta} \; [u]_\times \f]
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpThetaUVector &v)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpThetaUVector &v)
{
- unsigned int i,j;
+ unsigned int i, j;
double theta, si, co, sinc, mcosc;
vpRotationMatrix R;
- theta = sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
+ theta = sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]);
si = sin(theta);
co = cos(theta);
- sinc = vpMath::sinc(si,theta);
- mcosc = vpMath::mcosc(co,theta);
-
- R[0][0] = co + mcosc*v[0]*v[0];
- R[0][1] = -sinc*v[2] + mcosc*v[0]*v[1];
- R[0][2] = sinc*v[1] + mcosc*v[0]*v[2];
- R[1][0] = sinc*v[2] + mcosc*v[1]*v[0];
- R[1][1] = co + mcosc*v[1]*v[1];
- R[1][2] = -sinc*v[0] + mcosc*v[1]*v[2];
- R[2][0] = -sinc*v[1] + mcosc*v[2]*v[0];
- R[2][1] = sinc*v[0] + mcosc*v[2]*v[1];
- R[2][2] = co + mcosc*v[2]*v[2];
-
- for (i=0;i<3;i++) for (j=0;j<3;j++) (*this)[i][j] = R[i][j];
-
- return *this ;
+ sinc = vpMath::sinc(si, theta);
+ mcosc = vpMath::mcosc(co, theta);
+
+ R[0][0] = co + mcosc * v[0] * v[0];
+ R[0][1] = -sinc * v[2] + mcosc * v[0] * v[1];
+ R[0][2] = sinc * v[1] + mcosc * v[0] * v[2];
+ R[1][0] = sinc * v[2] + mcosc * v[1] * v[0];
+ R[1][1] = co + mcosc * v[1] * v[1];
+ R[1][2] = -sinc * v[0] + mcosc * v[1] * v[2];
+ R[2][0] = -sinc * v[1] + mcosc * v[2] * v[0];
+ R[2][1] = sinc * v[0] + mcosc * v[2] * v[1];
+ R[2][2] = co + mcosc * v[2] * v[2];
+
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 3; j++)
+ (*this)[i][j] = R[i][j];
+
+ return *this;
}
/*!
Build a rotation matrix from an homogeneous matrix.
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- (*this)[i][j] = M[i][j] ;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ (*this)[i][j] = M[i][j];
- return *this ;
+ return *this;
}
/*!
@@ -530,8 +495,7 @@ vpRotationMatrix::buildFrom(const vpHomogeneousMatrix &M)
\sa buildFrom(const vpThetaUVector &)
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpPoseVector &p)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpPoseVector &p)
{
vpThetaUVector tu(p);
return buildFrom(tu);
@@ -540,13 +504,13 @@ vpRotationMatrix::buildFrom(const vpPoseVector &p)
/*!
Transform a vector representing the Euler angle
into a rotation matrix.
- Rzyz(\f$ \phi, \theta , \psi \f$) = Rot(\f$ z,\phi \f$) Rot(\f$ y,\theta \f$) Rot(\f$ z,\psi \f$)
+ Rzyz(\f$ \phi, \theta , \psi \f$) = Rot(\f$ z,\phi \f$) Rot(\f$ y,\theta
+ \f$) Rot(\f$ z,\psi \f$)
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRzyzVector &v)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyzVector &v)
{
- double c0,c1,c2,s0,s1,s2;
+ double c0, c1, c2, s0, s1, s2;
c0 = cos(v[0]);
c1 = cos(v[1]);
@@ -555,20 +519,19 @@ vpRotationMatrix::buildFrom(const vpRzyzVector &v)
s1 = sin(v[1]);
s2 = sin(v[2]);
- (*this)[0][0] = c0*c1*c2 - s0*s2;
- (*this)[0][1] = -c0*c1*s2 - s0*c2;
- (*this)[0][2] = c0*s1;
- (*this)[1][0] = s0*c1*c2+c0*s2 ;
- (*this)[1][1] = -s0*c1*s2 + c0*c2 ;
- (*this)[1][2] = s0*s1;
- (*this)[2][0] = -s1*c2;
- (*this)[2][1] = s1*s2;
+ (*this)[0][0] = c0 * c1 * c2 - s0 * s2;
+ (*this)[0][1] = -c0 * c1 * s2 - s0 * c2;
+ (*this)[0][2] = c0 * s1;
+ (*this)[1][0] = s0 * c1 * c2 + c0 * s2;
+ (*this)[1][1] = -s0 * c1 * s2 + c0 * c2;
+ (*this)[1][2] = s0 * s1;
+ (*this)[2][0] = -s1 * c2;
+ (*this)[2][1] = s1 * s2;
(*this)[2][2] = c1;
- return (*this) ;
+ return (*this);
}
-
/*!
Transform a vector representing the Rxyz angle into a rotation
@@ -577,10 +540,9 @@ vpRotationMatrix::buildFrom(const vpRzyzVector &v)
) Rot(\f$ z,\phi \f$)
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRxyzVector &v)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpRxyzVector &v)
{
- double c0,c1,c2,s0,s1,s2;
+ double c0, c1, c2, s0, s1, s2;
c0 = cos(v[0]);
c1 = cos(v[1]);
@@ -589,20 +551,18 @@ vpRotationMatrix::buildFrom(const vpRxyzVector &v)
s1 = sin(v[1]);
s2 = sin(v[2]);
- (*this)[0][0] = c1*c2;
- (*this)[0][1] = -c1*s2;
+ (*this)[0][0] = c1 * c2;
+ (*this)[0][1] = -c1 * s2;
(*this)[0][2] = s1;
- (*this)[1][0] = c0*s2+s0*s1*c2;
- (*this)[1][1] = c0*c2-s0*s1*s2;
- (*this)[1][2] = -s0*c1;
- (*this)[2][0] = -c0*s1*c2+s0*s2;
- (*this)[2][1] = c0*s1*s2+c2*s0;
- (*this)[2][2] = c0*c1;
-
- return (*this) ;
-}
-
+ (*this)[1][0] = c0 * s2 + s0 * s1 * c2;
+ (*this)[1][1] = c0 * c2 - s0 * s1 * s2;
+ (*this)[1][2] = -s0 * c1;
+ (*this)[2][0] = -c0 * s1 * c2 + s0 * s2;
+ (*this)[2][1] = c0 * s1 * s2 + c2 * s0;
+ (*this)[2][2] = c0 * c1;
+ return (*this);
+}
/*!
Transform a vector representing the Rzyx angle
@@ -610,10 +570,9 @@ vpRotationMatrix::buildFrom(const vpRxyzVector &v)
Rxyz(\f$ \phi, \theta , \psi \f$) =
Rot(\f$ z, \psi \f$) Rot(\f$ y, \theta \f$)Rot(\f$ x, \phi \f$)
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpRzyxVector &v)
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpRzyxVector &v)
{
- double c0,c1,c2,s0,s1,s2;
+ double c0, c1, c2, s0, s1, s2;
c0 = cos(v[0]);
c1 = cos(v[1]);
@@ -622,79 +581,75 @@ vpRotationMatrix::buildFrom(const vpRzyxVector &v)
s1 = sin(v[1]);
s2 = sin(v[2]);
- (*this)[0][0] = c0*c1 ;
- (*this)[0][1] = c0*s1*s2 - s0*c2 ;
- (*this)[0][2] = c0*s1*c2 + s0*s2 ;
+ (*this)[0][0] = c0 * c1;
+ (*this)[0][1] = c0 * s1 * s2 - s0 * c2;
+ (*this)[0][2] = c0 * s1 * c2 + s0 * s2;
- (*this)[1][0] = s0*c1 ;
- (*this)[1][1] = s0*s1*s2 + c0*c2 ;
- (*this)[1][2] = s0*s1*c2 - c0*s2 ;
+ (*this)[1][0] = s0 * c1;
+ (*this)[1][1] = s0 * s1 * s2 + c0 * c2;
+ (*this)[1][2] = s0 * s1 * c2 - c0 * s2;
- (*this)[2][0] = -s1 ;
- (*this)[2][1] = c1*s2 ;
- (*this)[2][2] = c1*c2 ;
+ (*this)[2][0] = -s1;
+ (*this)[2][1] = c1 * s2;
+ (*this)[2][2] = c1 * c2;
- return (*this) ;
+ return (*this);
}
-
-
/*!
- Construct a 3-by-3 rotation matrix from \f$ \theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T\f$ angle representation.
+ 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) ;
- return *this ;
+ vpThetaUVector tu(tux, tuy, tuz);
+ buildFrom(tu);
+ return *this;
}
-
/*!
Construct a 3-by-3 rotation matrix from a quaternion representation.
*/
-vpRotationMatrix
-vpRotationMatrix::buildFrom(const vpQuaternionVector& q){
+vpRotationMatrix vpRotationMatrix::buildFrom(const vpQuaternionVector &q)
+{
double a = q.w();
double b = q.x();
double c = q.y();
double d = q.z();
- (*this)[0][0] = a*a+b*b-c*c-d*d;
- (*this)[0][1] = 2*b*c-2*a*d;
- (*this)[0][2] = 2*a*c+2*b*d;
+ (*this)[0][0] = a * a + b * b - c * c - d * d;
+ (*this)[0][1] = 2 * b * c - 2 * a * d;
+ (*this)[0][2] = 2 * a * c + 2 * b * d;
- (*this)[1][0] = 2*a*d+2*b*c;
- (*this)[1][1] = a*a-b*b+c*c-d*d;
- (*this)[1][2] = 2*c*d-2*a*b;
+ (*this)[1][0] = 2 * a * d + 2 * b * c;
+ (*this)[1][1] = a * a - b * b + c * c - d * d;
+ (*this)[1][2] = 2 * c * d - 2 * a * b;
- (*this)[2][0] = 2*b*d-2*a*c;
- (*this)[2][1] = 2*a*b+2*c*d;
- (*this)[2][2] = a*a-b*b-c*c+d*d;
+ (*this)[2][0] = 2 * b * d - 2 * a * c;
+ (*this)[2][1] = 2 * a * b + 2 * c * d;
+ (*this)[2][2] = a * a - b * b - c * c + d * d;
return *this;
}
/*!
Allow to multiply a scalar by a rotation matrix.
*/
-vpRotationMatrix operator*(const double &x,const vpRotationMatrix &R)
+vpRotationMatrix operator*(const double &x, const vpRotationMatrix &R)
{
vpRotationMatrix C;
- unsigned int Rrow = R.getRows() ;
- unsigned int Rcol = R.getCols() ;
+ unsigned int Rrow = R.getRows();
+ unsigned int Rcol = R.getCols();
- for (unsigned int i=0;i<Rrow;i++)
- for(unsigned int j=0;j<Rcol;j++)
- C[i][j]= R[i][j]*x;
+ for (unsigned int i = 0; i < Rrow; i++)
+ for (unsigned int j = 0; j < Rcol; j++)
+ C[i][j] = R[i][j] * x;
- return C ;
+ return C;
}
/*!
- Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation matrix.
+ Return the \f$\theta {\bf u}\f$ vector that corresponds to the rotation
+ matrix.
*/
vpThetaUVector vpRotationMatrix::getThetaUVector()
{
@@ -706,8 +661,8 @@ vpThetaUVector vpRotationMatrix::getThetaUVector()
/*!
Extract a column vector from a rotation matrix.
\warning All the indexes start from 0 in this function.
- \param j : Index of the column to extract. If j=0, the first column is extracted.
- \return The extracted column vector.
+ \param j : Index of the column to extract. If j=0, the first column is
+extracted. \return The extracted column vector.
The following example shows how to use this function:
\code
@@ -730,15 +685,13 @@ Last column:
1
\endcode
*/
-vpColVector
-vpRotationMatrix::getCol(const unsigned int j) const
+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++)
+ for (unsigned int i = 0; i < nb_rows; i++)
c[i] = (*this)[i][j];
return c;
}
@@ -752,10 +705,6 @@ vpRotationMatrix::getCol(const unsigned int j) const
\sa eye()
*/
-void
-vpRotationMatrix::setIdentity()
-{
- eye();
-}
+void vpRotationMatrix::setIdentity() { eye(); }
#endif //#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
diff --git a/modules/core/src/math/transformation/vpRotationVector.cpp b/modules/core/src/math/transformation/vpRotationVector.cpp
index 6f1cb09..1f78243 100644
--- a/modules/core/src/math/transformation/vpRotationVector.cpp
+++ b/modules/core/src/math/transformation/vpRotationVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,8 +39,8 @@
#include <algorithm>
#include <math.h>
-#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRotationVector.h>
#include <visp3/core/vpRowVector.h>
/*!
@@ -48,8 +49,6 @@
(cannot be used as is !).
*/
-
-
/*!
Return the transpose of the rotation vector.
@@ -58,26 +57,27 @@ vpRowVector vpRotationVector::t() const
{
vpRowVector v(dsize);
- for (unsigned int i=0; i< dsize; i++)
+ for (unsigned int i = 0; i < dsize; i++)
v[i] = data[i];
return v;
}
/*!
- Operator that allows to multiply each element of a rotation vector by a scalar.
+ Operator that allows to multiply each element of a rotation vector by a
+ scalar.
\param x : The scalar.
- \return The rotation vector multiplied by the scalar as a column vector. The current
- rotation vector (*this) is unchanged.
+ \return The rotation vector multiplied by the scalar as a column vector. The
+ current rotation vector (*this) is unchanged.
*/
vpColVector vpRotationVector::operator*(double x) const
{
vpColVector v(dsize);
- for (unsigned int i=0;i<dsize;i++)
+ for (unsigned int i = 0; i < dsize; i++)
v[i] = (*this)[i] * x;
return v;
}
@@ -86,25 +86,26 @@ vpColVector vpRotationVector::operator*(double x) const
\relates vpRotationVector
Allows to multiply a scalar by rotaion vector.
*/
-vpColVector operator*(const double &x,const vpRotationVector &v)
+vpColVector operator*(const double &x, const vpRotationVector &v)
{
- vpColVector vout ;
- vout = v*x ;
- return vout ;
+ vpColVector vout;
+ vout = v * x;
+ return vout;
}
/*!
- Return the sum square of all the elements \f$r_{i}\f$ of the rotation vector r(m).
+ Return the sum square of all the elements \f$r_{i}\f$ of the rotation vector
+ r(m).
\return The value \f[\sum{i=0}^{m} r_i^{2}\f].
*/
double vpRotationVector::sumSquare() const
{
- double sum_square=0.0;
+ double sum_square = 0.0;
- for (unsigned int i=0;i<rowNum;i++) {
- double x=rowPtrs[i][0];
- sum_square += x*x;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double x = rowPtrs[i][0];
+ sum_square += x * x;
}
return sum_square;
diff --git a/modules/core/src/math/transformation/vpRxyzVector.cpp b/modules/core/src/math/transformation/vpRxyzVector.cpp
index 74fb40d..c84bc29 100644
--- a/modules/core/src/math/transformation/vpRxyzVector.cpp
+++ b/modules/core/src/math/transformation/vpRxyzVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,19 +44,15 @@
/*!
\file vpRxyzVector.cpp
- \brief class that consider the case of the Rxyz angle parameterization for the rotation :
- Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
+ \brief class that consider the case of the Rxyz angle parameterization for
+ the rotation : Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi)
*/
/*! Default constructor that initialize all the 3 angles to zero. */
-vpRxyzVector::vpRxyzVector()
- : vpRotationVector(3)
-{}
+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).
@@ -63,22 +60,17 @@ vpRxyzVector::vpRxyzVector(const vpRxyzVector &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);
}
-/*!
+/*!
Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
angles from a rotation matrix.
\param R : Rotation matrix used to initialize the Euler angles.
*/
-vpRxyzVector::vpRxyzVector(const vpRotationMatrix& R)
- : vpRotationVector (3)
-{
- buildFrom(R) ;
-}
+vpRxyzVector::vpRxyzVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); }
/*!
Constructor that initialize \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
@@ -86,64 +78,59 @@ vpRxyzVector::vpRxyzVector(const vpRotationMatrix& R)
\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)
+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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rxyz[i];
}
-/*!
+/*!
Convert a rotation matrix into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler
angles vector.
-
+
\param R : Rotation matrix used as input.
- \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+ \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRxyzVector
-vpRxyzVector::buildFrom(const vpRotationMatrix& R)
+vpRxyzVector vpRxyzVector::buildFrom(const vpRotationMatrix &R)
{
double COEF_MIN_ROT = 1e-6;
- double phi ;
+ double phi;
- if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
- else phi = atan2(-R[1][2], R[2][2]) ;
+ if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT))
+ phi = 0;
+ else
+ phi = atan2(-R[1][2], R[2][2]);
- double si = sin(phi) ;
- double co = cos(phi) ;
- double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
- double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);
+ double si = sin(phi);
+ double co = cos(phi);
+ double theta = atan2(R[0][2], -si * R[1][2] + co * R[2][2]);
+ double psi = atan2(co * R[1][0] + si * R[2][0], co * R[1][1] + si * R[2][1]);
buildFrom(phi, theta, psi);
- return *this ;
+ return *this;
}
-/*!
- Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{xyz}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
- input.
- \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a
+ \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector. \param tu :
+ \f$\theta {\bf u}\f$ representation of a rotation used here as input.
+ \return \f$R_{xyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRxyzVector
-vpRxyzVector::buildFrom(const vpThetaUVector& tu)
+vpRxyzVector vpRxyzVector::buildFrom(const vpThetaUVector &tu)
{
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
+ vpRotationMatrix R;
+ R.buildFrom(tu);
+ buildFrom(R);
- return *this ;
+ return *this;
}
/*!
@@ -152,12 +139,11 @@ 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 ;
- data[2] = psi ;
+ data[0] = phi;
+ data[1] = theta;
+ data[2] = psi;
}
/*!
@@ -181,7 +167,7 @@ int main()
*/
vpRxyzVector &vpRxyzVector::operator=(double v)
{
- for (unsigned int i=0; i< dsize; i++)
+ for (unsigned int i = 0; i < dsize; i++)
data[i] = v;
return *this;
@@ -192,7 +178,8 @@ vpRxyzVector &vpRxyzVector::operator=(double v)
Copy operator that initializes a \f$R_{xyz}=(\varphi,\theta,\psi)\f$
Euler angles vector from a 3-dimension column vector.
- \param rxyz : 3-dimension vector containing the values of the rotation vector.
+ \param rxyz : 3-dimension vector containing the values of the rotation
+vector.
\code
#include <visp3/core/vpRxyzVector.h>
@@ -212,9 +199,10 @@ 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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rxyz[i];
return *this;
diff --git a/modules/core/src/math/transformation/vpRzyxVector.cpp b/modules/core/src/math/transformation/vpRzyxVector.cpp
index 72b76d6..b7a9475 100644
--- a/modules/core/src/math/transformation/vpRzyxVector.cpp
+++ b/modules/core/src/math/transformation/vpRzyxVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,25 +38,20 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpRzyxVector.h>
#include <math.h>
+#include <visp3/core/vpRzyxVector.h>
/*!
\file vpRzyxVector.cpp
- \brief class that consider the case of the Rzyx angle parameterization for the rotation :
- Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta,Rot(x,psi)
+ \brief class that consider the case of the Rzyx angle parameterization for
+ the rotation : Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta,Rot(x,psi)
*/
/*! Default constructor that initialize all the 3 angles to zero. */
-vpRzyxVector::vpRzyxVector()
- : vpRotationVector(3)
-{}
+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).
@@ -63,22 +59,17 @@ vpRzyxVector::vpRzyxVector(const vpRzyxVector &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);
}
-/*!
+/*!
Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
angles from a rotation matrix.
\param R : Rotation matrix used to initialize the Euler angles.
*/
-vpRzyxVector::vpRzyxVector(const vpRotationMatrix& R)
- : vpRotationVector (3)
-{
- buildFrom(R) ;
-}
+vpRzyxVector::vpRzyxVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); }
/*!
Constructor that initialize \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
@@ -86,74 +77,66 @@ vpRzyxVector::vpRzyxVector(const vpRotationMatrix& R)
\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)
+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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rzyx[i];
}
-/*!
+/*!
Convert a rotation matrix into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
angles vector.
-
+
Source: R. Paul, Robot Manipulators: Mathematics, Programming, and Control.
MIT Press, 1981, p. 71
\param R : Rotation matrix used as input.
- \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+ \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRzyxVector
-vpRzyxVector::buildFrom(const vpRotationMatrix& R)
+vpRzyxVector vpRzyxVector::buildFrom(const vpRotationMatrix &R)
{
double nx = R[0][0];
double ny = R[1][0];
- double phi = atan2(ny,nx) ;
- double si = sin(phi) ;
- double co = cos(phi) ;
+ double phi = atan2(ny, nx);
+ double si = sin(phi);
+ double co = cos(phi);
double nz = R[2][0];
- double theta = atan2(-nz, co*nx+si*ny) ;
+ double theta = atan2(-nz, co * nx + si * ny);
double ax = R[0][2];
double ay = R[1][2];
double ox = R[0][1];
double oy = R[1][1];
- double psi = atan2(si*ax-co*ay,-si*ox+co*oy);
+ double psi = atan2(si * ax - co * ay, -si * ox + co * oy);
buildFrom(phi, theta, psi);
- return *this ;
+ return *this;
}
-
-/*!
- Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyx}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
- input.
- \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a
+ \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector. \param tu :
+ \f$\theta {\bf u}\f$ representation of a rotation used here as input.
+ \return \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRzyxVector
-vpRzyxVector::buildFrom(const vpThetaUVector& tu)
+vpRzyxVector vpRzyxVector::buildFrom(const vpThetaUVector &tu)
{
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
-
- return *this ;
+ vpRotationMatrix R;
+ R.buildFrom(tu);
+ buildFrom(R);
+
+ return *this;
}
/*!
@@ -162,12 +145,11 @@ 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 ;
- data[2] = psi ;
+ data[0] = phi;
+ data[1] = theta;
+ data[2] = psi;
}
/*!
@@ -191,7 +173,7 @@ int main()
*/
vpRzyxVector &vpRzyxVector::operator=(double v)
{
- for (unsigned int i=0; i< dsize; i++)
+ for (unsigned int i = 0; i < dsize; i++)
data[i] = v;
return *this;
@@ -202,7 +184,8 @@ vpRzyxVector &vpRzyxVector::operator=(double v)
Copy operator that initializes a \f$R_{zyx}=(\varphi,\theta,\psi)\f$ Euler
angles vector from a 3-dimension column vector.
- \param rzyx : 3-dimension vector containing the values of the rotation vector.
+ \param rzyx : 3-dimension vector containing the values of the rotation
+vector.
\code
#include <visp3/core/vpRzyxVector.h>
@@ -222,9 +205,10 @@ 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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rzyx[i];
return *this;
diff --git a/modules/core/src/math/transformation/vpRzyzVector.cpp b/modules/core/src/math/transformation/vpRzyzVector.cpp
index 53fc79e..e2a5b66 100644
--- a/modules/core/src/math/transformation/vpRzyzVector.cpp
+++ b/modules/core/src/math/transformation/vpRzyzVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,24 +38,19 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpRzyzVector.h>
#include <math.h>
+#include <visp3/core/vpRzyzVector.h>
/*!
\file vpRzyzVector.cpp
- \brief class that consider the case of the Rzyz angle parameterization for the rotation :
- Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
+ \brief class that consider the case of the Rzyz angle parameterization for
+ the rotation : Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
*/
/*! Default constructor that initialize all the 3 angles to zero. */
-vpRzyzVector::vpRzyzVector()
- : vpRotationVector (3)
-{}
+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).
@@ -62,22 +58,17 @@ vpRzyzVector::vpRzyzVector(const vpRzyzVector &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);
}
-/*!
+/*!
Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
angles from a rotation matrix.
\param R : Rotation matrix used to initialize the Euler angles.
*/
-vpRzyzVector::vpRzyzVector(const vpRotationMatrix& R)
- : vpRotationVector (3)
-{
- buildFrom(R) ;
-}
+vpRzyzVector::vpRzyzVector(const vpRotationMatrix &R) : vpRotationVector(3) { buildFrom(R); }
/*!
Constructor that initialize \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
@@ -85,66 +76,58 @@ vpRzyzVector::vpRzyzVector(const vpRotationMatrix& R)
\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)
+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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rzyz[i];
}
-/*!
+/*!
Convert a rotation matrix into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler
angles vector.
-
+
\param R : Rotation matrix used as input.
- \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+ \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRzyzVector
-vpRzyzVector::buildFrom(const vpRotationMatrix& R)
+vpRzyzVector vpRzyzVector::buildFrom(const vpRotationMatrix &R)
{
- double phi ;
- if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
- phi = 0 ;
+ double phi;
+ if ((fabs(R[1][2]) < 1e-6) && (fabs(R[0][2]) < 1e-6))
+ phi = 0;
else
- phi = atan2(R[1][2],R[0][2]) ;
- double cphi = cos(phi) ;
- double sphi = sin(phi) ;
+ phi = atan2(R[1][2], R[0][2]);
+ double cphi = cos(phi);
+ double sphi = sin(phi);
- double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);
+ double 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);
- return *this ;
+ return *this;
}
-
-/*!
- Convert a \f$\theta {\bf u}\f$ vector into a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
- Euler angles vector.
- \param tu : \f$\theta {\bf u}\f$ representation of a rotation used here as
- input.
- \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
+/*!
+ Convert a \f$\theta {\bf u}\f$ vector into a
+ \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector. \param tu :
+ \f$\theta {\bf u}\f$ representation of a rotation used here as input.
+ \return \f$R_{zyz}=(\varphi,\theta,\psi)\f$ Euler angles vector.
*/
-vpRzyzVector
-vpRzyzVector::buildFrom(const vpThetaUVector& tu)
+vpRzyzVector vpRzyzVector::buildFrom(const vpThetaUVector &tu)
{
- vpRotationMatrix R ;
- R.buildFrom(tu) ;
- buildFrom(R) ;
+ vpRotationMatrix R;
+ R.buildFrom(tu);
+ buildFrom(R);
- return *this ;
+ return *this;
}
/*!
@@ -168,7 +151,7 @@ int main()
*/
vpRzyzVector &vpRzyzVector::operator=(double v)
{
- for (unsigned int i=0; i< dsize; i++)
+ for (unsigned int i = 0; i < dsize; i++)
data[i] = v;
return *this;
@@ -180,12 +163,11 @@ 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 ;
- data[2] = psi ;
+ data[0] = phi;
+ data[1] = theta;
+ data[2] = psi;
}
/*!
@@ -193,7 +175,8 @@ vpRzyzVector::buildFrom(const double phi, const double theta, const double psi)
Copy operator that initializes a \f$R_{zyz}=(\varphi,\theta,\psi)\f$
Euler angles vector from a 3-dimension column vector.
- \param rzyz : 3-dimension vector containing the values of the rotation vector.
+ \param rzyz : 3-dimension vector containing the values of the rotation
+vector.
\code
#include <visp3/core/vpRzyzVector.h>
@@ -213,9 +196,10 @@ 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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = rzyz[i];
return *this;
diff --git a/modules/core/src/math/transformation/vpThetaUVector.cpp b/modules/core/src/math/transformation/vpThetaUVector.cpp
index 44dbf9c..14dd03a 100644
--- a/modules/core/src/math/transformation/vpThetaUVector.cpp
+++ b/modules/core/src/math/transformation/vpThetaUVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,99 +42,66 @@
rotation
*/
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <visp3/core/vpThetaUVector.h>
const double vpThetaUVector::minimum = 0.0001;
/*! Default constructor that initialize all the 3 angles to zero. */
-vpThetaUVector::vpThetaUVector()
- : vpRotationVector(3)
-{}
+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)
+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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = tu[i];
}
/*!
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);
}
@@ -141,126 +109,120 @@ vpThetaUVector::vpThetaUVector(const double tux, const double tuy, const double
/*!
Converts an homogeneous matrix into a \f$\theta {\bf u}\f$ vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpHomogeneousMatrix& M)
+vpThetaUVector vpThetaUVector::buildFrom(const vpHomogeneousMatrix &M)
{
vpRotationMatrix R;
M.extract(R);
buildFrom(R);
- return *this ;
+ return *this;
}
/*!
Converts a pose vector into a \f$\theta {\bf u}\f$ vector copying
the \f$\theta {\bf u}\f$ values contained in the pose vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpPoseVector& p)
+vpThetaUVector vpThetaUVector::buildFrom(const vpPoseVector &p)
{
- for(unsigned int i=0; i<3; i++)
- data[i] = p[i+3];
+ for (unsigned int i = 0; i < 3; i++)
+ data[i] = p[i + 3];
- return *this ;
+ return *this;
}
/*!
Converts a rotation matrix into a \f$\theta {\bf u}\f$ vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRotationMatrix& R)
+vpThetaUVector vpThetaUVector::buildFrom(const vpRotationMatrix &R)
{
- double s,c,theta;
+ 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])
- + (R[2][1]-R[1][2])*(R[2][1]-R[1][2]);
- s = sqrt(s)/2.0;
- c = (R[0][0]+R[1][1]+R[2][2]-1.0)/2.0;
- theta=atan2(s,c); /* theta in [0, PI] since s > 0 */
+ s = (R[1][0] - R[0][1]) * (R[1][0] - R[0][1]) + (R[2][0] - R[0][2]) * (R[2][0] - R[0][2]) +
+ (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
+ s = sqrt(s) / 2.0;
+ c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
+ theta = atan2(s, c); /* theta in [0, PI] since s > 0 */
// General case when theta != pi. If theta=pi, c=-1
- if ( (1+c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
+ if ((1 + c) > minimum) // Since -1 <= c <= 1, no fabs(1+c) is required
{
- double sinc = vpMath::sinc(s,theta);
+ double sinc = vpMath::sinc(s, theta);
- data[0] = (R[2][1]-R[1][2])/(2*sinc);
- data[1] = (R[0][2]-R[2][0])/(2*sinc);
- data[2] = (R[1][0]-R[0][1])/(2*sinc);
- }
- else /* theta near PI */
+ data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
+ data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
+ data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
+ } else /* theta near PI */
{
- if ( (R[0][0]-c) < std::numeric_limits<double>::epsilon() )
+ if ((R[0][0] - c) < std::numeric_limits<double>::epsilon())
data[0] = 0.;
else
- data[0] = theta*(sqrt((R[0][0]-c)/(1-c)));
- if ((R[2][1]-R[1][2]) < 0) data[0] = -data[0];
+ data[0] = theta * (sqrt((R[0][0] - c) / (1 - c)));
+ if ((R[2][1] - R[1][2]) < 0)
+ data[0] = -data[0];
- if ( (R[1][1]-c) < std::numeric_limits<double>::epsilon() )
+ if ((R[1][1] - c) < std::numeric_limits<double>::epsilon())
data[1] = 0.;
else
- data[1] = theta*(sqrt((R[1][1]-c)/(1-c)));
+ data[1] = theta * (sqrt((R[1][1] - c) / (1 - c)));
- if ((R[0][2]-R[2][0]) < 0) data[1] = -data[1];
+ if ((R[0][2] - R[2][0]) < 0)
+ data[1] = -data[1];
- if ( (R[2][2]-c) < std::numeric_limits<double>::epsilon() )
+ if ((R[2][2] - c) < std::numeric_limits<double>::epsilon())
data[2] = 0.;
else
- data[2] = theta*(sqrt((R[2][2]-c)/(1-c)));
+ data[2] = theta * (sqrt((R[2][2] - c) / (1 - c)));
- if ((R[1][0]-R[0][1]) < 0) data[2] = -data[2];
+ if ((R[1][0] - R[0][1]) < 0)
+ data[2] = -data[2];
}
- return *this ;
+ return *this;
}
-/*!
+/*!
Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-x
representation vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRzyxVector& rzyx)
+vpThetaUVector vpThetaUVector::buildFrom(const vpRzyxVector &rzyx)
{
- vpRotationMatrix R(rzyx) ;
+ vpRotationMatrix R(rzyx);
- buildFrom(R) ;
- return *this ;
+ buildFrom(R);
+ return *this;
}
-/*!
+/*!
Build a \f$\theta {\bf u}\f$ vector from an Euler z-y-z
representation vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRzyzVector& rzyz)
+vpThetaUVector vpThetaUVector::buildFrom(const vpRzyzVector &rzyz)
{
- vpRotationMatrix R(rzyz) ;
+ vpRotationMatrix R(rzyz);
- buildFrom(R) ;
- return *this ;
+ buildFrom(R);
+ return *this;
}
/*!
Build a \f$\theta {\bf u}\f$ vector from an Euler x-y-z
representation vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpRxyzVector& rxyz)
+vpThetaUVector vpThetaUVector::buildFrom(const vpRxyzVector &rxyz)
{
- vpRotationMatrix R(rxyz) ;
+ vpRotationMatrix R(rxyz);
- buildFrom(R) ;
- return *this ;
+ buildFrom(R);
+ return *this;
}
/*!
Build a \f$\theta {\bf u}\f$ vector from a quaternion
representation vector.
*/
-vpThetaUVector
-vpThetaUVector::buildFrom(const vpQuaternionVector& q)
+vpThetaUVector vpThetaUVector::buildFrom(const vpQuaternionVector &q)
{
- vpRotationMatrix R(q) ;
+ vpRotationMatrix R(q);
- buildFrom(R) ;
- return *this ;
+ buildFrom(R);
+ return *this;
}
/*!
@@ -286,7 +248,7 @@ int main()
*/
vpThetaUVector &vpThetaUVector::operator=(double v)
{
- for (unsigned int i=0; i< dsize; i++)
+ for (unsigned int i = 0; i < dsize; i++)
data[i] = v;
return *this;
@@ -294,9 +256,11 @@ vpThetaUVector &vpThetaUVector::operator=(double v)
/*!
- Copy operator that initializes a \f$\theta {\bf u}\f$ vector from a 3-dimension column vector \e tu.
+ Copy operator that initializes a \f$\theta {\bf u}\f$ vector from a
+3-dimension column vector \e tu.
- \param tu : 3-dimension vector containing the values of the \f$\theta {\bf u}\f$ vector.
+ \param tu : 3-dimension vector containing the values of the \f$\theta {\bf
+u}\f$ vector.
\code
#include <visp3/core/vpThetaUVector.h>
@@ -316,9 +280,10 @@ 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++)
+ for (unsigned int i = 0; i < 3; i++)
data[i] = tu[i];
return *this;
@@ -352,24 +317,24 @@ int main()
\sa getTheta(), getU()
*/
-void
-vpThetaUVector::extract(double &theta, vpColVector &u) const
+void vpThetaUVector::extract(double &theta, vpColVector &u) const
{
u.resize(3);
theta = getTheta();
- //if (theta == 0) {
+ // if (theta == 0) {
if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
u = 0;
return;
}
- for (unsigned int i=0 ; i < 3 ; i++)
- u[i] = data[i] / theta ;
+ for (unsigned int i = 0; i < 3; i++)
+ u[i] = data[i] / theta;
}
/*!
- Get the rotation angle \f$ \theta \f$ from the \f$ \theta {\bf u} \f$ representation.
+ Get the rotation angle \f$ \theta \f$ from the \f$ \theta {\bf u} \f$
+representation.
\return Rotation angle \f$ \theta \f$ in rad.
@@ -388,14 +353,12 @@ int main()
\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]); }
/*!
- Get the unit vector \f$\bf u \f$ from the \f$ \theta {\bf u} \f$ representation.
+ Get the unit vector \f$\bf u \f$ from the \f$ \theta {\bf u} \f$
+representation.
\return 3-dim unit vector \f${\bf u} = (u_{x},u_{y},u_{z})^{\top} \f$
representing the rotation axis.
@@ -406,7 +369,7 @@ double vpThetaUVector::getTheta() const
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;
@@ -420,22 +383,20 @@ vpColVector vpThetaUVector::getU() const
vpColVector u(3);
double theta = getTheta();
- //if (theta == 0) {
+ // if (theta == 0) {
if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
u = 0;
return u;
}
- for (unsigned int i=0 ; i < 3 ; i++)
+ for (unsigned int i = 0; i < 3; i++)
u[i] = data[i] / theta;
return u;
}
-
/*!
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 f0c51d2..3215133 100644
--- a/modules/core/src/math/transformation/vpTranslationVector.cpp
+++ b/modules/core/src/math/transformation/vpTranslationVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#include <stdio.h>
#include <string.h>
@@ -50,11 +50,11 @@
/*!
Construct a translation vector \f$ \bf t \f$ from 3 doubles.
- \param tx,ty,tz : Translation respectively along x, y and z axis. Values are in meters.
+ \param tx,ty,tz : Translation respectively along x, y and z axis. Values are
+ in meters.
*/
-vpTranslationVector::vpTranslationVector(const double tx, const double ty, const double tz)
- : vpArray2D<double>(3, 1)
+vpTranslationVector::vpTranslationVector(const double tx, const double ty, const double tz) : vpArray2D<double>(3, 1)
{
(*this)[0] = tx;
(*this)[1] = ty;
@@ -62,27 +62,22 @@ vpTranslationVector::vpTranslationVector(const double tx, const double ty, const
}
/*!
- Construct a translation vector \f$ \bf t \f$ from the translation contained in
- an homogeneous matrix.
+ Construct a translation vector \f$ \bf t \f$ from the translation contained
+ in an homogeneous matrix.
\param M : Homogeneous matrix where translations are in meters.
*/
-vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M)
- : vpArray2D<double>(3, 1)
-{
- M.extract( *this );
-}
+vpTranslationVector::vpTranslationVector(const vpHomogeneousMatrix &M) : vpArray2D<double>(3, 1) { M.extract(*this); }
/*!
- Construct a translation vector \f$ \bf t \f$ from the translation contained in
- a pose vector.
+ Construct a translation vector \f$ \bf t \f$ from the translation contained
+ in a pose vector.
\param p : Pose vector where translations are in meters.
*/
-vpTranslationVector::vpTranslationVector(const vpPoseVector &p)
- : vpArray2D<double>(3, 1)
+vpTranslationVector::vpTranslationVector(const vpPoseVector &p) : vpArray2D<double>(3, 1)
{
(*this)[0] = p[0];
(*this)[1] = p[1];
@@ -99,13 +94,11 @@ vpTranslationVector::vpTranslationVector(const vpPoseVector &p)
vpTranslationVector t2(t1); // t2 is now a copy of t1
\endcode
*/
-vpTranslationVector::vpTranslationVector (const vpTranslationVector &tv)
- : vpArray2D<double>(tv)
-{
-}
+vpTranslationVector::vpTranslationVector(const vpTranslationVector &tv) : vpArray2D<double>(tv) {}
/*!
- Construct a translation vector \f$ \bf t \f$ from a 3-dimension column vector.
+ Construct a translation vector \f$ \bf t \f$ from a 3-dimension column
+ vector.
\param v : 3-dimension column vector.
@@ -117,12 +110,13 @@ vpTranslationVector::vpTranslationVector (const vpTranslationVector &tv)
\endcode
*/
-vpTranslationVector::vpTranslationVector (const vpColVector &v)
- : vpArray2D<double>(v)
+vpTranslationVector::vpTranslationVector(const vpColVector &v) : vpArray2D<double>(v)
{
if (v.size() != 3) {
throw(vpException(vpException::dimensionError,
- "Cannot construct a translation vector from a %d-dimension column vector", v.size()));
+ "Cannot construct a translation vector from a "
+ "%d-dimension column vector",
+ v.size()));
}
}
@@ -137,11 +131,10 @@ 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 ;
+ M.extract(*this);
+ return *this;
}
/*!
@@ -153,13 +146,12 @@ vpTranslationVector::buildFrom(const vpHomogeneousMatrix& M)
\return The build translation vector.
*/
-vpTranslationVector
-vpTranslationVector::buildFrom(const vpPoseVector& p)
+vpTranslationVector vpTranslationVector::buildFrom(const vpPoseVector &p)
{
(*this)[0] = p[0];
(*this)[1] = p[1];
(*this)[2] = p[2];
- return *this ;
+ return *this;
}
/*!
@@ -171,8 +163,7 @@ vpTranslationVector::buildFrom(const vpPoseVector& p)
\return The build translation vector.
*/
-vpTranslationVector
-vpTranslationVector::buildFrom(const vpColVector& v)
+vpTranslationVector vpTranslationVector::buildFrom(const vpColVector &v)
{
if (v.size() != 3) {
throw(vpException(vpException::dimensionError,
@@ -182,7 +173,7 @@ vpTranslationVector::buildFrom(const vpColVector& v)
(*this)[0] = v[0];
(*this)[1] = v[1];
(*this)[2] = v[2];
- return *this ;
+ return *this;
}
/*!
@@ -193,11 +184,10 @@ 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 ;
+ return *this;
}
/*!
@@ -206,12 +196,11 @@ vpTranslationVector::buildFrom(const double tx, const double ty, const double tz
\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 ;
- (*this)[2] = tz ;
+ (*this)[0] = tx;
+ (*this)[1] = ty;
+ (*this)[2] = tz;
}
/*!
@@ -219,8 +208,9 @@ vpTranslationVector::set(const double tx, const double ty, const double tz)
\param tv : Translation vector to add.
- \return The sum of the current translation vector (*this) and the one to add.
- \code
+ \return The sum of the current translation vector (*this) and the one to
+ add.
+\code
vpTranslationVector t1(1,2,3);
vpTranslationVector t2(4,5,6);
vpTranslationVector t3;
@@ -231,12 +221,12 @@ vpTranslationVector::set(const double tx, const double ty, const double tz)
\endcode
*/
-vpTranslationVector
-vpTranslationVector::operator+(const vpTranslationVector &tv) const
+vpTranslationVector vpTranslationVector::operator+(const vpTranslationVector &tv) const
{
vpTranslationVector s;
- for (unsigned int i=0;i<3;i++) s[i] = (*this)[i]+tv[i] ;
+ for (unsigned int i = 0; i < 3; i++)
+ s[i] = (*this)[i] + tv[i];
return s;
}
@@ -245,11 +235,14 @@ vpTranslationVector::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
+ \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;
+ v[0] = 4;
+ v[1] = 5;
+ v[2] = 6;
vpTranslationVector t2;
t2 = t1 + v;
@@ -258,16 +251,16 @@ vpTranslationVector::operator+(const vpTranslationVector &tv) const
\endcode
*/
-vpTranslationVector
-vpTranslationVector::operator+(const vpColVector &v) 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;
- for (unsigned int i=0;i<3;i++) s[i] = (*this)[i]+v[i] ;
+ for (unsigned int i = 0; i < 3; i++)
+ s[i] = (*this)[i] + v[i];
return s;
}
@@ -277,29 +270,29 @@ 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.
+ \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;
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2(4,5,6);
+ vpTranslationVector t3;
- t3 = t2 - t1;
+ t3 = t2 - t1;
// t1 and t2 leave unchanged
// t3 is now equal to : 3, 3, 3
\endcode
*/
-vpTranslationVector
-vpTranslationVector::operator-(const vpTranslationVector &tv) const
+vpTranslationVector vpTranslationVector::operator-(const vpTranslationVector &tv) const
{
- vpTranslationVector sub ;
+ vpTranslationVector sub;
- for (unsigned int i=0;i<3;i++) sub[i] = (*this)[i]-tv[i] ;
+ for (unsigned int i = 0; i < 3; i++)
+ sub[i] = (*this)[i] - tv[i];
return sub;
}
-
/*!
Operator that allows to negate a translation vector.
@@ -307,26 +300,26 @@ vpTranslationVector::operator-(const vpTranslationVector &tv) const
(*this) is unchanged.
\code
- vpTranslationVector t1(1,2,3);
- vpTranslationVector t2;
+ vpTranslationVector t1(1,2,3);
+ vpTranslationVector t2;
t2 = -t1;
- // t1 is unchanged
+ // t1 is unchanged
// t2 is now equal to : [-1, -2, -3]
\endcode
*/
-vpTranslationVector vpTranslationVector::operator-() const //negate
+vpTranslationVector vpTranslationVector::operator-() const // negate
{
vpTranslationVector tv;
- for (unsigned int i=0;i<dsize;i++)
- {
- *(tv.data + i) = -*(data + i) ;
+ for (unsigned int i = 0; i < dsize; i++) {
+ *(tv.data + i) = -*(data + i);
}
return tv;
}
/*!
- Operator that allows to multiply each element of a translation vector by a scalar.
+ Operator that allows to multiply each element of a translation vector by a
+ scalar.
\param x : The scalar.
@@ -334,17 +327,17 @@ vpTranslationVector vpTranslationVector::operator-() const //negate
translation vector (*this) is unchanged.
\code
- vpTranslationVector t1(1,2,3);
+ vpTranslationVector t1(1,2,3);
t2 = t1 * 3;
- // t1 is unchanged
+ // t1 is unchanged
// t2 is now equal to : [3, 6, 9]
\endcode
*/
-vpTranslationVector vpTranslationVector::operator*(const double x) const
+vpTranslationVector vpTranslationVector::operator*(const double x) const
{
vpTranslationVector tv;
- for (unsigned int i=0;i<dsize;i++) {
- *(tv.data + i) = (*(data + i)) * x ;
+ for (unsigned int i = 0; i < dsize; i++) {
+ *(tv.data + i) = (*(data + i)) * x;
}
return tv;
@@ -362,8 +355,8 @@ vpTranslationVector vpTranslationVector::operator*(const double x) const
vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const
{
vpMatrix M(rowNum, v.getCols());
- for (unsigned int i=0; i<rowNum; i++) {
- for (unsigned int j=0; j<v.getCols(); j++) {
+ for (unsigned int i = 0; i < rowNum; i++) {
+ for (unsigned int j = 0; j < v.getCols(); j++) {
M[i][j] = (*this)[i] * v[j];
}
}
@@ -371,7 +364,8 @@ vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const
}
/*!
- Operator that allows to multiply each element of a translation vector by a scalar.
+ Operator that allows to multiply each element of a translation vector by a
+ scalar.
\param x : The scalar.
@@ -379,12 +373,13 @@ vpMatrix vpTranslationVector::operator*(const vpRowVector &v) const
*/
vpTranslationVector &vpTranslationVector::operator*=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] *= x;
return (*this);
}
/*!
- Operator that allows to divide each element of a translation vector by a scalar.
+ Operator that allows to divide each element of a translation vector by a
+ scalar.
\param x : The scalar.
@@ -392,13 +387,14 @@ vpTranslationVector &vpTranslationVector::operator*=(double x)
*/
vpTranslationVector &vpTranslationVector::operator/=(double x)
{
- for (unsigned int i=0;i<rowNum;i++)
+ for (unsigned int i = 0; i < rowNum; i++)
(*this)[i] /= x;
return (*this);
}
/*!
- Operator that allows to divide each element of a translation vector by a scalar.
+ Operator that allows to divide each element of a translation vector by a
+ scalar.
\param x : The scalar.
@@ -415,8 +411,8 @@ vpTranslationVector &vpTranslationVector::operator/=(double x)
vpTranslationVector vpTranslationVector::operator/(const double x) const
{
vpTranslationVector tv;
- for (unsigned int i=0;i<dsize;i++) {
- *(tv.data + i) = (*(data + i)) / x ;
+ for (unsigned int i = 0; i < dsize; i++) {
+ *(tv.data + i) = (*(data + i)) / x;
}
return tv;
@@ -441,20 +437,21 @@ vpTranslationVector vpTranslationVector::operator/(const double x) const
vpTranslationVector &vpTranslationVector::operator=(const vpColVector &tv)
{
if (tv.size() != 3) {
- throw(vpException(vpException::dimensionError, "Cannot initialize a translation vector from a %d-dimension col vector", tv.size()));
+ throw(vpException(vpException::dimensionError,
+ "Cannot initialize a translation vector from a "
+ "%d-dimension col vector",
+ tv.size()));
}
- unsigned int k = tv.size() ;
- if (rowNum != k){
+ unsigned int k = tv.size();
+ if (rowNum != k) {
try {
resize(k, 1);
- }
- catch(...)
- {
- throw ;
+ } catch (...) {
+ throw;
}
}
- memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ memcpy(data, tv.data, rowNum * sizeof(double));
return *this;
}
@@ -473,23 +470,21 @@ vpTranslationVector &vpTranslationVector::operator=(const vpColVector &tv)
*/
vpTranslationVector &vpTranslationVector::operator=(const vpTranslationVector &tv)
{
- unsigned int k = tv.rowNum ;
- if (rowNum != k){
+ unsigned int k = tv.rowNum;
+ if (rowNum != k) {
try {
resize(k, 1);
- }
- catch(...)
- {
- throw ;
+ } catch (...) {
+ throw;
}
}
- memcpy(data, tv.data, rowNum*sizeof(double)) ;
+ memcpy(data, tv.data, rowNum * sizeof(double));
return *this;
}
-/*!
+/*!
Initialize each element of a translation vector to the same value x.
\param x : Value to set for each element of the translation vector.
@@ -500,12 +495,12 @@ vpTranslationVector &vpTranslationVector::operator=(const vpTranslationVector &t
// Here t is set to 3,3,3
\endcode
*/
-vpTranslationVector & vpTranslationVector::operator=(double x)
+vpTranslationVector &vpTranslationVector::operator=(double x)
{
- double *d = data ;
+ double *d = data;
- for (int i=0;i<3;i++)
- *(d++)= x ;
+ for (int i = 0; i < 3; i++)
+ *(d++) = x;
return *this;
}
@@ -527,13 +522,18 @@ vpTranslationVector & vpTranslationVector::operator=(double x)
\param M : Skew symmetric matrix of translation vector \f$t\f$.
*/
-void
-vpTranslationVector::skew(const vpTranslationVector &tv, vpMatrix &M)
+void vpTranslationVector::skew(const vpTranslationVector &tv, vpMatrix &M)
{
- M.resize(3,3) ;
- M[0][0] = 0 ; M[0][1] = -tv[2] ; M[0][2] = tv[1] ;
- M[1][0] = tv[2] ; M[1][1] = 0 ; M[1][2] = -tv[0] ;
- M[2][0] = -tv[1]; M[2][1] = tv[0] ; M[2][2] = 0 ;
+ M.resize(3, 3);
+ M[0][0] = 0;
+ M[0][1] = -tv[2];
+ M[0][2] = tv[1];
+ M[1][0] = tv[2];
+ M[1][1] = 0;
+ M[1][2] = -tv[0];
+ M[2][0] = -tv[1];
+ M[2][1] = tv[0];
+ M[2][2] = 0;
}
/*!
@@ -554,11 +554,10 @@ vpTranslationVector::skew(const vpTranslationVector &tv, vpMatrix &M)
\return Skew symmetric matrix \f$M\f$ of translation vector \f$t\f$.
*/
-vpMatrix
-vpTranslationVector::skew(const vpTranslationVector &tv)
+vpMatrix vpTranslationVector::skew(const vpTranslationVector &tv)
{
vpMatrix M(3, 3);
- skew(tv,M);
+ skew(tv, M);
return M;
}
@@ -572,7 +571,7 @@ vpTranslationVector::skew(const vpTranslationVector &tv)
t_z & 0 & -t_x \\
-t_y & t_x & 0
\end{array}\right)
- \f]
+ \f]
and where \f$(t_x,t_y,t_z)\f$ are the coordinates of the translation
vector.
@@ -580,29 +579,25 @@ vpTranslationVector::skew(const vpTranslationVector &tv)
\return Skew symmetric matrix \f$M\f$ of the translation vector.
*/
-vpMatrix
-vpTranslationVector::skew() const
+vpMatrix vpTranslationVector::skew() const
{
vpMatrix M(3, 3);
- skew(*this,M);
+ skew(*this, M);
return M;
}
-
/*!
Return the cross product of two translation vectors \f$a \times b\f$.
- \param a,b : Translation vectors in input.
+ \param a,b : Translation vectors in input.
\return The cross product of two translation vectors \f$a \times
b\f$.
*/
-vpTranslationVector
-vpTranslationVector::cross(const vpTranslationVector &a,
- const vpTranslationVector &b)
+vpTranslationVector vpTranslationVector::cross(const vpTranslationVector &a, const vpTranslationVector &b)
{
- vpMatrix skew_a = vpTranslationVector::skew(a) ;
+ vpMatrix skew_a = vpTranslationVector::skew(a);
return (vpTranslationVector)(skew_a * b);
}
@@ -612,7 +607,7 @@ vpTranslationVector::cross(const vpTranslationVector &a,
vpRowVector vpTranslationVector::t() const
{
vpRowVector v(rowNum);
- memcpy(v.data, data, rowNum*sizeof(double)) ;
+ memcpy(v.data, data, rowNum * sizeof(double));
return v;
}
@@ -625,27 +620,28 @@ vpRowVector vpTranslationVector::t() const
*/
double vpTranslationVector::euclideanNorm() const
{
- double norm=0.0;
- for (unsigned int i=0;i<dsize;i++) {
- double x = *(data +i);
- norm += x*x;
+ double norm = 0.0;
+ for (unsigned int i = 0; i < dsize; i++) {
+ double x = *(data + i);
+ norm += x * x;
}
return sqrt(norm);
}
/*!
- Return the sum square of all the elements \f$t_{i}\f$ of the translation vector t(m).
+ Return the sum square of all the elements \f$t_{i}\f$ of the translation
+ vector t(m).
\return The value \f[\sum{i=0}^{m} t_i^{2}\f].
*/
double vpTranslationVector::sumSquare() const
{
- double sum_square=0.0;
+ double sum_square = 0.0;
- for (unsigned int i=0;i<rowNum;i++) {
- double x=rowPtrs[i][0];
- sum_square += x*x;
+ for (unsigned int i = 0; i < rowNum; i++) {
+ double x = rowPtrs[i][0];
+ sum_square += x * x;
}
return sum_square;
diff --git a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp
index 9f56dbe..6bc7fb0 100644
--- a/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp
+++ b/modules/core/src/math/transformation/vpVelocityTwistMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,11 @@
*
*****************************************************************************/
-#include <sstream>
#include <assert.h>
+#include <sstream>
-#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/core/vpException.h>
-
+#include <visp3/core/vpVelocityTwistMatrix.h>
/*!
\file vpVelocityTwistMatrix.cpp
@@ -50,19 +50,15 @@
transform a velocity skew from one frame to an other.
*/
-
/*!
Copy operator that allow to set a velocity twist matrix from an other one.
\param V : Velocity twist matrix to copy.
*/
-vpVelocityTwistMatrix &
-vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
+vpVelocityTwistMatrix &vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
{
- for (int i=0; i<6; i++)
- {
- for (int j=0; j<6; j++)
- {
+ for (int i = 0; i < 6; i++) {
+ for (int j = 0; j < 6; j++) {
rowPtrs[i][j] = V.rowPtrs[i][j];
}
}
@@ -70,17 +66,15 @@ vpVelocityTwistMatrix::operator=(const vpVelocityTwistMatrix &V)
return *this;
}
-
/*!
- Initialize a 6x6 velocity twist matrix as identity.
+ Initialize a 6x6 velocity twist matrix as identity.
*/
-void
-vpVelocityTwistMatrix::eye()
-{
- for (unsigned int i=0 ; i < 6 ; i++)
- for (unsigned int j=0 ; j < 6; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
+void vpVelocityTwistMatrix::eye()
+{
+ for (unsigned int i = 0; i < 6; i++)
+ for (unsigned int j = 0; j < 6; j++)
+ if (i == j)
+ (*this)[i][j] = 1.0;
else
(*this)[i][j] = 0.0;
}
@@ -88,22 +82,15 @@ vpVelocityTwistMatrix::eye()
/*!
Initialize a velocity twist transformation matrix as identity.
*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix()
- : vpArray2D<double>(6, 6)
-{
- eye() ;
-}
+vpVelocityTwistMatrix::vpVelocityTwistMatrix() : vpArray2D<double>(6, 6) { eye(); }
/*!
- Initialize a velocity twist transformation matrix from another velocity twist matrix.
+ Initialize a velocity twist transformation matrix from another velocity
+ twist matrix.
\param V : Velocity twist matrix used as initializer.
*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V)
- : vpArray2D<double>(6, 6)
-{
- *this = V;
-}
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V) : vpArray2D<double>(6, 6) { *this = V; }
/*!
@@ -111,14 +98,24 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpVelocityTwistMatrix &V)
\f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
\\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
- \param M : Homogeneous matrix \f$M\f$ used to initialize the velocity twist
- transformation matrix.
+ \param M : Homogeneous matrix \f$\bf M\f$ used to initialize the velocity
+ twist transformation matrix. \param full : Boolean used to indicate which
+ matrix should be filled.
+ - When set to true, use the complete velocity skew transformation :
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+ - When set to false, use the block diagonal velocity skew transformation:
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M)
- : vpArray2D<double>(6, 6)
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M, bool full) : vpArray2D<double>(6, 6)
{
- buildFrom(M);
+ if (full)
+ buildFrom(M);
+ else
+ buildFrom(M.getRotationMatrix());
}
/*!
@@ -126,91 +123,128 @@ vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpHomogeneousMatrix &M)
Initialize a velocity twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
\param t : Translation vector.
-
- \param thetau : \f$\theta u\f$ rotation vector.
+ \param thetau : \f$\theta u\f$ rotation vector used to initialize rotation
+ vector \f$R\f$ .
*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
- const vpThetaUVector &thetau)
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, const vpThetaUVector &thetau)
: vpArray2D<double>(6, 6)
{
- buildFrom(t, thetau) ;
+ buildFrom(t, thetau);
+}
+
+/*!
+
+ Initialize a velocity twist transformation matrix from a rotation vector
+ with \f$\theta u \f$ parametrization.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3}\\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
+ \param thetau : \f$\theta u\f$ rotation vector used to initialize rotation
+ vector \f$R\f$ .
+
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpThetaUVector &thetau) : vpArray2D<double>(6, 6)
+{
+ buildFrom(thetau);
}
/*!
Initialize a velocity twist transformation matrix from a translation vector
- \e t and a rotation matrix M.
+ \e t and a rotation matrix \e R.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
\param t : Translation vector.
-
\param R : Rotation matrix.
*/
-vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t,
- const vpRotationMatrix &R)
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpTranslationVector &t, const vpRotationMatrix &R)
: vpArray2D<double>(6, 6)
{
- buildFrom(t,R) ;
+ buildFrom(t, R);
}
/*!
+ Initialize a velocity twist transformation matrix from a rotation matrix \e
+ R.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
+ \param R : Rotation matrix.
+
+*/
+vpVelocityTwistMatrix::vpVelocityTwistMatrix(const vpRotationMatrix &R) : vpArray2D<double>(6, 6) { buildFrom(R); }
+
+/*!
+
Initialize a velocity twist transformation matrix from a translation vector
- \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$
- parametrization.
+ \f${\bf t}=(t_x, t_y, t_z)^T\f$ and a rotation vector with \f$\theta {\bf
+ u}=(\theta u_x, \theta u_y, \theta u_z)^T \f$ parametrization.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
\param tx,ty,tz : Translation vector in meters.
- \param tux,tuy,tuz : \f$\theta {\bf u}\f$ rotation vector expressed in radians.
+ \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<double>(6, 6)
{
- vpTranslationVector T(tx,ty,tz) ;
- vpThetaUVector tu(tux,tuy,tuz) ;
- buildFrom(T,tu) ;
+ vpTranslationVector t(tx, ty, tz);
+ vpThetaUVector tu(tux, tuy, tuz);
+ buildFrom(t, tu);
}
/*!
- Operator that allows to multiply a velocity twist transformation matrix by an
- other velocity twist transformation matrix.
+ Operator that allows to multiply a velocity twist transformation matrix by
+ an other velocity twist transformation matrix.
*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const
+vpVelocityTwistMatrix vpVelocityTwistMatrix::operator*(const vpVelocityTwistMatrix &V) const
{
- vpVelocityTwistMatrix p ;
-
- for (unsigned int i=0;i<6;i++)
- for (unsigned int j=0;j<6;j++)
- {
- double s =0 ;
- for (int k=0;k<6;k++)
- s +=rowPtrs[i][k] * V.rowPtrs[k][j];
- p[i][j] = s ;
+ vpVelocityTwistMatrix p;
+
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ double s = 0;
+ for (int k = 0; k < 6; k++)
+ s += rowPtrs[i][k] * V.rowPtrs[k][j];
+ p[i][j] = s;
}
+ }
return p;
}
/*!
- Operator that allows to multiply a velocity twist transformation matrix by a matrix.
+ Operator that allows to multiply a velocity twist transformation matrix by a
+matrix.
- As shown in the example below, this operator can be used to compute the corresponding
- camera velocity skew from the joint velocities knowing the robot jacobian.
+ As shown in the example below, this operator can be used to compute the
+corresponding camera velocity skew from the joint velocities knowing the robot
+jacobian.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpSimulatorCamera.h>
int main()
{
@@ -218,8 +252,8 @@ int main()
vpColVector q_vel(6); // Joint velocity on the 6 joints
// ... q_vel need here to be initialized
-
- vpColVector c_v(6); // Velocity in the camera frame: vx,vy,vz,wx,wy,wz
+
+ 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);
@@ -234,26 +268,25 @@ int main()
}
\endcode
- \exception vpException::dimensionError If M is not a 6 rows dimension matrix.
+ \exception vpException::dimensionError If M is not a 6 rows dimension
+matrix.
*/
-vpMatrix
-vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
+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",
+ throw(vpException(vpException::dimensionError, "Cannot multiply a (6x6) velocity twist matrix by a (%dx%d) matrix",
M.getRows(), M.getCols()));
}
- vpMatrix p(6, M.getCols()) ;
- for (unsigned int i=0;i<6;i++)
- for (unsigned int j=0;j<M.getCols();j++)
- {
- double s =0 ;
- for (unsigned int k=0;k<6;k++)
- s += rowPtrs[i][k] * M[k][j];
- p[i][j] = s ;
- }
+ vpMatrix p(6, M.getCols());
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < M.getCols(); j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < 6; k++)
+ s += rowPtrs[i][k] * M[k][j];
+ p[i][j] = s;
+ }
+ }
return p;
}
@@ -264,61 +297,84 @@ vpVelocityTwistMatrix::operator*(const vpMatrix &M) const
\param v : Velocity skew vector.
- \exception vpException::dimensionError If v is not a 6 dimension column vector.
+ \exception vpException::dimensionError If v is not a 6 dimension column
+ vector.
*/
-vpColVector
-vpVelocityTwistMatrix::operator*(const vpColVector &v) const
+vpColVector vpVelocityTwistMatrix::operator*(const vpColVector &v) const
{
vpColVector c(6);
- if (6 != v.getRows())
- {
+ if (6 != v.getRows()) {
throw(vpException(vpException::dimensionError,
- "Cannot multiply a (6x6) velocity twist matrix by a (%d) column vector",
+ "Cannot multiply a (6x6) velocity twist matrix by a "
+ "(%d) column vector",
v.getRows()));
}
c = 0.0;
- for (unsigned int i=0;i<6;i++) {
- for (unsigned int j=0;j<6;j++) {
+ for (unsigned int i = 0; i < 6; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
{
- c[i]+=rowPtrs[i][j] * v[j];
+ c[i] += rowPtrs[i][j] * v[j];
}
}
}
- return c ;
+ return c;
}
+/*!
+
+ Build a velocity twist transformation block diagonal matrix from a rotation
+ matrix R.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
+ \param R : Rotation matrix.
+
+*/
+vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpRotationMatrix &R)
+{
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ (*this)[i][j] = R[i][j];
+ (*this)[i + 3][j + 3] = R[i][j];
+ (*this)[i][j + 3] = 0;
+ }
+ }
+ return (*this);
+}
/*!
Build a velocity twist transformation matrix from a translation vector
- \e t and a rotation matrix M.
+ \e t and a rotation matrix \e R.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
\param t : Translation vector.
-
+
\param R : Rotation matrix.
*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
- const vpRotationMatrix &R)
+vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, const vpRotationMatrix &R)
{
- unsigned int i, j;
- vpMatrix skewaR = t.skew(t)*R ;
-
- for (i=0 ; i < 3 ; i++)
- for (j=0 ; j < 3 ; j++)
- {
- (*this)[i][j] = R[i][j] ;
- (*this)[i+3][j+3] = R[i][j] ;
- (*this)[i][j+3] = skewaR[i][j] ;
+ vpMatrix skewaR = t.skew(t) * R;
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ (*this)[i][j] = R[i][j];
+ (*this)[i + 3][j + 3] = R[i][j];
+ (*this)[i][j + 3] = skewaR[i][j];
}
- return (*this) ;
+ }
+
+ return (*this);
}
/*!
@@ -326,21 +382,39 @@ vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
Initialize a velocity twist transformation matrix from a translation vector
\e t and a rotation vector with \f$\theta u \f$ parametrization.
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
\param t : Translation vector.
-
- \param thetau : \f$\theta {\bf u}\f$ rotation vector.
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector used to create rotation
+ matrix \f${\bf R}\f$.
*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
- const vpThetaUVector &thetau)
+vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t, const vpThetaUVector &thetau)
{
- vpRotationMatrix R ;
- R.buildFrom(thetau) ;
- buildFrom(t,R) ;
- return (*this) ;
+ buildFrom(t, vpRotationMatrix(thetau));
+ return (*this);
}
+/*!
+
+ Initialize a velocity twist transformation matrix from a rotation vector
+ with \f$\theta u \f$ parametrization.
+
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+
+ \param thetau : \f$\theta {\bf u}\f$ rotation vector used to create rotation
+ matrix \f${\bf R}\f$.
+
+*/
+vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpThetaUVector &thetau)
+{
+ buildFrom(vpRotationMatrix(thetau));
+ return (*this);
+}
/*!
@@ -348,65 +422,66 @@ vpVelocityTwistMatrix::buildFrom(const vpTranslationVector &t,
\f$M\f$ with \f[ {\bf M} = \left[\begin{array}{cc} {\bf R} & {\bf t}
\\ {\bf 0}_{1\times 3} & 1 \end{array} \right] \f]
- \param M : Homogeneous matrix \f$M\f$ used to initialize the twist
+ \param M : Homogeneous matrix \f$M\f$ used to initialize the velocity twist
transformation matrix.
+ \param full : Boolean used to indicate which matrix should be filled.
+ - When set to true, use the complete velocity skew transformation :
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & [{\bf t}]_\times \; {\bf R}
+ \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
+ - When set to false, use the block diagonal velocity skew transformation:
+ \f[ {\bf V} = \left[\begin{array}{cc} {\bf R} & {\bf 0}_{3\times 3} \\
+ {\bf 0}_{3\times 3} & {\bf R} \end{array} \right] \f]
*/
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M)
+vpVelocityTwistMatrix vpVelocityTwistMatrix::buildFrom(const vpHomogeneousMatrix &M, bool full)
{
- vpTranslationVector tv ;
- vpRotationMatrix R ;
- M.extract(R) ;
- M.extract(tv) ;
+ if (full)
+ buildFrom(M.getTranslationVector(), M.getRotationMatrix());
+ else
+ buildFrom(M.getRotationMatrix());
- buildFrom(tv, R) ;
- return (*this) ;
+ return (*this);
}
-
//! Invert the velocity twist matrix.
-vpVelocityTwistMatrix
-vpVelocityTwistMatrix::inverse() const
+vpVelocityTwistMatrix vpVelocityTwistMatrix::inverse() const
{
vpVelocityTwistMatrix Wi;
- vpRotationMatrix R;extract(R);
- vpTranslationVector T;extract(T);
- vpTranslationVector RtT ; RtT = -(R.t()*T) ;
+ vpRotationMatrix R;
+ extract(R);
+ vpTranslationVector T;
+ extract(T);
+ vpTranslationVector RtT;
+ RtT = -(R.t() * T);
- Wi.buildFrom(RtT,R.t());
+ Wi.buildFrom(RtT, R.t());
- return Wi ;
+ return Wi;
}
-
//! 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
+void vpVelocityTwistMatrix::extract(vpRotationMatrix &R) const
{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- R[i][j] = (*this)[i][j];
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ R[i][j] = (*this)[i][j];
}
//! Extract the translation vector from the velocity twist matrix.
-void
-vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
+void vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
{
- vpRotationMatrix R;extract(R);
- vpMatrix skTR(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- skTR[i][j] = (*this)[i][j+3];
-
- vpMatrix skT = skTR*R.t();
+ vpRotationMatrix R;
+ extract(R);
+ vpMatrix skTR(3, 3);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ skTR[i][j] = (*this)[i][j + 3];
+
+ vpMatrix skT = skTR * R.t();
tv[0] = skT[2][1];
tv[1] = skT[0][2];
tv[2] = skT[1][0];
@@ -431,90 +506,91 @@ vpVelocityTwistMatrix::extract(vpTranslationVector &tv) const
\sa std::ostream &operator<<(std::ostream &s, const vpArray2D<Type> &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;
unsigned int m = getRows();
unsigned int n = getCols();
- std::vector<std::string> values(m*n);
+ std::vector<std::string> values(m * n);
std::ostringstream oss;
std::ostringstream ossFixed;
std::ios_base::fmtflags original_flags = oss.flags();
// ossFixed <<std::fixed;
- ossFixed.setf ( std::ios::fixed, std::ios::floatfield );
+ ossFixed.setf(std::ios::fixed, std::ios::floatfield);
- size_type maxBefore=0; // the length of the integral part
- size_type maxAfter=0; // number of decimals plus
+ size_type maxBefore = 0; // the length of the integral part
+ size_type maxAfter = 0; // number of decimals plus
// one place for the decimal point
- for (unsigned int i=0;i<m;++i) {
- for (unsigned int j=0;j<n;++j){
+ for (unsigned int i = 0; i < m; ++i) {
+ for (unsigned int j = 0; j < n; ++j) {
oss.str("");
oss << (*this)[i][j];
- if (oss.str().find("e")!=std::string::npos){
+ if (oss.str().find("e") != std::string::npos) {
ossFixed.str("");
ossFixed << (*this)[i][j];
oss.str(ossFixed.str());
}
- values[i*n+j]=oss.str();
- size_type thislen=values[i*n+j].size();
- size_type p=values[i*n+j].find('.');
+ values[i * n + j] = oss.str();
+ size_type thislen = values[i * n + j].size();
+ size_type p = values[i * n + j].find('.');
- if (p==std::string::npos){
- maxBefore=vpMath::maximum(maxBefore, thislen);
+ if (p == std::string::npos) {
+ maxBefore = vpMath::maximum(maxBefore, thislen);
// maxAfter remains the same
- } else{
- maxBefore=vpMath::maximum(maxBefore, p);
- maxAfter=vpMath::maximum(maxAfter, thislen-p-1);
+ } else {
+ maxBefore = vpMath::maximum(maxBefore, p);
+ maxAfter = vpMath::maximum(maxAfter, thislen - p - 1);
}
}
}
- size_type totalLength=length;
+ size_type totalLength = length;
// increase totalLength according to maxBefore
- totalLength=vpMath::maximum(totalLength,maxBefore);
+ totalLength = vpMath::maximum(totalLength, maxBefore);
// decrease maxAfter according to totalLength
- maxAfter=std::min(maxAfter, totalLength-maxBefore);
- if (maxAfter==1) maxAfter=0;
+ maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
+ if (maxAfter == 1)
+ maxAfter = 0;
// the following line is useful for debugging
- //std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
+ // std::cerr <<totalLength <<" " <<maxBefore <<" " <<maxAfter <<"\n";
- if (intro) s <<intro;
- s <<"["<<m<<","<<n<<"]=\n";
+ if (intro)
+ s << intro;
+ s << "[" << m << "," << n << "]=\n";
- for (unsigned int i=0;i<m;i++) {
- s <<" ";
- for (unsigned int j=0;j<n;j++){
- size_type p=values[i*n+j].find('.');
+ for (unsigned int i = 0; i < m; i++) {
+ s << " ";
+ for (unsigned int j = 0; j < n; j++) {
+ size_type p = values[i * n + j].find('.');
s.setf(std::ios::right, std::ios::adjustfield);
s.width((std::streamsize)maxBefore);
- s <<values[i*n+j].substr(0,p).c_str();
+ s << values[i * n + j].substr(0, p).c_str();
- if (maxAfter>0){
+ if (maxAfter > 0) {
s.setf(std::ios::left, std::ios::adjustfield);
- if (p!=std::string::npos){
+ if (p != std::string::npos) {
s.width((std::streamsize)maxAfter);
- s <<values[i*n+j].substr(p,maxAfter).c_str();
- } else{
- assert(maxAfter>1);
+ s << values[i * n + j].substr(p, maxAfter).c_str();
+ } else {
+ assert(maxAfter > 1);
s.width((std::streamsize)maxAfter);
- s <<".0";
+ s << ".0";
}
}
- s <<' ';
+ s << ' ';
}
- s <<std::endl;
+ s << std::endl;
}
s.flags(original_flags); // restore s to standard state
- return (int)(maxBefore+maxAfter);
+ return (int)(maxBefore + maxAfter);
}
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
@@ -525,10 +601,6 @@ vpVelocityTwistMatrix::print(std::ostream& s, unsigned int length, char const* i
Set the velocity twist transformation matrix to identity.
*/
-void
-vpVelocityTwistMatrix::setIdentity()
-{
- eye() ;
-}
+void vpVelocityTwistMatrix::setIdentity() { eye(); }
#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
diff --git a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
index cd86d55..930da90 100644
--- a/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
+++ b/modules/core/src/math/transformation/vpXmlParserHomogeneousMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,11 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpXmlParserHomogeneousMatrix.cpp
- \brief Definition of the vpXmlParserHomogeneousMatrix class member functions.
- Class vpXmlParserHomogeneousMatrix allowed to load and save an homogeneous matrix in a XML file.
+ \brief Definition of the vpXmlParserHomogeneousMatrix class member
+ functions. Class vpXmlParserHomogeneousMatrix allowed to load and save an
+ homogeneous matrix in a XML file.
*/
#include <visp3/core/vpXmlParserHomogeneousMatrix.h>
@@ -50,35 +51,35 @@
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
-/* -------------------------------------------------------------------------- */
-/* --- LABEL XML ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
-#define LABEL_XML_ROOT "root"
-#define LABEL_XML_M "homogeneous_transformation"
-#define LABEL_XML_M_NAME "name"
-#define LABEL_XML_VALUE "values"
-#define LABEL_XML_TRANSLATION "translation"
-#define LABEL_XML_TX "tx"
-#define LABEL_XML_TY "ty"
-#define LABEL_XML_TZ "tz"
-#define LABEL_XML_ROTATION "rotation"
-#define LABEL_XML_TUX "theta_ux"
-#define LABEL_XML_TUY "theta_uy"
-#define LABEL_XML_TUZ "theta_uz"
+/* --------------------------------------------------------------------------
+ */
+/* --- LABEL XML ------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
+
+#define LABEL_XML_ROOT "root"
+#define LABEL_XML_M "homogeneous_transformation"
+#define LABEL_XML_M_NAME "name"
+#define LABEL_XML_VALUE "values"
+#define LABEL_XML_TRANSLATION "translation"
+#define LABEL_XML_TX "tx"
+#define LABEL_XML_TY "ty"
+#define LABEL_XML_TZ "tz"
+#define LABEL_XML_ROTATION "rotation"
+#define LABEL_XML_TUX "theta_ux"
+#define LABEL_XML_TUY "theta_uy"
+#define LABEL_XML_TUZ "theta_uz"
/*!
Default constructor
*/
-vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix()
- : vpXmlParser(), m_M(), m_name()
-{
-}
+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;
@@ -89,13 +90,12 @@ vpXmlParserHomogeneousMatrix::vpXmlParserHomogeneousMatrix(vpXmlParserHomogeneou
\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_M = twinParser.m_M;
this->m_name = twinParser.m_name;
- return *this ;
+ return *this;
}
/*!
@@ -106,33 +106,28 @@ vpXmlParserHomogeneousMatrix::operator =(const vpXmlParserHomogeneousMatrix& twi
\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;
doc = xmlParseFile(filename.c_str());
- if (doc == NULL)
- {
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " I cannot open the file "<< filename << std::endl;
+ if (doc == NULL) {
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " I cannot open the file " << filename << std::endl;
return SEQUENCE_ERROR;
}
node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
+ if (node == NULL) {
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
- int ret = this ->read (doc, node, name);
+ int ret = this->read(doc, node, name);
- M = m_M ;
+ M = m_M;
xmlFreeDoc(doc);
@@ -147,31 +142,27 @@ vpXmlParserHomogeneousMatrix::parse(vpHomogeneousMatrix &M, const std::string &f
\return error code.
*/
-int
-vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
- const std::string &name)
+int vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::string &filename,
+ const std::string &name)
{
xmlDocPtr doc;
xmlNodePtr node;
- doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR
- + XML_PARSE_NOBLANKS);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- node = xmlNewNode(NULL,(xmlChar*)LABEL_XML_ROOT);
- xmlDocSetRootElement(doc,node);
- xmlNodePtr node_tmp = xmlNewComment((xmlChar*)
- "This file stores homogeneous matrix used\n"
- " in the vpHomogeneousMatrix Class of ViSP available\n"
- " at http://www.irisa.fr/lagadic/visp/visp.html .\n"
- " It can be read with the parse method of\n"
- " the vpXmlParserHomogeneousMatrix class.");
- xmlAddChild(node,node_tmp);
+ 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.");
+ xmlAddChild(node, node_tmp);
}
node = xmlDocGetRootElement(doc);
- if (node == NULL)
- {
+ if (node == NULL) {
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
@@ -180,11 +171,11 @@ vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::stri
int M_isFound = count(doc, node, name);
- if( M_isFound > 0){
- //vpCERROR
- std::cout << "There is already an homogeneous matrix "<< std::endl
- << "available in the file with the input name: "<< name << "."<< std::endl
- << "Please delete it manually from the xml file."<< std::endl;
+ if (M_isFound > 0) {
+ // vpCERROR
+ std::cout << "There is already an homogeneous matrix " << std::endl
+ << "available in the file with the input name: " << name << "." << std::endl
+ << "Please delete it manually from the xml file." << std::endl;
xmlFreeDoc(doc);
return SEQUENCE_ERROR;
}
@@ -193,13 +184,12 @@ vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::stri
xmlSaveFormatFile(filename.c_str(), doc, 1);
xmlFreeDoc(doc);
-// std::cout << "Homogeneous matrix '"<< name << "' saved in the file named "<< filename << " correctly." << std::endl;
+ // std::cout << "Homogeneous matrix '"<< name << "' saved in the file named
+ // "<< filename << " correctly." << std::endl;
return SEQUENCE_OK;
}
-
-
/*!
Read Homogeneous matrix values from a XML file.
@@ -208,9 +198,7 @@ vpXmlParserHomogeneousMatrix::save(const vpHomogeneousMatrix &M, const std::stri
\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;
@@ -218,63 +206,58 @@ vpXmlParserHomogeneousMatrix::read (xmlDocPtr doc, xmlNodePtr node,
vpXmlCodeSequenceType back = SEQUENCE_OK;
unsigned int nbM = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
- if (prop == CODE_XML_M){
- if (SEQUENCE_OK == this->read_matrix (doc, node, name))
+ if (prop == CODE_XML_M) {
+ if (SEQUENCE_OK == this->read_matrix(doc, node, name))
nbM++;
- }
- else back = SEQUENCE_ERROR;
+ } else
+ back = SEQUENCE_ERROR;
}
- if (nbM == 0){
+ if (nbM == 0) {
back = SEQUENCE_ERROR;
- vpCERROR << "No Homogeneous matrix is available" << std::endl
- << "with name: " << name << std::endl;
- }
- else if(nbM > 1){
+ vpCERROR << "No Homogeneous matrix is available" << std::endl << "with name: " << name << std::endl;
+ } else if (nbM > 1) {
back = SEQUENCE_ERROR;
- vpCERROR << nbM << " There are more Homogeneous matrix" << std::endl
- << "with the same name : " << std::endl
- << "precise your choice..." << std::endl;
+ vpCERROR << nbM << " There are more Homogeneous matrix" << std::endl
+ << "with the same name : " << std::endl
+ << "precise your choice..." << std::endl;
}
return back;
}
/*!
- Read homogeneous matrix names from a XML file and read if there is already a homogeneous matrix
- with the same name.
+ Read homogeneous matrix names from a XML file and read if there is already a
+ homogeneous matrix with the same name.
\param doc : XML file.
\param node : XML tree, pointing on a marker equipement.
\param name : name of the homogeneous matrix.
- \return 1 if there is an homogeneous matrix corresponding with the input name, 0 otherwise.
+ \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;
int nbM = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
}
- if (prop== CODE_XML_M) {
- if (SEQUENCE_OK == this->read_matrix (doc, node, name))
+ if (prop == CODE_XML_M) {
+ if (SEQUENCE_OK == this->read_matrix(doc, node, name))
nbM++;
}
}
@@ -292,9 +275,7 @@ 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. */
@@ -303,29 +284,25 @@ vpXmlParserHomogeneousMatrix::read_matrix (xmlDocPtr doc, xmlNodePtr node,
vpXmlCodeSequenceType back = SEQUENCE_OK;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
// vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
-
- switch (prop)
- {
+ switch (prop) {
case CODE_XML_M_NAME: {
- char * val_char = xmlReadCharChild(doc, node);
+ char *val_char = xmlReadCharChild(doc, node);
M_name_tmp = val_char;
xmlFree(val_char);
break;
}
- case CODE_XML_VALUE: //VALUE
- if (name == M_name_tmp)
- {
+ case CODE_XML_VALUE: // VALUE
+ if (name == M_name_tmp) {
std::cout << "Found Homogeneous Matrix with name: \"" << M_name_tmp << "\"" << std::endl;
back = read_values(doc, node, M_tmp);
}
@@ -345,23 +322,19 @@ vpXmlParserHomogeneousMatrix::read_matrix (xmlDocPtr doc, xmlNodePtr node,
back = SEQUENCE_ERROR;
break;
}
-
}
- if( !(name == M_name_tmp)){
+ if (!(name == M_name_tmp)) {
back = SEQUENCE_ERROR;
- }
- else{
- this-> m_M = M_tmp;
- //std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
- //std::cout << this-> M << std::endl;
- this-> m_name = M_name_tmp;
-
+ } else {
+ this->m_M = M_tmp;
+ // std::cout << "Convert in Homogeneous Matrix:"<< std::endl;
+ // std::cout << this-> M << std::endl;
+ this->m_name = M_name_tmp;
}
return back;
}
-
/*!
Read homogeneous matrix fields from a XML file.
@@ -373,36 +346,33 @@ 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;
vpXmlCodeType prop;
/* read value in the XML file. */
- double tx_=0.;
- double ty_=0.;
- double tz_=0.;
- double tux_=0.;
- double tuy_=0.;
- double tuz_=0.;
+ double tx_ = 0.;
+ double ty_ = 0.;
+ double tz_ = 0.;
+ double tux_ = 0.;
+ double tuy_ = 0.;
+ double tuz_ = 0.;
vpXmlCodeSequenceType back = SEQUENCE_OK;
- //int validation = 0;
+ // int validation = 0;
- for (node = node->xmlChildrenNode; node != NULL; node = node->next)
- {
+ for (node = node->xmlChildrenNode; node != NULL; node = node->next) {
// vpDEBUG_TRACE (15, "Carac : %s.", node ->name);
- if (node->type != XML_ELEMENT_NODE) continue;
- if (SEQUENCE_OK != str2xmlcode ((char*)(node ->name), prop))
- {
+ if (node->type != XML_ELEMENT_NODE)
+ continue;
+ if (SEQUENCE_OK != str2xmlcode((char *)(node->name), prop)) {
prop = CODE_XML_OTHER;
back = SEQUENCE_ERROR;
}
- switch (prop)
- {
+ switch (prop) {
case CODE_XML_TX:
tx_ = xmlReadDoubleChild(doc, node);
@@ -441,16 +411,15 @@ vpXmlParserHomogeneousMatrix::read_values (xmlDocPtr doc, xmlNodePtr node,
}
}
- if (nb != 6)
- {
- vpCERROR <<"ERROR in 'model' field:\n";
+ if (nb != 6) {
+ vpCERROR << "ERROR in 'model' field:\n";
vpCERROR << "it must contain 6 parameters\n";
return SEQUENCE_ERROR;
}
// Create the Homogeneous matrix
- M.buildFrom(tx_,ty_,tz_,tux_,tuy_,tuz_);
+ M.buildFrom(tx_, ty_, tz_, tux_, tuy_, tuz_);
// std::cout << "Read values from file:" << std::endl;
// std::cout << "tx:" << tx_<< std::endl;
@@ -472,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;
@@ -488,55 +456,56 @@ write (xmlNodePtr node, const std::string& name)
vpThetaUVector tu(R);
// <homogeneous_transformation>
- node_tmp = xmlNewComment((xmlChar*)"Homogeneous Matrix");
- xmlAddChild(node,node_tmp);
- node_matrix = xmlNewNode(NULL,(xmlChar*)LABEL_XML_M);
- xmlAddChild(node,node_matrix);
+ node_tmp = xmlNewComment((xmlChar *)"Homogeneous Matrix");
+ xmlAddChild(node, node_tmp);
+ node_matrix = xmlNewNode(NULL, (xmlChar *)LABEL_XML_M);
+ xmlAddChild(node, node_matrix);
{
//<name>
- if(!name.empty()){
- node_tmp = xmlNewComment((xmlChar*)"Name of the homogeneous matrix");
- xmlAddChild(node_matrix,node_tmp);
- xmlNewTextChild(node_matrix,NULL,(xmlChar*)LABEL_XML_M_NAME, (xmlChar*)name.c_str());
+ if (!name.empty()) {
+ node_tmp = xmlNewComment((xmlChar *)"Name of the homogeneous matrix");
+ xmlAddChild(node_matrix, node_tmp);
+ xmlNewTextChild(node_matrix, NULL, (xmlChar *)LABEL_XML_M_NAME, (xmlChar *)name.c_str());
}
//<values>
- node_values = xmlNewNode(NULL,(xmlChar*)LABEL_XML_VALUE);
- xmlAddChild(node_matrix,node_values);
+ node_values = xmlNewNode(NULL, (xmlChar *)LABEL_XML_VALUE);
+ xmlAddChild(node_matrix, node_values);
{
char str[11];
- node_tmp = xmlNewComment((xmlChar*)"Translation vector with values in meters");
- xmlAddChild(node_values,node_tmp);
+ node_tmp = xmlNewComment((xmlChar *)"Translation vector with values in meters");
+ xmlAddChild(node_values, node_tmp);
//<tx>
- sprintf(str,"%f", m_M[0][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TX,(xmlChar*)str);
+ sprintf(str, "%f", m_M[0][3]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TX, (xmlChar *)str);
//<ty>
- sprintf(str,"%f", m_M[1][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TY,(xmlChar*)str);
+ sprintf(str, "%f", m_M[1][3]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TY, (xmlChar *)str);
//<tz>
- sprintf(str,"%f", m_M[2][3]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TZ,(xmlChar*)str);
+ sprintf(str, "%f", m_M[2][3]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TZ, (xmlChar *)str);
- node_tmp = xmlNewComment((xmlChar*)"Rotational vector expressed in angle axis representation with values in radians");
- xmlAddChild(node_values,node_tmp);
+ node_tmp = xmlNewComment((xmlChar *)"Rotational vector expressed in angle axis "
+ "representation with values in radians");
+ xmlAddChild(node_values, node_tmp);
//<tux>
- sprintf(str,"%f", tu[0]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUX,(xmlChar*)str);
+ sprintf(str, "%f", tu[0]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUX, (xmlChar *)str);
//<tuy>
- sprintf(str,"%f", tu[1]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUY,(xmlChar*)str);
+ sprintf(str, "%f", tu[1]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUY, (xmlChar *)str);
//<tuz>
- sprintf(str,"%f", tu[2]);
- xmlNewTextChild(node_values,NULL,(xmlChar*)LABEL_XML_TUZ,(xmlChar*)str);
+ sprintf(str, "%f", tu[2]);
+ xmlNewTextChild(node_values, NULL, (xmlChar *)LABEL_XML_TUZ, (xmlChar *)str);
}
}
return back;
@@ -550,52 +519,33 @@ write (xmlNodePtr node, const std::string& name)
\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;
// DEBUG_TRACE (9, "# Entree :str=%s.", str);
- if (! strcmp (str, LABEL_XML_M))
- {
+ if (!strcmp(str, LABEL_XML_M)) {
val_int = CODE_XML_M;
- }
- else if (! strcmp (str, LABEL_XML_M_NAME))
- {
+ } else if (!strcmp(str, LABEL_XML_M_NAME)) {
val_int = CODE_XML_M_NAME;
- }
- else if (! strcmp (str, LABEL_XML_VALUE))
- {
+ } else if (!strcmp(str, LABEL_XML_VALUE)) {
val_int = CODE_XML_VALUE;
- }
- else if (! strcmp (str, LABEL_XML_TX))
- {
+ } else if (!strcmp(str, LABEL_XML_TX)) {
val_int = CODE_XML_TX;
- }
- else if (! strcmp (str, LABEL_XML_TY))
- {
+ } else if (!strcmp(str, LABEL_XML_TY)) {
val_int = CODE_XML_TY;
- }
- else if (! strcmp (str, LABEL_XML_TZ))
- {
+ } else if (!strcmp(str, LABEL_XML_TZ)) {
val_int = CODE_XML_TZ;
- }
- else if (! strcmp (str, LABEL_XML_TUX))
- {
+ } else if (!strcmp(str, LABEL_XML_TUX)) {
val_int = CODE_XML_TUX;
- }
- else if (! strcmp (str, LABEL_XML_TUY))
- {
+ } else if (!strcmp(str, LABEL_XML_TUY)) {
val_int = CODE_XML_TUY;
- }
- else if (! strcmp (str, LABEL_XML_TUZ))
- {
+ } else if (!strcmp(str, LABEL_XML_TUZ)) {
val_int = CODE_XML_TUZ;
- }
- else
- {
+ } else {
val_int = CODE_XML_OTHER;
}
res = val_int;
@@ -603,6 +553,7 @@ vpXmlParserHomogeneousMatrix::str2xmlcode (char * str, vpXmlCodeType & res)
return back;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
-void dummy_vpXmlParserHomogeneousMatrix() {};
-#endif //VISP_HAVE_XML2
+// Work arround to avoid warning:
+// libvisp_core.a(vpXmlParserHomogeneousMatrix.cpp.o) has no symbols
+void dummy_vpXmlParserHomogeneousMatrix(){};
+#endif // VISP_HAVE_XML2
diff --git a/modules/core/src/tools/convert/vpConvert.cpp b/modules/core/src/tools/convert/vpConvert.cpp
index 8a1c19e..ae60335 100644
--- a/modules/core/src/tools/convert/vpConvert.cpp
+++ b/modules/core/src/tools/convert/vpConvert.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,394 +42,423 @@
\brief Tools for type or general conversion.
*/
-#include <vector> // std::vector
-#include <algorithm> // std::transform
+#include <algorithm> // std::transform
+#include <vector> // std::vector
#include <visp3/core/vpConvert.h>
-
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- /**!
- Unary function used to transform a cv::KeyPoint to a vpImagePoint.
- \param keypoint : KeyPoint to convert.
-
- \return A vpImagePoint with the 2D coordinates corresponding to the location of the KeyPoint.
- */
- vpImagePoint vpConvert::keyPointToVpImagePoint(const cv::KeyPoint &keypoint) {
- return vpImagePoint(keypoint.pt.y, keypoint.pt.x);
- }
-
- /*!
- Unary function to convert a cv::Point2f to a vpImagePoint.
- \param point : Point to convert.
+/**!
+ Unary function used to transform a cv::KeyPoint to a vpImagePoint.
+ \param keypoint : KeyPoint to convert.
+
+ \return A vpImagePoint with the 2D coordinates corresponding to the
+ location of the KeyPoint.
+ */
+vpImagePoint vpConvert::keyPointToVpImagePoint(const cv::KeyPoint &keypoint)
+{
+ return vpImagePoint(keypoint.pt.y, keypoint.pt.x);
+}
- \return A vpImagePoint with the 2D coordinates stored in cv::Point2f.
- */
- vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) {
- return vpImagePoint(point.y, point.x);
- }
+/*!
+ Unary function to convert a cv::Point2f to a vpImagePoint.
+ \param point : Point to convert.
- /*!
- Unary function to convert a cv::Point2d to a vpImagePoint.
- \param point : Point to convert.
+ \return A vpImagePoint with the 2D coordinates stored in cv::Point2f.
+ */
+vpImagePoint vpConvert::point2fToVpImagePoint(const cv::Point2f &point) { return vpImagePoint(point.y, point.x); }
- \return A vpImagePoint with the 2D coordinates stored in cv::Point2d.
- */
- vpImagePoint vpConvert::point2dToVpImagePoint(const cv::Point2d &point) {
- return vpImagePoint(point.y, point.x);
- }
+/*!
+ Unary function to convert a cv::Point2d to a vpImagePoint.
+ \param point : Point to convert.
- /*!
- Unary function to convert a cv::Point3f to a vpPoint (object frame).
- \param point3f : Point to convert.
- \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3f.
- */
- vpPoint vpConvert::point3fToVpObjectPoint(const cv::Point3f &point3f) {
- vpPoint pt;
- pt.set_oX(point3f.x);
- pt.set_oY(point3f.y);
- pt.set_oZ(point3f.z);
- pt.set_oW(1.0);
- return pt;
- }
+ \return A vpImagePoint with the 2D coordinates stored in cv::Point2d.
+ */
+vpImagePoint vpConvert::point2dToVpImagePoint(const cv::Point2d &point) { return vpImagePoint(point.y, point.x); }
- /*!
- Unary function to convert a cv::Point3f to a vpPoint (camera frame).
- \param point3f : Point to convert.
- \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3f.
- */
- vpPoint vpConvert::point3fToVpCamPoint(const cv::Point3f &point3f) {
- vpPoint pt;
- pt.set_X(point3f.x);
- pt.set_Y(point3f.y);
- pt.set_Z(point3f.z);
- pt.set_W(1.0);
- return pt;
- }
+/*!
+ Unary function to convert a cv::Point3f to a vpPoint (object frame).
+ \param point3f : Point to convert.
+ \return A vpPoint with 3D coordinates in the object frame from from a
+ cv::Point3f.
+ */
+vpPoint vpConvert::point3fToVpObjectPoint(const cv::Point3f &point3f)
+{
+ vpPoint pt;
+ pt.set_oX(point3f.x);
+ pt.set_oY(point3f.y);
+ pt.set_oZ(point3f.z);
+ pt.set_oW(1.0);
+ return pt;
+}
- /*!
- Unary function to convert a cv::Point3d to a vpPoint (object frame).
- \param point3d : Point to convert.
- \return A vpPoint with 3D coordinates in the object frame from from a cv::Point3d.
- */
- vpPoint vpConvert::point3dToVpObjectPoint(const cv::Point3d &point3d) {
- vpPoint pt;
- pt.set_oX(point3d.x);
- pt.set_oY(point3d.y);
- pt.set_oZ(point3d.z);
- pt.set_oW(1.0);
- return pt;
- }
+/*!
+ Unary function to convert a cv::Point3f to a vpPoint (camera frame).
+ \param point3f : Point to convert.
+ \return A vpPoint with 3D coordinates in the camera frame from from a
+ cv::Point3f.
+ */
+vpPoint vpConvert::point3fToVpCamPoint(const cv::Point3f &point3f)
+{
+ vpPoint pt;
+ pt.set_X(point3f.x);
+ pt.set_Y(point3f.y);
+ pt.set_Z(point3f.z);
+ pt.set_W(1.0);
+ return pt;
+}
- /*!
- Unary function to convert a cv::Point3d to a vpPoint (camera frame).
- \param point3d : Point to convert.
- \return A vpPoint with 3D coordinates in the camera frame from from a cv::Point3d.
- */
- vpPoint vpConvert::point3dToVpCamPoint(const cv::Point3d &point3d) {
- vpPoint pt;
- pt.set_X(point3d.x);
- pt.set_Y(point3d.y);
- pt.set_Z(point3d.z);
- pt.set_W(1.0);
- return pt;
- }
+/*!
+ Unary function to convert a cv::Point3d to a vpPoint (object frame).
+ \param point3d : Point to convert.
+ \return A vpPoint with 3D coordinates in the object frame from from a
+ cv::Point3d.
+ */
+vpPoint vpConvert::point3dToVpObjectPoint(const cv::Point3d &point3d)
+{
+ vpPoint pt;
+ pt.set_oX(point3d.x);
+ pt.set_oY(point3d.y);
+ pt.set_oZ(point3d.z);
+ pt.set_oW(1.0);
+ return pt;
+}
- /*!
- Unary function to convert a vpImagePoint to a cv::Point2f.
- \param point : Image point to convert.
+/*!
+ Unary function to convert a cv::Point3d to a vpPoint (camera frame).
+ \param point3d : Point to convert.
+ \return A vpPoint with 3D coordinates in the camera frame from from a
+ cv::Point3d.
+ */
+vpPoint vpConvert::point3dToVpCamPoint(const cv::Point3d &point3d)
+{
+ vpPoint pt;
+ pt.set_X(point3d.x);
+ pt.set_Y(point3d.y);
+ pt.set_Z(point3d.z);
+ pt.set_W(1.0);
+ return pt;
+}
- \return A cv::Point2f with the 2D coordinates stored in vpImagePoint.
- */
- cv::Point2f vpConvert::vpImagePointToPoint2f(const vpImagePoint &point) {
- return cv::Point2f((float) point.get_u(), (float) point.get_v());
- }
+/*!
+ Unary function to convert a vpImagePoint to a cv::Point2f.
+ \param point : Image point to convert.
- /*!
- Unary function to convert a vpImagePoint to a cv::Point2d.
- \param point : Image point to convert.
+ \return A cv::Point2f with the 2D coordinates stored in vpImagePoint.
+ */
+cv::Point2f vpConvert::vpImagePointToPoint2f(const vpImagePoint &point)
+{
+ return cv::Point2f((float)point.get_u(), (float)point.get_v());
+}
- \return A cv::Point2d with the 2D coordinates stored in vpImagePoint.
- */
- cv::Point2d vpConvert::vpImagePointToPoint2d(const vpImagePoint &point) {
- return cv::Point2d(point.get_u(), point.get_v());
- }
+/*!
+ Unary function to convert a vpImagePoint to a cv::Point2d.
+ \param point : Image point to convert.
- /*!
- Unary function to convert the 3D coordinates in the camera frame to a cv::Point3f.
- \param point : Point to convert.
+ \return A cv::Point2d with the 2D coordinates stored in vpImagePoint.
+ */
+cv::Point2d vpConvert::vpImagePointToPoint2d(const vpImagePoint &point)
+{
+ return cv::Point2d(point.get_u(), point.get_v());
+}
- \return A cv::Point3f with the 3D coordinates stored in vpPoint in the camera frame.
- */
- cv::Point3f vpConvert::vpCamPointToPoint3f(const vpPoint &point) {
- return cv::Point3f((float) point.get_X(), (float) point.get_Y(), (float) point.get_Z());
- }
+/*!
+ Unary function to convert the 3D coordinates in the camera frame to a
+ cv::Point3f. \param point : Point to convert.
- /*!
- Unary function to convert the 3D coordinates in the camera frame to a cv::Point3d.
- \param point : Point to convert.
+ \return A cv::Point3f with the 3D coordinates stored in vpPoint in the
+ camera frame.
+ */
+cv::Point3f vpConvert::vpCamPointToPoint3f(const vpPoint &point)
+{
+ return cv::Point3f((float)point.get_X(), (float)point.get_Y(), (float)point.get_Z());
+}
- \return A cv::Point3d with the 3D coordinates stored in vpPoint in the camera frame.
- */
- cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point) {
- return cv::Point3d(point.get_X(), point.get_Y(), point.get_Z());
- }
+/*!
+ Unary function to convert the 3D coordinates in the camera frame to a
+ cv::Point3d. \param point : Point to convert.
- /*!
- Unary function to convert the 3D coordinates in the object frame to a cv::Point3f.
- \param point : Point to convert.
+ \return A cv::Point3d with the 3D coordinates stored in vpPoint in the
+ camera frame.
+ */
+cv::Point3d vpConvert::vpCamPointToPoint3d(const vpPoint &point)
+{
+ return cv::Point3d(point.get_X(), point.get_Y(), point.get_Z());
+}
- \return A cv::Point3f with the 3D coordinates stored in vpPoint in the object frame.
- */
- cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point) {
- return cv::Point3f((float) point.get_oX(), (float) point.get_oY(), (float) point.get_oZ());
- }
+/*!
+ Unary function to convert the 3D coordinates in the object frame to a
+ cv::Point3f. \param point : Point to convert.
- /*!
- Unary function to convert the 3D coordinates in the object frame to a cv::Point3d.
- \param point : Point to convert.
+ \return A cv::Point3f with the 3D coordinates stored in vpPoint in the
+ object frame.
+ */
+cv::Point3f vpConvert::vpObjectPointToPoint3f(const vpPoint &point)
+{
+ return cv::Point3f((float)point.get_oX(), (float)point.get_oY(), (float)point.get_oZ());
+}
- \return A cv::Point3d with the 3D coordinates stored in vpPoint in the object frame.
- */
- cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point) {
- return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
- }
+/*!
+ Unary function to convert the 3D coordinates in the object frame to a
+ cv::Point3d. \param point : Point to convert.
- /*!
- Unary function to return the train index stored in a cv::DMatch.
- \param match : The cv::DMatch we want to get the train index.
+ \return A cv::Point3d with the 3D coordinates stored in vpPoint in the
+ object frame.
+ */
+cv::Point3d vpConvert::vpObjectPointToPoint3d(const vpPoint &point)
+{
+ return cv::Point3d(point.get_oX(), point.get_oY(), point.get_oZ());
+}
- \return The train index stored in a cv::DMatch.
- */
- int vpConvert::dMatchToTrainIndex(const cv::DMatch &match) {
- return match.trainIdx;
- }
+/*!
+ Unary function to return the train index stored in a cv::DMatch.
+ \param match : The cv::DMatch we want to get the train index.
+ \return The train index stored in a cv::DMatch.
+ */
+int vpConvert::dMatchToTrainIndex(const cv::DMatch &match) { return match.trainIdx; }
- /*!
- Convert a cv::KeyPoint to a vpImagePoint.
- \param from : cv::KeyPoint to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to) {
- to = keyPointToVpImagePoint(from);
- }
+/*!
+ Convert a cv::KeyPoint to a vpImagePoint.
+ \param from : cv::KeyPoint to convert.
+ \param to : vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const cv::KeyPoint &from, vpImagePoint &to) { to = keyPointToVpImagePoint(from); }
- /*!
- Convert a cv::Point2f to a vpImagePoint.
- \param from : cv::Point2f to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to) {
- to = point2fToVpImagePoint(from);
- }
+/*!
+ Convert a cv::Point2f to a vpImagePoint.
+ \param from : cv::Point2f to convert.
+ \param to : vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const cv::Point2f &from, vpImagePoint &to) { to = point2fToVpImagePoint(from); }
- /*!
- Convert a cv::Point2d to a vpImagePoint.
- \param from : cv::Point2d to convert.
- \param to : vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) {
- to = point2dToVpImagePoint(from);
- }
+/*!
+ Convert a cv::Point2d to a vpImagePoint.
+ \param from : cv::Point2d to convert.
+ \param to : vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const cv::Point2d &from, vpImagePoint &to) { to = point2dToVpImagePoint(from); }
- /*!
- Convert a cv::Point3f to a vpPoint.
- \param from : cv::Point3f to convert.
- \param to : vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = point3fToVpCamPoint(from);
- } else {
- to = point3fToVpObjectPoint(from);
- }
- }
+/*!
+ Convert a cv::Point3f to a vpPoint.
+ \param from : cv::Point3f to convert.
+ \param to : vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in
+ the object frame.
+ */
+void vpConvert::convertFromOpenCV(const cv::Point3f &from, vpPoint &to, const bool cameraFrame)
+{
+ if (cameraFrame) {
+ to = point3fToVpCamPoint(from);
+ } else {
+ to = point3fToVpObjectPoint(from);
+ }
+}
- /*!
- Convert a cv::Point3d to a vpPoint.
- \param from : cv::Point3d to convert.
- \param to : vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = point3dToVpCamPoint(from);
- } else {
- to = point3dToVpObjectPoint(from);
- }
- }
+/*!
+ Convert a cv::Point3d to a vpPoint.
+ \param from : cv::Point3d to convert.
+ \param to : vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in
+ the object frame.
+ */
+void vpConvert::convertFromOpenCV(const cv::Point3d &from, vpPoint &to, const bool cameraFrame)
+{
+ if (cameraFrame) {
+ to = point3dToVpCamPoint(from);
+ } else {
+ to = point3dToVpObjectPoint(from);
+ }
+}
- /*!
- Convert a vector of cv::KeyPoint to a vector of vpImagePoint.
- \param from : Vector of cv::KeyPoint to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), keyPointToVpImagePoint);
- }
+/*!
+ Convert a vector of cv::KeyPoint to a vector of vpImagePoint.
+ \param from : Vector of cv::KeyPoint to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::KeyPoint> &from, std::vector<vpImagePoint> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), keyPointToVpImagePoint);
+}
- /*!
- Convert a vector of cv::Point2f to a vector of vpImagePoint.
- \param from : Vector of cv::Point2f to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), point2fToVpImagePoint);
- }
+/*!
+ Convert a vector of cv::Point2f to a vector of vpImagePoint.
+ \param from : Vector of cv::Point2f to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::Point2f> &from, std::vector<vpImagePoint> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), point2fToVpImagePoint);
+}
- /*!
- Convert a vector of cv::Point2d to a vector of vpImagePoint.
- \param from : Vector of cv::Point2d to convert.
- \param to : Vector of vpImagePoint converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), point2dToVpImagePoint);
- }
+/*!
+ Convert a vector of cv::Point2d to a vector of vpImagePoint.
+ \param from : Vector of cv::Point2d to convert.
+ \param to : Vector of vpImagePoint converted.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::Point2d> &from, std::vector<vpImagePoint> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), point2dToVpImagePoint);
+}
- /*!
- Convert a vector of cv::Point3f to a vector of vpPoint.
- \param from : Vector of cv::Point3f to convert.
- \param to : Vector of vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), point3fToVpCamPoint);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), point3fToVpObjectPoint);
- }
- }
+/*!
+ Convert a vector of cv::Point3f to a vector of vpPoint.
+ \param from : Vector of cv::Point3f to convert.
+ \param to : Vector of vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in
+ the object frame.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::Point3f> &from, std::vector<vpPoint> &to,
+ const bool cameraFrame)
+{
+ to.resize(from.size());
+ if (cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), point3fToVpCamPoint);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), point3fToVpObjectPoint);
+ }
+}
- /*!
- Convert a vector of cv::Point3d to a vector of vpPoint.
- \param from : Vector of cv::Point3d to convert.
- \param to : Vector of vpPoint converted.
- \param cameraFrame : If true, convert into the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), point3dToVpCamPoint);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), point3dToVpObjectPoint);
- }
- }
+/*!
+ Convert a vector of cv::Point3d to a vector of vpPoint.
+ \param from : Vector of cv::Point3d to convert.
+ \param to : Vector of vpPoint converted.
+ \param cameraFrame : If true, convert into the camera frame, otherwise in
+ the object frame.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::Point3d> &from, std::vector<vpPoint> &to,
+ const bool cameraFrame)
+{
+ to.resize(from.size());
+ if (cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), point3dToVpCamPoint);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), point3dToVpObjectPoint);
+ }
+}
- /*!
- Convert a vector of cv::DMatch to a vector of unsigned int (for a query index 0, to[0] ==> train index).
+/*!
+ Convert a vector of cv::DMatch to a vector of unsigned int (for a query
+ index 0, to[0] ==> train index).
- \warning The list of query indexes in DMatch must be ordered in a way that from[i].queryIdx == i.
+ \warning The list of query indexes in DMatch must be ordered in a way that
+ from[i].queryIdx == i.
- \param from : Vector of cv::DMatch to convert.
- \param to : Vector of unsigned int converted.
- */
- void vpConvert::convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), dMatchToTrainIndex);
- }
+ \param from : Vector of cv::DMatch to convert.
+ \param to : Vector of unsigned int converted.
+ */
+void vpConvert::convertFromOpenCV(const std::vector<cv::DMatch> &from, std::vector<unsigned int> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), dMatchToTrainIndex);
+}
- /*!
- Convert a vpImagePoint to a cv::Point2f.
- \param from : vpImagePoint to convert.
- \param to : cv::Point2f converted.
- */
- void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2f &to) {
- to = vpImagePointToPoint2f(from);
- }
+/*!
+ Convert a vpImagePoint to a cv::Point2f.
+ \param from : vpImagePoint to convert.
+ \param to : cv::Point2f converted.
+ */
+void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2f &to) { to = vpImagePointToPoint2f(from); }
- /*!
- Convert a vpImagePoint to a cv::Point2d.
- \param from : vpImagePoint to convert.
- \param to : cv::Point2d converted.
- */
- void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) {
- to = vpImagePointToPoint2d(from);
- }
+/*!
+ Convert a vpImagePoint to a cv::Point2d.
+ \param from : vpImagePoint to convert.
+ \param to : cv::Point2d converted.
+ */
+void vpConvert::convertToOpenCV(const vpImagePoint &from, cv::Point2d &to) { to = vpImagePointToPoint2d(from); }
- /*!
- Convert a vpPoint to a cv::Point3f.
- \param from : vpPoint to convert.
- \param to : cv::Point3f converted.
- \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = vpCamPointToPoint3f(from);
- } else {
- to = vpObjectPointToPoint3f(from);
- }
- }
+/*!
+ Convert a vpPoint to a cv::Point3f.
+ \param from : vpPoint to convert.
+ \param to : cv::Point3f converted.
+ \param cameraFrame : If true, convert from coordinates in the camera frame,
+ otherwise in the object frame.
+ */
+void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3f &to, const bool cameraFrame)
+{
+ if (cameraFrame) {
+ to = vpCamPointToPoint3f(from);
+ } else {
+ to = vpObjectPointToPoint3f(from);
+ }
+}
- /*!
- Convert a vpPoint to a cv::Point3d.
- \param from : vpPoint to convert.
- \param to : cv::Point3d converted.
- \param cameraFrame : If true, convert from coordinates in the camera frame, otherwise in the object frame.
- */
- void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame) {
- if(cameraFrame) {
- to = vpCamPointToPoint3d(from);
- } else {
- to = vpObjectPointToPoint3d(from);
- }
- }
+/*!
+ Convert a vpPoint to a cv::Point3d.
+ \param from : vpPoint to convert.
+ \param to : cv::Point3d converted.
+ \param cameraFrame : If true, convert from coordinates in the camera frame,
+ otherwise in the object frame.
+ */
+void vpConvert::convertToOpenCV(const vpPoint &from, cv::Point3d &to, const bool cameraFrame)
+{
+ if (cameraFrame) {
+ to = vpCamPointToPoint3d(from);
+ } else {
+ to = vpObjectPointToPoint3d(from);
+ }
+}
- /*!
- Convert a vector of vpImagePoint to a vector of cv::Point2f.
- \param from : Vector of vpImagePoint to convert.
- \param to : Vector of cv::Point2f converted.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2f);
- }
+/*!
+ Convert a vector of vpImagePoint to a vector of cv::Point2f.
+ \param from : Vector of vpImagePoint to convert.
+ \param to : Vector of cv::Point2f converted.
+ */
+void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2f> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2f);
+}
- /*!
- Convert a vector of vpImagePoint to a vector of cv::Point2d.
- \param from : Vector of vpImagePoint to convert.
- \param to : Vector of cv::Point2d converted.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to) {
- to.resize(from.size());
- std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2d);
- }
+/*!
+ Convert a vector of vpImagePoint to a vector of cv::Point2d.
+ \param from : Vector of vpImagePoint to convert.
+ \param to : Vector of cv::Point2d converted.
+ */
+void vpConvert::convertToOpenCV(const std::vector<vpImagePoint> &from, std::vector<cv::Point2d> &to)
+{
+ to.resize(from.size());
+ std::transform(from.begin(), from.end(), to.begin(), vpImagePointToPoint2d);
+}
- /*!
- Convert a vector of vpPoint to a vector of cv::Point3f.
- \param from : Vector of vpPoint to convert.
- \param to : Vector of cv::Point3f converted.
- \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3f);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3f);
- }
- }
+/*!
+ Convert a vector of vpPoint to a vector of cv::Point3f.
+ \param from : Vector of vpPoint to convert.
+ \param to : Vector of cv::Point3f converted.
+ \param cameraFrame : If true, the camera frame is considered, otherwise the
+ object frame.
+ */
+void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3f> &to, const bool cameraFrame)
+{
+ to.resize(from.size());
+ if (cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3f);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3f);
+ }
+}
- /*!
- Convert a vector of vpPoint to a vector of cv::Point3d.
- \param from : Vector of vpPoint to convert.
- \param to : Vector of cv::Point3d converted.
- \param cameraFrame : If true, the camera frame is considered, otherwise the object frame.
- */
- void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame) {
- to.resize(from.size());
- if(cameraFrame) {
- std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3d);
- } else {
- std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3d);
- }
- }
+/*!
+ Convert a vector of vpPoint to a vector of cv::Point3d.
+ \param from : Vector of vpPoint to convert.
+ \param to : Vector of cv::Point3d converted.
+ \param cameraFrame : If true, the camera frame is considered, otherwise the
+ object frame.
+ */
+void vpConvert::convertToOpenCV(const std::vector<vpPoint> &from, std::vector<cv::Point3d> &to, const bool cameraFrame)
+{
+ to.resize(from.size());
+ if (cameraFrame) {
+ std::transform(from.begin(), from.end(), to.begin(), vpCamPointToPoint3d);
+ } else {
+ std::transform(from.begin(), from.end(), to.begin(), vpObjectPointToPoint3d);
+ }
+}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpConvert.cpp.o) has no symbols
-void dummy_vpConvert() {};
+// Work arround to avoid warning: libvisp_core.a(vpConvert.cpp.o) has no
+// symbols
+void dummy_vpConvert(){};
#endif
diff --git a/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp
new file mode 100644
index 0000000..e7dd5fc
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/vpCPUFeatures.cpp
@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * CPU features (hardware capabilities).
+ *
+ *****************************************************************************/
+
+#include "x86/cpu_x86.h"
+#include <visp3/core/vpCPUFeatures.h>
+
+namespace vpCPUFeatures
+{
+static const FeatureDetector::cpu_x86 cpu_features;
+
+bool checkSSE2() { return cpu_features.HW_SSE2; }
+
+bool checkSSE3() { return cpu_features.HW_SSE3; }
+
+bool checkSSSE3() { return cpu_features.HW_SSSE3; }
+
+bool checkSSE41() { return cpu_features.HW_SSE41; }
+
+bool checkSSE42() { return cpu_features.HW_SSE42; }
+
+bool checkAVX() { return cpu_features.HW_AVX; }
+
+bool checkAVX2() { return cpu_features.HW_AVX2; }
+
+void printCPUInfo() { cpu_features.print(); }
+} // namespace vpCPUFeatures
diff --git a/modules/core/src/tools/cpu-features/x86/LICENSE.txt b/modules/core/src/tools/cpu-features/x86/LICENSE.txt
new file mode 100644
index 0000000..670154e
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/x86/LICENSE.txt
@@ -0,0 +1,116 @@
+CC0 1.0 Universal
+
+Statement of Purpose
+
+The laws of most jurisdictions throughout the world automatically confer
+exclusive Copyright and Related Rights (defined below) upon the creator and
+subsequent owner(s) (each and all, an "owner") of an original work of
+authorship and/or a database (each, a "Work").
+
+Certain owners wish to permanently relinquish those rights to a Work for the
+purpose of contributing to a commons of creative, cultural and scientific
+works ("Commons") that the public can reliably and without fear of later
+claims of infringement build upon, modify, incorporate in other works, reuse
+and redistribute as freely as possible in any form whatsoever and for any
+purposes, including without limitation commercial purposes. These owners may
+contribute to the Commons to promote the ideal of a free culture and the
+further production of creative, cultural and scientific works, or to gain
+reputation or greater distribution for their Work in part through the use and
+efforts of others.
+
+For these and/or other purposes and motivations, and without any expectation
+of additional consideration or compensation, the person associating CC0 with a
+Work (the "Affirmer"), to the extent that he or she is an owner of Copyright
+and Related Rights in the Work, voluntarily elects to apply CC0 to the Work
+and publicly distribute the Work under its terms, with knowledge of his or her
+Copyright and Related Rights in the Work and the meaning and intended legal
+effect of CC0 on those rights.
+
+1. Copyright and Related Rights. A Work made available under CC0 may be
+protected by copyright and related or neighboring rights ("Copyright and
+Related Rights"). Copyright and Related Rights include, but are not limited
+to, the following:
+
+ i. the right to reproduce, adapt, distribute, perform, display, communicate,
+ and translate a Work;
+
+ ii. moral rights retained by the original author(s) and/or performer(s);
+
+ iii. publicity and privacy rights pertaining to a person's image or likeness
+ depicted in a Work;
+
+ iv. rights protecting against unfair competition in regards to a Work,
+ subject to the limitations in paragraph 4(a), below;
+
+ v. rights protecting the extraction, dissemination, use and reuse of data in
+ a Work;
+
+ vi. database rights (such as those arising under Directive 96/9/EC of the
+ European Parliament and of the Council of 11 March 1996 on the legal
+ protection of databases, and under any national implementation thereof,
+ including any amended or successor version of such directive); and
+
+ vii. other similar, equivalent or corresponding rights throughout the world
+ based on applicable law or treaty, and any national implementations thereof.
+
+2. Waiver. To the greatest extent permitted by, but not in contravention of,
+applicable law, Affirmer hereby overtly, fully, permanently, irrevocably and
+unconditionally waives, abandons, and surrenders all of Affirmer's Copyright
+and Related Rights and associated claims and causes of action, whether now
+known or unknown (including existing as well as future claims and causes of
+action), in the Work (i) in all territories worldwide, (ii) for the maximum
+duration provided by applicable law or treaty (including future time
+extensions), (iii) in any current or future medium and for any number of
+copies, and (iv) for any purpose whatsoever, including without limitation
+commercial, advertising or promotional purposes (the "Waiver"). Affirmer makes
+the Waiver for the benefit of each member of the public at large and to the
+detriment of Affirmer's heirs and successors, fully intending that such Waiver
+shall not be subject to revocation, rescission, cancellation, termination, or
+any other legal or equitable action to disrupt the quiet enjoyment of the Work
+by the public as contemplated by Affirmer's express Statement of Purpose.
+
+3. Public License Fallback. Should any part of the Waiver for any reason be
+judged legally invalid or ineffective under applicable law, then the Waiver
+shall be preserved to the maximum extent permitted taking into account
+Affirmer's express Statement of Purpose. In addition, to the extent the Waiver
+is so judged Affirmer hereby grants to each affected person a royalty-free,
+non transferable, non sublicensable, non exclusive, irrevocable and
+unconditional license to exercise Affirmer's Copyright and Related Rights in
+the Work (i) in all territories worldwide, (ii) for the maximum duration
+provided by applicable law or treaty (including future time extensions), (iii)
+in any current or future medium and for any number of copies, and (iv) for any
+purpose whatsoever, including without limitation commercial, advertising or
+promotional purposes (the "License"). The License shall be deemed effective as
+of the date CC0 was applied by Affirmer to the Work. Should any part of the
+License for any reason be judged legally invalid or ineffective under
+applicable law, such partial invalidity or ineffectiveness shall not
+invalidate the remainder of the License, and in such case Affirmer hereby
+affirms that he or she will not (i) exercise any of his or her remaining
+Copyright and Related Rights in the Work or (ii) assert any associated claims
+and causes of action with respect to the Work, in either case contrary to
+Affirmer's express Statement of Purpose.
+
+4. Limitations and Disclaimers.
+
+ a. No trademark or patent rights held by Affirmer are waived, abandoned,
+ surrendered, licensed or otherwise affected by this document.
+
+ b. Affirmer offers the Work as-is and makes no representations or warranties
+ of any kind concerning the Work, express, implied, statutory or otherwise,
+ including without limitation warranties of title, merchantability, fitness
+ for a particular purpose, non infringement, or the absence of latent or
+ other defects, accuracy, or the present or absence of errors, whether or not
+ discoverable, all to the greatest extent permissible under applicable law.
+
+ c. Affirmer disclaims responsibility for clearing rights of other persons
+ that may apply to the Work or any use thereof, including without limitation
+ any person's Copyright and Related Rights in the Work. Further, Affirmer
+ disclaims responsibility for obtaining any necessary consents, permissions
+ or other rights required for any use of the Work.
+
+ d. Affirmer understands and acknowledges that Creative Commons is not a
+ party to this document and has no duty or obligation with respect to this
+ CC0 or use of the Work.
+
+For more information, please see
+<http://creativecommons.org/publicdomain/zero/1.0/>
diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp
new file mode 100644
index 0000000..4a5c048
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.cpp
@@ -0,0 +1,264 @@
+/* cpu_x86.cpp
+ *
+ * Author : Alexander J. Yee
+ * Date Created : 04/12/2014
+ * Last Modified : 04/12/2014
+ *
+ * Modification for ViSP:
+ * - UNKNOWN_ARCH (ARM, ...)
+ * - ifndef _XCR_XFEATURE_ENABLED_MASK (MinGW)
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Dependencies
+#include "cpu_x86.h"
+#include <cstring>
+#include <iostream>
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+#if defined(__x86_64__) || defined(_M_X64) || defined(__i386) || defined(_M_IX86)
+#if _WIN32
+#include "cpu_x86_Windows.ipp"
+#elif defined(__GNUC__) || defined(__clang__)
+#include "cpu_x86_Linux.ipp"
+#else
+//# error "No cpuid intrinsic defined for compiler."
+#define UNKNOWN_ARCH
+#endif
+#else
+//# error "No cpuid intrinsic defined for processor architecture."
+#define UNKNOWN_ARCH
+#endif
+
+#ifndef _XCR_XFEATURE_ENABLED_MASK
+#define _XCR_XFEATURE_ENABLED_MASK 0
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+namespace FeatureDetector
+{
+using std::cout;
+using std::endl;
+using std::memcpy;
+using std::memset;
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+void cpu_x86::print(const char *label, bool yes)
+{
+ cout << label;
+ cout << (yes ? "Yes" : "No") << endl;
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+cpu_x86::cpu_x86()
+{
+ memset(this, 0, sizeof(*this));
+ detect_host();
+}
+bool cpu_x86::detect_OS_AVX()
+{
+#ifndef UNKNOWN_ARCH
+ // Copied from: http://stackoverflow.com/a/22521619/922184
+
+ bool avxSupported = false;
+
+ int cpuInfo[4];
+ cpuid(cpuInfo, 1);
+
+ bool osUsesXSAVE_XRSTORE = (cpuInfo[2] & (1 << 27)) != 0;
+ bool cpuAVXSuport = (cpuInfo[2] & (1 << 28)) != 0;
+
+ if (osUsesXSAVE_XRSTORE && cpuAVXSuport) {
+ uint64_t xcrFeatureMask = xgetbv(_XCR_XFEATURE_ENABLED_MASK);
+ avxSupported = (xcrFeatureMask & 0x6) == 0x6;
+ }
+
+ return avxSupported;
+#else
+ return false;
+#endif
+}
+bool cpu_x86::detect_OS_AVX512()
+{
+#ifndef UNKNOWN_ARCH
+ if (!detect_OS_AVX())
+ return false;
+
+ uint64_t xcrFeatureMask = xgetbv(_XCR_XFEATURE_ENABLED_MASK);
+ return (xcrFeatureMask & 0xe6) == 0xe6;
+#else
+ return false;
+#endif
+}
+std::string cpu_x86::get_vendor_string()
+{
+#ifndef UNKNOWN_ARCH
+ int32_t CPUInfo[4];
+ char name[13];
+
+ cpuid(CPUInfo, 0);
+ memcpy(name + 0, &CPUInfo[1], 4);
+ memcpy(name + 4, &CPUInfo[3], 4);
+ memcpy(name + 8, &CPUInfo[2], 4);
+ name[12] = '\0';
+
+ return name;
+#else
+ return std::string();
+#endif
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+void cpu_x86::detect_host()
+{
+#ifndef UNKNOWN_ARCH
+ // OS Features
+ OS_x64 = detect_OS_x64();
+ OS_AVX = detect_OS_AVX();
+ OS_AVX512 = detect_OS_AVX512();
+
+ // Vendor
+ std::string vendor(get_vendor_string());
+ if (vendor == "GenuineIntel") {
+ Vendor_Intel = true;
+ } else if (vendor == "AuthenticAMD") {
+ Vendor_AMD = true;
+ }
+
+ int info[4];
+ cpuid(info, 0);
+ int nIds = info[0];
+
+ cpuid(info, 0x80000000);
+ uint32_t nExIds = info[0];
+
+ // Detect Features
+ if (nIds >= 0x00000001) {
+ cpuid(info, 0x00000001);
+ HW_MMX = (info[3] & ((int)1 << 23)) != 0;
+ HW_SSE = (info[3] & ((int)1 << 25)) != 0;
+ HW_SSE2 = (info[3] & ((int)1 << 26)) != 0;
+ HW_SSE3 = (info[2] & ((int)1 << 0)) != 0;
+
+ HW_SSSE3 = (info[2] & ((int)1 << 9)) != 0;
+ HW_SSE41 = (info[2] & ((int)1 << 19)) != 0;
+ HW_SSE42 = (info[2] & ((int)1 << 20)) != 0;
+ HW_AES = (info[2] & ((int)1 << 25)) != 0;
+
+ HW_AVX = (info[2] & ((int)1 << 28)) != 0;
+ HW_FMA3 = (info[2] & ((int)1 << 12)) != 0;
+
+ HW_RDRAND = (info[2] & ((int)1 << 30)) != 0;
+ }
+ if (nIds >= 0x00000007) {
+ cpuid(info, 0x00000007);
+ HW_AVX2 = (info[1] & ((int)1 << 5)) != 0;
+
+ HW_BMI1 = (info[1] & ((int)1 << 3)) != 0;
+ HW_BMI2 = (info[1] & ((int)1 << 8)) != 0;
+ HW_ADX = (info[1] & ((int)1 << 19)) != 0;
+ HW_MPX = (info[1] & ((int)1 << 14)) != 0;
+ HW_SHA = (info[1] & ((int)1 << 29)) != 0;
+ HW_PREFETCHWT1 = (info[2] & ((int)1 << 0)) != 0;
+
+ HW_AVX512_F = (info[1] & ((int)1 << 16)) != 0;
+ HW_AVX512_CD = (info[1] & ((int)1 << 28)) != 0;
+ HW_AVX512_PF = (info[1] & ((int)1 << 26)) != 0;
+ HW_AVX512_ER = (info[1] & ((int)1 << 27)) != 0;
+ HW_AVX512_VL = (info[1] & ((int)1 << 31)) != 0;
+ HW_AVX512_BW = (info[1] & ((int)1 << 30)) != 0;
+ HW_AVX512_DQ = (info[1] & ((int)1 << 17)) != 0;
+ HW_AVX512_IFMA = (info[1] & ((int)1 << 21)) != 0;
+ HW_AVX512_VBMI = (info[2] & ((int)1 << 1)) != 0;
+ }
+ if (nExIds >= 0x80000001) {
+ cpuid(info, 0x80000001);
+ HW_x64 = (info[3] & ((int)1 << 29)) != 0;
+ HW_ABM = (info[2] & ((int)1 << 5)) != 0;
+ HW_SSE4a = (info[2] & ((int)1 << 6)) != 0;
+ HW_FMA4 = (info[2] & ((int)1 << 16)) != 0;
+ HW_XOP = (info[2] & ((int)1 << 11)) != 0;
+ }
+#endif
+}
+void cpu_x86::print() const
+{
+ cout << "CPU Vendor:" << endl;
+ print(" AMD = ", Vendor_AMD);
+ print(" Intel = ", Vendor_Intel);
+ cout << endl;
+
+ cout << "OS Features:" << endl;
+#ifdef _WIN32
+ print(" 64-bit = ", OS_x64);
+#endif
+ print(" OS AVX = ", OS_AVX);
+ print(" OS AVX512 = ", OS_AVX512);
+ cout << endl;
+
+ cout << "Hardware Features:" << endl;
+ print(" MMX = ", HW_MMX);
+ print(" x64 = ", HW_x64);
+ print(" ABM = ", HW_ABM);
+ print(" RDRAND = ", HW_RDRAND);
+ print(" BMI1 = ", HW_BMI1);
+ print(" BMI2 = ", HW_BMI2);
+ print(" ADX = ", HW_ADX);
+ print(" MPX = ", HW_MPX);
+ print(" PREFETCHWT1 = ", HW_PREFETCHWT1);
+ cout << endl;
+
+ cout << "SIMD: 128-bit" << endl;
+ print(" SSE = ", HW_SSE);
+ print(" SSE2 = ", HW_SSE2);
+ print(" SSE3 = ", HW_SSE3);
+ print(" SSSE3 = ", HW_SSSE3);
+ print(" SSE4a = ", HW_SSE4a);
+ print(" SSE4.1 = ", HW_SSE41);
+ print(" SSE4.2 = ", HW_SSE42);
+ print(" AES-NI = ", HW_AES);
+ print(" SHA = ", HW_SHA);
+ cout << endl;
+
+ cout << "SIMD: 256-bit" << endl;
+ print(" AVX = ", HW_AVX);
+ print(" XOP = ", HW_XOP);
+ print(" FMA3 = ", HW_FMA3);
+ print(" FMA4 = ", HW_FMA4);
+ print(" AVX2 = ", HW_AVX2);
+ cout << endl;
+
+ cout << "SIMD: 512-bit" << endl;
+ print(" AVX512-F = ", HW_AVX512_F);
+ print(" AVX512-CD = ", HW_AVX512_CD);
+ print(" AVX512-PF = ", HW_AVX512_PF);
+ print(" AVX512-ER = ", HW_AVX512_ER);
+ print(" AVX512-VL = ", HW_AVX512_VL);
+ print(" AVX512-BW = ", HW_AVX512_BW);
+ print(" AVX512-DQ = ", HW_AVX512_DQ);
+ print(" AVX512-IFMA = ", HW_AVX512_IFMA);
+ print(" AVX512-VBMI = ", HW_AVX512_VBMI);
+ cout << endl;
+
+ cout << "Summary:" << endl;
+ print(" Safe to use AVX: ", HW_AVX && OS_AVX);
+ print(" Safe to use AVX512: ", HW_AVX512_F && OS_AVX512);
+ cout << endl;
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+} // namespace FeatureDetector
+#endif
diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86.h b/modules/core/src/tools/cpu-features/x86/cpu_x86.h
new file mode 100644
index 0000000..dbf04f4
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/x86/cpu_x86.h
@@ -0,0 +1,98 @@
+/* cpu_x86.h
+ *
+ * Author : Alexander J. Yee
+ * Date Created : 04/12/2014
+ * Last Modified : 04/12/2014
+ *
+ */
+
+#pragma once
+#ifndef _cpu_x86_H
+#define _cpu_x86_H
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Dependencies
+#include <stdint.h>
+#include <string>
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+namespace FeatureDetector
+{
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+struct cpu_x86 {
+ // Vendor
+ bool Vendor_AMD;
+ bool Vendor_Intel;
+
+ // OS Features
+ bool OS_x64;
+ bool OS_AVX;
+ bool OS_AVX512;
+
+ // Misc.
+ bool HW_MMX;
+ bool HW_x64;
+ bool HW_ABM;
+ bool HW_RDRAND;
+ bool HW_BMI1;
+ bool HW_BMI2;
+ bool HW_ADX;
+ bool HW_PREFETCHWT1;
+ bool HW_MPX;
+
+ // SIMD: 128-bit
+ bool HW_SSE;
+ bool HW_SSE2;
+ bool HW_SSE3;
+ bool HW_SSSE3;
+ bool HW_SSE41;
+ bool HW_SSE42;
+ bool HW_SSE4a;
+ bool HW_AES;
+ bool HW_SHA;
+
+ // SIMD: 256-bit
+ bool HW_AVX;
+ bool HW_XOP;
+ bool HW_FMA3;
+ bool HW_FMA4;
+ bool HW_AVX2;
+
+ // SIMD: 512-bit
+ bool HW_AVX512_F;
+ bool HW_AVX512_PF;
+ bool HW_AVX512_ER;
+ bool HW_AVX512_CD;
+ bool HW_AVX512_VL;
+ bool HW_AVX512_BW;
+ bool HW_AVX512_DQ;
+ bool HW_AVX512_IFMA;
+ bool HW_AVX512_VBMI;
+
+public:
+ cpu_x86();
+
+ void print() const;
+
+ static void cpuid(int32_t out[4], int32_t x);
+ static std::string get_vendor_string();
+
+private:
+ void detect_host();
+ static void print(const char *label, bool yes);
+
+ static bool detect_OS_x64();
+ static bool detect_OS_AVX();
+ static bool detect_OS_AVX512();
+};
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+} // namespace FeatureDetector{
+#endif
+#endif
diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp b/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp
new file mode 100644
index 0000000..d192e2d
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/x86/cpu_x86_Linux.ipp
@@ -0,0 +1,41 @@
+/* cpu_x86_Linux.ipp
+ *
+ * Author : Alexander J. Yee
+ * Date Created : 04/12/2014
+ * Last Modified : 04/12/2014
+ *
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Dependencies
+#include <cpuid.h>
+#include "cpu_x86.h"
+namespace FeatureDetector{
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+void cpu_x86::cpuid(int32_t out[4], int32_t x){
+ __cpuid_count(x, 0, out[0], out[1], out[2], out[3]);
+}
+uint64_t xgetbv(unsigned int index){
+ uint32_t eax, edx;
+ __asm__ __volatile__("xgetbv" : "=a"(eax), "=d"(edx) : "c"(index));
+ return ((uint64_t)edx << 32) | eax;
+}
+#define _XCR_XFEATURE_ENABLED_MASK 0
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Detect 64-bit
+bool cpu_x86::detect_OS_x64(){
+ // We only support x64 on Linux.
+ return true;
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+}
diff --git a/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp b/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp
new file mode 100644
index 0000000..e541f9b
--- /dev/null
+++ b/modules/core/src/tools/cpu-features/x86/cpu_x86_Windows.ipp
@@ -0,0 +1,112 @@
+/* cpu_x86_Windows.ipp
+ *
+ * Author : Alexander J. Yee
+ * Date Created : 04/12/2014
+ * Last Modified : 04/12/2014
+ *
+ * Modification for ViSP:
+ * - _xgetbv (MinGW)
+ * - __cpuidex (MinGW)
+ * - _xgetbv (Visual Studio 2010)
+ */
+
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Dependencies
+#include <Windows.h>
+#include <intrin.h>
+#include "cpu_x86.h"
+namespace FeatureDetector{
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+#if defined _MSC_VER && _MSC_VER <= 1600
+//ref: https://github.com/webmproject/libwebp/blob/v0.6.0/src/dsp/cpu.c#L82
+//ref: http://forums.codeguru.com/showthread.php?551499-xgetbv
+//note: code to return the uint64_t value
+//return ((uint64_t)edx_ << 32) | eax_;
+//could be discard?
+//ref: http://forums.codeguru.com/showthread.php?551499-xgetbv&s=aa59e5d6a36eb176c820406e707b42e4&p=2185193#post2185193
+//ref: https://stackoverflow.com/a/25824252/6055233
+uint64_t _xgetbv(unsigned int ext_ctrl_reg)
+{
+ uint32_t eax_, edx_;
+ __asm {
+ mov ecx, [ext_ctrl_reg]
+ __asm _emit 0x0f __asm _emit 0x01 __asm _emit 0xd0 /* xgetbv() */
+ mov eax_, eax
+ mov edx_, edx
+ }
+ return ((uint64_t)edx_ << 32) | eax_;
+}
+#endif
+#if defined(__MINGW32__)
+unsigned __int64 _xgetbv(unsigned int index)
+{
+#if defined(__x86_64__) || defined(_AMD64_)
+ unsigned __int64 val1, val2;
+#else
+ unsigned __LONG32 val1, val2;
+#endif /* defined(__x86_64__) || defined(_AMD64_) */
+
+ __asm__ __volatile__(
+ "xgetbv"
+ : "=a" (val1), "=d" (val2)
+ : "c" (index));
+
+ return (((unsigned __int64)val2) << 32) | val1;
+}
+#endif
+#if defined(__MINGW32__)
+void __cpuidex(int CPUInfo[4], int function_id, int subfunction_id) {
+ __asm__ __volatile__ (
+ "cpuid"
+ : "=a" (CPUInfo [0]), "=b" (CPUInfo [1]), "=c" (CPUInfo [2]), "=d" (CPUInfo [3])
+ : "a" (function_id), "c" (subfunction_id));
+}
+#endif
+void cpu_x86::cpuid(int32_t out[4], int32_t x){
+ __cpuidex(out, x, 0);
+}
+__int64 xgetbv(unsigned int x){
+ return _xgetbv(x);
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+// Detect 64-bit - Note that this snippet of code for detecting 64-bit has been copied from MSDN.
+typedef BOOL (WINAPI *LPFN_ISWOW64PROCESS) (HANDLE, PBOOL);
+BOOL IsWow64()
+{
+ BOOL bIsWow64 = FALSE;
+
+#ifndef WINRT // Turned off on UWP where GetModuleHandle() doesn't exist
+ LPFN_ISWOW64PROCESS fnIsWow64Process = (LPFN_ISWOW64PROCESS) GetProcAddress(
+ GetModuleHandle(TEXT("kernel32")), "IsWow64Process");
+
+ if (NULL != fnIsWow64Process)
+ {
+ if (!fnIsWow64Process(GetCurrentProcess(), &bIsWow64))
+ {
+ printf("Error Detecting Operating System.\n");
+ printf("Defaulting to 32-bit OS.\n\n");
+ bIsWow64 = FALSE;
+ }
+ }
+#endif
+ return bIsWow64;
+}
+bool cpu_x86::detect_OS_x64(){
+#ifdef _M_X64
+ return true;
+#else
+ return IsWow64() != 0;
+#endif
+}
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+////////////////////////////////////////////////////////////////////////////////
+}
diff --git a/modules/core/src/tools/exceptions/vpException.cpp b/modules/core/src/tools/exceptions/vpException.cpp
index 42c3986..c682167 100644
--- a/modules/core/src/tools/exceptions/vpException.cpp
+++ b/modules/core/src/tools/exceptions/vpException.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,39 +36,32 @@
*
*****************************************************************************/
-
/* \file vpException.cpp
\brief error that can be emited by the vp class and its derivates
*/
-#include <stdio.h>
#include "visp3/core/vpException.h"
+#include <stdio.h>
+/* -------------------------------------------------------------------------
+ */
+/* --- CONSTRUCTORS --------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
+vpException::vpException(int id) : code(id), message() {}
-/* ------------------------------------------------------------------------- */
-/* --- CONSTRUCTORS -------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-vpException::vpException (int id)
- : code (id), message ()
-{
-}
-
-vpException::vpException (int id, const std::string & msg)
- : code (id), message (msg)
-{
-}
+vpException::vpException(int id, const 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);
setMessage(format, args);
- va_end (args);
+ 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);
}
@@ -81,11 +75,10 @@ vpException::vpException (const int id, const char* format, va_list args)
// {
// }
-
-void vpException::setMessage(const char* format, va_list args)
+void vpException::setMessage(const char *format, va_list args)
{
char buffer[1024];
- vsnprintf (buffer, 1024, format, args);
+ vsnprintf(buffer, 1024, format, args);
std::string msg(buffer);
message = msg;
}
@@ -94,51 +87,41 @@ 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;
-}
+int vpException::getCode(void) { return this->code; }
/*!
Overloading of the what() method of std::exception to return the vpException
message.
-
+
\return pointer on the array of \e char related to the error string.
*/
-const char* vpException::what () const throw()
-{
- return (this->message) .c_str();
-}
-
+const char *vpException::what() const throw() { return (this->message).c_str(); }
-/* ------------------------------------------------------------------------- */
-/* --- MODIFIORS ----------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- MODIFIORS -----------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
-/* ------------------------------------------------------------------------- */
-/* --- OP << --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- OP << ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
-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;
- return os;
+ return os;
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp
index ad98e16..0b27364 100644
--- a/modules/core/src/tools/file/vpIoTools.cpp
+++ b/modules/core/src/tools/file/vpIoTools.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,31 +40,37 @@
\file vpIoTools.cpp
\brief File and directories basic tools.
*/
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpIoException.h>
-#include <stdlib.h>
-#include <stdio.h>
+#include <algorithm>
+#include <cmath>
+#include <errno.h>
+#include <fcntl.h>
#include <fstream>
+#include <limits>
+#include <stdio.h>
+#include <stdlib.h>
#include <string.h>
#include <sys/stat.h>
#include <sys/types.h>
-#include <fcntl.h>
-#include <limits>
-#include <cmath>
-#include <algorithm>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpIoException.h>
+#include <visp3/core/vpIoTools.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
+#include <dirent.h>
+#include <unistd.h>
#elif defined(_WIN32)
-# include <windows.h>
-# include <direct.h>
+#include <direct.h>
+#include <windows.h>
#endif
#if !defined(_WIN32)
-# include <wordexp.h>
+#include <wordexp.h>
#endif
#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
-# include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IOS macro
+#endif
+
+#ifndef PATH_MAX
+#define PATH_MAX _MAX_PATH
#endif
std::string vpIoTools::baseName = "";
@@ -72,50 +79,66 @@ std::string vpIoTools::configFile = "";
std::vector<std::string> vpIoTools::configVars = std::vector<std::string>();
std::vector<std::string> vpIoTools::configValues = std::vector<std::string>();
+#if TARGET_OS_IOS == 0 // The following code is not working on iOS since
+ // wordexp() is not available
+namespace
+{
+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) {
+ str.replace(start_pos, search.length(), replace);
+ start_pos += replace.length(); // Handles case where 'replace' is a
+ // substring of 'search'
+ }
+}
+}
+#endif
+
/*!
Return build informations (OS, compiler, build flags, used 3rd parties...).
*/
-const std::string& vpIoTools::getBuildInformation()
+const std::string &vpIoTools::getBuildInformation()
{
static std::string build_info =
#include "version_string.inc"
- ;
+ ;
return build_info;
}
/*!
Sets the base name (prefix) of the experiment files.
-
+
\param s : Prefix of the experiment files.
*/
-void vpIoTools::setBaseName(const std::string &s) {baseName = s;}
+void vpIoTools::setBaseName(const std::string &s) { baseName = s; }
/*!
Sets the base directory of the experiment files.
-
+
\param dir : Directory where the data will be saved.
*/
-void vpIoTools::setBaseDir(const std::string &dir) {baseDir = dir + "/";}
+void vpIoTools::setBaseDir(const std::string &dir) { baseDir = dir + "/"; }
/*!
Gets the base name (prefix) of the experiment files.
-
+
\return the base name of the experiment files.
*/
-std::string vpIoTools::getBaseName() {return baseName;}
+std::string vpIoTools::getBaseName() { return baseName; }
/*!
Gets the full path of the experiment files : baseDir/baseName
-
+
\return the full path of the experiment files.
*/
-std::string vpIoTools::getFullName() {return baseDir + baseName;}
+std::string vpIoTools::getFullName() { return baseDir + baseName; }
/*!
Get the user name.
- Under unix, get the content of the LOGNAME environment variable. For most
- purposes (especially in conjunction with crontab), it is more useful to use
- the environment variable LOGNAME to find out who the user is, rather than
- the getlogin() function. This is more flexible precisely because the user
- can set LOGNAME arbitrarily.
+ purposes (especially in conjunction with crontab), it is more useful to
+ use the environment variable LOGNAME to find out who the user is, rather
+ than the getlogin() function. This is more flexible precisely because the
+ user can set LOGNAME arbitrarily.
- Under windows, uses the GetUserName() function.
\param username : The user name.
@@ -125,45 +148,44 @@ std::string vpIoTools::getFullName() {return baseDir + baseName;}
\sa getUserName()
*/
-void
-vpIoTools::getUserName(std::string &username)
+void vpIoTools::getUserName(std::string &username)
{
- // With MinGW, UNIX and _WIN32 are defined
+// With MinGW, UNIX and _WIN32 are defined
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
// Get the user name.
char *_username = NULL;
_username = ::getenv("LOGNAME");
if (_username == NULL) {
- vpERROR_TRACE( "Cannot get the username. Check your LOGNAME environment variable" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
+ vpERROR_TRACE("Cannot get the username. Check your LOGNAME environment variable");
+ throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username"));
}
username = _username;
#elif defined(_WIN32)
-# if ( ! defined(WINRT) )
+#if (!defined(WINRT))
unsigned int info_buffer_size = 1024;
- TCHAR *infoBuf = new TCHAR [info_buffer_size];
- DWORD bufCharCount = (DWORD) info_buffer_size;
+ TCHAR *infoBuf = new TCHAR[info_buffer_size];
+ DWORD bufCharCount = (DWORD)info_buffer_size;
// Get the user name.
- if( ! GetUserName( infoBuf, &bufCharCount ) ) {
- delete [] infoBuf;
- throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username")) ;
+ if (!GetUserName(infoBuf, &bufCharCount)) {
+ delete[] infoBuf;
+ 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"));
-# endif
+ delete[] infoBuf;
+#else
+ throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username: not implemented on Universal "
+ "Windows Platform"));
+#endif
#endif
}
/*!
Get the user name.
- Under unix, get the content of the LOGNAME environment variable. For most
- purposes (especially in conjunction with crontab), it is more useful to use
- the environment variable LOGNAME to find out who the user is, rather than
- the getlogin() function. This is more flexible precisely because the user
- can set LOGNAME arbitrarily.
+ purposes (especially in conjunction with crontab), it is more useful to
+ use the environment variable LOGNAME to find out who the user is, rather
+ than the getlogin() function. This is more flexible precisely because the
+ user can set LOGNAME arbitrarily.
- Under windows, uses the GetUserName() function.
\return The user name.
@@ -173,8 +195,7 @@ vpIoTools::getUserName(std::string &username)
\sa getUserName(std::string &)
*/
-std::string
-vpIoTools::getUserName()
+std::string vpIoTools::getUserName()
{
std::string username;
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
@@ -182,29 +203,27 @@ vpIoTools::getUserName()
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)
-# if ( ! defined(WINRT) )
+#if (!defined(WINRT))
unsigned int info_buffer_size = 1024;
- TCHAR *infoBuf = new TCHAR [info_buffer_size];
- DWORD bufCharCount = (DWORD) info_buffer_size;
+ TCHAR *infoBuf = new TCHAR[info_buffer_size];
+ DWORD bufCharCount = (DWORD)info_buffer_size;
// Get the user name.
- if( ! GetUserName( infoBuf, &bufCharCount ) ) {
- delete [] infoBuf;
- vpERROR_TRACE( "Cannot get the username" );
- throw(vpIoException(vpIoException::cantGetUserName,
- "Cannot get the username")) ;
-
+ if (!GetUserName(infoBuf, &bufCharCount)) {
+ delete[] infoBuf;
+ vpERROR_TRACE("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"));
-# endif
+ delete[] infoBuf;
+#else
+ throw(vpIoException(vpIoException::cantGetUserName, "Cannot get the username: not implemented on Universal "
+ "Windows Platform"));
+#endif
#endif
return username;
}
@@ -240,19 +259,18 @@ int main()
\sa getenv(std::string &)
*/
-std::string
-vpIoTools::getenv(const char *env)
+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;
@@ -292,11 +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".
@@ -307,33 +321,29 @@ 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){
+ if (version.size() == 0) {
major = 0;
minor = 0;
patch = 0;
- }
- else{
+ } else {
size_t major_pos = version.find('.');
std::string major_str = version.substr(0, major_pos);
major = (unsigned)atoi(major_str.c_str());
-
- if(major_pos != std::string::npos){
- size_t minor_pos = version.find('.', major_pos+1);
- std::string minor_str = version.substr(major_pos+1, (minor_pos - (major_pos+1)));
+
+ if (major_pos != std::string::npos) {
+ size_t minor_pos = version.find('.', major_pos + 1);
+ std::string minor_str = version.substr(major_pos + 1, (minor_pos - (major_pos + 1)));
minor = (unsigned)atoi(minor_str.c_str());
-
- if(minor_pos != std::string::npos){
- std::string patch_str = version.substr(minor_pos+1);
+
+ if (minor_pos != std::string::npos) {
+ std::string patch_str = version.substr(minor_pos + 1);
patch = (unsigned)atoi(patch_str.c_str());
- }
- else{
+ } else {
patch = 0;
}
- }
- else{
+ } else {
minor = 0;
patch = 0;
}
@@ -354,36 +364,47 @@ vpIoTools::getVersion(const std::string &version, unsigned int &major, unsigned
\sa checkDirectory(const std::string &)
*/
-bool
-vpIoTools::checkDirectory(const char *dirname )
+bool vpIoTools::checkDirectory(const char *dirname)
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
struct stat stbuf;
+#elif defined(_WIN32) && defined(__MINGW32__)
+ struct stat stbuf;
#elif defined(_WIN32)
struct _stat stbuf;
#endif
- if ( dirname == NULL || dirname[0] == '\0' ) {
+ if (dirname == NULL || dirname[0] == '\0') {
return false;
}
std::string _dirname = path(dirname);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _dirname.c_str(), &stbuf ) != 0 )
+ 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)
+ _dirname = _dirname.substr(0, _dirname.size() - 1);
+ if (stat(_dirname.c_str(), &stbuf) != 0)
#elif defined(_WIN32)
- if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
+ if (_stat(_dirname.c_str(), &stbuf) != 0)
#endif
{
return false;
}
- if ( (stbuf.st_mode & S_IFDIR) == 0 ) {
+#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 ( (stbuf.st_mode & S_IWUSR) == 0 )
+ if ((stbuf.st_mode & S_IWUSR) == 0)
#elif defined(_WIN32)
- if ( (stbuf.st_mode & S_IWRITE) == 0 )
+ if ((stbuf.st_mode & S_IWRITE) == 0)
#endif
{
return false;
@@ -404,14 +425,62 @@ vpIoTools::checkDirectory(const char *dirname )
\sa checkDirectory(const char *)
*/
-bool
-vpIoTools::checkDirectory(const std::string &dirname )
+bool vpIoTools::checkDirectory(const std::string &dirname) { return vpIoTools::checkDirectory(dirname.c_str()); }
+
+// See:
+// https://gist.github.com/JonathonReinhart/8c0d90191c38af2dcadb102c4e202950
+int vpIoTools::mkdir_p(const char *path, const int mode)
{
- return vpIoTools::checkDirectory(dirname.c_str());
+ /* Adapted from http://stackoverflow.com/a/2336245/119527 */
+ const size_t len = strlen(path);
+ char _path[PATH_MAX];
+ char *p = NULL;
+ const char sep = vpIoTools::separator;
+
+ errno = 0;
+ if (len > sizeof(_path) - 1) {
+ errno = ENAMETOOLONG;
+ return -1;
+ }
+ /* Copy string so its mutable */
+ strcpy(_path, path);
+
+ /* Iterate over the string */
+ for (p = _path + 1; *p; p++) { // path cannot be empty
+ if (*p == sep) {
+ /* Temporarily truncate */
+ *p = '\0';
+
+#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
+ if (!checkDirectory(_path) && _mkdir(_path) != 0)
+#endif
+ {
+ if (errno != EEXIST)
+ return -1;
+ }
+ *p = sep;
+ }
+ }
+
+#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)
+#endif
+ {
+ if (errno != EEXIST)
+ return -1;
+ }
+
+ return 0;
}
-/*!
- Create a new directory.
+/*!
+ Create a new directory. It will create recursively the parent directories if
+ needed.
\param dirname : Directory to create. The directory name
is converted to the current system's format; see path().
@@ -423,52 +492,53 @@ vpIoTools::checkDirectory(const std::string &dirname )
\sa makeDirectory(const std::string &)
*/
-void
-vpIoTools::makeDirectory(const char *dirname )
+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;
+ return;
+#endif
+
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
struct stat stbuf;
+#elif defined(_WIN32) && defined(__MINGW32__)
+ struct stat stbuf;
#elif defined(_WIN32)
struct _stat stbuf;
#endif
- if ( dirname == NULL || dirname[0] == '\0' ) {
- vpERROR_TRACE( "invalid directory name\n");
- throw(vpIoException(vpIoException::invalidDirectoryName,
- "invalid directory name")) ;
+ if (dirname == NULL || dirname[0] == '\0') {
+ vpERROR_TRACE("invalid directory name\n");
+ throw(vpIoException(vpIoException::invalidDirectoryName, "invalid directory name"));
}
std::string _dirname = path(dirname);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _dirname.c_str(), &stbuf ) != 0 )
+ if (stat(_dirname.c_str(), &stbuf) != 0)
+#elif defined(_WIN32) && defined(__MINGW32__)
+ if (stat(_dirname.c_str(), &stbuf) != 0)
#elif defined(_WIN32)
- if ( _stat( _dirname.c_str(), &stbuf ) != 0 )
+ if (_stat(_dirname.c_str(), &stbuf) != 0)
#endif
{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
- if ( mkdir( _dirname.c_str(), (mode_t)0755 ) != 0 )
-#elif defined(_WIN32)
- if ( _mkdir( _dirname.c_str()) != 0 )
-#endif
- {
- vpERROR_TRACE("unable to create directory '%s'\n", dirname );
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
+ 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"));
}
- vpDEBUG_TRACE(2,"has created directory '%s'\n", dirname );
+
+ vpDEBUG_TRACE(2, "has created directory '%s'\n", dirname);
}
- if ( checkDirectory( dirname ) == false) {
- vpERROR_TRACE("unable to create directory '%s'\n", dirname );
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
+ if (checkDirectory(dirname) == false) {
+ vpERROR_TRACE("unable to create directory '%s'\n", dirname);
+ throw(vpIoException(vpIoException::cantCreateDirectory, "unable to create directory"));
}
}
/*!
-
- Create a new directory.
+ Create a new directory. It will create recursively the parent directories if
+ needed.
\param dirname : Directory to create. The directory name
is converted to the current system's format; see path().
@@ -478,21 +548,17 @@ vpIoTools::makeDirectory(const char *dirname )
\sa makeDirectory(const char *)
*/
-void
-vpIoTools::makeDirectory(const std::string &dirname )
+void vpIoTools::makeDirectory(const std::string &dirname)
{
try {
vpIoTools::makeDirectory(dirname.c_str());
- }
- catch (...) {
- vpERROR_TRACE("unable to create directory '%s'\n",dirname.c_str());
- throw(vpIoException(vpIoException::cantCreateDirectory,
- "unable to create directory")) ;
+ } catch (...) {
+ vpERROR_TRACE("unable to create directory '%s'\n", dirname.c_str());
+ throw(vpIoException(vpIoException::cantCreateDirectory, "unable to create directory"));
}
}
/*!
-
Check if a file exists.
\param filename : Filename to test if it exists.
@@ -504,8 +570,7 @@ vpIoTools::makeDirectory(const std::string &dirname )
\sa checkFilename(const std::string &)
*/
-bool
-vpIoTools::checkFilename(const char *filename)
+bool vpIoTools::checkFilename(const char *filename)
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
struct stat stbuf;
@@ -513,26 +578,26 @@ vpIoTools::checkFilename(const char *filename)
struct _stat stbuf;
#endif
- if ( filename == NULL || filename[0] == '\0' ) {
+ if (filename == NULL || filename[0] == '\0') {
return false;
}
std::string _filename = path(filename);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( stat( _filename.c_str(), &stbuf ) != 0 )
+ if (stat(_filename.c_str(), &stbuf) != 0)
#elif defined(_WIN32)
- if ( _stat( _filename.c_str(), &stbuf ) != 0 )
+ if (_stat(_filename.c_str(), &stbuf) != 0)
#endif
{
return false;
}
- if ( (stbuf.st_mode & S_IFREG) == 0 ) {
+ if ((stbuf.st_mode & S_IFREG) == 0) {
return false;
}
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( (stbuf.st_mode & S_IRUSR) == 0 )
+ if ((stbuf.st_mode & S_IRUSR) == 0)
#elif defined(_WIN32)
- if ( (stbuf.st_mode & S_IREAD) == 0 )
+ if ((stbuf.st_mode & S_IREAD) == 0)
#endif
{
return false;
@@ -552,11 +617,7 @@ 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()); }
/*!
@@ -570,63 +631,76 @@ vpIoTools::checkFilename(const std::string &filename)
\sa copy(const std::string &, const std::string &)
*/
-bool
-vpIoTools::copy(const char *src, const char *dst)
+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 (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 TARGET_OS_IOS == 0 // The following code is not working on iOS since
+ // wordexp() is not available
char cmd[FILENAME_MAX];
int ret;
sprintf(cmd, "cp -p %s %s", src, dst);
ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
+#endif
#elif defined(_WIN32)
-# if ( ! defined(WINRT) )
+#if (!defined(WINRT))
char cmd[FILENAME_MAX];
int ret;
std::string src_ = vpIoTools::path(src);
std::string dst_ = vpIoTools::path(dst);
sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
-# endif
+#else
+ throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on Universal Windows Platform",
+ src, dst));
#endif
- }
- else if ( vpIoTools::checkDirectory(src) ) {
- //std::cout << "copy directory: " << src << " in " << dst << std::endl;
+#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 TARGET_OS_IOS == 0 // The following code is not working on iOS since
+ // wordexp() is not available
char cmd[FILENAME_MAX];
int ret;
sprintf(cmd, "cp -p -r %s %s", src, dst);
ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
+#endif
#elif defined(_WIN32)
-# if ( ! defined(WINRT) )
+#if (!defined(WINRT))
char cmd[FILENAME_MAX];
int ret;
std::string src_ = vpIoTools::path(src);
std::string dst_ = vpIoTools::path(dst);
sprintf(cmd, "copy %s %s", src_.c_str(), dst_.c_str());
ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
-# endif
+#else
+ throw(vpIoException(vpException::fatalError, "Cannot copy %s in %s: not implemented on Universal Windows Platform",
+ src, dst));
#endif
- }
- else {
+#endif
+ } else {
std::cout << "Cannot copy: " << src << " in " << dst << std::endl;
return false;
}
@@ -643,8 +717,7 @@ vpIoTools::copy(const char *src, const char *dst)
\sa copy(const char *, const char *)
*/
-bool
-vpIoTools::copy(const std::string &src, const std::string &dst)
+bool vpIoTools::copy(const std::string &src, const std::string &dst)
{
return vpIoTools::copy(src.c_str(), dst.c_str());
}
@@ -659,41 +732,46 @@ vpIoTools::copy(const std::string &src, const std::string &dst)
\sa remove(const std::string &)
*/
-bool
-vpIoTools::remove(const char *file_or_dir)
+bool vpIoTools::remove(const char *file_or_dir)
{
// Check if we have to consider a file or a directory
- if ( vpIoTools::checkFilename(file_or_dir) ) {
- //std::cout << "remove file: " << file_or_dir << std::endl;
+ if (vpIoTools::checkFilename(file_or_dir)) {
+ // std::cout << "remove file: " << file_or_dir << std::endl;
if (::remove(file_or_dir) != 0)
return false;
else
return true;
- }
- else if ( vpIoTools::checkDirectory(file_or_dir) ) {
- //std::cout << "remove directory: " << file_or_dir << std::endl;
+ } 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
- char cmd[FILENAME_MAX];
- sprintf(cmd, "rm -rf %s", file_or_dir);
- int ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //std::cout << cmd << " return value: " << ret << std::endl;
- return true;
+#if TARGET_OS_IOS == 0 // The following code is not working on iOS since
+ // wordexp() is not available
+ char cmd[FILENAME_MAX];
+ sprintf(cmd, "rm -rf %s", file_or_dir);
+ int ret = system(cmd);
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
+#endif
#elif defined(_WIN32)
-# if ( ! defined(WINRT) )
- char cmd[FILENAME_MAX];
- std::string file_or_dir_ = vpIoTools::path(file_or_dir);
+#if (!defined(WINRT))
+ char cmd[FILENAME_MAX];
+ std::string file_or_dir_ = vpIoTools::path(file_or_dir);
sprintf(cmd, "rmdir /S /Q %s", file_or_dir_.c_str());
- int ret = system(cmd);
- if (ret) {}; // to avoid a warning
- //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));
-# endif
+ int ret = system(cmd);
+ if (ret) {
+ }; // to avoid a warning
+ // 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));
#endif
- }
- else {
+#endif
+ } else {
std::cout << "Cannot remove: " << file_or_dir << std::endl;
return false;
}
@@ -709,11 +787,7 @@ 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()); }
/*!
@@ -726,8 +800,7 @@ vpIoTools::remove(const std::string &file_or_dir)
\sa rename(const std::string &, const std::string &)
*/
-bool
-vpIoTools::rename(const char *oldfilename, const char *newfilename)
+bool vpIoTools::rename(const char *oldfilename, const char *newfilename)
{
if (::rename(oldfilename, newfilename) != 0)
return false;
@@ -746,14 +819,11 @@ 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());
}
-
-
/*!
Converts a path name to the current system's format.
@@ -766,32 +836,29 @@ vpIoTools::rename(const std::string &oldfilename, const std::string &newfilename
\sa path(const std::string &)
*/
-std::string
-vpIoTools::path(const char *pathname)
+std::string vpIoTools::path(const char *pathname)
{
std::string path(pathname);
#if defined(_WIN32)
- for(unsigned int i=0 ; i<path.length() ; i++)
- if( path[i] == '/') path[i] = '\\';
+ for (unsigned int i = 0; i < path.length(); i++)
+ if (path[i] == '/')
+ path[i] = '\\';
#elif defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))
- for(unsigned int i=0 ; i<path.length() ; i++)
- if( path[i] == '\\') path[i] = '/';
-# if TARGET_OS_IOS == 0 // The following code is not working on iOS since wordexp() is not available
+ for (unsigned int i = 0; i < path.length(); i++)
+ if (path[i] == '\\')
+ path[i] = '/';
+#if TARGET_OS_IOS == 0 // The following code is not working on iOS since
+ // wordexp() is not available
wordexp_t exp_result;
- wordexp(path.c_str(), &exp_result, 0);
- path = "";
- //If path contains whitespace, wordexp_t will try to expand each word separated by whitespaces
- //This is why we need to concatenate the results
- for(size_t i = 0; i < exp_result.we_wordc; i++) {
- path += exp_result.we_wordv[i];
- if(i < exp_result.we_wordc-1) {
- path += " ";
- }
- }
+ // escape quote character
+ replaceAll(path, "'", "'\\''");
+ // add quotes to handle special characters like parentheses and spaces
+ wordexp(std::string("'" + path + "'").c_str(), &exp_result, 0);
+ path = exp_result.we_wordc == 1 ? exp_result.we_wordv[0] : "";
wordfree(&exp_result);
-# endif
+#endif
#endif
return path;
@@ -799,7 +866,7 @@ vpIoTools::path(const char *pathname)
/*!
Converts a path name to the current system's format.
-
+
\param pathname : Path name to convert. Under windows, converts all
the "/" characters in the \e pathname string into "\\"
characters. Under Unix systems converts all the "\\" characters in
@@ -809,58 +876,50 @@ 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.
- \param confFile : path to the file containing the configuration parameters to parse.
+ \param confFile : path to the file containing the configuration parameters to
+ parse.
\return true if succeed, false otherwise.
*/
bool vpIoTools::loadConfigFile(const std::string &confFile)
{
configFile = path(confFile);
- configVars.clear();configValues.clear();
+ configVars.clear();
+ configValues.clear();
std::ifstream confContent(configFile.c_str(), std::ios::in);
- if(confContent.is_open())
- {
- std::string line,var,val;
+ if (confContent.is_open()) {
+ std::string line, var, val;
long unsigned int k;
int c;
std::string stop[3] = {" ", "\t", "#"};
- while(std::getline(confContent, line))
- {
- if((line.compare(0,1,"#") != 0) && (line.size() > 2))
- {
- try
- {
+ while (std::getline(confContent, line)) {
+ if ((line.compare(0, 1, "#") != 0) && (line.size() > 2)) {
+ try {
// name of the variable
k = (unsigned long)line.find(" ");
- var = line.substr(0,k);
+ var = line.substr(0, k);
// look for the end of the actual value
c = 200;
- for(unsigned i=0;i<3;++i)
- c = vpMath::minimum(c,(int)line.find(stop[i],k+1));
- if(c==-1)
+ for (unsigned i = 0; i < 3; ++i)
+ c = vpMath::minimum(c, (int)line.find(stop[i], k + 1));
+ if (c == -1)
c = (int)line.size();
- long unsigned int c_ = (long unsigned int) c;
- val = line.substr(k+1,c_-k-1);
+ long unsigned int c_ = (long unsigned int)c;
+ val = line.substr(k + 1, c_ - k - 1);
configVars.push_back(var);
configValues.push_back(val);
+ } catch (...) {
}
- catch(...){}
}
}
confContent.close();
- }
- else {
+ } else {
return false;
}
return true;
@@ -877,22 +936,20 @@ bool vpIoTools::loadConfigFile(const std::string &confFile)
bool vpIoTools::readConfigVar(const std::string &var, float &value)
{
bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- if(configValues[k].compare("PI") == 0)
- value = (float) M_PI;
- else if(configValues[k].compare("PI/2") == 0)
- value = (float) (M_PI/2.0);
- else if(configValues[k].compare("-PI/2") == 0)
- value = (float) (-M_PI/2.0);
- else
- value = (float) atof(configValues[k].c_str());
- found = true;
- }
+ for (unsigned int k = 0; k < configVars.size() && found == false; ++k) {
+ if (configVars[k] == var) {
+ if (configValues[k].compare("PI") == 0)
+ value = (float)M_PI;
+ else if (configValues[k].compare("PI/2") == 0)
+ value = (float)(M_PI / 2.0);
+ else if (configValues[k].compare("-PI/2") == 0)
+ value = (float)(-M_PI / 2.0);
+ else
+ value = (float)atof(configValues[k].c_str());
+ found = true;
}
- if(found == false)
+ }
+ if (found == false)
std::cout << var << " not found in config file" << std::endl;
return found;
}
@@ -907,22 +964,20 @@ bool vpIoTools::readConfigVar(const std::string &var, float &value)
bool vpIoTools::readConfigVar(const std::string &var, double &value)
{
bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- if(configValues[k].compare("PI") == 0)
- value = M_PI;
- else if(configValues[k].compare("PI/2") == 0)
- value = M_PI/2;
- else if(configValues[k].compare("-PI/2") == 0)
- value = -M_PI/2;
- else
- value = atof(configValues[k].c_str());
- found = true;
- }
+ for (unsigned int k = 0; k < configVars.size() && found == false; ++k) {
+ if (configVars[k] == var) {
+ if (configValues[k].compare("PI") == 0)
+ value = M_PI;
+ else if (configValues[k].compare("PI/2") == 0)
+ value = M_PI / 2;
+ else if (configValues[k].compare("-PI/2") == 0)
+ value = -M_PI / 2;
+ else
+ value = atof(configValues[k].c_str());
+ found = true;
}
- if(found == false)
+ }
+ if (found == false)
std::cout << var << " not found in config file" << std::endl;
return found;
}
@@ -938,15 +993,13 @@ bool vpIoTools::readConfigVar(const std::string &var, double &value)
bool vpIoTools::readConfigVar(const std::string &var, int &value)
{
bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- value = atoi(configValues[k].c_str());
- found = true;
- }
+ for (unsigned int k = 0; k < configVars.size() && found == false; ++k) {
+ if (configVars[k] == var) {
+ value = atoi(configValues[k].c_str());
+ found = true;
}
- if(found == false)
+ }
+ if (found == false)
std::cout << var << " not found in config file" << std::endl;
return found;
}
@@ -962,8 +1015,8 @@ bool vpIoTools::readConfigVar(const std::string &var, int &value)
bool vpIoTools::readConfigVar(const std::string &var, unsigned int &value)
{
int v = 0;
- bool found = readConfigVar(var,v);
- value = (unsigned int) v;
+ bool found = readConfigVar(var, v);
+ value = (unsigned int)v;
return found;
}
@@ -978,8 +1031,8 @@ bool vpIoTools::readConfigVar(const std::string &var, unsigned int &value)
bool vpIoTools::readConfigVar(const std::string &var, bool &value)
{
int v = 0;
- bool found = readConfigVar(var,v);
- value = (v!=0);
+ bool found = readConfigVar(var, v);
+ value = (v != 0);
return found;
}
@@ -994,7 +1047,7 @@ bool vpIoTools::readConfigVar(const std::string &var, bool &value)
bool vpIoTools::readConfigVar(const std::string &var, vpColor &value)
{
unsigned int v = 0;
- bool found = readConfigVar(var,v);
+ bool found = readConfigVar(var, v);
value = vpColor::getColor(v);
return found;
}
@@ -1010,15 +1063,13 @@ bool vpIoTools::readConfigVar(const std::string &var, vpColor &value)
bool vpIoTools::readConfigVar(const std::string &var, std::string &value)
{
bool found = false;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
- value = configValues[k];
- found = true;
- }
+ for (unsigned int k = 0; k < configVars.size() && found == false; ++k) {
+ if (configVars[k] == var) {
+ value = configValues[k];
+ found = true;
}
- if(found == false)
+ }
+ if (found == false)
std::cout << var << " not found in config file" << std::endl;
return found;
}
@@ -1026,7 +1077,8 @@ bool vpIoTools::readConfigVar(const std::string &var, std::string &value)
/*!
Tries to read the parameter named \e var as a \e vpMatrix.
If \e nCols and \e nRows are indicated, will resize the matrix.
- Otherwise, will try to read as many values as indicated by the dimension of \e value.
+ Otherwise, will try to read as many values as indicated by the dimension of
+ \e value.
\param var : Name of the parameter in the configuration file.
\param value : Value to be read.
@@ -1035,37 +1087,35 @@ 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<double> &value, const unsigned int &nCols, const unsigned int &nRows)
+bool vpIoTools::readConfigVar(const std::string &var, vpArray2D<double> &value, const unsigned int &nCols,
+ const unsigned int &nRows)
{
bool found = false;
std::string nb;
- for(unsigned int k=0;k<configVars.size() && found==false;++k)
- {
- if(configVars[k] == var)
- {
+ for (unsigned int k = 0; k < configVars.size() && found == false; ++k) {
+ if (configVars[k] == var) {
found = true;
// resize or not
- if(nCols != 0 && nRows != 0)
+ if (nCols != 0 && nRows != 0)
value.resize(nRows, nCols);
- size_t ind=0,ind2;
- for(unsigned int i=0;i<value.getRows();++i)
- for(unsigned int j=0;j<value.getCols();++j)
- {
- ind2 = configValues[k].find(",",ind);
- nb = configValues[k].substr(ind,ind2-ind);
- if(nb.compare("PI") == 0)
+ size_t ind = 0, ind2;
+ for (unsigned int i = 0; i < value.getRows(); ++i)
+ for (unsigned int j = 0; j < value.getCols(); ++j) {
+ ind2 = configValues[k].find(",", ind);
+ nb = configValues[k].substr(ind, ind2 - ind);
+ if (nb.compare("PI") == 0)
value[i][j] = M_PI;
- else if(nb.compare("PI/2") == 0)
- value[i][j] = M_PI/2;
- else if(nb.compare("-PI/2") == 0)
- value[i][j] = -M_PI/2;
+ else if (nb.compare("PI/2") == 0)
+ value[i][j] = M_PI / 2;
+ else if (nb.compare("-PI/2") == 0)
+ value[i][j] = -M_PI / 2;
else
value[i][j] = atof(nb.c_str());
- ind = ind2+1;
+ ind = ind2 + 1;
}
}
}
- if(found == false)
+ if (found == false)
std::cout << var << " not found in config file" << std::endl;
return found;
}
@@ -1073,7 +1123,8 @@ bool vpIoTools::readConfigVar(const std::string &var, vpArray2D<double> &value,
// construct experiment filename & path
/*!
- Augments the prefix of the experiment files by \e strTrue if \e cond is verified, and by \e strFalse otherwise.
+ Augments the prefix of the experiment files by \e strTrue if \e cond is
+ verified, and by \e strFalse otherwise.
\param strTrue : String to add if \e cond is true
\param cond : Condition managing the file name
@@ -1081,14 +1132,15 @@ bool vpIoTools::readConfigVar(const std::string &var, vpArray2D<double> &value,
*/
void vpIoTools::addNameElement(const std::string &strTrue, const bool &cond, const std::string &strFalse)
{
- if(cond)
+ if (cond)
baseName += "_" + strTrue;
- else if(strFalse != "")
+ else if (strFalse != "")
baseName += "_" + strFalse;
}
/*!
- Augments the prefix of the experiment files by \e strTrue followed by \e val.
+ Augments the prefix of the experiment files by \e strTrue followed by \e
+ val.
\param strTrue : String to add
\param val : Value to add
@@ -1096,18 +1148,17 @@ void vpIoTools::addNameElement(const std::string &strTrue, const bool &cond, con
*/
void vpIoTools::addNameElement(const std::string &strTrue, const double &val)
{
- //if(val != 0.)
- if(std::fabs(val) < std::numeric_limits<double>::epsilon())
- {
- char valC[256];
- sprintf(valC, "%.3f", val);
- std::string valS(valC);
- baseName += "_" + strTrue + valS;
- }
+ // if(val != 0.)
+ if (std::fabs(val) < std::numeric_limits<double>::epsilon()) {
+ char valC[256];
+ sprintf(valC, "%.3f", val);
+ std::string valS(valC);
+ baseName += "_" + strTrue + valS;
+ }
}
/*!
- Creates the directory \e baseDir/baseName. If already exists, empties
+ Creates the directory \e baseDir/baseName. If already exists, empties
it if \e empty is true.
Useful to save the images corresponding to a particular experiment.
@@ -1116,12 +1167,11 @@ void vpIoTools::addNameElement(const std::string &strTrue, const double &val)
*/
void vpIoTools::createBaseNamePath(const bool &empty)
{
- if(vpIoTools::checkDirectory(baseDir + baseName) == false) {
+ if (vpIoTools::checkDirectory(baseDir + baseName) == false) {
vpIoTools::makeDirectory(baseDir + baseName);
std::cout << "creating directory " + baseDir + baseName << std::endl;
- }
- else {
- if(empty) {
+ } else {
+ if (empty) {
std::cout << "emptying directory " + baseDir + baseName << std::endl;
vpIoTools::remove(baseDir + baseName + "/*");
}
@@ -1136,7 +1186,7 @@ void vpIoTools::createBaseNamePath(const bool &empty)
*/
void vpIoTools::saveConfigFile(const bool &actuallySave)
{
- if(actuallySave) {
+ if (actuallySave) {
std::string dest = baseDir + "/" + baseName + "_config.txt";
// file copy
vpIoTools::copy(configFile, dest);
@@ -1144,24 +1194,31 @@ void vpIoTools::saveConfigFile(const bool &actuallySave)
}
/*!
- Get ViSP images data path. ViSP images data can be installed from Debian or Ubuntu \e visp-images-data package.
- It can be also installed from ViSP-images.zip that can be found on http://visp.inria.fr/download page.
+ Get ViSP images data path. ViSP images data can be installed from Debian or
+ Ubuntu \e visp-images-data package. It can be also installed from
+ ViSP-images.zip that can be found on http://visp.inria.fr/download page.
This function returns the path to the folder that contains the data.
- - It checks first if \e visp-images-data package is installed. In that case returns then \e /usr/share/visp-images-data".
- - Then it checks if VISP_INPUT_IMAGE_PATH environment variable that gives the location of the data is set. In that
- case returns the content of this environment var.
+ - It checks first if \e visp-images-data package is installed. In that case
+ returns then \e /usr/share/visp-images-data".
+ - Then it checks if VISP_INPUT_IMAGE_PATH environment variable that gives the
+ location of the data is set. In that case returns the content of this
+ environment var.
If the path is not found, returns an empty string.
*/
std::string vpIoTools::getViSPImagesDataPath()
{
std::string data_path;
- std::string file_to_test("ViSP-images/mbt/cube.cao");
+ std::string file_to_test("mbt/cube.cao");
std::string filename;
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
// Test if visp-images-data package is u-installed (Ubuntu and Debian)
- data_path = "/usr/share/visp-images-data";
+ data_path = "/usr/share/visp-images-data/ViSP-images";
+ filename = data_path + "/" + file_to_test;
+ if (vpIoTools::checkFilename(filename))
+ return data_path;
+ data_path = "/usr/share/visp-images-data/visp-images";
filename = data_path + "/" + file_to_test;
if (vpIoTools::checkFilename(filename))
return data_path;
@@ -1172,25 +1229,34 @@ std::string vpIoTools::getViSPImagesDataPath()
filename = data_path + "/" + file_to_test;
if (vpIoTools::checkFilename(filename))
return data_path;
- }
- catch(...) {
+ data_path = vpIoTools::getenv("VISP_INPUT_IMAGE_PATH") + "/ViSP-images";
+ filename = data_path + "/" + file_to_test;
+ if (vpIoTools::checkFilename(filename))
+ return data_path;
+ data_path = vpIoTools::getenv("VISP_INPUT_IMAGE_PATH") + "/visp-images";
+ filename = data_path + "/" + file_to_test;
+ if (vpIoTools::checkFilename(filename))
+ return data_path;
+ } catch (...) {
}
data_path = "";
return data_path;
}
/*!
- Returns the extension of the file or an empty string if the file has no extension. If checkFile flag is set,
- it will check first if the pathname denotes a directory and so return an empty string and second it will check
- if the file denoted by the pathanme exists. If so, it will return the extension if present.
- \param pathname : The pathname of the file we want to get the extension.
- \param checkFile : If true, the file must exist otherwise an empty string will be returned.
- \return The extension of the file or an empty string if the file has no extension.
- or if the pathname is empty.
+ Returns the extension of the file or an empty string if the file has no
+ extension. If checkFile flag is set, it will check first if the pathname
+ denotes a directory and so return an empty string and second it will check
+ if the file denoted by the pathanme exists. If so, it will return the
+ extension if present. \param pathname : The pathname of the file we want to
+ get the extension. \param checkFile : If true, the file must exist
+ otherwise an empty string will be returned. \return The extension of the
+ file or an empty string if the file has no extension. or if the pathname is
+ empty.
*/
-std::string vpIoTools::getFileExtension(const std::string& pathname, const bool checkFile)
+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 "";
}
@@ -1199,59 +1265,59 @@ std::string vpIoTools::getFileExtension(const std::string& pathname, const bool
std::string altsep = "/";
std::string extsep = ".";
#else
- //On Unix, or on the Mac
+ // On Unix, or on the Mac
std::string sep = "/";
std::string altsep = "";
std::string extsep = ".";
#endif
- //Python 2.7.8 module.
-//# Split a path in root and extension.
-//# The extension is everything starting at the last dot in the last
-//# pathname component; the root is everything before that.
-//# It is always true that root + ext == p.
-//
-//# Generic implementation of splitext, to be parametrized with
-//# the separators
-//def _splitext(p, sep, altsep, extsep):
-// """Split the extension from a pathname.
-//
-// Extension is everything from the last dot to the end, ignoring
-// leading dots. Returns "(root, ext)"; ext may be empty."""
-//
-// sepIndex = p.rfind(sep)
-// if altsep:
-// altsepIndex = p.rfind(altsep)
-// sepIndex = max(sepIndex, altsepIndex)
-//
-// dotIndex = p.rfind(extsep)
-// if dotIndex > sepIndex:
-// # skip all leading dots
-// filenameIndex = sepIndex + 1
-// while filenameIndex < dotIndex:
-// if p[filenameIndex] != extsep:
-// return p[:dotIndex], p[dotIndex:]
-// filenameIndex += 1
-//
-// return p, ''
+ // Python 2.7.8 module.
+ //# Split a path in root and extension.
+ //# The extension is everything starting at the last dot in the last
+ //# pathname component; the root is everything before that.
+ //# It is always true that root + ext == p.
+ //
+ //# Generic implementation of splitext, to be parametrized with
+ //# the separators
+ // def _splitext(p, sep, altsep, extsep):
+ // """Split the extension from a pathname.
+ //
+ // Extension is everything from the last dot to the end, ignoring
+ // leading dots. Returns "(root, ext)"; ext may be empty."""
+ //
+ // sepIndex = p.rfind(sep)
+ // if altsep:
+ // altsepIndex = p.rfind(altsep)
+ // sepIndex = max(sepIndex, altsepIndex)
+ //
+ // dotIndex = p.rfind(extsep)
+ // if dotIndex > sepIndex:
+ // # skip all leading dots
+ // filenameIndex = sepIndex + 1
+ // while filenameIndex < dotIndex:
+ // if p[filenameIndex] != extsep:
+ // return p[:dotIndex], p[dotIndex:]
+ // filenameIndex += 1
+ //
+ // return p, ''
int sepIndex = (int)pathname.rfind(sep);
- if(!altsep.empty()) {
+ if (!altsep.empty()) {
int altsepIndex = (int)pathname.rfind(altsep);
- sepIndex = (std::max)(sepIndex, altsepIndex);
+ sepIndex = ((std::max))(sepIndex, altsepIndex);
}
size_t dotIndex = pathname.rfind(extsep);
- if(dotIndex != std::string::npos) {
- //The extsep character exists
- if((sepIndex != (int)std::string::npos && (int)dotIndex > sepIndex) || sepIndex == (int)std::string::npos) {
- if(sepIndex == (int)std::string::npos) {
+ if (dotIndex != std::string::npos) {
+ // The extsep character exists
+ if ((sepIndex != (int)std::string::npos && (int)dotIndex > sepIndex) || sepIndex == (int)std::string::npos) {
+ if (sepIndex == (int)std::string::npos) {
sepIndex = -1;
}
size_t filenameIndex = (size_t)(sepIndex + 1);
- while(filenameIndex < dotIndex) {
- if(pathname.compare(filenameIndex, 1, extsep) != 0) {
+ while (filenameIndex < dotIndex) {
+ if (pathname.compare(filenameIndex, 1, extsep) != 0) {
return pathname.substr(dotIndex);
}
filenameIndex++;
@@ -1259,7 +1325,6 @@ std::string vpIoTools::getFileExtension(const std::string& pathname, const bool
}
}
-
return "";
}
@@ -1268,14 +1333,13 @@ std::string vpIoTools::getFileExtension(const std::string& pathname, const bool
\return The name of the file or directory denoted by this pathname, or an
empty string if this pathname's name sequence is empty.
*/
-std::string vpIoTools::getName(const std::string& pathname)
+std::string vpIoTools::getName(const std::string &pathname)
{
- if(pathname.size() > 0)
- {
+ if (pathname.size() > 0) {
std::string convertedPathname = vpIoTools::path(pathname);
size_t index = convertedPathname.find_last_of(vpIoTools::separator);
- if(index != std::string::npos) {
+ if (index != std::string::npos) {
return convertedPathname.substr(index + 1);
}
@@ -1286,11 +1350,12 @@ std::string vpIoTools::getName(const std::string& pathname)
}
/*!
- Returns the name of the file without extension or directory denoted by this pathname.
- \return The name of the file without extension or directory denoted by this pathname, or an
- empty string if this pathname's name sequence is empty.
+ Returns the name of the file without extension or directory denoted by this
+ pathname. \return The name of the file without extension or directory
+ denoted by this pathname, or an empty string if this pathname's name
+ sequence is empty.
*/
-std::string vpIoTools::getNameWE(const std::string& pathname)
+std::string vpIoTools::getNameWE(const std::string &pathname)
{
std::string name = vpIoTools::getName(pathname);
size_t found = name.find_last_of(".");
@@ -1299,33 +1364,34 @@ std::string vpIoTools::getNameWE(const std::string& pathname)
}
/*!
- Returns the pathname string of this pathname's parent.
+ Returns the pathname string of this pathname's parent.
\return The pathname string of this pathname's parent, or
an empty string if this pathname does not name a parent directory.
*/
-std::string vpIoTools::getParent(const std::string& pathname)
+std::string vpIoTools::getParent(const std::string &pathname)
{
- if(pathname.size() > 0)
- {
- std::string convertedPathname = vpIoTools::path(pathname);
+ if (pathname.size() > 0) {
+ std::string convertedPathname = vpIoTools::path(pathname);
- size_t index = convertedPathname.find_last_of(vpIoTools::separator);
- if(index != std::string::npos) {
- return convertedPathname.substr(0, index);
- }
- }
+ size_t index = convertedPathname.find_last_of(vpIoTools::separator);
+ if (index != std::string::npos) {
+ return convertedPathname.substr(0, index);
+ }
+ }
- return "";
+ return "";
}
/*!
- Returns the absolute path using realpath() on Unix systems or GetFullPathName() on Windows systems.
- \return According to realpath() manual, returns an absolute pathname that names the same file,
- whose resolution does not involve '.', '..', or symbolic links for Unix systems.
- According to GetFullPathName() documentation, retrieves the full path of the specified file for
- Windows systems.
+ Returns the absolute path using realpath() on Unix systems or
+ GetFullPathName() on Windows systems. \return According to realpath()
+ manual, returns an absolute pathname that names the same file, whose
+ resolution does not involve '.', '..', or symbolic links for Unix systems.
+ According to GetFullPathName() documentation, retrieves the full path of the
+ specified file for Windows systems.
*/
-std::string vpIoTools::getAbsolutePathname(const std::string &pathname) {
+std::string vpIoTools::getAbsolutePathname(const std::string &pathname)
+{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
std::string real_path_str = pathname;
@@ -1337,7 +1403,7 @@ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) {
}
return real_path_str;
#elif defined(_WIN32)
-# if ( ! defined(WINRT) )
+#if (!defined(WINRT))
std::string real_path_str = pathname;
DWORD retval = 0;
TCHAR buffer[4096] = TEXT("");
@@ -1347,9 +1413,12 @@ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) {
real_path_str = buffer;
}
return real_path_str;
-# else
- throw(vpIoException(vpException::fatalError, "Cannot get absolute path of %s: not implemented on Universal Windows Platform", pathname.c_str()));
-# endif
+#else
+ throw(vpIoException(vpException::fatalError,
+ "Cannot get absolute path of %s: not implemented on "
+ "Universal Windows Platform",
+ pathname.c_str()));
+#endif
#endif
}
@@ -1363,45 +1432,40 @@ 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 "";
- }
-
- if(child.size() == 0)
- {
- return vpIoTools::path(parent);
- }
-
- if(parent.size() == 0)
- {
- return vpIoTools::path(child);
- }
-
- std::string convertedParent = vpIoTools::path(parent);
- std::string convertedChild = vpIoTools::path(child);
-
- std::stringstream ss;
- ss << vpIoTools::separator;
- std::string stringSeparator;
- ss >> stringSeparator;
-
- std::string lastConvertedParentChar = convertedParent.substr(convertedParent.size() - 1);
- std::string firstConvertedChildChar = convertedChild.substr(0, 1);
-
- if(lastConvertedParentChar == stringSeparator)
- {
- convertedParent = convertedParent.substr(0, convertedParent.size() - 1);
- }
-
- if(firstConvertedChildChar == stringSeparator)
- {
- convertedChild = convertedChild.substr(1);
- }
-
- return std::string(convertedParent + vpIoTools::separator + convertedChild);
+ if (child.size() == 0 && parent.size() == 0) {
+ return "";
+ }
+
+ if (child.size() == 0) {
+ return vpIoTools::path(parent);
+ }
+
+ if (parent.size() == 0) {
+ return vpIoTools::path(child);
+ }
+
+ std::string convertedParent = vpIoTools::path(parent);
+ std::string convertedChild = vpIoTools::path(child);
+
+ std::stringstream ss;
+ ss << vpIoTools::separator;
+ std::string stringSeparator;
+ ss >> stringSeparator;
+
+ std::string lastConvertedParentChar = convertedParent.substr(convertedParent.size() - 1);
+ std::string firstConvertedChildChar = convertedChild.substr(0, 1);
+
+ if (lastConvertedParentChar == stringSeparator) {
+ convertedParent = convertedParent.substr(0, convertedParent.size() - 1);
+ }
+
+ if (firstConvertedChildChar == stringSeparator) {
+ convertedChild = convertedChild.substr(1);
+ }
+
+ return std::string(convertedParent + vpIoTools::separator + convertedChild);
}
/*!
@@ -1409,35 +1473,37 @@ std::string vpIoTools::createFilePath(const std::string& parent, const std::stri
\return true if the pathname is absolute, false otherwise.
*/
-bool vpIoTools::isAbsolutePathname(const std::string& pathname)
+bool vpIoTools::isAbsolutePathname(const std::string &pathname)
{
//# Inspired by the Python 2.7.8 module.
- //# Return whether a path is absolute.
- //# Trivial in Posix, harder on the Mac or MS-DOS.
- //# For DOS it is absolute if it starts with a slash or backslash (current
- //# volume), or if a pathname after the volume letter and colon / UNC resource
- //# starts with a slash or backslash.
- //
- //def isabs(s):
- // """Test whether a path is absolute"""
- // s = splitdrive(s)[1]
- // return s != '' and s[:1] in '/\\'
- std::string path = splitDrive(pathname).second;
- return path.size() > 0 && (path.substr(0, 1) == "/" || path.substr(0, 1) == "\\");
+ //# Return whether a path is absolute.
+ //# Trivial in Posix, harder on the Mac or MS-DOS.
+ //# For DOS it is absolute if it starts with a slash or backslash (current
+ //# volume), or if a pathname after the volume letter and colon / UNC
+ // resource # starts with a slash or backslash.
+ //
+ // def isabs(s):
+ // """Test whether a path is absolute"""
+ // s = splitdrive(s)[1]
+ // return s != '' and s[:1] in '/\\'
+ std::string path = splitDrive(pathname).second;
+ return path.size() > 0 && (path.substr(0, 1) == "/" || path.substr(0, 1) == "\\");
}
/*!
Return true if the two pathnames are identical.
\return true if the two pathnames are identical, false otherwise.
- \note It uses path() to normalize the path and getAbsolutePathname() to get the absolute 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) {
- //Normalize path
+bool vpIoTools::isSamePathname(const std::string &pathname1, const std::string &pathname2)
+{
+ // Normalize path
std::string path1_normalize = vpIoTools::path(pathname1);
std::string path2_normalize = vpIoTools::path(pathname2);
- //Get absolute path
+ // Get absolute path
path1_normalize = vpIoTools::getAbsolutePathname(path1_normalize);
path2_normalize = vpIoTools::getAbsolutePathname(path2_normalize);
@@ -1445,32 +1511,35 @@ bool vpIoTools::isSamePathname(const std::string& pathname1, const std::string&
}
/*!
- Split a path in a drive specification (a drive letter followed by a colon) and the path specification.
- It is always true that drivespec + pathspec == p
- Inspired by the Python 2.7.8 module.
- \return a pair whose the first element is the drive specification and the second element
- the path specification
+ Split a path in a drive specification (a drive letter followed by a colon)
+ and the path specification. It is always true that drivespec + pathspec ==
+ p Inspired by the Python 2.7.8 module. \return a pair whose the first
+ element is the drive specification and the second element the path
+ specification
*/
-std::pair<std::string, std::string> vpIoTools::splitDrive(const std::string& pathname)
+std::pair<std::string, std::string> vpIoTools::splitDrive(const std::string &pathname)
{
//# Split a path in a drive specification (a drive letter followed by a
//# colon) and the path specification.
//# It is always true that drivespec + pathspec == p
-//def splitdrive(p):
-// """Split a pathname into drive/UNC sharepoint and relative path specifiers.
-// Returns a 2-tuple (drive_or_unc, path); either part may be empty.
+// def splitdrive(p):
+// """Split a pathname into drive/UNC sharepoint and relative path
+// specifiers. Returns a 2-tuple (drive_or_unc, path); either part may be
+// empty.
//
// If you assign
// result = splitdrive(p)
// It is always true that:
// result[0] + result[1] == p
//
-// If the path contained a drive letter, drive_or_unc will contain everything
-// up to and including the colon. e.g. splitdrive("c:/dir") returns ("c:", "/dir")
+// If the path contained a drive letter, drive_or_unc will contain
+// everything up to and including the colon. e.g. splitdrive("c:/dir")
+// returns ("c:", "/dir")
//
-// If the path contained a UNC path, the drive_or_unc will contain the host name
-// and share up to but not including the fourth directory separator character.
-// e.g. splitdrive("//host/computer/dir") returns ("//host/computer", "/dir")
+// If the path contained a UNC path, the drive_or_unc will contain the host
+// name and share up to but not including the fourth directory separator
+// character. e.g. splitdrive("//host/computer/dir") returns
+// ("//host/computer", "/dir")
//
// Paths cannot contain both a drive letter and a UNC path.
//
@@ -1497,51 +1566,51 @@ std::pair<std::string, std::string> vpIoTools::splitDrive(const std::string& pat
// return p[:2], p[2:]
// return '', p
-
- //On Unix, the drive is always empty.
- //On the Mac, the drive is always empty (don't use the volume name -- it doesn't have the same
- //syntactic and semantic oddities as DOS drive letters, such as there being a separate current directory per drive).
+// On Unix, the drive is always empty.
+// On the Mac, the drive is always empty (don't use the volume name -- it
+// doesn't have the same syntactic and semantic oddities as DOS drive
+// letters, such as there being a separate current directory per drive).
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
return std::pair<std::string, std::string>("", pathname);
#else
- const std::string sep = "\\";
- const std::string sepsep = "\\\\";
- const std::string altsep = "/";
-
- if(pathname.size() > 1) {
- std::string normPathname = pathname;
- std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), *sep.c_str());
-
- if(normPathname.substr(0, 2) == sepsep && normPathname.substr(2, 1) != sep) {
- // is a UNC path:
- // vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
- // \\machine\mountpoint\directory\etc\...
- // directory ^^^^^^^^^^^^^^^
- size_t index = normPathname.find(sep, 2);
- if(index == std::string::npos) {
- return std::pair<std::string, std::string>("", pathname);
- }
-
- size_t index2 = normPathname.find(sep, index + 1);
- //# a UNC path can't have two slashes in a row
- //# (after the initial two)
- if(index2 == index + 1) {
- return std::pair<std::string, std::string>("", pathname);
- }
-
- if(index2 == std::string::npos) {
- index2 = pathname.size();
- }
-
- return std::pair<std::string, std::string>(pathname.substr(0, index2), pathname.substr(index2));
- }
-
- if(normPathname[1] == ':') {
- return std::pair<std::string, std::string>(pathname.substr(0, 2), pathname.substr(2));
- }
- }
-
- return std::pair<std::string, std::string>("", pathname);
+ const std::string sep = "\\";
+ const std::string sepsep = "\\\\";
+ const std::string altsep = "/";
+
+ if (pathname.size() > 1) {
+ std::string normPathname = pathname;
+ std::replace(normPathname.begin(), normPathname.end(), *altsep.c_str(), *sep.c_str());
+
+ if (normPathname.substr(0, 2) == sepsep && normPathname.substr(2, 1) != sep) {
+ // is a UNC path:
+ // vvvvvvvvvvvvvvvvvvvv drive letter or UNC path
+ // \\machine\mountpoint\directory\etc\...
+ // directory ^^^^^^^^^^^^^^^
+ size_t index = normPathname.find(sep, 2);
+ if (index == std::string::npos) {
+ return std::pair<std::string, std::string>("", pathname);
+ }
+
+ size_t index2 = normPathname.find(sep, index + 1);
+ //# a UNC path can't have two slashes in a row
+ //# (after the initial two)
+ if (index2 == index + 1) {
+ return std::pair<std::string, std::string>("", pathname);
+ }
+
+ if (index2 == std::string::npos) {
+ index2 = pathname.size();
+ }
+
+ return std::pair<std::string, std::string>(pathname.substr(0, index2), pathname.substr(index2));
+ }
+
+ if (normPathname[1] == ':') {
+ return std::pair<std::string, std::string>(pathname.substr(0, 2), pathname.substr(2));
+ }
+ }
+
+ return std::pair<std::string, std::string>("", pathname);
#endif
}
@@ -1593,7 +1662,7 @@ other
example
\endcode
*/
-std::vector<std::string> vpIoTools::splitChain(const std::string & chain, const std::string & sep)
+std::vector<std::string> vpIoTools::splitChain(const std::string &chain, const std::string &sep)
{
size_t startIndex = 0;
@@ -1601,11 +1670,11 @@ std::vector<std::string> vpIoTools::splitChain(const std::string & chain, const
std::vector<std::string> subChain;
size_t sepIndex = chainToSplit.find(sep);
- while(sepIndex != std::string::npos) {
+ while (sepIndex != std::string::npos) {
std::string sub = chainToSplit.substr(startIndex, sepIndex);
- if (! sub.empty())
- subChain.push_back( sub );
- chainToSplit = chainToSplit.substr(sepIndex+1, chain.size()-1);
+ if (!sub.empty())
+ subChain.push_back(sub);
+ chainToSplit = chainToSplit.substr(sepIndex + 1, chain.size() - 1);
sepIndex = chainToSplit.find(sep);
}
@@ -1614,3 +1683,71 @@ std::vector<std::string> vpIoTools::splitChain(const std::string & chain, const
return subChain;
}
+
+/*!
+ List of files in directory, in alphabetical order.
+ There is no difference if pathname contains terminating backslash or not
+ Unlike scandir(), does not return "." and ".."
+ \param pathname : path to directory
+ \return A vector of files' names in that directory
+ */
+std::vector<std::string> vpIoTools::getDirFiles(const std::string &pathname)
+{
+
+ if (!checkDirectory(pathname)) {
+ 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
+
+ std::vector<std::string> 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()));
+ }
+ for (int i = 0; i < filesCount; i++) {
+ std::string fileName = list[i]->d_name;
+ if (fileName != "." && fileName != "..") {
+ files.push_back(fileName);
+ }
+ free(list[i]);
+ }
+ free(list);
+ std::sort(files.begin(), files.end());
+ return files;
+
+#elif defined(_WIN32)
+#if (!defined(WINRT))
+
+ std::vector<std::string> files;
+ std::string fileMask = dirName;
+ fileMask.append("\\*");
+ WIN32_FIND_DATA FindFileData;
+ HANDLE hFind = FindFirstFile(fileMask.c_str(), &FindFileData);
+ // Directory is empty
+ if (HandleToLong(&hFind) == ERROR_FILE_NOT_FOUND) {
+ return files;
+ }
+ if (hFind == INVALID_HANDLE_VALUE) {
+ throw(vpIoException(vpException::fatalError, "Cannot read files of directory %s", dirName.c_str()));
+ }
+ do {
+ std::string fileName = FindFileData.cFileName;
+ if (fileName != "." && fileName != "..") {
+ files.push_back(fileName);
+ }
+ } while (FindNextFile(hFind, &FindFileData));
+ FindClose(hFind);
+ std::sort(files.begin(), files.end());
+ return files;
+
+#else
+ throw(vpIoException(vpException::fatalError,
+ "Cannot read files of directory %s: not implemented on "
+ "Universal Windows Platform",
+ dirName.c_str()));
+#endif
+#endif
+}
diff --git a/modules/core/src/tools/geometry/vpPlane.cpp b/modules/core/src/tools/geometry/vpPlane.cpp
index 19c65aa..1754203 100644
--- a/modules/core/src/tools/geometry/vpPlane.cpp
+++ b/modules/core/src/tools/geometry/vpPlane.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpPlane.cpp
\brief definition of the vpPlane class member functions
@@ -44,21 +44,20 @@
#include <visp3/core/vpPlane.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
/*!
Copy operator.
*/
-vpPlane&
-vpPlane::operator =(const vpPlane& p)
+vpPlane &vpPlane::operator=(const vpPlane &p)
{
- A = p.A ;
- B = p.B ;
- C = p.C ;
- D = p.D ;
+ A = p.A;
+ B = p.B;
+ C = p.C;
+ D = p.D;
- return *this ;
+ return *this;
}
/*!
@@ -76,22 +75,20 @@ 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.
*/
-vpPlane::vpPlane(const vpPlane& P)
- : A(0), B(0), C(0), D(0)
+vpPlane::vpPlane(const vpPlane &P) : A(0), B(0), C(0), D(0)
{
- setA(P.getA()) ;
- setB(P.getB()) ;
- setC(P.getC()) ;
- setD(P.getD()) ;
+ setA(P.getA());
+ setB(P.getB());
+ setC(P.getC());
+ setD(P.getD());
}
-/*!
+/*!
Plane constructor from a point \e P on the plane and the normal
\e n to the plane.
@@ -100,28 +97,27 @@ vpPlane::vpPlane(const vpPlane& P)
(x,y,z) are the coordinates of a point and \f$[A,B,C]^T\f$ is the normal
vector of the plane.
- \param P : A point with coordinates (x,y,z) on the plane. The \e frame parameter indicates
- if the coordinates of this points that are used are expressed in the camera of object frame.
-
+ \param P : A point with coordinates (x,y,z) on the plane. The \e frame
+ parameter indicates if the coordinates of this points that are used are
+ expressed in the camera of object frame.
+
\param n : The normal to the plane.
\param frame: Indicates if the plane should be initialized from the point P
coordinates expressed in the camera or object frame.
*/
-vpPlane::vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame)
- : A(0), B(0), C(0), D(0)
+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:
+ // Equation of the plane is given by:
A = n[0];
B = n[1];
C = n[2];
if (frame == vpPlane::camera_frame)
- D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
+ D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z());
else
- D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
-
+ D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
}
/*!
@@ -129,12 +125,12 @@ vpPlane::vpPlane(const vpPoint& P, const vpColVector &n, vpPlaneFrame frame)
\param P : Plane used as initializer.
*/
-void vpPlane::init(const vpPlane& P)
+void vpPlane::init(const vpPlane &P)
{
- setA(P.getA()) ;
- setB(P.getB()) ;
- setC(P.getC()) ;
- setD(P.getD()) ;
+ setA(P.getA());
+ setB(P.getB());
+ setC(P.getC());
+ setD(P.getD());
}
/*!
@@ -143,19 +139,19 @@ void vpPlane::init(const vpPlane& P)
\param P : A point with coordinates (x,y,z) on the plane.
The size of the vector should be 3, with P[0]=x, with P[1]=y, with P[2]=z.
-
+
\param n : The normal to the plane.
\sa vpPlane(const vpPoint&, const vpColVector &)
*/
-void vpPlane::init(const vpColVector & P, const vpColVector &n)
+void vpPlane::init(const vpColVector &P, const vpColVector &n)
{
- //Equation of the plane is given by:
+ // Equation of the plane is given by:
A = n[0];
B = n[1];
C = n[2];
- D=-(A*P[0]+B*P[1]+C*P[2]);
+ D = -(A * P[0] + B * P[1] + C * P[2]);
}
/*!
@@ -169,54 +165,51 @@ 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);
vpColVector n(3);
if (frame == vpPlane::camera_frame) {
- //Calculate vector corresponding to PQ
- a[0]=P.get_X()-Q.get_X();
- a[1]=P.get_Y()-Q.get_Y();
- a[2]=P.get_Z()-Q.get_Z();
-
- //Calculate vector corresponding to PR
- b[0]=P.get_X()-R.get_X();
- b[1]=P.get_Y()-R.get_Y();
- b[2]=P.get_Z()-R.get_Z();
- }
- else {
- //Calculate vector corresponding to PQ
- a[0]=P.get_oX()-Q.get_oX();
- a[1]=P.get_oY()-Q.get_oY();
- a[2]=P.get_oZ()-Q.get_oZ();
-
- //Calculate vector corresponding to PR
- b[0]=P.get_oX()-R.get_oX();
- b[1]=P.get_oY()-R.get_oY();
- b[2]=P.get_oZ()-R.get_oZ();
+ // Calculate vector corresponding to PQ
+ a[0] = P.get_X() - Q.get_X();
+ a[1] = P.get_Y() - Q.get_Y();
+ a[2] = P.get_Z() - Q.get_Z();
+
+ // Calculate vector corresponding to PR
+ b[0] = P.get_X() - R.get_X();
+ b[1] = P.get_Y() - R.get_Y();
+ b[2] = P.get_Z() - R.get_Z();
+ } else {
+ // Calculate vector corresponding to PQ
+ a[0] = P.get_oX() - Q.get_oX();
+ a[1] = P.get_oY() - Q.get_oY();
+ a[2] = P.get_oZ() - Q.get_oZ();
+
+ // Calculate vector corresponding to PR
+ b[0] = P.get_oX() - R.get_oX();
+ b[1] = P.get_oY() - R.get_oY();
+ b[2] = P.get_oZ() - R.get_oZ();
}
- //Calculate normal vector to plane PQ x PR
- n=vpColVector::cross(a,b);
+ // Calculate normal vector to plane PQ x PR
+ n = vpColVector::cross(a, b);
- //Equation of the plane is given by:
+ // Equation of the plane is given by:
A = n[0];
B = n[1];
C = n[2];
if (frame == vpPlane::camera_frame)
- D=-(A*P.get_X()+B*P.get_Y()+C*P.get_Z());
+ D = -(A * P.get_X() + B * P.get_Y() + C * P.get_Z());
else
- D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
+ D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
- double norm = sqrt(A*A+B*B+C*C) ;
- A /= norm ;
- B /= norm ;
- C /= norm ;
- D /= norm ;
+ double norm = sqrt(A * A + B * B + C * C);
+ A /= norm;
+ B /= norm;
+ C /= norm;
+ D /= norm;
}
-
/*!
Compute the equation of a plane given three point P, Q, R.
@@ -229,10 +222,9 @@ vpPlane::init(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFrame
\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) ;
+ init(P, Q, R, frame);
}
/*!
@@ -243,14 +235,14 @@ vpPlane::vpPlane(const vpPoint &P, const vpPoint &Q, const vpPoint &R, vpPlaneFr
\sa getNormal(vpColVector &n)
*/
-vpColVector vpPlane::getNormal() const
+vpColVector vpPlane::getNormal() const
{
vpColVector n(3);
- n[0] = A ;
- n[1] = B ;
- n[2] = C ;
+ n[0] = A;
+ n[1] = B;
+ n[2] = C;
- return n ;
+ return n;
}
/*!
@@ -263,12 +255,12 @@ vpColVector vpPlane::getNormal() const
\sa getNormal()
*/
-void vpPlane::getNormal(vpColVector &n) const
+void vpPlane::getNormal(vpColVector &n) const
{
- n.resize(3) ;
- n[0] = A ;
- n[1] = B ;
- n[2] = C ;
+ n.resize(3);
+ n[0] = A;
+ n[1] = B;
+ n[2] = C;
}
/*!
@@ -277,88 +269,78 @@ void vpPlane::getNormal(vpColVector &n) const
\param P : point to be projected on the plane
\param Pproj : result of the projection (pproj belongs to the plane)
*/
-void
-vpPlane::projectionPointOnPlan(const vpPoint& P, vpPoint& Pproj) const
+void vpPlane::projectionPointOnPlan(const vpPoint &P, vpPoint &Pproj) const
{
- double x0,y0,z0 ;
- double rho ;
+ double x0, y0, z0;
+ double rho;
- x0 = P.get_X()/P.get_W() ;
- y0 = P.get_Y()/P.get_W() ;
- z0 = P.get_Z()/P.get_W() ;
+ x0 = P.get_X() / P.get_W();
+ y0 = P.get_Y() / P.get_W();
+ z0 = P.get_Z() / P.get_W();
- rho = - (A*x0+B*y0+C*z0+D)/(A*A+B*B+C*C) ;
+ rho = -(A * x0 + B * y0 + C * z0 + D) / (A * A + B * B + C * C);
- Pproj.set_X(x0+A*rho) ;
- Pproj.set_Y(y0+B*rho) ;
- Pproj.set_Z(z0+C*rho) ;
- Pproj.set_W(1) ;
+ Pproj.set_X(x0 + A * rho);
+ Pproj.set_Y(y0 + B * rho);
+ Pproj.set_Z(z0 + C * rho);
+ Pproj.set_W(1);
}
-
-double
-vpPlane::rayIntersection(const vpPoint &M0,
- const vpPoint &M1,
- vpColVector &H ) const
+double vpPlane::rayIntersection(const vpPoint &M0, const vpPoint &M1, vpColVector &H) const
{
- double k,scal;
+ double k, scal;
// if(M0.get_X()!=0 || M0.get_Y()!=0 || M0.get_Z()!=0)
- if(std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon()
- || std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon()
- || std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon())
- {
+ if (std::fabs(M0.get_X()) > std::numeric_limits<double>::epsilon() ||
+ std::fabs(M0.get_Y()) > std::numeric_limits<double>::epsilon() ||
+ std::fabs(M0.get_Z()) > std::numeric_limits<double>::epsilon()) {
double R[3];
- R[0]= M1.get_X() - M0.get_X();
- R[1]= M1.get_Y() - M0.get_Y();
- R[2]= M1.get_Z() - M0.get_Z();
+ R[0] = M1.get_X() - M0.get_X();
+ R[1] = M1.get_Y() - M0.get_Y();
+ R[2] = M1.get_Z() - M0.get_Z();
- scal = getA()*R[0] + getB()*R[1] + getC()*R[2];
- //if (scal != 0)
+ scal = getA() * R[0] + getB() * R[1] + getC() * R[2];
+ // if (scal != 0)
if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -( getA()*M0.get_X() + getB()*M0.get_Y() + getC()*M0.get_Z() + getD())/scal;
+ k = -(getA() * M0.get_X() + getB() * M0.get_Y() + getC() * M0.get_Z() + getD()) / scal;
else
k = 0;
- H[0] = M0.get_X()+ k*R[0];
- H[1] = M0.get_Y()+ k*R[1];
- H[2] = M0.get_Z()+ k*R[2];
- }
- else
- {
- scal = getA()*M1.get_X() + getB()*M1.get_Y() + getC()*M1.get_Z();
- //if (scal != 0)
+ H[0] = M0.get_X() + k * R[0];
+ H[1] = M0.get_Y() + k * R[1];
+ H[2] = M0.get_Z() + k * R[2];
+ } else {
+ scal = getA() * M1.get_X() + getB() * M1.get_Y() + getC() * M1.get_Z();
+ // if (scal != 0)
if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -getD()/scal;
+ k = -getD() / scal;
else
- k=0;
- H[0] = k*M1.get_X();
- H[1] = k*M1.get_Y();
- H[2] = k*M1.get_Z();
+ k = 0;
+ H[0] = k * M1.get_X();
+ H[1] = k * M1.get_Y();
+ H[2] = k * M1.get_Z();
}
return k;
-
}
-double vpPlane::getIntersection(const vpColVector &M1,vpColVector &H )const
+double vpPlane::getIntersection(const vpColVector &M1, vpColVector &H) const
{
- double k,scal;
+ double k, scal;
- scal = A*M1[0] + B*M1[1] + C*M1[2];
- //if (scal != 0)
+ scal = A * M1[0] + B * M1[1] + C * M1[2];
+ // if (scal != 0)
if (std::fabs(scal) > std::numeric_limits<double>::epsilon())
- k = -getD()/scal;
+ k = -getD() / scal;
else
- k=0;
- H[0] = k*M1[0];
- H[1] = k*M1[1];
- H[2] = k*M1[2];
+ k = 0;
+ H[0] = k * M1[0];
+ H[1] = k * M1[1];
+ H[2] = k * M1[2];
return k;
-
}
/*!
@@ -372,11 +354,11 @@ double vpPlane::getIntersection(const vpColVector &M1,vpColVector &H )const
void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo)
{
// Save current plane parameters
- double Ao = A, Bo = B, Co = C, Do =D ;
- A = cMo[0][0]*Ao + cMo[0][1]*Bo + cMo[0][2]*Co;
- B = cMo[1][0]*Ao + cMo[1][1]*Bo + cMo[1][2]*Co;
- C = cMo[2][0]*Ao + cMo[2][1]*Bo + cMo[2][2]*Co;
- D = Do - (cMo[0][3]*A + cMo[1][3]*B + cMo[2][3]*C);
+ double Ao = A, Bo = B, Co = C, Do = D;
+ A = cMo[0][0] * Ao + cMo[0][1] * Bo + cMo[0][2] * Co;
+ B = cMo[1][0] * Ao + cMo[1][1] * Bo + cMo[1][2] * Co;
+ C = cMo[2][0] * Ao + cMo[2][1] * Bo + cMo[2][2] * Co;
+ D = Do - (cMo[0][3] * A + cMo[1][3] * B + cMo[2][3] * C);
}
/*!
@@ -385,8 +367,7 @@ void vpPlane::changeFrame(const vpHomogeneousMatrix &cMo)
A,B,C and D correspond to the parameters of the plane.
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpPlane& p)
+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 12204f7..354d03e 100644
--- a/modules/core/src/tools/geometry/vpPolygon.cpp
+++ b/modules/core/src/tools/geometry/vpPolygon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,18 +38,16 @@
*
*****************************************************************************/
-
-
-#include <visp3/core/vpPolygon.h>
-#include <visp3/core/vpException.h>
+#include <limits>
+#include <set>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPolygon.h>
#include <visp3/core/vpUniRand.h>
-#include <set>
-#include <limits>
/*!
Basic constructor.
-
+
By default, it defines a triangle with the three 2D points coordinates :
\f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
*/
@@ -56,23 +55,24 @@ vpPolygon::vpPolygon()
: _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
{
std::vector<vpImagePoint> corners;
- corners.push_back(vpImagePoint(0,0));
- corners.push_back(vpImagePoint(1,0));
- corners.push_back(vpImagePoint(0,1));
+ corners.push_back(vpImagePoint(0, 0));
+ corners.push_back(vpImagePoint(1, 0));
+ corners.push_back(vpImagePoint(0, 1));
init(corners);
}
/*!
Constructor which initialises the polygon thanks to the given corners.
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
+
\param corners : The Points defining the corners.
*/
-vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
+vpPolygon::vpPolygon(const std::vector<vpImagePoint> &corners)
: _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
{
- if(corners.size() < 3){
+ if (corners.size() < 3) {
_goodPoly = false;
}
init(corners);
@@ -81,14 +81,15 @@ vpPolygon::vpPolygon(const std::vector<vpImagePoint>& corners)
/*!
Constructor which initialises the polygon thanks to the given corners.
- \warning the corners must be ordered (either clockwise or counter clockwise).
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
\param corners : The Points defining the corners.
*/
-vpPolygon::vpPolygon(const std::list<vpImagePoint>& corners)
+vpPolygon::vpPolygon(const std::list<vpImagePoint> &corners)
: _corners(), _center(), _area(0.), _goodPoly(true), _bbox(), m_PnPolyConstants(), m_PnPolyMultiples()
{
- if(corners.size() < 3){
+ if (corners.size() < 3) {
_goodPoly = false;
}
init(corners);
@@ -96,35 +97,26 @@ vpPolygon::vpPolygon(const std::list<vpImagePoint>& corners)
/*!
Copy constructor
-
+
\param poly : The polygon used for the initialisation.
*/
vpPolygon::vpPolygon(const vpPolygon &poly)
- : _corners(), _center(), _area(0.), _goodPoly(true), _bbox()
+ : _corners(poly._corners), _center(poly._center), _area(poly._area), _goodPoly(poly._goodPoly), _bbox(poly._bbox),
+ 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;
}
/*!
Basic destructor
*/
-vpPolygon::~vpPolygon()
-{
-}
+vpPolygon::~vpPolygon() {}
/*!
Equal operator.
Assign \e poly to this polygon and return a reference to it.
-*/
-vpPolygon &
-vpPolygon::operator=(const vpPolygon& poly)
+*/
+vpPolygon &vpPolygon::operator=(const vpPolygon &poly)
{
_corners = poly._corners;
_center = poly._center;
@@ -139,45 +131,39 @@ vpPolygon::operator=(const vpPolygon& poly)
/*!
Initialises the triangle thanks to the collection of 2D points (in pixel).
- \warning the corners must be ordered (either clockwise or counter clockwise).
-
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
+
\param corners : The corners of the polyon.
*/
-void
-vpPolygon::buildFrom(const std::vector<vpImagePoint>& corners)
-{
- init(corners);
-}
+void vpPolygon::buildFrom(const std::vector<vpImagePoint> &corners) { init(corners); }
/*!
Initialises the polygon thanks to the collection of 2D points (in pixel).
- \warning the corners must be ordered (either clockwise or counter clockwise).
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
\param corners : The corners of the polyon.
*/
-void
-vpPolygon::buildFrom(const std::list<vpImagePoint>& corners)
-{
- init(corners);
-}
+void vpPolygon::buildFrom(const std::list<vpImagePoint> &corners) { init(corners); }
/*!
- Initialises the triangle thanks to the collection of 2D points (in meter). The
- fields \e x and \e y are used to compute the corresponding coordinates in
- pixel thanks to the camera parameters \e cam.
+ Initialises the triangle thanks to the collection of 2D points (in meter).
+ The fields \e x and \e y are used to compute the corresponding coordinates
+ in pixel thanks to the camera parameters \e cam.
- \warning the corners must be ordered (either clockwise or counter clockwise).
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
\param corners : The corners of the polyon.
- \param cam : The camera parameters used to convert the coordinates from meter
- to pixel.
+ \param cam : The camera parameters used to convert the coordinates from
+ meter to pixel.
*/
-void
-vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParameters& cam)
+void vpPolygon::buildFrom(const std::vector<vpPoint> &corners, const vpCameraParameters &cam)
{
std::vector<vpImagePoint> ipCorners(corners.size());
- for(unsigned int i=0; i<corners.size(); ++i){
+ for (unsigned int i = 0; i < corners.size(); ++i) {
vpMeterPixelConversion::convertPoint(cam, corners[i].get_x(), corners[i].get_y(), ipCorners[i]);
}
buildFrom(ipCorners);
@@ -188,19 +174,22 @@ vpPolygon::buildFrom(const std::vector<vpPoint>& corners, const vpCameraParamete
A right click is used to stop the addition of new corners.
\param I : The image where to click to initialise the corners.
+ \param size : Cross size in terms of number of pixels that is displayed over
+ 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<unsigned char>& I)
+void vpPolygon::initClick(const vpImage<unsigned char> &I, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
vpImagePoint ip;
std::vector<vpImagePoint> cornersClick;
- while(button == vpMouseButton::button1){
+ while (button == vpMouseButton::button1) {
bool ret = vpDisplay::getClick(I, ip, button, true);
- if(ret && button == vpMouseButton::button1){
- vpDisplay::displayCross(I, ip, 5, vpColor::red);
+ if (ret && button == vpMouseButton::button1) {
+ vpDisplay::displayCross(I, ip, size, color, thickness);
cornersClick.push_back(ip);
vpDisplay::flush(I);
}
@@ -209,17 +198,45 @@ vpPolygon::initClick(const vpImage<unsigned char>& I)
buildFrom(cornersClick);
}
+/*!
+ Initialises the polygon by (left-)clicking to add a corners to the polygon.
+ A right click is used to stop the addition of new corners.
+
+ \param I : The image where to click to initialise the corners.
+ \param size : Size of the cross in terms of number of pixels that is
+ displayed over 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<vpRGBa> &I, unsigned int size, const vpColor &color, unsigned int thickness)
+{
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
+ vpImagePoint ip;
+
+ std::vector<vpImagePoint> cornersClick;
+
+ while (button == vpMouseButton::button1) {
+ bool ret = vpDisplay::getClick(I, ip, button, true);
+ if (ret && button == vpMouseButton::button1) {
+ vpDisplay::displayCross(I, ip, size, color, thickness);
+ cornersClick.push_back(ip);
+ vpDisplay::flush(I);
+ }
+ }
+
+ buildFrom(cornersClick);
+}
/*!
Intialises the polygon using the collection of image points. This method
computes some internal variables such as center, area, ...
- \warning the corners must be ordered (either clockwise or counter clockwise).
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
\param corners : The corners of the polygon.
*/
-void
-vpPolygon::init(const std::vector<vpImagePoint>& corners)
+void vpPolygon::init(const std::vector<vpImagePoint> &corners)
{
_corners = corners;
@@ -230,17 +247,16 @@ vpPolygon::init(const std::vector<vpImagePoint>& corners)
precalcValuesPnPoly();
}
-
/*!
Intialises the polygon using the collection of image points. This method
computes some internal variables such as center, area, ...
- \warning the corners must be ordered (either clockwise or counter clockwise).
+ \warning the corners must be ordered (either clockwise or counter
+ clockwise).
\param corners : The corners of the polygon.
*/
-void
-vpPolygon::init(const std::list<vpImagePoint>& corners)
+void vpPolygon::init(const std::list<vpImagePoint> &corners)
{
_corners.insert(_corners.begin(), corners.begin(), corners.end());
@@ -251,136 +267,134 @@ vpPolygon::init(const std::list<vpImagePoint>& corners)
precalcValuesPnPoly();
}
-
-
/*!
Test if two segments are intersecting.
-
- \throw vpException::divideByZeroError if the two lines are aligned (denominator equal to zero).
-
+
+ \throw vpException::divideByZeroError if the two lines are aligned
+ (denominator equal to zero).
+
\param ip1 : The first image point of the first segment.
\param ip2 : The second image point of the first segment.
\param ip3 : The first image point of the second segment.
\param ip4 : The second image point of the second segment.
*/
-bool
-vpPolygon::testIntersectionSegments(const vpImagePoint& ip1, const vpImagePoint& ip2, const vpImagePoint& ip3, const vpImagePoint& ip4) const
+bool vpPolygon::testIntersectionSegments(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpImagePoint &ip3,
+ const vpImagePoint &ip4) const
{
double di1 = ip2.get_i() - ip1.get_i();
double dj1 = ip2.get_j() - ip1.get_j();
-
+
double di2 = ip4.get_i() - ip3.get_i();
double dj2 = ip4.get_j() - ip3.get_j();
-
+
double denominator = di1 * dj2 - dj1 * di2;
-
- if(fabs(denominator) < std::numeric_limits<double>::epsilon()){
+
+ if (fabs(denominator) < std::numeric_limits<double>::epsilon()) {
throw vpException(vpException::divideByZeroError, "Denominator is null, lines are parallels");
}
-
- double alpha = - ( ( ip1.get_i() - ip3.get_i() ) * dj2 + di2 * ( ip3.get_j() - ip1.get_j())) / denominator;
- if(alpha < 0 || alpha >= 1){
+
+ double alpha = -((ip1.get_i() - ip3.get_i()) * dj2 + di2 * (ip3.get_j() - ip1.get_j())) / denominator;
+ if (alpha < 0 || alpha >= 1) {
return false;
}
-
- double beta = - (di1 * (ip3.get_j() - ip1.get_j() ) + dj1 * (ip1.get_i() - ip3.get_i()) ) / denominator;
- if(beta < 0 || beta >= 1){
+
+ double beta = -(di1 * (ip3.get_j() - ip1.get_j()) + dj1 * (ip1.get_i() - ip3.get_i())) / denominator;
+ if (beta < 0 || beta >= 1) {
return false;
}
-
+
return true;
}
/*!
Check if the 2D point \f$ ip \f$ is inside the polygon.
-
+
\param ip : The point which have to be tested.
\param method : Method to use for Point In Polygon test.
-
+
\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) {
- std::cerr << "It is not a polygon (_corners.size() < 3)!" << std::endl;
return false;
}
bool test = false;
switch (method) {
- case PnPolySegmentIntersection:
- {
- 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).
-
- bool oddNbIntersections = false;
- for(unsigned int i=0; i<_corners.size(); ++i){
- vpImagePoint ip1 = _corners[i];
- vpImagePoint ip2 = _corners[(i+1)%_corners.size()];
- bool intersection = false;
-
- // If the points are the same we continue without trying to found
- // an intersection
- if (ip1 == ip2)
- continue;
-
- try{
- intersection = testIntersectionSegments(ip1, ip2, ip, infPoint );
- } catch(...){
- return isInside(ip);
- }
-
- if(intersection){
- oddNbIntersections = !oddNbIntersections;
- }
- }
-
- test = oddNbIntersections;
+ case PnPolySegmentIntersection: {
+ 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).
+
+ bool oddNbIntersections = false;
+ for (unsigned int i = 0; i < _corners.size(); ++i) {
+ vpImagePoint ip1 = _corners[i];
+ vpImagePoint ip2 = _corners[(i + 1) % _corners.size()];
+ bool intersection = false;
+
+ // If the points are the same we continue without trying to found
+ // an intersection
+ if (ip1 == ip2)
+ continue;
+
+ try {
+ intersection = testIntersectionSegments(ip1, ip2, ip, infPoint);
+ } catch (...) {
+ return isInside(ip);
}
- break;
-
- //Reference: http://alienryderflex.com/polygon/
- case PnPolyRayCasting:
- 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());
- }
-
- j=i;
- }
-
- test = oddNodes;
+
+ if (intersection) {
+ oddNbIntersections = !oddNbIntersections;
+ }
+ }
+
+ test = oddNbIntersections;
+ } break;
+
+ // Reference: http://alienryderflex.com/polygon/
+ case PnPolyRayCasting:
+ 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());
}
- break;
+
+ j = i;
+ }
+
+ test = oddNodes;
+ } break;
}
return test;
}
-void
-vpPolygon::precalcValuesPnPoly() {
+void vpPolygon::precalcValuesPnPoly()
+{
if (_corners.size() < 3) {
- std::cerr << "It is not a polygon (_corners.size() < 3)!" << std::endl;
return;
}
m_PnPolyConstants.resize(_corners.size());
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<double>::epsilon())) {
+ 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<double>::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());
+ 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());
}
j = i;
@@ -396,44 +410,40 @@ vpPolygon::precalcValuesPnPoly() {
\f]
*/
-void
-vpPolygon::updateArea()
+void vpPolygon::updateArea()
{
- if(_corners.size() == 0){
+ if (_corners.size() == 0) {
_area = 0;
_goodPoly = false;
return;
}
_area = 0;
- for(unsigned int i=0; i<_corners.size(); ++i){
- unsigned int i_p_1 = ( i+1 ) % _corners.size();
- _area += _corners[i].get_j() * _corners[i_p_1].get_i()
- - _corners[i_p_1].get_j() * _corners[i].get_i();
+ for (unsigned int i = 0; i < _corners.size(); ++i) {
+ unsigned int i_p_1 = (i + 1) % _corners.size();
+ _area += _corners[i].get_j() * _corners[i_p_1].get_i() - _corners[i_p_1].get_j() * _corners[i].get_i();
}
_area /= 2;
- if(_area < 0){
- _area = - _area;
+ if (_area < 0) {
+ _area = -_area;
}
}
-
/*!
Update the \c _center attribute of the polygon using the corners.
The i coordinate is computed using:
\f[
- i = \frac{1}{6{area}} \sum_{i=0}^{n-1} (i_i + i_{i+1})(i_{i+1} j_i - j_{i+1} i_i)
- \f]
+ i = \frac{1}{6{area}} \sum_{i=0}^{n-1} (i_i + i_{i+1})(i_{i+1} j_i - j_{i+1}
+ i_i) \f]
The computation of the j coordinate is similar.
*/
-void
-vpPolygon::updateCenter()
+void vpPolygon::updateCenter()
{
- if(_corners.size() == 0){
+ if (_corners.size() == 0) {
_center = vpImagePoint(0, 0);
_goodPoly = false;
return;
@@ -449,22 +459,20 @@ vpPolygon::updateCenter()
(_corners[i+1].get_i() * _corners[i].get_j() - _corners[i+1].get_j() * _corners[i].get_i());
}
#else
- for(unsigned int i=0; i<_corners.size(); ++i){
- unsigned int i_p_1 = ( i+1 ) % _corners.size();
+ 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
- if(_area > 0){
+ if (_area > 0) {
_center.set_i(fabs(i_tmp / (6 * _area)));
_center.set_j(fabs(j_tmp / (6 * _area)));
- }else{
+ } else {
_center = _corners[0];
_goodPoly = false;
}
@@ -475,10 +483,9 @@ vpPolygon::updateCenter()
\sa getBoundingBox()
*/
-void
-vpPolygon::updateBoundingBox()
+void vpPolygon::updateBoundingBox()
{
- if(_corners.size() == 0){
+ if (_corners.size() == 0) {
_bbox.setBottomRight(vpImagePoint(0, 0));
_bbox.setTopLeft(vpImagePoint(0, 0));
_goodPoly = false;
@@ -487,7 +494,7 @@ vpPolygon::updateBoundingBox()
std::set<double> setI;
std::set<double> setJ;
- for(unsigned int i=0; i<_corners.size(); ++i){
+ for (unsigned int i = 0; i < _corners.size(); ++i) {
setI.insert(_corners[i].get_i());
setJ.insert(_corners[i].get_j());
}
@@ -498,7 +505,7 @@ vpPolygon::updateBoundingBox()
--iterJ;
vpImagePoint br(*iterI, *iterJ);
- if(tl == br){
+ if (tl == br) {
_goodPoly = false;
}
_bbox.setTopLeft(tl);
@@ -513,34 +520,12 @@ 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<unsigned char>& I, const vpColor& color, unsigned int thickness) const
+void vpPolygon::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness) const
{
const unsigned int N = (unsigned int)_corners.size();
- for(unsigned int i=0; i<N; ++i){
- vpDisplay::displayLine(I, _corners[i], _corners[(i+1)%N], color, thickness);
- }
-}
-
-bool
-vpPolygon::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const double &i_test, const double &j_test, const double &i, const double &j)
-{
- double dx = p2.get_j() - p1.get_j();
- double dy = p2.get_i() - p1.get_i();
- double ex = j - j_test;
- double ey = i - i_test;
-
- double den = dx * ey - dy * ex;
- double t = 0, u = 0;
- //if(den != 0){
- if(std::fabs(den) > std::fabs(den)*std::numeric_limits<double>::epsilon()){
- t = -( ey * ( p1.get_j() - j_test ) + ex * ( -p1.get_i() + i_test ) ) / den;
- u = -( dx * ( -p1.get_i() + i_test ) + dy * ( p1.get_j() - j_test ) ) / den;
- }
- else{
- throw vpException(vpException::divideByZeroError, "denominator null");
+ for (unsigned int i = 0; i < N; ++i) {
+ vpDisplay::displayLine(I, _corners[i], _corners[(i + 1) % N], color, thickness);
}
- return ( t >= std::numeric_limits<double>::epsilon() && t < 1.0 && u >= std::numeric_limits<double>::epsilon() && u < 1.0);
}
/*!
@@ -551,10 +536,11 @@ vpPolygon::intersect(const vpImagePoint& p1, const vpImagePoint& p2, const doubl
\param j : j-coordinate of the image point to test.
\param method : Method to use for Point In Polygon test.
- \return True if the point defined by (i,j) is inside the polygon, False otherwise.
+ \return True if the point defined by (i,j) is inside the polygon, False
+ otherwise.
*/
-bool
-vpPolygon::isInside(const std::vector<vpImagePoint>& roi, const double &i, const double &j, const PointInPolygonMethod &method)
+bool vpPolygon::isInside(const std::vector<vpImagePoint> &roi, const double &i, const double &j,
+ const PointInPolygonMethod &method)
{
vpPolygon poly(roi);
return poly.isInside(vpImagePoint(i, j), method);
@@ -563,8 +549,4 @@ vpPolygon::isInside(const std::vector<vpImagePoint>& roi, const double &i, const
/*!
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 bbf5143..52876d4 100644
--- a/modules/core/src/tools/geometry/vpPolygon3D.cpp
+++ b/modules/core/src/tools/geometry/vpPolygon3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,31 +44,30 @@
\brief Implements a polygon of the model used by the model-based tracker.
*/
-#include <visp3/core/vpPolygon3D.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpPolygon3D.h>
/*!
Basic constructor.
*/
vpPolygon3D::vpPolygon3D()
- : nbpt(0), nbCornersInsidePrev(0),
- p(NULL), polyClipped(), clippingFlag(vpPolygon3D::NO_CLIPPING),
+ : 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)
+vpPolygon3D::vpPolygon3D(const vpPolygon3D &mbtp)
+ : nbpt(mbtp.nbpt), nbCornersInsidePrev(mbtp.nbCornersInsidePrev), p(NULL), polyClipped(mbtp.polyClipped),
+ clippingFlag(mbtp.clippingFlag), distNearClip(mbtp.distNearClip), distFarClip(mbtp.distFarClip)
{
- if (p) delete [] p;
- p = new vpPoint [nbpt];
- for(unsigned int i = 0; i < nbpt; i++)
+ if (p)
+ delete[] p;
+ p = new vpPoint[nbpt];
+ for (unsigned int i = 0; i < nbpt; i++)
p[i] = mbtp.p[i];
}
-vpPolygon3D& vpPolygon3D::operator=(const vpPolygon3D& mbtp)
+vpPolygon3D &vpPolygon3D::operator=(const vpPolygon3D &mbtp)
{
nbpt = mbtp.nbpt;
nbCornersInsidePrev = mbtp.nbCornersInsidePrev;
@@ -76,9 +76,10 @@ vpPolygon3D& vpPolygon3D::operator=(const vpPolygon3D& mbtp)
distNearClip = mbtp.distNearClip;
distFarClip = mbtp.distFarClip;
- if (p) delete [] p;
- p = new vpPoint [nbpt];
- for(unsigned int i = 0; i < nbpt; i++)
+ if (p)
+ delete[] p;
+ p = new vpPoint[nbpt];
+ for (unsigned int i = 0; i < nbpt; i++)
p[i] = mbtp.p[i];
return (*this);
@@ -89,8 +90,7 @@ vpPolygon3D& vpPolygon3D::operator=(const vpPolygon3D& mbtp)
*/
vpPolygon3D::~vpPolygon3D()
{
- if (p !=NULL)
- {
+ if (p != NULL) {
delete[] p;
p = NULL;
}
@@ -103,10 +103,9 @@ vpPolygon3D::~vpPolygon3D()
\param _index : the index of the corner
*/
-vpPoint &
-vpPolygon3D::getPoint(const unsigned int _index)
+vpPoint &vpPolygon3D::getPoint(const unsigned int _index)
{
- if(_index >= nbpt){
+ if (_index >= nbpt) {
throw vpException(vpException::dimensionError, "index out of range");
}
return p[_index];
@@ -114,43 +113,40 @@ vpPolygon3D::getPoint(const unsigned int _index)
/*!
Set the number of points which are the corners of the polygon.
-
+
\param nb : The number of corners.
*/
-void
-vpPolygon3D::setNbPoint(const unsigned int nb)
+void vpPolygon3D::setNbPoint(const unsigned int nb)
{
- nbpt = nb ;
+ nbpt = nb;
if (p != NULL)
delete[] p;
- p = new vpPoint[nb] ;
+ p = new vpPoint[nb];
}
/*!
Add a corner point to the list of polygon's corners.
-
+
\param n : The index of the corner.
\param P : The point to add.
*/
-void
-vpPolygon3D::addPoint(const unsigned int n, const vpPoint &P)
+void vpPolygon3D::addPoint(const unsigned int n, const vpPoint &P)
{
- //if( p!NULL && n < nbpt )
- p[n] = P ;
+ // if( p!NULL && n < nbpt )
+ p[n] = P;
}
/*!
- Project the 3D corner points into the image thanks to the pose of the camera.
-
+ Project the 3D corner points into the image thanks to the pose of the
+ camera.
+
\param cMo : The pose of the camera.
*/
-void
-vpPolygon3D::changeFrame(const vpHomogeneousMatrix &cMo)
+void vpPolygon3D::changeFrame(const vpHomogeneousMatrix &cMo)
{
- for (unsigned int i = 0 ; i < nbpt ; i++)
- {
- p[i].changeFrame(cMo) ;
- p[i].projection() ;
+ for (unsigned int i = 0; i < nbpt; i++) {
+ p[i].changeFrame(cMo);
+ p[i].projection();
}
}
@@ -158,91 +154,90 @@ vpPolygon3D::changeFrame(const vpHomogeneousMatrix &cMo)
Compute the region of interest in the image according to the used clipping.
\warning If the FOV clipping is used, camera normals have to be precomputed.
-
+
\param cam : camera parameters used to compute the field of view.
*/
-void
-vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam)
+void vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam)
{
polyClipped.clear();
std::vector<vpColVector> fovNormals;
- std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp;
- std::vector<std::pair<vpPoint,unsigned int> > polyClippedTemp2;
+ std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp;
+ std::vector<std::pair<vpPoint, unsigned int> > polyClippedTemp2;
- if(cam.isFovComputed() && clippingFlag > 3)
+ if (cam.isFovComputed() && clippingFlag > 3)
fovNormals = cam.getFovNormals();
- for(unsigned int i = 0 ; i < nbpt ; i++){
- p[i%nbpt].projection();
- polyClippedTemp.push_back(std::make_pair(p[i%nbpt],vpPolygon3D::NO_CLIPPING));
+ for (unsigned int i = 0; i < nbpt; i++) {
+ p[i % nbpt].projection();
+ polyClippedTemp.push_back(std::make_pair(p[i % nbpt], vpPolygon3D::NO_CLIPPING));
}
- if(clippingFlag != vpPolygon3D::NO_CLIPPING) {
- for(unsigned int i = 1 ; i < 64 ; i=i*2)
- {
- if(((clippingFlag & i) == i) || ((clippingFlag > vpPolygon3D::FAR_CLIPPING) && (i==1)))
- {
- if(i > vpPolygon3D::FAR_CLIPPING && !cam.isFovComputed()) // To make sure we do not compute FOV clipping if camera normals are not computed
+ 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
continue;
- for(unsigned int j = 0 ; j < polyClippedTemp.size() ; j++)
- {
- vpPoint p1Clipped = polyClippedTemp[j].first;
- vpPoint p2Clipped = polyClippedTemp[(j+1)%polyClippedTemp.size()].first;
-
- unsigned int p2ClippedInfoBefore = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
- unsigned int p1ClippedInfo = polyClippedTemp[j].second;
- unsigned int p2ClippedInfo = polyClippedTemp[(j+1)%polyClippedTemp.size()].second;
-
- bool problem = true;
-
- switch(i){
- case 1:
- problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- i, distNearClip));
- break;
- case 2:
- problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- i, distFarClip));
- break;
- case 4:
- problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
- break;
- case 8:
- problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
- break;
- case 16:
- problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[2], vpPolygon3D::UP_CLIPPING));
- break;
- case 32:
- problem = !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo, p2ClippedInfo,
- fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
- break;
+ for (unsigned int j = 0; j < polyClippedTemp.size(); j++) {
+ vpPoint p1Clipped = polyClippedTemp[j].first;
+ vpPoint p2Clipped = polyClippedTemp[(j + 1) % polyClippedTemp.size()].first;
+
+ unsigned int p2ClippedInfoBefore = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
+ unsigned int p1ClippedInfo = polyClippedTemp[j].second;
+ unsigned int p2ClippedInfo = polyClippedTemp[(j + 1) % polyClippedTemp.size()].second;
+
+ bool problem = true;
+
+ switch (i) {
+ case 1:
+ problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, i, distNearClip));
+ break;
+ case 2:
+ problem = !(vpPolygon3D::getClippedPointsDistance(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, i, distFarClip));
+ break;
+ case 4:
+ problem =
+ !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, fovNormals[0], vpPolygon3D::LEFT_CLIPPING));
+ break;
+ case 8:
+ problem =
+ !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, fovNormals[1], vpPolygon3D::RIGHT_CLIPPING));
+ break;
+ case 16:
+ problem =
+ !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, fovNormals[2], vpPolygon3D::UP_CLIPPING));
+ break;
+ case 32:
+ problem =
+ !(vpPolygon3D::getClippedPointsFovGeneric(p1Clipped, p2Clipped, p1Clipped, p2Clipped, p1ClippedInfo,
+ p2ClippedInfo, fovNormals[3], vpPolygon3D::DOWN_CLIPPING));
+ break;
+ }
+
+ if (!problem) {
+ p1Clipped.projection();
+ polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
+
+ if (p2ClippedInfo != p2ClippedInfoBefore) {
+ p2Clipped.projection();
+ polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
}
- if(!problem)
- {
- p1Clipped.projection();
- polyClippedTemp2.push_back(std::make_pair(p1Clipped, p1ClippedInfo));
-
- if(p2ClippedInfo != p2ClippedInfoBefore)
- {
+ if (nbpt == 2) {
+ if (p2ClippedInfo == p2ClippedInfoBefore) {
p2Clipped.projection();
polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
}
-
- if(nbpt == 2){
- if(p2ClippedInfo == p2ClippedInfoBefore)
- {
- p2Clipped.projection();
- polyClippedTemp2.push_back(std::make_pair(p2Clipped, p2ClippedInfo));
- }
- break;
- }
+ break;
}
+ }
}
polyClippedTemp = polyClippedTemp2;
@@ -253,7 +248,7 @@ vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam)
polyClipped = polyClippedTemp;
}
-
+
/*!
Get the clipped points according to a plane equation.
@@ -269,148 +264,145 @@ vpPolygon3D::computePolygonClipped(const vpCameraParameters &cam)
\param C : Param C from plane equation.
\param D : Param D from plane equation.
\param flag : flag specifying the clipping used when calling this function.
-
+
\return True if the points have been clipped, False otherwise
*/
-bool
-vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const vpColVector &normal, const unsigned int &flag)
-{
+bool vpPolygon3D::getClippedPointsFovGeneric(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped,
+ vpPoint &p2Clipped, unsigned int &p1ClippedInfo,
+ unsigned int &p2ClippedInfo, const vpColVector &normal,
+ const unsigned int &flag)
+{
vpRowVector p1Vec(3);
- p1Vec[0] = p1.get_X(); p1Vec[1] = p1.get_Y(); p1Vec[2] = p1.get_Z();
+ p1Vec[0] = p1.get_X();
+ p1Vec[1] = p1.get_Y();
+ p1Vec[2] = p1.get_Z();
p1Vec.normalize();
-
+
vpRowVector p2Vec(3);
- p2Vec[0] = p2.get_X(); p2Vec[1] = p2.get_Y(); p2Vec[2] = p2.get_Z();
+ p2Vec[0] = p2.get_X();
+ p2Vec[1] = p2.get_Y();
+ p2Vec[2] = p2.get_Z();
p2Vec.normalize();
-
- if((clippingFlag & flag) == flag){
- double beta1 = acos( p1Vec * normal );
- double beta2 = acos( p2Vec * normal );
-// std::cout << beta1 << " && " << beta2 << std::endl;
+ if ((clippingFlag & flag) == flag) {
+ double beta1 = acos(p1Vec * normal);
+ double beta2 = acos(p2Vec * normal);
+
+ // std::cout << beta1 << " && " << beta2 << std::endl;
// if(!(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0))
- if(beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
+ if (beta1 < M_PI / 2.0 && beta2 < M_PI / 2.0)
return false;
- else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0){
+ else if (beta1 < M_PI / 2.0 || beta2 < M_PI / 2.0) {
vpPoint pClipped;
double t = -(normal[0] * p1.get_X() + normal[1] * p1.get_Y() + normal[2] * p1.get_Z());
- t = t / ( normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) + normal[2] * (p2.get_Z() - p1.get_Z()) );
-
- pClipped.set_X((p2.get_X() - p1.get_X())*t + p1.get_X());
- pClipped.set_Y((p2.get_Y() - p1.get_Y())*t + p1.get_Y());
- pClipped.set_Z((p2.get_Z() - p1.get_Z())*t + p1.get_Z());
-
- if(beta1 < M_PI / 2.0){
+ t = t / (normal[0] * (p2.get_X() - p1.get_X()) + normal[1] * (p2.get_Y() - p1.get_Y()) +
+ normal[2] * (p2.get_Z() - p1.get_Z()));
+
+ pClipped.set_X((p2.get_X() - p1.get_X()) * t + p1.get_X());
+ pClipped.set_Y((p2.get_Y() - p1.get_Y()) * t + p1.get_Y());
+ pClipped.set_Z((p2.get_Z() - p1.get_Z()) * t + p1.get_Z());
+
+ if (beta1 < M_PI / 2.0) {
p1ClippedInfo = p1ClippedInfo | flag;
p1Clipped = pClipped;
- }
- else{
+ } else {
p2ClippedInfo = p2ClippedInfo | flag;
p2Clipped = pClipped;
}
}
}
-
+
return true;
}
-bool
-vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2,
- vpPoint &p1Clipped, vpPoint &p2Clipped,
- unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
- const unsigned int &flag, const double &distance)
+bool vpPolygon3D::getClippedPointsDistance(const vpPoint &p1, const vpPoint &p2, vpPoint &p1Clipped, vpPoint &p2Clipped,
+ unsigned int &p1ClippedInfo, unsigned int &p2ClippedInfo,
+ const unsigned int &flag, const double &distance)
{
- // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
- // to avoid a valgrind "Source and destination overlap in memcpy" error,
- // we introduce a two temporary points.
- vpPoint p1Clipped_, p2Clipped_;
- p1Clipped_ = p1;
- p2Clipped_ = p2;
+ // Since p1 and p1Clipped can be the same object as well as p2 and p2Clipped
+ // to avoid a valgrind "Source and destination overlap in memcpy" error,
+ // we introduce a two temporary points.
+ vpPoint p1Clipped_, p2Clipped_;
+ p1Clipped_ = p1;
+ p2Clipped_ = p2;
- p1Clipped = p1Clipped_;
- p2Clipped = p2Clipped_;
+ p1Clipped = p1Clipped_;
+ p2Clipped = p2Clipped_;
+ bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
+ if (flag == vpPolygon3D::FAR_CLIPPING)
+ test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
- bool test1 = (p1Clipped.get_Z() < distance && p2Clipped.get_Z() < distance);
- if(flag == vpPolygon3D::FAR_CLIPPING)
- test1 = (p1Clipped.get_Z() > distance && p2Clipped.get_Z() > distance);
+ bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
+ if (flag == vpPolygon3D::FAR_CLIPPING)
+ test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
- bool test2 = (p1Clipped.get_Z() < distance || p2Clipped.get_Z() < distance);
- if(flag == vpPolygon3D::FAR_CLIPPING)
- test2 = (p1Clipped.get_Z() > distance || p2Clipped.get_Z() > distance);
+ bool test3 = (p1Clipped.get_Z() < distance);
+ if (flag == vpPolygon3D::FAR_CLIPPING)
+ test3 = (p1Clipped.get_Z() > distance);
- bool test3 = (p1Clipped.get_Z() < distance);
- if(flag == vpPolygon3D::FAR_CLIPPING)
- test3 = (p1Clipped.get_Z() > distance);
-
- if(test1)
- return false;
+ if (test1)
+ return false;
- else if(test2){
- vpPoint pClippedNear;
- double t;
- t = (p2Clipped.get_Z() - p1Clipped.get_Z());
- t = (distance - p1Clipped.get_Z()) / t;
-
- pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X())*t + p1Clipped.get_X());
- pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y())*t + p1Clipped.get_Y());
- pClippedNear.set_Z(distance);
-
- if(test3){
- p1Clipped = pClippedNear;
- if(flag == vpPolygon3D::FAR_CLIPPING)
- p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
- else
- p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
- }
- else{
- p2Clipped = pClippedNear;
- if(flag == vpPolygon3D::FAR_CLIPPING)
- p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
- else
- p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
- }
+ else if (test2) {
+ vpPoint pClippedNear;
+ double t;
+ t = (p2Clipped.get_Z() - p1Clipped.get_Z());
+ t = (distance - p1Clipped.get_Z()) / t;
+
+ pClippedNear.set_X((p2Clipped.get_X() - p1Clipped.get_X()) * t + p1Clipped.get_X());
+ pClippedNear.set_Y((p2Clipped.get_Y() - p1Clipped.get_Y()) * t + p1Clipped.get_Y());
+ pClippedNear.set_Z(distance);
+
+ if (test3) {
+ p1Clipped = pClippedNear;
+ if (flag == vpPolygon3D::FAR_CLIPPING)
+ p1ClippedInfo = p1ClippedInfo | vpPolygon3D::FAR_CLIPPING;
+ else
+ p1ClippedInfo = p1ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
+ } else {
+ p2Clipped = pClippedNear;
+ if (flag == vpPolygon3D::FAR_CLIPPING)
+ p2ClippedInfo = p2ClippedInfo | vpPolygon3D::FAR_CLIPPING;
+ else
+ p2ClippedInfo = p2ClippedInfo | vpPolygon3D::NEAR_CLIPPING;
}
+ }
- return true;
+ return true;
}
/*!
Get the region of interest in the image.
-
+
\warning Suppose that changeFrame() has already been called.
-
+
\param cam : camera parameters.
\return Image point corresponding to the region of interest.
*/
-std::vector<vpImagePoint>
-vpPolygon3D::getRoi(const vpCameraParameters &cam)
-{
+std::vector<vpImagePoint> vpPolygon3D::getRoi(const vpCameraParameters &cam)
+{
std::vector<vpImagePoint> roi;
- for (unsigned int i = 0; i < nbpt; i ++){
+ for (unsigned int i = 0; i < nbpt; i++) {
vpImagePoint ip;
vpMeterPixelConversion::convertPoint(cam, p[i].get_x(), p[i].get_y(), ip);
roi.push_back(ip);
}
-
+
return roi;
}
/*!
Get the region of interest in the image.
-
+
\param cam : camera parameters.
\param cMo : pose.
\return Image point corresponding to the region of interest.
*/
-std::vector<vpImagePoint>
-vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
+std::vector<vpImagePoint> vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo)
{
changeFrame(cMo);
return getRoi(cam);
@@ -420,14 +412,14 @@ vpPolygon3D::getRoi(const vpCameraParameters &cam, const vpHomogeneousMatrix &cM
/*!
Get the 3D points of the clipped region of interest.
- \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+ \warning Suppose that changeFrame() and computePolygonClipped() have already
+ been called.
\param points : resulting points.
*/
-void
-vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
+void vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
{
- for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ for (unsigned int i = 0; i < polyClipped.size(); i++) {
points.push_back(polyClipped[i].first);
}
}
@@ -436,122 +428,121 @@ vpPolygon3D::getRoiClipped(std::vector<vpPoint> &points)
/*!
Get the 3D clipped points and their clipping information.
- \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+ \warning Suppose that changeFrame() and computePolygonClipped() have already
+ been called.
\param poly : resulting points plus clipping information.
*/
-void
-vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint,unsigned int> > &poly)
-{
- poly = polyClipped;
-}
+void vpPolygon3D::getPolygonClipped(std::vector<std::pair<vpPoint, unsigned int> > &poly) { poly = polyClipped; }
/*!
Get the 3D clipped points.
- \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
+ \warning Suppose that changeFrame() and computePolygonClipped() have already
+ been called.
\param poly : resulting points.
*/
-void
-vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
+void vpPolygon3D::getPolygonClipped(std::vector<vpPoint> &poly)
{
- for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ for (unsigned int i = 0; i < polyClipped.size(); i++) {
poly.push_back(polyClipped[i].first);
}
}
/*!
Get the region of interest clipped in the image.
-
- \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
-
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already
+ been called.
+
\param cam : camera parameters.
\param roi : image point corresponding to the region of interest.
*/
-void
-vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
+void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi)
{
- for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ for (unsigned int i = 0; i < polyClipped.size(); i++) {
vpImagePoint ip;
- vpMeterPixelConversion::convertPoint(cam,polyClipped[i].first.get_x(),polyClipped[i].first.get_y(),ip);
-// std::cout << "## " << ip.get_j() << " - " << ip.get_i() << std::endl;
+ vpMeterPixelConversion::convertPoint(cam, polyClipped[i].first.get_x(), polyClipped[i].first.get_y(), ip);
+ // std::cout << "## " << ip.get_j() << " - " << ip.get_i() <<
+ // std::endl;
roi.push_back(ip);
}
}
/*!
Get the region of interest clipped in the image.
-
+
\param cam : camera parameters.
\param cMo : pose.
\param roi : image point corresponding to the region of interest.
*/
-void
-vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi, const vpHomogeneousMatrix &cMo)
+void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<vpImagePoint> &roi,
+ const vpHomogeneousMatrix &cMo)
{
changeFrame(cMo);
computePolygonClipped(cam);
getRoiClipped(cam, roi);
}
-
+
/*!
- Get the region of interest clipped in the image and the information to know if it's a clipped point.
-
- \warning Suppose that changeFrame() and computePolygonClipped() have already been called.
-
+ Get the region of interest clipped in the image and the information to know
+ if it's a clipped point.
+
+ \warning Suppose that changeFrame() and computePolygonClipped() have already
+ been called.
+
\param cam : camera parameters.
- \param roi : image point corresponding to the region of interest with clipping information.
+ \param roi : image point corresponding to the region of interest with
+ clipping information.
*/
-void
-vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi)
+void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi)
{
- for(unsigned int i = 0 ; i < polyClipped.size() ; i++){
+ 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));
}
}
/*!
- Get the region of interest clipped in the image and the information to know if it's a clipped point.
-
+ Get the region of interest clipped in the image and the information to know
+ if it's a clipped point.
+
\param cam : camera parameters.
- \param roi : image point corresponding to the region of interest with clipping information.
- \param cMo : pose.
+ \param roi : image point corresponding to the region of interest with
+ clipping information. \param cMo : pose.
*/
-void
-vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint,unsigned int> > &roi, const vpHomogeneousMatrix &cMo)
+void vpPolygon3D::getRoiClipped(const vpCameraParameters &cam, std::vector<std::pair<vpImagePoint, unsigned int> > &roi,
+ const vpHomogeneousMatrix &cMo)
{
changeFrame(cMo);
computePolygonClipped(cam);
getRoiClipped(cam, roi);
}
-
-
/*!
- Static method to check the number of points of a region defined by the vector of image point that are inside the image.
+ Static method to check the number of points of a region defined by the
+ vector of image point that are inside the image.
\param I : The image used for its size.
\param cam : The camera parameters.
*/
-unsigned int
-vpPolygon3D::getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCameraParameters &cam)
+unsigned int vpPolygon3D::getNbCornerInsideImage(const vpImage<unsigned char> &I, const vpCameraParameters &cam)
{
unsigned int nbPolyIn = 0;
- for (unsigned int i = 0; i < nbpt; i ++){
- if(p[i].get_Z() > 0){
+ 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()))
+ if ((ip.get_i() >= 0) && (ip.get_j() >= 0) && (ip.get_i() < I.getHeight()) && (ip.get_j() < I.getWidth()))
nbPolyIn++;
}
}
-
+
nbCornersInsidePrev = nbPolyIn;
-
+
return nbPolyIn;
}
@@ -562,93 +553,95 @@ vpPolygon3D::getNbCornerInsideImage(const vpImage<unsigned char>& I, const vpCam
/*!
Static method to compute the clipped points from a set of initial points.
- \warning When using FOV clipping and personnal camera parameters, camera normals have to be computed before (see vpCameraParameters::computeFov())
+ \warning When using FOV clipping and personnal camera parameters, camera
+ normals have to be computed before (see vpCameraParameters::computeFov())
\param ptIn : Input points
\param ptOut : Output points (result of the clipping).
\param cMo : Pose considered for the clipping.
- \param clippingFlags: Clipping flag (see vpPolygon3D::vpPolygon3DClippingType).
- \param cam : Camera parameters (Only used if clipping flags contain FOV clipping).
- \param znear : Near clipping distance value (Only used if clipping flags contain Near clipping).
- \param zfar : Far clipping distance value (Only used if clipping flags contain Far clipping).
+ \param clippingFlags: Clipping flag (see
+ vpPolygon3D::vpPolygon3DClippingType). \param cam : Camera parameters (Only
+ used if clipping flags contain FOV clipping). \param znear : Near clipping
+ distance value (Only used if clipping flags contain Near clipping). \param
+ zfar : Far clipping distance value (Only used if clipping flags contain Far
+ clipping).
*/
-void
-vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut, const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
- const vpCameraParameters &cam, const double &znear, const double &zfar)
+void vpPolygon3D::getClippedPolygon(const std::vector<vpPoint> &ptIn, std::vector<vpPoint> &ptOut,
+ const vpHomogeneousMatrix &cMo, const unsigned int &clippingFlags,
+ const vpCameraParameters &cam, const double &znear, const double &zfar)
{
- ptOut.clear();
- vpPolygon3D poly;
- poly.setNbPoint((unsigned int)ptIn.size());
- poly.setClipping(clippingFlags);
+ ptOut.clear();
+ vpPolygon3D poly;
+ poly.setNbPoint((unsigned int)ptIn.size());
+ poly.setClipping(clippingFlags);
- if((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
- poly.setNearClippingDistance(znear);
+ if ((clippingFlags & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ poly.setNearClippingDistance(znear);
- if((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
- poly.setFarClippingDistance(zfar);
+ if ((clippingFlags & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ poly.setFarClippingDistance(zfar);
- for(unsigned int i = 0; i < ptIn.size(); i++)
- poly.addPoint(i,ptIn[i]);
+ for (unsigned int i = 0; i < ptIn.size(); i++)
+ poly.addPoint(i, ptIn[i]);
- poly.changeFrame(cMo);
- poly.computePolygonClipped(cam);
- poly.getPolygonClipped(ptOut);
+ poly.changeFrame(cMo);
+ poly.computePolygonClipped(cam);
+ poly.getPolygonClipped(ptOut);
}
-void
-vpPolygon3D::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int & i_min, int &i_max, int &j_min, int &j_max)
+void vpPolygon3D::getMinMaxRoi(const std::vector<vpImagePoint> &iroi, int &i_min, int &i_max, int &j_min, int &j_max)
{
- // i_min_d = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
- double i_min_d = (double) INT_MAX;
+ // i_min_d = std::numeric_limits<double>::max(); // create an error under
+ // Windows. To fix it we have to add #undef max
+ double i_min_d = (double)INT_MAX;
double i_max_d = 0;
- double j_min_d = (double) INT_MAX;
+ double j_min_d = (double)INT_MAX;
double j_max_d = 0;
- for (unsigned int i = 0; i < iroi.size(); i += 1){
- if(i_min_d > iroi[i].get_i())
+ for (unsigned int i = 0; i < iroi.size(); i += 1) {
+ if (i_min_d > iroi[i].get_i())
i_min_d = iroi[i].get_i();
-
- if(iroi[i].get_i() < 0)
+
+ if (iroi[i].get_i() < 0)
i_min_d = 1;
-
- if((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
+
+ if ((iroi[i].get_i() > 0) && (i_max_d < iroi[i].get_i()))
i_max_d = iroi[i].get_i();
-
- if(j_min_d > iroi[i].get_j())
+
+ if (j_min_d > iroi[i].get_j())
j_min_d = iroi[i].get_j();
-
- if(iroi[i].get_j() < 0)
- j_min_d = 1;//border
-
- if((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
+
+ if (iroi[i].get_j() < 0)
+ j_min_d = 1; // border
+
+ if ((iroi[i].get_j() > 0) && j_max_d < iroi[i].get_j())
j_max_d = iroi[i].get_j();
}
- i_min = static_cast<int> (i_min_d);
- i_max = static_cast<int> (i_max_d);
- j_min = static_cast<int> (j_min_d);
- j_max = static_cast<int> (j_max_d);
+ i_min = static_cast<int>(i_min_d);
+ i_max = static_cast<int>(i_max_d);
+ j_min = static_cast<int>(j_min_d);
+ j_max = static_cast<int>(j_max_d);
}
/*!
- Static method to check whether the region defined by the vector of image point
- is contained entirely in the image.
+ Static method to check whether the region defined by the vector of image
+ point is contained entirely in the image.
\param I : The image used for its size.
\param corners : The vector of points defining a region
*/
-bool
-vpPolygon3D::roiInsideImage(const vpImage<unsigned char>& I, const std::vector<vpImagePoint>& corners)
+bool vpPolygon3D::roiInsideImage(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &corners)
{
double nbPolyIn = 0;
- for(unsigned int i=0; i<corners.size(); ++i){
- if((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) &&
- (corners[i].get_i() < I.getHeight()) && (corners[i].get_j() < I.getWidth())){
+ for (unsigned int i = 0; i < corners.size(); ++i) {
+ if ((corners[i].get_i() >= 0) && (corners[i].get_j() >= 0) && (corners[i].get_i() < I.getHeight()) &&
+ (corners[i].get_j() < I.getWidth())) {
nbPolyIn++;
}
}
-
- if(nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
+
+ if (nbPolyIn < 3 && nbPolyIn < 0.7 * corners.size())
return false;
-
+
return true;
}
diff --git a/modules/core/src/tools/geometry/vpRect.cpp b/modules/core/src/tools/geometry/vpRect.cpp
index 48045e7..60f40b1 100644
--- a/modules/core/src/tools/geometry/vpRect.cpp
+++ b/modules/core/src/tools/geometry/vpRect.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,9 +42,8 @@
\ingroup libtools
*/
-
-#include <visp3/core/vpRect.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRect.h>
/*!
Constructs a default rectangle with the \e top, \e left corner set to (0,0)
@@ -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
@@ -88,29 +85,25 @@ vpRect::vpRect(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
{
this->left = topLeft.get_u();
this->top = topLeft.get_v();
-
- setBottom( bottomRight.get_v() );
- setRight( bottomRight.get_u() );
+
+ setBottom(bottomRight.get_v());
+ setRight(bottomRight.get_u());
}
/*!
Constructs a rectangle that is a copy of \e r.
*/
-vpRect::vpRect(const vpRect& r)
- : left(0), top(0), width(0), height(0)
-{
- *this = r;
-}
+vpRect::vpRect(const vpRect &r) : left(0), top(0), width(0), height(0) { *this = r; }
/*!
Assign \e r to this rectangle and return a reference to this rectangle.
-*/
-vpRect &vpRect::operator=(const vpRect& r)
+*/
+vpRect &vpRect::operator=(const vpRect &r)
{
- this->left = r.left;
- this->top = r.top;
- this->width = r.width;
+ this->left = r.left;
+ this->top = r.top;
+ this->width = r.width;
this->height = r.height;
return *this;
}
@@ -120,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<vpImagePoint> &ip)
- : left(0), top(0), width(0), height(0)
-{
- set(ip);
-}
+vpRect::vpRect(const std::vector<vpImagePoint> &ip) : left(0), top(0), width(0), height(0) { set(ip); }
/*!
Check if an image point belongs to the rectangle.
@@ -133,8 +122,10 @@ vpRect::vpRect(const std::vector<vpImagePoint> &ip)
\return Returns true if the point belongs to the rectangle.
*/
-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());
+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());
}
/*!
@@ -177,25 +168,28 @@ void vpRect::set(const vpImagePoint &topLeft, double w, double h)
void vpRect::set(const std::vector<vpImagePoint> &ip)
{
if (ip.size() < 1)
- throw (vpException(vpException::dimensionError,
- "At least 1 point is requested to build a rectangle"));
+ throw(vpException(vpException::dimensionError, "At least 1 point is requested to build a rectangle"));
double minu, maxu;
double minv, maxv;
minu = maxu = ip[0].get_u();
minv = maxv = ip[0].get_v();
- for(size_t i=1; i<ip.size(); i++) {
- double u = ip[i].get_u();
- double v = ip[i].get_v();
- if ( u < minu ) minu = u;
- else if (u > maxu) maxu = u;
- if ( v < minv ) minv = v;
- else if (v > maxv) maxv = v;
+ for (size_t i = 1; i < ip.size(); i++) {
+ double u = ip[i].get_u();
+ double v = ip[i].get_v();
+ if (u < minu)
+ minu = u;
+ else if (u > maxu)
+ maxu = u;
+ if (v < minv)
+ minv = v;
+ else if (v > maxv)
+ maxv = v;
}
- setLeft (minu);
- setTop (minv);
- setRight (maxu);
+ setLeft(minu);
+ setTop(minv);
+ setRight(maxu);
setBottom(maxv);
}
@@ -211,17 +205,14 @@ void vpRect::set(const vpImagePoint &topLeft, const vpImagePoint &bottomRight)
this->left = topLeft.get_u();
this->top = topLeft.get_v();
- setBottom( bottomRight.get_v() );
- setRight( bottomRight.get_u() );
+ setBottom(bottomRight.get_v());
+ setRight(bottomRight.get_u());
}
/*!
Set the rectangle from \e r.
*/
-void vpRect::set(const vpRect& r)
-{
- *this = r;
-}
+void vpRect::set(const vpRect &r) { *this = r; }
/*!
Operator to compare two rectangles.
@@ -229,11 +220,12 @@ void vpRect::set(const vpRect& r)
*/
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<double>::epsilon()
- && std::fabs(left-r.left) <= std::fabs(left)*std::numeric_limits<double>::epsilon()
- && std::fabs(width-r.width) <= std::fabs(width)*std::numeric_limits<double>::epsilon()
- && std::fabs(height-r.height) <= std::fabs(height)*std::numeric_limits<double>::epsilon());
+ // 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<double>::epsilon() &&
+ std::fabs(left - r.left) <= std::fabs(left) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(width - r.width) <= std::fabs(width) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(height - r.height) <= std::fabs(height) * std::numeric_limits<double>::epsilon());
}
/*!
@@ -242,23 +234,30 @@ bool vpRect::operator==(const vpRect &r) const
*/
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<double>::epsilon()
-// || std::fabs(left-r.left) > std::fabs(left)*std::numeric_limits<double>::epsilon()
-// || std::fabs(width-r.width) > std::fabs(width)*std::numeric_limits<double>::epsilon()
-// || std::fabs(height-r.height) > std::fabs(height)*std::numeric_limits<double>::epsilon());
+ // 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<double>::epsilon()
+ // || std::fabs(left-r.left) >
+ // std::fabs(left)*std::numeric_limits<double>::epsilon()
+ // || std::fabs(width-r.width) >
+ // std::fabs(width)*std::numeric_limits<double>::epsilon()
+ // || std::fabs(height-r.height) >
+ // std::fabs(height)*std::numeric_limits<double>::epsilon());
return !(*this == r);
}
/*!
Intersection operator.
- \return Intersection rectangle or null rectangle if the two rectangles do not intersect.
+ \return Intersection rectangle or null rectangle if the two rectangles do not
+ intersect.
*/
-vpRect& vpRect::operator&=(const vpRect &r) {
- double x1 = std::max(left, r.left);
- double y1 = std::max(top, r.top);
- width = std::min(left + width, r.left + r.width) - x1;
- height = std::min(top + height, r.top + r.height) - y1;
+vpRect &vpRect::operator&=(const vpRect &r)
+{
+ double x1 = (std::max)(left, r.left);
+ double y1 = (std::max)(top, r.top);
+ width = (std::min)(left + width, r.left + r.width) - x1;
+ height = (std::min)(top + height, r.top + r.height) - y1;
left = x1;
top = y1;
@@ -271,9 +270,11 @@ vpRect& vpRect::operator&=(const vpRect &r) {
/*!
Intersection operator.
- \return Intersection rectangle or null rectangle if the two rectangles do not intersect.
+ \return Intersection rectangle or null rectangle if the two rectangles do not
+ intersect.
*/
-vpRect vpRect::operator&(const vpRect &r) const {
+vpRect vpRect::operator&(const vpRect &r) const
+{
vpRect a = *this;
return a &= r;
}
@@ -286,12 +287,14 @@ vpRect vpRect::operator&(const vpRect &r) const {
\return Returns true if the point belongs to the rectangle.
*/
-VISP_EXPORT bool inRectangle( const vpImagePoint &ip, const vpRect &rect ) {
- return ( ip.get_i() <= rect.getBottom() && ip.get_i() >= rect.getTop() && ip.get_j() <= rect.getRight() && ip.get_j() >= rect.getLeft());
+VISP_EXPORT bool inRectangle(const vpImagePoint &ip, const vpRect &rect)
+{
+ return (ip.get_i() <= rect.getBottom() && ip.get_i() >= rect.getTop() && ip.get_j() <= rect.getRight() &&
+ ip.get_j() >= rect.getLeft());
}
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpRect& r)
- {
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpRect &r)
+{
os << r.getLeft() << ", " << r.getTop() << ", " << r.getWidth() << ", " << r.getHeight();
return os;
}
diff --git a/modules/core/src/tools/geometry/vpTriangle.cpp b/modules/core/src/tools/geometry/vpTriangle.cpp
index 67389c8..107aa34 100644
--- a/modules/core/src/tools/geometry/vpTriangle.cpp
+++ b/modules/core/src/tools/geometry/vpTriangle.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,43 +37,45 @@
*
*****************************************************************************/
-#include <visp3/core/vpTriangle.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTriangle.h>
/*!
Basic constructor.
-
- By default, the three 2D points coordinates which define the triangle are \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
+
+ By default, the three 2D points coordinates which define the triangle are
+ \f$ (0,0) \f$, \f$ (1,0) \f$ and \f$ (0,1) \f$.
*/
vpTriangle::vpTriangle()
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+ : 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));
+ init(vpImagePoint(0, 0), vpImagePoint(1, 0), vpImagePoint(0, 1));
}
/*!
- Constructor which initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
-
+ Constructor which initialise the triangle thanks to the three 2D points \f$
+ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
+
\param iP1 : The first apex of the triangle.
\param iP2 : The first apex of the triangle.
\param iP3 : The first apex of the triangle.
*/
vpTriangle::vpTriangle(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+ : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(),
+ apex2(), apex3()
{
- init(iP1,iP2,iP3);
+ init(iP1, iP2, iP3);
}
/*!
Copy constructor
-
+
\param tri : The triangle used for the initialisation.
*/
vpTriangle::vpTriangle(const vpTriangle &tri)
- : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0),
- ptempo0(0), ptempo1(0), area(0), apex1(), apex2(), apex3()
+ : goodTriange(true), S1(), uvinv00(0), uvinv01(0), uvinv10(0), uvinv11(0), ptempo0(0), ptempo1(0), area(0), apex1(),
+ apex2(), apex3()
{
*this = tri;
}
@@ -80,15 +83,12 @@ vpTriangle::vpTriangle(const vpTriangle &tri)
/*!
Basic destructor
*/
-vpTriangle::~vpTriangle()
-{
-}
+vpTriangle::~vpTriangle() {}
/*!
Assign \e tri to this triangle and return a reference to this triangle.
-*/
-vpTriangle &
-vpTriangle::operator=(const vpTriangle& tri)
+*/
+vpTriangle &vpTriangle::operator=(const vpTriangle &tri)
{
goodTriange = tri.goodTriange;
S1 = tri.S1;
@@ -99,81 +99,79 @@ vpTriangle::operator=(const vpTriangle& tri)
ptempo0 = tri.ptempo0;
ptempo1 = tri.ptempo1;
area = tri.area;
+ apex1 = tri.apex1;
+ apex2 = tri.apex2;
+ apex3 = tri.apex3;
return *this;
};
/*!
- Initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2 \f$ and \f$ iP3 \f$
-
+ Initialise the triangle thanks to the three 2D points \f$ iP1 \f$, \f$ iP2
+ \f$ and \f$ iP3 \f$
+
\param iP1 : The first apex of the triangle.
\param iP2 : The first apex of the triangle.
\param iP3 : The first apex of the triangle.
*/
-void
-vpTriangle::buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
+void vpTriangle::buildFrom(const vpImagePoint &iP1, const vpImagePoint &iP2, const vpImagePoint &iP3)
{
- init(iP1,iP2,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;
apex2 = iP2;
apex3 = iP3;
-
- vpMatrix uv(2,2);
- vpMatrix uvinv(2,2);
+
+ vpMatrix uv(2, 2);
+ vpMatrix uvinv(2, 2);
uv[0][0] = iP2.get_i() - iP1.get_i();
uv[1][0] = iP3.get_i() - iP1.get_i();
uv[0][1] = iP2.get_j() - iP1.get_j();
uv[1][1] = iP3.get_j() - iP1.get_j();
- try
- {
- uvinv=uv.inverseByLU();
+ try {
+ uvinv = uv.inverseByLU();
goodTriange = true;
- }
- catch(...)
- {
+ } catch (...) {
goodTriange = false;
- std::cout<<"Empty triangle"<<std::endl;
+ std::cout << "Empty triangle" << std::endl;
}
-
- uvinv00=uvinv[0][0];
- uvinv01=uvinv[0][1];
- uvinv10=uvinv[1][0];
- uvinv11=uvinv[1][1];
+
+ uvinv00 = uvinv[0][0];
+ uvinv01 = uvinv[0][1];
+ uvinv10 = uvinv[1][0];
+ uvinv11 = uvinv[1][1];
S1 = iP1;
area = 0.5 * fabs(uv.det());
}
-
/*!
Check if the 2D point \f$ iP \f$ is inside the triangle.
-
+
\param iP : The point which coulb be inside the triangle.
- \param threshold : A threshold used to define the accuracy of the computation when the point is very near from the edges of the triangle. 0 is the smallest value.
-
- \return Returns true if the point is inside the triangle. Returns false otherwise.
+ \param threshold : A threshold used to define the accuracy of the
+ computation when the point is very near from the edges of the triangle. 0 is
+ the smallest value.
+
+ \return Returns true if the point is inside the triangle. Returns false
+ otherwise.
*/
-bool
-vpTriangle::inTriangle(const vpImagePoint &iP, double threshold)
+bool vpTriangle::inTriangle(const vpImagePoint &iP, double threshold)
{
- if(!goodTriange)
+ if (!goodTriange)
return false;
-
- if(threshold < 0)
+
+ if (threshold < 0)
threshold = 0;
-
+
ptempo0 = iP.get_i() - S1.get_i();
ptempo1 = iP.get_j() - S1.get_j();
-
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
-
- return (p_ds_uv0+p_ds_uv1<1.+threshold && p_ds_uv0>-threshold && p_ds_uv1>-threshold);
-}
+ double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
+ double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
+
+ return (p_ds_uv0 + p_ds_uv1 < 1. + threshold && p_ds_uv0 > -threshold && p_ds_uv1 > -threshold);
+}
diff --git a/modules/core/src/tools/histogram/vpHistogram.cpp b/modules/core/src/tools/histogram/vpHistogram.cpp
index c4d5cf4..ff8402a 100644
--- a/modules/core/src/tools/histogram/vpHistogram.cpp
+++ b/modules/core/src/tools/histogram/vpHistogram.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,116 +46,114 @@
*/
#include <stdlib.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpHistogram.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpDisplay.h>
-
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
#include <visp3/core/vpThread.h>
-namespace {
- struct Histogram_Param_t {
- unsigned int m_start_index;
- unsigned int m_end_index;
+namespace
+{
+struct Histogram_Param_t {
+ unsigned int m_start_index;
+ unsigned int m_end_index;
- unsigned int m_lut[256];
- unsigned int *m_histogram;
- const vpImage<unsigned char> *m_I;
+ unsigned int m_lut[256];
+ unsigned int *m_histogram;
+ const vpImage<unsigned char> *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<unsigned char> * 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<unsigned char> *const I)
+ : m_start_index(start_index), m_end_index(end_index), m_lut(), m_histogram(NULL), m_I(I)
+ {
+ }
- ~Histogram_Param_t() {
- if(m_histogram != NULL) {
- delete []m_histogram;
- }
+ ~Histogram_Param_t()
+ {
+ if (m_histogram != NULL) {
+ delete[] m_histogram;
}
- };
-
- vpThread::Return computeHistogramThread(vpThread::Args args) {
- Histogram_Param_t *histogram_param = static_cast<Histogram_Param_t *>(args);
- unsigned int start_index = histogram_param->m_start_index;
- unsigned int end_index = histogram_param->m_end_index;
-
- const vpImage<unsigned char> *I = histogram_param->m_I;
-
- unsigned char *ptrStart = (unsigned char*) (I->bitmap) + start_index;
- unsigned char *ptrEnd = (unsigned char*) (I->bitmap) + end_index;
- unsigned char *ptrCurrent = ptrStart;
-
-
-// while(ptrCurrent != ptrEnd) {
-// histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
-// ++ptrCurrent;
-// }
-
- if(end_index - start_index >= 8) {
- //Unroll loop version
- for(; ptrCurrent <= ptrEnd - 8;) {
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ }
+};
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+vpThread::Return computeHistogramThread(vpThread::Args args)
+{
+ Histogram_Param_t *histogram_param = static_cast<Histogram_Param_t *>(args);
+ unsigned int start_index = histogram_param->m_start_index;
+ unsigned int end_index = histogram_param->m_end_index;
+
+ const vpImage<unsigned char> *I = histogram_param->m_I;
+
+ unsigned char *ptrStart = (unsigned char *)(I->bitmap) + start_index;
+ unsigned char *ptrEnd = (unsigned char *)(I->bitmap) + end_index;
+ unsigned char *ptrCurrent = ptrStart;
+
+ // while(ptrCurrent != ptrEnd) {
+ // histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ]
+ // ] ++;
+ // ++ptrCurrent;
+ // }
+
+ if (end_index - start_index >= 8) {
+ // Unroll loop version
+ for (; ptrCurrent <= ptrEnd - 8;) {
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
- ++ptrCurrent;
- }
- }
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
- for(; ptrCurrent != ptrEnd; ++ptrCurrent) {
- histogram_param->m_histogram[ histogram_param->m_lut[ *ptrCurrent ] ] ++;
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
+ ++ptrCurrent;
}
+ }
- return 0;
+ for (; ptrCurrent != ptrEnd; ++ptrCurrent) {
+ histogram_param->m_histogram[histogram_param->m_lut[*ptrCurrent]]++;
}
+
+ return 0;
+}
}
#endif
-bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second);
+bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second);
// comparison,
-bool compare_vpHistogramPeak (vpHistogramPeak first, vpHistogramPeak second)
+bool compare_vpHistogramPeak(vpHistogramPeak first, vpHistogramPeak second)
{
- if (first.getValue() > second.getValue()) return true;
- else return false;
+ if (first.getValue() > second.getValue())
+ return true;
+ else
+ return false;
}
-
-
/*!
Defaut constructor for a gray level histogram.
*/
-vpHistogram::vpHistogram() : histogram(NULL), size(256)
-{
- init();
-}
+vpHistogram::vpHistogram() : histogram(NULL), size(256) { init(); }
/*!
Copy constructor of a gray level histogram.
*/
-vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256)
+vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256)
{
init(h.size);
memcpy(histogram, h.histogram, size * sizeof(unsigned));
@@ -167,8 +166,7 @@ vpHistogram::vpHistogram(const vpHistogram &h) : histogram(NULL), size(256)
\sa calculate()
*/
-vpHistogram::vpHistogram(const vpImage<unsigned char> &I)
- : histogram(NULL), size(256)
+vpHistogram::vpHistogram(const vpImage<unsigned char> &I) : histogram(NULL), size(256)
{
init();
@@ -182,7 +180,7 @@ vpHistogram::~vpHistogram()
{
if (histogram != NULL) {
// vpTRACE("free: %p", &histogram);
- delete [] histogram;
+ delete[] histogram;
histogram = NULL;
size = 0;
}
@@ -201,8 +199,7 @@ vpHistogram::~vpHistogram()
\endcode
*/
-vpHistogram &
-vpHistogram::operator=(const vpHistogram &h)
+vpHistogram &vpHistogram::operator=(const vpHistogram &h)
{
init(h.size);
memcpy(histogram, h.histogram, size * sizeof(unsigned));
@@ -215,23 +212,21 @@ vpHistogram::operator=(const vpHistogram &h)
- Allocates the array
- Initialise all the values to zero.
*/
-void
-vpHistogram::init(unsigned size_)
+void vpHistogram::init(unsigned size_)
{
if (histogram != NULL) {
- delete [] histogram;
+ delete[] histogram;
histogram = NULL;
}
this->size = size_;
- histogram = new unsigned [size];
+ histogram = new unsigned[size];
memset(histogram, 0, size * sizeof(unsigned));
// vpTRACE("alloc: %p", &histogram);
}
-
/*!
Calculate the histogram from a gray level image.
@@ -242,14 +237,14 @@ vpHistogram::init(unsigned size_)
*/
void vpHistogram::calculate(const vpImage<unsigned char> &I, const unsigned int nbins, const unsigned int nbThreads)
{
- if(size != nbins) {
+ if (size != nbins) {
if (histogram != NULL) {
- delete [] histogram;
+ delete[] histogram;
histogram = NULL;
}
size = nbins > 256 ? 256 : (nbins > 0 ? nbins : 256);
- if(nbins > 256 || nbins == 0) {
+ if (nbins > 256 || nbins == 0) {
std::cerr << "nbins=" << nbins << " , nbins should be between ]0 ; 256] ; use by default nbins=256" << std::endl;
}
histogram = new unsigned int[size];
@@ -257,7 +252,6 @@ void vpHistogram::calculate(const vpImage<unsigned char> &I, const unsigned int
memset(histogram, 0, size * sizeof(unsigned int));
-
bool use_single_thread;
#if !defined(VISP_HAVE_PTHREAD) && !defined(_WIN32)
use_single_thread = true;
@@ -265,80 +259,79 @@ void vpHistogram::calculate(const vpImage<unsigned char> &I, const unsigned int
use_single_thread = (nbThreads == 0 || nbThreads == 1);
#endif
- if(!use_single_thread && I.getSize() <= nbThreads) {
+ if (!use_single_thread && I.getSize() <= nbThreads) {
use_single_thread = true;
}
-
unsigned int lut[256];
- for(unsigned int i = 0; i < 256; i++) {
- lut[i] = (unsigned int) (i * size / 256.0);
+ for (unsigned int i = 0; i < 256; i++) {
+ lut[i] = (unsigned int)(i * size / 256.0);
}
- if(use_single_thread) {
- //Single thread
+ if (use_single_thread) {
+ // Single thread
- unsigned int size_ = I.getWidth()*I.getHeight();
- unsigned char *ptrStart = (unsigned char*) I.bitmap;
+ unsigned int size_ = I.getWidth() * I.getHeight();
+ unsigned char *ptrStart = (unsigned char *)I.bitmap;
unsigned char *ptrEnd = ptrStart + size_;
unsigned char *ptrCurrent = ptrStart;
- while(ptrCurrent != ptrEnd) {
- histogram[ lut[ *ptrCurrent ] ] ++;
+ while (ptrCurrent != ptrEnd) {
+ histogram[lut[*ptrCurrent]]++;
++ptrCurrent;
}
} else {
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
- //Multi-threads
+ // Multi-threads
std::vector<vpThread *> threadpool;
std::vector<Histogram_Param_t *> histogramParams;
unsigned int image_size = I.getSize();
unsigned int step = image_size / nbThreads;
- unsigned int last_step = image_size - step * (nbThreads-1);
+ unsigned int last_step = image_size - step * (nbThreads - 1);
- for(unsigned int index = 0; index < nbThreads; index++) {
- unsigned int start_index = index*step;
- unsigned int end_index = (index+1)*step;
+ for (unsigned int index = 0; index < nbThreads; index++) {
+ unsigned int start_index = index * step;
+ unsigned int end_index = (index + 1) * step;
- if(index == nbThreads-1) {
- end_index = start_index+last_step;
+ if (index == nbThreads - 1) {
+ end_index = start_index + last_step;
}
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));
+ memcpy(histogram_param->m_lut, lut, 256 * sizeof(unsigned int));
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);
}
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
// Wait until thread ends up
threadpool[cpt]->join();
}
- for(unsigned int cpt1 = 0; cpt1 < size; cpt1++) {
+ for (unsigned int cpt1 = 0; cpt1 < size; cpt1++) {
unsigned int sum = 0;
- for(size_t cpt2 = 0; cpt2 < histogramParams.size(); cpt2++) {
+ for (size_t cpt2 = 0; cpt2 < histogramParams.size(); cpt2++) {
sum += histogramParams[cpt2]->m_histogram[cpt1];
}
histogram[cpt1] = sum;
}
- //Delete
- for(size_t cpt = 0; cpt < threadpool.size(); cpt++) {
+ // Delete
+ for (size_t cpt = 0; cpt < threadpool.size(); cpt++) {
delete threadpool[cpt];
}
- for(size_t cpt = 0; cpt < histogramParams.size(); cpt++) {
+ for (size_t cpt = 0; cpt < histogramParams.size(); cpt++) {
delete histogramParams[cpt];
}
#endif
@@ -346,47 +339,49 @@ void vpHistogram::calculate(const vpImage<unsigned char> &I, const unsigned int
}
/*!
- Display the histogram distribution in an image, the minimal image size is 36x36 px.
+ Display the histogram distribution in an image, the minimal image size is
+ 36x36 px.
\param I : Image with the histogram distribution displayed.
\param color : Color used for the display.
\param thickness : Thickness of the line.
- \param maxValue_ : Maximum value in the histogram, if 0 it will be computed from the current histogram.
- Useful to plot a 3 channels histogram for a RGB image for example to keep a coherent vertical scale between the channels.
+ \param maxValue_ : Maximum value in the histogram, if 0 it will be computed
+ from the current histogram. Useful to plot a 3 channels histogram for a RGB
+ image for example to keep a coherent vertical scale between the channels.
*/
void vpHistogram::display(const vpImage<unsigned char> &I, const vpColor &color, const unsigned int thickness,
- const unsigned int maxValue_) {
+ const unsigned int maxValue_)
+{
unsigned int width = I.getWidth(), height = I.getHeight();
- //Minimal width and height are 36 px
- if(width < 36 || height < 36) {
+ // Minimal width and height are 36 px
+ if (width < 36 || height < 36) {
std::cerr << "Image I must have at least width >= 36 && height >= 36 !" << std::endl;
return;
}
unsigned int maxValue = maxValue_;
- if(maxValue == 0) {
- for(unsigned int i = 0; i < size; i++) {
- if(histogram[i] > maxValue) {
+ if (maxValue == 0) {
+ for (unsigned int i = 0; i < size; i++) {
+ if (histogram[i] > maxValue) {
maxValue = histogram[i];
}
}
}
if (maxValue == 0) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot display histogram; max value=0"));
+ throw(vpException(vpException::divideByZeroError, "Cannot display histogram; max value=0"));
}
- //Keep 12 free pixels at the top
- unsigned int max_height = height-12;
- double ratio_height = max_height / (double) maxValue;
- double ratio_width = (width-1) / (double) (size-1.0);
+ // Keep 12 free pixels at the top
+ unsigned int max_height = height - 12;
+ double ratio_height = max_height / (double)maxValue;
+ double ratio_width = (width - 1) / (double)(size - 1.0);
- for(unsigned int i = 1; i < size; i++) {
- unsigned int value1 = histogram[i-1];
+ for (unsigned int i = 1; i < size; i++) {
+ unsigned int value1 = histogram[i - 1];
unsigned int value2 = histogram[i];
- vpImagePoint startPt((height-1)-(value1*ratio_height), (i-1)*ratio_width);
- vpImagePoint endPt((height-1)-(value2*ratio_height), (i*ratio_width));
+ 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);
}
}
@@ -411,13 +406,11 @@ void vpHistogram::display(const vpImage<unsigned char> &I, const vpColor &color,
\sa calculate()
*/
-void
-vpHistogram::smooth(const unsigned int fsize)
+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;
@@ -425,19 +418,18 @@ vpHistogram::smooth(const unsigned int fsize)
int hsize = (int)fsize / 2; // half filter size
- for (unsigned i=0; i < size; i ++) {
+ for (unsigned i = 0; i < size; i++) {
unsigned int sum = 0;
unsigned int nb = 0;
- for (int j=-hsize; j <= hsize; j ++) {
+ for (int j = -hsize; j <= hsize; j++) {
// exploitation of the overflow to detect negative value...
- if ( /*(i + j) >= 0 &&*/ (i + (unsigned int)j) < size ) {
- sum += h.histogram[i + (unsigned int)j];
- nb ++;
+ if (/*(i + j) >= 0 &&*/ (i + (unsigned int)j) < size) {
+ sum += h.histogram[i + (unsigned int)j];
+ nb++;
}
}
histogram[i] = sum / nb;
}
-
}
/*!
@@ -454,17 +446,16 @@ vpHistogram::smooth(const unsigned int fsize)
\sa sort()
*/
-unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & peaks)
+unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> &peaks)
{
if (histogram == NULL) {
vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
+ throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised"));
}
- int prev_slope; // Previous histogram inclination
- vpHistogramPeak p; // An histogram peak
- unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
+ int prev_slope; // Previous histogram inclination
+ vpHistogramPeak p; // An histogram peak
+ unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
peaks.clear();
@@ -474,34 +465,32 @@ unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & peaks)
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
+ for (unsigned i = 0; i < size - 1; i++) {
+ int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
-// if ((prev_slope < 0) && (next_slope > 0) ) {
-// sum_level += i;
-// cpt ++;
-// continue;
-// }
+ // if ((prev_slope < 0) && (next_slope > 0) ) {
+ // sum_level += i;
+ // cpt ++;
+ // continue;
+ // }
- if ((prev_slope > 0) && (next_slope == 0) ) {
+ if ((prev_slope > 0) && (next_slope == 0)) {
sum_level += i + 1;
- cpt ++;
+ cpt++;
continue;
}
-
// Peak detection
- if ( (prev_slope > 0) && (next_slope < 0) ) {
+ if ((prev_slope > 0) && (next_slope < 0)) {
sum_level += i;
- cpt ++;
+ cpt++;
unsigned int level = sum_level / cpt;
p.set((unsigned char)level, histogram[level]);
// vpTRACE("add %d %d", p.getLevel(), p.getValue());
peaks.push_back(p);
- nbpeaks ++;
-
+ nbpeaks++;
}
prev_slope = next_slope;
@@ -509,10 +498,10 @@ unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & peaks)
cpt = 0;
}
if (prev_slope > 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
+ p.set((unsigned char)size - 1u, histogram[size - 1]);
// vpTRACE("add %d %d", p.getLevel(), p.getValue());
peaks.push_back(p);
- nbpeaks ++;
+ nbpeaks++;
}
return nbpeaks;
@@ -535,9 +524,7 @@ unsigned vpHistogram::getPeaks(std::list<vpHistogramPeak> & 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<vpHistogramPeak> peaks;
unsigned nbpeaks; // Number of peaks in the histogram (ie local maxima)
@@ -560,8 +547,7 @@ unsigned vpHistogram::getPeaks(unsigned char dist,
// Parse the peaks list to get the peak with a distance greater
// than dist to the highest
peak1 = peaks.front();
- for(std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++ it)
- {
+ for (std::list<vpHistogramPeak>::const_iterator it = peaks.begin(); it != peaks.end(); ++it) {
vpHistogramPeak p = *it;
if (abs(p.getLevel() - peak1.getLevel()) > dist) {
// The second peak is found
@@ -575,7 +561,6 @@ unsigned vpHistogram::getPeaks(unsigned char dist,
return 1;
}
-
/*!
Determine the two highest peaks in the histogram and compute a
@@ -591,87 +576,84 @@ unsigned vpHistogram::getPeaks(unsigned char dist,
\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
+ unsigned char *peak; // Local maxima values
int prev_slope; // Previous histogram inclination
unsigned index_highest_peak; // Index in peak[] array of the highest peak
unsigned index_second_peak; // Index in peak[] array of the second peak
unsigned int prof;
- unsigned int maxprof; // Nb pixels difference between 2 maxi peaks
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
- unsigned int nbpeaks; // Number of peaks in the histogram (ie local maxima)
+ unsigned int maxprof; // Nb pixels difference between 2 maxi peaks
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
+ unsigned int nbpeaks; // Number of peaks in the histogram (ie local maxima)
// Init the valey
valey.set(0, 0);
// Allocation for the
- peak = new unsigned char [size];
+ peak = new unsigned char[size];
// Parse the histogram to get the local maxima
nbpeaks = 0;
prev_slope = 1;
- for (unsigned i = 0; i < size-1; i++) {
- int next_slope = (int)histogram[i+1] - (int)histogram[i]; // Next histogram inclination
+ for (unsigned i = 0; i < size - 1; i++) {
+ int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
if (next_slope == 0)
continue;
// Peak detection
- if ( (prev_slope > 0) && (next_slope < 0) )
- peak[nbpeaks ++] = (unsigned char)i;
+ if ((prev_slope > 0) && (next_slope < 0))
+ peak[nbpeaks++] = (unsigned char)i;
prev_slope = next_slope;
}
if (prev_slope > 0)
- peak[nbpeaks ++] = (unsigned char)(size-1);
+ peak[nbpeaks++] = (unsigned char)(size - 1);
-// vpTRACE("nb peaks: %d", nbpeaks);
-// for (unsigned i=0; i < nbpeaks; i ++)
-// vpTRACE("peak %d: pos %d value: %d", i, peak[i], histogram[ peak[i] ]);
+ // vpTRACE("nb peaks: %d", nbpeaks);
+ // for (unsigned i=0; i < nbpeaks; i ++)
+ // vpTRACE("peak %d: pos %d value: %d", i, peak[i], histogram[ peak[i]
+ // ]);
// Get the global maximum
index_highest_peak = 0;
- for (unsigned int i=0; i < nbpeaks; i++) {
- if (histogram[ peak[i] ] > histogram[ peak[index_highest_peak] ]) {
+ for (unsigned int i = 0; i < nbpeaks; i++) {
+ if (histogram[peak[i]] > histogram[peak[index_highest_peak]]) {
index_highest_peak = i;
}
}
-// vpTRACE("highest peak index: %d pos: %d value: %d",
-// index_highest_peak, peak[index_highest_peak],
-// histogram[ peak[index_highest_peak] ]);
+ // vpTRACE("highest peak index: %d pos: %d value: %d",
+ // index_highest_peak, peak[index_highest_peak],
+ // histogram[ peak[index_highest_peak] ]);
- maxprof=0;
- index_second_peak=index_highest_peak;
+ maxprof = 0;
+ index_second_peak = index_highest_peak;
// Search second local maximum on the left of the global maximum
for (unsigned i = 0; i < index_highest_peak; i++) {
if (peak[index_highest_peak] - peak[i] > dist) {
- prof=0;
- for (int j=peak[i]; j <= peak[index_highest_peak]-dist; j++)
- if((histogram[peak[i]] - histogram[j]) > prof)
- prof = histogram[peak[i]] - histogram[j];
+ prof = 0;
+ for (int j = peak[i]; j <= peak[index_highest_peak] - dist; j++)
+ if ((histogram[peak[i]] - histogram[j]) > prof)
+ prof = histogram[peak[i]] - histogram[j];
if (prof > maxprof) {
- maxprof = prof;
- index_second_peak = i;
+ maxprof = prof;
+ index_second_peak = i;
}
- }
+ }
}
// Search second local maximum on the right of the global maximum
- for (unsigned i = index_highest_peak+1; i < nbpeaks; i++) {
+ for (unsigned i = index_highest_peak + 1; i < nbpeaks; i++) {
if (peak[i] - peak[index_highest_peak] > dist) {
- prof=0;
- for (int j=peak[index_highest_peak]+dist; j <= peak[i]; j++)
- if((histogram[peak[i]] - histogram[j]) > prof)
- prof = histogram[peak[i]] - histogram[j];
+ prof = 0;
+ for (int j = peak[index_highest_peak] + dist; j <= peak[i]; j++)
+ if ((histogram[peak[i]] - histogram[j]) > prof)
+ prof = histogram[peak[i]] - histogram[j];
if (prof > maxprof) {
maxprof = prof;
@@ -679,43 +661,40 @@ vpHistogram::getPeaks(unsigned char dist,
}
}
}
-// vpTRACE("second peak index: %d pos: %d value: %d",
-// index_second_peak, peak[index_second_peak],
-// histogram[ peak[index_second_peak] ]);
+ // vpTRACE("second peak index: %d pos: %d value: %d",
+ // index_second_peak, peak[index_second_peak],
+ // histogram[ peak[index_second_peak] ]);
// Determine position of the first and second highest peaks
- if (peak[index_highest_peak] < peak[index_second_peak])
- {
- peakr.set(peak[index_second_peak], histogram[ peak[index_second_peak] ]);
- peakl.set(peak[index_highest_peak], histogram[ peak[index_highest_peak] ]);
- }
- else
- {
- peakl.set(peak[index_second_peak], histogram[ peak[index_second_peak] ]);
- peakr.set(peak[index_highest_peak], histogram[ peak[index_highest_peak]]);
+ if (peak[index_highest_peak] < peak[index_second_peak]) {
+ peakr.set(peak[index_second_peak], histogram[peak[index_second_peak]]);
+ peakl.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]);
+ } else {
+ peakl.set(peak[index_second_peak], histogram[peak[index_second_peak]]);
+ peakr.set(peak[index_highest_peak], histogram[peak[index_highest_peak]]);
}
if (peakl == peakr) {
- delete [] peak;
+ delete[] peak;
return (false); // Not a bimodal histogram
}
// Search the valey
mini = peakl.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
+ sumindmini = 0;
+ nbmini = 0;
+ for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini = histogram[i];
+ nbmini = 1;
+ sumindmini = i;
continue;
}
if (histogram[i] == mini) {
sumindmini += i;
- nbmini ++;
+ nbmini++;
}
}
// vpTRACE("nbmini %d", nbmini);
@@ -726,15 +705,14 @@ vpHistogram::getPeaks(unsigned char dist,
// no valey found
valey.set(0, 0);
- delete [] peak;
+ delete[] peak;
return false;
- }
- else {
- mini = sumindmini/nbmini; // mean
+ } else {
+ mini = sumindmini / nbmini; // mean
valey.set((unsigned char)mini, histogram[mini]);
- delete [] peak;
+ delete[] peak;
return (true);
}
@@ -751,17 +729,16 @@ vpHistogram::getPeaks(unsigned char dist,
\sa sort()
*/
-unsigned vpHistogram::getValey(std::list<vpHistogramValey> & valey)
+unsigned vpHistogram::getValey(std::list<vpHistogramValey> &valey)
{
if (histogram == NULL) {
vpERROR_TRACE("Histogram array not initialised\n");
- throw (vpImageException(vpImageException::notInitializedError,
- "Histogram array not initialised")) ;
+ throw(vpImageException(vpImageException::notInitializedError, "Histogram array not initialised"));
}
- int prev_slope; // Previous histogram inclination
- vpHistogramValey p; // An histogram valey
- unsigned nbvaley; // Number of valey in the histogram (ie local minima)
+ int prev_slope; // Previous histogram inclination
+ vpHistogramValey p; // An histogram valey
+ unsigned nbvaley; // Number of valey in the histogram (ie local minima)
valey.clear();
@@ -771,27 +748,26 @@ unsigned vpHistogram::getValey(std::list<vpHistogramValey> & valey)
nbvaley = 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
+ for (unsigned i = 0; i < size - 1; i++) {
+ int next_slope = (int)histogram[i + 1] - (int)histogram[i]; // Next histogram inclination
- if ((prev_slope < 0) && (next_slope == 0) ) {
+ if ((prev_slope < 0) && (next_slope == 0)) {
sum_level += i + 1;
- cpt ++;
+ cpt++;
continue;
}
// Valey detection
- if ( (prev_slope < 0) && (next_slope > 0) ) {
+ if ((prev_slope < 0) && (next_slope > 0)) {
sum_level += i;
- cpt ++;
+ cpt++;
unsigned int level = sum_level / cpt;
p.set((unsigned char)level, histogram[level]);
// vpTRACE("add %d %d", p.getLevel(), p.getValue());
valey.push_back(p);
- nbvaley ++;
-
+ nbvaley++;
}
prev_slope = next_slope;
@@ -799,10 +775,10 @@ unsigned vpHistogram::getValey(std::list<vpHistogramValey> & valey)
cpt = 0;
}
if (prev_slope < 0) {
- p.set((unsigned char)size-1u, histogram[size-1]);
+ p.set((unsigned char)size - 1u, histogram[size - 1]);
// vpTRACE("add %d %d", p.getLevel(), p.getValue());
valey.push_back(p);
- nbvaley ++;
+ nbvaley++;
}
return nbvaley;
@@ -822,10 +798,7 @@ unsigned vpHistogram::getValey(std::list<vpHistogramValey> & 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
@@ -833,30 +806,29 @@ vpHistogram::getValey(const vpHistogramPeak & peak1,
if (peak1.getLevel() > peak2.getLevel()) {
peakl = peak2;
peakr = peak1;
- }
- else {
+ } else {
peakl = peak1;
peakr = peak2;
}
// Search the valey
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
mini = peakl.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i <= peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
+ sumindmini = 0;
+ nbmini = 0;
+ for (unsigned i = peakl.getLevel(); i <= peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini = histogram[i];
+ nbmini = 1;
+ sumindmini = i;
continue;
}
if (histogram[i] == mini) {
sumindmini += i;
- nbmini ++;
+ nbmini++;
}
}
@@ -865,9 +837,8 @@ vpHistogram::getValey(const vpHistogramPeak & peak1,
valey.set(0, 0);
return false;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
+ } else {
+ unsigned int minipos = sumindmini / nbmini; // position of the minimum
valey.set((unsigned char)minipos, histogram[minipos]);
return true;
@@ -891,36 +862,33 @@ 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,
- vpHistogramValey & valeyr)
+unsigned vpHistogram::getValey(unsigned char dist, const vpHistogramPeak &peak, vpHistogramValey &valeyl,
+ vpHistogramValey &valeyr)
{
unsigned int ret = 0x11;
- unsigned int nbmini; // Minimum numbers
- unsigned int sumindmini; // Sum
- unsigned int mini; // current minimum
- vpHistogramPeak peakr, peakl; // Left and right peaks of peak
+ unsigned int nbmini; // Minimum numbers
+ unsigned int sumindmini; // Sum
+ unsigned int mini; // current minimum
+ vpHistogramPeak peakr, peakl; // Left and right peaks of peak
std::list<vpHistogramPeak> peaks; // list of histogram peaks
- // unsigned int nbpeaks=0; // Number of peaks in the histogram (ie local maxima)
+ // unsigned int nbpeaks=0; // Number of peaks in the histogram (ie local
+ // maxima)
if (peak.getLevel() == 0) {
valeyl.set(0, 0);
ret &= 0x01;
}
- if (peak.getLevel() == size -1) {
- valeyr.set((unsigned char)(size-1), 0);
+ if (peak.getLevel() == size - 1) {
+ valeyr.set((unsigned char)(size - 1), 0);
ret &= 0x10;
}
if (ret >> 1) // consider the left part of the requested peak
{
// If the list of peaks is empty, compute it
- if (peaks.empty())
- {
+ if (peaks.empty()) {
/* nbpeaks = */ getPeaks(peaks);
- }
+ }
// if (1) {
// // vpTRACE("nb peaks: %d", nbpeaks);
@@ -935,58 +903,55 @@ vpHistogram::getValey(unsigned char dist,
// Go to the requested peak in the list
std::list<vpHistogramPeak>::const_iterator it;
unsigned index = 0;
- for (it = peaks.begin(); it != peaks.end(); ++ it)
- {
+ for (it = peaks.begin(); it != peaks.end(); ++it) {
if (peak == *it) {
// we are on the peak.
break;
}
- index ++;
+ index++;
}
bool found = false;
if (index == 0) {
// No chance to get a peak on the left
// should not occur !
- peakl.set(0,0);
- }
- else {
+ peakl.set(0, 0);
+ } else {
// search for the nearest peak on the left that matches the distance
std::list<vpHistogramPeak>::const_iterator lit; // left iterator
- for(lit = peaks.begin(); lit != it; ++ lit)
- {
+ for (lit = peaks.begin(); lit != it; ++lit) {
if (abs((*lit).getLevel() - peak.getLevel()) > dist) {
// peakl found
peakl = *lit;
- found = true; // we cannot stop here, since the other peaks on the right may exist
+ found = true; // we cannot stop here, since the other peaks on the
+ // right may exist
}
}
}
- if ( ! found)
- peakl.set(0,0);
+ if (!found)
+ peakl.set(0, 0);
// Search the valey on the left
mini = peak.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=peakl.getLevel(); i < peak.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
+ sumindmini = 0;
+ nbmini = 0;
+ for (unsigned i = peakl.getLevel(); i < peak.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini = histogram[i];
+ nbmini = 1;
+ sumindmini = i;
continue;
}
if (histogram[i] == mini) {
sumindmini += i;
- nbmini ++;
+ nbmini++;
}
}
if (nbmini == 0) {
valeyl.set(0, 0);
ret &= 0x01;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
+ } else {
+ unsigned int minipos = sumindmini / nbmini; // position of the minimum
valeyl.set((unsigned char)minipos, histogram[minipos]);
ret &= 0x11;
}
@@ -996,56 +961,54 @@ vpHistogram::getValey(unsigned char dist,
// If the list of peaks is empty, compute it
if (peaks.empty()) {
/* nbpeaks = */ getPeaks(peaks);
- }
+ }
// Go to the requested peak in the list
std::list<vpHistogramPeak>::const_iterator it;
unsigned index = 0;
- for (it = peaks.begin(); it != peaks.end(); ++ it)
- {
+ for (it = peaks.begin(); it != peaks.end(); ++it) {
if (peak == *it) {
// we are on the peak.
break;
}
- index ++;
+ index++;
}
bool found = false;
// search for the nearest peak on the right that matches the distance
std::list<vpHistogramPeak>::const_iterator rit; // right iterator
- for(rit = it; rit != peaks.end(); ++ rit)
+ for (rit = it; rit != peaks.end(); ++rit)
if (abs((*rit).getLevel() - peak.getLevel()) > dist) {
- // peakr found
- peakr = *rit;
- found = true;
- break; // we can stop here
- }
+ // peakr found
+ peakr = *rit;
+ found = true;
+ break; // we can stop here
+ }
- if ( ! found)
- peakr.set((unsigned char)(size-1),0);
+ if (!found)
+ peakr.set((unsigned char)(size - 1), 0);
// Search the valey on the right
mini = peak.getValue();
- sumindmini=0;
- nbmini=0;
- for (unsigned i=(unsigned int)peak.getLevel()+1; i <= (unsigned int)peakr.getLevel(); i++) {
- if (histogram[i] < mini) {
- mini=histogram[i];
- nbmini=1;
- sumindmini=i;
+ sumindmini = 0;
+ nbmini = 0;
+ for (unsigned i = (unsigned int)peak.getLevel() + 1; i <= (unsigned int)peakr.getLevel(); i++) {
+ if (histogram[i] < mini) {
+ mini = histogram[i];
+ nbmini = 1;
+ sumindmini = i;
continue;
}
if (histogram[i] == mini) {
sumindmini += i;
- nbmini ++;
+ nbmini++;
}
}
if (nbmini == 0) {
- valeyr.set((unsigned char)(size-1), 0);
+ valeyr.set((unsigned char)(size - 1), 0);
ret &= 0x10;
- }
- else {
- unsigned int minipos = sumindmini/nbmini; // position of the minimum
+ } else {
+ unsigned int minipos = sumindmini / nbmini; // position of the minimum
valeyr.set((unsigned char)minipos, histogram[minipos]);
ret &= 0x11;
}
@@ -1062,10 +1025,10 @@ vpHistogram::getValey(unsigned char dist,
\sa getPeak()
*/
-unsigned vpHistogram::sort(std::list<vpHistogramPeak> & peaks)
+unsigned vpHistogram::sort(std::list<vpHistogramPeak> &peaks)
{
- if ( peaks.empty() ) {
+ if (peaks.empty()) {
return 0;
}
@@ -1086,11 +1049,7 @@ unsigned vpHistogram::sort(std::list<vpHistogramPeak> & 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())); }
/*!
@@ -1104,14 +1063,13 @@ vpHistogram::write(const std::string &filename)
\return true if the file could be written, false otherwise.
*/
-bool
-vpHistogram::write(const char *filename)
+bool vpHistogram::write(const char *filename)
{
FILE *fd = fopen(filename, "w");
if (fd == NULL)
return false;
- for (unsigned i=0; i < size; i ++)
- fprintf(fd, "%u %d\n", i, histogram[i]);
+ for (unsigned i = 0; i < size; i++)
+ fprintf(fd, "%u %u\n", i, histogram[i]);
fclose(fd);
return true;
diff --git a/modules/core/src/tools/histogram/vpHistogramPeak.cpp b/modules/core/src/tools/histogram/vpHistogramPeak.cpp
index 8bd3d99..1dc8b78 100644
--- a/modules/core/src/tools/histogram/vpHistogramPeak.cpp
+++ b/modules/core/src/tools/histogram/vpHistogramPeak.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,7 +45,6 @@
#include <visp3/core/vpHistogramPeak.h>
-
/*!
Defaut constructor for a gray level histogram peak.
*/
@@ -53,17 +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; }
/*!
@@ -75,8 +70,7 @@ vpHistogramPeak::vpHistogramPeak(const vpHistogramPeak &p)
vpHistogramPeak p2 = p1; // Peak p2 is set to 0, 255
\endcode
*/
-vpHistogramPeak &
-vpHistogramPeak::operator=(const vpHistogramPeak &p)
+vpHistogramPeak &vpHistogramPeak::operator=(const vpHistogramPeak &p)
{
setLevel(p.level);
setValue(p.value);
@@ -91,19 +85,15 @@ 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();
return s;
diff --git a/modules/core/src/tools/histogram/vpHistogramValey.cpp b/modules/core/src/tools/histogram/vpHistogramValey.cpp
index 4a02fb3..1ba2a81 100644
--- a/modules/core/src/tools/histogram/vpHistogramValey.cpp
+++ b/modules/core/src/tools/histogram/vpHistogramValey.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -54,8 +55,7 @@
vpHistogramValey v2 = v1; // Valey p2 is set to 0, 255
\endcode
*/
-vpHistogramValey &
-vpHistogramValey::operator=(const vpHistogramValey &v)
+vpHistogramValey &vpHistogramValey::operator=(const vpHistogramValey &v)
{
setLevel(v.level);
setValue(v.value);
@@ -70,25 +70,23 @@ vpHistogramValey::operator=(const vpHistogramValey &v)
\param v : Gray level histogram valey to compar.
*/
-bool
-vpHistogramValey::operator==(const vpHistogramValey &v) const
+bool vpHistogramValey::operator==(const vpHistogramValey &v) const
{
- return ( (level == v.level) && (value == v.value) );
+ return ((level == v.level) && (value == v.value));
}
/*!
\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();
return s;
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/core/src/tools/network/vpClient.cpp b/modules/core/src/tools/network/vpClient.cpp
index a03b6d2..069c768 100644
--- a/modules/core/src/tools/network/vpClient.cpp
+++ b/modules/core/src/tools/network/vpClient.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,61 +38,56 @@
#include <visp3/core/vpClient.h>
-vpClient::vpClient() : vpNetwork(), numberOfAttempts(0)
-{
-}
+vpClient::vpClient() : vpNetwork(), numberOfAttempts(0) {}
/*!
Disconnect the client from all the servers, and close the sockets.
*/
-vpClient::~vpClient()
-{
- stop();
-}
+vpClient::~vpClient() { stop(); }
/*!
- Connect to the server represented by the given hostname, and at a given port.
-
+ Connect to the server represented by the given hostname, and at a given
+ port.
+
\sa vpClient::connectToIP();
\param hostname : Hostname of the server.
\param port_serv : Port used for the connection.
-
+
\return True if the connection has been etablished, false otherwise.
*/
bool vpClient::connectToHostname(const std::string &hostname, const unsigned int &port_serv)
{
// get server host information from hostname
- struct hostent *server = gethostbyname( hostname.c_str() );
-
- if ( server == NULL )
- {
- std::string noSuchHostMessage( "ERROR, " );
- noSuchHostMessage.append( hostname );
- noSuchHostMessage.append( ": no such host\n" );
- vpERROR_TRACE( noSuchHostMessage.c_str(),
- "vpClient::connectToHostname(const std::string &hostname, const int &port_serv)" );
+ struct hostent *server = gethostbyname(hostname.c_str());
+
+ if (server == NULL) {
+ std::string noSuchHostMessage("ERROR, ");
+ noSuchHostMessage.append(hostname);
+ noSuchHostMessage.append(": no such host\n");
+ vpERROR_TRACE(noSuchHostMessage.c_str(), "vpClient::connectToHostname("
+ "const std::string &hostname, "
+ "const int &port_serv)");
return false;
}
vpNetwork::vpReceptor serv;
-
- serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
+
+ serv.socketFileDescriptorReceptor = socket(AF_INET, SOCK_STREAM, 0);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( serv.socketFileDescriptorReceptor < 0){
+ if (serv.socketFileDescriptorReceptor < 0) {
#else
- if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
-#endif
- vpERROR_TRACE( "ERROR opening socket",
- "vpClient::connectToHostname()" );
+ if (serv.socketFileDescriptorReceptor == INVALID_SOCKET) {
+#endif
+ vpERROR_TRACE("ERROR opening socket", "vpClient::connectToHostname()");
return false;
}
-
- memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
+
+ memset((char *)&serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
serv.receptorAddress.sin_family = AF_INET;
- memmove( (char *) &serv.receptorAddress.sin_addr.s_addr, (char *) server->h_addr, (unsigned)server->h_length );
- serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
+ memmove((char *)&serv.receptorAddress.sin_addr.s_addr, (char *)server->h_addr, (unsigned)server->h_length);
+ serv.receptorAddress.sin_port = htons((unsigned short)port_serv);
serv.receptorIP = inet_ntoa(*(in_addr *)server->h_addr);
return connectServer(serv);
@@ -99,110 +95,104 @@ bool vpClient::connectToHostname(const std::string &hostname, const unsigned int
/*!
Connect to the server represented by the given ip, and at a given port.
-
+
\sa vpClient::connectToHostname()
\param ip : IP of the server.
\param port_serv : Port used for the connection.
-
+
\return True if the connection has been etablished, false otherwise.
*/
bool vpClient::connectToIP(const std::string &ip, const unsigned int &port_serv)
{
vpNetwork::vpReceptor serv;
-
- serv.socketFileDescriptorReceptor = socket( AF_INET, SOCK_STREAM, 0 );
+
+ serv.socketFileDescriptorReceptor = socket(AF_INET, SOCK_STREAM, 0);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if ( serv.socketFileDescriptorReceptor < 0){
+ if (serv.socketFileDescriptorReceptor < 0) {
#else
- if ( serv.socketFileDescriptorReceptor == INVALID_SOCKET){
+ if (serv.socketFileDescriptorReceptor == INVALID_SOCKET) {
#endif
- vpERROR_TRACE( "ERROR opening socket",
- "vpClient::connectToIP()" );
+ vpERROR_TRACE("ERROR opening socket", "vpClient::connectToIP()");
return false;
}
-
- memset((char *) &serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
+
+ memset((char *)&serv.receptorAddress, '\0', sizeof(serv.receptorAddress));
serv.receptorAddress.sin_family = AF_INET;
serv.receptorAddress.sin_addr.s_addr = inet_addr(ip.c_str());
- serv.receptorAddress.sin_port = htons( (unsigned short)port_serv );
-
+ serv.receptorAddress.sin_port = htons((unsigned short)port_serv);
+
return connectServer(serv);
}
/*!
Deconnect from the server at a specific index.
-
+
\param index : Index of the server.
*/
void vpClient::deconnect(const unsigned int &index)
-{
- if(index < receptor_list.size())
- {
+{
+ if (index < receptor_list.size()) {
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- shutdown( receptor_list[index].socketFileDescriptorReceptor, SHUT_RDWR );
+ shutdown(receptor_list[index].socketFileDescriptorReceptor, SHUT_RDWR);
#else // _WIN32
- shutdown( receptor_list[index].socketFileDescriptorReceptor, SD_BOTH );
+ shutdown(receptor_list[index].socketFileDescriptorReceptor, SD_BOTH);
#endif
- receptor_list.erase(receptor_list.begin()+(int)index);
- }
+ receptor_list.erase(receptor_list.begin() + (int)index);
+ }
}
/*!
- Stops the server and close the sockets.
+ Stops the server and close the sockets.
*/
void vpClient::stop()
{
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++){
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- shutdown( receptor_list[i].socketFileDescriptorReceptor, SHUT_RDWR );
+ shutdown(receptor_list[i].socketFileDescriptorReceptor, SHUT_RDWR);
#else // _WIN32
- shutdown( receptor_list[i].socketFileDescriptorReceptor, SD_BOTH );
+ shutdown(receptor_list[i].socketFileDescriptorReceptor, SD_BOTH);
#endif
- receptor_list.erase(receptor_list.begin()+(int)i);
+ receptor_list.erase(receptor_list.begin() + (int)i);
i--;
}
}
/*!
- Print the servers.
+ Print the servers.
*/
-void vpClient::print()
-{
- vpNetwork::print("Server");
-}
+void vpClient::print() { vpNetwork::print("Server"); }
-//Private function
+// Private function
bool vpClient::connectServer(vpNetwork::vpReceptor &serv)
{
- serv.receptorAddressSize = sizeof( serv.receptorAddress );
-
+ serv.receptorAddressSize = sizeof(serv.receptorAddress);
+
numberOfAttempts = 15;
unsigned int ind = 1;
- int connectionResult=-1;
-
- while(ind <= numberOfAttempts){
+ int connectionResult = -1;
+
+ while (ind <= numberOfAttempts) {
std::cout << "Attempt number " << ind << "..." << std::endl;
-
- connectionResult = connect( serv.socketFileDescriptorReceptor,
- (sockaddr*) &serv.receptorAddress,
- serv.receptorAddressSize );
- if(connectionResult >= 0)
+
+ connectionResult =
+ connect(serv.socketFileDescriptorReceptor, (sockaddr *)&serv.receptorAddress, serv.receptorAddressSize);
+ if (connectionResult >= 0)
break;
-
+
ind++;
vpTime::wait(1000);
}
-
- if( connectionResult< 0 )
- {
- vpERROR_TRACE( "ERROR connecting, the server may not be waiting for connection at this port.",
- "vpClient::connectServer()");
-
+
+ if (connectionResult < 0) {
+ vpERROR_TRACE("ERROR connecting, the server may not be waiting for "
+ "connection at this port.",
+ "vpClient::connectServer()");
+
return false;
}
-
+
receptor_list.push_back(serv);
#ifdef SO_NOSIGPIPE
diff --git a/modules/core/src/tools/network/vpNetwork.cpp b/modules/core/src/tools/network/vpNetwork.cpp
index 0625f7e..34d4d53 100644
--- a/modules/core/src/tools/network/vpNetwork.cpp
+++ b/modules/core/src/tools/network/vpNetwork.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,13 @@
*
*****************************************************************************/
-
#include <visp3/core/vpNetwork.h>
vpNetwork::vpNetwork()
- : emitter(), receptor_list(), readFileDescriptor(), socketMax(0), request_list(),
- max_size_message(999999), separator("[*@*]"), beginning("[*start*]"), end("[*end*]"),
- param_sep("[*|*]"), currentMessageReceived(), tv(), tv_sec(0), tv_usec(10),
- verboseMode(false)
-{
+ : 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
tv.tv_usec = (int)tv_usec;
@@ -52,11 +51,11 @@ vpNetwork::vpNetwork()
#endif
#if defined(_WIN32)
- //Enable the sockets to be used
- //Note that: if we were using "winsock.h" instead of "winsock2.h" we would had to use:
- //WSAStartup(MAKEWORD(1,0), &WSAData);
+ // Enable the sockets to be used
+ // Note that: if we were using "winsock.h" instead of "winsock2.h" we would
+ // had to use: WSAStartup(MAKEWORD(1,0), &WSAData);
WSADATA WSAData;
- WSAStartup(MAKEWORD(2,0), &WSAData);
+ WSAStartup(MAKEWORD(2, 0), &WSAData);
#endif
}
@@ -68,150 +67,144 @@ vpNetwork::~vpNetwork()
}
/*!
- Add a decoding request to the emitter. This request will be used to decode the received messages.
- Each request must have a different id.
-
- \warning vpRequest is a virtual pure class. It has to be implemented according to the way how you want
- to decode the message received.
-
+ Add a decoding request to the emitter. This request will be used to decode
+ the received messages. Each request must have a different id.
+
+ \warning vpRequest is a virtual pure class. It has to be implemented
+ according to the way how you want to decode the message received.
+
\sa vpNetwork::removeDecodingRequest()
-
+
\param req : Request to add.
*/
void vpNetwork::addDecodingRequest(vpRequest *req)
{
bool alreadyHas = false;
-
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- if(request_list[i]->getId() == req->getId()){
+
+ for (unsigned int i = 0; i < request_list.size(); i++)
+ if (request_list[i]->getId() == req->getId()) {
alreadyHas = true;
break;
}
-
- if(alreadyHas)
- std::cout << "Server already has one request with the similar ID. Request hasn't been added." << std::endl;
+
+ if (alreadyHas)
+ std::cout << "Server already has one request with the similar ID. "
+ "Request hasn't been added."
+ << std::endl;
else
request_list.push_back(req);
}
/*!
- Delete a decoding request from the emitter.
-
+ Delete a decoding request from the emitter.
+
\sa vpNetwork::addDecodingRequest()
-
+
\param id : Id of the request to delete.
*/
void vpNetwork::removeDecodingRequest(const char *id)
{
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- {
- if(request_list[i]->getId() == id)
- {
- request_list.erase(request_list.begin()+(int)i);
+ for (unsigned int i = 0; i < request_list.size(); i++) {
+ if (request_list[i]->getId() == id) {
+ request_list.erase(request_list.begin() + (int)i);
break;
}
}
}
/*!
- Print the receptors.
-
+ Print the receptors.
+
\param id : Message to display before the receptor's index.
*/
void vpNetwork::print(const char *id)
{
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
- {
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
std::cout << id << i << " : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
}
}
/*!
- Get the receptor index from its name. The name can be either the IP, or its name on the network.
-
+ Get the receptor index from its name. The name can be either the IP, or its
+ name on the network.
+
\param name : Name of the receptor.
-
+
\return Index of the receptor, or -1 if an error occurs.
*/
int vpNetwork::getReceptorIndex(const char *name)
{
struct hostent *server = gethostbyname(name);
-
- if ( server == NULL )
- {
- std::string noSuchHostMessage( "ERROR, " );
- noSuchHostMessage.append( name );
- noSuchHostMessage.append( ": no such host\n" );
- vpERROR_TRACE( noSuchHostMessage.c_str(), "vpNetwork::getReceptorIndex()" );
+
+ if (server == NULL) {
+ std::string noSuchHostMessage("ERROR, ");
+ noSuchHostMessage.append(name);
+ noSuchHostMessage.append(": no such host\n");
+ vpERROR_TRACE(noSuchHostMessage.c_str(), "vpNetwork::getReceptorIndex()");
return -1;
}
-
+
std::string ip = inet_ntoa(*(in_addr *)server->h_addr);
-
- for(int i = 0 ; i < (int)receptor_list.size() ; i++)
- {
- if(receptor_list[(unsigned)i].receptorIP == ip)
+
+ for (int i = 0; i < (int)receptor_list.size(); i++) {
+ if (receptor_list[(unsigned)i].receptorIP == ip)
return i;
}
-
+
return -1;
}
/*!
Send a request to the first receptor in the list.
-
+
\sa vpNetwork::sendRequestTo()
\sa vpNetwork::sendAndEncodeRequest()
\sa vpNetwork::sendAndEncodeRequestTo()
\sa vpNetwork::send()
\sa vpNetwork::sendTo()
-
+
\param req : Request to send.
-
+
\return The number of bytes that have been sent, -1 if an error occured.
*/
-int vpNetwork::sendRequest(vpRequest &req)
-{
- return sendRequestTo(req,0);
-}
+int vpNetwork::sendRequest(vpRequest &req) { return sendRequestTo(req, 0); }
/*!
Send a request to a specific receptor.
-
+
\sa vpNetwork::sendRequest()
\sa vpNetwork::sendAndEncodeRequest()
\sa vpNetwork::sendAndEncodeRequestTo()
\sa vpNetwork::send()
\sa vpNetwork::sendTo()
-
+
\param req : Request to send.
\param dest : Index of the receptor receiving the request.
-
+
\return The number of bytes that have been sent, -1 if an error occured.
*/
int vpNetwork::sendRequestTo(vpRequest &req, const unsigned int &dest)
{
int size = (int)receptor_list.size();
- int sizeMinusOne = (int)receptor_list.size()-1;
- if(size == 0 || dest > (unsigned)sizeMinusOne)
- {
- if(verboseMode)
- vpTRACE( "Cannot Send Request! Bad Index" );
+ int sizeMinusOne = (int)receptor_list.size() - 1;
+ if (size == 0 || dest > (unsigned)sizeMinusOne) {
+ if (verboseMode)
+ vpTRACE("Cannot Send Request! Bad Index");
return 0;
}
-
+
std::string message = beginning + req.getId() + separator;
-
- if(req.size() != 0){
+
+ if (req.size() != 0) {
message += req[0];
-
- for(unsigned int i = 1 ; i < req.size() ; i++){
- message += param_sep + req[i];
+
+ for (unsigned int i = 1; i < req.size(); i++) {
+ message += param_sep + req[i];
}
}
-
+
message += end;
-
+
int flags = 0;
//#if ! defined(APPLE) && ! defined(SOLARIS) && ! defined(_WIN32)
#if defined(__linux__)
@@ -220,26 +213,26 @@ int vpNetwork::sendRequestTo(vpRequest &req, const unsigned int &dest)
#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);
+ (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;
}
/*!
Send and encode a request to the first receptor in the list.
-
+
\sa vpNetwork::sendRequestTo()
\sa vpNetwork::sendAndEncodeRequest()
\sa vpNetwork::sendAndEncodeRequestTo()
\sa vpNetwork::send()
\sa vpNetwork::sendTo()
-
+
\param req : Request to send.
-
+
\return The number of bytes that have been sent, -1 if an error occured.
*/
int vpNetwork::sendAndEncodeRequest(vpRequest &req)
@@ -250,29 +243,29 @@ int vpNetwork::sendAndEncodeRequest(vpRequest &req)
/*!
Send and encode a request to a specific receptor.
-
+
\sa vpNetwork::sendRequest()
\sa vpNetwork::sendAndEncodeRequest()
\sa vpNetwork::sendAndEncodeRequestTo()
\sa vpNetwork::send()
\sa vpNetwork::sendTo()
-
+
\param req : Request to send.
\param dest : Index of the receptor receiving the request.
-
+
\return The number of bytes that have been sent, -1 if an error occured.
*/
int vpNetwork::sendAndEncodeRequestTo(vpRequest &req, const unsigned int &dest)
{
req.encode();
- return sendRequestTo(req,dest);
+ return sendRequestTo(req, dest);
}
/*!
Receive requests untils there is requests to receive.
-
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequestOnce()
@@ -289,10 +282,11 @@ std::vector<int> vpNetwork::receiveRequest()
}
/*!
- Receives requests, from a specific emitter, untils there is request to receive.
-
+ Receives requests, from a specific emitter, untils there is request to
+ receive.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequest()
\sa vpNetwork::receiveRequestOnce()
@@ -301,7 +295,7 @@ std::vector<int> vpNetwork::receiveRequest()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message
*/
std::vector<int> vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitting)
@@ -312,11 +306,11 @@ std::vector<int> vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitt
/*!
Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
+ This message can represent an entire request or not. Several calls to this
+ function might be necessary to get the entire request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -325,7 +319,7 @@ std::vector<int> vpNetwork::receiveRequestFrom(const unsigned int &receptorEmitt
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::receiveRequestOnce()
@@ -335,12 +329,13 @@ int vpNetwork::receiveRequestOnce()
}
/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
+ Receives a message once (in the limit of the Maximum message size value),
+ from a specific emitter. This message can represent an entire request or
+ not. Several calls to this function might be necessary to get the entire
+ request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -349,9 +344,9 @@ int vpNetwork::receiveRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message.
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting)
@@ -362,9 +357,9 @@ int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting)
/*!
Receives and decode requests untils there is requests to receive.
-
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequestOnce()
@@ -377,18 +372,19 @@ int vpNetwork::receiveRequestOnceFrom(const unsigned int &receptorEmitting)
std::vector<int> vpNetwork::receiveAndDecodeRequest()
{
std::vector<int> res = receiveRequest();
- for(unsigned int i = 0 ; i < res.size() ; i++)
- if(res[i] != -1)
+ for (unsigned int i = 0; i < res.size(); i++)
+ if (res[i] != -1)
request_list[(unsigned)res[i]]->decode();
-
+
return res;
}
/*!
- Receives and decode requests, from a specific emitter, untils there is request to receive.
-
+ Receives and decode requests, from a specific emitter, untils there is
+ request to receive.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequest()
\sa vpNetwork::receiveRequestOnce()
@@ -397,27 +393,28 @@ std::vector<int> vpNetwork::receiveAndDecodeRequest()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message
*/
std::vector<int> vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &receptorEmitting)
{
std::vector<int> res = receiveRequestFrom(receptorEmitting);
- for(unsigned int i = 0 ; i < res.size() ; i++)
- if(res[i] != -1)
+ for (unsigned int i = 0; i < res.size(); i++) {
+ if (res[i] != -1)
request_list[(unsigned)res[i]]->decode();
-
- return res;
+ }
+
+ return res;
}
/*!
Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
- If it represents an entire request, it decodes the request.
-
+ This message can represent an entire request or not. Several calls to this
+ function might be necessary to get the entire request. If it represents an
+ entire request, it decodes the request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -426,26 +423,26 @@ std::vector<int> vpNetwork::receiveAndDecodeRequestFrom(const unsigned int &rece
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::receiveAndDecodeRequestOnce()
{
int res = receiveRequestOnce();
- if(res != -1)
+ if (res != -1)
request_list[(unsigned)res]->decode();
-
+
return res;
}
/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
- If it represents an entire request, it decodes the request.
-
+ Receives a message once (in the limit of the Maximum message size value),
+ from a specific emitter. This message can represent an entire request or
+ not. Several calls to this function might be necessary to get the entire
+ request. If it represents an entire request, it decodes the request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -454,58 +451,56 @@ int vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message.
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::receiveAndDecodeRequestOnceFrom(const unsigned int &receptorEmitting)
{
int res = receiveRequestOnceFrom(receptorEmitting);
- if(res != -1)
+ if (res != -1)
request_list[(unsigned)res]->decode();
-
+
return res;
}
-
//######## Definition of Template Functions ########
//# #
//##################################################
-
/*!
Handle requests until there are requests to handle.
-
- \warning : This function doesn't decode the requests. If it does handle a request that hasn't been ran yet,
- The request's parameters will be replace.
-
+
+ \warning : This function doesn't decode the requests. If it does handle a
+ request that hasn't been ran yet, The request's parameters will be replace.
+
\sa vpNetwork::handleFirstRequest()
-
- \return : The list of index corresponding to the requests that have been handled.
+
+ \return : The list of index corresponding to the requests that have been
+ handled.
*/
std::vector<int> vpNetwork::_handleRequests()
{
std::vector<int> resIndex;
int index = _handleFirstRequest();
-
- while(index != -1)
- {
+
+ while (index != -1) {
resIndex.push_back(index);
index = _handleFirstRequest();
}
-
+
return resIndex;
}
/*!
Handle the first request in the queue.
-
- \warning : This function doesn't run the request. If it does handle a request that hasn't been ran yet,
- The request's parameters will be replace.
-
+
+ \warning : This function doesn't run the request. If it does handle a
+ request that hasn't been ran yet, The request's parameters will be replace.
+
\sa vpNetwork::handleRequests()
-
+
\return : The index of the request that has been handled.
*/
int vpNetwork::_handleFirstRequest()
@@ -513,89 +508,86 @@ int vpNetwork::_handleFirstRequest()
size_t indStart = currentMessageReceived.find(beginning);
size_t indSep = currentMessageReceived.find(separator);
size_t indEnd = currentMessageReceived.find(end);
-
- if (indStart == std::string::npos && indSep == std::string::npos && indEnd == std::string::npos)
- {
- if(currentMessageReceived.size() != 0)
+
+ if (indStart == std::string::npos && indSep == std::string::npos && indEnd == std::string::npos) {
+ if (currentMessageReceived.size() != 0)
currentMessageReceived.clear();
-
- if(verboseMode)
+
+ if (verboseMode)
vpTRACE("Incorrect message");
-
+
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)
- {
- if(verboseMode)
+
+ if (indEnd < indStart) {
+ if (verboseMode)
vpTRACE("Incorrect message");
- currentMessageReceived.erase((unsigned)indStart,indEnd+end.size());
+ currentMessageReceived.erase((unsigned)indStart, indEnd + end.size());
return -1;
}
-
- size_t indStart2 = currentMessageReceived.find(beginning,indStart+1);
- if(indStart2 != std::string::npos && indStart2 < indEnd)
- {
- if(verboseMode)
+
+ size_t indStart2 = currentMessageReceived.find(beginning, indStart + 1);
+ if (indStart2 != std::string::npos && indStart2 < indEnd) {
+ if (verboseMode)
vpTRACE("Incorrect message");
- currentMessageReceived.erase((unsigned)indStart,(unsigned)indStart2);
+ currentMessageReceived.erase((unsigned)indStart, (unsigned)indStart2);
return -1;
}
-
+
size_t deb = indStart + beginning.size();
std::string id = currentMessageReceived.substr((unsigned)deb, indSep - deb);
-
- //deb = indSep+separator.size();
- //std::string params = currentMessageReceived.substr((unsigned)deb, (unsigned)(indEnd - deb));
-
-// std::cout << "Handling : " << currentMessageReceived.substr(indStart, indEnd+end.size() - indStart) << std::endl;
-
+
+ // deb = indSep+separator.size();
+ // std::string params = currentMessageReceived.substr((unsigned)deb,
+ // (unsigned)(indEnd - deb));
+
+ // std::cout << "Handling : " << currentMessageReceived.substr(indStart,
+ // indEnd+end.size() - indStart) << std::endl;
+
int indRequest = 0;
bool hasBeenFound = false;
- for(unsigned int i = 0 ; i < request_list.size() ; i++)
- {
- if(id == request_list[i]->getId()){
- hasBeenFound = true;
- request_list[i]->clear();
- indRequest = (int)i;
- break;
+ for (unsigned int i = 0; i < request_list.size(); i++) {
+ if (id == request_list[i]->getId()) {
+ hasBeenFound = true;
+ request_list[i]->clear();
+ indRequest = (int)i;
+ break;
}
}
-
- if(!hasBeenFound){
- //currentMessageReceived.erase(indStart,indEnd+end.size());
- if(verboseMode)
+
+ if (!hasBeenFound) {
+ // currentMessageReceived.erase(indStart,indEnd+end.size());
+ if (verboseMode)
vpTRACE("No request corresponds to the received message");
return -1;
}
-
+
size_t indDebParam = indSep + separator.size();
- size_t indEndParam = currentMessageReceived.find(param_sep,indDebParam);
-
+ size_t indEndParam = currentMessageReceived.find(param_sep, indDebParam);
+
std::string param;
- while(indEndParam != std::string::npos || indEndParam < indEnd)
- {
- param = currentMessageReceived.substr((unsigned)indDebParam,(unsigned)(indEndParam - indDebParam));
+ while (indEndParam != std::string::npos || indEndParam < indEnd) {
+ param = currentMessageReceived.substr((unsigned)indDebParam, (unsigned)(indEndParam - indDebParam));
request_list[(unsigned)indRequest]->addParameter(param);
- indDebParam = indEndParam+param_sep.size();
- indEndParam = currentMessageReceived.find(param_sep,indDebParam);
+ indDebParam = indEndParam + param_sep.size();
+ indEndParam = currentMessageReceived.find(param_sep, indDebParam);
}
-
+
param = currentMessageReceived.substr((unsigned)indDebParam, indEnd - indDebParam);
request_list[(unsigned)indRequest]->addParameter(param);
- currentMessageReceived.erase(indStart,indEnd+end.size());
-
+ currentMessageReceived.erase(indStart, indEnd + end.size());
+
return indRequest;
}
/*!
Receive requests untils there is requests to receive.
-
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequestOnce()
@@ -607,14 +599,16 @@ int vpNetwork::_handleFirstRequest()
*/
void vpNetwork::_receiveRequest()
{
- while(_receiveRequestOnce() > 0) {};
+ while (_receiveRequestOnce() > 0) {
+ };
}
/*!
- Receives requests, from a specific emitter, untils there is request to receive.
-
+ Receives requests, from a specific emitter, untils there is request to
+ receive.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequest()
\sa vpNetwork::receiveRequestOnce()
@@ -623,21 +617,22 @@ void vpNetwork::_receiveRequest()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message
*/
void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting)
{
- while(_receiveRequestOnceFrom(receptorEmitting) > 0) {};
+ while (_receiveRequestOnceFrom(receptorEmitting) > 0) {
+ };
}
/*!
Receives a message once (in the limit of the Maximum message size value).
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
+ This message can represent an entire request or not. Several calls to this
+ function might be necessary to get the entire request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -646,18 +641,17 @@ void vpNetwork::_receiveRequestFrom(const unsigned int &receptorEmitting)
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::_receiveRequestOnce()
{
- if(receptor_list.size() == 0)
- {
- if(verboseMode)
- vpTRACE( "No Receptor!" );
+ if (receptor_list.size() == 0) {
+ if (verboseMode)
+ vpTRACE("No Receptor!");
return -1;
}
-
+
tv.tv_sec = tv_sec;
#if TARGET_OS_IPHONE
tv.tv_usec = (int)tv_usec;
@@ -665,65 +659,63 @@ int vpNetwork::_receiveRequestOnce()
tv.tv_usec = tv_usec;
#endif
- FD_ZERO(&readFileDescriptor);
-
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(i == 0)
+ FD_ZERO(&readFileDescriptor);
+
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
+ if (i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor);
+ if (socketMax < receptor_list[i].socketFileDescriptorReceptor)
socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- 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){
- if(verboseMode)
- vpERROR_TRACE( "Select error" );
+
+ if (value == -1) {
+ if (verboseMode)
+ vpERROR_TRACE("Select error");
return -1;
- }
- else if(value == 0){
- //Timeout
+ } else if (value == 0) {
+ // Timeout
return 0;
- }
- else{
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
- char *buf = new char [max_size_message];
+ } else {
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
+ if (FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor)) {
+ char *buf = new char[max_size_message];
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes=(int)recv(receptor_list[i].socketFileDescriptorReceptor, buf, max_size_message, 0);
+ 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)
- {
+
+ if (numbytes <= 0) {
std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
- delete [] buf;
+ receptor_list.erase(receptor_list.begin() + (int)i);
+ delete[] buf;
return numbytes;
- }
- else if(numbytes > 0){
+ } else {
std::string returnVal(buf, (unsigned int)numbytes);
currentMessageReceived.append(returnVal);
}
- delete [] buf;
+ delete[] buf;
break;
}
}
}
-
+
return numbytes;
}
/*!
- Receives a message once (in the limit of the Maximum message size value), from a specific emitter.
- This message can represent an entire request or not. Several calls to this function
- might be necessary to get the entire request.
-
+ Receives a message once (in the limit of the Maximum message size value),
+ from a specific emitter. This message can represent an entire request or
+ not. Several calls to this function might be necessary to get the entire
+ request.
+
\warning Requests will be received but not decoded.
-
+
\sa vpNetwork::receive()
\sa vpNetwork::receiveRequestFrom()
\sa vpNetwork::receiveRequest()
@@ -732,22 +724,21 @@ int vpNetwork::_receiveRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestFrom()
\sa vpNetwork::receiveAndDecodeRequestOnce()
\sa vpNetwork::receiveAndDecodeRequestOnceFrom()
-
+
\param receptorEmitting : Index of the receptor emitting the message.
-
+
\return The number of bytes received, -1 if an error occured.
*/
int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting)
{
int size = (int)receptor_list.size();
- int sizeMinusOne = (int)receptor_list.size()-1;
- if(size == 0 || receptorEmitting > (unsigned)sizeMinusOne )
- {
- if(verboseMode)
- vpTRACE( "No receptor at the specified index!" );
+ int sizeMinusOne = (int)receptor_list.size() - 1;
+ if (size == 0 || receptorEmitting > (unsigned)sizeMinusOne) {
+ if (verboseMode)
+ vpTRACE("No receptor at the specified index!");
return -1;
}
-
+
tv.tv_sec = tv_sec;
#if TARGET_OS_IPHONE
tv.tv_usec = (int)tv_usec;
@@ -755,47 +746,42 @@ int vpNetwork::_receiveRequestOnceFrom(const unsigned int &receptorEmitting)
tv.tv_usec = tv_usec;
#endif
- FD_ZERO(&readFileDescriptor);
-
+ 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)
- vpERROR_TRACE( "Select error" );
+ if (value == -1) {
+ if (verboseMode)
+ vpERROR_TRACE("Select error");
return -1;
- }
- else if(value == 0){
- //Timeout
+ } else if (value == 0) {
+ // Timeout
return 0;
- }
- else{
- if(FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor,&readFileDescriptor)){
- char *buf = new char [max_size_message];
+ } else {
+ if (FD_ISSET((unsigned int)receptor_list[receptorEmitting].socketFileDescriptorReceptor, &readFileDescriptor)) {
+ char *buf = new char[max_size_message];
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- numbytes=(int)recv(receptor_list[receptorEmitting].socketFileDescriptorReceptor, buf, max_size_message, 0);
+ 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::endl;
- receptor_list.erase(receptor_list.begin()+(int)receptorEmitting);
- delete [] buf;
+ if (numbytes <= 0) {
+ std::cout << "Disconnected : " << inet_ntoa(receptor_list[receptorEmitting].receptorAddress.sin_addr)
+ << std::endl;
+ receptor_list.erase(receptor_list.begin() + (int)receptorEmitting);
+ delete[] buf;
return numbytes;
- }
- else if(numbytes > 0){
+ } else {
std::string returnVal(buf, (unsigned int)numbytes);
currentMessageReceived.append(returnVal);
}
- delete [] buf;
+ delete[] buf;
}
}
-
+
return numbytes;
}
-
-
-
diff --git a/modules/core/src/tools/network/vpRequest.cpp b/modules/core/src/tools/network/vpRequest.cpp
index b7c627c..193e2b8 100644
--- a/modules/core/src/tools/network/vpRequest.cpp
+++ b/modules/core/src/tools/network/vpRequest.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,48 +38,42 @@
#include <visp3/core/vpRequest.h>
-vpRequest::vpRequest()
- : request_id(""), listOfParams()
-{}
+vpRequest::vpRequest() : request_id(""), listOfParams() {}
+
+vpRequest::~vpRequest() {}
-vpRequest::~vpRequest()
-{}
-
/*!
Add a message as parameter of the request.
-
+
\sa vpNetwork::addParameterObject()
-
+
\param params : Array of characters representing the message to add.
*/
void vpRequest::addParameter(char *params)
-{
+{
std::string val = params;
listOfParams.push_back(val);
}
/*!
Add a message as parameter of the request.
-
+
\sa vpNetwork::addParameterObject()
-
+
\param params : std::string representing the message to add.
*/
-void vpRequest::addParameter(std::string ¶ms)
-{
- listOfParams.push_back(params);
-}
+void vpRequest::addParameter(std::string ¶ms) { listOfParams.push_back(params); }
/*!
Add messages as parameters of the request.
Each message corresponds to one parameter.
-
+
\sa vpNetwork::addParameterObject()
-
+
\param listOfparams : Array of std::string representing the messages to add.
*/
-void vpRequest::addParameter(std::vector< std::string > &listOfparams)
-{
- for(unsigned int i = 0; i < listOfparams.size() ; i++)
+void vpRequest::addParameter(std::vector<std::string> &listOfparams)
+{
+ for (unsigned int i = 0; i < listOfparams.size(); i++)
listOfparams.push_back(listOfparams[i]);
}
diff --git a/modules/core/src/tools/network/vpServer.cpp b/modules/core/src/tools/network/vpServer.cpp
index 80661af..f24b357 100644
--- a/modules/core/src/tools/network/vpServer.cpp
+++ b/modules/core/src/tools/network/vpServer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,13 +39,13 @@
#include <visp3/core/vpServer.h>
#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
-# include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
#endif
/*!
Construct a server on the machine launching it.
*/
-vpServer::vpServer( ) : adress(), port(0), started(false), max_clients(10)
+vpServer::vpServer() : adress(), port(0), started(false), max_clients(10)
{
int protocol = 0;
emitter.socketFileDescriptorEmitter = socket(AF_INET, SOCK_STREAM, protocol);
@@ -54,22 +55,22 @@ vpServer::vpServer( ) : adress(), port(0), started(false), max_clients(10)
if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
#endif
{
- vpERROR_TRACE( "vpServer::vpServer(), cannot open socket." );
+ vpERROR_TRACE("vpServer::vpServer(), cannot open socket.");
}
emitter.emitterAddress.sin_family = AF_INET;
emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY;
emitter.emitterAddress.sin_port = 0;
-
+
adress = inet_ntoa(emitter.emitterAddress.sin_addr);
port = emitter.emitterAddress.sin_port;
}
/*!
Construct a server on the machine launching it, with a specified port.
-
+
\param port_serv : server's port.
*/
-vpServer::vpServer( const int &port_serv ) : adress(), port(0), started(false), max_clients(10)
+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);
@@ -79,23 +80,23 @@ vpServer::vpServer( const int &port_serv ) : adress(), port(0), started(false),
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_port = htons( (unsigned short)port_serv );
-
+ emitter.emitterAddress.sin_addr.s_addr = INADDR_ANY; // inet_addr("127.0.0.1");;
+ emitter.emitterAddress.sin_port = htons((unsigned short)port_serv);
+
adress = inet_ntoa(emitter.emitterAddress.sin_addr);
port = port_serv;
}
/*!
Construct a server on the machine at a given adress, with a specified port.
-
+
\param adress_serv : server's adress.
\param port_serv : server's port.
*/
-vpServer::vpServer( const std::string &adress_serv,const int &port_serv )
+vpServer::vpServer(const std::string &adress_serv, const int &port_serv)
: adress(), port(0), started(false), max_clients(10)
{
int protocol = 0;
@@ -106,70 +107,71 @@ vpServer::vpServer( const std::string &adress_serv,const int &port_serv )
if (emitter.socketFileDescriptorEmitter == INVALID_SOCKET)
#endif
{
- vpERROR_TRACE( "vpServer::vpServer(const std::string &adress_serv,const int &port_serv), cannot open socket." );
+ vpERROR_TRACE("vpServer::vpServer(const std::string &adress_serv,const "
+ "int &port_serv), cannot open socket.");
}
emitter.emitterAddress.sin_family = AF_INET;
emitter.emitterAddress.sin_addr.s_addr = inet_addr(adress_serv.c_str());
- emitter.emitterAddress.sin_port = htons( (unsigned short)port_serv );
-
+ emitter.emitterAddress.sin_port = htons((unsigned short)port_serv);
+
adress = adress_serv;
port = port_serv;
}
/*!
- Shutdown the server.
+ Shutdown the server.
*/
vpServer::~vpServer()
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- close( emitter.socketFileDescriptorEmitter );
-#else //Win32
- closesocket( (unsigned)emitter.socketFileDescriptorEmitter );
+ close(emitter.socketFileDescriptorEmitter);
+#else // Win32
+ closesocket((unsigned)emitter.socketFileDescriptorEmitter);
#endif
- for(unsigned int i = 0 ; i < receptor_list.size() ; i++)
+ for (unsigned int i = 0; i < receptor_list.size(); i++)
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- close( receptor_list[i].socketFileDescriptorReceptor );
-#else //Win32
- closesocket( (unsigned)receptor_list[i].socketFileDescriptorReceptor );
+ close(receptor_list[i].socketFileDescriptorReceptor);
+#else // Win32
+ closesocket((unsigned)receptor_list[i].socketFileDescriptorReceptor);
#endif
-
}
/*!
- Enable the server to wait for clients (on the limit of the maximum limit).
-
+ Enable the server to wait for clients (on the limit of the maximum limit).
+
\return True if the server has started, false otherwise.
*/
bool vpServer::start()
-{
+{
int serverStructLength = sizeof(emitter.emitterAddress);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- int bindResult = bind( emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, (unsigned)serverStructLength );
-#else //Win32
- int bindResult = bind( (unsigned)emitter.socketFileDescriptorEmitter, (struct sockaddr *) &emitter.emitterAddress, serverStructLength );
+ int bindResult = bind(emitter.socketFileDescriptorEmitter, (struct sockaddr *)&emitter.emitterAddress,
+ (unsigned)serverStructLength);
+#else // Win32
+ int bindResult = bind((unsigned)emitter.socketFileDescriptorEmitter, (struct sockaddr *)&emitter.emitterAddress,
+ serverStructLength);
#endif
-
-
- if( bindResult < 0 )
- {
+
+ if (bindResult < 0) {
std::cout << "Error id : " << bindResult << std::endl;
- std::string errorMessage( "vpServer::vpServer(), cannot bind to port" );
+ std::string errorMessage("vpServer::vpServer(), cannot bind to port");
char posrtNumberString[32];
- sprintf( posrtNumberString, "%d", port );
+ sprintf(posrtNumberString, "%d", port);
errorMessage += " ";
errorMessage += posrtNumberString;
errorMessage += " The port may be already used.";
- vpERROR_TRACE( errorMessage.c_str() );
+ vpERROR_TRACE(errorMessage.c_str());
return false;
}
-
+
#ifdef SO_NOSIGPIPE
// Mac OS X does not have the MSG_NOSIGNAL flag. It does have this
// connections based version, however.
if (emitter.socketFileDescriptorEmitter > 0) {
int set_option = 1;
- if (0 == setsockopt(emitter.socketFileDescriptorEmitter, SOL_SOCKET, SO_NOSIGPIPE, &set_option, sizeof(set_option))) {
+ 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;
}
@@ -177,30 +179,31 @@ bool vpServer::start()
#endif // SO_NOSIGPIPE
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- listen( emitter.socketFileDescriptorEmitter, (int)max_clients );
-#else //Win32
- listen( (unsigned)emitter.socketFileDescriptorEmitter, (int)max_clients );
+ listen(emitter.socketFileDescriptorEmitter, (int)max_clients);
+#else // Win32
+ listen((unsigned)emitter.socketFileDescriptorEmitter, (int)max_clients);
#endif
-
+
std::cout << "Server ready" << std::endl;
-
+
started = true;
-
+
return true;
}
/*!
Check if a client has connected or deconnected the server
-
- \return True if a client connected or deconnected, false otherwise OR server not started yet.
+
+ \return True if a client connected or deconnected, false otherwise OR server
+ not started yet.
*/
bool vpServer::checkForConnections()
{
- if(!started)
- if(!start()){
+ if (!started)
+ if (!start()) {
return false;
}
-
+
tv.tv_sec = tv_sec;
#if TARGET_OS_IPHONE
tv.tv_usec = (int)tv_usec;
@@ -208,80 +211,76 @@ bool vpServer::checkForConnections()
tv.tv_usec = tv_usec;
#endif
- FD_ZERO(&readFileDescriptor);
-
+ FD_ZERO(&readFileDescriptor);
+
socketMax = emitter.socketFileDescriptorEmitter;
- FD_SET((unsigned)emitter.socketFileDescriptorEmitter,&readFileDescriptor);
+ FD_SET((unsigned)emitter.socketFileDescriptorEmitter, &readFileDescriptor);
- for(unsigned int i=0; i<receptor_list.size(); i++){
- FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor);
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
+ FD_SET((unsigned)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor);
- if(i == 0)
+ if (i == 0)
+ socketMax = receptor_list[i].socketFileDescriptorReceptor;
+
+ if (socketMax < receptor_list[i].socketFileDescriptorReceptor)
socketMax = receptor_list[i].socketFileDescriptorReceptor;
-
- if(socketMax < receptor_list[i].socketFileDescriptorReceptor) socketMax = receptor_list[i].socketFileDescriptorReceptor;
}
-
- int value = select((int)socketMax+1,&readFileDescriptor,NULL,NULL,&tv);
- if(value == -1){
- //vpERROR_TRACE( "vpServer::run(), select()" );
+
+ int value = select((int)socketMax + 1, &readFileDescriptor, NULL, NULL, &tv);
+ if (value == -1) {
+ // vpERROR_TRACE( "vpServer::run(), select()" );
return false;
- }
- else if(value == 0){
+ } else if (value == 0) {
return false;
- }
- else{
- if(FD_ISSET((unsigned int)emitter.socketFileDescriptorEmitter,&readFileDescriptor)){
+ } else {
+ if (FD_ISSET((unsigned int)emitter.socketFileDescriptorEmitter, &readFileDescriptor)) {
vpNetwork::vpReceptor client;
client.receptorAddressSize = sizeof(client.receptorAddress);
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- client.socketFileDescriptorReceptor = accept(emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
-#else //Win32
- client.socketFileDescriptorReceptor = accept((unsigned int)emitter.socketFileDescriptorEmitter,(struct sockaddr*) &client.receptorAddress, &client.receptorAddressSize);
+ client.socketFileDescriptorReceptor = accept(
+ emitter.socketFileDescriptorEmitter, (struct sockaddr *)&client.receptorAddress, &client.receptorAddressSize);
+#else // Win32
+ client.socketFileDescriptorReceptor =
+ accept((unsigned int)emitter.socketFileDescriptorEmitter, (struct sockaddr *)&client.receptorAddress,
+ &client.receptorAddressSize);
#endif
-
+
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- if((client.socketFileDescriptorReceptor) == -1)
+ if ((client.socketFileDescriptorReceptor) == -1)
#else
- if((client.socketFileDescriptorReceptor) == INVALID_SOCKET)
+ if ((client.socketFileDescriptorReceptor) == INVALID_SOCKET)
#endif
- vpERROR_TRACE( "vpServer::run(), accept()" );
-
+ vpERROR_TRACE("vpServer::run(), accept()");
+
client.receptorIP = inet_ntoa(client.receptorAddress.sin_addr);
printf("New client connected : %s\n", inet_ntoa(client.receptorAddress.sin_addr));
receptor_list.push_back(client);
-
+
return true;
- }
- else{
- for(unsigned int i=0; i<receptor_list.size(); i++){
- if(FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor,&readFileDescriptor)){
+ } else {
+ for (unsigned int i = 0; i < receptor_list.size(); i++) {
+ if (FD_ISSET((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &readFileDescriptor)) {
char deco;
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
ssize_t numbytes = recv(receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
-#else //Win32
+#else // Win32
int numbytes = recv((unsigned int)receptor_list[i].socketFileDescriptorReceptor, &deco, 1, MSG_PEEK);
#endif
-
- if(numbytes == 0)
- {
+
+ if (numbytes == 0) {
std::cout << "Disconnected : " << inet_ntoa(receptor_list[i].receptorAddress.sin_addr) << std::endl;
- receptor_list.erase(receptor_list.begin()+(int)i);
+ receptor_list.erase(receptor_list.begin() + (int)i);
return 0;
}
}
}
}
}
-
+
return false;
}
/*!
- Print the connected clients.
+ Print the connected clients.
*/
-void vpServer::print()
-{
- vpNetwork::print("Client");
-}
-
+void vpServer::print() { vpNetwork::print("Client"); }
diff --git a/modules/core/src/tools/network/vpUDPClient.cpp b/modules/core/src/tools/network/vpUDPClient.cpp
new file mode 100644
index 0000000..ff72b05
--- /dev/null
+++ b/modules/core/src/tools/network/vpUDPClient.cpp
@@ -0,0 +1,226 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * UDP Client
+ *
+ *****************************************************************************/
+
+#include <cstring>
+#include <sstream>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+#include <arpa/inet.h>
+#include <errno.h>
+#include <netdb.h>
+#include <unistd.h>
+#define DWORD int
+#else
+#if defined(__MINGW32__)
+#define _WIN32_WINNT _WIN32_WINNT_VISTA // 0x0600
+#endif
+#include <Ws2tcpip.h>
+#endif
+
+#include <visp3/core/vpUDPClient.h>
+
+/*!
+ Create a (IPv4) UDP client.
+
+ \param hostname : Server hostname or IP address.
+ \param port : Server port number.
+*/
+vpUDPClient::vpUDPClient(const std::string &hostname, const int port)
+ : m_serverAddress(), m_serverLength(0), m_socketFileDescriptor()
+#if defined(_WIN32)
+ ,
+ m_wsa()
+#endif
+{
+ init(hostname, port);
+}
+
+vpUDPClient::~vpUDPClient()
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ close(m_socketFileDescriptor);
+#else
+ closesocket(m_socketFileDescriptor);
+ WSACleanup();
+#endif
+}
+
+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();
+ throw vpException(vpException::fatalError, ss.str());
+ }
+#endif
+
+ /* socket: create the socket */
+ m_socketFileDescriptor = socket(AF_INET, SOCK_DGRAM, 0);
+#if defined(_WIN32)
+ if (m_socketFileDescriptor == INVALID_SOCKET)
+#else
+ if (m_socketFileDescriptor < 0)
+#endif
+ throw vpException(vpException::fatalError, "Error opening UDP socket for the client!");
+
+ /* build the server's Internet address */
+ memset(&m_serverAddress, 0, sizeof(m_serverAddress));
+ std::stringstream ss;
+ ss << port;
+ struct addrinfo hints;
+ struct addrinfo *result = NULL;
+ struct addrinfo *ptr = NULL;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_DGRAM;
+ hints.ai_protocol = IPPROTO_UDP;
+
+ DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result);
+ if (dwRetval != 0) {
+ ss.str("");
+ ss << "getaddrinfo failed with error: " << dwRetval;
+ throw vpException(vpException::fatalError, ss.str());
+ }
+
+ for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
+ if (ptr->ai_family == AF_INET && ptr->ai_socktype == SOCK_DGRAM) {
+ m_serverAddress = *(struct sockaddr_in *)ptr->ai_addr;
+ break;
+ }
+ }
+
+ freeaddrinfo(result);
+
+ m_serverLength = sizeof(m_serverAddress);
+}
+
+/*!
+ Receive data sent by the server.
+
+ \param msg : ASCII message or byte data.
+ \param timeoutMs : Timeout in millisecond (if zero, the call is blocking).
+
+ \return The message length / size of the byte array sent received, or -1 if
+ there is an error, or 0 if there is a timeout.
+
+ \note
+ To transform the ASCII representation of an integer:
+ \code
+ int val = atoi(msg.c_str());
+ //or
+ std::istringstream ss(msg);
+ ss >> val;
+ \endcode
+ To convert from a byte array to an integer:
+ \code
+ int val = *reinterpret_cast<const int *>(msg.c_str());
+ \endcode
+*/
+int vpUDPClient::receive(std::string &msg, const int timeoutMs)
+{
+ fd_set s;
+ FD_ZERO(&s);
+ FD_SET(m_socketFileDescriptor, &s);
+ struct timeval timeout;
+ if (timeoutMs > 0) {
+ 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);
+
+ if (retval == -1) {
+ std::cerr << "Error select!" << std::endl;
+ return -1;
+ }
+
+ 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,
+ (socklen_t *)&m_serverLength);
+ if (length <= 0) {
+ return length < 0 ? -1 : 0;
+ }
+
+ msg = std::string(m_buf, length);
+ return length;
+ }
+
+ // Timeout
+ return 0;
+}
+
+/*!
+ Send data to the server.
+
+ \param msg : ASCII message or byte data.
+
+ \return The message length / size of the byte array sent.
+
+ \note
+ To send the ASCII representation of an integer:
+ \code
+ int val = 1024;
+ std::ostringstream os;
+ os << val;
+ 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)];
+ 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);
+ \endcode
+*/
+int vpUDPClient::send(const std::string &msg)
+{
+ if (msg.size() > VP_MAX_UDP_PAYLOAD) {
+ std::cerr << "Message is too long!" << std::endl;
+ return 0;
+ }
+
+/* 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);
+#else
+ 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
new file mode 100644
index 0000000..2397bd7
--- /dev/null
+++ b/modules/core/src/tools/network/vpUDPServer.cpp
@@ -0,0 +1,319 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * UDP Server
+ *
+ *****************************************************************************/
+
+#include <cstring>
+#include <sstream>
+
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+#include <arpa/inet.h>
+#include <errno.h>
+#include <netdb.h>
+#include <unistd.h>
+#define DWORD int
+#define WSAGetLastError() strerror(errno)
+#else
+#if defined(__MINGW32__)
+#define _WIN32_WINNT _WIN32_WINNT_VISTA // 0x0600
+#endif
+#include <Ws2tcpip.h>
+#endif
+
+#include <visp3/core/vpUDPServer.h>
+
+/*!
+ Create a (IPv4) UDP server.
+
+ \param port : Server port number.
+ \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)
+#if defined(_WIN32)
+ ,
+ m_wsa()
+#endif
+{
+ init("", port);
+}
+
+/*!
+ Create a UDP server.
+
+ \param hostname : Server hostname or IP address.
+ \param port : Server port number.
+*/
+vpUDPServer::vpUDPServer(const std::string &hostname, const int port)
+ : m_clientAddress(), m_clientLength(0), m_serverAddress(), m_socketFileDescriptor(0)
+#if defined(_WIN32)
+ ,
+ m_wsa()
+#endif
+{
+ init(hostname, port);
+}
+
+vpUDPServer::~vpUDPServer()
+{
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+ close(m_socketFileDescriptor);
+#else
+ closesocket(m_socketFileDescriptor);
+ WSACleanup();
+#endif
+}
+
+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();
+ throw vpException(vpException::fatalError, ss.str());
+ }
+#endif
+
+ /* socket: create the socket */
+ m_socketFileDescriptor = socket(AF_INET, SOCK_DGRAM, 0);
+#if defined(_WIN32)
+ if (m_socketFileDescriptor == INVALID_SOCKET)
+#else
+ if (m_socketFileDescriptor < 0)
+#endif
+ 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
+ int optval = 1;
+ 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));
+#endif
+
+ /* build the server's Internet address */
+ memset(&m_serverAddress, 0, sizeof(m_serverAddress));
+ if (hostname.empty()) {
+ m_serverAddress.sin_family = AF_INET;
+ m_serverAddress.sin_addr.s_addr = htonl(INADDR_ANY);
+ m_serverAddress.sin_port = htons((unsigned short)port);
+ } else {
+ std::stringstream ss;
+ ss << port;
+ struct addrinfo hints;
+ struct addrinfo *result = NULL;
+ struct addrinfo *ptr = NULL;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_DGRAM;
+ hints.ai_protocol = IPPROTO_UDP;
+
+ DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result);
+ if (dwRetval != 0) {
+ ss.str("");
+ ss << "getaddrinfo failed with error: " << dwRetval;
+ throw vpException(vpException::fatalError, ss.str());
+ }
+
+ for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
+ if (ptr->ai_family == AF_INET && ptr->ai_socktype == SOCK_DGRAM) {
+ m_serverAddress = *(struct sockaddr_in *)ptr->ai_addr;
+ break;
+ }
+ }
+
+ freeaddrinfo(result);
+ }
+
+ /* 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!");
+
+ m_clientLength = sizeof(m_clientAddress);
+}
+
+/*!
+ Receive data sent by a client.
+
+ \param msg : ASCII message or byte data according to the data sent by the
+ client. \param timeoutMs : Timeout in millisecond (if zero, the call is
+ blocking).
+
+ \return The message length / size of the byte array sent received, or -1 if
+ there is an error, or 0 if there is a timeout. \note See
+ vpUDPClient::receive for an example.
+*/
+int vpUDPServer::receive(std::string &msg, const int timeoutMs)
+{
+ std::string hostInfo = "";
+ return receive(msg, hostInfo, timeoutMs);
+}
+
+/*!
+ Receive data sent by a client.
+
+ \param msg : ASCII message or byte data according to the data send by the
+ client. \param hostInfo : Information about the client ("client_name
+ client_ip client_port"). \param timeoutMs : Timeout in millisecond (if zero,
+ the call is blocking).
+
+ \return The message length / size of the byte array sent received, or -1 if
+ 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)
+{
+ fd_set s;
+ FD_ZERO(&s);
+ FD_SET(m_socketFileDescriptor, &s);
+ struct timeval timeout;
+ if (timeoutMs > 0) {
+ 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);
+
+ if (retval == -1) {
+ std::cerr << "Error select!" << std::endl;
+ return -1;
+ }
+
+ 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,
+ (socklen_t *)&m_clientLength);
+#else
+ int length =
+ recvfrom(m_socketFileDescriptor, m_buf, sizeof(m_buf), 0, (struct sockaddr *)&m_clientAddress, &m_clientLength);
+#endif
+ if (length <= 0) {
+ return length < 0 ? -1 : 0;
+ }
+
+ msg = std::string(m_buf, length);
+
+ /* 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);
+
+ std::string hostName = "", hostIp = "", hostPort = "";
+ if (dwRetval != 0) {
+ 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));
+ if (ptr == NULL) {
+ std::cerr << "inet_ntop failed with error: " << WSAGetLastError() << std::endl;
+ } else {
+ hostIp = result;
+ }
+
+ std::stringstream ss;
+ ss << hostName << " " << hostIp << " " << hostPort;
+ hostInfo = ss.str();
+
+ return length;
+ }
+
+ // Timeout
+ return 0;
+}
+
+/*!
+ Send data to a client.
+
+ \param msg : ASCII message or byte data.
+ \param hostname : Client hostname (hostname or ip address).
+ \param port : Client port number.
+
+ \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)
+{
+ if (msg.size() > VP_MAX_UDP_PAYLOAD) {
+ std::cerr << "Message is too long!" << std::endl;
+ return 0;
+ }
+
+ // Create client address
+ memset(&m_clientAddress, 0, sizeof(m_clientAddress));
+ std::stringstream ss;
+ ss << port;
+ struct addrinfo hints;
+ struct addrinfo *result = NULL;
+ struct addrinfo *ptr = NULL;
+
+ memset(&hints, 0, sizeof(hints));
+ hints.ai_family = AF_INET;
+ hints.ai_socktype = SOCK_DGRAM;
+ hints.ai_protocol = IPPROTO_UDP;
+
+ DWORD dwRetval = getaddrinfo(hostname.c_str(), ss.str().c_str(), &hints, &result);
+ if (dwRetval != 0) {
+ ss.str("");
+ ss << "getaddrinfo failed with error: " << dwRetval;
+ throw vpException(vpException::fatalError, ss.str());
+ }
+
+ for (ptr = result; ptr != NULL; ptr = ptr->ai_next) {
+ if (ptr->ai_family == AF_INET && ptr->ai_socktype == SOCK_DGRAM) {
+ m_clientAddress = *(struct sockaddr_in *)ptr->ai_addr;
+ break;
+ }
+ }
+
+ 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);
+#else
+ 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 5ce2323..ed2ade1 100644
--- a/modules/core/src/tools/time/vpTime.cpp
+++ b/modules/core/src/tools/time/vpTime.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,12 +37,10 @@
*
*****************************************************************************/
-
#include <ctime>
-#include <visp3/core/vpTime.h>
#include <visp3/core/vpDebug.h>
-
+#include <visp3/core/vpTime.h>
/*!
\file vpTime.cpp
@@ -49,15 +48,14 @@
*/
-
// Unix depend version
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <sys/time.h>
-# include <unistd.h>
+#include <sys/time.h>
+#include <unistd.h>
#elif defined(_WIN32)
-# include <windows.h>
-# include <winbase.h>
+//#include <winbase.h>
+#include <windows.h>
#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -76,13 +74,10 @@ double minTimeForUsleepCall = 4;
/*!
\return The time during which a while loop is used to handle the time
- wainting functions. When the time to wait is greater than that value, we use
- non blocking functions like usleep() instead.
+ wainting functions. When the time to wait is greater than that value, we
+ use non blocking functions like usleep() instead.
*/
-double getMinTimeForUsleepCall()
-{
- return minTimeForUsleepCall;
-}
+double getMinTimeForUsleepCall() { return minTimeForUsleepCall; }
/*!
\fn vpTime::measureTimeMs()
@@ -94,22 +89,21 @@ double measureTimeMs()
{
#if defined(_WIN32)
#if !defined(WINRT)
- LARGE_INTEGER time, frequency;
+ LARGE_INTEGER time, frequency;
QueryPerformanceFrequency(&frequency);
- if(frequency.QuadPart == 0){
- return(timeGetTime());
- }
- else{
+ if (frequency.QuadPart == 0) {
+ return (timeGetTime());
+ } else {
QueryPerformanceCounter(&time);
- return (double)(1000.0*time.QuadPart/frequency.QuadPart);
+ return (double)(1000.0 * time.QuadPart / frequency.QuadPart);
}
-# else
- throw(vpException(vpException::fatalError, "Cannot get time: not implemented on Universal Windows Platform"));
-# endif
+#else
+ 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
struct timeval tp;
- gettimeofday(&tp,0);
- return(1000.0*tp.tv_sec + tp.tv_usec/1000.0);
+ gettimeofday(&tp, 0);
+ return (1000.0 * tp.tv_sec + tp.tv_usec / 1000.0);
#endif
}
@@ -121,23 +115,22 @@ double measureTimeMs()
double measureTimeMicros()
{
#if defined(_WIN32)
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- LARGE_INTEGER time, frequency;
+#if !defined(WINRT)
+ LARGE_INTEGER time, frequency;
QueryPerformanceFrequency(&frequency);
- if(frequency.QuadPart == 0){
- return(timeGetTime());
- }
- else{
+ if (frequency.QuadPart == 0) {
+ return (timeGetTime());
+ } else {
QueryPerformanceCounter(&time);
- return (double)(1000000.0*time.QuadPart/frequency.QuadPart);
+ return (double)(1000000.0 * time.QuadPart / frequency.QuadPart);
}
-# elif WINAPI_FAMILY_PARTITION(WINAPI_PARTITION_APP)
- throw(vpException(vpException::fatalError, "Cannot get time: not implemented on Universal Windows Platform"));
-# endif
#else
+ 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
struct timeval tp;
- gettimeofday(&tp,0);
- return(1000000.0*tp.tv_sec + tp.tv_usec);
+ gettimeofday(&tp, 0);
+ return (1000000.0 * tp.tv_sec + tp.tv_usec);
#endif
}
@@ -145,8 +138,8 @@ double measureTimeMicros()
Wait t miliseconds after t0 (in ms).
- The waiting is done by a call to usleep() if the time to wait is greater than
- vpTime::minTimeForUsleepCall.
+ The waiting is done by a call to usleep() if the time to wait is greater
+ than vpTime::minTimeForUsleepCall.
\param t0 : reference time (in ms)
\param t : time to wait (in ms)
@@ -161,12 +154,12 @@ int wait(double t0, double t)
timeToWait = t0 + t - timeCurrent;
- if ( timeToWait <= 0. ) // no need to wait
- return(1);
+ if (timeToWait <= 0.) // no need to wait
+ return (1);
else {
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
if (timeToWait > vpTime::minTimeForUsleepCall) {
- usleep((useconds_t)((timeToWait-vpTime::minTimeForUsleepCall)*1000));
+ usleep((useconds_t)((timeToWait - vpTime::minTimeForUsleepCall) * 1000));
}
// Blocking loop to have an accurate waiting
do {
@@ -177,9 +170,9 @@ int wait(double t0, double t)
return 0;
#elif defined(_WIN32)
-# if !defined(WINRT_8_0)
+#if !defined(WINRT_8_0)
if (timeToWait > vpTime::minTimeForUsleepCall) {
- Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
+ Sleep((DWORD)(timeToWait - vpTime::minTimeForUsleepCall));
}
// Blocking loop to have an accurate waiting
do {
@@ -189,9 +182,10 @@ int wait(double t0, double t)
} while (timeToWait > 0.);
return 0;
-# else
- throw(vpException(vpException::functionNotImplementedError, "vpTime::wait() is not implemented on Windows Phone 8.0"));
-# endif
+#else
+ throw(vpException(vpException::functionNotImplementedError,
+ "vpTime::wait() is not implemented on Windows Phone 8.0"));
+#endif
#endif
}
}
@@ -199,8 +193,8 @@ int wait(double t0, double t)
/*!
Wait t miliseconds from now.
- The waiting is done by a call to usleep() if the time to wait is greater than
- vpTime::minTimeForUsleepCall.
+ The waiting is done by a call to usleep() if the time to wait is greater
+ than vpTime::minTimeForUsleepCall.
\param t : Time to wait in ms.
@@ -209,13 +203,13 @@ void wait(double t)
{
double timeToWait = t;
- if ( timeToWait <= 0. ) // no need to wait
+ if (timeToWait <= 0.) // no need to wait
return;
else {
#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 {
@@ -226,10 +220,10 @@ void wait(double t)
return;
#elif defined(_WIN32)
-# if !defined(WINRT_8_0)
+#if !defined(WINRT_8_0)
double t0 = measureTimeMs();
if (timeToWait > vpTime::minTimeForUsleepCall) {
- Sleep((DWORD)(timeToWait-vpTime::minTimeForUsleepCall));
+ Sleep((DWORD)(timeToWait - vpTime::minTimeForUsleepCall));
}
// Blocking loop to have an accurate waiting
do {
@@ -239,9 +233,10 @@ void wait(double t)
} while (timeToWait > 0.);
return;
-# else
- throw(vpException(vpException::functionNotImplementedError, "vpTime::wait() is not implemented on Windows Phone 8.0"));
-# endif
+#else
+ throw(vpException(vpException::functionNotImplementedError,
+ "vpTime::wait() is not implemented on Windows Phone 8.0"));
+#endif
#endif
}
}
@@ -252,10 +247,7 @@ void wait(double t)
\sa measureTimeMs()
*/
-double measureTimeSecond()
-{
- return vpTime::measureTimeMs()/1000.0 ;
-}
+double measureTimeSecond() { return vpTime::measureTimeMs() / 1000.0; }
/*!
Sleep t miliseconds from now.
@@ -266,13 +258,14 @@ double measureTimeSecond()
void sleepMs(double t)
{
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep((useconds_t)(t*1000));
+ usleep((useconds_t)(t * 1000));
#elif defined(_WIN32)
-# if !defined(WINRT_8_0)
+#if !defined(WINRT_8_0)
Sleep((DWORD)(t));
-# else
- throw(vpException(vpException::functionNotImplementedError, "vpTime::sleepMs() is not implemented on Windows Phone 8.0"));
-# endif
+#else
+ throw(vpException(vpException::functionNotImplementedError,
+ "vpTime::sleepMs() is not implemented on Windows Phone 8.0"));
+#endif
#endif
}
@@ -323,7 +316,8 @@ void sleepMs(double t)
| %% | A % sign | % |
* The specifiers marked with an asterisk (*) are locale-dependent.
- \return A formated date and time string. When default format is used, the returned string contains "YYYY/MM/DD hh:mm:ss".
+ \return A formated date and time string. When default format is used, the
+returned string contains "YYYY/MM/DD hh:mm:ss".
The following example shows how to use this function:
\code
@@ -333,8 +327,8 @@ 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 << "%F format : " << vpTime::getDateTime("%F") << std::endl;
+ std::cout << "%X format : " << vpTime::getDateTime("%X") << std::endl;
return 0;
}
@@ -348,12 +342,13 @@ int main()
\endcode
*/
-std::string getDateTime(const std::string &format) {
+std::string getDateTime(const std::string &format)
+{
time_t rawtime;
- struct tm * timeinfo;
+ struct tm *timeinfo;
char buffer[80];
- time (&rawtime);
+ time(&rawtime);
timeinfo = localtime(&rawtime);
strftime(buffer, 80, format.c_str(), timeinfo);
diff --git a/modules/core/src/tools/xml/vpXmlParser.cpp b/modules/core/src/tools/xml/vpXmlParser.cpp
index 1de2768..d4de9a3 100644
--- a/modules/core/src/tools/xml/vpXmlParser.cpp
+++ b/modules/core/src/tools/xml/vpXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,83 +35,76 @@
* Romain Tallonneau
*
*****************************************************************************/
-
-#include <visp3/core/vpXmlParser.h>
+
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpXmlParser.h>
#ifdef VISP_HAVE_XML2
-#include <visp3/core/vpException.h>
-#include <visp3/core/vpDebug.h>
#include <libxml/parser.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpException.h>
-#include <string>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <string>
#include <typeinfo>
/*!
Basic constructor.
-
+
Initialise the main tag with default value.
*/
-vpXmlParser::vpXmlParser() : nodeMap(), main_tag("config")
-{
-}
+vpXmlParser::vpXmlParser() : nodeMap(), main_tag("config") {}
/*!
Basic destructor that does nothing.
- \warning As stated in http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser
- to clean up memory allocated by the xml2 library itself, the user should call
- xmlCleanupParser() only when the process has finished using the xml2 library.
- In case of doubt abstain from calling this function or do it just before calling exit()
- to avoid leak reports from valgrind ! That's why
- in ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the static
- function vpXmlParser::cleanup() that calls xmlCleanupParser() that could be called
- just before exit().
-
+ \warning As stated in
+ http://xmlsoft.org/html/libxml-parser.html#xmlCleanupParser to clean up
+ memory allocated by the xml2 library itself, the user should call
+ xmlCleanupParser() only when the process has finished using the xml2
+ library. In case of doubt abstain from calling this function or do it just
+ before calling exit() to avoid leak reports from valgrind ! That's why in
+ ViSP the destructor doesn't call xmlCleanupParser(). Rather we provide the
+ static function vpXmlParser::cleanup() that calls xmlCleanupParser() that
+ could be called just before exit().
+
\sa cleanup()
*/
vpXmlParser::~vpXmlParser()
{
- //xmlCleanupParser();
+ // xmlCleanupParser();
}
/*!
- Cpoy constructor.
-
+ Cpoy constructor.
+
\param _twin : The parser to copy.
*/
-vpXmlParser::vpXmlParser(const vpXmlParser& _twin)
- : nodeMap(), main_tag("config")
-{
- main_tag = _twin.main_tag;
- nodeMap = _twin.nodeMap;
-}
+vpXmlParser::vpXmlParser(const vpXmlParser &_twin) : nodeMap(_twin.nodeMap), main_tag(_twin.main_tag) {}
/* utilities functions to read/write data from an xml document */
/*!
- Read an array of character.
-
- \warning The array of characters is allocated and must be explicitly freed to
- avoid memory leak.
-
+ Read an array of character.
+
+ \warning The array of characters is allocated and must be explicitly freed
+ to avoid memory leak.
+
\param doc : The main xml document
\param node : a pointer to the node to read value
-
+
\return pointer to an allocated array of character.
*/
-char*
-vpXmlParser::xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node)
+char *vpXmlParser::xmlReadCharChild(xmlDocPtr doc, xmlNodePtr node)
{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read char*";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read char*";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- return (char *) xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ return (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
}
/*!
@@ -123,15 +117,14 @@ vpXmlParser::xmlReadCharChild (xmlDocPtr doc, xmlNodePtr node)
\return std::string representing the value.
*/
-std::string
-vpXmlParser::xmlReadStringChild (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";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read std::string";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char* dataCh = (char*)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ char *dataCh = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
std::string dataStr = dataCh;
xmlFree(dataCh);
return dataStr;
@@ -139,183 +132,182 @@ vpXmlParser::xmlReadStringChild (xmlDocPtr doc, xmlNodePtr node)
/*!
read an int
-
- \warning throw a vpException::ioError if the value cannot be parsed to an integer
-
+
+ \warning throw a vpException::ioError if the value cannot be parsed to an
+ integer
+
\param doc : The main xml document
\param node : a pointer to the node to read value
-
+
\return the integer value in the node
*/
-int
-vpXmlParser::xmlReadIntChild (xmlDocPtr doc, xmlNodePtr node)
+int vpXmlParser::xmlReadIntChild(xmlDocPtr doc, xmlNodePtr node)
{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read int";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read int";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char * val_char;
- char * control_convert;
+ char *val_char;
+ char *control_convert;
int val_int;
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_int = (int)strtol ((char *)val_char, &control_convert, 10);
+ val_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ val_int = (int)strtol((char *)val_char, &control_convert, 10);
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
+ if (val_char == control_convert) {
+ xmlFree((xmlChar *)val_char);
throw vpException(vpException::ioError, "cannot parse entry to int");
}
- xmlFree((xmlChar*) val_char);
+ xmlFree((xmlChar *)val_char);
return val_int;
}
/*!
read an int
-
- \warning throw a vpException::ioError if the value cannot be parsed to an unsigned integer
-
+
+ \warning throw a vpException::ioError if the value cannot be parsed to an
+ unsigned integer
+
\param doc : The main xml document
\param node : a pointer to the node to read value
-
+
\return the unsigned integer value in the node
*/
-unsigned int
-vpXmlParser::xmlReadUnsignedIntChild (xmlDocPtr doc, xmlNodePtr node)
+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";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read unsigned int";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char * val_char;
- char * control_convert;
+ char *val_char;
+ char *control_convert;
unsigned int val_uint;
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_uint = (unsigned int)strtoul ((char *)val_char, &control_convert, 10);
+ val_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ val_uint = (unsigned int)strtoul((char *)val_char, &control_convert, 10);
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
+ if (val_char == control_convert) {
+ xmlFree((xmlChar *)val_char);
throw vpException(vpException::ioError, "cannot parse entry to int");
}
- xmlFree((xmlChar*) val_char);
+ xmlFree((xmlChar *)val_char);
return val_uint;
}
/*!
read a double
-
- \warning throw a vpException::ioError if the value cannot be parsed to a double
-
+
+ \warning throw a vpException::ioError if the value cannot be parsed to a
+ double
+
\param doc : The main xml document
\param node : a pointer to the node to read value
-
+
\return the double value in the node
*/
-double
-vpXmlParser::xmlReadDoubleChild (xmlDocPtr doc, xmlNodePtr node)
+double vpXmlParser::xmlReadDoubleChild(xmlDocPtr doc, xmlNodePtr node)
{
- if(node ->xmlChildrenNode == NULL){
- std::string errorMsg = "Empty node " + std::string((char*)node->name) + ", cannot read double";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read double";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char * val_char;
- char * control_convert;
+ char *val_char;
+ char *control_convert;
double val_double;
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
- val_double = strtod ((char *)val_char, &control_convert);
+ val_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ val_double = strtod((char *)val_char, &control_convert);
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
+ if (val_char == control_convert) {
+ xmlFree((xmlChar *)val_char);
throw vpException(vpException::ioError, "cannot parse entry to double");
}
- xmlFree((xmlChar*) val_char);
+ xmlFree((xmlChar *)val_char);
return val_double;
}
/*!
read a float
- \warning throw a vpException::ioError if the value cannot be parsed to a float
+ \warning throw a vpException::ioError if the value cannot be parsed to a
+ float
\param doc : The main xml document
\param node : a pointer to the node to read value
\return the float value in the node
*/
-float
-vpXmlParser::xmlReadFloatChild (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";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read float";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char * val_char;
- char * control_convert;
+ char *val_char;
+ char *control_convert;
float val_float;
- val_char = (char *) xmlNodeListGetString(doc, node ->xmlChildrenNode, 1);
+ val_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
#if defined(VISP_HAVE_FUNC_STRTOF)
- val_float = strtof ((char *)val_char, &control_convert);
+ val_float = strtof((char *)val_char, &control_convert);
#else
- val_float = (float) strtod ((char *)val_char, &control_convert);
+ val_float = (float)strtod((char *)val_char, &control_convert);
#endif
- if (val_char == control_convert){
- xmlFree((xmlChar*) val_char);
+ if (val_char == control_convert) {
+ xmlFree((xmlChar *)val_char);
throw vpException(vpException::ioError, "cannot parse entry to float");
}
- xmlFree((xmlChar*) val_char);
+ xmlFree((xmlChar *)val_char);
return val_float;
}
/*!
read a boolean
- \warning throw a vpException::ioError if the value cannot be parsed to a bool
+ \warning throw a vpException::ioError if the value cannot be parsed to a
+ bool
\param doc : The main xml document
\param node : a pointer to the node to read value
\return the bool value in the node
*/
-bool
-vpXmlParser::xmlReadBoolChild (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";
+ if (node->xmlChildrenNode == NULL) {
+ std::string errorMsg = "Empty node " + std::string((char *)node->name) + ", cannot read bool";
std::cerr << errorMsg << std::endl;
throw vpException(vpException::fatalError, errorMsg);
}
- char * val_char;
+ char *val_char;
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_char = (char *)xmlNodeListGetString(doc, node->xmlChildrenNode, 1);
+ val_bool = val_char[0] != '0'; // reading only 1st character : bool xml storage is '0' or '1'
- xmlFree((xmlChar*) val_char);
+ xmlFree((xmlChar *)val_char);
return val_bool;
}
/*!
- write an array of character.
-
+ write an array of character.
+
\param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
+ \param 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);
- xmlAddChild(node, tmp);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)value);
+ xmlAddChild(node, tmp);
}
/*!
@@ -325,65 +317,59 @@ vpXmlParser::xmlWriteCharChild(xmlNodePtr node, const char* label, const char* v
\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());
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)value.c_str());
xmlAddChild(node, tmp);
}
-
/*!
- write an integer.
-
+ write an integer.
+
\param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
+ \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);
xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)str);
+ xmlAddChild(node, tmp);
}
/*!
- write an unsigned integer.
-
+ write an unsigned integer.
+
\param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
+ \param 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);
xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)str);
+ xmlAddChild(node, tmp);
}
-
/*!
- write a double.
-
+ write a double.
+
\param node : a pointer to the node to read value
- \param label : label (name of the data) of the node
+ \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);
xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
- xmlAddChild(node, tmp);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)str);
+ xmlAddChild(node, tmp);
}
/*!
@@ -393,13 +379,12 @@ vpXmlParser::xmlWriteDoubleChild(xmlNodePtr node, const char* label, const doubl
\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);
xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)str);
xmlAddChild(node, tmp);
}
@@ -410,44 +395,44 @@ vpXmlParser::xmlWriteFloatChild(xmlNodePtr node, const char* label, const float
\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);
xmlNodePtr tmp;
- tmp = xmlNewChild(node, NULL, (xmlChar*)label, (xmlChar*)str);
+ tmp = xmlNewChild(node, NULL, (xmlChar *)label, (xmlChar *)str);
xmlAddChild(node, tmp);
}
-/* -------------------------------------------------------------------------- */
-/* MAIN METHODS */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* MAIN METHODS */
+/* --------------------------------------------------------------------------
+ */
/*!
parse the document.
- The data in the file are stored in the attributes of the child class. This
+ The data in the file are stored in the attributes of the child class. This
method calls the readMainClass method which has to be implemented for every
child class depending on the content to parse.
-
+
\param filename : name of the file to parse
*/
-void
-vpXmlParser::parse(const std::string& filename)
+void vpXmlParser::parse(const std::string &filename)
{
xmlDocPtr doc;
xmlNodePtr root_node;
doc = xmlParseFile(filename.c_str());
- if(doc == NULL){
- vpERROR_TRACE("cannot open file");
- throw vpException(vpException::ioError, "cannot open file");
+ if (doc == NULL) {
+ vpERROR_TRACE("cannot open file");
+ throw vpException(vpException::ioError, "cannot open file");
}
root_node = xmlDocGetRootElement(doc);
- if(root_node == NULL){
- vpERROR_TRACE("cannot get root element");
- throw vpException(vpException::ioError, "cannot get root element");
+ if (root_node == NULL) {
+ vpERROR_TRACE("cannot get root element");
+ throw vpException(vpException::ioError, "cannot get root element");
}
readMainClass(doc, root_node);
@@ -458,51 +443,49 @@ vpXmlParser::parse(const std::string& filename)
/*!
Save the content of the class in the file given in parameters.
The data of the class are in the child class.
- This method calls the write_main_class method which has to be implemented for
- every class depending on the data to save.
-
+ This method calls the write_main_class method which has to be implemented
+ for every class depending on the data to save.
+
\param filename : the name of the file used to record the data
- \param append : if true and if the file exists, the data will be added to the
- data already in the file
+ \param append : if true and if the file exists, the data will be added to
+ the data already in the file
*/
-void
-vpXmlParser::save(const std::string& filename, const bool append)
+void vpXmlParser::save(const std::string &filename, const bool append)
{
xmlDocPtr doc;
xmlNodePtr root_node;
doc = xmlReadFile(filename.c_str(), NULL, XML_PARSE_NOWARNING + XML_PARSE_NOERROR + XML_PARSE_NOBLANKS);
- if (doc == NULL){
- doc = xmlNewDoc ((xmlChar*)"1.0");
- root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
+ if (doc == NULL) {
+ doc = xmlNewDoc((xmlChar *)"1.0");
+ root_node = xmlNewNode(NULL, (xmlChar *)main_tag.c_str());
xmlDocSetRootElement(doc, root_node);
- }
- else{
- if(!append){
+ } else {
+ if (!append) {
xmlFreeDoc(doc);
if (remove(filename.c_str()) != 0)
throw vpException(vpException::ioError, "Cannot remove existing xml file");
- doc = xmlNewDoc ((xmlChar*)"1.0");
- root_node = xmlNewNode(NULL, (xmlChar*)main_tag.c_str());
- xmlDocSetRootElement(doc, root_node);
- }
+ doc = xmlNewDoc((xmlChar *)"1.0");
+ root_node = xmlNewNode(NULL, (xmlChar *)main_tag.c_str());
+ xmlDocSetRootElement(doc, root_node);
+ }
}
root_node = xmlDocGetRootElement(doc);
- if (root_node == NULL)
- {
- vpERROR_TRACE("problem to get the root node");
- throw vpException(vpException::ioError, "problem to get the root node");
+ if (root_node == NULL) {
+ vpERROR_TRACE("problem to get the root node");
+ throw vpException(vpException::ioError, "problem to get the root node");
}
- writeMainClass(root_node);
+ writeMainClass(root_node);
xmlSaveFormatFile(filename.c_str(), doc, 1);
xmlFreeDoc(doc);
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpXmlParser.cpp.o) has no symbols
-void dummy_vpXmlParser() {};
+// Work arround to avoid warning: libvisp_core.a(vpXmlParser.cpp.o) has no
+// symbols
+void dummy_vpXmlParser(){};
#endif
diff --git a/modules/core/src/tracking/forward-projection/vpCircle.cpp b/modules/core/src/tracking/forward-projection/vpCircle.cpp
index caefb1b..4aab5c7 100644
--- a/modules/core/src/tracking/forward-projection/vpCircle.cpp
+++ b/modules/core/src/tracking/forward-projection/vpCircle.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,39 +36,34 @@
*
*****************************************************************************/
-
#include <visp3/core/vpCircle.h>
#include <visp3/core/vpFeatureDisplay.h>
-void
-vpCircle::init()
+void vpCircle::init()
{
- oP.resize(7) ;
- cP.resize(7) ;
+ oP.resize(7);
+ cP.resize(7);
- p.resize(5) ;
+ p.resize(5);
}
-/*!
- Set the world coordinates of the circle from the intersection of a plane and a sphere.
- We mean here the coordinates of the circle in the object frame
-
- \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
- oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
- oP[6] corresponds to the radius of the sphere.
+/*!
+ Set the world coordinates of the circle from the intersection of a plane and
+ a sphere. We mean here the coordinates of the circle in the object frame
+
+ \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane
+ equation Ax + By + Cz = 0. oP[3], oP[4], oP[5] correspond to X, Y, Z the
+ coordinates of the center of the sphere. oP[6] corresponds to the radius of
+ the sphere.
*/
-void
-vpCircle::setWorldCoordinates(const vpColVector& oP_)
-{
- this->oP = oP_ ;
-}
+void vpCircle::setWorldCoordinates(const vpColVector &oP_) { this->oP = oP_; }
+
+/*!
+ Set the world coordinates of the circle from the intersection of a plane and
+ a sphere. We mean here the coordinates of the circle in the object frame
-/*!
- Set the world coordinates of the circle from the intersection of a plane and a sphere.
- We mean here the coordinates of the circle in the object frame
-
\param A : A from the plane equation Ax + By + Cz = 0.
\param B : B from the plane equation Ax + By + Cz = 0.
\param C : C from the plane equation Ax + By + Cz = 0.
@@ -76,45 +72,39 @@ vpCircle::setWorldCoordinates(const vpColVector& 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 ;
- oP[2] = C ;
- oP[3] = X0 ;
- oP[4] = Y0 ;
- oP[5] = Z0 ;
- oP[6] = R ;
+ oP[0] = A;
+ oP[1] = B;
+ oP[2] = C;
+ oP[3] = X0;
+ oP[4] = Y0;
+ oP[5] = Z0;
+ oP[6] = R;
}
+vpCircle::vpCircle() { init(); }
+/*!
+ Construct the circle from the intersection of a plane and a sphere.
-vpCircle::vpCircle()
-{
- init() ;
-}
+ \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane
+ equation Ax + By + Cz = 0. oP[3], oP[4], oP[5] correspond to X, Y, Z the
+ coordinates of the center of the sphere. oP[6] corresponds to the radius of
+ the sphere.
-/*!
- Construct the circle from the intersection of a plane and a sphere.
-
- \param oP_ : oP[0], oP[1], oP[2] correspond to A, B, C from the plane equation Ax + By + Cz = 0.
- oP[3], oP[4], oP[5] correspond to X, Y, Z the coordinates of the center of the sphere.
- oP[6] corresponds to the radius of the sphere.
-
\sa setWorldCoordinates()
*/
-vpCircle::vpCircle(const vpColVector& oP_)
+vpCircle::vpCircle(const vpColVector &oP_)
{
- init() ;
- setWorldCoordinates(oP_) ;
+ init();
+ setWorldCoordinates(oP_);
}
-/*!
- Construct the circle from the intersection of a plane and a sphere.
-
+/*!
+ Construct the circle from the intersection of a plane and a sphere.
+
\param A : A from the plane equation Ax + By + Cz = 0.
\param B : B from the plane equation Ax + By + Cz = 0.
\param C : C from the plane equation Ax + By + Cz = 0.
@@ -122,289 +112,256 @@ vpCircle::vpCircle(const vpColVector& oP_)
\param Y0 : Y Coordinate of the center of the sphere.
\param Z0 : Z Coordinate of the center of the sphere.
\param R : Radius of the sphere.
-
+
\sa setWorldCoordinates()
*/
-vpCircle::vpCircle(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R)
-{
- init() ;
- setWorldCoordinates(A, B, C,
- X0, Y0, Z0,
- R) ;
-}
-
-vpCircle::~vpCircle()
+vpCircle::vpCircle(const double A, const double B, const double C, const double X0, const double Y0, const double Z0,
+ const double R)
{
+ init();
+ setWorldCoordinates(A, B, C, X0, Y0, Z0, R);
}
-
-
+vpCircle::~vpCircle() {}
/*!
Perspective projection of the circle.
- From the 3D parameters of the circle in the camera frame available in cP, computes the 2D parameters of the ellipse resulting from the perspective projection in the image plane. Those 2D parameters are available in p vector.
+ From the 3D parameters of the circle in the camera frame available in cP,
+ computes the 2D parameters of the ellipse resulting from the perspective
+ projection in the image plane. Those 2D parameters are available in p
+ vector.
- See vpCircle::projection(const vpColVector &, vpColVector &) for a more detailed description of the parameters.
+ See vpCircle::projection(const vpColVector &, vpColVector &) for a more
+ detailed description of the parameters.
*/
-void
-vpCircle::projection()
-{
- projection(cP,p) ;
-}
+void vpCircle::projection() { projection(cP, p); }
/*!
Perspective projection of the circle.
- \param cP_: 3D cercle input parameters. This vector is of dimension 7. It contains the following parameters: A, B, C, X0, Y0, Z0, r where
- - A,B,C are the parameters of the plane with equation Ax+By+Cz+D=0 containing the circle
+ \param cP_: 3D cercle input parameters. This vector is of dimension 7. It
+ contains the following parameters: A, B, C, X0, Y0, Z0, r where
+ - A,B,C are the parameters of the plane with equation Ax+By+Cz+D=0
+ containing the circle
- X0,Y0,Z0 are the 3D coordinates of the cercle in the camera frame
- r is the circle radius.
- \param p_: 2D circle output parameters. This is a 5 dimension vector. It contains the following parameters: xc, yc, m20, m11, m02 where:
- - xc,yc are the normalized coordinates of the center of the ellipse (ie the perspective projection of a 3D
- circle becomes a 2D ellipse in the image) in the image plane.
+ \param p_: 2D circle output parameters. This is a 5 dimension vector. It
+ contains the following parameters: xc, yc, m20, m11, m02 where:
+ - xc,yc are the normalized coordinates of the center of the ellipse (ie the
+ perspective projection of a 3D circle becomes a 2D ellipse in the image) in
+ the image plane.
- mu20,mu11,mu02 are the second order centered moments of the ellipse.
*/
-void
-vpCircle::projection(const vpColVector &cP_, vpColVector &p_)
+void vpCircle::projection(const vpColVector &cP_, vpColVector &p_)
{
- vpColVector K(6) ;
+ vpColVector K(6);
{
- double A = cP_[0] ;
- double B = cP_[1] ;
- double C = cP_[2] ;
+ double A = cP_[0];
+ double B = cP_[1];
+ double C = cP_[2];
- double X0 = cP_[3] ;
- double Y0 = cP_[4] ;
- double Z0 = cP_[5] ;
+ double X0 = cP_[3];
+ double Y0 = cP_[4];
+ double Z0 = cP_[5];
- double r = cP_[6];
+ double r = cP_[6];
// projection
- double s = X0*X0 + Y0*Y0 + Z0*Z0 - r*r ;
- double det = A*X0+B*Y0+C*Z0;
- A = A/det ;
- B = B/det ;
- C = C/det ;
-
- K[0] = 1 - 2*A*X0 + A*A*s;
- K[1] = 1 - 2*B*Y0 + B*B*s;
- K[2] = -A*Y0 - B*X0 + A*B*s;
- K[3] = -C*X0 - A*Z0 + A*C*s;
- K[4] = -C*Y0 - B*Z0 + B*C*s;
- K[5] = 1 - 2*C*Z0 + C*C*s;
-
+ double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
+ double det = A * X0 + B * Y0 + C * Z0;
+ A = A / det;
+ B = B / det;
+ C = C / det;
+
+ K[0] = 1 - 2 * A * X0 + A * A * s;
+ K[1] = 1 - 2 * B * Y0 + B * B * s;
+ K[2] = -A * Y0 - B * X0 + A * B * s;
+ K[3] = -C * X0 - A * Z0 + A * C * s;
+ K[4] = -C * Y0 - B * Z0 + B * C * s;
+ K[5] = 1 - 2 * C * Z0 + C * C * s;
}
-// {
-// std::cout << "K dans vpCircle::projection(): " << std::endl;
-// for (unsigned int i=1; i<6; i++)
-// std::cout << K[i]/K[0] << std::endl;
-// }
- double det = K[2]*K[2] -K[0]*K[1];
- if (fabs(det) < 1e-8)
- {
- vpERROR_TRACE("division par 0") ;
- throw(vpException(vpException::divideByZeroError,
- "division par 0")) ;
+ // {
+ // std::cout << "K dans vpCircle::projection(): " << std::endl;
+ // for (unsigned int i=1; i<6; i++)
+ // std::cout << K[i]/K[0] << std::endl;
+ // }
+ double det = K[2] * K[2] - K[0] * K[1];
+ if (fabs(det) < 1e-8) {
+ vpERROR_TRACE("division par 0");
+ throw(vpException(vpException::divideByZeroError, "division par 0"));
}
- double xc = (K[1]*K[3]-K[2]*K[4])/det;
- double yc = (K[0]*K[4]-K[2]*K[3])/det;
+ double xc = (K[1] * K[3] - K[2] * K[4]) / det;
+ double yc = (K[0] * K[4] - K[2] * K[3]) / det;
- double c = sqrt( (K[0]-K[1])*(K[0]-K[1]) + 4*K[2]*K[2] );
- double s = 2*(K[0]*xc*xc + 2*K[2]*xc*yc + K[1]*yc*yc - K[5]);
+ double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
+ double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);
- double A,B,E ;
+ double A, B, E;
- //if (fabs(K[2])<1e-6)
- if (fabs(K[2])<std::numeric_limits<double>::epsilon())
- {
+ // if (fabs(K[2])<1e-6)
+ if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
E = 0.0;
- if (K[0] > K[1])
- {
- A = sqrt(s/(K[0] + K[1] + c));
- B = sqrt(s/(K[0] + K[1] - c));
+ if (K[0] > K[1]) {
+ A = sqrt(s / (K[0] + K[1] + c));
+ B = sqrt(s / (K[0] + K[1] - c));
+ } else {
+ A = sqrt(s / (K[0] + K[1] - c));
+ B = sqrt(s / (K[0] + K[1] + c));
}
- else
- {
- A = sqrt(s/(K[0] + K[1] - c));
- B = sqrt(s/(K[0] + K[1] + c));
- }
- }
- else
- {
- E = (K[1] - K[0] + c)/(2*K[2]);
- if ( fabs(E) > 1.0)
- {
- A = sqrt(s/(K[0] + K[1] + c));
- B = sqrt(s/(K[0] + K[1] - c));
- }
- else
- {
- A = sqrt(s/(K[0] + K[1] - c));
- B = sqrt(s/(K[0] + K[1] + c));
- E = -1.0/E;
+ } else {
+ E = (K[1] - K[0] + c) / (2 * K[2]);
+ if (fabs(E) > 1.0) {
+ A = sqrt(s / (K[0] + K[1] + c));
+ B = sqrt(s / (K[0] + K[1] - c));
+ } else {
+ A = sqrt(s / (K[0] + K[1] - c));
+ B = sqrt(s / (K[0] + K[1] + c));
+ E = -1.0 / E;
}
}
- det = (1.0 + vpMath::sqr(E));
- double mu20 = (vpMath::sqr(A) + vpMath::sqr(B*E)) /det ;
- double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) *E / det ;
- double mu02 = (vpMath::sqr(B) + vpMath::sqr(A*E)) / det ;
+ det = (1.0 + vpMath::sqr(E));
+ double mu20 = (vpMath::sqr(A) + vpMath::sqr(B * E)) / det;
+ double mu11 = (vpMath::sqr(A) - vpMath::sqr(B)) * E / det;
+ double mu02 = (vpMath::sqr(B) + vpMath::sqr(A * E)) / det;
- p_[0] = xc ;
- p_[1] = yc ;
- p_[2] = mu20 ;
- p_[3] = mu11 ;
- p_[4] = mu02 ;
+ p_[0] = xc;
+ p_[1] = yc;
+ p_[2] = mu20;
+ p_[3] = mu11;
+ p_[4] = mu02;
}
//! perspective projection of the circle
-void
-vpCircle::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
{
- double A,B,C ;
- A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
- B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
- C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
+ double A, B, C;
+ A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
+ B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
+ C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
- double X0,Y0,Z0 ;
- X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
- Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
- Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
- double R = oP[6] ;
+ double X0, Y0, Z0;
+ X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
+ Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
+ Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
+ double R = oP[6];
- cP_[0] = A ;
- cP_[1] = B ;
- cP_[2] = C ;
+ cP_[0] = A;
+ cP_[1] = B;
+ cP_[2] = C;
- cP_[3] = X0 ;
- cP_[4] = Y0 ;
- cP_[5] = Z0 ;
+ cP_[3] = X0;
+ cP_[4] = Y0;
+ cP_[5] = Z0;
- cP_[6] = R ;
+ cP_[6] = R;
// vpTRACE("_cP :") ; std::cout << _cP.t() ;
-
}
//! perspective projection of the circle
-void
-vpCircle::changeFrame(const vpHomogeneousMatrix &cMo)
+void vpCircle::changeFrame(const vpHomogeneousMatrix &cMo)
{
- double A,B,C ;
- A = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2];
- B = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2];
- C = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2];
+ double A, B, C;
+ A = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2];
+ B = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2];
+ C = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2];
- double X0,Y0,Z0 ;
- X0 = cMo[0][3] + cMo[0][0]*oP[3] + cMo[0][1]*oP[4] + cMo[0][2]*oP[5];
- Y0 = cMo[1][3] + cMo[1][0]*oP[3] + cMo[1][1]*oP[4] + cMo[1][2]*oP[5];
- Z0 = cMo[2][3] + cMo[2][0]*oP[3] + cMo[2][1]*oP[4] + cMo[2][2]*oP[5];
- double R = oP[6] ;
+ double X0, Y0, Z0;
+ X0 = cMo[0][3] + cMo[0][0] * oP[3] + cMo[0][1] * oP[4] + cMo[0][2] * oP[5];
+ Y0 = cMo[1][3] + cMo[1][0] * oP[3] + cMo[1][1] * oP[4] + cMo[1][2] * oP[5];
+ Z0 = cMo[2][3] + cMo[2][0] * oP[3] + cMo[2][1] * oP[4] + cMo[2][2] * oP[5];
+ double R = oP[6];
- cP[0] = A ;
- cP[1] = B ;
- cP[2] = C ;
+ cP[0] = A;
+ cP[1] = B;
+ cP[2] = C;
- cP[3] = X0 ;
- cP[4] = Y0 ;
- cP[5] = Z0 ;
+ cP[3] = X0;
+ cP[4] = Y0;
+ cP[5] = Z0;
- cP[6] = R ;
+ cP[6] = R;
// vpTRACE("_cP :") ; std::cout << _cP.t() ;
-
}
-void vpCircle::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
+void vpCircle::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
const unsigned int thickness)
{
- vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
- cam, I, color, thickness) ;
+ vpFeatureDisplay::displayEllipse(p[0], p[1], p[2], p[3], p[4], cam, I, color, thickness);
}
// non destructive wrt. cP and p
-void vpCircle::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpCircle::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
- cam, I, color, thickness) ;
-
+ vpColVector _cP, _p;
+ changeFrame(cMo, _cP);
+ projection(_cP, _p);
+ vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
}
//! for memory issue (used by the vpServo class only)
vpCircle *vpCircle::duplicate() const
{
- vpCircle *feature = new vpCircle(*this) ;
- return feature ;
+ vpCircle *feature = new vpCircle(*this);
+ return feature;
}
/*!
Computes the coordinates of the point corresponding to the intersection
between a circle and a line.
- \warning This functions assumes changeFrame() and projection() have already been called.
+ \warning This functions assumes changeFrame() and projection() have already
+ been called.
\sa changeFrame(), projection()
\param circle : Circle to consider for the intersection.
- \param cam : Camera parameters that have to be used for the intersection computation.
- \param rho : The rho parameter of the line.
- \param theta : The theta parameter of the line.
- \param i : resulting i-coordinate of the intersection point.
- \param j : resulting j-coordinate of the intersection point.
+ \param cam : Camera parameters that have to be used for the intersection
+ computation. \param rho : The rho parameter of the line. \param theta : The
+ theta parameter of the line. \param i : resulting i-coordinate of the
+ intersection point. \param j : resulting j-coordinate of the intersection
+ point.
*/
-void
-vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho, const double &theta, double &i, double &j)
+void vpCircle::computeIntersectionPoint(const vpCircle &circle, const vpCameraParameters &cam, const double &rho,
+ const double &theta, double &i, double &j)
{
- // This was taken from the code of art-v1. (from the artCylinder class)
- double px = cam.get_px() ;
- double py = cam.get_py() ;
- double u0 = cam.get_u0() ;
- double v0 = cam.get_v0() ;
-
- double mu11 = circle.p[3];
- double mu02 = circle.p[4];
- double mu20 = circle.p[2];
- double Xg = u0 + circle.p[0]*px;
- double Yg = v0 + circle.p[1]*py;
-
- // Find Intersection between line and ellipse in the image.
-
- // Optimised calculation for X
- double stheta = sin(theta);
- double ctheta = cos(theta);
- double sctheta = stheta*ctheta;
- double m11yg = mu11*Yg;
- double ctheta2 = vpMath::sqr(ctheta);
- double m02xg = mu02*Xg;
- double m11stheta = mu11*stheta;
- j = ((mu11*Xg*sctheta-mu20*Yg*sctheta+mu20*rho*ctheta
- -m11yg+m11yg*ctheta2+m02xg-m02xg*ctheta2+
- m11stheta*rho)/(mu20*ctheta2+2.0*m11stheta*ctheta
- +mu02-mu02*ctheta2));
- //Optimised calculation for Y
- double rhom02 = rho*mu02;
- double sctheta2 = stheta*ctheta2;
- double ctheta3 = ctheta2*ctheta;
- i = (-(-rho*mu11*stheta*ctheta-rhom02+rhom02*ctheta2
- +mu11*Xg*sctheta2-mu20*Yg*sctheta2-ctheta*mu11*Yg
- +ctheta3*mu11*Yg+ctheta*mu02*Xg-ctheta3*mu02*Xg)/
- (mu20*ctheta2+2.0*mu11*stheta*ctheta+mu02-
- mu02*ctheta2)/stheta);
+ // This was taken from the code of art-v1. (from the artCylinder class)
+ double px = cam.get_px();
+ double py = cam.get_py();
+ double u0 = cam.get_u0();
+ double v0 = cam.get_v0();
+
+ double mu11 = circle.p[3];
+ double mu02 = circle.p[4];
+ double mu20 = circle.p[2];
+ double Xg = u0 + circle.p[0] * px;
+ double Yg = v0 + circle.p[1] * py;
+
+ // Find Intersection between line and ellipse in the image.
+
+ // Optimised calculation for X
+ double stheta = sin(theta);
+ double ctheta = cos(theta);
+ double sctheta = stheta * ctheta;
+ double m11yg = mu11 * Yg;
+ double ctheta2 = vpMath::sqr(ctheta);
+ double m02xg = mu02 * Xg;
+ double m11stheta = mu11 * stheta;
+ j = ((mu11 * Xg * sctheta - mu20 * Yg * sctheta + mu20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
+ m02xg * ctheta2 + m11stheta * rho) /
+ (mu20 * ctheta2 + 2.0 * m11stheta * ctheta + mu02 - mu02 * ctheta2));
+ // Optimised calculation for Y
+ double rhom02 = rho * mu02;
+ double sctheta2 = stheta * ctheta2;
+ double ctheta3 = ctheta2 * ctheta;
+ i = (-(-rho * mu11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + mu11 * Xg * sctheta2 - mu20 * Yg * sctheta2 -
+ ctheta * mu11 * Yg + ctheta3 * mu11 * Yg + ctheta * mu02 * Xg - ctheta3 * mu02 * Xg) /
+ (mu20 * ctheta2 + 2.0 * mu11 * stheta * ctheta + mu02 - mu02 * ctheta2) / stheta);
}
diff --git a/modules/core/src/tracking/forward-projection/vpCylinder.cpp b/modules/core/src/tracking/forward-projection/vpCylinder.cpp
index 4a58a8a..851cb83 100644
--- a/modules/core/src/tracking/forward-projection/vpCylinder.cpp
+++ b/modules/core/src/tracking/forward-projection/vpCylinder.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,24 +36,22 @@
*
*****************************************************************************/
-
#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpFeatureDisplay.h>
-
-void
-vpCylinder::init()
+void vpCylinder::init()
{
- oP.resize(7) ;
- cP.resize(7) ;
+ oP.resize(7);
+ cP.resize(7);
- p.resize(4) ;
+ p.resize(4);
}
/*!
- Set the cylinder parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
+ Set the cylinder parameters \f$^{o}{\bf P} =
+ ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$ expressed in the world
+ frame.
\param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
@@ -69,47 +68,37 @@ 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} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
+ Set the cylinder parameters \f$^{o}{\bf P} =
+ ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$ expressed in the world
+ frame.
\param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
*/
-void
-vpCylinder::setWorldCoordinates(const double A, const double B,
- const double C,
- const double X0, const double Y0,
- const double Z0,
- const double R)
+void vpCylinder::setWorldCoordinates(const double A, const double B, const double C, const double X0, const double Y0,
+ const double Z0, const double R)
{
- oP[0] = A ;
- oP[1] = B ;
- oP[2] = C ;
- oP[3] = X0 ;
- oP[4] = Y0 ;
- oP[5] = Z0 ;
- oP[6] = R ;
+ oP[0] = A;
+ oP[1] = B;
+ oP[2] = C;
+ oP[3] = X0;
+ oP[4] = Y0;
+ oP[5] = Z0;
+ oP[6] = R;
}
-
/*!
Default constructor.
*/
-vpCylinder::vpCylinder()
-{
- init() ;
-}
+vpCylinder::vpCylinder() { init(); }
/*!
- Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
+ Create and initialize a cylinder with parameters \f$^{o}{\bf P} =
+ ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$ expressed in the world
+ frame.
\param o_P : Vector of parameters \f$^{o}{\bf P}\f$.
@@ -127,45 +116,39 @@ vpCylinder::vpCylinder()
\endcode
\sa setWorldCoordinates(const vpColVector&)
*/
-vpCylinder::vpCylinder(const vpColVector& o_P)
+vpCylinder::vpCylinder(const vpColVector &o_P)
{
- init() ;
- setWorldCoordinates(o_P) ;
+ init();
+ setWorldCoordinates(o_P);
}
/*!
- Create and initialize a cylinder with parameters \f$^{o}{\bf P} = ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$
- expressed in the world frame.
+ Create and initialize a cylinder with parameters \f$^{o}{\bf P} =
+ ({^o}A,{^o}B,{^o}C,{^o}X_0,{^o}Y_0,{^o}Z_0,R)\f$ expressed in the world
+ frame.
\param A,B,C,X0,Y0,Z0,R : Cylinder parameters \f$^{o}{\bf P}\f$.
- \sa setWorldCoordinates(const double,const double,const double,const double,const double,const double,const double)
+ \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) ;
+ init();
+ setWorldCoordinates(A, B, C, X0, Y0, Z0, R);
}
/*!
Default constructor.
*/
-vpCylinder::~vpCylinder()
-{
-}
-
+vpCylinder::~vpCylinder() {}
/*!
Perspective projection of the cylinder.
- From the parameters of the cylinder in the camera frame, compute the perspective
- projection of the cylinder in the image plane.
+ From the parameters of the cylinder in the camera frame, compute the
+ perspective projection of the cylinder in the image plane.
\code
vpCylinder cylinder;
@@ -181,21 +164,17 @@ vpCylinder::~vpCylinder()
\sa projection(const vpColVector &, vpColVector &)
*/
-void
-vpCylinder::projection()
-{
- projection(cP,p) ;
-}
-
+void vpCylinder::projection() { projection(cP, p); }
/*!
Perspective projection of the cylinder.
- From the parameters of the cylinder in the camera frame \f$c{\bf P}\f$, compute the perspective
- projection of the cylinder in the image plane.
+ From the parameters of the cylinder in the camera frame \f$c{\bf P}\f$,
+ compute the perspective projection of the cylinder in the image plane.
\param cP_ [in] : Cylinder parameters in the camera frame.
- \param p_ [out] : Parameters of the cylinder in the image plane obtained by perspective projection.
+ \param p_ [out] : Parameters of the cylinder in the image plane obtained by
+ perspective projection.
\exception vpException::fatalError : The camera is inside the cylinder.
@@ -206,64 +185,61 @@ vpCylinder::projection()
cylinder.setWorldCoordinates(oP); // Set the cylinder world frame parameters
vpHomogeneousMatrix cMo; // Camera to world frame transformation
- vpColVector cP(7); // Parameters of the cylinder in the camera frame
- cylinder.changeFrame(cMo, cP); // Update cP parameters
+ vpColVector cP(7); // Parameters of the cylinder in the
+ camera frame cylinder.changeFrame(cMo, cP); // Update cP parameters
- vpColVector p(4); // Parameters of the cylinder in the image plane
- cylinder.projection(cP, p); // Compute the perspective projection and update p
- \endcode
+ vpColVector p(4); // Parameters of the cylinder in the image
+ plane cylinder.projection(cP, p); // Compute the perspective
+ projection and update p \endcode
\sa projection()
*/
-void
-vpCylinder::projection(const vpColVector &cP_, vpColVector &p_)
+void vpCylinder::projection(const vpColVector &cP_, vpColVector &p_)
{
- //calcul de la scene 2-D
+ // calcul de la scene 2-D
double co, si, e, x0, y0, z0;
- double A,B,C, X0, Y0, Z0, R ;
+ double A, B, C, X0, Y0, Z0, R;
double s, a, b, c, zero;
- A = cP_[0] ;
- B = cP_[1] ;
- C = cP_[2] ;
- X0 = cP_[3] ;
- Y0 = cP_[4] ;
- Z0 = cP_[5] ;
- R= cP_[6] ;
- zero = A*X0 + B*Y0 + C*Z0; // should be zero for a good reprensetation of the cylinder
-
- s = X0*X0 + Y0*Y0 + Z0*Z0 - R*R - zero*zero;
- if (s < 0)
- {
- printf("The camera is inside the cylinder with s=%f !\n", s);
- throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
- }
- s = 1.0/sqrt(s);
- a = X0 - A*zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
- b = Y0 - B*zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
- c = Z0 - C*zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
- x0 = C*Y0 - B*Z0;
- y0 = A*Z0 - C*X0;
- z0 = B*X0 - A*Y0;
+ A = cP_[0];
+ B = cP_[1];
+ C = cP_[2];
+ X0 = cP_[3];
+ Y0 = cP_[4];
+ Z0 = cP_[5];
+ R = cP_[6];
+ zero = A * X0 + B * Y0 + C * Z0; // should be zero for a good reprensetation of the cylinder
+
+ s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
+ if (s < 0) {
+ printf("The camera is inside the cylinder with s=%f !\n", s);
+ throw vpException(vpException::fatalError, "The camera is inside the cylinder!");
+ }
+ s = 1.0 / sqrt(s);
+ a = X0 - A * zero; //(1-A*A)*X0 - A*B*Y0 - A*C*Z0;
+ b = Y0 - B * zero; // - A*B*X0 + (1-B*B)*Y0 - B*C*Z0;
+ c = Z0 - C * zero; //- A*C*X0 - B*C*Y0 + (1-C*C)*Z0;
+ x0 = C * Y0 - B * Z0;
+ y0 = A * Z0 - C * X0;
+ z0 = B * X0 - A * Y0;
// rho1 / theta1
- co = R*a*s-x0;
- si = R*b*s-y0;
- e = sqrt(co*co + si*si);
- p_[0] = -(R*c*s-z0)/e ; // rho1
- p_[1] = atan2(si,co) ; // theta 1
+ co = R * a * s - x0;
+ si = R * b * s - y0;
+ e = sqrt(co * co + si * si);
+ p_[0] = -(R * c * s - z0) / e; // rho1
+ p_[1] = atan2(si, co); // theta 1
// while (p[1] > M_PI/2) { p[1] -= M_PI ; p[0] *= -1 ; }
// while (p[1] < -M_PI/2) { p[1] += M_PI ; p[0] *= -1 ; }
// rho2 / theta2
- co = R*a*s+x0;
- si = R*b*s+y0;
- e = sqrt(co*co + si*si);
- p_[2] = -( R*c*s+z0 )/e ; //rho2
- p_[3] = atan2( si,co ) ; //theta2
-
+ co = R * a * s + x0;
+ si = R * b * s + y0;
+ e = sqrt(co * co + si * si);
+ p_[2] = -(R * c * s + z0) / e; // rho2
+ p_[3] = atan2(si, co); // theta2
// while (p[3] > M_PI/2) { p[3] -= M_PI ; p[2] *= -1 ; }
// while (p[3] < -M_PI/2) { p[3] += M_PI ; p[2] *= -1 ; }
@@ -272,65 +248,63 @@ vpCylinder::projection(const vpColVector &cP_, vpColVector &p_)
}
/*!
- From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
- the cylinder internal parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+ From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame,
+ compute the cylinder internal parameters \f$^{c}{\bf P}\f$ expressed in the
+ camera frame.
\param cMo : Camera to world frame transformation.
\sa changeFrame(const vpHomogeneousMatrix &, vpColVector &)
*/
-void
-vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo,cP) ;
-}
+void vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); }
/*!
- From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame, compute
- the cylinder parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+ From the cylinder parameters \f$^{o}{\bf P}\f$ expressed in the world frame,
+ compute the cylinder parameters \f$^{c}{\bf P}\f$ expressed in the camera
+ frame.
\param cMo : Camera to world frame transformation.
- \param cP_ [out] : Parameters \f$^{c}{\bf P}\f$ expressed in the camera frame.
+ \param cP_ [out] : Parameters \f$^{c}{\bf P}\f$ expressed in the camera
+ frame.
\sa changeFrame(const vpHomogeneousMatrix &)
*/
-void
-vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+void vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
{
double X1, Y1, Z1;
double X2, Y2, Z2;
double s, a, b, c;
- double oA,oB,oC, oX0, oY0, oZ0 ;
- oA = oP[0] ;
- oB = oP[1] ;
- oC = oP[2] ;
- oX0 = oP[3] ;
- oY0 = oP[4] ;
- oZ0 = oP[5] ;
-
- X1 = cMo[0][0]*oA + cMo[0][1]*oB + cMo[0][2]*oC ;
- Y1 = cMo[1][0]*oA + cMo[1][1]*oB + cMo[1][2]*oC ;
- Z1 = cMo[2][0]*oA + cMo[2][1]*oB + cMo[2][2]*oC ;
- s = sqrt ( X1*X1 + Y1*Y1 + Z1*Z1 );
+ double oA, oB, oC, oX0, oY0, oZ0;
+ oA = oP[0];
+ oB = oP[1];
+ oC = oP[2];
+ oX0 = oP[3];
+ oY0 = oP[4];
+ oZ0 = oP[5];
+
+ X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
+ Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
+ Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
+ s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
a = X1 / s;
b = Y1 / s;
c = Z1 / s;
// set axis coordinates in camera frame
- cP_[0] = a ;
- cP_[1] = b ;
- cP_[2] = c ;
+ cP_[0] = a;
+ cP_[1] = b;
+ cP_[2] = c;
- X2 = cMo[0][3] + cMo[0][0]*oX0 + cMo[0][1]*oY0 + cMo[0][2]*oZ0;
- Y2 = cMo[1][3] + cMo[1][0]*oX0 + cMo[1][1]*oY0 + cMo[1][2]*oZ0;
- Z2 = cMo[2][3] + cMo[2][0]*oX0 + cMo[2][1]*oY0 + cMo[2][2]*oZ0;
+ X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
+ Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
+ Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
- // adding the constraint X0 is the nearest point to the origin (A^T . X0 = 0)
- // using the projection operator (I - AA^T) orthogonal to A
- cP_[3] = (1-a*a)*X2 - a*b*Y2 - a*c*Z2;
- cP_[4] = -a*b*X2 + (1-b*b)*Y2 - b*c*Z2;
- cP_[5] = -a*c*X2 - b*c*Y2 + (1-c*c)*Z2;
+ // adding the constraint X0 is the nearest point to the origin (A^T . X0 =
+ // 0) using the projection operator (I - AA^T) orthogonal to A
+ cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
+ cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
+ cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
/* old version for the same onstraint
if ( fabs(a) > 0.25 )
@@ -380,20 +354,19 @@ vpCylinder::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
_cP[5] = zz ;
}
*/
- //radius
- cP_[6] = oP[6] ;
-
+ // radius
+ cP_[6] = oP[6];
}
/*!
- Compute the Z coordinate for the given normalized coordinate in the camera frame.
+ Compute the Z coordinate for the given normalized coordinate in the camera
+ frame.
*/
-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 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 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;
}
@@ -401,38 +374,28 @@ double vpCylinder::computeZ(const double x, const double y) const {
//! for memory issue (used by the vpServo class only)
vpCylinder *vpCylinder::duplicate() const
{
- vpCylinder *feature = new vpCylinder(*this) ;
- return feature ;
+ vpCylinder *feature = new vpCylinder(*this);
+ return feature;
}
/*!
Display the projection of the cylinder in the image as two lines.
*/
-void
-vpCylinder::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpCylinder::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP(7), _p(4) ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayCylinder(_p[0],_p[1], _p[2], _p[3],
- cam, I, color, thickness) ;
-
+ vpColVector _cP(7), _p(4);
+ changeFrame(cMo, _cP);
+ projection(_cP, _p);
+ vpFeatureDisplay::displayCylinder(_p[0], _p[1], _p[2], _p[3], cam, I, color, thickness);
}
/*!
Display the projection of the cylinder in the image as two lines.
*/
-void
-vpCylinder::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpCylinder::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
+ const unsigned int thickness)
{
- vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3],
- cam, I, color, thickness) ;
+ vpFeatureDisplay::displayCylinder(p[0], p[1], p[2], p[3], cam, I, color, thickness);
}
diff --git a/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp
index 5a290df..c0eb3dd 100644
--- a/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp
+++ b/modules/core/src/tracking/forward-projection/vpForwardProjection.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,32 +36,25 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpForwardProjection.h>
#include <visp3/core/vpDebug.h>
-
+#include <visp3/core/vpForwardProjection.h>
/*!
\file vpForwardProjection.cpp
\brief class that defines what is a point
*/
-
-
-
-
/*!
Print to stdout the feature parameters in:
- the object frame
- the camera frame
- the image plane.
*/
-void
-vpForwardProjection::print() const
+void vpForwardProjection::print() const
{
- std::cout << "oP : " << oP.t() ;
- std::cout << "cP : " << cP.t() ;
- std::cout << "p : " << p.t() ;
+ std::cout << "oP : " << oP.t();
+ std::cout << "cP : " << cP.t();
+ std::cout << "p : " << p.t();
}
/*!
@@ -71,14 +65,10 @@ vpForwardProjection::print() const
\warning Be careful to update vpTracker::p and vpTracker::cP public
attributes before the call of this method.
*/
-void
-vpForwardProjection::project()
-{
- projection(cP, p) ;
-}
+void vpForwardProjection::project() { projection(cP, p); }
+
+/*!
-/*!
-
Compute the feature parameters in the camera frame (vpTracker::cP)
and than compute the projection of these parameters in the image
plane (vpTracker::p).
@@ -86,28 +76,24 @@ vpForwardProjection::project()
\warning The feature parameters in the object frame
(vpForwardProjection:oP) need to be set prior the use of this
method. To initialize these parameters see setWorldCoordinates().
-
+
\param cMo : The homogeneous matrix corresponding to the pose
between the camera frame and the object frame.
*/
-void
-vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
+void vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
{
- try{
- changeFrame(cMo) ;
- projection() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ changeFrame(cMo);
+ projection();
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
+/*!
-/*!
-
Track the feature parameters in the camera frame (vpTracker::cP)
and than compute the projection of these parameters in the image
plane (vpTracker::p).
@@ -117,21 +103,18 @@ vpForwardProjection::project(const vpHomogeneousMatrix &cMo)
\warning The feature parameters in the object frame
(vpForwardProjection:oP) need to be set prior the use of this
method. To initialize these parameters see setWorldCoordinates().
-
+
\param cMo : The homogeneous matrix corresponding to the pose
between the camera frame and the object frame.
*/
-void
-vpForwardProjection::track(const vpHomogeneousMatrix &cMo)
+void vpForwardProjection::track(const vpHomogeneousMatrix &cMo)
{
- try{
- project(cMo) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ project(cMo);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
diff --git a/modules/core/src/tracking/forward-projection/vpLine.cpp b/modules/core/src/tracking/forward-projection/vpLine.cpp
index 6db0086..b80cde2 100644
--- a/modules/core/src/tracking/forward-projection/vpLine.cpp
+++ b/modules/core/src/tracking/forward-projection/vpLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#include <visp3/core/vpLine.h>
#include <visp3/core/vpDebug.h>
@@ -48,34 +48,26 @@
\brief class that defines what is a line
*/
-
-
/*!
Initialize the memory space requested for the 2D line parameters (\e
p) in the image plane and for 3D line parameters (\e oP and \e cP)
respectively in the object frame and the camera frame.
*/
-void
-vpLine::init()
+void vpLine::init()
{
- oP.resize(8) ;
- cP.resize(8) ;
- p.resize(2) ;
+ oP.resize(8);
+ cP.resize(8);
+ p.resize(2);
}
-/*!
+/*!
Default constructor.
*/
-vpLine::vpLine()
-{
- init() ;
-}
-
-
+vpLine::vpLine() { init(); }
/*!
-
+
Sets the parameters \e oP which define the line in the object
frame. As said in the class description, the line is defined as the
intersection of two planes. The different parameters here define the
@@ -90,25 +82,21 @@ vpLine::vpLine()
\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 ;
- oP[2] = C1 ;
- oP[3] = D1 ;
-
- oP[4] = A2 ;
- oP[5] = B2 ;
- oP[6] = C2 ;
- oP[7] = D2 ;
+ oP[0] = A1;
+ oP[1] = B1;
+ oP[2] = C1;
+ oP[3] = D1;
+
+ oP[4] = A2;
+ oP[5] = B2;
+ oP[6] = C2;
+ oP[7] = D2;
}
-
-/*!
+/*!
Sets the parameters \e oP which define the line in the object frame. As
said in the class description, the line is defined as the
@@ -126,17 +114,15 @@ vpLine::setWorldCoordinates(const double &A1, const double &B1,
B2 \\ C2 \\ D2 \end{array}\right] \f]
*/
-void
-vpLine::setWorldCoordinates(const vpColVector &oP_)
+void vpLine::setWorldCoordinates(const vpColVector &oP_)
{
if (oP_.getRows() != 8)
throw vpException(vpException::dimensionError, "Size of oP is not equal to 8 as it should be");
- this->oP = oP_ ;
+ this->oP = oP_;
}
-
-/*!
+/*!
Sets the parameters \e oP which define the line in the object frame. As
said in the class description, the line is defined as the
@@ -151,31 +137,26 @@ vpLine::setWorldCoordinates(const vpColVector &oP_)
\param oP1 : The column vector which contains the four parameters
needed to define the equations of the first plane. \f[ oP1 =
\left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1 \end{array}\right] \f]
-
+
\param oP2 : The column vector which contains the four parameters
needed to define the equations of the second plane. \f[ oP2 =
\left[\begin{array}{c} A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
*/
-void
-vpLine::setWorldCoordinates(const vpColVector &oP1,
- const vpColVector &oP2)
+void vpLine::setWorldCoordinates(const vpColVector &oP1, const vpColVector &oP2)
{
- if (oP1.getRows() != 4)
- throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
+ if (oP1.getRows() != 4)
+ throw vpException(vpException::dimensionError, "Size of oP1 is not equal to 4 as it should be");
- if (oP2.getRows() != 4)
- throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
+ if (oP2.getRows() != 4)
+ throw vpException(vpException::dimensionError, "Size of oP2 is not equal to 4 as it should be");
- for (unsigned int i=0 ; i < 4 ; i++)
- {
- oP[i] = oP1[i] ;
- oP[i+4] = oP2[i] ;
+ for (unsigned int i = 0; i < 4; i++) {
+ oP[i] = oP1[i];
+ oP[i + 4] = oP2[i];
}
-
}
-
/*!
Computes the 2D parameters \e p of the line in the image plane thanks to
@@ -195,7 +176,8 @@ vpLine::setWorldCoordinates(const vpColVector &oP1,
//Set the coordinates of the line in the object frame in meter.
line.setWorldCoordinates( 1, 0, 0, -0.5, 0, 0, 1, 0.5)
- //Here the line is define by the intersection between the plane X = 0.5m and Z = 0.5m
+ //Here the line is define by the intersection between the plane X = 0.5m and
+ Z = 0.5m
//Create the homogeneous matrix
vpHomogeneousMatrix cMo;
@@ -208,12 +190,7 @@ vpLine::setWorldCoordinates(const vpColVector &oP1,
line.projection();
\endcode
*/
-void
-vpLine::projection()
-{
- projection(cP,p) ;
-}
-
+void vpLine::projection() { projection(cP, p); }
/*!
@@ -225,56 +202,55 @@ vpLine::projection()
\param cP_ : The vector containing the line features relative to the
camera frame. \f[ cP = \left[\begin{array}{c}A1 \\ B1 \\ C1 \\ D1
\\ A2 \\ B2 \\ C2 \\ D2 \end{array}\right] \f]
-
+
\param p_ : The vector which contains the 2D line features expressed
in the image plane. \f[ p = \left[\begin{array}{c} \rho \\ \theta
\end{array}\right] \f]
- \exception vpException::fatalError : Degenerate case, the image of the straight line is a point.
+ \exception vpException::fatalError : Degenerate case, the image of the
+ straight line is a point.
*/
-void
-vpLine::projection(const vpColVector &cP_, vpColVector &p_)
+void vpLine::projection(const vpColVector &cP_, vpColVector &p_)
{
- //projection
+ // projection
if (cP.getRows() != 8)
throw vpException(vpException::dimensionError, "Size of cP is not equal to 8 as it should be");
double A1, A2, B1, B2, C1, C2, D1, D2;
- A1=cP_[0] ;
- B1=cP_[1] ;
- C1=cP_[2] ;
- D1=cP_[3] ;
+ A1 = cP_[0];
+ B1 = cP_[1];
+ C1 = cP_[2];
+ D1 = cP_[3];
- A2=cP_[4] ;
- B2=cP_[5] ;
- C2=cP_[6] ;
- D2=cP_[7] ;
+ A2 = cP_[4];
+ B2 = cP_[5];
+ C2 = cP_[6];
+ D2 = cP_[7];
double a, b, c, s;
- a = A2*D1 - A1*D2;
- b = B2*D1 - B1*D2;
- c = C2*D1 - C1*D2;
- s = a*a+b*b;
+ a = A2 * D1 - A1 * D2;
+ b = B2 * D1 - B1 * D2;
+ c = C2 * D1 - C1 * D2;
+ s = a * a + b * b;
if (s <= 1e-8) // seuil pas terrible
- {
- printf("Degenerate case: the image of the straight line is a point!\n");
- throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
- }
- s = 1.0/sqrt(s);
+ {
+ printf("Degenerate case: the image of the straight line is a point!\n");
+ throw vpException(vpException::fatalError, "Degenerate case: the image of the straight line is a point!");
+ }
+ s = 1.0 / sqrt(s);
- double rho = -c*s ;
- double theta = atan2( b, a);
+ double rho = -c * s;
+ double theta = atan2(b, a);
if (p.getRows() != 2)
p.resize(2);
- p_[0] = rho ;
- p_[1] = theta ;
+ p_[0] = rho;
+ p_[1] = theta;
}
-
/*!
Computes the line parameters \e cP in the camera frame thanks to the
@@ -289,7 +265,7 @@ vpLine::projection(const vpColVector &cP_, vpColVector &p_)
As a result of this method, line parameters \e cP in the camera
frame are updated in the vpTracker::cP public attribute.
-
+
\param cMo : The homogeneous matrix corresponding to the pose
between the camera frame and the object frame.
@@ -300,7 +276,8 @@ vpLine::projection(const vpColVector &cP_, vpColVector &p_)
//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;
@@ -310,12 +287,7 @@ 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); }
/*!
@@ -358,8 +330,7 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo)
\endcode
*/
-void
-vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+void vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
{
double a1, a2, b1, b2, c1, c2, d1, d2;
@@ -371,25 +342,25 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
if (cP.getRows() != 8)
cP.resize(8);
- a1=oP[0] ;
- b1=oP[1] ;
- c1=oP[2] ;
- d1=oP[3] ;
+ a1 = oP[0];
+ b1 = oP[1];
+ c1 = oP[2];
+ d1 = oP[3];
- a2=oP[4] ;
- b2=oP[5] ;
- c2=oP[6] ;
- d2=oP[7] ;
+ a2 = oP[4];
+ b2 = oP[5];
+ c2 = oP[6];
+ d2 = oP[7];
- A1 = cMo[0][0]*a1 + cMo[0][1]*b1 + cMo[0][2]*c1;
- B1 = cMo[1][0]*a1 + cMo[1][1]*b1 + cMo[1][2]*c1;
- C1 = cMo[2][0]*a1 + cMo[2][1]*b1 + cMo[2][2]*c1;
- D1 = d1 - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
+ A1 = cMo[0][0] * a1 + cMo[0][1] * b1 + cMo[0][2] * c1;
+ B1 = cMo[1][0] * a1 + cMo[1][1] * b1 + cMo[1][2] * c1;
+ C1 = cMo[2][0] * a1 + cMo[2][1] * b1 + cMo[2][2] * c1;
+ D1 = d1 - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
- A2 = cMo[0][0]*a2 + cMo[0][1]*b2 + cMo[0][2]*c2;
- B2 = cMo[1][0]*a2 + cMo[1][1]*b2 + cMo[1][2]*c2;
- C2 = cMo[2][0]*a2 + cMo[2][1]*b2 + cMo[2][2]*c2;
- D2 = d2 - (cMo[0][3]*A2 + cMo[1][3]*B2 + cMo[2][3]*C2);
+ A2 = cMo[0][0] * a2 + cMo[0][1] * b2 + cMo[0][2] * c2;
+ B2 = cMo[1][0] * a2 + cMo[1][1] * b2 + cMo[1][2] * c2;
+ C2 = cMo[2][0] * a2 + cMo[2][1] * b2 + cMo[2][2] * c2;
+ D2 = d2 - (cMo[0][3] * A2 + cMo[1][3] * B2 + cMo[2][3] * C2);
// in case of verification
// ap1 = A1; bp1 = B1; cp1 = C1; dp1 = D1;
@@ -401,16 +372,16 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
// Adding constraints on the straight line to have a unique representation
// direction of the straight line = N1 x N2
- a2 = B1*C2 - C1*B2;
- b2 = C1*A2 - A1*C2;
- c2 = A1*B2 - B1*A2;
+ a2 = B1 * C2 - C1 * B2;
+ b2 = C1 * A2 - A1 * C2;
+ c2 = A1 * B2 - B1 * A2;
// Constraint D1 = 0 (the origin belongs to P1)
- a1 = A2*D1 - A1*D2;
- b1 = B2*D1 - B1*D2;
- c1 = C2*D1 - C1*D2;
+ a1 = A2 * D1 - A1 * D2;
+ b1 = B2 * D1 - B1 * D2;
+ c1 = C2 * D1 - C1 * D2;
- if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
+ if (fabs(D2) < fabs(D1)) // to be sure that D2 <> 0
{
A2 = A1;
B2 = B1;
@@ -419,33 +390,32 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
}
// Constraint A1^2 + B1^2 + C1^2 = 1
- d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
- cP_[0] = A1 = a1*d1 ;
- cP_[1] = B1 = b1*d1 ;
- cP_[2] = C1 = c1*d1 ;
- cP_[3] = 0 ;
+ d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
+ cP_[0] = A1 = a1 * d1;
+ cP_[1] = B1 = b1 * d1;
+ cP_[2] = C1 = c1 * d1;
+ cP_[3] = 0;
// Constraint A1 A2 + B1 B2 + C1 C2 = 0 (P2 orthogonal to P1)
// N2_new = (N1 x N2) x N1_new
- a1 = b2*C1 - c2*B1;
- b1 = c2*A1 - a2*C1;
- c1 = a2*B1 - b2*A1;
+ a1 = b2 * C1 - c2 * B1;
+ b1 = c2 * A1 - a2 * C1;
+ c1 = a2 * B1 - b2 * A1;
// Constraint A2^2 + B2^2 + C2^2 = 1
- d1 = 1.0/sqrt(a1*a1 + b1*b1 + c1*c1);
- a1 *= d1 ;
- b1 *= d1 ;
- c1 *= d1 ;
+ d1 = 1.0 / sqrt(a1 * a1 + b1 * b1 + c1 * c1);
+ a1 *= d1;
+ b1 *= d1;
+ c1 *= d1;
// D2_new = D2 / (N2^T . N2_new)
- D2 /= (A2*a1 + B2*b1 + C2*c1);
+ D2 /= (A2 * a1 + B2 * b1 + C2 * c1);
A2 = a1;
B2 = b1;
C2 = c1;
// Constraint D2 < 0
- if (D2 > 0)
- {
+ if (D2 > 0) {
A2 = -A2;
B2 = -B2;
C2 = -C2;
@@ -454,13 +424,13 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
// vpERROR_TRACE("A1 B1 C1 D1 %f %f %f %f ", A1, B1, C1, D1) ;
// vpERROR_TRACE("A2 B2 C2 D2 %f %f %f %f ", A2, B2, C2, D2) ;
- cP_[4] = A2;
- cP_[5] = B2;
- cP_[6] = C2;
- cP_[7] = D2;
+ cP_[4] = A2;
+ cP_[5] = B2;
+ cP_[6] = C2;
+ cP_[7] = D2;
// in case of verification
- /*
+ /*
x = -A2*D2;
y = -B2*D2;
z = -C2*D2;
@@ -472,18 +442,15 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
exit(-1);
}
d1 = A1*x+B1*y+C1*z+D1;
- d2 = A2*x+B2*y+C2*z+D2;
+ d2 = A2*x+B2*y+C2*z+D2;
if ((fabs(d1) > 1e-8) || (fabs(d2) > 1e-8))
{
printf("PB in VPline: Pn1 : 0 = %lf, Pn2: 0 = %lf\n",d1,d2);
exit(-1);
}
*/
-
}
-
-
/*!
Displays the line in the image \e I thanks to the 2D parameters of
@@ -500,15 +467,12 @@ vpLine::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
\param thickness : Thickness of the feature representation.
*/
-void vpLine::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpLine::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
+ const unsigned int thickness)
{
- vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness) ;
+ vpFeatureDisplay::displayLine(p[0], p[1], cam, I, color, thickness);
}
-
/*!
Displays the line in the image \e I thanks to the parameters in the
@@ -530,21 +494,15 @@ void vpLine::display(const vpImage<unsigned char> &I,
\param thickness : Thickness of the feature representation.
*/
// non destructive wrt. cP and p
-void vpLine::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayLine(_p[0],_p[1],
- cam, I, color, thickness) ;
-
+ vpColVector _cP, _p;
+ changeFrame(cMo, _cP);
+ projection(_cP, _p);
+ vpFeatureDisplay::displayLine(_p[0], _p[1], cam, I, color, thickness);
}
-
/*!
Create an object with the same type.
@@ -557,8 +515,8 @@ void vpLine::display(const vpImage<unsigned char> &I,
*/
vpLine *vpLine::duplicate() const
{
- vpLine *feature = new vpLine(*this) ;
- return feature ;
+ vpLine *feature = new vpLine(*this);
+ return feature;
}
/*
diff --git a/modules/core/src/tracking/forward-projection/vpPoint.cpp b/modules/core/src/tracking/forward-projection/vpPoint.cpp
index 0ba8bc0..137c8cf 100644
--- a/modules/core/src/tracking/forward-projection/vpPoint.cpp
+++ b/modules/core/src/tracking/forward-projection/vpPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,33 +36,32 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpFeatureDisplay.h>
+#include <visp3/core/vpPoint.h>
/*!
\file vpPoint.cpp
\brief class that defines what is a point
*/
-
-
-void
-vpPoint::init()
+void vpPoint::init()
{
- p.resize(3) ; p = 0 ; p[2] = 1 ;
- oP.resize(4) ; oP = 0 ; oP[3] = 1 ;
- cP.resize(4) ; cP = 0 ; cP[3] = 1 ;
+ p.resize(3);
+ p = 0;
+ p[2] = 1;
+ oP.resize(4);
+ oP = 0;
+ oP[3] = 1;
+ cP.resize(4);
+ cP = 0;
+ cP[3] = 1;
- //default value Z (1 meters)
- set_Z(1) ;
+ // default value Z (1 meters)
+ set_Z(1);
}
-vpPoint::vpPoint()
-{
- init();
-}
+vpPoint::vpPoint() { init(); }
/*!
Construction from 3D coordinates in the object frame.
@@ -74,13 +74,14 @@ vpPoint::vpPoint(double oX, double oY, double oZ)
}
/*!
- Construction from 3D point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
- of dimension 3 or 4.
- - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
- where P[0]=X, P[1]=Y, P[2]=Z.
- - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
- where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+ Construction from 3D point world coordinates. We mean here the coordinates
+ of the point in the object frame. \param P: Vector containing the
+ coordinates of the 3D point in the object frame. This vector could be of
+ dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, 1) where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, W) where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
*/
vpPoint::vpPoint(const vpColVector &P)
{
@@ -89,13 +90,14 @@ vpPoint::vpPoint(const vpColVector &P)
}
/*!
- Construction from 3D point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
- of dimension 3 or 4.
- - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
- where P[0]=X, P[1]=Y, P[2]=Z.
- - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
- where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+ Construction from 3D point world coordinates. We mean here the coordinates
+ of the point in the object frame. \param P: Vector containing the
+ coordinates of the 3D point in the object frame. This vector could be of
+ dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, 1) where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, W) where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
*/
vpPoint::vpPoint(const std::vector<double> &P)
{
@@ -104,11 +106,11 @@ vpPoint::vpPoint(const std::vector<double> &P)
}
/*!
- Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
- \param oX, oY, oZ: Coordinates of a 3D point in the object frame.
+ Set the 3D point world coordinates. We mean here the coordinates of the
+ point in the object frame. \param oX, oY, oZ: Coordinates of a 3D point in
+ the object frame.
*/
-void
-vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
+void vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
{
oP[0] = oX;
oP[1] = oY;
@@ -117,169 +119,155 @@ vpPoint::setWorldCoordinates(const double oX, const double oY, const double oZ)
}
/*!
- Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
- of dimension 3 or 4.
- - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
- where P[0]=X, P[1]=Y, P[2]=Z.
- - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
- where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+ Set the 3D point world coordinates. We mean here the coordinates of the
+ point in the object frame. \param P: Vector containing the coordinates of
+ the 3D point in the object frame. This vector could be of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, 1) where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, W) where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
*/
-void
-vpPoint::setWorldCoordinates(const vpColVector &P)
+void vpPoint::setWorldCoordinates(const vpColVector &P)
{
if (P.size() == 3) {
oP[0] = P[0];
oP[1] = P[1];
oP[2] = P[2];
oP[3] = 1.;
- }
- else if (P.size() == 4) {
+ } else if (P.size() == 4) {
oP[0] = P[0];
oP[1] = P[1];
oP[2] = P[2];
oP[3] = P[3];
- oP /= oP[3] ;
- }
- else {
- throw(vpException(vpException::dimensionError,
- "Cannot initialize vpPoint from vector with size %d", P.size()));
+ oP /= oP[3];
+ } else {
+ throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
}
}
/*!
- Set the 3D point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Vector containing the coordinates of the 3D point in the object frame. This vector could be
- of dimension 3 or 4.
- - If dimension is 3, \e P corresponds to the normalized coordinates P = (X, Y, Z, 1)
- where P[0]=X, P[1]=Y, P[2]=Z.
- - If dimension is 4, \e P corresponds to the normalized coordinates P = (X, Y, Z, W)
- where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
+ Set the 3D point world coordinates. We mean here the coordinates of the
+ point in the object frame. \param P: Vector containing the coordinates of
+ the 3D point in the object frame. This vector could be of dimension 3 or 4.
+ - If dimension is 3, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, 1) where P[0]=X, P[1]=Y, P[2]=Z.
+ - If dimension is 4, \e P corresponds to the normalized coordinates P = (X,
+ Y, Z, W) where P[0]=X, P[1]=Y, P[2]=Z and P[3]=W.
*/
-void
-vpPoint::setWorldCoordinates(const std::vector<double> &P)
+void vpPoint::setWorldCoordinates(const std::vector<double> &P)
{
if (P.size() == 3) {
oP[0] = P[0];
oP[1] = P[1];
oP[2] = P[2];
oP[3] = 1.;
- }
- else if (P.size() == 4) {
+ } else if (P.size() == 4) {
oP[0] = P[0];
oP[1] = P[1];
oP[2] = P[2];
oP[3] = P[3];
- oP /= oP[3] ;
- }
- else {
- throw(vpException(vpException::dimensionError,
- "Cannot initialize vpPoint from vector with size %d", P.size()));
+ oP /= oP[3];
+ } else {
+ throw(vpException(vpException::dimensionError, "Cannot initialize vpPoint from vector with size %d", P.size()));
}
}
-//! Get the point world coordinates. We mean here the coordinates of the point in the object frame.
-void
-vpPoint::getWorldCoordinates(double& oX, double& oY, double& oZ)
+//! Get the point world coordinates. We mean here the coordinates of the point
+//! in the object frame.
+void vpPoint::getWorldCoordinates(double &oX, double &oY, double &oZ)
{
oX = oP[0];
oY = oP[1];
oZ = oP[2];
}
-
/*!
- Get the point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ Get the point world coordinates. We mean here the coordinates of the point
+ in the object frame. \param P: Normalized coordinates of the point in the
+ object frame P = (X, Y, Z, 1)
*/
-void
-vpPoint::getWorldCoordinates(vpColVector &P)
-{
- P = oP;
-}
+void vpPoint::getWorldCoordinates(vpColVector &P) { P = oP; }
/*!
- Get the point world coordinates. We mean here the coordinates of the point in the object frame.
- \param P: Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ Get the point world coordinates. We mean here the coordinates of the point
+ in the object frame. \param P: Normalized coordinates of the point in the
+ object frame P = (X, Y, Z, 1)
*/
-void
-vpPoint::getWorldCoordinates(std::vector<double> &P)
+void vpPoint::getWorldCoordinates(std::vector<double> &P)
{
P.resize(oP.size());
- for(unsigned int i = 0; i < oP.size(); i++)
+ for (unsigned int i = 0; i < oP.size(); i++)
P[i] = oP[i];
}
/*!
- Return the point world coordinates. We mean here the coordinates of the point in the object frame.
- \return Normalized coordinates of the point in the object frame P = (X, Y, Z, 1)
+ Return the point world coordinates. We mean here the coordinates of the
+ point in the object frame. \return Normalized coordinates of the point in
+ the object frame P = (X, Y, Z, 1)
*/
-vpColVector
-vpPoint::getWorldCoordinates(void)
-{
- return this->oP;
-}
+vpColVector vpPoint::getWorldCoordinates(void) { return this->oP; }
/*!
Compute the perspective projection of a point _cP.
- \param _cP : Three dimension vector that corresponds to the coordinates of the point in the camera frame.
- \param _p : Coordinates of the point in the image plane obtained by perspective projection.
+ \param _cP : Three dimension vector that corresponds to the coordinates of
+ the point in the camera frame. \param _p : Coordinates of the point in the
+ image plane obtained by perspective projection.
*/
-void
-vpPoint::projection(const vpColVector &_cP, vpColVector &_p)
+void vpPoint::projection(const vpColVector &_cP, vpColVector &_p)
{
- _p.resize(3) ;
+ _p.resize(3);
- _p[0] = _cP[0]/_cP[2] ;
- _p[1] = _cP[1]/_cP[2] ;
- _p[2] = 1 ;
+ _p[0] = _cP[0] / _cP[2];
+ _p[1] = _cP[1] / _cP[2];
+ _p[2] = 1;
}
/*!
- From the 3D coordinates of the point in the object frame set using for example
- setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
- of the point in the camera frame.
+ From the 3D coordinates of the point in the object frame set using for
+ example setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the
+ 3D coordinates of the point in the camera frame.
\param cMo : Transformation from camera to object frame.
\param _cP : 3D coordinates of the point in the camera frame.
*/
-void
-vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
+void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &_cP)
{
- _cP.resize(4) ;
+ _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 ;
- _cP[1] *=d ;
- _cP[2] *=d ;
- _cP[3] *=d ; ;
+ double d = 1 / _cP[3];
+ _cP[0] *= d;
+ _cP[1] *= d;
+ _cP[2] *= d;
+ _cP[3] *= d;
+ ;
}
/*!
- From the 3D coordinates of the point in the object frame set using for example
- setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the 3D coordinates
- of the point in the camera frame.
+ From the 3D coordinates of the point in the object frame set using for
+ example setWorldCoordinates() or set_oX(), set_oY(), set_oZ(), compute the
+ 3D coordinates of the point in the camera frame.
\param cMo : Transformation from camera to object frame.
*/
-void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo) {
- double X = cMo[0][0]*oP[0]+ cMo[0][1]*oP[1]+ cMo[0][2]*oP[2]+ cMo[0][3]*oP[3] ;
- double Y = cMo[1][0]*oP[0]+ cMo[1][1]*oP[1]+ cMo[1][2]*oP[2]+ cMo[1][3]*oP[3] ;
- double Z = cMo[2][0]*oP[0]+ cMo[2][1]*oP[1]+ cMo[2][2]*oP[2]+ cMo[2][3]*oP[3] ;
- double W = cMo[3][0]*oP[0]+ cMo[3][1]*oP[1]+ cMo[3][2]*oP[2]+ cMo[3][3]*oP[3] ;
-
- double d = 1/W ;
- cP[0] =X*d ;
- cP[1] =Y*d ;
- cP[2] =Z*d ;
- cP[3] =1 ;
+void vpPoint::changeFrame(const vpHomogeneousMatrix &cMo)
+{
+ double X = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3] * oP[3];
+ double Y = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3] * oP[3];
+ double Z = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3] * oP[3];
+ double W = cMo[3][0] * oP[0] + cMo[3][1] * oP[1] + cMo[3][2] * oP[2] + cMo[3][3] * oP[3];
+
+ double d = 1 / W;
+ cP[0] = X * d;
+ cP[1] = Y * d;
+ cP[2] = Z * d;
+ cP[3] = 1;
}
#if 0
@@ -360,58 +348,46 @@ operator*(const vpHomography &aHb, const vpPoint& bP)
//! For memory issue (used by the vpServo class only).
vpPoint *vpPoint::duplicate() const
{
- vpPoint *feature = new vpPoint(*this) ;
- return feature ;
+ vpPoint *feature = new vpPoint(*this);
+ return feature;
}
/*!
Display the point in the image.
*/
-void
-vpPoint::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpPoint::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
+ vpColVector _cP, _p;
+ changeFrame(cMo, _cP);
- if(_cP[2] < 0) // no display if point is behind the camera
- return;
+ if (_cP[2] < 0) // no display if point is behind the camera
+ return;
- vpPoint::projection(_cP,_p) ;
- vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
+ vpPoint::projection(_cP, _p);
+ vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
}
/*!
Display the point in the image.
*/
-void
-vpPoint::display(const vpImage<vpRGBa> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpPoint::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
+ vpColVector _cP, _p;
+ changeFrame(cMo, _cP);
- if(_cP[2] < 0) // no display if point is behind the camera
- return;
+ if (_cP[2] < 0) // no display if point is behind the camera
+ return;
- vpPoint::projection(_cP,_p) ;
- vpFeatureDisplay::displayPoint(_p[0],_p[1], cam, I, color, thickness) ;
+ vpPoint::projection(_cP, _p);
+ vpFeatureDisplay::displayPoint(_p[0], _p[1], cam, I, color, thickness);
}
-VISP_EXPORT std::ostream& operator<<(std::ostream& os, const vpPoint& /* vpp */)
-{
- return( os<<"vpPoint" );
-}
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPoint & /* vpp */) { return (os << "vpPoint"); }
-vpPoint&
-vpPoint::operator=(const vpPoint& vpp )
+vpPoint &vpPoint::operator=(const vpPoint &vpp)
{
p = vpp.p;
cP = vpp.cP;
@@ -424,40 +400,37 @@ vpPoint::operator=(const vpPoint& vpp )
/*!
Display the point in the image.
*/
-void
-vpPoint::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpPoint::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
+ const unsigned int thickness)
{
- vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness) ;
+ vpFeatureDisplay::displayPoint(p[0], p[1], cam, I, color, thickness);
}
// Get coordinates
//! Get the point X coordinate in the camera frame.
-double vpPoint::get_X() const { return cP[0] ; }
+double vpPoint::get_X() const { return cP[0]; }
//! Get the point Y coordinate in the camera frame.
-double vpPoint::get_Y() const { return cP[1] ; }
+double vpPoint::get_Y() const { return cP[1]; }
//! Get the point Z coordinate in the camera frame.
-double vpPoint::get_Z() const { return cP[2] ; }
+double vpPoint::get_Z() const { return cP[2]; }
//! Get the point W coordinate in the camera frame.
-double vpPoint::get_W() const { return cP[3] ; }
+double vpPoint::get_W() const { return cP[3]; }
//! Get the point X coordinate in the object frame.
-double vpPoint::get_oX() const { return oP[0] ; }
+double vpPoint::get_oX() const { return oP[0]; }
//! Get the point Y coordinate in the object frame.
-double vpPoint::get_oY() const { return oP[1] ; }
+double vpPoint::get_oY() const { return oP[1]; }
//! Get the point Z coordinate in the object frame.
-double vpPoint::get_oZ() const { return oP[2] ; }
+double vpPoint::get_oZ() const { return oP[2]; }
//! Get the point W coordinate in the object frame.
-double vpPoint::get_oW() const { return oP[3] ; }
+double vpPoint::get_oW() const { return oP[3]; }
//! Get the point x coordinate in the image plane.
-double vpPoint::get_x() const { return p[0] ; }
+double vpPoint::get_x() const { return p[0]; }
//! Get the point y coordinate in the image plane.
-double vpPoint::get_y() const { return p[1] ; }
+double vpPoint::get_y() const { return p[1]; }
//! Get the point w coordinate in the image plane.
-double vpPoint::get_w() const { return p[2] ; }
+double vpPoint::get_w() const { return p[2]; }
/*!
Perspective projection of the point.
@@ -467,34 +440,35 @@ double vpPoint::get_w() const { return p[2] ; }
attribute cP (current //3D coordinates in the camera frame).
*/
-void vpPoint::projection() {
- double d = 1/cP[2] ;
- p[0] = cP[0]*d ;
- p[1] = cP[1]*d ;
- p[2] = 1 ;
+void vpPoint::projection()
+{
+ double d = 1 / cP[2];
+ p[0] = cP[0] * d;
+ p[1] = cP[1] * d;
+ p[2] = 1;
}
//! Set the point X coordinate in the camera frame.
-void vpPoint::set_X(const double X) { cP[0] = X ; }
+void vpPoint::set_X(const double X) { cP[0] = X; }
//! Set the point Y coordinate in the camera frame.
-void vpPoint::set_Y(const double Y) { cP[1] = Y ; }
+void vpPoint::set_Y(const double Y) { cP[1] = Y; }
//! Set the point Z coordinate in the camera frame.
-void vpPoint::set_Z(const double Z) { cP[2] = Z ; }
+void vpPoint::set_Z(const double Z) { cP[2] = Z; }
//! Set the point W coordinate in the camera frame.
-void vpPoint::set_W(const double W) { cP[3] = W ; }
+void vpPoint::set_W(const double W) { cP[3] = W; }
//! Set the point X coordinate in the object frame.
-void vpPoint::set_oX(const double oX) { oP[0] = oX ; }
+void vpPoint::set_oX(const double oX) { oP[0] = oX; }
//! Set the point Y coordinate in the object frame.
-void vpPoint::set_oY(const double oY) { oP[1] = oY ; }
+void vpPoint::set_oY(const double oY) { oP[1] = oY; }
//! Set the point Z coordinate in the object frame.
-void vpPoint::set_oZ(const double oZ) { oP[2] = oZ ; }
+void vpPoint::set_oZ(const double oZ) { oP[2] = oZ; }
//! Set the point W coordinate in the object frame.
-void vpPoint::set_oW(const double oW) { oP[3] = oW ; }
+void vpPoint::set_oW(const double oW) { oP[3] = oW; }
//! Set the point x coordinate in the image plane.
-void vpPoint::set_x(const double x) { p[0] = x ; }
+void vpPoint::set_x(const double x) { p[0] = x; }
//! Set the point y coordinate in the image plane.
-void vpPoint::set_y(const double y) { p[1] = y ; }
+void vpPoint::set_y(const double y) { p[1] = y; }
//! Set the point w coordinate in the image plane.
-void vpPoint::set_w(const double w) { p[2] = w ; }
+void vpPoint::set_w(const double w) { p[2] = w; }
diff --git a/modules/core/src/tracking/forward-projection/vpSphere.cpp b/modules/core/src/tracking/forward-projection/vpSphere.cpp
index 01278cc..5a2ed0b 100644
--- a/modules/core/src/tracking/forward-projection/vpSphere.cpp
+++ b/modules/core/src/tracking/forward-projection/vpSphere.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,197 +36,148 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpSphere.h>
#include <visp3/core/vpFeatureDisplay.h>
+#include <visp3/core/vpSphere.h>
-
-void
-vpSphere::init()
+void vpSphere::init()
{
- oP.resize(4) ;
- cP.resize(4) ;
+ oP.resize(4);
+ cP.resize(4);
- p.resize(5) ;
+ p.resize(5);
}
-void
-vpSphere::setWorldCoordinates(const vpColVector& oP_)
-{
- this->oP = oP_ ;
-}
+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 ;
- oP[2] = Z0 ;
- oP[3] = R ;
+ oP[0] = X0;
+ oP[1] = Y0;
+ oP[2] = Z0;
+ oP[3] = R;
}
+vpSphere::vpSphere() { init(); }
-
-vpSphere::vpSphere()
-{
- init() ;
-}
-
-
-vpSphere::vpSphere(const vpColVector& oP_)
-{
- init() ;
- setWorldCoordinates(oP_) ;
-}
-
-vpSphere::vpSphere(const double X0, const double Y0,
- const double Z0, const double R)
+vpSphere::vpSphere(const vpColVector &oP_)
{
- init() ;
- setWorldCoordinates(X0, Y0, Z0, R) ;
+ init();
+ setWorldCoordinates(oP_);
}
-vpSphere::~vpSphere()
+vpSphere::vpSphere(const double X0, const double Y0, const double Z0, const double R)
{
+ init();
+ setWorldCoordinates(X0, Y0, Z0, R);
}
+vpSphere::~vpSphere() {}
//! perspective projection of the sphere
-void
-vpSphere::projection()
-{
- projection(cP,p) ;
-}
+void vpSphere::projection() { projection(cP, p); }
//! Perspective projection of the circle.
-void
-vpSphere::projection(const vpColVector &cP_, vpColVector &p_)
+void vpSphere::projection(const vpColVector &cP_, vpColVector &p_)
{
- double x0, y0, z0; //variables intermediaires
-// double k0, k1, k2, k3, k4; //variables intermediaires
- double E, A, B; //variables intermediaires
+ double x0, y0, z0; // variables intermediaires
+ // double k0, k1, k2, k3, k4; //variables intermediaires
+ double E, A, B; // variables intermediaires
- //calcul des parametres M20, M11, M02 de l'ellipse
- double s, r; //variables intermediaires
- r = cP_[3];
+ // calcul des parametres M20, M11, M02 de l'ellipse
+ double s, r; // variables intermediaires
+ r = cP_[3];
- x0 = cP_[0] ;
- y0 = cP_[1] ;
- z0 = cP_[2] ;
+ x0 = cP_[0];
+ y0 = cP_[1];
+ z0 = cP_[2];
- s = r*r - y0*y0 -z0*z0;
+ s = r * r - y0 * y0 - z0 * z0;
-// k0 = (r*r - x0*x0 -z0*z0)/s;
-// k1 = (x0*y0)/s;
-// k2 = (x0*z0)/s;
-// k3 = (y0*z0)/s;
-// k4 = (r*r - x0*x0 -y0*y0)/s;
+ // k0 = (r*r - x0*x0 -z0*z0)/s;
+ // k1 = (x0*y0)/s;
+ // k2 = (x0*z0)/s;
+ // k3 = (y0*z0)/s;
+ // k4 = (r*r - x0*x0 -y0*y0)/s;
- if ((s = z0*z0 - r*r) < 0.0)
- {
+ if ((s = z0 * z0 - r * r) < 0.0) {
vpERROR_TRACE("sphere derriere le plan image\n");
}
- p_[0] = x0*z0/s ; //x
- p_[1] = y0*z0/s ; //y
+ p_[0] = x0 * z0 / s; // x
+ p_[1] = y0 * z0 / s; // y
- if (fabs(x0) > 1e-6)
- {
+ if (fabs(x0) > 1e-6) {
// vpERROR_TRACE(" %f",r) ;
- double e = y0/x0;
- double b = r/sqrt(s);
- double a = x0*x0 + y0*y0 + z0*z0 - r*r;
- if (a < 0.0)
- {
+ double e = y0 / x0;
+ double b = r / sqrt(s);
+ double a = x0 * x0 + y0 * y0 + z0 * z0 - r * r;
+ if (a < 0.0) {
vpERROR_TRACE("sphere derriere le plan image\n");
}
- a = r*sqrt(a)/s;
- if (fabs(e) <= 1.0)
- {
- E = e;
+ a = r * sqrt(a) / s;
+ if (fabs(e) <= 1.0) {
+ E = e;
A = a;
B = b;
- }
- else
- {
- E = -1.0/e;
+ } else {
+ E = -1.0 / e;
A = b;
B = a;
}
- }
- else
- {
+ } else {
// vpERROR_TRACE(" %f",r) ;
E = 0.0;
- A = r/sqrt(s);
- B = r*sqrt(y0*y0+z0*z0-r*r)/s;
+ A = r / sqrt(s);
+ B = r * sqrt(y0 * y0 + z0 * z0 - r * r) / s;
}
- p_[2] = ( A*A + B*B * E*E) / (1.0 + E*E); // mu20
- p_[3] = ( A*A - B*B) * E / (1.0 + E*E); // mu11
- p_[4] = ( B*B + A*A * E*E) / (1.0 + E*E); // mu02
+ p_[2] = (A * A + B * B * E * E) / (1.0 + E * E); // mu20
+ p_[3] = (A * A - B * B) * E / (1.0 + E * E); // mu11
+ p_[4] = (B * B + A * A * E * E) / (1.0 + E * E); // mu02
// vpERROR_TRACE(" %f",r) ;
// std::cout << p.t() ;
}
//! perspective projection of the circle
-void
-vpSphere::changeFrame(const vpHomogeneousMatrix &cMo)
-{
- changeFrame(cMo,cP) ;
-}
+void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo) { changeFrame(cMo, cP); }
//! Perspective projection of the circle.
-void
-vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
+void vpSphere::changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP_)
{
- double x0, y0, z0; //variables intermediaires
+ double x0, y0, z0; // variables intermediaires
- x0 = cMo[0][0]*oP[0] + cMo[0][1]*oP[1] + cMo[0][2]*oP[2] + cMo[0][3];
- y0 = cMo[1][0]*oP[0] + cMo[1][1]*oP[1] + cMo[1][2]*oP[2] + cMo[1][3];
- z0 = cMo[2][0]*oP[0] + cMo[2][1]*oP[1] + cMo[2][2]*oP[2] + cMo[2][3];
+ x0 = cMo[0][0] * oP[0] + cMo[0][1] * oP[1] + cMo[0][2] * oP[2] + cMo[0][3];
+ y0 = cMo[1][0] * oP[0] + cMo[1][1] * oP[1] + cMo[1][2] * oP[2] + cMo[1][3];
+ z0 = cMo[2][0] * oP[0] + cMo[2][1] * oP[1] + cMo[2][2] * oP[2] + cMo[2][3];
cP_[3] = oP[3];
- cP_[0] = x0 ;
- cP_[1] = y0 ;
- cP_[2] = z0 ;
+ cP_[0] = x0;
+ cP_[1] = y0;
+ cP_[2] = z0;
}
//! for memory issue (used by the vpServo class only)
vpSphere *vpSphere::duplicate() const
{
- vpSphere *feature = new vpSphere(*this) ;
- return feature ;
+ vpSphere *feature = new vpSphere(*this);
+ return feature;
}
-
-
// non destructive wrt. cP and p
-void vpSphere::display(const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const unsigned int thickness)
+void vpSphere::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &color, const unsigned int thickness)
{
- vpColVector _cP, _p ;
- changeFrame(cMo,_cP) ;
- projection(_cP,_p) ;
- vpFeatureDisplay::displayEllipse(_p[0],_p[1],_p[2],_p[3], _p[4],
- cam, I, color, thickness) ;
-
+ vpColVector _cP, _p;
+ changeFrame(cMo, _cP);
+ projection(_cP, _p);
+ vpFeatureDisplay::displayEllipse(_p[0], _p[1], _p[2], _p[3], _p[4], cam, I, color, thickness);
}
-
-
-void vpSphere::display(const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpColor &color,
+void vpSphere::display(const vpImage<unsigned char> &I, const vpCameraParameters &cam, const vpColor &color,
const unsigned int thickness)
{
- vpFeatureDisplay::displayEllipse(p[0],p[1],p[2],p[3], p[4],
- cam, I, color, thickness) ;
+ 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 2e515cd..fa307d2 100644
--- a/modules/core/src/tracking/moments/vpMoment.cpp
+++ b/modules/core/src/tracking/moments/vpMoment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,27 +41,26 @@
\brief Base class for all 2D moments.
*/
-
-#include <visp3/core/vpMomentObject.h>
+#include <cstring>
#include <visp3/core/vpMoment.h>
#include <visp3/core/vpMomentDatabase.h>
-#include <cstring>
+#include <visp3/core/vpMomentObject.h>
/*!
Default constructor
*/
-vpMoment::vpMoment(): object(NULL), moments(NULL), values() {}
-
+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
+ If the moment depends on other moments, these moments must be linked to the
+same database. \attention Two moments of the same class cannot be stored in
+the same database
+\code
+#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMomentCentered.h>
int main()
{
@@ -68,8 +68,7 @@ int main()
std::vector<vpPoint> vec_p;
p.set_x(1); p.set_y(1); // coordinates in meters in the image plane (vertex 1)
- vec_p.push_back(p);
- p.set_x(2); p.set_y(2); // coordinates in meters in the image plane (vertex 2)
+ 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,44 +95,47 @@ int main()
\param data_base : database of moment primitives.
*/
-void vpMoment::linkTo(vpMomentDatabase& data_base){
- if (strlen( name() ) >= 255) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the moment name"));
+void vpMoment::linkTo(vpMomentDatabase &data_base)
+{
+ if (strlen(name()) >= 255) {
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the moment name"));
}
- std::strcpy(_name,name());
- this->moments=&data_base;
+ std::strcpy(_name, name());
+ this->moments = &data_base;
- data_base.add(*this,_name);
+ data_base.add(*this, _name);
}
-
/*!
- Updates the moment with the current object. This does not compute any values.
- \param moment_object : object descriptor of the current camera vision.
+ Updates the moment with the current object. This does not compute any
+ values. \param moment_object : object descriptor of the current camera
+ vision.
*/
-void vpMoment::update(vpMomentObject& moment_object){
- this->object=&moment_object;
-}
+void vpMoment::update(vpMomentObject &moment_object) { this->object = &moment_object; }
/*!
Prints the moment contents to a stream
\param os : a std::stream.
\param m : a moment instance.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMoment& m){
- for(std::vector<double>::const_iterator i = m.values.begin();i!=m.values.end(); ++i)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMoment &m)
+{
+ for (std::vector<double>::const_iterator i = m.values.begin(); i != m.values.end(); ++i)
os << *i << ",";
return os;
}
/*!
-Prints values of all dependent moments required to calculate a specific vpMoment.
-Not made pure to maintain compatibility
-Recommended : Types inheriting from vpMoment should implement this function
+Prints values of all dependent moments required to calculate a specific
+vpMoment. Not made pure to maintain compatibility Recommended : Types
+inheriting from vpMoment should implement this function
*/
-void vpMoment::printDependencies(std::ostream& os) const{
- os << " WARNING : Falling back to base class version of printDependencies(). To prevent that, this has to be implemented in the derived classes!" << std::endl;
+void vpMoment::printDependencies(std::ostream &os) const
+{
+ os << " WARNING : Falling back to base class version of "
+ "printDependencies(). To prevent that, this has to be implemented in "
+ "the derived classes!"
+ << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentAlpha.cpp b/modules/core/src/tracking/moments/vpMomentAlpha.cpp
index c47e1c3..9e4927b 100644
--- a/modules/core/src/tracking/moments/vpMomentAlpha.cpp
+++ b/modules/core/src/tracking/moments/vpMomentAlpha.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,107 +36,102 @@
*
*****************************************************************************/
+#include <cmath>
#include <visp3/core/vpMomentAlpha.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/core/vpMomentCentered.h>
-#include <cmath>
/*!
- Empty constructor. Initializes alpha moment as a reference alpha with a value in \f$[-\pi/2..\pi/2]\f$.
- A default-constructed alpha moment may be used as a reference information for other alphas. A reference
- alpha is a class harbouring an alpha value computed for a \f$[-\pi/2..\pi/2]\f$ portion of the circle.
+ Empty constructor. Initializes alpha moment as a reference alpha with a
+ value in \f$[-\pi/2..\pi/2]\f$. A default-constructed alpha moment may be
+ used as a reference information for other alphas. A reference alpha is a
+ 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 value computed in \f$[-\pi..\pi]\f$.
- \param ref_ : vector of 3rd order centered moments corresponding to the reference alpha in
- the following order: \f$\mu_{30},\mu_{21},\mu_{12},\mu_{03}\f$.
- \param alpha_ref : value of the reference alpha.
+ Common constructor. Initializes alpha moment as a non-reference alpha with a
+ value computed in \f$[-\pi..\pi]\f$. \param ref_ : vector of 3rd order
+ centered moments corresponding to the reference alpha in the following
+ order: \f$\mu_{30},\mu_{21},\mu_{12},\mu_{03}\f$. \param alpha_ref : value
+ of the reference alpha.
*/
-vpMomentAlpha::vpMomentAlpha(std::vector<double>& ref_, double alpha_ref)
- : vpMoment(),isRef(false),symmetric(false),ref(ref_),alphaRef(alpha_ref)
+vpMomentAlpha::vpMomentAlpha(const std::vector<double> &ref_, double alpha_ref)
+ : vpMoment(), isRef(false), symmetric(false), ref(ref_), alphaRef(alpha_ref)
{
- for (std::vector<double>::iterator it = ref_.begin(); it!=ref_.end(); ++it)
- if (std::fabs(*it)<=1e-4)
+ for (std::vector<double>::const_iterator it = ref_.begin(); it != ref_.end(); ++it)
+ if (std::fabs(*it) <= 1e-4)
symmetric = true;
values.resize(1);
}
/*!
- Compute the value of the alpha-moment.
+ Compute the value of the alpha-moment.
Depends on vpMomentCentered.
*/
-void vpMomentAlpha::compute(){
- //symmetric = symmetric | this->getObject().isSymmetric();
- bool found_moment_centered;
+void vpMomentAlpha::compute()
+{
+ // symmetric = symmetric | this->getObject().isSymmetric();
+ bool found_moment_centered;
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
- found_moment_centered)));
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered)));
- if (!found_moment_centered)
- throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
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 * atan2(2.0 * momentCentered.get(1, 1),
+ // (momentCentered.get(2, 0) - momentCentered.get(0, 2)));
double alpha = 0.5 * atan(t);
- std::vector<double> rotMu(4);
- //std::vector<double> realMu(4);
-
- if (isRef)
- {
- alphaRef = alpha;
- }
- else
- {
- if (!symmetric)
- {
- double r11 = cos(alpha - alphaRef);
- double r12 = sin(alpha - alphaRef);
+ std::vector<double> rotMu(4);
+ // std::vector<double> realMu(4);
+
+ if (isRef) {
+ alphaRef = alpha;
+ } else {
+ if (!symmetric) {
+ double r11 = cos(alpha - alphaRef);
+ double r12 = sin(alpha - alphaRef);
double r21 = -r12;
- double r22 = r11;
- unsigned int idx = 0;
+ double r22 = r11;
+ unsigned int idx = 0;
unsigned int order = 4;
- for (unsigned int c = 0; c < (order) * (order); c++)
- {
- unsigned int i = c % order;
- unsigned int j = c / order;
-
- if (i + j == 3)
- {
- double r11_k = 1.;
- for (unsigned int k = 0; k <= i; k++)
- {
- double r12_i_k = pow(r12, (int)(i - k));
- double comb_i_k = static_cast<double> (vpMath::comb(i, k));
- for (unsigned int l = 0; l <= j; l++)
- {
- rotMu[idx] += static_cast<double> (comb_i_k * vpMath::comb(j, l) * r11_k * pow(r21, (int)l) * r12_i_k
- * pow(r22, (int)(j - l)) * momentCentered.get(k + l, (unsigned int)(int)(i + j - k - l)));
- }
- r11_k *= r11;
- }
- //realMu[idx] = momentCentered.get(i, j);
- idx++;
- }
- }
-
- double sum = 0.;
- bool signChange = true;
- for (unsigned int i = 0; i < 4; i++)
- {
- if (std::fabs(rotMu[i]) > 1e10 * std::numeric_limits<double>::epsilon() && std::fabs(ref[i]) > 1e10
- * std::numeric_limits<double>::epsilon() && rotMu[i] * ref[i] > 0)
- signChange = false;
+ for (unsigned int c = 0; c < (order) * (order); c++) {
+ unsigned int i = c % order;
+ unsigned int j = c / order;
+
+ if (i + j == 3) {
+ double r11_k = 1.;
+ for (unsigned int k = 0; k <= i; k++) {
+ double r12_i_k = pow(r12, (int)(i - k));
+ double comb_i_k = static_cast<double>(vpMath::comb(i, k));
+ for (unsigned int l = 0; l <= j; l++) {
+ rotMu[idx] += static_cast<double>(comb_i_k * vpMath::comb(j, l) * r11_k * pow(r21, (int)l) * r12_i_k *
+ pow(r22, (int)(j - l)) *
+ momentCentered.get(k + l, (unsigned int)(int)(i + j - k - l)));
+ }
+ r11_k *= r11;
+ }
+ // realMu[idx] = momentCentered.get(i, j);
+ idx++;
+ }
+ }
+
+ double sum = 0.;
+ bool signChange = true;
+ for (unsigned int i = 0; i < 4; i++) {
+ if (std::fabs(rotMu[i]) > 1e10 * std::numeric_limits<double>::epsilon() &&
+ std::fabs(ref[i]) > 1e10 * std::numeric_limits<double>::epsilon() && rotMu[i] * ref[i] > 0)
+ signChange = false;
sum += std::fabs(rotMu[i] * ref[i]);
- }
+ }
- if (sum < 1e4 * std::numeric_limits<double>::epsilon())
- signChange = false;
+ if (sum < 1e4 * std::numeric_limits<double>::epsilon())
+ signChange = false;
if (signChange) {
// alpha = alpha + M_PI;
if (alpha < 0)
@@ -143,32 +139,34 @@ void vpMomentAlpha::compute(){
else
alpha -= M_PI;
}
- }
- }
- values[0] = alpha;
+ }
+ }
+ values[0] = alpha;
}
/*!
Prints the value of the major-axis orientation in degrees and rad
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAlpha& c){
- os << (__FILE__) << std::endl;
- os << "Alpha = " << c.values[0] << "rad = " << vpMath::deg(c.values[0]) << "deg " << std::endl;
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAlpha &c)
+{
+ os << (__FILE__) << std::endl;
+ os << "Alpha = " << c.values[0] << "rad = " << vpMath::deg(c.values[0]) << "deg " << std::endl;
+ return os;
}
/*!
Prints the dependencies of alpha, namely centered moments mu11, mu20 ad mu02
*/
-void vpMomentAlpha::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&> (getMoments().get("vpMomentCentered",
- found_moment_centered)));
- if (!found_moment_centered)
- throw vpException(vpException::notInitialized, "vpMomentCentered not found");
-
- os << "mu11 = " << momentCentered.get(1, 1) << "\t";
- os << "mu20 = " << momentCentered.get(2, 0) << "\t";
- os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
+void vpMomentAlpha::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ bool found_moment_centered;
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered)));
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+
+ os << "mu11 = " << momentCentered.get(1, 1) << "\t";
+ os << "mu20 = " << momentCentered.get(2, 0) << "\t";
+ os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentArea.cpp b/modules/core/src/tracking/moments/vpMomentArea.cpp
index a8aec16..ac962a7 100644
--- a/modules/core/src/tracking/moments/vpMomentArea.cpp
+++ b/modules/core/src/tracking/moments/vpMomentArea.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,46 +35,50 @@
* Manikandan Bakthavatchalam
*
*****************************************************************************/
+#include <cmath>
#include <visp3/core/vpMomentArea.h>
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
-#include <cmath>
+#include <visp3/core/vpMomentObject.h>
/*!
Has the area \f$ a = m_{00} = \mu_{00} \f$.
Gets the value of m00 from vpMomentCentered.
*/
-void vpMomentArea::compute(){
- /* getObject() returns a reference to a vpMomentObject. This is public member of vpMoment */
- if(getObject().getType()==vpMomentObject::DISCRETE) {
- bool found_moment_centered;
- /* getMoments() returns a reference to a vpMomentDatabase. It is a protected member of and is inherited from vpMoment
- * .get() is a member function of vpMomentDatabase that returns a specific moment which is linked to it
- */
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- values[0] = momentCentered.get(2,0) + momentCentered.get(0,2);
- }
- else {
- values[0] = getObject().get(0,0);
- }
+void vpMomentArea::compute()
+{
+ /* getObject() returns a reference to a vpMomentObject. This is public
+ * member of vpMoment */
+ if (getObject().getType() == vpMomentObject::DISCRETE) {
+ bool found_moment_centered;
+ /* getMoments() returns a reference to a vpMomentDatabase. It is a
+ * protected member of and is inherited from vpMoment .get() is a member
+ * function of vpMomentDatabase that returns a specific moment which is
+ * linked to it
+ */
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered));
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ values[0] = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ } else {
+ values[0] = getObject().get(0, 0);
+ }
}
/*!
Default constructor.
*/
-vpMomentArea::vpMomentArea() : vpMoment(){
- values.resize(1);
-}
+vpMomentArea::vpMomentArea() : vpMoment() { values.resize(1); }
/*!
Outputs the moment's values to a stream.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentArea& m){
- os << (__FILE__) << std::endl;
- os << "a(m00) = " << m.values[0] << std::endl;
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentArea &m)
+{
+ os << (__FILE__) << std::endl;
+ os << "a(m00) = " << m.values[0] << std::endl;
+ return os;
}
/*!
@@ -81,20 +86,20 @@ If the vpMomentObject type is
1. DISCRETE(set of discrete points), uses mu20+mu02
2. DENSE_FULL_OBJECT(from image) used mu00
*/
-void vpMomentArea::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
+void vpMomentArea::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ bool found_moment_centered;
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered));
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
- if(getObject().getType()==vpMomentObject::DISCRETE)
- {
- os << "mu20 = " << momentCentered.get(2, 0) << "\t";
- os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
- }
- else
- {
- os << "mu00 = " << momentCentered.get(0, 0) << std::endl;
- }
+ if (getObject().getType() == vpMomentObject::DISCRETE) {
+ os << "mu20 = " << momentCentered.get(2, 0) << "\t";
+ os << "mu02 = " << momentCentered.get(0, 2) << std::endl;
+ } else {
+ os << "mu00 = " << momentCentered.get(0, 0) << std::endl;
+ }
}
diff --git a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp
index d6d977f..3ccde63 100644
--- a/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp
+++ b/modules/core/src/tracking/moments/vpMomentAreaNormalized.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,55 +35,61 @@
* Filip Novotny
*
*****************************************************************************/
+#include <cmath>
#include <visp3/core/vpMomentAreaNormalized.h>
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
-#include <cmath>
-
+#include <visp3/core/vpMomentObject.h>
/*!
Computes the normalized area \f$ a_n=Z^* \sqrt{\frac{a^*}{a}} \f$.
Depends on vpMomentCentered.
*/
-void vpMomentAreaNormalized::compute(){
- bool found_moment_centered;
-
- /* getMoments() returns a reference to a vpMomentDatabase. (a protected member inherited from vpMoment)
- .get() is a member function of vpMomentDatabase that returns a specific moment which is linked to it*/
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
+void vpMomentAreaNormalized::compute()
+{
+ bool found_moment_centered;
+
+ /* getMoments() returns a reference to a vpMomentDatabase. (a protected
+ member inherited from vpMoment)
+ .get() is a member function of vpMomentDatabase that returns
+ a specific moment which is linked to it*/
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
- double a;
- /* getObject() returns a reference to the vpMomentObject from which
- * the moment values are calculated. (public member of vpMoment)*/
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
+ double a;
+ /* getObject() returns a reference to the vpMomentObject from which
+ * the moment values are calculated. (public member of vpMoment)*/
+ if (getObject().getType() == vpMomentObject::DISCRETE)
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ else
+ a = getObject().get(0, 0);
- values[0] = desiredDepth*sqrt(desiredSurface/a);
+ values[0] = desiredDepth * sqrt(desiredSurface / a);
}
/*!
Default constructor.
- \param desired_surface : desired area \e a* when the visual servoing converges.
- \param desired_depth : desired depth \e Z* when the visual servoing converges.
+ \param desired_surface : desired area \e a* when the visual servoing
+ converges. \param desired_depth : desired depth \e Z* when the visual
+ servoing converges.
*/
vpMomentAreaNormalized::vpMomentAreaNormalized(double desired_surface, double desired_depth)
- : vpMoment(),desiredSurface(desired_surface),desiredDepth(desired_depth)
+ : vpMoment(), desiredSurface(desired_surface), desiredDepth(desired_depth)
{
- values.resize(1);
+ values.resize(1);
}
/*!
Outputs the moment's values to a stream.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentAreaNormalized& m){
- os << (__FILE__) << std::endl;
- os << "An = " << m.values[0] << std::endl ;
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentAreaNormalized &m)
+{
+ os << (__FILE__) << std::endl;
+ os << "An = " << m.values[0] << std::endl;
+ return os;
}
/*!
@@ -93,20 +100,22 @@ Prints dependencies namely,
3. Area moment at current pose
m00 if DENSE moment object, (mu20 + mu02) if DISCRETE moment object
*/
-void vpMomentAreaNormalized::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "Desired depth Z* = " << desiredDepth << std::endl;
- os << "Desired area m00* = " << desiredSurface << std::endl;
+void vpMomentAreaNormalized::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ os << "Desired depth Z* = " << desiredDepth << std::endl;
+ os << "Desired area m00* = " << desiredSurface << std::endl;
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered));
- if(!found_moment_centered)
- throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ bool found_moment_centered;
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered));
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
- os << "a = " << a << std::endl;
+ double a;
+ if (getObject().getType() == vpMomentObject::DISCRETE)
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ else
+ a = getObject().get(0, 0);
+ os << "a = " << a << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentBasic.cpp b/modules/core/src/tracking/moments/vpMomentBasic.cpp
index 7c0905c..7641b04 100644
--- a/modules/core/src/tracking/moments/vpMomentBasic.cpp
+++ b/modules/core/src/tracking/moments/vpMomentBasic.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,55 +41,51 @@
/*!
Default constructor.
*/
-vpMomentBasic::vpMomentBasic() : vpMoment(){
-
-}
+vpMomentBasic::vpMomentBasic() : vpMoment() {}
/*!
- Retrieve all moments of all orders computed. vpMomentBasic::get()[j*order+i] refers to moment \f$m_{ij}\f$.
- \return all computed moments.
+ Retrieve all moments of all orders computed. vpMomentBasic::get()[j*order+i]
+ refers to moment \f$m_{ij}\f$. \return all computed moments.
Same behaviour as vpMomentObject.
*/
-const std::vector<double>& vpMomentBasic::get() const{
- return getObject().get();
-}
+const std::vector<double> &vpMomentBasic::get() const { return getObject().get(); }
/*!
- Gets the desired moment using indexes.
+ Gets the desired moment using indexes.
\param i : first index of the 2D moment.
\param j : second index of the 2D moment.
\return \f$m_{ij}\f$ moment.
Same behaviour as vpMomentObject.
*/
-double vpMomentBasic::get(unsigned int i,unsigned int j) const{
- return getObject().get(i,j);
-}
+double vpMomentBasic::get(unsigned int i, unsigned int j) const { return getObject().get(i, j); }
/*!
- Dummy function. Everything is already done in object.
+ Dummy function. Everything is already done in object.
*/
-void vpMomentBasic::compute(){
-
-}
+void vpMomentBasic::compute() {}
/*!
Outputs the moment's values to a stream.
Same output as in vpMomentObject.
*/
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentBasic& m){
- os << (__FILE__) << std::endl;
- vpMomentObject::printWithIndices(m.getObject(), os);
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentBasic &m)
+{
+ os << (__FILE__) << std::endl;
+ vpMomentObject::printWithIndices(m.getObject(), os);
+ return os;
}
/*!
-No dependencies on other vpMoments, since basic moments are computed in vpMomentObject
-Just prints the basic moments in vpMomentObject with indices
+No dependencies on other vpMoments, since basic moments are computed in
+vpMomentObject Just prints the basic moments in vpMomentObject with indices
*/
-void vpMomentBasic::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "No dependencies on other vpMoments, since basic moments are computed in vpMomentObject" << std::endl;
- vpMomentObject::printWithIndices(getObject(), os);
+void vpMomentBasic::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ os << "No dependencies on other vpMoments, since basic moments are "
+ "computed in vpMomentObject"
+ << std::endl;
+ vpMomentObject::printWithIndices(getObject(), os);
}
diff --git a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp
index d80a458..00ee829 100644
--- a/modules/core/src/tracking/moments/vpMomentCInvariant.cpp
+++ b/modules/core/src/tracking/moments/vpMomentCInvariant.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Descriptor for various invariants used to drive space roations around X and Y axis.
+ * Descriptor for various invariants used to drive space roations around X and
+ *Y axis.
*
* Authors:
* Filip Novotny
@@ -44,231 +46,249 @@
(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);
}
/*!
- Computes some temporary invariants which are used internally to compute the final invariants.
- \param momentCentered : centered moments
- \param I : invariant output values
+ Computes some temporary invariants which are used internally to compute the
+ final invariants. \param momentCentered : centered moments \param I :
+ invariant output values
*/
-void vpMomentCInvariant::computeI(const vpMomentCentered& momentCentered, std::vector<double>& I_val){
-
- double mu30 = momentCentered.get(3,0);
- double mu30_2 = mu30*mu30;
- double mu30_3 = mu30_2*mu30;
-
- double mu03 = momentCentered.get(0,3);
- double mu03_2 = mu03*mu03;
- double mu03_3 = mu03*mu03_2;
-
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu50 = momentCentered.get(5,0);
- double mu32 = momentCentered.get(3,2);
- double mu14 = momentCentered.get(1,4);
- double mu05 = momentCentered.get(0,5);
- double mu23 = momentCentered.get(2,3);
- double mu41 = momentCentered.get(4,1);
- double mu40 = momentCentered.get(4,0);
- double mu04 = momentCentered.get(0,4);
- double mu31 = momentCentered.get(3,1);
- double mu13 = momentCentered.get(1,3);
- double mu22 = momentCentered.get(2,2);
- double mu21 = momentCentered.get(2,1);
- double mu12 = momentCentered.get(1,2);
- double mu11 = momentCentered.get(1,1);
-
- double mu11_2 = mu11*mu11;
- double mu12_2 = mu12*mu12;
- double mu21_2 = mu21*mu21;
- double mu22_2 = mu22*mu22;
- double mu13_2 = mu13*mu13;
- double mu31_2 = mu31*mu31;
- double mu04_2 = mu04*mu04;
- double mu40_2 = mu40*mu40;
- double mu21_3 = mu21*mu21_2;
- double mu12_3 = mu12_2*mu12;
- double mu12_4 = mu12_3*mu12;
- double mu21_4 = mu21_2*mu21_2;
-
- //double kappa = mu30_2+mu03_2-3*mu21_2+6*mu21*mu03; //Used in I8 calculation but simplified with MAPLE and found it to be wrong
- double zeta = mu20-mu02;
- double zeta_2 = zeta * zeta;
- double omicron = (mu03_2+3*mu03*mu21+mu30*(mu30+3*mu12));
- double omega = mu50+2*mu32+mu14;
- double nu = mu05+2*mu23+mu41;
- double ro = mu50-2*mu32-3*mu14;
- double gamma = mu05-2*mu23-3*mu41;
-
- double delta = mu50-10*mu32+5*mu14;
- double phi = mu05-10*mu23+5*mu41;
- double omega_2 = omega*omega;
- double nu_2 = nu*nu;
- double ro_2 = ro*ro;
- double gamma_2 = gamma*gamma;
- double delta_2 = delta*delta;
- double phi_2 = phi*phi;
-
- I_val[1]=-mu20*mu02+mu11_2;
- I_val[2]=zeta_2+4*mu11_2;
- I_val[3]=(mu30-3*mu12)*(mu30-3*mu12)+(mu03-3*mu21)*(mu03-3*mu21);
- I_val[4]=(mu30+mu12)*(mu30+mu12)+(mu21+mu03)*(mu21+mu03);
- I_val[5]=-mu30_2*mu03_2+(-4*mu12_3+6*mu21*mu12*mu03)*mu30-4*mu21_3*mu03+3*mu21_2*mu12_2;
- I_val[6]=3*mu12_4+2*mu30*mu12_3+(3*mu30_2-6*mu03*mu21)*mu12_2-6*mu30*mu21*(mu21+mu03)*mu12+2*mu30_2*mu03_2+2*mu21_3*mu03+3*mu21_2*mu03_2+3*mu21_4;
- I_val[7]=(3*mu21+2*mu03)*mu12_3+3*mu30*(mu03+2*mu21)*mu12_2-3*mu21*(mu30+mu03+mu21)*(-mu30+mu03+mu21)*mu12+mu30*(-mu30_2*mu03-2*mu21_3-3*mu03*mu21_2+mu03_3);
- //I_val[8]=3*mu21_4-3*mu21_3*mu03+(3*mu03_2+kappa-6*mu12_2)*mu21_2-mu03*(-15*mu12_2+kappa)*mu21-(-3*mu12_2*mu30+(2*kappa-3*mu03_2)*mu12+kappa*mu30)*mu12;
- I_val[8] = 3*mu03*mu21_3-2*mu03_2*mu21_2+mu21_2*mu30_2+3*mu12_2*mu03*mu21-mu03*mu21*mu30_2-mu03_3*mu21+3*mu12_3*mu30-2*mu12_2*mu30_2+mu12_2*mu03_2-mu12*mu30_3-mu12*mu30*mu03_2+3*mu12*mu30*mu21_2-6*mu12*mu30*mu03*mu21;
- I_val[9]=omicron*omicron;
-
- I_val[10]=mu40*mu04-4*mu31*mu13+3*mu22_2;
- I_val[11]=3*mu13_2+2*mu31*mu13+(-3*mu40-3*mu04)*mu22-2*mu40*mu04+3*mu31_2;
- I_val[12]=3*mu04_2+(2*mu40+12*mu22)*mu04+3*mu40_2+12*mu40*mu22+16*mu31*mu13;
- I_val[13]=omega_2+nu_2;
- I_val[14]=ro_2+gamma_2;
- I_val[15]=delta_2+phi_2;
-
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
-
- c[1]=momentCentered.get(2,0)-momentCentered.get(0,2);
- s[1]=2*momentCentered.get(1,1);
- c[2]=momentCentered.get(0,3)-3*momentCentered.get(2,1);
- s[2]=momentCentered.get(3,0)-3*momentCentered.get(1,2);
- c[3]=c[1]*c[1]-s[1]*s[1];
- s[3]=2*s[1]*c[1];
-
- II[1]=c[1]*c[1]+s[1]*s[1];
- II[2]=c[2]*c[2]+s[2]*s[2];
- II[3]=momentCentered.get(2,0)+momentCentered.get(0,2);
-
- K=(II[1]*(II[3]*sqrt(std::fabs(II[3]))))/sqrt(std::fabs(a));
-
- /*
- * Intermediate quantities required for calculation of normalized version of Sx and Sy
- * The pij doubles below are the respective centered moment values mu_ij scaled by mu20 + mu02
- */
- double p20 = momentCentered.get(2,0)/II[3]; // II[3] is the normalization factor for the 2nd order moments
- double p11 = momentCentered.get(1,1)/II[3];
- double p02 = momentCentered.get(0,2)/II[3];
-
- double d = sqrt(std::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;
- double p03 = momentCentered.get(0,3)*d;
-
- cn[1] = p20 - p02;
- sn[1] = 2.0*p11;
- sn[2] = p30 - 3.0*p12;
- cn[2] = p03 - 3.0*p21;
-
- cn[3] = cn[1]*cn[1]-sn[1]*sn[1];
- sn[3] = 2.0*sn[1]*cn[1];
-
- In1 = cn[1]*cn[1]+sn[1]*sn[1];
+void vpMomentCInvariant::computeI(const vpMomentCentered &momentCentered, std::vector<double> &I_val)
+{
+
+ double mu30 = momentCentered.get(3, 0);
+ double mu30_2 = mu30 * mu30;
+ double mu30_3 = mu30_2 * mu30;
+
+ double mu03 = momentCentered.get(0, 3);
+ double mu03_2 = mu03 * mu03;
+ double mu03_3 = mu03 * mu03_2;
+
+ double mu20 = momentCentered.get(2, 0);
+ double mu02 = momentCentered.get(0, 2);
+ double mu50 = momentCentered.get(5, 0);
+ double mu32 = momentCentered.get(3, 2);
+ double mu14 = momentCentered.get(1, 4);
+ double mu05 = momentCentered.get(0, 5);
+ double mu23 = momentCentered.get(2, 3);
+ double mu41 = momentCentered.get(4, 1);
+ double mu40 = momentCentered.get(4, 0);
+ double mu04 = momentCentered.get(0, 4);
+ double mu31 = momentCentered.get(3, 1);
+ double mu13 = momentCentered.get(1, 3);
+ double mu22 = momentCentered.get(2, 2);
+ double mu21 = momentCentered.get(2, 1);
+ double mu12 = momentCentered.get(1, 2);
+ double mu11 = momentCentered.get(1, 1);
+
+ double mu11_2 = mu11 * mu11;
+ double mu12_2 = mu12 * mu12;
+ double mu21_2 = mu21 * mu21;
+ double mu22_2 = mu22 * mu22;
+ double mu13_2 = mu13 * mu13;
+ double mu31_2 = mu31 * mu31;
+ double mu04_2 = mu04 * mu04;
+ double mu40_2 = mu40 * mu40;
+ double mu21_3 = mu21 * mu21_2;
+ double mu12_3 = mu12_2 * mu12;
+ double mu12_4 = mu12_3 * mu12;
+ double mu21_4 = mu21_2 * mu21_2;
+
+ // double kappa = mu30_2+mu03_2-3*mu21_2+6*mu21*mu03; //Used in I8
+ // calculation but simplified with MAPLE and found it to be wrong
+ double zeta = mu20 - mu02;
+ double zeta_2 = zeta * zeta;
+ double omicron = (mu03_2 + 3 * mu03 * mu21 + mu30 * (mu30 + 3 * mu12));
+ double omega = mu50 + 2 * mu32 + mu14;
+ double nu = mu05 + 2 * mu23 + mu41;
+ double ro = mu50 - 2 * mu32 - 3 * mu14;
+ double gamma = mu05 - 2 * mu23 - 3 * mu41;
+
+ double delta = mu50 - 10 * mu32 + 5 * mu14;
+ double phi = mu05 - 10 * mu23 + 5 * mu41;
+ double omega_2 = omega * omega;
+ double nu_2 = nu * nu;
+ double ro_2 = ro * ro;
+ double gamma_2 = gamma * gamma;
+ double delta_2 = delta * delta;
+ double phi_2 = phi * phi;
+
+ I_val[1] = -mu20 * mu02 + mu11_2;
+ I_val[2] = zeta_2 + 4 * mu11_2;
+ I_val[3] = (mu30 - 3 * mu12) * (mu30 - 3 * mu12) + (mu03 - 3 * mu21) * (mu03 - 3 * mu21);
+ I_val[4] = (mu30 + mu12) * (mu30 + mu12) + (mu21 + mu03) * (mu21 + mu03);
+ I_val[5] = -mu30_2 * mu03_2 + (-4 * mu12_3 + 6 * mu21 * mu12 * mu03) * mu30 - 4 * mu21_3 * mu03 + 3 * mu21_2 * mu12_2;
+ I_val[6] = 3 * mu12_4 + 2 * mu30 * mu12_3 + (3 * mu30_2 - 6 * mu03 * mu21) * mu12_2 -
+ 6 * mu30 * mu21 * (mu21 + mu03) * mu12 + 2 * mu30_2 * mu03_2 + 2 * mu21_3 * mu03 + 3 * mu21_2 * mu03_2 +
+ 3 * mu21_4;
+ I_val[7] = (3 * mu21 + 2 * mu03) * mu12_3 + 3 * mu30 * (mu03 + 2 * mu21) * mu12_2 -
+ 3 * mu21 * (mu30 + mu03 + mu21) * (-mu30 + mu03 + mu21) * mu12 +
+ mu30 * (-mu30_2 * mu03 - 2 * mu21_3 - 3 * mu03 * mu21_2 + mu03_3);
+ // I_val[8]=3*mu21_4-3*mu21_3*mu03+(3*mu03_2+kappa-6*mu12_2)*mu21_2-mu03*(-15*mu12_2+kappa)*mu21-(-3*mu12_2*mu30+(2*kappa-3*mu03_2)*mu12+kappa*mu30)*mu12;
+ I_val[8] = 3 * mu03 * mu21_3 - 2 * mu03_2 * mu21_2 + mu21_2 * mu30_2 + 3 * mu12_2 * mu03 * mu21 -
+ mu03 * mu21 * mu30_2 - mu03_3 * mu21 + 3 * mu12_3 * mu30 - 2 * mu12_2 * mu30_2 + mu12_2 * mu03_2 -
+ mu12 * mu30_3 - mu12 * mu30 * mu03_2 + 3 * mu12 * mu30 * mu21_2 - 6 * mu12 * mu30 * mu03 * mu21;
+ I_val[9] = omicron * omicron;
+
+ I_val[10] = mu40 * mu04 - 4 * mu31 * mu13 + 3 * mu22_2;
+ I_val[11] = 3 * mu13_2 + 2 * mu31 * mu13 + (-3 * mu40 - 3 * mu04) * mu22 - 2 * mu40 * mu04 + 3 * mu31_2;
+ I_val[12] = 3 * mu04_2 + (2 * mu40 + 12 * mu22) * mu04 + 3 * mu40_2 + 12 * mu40 * mu22 + 16 * mu31 * mu13;
+ I_val[13] = omega_2 + nu_2;
+ I_val[14] = ro_2 + gamma_2;
+ I_val[15] = delta_2 + phi_2;
+
+ double a;
+ if (getObject().getType() == vpMomentObject::DISCRETE)
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ else
+ a = getObject().get(0, 0);
+
+ c[1] = momentCentered.get(2, 0) - momentCentered.get(0, 2);
+ s[1] = 2 * momentCentered.get(1, 1);
+ c[2] = momentCentered.get(0, 3) - 3 * momentCentered.get(2, 1);
+ s[2] = momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2);
+ c[3] = c[1] * c[1] - s[1] * s[1];
+ s[3] = 2 * s[1] * c[1];
+
+ II[1] = c[1] * c[1] + s[1] * s[1];
+ II[2] = c[2] * c[2] + s[2] * s[2];
+ II[3] = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+
+ K = (II[1] * (II[3] * sqrt(std::fabs(II[3])))) / sqrt(std::fabs(a));
+
+ /*
+ * Intermediate quantities required for calculation of normalized version of
+ * Sx and Sy The pij doubles below are the respective centered moment values
+ * mu_ij scaled by mu20 + mu02
+ */
+ double p20 = momentCentered.get(2, 0) / II[3]; // II[3] is the normalization factor for the 2nd order moments
+ double p11 = momentCentered.get(1, 1) / II[3];
+ double p02 = momentCentered.get(0, 2) / II[3];
+
+ double d =
+ sqrt(std::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;
+ double p03 = momentCentered.get(0, 3) * d;
+
+ cn[1] = p20 - p02;
+ sn[1] = 2.0 * p11;
+ sn[2] = p30 - 3.0 * p12;
+ cn[2] = p03 - 3.0 * p21;
+
+ cn[3] = cn[1] * cn[1] - sn[1] * sn[1];
+ sn[3] = 2.0 * sn[1] * cn[1];
+
+ In1 = cn[1] * cn[1] + sn[1] * sn[1];
}
/*!
Computes translation-plane-rotation-scale invariants.
Depends on vpMomentCentered.
- All possible invariants are computed here. The selection of the invariant is done afterwards.
+ All possible invariants are computed here. The selection of the invariant is
+ done afterwards.
*/
-void vpMomentCInvariant::compute(){
- if(getObject().getOrder()<5) throw vpException(vpException::notInitialized,"Order is not high enough for vpMomentCInvariant. Specify at least order 5.");
- bool found_moment_centered;
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(getMoments().get("vpMomentCentered",found_moment_centered)));
+void vpMomentCInvariant::compute()
+{
+ if (getObject().getOrder() < 5)
+ throw vpException(vpException::notInitialized, "Order is not high enough for vpMomentCInvariant. "
+ "Specify at least order 5.");
+ bool found_moment_centered;
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(getMoments().get("vpMomentCentered", found_moment_centered)));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
- computeI(momentCentered,I);
- double II3_2 = II[3]*II[3];
- double II3_3 = II3_2*II[3];
+ computeI(momentCentered, I);
+ double II3_2 = II[3] * II[3];
+ double II3_3 = II3_2 * II[3];
- double a;
- if(getObject().getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = getObject().get(0,0);
+ double a;
+ if (getObject().getType() == vpMomentObject::DISCRETE)
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ else
+ a = getObject().get(0, 0);
- values[0] = I[1]/I[2];
- values[1] = I[3]/I[4];
+ values[0] = I[1] / I[2];
+ values[1] = I[3] / I[4];
- values[2] = I[5]/I[6];
+ values[2] = I[5] / I[6];
- values[3] = I[7]/I[6];
+ values[3] = I[7] / I[6];
- values[4] = I[8]/I[6];
+ values[4] = I[8] / I[6];
- values[5] = I[9]/I[6];
+ values[5] = I[9] / I[6];
- values[6] = I[11]/I[10];
+ values[6] = I[11] / I[10];
- values[7] = I[12]/I[10];
+ values[7] = I[12] / I[10];
- values[8] = I[13]/I[15];
+ values[8] = I[13] / I[15];
- values[9] = I[14]/I[15];
+ values[9] = I[14] / I[15];
- if (flg_sxsynormalization_)
- calcSxSyNormalized(values[10], values[11]);
- else
- calcSxSy(values[10], values[11]);
+ if (flg_sxsynormalization_)
+ calcSxSyNormalized(values[10], values[11]);
+ else
+ calcSxSy(values[10], values[11]);
- values[12] = II[1]/(II3_2); // Px
- values[13] = a*II[2]/(II3_3); // Py
+ values[12] = II[1] / (II3_2); // Px
+ values[13] = a * II[2] / (II3_3); // Py
}
/*!
Sx and Sy as it was inside compute()
*/
-void vpMomentCInvariant::calcSxSy(double& sx, double& sy) const{
- sx = (c[2]*c[3]+s[2]*s[3])/K;
- sy = (s[2]*c[3]-c[2]*s[3])/K;
+void vpMomentCInvariant::calcSxSy(double &sx, double &sy) const
+{
+ sx = (c[2] * c[3] + s[2] * s[3]) / K;
+ sy = (s[2] * c[3] - c[2] * s[3]) / K;
}
/*!
* Sx and Sy from normalized 2nd and 3rd order moments
- * Numerically better (than in the usual Sx,Sy when K appears in the denominator)
+ * Numerically better (than in the usual Sx,Sy when K appears in the
+ * denominator)
*/
-void vpMomentCInvariant::calcSxSyNormalized(double& sx, double& sy) const{
- sx = (cn[2]*cn[3] + sn[2]*sn[3]) / In1;
- sy = (sn[2]*cn[3] - cn[2]*sn[3]) / In1;
+void vpMomentCInvariant::calcSxSyNormalized(double &sx, double &sy) const
+{
+ sx = (cn[2] * cn[3] + sn[2] * sn[3]) / In1;
+ sy = (sn[2] * cn[3] - cn[2] * sn[3]) / In1;
}
/*!
Prints the temporary invariants. Used for debug purposes only
\param index : index of the temporary invariant
*/
-void vpMomentCInvariant::printI(unsigned int index){
- std::cout << "I("<< index << ")=" << I[index] << std::endl;
-}
+void vpMomentCInvariant::printI(unsigned int index) { std::cout << "I(" << index << ")=" << I[index] << std::endl; }
/*!
Print out all invariants that were computed
There are 15 of them, as in [Point-based and region based.ITRO05]
\cite Tahri05z
*/
-void vpMomentCInvariant::printInvariants(std::ostream& os) const{
- for (unsigned int i = 1; i < I.size(); ++i){ // i = 1 since vector I has been indexed from 1 in vpMomentCinvariant
- os << "I[" << i << "]=" << I[i] << std::endl;
- }
- os << std::endl;
-
+void vpMomentCInvariant::printInvariants(std::ostream &os) const
+{
+ for (unsigned int i = 1; i < I.size(); ++i) { // i = 1 since vector I has been indexed from 1 in
+ // vpMomentCinvariant
+ os << "I[" << i << "]=" << I[i] << std::endl;
+ }
+ os << std::endl;
}
/*!
Outputs the moment's values to a stream.
*/
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpMomentCInvariant& c){
- for(unsigned int i = 0;i<c.values.size();i++){
- os << c.values[i] << "," << std::endl;
- }
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCInvariant &c)
+{
+ for (unsigned int i = 0; i < c.values.size(); i++) {
+ os << c.values[i] << "," << std::endl;
+ }
+ return os;
}
-
diff --git a/modules/core/src/tracking/moments/vpMomentCentered.cpp b/modules/core/src/tracking/moments/vpMomentCentered.cpp
index bb1b73e..5902580 100644
--- a/modules/core/src/tracking/moments/vpMomentCentered.cpp
+++ b/modules/core/src/tracking/moments/vpMomentCentered.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,80 +36,87 @@
*
*****************************************************************************/
+#include <cassert>
+#include <exception>
#include <visp3/core/vpMomentCentered.h>
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentGravityCenter.h>
-#include <exception>
-#include <cassert>
+#include <visp3/core/vpMomentObject.h>
/*!
- To set the values of centred moments. Required when normalizing the moment values.
+ To set the values of centred moments. Required when normalizing the moment
+ values.
@param i : first index of the 2D moment.
@param j : second index of the 2D moment.
@param value : value of the moment.
*/
-void vpMomentCentered::set(unsigned int i, unsigned int j, double value){
- vpMomentObject mobj = getObject();
- assert(i+j<=mobj.getOrder());
- if(i+j>mobj.getOrder()) throw vpException(vpException::badValue,"You cannot set that value.");
- values[j*(mobj.getOrder()+1)+i] = value;
+void vpMomentCentered::set(unsigned int i, unsigned int j, double value)
+{
+ vpMomentObject mobj = getObject();
+ assert(i + j <= mobj.getOrder());
+ if (i + j > mobj.getOrder())
+ throw vpException(vpException::badValue, "You cannot set that value.");
+ values[j * (mobj.getOrder() + 1) + i] = value;
}
/*!
- Computes centered moments of all available orders.
+ Computes centered moments of all available orders.
Depends on vpMomentGravityCenter.
*/
-void vpMomentCentered::compute(){
- bool found_moment_gravity;
- values.resize((getObject().getOrder()+1)*(getObject().getOrder()+1));
-
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- unsigned int order = getObject().getOrder()+1;
- for(unsigned int j=0;j<(order);j++){
- for(unsigned int i=0;i<order-j;i++){
- unsigned int c = order*j+i;
- values[c]=0;
- for(unsigned int k=0;k<=i;k++){
- double Xg_i_k = pow(-momentGravity.get()[0],(int)(i-k));
- double comb_i_k = static_cast<double>( vpMath::comb(i,k) );
- for(unsigned int l=0;l<=j;l++){
- values[c]+= static_cast<double>( comb_i_k*vpMath::comb(j,l)
- *Xg_i_k
- *pow(-momentGravity.get()[1],(int)(j-l))*getObject().get(k,l) );
- }
- }
+void vpMomentCentered::compute()
+{
+ bool found_moment_gravity;
+ values.resize((getObject().getOrder() + 1) * (getObject().getOrder() + 1));
+
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(getMoments().get("vpMomentGravityCenter", found_moment_gravity));
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+
+ unsigned int order = getObject().getOrder() + 1;
+ for (unsigned int j = 0; j < (order); j++) {
+ for (unsigned int i = 0; i < order - j; i++) {
+ unsigned int c = order * j + i;
+ values[c] = 0;
+ for (unsigned int k = 0; k <= i; k++) {
+ double Xg_i_k = pow(-momentGravity.get()[0], (int)(i - k));
+ double comb_i_k = static_cast<double>(vpMath::comb(i, k));
+ for (unsigned int l = 0; l <= j; l++) {
+ values[c] += static_cast<double>(comb_i_k * vpMath::comb(j, l) * Xg_i_k *
+ pow(-momentGravity.get()[1], (int)(j - l)) * getObject().get(k, l));
}
+ }
}
-
+ }
}
/*!
Default constructor.
*/
-vpMomentCentered::vpMomentCentered() : vpMoment(){
-
-}
+vpMomentCentered::vpMomentCentered() : vpMoment() {}
/*!
- Gets the desired moment using indexes.
+ Gets the desired moment using indexes.
\param i : first index of the centered moment.
\param j : second index of the centered moment.
\return \f$\mu_{ij}\f$ moment.
*/
-double vpMomentCentered::get(unsigned int i,unsigned int j) const {
- unsigned int order = getObject().getOrder();
- assert(i+j<=order);
- if(i+j>order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
-
- return values[j*(order+1)+i];
+double vpMomentCentered::get(unsigned int i, unsigned int j) const
+{
+ unsigned int order = getObject().getOrder();
+ assert(i + j <= order);
+ if (i + j > order)
+ throw vpException(vpException::badValue, "The requested value has not "
+ "been computed, you should "
+ "specify a higher order.");
+
+ return values[j * (order + 1) + i];
}
/*!
- Outputs the centered moment's values \f$\mu_{ij}\f$ to a stream presented as a matrix.
- The first line corresponds to \f$\mu_{0[0:order]}\f$, the second one to \f$\mu_{1[0:order]}\f$
- Values in table corresponding to a higher order are marked with an "x" and not computed.
+ Outputs the centered moment's values \f$\mu_{ij}\f$ to a stream presented as
+a matrix. The first line corresponds to \f$\mu_{0[0:order]}\f$, the second one
+to \f$\mu_{1[0:order]}\f$ Values in table corresponding to a higher order are
+marked with an "x" and not computed.
For example, if the maximal order is 3, the following values are provided:
@@ -119,39 +127,40 @@ u02 u12 x x
u30 x x x
\endcode
- This output will be followed by an output with indexes as produced by printWithIndices() function
+ This output will be followed by an output with indexes as produced by
+printWithIndices() function
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentCentered& m){
- for(unsigned int i = 0;i<m.values.size();i++){
- if(i%(m.getObject().getOrder()+1)==0)
- os << std::endl;
-
- if((i%(m.getObject().getOrder()+1)+i/(m.getObject().getOrder()+1))<m.getObject().getOrder()+1)
- os << m.values[i] ;
- else
- os << "x";
-
- os << "\t";
- }
- os << std::endl;
- m.printWithIndices(os);
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentCentered &m)
+{
+ for (unsigned int i = 0; i < m.values.size(); i++) {
+ if (i % (m.getObject().getOrder() + 1) == 0)
+ os << std::endl;
+
+ if ((i % (m.getObject().getOrder() + 1) + i / (m.getObject().getOrder() + 1)) < m.getObject().getOrder() + 1)
+ os << m.values[i];
+ else
+ os << "x";
+
+ os << "\t";
+ }
+ os << std::endl;
+ m.printWithIndices(os);
+ return os;
}
/*!
Print in a readable form which looks better than output from << operator
*/
-void
-vpMomentCentered::printWithIndices(std::ostream& os) const {
- unsigned int orderp1 = getObject().getOrder()+1;
- for(unsigned int k=0; k<orderp1; k++) {
- for(unsigned int l=0; l<orderp1-k; l++)
- {
- os << "mu[" << k << "," << l << "] = " << this->get(k,l) << "\t";
- }
- os << std::endl;
+void vpMomentCentered::printWithIndices(std::ostream &os) const
+{
+ unsigned int orderp1 = getObject().getOrder() + 1;
+ for (unsigned int k = 0; k < orderp1; k++) {
+ for (unsigned int l = 0; l < orderp1 - k; l++) {
+ os << "mu[" << k << "," << l << "] = " << this->get(k, l) << "\t";
}
os << std::endl;
+ }
+ os << std::endl;
}
/*!
@@ -160,21 +169,23 @@ which are
1. Raw geometric moments (vpMomentObject) and
2. Centre of gravity (vpMomentGravityCentered)
*/
-void
-vpMomentCentered::printDependencies(std::ostream& os) const {
- os << (__FILE__) << std::endl;
- /*
- Retreive the raw moments
- */
- const vpMomentObject objt = getObject();
- vpMomentObject::printWithIndices(objt, os);
-
- /*
- Get xg,yg
- */
- bool found_moment_gravity;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity)
- throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- os << "Xg = " << momentGravity.getXg() << "\t" << "Yg = " << momentGravity.getYg() << std::endl;
+void vpMomentCentered::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ /*
+ Retreive the raw moments
+ */
+ const vpMomentObject objt = getObject();
+ vpMomentObject::printWithIndices(objt, os);
+
+ /*
+ Get xg,yg
+ */
+ bool found_moment_gravity;
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(getMoments().get("vpMomentGravityCenter", found_moment_gravity));
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+ os << "Xg = " << momentGravity.getXg() << "\t"
+ << "Yg = " << momentGravity.getYg() << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentCommon.cpp b/modules/core/src/tracking/moments/vpMomentCommon.cpp
index 3e8f264..fc39601 100644
--- a/modules/core/src/tracking/moments/vpMomentCommon.cpp
+++ b/modules/core/src/tracking/moments/vpMomentCommon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,18 +40,21 @@
#include <visp3/core/vpMomentObject.h>
/*!
- Default constructor. Initializes the common database with the following moments:
- basic, gravity,centered,centered+normalized,normalized gravity,normalized surface, scale-plane-rotation-translation invariant, alpha, symmetric invariant.
- \param dstSurface : destination surface. You may use vpMomentCommon::getSurface().
- \param ref : reference 3rd order moments (see vpMomentAlpha). You may use vpMomentCommon::getMu3().
- \param refAlpha : reference alpha (see vpMomentAlpha). You may use vpMomentCommon::getAlpha().
- \param dstZ : destination depth.
- \param flg_sxsyfromnormalized : flag to enable calculation of sx,sy from normalized moments.
+ Default constructor. Initializes the common database with the following
+ moments: basic, gravity,centered,centered+normalized,normalized
+ gravity,normalized surface, scale-plane-rotation-translation invariant,
+ alpha, symmetric invariant. \param dstSurface : destination surface. You may
+ use vpMomentCommon::getSurface(). \param ref : reference 3rd order moments
+ (see vpMomentAlpha). You may use vpMomentCommon::getMu3(). \param refAlpha
+ : reference alpha (see vpMomentAlpha). You may use
+ vpMomentCommon::getAlpha(). \param dstZ : destination depth. \param
+ flg_sxsyfromnormalized : flag to enable calculation of sx,sy from normalized
+ moments.
*/
-vpMomentCommon::vpMomentCommon(double dstSurface,std::vector<double> ref,double refAlpha,double dstZ, bool flg_sxsyfromnormalized)
+vpMomentCommon::vpMomentCommon(double dstSurface, const std::vector<double> &ref, double refAlpha, double dstZ,
+ bool flg_sxsyfromnormalized)
: vpMomentDatabase(), momentBasic(), momentGravity(), momentCentered(), momentGravityNormalized(),
- momentSurfaceNormalized(dstSurface,dstZ), momentCInvariant(), momentAlpha(ref,refAlpha),
- momentArea()
+ momentSurfaceNormalized(dstSurface, dstZ), momentCInvariant(), momentAlpha(ref, refAlpha), momentArea()
{
momentCInvariant = new vpMomentCInvariant(flg_sxsyfromnormalized);
@@ -65,19 +69,19 @@ vpMomentCommon::vpMomentCommon(double dstSurface,std::vector<double> ref,double
}
/*!
-Updates all moments in the database with the object and computes all their values.
-This is possible because this particular database knows the link between the moments it contains.
-The order of computation is as follows:
+Updates all moments in the database with the object and computes all their
+values. This is possible because this particular database knows the link
+between the moments it contains. The order of computation is as follows:
vpMomentGravityCenter,vpMomentCentered,vpMomentAlpha,vpMomentCInvariant,vpMomentSInvariant,vpMomentAreaNormalized,vpMomentGravityCenterNormalized
\param object : Moment object.
Example of using a preconfigured database to compute one of the C-invariants:
\code
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpPoint.h>
+#include <iostream>
#include <visp3/core/vpMomentCInvariant.h>
#include <visp3/core/vpMomentCommon.h>
-#include <iostream>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
int main()
{
@@ -89,9 +93,11 @@ int main()
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)
+ 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)
+ 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
@@ -107,107 +113,118 @@ int main()
//get C-invariant
vpMomentCInvariant& C = static_cast<vpMomentCInvariant&>(db.get("vpMomentCInvariant",success));
- if(success){
- std::cout << C.get(0) << std:: std::endl;
- }else
- std::cout << "vpMomentCInvariant not found." << std::endl;
+ if(success) {
+ std::cout << C.get(0) << std:: std::endl;
+ } else
+ std::cout << "vpMomentCInvariant not found." << std::endl;
return 0;
}
\endcode
*/
-void vpMomentCommon::updateAll(vpMomentObject& object){
- try {
- vpMomentDatabase::updateAll(object);
-
- momentGravity.compute();
- momentCentered.compute();
- momentAlpha.compute();
- momentCInvariant->compute();
+void vpMomentCommon::updateAll(vpMomentObject &object)
+{
+ try {
+ vpMomentDatabase::updateAll(object);
- momentSurfaceNormalized.compute();
- momentGravityNormalized.compute();
- momentArea.compute();
+ momentGravity.compute();
+ momentCentered.compute();
+ momentAlpha.compute();
+ momentCInvariant->compute();
- } catch(const char* ex){
- std::cout << "exception:" << ex <<std::endl;
+ momentSurfaceNormalized.compute();
+ momentGravityNormalized.compute();
+ momentArea.compute();
- }
+ } catch (const char *ex) {
+ std::cout << "exception:" << ex << std::endl;
+ }
}
/*!
Gets the surface of an object
\param object : moment object
*/
-double vpMomentCommon::getSurface(vpMomentObject& object) {
- vpMomentDatabase moments;
+double vpMomentCommon::getSurface(vpMomentObject &object)
+{
+ vpMomentDatabase moments;
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
+ vpMomentGravityCenter momentGravity;
+ momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;
+ momentCentered.linkTo(moments);
- moments.updateAll(object);
+ moments.updateAll(object);
- momentGravity.compute();
- momentCentered.compute();
+ momentGravity.compute();
+ momentCentered.compute();
- double a;
- if (object.getType()==vpMomentObject::DISCRETE)
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- else
- a = object.get(0,0);
+ double a;
+ if (object.getType() == vpMomentObject::DISCRETE)
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ else
+ a = object.get(0, 0);
- return a;
+ return a;
}
/*!
Gets a reference alpha of an object.
\param object : Moment object.
*/
-double vpMomentCommon::getAlpha(vpMomentObject& object) {
- vpMomentDatabase moments;
+double vpMomentCommon::getAlpha(vpMomentObject &object)
+{
+ vpMomentDatabase moments;
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
- vpMomentAlpha momentAlpha; momentAlpha.linkTo(moments);
+ vpMomentGravityCenter momentGravity;
+ momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;
+ momentCentered.linkTo(moments);
+ vpMomentAlpha momentAlpha;
+ momentAlpha.linkTo(moments);
- moments.updateAll(object);
- momentGravity.compute();
- momentCentered.compute();
- momentAlpha.compute();
+ moments.updateAll(object);
+ momentGravity.compute();
+ momentCentered.compute();
+ momentAlpha.compute();
- return momentAlpha.get();
+ return momentAlpha.get();
}
/*!
Gets the reference 3rd order moments of an object.
\param object : Moment object.
*/
-std::vector<double> vpMomentCommon::getMu3(vpMomentObject& object) {
- vpMomentDatabase moments;
+std::vector<double> vpMomentCommon::getMu3(vpMomentObject &object)
+{
+ vpMomentDatabase moments;
- vpMomentGravityCenter momentGravity;momentGravity.linkTo(moments);
- vpMomentCentered momentCentered;momentCentered.linkTo(moments);
+ vpMomentGravityCenter momentGravity;
+ momentGravity.linkTo(moments);
+ vpMomentCentered momentCentered;
+ momentCentered.linkTo(moments);
- moments.updateAll(object);
+ moments.updateAll(object);
- momentGravity.compute();
- momentCentered.compute();
+ momentGravity.compute();
+ momentCentered.compute();
- std::vector<double> mu(4);
- unsigned int idx=0;
- for (unsigned int j=0; j<4; j++) {
- for (unsigned int i=0; i<4; i++) {
- if (i+j==3){
- mu[idx] = momentCentered.get(i,j);
- idx++;
- }
+ std::vector<double> mu(4);
+ unsigned int idx = 0;
+ for (unsigned int j = 0; j < 4; j++) {
+ for (unsigned int i = 0; i < 4; i++) {
+ if (i + j == 3) {
+ mu[idx] = momentCentered.get(i, j);
+ idx++;
}
}
- return mu;
+ }
+ return mu;
}
-vpMomentCommon::~vpMomentCommon(){
- if (momentCInvariant)
- delete momentCInvariant;
+vpMomentCommon::~vpMomentCommon()
+{
+ if (momentCInvariant)
+ delete momentCInvariant;
}
diff --git a/modules/core/src/tracking/moments/vpMomentDatabase.cpp b/modules/core/src/tracking/moments/vpMomentDatabase.cpp
index 62e9814..524ea8c 100644
--- a/modules/core/src/tracking/moments/vpMomentDatabase.cpp
+++ b/modules/core/src/tracking/moments/vpMomentDatabase.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,61 +36,69 @@
*
*****************************************************************************/
-#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMoment.h>
-#include <typeinfo>
#include <iostream>
+#include <typeinfo>
+#include <visp3/core/vpMoment.h>
+#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentObject.h>
/*!
- Adds a moment to the database.
- \param moment : moment to add
- \param : name of the moment's class
+ Adds a moment to the database.
+ \param moment : moment to add
+ \param : name of the moment's class
- \attention You cannot add two moments with the same name. The rules for insersion are the same as those of std::map.
+ \attention You cannot add two moments with the same name. The rules
+ for insersion are the same as those of std::map.
*/
-void vpMomentDatabase::add(vpMoment& moment,const char* name){
- moments.insert(std::pair<const char*,vpMoment*>((const char*)name,&moment));
+void vpMomentDatabase::add(vpMoment &moment, const char *name)
+{
+ moments.insert(std::pair<const char *, vpMoment *>((const char *)name, &moment));
}
/*!
Retrieves a moment from the database.
\param type : Name of the moment's class.
- \param found : true if the moment's type exists in the database, false otherwise.
- \return Moment corresponding to \e type.
+ \param found : true if the moment's type exists in the database, false
+ otherwise. \return Moment corresponding to \e type.
*/
-const vpMoment& vpMomentDatabase::get(const char* type, bool& found) const {
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator it = moments.find(type);
-
- found = (it!=moments.end());
- return *(it->second);
+const vpMoment &vpMomentDatabase::get(const char *type, bool &found) const
+{
+ std::map<const char *, vpMoment *, vpMomentDatabase::cmp_str>::const_iterator it = moments.find(type);
+
+ found = (it != moments.end());
+ return *(it->second);
}
/*!
- Updates the moment object for all moments in the database
- \param object : Moment object for which all the moments in the database should be updated.
+ Updates the moment object for all moments in the database
+ \param object : Moment object for which all the moments in the database
+ should be updated.
- Sometimes, it might be useful to update the whole database when computing only one moment when this moment depends on other moments.
- The example provided in the header of this class gives an example that shows how to compute gravity center moment and the centered moment using a mass update.
+ Sometimes, it might be useful to update the whole database when computing
+ only one moment when this moment depends on other moments. The example
+ provided in the header of this class gives an example that shows how to
+ compute gravity center moment and the centered moment using a mass update.
*/
-void vpMomentDatabase::updateAll(vpMomentObject& object){
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
- for(itr = moments.begin(); itr != moments.end(); ++itr){
- (*itr).second->update(object);
- }
+void vpMomentDatabase::updateAll(vpMomentObject &object)
+{
+ std::map<const char *, vpMoment *, vpMomentDatabase::cmp_str>::const_iterator itr;
+ for (itr = moments.begin(); itr != moments.end(); ++itr) {
+ (*itr).second->update(object);
+ }
}
/*!
- Outputs all the moments values in the database to a stream.
+ Outputs all the moments values in the database to a stream.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentDatabase& m){
- std::map<const char*,vpMoment*,vpMomentDatabase::cmp_str>::const_iterator itr;
- os << "{";
-
- for(itr = m.moments.begin(); itr != m.moments.end(); ++itr){
- os << (*itr).first << ": [" << *((*itr).second) << "],";
- }
- os << "}";
-
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentDatabase &m)
+{
+ std::map<const char *, vpMoment *, vpMomentDatabase::cmp_str>::const_iterator itr;
+ os << "{";
+
+ for (itr = m.moments.begin(); itr != m.moments.end(); ++itr) {
+ os << (*itr).first << ": [" << *((*itr).second) << "],";
+ }
+ os << "}";
+
+ return os;
}
diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp
index 491f716..5a221ee 100644
--- a/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp
+++ b/modules/core/src/tracking/moments/vpMomentGravityCenter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,44 +39,44 @@
#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentObject.h>
/*!
- Computes the two gravity center coordinates commonly called \f$x_g\f$ and \f$y_g\f$.
+ Computes the two gravity center coordinates commonly called \f$x_g\f$ and
+ \f$y_g\f$.
*/
-void vpMomentGravityCenter::compute(){
- values[0] = getObject().get(1,0)/getObject().get(0,0);
- values[1] = getObject().get(0,1)/getObject().get(0,0);
+void vpMomentGravityCenter::compute()
+{
+ values[0] = getObject().get(1, 0) / getObject().get(0, 0);
+ values[1] = getObject().get(0, 1) / getObject().get(0, 0);
}
/*!
Default constructor.
*/
-vpMomentGravityCenter::vpMomentGravityCenter() : vpMoment(){
- values.resize(2);
-}
+vpMomentGravityCenter::vpMomentGravityCenter() : vpMoment() { values.resize(2); }
/*!
Returns a vector of the two gravity center coordinates.
\return Coordinates in the following moment: \f$(x_g,y_g)\f$.
*/
-const std::vector<double>& vpMomentGravityCenter::get() const{
- return values;
-}
+const std::vector<double> &vpMomentGravityCenter::get() const { return values; }
/*!
Outputs the moment's values to a stream.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenter& m){
- os << (__FILE__) << std::endl;
- os << "(Xg,Yg) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenter &m)
+{
+ os << (__FILE__) << std::endl;
+ os << "(Xg,Yg) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
+ return os;
}
/*!
Prints its dependencies
Basic moments m10, m01 and m00 from vpMomentObject
*/
-void vpMomentGravityCenter::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- os << "m10 = " << getObject().get(1,0) << "\t";
- os << "m00 = " << getObject().get(0,1) << "\t";
- os << "m00 = " << getObject().get(0,0) << std::endl;
+void vpMomentGravityCenter::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ os << "m10 = " << getObject().get(1, 0) << "\t";
+ os << "m00 = " << getObject().get(0, 1) << "\t";
+ os << "m00 = " << getObject().get(0, 0) << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
index 2b3df9f..bc7dbe2 100644
--- a/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
+++ b/modules/core/src/tracking/moments/vpMomentGravityCenterNormalized.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,52 +29,58 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * 2D normalized gravity center moment descriptor (usually described by the pair Xn,Yn)
+ * 2D normalized gravity center moment descriptor (usually described by the
+ *pair Xn,Yn)
*
* Authors:
* Filip Novotny
*
*****************************************************************************/
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentGravityCenterNormalized.h>
#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/core/vpMomentAreaNormalized.h>
/*!
Computes normalized gravity center moment.
Depends on vpMomentAreaNormalized and on vpMomentGravityCenter.
*/
-void vpMomentGravityCenterNormalized::compute(){
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
-
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+void vpMomentGravityCenterNormalized::compute()
+{
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ getMoments().get("vpMomentAreaNormalized", found_moment_surface_normalized));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(getMoments().get("vpMomentGravityCenter", found_moment_gravity));
- double Xn = momentGravity.get()[0]*momentSurfaceNormalized.get()[0];
- double Yn = momentGravity.get()[1]*momentSurfaceNormalized.get()[0];
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
- values[0] = Xn;
- values[1] = Yn;
+ double Xn = momentGravity.get()[0] * momentSurfaceNormalized.get()[0];
+ double Yn = momentGravity.get()[1] * momentSurfaceNormalized.get()[0];
+ values[0] = Xn;
+ values[1] = Yn;
}
/*!
Default constructor.
*/
-vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() : vpMomentGravityCenter(){}
+vpMomentGravityCenterNormalized::vpMomentGravityCenterNormalized() : vpMomentGravityCenter() {}
/*!
Outputs the moment's values to a stream.
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentGravityCenterNormalized& m){
- os << (__FILE__) << std::endl;
- os << "(Xn,Yn) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentGravityCenterNormalized &m)
+{
+ os << (__FILE__) << std::endl;
+ os << "(Xn,Yn) = (" << m.values[0] << ", " << m.values[1] << ")" << std::endl;
+ return os;
}
/*!
@@ -81,16 +88,22 @@ Prints the dependent moments,
1. centre of gravity
2. normalized area moment
*/
-void vpMomentGravityCenterNormalized::printDependencies(std::ostream& os) const{
- os << (__FILE__) << std::endl;
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
+void vpMomentGravityCenterNormalized::printDependencies(std::ostream &os) const
+{
+ os << (__FILE__) << std::endl;
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(getMoments().get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(getMoments().get("vpMomentGravityCenter",found_moment_gravity));
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ getMoments().get("vpMomentAreaNormalized", found_moment_surface_normalized));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(getMoments().get("vpMomentGravityCenter", found_moment_gravity));
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- os << "Xg = " << momentGravity.get()[0] << "\t" << "Yg = " << momentGravity.get()[1] << std::endl;
- os << "An = " << momentSurfaceNormalized.get()[0] << std::endl;
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+ os << "Xg = " << momentGravity.get()[0] << "\t"
+ << "Yg = " << momentGravity.get()[1] << std::endl;
+ os << "An = " << momentSurfaceNormalized.get()[0] << std::endl;
}
diff --git a/modules/core/src/tracking/moments/vpMomentObject.cpp b/modules/core/src/tracking/moments/vpMomentObject.cpp
index 3e0c858..f0f710f 100644
--- a/modules/core/src/tracking/moments/vpMomentObject.cpp
+++ b/modules/core/src/tracking/moments/vpMomentObject.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,12 @@
*
*****************************************************************************/
+#include <stdexcept>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/core/vpConfig.h>
-#include <stdexcept>
#include <cmath>
#include <limits>
@@ -59,106 +60,97 @@
\return moment value
*/
-double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint>& points)
+double vpMomentObject::calc_mom_polygon(unsigned int p, unsigned int q, const std::vector<vpPoint> &points)
{
- unsigned int i,k,l;
- double den,mom;
+ unsigned int i, k, l;
+ double den, mom;
double x_p_k;
double y_l;
double y_q_l;
- den = static_cast<double>( (p+q+2)*(p+q+1)*vpMath::comb(p+q,p) );
+ den = static_cast<double>((p + q + 2) * (p + q + 1) * vpMath::comb(p + q, p));
mom = 0.0;
- for (i=1;i<=points.size()-1;i++)
- {
+ for (i = 1; i <= points.size() - 1; i++) {
double s = 0.0;
- double x_k=1;
- for (k=0;k<=p;k++)
- {
- y_l=1;
- x_p_k = pow(points[i-1].get_x(), (int)(p-k));
- for (l=0;l<=q;l++)
- {
- y_q_l=pow(points[i-1].get_y(), (int)(q-l));
-
- s += static_cast<double>(
- vpMath::comb(k+l,l)
- *vpMath::comb(p+q-k-l,q-l)
- *x_k
- *x_p_k
- *y_l
- *y_q_l );
-
- y_l*=points[i].get_y();
+ double x_k = 1;
+ for (k = 0; k <= p; k++) {
+ y_l = 1;
+ x_p_k = pow(points[i - 1].get_x(), (int)(p - k));
+ for (l = 0; l <= q; l++) {
+ y_q_l = pow(points[i - 1].get_y(), (int)(q - l));
- }
- x_k*=points[i].get_x();
+ s += static_cast<double>(vpMath::comb(k + l, l) * vpMath::comb(p + q - k - l, q - l) * x_k * x_p_k * y_l *
+ y_q_l);
+ y_l *= points[i].get_y();
+ }
+ x_k *= points[i].get_x();
}
- s *= ((points[i-1].get_x())*(points[i].get_y())-(points[i].get_x())*(points[i-1].get_y()));
+ s *= ((points[i - 1].get_x()) * (points[i].get_y()) - (points[i].get_x()) * (points[i - 1].get_y()));
mom += s;
}
mom /= den;
- return(mom);
+ return (mom);
}
/*!
Caching to avoid redundant multiplications.
- \param cache : Lookup table that contains the order by order values. For example,
- if the order is 3, cache will contain:
- \code
+ \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
+\endcode
\param x, y : Coordinates of a point.
*/
-void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y){
- cache[0]=1;
+void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y)
+{
+ cache[0] = 1;
- for(unsigned int i=1;i<order;i++)
- cache[i]=cache[i-1]*x;
+ for (unsigned int i = 1; i < order; i++)
+ cache[i] = cache[i - 1] * x;
- for(unsigned int j=order;j<order*order;j+=order)
- cache[j]=cache[j-order]*y;
+ for (unsigned int j = order; j < order * order; j += order)
+ cache[j] = cache[j - order] * y;
- 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];
- }
+ 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];
}
+ }
}
/*!
* Manikandan.B
* Need to cache intensity along with the coordinates for photometric moments
*/
-void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y, double IntensityNormalized) {
+void vpMomentObject::cacheValues(std::vector<double> &cache, double x, double y, double IntensityNormalized)
+{
- cache[0]=IntensityNormalized;
+ cache[0] = IntensityNormalized;
- double invIntensityNormalized = 0.;
- if (std::fabs(IntensityNormalized)>=std::numeric_limits<double>::epsilon())
- invIntensityNormalized = 1.0/IntensityNormalized;
+ double invIntensityNormalized = 0.;
+ if (std::fabs(IntensityNormalized) >= std::numeric_limits<double>::epsilon())
+ invIntensityNormalized = 1.0 / IntensityNormalized;
- for(unsigned int i=1;i<order;i++)
- cache[i]=cache[i-1]*x;
+ for (unsigned int i = 1; i < order; i++)
+ cache[i] = cache[i - 1] * x;
- for(unsigned int j=order;j<order*order;j+=order)
- cache[j]=cache[j-order]*y;
+ for (unsigned int j = order; j < order * order; j += order)
+ cache[j] = cache[j - order] * y;
- 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;
- }
+ 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;
}
+ }
}
-
/*!
Computes basic moments from a vector of points.
There are two cases:
@@ -171,7 +163,8 @@ void vpMomentObject::cacheValues(std::vector<double>& cache,double x, double y,
\param points : Vector of points.
- The code below shows how to use this function to consider a dense object defined by a closed contour.
+ The code below shows how to use this function to consider a dense object
+defined by a closed contour.
\code
#include <visp3/core/vpMomentObject.h>
@@ -200,9 +193,11 @@ int main()
return 0;
}
+
\endcode
- This other example shows how to consider an object as a discrete set of four points.
+ This other example shows how to consider an object as a discrete set of four
+points.
\code
#include <visp3/core/vpMomentObject.h>
@@ -229,26 +224,28 @@ int main()
return 0;
}
+
\endcode
*/
-void vpMomentObject::fromVector(std::vector<vpPoint>& points){
- if(type==vpMomentObject::DENSE_POLYGON){
- if(std::fabs(points.rbegin()->get_x()-points.begin()->get_x())>std::numeric_limits<double>::epsilon() ||
- std::fabs(points.rbegin()->get_y()-points.begin()->get_y())>std::numeric_limits<double>::epsilon()){
- points.resize(points.size()+1);
- points[points.size()-1] = points[0];
+void vpMomentObject::fromVector(std::vector<vpPoint> &points)
+{
+ if (type == vpMomentObject::DENSE_POLYGON) {
+ if (std::fabs(points.rbegin()->get_x() - points.begin()->get_x()) > std::numeric_limits<double>::epsilon() ||
+ std::fabs(points.rbegin()->get_y() - points.begin()->get_y()) > std::numeric_limits<double>::epsilon()) {
+ points.resize(points.size() + 1);
+ points[points.size() - 1] = points[0];
}
- for(unsigned int j=0;j<order*order;j++){
- values[j]=calc_mom_polygon(j%order,j/order,points);
+ for (unsigned int j = 0; j < order * order; j++) {
+ values[j] = calc_mom_polygon(j % order, j / order, points);
}
} else {
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
- for(unsigned int i=0;i<points.size();i++){
- cacheValues(cache,points[i].get_x(),points[i].get_y());
- for(unsigned int j=0;j<order;j++){
- for(unsigned int k=0;k<order-j;k++){
- values[j*order+k]+=cache[j*order+k];
+ std::vector<double> cache(order * order, 0.);
+ values.assign(order * order, 0);
+ for (unsigned int i = 0; i < points.size(); i++) {
+ cacheValues(cache, points[i].get_x(), points[i].get_y());
+ for (unsigned int j = 0; j < order; j++) {
+ for (unsigned int k = 0; k < order - j; k++) {
+ values[j * order + k] += cache[j * order + k];
}
}
}
@@ -257,102 +254,108 @@ void vpMomentObject::fromVector(std::vector<vpPoint>& points){
/*!
Computes basic moments from an image.
- There is no assumption made about whether the input is dense or discrete but it's more common to use vpMomentObject::DENSE_FULL_OBJECT with this method.
+ There is no assumption made about whether the input is dense or discrete but
+it's more common to use vpMomentObject::DENSE_FULL_OBJECT with this method.
\param image : Image to consider.
- \param threshold : Pixels with a luminance lower than this threshold will be considered.
- \param cam : Camera parameters used to convert pixels coordinates in meters in the image plane.
+ \param threshold : Pixels with a luminance lower than this threshold will be
+considered. \param cam : Camera parameters used to convert pixels coordinates
+in meters in the image plane.
The code below shows how to use this function.
\code
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMomentObject.h>
int main()
{
- vpCameraParameters cam; // Camera parameters used for pixel to meter conversion
- vpImage<unsigned char> I(288, 384); // Image used to define the object
+ vpCameraParameters cam; // Camera parameters used for pixel to
+meter conversion vpImage<unsigned char> I(288, 384); // Image used to define
+the object
// ... Initialize the image
- unsigned char threshold = 128; // Gray level used to define which part of the image belong to the dense object
+ unsigned char threshold = 128; // Gray level used to define which part of
+the image belong to the dense object
- vpMomentObject obj(3); // Create an image moment object with 3 as maximum order
- obj.fromImage(I, threshold, cam); // Initialize the object from the image
+ vpMomentObject obj(3); // Create an image moment object with 3 as maximum
+order obj.fromImage(I, threshold, cam); // Initialize the object from the
+image
return 0;
}
\endcode
*/
-void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned char threshold, const vpCameraParameters& cam){
+void vpMomentObject::fromImage(const vpImage<unsigned char> &image, unsigned char threshold,
+ const vpCameraParameters &cam)
+{
#ifdef VISP_HAVE_OPENMP
- #pragma omp parallel shared(threshold)
+#pragma omp parallel shared(threshold)
{
- std::vector<double> curvals(order*order);
- curvals.assign(order*order,0.);
+ std::vector<double> curvals(order * order);
+ curvals.assign(order * order, 0.);
- #pragma omp for nowait//automatically organize loop counter between threads
- for(int i=0;i<(int)image.getCols();i++){
- for(int j=0;j<(int)image.getRows();j++){
+#pragma omp for nowait // automatically organize loop counter between threads
+ for (int i = 0; i < (int)image.getCols(); i++) {
+ for (int j = 0; j < (int)image.getRows(); j++) {
unsigned int i_ = static_cast<unsigned int>(i);
unsigned int j_ = static_cast<unsigned int>(j);
- if(image[j_][i_]>threshold){
- double x=0;
- double y=0;
- vpPixelMeterConversion::convertPoint(cam,i_,j_,x,y);
-
- double yval=1.;
- for(unsigned int k=0;k<order;k++){
- double xval=1.;
- for(unsigned int l=0;l<order-k;l++){
- curvals[(k*order+l)]+=(xval*yval);
- xval*=x;
+ if (image[j_][i_] > threshold) {
+ double x = 0;
+ double y = 0;
+ vpPixelMeterConversion::convertPoint(cam, i_, j_, x, y);
+
+ double yval = 1.;
+ for (unsigned int k = 0; k < order; k++) {
+ double xval = 1.;
+ for (unsigned int l = 0; l < order - k; l++) {
+ curvals[(k * order + l)] += (xval * yval);
+ xval *= x;
}
- yval*=y;
+ yval *= y;
}
}
}
}
- #pragma omp master //only set this variable in master thread
+#pragma omp master // only set this variable in master thread
{
- values.assign(order*order, 0.);
+ values.assign(order * order, 0.);
}
- #pragma omp barrier
- for(unsigned int k=0;k<order;k++){
- for(unsigned int l=0;l<order-k;l++){
- #pragma omp atomic
- values[k*order+l]+= curvals[k*order+l];
+#pragma omp barrier
+ for (unsigned int k = 0; k < order; k++) {
+ for (unsigned int l = 0; l < order - k; l++) {
+#pragma omp atomic
+ values[k * order + l] += curvals[k * order + l];
}
}
-
}
#else
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
- for(unsigned int i=0;i<image.getCols();i++){
- for(unsigned int j=0;j<image.getRows();j++){
- if(image[j][i]>threshold){
- double x=0;
- double y=0;
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
- cacheValues(cache,x,y);
- for(unsigned int k=0;k<order;k++){
- for(unsigned int l=0;l<order-k;l++){
- values[k*order+l]+=cache[k*order+l];
- }
- }
- }
+ std::vector<double> cache(order * order, 0.);
+ values.assign(order * order, 0);
+ for (unsigned int i = 0; i < image.getCols(); i++) {
+ for (unsigned int j = 0; j < image.getRows(); j++) {
+ if (image[j][i] > threshold) {
+ double x = 0;
+ double y = 0;
+ vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
+ cacheValues(cache, x, y);
+ for (unsigned int k = 0; k < order; k++) {
+ for (unsigned int l = 0; l < order - k; l++) {
+ values[k * order + l] += cache[k * order + l];
+ }
}
+ }
}
+ }
#endif
- //Normalisation equivalent to sampling interval/pixel size delX x delY
- double norm_factor = 1./(cam.get_px()*cam.get_py());
- for (std::vector<double>::iterator it = values.begin(); it!=values.end(); ++it) {
- *it = (*it) * norm_factor;
- }
+ // Normalisation equivalent to sampling interval/pixel size delX x delY
+ double norm_factor = 1. / (cam.get_px() * cam.get_py());
+ for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
+ *it = (*it) * norm_factor;
+ }
}
/*!
@@ -361,87 +364,91 @@ void vpMomentObject::fromImage(const vpImage<unsigned char>& image, unsigned cha
* Intended to be used by 'vpMomentObject's of type DENSE_FULL_OBJECT
* @param image : Grayscale image
* @param cam : Camera parameters (to change to )
- * @param bg_type : White/Black background surrounding the image
- * @param normalize_with_pix_size : This flag if SET, the moments, after calculation are normalized w.r.t pixel size
- * available from camera parameters
+ * @param bg_type : White/Black background surrounding the
+ * image
+ * @param normalize_with_pix_size : This flag if SET, the moments, after
+ * calculation are normalized w.r.t pixel size available from camera
+ * parameters
*/
-void vpMomentObject::fromImage(const vpImage<unsigned char>& image, const vpCameraParameters& cam,
- vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
+void vpMomentObject::fromImage(const vpImage<unsigned char> &image, const vpCameraParameters &cam,
+ vpCameraImgBckGrndType bg_type, bool normalize_with_pix_size)
{
- std::vector<double> cache(order*order,0.);
- values.assign(order*order,0);
+ std::vector<double> cache(order * order, 0.);
+ values.assign(order * order, 0);
// (x,y) - Pixel co-ordinates in metres
- double x=0;
- double y=0;
- //for indexing into cache[] and values[]
+ double x = 0;
+ double y = 0;
+ // for indexing into cache[] and values[]
unsigned int idx = 0;
unsigned int kidx = 0;
double intensity = 0;
- //double Imax = static_cast<double>(image.getMaxValue());
+ // double Imax = static_cast<double>(image.getMaxValue());
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
- iscale = 1.0/Imax;
+ 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
+ iscale = 1.0 / Imax;
}
if (bg_type == vpMomentObject::WHITE) {
- /////////// WHITE BACKGROUND ///////////
- for(unsigned int j=0;j<image.getRows();j++){
- for(unsigned int i=0;i<image.getCols();i++){
- x = 0;
- y = 0;
- intensity = (double)(image[j][i])*iscale;
- double intensity_white = 1. - intensity;
-
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
- cacheValues(cache,x,y, intensity_white); // Modify 'cache' which has x^p*y^q to x^p*y^q*(1 - I(x,y))
-
- // Copy to "values"
- for(unsigned int k=0;k<order;k++){
- kidx = k*order;
- for(unsigned int l=0;l<order-k;l++){
- idx = kidx+l;
- values[idx]+= cache[idx];
- }
- }
+ /////////// WHITE BACKGROUND ///////////
+ for (unsigned int j = 0; j < image.getRows(); j++) {
+ for (unsigned int i = 0; i < image.getCols(); i++) {
+ x = 0;
+ y = 0;
+ intensity = (double)(image[j][i]) * iscale;
+ double intensity_white = 1. - intensity;
+
+ vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
+ cacheValues(cache, x, y, intensity_white); // Modify 'cache' which has
+ // x^p*y^q to x^p*y^q*(1 -
+ // I(x,y))
+
+ // Copy to "values"
+ for (unsigned int k = 0; k < order; k++) {
+ kidx = k * order;
+ for (unsigned int l = 0; l < order - k; l++) {
+ idx = kidx + l;
+ values[idx] += cache[idx];
}
+ }
}
- }
- else {
- /////////// BLACK BACKGROUND ///////////
- for(unsigned int j=0;j<image.getRows();j++){
- for(unsigned int i=0;i<image.getCols();i++){
- x = 0;
- y = 0;
- intensity = (double)(image[j][i])*iscale;
- vpPixelMeterConversion::convertPoint(cam,i,j,x,y);
-
- // Cache values for fast moment calculation
- cacheValues(cache,x,y, intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
-
- // Copy to moments array 'values'
- for(unsigned int k=0;k<order;k++){
- kidx = k*order;
- for(unsigned int l=0;l<order-k;l++){
- idx = kidx+l;
- values[idx]+= cache[idx];
- }
- }
-
+ }
+ } else {
+ /////////// BLACK BACKGROUND ///////////
+ for (unsigned int j = 0; j < image.getRows(); j++) {
+ for (unsigned int i = 0; i < image.getCols(); i++) {
+ x = 0;
+ y = 0;
+ intensity = (double)(image[j][i]) * iscale;
+ vpPixelMeterConversion::convertPoint(cam, i, j, x, y);
+
+ // Cache values for fast moment calculation
+ cacheValues(cache, x, y,
+ intensity); // Modify 'cache' which has x^p*y^q to x^p*y^q*I(x,y)
+
+ // Copy to moments array 'values'
+ for (unsigned int k = 0; k < order; k++) {
+ kidx = k * order;
+ for (unsigned int l = 0; l < order - k; l++) {
+ idx = kidx + l;
+ values[idx] += cache[idx];
}
+ }
}
+ }
}
- if (normalize_with_pix_size){
- // Normalisation equivalent to sampling interval/pixel size delX x delY
- double norm_factor = 1./(cam.get_px()*cam.get_py());
- for (std::vector<double>::iterator it = values.begin(); it!=values.end(); ++it) {
- *it = (*it) * norm_factor;
- }
+ if (normalize_with_pix_size) {
+ // Normalisation equivalent to sampling interval/pixel size delX x delY
+ double norm_factor = 1. / (cam.get_px() * cam.get_py());
+ for (std::vector<double>::iterator it = values.begin(); it != values.end(); ++it) {
+ *it = (*it) * norm_factor;
+ }
}
}
@@ -449,32 +456,34 @@ void vpMomentObject::fromImage(const vpImage<unsigned char>& image, const vpCame
Does exactly the work of the default constructor as it existed in the very
first version of vpMomentObject
*/
-void
-vpMomentObject::init(unsigned int orderinp) {
- order = orderinp + 1;
- type = vpMomentObject::DENSE_FULL_OBJECT;
- flg_normalize_intensity = true; // By default, the intensity values are normalized
- values.resize((order+1)*(order+1));
- values.assign((order+1)*(order+1),0);
+void vpMomentObject::init(unsigned int orderinp)
+{
+ order = orderinp + 1;
+ type = vpMomentObject::DENSE_FULL_OBJECT;
+ flg_normalize_intensity = true; // By default, the intensity values are normalized
+ values.resize((order + 1) * (order + 1));
+ values.assign((order + 1) * (order + 1), 0);
}
/*!
Helper to copy constructor
*/
-void
-vpMomentObject::init(const vpMomentObject& objin){
- order = objin.getOrder()+1;
- type = objin.getType();
- flg_normalize_intensity = objin.flg_normalize_intensity;
- values.resize(objin.values.size());
- values = objin.values;
+void vpMomentObject::init(const vpMomentObject &objin)
+{
+ order = objin.getOrder() + 1;
+ type = objin.getType();
+ flg_normalize_intensity = objin.flg_normalize_intensity;
+ values.resize(objin.values.size());
+ values = objin.values;
}
/*!
Default constructor.
- Initializes the object with the maximum used order. You cannot use higher order moments than the order of the moment object.
- The parameter specified is the highest desired included order.
- All orders up to this values will be computed. In other words, a vpMomentObject will compute all \f$ m_{ij} \f$ moments with \f$ i+j \in [0..order] \f$.
+ Initializes the object with the maximum used order. You cannot use higher
+ order moments than the order of the moment object. The parameter specified
+ is the highest desired included order. All orders up to this values will be
+ computed. In other words, a vpMomentObject will compute all \f$ m_{ij} \f$
+ moments with \f$ i+j \in [0..order] \f$.
\param max_order : Maximum reached order (i+j) to be used. All
considered i+j will be of order smaller or equal than this
@@ -484,34 +493,35 @@ 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);
+ init(max_order);
}
/*!
Copy constructor
*/
-vpMomentObject::vpMomentObject(const vpMomentObject& srcobj)
- : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT),
- values()
+vpMomentObject::vpMomentObject(const vpMomentObject &srcobj)
+ : flg_normalize_intensity(true), order(1), type(vpMomentObject::DENSE_FULL_OBJECT), values()
{
- init(srcobj);
+ init(srcobj);
}
/*!
- Returns all basic moment values \f$m_{ij}\f$ with \f$i \in [0:\mbox{order}]\f$ and \f$j \in [0:\mbox{order}]\f$.
+ Returns all basic moment values \f$m_{ij}\f$ with \f$i \in
+[0:\mbox{order}]\f$ and \f$j \in [0:\mbox{order}]\f$.
- \return Vector of moment values. To access \f$m_{ij}\f$, you have to read vpMomentObject::get()[j*(order+1)+i].
+ \return Vector of moment values. To access \f$m_{ij}\f$, you have to read
+vpMomentObject::get()[j*(order+1)+i].
For example, if the maximal order is 3, the following values are provided:
\code
m00 m10 m20 m01 m11 m21 m02 m12 m12 m30 m03
\endcode
- To access for example to the basic moment m12, you should use this kind of code:
- \code
+ To access for example to the basic moment m12, you should use this kind of
+code:
+\code
vpMomentObject obj(3);
// ... initialise the object using fromVector() or fromImage()
std::vector mij = obj.get();
@@ -519,10 +529,7 @@ double m12;
m12 = mij[2*(obj.getOrder()+1)+1]; // i=1 and j=2
\endcode
*/
-const std::vector<double>& vpMomentObject::get() const {
- return values;
-
-}
+const std::vector<double> &vpMomentObject::get() const { return values; }
/*!
Returns the basic moment value \f$m_{ij}\f$ corresponding to i,j indexes
@@ -530,11 +537,15 @@ const std::vector<double>& vpMomentObject::get() const {
\param i : First moment index, with \f$i+j \leq order\f$.
\param j : Second moment index, with \f$i+j \leq order\f$.
*/
-double vpMomentObject::get(unsigned int i, unsigned int j) const {
- assert(i+j<=getOrder());
- if(i+j>=order) throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
+double vpMomentObject::get(unsigned int i, unsigned int j) const
+{
+ assert(i + j <= getOrder());
+ if (i + j >= order)
+ throw vpException(vpException::badValue, "The requested value has not "
+ "been computed, you should "
+ "specify a higher order.");
- return values[j*order+i];
+ return values[j * order + i];
}
/*!
@@ -543,16 +554,20 @@ 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){
- assert(i+j<=getOrder());
- if(i+j>=order) throw vpException(vpException::badValue,"The requested value cannot be set, you should specify a higher order for the moment object.");
- values[j*order+i] = value_ij;
+void vpMomentObject::set(unsigned int i, unsigned int j, const double &value_ij)
+{
+ assert(i + j <= getOrder());
+ if (i + j >= order)
+ throw vpException(vpException::badValue, "The requested value cannot be set, you should specify "
+ "a higher order for the moment object.");
+ values[j * order + i] = value_ij;
}
/*!
- Outputs the basic moment's values \f$m_{ij}\f$ to a stream presented as a matrix.
- The first line corresponds to \f$m_{0[0:order]}\f$, the second one to \f$m_{1[0:order]}\f$
- Values in table corresponding to a higher order are marked with an "x" and not computed.
+ Outputs the basic moment's values \f$m_{ij}\f$ to a stream presented as a
+ matrix. The first line corresponds to \f$m_{0[0:order]}\f$, the second one
+ to \f$m_{1[0:order]}\f$ Values in table corresponding to a higher order are
+ marked with an "x" and not computed.
For example, if the maximal order is 3, the following values are provided:
@@ -564,40 +579,41 @@ void vpMomentObject::set(unsigned int i, unsigned int j, const double& value_ij)
\endcode
*/
-VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpMomentObject& m){
- for(unsigned int i = 0;i<m.values.size();i++){
-
- if(i%(m.order)==0)
- os << std::endl;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpMomentObject &m)
+{
+ for (unsigned int i = 0; i < m.values.size(); i++) {
- if((i%(m.order)+i/(m.order))<m.order)
- os << m.values[i] ;
- else
- os << "x";
+ if (i % (m.order) == 0)
+ os << std::endl;
- os << "\t";
+ if ((i % (m.order) + i / (m.order)) < m.order)
+ os << m.values[i];
+ else
+ os << "x";
- }
+ os << "\t";
+ }
- return os;
+ return os;
}
/*!
Outputs the raw moment values \f$m_{ij}\f$ in indexed form.
- The moment values are same as provided by the operator << which outputs x for uncalculated moments.
+ The moment values are same as provided by the operator << which outputs x
+ for uncalculated moments.
*/
-void
-vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os) {
- std::vector<double> moment = momobj.get();
- os << std::endl <<"Order of vpMomentObject: "<<momobj.getOrder()<<std::endl;
- // Print out values. This is same as printing using operator <<
- for(unsigned int k=0; k<=momobj.getOrder(); k++) {
- for(unsigned int l=0; l<(momobj.getOrder()+1)-k; l++){
- os << "m[" << l << "," << k << "] = " << moment[k*(momobj.getOrder()+1)+ l] << "\t";
- }
- os << std::endl;
+void vpMomentObject::printWithIndices(const vpMomentObject &momobj, std::ostream &os)
+{
+ std::vector<double> moment = momobj.get();
+ os << std::endl << "Order of vpMomentObject: " << momobj.getOrder() << std::endl;
+ // Print out values. This is same as printing using operator <<
+ for (unsigned int k = 0; k <= momobj.getOrder(); k++) {
+ for (unsigned int l = 0; l < (momobj.getOrder() + 1) - k; l++) {
+ os << "m[" << l << "," << k << "] = " << moment[k * (momobj.getOrder() + 1) + l] << "\t";
}
- os <<std::endl;
+ os << std::endl;
+ }
+ os << std::endl;
}
/*!
@@ -608,11 +624,11 @@ vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os)
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
+ Instead of accessing the moment m21 as obj.get(2,1), you can now do
+Mpq[2][1]. This is useful when you want to use the functions available in
+vpMatrix. One use case i see now is to copy the contents of the matrix to a
+file or std::cout. For instance, like
+\code
// Print to console
Mpq.maplePrint(std::cout);
// Or write to a file
@@ -623,29 +639,32 @@ vpMomentObject::printWithIndices(const vpMomentObject& momobj, std::ostream& os)
The output can be copied and pasted to MAPLE as a matrix.
\warning
-The moments that are not calculated have zeros. For instance, for a vpMomentObject of order 8,
-the moment m[7,2] is not calculated. It will have 0 by default. User discretion is advised.
+The moments that are not calculated have zeros. For instance, for a
+vpMomentObject of order 8, the moment m[7,2] is not calculated. It will have 0
+by default. User discretion is advised.
*/
-vpMatrix
-vpMomentObject::convertTovpMatrix(const vpMomentObject& momobj) {
- std::vector<double> moment = momobj.get();
- unsigned int order = momobj.getOrder();
- vpMatrix M(order+1, order+1);
- for(unsigned int k=0; k<=order; k++) {
- for(unsigned int l=0; l<(order+1)-k; l++){
- M[l][k] = moment[k*(order+1)+ l];
- }
+vpMatrix vpMomentObject::convertTovpMatrix(const vpMomentObject &momobj)
+{
+ std::vector<double> moment = momobj.get();
+ unsigned int order = momobj.getOrder();
+ vpMatrix M(order + 1, order + 1);
+ for (unsigned int k = 0; k <= order; k++) {
+ for (unsigned int l = 0; l < (order + 1) - k; l++) {
+ M[l][k] = moment[k * (order + 1) + l];
}
- return M;
+ }
+ return M;
}
/*!
Nothing to destruct. This will allow for a polymorphic usage
For instance,
\code
- vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER); where vpWeightedMomentObject is child class of vpMomentObject
+ vpMomentObject* obj = new vpWeightedMomentObject(weightfunc,ORDER);
\endcode
+ where vpWeightedMomentObject is child class of vpMomentObject
*/
-vpMomentObject::~vpMomentObject(){
-// deliberate empty
+vpMomentObject::~vpMomentObject()
+{
+ // deliberate empty
}
diff --git a/modules/core/src/tracking/vpTracker.cpp b/modules/core/src/tracking/vpTracker.cpp
index 96aeeeb..73ee14a 100644
--- a/modules/core/src/tracking/vpTracker.cpp
+++ b/modules/core/src/tracking/vpTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,44 +36,29 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpTracker.h>
#include <visp3/core/vpDebug.h>
-
+#include <visp3/core/vpTracker.h>
/*!
\file vpTracker.cpp
\brief Class that defines what is a generic tracker.
*/
-
-void
-vpTracker::init()
-{
- cPAvailable = false ;
-}
-
-
+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)
{
p = tracker.p;
cP = tracker.cP;
cPAvailable = tracker.cPAvailable;
-
+
return *this;
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/core/test/camera/testCameraParametersConversion.cpp b/modules/core/test/camera/testCameraParametersConversion.cpp
index 369dab5..2e60bce 100644
--- a/modules/core/test/camera/testCameraParametersConversion.cpp
+++ b/modules/core/test/camera/testCameraParametersConversion.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,8 +37,6 @@
*
*****************************************************************************/
-
-
/*!
\file testCameraParametersConversion.cpp
@@ -45,26 +44,25 @@
vpPixelMeterConversion class.
*/
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpDebug.h>
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/core/vpMath.h>
-#include <stdlib.h>
-#include <stdio.h>
int main()
{
try {
vpCameraParameters cam;
- double px,py,u0,v0;
+ double px, py, u0, v0;
px = 1657.429131;
py = 1658.818598;
u0 = 322.2437833;
v0 = 230.8012737;
vpCameraParameters camDist;
- double px_dist,py_dist,u0_dist,v0_dist,kud_dist,kdu_dist;
+ double px_dist, py_dist, u0_dist, v0_dist, kud_dist, kdu_dist;
px_dist = 1624.824731;
py_dist = 1625.263641;
u0_dist = 324.0923411;
@@ -72,40 +70,42 @@ int main()
kud_dist = -0.1741532338;
kdu_dist = 0.1771165148;
- cam.initPersProjWithoutDistortion(px,py,u0,v0);
- camDist.initPersProjWithDistortion(px_dist,py_dist,u0_dist,v0_dist,
- kud_dist, kdu_dist);
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+ camDist.initPersProjWithDistortion(px_dist, py_dist, u0_dist, v0_dist, kud_dist, kdu_dist);
double u1 = 320;
double v1 = 240;
double x1 = 0, y1 = 0;
double u2 = 0, v2 = 0;
- vpPixelMeterConversion::convertPoint(cam,u1,v1,x1,y1);
- vpMeterPixelConversion::convertPoint(cam,x1,y1,u2,v2);
- if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
+ vpPixelMeterConversion::convertPoint(cam, u1, v1, x1, y1);
+ vpMeterPixelConversion::convertPoint(cam, x1, y1, u2, v2);
+ if (!vpMath::equal(u1, u2) || !vpMath::equal(v1, v2)) {
vpTRACE("Error in convertPoint without distortion:\n"
"u1 = %f, u2 = %f\n"
- "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
+ "v1 = %f, v2 = %f\n",
+ u1, u2, v1, v2);
return -1;
}
vpTRACE("convertPoint without distortion :\n"
"u1 - u2 = %.20f\n"
- "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
+ "v1 - v2 = %.20f\n",
+ u1 - u2, v1 - v2);
- vpPixelMeterConversion::convertPoint(camDist,u1,v1,x1,y1);
- vpMeterPixelConversion::convertPoint(camDist,x1,y1,u2,v2);
- if(!vpMath::equal(u1,u2) || !vpMath::equal(v1,v2)){
+ vpPixelMeterConversion::convertPoint(camDist, u1, v1, x1, y1);
+ vpMeterPixelConversion::convertPoint(camDist, x1, y1, u2, v2);
+ if (!vpMath::equal(u1, u2) || !vpMath::equal(v1, v2)) {
vpTRACE("Error in convertPoint with distortion :\n"
"u1 = %f, u2 = %f\n"
- "v1 = %f, v2 = %f\n",u1,u2,v1,v2);
+ "v1 = %f, v2 = %f\n",
+ u1, u2, v1, v2);
return -1;
}
vpTRACE("convertPoint with distortion :\n"
"u1 - u2 = %.20f\n"
- "v1 - v2 = %.20f\n",u1 - u2,v1 - v2);
+ "v1 - v2 = %.20f\n",
+ u1 - u2, v1 - v2);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/image/testConversion.cpp b/modules/core/test/image/testConversion.cpp
index dc6da7a..92401b8 100644
--- a/modules/core/test/image/testConversion.cpp
+++ b/modules/core/test/image/testConversion.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,18 +36,17 @@
*
*****************************************************************************/
-#include <stdlib.h>
#include <iomanip>
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpTime.h>
-
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
/*!
\example testConversion.cpp
@@ -55,8 +55,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:n:h"
-
+#define GETOPTARGS "cdi:o:n:h"
/*
Print the program options.
@@ -83,8 +82,8 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -100,8 +99,7 @@ OPTIONS: Default\n\
Set the number of benchmark iterations.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str(), nbiter);
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str(), nbiter);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -120,25 +118,36 @@ 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, const std::string &user,
int &nbIterations)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'n': nbIterations = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, opath, user, nbIterations); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'n':
+ nbIterations = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user, nbIterations);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user, nbIterations); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user, nbIterations);
+ return false;
+ break;
}
}
@@ -153,77 +162,72 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
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;
+ 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)) );
+ while (pt_input != pt_end) {
+ *pt_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2)));
pt_input += 4;
- pt_output ++;
+ 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;
+ 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)) );
+ while (pt_input != pt_end) {
+ *pt_output = (unsigned char)(0.2126 * (*pt_input) + 0.7152 * (*(pt_input + 1)) + 0.0722 * (*(pt_input + 2)));
pt_input += 3;
- pt_output ++;
+ pt_output++;
}
}
-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
- //step is negative
- int lineStep = (flip) ? -(int)(width*3) : (int)(width*3);
+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
+ // step is negative
+ int lineStep = (flip) ? -(int)(width * 3) : (int)(width * 3);
- //starting source address = last line if we need to flip the image
- unsigned char * src = (flip) ? bgr+(width*height*3)+lineStep : bgr;
+ // starting source address = last line if we need to flip the image
+ unsigned char *src = (flip) ? bgr + (width * height * 3) + lineStep : bgr;
- unsigned int j=0;
- unsigned int i=0;
+ unsigned int j = 0;
+ unsigned int i = 0;
- for(i=0 ; i < height ; i++)
- {
+ 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)) ;
- line+=3;
+ for (j = 0; j < width; j++) {
+ *grey++ = (unsigned char)(0.2126 * *(line + 2) + 0.7152 * *(line + 1) + 0.0722 * *(line + 0));
+ line += 3;
}
- //go to the next line
- src+=lineStep;
+ // go to the next line
+ src += lineStep;
}
}
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-void computeRegularBGRToGrayscale(const cv::Mat& src, vpImage<unsigned char>& dest)
+void computeRegularBGRToGrayscale(const cv::Mat &src, vpImage<unsigned char> &dest)
{
- if(src.type() == CV_8UC3) {
+ 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);
+ if (src.isContinuous()) {
+ computeRegularBGRToGrayscale((unsigned char *)src.data, (unsigned char *)dest.bitmap, (unsigned int)src.cols,
+ (unsigned int)src.rows, false);
}
}
}
#endif
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -235,14 +239,15 @@ main(int argc, const char ** argv)
std::string username;
int nbIterations = 100;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -254,7 +259,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbIterations) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -271,11 +276,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username, nbIterations);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -286,8 +289,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -295,14 +297,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
@@ -310,41 +311,42 @@ main(int argc, const char ** argv)
// Here starts really the test
//
- vpImage<unsigned char> Ig ; // Grey image
- vpImage<vpRGBa> Ic ; // Color image
+ vpImage<unsigned char> Ig; // Grey image
+ vpImage<vpRGBa> Ic; // Color image
//-------------------- .pgm -> .ppm
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, "ViSP-images/Klimt/Klimt.pgm");
- std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ig, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ std::cout << " Load " << filename << std::endl;
+ vpImageIo::read(Ig, filename);
// Create a color image from the grey
vpImageConvert::convert(Ig, Ic);
filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
+ vpImageIo::write(Ic, filename);
//-------------------- .ppm -> .pgm
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, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
+ vpImageIo::read(Ic, filename);
// Create a grey image from the color
vpImageConvert::convert(Ic, Ig);
filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
+ vpImageIo::write(Ig, filename);
//-------------------- YUV -> RGB
std::cout << "** Convert YUV pixel value to a RGB value" << std::endl;
- unsigned char y=187, u=10, v=30;
+ unsigned char y = 187, u = 10, v = 30;
unsigned char r, g, b;
// Convert a YUV pixel value to a RGB value
vpImageConvert::YUVToRGB(y, u, v, r, g, b);
- 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
@@ -353,36 +355,37 @@ main(int argc, const char ** argv)
// Convert a IplImage to a vpImage<vpRGBa>
////////////////////////
std::cout << "** Convert an IplImage to a vpImage<vpRGBa>" << std::endl;
- IplImage* image = NULL; /*!< The image read / acquired */
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ IplImage *image = NULL; /*!< The image read / acquired */
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
/* 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 << " Cannot read image: "<< filename << std::endl;
+ 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);
}
vpImageConvert::convert(image, Ic);
filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
+ vpImageIo::write(Ic, filename);
std::cout << " Convert result in " << filename << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
/* Read the pgm image */
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 != NULL)
+ cvReleaseImage(&image);
+ if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
std::cout << " Cannot read image: " << filename << std::endl;
return (-1);
}
vpImageConvert::convert(image, Ic);
filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
+ vpImageIo::write(Ic, filename);
std::cout << " Convert result in " << filename << std::endl;
@@ -390,37 +393,39 @@ main(int argc, const char ** argv)
// Convert a IplImage to a vpImage<unsigned char>
////////////////////////////
std::cout << "** Convert an IplImage to a vpImage<unsigned char>" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
/* Read the color image */
- 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) {
- std::cout << " Cannot read image: "<< 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) {
+ std::cout << " Cannot read image: " << filename << std::endl;
return (-1);
}
vpImageConvert::convert(image, Ig);
filename = vpIoTools::createFilePath(opath, "Klimt_color_cv.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
+ vpImageIo::write(Ig, filename);
std::cout << " Convert result in " << filename << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
/* Read the pgm image */
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) {
- std::cout << " Cannot read image: "<< filename << std::endl;
+ if (image != NULL)
+ cvReleaseImage(&image);
+ if ((image = cvLoadImage(filename.c_str(), CV_LOAD_IMAGE_GRAYSCALE)) == NULL) {
+ std::cout << " Cannot read image: " << filename << std::endl;
return (-1);
}
vpImageConvert::convert(image, Ig);
filename = vpIoTools::createFilePath(opath, "Klimt_grey_cv.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
+ vpImageIo::write(Ig, filename);
std::cout << " Convert result in " << filename << std::endl;
@@ -428,20 +433,21 @@ main(int argc, const char ** argv)
// Convert a vpImage<vpRGBa> to a IplImage
////////////////////////////////////
std::cout << "** Convert a vpImage<vpRGBa> to an IplImage" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
/* Read the color image */
// Load a color image from the disk
std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
+ vpImageIo::read(Ic, filename);
vpImageConvert::convert(Ic, image);
filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cv.ppm");
/* Save the the current image */
std::cout << " Write " << filename << std::endl;
- if((cvSaveImage(filename.c_str(), image)) == 0) {
+ if ((cvSaveImage(filename.c_str(), image)) == 0) {
std::cout << " Cannot write image: " << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
+ if (image != NULL)
+ cvReleaseImage(&image);
return (-1);
}
std::cout << " Convert result in " << filename << std::endl;
@@ -450,33 +456,35 @@ main(int argc, const char ** argv)
// Convert a vpImage<unsigned char> to an IplImage
////////////////////////////////////////
std::cout << "** Convert a vpImage<unsigned char> to an IplImage" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
/* Read the grey image */
// Load a color image from the disk
std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ig, filename) ;
+ vpImageIo::read(Ig, filename);
vpImageConvert::convert(Ig, image);
filename = vpIoTools::createFilePath(opath, "Klimt_ipl_grey_cv.pgm");
/* Save the the current image */
std::cout << " Write " << filename << std::endl;
- if((cvSaveImage(filename.c_str(), image)) == 0) {
- std::cout << " Cannot write image: "<< std::endl << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
+ if ((cvSaveImage(filename.c_str(), image)) == 0) {
+ std::cout << " Cannot write image: " << std::endl << filename << std::endl;
+ if (image != NULL)
+ cvReleaseImage(&image);
return (-1);
}
std::cout << " Convert result in " << filename << std::endl;
- if(image!=NULL) cvReleaseImage( &image );
+ if (image != NULL)
+ cvReleaseImage(&image);
double t1 = vpTime::measureTimeMs();
std::cout << "== Conversion c interface : " << t1 - t0 << " ms" << std::endl;
#endif
- /* ------------------------------------------------------------------------ */
- /* conversion for the new c++ interface */
- /* ------------------------------------------------------------------------ */
+/* ------------------------------------------------------------------------ */
+/* conversion for the new c++ interface */
+/* ------------------------------------------------------------------------ */
#if VISP_HAVE_OPENCV_VERSION >= 0x020100
double t2 = vpTime::measureTimeMs();
@@ -485,65 +493,65 @@ main(int argc, const char ** argv)
////////////////////////
std::cout << "** Convert a cv::Mat to a vpImage<vpRGBa>" << std::endl;
cv::Mat imageMat;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ 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.
- if(imageMat.data == NULL){
- std::cout << " Cannot read image: "<< 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;
}
vpImageConvert::convert(imageMat, Ic);
filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
+ vpImageIo::write(Ic, filename);
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
/* Read the pgm image */
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;
+ imageMat = cv::imread(filename, 0); // forced to grayscale.
+ if (imageMat.data == NULL) {
+ std::cout << " Cannot read image: " << filename << std::endl;
return (-1);
}
vpImageConvert::convert(imageMat, Ic);
filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ic, filename) ;
+ vpImageIo::write(Ic, filename);
///////////////////////////
// Convert a cv::Mat to a vpImage<unsigned char>
////////////////////////////
std::cout << "** Convert a cv::Mat to a vpImage<nsigned char>" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ 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.
- if(imageMat.data == NULL){
+ 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;
}
vpImageConvert::convert(imageMat, Ig);
filename = vpIoTools::createFilePath(opath, "Klimt_color_cvMat.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
+ vpImageIo::write(Ig, filename);
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ 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);
- if(imageMat.data == NULL){
- std::cout << " Cannot read image: "<< filename << std::endl;
+ if (imageMat.data == NULL) {
+ std::cout << " Cannot read image: " << filename << std::endl;
return (-1);
}
vpImageConvert::convert(imageMat, Ig);
filename = vpIoTools::createFilePath(opath, "Klimt_grey_cvMat.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(Ig, filename) ;
+ vpImageIo::write(Ig, filename);
std::cout << " Convert result in " << filename << std::endl;
@@ -551,18 +559,18 @@ main(int argc, const char ** argv)
// Convert a vpImage<vpRGBa> to a cv::Mat
////////////////////////////////////
std::cout << "** Convert a vpImage<vpRGBa> to a cv::Mat" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
/* Read the color image */
// Load a color image from the disk
std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
+ vpImageIo::read(Ic, filename);
vpImageConvert::convert(Ic, imageMat);
filename = vpIoTools::createFilePath(opath, "Klimt_ipl_color_cvMat.ppm");
/* Save the the current image */
std::cout << " Resulting image saved in: " << filename << std::endl;
- if(!cv::imwrite(filename, imageMat)){
+ if (!cv::imwrite(filename, imageMat)) {
std::cout << " Cannot write image: " << filename << std::endl;
return (-1);
}
@@ -572,7 +580,7 @@ main(int argc, const char ** argv)
// Convert a vpImage<unsigned char> to a cv::Mat
////////////////////////////////////////
std::cout << "** Convert a vpImage<unsigned char> to a cv::Mat" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
/* Read the grey image */
@@ -584,8 +592,8 @@ main(int argc, const char ** argv)
/* Save the the current image */
std::cout << " Resulting image saved in: " << filename << std::endl;
- if(!cv::imwrite(filename, imageMat)){
- std::cout << " Cannot write image: "<< filename << std::endl;
+ if (!cv::imwrite(filename, imageMat)) {
+ std::cout << " Cannot write image: " << filename << std::endl;
return (-1);
}
std::cout << " Convert result in " << filename << std::endl;
@@ -598,106 +606,107 @@ main(int argc, const char ** argv)
// Split a vpImage<vpRGBa> to vpImage<unsigned char>
////////////////////////////////////
std::cout << "** Split a vpImage<vpRGBa> to vpImage<unsigned char>" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
/* Read the color image */
// Load a color image from the disk
std::cout << " Load " << filename << std::endl;
- vpImageIo::read(Ic, filename) ;
- vpImage<unsigned char> R,G,B,a;
- vpImageConvert::split(Ic, &R,NULL,&B);
- double begintime = vpTime::measureTimeMs();
- for(int i=0; i<1000;i++){
- vpImageConvert::split(Ic, &R,NULL,&B);
+ vpImageIo::read(Ic, filename);
+ vpImage<unsigned char> R, G, B, a;
+ vpImageConvert::split(Ic, &R, NULL, &B);
+ double begintime = vpTime::measureTimeMs();
+ for (int i = 0; i < 1000; i++) {
+ vpImageConvert::split(Ic, &R, NULL, &B);
}
double endtime = vpTime::measureTimeMs();
- std::cout << " Time for 1000 split (ms): " << endtime - begintime <<std::endl;
+ std::cout << " Time for 1000 split (ms): " << endtime - begintime << std::endl;
filename = vpIoTools::createFilePath(opath, "Klimt_RChannel.pgm");
/* Save the the current image */
std::cout << " Save Klimt R channel: " << filename << std::endl;
- vpImageIo::write(R, filename) ;
+ vpImageIo::write(R, filename);
- filename = vpIoTools::createFilePath(opath, "Klimt_BChannel.pgm");
+ filename = vpIoTools::createFilePath(opath, "Klimt_BChannel.pgm");
/* Save the the current image */
std::cout << " Save Klimt B channel: " << filename << std::endl;
- vpImageIo::write(B, filename) ;
+ vpImageIo::write(B, filename);
////////////////////////////////////
// Merge 4 vpImage<unsigned char> (RGBa) to vpImage<vpRGBa>
////////////////////////////////////
std::cout << "** Merge 4 vpImage<unsigned char> (RGBa) to vpImage<vpRGBa>" << std::endl;
vpImageConvert::split(Ic, &R, &G, &B, &a);
- begintime = vpTime::measureTimeMs();
+ begintime = vpTime::measureTimeMs();
vpImage<vpRGBa> I_merge;
- for(int i=0; i<1000; i++){
+ for (int i = 0; i < 1000; i++) {
vpImageConvert::merge(&R, &G, &B, &a, I_merge);
}
endtime = vpTime::measureTimeMs();
- std::cout << " Time for 1000 merge (ms): "<< endtime - begintime <<std::endl;
+ std::cout << " Time for 1000 merge (ms): " << endtime - begintime << std::endl;
- filename = vpIoTools::createFilePath(opath, "Klimt_merge.ppm");
+ filename = vpIoTools::createFilePath(opath, "Klimt_merge.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
- vpImageIo::write(I_merge, filename) ;
+ vpImageIo::write(I_merge, filename);
////////////////////////////////////
- // Convert a vpImage<vpRGBa> in RGB color space to a vpImage<vpRGBa> in HSV color
+ // Convert a vpImage<vpRGBa> in RGB color space to a vpImage<vpRGBa> in
+ // HSV color
////////////////////////////////////
- std::cout << "** Convert a vpImage<vpRGBa> in RGB color space to a vpImage<vpRGBa> in HSV color" << std::endl;
+ std::cout << "** Convert a vpImage<vpRGBa> in RGB color space to a "
+ "vpImage<vpRGBa> in HSV color"
+ << std::endl;
unsigned int size = Ic.getSize();
unsigned int w = Ic.getWidth(), h = Ic.getHeight();
unsigned char *hue = new unsigned char[size];
unsigned char *saturation = new unsigned char[size];
unsigned char *value = new unsigned char[size];
- vpImageConvert::RGBaToHSV((unsigned char *) Ic.bitmap, hue, saturation, value, size);
+ vpImageConvert::RGBaToHSV((unsigned char *)Ic.bitmap, hue, saturation, value, size);
vpImage<unsigned char> I_hue(hue, h, w);
vpImage<unsigned char> I_saturation(saturation, h, w);
vpImage<unsigned char> I_value(value, h, w);
vpImage<vpRGBa> I_HSV;
vpImageConvert::merge(&I_hue, &I_saturation, &I_value, NULL, I_HSV);
- filename = vpIoTools::createFilePath(opath, "Klimt_HSV.ppm");
+ filename = vpIoTools::createFilePath(opath, "Klimt_HSV.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_HSV, filename);
- //Check the conversion RGBa <==> HSV
+ // Check the conversion RGBa <==> HSV
double *hue2 = new double[size];
double *saturation2 = new double[size];
double *value2 = new double[size];
- vpImageConvert::RGBaToHSV((unsigned char *) Ic.bitmap, hue2, saturation2, value2, size);
+ vpImageConvert::RGBaToHSV((unsigned char *)Ic.bitmap, hue2, saturation2, value2, size);
- unsigned char *rgba = new unsigned char[size*4];
+ unsigned char *rgba = new unsigned char[size * 4];
vpImageConvert::HSVToRGBa(hue2, saturation2, value2, rgba, size);
- if(hue2 != NULL) {
+ if (hue2 != NULL) {
delete[] hue2;
hue2 = NULL;
}
- if(saturation2 != NULL) {
+ if (saturation2 != NULL) {
delete[] saturation2;
saturation2 = NULL;
}
- if(value2 != NULL) {
+ if (value2 != NULL) {
delete[] value2;
value2 = NULL;
}
- vpImage<vpRGBa> I_HSV2RGBa((vpRGBa *) rgba, h, w);
- filename = vpIoTools::createFilePath(opath, "Klimt_HSV2RGBa.ppm");
+ vpImage<vpRGBa> I_HSV2RGBa((vpRGBa *)rgba, h, w);
+ filename = vpIoTools::createFilePath(opath, "Klimt_HSV2RGBa.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_HSV2RGBa, filename);
- for(unsigned int i = 0; i < Ic.getHeight(); i++) {
- for(unsigned int j = 0; j < Ic.getWidth(); j++) {
- if(Ic[i][j].R != I_HSV2RGBa[i][j].R ||
- Ic[i][j].G != I_HSV2RGBa[i][j].G ||
- Ic[i][j].B != I_HSV2RGBa[i][j].B) {
+ for (unsigned int i = 0; i < Ic.getHeight(); i++) {
+ for (unsigned int j = 0; j < Ic.getWidth(); j++) {
+ if (Ic[i][j].R != I_HSV2RGBa[i][j].R || Ic[i][j].G != I_HSV2RGBa[i][j].G || Ic[i][j].B != I_HSV2RGBa[i][j].B) {
std::cerr << "Ic[i][j].R=" << static_cast<unsigned>(Ic[i][j].R)
<< " ; I_HSV2RGBa[i][j].R=" << static_cast<unsigned>(I_HSV2RGBa[i][j].R) << std::endl;
std::cerr << "Ic[i][j].G=" << static_cast<unsigned>(Ic[i][j].G)
@@ -713,28 +722,29 @@ 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;
- unsigned char *rgba2 = new unsigned char[size*4];
- memset(rgba2, 127, size*4);
- vpImage<vpRGBa> I_copyData((vpRGBa *) rgba2, h, w, true);
+ unsigned char *rgba2 = new unsigned char[size * 4];
+ memset(rgba2, 127, size * 4);
+ vpImage<vpRGBa> I_copyData((vpRGBa *)rgba2, h, w, true);
// Delete the array
delete[] rgba2;
- filename = vpIoTools::createFilePath(opath, "I_copyData.ppm");
+ filename = vpIoTools::createFilePath(opath, "I_copyData.ppm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_copyData, filename);
- if(I_copyData.getSize() > 0) {
+ if (I_copyData.getSize() > 0) {
I_copyData[0][0].R = 10;
}
-
- //Benchmark and test RGBa / RGB / cv::Mat to Grayscale conversion
+ // Benchmark and test RGBa / RGB / cv::Mat to Grayscale conversion
{
- std::cout << "** Benchmark and test RGBa / RGB / cv::Mat to Grayscale conversion" << std::endl;
- //RGBa to Grayscale
+ std::cout << "** Benchmark and test RGBa / RGB / cv::Mat to Grayscale "
+ "conversion"
+ << std::endl;
+ // RGBa to Grayscale
vpImage<vpRGBa> I_color;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
vpImageIo::read(I_color, filename);
vpImage<unsigned char> I_gray_sse(I_color.getHeight(), I_color.getWidth());
@@ -742,23 +752,23 @@ main(int argc, const char ** argv)
unsigned char value_sse = 0, value_regular = 0;
double t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
vpImageConvert::convert(I_color, I_gray_sse);
value_sse += I_gray_sse[0][0];
}
t_sse = vpTime::measureTimeMs() - t_sse;
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());
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
+ 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;
- //Compute the error between the SSE and regular version
+ // Compute the error between the SSE and regular version
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++) {
+ 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]);
}
}
@@ -766,31 +776,32 @@ main(int argc, const char ** argv)
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 << " 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 << " 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;
- //To prevent the iteration loop to not be optimized?
+ // To prevent the iteration loop to not be optimized?
std::cout << " value_sse=" << static_cast<unsigned>(value_sse)
<< " ; value_regular=" << static_cast<unsigned>(value_regular) << std::endl;
- filename = vpIoTools::createFilePath(opath, "I_rgba2gray_sse.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_rgba2gray_sse.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_gray_sse, filename);
- filename = vpIoTools::createFilePath(opath, "I_rgba2gray_regular.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_rgba2gray_regular.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_gray_regular, filename);
- //RGB to Grayscale conversion
+ // 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()];
t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
vpImageConvert::RGBToGrey(rgb_array, rgb2gray_array_sse, I_color.getWidth(), I_color.getHeight(), false);
value_sse += rgb2gray_array_sse[0];
}
@@ -798,45 +809,48 @@ main(int argc, const char ** argv)
unsigned char *rgb2gray_array_regular = new unsigned char[I_color.getSize()];
t_regular = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
computeRegularRGBToGrayscale(rgb_array, rgb2gray_array_regular, I_color.getSize());
value_regular += rgb2gray_array_regular[0];
}
t_regular = vpTime::measureTimeMs() - t_regular;
vpImage<unsigned char> I_gray2rgba_sse(rgb2gray_array_sse, I_color.getHeight(), I_color.getWidth(), false);
- vpImage<unsigned char> I_gray2rgba_regular(rgb2gray_array_regular, I_color.getHeight(), I_color.getWidth(), false);
+ vpImage<unsigned char> I_gray2rgba_regular(rgb2gray_array_regular, I_color.getHeight(), I_color.getWidth(),
+ false);
- //Compute the error between the SSE and regular version
+ // 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]);
+ 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]);
}
}
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 << " 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 << " 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;
- //To prevent the iteration loop to not be optimized?
+ // To prevent the iteration loop to not be optimized?
std::cout << " value_sse=" << static_cast<unsigned>(value_sse)
<< " ; value_regular=" << static_cast<unsigned>(value_regular) << std::endl;
- filename = vpIoTools::createFilePath(opath, "I_rgb2gray_sse.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_rgb2gray_sse.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_gray2rgba_sse, filename);
- filename = vpIoTools::createFilePath(opath, "I_rgb2gray_regular.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_rgb2gray_regular.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_gray2rgba_regular, filename);
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
- //BGR cv::Mat to Grayscale
+ // BGR cv::Mat to Grayscale
std::cout << "\n BGR cv::Mat to Grayscale" << std::endl;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
cv::Mat colorMat = cv::imread(filename);
std::cout << " colorMat=" << colorMat.cols << "x" << colorMat.rows << std::endl;
@@ -845,52 +859,53 @@ main(int argc, const char ** argv)
value_regular = 0;
t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
vpImageConvert::convert(colorMat, I_mat2gray_sse, false);
value_sse += I_mat2gray_sse[0][0];
}
t_sse = vpTime::measureTimeMs() - t_sse;
t_regular = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
computeRegularBGRToGrayscale(colorMat, I_mat2gray_regular);
value_regular += I_mat2gray_sse[0][0];
}
t_regular = vpTime::measureTimeMs() - t_regular;
- //Compute the error between the SSE and regular version
+ // 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_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]) * (I_mat2gray_sse[i][j] - I_mat2gray_regular[i][j]);
+ 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]);
}
}
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 << " 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;
- //To prevent the iteration loop to not be optimized?
+ // To prevent the iteration loop to not be optimized?
std::cout << " value_sse=" << static_cast<unsigned>(value_sse)
<< " ; value_regular=" << static_cast<unsigned>(value_regular) << std::endl;
- filename = vpIoTools::createFilePath(opath, "I_mat2gray_sse.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_mat2gray_sse.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_mat2gray_sse, filename);
- filename = vpIoTools::createFilePath(opath, "I_mat2gray_regular.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_mat2gray_regular.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_mat2gray_regular, filename);
-
- //BGR cv::Mat to Grayscale cv::Mat
+ // BGR cv::Mat to Grayscale cv::Mat
std::cout << "\n BGR Mat to Grayscale Mat" << std::endl;
cv::Mat grayscaleMat(colorMat.size(), CV_8U);
unsigned char value_mat = 0;
double t_opencv = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
cv::cvtColor(colorMat, grayscaleMat, cv::COLOR_BGR2GRAY);
value_mat += grayscaleMat.ptr<uchar>(0)[0];
}
@@ -898,61 +913,58 @@ main(int argc, const char ** argv)
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 << " Speed-up=" << (t_opencv / t_sse) << "X" << std::endl;
std::cout << " value_mat=" << static_cast<unsigned>(value_mat) << std::endl;
vpImage<unsigned char> I_grayscale_mat;
vpImageConvert::convert(grayscaleMat, I_grayscale_mat);
- filename = vpIoTools::createFilePath(opath, "grayscaleMat.pgm");
+ filename = vpIoTools::createFilePath(opath, "grayscaleMat.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_grayscale_mat, filename);
-
- //Test RGB to Grayscale + Flip
+ // 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<unsigned char> I_rgb2gray_flip_sse(rgb2gray_flip_array_sse, I_color.getHeight(), I_color.getWidth());
- filename = vpIoTools::createFilePath(opath, "I_rgb2gray_flip_sse.pgm");
+ filename = vpIoTools::createFilePath(opath, "I_rgb2gray_flip_sse.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_rgb2gray_flip_sse, filename);
-
- //Test BGR to Grayscale + Flip
+ // 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<unsigned char> 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");
+ filename = vpIoTools::createFilePath(opath, "I_bgr2gray_flip_sse.pgm");
std::cout << " Resulting image saved in: " << filename << std::endl;
vpImageIo::write(I_bgr2gray_flip_sse, filename);
-
- //Test RGB to Grayscale + Flip + Crop
+ // Test RGB to Grayscale + Flip + Crop
std::cout << "\n RGB to Grayscale + Flip + Crop" << std::endl;
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? " << 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<vpRGBa> 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: " << 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<vpRGBa> 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");
+ 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(),
@@ -960,38 +972,36 @@ main(int argc, const char ** argv)
vpImage<unsigned char> 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
+ // Test BGR to Grayscale + Flip + Crop
std::cout << "\n BGR to Grayscale + Flip + Crop" << std::endl;
vpImage<unsigned char> 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
+ // Test BGR to Grayscale + Flip + Crop + No continuous Mat
std::cout << "\n BGR to Grayscale + Flip + Crop + No continuous Mat" << std::endl;
- vpImage<unsigned char> I_bgr2gray_flip_crop_no_continuous_sse(I_color_crop.getHeight(),
- I_color_crop.getWidth());
+ vpImage<unsigned char> 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);
+
+ delete[] rgb_array_crop;
#endif
- delete [] rgb_array;
+ delete[] rgb_array;
std::cout << "Test succeed" << std::endl;
}
return 0;
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return 1;
}
diff --git a/modules/core/test/image/testCrop.cpp b/modules/core/test/image/testCrop.cpp
index c0f9283..dfa8f25 100644
--- a/modules/core/test/image/testCrop.cpp
+++ b/modules/core/test/image/testCrop.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,13 @@
*
*****************************************************************************/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpRect.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpDebug.h>
#include <stdlib.h>
@@ -56,10 +57,7 @@
*/
// 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);
+#define GETOPTARGS "cdi:o:h"
/*
@@ -72,7 +70,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\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, 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\
@@ -87,7 +86,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -100,8 +99,7 @@ OPTIONS: Default\n\
Klimt_cropped.pgm output image is written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -119,23 +117,32 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -150,8 +157,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -162,14 +168,15 @@ main(int argc, const char ** argv)
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -181,7 +188,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -198,11 +205,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -213,8 +218,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -222,14 +226,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
@@ -240,9 +243,9 @@ main(int argc, const char ** argv)
vpImage<unsigned char> C; // Cropped output image
// Read the input grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
// Specify the cropping area
vpRect roi;
@@ -257,10 +260,9 @@ main(int argc, const char ** argv)
// Write the cropped image on the disk
filename = vpIoTools::createFilePath(opath, "Klimt_crop.pgm");
std::cout << "Write cropped image: " << filename << std::endl;
- vpImageIo::write(C, filename) ;
+ vpImageIo::write(C, filename);
return 0;
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return 1;
}
diff --git a/modules/core/test/image/testCropAdvanced.cpp b/modules/core/test/image/testCropAdvanced.cpp
index 1de4ce1..cb99bc3 100644
--- a/modules/core/test/image/testCropAdvanced.cpp
+++ b/modules/core/test/image/testCropAdvanced.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,11 @@
*
*****************************************************************************/
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-
/*!
\example testCropAdvanced.cpp
@@ -51,10 +51,7 @@
*/
// 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);
+#define GETOPTARGS "cdi:o:h"
/*
@@ -67,7 +64,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\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, 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\
@@ -80,7 +78,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -93,8 +91,7 @@ OPTIONS: Default\n\
resulting images are written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -112,23 +109,32 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -143,7 +149,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -153,14 +159,15 @@ int main(int argc, const char ** argv)
std::string opath;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -172,7 +179,7 @@ int main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -189,11 +196,9 @@ int main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -204,8 +209,7 @@ int main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -213,28 +217,26 @@ int main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
- if (1){
+ if (1) {
vpImage<unsigned char> I;
// Read the input grey image from the disk
- std::string filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ std::string filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl;
- std::vector< vpImage<unsigned char> > reference;
+ std::vector<vpImage<unsigned char> > reference;
vpImage<unsigned char> crop;
vpImageTools::crop(I, 100, 100, 640, 640, crop);
@@ -311,18 +313,18 @@ int main(int argc, const char ** argv)
}
}
- if (1){
+ if (1) {
vpImage<vpRGBa> I;
// Read the input color image from the disk
- std::string filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ std::string filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl;
vpImage<vpRGBa> crop;
- std::vector< vpImage<vpRGBa> > reference;
+ std::vector<vpImage<vpRGBa> > reference;
vpImageTools::crop(I, 100, 100, 640, 640, crop);
reference.push_back(crop);
@@ -400,8 +402,7 @@ int main(int argc, const char ** argv)
std::cout << "Test succeed" << std::endl;
return 0;
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return -1;
}
diff --git a/modules/core/test/image/testImageAddSub.cpp b/modules/core/test/image/testImageAddSub.cpp
index 55b8308..be5c934 100644
--- a/modules/core/test/image/testImageAddSub.cpp
+++ b/modules/core/test/image/testImageAddSub.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,12 @@
*
*****************************************************************************/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpDebug.h>
/*!
\example testImageAddSub.cpp
@@ -49,10 +50,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:n:h"
-
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user, int nbiter);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user, int &nbiter);
+#define GETOPTARGS "cdi:o:n:h"
/*
Print the program options.
@@ -64,7 +62,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\param user : Username.
\param nbiter : Number of benchmark iterations.
*/
-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, const std::string &ipath, const std::string &opath,
+ const std::string &user, int nbiter)
{
fprintf(stdout, "\n\
Test images addition / substraction.\n\
@@ -78,7 +77,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -94,8 +93,7 @@ OPTIONS: Default\n\
Set the number of benchmark iterations.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str(), nbiter);
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str(), nbiter);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -112,24 +110,36 @@ 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, 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;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'n': nbiter = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, opath, user, nbiter); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'n':
+ nbiter = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user, nbiter);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user, nbiter); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user, nbiter);
+ return false;
+ break;
}
}
@@ -144,46 +154,47 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-void regularImageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires, const bool saturate) {
+void regularImageAdd(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, vpImage<unsigned char> &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"));
+ throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
Ires.resize(I1.getHeight(), I1.getWidth());
}
- unsigned char *ptr_I1 = I1.bitmap;
- unsigned char *ptr_I2 = I2.bitmap;
+ unsigned char *ptr_I1 = I1.bitmap;
+ 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<unsigned char>( (short int) *ptr_I1 + (short int) *ptr_I2 ) : *ptr_I1 + *ptr_I2;
+ *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((short int)*ptr_I1 + (short int)*ptr_I2) : *ptr_I1 + *ptr_I2;
}
}
void regularImageSubtract(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- vpImage<unsigned char> &Ires, const bool saturate) {
+ vpImage<unsigned char> &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"));
+ throw(vpException(vpException::dimensionError, "The two images do not have the same size"));
}
if ((I1.getHeight() != Ires.getHeight()) || (I1.getWidth() != Ires.getWidth())) {
Ires.resize(I1.getHeight(), I1.getWidth());
}
- unsigned char *ptr_I1 = I1.bitmap;
- unsigned char *ptr_I2 = I2.bitmap;
+ unsigned char *ptr_I1 = I1.bitmap;
+ 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<unsigned char>( (short int) *ptr_I1 - (short int) *ptr_I2 ) : *ptr_I1 - *ptr_I2;
+ *ptr_Ires = saturate ? vpMath::saturate<unsigned char>((short int)*ptr_I1 - (short int)*ptr_I2) : *ptr_I1 - *ptr_I2;
}
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -195,14 +206,15 @@ main(int argc, const char ** argv)
std::string username;
int nbIterations = 100;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -214,7 +226,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbIterations) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
// Get the option values
@@ -231,11 +243,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username, nbIterations);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(EXIT_FAILURE);
@@ -246,8 +256,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -255,14 +264,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(EXIT_FAILURE);
}
@@ -272,11 +280,10 @@ main(int argc, const char ** argv)
vpImage<unsigned char> I; // Input image
// Read the input grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read image: " << filename << std::endl << std::endl;
vpImageIo::read(I, filename);
-
// Test add image
vpImage<unsigned char> Inull(I.getHeight(), I.getWidth(), 0);
vpImage<unsigned char> Iadd;
@@ -296,7 +303,6 @@ main(int argc, const char ** argv)
std::cout << "In-place (Iadd == I)? " << (Iadd == I) << std::endl;
}
-
// Test subtract image
Inull = 0;
vpImage<unsigned char> Isub;
@@ -316,11 +322,10 @@ main(int argc, const char ** argv)
std::cout << "In-place (Isub == I)? " << (Isub == I) << std::endl;
}
-
// Test add image saturation
vpImage<unsigned char> I2(I.getHeight(), I.getWidth());
for (unsigned int cpt = 0; cpt < I2.getSize(); cpt++) {
- I2.bitmap[cpt] = (unsigned char) cpt;
+ I2.bitmap[cpt] = (unsigned char)cpt;
}
// No saturation
@@ -328,7 +333,8 @@ main(int argc, const char ** argv)
vpImage<unsigned char> 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;
}
@@ -337,19 +343,20 @@ main(int argc, const char ** argv)
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;
}
-
// Test subtract image saturation
// No saturation
vpImageTools::imageSubtract(I, I2, Isub, false);
vpImage<unsigned char> 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;
}
@@ -358,12 +365,12 @@ main(int argc, const char ** argv)
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;
}
-
// Benchmark
// Benchmark add no saturation
Iadd = 0;
@@ -393,7 +400,6 @@ main(int argc, const char ** argv)
std::cout << "Write: " << filename << std::endl;
vpImageIo::write(Iadd_regular, filename);
-
// Benchmark add saturation
Iadd = 0;
t_sse = vpTime::measureTimeMs();
@@ -410,7 +416,7 @@ main(int argc, const char ** argv)
}
t = vpTime::measureTimeMs() - t;
std::cout << "Add saturation ; t (" << nbIterations << " iterations)=" << t << " ms"
- << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
+ << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
std::cout << "(Iadd == Iadd_regular)? " << (Iadd == Iadd_regular) << std::endl;
// Write add no saturation
@@ -422,7 +428,6 @@ main(int argc, const char ** argv)
std::cout << "Write: " << filename << std::endl;
vpImageIo::write(Iadd_regular, filename);
-
// Benchmark subtract no saturation
Isub = I2;
t_sse = vpTime::measureTimeMs();
@@ -439,7 +444,7 @@ main(int argc, const char ** argv)
}
t = vpTime::measureTimeMs() - t;
std::cout << "Subtract regular no saturation ; t (" << nbIterations << " iterations)=" << t << " ms"
- << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
+ << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl;
// Write subtract no saturation
@@ -451,7 +456,6 @@ main(int argc, const char ** argv)
std::cout << "Write: " << filename << std::endl;
vpImageIo::write(Isub_regular, filename);
-
// Benchmark subtract saturation
Isub = I2;
t_sse = vpTime::measureTimeMs();
@@ -468,7 +472,7 @@ main(int argc, const char ** argv)
}
t = vpTime::measureTimeMs() - t;
std::cout << "Subtract saturation ; t (" << nbIterations << " iterations)=" << t << " ms"
- << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
+ << " ; Speed-up: " << (t / t_sse) << "X" << std::endl;
std::cout << "(Isub == Isub_regular)? " << (Isub == Isub_regular) << std::endl;
// Write subtract no saturation
@@ -480,7 +484,6 @@ main(int argc, const char ** argv)
std::cout << "Write: " << filename << std::endl;
vpImageIo::write(Isub_regular, filename);
-
// Invert Klimt with image cropped
if (I.getWidth() >= 411 && I.getHeight() >= 507) {
vpRect r_crop(0, 0, 411, 507);
@@ -506,8 +509,7 @@ main(int argc, const char ** argv)
}
return EXIT_SUCCESS;
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cerr << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/core/test/image/testImageBinarise.cpp b/modules/core/test/image/testImageBinarise.cpp
index 7db5caa..ff1b6d0 100644
--- a/modules/core/test/image/testImageBinarise.cpp
+++ b/modules/core/test/image/testImageBinarise.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,16 +44,15 @@
#include <visp3/core/vpImageTools.h>
-
int main()
{
std::cout << "Test vpImageTools::binarise() with different data types." << std::endl;
unsigned int width = 5, height = 4;
- unsigned char *uchar_array = new unsigned char[width*height];
- double *double_array = new double[width*height];
- vpRGBa *rgba_array = new vpRGBa[width*height];
- for(unsigned char i = 0; i < width*height; i++) {
+ unsigned char *uchar_array = new unsigned char[width * height];
+ double *double_array = new double[width * height];
+ vpRGBa *rgba_array = new vpRGBa[width * height];
+ for (unsigned char i = 0; i < width * height; i++) {
uchar_array[i] = i;
double_array[i] = i;
rgba_array[i] = vpRGBa(i, i, i, i);
@@ -63,68 +63,69 @@ int main()
vpImage<vpRGBa> I_rgba(rgba_array, height, width);
std::cout << "I:" << std::endl;
- for(unsigned int i = 0; i < I.getHeight(); i++) {
- for(unsigned int j = 0; j < I.getWidth(); j++) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
std::cout << static_cast<unsigned>(I[i][j]) << " ";
}
std::cout << std::endl;
}
std::cout << "\nI_double:" << std::endl;
- for(unsigned int i = 0; i < I_double.getHeight(); i++) {
- for(unsigned int j = 0; j < I_double.getWidth(); j++) {
+ for (unsigned int i = 0; i < I_double.getHeight(); i++) {
+ for (unsigned int j = 0; j < I_double.getWidth(); j++) {
std::cout << I_double[i][j] << " ";
}
std::cout << std::endl;
}
std::cout << "\nI_rgba:" << std::endl;
- for(unsigned int i = 0; i < I_rgba.getHeight(); i++) {
- for(unsigned int j = 0; j < I_rgba.getWidth(); j++) {
+ for (unsigned int i = 0; i < I_rgba.getHeight(); i++) {
+ for (unsigned int j = 0; j < I_rgba.getWidth(); j++) {
std::cout << static_cast<unsigned>(I_rgba[i][j].R) << " ; " << static_cast<unsigned>(I_rgba[i][j].G) << " ; "
- << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned int>(I_rgba[i][j].A) << std::endl;
+ << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned int>(I_rgba[i][j].A)
+ << std::endl;
}
std::cout << std::endl;
}
- vpImageTools::binarise(I, (unsigned char) 5, (unsigned char) 12, (unsigned char) 0, (unsigned char) 127, (unsigned char) 255);
+ vpImageTools::binarise(I, (unsigned char)5, (unsigned char)12, (unsigned char)0, (unsigned char)127,
+ (unsigned char)255);
vpImageTools::binarise(I_double, 5.0, 12.0, 0.0, 127.0, 255.0);
vpImageTools::binarise(I_rgba, vpRGBa(5), vpRGBa(12), vpRGBa(0), vpRGBa(127), vpRGBa(255));
std::cout << "\nI binarise:" << std::endl;
- for(unsigned int i = 0; i < I.getHeight(); i++) {
- for(unsigned int j = 0; j < I.getWidth(); j++) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
std::cout << static_cast<unsigned>(I[i][j]) << " ";
}
std::cout << std::endl;
}
std::cout << "\nI_double binarise:" << std::endl;
- for(unsigned int i = 0; i < I_double.getHeight(); i++) {
- for(unsigned int j = 0; j < I_double.getWidth(); j++) {
+ for (unsigned int i = 0; i < I_double.getHeight(); i++) {
+ for (unsigned int j = 0; j < I_double.getWidth(); j++) {
std::cout << I_double[i][j] << " ";
}
std::cout << std::endl;
}
std::cout << "\nI_rgba binarise:" << std::endl;
- for(unsigned int i = 0; i < I_rgba.getHeight(); i++) {
- for(unsigned int j = 0; j < I_rgba.getWidth(); j++) {
+ for (unsigned int i = 0; i < I_rgba.getHeight(); i++) {
+ for (unsigned int j = 0; j < I_rgba.getWidth(); j++) {
std::cout << static_cast<unsigned>(I_rgba[i][j].R) << " ; " << static_cast<unsigned>(I_rgba[i][j].G) << " ; "
- << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned>(I_rgba[i][j].A) << std::endl;
+ << static_cast<unsigned>(I_rgba[i][j].B) << " ; " << static_cast<unsigned>(I_rgba[i][j].A) << std::endl;
}
std::cout << std::endl;
}
-
- //Check if results are the same between iterate and LUT methods
+ // Check if results are the same between iterate and LUT methods
width = 32;
height = 8;
- unsigned char *uchar_array1 = new unsigned char[width*height];
- unsigned char *uchar_array2 = new unsigned char[width*height];
- for(unsigned int i = 0; i < 256; i++) {
- uchar_array1[i] = (unsigned char) i;
- uchar_array2[i] = (unsigned char) i;
+ unsigned char *uchar_array1 = new unsigned char[width * height];
+ unsigned char *uchar_array2 = new unsigned char[width * height];
+ for (unsigned int i = 0; i < 256; i++) {
+ uchar_array1[i] = (unsigned char)i;
+ uchar_array2[i] = (unsigned char)i;
}
vpImage<unsigned char> I_uchar1(uchar_array1, height, width);
@@ -135,24 +136,23 @@ int main()
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]) {
+ for (unsigned int i = 0; i < height; i++) {
+ for (unsigned int j = 0; j < width; j++) {
+ if (I_uchar1[i][j] != I_uchar2[i][j]) {
std::cerr << "Results are different between iterate and LUT methods !" << std::endl;
return -1;
}
}
}
-
- //Test performance between iterate and LUT methods
+ // Test performance between iterate and LUT methods
width = 640;
height = 480;
- unsigned char *uchar_array_perf_lut = new unsigned char[width*height];
- unsigned char *uchar_array_perf_iterate = new unsigned char[width*height];
- for(unsigned int i = 0; i < width*height; i++) {
- uchar_array_perf_lut[i] = (unsigned char) i;
- uchar_array_perf_iterate[i] = (unsigned char) i;
+ unsigned char *uchar_array_perf_lut = new unsigned char[width * height];
+ unsigned char *uchar_array_perf_iterate = new unsigned char[width * height];
+ for (unsigned int i = 0; i < width * height; i++) {
+ uchar_array_perf_lut[i] = (unsigned char)i;
+ uchar_array_perf_iterate[i] = (unsigned char)i;
}
vpImage<unsigned char> I_perf_lut(uchar_array_perf_lut, height, width);
@@ -160,20 +160,19 @@ int main()
unsigned int nbIterations = 100;
double t1 = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
vpImageTools::binarise(I_perf_iterate, threshold1, threshold2, value1, value2, value3, false);
}
t1 = vpTime::measureTimeMs() - t1;
std::cout << "Iterate: " << t1 << " ms for " << nbIterations << " iterations." << std::endl;
double t2 = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
vpImageTools::binarise(I_perf_lut, threshold1, threshold2, value1, value2, value3, true);
}
t2 = vpTime::measureTimeMs() - t2;
std::cout << "LUT: " << t2 << " ms for " << nbIterations << " iterations." << std::endl;
-
std::cout << "\ntestImageBinarise ok !" << std::endl;
return 0;
}
diff --git a/modules/core/test/image/testImageComparison.cpp b/modules/core/test/image/testImageComparison.cpp
index cb432e5..fd0a229 100644
--- a/modules/core/test/image/testImageComparison.cpp
+++ b/modules/core/test/image/testImageComparison.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,7 +52,6 @@
void usage(const char *name, const char *badparam, std::string ipath);
bool getOptions(int argc, const char **argv, std::string &ipath);
-
/*
Print the program options.
@@ -73,15 +73,14 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str());
+ Print the help.\n\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -102,15 +101,22 @@ bool getOptions(int argc, const char **argv, std::string &ipath)
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath); return false; break;
+ usage(argv[0], optarg_, ipath);
+ return false;
+ break;
}
}
@@ -125,7 +131,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath)
return true;
}
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
std::string opt_ipath;
@@ -133,11 +140,12 @@ int main(int argc, const char ** argv) {
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty()) {
+ if (!env_ipath.empty()) {
ipath = env_ipath;
}
@@ -146,7 +154,7 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_ipath) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -158,8 +166,7 @@ int main(int argc, const char ** argv) {
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -169,12 +176,11 @@ int main(int argc, const char ** argv) {
// Test if an input path is set
if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(EXIT_FAILURE);
}
@@ -182,8 +188,8 @@ int main(int argc, const char ** argv) {
// Here starts really the test
//
- //Load grayscale Klimt
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ // Load grayscale Klimt
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImage<unsigned char> I_Klimt1, I_Klimt2;
vpImageIo::read(I_Klimt1, filename);
@@ -196,8 +202,8 @@ int main(int argc, const char ** argv) {
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) ) {
+ // The two images should be equal
+ if (!(I_Klimt1 == I_Klimt2) || (I_Klimt1 != I_Klimt2)) {
std::stringstream ss;
ss << "\nProblem when comparing two grayscale images!\n";
ss << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) << std::endl;
@@ -206,19 +212,19 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, ss.str());
}
- //Modify I_Klimt1
- if (I_Klimt1[I_Klimt1.getHeight()/2][I_Klimt1.getWidth()/2] < 255) {
- I_Klimt1[I_Klimt1.getHeight()/2][I_Klimt1.getWidth()/2]++;
+ // Modify I_Klimt1
+ if (I_Klimt1[I_Klimt1.getHeight() / 2][I_Klimt1.getWidth() / 2] < 255) {
+ I_Klimt1[I_Klimt1.getHeight() / 2][I_Klimt1.getWidth() / 2]++;
} else {
- I_Klimt1[I_Klimt1.getHeight()/2][I_Klimt1.getWidth()/2]--;
+ I_Klimt1[I_Klimt1.getHeight() / 2][I_Klimt1.getWidth() / 2]--;
}
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;
- //The two images should be different
- if ( (I_Klimt1 == I_Klimt2) || !(I_Klimt1 != I_Klimt2) ) {
+ // The two images should be different
+ if ((I_Klimt1 == I_Klimt2) || !(I_Klimt1 != I_Klimt2)) {
std::stringstream ss;
ss << "\nProblem when comparing two grayscale images!\n";
ss << "(I_Klimt1 == I_Klimt2)=" << (I_Klimt1 == I_Klimt2) << std::endl;
@@ -227,9 +233,8 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, ss.str());
}
-
- //Load color Klimt
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ // Load color Klimt
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImage<vpRGBa> I_color_Klimt1, I_color_Klimt2;
vpImageIo::read(I_color_Klimt1, filename);
@@ -242,8 +247,8 @@ int main(int argc, const char ** argv) {
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) ) {
+ // The two images should be equal
+ 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;
@@ -252,19 +257,19 @@ int main(int argc, const char ** argv) {
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++;
+ // 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++;
} 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;
- //The two images should be different
- if ( (I_color_Klimt1 == I_color_Klimt2) || !(I_color_Klimt1 != I_color_Klimt2) ) {
+ // The two images should be different
+ 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;
@@ -273,7 +278,7 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, ss.str());
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "\nCatch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/core/test/image/testImageFilter.cpp b/modules/core/test/image/testImageFilter.cpp
index 46a6f75..ae05df3 100644
--- a/modules/core/test/image/testImageFilter.cpp
+++ b/modules/core/test/image/testImageFilter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,11 +39,11 @@
*/
#include <iostream>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
#include <opencv2/imgproc/imgproc.hpp>
@@ -51,16 +52,18 @@
// List of allowed command line options
#define GETOPTARGS "cdi:p:h"
-namespace {
- /*
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- */
- void usage(const char *name, const char *badparam, std::string ipath) {
- fprintf(stdout, "\n\
+namespace
+{
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ */
+void usage(const char *name, const char *badparam, std::string ipath)
+{
+ fprintf(stdout, "\n\
Test vpImageFilter class.\n\
\n\
SYNOPSIS\n\
@@ -68,11 +71,11 @@ namespace {
[-h]\n \
", name);
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm,\n\
+ From this path read \"Klimt/Klimt.pgm,\n\
.ppm, .jpeg and .png images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -83,68 +86,80 @@ namespace {
Example: -p /my_path_to/image.png\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str());
+ Print the help.\n\n", ipath.c_str());
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
- }
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
- /*!
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param ppath : Personal image path.
- \return false if the program has to be stopped, true otherwise.
- */
- bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath) {
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
-
- case 'c':
- case 'd':
- break;
-
- default:
- usage(argv[0], optarg_, ipath); return false; break;
- }
- }
+/*!
+ Set the program options.
- if ((c == 1) || (c == -1)) {
- // standalone param or error
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param ppath : Personal image path.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'h':
usage(argv[0], NULL, ipath);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath);
+ return false;
+ break;
}
+ }
- return true;
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
}
+ return true;
+}
+
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- bool check_results(const cv::Mat &mat, const vpImage<double> &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<double>((int) i, (int)j), I[i][j], std::numeric_limits<double>::epsilon())) {
- return false;
- }
+bool check_results(const cv::Mat &mat, const vpImage<double> &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<double>((int)i, (int)j), I[i][j], std::numeric_limits<double>::epsilon())) {
+ return false;
}
}
-
- return true;
}
+
+ return true;
+}
#endif
}
-int main(int argc, const char *argv[]) {
+int main(int argc, const char *argv[])
+{
try {
std::string env_ipath;
std::string opt_ipath;
@@ -152,16 +167,17 @@ int main(int argc, const char *argv[]) {
std::string ipath;
std::string filename;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_ppath) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
// Get the option values
@@ -172,23 +188,21 @@ int main(int argc, const char *argv[]) {
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
-
//
// Here starts really the test
//
- //Test on small images first
- vpImage<unsigned char> I(6,6);
+ // Test on small images first
+ vpImage<unsigned char> I(6, 6);
for (unsigned int i = 0; i < I.getSize(); i++) {
- I.bitmap[i] = (unsigned char) i;
+ I.bitmap[i] = (unsigned char)i;
}
std::cout << "I:\n" << I << std::endl;
@@ -216,8 +230,7 @@ int main(int argc, const char *argv[]) {
}
std::cout << "kernel_3:\n" << kernel_3 << std::endl;
-
- //Test correlation
+ // Test correlation
vpImage<double> I_correlation_1, I_correlation_2, I_correlation_3;
vpImageFilter::filter(I, I_correlation_1, kernel_1);
vpImageFilter::filter(I, I_correlation_2, kernel_2);
@@ -258,13 +271,18 @@ int main(int argc, const char *argv[]) {
cv::filter2D(matImg, matImg_correlation_3, CV_64F, mat_kernel_3);
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) << 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) << 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) << 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)
+ << 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)
+ << 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)
+ << std::endl;
#endif
-
- //Test convolution
+ // Test convolution
vpImage<double> I_convolution_1, I_convolution_2, I_convolution_3;
vpImageFilter::filter(I, I_convolution_1, kernel_1, true);
vpImageFilter::filter(I, I_convolution_2, kernel_2, true);
@@ -282,25 +300,33 @@ 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::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);
- 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::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);
- 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::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);
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) << 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) << 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) << 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)
+ << 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)
+ << 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)
+ << std::endl;
#endif
-
- //Test on real image
+ // Test on real image
if (opt_ppath.empty()) {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImageIo::read(I, filename);
} else {
filename = opt_ppath;
@@ -308,14 +334,13 @@ int main(int argc, const char *argv[]) {
printf("Image \"%s\" read successfully\n", filename.c_str());
}
-
- //Test correlation
+ // Test correlation
double t = vpTime::measureTimeMs();
vpImageFilter::filter(I, I_correlation_1, kernel_1);
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);
@@ -325,24 +350,24 @@ 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);
+ 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;
return EXIT_FAILURE;
}
- test = check_results(matImg_correlation_2, I_correlation_2, kernel_2.getRows()/2, kernel_2.getCols()/2);
+ 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;
return EXIT_FAILURE;
}
- test = check_results(matImg_correlation_3, I_correlation_3, kernel_3.getRows()/2, kernel_3.getCols()/2);
+ 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;
@@ -350,14 +375,13 @@ int main(int argc, const char *argv[]) {
}
#endif
-
- //Test convolution
+ // Test convolution
t = vpTime::measureTimeMs();
vpImageFilter::filter(I, I_convolution_1, kernel_1, true);
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)
@@ -366,24 +390,24 @@ int main(int argc, const char *argv[]) {
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);
+ 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;
return EXIT_FAILURE;
}
- test = check_results(matImg_convolution_2, I_convolution_2, kernel_2.getRows()/2, kernel_2.getCols()/2);
+ 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;
return EXIT_FAILURE;
}
- test = check_results(matImg_convolution_3, I_convolution_3, kernel_3.getRows()/2, kernel_3.getCols()/2);
+ 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;
@@ -391,14 +415,33 @@ int main(int argc, const char *argv[]) {
}
#endif
-
- //Test Sobel
+ // Test Sobel
vpMatrix kernel_sobel_x(5, 5);
- kernel_sobel_x[0][0] = 1.0; kernel_sobel_x[0][1] = 2.0; kernel_sobel_x[0][2] = 0.0; kernel_sobel_x[0][3] = -2.0; kernel_sobel_x[0][4] = -1.0;
- kernel_sobel_x[1][0] = 4.0; kernel_sobel_x[1][1] = 8.0; kernel_sobel_x[1][2] = 0.0; kernel_sobel_x[1][3] = -8.0; kernel_sobel_x[1][4] = -4.0;
- kernel_sobel_x[2][0] = 6.0; kernel_sobel_x[2][1] = 12.0; kernel_sobel_x[2][2] = 0.0; kernel_sobel_x[2][3] = -12.0; kernel_sobel_x[2][4] = -6.0;
- kernel_sobel_x[3][0] = 4.0; kernel_sobel_x[3][1] = 8.0; kernel_sobel_x[3][2] = 0.0; kernel_sobel_x[3][3] = -8.0; kernel_sobel_x[3][4] = -4.0;
- kernel_sobel_x[4][0] = 1.0; kernel_sobel_x[4][1] = 2.0; kernel_sobel_x[4][2] = 0.0; kernel_sobel_x[4][3] = -2.0; kernel_sobel_x[4][4] = -1.0;
+ kernel_sobel_x[0][0] = 1.0;
+ kernel_sobel_x[0][1] = 2.0;
+ kernel_sobel_x[0][2] = 0.0;
+ kernel_sobel_x[0][3] = -2.0;
+ kernel_sobel_x[0][4] = -1.0;
+ kernel_sobel_x[1][0] = 4.0;
+ kernel_sobel_x[1][1] = 8.0;
+ kernel_sobel_x[1][2] = 0.0;
+ kernel_sobel_x[1][3] = -8.0;
+ kernel_sobel_x[1][4] = -4.0;
+ kernel_sobel_x[2][0] = 6.0;
+ kernel_sobel_x[2][1] = 12.0;
+ kernel_sobel_x[2][2] = 0.0;
+ kernel_sobel_x[2][3] = -12.0;
+ kernel_sobel_x[2][4] = -6.0;
+ kernel_sobel_x[3][0] = 4.0;
+ kernel_sobel_x[3][1] = 8.0;
+ kernel_sobel_x[3][2] = 0.0;
+ kernel_sobel_x[3][3] = -8.0;
+ kernel_sobel_x[3][4] = -4.0;
+ kernel_sobel_x[4][0] = 1.0;
+ kernel_sobel_x[4][1] = 2.0;
+ kernel_sobel_x[4][2] = 0.0;
+ kernel_sobel_x[4][3] = -2.0;
+ kernel_sobel_x[4][4] = -1.0;
vpImage<double> I_sobel_x;
t = vpTime::measureTimeMs();
@@ -414,7 +457,9 @@ int main(int argc, const char *argv[]) {
std::cout << "Time to do cv::Sobel: " << t << " ms" << std::endl;
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) << 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)
+ << std::endl;
#endif
vpImage<double> I_double, Iu, Iv;
@@ -428,15 +473,22 @@ int main(int argc, const char *argv[]) {
cv::Mat matImg_sobel_y;
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) << std::endl;
- std::cout << "(Iv == matImg_sobel_y)? " << check_results(matImg_sobel_y, Iv, kernel_sobel_x.getRows()/2, kernel_sobel_x.getCols()/2) << std::endl;
+ std::cout << "(Iu == matImg_sobel_x)? "
+ << 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)
+ << std::endl;
#endif
-
- //Test Sobel separable filters
+ // Test Sobel separable filters
vpImage<double> I_sep_filtered;
vpColVector kernel_sep_x(5);
- kernel_sep_x[0] = 1.0; kernel_sep_x[1] = 2.0; kernel_sep_x[2] = 0.0; kernel_sep_x[3] = -2.0; kernel_sep_x[4] = -1.0;
+ kernel_sep_x[0] = 1.0;
+ kernel_sep_x[1] = 2.0;
+ kernel_sep_x[2] = 0.0;
+ kernel_sep_x[3] = -2.0;
+ kernel_sep_x[4] = -1.0;
vpColVector kernel_sep_y(5);
kernel_sep_y[0] = 1.0;
kernel_sep_y[1] = 4.0;
@@ -450,7 +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) {
@@ -458,8 +510,7 @@ int main(int argc, const char *argv[]) {
return EXIT_FAILURE;
}
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch an exception: " << e.what() << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/core/test/image/testImageMorphology.cpp b/modules/core/test/image/testImageMorphology.cpp
index 9e82e86..544fd3c 100644
--- a/modules/core/test/image/testImageMorphology.cpp
+++ b/modules/core/test/image/testImageMorphology.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,14 +43,13 @@
#include <visp3/core/vpImageMorphology.h>
#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
// List of allowed command line options
#define GETOPTARGS "cdi:n:h"
-
/*
Print the program options.
@@ -72,8 +72,8 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -81,8 +81,7 @@ OPTIONS: Default\n\
Set the number of benchmark iterations.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), nbiter);
+ Print the help.\n\n", ipath.c_str(), nbiter);
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -100,20 +99,29 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, int &nbIterations)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'n': nbIterations = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, nbIterations); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'n':
+ nbIterations = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, nbIterations);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, nbIterations); return false; break;
+ usage(argv[0], optarg_, ipath, nbIterations);
+ return false;
+ break;
}
}
@@ -128,10 +136,11 @@ bool getOptions(int argc, const char **argv, std::string &ipath, int &nbIteratio
return true;
}
-void printMatrix(const vpImage<unsigned char> &I, const std::string &name) {
+void printMatrix(const vpImage<unsigned char> &I, const std::string &name)
+{
std::cout << "\n" << name << ":" << std::endl;
- for(unsigned int i = 0; i < I.getHeight(); i++) {
- for(unsigned int j = 0; j < I.getWidth(); j++) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
std::cout << static_cast<unsigned>(I[i][j]) << " ";
}
std::cout << std::endl;
@@ -139,15 +148,17 @@ void printMatrix(const vpImage<unsigned char> &I, const std::string &name) {
}
// Erosion in the general case on grayscale images
-void generalErosion(vpImage<unsigned char> &I, vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4) {
- if(I.getSize() == 0) {
+void generalErosion(vpImage<unsigned char> &I,
+ vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4)
+{
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
const unsigned char null_value = 255;
- vpImage<unsigned char> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<unsigned char> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -156,40 +167,47 @@ void generalErosion(vpImage<unsigned char> &I, vpImageMorphology::vpConnexityTyp
}
} else {
J[i][0] = null_value;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = null_value;
}
}
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();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
for (unsigned int j = 0; j < I.getWidth(); j++) {
unsigned char min_value = null_value;
for (int k = 0; k < 5; k++) {
- min_value = std::min(min_value, *(ptr_curr_J + j + offset[k]));
+ min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = min_value;
}
}
} else {
- //CONNEXITY_8
- unsigned int offset[9] = {0, 1, 2, J.getWidth(), J.getWidth()+1, J.getWidth()+2,
- J.getWidth()*2, J.getWidth()*2 + 1, J.getWidth()*2 + 2};
+ // CONNEXITY_8
+ unsigned int offset[9] = {0,
+ 1,
+ 2,
+ J.getWidth(),
+ J.getWidth() + 1,
+ J.getWidth() + 2,
+ J.getWidth() * 2,
+ J.getWidth() * 2 + 1,
+ J.getWidth() * 2 + 2};
for (unsigned int i = 0; i < I.getHeight(); i++) {
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
for (unsigned int j = 0; j < I.getWidth(); j++) {
unsigned char min_value = null_value;
for (int k = 0; k < 9; k++) {
- min_value = std::min(min_value, *(ptr_curr_J + j + offset[k]));
+ min_value = (std::min)(min_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = min_value;
@@ -198,16 +216,18 @@ void generalErosion(vpImage<unsigned char> &I, vpImageMorphology::vpConnexityTyp
}
}
-//Dilatation in the general case on grayscale images
-void generalDilatation(vpImage<unsigned char> &I, vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4) {
- if(I.getSize() == 0) {
+// Dilatation in the general case on grayscale images
+void generalDilatation(vpImage<unsigned char> &I,
+ vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4)
+{
+ if (I.getSize() == 0) {
std::cerr << "Input image is empty!" << std::endl;
return;
}
const unsigned char null_value = 0;
- vpImage<unsigned char> J(I.getHeight()+2, I.getWidth()+2);
+ vpImage<unsigned char> J(I.getHeight() + 2, I.getWidth() + 2);
// Copy I to J and add border
for (unsigned int i = 0; i < J.getHeight(); i++) {
if (i == 0 || i == J.getHeight() - 1) {
@@ -216,40 +236,47 @@ void generalDilatation(vpImage<unsigned char> &I, vpImageMorphology::vpConnexity
}
} else {
J[i][0] = null_value;
- memcpy(J[i]+1, I[i-1], sizeof(unsigned char)*I.getWidth());
+ memcpy(J[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
J[i][J.getWidth() - 1] = null_value;
}
}
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();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
for (unsigned int j = 0; j < I.getWidth(); j++) {
unsigned char max_value = null_value;
for (int k = 0; k < 5; k++) {
- max_value = std::max(max_value, *(ptr_curr_J + j + offset[k]));
+ max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = max_value;
}
}
} else {
- //CONNEXITY_8
- unsigned int offset[9] = {0, 1, 2, J.getWidth(), J.getWidth()+1, J.getWidth()+2,
- J.getWidth()*2, J.getWidth()*2 + 1, J.getWidth()*2 + 2};
+ // CONNEXITY_8
+ unsigned int offset[9] = {0,
+ 1,
+ 2,
+ J.getWidth(),
+ J.getWidth() + 1,
+ J.getWidth() + 2,
+ J.getWidth() * 2,
+ J.getWidth() * 2 + 1,
+ J.getWidth() * 2 + 2};
for (unsigned int i = 0; i < I.getHeight(); i++) {
- unsigned char *ptr_curr_J = J.bitmap + i*J.getWidth();
- unsigned char *ptr_curr_I = I.bitmap + i*I.getWidth();
+ unsigned char *ptr_curr_J = J.bitmap + i * J.getWidth();
+ unsigned char *ptr_curr_I = I.bitmap + i * I.getWidth();
for (unsigned int j = 0; j < I.getWidth(); j++) {
unsigned char max_value = null_value;
for (int k = 0; k < 9; k++) {
- max_value = std::max(max_value, *(ptr_curr_J + j + offset[k]));
+ max_value = (std::max)(max_value, *(ptr_curr_J + j + offset[k]));
}
*(ptr_curr_I + j) = max_value;
@@ -258,18 +285,19 @@ void generalDilatation(vpImage<unsigned char> &I, vpImageMorphology::vpConnexity
}
}
-//Generate a magic square matrix to get a consistent grayscale image
-void magicSquare(vpImage<unsigned char> &magic_square, const int N) {
- magic_square.resize((unsigned int) N, (unsigned int) N, 0);
+// Generate a magic square matrix to get a consistent grayscale image
+void magicSquare(vpImage<unsigned char> &magic_square, const int N)
+{
+ magic_square.resize((unsigned int)N, (unsigned int)N, 0);
int n = 1;
- int i = 0, j = N/2;
+ int i = 0, j = N / 2;
- while (n <= N*N) {
+ while (n <= N * N) {
magic_square[i][j] = vpMath::saturate<unsigned char>(n);
n++;
- int newi = vpMath::modulo((i-1), N), newj = vpMath::modulo((j+1), N);
+ int newi = vpMath::modulo((i - 1), N), newj = vpMath::modulo((j + 1), N);
if (magic_square[newi][newj]) {
i++;
@@ -280,7 +308,8 @@ void magicSquare(vpImage<unsigned char> &magic_square, const int N) {
}
}
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
std::string opt_ipath;
@@ -288,16 +317,17 @@ int main(int argc, const char ** argv) {
std::string filename;
int nbIterations = 100;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, nbIterations) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
// Get the option values
@@ -308,8 +338,7 @@ int main(int argc, const char ** argv) {
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -317,59 +346,49 @@ int main(int argc, const char ** argv) {
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, nbIterations);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " 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<unsigned char> I(image_data, 8, 16, true);
printMatrix(I, "I");
-
- //Regular code
- //Dilatation
+ // Regular code
+ // Dilatation
vpImage<unsigned char> I_dilatation1 = I;
vpImage<unsigned char> 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");
-
- //Erosion
+ // Erosion
vpImage<unsigned char> I_erosion1 = I_dilatation1;
vpImage<unsigned char> 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");
-
- //SSE code
- //Dilatation
+ // SSE code
+ // Dilatation
vpImage<unsigned char> I_dilatation1_sse = I;
vpImage<unsigned char> I_dilatation2_sse = I;
@@ -389,8 +408,7 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I_dilatation2 != I_dilatation2_sse)");
}
-
- //Erosion
+ // Erosion
vpImage<unsigned char> I_erosion1_sse = I_dilatation1_sse;
vpImage<unsigned char> I_erosion2_sse = I_dilatation2_sse;
@@ -410,18 +428,12 @@ int main(int argc, const char ** argv) {
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
- };
+ // 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};
vpImage<unsigned char> I_check_dilated1(image_data_dilated1, 8, 16, true);
@@ -429,17 +441,11 @@ int main(int argc, const char ** argv) {
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
- };
+ 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};
vpImage<unsigned char> I_check_dilated2(image_data_dilated2, 8, 16, true);
@@ -447,17 +453,11 @@ int main(int argc, const char ** argv) {
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
- };
+ 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};
vpImage<unsigned char> I_check_eroded1(image_data_eroded1, 8, 16, true);
@@ -465,17 +465,11 @@ int main(int argc, const char ** argv) {
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
- };
+ 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};
vpImage<unsigned char> I_check_eroded2(image_data_eroded2, 8, 16, true);
@@ -483,14 +477,12 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I_check_eroded2 != I_erosion2_sse)");
}
-
-
- //Morphology on grayscale images
+ // Morphology on grayscale images
vpImage<unsigned char> I_magic_square;
magicSquare(I_magic_square, 17);
printMatrix(I_magic_square, "I_magic_square");
- //Dilatation CONNEXITY_4 grayscale
+ // Dilatation CONNEXITY_4 grayscale
vpImage<unsigned char> I_magic_square_dilatation1 = I_magic_square;
vpImage<unsigned char> I_magic_square_dilatation1_sse = I_magic_square;
@@ -501,7 +493,7 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I_magic_square_dilatation1 != I_magic_square_dilatation1_sse)");
}
- //Dilatation CONNEXITY_8 grayscale
+ // Dilatation CONNEXITY_8 grayscale
vpImage<unsigned char> I_magic_square_dilatation2 = I_magic_square;
vpImage<unsigned char> I_magic_square_dilatation2_sse = I_magic_square;
@@ -512,7 +504,7 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I_magic_square_dilatation2 != I_magic_square_dilatation2_sse)");
}
- //Erosion CONNEXITY_4 grayscale
+ // Erosion CONNEXITY_4 grayscale
vpImage<unsigned char> I_magic_square_erosion1 = I_magic_square_dilatation1;
vpImage<unsigned char> I_magic_square_erosion1_sse = I_magic_square_dilatation1_sse;
@@ -523,7 +515,7 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I_magic_square_erosion1 != I_magic_square_erosion1_sse)");
}
- //Dilatation CONNEXITY_8 grayscale
+ // Dilatation CONNEXITY_8 grayscale
vpImage<unsigned char> I_magic_square_erosion2 = I_magic_square_dilatation2;
vpImage<unsigned char> I_magic_square_erosion2_sse = I_magic_square_dilatation2_sse;
@@ -534,27 +526,22 @@ int main(int argc, const char ** argv) {
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
- };
+ // 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<unsigned char> I2_check_dilated1(image_data2_dilated1, 17, 17, true);
@@ -562,26 +549,21 @@ int main(int argc, const char ** argv) {
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
- };
+ 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<unsigned char> I2_check_dilated2(image_data2_dilated2, 17, 17, true);
@@ -589,26 +571,21 @@ int main(int argc, const char ** argv) {
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
- };
+ 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<unsigned char> I2_check_eroded1(image_data2_eroded1, 17, 17, true);
@@ -618,26 +595,21 @@ int main(int argc, const char ** argv) {
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
- };
+ 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<unsigned char> I2_check_eroded2(image_data2_eroded2, 17, 17, true);
@@ -645,29 +617,26 @@ int main(int argc, const char ** argv) {
throw vpException(vpException::fatalError, "(I2_check_eroded2 != I_magic_square_erosion2_sse)");
}
-
-
-
std::cout << std::endl;
vpImage<unsigned char> I_Klimt;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImageIo::read(I_Klimt, filename);
-
- //Benchmark on binarized image (compare regular code with SSE implementation)
-
+ // Benchmark on binarized image (compare regular code with SSE
+ // implementation)
vpImage<unsigned char> I_Klimt_binarized = I_Klimt;
- vpImageTools::binarise(I_Klimt_binarized, (unsigned char) 127, (unsigned char) 127, (unsigned char) 0, (unsigned char) 1, (unsigned char) 1, true);
-
+ vpImageTools::binarise(I_Klimt_binarized, (unsigned char)127, (unsigned char)127, (unsigned char)0,
+ (unsigned char)1, (unsigned char)1, true);
- //Dilatation CONNEXITY_4
+ // Dilatation CONNEXITY_4
vpImage<unsigned char> I_Klimt_binarized_dilatation1 = I_Klimt_binarized;
vpImage<unsigned char> 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::CONNEXITY_4);
+ vpImageMorphology::dilatation(I_Klimt_binarized_dilatation1, (unsigned char)1, (unsigned char)0,
+ vpImageMorphology::CONNEXITY_4);
}
t = vpTime::measureTimeMs() - t;
@@ -677,19 +646,20 @@ int main(int argc, const char ** argv) {
}
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
-
+ 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"
+ << " ; speed-up=" << (t / t_sse) << "X" << std::endl;
- //Dilatation CONNEXITY_8
+ // Dilatation CONNEXITY_8
vpImage<unsigned char> I_Klimt_binarized_dilatation2 = I_Klimt_binarized;
vpImage<unsigned char> 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::CONNEXITY_8);
+ vpImageMorphology::dilatation(I_Klimt_binarized_dilatation2, (unsigned char)1, (unsigned char)0,
+ vpImageMorphology::CONNEXITY_8);
}
t = vpTime::measureTimeMs() - t;
@@ -699,19 +669,20 @@ int main(int argc, const char ** argv) {
}
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
+ 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"
+ << " ; speed-up=" << (t / t_sse) << "X" << std::endl;
-
- //Erosion CONNEXITY_4
+ // Erosion CONNEXITY_4
vpImage<unsigned char> I_Klimt_binarized_erosion1 = I_Klimt_binarized;
vpImage<unsigned char> I_Klimt_binarized_erosion1_sse = I_Klimt_binarized;
t = vpTime::measureTimeMs();
for (int cpt = 0; cpt < nbIterations; cpt++) {
- vpImageMorphology::erosion(I_Klimt_binarized_erosion1, (unsigned char) 1, (unsigned char) 0, vpImageMorphology::CONNEXITY_4);
+ vpImageMorphology::erosion(I_Klimt_binarized_erosion1, (unsigned char)1, (unsigned char)0,
+ vpImageMorphology::CONNEXITY_4);
}
t = vpTime::measureTimeMs() - t;
@@ -722,18 +693,18 @@ int main(int argc, const char ** argv) {
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;
-
+ << (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
+ // Erosion CONNEXITY_8
vpImage<unsigned char> I_Klimt_binarized_erosion2 = I_Klimt_binarized;
vpImage<unsigned char> I_Klimt_binarized_erosion2_sse = I_Klimt_binarized;
t = vpTime::measureTimeMs();
for (int cpt = 0; cpt < nbIterations; cpt++) {
- vpImageMorphology::erosion(I_Klimt_binarized_erosion2, (unsigned char) 1, (unsigned char) 0, vpImageMorphology::CONNEXITY_8);
+ vpImageMorphology::erosion(I_Klimt_binarized_erosion2, (unsigned char)1, (unsigned char)0,
+ vpImageMorphology::CONNEXITY_8);
}
t = vpTime::measureTimeMs() - t;
@@ -744,16 +715,14 @@ int main(int argc, const char ** argv) {
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;
-
+ << (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)
- //Benchmark on grayscale images (compare regular code with SSE implementation)
-
-
- //Dilatation CONNEXITY_4 grayscale
+ // Dilatation CONNEXITY_4 grayscale
vpImage<unsigned char> I_Klimt_dilatation1 = I_Klimt;
vpImage<unsigned char> I_Klimt_dilatation1_sse = I_Klimt;
@@ -770,12 +739,10 @@ int main(int argc, const char ** argv) {
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
-
+ << (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
+ // Dilatation CONNEXITY_8 grayscale
vpImage<unsigned char> I_Klimt_dilatation2 = I_Klimt;
vpImage<unsigned char> I_Klimt_dilatation2_sse = I_Klimt;
@@ -792,12 +759,10 @@ int main(int argc, const char ** argv) {
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
+ << (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
+ // Erosion CONNEXITY_4 grayscale
vpImage<unsigned char> I_Klimt_erosion1 = I_Klimt;
vpImage<unsigned char> I_Klimt_erosion1_sse = I_Klimt;
@@ -813,13 +778,11 @@ int main(int argc, const char ** argv) {
}
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
-
+ 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
+ // Erosion CONNEXITY_8 grayscale
vpImage<unsigned char> I_Klimt_erosion2 = I_Klimt;
vpImage<unsigned char> I_Klimt_erosion2_sse = I_Klimt;
@@ -835,20 +798,18 @@ int main(int argc, const char ** argv) {
}
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"
- << " ; speed-up=" << (t/t_sse) << "X" << std::endl;
-
+ 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
+// 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
+ // Dilatation CONNEXITY_4 grayscale
cv::Mat matImg_dilatation1;
vpImageConvert::convert(I_Klimt, matImg_dilatation1);
@@ -861,12 +822,9 @@ int main(int argc, const char ** argv) {
vpImage<unsigned char> 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
+ // Dilatation CONNEXITY_8 grayscale
cv::Mat matImg_dilatation2;
vpImageConvert::convert(I_Klimt, matImg_dilatation2);
@@ -879,11 +837,9 @@ int main(int argc, const char ** argv) {
vpImage<unsigned char> 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
+ // Erosion CONNEXITY_4 grayscale
cv::Mat matImg_erosion1;
vpImageConvert::convert(I_Klimt, matImg_erosion1);
@@ -895,13 +851,10 @@ int main(int argc, const char ** argv) {
vpImage<unsigned char> 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)
- << " ; t_opencv=" << t_opencv << " ms"
- << std::endl;
-
+ 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
+ // Erosion CONNEXITY_8 grayscale
cv::Mat matImg_erosion2;
vpImageConvert::convert(I_Klimt, matImg_erosion2);
@@ -913,14 +866,12 @@ int main(int argc, const char ** argv) {
vpImage<unsigned char> 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)
- << " ; t_opencv=" << t_opencv << " ms"
- << std::endl;
+ std::cout << "(I_matImg_erosion2 == I_Klimt_erosion2_sse)? " << (I_matImg_erosion2 == I_Klimt_erosion2_sse)
+ << " ; t_opencv=" << t_opencv << " ms" << std::endl;
#endif
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "\nCatch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/core/test/image/testImagePoint.cpp b/modules/core/test/image/testImagePoint.cpp
index e75a91d..2c84e16 100644
--- a/modules/core/test/image/testImagePoint.cpp
+++ b/modules/core/test/image/testImagePoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,8 +42,8 @@
*/
-#include <visp3/core/vpImagePoint.h>
#include <iostream>
+#include <visp3/core/vpImagePoint.h>
int main()
{
@@ -57,20 +58,15 @@ int main()
ip3.set_j(11.10001);
ip3.set_i(10.1);
- std::cout << "We define ip1 with coordinates: "
- << ip1 << std::endl;
+ std::cout << "We define ip1 with coordinates: " << ip1 << std::endl;
-
- std::cout << "We define ip2 with coordinates: "
- << ip2 << std::endl;
+ std::cout << "We define ip2 with coordinates: " << ip2 << std::endl;
- std::cout << "We define ip3 with coordinates: "
- << ip3 << std::endl;
+ std::cout << "We define ip3 with coordinates: " << ip3 << std::endl;
if (ip1 == ip2) {
std::cout << "ip1 == ip2" << std::endl;
- }
- else {
+ } else {
std::cout << "ip1 != ip2 (bad result)" << std::endl;
return -1;
}
@@ -78,27 +74,23 @@ int main()
if (ip1 != ip2) {
std::cout << "ip1 != ip2 (bad result)" << std::endl;
return -1;
- }
- else {
+ } else {
std::cout << "ip1 == ip2" << std::endl;
}
if (ip1 == ip3) {
std::cout << "ip1 == ip3 (bad result)" << std::endl;
return -1;
- }
- else {
+ } else {
std::cout << "ip1 != ip3" << std::endl;
}
if (ip1 != ip3) {
std::cout << "ip1 != ip3" << std::endl;
- }
- else {
+ } else {
std::cout << "ip1 == ip3 (bad result)" << std::endl;
return -1;
}
-
return 0;
}
diff --git a/modules/core/test/image/testImagePrint.cpp b/modules/core/test/image/testImagePrint.cpp
index f12f4b5..513f0c1 100644
--- a/modules/core/test/image/testImagePrint.cpp
+++ b/modules/core/test/image/testImagePrint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,8 @@
\brief Test image print.
*/
-int main() {
+int main()
+{
unsigned int size = 16;
vpImage<int> I_int(size, size);
vpImage<unsigned char> I_uchar(size, size);
@@ -48,9 +50,9 @@ int main() {
for (unsigned int i = 0, cpt = 0; i < size; i++) {
for (unsigned int j = 0; j < size; j++, cpt++) {
- I_int[i][j] = (int) cpt;
- I_uchar[i][j] = (unsigned char) cpt;
- I_char[i][j] = (char) cpt;
+ I_int[i][j] = (int)cpt;
+ I_uchar[i][j] = (unsigned char)cpt;
+ I_char[i][j] = (char)cpt;
}
}
@@ -61,9 +63,9 @@ int main() {
for (unsigned int i = 0, cpt = 0; i < size; i++) {
for (unsigned int j = 0; j < size; j++, cpt++) {
- I_float[i][j] = (float) sqrt((double) cpt);
- I_double[i][j] = sqrt((double) cpt);
- I_rgba[i][j] = vpRGBa( (unsigned char) cpt);
+ I_float[i][j] = (float)sqrt((double)cpt);
+ I_double[i][j] = sqrt((double)cpt);
+ I_rgba[i][j] = vpRGBa((unsigned char)cpt);
}
}
diff --git a/modules/core/test/image/testImageResize.cpp b/modules/core/test/image/testImageResize.cpp
index 62796f0..181af54 100644
--- a/modules/core/test/image/testImageResize.cpp
+++ b/modules/core/test/image/testImageResize.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -32,18 +33,18 @@
*
*****************************************************************************/
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
/*!
\example testImageResize.cpp
@@ -53,21 +54,21 @@
// List of allowed command line options
#define GETOPTARGS "cdi:W:H:m:h"
-
-namespace {
- /*
- Print the program options.
-
- \param name : Program name.
- \param badparam : Bad parameter name.
- \param ipath: Input image path.
- \param w : Resize width.
- \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)
- {
- fprintf(stdout, "\n\
+namespace
+{
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param w : Resize width.
+ \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)
+{
+ fprintf(stdout, "\n\
Test image resize.\n\
\n\
SYNOPSIS\n\
@@ -75,23 +76,23 @@ namespace {
[-h]\n \
", name);
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
\n\
- -W <width> %d\n\
+ -W <width> %u\n\
Set the new image width.\n\
\n\
- -H <height> %d\n\
+ -H <height> %u\n\
Set the new image height.\n\
\n\
- -m <method> %d\n\
+ -m <method> %d\n\
Set resize interpolation method.\n\
\n\
-c \n\
@@ -101,63 +102,77 @@ namespace {
Disable image display.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), w, h, m);
+ Print the help.\n\n", ipath.c_str(), w, h, m);
- if (badparam)
- fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
- }
-
- /*!
- Set the program options.
-
- \param argc : Command line number of parameters.
- \param argv : Array of command line parameters.
- \param ipath: Input image path.
- \param w : Resize width.
- \param h : Resize height.
- \param m : Resize interpolation method.
- \param opt_display : Do not display if set.
- \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 &opt_display, bool &opt_click)
- {
- const char *optarg_;
- int c;
- while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
-
- switch (c) {
- case 'i': ipath = optarg_; break;
- case 'W': w = (unsigned int) atoi(optarg_); break;
- case 'H': h = (unsigned int) atoi(optarg_); break;
- case 'm': method = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, w, h, method); return false; break;
-
- case 'c': opt_click = false; break;
- case 'd': opt_display = false; break;
- break;
-
- default:
- usage(argv[0], optarg_, ipath, w, h, method); return false; break;
- }
- }
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
- if ((c == 1) || (c == -1)) {
- // standalone param or error
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param w : Resize width.
+ \param h : Resize height.
+ \param m : Resize interpolation method.
+ \param opt_display : Do not display if set.
+ \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 &opt_display, bool &opt_click)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'W':
+ w = (unsigned int)atoi(optarg_);
+ break;
+ case 'H':
+ h = (unsigned int)atoi(optarg_);
+ break;
+ case 'm':
+ method = atoi(optarg_);
+ break;
+ case 'h':
usage(argv[0], NULL, ipath, w, h, method);
- std::cerr << "ERROR: " << std::endl;
- std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
return false;
+ break;
+
+ case 'c':
+ opt_click = false;
+ break;
+ case 'd':
+ opt_display = false;
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, w, h, method);
+ return false;
+ break;
}
+ }
- return true;
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, w, h, method);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
}
+
+ return true;
+}
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -170,16 +185,17 @@ main(int argc, const char ** argv)
bool opt_display = true;
bool opt_click = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, width, height, method, opt_display, opt_click) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
// Get the option values
@@ -190,8 +206,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -199,14 +214,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ 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 <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(EXIT_FAILURE);
}
@@ -216,53 +230,57 @@ main(int argc, const char ** argv)
for (int m = 0; m < 3; m++) {
std::cout << "Interpolation method: " << m << std::endl;
- vpImage<unsigned char> Itest(3,4);
+ vpImage<unsigned char> Itest(3, 4);
for (unsigned int cpt = 0; cpt < Itest.getSize(); cpt++) {
Itest.bitmap[cpt] = cpt;
}
- vpImage<unsigned char> 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);
+ vpImage<unsigned char> 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);
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;
- Itest.resize(4,4);
+ 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::vpImageInterpolationType) m);
- vpImageTools::resize(Itest_resize, Itest_resize2, Itest.getWidth(), Itest.getHeight(), (vpImageTools::vpImageInterpolationType) m);
+ 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::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;
}
- //Grayscale image
+ // Grayscale image
vpImage<unsigned char> I; // Input image
// Read the input grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read image: " << filename << std::endl;
vpImageIo::read(I, filename);
vpImage<unsigned char> 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)
+#if defined(VISP_HAVE_X11)
vpDisplayX *d1 = new vpDisplayX, *d2 = new vpDisplayX;
-#elif defined (VISP_HAVE_OPENCV)
+#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV *d1 = new vpDisplayOpenCV, *d2 = new vpDisplayOpenCV;
-#elif defined (VISP_HAVE_GTK)
+#elif defined(VISP_HAVE_GTK)
vpDisplayGTK *d1 = new vpDisplayGTK, *d2 = new vpDisplayGTK;
-#elif defined (VISP_HAVE_GDI)
+#elif defined(VISP_HAVE_GDI)
vpDisplayGDI *d1 = new vpDisplayGDI, *d2 = new vpDisplayGDI;
-#elif defined (VISP_HAVE_D3D9)
+#elif defined(VISP_HAVE_D3D9)
vpDisplayD3D *d1 = new vpDisplayD3D, *d2 = new vpDisplayD3D;
#else
std::cerr << "No display available!" << std::endl;
@@ -270,9 +288,10 @@ 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) || defined (VISP_HAVE_D3D9)
+#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);
@@ -286,30 +305,30 @@ main(int argc, const char ** argv)
}
}
-
- //Color image
+ // Color image
vpImage<vpRGBa> I_color; // Input image
// Read the input grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "\nRead image: " << filename << std::endl;
vpImageIo::read(I_color, filename);
vpImage<vpRGBa> 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)
+#if defined(VISP_HAVE_X11)
vpDisplayX *d3 = new vpDisplayX, *d4 = new vpDisplayX;
-#elif defined (VISP_HAVE_OPENCV)
+#elif defined(VISP_HAVE_OPENCV)
vpDisplayOpenCV *d3 = new vpDisplayOpenCV, *d4 = new vpDisplayOpenCV;
-#elif defined (VISP_HAVE_GTK)
+#elif defined(VISP_HAVE_GTK)
vpDisplayGTK *d3 = new vpDisplayGTK, *d4 = new vpDisplayGTK;
-#elif defined (VISP_HAVE_GDI)
+#elif defined(VISP_HAVE_GDI)
vpDisplayGDI *d3 = new vpDisplayGDI, *d4 = new vpDisplayGDI;
-#elif defined (VISP_HAVE_D3D9)
+#elif defined(VISP_HAVE_D3D9)
vpDisplayD3D *d3 = new vpDisplayD3D, *d4 = new vpDisplayD3D;
#else
std::cerr << "No display available!" << std::endl;
@@ -317,9 +336,10 @@ 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) || defined (VISP_HAVE_D3D9)
+#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);
@@ -332,18 +352,19 @@ main(int argc, const char ** argv)
}
}
-
-#if defined (VISP_HAVE_X11) || defined (VISP_HAVE_OPENCV) || defined (VISP_HAVE_GTK) || defined (VISP_HAVE_GDI) || defined (VISP_HAVE_D3D9)
+#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;
delete d3;
delete d4;
#endif
-
vpImage<vpRGBa> I_color_double, I_color_double_half;
- 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::vpImageInterpolationType) method);
+ 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::vpImageInterpolationType)method);
std::cout << "\n(I_color == I_color_double_half)? " << (I_color == I_color_double_half) << std::endl;
double root_mean_square_error = 0.0;
@@ -353,11 +374,13 @@ 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<vpRGBa> I_color_half, I_color_half_double;
- 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::vpImageInterpolationType) method);
+ 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::vpImageInterpolationType)method);
std::cout << "\n(I_color == I_color_half_double)? " << (I_color == I_color_half_double) << std::endl;
root_mean_square_error = 0.0;
@@ -367,12 +390,10 @@ 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) {
+ } catch (const vpException &e) {
std::cerr << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/core/test/image/testIoPGM.cpp b/modules/core/test/image/testIoPGM.cpp
index 2990c13..05e53fd 100644
--- a/modules/core/test/image/testIoPGM.cpp
+++ b/modules/core/test/image/testIoPGM.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,13 +37,13 @@
*
*****************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
-#include <stdio.h>
/*!
\example testIoPGM.cpp
@@ -52,10 +53,7 @@
*/
// 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);
+#define GETOPTARGS "cdi:o:h"
/*
@@ -68,7 +66,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\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, 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\
@@ -82,7 +81,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -95,8 +94,7 @@ OPTIONS: Default\n\
Klimt_grey.pgm output image is written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -114,23 +112,32 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -145,8 +152,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -157,14 +163,15 @@ main(int argc, const char ** argv)
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -176,7 +183,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -193,11 +200,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -208,8 +213,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -217,14 +221,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
@@ -233,25 +236,24 @@ main(int argc, const char ** argv)
//
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "Read image: " << filename << std::endl;
vpImageIo::read(I, filename);
// Write the content of the image on the disk
filename = vpIoTools::createFilePath(opath, "Klimt_grey.pgm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
+ vpImageIo::write(I, filename);
try {
// Try to load a non existing image (test for exceptions)
// Load a non existing grey image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.pgm");
+ filename = vpIoTools::createFilePath(ipath, "image-that-does-not-exist.pgm");
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(vpException &e) {
+ vpImageIo::read(I, filename);
+ } catch (vpException &e) {
std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
}
@@ -259,14 +261,12 @@ main(int argc, const char ** argv)
// Try to write an image to a non existing directory
filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.pgm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
- }
- catch(vpException &e) {
+ vpImageIo::write(I, filename);
+ } catch (vpException &e) {
std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/image/testIoPPM.cpp b/modules/core/test/image/testIoPPM.cpp
index 454fb53..5010d37 100644
--- a/modules/core/test/image/testIoPPM.cpp
+++ b/modules/core/test/image/testIoPPM.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,14 +37,14 @@
*
*****************************************************************************/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
/*!
\example testIoPPM.cpp
@@ -53,10 +54,7 @@
*/
// 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);
+#define GETOPTARGS "cdi:o:h"
/*
@@ -69,7 +67,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\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, 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\
@@ -83,8 +82,8 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -97,8 +96,7 @@ OPTIONS: Default\n\
are written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -116,23 +114,32 @@ 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, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -147,8 +154,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -159,14 +165,15 @@ main(int argc, const char ** argv)
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -178,7 +185,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -195,11 +202,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -210,8 +215,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -219,14 +223,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
@@ -236,62 +239,57 @@ main(int argc, const char ** argv)
/////////////////////////////////////////////////////////////////////
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Load a color image from the disk and convert it to a grey level one
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "Read image: " << filename << std::endl;
vpImageIo::read(I, filename);
// Write the content of the image on the disk
filename = vpIoTools::createFilePath(opath, "Klimt_grey.ppm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
+ vpImageIo::write(I, filename);
// Try to load a non existing image (test for exceptions)
- try
- {
+ try {
// Load a non existing grey image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/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)
- {
+ vpImageIo::read(I, filename);
+ } catch (vpImageException &e) {
vpERROR_TRACE("at main level");
- std::cout << e << std::endl ;
+ std::cout << e << std::endl;
}
// Try to write an image to a non existing directory
try {
filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(I, filename) ;
- }
- catch(vpException &e) {
+ vpImageIo::write(I, filename);
+ } catch (vpException &e) {
std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
}
/////////////////////////////////////////////////////////////////////
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "Read image: " << filename << std::endl;
vpImageIo::read(Irgba, filename);
// Write the content of the color image on the disk
filename = vpIoTools::createFilePath(opath, "Klimt_color.ppm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(Irgba, filename) ;
+ vpImageIo::write(Irgba, filename);
try {
// Try to load a non existing image (test for exceptions)
// Load a non existing color image
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/image-that-does-not-exist.ppm");
+ filename = vpIoTools::createFilePath(ipath, "image-that-does-not-exist.ppm");
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(Irgba, filename) ;
- }
- catch(vpException &e) {
+ vpImageIo::read(Irgba, filename);
+ } catch (vpException &e) {
std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
}
@@ -299,16 +297,13 @@ main(int argc, const char ** argv)
// Try to write a color image to a non existing directory
filename = vpIoTools::createFilePath(opath, "directory-that-does-not-exist/Klimt.ppm");
std::cout << "Write image: " << filename << std::endl;
- vpImageIo::write(Irgba, filename) ;
- }
- catch(vpException &e) {
+ vpImageIo::write(Irgba, filename);
+ } catch (vpException &e) {
std::cout << "Catch an exception due to a non existing file: " << e << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/modules/core/test/image/testPerformanceLUT.cpp b/modules/core/test/image/testPerformanceLUT.cpp
index fce117d..722caf7 100644
--- a/modules/core/test/image/testPerformanceLUT.cpp
+++ b/modules/core/test/image/testPerformanceLUT.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,13 @@
*
*****************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMath.h>
-#include <stdlib.h>
-#include <stdio.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
/*!
\example testPerformanceLUT.cpp
@@ -51,7 +52,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:t:h"
+#define GETOPTARGS "cdi:o:t:h"
/*
Print the program options.
@@ -63,7 +64,8 @@
\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, 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\
@@ -77,7 +79,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -93,8 +95,7 @@ OPTIONS: Default\n\
Set the number of threads to use for the computation.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -113,25 +114,36 @@ 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, unsigned int &nbThreads)
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, const std::string &user,
+ unsigned int &nbThreads)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 't': nbThreads = (unsigned int) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 't':
+ nbThreads = (unsigned int)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -147,56 +159,59 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
}
/*!
- Iterate over pixels using raw pointer and adjust the pixel intensities with the formula:
- new_intensity = old_intensitie * alpha + beta.
+ Iterate over pixels using raw pointer and adjust the pixel intensities with
+ the formula: new_intensity = old_intensitie * alpha + beta.
\param I : Input color image.
\param alpha : Gain.
\param beta: Offset.
*/
-void iterate_method1(vpImage<vpRGBa> &I, const double alpha, const double beta) {
+void iterate_method1(vpImage<vpRGBa> &I, const double alpha, const double beta)
+{
unsigned int size = I.getWidth() * I.getHeight();
- unsigned char *ptrStart = (unsigned char*) I.bitmap;
- unsigned char *ptrEnd = ptrStart + size*4;
+ unsigned char *ptrStart = (unsigned char *)I.bitmap;
+ unsigned char *ptrEnd = ptrStart + size * 4;
unsigned char *ptrCurrent = ptrStart;
- while(ptrCurrent != ptrEnd) {
+ while (ptrCurrent != ptrEnd) {
*ptrCurrent = vpMath::saturate<unsigned char>((*ptrCurrent) * alpha + beta);
++ptrCurrent;
}
}
/*!
- Iterate over pixels using raw pointer and adjust the pixel intensities with the formula:
- new_intensity = old_intensitie * alpha + beta.
+ Iterate over pixels using raw pointer and adjust the pixel intensities with
+ the formula: new_intensity = old_intensitie * alpha + beta.
\param I : Input grayscale image.
\param alpha : Gain.
\param beta: Offset.
*/
-void iterate_method1(vpImage<unsigned char> &I, const double alpha, const double beta) {
+void iterate_method1(vpImage<unsigned char> &I, const double alpha, const double beta)
+{
unsigned int size = I.getWidth() * I.getHeight();
- unsigned char *ptrStart = (unsigned char*) I.bitmap;
+ unsigned char *ptrStart = (unsigned char *)I.bitmap;
unsigned char *ptrEnd = ptrStart + size;
unsigned char *ptrCurrent = ptrStart;
- while(ptrCurrent != ptrEnd) {
+ while (ptrCurrent != ptrEnd) {
*ptrCurrent = vpMath::saturate<unsigned char>((*ptrCurrent) * alpha + beta);
++ptrCurrent;
}
}
/*!
- Iterate over pixels using a double for loop and adjust the pixel intensities with the formula:
- new_intensity = old_intensitie * alpha + beta.
+ Iterate over pixels using a double for loop and adjust the pixel intensities
+ with the formula: new_intensity = old_intensitie * alpha + beta.
\param I : Input color image.
\param alpha : Gain.
\param beta: Offset.
*/
-void iterate_method2(vpImage<vpRGBa> &I, const double alpha, const double beta) {
- for(unsigned int i = 0; i < I.getHeight(); i++) {
- for(unsigned int j = 0; j < I.getWidth(); j++) {
+void iterate_method2(vpImage<vpRGBa> &I, const double alpha, const double beta)
+{
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j].R = vpMath::saturate<unsigned char>(I[i][j].R * alpha + beta);
I[i][j].G = vpMath::saturate<unsigned char>(I[i][j].G * alpha + beta);
I[i][j].B = vpMath::saturate<unsigned char>(I[i][j].B * alpha + beta);
@@ -205,8 +220,7 @@ void iterate_method2(vpImage<vpRGBa> &I, const double alpha, const double beta)
}
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -218,14 +232,15 @@ main(int argc, const char ** argv)
std::string username;
unsigned int nbThreads = 4;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -237,7 +252,7 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username, nbThreads) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -254,11 +269,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -269,8 +282,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -278,18 +290,16 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
//
// Here starts really the test
//
@@ -298,7 +308,7 @@ main(int argc, const char ** argv)
vpImage<vpRGBa> I_iterate1, I_iterate2, I_lut;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "\nRead image: " << filename << std::endl;
vpImageIo::read(I_iterate1, filename);
vpImageIo::read(I_iterate2, filename);
@@ -309,38 +319,36 @@ main(int argc, const char ** argv)
double alpha = 1.5, beta = -30.0;
unsigned int nbIterations = 10;
- //Iterate method 1
+ // Iterate method 1
double t_iterate1 = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
iterate_method1(I_iterate1, alpha, beta);
}
t_iterate1 = vpTime::measureTimeMs() - t_iterate1;
std::cout << "t_iterate1=" << t_iterate1 << " ms ; t_iterate1/" << nbIterations << "="
- << (t_iterate1/nbIterations) << " ms" << std::endl;
+ << (t_iterate1 / nbIterations) << " ms" << std::endl;
filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate1.ppm");
vpImageIo::write(I_iterate1, filename);
-
- //Iterate method 2
+ // Iterate method 2
double t_iterate2 = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
iterate_method2(I_iterate2, alpha, beta);
}
t_iterate2 = vpTime::measureTimeMs() - t_iterate2;
std::cout << "t_iterate2=" << t_iterate2 << " ms ; t_iterate2/" << nbIterations << "="
- << (t_iterate2/nbIterations) << " ms" << std::endl;
+ << (t_iterate2 / nbIterations) << " ms" << std::endl;
filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate2.ppm");
vpImageIo::write(I_iterate2, filename);
-
- //LUT method
+ // LUT method
double t_lut = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ // Construct the LUT
vpRGBa lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
@@ -350,60 +358,56 @@ 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);
-
- //Check results
+ // Check results
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]) {
+ 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]) {
same = false;
}
}
}
- if(!same) {
+ if (!same) {
std::cerr << "Color images are different!" << std::endl;
return -1;
}
-
- //Test LUT on grayscale image
+ // Test LUT on grayscale image
vpImage<unsigned char> I_iterate_grayscale1, I_lut_grayscale;
// Load a grayscale image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
std::cout << "\nRead image: " << filename << std::endl;
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;
-
- //Iterate method 1 on grayscale
+ // Iterate method 1 on grayscale
double t_iterate_grayscale1 = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
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 << "="
- << (t_iterate_grayscale1/nbIterations) << " ms" << std::endl;
+ << (t_iterate_grayscale1 / nbIterations) << " ms" << std::endl;
filename = vpIoTools::createFilePath(opath, "Klimt_performance_iterate1_grayscale.pgm");
vpImageIo::write(I_iterate_grayscale1, filename);
-
- //LUT method on grayscale
+ // LUT method on grayscale
double t_lut_grayscale = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations; cpt++) {
+ // Construct the LUT
unsigned char lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i] = vpMath::saturate<unsigned char>(alpha * i + beta);
}
@@ -411,36 +415,34 @@ main(int argc, const char ** argv)
}
t_lut_grayscale = vpTime::measureTimeMs() - t_lut_grayscale;
std::cout << "t_lut_grayscale=" << t_lut_grayscale << " ms ; t_lut_grayscale/" << nbIterations << "="
- << (t_lut_grayscale/nbIterations) << " ms" << std::endl;
+ << (t_lut_grayscale / nbIterations) << " ms" << std::endl;
filename = vpIoTools::createFilePath(opath, "Klimt_performance_lut_grayscale.pgm");
vpImageIo::write(I_lut_grayscale, filename);
-
- //Check grayscale image
+ // Check grayscale image
same = true;
- for(unsigned int i = 0; i < I_lut_grayscale.getHeight() && same; i++) {
- for(unsigned int j = 0; j < I_lut_grayscale.getWidth() && same; j++) {
- if(I_lut_grayscale[i][j] != I_iterate_grayscale1[i][j]) {
+ for (unsigned int i = 0; i < I_lut_grayscale.getHeight() && same; i++) {
+ for (unsigned int j = 0; j < I_lut_grayscale.getWidth() && same; j++) {
+ if (I_lut_grayscale[i][j] != I_iterate_grayscale1[i][j]) {
same = false;
}
}
}
- if(!same) {
+ if (!same) {
std::cerr << "Grayscale images are different!" << std::endl;
return -1;
}
-
- //Computation time on color image
+ // Computation time on color image
vpImageIo::read(I_lut, filename);
double t_lut_multithread = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations*10; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations * 10; cpt++) {
+ // Construct the LUT
vpRGBa lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
@@ -451,14 +453,13 @@ main(int argc, const char ** argv)
}
t_lut_multithread = vpTime::measureTimeMs() - t_lut_multithread;
-
vpImageIo::read(I_lut, filename);
double t_lut_singlethread = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations*10; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations * 10; cpt++) {
+ // Construct the LUT
vpRGBa lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
lut[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
@@ -469,18 +470,17 @@ 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
+ // Computation time on grayscale image
vpImageIo::read(I_lut_grayscale, filename);
t_lut_multithread = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations*10; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations * 10; cpt++) {
+ // Construct the LUT
unsigned char lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i] = vpMath::saturate<unsigned char>(alpha * i + beta);
}
@@ -488,14 +488,13 @@ main(int argc, const char ** argv)
}
t_lut_multithread = vpTime::measureTimeMs() - t_lut_multithread;
-
vpImageIo::read(I_lut_grayscale, filename);
t_lut_singlethread = vpTime::measureTimeMs();
- for(unsigned int cpt = 0; cpt < nbIterations*10; cpt++) {
- //Construct the LUT
+ for (unsigned int cpt = 0; cpt < nbIterations * 10; cpt++) {
+ // Construct the LUT
unsigned char lut[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut[i] = vpMath::saturate<unsigned char>(alpha * i + beta);
}
@@ -503,24 +502,22 @@ 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
+ // Check performLut with multithreading and image size not divisible by 8
vpImage<unsigned char> I_test_grayscale(49, 7);
- //Construct the LUT
+ // Construct the LUT
unsigned char lut_grayscale[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut_grayscale[i] = vpMath::saturate<unsigned char>(alpha * i + beta);
}
I_test_grayscale.performLut(lut_grayscale, nbThreads);
vpImage<vpRGBa> I_test_color(49, 7);
- //Construct the LUT
+ // Construct the LUT
vpRGBa lut_color[256];
- for(unsigned int i = 0; i < 256; i++) {
+ for (unsigned int i = 0; i < 256; i++) {
lut_color[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
lut_color[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
lut_color[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
@@ -528,10 +525,8 @@ main(int argc, const char ** argv)
}
I_test_color.performLut(lut_color, nbThreads);
-
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch an exception: " << e.what() << std::endl;
return 1;
}
diff --git a/modules/core/test/image/testReadImage.cpp b/modules/core/test/image/testReadImage.cpp
index 081bff9..d17eb1f 100644
--- a/modules/core/test/image/testReadImage.cpp
+++ b/modules/core/test/image/testReadImage.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,11 +36,11 @@
*
*****************************************************************************/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpDebug.h>
#include <stdlib.h>
@@ -51,7 +52,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:p:h"
+#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);
@@ -79,7 +80,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm,\n\
+ From this path read \"Klimt/Klimt.pgm,\n\
.ppm, .jpeg and .png images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -90,8 +91,7 @@ OPTIONS: Default\n\
Example: -p /my_path_to/image.png\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str());
+ Print the help.\n\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -111,20 +111,29 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'p': ppath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath); return false; break;
+ usage(argv[0], optarg_, ipath);
+ return false;
+ break;
}
}
@@ -139,8 +148,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -149,16 +157,17 @@ main(int argc, const char ** argv)
std::string ipath;
std::string filename;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_ppath) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -169,61 +178,55 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
}
}
-
//
// Here starts really the test
//
/////////////////////////////////////////////////////////////////////
// Create a grey level image
- //vpImage<vpRGBa> I;
+ // vpImage<vpRGBa> I;
vpImage<unsigned char> I;
vpImage<vpRGBa> Irgb;
- if (opt_ppath.empty())
- {
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(I,filename);
+ if (opt_ppath.empty()) {
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(I, filename);
printf("Read ppm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(I,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(I, filename);
printf("Read pgm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
- vpImageIo::read(I,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.jpeg");
+ vpImageIo::read(I, filename);
printf("Read jpeg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
- vpImageIo::read(I,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.png");
+ vpImageIo::read(I, filename);
printf("Read png ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpImageIo::read(Irgb,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImageIo::read(Irgb, filename);
printf("Read ppm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpImageIo::read(Irgb,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImageIo::read(Irgb, filename);
printf("Read pgm ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.jpeg");
- vpImageIo::read(Irgb,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.jpeg");
+ vpImageIo::read(Irgb, filename);
printf("Read jpeg ok\n");
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.png");
- vpImageIo::read(Irgb,filename);
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.png");
+ vpImageIo::read(Irgb, filename);
printf("Read png ok\n");
- }
- else
- {
+ } else {
filename = opt_ppath;
vpImageIo::read(I, filename);
printf("Image \"%s\" read successfully\n", filename.c_str());
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
return 0;
diff --git a/modules/core/test/image/testUndistortImage.cpp b/modules/core/test/image/testUndistortImage.cpp
index 41ee39a..d6ff586 100644
--- a/modules/core/test/image/testUndistortImage.cpp
+++ b/modules/core/test/image/testUndistortImage.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,17 +36,15 @@
*
*****************************************************************************/
-
-
+#include <stdlib.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpRGBa.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpTime.h>
-#include <stdlib.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
/*!
\example testUndistortImage.cpp
@@ -57,14 +56,11 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:o:h"
+#define GETOPTARGS "cdi:o:h"
//#define COLOR
#define BW
-void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
-
/*
Print the program options.
@@ -76,7 +72,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
\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, const std::string &ipath, const std::string &opath,
+ const std::string &user)
{
fprintf(stdout, "\n\
Read an image from the disk, undistort it \n\
@@ -92,7 +89,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -105,8 +102,7 @@ OPTIONS: Default\n\
Klimt_undistorted.pgm output image is written.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), opath.c_str(), user.c_str());
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -124,23 +120,32 @@ 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, const std::string &user)
{
const char *optarg_;
int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath, opath, user); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath, opath, user); return false; break;
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
}
}
@@ -155,7 +160,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &op
return true;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -166,14 +171,15 @@ int main(int argc, const char ** argv)
std::string filename;
std::string username;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -185,7 +191,7 @@ int main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -202,11 +208,9 @@ int main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(opath);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << opath << std::endl;
std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
exit(-1);
@@ -217,8 +221,7 @@ int main(int argc, const char ** argv)
// the input path comming from the command line option
if (opt_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -226,20 +229,19 @@ int main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
- //
- // Here starts really the test
- //
+//
+// Here starts really the test
+//
#if defined BW
vpImage<unsigned char> I; // Input image
vpImage<unsigned char> U; // undistorted output image
@@ -248,15 +250,15 @@ int main(int argc, const char ** argv)
vpImage<vpRGBa> U; // undistorted output image
#endif
vpCameraParameters cam;
- cam.initPersProjWithDistortion(600,600,192,144,-0.17,0.17);
- // Read the input grey image from the disk
+ cam.initPersProjWithDistortion(600, 600, 192, 144, -0.17, 0.17);
+// Read the input grey image from the disk
#if defined BW
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm") ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
#elif defined COLOR
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
#endif
std::cout << "Read image: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ vpImageIo::read(I, filename);
std::cout << "Undistortion in process... " << std::endl;
vpImageTools::undistort(I, cam, U);
@@ -265,26 +267,24 @@ int main(int argc, const char ** argv)
// For the test, to have a significant time measure we repeat the
// undistortion 100 times
- for(unsigned int i=0;i<100;i++)
+ for (unsigned int i = 0; i < 100; i++)
// Create the undistorted image
vpImageTools::undistort(I, cam, U);
double endtime = vpTime::measureTimeMs();
- std::cout<<"Time for 100 undistortion (ms): "<< endtime - begintime
- << std::endl;
+ std::cout << "Time for 100 undistortion (ms): " << endtime - begintime << std::endl;
- // Write the undistorted image on the disk
+// 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) ;
+ vpImageIo::write(U, filename);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/math/testArray2D.cpp b/modules/core/test/math/testArray2D.cpp
index af0c56e..2035a17 100644
--- a/modules/core/test/math/testArray2D.cpp
+++ b/modules/core/test/math/testArray2D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,23 +43,22 @@
*/
#include <cmath>
-#include <vector>
#include <limits>
+#include <vector>
#include <visp3/core/vpTranslationVector.h>
-template<typename Type>
-bool test(const std::string &s, const vpArray2D<Type> &A, const std::vector<Type> &bench)
+template <typename Type> bool test(const std::string &s, const vpArray2D<Type> &A, const std::vector<Type> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") = \n" << A << std::endl;
- if(bench.size() != A.size()) {
+ if (bench.size() != A.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<A.size(); i++) {
- if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < A.size(); i++) {
+ if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -82,10 +82,10 @@ int main()
vpArray2D<double> A(3, 4);
std::vector<double> bench(12);
- for(unsigned int i=0; i<3; i++) {
- for(unsigned int j=0; j<4; j++) {
- A[i][j] = (double)(i+j);
- bench[i*4+j] = (double)(i+j);
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 4; j++) {
+ A[i][j] = (double)(i + j);
+ bench[i * 4 + j] = (double)(i + j);
}
}
if (test("A", A, bench) == false)
@@ -127,10 +127,10 @@ int main()
vpArray2D<float> A(3, 4);
std::vector<float> bench(12);
- for(unsigned int i=0; i<3; i++) {
- for(unsigned int j=0; j<4; j++) {
- A[i][j] = (float)(i+j);
- bench[i*4+j] = (float)(i+j);
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 4; j++) {
+ A[i][j] = (float)(i + j);
+ bench[i * 4 + j] = (float)(i + j);
}
}
if (test("A", A, bench) == false)
@@ -158,6 +158,37 @@ int main()
if (test("A", A, bench3) == false)
return err;
}
+ {
+ // Test Hadamard product
+ std::cout << "\nTest Hadamard product" << std::endl;
+ vpArray2D<int> A1(3, 5), A2(3, 5);
+ vpRowVector R1(15), R2(15);
+ vpColVector C1(15), C2(15);
+
+ for (unsigned int i = 0; i < A1.size(); i++) {
+ A1.data[i] = i;
+ A2.data[i] = i + 2;
+ R1.data[i] = i;
+ R2.data[i] = i + 2;
+ C1.data[i] = i;
+ C2.data[i] = i + 2;
+ }
+
+ std::cout << "A1:\n" << A1 << std::endl;
+ std::cout << "\nA2:\n" << A2 << std::endl;
+ A2 = A1.hadamard(A2);
+ std::cout << "\nRes:\n" << A2 << std::endl;
+
+ std::cout << "\nR1:\n" << R1 << std::endl;
+ std::cout << "\nR2:\n" << R2 << std::endl;
+ R2 = R1.hadamard(R2);
+ std::cout << "\nRes:\n" << R2 << std::endl;
+
+ std::cout << "\nC1:\n" << C1 << std::endl;
+ std::cout << "\nC2:\n" << C2 << std::endl;
+ C2 = C1.hadamard(C2);
+ std::cout << "\nRes:\n" << C2 << std::endl;
+ }
std::cout << "All tests succeed" << std::endl;
return 0;
}
diff --git a/modules/core/test/math/testColVector.cpp b/modules/core/test/math/testColVector.cpp
index 31a0374..fab5e42 100644
--- a/modules/core/test/math/testColVector.cpp
+++ b/modules/core/test/math/testColVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,25 +42,26 @@
Test some vpColVector functionalities.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpGaussRand.h>
+#include <visp3/core/vpMath.h>
-
+namespace
+{
bool test(const std::string &s, const vpColVector &v, const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v.t() << "]^T" << std::endl;
- if(bench.size() != v.size()) {
+ if (bench.size() != v.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<v.size(); i++) {
- if (std::fabs(v[i]-bench[i]) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < v.size(); i++) {
+ if (std::fabs(v[i] - bench[i]) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -68,39 +70,48 @@ bool test(const std::string &s, const vpColVector &v, const std::vector<double>
return true;
}
-double computeRegularSum(const vpColVector &v) {
+double computeRegularSum(const vpColVector &v)
+{
double sum = 0.0;
- for(unsigned int i = 0; i < v.getRows(); i++) {
+ for (unsigned int i = 0; i < v.getRows(); i++) {
sum += v[i];
}
return sum;
}
-double computeRegularSumSquare(const vpColVector &v) {
+double computeRegularSumSquare(const vpColVector &v)
+{
double sum_square = 0.0;
- for(unsigned int i = 0; i < v.getRows(); i++) {
+ for (unsigned int i = 0; i < v.getRows(); i++) {
sum_square += v[i] * v[i];
}
return sum_square;
}
-double computeRegularStdev(const vpColVector &v) {
+double computeRegularStdev(const vpColVector &v)
+{
double mean_value = computeRegularSum(v) / v.getRows();
double sum_squared_diff = 0.0;
- for(unsigned int i = 0; i < v.size(); i++) {
- sum_squared_diff += (v[i]-mean_value) * (v[i]-mean_value);
+ for (unsigned int i = 0; i < v.size(); i++) {
+ sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
}
- double divisor = (double) v.size();
+ double divisor = (double)v.size();
return std::sqrt(sum_squared_diff / divisor);
}
+double getRandomValues(const double min, const double max)
+{
+ return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
+}
+}
+
int main()
{
int err = 1;
@@ -113,7 +124,7 @@ int main()
std::vector<double> bench1(4, 3);
if (test("v", v, bench1) == false)
return err;
- std::vector<double> bench2(4, 3./6);
+ std::vector<double> bench2(4, 3. / 6);
v.normalize();
if (test("v", v, bench2) == false)
return err;
@@ -127,9 +138,9 @@ int main()
{
vpColVector v(4);
std::vector<double> bench1(4);
- for(unsigned int i=0; i<v.size(); i++) {
+ for (unsigned int i = 0; i < v.size(); i++) {
v[i] = (double)i;
- bench1[i] = (double)i;
+ bench1[i] = (double)i;
}
if (test("v", v, bench1) == false)
return err;
@@ -148,7 +159,7 @@ int main()
bench3.push_back(3);
vpColVector r1;
- for(size_t i=0; i<4; i++)
+ for (size_t i = 0; i < 4; i++)
r1.stack((double)i);
vpColVector r2 = r1.extract(1, 3);
@@ -159,7 +170,7 @@ int main()
{
vpMatrix M(4, 1);
std::vector<double> bench(4);
- for(unsigned int i=0; i<M.getRows(); i++) {
+ for (unsigned int i = 0; i < M.getRows(); i++) {
M[i][0] = i;
bench[i] = i;
}
@@ -215,7 +226,7 @@ int main()
{
vpColVector r1(3, 1);
vpColVector r2 = -r1;
- std::vector<double> bench(3,-1);
+ std::vector<double> bench(3, -1);
// v contains [-1 -1 -1]
if (test("r2", r2, bench) == false)
return err;
@@ -241,7 +252,7 @@ int main()
std::cout << "test r1: " << r1 << std::endl;
std::cout << "test r2: " << r2 << std::endl;
vpColVector r = r1 + r2;
- std::cout << "test r1+r2: " << r1+r2 << std::endl;
+ std::cout << "test r1+r2: " << r1 + r2 << std::endl;
std::cout << "test r: " << r << std::endl;
std::vector<double> bench(3, 6);
if (test("r", r, bench) == false)
@@ -274,7 +285,8 @@ int main()
}
{
- // Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ // Test mean, median and standard deviation against Matlab with rng(0) and
+ // rand(10,1)*10
vpColVector r(10);
r[0] = 8.1472;
r[1] = 9.0579;
@@ -289,28 +301,28 @@ int main()
std::cout << "** Test mean" << std::endl;
double res = vpColVector::mean(r);
- if(!vpMath::equal(res, 6.2386, 0.001)) {
+ if (!vpMath::equal(res, 6.2386, 0.001)) {
std::cout << "Test fails: bad mean " << res << std::endl;
return err;
}
std::cout << "** Test stdev" << std::endl;
res = vpColVector::stdev(r);
- if(!vpMath::equal(res, 3.2810, 0.001)) {
+ if (!vpMath::equal(res, 3.2810, 0.001)) {
std::cout << "Test fails: bad stdev " << res << std::endl;
return err;
}
std::cout << "** Test stdev(bessel)" << std::endl;
res = vpColVector::stdev(r, true);
- if(!vpMath::equal(res, 3.4585, 0.001)) {
+ if (!vpMath::equal(res, 3.4585, 0.001)) {
std::cout << "Test fails: bad stdev(bessel) " << res << std::endl;
return err;
}
std::cout << "** Test median" << std::endl;
res = vpColVector::median(r);
- if(!vpMath::equal(res, 7.2354, 0.001)) {
+ if (!vpMath::equal(res, 7.2354, 0.001)) {
std::cout << "Test fails: bad median " << res << std::endl;
return err;
}
@@ -319,7 +331,7 @@ int main()
std::cout << "** Test median (odd)" << std::endl;
r.stack(1.5761);
res = vpColVector::median(r);
- if(!vpMath::equal(res, 6.3236, 0.001)) {
+ if (!vpMath::equal(res, 6.3236, 0.001)) {
std::cout << "Test fails: bad median (odd) " << res << std::endl;
return err;
}
@@ -327,7 +339,7 @@ int main()
r.print(std::cout, 8, "r");
}
- //Test sum, sumSquare, stdev
+ // Test sum, sumSquare, stdev
{
srand(0);
vpGaussRand noise(10.0, 0.0);
@@ -336,7 +348,7 @@ int main()
unsigned int size = 117;
vpColVector v(size);
- for(unsigned int cpt = 0; cpt < v.getRows(); cpt++) {
+ for (unsigned int cpt = 0; cpt < v.getRows(); cpt++) {
v[cpt] = rand() % 1000 + noise();
}
@@ -344,76 +356,116 @@ int main()
double regular_sum = 0.0;
double t_regular = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
regular_sum += computeRegularSum(v);
}
t_regular = vpTime::measureTimeMs() - t_regular;
double sse_sum = 0.0;
double t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
sse_sum += v.sum();
}
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 << "Speed-up: " << (t_regular/t_sse) << "X" << std::endl;
+ std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl;
- if( !vpMath::equal(regular_sum, sse_sum, std::numeric_limits<double>::epsilon()) ) {
+ if (!vpMath::equal(regular_sum, sse_sum, std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem when computing v.sum()!" << std::endl;
- return -1;
+ return EXIT_FAILURE;
}
-
double regular_sumSquare = 0.0;
t_regular = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
regular_sumSquare += computeRegularSumSquare(v);
}
t_regular = vpTime::measureTimeMs() - t_regular;
double sse_sumSquare = 0.0;
t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
sse_sumSquare += v.sumSquare();
}
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 << "Speed-up: " << (t_regular/t_sse) << "X" << std::endl;
+ std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl;
- if( !vpMath::equal(regular_sumSquare, sse_sumSquare, std::numeric_limits<double>::epsilon()) ) {
+ if (!vpMath::equal(regular_sumSquare, sse_sumSquare, std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem when computing v.sumSquare()!" << std::endl;
- return -1;
+ return EXIT_FAILURE;
}
-
double regular_stdev = 0.0;
t_regular = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
regular_stdev += computeRegularStdev(v);
}
t_regular = vpTime::measureTimeMs() - t_regular;
double sse_stdev = 0.0;
t_sse = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
sse_stdev += vpColVector::stdev(v, false);
}
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 << "Speed-up: " << (t_regular/t_sse) << "X" << std::endl;
+ std::cout << "Speed-up: " << (t_regular / t_sse) << "X" << std::endl;
- if( !vpMath::equal(regular_stdev, sse_stdev, std::numeric_limits<double>::epsilon()) ) {
+ if (!vpMath::equal(regular_stdev, sse_stdev, std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem when computing vpColVector::stdev()!" << std::endl;
- return -1;
+ return EXIT_FAILURE;
+ }
+ }
+
+ {
+ // Test insert with big vector
+ const unsigned int nb = 1000;
+ const unsigned int size = 10000;
+ std::vector<vpColVector> vec(nb);
+
+ for (size_t i = 0; i < nb; i++) {
+ vpColVector v(size);
+ for (unsigned int j = 0; j < size; j++) {
+ v[j] = getRandomValues(-100.0, 100.0);
+ }
+ vec[i] = v;
+ }
+
+ vpColVector v_big(nb * size);
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < nb; i++) {
+ v_big.insert(i * size, vec[(size_t)i]);
}
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nBig insert: " << t << " ms" << std::endl;
+
+ 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<double>::epsilon())) {
+ std::cerr << "Problem in vpColVector insert()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ // Try to insert empty vpColVector
+ vpColVector v1(2), v2, v3;
+ v1.insert(0, v2);
+ v3.insert(0, v2);
+
+ std::cout << "Insert empty vectors:" << std::endl;
+ std::cout << "v1: " << v1.t() << std::endl;
+ std::cout << "v2: " << v2.t() << std::endl;
+ std::cout << "v3: " << v3.t() << std::endl;
}
std::cout << "\nAll tests succeed" << std::endl;
- return 0;
+ return EXIT_SUCCESS;
}
diff --git a/modules/core/test/math/testKalmanAcceleration.cpp b/modules/core/test/math/testKalmanAcceleration.cpp
index 926c9cb..39ce258 100644
--- a/modules/core/test/math/testKalmanAcceleration.cpp
+++ b/modules/core/test/math/testKalmanAcceleration.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,12 +43,11 @@
with constant acceleration state model.
*/
-#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
-int
-main()
+int main()
{
try {
unsigned int nsignal = 1; // Number of signal to filter
@@ -62,36 +62,34 @@ main()
model = vpLinearKalmanFilterInstantiation::stateConstAccWithColoredNoise_MeasureVel;
kalman.setStateModel(model);
- unsigned int size_state_vector = kalman.getStateSize()*nsignal;
- unsigned int size_measure_vector = kalman.getMeasureSize()*nsignal;
+ unsigned int size_state_vector = kalman.getStateSize() * nsignal;
+ unsigned int size_measure_vector = kalman.getMeasureSize() * nsignal;
vpColVector sigma_measure(size_measure_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++)
+ for (unsigned int signal = 0; signal < nsignal; signal++)
sigma_measure = 0.0001;
vpColVector sigma_state(size_state_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[3*signal] = 0.; // not used
- sigma_state[3*signal+1] = 0.000001;
- sigma_state[3*signal+2] = 0.000001;
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ sigma_state[3 * signal] = 0.; // not used
+ sigma_state[3 * signal + 1] = 0.000001;
+ sigma_state[3 * signal + 2] = 0.000001;
}
vpColVector velocity_measure(size_measure_vector);
double rho = 0.9; // correlation
- double dt = 0.2; // sampling period
+ double dt = 0.2; // sampling period
- for (unsigned int signal=0; signal < nsignal; signal ++)
- velocity_measure[signal] = 3+2*signal;
+ for (unsigned int signal = 0; signal < nsignal; signal++)
+ velocity_measure[signal] = 3 + 2 * signal;
kalman.verbose(false);
kalman.initFilter(nsignal, sigma_state, sigma_measure, rho, dt);
-
- for (unsigned int iter=0; iter <= niter; iter++) {
+ 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));
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ velocity_measure[signal] = 3 + 2 * signal + 0.3 * sin(vpMath::rad(360. / niter * iter));
}
std::cout << "measure : " << velocity_measure.t() << std::endl;
@@ -105,13 +103,12 @@ main()
std::cout << "Xest: " << kalman.Xest.t() << std::endl;
std::cout << "Xpre: " << kalman.Xpre.t() << std::endl;
- flog << std::endl;
+ flog << std::endl;
}
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 0;
}
diff --git a/modules/core/test/math/testKalmanVelocity.cpp b/modules/core/test/math/testKalmanVelocity.cpp
index fdcd258..aa5d23f 100644
--- a/modules/core/test/math/testKalmanVelocity.cpp
+++ b/modules/core/test/math/testKalmanVelocity.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,24 +43,23 @@
with constant velocity state model.
*/
-#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
-#include <iostream>
#include <fstream>
+#include <iostream>
+#include <visp3/core/vpLinearKalmanFilterInstantiation.h>
typedef enum {
Position, // Considered measures are the succesive positions of the target
Velocity // Considered measures are the succesive velocities of the target
} vpMeasureType;
-int
-main()
+int main()
{
try {
unsigned int nsignal = 2; // Number of signal to filter
unsigned int niter = 200;
- unsigned int size_state_vector = 2*nsignal;
- unsigned int size_measure_vector = 1*nsignal;
- //vpMeasureType measure_t = Velocity;
+ unsigned int size_state_vector = 2 * nsignal;
+ unsigned int size_measure_vector = 1 * nsignal;
+ // vpMeasureType measure_t = Velocity;
vpMeasureType measure_t = Position;
std::string filename = "/tmp/log.dat";
@@ -68,29 +68,29 @@ main()
vpLinearKalmanFilterInstantiation kalman;
vpColVector sigma_measure(size_measure_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++)
+ for (unsigned int signal = 0; signal < nsignal; signal++)
sigma_measure = 0.000001;
vpColVector sigma_state(size_state_vector);
switch (measure_t) {
case Velocity:
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[2*signal] = 0.; // not used
- sigma_state[2*signal+1] = 0.000001;
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ sigma_state[2 * signal] = 0.; // not used
+ sigma_state[2 * signal + 1] = 0.000001;
}
break;
case Position:
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- sigma_state[2*signal] = 0.000001;
- sigma_state[2*signal+1] = 0; // not used
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ sigma_state[2 * signal] = 0.000001;
+ sigma_state[2 * signal + 1] = 0; // not used
}
break;
}
vpColVector measure(size_measure_vector);
- for (unsigned int signal=0; signal < nsignal; signal ++) {
- measure[signal] = 3+2*signal;
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ measure[signal] = 3 + 2 * signal;
}
kalman.verbose(true);
@@ -112,10 +112,10 @@ main()
break;
}
- for (unsigned int iter=0; iter <= niter; iter++) {
+ 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));
+ for (unsigned int signal = 0; signal < nsignal; signal++) {
+ measure[signal] = 3 + 2 * signal + 0.3 * sin(vpMath::rad(360. / niter * iter));
}
std::cout << "measure : " << measure.t() << std::endl;
@@ -130,8 +130,7 @@ main()
flog.close();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/math/testMath.cpp b/modules/core/test/math/testMath.cpp
index 1166439..c009a78 100644
--- a/modules/core/test/math/testMath.cpp
+++ b/modules/core/test/math/testMath.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,445 +42,453 @@
Test some vpMath functionalities.
*/
+#include <cfloat>
#include <iostream>
#include <limits>
-#include <cfloat>
#include <visp3/core/vpMath.h>
#if defined _MSC_VER && _MSC_VER >= 1200
- #pragma warning( disable: 4723 )
+#pragma warning(disable : 4723)
// 4723 : potential divide by 0
#endif
#ifdef WIN32
- #ifndef NAN
- //https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
- //http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
- static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
- #define NAN (*(const float *) __nan)
- #endif
+#ifndef NAN
+// https://msdn.microsoft.com/en-us/library/w22adx1s%28v=vs.120%29.aspx
+// http://tdistler.com/2011/03/24/how-to-define-nan-not-a-number-on-windows
+static const unsigned long __nan[2] = {0xffffffff, 0x7fffffff};
+#define NAN (*(const float *)__nan)
+#endif
#endif
-int main() {
- //Test isNaN
- if(vpMath::isNaN(0.0)) {
+int main()
+{
+ // Test isNaN
+ if (vpMath::isNaN(0.0)) {
std::cerr << "Fail: IsNaN(0.0)=" << vpMath::isNaN(0.0) << " / should be false" << std::endl;
return -1;
}
double num = 1.0, den = 0.0;
- if(vpMath::isNaN(num/den)) {
- std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num/den) << " / should be false" << std::endl;
+ if (vpMath::isNaN(num / den)) {
+ std::cerr << "Fail: IsNaN(1.0/0.0)=" << vpMath::isNaN(num / den) << " / should be false" << std::endl;
return -1;
}
- if(!vpMath::isNaN(NAN)) {
+ if (!vpMath::isNaN(NAN)) {
std::cerr << "Fail: IsNaN(NAN)=" << vpMath::isNaN(NAN) << " / should be true" << std::endl;
return -1;
}
num = 0.0;
- if(!vpMath::isNaN(num/den)) {
- std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num/den) << " / should be true" << std::endl;
+ if (!vpMath::isNaN(num / den)) {
+ std::cerr << "Fail: IsNaN(0.0/0.0)=" << vpMath::isNaN(num / den) << " / should be true" << std::endl;
return -1;
}
- if(!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
+ if (!vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())) {
std::cerr << "Fail: IsNaN(quiet_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::quiet_NaN())
- << " / should be true" << std::endl;
+ << " / should be true" << std::endl;
return -1;
}
- if(!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
+ if (!vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())) {
std::cerr << "Fail: IsNaN(signaling_NaN)=" << vpMath::isNaN(std::numeric_limits<double>::signaling_NaN())
- << " / should be true" << std::endl;
+ << " / should be true" << std::endl;
return -1;
}
- if(vpMath::isNaN(std::numeric_limits<double>::infinity())) {
+ if (vpMath::isNaN(std::numeric_limits<double>::infinity())) {
std::cerr << "Fail: IsNaN(infinity)=" << vpMath::isNaN(std::numeric_limits<double>::infinity())
- << " / should be false" << std::endl;
+ << " / should be false" << std::endl;
return -1;
}
- if(vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
- std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0/std::numeric_limits<double>::epsilon())
- << " / should be false" << std::endl;
+ if (vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())) {
+ std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0 / std::numeric_limits<double>::epsilon())
+ << " / should be false" << std::endl;
return -1;
}
- if(!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
- std::cerr << "Fail: IsNaN(1.0/epsilon)=" << vpMath::isNaN(1.0/std::numeric_limits<double>::epsilon())
- << " / should be true" << std::endl;
+ if (!vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::infinity())) {
+ std::cerr << "Fail: IsNaN(infinity - infinity)="
+ << vpMath::isNaN(std::numeric_limits<double>::infinity() - std::numeric_limits<double>::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;
+ if (!vpMath::isNaN(a / b)) {
+ std::cerr << "Fail: IsNaN(0.0f/0.0f)=" << vpMath::isNaN(a / b) << " / should be true" << std::endl;
return -1;
}
std::cout << "vpMath::isNaN is Ok !" << std::endl;
-
- //Test isInf
+// Test isInf
#if !defined(VISP_HAVE_FUNC__FINITE)
- //Disable this test if using _finite as (!_finite(NAN)) returns true whereas isinf(NAN) returns false
- if(vpMath::isInf(NAN)) {
+ // Disable this test if using _finite as (!_finite(NAN)) returns true
+ // whereas isinf(NAN) returns false
+ if (vpMath::isInf(NAN)) {
std::cerr << "Fail: vpMath::isInf(NAN)=" << vpMath::isInf(NAN) << " / should be false" << std::endl;
return -1;
}
#endif
- if(!vpMath::isInf(1.0/a)) {
- std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0/a) << " / should be true" << std::endl;
+ if (!vpMath::isInf(1.0 / a)) {
+ std::cerr << "Fail: vpMath::isInf(1.0/0.0)=" << vpMath::isInf(1.0 / a) << " / should be true" << std::endl;
return -1;
}
- if(vpMath::isInf(0.0)) {
+ if (vpMath::isInf(0.0)) {
std::cerr << "Fail: vpMath::isInf(0.0)=" << vpMath::isInf(0.0) << " / should be false" << std::endl;
return -1;
}
- if(!vpMath::isInf(exp(800.))) {
+ if (!vpMath::isInf(exp(800.))) {
std::cerr << "Fail: vpMath::isInf(exp(800.))=" << vpMath::isInf(exp(800.)) << " / should be true" << std::endl;
return -1;
}
- if(vpMath::isInf(DBL_MIN/2.0)) {
- std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN/2.0) << " / should be false" << std::endl;
+ if (vpMath::isInf(DBL_MIN / 2.0)) {
+ std::cerr << "Fail: vpMath::isInf(DBL_MIN/2.0)=" << vpMath::isInf(DBL_MIN / 2.0) << " / should be false"
+ << std::endl;
return -1;
}
std::cout << "vpMath::isInf is Ok !" << std::endl;
-
- //Test round
- if(vpMath::round(2.3) != 2) {
+ // Test round
+ if (vpMath::round(2.3) != 2) {
std::cerr << "Fail: vpMath::round(2.3)=" << vpMath::round(2.3) << " / should be 2" << std::endl;
return -1;
}
- if(vpMath::round(3.8) != 4) {
+ if (vpMath::round(3.8) != 4) {
std::cerr << "Fail: vpMath::round(3.8)=" << vpMath::round(3.8) << " / should be 4" << std::endl;
return -1;
}
- if(vpMath::round(5.5) != 6) {
+ if (vpMath::round(5.5) != 6) {
std::cerr << "Fail: vpMath::round(5.5)=" << vpMath::round(5.5) << " / should be 6" << std::endl;
return -1;
}
- if(vpMath::round(-2.3) != -2) {
+ if (vpMath::round(-2.3) != -2) {
std::cerr << "Fail: vpMath::round(-2.3)=" << vpMath::round(-2.3) << " / should be -2" << std::endl;
return -1;
}
- if(vpMath::round(-3.8) != -4) {
+ if (vpMath::round(-3.8) != -4) {
std::cerr << "Fail: vpMath::round(-3.8)=" << vpMath::round(-3.8) << " / should be -4" << std::endl;
return -1;
}
- if(vpMath::round(-5.5) != -6) {
+ if (vpMath::round(-5.5) != -6) {
std::cerr << "Fail: vpMath::round(-5.5)=" << vpMath::round(-5.5) << " / should be -6" << std::endl;
return -1;
}
- if(vpMath::round(0.0) != 0) {
+ if (vpMath::round(0.0) != 0) {
std::cerr << "Fail: vpMath::round(0.0)=" << vpMath::round(0.0) << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::round is Ok !" << std::endl;
-
- //Test saturate functions
- //unsigned char
+ // Test saturate functions
+ // unsigned char
char char_value = -127;
unsigned char uchar_value = vpMath::saturate<unsigned char>(char_value);
- if(uchar_value != 0) {
+ if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-127)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
unsigned short ushort_value = 60000;
uchar_value = vpMath::saturate<unsigned char>(ushort_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(60000)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
int int_value = 70000;
uchar_value = vpMath::saturate<unsigned char>(int_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(70000)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
int_value = -70000;
uchar_value = vpMath::saturate<unsigned char>(int_value);
- if(uchar_value != 0) {
+ if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-70000)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
short short_value = 30000;
uchar_value = vpMath::saturate<unsigned char>(short_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(30000)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
short_value = -30000;
uchar_value = vpMath::saturate<unsigned char>(short_value);
- if(uchar_value != 0) {
+ if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-30000)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
unsigned int uint_value = 10000;
uchar_value = vpMath::saturate<unsigned char>(uint_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
float float_value = 10000.1f;
uchar_value = vpMath::saturate<unsigned char>(float_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.1f)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
float_value = -10000.1f;
uchar_value = vpMath::saturate<unsigned char>(float_value);
- if(uchar_value != 0) {
+ if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.1f)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
double double_value = 10000.1;
uchar_value = vpMath::saturate<unsigned char>(double_value);
- if(uchar_value != UCHAR_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX << std::endl;
+ if (uchar_value != UCHAR_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned char>(10000.0)=" << uchar_value << " / should be " << UCHAR_MAX
+ << std::endl;
return -1;
}
double_value = -10000.1;
uchar_value = vpMath::saturate<unsigned char>(double_value);
- if(uchar_value != 0) {
+ if (uchar_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned char>(-10000.0)=" << uchar_value << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::saturate<unsigned char>() is Ok !" << std::endl;
-
- //char
+ // char
uchar_value = 255;
char_value = vpMath::saturate<char>(uchar_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(255)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
ushort_value = 60000;
char_value = vpMath::saturate<char>(ushort_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(60000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
int_value = 70000;
char_value = vpMath::saturate<char>(int_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(70000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
int_value = -70000;
char_value = vpMath::saturate<char>(int_value);
- if(char_value != (char)SCHAR_MIN) {
+ if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-70000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
short_value = 30000;
char_value = vpMath::saturate<char>(short_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(30000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
short_value = -30000;
char_value = vpMath::saturate<char>(short_value);
- if(char_value != (char)SCHAR_MIN) {
+ if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-30000)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
uint_value = 10000;
char_value = vpMath::saturate<char>(uint_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
float_value = 10000.1f;
char_value = vpMath::saturate<char>(float_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000.1f)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
float_value = -10000.1f;
char_value = vpMath::saturate<char>(float_value);
- if(char_value != (char)SCHAR_MIN) {
+ if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-10000.1f)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
double_value = 10000.1;
char_value = vpMath::saturate<char>(double_value);
- if(char_value != SCHAR_MAX) {
+ if (char_value != SCHAR_MAX) {
std::cerr << "Fail: vpMath::saturate<char>(10000.1)=" << char_value << " / should be " << SCHAR_MAX << std::endl;
return -1;
}
double_value = -10000.1;
char_value = vpMath::saturate<char>(double_value);
- if(char_value != (char)SCHAR_MIN) {
+ if (char_value != (char)SCHAR_MIN) {
std::cerr << "Fail: vpMath::saturate<char>(-10000.1)=" << char_value << " / should be " << SCHAR_MIN << std::endl;
return -1;
}
std::cout << "vpMath::saturate<char>() is Ok !" << std::endl;
-
- //unsigned short
+ // unsigned short
char_value = -127;
ushort_value = vpMath::saturate<unsigned short>(char_value);
- if(ushort_value != 0) {
+ if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-127)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
short_value = -30000;
ushort_value = vpMath::saturate<unsigned short>(short_value);
- if(ushort_value != 0) {
+ if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-30000)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
int_value = 70000;
ushort_value = vpMath::saturate<unsigned short>(int_value);
- if(ushort_value != USHRT_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ if (ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
+ << std::endl;
return -1;
}
int_value = -70000;
ushort_value = vpMath::saturate<unsigned short>(int_value);
- if(ushort_value != 0) {
+ if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-70000)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
uint_value = 70000;
ushort_value = vpMath::saturate<unsigned short>(uint_value);
- if(ushort_value != USHRT_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ if (ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000)=" << ushort_value << " / should be " << USHRT_MAX
+ << std::endl;
return -1;
}
float_value = 70000.1f;
ushort_value = vpMath::saturate<unsigned short>(float_value);
- if(ushort_value != USHRT_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ if (ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1f)=" << ushort_value << " / should be " << USHRT_MAX
+ << std::endl;
return -1;
}
float_value = -10000.1f;
ushort_value = vpMath::saturate<unsigned short>(float_value);
- if(ushort_value != 0) {
+ if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1f)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
double_value = 70000.1;
ushort_value = vpMath::saturate<unsigned short>(double_value);
- if(ushort_value != USHRT_MAX) {
- std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX << std::endl;
+ if (ushort_value != USHRT_MAX) {
+ std::cerr << "Fail: vpMath::saturate<unsigned short>(70000.1)=" << ushort_value << " / should be " << USHRT_MAX
+ << std::endl;
return -1;
}
double_value = -10000.1;
ushort_value = vpMath::saturate<unsigned short>(double_value);
- if(ushort_value != 0) {
+ if (ushort_value != 0) {
std::cerr << "Fail: vpMath::saturate<unsigned short>(-10000.1)=" << ushort_value << " / should be 0" << std::endl;
return -1;
}
std::cout << "vpMath::saturate<unsigned short>() is Ok !" << std::endl;
-
- //short
+ // short
ushort_value = 60000;
short_value = vpMath::saturate<short>(ushort_value);
- if(short_value != SHRT_MAX) {
+ if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(60000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
int_value = 70000;
short_value = vpMath::saturate<short>(int_value);
- if(short_value != SHRT_MAX) {
+ if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
int_value = -70000;
short_value = vpMath::saturate<short>(int_value);
- if(short_value != SHRT_MIN) {
+ if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(-70000)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
uint_value = 70000;
short_value = vpMath::saturate<short>(uint_value);
- if(short_value != SHRT_MAX) {
+ if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
float_value = 70000.1f;
short_value = vpMath::saturate<short>(float_value);
- if(short_value != SHRT_MAX) {
+ if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1f)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
float_value = -70000.1f;
short_value = vpMath::saturate<short>(float_value);
- if(short_value != SHRT_MIN) {
+ if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(-70000.1f)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
double_value = 70000.1;
short_value = vpMath::saturate<short>(double_value);
- if(short_value != SHRT_MAX) {
+ if (short_value != SHRT_MAX) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MAX << std::endl;
return -1;
}
double_value = -70000.1;
short_value = vpMath::saturate<short>(double_value);
- if(short_value != SHRT_MIN) {
+ if (short_value != SHRT_MIN) {
std::cerr << "Fail: vpMath::saturate<short>(70000.1)=" << short_value << " / should be " << SHRT_MIN << std::endl;
return -1;
}
std::cout << "vpMath::saturate<short>() is Ok !" << std::endl;
-
- //Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ // Test mean, median and standard deviation against Matlab with rng(0) and
+ // rand(10,1)*10
std::vector<double> vectorOfDoubles(10);
vectorOfDoubles[0] = 8.1472;
vectorOfDoubles[1] = 9.0579;
@@ -492,43 +501,41 @@ int main() {
vectorOfDoubles[8] = 9.5751;
vectorOfDoubles[9] = 9.6489;
-
double res = vpMath::getMean(vectorOfDoubles);
- if(!vpMath::equal(res, 6.2386, 0.001)) {
+ if (!vpMath::equal(res, 6.2386, 0.001)) {
std::cerr << "Problem with vpMath::getMean()=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getMean() is Ok !" << std::endl;
res = vpMath::getStdev(vectorOfDoubles);
- if(!vpMath::equal(res, 3.2810, 0.001)) {
+ if (!vpMath::equal(res, 3.2810, 0.001)) {
std::cerr << "Problem with vpMath::getStdev()=" << res << std::endl;
return -1;
}
res = vpMath::getStdev(vectorOfDoubles, true);
- if(!vpMath::equal(res, 3.4585, 0.001)) {
+ if (!vpMath::equal(res, 3.4585, 0.001)) {
std::cerr << "Problem with vpMath::getStdev() with Bessel correction=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getStdev() is Ok !" << std::endl;
res = vpMath::getMedian(vectorOfDoubles);
- if(!vpMath::equal(res, 7.2354, 0.001)) {
+ if (!vpMath::equal(res, 7.2354, 0.001)) {
std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
return -1;
}
- //Test median with odd number of elements
+ // Test median with odd number of elements
vectorOfDoubles.push_back(1.5761);
res = vpMath::getMedian(vectorOfDoubles);
- if(!vpMath::equal(res, 6.3236, 0.001)) {
+ if (!vpMath::equal(res, 6.3236, 0.001)) {
std::cerr << "Problem with vpMath::getMedian()=" << res << std::endl;
return -1;
}
std::cout << "vpMath::getMedian() is Ok !" << std::endl;
-
std::cout << "OK !" << std::endl;
return 0;
}
diff --git a/modules/core/test/math/testMatrix.cpp b/modules/core/test/math/testMatrix.cpp
index 5715e0f..a003784 100644
--- a/modules/core/test/math/testMatrix.cpp
+++ b/modules/core/test/math/testMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,25 +44,27 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpGEMM.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/core/vpGEMM.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+namespace
+{
bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << M.getRows() << "," << M.getCols() << ") = \n" << M << std::endl;
- if(bench.size() != M.size()) {
+ if (bench.size() != M.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<M.size(); i++) {
- if (std::fabs(M.data[i]-bench[i]) > std::fabs(M.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < M.size(); i++) {
+ if (std::fabs(M.data[i] - bench[i]) > std::fabs(M.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -70,8 +73,171 @@ bool test(const std::string &s, const vpMatrix &M, const std::vector<double> &be
return true;
}
-int
-main()
+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<double>::epsilon())
+{
+ if (A.getRows() != B.getRows() || A.getCols() != B.getCols()) {
+ return false;
+ }
+
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols(); j++) {
+ if (!vpMath::equal(A[i][j], B[i][j], tol)) {
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+#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 M(rows, cols);
+
+ for (unsigned int i = 0; i < M.getRows(); i++) {
+ for (unsigned int j = 0; j < M.getCols(); j++) {
+ M[i][j] = getRandomValues(min, max);
+ }
+ }
+
+ return M;
+}
+
+vpColVector generateRandomVector(const unsigned int rows, const double min, const double max)
+{
+ vpColVector v(rows);
+
+ for (unsigned int i = 0; i < v.getRows(); i++) {
+ v[i] = getRandomValues(min, max);
+ }
+
+ return v;
+}
+
+// Copy of vpMatrix::mult2Matrices
+vpMatrix dgemm_regular(const vpMatrix &A, const vpMatrix &B)
+{
+ vpMatrix C;
+
+ if ((A.getRows() != C.getRows()) || (B.getCols() != C.getCols()))
+ 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()));
+ }
+
+ // 5/12/06 some "very" simple optimization to avoid indexation
+ unsigned int BcolNum = B.getCols();
+ unsigned int BrowNum = B.getRows();
+ unsigned int i, j, k;
+ for (i = 0; i < A.getRows(); i++) {
+ double *rowptri = A[i];
+ double *ci = C[i];
+ for (j = 0; j < BcolNum; j++) {
+ double s = 0;
+ for (k = 0; k < BrowNum; k++)
+ s += rowptri[k] * B[k][j];
+ ci[j] = s;
+ }
+ }
+
+ return C;
+}
+
+// Copy of vpMatrix::AtA
+vpMatrix AtA_regular(const vpMatrix &A)
+{
+ vpMatrix B;
+ B.resize(A.getCols(), A.getCols(), false);
+
+ unsigned int i, j, k;
+ double s;
+ double *ptr;
+ for (i = 0; i < A.getCols(); i++) {
+ double *Bi = B[i];
+ for (j = 0; j < i; j++) {
+ ptr = A.data;
+ s = 0;
+ for (k = 0; k < A.getRows(); k++) {
+ s += (*(ptr + i)) * (*(ptr + j));
+ ptr += A.getCols();
+ }
+ *Bi++ = s;
+ B[j][i] = s;
+ }
+ ptr = A.data;
+ s = 0;
+ for (k = 0; k < A.getRows(); k++) {
+ s += (*(ptr + i)) * (*(ptr + i));
+ ptr += A.getCols();
+ }
+ *Bi = s;
+ }
+
+ return B;
+}
+
+// Copy of vpMatrix::multMatrixVector
+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()));
+ }
+
+ w.resize(A.getRows(), true);
+
+ for (unsigned int j = 0; j < A.getCols(); j++) {
+ double vj = v[j]; // optimization em 5/12/2006
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ w[i] += A[i][j] * vj;
+ }
+ }
+
+ return w;
+}
+
+// Copy of vpMatrix::operator*(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()));
+ }
+
+ M.resize(A.getRows(), 6, false);
+
+ unsigned int VcolNum = V.getCols();
+ unsigned int VrowNum = V.getRows();
+
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ double *rowptri = A[i];
+ double *ci = M[i];
+ for (unsigned int j = 0; j < VcolNum; j++) {
+ double s = 0;
+ for (unsigned int k = 0; k < VrowNum; k++)
+ s += rowptri[k] * V[k][j];
+ ci[j] = s;
+ }
+ }
+
+ return M;
+}
+#endif
+}
+
+int main()
{
try {
int err = 1;
@@ -87,20 +253,20 @@ main()
return err;
}
{
- vpMatrix M(4,5);
+ vpMatrix M(4, 5);
int val = 0;
- for(unsigned int i=0; i<M.getRows(); i++) {
- for(unsigned int j=0; j<M.getCols(); j++) {
+ for (unsigned int i = 0; i < M.getRows(); i++) {
+ for (unsigned int j = 0; j < M.getCols(); j++) {
M[i][j] = val++;
}
}
- std::cout <<"M ";
- M.print (std::cout, 4);
+ std::cout << "M ";
+ M.print(std::cout, 4);
vpMatrix N;
N.init(M, 0, 1, 2, 3);
- std::cout <<"N ";
- N.print (std::cout, 4);
+ std::cout << "N ";
+ N.print(std::cout, 4);
std::string header("My 4-by-5 matrix\nwith a second line");
// Save matrix in text format
@@ -172,80 +338,79 @@ main()
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST PRETTY PRINT---" << std::endl;
std::cout << "------------------------" << std::endl;
- vpMatrix M ;
+ vpMatrix M;
M.eye(4);
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
- M.print (std::cout, 4);
+ M.print(std::cout, 4);
std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
+ M.resize(3, 3);
M.eye(3);
- M[1][0]=1.235;
- M[1][1]=12.345;
- M[1][2]=.12345;
+ M[1][0] = 1.235;
+ M[1][1] = 12.345;
+ M[1][2] = .12345;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M;
std::cout << "call M.print (std::cout, 6);" << std::endl;
- M.print (std::cout, 6);
+ M.print(std::cout, 6);
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
- M[0][0]=-1.235;
- M[1][0]=-12.235;
+ M[0][0] = -1.235;
+ M[1][0] = -12.235;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 10);" << std::endl;
- M.print (std::cout, 10);
+ M.print(std::cout, 10);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 2);" << std::endl;
- M.print (std::cout, 2);
+ M.print(std::cout, 2);
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
+ M.resize(3, 3);
M.eye(3);
- M[0][2]=-0.0000000876;
+ M[0][2] = -0.0000000876;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
- M.print (std::cout, 4);
+ M.print(std::cout, 4);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
- M.print (std::cout, 10, "M");
+ M.print(std::cout, 10, "M");
std::cout << std::endl;
std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
- M.print (std::cout, 20, "M");
+ M.print(std::cout, 20, "M");
std::cout << std::endl;
-
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST RESIZE --------" << std::endl;
std::cout << "------------------------" << std::endl;
- std::cout << "5x5" << std::endl;
- M.resize(5,5,false);
+ std::cout << "5x5" << std::endl;
+ M.resize(5, 5, false);
std::cout << M << std::endl;
std::cout << "3x2" << std::endl;
- M.resize(3,2,false);
+ M.resize(3, 2, false);
std::cout << M << std::endl;
std::cout << "2x2" << std::endl;
- M.resize(2,2,false);
+ M.resize(2, 2, false);
std::cout << M << std::endl;
std::cout << "------------------------" << std::endl;
vpVelocityTwistMatrix vMe;
- vpMatrix A(1,6),B;
+ vpMatrix A(1, 6), B;
- A=1.0;
- //vMe=1.0;
- B=A*vMe;
+ A = 1.0;
+ // vMe=1.0;
+ B = A * vMe;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
@@ -269,7 +434,7 @@ main()
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
+ M.resize(3, 3);
M.eye(3);
M[1][0] = 1.5;
@@ -284,7 +449,7 @@ main()
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpGEMM " << std::endl;
std::cout << "------------------------" << std::endl;
- M.resize(3,3) ;
+ M.resize(3, 3);
M.eye(3);
vpMatrix N(3, 3);
N[0][0] = 2;
@@ -297,17 +462,402 @@ main()
vpMatrix D;
- //realise the operation D = 2 * M^T * N + 3 C
+ // realise the operation D = 2 * M^T * N + 3 C
vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
std::cout << D << std::endl;
+ }
+
+ {
+ std::cout << "------------------------" << 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
+ const unsigned int size = 100;
+
+ vpMatrix m_big(nb * size, 6);
+ std::vector<vpMatrix> submatrices(nb);
+ for (size_t cpt = 0; cpt < submatrices.size(); cpt++) {
+ vpMatrix m(size, 6);
+
+ for (unsigned int i = 0; i < m.getRows(); i++) {
+ for (unsigned int j = 0; j < m.getCols(); j++) {
+ m[i][j] = getRandomValues(-100.0, 100.0);
+ }
+ }
+
+ submatrices[cpt] = m;
+ }
+
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < nb; i++) {
+ m_big.insert(submatrices[(size_t)i], i * size, 0);
+ }
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Matrix insert(): " << t << " ms" << std::endl;
+
+ 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],
+ std::numeric_limits<double>::epsilon())) {
+ std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+ }
+
+ // Try to insert empty matrices
+ vpMatrix m1(2, 3), m2, m3;
+ m1.insert(m2, 0, 0);
+ m3.insert(m2, 0, 0);
+
+ std::cout << "Insert empty matrices:" << std::endl;
+ std::cout << "m1:\n" << m1 << std::endl;
+ std::cout << "m2:\n" << m2 << std::endl;
+ std::cout << "m3:\n" << m3 << std::endl;
- std::cout << "All tests succeed" << std::endl;
- return 0;
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpMatrix stack()" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ {
+ vpMatrix L, L2(2, 6);
+ L2 = 2;
+ L.stack(L2);
+ std::cout << "L:\n" << L << std::endl;
+ L2.resize(3, 6);
+ L2 = 3;
+ L.stack(L2);
+ std::cout << "L:\n" << L << std::endl;
+ }
+
+ vpMatrix m_big_stack;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < nb; i++) {
+ m_big_stack.stack(submatrices[(size_t)i]);
+ }
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nMatrix stack(): " << t << " ms" << std::endl;
+
+ if (!equalMatrix(m_big, m_big_stack)) {
+ std::cerr << "Problem with vpMatrix stack()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpMatrix stack(vpRowVector)" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ vpMatrix m_big_stack_row;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < m_big_stack.getRows(); i++) {
+ m_big_stack_row.stack(m_big_stack.getRow(i));
+ }
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nMatrix stack(vpRowVector): " << t << " ms" << std::endl;
+
+ if (!equalMatrix(m_big_stack, m_big_stack_row)) {
+ std::cerr << "Problem with vpMatrix stack(vpRowVector)!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpMatrix::stack()" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ {
+ vpMatrix L, L2(2, 6), L_tmp;
+ L2 = 2;
+ vpMatrix::stack(L_tmp, L2, L);
+ std::cout << "L:\n" << L << std::endl;
+ L2.resize(3, 6);
+ L2 = 3;
+ L_tmp = L;
+ vpMatrix::stack(L_tmp, L2, L);
+ std::cout << "L:\n" << L << std::endl;
+ }
+
+ 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);
+ m_big_stack_static_tmp = m_big_stack_static;
+ }
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nMatrix::stack(): " << t << " ms" << std::endl;
+
+ if (!equalMatrix(m_big, m_big_stack_static)) {
+ std::cerr << "Problem with vpMatrix::stack()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ std::cout << "------------------------" << 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);
+ 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;
+
+ if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
+ std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
+ "vpMatrix)!"
+ << std::endl;
+ return EXIT_FAILURE;
+ }
}
- }
- catch(vpException &e) {
+
+ {
+ vpMatrix m1(11, 9), m2(3, 4);
+ for (unsigned int i = 0; i < m2.getRows(); i++) {
+ for (unsigned int j = 0; j < m2.getCols(); j++) {
+ m2[i][j] = getRandomValues(-100.0, 100.0);
+ }
+ }
+
+ unsigned int offset_i = 4, offset_j = 3;
+ m1.insert(m2, offset_i, offset_j);
+
+ 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<double>::epsilon())) {
+ std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ offset_i = 4, offset_j = 5;
+ m1.insert(m2, offset_i, offset_j);
+
+ 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<double>::epsilon())) {
+ std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ offset_i = 8, offset_j = 5;
+ m1.insert(m2, offset_i, offset_j);
+
+ 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<double>::epsilon())) {
+ std::cerr << "Problem with vpMatrix insert()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+ }
+
+ {
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ vpMatrix A(5, 6), B(5, 4);
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols(); j++) {
+ A[i][j] = i * A.getCols() + j;
+
+ if (j < B.getCols()) {
+ B[i][j] = (i * B.getCols() + j) * 10;
+ }
+ }
+ }
+
+ vpMatrix juxtaposeM;
+ vpMatrix::juxtaposeMatrices(A, B, juxtaposeM);
+ std::cout << "juxtaposeM:\n" << juxtaposeM << std::endl;
+ }
+
+#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
+ {
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- BENCHMARK dgemm/dgemv" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ size_t nb_matrices = 10000;
+ unsigned int rows = 200, cols = 6;
+ double min = -1.0, max = 1.0;
+ std::vector<vpMatrix> vec_A, vec_B, vec_C, vec_C_regular;
+ vec_C.reserve(nb_matrices);
+ vec_C_regular.reserve(nb_matrices);
+
+ for (size_t i = 0; i < nb_matrices; i++) {
+ vec_A.push_back(generateRandomMatrix(cols, rows, min, max));
+ vec_B.push_back(generateRandomMatrix(rows, cols, min, max));
+ }
+
+ double t = vpTime::measureTimeMs();
+ for (size_t i = 0; i < nb_matrices; i++) {
+ 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 << "Lapack: " << t << " ms" << std::endl;
+ std::cout << "vec_C:\n" << vec_C.back() << std::endl;
+
+ t = vpTime::measureTimeMs();
+ for (size_t i = 0; i < nb_matrices; i++) {
+ vec_C_regular.push_back(dgemm_regular(vec_A[i], vec_B[i]));
+ }
+ t = vpTime::measureTimeMs() - t;
+ 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 AB, AB_regular;
+
+ t = vpTime::measureTimeMs();
+ AB = A * B;
+ t = vpTime::measureTimeMs() - t;
+ 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;
+
+ 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;
+ bool res = equalMatrix(AB, AB_regular, 1e-9);
+ std::cout << "Check result: " << res << std::endl;
+ if (!res) {
+ std::cerr << "Problem with matrix multiplication!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ int nb_iterations = 1000;
+ vpMatrix L = generateRandomMatrix(1000, 6, min, max);
+ vpMatrix LTL, LTL_regular;
+
+ t = vpTime::measureTimeMs();
+ 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 << "Lapack: " << t << " ms" << std::endl;
+ std::cout << "LTL:\n" << LTL << std::endl;
+
+ t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++)
+ LTL_regular = AtA_regular(L);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nRegular: " << t << " ms" << std::endl;
+ std::cout << "LTL_regular:\n" << LTL_regular << std::endl;
+ res = equalMatrix(LTL, LTL_regular, 1e-9);
+ std::cout << "Check result: " << res << std::endl;
+ if (!res) {
+ std::cerr << "Problem with vpMatrix::AtA()!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ vpMatrix LT = generateRandomMatrix(6, 1000, min, max);
+ vpColVector R = generateRandomVector(1000, min, max);
+ vpMatrix LTR, LTR_regular;
+
+ t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++)
+ LTR = LT * R;
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\n"
+ << nb_iterations
+ << " iterations of matrix vector multiplication: (6x1000) x "
+ "(1000x1)"
+ << std::endl;
+ std::cout << "Lapack: " << t << " ms" << std::endl;
+ std::cout << "LTR:\n" << LTR.t() << std::endl;
+
+ t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++)
+ LTR_regular = dgemv_regular(LT, R);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nRegular: " << t << " ms" << std::endl;
+ std::cout << "LTR_regular:\n" << LTR_regular.t() << std::endl;
+ res = equalMatrix(LTR, LTR_regular, 1e-9);
+ std::cout << "Check result: " << res << std::endl;
+ if (!res) {
+ std::cerr << "Problem with dgemv!" << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ 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();
+ for (int i = 0; i < nb_iterations; i++)
+ LV = L * V;
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\n"
+ << nb_iterations
+ << " iterations of matrix velocity twist matrix "
+ "multiplication: (1000x6) x (6x6)"
+ << std::endl;
+ std::cout << "Lapack: " << t << " ms" << std::endl;
+
+ t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++)
+ LV_regular = mat_mul_twist_matrix(L, V);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Regular: " << t << " ms" << std::endl;
+ 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;
+ return EXIT_FAILURE;
+ }
+ }
+#endif
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ {
+ std::vector<vpMatrix> vec_mat;
+ vec_mat.emplace_back(5, 5);
+
+ vpMatrix A(4, 4), B(4, 4);
+ A = 1;
+ B = 2;
+ vpMatrix res = A + B;
+ std::cout << "\n1) A+B:\n" << res << std::endl;
+
+ vpMatrix res2;
+ res2 = A + B;
+ std::cout << "\n2) A+B:\n" << res2 << std::endl;
+ }
+#endif
+
+ {
+ std::cout << "------------------------" << std::endl;
+ std::cout << "--- TEST vpMatrix::hadamard()" << std::endl;
+ std::cout << "------------------------" << std::endl;
+
+ vpMatrix M1(3, 5), M2(3, 5);
+ for (unsigned int i = 0; i < M1.size(); i++) {
+ M1.data[i] = i;
+ M2.data[i] = i + 2;
+ }
+
+ std::cout << "M1:\n" << M1 << std::endl;
+ std::cout << "\nM2:\n" << M2 << std::endl;
+ M2 = M1.hadamard(M2);
+ std::cout << "\nRes:\n" << M2 << std::endl;
+ }
+
+ std::cout << "\nAll tests succeed" << std::endl;
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
- return 1;
+ return EXIT_FAILURE;
}
}
-
diff --git a/modules/core/test/math/testMatrixDeterminant.cpp b/modules/core/test/math/testMatrixDeterminant.cpp
new file mode 100644
index 0000000..c772112
--- /dev/null
+++ b/modules/core/test/math/testMatrixDeterminant.cpp
@@ -0,0 +1,464 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test various determinant computation methods.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testMatrixDeterminant.cpp
+ \brief Test matrix determinant computation.
+*/
+
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdn:i:pf:R:C:vh"
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test matrix inversions\n\
+using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
+Outputs a comparison of these methods.\n\
+\n\
+SYNOPSIS\n\
+ %s [-n <number of matrices>] [-f <plot filename>]\n\
+ [-R <number of rows>] [-C <number of columns>]\n\
+ [-i <number of iterations>] [-p] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -n <number of matrices> \n\
+ Number of matrices inverted during each test loop.\n\
+\n\
+ -i <number of iterations> \n\
+ Number of iterations of the test.\n\
+\n\
+ -f <plot filename> \n\
+ Set output path for plot output.\n\
+ The plot logs the times of \n\
+ the different inversion methods: \n\
+ QR,LU,Cholesky and Pseudo-inverse.\n\
+\n\
+ -R <number of rows>\n\
+ Number of rows of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -C <number of columns>\n\
+ Number of colums of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -p \n\
+ Plot into filename in the gnuplot format. \n\
+ If this option is used, tests results will be logged \n\
+ into a filename specified with -f.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
+
+/*!
+
+ Set the program options.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
+ bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'n':
+ nb_matrices = (unsigned int)atoi(optarg_);
+ break;
+ case 'i':
+ nb_iterations = (unsigned int)atoi(optarg_);
+ break;
+ case 'f':
+ plotfile = optarg_;
+ use_plot_file = true;
+ break;
+ case 'p':
+ use_plot_file = true;
+ break;
+ case 'R':
+ nbrows = (unsigned int)atoi(optarg_);
+ break;
+ case 'C':
+ nbcols = (unsigned int)atoi(optarg_);
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ // add default options -c -d
+ case 'c':
+ break;
+ case 'd':
+ break;
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
+{
+ vpMatrix A;
+ A.resize(nbrows, nbcols);
+
+ for (unsigned int i = 0; i < A.getRows(); i++)
+ for (unsigned int j = 0; j < A.getCols(); j++)
+ A[i][j] = (double)rand() / (double)RAND_MAX;
+ return A;
+}
+
+void create_bench(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
+ std::vector<vpMatrix> &bench)
+{
+ if (verbose)
+ 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);
+ bench.push_back(M);
+ }
+}
+
+void test_det_default(bool verbose, const std::vector<vpMatrix> &bench, double &time, std::vector<double> &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;
+
+ result.resize(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().det();
+ }
+ time = vpTime::measureTimeMs() - t;
+}
+
+#if defined(VISP_HAVE_EIGEN3)
+void test_det_eigen3(bool verbose, const std::vector<vpMatrix> &bench, double &time, std::vector<double> &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;
+
+ result.resize(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().detByLUEigen3();
+ }
+ time = vpTime::measureTimeMs() - t;
+}
+#endif
+
+#if defined(VISP_HAVE_GSL)
+void test_det_gsl(bool verbose, const std::vector<vpMatrix> &bench, double &time, std::vector<double> &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;
+
+ result.resize(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().detByLUGsl();
+ }
+ time = vpTime::measureTimeMs() - t;
+}
+#endif
+
+#if defined(VISP_HAVE_LAPACK)
+void test_det_lapack(bool verbose, const std::vector<vpMatrix> &bench, double &time, std::vector<double> &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;
+
+ result.resize(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().detByLULapack();
+ }
+ time = vpTime::measureTimeMs() - t;
+}
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+void test_det_opencv(bool verbose, const std::vector<vpMatrix> &bench, double &time, std::vector<double> &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;
+
+ result.resize(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().detByLUOpenCV();
+ }
+ time = vpTime::measureTimeMs() - t;
+}
+#endif
+
+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";
+ if (verbose || !use_plot_file) {
+ std::cout << method << time << std::endl;
+ }
+}
+
+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)
+ unsigned int nb_matrices = 1000;
+ unsigned int nb_iterations = 10;
+ unsigned int nb_rows = 6;
+ unsigned int nb_cols = 6;
+ bool verbose = false;
+ std::string plotfile("plot-det.csv");
+ bool use_plot_file = false;
+ 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) {
+ exit(-1);
+ }
+
+ if (use_plot_file) {
+ of.open(plotfile.c_str());
+ of << "iter"
+ << "\t";
+
+ of << "\"Determinant default\""
+ << "\t";
+
+#if defined(VISP_HAVE_LAPACK)
+ of << "\"Determinant Lapack\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_EIGEN3)
+ of << "\"Determinant Eigen3\""
+ << "\t";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ of << "\"Determinant OpenCV\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_GSL)
+ of << "\"Determinant GSL\""
+ << "\t";
+#endif
+ of << std::endl;
+ }
+
+ int ret = EXIT_SUCCESS;
+ for (unsigned int iter = 0; iter < nb_iterations; iter++) {
+ std::vector<vpMatrix> bench;
+ create_bench(nb_matrices, nb_rows, nb_cols, verbose, bench);
+
+ if (use_plot_file)
+ of << iter << "\t";
+
+ double time;
+
+ std::vector<double> result_default;
+ test_det_default(verbose, bench, time, result_default);
+ save_time("Determinant default: ", verbose, use_plot_file, of, time);
+
+#if defined(VISP_HAVE_LAPACK)
+ std::vector<double> result_lapack;
+ test_det_lapack(verbose, bench, time, result_lapack);
+ save_time("Determinant by Lapack: ", verbose, use_plot_file, of, time);
+#endif
+
+#if defined(VISP_HAVE_EIGEN3)
+ std::vector<double> result_eigen3;
+ test_det_eigen3(verbose, bench, time, result_eigen3);
+ save_time("Determinant by Eigen3: ", verbose, use_plot_file, of, time);
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ std::vector<double> result_opencv;
+ test_det_opencv(verbose, bench, time, result_opencv);
+ save_time("Determinant by OpenCV: ", verbose, use_plot_file, of, time);
+#endif
+
+#if defined(VISP_HAVE_GSL)
+ std::vector<double> result_gsl;
+ test_det_gsl(verbose, bench, time, result_gsl);
+ save_time("Determinant by GSL: ", verbose, use_plot_file, of, time);
+#endif
+
+ if (use_plot_file)
+ of << std::endl;
+
+#if defined(VISP_HAVE_GSL) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ // 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;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+#if defined(VISP_HAVE_GSL) && defined(VISP_HAVE_LAPACK)
+ // 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;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+#if defined(VISP_HAVE_GSL) && defined(VISP_HAVE_EIGEN3)
+ // 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;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+#if defined(VISP_HAVE_LAPACK) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ // 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::endl;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+#if defined(VISP_HAVE_EIGEN3) && (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ // 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::endl;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+#if defined(VISP_HAVE_EIGEN3) && defined(VISP_HAVE_LAPACK)
+ // 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::endl;
+ ret = EXIT_FAILURE;
+ }
+ }
+#endif
+ }
+ if (use_plot_file) {
+ of.close();
+ std::cout << "Result saved in " << plotfile << std::endl;
+ }
+
+ if (ret == EXIT_SUCCESS) {
+ std::cout << "Test succeed" << std::endl;
+ } else {
+ std::cout << "Test failed" << std::endl;
+ }
+
+ return ret;
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Test does nothing since you dont't have Eigen3, Lapack, "
+ "OpenCV or GSL 3rd party"
+ << std::endl;
+ return EXIT_SUCCESS;
+#endif
+ } catch (const vpException &e) {
+ std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/core/test/math/testMatrixException.cpp b/modules/core/test/math/testMatrixException.cpp
index cf7e2bd..5b6e0be 100644
--- a/modules/core/test/math/testMatrixException.cpp
+++ b/modules/core/test/math/testMatrixException.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,33 +42,32 @@
\brief Tests matrix exception
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpMatrix.h>
int main()
{
- vpMatrix M ;
- vpMatrix M1(2,3) ;
- vpMatrix M2(3,3) ;
- vpMatrix M3(2,2) ;
+ vpMatrix M;
+ vpMatrix M1(2, 3);
+ vpMatrix M3(2, 2);
std::cout << "** test matrix exception during multiplication" << std::endl;
try {
- M = M1*M3 ;
- }
- catch (vpException &e) {
+ M = M1 * M3;
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
std::cout << "** test matrix exception during addition" << std::endl;
try {
- M = M1+M3 ;
- }
- catch (vpException &e) {
+ M = M1 + M3;
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
+
+ return 0;
}
diff --git a/modules/core/test/math/testMatrixInverse.cpp b/modules/core/test/math/testMatrixInverse.cpp
index f24785b..58f0132 100644
--- a/modules/core/test/math/testMatrixInverse.cpp
+++ b/modules/core/test/math/testMatrixInverse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -31,39 +32,29 @@
* Test various inversions.
*
* Authors:
- * Filip Novotny
+ * Fabien Spindler
*
*****************************************************************************/
-
/*!
\example testMatrixInverse.cpp
\brief Test various matrix inversions.
*/
+#include <cmath>
+#include <fstream>
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
-
-#include <visp3/core/vpTime.h>
-
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTime.h>
#include <visp3/io/vpParseArgv.h>
-#include <vector>
-#include <stdlib.h>
-#include <stdio.h>
-#include <fstream>
-#include <iostream>
-#include <cmath>
-// List of allowed command line options
-#define GETOPTARGS "cdn:i:pf:R:C:vh"
-void usage(const char *name, const char *badparam);
-bool getOptions(int argc, const char **argv,
- unsigned int& nb_matrices, unsigned int& nb_iterations,
- bool& use_plot_file, std::string& plotfile,
- unsigned int& nbrows, unsigned int& nbcols, bool& verbose);
-void writeTime(const char *name, double time);
-vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols);
+// List of allowed command line options
+#define GETOPTARGS "cdn:i:pf:R:C:vh"
/*!
@@ -116,11 +107,11 @@ OPTIONS: Default\n\
Print the help.\n\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
+
/*!
Set the program options.
@@ -128,17 +119,18 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
case 'n':
nb_matrices = (unsigned int)atoi(optarg_);
break;
@@ -161,14 +153,15 @@ bool getOptions(int argc, const char **argv,
case 'v':
verbose = true;
break;
- // add default options -c -d
+ // add default options -c -d
case 'c':
- break;
+ break;
case 'd':
- break;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -183,141 +176,421 @@ bool getOptions(int argc, const char **argv,
return true;
}
-void writeTime(const char *name, double time)
+vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
{
- std::cout << name << " time=" << time << " ms." << std::endl;
+ vpMatrix A;
+ A.resize(nbrows, nbcols);
+
+ for (unsigned int i = 0; i < A.getRows(); i++)
+ for (unsigned int j = 0; j < A.getCols(); j++)
+ A[i][j] = (double)rand() / (double)RAND_MAX;
+ return A;
}
-vpMatrix makeRandomMatrix(unsigned int nbrows, unsigned int nbcols)
+vpMatrix make_random_symmetric_positive_matrix(unsigned int n)
{
vpMatrix A;
- A.resize(nbrows,nbcols);
+ A.resize(n, n);
+ vpMatrix I;
+ I.eye(n);
+
+ for (unsigned int i = 0; i < A.getRows(); i++)
+ for (unsigned int j = 0; j < A.getCols(); j++)
+ A[i][j] = (double)rand() / (double)RAND_MAX;
- for (unsigned int i=0 ; i < A.getRows() ; i++)
- for (unsigned int j=0 ; j < A.getCols() ; j++)
- A[i][j] = (double)rand()/(double)RAND_MAX;
+ A = 0.5 * (A + A.t());
+ A = A + n * I;
return A;
}
+void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
+ std::vector<vpMatrix> &bench)
+{
+ if (verbose)
+ 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)
+ 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;
+ 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;
+ }
+ }
+#else
+ M = make_random_matrix(nb_rows, nb_cols);
+#endif
+ bench.push_back(M);
+ }
+}
+
+void create_bench_symmetric_positive_matrix(unsigned int nb_matrices, unsigned int n, bool verbose,
+ std::vector<vpMatrix> &bench)
+{
+ if (verbose)
+ 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)
+ 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;
+ 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 not "
+ "invertibleL: det="
+ << det << ". Retrying..." << std::endl;
+ }
+ }
+#else
+ M = make_random_symmetric_positive_matrix(n);
+#endif
+ bench.push_back(M);
+ }
+}
+
+int test_inverse(const std::vector<vpMatrix> &bench, const std::vector<vpMatrix> &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;
+ return EXIT_FAILURE;
+ }
+ }
+ return EXIT_SUCCESS;
+}
-int
-main(int argc, const char ** argv)
+#if defined(VISP_HAVE_EIGEN3)
+int test_inverse_lu_eigen3(bool verbose, const std::vector<vpMatrix> &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()
+ << " matrix using LU decomposition (Eigen3)." << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().inverseByLUEigen3() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+#endif
+
+#if defined(VISP_HAVE_LAPACK)
+int test_inverse_lu_lapack(bool verbose, const std::vector<vpMatrix> &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()
+ << " matrix using LU decomposition (Lapack)." << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().inverseByLULapack() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+
+int test_inverse_cholesky_lapack(bool verbose, const std::vector<vpMatrix> &bench, double &time)
+{
+ if (verbose)
+ 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::vector<vpMatrix> 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();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+
+int test_inverse_qr_lapack(bool verbose, const std::vector<vpMatrix> &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()
+ << " matrix using QR decomposition (Lapack)" << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().inverseByQRLapack() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+#endif
+
+#if defined(VISP_HAVE_GSL)
+int test_inverse_lu_gsl(bool verbose, const std::vector<vpMatrix> &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()
+ << " matrix using LU decomposition (GSL)" << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().inverseByLUGsl() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+int test_inverse_lu_opencv(bool verbose, const std::vector<vpMatrix> &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()
+ << " matrix using LU decomposition (OpenCV)" << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().inverseByLUOpenCV() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+
+int test_inverse_cholesky_opencv(bool verbose, const std::vector<vpMatrix> &bench, double &time)
+{
+ if (verbose)
+ 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()
+ << " matrix using Cholesky decomposition (OpenCV)" << std::endl;
+ std::vector<vpMatrix> 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();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+#endif
+
+#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<vpMatrix> &bench, double &time)
+{
+ if (verbose)
+ std::cout << "Test pseudo inverse using either Eigen3, Lapack, OpenCV or "
+ "GSL 3rd party"
+ << std::endl;
+ // Compute inverse
+ if (verbose)
+ std::cout << " Pseudo inverting " << bench[0].AtA().getRows() << "x" << bench[0].AtA().getCols() << " matrix"
+ << std::endl;
+ std::vector<vpMatrix> result(bench.size());
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ result[i] = bench[i].AtA().pseudoInverse() * bench[i].transpose();
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ // Test inverse
+ return test_inverse(bench, result);
+}
+#endif
+
+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";
+ if (verbose || !use_plot_file) {
+ std::cout << method << time << std::endl;
+ }
+}
+
+int main(int argc, const char *argv[])
{
-#ifdef VISP_HAVE_LAPACK_C
try {
- unsigned int nb_matrices=1000;
- unsigned int nb_iterations=10;
+#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;
unsigned int nb_cols = 6;
bool verbose = false;
- std::string plotfile("plot.txt");
- bool use_plot_file=false;
+ std::string plotfile("plot-inv.csv");
+ bool use_plot_file = false;
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) {
- exit (-1);
+ if (getOptions(argc, argv, nb_matrices, nb_iterations, use_plot_file, plotfile, nb_rows, nb_cols, verbose) ==
+ false) {
+ exit(-1);
}
- if(use_plot_file){
+ if (use_plot_file) {
of.open(plotfile.c_str());
+ of << "iter"
+ << "\t";
+
+#if defined(VISP_HAVE_LAPACK)
+ of << "\"LU Lapack\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_EIGEN3)
+ of << "\"LU Eigen3\""
+ << "\t";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ of << "\"LU OpenCV\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_GSL)
+ of << "\"LU GSL\""
+ << "\t";
+#endif
+
+#if defined(VISP_HAVE_LAPACK)
+ of << "\"Cholesky Lapack\""
+ << "\t";
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ of << "\"Cholesky OpenCV\""
+ << "\t";
+#endif
+
+#if defined(VISP_HAVE_LAPACK)
+ of << "\"QR Lapack\""
+ << "\t";
+#endif
+
+#if defined(VISP_HAVE_LAPACK) || (VISP_HAVE_OPENCV_VERSION >= 0x020101) || defined(VISP_HAVE_GSL)
+ of << "\"Pseudo inverse (Lapack, OpenCV, GSL)\""
+ << "\t";
+#endif
+ of << std::endl;
}
- for(unsigned int iter=0;iter<nb_iterations;iter++){
- std::vector<vpMatrix> benchQR;
- std::vector<vpMatrix> benchLU;
- std::vector<vpMatrix> benchCholesky;
- std::vector<vpMatrix> benchPseudoInverse;
- if(verbose)
- std::cout << "********* generating matrices for iteration " << iter << "." << std::endl;
- for(unsigned int i=0;i<nb_matrices;i++){
- vpMatrix cur;
- double det=0.;
- //don't put singular matrices in the benchmark
- for(cur=makeRandomMatrix(nb_rows,nb_cols); std::fabs(det=cur.AtA().det())<.01; cur = makeRandomMatrix(nb_rows,nb_cols))
- if(verbose){
- std::cout << "Generated random matrix A*tA=" << std::endl << cur.AtA() << std::endl;
- std::cout << "generated random matrix not invertibleL: det="<<det<< ". Retrying..." << std::endl;
- }
- benchCholesky.push_back(cur);
- benchQR.push_back(cur);
- benchLU.push_back(cur);
- benchPseudoInverse.push_back(cur);
- }
+ int ret = EXIT_SUCCESS;
+ for (unsigned int iter = 0; iter < nb_iterations; iter++) {
+ std::vector<vpMatrix> bench_random_matrices;
+ create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices);
+ std::vector<vpMatrix> 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";
+
+ double time;
+// LU decomposition
+#if defined(VISP_HAVE_LAPACK)
+ ret += test_inverse_lu_lapack(verbose, bench_random_matrices, time);
+ save_time("Inverse by LU (Lapack): ", verbose, use_plot_file, of, time);
+#endif
- if(verbose)
- std::cout << "\t Inverting " << benchCholesky[0].AtA().getRows() << "x" << benchCholesky[0].AtA().getCols() << " matrix using cholesky decomposition." << std::endl;
- double t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchCholesky[i]=benchCholesky[i].AtA().inverseByCholesky()*benchCholesky[i].transpose();
- }
- double chol_time = vpTime::measureTimeMs() - t ;
-
- if(verbose)
- std::cout << "\t Inverting " << benchLU[0].AtA().getRows() << "x" << benchLU[0].AtA().getCols() << " matrix using LU decomposition." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++)
- benchLU[i] = benchLU[i].AtA().inverseByLU()*benchLU[i].transpose();
- double lu_time = vpTime::measureTimeMs() -t ;
-
- if(verbose)
- std::cout << "\t Inverting " << benchQR[0].AtA().getRows() << "x" << benchQR[0].AtA().getCols() << " matrix using QR decomposition." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchQR[i]=benchQR[i].AtA().inverseByQR()*benchQR[i].transpose();
- }
- double qr_time = vpTime::measureTimeMs() - t ;
+#if defined(VISP_HAVE_EIGEN3)
+ ret += test_inverse_lu_eigen3(verbose, bench_random_matrices, time);
+ save_time("Inverse by LU (Eigen3): ", verbose, use_plot_file, of, time);
+#endif
- if(verbose)
- std::cout << "\t Inverting " << benchPseudoInverse[0].AtA().getRows() << "x" << benchPseudoInverse[0].AtA().getCols() << " matrix while computing pseudo-inverse." << std::endl;
- t = vpTime::measureTimeMs() ;
- for(unsigned int i=0;i<nb_matrices;i++){
- benchPseudoInverse[i]=benchPseudoInverse[i].pseudoInverse();
- }
- double pi_time = vpTime::measureTimeMs() - t ;
-
- double avg_err_lu_qr=0.;
- double avg_err_lu_pi=0.;
- double avg_err_lu_chol=0.;
- double avg_err_qr_pi=0.;
- double avg_err_qr_chol=0.;
- double avg_err_pi_chol=0.;
-
- for(unsigned int i=0;i<nb_matrices;i++){
- avg_err_lu_qr+= (benchQR[i]-benchLU[i]).euclideanNorm();
- avg_err_lu_pi+= (benchPseudoInverse[i]-benchLU[i]).euclideanNorm();
- avg_err_qr_pi+= (benchPseudoInverse[i]-benchQR[i]).euclideanNorm();
- avg_err_qr_chol+= (benchCholesky[i]-benchQR[i]).euclideanNorm();
- avg_err_lu_chol+= (benchCholesky[i]-benchLU[i]).euclideanNorm();
- avg_err_pi_chol+= (benchCholesky[i]-benchPseudoInverse[i]).euclideanNorm();
- }
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ ret += test_inverse_lu_opencv(verbose, bench_random_matrices, time);
+ save_time("Inverse by LU (OpenCV): ", verbose, use_plot_file, of, time);
+#endif
+
+#if defined(VISP_HAVE_GSL)
+ ret += test_inverse_lu_gsl(verbose, bench_random_matrices, time);
+ save_time("Inverse by LU (GSL): ", verbose, use_plot_file, of, time);
+#endif
- avg_err_lu_qr/=nb_matrices;
- avg_err_lu_pi/=nb_matrices;
- avg_err_qr_pi/=nb_matrices;
+// 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);
+#endif
- if(use_plot_file){
- of << iter << "\t" << lu_time << "\t" << qr_time << "\t" << pi_time << "\t" << chol_time << "\t" << avg_err_lu_qr << "\t" << avg_err_qr_pi << "\t" << avg_err_lu_pi << "\t" << avg_err_qr_chol << "\t" << avg_err_lu_chol << "\t" << avg_err_pi_chol << std::endl;
- }
- if(verbose || !use_plot_file){
- writeTime("LU",lu_time);
- writeTime("QR",qr_time);
- writeTime("Pseudo-inverse",pi_time);
- writeTime("Cholesky",chol_time);
- }
+#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);
+#endif
+
+// QR decomposition
+#if defined(VISP_HAVE_LAPACK)
+ ret += test_inverse_qr_lapack(verbose, bench_random_matrices, time);
+ save_time("Inverse by QR (Lapack): ", verbose, use_plot_file, of, time);
+#endif
+
+// Pseudo-inverse with SVD
+#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);
+#endif
+
+ if (use_plot_file)
+ of << std::endl;
+ }
+ if (use_plot_file) {
+ of.close();
+ std::cout << "Result saved in " << plotfile << std::endl;
}
- return 0;
- }
- catch(vpException &e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
- }
+ if (ret == EXIT_SUCCESS) {
+ std::cout << "Test succeed" << std::endl;
+ } else {
+ std::cout << "Test failed" << std::endl;
+ }
+
+ return ret;
#else
- (void)argc;
- (void)argv;
- std::cout << "You don't have lapack installed" << std::endl;
- return 0;
+ (void)argc;
+ (void)argv;
+ std::cout << "Test does nothing since you dont't have Eigen3, Lapack, "
+ "OpenCV or GSL 3rd party"
+ << std::endl;
+ return EXIT_SUCCESS;
#endif
+ } catch (const vpException &e) {
+ std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
+ return EXIT_FAILURE;
+ }
}
-
diff --git a/modules/core/test/math/testMatrixPseudoInverse.cpp b/modules/core/test/math/testMatrixPseudoInverse.cpp
new file mode 100644
index 0000000..c21b62f
--- /dev/null
+++ b/modules/core/test/math/testMatrixPseudoInverse.cpp
@@ -0,0 +1,720 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test various svd decompositions.
+ *
+ * Authors:
+ * Fabien Spindler
+ *
+ *****************************************************************************/
+
+/*!
+ \example testMatrixPseudoInverse.cpp
+ \brief Test various pseudo inverse implementations.
+*/
+
+#include <algorithm>
+#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
+
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdn:i:pf:R:C:vh"
+
+/*!
+
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Test matrix pseudo-inverse.\n\
+Outputs a comparison of the results obtained by supported 3rd parties.\n\
+\n\
+SYNOPSIS\n\
+ %s [-n <number of matrices>] [-f <plot filename>]\n\
+ [-R <number of rows>] [-C <number of columns>]\n\
+ [-i <number of iterations>] [-p] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -n <number of matrices> \n\
+ Number of matrices inverted during each test loop.\n\
+\n\
+ -i <number of iterations> \n\
+ Number of iterations of the test.\n\
+\n\
+ -f <plot filename> \n\
+ Set output path for plot output.\n\
+ The plot logs the times of \n\
+ the different inversion methods: \n\
+ QR,LU,Cholesky and Pseudo-inverse.\n\
+\n\
+ -R <number of rows>\n\
+ Number of rows of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -C <number of columns>\n\
+ Number of colums of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -p \n\
+ Plot into filename in the gnuplot format. \n\
+ If this option is used, tests results will be logged \n\
+ into a filename specified with -f.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
+
+/*!
+
+ Set the program options.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
+ bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'n':
+ nb_matrices = (unsigned int)atoi(optarg_);
+ break;
+ case 'i':
+ nb_iterations = (unsigned int)atoi(optarg_);
+ break;
+ case 'f':
+ plotfile = optarg_;
+ use_plot_file = true;
+ break;
+ case 'p':
+ use_plot_file = true;
+ break;
+ case 'R':
+ nbrows = (unsigned int)atoi(optarg_);
+ break;
+ case 'C':
+ nbcols = (unsigned int)atoi(optarg_);
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ // add default options -c -d
+ case 'c':
+ break;
+ case 'd':
+ break;
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
+{
+ vpMatrix A;
+ A.resize(nbrows, nbcols);
+
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols(); j++) {
+ A[i][j] = (double)rand() / (double)RAND_MAX;
+ }
+ }
+
+ return A;
+}
+
+void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
+ std::vector<vpMatrix> &bench)
+{
+ if (verbose)
+ 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);
+ bench.push_back(M);
+ }
+}
+
+int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &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;
+ return EXIT_FAILURE;
+ }
+ }
+ return EXIT_SUCCESS;
+}
+
+int test_pseudo_inverse(const std::vector<vpMatrix> &A, const std::vector<vpMatrix> &Api,
+ const std::vector<vpColVector> &sv, const std::vector<vpMatrix> &imA,
+ const std::vector<vpMatrix> &imAt, const std::vector<vpMatrix> &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;
+ return EXIT_FAILURE;
+ }
+ }
+
+ // test kerA
+ for (unsigned int i = 0; i < kerAt.size(); i++) {
+ if (kerAt[i].size()) {
+ vpMatrix nullspace = A[i] * kerAt[i].t();
+ double error = nullspace.euclideanNorm();
+
+ if (error > allowed_error) {
+ std::cout << "Bad kernel [" << i << "]: euclidean norm: " << error << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ // test sv, imA, imAt, kerA
+ for (unsigned int i = 0; i < kerAt.size(); i++) {
+ unsigned int rank = imA[i].getCols();
+ vpMatrix U, S(rank, A[i].getCols()), Vt(A[i].getCols(), A[i].getCols());
+ U = imA[i];
+
+ for (unsigned int j = 0; j < rank; j++)
+ S[j][j] = sv[i][j];
+
+ Vt.insert(imAt[i].t(), 0, 0);
+ Vt.insert(kerAt[i], imAt[i].getCols(), 0);
+
+ 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;
+ return EXIT_FAILURE;
+ }
+ }
+
+ return EXIT_SUCCESS;
+}
+
+int test_pseudo_inverse_default(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &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;
+
+ size_t size = bench.size();
+ std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
+ std::vector<vpColVector> sv(size);
+ int ret = EXIT_SUCCESS;
+
+ // test 0
+ unsigned int test = 0;
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ PI[i] = bench[i].pseudoInverse();
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 1
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverse(PI[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 2
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverse(PI[i], sv[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 3
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverse(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
+ }
+
+ return ret;
+}
+
+#if defined(VISP_HAVE_EIGEN3)
+int test_pseudo_inverse_eigen3(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &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;
+
+ size_t size = bench.size();
+ std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
+ std::vector<vpColVector> sv(size);
+ int ret = EXIT_SUCCESS;
+
+ // test 0
+ unsigned int test = 0;
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ PI[i] = bench[i].pseudoInverseEigen3();
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 1
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseEigen3(PI[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 2
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseEigen3(PI[i], sv[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 3
+ 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]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
+ }
+
+ return ret;
+}
+#endif
+
+#if defined(VISP_HAVE_LAPACK)
+int test_pseudo_inverse_lapack(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &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;
+
+ size_t size = bench.size();
+ std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
+ std::vector<vpColVector> sv(size);
+ int ret = EXIT_SUCCESS;
+
+ // test 0
+ unsigned int test = 0;
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ PI[i] = bench[i].pseudoInverseLapack();
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 1
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseLapack(PI[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 2
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseLapack(PI[i], sv[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 3
+ 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]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
+ }
+
+ return ret;
+}
+#endif
+
+#if defined(VISP_HAVE_GSL)
+int test_pseudo_inverse_gsl(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &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;
+
+ size_t size = bench.size();
+ std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
+ std::vector<vpColVector> sv(size);
+ int ret = EXIT_SUCCESS;
+
+ // test 0
+ unsigned int test = 0;
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ PI[i] = bench[i].pseudoInverseGsl();
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 1
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseGsl(PI[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 2
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseGsl(PI[i], sv[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 3
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseGsl(PI[i], sv[i], 1e-6, imA[i], imAt[i], kerAt[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
+ }
+
+ return ret;
+}
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+int test_pseudo_inverse_opencv(bool verbose, const std::vector<vpMatrix> &bench, std::vector<double> &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;
+
+ size_t size = bench.size();
+ std::vector<vpMatrix> PI(size), imA(size), imAt(size), kerAt(size);
+ std::vector<vpColVector> sv(size);
+ int ret = EXIT_SUCCESS;
+
+ // test 0
+ unsigned int test = 0;
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ PI[i] = bench[i].pseudoInverseOpenCV();
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 1
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseOpenCV(PI[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 2
+ test++;
+ t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ bench[i].pseudoInverseOpenCV(PI[i], sv[i]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI);
+ }
+
+ // test 3
+ 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]);
+ }
+ time[test] = vpTime::measureTimeMs() - t;
+
+ for (unsigned int i = 0; i < time.size(); i++) {
+ ret += test_pseudo_inverse(bench, PI, sv, imA, imAt, kerAt);
+ }
+
+ return ret;
+}
+#endif
+
+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<double> &time)
+{
+ for (size_t i = 0; i < time.size(); i++) {
+ if (use_plot_file)
+ of << time[i] << "\t";
+ if (verbose) {
+ std::cout << " " << method << " svd(" << nrows << "x" << ncols << ")"
+ << " test " << i << ": " << time[i] << std::endl;
+ }
+ }
+}
+
+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)
+ unsigned int nb_matrices = 500;
+ unsigned int nb_iterations = 10;
+ unsigned int nb_rows = 12;
+ unsigned int nb_cols = 6;
+ bool verbose = false;
+ std::string plotfile("plot-pseudo-inv.csv");
+ 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_test_matrix_size = 3; // 3 tests: m > n, m = n, m < n
+ std::vector<double> time(nb_svd_functions);
+ std::vector<unsigned int> 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) {
+ exit(-1);
+ }
+
+ for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
+ // consider m > n, m = n, m < n
+ if (s == 0) {
+ nrows[s] = nb_rows;
+ ncols[s] = nb_cols;
+ } else if (s == 1) {
+ nrows[s] = nb_cols;
+ ncols[s] = nb_cols;
+ } else {
+ nrows[s] = nb_cols;
+ ncols[s] = nb_rows;
+ }
+ }
+
+ if (use_plot_file) {
+ of.open(plotfile.c_str());
+ of << "iter"
+ << "\t";
+
+ 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 << "\""
+ << "\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 << "\""
+ << "\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 << "\""
+ << "\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 << "\""
+ << "\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 << "\""
+ << "\t";
+#endif
+ }
+ of << std::endl;
+ }
+
+ int ret = EXIT_SUCCESS;
+ for (unsigned int iter = 0; iter < nb_iterations; iter++) {
+
+ if (use_plot_file)
+ of << iter << "\t";
+
+ for (unsigned int s = 0; s < nb_test_matrix_size; s++) {
+ std::vector<vpMatrix> 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);
+
+#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);
+#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);
+#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);
+#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);
+#endif
+ }
+ if (use_plot_file)
+ of << std::endl;
+ }
+ if (use_plot_file) {
+ of.close();
+ std::cout << "Result saved in " << plotfile << std::endl;
+ }
+
+ if (ret == EXIT_SUCCESS) {
+ std::cout << "Test succeed" << std::endl;
+ } else {
+ std::cout << "Test failed" << std::endl;
+ }
+
+ return ret;
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Test does nothing since you dont't have Eigen3, Lapack, "
+ "OpenCV or GSL 3rd party"
+ << std::endl;
+ return EXIT_SUCCESS;
+#endif
+ } catch (const vpException &e) {
+ std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/core/test/math/testPoseVector.cpp b/modules/core/test/math/testPoseVector.cpp
index 6c9aa98..49d9aa9 100644
--- a/modules/core/test/math/testPoseVector.cpp
+++ b/modules/core/test/math/testPoseVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,8 +43,8 @@
*/
#include <cmath>
-#include <vector>
#include <limits>
+#include <vector>
#include <visp3/core/vpPoseVector.h>
@@ -52,12 +53,12 @@ bool test(const std::string &s, const vpArray2D<double> &A, const std::vector<do
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A << std::endl;
- if(bench.size() != A.size()) {
+ if (bench.size() != A.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<A.size(); i++) {
- if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < A.size(); i++) {
+ if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -69,7 +70,7 @@ int main()
{
{
vpPoseVector p;
- std::vector<double> bench(6,0);
+ std::vector<double> bench(6, 0);
int err = 1;
if (test("p", p, bench) == false)
return err;
diff --git a/modules/core/test/math/testQuaternion.cpp b/modules/core/test/math/testQuaternion.cpp
index e03542c..5d3a53f 100644
--- a/modules/core/test/math/testQuaternion.cpp
+++ b/modules/core/test/math/testQuaternion.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,102 +36,88 @@
*
*****************************************************************************/
-
/*!
\file testQuaternion.cpp
\brief Tests quaternion operations.
*/
-
#include <limits>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpQuaternionVector.h>
-#include <visp3/core/vpException.h>
-
int main()
{
try {
- //Test addition of two quaternions
+ // Test addition of two quaternions
vpQuaternionVector q1(2.1, -1, -3.7, 1.5);
vpQuaternionVector q2(0.5, 1.4, 0.7, 2.5);
vpQuaternionVector q3 = q1 + q2;
std::cout << "q3=" << q3 << std::endl;
- if(!vpMath::equal(q3.x(), 2.6, std::numeric_limits<double>::epsilon()) ||
- !vpMath::equal(q3.y(), 0.4, std::numeric_limits<double>::epsilon()) ||
- !vpMath::equal(q3.z(), -3.0, std::numeric_limits<double>::epsilon()) ||
- !vpMath::equal(q3.w(), 4.0, std::numeric_limits<double>::epsilon())) {
+ if (!vpMath::equal(q3.x(), 2.6, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.y(), 0.4, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.z(), -3.0, std::numeric_limits<double>::epsilon()) ||
+ !vpMath::equal(q3.w(), 4.0, std::numeric_limits<double>::epsilon())) {
throw vpException(vpException::fatalError, "Problem with addition of two quaternions !");
}
-
- //Test subtraction of two quaternions
- vpQuaternionVector q4 = q3-q1;
+ // Test subtraction of two quaternions
+ vpQuaternionVector q4 = q3 - q1;
std::cout << "q4=" << q4 << std::endl;
- if(!vpMath::equal(q4.x(), q2.x(), std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q4.y(), q2.y(), std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q4.z(), q2.z(), std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q4.w(), q2.w(), std::numeric_limits<double>::epsilon()*1e4)) {
+ if (!vpMath::equal(q4.x(), q2.x(), std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q4.y(), q2.y(), std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q4.z(), q2.z(), std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q4.w(), q2.w(), std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with subtraction of two quaternions !");
}
-
- //Test multiplication of two quaternions
- //https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
+ // Test multiplication of two quaternions
+ // https://www.wolframalpha.com/input/?i=quaternion+-Sin%5BPi%5D%2B3i%2B4j%2B3k+multiplied+by+-1j%2B3.9i%2B4-3k&lk=3
vpQuaternionVector q5(3.0, 4.0, 3.0, -sin(M_PI));
vpQuaternionVector q6(3.9, -1.0, -3.0, 4.0);
vpQuaternionVector q7 = q5 * q6;
std::cout << "q7=" << q7 << std::endl;
- if(!vpMath::equal(q7.x(), 3.0, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7.y(), 36.7, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7.z(), -6.6, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7.w(), 1.3, std::numeric_limits<double>::epsilon()*1e4)) {
+ if (!vpMath::equal(q7.x(), 3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7.y(), 36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7.z(), -6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with multiplication of two quaternions !");
}
-
- //Test quaternion conjugate
+ // Test quaternion conjugate
vpQuaternionVector q7_conj = q7.conjugate();
std::cout << "q7_conj=" << q7_conj << std::endl;
- if(!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits<double>::epsilon()*1e4) ||
- !vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits<double>::epsilon()*1e4)) {
+ if (!vpMath::equal(q7_conj.x(), -3.0, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7_conj.y(), -36.7, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7_conj.z(), 6.6, std::numeric_limits<double>::epsilon() * 1e4) ||
+ !vpMath::equal(q7_conj.w(), 1.3, std::numeric_limits<double>::epsilon() * 1e4)) {
throw vpException(vpException::fatalError, "Problem with quaternion conjugate !");
}
-
- //Test quaternion inverse
+ // 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)) {
+ 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
+ // 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)) {
+ if (!vpMath::equal(q7_norm, 37.4318, 0.0001)) {
throw vpException(vpException::fatalError, "Problem with quaternion magnitude !");
}
-
- //Test quaternion normalization
+ // 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)) {
+ 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
+ // Test copy constructor
vpQuaternionVector q_copy1 = vpQuaternionVector(0, 0, 1, 1);
std::cout << "q_copy1=" << q_copy1 << std::endl;
vpQuaternionVector q_copy2 = q_copy1;
@@ -138,17 +125,14 @@ int main()
std::cout << "q_copy1 after set=" << q_copy1 << std::endl;
std::cout << "q_copy2=" << q_copy2 << std::endl;
-
- //Test assignment operator
+ // Test assignment operator
vpQuaternionVector q_copy3(10, 10, 10, 10);
q_copy3 = q_copy1;
std::cout << "q_copy3=" << q_copy3 << std::endl;
-
std::cout << "vpQuaternion operations are ok !" << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/math/testRobust.cpp b/modules/core/test/math/testRobust.cpp
index 5a32f44..042eb44 100644
--- a/modules/core/test/math/testRobust.cpp
+++ b/modules/core/test/math/testRobust.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,17 +42,16 @@
Test some vpMath functionalities.
*/
-
-#include <visp3/core/vpRobust.h>
-#include <string>
#include <fstream>
+#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpRobust.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <iostream>
// List of allowed command line options
-#define GETOPTARGS "cdho:"
+#define GETOPTARGS "cdho:"
void usage(const char *name, const char *badparam, std::string ofilename);
bool getOptions(int argc, const char **argv, std::string &ofilename);
@@ -88,8 +88,7 @@ OPTIONS: Default\n\
weights.\n\
\n\
-h\n\
- Print the help.\n",
- ofilename.c_str());
+ Print the help.\n", ofilename.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -107,19 +106,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, std::string &ofilename)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'o': ofilename = optarg_; break;
- case 'h': usage(argv[0], NULL, ofilename); return false; break;
+ case 'o':
+ ofilename = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ofilename);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
usage(argv[0], optarg_, ofilename);
- return false; break;
+ return false;
+ break;
}
}
@@ -134,16 +139,13 @@ bool getOptions(int argc, const char **argv, std::string &ofilename)
return true;
}
-
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string ofilename;
std::string username;
- // Set the default output filename
+// Set the default output filename
#if defined(_WIN32)
ofilename = "C:/temp";
#else
@@ -161,11 +163,9 @@ main(int argc, const char ** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(ofilename);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, ofilename);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << ofilename << std::endl;
std::cerr << " Check your -o " << ofilename << " option " << std::endl;
exit(-1);
@@ -177,43 +177,35 @@ main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, ofilename) == false) {
- exit (-1);
+ exit(-1);
}
- double sig = 1 ;
+ double sig = 1;
- double w ;
+ double w;
std::ofstream f;
std::cout << "Create file: " << ofilename << std::endl;
f.open(ofilename.c_str());
if (f.fail()) {
usage(argv[0], NULL, ofilename);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create the file: " << ofilename << std::endl;
std::cerr << " Check your -o " << ofilename << " option " << std::endl;
exit(-1);
-
}
- double x = -10 ;
- while (x<10)
- {
- if (fabs(x/sig)<=(4.6851))
- {
- w = vpMath::sqr(1-vpMath::sqr(x/(sig*4.6851)));
- }
- else
- {
+ double x = -10;
+ while (x < 10) {
+ if (fabs(x / sig) <= (4.6851)) {
+ w = vpMath::sqr(1 - vpMath::sqr(x / (sig * 4.6851)));
+ } else {
w = 0;
}
- f << x <<" "<<w <<std::endl ;
- x+= 0.01 ;
+ f << x << " " << w << std::endl;
+ x += 0.01;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/modules/core/test/math/testRotation.cpp b/modules/core/test/math/testRotation.cpp
index 4001d77..c24d76a 100644
--- a/modules/core/test/math/testRotation.cpp
+++ b/modules/core/test/math/testRotation.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,22 +37,20 @@
*
*****************************************************************************/
-
/*!
\file testRotation.cpp
\brief Tests transformation within various representations of rotation.
*/
-
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpQuaternionVector.h>
#include <visp3/core/vpRotationMatrix.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpQuaternionVector.h>
-#include <stdlib.h>
-#include <stdio.h>
#include <cassert>
#include <limits>
+#include <stdio.h>
+#include <stdlib.h>
static unsigned int cpt = 0;
@@ -59,12 +58,12 @@ bool test(const std::string &s, const vpArray2D<double> &v, const std::vector<do
{
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
- if(bench.size() != v.size()) {
+ if (bench.size() != v.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<v.size(); i++) {
- if (std::fabs(v.data[i]-bench[i]) > std::fabs(v.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < v.size(); i++) {
+ if (std::fabs(v.data[i] - bench[i]) > std::fabs(v.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -77,8 +76,8 @@ bool test(const std::string &s, const vpRotationVector &v, const double &bench)
{
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
- for (unsigned int i=0; i<v.size(); i++) {
- if (std::fabs(v[i]-bench) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < v.size(); i++) {
+ if (std::fabs(v[i] - bench) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -90,270 +89,267 @@ bool test(const std::string &s, const vpRotationVector &v, const double &bench)
int main()
{
try {
- int err = 1;
{
vpThetaUVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
std::vector<double> bench1(3, vpMath::rad(10));
if (test("r1", r1, bench1) == false)
- return err;
+ return EXIT_FAILURE;
vpThetaUVector r2 = r1;
if (test("r2", r2, bench1) == false)
- return err;
+ return EXIT_FAILURE;
if (test("r2", r2, vpMath::rad(10)) == false)
- return err;
+ return EXIT_FAILURE;
vpThetaUVector r3;
r3 = vpMath::rad(10);
if (test("r3", r3, bench1) == false)
- return err;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < r3.size(); i++) {
+ if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
vpColVector r4 = 0.5 * r1;
std::vector<double> bench2(3, vpMath::rad(5));
if (test("r4", r4, bench2) == false)
- return err;
+ return EXIT_FAILURE;
vpThetaUVector r5(r3);
if (test("r5", r5, bench1) == false)
- return err;
+ return EXIT_FAILURE;
}
{
vpRxyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
std::vector<double> bench1(3, vpMath::rad(10));
if (test("r1", r1, bench1) == false)
- return err;
+ return EXIT_FAILURE;
vpRxyzVector r2 = r1;
if (test("r2", r2, bench1) == false)
- return err;
+ return EXIT_FAILURE;
if (test("r2", r2, vpMath::rad(10)) == false)
- return err;
+ return EXIT_FAILURE;
vpRxyzVector r3;
r3 = vpMath::rad(10);
if (test("r3", r3, bench1) == false)
- return err;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < r3.size(); i++) {
+ if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
vpColVector r4 = 0.5 * r1;
std::vector<double> bench2(3, vpMath::rad(5));
if (test("r4", r4, bench2) == false)
- return err;
+ return EXIT_FAILURE;
vpRxyzVector r5(r3);
if (test("r5", r5, bench1) == false)
- return err;
+ return EXIT_FAILURE;
}
{
vpRzyxVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
std::vector<double> bench1(3, vpMath::rad(10));
if (test("r1", r1, bench1) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyxVector r2 = r1;
if (test("r2", r2, bench1) == false)
- return err;
+ return EXIT_FAILURE;
if (test("r2", r2, vpMath::rad(10)) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyxVector r3;
r3 = vpMath::rad(10);
if (test("r3", r3, bench1) == false)
- return err;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < r3.size(); i++) {
+ if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
vpColVector r4 = 0.5 * r1;
std::vector<double> bench2(3, vpMath::rad(5));
if (test("r4", r4, bench2) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyxVector r5(r3);
if (test("r5", r5, bench1) == false)
- return err;
+ return EXIT_FAILURE;
}
{
vpRzyzVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
std::vector<double> bench1(3, vpMath::rad(10));
if (test("r1", r1, bench1) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyzVector r2 = r1;
if (test("r2", r2, bench1) == false)
- return err;
+ return EXIT_FAILURE;
if (test("r2", r2, vpMath::rad(10)) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyzVector r3;
r3 = vpMath::rad(10);
if (test("r3", r3, bench1) == false)
- return err;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < r3.size(); i++) {
+ if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
vpColVector r4 = 0.5 * r1;
std::vector<double> bench2(3, vpMath::rad(5));
if (test("r4", r4, bench2) == false)
- return err;
+ return EXIT_FAILURE;
vpRzyzVector r5(r3);
if (test("r5", r5, bench1) == false)
- return err;
+ return EXIT_FAILURE;
}
{
vpQuaternionVector r1(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
std::vector<double> bench1(4, vpMath::rad(10));
if (test("r1", r1, bench1) == false)
- return err;
+ return EXIT_FAILURE;
vpQuaternionVector r2 = r1;
if (test("r2", r2, bench1) == false)
- return err;
+ return EXIT_FAILURE;
if (test("r2", r2, vpMath::rad(10)) == false)
- return err;
+ return EXIT_FAILURE;
vpQuaternionVector r3;
r3.set(vpMath::rad(10), vpMath::rad(10), vpMath::rad(10), vpMath::rad(10));
if (test("r3", r3, bench1) == false)
- return err;
+ 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<double>::epsilon()) {
+ for (unsigned int i = 0; i < r3.size(); i++) {
+ if (std::fabs(r3[i] - bench1[i]) > std::fabs(r3[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
- return false;
+ return EXIT_FAILURE;
}
}
vpColVector r4 = 0.5 * r1;
std::vector<double> bench2(4, vpMath::rad(5));
if (test("r4", r4, bench2) == false)
- return err;
+ return EXIT_FAILURE;
vpQuaternionVector r5(r3);
if (test("r5", r5, bench1) == false)
- return err;
+ return EXIT_FAILURE;
}
{
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
+ for (int i = -10; i < 10; i++) {
+ for (int j = -10; j < 10; j++) {
+ vpThetaUVector tu(vpMath::rad(90 + i), vpMath::rad(170 + j), vpMath::rad(45));
+ tu.buildFrom(vpRotationMatrix(tu)); // put some coherence into rotation convention
- std::cout << "Initialization " <<std::endl ;
+ std::cout << "Initialization " << std::endl;
double theta;
vpColVector u;
tu.extract(theta, u);
- std::cout << "theta=" << vpMath::deg(theta) << std::endl ;
- std::cout << "u=" << u << std::endl ;
+ std::cout << "theta=" << vpMath::deg(theta) << std::endl;
+ std::cout << "u=" << u << std::endl;
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(tu) ;
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
+ R.buildFrom(tu);
- std::cout << "Matrix R" ;
- if (R.isARotationMatrix()==1) std::cout <<" is a rotation matrix " << std::endl ;
- else std::cout <<" is not a rotation matrix " << std::endl ;
+ std::cout << "Matrix R";
+ if (R.isARotationMatrix() == 1)
+ std::cout << " is a rotation matrix " << std::endl;
+ else
+ std::cout << " is not a rotation matrix " << std::endl;
- std::cout << R << std::endl ;
+ std::cout << 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 ;
+ std::cout << q << std::endl;
R.buildFrom(q);
- std::cout << "From vpQuaternionVector to vpRotationMatrix " << std::endl ;
-
- std::cout << "From vpRotationMatrix to vpRxyzVector " << std::endl ;
- vpRxyzVector RxyzBuildFromR(R) ;
- std::cout << RxyzBuildFromR <<std::endl ;
+ std::cout << "From 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 << "From vpRxyzVector to vpThetaUVector " << std::endl;
+ std::cout << " use From vpRxyzVector to vpRotationMatrix " << std::endl;
+ std::cout << " use From vpRotationMatrix to vpThetaUVector " << std::endl;
+ vpThetaUVector tuBuildFromEu;
+ tuBuildFromEu.buildFrom(R);
- vpThetaUVector tuBuildFromEu ;
- tuBuildFromEu.buildFrom(R) ;
-
- std::cout << std::endl ;
- std::cout << "result : should equivalent to the first one " << std::endl ;
-
+ std::cout << std::endl;
+ std::cout << "result : should equivalent to the first one " << std::endl;
double theta2;
vpColVector u2;
tuBuildFromEu.extract(theta2, u2);
- std::cout << "theta=" << vpMath::deg(theta2) << std::endl ;
- std::cout << "u=" << u2 << std::endl ;
+ std::cout << "theta=" << vpMath::deg(theta2) << std::endl;
+ std::cout << "u=" << u2 << std::endl;
- assert(vpMath::abs(theta2-theta)<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[0]-u2[0])<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[1]-u2[1])<std::numeric_limits<double>::epsilon()*1e10);
- assert(vpMath::abs(u[2]-u2[2])<std::numeric_limits<double>::epsilon()*1e10);
+ assert(vpMath::abs(theta2 - theta) < std::numeric_limits<double>::epsilon() * 1e10);
+ assert(vpMath::abs(u[0] - u2[0]) < std::numeric_limits<double>::epsilon() * 1e10);
+ assert(vpMath::abs(u[1] - u2[1]) < std::numeric_limits<double>::epsilon() * 1e10);
+ assert(vpMath::abs(u[2] - u2[2]) < std::numeric_limits<double>::epsilon() * 1e10);
}
- vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
- std::cout << "Initialization vpRzyzVector " <<std::endl ;
- std::cout << rzyz << std::endl ;
- std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(rzyz) ;
- std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl ;
- vpRzyzVector rzyz_final ;
- rzyz_final.buildFrom(R) ;
- std::cout << rzyz_final << std::endl ;
-
- vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45)) ;
- std::cout << "Initialization vpRzyxVector " <<std::endl ;
- std::cout << rzyx << std::endl ;
- std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl ;
- R.buildFrom(rzyx) ;
- std::cout << R << std::endl ;
- std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl ;
- vpRzyxVector rzyx_final ;
- rzyx_final.buildFrom(R) ;
- std::cout << rzyx_final << std::endl ;
+ vpRzyzVector rzyz(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45));
+ std::cout << "Initialization vpRzyzVector " << std::endl;
+ std::cout << rzyz << std::endl;
+ std::cout << "From vpRzyzVector to vpRotationMatrix " << std::endl;
+ R.buildFrom(rzyz);
+ std::cout << "From vpRotationMatrix to vpRzyzVector " << std::endl;
+ vpRzyzVector rzyz_final;
+ rzyz_final.buildFrom(R);
+ std::cout << rzyz_final << std::endl;
+
+ vpRzyxVector rzyx(vpMath::rad(180), vpMath::rad(120), vpMath::rad(45));
+ std::cout << "Initialization vpRzyxVector " << std::endl;
+ std::cout << rzyx << std::endl;
+ std::cout << "From vpRzyxVector to vpRotationMatrix " << std::endl;
+ R.buildFrom(rzyx);
+ std::cout << R << std::endl;
+ std::cout << "From vpRotationMatrix to vpRzyxVector " << std::endl;
+ vpRzyxVector rzyx_final;
+ rzyx_final.buildFrom(R);
+ std::cout << rzyx_final << std::endl;
}
}
std::cout << "All tests succeed" << std::endl;
- return 0;
- }
- catch(vpException &e) {
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
- return 1;
+ return EXIT_FAILURE;
}
}
diff --git a/modules/core/test/math/testRowVector.cpp b/modules/core/test/math/testRowVector.cpp
index 431669a..04db35a 100644
--- a/modules/core/test/math/testRowVector.cpp
+++ b/modules/core/test/math/testRowVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,24 +42,23 @@
Test some vpRowVector functionalities.
*/
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRowVector.h>
-
bool test(const std::string &s, const vpRowVector &v, const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << v.getRows() << "," << v.getCols() << ") = [" << v << "]" << std::endl;
- if(bench.size() != v.size()) {
+ if (bench.size() != v.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<v.size(); i++) {
- if (std::fabs(v[i]-bench[i]) > std::fabs(v[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < v.size(); i++) {
+ if (std::fabs(v[i] - bench[i]) > std::fabs(v[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -79,7 +79,7 @@ int main()
std::vector<double> bench1(4, 3);
if (test("v", v, bench1) == false)
return err;
- std::vector<double> bench2(4, 3./6);
+ std::vector<double> bench2(4, 3. / 6);
v.normalize();
if (test("v", v, bench2) == false)
return err;
@@ -93,9 +93,9 @@ int main()
{
vpRowVector v(4);
std::vector<double> bench1(4);
- for(unsigned int i=0; i<v.size(); i++) {
+ for (unsigned int i = 0; i < v.size(); i++) {
v[i] = (double)i;
- bench1[i] = (double)i;
+ bench1[i] = (double)i;
}
if (test("v", v, bench1) == false)
return err;
@@ -114,7 +114,7 @@ int main()
bench3.push_back(3);
vpRowVector r1;
- for(size_t i=0; i<4; i++)
+ for (size_t i = 0; i < 4; i++)
r1.stack((double)i);
vpRowVector r2 = r1.extract(1, 3);
@@ -124,7 +124,7 @@ int main()
{
vpMatrix M(1, 4);
std::vector<double> bench(4);
- for(unsigned int i=0; i<M.getCols(); i++) {
+ for (unsigned int i = 0; i < M.getCols(); i++) {
M[0][i] = i;
bench[i] = i;
}
@@ -178,7 +178,7 @@ int main()
{
vpRowVector r1(3, 1);
vpRowVector r2 = -r1;
- std::vector<double> bench(3,-1);
+ std::vector<double> bench(3, -1);
// v contains [-1 -1 -1]
if (test("r2", r2, bench) == false)
return err;
@@ -229,7 +229,8 @@ int main()
return err;
}
{
- // Test mean, median and standard deviation against Matlab with rng(0) and rand(10,1)*10
+ // Test mean, median and standard deviation against Matlab with rng(0) and
+ // rand(10,1)*10
vpRowVector r(10);
r[0] = 8.1472;
r[1] = 9.0579;
@@ -244,28 +245,28 @@ int main()
std::cout << "** Test mean" << std::endl;
double res = vpRowVector::mean(r);
- if(!vpMath::equal(res, 6.2386, 0.001)) {
+ if (!vpMath::equal(res, 6.2386, 0.001)) {
std::cout << "Test fails: bad mean " << res << std::endl;
return err;
}
std::cout << "** Test stdev" << std::endl;
res = vpRowVector::stdev(r);
- if(!vpMath::equal(res, 3.2810, 0.001)) {
+ if (!vpMath::equal(res, 3.2810, 0.001)) {
std::cout << "Test fails: bad stdev " << res << std::endl;
return err;
}
std::cout << "** Test stdev(bessel)" << std::endl;
res = vpRowVector::stdev(r, true);
- if(!vpMath::equal(res, 3.4585, 0.001)) {
+ if (!vpMath::equal(res, 3.4585, 0.001)) {
std::cout << "Test fails: bad stdev(bessel) " << res << std::endl;
return err;
}
std::cout << "** Test median" << std::endl;
res = vpRowVector::median(r);
- if(!vpMath::equal(res, 7.2354, 0.001)) {
+ if (!vpMath::equal(res, 7.2354, 0.001)) {
std::cout << "Test fails: bad median " << res << std::endl;
return err;
}
@@ -274,7 +275,7 @@ int main()
std::cout << "** Test median (odd)" << std::endl;
r.stack(1.5761);
res = vpRowVector::median(r);
- if(!vpMath::equal(res, 6.3236, 0.001)) {
+ if (!vpMath::equal(res, 6.3236, 0.001)) {
std::cout << "Test fails: bad median (odd) " << res << std::endl;
return err;
}
diff --git a/modules/core/test/math/testSvd.cpp b/modules/core/test/math/testSvd.cpp
index 2158ee8..2889b84 100644
--- a/modules/core/test/math/testSvd.cpp
+++ b/modules/core/test/math/testSvd.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,167 +37,401 @@
*
*****************************************************************************/
-
/*!
\example testSvd.cpp
\brief Test various svd decompositions.
*/
-#include <visp3/core/vpTime.h>
-
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpColVector.h>
-#include <vector>
#include <algorithm>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+#include <vector>
-bool testSvdOpenCvGSLCoherence(double epsilon);
-#ifdef VISP_HAVE_GSL
-bool testRandom(double epsilon);
-#endif
+#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdn:i:pf:R:C:vh"
+
+/*!
+ Print the program options.
-#define abs(x) ((x) < 0 ? - (x) : (x))
-#ifdef VISP_HAVE_GSL
+ \param name : Program name.
+ \param badparam : Bad parameter name.
-bool testRandom(double epsilon)
+ */
+void usage(const char *name, const char *badparam)
{
- vpMatrix L0(6,6);
- vpMatrix L1(6,6);
+ fprintf(stdout, "\n\
+Test matrix inversions\n\
+using LU, QR and Cholesky methods as well as Pseudo-inverse.\n\
+Outputs a comparison of these methods.\n\
+\n\
+SYNOPSIS\n\
+ %s [-n <number of matrices>] [-f <plot filename>]\n\
+ [-R <number of rows>] [-C <number of columns>]\n\
+ [-i <number of iterations>] [-p] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -n <number of matrices> \n\
+ Number of matrices inverted during each test loop.\n\
+\n\
+ -i <number of iterations> \n\
+ Number of iterations of the test.\n\
+\n\
+ -f <plot filename> \n\
+ Set output path for plot output.\n\
+ The plot logs the times of \n\
+ the different inversion methods: \n\
+ QR,LU,Cholesky and Pseudo-inverse.\n\
+\n\
+ -R <number of rows>\n\
+ Number of rows of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -C <number of columns>\n\
+ Number of colums of the automatically generated matrices \n\
+ we test on.\n\
+\n\
+ -p \n\
+ Plot into filename in the gnuplot format. \n\
+ If this option is used, tests results will be logged \n\
+ into a filename specified with -f.\n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
- for (unsigned int i=0 ; i < L0.getRows() ; i++)
- for (unsigned int j=0 ; j < L0.getCols() ; j++)
- L1[i][j] = L0[i][j] = (double)rand()/(double)RAND_MAX;
+/*!
- vpColVector W0(L0.getCols()) ;
- vpMatrix V0(L0.getCols(), L0.getCols()) ;
- vpColVector W1(L1.getCols()) ;
- vpMatrix V1(L1.getCols(), L1.getCols()) ;
+ Set the program options.
- L0.svdNr(W0,V0);
- L1.svdGsl(W1,V1);
+ \return false if the program has to be stopped, true otherwise.
- vpColVector _W0 = vpColVector::sort(W0);
- vpColVector _W1 = vpColVector::sort(W1);
+*/
+bool getOptions(int argc, const char **argv, unsigned int &nb_matrices, unsigned int &nb_iterations,
+ bool &use_plot_file, std::string &plotfile, unsigned int &nbrows, unsigned int &nbcols, bool &verbose)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'n':
+ nb_matrices = (unsigned int)atoi(optarg_);
+ break;
+ case 'i':
+ nb_iterations = (unsigned int)atoi(optarg_);
+ break;
+ case 'f':
+ plotfile = optarg_;
+ use_plot_file = true;
+ break;
+ case 'p':
+ use_plot_file = true;
+ break;
+ case 'R':
+ nbrows = (unsigned int)atoi(optarg_);
+ break;
+ case 'C':
+ nbcols = (unsigned int)atoi(optarg_);
+ break;
+ case 'v':
+ verbose = true;
+ break;
+ // add default options -c -d
+ case 'c':
+ break;
+ case 'd':
+ break;
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
- vpColVector diff = _W0-_W1;
- double error=-1.0;
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
- for(unsigned int i=0;i<6;i++)
- error=std::max(abs(diff[i]),error);
+vpMatrix make_random_matrix(unsigned int nbrows, unsigned int nbcols)
+{
+ vpMatrix A;
+ A.resize(nbrows, nbcols);
- return error<epsilon;
+ for (unsigned int i = 0; i < A.getRows(); i++) {
+ for (unsigned int j = 0; j < A.getCols(); j++) {
+ A[i][j] = (double)rand() / (double)RAND_MAX;
+ }
+ }
+ return A;
}
-#endif
+void create_bench_random_matrix(unsigned int nb_matrices, unsigned int nb_rows, unsigned int nb_cols, bool verbose,
+ std::vector<vpMatrix> &bench)
+{
+ if (verbose)
+ 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)
+ // 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; 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;
+ // }
+ // }
+ //#else
+ M = make_random_matrix(nb_rows, nb_cols);
+ //#endif
+ bench.push_back(M);
+ }
+}
-bool testSvdOpenCvGSLCoherence(double epsilon)
+int test_svd(std::vector<vpMatrix> M, std::vector<vpMatrix> U, std::vector<vpColVector> s, std::vector<vpMatrix> V)
{
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && defined (VISP_HAVE_GSL) // Require opencv >= 2.1.1
- vpMatrix A;
- vpMatrix vA;
- vpColVector wA;
- vpMatrix B;
- vpMatrix vB;
- vpColVector wB;
- A.resize(6,6);
- B.resize(6,6);
- vA.resize(6,6);
- vB.resize(6,6);
- wA.resize(6);
- wB.resize(6);
-
- for (unsigned int i=0 ; i < A.getRows() ; i++)
- for (unsigned int j=0 ; j < A.getCols() ; j++)
- B[i][j] = A[i][j] = (double)rand()/(double)RAND_MAX;
-
- A.svdOpenCV(wA,vA);
- B.svdGsl(wB,vB);
-
- bool error=false;
- for (unsigned int i=0 ; i < A.getRows() ; i++){
- error = error | (abs(wA[i]-wB[i])>epsilon);
+ for (unsigned int i = 0; i < M.size(); i++) {
+ vpMatrix S;
+ S.diag(s[i]);
+ vpMatrix U_S_V = U[i] * S * V[i].t();
+ vpMatrix D = M[i] - U_S_V;
+ if (D.euclideanNorm() > 1e-6) {
+ std::cout << "SVD decomposition failed" << std::endl;
+ return EXIT_FAILURE;
+ }
}
+ return EXIT_SUCCESS;
+}
- return !error;
-#else
- (void)epsilon;
- return true;
+#if defined(VISP_HAVE_EIGEN3)
+int test_svd_eigen3(bool verbose, const std::vector<vpMatrix> &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::vector<vpMatrix> U = bench;
+ std::vector<vpMatrix> V(bench.size());
+ std::vector<vpColVector> s(bench.size());
+
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ U[i].svdEigen3(s[i], V[i]);
+ }
+
+ time = vpTime::measureTimeMs() - t;
+
+ return test_svd(bench, U, s, V);
+}
#endif
+
+#if defined(VISP_HAVE_LAPACK)
+int test_svd_lapack(bool verbose, const std::vector<vpMatrix> &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::vector<vpMatrix> U = bench;
+ std::vector<vpMatrix> V(bench.size());
+ std::vector<vpColVector> s(bench.size());
+
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ U[i].svdLapack(s[i], V[i]);
+ }
+ time = vpTime::measureTimeMs() - t;
+
+ return test_svd(bench, U, s, V);
}
+#endif
-int
-main()
+#if defined(VISP_HAVE_GSL)
+int test_svd_gsl(bool verbose, const std::vector<vpMatrix> &bench, double &time)
{
- try {
- vpMatrix L(60000,6), Ls ;
- for (unsigned int i=0 ; i < L.getRows() ; i++)
- for (unsigned int j=0 ; j < L.getCols() ; j++)
- L[i][j] = 2*i+j + cos((double)(i+j))+((double)(i)) ;
- // std::cout << L << std::endl ;
- Ls = L ;
- std::cout << "--------------------------------------"<<std::endl ;
-
- vpColVector W(L.getCols()) ;
- vpMatrix V(L.getCols(), L.getCols()) ;
-
- double t = vpTime::measureTimeMs() ;
- L.svdNr(W,V) ;
- t = vpTime::measureTimeMs() -t ;
-
- std::cout <<"svdNr Numerical recipes \n time " <<t << std::endl;
- std::cout << W.t() ;
- std::cout << "--------------------------------------"<<std::endl ;
-
-
-#ifdef VISP_HAVE_GSL
- L = Ls ;
- t = vpTime::measureTimeMs() ;
- L.svdGsl(W,V) ;
- t = vpTime::measureTimeMs() -t ;
- std::cout <<"svdGsl_mod \n time " <<t << std::endl;
- std::cout << W.t() ;
-
- std::cout << "--------------------------------------"<<std::endl ;
- std::cout << "TESTING RANDOM MATRICES:" ;
-
- bool ret = true;
- for(unsigned int i=0;i<2000;i++)
- ret = ret & testRandom(0.00001);
- if(ret)
- std:: cout << "Success"<< std:: endl;
- else
- std:: cout << "Fail"<< std:: endl;
-
- std::cout << "--------------------------------------"<<std::endl ;
+ 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::vector<vpMatrix> U = bench;
+ std::vector<vpMatrix> V(bench.size());
+ std::vector<vpColVector> s(bench.size());
+
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ U[i].svdGsl(s[i], V[i]);
+ }
+
+ time = vpTime::measureTimeMs() - t;
+
+ return test_svd(bench, U, s, V);
+}
#endif
- std::cout << "--------------------------------------"<<std::endl ;
- std::cout << "TESTING OPENCV-GSL coherence:" ;
-
- bool ret2 = true;
- for(unsigned int i=0;i<1;i++)
- ret2 = ret2 & testSvdOpenCvGSLCoherence(0.00001);
- if(ret2)
- std:: cout << "Success"<< std:: endl;
- else
- std:: cout << "Fail"<< std:: endl;
-
- std::cout << "--------------------------------------"<<std::endl ;
-
- L = Ls ;
- t = vpTime::measureTimeMs() ;
- L.svdFlake(W,V) ;
- t = vpTime::measureTimeMs() -t ;
- std::cout <<"svdFlake\n time " <<t << std::endl;
- std::cout << W.t() ;
- return 0;
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+int test_svd_opencv(bool verbose, const std::vector<vpMatrix> &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::vector<vpMatrix> U = bench;
+ std::vector<vpMatrix> V(bench.size());
+ std::vector<vpColVector> s(bench.size());
+
+ double t = vpTime::measureTimeMs();
+ for (unsigned int i = 0; i < bench.size(); i++) {
+ U[i].svdOpenCV(s[i], V[i]);
}
- catch(vpException &e) {
- std::cout << "Catch an exception: " << e << std::endl;
- return 1;
+ time = vpTime::measureTimeMs() - t;
+
+ return test_svd(bench, U, s, V);
+}
+#endif
+
+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";
+ if (verbose || !use_plot_file) {
+ std::cout << method << time << std::endl;
}
}
+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)
+ unsigned int nb_matrices = 1000;
+ unsigned int nb_iterations = 10;
+ unsigned int nb_rows = 6;
+ unsigned int nb_cols = 6;
+ bool verbose = false;
+ std::string plotfile("plot-svd.csv");
+ bool use_plot_file = false;
+ 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) {
+ exit(-1);
+ }
+
+ if (use_plot_file) {
+ of.open(plotfile.c_str());
+ of << "iter"
+ << "\t";
+
+#if defined(VISP_HAVE_LAPACK)
+ of << "\"SVD Lapack\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_EIGEN3)
+ of << "\"SVD Eigen3\""
+ << "\t";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ of << "\"SVD OpenCV\""
+ << "\t";
+#endif
+#if defined(VISP_HAVE_GSL)
+ of << "\"SVD GSL\""
+ << "\t";
+#endif
+ of << std::endl;
+ }
+
+ int ret = EXIT_SUCCESS;
+ for (unsigned int iter = 0; iter < nb_iterations; iter++) {
+ std::vector<vpMatrix> bench_random_matrices;
+ create_bench_random_matrix(nb_matrices, nb_rows, nb_cols, verbose, bench_random_matrices);
+
+ if (use_plot_file)
+ of << iter << "\t";
+ double time;
+
+#if defined(VISP_HAVE_LAPACK)
+ ret += test_svd_lapack(verbose, bench_random_matrices, time);
+ save_time("SVD (Lapack): ", verbose, use_plot_file, of, time);
+#endif
+
+#if defined(VISP_HAVE_EIGEN3)
+ ret += test_svd_eigen3(verbose, bench_random_matrices, time);
+ save_time("SVD (Eigen3): ", verbose, use_plot_file, of, time);
+#endif
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
+ ret += test_svd_opencv(verbose, bench_random_matrices, time);
+ save_time("SVD (OpenCV): ", verbose, use_plot_file, of, time);
+#endif
+
+#if defined(VISP_HAVE_GSL)
+ ret += test_svd_gsl(verbose, bench_random_matrices, time);
+ save_time("SVD (GSL): ", verbose, use_plot_file, of, time);
+#endif
+ if (use_plot_file)
+ of << std::endl;
+ }
+ if (use_plot_file) {
+ of.close();
+ std::cout << "Result saved in " << plotfile << std::endl;
+ }
+
+ if (ret == EXIT_SUCCESS) {
+ std::cout << "Test succeed" << std::endl;
+ } else {
+ std::cout << "Test failed" << std::endl;
+ }
+
+ return ret;
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Test does nothing since you dont't have Eigen3, Lapack, "
+ "OpenCV or GSL 3rd party"
+ << std::endl;
+ return EXIT_SUCCESS;
+#endif
+ } catch (const vpException &e) {
+ std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/core/test/math/testTranslationVector.cpp b/modules/core/test/math/testTranslationVector.cpp
index 9fd73fd..2aa27cc 100644
--- a/modules/core/test/math/testTranslationVector.cpp
+++ b/modules/core/test/math/testTranslationVector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,8 +43,8 @@
*/
#include <cmath>
-#include <vector>
#include <limits>
+#include <vector>
#include <visp3/core/vpTranslationVector.h>
@@ -52,12 +53,12 @@ bool test(const std::string &s, const vpArray2D<double> &A, const std::vector<do
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << A.getRows() << "," << A.getCols() << ") =" << A << std::endl;
- if(bench.size() != A.size()) {
+ if (bench.size() != A.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<A.size(); i++) {
- if (std::fabs(A.data[i]-bench[i]) > std::fabs(A.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < A.size(); i++) {
+ if (std::fabs(A.data[i] - bench[i]) > std::fabs(A.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -70,7 +71,7 @@ int main()
int err = 1;
{
vpTranslationVector t;
- std::vector<double> bench(3,0);
+ std::vector<double> bench(3, 0);
if (test("t", t, bench) == false)
return err;
}
diff --git a/modules/core/test/math/testTwistMatrix.cpp b/modules/core/test/math/testTwistMatrix.cpp
index 5148f8a..ca099fa 100644
--- a/modules/core/test/math/testTwistMatrix.cpp
+++ b/modules/core/test/math/testTwistMatrix.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,24 +42,21 @@
\brief Test some vpMatrix functionalities.
*/
-
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-int
-main()
+int main()
{
try {
vpTRACE("--------------------------");
@@ -73,9 +71,9 @@ main()
// Set the rotation
vpRxyzVector cre;
- cre[0] = M_PI/2.;
- cre[1] = -M_PI/2.;
- cre[2] = -M_PI/4.;
+ cre[0] = M_PI / 2.;
+ cre[1] = -M_PI / 2.;
+ cre[2] = -M_PI / 4.;
// Build rotation matrix
vpRotationMatrix cRe(cre);
@@ -84,7 +82,7 @@ main()
vpVelocityTwistMatrix cVe(cte, cRe);
vpTRACE("cVe twist matrix:");
- cVe.print (std::cout, 6);
+ cVe.print(std::cout, 6);
// Set a speed skew
vpColVector ev(6);
@@ -92,12 +90,12 @@ main()
ev[0] = 1.;
ev[1] = 0.1;
ev[2] = -0.5;
- ev[3] = M_PI/180.;
- ev[4] = M_PI/18.;
- ev[5] = M_PI/10.;
+ ev[3] = M_PI / 180.;
+ ev[4] = M_PI / 18.;
+ ev[5] = M_PI / 10.;
vpTRACE("ev colvector:");
- ev.print (std::cout, 6);
+ ev.print(std::cout, 6);
// Set a speed skew
vpColVector cv;
@@ -105,10 +103,9 @@ main()
cv = cVe * ev;
vpTRACE("cv = cVe * ev:");
- cv.print (std::cout, 6);
+ cv.print(std::cout, 6);
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/network/testClient.cpp b/modules/core/test/network/testClient.cpp
index 703b906..0d5530b 100644
--- a/modules/core/test/network/testClient.cpp
+++ b/modules/core/test/network/testClient.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,25 +53,23 @@ int main()
vpClient client;
client.connectToHostname(servername, port);
- //client.connectToIP("127.0.0.1",port);
+ // client.connectToIP("127.0.0.1",port);
int val = 0;
- while(1)
- {
- if(client.send(&val) != sizeof(int)) //Sending the new value to the first client
+ 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
+ if (client.receive(&val) != sizeof(int)) // Receiving a value from the first client
std::cout << "Error while receiving" << std::endl;
else
std::cout << "Received : " << val << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/network/testServer.cpp b/modules/core/test/network/testServer.cpp
index 651731a..d088fdb 100644
--- a/modules/core/test/network/testServer.cpp
+++ b/modules/core/test/network/testServer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,32 +49,30 @@ int main()
{
try {
int port = 35000;
- vpServer serv(port); //Launch the server on localhost
+ vpServer serv(port); // Launch the server on localhost
serv.start();
bool run = true;
int val;
- while(run){
+ while (run) {
serv.checkForConnections();
- if(serv.getNumberOfClients() > 0)
- {
- if(serv.receive(&val) != sizeof(int)) //Receiving a value from the first client
+ if (serv.getNumberOfClients() > 0) {
+ if (serv.receive(&val) != sizeof(int)) // Receiving a value from the first client
std::cout << "Error while receiving" << std::endl;
else
std::cout << "Received : " << val << std::endl;
- val = val+1;
- if(serv.send(&val) != sizeof(int)) //Sending the new value to the first client
+ val = val + 1;
+ if (serv.send(&val) != sizeof(int)) // Sending the new value to the first client
std::cout << "Error while sending" << std::endl;
else
std::cout << "Sending : " << val << std::endl;
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/core/test/network/testUDPClient.cpp b/modules/core/test/network/testUDPClient.cpp
new file mode 100644
index 0000000..eeb6237
--- /dev/null
+++ b/modules/core/test/network/testUDPClient.cpp
@@ -0,0 +1,98 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test for UDP client.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testUDPClient.cpp
+
+ Example of a UDP client.
+*/
+
+#include <cstdlib>
+#include <cstring>
+#include <iostream>
+#include <visp3/core/vpUDPClient.h>
+
+namespace
+{
+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) {}
+};
+}
+
+int main()
+{
+ try {
+ std::string servername = "127.0.0.1";
+ unsigned int port = 50037;
+ vpUDPClient client(servername, port);
+
+ // Send custom data type
+ 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;
+
+ if (client.receive(msg)) {
+ data_type.double_val = *reinterpret_cast<const double *>(msg.c_str());
+ data_type.int_val = *reinterpret_cast<const int *>(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;
+ }
+
+ // Send user message
+ while (true) {
+ std::cout << "Enter the message to send:" << std::endl;
+ std::string msg = "";
+ std::getline(std::cin, msg);
+ if (client.send(msg) != (int)msg.size())
+ std::cerr << "Error client.send()!" << std::endl;
+ if (client.receive(msg))
+ std::cout << "Receive from the server: " << msg << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/core/test/network/testUDPServer.cpp b/modules/core/test/network/testUDPServer.cpp
new file mode 100644
index 0000000..eaccefb
--- /dev/null
+++ b/modules/core/test/network/testUDPServer.cpp
@@ -0,0 +1,117 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test for UDP server.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testUDPServer.cpp
+
+ Example of a UDP server.
+*/
+
+#include <cstdlib>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <sstream>
+#include <vector>
+#include <visp3/core/vpUDPServer.h>
+
+namespace
+{
+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) {}
+};
+}
+
+int main()
+{
+ try {
+ int port = 50037;
+ vpUDPServer server(port);
+
+ std::string msg = "", hostInfo = "";
+ // Receive and send custom data type
+ 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
+ << " from: " << hostInfo << std::endl;
+
+ // Get address and port
+ std::istringstream iss(hostInfo);
+ std::vector<std::string> tokens;
+ std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(),
+ 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));
+
+ server.send(msg, tokens[1], atoi(tokens[2].c_str()));
+ }
+
+ // Receive and send message
+ while (true) {
+ 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;
+
+ // Get address and port
+ std::istringstream iss(hostInfo);
+ std::vector<std::string> tokens;
+ std::copy(std::istream_iterator<std::string>(iss), std::istream_iterator<std::string>(),
+ std::back_inserter(tokens));
+ server.send("Echo: " + msg, tokens[1], atoi(tokens[2].c_str()));
+ } else if (res == 0) {
+ std::cout << "Receive timeout" << std::endl;
+ } else {
+ std::cerr << "Error server.receive()!" << std::endl;
+ }
+ }
+
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/core/test/tools/convert/testConvert.cpp b/modules/core/test/tools/convert/testConvert.cpp
index 1453ce1..da9556a 100644
--- a/modules/core/test/tools/convert/testConvert.cpp
+++ b/modules/core/test/tools/convert/testConvert.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,17 +44,15 @@
*/
-#include <iostream> // std::cout
-#include <limits> // std::numeric_limits
+#include <iostream> // std::cout
+#include <limits> // std::numeric_limits
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpConvert.h>
+bool areSame(double a, double b) { return fabs(a - b) < std::numeric_limits<double>::epsilon(); }
-bool areSame(double a, double b) {
- return fabs(a - b) < std::numeric_limits<double>::epsilon();
-}
-
-void testConvertFromImagePointToPoint2f() {
+void testConvertFromImagePointToPoint2f()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
vpImagePoint imPt1(12.5f, .85f);
vpImagePoint imPt2(-44.26f, 125.11f);
@@ -65,9 +64,18 @@ void testConvertFromImagePointToPoint2f() {
vpConvert::convertToOpenCV(imPt3, pt3);
int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+ if (areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<vpImagePoint> listOfImPts(3);
listOfImPts[0] = imPt1;
@@ -77,9 +85,12 @@ void testConvertFromImagePointToPoint2f() {
std::vector<cv::Point2f> listOfPts;
vpConvert::convertToOpenCV(listOfImPts, listOfPts);
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ if (listOfImPts.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -89,7 +100,8 @@ void testConvertFromImagePointToPoint2f() {
#endif
}
-void testConvertFromPoint2fToImagePoint() {
+void testConvertFromPoint2fToImagePoint()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
vpImagePoint imPt1, imPt2, imPt3;
@@ -99,9 +111,18 @@ void testConvertFromPoint2fToImagePoint() {
vpConvert::convertFromOpenCV(pt3, imPt3);
int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+ if (areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<vpImagePoint> listOfImPts;
@@ -112,9 +133,12 @@ void testConvertFromPoint2fToImagePoint() {
vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ if (listOfImPts.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -124,7 +148,8 @@ void testConvertFromPoint2fToImagePoint() {
#endif
}
-void testConvertFromImagePointToPoint2d() {
+void testConvertFromImagePointToPoint2d()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
vpImagePoint imPt1(12.5, .85);
vpImagePoint imPt2(-44.26, 125.11);
@@ -136,9 +161,18 @@ void testConvertFromImagePointToPoint2d() {
vpConvert::convertToOpenCV(imPt3, pt3);
int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+ if (areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<vpImagePoint> listOfImPts(3);
listOfImPts[0] = imPt1;
@@ -148,9 +182,12 @@ void testConvertFromImagePointToPoint2d() {
std::vector<cv::Point2d> listOfPts;
vpConvert::convertToOpenCV(listOfImPts, listOfPts);
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ if (listOfImPts.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -160,7 +197,8 @@ void testConvertFromImagePointToPoint2d() {
#endif
}
-void testConvertFromPoint2dToImagePoint() {
+void testConvertFromPoint2dToImagePoint()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
vpImagePoint imPt1, imPt2, imPt3;
@@ -170,9 +208,18 @@ void testConvertFromPoint2dToImagePoint() {
vpConvert::convertFromOpenCV(pt3, imPt3);
int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y)) nbOk++; else nbNOk++;
+ if (areSame(imPt1.get_u(), pt1.x) && areSame(imPt1.get_v(), pt1.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt2.get_u(), pt2.x) && areSame(imPt2.get_v(), pt2.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt3.get_u(), pt3.x) && areSame(imPt3.get_v(), pt3.y))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<vpImagePoint> listOfImPts;
@@ -183,9 +230,12 @@ void testConvertFromPoint2dToImagePoint() {
vpConvert::convertFromOpenCV(listOfPts, listOfImPts);
- if(listOfImPts.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y)) nbOk++; else nbNOk++;
+ if (listOfImPts.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfImPts[i].get_u(), listOfPts[i].x) && areSame(listOfImPts[i].get_v(), listOfPts[i].y))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -195,7 +245,8 @@ void testConvertFromPoint2dToImagePoint() {
#endif
}
-void testConvertFromKeyPointToImagePoint() {
+void testConvertFromKeyPointToImagePoint()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
cv::KeyPoint kp1(12.5f, .85f, 0), kp2(-44.26f, 125.11f, 0), kp3(0.0f, -1.756e-10f, 0);
vpImagePoint imPt1, imPt2, imPt3;
@@ -205,9 +256,18 @@ void testConvertFromKeyPointToImagePoint() {
vpConvert::convertFromOpenCV(kp3, imPt3);
int nbOk = 0, nbNOk = 0;
- if(areSame(imPt1.get_u(), kp1.pt.x) && areSame(imPt1.get_v(), kp1.pt.y)) nbOk++; else nbNOk++;
- if(areSame(imPt2.get_u(), kp2.pt.x) && areSame(imPt2.get_v(), kp2.pt.y)) nbOk++; else nbNOk++;
- if(areSame(imPt3.get_u(), kp3.pt.x) && areSame(imPt3.get_v(), kp3.pt.y)) nbOk++; else nbNOk++;
+ if (areSame(imPt1.get_u(), kp1.pt.x) && areSame(imPt1.get_v(), kp1.pt.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt2.get_u(), kp2.pt.x) && areSame(imPt2.get_v(), kp2.pt.y))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(imPt3.get_u(), kp3.pt.x) && areSame(imPt3.get_v(), kp3.pt.y))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<cv::KeyPoint> listOfKeyPoints(3);
listOfKeyPoints[0] = kp1;
@@ -217,9 +277,13 @@ void testConvertFromKeyPointToImagePoint() {
std::vector<vpImagePoint> listOfImPts;
vpConvert::convertFromOpenCV(listOfKeyPoints, listOfImPts);
- if(listOfImPts.size() == listOfKeyPoints.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfImPts[i].get_u(), listOfKeyPoints[i].pt.x) && areSame(listOfImPts[i].get_v(), listOfKeyPoints[i].pt.y)) nbOk++; else nbNOk++;
+ if (listOfImPts.size() == listOfKeyPoints.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfImPts[i].get_u(), listOfKeyPoints[i].pt.x) &&
+ areSame(listOfImPts[i].get_v(), listOfKeyPoints[i].pt.y))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -229,7 +293,8 @@ void testConvertFromKeyPointToImagePoint() {
#endif
}
-void testConvertFromPoint3fToPoint() {
+void testConvertFromPoint3fToPoint()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
cv::Point3f pt1(12.5f, .85f, 110.0f), pt2(-44.26f, 125.11f, -98e2f), pt3(0.0f, -1.756e-10f, 0.00015f);
vpPoint point1, point2, point3;
@@ -239,9 +304,18 @@ 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())) nbOk++; else nbNOk++;
- if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
+ if (areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<cv::Point3f> listOfPoints3f(3);
listOfPoints3f[0] = pt1;
@@ -251,9 +325,13 @@ void testConvertFromPoint3fToPoint() {
std::vector<vpPoint> listOfPts;
vpConvert::convertFromOpenCV(listOfPoints3f, listOfPts);
- if(listOfPoints3f.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
+ if (listOfPoints3f.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) &&
+ areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -263,7 +341,8 @@ void testConvertFromPoint3fToPoint() {
#endif
}
-void testConvertFromPointToPoint3f() {
+void testConvertFromPointToPoint3f()
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
cv::Point3f pt1, pt2, pt3;
vpPoint point1, point2, point3;
@@ -284,9 +363,18 @@ 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())) nbOk++; else nbNOk++;
- if(areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ())) nbOk++; else nbNOk++;
- if(areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ())) nbOk++; else nbNOk++;
+ if (areSame(pt1.x, point1.get_oX()) && areSame(pt1.y, point1.get_oY()) && areSame(pt1.z, point1.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(pt2.x, point2.get_oX()) && areSame(pt2.y, point2.get_oY()) && areSame(pt2.z, point2.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
+ if (areSame(pt3.x, point3.get_oX()) && areSame(pt3.y, point3.get_oY()) && areSame(pt3.z, point3.get_oZ()))
+ nbOk++;
+ else
+ nbNOk++;
std::vector<cv::Point3f> listOfPoints3f;
std::vector<vpPoint> listOfPts(3);
@@ -296,9 +384,13 @@ void testConvertFromPointToPoint3f() {
vpConvert::convertToOpenCV(listOfPts, listOfPoints3f);
- if(listOfPoints3f.size() == listOfPts.size()) {
- for(size_t i = 0; i < 3; i++) {
- if(areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) && areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z)) nbOk++; else nbNOk++;
+ if (listOfPoints3f.size() == listOfPts.size()) {
+ for (size_t i = 0; i < 3; i++) {
+ if (areSame(listOfPts[i].get_oX(), listOfPoints3f[i].x) && areSame(listOfPts[i].get_oY(), listOfPoints3f[i].y) &&
+ areSame(listOfPts[i].get_oZ(), listOfPoints3f[i].z))
+ nbOk++;
+ else
+ nbNOk++;
}
} else {
nbNOk += 3;
@@ -308,7 +400,8 @@ void testConvertFromPointToPoint3f() {
#endif
}
-int main() {
+int main()
+{
testConvertFromImagePointToPoint2f();
testConvertFromPoint2fToImagePoint();
testConvertFromImagePointToPoint2d();
@@ -317,5 +410,5 @@ int main() {
testConvertFromKeyPointToImagePoint();
testConvertFromPoint3fToPoint();
testConvertFromPointToPoint3f();
- return 0;
+ return 0;
}
diff --git a/modules/core/test/tools/cpu-features/testCPUFeatures.cpp b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp
new file mode 100644
index 0000000..c125a07
--- /dev/null
+++ b/modules/core/test/tools/cpu-features/testCPUFeatures.cpp
@@ -0,0 +1,76 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Print CPU features.
+ *
+ *****************************************************************************/
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpCPUFeatures.h>
+
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+
+#if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <pmmintrin.h>
+#define VISP_HAVE_SSE3 1
+#else
+#define VISP_HAVE_SSE3 0
+#endif
+#if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <tmmintrin.h>
+#define VISP_HAVE_SSSE3 1
+#else
+#define VISP_HAVE_SSSE3 0
+#endif
+#else
+#define VISP_HAVE_SSE2 0
+#define VISP_HAVE_SSE3 0
+#define VISP_HAVE_SSSE3 0
+#endif
+
+#define VALUE_TO_STRING(x) #x
+#define VALUE(x) VALUE_TO_STRING(x)
+
+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;
+
+ return EXIT_SUCCESS;
+}
diff --git a/modules/core/test/tools/geometry/testPolygon.cpp b/modules/core/test/tools/geometry/testPolygon.cpp
index 59b2b81..4e19c7a 100644
--- a/modules/core/test/tools/geometry/testPolygon.cpp
+++ b/modules/core/test/tools/geometry/testPolygon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,14 @@
*
*****************************************************************************/
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPolygon.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayGDI.h>
-
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#include <math.h>
@@ -55,7 +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);
/*!
@@ -89,7 +89,7 @@ OPTIONS: \n\
Print the help.\n\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -103,20 +103,31 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': opt_click = false; break;
- case 'd': opt_display = false; break;
- case 'm': method = atoi(optarg_); break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ opt_click = false;
+ break;
+ case 'd':
+ opt_display = false;
+ break;
+ case 'm':
+ method = atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
- usage(argv[0], optarg_); return false; break;
+ usage(argv[0], optarg_);
+ return false;
+ break;
}
}
@@ -131,12 +142,13 @@ bool getOptions(int argc, const char **argv, bool& opt_display, bool& opt_click,
return true;
}
-/* -------------------------------------------------------------------------- */
-/* MAIN FUNCTION */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* MAIN FUNCTION */
+/* --------------------------------------------------------------------------
+ */
-int
-main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_display = true;
@@ -149,7 +161,7 @@ main(int argc, const char** argv)
return (-1);
}
- std::vector <vpImagePoint> vec1;
+ std::vector<vpImagePoint> vec1;
vec1.push_back(vpImagePoint(200, 200));
vec1.push_back(vpImagePoint(200, 400));
vec1.push_back(vpImagePoint(320, 400));
@@ -158,15 +170,14 @@ main(int argc, const char** argv)
vpPolygon p1;
p1.buildFrom(vec1);
- std::vector <vpImagePoint> vec2;
+ std::vector<vpImagePoint> vec2;
vec2.push_back(vpImagePoint(20, 20));
vec2.push_back(vpImagePoint(100, 20));
vec2.push_back(vpImagePoint(100, 100));
vec2.push_back(vpImagePoint(20, 100));
vpPolygon p2(vec2);
-
- std::vector <vpImagePoint> vec3;
+ std::vector<vpImagePoint> vec3;
vpPolygon p3(vec3);
#if defined VISP_HAVE_X11
@@ -191,8 +202,7 @@ main(int argc, const char** argv)
std::cout << " area : " << p3.getArea() << std::endl;
std::cout << " center : " << p3.getCenter() << std::endl;
-
- if(opt_display) {
+ if (opt_display) {
#if (defined VISP_HAVE_X11) || (defined VISP_HAVE_GTK) || (defined VISP_HAVE_GDI)
display.init(I, 10, 10, "Test vpPolygon");
#endif
@@ -209,7 +219,6 @@ main(int argc, const char** argv)
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);
@@ -227,9 +236,9 @@ 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);
}
}
@@ -238,9 +247,9 @@ main(int argc, const char** argv)
std::cout << "Click to continue." << std::endl;
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)){
+ 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)) {
vpDisplay::displayPoint(I, vpImagePoint(i, j), vpColor::green);
}
}
@@ -252,7 +261,7 @@ main(int argc, const char** argv)
vpDisplay::getClick(I);
vpDisplay::close(I);
- //Benchmark Point In Polygon test method
+ // Benchmark Point In Polygon test method
std::vector<vpImagePoint> corners = p4.getCorners();
std::cout << "Nb polygon corners=" << corners.size() << std::endl;
@@ -306,11 +315,8 @@ main(int argc, const char** argv)
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
-
diff --git a/modules/core/test/tools/geometry/testRect.cpp b/modules/core/test/tools/geometry/testRect.cpp
index d9b7b38..029c360 100644
--- a/modules/core/test/tools/geometry/testRect.cpp
+++ b/modules/core/test/tools/geometry/testRect.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpRect.h>
-int main() {
+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);
diff --git a/modules/core/test/tools/histogram/testHistogram.cpp b/modules/core/test/tools/histogram/testHistogram.cpp
index fbe5377..fe68d0b 100644
--- a/modules/core/test/tools/histogram/testHistogram.cpp
+++ b/modules/core/test/tools/histogram/testHistogram.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,13 @@
*
*****************************************************************************/
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpHistogram.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpHistogram.h>
-#include <stdlib.h>
-#include <stdio.h>
/*!
\example testHistogram.cpp
@@ -51,8 +52,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:t:h"
-
+#define GETOPTARGS "cdi:t:h"
/*
Print the program options.
@@ -76,7 +76,7 @@ SYNOPSIS\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.ppm\"\n\
+ From this path read \"Klimt/Klimt.ppm\"\n\
image.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
@@ -85,8 +85,7 @@ OPTIONS: Default\n\
-t <nb threads>\n\
Set the number of threads to use for the computation.\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str());
+ Print the help.\n\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -110,16 +109,25 @@ bool getOptions(int argc, const char **argv, std::string &ipath, unsigned int &n
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 't': nbThreads = (unsigned int) atoi(optarg_); break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 't':
+ nbThreads = (unsigned int)atoi(optarg_);
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, ipath); return false; break;
+ usage(argv[0], optarg_, ipath);
+ return false;
+ break;
}
}
@@ -141,13 +149,14 @@ bool getOptions(int argc, const char **argv, std::string &ipath, unsigned int &n
\param nbBins : Number of histogram bins.
\param nbThreads : Number of computation threads.
*/
-unsigned int histogramSum(const vpImage<unsigned char> &I, const unsigned int nbBins, const unsigned int nbThreads) {
+unsigned int histogramSum(const vpImage<unsigned char> &I, const unsigned int nbBins, const unsigned int nbThreads)
+{
unsigned int sum = 0;
vpHistogram histogram;
histogram.calculate(I, nbBins, nbThreads);
- for(unsigned int cpt = 0; cpt < histogram.getSize(); cpt++) {
+ for (unsigned int cpt = 0; cpt < histogram.getSize(); cpt++) {
sum += histogram[cpt];
}
@@ -160,7 +169,8 @@ unsigned int histogramSum(const vpImage<unsigned char> &I, const unsigned int nb
\param I : Input color image.
\param nbBins : Number of histogram bins.
*/
-bool compareHistogram(const vpImage<unsigned char> &I, const unsigned int nbBins) {
+bool compareHistogram(const vpImage<unsigned char> &I, const unsigned int nbBins)
+{
vpHistogram histogram_single_threaded;
histogram_single_threaded.calculate(I, nbBins, 1);
@@ -168,10 +178,10 @@ bool compareHistogram(const vpImage<unsigned char> &I, const unsigned int nbBins
histogram_multi_threaded.calculate(I, nbBins, 4);
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;
+ 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;
return false;
}
@@ -179,7 +189,7 @@ bool compareHistogram(const vpImage<unsigned char> &I, const unsigned int nbBins
sum += histogram_single_threaded[cpt];
}
- if(sum != I.getSize()) {
+ if (sum != I.getSize()) {
std::cerr << "Sum of histogram is different with the image size!" << std::endl;
return false;
}
@@ -187,8 +197,7 @@ bool compareHistogram(const vpImage<unsigned char> &I, const unsigned int nbBins
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -197,17 +206,17 @@ main(int argc, const char ** argv)
std::string filename;
unsigned int nbThreads = 4;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
if (getOptions(argc, argv, opt_ipath, nbThreads) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -218,8 +227,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -227,18 +235,16 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
//
// Here starts really the test
//
@@ -247,7 +253,7 @@ main(int argc, const char ** argv)
vpImage<unsigned char> I;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
std::cout << "Read image: " << filename << std::endl;
vpImageIo::read(I, filename);
@@ -259,44 +265,43 @@ main(int argc, const char ** argv)
unsigned int sum_single_multithread = 0;
double t_single_thread = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
sum_single_thread = histogramSum(I, nbBins, 1);
}
t_single_thread = vpTime::measureTimeMs() - t_single_thread;
double t_multithread = vpTime::measureTimeMs();
- for(int iteration = 0; iteration < nbIterations; iteration++) {
+ for (int iteration = 0; iteration < nbIterations; iteration++) {
sum_single_multithread = histogramSum(I, nbBins, nbThreads);
}
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;
+ << " 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;
+ << " 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()) {
+ if (sum_single_thread != I.getSize() || sum_single_multithread != I.getSize()) {
std::cerr << "Problem with histogram!" << std::endl;
return -1;
}
nbBins = 101;
- if(!compareHistogram(I, nbBins)) {
+ if (!compareHistogram(I, nbBins)) {
std::cerr << "Histogram are different!" << std::endl;
return -1;
}
-
- //Test histogram computation on empty image
+ // Test histogram computation on empty image
vpHistogram histogram;
vpImage<unsigned char> I_test(0, 0);
histogram.calculate(I_test, 256, 4);
- if(histogram.getSize() == 256) {
- for(unsigned int cpt = 0; cpt < 256; cpt++) {
- if(histogram[cpt] != 0) {
+ 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;
+ << " but should be zero!" << std::endl;
}
}
} else {
@@ -304,23 +309,22 @@ main(int argc, const char ** argv)
return -1;
}
-
- //Test histogram computation on image size < nbThreads
+ // Test histogram computation on image size < nbThreads
I_test.init(3, 1);
I_test = 100;
histogram.calculate(I_test, 256, 4);
- if(histogram.getSize() == 256) {
- for(unsigned int cpt = 0; cpt < 256; cpt++) {
- if(cpt == 100) {
- if(histogram[cpt] != I_test.getSize()) {
+ if (histogram.getSize() == 256) {
+ 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]
- << " but should be: " << I_test.getSize() << std::endl;
+ << " but should be: " << I_test.getSize() << std::endl;
return -1;
}
} else {
- if(histogram[cpt] != 0) {
+ if (histogram[cpt] != 0) {
std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt]
- << " but should be zero!" << std::endl;
+ << " but should be zero!" << std::endl;
}
}
}
@@ -329,23 +333,22 @@ main(int argc, const char ** argv)
return -1;
}
-
- //Test histogram computation on small image size
+ // Test histogram computation on small image size
I_test.init(7, 1);
I_test = 50;
histogram.calculate(I_test, 256, 4);
- if(histogram.getSize() == 256) {
- for(unsigned int cpt = 0; cpt < 256; cpt++) {
- if(cpt == 50) {
- if(histogram[cpt] != I_test.getSize()) {
+ if (histogram.getSize() == 256) {
+ 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]
- << " but should be: " << I_test.getSize() << std::endl;
+ << " but should be: " << I_test.getSize() << std::endl;
return -1;
}
} else {
- if(histogram[cpt] != 0) {
+ if (histogram[cpt] != 0) {
std::cerr << "Problem with histogram computation: histogram[" << cpt << "]=" << histogram[cpt]
- << " but should be zero!" << std::endl;
+ << " but should be zero!" << std::endl;
}
}
}
@@ -354,11 +357,9 @@ main(int argc, const char ** argv)
return -1;
}
-
std::cout << "testHistogram is OK!" << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch an exception: " << e.what() << std::endl;
return 1;
}
diff --git a/modules/core/test/tools/io/testBuildInformation.cpp b/modules/core/test/tools/io/testBuildInformation.cpp
index 2cdfaf6..2151ea9 100644
--- a/modules/core/test/tools/io/testBuildInformation.cpp
+++ b/modules/core/test/tools/io/testBuildInformation.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,7 +45,6 @@
#include <iostream>
#include <visp3/core/vpIoTools.h>
-
int main()
{
std::string info = vpIoTools::getBuildInformation();
diff --git a/modules/core/test/tools/io/testIoTools.cpp b/modules/core/test/tools/io/testIoTools.cpp
index 3d7c1da..e0b0360 100644
--- a/modules/core/test/tools/io/testIoTools.cpp
+++ b/modules/core/test/tools/io/testIoTools.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,21 +42,17 @@
\brief Test functions in IoTools.
*/
+#include <iostream>
#include <stdio.h>
#include <string.h>
-#include <iostream>
#include <visp3/core/vpIoTools.h>
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
const char c = vpIoTools::separator;
- if(c == '\\')
- {
+ if (c == '\\') {
std::cout << "The directory separator character is '" << c << "' (Windows platform)." << std::endl;
- }
- else {
+ } else {
std::cout << "The directory separator character is '" << c << "' (Unix like platform)." << std::endl;
}
@@ -68,15 +65,14 @@ main(int argc, const char ** argv)
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"))
- {
+ if (argc == 3 && std::string(argv[1]) == std::string("-i")) {
std::cout << "Parent of " << argv[2] << " is " << vpIoTools::getParent(argv[2]) << std::endl;
std::cout << "Name of " << argv[2] << " is " << vpIoTools::getName(argv[2]) << std::endl;
}
std::string windowsPathnameStyle = "\\usr\\bin\\java";
- std::cout << "Parent of " << windowsPathnameStyle << " is " << vpIoTools::getParent(windowsPathnameStyle) << std::endl;
+ std::cout << "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";
@@ -91,12 +87,14 @@ main(int argc, const char ** argv)
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;
@@ -112,229 +110,212 @@ main(int argc, const char ** argv)
pathname = "fictional directory/fictional file.txt";
std::cout << "isAbsolutePath of " << pathname << "=" << vpIoTools::isAbsolutePathname(pathname) << std::endl;
-
- //Test vpIoTools::splitDrive
+ // 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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").first << " should be=c:" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("c:\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
+ if (strcmp(vpIoTools::splitDrive("c:\\foo\\bar").second.c_str(), "\\foo\\bar") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("c:\\foo\\bar").second << " should be=\\foo\\bar" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("c:/foo/bar").first.c_str(), "c:") == 0) {
+ if (strcmp(vpIoTools::splitDrive("c:/foo/bar").first.c_str(), "c:") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").first << " should be=c:" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("c:/foo/bar").second.c_str(), "/foo/bar") == 0) {
+ if (strcmp(vpIoTools::splitDrive("c:/foo/bar").second.c_str(), "/foo/bar") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("c:/foo/bar").second << " should be=/foo/bar" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "\\\\conky\\mountpoint") == 0) {
+ if (strcmp(vpIoTools::splitDrive("\\\\conky\\mountpoint\\foo\\bar").first.c_str(), "\\\\conky\\mountpoint") == 0) {
nbOk++;
- }
- else {
+ } 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 {
+ } 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 {
+ } 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 {
+ } 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 {
+ } 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 {
+ } 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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("///conky/mountpoint/foo/bar").first << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second.c_str(), "///conky/mountpoint/foo/bar") == 0) {
+ if (strcmp(vpIoTools::splitDrive("///conky/mountpoint/foo/bar").second.c_str(), "///conky/mountpoint/foo/bar") == 0) {
nbOk++;
- }
- else {
+ } 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 {
+ } 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 {
+ } 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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::splitDrive("//conky//mountpoint/foo/bar").first << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second.c_str(), "//conky//mountpoint/foo/bar") == 0) {
+ if (strcmp(vpIoTools::splitDrive("//conky//mountpoint/foo/bar").second.c_str(), "//conky//mountpoint/foo/bar") == 0) {
nbOk++;
- }
- else {
+ } 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;
}
- 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 > 0) {
+ if (nbFail) {
std::cerr << "Failed test: vpIoTools::splitDrive (Win32)" << std::endl;
return EXIT_FAILURE;
}
#endif
-
- //Test vpIoTools::getFileExtension
+// Test vpIoTools::getFileExtension
#if defined(_WIN32)
nbFail = 0;
nbOk = 0;
- if(strcmp(vpIoTools::getFileExtension("foo.ext").c_str(), ".ext") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("foo.ext").c_str(), ".ext") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("/foo/foo.ext").c_str(), ".ext") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("/foo/foo.ext").c_str(), ".ext") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("/foo/foo.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension(".ext").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension(".ext").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("\\foo.ext\\foo") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("foo.ext\\").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("foo.ext\\").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.ext\\") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("xx/foo.bar.ext").c_str(), ".ext") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("xx/foo.bar.ext").c_str(), ".ext") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("xx/foo.bar.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("xx\\foo.bar.ext").c_str(), ".ext") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("xx\\foo.bar.ext").c_str(), ".ext") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("xx\\foo.bar.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("c:a/b\\c.d").c_str(), ".d") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("c:a/b\\c.d").c_str(), ".d") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("c:a/b\\c.d") << " should be=.d" << std::endl;
}
- std::cout << "Test vpIoTools::getFileExtension (WIN32 platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
+ std::cout << "Test vpIoTools::getFileExtension (WIN32 platform) - passed: " << nbOk << "/" << (nbOk + nbFail)
+ << std::endl;
- if (nbFail > 0) {
+ if (nbFail) {
std::cerr << "Failed test: vpIoTools::getFileExtension (WIN32 platform)" << std::endl;
return EXIT_FAILURE;
}
@@ -342,107 +323,146 @@ main(int argc, const char ** argv)
nbFail = 0;
nbOk = 0;
- if(strcmp(vpIoTools::getFileExtension("foo.bar").c_str(), ".bar") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("foo.bar").c_str(), ".bar") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.bar") << " should be=.bar" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("foo.boo.bar").c_str(), ".bar") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("foo.boo.bar").c_str(), ".bar") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.bar") << " should be=.bar" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("foo.boo.biff.bar").c_str(), ".bar") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("foo.boo.biff.bar").c_str(), ".bar") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("foo.boo.biff.bar") << " should be=.bar" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension(".csh.rc").c_str(), ".rc") == 0) {
+ if (strcmp(vpIoTools::getFileExtension(".csh.rc").c_str(), ".rc") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension(".csh.rc") << " should be=.rc" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("nodots").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("nodots").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("nodots") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension(".cshrc").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension(".cshrc").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension(".cshrc") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("...manydots").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("...manydots").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
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 {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("...manydots.ext") << " should be=.ext" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension(".").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension(".").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension(".") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("..").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("..").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("..") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("........").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("........").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("........") << " should be=" << std::endl;
}
- if(strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
+ if (strcmp(vpIoTools::getFileExtension("").c_str(), "") == 0) {
nbOk++;
- }
- else {
+ } else {
nbFail++;
std::cout << "Fail=" << vpIoTools::getFileExtension("") << " should be=" << std::endl;
}
- std::cout << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
+ std::cout << "Test vpIoTools::getFileExtension (Unix-like platform) - passed: " << nbOk << "/" << (nbOk + nbFail)
+ << std::endl;
+#endif
+
+ // Test makeDirectory()
+ try {
+ 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/";
+#endif
+ vpIoTools::makeDirectory(directory_filename);
+ vpIoTools::makeDirectory(directory_filename);
+ std::cout << "Create directories: " << directory_filename
+ << " ; 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";
+#endif
+ vpIoTools::makeDirectory(directory_filename);
+ std::cout << "Create directories: " << directory_filename
+ << " ; 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";
+#endif
+ vpIoTools::makeDirectory(directory_filename);
+ vpIoTools::makeDirectory(directory_filename);
+ std::cout << "Create directories: " << directory_filename
+ << " ; 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";
#endif
+ vpIoTools::makeDirectory(directory_filename);
+ std::cout << "Create directories: " << directory_filename
+ << " ; check: " << vpIoTools::checkDirectory(directory_filename) << std::endl;
+ } catch (const vpException &e) {
+ std::cerr << "Exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+ // Get the user login name
+ std::string username = "";
+ vpIoTools::getUserName(username);
+ std::ofstream dummy_file;
- //Test isSamePathname()
+// Test isSamePathname()
#if defined(_WIN32)
std::string path1 = "tmp/test/file.txt";
std::string path2 = "tmp/test/../test/file.txt";
@@ -451,47 +471,44 @@ main(int argc, const char ** argv)
nbFail = 0;
bool res;
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
path1 = ".\\tmp/test/file.txt";
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
path1 = ".\\tmp/test\\../fake dir/..\\test\\file.txt";
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
path2 = "/tmp/test/../test/file.txt";
- res = vpIoTools::isSamePathname(path1, path2); //False
+ res = vpIoTools::isSamePathname(path1, path2); // False
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk : nbOk+1;
- nbFail = res ? nbFail+1 : nbFail;
+ nbOk = res ? nbOk : nbOk + 1;
+ nbFail = res ? nbFail + 1 : nbFail;
- std::cout << "Test vpIoTools::isSamePathname (WIN32 platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
- if (nbFail > 0) {
+ 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;
return EXIT_FAILURE;
}
#else
- //realpath requires not fake path, so we create dummy file and directories
+ // realpath requires not fake path, so we create dummy file and directories
- // Get the user login name
- std::string username = "";
- vpIoTools::getUserName(username);
- vpIoTools::makeDirectory("/tmp/" + username);
vpIoTools::makeDirectory("/tmp/" + username + "/test");
vpIoTools::makeDirectory("/tmp/" + username + "/dummy dir");
std::string path1 = "/tmp/" + username + "/test/file.txt";
std::string path2 = "/tmp/" + username + "/test/../test/file.txt";
- std::ofstream dummy_file(path1.c_str());
+ dummy_file.open(path1.c_str());
if (!dummy_file.is_open()) {
return EXIT_SUCCESS;
}
@@ -501,42 +518,59 @@ main(int argc, const char ** argv)
nbFail = 0;
bool res;
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
path1 = "\\tmp/" + username + "/./test/file.txt";
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ 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
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
path2 = "/tmp/" + username + "/test/../test";
- res = vpIoTools::isSamePathname(path1, path2); //False
+ res = vpIoTools::isSamePathname(path1, path2); // False
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk : nbOk+1;
- nbFail = res ? nbFail+1 : nbFail;
+ nbOk = res ? nbOk : nbOk + 1;
+ nbFail = res ? nbFail + 1 : nbFail;
path1 = "/tmp/" + username + "/test/";
- res = vpIoTools::isSamePathname(path1, path2); //True
+ res = vpIoTools::isSamePathname(path1, path2); // True
std::cout << "vpIoTools::isSamePathname(" << path1 << ", " << path2 << ")? " << res << std::endl;
- nbOk = res ? nbOk+1 : nbOk;
- nbFail = res ? nbFail : nbFail+1;
+ nbOk = res ? nbOk + 1 : nbOk;
+ nbFail = res ? nbFail : nbFail + 1;
- std::cout << "Test vpIoTools::isSamePathname (Unix platform) - passed: " << nbOk << "/" << (nbOk+nbFail) << std::endl;
- if (nbFail > 0) {
+ 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;
return EXIT_FAILURE;
}
#endif
+ // Test checkFilename()
+ 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";
+ dummy_file.open(path1.c_str());
+ if (!dummy_file.is_open()) {
+ return EXIT_SUCCESS;
+ }
+ dummy_file.close();
+
+ if (!vpIoTools::checkFilename(path1)) {
+ std::cerr << "Problem with checkFilename(" << path1 << ")!" << std::endl;
+ return EXIT_FAILURE;
+ }
+ std::cout << "Test vpIoTools::checkFilename() is ok." << std::endl;
std::cout << std::endl << "End" << std::endl;
return EXIT_SUCCESS;
diff --git a/modules/core/test/tools/threading/testMutex.cpp b/modules/core/test/tools/threading/testMutex.cpp
index cd40b78..afc4803 100644
--- a/modules/core/test/tools/threading/testMutex.cpp
+++ b/modules/core/test/tools/threading/testMutex.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,21 +51,22 @@
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
-int thread_counter=0;
+int thread_counter = 0;
vpMutex mutex;
vpThread::Return doSomeThing(vpThread::Args args)
{
mutex.lock();
- unsigned int thread_id = *((unsigned int *) args);
+ unsigned int thread_id = *((unsigned int *)args);
std::cout << "Started job " << thread_counter << " with id " << thread_id << std::endl;
- for(unsigned long i=0; i<(0xFFFF);i++) {};
+ for (unsigned long i = 0; i < (0xFFFF); i++) {
+ };
std::cout << "Ended job " << thread_counter << std::endl;
- thread_counter ++;
+ thread_counter++;
mutex.unlock();
return 0;
@@ -72,18 +74,17 @@ vpThread::Return doSomeThing(vpThread::Args args)
int main(void)
{
- unsigned int nthread=10;
- vpThread *thread = new vpThread [nthread];
- unsigned int *thread_id = new unsigned int [nthread];
+ unsigned int nthread = 10;
+ vpThread *thread = new vpThread[nthread];
+ unsigned int *thread_id = new unsigned int[nthread];
- for(unsigned int i=0; i<nthread; i++)
- {
+ for (unsigned int i = 0; i < nthread; i++) {
thread_id[i] = i;
thread[i].create((vpThread::Fn)&doSomeThing, (vpThread::Args)&thread_id[i]);
}
- delete [] thread;
- delete [] thread_id;
+ delete[] thread;
+ delete[] thread_id;
return 0;
}
@@ -91,10 +92,10 @@ int main(void)
#else
int main()
{
-# 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::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl;
-# else
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
}
#endif
diff --git a/modules/core/test/tools/threading/testThread.cpp b/modules/core/test/tools/threading/testThread.cpp
index 768b954..cae484a 100644
--- a/modules/core/test/tools/threading/testThread.cpp
+++ b/modules/core/test/tools/threading/testThread.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -69,18 +70,19 @@ vpThread::Return myBarFunction(vpThread::Args args)
vpThread::Return myQuxFunction(vpThread::Args args)
{
- unsigned int args_ = *((unsigned int *) args);
+ unsigned int args_ = *((unsigned int *)args);
std::cout << "qux arg: " << args_ << std::endl;
// do stuff...
return 0;
}
-int main()
+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;
@@ -95,9 +97,12 @@ int main()
std::cout << "bar: " << bar.joinable() << std::endl;
std::cout << "qux: " << qux.joinable() << std::endl;
- if (foo.joinable()) foo.join();
- if (bar.joinable()) bar.join();
- if (qux.joinable()) qux.join();
+ if (foo.joinable())
+ foo.join();
+ if (bar.joinable())
+ bar.join();
+ if (qux.joinable())
+ qux.join();
std::cout << "Joinable after joining:" << std::endl;
std::cout << "foo: " << foo.joinable() << std::endl;
@@ -114,10 +119,10 @@ int main()
int main()
{
-# 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::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl;
-# else
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
}
#endif
diff --git a/modules/core/test/tools/threading/testThread2.cpp b/modules/core/test/tools/threading/testThread2.cpp
index 8bf577d..20eab1f 100644
--- a/modules/core/test/tools/threading/testThread2.cpp
+++ b/modules/core/test/tools/threading/testThread2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,138 +47,149 @@
#include <stdlib.h>
#include <time.h>
-#include <visp3/core/vpThread.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpThread.h>
+namespace
+{
+//! [functor-thread-example declaration]
+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)
+ {
+ }
-namespace {
- //! [functor-thread-example declaration]
- 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() : 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(); }
- void operator()() {
- computeImpl();
- }
+ vpColVector getVectorAdd() const { return m_add; }
- vpColVector getVectorAdd() const {
- return m_add;
- }
+ vpColVector getVectorMul() const { return m_mul; }
- vpColVector getVectorMul() const {
- return m_mul;
- }
+private:
+ vpColVector m_add;
+ vpColVector m_mul;
+ vpColVector m_v1;
+ vpColVector m_v2;
+ unsigned int m_indexStart;
+ unsigned int m_indexEnd;
+
+ void computeImpl()
+ {
+ m_add.resize(m_indexEnd - m_indexStart);
+ m_mul.resize(m_indexEnd - m_indexStart);
- private:
- vpColVector m_add;
- vpColVector m_mul;
- vpColVector m_v1;
- vpColVector m_v2;
- unsigned int m_indexStart;
- unsigned int m_indexEnd;
-
- void computeImpl() {
- m_add.resize(m_indexEnd - m_indexStart);
- m_mul.resize(m_indexEnd - m_indexStart);
-
- //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++) {
- m_add[cpt] = m_v1[i] + m_v2[i];
- m_mul[cpt] = m_v1[i] * m_v2[i];
- }
+ // 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++) {
+ m_add[cpt] = m_v1[i] + m_v2[i];
+ m_mul[cpt] = m_v1[i] * m_v2[i];
}
}
- };
- //! [functor-thread-example declaration]
-
- //! [functor-thread-example threadFunction]
- vpThread::Return arithmThread(vpThread::Args args) {
- ArithmFunctor* f = static_cast<ArithmFunctor*>(args);
- (*f)();
- return 0;
}
- //! [functor-thread-example threadFunction]
+};
+//! [functor-thread-example declaration]
- void insert(vpColVector &v1, const vpColVector &v2) {
- unsigned int size = v1.size();
- v1.resize(size + v2.size(), false);
+//! [functor-thread-example threadFunction]
+vpThread::Return arithmThread(vpThread::Args args)
+{
+ ArithmFunctor *f = static_cast<ArithmFunctor *>(args);
+ (*f)();
+ return 0;
+}
+//! [functor-thread-example threadFunction]
- for (unsigned int i = 0, cpt = size; i < v2.size(); i++, cpt++) {
- v1[cpt] = v2[i];
- }
+void insert(vpColVector &v1, const vpColVector &v2)
+{
+ unsigned int size = v1.size();
+ v1.resize(size + v2.size(), false);
+
+ for (unsigned int i = 0, cpt = size; i < v2.size(); i++, cpt++) {
+ v1[cpt] = v2[i];
}
+}
- 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++) {
- add += v1[i] + v2[i];
- mul += v1[i] * v2[i];
- }
+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++) {
+ add += v1[i] + v2[i];
+ mul += v1[i] * v2[i];
+ }
- double add_th = res_add.sum();
- double mul_th = res_mul.sum();
+ double add_th = res_add.sum();
+ double mul_th = res_mul.sum();
- std::cout << "add=" << add << " ; add_th=" << add_th << std::endl;
- std::cout << "mul=" << mul << " ; mul_th=" << mul_th << std::endl;
+ std::cout << "add=" << add << " ; add_th=" << add_th << std::endl;
+ std::cout << "mul=" << mul << " ; mul_th=" << mul_th << std::endl;
- if (!vpMath::equal(add, add_th, std::numeric_limits<double>::epsilon())) {
- std::cerr << "Problem: add=" << add << " ; add_th=" << add_th << std::endl;
- return false;
- }
-
- if (!vpMath::equal(mul, mul_th, std::numeric_limits<double>::epsilon())) {
- std::cerr << "Problem: mul=" << mul << " ; mul_th=" << mul_th << std::endl;
- return false;
- }
+ if (!vpMath::equal(add, add_th, std::numeric_limits<double>::epsilon())) {
+ std::cerr << "Problem: add=" << add << " ; add_th=" << add_th << std::endl;
+ return false;
+ }
- return true;
+ if (!vpMath::equal(mul, mul_th, std::numeric_limits<double>::epsilon())) {
+ std::cerr << "Problem: mul=" << mul << " ; mul_th=" << mul_th << std::endl;
+ return false;
}
-}
-int main() {
- unsigned int nb_threads = 4;
- unsigned int size = 1000007;
- srand((unsigned int) time(NULL));
+ return true;
+}
+}
- vpColVector v1(size), v2(size);
- for (unsigned int i = 0; i < size; i++) {
- v1[i] = rand() % 101;
- v2[i] = rand() % 101;
+int main()
+{
+ std::string appveyor_threading = "";
+ try {
+ appveyor_threading = vpIoTools::getenv("APPVEYOR_THREADING");
+ } catch (...) {
}
- //! [functor-thread-example threadCreation]
- std::vector<vpThread> threads(nb_threads);
- std::vector<ArithmFunctor> functors(nb_threads);
- unsigned int split = size / nb_threads;
- for (unsigned int i = 0; i < nb_threads; i++) {
- if (i < nb_threads-1) {
- functors[i] = ArithmFunctor(v1, v2, i*split, (i+1)*split);
- } else {
- functors[i] = ArithmFunctor(v1, v2, i*split, size);
+ if (appveyor_threading == "true") {
+ unsigned int nb_threads = 4;
+ unsigned int size = 1000007;
+ srand((unsigned int)time(NULL));
+
+ vpColVector v1(size), v2(size);
+ for (unsigned int i = 0; i < size; i++) {
+ v1[i] = rand() % 101;
+ v2[i] = rand() % 101;
}
- threads[i].create((vpThread::Fn) arithmThread, (vpThread::Args) &functors[i]);
- }
- //! [functor-thread-example threadCreation]
+ //! [functor-thread-example threadCreation]
+ std::vector<vpThread> threads(nb_threads);
+ std::vector<ArithmFunctor> functors(nb_threads);
+ unsigned int split = size / nb_threads;
+ for (unsigned int i = 0; i < nb_threads; i++) {
+ if (i < nb_threads - 1) {
+ functors[i] = ArithmFunctor(v1, v2, i * split, (i + 1) * split);
+ } else {
+ functors[i] = ArithmFunctor(v1, v2, i * split, size);
+ }
- //! [functor-thread-example getResults]
- vpColVector res_add, res_mul;
- for (size_t i = 0; i < nb_threads; i++) {
- threads[i].join();
+ std::cout << "Create thread: " << i << std::endl;
+ threads[i].create((vpThread::Fn)arithmThread, (vpThread::Args)&functors[i]);
+ }
+ //! [functor-thread-example threadCreation]
- insert(res_add, functors[i].getVectorAdd());
- insert(res_mul, functors[i].getVectorMul());
- }
- //! [functor-thread-example getResults]
+ //! [functor-thread-example getResults]
+ vpColVector res_add, res_mul;
+ for (size_t i = 0; i < nb_threads; i++) {
+ std::cout << "Join thread: " << i << std::endl;
+ threads[i].join();
+
+ insert(res_add, functors[i].getVectorAdd());
+ insert(res_mul, functors[i].getVectorMul());
+ }
+ //! [functor-thread-example getResults]
- if (!check(v1, v2, res_add, res_mul)) {
- return EXIT_FAILURE;
+ if (!check(v1, v2, res_add, res_mul)) {
+ return EXIT_FAILURE;
+ }
}
std::cout << "testThread2 is ok!" << std::endl;
@@ -186,16 +198,16 @@ int main() {
#else
-#include <iostream>
#include <cstdlib>
+#include <iostream>
int main()
{
-# 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::cout << "You should enable pthread usage and rebuild ViSP..." << std::endl;
-# else
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
return EXIT_SUCCESS;
}
#endif
diff --git a/modules/core/test/tools/time/testTime.cpp b/modules/core/test/tools/time/testTime.cpp
index 27aed6d..a582e5d 100644
--- a/modules/core/test/tools/time/testTime.cpp
+++ b/modules/core/test/tools/time/testTime.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,17 +45,17 @@
*/
#include <visp3/core/vpConfig.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
+#include <unistd.h>
#elif defined(_WIN32)
-# include <windows.h>
-# include <mmsystem.h>
-# include <winbase.h>
+//#include <mmsystem.h>
+//#include <winbase.h>
+#include <windows.h>
#endif
-#include <stdlib.h>
-#include <stdio.h>
-#include <time.h>
#include <cmath>
#include <iostream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
#include <visp3/core/vpTime.h>
@@ -65,8 +66,8 @@ int main()
double v = 0;
double t0 = vpTime::measureTimeMs();
- for (int i =0 ; i < 100000; i ++)
- for (int j =0 ; j < 100; j ++)
+ for (int i = 0; i < 100000; i++)
+ for (int j = 0; j < 100; j++)
v = i * 2 / 3. + j;
std::cout << "Computed dummy value: " << v << std::endl;
@@ -75,18 +76,18 @@ int main()
double t2 = vpTime::measureTimeMs();
- // Sleep 10ms
+// Sleep 10ms
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep(10*1000);
+ usleep(10 * 1000);
#elif defined(_WIN32)
Sleep(10);
#endif
double t3 = vpTime::measureTimeMs();
- // Sleep 2ms
+// Sleep 2ms
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- usleep(2*1000);
+ usleep(2 * 1000);
#elif defined(_WIN32)
Sleep(2);
#endif
@@ -119,12 +120,11 @@ int main()
std::cout << "t8-t7: wait(2 ms): " << t8 - t7 << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
-# else
+#else
std::cout << "vpTime is not implemented on Universal Windows Platform" << std::endl;
-# endif
+#endif
}
diff --git a/modules/core/test/tools/xml/testXmlParser.cpp b/modules/core/test/tools/xml/testXmlParser.cpp
index b24db08..6012b4f 100644
--- a/modules/core/test/tools/xml/testXmlParser.cpp
+++ b/modules/core/test/tools/xml/testXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,14 +36,13 @@
*
*****************************************************************************/
-
/*!
\example testXmlParser.cpp
XML parser example.
-
- This example contains the declaration of a class used to read and write data
- in a xml file like:
+
+ This example contains the declaration of a class used to read and write data
+ in a xml file like:
\code
<config>
<range>5.5</range>
@@ -51,79 +51,71 @@
<name>Object</name>
</config>
\endcode
-
-*/
+*/
#include <visp3/core/vpConfig.h>
#include <iostream>
#if defined(VISP_HAVE_XML2)
-#include <visp3/core/vpXmlParser.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpXmlParser.h>
#include <visp3/io/vpParseArgv.h>
#include <string>
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-/* -------------------------------------------------------------------------- */
-/* CLASS EXAMPLE */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* CLASS EXAMPLE */
+/* --------------------------------------------------------------------------
+ */
/*!
\class vpExampleDataParser
- \brief Class example used to show how to implement a xml parser based on the
+ \brief Class example used to show how to implement a xml parser based on the
vpXmlParser
*/
-class vpExampleDataParser: public vpXmlParser
+class vpExampleDataParser : public vpXmlParser
{
protected:
double m_range;
int m_step;
int m_size_filter;
std::string m_name;
-
- typedef enum{
- config,
- range,
- step,
- size_filter,
- name
- }dataToParse;
-
-
+
+ typedef enum { config, range, step, size_filter, name } dataToParse;
+
public:
vpExampleDataParser();
virtual ~vpExampleDataParser();
-
+
// Data accessors.
- double getRange() const {return m_range;}
- int getStep() const {return m_step;}
- int getSizeFilter() const {return m_size_filter;}
- std::string getName() const {return m_name;}
-
- void setRange(const double _range) {m_range = _range;}
- void setStep(const int _step) {m_step = _step;}
- void setSizeFilter(const int _size_filter) {m_size_filter = _size_filter;}
- void setName(const std::string& _name) { m_name = _name;}
-
-protected:
- virtual void readMainClass (xmlDocPtr doc, xmlNodePtr node);
- virtual void writeMainClass (xmlNodePtr node);
+ double getRange() const { return m_range; }
+ int getStep() const { return m_step; }
+ int getSizeFilter() const { return m_size_filter; }
+ std::string getName() const { return m_name; }
+
+ void setRange(const double _range) { m_range = _range; }
+ void setStep(const int _step) { m_step = _step; }
+ void setSizeFilter(const int _size_filter) { m_size_filter = _size_filter; }
+ void setName(const std::string &_name) { m_name = _name; }
+
+protected:
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+ virtual void writeMainClass(xmlNodePtr node);
};
/*!
Constructor.
- Initialise the map according to the data to parse, and initialise data to
- default values.
-
+ Initialise the map according to the data to parse, and initialise data to
+ default values.
+
*/
-vpExampleDataParser::vpExampleDataParser()
- : m_range(0.), m_step(0), m_size_filter(0), m_name("")
+vpExampleDataParser::vpExampleDataParser() : m_range(0.), m_step(0), m_size_filter(0), m_name("")
{
nodeMap["config"] = config;
nodeMap["range"] = range;
@@ -134,29 +126,25 @@ vpExampleDataParser::vpExampleDataParser()
/*!
Destructor.
-
+
*/
-vpExampleDataParser::~vpExampleDataParser()
-{
-
-}
+vpExampleDataParser::~vpExampleDataParser() {}
/*!
- Read the main class. This method corresponds to the parsing of the main
- document (which contains the whole data in the class). At this point, the
- document exists and is open.
-
- \param doc : Pointer to the document to parse.
- \param node : Pointer to the root node of the document.
+ Read the main class. This method corresponds to the parsing of the main
+ document (which contains the whole data in the class). At this point, the
+ document exists and is open.
+
+ \param doc : Pointer to the document to parse.
+ \param node : Pointer to the root node of the document.
*/
-void
-vpExampleDataParser::readMainClass (xmlDocPtr doc, xmlNodePtr node)
+void vpExampleDataParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
case range:
this->m_range = xmlReadDoubleChild(doc, dataNode);
break;
@@ -166,9 +154,9 @@ vpExampleDataParser::readMainClass (xmlDocPtr doc, xmlNodePtr node)
case size_filter:
this->m_size_filter = xmlReadIntChild(doc, dataNode);
break;
- case name:{
+ case name: {
this->m_name = xmlReadStringChild(doc, dataNode);
- }break;
+ } break;
default:
vpTRACE("unknown tag in readConfigNode : %d, %s", iter_data->second, (iter_data->first).c_str());
break;
@@ -179,33 +167,33 @@ vpExampleDataParser::readMainClass (xmlDocPtr doc, xmlNodePtr node)
}
/*!
- Write the data in the file.
- The file has already been opened or created in the save() method. And the root
- node (corresponding to the main tag) has already been writen.
-
- \param node : Pointer to the root node.
+ Write the data in the file.
+ The file has already been opened or created in the save() method. And the
+ root node (corresponding to the main tag) has already been writen.
+
+ \param node : Pointer to the root node.
*/
-void
-vpExampleDataParser::writeMainClass(xmlNodePtr node)
+void vpExampleDataParser::writeMainClass(xmlNodePtr node)
{
- xmlWriteDoubleChild(node, (const char*)"range", m_range);
- xmlWriteIntChild(node, (const char*)"step", m_step);
- xmlWriteIntChild(node, (const char*)"size_filter", m_size_filter);
- xmlWriteCharChild(node, (const char*)"name", m_name.c_str());
+ xmlWriteDoubleChild(node, (const char *)"range", m_range);
+ xmlWriteIntChild(node, (const char *)"step", m_step);
+ xmlWriteIntChild(node, (const char *)"size_filter", m_size_filter);
+ xmlWriteCharChild(node, (const char *)"name", m_name.c_str());
}
-
#endif // doxygen
-/* -------------------------------------------------------------------------- */
-/* COMMAND LINE OPTIONS */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* COMMAND LINE OPTIONS */
+/* --------------------------------------------------------------------------
+ */
// List of allowed command line options
-#define GETOPTARGS "cdo:h"
+#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);
/*!
@@ -217,7 +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\
@@ -237,7 +225,7 @@ OPTIONS: Default\n\
Print the help.\n\n", opath.c_str(), user.c_str());
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
}
@@ -251,22 +239,29 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, opath, user); return false; break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, opath, user);
+ return false;
+ break;
case 'c':
case 'd':
break;
default:
- usage(argv[0], optarg_, opath, user); return false; break;
+ usage(argv[0], optarg_, opath, user);
+ return false;
+ break;
}
}
@@ -281,13 +276,13 @@ bool getOptions(int argc, const char **argv, std::string &opath, const std::stri
return true;
}
+/* --------------------------------------------------------------------------
+ */
+/* MAIN FUNCTION */
+/* --------------------------------------------------------------------------
+ */
-
-/* -------------------------------------------------------------------------- */
-/* MAIN FUNCTION */
-/* -------------------------------------------------------------------------- */
-
-int main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
try {
std::string opt_opath;
@@ -295,13 +290,13 @@ int main(int argc, const char** argv)
std::string filename;
std::string username;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << " testXmlParser.cpp" <<std::endl << std::endl ;
- std::cout << " writing and readind data using a xml parser" << std::endl ;
- std::cout << "-------------------------------------------------------" << std::endl ;
- std::cout << std::endl ;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << " testXmlParser.cpp" << std::endl << std::endl;
+ std::cout << " writing and readind data using a xml parser" << std::endl;
+ std::cout << "-------------------------------------------------------" << std::endl;
+ std::cout << std::endl;
- // Set the default output path
+// Set the default output path
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
opt_opath = "/tmp";
#elif defined(_WIN32)
@@ -313,7 +308,7 @@ int main(int argc, const char** argv)
// Read the command line options
if (getOptions(argc, argv, opt_opath, username) == false) {
- exit (-1);
+ exit(-1);
}
// Get the option values
@@ -328,11 +323,9 @@ int main(int argc, const char** argv)
try {
// Create the dirname
vpIoTools::makeDirectory(dirname);
- }
- catch (...) {
+ } catch (...) {
usage(argv[0], NULL, opath, username);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot create " << dirname << std::endl;
std::cerr << " Check your -o " << opath << " option " << std::endl;
exit(-1);
@@ -361,7 +354,7 @@ int main(int argc, const char** argv)
parser2.parse(filename);
- std::cout << "Read from " << filename << std::endl ;
+ std::cout << "Read from " << filename << std::endl;
std::cout << "Range : " << parser2.getRange() << std::endl;
std::cout << "Step : " << parser2.getStep() << std::endl;
std::cout << "Filter size : " << parser2.getSizeFilter() << std::endl;
@@ -371,8 +364,7 @@ int main(int argc, const char** argv)
// Clean up memory allocated by the xml library
vpXmlParser::cleanup();
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/detection/CMakeLists.txt b/modules/detection/CMakeLists.txt
index bba6cc4..3aacc2d 100644
--- a/modules/detection/CMakeLists.txt
+++ b/modules/detection/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -52,9 +53,20 @@ if(USE_ZBAR)
# Add specific build flag to turn off warnings coming from zbar 3rd party
vp_set_source_file_compile_flag(src/barcode/vpDetectorQRCode.cpp -Wno-unused-parameter)
endif()
+if(WITH_APRILTAG)
+ # April Tag is private
+ include_directories(${APRILTAG_INCLUDE_DIRS})
-vp_add_module(detection visp_core)
+ # Add specific build flag to turn off warnings coming from apriltag 3rd party
+ if(UNIX OR MINGW)
+ vp_set_source_file_compile_flag(src/tag/vpDetectorAprilTag.cpp -Wno-unused-parameter -Wno-strict-overflow)
+ else(WIN32)
+ vp_set_source_file_compile_flag(src/tag/vpDetectorAprilTag.cpp /wd"4200")
+ endif()
+endif()
+
+vp_add_module(detection visp_core visp_vision PRIVATE_OPTIONAL ${APRILTAG_LIBRARIES})
vp_glob_module_sources()
vp_module_include_directories(${opt_incs})
vp_create_module(${opt_libs})
-
+vp_add_tests(DEPENDS_ON visp_detection visp_gui visp_io)
diff --git a/modules/detection/include/visp3/detection/vpDetectorAprilTag.h b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h
new file mode 100644
index 0000000..9b8871e
--- /dev/null
+++ b/modules/detection/include/visp3/detection/vpDetectorAprilTag.h
@@ -0,0 +1,291 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Base class for April Tag detection.
+ *
+ *****************************************************************************/
+#ifndef __vpDetectorAprilTag_h__
+#define __vpDetectorAprilTag_h__
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_APRILTAG
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/detection/vpDetectorBase.h>
+
+/*!
+ \class vpDetectorAprilTag
+ \ingroup group_detection_tag
+ Base class for AprilTag detector. This class is a wrapper over <a
+href="https://april.eecs.umich.edu/software/apriltag.html">AprilTag</a>. 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 <I> AprilTag: A robust and flexible visual fiducial system </I>
+(\cite olson2011tags) and <I> AprilTag 2: Efficient and robust fiducial
+detection
+</I> (\cite wang2016iros).
+
+ The detect() function allows to detect multiple tags in an image. Once
+detected, for each tag it is possible to retrieve the location of the corners
+using getPolygon(), the encoded message using getMessage(), the bounding box
+using getBBox() and the center of gravity using getCog().
+
+ If camera parameters and the size of the tag are provided, thanks to
+ detect(const vpImage<unsigned char> &, const double, const
+vpCameraParameters &, std::vector<vpHomogeneousMatrix> &) this class allows
+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 <visp3/detection/vpDetectorAprilTag.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#ifdef VISP_HAVE_APRILTAG
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "image-tag36h11.pgm");
+
+ vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11);
+
+ bool status = detector.detect(I);
+ if (status) {
+ for(size_t i=0; i < detector.getNbObjects(); i++) {
+ std::cout << "Tag code " << i << ":" << std::endl;
+ std::vector<vpImagePoint> p = detector.getPolygon(i);
+ for(size_t j=0; j < p.size(); j++)
+ std::cout << " Point " << j << ": " << p[j] << std::endl;
+ std::cout << " Message: \"" << detector.getMessage(i) << "\"" << std::endl;
+ }
+ }
+#endif
+}
+ \endcode
+
+ The previous example may produce results like:
+ \code
+Tag code 0:
+ Point 0: 124.008, 442.226
+ Point 1: 194.614, 441.237
+ Point 2: 184.833, 540.386
+ Point 3: 111.948, 533.634
+ Message: "36h11 id: 0"
+Tag code 1:
+ Point 0: 245.327, 438.801
+ Point 1: 338.116, 437.221
+ Point 2: 339.341, 553.539
+ Point 3: 238.954, 543.855
+ Message: "36h11 id: 1"
+ \endcode
+
+ This other example shows how to estimate the 3D pose of 36h11 AprilTag
+patterns.
+\code
+#include <visp3/detection/vpDetectorAprilTag.h>
+#include <visp3/io/vpImageIo.h>
+
+int main()
+{
+#ifdef VISP_HAVE_APRILTAG
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, "image-tag36h11.pgm");
+
+ vpDetectorAprilTag detector(vpDetectorAprilTag::TAG_36h11);
+ std::vector<vpHomogeneousMatrix> cMo;
+ vpCameraParameters cam;
+ 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;
+ }
+ }
+#endif
+}
+ \endcode
+ The previous example may produce results like:
+ \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
+
+ Other examples are also provided in tutorial-apriltag-detector.cpp and
+ tutorial-apriltag-detector-live.cpp
+*/
+class VISP_EXPORT vpDetectorAprilTag : public vpDetectorBase
+{
+
+public:
+ enum vpAprilTagFamily {
+ TAG_36h11, /*!< AprilTag <a
+ href="https://april.eecs.umich.edu/software/apriltag.html">36h11</a>
+ pattern (recommended) */
+ TAG_36h10, /*!< AprilTag <a
+ href="https://april.eecs.umich.edu/software/apriltag.html">36h10</a>
+ pattern */
+ TAG_36ARTOOLKIT, /*!< <a href="https://artoolkit.org/">ARToolKit</a>
+ pattern. */
+ TAG_25h9, /*!< AprilTag <a
+ href="https://april.eecs.umich.edu/software/apriltag.html">25h9</a>
+ pattern */
+ TAG_25h7, /*!< AprilTag <a
+ href="https://april.eecs.umich.edu/software/apriltag.html">25h7</a>
+ pattern */
+ TAG_16h5 /*!< AprilTag <a
+ href="https://april.eecs.umich.edu/software/apriltag.html">16h5</a>
+ pattern */
+ };
+
+ enum vpPoseEstimationMethod {
+ HOMOGRAPHY_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach
+ initialized by the homography approach */
+ DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach
+ initialized by the Dementhon approach */
+ LAGRANGE_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach
+ initialized by the Lagrange approach */
+ BEST_RESIDUAL_VIRTUAL_VS /*!< Non linear virtual visual servoing approach
+ initialized by the approach that gives the
+ lowest residual */
+ };
+
+ vpDetectorAprilTag(const vpAprilTagFamily &tagFamily = TAG_36h11,
+ const vpPoseEstimationMethod &poseEstimationMethod = HOMOGRAPHY_VIRTUAL_VS);
+ virtual ~vpDetectorAprilTag();
+
+ bool detect(const vpImage<unsigned char> &I);
+ bool detect(const vpImage<unsigned char> &I, const double tagSize, const vpCameraParameters &cam,
+ std::vector<vpHomogeneousMatrix> &cMo_vec);
+
+ /*!
+ Return the pose estimation method.
+ */
+ inline vpPoseEstimationMethod getPoseEstimationMethod() const { return m_poseEstimationMethod; }
+
+ void setAprilTagNbThreads(const int nThreads);
+ void setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod);
+ void setAprilTagQuadDecimate(const float quadDecimate);
+ void setAprilTagQuadSigma(const float quadSigma);
+ void setAprilTagRefineDecode(const bool refineDecode);
+ void setAprilTagRefineEdges(const bool refineEdges);
+ void setAprilTagRefinePose(const bool refinePose);
+
+ /*! Allow to enable the display of overlay tag information in the windows
+ * (vpDisplay) associated to the input image. */
+ inline void setDisplayTag(const bool display) { m_displayTag = display; }
+
+protected:
+ bool m_displayTag;
+ vpPoseEstimationMethod m_poseEstimationMethod;
+ vpAprilTagFamily m_tagFamily;
+
+private:
+ vpDetectorAprilTag(const vpDetectorAprilTag &); // noncopyable
+ vpDetectorAprilTag &operator=(const vpDetectorAprilTag &); //
+
+ // PIMPL idiom
+ class Impl;
+ Impl *m_impl;
+};
+
+inline std::ostream &operator<<(std::ostream &os, const vpDetectorAprilTag::vpPoseEstimationMethod &method)
+{
+ switch (method) {
+ case vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS:
+ os << "HOMOGRAPHY_VIRTUAL_VS";
+ break;
+
+ case vpDetectorAprilTag::DEMENTHON_VIRTUAL_VS:
+ os << "DEMENTHON_VIRTUAL_VS";
+ break;
+
+ case vpDetectorAprilTag::LAGRANGE_VIRTUAL_VS:
+ os << "LAGRANGE_VIRTUAL_VS";
+ break;
+
+ case vpDetectorAprilTag::BEST_RESIDUAL_VIRTUAL_VS:
+ os << "BEST_RESIDUAL_VIRTUAL_VS";
+ break;
+
+ default:
+ break;
+ }
+
+ return os;
+}
+
+inline std::ostream &operator<<(std::ostream &os, const vpDetectorAprilTag::vpAprilTagFamily &tagFamily)
+{
+ switch (tagFamily) {
+ case vpDetectorAprilTag::TAG_36h11:
+ os << "36h11";
+ break;
+
+ case vpDetectorAprilTag::TAG_36h10:
+ os << "36h10";
+ break;
+
+ case vpDetectorAprilTag::TAG_36ARTOOLKIT:
+ os << "36artoolkit";
+ break;
+
+ case vpDetectorAprilTag::TAG_25h9:
+ os << "25h9";
+ break;
+
+ case vpDetectorAprilTag::TAG_25h7:
+ os << "25h7";
+ break;
+
+ case vpDetectorAprilTag::TAG_16h5:
+ os << "16h5";
+ break;
+
+ default:
+ break;
+ }
+
+ return os;
+}
+
+#endif
+#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorBase.h b/modules/detection/include/visp3/detection/vpDetectorBase.h
index ff5d117..d7bd435 100644
--- a/modules/detection/include/visp3/detection/vpDetectorBase.h
+++ b/modules/detection/include/visp3/detection/vpDetectorBase.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,10 +39,10 @@
#ifndef __vpDetectorBase_h__
#define __vpDetectorBase_h__
-#include <vector>
-#include <utility>
-#include <string>
#include <assert.h>
+#include <string>
+#include <utility>
+#include <vector>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
@@ -53,17 +54,22 @@
Base class for object detection.
This class is a generic class that can be used to detect:
- - bar codes like QRcodes of Data matrices. The example given in tutorial-barcode-detector.cpp shows
- how to detect one or more bar codes in an image. In tutorial-barcode-detector-live.cpp you will find
- an other example that shows how to use this class to detect bar codes in images acquired by a camera.
+ - bar codes like QRcodes of Data matrices. The example given in
+ tutorial-barcode-detector.cpp shows how to detect one or more bar codes in
+ an image. In tutorial-barcode-detector-live.cpp you will find an other
+ example that shows how to use this class to detect bar codes in images
+ acquired by a camera.
- faces. An example is provided in tutorial-face-detector-live.cpp.
*/
class VISP_EXPORT vpDetectorBase
{
protected:
- std::vector< std::vector<vpImagePoint> > m_polygon; //!< For each object, defines the polygon that contains the object.
- std::vector< std::string > m_message; //!< Message attached to each object.
- size_t m_nb_objects; //!< Number of detected objects.
+ std::vector<std::vector<vpImagePoint> > m_polygon; //!< For each object,
+ //!< defines the polygon
+ //!< that contains the
+ //!< object.
+ std::vector<std::string> m_message; //!< Message attached to each object.
+ size_t m_nb_objects; //!< Number of detected objects.
public:
/*!
@@ -73,7 +79,7 @@ public:
/*!
Default destructor.
*/
- virtual ~vpDetectorBase() {};
+ virtual ~vpDetectorBase() {}
/*!
Detect objects in an image.
@@ -81,43 +87,46 @@ public:
\return true if one or multiple objects are detected, false otherwise.
*/
virtual bool detect(const vpImage<unsigned char> &I) = 0;
+
+ /** @name Inherited functionalities from vpDetectorBase */
+ //@{
+
/*!
- Returns object container box as a vector of points.
+ Return the bounding box of the ith object.
*/
- std::vector< std::vector<vpImagePoint> > & getPolygon()
- {
- return m_polygon;
- }
+ vpRect getBBox(size_t i) const;
/*!
- Returns ith object container box as a vector of points.
+ Return the center of gravity location of the ith object.
*/
- std::vector<vpImagePoint> & getPolygon(size_t i);
-
+ vpImagePoint getCog(size_t i) const;
+
/*!
Returns the contained message of the ith object if there is one.
*/
- std::string & getMessage(size_t i);
-
+ std::vector<std::string> &getMessage() { return m_message; }
+
/*!
Returns the contained message of the ith object if there is one.
*/
- std::vector< std::string > & getMessage() { return m_message; }
-
+ std::string &getMessage(size_t i);
+
/*!
Return the number of objects that are detected.
*/
- size_t getNbObjects() const {return m_nb_objects; }
-
+ size_t getNbObjects() const { return m_nb_objects; }
+
/*!
- Return the center of gravity location of the ith object.
+ Returns object container box as a vector of points.
*/
- vpImagePoint getCog(size_t i) const ;
-
+ std::vector<std::vector<vpImagePoint> > &getPolygon() { return m_polygon; }
+
/*!
- Return the bounding box of the ith object.
+ Returns ith object container box as a vector of points.
*/
- vpRect getBBox(size_t i) const ;
+ std::vector<vpImagePoint> &getPolygon(size_t i);
+
+ //@}
};
#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h
index 5e89820..b861b25 100644
--- a/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h
+++ b/modules/detection/include/visp3/detection/vpDetectorDataMatrixCode.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,31 +39,32 @@
#ifndef __vpDetectorDataMatrixCode_h__
#define __vpDetectorDataMatrixCode_h__
-#include <vector>
-#include <utility>
#include <string>
+#include <utility>
+#include <vector>
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_DMTX
-#include <visp3/detection/vpDetectorBase.h>
#include <visp3/core/vpImage.h>
+#include <visp3/detection/vpDetectorBase.h>
/*!
\class vpDetectorDataMatrixCode
\ingroup group_detection_barcode
Base class for bar code detector. This class is a wrapper over libdmtx
- available from http://www.libdmtx.org. Installation instructions are provided
- here https://visp.inria.fr/3rd_dmtx.
+ available from http://www.libdmtx.org. Installation instructions are
+provided here https://visp.inria.fr/3rd_dmtx.
- The detect() function allows to detect multiple QR codes in an image. Once detected,
- for each QR code it is possible to retrieve the location of the corners using getPolygon(),
- the encoded message using getMessage(), the bounding box using getBBox() and the center
- of gravity using getCog().
+ The detect() function allows to detect multiple QR codes in an image. Once
+detected, for each QR code it is possible to retrieve the location of the
+corners using getPolygon(), the encoded message using getMessage(), the
+bounding box using getBBox() and the center of gravity using getCog().
- The following sample code shows how to use this class to detect QR codes in an image.
- \code
+ The following sample code shows how to use this class to detect QR codes in
+an image.
+\code
#include <visp3/detection/vpDetectorDataMatrixCode.h>
#include <visp3/io/vpImageIo.h>
@@ -112,7 +114,7 @@ class VISP_EXPORT vpDetectorDataMatrixCode : public vpDetectorBase
{
public:
vpDetectorDataMatrixCode();
- virtual ~vpDetectorDataMatrixCode() {};
+ virtual ~vpDetectorDataMatrixCode(){};
bool detect(const vpImage<unsigned char> &I);
};
diff --git a/modules/detection/include/visp3/detection/vpDetectorFace.h b/modules/detection/include/visp3/detection/vpDetectorFace.h
index fb3f2da..4db185c 100644
--- a/modules/detection/include/visp3/detection/vpDetectorFace.h
+++ b/modules/detection/include/visp3/detection/vpDetectorFace.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,24 +43,27 @@
#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
-#include <algorithm> // needed by std::min in opencv2/objdetect/objdetect.hpp
+#include <algorithm> // needed by (std::min) in opencv2/objdetect/objdetect.hpp
-#include <opencv2/objdetect/objdetect.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/objdetect/objdetect.hpp>
#include <visp3/detection/vpDetectorBase.h>
/*!
\class vpDetectorFace
\ingroup group_detection_face
- The vpDetectorFace class is a wrapper over OpenCV Haar cascade face detection capabilities.
- To use this class ViSP should be build against OpenCV 2.2.0 or a more recent version. Installation
- instructions are provided here https://visp.inria.fr/3rd_opencv.
+ The vpDetectorFace class is a wrapper over OpenCV Haar cascade face
+detection capabilities. To use this class ViSP should be build against
+OpenCV 2.2.0 or a more recent version. Installation instructions are provided
+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
+ The following sample code shows how to use this class to detect the largest
+face in the image. The cascade classifier file
+"haarcascade_frontalface_alt.xml" can be found in ViSP source code or in
+OpenCV.
+\code
#include <visp3/detection/vpDetectorFace.h>
int main()
@@ -78,25 +82,26 @@ int main()
}
\endcode
- A more complete example that works with images acquired from a camera is provided in tutorial-face-detector-live.cpp.
+ A more complete example that works with images acquired from a camera is
+provided in tutorial-face-detector-live.cpp.
*/
class VISP_EXPORT vpDetectorFace : public vpDetectorBase
{
protected:
- std::vector<cv::Rect> m_faces; //!< Bounding box of each detected face.
+ std::vector<cv::Rect> m_faces; //!< Bounding box of each detected face.
cv::CascadeClassifier m_face_cascade; //!< Haar cascade classifier file name.
- cv::Mat m_frame_gray; //!< OpenCV image used as input for the face detection.
+ cv::Mat m_frame_gray; //!< OpenCV image used as input for the face detection.
public:
vpDetectorFace();
/*!
Default destructor.
*/
- virtual ~vpDetectorFace() {};
- void setCascadeClassifierFile(const std::string &filename);
+ virtual ~vpDetectorFace(){};
bool detect(const vpImage<unsigned char> &I);
bool detect(const cv::Mat &frame_gray);
+ void setCascadeClassifierFile(const std::string &filename);
};
#endif
diff --git a/modules/detection/include/visp3/detection/vpDetectorQRCode.h b/modules/detection/include/visp3/detection/vpDetectorQRCode.h
index 1be86b2..f045d8f 100644
--- a/modules/detection/include/visp3/detection/vpDetectorQRCode.h
+++ b/modules/detection/include/visp3/detection/vpDetectorQRCode.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,9 +39,9 @@
#ifndef __vpDetectorQRCode_h__
#define __vpDetectorQRCode_h__
-#include <vector>
-#include <utility>
#include <string>
+#include <utility>
+#include <vector>
#include <visp3/core/vpConfig.h>
@@ -48,23 +49,24 @@
#include <zbar.h>
-#include <visp3/detection/vpDetectorBase.h>
#include <visp3/core/vpImage.h>
+#include <visp3/detection/vpDetectorBase.h>
/*!
\class vpDetectorQRCode
\ingroup group_detection_barcode
Base class for bar code detector. This class is a wrapper over libzbar
- available from http://zbar.sourceforge.net. Installation instructions are provided
- here https://visp.inria.fr/3rd_zbar.
+ available from http://zbar.sourceforge.net. Installation instructions are
+provided here https://visp.inria.fr/3rd_zbar.
- The detect() function allows to detect multiple QR codes in an image. Once detected,
- for each QR code it is possible to retrieve the location of the corners using getPolygon(),
- the encoded message using getMessage(), the bounding box using getBBox() and the center
- of gravity using getCog().
+ The detect() function allows to detect multiple QR codes in an image. Once
+detected, for each QR code it is possible to retrieve the location of the
+corners using getPolygon(), the encoded message using getMessage(), the
+bounding box using getBBox() and the center of gravity using getCog().
- The following sample code shows how to use this class to detect QR codes in an image.
- \code
+ The following sample code shows how to use this class to detect QR codes in
+an image.
+\code
#include <visp3/detection/vpDetectorQRCode.h>
#include <visp3/io/vpImageIo.h>
@@ -116,7 +118,7 @@ protected:
public:
vpDetectorQRCode();
- virtual ~vpDetectorQRCode() {};
+ virtual ~vpDetectorQRCode(){};
bool detect(const vpImage<unsigned char> &I);
};
diff --git a/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp
index 8171f76..a4d1bff 100644
--- a/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp
+++ b/modules/detection/src/barcode/vpDetectorDataMatrixCode.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,12 +49,11 @@
/*!
Default constructor that does nothing.
*/
-vpDetectorDataMatrixCode::vpDetectorDataMatrixCode()
-{
-}
+vpDetectorDataMatrixCode::vpDetectorDataMatrixCode() {}
/*!
- Detect datamatrix bar codes in the image. Return true if a bar code is detected, false otherwise.
+ Detect datamatrix bar codes in the image. Return true if a bar code is
+ detected, false otherwise.
\param I : Input image.
*/
@@ -63,12 +63,12 @@ bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
m_message.clear();
m_polygon.clear();
m_nb_objects = 0;
- DmtxRegion *reg;
- DmtxDecode *dec;
- DmtxImage *img;
- DmtxMessage *msg;
+ DmtxRegion *reg;
+ DmtxDecode *dec;
+ 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);
@@ -78,9 +78,9 @@ bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
do {
reg = dmtxRegionFindNext(dec, 0);
- if(reg != NULL) {
+ if (reg != NULL) {
msg = dmtxDecodeMatrixRegion(dec, reg, DmtxUndefined);
- if(msg != NULL) {
+ if (msg != NULL) {
std::vector<vpImagePoint> polygon;
@@ -93,23 +93,21 @@ bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
dmtxMatrix3VMultiplyBy(&p11, reg->fit2raw);
dmtxMatrix3VMultiplyBy(&p01, reg->fit2raw);
- polygon.push_back(vpImagePoint(I.getHeight()-p00.Y, p00.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p10.Y, p10.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p11.Y, p11.X));
- polygon.push_back(vpImagePoint(I.getHeight()-p01.Y, p01.X));
+ polygon.push_back(vpImagePoint(I.getHeight() - p00.Y, p00.X));
+ polygon.push_back(vpImagePoint(I.getHeight() - p10.Y, p10.X));
+ polygon.push_back(vpImagePoint(I.getHeight() - p11.Y, p11.X));
+ polygon.push_back(vpImagePoint(I.getHeight() - p01.Y, p01.X));
m_polygon.push_back(polygon);
detected = true;
- m_message.push_back( (const char *)msg->output);
+ m_message.push_back((const char *)msg->output);
- m_nb_objects ++;
- }
- else {
+ m_nb_objects++;
+ } else {
end = true;
}
dmtxMessageDestroy(&msg);
- }
- else {
+ } else {
end = true;
}
dmtxRegionDestroy(®);
@@ -122,6 +120,7 @@ bool vpDetectorDataMatrixCode::detect(const vpImage<unsigned char> &I)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDetectorDataMatrixCode.cpp.o) has no symbols
-void dummy_vpDetectorDataMatrixCode() {};
+// Work arround to avoid warning:
+// libvisp_core.a(vpDetectorDataMatrixCode.cpp.o) has no symbols
+void dummy_vpDetectorDataMatrixCode(){};
#endif
diff --git a/modules/detection/src/barcode/vpDetectorQRCode.cpp b/modules/detection/src/barcode/vpDetectorQRCode.cpp
index 6fbed3f..a358fe4 100644
--- a/modules/detection/src/barcode/vpDetectorQRCode.cpp
+++ b/modules/detection/src/barcode/vpDetectorQRCode.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
#include <visp3/detection/vpDetectorQRCode.h>
-
/*!
Default constructor.
*/
@@ -52,7 +52,8 @@ vpDetectorQRCode::vpDetectorQRCode() : m_scanner()
}
/*!
- Detect QR codes in the image. Return true if a code is detected, false otherwise.
+ Detect QR codes in the image. Return true if a code is detected, false
+ otherwise.
\param I : Input image.
*/
@@ -71,17 +72,15 @@ bool vpDetectorQRCode::detect(const vpImage<unsigned char> &I)
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);
+ m_nb_objects = (size_t)m_scanner.scan(img);
// extract results
- for(zbar::Image::SymbolIterator symbol = img.symbol_begin();
- symbol != img.symbol_end();
- ++symbol) {
- m_message.push_back( symbol->get_data() );
+ for (zbar::Image::SymbolIterator symbol = img.symbol_begin(); symbol != img.symbol_end(); ++symbol) {
+ m_message.push_back(symbol->get_data());
detected = true;
std::vector<vpImagePoint> polygon;
- for(unsigned int i=0; i < (unsigned int)symbol->get_location_size(); i++){
+ 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);
@@ -93,6 +92,7 @@ bool vpDetectorQRCode::detect(const vpImage<unsigned char> &I)
return detected;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDetectorQRCode.cpp.o) has no symbols
-void dummy_vpDetectorQRCode() {};
+// Work arround to avoid warning: libvisp_core.a(vpDetectorQRCode.cpp.o) has
+// no symbols
+void dummy_vpDetectorQRCode(){};
#endif
diff --git a/modules/detection/src/face/vpDetectorFace.cpp b/modules/detection/src/face/vpDetectorFace.cpp
index d6aee1d..372db7e 100644
--- a/modules/detection/src/face/vpDetectorFace.cpp
+++ b/modules/detection/src/face/vpDetectorFace.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,43 +41,41 @@
#include <algorithm>
-#include <visp3/detection/vpDetectorFace.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/detection/vpDetectorFace.h>
-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.
- \param filename : Full path to access to the file. Such a file can be found in OpenCV. Within the last versions
- it was name "haarcascade_frontalface_alt.xml".
+ \param filename : Full path to access to the file. Such a file can be found
+ in OpenCV. Within the last versions it was name
+ "haarcascade_frontalface_alt.xml".
*/
void vpDetectorFace::setCascadeClassifierFile(const std::string &filename)
{
- if( ! m_face_cascade.load( filename ) ) {
+ if (!m_face_cascade.load(filename)) {
throw vpException(vpException::ioError, "Cannot read haar file: %s", filename.c_str());
}
}
/*!
- Allows to detect a face in the image. When more than one face is detected, faces are sorted from largest to smallest.
+ Allows to detect a face in the image. When more than one face is detected,
+ faces are sorted from largest to smallest.
- \param I : Input image to process. This ViSP image is converted internally in an OpenCV cv::Mat image.
- If you original image is an gray level OpenCV image, we suggest rather the use of detect(const cv::Mat &).
- \return true if one or more faces are found, false otherwise.
+ \param I : Input image to process. This ViSP image is converted internally
+ in an OpenCV cv::Mat image. If you original image is an gray level OpenCV
+ image, we suggest rather the use of detect(const cv::Mat &). \return true
+ if one or more faces are found, false otherwise.
The number of detected faces is returned using getNbObjects().
- If a face is found the functions getBBox(), getCog() return some information about the location of the face.
+ If a face is found the functions getBBox(), getCog() return some
+ information about the location of the face.
The largest face is always available using getBBox(0) or getCog(0).
@@ -89,13 +88,15 @@ bool vpDetectorFace::detect(const vpImage<unsigned char> &I)
return detect(m_frame_gray);
}
/*!
- Allows to detect a face in the image. When more than one face is detected, faces are sorted from largest to smallest.
+ Allows to detect a face in the image. When more than one face is detected,
+ faces are sorted from largest to smallest.
\param frame_gray : Input gray level image to process.
\return true if one or more faces are found, false otherwise.
The number of detected faces is returned using getNbObjects().
- If a face is found the functions getBBox(), getCog() return some information about the location of the face.
+ If a face is found the functions getBBox(), getCog() return some
+ information about the location of the face.
The largest face is always available using getBBox(0) or getCog(0).
*/
@@ -108,9 +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();
@@ -118,31 +119,32 @@ bool vpDetectorFace::detect(const cv::Mat &frame_gray)
std::sort(m_faces.begin(), m_faces.end(), vpSortLargestFace);
if (m_faces.size())
- for( size_t i = 0; i < m_faces.size(); i++ ) {
- std::ostringstream message;
- message << "Face " << i;
- m_message.push_back( message.str() );
+ for (size_t i = 0; i < m_faces.size(); i++) {
+ std::ostringstream message;
+ message << "Face " << i;
+ m_message.push_back(message.str());
- detected = true;
+ detected = true;
- std::vector<vpImagePoint> polygon;
- double x = m_faces[i].tl().x;
- double y = m_faces[i].tl().y;
- double w = m_faces[i].size().width;
- double h = m_faces[i].size().height;
+ std::vector<vpImagePoint> polygon;
+ double x = m_faces[i].tl().x;
+ double y = m_faces[i].tl().y;
+ double w = m_faces[i].size().width;
+ double h = m_faces[i].size().height;
- polygon.push_back(vpImagePoint(y , x ));
- polygon.push_back(vpImagePoint(y+h, x ));
- polygon.push_back(vpImagePoint(y+h, x+w));
- polygon.push_back(vpImagePoint(y , x+w));
+ polygon.push_back(vpImagePoint(y, x));
+ polygon.push_back(vpImagePoint(y + h, x));
+ polygon.push_back(vpImagePoint(y + h, x + w));
+ polygon.push_back(vpImagePoint(y, x + w));
- m_polygon.push_back(polygon);
- }
+ m_polygon.push_back(polygon);
+ }
return detected;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDetectorFace.cpp.o) has no symbols
-void dummy_vpDetectorFace() {};
+// Work arround to avoid warning: libvisp_core.a(vpDetectorFace.cpp.o) has no
+// symbols
+void dummy_vpDetectorFace(){};
#endif
diff --git a/modules/detection/src/tag/vpDetectorAprilTag.cpp b/modules/detection/src/tag/vpDetectorAprilTag.cpp
new file mode 100644
index 0000000..9c7b1d3
--- /dev/null
+++ b/modules/detection/src/tag/vpDetectorAprilTag.cpp
@@ -0,0 +1,463 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Base class for April Tag detection.
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_APRILTAG
+#include <map>
+
+#include <apriltag.h>
+#include <common/homography.h>
+#include <tag16h5.h>
+#include <tag25h7.h>
+#include <tag25h9.h>
+#include <tag36artoolkit.h>
+#include <tag36h10.h>
+#include <tag36h11.h>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/detection/vpDetectorAprilTag.h>
+#include <visp3/vision/vpPose.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+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)
+ {
+ switch (m_tagFamily) {
+ case TAG_36h11:
+ m_tf = tag36h11_create();
+ break;
+
+ case TAG_36h10:
+ m_tf = tag36h10_create();
+ break;
+
+ case TAG_36ARTOOLKIT:
+ m_tf = tag36artoolkit_create();
+ break;
+
+ case TAG_25h9:
+ m_tf = tag25h9_create();
+ break;
+
+ case TAG_25h7:
+ m_tf = tag25h7_create();
+ break;
+
+ case TAG_16h5:
+ m_tf = tag16h5_create();
+ break;
+
+ default:
+ throw vpException(vpException::fatalError, "Unknow Tag family!");
+ break;
+ }
+
+ m_td = apriltag_detector_create();
+ apriltag_detector_add_family(m_td, m_tf);
+
+ m_mapOfCorrespondingPoseMethods[DEMENTHON_VIRTUAL_VS] = vpPose::DEMENTHON;
+ m_mapOfCorrespondingPoseMethods[LAGRANGE_VIRTUAL_VS] = vpPose::LAGRANGE;
+ }
+
+ ~Impl()
+ {
+ apriltag_detector_destroy(m_td);
+
+ switch (m_tagFamily) {
+ case TAG_36h11:
+ tag36h11_destroy(m_tf);
+ break;
+
+ case TAG_36h10:
+ tag36h10_destroy(m_tf);
+ break;
+
+ case TAG_36ARTOOLKIT:
+ tag36artoolkit_destroy(m_tf);
+ break;
+
+ case TAG_25h9:
+ tag25h9_destroy(m_tf);
+ break;
+
+ case TAG_25h7:
+ tag25h7_destroy(m_tf);
+ break;
+
+ case TAG_16h5:
+ tag16h5_destroy(m_tf);
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ bool detect(const vpImage<unsigned char> &I, std::vector<std::vector<vpImagePoint> > &polygons,
+ std::vector<std::string> &messages, const bool computePose, const bool displayTag)
+ {
+ m_tagPoses.clear();
+
+ image_u8_t im = {/*.width =*/(int32_t)I.getWidth(),
+ /*.height =*/(int32_t)I.getHeight(),
+ /*.stride =*/(int32_t)I.getWidth(),
+ /*.buf =*/I.bitmap};
+
+ zarray_t *detections = apriltag_detector_detect(m_td, &im);
+ int nb_detections = zarray_size(detections);
+ bool detected = nb_detections > 0;
+
+ polygons.resize((size_t)nb_detections);
+ messages.resize((size_t)nb_detections);
+
+ for (int i = 0; i < zarray_size(detections); i++) {
+ apriltag_detection_t *det;
+ zarray_get(detections, i, &det);
+
+ std::vector<vpImagePoint> polygon;
+ for (int j = 0; j < 4; j++) {
+ polygon.push_back(vpImagePoint(det->p[j][1], det->p[j][0]));
+ }
+ polygons[i] = polygon;
+ std::stringstream ss;
+ ss << m_tagFamily << " id: " << det->id;
+ 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],
+ 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],
+ 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],
+ 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],
+ vpColor::yellow, 2);
+ }
+
+ if (computePose) {
+ vpHomogeneousMatrix cMo;
+ 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);
+
+ for (int i = 0; i < 3; i++) {
+ for (int j = 0; j < 3; j++) {
+ cMo[i][j] = MATD_EL(M, i, j);
+ }
+ cMo[i][3] = MATD_EL(M, i, 3);
+ }
+
+ matd_destroy(M);
+ }
+
+ // Add marker object points
+ vpPoint pt;
+ vpImagePoint imPt;
+ double x = 0.0, y = 0.0;
+ std::vector<vpPoint> pts(4);
+ pt.setWorldCoordinates(-m_tagSize / 2.0, -m_tagSize / 2.0, 0.0);
+ imPt.set_uv(det->p[0][0], det->p[0][1]);
+ vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+ pts[0] = pt;
+
+ pt.setWorldCoordinates(m_tagSize / 2.0, -m_tagSize / 2.0, 0.0);
+ imPt.set_uv(det->p[1][0], det->p[1][1]);
+ vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+ pts[1] = pt;
+
+ pt.setWorldCoordinates(m_tagSize / 2.0, m_tagSize / 2.0, 0.0);
+ imPt.set_uv(det->p[2][0], det->p[2][1]);
+ vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+ pts[2] = pt;
+
+ pt.setWorldCoordinates(-m_tagSize / 2.0, m_tagSize / 2.0, 0.0);
+ imPt.set_uv(det->p[3][0], det->p[3][1]);
+ vpPixelMeterConversion::convertPoint(m_cam, imPt, x, y);
+ pt.set_x(x);
+ pt.set_y(y);
+ pts[3] = pt;
+
+ vpPose pose;
+ pose.addPoints(pts);
+
+ if (m_poseEstimationMethod != HOMOGRAPHY_VIRTUAL_VS) {
+ if (m_poseEstimationMethod == BEST_RESIDUAL_VIRTUAL_VS) {
+ vpHomogeneousMatrix cMo_dementhon, cMo_lagrange, cMo_homography = cMo;
+
+ double residual_dementhon = std::numeric_limits<double>::max(),
+ residual_lagrange = std::numeric_limits<double>::max();
+ double residual_homography = pose.computeResidual(cMo_homography);
+
+ if (pose.computePose(vpPose::DEMENTHON, cMo_dementhon)) {
+ residual_dementhon = pose.computeResidual(cMo_dementhon);
+ }
+
+ if (pose.computePose(vpPose::LAGRANGE, cMo_lagrange)) {
+ residual_lagrange = pose.computeResidual(cMo_lagrange);
+ }
+
+ if (residual_dementhon < residual_lagrange) {
+ if (residual_dementhon < residual_homography) {
+ cMo = cMo_dementhon;
+ } else {
+ cMo = cMo_homography;
+ }
+ } else if (residual_lagrange < residual_homography) {
+ cMo = cMo_lagrange;
+ } else {
+ // cMo = cMo_homography; //already the case
+ }
+ } else {
+ pose.computePose(m_mapOfCorrespondingPoseMethods[m_poseEstimationMethod], cMo);
+ }
+ }
+
+ // Compute final pose using VVS
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+ m_tagPoses.push_back(cMo);
+ }
+ }
+
+ apriltag_detections_destroy(detections);
+
+ return detected;
+ }
+
+ void getTagPoses(std::vector<vpHomogeneousMatrix> &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 setQuadSigma(const float quadSigma) { m_td->quad_sigma = quadSigma; }
+
+ 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 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; }
+
+protected:
+ vpCameraParameters m_cam;
+ std::map<vpPoseEstimationMethod, vpPose::vpPoseMethodType> m_mapOfCorrespondingPoseMethods;
+ vpPoseEstimationMethod m_poseEstimationMethod;
+ vpAprilTagFamily m_tagFamily;
+ std::vector<vpHomogeneousMatrix> m_tagPoses;
+ double m_tagSize;
+ apriltag_detector_t *m_td;
+ apriltag_family_t *m_tf;
+};
+#endif // DOXYGEN_SHOULD_SKIP_THIS
+
+/*!
+ 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))
+{
+}
+
+/*!
+ Destructor that desallocate memory.
+*/
+vpDetectorAprilTag::~vpDetectorAprilTag() { delete m_impl; }
+
+/*!
+ Detect AprilTag tags in the image. Return true if at least one tag is
+ detected, false otherwise.
+
+ \param I : Input image.
+ \return true if at least one tag is detected.
+*/
+bool vpDetectorAprilTag::detect(const vpImage<unsigned char> &I)
+{
+ m_message.clear();
+ m_polygon.clear();
+ m_nb_objects = 0;
+
+ bool detected = m_impl->detect(I, m_polygon, m_message, false, m_displayTag);
+ m_nb_objects = m_message.size();
+
+ return detected;
+}
+
+/*!
+ Detect AprilTag tags in the image and compute the corresponding tag poses.
+
+ \param I : Input image.
+ \param tagSize : Tag size in meter corresponding to the external width of
+ 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<unsigned char> &I, const double tagSize, const vpCameraParameters &cam,
+ std::vector<vpHomogeneousMatrix> &cMo_vec)
+{
+ m_message.clear();
+ m_polygon.clear();
+ m_nb_objects = 0;
+
+ m_impl->setTagSize(tagSize);
+ m_impl->setCameraParameters(cam);
+ bool detected = m_impl->detect(I, m_polygon, m_message, true, m_displayTag);
+ m_nb_objects = m_message.size();
+ m_impl->getTagPoses(cMo_vec);
+
+ return detected;
+}
+
+/*!
+ Set the number of threads for April Tag detection (default is 1).
+
+ \param nThreads : Number of thread.
+*/
+void vpDetectorAprilTag::setAprilTagNbThreads(const int nThreads)
+{
+ if (nThreads > 0)
+ m_impl->setNbThreads(nThreads);
+}
+
+/*!
+ Set the method to use to compute the pose, \see vpPoseEstimationMethod
+
+ \param poseEstimationMethod : The method to used.
+*/
+void vpDetectorAprilTag::setAprilTagPoseEstimationMethod(const vpPoseEstimationMethod &poseEstimationMethod)
+{
+ m_poseEstimationMethod = poseEstimationMethod;
+ m_impl->setPoseEstimationMethod(poseEstimationMethod);
+}
+
+/*!
+ From the AprilTag code:
+ <blockquote>
+ detection of quads can be done on a lower-resolution image,
+ improving speed at a cost of pose accuracy and a slight
+ decrease in detection rate. Decoding the binary payload is
+ still done at full resolution.
+ </blockquote>
+ Default is 1.0, increase this value to reduce the computation time.
+
+ \param quadDecimate : Value for quad_decimate.
+*/
+void vpDetectorAprilTag::setAprilTagQuadDecimate(const float quadDecimate) { m_impl->setQuadDecimate(quadDecimate); }
+
+/*!
+ From the AprilTag code:
+ <blockquote>
+ What Gaussian blur should be applied to the segmented image
+ (used for quad detection?) Parameter is the standard deviation
+ in pixels. Very noisy images benefit from non-zero values
+ (e.g. 0.8).
+ </blockquote>
+ Default is 0.0.
+
+ \param quadSigma : Value for quad_sigma.
+*/
+void vpDetectorAprilTag::setAprilTagQuadSigma(const float quadSigma) { m_impl->setQuadSigma(quadSigma); }
+
+/*!
+ From the AprilTag code:
+ <blockquote>
+ when non-zero, detections are refined in a way intended to
+ increase the number of detected tags. Especially effective for
+ very small tags near the resolution threshold (e.g. 10px on a
+ side).
+ </blockquote>
+ Default is 0.
+
+ \param refineDecode : If true, set refine_decode to 1.
+*/
+void vpDetectorAprilTag::setAprilTagRefineDecode(const bool refineDecode) { m_impl->setRefineDecode(refineDecode); }
+
+/*!
+ From the AprilTag code:
+ <blockquote>
+ When non-zero, the edges of the each quad are adjusted to "snap
+ to" strong gradients nearby. This is useful when decimation is
+ employed, as it can increase the quality of the initial quad
+ estimate substantially. Generally recommended to be on (1).
+ Very computationally inexpensive. Option is ignored if
+ quad_decimate = 1.
+ </blockquote>
+ Default is 1.
+
+ \param refineEdges : If true, set refine_edges to 1.
+*/
+void vpDetectorAprilTag::setAprilTagRefineEdges(const bool refineEdges) { m_impl->setRefineEdges(refineEdges); }
+
+/*!
+ From the AprilTag code:
+ <blockquote>
+ when non-zero, detections are refined in a way intended to
+ increase the accuracy of the extracted pose. This is done by
+ maximizing the contrast around the black and white border of
+ the tag. This generally increases the number of successfully
+ detected tags, though not as effectively (or quickly) as
+ refine_decode.
+ This option must be enabled in order for "goodness" to be
+ computed.
+ </blockquote>
+ Default is 0.
+
+ \param refinePose : If true, set refine_pose to 1.
+*/
+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
+void dummy_vpDetectorAprilTag() {}
+#endif
diff --git a/modules/detection/src/vpDetectorBase.cpp b/modules/detection/src/vpDetectorBase.cpp
index ae81ad1..8f9e6ad 100644
--- a/modules/detection/src/vpDetectorBase.cpp
+++ b/modules/detection/src/vpDetectorBase.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,70 +39,71 @@
#include <visp3/detection/vpDetectorBase.h>
- /*!
- Default constructor.
- */
-vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0)
-{}
+/*!
+ Default constructor.
+*/
+vpDetectorBase::vpDetectorBase() : m_polygon(), m_message(), m_nb_objects(0) {}
/*!
- Returns ith object container box as a vector of points.
+ Returns ith object container box as a vector of points.
*/
-std::vector<vpImagePoint> &
-vpDetectorBase::getPolygon(size_t i)
+std::vector<vpImagePoint> &vpDetectorBase::getPolygon(size_t i)
{
- if (i < m_polygon.size())
- return m_polygon[i];
- else
- throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i, m_polygon.size()));
+ if (i < m_polygon.size())
+ return m_polygon[i];
+ else
+ throw(vpException(vpException::badValue, "Bad index to retrieve object %d. Only %d objects are detected.", i,
+ m_polygon.size()));
}
/*!
- Returns the contained message of the ith object if there is one.
+ Returns the contained message of the ith object if there is one.
*/
-std::string &
-vpDetectorBase::getMessage(size_t i)
+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()));
+ if (i < m_polygon.size())
+ return m_message[i];
+ else
+ throw(vpException(vpException::badValue, "Bad index to retrieve object %d . Only %d objects are detected.", i,
+ m_polygon.size()));
}
/*!
- Return the center of gravity location of the ith object.
+ Return the center of gravity location of the ith object.
*/
-vpImagePoint
-vpDetectorBase::getCog(size_t i) const
+vpImagePoint vpDetectorBase::getCog(size_t i) const
{
- vpImagePoint cog(0, 0);
- for (size_t j = 0; j < m_polygon[i].size(); j++) {
- cog += m_polygon[i][j];
- }
- cog /= (double)m_polygon[i].size();
- return cog;
+ vpImagePoint cog(0, 0);
+ for (size_t j = 0; j < m_polygon[i].size(); j++) {
+ cog += m_polygon[i][j];
+ }
+ cog /= (double)m_polygon[i].size();
+ return cog;
}
/*!
Return the bounding box of the ith object.
*/
-vpRect
-vpDetectorBase::getBBox(size_t i) const
+vpRect vpDetectorBase::getBBox(size_t i) const
{
- assert(m_polygon[i].size() > 2);
+ assert(m_polygon[i].size() > 2);
- double left, right;
- double top, bottom;
- left = right = m_polygon[i][0].get_u();
- top = bottom = m_polygon[i][0].get_v();
- for (size_t j = 0; j < m_polygon[i].size(); j++) {
- double u = m_polygon[i][j].get_u();
- double v = m_polygon[i][j].get_v();
- if (u < left) left = u;
- if (u > right) right = u;
- if (v < top) top = v;
- if (v > bottom) bottom = v;
- }
- vpRect roi(vpImagePoint(top, left), vpImagePoint(bottom, right));
- return roi;
+ double left, right;
+ double top, bottom;
+ left = right = m_polygon[i][0].get_u();
+ top = bottom = m_polygon[i][0].get_v();
+ for (size_t j = 0; j < m_polygon[i].size(); j++) {
+ double u = m_polygon[i][j].get_u();
+ double v = m_polygon[i][j].get_v();
+ if (u < left)
+ left = u;
+ if (u > right)
+ right = u;
+ if (v < top)
+ top = v;
+ if (v > bottom)
+ bottom = v;
+ }
+ vpRect roi(vpImagePoint(top, left), vpImagePoint(bottom, right));
+ return roi;
}
diff --git a/modules/detection/test/testAprilTag.cpp b/modules/detection/test/testAprilTag.cpp
new file mode 100644
index 0000000..ceb66da
--- /dev/null
+++ b/modules/detection/test/testAprilTag.cpp
@@ -0,0 +1,404 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test AprilTag detection.
+ *
+ *****************************************************************************/
+/*!
+ \example testAprilTag.cpp
+
+ \brief Test AprilTag detection.
+*/
+
+#include <iostream>
+#include <map>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/detection/vpDetectorAprilTag.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+#if defined(VISP_HAVE_APRILTAG)
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:p:h"
+
+namespace
+{
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ */
+void usage(const char *name, const char *badparam, std::string ipath)
+{
+ fprintf(stdout, "\n\
+ Test AprilTag detection.\n\
+ \n\
+ SYNOPSIS\n\
+ %s [-c] [-d] [-i <input image path>] [-p <personal image path>]\n\
+ [-h]\n \
+ ", name);
+
+ fprintf(stdout, "\n\
+ OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"AprilTag/AprilTag.pgm image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+ \n\
+ -p <personal image path> \n\
+ Path to an image used to test image reading function.\n\
+ Example: -p /my_path_to/image.png\n\
+ \n\
+ -c \n\
+ Disable the mouse click. Useful to automaze the \n\
+ execution of this program without human intervention.\n\
+ \n\
+ -d \n\
+ Turn off the display.\n\
+ \n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param ppath : Personal image path.
+ \param click_allowed : Mouse click activation.
+ \param display : Display activation.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &ppath, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+struct TagGroundTruth {
+ std::string message;
+ std::vector<vpImagePoint> corners;
+
+ TagGroundTruth(const std::string &msg, const std::vector<vpImagePoint> &c) : message(msg), corners(c) {}
+
+ bool operator==(const TagGroundTruth &b) const
+ {
+ if (message != b.message || corners.size() != b.corners.size())
+ return false;
+
+ for (size_t i = 0; i < corners.size(); i++) {
+ // Allow 0.5 pixel of difference
+ if (!vpMath::equal(corners[i].get_u(), b.corners[i].get_u(), 0.5) ||
+ !vpMath::equal(corners[i].get_v(), b.corners[i].get_v(), 0.5)) {
+ return false;
+ }
+ }
+
+ return true;
+ }
+
+ bool operator!=(const TagGroundTruth &b) const { return !(*this == b); }
+};
+
+std::ostream &operator<<(std::ostream &os, TagGroundTruth &t)
+{
+ os << t.message << std::endl;
+ for (size_t i = 0; i < t.corners.size(); i++)
+ os << t.corners[i] << std::endl;
+
+ return os;
+}
+}
+
+int main(int argc, const char *argv[])
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_ppath;
+ std::string ipath;
+ std::string filename;
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_click_allowed, opt_display) == false) {
+ exit(EXIT_FAILURE);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ vpImage<unsigned char> I;
+ if (opt_ppath.empty()) {
+ filename = vpIoTools::createFilePath(ipath, "AprilTag/AprilTag.pgm");
+ } else {
+ filename = opt_ppath;
+ }
+
+ if (!vpIoTools::checkFilename(filename)) {
+ std::cerr << "Filename: " << filename << " does not exist." << std::endl;
+ return EXIT_SUCCESS;
+ }
+ vpImageIo::read(I, filename);
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d;
+#else
+ opt_display = false;
+#endif
+
+ 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<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagNbThreads(nThreads);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setDisplayTag(display_tag);
+
+ vpCameraParameters cam;
+ 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)
+ d.init(I, 0, 0, "AprilTag detection");
+#endif
+ vpDisplay::display(I);
+ }
+
+ std::vector<vpHomogeneousMatrix> cMo_vec;
+ dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
+
+ // Ground truth
+ std::map<std::string, TagGroundTruth> mapOfTagsGroundTruth;
+ bool use_detection_ground_truth = false;
+ {
+ 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;
+
+ 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) {
+ std::vector<vpImagePoint> 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)));
+ }
+ }
+ }
+
+ std::map<std::string, vpPoseVector> mapOfPosesGroundTruth;
+ bool use_pose_ground_truth = false;
+ {
+ 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;
+
+ 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)));
+ }
+ }
+ }
+
+ for (size_t i = 0; i < detector->getNbObjects(); i++) {
+ std::vector<vpImagePoint> p = detector->getPolygon(i);
+
+ if (use_detection_ground_truth) {
+ std::string message = detector->getMessage(i);
+ std::replace(message.begin(), message.end(), ' ', '_');
+ std::map<std::string, TagGroundTruth>::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;
+ return EXIT_FAILURE;
+ } else if (it->second != current) {
+ std::cerr << "Problem, current detection:\n" << current << "\nGround truth:\n" << it->second << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+
+ 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),
+ vpColor::red);
+ }
+ }
+
+ if (opt_display) {
+ vpDisplay::displayText(I, 20, 20, "Click to display tag poses", vpColor::red);
+ vpDisplay::flush(I);
+ if (opt_click_allowed)
+ vpDisplay::getClick(I);
+
+ vpDisplay::display(I);
+ }
+
+ 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);
+
+ 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<std::string, vpPoseVector>::iterator it = mapOfPosesGroundTruth.find(message);
+ if (it == mapOfPosesGroundTruth.end()) {
+ 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::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+ }
+ }
+
+ if (opt_display) {
+ vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::flush(I);
+ if (opt_click_allowed)
+ vpDisplay::getClick(I);
+ }
+
+ delete detector;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ std::cout << "\ntestAprilTag is ok." << std::endl;
+ return EXIT_SUCCESS;
+}
+#else
+int main()
+{
+ std::cout << "Need ViSP AprilTag." << std::endl;
+ return 0;
+}
+#endif
diff --git a/modules/gui/CMakeLists.txt b/modules/gui/CMakeLists.txt
index 7d797a3..416e5c7 100644
--- a/modules/gui/CMakeLists.txt
+++ b/modules/gui/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/gui/include/visp3/gui/vpD3DRenderer.h b/modules/gui/include/visp3/gui/vpD3DRenderer.h
index 7e27460..475b191 100644
--- a/modules/gui/include/visp3/gui/vpD3DRenderer.h
+++ b/modules/gui/include/visp3/gui/vpD3DRenderer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,16 +40,18 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_D3D9))
#ifndef VPD3DRENDERER_HH
#define VPD3DRENDERER_HH
-#include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <d3dx9.h>
-#include <visp3/gui/vpWin32Renderer.h>
#include <visp3/core/vpDisplayException.h>
-
+#include <visp3/gui/vpWin32Renderer.h>
+#include <windows.h>
#include <iostream>
@@ -62,99 +65,89 @@
class VISP_EXPORT vpD3DRenderer : public vpWin32Renderer
{
- IDirect3D9 * pD3D;
+ IDirect3D9 *pD3D;
- //The d3d device we will be working with.
- IDirect3DDevice9 * pd3dDevice;
+ // The d3d device we will be working with.
+ IDirect3DDevice9 *pd3dDevice;
- //Sprite used to render the texture.
- ID3DXSprite * pSprite;
+ // Sprite used to render the texture.
+ ID3DXSprite *pSprite;
- //The system memory texture :
- //The one we will be drawing on.
- IDirect3DTexture9 * pd3dText;
+ // The system memory texture :
+ // The one we will be drawing on.
+ IDirect3DTexture9 *pd3dText;
- //The video memory texture :
- //The one we will use for display.
- IDirect3DTexture9 * pd3dVideoText;
+ // The video memory texture :
+ // The one we will use for display.
+ IDirect3DTexture9 *pd3dVideoText;
- //The texture's width.
+ // The texture's width.
unsigned int textWidth;
- //The window's handle.
+ // The window's handle.
HWND hWnd;
- //Colors for overlay drawn with d3d directly.
+ // Colors for overlay drawn with d3d directly.
unsigned long colors[vpColor::id_unknown];
- //Colors for overlay drawn with GDI.
+ // Colors for overlay drawn with GDI.
COLORREF colorsGDI[vpColor::id_unknown];
- //Font used for text drawing.
+ // Font used for text drawing.
HFONT hFont;
public:
-
bool init(HWND hwnd, unsigned int width, unsigned int height);
bool render();
vpD3DRenderer();
virtual ~vpD3DRenderer();
- void setImg(const vpImage<vpRGBa>& im);
- void setImg(const vpImage<unsigned char>& im);
- void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
- void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+ void setImg(const vpImage<vpRGBa> &im);
+ void setImg(const vpImage<unsigned char> &im);
+ void setImgROI(const vpImage<vpRGBa> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ void setImgROI(const vpImage<unsigned char> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
void setPixel(const vpImagePoint &iP, const vpColor &color);
- void drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness, int style=PS_SOLID);
+ void 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, unsigned int thickness=1);
+ 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, unsigned int thickness=1);
+ void drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, unsigned int h,
+ unsigned int thickness = 1);
void getImage(vpImage<vpRGBa> &I);
-
-
private:
-
void initView(float, float);
/*!
Sub function for circle drawing.
Circle drawing is based on Bresenham 's circle algorithm.
*/
- void subDrawCircle(int i, int j,
- int x, int y,
- vpColor col, unsigned char* buf,
- unsigned int pitch, unsigned int maxX, unsigned int maxY);
-
- void convert(const vpImage<vpRGBa>& I, unsigned char * imBuffer, unsigned int pitch);
- void convert(const vpImage<unsigned char>& I, unsigned char * imBuffer, unsigned int pitch);
- void convertROI(const vpImage<vpRGBa>& I, unsigned char * imBuffer, unsigned int pitch,
- int i_min, int j_min, int i_max, int j_max);
- void convertROI(const vpImage<unsigned char>& 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<vpRGBa> &I, unsigned char *imBuffer, unsigned int pitch);
+ void convert(const vpImage<unsigned char> &I, unsigned char *imBuffer, unsigned int pitch);
+ void convertROI(const vpImage<vpRGBa> &I, unsigned char *imBuffer, unsigned int pitch, int i_min, int j_min,
+ int i_max, int j_max);
+ void convertROI(const vpImage<unsigned char> &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.
@@ -163,13 +156,12 @@ private:
\param x The x-coordinate of the pixel (in the locked rectangle base)
\param y The y-coordinate of the pixel (in the locked rectangle base)
\param color The color of the pixel.
- \param maxX The maximum x value (equals to the width of the locked rectangle).
- \param maxY The maximum y value (equals to the height of the locked rectangle).
+ \param maxX The maximum x value (equals to the width of the locked
+ rectangle). \param maxY The maximum y value (equals to the height of the
+ locked rectangle).
*/
- inline void setBufferPixel(unsigned char* buf, unsigned int pitch,
- int x, int y,
- const vpColor &color,
+ 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;
@@ -179,8 +171,8 @@ private:
c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
}
- if(x>=0 && y>=0 && x<= (int)maxX && y<= (int)maxY)
- *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
+ if (x >= 0 && y >= 0 && x <= (int)maxX && y <= (int)maxY)
+ *(unsigned long *)(buf + (y * pitch) + (x << 2)) = c; // colors[color];
}
/*!
Unsafe useful inline function to set a pixel in a texture buffer.
@@ -191,9 +183,7 @@ private:
\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)
@@ -202,11 +192,10 @@ private:
c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
}
- *(unsigned long*)(buf + (y*pitch) + (x<<2)) = c; //colors[color];
+ *(unsigned long *)(buf + (y * pitch) + (x << 2)) = c; // colors[color];
}
unsigned int supPowerOf2(unsigned int n);
-
};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayD3D.h b/modules/gui/include/visp3/gui/vpDisplayD3D.h
index 2408acb..4d37777 100644
--- a/modules/gui/include/visp3/gui/vpDisplayD3D.h
+++ b/modules/gui/include/visp3/gui/vpDisplayD3D.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,12 +39,11 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDisplay.h>
-#if ( defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_D3D9))
#ifndef VPDISPLAYD3D_HH
#define VPDISPLAYD3D_HH
-
#include <visp3/gui/vpDisplayWin32.h>
/*!
@@ -51,9 +51,9 @@
\ingroup group_gui_display
- \brief Display for windows using Direct3D 3rd party. Thus to enable this class
- Direct3D should be installed. Installation instructions are provided here
- https://visp.inria.fr/3rd_d3d/
+ \brief Display for windows using Direct3D 3rd party. Thus to enable this
+class Direct3D should be installed. Installation instructions are provided
+here https://visp.inria.fr/3rd_d3d/
Direct3D is part of the DirectX API available under Windows
operating systems.
@@ -63,8 +63,8 @@
The example below shows how to display an image with this video device.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -108,14 +108,15 @@ class VISP_EXPORT vpDisplayD3D : public vpDisplayWin32
{
public:
vpDisplayD3D();
- vpDisplayD3D(int winx, int winy, const std::string &title="");
+ vpDisplayD3D(int winx, int winy, const std::string &title = "");
vpDisplayD3D(vpImage<unsigned char> &I, vpScaleType type);
- vpDisplayD3D(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayD3D(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType type);
- vpDisplayD3D(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayD3D(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
virtual ~vpDisplayD3D();
-
};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayGDI.h b/modules/gui/include/visp3/gui/vpDisplayGDI.h
index 2a78a33..a469d19 100644
--- a/modules/gui/include/visp3/gui/vpDisplayGDI.h
+++ b/modules/gui/include/visp3/gui/vpDisplayGDI.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,7 +39,7 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDisplay.h>
-#if ( defined(VISP_HAVE_GDI) )
+#if (defined(VISP_HAVE_GDI))
#ifndef vpDisplayGDI_HH
#define vpDisplayGDI_HH
@@ -52,14 +53,14 @@
\brief Display for windows using GDI (available on any windows 32 platform).
- GDI stands for Graphics Device Interface and is a core component of Microsoft
- Windows operating systems used for displaying graphics in a window.
+ GDI stands for Graphics Device Interface and is a core component of
+Microsoft Windows operating systems used for displaying graphics in a window.
The example below shows how to display an image with this video device.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -129,15 +130,16 @@ class VISP_EXPORT vpDisplayGDI : public vpDisplayWin32
{
public:
vpDisplayGDI();
- vpDisplayGDI(int winx, int winy, const std::string &title="");
+ vpDisplayGDI(int winx, int winy, const std::string &title = "");
vpDisplayGDI(vpImage<unsigned char> &I, vpScaleType type);
- vpDisplayGDI(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayGDI(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
vpDisplayGDI(vpImage<vpRGBa> &I, vpScaleType type);
- vpDisplayGDI(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayGDI(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
virtual ~vpDisplayGDI();
};
#endif
#endif
-
diff --git a/modules/gui/include/visp3/gui/vpDisplayGTK.h b/modules/gui/include/visp3/gui/vpDisplayGTK.h
index 1590fe6..d7bec9b 100644
--- a/modules/gui/include/visp3/gui/vpDisplayGTK.h
+++ b/modules/gui/include/visp3/gui/vpDisplayGTK.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,14 +41,13 @@
#define vpDisplayGTK_h
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GTK) )
+#if (defined(VISP_HAVE_GTK))
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
-#include <gtk/gtk.h>
#include <gdk/gdkrgb.h>
-
+#include <gtk/gtk.h>
/*!
\file vpDisplayGTK.h
@@ -60,18 +60,18 @@
\ingroup group_gui_display
- \brief The vpDisplayGTK allows to display image using the GTK 3rd party library.
- Thus to enable this class GTK should be installed. Installation
+ \brief The vpDisplayGTK allows to display image using the GTK 3rd party
+library. Thus to enable this class GTK should be installed. Installation
instructions are provided here https://visp.inria.fr/3rd_gtk.
The example below shows how to display an image with this video device.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/io/vpImageIo.h>
-int main()
+int main()
{
#if defined(VISP_HAVE_GTK)
vpImage<unsigned char> I; // Grey level image
@@ -83,7 +83,7 @@ int main()
vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
#endif
- vpDisplayGTK d;
+ vpDisplayGTK d;
// Initialize the display with the image I. Display and image are
// now link together.
@@ -111,23 +111,23 @@ int main()
vpDisplay::flush(I);
// Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
+ std::cout << "Check keyboard events..." << std::endl;
char key[10];
bool ret;
for (int i=0; i< 200; i++) {
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
+ if (ret)
std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
vpTime::wait(40);
}
// Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
+ std::cout << "Wait for a keyboard event..." << std::endl;
ret = vpDisplay::getKeyboardEvent(I, key, true);
std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
+ if (ret)
std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
+
// Wait for a click in the display window
std::cout << "Wait for a button click..." << std::endl;
vpDisplay::getClick(I);
@@ -135,25 +135,24 @@ int main()
}
\endcode
*/
-class VISP_EXPORT vpDisplayGTK: public vpDisplay
+class VISP_EXPORT vpDisplayGTK : public vpDisplay
{
private:
//! true if GTK display is ready to use
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;
- GdkColormap *colormap;
+ GdkColor blue, red, yellow, green, cyan, orange, white, black, gdkcolor, lightBlue, darkBlue, lightRed, darkRed,
+ lightGreen, darkGreen, purple, lightGray, gray, darkGray;
+ GdkColormap *colormap;
GdkFont *font;
- guchar *vectgtk;
- GdkColor **col ;
- int ncol, nrow ;
+ guchar *vectgtk;
+ GdkColor **col;
+ int ncol, nrow;
typedef enum {
- id_black=0,
+ id_black = 0,
id_white,
id_lightGray,
id_gray,
@@ -175,68 +174,76 @@ private:
} vpColorIdentifier;
public:
- vpDisplayGTK() ;
- vpDisplayGTK(int winx, int winy, const std::string &title="");
+ vpDisplayGTK();
+ vpDisplayGTK(int winx, int winy, const std::string &title = "");
vpDisplayGTK(vpImage<unsigned char> &I, vpScaleType type);
- vpDisplayGTK(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayGTK(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
vpDisplayGTK(vpImage<vpRGBa> &I, vpScaleType type);
- vpDisplayGTK(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayGTK(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
- virtual ~vpDisplayGTK() ;
+ virtual ~vpDisplayGTK();
- void getImage(vpImage<vpRGBa> &I) ;
+ void getImage(vpImage<vpRGBa> &I);
unsigned int getScreenDepth();
unsigned int getScreenHeight();
void getScreenSize(unsigned int &width, unsigned int &height);
unsigned int getScreenWidth();
- void init(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="");
- void init(vpImage<vpRGBa> &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<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "");
+ void init(vpImage<vpRGBa> &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 );
- void setTitle(const std::string &title) ;
+ void setFont(const std::string &fontname);
+ void setTitle(const std::string &title);
void setWindowPosition(int winx, int winy);
- void clearDisplay(const vpColor &color=vpColor::white) ;
-
- void closeDisplay() ;
-
- void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color=vpColor::white, unsigned int w=4,unsigned int h=2, unsigned int thickness=1) ;
- void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color=vpColor::green) ;
-
- void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
-
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const unsigned char *I) ;
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
-
- void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
-
- void displayPoint(const vpImagePoint &ip, const vpColor &color, 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, bool fill = false, unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness=1) ;
-
- void flushDisplay() ;
- void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height) ;
-
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true) ;
- bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true) ;
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(std::string &key, bool blocking=true);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
-} ;
+ void clearDisplay(const vpColor &color = vpColor::white);
+
+ void closeDisplay();
+
+ void displayArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color = vpColor::white,
+ unsigned int w = 4, unsigned int h = 2, unsigned int thickness = 1);
+ void displayCharString(const vpImagePoint &ip, const char *text, const vpColor &color = vpColor::green);
+
+ void displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1);
+ void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1);
+ void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness = 1);
+
+ void displayImage(const vpImage<vpRGBa> &I);
+ void displayImage(const vpImage<unsigned char> &I);
+ void displayImage(const unsigned char *I);
+
+ void displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+
+ void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness = 1);
+
+ void displayPoint(const vpImagePoint &ip, const vpColor &color, 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,
+ bool fill = false, unsigned int thickness = 1);
+ void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, unsigned int thickness = 1);
+
+ void flushDisplay();
+ void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ bool getClick(bool blocking = true);
+ bool getClick(vpImagePoint &ip, bool blocking = true);
+ bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ bool getKeyboardEvent(bool blocking = true);
+ bool getKeyboardEvent(std::string &key, bool blocking = true);
+ bool getPointerMotionEvent(vpImagePoint &ip);
+ bool getPointerPosition(vpImagePoint &ip);
+};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h
index 3a9fd41..d31ce17 100644
--- a/modules/gui/include/visp3/gui/vpDisplayOpenCV.h
+++ b/modules/gui/include/visp3/gui/vpDisplayOpenCV.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,12 +48,12 @@
#include <visp3/core/vpImageConvert.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/core/core.hpp>
-# include <opencv2/highgui/highgui.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
#else
-# include <cv.h>
-# include <highgui.h>
-# include <cxcore.h>
+#include <cv.h>
+#include <cxcore.h>
+#include <highgui.h>
#endif
/*!
@@ -60,7 +61,6 @@
\brief Define the OpenCV console to display images.
*/
-
/*!
\class vpDisplayOpenCV
@@ -74,11 +74,11 @@
The example below shows how to display an image with this video device.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpImageIo.h>
-int main()
+int main()
{
#if defined(VISP_HAVE_OPENCV)
vpImage<unsigned char> I; // Grey level image
@@ -86,7 +86,7 @@ int main()
// Read an image in PGM P5 format
vpImageIo::read(I, "/local/soft/ViSP/ViSP-images/Klimt/Klimt.pgm");
- vpDisplayOpenCV d;
+ vpDisplayOpenCV d;
// Initialize the display with the image I. Display and image are
// now link together.
@@ -114,23 +114,23 @@ int main()
vpDisplay::flush(I);
// Get non blocking keyboard events
- std::cout << "Check keyboard events..." << std::endl;
+ std::cout << "Check keyboard events..." << std::endl;
char key[10];
bool ret;
for (int i=0; i< 200; i++) {
bool ret = vpDisplay::getKeyboardEvent(I, key, false);
- if (ret)
+ if (ret)
std::cout << "keyboard event: key: " << "\"" << key << "\"" << std::endl;
- vpTime::wait(40);
+ vpTime::wait(40);
}
// Get a blocking keyboard event
- std::cout << "Wait for a keyboard event..." << std::endl;
+ std::cout << "Wait for a keyboard event..." << std::endl;
ret = vpDisplay::getKeyboardEvent(I, key, true);
std::cout << "keyboard event: " << ret << std::endl;
- if (ret)
+ if (ret)
std::cout << "key: " << "\"" << key << "\"" << std::endl;
-
+
// Wait for a click in the display window
std::cout << "Wait for a button click..." << std::endl;
vpDisplay::getClick(I);
@@ -139,137 +139,148 @@ int main()
\endcode
*/
-class VISP_EXPORT vpDisplayOpenCV: public vpDisplay
+class VISP_EXPORT vpDisplayOpenCV : public vpDisplay
{
private:
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
//! true if OpenCV display is ready to use
- IplImage* m_background;
- CvScalar *col ;
+ IplImage *m_background;
+ CvScalar *col;
CvScalar cvcolor;
CvFont *font;
#else
cv::Mat m_background;
- cv::Scalar *col ;
+ cv::Scalar *col;
cv::Scalar cvcolor;
int font;
float fontScale;
#endif
static std::vector<std::string> m_listTitles;
- static unsigned int m_nbWindows ;
- int fontHeight;
+ static unsigned int m_nbWindows;
+ int fontHeight;
int x_move;
int y_move;
bool move;
- int x_lbuttondown ;
- int y_lbuttondown ;
+ int x_lbuttondown;
+ int y_lbuttondown;
bool lbuttondown;
- int x_mbuttondown ;
- int y_mbuttondown ;
+ int x_mbuttondown;
+ int y_mbuttondown;
bool mbuttondown;
- int x_rbuttondown ;
- int y_rbuttondown ;
+ int x_rbuttondown;
+ int y_rbuttondown;
bool rbuttondown;
- int x_lbuttonup ;
- int y_lbuttonup ;
+ int x_lbuttonup;
+ int y_lbuttonup;
bool lbuttonup;
- int x_mbuttonup ;
- int y_mbuttonup ;
+ int x_mbuttonup;
+ int y_mbuttonup;
bool mbuttonup;
- int x_rbuttonup ;
- int y_rbuttonup ;
+ int x_rbuttonup;
+ int y_rbuttonup;
bool rbuttonup;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpDisplayOpenCV(const vpDisplayOpenCV &)
-// : vpDisplay(),
-// #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
-// background(NULL), col(NULL), cvcolor(), font(NULL),
-// #else
-// background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN), fontScale(0.8f),
-// #endif
-// fontHeight(10), x_move(0), y_move(0) , move(false),
-// x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
-// x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
-// x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
-// x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
-// x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
-// x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpDisplayOpenCV &operator=(const vpDisplayOpenCV &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpDisplayOpenCV(const vpDisplayOpenCV &)
+ // : vpDisplay(),
+ // #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ // background(NULL), col(NULL), cvcolor(), font(NULL),
+ // #else
+ // background(), col(NULL), cvcolor(), font(cv::FONT_HERSHEY_PLAIN),
+ // fontScale(0.8f),
+ // #endif
+ // fontHeight(10), x_move(0), y_move(0) , move(false),
+ // x_lbuttondown(0), y_lbuttondown(0), lbuttondown(false),
+ // x_mbuttondown(0), y_mbuttondown(0), mbuttondown(false),
+ // x_rbuttondown(0), y_rbuttondown(0), rbuttondown(false),
+ // x_lbuttonup(0), y_lbuttonup(0), lbuttonup(false),
+ // x_mbuttonup(0), y_mbuttonup(0), mbuttonup(false),
+ // x_rbuttonup(0), y_rbuttonup(0), rbuttonup(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpDisplayOpenCV &operator=(const vpDisplayOpenCV &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpDisplayOpenCV() ;
- vpDisplayOpenCV(int winx, int winy, const std::string &title="") ;
+ vpDisplayOpenCV();
+ vpDisplayOpenCV(int winx, int winy, const std::string &title = "");
vpDisplayOpenCV(vpImage<unsigned char> &I, vpScaleType type);
- vpDisplayOpenCV(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayOpenCV(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
vpDisplayOpenCV(vpImage<vpRGBa> &I, vpScaleType type);
- vpDisplayOpenCV(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayOpenCV(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
- virtual ~vpDisplayOpenCV() ;
+ virtual ~vpDisplayOpenCV();
void getImage(vpImage<vpRGBa> &I);
unsigned int getScreenHeight();
void getScreenSize(unsigned int &width, unsigned int &height);
unsigned int getScreenWidth();
- void init(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="") ;
- void init(vpImage<vpRGBa> &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<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "");
+ void init(vpImage<vpRGBa> &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 );
- void setTitle(const std::string &title) ;
+ void setFont(const std::string &font);
+ void setTitle(const std::string &title);
void setWindowPosition(int winx, int winy);
- void clearDisplay(const vpColor &color=vpColor::white) ;
+ void clearDisplay(const vpColor &color = vpColor::white);
+
+ void closeDisplay();
- 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,
+ 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 displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false, unsigned int thickness=1);
- void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1) ;
- void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
+ void displayImage(const vpImage<unsigned char> &I);
+ void displayImage(const vpImage<vpRGBa> &I);
+ void displayImage(const unsigned char *I);
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const unsigned char *I) ;
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
- void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1) ;
- void displayPoint(const vpImagePoint &ip, const vpColor &color, 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, bool fill = false, unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false, 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,
+ 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 flushDisplay();
void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true) ;
- bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true) ;
+ bool getClick(bool blocking = true);
+ bool getClick(vpImagePoint &ip, bool blocking = true);
+ bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(std::string &key, bool blocking=true);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
+ bool getKeyboardEvent(bool blocking = true);
+ bool getKeyboardEvent(std::string &key, bool blocking = true);
+ bool getPointerMotionEvent(vpImagePoint &ip);
+ bool getPointerPosition(vpImagePoint &ip);
- static void on_mouse( int event, int x, int y, int flags, void* param );
-} ;
+ static void on_mouse(int event, int x, int y, int flags, void *param);
+};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpDisplayWin32.h b/modules/gui/include/visp3/gui/vpDisplayWin32.h
index 175f9a7..210f10d 100644
--- a/modules/gui/include/visp3/gui/vpDisplayWin32.h
+++ b/modules/gui/include/visp3/gui/vpDisplayWin32.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,29 +38,31 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
#ifndef vpDisplayWin32_hh
#define vpDisplayWin32_hh
#include <string>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <windows.h>
-#include <visp3/gui/vpWin32Window.h>
-#include <visp3/gui/vpWin32Renderer.h>
-#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImage.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/gui/vpWin32Window.h>
+#include <windows.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
Used to pass parameters to the window's thread.
*/
-struct threadParam
-{
+struct threadParam {
//! Pointer to the display associated with the window.
- vpDisplayWin32 * vpDisp;
+ vpDisplayWin32 *vpDisp;
//! X position of the window.
int x;
@@ -112,25 +115,26 @@ protected:
RECT roi;
//! Function used to launch the window in a thread.
- friend void vpCreateWindow(threadParam * param);
+ friend void vpCreateWindow(threadParam *param);
public:
+ explicit vpDisplayWin32(vpWin32Renderer *rend = NULL);
- vpDisplayWin32(vpWin32Renderer * rend = NULL);
+ vpDisplayWin32(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "");
- vpDisplayWin32(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="");
-
- vpDisplayWin32(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="");
+ vpDisplayWin32(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "");
virtual ~vpDisplayWin32();
- void clearDisplay(const vpColor &color=vpColor::white);
+ void clearDisplay(const vpColor &color = vpColor::white);
void closeDisplay();
void displayImage(const vpImage<vpRGBa> &I);
void displayImage(const vpImage<unsigned char> &I);
-
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+
+ void displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
void flushDisplay();
void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
@@ -140,74 +144,57 @@ public:
void getScreenSize(unsigned int &width, unsigned int &height);
unsigned int getScreenWidth();
- void init(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="");
- void init(vpImage<vpRGBa> &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) { window.setScale(scale); m_scale = scale; }
+ void init(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "");
+ void init(vpImage<vpRGBa> &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)
+ {
+ window.setScale(scale);
+ m_scale = scale;
+ }
+ 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,
- unsigned int thickness=1);
+ 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, bool fill = false,
- unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle,
- const vpColor &color, bool fill = false,
- 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,
+ 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(bool blocking = true);
- bool getClick(vpImagePoint &ip, 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 getKeyboardEvent( 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);
- bool getPointerPosition (vpImagePoint &ip);
+ bool getPointerMotionEvent(vpImagePoint &ip);
+ bool getPointerPosition(vpImagePoint &ip);
void waitForInit();
-
};
#endif
#endif
-
diff --git a/modules/gui/include/visp3/gui/vpDisplayX.h b/modules/gui/include/visp3/gui/vpDisplayX.h
index 286558b..eb118e7 100644
--- a/modules/gui/include/visp3/gui/vpDisplayX.h
+++ b/modules/gui/include/visp3/gui/vpDisplayX.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,7 +43,7 @@
#include <visp3/core/vpDisplay.h>
#ifdef VISP_HAVE_X11
-//namespace X11name
+// namespace X11name
//{
#include <X11/Xlib.h>
#include <X11/Xutil.h>
@@ -50,24 +51,21 @@
//#include <X11/cursorfont.h>
//} ;
-//using namespace X11name ;
+// using namespace X11name ;
// Work arround to use this class with Eigen3
#ifdef Success
-# undef Success // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253
+#undef Success // See http://eigen.tuxfamily.org/bz/show_bug.cgi?id=253
#endif
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRect.h>
-
-
/*!
\file vpDisplayX.h
\brief Define the X11 console to display images.
*/
-
/*!
\class vpDisplayX
@@ -78,17 +76,17 @@
instructions are provided here https://visp.inria.fr/3rd_x11.
This class define the X11 console to display images
- It also define method to display some geometric feature (point, line, circle)
- in the image.
+ It also define method to display some geometric feature (point, line,
+circle) in the image.
The example below shows how to display an image with this video device.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
-int main()
+int main()
{
vpImage<unsigned char> I; // Grey level image
@@ -150,117 +148,119 @@ int main()
*/
-class VISP_EXPORT vpDisplayX: public vpDisplay
+class VISP_EXPORT vpDisplayX : public vpDisplay
{
private:
- Display *display ;
- Window window ;
- XImage *Ximage ;
- Colormap lut ;
- GC context ;
- int screen ;
- XEvent event;
- Pixmap pixmap;
- unsigned long *x_color; // Array of predefined colors
- unsigned int screen_depth ;
- unsigned short colortable[256];
- XColor xcolor;
- XGCValues values;
+ Display *display;
+ Window window;
+ XImage *Ximage;
+ Colormap lut;
+ GC context;
+ int screen;
+ XEvent event;
+ Pixmap pixmap;
+ unsigned long *x_color; // Array of predefined colors
+ unsigned int screen_depth;
+ unsigned short colortable[256];
+ XColor xcolor;
+ XGCValues values;
bool ximage_data_init;
unsigned int RMask, GMask, BMask;
int RShift, GShift, BShift;
- //private:
+ // private:
//#ifndef DOXYGEN_SHOULD_SKIP_THIS
// vpDisplayX(const vpDisplayX &)
- // : vpDisplay(), display(NULL), window(), Ximage(NULL), lut(), context(), screen(), event(), pixmap(),
- // x_color(NULL), screen_depth(8), xcolor(), values(), ximage_data_init(false),
- // RMask(0), GMask(0), BMask(0), RShift(0), GShift(0), BShift(0)
+ // : vpDisplay(), display(NULL), window(), Ximage(NULL), lut(),
+ // context(), screen(), event(), pixmap(),
+ // x_color(NULL), screen_depth(8), xcolor(), values(),
+ // ximage_data_init(false), RMask(0), GMask(0), BMask(0), RShift(0),
+ // GShift(0), BShift(0)
// {
- // throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
// }
// vpDisplayX &operator=(const vpDisplayX &){
- // throw vpException(vpException::functionNotImplementedError, "Not implemented!");
- // return *this;
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
// }
//#endif
public:
- vpDisplayX() ;
- vpDisplayX(int winx, int winy, const std::string &title="") ;
+ vpDisplayX();
+ vpDisplayX(int winx, int winy, const std::string &title = "");
vpDisplayX(vpImage<unsigned char> &I, vpScaleType type);
- vpDisplayX(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayX(vpImage<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
vpDisplayX(vpImage<vpRGBa> &I, vpScaleType type);
- vpDisplayX(vpImage<vpRGBa> &I, int winx=-1, int winy=-1, const std::string &title="", vpScaleType type=SCALE_DEFAULT) ;
+ vpDisplayX(vpImage<vpRGBa> &I, int winx = -1, int winy = -1, const std::string &title = "",
+ vpScaleType type = SCALE_DEFAULT);
+
+ virtual ~vpDisplayX();
- virtual ~vpDisplayX() ;
-
- void getImage(vpImage<vpRGBa> &I) ;
+ void getImage(vpImage<vpRGBa> &I);
unsigned int getScreenDepth();
unsigned int getScreenHeight();
void getScreenSize(unsigned int &width, unsigned int &height);
unsigned int getScreenWidth();
- void init(vpImage<unsigned char> &I, int winx=-1, int winy=-1, const std::string &title="") ;
- void init(vpImage<vpRGBa> &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<unsigned char> &I, int winx = -1, int winy = -1, const std::string &title = "");
+ void init(vpImage<vpRGBa> &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 clearDisplay(const vpColor &color = vpColor::white);
- void closeDisplay() ;
+ 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, 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 displayCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill = false,
+ unsigned int thickness = 1);
+ void displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness = 1);
+ void displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness = 1);
- void displayImage(const vpImage<unsigned char> &I) ;
- void displayImage(const vpImage<vpRGBa> &I) ;
- void displayImage(const unsigned char *I) ;
+ void displayImage(const vpImage<unsigned char> &I);
+ void displayImage(const vpImage<vpRGBa> &I);
+ void displayImage(const unsigned char *I);
- void displayImageROI(const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- void displayImageROI(const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ void displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
- void displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness=1) ;
- void displayPoint(const vpImagePoint &ip, const vpColor &color, 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, bool fill = false, unsigned int thickness=1) ;
- void displayRectangle(const vpRect &rectangle, const vpColor &color, bool fill = false,
- 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,
+ 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 flushDisplay();
void flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height);
- bool getClick(bool blocking=true) ;
- bool getClick(vpImagePoint &ip, bool blocking=true);
- bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true) ;
- bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType& button, bool blocking=true);
+ bool getClick(bool blocking = true);
+ bool getClick(vpImagePoint &ip, bool blocking = true);
+ bool getClick(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
+ bool getClickUp(vpImagePoint &ip, vpMouseButton::vpMouseButtonType &button, bool blocking = true);
- bool getKeyboardEvent(bool blocking=true);
- bool getKeyboardEvent(std::string &key, bool blocking=true);
+ bool getKeyboardEvent(bool blocking = true);
+ bool getKeyboardEvent(std::string &key, bool blocking = true);
int getMsb(unsigned int u32val);
- bool getPointerMotionEvent (vpImagePoint &ip);
- bool getPointerPosition (vpImagePoint &ip);
+ bool getPointerMotionEvent(vpImagePoint &ip);
+ bool getPointerPosition(vpImagePoint &ip);
void setFont(const std::string &font);
- void setTitle(const std::string &title) ;
+ void setTitle(const std::string &title);
void setWindowPosition(int winx, int winy);
-} ;
+};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpGDIRenderer.h b/modules/gui/include/visp3/gui/vpGDIRenderer.h
index ab76261..923dd59 100644
--- a/modules/gui/include/visp3/gui/vpGDIRenderer.h
+++ b/modules/gui/include/visp3/gui/vpGDIRenderer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,37 +38,39 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) )
+#if (defined(VISP_HAVE_GDI))
#ifndef vpGDIRenderer_HH
#define vpGDIRenderer_HH
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <windows.h>
-#include <visp3/gui/vpWin32Renderer.h>
+#include <visp3/core/vpDisplayException.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpDisplayException.h>
+#include <visp3/gui/vpWin32Renderer.h>
#include <visp3/core/vpMath.h>
class VISP_EXPORT vpGDIRenderer : public vpWin32Renderer
{
- //the handle of the associated window
+ // the handle of the associated window
HWND m_hWnd;
- //the bitmap object to display
+ // the bitmap object to display
HBITMAP m_bmp;
- //colors for overlay
+ // colors for overlay
COLORREF m_colors[vpColor::id_unknown];
- //font used to draw text
+ // font used to draw text
HFONT m_hFont;
- //used to ensure that only one thread at a time is accessing bmp
+ // used to ensure that only one thread at a time is accessing bmp
CRITICAL_SECTION m_criticalSection;
unsigned int m_bmp_width;
@@ -82,59 +85,53 @@ public:
bool render();
- void setImg(const vpImage<vpRGBa>& I);
- void setImg(const vpImage<unsigned char>& I);
- void setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
- void setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height );
+ void setImg(const vpImage<vpRGBa> &I);
+ void setImg(const vpImage<unsigned char> &I);
+ void setImgROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ void setImgROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
void setPixel(const vpImagePoint &iP, const vpColor &color);
- void drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color, unsigned int thickness, int style=PS_SOLID);
+ void 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, unsigned int thickness=1);
+ 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, unsigned int thickness=1);
+ void drawArrow(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int w, unsigned int h,
+ unsigned int thickness = 1);
void getImage(vpImage<vpRGBa> &I);
private:
+ // updates the renderer hbitmaps.
+ bool updateBitmap(HBITMAP &hBmp, unsigned char *imBuffer, unsigned int w, unsigned int h);
+ // updates the renderer hbitmaps.
+ bool updateBitmapROI(unsigned char *imBuffer, int i_min, int j_min, int w, int h);
- //updates the renderer hbitmaps.
- bool updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
- unsigned int w, unsigned int h);
- //updates the renderer hbitmaps.
- bool updateBitmapROI(unsigned char * imBuffer, int i_min, int j_min, int w, int h);
-
- //converts a vpImage<vpRGBa> into a HBITMAP .
- void convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp);
-
- //converst a vpImage<unsigned char> into a HBITMAP .
- void convert(const vpImage<unsigned char> &I, HBITMAP& hBmp);
+ // converts a vpImage<vpRGBa> into a HBITMAP .
+ void convert(const vpImage<vpRGBa> &I, HBITMAP &hBmp);
- //converts a vpImage<vpRGBa> into a HBITMAP .
- void convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ // converst a vpImage<unsigned char> into a HBITMAP .
+ void convert(const vpImage<unsigned char> &I, HBITMAP &hBmp);
- //converst a vpImage<unsigned char> into a HBITMAP .
- void convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height);
+ // converts a vpImage<vpRGBa> into a HBITMAP .
+ void convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
+ // converst a vpImage<unsigned char> into a HBITMAP .
+ void convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height);
};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpPlot.h b/modules/gui/include/visp3/gui/vpPlot.h
index 1c2a9c5..4246209 100644
--- a/modules/gui/include/visp3/gui/vpPlot.h
+++ b/modules/gui/include/visp3/gui/vpPlot.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -55,12 +56,14 @@
instance of the class open a window which contains between 1 and 4
graphics. Each one contains a desired number of curves.
- \warning This class is only available if one of the display functionalities (X11, GDI, GTK, OpenCV or Direct3D)
- is available. In visp3/core/vpConfig.h header file, you should have VISP_HAVE_DISPLAY define.
+ \warning This class is only available if one of the display functionalities
+(X11, GDI, GTK, OpenCV or Direct3D) is available. In visp3/core/vpConfig.h
+header file, you should have VISP_HAVE_DISPLAY define.
- The example below shows how to use the vpPlot class. An other example provided in tutoral-ibvs-plotter.cpp
- and described in \ref tutorial-plotter shows how to use this class to plot in real-time some curves during
- an image-based visual servo.
+ The example below shows how to use the vpPlot class. An other example
+provided in tutoral-ibvs-plotter.cpp and described in \ref tutorial-plotter
+shows how to use this class to plot in real-time some curves during an
+image-based visual servo.
\code
#include <visp3/gui/vpPlot.h>
@@ -72,8 +75,7 @@ int main ()
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);
+ 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);
@@ -112,89 +114,91 @@ int main ()
class VISP_EXPORT vpPlot
{
- public:
- vpImage<unsigned char> I;
-
- private:
- vpDisplay *display;
-
- unsigned int graphNbr;
- vpPlotGraph* graphList;
-
- unsigned int margei;
- unsigned int margej;
-
- float factori;
- float factorj;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpPlot(const vpPlot &)
-// : I(), display(NULL), graphNbr(0), graphList(NULL), margei(0), margej(0),
-// factori(0), factorj(0)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpPlot &operator=(const vpPlot &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+public:
+ vpImage<unsigned char> I;
+
+private:
+ vpDisplay *display;
+
+ unsigned int graphNbr;
+ vpPlotGraph *graphList;
+
+ unsigned int margei;
+ unsigned int margej;
+
+ float factori;
+ float factorj;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpPlot(const vpPlot &)
+ // : I(), display(NULL), graphNbr(0), graphList(NULL), margei(0),
+ // margej(0),
+ // factori(0), factorj(0)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpPlot &operator=(const vpPlot &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpPlot();
- vpPlot(const unsigned int nbGraph,
- const unsigned int height=700,
- const unsigned int width=700,
- const int x=-1, const int y=-1, const 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 initGraph (unsigned int graphNum, unsigned int curveNbr);
-
- void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax);
- void initRange (const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax);
- void navigate (void);
-
- void plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y);
- void plot(const unsigned int graphNum, const double x, const vpColVector &v_y);
- void plot(const unsigned int graphNum, const double x, const vpRowVector &v_y);
- void plot(const unsigned int graphNum, const double x, const vpPoseVector &v_y);
- void plot(const unsigned int graphNum, const double x, const vpTranslationVector &v_y);
- void plot(const unsigned int graphNum, const double x, const vpRotationVector &v_y);
- vpMouseButton::vpMouseButtonType plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z);
- vpMouseButton::vpMouseButtonType plot(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z);
-
- void resetPointList (const unsigned int graphNum);
- void resetPointList (const unsigned int graphNum, const unsigned int curveNum);
-
- void saveData(const unsigned int graphNum, const 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.
-
- To know which font are available, on Unix you can use xfontsel or xlsfonts utilities.
- */
- void setFont(const std::string &font)
- {
- if (display->isInitialised())
- vpDisplay::setFont(I, font.c_str());
- }
- 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);
-
- private:
- void initNbGraph (unsigned int nbGraph);
- void displayGrid();
+ 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();
+ 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 initGraph(unsigned int graphNum, unsigned int curveNbr);
+
+ void initRange(const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax);
+ void initRange(const unsigned int graphNum, double xmin, double xmax, double ymin, double ymax, double zmin,
+ double zmax);
+ void navigate(void);
+
+ void plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y);
+ void plot(const unsigned int graphNum, const double x, const vpColVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpRowVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpPoseVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpTranslationVector &v_y);
+ void plot(const unsigned int graphNum, const double x, const vpRotationVector &v_y);
+ vpMouseButton::vpMouseButtonType plot(const unsigned int graphNum, const unsigned int curveNum, const double x,
+ const double y, const double z);
+ vpMouseButton::vpMouseButtonType plot(const unsigned int graphNum, const double x, const vpColVector &v_y,
+ const vpColVector &v_z);
+
+ void resetPointList(const unsigned int graphNum);
+ void resetPointList(const unsigned int graphNum, const unsigned int curveNum);
+
+ void saveData(const unsigned int graphNum, const 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.
+
+ To know which font are available, on Unix you can use xfontsel or xlsfonts
+ utilities.
+ */
+ void setFont(const std::string &font)
+ {
+ if (display->isInitialised())
+ vpDisplay::setFont(I, font.c_str());
+ }
+ 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);
+
+private:
+ void initNbGraph(unsigned int nbGraph);
+ void displayGrid();
};
#endif
diff --git a/modules/gui/include/visp3/gui/vpPlotCurve.h b/modules/gui/include/visp3/gui/vpPlotCurve.h
index a980537..f114a7f 100644
--- a/modules/gui/include/visp3/gui/vpPlotCurve.h
+++ b/modules/gui/include/visp3/gui/vpPlotCurve.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -53,40 +54,34 @@
#if defined(VISP_HAVE_DISPLAY)
-//! Different styles to plot the curve.
-typedef enum
-{
- point,
- line,
- dashed_line,
- marker
-} vpCurveStyle;
-
class vpPlotCurve
{
- public:
- vpColor color;
- vpCurveStyle curveStyle;
- unsigned int thickness ;
- //vpMarkerStyle markerStyle;
- //char lineStyle[20];
- //vpList<vpImagePoint> pointList;
- unsigned int nbPoint;
- vpImagePoint lastPoint;
- std::list<double> pointListx;
- std::list<double> pointListy;
- std::list<double> pointListz;
- std::string legend;
- double xmin;
- double xmax;
- double ymin;
- double ymax;
-
- public:
- vpPlotCurve();
- ~vpPlotCurve();
- void plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y);
- void plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy);
+public:
+ //! Different styles to plot the curve.
+ typedef enum { point, line, dashed_line, marker } vpCurveStyle;
+ vpColor color;
+ vpCurveStyle curveStyle;
+ unsigned int thickness;
+ // vpMarkerStyle markerStyle;
+ // char lineStyle[20];
+ // vpList<vpImagePoint> pointList;
+ unsigned int nbPoint;
+ vpImagePoint lastPoint;
+ std::list<double> pointListx;
+ std::list<double> pointListy;
+ std::list<double> pointListz;
+ std::string legend;
+ double xmin;
+ double xmax;
+ double ymin;
+ double ymax;
+
+public:
+ vpPlotCurve();
+ ~vpPlotCurve();
+ void plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y);
+ void plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx,
+ const double zoomy);
};
#endif
diff --git a/modules/gui/include/visp3/gui/vpPlotGraph.h b/modules/gui/include/visp3/gui/vpPlotGraph.h
index 87deada..da77c05 100644
--- a/modules/gui/include/visp3/gui/vpPlotGraph.h
+++ b/modules/gui/include/visp3/gui/vpPlotGraph.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,8 +44,8 @@
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpPlotCurve.h>
#include <visp3/core/vpMouseButton.h>
+#include <visp3/gui/vpPlotCurve.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRect.h>
@@ -56,162 +57,171 @@
class vpPlotGraph
{
- public:
- double xorg;
- double yorg;
- double zoomx;
- double zoomy;
- double xmax;
- double ymax;
- double xmin;
- double ymin;
- double xdelt;
- double ydelt;
- bool gridx;
- bool gridy;
- vpColor gridColor;
- std::string title;
- std::string unitx;
- std::string unity;
- unsigned int curveNbr;
- vpPlotCurve* curveList;
- bool scaleInitialized;
- bool firstPoint;
-
- int nbDivisionx;
- int nbDivisiony;
-
- //Graph complet
- vpImagePoint topLeft;
- unsigned int width;
- unsigned int height;
- vpRect graphZone;
-
- //Zone d'affichage
- vpImagePoint dTopLeft;
- unsigned int dWidth;
- unsigned int dHeight;
- vpRect dGraphZone;
-
- //Zone d'affichage
- vpImagePoint dTopLeft3D;
-// int dWidth;
-// int dHeight;
- vpRect dGraphZone3D;
-
- //3D part
- vpCameraParameters cam;
- vpHomogeneousMatrix cMo;
- vpHomogeneousMatrix cMf;
- double w_xval;
- double w_xsize;
- double w_yval;
- double w_ysize;
- double w_zval;
- double w_zsize;
- double ptXorg;
- double ptYorg;
- double ptZorg;
- double zoomx_3D;
- double zoomy_3D;
- double zoomz_3D;
-
- int nbDivisionz;
-
- double zorg;
- double zoomz;
- double zmax;
- double zmin;
- double zdelt;
- std::string unitz;
- vpImagePoint old_iPr;
- vpImagePoint old_iPz;
- bool blockedr;
- bool blockedz;
- bool blocked;
-
- unsigned int epsi;
- unsigned int epsj;
-
- bool dispUnit;
- bool dispTitle;
- bool dispLegend;
-
- unsigned int gridThickness;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpPlotGraph(const vpPlotGraph &)
-// : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10),
-// xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(), curveNbr(1), curveList(NULL),
-// scaleInitialized(false), firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
-// width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
-// dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
-// w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.),
-// nbDivisionz(10), zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(),
-// blockedr(false), blockedz(false), blocked(false), epsi(5), epsj(6),
-// dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpPlotGraph &operator=(const vpPlotGraph &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+public:
+ double xorg;
+ double yorg;
+ double zoomx;
+ double zoomy;
+ double xmax;
+ double ymax;
+ double xmin;
+ double ymin;
+ double xdelt;
+ double ydelt;
+ bool gridx;
+ bool gridy;
+ vpColor gridColor;
+ std::string title;
+ std::string unitx;
+ std::string unity;
+ unsigned int curveNbr;
+ vpPlotCurve *curveList;
+ bool scaleInitialized;
+ bool firstPoint;
+
+ int nbDivisionx;
+ int nbDivisiony;
+
+ // Graph complet
+ vpImagePoint topLeft;
+ unsigned int width;
+ unsigned int height;
+ vpRect graphZone;
+
+ // Zone d'affichage
+ vpImagePoint dTopLeft;
+ unsigned int dWidth;
+ unsigned int dHeight;
+ vpRect dGraphZone;
+
+ // Zone d'affichage
+ vpImagePoint dTopLeft3D;
+ // int dWidth;
+ // int dHeight;
+ vpRect dGraphZone3D;
+
+ // 3D part
+ vpCameraParameters cam;
+ vpHomogeneousMatrix cMo;
+ vpHomogeneousMatrix cMf;
+ double w_xval;
+ double w_xsize;
+ double w_yval;
+ double w_ysize;
+ double w_zval;
+ double w_zsize;
+ double ptXorg;
+ double ptYorg;
+ double ptZorg;
+ double zoomx_3D;
+ double zoomy_3D;
+ double zoomz_3D;
+
+ int nbDivisionz;
+
+ double zorg;
+ double zoomz;
+ double zmax;
+ double zmin;
+ double zdelt;
+ std::string unitz;
+ vpImagePoint old_iPr;
+ vpImagePoint old_iPz;
+ bool blockedr;
+ bool blockedz;
+ bool blocked;
+
+ unsigned int epsi;
+ unsigned int epsj;
+
+ bool dispUnit;
+ bool dispTitle;
+ bool dispLegend;
+
+ unsigned int gridThickness;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpPlotGraph(const vpPlotGraph &)
+ // : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10),
+ // xmin(0), ymin(-10),
+ // xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(),
+ // curveNbr(1), curveList(NULL), scaleInitialized(false),
+ // firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
+ // width(0), height(0), graphZone(), dTopLeft(), dWidth(0),
+ // dHeight(0), dGraphZone(), dTopLeft3D(), dGraphZone3D(), cam(),
+ // cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
+ // w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0),
+ // zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.), nbDivisionz(10),
+ // zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(),
+ // old_iPz(), blockedr(false), blockedz(false), blocked(false),
+ // epsi(5), epsj(6), dispUnit(false), dispTitle(false),
+ // dispLegend(false), gridThickness(1)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpPlotGraph &operator=(const vpPlotGraph &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpPlotGraph();
- ~vpPlotGraph();
-
- bool check3Dline(vpImagePoint &iP1, vpImagePoint &iP2);
- bool check3Dpoint(vpImagePoint &iP);
- void clearGraphZone(vpImage<unsigned char> &I);
- void computeGraphParameters();
- void computeGraphParameters3D();
-
- void initGraph (unsigned int nbCurve);
- void initSize (vpImagePoint topLeft, unsigned int width, unsigned int height, unsigned int margei, unsigned int margej);
- void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const bool gx, const bool gy);
- void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin, const double ymax, const int nbDivy, const double zmin, const double zmax, const int nbDivz, const bool gx, const bool gy);
-
- void displayGrid (vpImage<unsigned char> &I);
- void displayUnit (vpImage<unsigned char> &I);
- void displayTitle (vpImage<unsigned char> &I);
- void displayLegend (vpImage<unsigned char> &I);
- void displayGrid3D (vpImage<unsigned char> &I);
-
- void findPose();
-
- bool getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP);
-
- bool move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button);
- vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b);
-
- void plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y);
- vpMouseButton::vpMouseButtonType plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z);
- void replot (vpImage<unsigned char> &I);
- void replot3D (vpImage<unsigned char> &I);
-
- void rescalex(unsigned int side, double extremity);
- void rescaley(unsigned int side, double extremity);
- void rescalez(unsigned int side, double extremity);
- //void rescale(double &min, double &max, double &delta, const int nbDiv, int side);
- void resetPointList(const unsigned int curveNum);
-
- void setCurveColor(const unsigned int curveNum, const vpColor color);
- void setCurveThickness(const unsigned int curveNum, const unsigned int thickness);
- void setGridThickness (const unsigned int thickness) {
- this->gridThickness = thickness;
- };
- void setLegend (const unsigned int curveNum, const std::string &legend);
- void setTitle (const std::string &title);
- void setUnitX (const std::string &unitx);
- void setUnitY (const std::string &unity);
- void setUnitZ (const std::string &unitz);
+ vpPlotGraph();
+ ~vpPlotGraph();
+
+ bool check3Dline(vpImagePoint &iP1, vpImagePoint &iP2);
+ bool check3Dpoint(vpImagePoint &iP);
+ void clearGraphZone(vpImage<unsigned char> &I);
+ void computeGraphParameters();
+ void computeGraphParameters3D();
+
+ void initGraph(unsigned int nbCurve);
+ void initSize(vpImagePoint topLeft, unsigned int width, unsigned int height, unsigned int margei,
+ unsigned int margej);
+ void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin,
+ const double ymax, const int nbDivy, const bool gx, const bool gy);
+ void initScale(vpImage<unsigned char> &I, const double xmin, const double xmax, const int nbDivx, const double ymin,
+ const double ymax, const int nbDivy, const double zmin, const double zmax, const int nbDivz,
+ const bool gx, const bool gy);
+
+ void displayGrid(vpImage<unsigned char> &I);
+ void displayUnit(vpImage<unsigned char> &I);
+ void displayTitle(vpImage<unsigned char> &I);
+ void displayLegend(vpImage<unsigned char> &I);
+ void displayGrid3D(vpImage<unsigned char> &I);
+
+ void findPose();
+
+ bool getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP);
+
+ bool move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button);
+ vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b);
+
+ void plot(vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y);
+ vpMouseButton::vpMouseButtonType plot(vpImage<unsigned char> &I, const unsigned int curveNb, const double x,
+ const double y, const double z);
+ void replot(vpImage<unsigned char> &I);
+ void replot3D(vpImage<unsigned char> &I);
+
+ void rescalex(unsigned int side, double extremity);
+ void rescaley(unsigned int side, double extremity);
+ void rescalez(unsigned int side, double extremity);
+ // void rescale(double &min, double &max, double &delta, const int nbDiv,
+ // int side);
+ void resetPointList(const unsigned int curveNum);
+
+ void setCurveColor(const unsigned int curveNum, const vpColor &color);
+ void setCurveThickness(const unsigned int curveNum, const unsigned int thickness);
+ void setGridThickness(const unsigned int thickness) { this->gridThickness = thickness; };
+ void setLegend(const unsigned int curveNum, const std::string &legend);
+ void setTitle(const std::string &title);
+ void setUnitX(const std::string &unitx);
+ void setUnitY(const std::string &unity);
+ void setUnitZ(const std::string &unitz);
};
#endif
#endif
#endif
-
diff --git a/modules/gui/include/visp3/gui/vpProjectionDisplay.h b/modules/gui/include/visp3/gui/vpProjectionDisplay.h
index 3ad7944..3e4b282 100644
--- a/modules/gui/include/visp3/gui/vpProjectionDisplay.h
+++ b/modules/gui/include/visp3/gui/vpProjectionDisplay.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpProjectionDisplay_H
#define vpProjectionDisplay_H
@@ -45,17 +45,18 @@
*/
#include <visp3/core/vpConfig.h>
-#if defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_DISPLAY)
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpForwardProjection.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/core/vpForwardProjection.h>
+#include <visp3/gui/vpDisplayX.h>
#include <list>
@@ -67,76 +68,67 @@
class VISP_EXPORT vpProjectionDisplay
{
private:
- vpImage<unsigned char> Icam ;
- vpImage<unsigned char> Iext ;
+ vpImage<unsigned char> Icam;
+ vpImage<unsigned char> Iext;
#if defined VISP_HAVE_X11
- vpDisplayX dIcam ;
- vpDisplayX dIext ;
+ vpDisplayX dIcam;
+ vpDisplayX dIext;
#elif defined VISP_HAVE_GTK
- vpDisplayGTK dIcam ;
- vpDisplayGTK dIext ;
+ vpDisplayGTK dIcam;
+ vpDisplayGTK dIext;
#elif defined VISP_HAVE_GDI
- vpDisplayGDI dIcam ;
- vpDisplayGDI dIext ;
+ vpDisplayGDI dIcam;
+ vpDisplayGDI dIext;
#elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV dIcam ;
- vpDisplayOpenCV dIext ;
+ vpDisplayOpenCV dIcam;
+ vpDisplayOpenCV dIext;
#elif defined(VISP_HAVE_D3D9)
- vpDisplayD3D dIcam ;
- vpDisplayD3D dIext ;
+ vpDisplayD3D dIcam;
+ vpDisplayD3D dIext;
#endif
public:
- void init() ;
- void init(int select) ;
- void close() ;
- static int internalView() { return 0x01 ; }
- static int externalView() { return 0x02 ; }
+ void init();
+ void init(int select);
+ void close();
+ static int internalView() { return 0x01; }
+ static int externalView() { return 0x02; }
/*! Default constructor. */
vpProjectionDisplay()
: Icam(), Iext(),
-#if defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_DISPLAY)
dIcam(), dIext(),
#endif
listFp(), o(), x(), y(), z(), traj()
{
init();
}
- vpProjectionDisplay(int select)
+ explicit vpProjectionDisplay(int select)
: Icam(), Iext(),
-#if defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_DISPLAY)
dIcam(), dIext(),
#endif
listFp(), o(), x(), y(), z(), traj()
{
- init(select) ;
+ init(select);
}
- void insert( vpForwardProjection &fp) ;
- void display(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const bool &displayTraj = false,
- const unsigned int thickness=1);
- void displayCamera(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const unsigned int thickness=1) ;
+ void insert(vpForwardProjection &fp);
+ void display(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, const vpColor &color, const bool &displayTraj = false,
+ const unsigned int thickness = 1);
+ void displayCamera(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cextMo, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, const unsigned int thickness = 1);
private:
- std::list<vpForwardProjection *> listFp ;
- vpPoint o ;
- vpPoint x ;
- vpPoint y ;
- vpPoint z ;
+ std::list<vpForwardProjection *> listFp;
+ vpPoint o;
+ vpPoint x;
+ vpPoint y;
+ vpPoint z;
vpMatrix traj;
-} ;
-
-
+};
#endif
#endif
diff --git a/modules/gui/include/visp3/gui/vpWin32API.h b/modules/gui/include/visp3/gui/vpWin32API.h
index f70a641..0b895b3 100644
--- a/modules/gui/include/visp3/gui/vpWin32API.h
+++ b/modules/gui/include/visp3/gui/vpWin32API.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,20 +41,25 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
-#include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <string>
+#include <windows.h>
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);
diff --git a/modules/gui/include/visp3/gui/vpWin32Renderer.h b/modules/gui/include/visp3/gui/vpWin32Renderer.h
index 5acf6d8..61d752f 100644
--- a/modules/gui/include/visp3/gui/vpWin32Renderer.h
+++ b/modules/gui/include/visp3/gui/vpWin32Renderer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,53 +38,58 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
#ifndef vpWin32Renderer_HH
#define vpWin32Renderer_HH
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpColor.h>
-#include <windows.h>
+#include <visp3/core/vpImage.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <visp3/core/vpDebug.h>
+#include <windows.h>
class VISP_EXPORT vpWin32Renderer
{
protected:
- //the size of the display
+ // the size of the display
unsigned int m_rwidth;
unsigned int m_rheight;
unsigned int m_rscale;
public:
- vpWin32Renderer() : m_rwidth(0), m_rheight(0), m_rscale(1) {};
+ vpWin32Renderer() : m_rwidth(0), m_rheight(0), m_rscale(1){};
//! Destructor.
- virtual ~vpWin32Renderer() {};
+ virtual ~vpWin32Renderer(){};
//! Inits the display .
- virtual bool init(HWND hWnd, unsigned int w, unsigned int h) =0;
+ virtual bool init(HWND hWnd, unsigned int w, unsigned int h) = 0;
//! Renders the image.
- virtual bool render() =0;
+ virtual bool render() = 0;
/*!
Sets the image to display.
\param im The image to display.
*/
- virtual void setImg(const vpImage<vpRGBa>& im) =0;
- virtual void setImg(const vpImage<unsigned char>& im) =0;
- virtual void setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
- virtual void setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height ) =0;
+ virtual void setImg(const vpImage<vpRGBa> &im) = 0;
+ virtual void setImg(const vpImage<unsigned char> &im) = 0;
+ virtual void setImgROI(const vpImage<vpRGBa> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height) = 0;
+ virtual void setImgROI(const vpImage<unsigned char> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height) = 0;
/*!
Sets the pixel at (x,y).
\param iP The coordinates of the pixel.
\param color The color of the pixel.
*/
- virtual void setPixel(const vpImagePoint &iP, const vpColor &color) =0;
+ virtual void setPixel(const vpImagePoint &iP, const vpColor &color) = 0;
void setScale(unsigned int scale) { m_rscale = scale; }
void setHeight(unsigned int height) { m_rheight = height; }
@@ -97,9 +103,8 @@ public:
\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, int style=PS_SOLID) =0;
+ virtual void drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness,
+ int style = PS_SOLID) = 0;
/*!
Draws a rectangle.
@@ -110,16 +115,14 @@ public:
\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, bool fill=false,
- unsigned int thickness=1) =0;
+ virtual void drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color,
+ bool fill = false, unsigned int thickness = 1) = 0;
/*!
Clears the image to color c.
\param color The color used to fill the image.
*/
- virtual void clear(const vpColor &color) =0;
+ virtual void clear(const vpColor &color) = 0;
/*!
Draws a circle.
@@ -129,8 +132,8 @@ public:
\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, unsigned int thickness=1) =0;
+ virtual void drawCircle(const vpImagePoint ¢er, unsigned int radius, const vpColor &color, bool fill,
+ unsigned int thickness = 1) = 0;
/*!
Draws some text.
@@ -138,8 +141,7 @@ public:
\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.
@@ -148,8 +150,8 @@ public:
\param color The cross' color
\param thickness Thickness of the drawing
*/
- virtual void drawCross(const vpImagePoint &ip, unsigned int size,
- const vpColor &color, unsigned int thickness=1) =0;
+ virtual void drawCross(const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness = 1) = 0;
/*!
Draws an arrow.
@@ -159,15 +161,14 @@ public:
\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.
\param I Image returned.
*/
- virtual void getImage(vpImage<vpRGBa> &I) =0;
+ virtual void getImage(vpImage<vpRGBa> &I) = 0;
};
#endif
diff --git a/modules/gui/include/visp3/gui/vpWin32Window.h b/modules/gui/include/visp3/gui/vpWin32Window.h
index f939742..0422933 100644
--- a/modules/gui/include/visp3/gui/vpWin32Window.h
+++ b/modules/gui/include/visp3/gui/vpWin32Window.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,37 +39,37 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
#ifndef vpWin32Window_HH
#define vpWin32Window_HH
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <windows.h>
-#include <visp3/gui/vpWin32Renderer.h>
-#include <visp3/gui/vpGDIRenderer.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpDisplayException.h>
+#include <visp3/gui/vpGDIRenderer.h>
+#include <visp3/gui/vpWin32Renderer.h>
+#include <windows.h>
+// ViSP-defined messages for window's callback function
+#define vpWM_GETCLICK WM_USER + 1
+#define vpWM_DISPLAY WM_USER + 2
+#define vpWM_GETCLICKUP WM_USER + 3
+#define vpWM_CLOSEDISPLAY WM_USER + 4
+#define vpWM_GETPOINTERMOTIONEVENT WM_USER + 5
+#define vpWM_DISPLAY_ROI WM_USER + 6
-//ViSP-defined messages for window's callback function
-#define vpWM_GETCLICK WM_USER+1
-#define vpWM_DISPLAY WM_USER+2
-#define vpWM_GETCLICKUP WM_USER+3
-#define vpWM_CLOSEDISPLAY WM_USER+4
-#define vpWM_GETPOINTERMOTIONEVENT WM_USER+5
-#define vpWM_DISPLAY_ROI WM_USER+6
-
-//No specific mouse button query
+// No specific mouse button query
#define vpNO_BUTTON_QUERY -1
class vpDisplayWin32;
class VISP_EXPORT vpWin32Window
{
-private :
-
+private:
HINSTANCE hInst;
//! Window's handle
@@ -87,7 +88,7 @@ private :
HANDLE semaKey;
//! Handle for the mouse event semaphore
HANDLE semaMove;
-
+
//! X coordinate of the click
int clickX;
int clickXUp;
@@ -108,20 +109,19 @@ private :
static bool registered;
//! The renderer used by the window
- vpWin32Renderer * renderer;
+ vpWin32Renderer *renderer;
public:
-
- vpWin32Window(vpWin32Renderer * rend = NULL);
+ explicit vpWin32Window(vpWin32Renderer *rend = NULL);
virtual ~vpWin32Window();
- HWND getHWnd(){ return hWnd;}
+ HWND getHWnd() { return hWnd; }
//! Returns true if the window is initialized
- bool isInitialized(){ return initialized; }
+ 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); };
@@ -131,8 +131,7 @@ public:
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 50d45d4..ca49c90 100644
--- a/modules/gui/src/display/vpDisplayGTK.cpp
+++ b/modules/gui/src/display/vpDisplayGTK.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
/*!
\file vpDisplayGTK.cpp
\brief Define the GTK console to display images.
@@ -45,23 +45,23 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GTK) )
+#if (defined(VISP_HAVE_GTK))
+#include <cmath> // std::fabs
+#include <iostream>
+#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
// Display stuff
#include <visp3/core/vpDisplay.h>
#include <visp3/gui/vpDisplayGTK.h>
-//debug / exception
+// debug / exception
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplayException.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpMath.h>
/*!
@@ -72,22 +72,25 @@
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &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) ;
+ init(I);
}
/*!
@@ -101,25 +104,25 @@ vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &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<unsigned char> &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) ;
+ init(I, x, y, title);
}
/*!
@@ -130,21 +133,24 @@ vpDisplayGTK::vpDisplayGTK(vpImage<unsigned char> &I,
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &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) ;
+ init(I);
}
/*!
@@ -157,24 +163,24 @@ vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &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<vpRGBa> &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) ;
+ init(I, x, y, title);
}
/*!
@@ -188,8 +194,8 @@ vpDisplayGTK::vpDisplayGTK(vpImage<vpRGBa> &I,
To initialize the display size, you need to call init().
\code
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGTK.h>
int main()
{
@@ -200,14 +206,12 @@ 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 ;
+ m_windowXPosition = x;
+ m_windowYPosition = y;
m_title = title;
}
@@ -219,8 +223,8 @@ vpDisplayGTK::vpDisplayGTK(int x, int y, const std::string &title)
init(vpImage<vpRGBa> &, int, int, const std::string &).
\code
-#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGTK.h>
int main()
{
@@ -231,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)
{
}
@@ -243,10 +245,7 @@ vpDisplayGTK::vpDisplayGTK()
/*!
Destructor.
*/
-vpDisplayGTK::~vpDisplayGTK()
-{
- closeDisplay() ;
-}
+vpDisplayGTK::~vpDisplayGTK() { closeDisplay(); }
/*!
Initialize the display (size, position and title) of a gray level image.
@@ -256,34 +255,28 @@ vpDisplayGTK::~vpDisplayGTK()
\param title : Window title.
*/
-void
-vpDisplayGTK::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const std::string &title)
-{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
+void vpDisplayGTK::init(vpImage<unsigned char> &I, int x, int y, const std::string &title)
+{
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
}
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
- if (! title.empty())
+ if (!title.empty())
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 ;
+ I.display = this;
+ m_displayHasBeenInitialized = true;
}
-/*!
+/*!
Initialize the display (size, position and title) of a color
image in RGBa format.
@@ -292,31 +285,25 @@ vpDisplayGTK::init(vpImage<unsigned char> &I,
\param title : Window title.
*/
-void
-vpDisplayGTK::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const std::string &title)
+void vpDisplayGTK::init(vpImage<vpRGBa> &I, int x, int y, const std::string &title)
{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
}
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
- if (! title.empty())
+ if (!title.empty())
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 ;
+ I.display = this;
+ m_displayHasBeenInitialized = true;
}
/*!
Initialize the display size, position and title.
@@ -326,34 +313,30 @@ vpDisplayGTK::init(vpImage<vpRGBa> &I,
\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 ;
- char **argv ;
+ int *argc = NULL;
+ char **argv;
gtk_init(argc, &argv);
setScale(m_scaleType, w, h);
- m_width = w/m_scale;
- m_height = h/m_scale;
+ m_width = w / m_scale;
+ m_height = h / m_scale;
/* 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);
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
gtk_window_move(GTK_WINDOW(widget), m_windowXPosition, m_windowYPosition);
@@ -370,80 +353,80 @@ vpDisplayGTK::init(unsigned int w, unsigned int h,
/* 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);
- gdk_colormap_alloc_color(colormap,&lightBlue,FALSE,TRUE);
- col[vpColor::id_lightBlue] = &lightBlue ;
-
- gdk_color_parse("blue",&blue);
- gdk_colormap_alloc_color(colormap,&blue,FALSE,TRUE);
- col[vpColor::id_blue] = &blue ;
-
- gdk_color_parse("dark blue",&darkBlue);
- gdk_colormap_alloc_color(colormap,&darkBlue,FALSE,TRUE);
- col[vpColor::id_darkBlue] = &darkBlue ;
-
- gdk_color_parse("#FF8C8C",&lightRed);
- gdk_colormap_alloc_color(colormap,&lightRed,FALSE,TRUE);
- col[vpColor::id_lightRed] = &lightRed ;
-
- gdk_color_parse("red",&red);
- gdk_colormap_alloc_color(colormap,&red,FALSE,TRUE);
- col[vpColor::id_red] = &red ;
-
- gdk_color_parse("dark red",&darkRed);
- gdk_colormap_alloc_color(colormap,&darkRed,FALSE,TRUE);
- col[vpColor::id_darkRed] = &darkRed ;
-
- gdk_color_parse("light green",&lightGreen);
- gdk_colormap_alloc_color(colormap,&lightGreen,FALSE,TRUE);
- col[vpColor::id_lightGreen] = &lightGreen ;
-
- gdk_color_parse("green",&green);
- gdk_colormap_alloc_color(colormap,&green,FALSE,TRUE);
- col[vpColor::id_green] = &green ;
-
- gdk_color_parse("dark green",&darkGreen);
- gdk_colormap_alloc_color(colormap,&darkGreen,FALSE,TRUE);
- col[vpColor::id_darkGreen] = &darkGreen ;
-
- gdk_color_parse("yellow",&yellow);
- gdk_colormap_alloc_color(colormap,&yellow,FALSE,TRUE);
- col[vpColor::id_yellow] = &yellow ;
-
- gdk_color_parse("cyan",&cyan);
- gdk_colormap_alloc_color(colormap,&cyan,FALSE,TRUE);
- col[vpColor::id_cyan] = &cyan ;
-
- gdk_color_parse("orange",&orange);
- gdk_colormap_alloc_color(colormap,&orange,FALSE,TRUE);
- col[vpColor::id_orange] = &orange ;
-
- gdk_color_parse("purple",&purple);
- gdk_colormap_alloc_color(colormap,&purple,FALSE,TRUE);
- col[vpColor::id_purple] = &purple ;
-
- gdk_color_parse("white",&white);
- gdk_colormap_alloc_color(colormap,&white,FALSE,TRUE);
- col[vpColor::id_white] = &white ;
-
- gdk_color_parse("black",&black);
- gdk_colormap_alloc_color(colormap,&black,FALSE,TRUE);
- col[vpColor::id_black] = &black ;
-
- gdk_color_parse("#C0C0C0",&lightGray);
- gdk_colormap_alloc_color(colormap,&lightGray,FALSE,TRUE);
- col[vpColor::id_lightGray] = &lightGray ;
-
- gdk_color_parse("#808080",&gray);
- gdk_colormap_alloc_color(colormap,&gray,FALSE,TRUE);
- col[vpColor::id_gray] = &gray ;
-
- gdk_color_parse("#404040",&darkGray);
- gdk_colormap_alloc_color(colormap,&darkGray,FALSE,TRUE);
- col[vpColor::id_darkGray] = &darkGray ;
+ gdk_color_parse("light blue", &lightBlue);
+ gdk_colormap_alloc_color(colormap, &lightBlue, FALSE, TRUE);
+ col[vpColor::id_lightBlue] = &lightBlue;
+
+ gdk_color_parse("blue", &blue);
+ gdk_colormap_alloc_color(colormap, &blue, FALSE, TRUE);
+ col[vpColor::id_blue] = &blue;
+
+ gdk_color_parse("dark blue", &darkBlue);
+ gdk_colormap_alloc_color(colormap, &darkBlue, FALSE, TRUE);
+ col[vpColor::id_darkBlue] = &darkBlue;
+
+ gdk_color_parse("#FF8C8C", &lightRed);
+ gdk_colormap_alloc_color(colormap, &lightRed, FALSE, TRUE);
+ col[vpColor::id_lightRed] = &lightRed;
+
+ gdk_color_parse("red", &red);
+ gdk_colormap_alloc_color(colormap, &red, FALSE, TRUE);
+ col[vpColor::id_red] = &red;
+
+ gdk_color_parse("dark red", &darkRed);
+ gdk_colormap_alloc_color(colormap, &darkRed, FALSE, TRUE);
+ col[vpColor::id_darkRed] = &darkRed;
+
+ gdk_color_parse("light green", &lightGreen);
+ gdk_colormap_alloc_color(colormap, &lightGreen, FALSE, TRUE);
+ col[vpColor::id_lightGreen] = &lightGreen;
+
+ gdk_color_parse("green", &green);
+ gdk_colormap_alloc_color(colormap, &green, FALSE, TRUE);
+ col[vpColor::id_green] = &green;
+
+ gdk_color_parse("dark green", &darkGreen);
+ gdk_colormap_alloc_color(colormap, &darkGreen, FALSE, TRUE);
+ col[vpColor::id_darkGreen] = &darkGreen;
+
+ gdk_color_parse("yellow", &yellow);
+ gdk_colormap_alloc_color(colormap, &yellow, FALSE, TRUE);
+ col[vpColor::id_yellow] = &yellow;
+
+ gdk_color_parse("cyan", &cyan);
+ gdk_colormap_alloc_color(colormap, &cyan, FALSE, TRUE);
+ col[vpColor::id_cyan] = &cyan;
+
+ gdk_color_parse("orange", &orange);
+ gdk_colormap_alloc_color(colormap, &orange, FALSE, TRUE);
+ col[vpColor::id_orange] = &orange;
+
+ gdk_color_parse("purple", &purple);
+ gdk_colormap_alloc_color(colormap, &purple, FALSE, TRUE);
+ col[vpColor::id_purple] = &purple;
+
+ gdk_color_parse("white", &white);
+ gdk_colormap_alloc_color(colormap, &white, FALSE, TRUE);
+ col[vpColor::id_white] = &white;
+
+ gdk_color_parse("black", &black);
+ gdk_colormap_alloc_color(colormap, &black, FALSE, TRUE);
+ col[vpColor::id_black] = &black;
+
+ gdk_color_parse("#C0C0C0", &lightGray);
+ gdk_colormap_alloc_color(colormap, &lightGray, FALSE, TRUE);
+ col[vpColor::id_lightGray] = &lightGray;
+
+ gdk_color_parse("#808080", &gray);
+ gdk_colormap_alloc_color(colormap, &gray, FALSE, TRUE);
+ col[vpColor::id_gray] = &gray;
+
+ gdk_color_parse("#404040", &darkGray);
+ gdk_colormap_alloc_color(colormap, &darkGray, FALSE, TRUE);
+ col[vpColor::id_darkGray] = &darkGray;
// Try to load a default font
font = gdk_font_load("-*-times-medium-r-normal-*-16-*-*-*-*-*-*-*");
@@ -453,13 +436,12 @@ vpDisplayGTK::init(unsigned int w, unsigned int h,
font = gdk_font_load("-*-courier 10 pitch-medium-r-normal-*-16-*-*-*-*-*-*-*");
m_title = title;
- if(!title.empty())
+ if (!title.empty())
gdk_window_set_title(widget->window, m_title.c_str());
- m_displayHasBeenInitialized = true ;
+ m_displayHasBeenInitialized = true;
}
-
/*!
\warning Not implemented yet.
@@ -474,36 +456,28 @@ vpDisplayGTK::init(unsigned int w, unsigned int h,
\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.
\param title : Window title.
*/
-void
-vpDisplayGTK::setTitle(const std::string &title)
+void vpDisplayGTK::setTitle(const std::string &title)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
m_title = title;
- if(!title.empty())
+ if (!title.empty())
gdk_window_set_title(widget->window, m_title.c_str());
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
/*!
Set the window position in the screen.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
@@ -511,17 +485,13 @@ vpDisplayGTK::setTitle(const std::string &title)
void vpDisplayGTK::setWindowPosition(int winx, int winy)
{
- if (m_displayHasBeenInitialized) {
+ if (m_displayHasBeenInitialized) {
gtk_window_move(GTK_WINDOW(widget), winx, winy);
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
/*!
Display the gray level image \e I (8bits).
@@ -536,37 +506,25 @@ void vpDisplayGTK::setWindowPosition(int winx, int winy)
void vpDisplayGTK::displayImage(const vpImage<unsigned char> &I)
{
- if (m_displayHasBeenInitialized)
- {
- if(m_scale == 1) {
+ 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{
+ } else {
vpImage<unsigned char> 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
/*!
Display a selection of the gray level image \e I (8bits).
@@ -575,43 +533,36 @@ void vpDisplayGTK::displayImage(const vpImage<unsigned char> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w : Width of the region of interest
-
+
\param h : Height of the region of interest
\sa init(), closeDisplay()
*/
-void vpDisplayGTK::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
+void vpDisplayGTK::displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
vpImage<unsigned char> Itemp;
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);
+ 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
/*!
Display the color image \e I in RGBa format (32bits).
@@ -626,35 +577,24 @@ void vpDisplayGTK::displayImageROI ( const vpImage<unsigned char> &I,const vpIma
void vpDisplayGTK::displayImage(const vpImage<vpRGBa> &I)
{
- if (m_displayHasBeenInitialized)
- {
+ 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,
- (unsigned char *)I.bitmap,
- (gint)(4*m_width));
- }
- else {
+ 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<vpRGBa> 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));
+ // while (g_main_iteration(FALSE));
/* 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
@@ -666,39 +606,33 @@ void vpDisplayGTK::displayImage(const vpImage<vpRGBa> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w : Width of the region of interest
-
+
\param h : Height of the region of interest
\sa init(), closeDisplay()
*/
-void vpDisplayGTK::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int w, const unsigned int h )
+void vpDisplayGTK::displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
vpImage<vpRGBa> Itemp;
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);
+ 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
@@ -707,10 +641,7 @@ void vpDisplayGTK::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint
\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.
@@ -719,14 +650,13 @@ void vpDisplayGTK::displayImage(const unsigned char * /* I */)
*/
void vpDisplayGTK::closeDisplay()
{
- if (col != NULL)
- {
- delete [] col ; col = NULL ;
+ if (col != NULL) {
+ delete[] col;
+ col = NULL;
}
- if (widget != NULL)
- {
- gdk_window_hide (widget->window);
+ if (widget != NULL) {
+ gdk_window_hide(widget->window);
gdk_window_destroy(widget->window);
gtk_widget_destroy(widget);
widget = NULL;
@@ -734,52 +664,39 @@ void vpDisplayGTK::closeDisplay()
m_displayHasBeenInitialized = false;
}
-
/*!
Flushes the display buffer.
It's necessary to use this function to see the results of any drawing.
*/
void vpDisplayGTK::flushDisplay()
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
gdk_window_clear(widget->window);
gdk_flush();
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
/*!
Flushes the display buffer.
It's necessary to use this function to see the results of any drawing.
*/
-void vpDisplayGTK::flushDisplayROI(const vpImagePoint &/*iP*/, const unsigned int /*width*/, const unsigned int /*height*/)
+void vpDisplayGTK::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned int /*width*/,
+ const unsigned int /*height*/)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
gdk_window_clear(widget->window);
gdk_flush();
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ 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.
@@ -788,56 +705,46 @@ 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() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
-
- //if ((a==0)&&(b==0))
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
- {
+ if (m_displayHasBeenInitialized) {
+ double a = ip2.get_i() - ip1.get_i();
+ double b = ip2.get_j() - ip1.get_j();
+ double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
+
+ // if ((a==0)&&(b==0))
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
// DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
+ } else {
+ a /= lg;
+ b /= lg;
vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
+ ip3.set_i(ip2.get_i() - w * a);
+ ip3.set_j(ip2.get_j() - w * b);
vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
+ ip4.set_i(ip3.get_i() - b * h);
+ ip4.set_j(ip3.get_j() + a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
+ ip4.set_i(ip3.get_i() + b * h);
+ ip4.set_j(ip3.get_j() - a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- displayLine ( ip1, ip2, color, thickness ) ;
+ displayLine(ip1, ip2, color, thickness);
}
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
/*!
Display a string at the image point \e ip location.
@@ -849,33 +756,25 @@ 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 (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap, &gdkcolor, FALSE, TRUE);
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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
/*!
@@ -887,45 +786,35 @@ void vpDisplayGTK::displayCharString ( const vpImagePoint &ip,
\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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ 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_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 */
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ 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"));
}
}
/*!
@@ -935,35 +824,29 @@ 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)
- {
+ if (m_displayHasBeenInitialized) {
double i = ip.get_i();
double j = ip.get_j();
vpImagePoint ip1, ip2;
- ip1.set_i( i-size/2 );
- ip1.set_j( j );
- ip2.set_i( i+size/2 );
- ip2.set_j( j );
- displayLine ( ip1, ip2, color, thickness ) ;
+ ip1.set_i(i - size / 2);
+ ip1.set_j(j);
+ ip2.set_i(i + size / 2);
+ ip2.set_j(j);
+ displayLine(ip1, ip2, color, thickness);
- ip1.set_i( i );
- ip1.set_j( j-size/2 );
- ip2.set_i( i );
- ip2.set_j( j+size/2 );
+ ip1.set_i(i);
+ ip1.set_j(j - size / 2);
+ ip2.set_i(i);
+ ip2.set_j(j + size / 2);
- displayLine ( ip1, ip2, color, thickness ) ;
+ displayLine(ip1, ip2, color, thickness);
}
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
/*!
@@ -972,42 +855,30 @@ void vpDisplayGTK::displayCross ( const vpImagePoint &ip,
\param color : Line color.
\param thickness : Line thickness.
*/
-void vpDisplayGTK::displayDotLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
+void vpDisplayGTK::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ 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_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")) ;
+ 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"));
}
}
@@ -1017,38 +888,28 @@ 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,
- unsigned int thickness )
+void vpDisplayGTK::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
- if ( thickness == 1 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ 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_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")) ;
+ 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"));
}
}
@@ -1058,41 +919,31 @@ 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 (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap, &gdkcolor, FALSE, TRUE);
gdk_gc_set_foreground(m_gc, &gdkcolor);
}
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);
}
- 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 );
- }
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
}
-
-/*!
+/*!
Display a rectangle with \e topLeft as the top-left corner and \e
width and \e height the rectangle size.
@@ -1105,52 +956,39 @@ 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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ 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) ;
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ 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"));
}
}
-/*!
+/*!
Display a rectangle.
\param topLeft : Top-left corner of the rectangle.
@@ -1162,52 +1000,39 @@ 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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ 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);
- int w = vpMath::round( bottomRight.get_u() - topLeft.get_u() );
- int h = vpMath::round( bottomRight.get_v() - topLeft.get_v() );
+ 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) ;
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ 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"));
}
}
@@ -1223,54 +1048,41 @@ 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 (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
gdk_gc_set_foreground(m_gc, col[color.id]);
else {
- gdkcolor.red = 256 * color.R;
+ gdkcolor.red = 256 * color.R;
gdkcolor.green = 256 * color.G;
- gdkcolor.blue = 256 * color.B;
- gdk_colormap_alloc_color(colormap,&gdkcolor,FALSE,TRUE);
+ gdkcolor.blue = 256 * color.B;
+ gdk_colormap_alloc_color(colormap, &gdkcolor, FALSE, TRUE);
gdk_gc_set_foreground(m_gc, &gdkcolor);
}
- if ( thickness == 1 ) thickness = 0;
+ 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 ),
- vpMath::round( rectangle.getHeight()/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 ),
- vpMath::round( rectangle.getHeight()/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) ;
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ 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"));
}
}
-
/*!
Wait for a click from one of the mouse button.
@@ -1286,37 +1098,34 @@ vpDisplayGTK::displayRectangle ( const vpRect &rectangle,
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayGTK::getClick(bool blocking)
+bool vpDisplayGTK::getClick(bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
// flushDisplay() ;
- //int cpt =0;
+ // int cpt =0;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())!=NULL){
- //cpt++;
+ while ((ev = gdk_event_get()) != NULL) {
+ // cpt++;
// printf("event %d type %d on window %p My window %p\n",
- //cpt, ev->type, ev->any.window, widget->window);
+ // cpt, ev->type, ev->any.window, widget->window);
- if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS){
- ret = true ;
- //printf("Click detection\n");
+ if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
+ ret = true;
+ // printf("Click detection\n");
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1337,45 +1146,41 @@ vpDisplayGTK::getClick(bool blocking)
to \e false.
*/
-bool
-vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking)
+bool vpDisplayGTK::getClick(vpImagePoint &ip, bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- double u, v ;
+ double u, v;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())!=NULL){
+ while ((ev = gdk_event_get()) != NULL) {
if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale );
- ret = true ;
+ u = ((GdkEventButton *)ev)->x;
+ v = ((GdkEventButton *)ev)->y;
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
+ ret = true;
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
- return ret ;
+ return ret;
}
-
/*!
Wait for a mouse button click and get the position of the clicked
pixel. The button used to click is also set.
-
+
\param ip [out] : The coordinates of the clicked image point.
\param button [out] : The button used to click.
@@ -1390,46 +1195,44 @@ 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;
if (m_displayHasBeenInitialized) {
- double u, v ;
+ double u, v;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())){
- if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS){
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale );
+ while ((ev = gdk_event_get())) {
+ if (ev->any.window == widget->window && ev->type == GDK_BUTTON_PRESS) {
+ u = ((GdkEventButton *)ev)->x;
+ v = ((GdkEventButton *)ev)->y;
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
switch ((int)((GdkEventButton *)ev)->button) {
case 1:
- button = vpMouseButton::button1; break;
+ button = vpMouseButton::button1;
+ break;
case 2:
- button = vpMouseButton::button2; break;
+ button = vpMouseButton::button2;
+ break;
case 3:
- button = vpMouseButton::button3; break;
+ button = vpMouseButton::button3;
+ break;
}
- ret = true ;
+ ret = true;
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1457,49 +1260,46 @@ 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;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
- //flushDisplay() ;
- double u, v ;
+ // flushDisplay() ;
+ double u, v;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())!=NULL){
- if ( ev->any.window == widget->window
- && ev->type == GDK_BUTTON_RELEASE) {
- u = ((GdkEventButton *)ev)->x ;
- v = ((GdkEventButton *)ev)->y ;
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale );
-
- switch ( ( int ) ( ( GdkEventButton * ) ev )->button ) {
+ while ((ev = gdk_event_get()) != NULL) {
+ if (ev->any.window == widget->window && ev->type == GDK_BUTTON_RELEASE) {
+ u = ((GdkEventButton *)ev)->x;
+ v = ((GdkEventButton *)ev)->y;
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
+
+ switch ((int)((GdkEventButton *)ev)->button) {
case 1:
- button = vpMouseButton::button1; break;
+ button = vpMouseButton::button1;
+ break;
case 2:
- button = vpMouseButton::button2; break;
+ button = vpMouseButton::button2;
+ break;
case 3:
- button = vpMouseButton::button3; break;
+ button = vpMouseButton::button3;
+ break;
}
- ret = true ;
+ ret = true;
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "GTK not initialized" ) ) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1511,8 +1311,7 @@ vpDisplayGTK::getClickUp(vpImagePoint &ip,
void vpDisplayGTK::getImage(vpImage<vpRGBa> &I)
{
// should certainly be optimized.
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
GdkImage *ImageGtk;
/*
@@ -1520,33 +1319,27 @@ void vpDisplayGTK::getImage(vpImage<vpRGBa> &I)
ImageGtk = gdk_image_get(m_background, 0, 0, (gint)m_width, (gint)m_height);
- I.resize(m_height, m_width) ;
+ I.resize(m_height, m_width);
guint32 pixel;
- gint x,y;
- guchar OctetRouge,OctetVert,OctetBleu,mask;
+ gint x, y;
+ guchar OctetRouge, OctetVert, OctetBleu, mask;
mask = 0x000000FF;
- for (y=0;y<(gint)m_height;y++)
- {
- for (x=0;x<(gint)m_width;x++)
- {
- pixel = gdk_image_get_pixel(ImageGtk,x,y);
- OctetBleu = (guchar)pixel & mask;
- OctetVert = (guchar)(pixel>>8) & mask;
- OctetRouge = (guchar)(pixel>>16) & mask;
+ for (y = 0; y < (gint)m_height; y++) {
+ for (x = 0; x < (gint)m_width; x++) {
+ pixel = gdk_image_get_pixel(ImageGtk, x, y);
+ OctetBleu = (guchar)pixel & mask;
+ OctetVert = (guchar)(pixel >> 8) & mask;
+ OctetRouge = (guchar)(pixel >> 16) & mask;
I[y][x].R = OctetRouge;
I[y][x].G = OctetVert;
I[y][x].B = OctetBleu;
I[y][x].A = vpRGBa::alpha_default; // default opacity
}
}
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
- }
-
}
/*!
@@ -1559,7 +1352,7 @@ unsigned int vpDisplayGTK::getScreenDepth()
unsigned int depth;
- depth = (unsigned int)gdk_window_get_visual(widget->window)->depth ;
+ depth = (unsigned int)gdk_window_get_visual(widget->window)->depth;
return (depth);
}
@@ -1579,36 +1372,33 @@ unsigned int vpDisplayGTK::getScreenDepth()
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayGTK::getKeyboardEvent(bool blocking)
+bool vpDisplayGTK::getKeyboardEvent(bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- int cpt =0;
+ int cpt = 0;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())!=NULL){
+ while ((ev = gdk_event_get()) != NULL) {
cpt++;
// printf("event %d type %d on window %p My window %p\n",
- //cpt, ev->type, ev->any.window, widget->window);
+ // cpt, ev->type, ev->any.window, widget->window);
- if (ev->any.window == widget->window && ev->type == GDK_KEY_PRESS){
- ret = true ;
- //printf("Key press detection\n");
+ if (ev->any.window == widget->window && ev->type == GDK_KEY_PRESS) {
+ ret = true;
+ // printf("Key press detection\n");
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1632,38 +1422,36 @@ vpDisplayGTK::getKeyboardEvent(bool blocking)
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayGTK::getKeyboardEvent(std::string &key, bool blocking)
+bool vpDisplayGTK::getKeyboardEvent(std::string &key, bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- int cpt =0;
+ int cpt = 0;
do {
GdkEvent *ev = NULL;
- while ((ev = gdk_event_get())!=NULL){
+ while ((ev = gdk_event_get()) != NULL) {
cpt++;
// printf("event %d type %d on window %p My window %p\n",
- //cpt, ev->type, ev->any.window, widget->window);
-
- if (ev->any.window == widget->window && ev->type == GDK_KEY_PRESS){
- //std::cout << "Key val: \"" << gdk_keyval_name (ev->key.keyval) /*ev->key.string*/ << "\"" << std::endl;
- key = gdk_keyval_name (ev->key.keyval);
- ret = true ;
- //printf("Key press detection\n");
+ // cpt, ev->type, ev->any.window, widget->window);
+
+ if (ev->any.window == widget->window && ev->type == GDK_KEY_PRESS) {
+ // std::cout << "Key val: \"" << gdk_keyval_name (ev->key.keyval)
+ // /*ev->key.string*/ << "\"" << std::endl;
+ key = gdk_keyval_name(ev->key.keyval);
+ ret = true;
+ // printf("Key press detection\n");
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- if (blocking){
+ if (blocking) {
flushDisplay();
vpTime::wait(100);
}
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1680,28 +1468,25 @@ vpDisplayGTK::getKeyboardEvent(std::string &key, bool blocking)
was not initialized.
*/
-bool
-vpDisplayGTK::getPointerMotionEvent ( vpImagePoint &ip)
+bool vpDisplayGTK::getPointerMotionEvent(vpImagePoint &ip)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
GdkEvent *ev = NULL;
- if ((ev = gdk_event_get())){
- if (ev->any.window == widget->window && ev->type == GDK_MOTION_NOTIFY){
- double u = ((GdkEventMotion *)ev)->x ;
- double v = ((GdkEventMotion *)ev)->y ;
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale );
-
- ret = true ;
+ if ((ev = gdk_event_get())) {
+ if (ev->any.window == widget->window && ev->type == GDK_MOTION_NOTIFY) {
+ double u = ((GdkEventMotion *)ev)->x;
+ double v = ((GdkEventMotion *)ev)->y;
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
+
+ ret = true;
}
- gdk_event_free(ev) ;
+ gdk_event_free(ev);
}
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
return ret;
}
@@ -1716,21 +1501,17 @@ vpDisplayGTK::getPointerMotionEvent ( vpImagePoint &ip)
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayGTK::getPointerPosition ( vpImagePoint &ip)
+bool vpDisplayGTK::getPointerPosition(vpImagePoint &ip)
{
- if (m_displayHasBeenInitialized)
- {
- int u,v;
+ if (m_displayHasBeenInitialized) {
+ int u, v;
gdk_window_get_pointer(widget->window, &u, &v, NULL);
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale );
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "GTK not initialized")) ;
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "GTK not initialized"));
}
-
+
return true;
}
@@ -1738,13 +1519,13 @@ vpDisplayGTK::getPointerPosition ( vpImagePoint &ip)
Gets screen resolution.
\param w, h : Horizontal and vertical screen resolution.
*/
-void vpDisplayGTK::getScreenSize ( unsigned int &w, unsigned int &h )
+void vpDisplayGTK::getScreenSize(unsigned int &w, unsigned int &h)
{
w = h = 0;
- if (! m_displayHasBeenInitialized) {
- int *argc=NULL ;
- char **argv ;
+ if (!m_displayHasBeenInitialized) {
+ int *argc = NULL;
+ char **argv;
gtk_init(argc, &argv);
@@ -1752,17 +1533,15 @@ void vpDisplayGTK::getScreenSize ( unsigned int &w, unsigned int &h )
gtk_window_set_default_size(GTK_WINDOW(widget_), 100, 100);
gtk_widget_show(widget_);
- GdkScreen *screen_ = gdk_window_get_screen(widget_->window);
+ GdkScreen *screen_ = gdk_window_get_screen(widget_->window);
w = (unsigned int)gdk_screen_get_width(screen_);
h = (unsigned int)gdk_screen_get_height(screen_);
gtk_widget_destroy(widget_);
- }
- else {
- GdkScreen *screen_ = gdk_window_get_screen(widget->window);
+ } else {
+ GdkScreen *screen_ = gdk_window_get_screen(widget->window);
w = (unsigned int)gdk_screen_get_width(screen_);
h = (unsigned int)gdk_screen_get_height(screen_);
}
-
}
/*!
@@ -1786,6 +1565,7 @@ unsigned int vpDisplayGTK::getScreenHeight()
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayGTK.cpp.o) has no symbols
-void dummy_vpDisplayGTK() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayGTK.cpp.o) has no
+// symbols
+void dummy_vpDisplayGTK(){};
#endif
diff --git a/modules/gui/src/display/vpDisplayOpenCV.cpp b/modules/gui/src/display/vpDisplayOpenCV.cpp
index 28e529f..a59a4df 100644
--- a/modules/gui/src/display/vpDisplayOpenCV.cpp
+++ b/modules/gui/src/display/vpDisplayOpenCV.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
/*!
\file vpDisplayOpenCV.cpp
\brief Define the OpenCV console to display images.
@@ -47,37 +47,37 @@
#if defined(VISP_HAVE_OPENCV)
+#include <cmath> // std::fabs
+#include <iostream>
+#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
// Display stuff
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
-//debug / exception
+// debug / exception
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplayException.h>
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/core/core_c.h> // for CV_FILLED versus cv::FILLED
+#include <opencv2/core/core_c.h> // for CV_FILLED versus cv::FILLED
+#include <opencv2/imgproc/imgproc.hpp>
-# ifndef CV_RGB
-# define CV_RGB( r, g, b ) cv::Scalar( (b), (g), (r), 0 )
-# endif
+#ifndef CV_RGB
+#define CV_RGB(r, g, b) cv::Scalar((b), (g), (r), 0)
+#endif
#endif
#ifdef VISP_HAVE_X11
-# include <visp3/gui/vpDisplayX.h> // to get screen resolution
+#include <visp3/gui/vpDisplayX.h> // to get screen resolution
#elif defined(_WIN32)
-# include <windows.h>
+#include <windows.h>
#endif
std::vector<std::string> vpDisplayOpenCV::m_listTitles = std::vector<std::string>();
@@ -92,31 +92,32 @@ unsigned int vpDisplayOpenCV::m_nbWindows = 0;
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I, vpScaleType scaleType)
- :
- vpDisplay(),
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+ : vpDisplay(),
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
m_background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
+#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), y_rbuttonup(0), rbuttonup(false)
+#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());
- init(I) ;
+ init(I);
}
/*!
@@ -130,37 +131,35 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I, vpScaleType scaleTyp
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I,
- int x,
- int y,
- const std::string &title, vpScaleType scaleType)
- :
- vpDisplay(),
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &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
+#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), y_rbuttonup(0), rbuttonup(false)
+#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());
- init(I, x, y, title) ;
+ init(I, x, y, title);
}
-
/*!
Constructor. Initialize a display to visualize a RGBa image (32 bits).
@@ -168,29 +167,31 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage<unsigned char> &I,
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I, vpScaleType scaleType)
:
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
m_background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
+#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), y_rbuttonup(0), rbuttonup(false)
+#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());
- init(I) ;
+ init(I);
}
/*!
@@ -202,32 +203,31 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I,
- int x,
- int y,
- const std::string &title, vpScaleType scaleType)
+vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I, int x, int y, const std::string &title, vpScaleType scaleType)
:
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
m_background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
+#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), y_rbuttonup(0), rbuttonup(false)
+#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());
- init(I, x, y, title) ;
+ init(I, x, y, title);
}
/*!
@@ -241,8 +241,8 @@ vpDisplayOpenCV::vpDisplayOpenCV(vpImage<vpRGBa> &I,
To initialize the display size, you need to call init().
\code
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
int main()
{
@@ -252,28 +252,24 @@ int main()
}
\endcode
*/
-vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const std::string &title)
+vpDisplayOpenCV::vpDisplayOpenCV(int x, int y, const std::string &title)
:
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
m_background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
+#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), y_rbuttonup(0), rbuttonup(false)
+#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;
m_windowYPosition = y;
- if(! title.empty()){
+ if (!title.empty()) {
m_title = title;
- }
- else{
+ } else {
std::ostringstream s;
s << m_nbWindows++;
m_title = std::string("Window ") + s.str();
@@ -282,8 +278,8 @@ vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const std::string &title)
bool isInList;
do {
isInList = false;
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(m_listTitles[i] == m_title){
+ for (size_t i = 0; i < m_listTitles.size(); i++) {
+ if (m_listTitles[i] == m_title) {
std::ostringstream s;
s << m_nbWindows++;
m_title = std::string("Window ") + s.str();
@@ -291,8 +287,7 @@ vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const std::string &title)
break;
}
}
- }
- while(isInList);
+ } while (isInList);
m_listTitles.push_back(m_title);
}
@@ -305,8 +300,8 @@ vpDisplayOpenCV::vpDisplayOpenCV ( int x, int y, const std::string &title)
init(vpImage<vpRGBa> &, int, int, const std::string &).
\code
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
int main()
{
@@ -318,18 +313,15 @@ int main()
*/
vpDisplayOpenCV::vpDisplayOpenCV()
:
- #if (VISP_HAVE_OPENCV_VERSION < 0x020408)
+#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
m_background(NULL), col(NULL), cvcolor(), font(NULL),
- #else
+#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), y_rbuttonup(0), rbuttonup(false)
+#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)
{
}
@@ -338,7 +330,7 @@ vpDisplayOpenCV::vpDisplayOpenCV()
*/
vpDisplayOpenCV::~vpDisplayOpenCV()
{
- closeDisplay() ;
+ closeDisplay();
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
cvReleaseImage(&m_background);
#endif
@@ -352,24 +344,18 @@ vpDisplayOpenCV::~vpDisplayOpenCV()
\param title : Window title.
*/
-void
-vpDisplayOpenCV::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const std::string &title)
+void vpDisplayOpenCV::init(vpImage<unsigned char> &I, int x, int y, const std::string &title)
{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
}
setScale(m_scaleType, I.getWidth(), I.getHeight());
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
- m_displayHasBeenInitialized = true ;
+ init(I.getWidth(), I.getHeight(), x, y, title);
+ I.display = this;
+ m_displayHasBeenInitialized = true;
}
-/*!
+/*!
Initialize the display (size, position and title) of a color
image in RGBa format.
@@ -378,22 +364,16 @@ vpDisplayOpenCV::init(vpImage<unsigned char> &I,
\param title : Window title.
*/
-void
-vpDisplayOpenCV::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const std::string &title)
+void vpDisplayOpenCV::init(vpImage<vpRGBa> &I, int x, int y, const std::string &title)
{
- if ((I.getHeight() == 0) || (I.getWidth()==0))
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Image not initialized")) ;
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Image not initialized"));
}
setScale(m_scaleType, I.getWidth(), I.getHeight());
- init (I.getWidth(), I.getHeight(), x, y, title) ;
- I.display = this ;
- m_displayHasBeenInitialized = true ;
+ init(I.getWidth(), I.getHeight(), x, y, title);
+ I.display = this;
+ m_displayHasBeenInitialized = true;
}
/*!
@@ -406,14 +386,11 @@ vpDisplayOpenCV::init(vpImage<vpRGBa> &I,
\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);
- this->m_width = w / m_scale;
+ this->m_width = w / m_scale;
this->m_height = h / m_scale;
if (x != -1)
@@ -426,11 +403,10 @@ vpDisplayOpenCV::init(unsigned int w, unsigned int h,
int flags = cv::WINDOW_AUTOSIZE;
#endif
- if(m_title.empty()){
- if(!title.empty()){
+ if (m_title.empty()) {
+ if (!title.empty()) {
m_title = std::string(title);
- }
- else{
+ } else {
std::ostringstream s;
s << m_nbWindows++;
@@ -438,10 +414,10 @@ vpDisplayOpenCV::init(unsigned int w, unsigned int h,
}
bool isInList;
- do{
+ do {
isInList = false;
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(m_listTitles[i] == m_title){
+ for (size_t i = 0; i < m_listTitles.size(); i++) {
+ if (m_listTitles[i] == m_title) {
std::ostringstream s;
s << m_nbWindows++;
m_title = std::string("Window ") + s.str();
@@ -449,25 +425,23 @@ vpDisplayOpenCV::init(unsigned int w, unsigned int h,
break;
}
}
- }
- while(isInList);
+ } while (isInList);
m_listTitles.push_back(m_title);
}
- /* Create the window*/
+/* 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")) ;
+ if (cvNamedWindow(this->m_title.c_str(), flags) < 0) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV was not built with a display device"));
}
#else
- cv::namedWindow( this->m_title, flags );
+ 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;
@@ -477,70 +451,69 @@ vpDisplayOpenCV::init(unsigned int w, unsigned int h,
mbuttonup = false;
rbuttonup = false;
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvSetMouseCallback( this->m_title.c_str(), on_mouse, this );
- col = new CvScalar[vpColor::id_unknown] ;
+ cvSetMouseCallback(this->m_title.c_str(), on_mouse, this);
+ col = new CvScalar[vpColor::id_unknown];
#else
- cv::setMouseCallback( this->m_title, on_mouse, this );
- col = new cv::Scalar[vpColor::id_unknown] ;
+ cv::setMouseCallback(this->m_title, on_mouse, this);
+ col = new cv::Scalar[vpColor::id_unknown];
#endif
/* Create color */
vpColor pcolor; // Predefined colors
pcolor = vpColor::lightBlue;
- col[vpColor::id_lightBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_lightBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::blue;
- col[vpColor::id_blue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_blue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkBlue;
- col[vpColor::id_darkBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_darkBlue] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightRed;
- col[vpColor::id_lightRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_lightRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::red;
- col[vpColor::id_red] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_red] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkRed;
- col[vpColor::id_darkRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_darkRed] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGreen;
- col[vpColor::id_lightGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_lightGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::green;
- col[vpColor::id_green] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_green] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGreen;
- col[vpColor::id_darkGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_darkGreen] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::yellow;
- col[vpColor::id_yellow] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_yellow] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::cyan;
- col[vpColor::id_cyan] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_cyan] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::orange;
- col[vpColor::id_orange] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_orange] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::purple;
- col[vpColor::id_purple] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_purple] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::white;
- col[vpColor::id_white] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_white] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::black;
- col[vpColor::id_black] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_black] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGray;
- col[vpColor::id_lightGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_lightGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::gray;
- col[vpColor::id_gray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_gray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGray;
- col[vpColor::id_darkGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B) ;
+ col[vpColor::id_darkGray] = CV_RGB(pcolor.R, pcolor.G, pcolor.B);
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
font = new CvFont;
- cvInitFont( font, CV_FONT_HERSHEY_PLAIN, 0.70f,0.70f);
+ cvInitFont(font, CV_FONT_HERSHEY_PLAIN, 0.70f, 0.70f);
CvSize fontSize;
int baseline;
- cvGetTextSize( "A", font, &fontSize, &baseline );
+ cvGetTextSize("A", font, &fontSize, &baseline);
#else
int thickness = 1;
cv::Size fontSize;
int baseline;
- fontSize = cv::getTextSize( "A", font, fontScale, thickness, &baseline );
+ fontSize = cv::getTextSize("A", font, fontScale, thickness, &baseline);
#endif
fontHeight = fontSize.height + baseline;
- m_displayHasBeenInitialized = true ;
+ m_displayHasBeenInitialized = true;
}
-
/*!
\warning This method is not yet implemented.
@@ -556,11 +529,7 @@ vpDisplayOpenCV::init(unsigned int w, unsigned int h,
\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.
@@ -569,21 +538,20 @@ vpDisplayOpenCV::setFont(const std::string & /* font */)
\param title : Window title.
*/
-void
-vpDisplayOpenCV::setTitle(const std::string & /* title */)
+void vpDisplayOpenCV::setTitle(const std::string & /* title */)
{
-// static bool warn_displayed = false;
-// if (! warn_displayed) {
-// vpTRACE("Not implemented");
-// warn_displayed = true;
-// }
+ // static bool warn_displayed = false;
+ // if (! warn_displayed) {
+ // vpTRACE("Not implemented");
+ // warn_displayed = true;
+ // }
}
-
/*!
Set the window position in the screen.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
@@ -594,15 +562,12 @@ void vpDisplayOpenCV::setWindowPosition(int winx, int winy)
this->m_windowXPosition = winx;
this->m_windowYPosition = winy;
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvMoveWindow( this->m_title.c_str(), winx, winy );
+ cvMoveWindow(this->m_title.c_str(), winx, winy);
#else
- cv::moveWindow( this->m_title.c_str(), winx, winy );
+ cv::moveWindow(this->m_title.c_str(), winx, winy);
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
/*!
@@ -618,42 +583,40 @@ void vpDisplayOpenCV::setWindowPosition(int winx, int winy)
*/
void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &I)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
int depth = 8;
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 != 0) cvReleaseImage(&m_background);
- m_background = cvCreateImage( size, depth, channels );
+ 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 != 0)
+ cvReleaseImage(&m_background);
+ m_background = cvCreateImage(size, depth, channels);
}
- }
- else {
- m_background = cvCreateImage( size, depth, channels );
+ } else {
+ m_background = cvCreateImage(size, depth, channels);
}
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);
- for (unsigned int j=0; j<m_width; j++) {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
- }
- else {
- for (unsigned int i=0; i<m_height; i++) {
- 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;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ } else {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
}
@@ -662,40 +625,36 @@ void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &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 || m_background.cols != (int) m_width){
- m_background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ 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);
- for (unsigned int j=0; j<m_width; j++) {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
- }
- else {
- for (unsigned int i=0; i<m_height; i++) {
- 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;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ } else {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
}
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -707,60 +666,60 @@ void vpDisplayOpenCV::displayImage(const vpImage<unsigned char> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w, h : Width and height of the region of interest
-
+
\sa init(), closeDisplay()
*/
-void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
+void vpDisplayOpenCV::displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
int depth = 8;
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 != 0) cvReleaseImage(&m_background);
- m_background = cvCreateImage( size, depth, channels );
+ 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 != 0)
+ cvReleaseImage(&m_background);
+ m_background = cvCreateImage(size, depth, channels);
}
- }
- else {
- m_background = cvCreateImage( size, depth, channels );
+ } else {
+ m_background = cvCreateImage(size, depth, channels);
}
if (m_scale == 1) {
unsigned int i_min = (unsigned int)iP.get_i();
unsigned int j_min = (unsigned int)iP.get_j();
- 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->imageData + (int)(i*m_background->widthStep + j_min*3);
- for (unsigned int j=j_min; j<j_max; j++) {
+ 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->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;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
- }
- 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);
- 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);
- for (int j=j_min; j<j_max; j++) {
- unsigned char val = I[i*m_scale][j*m_scale];
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ } 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);
+ 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);
+ for (int j = j_min; j < j_max; j++) {
+ unsigned char val = I[i * m_scale][j * m_scale];
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
}
@@ -769,51 +728,46 @@ void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vp
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 || m_background.cols != (int) m_width){
- m_background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ 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) {
unsigned int i_min = (unsigned int)iP.get_i();
unsigned int j_min = (unsigned int)iP.get_j();
- 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);
- for (unsigned int j=j_min; j<j_max; j++) {
+ 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);
+ for (unsigned int j = j_min; j < j_max; j++) {
unsigned char val = I[i][j];
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
- }
- 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);
- 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);
- for (int j=j_min; j<j_max; j++) {
- unsigned char val = I[i*m_scale][j*m_scale];
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
- * ( dst_24 ++ ) = val;
+ } 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);
+ 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);
+ for (int j = j_min; j < j_max; j++) {
+ unsigned char val = I[i * m_scale][j * m_scale];
+ *(dst_24++) = val;
+ *(dst_24++) = val;
+ *(dst_24++) = val;
}
}
}
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-
/*!
Display the color image \e I in RGBa format (32bits).
@@ -828,42 +782,40 @@ void vpDisplayOpenCV::displayImageROI ( const vpImage<unsigned char> &I,const vp
void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &I)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
int depth = 8;
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 != 0) cvReleaseImage(&m_background);
- m_background = cvCreateImage( size, depth, channels );
+ 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 != 0)
+ cvReleaseImage(&m_background);
+ m_background = cvCreateImage(size, depth, channels);
}
- }
- else {
- m_background = cvCreateImage( size, depth, channels );
+ } else {
+ m_background = cvCreateImage(size, depth, channels);
}
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);
- for (unsigned int j=0; j<m_width; j++) {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
- }
- else {
- for (unsigned int i=0; i<m_height; i++) {
- 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;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ } else {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
}
@@ -871,39 +823,35 @@ void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &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 || m_background.cols != (int) m_width){
- m_background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ 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);
- for (unsigned int j=0; j<m_width; j++) {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
- }
- else {
- for (unsigned int i=0; i<m_height; i++) {
- 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;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ } else {
+ for (unsigned int i = 0; i < m_height; i++) {
+ 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;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
}
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -915,60 +863,60 @@ void vpDisplayOpenCV::displayImage(const vpImage<vpRGBa> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w, h : Width and height of the region of interest
-
+
\sa init(), closeDisplay()
*/
-void vpDisplayOpenCV::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
+void vpDisplayOpenCV::displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
int depth = 8;
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 != 0) cvReleaseImage(&m_background);
- m_background = cvCreateImage( size, depth, channels );
+ 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 != 0)
+ cvReleaseImage(&m_background);
+ m_background = cvCreateImage(size, depth, channels);
}
- }
- else {
- m_background = cvCreateImage( size, depth, channels );
+ } else {
+ m_background = cvCreateImage(size, depth, channels);
}
if (m_scale == 1) {
unsigned int i_min = (unsigned int)iP.get_i();
unsigned int j_min = (unsigned int)iP.get_j();
- 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->imageData + (int)(i*m_background->widthStep + j_min*3);
- for (unsigned int j=j_min; j<j_max; j++) {
+ 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->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;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
- }
- 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);
- 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);
- for (int j=j_min; j<j_max; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
- * ( dst_24 ++ ) = val.B;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ } 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);
+ 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);
+ for (int j = j_min; j < j_max; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
}
@@ -976,47 +924,43 @@ void vpDisplayOpenCV::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePo
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 || m_background.cols != (int) m_width){
- m_background = cv::Mat( size, CV_MAKETYPE(depth, channels) );
+ 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) {
unsigned int i_min = (unsigned int)iP.get_i();
unsigned int j_min = (unsigned int)iP.get_j();
- 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);
- for (unsigned int j=j_min; j<j_max; j++) {
+ 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);
+ for (unsigned int j = j_min; j < j_max; j++) {
vpRGBa val = I[i][j];
- * ( dst_24 ++ ) = val.B;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
- }
- 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);
- 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);
- for (int j=j_min; j<j_max; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
- * ( dst_24 ++ ) = val.B;
- * ( dst_24 ++ ) = val.G;
- * ( dst_24 ++ ) = val.R;
+ } 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);
+ 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);
+ for (int j = j_min; j < j_max; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
+ *(dst_24++) = val.B;
+ *(dst_24++) = val.G;
+ *(dst_24++) = val.R;
}
}
}
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1025,10 +969,7 @@ void vpDisplayOpenCV::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePo
\sa init(), closeDisplay()
*/
-void vpDisplayOpenCV::displayImage(const unsigned char * /* I */)
-{
- vpTRACE(" not implemented ") ;
-}
+void vpDisplayOpenCV::displayImage(const unsigned char * /* I */) { vpTRACE(" not implemented "); }
/*!
@@ -1039,38 +980,36 @@ void vpDisplayOpenCV::displayImage(const unsigned char * /* I */)
*/
void vpDisplayOpenCV::closeDisplay()
{
- if (col != NULL)
- {
- delete [] col ; col = NULL ;
+ if (col != NULL) {
+ delete[] col;
+ col = NULL;
}
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if (font != NULL)
- {
- delete font ;
- font = NULL ;
+ if (font != NULL) {
+ delete font;
+ font = NULL;
}
#endif
if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvDestroyWindow( this->m_title.c_str() );
+ cvDestroyWindow(this->m_title.c_str());
#else
- cv::destroyWindow( this->m_title );
+ cv::destroyWindow(this->m_title);
#endif
- for(size_t i = 0 ; i < m_listTitles.size() ; i++){
- if(m_title == m_listTitles[i]){
- m_listTitles.erase(m_listTitles.begin()+(long int)i);
- break;
- }
+ for (size_t i = 0; i < m_listTitles.size(); i++) {
+ if (m_title == m_listTitles[i]) {
+ m_listTitles.erase(m_listTitles.begin() + (long int)i);
+ break;
+ }
}
m_title.clear();
- m_displayHasBeenInitialized= false;
+ m_displayHasBeenInitialized = false;
}
}
-
/*!
Flushes the OpenCV buffer.
It's necessary to use this function to see the results of any drawing.
@@ -1078,20 +1017,16 @@ void vpDisplayOpenCV::closeDisplay()
*/
void vpDisplayOpenCV::flushDisplay()
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvShowImage(this->m_title.c_str(), m_background );
+ cvShowImage(this->m_title.c_str(), m_background);
cvWaitKey(5);
#else
- cv::imshow(this->m_title, m_background );
+ cv::imshow(this->m_title, m_background);
cv::waitKey(5);
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1100,33 +1035,29 @@ 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*/, const unsigned int /*height*/)
+void vpDisplayOpenCV::flushDisplayROI(const vpImagePoint & /*iP*/, const unsigned int /*width*/,
+ const unsigned int /*height*/)
{
- if (m_displayHasBeenInitialized)
- {
+ if (m_displayHasBeenInitialized) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- cvShowImage(this->m_title.c_str(), m_background );
+ cvShowImage(this->m_title.c_str(), m_background);
cvWaitKey(5);
#else
- cv::imshow(this->m_title.c_str(), m_background );
+ cv::imshow(this->m_title.c_str(), m_background);
cv::waitKey(5);
#endif
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-
/*!
\warning Not implemented yet.
*/
void vpDisplayOpenCV::clearDisplay(const vpColor & /* color */)
{
static bool warn_displayed = false;
- if (! warn_displayed) {
+ if (!warn_displayed) {
vpTRACE("Not implemented");
warn_displayed = true;
}
@@ -1139,53 +1070,43 @@ 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() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
-
- //if ((a==0)&&(b==0))
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon())
- &&(std::fabs(b)<= std::numeric_limits<double>::epsilon()))
- {
+ if (m_displayHasBeenInitialized) {
+ double a = ip2.get_i() - ip1.get_i();
+ double b = ip2.get_j() - ip1.get_j();
+ double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
+
+ // if ((a==0)&&(b==0))
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
// DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
+ } else {
+ a /= lg;
+ b /= lg;
vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
+ ip3.set_i(ip2.get_i() - w * a);
+ ip3.set_j(ip2.get_j() - w * b);
vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
+ ip4.set_i(ip3.get_i() - b * h);
+ ip4.set_j(ip3.get_j() + a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
+ ip4.set_i(ip3.get_i() + b * h);
+ ip4.set_j(ip3.get_j() - a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- displayLine ( ip1, ip2, color, thickness ) ;
+ displayLine(ip1, ip2, color, thickness);
}
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1200,43 +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 (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] );
+ cvPutText(m_background, text,
+ 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 ) ),
- font, fontScale, col[color.id] );
+ cv::putText(m_background, text,
+ 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) ;
+ } 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 );
+ cvPutText(m_background, text,
+ 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 ) ),
- font, fontScale, cvcolor );
+ cv::putText(m_background, text,
+ 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
/*!
@@ -1245,47 +1156,37 @@ void vpDisplayOpenCV::displayCharString(const vpImagePoint &ip,
\param radius : Circle radius.
\param color : Circle color.
\param fill : When set to true fill the circle.
- \param thickness : Thickness of the circle. This parameter is only useful
+ \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 (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 ) ),
- (int)radius / m_scale, col[color.id], (int)thickness);
+ cvCircle(m_background,
+ 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 ) ),
- (int)radius / m_scale, col[color.id], (int)thickness);
+ cv::circle(m_background,
+ 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) ;
+ } 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 ) ),
- (int)radius / m_scale, cvcolor, (int)thickness);
+ cvCircle(m_background,
+ 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 ) ),
- (int)radius / m_scale, cvcolor, (int)thickness);
+ cv::circle(m_background,
+ cv::Point(vpMath::round(center.get_u() / m_scale), vpMath::round(center.get_v() / m_scale)),
+ (int)radius / m_scale, cvcolor, (int)thickness);
#endif
}
- }
- else {
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
int filled = cv::FILLED;
#else
@@ -1293,37 +1194,29 @@ void vpDisplayOpenCV::displayCircle(const vpImagePoint ¢er,
#endif
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 ) ),
- (int)radius / m_scale, col[color.id], filled);
+ cvCircle(m_background,
+ 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 ) ),
- (int)radius / m_scale, col[color.id], filled);
+ cv::circle(m_background,
+ 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) ;
+ } 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 ) ),
- (int)radius / m_scale, cvcolor, filled);
+ cvCircle(m_background,
+ 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 ) ),
- (int)radius / m_scale, cvcolor, filled);
+ cv::circle(m_background,
+ 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1334,32 +1227,24 @@ 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,
- unsigned int thickness)
+void vpDisplayOpenCV::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
- vpImagePoint top,bottom,left,right;
- top.set_i(ip.get_i()-size/2);
+ if (m_displayHasBeenInitialized) {
+ vpImagePoint top, bottom, left, right;
+ top.set_i(ip.get_i() - size / 2);
top.set_j(ip.get_j());
- bottom.set_i(ip.get_i()+size/2);
+ bottom.set_i(ip.get_i() + size / 2);
bottom.set_j(ip.get_j());
left.set_i(ip.get_i());
- left.set_j(ip.get_j()-size/2);
+ left.set_j(ip.get_j() - size / 2);
right.set_i(ip.get_i());
- right.set_j(ip.get_j()+size/2);
- displayLine(top, bottom, color, thickness) ;
- displayLine(left, right, color, thickness) ;
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ right.set_j(ip.get_j() + size / 2);
+ displayLine(top, bottom, color, thickness);
+ displayLine(left, right, color, thickness);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
-
}
/*!
@@ -1369,87 +1254,59 @@ vpDisplayOpenCV::displayCross(const vpImagePoint &ip,
\param color : Line color.
\param thickness : Line thickness.
*/
-void
-vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness)
+void vpDisplayOpenCV::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
- double size = 10.*m_scale;
+ 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 deltaj = size/length*(ip2.get_j() - ip1.get_j());
- double deltai = size/length*(ip2.get_i() - ip1.get_i());
+ 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)) {
- double i = slope*j+orig;
- displayLine(vpImagePoint(i, j), vpImagePoint(i+deltai, j+deltaj), color, thickness);
+ 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)) {
+ double i = slope * j + orig;
+ displayLine(vpImagePoint(i, j), vpImagePoint(i + deltai, j + deltaj), color, thickness);
}
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-
/*!
Display a line from image point \e ip1 to image point \e ip2.
\param ip1,ip2 : Initial and final image points.
\param color : Line color.
\param thickness : Line thickness.
*/
-void
-vpDisplayOpenCV::displayLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness)
+void vpDisplayOpenCV::displayLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
+ 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) ;
+ } 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1461,54 +1318,37 @@ vpDisplayOpenCV::displayLine(const vpImagePoint &ip1,
*/
void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, const vpColor &color, unsigned int thickness)
{
- if (m_displayHasBeenInitialized)
- {
- for(unsigned int i=0; i<thickness; i++) {
+ 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 ) ),
- 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);
- #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 ) ),
- 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);
- #endif
+#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)),
+ 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);
+#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)),
+ 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);
+#endif
}
}
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-/*!
+/*!
Display a rectangle with \e topLeft as the top-left corner and \e
width and \e height the rectangle size.
@@ -1521,52 +1361,38 @@ void vpDisplayOpenCV::displayPoint(const vpImagePoint &ip, const vpColor &color,
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 (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 ) ),
- 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((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) ;
+ } 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 ) ),
- 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((topLeft.get_u() + w) / m_scale), vpMath::round((topLeft.get_v() + h) / m_scale)),
+ cvcolor, (int)thickness);
#endif
}
- }
- else {
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
int filled = cv::FILLED;
#else
@@ -1574,48 +1400,36 @@ 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 ) ),
- 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((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) ;
+ } 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 ) ),
- 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((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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-/*!
+/*!
Display a rectangle.
\param topLeft : Top-left corner of the rectangle.
@@ -1627,52 +1441,38 @@ 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 (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) ),
- 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(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) ;
+ } 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 ) ),
- cvcolor, (int)thickness);
+ 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)),
+ 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 {
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
int filled = cv::FILLED;
#else
@@ -1680,45 +1480,33 @@ 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( bottomRight.get_u() / m_scale ),
- vpMath::round( bottomRight.get_v() / 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(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) ;
+ } 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 ) ),
- cvcolor, filled);
+ 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)),
+ 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")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -1734,53 +1522,43 @@ 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 (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 ) ),
- col[color.id], (int)thickness);
+ 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)),
+ col[color.id], (int)thickness);
#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
+ } 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 ) ),
- cvcolor, (int)thickness);
+ 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)),
+ cvcolor, (int)thickness);
#endif
}
- }
- else {
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
int filled = cv::FILLED;
#else
@@ -1788,50 +1566,40 @@ 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 ) ),
- col[color.id], filled);
+ 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)),
+ col[color.id], filled);
#endif
- }
- else {
- cvcolor = CV_RGB(color.R, color.G, color.B) ;
+ } 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 ) ),
- cvcolor, filled);
+ 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)),
+ cvcolor, filled);
#endif
}
}
- }
- else
- {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
-
-
/*!
Wait for a click from one of the mouse button.
@@ -1841,34 +1609,33 @@ vpDisplayOpenCV::displayRectangle(const vpRect &rectangle,
- When set to false, returns true only if a mouse button is
pressed, otherwise returns false.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayOpenCV::getClick(bool blocking)
+bool vpDisplayOpenCV::getClick(bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- flushDisplay() ;
- if (blocking){
+ flushDisplay();
+ if (blocking) {
lbuttondown = false;
mbuttondown = false;
rbuttondown = false;
}
do {
- if (lbuttondown){
- ret = true ;
+ if (lbuttondown) {
+ ret = true;
lbuttondown = false;
}
- if (mbuttondown){
- ret = true ;
+ if (mbuttondown) {
+ ret = true;
mbuttondown = false;
}
- if (rbuttondown){
- ret = true ;
+ if (rbuttondown) {
+ ret = true;
rbuttondown = false;
}
if (blocking)
@@ -1877,11 +1644,9 @@ vpDisplayOpenCV::getClick(bool blocking)
#else
cv::waitKey(10);
#endif
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
return ret;
}
@@ -1896,51 +1661,50 @@ vpDisplayOpenCV::getClick(bool blocking)
\param blocking [in] : true for a blocking behaviour waiting a mouse
button click, false for a non blocking behaviour.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
+bool vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- flushDisplay() ;
+ flushDisplay();
double u, v;
- if (blocking){
+ if (blocking) {
lbuttondown = false;
mbuttondown = false;
rbuttondown = false;
}
do {
- if (lbuttondown){
- ret = true ;
+ if (lbuttondown) {
+ ret = true;
u = (unsigned int)x_lbuttondown * m_scale;
v = (unsigned int)y_lbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
lbuttondown = false;
}
- if (mbuttondown){
- ret = true ;
+ if (mbuttondown) {
+ ret = true;
u = (unsigned int)x_mbuttondown * m_scale;
v = (unsigned int)y_mbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
mbuttondown = false;
}
- if (rbuttondown){
- ret = true ;
+ if (rbuttondown) {
+ ret = true;
u = (unsigned int)x_rbuttondown * m_scale;
v = (unsigned int)y_rbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
rbuttondown = false;
}
if (blocking)
@@ -1949,26 +1713,23 @@ vpDisplayOpenCV::getClick(vpImagePoint &ip, bool blocking)
#else
cv::waitKey(10);
#endif
- } while ( ret == false && blocking == true);
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- return ret ;
+ return ret;
}
-
/*!
Wait for a mouse button click and get the position of the clicked
pixel. The button used to click is also set.
-
+
\param ip [out] : The coordinates of the clicked image point.
\param button [out] : The button used to click.
- \param blocking [in] :
+ \param blocking [in] :
- When set to true, this method waits until a mouse button is
pressed and then returns always true.
- When set to false, returns true only if a mouse button is
@@ -1978,46 +1739,43 @@ 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;
if (m_displayHasBeenInitialized) {
- //flushDisplay() ;
+ // flushDisplay() ;
double u, v;
- if (blocking){
+ if (blocking) {
lbuttondown = false;
mbuttondown = false;
rbuttondown = false;
}
do {
- if (lbuttondown){
- ret = true ;
+ if (lbuttondown) {
+ ret = true;
u = (unsigned int)x_lbuttondown * m_scale;
v = (unsigned int)y_lbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button1;
lbuttondown = false;
}
- if (mbuttondown){
- ret = true ;
+ if (mbuttondown) {
+ ret = true;
u = (unsigned int)x_mbuttondown * m_scale;
v = (unsigned int)y_mbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button2;
mbuttondown = false;
}
- if (rbuttondown){
- ret = true ;
+ if (rbuttondown) {
+ ret = true;
u = (unsigned int)x_rbuttondown * m_scale;
v = (unsigned int)y_rbuttondown * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button3;
rbuttondown = false;
}
@@ -2027,11 +1785,9 @@ vpDisplayOpenCV::getClick(vpImagePoint &ip,
#else
cv::waitKey(10);
#endif
- } while ( ret == false && blocking == true);
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
return ret;
}
@@ -2050,8 +1806,8 @@ vpDisplayOpenCV::getClick(vpImagePoint &ip,
\param blocking [in] : true for a blocking behaviour waiting a mouse
button click, false for a non blocking behaviour.
- \return
- - true if a button was clicked. This is always the case if blocking is set
+ \return
+ - true if a button was clicked. This is always the case if blocking is set
to \e true.
- false if no button was clicked. This can occur if blocking is set
to \e false.
@@ -2059,45 +1815,42 @@ 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) {
- //flushDisplay() ;
+ // flushDisplay() ;
double u, v;
- if (blocking){
+ if (blocking) {
lbuttonup = false;
mbuttonup = false;
rbuttonup = false;
}
do {
- if (lbuttonup){
- ret = true ;
+ if (lbuttonup) {
+ ret = true;
u = (unsigned int)x_lbuttonup * m_scale;
v = (unsigned int)y_lbuttonup * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button1;
lbuttonup = false;
}
- if (mbuttonup){
- ret = true ;
+ if (mbuttonup) {
+ ret = true;
u = (unsigned int)x_mbuttonup * m_scale;
v = (unsigned int)y_mbuttonup * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button2;
mbuttonup = false;
}
- if (rbuttonup){
- ret = true ;
+ if (rbuttonup) {
+ ret = true;
u = (unsigned int)x_rbuttonup * m_scale;
v = (unsigned int)y_rbuttonup * m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
button = vpMouseButton::button3;
rbuttonup = false;
}
@@ -2107,11 +1860,9 @@ vpDisplayOpenCV::getClickUp(vpImagePoint &ip,
#else
cv::waitKey(10);
#endif
- } while ( ret == false && blocking == true);
- }
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "OpenCV not initialized" ) ) ;
+ } while (ret == false && blocking == true);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
return ret;
}
@@ -2126,11 +1877,10 @@ void vpDisplayOpenCV::getImage(vpImage<vpRGBa> &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 = (vpDisplayOpenCV*)display;
- switch ( event )
- {
+ vpDisplayOpenCV *disp = static_cast<vpDisplayOpenCV *>(display);
+ switch (event) {
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
case CV_EVENT_MOUSEMOVE:
#else
@@ -2209,7 +1959,7 @@ void vpDisplayOpenCV::on_mouse( int event, int x, int y, int /*flags*/, void* di
break;
}
- default :
+ default:
break;
}
}
@@ -2224,21 +1974,20 @@ void vpDisplayOpenCV::on_mouse( int event, int x, int y, int /*flags*/, void* di
- When set to false, returns true only if a key is
pressed, otherwise returns false.
- \return
- - true if a key was pressed. This is always the case if blocking is set
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
to \e true.
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayOpenCV::getKeyboardEvent(bool blocking)
+bool vpDisplayOpenCV::getKeyboardEvent(bool blocking)
{
if (m_displayHasBeenInitialized) {
int delay;
- flushDisplay() ;
- if (blocking)
+ flushDisplay();
+ if (blocking)
delay = 0;
- else
+ else
delay = 10;
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
@@ -2250,12 +1999,10 @@ vpDisplayOpenCV::getKeyboardEvent(bool blocking)
if (key_pressed == -1)
return false;
return true;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- //return false; // Never reached after throw()
+ // return false; // Never reached after throw()
}
/*!
@@ -2270,21 +2017,20 @@ vpDisplayOpenCV::getKeyboardEvent(bool blocking)
\param key [out]: If possible, an ISO Latin-1 character
corresponding to the keyboard key.
- \return
- - true if a key was pressed. This is always the case if blocking is set
+ \return
+ - true if a key was pressed. This is always the case if blocking is set
to \e true.
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayOpenCV::getKeyboardEvent(std::string &key, bool blocking)
+bool vpDisplayOpenCV::getKeyboardEvent(std::string &key, bool blocking)
{
if (m_displayHasBeenInitialized) {
int delay;
- flushDisplay() ;
- if (blocking)
+ flushDisplay();
+ if (blocking)
delay = 0;
- else
+ else
delay = 10;
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
@@ -2295,50 +2041,46 @@ vpDisplayOpenCV::getKeyboardEvent(std::string &key, bool blocking)
if (key_pressed == -1)
return false;
else {
- //std::cout << "Key pressed: \"" << key_pressed << "\"" << std::endl;
+ // std::cout << "Key pressed: \"" << key_pressed << "\"" << std::endl;
std::stringstream ss;
ss << key_pressed;
key = ss.str();
}
return true;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
- }
- //return false; // Never reached after throw()
+ // return false; // Never reached after throw()
}
/*!
Get the coordinates of the mouse pointer.
-
+
\param ip [out] : The coordinates of the mouse pointer.
-
+
\return true if a pointer motion event was received, false otherwise.
-
+
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayOpenCV::getPointerMotionEvent (vpImagePoint &ip )
+bool vpDisplayOpenCV::getPointerMotionEvent(vpImagePoint &ip)
{
bool ret = false;
if (m_displayHasBeenInitialized) {
- //flushDisplay() ;
- if (move){
- ret = true ;
+ // flushDisplay() ;
+ if (move) {
+ ret = true;
double u = (unsigned int)x_move / m_scale;
double v = (unsigned int)y_move / m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
move = false;
}
}
else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
return ret;
}
@@ -2353,24 +2095,20 @@ vpDisplayOpenCV::getPointerMotionEvent (vpImagePoint &ip )
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayOpenCV::getPointerPosition ( vpImagePoint &ip)
+bool vpDisplayOpenCV::getPointerPosition(vpImagePoint &ip)
{
if (m_displayHasBeenInitialized) {
bool moved = getPointerMotionEvent(ip);
- if (!moved)
- {
+ if (!moved) {
double u, v;
u = (unsigned int)x_move / m_scale;
v = (unsigned int)y_move / m_scale;
- ip.set_u( u );
- ip.set_v( v );
+ ip.set_u(u);
+ ip.set_v(v);
}
return false;
- }
- else {
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "OpenCV not initialized")) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "OpenCV not initialized"));
}
}
@@ -2378,7 +2116,7 @@ vpDisplayOpenCV::getPointerPosition ( vpImagePoint &ip)
Gets screen resolution in pixels.
\param w, h : Horizontal and vertical screen resolution.
*/
-void vpDisplayOpenCV::getScreenSize ( unsigned int &w, unsigned int &h )
+void vpDisplayOpenCV::getScreenSize(unsigned int &w, unsigned int &h)
{
w = h = 0;
@@ -2386,19 +2124,18 @@ void vpDisplayOpenCV::getScreenSize ( unsigned int &w, unsigned int &h )
vpDisplayX d;
d.getScreenSize(w, h);
#elif defined(VISP_HAVE_XRANDR)
- std::string command="xrandr | grep '*'";
- FILE *fpipe = (FILE*)popen(command.c_str(),"r");
+ std::string command = "xrandr | grep '*'";
+ FILE *fpipe = (FILE *)popen(command.c_str(), "r");
char line[256];
- while ( fgets( line, sizeof(line), fpipe))
- {
+ while (fgets(line, sizeof(line), fpipe)) {
std::string str(line);
std::size_t found = str.find("Failed");
if (found == std::string::npos) {
std::vector<std::string> elm;
elm = vpIoTools::splitChain(str, " ");
- for(size_t i=0; i<elm.size(); i++) {
- if (! elm[i].empty()) {
+ for (size_t i = 0; i < elm.size(); i++) {
+ if (!elm[i].empty()) {
std::vector<std::string> resolution = vpIoTools::splitChain(elm[i], "x");
if (resolution.size() == 2) {
std::istringstream sswidth(resolution[0]), ssheight(resolution[1]);
@@ -2412,12 +2149,13 @@ void vpDisplayOpenCV::getScreenSize ( unsigned int &w, unsigned int &h )
}
pclose(fpipe);
#elif defined(_WIN32)
-# if !defined(WINRT)
+#if !defined(WINRT)
w = GetSystemMetrics(SM_CXSCREEN);
h = GetSystemMetrics(SM_CYSCREEN);
-# else
- throw(vpException(vpException::functionNotImplementedError, "The function vpDisplayOpenCV::getScreenSize() is not implemented on winrt"));
-# endif
+#else
+ throw(vpException(vpException::functionNotImplementedError, "The function vpDisplayOpenCV::getScreenSize() is not "
+ "implemented on winrt"));
+#endif
#endif
}
@@ -2442,6 +2180,7 @@ unsigned int vpDisplayOpenCV::getScreenHeight()
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayOpenCV.cpp.o) has no symbols
-void dummy_vpDisplayOpenCV() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayOpenCV.cpp.o) has no
+// symbols
+void dummy_vpDisplayOpenCV(){};
#endif
diff --git a/modules/gui/src/display/vpDisplayX.cpp b/modules/gui/src/display/vpDisplayX.cpp
index 48bc0d2..c27bf09 100644
--- a/modules/gui/src/display/vpDisplayX.cpp
+++ b/modules/gui/src/display/vpDisplayX.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,21 +42,20 @@
\brief Define the X11 console to display images.
*/
-
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_X11
+#include <cmath> // std::fabs
+#include <iostream>
+#include <limits> // numeric_limits
#include <stdio.h>
#include <stdlib.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
// Display stuff
#include <visp3/core/vpDisplay.h>
#include <visp3/gui/vpDisplayX.h>
-//debug / exception
+// debug / exception
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplayException.h>
@@ -71,22 +71,26 @@
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayX::vpDisplayX (vpImage<unsigned char> &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)
+vpDisplayX::vpDisplayX(vpImage<unsigned char> &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)
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init ( I ) ;
+ init(I);
}
/*!
@@ -100,24 +104,25 @@ vpDisplayX::vpDisplayX (vpImage<unsigned char> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayX::vpDisplayX ( vpImage<unsigned char> &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<unsigned char> &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 ) ;
+ init(I, x, y, title);
}
/*!
@@ -128,20 +133,24 @@ vpDisplayX::vpDisplayX ( vpImage<unsigned char> &I,
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &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)
+vpDisplayX::vpDisplayX(vpImage<vpRGBa> &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)
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init ( I ) ;
+ init(I);
}
/*!
@@ -154,23 +163,24 @@ vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &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<vpRGBa> &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 ) ;
+ init(I, x, y, title);
}
/*!
@@ -184,8 +194,8 @@ vpDisplayX::vpDisplayX ( vpImage<vpRGBa> &I,
To initialize the display size, you need to call init().
\code
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
int main()
{
@@ -195,14 +205,13 @@ 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)
+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)
{
- m_windowXPosition = x ;
- m_windowYPosition = y ;
+ m_windowXPosition = x;
+ m_windowYPosition = y;
m_title = title;
}
@@ -215,8 +224,8 @@ vpDisplayX::vpDisplayX ( int x, int y, const std::string &title )
init(vpImage<vpRGBa> &, int, int, const std::string &).
\code
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
int main()
{
@@ -227,20 +236,16 @@ 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)
{
}
/*!
Destructor.
*/
-vpDisplayX::~vpDisplayX()
-{
- closeDisplay() ;
-}
+vpDisplayX::~vpDisplayX() { closeDisplay(); }
/*!
Initialize the display (size, position and title) of a gray level image.
@@ -250,108 +255,91 @@ vpDisplayX::~vpDisplayX()
\param title : Window title.
*/
-void
-vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const std::string &title )
+void vpDisplayX::init(vpImage<unsigned char> &I, int x, int y, const std::string &title)
{
setScale(m_scaleType, I.getWidth(), I.getHeight());
if (x_color == NULL) {
// id_unknown = number of predefined colors
- x_color= new unsigned long [vpColor::id_unknown];
+ x_color = new unsigned long[vpColor::id_unknown];
}
- XSizeHints hints;
+ XSizeHints hints;
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
- if (! title.empty())
+ if (!title.empty())
m_title = title;
// Positionnement de la fenetre dans l'ecran.
- if ( ( m_windowXPosition < 0 ) || ( m_windowYPosition < 0 ) )
- {
+ if ((m_windowXPosition < 0) || (m_windowYPosition < 0)) {
hints.flags = 0;
- }
- else
- {
+ } else {
hints.flags = USPosition;
hints.x = m_windowXPosition;
hints.y = m_windowYPosition;
}
// setup X11 --------------------------------------------------
- m_width = I.getWidth() / m_scale;
+ m_width = I.getWidth() / m_scale;
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." ) ) ;
+ display = XOpenDisplay(NULL);
+ if (display == NULL) {
+ vpERROR_TRACE("Can't connect display on server %s.\n", XDisplayName(NULL));
+ throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server."));
}
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
+ 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 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
+ 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."));
}
//
// Create color table for 8 and 16 bits screen
//
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ if (screen_depth == 8) {
+ lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll);
+ xcolor.flags = DoRed | DoGreen | DoBlue;
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
+ for (unsigned int i = 0; i < 256; i++) {
+ xcolor.pixel = i;
xcolor.red = 256 * i;
xcolor.green = 256 * i;
xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
+ XStoreColor(display, lut, &xcolor);
}
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
}
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
+ else if (screen_depth == 16) {
+ for (unsigned int i = 0; i < 256; i++) {
xcolor.pad = 0;
xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
+ if (XAllocColor(display, lut, &xcolor) == 0) {
+ vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i);
+ throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors."));
}
colortable[i] = xcolor.pixel;
}
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
- Visual *visual = DefaultVisual (display, screen);
+ Visual *visual = DefaultVisual(display, screen);
RMask = visual->red_mask;
GMask = visual->green_mask;
BMask = visual->blue_mask;
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
GShift = 15 - getMsb(GMask);
BShift = 15 - getMsb(BMask);
}
@@ -359,322 +347,312 @@ vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const std::string &t
//
// Create colors for overlay
//
- switch ( screen_depth )
- {
+ switch (screen_depth) {
case 8:
// Color BLACK and WHITE are set properly by default.
// Color LIGHT GRAY.
x_color[vpColor::id_lightGray] = 254;
- xcolor.pixel = x_color[vpColor::id_lightGray] ;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightGray];
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XStoreColor(display, lut, &xcolor);
// Color GRAY.
x_color[vpColor::id_gray] = 253;
- xcolor.pixel = x_color[vpColor::id_gray] ;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_gray];
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GRAY.
x_color[vpColor::id_darkGray] = 252;
- xcolor.pixel = x_color[vpColor::id_darkGray] ;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkGray];
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT RED.
x_color[vpColor::id_lightRed] = 251;
- xcolor.pixel = x_color[vpColor::id_lightRed] ;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightRed];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XStoreColor(display, lut, &xcolor);
// Color RED.
x_color[vpColor::id_red] = 250;
- xcolor.pixel = x_color[vpColor::id_red] ;
- xcolor.red = 256 * 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_red];
+ xcolor.red = 256 * 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color DARK RED.
x_color[vpColor::id_darkRed] = 249;
- xcolor.pixel = x_color[vpColor::id_darkRed] ;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkRed];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT GREEN.
x_color[vpColor::id_lightGreen] = 248;
- xcolor.pixel = x_color[vpColor::id_lightGreen] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightGreen];
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XStoreColor(display, lut, &xcolor);
// Color GREEN.
x_color[vpColor::id_green] = 247;
- xcolor.pixel = x_color[vpColor::id_green];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_green];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GREEN.
x_color[vpColor::id_darkGreen] = 246;
- xcolor.pixel = x_color[vpColor::id_darkGreen] ;
- xcolor.red = 0;
- xcolor.green = 256 * 128;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkGreen];
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT BLUE.
x_color[vpColor::id_lightBlue] = 245;
- xcolor.pixel = x_color[vpColor::id_lightBlue] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightBlue];
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color BLUE.
x_color[vpColor::id_blue] = 244;
- xcolor.pixel = x_color[vpColor::id_blue];
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_blue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color DARK BLUE.
x_color[vpColor::id_darkBlue] = 243;
- xcolor.pixel = x_color[vpColor::id_darkBlue] ;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkBlue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
// Color YELLOW.
x_color[vpColor::id_yellow] = 242;
- xcolor.pixel = x_color[vpColor::id_yellow];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_yellow];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color ORANGE.
x_color[vpColor::id_orange] = 241;
- xcolor.pixel = x_color[vpColor::id_orange];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_orange];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color CYAN.
x_color[vpColor::id_cyan] = 240;
- xcolor.pixel = x_color[vpColor::id_cyan];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_cyan];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color PURPLE.
x_color[vpColor::id_purple] = 239;
- xcolor.pixel = x_color[vpColor::id_purple];
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_purple];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
break;
case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ case 24: {
+ xcolor.flags = DoRed | DoGreen | DoBlue;
// Couleur BLACK.
- xcolor.pad = 0;
- xcolor.red = 0;
+ xcolor.pad = 0;
+ xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_black] = xcolor.pixel;
// Couleur WHITE.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 256* 255;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_white] = xcolor.pixel;
// Couleur LIGHT GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGray] = xcolor.pixel;
// Couleur GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_gray] = xcolor.pixel;
// Couleur DARK GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGray] = xcolor.pixel;
// Couleur LIGHT RED.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightRed] = xcolor.pixel;
// Couleur RED.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_red] = xcolor.pixel;
// Couleur DARK RED.
- xcolor.pad = 0;
- xcolor.red = 256* 128;
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkRed] = xcolor.pixel;
// Couleur LIGHT GREEN.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGreen] = xcolor.pixel;
// Couleur GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256*255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_green] = xcolor.pixel;
// Couleur DARK GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256* 128;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGreen] = xcolor.pixel;
// Couleur LIGHT Blue.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightBlue] = xcolor.pixel;
// Couleur BLUE.
xcolor.pad = 0;
xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_blue] = xcolor.pixel;
// Couleur DARK BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
+ xcolor.pad = 0;
+ xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 256* 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkBlue] = xcolor.pixel;
// Couleur YELLOW.
xcolor.pad = 0;
xcolor.red = 256 * 255;
xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_yellow] = xcolor.pixel;
// Couleur ORANGE.
xcolor.pad = 0;
xcolor.red = 256 * 255;
xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_orange] = xcolor.pixel;
// Couleur CYAN.
xcolor.pad = 0;
xcolor.red = 0;
xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_cyan] = xcolor.pixel;
// Couleur PURPLE.
xcolor.pad = 0;
xcolor.red = 256 * 128;
xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_purple] = xcolor.pixel;
break;
}
}
- XSetStandardProperties ( display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
+ 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);
+ XSelectInput(display, window,
+ ExposureMask | ButtonPressMask | ButtonReleaseMask | KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask | PointerMotionMask);
// graphic context creation
values.plane_mask = AllPlanes;
values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
-
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
+ values.foreground = WhitePixel(display, screen);
+ values.background = BlackPixel(display, screen);
+ context = XCreateGC(display, window, GCPlaneMask | GCFillStyle | GCForeground | GCBackground, &values);
+ if (context == NULL) {
+ vpERROR_TRACE("Can't create graphics context.");
+ throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context"));
}
// Pixmap creation.
- pixmap = XCreatePixmap ( display, window, m_width, m_height, screen_depth );
+ pixmap = XCreatePixmap(display, window, m_width, m_height, screen_depth);
// Hangs when forward X11 is used to send the display to an other computer
// do
@@ -682,24 +660,22 @@ vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const std::string &t
// while ( event.xany.type != Expose );
{
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- m_width, m_height, XBitmapPad ( display ), 0 );
+ 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 ;
+ m_displayHasBeenInitialized = true;
- XStoreName ( display, window, m_title.c_str() );
+ XStoreName(display, window, m_title.c_str());
- XSync ( display, 1 );
+ XSync(display, 1);
- I.display = this ;
+ I.display = this;
}
-/*!
+/*!
Initialize the display (size, position and title) of a color
image in RGBa format.
@@ -708,32 +684,28 @@ vpDisplayX::init ( vpImage<unsigned char> &I, int x, int y, const std::string &t
\param title : Window title.
*/
-void
-vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const std::string &title)
+void vpDisplayX::init(vpImage<vpRGBa> &I, int x, int y, const std::string &title)
{
setScale(m_scaleType, I.getWidth(), I.getHeight());
- XSizeHints hints;
+ XSizeHints hints;
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
if (x_color == NULL) {
// id_unknown = number of predefined colors
- x_color= new unsigned long [vpColor::id_unknown];
+ x_color = new unsigned long[vpColor::id_unknown];
}
- if (! title.empty())
+ if (!title.empty())
m_title = title;
// Positionnement de la fenetre dans l'ecran.
- if ( ( m_windowXPosition < 0 ) || ( m_windowYPosition < 0 ) )
- {
+ if ((m_windowXPosition < 0) || (m_windowYPosition < 0)) {
hints.flags = 0;
- }
- else
- {
+ } else {
hints.flags = USPosition;
hints.x = m_windowXPosition;
hints.y = m_windowYPosition;
@@ -743,400 +715,376 @@ vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const std::string &title)
m_width = I.getWidth() / m_scale;
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." ) ) ;
+ if ((display = XOpenDisplay(NULL)) == NULL) {
+ vpERROR_TRACE("Can't connect display on server %s.\n", XDisplayName(NULL));
+ throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server."));
}
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
+ screen = DefaultScreen(display);
+ lut = DefaultColormap(display, screen);
+ screen_depth = (unsigned int)DefaultDepth(display, screen);
- vpDEBUG_TRACE ( 1, "Screen depth: %d\n", screen_depth );
+ 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 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
+ 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."));
}
//
// Create color table for 8 and 16 bits screen
//
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ if (screen_depth == 8) {
+ lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll);
+ xcolor.flags = DoRed | DoGreen | DoBlue;
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
+ for (unsigned int i = 0; i < 256; i++) {
+ xcolor.pixel = i;
xcolor.red = 256 * i;
xcolor.green = 256 * i;
xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
+ XStoreColor(display, lut, &xcolor);
}
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
}
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
+ else if (screen_depth == 16) {
+ for (unsigned int i = 0; i < 256; i++) {
xcolor.pad = 0;
xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
+ if (XAllocColor(display, lut, &xcolor) == 0) {
+ vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i);
+ throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors."));
}
colortable[i] = xcolor.pixel;
}
- Visual *visual = DefaultVisual (display, screen);
+ Visual *visual = DefaultVisual(display, screen);
RMask = visual->red_mask;
GMask = visual->green_mask;
BMask = visual->blue_mask;
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
GShift = 15 - getMsb(GMask);
BShift = 15 - getMsb(BMask);
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
}
-
//
// Create colors for overlay
//
- switch ( screen_depth )
- {
+ switch (screen_depth) {
case 8:
// Color BLACK and WHITE are set properly.
// Color LIGHT GRAY.
x_color[vpColor::id_lightGray] = 254;
- xcolor.pixel = x_color[vpColor::id_lightGray] ;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightGray];
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XStoreColor(display, lut, &xcolor);
// Color GRAY.
x_color[vpColor::id_gray] = 253;
- xcolor.pixel = x_color[vpColor::id_gray] ;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_gray];
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GRAY.
x_color[vpColor::id_darkGray] = 252;
- xcolor.pixel = x_color[vpColor::id_darkGray] ;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkGray];
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT RED.
x_color[vpColor::id_lightRed] = 251;
- xcolor.pixel = x_color[vpColor::id_lightRed] ;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightRed];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XStoreColor(display, lut, &xcolor);
// Color RED.
x_color[vpColor::id_red] = 250;
- xcolor.pixel = x_color[vpColor::id_red] ;
- xcolor.red = 256 * 255;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_red];
+ xcolor.red = 256 * 255;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color DARK RED.
x_color[vpColor::id_darkRed] = 249;
- xcolor.pixel = x_color[vpColor::id_darkRed] ;
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkRed];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT GREEN.
x_color[vpColor::id_lightGreen] = 248;
- xcolor.pixel = x_color[vpColor::id_lightGreen] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightGreen];
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XStoreColor(display, lut, &xcolor);
// Color GREEN.
x_color[vpColor::id_green] = 247;
- xcolor.pixel = x_color[vpColor::id_green];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_green];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GREEN.
x_color[vpColor::id_darkGreen] = 246;
- xcolor.pixel = x_color[vpColor::id_darkGreen] ;
- xcolor.red = 0;
- xcolor.green = 256 * 128;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkGreen];
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT BLUE.
x_color[vpColor::id_lightBlue] = 245;
- xcolor.pixel = x_color[vpColor::id_lightBlue] ;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_lightBlue];
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color BLUE.
x_color[vpColor::id_blue] = 244;
- xcolor.pixel = x_color[vpColor::id_blue];
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_blue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color DARK BLUE.
x_color[vpColor::id_darkBlue] = 243;
- xcolor.pixel = x_color[vpColor::id_darkBlue] ;
- xcolor.red = 0;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_darkBlue];
+ xcolor.red = 0;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
// Color YELLOW.
x_color[vpColor::id_yellow] = 242;
- xcolor.pixel = x_color[vpColor::id_yellow];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_yellow];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color ORANGE.
x_color[vpColor::id_orange] = 241;
- xcolor.pixel = x_color[vpColor::id_orange];
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_orange];
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 165;
+ xcolor.blue = 0;
+ XStoreColor(display, lut, &xcolor);
// Color CYAN.
x_color[vpColor::id_cyan] = 240;
- xcolor.pixel = x_color[vpColor::id_cyan];
- xcolor.red = 0;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_cyan];
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XStoreColor(display, lut, &xcolor);
// Color PURPLE.
x_color[vpColor::id_purple] = 239;
- xcolor.pixel = x_color[vpColor::id_purple];
- xcolor.red = 256 * 128;
- xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = x_color[vpColor::id_purple];
+ xcolor.red = 256 * 128;
+ xcolor.green = 0;
+ xcolor.blue = 256 * 128;
+ XStoreColor(display, lut, &xcolor);
break;
case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ case 24: {
+ xcolor.flags = DoRed | DoGreen | DoBlue;
// Couleur BLACK.
- xcolor.pad = 0;
- xcolor.red = 0;
+ xcolor.pad = 0;
+ xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_black] = xcolor.pixel;
// Couleur WHITE.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
- xcolor.green = 256* 255;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_white] = xcolor.pixel;
// Couleur LIGHT GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 192;
- xcolor.green = 256 * 192;
- xcolor.blue = 256 * 192;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 192;
+ xcolor.green = 256 * 192;
+ xcolor.blue = 256 * 192;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGray] = xcolor.pixel;
// Couleur GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 128;
- xcolor.green = 256 * 128;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_gray] = xcolor.pixel;
// Couleur DARK GRAY.
- xcolor.pad = 0;
- xcolor.red = 256 * 64;
- xcolor.green = 256 * 64;
- xcolor.blue = 256 * 64;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 64;
+ xcolor.green = 256 * 64;
+ xcolor.blue = 256 * 64;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGray] = xcolor.pixel;
// Couleur LIGHT RED.
- xcolor.pad = 0;
- xcolor.red = 256 * 255;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 140;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightRed] = xcolor.pixel;
// Couleur RED.
- xcolor.pad = 0;
- xcolor.red = 256* 255;
+ xcolor.pad = 0;
+ xcolor.red = 256 * 255;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_red] = xcolor.pixel;
// Couleur DARK RED.
- xcolor.pad = 0;
- xcolor.red = 256* 128;
+ xcolor.pad = 0;
+ xcolor.red = 256 * 128;
xcolor.green = 0;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkRed] = xcolor.pixel;
// Couleur LIGHT GREEN.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 255;
- xcolor.blue = 256 * 140;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 256 * 140;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGreen] = xcolor.pixel;
// Couleur GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256*255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 255;
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_green] = xcolor.pixel;
// Couleur DARK GREEN.
- xcolor.pad = 0;
- xcolor.red = 0;
- xcolor.green = 256* 128;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 0;
+ xcolor.green = 256 * 128;
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGreen] = xcolor.pixel;
// Couleur LIGHT Blue.
- xcolor.pad = 0;
- xcolor.red = 256 * 140;
- xcolor.green = 256 * 140;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.pad = 0;
+ xcolor.red = 256 * 140;
+ xcolor.green = 256 * 140;
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightBlue] = xcolor.pixel;
// Couleur BLUE.
xcolor.pad = 0;
xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 256* 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_blue] = xcolor.pixel;
// Couleur DARK BLUE.
- xcolor.pad = 0;
- xcolor.red = 0;
+ xcolor.pad = 0;
+ xcolor.red = 0;
xcolor.green = 0;
- xcolor.blue = 256* 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkBlue] = xcolor.pixel;
// Couleur YELLOW.
xcolor.pad = 0;
xcolor.red = 256 * 255;
xcolor.green = 256 * 255;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_yellow] = xcolor.pixel;
// Couleur ORANGE.
xcolor.pad = 0;
xcolor.red = 256 * 255;
xcolor.green = 256 * 165;
- xcolor.blue = 0;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 0;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_orange] = xcolor.pixel;
// Couleur CYAN.
xcolor.pad = 0;
xcolor.red = 0;
xcolor.green = 256 * 255;
- xcolor.blue = 256 * 255;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 255;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_cyan] = xcolor.pixel;
// Couleur PURPLE.
xcolor.pad = 0;
xcolor.red = 256 * 128;
xcolor.green = 0;
- xcolor.blue = 256 * 128;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * 128;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_purple] = xcolor.pixel;
break;
}
}
- XSetStandardProperties ( display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
+ 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);
+ XSelectInput(display, window,
+ ExposureMask | ButtonPressMask | ButtonReleaseMask | KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask | PointerMotionMask);
// Creation du contexte graphique
values.plane_mask = AllPlanes;
values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
+ values.foreground = WhitePixel(display, screen);
+ values.background = BlackPixel(display, screen);
+ context = XCreateGC(display, window, GCPlaneMask | GCFillStyle | GCForeground | GCBackground, &values);
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
+ if (context == NULL) {
+ vpERROR_TRACE("Can't create graphics context.");
+ throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context"));
}
// Pixmap creation.
- pixmap = XCreatePixmap ( display, window, m_width, m_height, screen_depth );
+ pixmap = XCreatePixmap(display, window, m_width, m_height, screen_depth);
// Hangs when forward X11 is used to send the display to an other computer
// do
@@ -1144,22 +1092,19 @@ vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const std::string &title)
// while ( event.xany.type != Expose );
{
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- m_width, m_height, XBitmapPad ( display ), 0 );
-
+ 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 ;
+ m_displayHasBeenInitialized = true;
- XSync ( display, true );
+ XSync(display, true);
- XStoreName ( display, window, m_title.c_str() );
+ XStoreName(display, window, m_title.c_str());
- I.display = this ;
+ I.display = this;
}
/*!
@@ -1169,31 +1114,29 @@ vpDisplayX::init ( vpImage<vpRGBa> &I, int x, int y, const std::string &title)
\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);
if (x_color == NULL) {
// id_unknown = number of predefined colors
- x_color= new unsigned long [vpColor::id_unknown];
+ x_color = new unsigned long[vpColor::id_unknown];
}
- /* setup X11 ------------------------------------------------------------- */
- this->m_width = w / m_scale;
+ /* setup X11 -------------------------------------------------------------
+ */
+ this->m_width = w / m_scale;
this->m_height = h / m_scale;
- XSizeHints hints;
+ XSizeHints hints;
if (x != -1)
- m_windowXPosition = x ;
+ m_windowXPosition = x;
if (y != -1)
- m_windowYPosition = y ;
+ m_windowYPosition = y;
// Positionnement de la fenetre dans l'ecran.
- if ( ( m_windowXPosition < 0 ) || ( m_windowYPosition < 0 ) )
- {
+ if ((m_windowXPosition < 0) || (m_windowYPosition < 0)) {
hints.flags = 0;
- }
- else
- {
+ } else {
hints.flags = USPosition;
hints.x = m_windowXPosition;
hints.y = m_windowYPosition;
@@ -1201,77 +1144,62 @@ void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const std:
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." ) ) ;
+ if ((display = XOpenDisplay(NULL)) == NULL) {
+ vpERROR_TRACE("Can't connect display on server %s.\n", XDisplayName(NULL));
+ throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server."));
}
- screen = DefaultScreen ( display );
- lut = DefaultColormap ( display, screen );
- screen_depth = (unsigned int)DefaultDepth ( display, screen );
+ screen = DefaultScreen(display);
+ lut = DefaultColormap(display, screen);
+ screen_depth = (unsigned int)DefaultDepth(display, screen);
- vpTRACE ( "Screen depth: %d\n", screen_depth );
+ 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 )
- {
- vpERROR_TRACE ( "Can't create window." );
- throw ( vpDisplayException ( vpDisplayException::cannotOpenWindowError,
- "Can't create window." ) ) ;
+ 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."));
}
-
//
// Create color table for 8 and 16 bits screen
//
- if ( screen_depth == 8 )
- {
- lut = XCreateColormap ( display, window,
- DefaultVisual ( display, screen ), AllocAll ) ;
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ if (screen_depth == 8) {
+ lut = XCreateColormap(display, window, DefaultVisual(display, screen), AllocAll);
+ xcolor.flags = DoRed | DoGreen | DoBlue;
- for ( unsigned int i = 0 ; i < 256 ; i++ )
- {
- xcolor.pixel = i ;
+ for (unsigned int i = 0; i < 256; i++) {
+ xcolor.pixel = i;
xcolor.red = 256 * i;
xcolor.green = 256 * i;
xcolor.blue = 256 * i;
- XStoreColor ( display, lut, &xcolor );
+ XStoreColor(display, lut, &xcolor);
}
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
}
- else if ( screen_depth == 16 )
- {
- for ( unsigned int i = 0; i < 256; i ++ )
- {
+ else if (screen_depth == 16) {
+ for (unsigned int i = 0; i < 256; i++) {
xcolor.pad = 0;
xcolor.red = xcolor.green = xcolor.blue = 256 * i;
- if ( XAllocColor ( display, lut, &xcolor ) == 0 )
- {
- vpERROR_TRACE ( "Can't allocate 256 colors. Only %d allocated.", i );
- throw ( vpDisplayException ( vpDisplayException::colorAllocError,
- "Can't allocate 256 colors." ) ) ;
+ if (XAllocColor(display, lut, &xcolor) == 0) {
+ vpERROR_TRACE("Can't allocate 256 colors. Only %d allocated.", i);
+ throw(vpDisplayException(vpDisplayException::colorAllocError, "Can't allocate 256 colors."));
}
colortable[i] = xcolor.pixel;
}
- XSetWindowColormap ( display, window, lut ) ;
- XInstallColormap ( display, lut ) ;
+ XSetWindowColormap(display, window, lut);
+ XInstallColormap(display, lut);
- Visual *visual = DefaultVisual (display, screen);
+ Visual *visual = DefaultVisual(display, screen);
RMask = visual->red_mask;
GMask = visual->green_mask;
BMask = visual->blue_mask;
- RShift = 15 - getMsb(RMask); /* these are right-shifts */
+ RShift = 15 - getMsb(RMask); /* these are right-shifts */
GShift = 15 - getMsb(GMask);
BShift = 15 - getMsb(BMask);
}
@@ -1281,8 +1209,7 @@ void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const std:
//
// Create colors for overlay
//
- switch ( screen_depth )
- {
+ switch (screen_depth) {
case 8:
// Color BLACK: default set to 0
@@ -1291,332 +1218,324 @@ void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const std:
// Color LIGHT GRAY.
pcolor = vpColor::lightGray;
- xcolor.pixel = 254 ; // affected to 254
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 254; // affected to 254
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color GRAY.
pcolor = vpColor::gray;
- xcolor.pixel = 253 ; // affected to 253
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 253; // affected to 253
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GRAY.
pcolor = vpColor::darkGray;
- xcolor.pixel = 252 ; // affected to 252
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 252; // affected to 252
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT RED.
pcolor = vpColor::lightRed;
- xcolor.pixel = 251 ; // affected to 251
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 251; // affected to 251
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color RED.
pcolor = vpColor::red;
- xcolor.pixel = 250 ; // affected to 250
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 250; // affected to 250
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color DARK RED.
pcolor = vpColor::darkRed;
- xcolor.pixel = 249 ; // affected to 249
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 249; // affected to 249
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT GREEN.
pcolor = vpColor::lightGreen;
- xcolor.pixel = 248 ; // affected to 248
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 248; // affected to 248
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color GREEN.
pcolor = vpColor::green;
- xcolor.pixel = 247; // affected to 247
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 247; // affected to 247
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color DARK GREEN.
pcolor = vpColor::darkGreen;
- xcolor.pixel = 246 ; // affected to 246
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 246; // affected to 246
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color LIGHT BLUE.
pcolor = vpColor::lightBlue;
- xcolor.pixel = 245 ; // affected to 245
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 245; // affected to 245
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color BLUE.
pcolor = vpColor::blue;
- xcolor.pixel = 244; // affected to 244
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 244; // affected to 244
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color DARK BLUE.
pcolor = vpColor::darkBlue;
- xcolor.pixel = 243 ; // affected to 243
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 243; // affected to 243
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color YELLOW.
pcolor = vpColor::yellow;
- xcolor.pixel = 242; // affected to 242
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 242; // affected to 242
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color ORANGE.
pcolor = vpColor::orange;
- xcolor.pixel = 241; // affected to 241
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 241; // affected to 241
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color CYAN.
pcolor = vpColor::cyan;
- xcolor.pixel = 240; // affected to 240
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 240; // affected to 240
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
// Color PURPLE.
pcolor = vpColor::purple;
- xcolor.pixel = 239; // affected to 239
- xcolor.red = 256 * pcolor.R;
- xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XStoreColor ( display, lut, &xcolor );
+ xcolor.pixel = 239; // affected to 239
+ xcolor.red = 256 * pcolor.R;
+ xcolor.green = 256 * pcolor.G;
+ xcolor.blue = 256 * pcolor.B;
+ XStoreColor(display, lut, &xcolor);
break;
case 16:
- case 24:
- {
- xcolor.flags = DoRed | DoGreen | DoBlue ;
+ case 24: {
+ xcolor.flags = DoRed | DoGreen | DoBlue;
// Couleur BLACK.
pcolor = vpColor::black;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_black] = xcolor.pixel;
// Color WHITE.
pcolor = vpColor::white;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_white] = xcolor.pixel;
// Color LIGHT GRAY.
pcolor = vpColor::lightGray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGray] = xcolor.pixel;
// Color GRAY.
pcolor = vpColor::gray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_gray] = xcolor.pixel;
// Color DARK GRAY.
pcolor = vpColor::darkGray;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGray] = xcolor.pixel;
// Color LIGHT RED.
pcolor = vpColor::lightRed;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightRed] = xcolor.pixel;
// Color RED.
pcolor = vpColor::red;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_red] = xcolor.pixel;
// Color DARK RED.
pcolor = vpColor::darkRed;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkRed] = xcolor.pixel;
// Color LIGHT GREEN.
pcolor = vpColor::lightGreen;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightGreen] = xcolor.pixel;
// Color GREEN.
pcolor = vpColor::green;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_green] = xcolor.pixel;
// Color DARK GREEN.
pcolor = vpColor::darkGreen;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkGreen] = xcolor.pixel;
// Color LIGHT BLUE.
pcolor = vpColor::lightBlue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_lightBlue] = xcolor.pixel;
// Color BLUE.
pcolor = vpColor::blue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_blue] = xcolor.pixel;
// Color DARK BLUE.
pcolor = vpColor::darkBlue;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_darkBlue] = xcolor.pixel;
// Color YELLOW.
pcolor = vpColor::yellow;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_yellow] = xcolor.pixel;
// Color ORANGE.
pcolor = vpColor::orange;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_orange] = xcolor.pixel;
// Color CYAN.
pcolor = vpColor::cyan;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_cyan] = xcolor.pixel;
// Color PURPLE.
pcolor = vpColor::purple;
- xcolor.pad = 0;
- xcolor.red = 256 * pcolor.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * pcolor.R;
xcolor.green = 256 * pcolor.G;
- xcolor.blue = 256 * pcolor.B;
- XAllocColor ( display, lut, &xcolor );
+ xcolor.blue = 256 * pcolor.B;
+ XAllocColor(display, lut, &xcolor);
x_color[vpColor::id_purple] = xcolor.pixel;
break;
}
}
- XSetStandardProperties ( display, window, this->m_title.c_str(), this->m_title.c_str(), None, 0, 0, &hints );
- XMapWindow ( display, window ) ;
+ 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);
+ XSelectInput(display, window,
+ ExposureMask | ButtonPressMask | ButtonReleaseMask | KeyPressMask | KeyReleaseMask |
+ StructureNotifyMask | PointerMotionMask);
/* Creation du contexte graphique */
values.plane_mask = AllPlanes;
values.fill_style = FillSolid;
- values.foreground = WhitePixel ( display, screen );
- values.background = BlackPixel ( display, screen );
- context = XCreateGC ( display, window,
- GCPlaneMask | GCFillStyle | GCForeground | GCBackground,
- &values );
+ values.foreground = WhitePixel(display, screen);
+ values.background = BlackPixel(display, screen);
+ context = XCreateGC(display, window, GCPlaneMask | GCFillStyle | GCForeground | GCBackground, &values);
- if ( context == NULL )
- {
- vpERROR_TRACE ( "Can't create graphics context." );
- throw ( vpDisplayException ( vpDisplayException::XWindowsError,
- "Can't create graphics context" ) ) ;
+ if (context == NULL) {
+ vpERROR_TRACE("Can't create graphics context.");
+ throw(vpDisplayException(vpDisplayException::XWindowsError, "Can't create graphics context"));
}
// Pixmap creation.
- pixmap = XCreatePixmap ( display, window, m_width, m_height, screen_depth );
+ pixmap = XCreatePixmap(display, window, m_width, m_height, screen_depth);
// Hangs when forward X11 is used to send the display to an other computer
// do
@@ -1624,21 +1543,20 @@ void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const std:
// while ( event.xany.type != Expose );
{
- Ximage = XCreateImage ( display, DefaultVisual ( display, screen ),
- screen_depth, ZPixmap, 0, NULL,
- m_width, m_height, XBitmapPad ( display ), 0 );
+ 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 ;
+ m_displayHasBeenInitialized = true;
- XSync ( display, true );
+ XSync(display, true);
- XStoreName ( display, window, m_title.c_str() );
+ XStoreName(display, window, m_title.c_str());
}
-/*!
+/*!
Set the font used to display a text in overlay. The display is
performed using displayCharString().
@@ -1652,28 +1570,20 @@ void vpDisplayX::init ( unsigned int w, unsigned int h, int x, int y, const std:
\sa displayCharString()
*/
-void vpDisplayX::setFont( const std::string &font )
+void vpDisplayX::setFont(const std::string &font)
{
- if ( m_displayHasBeenInitialized )
- {
- if (!font.empty())
- {
- try
- {
+ if (m_displayHasBeenInitialized) {
+ if (!font.empty()) {
+ try {
Font stringfont;
- stringfont = XLoadFont (display, font.c_str()) ; //"-adobe-times-bold-r-normal--18*");
- XSetFont (display, context, stringfont);
- }
- catch(...)
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,"Bad font" ) ) ;
+ stringfont = XLoadFont(display, font.c_str()); //"-adobe-times-bold-r-normal--18*");
+ XSetFont(display, context, stringfont);
+ } catch (...) {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Bad font"));
}
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -1681,39 +1591,32 @@ void vpDisplayX::setFont( const std::string &font )
Set the window title.
\param title : Window title.
*/
-void
-vpDisplayX::setTitle(const std::string &title)
+void vpDisplayX::setTitle(const std::string &title)
{
- if ( m_displayHasBeenInitialized )
- {
+ if (m_displayHasBeenInitialized) {
m_title = title;
- if(! title.empty())
- XStoreName ( display, window, m_title.c_str() );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ if (!title.empty())
+ XStoreName(display, window, m_title.c_str());
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
/*!
Set the window position in the screen.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
\exception vpDisplayException::notInitializedError : If the video
device is not initialized.
*/
void vpDisplayX::setWindowPosition(int winx, int winy)
{
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
XMoveWindow(display, window, winx, winy);
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -1728,42 +1631,37 @@ void vpDisplayX::setWindowPosition(int winx, int winy)
\sa init(), closeDisplay()
*/
-void vpDisplayX::displayImage ( const vpImage<unsigned char> &I )
+void vpDisplayX::displayImage(const vpImage<unsigned char> &I)
{
- if ( m_displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 8:
- {
+ if (m_displayHasBeenInitialized) {
+ switch (screen_depth) {
+ case 8: {
// Correction de l'image de facon a liberer les niveaux de gris
// ROUGE, VERT, BLEU, JAUNE
unsigned char nivGrisMax = 255 - vpColor::id_unknown;
if (m_scale == 1) {
- unsigned char *src_8 = ( unsigned char * ) I.bitmap;
- unsigned char *dst_8 = ( unsigned char * ) Ximage->data;
+ unsigned char *src_8 = (unsigned char *)I.bitmap;
+ unsigned char *dst_8 = (unsigned char *)Ximage->data;
unsigned int i = 0;
unsigned int size = m_width * m_height;
- while ( i < size )
- {
- unsigned char nivGris = src_8[i] ;
- if ( nivGris > nivGrisMax )
+ while (i < size) {
+ unsigned char nivGris = src_8[i];
+ if (nivGris > nivGrisMax)
dst_8[i] = 255;
else
dst_8[i] = nivGris;
- i++ ;
+ i++;
}
- }
- else {
+ } else {
// Correction de l'image de facon a liberer les niveaux de gris
// ROUGE, VERT, BLEU, JAUNE
- unsigned char *dst_8 = ( unsigned char * ) Ximage->data;
+ unsigned char *dst_8 = (unsigned char *)Ximage->data;
unsigned int k = 0;
- for (unsigned int i=0; i<m_height; i++) {
- for (unsigned int j=0; j<m_width; j++) {
- unsigned char nivGris = I[i*m_scale][j*m_scale];
- if ( nivGris > nivGrisMax )
+ for (unsigned int i = 0; i < m_height; i++) {
+ for (unsigned int j = 0; j < m_width; j++) {
+ unsigned char nivGris = I[i * m_scale][j * m_scale];
+ if (nivGris > nivGrisMax)
dst_8[k++] = 255;
else
dst_8[k++] = nivGris;
@@ -1772,110 +1670,98 @@ void vpDisplayX::displayImage ( const vpImage<unsigned char> &I )
}
// Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height);
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
- case 16:
- {
+ case 16: {
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 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]] ;
+ 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 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]];
}
}
- }
- else {
- for ( unsigned int i = 0; i < m_height ; 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=0 ; j < m_width; j++ )
- {
- * ( dst_16 + j ) = ( unsigned short ) colortable[I[i*m_scale][j*m_scale]] ;
+ } else {
+ for (unsigned int i = 0; i < m_height; 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 = 0; j < m_width; j++) {
+ *(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 );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height);
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
case 24:
- default:
- {
- unsigned char *dst_32 = ( unsigned char* ) Ximage->data;
+ default: {
+ unsigned char *dst_32 = (unsigned char *)Ximage->data;
if (m_scale == 1) {
- unsigned int size_ = m_width * m_height ;
- unsigned char *bitmap = I.bitmap ;
+ unsigned int size_ = m_width * m_height;
+ unsigned char *bitmap = I.bitmap;
unsigned char *n = I.bitmap + size_;
- //for (unsigned int i = 0; i < size; i++) // suppression de l'iterateur i
+ // for (unsigned int i = 0; i < size; i++) // suppression de
+ // l'iterateur i
if (XImageByteOrder(display) == 1) {
// big endian
- while ( bitmap < n )
- {
- unsigned char val = * ( bitmap++ );
- * ( dst_32 ++ ) = vpRGBa::alpha_default;
- * ( dst_32 ++ ) = val; // Red
- * ( dst_32 ++ ) = val; // Green
- * ( dst_32 ++ ) = val; // Blue
+ while (bitmap < n) {
+ unsigned char val = *(bitmap++);
+ *(dst_32++) = vpRGBa::alpha_default;
+ *(dst_32++) = val; // Red
+ *(dst_32++) = val; // Green
+ *(dst_32++) = val; // Blue
}
- }
- else {
+ } else {
// little endian
- while ( bitmap < n )
- {
- unsigned char val = * ( bitmap++ );
- * ( dst_32 ++ ) = val; // Blue
- * ( dst_32 ++ ) = val; // Green
- * ( dst_32 ++ ) = val; // Red
- * ( dst_32 ++ ) = vpRGBa::alpha_default;
+ while (bitmap < n) {
+ unsigned char val = *(bitmap++);
+ *(dst_32++) = val; // Blue
+ *(dst_32++) = val; // Green
+ *(dst_32++) = val; // Red
+ *(dst_32++) = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
if (XImageByteOrder(display) == 1) {
// big endian
- for (unsigned int i=0; i<m_height; i++) {
- for (unsigned int j=0; j<m_width; j++) {
- unsigned char val = I[i*m_scale][j*m_scale];
- * ( dst_32 ++ ) = vpRGBa::alpha_default;
- * ( dst_32 ++ ) = val; // Red
- * ( dst_32 ++ ) = val; // Green
- * ( dst_32 ++ ) = val; // Blue
+ for (unsigned int i = 0; i < m_height; i++) {
+ for (unsigned int j = 0; j < m_width; j++) {
+ unsigned char val = I[i * m_scale][j * m_scale];
+ *(dst_32++) = vpRGBa::alpha_default;
+ *(dst_32++) = val; // Red
+ *(dst_32++) = val; // Green
+ *(dst_32++) = val; // Blue
}
}
- }
- else {
+ } else {
// little endian
- for (unsigned int i=0; i<m_height; i++) {
- for (unsigned int j=0; j<m_width; j++) {
- unsigned char val = I[i*m_scale][j*m_scale];
- * ( dst_32 ++ ) = val; // Blue
- * ( dst_32 ++ ) = val; // Green
- * ( dst_32 ++ ) = val; // Red
- * ( dst_32 ++ ) = vpRGBa::alpha_default;
+ for (unsigned int i = 0; i < m_height; i++) {
+ for (unsigned int j = 0; j < m_width; j++) {
+ unsigned char val = I[i * m_scale][j * m_scale];
+ *(dst_32++) = val; // Blue
+ *(dst_32++) = val; // Green
+ *(dst_32++) = val; // Red
+ *(dst_32++) = vpRGBa::alpha_default;
}
}
}
}
// Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ 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" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
/*!
@@ -1889,80 +1775,71 @@ void vpDisplayX::displayImage ( const vpImage<unsigned char> &I )
\sa init(), closeDisplay()
*/
-void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
+void vpDisplayX::displayImage(const vpImage<vpRGBa> &I)
{
- if ( m_displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
+ if (m_displayHasBeenInitialized) {
+ switch (screen_depth) {
case 16: {
- vpRGBa* bitmap = I.bitmap;
+ vpRGBa *bitmap = I.bitmap;
unsigned int r, g, b;
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 short *dst_16 = (unsigned short *) dst_8;
- for ( unsigned int j=0 ; j < m_width; j++ )
- {
+ for (unsigned int i = 0; i < m_height; 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 = 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 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];
+ } else {
+ for (unsigned int i = 0; i < m_height; 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 = 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 );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ XPutImage(display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height);
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
case 24:
- case 32:
- {
+ case 32: {
/*
- * 32-bit source, 24/32-bit destination
- */
- unsigned char *dst_32 = NULL;
- dst_32 = ( unsigned char* ) Ximage->data;
+ * 32-bit source, 24/32-bit destination
+ */
+ unsigned char *dst_32 = NULL;
+ dst_32 = (unsigned char *)Ximage->data;
if (m_scale == 1) {
- vpRGBa* bitmap = I.bitmap;
+ vpRGBa *bitmap = I.bitmap;
unsigned int sizeI = m_width * m_height;
if (XImageByteOrder(display) == 1) {
// big endian
- for ( unsigned int i = 0; i < sizeI ; i++ ) {
+ for (unsigned int i = 0; i < sizeI; i++) {
*(dst_32++) = bitmap->A;
*(dst_32++) = bitmap->R;
*(dst_32++) = bitmap->G;
*(dst_32++) = bitmap->B;
bitmap++;
}
- }
- else {
+ } else {
// little endian
- for ( unsigned int i = 0; i < sizeI; i++ ) {
+ for (unsigned int i = 0; i < sizeI; i++) {
*(dst_32++) = bitmap->B;
*(dst_32++) = bitmap->G;
*(dst_32++) = bitmap->R;
@@ -1970,25 +1847,23 @@ void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
bitmap++;
}
}
- }
- else {
+ } else {
if (XImageByteOrder(display) == 1) {
// big endian
- for (unsigned int i=0; i<m_height; i++) {
- for (unsigned int j=0; j<m_width; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
+ for (unsigned int i = 0; i < m_height; i++) {
+ for (unsigned int j = 0; j < m_width; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
*(dst_32++) = val.A;
*(dst_32++) = val.R;
*(dst_32++) = val.G;
*(dst_32++) = val.B;
}
}
- }
- else {
+ } else {
// little endian
- for (unsigned int i=0; i<m_height; i++) {
- for (unsigned int j=0; j<m_width; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
+ for (unsigned int i = 0; i < m_height; i++) {
+ for (unsigned int j = 0; j < m_width; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
*(dst_32++) = val.B;
*(dst_32++) = val.G;
*(dst_32++) = val.R;
@@ -1999,19 +1874,16 @@ void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
}
// Affichage de l'image dans la Pixmap.
- XPutImage ( display, pixmap, context, Ximage, 0, 0, 0, 0, m_width, m_height );
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ 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 ) ) ;
+ throw(vpDisplayException(vpDisplayException::depthNotSupportedError,
+ "Unsupported depth (%d bpp) for color display", screen_depth));
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2025,34 +1897,28 @@ void vpDisplayX::displayImage ( const vpImage<vpRGBa> &I )
\param bitmap : Pointer to the image bitmap.
\sa init(), closeDisplay()
-*/
-void vpDisplayX::displayImage ( const unsigned char *bitmap )
+*/
+void vpDisplayX::displayImage(const unsigned char *bitmap)
{
- if ( m_displayHasBeenInitialized )
- {
- unsigned char *dst_32 = ( unsigned char* ) Ximage->data;
- for ( unsigned int i = 0; i < m_width * m_height; i++ )
- {
- * ( dst_32 ++ ) = *bitmap; // red component.
- * ( dst_32 ++ ) = *bitmap; // green component.
- * ( dst_32 ++ ) = *bitmap; // blue component.
- * ( dst_32 ++ ) = *bitmap; // luminance component.
- bitmap ++;
+ if (m_displayHasBeenInitialized) {
+ unsigned char *dst_32 = (unsigned char *)Ximage->data;
+ for (unsigned int i = 0; i < m_width * m_height; i++) {
+ *(dst_32++) = *bitmap; // red component.
+ *(dst_32++) = *bitmap; // green component.
+ *(dst_32++) = *bitmap; // blue component.
+ *(dst_32++) = *bitmap; // luminance component.
+ bitmap++;
}
// Affichage de l'image dans la Pixmap.
- 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" ) ) ;
+ 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"));
}
}
-
/*!
Display a selection of the gray level image \e I (8bits).
@@ -2061,44 +1927,39 @@ void vpDisplayX::displayImage ( const unsigned char *bitmap )
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w, h : Width and height of the region of interest
-
+
\sa init(), closeDisplay()
*/
-void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I, const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
+void vpDisplayX::displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if ( m_displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
- case 8:
- {
+ if (m_displayHasBeenInitialized) {
+ switch (screen_depth) {
+ case 8: {
// Correction de l'image de facon a liberer les niveaux de gris
// ROUGE, VERT, BLEU, JAUNE
unsigned char nivGrisMax = 255 - vpColor::id_unknown;
if (m_scale == 1) {
- unsigned char *src_8 = ( unsigned char * ) I.bitmap;
- unsigned char *dst_8 = ( unsigned char * ) Ximage->data;
+ unsigned char *src_8 = (unsigned char *)I.bitmap;
+ unsigned char *dst_8 = (unsigned char *)Ximage->data;
unsigned int iwidth = I.getWidth();
- src_8 = src_8 + (int)(iP.get_i()*iwidth+ iP.get_j());
- dst_8 = dst_8 + (int)(iP.get_i()*m_width+ iP.get_j());
+ src_8 = src_8 + (int)(iP.get_i() * iwidth + iP.get_j());
+ dst_8 = dst_8 + (int)(iP.get_i() * m_width + iP.get_j());
unsigned int i = 0;
- while (i < h)
- {
+ while (i < h) {
unsigned int j = 0;
- while (j < w)
- {
- unsigned char nivGris = *(src_8+j);
- if ( nivGris > nivGrisMax )
- *(dst_8+j) = 255;
+ while (j < w) {
+ unsigned char nivGris = *(src_8 + j);
+ if (nivGris > nivGrisMax)
+ *(dst_8 + j) = 255;
else
- *(dst_8+j) = nivGris;
+ *(dst_8 + j) = nivGris;
j++;
}
src_8 = src_8 + iwidth;
@@ -2106,135 +1967,127 @@ void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I, const vpImag
i++;
}
- XPutImage ( display, pixmap, context, Ximage, (int)iP.get_u(), (int)iP.get_v(), (int)iP.get_u(), (int)iP.get_v(), w, h );
- }
- else {
+ 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_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);
unsigned int i_min_ = (unsigned int)i_min;
unsigned int i_max_ = (unsigned int)i_max;
unsigned int j_min_ = (unsigned int)j_min;
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*m_width;
- for (unsigned int j=j_min_; j<j_max_; j++) {
- unsigned char nivGris = I[i*m_scale][j*m_scale];
- if ( nivGris > nivGrisMax )
+ for (unsigned int i = i_min_; i < i_max_; i++) {
+ unsigned char *dst_8 = (unsigned char *)Ximage->data + i * m_width;
+ for (unsigned int j = j_min_; j < j_max_; j++) {
+ unsigned char nivGris = I[i * m_scale][j * m_scale];
+ if (nivGris > nivGrisMax)
dst_8[j] = 255;
else
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.
- XSetWindowBackgroundPixmap ( display, window, pixmap );
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
- case 16:
- {
+ 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;
- 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++ )
- {
- * ( dst_16 + j ) = ( unsigned short ) colortable[I[i][j]] ;
+ 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;
+ 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++) {
+ *(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 );
- }
- 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);
+ 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);
unsigned int i_min_ = (unsigned int)i_min;
unsigned int i_max_ = (unsigned int)i_max;
unsigned int j_min_ = (unsigned int)j_min;
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 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]] ;
+ for (unsigned int i = i_min_; i < i_max_; 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 = j_min_; j < j_max_; j++) {
+ *(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 );
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
case 24:
- default:
- {
+ 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
unsigned int i = 0;
- while (i < h)
- {
+ while (i < h) {
unsigned int j = 0;
- while (j < w)
- {
- unsigned char val = *(src_8+j);
- *(dst_32+4*j) = vpRGBa::alpha_default;
- *(dst_32+4*j+1) = val;
- *(dst_32+4*j+2) = val;
- *(dst_32+4*j+3) = val;
+ while (j < w) {
+ unsigned char val = *(src_8 + j);
+ *(dst_32 + 4 * j) = vpRGBa::alpha_default;
+ *(dst_32 + 4 * j + 1) = val;
+ *(dst_32 + 4 * j + 2) = val;
+ *(dst_32 + 4 * j + 3) = val;
j++;
}
src_8 = src_8 + iwidth;
- dst_32 = dst_32 + 4*m_width;
+ dst_32 = dst_32 + 4 * m_width;
i++;
}
- }
- else {
+ } else {
// little endian
unsigned int i = 0;
- while (i < h)
- {
+ while (i < h) {
unsigned int j = 0;
- while (j < w)
- {
- unsigned char val = *(src_8+j);
- *(dst_32+4*j) = val;
- *(dst_32+4*j+1) = val;
- *(dst_32+4*j+2) = val;
- *(dst_32+4*j+3) = vpRGBa::alpha_default;
+ while (j < w) {
+ unsigned char val = *(src_8 + j);
+ *(dst_32 + 4 * j) = val;
+ *(dst_32 + 4 * j + 1) = val;
+ *(dst_32 + 4 * j + 2) = val;
+ *(dst_32 + 4 * j + 3) = vpRGBa::alpha_default;
j++;
}
src_8 = src_8 + iwidth;
- dst_32 = dst_32 + 4*m_width;
+ dst_32 = dst_32 + 4 * m_width;
i++;
}
}
- 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);
+ 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);
unsigned int i_min_ = (unsigned int)i_min;
unsigned int i_max_ = (unsigned int)i_max;
@@ -2243,47 +2096,42 @@ void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I, const vpImag
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);
- 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;
- * ( dst_32 ++ ) = val;
- * ( dst_32 ++ ) = val;
- * ( dst_32 ++ ) = val;
+ 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);
+ 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;
+ *(dst_32++) = val;
+ *(dst_32++) = val;
+ *(dst_32++) = val;
}
}
- }
- else {
+ } 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);
- for (unsigned int j=j_min_; j<j_max_; j++) {
- unsigned char val = I[i*m_scale][j*m_scale];
- * ( dst_32 ++ ) = val;
- * ( dst_32 ++ ) = val;
- * ( dst_32 ++ ) = val;
- * ( dst_32 ++ ) = vpRGBa::alpha_default;
+ 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);
+ for (unsigned int j = j_min_; j < j_max_; j++) {
+ unsigned char val = I[i * m_scale][j * m_scale];
+ *(dst_32++) = val;
+ *(dst_32++) = val;
+ *(dst_32++) = val;
+ *(dst_32++) = vpRGBa::alpha_default;
}
}
}
- 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 );
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
-
/*!
Display a selection of the color image \e I in RGBa format (32bits).
@@ -2292,87 +2140,80 @@ void vpDisplayX::displayImageROI ( const vpImage<unsigned char> &I, const vpImag
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param w, h : Width and height of the region of interest
-
+
\sa init(), closeDisplay()
*/
-void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP,
- const unsigned int w, const unsigned int h )
+void vpDisplayX::displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int w,
+ const unsigned int h)
{
- if ( m_displayHasBeenInitialized )
- {
- switch ( screen_depth )
- {
+ if (m_displayHasBeenInitialized) {
+ switch (screen_depth) {
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;
- 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 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++) {
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 );
- }
- else {
+ 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_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);
unsigned int i_min_ = (unsigned int)i_min;
unsigned int i_max_ = (unsigned int)i_max;
unsigned int j_min_ = (unsigned int)j_min;
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 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];
+ for (unsigned int i = i_min_; i < i_max_; 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 = 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 );
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
}
case 24:
- case 32:
- {
+ case 32: {
/*
- * 32-bit source, 24/32-bit destination
- */
+ * 32-bit source, 24/32-bit destination
+ */
if (m_scale == 1) {
- unsigned char *dst_32 = ( unsigned char* ) Ximage->data;
- vpRGBa* src_32 = I.bitmap;
+ unsigned char *dst_32 = (unsigned char *)Ximage->data;
+ vpRGBa *src_32 = I.bitmap;
unsigned int iwidth = I.getWidth();
- src_32 = src_32 + (int)(iP.get_i()*iwidth+ iP.get_j());
- dst_32 = dst_32 + (int)(iP.get_i()*4*m_width+ iP.get_j()*4);
+ src_32 = src_32 + (int)(iP.get_i() * iwidth + iP.get_j());
+ dst_32 = dst_32 + (int)(iP.get_i() * 4 * m_width + iP.get_j() * 4);
unsigned int i = 0;
@@ -2381,44 +2222,43 @@ void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &
while (i < h) {
unsigned int j = 0;
while (j < w) {
- *(dst_32+4*j) = (src_32+j)->A;
- *(dst_32+4*j+1) = (src_32+j)->R;
- *(dst_32+4*j+2) = (src_32+j)->G;
- *(dst_32+4*j+3) = (src_32+j)->B;
+ *(dst_32 + 4 * j) = (src_32 + j)->A;
+ *(dst_32 + 4 * j + 1) = (src_32 + j)->R;
+ *(dst_32 + 4 * j + 2) = (src_32 + j)->G;
+ *(dst_32 + 4 * j + 3) = (src_32 + j)->B;
j++;
}
src_32 = src_32 + iwidth;
- dst_32 = dst_32 + 4*m_width;
+ dst_32 = dst_32 + 4 * m_width;
i++;
}
- }
- else {
+ } else {
// little endian
while (i < h) {
unsigned int j = 0;
while (j < w) {
- *(dst_32+4*j) = (src_32+j)->B;
- *(dst_32+4*j+1) = (src_32+j)->G;
- *(dst_32+4*j+2) = (src_32+j)->R;
- *(dst_32+4*j+3) = (src_32+j)->A;
+ *(dst_32 + 4 * j) = (src_32 + j)->B;
+ *(dst_32 + 4 * j + 1) = (src_32 + j)->G;
+ *(dst_32 + 4 * j + 2) = (src_32 + j)->R;
+ *(dst_32 + 4 * j + 3) = (src_32 + j)->A;
j++;
}
src_32 = src_32 + iwidth;
- dst_32 = dst_32 + 4*m_width;
+ dst_32 = dst_32 + 4 * m_width;
i++;
}
}
- 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);
+ 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);
unsigned int i_min_ = (unsigned int)i_min;
unsigned int i_max_ = (unsigned int)i_max;
@@ -2427,23 +2267,22 @@ void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &
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);
- for (unsigned int j=j_min_; j<j_max_; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
+ 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);
+ for (unsigned int j = j_min_; j < j_max_; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
*(dst_32++) = val.A;
*(dst_32++) = val.R;
*(dst_32++) = val.G;
*(dst_32++) = val.B;
}
}
- }
- else {
+ } 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);
- for (unsigned int j=j_min_; j<j_max_; j++) {
- vpRGBa val = I[i*m_scale][j*m_scale];
+ 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);
+ for (unsigned int j = j_min_; j < j_max_; j++) {
+ vpRGBa val = I[i * m_scale][j * m_scale];
*(dst_32++) = val.B;
*(dst_32++) = val.G;
*(dst_32++) = val.R;
@@ -2451,22 +2290,18 @@ void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &
}
}
}
- 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 );
+ XSetWindowBackgroundPixmap(display, window, pixmap);
break;
-
}
default:
- throw ( vpDisplayException ( vpDisplayException::depthNotSupportedError,
- "Unsupported depth (%d bpp) for color display", screen_depth ) ) ;
+ throw(vpDisplayException(vpDisplayException::depthNotSupportedError,
+ "Unsupported depth (%d bpp) for color display", screen_depth));
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2479,30 +2314,28 @@ void vpDisplayX::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &
*/
void vpDisplayX::closeDisplay()
{
- if ( m_displayHasBeenInitialized )
- {
- if ( ximage_data_init == true )
- free ( Ximage->data );
+ if (m_displayHasBeenInitialized) {
+ if (ximage_data_init == true)
+ free(Ximage->data);
Ximage->data = NULL;
- XDestroyImage ( Ximage );
+ XDestroyImage(Ximage);
- XFreePixmap ( display, pixmap );
+ XFreePixmap(display, pixmap);
- XFreeGC ( display, context );
- XDestroyWindow ( display, window );
- XCloseDisplay ( display );
+ XFreeGC(display, context);
+ XDestroyWindow(display, window);
+ XCloseDisplay(display);
m_displayHasBeenInitialized = false;
if (x_color != NULL) {
- delete [] x_color;
+ delete[] x_color;
x_color = NULL;
}
}
}
-
/*!
Flushes the X buffer.
It's necessary to use this function to see the results of any drawing.
@@ -2510,15 +2343,11 @@ void vpDisplayX::closeDisplay()
*/
void vpDisplayX::flushDisplay()
{
- if ( m_displayHasBeenInitialized )
- {
- XClearWindow ( display, window );
- XFlush ( display );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ if (m_displayHasBeenInitialized) {
+ XClearWindow(display, window);
+ XFlush(display);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2531,49 +2360,40 @@ void vpDisplayX::flushDisplay()
*/
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 );
- XFlush ( display );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ 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);
+ XFlush(display);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
-
/*!
Set the window backgroud to \e color.
\param color : Background color.
*/
-void vpDisplayX::clearDisplay ( const vpColor &color )
+void vpDisplayX::clearDisplay(const vpColor &color)
{
- if ( m_displayHasBeenInitialized )
- {
+ if (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
- XSetWindowBackground ( display, window, x_color[color.id] );
+ XSetWindowBackground(display, window, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ XSetForeground(display, context, xcolor.pixel);
}
- XClearWindow ( display, window );
+ XClearWindow(display, window);
- XFreePixmap ( display, pixmap );
+ XFreePixmap(display, pixmap);
// Pixmap creation.
- pixmap = XCreatePixmap ( display, window, m_width, m_height, screen_depth );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ pixmap = XCreatePixmap(display, window, m_width, m_height, screen_depth);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2584,52 +2404,43 @@ 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() ;
- double b = ip2.get_j() - ip1.get_j() ;
- double lg = sqrt ( vpMath::sqr ( a ) + vpMath::sqr ( b ) ) ;
-
- //if ( ( a==0 ) && ( b==0 ) )
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon() )&&(std::fabs(b) <= std::numeric_limits<double>::epsilon()) )
- {
+ if (m_displayHasBeenInitialized) {
+ double a = ip2.get_i() - ip1.get_i();
+ double b = ip2.get_j() - ip1.get_j();
+ double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
+
+ // if ( ( a==0 ) && ( b==0 ) )
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
// DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
+ } else {
+ a /= lg;
+ b /= lg;
vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
+ ip3.set_i(ip2.get_i() - w * a);
+ ip3.set_j(ip2.get_j() - w * b);
vpImagePoint ip4;
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
+ ip4.set_i(ip3.get_i() - b * h);
+ ip4.set_j(ip3.get_j() + a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
+ ip4.set_i(ip3.get_i() + b * h);
+ ip4.set_j(ip3.get_j() - a * h);
- if (lg > 2*vpImagePoint::distance(ip2, ip4) )
- displayLine ( ip2, ip4, color, thickness ) ;
+ if (lg > 2 * vpImagePoint::distance(ip2, ip4))
+ displayLine(ip2, ip4, color, thickness);
- displayLine ( ip1, ip2, color, thickness ) ;
+ displayLine(ip1, ip2, color, thickness);
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2644,30 +2455,23 @@ 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 (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ 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 ) );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ 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"));
}
}
@@ -2680,48 +2484,36 @@ void vpDisplayX::displayCharString ( const vpImagePoint &ip,
\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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ 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 )
- {
- 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, 23040 ); /* 23040 = 360*64 */
+ 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,
+ 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,
+ 23040); /* 23040 = 360*64 */
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -2732,34 +2524,28 @@ 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 )
- {
+ if (m_displayHasBeenInitialized) {
double i = ip.get_i();
double j = ip.get_j();
vpImagePoint ip1, ip2;
- ip1.set_i( i-cross_size/2 );
- ip1.set_j( j );
- ip2.set_i( i+cross_size/2 );
- ip2.set_j( j );
- displayLine ( ip1, ip2, color, thickness ) ;
+ ip1.set_i(i - cross_size / 2);
+ ip1.set_j(j);
+ ip2.set_i(i + cross_size / 2);
+ ip2.set_j(j);
+ displayLine(ip1, ip2, color, thickness);
- ip1.set_i( i );
- ip1.set_j( j-cross_size/2 );
- ip2.set_i( i );
- ip2.set_j( j+cross_size/2 );
+ ip1.set_i(i);
+ ip1.set_j(j - cross_size / 2);
+ ip2.set_i(i);
+ ip2.set_j(j + cross_size / 2);
- displayLine ( ip1, ip2, color, thickness ) ;
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ displayLine(ip1, ip2, color, thickness);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
/*!
@@ -2768,39 +2554,30 @@ void vpDisplayX::displayCross ( const vpImagePoint &ip,
\param color : Line color.
\param thickness : Line thickness.
*/
-void vpDisplayX::displayDotLine ( const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
- unsigned int thickness )
+void vpDisplayX::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- if ( m_displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ 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 ) );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ 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"));
}
}
@@ -2810,39 +2587,30 @@ 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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ 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 ) );
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ 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"));
}
}
@@ -2852,42 +2620,33 @@ void vpDisplayX::displayLine ( const vpImagePoint &ip1,
\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 (m_displayHasBeenInitialized) {
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ XSetForeground(display, context, xcolor.pixel);
}
-
+
if (thickness == 1) {
- 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 );
+ 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);
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
-/*!
+/*!
Display a rectangle with \e topLeft as the top-left corner and \e
width and \e height the rectangle size.
@@ -2900,50 +2659,36 @@ void vpDisplayX::displayPoint ( const vpImagePoint &ip, const vpColor &color, un
rectangle. This parameter is only useful when \e fill is set to
false.
*/
-void
-vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
- unsigned int w, unsigned int h,
- const vpColor &color, bool fill,
- unsigned int thickness )
+void vpDisplayX::displayRectangle(const vpImagePoint &topLeft, unsigned int w, unsigned int h, const vpColor &color,
+ bool fill, unsigned int thickness)
{
- if ( m_displayHasBeenInitialized )
- {
- if ( thickness == 1 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
- }
- XSetLineAttributes ( display, context, thickness,
- LineSolid, CapButt, JoinBevel );
- if ( fill == false )
- {
- XDrawRectangle ( display, pixmap, context,
- vpMath::round( topLeft.get_u()/m_scale ),
- vpMath::round( topLeft.get_v()/m_scale ),
- w/m_scale, h/m_scale );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ XSetForeground(display, context, xcolor.pixel);
}
- 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 );
+ 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);
+ } 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);
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
-/*!
+/*!
Display a rectangle.
\param topLeft : Top-left corner of the rectangle.
@@ -2955,53 +2700,43 @@ vpDisplayX::displayRectangle ( const vpImagePoint &topLeft,
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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ XAllocColor(display, lut, &xcolor);
+ 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()) );
- if ( fill == false )
- {
+ 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()));
+ if (fill == false) {
- XDrawRectangle ( display, pixmap, context,
- vpMath::round( topLeft_.get_u() < bottomRight_.get_u() ? topLeft_.get_u() : bottomRight_.get_u() ),
- vpMath::round( topLeft_.get_v() < bottomRight_.get_v() ? topLeft_.get_v() : bottomRight_.get_v() ),
- w > 0 ? w : 1, h > 0 ? h : 1 );
- }
- else
- {
- XFillRectangle ( display, pixmap, context,
- vpMath::round( topLeft_.get_u() < bottomRight_.get_u() ? topLeft_.get_u() : bottomRight_.get_u() ),
- vpMath::round( topLeft_.get_v() < bottomRight_.get_v() ? topLeft_.get_v() : bottomRight_.get_v() ),
- w, h );
+ XDrawRectangle(display, pixmap, context,
+ vpMath::round(topLeft_.get_u() < bottomRight_.get_u() ? topLeft_.get_u() : bottomRight_.get_u()),
+ vpMath::round(topLeft_.get_v() < bottomRight_.get_v() ? topLeft_.get_v() : bottomRight_.get_v()),
+ w > 0 ? w : 1, h > 0 ? h : 1);
+ } else {
+ XFillRectangle(display, pixmap, context,
+ vpMath::round(topLeft_.get_u() < bottomRight_.get_u() ? topLeft_.get_u() : bottomRight_.get_u()),
+ vpMath::round(topLeft_.get_v() < bottomRight_.get_v() ? topLeft_.get_v() : bottomRight_.get_v()),
+ w, h);
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -3017,50 +2752,38 @@ 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 ) thickness = 0;
+ if (m_displayHasBeenInitialized) {
+ if (thickness == 1)
+ thickness = 0;
if (color.id < vpColor::id_unknown)
- XSetForeground ( display, context, x_color[color.id] );
+ XSetForeground(display, context, x_color[color.id]);
else {
- xcolor.pad = 0;
- xcolor.red = 256 * color.R;
+ xcolor.pad = 0;
+ xcolor.red = 256 * color.R;
xcolor.green = 256 * color.G;
- xcolor.blue = 256 * color.B;
- XAllocColor ( display, lut, &xcolor );
- XSetForeground ( display, context, xcolor.pixel );
+ xcolor.blue = 256 * color.B;
+ 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( 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 ) );
+ 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));
+ } 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));
}
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -3080,41 +2803,33 @@ vpDisplayX::displayRectangle ( const vpRect &rectangle,
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayX::getClick(bool blocking)
+bool vpDisplayX::getClick(bool blocking)
{
bool ret = false;
- if ( m_displayHasBeenInitialized ) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ if (m_displayHasBeenInitialized) {
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
+ if (blocking) {
+ XCheckMaskEvent(display, ButtonPressMask, &event);
+ XCheckMaskEvent(display, ButtonReleaseMask, &event);
+ XMaskEvent(display, ButtonPressMask, &event);
ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ } else {
+ ret = XCheckMaskEvent(display, ButtonPressMask, &event);
}
- if(ret){
+ 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" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
return ret;
}
@@ -3135,52 +2850,43 @@ vpDisplayX::getClick(bool blocking)
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool
-vpDisplayX::getClick ( vpImagePoint &ip, bool blocking )
+bool vpDisplayX::getClick(vpImagePoint &ip, bool blocking)
{
bool ret = false;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
+ if (blocking) {
+ XCheckMaskEvent(display, ButtonPressMask, &event);
+ XCheckMaskEvent(display, ButtonReleaseMask, &event);
+ XMaskEvent(display, ButtonPressMask, &event);
ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ } else {
+ ret = XCheckMaskEvent(display, ButtonPressMask, &event);
}
- if(ret){
+ if (ret) {
// Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x * m_scale);
- ip.set_v( (double)event.xbutton.y * m_scale);
+ 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"));
}
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
+ return ret;
}
/*!
Wait for a mouse button click and get the position of the clicked
pixel. The button used to click is also set.
-
+
\param ip [out] : The coordinates of the clicked image point.
\param button [out] : The button used to click.
@@ -3195,53 +2901,48 @@ 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;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonPressMask ,&event );
+ if (blocking) {
+ XCheckMaskEvent(display, ButtonPressMask, &event);
+ XCheckMaskEvent(display, ButtonReleaseMask, &event);
+ XMaskEvent(display, ButtonPressMask, &event);
ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonPressMask, &event);
+ } else {
+ ret = XCheckMaskEvent(display, ButtonPressMask, &event);
}
- if(ret){
+ if (ret) {
// Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x * m_scale);
- ip.set_v( (double)event.xbutton.y * m_scale);
- switch ( event.xbutton.button ) {
- case Button1: button = vpMouseButton::button1; break;
- case Button2: button = vpMouseButton::button2; break;
- case Button3: button = vpMouseButton::button3; break;
+ 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) {
+ case Button1:
+ button = vpMouseButton::button1;
+ break;
+ case Button2:
+ button = vpMouseButton::button2;
+ break;
+ case Button3:
+ button = vpMouseButton::button3;
+ break;
}
}
}
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
+ return ret;
}
/*!
@@ -3267,124 +2968,114 @@ 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;
- if ( m_displayHasBeenInitialized ) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ if (m_displayHasBeenInitialized) {
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
- if(blocking){
- XCheckMaskEvent(display , ButtonPressMask, &event);
- XCheckMaskEvent(display , ButtonReleaseMask, &event);
- XMaskEvent ( display, ButtonReleaseMask ,&event );
+ if (blocking) {
+ XCheckMaskEvent(display, ButtonPressMask, &event);
+ XCheckMaskEvent(display, ButtonReleaseMask, &event);
+ XMaskEvent(display, ButtonReleaseMask, &event);
ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , ButtonReleaseMask, &event);
+ } else {
+ ret = XCheckMaskEvent(display, ButtonReleaseMask, &event);
}
- if(ret){
+ if (ret) {
/* Recuperation de la coordonnee du pixel clique. */
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x * m_scale);
- ip.set_v( (double)event.xbutton.y * m_scale);
- switch ( event.xbutton.button ) {
- case Button1: button = vpMouseButton::button1; break;
- case Button2: button = vpMouseButton::button2; break;
- case Button3: button = vpMouseButton::button3; break;
+ 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) {
+ case Button1:
+ button = vpMouseButton::button1;
+ break;
+ case Button2:
+ button = vpMouseButton::button2;
+ break;
+ case Button3:
+ button = vpMouseButton::button3;
+ break;
}
}
}
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
+ return ret;
}
/*
Gets the displayed image (including the overlay plane)
- and returns an RGBa image. If a scale factor is set using setScale(), the size of the image is the
- size of the downscaled image.
+ and returns an RGBa image. If a scale factor is set using setScale(), the
+ size of the image is the size of the downscaled image.
\param I : Image to get.
*/
-void vpDisplayX::getImage ( vpImage<vpRGBa> &I )
+void vpDisplayX::getImage(vpImage<vpRGBa> &I)
{
- if ( m_displayHasBeenInitialized )
- {
- XImage *xi ;
+ 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 ) ;
+ I.resize(m_height, m_width);
- unsigned char *src_32 = NULL;
- src_32 = ( unsigned char* ) xi->data;
+ unsigned char *src_32 = NULL;
+ src_32 = (unsigned char *)xi->data;
if (screen_depth == 16) {
- for ( unsigned int i = 0; i < I.getHeight() ; i++ ) {
- size_t i_ = i*m_width;
- for ( unsigned int j = 0; j < m_height ; j++ ) {
- size_t ij_ = i_+j;
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ size_t i_ = i * m_width;
+ for (unsigned int j = 0; j < m_height; j++) {
+ size_t ij_ = i_ + j;
unsigned long pixel = XGetPixel(xi, (int)j, (int)i);
I.bitmap[ij_].R = (((pixel & RMask) << RShift) >> 8);
I.bitmap[ij_].G = (((pixel & GMask) << GShift) >> 8);
I.bitmap[ij_].B = (((pixel & BMask) << BShift) >> 8);
- // On OSX the bottom/right corner (arround the resizing icon) has alpha component
- // with different values than 255. That's why we force alpha to vpRGBa::alpha_default
+ // On OSX the bottom/right corner (arround the resizing icon) has
+ // alpha component with different values than 255. That's why we
+ // force alpha to vpRGBa::alpha_default
I.bitmap[ij_].A = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
if (XImageByteOrder(display) == 1) {
// big endian
- for ( unsigned int i = 0; i < m_width * m_height ; i++ ) {
- // On OSX the bottom/right corner (arround the resizing icon) has alpha component
- // with different values than 255. That's why we force alpha to vpRGBa::alpha_default
- I.bitmap[i].A = vpRGBa::alpha_default; //src_32[i*4] ;
- I.bitmap[i].R = src_32[i*4 + 1] ;
- I.bitmap[i].G = src_32[i*4 + 2] ;
- I.bitmap[i].B = src_32[i*4 + 3] ;
+ for (unsigned int i = 0; i < m_width * m_height; i++) {
+ // On OSX the bottom/right corner (arround the resizing icon) has
+ // alpha component with different values than 255. That's why we
+ // force alpha to vpRGBa::alpha_default
+ I.bitmap[i].A = vpRGBa::alpha_default; // src_32[i*4] ;
+ I.bitmap[i].R = src_32[i * 4 + 1];
+ I.bitmap[i].G = src_32[i * 4 + 2];
+ I.bitmap[i].B = src_32[i * 4 + 3];
}
- }
- else {
+ } else {
// little endian
- for ( unsigned int i = 0; i < m_width * m_height ; i++ ) {
- I.bitmap[i].B = src_32[i*4] ;
- I.bitmap[i].G = src_32[i*4 + 1] ;
- I.bitmap[i].R = src_32[i*4 + 2] ;
- // On OSX the bottom/right corner (arround the resizing icon) has alpha component
- // with different values than 255. That's why we force alpha to vpRGBa::alpha_default
- I.bitmap[i].A = vpRGBa::alpha_default; //src_32[i*4 + 3];
+ for (unsigned int i = 0; i < m_width * m_height; i++) {
+ I.bitmap[i].B = src_32[i * 4];
+ I.bitmap[i].G = src_32[i * 4 + 1];
+ I.bitmap[i].R = src_32[i * 4 + 2];
+ // On OSX the bottom/right corner (arround the resizing icon) has
+ // alpha component with different values than 255. That's why we
+ // force alpha to vpRGBa::alpha_default
+ I.bitmap[i].A = vpRGBa::alpha_default; // src_32[i*4 + 3];
}
}
}
- XDestroyImage ( xi ) ;
- }
- else
- {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ XDestroyImage(xi);
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
}
@@ -3394,41 +3085,39 @@ void vpDisplayX::getImage ( vpImage<vpRGBa> &I )
unsigned int vpDisplayX::getScreenDepth()
{
Display *display_;
- int screen_;
- unsigned int depth;
+ int screen_;
+ unsigned int depth;
- if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
- {
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server %s.", XDisplayName ( NULL ) ) ) ;
+ if ((display_ = XOpenDisplay(NULL)) == NULL) {
+ throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server %s.",
+ XDisplayName(NULL)));
}
- screen_ = DefaultScreen ( display_ );
- depth = (unsigned int)DefaultDepth ( display_, screen_ );
+ screen_ = DefaultScreen(display_);
+ depth = (unsigned int)DefaultDepth(display_, screen_);
- XCloseDisplay ( display_ );
+ XCloseDisplay(display_);
- return ( depth );
+ return (depth);
}
/*!
Gets screen resolution in pixels.
\param w, h : Horizontal and vertical screen resolution.
*/
-void vpDisplayX::getScreenSize ( unsigned int &w, unsigned int &h )
+void vpDisplayX::getScreenSize(unsigned int &w, unsigned int &h)
{
Display *display_;
- int screen_;
+ int screen_;
- if ( ( display_ = XOpenDisplay ( NULL ) ) == NULL )
- {
- throw ( vpDisplayException ( vpDisplayException::connexionError,
- "Can't connect display on server %s.", XDisplayName ( NULL ) ) ) ;
+ if ((display_ = XOpenDisplay(NULL)) == NULL) {
+ throw(vpDisplayException(vpDisplayException::connexionError, "Can't connect display on server %s.",
+ XDisplayName(NULL)));
}
- screen_ = DefaultScreen ( display_ );
- w = (unsigned int)DisplayWidth ( display_, screen_ );
- h = (unsigned int)DisplayHeight ( display_, screen_ );
+ screen_ = DefaultScreen(display_);
+ w = (unsigned int)DisplayWidth(display_, screen_);
+ h = (unsigned int)DisplayHeight(display_, screen_);
- XCloseDisplay ( display_ );
+ XCloseDisplay(display_);
}
/*!
@@ -3471,26 +3160,22 @@ unsigned int vpDisplayX::getScreenHeight()
was not initialized.
*/
-bool
-vpDisplayX::getKeyboardEvent(bool blocking)
+bool vpDisplayX::getKeyboardEvent(bool blocking)
{
bool ret = false;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
// Event testing
- if(blocking){
- XMaskEvent ( display, KeyPressMask ,&event );
+ if (blocking) {
+ XMaskEvent(display, KeyPressMask, &event);
ret = true;
+ } else {
+ ret = XCheckMaskEvent(display, KeyPressMask, &event);
}
- else{
- ret = XCheckMaskEvent(display , KeyPressMask, &event);
- }
- }
- else {
- vpERROR_TRACE ( "X not initialized " ) ;
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
+ } else {
+ vpERROR_TRACE("X not initialized ");
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
return ret;
}
@@ -3517,36 +3202,30 @@ vpDisplayX::getKeyboardEvent(bool blocking)
was not initialized.
*/
-bool
-vpDisplayX::getKeyboardEvent(std::string &key, bool blocking)
+bool vpDisplayX::getKeyboardEvent(std::string &key, bool blocking)
{
bool ret = false;
- KeySym keysym;
+ KeySym keysym;
// int count;
XComposeStatus compose_status;
char buffer;
-
- if ( m_displayHasBeenInitialized ) {
+
+ if (m_displayHasBeenInitialized) {
// Event testing
- if(blocking){
- XMaskEvent ( display, KeyPressMask ,&event );
- /* count = */ XLookupString ((XKeyEvent *)&event, &buffer, 1,
- &keysym, &compose_status);
+ if (blocking) {
+ XMaskEvent(display, KeyPressMask, &event);
+ /* count = */ XLookupString((XKeyEvent *)&event, &buffer, 1, &keysym, &compose_status);
key = buffer;
ret = true;
- }
- else{
- ret = XCheckMaskEvent(display , KeyPressMask, &event);
+ } 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" ) ) ;
+ } else {
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "X not initialized"));
}
return ret;
}
@@ -3562,37 +3241,29 @@ vpDisplayX::getKeyboardEvent(std::string &key, bool blocking)
was not initialized.
*/
-bool
-vpDisplayX::getPointerMotionEvent ( vpImagePoint &ip)
+bool vpDisplayX::getPointerMotionEvent(vpImagePoint &ip)
{
bool ret = false;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
- ret = XCheckMaskEvent(display , PointerMotionMask, &event);
+ ret = XCheckMaskEvent(display, PointerMotionMask, &event);
- if(ret){
+ if (ret) {
// Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)event.xbutton.x * m_scale);
- ip.set_v( (double)event.xbutton.y * m_scale);
+ 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"));
}
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
+ return ret;
}
/*!
@@ -3605,37 +3276,29 @@ vpDisplayX::getPointerMotionEvent ( vpImagePoint &ip)
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayX::getPointerPosition ( vpImagePoint &ip)
+bool vpDisplayX::getPointerPosition(vpImagePoint &ip)
{
bool ret = false;
- if ( m_displayHasBeenInitialized ) {
+ if (m_displayHasBeenInitialized) {
- Window rootwin, childwin ;
- int root_x, root_y, win_x, win_y ;
- unsigned int modifier ;
+ Window rootwin, childwin;
+ int root_x, root_y, win_x, win_y;
+ unsigned int modifier;
// Event testing
ret = true;
- if(ret){
+ if (ret) {
// Get mouse position
- if ( XQueryPointer ( display,
- window,
- &rootwin, &childwin,
- &root_x, &root_y,
- &win_x, &win_y,
- &modifier ) ) {
- ip.set_u( (double)win_x * m_scale);
- ip.set_v( (double)win_y * m_scale);
+ 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"));
}
- else {
- throw ( vpDisplayException ( vpDisplayException::notInitializedError,
- "X not initialized" ) ) ;
- }
- return ret ;
+ return ret;
}
/*!
@@ -3645,7 +3308,7 @@ int vpDisplayX::getMsb(unsigned int u32val)
{
int i;
- for (i = 31; i >= 0; --i) {
+ for (i = 31; i >= 0; --i) {
if (u32val & 0x80000000L)
break;
u32val <<= 1;
@@ -3654,7 +3317,7 @@ int vpDisplayX::getMsb(unsigned int u32val)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayX.cpp.o) has no symbols
-void dummy_vpDisplayX() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayX.cpp.o) has no
+// symbols
+void dummy_vpDisplayX(){};
#endif
-
diff --git a/modules/gui/src/display/windows/vpD3DRenderer.cpp b/modules/gui/src/display/windows/vpD3DRenderer.cpp
index e449917..bcc74a4 100644
--- a/modules/gui/src/display/windows/vpD3DRenderer.cpp
+++ b/modules/gui/src/display/windows/vpD3DRenderer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,11 +40,11 @@
#include <algorithm>
#include <visp3/core/vpConfig.h>
-#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
+#if (defined(_WIN32) & defined(VISP_HAVE_D3D9))
-#include <visp3/gui/vpD3DRenderer.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpMath.h>
+#include <visp3/gui/vpD3DRenderer.h>
/*
Be careful, when using :
@@ -72,93 +73,93 @@
*/
vpD3DRenderer::vpD3DRenderer()
{
- pD3D=NULL;
- pd3dDevice=NULL;
- pd3dText=NULL;
- pd3dVideoText=NULL;
-
- //D3D palette
+ pD3D = NULL;
+ pd3dDevice = NULL;
+ pSprite = NULL;
+ pd3dText = NULL;
+ pd3dVideoText = NULL;
+ textWidth = 0;
+
+ // 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);
+ 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
+ // initialize the GDI palette
pcolor = vpColor::black;
- colorsGDI[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightBlue;
- colorsGDI[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::blue;
- colorsGDI[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkBlue;
- colorsGDI[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::cyan;
- colorsGDI[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGreen;
- colorsGDI[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::green;
- colorsGDI[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGreen;
- colorsGDI[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightRed;
- colorsGDI[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::red;
- colorsGDI[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkRed;
- colorsGDI[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::white;
- colorsGDI[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGray;
- colorsGDI[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::gray;
colorsGDI[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGray;
- colorsGDI[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::yellow;
- colorsGDI[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_yellow] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::orange;
- colorsGDI[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ colorsGDI[vpColor::id_orange] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::purple;
- colorsGDI[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ 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);
+ // 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);
}
/*!
@@ -169,13 +170,13 @@ vpD3DRenderer::~vpD3DRenderer()
{
DeleteObject(hFont);
- if(pd3dDevice != NULL)
+ if (pd3dDevice != NULL)
pd3dDevice->Release();
- if(pD3D != NULL)
+ if (pD3D != NULL)
pD3D->Release();
- if(pd3dText != NULL)
+ if (pd3dText != NULL)
pd3dText->Release();
- if(pd3dVideoText != NULL)
+ if (pd3dVideoText != NULL)
pd3dVideoText->Release();
}
@@ -186,13 +187,12 @@ vpD3DRenderer::~vpD3DRenderer()
*/
unsigned int vpD3DRenderer::supPowerOf2(unsigned int n)
{
- unsigned int i=0;
- while(n>1)
- {
- n>>=1;
+ unsigned int i = 0;
+ while (n > 1) {
+ n >>= 1;
i++;
}
- return static_cast<unsigned int>(1<<(i+1));
+ return static_cast<unsigned int>(1 << (i + 1));
}
/*!
@@ -204,83 +204,61 @@ unsigned int vpD3DRenderer::supPowerOf2(unsigned int n)
*/
bool vpD3DRenderer::init(HWND hwnd, unsigned int width, unsigned int height)
{
- //simple stuff
+ // simple stuff
m_rwidth = width;
m_rheight = height;
hWnd = hwnd;
- //D3D initialize
- if(NULL == (pD3D = Direct3DCreate9(D3D_SDK_VERSION)))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't initialize D3D!");
+ // D3D initialize
+ if (NULL == (pD3D = Direct3DCreate9(D3D_SDK_VERSION)))
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't initialize D3D!");
D3DDISPLAYMODE d3ddm;
- if(FAILED(pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm)))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't get the adapter's display mode!");
-
+ if (FAILED(pD3D->GetAdapterDisplayMode(D3DADAPTER_DEFAULT, &d3ddm)))
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't get the adapter's display mode!");
D3DPRESENT_PARAMETERS d3dpp;
ZeroMemory(&d3dpp, sizeof(d3dpp));
- d3dpp.BackBufferCount=1;
- d3dpp.Windowed = TRUE;
+ d3dpp.BackBufferCount = 1;
+ d3dpp.Windowed = TRUE;
d3dpp.SwapEffect = D3DSWAPEFFECT_DISCARD;
d3dpp.BackBufferFormat = d3ddm.Format;
- //creates a d3d device
- if( FAILED(pD3D->CreateDevice(D3DADAPTER_DEFAULT , D3DDEVTYPE_HAL, hWnd,
- D3DCREATE_SOFTWARE_VERTEXPROCESSING |
- D3DCREATE_MULTITHREADED,
- &d3dpp, &pd3dDevice )))
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create the Direct3D device!");
-
+ // creates a d3d device
+ if (FAILED(pD3D->CreateDevice(D3DADAPTER_DEFAULT, D3DDEVTYPE_HAL, hWnd,
+ D3DCREATE_SOFTWARE_VERTEXPROCESSING | D3DCREATE_MULTITHREADED, &d3dpp, &pd3dDevice)))
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't create the Direct3D device!");
- //disables scene lightning
+ // disables scene lightning
pd3dDevice->SetRenderState(D3DRS_LIGHTING, FALSE);
+ // inits the direct3D view (for 2D rendering)
+ initView((float)m_rwidth, (float)m_rheight);
- //inits the direct3D view (for 2D rendering)
- initView((float)m_rwidth,(float)m_rheight);
-
+ // computes texture size (needs to be a power-of-2 large square)
+ textWidth = supPowerOf2((m_rwidth > m_rheight) ? m_rwidth : m_rheight);
- //computes texture size (needs to be a power-of-2 large square)
- textWidth = supPowerOf2( (m_rwidth>m_rheight) ? m_rwidth : m_rheight );
-
- //creates the system memory texture (the one we will directly modify)
- //unfortunately, needs to be X8R8G8B8 in order to be able to use GDI drawing
- //functions
- if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, 0,
- D3DFMT_X8R8G8B8, D3DPOOL_SYSTEMMEM , &pd3dText)
- != D3D_OK)
- {
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create memory texture!");
+ // creates the system memory texture (the one we will directly modify)
+ // unfortunately, needs to be X8R8G8B8 in order to be able to use GDI
+ // drawing functions
+ if (D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, 0, D3DFMT_X8R8G8B8, D3DPOOL_SYSTEMMEM,
+ &pd3dText) != D3D_OK) {
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't create memory texture!");
}
- //creates the video memory texture (the one we will display) -
- if( D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT,
- D3DUSAGE_DYNAMIC ,
- D3DFMT_X8R8G8B8, D3DPOOL_DEFAULT, &pd3dVideoText)
- != D3D_OK)
- {
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create video texture!");
+ // creates the video memory texture (the one we will display) -
+ if (D3DXCreateTexture(pd3dDevice, textWidth, textWidth, D3DX_DEFAULT, D3DUSAGE_DYNAMIC, D3DFMT_X8R8G8B8,
+ D3DPOOL_DEFAULT, &pd3dVideoText) != D3D_OK) {
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't create video texture!");
}
- //creates the sprite used to render the texture
- if(D3DXCreateSprite(pd3dDevice, &pSprite) != S_OK)
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't create the texture's sprite!");
-
-
+ // creates the sprite used to render the texture
+ if (D3DXCreateSprite(pd3dDevice, &pSprite) != S_OK)
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't create the texture's sprite!");
return true;
}
-
-
-
/*!
Initialize the view (orthogonal ...).
\param WindowWidth The Width of the window.
@@ -294,25 +272,23 @@ void vpD3DRenderer::initView(float WindowWidth, float WindowHeight)
D3DXMatrixOrthoLH(&Ortho2D, WindowWidth, WindowHeight, 0.0f, 1.0f);
D3DXMatrixIdentity(&Identity);
- if( pd3dDevice->SetTransform(D3DTS_PROJECTION, &Ortho2D) != D3D_OK
- || pd3dDevice->SetTransform(D3DTS_WORLD, &Identity) != D3D_OK
- || pd3dDevice->SetTransform(D3DTS_VIEW, &Identity) != D3D_OK)
- throw vpDisplayException(vpDisplayException::notInitializedError,
- "Can't set the view!");
+ if (pd3dDevice->SetTransform(D3DTS_PROJECTION, &Ortho2D) != D3D_OK ||
+ pd3dDevice->SetTransform(D3DTS_WORLD, &Identity) != D3D_OK ||
+ pd3dDevice->SetTransform(D3DTS_VIEW, &Identity) != D3D_OK)
+ throw vpDisplayException(vpDisplayException::notInitializedError, "Can't set the view!");
}
-
/*!
Converts a ViSP RGBA image to the Direct3D texture format (BGRA).
\param I Image to convert.
\param imBuffer Destination buffer.
\param pitch Pitch of the destination texture.
*/
-void vpD3DRenderer::convert(const vpImage<vpRGBa>& I, unsigned char * imBuffer, unsigned int pitch)
+void vpD3DRenderer::convert(const vpImage<vpRGBa> &I, unsigned char *imBuffer, unsigned int pitch)
{
if (m_rscale == 1) {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int ii_ = i*pitch;
+ unsigned int ii_ = i * pitch;
for (unsigned int j = 0; j < m_rwidth; j++) {
vpRGBa val = I[i][j];
unsigned int index_ = ii_ + j * 4;
@@ -322,13 +298,12 @@ void vpD3DRenderer::convert(const vpImage<vpRGBa>& I, unsigned char * imBuffer,
imBuffer[++index_] = val.A;
}
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int i_ = i*m_rscale;
- unsigned int ii_ = i*pitch;
+ unsigned int i_ = i * m_rscale;
+ unsigned int ii_ = i * pitch;
for (unsigned int j = 0; j < m_rwidth; j++) {
- vpRGBa val = I[i_][j*m_rscale];
+ vpRGBa val = I[i_][j * m_rscale];
unsigned int index_ = ii_ + j * 4;
imBuffer[index_] = val.B;
imBuffer[++index_] = val.G;
@@ -345,11 +320,11 @@ void vpD3DRenderer::convert(const vpImage<vpRGBa>& I, unsigned char * imBuffer,
\param imBuffer Destination buffer.
\param pitch Pitch of the destination texture.
*/
-void vpD3DRenderer::convert(const vpImage<unsigned char>& I, unsigned char * imBuffer, unsigned int pitch)
+void vpD3DRenderer::convert(const vpImage<unsigned char> &I, unsigned char *imBuffer, unsigned int pitch)
{
if (m_rscale == 1) {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int ii_ = i*pitch;
+ unsigned int ii_ = i * pitch;
for (unsigned int j = 0; j < m_rwidth; j++) {
unsigned char val = I[i][j];
unsigned int index_ = ii_ + j * 4;
@@ -359,13 +334,12 @@ void vpD3DRenderer::convert(const vpImage<unsigned char>& I, unsigned char * imB
imBuffer[++index_] = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int i_ = i*m_rscale;
- unsigned int ii_ = i*pitch;
+ unsigned int i_ = i * m_rscale;
+ unsigned int ii_ = i * pitch;
for (unsigned int j = 0; j < m_rwidth; j++) {
- unsigned char val = I[i_][j*m_rscale];
+ unsigned char val = I[i_][j * m_rscale];
unsigned int index_ = ii_ + j * 4;
imBuffer[index_] = val;
imBuffer[++index_] = val;
@@ -384,8 +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<unsigned char>& I, unsigned char * imBuffer, unsigned int pitch,
- int i_min, int j_min, int i_max, int j_max)
+void vpD3DRenderer::convertROI(const vpImage<unsigned char> &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;
@@ -393,7 +367,7 @@ void vpD3DRenderer::convertROI(const vpImage<unsigned char>& I, unsigned char *
if (m_rscale == 1) {
for (int i = 0; i < h; i++) {
unsigned int i_ = i_min + i;
- unsigned int ii_ = i*pitch;
+ unsigned int ii_ = i * pitch;
for (int j = 0; j < w; j++) {
unsigned char val = I[i_][j_min + j];
unsigned int index_ = ii_ + j * 4;
@@ -403,13 +377,12 @@ void vpD3DRenderer::convertROI(const vpImage<unsigned char>& I, unsigned char *
imBuffer[++index_] = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
for (int i = 0; i < h; i++) {
- unsigned int i_ = (i_min + i)*m_rscale;
- unsigned int ii_ = i*pitch;
+ unsigned int i_ = (i_min + i) * m_rscale;
+ unsigned int ii_ = i * pitch;
for (int j = 0; j < w; j++) {
- unsigned char val = I[i_][(j_min + j)*m_rscale];
+ unsigned char val = I[i_][(j_min + j) * m_rscale];
unsigned int index_ = ii_ + j * 4;
imBuffer[index_] = val;
imBuffer[++index_] = val;
@@ -428,8 +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<vpRGBa>& I, unsigned char * imBuffer, unsigned int pitch,
- int i_min, int j_min, int i_max, int j_max)
+void vpD3DRenderer::convertROI(const vpImage<vpRGBa> &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;
@@ -437,7 +410,7 @@ void vpD3DRenderer::convertROI(const vpImage<vpRGBa>& I, unsigned char * imBuffe
if (m_rscale == 1) {
for (int i = 0; i < h; i++) {
unsigned int i_ = i_min + i;
- unsigned int ii_ = i*pitch;
+ unsigned int ii_ = i * pitch;
for (int j = 0; j < w; j++) {
vpRGBa val = I[i_][j_min + j];
unsigned int index_ = ii_ + j * 4;
@@ -447,13 +420,12 @@ void vpD3DRenderer::convertROI(const vpImage<vpRGBa>& I, unsigned char * imBuffe
imBuffer[++index_] = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
for (int i = 0; i < h; i++) {
- unsigned int i_ = (i_min + i)*m_rscale;
- unsigned int ii_ = i*pitch;
+ unsigned int i_ = (i_min + i) * m_rscale;
+ unsigned int ii_ = i * pitch;
for (int j = 0; j < w; j++) {
- vpRGBa val = I[i_][(j_min + j)*m_rscale];
+ vpRGBa val = I[i_][(j_min + j) * m_rscale];
unsigned int index_ = ii_ + j * 4;
imBuffer[index_] = val.B;
imBuffer[++index_] = val.G;
@@ -468,36 +440,34 @@ void vpD3DRenderer::convertROI(const vpImage<vpRGBa>& I, unsigned char * imBuffe
Sets the image to display.
\param im The image to display.
*/
-void vpD3DRenderer::setImg(const vpImage<vpRGBa>& im)
+void vpD3DRenderer::setImg(const vpImage<vpRGBa> &im)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top=0; r.left=0;
- r.bottom=static_cast<signed long>(m_rheight);
- r.right=static_cast<signed long>(m_rwidth);
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<signed long>(m_rheight);
+ r.right = static_cast<signed long>(m_rwidth);
+
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ // gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //fills this texture with the image data (converted to bgra)
+ // fills this texture with the image data (converted to bgra)
convert(im, buf, pitch);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
-
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
@@ -505,118 +475,116 @@ void vpD3DRenderer::setImg(const vpImage<vpRGBa>& im)
Sets the image to display.
\param im The image to display.
*/
-void vpD3DRenderer::setImgROI(const vpImage<vpRGBa>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpD3DRenderer::setImgROI(const vpImage<vpRGBa> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
- 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_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);
RECT r;
- r.top = (LONG)i_min; r.left = (LONG)j_min;
- r.bottom = (LONG)i_max; r.right = (LONG)j_max;
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ r.top = (LONG)i_min;
+ r.left = (LONG)j_min;
+ r.bottom = (LONG)i_max;
+ r.right = (LONG)j_max;
+
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //fills this texture with the image data (converted to bgra)
+ // fills this texture with the image data (converted to bgra)
convertROI(im, buf, pitch, i_min, j_min, i_max, j_max);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
-
/*!
Sets the image to display.
\param im The image to display.
*/
-void vpD3DRenderer::setImg(const vpImage<unsigned char>& im)
+void vpD3DRenderer::setImg(const vpImage<unsigned char> &im)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top = 0;
- r.left = 0;
+ r.top = 0;
+ r.left = 0;
r.bottom = static_cast<LONG>(m_rheight);
- r.right = static_cast<LONG>(m_rwidth);
+ r.right = static_cast<LONG>(m_rwidth);
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //fills this texture with the image data (converted to bgra)
+ // fills this texture with the image data (converted to bgra)
convert(im, buf, pitch);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
-
/*!
Sets the image to display.
\param im The image to display.
*/
-void vpD3DRenderer::setImgROI(const vpImage<unsigned char>& im, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpD3DRenderer::setImgROI(const vpImage<unsigned char> &im, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
- 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_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);
RECT r;
- r.top=(LONG)i_min; r.left= (LONG)j_min;
- r.bottom= (LONG)i_max; r.right= (LONG)j_max;
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ r.top = (LONG)i_min;
+ r.left = (LONG)j_min;
+ r.bottom = (LONG)i_max;
+ r.right = (LONG)j_max;
+
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ // gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //fills this texture with the image data (converted to bgra)
+ // fills this texture with the image data (converted to bgra)
convertROI(im, buf, pitch, i_min, j_min, i_max, j_max);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
@@ -628,86 +596,83 @@ void vpD3DRenderer::setImgROI(const vpImage<unsigned char>& im, const vpImagePoi
bool vpD3DRenderer::render()
{
// Clears the back buffer to a blue color
- //pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET, D3DCOLOR_XRGB(0,0,255), 1.0f, 0 );
+ // pd3dDevice->Clear( 0, NULL, D3DCLEAR_TARGET,
+ // D3DCOLOR_XRGB(0,0,255), 1.0f, 0 );
- //Begins the scene.
+ // Begins the scene.
pd3dDevice->BeginScene();
- //Texture rectangle to display
+ // Texture rectangle to display
RECT r;
- r.top = 0;
- r.left = 0;
+ r.top = 0;
+ r.left = 0;
r.bottom = static_cast<LONG>(m_rheight);
- r.right = static_cast<LONG>(m_rwidth);
+ r.right = static_cast<LONG>(m_rwidth);
- //Updates the video memory texture with the content of the system
- //memory texture
- pd3dDevice->UpdateTexture(pd3dText,pd3dVideoText);
+ // Updates the video memory texture with the content of the system
+ // memory texture
+ pd3dDevice->UpdateTexture(pd3dText, pd3dVideoText);
- //Displays this texture as a sprite
+// Displays this texture as a sprite
#if (D3DX_SDK_VERSION <= 9)
pSprite->Begin(); //
- pSprite->Draw(pd3dVideoText, &r, NULL, NULL, NULL, NULL, 0xFFFFFFFF );
+ pSprite->Draw(pd3dVideoText, &r, NULL, NULL, NULL, NULL, 0xFFFFFFFF);
#else
pSprite->Begin(0);
- pSprite->Draw(pd3dVideoText, &r, NULL, NULL, 0xFFFFFFFF );
+ pSprite->Draw(pd3dVideoText, &r, NULL, NULL, 0xFFFFFFFF);
#endif
pSprite->End();
- //Ends the scene.
+ // Ends the scene.
pd3dDevice->EndScene();
- //Presents the backbuffer
- pd3dDevice->Present( NULL, NULL, NULL, NULL );
+ // Presents the backbuffer
+ pd3dDevice->Present(NULL, NULL, NULL, NULL);
return true;
}
-
/*!
Sets a pixel to color at position (j,i).
\param ip : The pixel coordinates.
\param color : the color of the point.
*/
-void vpD3DRenderer::setPixel(const vpImagePoint &iP,
- const vpColor &color)
+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 || iPscaled.get_j()>=(int)m_rwidth)
- {
+ if (iPscaled.get_i() < 0 || iPscaled.get_j() < 0 || iPscaled.get_i() >= (int)m_rheight ||
+ iPscaled.get_j() >= (int)m_rwidth) {
return;
}
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top=(LONG)iPscaled.get_i();
- r.left=(LONG)iPscaled.get_j();
- r.bottom=(LONG)iPscaled.get_i()+1;
- r.right=(LONG)iPscaled.get_j()+1;
+ r.top = (LONG)iPscaled.get_i();
+ r.left = (LONG)iPscaled.get_j();
+ r.bottom = (LONG)iPscaled.get_i() + 1;
+ r.right = (LONG)iPscaled.get_j() + 1;
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
- unsigned int pitch = static_cast<unsigned int>( d3dLRect.Pitch );
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ // gets the buffer and pitch of the texture
+ unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //the coordinates are in the locked area base
- setBufferPixel(buf, pitch, 0, 0,color);
+ // the coordinates are in the locked area base
+ setBufferPixel(buf, pitch, 0, 0, color);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
@@ -718,25 +683,22 @@ void vpD3DRenderer::setPixel(const vpImagePoint &iP,
\param thickness : Thickness of the line.
\param style style of the line
*/
-void vpD3DRenderer::drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
+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)
- {
- //Will contain the texture's surface drawing context
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
+ // Will contain the texture's surface drawing context
HDC hDCMem;
- //The texture's surface
- IDirect3DSurface9 * pd3dSurf;
+ // The texture's surface
+ IDirect3DSurface9 *pd3dSurf;
pd3dText->GetSurfaceLevel(0, &pd3dSurf);
- //We get its DC
+ // We get its DC
pd3dSurf->GetDC(&hDCMem);
- //create the pen
+ // create the pen
HPEN hPen;
if (color.id < vpColor::id_unknown)
hPen = CreatePen(style, static_cast<int>(thickness), colorsGDI[color.id]);
@@ -745,46 +707,45 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1,
hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
}
- //we don't use the bkColor
+ // we don't use the bkColor
SetBkMode(hDCMem, TRANSPARENT);
- //select the pen
+ // select the pen
SelectObject(hDCMem, hPen);
- // Warning: When thickness > 1 and pen style is PS_DASHDOT, the drawing displays a solid line
- // That's why in that case we implement the dashdot line manually drawing multiple small lines
+ // Warning: When thickness > 1 and pen style is PS_DASHDOT, the drawing
+ // displays a solid line That's why in that case we implement the dashdot
+ // line manually drawing multiple small lines
if (thickness != 1 && style != PS_SOLID) {
- double size = 10.*m_rscale;
+ 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 deltaj = size / length*(ip2.get_j() - ip1.get_j());
- double deltai = size / length*(ip2.get_i() - ip1.get_i());
+ 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();
+ 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)) {
- double i = slope*j + orig;
- //move to the starting point
+ double i = slope * j + orig;
+ // move to the starting point
MoveToEx(hDCMem, vpMath::round(j / m_rscale), vpMath::round(i / m_rscale), NULL);
- //Draw the line
+ // Draw the line
LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale));
}
- }
- else {
- //move to the starting point
+ } else {
+ // move to the starting point
MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), NULL);
- //Draw the line
+ // Draw the line
LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale));
}
- //Releases the DC
+ // Releases the DC
pd3dSurf->ReleaseDC(hDCMem);
- //Releases the surface's interface
+ // Releases the surface's interface
pd3dSurf->Release();
- //Deletes additional objects
+ // Deletes additional objects
DeleteObject(hPen);
}
}
-
/*!
Draws a rectangle.
\param topLeft its top left point's coordinates
@@ -794,29 +755,23 @@ void vpD3DRenderer::drawLine(const vpImagePoint &ip1,
\param fill When set to true fill the rectangle.
\param thickness : Line thickness
*/
-void vpD3DRenderer::drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
+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)
- {
+ // 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(height, 0), topLeft, color, thickness);
- }
- else
- {
+ } 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!"<<std::endl;
return;
}
@@ -824,34 +779,35 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft,
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top = (LONG)((topLeftScaled.get_i()>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.top = (LONG)((topLeftScaled.get_i() > 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);
/* unsigned */ int rectW = r.right - r.left;
/* unsigned */ int rectH = r.bottom - r.top;
- //locks the texture to directly access it
- if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK)
- {
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *)d3dLRect.pBits;
+ unsigned 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)
- {
- for (int x = 0; x<rectW; x++) {
- for (int y = 0; y<rectH; y++)
+ 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);
}
}
- //unlocks the texture
+ // unlocks the texture
if (pd3dText->UnlockRect(0) != D3D_OK)
vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
@@ -864,27 +820,25 @@ void vpD3DRenderer::drawRect(const vpImagePoint &topLeft,
*/
void vpD3DRenderer::clear(const vpColor &color)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>( m_rheight );
- r.right = static_cast<LONG>( m_rwidth );
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>(m_rheight);
+ r.right = static_cast<LONG>(m_rwidth);
+
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- long * buf = (long *) ( d3dLRect.pBits );
+ long *buf = (long *)(d3dLRect.pBits);
unsigned long c;
if (color.id < vpColor::id_unknown)
@@ -892,50 +846,43 @@ void vpD3DRenderer::clear(const vpColor &color)
else {
c = D3DCOLOR_ARGB(0xFF, color.R, color.G, color.B);
}
- long * end = (long*)(buf + (pitch * m_rheight));
+ long *end = (long *)(buf + (pitch * m_rheight));
- //fills the whole image
+ // fills the whole image
while (buf < end)
- *buf++ = static_cast<long>( c );
+ *buf++ = static_cast<long>(c);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
-
-
-//writes current circle pixels using symetry to reduce the algorithm's complexity
-void vpD3DRenderer::subDrawCircle(int i, int j,
- int x, int y,
- vpColor col,
- unsigned char* buf, unsigned int pitch,
+// writes current circle pixels using symetry to reduce the algorithm's
+// complexity
+void vpD3DRenderer::subDrawCircle(int i, int j, int x, int y, vpColor col, unsigned char *buf, unsigned int pitch,
unsigned int maxX, unsigned int maxY)
{
if (x == 0) {
- setBufferPixel(buf, pitch, i , j+y, col, maxX, maxY);
- setBufferPixel(buf, pitch, i , j-y, col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y, j, col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y, j, col, maxX, maxY);
- } else
- if (x == y) {
- setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
- } else
- if (x < y) {
- setBufferPixel(buf, pitch, i+x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j+y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-x,j-y,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y,j+x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y,j+x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i+y,j-x,col, maxX, maxY);
- setBufferPixel(buf, pitch, i-y,j-x,col, maxX, maxY);
- }
-
+ setBufferPixel(buf, pitch, i, j + y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i, j - y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i + y, j, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - y, j, col, maxX, maxY);
+ } else if (x == y) {
+ setBufferPixel(buf, pitch, i + x, j + y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - x, j + y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i + x, j - y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - x, j - y, col, maxX, maxY);
+ } else if (x < y) {
+ setBufferPixel(buf, pitch, i + x, j + y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - x, j + y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i + x, j - y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - x, j - y, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i + y, j + x, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - y, j + x, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i + y, j - x, col, maxX, maxY);
+ setBufferPixel(buf, pitch, i - y, j - x, col, maxX, maxY);
+ }
}
/*!
@@ -944,103 +891,103 @@ void vpD3DRenderer::subDrawCircle(int i, int j,
\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
- || 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)
+ 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)
return;
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
D3DLOCKED_RECT d3dLRect;
RECT rec;
int radiusScaled_ = static_cast<int>(radiusScaled);
- int rleft = (vpMath::round(centerScaled.get_j()- radiusScaled_) > 0) ? vpMath::round(centerScaled.get_j())- radiusScaled_ : 0;
- int rtop = (vpMath::round(centerScaled.get_i()- radiusScaled_) > 0) ? vpMath::round(centerScaled.get_i())- radiusScaled_ : 0;
-
- 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);
-
- //used as maxX and maxY for setBufferPixel
- unsigned int rectW = static_cast<unsigned int> ( rec.right - rleft );
- unsigned int rectH = static_cast<unsigned int> ( rec.bottom - rtop );
-
- //locks the texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &rec, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ int rleft = (vpMath::round(centerScaled.get_j() - radiusScaled_) > 0)
+ ? vpMath::round(centerScaled.get_j()) - radiusScaled_
+ : 0;
+ int rtop = (vpMath::round(centerScaled.get_i() - radiusScaled_) > 0)
+ ? vpMath::round(centerScaled.get_i()) - radiusScaled_
+ : 0;
+
+ 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);
+
+ // used as maxX and maxY for setBufferPixel
+ unsigned int rectW = static_cast<unsigned int>(rec.right - rleft);
+ unsigned int rectH = static_cast<unsigned int>(rec.bottom - rtop);
+
+ // locks the texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &rec, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
// Bresenham 's circle algorithm
int x = 0;
int y = static_cast<int>(radiusScaled);
- int p = (3 - (y<<1));
+ int p = (3 - (y << 1));
vpImagePoint ip;
- ip.set_i(centerScaled.get_i()-rtop);
- ip.set_j(centerScaled.get_j()-rleft);
+ 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);
- while(x < y){
+ while (x < y) {
x++;
- if (p < 0)
- {
- p += ((x<<1)+1)<<1;
- }
- else
- {
+ if (p < 0) {
+ p += ((x << 1) + 1) << 1;
+ } else {
y--;
- p += (((x-y)<<1)+1)<<1;
+ p += (((x - y) << 1) + 1) << 1;
}
subDrawCircle(vpMath::round(ip.get_i()), vpMath::round(ip.get_j()), x, y, color, buf, pitch, rectW, rectH);
}
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
-
-
/*!
Draws some text.
\param ip its top left point's coordinates
\param text The string to display
\param color The text's color
*/
-void vpD3DRenderer::drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color)
+void vpD3DRenderer::drawText(const vpImagePoint &ip, const char *text, const vpColor &color)
{
- //Will contain the texture's surface drawing context
+ // Will contain the texture's surface drawing context
HDC hDCMem;
- //The texture's surface
- IDirect3DSurface9 * pd3dSurf;
+ // The texture's surface
+ IDirect3DSurface9 *pd3dSurf;
pd3dText->GetSurfaceLevel(0, &pd3dSurf);
- //We get its DC
+ // We get its DC
pd3dSurf->GetDC(&hDCMem);
- //Select the font
+ // Select the font
SelectObject(hDCMem, hFont);
- //set the text color
+ // set the text color
if (color.id < vpColor::id_unknown)
SetTextColor(hDCMem, colorsGDI[color.id]);
else {
@@ -1048,27 +995,26 @@ void vpD3DRenderer::drawText(const vpImagePoint &ip, const char * text,
SetTextColor(hDCMem, gdicolor);
}
- //we don't use the bkColor
+ // we don't use the bkColor
SetBkMode(hDCMem, TRANSPARENT);
SIZE size;
- int length = (int) strlen(text);
+ int length = (int)strlen(text);
- //get the displayed string dimensions
+ // get the displayed string dimensions
GetTextExtentPoint32(hDCMem, text, length, &size);
- //displays the string
+ // displays the string
TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale), text, length);
- //Releases the DC
+ // Releases the DC
pd3dSurf->ReleaseDC(hDCMem);
- //Releases the surface's interface
+ // Releases the surface's interface
pd3dSurf->Release();
- //Deletes additional objects
+ // Deletes additional objects
DeleteObject(hFont);
}
-
/*!
Draws a cross.
\param ip its center point's coordinates
@@ -1076,9 +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();
@@ -1105,38 +1049,34 @@ void vpD3DRenderer::drawCross(const vpImagePoint &ip,
\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();
double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
- //if ( ( a==0 ) && ( b==0 ) )
- if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) && (std::fabs(b) <= std::numeric_limits<double>::epsilon()))
- {
+ // if ( ( a==0 ) && ( b==0 ) )
+ if ((std::fabs(a) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(b) <= std::numeric_limits<double>::epsilon())) {
// DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
+ } else {
a /= lg;
b /= lg;
vpImagePoint ip3;
- ip3.set_i(ip2.get_i() - w*a);
- ip3.set_j(ip2.get_j() - w*b);
+ ip3.set_i(ip2.get_i() - w * a);
+ ip3.set_j(ip2.get_j() - w * b);
vpImagePoint ip4;
- ip4.set_i(ip3.get_i() - b*h);
- ip4.set_j(ip3.get_j() + a*h);
+ ip4.set_i(ip3.get_i() - b * h);
+ ip4.set_j(ip3.get_j() + a * h);
if (lg > 2 * vpImagePoint::distance(ip2, ip4))
drawLine(ip2, ip4, color, thickness);
- ip4.set_i(ip3.get_i() + b*h);
- ip4.set_j(ip3.get_j() - a*h);
+ ip4.set_i(ip3.get_i() + b * h);
+ ip4.set_j(ip3.get_j() - a * h);
if (lg > 2 * vpImagePoint::distance(ip2, ip4))
drawLine(ip2, ip4, color, thickness);
@@ -1151,27 +1091,25 @@ void vpD3DRenderer::drawArrow(const vpImagePoint &ip1,
\param imBuffer The texture's data.
\param pitch The texture's pitch.
*/
-void TextureToRGBa(vpImage<vpRGBa>& I, unsigned char * imBuffer,
- unsigned int pitch)
+void TextureToRGBa(vpImage<vpRGBa> &I, unsigned char *imBuffer, unsigned int pitch)
{
unsigned int j = I.getWidth();
- unsigned int k=0;
- for(unsigned int i=0; i<I.getHeight()* I.getWidth(); i++)
- {
- //go to the next line
- if(j==0){
+ unsigned int k = 0;
+ for (unsigned int i = 0; i < I.getHeight() * I.getWidth(); i++) {
+ // go to the next line
+ if (j == 0) {
k += pitch - (I.getWidth() * 4);
j = I.getWidth();
}
- //simple conversion from bgra to rgba
- I.bitmap[i].B = imBuffer[k+0];
- I.bitmap[i].G = imBuffer[k+1];
- I.bitmap[i].R = imBuffer[k+2];
- I.bitmap[i].A = imBuffer[k+3];
+ // simple conversion from bgra to rgba
+ I.bitmap[i].B = imBuffer[k + 0];
+ I.bitmap[i].G = imBuffer[k + 1];
+ I.bitmap[i].R = imBuffer[k + 2];
+ I.bitmap[i].A = imBuffer[k + 3];
- k+=4;
+ k += 4;
j--;
}
}
@@ -1182,43 +1120,42 @@ void TextureToRGBa(vpImage<vpRGBa>& I, unsigned char * imBuffer,
*/
void vpD3DRenderer::getImage(vpImage<vpRGBa> &I)
{
- //if the device has been initialized
- if(pd3dDevice != NULL)
- {
+ // if the device has been initialized
+ if (pd3dDevice != NULL) {
- //resize the destination image as needed
+ // resize the destination image as needed
I.resize(m_rheight, m_rwidth);
D3DLOCKED_RECT d3dLRect;
RECT r;
- r.top = 0;
- r.left = 0;
- r.bottom = static_cast<LONG>( m_rheight );
- r.right = static_cast<LONG>( m_rwidth );
-
- //locks the whole texture to directly access it
- if(pd3dText->LockRect(0, &d3dLRect, &r, 0)!= D3D_OK)
- {
- vpCERROR<<"D3D : Couldn't lock the texture!"<<std::endl;
+ r.top = 0;
+ r.left = 0;
+ r.bottom = static_cast<LONG>(m_rheight);
+ r.right = static_cast<LONG>(m_rwidth);
+
+ // locks the whole texture to directly access it
+ if (pd3dText->LockRect(0, &d3dLRect, &r, 0) != D3D_OK) {
+ vpCERROR << "D3D : Couldn't lock the texture!" << std::endl;
return;
}
- //gets the buffer and pitch of the texture
+ // gets the buffer and pitch of the texture
unsigned int pitch = static_cast<unsigned int>(d3dLRect.Pitch);
- unsigned char * buf = (unsigned char *) d3dLRect.pBits;
+ unsigned char *buf = (unsigned char *)d3dLRect.pBits;
- //fills this image with the texture's data
- TextureToRGBa(I,buf, pitch);
+ // fills this image with the texture's data
+ TextureToRGBa(I, buf, pitch);
- //unlocks the texture
- if( pd3dText->UnlockRect(0) != D3D_OK)
- vpCERROR<<"D3D : Couldn't unlock the texture!"<<std::endl;
+ // unlocks the texture
+ if (pd3dText->UnlockRect(0) != D3D_OK)
+ vpCERROR << "D3D : Couldn't unlock the texture!" << std::endl;
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpD3DRenderer.cpp.o) has no symbols
-void dummy_vpD3DRenderer() {};
+// Work arround to avoid warning: libvisp_core.a(vpD3DRenderer.cpp.o) has no
+// symbols
+void dummy_vpD3DRenderer(){};
#endif
#endif
diff --git a/modules/gui/src/display/windows/vpDisplayD3D.cpp b/modules/gui/src/display/windows/vpDisplayD3D.cpp
index e093e41..13d4fba 100644
--- a/modules/gui/src/display/windows/vpDisplayD3D.cpp
+++ b/modules/gui/src/display/windows/vpDisplayD3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,32 +36,31 @@
*
*****************************************************************************/
-/*!
+/*!
\file vpDisplayD3D.cpp
\brief windows 32 display using D3D
-*/
+*/
#include <visp3/core/vpConfig.h>
-#if ( defined(_WIN32) & defined(VISP_HAVE_D3D9) )
+#if (defined(_WIN32) & defined(VISP_HAVE_D3D9))
-#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpD3DRenderer.h>
+#include <visp3/gui/vpDisplayD3D.h>
/*!
\brief Basic constructor.
*/
-vpDisplayD3D::vpDisplayD3D(): vpDisplayWin32(new vpD3DRenderer()){}
+vpDisplayD3D::vpDisplayD3D() : vpDisplayWin32(new vpD3DRenderer()) {}
/*!
\brief Constructor : Initialize a display.
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
+ \param winx, winy The window is set at position x,y (column index, row
+ index). \param title Window's title.
*/
-vpDisplayD3D::vpDisplayD3D(int winx, int winy, const 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;
@@ -77,20 +77,24 @@ vpDisplayD3D::vpDisplayD3D(int winx, int winy, const std::string &title)
(32 bits).
\param I : Image to be displayed (note that image has to be initialized).
-\param winx, winy : The window is set at position x,y (column index, row index).
-\param title : Window's title.
-\param scaleType : If this parameter is set to:
+\param winx, winy : The window is set at position x,y (column index, row
+index). \param title : Window's title. \param scaleType : If this parameter is
+set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+and the columns.
*/
-vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType scaleType)
- : vpDisplayWin32(new vpD3DRenderer())
+vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
init(I);
@@ -102,25 +106,28 @@ vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, vpScaleType scaleType)
(32 bits).
\param I : Image to be displayed (note that image has to be initialized).
-\param winx, winy : The window is set at position x,y (column index, row index).
-\param title : Window's title.
-\param scaleType : If this parameter is set to:
+\param winx, winy : The window is set at position x,y (column index, row
+index). \param title : Window's title. \param scaleType : If this parameter is
+set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+and the columns.
*/
-vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I,
- int winx, int winy,
- const std::string &title, vpScaleType scaleType)
+vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I, int winx, int winy, const std::string &title, vpScaleType scaleType)
: vpDisplayWin32(new vpD3DRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init(I,winx,winy,title);
+ init(I, winx, winy, title);
}
/*!
@@ -132,15 +139,19 @@ vpDisplayD3D::vpDisplayD3D(vpImage<vpRGBa> &I,
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+and the columns.
*/
-vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I, vpScaleType scaleType)
- : vpDisplayWin32(new vpD3DRenderer())
+vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I, vpScaleType scaleType) : vpDisplayWin32(new vpD3DRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
init(I);
@@ -157,29 +168,33 @@ vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I, vpScaleType scaleType)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+and the columns.
*/
-vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I,
- int winx, int winy,
- const std::string &title, vpScaleType scaleType)
+vpDisplayD3D::vpDisplayD3D(vpImage<unsigned char> &I, int winx, int winy, const std::string &title,
+ vpScaleType scaleType)
: vpDisplayWin32(new vpD3DRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init(I,winx,winy,title);
+ init(I, winx, winy, title);
}
/*!
\brief Basic destructor.
*/
-vpDisplayD3D::~vpDisplayD3D(){}
+vpDisplayD3D::~vpDisplayD3D() {}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayD3D.cpp.o) has no symbols
-void dummy_vpDisplayD3D() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayD3D.cpp.o) has no
+// symbols
+void dummy_vpDisplayD3D(){};
#endif
-
diff --git a/modules/gui/src/display/windows/vpDisplayGDI.cpp b/modules/gui/src/display/windows/vpDisplayGDI.cpp
index b89aec4..b4b9ef0 100644
--- a/modules/gui/src/display/windows/vpDisplayGDI.cpp
+++ b/modules/gui/src/display/windows/vpDisplayGDI.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,27 +42,27 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) )
+#if (defined(VISP_HAVE_GDI))
#include <visp3/gui/vpDisplayGDI.h>
-//A vpDisplayGDI is just a vpDisplayWin32 which uses a vpGDIRenderer to do the drawing.
+// A vpDisplayGDI is just a vpDisplayWin32 which uses a vpGDIRenderer to do
+// the drawing.
/*!
\brief Basic constructor.
*/
-vpDisplayGDI::vpDisplayGDI(): vpDisplayWin32(new vpGDIRenderer()){}
+vpDisplayGDI::vpDisplayGDI() : vpDisplayWin32(new vpGDIRenderer()) {}
/*!
\brief Constructor : Initialize a display.
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
+ \param winx, winy The window is set at position x,y (column index, row
+ index). \param title Window's title.
*/
-vpDisplayGDI::vpDisplayGDI(int winx, int winy, const 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;
@@ -81,15 +82,19 @@ vpDisplayGDI::vpDisplayGDI(int winx, int winy, const std::string &title)
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I, vpScaleType scaleType)
- : vpDisplayWin32(new vpGDIRenderer())
+vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I, vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
init(I);
@@ -101,25 +106,28 @@ vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I, vpScaleType scaleType)
(32 bits).
\param I : image to be displayed (note that image has to be initialized).
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
- \param scaleType : If this parameter is set to:
+ \param winx, winy The window is set at position x,y (column index, row
+ index). \param title Window's title. \param scaleType : If this parameter
+ is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I,
- int winx, int winy,
- const std::string &title, vpScaleType scaleType)
+vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I, int winx, int winy, const std::string &title, vpScaleType scaleType)
: vpDisplayWin32(new vpGDIRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init(I,winx,winy,title);
+ init(I, winx, winy, title);
}
/*!
@@ -131,15 +139,19 @@ vpDisplayGDI::vpDisplayGDI(vpImage<vpRGBa> &I,
\param scaleType : If this parameter is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I, vpScaleType scaleType)
- : vpDisplayWin32(new vpGDIRenderer())
+vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I, vpScaleType scaleType) : vpDisplayWin32(new vpGDIRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
init(I);
@@ -151,33 +163,38 @@ vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I, vpScaleType scaleType)
(8 bits).
\param I Image to be displayed (note that image has to be initialized).
- \param winx, winy The window is set at position x,y (column index, row index).
- \param title Window's title.
- \param scaleType : If this parameter is set to:
+ \param winx, winy The window is set at position x,y (column index, row
+ index). \param title Window's title. \param scaleType : If this parameter
+ is set to:
- vpDisplay::SCALE_AUTO, the display size is adapted to ensure the image
is fully displayed in the screen;
- - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the same than the image size.
- - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines and the columns.
- - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines and the columns.
- - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines and the columns.
- - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines and the columns.
+ - vpDisplay::SCALE_DEFAULT or vpDisplay::SCALE_1, the display size is the
+ same than the image size.
+ - vpDisplay::SCALE_2, the display size is downscaled by 2 along the lines
+ and the columns.
+ - vpDisplay::SCALE_3, the display size is downscaled by 3 along the lines
+ and the columns.
+ - vpDisplay::SCALE_4, the display size is downscaled by 4 along the lines
+ and the columns.
+ - vpDisplay::SCALE_5, the display size is downscaled by 5 along the lines
+ and the columns.
*/
-vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I,
- int winx, int winy,
- const std::string &title, vpScaleType scaleType)
+vpDisplayGDI::vpDisplayGDI(vpImage<unsigned char> &I, int winx, int winy, const std::string &title,
+ vpScaleType scaleType)
: vpDisplayWin32(new vpGDIRenderer())
{
setScale(scaleType, I.getWidth(), I.getHeight());
- init(I,winx,winy,title);
+ init(I, winx, winy, title);
}
/*!
\brief Basic destructor.
*/
-vpDisplayGDI::~vpDisplayGDI(){}
+vpDisplayGDI::~vpDisplayGDI() {}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayGDI.cpp.o) has no symbols
-void dummy_vpDisplayGDI() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayGDI.cpp.o) has no
+// symbols
+void dummy_vpDisplayGDI(){};
#endif
diff --git a/modules/gui/src/display/windows/vpDisplayWin32.cpp b/modules/gui/src/display/windows/vpDisplayWin32.cpp
index 5428b2f..61c41c6 100644
--- a/modules/gui/src/display/windows/vpDisplayWin32.cpp
+++ b/modules/gui/src/display/windows/vpDisplayWin32.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,43 +37,34 @@
*****************************************************************************/
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
-#include <visp3/gui/vpDisplayWin32.h>
-#include <visp3/core/vpDisplayException.h>
#include <string>
+#include <visp3/core/vpDisplayException.h>
+#include <visp3/gui/vpDisplayWin32.h>
-const int vpDisplayWin32::MAX_INIT_DELAY = 5000;
+const int vpDisplayWin32::MAX_INIT_DELAY = 5000;
/*!
Thread entry point.
Used as a detour to initWindow.
*/
-void vpCreateWindow(threadParam * param)
+void vpCreateWindow(threadParam *param)
{
- //char* title = param->title;
- (param->vpDisp)->window.initWindow(param->title.c_str(), param->x, param->y,
- param->w, param->h);
+ // char* title = param->title;
+ (param->vpDisp)->window.initWindow(param->title.c_str(), param->x, param->y, param->w, param->h);
delete param;
}
/*!
Constructor.
*/
-vpDisplayWin32::vpDisplayWin32(vpWin32Renderer * rend) :
- iStatus(false), window(rend)
-{
-}
-
+vpDisplayWin32::vpDisplayWin32(vpWin32Renderer *rend) : iStatus(false), window(rend) {}
/*!
Destructor.
*/
-vpDisplayWin32::~vpDisplayWin32()
-{
- closeDisplay();
-}
-
+vpDisplayWin32::~vpDisplayWin32() { closeDisplay(); }
/*!
@@ -84,28 +76,22 @@ vpDisplayWin32::~vpDisplayWin32()
\param title : Window title.
*/
-void vpDisplayWin32::init(vpImage<unsigned char> &I,
- int x,
- int y,
- const std::string &title)
+void vpDisplayWin32::init(vpImage<unsigned char> &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")) ;
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ vpERROR_TRACE("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);
- window.renderer->setWidth(I.getWidth()/m_scale);
- window.renderer->setHeight(I.getHeight()/m_scale);
+ window.renderer->setWidth(I.getWidth() / m_scale);
+ window.renderer->setHeight(I.getHeight() / m_scale);
window.renderer->setImg(I);
- I.display = this ;
+ I.display = this;
}
-
/*!
Constructor. Initialize a display to visualize a RGBa level image
(32 bits).
@@ -114,28 +100,22 @@ void vpDisplayWin32::init(vpImage<unsigned char> &I,
\param x, y : The window is set at position x,y (column index, row index).
\param title : Window title.
*/
-void vpDisplayWin32::init(vpImage<vpRGBa> &I,
- int x,
- int y,
- const std::string &title)
+void vpDisplayWin32::init(vpImage<vpRGBa> &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")) ;
+ if ((I.getHeight() == 0) || (I.getWidth() == 0)) {
+ vpERROR_TRACE("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) ;
- window.renderer->setWidth(I.getWidth()/m_scale);
- window.renderer->setHeight(I.getHeight()/m_scale);
+ init(I.getWidth(), I.getHeight(), x, y, title);
+ window.renderer->setWidth(I.getWidth() / m_scale);
+ window.renderer->setHeight(I.getHeight() / m_scale);
window.renderer->setImg(I);
- I.display = this ;
+ I.display = this;
}
-
/*!
Initialize the display size, position and title.
@@ -144,9 +124,7 @@ void vpDisplayWin32::init(vpImage<vpRGBa> &I,
\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;
@@ -158,9 +136,9 @@ void vpDisplayWin32::init(unsigned int width, unsigned int height,
if (y != -1)
m_windowYPosition = y;
- //we prepare the window's thread creation
+ // we prepare the window's thread creation
setScale(m_scaleType, width, height);
- threadParam * param = new threadParam;
+ threadParam *param = new threadParam;
param->x = m_windowXPosition;
param->y = m_windowYPosition;
param->w = width / m_scale;
@@ -168,35 +146,30 @@ void vpDisplayWin32::init(unsigned int width, unsigned int height,
param->vpDisp = this;
param->title = this->m_title;
- //creates the window in a separate thread
- hThread = CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)vpCreateWindow,
- param,0,&threadId);
+ // creates the window in a separate thread
+ hThread = CreateThread(NULL, 0, (LPTHREAD_START_ROUTINE)vpCreateWindow, param, 0, &threadId);
- //the initialization worked
+ // the initialization worked
iStatus = (hThread != (HANDLE)NULL);
m_displayHasBeenInitialized = true;
}
-
/*!
If the window is not initialized yet, wait a little (MAX_INIT_DELAY).
\exception notInitializedError : the window isn't initialized
*/
void vpDisplayWin32::waitForInit()
{
- //if the window is not initialized yet
- if(!window.isInitialized())
- {
- //wait
- if( WAIT_OBJECT_0 != WaitForSingleObject(window.semaInit,MAX_INIT_DELAY))
- throw(vpDisplayException(vpDisplayException::notInitializedError,
- "Window not initialized")) ;
- //problem : the window is not initialized
+ // if the window is not initialized yet
+ if (!window.isInitialized()) {
+ // wait
+ if (WAIT_OBJECT_0 != WaitForSingleObject(window.semaInit, MAX_INIT_DELAY))
+ throw(vpDisplayException(vpDisplayException::notInitializedError, "Window not initialized"));
+ // problem : the window is not initialized
}
}
-
/*!
Display the color image \e I in RGBa format (32bits).
@@ -210,16 +183,15 @@ void vpDisplayWin32::waitForInit()
*/
void vpDisplayWin32::displayImage(const vpImage<vpRGBa> &I)
{
- //waits if the window is not initialized
+ // waits if the window is not initialized
waitForInit();
- //sets the image to render
+ // sets the image to render
window.renderer->setImg(I);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+ // sends a message to the window
+ // PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
}
-
/*!
Display a selection of the color image \e I in RGBa format (32bits).
@@ -228,27 +200,27 @@ void vpDisplayWin32::displayImage(const vpImage<vpRGBa> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param width : Width of the region of interest
-
+
\param height : Height of the region of interest
\sa init(), closeDisplay()
*/
-void vpDisplayWin32::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpDisplayWin32::displayImageROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //waits if the window is not initialized
+ // waits if the window is not initialized
waitForInit();
- //sets the image to render
- window.renderer->setImgROI(I,iP,width,height);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+ // sets the image to render
+ window.renderer->setImgROI(I, iP, width, height);
+ // sends a message to the window
+ // PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
}
-
/*!
Display the gray level image \e I (8bits).
@@ -262,13 +234,13 @@ void vpDisplayWin32::displayImageROI ( const vpImage<vpRGBa> &I,const vpImagePoi
*/
void vpDisplayWin32::displayImage(const vpImage<unsigned char> &I)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- //sets the image to render
+ // sets the image to render
window.renderer->setImg(I);
- //sends a message to the window
- //PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
+ // sends a message to the window
+ // PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
}
/*!
@@ -279,27 +251,27 @@ void vpDisplayWin32::displayImage(const vpImage<unsigned char> &I)
\warning Suppress the overlay drawing in the region of interest.
\param I : Image to display.
-
+
\param iP : Top left corner of the region of interest
-
+
\param width : Width of the region of interest
-
+
\param height : Height of the region of interest
\sa init(), closeDisplay()
*/
-void vpDisplayWin32::displayImageROI ( const vpImage<unsigned char> &I,const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpDisplayWin32::displayImageROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //waits if the window is not initialized
+ // waits if the window is not initialized
waitForInit();
- //sets the image to render
- window.renderer->setImgROI(I,iP,width,height);
- //sends a message to the window
- //PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
+ // sets the image to render
+ window.renderer->setImgROI(I, iP, width, height);
+ // sends a message to the window
+ // PostMessage(window.getHWnd(),vpWM_DISPLAY,0,0);
}
-
/*!
Wait for a click from one of the mouse button.
@@ -315,29 +287,27 @@ void vpDisplayWin32::displayImageROI ( const vpImage<unsigned char> &I,const vpI
- false if no button was clicked. This can occur if blocking is set
to \e false.
*/
-bool vpDisplayWin32::getClick( bool blocking)
+bool vpDisplayWin32::getClick(bool blocking)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
bool ret = false;
- //sends a message to the window
+ // sends a message to the window
// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
- //waits for a button to be pressed
- if(blocking){
+ // waits for a button to be pressed
+ if (blocking) {
WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0); //to erase previous events
+ WaitForSingleObject(window.semaClickUp, 0); // to erase previous events
WaitForSingleObject(window.semaClick, INFINITE);
ret = true;
- }
- else {
+ } else {
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
}
return ret;
}
-
/*!
Wait for a click from one of the mouse button and get the position
of the clicked image point.
@@ -356,37 +326,35 @@ bool vpDisplayWin32::getClick( bool blocking)
*/
bool vpDisplayWin32::getClick(vpImagePoint &ip, bool blocking)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- bool ret = false ;
+ bool ret = false;
double u, v;
- //tells the window there has been a getclick demand
+ // tells the window there has been a getclick demand
// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
- //waits for a click
- if(blocking){
+ // waits for a click
+ if (blocking) {
WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
+ WaitForSingleObject(window.semaClickUp, 0); // to erase previous events
WaitForSingleObject(window.semaClick, INFINITE);
ret = true;
- }
- else {
+ } else {
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
}
-
+
u = window.clickX;
v = window.clickY;
- ip.set_u( u*m_scale );
- ip.set_v( v*m_scale);
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
return ret;
}
-
/*!
Wait for a mouse button click and get the position of the clicked
pixel. The button used to click is also set.
-
+
\param ip [out] : The coordinates of the clicked image point.
\param button [out] : The button used to click.
@@ -401,36 +369,32 @@ 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
+ // wait if the window is not initialized
waitForInit();
bool ret = false;
double u, v;
- //tells the window there has been a getclickup demand
+ // tells the window there has been a getclickup demand
// PostMessage(window.getHWnd(), vpWM_GETCLICK, 0,0);
- //waits for a click
- if(blocking){
+ // waits for a click
+ if (blocking) {
WaitForSingleObject(window.semaClick, 0);
- WaitForSingleObject(window.semaClickUp, 0);//to erase previous events
+ WaitForSingleObject(window.semaClickUp, 0); // to erase previous events
WaitForSingleObject(window.semaClick, INFINITE);
ret = true;
- }
- else
+ } else
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClick, 0));
-
+
u = window.clickX;
v = window.clickY;
- ip.set_u( u*m_scale);
- ip.set_v( v*m_scale);
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
button = window.clickButton;
return ret;
}
-
/*!
Wait for a mouse button click release and get the position of the
image point were the click release occurs. The button used to click is
@@ -453,31 +417,28 @@ 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
+ // wait if the window is not initialized
waitForInit();
bool ret = false;
double u, v;
- //tells the window there has been a getclickup demand
+ // tells the window there has been a getclickup demand
// PostMessage(window.getHWnd(), vpWM_GETCLICKUP, 0,0);
- //waits for a click release
- if(blocking){
+ // waits for a click release
+ if (blocking) {
WaitForSingleObject(window.semaClickUp, 0);
- WaitForSingleObject(window.semaClick, 0);//to erase previous events
+ WaitForSingleObject(window.semaClick, 0); // to erase previous events
WaitForSingleObject(window.semaClickUp, INFINITE);
ret = true;
- }
- else
+ } else
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaClickUp, 0));
-
+
u = window.clickXUp;
v = window.clickYUp;
- ip.set_u( u*m_scale);
- ip.set_v( v*m_scale);
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
button = window.clickButtonUp;
return ret;
@@ -498,22 +459,21 @@ bool vpDisplayWin32::getClickUp(vpImagePoint &ip,
- false if no key was pressed. This can occur if blocking is set
to \e false.
*/
-bool vpDisplayWin32::getKeyboardEvent( bool blocking )
+bool vpDisplayWin32::getKeyboardEvent(bool blocking)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- bool ret = false ;
- //waits for a keyboard event
- if(blocking){
+ bool ret = false;
+ // waits for a keyboard event
+ if (blocking) {
WaitForSingleObject(window.semaKey, 0); // key down
WaitForSingleObject(window.semaKey, 0); // key up
WaitForSingleObject(window.semaKey, INFINITE);
ret = true;
- }
- else
+ } else
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
-
+
return ret;
}
/*!
@@ -537,54 +497,51 @@ bool vpDisplayWin32::getKeyboardEvent( bool blocking )
*/
bool vpDisplayWin32::getKeyboardEvent(std::string &key, bool blocking)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- bool ret = false ;
- //waits for a keyboard event
- if(blocking){
+ bool ret = false;
+ // waits for a keyboard event
+ if (blocking) {
WaitForSingleObject(window.semaKey, 0); // key down
WaitForSingleObject(window.semaKey, 0); // key up
WaitForSingleObject(window.semaKey, INFINITE);
ret = true;
- }
- else {
+ } else {
ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaKey, 0));
}
// printf("key: %ud\n", window.key);
std::stringstream ss;
ss << window.lpString;
key = ss.str();
-
+
return ret;
}
/*!
Get the coordinates of the mouse pointer.
-
+
\param ip [out] : The coordinates of the mouse pointer.
-
+
\return true if a pointer motion event was received, false otherwise.
-
+
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayWin32::getPointerMotionEvent (vpImagePoint &ip)
+bool vpDisplayWin32::getPointerMotionEvent(vpImagePoint &ip)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
bool ret = (WAIT_OBJECT_0 == WaitForSingleObject(window.semaMove, 0));
- if (ret)
- {
+ if (ret) {
double u, v;
- //tells the window there has been a getclick demand
- //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
+ // tells the window there has been a getclick demand
+ // PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
u = window.coordX;
v = window.coordY;
- ip.set_u( u*m_scale);
- ip.set_v( v*m_scale);
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
}
return ret;
@@ -600,21 +557,20 @@ vpDisplayWin32::getPointerMotionEvent (vpImagePoint &ip)
\exception vpDisplayException::notInitializedError : If the display
was not initialized.
*/
-bool
-vpDisplayWin32::getPointerPosition (vpImagePoint &ip)
+bool vpDisplayWin32::getPointerPosition(vpImagePoint &ip)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- bool ret = true ;
+ bool ret = true;
double u, v;
- //tells the window there has been a getclick demand
- //PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
-
+ // tells the window there has been a getclick demand
+ // PostMessage(window.getHWnd(), vpWM_GETPOINTERMOTIONEVENT, 0,0);
+
u = window.coordX;
v = window.coordY;
- ip.set_u( u*m_scale);
- ip.set_v( v*m_scale);
+ ip.set_u(u * m_scale);
+ ip.set_v(v * m_scale);
return ret;
}
@@ -622,21 +578,20 @@ vpDisplayWin32::getPointerPosition (vpImagePoint &ip)
/*!
Changes the window's position.
- \param winx, winy : Position of the upper-left window's border in the screen.
+ \param winx, winy : Position of the upper-left window's border in the
+ screen.
*/
void vpDisplayWin32::setWindowPosition(int winx, int winy)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
- //cahange the window position only
- SetWindowPos(window.hWnd,HWND_TOP, winx, winy, 0, 0,
- SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER |SWP_NOSIZE);
-
+ // cahange the window position only
+ SetWindowPos(window.hWnd, HWND_TOP, winx, winy, 0, 0,
+ SWP_ASYNCWINDOWPOS | SWP_NOACTIVATE | SWP_NOZORDER | SWP_NOSIZE);
}
-
/*!
Changes the window's titlebar text
@@ -644,22 +599,17 @@ void vpDisplayWin32::setWindowPosition(int winx, int winy)
*/
void vpDisplayWin32::setTitle(const std::string &windowtitle)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
SetWindowText(window.hWnd, windowtitle.c_str());
}
-
/*!
\brief Set the font used to display text.
\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
@@ -668,11 +618,11 @@ void vpDisplayWin32::setFont(const std::string & /* fontname */)
*/
void vpDisplayWin32::flushDisplay()
{
- //waits if the window is not initialized
+ // waits if the window is not initialized
waitForInit();
- //sends a message to the window
- PostMessage(window.getHWnd(), vpWM_DISPLAY, 0,0);
+ // sends a message to the window
+ PostMessage(window.getHWnd(), vpWM_DISPLAY, 0, 0);
}
/*!
@@ -682,43 +632,40 @@ void vpDisplayWin32::flushDisplay()
*/
void vpDisplayWin32::flushDisplayROI(const vpImagePoint &iP, const unsigned int width, const unsigned int height)
{
- //waits if the window is not initialized
+ // waits if the window is not initialized
waitForInit();
/*
Under windows, flushing an ROI takes more time than
flushing the whole image.
Therefore, we update the maximum area even when asked to update a region.
*/
- WORD left = (WORD)iP.get_u();
- WORD right = (WORD)(iP.get_u()+width-1);
+ WORD left = (WORD)iP.get_u();
+ WORD right = (WORD)(iP.get_u() + width - 1);
- WORD top = (WORD)iP.get_v();
- WORD bottom = (WORD)(iP.get_v()+height-1);
+ WORD top = (WORD)iP.get_v();
+ WORD bottom = (WORD)(iP.get_v() + height - 1);
- //sends a message to the window
+ // sends a message to the window
WPARAM wp = MAKEWPARAM(left, right);
LPARAM lp = MAKELPARAM(top, bottom);
- PostMessage(window.getHWnd(), vpWM_DISPLAY_ROI, wp,lp);
+ PostMessage(window.getHWnd(), vpWM_DISPLAY_ROI, wp, lp);
}
-
/*!
Display a point at the image point \e ip location.
\param ip : Point location.
\param color : Point color.
\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
+ // 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);
+ } else {
+ window.renderer->drawRect(ip, thickness * m_scale, thickness * m_scale, color, true, 1);
}
}
@@ -728,17 +675,14 @@ 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
+ // wait if the window is not initialized
waitForInit();
window.renderer->drawLine(ip1, ip2, color, thickness);
}
-
/*!
Display a dashed line from image point \e ip1 to image point \e ip2.
@@ -748,17 +692,15 @@ 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,
- unsigned int thickness )
+void vpDisplayWin32::displayDotLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
+ unsigned int thickness)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
window.renderer->drawLine(ip1, ip2, color, thickness, PS_DASHDOT);
}
-/*!
+/*!
Display a rectangle with \e topLeft as the top-left corner and \e
width and \e height the rectangle size.
@@ -771,18 +713,15 @@ 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
+ // wait if the window is not initialized
waitForInit();
- window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
+ window.renderer->drawRect(topLeft, width, height, color, fill, thickness);
}
-
-/*!
+/*!
Display a rectangle.
\param topLeft : Top-left corner of the rectangle.
@@ -794,16 +733,14 @@ 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
+ // wait if the window is not initialized
waitForInit();
- unsigned int width = static_cast<unsigned int>( bottomRight.get_j() - topLeft.get_j() );
- unsigned int height = static_cast<unsigned int>(bottomRight.get_i() - topLeft.get_i() );
- window.renderer->drawRect(topLeft,width,height,color, fill, thickness);
+ unsigned int width = static_cast<unsigned int>(bottomRight.get_j() - topLeft.get_j());
+ unsigned int height = static_cast<unsigned int>(bottomRight.get_i() - topLeft.get_i());
+ window.renderer->drawRect(topLeft, width, height, color, fill, thickness);
}
/*!
@@ -817,22 +754,17 @@ 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
+ // wait if the window is not initialized
waitForInit();
vpImagePoint topLeft;
topLeft.set_i(rectangle.getTop());
topLeft.set_j(rectangle.getLeft());
- window.renderer->drawRect(topLeft,
- static_cast<unsigned int>( rectangle.getWidth() ),
- static_cast<unsigned int>( rectangle.getHeight() ),
- color, fill, thickness);
+ window.renderer->drawRect(topLeft, static_cast<unsigned int>(rectangle.getWidth()),
+ static_cast<unsigned int>(rectangle.getHeight()), color, fill, thickness);
}
-
/*!
Display a circle.
\param center : Circle center position.
@@ -842,15 +774,12 @@ 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
+ // wait if the window is not initialized
waitForInit();
- window.renderer->drawCircle(center,radius,color,fill,thickness);
+ window.renderer->drawCircle(center, radius, color, fill, thickness);
}
/*!
@@ -859,13 +788,11 @@ 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
+ // wait if the window is not initialized
waitForInit();
- window.renderer->drawText(ip,text,color);
+ window.renderer->drawText(ip, text, color);
}
/*!
@@ -875,17 +802,14 @@ 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,
- unsigned int thickness)
+void vpDisplayWin32::displayCross(const vpImagePoint &ip, unsigned int size, const vpColor &color,
+ unsigned int thickness)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
window.renderer->drawCross(ip, size, color, thickness);
}
-
/*!
Display an arrow from image point \e ip1 to image point \e ip2.
\param ip1,ip2 : Initial and final image point.
@@ -893,30 +817,26 @@ void vpDisplayWin32::displayCross( const vpImagePoint &ip,
\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
+ // wait if the window is not initialized
waitForInit();
window.renderer->drawArrow(ip1, ip2, color, w, h, thickness);
}
-
/*!
Clears the display.
\param color : the color to fill the display with
*/
-void vpDisplayWin32::clearDisplay(const vpColor &color){
- //wait if the window is not initialized
+void vpDisplayWin32::clearDisplay(const vpColor &color)
+{
+ // wait if the window is not initialized
waitForInit();
window.renderer->clear(color);
}
-
/*!
Closes the display.
Destroys the window.
@@ -925,16 +845,16 @@ void vpDisplayWin32::closeDisplay()
{
if (m_displayHasBeenInitialized) {
waitForInit();
- PostMessage(window.getHWnd(), vpWM_CLOSEDISPLAY, 0,0);
- //if the destructor is called for a reason different than a
- //problem in the thread creation
+ PostMessage(window.getHWnd(), vpWM_CLOSEDISPLAY, 0, 0);
+ // if the destructor is called for a reason different than a
+ // problem in the thread creation
if (iStatus) {
- //waits for the thread to end
+ // waits for the thread to end
WaitForSingleObject(hThread, INFINITE);
CloseHandle(hThread);
}
- m_displayHasBeenInitialized = false ;
- window.initialized = false ;
+ m_displayHasBeenInitialized = false;
+ window.initialized = false;
}
}
@@ -944,7 +864,7 @@ void vpDisplayWin32::closeDisplay()
*/
void vpDisplayWin32::getImage(vpImage<vpRGBa> &I)
{
- //wait if the window is not initialized
+ // wait if the window is not initialized
waitForInit();
window.renderer->getImage(I);
}
@@ -953,7 +873,7 @@ void vpDisplayWin32::getImage(vpImage<vpRGBa> &I)
Gets screen resolution.
\param w, h : Horizontal and vertical screen resolution.
*/
-void vpDisplayWin32::getScreenSize ( unsigned int &w, unsigned int &h )
+void vpDisplayWin32::getScreenSize(unsigned int &w, unsigned int &h)
{
w = GetSystemMetrics(SM_CXSCREEN);
h = GetSystemMetrics(SM_CYSCREEN);
@@ -979,6 +899,7 @@ unsigned int vpDisplayWin32::getScreenHeight()
return height;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpDisplayWin32.cpp.o) has no symbols
-void dummy_vpDisplayWin32() {};
+// Work arround to avoid warning: libvisp_core.a(vpDisplayWin32.cpp.o) has no
+// symbols
+void dummy_vpDisplayWin32(){};
#endif
diff --git a/modules/gui/src/display/windows/vpGDIRenderer.cpp b/modules/gui/src/display/windows/vpGDIRenderer.cpp
index 88c0f35..9d9f8c4 100644
--- a/modules/gui/src/display/windows/vpGDIRenderer.cpp
+++ b/modules/gui/src/display/windows/vpGDIRenderer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,7 @@
#include <visp3/core/vpConfig.h>
#define GDI_ROBUST
-#if ( defined(VISP_HAVE_GDI) )
+#if (defined(VISP_HAVE_GDI))
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -46,56 +47,55 @@
/*!
Constructor.
*/
-vpGDIRenderer::vpGDIRenderer()
- : m_bmp(NULL), m_bmp_width(0), m_bmp_height(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
+ // if the screen depth is not 32bpp, throw an exception
int bpp = GetDeviceCaps(GetDC(NULL), BITSPIXEL);
- if( bpp != 32 )
+ if (bpp != 32)
throw vpDisplayException(vpDisplayException::depthNotSupportedError,
"vpGDIRenderer supports only 32bits depth: screen is %dbits depth!", bpp);
InitializeCriticalSection(&m_criticalSection);
- //initialize GDI the palette
+ // initialize GDI the palette
vpColor pcolor; // Predefined colors
-
+
pcolor = vpColor::black;
- m_colors[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_black] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightBlue;
- m_colors[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_lightBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::blue;
- m_colors[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_blue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkBlue;
- m_colors[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_darkBlue] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::cyan;
- m_colors[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_cyan] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGreen;
- m_colors[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_lightGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::green;
- m_colors[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_green] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGreen;
- m_colors[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_darkGreen] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightRed;
- m_colors[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_lightRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::red;
- m_colors[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_red] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkRed;
- m_colors[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_darkRed] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::white;
- m_colors[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_white] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::lightGray;
- m_colors[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_lightGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::gray;
m_colors[vpColor::id_gray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::darkGray;
- m_colors[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_darkGray] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::yellow;
- m_colors[vpColor::id_yellow]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_yellow] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::orange;
- m_colors[vpColor::id_orange]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_orange] = RGB(pcolor.R, pcolor.G, pcolor.B);
pcolor = vpColor::purple;
- m_colors[vpColor::id_purple]= RGB(pcolor.R, pcolor.G, pcolor.B);
+ m_colors[vpColor::id_purple] = RGB(pcolor.R, pcolor.G, pcolor.B);
m_rwidth = 0;
m_rheight = 0;
@@ -106,11 +106,11 @@ vpGDIRenderer::vpGDIRenderer()
*/
vpGDIRenderer::~vpGDIRenderer()
{
- //Deletes the critical section object
+ // Deletes the critical section object
DeleteCriticalSection(&m_criticalSection);
- //Deletes the bitmap
+ // Deletes the bitmap
DeleteObject(m_bmp);
- //Deletes the font object
+ // Deletes the font object
DeleteObject(m_hFont);
}
@@ -128,11 +128,9 @@ bool vpGDIRenderer::init(HWND hWindow, unsigned int width, unsigned int height)
m_rwidth = 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);
+ // 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);
return true;
}
@@ -140,9 +138,9 @@ bool vpGDIRenderer::init(HWND hWindow, unsigned int width, unsigned int height)
Sets the image to display.
\param I : The rgba image to display.
*/
-void vpGDIRenderer::setImg(const vpImage<vpRGBa>& I)
+void vpGDIRenderer::setImg(const vpImage<vpRGBa> &I)
{
- //converts the image into a HBITMAP
+ // converts the image into a HBITMAP
convert(I, m_bmp);
}
@@ -152,9 +150,10 @@ void vpGDIRenderer::setImg(const vpImage<vpRGBa>& I)
\param iP : Top left coordinates of the ROI.
\param width, height : ROI width and height.
*/
-void vpGDIRenderer::setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpGDIRenderer::setImgROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //converts the image into a HBITMAP
+ // converts the image into a HBITMAP
convertROI(I, iP, width, height);
}
@@ -162,9 +161,9 @@ void vpGDIRenderer::setImgROI(const vpImage<vpRGBa>& I, const vpImagePoint &iP,
Sets the image to display.
\param I : The grayscale image to display.
*/
-void vpGDIRenderer::setImg(const vpImage<unsigned char>& I)
+void vpGDIRenderer::setImg(const vpImage<unsigned char> &I)
{
- //converts the image into a HBITMAP
+ // converts the image into a HBITMAP
convert(I, m_bmp);
}
@@ -174,9 +173,10 @@ void vpGDIRenderer::setImg(const vpImage<unsigned char>& I)
\param iP : Top left coordinates of the ROI.
\param width, height : ROI width and height.
*/
-void vpGDIRenderer::setImgROI(const vpImage<unsigned char>& I, const vpImagePoint &iP, const unsigned int width, const unsigned int height )
+void vpGDIRenderer::setImgROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width,
+ const unsigned int height)
{
- //converts the image into a HBITMAP
+ // converts the image into a HBITMAP
convertROI(I, iP, width, height);
}
@@ -185,25 +185,22 @@ void vpGDIRenderer::setImgROI(const vpImage<unsigned char>& I, const vpImagePoin
*/
bool vpGDIRenderer::render()
{
- //gets the window's DC
+ // gets the window's DC
PAINTSTRUCT ps;
HDC hDCScreen = BeginPaint(m_hWnd, &ps);
- //create a memory DC
+ // create a memory DC
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //selects this bmp in memory
+ // selects this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
-
- //blits it on the window's DC
- BitBlt(hDCScreen, 0, 0,
- static_cast<int>( m_rwidth ),
- static_cast<int>( m_rheight ),
- hDCMem, 0, 0, SRCCOPY);
+
+ // blits it on the window's DC
+ BitBlt(hDCScreen, 0, 0, static_cast<int>(m_rwidth), static_cast<int>(m_rheight), hDCMem, 0, 0, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
- //DeleteDC(hDCMem);
+ // DeleteDC(hDCMem);
DeleteObject(hDCMem);
EndPaint(m_hWnd, &ps);
@@ -216,39 +213,38 @@ bool vpGDIRenderer::render()
\param I The image to convert.
\param hBmp The destination image.
*/
-void vpGDIRenderer::convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp)
+void vpGDIRenderer::convert(const vpImage<vpRGBa> &I, HBITMAP &hBmp)
{
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[m_rwidth * m_rheight * 4];
+ // allocate the buffer
+ 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++) {
- imBuffer[i+0] = I.bitmap[k].B;
- imBuffer[i+1] = I.bitmap[k].G;
- imBuffer[i+2] = I.bitmap[k].R;
- imBuffer[i+3] = I.bitmap[k].A;
+ 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;
+ imBuffer[i + 3] = I.bitmap[k].A;
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int i_ = i*m_rscale;
- unsigned int ii_ = i*m_rwidth;
+ unsigned int i_ = i * m_rscale;
+ unsigned int ii_ = i * m_rwidth;
for (unsigned int j = 0; j < m_rwidth; j++) {
- vpRGBa val = I[i_][j*m_rscale];
+ vpRGBa val = I[i_][j * m_rscale];
unsigned int index_ = (ii_ + j) * 4;
- imBuffer[ index_] = val.B;
+ imBuffer[index_] = val.B;
imBuffer[++index_] = val.G;
imBuffer[++index_] = val.R;
imBuffer[++index_] = val.A;
}
}
}
-
- //updates the bitmap's pixel data
+
+ // updates the bitmap's pixel data
updateBitmap(hBmp, imBuffer, m_rwidth, m_rheight);
- //we don't need this buffer anymore
- delete [] imBuffer;
+ // we don't need this buffer anymore
+ delete[] imBuffer;
}
/*!
@@ -257,88 +253,83 @@ void vpGDIRenderer::convert(const vpImage<vpRGBa> &I, HBITMAP& hBmp)
\param iP : Top left coordinates of the ROI.
\param width, height : ROI width and height.
*/
-void vpGDIRenderer::convertROI(const vpImage<vpRGBa> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
+void vpGDIRenderer::convertROI(const vpImage<vpRGBa> &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_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 h = i_max - i_min;
int w = j_max - j_min;
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[w * h * 4];
+ // allocate the buffer
+ unsigned char *imBuffer = new unsigned char[w * h * 4];
if (m_rscale == 1) {
- vpRGBa* bitmap = I.bitmap;
+ vpRGBa *bitmap = I.bitmap;
unsigned int iwidth = I.getWidth();
- bitmap = bitmap + (int)(i_min*iwidth + j_min);
+ bitmap = bitmap + (int)(i_min * iwidth + j_min);
int k = 0;
- for (int i=0 ; i < w * h * 4 ; i+=4)
- {
- imBuffer[i+0] = (bitmap+k)->B;
- imBuffer[i+1] = (bitmap+k)->G;
- imBuffer[i+2] = (bitmap+k)->R;
- imBuffer[i+3] = (bitmap+k)->A;
- //bitmap++;
+ for (int i = 0; i < w * h * 4; i += 4) {
+ imBuffer[i + 0] = (bitmap + k)->B;
+ imBuffer[i + 1] = (bitmap + k)->G;
+ imBuffer[i + 2] = (bitmap + k)->R;
+ imBuffer[i + 3] = (bitmap + k)->A;
+ // bitmap++;
k++;
- if (k == w)
- {
- bitmap = bitmap+iwidth;
+ if (k == w) {
+ bitmap = bitmap + iwidth;
k = 0;
}
}
- }
- else {
+ } else {
for (int i = 0; i < h; i++) {
- unsigned int i_ = (i_min + i)*m_rscale;
- unsigned int ii_ = i*w;
+ unsigned int i_ = (i_min + i) * m_rscale;
+ unsigned int ii_ = i * w;
for (int j = 0; j < w; j++) {
- vpRGBa val = I[i_][(j_min + j)*m_rscale];
+ vpRGBa val = I[i_][(j_min + j) * m_rscale];
unsigned int index_ = (ii_ + j) * 4;
- imBuffer[index_] = val.B;
+ imBuffer[index_] = val.B;
imBuffer[++index_] = val.G;
imBuffer[++index_] = val.R;
imBuffer[++index_] = val.A;
}
}
}
-
- //updates the bitmap's pixel data
+
+ // updates the bitmap's pixel data
updateBitmapROI(imBuffer, i_min, j_min, w, h);
- //we don't need this buffer anymore
- delete [] imBuffer;
+ // we don't need this buffer anymore
+ delete[] imBuffer;
}
-
/*!
Converts the image form ViSP in GDI's image format (bgra with padding).
\param I The image to convert.
\param hBmp The destination image.
*/
-void vpGDIRenderer::convert(const vpImage<unsigned char> &I, HBITMAP& hBmp)
+void vpGDIRenderer::convert(const vpImage<unsigned char> &I, HBITMAP &hBmp)
{
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[m_rwidth * m_rheight * 4];
+ // allocate the buffer
+ 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];
imBuffer[i + 3] = vpRGBa::alpha_default;
}
- }
- else {
+ } else {
for (unsigned int i = 0; i < m_rheight; i++) {
- unsigned int i_ = i*m_rscale;
- unsigned int ii_ = i*m_rwidth;
+ unsigned int i_ = i * m_rscale;
+ unsigned int ii_ = i * m_rwidth;
for (unsigned int j = 0; j < m_rwidth; j++) {
- unsigned char val = I[i_][j*m_rscale];
+ unsigned char val = I[i_][j * m_rscale];
unsigned int index_ = (ii_ + j) * 4;
imBuffer[index_] = val;
imBuffer[++index_] = val;
@@ -348,37 +339,37 @@ void vpGDIRenderer::convert(const vpImage<unsigned char> &I, HBITMAP& hBmp)
}
}
- //updates the bitmap's pixel data
+ // updates the bitmap's pixel data
updateBitmap(hBmp, imBuffer, m_rwidth, m_rheight);
- //we don't need this buffer anymore
- delete [] imBuffer;
+ // we don't need this buffer anymore
+ delete[] imBuffer;
}
-
/*!
Converts the image form ViSP in GDI's image format (bgra with padding).
\param I The image to convert.
\param iP : Top left coordinates of the ROI.
\param width, height : ROI width and height.
*/
-void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int width, const unsigned int height)
+void vpGDIRenderer::convertROI(const vpImage<unsigned char> &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_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 h = i_max - i_min;
int w = j_max - j_min;
- //allocate the buffer
- unsigned char * imBuffer = new unsigned char[w * h * 4];
-
+ // allocate the buffer
+ unsigned char *imBuffer = new unsigned char[w * h * 4];
+
if (m_rscale == 1) {
for (int i = 0; i < h; i++) {
unsigned int i_ = i_min + i;
- unsigned int ii_ = i*w;
+ unsigned int ii_ = i * w;
for (int j = 0; j < w; j++) {
unsigned char val = I[i_][j_min + j];
unsigned int index_ = (ii_ + j) * 4;
@@ -388,13 +379,12 @@ void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoi
imBuffer[++index_] = vpRGBa::alpha_default;
}
}
- }
- else {
+ } else {
for (int i = 0; i < h; i++) {
- unsigned int i_ = (i_min + i)*m_rscale;
- unsigned int ii_ = i*w;
+ unsigned int i_ = (i_min + i) * m_rscale;
+ unsigned int ii_ = i * w;
for (int j = 0; j < w; j++) {
- unsigned char val = I[i_][(j_min + j)*m_rscale];
+ unsigned char val = I[i_][(j_min + j) * m_rscale];
unsigned int index_ = (ii_ + j) * 4;
imBuffer[index_] = val;
imBuffer[++index_] = val;
@@ -404,11 +394,11 @@ void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoi
}
}
- //updates the bitmap's pixel data
+ // updates the bitmap's pixel data
updateBitmapROI(imBuffer, i_min, j_min, w, h);
- //we don't need this buffer anymore
- delete [] imBuffer;
+ // we don't need this buffer anymore
+ delete[] imBuffer;
}
/*!
@@ -421,29 +411,23 @@ void vpGDIRenderer::convertROI(const vpImage<unsigned char> &I, const vpImagePoi
\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
+ // the bitmap may only be accessed by one thread at the same time
+ // that's why we enter critical section
EnterCriticalSection(&m_criticalSection);
- //if the existing bitmap object is of the right size
- if( (m_bmp_width == w) && (m_bmp_height == h) && w != 0 && h != 0)
- {
- //just replace the content
+ // if the existing bitmap object is of the right size
+ if ((m_bmp_width == w) && (m_bmp_height == h) && w != 0 && h != 0) {
+ // just replace the content
SetBitmapBits(hBmp, w * h * 4, imBuffer);
- }
- else
- {
- if(hBmp != NULL)
- {
- //delete the old BITMAP
+ } else {
+ if (hBmp != NULL) {
+ // delete the old BITMAP
DeleteObject(hBmp);
}
- //create a new BITMAP from this buffer
- if( (hBmp = CreateBitmap(static_cast<int>(w),static_cast<int>(h),
- 1,32,(void*)imBuffer)) == NULL)
+ // create a new BITMAP from this buffer
+ if ((hBmp = CreateBitmap(static_cast<int>(w), static_cast<int>(h), 1, 32, (void *)imBuffer)) == NULL)
return false;
m_bmp_width = w;
@@ -454,7 +438,6 @@ bool vpGDIRenderer::updateBitmap(HBITMAP& hBmp, unsigned char * imBuffer,
return true;
}
-
/*!
Updates the bitmap to display.
Contains a critical section.
@@ -465,21 +448,21 @@ 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);
+ HBITMAP htmp = CreateBitmap(w, h, 1, 32, (void *)imBuffer);
- //get the window's DC
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
HDC hDCMem2 = CreateCompatibleDC(hDCScreen);
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
SelectObject(hDCMem2, htmp);
- BitBlt(hDCMem, j_min, i_min, w, h, hDCMem2, 0, 0,SRCCOPY);
+ BitBlt(hDCMem, j_min, i_min, w, h, hDCMem2, 0, 0, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
DeleteDC(hDCMem);
@@ -495,26 +478,23 @@ bool vpGDIRenderer::updateBitmapROI(unsigned char * imBuffer, int i_min, int j_m
\param iP : The pixel coordinates.
\param color : the color of the point.
*/
-void vpGDIRenderer::setPixel(const vpImagePoint &iP,
- const vpColor &color)
+void vpGDIRenderer::setPixel(const vpImagePoint &iP, const vpColor &color)
{
- //get the window's DC
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
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)
+ // display the result (flush)
// BitBlt(hDCScreen, x, y, 1, 1, hDCMem, x, y, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
@@ -530,47 +510,46 @@ void vpGDIRenderer::setPixel(const vpImagePoint &iP,
\param thickness : Thickness of the line.
\param style style of the line
*/
-void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
- const vpImagePoint &ip2,
- const vpColor &color,
+void vpGDIRenderer::drawLine(const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color,
unsigned int thickness, int style)
{
- HDC hDCScreen= NULL, hDCMem = NULL;
+ HDC hDCScreen = NULL, hDCMem = NULL;
HPEN hPen = NULL;
#ifdef GDI_ROBUST
double start = vpTime::measureTimeMs();
- while(vpTime::measureTimeMs()-start<1000){
+ while (vpTime::measureTimeMs() - start < 1000) {
hDCScreen = GetDC(m_hWnd);
- if(!hDCScreen) continue;
+ if (!hDCScreen)
+ continue;
hDCMem = CreateCompatibleDC(hDCScreen);
- if(!hDCMem){
+ if (!hDCMem) {
ReleaseDC(m_hWnd, hDCScreen);
continue;
}
- //create the pen
+ // create the pen
if (color.id < vpColor::id_unknown)
hPen = CreatePen(style, static_cast<int>(thickness), m_colors[color.id]);
else {
COLORREF gdicolor = RGB(color.R, color.G, color.B);
hPen = CreatePen(style, static_cast<int>(thickness), gdicolor);
}
- if(!hPen){
+ if (!hPen) {
DeleteDC(hDCMem);
ReleaseDC(m_hWnd, hDCScreen);
continue;
}
- if(!SetBkMode(hDCMem, TRANSPARENT)){
+ if (!SetBkMode(hDCMem, TRANSPARENT)) {
DeleteObject(hPen);
DeleteDC(hDCMem);
ReleaseDC(m_hWnd, hDCScreen);
continue;
}
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
- if(!SelectObject(hDCMem, m_bmp)){
+ if (!SelectObject(hDCMem, m_bmp)) {
LeaveCriticalSection(&m_criticalSection);
DeleteObject(hPen);
DeleteDC(hDCMem);
@@ -578,8 +557,8 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
continue;
}
- //select the pen
- if(!SelectObject(hDCMem, hPen)){
+ // select the pen
+ if (!SelectObject(hDCMem, hPen)) {
LeaveCriticalSection(&m_criticalSection);
DeleteObject(hPen);
DeleteDC(hDCMem);
@@ -588,12 +567,12 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
}
break;
}
- timelost+=(vpTime::measureTimeMs()-start);
+ timelost += (vpTime::measureTimeMs() - start);
#else
- //get the window's DC
+ // get the window's DC
hDCScreen = GetDC(m_hWnd);
hDCMem = CreateCompatibleDC(hDCScreen);
- //create the pen
+ // create the pen
if (color.id < vpColor::id_unknown)
hPen = CreatePen(style, static_cast<int>(thickness), m_colors[color.id]);
else {
@@ -602,34 +581,34 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
}
SetBkMode(hDCMem, TRANSPARENT);
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //select the pen
+ // select the pen
SelectObject(hDCMem, hPen);
#endif
- // Warning: When thickness > 1 and pen style is PS_DASHDOT, the drawing displays a solid line
- // That's why in that case we implement the dashdot line manually drawing multiple small lines
+ // Warning: When thickness > 1 and pen style is PS_DASHDOT, the drawing
+ // displays a solid line That's why in that case we implement the dashdot
+ // line manually drawing multiple small lines
if (thickness != 1 && style != PS_SOLID) {
- double size = 10.*m_rscale;
+ 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 deltaj = size / length*(ip2.get_j() - ip1.get_j());
- double deltai = size / length*(ip2.get_i() - ip1.get_i());
+ 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();
+ 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)) {
- double i = slope*j + orig;
- //move to the starting point
+ double i = slope * j + orig;
+ // move to the starting point
MoveToEx(hDCMem, vpMath::round(j / m_rscale), vpMath::round(i / m_rscale), NULL);
- //Draw the line
+ // Draw the line
LineTo(hDCMem, vpMath::round((j + deltaj) / m_rscale), vpMath::round((i + deltai) / m_rscale));
}
- }
- else {
- //move to the starting point
+ } else {
+ // move to the starting point
MoveToEx(hDCMem, vpMath::round(ip1.get_u() / m_rscale), vpMath::round(ip1.get_v() / m_rscale), NULL);
- //Draw the line
+ // Draw the line
LineTo(hDCMem, vpMath::round(ip2.get_u() / m_rscale), vpMath::round(ip2.get_v() / m_rscale));
}
@@ -649,19 +628,18 @@ void vpGDIRenderer::drawLine(const vpImagePoint &ip1,
\param fill When set to true fill the rectangle.
\param thickness : Line thickness
*/
-void vpGDIRenderer::drawRect(const vpImagePoint &topLeft,
- unsigned int width, unsigned int height,
- const vpColor &color, bool fill,
- unsigned int thickness)
+void vpGDIRenderer::drawRect(const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color,
+ bool fill, unsigned int thickness)
{
- if (thickness == 0) thickness = 1;
- //get the window's DC
+ if (thickness == 0)
+ thickness = 1;
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //create the pen
+ // create the pen
HPEN hPen;
- COLORREF gdicolor = RGB(0,0,0);
+ COLORREF gdicolor = RGB(0, 0, 0);
if (color.id < vpColor::id_unknown)
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), m_colors[color.id]);
@@ -670,34 +648,33 @@ void vpGDIRenderer::drawRect(const vpImagePoint &topLeft,
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
}
- //create an hollow or solid brush (depends on boolean fill)
+ // create an hollow or solid brush (depends on boolean fill)
LOGBRUSH lBrush;
- if(fill) {
+ if (fill) {
lBrush.lbStyle = BS_SOLID;
if (color.id < vpColor::id_unknown)
lBrush.lbColor = m_colors[color.id];
else {
lBrush.lbColor = gdicolor;
}
- }
- else lBrush.lbStyle = BS_HOLLOW;
+ } else
+ lBrush.lbStyle = BS_HOLLOW;
HBRUSH hbrush = CreateBrushIndirect(&lBrush);
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //select the brush
+ // select the brush
SelectObject(hDCMem, hbrush);
- //select the pen
+ // select the pen
SelectObject(hDCMem, hPen);
- //draw the rectangle
+ // 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));
+ vpMath::round((topLeft.get_u() + width) / m_rscale), vpMath::round((topLeft.get_v() + height) / m_rscale));
- //display the result (flush)
+ // display the result (flush)
// BitBlt(hDCScreen, j, i, width, height, hDCMem, j, i, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
@@ -720,7 +697,6 @@ void vpGDIRenderer::clear(const vpColor &color)
drawRect(ip, m_rwidth, m_rheight, color, true, 0);
}
-
/*!
Draws a circle.
\param center its center point's coordinates
@@ -729,15 +705,15 @@ 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
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //create the pen
+ // create the pen
HPEN hPen;
if (color.id < vpColor::id_unknown)
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), m_colors[color.id]);
@@ -746,35 +722,33 @@ void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
}
- //create an hollow brush
+ // create an hollow brush
LOGBRUSH lBrush;
lBrush.lbStyle = BS_HOLLOW;
HBRUSH hbrush = CreateBrushIndirect(&lBrush);
- //computes bounding rectangle
+ // computes bounding rectangle
int radius_ = static_cast<int>(radius);
- int x1 = vpMath::round(center.get_u() / m_rscale)-radius_ / m_rscale;
- int y1 = vpMath::round(center.get_v() / m_rscale)-radius_ / m_rscale;
- int x2 = vpMath::round(center.get_u() / m_rscale)+radius_ / m_rscale;
- int y2 = vpMath::round(center.get_v() / m_rscale)+radius_ / m_rscale;
+ int x1 = vpMath::round(center.get_u() / m_rscale) - radius_ / m_rscale;
+ int y1 = vpMath::round(center.get_v() / m_rscale) - radius_ / m_rscale;
+ int x2 = vpMath::round(center.get_u() / m_rscale) + radius_ / m_rscale;
+ int y2 = vpMath::round(center.get_v() / m_rscale) + radius_ / m_rscale;
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //select the brush
+ // select the brush
SelectObject(hDCMem, hbrush);
- //select the pen
+ // select the pen
SelectObject(hDCMem, hPen);
- //draw the circle
- if (fill==false)
+ // draw the circle
+ if (fill == false)
Ellipse(hDCMem, x1, y1, x2, y2);
- else
- {
- while (x2-x1 > 0)
- {
+ else {
+ while (x2 - x1 > 0) {
x1++;
x2--;
y1++;
@@ -783,7 +757,7 @@ void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
}
}
- //display the result (flush)
+ // display the result (flush)
// BitBlt(hDCScreen, x1, y1, x2-x1, y2-y1, hDCMem, x1, y1, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
@@ -794,28 +768,26 @@ void vpGDIRenderer::drawCircle(const vpImagePoint ¢er, unsigned int radius,
ReleaseDC(m_hWnd, hDCScreen);
}
-
/*!
Draws some text.
\param ip its top left point's coordinates
\param text The string to display
\param color The text's color
*/
-void vpGDIRenderer::drawText(const vpImagePoint &ip, const char * text,
- const vpColor &color)
+void vpGDIRenderer::drawText(const vpImagePoint &ip, const char *text, const vpColor &color)
{
- //get the window's DC
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //Select the font
+ // Select the font
SelectObject(hDCMem, m_hFont);
- //set the text color
+ // set the text color
if (color.id < vpColor::id_unknown)
SetTextColor(hDCMem, m_colors[color.id]);
else {
@@ -823,19 +795,19 @@ void vpGDIRenderer::drawText(const vpImagePoint &ip, const char * text,
SetTextColor(hDCMem, gdicolor);
}
- //we don't use the bkColor
+ // we don't use the bkColor
SetBkMode(hDCMem, TRANSPARENT);
SIZE size;
- int length = (int) strlen(text);
+ int length = (int)strlen(text);
- //get the displayed string dimensions
+ // get the displayed string dimensions
GetTextExtentPoint32(hDCMem, text, length, &size);
- //displays the string
+ // displays the string
TextOut(hDCMem, vpMath::round(ip.get_u() / m_rscale), vpMath::round(ip.get_v() / m_rscale), text, length);
- //display the result (flush)
+ // display the result (flush)
// BitBlt(hDCScreen, j, i, size.cx, size.cy, hDCMem, j, i, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
@@ -844,8 +816,6 @@ void vpGDIRenderer::drawText(const vpImagePoint &ip, const char * text,
ReleaseDC(m_hWnd, hDCScreen);
}
-
-
/*!
Draws a cross.
\param ip its center point's coordinates
@@ -853,20 +823,19 @@ 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<int>( size/2 / m_rscale);
+ /* unsigned */ int half_size = static_cast<int>(size / 2 / m_rscale);
// if half_size is equal to zero, nothing is displayed with the code
// just below. So, if half_size is equal to zero we just draw the
// pixel.
if (half_size) {
- //get the window's DC
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //create the pen
+ // create the pen
HPEN hPen;
if (color.id < vpColor::id_unknown)
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), m_colors[color.id]);
@@ -875,24 +844,24 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size,
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
}
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //select the pen
+ // select the pen
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);
- //Draw the first line (horizontal)
- 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) - 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));
- //move to the starting point
- 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);
+ // move to the starting point
+ 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);
- //display the result (flush)
+ // display the result (flush)
// BitBlt(hDCScreen, j-(size/2), i-(size/2), size, size,
// hDCMem, j-(size/2), i-(size/2), SRCCOPY);
@@ -901,8 +870,7 @@ void vpGDIRenderer::drawCross(const vpImagePoint &ip, unsigned int size,
DeleteObject(hPen);
DeleteDC(hDCMem);
ReleaseDC(m_hWnd, hDCScreen);
- }
- else {
+ } else {
setPixel(ip, color);
}
}
@@ -914,26 +882,24 @@ 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;
- double lg = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
+ double lg = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
- //computes the coordinates of the rectangle to blit later
+ // computes the coordinates of the rectangle to blit later
// unsigned int x = (j2 >= j1) ? j1 : j2;
// unsigned int y = (i2 >= i1) ? i1 : i2;
// unsigned int w = (j2 >= j1) ? j2-j1 : j1-j2;
// unsigned int h = (i2 >= i1) ? i2-i1 : i1-i2;
- //get the window's DC
+ // get the window's DC
HDC hDCScreen = GetDC(m_hWnd);
HDC hDCMem = CreateCompatibleDC(hDCScreen);
- //create the pen
+ // create the pen
HPEN hPen;
if (color.id < vpColor::id_unknown)
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), m_colors[color.id]);
@@ -942,50 +908,45 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1,
hPen = CreatePen(PS_SOLID, static_cast<int>(thickness), gdicolor);
}
- //select this bmp in memory
+ // select this bmp in memory
EnterCriticalSection(&m_criticalSection);
SelectObject(hDCMem, m_bmp);
- //select the pen
+ // select the pen
SelectObject(hDCMem, hPen);
-
- if ((a==0)&&(b==0))
- {
+ if ((a == 0) && (b == 0)) {
// DisplayCrossLarge(i1,j1,3,col) ;
- }
- else
- {
- a /= lg ;
- b /= lg ;
+ } else {
+ a /= lg;
+ b /= lg;
vpImagePoint ip3;
- ip3.set_i( ip2.get_i() / m_rscale - w*a );
- ip3.set_j( ip2.get_j() / m_rscale - w*b );
-
+ ip3.set_i(ip2.get_i() / m_rscale - w * a);
+ ip3.set_j(ip2.get_j() / m_rscale - w * b);
vpImagePoint ip4;
- //double t = 0 ;
- //while (t<=_l)
+ // double t = 0 ;
+ // while (t<=_l)
{
- ip4.set_i( ip3.get_i() - b*h );
- ip4.set_j( ip3.get_j() + a*h );
+ ip4.set_i(ip3.get_i() - b * h);
+ ip4.set_j(ip3.get_j() + a * h);
- if (lg > 2*vpImagePoint::distance(ip2 / m_rscale, ip4) ) {
+ 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()));
}
// t+=0.1 ;
}
- //t = 0 ;
- //while (t>= -_l)
+ // t = 0 ;
+ // while (t>= -_l)
{
- ip4.set_i( ip3.get_i() + b*h );
- ip4.set_j( ip3.get_j() - a*h );
+ ip4.set_i(ip3.get_i() + b * h);
+ ip4.set_j(ip3.get_j() - a * h);
- if (lg > 2*vpImagePoint::distance(ip2 / m_rscale, ip4) ) {
+ 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()));
}
@@ -995,8 +956,8 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1,
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)
+
+ // display the result (flush)
// BitBlt(hDCScreen, x, y, w, h, hDCMem, x, y, SRCCOPY);
LeaveCriticalSection(&m_criticalSection);
@@ -1012,29 +973,29 @@ void vpGDIRenderer::drawArrow(const vpImagePoint &ip1,
*/
void vpGDIRenderer::getImage(vpImage<vpRGBa> &I)
{
- //size of image buffer : m_rwidth*m_rheight*4
- unsigned int size = m_rwidth*m_rheight *4;
- unsigned char * imBuffer = new unsigned char[size];
+ // size of image buffer : m_rwidth*m_rheight*4
+ unsigned int size = m_rwidth * m_rheight * 4;
+ unsigned char *imBuffer = new unsigned char[size];
- //gets the hbitmap's bitmap
+ // gets the hbitmap's bitmap
GetBitmapBits(m_bmp, static_cast<LONG>(size), (void *)imBuffer);
- //resize the destination image as needed
+ // resize the destination image as needed
I.resize(m_rheight, m_rwidth);
- //copy the content
- for(unsigned int i=0 ; i<size ; i+=4)
- {
- I.bitmap[i>>2].R = imBuffer[i+2];
- I.bitmap[i>>2].G = imBuffer[i+1];
- I.bitmap[i>>2].B = imBuffer[i+0];
- I.bitmap[i>>2].A = vpRGBa::alpha_default; // default opacity
+ // copy the content
+ for (unsigned int i = 0; i < size; i += 4) {
+ I.bitmap[i >> 2].R = imBuffer[i + 2];
+ I.bitmap[i >> 2].G = imBuffer[i + 1];
+ I.bitmap[i >> 2].B = imBuffer[i + 0];
+ I.bitmap[i >> 2].A = vpRGBa::alpha_default; // default opacity
}
- delete [] imBuffer;
+ delete[] imBuffer;
}
#endif
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpGDIRenderer.cpp.o) has no symbols
-void dummy_vpGDIRenderer() {};
+// Work arround to avoid warning: libvisp_core.a(vpGDIRenderer.cpp.o) has no
+// symbols
+void dummy_vpGDIRenderer(){};
#endif
diff --git a/modules/gui/src/display/windows/vpWin32API.cpp b/modules/gui/src/display/windows/vpWin32API.cpp
index e5c291c..da0624b 100644
--- a/modules/gui/src/display/windows/vpWin32API.cpp
+++ b/modules/gui/src/display/windows/vpWin32API.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,66 +36,67 @@
*
*****************************************************************************/
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
-#include <visp3/gui/vpWin32API.h>
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
#include <visp3/core/vpTime.h>
-DWORD vpProcessErrors(const std::string &api_name){
+#include <visp3/gui/vpWin32API.h>
+DWORD vpProcessErrors(const std::string &api_name)
+{
LPVOID lpMsgBuf;
DWORD err = GetLastError();
-
- FormatMessage(
- FORMAT_MESSAGE_ALLOCATE_BUFFER |
- FORMAT_MESSAGE_FROM_SYSTEM |
- FORMAT_MESSAGE_IGNORE_INSERTS,
- NULL,
- err,
- MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),
- (LPTSTR) &lpMsgBuf,
- 0, NULL );
- std::cout << "call to " << api_name << " failed with the following error code: " << err << "(" << (LPTSTR)lpMsgBuf << ")" << std::endl;
+
+ FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS, NULL, err,
+ MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), (LPTSTR)&lpMsgBuf, 0, NULL);
+ std::cout << "call to " << api_name << " failed with the following error code: " << err << "(" << (LPTSTR)lpMsgBuf
+ << ")" << std::endl;
return err;
}
-BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd){
+BOOL vpLineTo(HDC hdc, int nXEnd, int nYEnd)
+{
BOOL ret = LineTo(hdc, nXEnd, nYEnd);
- if(ret == 0)
+ if (ret == 0)
vpProcessErrors("LineTo");
return ret;
}
-BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint){
+BOOL vpMoveToEx(HDC hdc, int X, int Y, LPPOINT lpPoint)
+{
BOOL ret = MoveToEx(hdc, X, Y, lpPoint);
- if(ret == 0)
+ if (ret == 0)
vpProcessErrors("MoveToEx");
return ret;
}
-BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc, DWORD dwRop){
- BOOL ret = BitBlt(hdcDest,nXDest,nYDest, nWidth, nHeight, hdcSrc, nXSrc, nYSrc, dwRop);
- if(ret == 0)
+BOOL vpBitBlt(HDC hdcDest, int nXDest, int nYDest, int nWidth, int nHeight, HDC hdcSrc, int nXSrc, int nYSrc,
+ DWORD dwRop)
+{
+ BOOL ret = BitBlt(hdcDest, nXDest, nYDest, nWidth, nHeight, hdcSrc, nXSrc, nYSrc, dwRop);
+ if (ret == 0)
vpProcessErrors("BitBlt");
return ret;
}
-BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase){
- BOOL ret = InvalidateRect(hWnd,lpRect,bErase);
- if(ret == 0)
+BOOL vpInvalidateRect(HWND hWnd, const RECT *lpRect, BOOL bErase)
+{
+ BOOL ret = InvalidateRect(hWnd, lpRect, bErase);
+ if (ret == 0)
vpProcessErrors("InvalidateRect");
return ret;
}
-void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h){
-
- HGDIOBJ ret = SelectObject(hDCMem,h);
- if(ret==NULL){
+void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h)
+{
+
+ HGDIOBJ ret = SelectObject(hDCMem, h);
+ if (ret == NULL) {
vpProcessErrors("SelectObject");
double ms = vpTime::measureTimeMs();
- while(ret==NULL && vpTime::measureTimeMs()-ms<5000){
+ while (ret == NULL && vpTime::measureTimeMs() - ms < 5000) {
DeleteObject(h);
DeleteDC(hDCMem);
ReleaseDC(hWnd, hDC);
@@ -102,39 +104,39 @@ void vpSelectObject(HWND hWnd, HDC hDC, HDC hDCMem, HGDIOBJ h){
}
}
-BOOL vpReleaseSemaphore(HANDLE hSemaphore,LONG IReleaseCount,LPLONG lpPreviousCount){
- BOOL ret = ReleaseSemaphore(hSemaphore,IReleaseCount,lpPreviousCount);
+BOOL vpReleaseSemaphore(HANDLE hSemaphore, LONG IReleaseCount, LPLONG lpPreviousCount)
+{
+ BOOL ret = ReleaseSemaphore(hSemaphore, IReleaseCount, lpPreviousCount);
#ifndef __MINGW32__
- if(ret==0){
+ if (ret == 0) {
vpProcessErrors("ReleaseSemaphore");
}
#endif
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){
+COLORREF vpSetPixel(HDC hdc, int X, int Y, COLORREF crColor)
+{
COLORREF ret = SetPixel(hdc, X, Y, crColor);
- if(ret == 0)
+ if (ret == 0)
vpProcessErrors("SetPixel");
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)
+ if (ret == NULL)
vpProcessErrors("CreateBitmap");
return ret;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpWin32API.cpp.o) has no symbols
-void dummy_vpWin32API() {};
+// Work arround to avoid warning: libvisp_core.a(vpWin32API.cpp.o) has no
+// symbols
+void dummy_vpWin32API(){};
#endif
diff --git a/modules/gui/src/display/windows/vpWin32Window.cpp b/modules/gui/src/display/windows/vpWin32Window.cpp
index 324fbd6..ec8c118 100644
--- a/modules/gui/src/display/windows/vpWin32Window.cpp
+++ b/modules/gui/src/display/windows/vpWin32Window.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,11 +36,11 @@
* Anthony Saunier
*
*****************************************************************************/
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
#include <visp3/gui/vpWin32API.h>
-#if ( defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) )
+#if (defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9))
#define MAX_SEM_COUNT 2147483647
@@ -47,48 +48,45 @@
#include <visp3/gui/vpWin32Window.h>
-//Should be already defined ...
+// Should be already defined ...
#ifndef GET_X_LPARAM
-# define GET_X_LPARAM(lp) ((int)(short)LOWORD(lp))
+#define GET_X_LPARAM(lp) ((int)(short)LOWORD(lp))
#endif
#ifndef GET_Y_LPARAM
-# define GET_Y_LPARAM(lp) ((int)(short)HIWORD(lp))
+#define GET_Y_LPARAM(lp) ((int)(short)HIWORD(lp))
#endif
-//declares the window as thread local
-//allows multiple displays
+// declares the window as thread local
+// allows multiple displays
#ifdef __MINGW32__
-__thread vpWin32Window * window;
+__thread vpWin32Window *window;
#else
-__declspec(thread) vpWin32Window * window;
+__declspec(thread) vpWin32Window *window;
#endif
-bool vpWin32Window::registered = false;
+bool vpWin32Window::registered = false;
/*!
The message callback
*/
LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
{
- //the first time this callback is executed, put the window in initialized state
- if (window != NULL)
- {
- if (!window->isInitialized())
- {
+ // the first time this callback is executed, put the window in initialized
+ // state
+ if (window != NULL) {
+ if (!window->isInitialized()) {
window->initialized = true;
- vpReleaseSemaphore(window->semaInit,1,NULL);
+ vpReleaseSemaphore(window->semaInit, 1, NULL);
}
}
- switch (message)
- {
+ switch (message) {
case vpWM_DISPLAY:
- //redraw the whole window
+ // redraw the whole window
InvalidateRect(window->getHWnd(), NULL, TRUE);
UpdateWindow(window->getHWnd());
break;
- case vpWM_DISPLAY_ROI:
- {
+ case vpWM_DISPLAY_ROI: {
RECT rect;
rect.left = LOWORD(wParam);
@@ -99,104 +97,90 @@ LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
InvalidateRect(window->getHWnd(), &rect, TRUE);
UpdateWindow(window->getHWnd());
- }
- break;
+ } break;
- case WM_LBUTTONDOWN:
- {
+ case WM_LBUTTONDOWN: {
window->clickX = GET_X_LPARAM(lParam);
window->clickY = GET_Y_LPARAM(lParam);
window->clickButton = vpMouseButton::button1;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaClick, 1, NULL);
+ } break;
- case WM_MBUTTONDOWN:
- {
+ case WM_MBUTTONDOWN: {
window->clickX = GET_X_LPARAM(lParam);
window->clickY = GET_Y_LPARAM(lParam);
window->clickButton = vpMouseButton::button2;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaClick, 1, NULL);
+ } break;
- case WM_RBUTTONDOWN:
- {
+ case WM_RBUTTONDOWN: {
window->clickX = GET_X_LPARAM(lParam);
window->clickY = GET_Y_LPARAM(lParam);
window->clickButton = vpMouseButton::button3;
- vpReleaseSemaphore(window->semaClick,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaClick, 1, NULL);
+ } break;
- case WM_LBUTTONUP:
- {
+ case WM_LBUTTONUP: {
window->clickXUp = GET_X_LPARAM(lParam);
window->clickYUp = GET_Y_LPARAM(lParam);
window->clickButtonUp = vpMouseButton::button1;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaClickUp, 1, NULL);
+ } break;
- case WM_MBUTTONUP:
- {
+ case WM_MBUTTONUP: {
window->clickXUp = GET_X_LPARAM(lParam);
window->clickYUp = GET_Y_LPARAM(lParam);
window->clickButtonUp = vpMouseButton::button2;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaClickUp, 1, NULL);
+ } break;
- case WM_RBUTTONUP:
- {
+ case WM_RBUTTONUP: {
window->clickXUp = GET_X_LPARAM(lParam);
window->clickYUp = GET_Y_LPARAM(lParam);
window->clickButtonUp = vpMouseButton::button3;
- vpReleaseSemaphore(window->semaClickUp,1,NULL);
- }
- break;
- case WM_MOUSEMOVE:
- {
+ vpReleaseSemaphore(window->semaClickUp, 1, NULL);
+ } break;
+ case WM_MOUSEMOVE: {
window->coordX = GET_X_LPARAM(lParam);
window->coordY = GET_Y_LPARAM(lParam);
- vpReleaseSemaphore(window->semaMove,1,NULL);
- }
- break;
+ vpReleaseSemaphore(window->semaMove, 1, NULL);
+ } break;
case WM_SYSKEYDOWN:
- // case WM_SYSKEYUP:
+ // case WM_SYSKEYUP:
case WM_KEYDOWN:
- //case WM_KEYUP:
- {
- GetKeyNameText((LONG)lParam, window->lpString, 10); // 10 is the size of lpString
- //window->key = MapVirtualKey(wParam, MAPVK_VK_TO_CHAR);
- vpReleaseSemaphore(window->semaKey,1,NULL);
- break;
- }
+ // case WM_KEYUP:
+ {
+ GetKeyNameText((LONG)lParam, window->lpString,
+ 10); // 10 is the size of lpString
+ // window->key = MapVirtualKey(wParam, MAPVK_VK_TO_CHAR);
+ vpReleaseSemaphore(window->semaKey, 1, NULL);
+ break;
+ }
case WM_COMMAND:
break;
- //we must prevent the window from erasing the background each time a
- //repaint is needed
+ // we must prevent the window from erasing the background each time a
+ // repaint is needed
case WM_ERASEBKGND:
return (LRESULT)1;
case WM_PAINT:
- //render the display
+ // render the display
window->renderer->render();
break;
case vpWM_CLOSEDISPLAY:
- //cleanup code here, if needed
- //destroys the window
+ // cleanup code here, if needed
+ // destroys the window
DestroyWindow(hWnd);
break;
@@ -212,18 +196,19 @@ LRESULT CALLBACK WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam)
/*!
Constructor.
*/
-vpWin32Window::vpWin32Window(vpWin32Renderer * rend): initialized(false)
+vpWin32Window::vpWin32Window(vpWin32Renderer *rend) : initialized(false)
{
renderer = rend;
-
- //registered is static member class and is initialized at the beginning of this file (registered = false)
-
- //creates the semaphores
- semaInit = CreateSemaphore(NULL,0,1,NULL);
- semaClick = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaClickUp = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaKey = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
- semaMove = CreateSemaphore(NULL,0,MAX_SEM_COUNT,NULL);
+
+ // registered is static member class and is initialized at the beginning of
+ // this file (registered = false)
+
+ // creates the semaphores
+ semaInit = CreateSemaphore(NULL, 0, 1, NULL);
+ semaClick = CreateSemaphore(NULL, 0, MAX_SEM_COUNT, NULL);
+ semaClickUp = CreateSemaphore(NULL, 0, MAX_SEM_COUNT, NULL);
+ semaKey = CreateSemaphore(NULL, 0, MAX_SEM_COUNT, NULL);
+ semaMove = CreateSemaphore(NULL, 0, MAX_SEM_COUNT, NULL);
}
/*!
@@ -239,7 +224,6 @@ vpWin32Window::~vpWin32Window()
CloseHandle(semaMove);
}
-
/*!
A standard window creation procedure.
Initialize the renderer and associate it with this thread
@@ -250,77 +234,74 @@ 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
+ // the window's style
DWORD style = WS_OVERLAPPEDWINDOW | WS_VISIBLE;
const char g_szClassName[] = "ViSPWindowClass";
RECT rect;
- rect.left=0;
- rect.right=static_cast<int>(w);
- rect.top=0;
- rect.bottom=static_cast<int>(h);
+ rect.left = 0;
+ rect.right = static_cast<int>(w);
+ rect.top = 0;
+ rect.bottom = static_cast<int>(h);
- //now we register the window's class
+ // now we register the window's class
WNDCLASSEX wcex;
wcex.cbSize = sizeof(WNDCLASSEX);
- wcex.style = CS_HREDRAW | CS_VREDRAW | CS_NOCLOSE ;
+ wcex.style = CS_HREDRAW | CS_VREDRAW | CS_NOCLOSE;
wcex.lpfnWndProc = (WNDPROC)WndProc;
- wcex.cbClsExtra = 0;
- wcex.cbWndExtra = 0;
- wcex.hInstance = hInst;
- wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
- wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
- wcex.hbrBackground =(HBRUSH)(COLOR_WINDOW+1);
+ wcex.cbClsExtra = 0;
+ wcex.cbWndExtra = 0;
+ wcex.hInstance = hInst;
+ wcex.hIcon = LoadIcon(NULL, IDI_APPLICATION);
+ wcex.hCursor = LoadCursor(NULL, IDC_ARROW);
+ wcex.hbrBackground = (HBRUSH)(COLOR_WINDOW + 1);
wcex.lpszMenuName = NULL;
wcex.lpszClassName = g_szClassName;
- wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
-
+ wcex.hIconSm = LoadIcon(NULL, IDI_APPLICATION);
+
RegisterClassEx(&wcex);
AdjustWindowRectEx(&rect, style, false, style);
- // std::cout << "win client size (orig)(w,h): " << rect.left << " " << rect.top << " " << rect.right << " " << rect.bottom << std::endl;
-
- //creates the window
- hWnd = CreateWindowEx(WS_EX_APPWINDOW, g_szClassName, title, style,
- posx, posy, rect.right-rect.left, rect.bottom-rect.top, NULL, NULL, hInst, NULL);
- if (hWnd == NULL)
- {
- DWORD err= GetLastError();
+ // std::cout << "win client size (orig)(w,h): " << rect.left << " " <<
+ // rect.top << " " << rect.right << " " << rect.bottom << std::endl;
+
+ // creates the window
+ hWnd = CreateWindowEx(WS_EX_APPWINDOW, g_szClassName, title, style, posx, posy, rect.right - rect.left,
+ rect.bottom - rect.top, NULL, NULL, hInst, NULL);
+ if (hWnd == NULL) {
+ DWORD err = GetLastError();
std::cout << "err CreateWindowEx=" << err << std::endl;
- throw vpDisplayException(vpDisplayException::cannotOpenWindowError,
- "Can't create the window!");
+ throw vpDisplayException(vpDisplayException::cannotOpenWindowError, "Can't create the window!");
}
SetWindowPos(hWnd, NULL, 0, 0, rect.right - rect.left, rect.bottom - rect.top, SWP_NOZORDER | SWP_NOMOVE);
- //needed if we want to access it from the callback method (message handler)
+ // needed if we want to access it from the callback method (message handler)
window = this;
- //initialize the renderer
+ // initialize the renderer
renderer->init(hWnd, w, h);
- //displays the window
+ // displays the window
ShowWindow(hWnd, SW_SHOWDEFAULT);
- //ShowWindow(hWnd, SW_SHOW);
+ // ShowWindow(hWnd, SW_SHOW);
UpdateWindow(hWnd);
MSG msg;
- //starts the message loop
- while (true)
- {
+ // starts the message loop
+ while (true) {
BOOL val = GetMessage(&msg, NULL, 0, 0);
- if(val==-1){
+ if (val == -1) {
std::cout << "GetMessage error:" << GetLastError() << std::endl;
break;
- }else if(val==0){
+ } else if (val == 0) {
break;
- }else{
- if (!TranslateAccelerator(msg.hwnd, NULL, &msg))
- {
+ } else {
+ if (!TranslateAccelerator(msg.hwnd, NULL, &msg)) {
TranslateMessage(&msg);
DispatchMessage(&msg);
}
@@ -330,6 +311,7 @@ void vpWin32Window::initWindow(const char* title, int posx, int posy, unsigned i
#endif
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpWin32Window.cpp.o) has no symbols
-void dummy_vpWin32Window() {};
+// Work arround to avoid warning: libvisp_core.a(vpWin32Window.cpp.o) has no
+// symbols
+void dummy_vpWin32Window(){};
#endif
diff --git a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp
index 6153e89..581301a 100644
--- a/modules/gui/src/forward-projection/vpProjectionDisplay.cpp
+++ b/modules/gui/src/forward-projection/vpProjectionDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,22 +36,21 @@
*
*****************************************************************************/
-
/*!
\file vpProjectionDisplay.cpp
\brief interface with the image for feature display
*/
#include <visp3/core/vpConfig.h>
-#if defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_DISPLAY)
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
-//Color / image / display
+// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
@@ -60,9 +60,7 @@
//#include <visp3/visual_features/vpBasicFeature.h>
-
-void
-vpProjectionDisplay::insert( vpForwardProjection &fp)
+void vpProjectionDisplay::insert(vpForwardProjection &fp)
{
// vpForwardProjection *f ;
// f = fp.duplicate() ;
@@ -71,105 +69,87 @@ vpProjectionDisplay::insert( vpForwardProjection &fp)
listFp.push_back(&fp);
}
-void
-vpProjectionDisplay::init()
+void vpProjectionDisplay::init()
{
- o.setWorldCoordinates(0,0,0) ;
- x.setWorldCoordinates(0.1,0,0) ;
- y.setWorldCoordinates(0,0.1,0) ;
- z.setWorldCoordinates(0,0,0.1) ;
- traj.resize(0,2);
+ o.setWorldCoordinates(0, 0, 0);
+ x.setWorldCoordinates(0.1, 0, 0);
+ y.setWorldCoordinates(0, 0.1, 0);
+ z.setWorldCoordinates(0, 0, 0.1);
+ traj.resize(0, 2);
}
-void
-vpProjectionDisplay::init(const int select)
+void vpProjectionDisplay::init(const int select)
{
- if (select & vpProjectionDisplay::internalView())
- {
- Icam.resize(256,256) ;
- dIcam.init(Icam,100,100) ;
+ if (select & vpProjectionDisplay::internalView()) {
+ Icam.resize(256, 256);
+ dIcam.init(Icam, 100, 100);
}
- if (select & vpProjectionDisplay::externalView())
- {
- Iext.resize(256,256) ;
- dIext.init(Iext,400,100) ;
+ if (select & vpProjectionDisplay::externalView()) {
+ Iext.resize(256, 256);
+ dIext.init(Iext, 400, 100);
}
- init() ;
+ init();
}
+void vpProjectionDisplay::close() {}
-void
-vpProjectionDisplay::close()
-{
-
-}
-
-void
-vpProjectionDisplay::display(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const vpColor &color,
- const bool &displayTraj,
- const unsigned int thickness)
+void vpProjectionDisplay::display(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &color,
+ const bool &displayTraj, const unsigned int thickness)
{
- for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin() ; it != listFp.end(); ++it )
- {
- vpForwardProjection *fp = *it ;
- fp->display(I,cextMo, cam, color, thickness) ;
- }
+ for (std::list<vpForwardProjection *>::const_iterator it = listFp.begin(); it != listFp.end(); ++it) {
+ vpForwardProjection *fp = *it;
+ fp->display(I, cextMo, cam, color, thickness);
+ }
- if(displayTraj) // display past camera positions
- for(unsigned int i=0;i<traj.getRows();++i)
- vpDisplay::displayCircle(I,(int)traj[i][0],(int)traj[i][1],2,vpColor::green,true);
+ if (displayTraj) // display past camera positions
+ for (unsigned int i = 0; i < traj.getRows(); ++i)
+ vpDisplay::displayCircle(I, (int)traj[i][0], (int)traj[i][1], 2, vpColor::green, true);
- displayCamera(I, cextMo, cMo, cam, thickness);
+ displayCamera(I, cextMo, cMo, cam, thickness);
- if(displayTraj) // store current camera position
- {
- const unsigned int n = traj.getRows();
- traj.resize(n+1, 2, false);
- vpMeterPixelConversion::convertPoint(cam,o.p[0],o.p[1],traj[n][1],traj[n][0]);
- }
+ if (displayTraj) // store current camera position
+ {
+ const unsigned int n = traj.getRows();
+ traj.resize(n + 1, 2, false);
+ vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], traj[n][1], traj[n][0]);
+ }
}
-
-void
-vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cextMo,
- const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam,
- const unsigned int thickness)
+void vpProjectionDisplay::displayCamera(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cextMo,
+ const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const unsigned int thickness)
{
- vpHomogeneousMatrix c1Mc ;
- c1Mc = cextMo*cMo.inverse() ;
+ vpHomogeneousMatrix c1Mc;
+ c1Mc = cextMo * cMo.inverse();
- o.track(c1Mc) ;
+ o.track(c1Mc);
- if(o.get_Z() < 0) // do not print camera if behind the external camera
- return;
+ if (o.get_Z() < 0) // do not print camera if behind the external camera
+ return;
- x.track(c1Mc) ;
- y.track(c1Mc) ;
- z.track(c1Mc) ;
+ x.track(c1Mc);
+ y.track(c1Mc);
+ z.track(c1Mc);
vpImagePoint ipo;
vpImagePoint ipx;
- vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo) ;
+ vpMeterPixelConversion::convertPoint(cam, o.p[0], o.p[1], ipo);
- vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4+thickness, 2+thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, x.p[0], x.p[1], ipx);
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::green, 4 + thickness, 2 + thickness, thickness);
- vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4+thickness, 2+thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, y.p[0], y.p[1], ipx);
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::blue, 4 + thickness, 2 + thickness, thickness);
- vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx) ;
- vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4+thickness, 2+thickness, thickness) ;
+ vpMeterPixelConversion::convertPoint(cam, z.p[0], z.p[1], ipx);
+ vpDisplay::displayArrow(I, ipo, ipx, vpColor::red, 4 + thickness, 2 + thickness, thickness);
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpProjectionDisplay.cpp.o) has no symbols
-void dummy_vpProjectionDisplay() {};
+// Work arround to avoid warning: libvisp_core.a(vpProjectionDisplay.cpp.o)
+// has no symbols
+void dummy_vpProjectionDisplay(){};
#endif
diff --git a/modules/gui/src/plot/vpPlot.cpp b/modules/gui/src/plot/vpPlot.cpp
index b7ca04a..f6b6c55 100644
--- a/modules/gui/src/plot/vpPlot.cpp
+++ b/modules/gui/src/plot/vpPlot.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,23 +36,21 @@
*
*****************************************************************************/
-
-
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_DISPLAY)
-#include <visp3/gui/vpPlot.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpMeterPixelConversion.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <fstream>
#include <list>
#include <vector>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
/*!
Default constructor.
@@ -59,8 +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.)
{
}
/*!
@@ -70,21 +68,20 @@ vpPlot::vpPlot() : I(), display(NULL), graphNbr(1), graphList(NULL), margei(30),
\warning You can modify the default window size, but this is not advised.
\warning Under Unix, when X11 is used to display the curves, the
- default font is set to "-adobe-times-medium-i-normal--10-100-75-75-p-52-iso8859-*".
- Note that you can chose an other one using "xfontsel".
+ default font is set to
+ "-adobe-times-medium-i-normal--10-100-75-75-p-52-iso8859-*". Note that you
+ can chose an other one using "xfontsel".
\param graph_nbr : The number of graph in the window.
\param height : Height of the window.
\param width : Width of the window.
\param x,y : The window is set at position x,y (column index, row index).
- \param title : Window title.
+ \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);
}
@@ -98,14 +95,13 @@ vpPlot::vpPlot(const unsigned int graph_nbr,
\param height : Height of the window.
\param width : Width of the window.
\param x,y : The window is set at position x,y (column index, row index).
- \param title : Window title.
+ \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);
-
+ I.init(height, width, 255);
+
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#elif defined VISP_HAVE_GDI
@@ -119,12 +115,12 @@ void vpPlot::init(const unsigned int graph_nbr,
#endif
display->init(I, x, y, title.c_str());
-
+
vpDisplay::display(I);
-
- factori = height/700.0f;
- factorj = width/700.0f;
-
+
+ factori = height / 700.0f;
+ factorj = width / 700.0f;
+
initNbGraph(graph_nbr);
}
@@ -133,59 +129,65 @@ void vpPlot::init(const unsigned int graph_nbr,
*/
vpPlot::~vpPlot()
{
- if (graphList != NULL)
- {
+ if (graphList != NULL) {
delete[] graphList;
graphList = NULL;
}
- if (display != NULL)
- {
+ if (display != NULL) {
delete display;
display = NULL;
}
}
/*!
- Function called by the constructor to initialize the window and the basic parameters.
+ Function called by the constructor to initialize the window and the basic
+ parameters.
\exception vpException::dimensionError if the parameter exceed the possible
number of graph (4).
\param graphNbr : The number of graph in the window.
*/
-void
-vpPlot::initNbGraph (unsigned int nbGraph)
+void vpPlot::initNbGraph(unsigned int nbGraph)
{
- if(nbGraph > 4){
+ if (nbGraph > 4) {
throw vpException(vpException::dimensionError, "Cannot create more than 4 graphs");
}
graphList = new vpPlotGraph[nbGraph];
graphNbr = nbGraph;
-
- switch (nbGraph)
- {
- case 1 :
- graphList[0].initSize(vpImagePoint(0,0), (unsigned int)(700*factorj),(unsigned int)(700*factori),margei,margej);
- break;
- case 2 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- break;
- case 3 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(700*factorj),(unsigned int)(350*factori),margei,margej);
- break;
- case 4 :
- graphList[0].initSize(vpImagePoint(0,0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[1].initSize(vpImagePoint(0,(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[2].initSize(vpImagePoint((unsigned int)(350*factori),0),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- graphList[3].initSize(vpImagePoint((unsigned int)(350*factori),(unsigned int)(350*factorj)),(unsigned int)(350*factorj),(unsigned int)(350*factori),margei,margej);
- break;
+
+ switch (nbGraph) {
+ case 1:
+ graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(700 * factorj), (unsigned int)(700 * factori), margei,
+ margej);
+ break;
+ case 2:
+ graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(700 * factorj), (unsigned int)(350 * factori), margei,
+ margej);
+ graphList[1].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(700 * factorj),
+ (unsigned int)(350 * factori), margei, margej);
+ break;
+ case 3:
+ graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei,
+ margej);
+ graphList[1].initSize(vpImagePoint(0, (unsigned int)(350 * factorj)), (unsigned int)(350 * factorj),
+ (unsigned int)(350 * factori), margei, margej);
+ graphList[2].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(700 * factorj),
+ (unsigned int)(350 * factori), margei, margej);
+ break;
+ case 4:
+ graphList[0].initSize(vpImagePoint(0, 0), (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei,
+ margej);
+ graphList[1].initSize(vpImagePoint(0, (unsigned int)(350 * factorj)), (unsigned int)(350 * factorj),
+ (unsigned int)(350 * factori), margei, margej);
+ graphList[2].initSize(vpImagePoint((unsigned int)(350 * factori), 0), (unsigned int)(350 * factorj),
+ (unsigned int)(350 * factori), margei, margej);
+ graphList[3].initSize(vpImagePoint((unsigned int)(350 * factori), (unsigned int)(350 * factorj)),
+ (unsigned int)(350 * factorj), (unsigned int)(350 * factori), margei, margej);
+ break;
}
-
- for (unsigned int i = 0; i < graphNbr; i++)
- {
+
+ for (unsigned int i = 0; i < graphNbr; i++) {
graphList[i].title.clear();
graphList[i].unitx.clear();
graphList[i].unity.clear();
@@ -194,22 +196,19 @@ vpPlot::initNbGraph (unsigned int nbGraph)
}
/*!
- Function which enables to initialize the number of curves which belongs to a graphic.
+ Function which enables to initialize the number of curves which belongs to a
+ graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNbr : The number of curves belonging to the graphic
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNbr : The number of curves belonging to the graphic
*/
-void
-vpPlot::initGraph (unsigned int graphNum, unsigned int curveNbr)
-{
- (graphList+graphNum)->initGraph(curveNbr);
-}
-
+void vpPlot::initGraph(unsigned int graphNum, unsigned int curveNbr) { (graphList + graphNum)->initGraph(curveNbr); }
// void
-// vpPlot::initRange (const int graphNum,
-// double xmin, double xmax, double /*xdelt*/,
-// double ymin, double ymax, double /*ydelt*/,
+// vpPlot::initRange (const int graphNum,
+// double xmin, double xmax, double /*xdelt*/,
+// double ymin, double ymax, double /*ydelt*/,
// const bool gx, const bool gy)
// {
// (graphList+graphNum)->initScale(I,xmin,xmax,10,ymin,ymax,10,gx,gy);
@@ -218,404 +217,415 @@ vpPlot::initGraph (unsigned int graphNum, unsigned int curveNbr)
/*!
This method enables to set the initial range of the selected graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param xmin : The initial minimum value along the x axis given in the user coordinates.
- \param xmax : The initial maximum value along the x axis given in the user coordinates.
- \param ymin : The initial minimum value along the y axis given in the user coordinates.
- \param ymax : The initial maximum value along the y axis given in the user coordinates.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param xmin : The initial minimum value along the x axis given in the
+ user coordinates. \param xmax : The initial maximum value along the x axis
+ given in the user coordinates. \param ymin : The initial minimum value along
+ the y axis given in the user coordinates. \param ymax : The initial maximum
+ value along the y axis given in the user coordinates.
*/
-void
-vpPlot::initRange (const unsigned int graphNum,
- double xmin, double xmax,
- double ymin, double ymax)
+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);
}
/*!
This method enables to set the initial range of the selected graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param xmin : The initial minimum value along the x axis given in the user coordinates.
- \param xmax : The initial maximum value along the x axis given in the user coordinates.
- \param ymin : The initial minimum value along the y axis given in the user coordinates.
- \param ymax : The initial maximum value along the y axis given in the user coordinates.
- \param zmin : The initial minimum value along the z axis given in the user coordinates.
- \param zmax : The initial maximum value along the z axis given in the user coordinates.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param xmin : The initial minimum value along the x axis given in the
+ user coordinates. \param xmax : The initial maximum value along the x axis
+ given in the user coordinates. \param ymin : The initial minimum value along
+ the y axis given in the user coordinates. \param ymax : The initial maximum
+ value along the y axis given in the user coordinates. \param zmin : The
+ initial minimum value along the z axis given in the user coordinates. \param
+ zmax : The initial maximum value along the z axis given in the user
+ coordinates.
*/
-void
-vpPlot::initRange (const unsigned int graphNum,
- double xmin, double xmax, double ymin,
- double ymax, double zmin, double zmax)
+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);
}
/*!
This function enables you to choose the color used to draw a given curve.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
- \param color : The color you want to use
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNum : The index of the curve in the list of the curves
+ belonging to the graphic. \param color : The color you want to use
*/
-void
-vpPlot::setColor (const unsigned int graphNum, const unsigned int curveNum, vpColor color)
+void vpPlot::setColor(const unsigned int graphNum, const unsigned int curveNum, vpColor color)
{
- (graphList+graphNum)->setCurveColor(curveNum, color);
+ (graphList + graphNum)->setCurveColor(curveNum, color);
}
/*!
display the grid for all graphics.
*/
-void
-vpPlot::displayGrid()
+void vpPlot::displayGrid()
{
for (unsigned int i = 0; i < graphNbr; i++)
graphList[i].displayGrid(I);
}
/*!
- This function enables you to add a new point in the curve. This point is drawn with the parameters of the curve.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
- \param x : The coordinate of the new point along the x axis and given in the user unit system.
- \param y : The coordinate of the new point along the y axis and given in the user unit system.
+ This function enables you to add a new point in the curve. This point is
+ drawn with the parameters of the curve.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNum : The index of the curve in the list of the curves
+ belonging to the graphic. \param x : The coordinate of the new point along
+ the x axis and given in the user unit system. \param y : The coordinate of
+ the new point along the y axis and given in the user unit system.
*/
-void
-vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
+void vpPlot::plot(const unsigned int graphNum, const unsigned int curveNum, const double x, const double y)
{
- (graphList+graphNum)->plot(I,curveNum,x,y);
+ (graphList + graphNum)->plot(I, curveNum, x, y);
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system.
*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpColVector &v_y)
+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)
+ if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
+ for (unsigned int i = 0; i < v_y.getRows(); ++i)
this->plot(graphNum, i, x, v_y[i]);
- }
- else
+ } else
vpTRACE("error in plot vector : not the right dimension");
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system.
*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpRowVector &v_y)
+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)
+ if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
+ for (unsigned int i = 0; i < v_y.getRows(); ++i)
this->plot(graphNum, i, x, v_y[i]);
- }
- else
+ } else
vpTRACE("error in plot vector : not the right dimension");
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system.
*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpPoseVector &v_y)
+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)
+ if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
+ for (unsigned int i = 0; i < v_y.getRows(); ++i)
this->plot(graphNum, i, x, v_y[i]);
- }
- else
+ } else
vpTRACE("error in plot vector : not the right dimension");
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system.
*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpTranslationVector &v_y)
+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)
+ if ((graphList + graphNum)->curveNbr == v_y.getRows()) {
+ for (unsigned int i = 0; i < v_y.getRows(); ++i)
this->plot(graphNum, i, x, v_y[i]);
- }
- else
+ } else
vpTRACE("error in plot vector : not the right dimension");
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system.
*/
-void vpPlot::plot(const unsigned int graphNum,
- const double x, const vpRotationVector &v_y)
+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)
+ if ((graphList + graphNum)->curveNbr == v_y.size()) {
+ for (unsigned int i = 0; i < v_y.size(); ++i)
this->plot(graphNum, i, x, v_y[i]);
- }
- else
+ } else
vpTRACE("error in plot vector : not the right dimension");
}
-
/*!
- This function enables you to add a new point in the curve. This point is drawn with the parameters of the curve.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
- \param x : The coordinate of the new point along the x axis and given in the user unit system.
- \param y : The coordinate of the new point along the y axis and given in the user unit system.
- \param z : The coordinate of the new point along the z axis and given in the user unit system.
+ This function enables you to add a new point in the curve. This point is
+ drawn with the parameters of the curve.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNum : The index of the curve in the list of the curves
+ belonging to the graphic. \param x : The coordinate of the new point along
+ the x axis and given in the user unit system. \param y : The coordinate of
+ the new point along the y axis and given in the user unit system. \param z :
+ The coordinate of the new point along the z axis and given in the user unit
+ system.
*/
-vpMouseButton::vpMouseButtonType
-vpPlot::plot (const unsigned int graphNum, const unsigned int curveNum, const double x, const double y, const double z)
+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);
+ return (graphList + graphNum)->plot(I, curveNum, x, y, z);
}
/*!
- This function enables you to add new points in all curves of a plot. These points are drawn with the parameters of the curves.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param x : The coordinate of the new points along the x axis and given in the user unit system.
- \param v_y : y coordinates vector. The coordinates of the new points along the y axis and given in the user unit system.
- \param v_z : z coordinates vector. The coordinates of the new points along the z axis and given in the user unit system.
+ This function enables you to add new points in all curves of a plot. These
+ points are drawn with the parameters of the curves.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param x : The coordinate of the new points along the x axis and
+ given in the user unit system. \param v_y : y coordinates vector. The
+ coordinates of the new points along the y axis and given in the user unit
+ system. \param v_z : z coordinates vector. The coordinates of the new points
+ along the z axis and given in the user unit system.
*/
-vpMouseButton::vpMouseButtonType
-vpPlot::plot(const unsigned int graphNum, const double x, const vpColVector &v_y, const vpColVector &v_z)
+vpMouseButton::vpMouseButtonType vpPlot::plot(const unsigned int graphNum, const double x, const vpColVector &v_y,
+ const vpColVector &v_z)
{
vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
- if((graphList+graphNum)->curveNbr == v_y.getRows() && (graphList+graphNum)->curveNbr == v_z.getRows())
- {
- for(unsigned int i = 0;i < v_y.getRows();++i)
+ if ((graphList + graphNum)->curveNbr == v_y.getRows() && (graphList + graphNum)->curveNbr == v_z.getRows()) {
+ for (unsigned int i = 0; i < v_y.getRows(); ++i)
button = this->plot(graphNum, i, x, v_y[i], v_z[i]);
- }
- else
- vpTRACE("error in plot vector : not the right dimension");
+ } else
+ vpTRACE("error in plot vector : not the right dimension");
return button;
}
/*!
- This method allows to change the point of view with the mouse if you have a 3D graphic.
- The navigation is performed using the mouse.
+ This method allows to change the point of view with the mouse if you have a
+ 3D graphic. The navigation is performed using the mouse.
- A click on left mouse button allows rotations
- A click on middle mouse button allows zoom
- A click on rigt mouse button quit the infinite navigation loop.
*/
-void
-vpPlot::navigate()
+void vpPlot::navigate()
{
vpMouseButton::vpMouseButtonType b = vpMouseButton::none;
-
+
bool blocked = false;
unsigned int iblocked = 0;
vpImagePoint iP;
-
- while (b != vpMouseButton::button3)
- {
- if (!blocked)
- {
- vpDisplay::getPointerPosition(I,iP);
- for (unsigned int i = 0; i < graphNbr ; i++)
- {
- if (iP.inRectangle((graphList+i)->graphZone))
- {
+
+ while (b != vpMouseButton::button3) {
+ if (!blocked) {
+ vpDisplay::getPointerPosition(I, iP);
+ for (unsigned int i = 0; i < graphNbr; i++) {
+ if (iP.inRectangle((graphList + i)->graphZone)) {
iblocked = i;
break;
}
}
- if ((graphList+iblocked)->move(I, b))
- {
- (graphList+iblocked)->replot3D(I);
+ if ((graphList + iblocked)->move(I, b)) {
+ (graphList + iblocked)->replot3D(I);
}
- blocked = (graphList+iblocked)->blocked;
- }
- else
- {
- if ((graphList+iblocked)->move(I, b))
- {
- (graphList+iblocked)->replot3D(I);
+ blocked = (graphList + iblocked)->blocked;
+ } else {
+ if ((graphList + iblocked)->move(I, b)) {
+ (graphList + iblocked)->replot3D(I);
}
- blocked = (graphList+iblocked)->blocked;
+ blocked = (graphList + iblocked)->blocked;
}
vpTime::sleepMs(20);
}
}
/*!
- This method displays the coordinates of the point pointed by the mouse pointer.
-
+ This method displays the coordinates of the point pointed by the mouse
+ pointer.
+
\param block : If true, this method waits a click.
*/
-void
-vpPlot::getPixelValue(const bool block)
+void vpPlot::getPixelValue(const bool block)
{
vpImagePoint iP;
-
+
if (block)
- vpDisplay::getClick(I,iP);
+ vpDisplay::getClick(I, iP);
else
- vpDisplay::getPointerPosition(I,iP);
-
- for (unsigned int i = 0; i < graphNbr; i++)
- {
- if ((graphList+i)->getPixelValue(I,iP)) break;
+ vpDisplay::getPointerPosition(I, iP);
+
+ for (unsigned int i = 0; i < graphNbr; i++) {
+ if ((graphList + i)->getPixelValue(I, iP))
+ break;
}
}
/*!
Sets the title of a graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param title : The graphic title.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param title : The graphic title.
*/
-void
-vpPlot::setTitle (const unsigned int graphNum, const std::string &title)
+void vpPlot::setTitle(const unsigned int graphNum, const std::string &title)
{
- (graphList+graphNum)->setTitle(title);
+ (graphList + graphNum)->setTitle(title);
}
/*!
Sets the unit system of the x axis.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param unitx : The name of the unit of the x axis.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param unitx : The name of the unit of the x axis.
*/
-void
-vpPlot::setUnitX (const unsigned int graphNum, const std::string &unitx)
+void vpPlot::setUnitX(const unsigned int graphNum, const std::string &unitx)
{
- (graphList+graphNum)->setUnitX(unitx);
+ (graphList + graphNum)->setUnitX(unitx);
}
/*!
Sets the unit system of the y axis.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param unity : The name of the unit of the y axis.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param unity : The name of the unit of the y axis.
*/
-void
-vpPlot::setUnitY (const unsigned int graphNum, const std::string &unity)
+void vpPlot::setUnitY(const unsigned int graphNum, const std::string &unity)
{
- (graphList+graphNum)->setUnitY(unity);
+ (graphList + graphNum)->setUnitY(unity);
}
/*!
Sets the unit system of the z axis.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param unitz : The name of the unit of the z axis.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param unitz : The name of the unit of the z axis.
*/
-void
-vpPlot::setUnitZ (const unsigned int graphNum, const std::string &unitz)
+void vpPlot::setUnitZ(const unsigned int graphNum, const std::string &unitz)
{
- (graphList+graphNum)->setUnitZ(unitz);
+ (graphList + graphNum)->setUnitZ(unitz);
}
/*!
Sets the legend of a curve.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
- \param legend : The legend of the curve.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNum : The index of the curve in the list of the curves
+ belonging to the graphic. \param legend : The legend of the curve.
*/
-void
-vpPlot::setLegend (const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
+void vpPlot::setLegend(const unsigned int graphNum, const unsigned int curveNum, const std::string &legend)
{
- (graphList+graphNum)->setLegend(curveNum, legend);
+ (graphList + graphNum)->setLegend(curveNum, legend);
}
/*!
- This method enables to erase the list of points stored for the graphic number \f$ graphNum \f$.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
+ This method enables to erase the list of points stored for the graphic
+ number \f$ graphNum \f$.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3.
*/
-void
-vpPlot::resetPointList (const unsigned int graphNum)
+void vpPlot::resetPointList(const unsigned int graphNum)
{
- for (unsigned int i = 0; i < (graphList+graphNum)->curveNbr; i++)
- (graphList+graphNum)->resetPointList(i);
+ for (unsigned int i = 0; i < (graphList + graphNum)->curveNbr; i++)
+ (graphList + graphNum)->resetPointList(i);
}
/*!
This function enables you to choose the thickness used to draw a given curve.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
- \param thickness : The thickness you want to use
+
+ \param graphNum : The index of the graph in the window. As the number of
+graphic in a window is less or equal to 4, this parameter is between 0 and 3.
+ \param curveNum : The index of the curve in the list of the curves belonging
+to the graphic. \param thickness : The thickness you want to use
*/
-void
-vpPlot::setThickness (const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness)
+void vpPlot::setThickness(const unsigned int graphNum, const unsigned int curveNum, const unsigned int thickness)
{
- (graphList+graphNum)->setCurveThickness(curveNum, thickness);
+ (graphList + graphNum)->setCurveThickness(curveNum, thickness);
}
/*!
-This function enables you to choose the thickness used to draw all the curves belonging to a given graphic.
+This function enables you to choose the thickness used to draw all the curves
+belonging to a given graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
+ \param graphNum : The index of the graph in the window. As the number of
+graphic in a window is less or equal to 4, this parameter is between 0 and 3.
\param thickness : The thickness you want to use
*/
-void
-vpPlot::setGraphThickness (const unsigned int graphNum, const unsigned int thickness)
+void vpPlot::setGraphThickness(const unsigned int graphNum, const unsigned int thickness)
{
- for (unsigned int curveNum=0; curveNum < (graphList+graphNum)->curveNbr; curveNum++)
- (graphList+graphNum)->setCurveThickness(curveNum, thickness);
+ for (unsigned int curveNum = 0; curveNum < (graphList + graphNum)->curveNbr; curveNum++)
+ (graphList + graphNum)->setCurveThickness(curveNum, thickness);
}
/*!
- This function enables you to choose the thickness used to draw the grid and the axis of a given graphic.
+ This function enables you to choose the thickness used to draw the grid and
+ the axis of a given graphic.
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param thickness : The thickness you want to use
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param thickness : The thickness you want to use
*/
-void
-vpPlot::setGridThickness (const unsigned int graphNum, const unsigned int thickness)
+void vpPlot::setGridThickness(const unsigned int graphNum, const unsigned int thickness)
{
- (graphList+graphNum)->setGridThickness(thickness);
+ (graphList + graphNum)->setGridThickness(thickness);
}
/*!
- This method enables to erase the list of points stored for the curve number \f$ curveNum \f$ contained in the graphic number \f$ graphNum \f$.
-
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param curveNum : The index of the curve in the list of the curves belonging to the graphic.
+ This method enables to erase the list of points stored for the curve number
+ \f$ curveNum \f$ contained in the graphic number \f$ graphNum \f$.
+
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param curveNum : The index of the curve in the list of the curves
+ belonging to the graphic.
*/
-void
-vpPlot::resetPointList (const unsigned int graphNum, const unsigned int curveNum)
+void vpPlot::resetPointList(const unsigned int graphNum, const unsigned int curveNum)
{
- (graphList+graphNum)->resetPointList(curveNum);
+ (graphList + graphNum)->resetPointList(curveNum);
}
/*!
- This function enables to save in a text file all the plotted points of a graphic.
+ This function enables to save in a text file all the plotted points of a
+ graphic.
The content of the file is the following:
- - The first line of the text file is the graphic title prefixed by \e title_prefix.
- - Then the successive points coordinates (x, y, z) are given for each curve. If the graphic has two curves:
+ - The first line of the text file is the graphic title prefixed by \e
+ title_prefix.
+ - Then the successive points coordinates (x, y, z) are given for each curve.
+ If the graphic has two curves:
- the first column corresponds to the x axis of the first curve
- the second column corresponds to the y axis of the first curve
- the third column corresponds to the z axis of the first curve
@@ -625,11 +635,13 @@ vpPlot::resetPointList (const unsigned int graphNum, const unsigned int curveNum
The columns are delimited thanks to tabulations.
- \param title_prefix : Prefix introducted in the first line of the saved file. To exploit a posteriori the resulting curves:
+ \param title_prefix : Prefix introducted in the first line of the saved
+ file. To exploit a posteriori the resulting curves:
- with gnuplot, set title_prefix to "# ".
- with Matlab, set title_prefix to "% ".
- \param graphNum : The index of the graph in the window. As the number of graphic in a window is less or equal to 4, this parameter is between 0 and 3.
- \param dataFile : Name of the text file.
+ \param graphNum : The index of the graph in the window. As the number of
+ graphic in a window is less or equal to 4, this parameter is between 0
+ and 3. \param dataFile : Name of the text file.
*/
void vpPlot::saveData(const unsigned int graphNum, const std::string &dataFile, const std::string &title_prefix)
{
@@ -638,66 +650,70 @@ void vpPlot::saveData(const unsigned int graphNum, const std::string &dataFile,
unsigned int ind;
double *p = new double[3];
- bool end=false;
-
- std::vector< std::list<double>::const_iterator > vec_iter_pointListx((graphList+graphNum)->curveNbr);
- std::vector< std::list<double>::const_iterator > vec_iter_pointListy((graphList+graphNum)->curveNbr);
- std::vector< std::list<double>::const_iterator > vec_iter_pointListz((graphList+graphNum)->curveNbr);
-
- fichier << 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();
-// (graphList+graphNum)->curveList[ind].pointListx.front();
-// (graphList+graphNum)->curveList[ind].pointListy.front();
-// (graphList+graphNum)->curveList[ind].pointListz.front();
+ bool end = false;
+
+ std::vector<std::list<double>::const_iterator> vec_iter_pointListx((graphList + graphNum)->curveNbr);
+ std::vector<std::list<double>::const_iterator> vec_iter_pointListy((graphList + graphNum)->curveNbr);
+ std::vector<std::list<double>::const_iterator> vec_iter_pointListz((graphList + graphNum)->curveNbr);
+
+ fichier << 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();
+ // (graphList+graphNum)->curveList[ind].pointListx.front();
+ // (graphList+graphNum)->curveList[ind].pointListy.front();
+ // (graphList+graphNum)->curveList[ind].pointListz.front();
}
- while (end == false)
- {
+ while (end == false) {
end = true;
- for(ind=0;ind<(graphList+graphNum)->curveNbr;ind++)
- {
-// if (!(graphList+graphNum)->curveList[ind].pointListx.outside()
-// && !(graphList+graphNum)->curveList[ind].pointListy.outside()
-// && !(graphList+graphNum)->curveList[ind].pointListz.outside())
- if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
- && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
- && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
- {
+ for (ind = 0; ind < (graphList + graphNum)->curveNbr; ind++) {
+ // if (!(graphList+graphNum)->curveList[ind].pointListx.outside()
+ // &&
+ // !(graphList+graphNum)->curveList[ind].pointListy.outside()
+ // &&
+ // !(graphList+graphNum)->curveList[ind].pointListz.outside())
+ if ((vec_iter_pointListx[ind] != (graphList + graphNum)->curveList[ind].pointListx.end()) &&
+ (vec_iter_pointListy[ind] != (graphList + graphNum)->curveList[ind].pointListy.end()) &&
+ (vec_iter_pointListz[ind] != (graphList + graphNum)->curveList[ind].pointListz.end())) {
p[0] = *vec_iter_pointListx[ind];
p[1] = *vec_iter_pointListy[ind];
p[2] = *vec_iter_pointListz[ind];
-// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
-// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
-// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
+ // p[0] =
+ // (graphList+graphNum)->curveList[ind].pointListx.value();
+ // p[1] =
+ // (graphList+graphNum)->curveList[ind].pointListy.value();
+ // p[2] =
+ // (graphList+graphNum)->curveList[ind].pointListz.value();
fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
++vec_iter_pointListx[ind];
++vec_iter_pointListy[ind];
++vec_iter_pointListz[ind];
-// (graphList+graphNum)->curveList[ind].pointListx.next();
-// (graphList+graphNum)->curveList[ind].pointListy.next();
-// (graphList+graphNum)->curveList[ind].pointListz.next();
-// if(!(graphList+graphNum)->curveList[ind].pointListx.nextOutside()
-// && !(graphList+graphNum)->curveList[ind].pointListy.nextOutside()
-// && !(graphList+graphNum)->curveList[ind].pointListz.nextOutside())
- if((vec_iter_pointListx[ind] != (graphList+graphNum)->curveList[ind].pointListx.end())
- && (vec_iter_pointListy[ind] != (graphList+graphNum)->curveList[ind].pointListy.end())
- && (vec_iter_pointListz[ind] != (graphList+graphNum)->curveList[ind].pointListz.end()))
+ // (graphList+graphNum)->curveList[ind].pointListx.next();
+ // (graphList+graphNum)->curveList[ind].pointListy.next();
+ // (graphList+graphNum)->curveList[ind].pointListz.next();
+ // if(!(graphList+graphNum)->curveList[ind].pointListx.nextOutside()
+ // &&
+ // !(graphList+graphNum)->curveList[ind].pointListy.nextOutside()
+ // &&
+ // !(graphList+graphNum)->curveList[ind].pointListz.nextOutside())
+ if ((vec_iter_pointListx[ind] != (graphList + graphNum)->curveList[ind].pointListx.end()) &&
+ (vec_iter_pointListy[ind] != (graphList + graphNum)->curveList[ind].pointListy.end()) &&
+ (vec_iter_pointListz[ind] != (graphList + graphNum)->curveList[ind].pointListz.end()))
end = false;
- }
- else
- {
-// p[0] = (graphList+graphNum)->curveList[ind].pointListx.value();
-// p[1] = (graphList+graphNum)->curveList[ind].pointListy.value();
-// p[2] = (graphList+graphNum)->curveList[ind].pointListz.value();
- p[0] = (graphList+graphNum)->curveList[ind].pointListx.back();
- p[1] = (graphList+graphNum)->curveList[ind].pointListy.back();
- p[2] = (graphList+graphNum)->curveList[ind].pointListz.back();
+ } else {
+ // p[0] =
+ // (graphList+graphNum)->curveList[ind].pointListx.value();
+ // p[1] =
+ // (graphList+graphNum)->curveList[ind].pointListy.value();
+ // p[2] =
+ // (graphList+graphNum)->curveList[ind].pointListz.value();
+ p[0] = (graphList + graphNum)->curveList[ind].pointListx.back();
+ p[1] = (graphList + graphNum)->curveList[ind].pointListy.back();
+ p[2] = (graphList + graphNum)->curveList[ind].pointListz.back();
fichier << p[0] << "\t" << p[1] << "\t" << p[2] << "\t";
}
}
@@ -710,5 +726,5 @@ void vpPlot::saveData(const unsigned int graphNum, const std::string &dataFile,
#elif !defined(VISP_BUILD_SHARED_LIBS)
// Work arround to avoid warning: libvisp_core.a(vpPlot.cpp.o) has no symbols
-void dummy_vpPlot() {};
+void dummy_vpPlot(){};
#endif
diff --git a/modules/gui/src/plot/vpPlotCurve.cpp b/modules/gui/src/plot/vpPlotCurve.cpp
index 437d854..28c84a7 100644
--- a/modules/gui/src/plot/vpPlotCurve.cpp
+++ b/modules/gui/src/plot/vpPlotCurve.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,17 +39,17 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp3/gui/vpPlotCurve.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlotCurve.h>
#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)
+vpPlotCurve::vpPlotCurve()
+ : color(vpColor::red), curveStyle(point), thickness(1), nbPoint(0), lastPoint(), pointListx(), pointListy(),
+ pointListz(), legend(), xmin(0), xmax(0), ymin(0), ymax(0)
{
}
@@ -59,26 +60,34 @@ vpPlotCurve::~vpPlotCurve()
pointListz.clear();
}
-void
-vpPlotCurve::plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y)
-{
+void vpPlotCurve::plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const double x, const double y)
+{
nbPoint++;
-
- if (nbPoint > 1)
- {
- vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
+
+ if (nbPoint > 1) {
+ vpDisplay::displayLine(I, lastPoint, iP, color, thickness);
}
-#if defined (VISP_HAVE_DISPLAY)
+#if defined(VISP_HAVE_DISPLAY)
double top;
double left;
double width;
double height;
-
- if (iP.get_i() <= lastPoint.get_i()) {top = iP.get_i()-5; height = lastPoint.get_i() - top+10;}
- else {top = lastPoint.get_i()-5; height = iP.get_i() - top+10;}
- if (iP.get_j() <= lastPoint.get_j()) {left = iP.get_j()-5; width = lastPoint.get_j() - left+10;}
- else {left = lastPoint.get_j()-5; width = iP.get_j() - left+10;}
- vpDisplay::flushROI(I,vpRect(left,top,width,height));
+
+ if (iP.get_i() <= lastPoint.get_i()) {
+ top = iP.get_i() - 5;
+ height = lastPoint.get_i() - top + 10;
+ } else {
+ top = lastPoint.get_i() - 5;
+ height = iP.get_i() - top + 10;
+ }
+ if (iP.get_j() <= lastPoint.get_j()) {
+ left = iP.get_j() - 5;
+ width = lastPoint.get_j() - left + 10;
+ } else {
+ left = lastPoint.get_j() - 5;
+ width = iP.get_j() - left + 10;
+ }
+ vpDisplay::flushROI(I, vpRect(left, top, width, height));
#endif
lastPoint = iP;
pointListx.push_back(x);
@@ -86,23 +95,22 @@ vpPlotCurve::plotPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
pointListz.push_back(0.0);
}
-void
-vpPlotCurve::plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx, const double zoomy)
+void vpPlotCurve::plotList(const vpImage<unsigned char> &I, const double xorg, const double yorg, const double zoomx,
+ const double zoomy)
{
std::list<double>::const_iterator it_ptListx = pointListx.begin();
std::list<double>::const_iterator it_ptListy = pointListy.begin();
-
+
unsigned int k = 0;
vpImagePoint iP;
- while (k < nbPoint)
- {
- iP.set_ij(yorg-(zoomy*(*it_ptListy)),xorg+(zoomx*(*it_ptListx)));
-
+ while (k < nbPoint) {
+ iP.set_ij(yorg - (zoomy * (*it_ptListy)), xorg + (zoomx * (*it_ptListx)));
+
if (k > 0)
- vpDisplay::displayLine(I,lastPoint, iP, color, thickness);
-
+ vpDisplay::displayLine(I, lastPoint, iP, color, thickness);
+
lastPoint = iP;
-
+
++it_ptListx;
++it_ptListy;
k++;
@@ -110,7 +118,8 @@ vpPlotCurve::plotList(const vpImage<unsigned char> &I, const double xorg, const
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpPlotCurve.cpp.o) has no symbols
-void dummy_vpPlotCurve() {};
+// Work arround to avoid warning: libvisp_core.a(vpPlotCurve.cpp.o) has no
+// symbols
+void dummy_vpPlotCurve(){};
#endif
#endif
diff --git a/modules/gui/src/plot/vpPlotGraph.cpp b/modules/gui/src/plot/vpPlotGraph.cpp
index 7620a19..7d15936 100644
--- a/modules/gui/src/plot/vpPlotGraph.cpp
+++ b/modules/gui/src/plot/vpPlotGraph.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,76 +40,70 @@
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp3/gui/vpPlotGraph.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/gui/vpPlotGraph.h>
//#include <visp3/vision/vpPose.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
-#include <cmath> // std::fabs
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <visp3/core/vpMath.h>
-#include <limits> // numeric_limits
#if defined(VISP_HAVE_DISPLAY)
-int laFonctionSansNom (const double delta);
+int laFonctionSansNom(const double delta);
void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3);
-
vpPlotGraph::vpPlotGraph()
- : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10),
- xdelt(1), ydelt(1), gridx(true), gridy(true), gridColor(), curveNbr(1), curveList(NULL),
- scaleInitialized(false), firstPoint(true), nbDivisionx(10), nbDivisiony(10), topLeft(),
- width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
- dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0),
- w_zval(0), w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.),
- nbDivisionz(10), zorg(1.), zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(),
- blockedr(false), blockedz(false), blocked(false), epsi(5), epsj(6),
- dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
+ : xorg(0.), yorg(0.), zoomx(1.), zoomy(1.), xmax(10), ymax(10), xmin(0), ymin(-10), xdelt(1), ydelt(1), gridx(true),
+ gridy(true), gridColor(), curveNbr(1), curveList(NULL), scaleInitialized(false), firstPoint(true), nbDivisionx(10),
+ nbDivisiony(10), topLeft(), width(0), height(0), graphZone(), dTopLeft(), dWidth(0), dHeight(0), dGraphZone(),
+ dTopLeft3D(), dGraphZone3D(), cam(), cMo(), cMf(), w_xval(0), w_xsize(0), w_yval(0), w_ysize(0), w_zval(0),
+ w_zsize(0), ptXorg(0), ptYorg(0), ptZorg(0), zoomx_3D(1.), zoomy_3D(1.), zoomz_3D(1.), nbDivisionz(10), zorg(1.),
+ zoomz(1.), zmax(10), zmin(-10), zdelt(1), old_iPr(), old_iPz(), blockedr(false), blockedz(false), blocked(false),
+ epsi(5), epsj(6), dispUnit(false), dispTitle(false), dispLegend(false), gridThickness(1)
{
- gridColor.setColor(200,200,200);
-
- old_iPr = vpImagePoint(-1,-1);
- old_iPz = vpImagePoint(-1,-1);
+ gridColor.setColor(200, 200, 200);
+
+ old_iPr = vpImagePoint(-1, -1);
+ old_iPz = vpImagePoint(-1, -1);
gridThickness = 1;
}
vpPlotGraph::~vpPlotGraph()
{
- if (curveList != NULL)
- {
+ if (curveList != NULL) {
delete[] curveList;
curveList = NULL;
}
}
-void
-vpPlotGraph::initGraph (unsigned int nbCurve)
+void vpPlotGraph::initGraph(unsigned int nbCurve)
{
curveList = new vpPlotCurve[nbCurve];
curveNbr = nbCurve;
-
- vpColor colors[6] = {vpColor::blue,vpColor::green,vpColor::red,vpColor::cyan,vpColor::orange,vpColor::purple};
-
- for (unsigned int i = 0; i < curveNbr; i++)
- {
- (curveList+i)->color = colors[i%6];
- (curveList+i)->curveStyle = line;
- (curveList+i)->pointListx.clear();
- (curveList+i)->pointListy.clear();
- (curveList+i)->legend.clear();
+
+ vpColor colors[6] = {vpColor::blue, vpColor::green, vpColor::red, vpColor::cyan, vpColor::orange, vpColor::purple};
+
+ for (unsigned int i = 0; i < curveNbr; i++) {
+ (curveList + i)->color = colors[i % 6];
+ (curveList + i)->curveStyle = vpPlotCurve::line;
+ (curveList + i)->pointListx.clear();
+ (curveList + i)->pointListy.clear();
+ (curveList + i)->legend.clear();
}
}
-void
-vpPlotGraph::initSize (vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei, unsigned int margej)
+void vpPlotGraph::initSize(vpImagePoint top_left, unsigned int w, unsigned int h, unsigned int margei,
+ unsigned int margej)
{
this->topLeft = top_left;
this->width = w;
@@ -116,74 +111,67 @@ vpPlotGraph::initSize (vpImagePoint top_left, unsigned int w, unsigned int h, un
graphZone.setTopLeft(topLeft);
graphZone.setWidth(width);
graphZone.setHeight(height);
-
- this->dTopLeft = vpImagePoint(topLeft.get_i()+margei,topLeft.get_j()+margej);
- this->dWidth = width-margej-10;
- this->dHeight = height-2*margei;
+
+ 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);
+ dGraphZone.setWidth(dWidth + 1);
+ dGraphZone.setHeight(dHeight + 1);
+
+ this->dTopLeft3D = vpImagePoint(topLeft.get_i() + margei, topLeft.get_j() + 10);
dGraphZone3D.setTopLeft(dTopLeft3D);
- dGraphZone3D.setWidth(dWidth+1);
- dGraphZone3D.setHeight(dHeight+1);
-
- if (this->dWidth > this->dHeight)
- {
+ dGraphZone3D.setWidth(dWidth + 1);
+ dGraphZone3D.setHeight(dHeight + 1);
+
+ if (this->dWidth > this->dHeight) {
w_ysize = 1.0;
- w_xsize = this->dWidth/this->dHeight;
+ w_xsize = this->dWidth / this->dHeight;
w_zsize = w_xsize;
-
- w_yval = w_ysize/2.0;
- w_xval = w_xsize/2.0;
- w_zval = w_zsize/2.0;
- }
- else if (this->dWidth == this->dHeight)
- {
+
+ w_yval = w_ysize / 2.0;
+ w_xval = w_xsize / 2.0;
+ w_zval = w_zsize / 2.0;
+ } else if (this->dWidth == this->dHeight) {
w_ysize = 1.0;
w_xsize = 1.0;
w_zsize = 1.0;
-
+
w_yval = 0.5;
w_xval = 0.5;
w_zval = 0.5;
- }
- else if (this->dWidth < this->dHeight)
- {
+ } else if (this->dWidth < this->dHeight) {
w_xsize = 1.0;
- w_ysize = this->dHeight/this->dWidth;
+ w_ysize = this->dHeight / this->dWidth;
w_zsize = w_ysize;
-
- w_yval = w_ysize/2.0;
- w_xval = w_xsize/2.0;
- w_zval = w_zsize/2.0;
+
+ w_yval = w_ysize / 2.0;
+ w_xval = w_xsize / 2.0;
+ w_zval = w_zsize / 2.0;
}
-
- cam.initPersProjWithoutDistortion(1000,1000,this->dWidth/2.0,this->dHeight/2.0);
-
+
+ cam.initPersProjWithoutDistortion(1000, 1000, this->dWidth / 2.0, this->dHeight / 2.0);
+
findPose();
- cMf.buildFrom(0,0,cMo[2][3],0,0,0);
+ cMf.buildFrom(0, 0, cMo[2][3], 0, 0, 0);
}
-
-void
-vpPlotGraph::findPose()
+void vpPlotGraph::findPose()
{
- vpPoint point[4];
- point[0].setWorldCoordinates(-w_xval,-w_yval,-w_zval);
- point[1].setWorldCoordinates(w_xval,-w_yval,-w_zval);
- point[2].setWorldCoordinates(w_xval,w_yval,-w_zval);
- point[3].setWorldCoordinates(-w_xval,w_yval,-w_zval);
-
+ vpPoint point_[4];
+ point_[0].setWorldCoordinates(-w_xval, -w_yval, -w_zval);
+ point_[1].setWorldCoordinates(w_xval, -w_yval, -w_zval);
+ point_[2].setWorldCoordinates(w_xval, w_yval, -w_zval);
+ point_[3].setWorldCoordinates(-w_xval, w_yval, -w_zval);
+
vpImagePoint iP[4];
- iP[0].set_ij(0,0);
- iP[1].set_ij(0,dWidth-1);
- iP[2].set_ij(dHeight-1,dWidth-1);
- iP[3].set_ij(dHeight-1,0);
+ iP[0].set_ij(0, 0);
+ iP[1].set_ij(0, dWidth - 1);
+ iP[2].set_ij(dHeight - 1, dWidth - 1);
+ iP[3].set_ij(dHeight - 1, 0);
- double x=0, y=0;
+ double x = 0, y = 0;
#if 0
// Modified by FS to remove dependency with visp_vision (pose) module
vpPose pose;
@@ -192,9 +180,9 @@ vpPlotGraph::findPose()
for (unsigned int i=0 ; i < 4 ; i++)
{
vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
- point[i].set_x(x);
- point[i].set_y(y);
- pose.addPoint(point[i]);
+ point_[i].set_x(x);
+ point_[i].set_y(y);
+ pose.addPoint(point_[i]);
}
pose.computePose(vpPose::LAGRANGE, cMo) ;
@@ -203,276 +191,257 @@ vpPlotGraph::findPose()
#else
// Instead of pose computation we use an approximation
double Z = 0;
- for(unsigned int i=0; i< 4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
vpPixelMeterConversion::convertPoint(cam, iP[i], x, y);
- Z = vpMath::maximum(Z, point[i].get_oX() / x);
- Z = vpMath::maximum(Z, point[i].get_oY() / y);
+ Z = vpMath::maximum(Z, point_[i].get_oX() / x);
+ Z = vpMath::maximum(Z, point_[i].get_oY() / y);
}
cMo[2][3] = Z;
#endif
}
-void
-vpPlotGraph::computeGraphParameters()
+void vpPlotGraph::computeGraphParameters()
{
- zoomx = dWidth/(xmax-xmin);
- zoomy = dHeight/(ymax-ymin);
- xorg = dTopLeft.get_j() - (xmin*zoomx);
- yorg = dTopLeft.get_i() + (ymax*zoomy);
+ zoomx = dWidth / (xmax - xmin);
+ zoomy = dHeight / (ymax - ymin);
+ xorg = dTopLeft.get_j() - (xmin * zoomx);
+ yorg = dTopLeft.get_i() + (ymax * zoomy);
}
-void
-vpPlotGraph::setCurveColor(const unsigned int curveNum, const vpColor color)
+void vpPlotGraph::setCurveColor(const unsigned int curveNum, const vpColor &color)
{
- (curveList+curveNum)->color = color;
+ (curveList + curveNum)->color = color;
}
-void
-vpPlotGraph::setTitle (const std::string &title_)
+void vpPlotGraph::setTitle(const std::string &title_)
{
title = title_;
dispTitle = true;
}
-void
-vpPlotGraph::setUnitX (const std::string &unit_x)
+void vpPlotGraph::setUnitX(const std::string &unit_x)
{
unitx = unit_x;
dispUnit = true;
}
-void
-vpPlotGraph::setUnitY (const std::string &unit_y)
+void vpPlotGraph::setUnitY(const std::string &unit_y)
{
unity = unit_y;
dispUnit = true;
}
-void
-vpPlotGraph::setUnitZ (const std::string &unit_z)
+void vpPlotGraph::setUnitZ(const std::string &unit_z)
{
unitz = 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;
+ (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;
+ (curveList + curveNum)->thickness = thickness;
}
-int
-laFonctionSansNom (const double delta)
+int laFonctionSansNom(const double delta)
{
double d = delta;
int power = 0;
- if (d < 1)
- {
- while (d < 1)
- {
+ if (d < 1) {
+ while (d < 1) {
d = d * 10;
power++;
}
power--;
return power;
}
-
- if (d >= 10)
- {
- while (d > 10)
- {
+
+ if (d >= 10) {
+ while (d > 10) {
d = d / 10;
power--;
}
power--;
return power;
}
-
+
return 0;
}
-void
-vpPlotGraph::displayGrid (vpImage<unsigned char> &I)
+void vpPlotGraph::displayGrid(vpImage<unsigned char> &I)
{
computeGraphParameters();
-
- xdelt = (xmax-xmin)/nbDivisionx;
- ydelt = (ymax-ymin)/nbDivisiony;
-
+
+ xdelt = (xmax - xmin) / nbDivisionx;
+ ydelt = (ymax - ymin) / nbDivisiony;
+
double t;
char valeur[20];
int power;
-
+
power = laFonctionSansNom(xdelt);
- for(t=xmin;t<=xmax;t=t+xdelt)
- {
- double x = xorg+(zoomx*t);
- if(gridy)
- vpDisplay::displayDotLine(I,vpImagePoint(dTopLeft.get_i(),x), vpImagePoint(dTopLeft.get_i()+dHeight,x), gridColor, gridThickness);
+ for (t = xmin; t <= xmax; t = t + xdelt) {
+ double x = xorg + (zoomx * t);
+ if (gridy)
+ vpDisplay::displayDotLine(I, vpImagePoint(dTopLeft.get_i(), x), vpImagePoint(dTopLeft.get_i() + dHeight, x),
+ gridColor, gridThickness);
else
- vpDisplay::displayDotLine(I,vpImagePoint(yorg,x), vpImagePoint(yorg-3,x), vpColor::black, gridThickness);
-
- if (t+xdelt <= xmax+1e-10)
- {
+ vpDisplay::displayDotLine(I, vpImagePoint(yorg, x), vpImagePoint(yorg - 3, x), vpColor::black, gridThickness);
+
+ if (t + xdelt <= xmax + 1e-10) {
double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
+ if (power != 0)
+ ttemp = t * pow(10.0, power);
+ else
+ 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)
- {
+ 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
}
-
+
power = laFonctionSansNom(ydelt);
- for(t=ymin;t<=ymax;t=t+ydelt)
- {
- double y = yorg-(zoomy*t);
- if(gridx)
- vpDisplay::displayDotLine(I,vpImagePoint(y,dTopLeft.get_j()), vpImagePoint(y,dTopLeft.get_j()+dWidth), gridColor, gridThickness);
+ for (t = ymin; t <= ymax; t = t + ydelt) {
+ double y = yorg - (zoomy * t);
+ if (gridx)
+ vpDisplay::displayDotLine(I, vpImagePoint(y, dTopLeft.get_j()), vpImagePoint(y, dTopLeft.get_j() + dWidth),
+ gridColor, gridThickness);
else
- vpDisplay::displayDotLine(I,vpImagePoint(y,xorg), vpImagePoint(y,xorg+3), vpColor::black, gridThickness);
-
+ vpDisplay::displayDotLine(I, vpImagePoint(y, xorg), vpImagePoint(y, xorg + 3), vpColor::black, gridThickness);
+
double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
-
+ if (power != 0)
+ ttemp = t * pow(10.0, power);
+ else
+ ttemp = t;
+
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);
+#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);
#endif
}
- if (power != 0)
- {
+ 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);
+#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);
#endif
}
- //Ligne horizontal
- vpDisplay::displayArrow(I,vpImagePoint(yorg,dTopLeft.get_j()), vpImagePoint(yorg,dTopLeft.get_j()+dWidth), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
- //Ligne verticale
- vpDisplay::displayArrow(I, vpImagePoint(dTopLeft.get_i()+dHeight,xorg), vpImagePoint(dTopLeft.get_i(),xorg), vpColor::black, 4*gridThickness, 2*gridThickness, gridThickness);
-
+ // Ligne horizontal
+ vpDisplay::displayArrow(I, vpImagePoint(yorg, dTopLeft.get_j()), vpImagePoint(yorg, dTopLeft.get_j() + dWidth),
+ vpColor::black, 4 * gridThickness, 2 * gridThickness, gridThickness);
+ // Ligne verticale
+ vpDisplay::displayArrow(I, vpImagePoint(dTopLeft.get_i() + dHeight, xorg), vpImagePoint(dTopLeft.get_i(), xorg),
+ vpColor::black, 4 * gridThickness, 2 * gridThickness, gridThickness);
+
if (dispUnit)
displayUnit(I);
if (dispTitle)
- displayTitle(I);
+ displayTitle(I);
if (dispLegend)
displayLegend(I);
-
- //vpDisplay::flushROI(I,graphZone);
+
+ // vpDisplay::flushROI(I,graphZone);
}
-void
-vpPlotGraph::displayUnit (vpImage<unsigned char> &
-#if defined(VISP_HAVE_X11) || defined (VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_GTK)
- I
+void vpPlotGraph::displayUnit(vpImage<unsigned char> &
+#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_D3D9) || \
+ defined(VISP_HAVE_GTK)
+ I
#endif
- )
-{
+)
+{
unsigned int offsetx = vpMath::minimum<unsigned int>((unsigned int)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);
+#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);
#endif
}
-void
-vpPlotGraph::displayTitle (vpImage<unsigned char> &I)
+void vpPlotGraph::displayTitle(vpImage<unsigned char> &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);
+ 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);
}
-void
-vpPlotGraph::displayLegend (vpImage<unsigned char> &I)
+void vpPlotGraph::displayLegend(vpImage<unsigned char> &I)
{
size_t offsetj = 0;
for (unsigned int i = 0; i < curveNbr; i++) {
- size_t offset = epsj * (curveList+i)->legend.size();
+ size_t offset = epsj * (curveList + i)->legend.size();
offsetj = vpMath::maximum(offset, offsetj);
}
- if (offsetj > dWidth) offsetj = dWidth;
+ if (offsetj > dWidth)
+ 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);
}
}
-void
-vpPlotGraph::rescalex(unsigned int side, double extremity)
+void vpPlotGraph::rescalex(unsigned int side, double extremity)
{
- switch (side)
- {
- case 0:
- xmin = (3*extremity-xmax)/2;
- break;
- case 1:
- xmax = (3*extremity-xmin)/2;
- break;
+ switch (side) {
+ case 0:
+ xmin = (3 * extremity - xmax) / 2;
+ break;
+ case 1:
+ xmax = (3 * extremity - xmin) / 2;
+ break;
}
-
- xdelt = (xmax-xmin)/(double)nbDivisionx;
+
+ xdelt = (xmax - xmin) / (double)nbDivisionx;
}
-void
-vpPlotGraph::rescaley(unsigned int side, double extremity)
+void vpPlotGraph::rescaley(unsigned int side, double extremity)
{
- switch (side)
- {
- case 0:
- ymin = (3*extremity-ymax)/2;
- break;
- case 1:
- ymax = (3*extremity-ymin)/2;
- break;
+ switch (side) {
+ case 0:
+ ymin = (3 * extremity - ymax) / 2;
+ break;
+ case 1:
+ ymax = (3 * extremity - ymin) / 2;
+ break;
}
-
- ydelt = (ymax-ymin)/(double)nbDivisiony;
+
+ ydelt = (ymax - ymin) / (double)nbDivisiony;
}
-void
-vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
- const double y_min, const double y_max, const int nbDivy, const bool gx, const bool gy)
+void vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
+ const double y_min, const double y_max, const int nbDivy, const bool gx, const bool gy)
{
this->xmin = x_min;
this->xmax = x_max;
@@ -485,15 +454,13 @@ vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const doub
computeGraphParameters();
clearGraphZone(I);
displayGrid(I);
- vpDisplay::flushROI(I,graphZone);
+ vpDisplay::flushROI(I, graphZone);
scaleInitialized = true;
}
-
-void
-vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
- const double y_min, const double y_max, const int nbDivy,
- const double z_min, const double z_max, const int nbDivz, const bool gx, const bool gy)
+void vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const double x_max, const int nbDivx,
+ const double y_min, const double y_max, const int nbDivy, const double z_min,
+ const double z_max, const int nbDivz, const bool gx, const bool gy)
{
this->xmin = x_min;
this->xmax = x_max;
@@ -509,353 +476,334 @@ vpPlotGraph::initScale(vpImage<unsigned char> &I, const double x_min, const doub
computeGraphParameters();
clearGraphZone(I);
displayGrid(I);
- vpDisplay::flushROI(I,graphZone);
+ vpDisplay::flushROI(I, graphZone);
scaleInitialized = true;
}
-void
-vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y)
+void vpPlotGraph::plot(vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y)
{
- if (!scaleInitialized)
- {
- if (x < 0)
- {
+ if (!scaleInitialized) {
+ if (x < 0) {
xmax = 0;
- rescalex(0,x);
+ rescalex(0, x);
}
- if (x > 0)
- {
+ if (x > 0) {
xmin = 0;
- rescalex(1,x);
+ rescalex(1, x);
}
- if (y < 0)
- {
+ if (y < 0) {
ymax = 0;
- rescaley(0,y);
+ rescaley(0, y);
}
- if (y > 0)
- {
+ if (y > 0) {
ymin = 0;
- rescaley(1,y);
+ rescaley(1, y);
}
scaleInitialized = true;
computeGraphParameters();
clearGraphZone(I);
displayGrid(I);
- //if (y == 0)
+ // if (y == 0)
if (std::fabs(y) <= std::numeric_limits<double>::epsilon())
scaleInitialized = false;
}
-
- if (firstPoint)
- {
-// clearGraphZone(I);
-// displayGrid(I);
-// vpDisplay::flushROI(I,graphZone);
+
+ if (firstPoint) {
+ // clearGraphZone(I);
+ // displayGrid(I);
+ // vpDisplay::flushROI(I,graphZone);
replot(I);
firstPoint = false;
}
-
- double i = yorg-(zoomy*y);
- double j = xorg+(zoomx*x);
-
- vpImagePoint iP(i,j);
-
- if (!iP.inRectangle(dGraphZone))
- {
- if (x > xmax) rescalex(1,x);
- else if(x < xmin) rescalex(0,x);
-
- if (y > ymax) rescaley(1,y);
- else if(y < ymin) rescaley(0,y);
-
+
+ double i = yorg - (zoomy * y);
+ double j = xorg + (zoomx * x);
+
+ vpImagePoint iP(i, j);
+
+ if (!iP.inRectangle(dGraphZone)) {
+ if (x > xmax)
+ rescalex(1, x);
+ else if (x < xmin)
+ rescalex(0, x);
+
+ if (y > ymax)
+ rescaley(1, y);
+ else if (y < ymin)
+ rescaley(0, y);
+
computeGraphParameters();
-
+
replot(I);
- i = yorg-(zoomy*y);
- j = xorg+(zoomx*x);
-
- iP.set_ij(i,j);
+ i = yorg - (zoomy * y);
+ j = xorg + (zoomx * x);
+
+ iP.set_ij(i, j);
}
-
- (curveList+curveNb)->plotPoint(I, iP, x, y);
+
+ (curveList + curveNb)->plotPoint(I, iP, x, y);
#if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
- vpDisplay::flushROI(I,graphZone);
- //vpDisplay::flush(I);
+ vpDisplay::flushROI(I, graphZone);
+// vpDisplay::flush(I);
#endif
}
-void
-vpPlotGraph::replot (vpImage<unsigned char> &I)
+void vpPlotGraph::replot(vpImage<unsigned char> &I)
{
clearGraphZone(I);
displayGrid(I);
for (unsigned int i = 0; i < curveNbr; i++)
- (curveList+i)->plotList(I,xorg,yorg,zoomx,zoomy);
- vpDisplay::flushROI(I,graphZone);
+ (curveList + i)->plotList(I, xorg, yorg, zoomx, zoomy);
+ vpDisplay::flushROI(I, graphZone);
}
-void
-vpPlotGraph::clearGraphZone(vpImage<unsigned char> &I)
-{
- vpDisplay::displayROI(I,graphZone);
-}
+void vpPlotGraph::clearGraphZone(vpImage<unsigned char> &I) { vpDisplay::displayROI(I, graphZone); }
-bool
-vpPlotGraph::getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP)
+bool vpPlotGraph::getPixelValue(vpImage<unsigned char> &I, vpImagePoint &iP)
{
- if (iP.inRectangle(dGraphZone))
- {
- double x = (iP.get_j()-xorg)/zoomx;
- double y = (yorg-iP.get_i())/zoomy;
-
- vpDisplay::displayROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
+ if (iP.inRectangle(dGraphZone)) {
+ double x = (iP.get_j() - xorg) / zoomx;
+ double y = (yorg - iP.get_i()) / zoomy;
+
+ vpDisplay::displayROI(I, vpRect(vpImagePoint(topLeft.get_i() + height - 20, topLeft.get_j()), width - 1, 19));
char valeur[200];
sprintf(valeur, " x: %f", x);
- vpDisplay::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::flush(I);
- vpDisplay::flushROI(I,vpRect(vpImagePoint(topLeft.get_i()+height-20,topLeft.get_j()),width-1,19));
+ 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));
return true;
}
return false;
}
-void
-vpPlotGraph::resetPointList(const unsigned int curveNum)
+void vpPlotGraph::resetPointList(const unsigned int curveNum)
{
- (curveList+curveNum)->pointListx.clear();
- (curveList+curveNum)->pointListy.clear();
- (curveList+curveNum)->pointListz.clear();
- (curveList+curveNum)->nbPoint = 0;
+ (curveList + curveNum)->pointListx.clear();
+ (curveList + curveNum)->pointListy.clear();
+ (curveList + curveNum)->pointListz.clear();
+ (curveList + curveNum)->nbPoint = 0;
firstPoint = true;
}
-
/************************************************************************************************/
-bool
-vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
+bool vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
{
bool iP1In = iP1.inRectangle(dGraphZone3D);
bool iP2In = iP2.inRectangle(dGraphZone3D);
-
- if (!iP1In || !iP2In)
- {
+
+ if (!iP1In || !iP2In) {
double dTopLeft_i = dTopLeft3D.get_i();
double dTopLeft_j = dTopLeft3D.get_j();
- double dBottomRight_i = dTopLeft_i+dHeight;
- double dBottomRight_j = dTopLeft_j+dWidth;
-
- //Cas vertical
- if (vpImagePoint::distance(iP1,iP2) < 9)
+ double dBottomRight_i = dTopLeft_i + dHeight;
+ double dBottomRight_j = dTopLeft_j + dWidth;
+
+ // Cas vertical
+ if (vpImagePoint::distance(iP1, iP2) < 9)
return false;
- if (fabs(iP2.get_j()-iP1.get_j()) <=2)
- {
- if (!iP1In && !iP2In)
- {
- if (iP1.get_i() < dTopLeft_i && iP2.get_i() < dTopLeft_i)
- return false;
- if (iP1.get_i() > dBottomRight_i && iP2.get_i() > dBottomRight_i)
- return false;
- if (iP1.get_j() < dTopLeft_j || iP1.get_j() > dBottomRight_j)
- return false;
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- else iP1.set_i(dBottomRight_i);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- else iP2.set_i(dBottomRight_i);
- }
- else if (!iP1In)
- {
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
- return true;
- }
- else if (!iP2In)
- {
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
- return true;
+ if (fabs(iP2.get_j() - iP1.get_j()) <= 2) {
+ if (!iP1In && !iP2In) {
+ if (iP1.get_i() < dTopLeft_i && iP2.get_i() < dTopLeft_i)
+ return false;
+ if (iP1.get_i() > dBottomRight_i && iP2.get_i() > dBottomRight_i)
+ return false;
+ if (iP1.get_j() < dTopLeft_j || iP1.get_j() > dBottomRight_j)
+ return false;
+ if (iP1.get_i() < dTopLeft_i)
+ iP1.set_i(dTopLeft_i);
+ else
+ iP1.set_i(dBottomRight_i);
+ if (iP2.get_i() < dTopLeft_i)
+ iP2.set_i(dTopLeft_i);
+ else
+ iP2.set_i(dBottomRight_i);
+ } else if (!iP1In) {
+ if (iP1.get_j() < dTopLeft_j)
+ iP1.set_j(dTopLeft_j);
+ if (iP1.get_j() > dBottomRight_j)
+ iP1.set_j(dBottomRight_j);
+ if (iP1.get_i() < dTopLeft_i)
+ iP1.set_i(dTopLeft_i);
+ if (iP1.get_i() > dBottomRight_i)
+ iP1.set_i(dBottomRight_i);
+ return true;
+ } else if (!iP2In) {
+ if (iP2.get_j() < dTopLeft_j)
+ iP2.set_j(dTopLeft_j);
+ if (iP2.get_j() > dBottomRight_j)
+ iP2.set_j(dBottomRight_j);
+ if (iP2.get_i() < dTopLeft_i)
+ iP2.set_i(dTopLeft_i);
+ if (iP2.get_i() > dBottomRight_i)
+ iP2.set_i(dBottomRight_i);
+ return true;
}
}
- //cas horizontal
- else if (fabs(iP2.get_i()-iP1.get_i()) <= 2)
- {
- if (!iP1In && !iP2In)
- {
- if (iP1.get_j() < dTopLeft_j && iP2.get_j() < dTopLeft_j)
- return false;
- if (iP1.get_j() > dBottomRight_j && iP2.get_j() > dBottomRight_j)
- return false;
- if (iP1.get_i() < dTopLeft_i || iP1.get_i() > dBottomRight_i)
- return false;
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- else iP1.set_j(dBottomRight_j);
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- else iP2.set_j(dBottomRight_j);
- }
- else if (!iP1In)
- {
- if (iP1.get_j() < dTopLeft_j) iP1.set_j(dTopLeft_j);
- if (iP1.get_j() > dBottomRight_j) iP1.set_j(dBottomRight_j);
- if (iP1.get_i() < dTopLeft_i) iP1.set_i(dTopLeft_i);
- if (iP1.get_i() > dBottomRight_i) iP1.set_i(dBottomRight_i);
- return true;
- }
- else if (!iP2In)
- {
- if (iP2.get_j() < dTopLeft_j) iP2.set_j(dTopLeft_j);
- if (iP2.get_j() > dBottomRight_j) iP2.set_j(dBottomRight_j);
- if (iP2.get_i() < dTopLeft_i) iP2.set_i(dTopLeft_i);
- if (iP2.get_i() > dBottomRight_i) iP2.set_i(dBottomRight_i);
- return true;
+ // cas horizontal
+ else if (fabs(iP2.get_i() - iP1.get_i()) <= 2) {
+ if (!iP1In && !iP2In) {
+ if (iP1.get_j() < dTopLeft_j && iP2.get_j() < dTopLeft_j)
+ return false;
+ if (iP1.get_j() > dBottomRight_j && iP2.get_j() > dBottomRight_j)
+ return false;
+ if (iP1.get_i() < dTopLeft_i || iP1.get_i() > dBottomRight_i)
+ return false;
+ if (iP1.get_j() < dTopLeft_j)
+ iP1.set_j(dTopLeft_j);
+ else
+ iP1.set_j(dBottomRight_j);
+ if (iP2.get_j() < dTopLeft_j)
+ iP2.set_j(dTopLeft_j);
+ else
+ iP2.set_j(dBottomRight_j);
+ } else if (!iP1In) {
+ if (iP1.get_j() < dTopLeft_j)
+ iP1.set_j(dTopLeft_j);
+ if (iP1.get_j() > dBottomRight_j)
+ iP1.set_j(dBottomRight_j);
+ if (iP1.get_i() < dTopLeft_i)
+ iP1.set_i(dTopLeft_i);
+ if (iP1.get_i() > dBottomRight_i)
+ iP1.set_i(dBottomRight_i);
+ return true;
+ } else if (!iP2In) {
+ if (iP2.get_j() < dTopLeft_j)
+ iP2.set_j(dTopLeft_j);
+ if (iP2.get_j() > dBottomRight_j)
+ iP2.set_j(dBottomRight_j);
+ if (iP2.get_i() < dTopLeft_i)
+ iP2.set_i(dTopLeft_i);
+ if (iP2.get_i() > dBottomRight_i)
+ iP2.set_i(dBottomRight_i);
+ return true;
}
}
-
- double a = (iP2.get_i()-iP1.get_i())/(iP2.get_j()-iP1.get_j());
- double b = iP1.get_i()-a*iP1.get_j();
-
- //test horizontal
- double jtop = (dTopLeft_i-b)/a;
- double jlow = (dBottomRight_i-b)/a;
- //test vertical
- double ileft = dTopLeft_j*a+b;
- double iright = (dBottomRight_j)*a+b;
-
+
+ double a = (iP2.get_i() - iP1.get_i()) / (iP2.get_j() - iP1.get_j());
+ double b = iP1.get_i() - a * iP1.get_j();
+
+ // test horizontal
+ double jtop = (dTopLeft_i - b) / a;
+ double jlow = (dBottomRight_i - b) / a;
+ // test vertical
+ double ileft = dTopLeft_j * a + b;
+ double iright = (dBottomRight_j)*a + b;
+
vpImagePoint iP[2];
int n = 0;
-
- if(jtop >= dTopLeft_j && jtop <= dBottomRight_j)
- {
- iP[n].set_ij(dTopLeft_i,jtop);
+
+ if (jtop >= dTopLeft_j && jtop <= dBottomRight_j) {
+ iP[n].set_ij(dTopLeft_i, jtop);
n++;
}
- if(jlow >= dTopLeft_j && jlow <= dBottomRight_j)
- {
- iP[n].set_ij(dBottomRight_i,jlow);
+ if (jlow >= dTopLeft_j && jlow <= dBottomRight_j) {
+ iP[n].set_ij(dBottomRight_i, jlow);
n++;
}
- if(ileft >= dTopLeft_i && ileft <= dBottomRight_i && n <2)
- {
- iP[n].set_ij(ileft,dTopLeft_j);
+ if (ileft >= dTopLeft_i && ileft <= dBottomRight_i && n < 2) {
+ iP[n].set_ij(ileft, dTopLeft_j);
n++;
}
- if(iright >= dTopLeft_i && iright <= dBottomRight_i && n <2)
- {
- iP[n].set_ij(iright,dBottomRight_j);
+ if (iright >= dTopLeft_i && iright <= dBottomRight_i && n < 2) {
+ iP[n].set_ij(iright, dBottomRight_j);
n++;
}
-
+
if (n < 2)
return false;
-
- if (!iP1In && !iP2In)
- {
- if (fabs(a) < 1)
- {
- if (vpMath::sign(iP1.get_j()-iP[0].get_j()) == vpMath::sign(iP2.get_j()-iP[0].get_j()))
- return false;
+
+ if (!iP1In && !iP2In) {
+ if (fabs(a) < 1) {
+ if (vpMath::sign(iP1.get_j() - iP[0].get_j()) == vpMath::sign(iP2.get_j() - iP[0].get_j()))
+ return false;
int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
- if (sign == vpMath::sign(iP[0].get_j()-iP[1].get_j()))
- {
- iP1 = iP[0]; iP2 = iP[1];
- }
- else
- {
- iP1 = iP[1]; iP2 = iP[0];
- }
- }
- else
- {
- if (vpMath::sign(iP1.get_i()-iP[0].get_i()) == vpMath::sign(iP2.get_i()-iP[0].get_i()))
- return false;
+ if (sign == vpMath::sign(iP[0].get_j() - iP[1].get_j())) {
+ iP1 = iP[0];
+ iP2 = iP[1];
+ } else {
+ iP1 = iP[1];
+ iP2 = iP[0];
+ }
+ } else {
+ if (vpMath::sign(iP1.get_i() - iP[0].get_i()) == vpMath::sign(iP2.get_i() - iP[0].get_i()))
+ return false;
int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
- if (sign == vpMath::sign(iP[0].get_i()-iP[1].get_i()))
- {
- iP1 = iP[0];iP2 = iP[1];
- }
- else
- {
- iP1 = iP[1];iP2 = iP[0];
- }
+ if (sign == vpMath::sign(iP[0].get_i() - iP[1].get_i())) {
+ iP1 = iP[0];
+ iP2 = iP[1];
+ } else {
+ iP1 = iP[1];
+ iP2 = iP[0];
+ }
}
- }
- else if (!iP1In)
- {
+ } else if (!iP1In) {
vpImagePoint iPtemp = iP1;
- if (fabs(a) < 1)
- {
+ if (fabs(a) < 1) {
int sign = vpMath::sign(iP1.get_j() - iP2.get_j());
- if (fabs(iP[0].get_j()-iP2.get_j()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_j()-iP2.get_j())) iP1 = iP[0];
- else iP1 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_j()-iP2.get_j())) iP1 = iP[1];
- else iP1 = iP[0];
- }
- }
- else
- {
+ if (fabs(iP[0].get_j() - iP2.get_j()) > 5) {
+ if (sign == vpMath::sign(iP[0].get_j() - iP2.get_j()))
+ iP1 = iP[0];
+ else
+ iP1 = iP[1];
+ } else {
+ if (sign == vpMath::sign(iP[1].get_j() - iP2.get_j()))
+ iP1 = iP[1];
+ else
+ iP1 = iP[0];
+ }
+ } else {
int sign = vpMath::sign(iP1.get_i() - iP2.get_i());
- if (fabs(iP[0].get_i()-iP2.get_i()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_i()-iP2.get_i())) iP1 = iP[0];
- else iP1 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_i()-iP2.get_i())) iP1 = iP[1];
- else iP1 = iP[0];
- }
+ if (fabs(iP[0].get_i() - iP2.get_i()) > 5) {
+ if (sign == vpMath::sign(iP[0].get_i() - iP2.get_i()))
+ iP1 = iP[0];
+ else
+ iP1 = iP[1];
+ } else {
+ if (sign == vpMath::sign(iP[1].get_i() - iP2.get_i()))
+ iP1 = iP[1];
+ else
+ iP1 = iP[0];
+ }
}
- if (vpImagePoint::distance(iP1,iP2) < 9)
- {
- iP1 = iPtemp;
+ if (vpImagePoint::distance(iP1, iP2) < 9) {
+ iP1 = iPtemp;
return false;
}
- }
- else if (!iP2In)
- {
+ } else if (!iP2In) {
vpImagePoint iPtemp = iP2;
- if (fabs(a) < 1)
- {
+ if (fabs(a) < 1) {
int sign = vpMath::sign(iP2.get_j() - iP1.get_j());
- if (fabs(iP[0].get_j()-iP1.get_j()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_j()-iP1.get_j())) iP2 = iP[0];
- else iP2 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_j()-iP1.get_j())) iP2 = iP[1];
- else iP2 = iP[0];
- }
- }
- else
- {
+ if (fabs(iP[0].get_j() - iP1.get_j()) > 5) {
+ if (sign == vpMath::sign(iP[0].get_j() - iP1.get_j()))
+ iP2 = iP[0];
+ else
+ iP2 = iP[1];
+ } else {
+ if (sign == vpMath::sign(iP[1].get_j() - iP1.get_j()))
+ iP2 = iP[1];
+ else
+ iP2 = iP[0];
+ }
+ } else {
int sign = vpMath::sign(iP2.get_i() - iP1.get_i());
- if (fabs(iP[0].get_i()-iP1.get_i()) > 5)
- {
- if (sign == vpMath::sign(iP[0].get_i()-iP1.get_i())) iP2 = iP[0];
- else iP2 = iP[1];
- }
- else
- {
- if (sign == vpMath::sign(iP[1].get_i()-iP1.get_i())) iP2 = iP[1];
- else iP2 = iP[0];
- }
+ if (fabs(iP[0].get_i() - iP1.get_i()) > 5) {
+ if (sign == vpMath::sign(iP[0].get_i() - iP1.get_i()))
+ iP2 = iP[0];
+ else
+ iP2 = iP[1];
+ } else {
+ if (sign == vpMath::sign(iP[1].get_i() - iP1.get_i()))
+ iP2 = iP[1];
+ else
+ iP2 = iP[0];
+ }
}
- if (vpImagePoint::distance(iP1,iP2) < 9)
- {
- iP2 = iPtemp;
+ if (vpImagePoint::distance(iP1, iP2) < 9) {
+ iP2 = iPtemp;
return false;
}
}
@@ -863,78 +811,70 @@ vpPlotGraph::check3Dline(vpImagePoint &iP1, vpImagePoint &iP2)
return true;
}
-bool
-vpPlotGraph::check3Dpoint(vpImagePoint &iP)
-{
- if (!iP.inRectangle(dGraphZone3D))
- {
+bool vpPlotGraph::check3Dpoint(vpImagePoint &iP)
+{
+ if (!iP.inRectangle(dGraphZone3D)) {
if (iP.get_i() < dTopLeft3D.get_i())
iP.set_i(dTopLeft3D.get_i());
- else if (iP.get_i() > dTopLeft3D.get_i()+dHeight)
- iP.set_i(dTopLeft3D.get_i()+dHeight-1);
- if (iP.get_j() <dTopLeft3D.get_j())
+ else if (iP.get_i() > dTopLeft3D.get_i() + dHeight)
+ iP.set_i(dTopLeft3D.get_i() + dHeight - 1);
+ if (iP.get_j() < dTopLeft3D.get_j())
iP.set_j(dTopLeft3D.get_j());
- else if (iP.get_j() > dTopLeft3D.get_j()+dWidth)
- iP.set_j(dTopLeft3D.get_j()+dWidth-1);
+ else if (iP.get_j() > dTopLeft3D.get_j() + dWidth)
+ iP.set_j(dTopLeft3D.get_j() + dWidth - 1);
return false;
}
return true;
}
-void
-vpPlotGraph::computeGraphParameters3D()
+void vpPlotGraph::computeGraphParameters3D()
{
- zoomx_3D = w_xsize/(xmax-xmin);
- zoomy_3D = w_ysize/(ymax-ymin);
- zoomz_3D = w_zsize/(zmax-zmin);
- ptXorg = w_xval - zoomx_3D*xmax;
- ptYorg = w_yval + zoomy_3D*ymin;
- ptZorg = w_zval - zoomz_3D*zmax;
+ zoomx_3D = w_xsize / (xmax - xmin);
+ zoomy_3D = w_ysize / (ymax - ymin);
+ zoomz_3D = w_zsize / (zmax - zmin);
+ ptXorg = w_xval - zoomx_3D * xmax;
+ ptYorg = w_yval + zoomy_3D * ymin;
+ ptZorg = w_zval - zoomz_3D * zmax;
}
void getGrid3DPoint(const double pente, vpImagePoint &iPunit, vpImagePoint &ip1, vpImagePoint &ip2, vpImagePoint &ip3)
{
- if (pente <= 1)
- {
- ip1 = iPunit-vpImagePoint(3,0);
- ip2 = iPunit+vpImagePoint(3,0);
- ip3 = iPunit-vpImagePoint(6,6);
- }
- else
- {
- ip1 = iPunit-vpImagePoint(0,3);
- ip2 = iPunit+vpImagePoint(0,3);
- ip3 = iPunit+vpImagePoint(6,6);
- }
+ if (pente <= 1) {
+ ip1 = iPunit - vpImagePoint(3, 0);
+ ip2 = iPunit + vpImagePoint(3, 0);
+ ip3 = iPunit - vpImagePoint(6, 6);
+ } else {
+ ip1 = iPunit - vpImagePoint(0, 3);
+ ip2 = iPunit + vpImagePoint(0, 3);
+ ip3 = iPunit + vpImagePoint(6, 6);
+ }
}
-void
-vpPlotGraph::displayGrid3D (vpImage<unsigned char> &I)
+void vpPlotGraph::displayGrid3D(vpImage<unsigned char> &I)
{
computeGraphParameters3D();
-
- xdelt = (xmax-xmin)/nbDivisionx;
- ydelt = (ymax-ymin)/nbDivisiony;
- zdelt = (zmax-zmin)/nbDivisionz;
-
+
+ xdelt = (xmax - xmin) / nbDivisionx;
+ ydelt = (ymax - ymin) / nbDivisiony;
+ zdelt = (zmax - zmin) / nbDivisionz;
+
vpPoint pt[6];
- pt[0].setWorldCoordinates(-w_xval,ptYorg,ptZorg);
- pt[1].setWorldCoordinates(w_xval,ptYorg,ptZorg);
- pt[2].setWorldCoordinates(ptXorg,-w_yval,ptZorg);
- pt[3].setWorldCoordinates(ptXorg,w_yval,ptZorg);
- pt[4].setWorldCoordinates(ptXorg,ptYorg,-w_zval);
- pt[5].setWorldCoordinates(ptXorg,ptYorg,w_zval);
-
+ pt[0].setWorldCoordinates(-w_xval, ptYorg, ptZorg);
+ pt[1].setWorldCoordinates(w_xval, ptYorg, ptZorg);
+ pt[2].setWorldCoordinates(ptXorg, -w_yval, ptZorg);
+ pt[3].setWorldCoordinates(ptXorg, w_yval, ptZorg);
+ pt[4].setWorldCoordinates(ptXorg, ptYorg, -w_zval);
+ pt[5].setWorldCoordinates(ptXorg, ptYorg, w_zval);
+
vpImagePoint iP[6];
- for (unsigned int i = 0; i < 6; i++)
- {
+ for (unsigned int i = 0; i < 6; i++) {
pt[i].track(cMo);
- double u=0.0,v=0.0;
- vpMeterPixelConversion::convertPoint(cam,pt[i].get_x(),pt[i].get_y(),u,v);
- iP[i].set_uv(u,v);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, pt[i].get_x(), pt[i].get_y(), u, v);
+ iP[i].set_uv(u, v);
iP[i] = iP[i] + dTopLeft3D;
}
-
+
int power;
double t;
char valeur[20];
@@ -945,450 +885,443 @@ vpPlotGraph::displayGrid3D (vpImage<unsigned char> &I)
vpImagePoint ip2;
vpImagePoint ip3;
vpImagePoint ip4;
-
+
power = laFonctionSansNom(xdelt);
- ptunit.setWorldCoordinates(-w_xval,ptYorg,ptZorg);
- //if (iP[0].get_j()-iP[1].get_j() != 0)
- if (std::fabs(iP[0].get_j()-iP[1].get_j()) >
- vpMath::maximum(std::fabs(iP[0].get_j()), std::fabs(iP[1].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[0].get_i()-iP[1].get_i())/(iP[0].get_j()-iP[1].get_j()));
- else pente = 2;
-
+ ptunit.setWorldCoordinates(-w_xval, ptYorg, ptZorg);
+ // if (iP[0].get_j()-iP[1].get_j() != 0)
+ if (std::fabs(iP[0].get_j() - iP[1].get_j()) >
+ vpMath::maximum(std::fabs(iP[0].get_j()), std::fabs(iP[1].get_j())) * std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[0].get_i() - iP[1].get_i()) / (iP[0].get_j() - iP[1].get_j()));
+ else
+ pente = 2;
+
unsigned int count = 1;
- for(t=xmin;t<=xmax;t=t+xdelt)
- {
- double x = ptXorg+(zoomx_3D*t);
+ for (t = xmin; t <= xmax; t = t + xdelt) {
+ double x = ptXorg + (zoomx_3D * t);
ptunit.set_oX(x);
ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
+ iPunit.set_uv(u, v);
iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip2, ip1, vpColor::black);
- if (count%2 == 1)
- {
+
+ getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
+
+ if (check3Dline(ip1, ip2)) {
+ vpDisplay::displayLine(I, ip2, ip1, vpColor::black);
+ if (count % 2 == 1) {
double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
+ if (power != 0)
+ ttemp = t * pow(10.0, power);
+ else
+ ttemp = t;
sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayText(I,ip3,valeur, vpColor::black);
+ vpDisplay::displayText(I, ip3, valeur, vpColor::black);
}
}
count++;
}
- if (power != 0)
- {
- ip4 = iP[1] -vpImagePoint(-15,10);
+ if (power != 0) {
+ ip4 = iP[1] - vpImagePoint(-15, 10);
sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayText(I,ip4,valeur, vpColor::black);
+ if (check3Dpoint(ip4))
+ vpDisplay::displayText(I, ip4, valeur, vpColor::black);
}
-
+
power = laFonctionSansNom(ydelt);
- ptunit.setWorldCoordinates(ptXorg,-w_yval,ptZorg);
- //if (iP[2].get_j()-iP[3].get_j() != 0)
- if (std::fabs(iP[2].get_j()-iP[3].get_j()) >
- vpMath::maximum(std::fabs(iP[2].get_j()), std::fabs(iP[3].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[2].get_i()-iP[3].get_i())/(iP[2].get_j()-iP[3].get_j()));
- else pente = 2;
+ ptunit.setWorldCoordinates(ptXorg, -w_yval, ptZorg);
+ // if (iP[2].get_j()-iP[3].get_j() != 0)
+ if (std::fabs(iP[2].get_j() - iP[3].get_j()) >
+ vpMath::maximum(std::fabs(iP[2].get_j()), std::fabs(iP[3].get_j())) * std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[2].get_i() - iP[3].get_i()) / (iP[2].get_j() - iP[3].get_j()));
+ else
+ pente = 2;
count = 0;
- for(t=ymin;t<=ymax;t=t+ydelt)
- {
- double y = ptYorg-(zoomy_3D*t);
+ for (t = ymin; t <= ymax; t = t + ydelt) {
+ double y = ptYorg - (zoomy_3D * t);
ptunit.set_oY(y);
ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
+ iPunit.set_uv(u, v);
iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
- if (count%2 == 1)
- {
+
+ getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
+
+ if (check3Dline(ip1, ip2)) {
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::black);
+ if (count % 2 == 1) {
double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
+ if (power != 0)
+ ttemp = t * pow(10.0, power);
+ else
+ ttemp = t;
sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayText(I,ip3,valeur, vpColor::black);
+ vpDisplay::displayText(I, ip3, valeur, vpColor::black);
}
}
count++;
}
- if (power != 0)
- {
- ip4 = iP[2] -vpImagePoint(-15,10);
+ if (power != 0) {
+ ip4 = iP[2] - vpImagePoint(-15, 10);
sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayText(I,ip4,valeur, vpColor::black);
+ if (check3Dpoint(ip4))
+ vpDisplay::displayText(I, ip4, valeur, vpColor::black);
}
-
+
power = laFonctionSansNom(zdelt);
- ptunit.setWorldCoordinates(ptXorg,ptYorg,-w_zval);
- //if (iP[4].get_j()-iP[5].get_j() != 0)
- if (std::fabs(iP[4].get_j()-iP[5].get_j()) >
- vpMath::maximum(std::fabs(iP[4].get_j()), std::fabs(iP[5].get_j()))* std::numeric_limits<double>::epsilon())
- pente = fabs((iP[4].get_i()-iP[5].get_i())/(iP[4].get_j()-iP[5].get_j()));
- else pente = 2;
+ ptunit.setWorldCoordinates(ptXorg, ptYorg, -w_zval);
+ // if (iP[4].get_j()-iP[5].get_j() != 0)
+ if (std::fabs(iP[4].get_j() - iP[5].get_j()) >
+ vpMath::maximum(std::fabs(iP[4].get_j()), std::fabs(iP[5].get_j())) * std::numeric_limits<double>::epsilon())
+ pente = fabs((iP[4].get_i() - iP[5].get_i()) / (iP[4].get_j() - iP[5].get_j()));
+ else
+ pente = 2;
count = 0;
- for(t=zmin;t<=zmax;t=t+zdelt)
- {
- double z = ptZorg+(zoomz_3D*t);
+ for (t = zmin; t <= zmax; t = t + zdelt) {
+ double z = ptZorg + (zoomz_3D * t);
ptunit.set_oZ(z);
ptunit.track(cMo);
- double u=0.0, v=0.0;
- vpMeterPixelConversion::convertPoint(cam,ptunit.get_x(),ptunit.get_y(),u,v);
- iPunit.set_uv(u,v);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, ptunit.get_x(), ptunit.get_y(), u, v);
+ iPunit.set_uv(u, v);
iPunit = iPunit + dTopLeft3D;
-
- getGrid3DPoint(pente,iPunit,ip1,ip2,ip3);
-
- if(check3Dline(ip1,ip2))
- {
- vpDisplay::displayLine(I,ip1, ip2, vpColor::black);
- if (count%2 == 1)
- {
+
+ getGrid3DPoint(pente, iPunit, ip1, ip2, ip3);
+
+ if (check3Dline(ip1, ip2)) {
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::black);
+ if (count % 2 == 1) {
double ttemp;
- if (power!=0)
- ttemp = t*pow(10.0,power);
- else ttemp = t;
+ if (power != 0)
+ ttemp = t * pow(10.0, power);
+ else
+ ttemp = t;
sprintf(valeur, "%.1f", ttemp);
- vpDisplay::displayText(I,ip3,valeur, vpColor::black);
+ vpDisplay::displayText(I, ip3, valeur, vpColor::black);
}
}
count++;
}
- if (power != 0)
- {
- ip4 = iP[5] -vpImagePoint(-15,10);
+ if (power != 0) {
+ ip4 = iP[5] - vpImagePoint(-15, 10);
sprintf(valeur, "x10e%d", -power);
- if(check3Dpoint(ip4))
- vpDisplay::displayText(I,ip4,valeur, vpColor::black);
+ if (check3Dpoint(ip4))
+ vpDisplay::displayText(I, ip4, valeur, vpColor::black);
}
-
-
-//Ligne horizontal
- if (check3Dline(iP[0],iP[1]))
- {
- vpDisplay::displayArrow(I,iP[0],iP[1], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[1].get_i(),iP[1].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayText(I,iPunit,unitx.c_str(), vpColor::black);
+
+ // Ligne horizontal
+ if (check3Dline(iP[0], iP[1])) {
+ vpDisplay::displayArrow(I, iP[0], iP[1], vpColor::black, gridThickness);
+ if (dispUnit) {
+ iPunit.set_ij(iP[1].get_i(), iP[1].get_j() - 10 * epsj);
+ check3Dpoint(iPunit);
+ vpDisplay::displayText(I, iPunit, unitx.c_str(), vpColor::black);
}
}
- if (check3Dline(iP[3],iP[2]))
- {
- vpDisplay::displayArrow(I,iP[3],iP[2], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[2].get_i(),iP[2].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayText(I,iPunit,unity.c_str(), vpColor::black);
+ if (check3Dline(iP[3], iP[2])) {
+ vpDisplay::displayArrow(I, iP[3], iP[2], vpColor::black, gridThickness);
+ if (dispUnit) {
+ iPunit.set_ij(iP[2].get_i(), iP[2].get_j() - 10 * epsj);
+ check3Dpoint(iPunit);
+ vpDisplay::displayText(I, iPunit, unity.c_str(), vpColor::black);
}
}
- if (check3Dline(iP[4],iP[5]))
- {
- vpDisplay::displayArrow(I,iP[4],iP[5], vpColor::black, gridThickness);
- if (dispUnit)
- {
- iPunit.set_ij(iP[5].get_i(),iP[5].get_j()-10*epsj);
- check3Dpoint (iPunit);
- vpDisplay::displayText(I,iPunit,unitz.c_str(), vpColor::black);
+ if (check3Dline(iP[4], iP[5])) {
+ vpDisplay::displayArrow(I, iP[4], iP[5], vpColor::black, gridThickness);
+ if (dispUnit) {
+ iPunit.set_ij(iP[5].get_i(), iP[5].get_j() - 10 * epsj);
+ check3Dpoint(iPunit);
+ vpDisplay::displayText(I, iPunit, unitz.c_str(), vpColor::black);
}
}
-
+
if (dispTitle)
displayTitle(I);
if (dispLegend)
displayLegend(I);
}
-vpMouseButton::vpMouseButtonType
-vpPlotGraph::plot (vpImage<unsigned char> &I, const unsigned int curveNb, const double x, const double y, const double z)
+vpMouseButton::vpMouseButtonType vpPlotGraph::plot(vpImage<unsigned char> &I, const unsigned int curveNb,
+ const double x, const double y, const double z)
{
- if (!scaleInitialized)
- {
- if (x < 0)
- {
+ if (!scaleInitialized) {
+ if (x < 0) {
xmax = 0;
- rescalex(0,x);
+ rescalex(0, x);
}
- if (x > 0)
- {
+ if (x > 0) {
xmin = 0;
- rescalex(1,x);
+ rescalex(1, x);
}
- if (y < 0)
- {
+ if (y < 0) {
ymax = 0;
- rescaley(0,y);
+ rescaley(0, y);
}
- if (y > 0)
- {
+ if (y > 0) {
ymin = 0;
- rescaley(1,y);
+ rescaley(1, y);
}
- if (z < 0)
- {
+ if (z < 0) {
zmax = 0;
- rescalez(0,z);
+ rescalez(0, z);
}
- if (z > 0)
- {
+ if (z > 0) {
zmin = 0;
- rescalez(1,z);
+ rescalez(1, z);
}
scaleInitialized = true;
computeGraphParameters3D();
clearGraphZone(I);
displayGrid3D(I);
- //if (std::fabs(y) == 0 || z == 0)
- if (std::fabs(y) <= std::numeric_limits<double>::epsilon() || std::fabs(z) <= std::numeric_limits<double>::epsilon())
+ // if (std::fabs(y) == 0 || z == 0)
+ if (std::fabs(y) <= std::numeric_limits<double>::epsilon() ||
+ std::fabs(z) <= std::numeric_limits<double>::epsilon())
scaleInitialized = false;
}
-
- if (firstPoint)
- {
+
+ if (firstPoint) {
clearGraphZone(I);
displayGrid3D(I);
- vpDisplay::flushROI(I,graphZone);
+ vpDisplay::flushROI(I, graphZone);
firstPoint = false;
}
-
+
bool changed = false;
- if (x > xmax) {rescalex(1,x); changed = true;}
- else if(x < xmin) {rescalex(0,x);changed = true;}
-
- if (y > ymax) {rescaley(1,y);changed = true;}
- else if(y < ymin) {rescaley(0,y);changed = true;}
-
- if (z > zmax) {rescalez(1,z);changed = true;}
- else if(z < zmin) {rescalez(0,z);changed = true;}
-
+ if (x > xmax) {
+ rescalex(1, x);
+ changed = true;
+ } else if (x < xmin) {
+ rescalex(0, x);
+ changed = true;
+ }
+
+ if (y > ymax) {
+ rescaley(1, y);
+ changed = true;
+ } else if (y < ymin) {
+ rescaley(0, y);
+ changed = true;
+ }
+
+ if (z > zmax) {
+ rescalez(1, z);
+ changed = true;
+ } else if (z < zmin) {
+ rescalez(0, z);
+ changed = true;
+ }
+
vpMouseButton::vpMouseButtonType button = vpMouseButton::none;
- if (changed || move(I, button))
- {
+ if (changed || move(I, button)) {
computeGraphParameters3D();
replot3D(I);
}
vpPoint pointPlot;
- pointPlot.setWorldCoordinates(ptXorg+(zoomx_3D*x),ptYorg-(zoomy_3D*y),ptZorg+(zoomz_3D*z));
+ pointPlot.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);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, pointPlot.get_x(), pointPlot.get_y(), u, v);
vpImagePoint iP;
- iP.set_uv(u,v);
+ iP.set_uv(u, v);
iP = iP + dTopLeft3D;
-
- (curveList+curveNb)->pointListz.end();
- if((curveList+curveNb)->nbPoint)
- {
- if (check3Dline((curveList+curveNb)->lastPoint,iP))
- vpDisplay::displayLine(I,(curveList+curveNb)->lastPoint, iP, (curveList+curveNb)->color, (curveList+curveNb)->thickness);
+
+ if ((curveList + curveNb)->nbPoint) {
+ if (check3Dline((curveList + curveNb)->lastPoint, iP))
+ vpDisplay::displayLine(I, (curveList + curveNb)->lastPoint, iP, (curveList + curveNb)->color,
+ (curveList + curveNb)->thickness);
}
-#if( defined VISP_HAVE_X11 || defined VISP_HAVE_GDI )
+#if (defined VISP_HAVE_X11 || defined VISP_HAVE_GDI)
double top;
double left;
double width_;
double height_;
-
- if (iP.get_i() <= (curveList+curveNb)->lastPoint.get_i()) {top = iP.get_i()-5; height_ = (curveList+curveNb)->lastPoint.get_i() - top+10;}
- else {top = (curveList+curveNb)->lastPoint.get_i()-5; height_ = iP.get_i() - top+10;}
- if (iP.get_j() <= (curveList+curveNb)->lastPoint.get_j()) {left = iP.get_j()-5; width_ = (curveList+curveNb)->lastPoint.get_j() - left+10;}
- else {left = (curveList+curveNb)->lastPoint.get_j()-5; width_ = iP.get_j() - left+10;}
- vpDisplay::flushROI(I,vpRect(left,top,width_,height_));
+
+ if (iP.get_i() <= (curveList + curveNb)->lastPoint.get_i()) {
+ top = iP.get_i() - 5;
+ height_ = (curveList + curveNb)->lastPoint.get_i() - top + 10;
+ } else {
+ top = (curveList + curveNb)->lastPoint.get_i() - 5;
+ height_ = iP.get_i() - top + 10;
+ }
+ if (iP.get_j() <= (curveList + curveNb)->lastPoint.get_j()) {
+ left = iP.get_j() - 5;
+ width_ = (curveList + curveNb)->lastPoint.get_j() - left + 10;
+ } else {
+ left = (curveList + curveNb)->lastPoint.get_j() - 5;
+ width_ = iP.get_j() - left + 10;
+ }
+ vpDisplay::flushROI(I, vpRect(left, top, width_, height_));
#endif
-
- (curveList+curveNb)->lastPoint = iP;
- (curveList+curveNb)->pointListx.push_back(x);
- (curveList+curveNb)->pointListy.push_back(y);
- (curveList+curveNb)->pointListz.push_back(z);
- (curveList+curveNb)->nbPoint++;
-
-#if( !defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
- vpDisplay::flushROI(I,graphZone);
+
+ (curveList + curveNb)->lastPoint = iP;
+ (curveList + curveNb)->pointListx.push_back(x);
+ (curveList + curveNb)->pointListy.push_back(y);
+ (curveList + curveNb)->pointListz.push_back(z);
+ (curveList + curveNb)->nbPoint++;
+
+#if (!defined VISP_HAVE_X11 && defined FLUSH_ON_PLOT)
+ vpDisplay::flushROI(I, graphZone);
#endif
return button;
}
-void
-vpPlotGraph::replot3D (vpImage<unsigned char> &I)
+void vpPlotGraph::replot3D(vpImage<unsigned char> &I)
{
clearGraphZone(I);
displayGrid3D(I);
-
- for (unsigned int i = 0; i < curveNbr; i++)
- {
- std::list<double>::const_iterator it_ptListx = (curveList+i)->pointListx.begin();
- std::list<double>::const_iterator it_ptListy = (curveList+i)->pointListy.begin();
- std::list<double>::const_iterator it_ptListz = (curveList+i)->pointListz.begin();
-
+
+ for (unsigned int i = 0; i < curveNbr; i++) {
+ std::list<double>::const_iterator it_ptListx = (curveList + i)->pointListx.begin();
+ std::list<double>::const_iterator it_ptListy = (curveList + i)->pointListy.begin();
+ std::list<double>::const_iterator it_ptListz = (curveList + i)->pointListz.begin();
+
unsigned int k = 0;
vpImagePoint iP;
vpPoint pointPlot;
- while (k < (curveList+i)->nbPoint)
- {
+ while (k < (curveList + i)->nbPoint) {
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);
- iP.set_uv(u,v);
+ double u = 0.0, v = 0.0;
+ vpMeterPixelConversion::convertPoint(cam, pointPlot.get_x(), pointPlot.get_y(), u, v);
+ iP.set_uv(u, v);
iP = iP + dTopLeft3D;
-
- //vpDisplay::displayCross(I,iP,3,vpColor::cyan);
- if (k > 0)
- {
- if (check3Dline((curveList+i)->lastPoint,iP))
- vpDisplay::displayLine(I,(curveList+i)->lastPoint, iP, (curveList+i)->color);
- //vpDisplay::displayCross(I,iP,3,vpColor::orange);
+
+ // vpDisplay::displayCross(I,iP,3,vpColor::cyan);
+ if (k > 0) {
+ if (check3Dline((curveList + i)->lastPoint, iP))
+ vpDisplay::displayLine(I, (curveList + i)->lastPoint, iP, (curveList + i)->color);
+ // vpDisplay::displayCross(I,iP,3,vpColor::orange);
}
-
- (curveList+i)->lastPoint = iP;
-
+
+ (curveList + i)->lastPoint = iP;
+
++it_ptListx;
++it_ptListy;
++it_ptListz;
k++;
}
}
- vpDisplay::flushROI(I,graphZone);
+ vpDisplay::flushROI(I, graphZone);
}
-
-
-void
-vpPlotGraph::rescalez(unsigned int side, double extremity)
+void vpPlotGraph::rescalez(unsigned int side, double extremity)
{
- switch (side)
- {
- case 0:
- zmin = (3*extremity-zmax)/2;
- break;
- case 1:
- zmax = (3*extremity-zmin)/2;
- break;
+ switch (side) {
+ case 0:
+ zmin = (3 * extremity - zmax) / 2;
+ break;
+ case 1:
+ zmax = (3 * extremity - zmin) / 2;
+ break;
}
-
- zdelt = (zmax-zmin)/(double)nbDivisionz;
+
+ zdelt = (zmax - zmin) / (double)nbDivisionz;
}
/*!
\param I : Image used to display the graph.
\param button : Mouse button used during navigation.
*/
-bool
-vpPlotGraph::move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button)
+bool vpPlotGraph::move(const vpImage<unsigned char> &I, vpMouseButton::vpMouseButtonType &button)
{
bool changed = false;
vpHomogeneousMatrix displacement = navigation(I, changed, button);
- //if (displacement[2][3] != 0)
+ // if (displacement[2][3] != 0)
if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- cMf = cMf*displacement;
- vpHomogeneousMatrix fMo = cMf.inverse()*cMo;
-
- cMo = cMf* displacement * fMo;
+ cMf = cMf * displacement;
+ vpHomogeneousMatrix fMo = cMf.inverse() * cMo;
+
+ cMo = cMf * displacement * fMo;
return changed;
}
-vpHomogeneousMatrix
-vpPlotGraph::navigation(const vpImage<unsigned char> &I, bool &changed, vpMouseButton::vpMouseButtonType &b)
+vpHomogeneousMatrix vpPlotGraph::navigation(const vpImage<unsigned char> &I, bool &changed,
+ vpMouseButton::vpMouseButtonType &b)
{
vpImagePoint iP;
vpImagePoint trash;
bool clicked = false;
bool clickedUp = false;
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
changed = false;
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
-
- if(!blocked) clicked = vpDisplay::getClick(I,iP,b,false);
+ // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
- if(blocked) clickedUp = vpDisplay::getClickUp(I,trash, b,false);
-
- if (clicked)
- {
+ if (!blocked)
+ clicked = vpDisplay::getClick(I, iP, b, false);
+
+ if (blocked)
+ clickedUp = vpDisplay::getClickUp(I, trash, b, false);
+
+ if (clicked) {
if (!iP.inRectangle(graphZone))
return mov;
}
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
+ if (clicked) {
+ if (b == vpMouseButton::button1)
+ blockedr = true;
+ if (b == vpMouseButton::button2)
+ blockedz = true;
blocked = true;
}
-
- else if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
+
+ else if (clickedUp) {
+ if (b == vpMouseButton::button1) {
+ old_iPr = vpImagePoint(-1, -1);
blockedr = false;
}
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
+ if (b == vpMouseButton::button2) {
+ old_iPz = vpImagePoint(-1, -1);
blockedz = false;
}
- if (!(blockedr || blockedz))
- {
+ if (!(blockedr || blockedz)) {
blocked = false;
- //while (vpDisplay::getClick(I,trash,b,false)) {};
+ // while (vpDisplay::getClick(I,trash,b,false)) {};
}
}
vpTime::sleepMs(5);
- vpDisplay::getPointerPosition(I,iP);
+ vpDisplay::getPointerPosition(I, iP);
+
+ if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
+ double width_ = vpMath::minimum(I.getWidth(), I.getHeight());
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
- double width_ = vpMath::minimum(I.getWidth(),I.getHeight());
-
double diffi = iP.get_i() - old_iPr.get_i();
double diffj = iP.get_j() - old_iPr.get_j();
-
- double anglei = diffi*360/width_;
- double anglej = diffj*360/width_;
- mov.buildFrom(0,0,0,vpMath::rad(anglei),vpMath::rad(-anglej),0);
+
+ double anglei = diffi * 360 / width_;
+ double anglej = diffj * 360 / width_;
+ mov.buildFrom(0, 0, 0, vpMath::rad(anglei), vpMath::rad(-anglej), 0);
changed = true;
}
- if (blockedr) old_iPr = iP;
+ if (blockedr)
+ old_iPr = iP;
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
+ if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
changed = true;
}
- if (blockedz) old_iPz = iP;
+ if (blockedz)
+ old_iPz = iP;
return mov;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpPlotGraph.cpp.o) has no symbols
-void dummy_vpPlotGraph() {};
+// Work arround to avoid warning: libvisp_core.a(vpPlotGraph.cpp.o) has no
+// symbols
+void dummy_vpPlotGraph(){};
#endif
#endif
diff --git a/modules/gui/test/display/testClick.cpp b/modules/gui/test/display/testClick.cpp
index fc847b2..358286a 100644
--- a/modules/gui/test/display/testClick.cpp
+++ b/modules/gui/test/display/testClick.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,22 +39,23 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
#include <iostream>
+#include <stdlib.h>
#include <string>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \
+ defined(VISP_HAVE_OPENCV))
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
/*!
\example testClick.cpp
@@ -63,20 +65,13 @@
*/
// List of allowed command line options
-#define GETOPTARGS "i:hlt:dc"
+#define GETOPTARGS "i:hlt:dc"
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
+typedef enum { vpX11, vpGTK, vpGDI, vpD3D, vpCV } vpDisplayType;
void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display );
+bool getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed,
+ bool &display);
/*!
@@ -99,20 +94,30 @@ SYNOPSIS\n\
", name);
std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
+ switch (dtype) {
+ case vpX11:
+ display = "X11";
+ break;
+ case vpGTK:
+ display = "GTK";
+ break;
+ case vpGDI:
+ display = "GDI";
+ break;
+ case vpD3D:
+ display = "D3D";
+ break;
+ case vpCV:
+ display = "CV";
+ break;
}
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -137,8 +142,7 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), display.c_str());
+ Print the help.\n\n", ipath.c_str(), display.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -162,47 +166,55 @@ 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 &display)
+bool getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed,
+ bool &display)
{
const char *optarg_;
- int c;
+ int c;
std::string sDisplayType;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'l':
+ list = true;
+ break;
+ case 't':
+ sDisplayType = optarg_;
// Parse the display type option
if (sDisplayType.compare("X11") == 0) {
dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
+ } else if (sDisplayType.compare("GTK") == 0) {
dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
+ } else if (sDisplayType.compare("GDI") == 0) {
dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
+ } else if (sDisplayType.compare("D3D") == 0) {
dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
+ } else if (sDisplayType.compare("CV") == 0) {
dtype = vpCV;
}
break;
- case 'h': usage(argv[0], NULL, ipath, dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
+ case 'h':
+ usage(argv[0], NULL, ipath, dtype);
+ return false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
default:
- usage(argv[0], optarg_, ipath, dtype); return false; break;
+ usage(argv[0], optarg_, ipath, dtype);
+ return false;
+ break;
}
}
-
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL, ipath, dtype);
@@ -214,20 +226,19 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
std::string opt_ipath;
- bool opt_list = false; // To print the list of video devices
+ bool opt_list = false; // To print the list of video devices
vpDisplayType opt_dtype; // Type of display to use
std::string ipath;
std::string filename;
bool opt_click_allowed = true;
bool opt_display = true;
- // Default display is one available
+// Default display is one available
#if defined VISP_HAVE_GTK
opt_dtype = vpGTK;
#elif defined VISP_HAVE_X11
@@ -237,20 +248,20 @@ main(int argc, const char ** argv)
#elif defined VISP_HAVE_D3D9
opt_dtype = vpD3D;
#elif defined VISP_HAVE_OPENCV
- opt_dtype = vpCV;
+ opt_dtype = vpCV;
#endif
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Print the list of video-devices available
@@ -259,31 +270,30 @@ main(int argc, const char ** argv)
std::cout << "List of video-devices available: \n";
#if defined VISP_HAVE_GTK
std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_X11
std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_GDI
std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_D3D9
std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_OPENCV
std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
+ nbDevices++;
+#endif
if (!nbDevices) {
std::cout << " No display is available\n";
}
return (0);
}
-
// Get the option values
if (!opt_ipath.empty())
ipath = opt_ipath;
@@ -292,8 +302,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -301,37 +310,35 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(I, filename);
// Create a display for the image
vpDisplay *display = NULL;
- switch(opt_dtype) {
+ switch (opt_dtype) {
case vpX11:
std::cout << "Requested X11 display functionnalities..." << std::endl;
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#else
std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
+ std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n";
return 0;
#endif
break;
@@ -341,8 +348,7 @@ 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;
@@ -352,8 +358,7 @@ 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;
@@ -363,8 +368,7 @@ 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;
@@ -374,66 +378,74 @@ main(int argc, const char ** argv)
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;
}
- if (opt_display){
+ if (opt_display) {
// We open a window using either X11 or GTK or GDI.
// Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
+ display->init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- if ( opt_click_allowed ){
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
+ if (opt_click_allowed) {
std::cout << "Click on a pixel to get his coordinates...\n";
vpImagePoint ip;
vpMouseButton::vpMouseButtonType button;
vpDisplay::getClick(I, ip, button);
- std::cout << " You click down on pixel (" << ip <<") ";
- switch(button) {
- case vpMouseButton::button1: std::cout << "with left button.\n"; break;
- case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
- case vpMouseButton::button3: std::cout << "with right button.\n"; break;
- case vpMouseButton::none: break;
+ std::cout << " You click down on pixel (" << ip << ") ";
+ switch (button) {
+ case vpMouseButton::button1:
+ std::cout << "with left button.\n";
+ break;
+ case vpMouseButton::button2:
+ std::cout << "with middle button.\n";
+ break;
+ case vpMouseButton::button3:
+ std::cout << "with right button.\n";
+ break;
+ case vpMouseButton::none:
+ break;
}
vpDisplay::getClickUp(I, ip, button);
- std::cout << " You click up on pixel (" << ip <<") ";
- switch(button) {
- case vpMouseButton::button1: std::cout << "with left button.\n"; break;
- case vpMouseButton::button2: std::cout << "with middle button.\n"; break;
- case vpMouseButton::button3: std::cout << "with right button.\n"; break;
- case vpMouseButton::none: break;
+ std::cout << " You click up on pixel (" << ip << ") ";
+ switch (button) {
+ case vpMouseButton::button1:
+ std::cout << "with left button.\n";
+ break;
+ case vpMouseButton::button2:
+ std::cout << "with middle button.\n";
+ break;
+ case vpMouseButton::button3:
+ std::cout << "with right button.\n";
+ break;
+ case vpMouseButton::none:
+ break;
}
- vpDisplay::getPointerPosition(I,ip);
+ vpDisplay::getPointerPosition(I, ip);
std::cout << " Pointer poisition : " << ip << std::endl;
std::cout << "A click to exit...\n";
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
}
delete display;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
+int main() { vpERROR_TRACE("You do not have display functionalities..."); }
#endif
diff --git a/modules/gui/test/display/testDisplayPolygonLines.cpp b/modules/gui/test/display/testDisplayPolygonLines.cpp
new file mode 100644
index 0000000..8216f1c
--- /dev/null
+++ b/modules/gui/test/display/testDisplayPolygonLines.cpp
@@ -0,0 +1,230 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test display polygon lines
+ *
+ *****************************************************************************/
+
+/*!
+ \example testDisplayPolygonLines.cpp
+
+ Test display polygon lines.
+*/
+
+#include <cstdlib>
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpParseArgv.h>
+
+// List of allowed command line options
+#define GETOPTARGS "cdh"
+
+void usage(const char *name, const char *badparam);
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
+
+/*!
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+
+ */
+void usage(const char *name, const char *badparam)
+{
+ fprintf(stdout, "\n\
+Display polygon lines.\n\
+\n\
+SYNOPSIS\n\
+ %s [-c] [-d] [-h]\n", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -c\n\
+ Disable the mouse click. Useful to automate the \n\
+ execution of this program without humain intervention.\n\
+\n\
+ -d \n\
+ Disable the image display. This can be useful \n\
+ for automatic tests. \n\
+\n\
+ -h\n\
+ Print the help.\n\n");
+
+ if (badparam) {
+ fprintf(stderr, "ERROR: \n");
+ fprintf(stderr, "\nBad parameter [%s]\n", badparam);
+ }
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param click_allowed : Enable/disable mouse click.
+ \param display : Set as true, activates the image display. This is
+ the default configuration. When set to false, the display is
+ disabled. This can be useful for automatic tests using crontab
+ under Unix or using the task manager under Windows.
+
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
+{
+ const char *optarg_;
+ int c;
+
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+ switch (c) {
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+
+ default:
+ usage(argv[0], optarg_);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+#ifdef VISP_HAVE_DISPLAY
+ bool opt_click_allowed = true;
+ bool opt_display = true;
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ return EXIT_FAILURE;
+ }
+
+ if (opt_display && opt_click_allowed) {
+ vpImage<unsigned char> I(480, 640, 127);
+ vpImage<vpRGBa> I_color(480, 640);
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX d, d2;
+#elif defined(VISP_HAVE_GTK)
+ vpDisplayGTK d, d2;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d, d2;
+#elif defined(VISP_HAVE_D3D9)
+ vpDisplayD3D d, d2;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d, d2;
+#endif
+ 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::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::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::displayDotLine(I, polygon.getCorners(), true, vpColor::red, 2);
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I);
+
+ 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;
+ unsigned char rgb[3];
+ vpImageConvert::HSVToRGB(&hue, &saturation, &value, rgb, 1);
+
+ for (unsigned int j = 0; j < I_color.getWidth(); j++) {
+ I_color[i][j].R = rgb[0];
+ I_color[i][j].G = rgb[1];
+ I_color[i][j].B = rgb[2];
+ }
+ }
+
+ 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::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::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::flush(I_color);
+ vpDisplay::getClick(I_color);
+ }
+#else
+ (void)argc;
+ (void)argv;
+#endif
+ return EXIT_SUCCESS;
+}
diff --git a/modules/gui/test/display/testDisplayRoi.cpp b/modules/gui/test/display/testDisplayRoi.cpp
index feed079..4652269 100644
--- a/modules/gui/test/display/testDisplayRoi.cpp
+++ b/modules/gui/test/display/testDisplayRoi.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,16 +46,16 @@
#include <stdlib.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/core/vpRect.h>
#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpRect.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -92,10 +93,9 @@ OPTIONS: Default\n\
Print the help.\n\n");
if (badparam) {
- fprintf(stderr, "ERROR: \n" );
+ fprintf(stderr, "ERROR: \n");
fprintf(stderr, "\nBad parameter [%s]\n", badparam);
}
-
}
/*!
@@ -116,16 +116,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
- usage(argv[0], optarg_); return false; break;
+ usage(argv[0], optarg_);
+ return false;
+ break;
}
}
@@ -140,7 +149,7 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
#ifdef VISP_HAVE_DISPLAY
bool opt_click_allowed = true;
@@ -148,12 +157,12 @@ int main(int argc, const char ** argv)
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
if (opt_display) {
- vpImage<unsigned char> I(480,640,255);
+ vpImage<unsigned char> I(480, 640, 255);
#if defined(VISP_HAVE_X11)
vpDisplayX d;
@@ -172,7 +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) {
@@ -181,14 +190,14 @@ int main(int argc, const char ** argv)
}
vpDisplay::close(I);
- vpImage<vpRGBa> C(480,640,vpRGBa(255,0,0,0));
+ vpImage<vpRGBa> C(480, 640, vpRGBa(255, 0, 0, 0));
- //vpDisplayX d;
+ // vpDisplayX d;
d.init(C);
vpDisplay::display(C);
vpDisplay::flush(C);
- C = vpRGBa(0,255,0,0);
+ C = vpRGBa(0, 255, 0, 0);
vpDisplay::displayROI(C, roi);
vpDisplay::flushROI(C, roi);
diff --git a/modules/gui/test/display/testDisplayScaled.cpp b/modules/gui/test/display/testDisplayScaled.cpp
index 8f75f5c..b9aebd9 100644
--- a/modules/gui/test/display/testDisplayScaled.cpp
+++ b/modules/gui/test/display/testDisplayScaled.cpp
@@ -1,27 +1,26 @@
#include <sstream>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpIoTools.h>
-template<typename Type>
-bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool click)
+template <typename Type> bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool click)
{
bool success = true;
- unsigned int radius(I.getHeight()/4);
+ unsigned int radius(I.getHeight() / 4);
int scale_ = (int)scale;
int radius_ = (int)radius;
unsigned int thickness = 2;
- vpImagePoint center(I.getHeight()/2, I.getWidth()/2);
+ vpImagePoint center(I.getHeight() / 2, I.getWidth() / 2);
vpImagePoint offset(30, 160);
vpImagePoint v_offset(radius, 0);
vpImagePoint h_offset(0, radius);
- vpRect roi(center, radius_+scale_, radius_);
+ vpRect roi(center, radius_ + scale_, radius_);
std::string itype;
// backup the input image
@@ -32,23 +31,19 @@ bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool
#ifdef VISP_HAVE_GDI
d = new vpDisplayGDI;
#endif
- }
- else if (display == "GTK") {
+ } else if (display == "GTK") {
#ifdef VISP_HAVE_GTK
d = new vpDisplayGTK;
#endif
- }
- else if (display == "X") {
+ } else if (display == "X") {
#ifdef VISP_HAVE_X11
d = new vpDisplayX;
#endif
- }
- else if (display == "OpenCV") {
+ } else if (display == "OpenCV") {
#ifdef VISP_HAVE_OPENCV
d = new vpDisplayOpenCV;
#endif
- }
- else if (display == "D3D9") {
+ } else if (display == "D3D9") {
#ifdef VISP_HAVE_D3D9
d = new vpDisplayD3D;
#endif
@@ -69,7 +64,7 @@ bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool
// Compare input and rendered images
if (sizeof(Type) == 1) {
itype = "uchar";
- vpImage<Type> Iinsert = I;
+ vpImage<Type> Iinsert = I;
vpImage<vpRGBa> Isampled;
vpImage<vpRGBa> Icolor;
vpImageConvert::convert(Iinsert, Icolor);
@@ -84,41 +79,41 @@ bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool
std::stringstream ss;
ss << "Isampled-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Isampled, ss.str());
- ss.str(""); ss.clear();
+ ss.str("");
+ ss.clear();
ss << "Irendered-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Irendered, ss.str());
vpImage<vpRGBa> Idiff;
vpImageTools::imageDifference(Isampled, Irendered, Idiff);
- ss.str(""); ss.clear();
+ ss.str("");
+ ss.clear();
ss << "Idiff-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Idiff, ss.str());
- }
- else {
+ } else {
std::cout << " ++ Test width scale= " << scale << " type= " << itype << ": succeed" << std::endl;
}
- }
- else {
+ } else {
itype = "rgba";
vpImage<Type> Iinsert = I;
vpImage<Type> Isampled; // vpRGBa necessary
@@ -136,67 +131,71 @@ bool test(const std::string &display, vpImage<Type> &I, unsigned int scale, bool
std::stringstream ss;
ss << "Isampled-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Isampled, ss.str());
- ss.str(""); ss.clear();
+ ss.str("");
+ ss.clear();
ss << "Irendered-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Irendered, ss.str());
vpImage<vpRGBa> Idiff;
vpImageTools::imageDifference(IsampledCopy, Irendered, Idiff);
- ss.str(""); ss.clear();
+ ss.str("");
+ ss.clear();
ss << "Idiff-" << itype << "-scale-" << scale;
- #ifdef VISP_HAVE_OPENCV
+#ifdef VISP_HAVE_OPENCV
ss << ".png";
- #else
+#else
ss << ".ppm";
- #endif
+#endif
vpImageIo::write(Idiff, ss.str());
- }
- else {
- std::cout << " ++ Test width scale= " << scale << " type= " << itype << ": succeed" << std::endl;
+ } else {
+ 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);
- 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);
+ 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);
}
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);
vpImage<vpRGBa> Irendered;
vpDisplay::getImage(I, Irendered);
-
std::stringstream ss;
ss << "overlay-" << display << "-" << itype << "-scale-" << scale;
#ifdef VISP_HAVE_OPENCV
@@ -232,18 +231,18 @@ int main(int argc, const char *argv[])
std::string env_ipath;
std::string ipath;
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "-c")
opt_click = false;
else if (std::string(argv[i]) == "-d")
opt_display = false;
else if (std::string(argv[i]) == "-i")
- opt_ipath = std::string(argv[i+1]);
+ 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 <image path>] [-c] [-d] [--help]\n" << std::endl;
std::cout << "\nOptions: " << std::endl;
std::cout << " -i <input image path> : set image input path.\n"
- << " From this path read \"ViSP-images/Klimt/Klimt.pgm\" image.\n"
+ << " From this path read \"Klimt/Klimt.pgm\" image.\n"
<< " Setting the VISP_INPUT_IMAGE_PATH environment\n"
<< " variable produces the same behaviour than using\n"
<< " this option." << std::endl;
@@ -254,11 +253,12 @@ int main(int argc, const char *argv[])
}
}
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Get the option values
@@ -290,20 +290,22 @@ int main(int argc, const char *argv[])
return 0;
}
vpImage<unsigned char> I;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
vpImageIo::read(I, filename);
vpImage<vpRGBa> C;
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
vpImageIo::read(C, filename);
int nbfailure = 0;
- for(unsigned int i=0; i<display.size(); i++) {
+ for (unsigned int i = 0; i < display.size(); i++) {
- for(unsigned int scale=1; scale<4; scale++) {
- if (! test(display[i], I, scale, opt_click) ) nbfailure ++;
- if (! test(display[i], C, scale, opt_click) ) nbfailure ++;
+ for (unsigned int scale = 1; scale < 4; scale++) {
+ if (!test(display[i], I, scale, opt_click))
+ nbfailure++;
+ if (!test(display[i], C, scale, opt_click))
+ nbfailure++;
}
}
if (nbfailure == 0)
diff --git a/modules/gui/test/display/testDisplays.cpp b/modules/gui/test/display/testDisplays.cpp
index 1198362..a2c882c 100644
--- a/modules/gui/test/display/testDisplays.cpp
+++ b/modules/gui/test/display/testDisplays.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,23 +39,24 @@
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
#include <iostream>
+#include <stdlib.h>
#include <string>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \
+ defined(VISP_HAVE_OPENCV))
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpRect.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
/*!
\example testDisplays.cpp
@@ -64,7 +66,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "hldc"
+#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);
@@ -121,17 +123,28 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'l': list = true; break;
- case 'h': usage(argv[0], NULL); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
+ case 'l':
+ list = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
default:
- usage(argv[0], optarg_); return false; break;
+ usage(argv[0], optarg_);
+ return false;
+ break;
}
}
@@ -146,103 +159,102 @@ bool getOptions(int argc, const char **argv, bool &list, bool &click_allowed, bo
return true;
}
-
void draw(vpImage<vpRGBa> &I)
{
vpImagePoint iP1, iP2;
- unsigned int w,h;
+ unsigned int w, h;
iP1.set_i(20);
iP1.set_j(10);
iP2.set_i(20);
iP2.set_j(30);
- vpDisplay::displayArrow (I, iP1, iP2, vpColor::green, 4, 2, 3);
+ vpDisplay::displayArrow(I, iP1, iP2, vpColor::green, 4, 2, 3);
iP1.set_i(20);
iP1.set_j(60);
- vpDisplay::displayText (I, iP1, "Test...", vpColor::black);
+ vpDisplay::displayText(I, iP1, "Test...", vpColor::black);
iP1.set_i(80);
iP1.set_j(220);
iP2.set_i(80);
iP2.set_j(480);
- vpDisplay::displayCircle (I, iP1, 30, vpColor::red, false, 3);
- vpDisplay::displayCircle (I, iP2, 30, vpColor::red, true, 3);
+ vpDisplay::displayCircle(I, iP1, 30, vpColor::red, false, 3);
+ vpDisplay::displayCircle(I, iP2, 30, vpColor::red, true, 3);
iP1.set_i(20);
iP1.set_j(220);
- vpDisplay::displayCross (I, iP1, 5, vpColor::blue, 1);
+ vpDisplay::displayCross(I, iP1, 5, vpColor::blue, 1);
iP1.set_i(140);
iP1.set_j(10);
iP2.set_i(140);
iP2.set_j(50);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+ vpDisplay::displayDotLine(I, iP1, iP2, vpColor::blue, 3);
iP1.set_i(120);
iP1.set_j(180);
iP2.set_i(160);
iP2.set_j(250);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+ vpDisplay::displayDotLine(I, iP1, iP2, vpColor::blue, 3);
iP1.set_i(160);
iP1.set_j(280);
iP2.set_i(120);
iP2.set_j(340);
- vpDisplay::displayDotLine (I, iP1, iP2, vpColor::blue, 3);
+ vpDisplay::displayDotLine(I, iP1, iP2, vpColor::blue, 3);
- //static void displayFrame (const vpImage< vpRGBa > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, vpColor color)
+ // static void displayFrame (const vpImage< vpRGBa > &I, const
+ // vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size,
+ // vpColor color)
iP1.set_i(140);
iP1.set_j(80);
iP2.set_i(140);
iP2.set_j(150);
- vpDisplay::displayLine (I, iP1, iP2, vpColor::orange, 3);
+ vpDisplay::displayLine(I, iP1, iP2, vpColor::orange, 3);
iP1.set_i(140);
iP1.set_j(400);
- vpDisplay::displayPoint (I, iP1, vpColor::red);
+ vpDisplay::displayPoint(I, iP1, vpColor::red);
iP1.set_i(350);
iP1.set_j(20);
w = 60;
h = 50;
- vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, false, 3);
+ vpDisplay::displayRectangle(I, iP1, w, h, vpColor::red, false, 3);
iP1.set_i(350);
iP1.set_j(110);
- vpDisplay::displayRectangle (I, iP1, w, h, vpColor::red, true, 3);
+ vpDisplay::displayRectangle(I, iP1, w, h, vpColor::red, true, 3);
iP1.set_i(350);
iP1.set_j(200);
iP2.set_i(400);
iP2.set_j(260);
- vpDisplay::displayRectangle (I, iP1, iP2, vpColor::orange, false, 3);
+ vpDisplay::displayRectangle(I, iP1, iP2, vpColor::orange, false, 3);
iP1.set_i(350);
iP1.set_j(290);
iP2.set_i(400);
iP2.set_j(350);
vpRect rectangle(iP1, iP2);
- vpDisplay::displayRectangle (I, rectangle, vpColor::yellow, false, 3);
+ vpDisplay::displayRectangle(I, rectangle, vpColor::yellow, false, 3);
iP1.set_i(380);
iP1.set_j(400);
- vpDisplay::displayRectangle (I, iP1, 45, w, h, vpColor::green, 3);
+ vpDisplay::displayRectangle(I, iP1, 45, w, h, vpColor::green, 3);
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- try{
+ try {
bool opt_list = false; // To print the list of video devices
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
- if (getOptions(argc, argv, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_list, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Print the list of video-devices available
@@ -251,24 +263,24 @@ main(int argc, const char ** argv)
std::cout << "List of video-devices available: \n";
#if defined VISP_HAVE_GTK
std::cout << " GTK\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_X11
std::cout << " X11\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_GDI
std::cout << " GDI\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_D3D9
std::cout << " D3D\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_OPENCV
std::cout << " OpenCV\n";
- nbDevices ++;
-#endif
+ nbDevices++;
+#endif
if (!nbDevices) {
std::cout << " No display is available\n";
}
@@ -291,10 +303,9 @@ main(int argc, const char ** argv)
vpDisplayX *displayX = NULL;
displayX = new vpDisplayX;
Ix.init(480, 640, 255);
- if (opt_display)
- {
- displayX->init(Ix, 100, 100,"Display X11") ;
- vpDisplay::display(Ix) ;
+ if (opt_display) {
+ displayX->init(Ix, 100, 100, "Display X11");
+ vpDisplay::display(Ix);
draw(Ix);
vpDisplay::flush(Ix);
if (opt_click_allowed)
@@ -306,10 +317,9 @@ main(int argc, const char ** argv)
vpDisplayOpenCV *displayCv = NULL;
displayCv = new vpDisplayOpenCV;
Icv.init(480, 640, 255);
- if (opt_display)
- {
- displayCv->init(Icv, 100, 100,"Display OpenCV") ;
- vpDisplay::display(Icv) ;
+ if (opt_display) {
+ displayCv->init(Icv, 100, 100, "Display OpenCV");
+ vpDisplay::display(Icv);
draw(Icv);
vpDisplay::flush(Icv);
if (opt_click_allowed)
@@ -321,10 +331,9 @@ main(int argc, const char ** argv)
vpDisplayGTK *displayGtk = NULL;
displayGtk = new vpDisplayGTK;
Igtk.init(480, 640, 255);
- if (opt_display)
- {
- displayGtk->init(Igtk, 100, 100,"Display GTK") ;
- vpDisplay::display(Igtk) ;
+ if (opt_display) {
+ displayGtk->init(Igtk, 100, 100, "Display GTK");
+ vpDisplay::display(Igtk);
draw(Igtk);
vpDisplay::flush(Igtk);
if (opt_click_allowed)
@@ -336,10 +345,9 @@ main(int argc, const char ** argv)
vpDisplayGDI *displayGdi = NULL;
displayGdi = new vpDisplayGDI;
Igdi.init(480, 640, 255);
- if (opt_display)
- {
- displayGdi->init(Igdi, 100, 100,"Display GDI") ;
- vpDisplay::display(Igdi) ;
+ if (opt_display) {
+ displayGdi->init(Igdi, 100, 100, "Display GDI");
+ vpDisplay::display(Igdi);
draw(Igdi);
vpDisplay::flush(Igdi);
if (opt_click_allowed)
@@ -351,10 +359,9 @@ main(int argc, const char ** argv)
vpDisplayD3D *displayD3d = NULL;
displayD3d = new vpDisplayD3D;
Id3d.init(480, 640, 255);
- if (opt_display)
- {
- displayD3d->init(Id3d, 100, 100,"Display Direct 3D") ;
- vpDisplay::display(Id3d) ;
+ if (opt_display) {
+ displayD3d->init(Id3d, 100, 100, "Display Direct 3D");
+ vpDisplay::display(Id3d);
draw(Id3d);
vpDisplay::flush(Id3d);
if (opt_click_allowed)
@@ -381,18 +388,13 @@ main(int argc, const char ** argv)
#if defined VISP_HAVE_D3D9
delete displayD3d;
#endif
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
- return(-1);
+ return (-1);
}
}
#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 eb2d500..7e78b34 100644
--- a/modules/gui/test/display/testMouseEvent.cpp
+++ b/modules/gui/test/display/testMouseEvent.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,26 +46,26 @@
*/
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
-#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 <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/core/vpMouseButton.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
-#include <visp3/core/vpMouseButton.h>
#include <visp3/core/vpTime.h>
@@ -79,7 +80,7 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:lp:ht:f:n:s:w"
+#define GETOPTARGS "cdi:lp:ht:f:n:s:w"
typedef enum {
vpX11,
vpGTK,
@@ -87,11 +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);
/*!
@@ -107,8 +107,8 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
\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\
@@ -120,20 +120,28 @@ SYNOPSIS\n\
[-f <first image>] [-n <number of images>] [-s <step>] \n\
[-t <type of video device>] [-l] [-w] [-c] [-d] [-h]\n \
", name);
-
+
std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
+ switch (dtype) {
+ case vpX11:
+ display = "X11";
+ break;
+ case vpGTK:
+ display = "GTK";
+ break;
+ case vpGDI:
+ display = "GDI";
+ break;
+ case vpD3D:
+ display = "D3D";
+ break;
}
- fprintf(stdout, "\n\
+ fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <test image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/cube/image.%%04d.pgm\"\n\
+ From this path read \"cube/image.%%04d.pgm\"\n\
images. These images come from ViSP-images-x.y.z.tar.gz\n\
available on the ViSP website.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
@@ -184,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);
@@ -214,47 +221,65 @@ 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;
- std::string sDisplayType;
+ int c;
+ std::string sDisplayType;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click = false; break;
- case 'd': display = false; break;
- case 't': sDisplayType = optarg_;
+ case 'c':
+ click = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 't':
+ sDisplayType = optarg_;
// Parse the display type option
- if (sDisplayType.compare("X11") == 0) {
- dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
- dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
- dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
- dtype = vpD3D;
- }
+ if (sDisplayType.compare("X11") == 0) {
+ dtype = vpX11;
+ } else if (sDisplayType.compare("GTK") == 0) {
+ dtype = vpGTK;
+ } else if (sDisplayType.compare("GDI") == 0) {
+ dtype = vpGDI;
+ } else if (sDisplayType.compare("D3D") == 0) {
+ dtype = vpD3D;
+ }
- break;
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 'p': ppath = optarg_; break;
- case 'f': first = (unsigned) atoi(optarg_); break;
- case 'n': nimages = (unsigned) atoi(optarg_); break;
- case 's': step = (unsigned) atoi(optarg_); break;
- case 'w': wait = true; break;
- case 'h': usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
- return false; break;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'l':
+ list = true;
+ break;
+ case 'p':
+ ppath = optarg_;
+ break;
+ case 'f':
+ first = (unsigned)atoi(optarg_);
+ break;
+ case 'n':
+ nimages = (unsigned)atoi(optarg_);
+ break;
+ case 's':
+ step = (unsigned)atoi(optarg_);
+ break;
+ case 'w':
+ wait = true;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, ppath, first, nimages, step, dtype);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath, ppath, first, nimages, step, dtype);
- return false; break;
+ return false;
+ break;
}
}
@@ -269,8 +294,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath, std::string &pp
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
std::string env_ipath;
std::string opt_ipath;
@@ -282,12 +306,12 @@ main(int argc, const char ** argv)
unsigned opt_nimages = 10;
unsigned opt_step = 1;
vpDisplayType opt_dtype; // Type of display to use
- bool opt_list = false; // To print the list of video devices
+ bool opt_list = false; // To print the list of video devices
bool opt_display = true;
bool opt_click = true;
bool opt_click_blocking = false;
- // Default display is one available
+// Default display is one available
#if defined VISP_HAVE_GTK
opt_dtype = vpGTK;
#elif defined VISP_HAVE_X11
@@ -298,18 +322,18 @@ main(int argc, const char ** argv)
opt_dtype = vpD3D;
#endif
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_ppath,opt_first, opt_nimages,
- opt_step, opt_dtype, opt_list, opt_display, opt_click,
- opt_click_blocking) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_dtype, opt_list, opt_display,
+ opt_click, opt_click_blocking) == false) {
+ exit(-1);
}
// Print the list of video-devices available
if (opt_list) {
@@ -317,19 +341,19 @@ main(int argc, const char ** argv)
std::cout << "List of video-devices available: \n";
#if defined VISP_HAVE_GTK
std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_X11
std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_GDI
std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_D3D9
std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
if (!nbDevices) {
std::cout << " No display is available\n";
@@ -337,7 +361,7 @@ main(int argc, const char ** argv)
return (0);
}
- if ( ! opt_display )
+ if (!opt_display)
opt_click_blocking = false; // turn off the waiting
// Get the option values
@@ -348,8 +372,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty() && opt_ppath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -357,15 +380,13 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty() ){
- usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step,opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ if (opt_ipath.empty() && env_ipath.empty() && opt_ppath.empty()) {
+ usage(argv[0], NULL, ipath, opt_ppath, opt_first, opt_nimages, opt_step, opt_dtype);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
<< " image path where test images are located." << std::endl
- << " Use -p <personal image path> option if you want to "<<std::endl
+ << " Use -p <personal image path> option if you want to " << std::endl
<< " use personal images." << std::endl
<< std::endl;
@@ -375,14 +396,13 @@ main(int argc, const char ** argv)
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
unsigned iter = opt_first;
std::ostringstream s;
char cfilename[FILENAME_MAX];
- if (opt_ppath.empty()){
-
+ if (opt_ppath.empty()) {
// Warning :
// the image sequence is not provided with the ViSP package
@@ -393,21 +413,20 @@ main(int argc, const char ** argv)
// terminate called after throwing an instance of 'vpImageException'
//
// The sequence is available on the visp www site
- // http://www.irisa.fr/lagadic/visp/visp.html
+ // https://visp.inria.fr/download/
// in the download section. It is named "ViSP-images.tar.gz"
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
s.setf(std::ios::right, std::ios::adjustfield);
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
+ } else {
- sprintf(cfilename,opt_ppath.c_str(), iter) ;
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
// Read the PGM image named "filename" on the disk, and put the
@@ -416,36 +435,31 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
+ try {
+ vpImageIo::read(I, filename);
+ } catch (...) {
// an exception is throwned if an exception from readPGM has been catched
// here this will result in the end of the program
// Note that another error message has been printed from readPGM
// to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option, " << std::endl
- << " or your -p " << opt_ppath << " option " <<std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable"
- << std::endl;
+ << " or your -p " << opt_ppath << " option " << std::endl
+ << " or VISP_INPUT_IMAGE_PATH environment variable" << std::endl;
exit(-1);
}
// Create a display for the image
vpDisplay *display = NULL;
- switch(opt_dtype) {
+ switch (opt_dtype) {
case vpX11:
std::cout << "Requested X11 display functionnalities..." << std::endl;
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#else
std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
+ std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n";
return 0;
#endif
break;
@@ -455,8 +469,7 @@ 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;
@@ -466,8 +479,7 @@ 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;
@@ -477,8 +489,7 @@ 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;
@@ -488,39 +499,37 @@ main(int argc, const char ** argv)
try {
// We open a window using either X11 or GTK or GDI.
// Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
+ display->init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
delete display;
exit(-1);
}
}
// double tms_1 = vpTime::measureTimeMs() ;
- unsigned niter=0 ;
+ unsigned niter = 0;
// this is the loop over the image sequence
- while (iter < opt_first + opt_nimages*opt_step) {
+ while (iter < opt_first + opt_nimages * opt_step) {
try {
- double tms = vpTime::measureTimeMs() ;
+ double tms = vpTime::measureTimeMs();
// set the new image name
- if (opt_ppath.empty()){
+ if (opt_ppath.empty()) {
s.str("");
s << "image." << std::setw(4) << std::setfill('0') << iter << ".pgm";
filename = vpIoTools::createFilePath(dirname, s.str());
- }
- else {
- sprintf(cfilename, opt_ppath.c_str(), iter) ;
+ } else {
+ sprintf(cfilename, opt_ppath.c_str(), iter);
filename = cfilename;
}
@@ -529,17 +538,17 @@ main(int argc, const char ** argv)
vpImageIo::read(I, filename);
if (opt_display) {
// Display the image
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
if (opt_click_blocking) {
std::cout << "A click in the image to continue..." << std::endl;
}
vpImagePoint ip;
- vpMouseButton::vpMouseButtonType button;
if (opt_click) {
+ vpMouseButton::vpMouseButtonType button;
bool pressed = vpDisplay::getClick(I, ip, button, opt_click_blocking);
if (pressed) {
switch (button) {
@@ -552,38 +561,34 @@ main(int argc, const char ** argv)
case vpMouseButton::button3:
std::cout << "Right button was pressed. Bye. " << std::endl;
delete display;
- return 0; break;
- case vpMouseButton::none: break;
+ return 0;
+ break;
+ case vpMouseButton::none:
+ break;
}
}
}
vpTime::wait(tms, 1000);
}
-
+
else {
// Synchronise the loop to 40 ms
- vpTime::wait(tms, 40) ;
+ vpTime::wait(tms, 40);
}
- niter++ ;
- }
- catch(...) {
+ niter++;
+ } catch (...) {
delete display;
- exit(-1) ;
+ exit(-1);
}
- iter += opt_step ;
+ iter += opt_step;
}
delete display;
// double tms_2 = vpTime::measureTimeMs() ;
// double tms_total = tms_2 - tms_1 ;
// std::cout << "Total Time : "<< tms_total<<std::endl;
-
}
#else
-int
-main()
-{
- vpERROR_TRACE("You do not have X11 or GTK display functionalities...");
-}
+int main() { vpERROR_TRACE("You do not have X11 or GTK display functionalities..."); }
#endif
diff --git a/modules/gui/test/display/testVideoDevice.cpp b/modules/gui/test/display/testVideoDevice.cpp
index d6bc919..8bcff51 100644
--- a/modules/gui/test/display/testVideoDevice.cpp
+++ b/modules/gui/test/display/testVideoDevice.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,25 +36,25 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
-#include <stdlib.h>
#include <iostream>
+#include <stdlib.h>
#include <string>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \
+ defined(VISP_HAVE_OPENCV))
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
/*!
\example testVideoDevice.cpp
@@ -63,20 +64,13 @@
*/
// List of allowed command line options
-#define GETOPTARGS "i:hlt:dc"
+#define GETOPTARGS "i:hlt:dc"
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
+typedef enum { vpX11, vpGTK, vpGDI, vpD3D, vpCV } vpDisplayType;
void usage(const char *name, const char *badparam, std::string ipath, vpDisplayType &dtype);
-bool getOptions(int argc, const char **argv,
- std::string &ipath, vpDisplayType &dtype, bool &list,
- bool &click_allowed, bool &display );
+bool getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed,
+ bool &display);
/*!
@@ -99,20 +93,30 @@ SYNOPSIS\n\
", name);
std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
+ switch (dtype) {
+ case vpX11:
+ display = "X11";
+ break;
+ case vpGTK:
+ display = "GTK";
+ break;
+ case vpGDI:
+ display = "GDI";
+ break;
+ case vpD3D:
+ display = "D3D";
+ break;
+ case vpCV:
+ display = "CV";
+ break;
}
fprintf(stdout, "\n\
OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
- From this path read \"ViSP-images/Klimt/Klimt.pgm\"\n\
- and \"ViSP-images/Klimt/Klimt.ppm\" images.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ and \"Klimt/Klimt.ppm\" images.\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -137,8 +141,7 @@ OPTIONS: Default\n\
Print the list of video-devices available and exit.\n\
\n\
-h\n\
- Print the help.\n\n",
- ipath.c_str(), display.c_str());
+ Print the help.\n\n", ipath.c_str(), display.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -158,47 +161,55 @@ 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 &display )
+bool getOptions(int argc, const char **argv, std::string &ipath, vpDisplayType &dtype, bool &list, bool &click_allowed,
+ bool &display)
{
const char *optarg_;
- int c;
+ int c;
std::string sDisplayType;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'i': ipath = optarg_; break;
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'l':
+ list = true;
+ break;
+ case 't':
+ sDisplayType = optarg_;
// Parse the display type option
if (sDisplayType.compare("X11") == 0) {
dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
+ } else if (sDisplayType.compare("GTK") == 0) {
dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
+ } else if (sDisplayType.compare("GDI") == 0) {
dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
+ } else if (sDisplayType.compare("D3D") == 0) {
dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
+ } else if (sDisplayType.compare("CV") == 0) {
dtype = vpCV;
}
break;
- case 'h': usage(argv[0], NULL, ipath,dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
+ case 'h':
+ usage(argv[0], NULL, ipath, dtype);
+ return false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
default:
- usage(argv[0], optarg_, ipath,dtype); return false; break;
+ usage(argv[0], optarg_, ipath, dtype);
+ return false;
+ break;
}
}
-
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], NULL, ipath, dtype);
@@ -210,20 +221,19 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
- try{
+ try {
std::string env_ipath;
std::string opt_ipath;
- bool opt_list = false; // To print the list of video devices
+ bool opt_list = false; // To print the list of video devices
vpDisplayType opt_dtype; // Type of display to use
std::string ipath;
std::string filename;
bool opt_click_allowed = true;
bool opt_display = true;
- // Default display is one available
+// Default display is one available
#if defined VISP_HAVE_GTK
opt_dtype = vpGTK;
#elif defined VISP_HAVE_X11
@@ -233,20 +243,20 @@ main(int argc, const char ** argv)
#elif defined VISP_HAVE_D3D9
opt_dtype = vpD3D;
#elif defined VISP_HAVE_OPENCV
- opt_dtype = vpCV;
+ opt_dtype = vpCV;
#endif
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Print the list of video-devices available
@@ -255,24 +265,24 @@ main(int argc, const char ** argv)
std::cout << "List of video-devices available: \n";
#if defined VISP_HAVE_GTK
std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_X11
std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_GDI
std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_D3D9
std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_OPENCV
std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
+ nbDevices++;
+#endif
if (!nbDevices) {
std::cout << " No display is available\n";
}
@@ -287,8 +297,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -296,45 +305,42 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath, opt_dtype);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
// Create a grey level image
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Create a color image
- vpImage<vpRGBa> Irgba ;
+ vpImage<vpRGBa> Irgba;
// Load a grey image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.pgm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(I, filename) ;
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(I, filename);
// Load a color image from the disk
- filename = vpIoTools::createFilePath(ipath, "ViSP-images/Klimt/Klimt.ppm");
- vpCTRACE << "Load " << filename << std::endl;
- vpImageIo::read(Irgba, filename) ;
-
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpCTRACE << "Load " << filename << std::endl;
+ vpImageIo::read(Irgba, filename);
// Create a display for the image
vpDisplay *display = NULL;
- switch(opt_dtype) {
+ switch (opt_dtype) {
case vpX11:
std::cout << "Requested X11 display functionnalities..." << std::endl;
#if defined VISP_HAVE_X11
display = new vpDisplayX;
#else
std::cout << " Sorry, X11 video device is not available.\n";
- std::cout << "Use \"" << argv[0]
- << " -l\" to print the list of available devices.\n";
+ std::cout << "Use \"" << argv[0] << " -l\" to print the list of available devices.\n";
return 0;
#endif
break;
@@ -344,8 +350,7 @@ 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;
@@ -355,8 +360,7 @@ 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;
@@ -366,8 +370,7 @@ 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;
@@ -377,29 +380,28 @@ main(int argc, const char ** argv)
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;
}
- if (opt_display){
+ if (opt_display) {
// We open a window using either X11 or GTK or GDI or D3D.
// Its size is automatically defined by the image (I) size
- display->init(I, 100, 100,"Display...") ;
+ display->init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(I) ;
+ if (opt_click_allowed)
+ vpDisplay::getClick(I);
display->close(I);
@@ -414,32 +416,32 @@ main(int argc, const char ** argv)
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
- std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
+ std::cout << "A click to continue...\n";
+ if (opt_click_allowed)
+ vpDisplay::getClick(I);
display->close(I);
// We open a window using either X11 or GTK or GDI or D3D.
// Its size is automatically defined by the image (I) size
- display->init(Irgba, 100, 100,"Color display...");
+ display->init(Irgba, 100, 100, "Color display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(Irgba) ;
- //Flush the display
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ // Flush the display
+ vpDisplay::flush(Irgba);
std::cout << "A click to continue...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(Irgba) ;
+ if (opt_click_allowed)
+ vpDisplay::getClick(Irgba);
display->close(Irgba);
@@ -454,28 +456,22 @@ main(int argc, const char ** argv)
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(Irgba) ;
- //Flush the display
- vpDisplay::flush(Irgba) ;
+ vpDisplay::display(Irgba);
+ // Flush the display
+ vpDisplay::flush(Irgba);
std::cout << "A click to exit...\n";
- if ( opt_click_allowed )
- vpDisplay::getClick(Irgba) ;
+ if (opt_click_allowed)
+ vpDisplay::getClick(Irgba);
}
delete display;
- }
- catch(...) {
- vpERROR_TRACE("Error while displaying the image") ;
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
+int main() { vpERROR_TRACE("You do not have display functionalities..."); }
#endif
-
diff --git a/modules/gui/test/display/testVideoDeviceDual.cpp b/modules/gui/test/display/testVideoDeviceDual.cpp
index 3563604..c22f792 100644
--- a/modules/gui/test/display/testVideoDeviceDual.cpp
+++ b/modules/gui/test/display/testVideoDeviceDual.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,19 +36,19 @@
*
*****************************************************************************/
-
+#include <iostream>
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/io/vpParseArgv.h>
-#include <stdlib.h>
-#include <iostream>
-#if (defined (VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_GTK) || defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_D3D9) || \
+ defined(VISP_HAVE_OPENCV))
/*!
\example testVideoDeviceDual.cpp
@@ -56,17 +57,10 @@
*/
-
// List of allowed command line options
-#define GETOPTARGS "hlt:dc"
+#define GETOPTARGS "hlt:dc"
-typedef enum {
- vpX11,
- vpGTK,
- vpGDI,
- vpD3D,
- vpCV
-} vpDisplayType;
+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);
@@ -90,12 +84,22 @@ SYNOPSIS\n\
", name);
std::string display;
- switch(dtype) {
- case vpX11: display = "X11"; break;
- case vpGTK: display = "GTK"; break;
- case vpGDI: display = "GDI"; break;
- case vpD3D: display = "D3D"; break;
- case vpCV: display = "CV"; break;
+ switch (dtype) {
+ case vpX11:
+ display = "X11";
+ break;
+ case vpGTK:
+ display = "GTK";
+ break;
+ case vpGDI:
+ display = "GDI";
+ break;
+ case vpD3D:
+ display = "D3D";
+ break;
+ case vpCV:
+ display = "CV";
+ break;
}
fprintf(stdout, "\n\
@@ -120,8 +124,7 @@ OPTIONS: Default\n\
Print the list of video-devices available and exit.\n\
\n\
-h\n\
- Print the help.\n\n",
- display.c_str());
+ Print the help.\n\n", display.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -140,42 +143,48 @@ 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;
+ int c;
std::string sDisplayType;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'l': list = true; break;
- case 't': sDisplayType = optarg_;
+ case 'l':
+ list = true;
+ break;
+ case 't':
+ sDisplayType = optarg_;
// Parse the display type option
if (sDisplayType.compare("X11") == 0) {
dtype = vpX11;
- }
- else if (sDisplayType.compare("GTK") == 0) {
+ } else if (sDisplayType.compare("GTK") == 0) {
dtype = vpGTK;
- }
- else if (sDisplayType.compare("GDI") == 0) {
+ } else if (sDisplayType.compare("GDI") == 0) {
dtype = vpGDI;
- }
- else if (sDisplayType.compare("D3D") == 0) {
+ } else if (sDisplayType.compare("D3D") == 0) {
dtype = vpD3D;
- }
- else if (sDisplayType.compare("CV") == 0) {
+ } else if (sDisplayType.compare("CV") == 0) {
dtype = vpCV;
}
break;
- case 'h': usage(argv[0], NULL, dtype); return false; break;
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
+ case 'h':
+ usage(argv[0], NULL, dtype);
+ return false;
+ break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
default:
- usage(argv[0], optarg_, dtype); return false; break;
+ usage(argv[0], optarg_, dtype);
+ return false;
+ break;
}
}
@@ -190,15 +199,15 @@ bool getOptions(int argc, const char **argv,
return true;
}
-int main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
- bool opt_list = false; // To print the list of video devices
+ bool opt_list = false; // To print the list of video devices
vpDisplayType opt_dtype; // Type of display to use
bool opt_click_allowed = true;
bool opt_display = true;
- // Default display is one available
+// Default display is one available
#if defined VISP_HAVE_GTK
opt_dtype = vpGTK;
#elif defined VISP_HAVE_X11
@@ -212,9 +221,8 @@ 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) {
- exit (-1);
+ if (getOptions(argc, argv, opt_dtype, opt_list, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Print the list of video-devices available
@@ -223,24 +231,24 @@ int main(int argc, const char ** argv)
std::cout << "List of video-devices available: \n";
#if defined VISP_HAVE_GTK
std::cout << " GTK (use \"-t GTK\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_X11
std::cout << " X11 (use \"-t X11\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_GDI
std::cout << " GDI (use \"-t GDI\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_D3D9
std::cout << " D3D (use \"-t D3D\" option to use it)\n";
- nbDevices ++;
+ nbDevices++;
#endif
#if defined VISP_HAVE_OPENCV
std::cout << " CV (use \"-t CV\" option to use it)\n";
- nbDevices ++;
-#endif
+ nbDevices++;
+#endif
if (!nbDevices) {
std::cout << " No display is available\n";
}
@@ -256,7 +264,7 @@ int main(int argc, const char ** argv)
vpDisplay *d1 = NULL, *d2 = NULL;
// Initialize the displays
- switch(opt_dtype) {
+ switch (opt_dtype) {
case vpX11:
std::cout << "Requested X11 display functionnalities..." << std::endl;
#if defined VISP_HAVE_X11
@@ -264,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;
@@ -276,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;
@@ -288,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;
@@ -300,8 +305,7 @@ 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;
@@ -312,18 +316,17 @@ int main(int argc, const char ** argv)
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;
}
- if (opt_display){
+ if (opt_display) {
int winx1 = 100, winy1 = 200;
d1->init(I1, winx1, winy1, "Display 1");
- int winx2 = winx1+10+(int)I1.getWidth(), winy2 = winy1;
+ int winx2 = winx1 + 10 + (int)I1.getWidth(), winy2 = winy1;
d2->init(I2, winx2, winy2, "Display 2");
vpDisplay::display(I1);
@@ -334,23 +337,18 @@ int main(int argc, const char ** argv)
}
std::cout << "A click in display 1 to exit..." << std::endl;
- if ( opt_click_allowed )
+ if (opt_click_allowed)
vpDisplay::getClick(I1);
delete d1;
delete d2;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
#else
-int
-main()
-{
- vpERROR_TRACE("You do not have display functionalities...");
-}
+int main() { vpERROR_TRACE("You do not have display functionalities..."); }
#endif
diff --git a/modules/imgproc/CMakeLists.txt b/modules/imgproc/CMakeLists.txt
new file mode 100644
index 0000000..c95bf84
--- /dev/null
+++ b/modules/imgproc/CMakeLists.txt
@@ -0,0 +1,56 @@
+#############################################################################
+#
+# This file is part of the ViSP software.
+# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See http://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp at inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# ViSP configuration file.
+#
+# Authors:
+# Fabien Spindler
+#
+#############################################################################
+
+vp_add_module(imgproc visp_core)
+
+# You can add cmake specific material like Find<Package>.cmake files in a specific
+# folder named for example "cmake" and uncomment the following line to use it.
+# vp_add_cmake_module_path(cmake)
+
+# You can add specific headers that are dynamically configured in
+# the include folder of the module. Create for example the file
+# cmake/templates/vpConfigImproc.h.in and uncomment the following line.
+# vp_add_config_file(cmake/templates/vpConfigImproc.h.in)
+
+vp_glob_module_sources()
+vp_module_include_directories()
+vp_create_module()
+
+vp_add_tests(DEPENDS_ON visp_imgproc visp_io)
+
+vp_set_source_file_compile_flag(src/vpCLAHE.cpp -Wno-strict-overflow)
diff --git a/modules/imgproc/README.md b/modules/imgproc/README.md
new file mode 100644
index 0000000..26d5548
--- /dev/null
+++ b/modules/imgproc/README.md
@@ -0,0 +1,9 @@
+This module contains basic functions for image processing:
+- Brightness and contrast adjustment
+- Connected components computation
+- Contours extraction
+- Gamma correction
+- Histogram equalization
+- Contrast stretching
+- Image sharpening
+- Retinex
diff --git a/modules/imgproc/doc/module.doc b/modules/imgproc/doc/module.doc
new file mode 100644
index 0000000..5bfee3b
--- /dev/null
+++ b/modules/imgproc/doc/module.doc
@@ -0,0 +1,69 @@
+/*******************************************
+ * Module imgproc
+ *******************************************/
+/*!
+ \defgroup module_imgproc imgproc: Image processing module
+ Image processing module content.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_brightness Brightness and contrast adjustment
+ Brightness and contrast adjustment.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_gamma Gamma correction
+ Gamma correction.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_histogram Histogram equalization
+ Histogram equalization.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_contrast Contrast stretching
+ Contrast stretching.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_sharpening Image sharpening
+ Image sharpening.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_retinex Retinex
+ Enhance the contrast of a color image using the Retinex technique, based on the Retinex ImageJ plugin:
+ <blockquote>
+ Retinex filtering is based on Land's theory of image perception, proposed to explain the perceived colour constancy
+ of objects under varying illumination conditions. Several approaches exist to implement the retinex principles,
+ among these the multiscale retinex with colour restoration algorithm (MSRCR) combines colour constancy
+ with local contrast enhancement so images are rendered similarly to how human vision is believed to operate.
+ </blockquote>
+ This method is based on the <a href="http://imagej.net/Retinex">Retinex ImageJ</a> plugin written by Francisco Jiménez Hernández,
+ which is a modified implementation of the Retinex filter from the <a href="https://docs.gimp.org/en/plug-in-retinex.html">GIMP package</a>
+ by Fabien Pelisson.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_connected_components Connected components
+ Connected components.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_contours Contours extraction
+ Contours extraction.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_morph Additional image morphology functions
+ Additional image morphology functions.
+*/
+/*!
+ \ingroup module_imgproc
+ \defgroup group_imgproc_threshold Automatic thresholding
+ Automatic thresholding using various well-known methods.
+
+ \image html img-auto-threshold-grid36-03.png Input image.
+ \image html img-auto-threshold-grid36-03-otsu.png Image automatically thresholded with the Otsu method.
+*/
diff --git a/modules/imgproc/include/visp3/imgproc/vpContours.h b/modules/imgproc/include/visp3/imgproc/vpContours.h
new file mode 100644
index 0000000..b448c03
--- /dev/null
+++ b/modules/imgproc/include/visp3/imgproc/vpContours.h
@@ -0,0 +1,254 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Basic contours extraction based on the orignal work of
+ * Sina Samangooei (ss at ecs.soton.ac.uk).
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/**
+ * Copyright (c) 2011, The University of Southampton and the individual
+ * contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the University of Southampton nor the names of its
+ * contributors may be used to endorse or promote products derived from this
+ * software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*!
+ \file vpContours.h
+ \brief Basic contours extraction.
+*/
+
+#ifndef __vpContours_h__
+#define __vpContours_h__
+
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpPolygon.h>
+
+namespace
+{
+typedef enum {
+ NORTH,
+ NORTH_EAST,
+ EAST,
+ SOUTH_EAST,
+ SOUTH,
+ SOUTH_WEST,
+ WEST,
+ NORTH_WEST,
+ LAST_DIRECTION
+} vpDirectionType;
+
+struct vpDirection {
+ vpDirectionType m_direction;
+
+ int m_dirx[8];
+ int m_diry[8];
+
+ vpDirection()
+ {
+ m_dirx[0] = 0;
+ m_dirx[1] = 1;
+ m_dirx[2] = 1;
+ m_dirx[3] = 1;
+ m_dirx[4] = 0;
+ m_dirx[5] = -1;
+ m_dirx[6] = -1;
+ m_dirx[7] = -1;
+
+ m_diry[0] = -1;
+ m_diry[1] = -1;
+ m_diry[2] = 0;
+ m_diry[3] = 1;
+ m_diry[4] = 1;
+ m_diry[5] = 1;
+ m_diry[6] = 0;
+ m_diry[7] = -1;
+ }
+
+ vpDirection clockwise()
+ {
+ vpDirection direction;
+ int directionSize = LAST_DIRECTION;
+ direction.m_direction = vpDirectionType(((int)m_direction + 1) % directionSize);
+
+ return direction;
+ }
+
+ vpDirection counterClockwise()
+ {
+ vpDirection direction;
+ int directionSize = (int)LAST_DIRECTION;
+ int idx = vpMath::modulo((int)m_direction - 1, directionSize);
+ direction.m_direction = vpDirectionType(idx);
+
+ return direction;
+ }
+
+ vpImagePoint active(const vpImage<int> &I, const vpImagePoint &point)
+ {
+ 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()) {
+ return vpImagePoint(-1, -1);
+ }
+
+ int pixel = I[yy][xx];
+ return pixel != 0 ? vpImagePoint(yy, xx) : vpImagePoint(-1, -1);
+ }
+};
+}
+
+namespace vp
+{
+typedef enum {
+ CONTOUR_OUTER, /*!< Outer contour. */
+ CONTOUR_HOLE /*!< Hole contour. */
+} 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_EXTERNAL /*!< Retrieve only external contours. */
+} vpContourRetrievalType;
+
+struct vpContour {
+ std::vector<vpContour *> m_children;
+ vpContourType m_contourType;
+ vpContour *m_parent;
+ std::vector<vpImagePoint> 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 vpContour &contour)
+ : m_children(), m_contourType(contour.m_contourType), m_parent(NULL), m_points(contour.m_points)
+ {
+
+ // Copy the underlying contours
+ for (std::vector<vpContour *>::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);
+ }
+ }
+
+ ~vpContour()
+ {
+ for (std::vector<vpContour *>::iterator it = m_children.begin(); it != m_children.end(); ++it) {
+ (*it)->m_parent = NULL;
+ if (*it != NULL) {
+ delete *it;
+ *it = NULL;
+ }
+ }
+ }
+
+ vpContour &operator=(const vpContour &other)
+ {
+ m_contourType = other.m_contourType;
+
+ if (m_parent == NULL) {
+ // We are a root or an unintialized contour so delete everything
+ for (std::vector<vpContour *>::iterator it = m_children.begin(); it != m_children.end(); ++it) {
+ (*it)->m_parent = NULL;
+ if (*it != NULL) {
+ delete *it;
+ *it = NULL;
+ }
+ }
+ } else {
+ // Make the current contour the root contour
+ // to avoid problem when deleting
+ m_parent = NULL;
+ }
+
+ m_children.clear();
+ for (std::vector<vpContour *>::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);
+ }
+
+ return *this;
+ }
+
+ void setParent(vpContour *parent)
+ {
+ m_parent = parent;
+
+ if (parent != NULL) {
+ parent->m_children.push_back(this);
+ }
+ }
+};
+
+VISP_EXPORT void drawContours(vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &contours,
+ unsigned char grayValue = 255);
+VISP_EXPORT void drawContours(vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &contours,
+ const vpColor &color);
+
+VISP_EXPORT void findContours(const vpImage<unsigned char> &I_original, vpContour &contours,
+ std::vector<std::vector<vpImagePoint> > &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
new file mode 100644
index 0000000..b3b9591
--- /dev/null
+++ b/modules/imgproc/include/visp3/imgproc/vpImgproc.h
@@ -0,0 +1,151 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Static functions for basic image processing functions.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpImgproc.h
+ \brief Basic image processing functions.
+
+*/
+
+#ifndef __vpImgproc_h__
+#define __vpImgproc_h__
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageMorphology.h>
+#include <visp3/imgproc/vpContours.h>
+
+#define USE_OLD_FILL_HOLE 0
+
+namespace vp
+{
+enum RETINEX_LEVEL { RETINEX_UNIFORM = 0, RETINEX_LOW = 1, RETINEX_HIGH = 2 };
+
+typedef enum {
+ AUTO_THRESHOLD_HUANG, /*!< Huang L.-K. and Wang M.-J.J. (1995) "Image
+ Thresholding by Minimizing the Measures of
+ Fuzziness" Pattern Recognition, 28(1): 41-51
+ \cite Huang_imagethresholding */
+ AUTO_THRESHOLD_INTERMODES, /*!< Prewitt, JMS & Mendelsohn, ML (1966), "The
+ analysis of cell images", Annals of the New
+ York Academy of Sciences 128: 1035-1053
+ \cite NYAS:NYAS1035 */
+ AUTO_THRESHOLD_ISODATA, /*!< Ridler, TW & Calvard, S (1978), "Picture
+ thresholding using an iterative selection
+ method", IEEE Transactions on Systems, Man and
+ Cybernetics 8: 630-632 \cite article4310039, */
+ AUTO_THRESHOLD_MEAN, /*!< Glasbey, CA (1993), "An analysis of
+ 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
+ */
+} vpAutoThresholdMethod;
+
+VISP_EXPORT void adjust(vpImage<unsigned char> &I, const double alpha, const double beta);
+VISP_EXPORT void adjust(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const double alpha,
+ const double beta);
+VISP_EXPORT void adjust(vpImage<vpRGBa> &I, const double alpha, const double beta);
+VISP_EXPORT void adjust(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const double alpha, const double beta);
+
+VISP_EXPORT void clahe(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const int blockRadius = 150,
+ const int bins = 256, const float slope = 3.0f, const bool fast = true);
+VISP_EXPORT void clahe(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const int blockRadius = 150,
+ const int bins = 256, const float slope = 3.0f, const bool fast = true);
+
+VISP_EXPORT void equalizeHistogram(vpImage<unsigned char> &I);
+VISP_EXPORT void equalizeHistogram(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2);
+VISP_EXPORT void equalizeHistogram(vpImage<vpRGBa> &I, const bool useHSV = false);
+VISP_EXPORT void equalizeHistogram(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const bool useHSV = false);
+
+VISP_EXPORT void gammaCorrection(vpImage<unsigned char> &I, const double gamma);
+VISP_EXPORT void gammaCorrection(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const double gamma);
+VISP_EXPORT void gammaCorrection(vpImage<vpRGBa> &I, const double gamma);
+VISP_EXPORT void gammaCorrection(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const double gamma);
+
+VISP_EXPORT void retinex(vpImage<vpRGBa> &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<vpRGBa> &I1, vpImage<vpRGBa> &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<unsigned char> &I);
+VISP_EXPORT void stretchContrast(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2);
+VISP_EXPORT void stretchContrast(vpImage<vpRGBa> &I);
+VISP_EXPORT void stretchContrast(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2);
+
+VISP_EXPORT void stretchContrastHSV(vpImage<vpRGBa> &I);
+VISP_EXPORT void stretchContrastHSV(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2);
+
+VISP_EXPORT void unsharpMask(vpImage<unsigned char> &I, const unsigned int size = 7, const double weight = 0.6);
+VISP_EXPORT void unsharpMask(const vpImage<unsigned char> &I, vpImage<unsigned char> &Ires, const unsigned int size = 7,
+ const double weight = 0.6);
+VISP_EXPORT void unsharpMask(vpImage<vpRGBa> &I, const unsigned int size = 7, const double weight = 0.6);
+VISP_EXPORT void unsharpMask(const vpImage<vpRGBa> &I, vpImage<vpRGBa> &Ires, const unsigned int size = 7,
+ const double weight = 0.6);
+
+VISP_EXPORT void
+connectedComponents(const vpImage<unsigned char> &I, vpImage<int> &labels, int &nbComponents,
+ const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4);
+
+VISP_EXPORT void fillHoles(vpImage<unsigned char> &I
+#if USE_OLD_FILL_HOLE
+ ,
+ const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4
+#endif
+);
+
+VISP_EXPORT void floodFill(vpImage<unsigned char> &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<unsigned char> &marker, const vpImage<unsigned char> &mask,
+ vpImage<unsigned char> &I,
+ const vpImageMorphology::vpConnexityType &connexity = vpImageMorphology::CONNEXITY_4);
+
+VISP_EXPORT unsigned char autoThreshold(vpImage<unsigned char> &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
new file mode 100644
index 0000000..3b8727f
--- /dev/null
+++ b/modules/imgproc/src/vpCLAHE.cpp
@@ -0,0 +1,528 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * CLAHE (Contrast Limited Adaptive Histogram Equalization) algorithm.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/**
+ * License: GPL
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License 2
+ * as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
+ */
+/**
+ * &lsquot;Contrast Limited Adaptive Histogram Equalization&rsquot; as
+ * described in
+ *
+ * <br />BibTeX:
+ * <pre>
+ * @article{zuiderveld94,
+ * author = {Zuiderveld, Karel},
+ * title = {Contrast limited adaptive histogram equalization},
+ * book = {Graphics gems IV},
+ * year = {1994},
+ * isbn = {0-12-336155-9},
+ * pages = {474--485},
+ * publisher = {Academic Press Professional, Inc.},
+ * address = {San Diego, CA, USA},
+ * }
+ * </pre>
+ *
+ * This version calculates the CDF for adjacent blocks and interpolates
+ * the respective CDF for each pixel location in between.
+ *
+ * @author Stephan Saalfeld <saalfeld at mpi-cbg.de>
+ * @version 0.3b
+ */
+
+/*!
+ \file vpCLAHE.cpp
+ \brief Contrast Limited Adaptive Histogram Equalization (CLAHE).
+*/
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/imgproc/vpImgproc.h>
+
+namespace
+{
+int fastRound(const float value) { return (int)(value + 0.5f); }
+
+void clipHistogram(const std::vector<int> &hist, std::vector<int> &clippedHist, const int limit)
+{
+ clippedHist = hist;
+ int clippedEntries = 0, clippedEntriesBefore = 0;
+ int histlength = (int)hist.size();
+
+ do {
+ clippedEntriesBefore = clippedEntries;
+ clippedEntries = 0;
+ for (int i = 0; i < histlength; i++) {
+ int d = clippedHist[i] - limit;
+ if (d > 0) {
+ clippedEntries += d;
+ clippedHist[i] = limit;
+ }
+ }
+
+ int d = clippedEntries / (histlength);
+ int m = clippedEntries % (histlength);
+ for (int i = 0; i < histlength; i++) {
+ clippedHist[i] += d;
+ }
+
+ if (m != 0) {
+ int s = (histlength - 1) / m;
+ for (int i = s / 2; i < histlength; i += s) {
+ ++(clippedHist[i]);
+ }
+ }
+ } while (clippedEntries != clippedEntriesBefore);
+}
+
+void createHistogram(const int blockRadius, const int bins, const int blockXCenter, const int blockYCenter,
+ const vpImage<unsigned char> &I, std::vector<int> &hist)
+{
+ std::fill(hist.begin(), hist.end(), 0);
+
+ int xMin = std::max(0, blockXCenter - blockRadius);
+ int yMin = std::max(0, blockYCenter - blockRadius);
+ int xMax = std::min((int)I.getWidth(), blockXCenter + blockRadius + 1);
+ int yMax = std::min((int)I.getHeight(), blockYCenter + blockRadius + 1);
+
+ for (int y = yMin; y < yMax; ++y) {
+ for (int x = xMin; x < xMax; ++x) {
+ ++hist[fastRound(I[y][x] / 255.0f * bins)];
+ }
+ }
+}
+
+std::vector<float> createTransfer(const std::vector<int> &hist, const int limit, std::vector<int> &cdfs)
+{
+ clipHistogram(hist, cdfs, limit);
+ int hMin = (int)hist.size() - 1;
+
+ for (int i = 0; i < hMin; ++i) {
+ if (cdfs[i] != 0) {
+ hMin = i;
+ }
+ }
+ int cdf = 0;
+ for (int i = hMin; i < (int)hist.size(); ++i) {
+ cdf += cdfs[i];
+ cdfs[i] = cdf;
+ }
+
+ int cdfMin = cdfs[hMin];
+ int cdfMax = cdfs[hist.size() - 1];
+
+ std::vector<float> transfer(hist.size());
+ for (int i = 0; i < (int)transfer.size(); ++i) {
+ transfer[i] = (cdfs[i] - cdfMin) / (float)(cdfMax - cdfMin);
+ }
+
+ return transfer;
+}
+
+float transferValue(const int v, std::vector<int> &clippedHist)
+{
+ int clippedHistLength = (int)clippedHist.size();
+ int hMin = clippedHistLength - 1;
+ for (int i = 0; i < hMin; i++) {
+ if (clippedHist[i] != 0) {
+ hMin = i;
+ }
+ }
+
+ int cdf = 0;
+ for (int i = hMin; i <= v; i++) {
+ cdf += clippedHist[i];
+ }
+
+ int cdfMax = cdf;
+ for (int i = v + 1; i < clippedHistLength; ++i) {
+ cdfMax += clippedHist[i];
+ }
+
+ int cdfMin = clippedHist[hMin];
+ return (cdf - cdfMin) / (float)(cdfMax - cdfMin);
+}
+
+float transferValue(const int v, const std::vector<int> &hist, std::vector<int> &clippedHist, const int limit)
+{
+ clipHistogram(hist, clippedHist, limit);
+
+ return transferValue(v, clippedHist);
+}
+}
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the contrast of a grayscale image locally using the Contrast Limited
+ Adaptative Histogram Equalization method. The limit parameter allows to
+ limit the slope of the transformation function to prevent the
+ overamplification of noise. This method is a transcription of the CLAHE
+ ImageJ plugin code by Stephan Saalfeld.
+
+ \param I1 : The first grayscale image.
+ \param I2 : The second grayscale image after application of the CLAHE
+ method. \param blockRadius : The size (2*blockRadius+1) of the local region
+ around a pixel for which the histogram is equalized. This size should be
+ larger than the size of features to be preserved. \param bins : The number
+ of histogram bins used for histogram equalization (between 1 and 256). The
+ number of histogram bins should be smaller than the number of pixels in a
+ block. \param slope : Limits the contrast stretch in the intensity transfer
+ function. Very large values will let the histogram equalization do whatever
+ it wants to do, that is result in maximal local contrast. The value 1 will
+ result in the original image. \param fast : Use the fast but less accurate
+ version of the filter. The fast version does not evaluate the intensity
+ transfer function for each pixel independently but for a grid of adjacent
+ boxes of the given block size only and interpolates for locations in
+ between.
+*/
+void vp::clahe(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const int blockRadius, const int bins,
+ const float slope, const bool fast)
+{
+ if (blockRadius < 0) {
+ std::cerr << "Error: blockRadius < 0!" << std::endl;
+ return;
+ }
+
+ if (bins < 0 || bins > 256) {
+ std::cerr << "Error: (bins < 0 || bins > 256)!" << std::endl;
+ return;
+ }
+
+ 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;
+ return;
+ }
+
+ I2.resize(I1.getHeight(), I1.getWidth());
+
+ if (fast) {
+ int blockSize = 2 * blockRadius + 1;
+ int limit = (int)(slope * blockSize * blockSize / bins + 0.5);
+
+ /* div */
+ int nc = I1.getWidth() / blockSize;
+ int nr = I1.getHeight() / blockSize;
+
+ /* % */
+ int cm = I1.getWidth() - nc * blockSize;
+ std::vector<int> cs;
+
+ switch (cm) {
+ case 0:
+ cs.resize(nc);
+ for (int i = 0; i < nc; ++i) {
+ cs[i] = i * blockSize + blockRadius + 1;
+ }
+ break;
+
+ case 1:
+ cs.resize(nc + 1);
+ for (int i = 0; i < nc; ++i) {
+ cs[i] = i * blockSize + blockRadius + 1;
+ }
+ cs[nc] = I1.getWidth() - blockRadius - 1;
+ break;
+
+ default:
+ cs.resize(nc + 2);
+ cs[0] = blockRadius + 1;
+ for (int i = 0; i < nc; ++i) {
+ cs[i + 1] = i * blockSize + blockRadius + 1 + cm / 2;
+ }
+ cs[nc + 1] = I1.getWidth() - blockRadius - 1;
+ }
+
+ int rm = I1.getHeight() - nr * blockSize;
+ std::vector<int> rs;
+
+ switch (rm) {
+ case 0:
+ rs.resize((size_t)nr);
+ for (int i = 0; i < nr; ++i) {
+ rs[i] = i * blockSize + blockRadius + 1;
+ }
+ break;
+
+ case 1:
+ rs.resize((size_t)(nr + 1));
+ for (int i = 0; i < nr; ++i) {
+ rs[i] = i * blockSize + blockRadius + 1;
+ }
+ rs[nr] = I1.getHeight() - blockRadius - 1;
+ break;
+
+ default:
+ rs.resize((size_t)(nr + 2));
+ rs[0] = blockRadius + 1;
+ for (int i = 0; i < nr; ++i) {
+ rs[i + 1] = i * blockSize + blockRadius + 1 + rm / 2;
+ }
+ rs[nr + 1] = I1.getHeight() - blockRadius - 1;
+ }
+
+ std::vector<int> hist((size_t)(bins + 1));
+ std::vector<int> cdfs((size_t)(bins + 1));
+ std::vector<float> tl;
+ std::vector<float> tr;
+ std::vector<float> br;
+ std::vector<float> bl;
+
+ for (int r = 0; r <= (int)rs.size(); ++r) {
+ int r0 = std::max(0, r - 1);
+ int r1 = std::min((int)rs.size() - 1, r);
+ int dr = rs[r1] - rs[r0];
+
+ createHistogram(blockRadius, bins, cs[0], rs[r0], I1, hist);
+ tr = createTransfer(hist, limit, cdfs);
+ if (r0 == r1) {
+ br = tr;
+ } else {
+ createHistogram(blockRadius, bins, cs[0], rs[r1], I1, hist);
+ br = createTransfer(hist, limit, cdfs);
+ }
+
+ int yMin = (r == 0 ? 0 : rs[r0]);
+ int yMax = (r < (int)rs.size() ? rs[r1] : I1.getHeight());
+
+ for (int c = 0; c <= (int)cs.size(); ++c) {
+ int c0 = std::max(0, c - 1);
+ int c1 = std::min((int)cs.size() - 1, c);
+ int dc = cs[c1] - cs[c0];
+
+ tl = tr;
+ bl = br;
+
+ if (c0 != c1) {
+ createHistogram(blockRadius, bins, cs[c1], rs[r0], I1, hist);
+ tr = createTransfer(hist, limit, cdfs);
+ if (r0 == r1) {
+ br = tr;
+ } else {
+ createHistogram(blockRadius, bins, cs[c1], rs[r1], I1, hist);
+ br = createTransfer(hist, limit, cdfs);
+ }
+ }
+
+ int xMin = (c == 0 ? 0 : cs[c0]);
+ int xMax = (c < (int)cs.size() ? cs[c1] : I1.getWidth());
+ for (int y = yMin; y < yMax; ++y) {
+ float wy = (float)(rs[r1] - y) / dr;
+
+ for (int x = xMin; x < xMax; ++x) {
+ float wx = (float)(cs[c1] - x) / dc;
+ int v = fastRound(I1[y][x] / 255.0f * bins);
+ float t00 = tl[v];
+ float t01 = tr[v];
+ float t10 = bl[v];
+ float t11 = br[v];
+ float t0 = 0.0f, t1 = 0.0f;
+
+ if (c0 == c1) {
+ t0 = t00;
+ t1 = t10;
+ } else {
+ t0 = wx * t00 + (1.0f - wx) * t01;
+ t1 = wx * t10 + (1.0f - wx) * t11;
+ }
+
+ float t = (r0 == r1) ? t0 : wy * t0 + (1.0f - wy) * t1;
+ I2[y][x] = std::max(0, std::min(255, fastRound(t * 255.0f)));
+ }
+ }
+ }
+ }
+ } else {
+ std::vector<int> hist(bins + 1), prev_hist(bins + 1);
+ std::vector<int> clippedHist(bins + 1);
+
+ bool first = true;
+ int xMin0 = 0;
+ int xMax0 = std::min((int)I1.getWidth(), blockRadius);
+
+ for (int y = 0; y < (int)I1.getHeight(); y++) {
+ int yMin = std::max(0, y - (int)blockRadius);
+ int yMax = std::min((int)I1.getHeight(), y + blockRadius + 1);
+ int h = yMax - yMin;
+
+#if 0
+ std::fill(hist.begin(), hist.end(), 0);
+ // Compute histogram for the current block
+ for (int yi = yMin; yi < yMax; yi++) {
+ for (int xi = xMin0; xi < xMax0; xi++) {
+ ++hist[fastRound(I1[yi][xi] / 255.0f * bins)];
+ }
+ }
+#else
+ if (first) {
+ first = false;
+ // Compute histogram for the block at (0,0)
+ for (int yi = yMin; yi < yMax; yi++) {
+ for (int xi = xMin0; xi < xMax0; xi++) {
+ ++hist[fastRound(I1[yi][xi] / 255.0f * bins)];
+ }
+ }
+ } else {
+ hist = prev_hist;
+
+ if (yMin > 0) {
+ int yMin1 = yMin - 1;
+ // Sliding histogram, remove top
+ for (int xi = xMin0; xi < xMax0; xi++) {
+ --hist[fastRound(I1[yMin1][xi] / 255.0f * bins)];
+ }
+ }
+
+ if (y + blockRadius < (int)I1.getHeight()) {
+ int yMax1 = yMax - 1;
+ // Sliding histogram, add bottom
+ for (int xi = xMin0; xi < xMax0; xi++) {
+ ++hist[fastRound(I1[yMax1][xi] / 255.0f * bins)];
+ }
+ }
+ }
+ prev_hist = hist;
+#endif
+
+ for (int x = 0; x < (int)I1.getWidth(); x++) {
+ int xMin = std::max(0, x - (int)blockRadius);
+ int xMax = x + blockRadius + 1;
+
+ if (xMin > 0) {
+ int xMin1 = xMin - 1;
+ // Sliding histogram, remove left
+ for (int yi = yMin; yi < yMax; yi++) {
+ --hist[fastRound(I1[yi][xMin1] / 255.0f * bins)];
+ }
+ }
+
+ if (xMax <= (int)I1.getWidth()) {
+ int xMax1 = xMax - 1;
+ // Sliding histogram, add right
+ for (int yi = yMin; yi < yMax; yi++) {
+ ++hist[fastRound(I1[yi][xMax1] / 255.0f * bins)];
+ }
+ }
+
+ int v = fastRound(I1[y][x] / 255.0f * bins);
+ 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);
+ }
+ }
+ }
+}
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the contrast of a color image locally using the Contrast Limited
+ Adaptative Histogram Equalization method. The limit parameter allows to
+ limit the slope of the transformation function to prevent the
+ overamplification of noise. This method is a transcription of the CLAHE
+ ImageJ plugin code by Stephan Saalfeld.
+
+ \param I1 : The first color image.
+ \param I2 : The second color image after application of the CLAHE method.
+ \param blockRadius : The size (2*blockRadius+1) of the local region around a
+ pixel for which the histogram is equalized. This size should be larger than
+ the size of features to be preserved. \param bins : The number of histogram
+ bins used for histogram equalization (between 1 and 256). The number of
+ histogram bins should be smaller than the number of pixels in a block.
+ \param slope : Limits the contrast stretch in the intensity transfer
+ function. Very large values will let the histogram equalization do whatever
+ it wants to do, that is result in maximal local contrast. The value 1 will
+ result in the original image. \param fast : Use the fast but less accurate
+ version of the filter. The fast version does not evaluate the intensity
+ transfer function for each pixel independently but for a grid of adjacent
+ boxes of the given block size only and interpolates for locations in
+ between.
+*/
+void vp::clahe(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const int blockRadius, const int bins, const float slope,
+ const bool fast)
+{
+ // Split
+ vpImage<unsigned char> pR(I1.getHeight(), I1.getWidth());
+ vpImage<unsigned char> pG(I1.getHeight(), I1.getWidth());
+ vpImage<unsigned char> pB(I1.getHeight(), I1.getWidth());
+ vpImage<unsigned char> pa(I1.getHeight(), I1.getWidth());
+
+ vpImageConvert::split(I1, &pR, &pG, &pB, &pa);
+
+ // Apply CLAHE independently on RGB channels
+ vpImage<unsigned char> resR, resG, resB;
+ clahe(pR, resR, blockRadius, bins, slope, fast);
+ clahe(pG, resG, blockRadius, bins, slope, fast);
+ clahe(pB, resB, blockRadius, bins, slope, fast);
+
+ I2.resize(I1.getHeight(), I1.getWidth());
+ unsigned int size = I2.getWidth() * I2.getHeight();
+ unsigned char *ptrStart = (unsigned char *)I2.bitmap;
+ unsigned char *ptrEnd = ptrStart + size * 4;
+ unsigned char *ptrCurrent = ptrStart;
+
+ unsigned int cpt = 0;
+ while (ptrCurrent != ptrEnd) {
+ *ptrCurrent = resR.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = resG.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = resB.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = pa.bitmap[cpt];
+ ++ptrCurrent;
+
+ cpt++;
+ }
+}
diff --git a/modules/imgproc/src/vpConnectedComponents.cpp b/modules/imgproc/src/vpConnectedComponents.cpp
new file mode 100644
index 0000000..20eb5ca
--- /dev/null
+++ b/modules/imgproc/src/vpConnectedComponents.cpp
@@ -0,0 +1,172 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Connected components.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpConnectedComponents.cpp
+ \brief Basic connected components.
+*/
+
+#include <queue>
+#include <visp3/imgproc/vpImgproc.h>
+
+namespace
+{
+void getNeighbors(const vpImage<unsigned char> &I, std::queue<vpImagePoint> &listOfNeighbors, const unsigned int i,
+ const unsigned int j, const vpImageMorphology::vpConnexityType &connexity)
+{
+ unsigned char currValue = I[i][j];
+
+ if (connexity == vpImageMorphology::CONNEXITY_4) {
+ // Top
+ if (I[i - 1][j] == currValue) {
+ listOfNeighbors.push(vpImagePoint(i - 1, j));
+ }
+
+ // Left
+ if (I[i][j - 1] == currValue) {
+ listOfNeighbors.push(vpImagePoint(i, j - 1));
+ }
+
+ // Right
+ if (I[i][j + 1] == currValue) {
+ listOfNeighbors.push(vpImagePoint(i, j + 1));
+ }
+
+ // Bottom
+ if (I[i + 1][j] == currValue) {
+ listOfNeighbors.push(vpImagePoint(i + 1, j));
+ }
+ } else {
+ for (int cpt1 = -1; cpt1 <= 1; cpt1++) {
+ for (int cpt2 = -1; cpt2 <= 1; cpt2++) {
+ // Everything except the current position
+ if (cpt1 != 0 || cpt2 != 0) {
+ if (I[(int)i + cpt1][(int)j + cpt2] == currValue) {
+ listOfNeighbors.push(vpImagePoint((int)i + cpt1, (int)j + cpt2));
+ }
+ }
+ }
+ }
+ }
+}
+
+void visitNeighbors(vpImage<unsigned char> &I_copy, std::queue<vpImagePoint> &listOfNeighbors, vpImage<int> &labels,
+ const int current_label, const vpImageMorphology::vpConnexityType &connexity)
+{
+ // Visit the neighbors
+ while (!listOfNeighbors.empty()) {
+ vpImagePoint imPt = listOfNeighbors.front();
+ unsigned int i = (unsigned int)imPt.get_i();
+ unsigned int j = (unsigned int)imPt.get_j();
+ listOfNeighbors.pop();
+
+ if (I_copy[i][j]) {
+ getNeighbors(I_copy, listOfNeighbors, i, j, connexity);
+
+ // Reset current position and set label
+ I_copy[i][j] = 0;
+ labels[i][j] = current_label;
+ }
+ }
+}
+} // namespace
+
+/*!
+ \ingroup group_imgproc_connected_components
+
+ Perform connected components detection.
+
+ \param I : Input image (0 means background).
+ \param labels : Label image that contain for each position the component
+ label. \param nbComponents : Number of connected components. \param
+ connexity : Type of connexity.
+*/
+void vp::connectedComponents(const vpImage<unsigned char> &I, vpImage<int> &labels, int &nbComponents,
+ const vpImageMorphology::vpConnexityType &connexity)
+{
+ if (I.getSize() == 0) {
+ return;
+ }
+
+ labels.resize(I.getHeight(), I.getWidth());
+
+ vpImage<unsigned char> I_copy(I.getHeight() + 2, I.getWidth() + 2);
+ // Copy and add border
+ for (unsigned int i = 0; i < I_copy.getHeight(); i++) {
+ if (i == 0 || i == I_copy.getHeight() - 1) {
+ memset(I_copy[i], 0, sizeof(unsigned char) * I_copy.getWidth());
+ } else {
+ I_copy[i][0] = 0;
+ memcpy(I_copy[i] + 1, I[i - 1], sizeof(unsigned char) * I.getWidth());
+ I_copy[i][I_copy.getWidth() - 1] = 0;
+ }
+ }
+
+ vpImage<int> labels_copy(I.getHeight() + 2, I.getWidth() + 2, 0);
+
+ int current_label = 1;
+ std::queue<vpImagePoint> listOfNeighbors;
+
+ for (unsigned int cpt1 = 0; cpt1 < I.getHeight(); cpt1++) {
+ unsigned int i = cpt1 + 1;
+
+ for (unsigned int cpt2 = 0; cpt2 < I.getWidth(); cpt2++) {
+ unsigned int j = cpt2 + 1;
+
+ if (I_copy[i][j] && labels_copy[i][j] == 0) {
+ // Get all the neighbors relative to the current position
+ getNeighbors(I_copy, listOfNeighbors, i, j, connexity);
+
+ // Reset current position and set label
+ I_copy[i][j] = 0;
+ labels_copy[i][j] = current_label;
+
+ visitNeighbors(I_copy, listOfNeighbors, labels_copy, current_label, connexity);
+
+ // Increment label
+ current_label++;
+ }
+ }
+ }
+
+ for (unsigned int i = 0; i < labels.getHeight(); i++) {
+ 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
new file mode 100644
index 0000000..c8f94dc
--- /dev/null
+++ b/modules/imgproc/src/vpContours.cpp
@@ -0,0 +1,468 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Basic contours extraction based on the orignal work of
+ * Sina Samangooei (ss at ecs.soton.ac.uk).
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/**
+ * Copyright (c) 2011, The University of Southampton and the individual
+ * contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * * Neither the name of the University of Southampton nor the names of its
+ * contributors may be used to endorse or promote products derived from this
+ * software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/*!
+ \file vpContours.cpp
+ \brief Basic contours extraction.
+*/
+
+#include <map>
+#include <visp3/imgproc/vpImgproc.h>
+
+namespace
+{
+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<double>::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<double>::epsilon()) {
+ direction.m_direction = SOUTH;
+ } else if (from.get_j() < to.get_j()) {
+ direction.m_direction = SOUTH_EAST;
+ } else {
+ direction.m_direction = SOUTH_WEST;
+ }
+ } else {
+ if (std::fabs(from.get_j() - to.get_j()) < std::numeric_limits<double>::epsilon()) {
+ direction.m_direction = NORTH;
+ } else if (from.get_j() < to.get_j()) {
+ direction.m_direction = NORTH_EAST;
+ } else {
+ direction.m_direction = NORTH_WEST;
+ }
+ }
+
+ return true;
+}
+
+bool crossesEastBorder(const vpImage<int> &I, bool checked[8], const vpImagePoint &point)
+{
+ vpDirection direction;
+ if (!fromTo(point, vpImagePoint(point.get_i(), point.get_j() + 1), direction)) {
+ return false;
+ }
+
+ bool b = checked[(int)direction.m_direction];
+
+ if (point.get_i() < 0 || point.get_j() < 0) {
+ return false;
+ }
+
+ 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);
+}
+
+void addContourPoint(vpImage<int> &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
+
+ unsigned int i = (unsigned int)point.get_i();
+ unsigned int j = (unsigned int)point.get_j();
+
+ if (crossesEastBorder(I, checked, point)) {
+ I[i][j] = -nbd;
+ } else if (I[i][j] == 1) {
+ // Only set if the pixel has not been visited before (3.4) (b)
+ I[i][j] = nbd;
+ } // Otherwise leave it alone
+}
+
+void followBorder(vpImage<int> &I, const vpImagePoint &ij, vpImagePoint &i2j2, vp::vpContour *border, const int nbd)
+{
+ vpDirection dir;
+ if (!fromTo(ij, i2j2, dir)) {
+ throw vpException(vpException::fatalError, "ij == i2j2");
+ }
+
+ vpDirection trace = dir.clockwise();
+ vpImagePoint i1j1(-1, -1);
+
+ // Find i1j1 (3.1)
+ while (trace.m_direction != dir.m_direction) {
+ vpImagePoint activePixel = trace.active(I, ij);
+
+ if (activePixel.get_i() >= 0 && activePixel.get_j() >= 0) {
+ i1j1 = activePixel;
+ break;
+ }
+
+ trace = trace.clockwise();
+ }
+
+ if (i1j1.get_i() < 0 || i1j1.get_j() < 0) {
+ //(3.1) ; single pixel contour
+ return;
+ }
+
+ i2j2 = i1j1;
+ vpImagePoint i3j3 = ij; //(3.2)
+
+ bool checked[8] = {false, false, false, false, false, false, false, false};
+
+ while (true) {
+ if (!fromTo(i3j3, i2j2, dir)) {
+ throw vpException(vpException::fatalError, "i3j3 == i2j2");
+ }
+
+ trace = dir.counterClockwise();
+ vpImagePoint i4j4(-1, -1);
+
+ // Reset checked
+ for (int cpt = 0; cpt < 8; cpt++) {
+ checked[cpt] = false;
+ }
+
+ while (true) {
+ i4j4 = trace.active(I, i3j3); //(3.3)
+ if (i4j4.get_i() >= 0 && i4j4.get_j() >= 0) {
+ break;
+ }
+
+ checked[(int)trace.m_direction] = true;
+ trace = trace.counterClockwise();
+ }
+
+ addContourPoint(I, border, i3j3, checked, nbd);
+
+ if (i4j4 == ij && i3j3 == i1j1) {
+ //(3.5)
+ break;
+ }
+
+ //(3.5)
+ i2j2 = i3j3;
+ i3j3 = i4j4;
+ }
+}
+
+bool isOuterBorderStart(const vpImage<int> &I, unsigned int i, unsigned int j)
+{
+ return (I[i][j] == 1 && (j == 0 || I[i][j - 1] == 0));
+}
+
+bool isHoleBorderStart(const vpImage<int> &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)
+{
+ if (level > 0) {
+ vp::vpContour *contour_node = new vp::vpContour;
+ contour_node->m_contourType = root.m_contourType;
+ contour_node->m_points = root.m_points;
+
+ contour_list.m_children.push_back(contour_node);
+ }
+
+ for (std::vector<vp::vpContour *>::const_iterator it = root.m_children.begin(); it != root.m_children.end(); ++it) {
+ getContoursList(**it, level + 1, contour_list);
+ }
+}
+} // namespace
+
+/*!
+ \ingroup group_imgproc_contours
+
+ Draw the input contours on the binary image.
+
+ \param I : Grayscale image where we want to draw the input contours.
+ \param contours : Detected contours.
+ \param grayValue : Drawing grayscale color.
+*/
+void vp::drawContours(vpImage<unsigned char> &I, const std::vector<std::vector<vpImagePoint> > &contours,
+ unsigned char grayValue)
+{
+ if (I.getSize() == 0) {
+ return;
+ }
+
+ for (std::vector<std::vector<vpImagePoint> >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) {
+ for (std::vector<vpImagePoint>::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;
+ }
+ }
+}
+
+/*!
+ \ingroup group_imgproc_contours
+
+ Draw the input contours on the color image.
+
+ \param I : Color image where we want to draw the input contours.
+ \param contours : Detected contours.
+ \param color : Drawing color.
+*/
+void vp::drawContours(vpImage<vpRGBa> &I, const std::vector<std::vector<vpImagePoint> > &contours, const vpColor &color)
+{
+ if (I.getSize() == 0) {
+ return;
+ }
+
+ for (std::vector<std::vector<vpImagePoint> >::const_iterator it1 = contours.begin(); it1 != contours.end(); ++it1) {
+ for (std::vector<vpImagePoint>::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);
+ }
+ }
+}
+
+/*!
+ \ingroup group_imgproc_contours
+
+ Extract contours from a binary image.
+
+ \param I_original : Input binary image (0 means background, 1 means
+ foreground, other values are not allowed). \param contours : Detected
+ contours. \param contourPts : List of contours, each contour contains a list
+ of contour points. \param retrievalMode : Contour retrieval mode.
+*/
+void vp::findContours(const vpImage<unsigned char> &I_original, vpContour &contours,
+ std::vector<std::vector<vpImagePoint> > &contourPts, const vpContourRetrievalType &retrievalMode)
+{
+ if (I_original.getSize() == 0) {
+ return;
+ }
+
+ // Clear output results
+ contourPts.clear();
+
+ // Copy uchar I_original into int I + padding
+ vpImage<int> I(I_original.getHeight() + 2, I_original.getWidth() + 2);
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ if (i == 0 || i == I.getHeight() - 1) {
+ memset(I.bitmap, 0, sizeof(int) * I.getWidth());
+ } else {
+ I[i][0] = 0;
+ for (unsigned int j = 0; j < I_original.getWidth(); j++) {
+ I[i][j + 1] = I_original[i - 1][j];
+ }
+ I[i][I.getWidth() - 1] = 0;
+ }
+ }
+
+ // Ref: http://openimaj.org/
+ // Ref: Satoshi Suzuki and others. Topological structural analysis of
+ // digitized binary images by border following.
+ int nbd = 1; // Newest border
+ int lnbd = 1; // Last newest border
+
+ // Background contour
+ // By default the root contour is a hole contour
+ vpContour *root = new vpContour(vp::CONTOUR_HOLE);
+
+ std::map<int, vpContour *> borderMap;
+ borderMap[lnbd] = root;
+
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ lnbd = 1; // Reset LNBD at the beginning of each scan row
+
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ int fji = I[i][j];
+
+ bool isOuter = isOuterBorderStart(I, i, j);
+ bool isHole = isHoleBorderStart(I, i, j);
+
+ if (isOuter || isHole) { // else (1) (c)
+ vpContour *border = new vpContour;
+ vpContour *borderPrime = NULL;
+ vpImagePoint from(i, j);
+
+ if (isOuter) {
+ //(1) (a)
+ nbd++;
+ from.set_j(from.get_j() - 1);
+ border->m_contourType = vp::CONTOUR_OUTER;
+ borderPrime = borderMap[lnbd];
+
+ // Table 1
+ switch (borderPrime->m_contourType) {
+ case vp::CONTOUR_OUTER:
+ border->setParent(borderPrime->m_parent);
+ break;
+
+ case vp::CONTOUR_HOLE:
+ border->setParent(borderPrime);
+ break;
+
+ default:
+ break;
+ }
+ } else {
+ //(1) (b)
+ nbd++;
+
+ if (fji > 1) {
+ lnbd = fji;
+ }
+
+ borderPrime = borderMap[lnbd];
+ from.set_j(from.get_j() + 1);
+ border->m_contourType = vp::CONTOUR_HOLE;
+
+ // Table 1
+ switch (borderPrime->m_contourType) {
+ case vp::CONTOUR_OUTER:
+ border->setParent(borderPrime);
+ break;
+
+ case vp::CONTOUR_HOLE:
+ border->setParent(borderPrime->m_parent);
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ vpImagePoint ij(i, j);
+ followBorder(I, ij, from, border, nbd);
+
+ //(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
+ I[i][j] = -nbd;
+ }
+
+ if (retrievalMode == CONTOUR_RETR_LIST || retrievalMode == CONTOUR_RETR_TREE) {
+ // Add contour points
+ contourPts.push_back(border->m_points);
+ }
+
+ borderMap[nbd] = border;
+ }
+
+ //(4)
+ if (fji != 0 && fji != 1) {
+ lnbd = std::abs(fji);
+ }
+ }
+ }
+
+ if (retrievalMode == CONTOUR_RETR_EXTERNAL || retrievalMode == CONTOUR_RETR_LIST) {
+ // Delete contours content
+ contours.m_parent = NULL;
+
+ for (std::vector<vpContour *>::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) {
+ (*it)->m_parent = NULL;
+ if (*it != NULL) {
+ delete *it;
+ *it = NULL;
+ }
+ }
+
+ contours.m_children.clear();
+ }
+
+ if (retrievalMode == CONTOUR_RETR_EXTERNAL) {
+ // Add only external contours
+ for (std::vector<vpContour *>::const_iterator it = root->m_children.begin(); it != root->m_children.end(); ++it) {
+ // Save children
+ std::vector<vpContour *> children_copy = (*it)->m_children;
+ // Erase children
+ (*it)->m_children.clear();
+ // Copy contour
+ contours.m_children.push_back(new vpContour(**it));
+ // Restore children
+ (*it)->m_children = children_copy;
+ // Set parent to children
+ for (size_t i = 0; i < contours.m_children.size(); i++) {
+ contours.m_children[i]->m_parent = &contours;
+ }
+ contourPts.push_back((*it)->m_points);
+ }
+ } else if (retrievalMode == CONTOUR_RETR_LIST) {
+ getContoursList(*root, 0, contours);
+
+ // Set parent to root
+ for (std::vector<vpContour *>::iterator it = contours.m_children.begin(); it != contours.m_children.end(); ++it) {
+ (*it)->m_parent = &contours;
+ }
+ } else {
+ // CONTOUR_RETR_TREE
+ contours = *root;
+ }
+
+ delete root;
+ root = NULL;
+}
diff --git a/modules/imgproc/src/vpFloodFill.cpp b/modules/imgproc/src/vpFloodFill.cpp
new file mode 100644
index 0000000..52d5b3d
--- /dev/null
+++ b/modules/imgproc/src/vpFloodFill.cpp
@@ -0,0 +1,173 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Flood fill algorithm.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/*
+ * Copyright (c) 2004-2007, Lode Vandevenne
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
+ * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
+ * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
+ * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
+ * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+ * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+ * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+ * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ *
+ */
+
+/*!
+ \file vpFloodFill.cpp
+ \brief Flood fill algorithm.
+*/
+
+#include <queue>
+#include <visp3/imgproc/vpImgproc.h>
+
+/*!
+ \ingroup group_imgproc_connected_components
+
+ Perform the flood fill algorithm.
+
+ \param I : Input image to flood fill.
+ \param seedPoint : Seed position in the image.
+ \param oldValue : Old value to replace.
+ \param newValue : New value to flood fill.
+ \param connexity : Type of connexity.
+*/
+void vp::floodFill(vpImage<unsigned char> &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
+ if (oldValue == newValue || I.getSize() == 0) {
+ return;
+ }
+
+ std::queue<vpImagePoint> seed_queue;
+
+ // Add initial seed point
+ seed_queue.push(seedPoint);
+
+ while (!seed_queue.empty()) {
+ vpImagePoint current_seed = seed_queue.front();
+ seed_queue.pop();
+
+ unsigned int x = (unsigned int)current_seed.get_j();
+ unsigned int y = (unsigned int)current_seed.get_i();
+ int x1 = (int)x;
+
+ // Find most left pixel
+ while (x1 >= 0 && I[y][x1] == oldValue) {
+ x1--;
+ }
+ x1++;
+
+ bool spanAbove = false, spanBelow = false;
+
+ while (x1 < (int)I.getWidth() && I[y][x1] == oldValue) {
+ I[y][x1] = newValue;
+
+ if (!spanAbove && y > 0) {
+ if (I[y - 1][x1] == oldValue) {
+ // North
+ spanAbove = true;
+ seed_queue.push(vpImagePoint(y - 1, x1));
+ }
+
+ if (connexity != vpImageMorphology::CONNEXITY_4) {
+ if (x1 > 0 && I[y - 1][x1 - 1] == oldValue) {
+ // North west
+ spanAbove = true;
+ seed_queue.push(vpImagePoint(y - 1, x1 - 1));
+ }
+ if (x1 < (int)I.getWidth() - 1 && I[y - 1][x1 + 1] == oldValue) {
+ // North east
+ spanAbove = true;
+ seed_queue.push(vpImagePoint(y - 1, x1 + 1));
+ }
+ }
+ } else if (spanAbove && y > 0 && I[y - 1][x1] != oldValue) {
+ spanAbove = false;
+ }
+
+ if (!spanBelow && y < I.getHeight() - 1) {
+ if (I[y + 1][x1] == oldValue) {
+ // South
+ seed_queue.push(vpImagePoint(y + 1, x1));
+ spanBelow = true;
+ }
+
+ if (connexity != vpImageMorphology::CONNEXITY_4) {
+ if (x1 > 0 && I[y + 1][x1 - 1] == oldValue) {
+ // South west
+ seed_queue.push(vpImagePoint(y + 1, x1 - 1));
+ spanBelow = true;
+ }
+ if (x1 < (int)I.getWidth() - 1 && I[y + 1][x1 + 1] == oldValue) {
+ // South east
+ seed_queue.push(vpImagePoint(y + 1, x1 + 1));
+ spanBelow = true;
+ }
+ }
+ } else if (spanBelow && y < I.getHeight() - 1 && I[y + 1][x1] != oldValue) {
+ spanBelow = false;
+ }
+
+ // TODO: improve 8-connexity
+ if (connexity != vpImageMorphology::CONNEXITY_4) {
+ spanBelow = false;
+ spanAbove = false;
+ }
+
+ x1++;
+ }
+ }
+}
diff --git a/modules/imgproc/src/vpImgproc.cpp b/modules/imgproc/src/vpImgproc.cpp
new file mode 100644
index 0000000..36c4425
--- /dev/null
+++ b/modules/imgproc/src/vpImgproc.cpp
@@ -0,0 +1,706 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Convert image types.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/* Autostretch HSV 0.10 --- image filter plug-in for GIMP
+ *
+ * Copyright (C) 1997 Scott Goehring
+ * Copyright (C) 1996 Federico Mena Quintero
+ *
+ * You can contact me at scott at poverty.bloomington.in.us
+ *
+ * This program is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*!
+ \file vpImgproc.cpp
+ \brief Basic image processing functions.
+*/
+
+#include <visp3/core/vpHistogram.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/imgproc/vpImgproc.h>
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the brightness of a grayscale image such as the new intensity is
+ alpha x old_intensity + beta.
+
+ \param I : The grayscale image to adjust the brightness.
+ \param alpha : Multiplication coefficient.
+ \param beta : Constant value added to the old intensity.
+*/
+void vp::adjust(vpImage<unsigned char> &I, const double alpha, const double beta)
+{
+ // Construct the look-up table
+ unsigned char lut[256];
+ for (unsigned int i = 0; i < 256; i++) {
+ lut[i] = vpMath::saturate<unsigned char>(alpha * i + beta);
+ }
+
+ // Apply the transformation using a LUT
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the brightness of a grayscale image such as the new intensity is
+ alpha x old_intensity + beta.
+
+ \param I1 : The original grayscale image.
+ \param I2 : The grayscale image after adjusting pixel intensities.
+ \param alpha : Multiplication coefficient.
+ \param beta : Constant value added to the old intensity.
+*/
+void vp::adjust(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const double alpha, const double beta)
+{
+ // Copy I1 to I2
+ I2 = I1;
+
+ vp::adjust(I2, alpha, beta);
+}
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the brightness of a color image such as the new intensity is alpha x
+ old_intensity + beta.
+
+ \param I : The color image to adjust the brightness.
+ \param alpha : Multiplication coefficient.
+ \param beta : Constant value added to the old intensity.
+*/
+void vp::adjust(vpImage<vpRGBa> &I, const double alpha, const double beta)
+{
+ // Construct the look-up table
+ vpRGBa lut[256];
+ for (unsigned int i = 0; i < 256; i++) {
+ lut[i].R = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].G = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].B = vpMath::saturate<unsigned char>(alpha * i + beta);
+ lut[i].A = vpMath::saturate<unsigned char>(alpha * i + beta);
+ }
+
+ // Apply the transformation using a LUT
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_brightness
+
+ Adjust the brightness of a color image such as the new intensity is alpha x
+ old_intensity + beta.
+
+ \param I1 : The original color image.
+ \param I2 : The color image after adjusting pixel intensities.
+ \param alpha : Multiplication coefficient.
+ \param beta : Constant value added to the old intensity.
+*/
+void vp::adjust(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const double alpha, const double beta)
+{
+ // Copy I1 to I2
+ I2 = I1;
+
+ vp::adjust(I2, alpha, beta);
+}
+
+/*!
+ \ingroup group_imgproc_histogram
+
+ Adjust the contrast of a grayscale image by performing an histogram
+ equalization. The intensity distribution is redistributed over the full [0 -
+ 255] range such as the cumulative histogram distribution becomes linear.
+
+ \param I : The grayscale image to apply histogram equalization.
+*/
+void vp::equalizeHistogram(vpImage<unsigned char> &I)
+{
+ if (I.getWidth() * I.getHeight() == 0) {
+ return;
+ }
+
+ // Calculate the histogram
+ vpHistogram hist;
+ hist.calculate(I);
+
+ // Calculate the cumulative distribution function
+ unsigned int cdf[256];
+ unsigned int cdfMin = /*std::numeric_limits<unsigned int>::max()*/ UINT_MAX, cdfMax = 0;
+ unsigned int minValue =
+ /*std::numeric_limits<unsigned int>::max()*/ UINT_MAX,
+ maxValue = 0;
+ cdf[0] = hist[0];
+
+ if (cdf[0] < cdfMin && cdf[0] > 0) {
+ cdfMin = cdf[0];
+ minValue = 0;
+ }
+
+ for (unsigned int i = 1; i < 256; i++) {
+ cdf[i] = cdf[i - 1] + hist[i];
+
+ if (cdf[i] < cdfMin && cdf[i] > 0) {
+ cdfMin = cdf[i];
+ minValue = i;
+ }
+
+ if (cdf[i] > cdfMax) {
+ cdfMax = cdf[i];
+ maxValue = i;
+ }
+ }
+
+ unsigned int nbPixels = I.getWidth() * I.getHeight();
+ if (nbPixels == cdfMin) {
+ // Only one brightness value in the image
+ return;
+ }
+
+ // 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);
+ }
+
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_histogram
+
+ Adjust the contrast of a grayscale image by performing an histogram
+ equalization. The intensity distribution is redistributed over the full [0 -
+ 255] range such as the cumulative histogram distribution becomes linear.
+
+ \param I1 : The first grayscale image.
+ \param I2 : The second grayscale image after histogram equalization.
+*/
+void vp::equalizeHistogram(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2)
+{
+ I2 = I1;
+ vp::equalizeHistogram(I2);
+}
+
+/*!
+ \ingroup group_imgproc_histogram
+
+ Adjust the contrast of a color image by performing an histogram
+ equalization. The intensity distribution is redistributed over the full [0 -
+ 255] range such as the cumulative histogram distribution becomes linear. The
+ alpha channel is ignored / copied from the source alpha channel.
+
+ \param I : The color image to apply histogram equalization.
+ \param useHSV : If true, the histogram equalization is performed on the
+ value channel (in HSV space), otherwise the histogram equalization is
+ performed independently on the RGB channels.
+*/
+void vp::equalizeHistogram(vpImage<vpRGBa> &I, const bool useHSV)
+{
+ if (I.getWidth() * I.getHeight() == 0) {
+ return;
+ }
+
+ if (!useHSV) {
+ // Split the RGBa image into 4 images
+ vpImage<unsigned char> pR(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pG(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pB(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pa(I.getHeight(), I.getWidth());
+
+ vpImageConvert::split(I, &pR, &pG, &pB, &pa);
+
+ // Apply histogram equalization for each channel
+ vp::equalizeHistogram(pR);
+ vp::equalizeHistogram(pG);
+ vp::equalizeHistogram(pB);
+
+ // Merge the result in I
+ unsigned int size = I.getWidth() * I.getHeight();
+ unsigned char *ptrStart = (unsigned char *)I.bitmap;
+ unsigned char *ptrEnd = ptrStart + size * 4;
+ unsigned char *ptrCurrent = ptrStart;
+
+ unsigned int cpt = 0;
+ while (ptrCurrent != ptrEnd) {
+ *ptrCurrent = pR.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = pG.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = pB.bitmap[cpt];
+ ++ptrCurrent;
+
+ *ptrCurrent = pa.bitmap[cpt];
+ ++ptrCurrent;
+
+ cpt++;
+ }
+ } else {
+ vpImage<unsigned char> hue(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> saturation(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> value(I.getHeight(), I.getWidth());
+
+ 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);
+
+ // 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);
+ }
+}
+
+/*!
+ \ingroup group_imgproc_histogram
+
+ Adjust the contrast of a color image by performing an histogram
+ equalization. The intensity distribution is redistributed over the full [0 -
+ 255] range such as the cumulative histogram distribution becomes linear. The
+ alpha channel is ignored / copied from the source alpha channel.
+
+ \param I1 : The first color image.
+ \param I2 : The second color image after histogram equalization.
+ \param useHSV : If true, the histogram equalization is performed on the
+ value channel (in HSV space), otherwise the histogram equalization is
+ performed independently on the RGB channels.
+*/
+void vp::equalizeHistogram(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const bool useHSV)
+{
+ I2 = I1;
+ vp::equalizeHistogram(I2, useHSV);
+}
+
+/*!
+ \ingroup group_imgproc_gamma
+
+ Perform a gamma correction on a grayscale image.
+
+ \param I : The grayscale image to apply gamma correction.
+ \param gamma : Gamma value.
+*/
+void vp::gammaCorrection(vpImage<unsigned char> &I, const double gamma)
+{
+ double inverse_gamma = 1.0;
+ if (gamma > 0) {
+ inverse_gamma = 1.0 / gamma;
+ } else {
+ 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<unsigned char>(pow((double)i / 255.0, inverse_gamma) * 255.0);
+ }
+
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_gamma
+
+ Perform a gamma correction on a grayscale image.
+
+ \param I1 : The first grayscale image.
+ \param I2 : The second grayscale image after gamma correction.
+ \param gamma : Gamma value.
+*/
+void vp::gammaCorrection(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2, const double gamma)
+{
+ I2 = I1;
+ vp::gammaCorrection(I2, gamma);
+}
+
+/*!
+ \ingroup group_imgproc_gamma
+
+ Perform a gamma correction on a color image.
+
+ \param I : The color image to apply gamma correction.
+ \param gamma : Gamma value.
+*/
+void vp::gammaCorrection(vpImage<vpRGBa> &I, const double gamma)
+{
+ double inverse_gamma = 1.0;
+ if (gamma > 0) {
+ inverse_gamma = 1.0 / gamma;
+ } else {
+ 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<unsigned char>(pow((double)i / 255.0, inverse_gamma) * 255.0);
+ lut[i].G = vpMath::saturate<unsigned char>(pow((double)i / 255.0, inverse_gamma) * 255.0);
+ lut[i].B = vpMath::saturate<unsigned char>(pow((double)i / 255.0, inverse_gamma) * 255.0);
+ lut[i].A = vpMath::saturate<unsigned char>(pow((double)i / 255.0, inverse_gamma) * 255.0);
+ }
+
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_gamma
+
+ Perform a gamma correction on a color image.
+
+ \param I1 : The first color image.
+ \param I2 : The second color image after gamma correction.
+ \param gamma : Gamma value.
+*/
+void vp::gammaCorrection(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const double gamma)
+{
+ I2 = I1;
+ vp::gammaCorrection(I2, gamma);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a grayscale image.
+
+ \param I : The grayscale image to stretch the contrast.
+*/
+void vp::stretchContrast(vpImage<unsigned char> &I)
+{
+ // Find min and max intensity values
+ unsigned char min = 255, max = 0;
+ I.getMinMaxValue(min, max);
+
+ unsigned char range = max - min;
+
+ // Construct the look-up table
+ unsigned char lut[256];
+ if (range > 0) {
+ for (unsigned int x = min; x <= max; x++) {
+ lut[x] = 255 * (x - min) / range;
+ }
+ } else {
+ lut[min] = min;
+ }
+
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a grayscale image.
+
+ \param I1 : The first input grayscale image.
+ \param I2 : The second output grayscale image.
+*/
+void vp::stretchContrast(const vpImage<unsigned char> &I1, vpImage<unsigned char> &I2)
+{
+ // Copy I1 to I2
+ I2 = I1;
+ vp::stretchContrast(I2);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a color image.
+
+ \param I : The color image to stretch the contrast.
+*/
+void vp::stretchContrast(vpImage<vpRGBa> &I)
+{
+ // Find min and max intensity values
+ vpRGBa min = 255, max = 0;
+
+ // Split the RGBa image into 4 images
+ vpImage<unsigned char> pR(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pG(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pB(I.getHeight(), I.getWidth());
+ vpImage<unsigned char> pa(I.getHeight(), I.getWidth());
+
+ vpImageConvert::split(I, &pR, &pG, &pB, &pa);
+ // Min max values calculated for each channel
+ unsigned char minChannel, maxChannel;
+ pR.getMinMaxValue(minChannel, maxChannel);
+ min.R = minChannel;
+ max.R = maxChannel;
+
+ pG.getMinMaxValue(minChannel, maxChannel);
+ min.G = minChannel;
+ max.G = maxChannel;
+
+ pB.getMinMaxValue(minChannel, maxChannel);
+ min.B = minChannel;
+ max.B = maxChannel;
+
+ pa.getMinMaxValue(minChannel, maxChannel);
+ min.A = minChannel;
+ max.A = maxChannel;
+
+ // Construct the look-up table
+ vpRGBa lut[256];
+ unsigned char rangeR = max.R - min.R;
+ if (rangeR > 0) {
+ for (unsigned int x = min.R; x <= max.R; x++) {
+ lut[x].R = 255 * (x - min.R) / rangeR;
+ }
+ } else {
+ lut[min.R].R = min.R;
+ }
+
+ unsigned char rangeG = max.G - min.G;
+ if (rangeG > 0) {
+ for (unsigned int x = min.G; x <= max.G; x++) {
+ lut[x].G = 255 * (x - min.G) / rangeG;
+ }
+ } else {
+ lut[min.G].G = min.G;
+ }
+
+ unsigned char rangeB = max.B - min.B;
+ if (rangeB > 0) {
+ for (unsigned int x = min.B; x <= max.B; x++) {
+ lut[x].B = 255 * (x - min.B) / rangeB;
+ }
+ } else {
+ lut[min.B].B = min.B;
+ }
+
+ unsigned char rangeA = max.A - min.A;
+ if (rangeA > 0) {
+ for (unsigned int x = min.A; x <= max.A; x++) {
+ lut[x].A = 255 * (x - min.A) / rangeA;
+ }
+ } else {
+ lut[min.A].A = min.A;
+ }
+
+ I.performLut(lut);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a color image.
+
+ \param I1 : The first input color image.
+ \param I2 : The second output color image.
+*/
+void vp::stretchContrast(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2)
+{
+ // Copy I1 to I2
+ I2 = I1;
+ vp::stretchContrast(I2);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a color image in the HSV color space.
+ The saturation and value components are stretch so the hue is preserved.
+
+ \param I : The color image to stetch the contrast in the HSV color space.
+*/
+void vp::stretchContrastHSV(vpImage<vpRGBa> &I)
+{
+ unsigned int size = I.getWidth() * I.getHeight();
+
+ // Convert RGB to HSV
+ vpImage<double> 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);
+
+ // Find min and max Saturation and Value
+ double minSaturation, maxSaturation, minValue, maxValue;
+ saturationImage.getMinMaxValue(minSaturation, maxSaturation);
+ valueImage.getMinMaxValue(minValue, maxValue);
+
+ double *ptrStart = saturationImage.bitmap;
+ double *ptrEnd = saturationImage.bitmap + size;
+ double *ptrCurrent = ptrStart;
+
+ // Stretch Saturation
+ if (maxSaturation - minSaturation > 0.0) {
+ while (ptrCurrent != ptrEnd) {
+ *ptrCurrent = (*ptrCurrent - minSaturation) / (maxSaturation - minSaturation);
+ ++ptrCurrent;
+ }
+ }
+
+ // Stretch Value
+ if (maxValue - minValue > 0.0) {
+ ptrStart = valueImage.bitmap;
+ ptrEnd = valueImage.bitmap + size;
+ ptrCurrent = ptrStart;
+
+ while (ptrCurrent != ptrEnd) {
+ *ptrCurrent = (*ptrCurrent - minValue) / (maxValue - minValue);
+ ++ptrCurrent;
+ }
+ }
+
+ // Convert HSV to RGBa
+ vpImageConvert::HSVToRGBa(hueImage.bitmap, saturationImage.bitmap, valueImage.bitmap, (unsigned char *)I.bitmap,
+ size);
+}
+
+/*!
+ \ingroup group_imgproc_contrast
+
+ Stretch the contrast of a color image in the HSV color space.
+ The saturation and value components are stretch so the hue is preserved.
+
+ \param I1 : The first input color image.
+ \param I2 : The second output color image.
+*/
+void vp::stretchContrastHSV(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2)
+{
+ // Copy I1 to I2
+ I2 = I1;
+ vp::stretchContrastHSV(I2);
+}
+
+/*!
+ \ingroup group_imgproc_sharpening
+
+ Sharpen a grayscale image using the unsharp mask technique.
+
+ \param I : The grayscale image to sharpen.
+ \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<unsigned char> &I, const unsigned int size, const double weight)
+{
+ if (weight < 1.0 && weight >= 0.0) {
+ // Gaussian blurred image
+ vpImage<double> I_blurred;
+ vpImageFilter::gaussianBlur(I, I_blurred, 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<unsigned char>(val); // val > 255 ? 255 : (val < 0 ? 0 : val);
+ }
+ }
+}
+
+/*!
+ \ingroup group_imgproc_sharpening
+
+ Sharpen a grayscale image using the unsharp mask technique.
+
+ \param I1 : The first input grayscale image.
+ \param I2 : The second output grayscale image.
+ \param 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<unsigned char> &I1, vpImage<unsigned char> &I2, const unsigned int size,
+ const double weight)
+{
+ // Copy I1 to I2
+ I2 = I1;
+ vp::unsharpMask(I2, size, weight);
+}
+
+/*!
+ \ingroup group_imgproc_sharpening
+
+ Sharpen a color image using the unsharp mask technique.
+
+ \param I : The color image to sharpen.
+ \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<vpRGBa> &I, const unsigned int size, const double weight)
+{
+ if (weight < 1.0 && weight >= 0.0) {
+ // Gaussian blurred image
+ vpImage<double> I_blurred_R, I_blurred_G, I_blurred_B;
+ vpImage<unsigned char> I_R, I_G, I_B;
+
+ vpImageConvert::split(I, &I_R, &I_G, &I_B);
+ vpImageFilter::gaussianBlur(I_R, I_blurred_R, size);
+ vpImageFilter::gaussianBlur(I_G, I_blurred_G, size);
+ vpImageFilter::gaussianBlur(I_B, I_blurred_B, 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);
+
+ I.bitmap[cpt].R = vpMath::saturate<unsigned char>(val_R);
+ I.bitmap[cpt].G = vpMath::saturate<unsigned char>(val_G);
+ I.bitmap[cpt].B = vpMath::saturate<unsigned char>(val_B);
+ }
+ }
+}
+
+/*!
+ \ingroup group_imgproc_sharpening
+
+ Sharpen a color image using the unsharp mask technique.
+
+ \param I1 : The first input color image.
+ \param I2 : The second output color image.
+ \param 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<vpRGBa> &I1, vpImage<vpRGBa> &I2, const unsigned int size, const double weight)
+{
+ // Copy I1 to I2
+ I2 = I1;
+ vp::unsharpMask(I2, size, weight);
+}
diff --git a/modules/imgproc/src/vpMorph.cpp b/modules/imgproc/src/vpMorph.cpp
new file mode 100644
index 0000000..9653a2c
--- /dev/null
+++ b/modules/imgproc/src/vpMorph.cpp
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Additional image morphology functions.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpMorph.cpp
+ \brief Additional image morphology functions.
+*/
+
+#include <visp3/core/vpImageTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+
+/*!
+ \ingroup group_imgproc_morph
+
+ Fill the holes in a binary image.
+
+ \param I : Input binary image (0 means background, 255 means foreground).
+*/
+void vp::fillHoles(vpImage<unsigned char> &I
+#if USE_OLD_FILL_HOLE
+ ,
+ const vpImageMorphology::vpConnexityType &connexity
+#endif
+)
+{
+ if (I.getSize() == 0) {
+ return;
+ }
+
+#if USE_OLD_FILL_HOLE
+ // Code similar to Matlab imfill(BW,'holes')
+ // Replaced by flood fill as imfill use imreconstruct
+ // and our reconstruct implementation is naive and inefficient
+ // Difference between new and old implementation:
+ // - new implementation allows to set the fill value
+ // - only background==0 is required, before it was 0 (background) / 1
+ // (foreground)
+ // - no more connexity option
+ vpImage<unsigned char> mask(I.getHeight() + 2, I.getWidth() + 2, 255);
+ // Copy I to mask + add border padding + complement
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ mask[i + 1][j + 1] = 255 - I[i][j];
+ }
+ }
+
+ vpImage<unsigned char> marker(I.getHeight() + 2, I.getWidth() + 2, 0);
+ // Create marker with 255 1-pixel border
+ for (unsigned int i = 0; i < marker.getHeight(); i++) {
+ if (i == 0 || i == marker.getHeight() - 1) {
+ for (unsigned int j = 0; j < marker.getWidth(); j++) {
+ marker[i][j] = 255;
+ }
+ } else {
+ marker[i][0] = 255;
+ marker[i][marker.getWidth() - 1] = 255;
+ }
+ }
+
+ vpImage<unsigned char> I_reconstruct;
+ reconstruct(marker, mask, I_reconstruct, connexity);
+
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ I[i][j] = 255 - I_reconstruct[i + 1][j + 1];
+ }
+ }
+#else
+ // Create flood fill mask
+ vpImage<unsigned char> 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());
+ }
+
+ // Perform flood fill
+ vp::floodFill(flood_fill_mask, vpImagePoint(0, 0), 0, 255);
+
+ // Get current mask
+ vpImage<unsigned char> 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());
+ }
+
+ // Get image with holes filled
+ vpImage<unsigned char> I_white(I.getHeight(), I.getWidth(), 255), I_holes;
+ vpImageTools::imageSubtract(I_white, mask, I_holes);
+ vpImageTools::imageAdd(I, I_holes, I, true);
+#endif
+}
+
+/*!
+ \ingroup group_imgproc_morph
+
+ Perform morphological reconstruction of the image \a marker under the image
+ \a mask. Definition from Gleb V. Tcheslavsk: > The morphological
+ reconstruction by dilation of a grayscale image \f$ g \f$ by a grayscale
+ marker image \f$ f \f$ > is defined as the geodesic dilation of \f$ f \f$
+ with respect to \f$ g \f$ repeated (iterated) until stability is reached:
+ \f[
+ R_{g}^{D} \left ( f \right ) = D_{g}^{\left ( k \right )} \left ( f \right
+ ) \f] with \f$ k \f$ such that: \f$ D_{g}^{\left ( k \right )} \left ( f
+ \right ) = D_{g}^{\left ( k+1 \right )} \left ( f \right ) \f$
+
+ \param marker : Grayscale image marker.
+ \param mask : Grayscale image mask.
+ \param h_kp1 : Image morphologically reconstructed.
+ \param connexity : Type of connexity.
+*/
+void vp::reconstruct(const vpImage<unsigned char> &marker, const vpImage<unsigned char> &mask,
+ vpImage<unsigned char> &h_kp1 /*alias I */, const vpImageMorphology::vpConnexityType &connexity)
+{
+ if (marker.getHeight() != mask.getHeight() || marker.getWidth() != mask.getWidth()) {
+ std::cerr << "marker.getHeight() != mask.getHeight() || "
+ "marker.getWidth() != mask.getWidth()"
+ << std::endl;
+ return;
+ }
+
+ if (marker.getSize() == 0) {
+ std::cerr << "Input images are empty!" << std::endl;
+ return;
+ }
+
+ vpImage<unsigned char> h_k = marker;
+ h_kp1 = h_k;
+
+ do {
+ // Dilatation
+ vpImageMorphology::dilatation(h_kp1, connexity);
+
+ // Keep min
+ for (unsigned int i = 0; i < h_kp1.getHeight(); i++) {
+ for (unsigned int j = 0; j < h_kp1.getWidth(); j++) {
+ h_kp1[i][j] = std::min(h_kp1[i][j], mask[i][j]);
+ }
+ }
+
+ if (h_kp1 == h_k) {
+ break;
+ }
+
+ h_k = h_kp1;
+ } while (true);
+}
diff --git a/modules/imgproc/src/vpRetinex.cpp b/modules/imgproc/src/vpRetinex.cpp
new file mode 100644
index 0000000..ccb8d91
--- /dev/null
+++ b/modules/imgproc/src/vpRetinex.cpp
@@ -0,0 +1,305 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Convert image types.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+/* Retinex_.java Using ImageJ Gaussian Filter
+ * Retinex filter algorithm based on the plugin for GIMP.
+ *
+ *@author Jimenez-Hernandez Francisco <jimenezf at fi.uaemex.mx>
+ *Developed at Birmingham University, School of Dentistry. Supervised by
+ *Gabriel Landini
+ *@version 1.0
+ *
+ * 8 July 2010
+ *
+ * This version uses ImageJ Gaussian blurring instead of GIMP's linear
+ *Gaussian because there is a bug in GIMP's implementation that shifts the
+ *results of the blurring to the right of the image when using more than 3
+ *scales.
+ *
+ * Based on:
+ * MSRCR Retinex
+ * (Multi-Scale Retinex with Color Restoration)
+ * 2003 Fabien Pelisson <Fabien.Pelisson at inrialpes.fr>
+ * Retinex GIMP plug-in
+ *
+ * Copyright (C) 2009 MAO Y.B
+ * 2009. 3. 3
+ * Visual Information Processing (VIP) Group, NJUST
+ *
+ * D. J. Jobson, Z. Rahman, and G. A. Woodell. A multi-scale
+ * Retinex for bridging the gap between color images and the
+ * human observation of scenes. IEEE Transactions on Image Processing,
+ * 1997, 6(7): 965-976
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+/*!
+ \file vpRetinex.cpp
+ \brief Retinex algorithm
+*/
+
+#include <functional>
+#include <numeric>
+
+#include <visp3/core/vpImageFilter.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/imgproc/vpImgproc.h>
+
+#define MAX_RETINEX_SCALES 8
+
+std::vector<double> retinexScalesDistribution(const int scaleDiv, const int level, const int scale)
+{
+ std::vector<double> scales(MAX_RETINEX_SCALES);
+
+ if (scaleDiv == 1) {
+ scales[0] = scale / 2.0;
+ } else if (scaleDiv == 2) {
+ scales[0] = scale / 2.0;
+ scales[1] = scale;
+ } else {
+ double size_step = scale / (double)scaleDiv;
+ int i;
+
+ switch (level) {
+ case vp::RETINEX_UNIFORM:
+ for (i = 0; i < scaleDiv; i++) {
+ scales[(size_t)i] = 2.0 + i * size_step;
+ }
+ break;
+
+ 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));
+ }
+ 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));
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ return scales;
+}
+
+// See: http://imagej.net/Retinex and
+// https://docs.gimp.org/en/plug-in-retinex.html
+void MSRCR(vpImage<vpRGBa> &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<double> retinexScales = retinexScalesDistribution(scaleDiv, level, _scale);
+
+ // Filtering according to the various scales.
+ // Summarize the results of the various filters according to a specific
+ // weight(here equivalent for all).
+ double weight = 1.0 / (double)scaleDiv;
+
+ std::vector<vpImage<double> > doubleRGB(3);
+ std::vector<vpImage<double> > doubleResRGB(3);
+ unsigned int size = I.getSize();
+
+ int kernelSize = _kernelSize;
+ if (kernelSize == -1) {
+ // Compute the kernel size from the input image size
+ kernelSize = (int)(std::min(I.getWidth(), I.getHeight()) / 2.0);
+ kernelSize = (kernelSize - kernelSize % 2) + 1;
+ }
+
+ for (int channel = 0; channel < 3; channel++) {
+ doubleRGB[(size_t)channel] = vpImage<double>(I.getHeight(), I.getWidth());
+ doubleResRGB[(size_t)channel] = vpImage<double>(I.getHeight(), I.getWidth());
+
+ for (unsigned int cpt = 0; cpt < size; cpt++) {
+ // Shift the pixel values by 1 to avoid problem with log(0)
+ switch (channel) {
+ case 0:
+ doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].R + 1.0;
+ break;
+
+ case 1:
+ doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].G + 1.0;
+ break;
+
+ case 2:
+ doubleRGB[(size_t)channel].bitmap[cpt] = I.bitmap[cpt].B + 1.0;
+ break;
+
+ default:
+ break;
+ }
+ }
+
+ for (int sc = 0; sc < scaleDiv; sc++) {
+ vpImage<double> blurImage;
+ double sigma = retinexScales[(size_t)sc];
+ 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]));
+ }
+ }
+ }
+
+ std::vector<double> dest(size * 3);
+ 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));
+
+ dest[cpt * 3] = gain * (std::log(alpha * doubleRGB[0].bitmap[cpt]) - logl) * doubleResRGB[0].bitmap[cpt] + offset;
+ dest[cpt * 3 + 1] =
+ gain * (std::log(alpha * doubleRGB[1].bitmap[cpt]) - logl) * doubleResRGB[1].bitmap[cpt] + offset;
+ dest[cpt * 3 + 2] =
+ gain * (std::log(alpha * doubleRGB[2].bitmap[cpt]) - logl) * doubleResRGB[2].bitmap[cpt] + offset;
+ }
+
+ double sum = std::accumulate(dest.begin(), dest.end(), 0.0);
+ double mean = sum / dest.size();
+
+ std::vector<double> diff(dest.size());
+ std::transform(dest.begin(), dest.end(), diff.begin(), std::bind2nd(std::minus<double>(), 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;
+ double maxi = mean + dynamic * stdev;
+ double range = maxi - mini;
+
+ if (vpMath::nul(range)) {
+ range = 1.0;
+ }
+
+ for (unsigned int cpt = 0; cpt < size; cpt++) {
+ I.bitmap[cpt].R = vpMath::saturate<unsigned char>((255.0 * (dest[cpt * 3 + 0] - mini) / range));
+ I.bitmap[cpt].G = vpMath::saturate<unsigned char>((255.0 * (dest[cpt * 3 + 1] - mini) / range));
+ I.bitmap[cpt].B = vpMath::saturate<unsigned char>((255.0 * (dest[cpt * 3 + 2] - mini) / range));
+ }
+}
+
+/*!
+ \ingroup group_imgproc_retinex
+
+ Apply the Retinex algorithm (the input image is modified).
+ \param I : The color image after application of the Retinex technique.
+ \param scale : Specifies the depth of the retinex effect. Minimum value is
+ 16, a value providing gross, unrefined filtering. Maximum value is 250.
+ Optimal and default value is 240. \param scaleDiv : Specifies the number of
+ iterations of the multiscale filter. Values larger than 2 exploit the
+ "multiscale" nature of the algorithm. \param level : Specifies distribution
+ of the Gaussian blurring kernel sizes for Scale division values > 2:
+ - 0, tends to treat all image intensities similarly,
+ - 1, enhances dark regions of the image,
+ - 2, enhances the bright regions of the image.
+ \param dynamic : Adjusts the color of the result. Large values produce less
+ saturated images. \param kernelSize : Kernel size for the gaussian blur
+ operation. If -1, the kernel size is calculated from the image size.
+*/
+void vp::retinex(vpImage<vpRGBa> &I, const int scale, const int scaleDiv, const int level, const double dynamic,
+ const int kernelSize)
+{
+ // Assert scale
+ if (scale < 16 || scale > 250) {
+ std::cerr << "Scale must be between the interval [16 - 250]" << std::endl;
+ return;
+ }
+
+ // Assert scaleDiv
+ if (scaleDiv < 1 || scaleDiv > 8) {
+ std::cerr << "Scale division must be between the interval [1 - 8]" << std::endl;
+ return;
+ }
+
+ if (I.getWidth() * I.getHeight() == 0) {
+ return;
+ }
+
+ MSRCR(I, scale, scaleDiv, level, dynamic, kernelSize);
+}
+
+/*!
+ \ingroup group_imgproc_retinex
+
+ Apply the Retinex algorithm.
+ \param I1 : The input color image.
+ \param I2 : The output color image after application of the Retinex
+ technique. \param scale : Specifies the depth of the retinex effect. Minimum
+ value is 16, a value providing gross, unrefined filtering. Maximum value is
+ 250. Optimal and default value is 240. \param scaleDiv : Specifies the
+ number of iterations of the multiscale filter. Values larger than 2 exploit
+ the "multiscale" nature of the algorithm. \param level : Specifies
+ distribution of the Gaussian blurring kernel sizes for Scale division values
+ > 2:
+ - 0, tends to treat all image intensities similarly,
+ - 1, enhances dark regions of the image,
+ - 2, enhances the bright regions of the image.
+ \param dynamic : Adjusts the color of the result. Large values produce less
+ saturated images. \param kernelSize : Kernel size for the gaussian blur
+ operation. If -1, the kernel size is calculated from the image size.
+*/
+void vp::retinex(const vpImage<vpRGBa> &I1, vpImage<vpRGBa> &I2, const int scale, const int scaleDiv, const int level,
+ const double dynamic, const int kernelSize)
+{
+ I2 = I1;
+ vp::retinex(I2, scale, scaleDiv, level, dynamic, kernelSize);
+}
diff --git a/modules/imgproc/src/vpThreshold.cpp b/modules/imgproc/src/vpThreshold.cpp
new file mode 100644
index 0000000..189e9aa
--- /dev/null
+++ b/modules/imgproc/src/vpThreshold.cpp
@@ -0,0 +1,421 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Automatic thresholding functions.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpThreshold.cpp
+ \brief Automatic thresholding functions.
+*/
+
+#include <visp3/core/vpHistogram.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+
+namespace
+{
+bool isBimodal(const std::vector<float> &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]) {
+ modes++;
+ }
+
+ if (modes > 2) {
+ return false;
+ }
+ }
+
+ return (modes == 2);
+}
+
+int computeThresholdHuang(const vpHistogram &hist)
+{
+ // Code ported from the AutoThreshold ImageJ plugin:
+ // Implements Huang's fuzzy thresholding method
+ // Uses Shannon's entropy function (one can also use Yager's entropy
+ // function) Huang L.-K. and Wang M.-J.J. (1995) "Image Thresholding by
+ // Minimizing the Measures of Fuzziness" Pattern Recognition, 28(1): 41-51
+ // Reimplemented (to handle 16-bit efficiently) by Johannes Schindelin Jan
+ // 31, 2011
+
+ // 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++) {
+ // do nothing
+ }
+
+ for (last = (size_t)hist.getSize() - 1; last > first && hist[(unsigned char)last] == 0; last--) {
+ // do nothing
+ }
+
+ if (first == last) {
+ return 0;
+ }
+
+ // Calculate the cumulative density and the weighted cumulative density
+ std::vector<float> S(last + 1);
+ std::vector<float> W(last + 1);
+
+ S[0] = (float)hist[0];
+ W[0] = 0.0f;
+ for (size_t i = std::max((size_t)1, first); i <= last; i++) {
+ S[i] = S[i - 1] + hist[(unsigned char)i];
+ W[i] = W[i - 1] + i * (float)hist[(unsigned char)i];
+ }
+
+ // Precalculate the summands of the entropy given the absolute difference x
+ // - mu (integral)
+ float C = (float)(last - first);
+ std::vector<float> Smu(last + 1 - first);
+
+ for (size_t i = 1; i < Smu.size(); i++) {
+ float mu = 1 / (1 + i / C);
+ Smu[i] = -mu * std::log(mu) - (1 - mu) * std::log(1 - mu);
+ }
+
+ // Calculate the threshold
+ int bestThreshold = 0;
+ float bestEntropy = std::numeric_limits<float>::max();
+
+ for (size_t threshold = first; threshold <= last; threshold++) {
+ float entropy = 0;
+ int mu = vpMath::round(W[threshold] / S[threshold]);
+ for (size_t i = first; i <= threshold; i++) {
+ entropy += Smu[(size_t)std::abs((int)i - mu)] * hist[(unsigned char)i];
+ }
+
+ mu = vpMath::round((W[last] - W[threshold]) / (S[last] - S[threshold]));
+ for (size_t i = threshold + 1; i <= last; i++) {
+ entropy += Smu[(size_t)std::abs((int)i - mu)] * hist[(unsigned char)i];
+ }
+
+ if (bestEntropy > entropy) {
+ bestEntropy = entropy;
+ bestThreshold = (int)threshold;
+ }
+ }
+
+ return bestThreshold;
+}
+
+int computeThresholdIntermodes(const vpHistogram &hist)
+{
+ if (hist.getSize() < 3) {
+ return -1;
+ }
+
+ // Code based on the AutoThreshold ImageJ plugin:
+ // J. M. S. Prewitt and M. L. Mendelsohn, "The analysis of cell images," in
+ // Annals of the New York Academy of Sciences, vol. 128, pp. 1035-1053,
+ // 1966. ported to ImageJ plugin by G.Landini from Antti Niemisto's Matlab
+ // code (GPL) Original Matlab code Copyright (C) 2004 Antti Niemisto See
+ // http://www.cs.tut.fi/~ant/histthresh/ for an excellent slide presentation
+ // and the original Matlab code.
+ //
+ // Assumes a bimodal histogram. The histogram needs is smoothed (using a
+ // running average of size 3, iteratively) until there are only two local
+ // maxima. j and k Threshold t is (j+k)/2. Images with histograms having
+ // extremely unequal peaks or a broad and flat valley are unsuitable for this
+ // method.
+
+ std::vector<float> hist_float(hist.getSize());
+ for (unsigned int cpt = 0; cpt < hist.getSize(); cpt++) {
+ hist_float[cpt] = (float)hist[cpt];
+ }
+
+ int iter = 0;
+ 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;
+ }
+
+ // 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;
+
+ iter++;
+
+ if (iter > 10000) {
+ std::cerr << "Intermodes Threshold not found after 10000 iterations!" << std::endl;
+ return -1;
+ }
+ }
+
+ // 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]) {
+ // Mode
+ tt += (int)cpt;
+ }
+ }
+
+ return (int)std::floor(tt / 2.0); // vpMath::round(tt / 2.0);
+}
+
+int computeThresholdIsoData(const vpHistogram &hist, const unsigned int imageSize)
+{
+ int threshold = 0;
+
+ // Code based on BSD Matlab isodata implementation by zephyr
+ // STEP 1: Compute mean intensity of image from histogram, set T=mean(I)
+ std::vector<float> cumsum(hist.getSize(), 0.0f);
+ std::vector<float> sum_ip(hist.getSize(), 0.0f);
+ cumsum[0] = (float)hist[0];
+ for (unsigned int cpt = 1; cpt < hist.getSize(); cpt++) {
+ sum_ip[cpt] = cpt * (float)hist[cpt] + sum_ip[cpt - 1];
+ cumsum[cpt] = (float)hist[cpt] + cumsum[cpt - 1];
+ }
+
+ int T = vpMath::round(sum_ip[255] / imageSize);
+
+ // 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)]);
+
+ 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)]);
+
+ T = T2;
+ T2 = vpMath::round((MAT + MBT) / 2.0f);
+ threshold = T2;
+ }
+
+ return threshold;
+}
+
+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.
+ // The threshold is the mean of the greyscale data
+ float sum_ip = 0.0f;
+ for (unsigned int cpt = 0; cpt < hist.getSize(); cpt++) {
+ sum_ip += cpt * (float)hist[cpt];
+ }
+
+ return (int)std::floor(sum_ip / 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,
+ // doi:10.1109/TSMC.1979.4310076
+
+ float mu_T = 0.0f;
+ float sum_ip_all[256];
+ for (int cpt = 0; cpt < (int)hist.getSize(); cpt++) {
+ mu_T += cpt * (float)hist[cpt];
+ sum_ip_all[cpt] = mu_T;
+ }
+
+ // Weight Background / Foreground
+ float w_B = 0.0f, w_F = 0.0f;
+
+ float max_sigma_b = 0.0f;
+ int threshold = 0;
+
+ for (int cpt = 0; cpt < 256; cpt++) {
+ w_B += hist[cpt];
+ if (vpMath::nul(w_B, std::numeric_limits<float>::epsilon())) {
+ continue;
+ }
+
+ w_F = (int)imageSize - w_B;
+ if (vpMath::nul(w_F, std::numeric_limits<float>::epsilon())) {
+ break;
+ }
+
+ // Mean Background / Foreground
+ float mu_B = sum_ip_all[cpt] / (float)w_B;
+ float mu_F = (mu_T - sum_ip_all[cpt]) / (float)w_F;
+ // If there is a case where (w_B * w_F) exceed FLT_MAX, normalize
+ // histogram
+ float sigma_b_sqr = w_B * w_F * (mu_B - mu_F) * (mu_B - mu_F);
+
+ if (sigma_b_sqr >= max_sigma_b) {
+ threshold = cpt;
+ max_sigma_b = sigma_b_sqr;
+ }
+ }
+
+ return threshold;
+}
+
+int computeThresholdTriangle(vpHistogram &hist)
+{
+ int threshold = 0;
+
+ // Zack, G. W., Rogers, W. E. and Latt, S. A., 1977,
+ // Automatic Measurement of Sister Chromatid Exchange Frequency,
+ // Journal of Histochemistry and Cytochemistry 25 (7), pp. 741-753
+
+ int left_bound = -1, right_bound = -1, max_idx = -1, max_value = 0;
+ // Find max value index and left / right most index
+ for (int cpt = 0; cpt < (int)hist.getSize(); cpt++) {
+ if (left_bound == -1 && hist[cpt] > 0) {
+ left_bound = (int)cpt;
+ }
+
+ if (right_bound == -1 && hist[(int)hist.getSize() - 1 - cpt] > 0) {
+ right_bound = (int)hist.getSize() - 1 - cpt;
+ }
+
+ if ((int)hist[cpt] > max_value) {
+ max_value = (int)hist[cpt];
+ max_idx = cpt;
+ }
+ }
+
+ // 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;
+
+ // Use the largest bound
+ bool flip = false;
+ if (max_idx - left_bound < right_bound - max_idx) {
+ // Flip histogram to get the largest bound to the left
+ flip = true;
+
+ int cpt_left = 0, cpt_right = (int)hist.getSize() - 1;
+ for (; cpt_left < cpt_right; cpt_left++, cpt_right--) {
+ unsigned int temp = hist[cpt_left];
+ hist.set(cpt_left, hist[cpt_right]);
+ hist.set(cpt_right, temp);
+ }
+
+ left_bound = (int)hist.getSize() - 1 - right_bound;
+ max_idx = (int)hist.getSize() - 1 - max_idx;
+ }
+
+ // Distance from a point to a line defined by two points:
+ //\textbf{distance} \left ( P_1, P_2, \left ( x_0,y_0 \right ) \right )
+ // = \frac{ \left | \left ( y_2-y_1 \right ) x_0 - \left ( x_2-x_1 \right )
+ // y_0 + x_2 y_1 - y_2 x_1 \right | }
+ // { \sqrt{ \left ( y_2 - y_1 \right )^{2} + \left ( x_2 - x_1 \right
+ // )^{2} } }
+ // Constants are ignored
+ float a = (float)max_value; // y_2 - y_1
+ float b = (float)(left_bound - max_idx); //-(x_2 - x_1)
+ float max_dist = 0.0f;
+
+ for (int cpt = left_bound + 1; cpt <= max_idx; cpt++) {
+ float dist = a * cpt + b * hist[cpt];
+
+ if (dist > max_dist) {
+ max_dist = dist;
+ threshold = cpt;
+ }
+ }
+ threshold--;
+
+ if (flip) {
+ threshold = (int)hist.getSize() - 1 - threshold;
+ }
+
+ return threshold;
+}
+} // namespace
+
+/*!
+ \ingroup group_imgproc_threshold
+
+ Automatic thresholding.
+
+ \param I : Input grayscale image.
+ \param method : Automatic thresholding method.
+ \param backgroundValue : Value to set to the background.
+ \param foregroundValue : Value to set to the foreground.
+*/
+unsigned char vp::autoThreshold(vpImage<unsigned char> &I, const vpAutoThresholdMethod &method,
+ const unsigned char backgroundValue, const unsigned char foregroundValue)
+{
+ if (I.getSize() == 0) {
+ return 0;
+ }
+
+ // Compute image histogram
+ vpHistogram histogram(I);
+ int threshold = -1;
+
+ switch (method) {
+ case AUTO_THRESHOLD_HUANG:
+ threshold = computeThresholdHuang(histogram);
+ break;
+
+ case AUTO_THRESHOLD_INTERMODES:
+ threshold = computeThresholdIntermodes(histogram);
+ break;
+
+ case AUTO_THRESHOLD_ISODATA:
+ threshold = computeThresholdIsoData(histogram, I.getSize());
+ break;
+
+ case AUTO_THRESHOLD_MEAN:
+ threshold = computeThresholdMean(histogram, I.getSize());
+ break;
+
+ case AUTO_THRESHOLD_OTSU:
+ threshold = computeThresholdOtsu(histogram, I.getSize());
+ break;
+
+ case AUTO_THRESHOLD_TRIANGLE:
+ threshold = computeThresholdTriangle(histogram);
+ break;
+
+ default:
+ break;
+ }
+
+ if (threshold != -1) {
+ // Threshold
+ 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
new file mode 100644
index 0000000..7307dc0
--- /dev/null
+++ b/modules/imgproc/test/testAutoThreshold.cpp
@@ -0,0 +1,310 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test automatic thresholding.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testAutoThreshold.cpp
+
+ \brief Test automatic thresholding.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test automatic thresholding.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ output result images are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+// Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit(EXIT_FAILURE);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ } catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ filename = vpIoTools::createFilePath(ipath, "calibration/grid36-03.pgm");
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, filename);
+ std::cout << "Read: " << filename << " (" << I.getWidth() << "x" << I.getHeight() << ")" << std::endl;
+
+ vpImage<unsigned char> I_thresh = I;
+
+ // Huang
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_huang.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ // Intermodes
+ I_thresh = I;
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_intermodes.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ // IsoData
+ I_thresh = I;
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_isodata.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ // Mean
+ I_thresh = I;
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_mean.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ // Otsu
+ I_thresh = I;
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_otsu.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ // Triangle
+ I_thresh = I;
+ 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;
+
+ filename = vpIoTools::createFilePath(opath, "grid36-03_auto_thresh_Triangle.pgm");
+ vpImageIo::write(I_thresh, filename);
+ std::cout << "Write: " << filename << std::endl;
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/imgproc/test/testConnectedComponents.cpp b/modules/imgproc/test/testConnectedComponents.cpp
new file mode 100644
index 0000000..d5fb6ed
--- /dev/null
+++ b/modules/imgproc/test/testConnectedComponents.cpp
@@ -0,0 +1,403 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test connected components.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+#include <map>
+#include <set>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testConnectedComponents.cpp
+
+ \brief Test connected components.
+*/
+
+// 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);
+bool checkLabels(const vpImage<int> &label1, const vpImage<int> &label2);
+
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test connected components.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ output result images are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+bool checkLabels(const vpImage<int> &label1, const vpImage<int> &label2)
+{
+ if (label1.getHeight() != label2.getHeight() || label1.getWidth() != label2.getWidth())
+ return false;
+
+ std::map<int, std::vector<vpImagePoint> > 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)) {
+ std::cerr << "label1[i][j] > 0 && label2[i][j] == 0 || label1[i][j] "
+ "== 0 && label2[i][j] > 0"
+ << std::endl;
+ return false;
+ }
+
+ if (label1[i][j])
+ map_label1[label1[i][j]].push_back(vpImagePoint(i, j));
+
+ if (label2[i][j])
+ map_label2[label2[i][j]].push_back(vpImagePoint(i, j));
+ }
+ }
+
+ if (map_label1.size() != map_label2.size()) {
+ std::cerr << "map_label1.size() != map_label2.size()" << std::endl;
+ return false;
+ }
+
+ for (std::map<int, std::vector<vpImagePoint> >::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();
+ int lab2 = label2[i][j];
+
+ std::vector<vpImagePoint>::const_iterator it_pt1 = it1->second.begin();
+ for (; it_pt1 != it1->second.end(); ++it_pt1) {
+ i = (unsigned int)it_pt1->get_i();
+ j = (unsigned int)it_pt1->get_j();
+ if (label2[i][j] != lab2) {
+ std::cerr << "label2[i][j] != lab2" << std::endl;
+ return false;
+ }
+ }
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+// Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit(EXIT_FAILURE);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ } catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ // Read Klimt.ppm
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImage<unsigned char> 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,
+ (unsigned char)255);
+ std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() << std::endl;
+
+ vpImage<int> labels_connex4;
+ int nbComponents = 0;
+ double t = vpTime::measureTimeMs();
+ 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;
+ std::cout << "nbComponents=" << nbComponents << std::endl;
+
+ vpImage<int> labels_connex8;
+ t = vpTime::measureTimeMs();
+ 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<vpRGBa> 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);
+ }
+ }
+ }
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_connected_components_4.ppm");
+ vpImageIo::write(labels_connex4_color, filename);
+
+ vpImage<vpRGBa> 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);
+ }
+ }
+ }
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_connected_components_8.ppm");
+ vpImageIo::write(labels_connex8_color, filename);
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ cv::Mat matImg;
+ vpImageConvert::convert(I, matImg);
+
+ cv::Mat matLabels_4;
+ double t_opencv = vpTime::measureTimeMs();
+ cv::connectedComponents(matImg, matLabels_4, 4);
+ t_opencv = vpTime::measureTimeMs() - t_opencv;
+
+ std::set<int> set_labels_connex4_opencv;
+ vpImage<int> 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<int>(i, j);
+
+ if (matLabels_4.at<int>(i, j))
+ set_labels_connex4_opencv.insert(matLabels_4.at<int>(i, j));
+ }
+ }
+
+ 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;
+ bool check_label = checkLabels(labels_connex4_opencv, labels_connex4);
+ 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)");
+ }
+
+ cv::Mat matLabels_8;
+ t_opencv = vpTime::measureTimeMs();
+ cv::connectedComponents(matImg, matLabels_8, 8);
+ t_opencv = vpTime::measureTimeMs() - t_opencv;
+
+ std::set<int> set_labels_connex8_opencv;
+ vpImage<int> 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<int>(i, j);
+
+ if (matLabels_8.at<int>(i, j))
+ set_labels_connex8_opencv.insert(matLabels_8.at<int>(i, j));
+ }
+ }
+
+ std::cout << "\n8-connexity connected components (OpenCV):" << 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 << "(labels_connex8_opencv == labels_connex8)? " <<
+ // (labels_connex8_opencv == labels_connex8) << std::endl;
+
+ if (!check_label) {
+ throw vpException(vpException::fatalError, "(labels_connex8_opencv != labels_connex8)");
+ }
+#endif
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/imgproc/test/testContours.cpp b/modules/imgproc/test/testContours.cpp
new file mode 100644
index 0000000..9727dee
--- /dev/null
+++ b/modules/imgproc/test/testContours.cpp
@@ -0,0 +1,439 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test contours extraction.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iomanip>
+
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testContours.cpp
+
+ \brief Test contours extraction.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test contours extraction.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ output result images are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+void printImage(const vpImage<unsigned char> &I, const std::string &name)
+{
+ std::cout << "\n" << name << ":" << std::endl;
+
+ std::cout << " ";
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ std::cout << std::setfill(' ') << std::setw(2) << j << " ";
+ }
+ std::cout << std::endl;
+
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ 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<unsigned int>(I[i][j]) << " ";
+ }
+
+ std::cout << std::endl;
+ }
+}
+
+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::endl;
+ std::cout << "\tnb children: " << contour.m_children.size() << std::endl;
+
+ for (std::vector<vp::vpContour *>::const_iterator it = contour.m_children.begin(); it != contour.m_children.end();
+ ++it) {
+ displayContourInfo(**it, level + 1);
+ }
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+// Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit(EXIT_FAILURE);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ } catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ 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};
+
+ vpImage<unsigned char> I_test_data(image_data, 14, 10, true);
+ std::cout << "Test with image data:" << std::endl;
+ printImage(I_test_data, "I_test_data");
+
+ vp::vpContour vp_contours;
+ std::vector<std::vector<vpImagePoint> > contours;
+ double t = vpTime::measureTimeMs();
+ vp::findContours(I_test_data, vp_contours, contours);
+ t = vpTime::measureTimeMs() - t;
+
+ displayContourInfo(vp_contours, 0);
+ std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t << " ms" << std::endl;
+
+ // Read Klimt.ppm
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImage<unsigned char> 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,
+ (unsigned char)1);
+
+ vpImage<unsigned char> 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");
+ vpImageIo::write(I2, filename);
+
+ t = vpTime::measureTimeMs();
+ vp::findContours(I, vp_contours, contours);
+ t = vpTime::measureTimeMs() - t;
+
+ displayContourInfo(vp_contours, 0);
+ std::cout << "\nTest with Klimt image:" << std::endl;
+ std::cout << "ViSP: nb contours=" << contours.size() << " ; t=" << t << " ms" << std::endl;
+
+ // Draw and save
+ vpImage<unsigned char> I_draw_contours(I2.getHeight(), I2.getWidth(), 0);
+ vp::drawContours(I_draw_contours, contours);
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted.pgm");
+ vpImageIo::write(I_draw_contours, filename);
+
+ vpImage<vpRGBa> 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");
+ vpImageIo::write(I_draw_contours_color, filename);
+
+ // Test retrieve list
+ vp::findContours(I, vp_contours, contours, vp::CONTOUR_RETR_LIST);
+ vpImage<unsigned char> I_draw_contours_list(I2.getHeight(), I2.getWidth(), 0);
+
+ vpImage<unsigned char> I_tmp_list(I.getHeight(), I.getWidth(), 0);
+ vp::drawContours(I_tmp_list, contours);
+
+ contours.clear();
+ for (std::vector<vp::vpContour *>::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;
+
+ 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<unsigned char> I_draw_contours_external(I2.getHeight(), I2.getWidth(), 0);
+
+ vpImage<unsigned char> I_tmp_external(I.getHeight(), I.getWidth(), 0);
+ vp::drawContours(I_tmp_external, contours);
+
+ contours.clear();
+ for (std::vector<vp::vpContour *>::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;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_external.pgm");
+ vpImageIo::write(I_draw_contours_external, filename);
+
+ // Test fillHoles
+ vpImage<unsigned char> I_holes = I_draw_contours_external;
+ vpImageTools::binarise(I_holes, (unsigned char)127, (unsigned char)255, (unsigned char)0, (unsigned char)255,
+ (unsigned char)255);
+
+ t = vpTime::measureTimeMs();
+ vp::fillHoles(I_holes);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "\nFill Holes: " << t << " ms" << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_contours_extracted_external_fill_holes.pgm");
+ vpImageIo::write(I_holes, filename);
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ cv::Mat matImg;
+ vpImageConvert::convert(I, matImg);
+
+ std::vector<std::vector<cv::Point> > contours_opencv;
+ double t_opencv = vpTime::measureTimeMs();
+ 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;
+
+ vpImage<unsigned char> I_draw_contours_opencv(I.getHeight(), I.getWidth(), 0);
+ for (std::vector<std::vector<cv::Point> >::const_iterator it1 = contours_opencv.begin();
+ it1 != contours_opencv.end(); ++it1) {
+ for (std::vector<cv::Point>::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;
+
+ 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);
+
+ I_draw_contours_opencv = 0;
+ for (std::vector<std::vector<cv::Point> >::const_iterator it1 = contours_opencv.begin();
+ it1 != contours_opencv.end(); ++it1) {
+ for (std::vector<cv::Point>::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;
+
+ // Test retrieve external
+ vpImageConvert::convert(I, matImg);
+ contours_opencv.clear();
+ cv::findContours(matImg, contours_opencv, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE);
+
+ I_draw_contours_opencv = 0;
+ for (std::vector<std::vector<cv::Point> >::const_iterator it1 = contours_opencv.begin();
+ it1 != contours_opencv.end(); ++it1) {
+ for (std::vector<cv::Point>::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;
+#endif
+
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/imgproc/test/testFloodFill.cpp b/modules/imgproc/test/testFloodFill.cpp
new file mode 100644
index 0000000..1697b7c
--- /dev/null
+++ b/modules/imgproc/test/testFloodFill.cpp
@@ -0,0 +1,383 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test flood fill algorithm.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <iomanip>
+
+#include <visp3/core/vpImageTools.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/imgproc/vpImgproc.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testFloodFill.cpp
+
+ \brief Test flood fill algorithm.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test flood fill algorithm.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ output result images are written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+void printImage(const vpImage<unsigned char> &I, const std::string &name)
+{
+ std::cout << "\n" << name << ":" << std::endl;
+
+ std::cout << " ";
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ std::cout << std::setfill(' ') << std::setw(2) << j << " ";
+ }
+ std::cout << std::endl;
+
+ std::cout << std::setfill(' ') << std::setw(3) << "+";
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
+ std::cout << std::setw(3) << "---";
+ }
+ std::cout << std::endl;
+
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ 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<unsigned int>(I[i][j]) << " ";
+ }
+
+ std::cout << std::endl;
+ }
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+// Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ exit(EXIT_FAILURE);
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ } catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+ exit(EXIT_FAILURE);
+ }
+
+ //
+ // 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<unsigned char> I_test_flood_fill_4_connexity(image_data, 8, 8, true);
+ vpImage<unsigned char> 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<unsigned char> 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<unsigned char> 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");
+
+ if (I_test_flood_fill_4_connexity != I_check_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;
+
+ // 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");
+
+ if (I_test_flood_fill_8_connexity != I_check_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;
+
+ // Read Klimt.ppm
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.pgm");
+ vpImage<unsigned char> I_klimt;
+ vpImageIo::read(I_klimt, filename);
+ 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,
+ (unsigned char)255);
+
+ int seed_x = 0;
+ int seed_y = 0;
+
+ vpImage<unsigned char> 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);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Flood fill on Klimt image (4-connexity): " << t << " ms" << std::endl;
+
+ filename = vpIoTools::createFilePath(opath, "Klimt_flood_fill_4_connexity.pgm");
+ vpImageIo::write(I_klimt_flood_fill_4_connexity, filename);
+
+ vpImage<unsigned char> 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);
+ 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");
+ vpImageIo::write(I_klimt_flood_fill_8_connexity, filename);
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020408
+ cv::Mat matImg_klimt_4_connexity, matImg_klimt_8_connexity;
+ vpImageConvert::convert(I_klimt, matImg_klimt_4_connexity);
+ vpImageConvert::convert(I_klimt, matImg_klimt_8_connexity);
+
+ // 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);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "OpenCV flood fill on Klimt image (4-connexity): " << t << " ms" << std::endl;
+
+ vpImage<unsigned char> 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");
+ 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);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "OpenCV flood fill on Klimt image (8-connexity): " << t << " ms" << std::endl;
+
+ vpImage<unsigned char> 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");
+ 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;
+ 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;
+
+ 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)");
+ }
+#endif
+
+ std::cout << "\nTest flood fill is ok!" << std::endl;
+ return EXIT_SUCCESS;
+ } catch (vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/imgproc/test/testImgproc.cpp b/modules/imgproc/test/testImgproc.cpp
new file mode 100644
index 0000000..1ff70a1
--- /dev/null
+++ b/modules/imgproc/test/testImgproc.cpp
@@ -0,0 +1,420 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test imgproc functions.
+ *
+ * Authors:
+ * Souriya Trinh
+ *
+ *****************************************************************************/
+
+#include <cstdio>
+#include <cstdlib>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/imgproc/vpImgproc.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
+
+/*!
+ \example testImgproc.cpp
+
+ \brief Test imgproc functions.
+*/
+
+// List of allowed command line options
+#define GETOPTARGS "cdi:o:h"
+
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user);
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user);
+
+/*
+ Print the program options.
+
+ \param name : Program name.
+ \param badparam : Bad parameter name.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ */
+void usage(const char *name, const char *badparam, std::string ipath, std::string opath, std::string user)
+{
+ fprintf(stdout, "\n\
+Test imgproc functions.\n\
+\n\
+SYNOPSIS\n\
+ %s [-i <input image path>] [-o <output image path>]\n\
+ [-h]\n \
+", name);
+
+ fprintf(stdout, "\n\
+OPTIONS: Default\n\
+ -i <input image path> %s\n\
+ Set image input path.\n\
+ From this path read \"Klimt/Klimt.pgm\"\n\
+ image.\n\
+ Setting the VISP_INPUT_IMAGE_PATH environment\n\
+ variable produces the same behaviour than using\n\
+ this option.\n\
+\n\
+ -o <output image path> %s\n\
+ Set image output path.\n\
+ From this directory, creates the \"%s\"\n\
+ subdirectory depending on the username, where \n\
+ Klimt_grey.pgm output image is written.\n\
+\n\
+ -h\n\
+ Print the help.\n\n", ipath.c_str(), opath.c_str(), user.c_str());
+
+ if (badparam)
+ fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
+}
+
+/*!
+ Set the program options.
+
+ \param argc : Command line number of parameters.
+ \param argv : Array of command line parameters.
+ \param ipath: Input image path.
+ \param opath : Output image path.
+ \param user : Username.
+ \return false if the program has to be stopped, true otherwise.
+*/
+bool getOptions(int argc, const char **argv, std::string &ipath, std::string &opath, std::string user)
+{
+ const char *optarg_;
+ int c;
+ while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
+
+ switch (c) {
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath, opath, user);
+ return false;
+ break;
+
+ case 'c':
+ case 'd':
+ break;
+
+ default:
+ usage(argv[0], optarg_, ipath, opath, user);
+ return false;
+ break;
+ }
+ }
+
+ if ((c == 1) || (c == -1)) {
+ // standalone param or error
+ usage(argv[0], NULL, ipath, opath, user);
+ std::cerr << "ERROR: " << std::endl;
+ std::cerr << " Bad argument " << optarg_ << std::endl << std::endl;
+ return false;
+ }
+
+ return true;
+}
+
+int main(int argc, const char **argv)
+{
+ try {
+ std::string env_ipath;
+ std::string opt_ipath;
+ std::string opt_opath;
+ std::string ipath;
+ std::string opath;
+ std::string filename;
+ std::string username;
+
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
+ env_ipath = vpIoTools::getViSPImagesDataPath();
+
+ // Set the default input path
+ if (!env_ipath.empty())
+ ipath = env_ipath;
+
+// Set the default output path
+#if defined(_WIN32)
+ opt_opath = "C:/temp";
+#else
+ opt_opath = "/tmp";
+#endif
+
+ // Get the user login name
+ vpIoTools::getUserName(username);
+
+ // Read the command line options
+ if (getOptions(argc, argv, opt_ipath, opt_opath, username) == false) {
+ return EXIT_FAILURE;
+ }
+
+ // Get the option values
+ if (!opt_ipath.empty())
+ ipath = opt_ipath;
+ if (!opt_opath.empty())
+ opath = opt_opath;
+
+ // Append to the output path string, the login name of the user
+ opath = vpIoTools::createFilePath(opath, username);
+
+ // Test if the output path exist. If no try to create it
+ if (vpIoTools::checkDirectory(opath) == false) {
+ try {
+ // Create the dirname
+ vpIoTools::makeDirectory(opath);
+ } catch (...) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot create " << opath << std::endl;
+ std::cerr << " Check your -o " << opt_opath << " option " << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+
+ // Compare ipath and env_ipath. If they differ, we take into account
+ // the input path comming from the command line option
+ if (!opt_ipath.empty() && !env_ipath.empty()) {
+ if (ipath != env_ipath) {
+ std::cout << std::endl << "WARNING: " << std::endl;
+ std::cout << " Since -i <visp image path=" << ipath << "> "
+ << " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
+ << " we skip the environment variable." << std::endl;
+ }
+ }
+
+ // Test if an input path is set
+ if (opt_ipath.empty() && env_ipath.empty()) {
+ usage(argv[0], NULL, ipath, opt_opath, username);
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
+ << " environment variable to specify the location of the " << std::endl
+ << " image path where test images are located." << std::endl
+ << std::endl;
+ return EXIT_FAILURE;
+ }
+
+ //
+ // Here starts really the test
+ //
+
+ //
+ // Test color functions using Klimt.ppm
+ //
+
+ // Read Klimt.ppm
+ filename = vpIoTools::createFilePath(ipath, "Klimt/Klimt.ppm");
+ vpImage<vpRGBa> 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;
+
+ // Adjust
+ double alpha = 1.5, beta = -10.0;
+ vpImage<vpRGBa> I_color_adjust;
+ double t = vpTime::measureTimeMs();
+ vp::adjust(I_color, I_color_adjust, alpha, beta);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do color adjust: " << t << " ms" << std::endl;
+
+ // Save adjust
+ filename = vpIoTools::createFilePath(opath, "Klimt_adjust.ppm");
+ vpImageIo::write(I_color_adjust, filename);
+
+ // Equalize Histogram
+ vpImage<vpRGBa> I_color_equalize_histogram;
+ 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;
+
+ // Save equalizeHistogram
+ filename = vpIoTools::createFilePath(opath, "Klimt_equalize_histogram.ppm");
+ vpImageIo::write(I_color_equalize_histogram, filename);
+
+ // Gamma correction
+ vpImage<vpRGBa> I_color_gamma_correction;
+ double gamma = 2.2;
+ 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;
+
+ // Save gammaCorrection
+ filename = vpIoTools::createFilePath(opath, "Klimt_gamma_correction.ppm");
+ vpImageIo::write(I_color_gamma_correction, filename);
+
+ // Retinex
+ vpImage<vpRGBa> I_color_retinex;
+ t = vpTime::measureTimeMs();
+ vp::retinex(I_color, I_color_retinex);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do color retinex: " << t << " ms" << std::endl;
+
+ // Save retinex
+ filename = vpIoTools::createFilePath(opath, "Klimt_retinex.ppm");
+ vpImageIo::write(I_color_retinex, filename);
+
+ // Stretch contrast
+ vpImage<vpRGBa> I_color_stretch_contrast;
+ 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;
+
+ // Save stretchContrast
+ filename = vpIoTools::createFilePath(opath, "Klimt_stretch_contrast.ppm");
+ vpImageIo::write(I_color_stretch_contrast, filename);
+
+ // Stretch Contrast HSV
+ vpImage<vpRGBa> I_color_stretch_contrast_HSV;
+ 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;
+
+ // Save stretchContrastHSV
+ filename = vpIoTools::createFilePath(opath, "Klimt_stretch_contrast_HSV.ppm");
+ vpImageIo::write(I_color_stretch_contrast_HSV, filename);
+
+ // Unsharp Mask
+ vpImage<vpRGBa> I_color_unsharp_mask;
+ t = vpTime::measureTimeMs();
+ vp::unsharpMask(I_color, I_color_unsharp_mask);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do color unsharp mask: " << t << " ms" << std::endl;
+
+ // Save unsharpMask
+ filename = vpIoTools::createFilePath(opath, "Klimt_unsharp_mask.ppm");
+ vpImageIo::write(I_color_unsharp_mask, filename);
+
+ // CLAHE
+ vpImage<vpRGBa> I_color_clahe;
+ t = vpTime::measureTimeMs();
+ vp::clahe(I_color, I_color_clahe);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do color CLAHE: " << t << " ms" << std::endl;
+
+ // Save CLAHE
+ filename = vpIoTools::createFilePath(opath, "Klimt_CLAHE.ppm");
+ vpImageIo::write(I_color_clahe, filename);
+
+ //
+ // Test grayscale function using image0000.pgm
+ //
+
+ // Read image0000.pgm
+ filename = vpIoTools::createFilePath(ipath, "mbt/cube/image0000.pgm");
+ vpImage<unsigned char> I;
+ std::cout << "\nRead image: " << filename << std::endl;
+ vpImageIo::read(I, filename);
+ std::cout << "Image: " << I.getWidth() << "x" << I.getHeight() << std::endl;
+
+ // Adjust
+ vpImage<unsigned char> I_adjust;
+ beta = -20.0;
+ t = vpTime::measureTimeMs();
+ vp::adjust(I, I_adjust, alpha, beta);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do grayscale adjust: " << t << " ms" << std::endl;
+
+ // Save adjust
+ filename = vpIoTools::createFilePath(opath, "image0000_adjust.pgm");
+ vpImageIo::write(I_adjust, filename);
+
+ // Equalize Histogram
+ vpImage<unsigned char> I_equalize_histogram;
+ 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;
+
+ // Save equalizeHistogram
+ filename = vpIoTools::createFilePath(opath, "image0000_equalize_histogram.pgm");
+ vpImageIo::write(I_equalize_histogram, filename);
+
+ // Gamma correction
+ vpImage<unsigned char> I_gamma_correction;
+ gamma = 1.8;
+ 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;
+
+ // Save gammaCorrection
+ filename = vpIoTools::createFilePath(opath, "image0000_gamma_correction.pgm");
+ vpImageIo::write(I_gamma_correction, filename);
+
+ // Stretch contrast
+ vpImage<unsigned char> I_stretch_contrast;
+ 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;
+
+ // Save stretchContrast
+ filename = vpIoTools::createFilePath(opath, "image0000_stretch_contrast.pgm");
+ vpImageIo::write(I_stretch_contrast, filename);
+
+ // Unsharp Mask
+ vpImage<unsigned char> I_unsharp_mask;
+ 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;
+
+ // Save unsharpMask
+ filename = vpIoTools::createFilePath(opath, "image0000_unsharp_mask.pgm");
+ vpImageIo::write(I_unsharp_mask, filename);
+
+ // CLAHE
+ vpImage<unsigned char> I_clahe;
+ t = vpTime::measureTimeMs();
+ vp::clahe(I, I_clahe);
+ t = vpTime::measureTimeMs() - t;
+ std::cout << "Time to do grayscale CLAHE: " << t << " ms" << std::endl;
+
+ // Save CLAHE
+ filename = vpIoTools::createFilePath(opath, "image0000_CLAHE.pgm");
+ vpImageIo::write(I_clahe, filename);
+
+ return EXIT_SUCCESS;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.what() << std::endl;
+ return EXIT_FAILURE;
+ }
+}
diff --git a/modules/io/CMakeLists.txt b/modules/io/CMakeLists.txt
index c13d36f..4d1c1d6 100644
--- a/modules/io/CMakeLists.txt
+++ b/modules/io/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -55,12 +56,6 @@ if(USE_PNG)
add_definitions(${PNG_DEFINITIONS})
endif()
-# videos: ffmpeg
-if(USE_FFMPEG)
- list(APPEND opt_incs ${FFMPEG_INCLUDE_DIRS})
- list(APPEND opt_libs ${FFMPEG_LIBRARIES})
-endif()
-
vp_add_module(io visp_core)
vp_glob_module_sources()
vp_module_include_directories(${opt_incs})
diff --git a/modules/io/include/visp3/io/vpDiskGrabber.h b/modules/io/include/visp3/io/vpDiskGrabber.h
index e7c7748..5d0d668 100644
--- a/modules/io/include/visp3/io/vpDiskGrabber.h
+++ b/modules/io/include/visp3/io/vpDiskGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpDiskGrabber.h
\brief Class to load image sequence from the disk.
@@ -43,10 +43,12 @@
#ifndef vpDiskGrabber_hh
#define vpDiskGrabber_hh
-#include <visp3/io/vpImageIo.h>
+#include <string>
+
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpFrameGrabber.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/io/vpImageIo.h>
/*!
\class vpDiskGrabber
@@ -103,54 +105,54 @@ int main(){
}
\endcode
*/
-class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber
+class VISP_EXPORT vpDiskGrabber : public vpFrameGrabber
{
private:
- long image_number ; //!< id of the next image to be read
- int image_step ; //!< increment between two image id
- unsigned int number_of_zero ; //!< number of zero in the image name (image.00000.pgm)
+ long m_image_number; //!< id of the current image to be read
+ long m_image_number_next; //!< id of the next image to be read
+ long m_image_step; //!< increment between two image id
+ unsigned int m_number_of_zero; //!< number of zero in the image name
+ //!< (image.00000.pgm)
+
+ std::string m_directory; //!< image location
+ std::string m_base_name; //!< image base name
+ std::string m_extension; //!< image extension
- char directory[FILENAME_MAX] ; //!< image location
- char base_name[FILENAME_MAX] ; //!< image base name
- char extension[FILENAME_MAX] ; //!< image extension
-
- bool useGenericName;
- char genericName[FILENAME_MAX];
+ bool m_use_generic_name;
+ std::string m_generic_name;
public:
vpDiskGrabber();
- vpDiskGrabber(const char *genericName);
- vpDiskGrabber(const char *dir, const char *basename,
- long number, int step, unsigned int noz,
- const char *ext) ;
- virtual ~vpDiskGrabber() ;
-
- void open(vpImage<unsigned char> &I) ;
- void open(vpImage<vpRGBa> &I) ;
- void open(vpImage<float> &I) ;
+ explicit vpDiskGrabber(const std::string &genericName);
+ explicit vpDiskGrabber(const std::string &dir, const std::string &basename, long number, int step, unsigned int noz,
+ const std::string &ext);
+ virtual ~vpDiskGrabber();
void acquire(vpImage<unsigned char> &I);
void acquire(vpImage<vpRGBa> &I);
- void acquire(vpImage<float> &I) ;
+ void acquire(vpImage<float> &I);
void acquire(vpImage<unsigned char> &I, long image_number);
void acquire(vpImage<vpRGBa> &I, long image_number);
- void acquire(vpImage<float> &I, long image_number) ;
+ void acquire(vpImage<float> &I, long image_number);
void close();
- void setDirectory(const char *dir);
- void setBaseName(const char *name);
- void setImageNumber(long number) ;
- void setStep(int a);
- void setNumberOfZero(unsigned int noz);
- void setExtension(const char *ext);
- void setGenericName(const char *genericName);
-
/*!
Return the current image number.
*/
- long getImageNumber() { return image_number; };
-} ;
+ long getImageNumber() { return m_image_number; };
-#endif
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+ void open(vpImage<float> &I);
+ void setBaseName(const std::string &name);
+ void setDirectory(const std::string &dir);
+ void setExtension(const std::string &ext);
+ void setGenericName(const std::string &genericName);
+ void setImageNumber(long number);
+ void setNumberOfZero(unsigned int noz);
+ void setStep(long step);
+};
+
+#endif
diff --git a/modules/io/include/visp3/io/vpFFMPEG.h b/modules/io/include/visp3/io/vpFFMPEG.h
deleted file mode 100644
index eff68d8..0000000
--- a/modules/io/include/visp3/io/vpFFMPEG.h
+++ /dev/null
@@ -1,268 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.txt at the root directory of this source
- * distribution for additional information about the GNU GPL.
- *
- * For using ViSP with software that can not be combined with the GNU
- * GPL, please contact Inria about acquiring a ViSP Professional
- * Edition License.
- *
- * See http://visp.inria.fr for more information.
- *
- * This software was developed at:
- * Inria Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- *
- * If you have questions regarding the use of this file, please contact
- * Inria at visp at inria.fr
- *
- * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
- * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
- *
- * Description:
- * Class that manages the FFMPEG library.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpFFMPEG.h
- \brief Class that manages the FFMPEG library
-*/
-
-#ifndef vpFFMPEG_H
-#define vpFFMPEG_H
-
-#include <visp3/io/vpImageIo.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <vector>
-
-#ifdef VISP_HAVE_FFMPEG
-
-// Fix for the following compilation error:
-// libavutil/common.h:154: error: UINT64_C was not declared in this scope
-// libavutil/common.h is no more autosufficient for C++ program because
-// stdint.h defines UINT64_C only for C program and not C++ program
-#ifdef __cplusplus
-# ifndef __STDC_CONSTANT_MACROS
-# define __STDC_CONSTANT_MACROS
-# endif
-# ifdef _STDINT_H
-# undef _STDINT_H
-# endif
-// On OS X
-# ifdef _STDINT_H_
-# undef _STDINT_H_
-# endif
-# include <stdint.h>
-
-# ifndef INT64_C
-# define INT64_C(c) (c ## LL)
-# endif
-# ifndef UINT64_C
-# define UINT64_C(c) (c ## ULL)
-# endif
-#endif
-// end fix
-
-
-extern "C"
-{
-#include <libavcodec/avcodec.h> // requested for CodecID enum
-//#include <avformat.h>
-//#include <swscale.h>
-}
-
-struct AVFormatContext;
-struct AVCodecContext;
-struct AVCodec;
-struct AVFrame;
-struct AVFrame;
-struct AVFrame;
-struct AVPacket;
-struct SwsContext;
-
-/*!
- \class vpFFMPEG
- \ingroup group_io_video
-
- \brief This class interfaces the FFmpeg library to enable video stream reading or writing.
-
- Here an example which explains how to use the class to read a video stream.
- \code
-#include <visp3/io/vpFFMPEG.h>
-
-int main ()
-{
-#ifdef VISP_HAVE_FFMPEG
- vpImage<vpRGBa> I; // The image to stores the frames
- vpFFMPEG ffmpeg;
- // Initialization
- ffmpeg.openStream("video.mpeg", vpFFMPEG::COLORED);
- ffmpeg.initStream();
- std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
- std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
- // Video reading
- int frameIndex = 0;
- ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
-#endif
-}
- \endcode
-
- If you want to open the video as a gray scaled video, you can use the following example.
- \code
-#include <visp3/io/vpFFMPEG.h>
-
-int main ()
-{
-#ifdef VISP_HAVE_FFMPEG
- vpImage<unsigned char> I; // The image to stores the frames
- vpFFMPEG ffmpeg;
- // Initialization
- ffmpeg.openStream("video.mpeg", vpFFMPEG::GRAY_SCALED);
- ffmpeg.initStream();
- std::cout << "framerate: " << ffmpeg.getFramerate() << "Hz" << std::endl;
- std::cout << "image size: " << ffmpeg.getWidth() << " " << ffmpeg.getHeight() << std::endl;
- // Video reading
- int frameIndex = 0;
- ffmpeg.getFrame(I,frameIndex); // Here the first frame (index 0) is read.
-#endif
-}
- \endcode
-*/
-class VISP_EXPORT vpFFMPEG
-{
- public:
- typedef enum
- {
- COLORED,
- GRAY_SCALED,
- }vpFFMPEGColorType;
-
- private:
- //! Video's height and width
- int width, height;
- //! Number of frame in the video.
- unsigned long frameNumber;
- //! FFMPEG variables
- AVFormatContext *pFormatCtx;
- AVCodecContext *pCodecCtx;
- AVCodec *pCodec;
- AVFrame *pFrame;
- AVFrame *pFrameRGB;
- AVFrame *pFrameGRAY;
- AVPacket *packet;
- SwsContext *img_convert_ctx ;
- unsigned int videoStream;
- int numBytes ;
- uint8_t * buffer ;
- std::vector<int64_t> index;
- //! Indicates if the openStream method was executed
- bool streamWasOpen;
- //! Indicates if the initStream method was executed
- bool streamWasInitialized;
- //! Indicates the video's color output.
- vpFFMPEGColorType color_type;
-
- //!The file which is the video file
- FILE *f;
- //!Buffers
- uint8_t *outbuf, *picture_buf;
- //!Buffer size
- int outbuf_size;
- //!Size of the data to write in the file
- int out_size;
- //!Bit rate of the video to write
- unsigned int bit_rate;
- //!Indicates if the openEncoder method was executed
- bool encoderWasOpened;
- double framerate_stream; // input stream
- int framerate_encoder; // output stream
-
- public:
- vpFFMPEG();
- ~vpFFMPEG();
-
- bool acquire(vpImage<vpRGBa> &I);
- bool acquire(vpImage<unsigned char> &I);
-
- void closeStream();
-
- bool endWrite();
-
- bool getFrame(vpImage<vpRGBa> &I, unsigned int frameNumber);
- bool getFrame(vpImage<unsigned char> &I, unsigned int frameNumber);
- /*!
- Gets the video's frame number.
-
- \return The value of the video's frame number.
- */
- inline unsigned long getFrameNumber() const {return frameNumber;}
- /*!
- Return the framerate used to encode the video. The video stream need
- to be opened before calling this function.
-
- \exception vpException::ioError : If the video stream was not opened priviously.
- */
- inline double getFramerate() const {
- if (streamWasOpen)
- return framerate_stream;
- throw vpException(vpException::ioError, "Video stream not opened.");
- }
- /*!
- Gets the video's height.
-
- \return The value of the video's height.
- */
- inline int getHeight() const {return height;}
- /*!
- Gets the video's width.
-
- \return The value of the video's width.
- */
- inline int getWidth() const {return width;}
-
- bool initStream();
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- bool openEncoder(const char *filename, unsigned int width, unsigned int height, CodecID codec = CODEC_ID_MPEG1VIDEO);
-#else
- bool openEncoder(const char *filename, unsigned int width, unsigned int height, AVCodecID codec = AV_CODEC_ID_MPEG1VIDEO);
-#endif
- bool openStream(const char *filename,vpFFMPEGColorType color_type);
-
- bool saveFrame(vpImage<vpRGBa> &I);
- bool saveFrame(vpImage<unsigned char> &I);
- /*!
- Sets the bit rate of the video when encoding.
-
- \param rate : the expected bit rate.
- */
- inline void setBitRate(const unsigned int rate) {this->bit_rate = rate;}
- /*!
- Sets the framerate of the video when encoding.
-
- \param framerate : the expected framerate.
- */
- inline void setFramerate(const int framerate) {framerate_encoder = framerate;}
-
- private:
- void copyBitmap(vpImage<vpRGBa> &I);
- void copyBitmap(vpImage<unsigned char> &I);
- void writeBitmap(vpImage<vpRGBa> &I);
- void writeBitmap(vpImage<unsigned char> &I);
-};
-#endif
-#endif
diff --git a/modules/io/include/visp3/io/vpImageIo.h b/modules/io/include/visp3/io/vpImageIo.h
index 4438e48..07aacb9 100644
--- a/modules/io/include/visp3/io/vpImageIo.h
+++ b/modules/io/include/visp3/io/vpImageIo.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpImageIo.h
\brief Read/write images
@@ -44,31 +44,33 @@
#ifndef vpIMAGEIO_H
#define vpIMAGEIO_H
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpRGBa.h>
-#include <stdio.h>
#include <iostream>
+#include <stdio.h>
#if defined(_WIN32)
-# include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
+#include <windows.h>
#endif
#if defined(VISP_HAVE_JPEG)
-#include <jpeglib.h>
#include <jerror.h>
+#include <jpeglib.h>
#endif
#if defined(VISP_HAVE_PNG)
#include <png.h>
#endif
-
/*!
\class vpImageIo
-
+
\ingroup group_io_image
\brief Read/write images with various image format.
@@ -76,14 +78,16 @@
This class has its own implementation of PGM and PPM images read/write.
This class may benefit from optional 3rd parties:
- - libpng: If installed this optional 3rd party is used to read/write PNG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_png.
- - libjpeg: If installed this optional 3rd party is used to read/write JPEG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_jpeg.
- - OpenCV: If installed this optional 3rd party is used to read/write other image
- formats TIFF, BMP, DIB, PBM, RASTER, JPEG2000. If libpng or libjpeg is not installed
- OpenCV is also used to consider these image formats. Installation instructions are
- provided here https://visp.inria.fr/3rd_opencv.
+ - libpng: If installed this optional 3rd party is used to read/write PNG
+images. Installation instructions are provided here
+https://visp.inria.fr/3rd_png.
+ - libjpeg: If installed this optional 3rd party is used to read/write JPEG
+images. Installation instructions are provided here
+https://visp.inria.fr/3rd_jpeg.
+ - OpenCV: If installed this optional 3rd party is used to read/write other
+image formats TIFF, BMP, DIB, PBM, RASTER, JPEG2000. If libpng or libjpeg is
+not installed OpenCV is also used to consider these image formats.
+Installation instructions are provided here https://visp.inria.fr/3rd_opencv.
The code below shows how to convert an PPM P6 image file format into
a PGM P5 image file format. The extension of the filename is here
@@ -104,22 +108,20 @@ int main()
#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::write(I, "Klimt.pgm"); // Write the image in a PGM P5 image file format
}
\endcode
- This other example available in tutorial-image-reader.cpp shows how to read/write
- jpeg images. It supposes that \c libjpeg is installed.
- \include tutorial-image-reader.cpp
+ This other example available in tutorial-image-reader.cpp shows how to
+read/write jpeg images. It supposes that \c libjpeg is installed. \include
+tutorial-image-reader.cpp
*/
class VISP_EXPORT vpImageIo
{
private:
-
- typedef enum
- {
+ typedef enum {
FORMAT_PGM,
FORMAT_PPM,
FORMAT_JPEG,
@@ -133,55 +135,52 @@ private:
FORMAT_JPEG2000,
FORMAT_UNKNOWN
} vpImageFormatType;
-
- static vpImageFormatType getFormat(const std::string &filename) ;
+
+ static vpImageFormatType getFormat(const std::string &filename);
static std::string getExtension(const std::string &filename);
public:
+ static void read(vpImage<unsigned char> &I, const std::string &filename);
+ static void read(vpImage<vpRGBa> &I, const std::string &filename);
- static void read(vpImage<unsigned char> &I, const std::string &filename) ;
- static void read(vpImage<vpRGBa> &I, const std::string &filename) ;
-
- static void write(const vpImage<unsigned char> &I, const std::string &filename) ;
- static void write(const vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void write(const vpImage<unsigned char> &I, const std::string &filename);
+ static void write(const vpImage<vpRGBa> &I, const std::string &filename);
- static void readPFM(vpImage<float> &I, const std::string &filename) ;
+ static void readPFM(vpImage<float> &I, const std::string &filename);
- static void readPGM(vpImage<unsigned char> &I, const std::string &filename) ;
- static void readPGM(vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void readPGM(vpImage<unsigned char> &I, const std::string &filename);
+ static void readPGM(vpImage<vpRGBa> &I, const std::string &filename);
- static void readPPM(vpImage<unsigned char> &I, const std::string &filename) ;
- static void readPPM(vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void readPPM(vpImage<unsigned char> &I, const std::string &filename);
+ static void readPPM(vpImage<vpRGBa> &I, const std::string &filename);
#if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV))
- static void readJPEG(vpImage<unsigned char> &I, const std::string &filename) ;
- static void readJPEG(vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void readJPEG(vpImage<unsigned char> &I, const std::string &filename);
+ static void readJPEG(vpImage<vpRGBa> &I, const std::string &filename);
#endif
#if (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV))
- static void readPNG(vpImage<unsigned char> &I, const std::string &filename) ;
- static void readPNG(vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void readPNG(vpImage<unsigned char> &I, const std::string &filename);
+ static void readPNG(vpImage<vpRGBa> &I, const std::string &filename);
#endif
- static void writePFM(const vpImage<float> &I, const std::string &filename) ;
+ static void writePFM(const vpImage<float> &I, const std::string &filename);
- static void writePGM(const vpImage<unsigned char> &I, const std::string &filename) ;
- static void writePGM(const vpImage<short> &I, const std::string &filename) ;
- static void writePGM(const vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void writePGM(const vpImage<unsigned char> &I, const std::string &filename);
+ static void writePGM(const vpImage<short> &I, const std::string &filename);
+ static void writePGM(const vpImage<vpRGBa> &I, const std::string &filename);
- static void writePPM(const vpImage<unsigned char> &I, const std::string &filename) ;
- static void writePPM(const vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void writePPM(const vpImage<unsigned char> &I, const std::string &filename);
+ static void writePPM(const vpImage<vpRGBa> &I, const std::string &filename);
#if (defined(VISP_HAVE_JPEG) || defined(VISP_HAVE_OPENCV))
- static void writeJPEG(const vpImage<unsigned char> &I, const std::string &filename) ;
- static void writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void writeJPEG(const vpImage<unsigned char> &I, const std::string &filename);
+ static void writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename);
#endif
#if (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV))
- static void writePNG(const vpImage<unsigned char> &I, const std::string &filename) ;
- static void writePNG(const vpImage<vpRGBa> &I, const std::string &filename) ;
+ static void writePNG(const vpImage<unsigned char> &I, const std::string &filename);
+ static void writePNG(const vpImage<vpRGBa> &I, const std::string &filename);
#endif
-
- } ;
+};
#endif
-
diff --git a/modules/io/include/visp3/io/vpKeyboard.h b/modules/io/include/visp3/io/vpKeyboard.h
index 69a143c..238a259 100644
--- a/modules/io/include/visp3/io/vpKeyboard.h
+++ b/modules/io/include/visp3/io/vpKeyboard.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpKeyboard_h
#define vpKeyboard_h
@@ -48,20 +48,21 @@
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-# include <iostream>
-# include <termios.h>
-# include <unistd.h>
-# include <stdlib.h>
-
+#include <iostream>
+#include <stdlib.h>
+#include <termios.h>
+#include <unistd.h>
/*!
\class vpKeyboard
\ingroup group_io_keyboard
- \brief Keybord management under unix (Linux or OSX). This class is not available under windows.
+ \brief Keybord management under unix (Linux or OSX). This class is not
+ available under windows.
Gets a key from the keyboard without waiting for the "Enter" key.
- \warning The key that was pressed is only detected if the terminal where the binary was launched is active.
+ \warning The key that was pressed is only detected if the terminal where the
+ binary was launched is active.
\code
vpKeyboard keyboard; // Turn on keyboard raw mode
@@ -97,7 +98,7 @@ private:
void setRawMode(bool active);
struct termios initial_settings, new_settings;
-} ;
+};
#endif // defined UNIX
diff --git a/modules/io/include/visp3/io/vpParallelPort.h b/modules/io/include/visp3/io/vpParallelPort.h
index 9cc4fa8..b99f6eb 100644
--- a/modules/io/include/visp3/io/vpParallelPort.h
+++ b/modules/io/include/visp3/io/vpParallelPort.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpParallelPort_h
#define vpParallelPort_h
@@ -48,13 +48,13 @@
#ifdef VISP_HAVE_PARPORT
+#include <errno.h>
#include <iostream>
-# include <stdio.h>
-# include <errno.h>
-# include <linux/parport.h>
-# include <linux/ppdev.h>
+#include <linux/parport.h>
+#include <linux/ppdev.h>
+#include <stdio.h>
-# include <visp3/io/vpParallelPortException.h>
+#include <visp3/io/vpParallelPortException.h>
/*!
@@ -96,7 +96,7 @@ private:
private:
int fd; // parallel port descriptor
char device[FILENAME_MAX];
-} ;
+};
#endif
diff --git a/modules/io/include/visp3/io/vpParallelPortException.h b/modules/io/include/visp3/io/vpParallelPortException.h
index 2df3547..7a14a70 100644
--- a/modules/io/include/visp3/io/vpParallelPortException.h
+++ b/modules/io/include/visp3/io/vpParallelPortException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,22 +29,23 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Exceptions that can be emited by the vpParallelPort class and its derivates.
+ * Exceptions that can be emited by the vpParallelPort class and its
+ *derivates.
*
* Authors:
* Fabien Spindler
*
*****************************************************************************/
-
#ifndef __vpParallelPortException_H
#define __vpParallelPortException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -58,12 +60,15 @@
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpParallelPortException
@@ -73,31 +78,27 @@
*/
class VISP_EXPORT vpParallelPortException : public vpException
{
- public:
- /*!
- \brief Lists the possible errors than can be emmited while calling
- vpParallelPort member
- */
- enum error
- {
- opening, /*!< Cannot access to the parallel port device. */
- closing /*!< Cannot close the parallel port device. */
- } ;
-
- public:
- vpParallelPortException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpParallelPortException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpParallelPortException (const int id)
- : vpException(id){ ; }
-
+public:
+ /*!
+ \brief Lists the possible errors than can be emmited while calling
+ vpParallelPort member
+ */
+ enum error {
+ opening, /*!< Cannot access to the parallel port device. */
+ closing /*!< Cannot close the parallel port device. */
+ };
+
+public:
+ vpParallelPortException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpParallelPortException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpParallelPortException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/io/include/visp3/io/vpParseArgv.h b/modules/io/include/visp3/io/vpParseArgv.h
index bba2d2e..a6410ca 100644
--- a/modules/io/include/visp3/io/vpParseArgv.h
+++ b/modules/io/include/visp3/io/vpParseArgv.h
@@ -31,7 +31,6 @@
#ifndef vpParseArgv_h
#define vpParseArgv_h
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
@@ -46,8 +45,8 @@
\code
#include <stdio.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
// Usage : [-bool] [-int <integer value>] [-long <long value>]
// [-float <float value>] [-double <double value>] [-string <string value>] [-h]
@@ -64,7 +63,7 @@ int main(int argc, const char ** argv)
// Parse the command line to set the variables
vpParseArgv::vpArgvInfo argTable[] =
{
- {"-bool", vpParseArgv::ARGV_CONSTANT, 0, (char *) &b_val,
+ {"-bool", vpParseArgv::ARGV_CONSTANT_BOOL, 0, (char *) &b_val,
"Flag enabled."},
{"-int", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &i_val,
"An integer value."},
@@ -78,8 +77,7 @@ 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,
@@ -93,13 +91,13 @@ int main(int argc, const char ** argv)
}
\endcode
- The code below shows an other way to parse command line arguments using
+ The code below shows an other way to parse command line arguments using
vpParseArgv class. Here command line options are only one character long.
\code
#include <stdio.h>
#include <stdlib.h>
-#include <visp3/io/vpParseArgv.h>
#include <visp3/core/vpMath.h>
+#include <visp3/io/vpParseArgv.h>
// List of allowed command line options
#define GETOPTARGS "bi:l:f:d:h" // double point mean here that the preceding option request an argument
@@ -147,67 +145,68 @@ int main(int argc, const char ** argv)
*/
-
class VISP_EXPORT vpParseArgv
{
- public:
+public:
/*!
Legal values for the type field of a vpArgvInfo.
*/
- typedef enum {
- ARGV_CONSTANT,/*!< Stand alone argument. */
- ARGV_INT, /*!< Argument is associated to an int. */
- ARGV_LONG, /*!< Argument is associated to a long. */
- ARGV_STRING, /*!< Argument is associated to a char * string. */
+ 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_BOOL, /*!< Stand alone argument associated to a bool var
+ that is set to true. */
+ ARGV_INT, /*!< Argument is associated to an int. */
+ ARGV_LONG, /*!< Argument is associated to a long. */
+ ARGV_STRING, /*!< Argument is associated to a char * string. */
ARGV_REST,
- ARGV_FLOAT, /*!< Argument is associated to a float. */
- ARGV_DOUBLE, /*!< Argument is associated to a double. */
+ ARGV_FLOAT, /*!< Argument is associated to a float. */
+ ARGV_DOUBLE, /*!< Argument is associated to a double. */
ARGV_FUNC,
ARGV_GENFUNC,
- ARGV_HELP, /*!< Argument is for help displaying. */
- ARGV_END /*!< End of the argument list. */
+ ARGV_HELP, /*!< Argument is for help displaying. */
+ ARGV_END /*!< End of the argument list. */
} vpArgvType;
- /*!
+ /*!
Flag bits.
*/
typedef enum {
- ARGV_NO_DEFAULTS = 0x1, /*!< No default options like -help. */
- ARGV_NO_LEFTOVERS = 0x2, /*!< Print an error message if an option is not in the argument list. */
- ARGV_NO_ABBREV = 0x4, /*!< No abrevation. Print an error message if an option is abrevated (ie "-i" in place of "-int" which is requested). */
- ARGV_DONT_SKIP_FIRST_ARG = 0x8, /*!< Don't skip first argument. */
- ARGV_NO_PRINT = 0x10 /*!< No printings. */
+ ARGV_NO_DEFAULTS = 0x1, /*!< No default options like -help. */
+ ARGV_NO_LEFTOVERS = 0x2, /*!< Print an error message if an option is not
+ in the argument list. */
+ ARGV_NO_ABBREV = 0x4, /*!< No abrevation. Print an error message if an
+ option is abrevated (ie "-i" in place of "-int"
+ which is requested). */
+ ARGV_DONT_SKIP_FIRST_ARG = 0x8, /*!< Don't skip first argument. */
+ ARGV_NO_PRINT = 0x10 /*!< No printings. */
} vpArgvFlags;
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /*!
+ /*!
- Structure used to specify how to handle argv options.
- */
+ Structure used to specify how to handle argv options.
+ */
typedef struct {
- const char *key; /*!< The key string that flags the option in the
- * argv array. */
- vpArgvType type; /*!< Indicates option type; see below. */
- const char *src; /*!< Value to be used in setting dst; usage
- * depends on type. */
- const char *dst; /*!< Address of value to be modified; usage
- * depends on type. */
- const char *help; /*!< Documentation message describing this option. */
+ const char *key; /*!< The key string that flags the option in the
+ * argv array. */
+ vpArgvType type; /*!< Indicates option type; see below. */
+ const char *src; /*!< Value to be used in setting dst; usage
+ * depends on type. */
+ const char *dst; /*!< Address of value to be modified; usage
+ * depends on type. */
+ const char *help; /*!< Documentation message describing this option. */
} vpArgvInfo;
#endif /* DOXYGEN_SHOULD_SKIP_THIS */
public:
static vpArgvInfo defaultTable[2];
- static bool parse(int *argcPtr, const char **argv,
- vpArgvInfo *argTable, int flags);
- static int parse(int argc, const char** argv, const char* validOpts, const char** param);
-
- private:
- static void printUsage (vpArgvInfo *argTable, int flags);
-
-
-
-} ;
+ static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags);
+ static int parse(int argc, const char **argv, const char *validOpts, const char **param);
+private:
+ static void printUsage(vpArgvInfo *argTable, int flags);
+};
#endif
diff --git a/modules/io/include/visp3/io/vpVideoReader.h b/modules/io/include/visp3/io/vpVideoReader.h
index 379f2ac..669628b 100644
--- a/modules/io/include/visp3/io/vpVideoReader.h
+++ b/modules/io/include/visp3/io/vpVideoReader.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,7 +48,6 @@
#include <string>
#include <visp3/io/vpDiskGrabber.h>
-#include <visp3/io/vpFFMPEG.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020200
#include "opencv2/highgui/highgui.hpp"
@@ -64,26 +64,26 @@
images. As it inherits from the vpFrameGrabber Class, it can be used like an
other frame grabber class.
- This class has its own implementation to read a sequence of PGM and PPM images.
+ This class has its own implementation to read a sequence of PGM and PPM
+images.
This class may benefit from optional 3rd parties:
- - libpng: If installed this optional 3rd party is used to read a sequence of PNG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_png.
- - libjpeg: If installed this optional 3rd party is used to read a sequence of JPEG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_jpeg.
- - OpenCV: If installed this optional 3rd party is used to read a sequence of images
- where images could be in TIFF, BMP, DIB, PBM, RASTER, JPEG2000 format. If libpng or
- libjpeg is not installed, OpenCV is also used to consider these image formats. OpenCV
- allows also to consider AVI, MPEG, MPEG4, MOV, OGV, WMV, FLV, MKV video formats.
- Installation instructions are provided here https://visp.inria.fr/3rd_opencv.
- - ffmpeg: If installed and enabled this optional 3rd party is used to read
- AVI, MPEG, MPEG4, MOV, OGV, WMV, FLV, MKV video formats. It means that if OpenCV is
- also installed, that OpenCV is not used to read a video. Installation instructions
- are provided here https://visp.inria.fr/3rd_ffmpeg.
-
+ - libpng: If installed this optional 3rd party is used to read a sequence of
+PNG images. Installation instructions are provided here
+https://visp.inria.fr/3rd_png.
+ - libjpeg: If installed this optional 3rd party is used to read a sequence
+of JPEG images. Installation instructions are provided here
+https://visp.inria.fr/3rd_jpeg.
+ - OpenCV: If installed this optional 3rd party is used to read a sequence of
+images where images could be in TIFF, BMP, DIB, PBM, RASTER, JPEG2000 format.
+If libpng or libjpeg is not installed, OpenCV is also used to consider these
+image formats. OpenCV allows also to consider AVI, MPEG, MPEG4, MOV, OGV, WMV,
+FLV, MKV video formats. Installation instructions are provided here
+https://visp.inria.fr/3rd_opencv.
+
The following example available in tutorial-video-reader.cpp shows how this
- class is really easy to use. It enables to read a video file named video.mpeg.
- \include tutorial-video-reader.cpp
+ class is really easy to use. It enables to read a video file named
+video.mpeg. \include tutorial-video-reader.cpp
As shown in the next example, this class allows also to access to a specific
frame. But be careful, for video files, the getFrame() method is not precise
@@ -95,7 +95,7 @@
int main()
{
-#ifdef VISP_HAVE_FFMPEG
+#ifdef VISP_HAVE_OPENCV
vpImage<vpRGBa> I;
vpVideoReader reader;
@@ -105,7 +105,7 @@ int main()
reader.open(I);
// Read the nearest key frame from the 3th frame
- reader.getFrame(I,2);
+ reader.getFrame(I, 2);
// After positionning the video reader use acquire to read the video frame by frame
reader.acquire(I);
@@ -114,14 +114,14 @@ int main()
#endif
}
\endcode
-
+
The other following example explains how to use the class to read a
sequence of images. The images are stored in the folder "./image" and are
named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ... As explained
- in setFirstFrameIndex() and setLastFrameIndex() it is also possible to set the
- first and last image numbers to read a portion of the sequence. If these two
- functions are not used, first and last image numbers are set automatically to
- match the first and image images of the sequence.
+ in setFirstFrameIndex() and setLastFrameIndex() it is also possible to set
+the first and last image numbers to read a portion of the sequence. If these
+two functions are not used, first and last image numbers are set automatically
+to match the first and image images of the sequence.
\code
#include <visp3/io/vpVideoReader.h>
@@ -144,9 +144,10 @@ int main()
return 0;
}
\endcode
-
- Note that it is also possible to access to a specific frame using getFrame().
- \code
+
+ Note that it is also possible to access to a specific frame using
+getFrame().
+\code
#include <visp3/io/vpVideoReader.h>
int main()
@@ -168,170 +169,222 @@ int main()
*/
class VISP_EXPORT vpVideoReader : public vpFrameGrabber
-{
- private:
- //!To read sequences of images
- vpDiskGrabber *imSequence;
-#ifdef VISP_HAVE_FFMPEG
- //!To read video files
- vpFFMPEG *ffmpeg;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- //!To read video files with OpenCV
- cv::VideoCapture capture;
- cv::Mat frame;
+{
+private:
+ //! To read sequences of images
+ vpDiskGrabber *imSequence;
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ //! To read video files with OpenCV
+ cv::VideoCapture capture;
+ cv::Mat frame;
#endif
- //!Types of available formats
- typedef enum
- {
- FORMAT_PGM,
- FORMAT_PPM,
- FORMAT_JPEG,
- FORMAT_PNG,
- // Formats supported by opencv
- FORMAT_TIFF,
- FORMAT_BMP,
- FORMAT_DIB,
- FORMAT_PBM,
- FORMAT_RASTER,
- FORMAT_JPEG2000,
- // Video format
- FORMAT_AVI,
- FORMAT_MPEG,
- FORMAT_MPEG4,
- FORMAT_MOV,
- FORMAT_OGV,
- FORMAT_WMV,
- FORMAT_FLV,
- FORMAT_MKV,
- FORMAT_UNKNOWN
- } vpVideoFormatType;
-
- //!Video's format which has to be read
- vpVideoFormatType formatType;
-
- //!Path to the video
- char fileName[FILENAME_MAX];
- //!Indicates if the path to the video is set.
- bool initFileName;
- //!Indicates if the video is "open".
- bool isOpen;
- //!Count the frame number when the class is used as a grabber.
- long frameCount; // Index of the next image
- //!The first frame index
- long firstFrame;
- //!The last frame index
- long lastFrame;
- bool firstFrameIndexIsSet;
- bool lastFrameIndexIsSet;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpVideoReader(const vpVideoReader &)
-// : vpFrameGrabber(), imSequence(NULL),
-// #ifdef VISP_HAVE_FFMPEG
-// ffmpeg(NULL),
-// #elif VISP_HAVE_OPENCV_VERSION >= 0x020100
-// capture(), frame(),
-// #endif
-// formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0),
-// firstFrame(0), lastFrame(0), firstFrameIndexIsSet(false), lastFrameIndexIsSet(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpVideoReader &operator=(const vpVideoReader &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
-
- public:
- vpVideoReader();
- ~vpVideoReader();
-
- void acquire(vpImage< vpRGBa > &I);
- void acquire(vpImage< unsigned char > &I);
- void close(){;}
-
- /*!
- \return true if the end of the sequence is reached.
- */
- inline bool end() {
- if (frameCount > lastFrame )
+ //! Types of available formats
+ typedef enum {
+ FORMAT_PGM,
+ FORMAT_PPM,
+ FORMAT_JPEG,
+ FORMAT_PNG,
+ // Formats supported by opencv
+ FORMAT_TIFF,
+ FORMAT_BMP,
+ FORMAT_DIB,
+ FORMAT_PBM,
+ FORMAT_RASTER,
+ FORMAT_JPEG2000,
+ // Video format
+ FORMAT_AVI,
+ FORMAT_MPEG,
+ FORMAT_MPEG4,
+ FORMAT_MOV,
+ FORMAT_OGV,
+ FORMAT_WMV,
+ FORMAT_FLV,
+ FORMAT_MKV,
+ FORMAT_UNKNOWN
+ } vpVideoFormatType;
+
+ //! Video's format which has to be read
+ vpVideoFormatType formatType;
+
+ //! Path to the video
+ char fileName[FILENAME_MAX];
+ //! Indicates if the path to the video is set.
+ bool initFileName;
+ //! Indicates if the video is "open".
+ bool isOpen;
+ //! Count the frame number when the class is used as a grabber.
+ long frameCount; // Index of the next image
+ //! The first frame index
+ long firstFrame;
+ //! The last frame index
+ long lastFrame;
+ bool firstFrameIndexIsSet;
+ bool lastFrameIndexIsSet;
+ //! The frame step
+ long frameStep;
+ double frameRate;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpVideoReader(const vpVideoReader &)
+ // : vpFrameGrabber(), imSequence(NULL),
+ // #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)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpVideoReader &operator=(const vpVideoReader &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
+
+public:
+ vpVideoReader();
+ ~vpVideoReader();
+
+ void acquire(vpImage<vpRGBa> &I);
+ void acquire(vpImage<unsigned char> &I);
+ void close() { ; }
+
+ /*!
+ \return true if the end of the sequence is reached.
+ */
+ inline bool end()
+ {
+ if (frameStep > 0) {
+ if (frameCount + frameStep > lastFrame)
+ return true;
+ } else if (frameStep < 0) {
+ if (frameCount + frameStep < firstFrame)
return true;
- return false;
}
- bool getFrame(vpImage<vpRGBa> &I, long frame);
- bool getFrame(vpImage<unsigned char> &I, long frame);
- double getFramerate();
-
- /*!
- Get the frame index of the next image. This index is updated at each call of the
- acquire method. It can be used to detect the end of a file (comparison
- with getLastFrameIndex()).
-
- \return Returns the current frame index.
- */
- inline long getFrameIndex() const { return frameCount;}
-
- /*!
- Gets the first frame index.
-
- \return Returns the first frame index.
- */
- inline long getFirstFrameIndex() const {return firstFrame;}
- /*!
- Gets the last frame index.
-
- \return Returns the last frame index.
- */
- inline long getLastFrameIndex() const {return lastFrame;}
- void open (vpImage< vpRGBa > &I);
- void open (vpImage< unsigned char > &I);
-
- vpVideoReader & operator>>(vpImage<unsigned char> &I);
- vpVideoReader & operator>>(vpImage<vpRGBa> &I);
-
- /*!
- Reset the frame counter and sets it to the first image index.
-
- By default the first frame index is set to 0.
-
- This method is useful if you use the class like a frame grabber (ie with theacquire method).
- */
- inline void resetFrameCounter() {frameCount = firstFrame;}
- void setFileName(const char *filename);
- void setFileName(const std::string &filename);
- /*!
- Enables to set the first frame index if you want to use the class like a grabber (ie with the
- acquire method).
-
- \param first_frame : The first frame index.
-
- \sa setLastFrameIndex()
- */
- inline void setFirstFrameIndex(const long first_frame) {
- this->firstFrameIndexIsSet = true;
- this->firstFrame = first_frame;
+ return false;
+ }
+ bool getFrame(vpImage<vpRGBa> &I, long frame);
+ bool getFrame(vpImage<unsigned char> &I, long frame);
+ /*!
+ Return the frame rate in Hz used to encode the video stream.
+
+ If the video is a sequence of images, return -1.
+ */
+ double getFramerate()
+ {
+ if (!isOpen) {
+ getProperties();
}
- /*!
- Enables to set the last frame index.
+ return frameRate;
+ }
- \param last_frame : The last frame index.
+ /*!
+ Get the frame index of the current image. This index is updated at each
+ call of the acquire method. It can be used to detect the end of a file
+ (comparison with getLastFrameIndex()).
- \sa setFirstFrameIndex()
- */
- inline void setLastFrameIndex(const long last_frame) {
- this->lastFrameIndexIsSet = true;
- this->lastFrame = last_frame;
- }
+ \return Returns the current frame index.
+
+ \sa end()
+ */
+ inline long getFrameIndex() const { return frameCount; }
+
+ /*!
+ Gets the first frame index.
- private:
- vpVideoFormatType getFormat(const char *filename);
- static std::string getExtension(const std::string &filename);
- void findFirstFrameIndex();
- void findLastFrameIndex();
- bool isImageExtensionSupported();
- bool isVideoExtensionSupported();
+ \return Returns the first frame index.
+ */
+ inline long getFirstFrameIndex()
+ {
+ if (!isOpen) {
+ getProperties();
+ }
+ return firstFrame;
+ }
+ /*!
+ Gets the last frame index.
+
+ \return Returns the last frame index.
+ */
+ inline long getLastFrameIndex()
+ {
+ if (!isOpen) {
+ getProperties();
+ }
+ return lastFrame;
+ }
+ /*!
+ Gets the frame step.
+
+ \return Returns the frame step value.
+ */
+ inline long getFrameStep() const { return frameStep; }
+ void open(vpImage<vpRGBa> &I);
+ void open(vpImage<unsigned char> &I);
+
+ vpVideoReader &operator>>(vpImage<unsigned char> &I);
+ vpVideoReader &operator>>(vpImage<vpRGBa> &I);
+
+ /*!
+ Reset the frame counter and sets it to the first image index.
+
+ By default the first frame index is set to 0.
+
+ This method is useful if you use the class like a frame grabber (ie with
+ theacquire method).
+ */
+ inline void resetFrameCounter() { frameCount = firstFrame; }
+ void setFileName(const char *filename);
+ void setFileName(const std::string &filename);
+ /*!
+ Enables to set the first frame index if you want to use the class like a
+ grabber (ie with the acquire method).
+
+ \param first_frame : The first frame index.
+
+ \sa setLastFrameIndex()
+ */
+ inline void setFirstFrameIndex(const long first_frame)
+ {
+ this->firstFrameIndexIsSet = true;
+ this->firstFrame = first_frame;
+ }
+ /*!
+ Enables to set the last frame index.
+
+ \param last_frame : The last frame index.
+
+ \sa setFirstFrameIndex()
+ */
+ inline void setLastFrameIndex(const long last_frame)
+ {
+ this->lastFrameIndexIsSet = true;
+ this->lastFrame = last_frame;
+ }
+
+ /*!
+ Sets the frame step index.
+ The default frame step is 1
+
+ \param frame_step : The frame index step.
+
+ \sa setFrameStep()
+*/
+ inline void setFrameStep(const long frame_step) { this->frameStep = frame_step; }
+
+private:
+ vpVideoFormatType getFormat(const char *filename);
+ static std::string getExtension(const std::string &filename);
+ void findFirstFrameIndex();
+ void findLastFrameIndex();
+ bool isImageExtensionSupported();
+ bool isVideoExtensionSupported();
+ long extractImageIndex(const std::string &imageName, const std::string &format);
+ bool checkImageNameFormat(const std::string &format);
+ void getProperties();
};
#endif
diff --git a/modules/io/include/visp3/io/vpVideoWriter.h b/modules/io/include/visp3/io/vpVideoWriter.h
index a6bbd26..09a76a4 100644
--- a/modules/io/include/visp3/io/vpVideoWriter.h
+++ b/modules/io/include/visp3/io/vpVideoWriter.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,77 +47,78 @@
#include <string>
#include <visp3/io/vpImageIo.h>
-#include <visp3/io/vpFFMPEG.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020200
-# include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui.hpp>
#elif VISP_HAVE_OPENCV_VERSION >= 0x020000
-# include <opencv/highgui.h>
+#include <opencv/highgui.h>
#endif
-
/*!
\class vpVideoWriter
\ingroup group_io_video
- \brief Class that enables to write easily a video file or a sequence of images.
-
- This class has its own implementation to write a sequence of PGM and PPM images.
+ \brief Class that enables to write easily a video file or a sequence of
+images.
+
+ This class has its own implementation to write a sequence of PGM and PPM
+images.
This class may benefit from optional 3rd parties:
- - libpng: If installed this optional 3rd party is used to write a sequence of PNG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_png.
- - libjpeg: If installed this optional 3rd party is used to write a sequence of JPEG images.
- Installation instructions are provided here https://visp.inria.fr/3rd_jpeg.
- - OpenCV: If installed this optional 3rd party is used to write a sequence of images
- where images could be in TIFF, BMP, DIB, PBM, RASTER, JPEG2000 format. If libpng or
- libjpeg is not installed, OpenCV is also used to consider these image formats. OpenCV
- allows also to consider AVI, MPEG, MPEG4, MOV, OGV, WMV, FLV, MKV video formats.
- Installation instructions are provided here https://visp.inria.fr/3rd_opencv.
- - ffmpeg: If installed and enabled this optional 3rd party is used to write
- AVI, MPEG, MPEG4, MOV, OGV, WMV, FLV, MKV video formats. It means that if OpenCV is
- also installed, that OpenCV is not used to read a video. Installation instructions
- are provided here https://visp.inria.fr/3rd_ffmpeg.
-
- The following example available in tutorial-video-recorder.cpp shows how this
- class can be used to record a video from a camera by default in an mpeg file.
- \include tutorial-video-recorder.cpp
-
- The following example shows also how this class can be used to write an image sequence.
- The images are stored in the folder "./image" and are named "image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ...
-
+ - libpng: If installed this optional 3rd party is used to write a sequence
+of PNG images. Installation instructions are provided here
+https://visp.inria.fr/3rd_png.
+ - libjpeg: If installed this optional 3rd party is used to write a sequence
+of JPEG images. Installation instructions are provided here
+https://visp.inria.fr/3rd_jpeg.
+ - OpenCV: If installed this optional 3rd party is used to write a sequence
+of images where images could be in TIFF, BMP, DIB, PBM, RASTER, JPEG2000
+format. If libpng or libjpeg is not installed, OpenCV is also used to consider
+these image formats. OpenCV allows also to consider AVI, MPEG, MPEG4, MOV,
+OGV, WMV, FLV, MKV video formats. Installation instructions are provided here
+https://visp.inria.fr/3rd_opencv.
+
+ The following example available in tutorial-video-recorder.cpp shows how
+this class can be used to record a video from a camera by default in an mpeg
+file. \include tutorial-video-recorder.cpp
+
+ The following example shows also how this class can be used to write an
+image sequence. The images are stored in the folder "./image" and are named
+"image0000.jpeg", "image0001.jpeg", "image0002.jpeg", ...
+
\code
#include <visp3/core/vpConfig.h>
#include <visp3/io/vpVideoWriter.h>
-
+
int main()
{
vpImage<vpRGBa> I;
vpVideoWriter writer;
-
+
//Initialize the writer.
writer.setFileName("./image/image%04d.jpeg");
-
+
writer.open(I);
-
+
for ( ; ; )
{
//Here the code to capture or create an image and stores it in I.
-
+
//Save the image
writer.saveFrame(I);
}
-
+
writer.close();
return 0;
}
\endcode
-
- The other following example explains how to use the class to write directly an mpeg file.
-
+
+ The other following example explains how to use the class to write directly
+an mpeg file.
+
\code
#include <visp3/io/vpVideoWriter.h>
@@ -129,16 +131,7 @@ int main()
// Set up the framerate to 30Hz. Default is 25Hz.
writer.setFramerate(30);
-#ifdef VISP_HAVE_FFMPEG
- // Set up the bit rate
- writer.setBitRate(1000000);
- // Set up the codec to use
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- writer.setCodec(CODEC_ID_MPEG2VIDEO);
-#else
- writer.setCodec(AV_CODEC_ID_MPEG2VIDEO);
-#endif
-#elif defined VISP_HAVE_OPENCV
+#if defined VISP_HAVE_OPENCV
writer.setCodec( CV_FOURCC('P','I','M','1') );
#endif
writer.setFileName("./test.mpeg");
@@ -161,151 +154,99 @@ int main()
*/
class VISP_EXPORT vpVideoWriter
-{
- private:
-#ifdef VISP_HAVE_FFMPEG
- //!To read video files
- vpFFMPEG *ffmpeg;
- //!The codec to use
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- CodecID codec;
-#else
- AVCodecID codec;
-#endif
- //!The bite rate
- unsigned int bit_rate;
- int framerate;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- cv::VideoWriter writer;
- int fourcc;
- double framerate;
+{
+private:
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ cv::VideoWriter writer;
+ int fourcc;
+ double framerate;
#endif
- //!Types of available formats
- typedef enum
- {
- FORMAT_PGM,
- FORMAT_PPM,
- FORMAT_JPEG,
- FORMAT_PNG,
- FORMAT_AVI,
- FORMAT_MPEG,
- FORMAT_MPEG4,
- FORMAT_MOV,
- FORMAT_UNKNOWN
- } vpVideoFormatType;
-
- //!Video's format which has to be writen
- vpVideoFormatType formatType;
-
- //!Path to the image sequence
- char fileName[FILENAME_MAX];
-
- //!Indicates if the path to the image sequence is set.
- bool initFileName;
-
- //!Indicates if the video is "open".
- bool isOpen;
-
- //!Count the frame number.
- unsigned int frameCount;
-
- //!The first frame index.
- unsigned int firstFrame;
-
- //!Size of the frame
- unsigned int width;
- unsigned int height;
-
- public:
- vpVideoWriter();
- ~vpVideoWriter();
-
- void close();
-
- /*!
- Gets the current frame index.
-
- \return Returns the current frame index.
- */
- inline unsigned int getCurrentFrameIndex() const {return frameCount;}
+ //! Types of available formats
+ typedef enum {
+ FORMAT_PGM,
+ FORMAT_PPM,
+ FORMAT_JPEG,
+ FORMAT_PNG,
+ FORMAT_AVI,
+ FORMAT_MPEG,
+ FORMAT_MPEG4,
+ FORMAT_MOV,
+ FORMAT_UNKNOWN
+ } vpVideoFormatType;
- void open (vpImage< vpRGBa > &I);
- void open (vpImage< unsigned char > &I);
- /*!
- Reset the frame counter and sets it to the first image index.
+ //! Video's format which has to be writen
+ vpVideoFormatType formatType;
- By default the first frame index is set to 0.
- */
- inline void resetFrameCounter() {frameCount = firstFrame;}
+ //! Path to the image sequence
+ char fileName[FILENAME_MAX];
- void saveFrame (vpImage< vpRGBa > &I);
- void saveFrame (vpImage< unsigned char > &I);
+ //! Indicates if the path to the image sequence is set.
+ bool initFileName;
-#ifdef VISP_HAVE_FFMPEG
- /*!
- Sets the bit rate of the video when encoding.
+ //! Indicates if the video is "open".
+ bool isOpen;
- \param bitrate : the expected bit rate.
+ //! Count the frame number.
+ unsigned int frameCount;
- By default the bit rate is set to 500 000.
- */
- inline void setBitRate(const unsigned int bitrate) {this->bit_rate = bitrate;}
+ //! The first frame index.
+ unsigned int firstFrame;
- /*!
- Sets the codec used to encode the video.
+ //! Size of the frame
+ unsigned int width;
+ unsigned int height;
- \param codec_id : the expected codec.
+public:
+ vpVideoWriter();
+ ~vpVideoWriter();
- By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
- AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
- AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
+ void close();
- Of course to use the codec it must be installed on your computer.
- */
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- inline void setCodec(const CodecID codec_id) {this->codec = codec_id;}
-#else
- inline void setCodec(const AVCodecID codec_id) {this->codec = codec_id;}
-#endif
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- inline void setCodec(const int fourcc_codec) {this->fourcc = fourcc_codec;}
-#endif
+ /*!
+ Gets the current frame index.
- void setFileName(const char *filename);
- void setFileName(const std::string &filename);
- /*!
- Enables to set the first frame index.
-
- \param first_frame : The first frame index.
- */
- inline void setFirstFrameIndex(const unsigned int first_frame) {this->firstFrame = first_frame;}
-#ifdef VISP_HAVE_FFMPEG
- /*!
- Sets the framerate in Hz of the video when encoding.
+ \return Returns the current frame index.
+ */
+ inline unsigned int getCurrentFrameIndex() const { return frameCount; }
- \param frame_rate : the expected framerate.
+ void open(vpImage<vpRGBa> &I);
+ void open(vpImage<unsigned char> &I);
+ /*!
+ Reset the frame counter and sets it to the first image index.
- By default the framerate is set to 25Hz.
- */
- inline void setFramerate(const int frame_rate) {
- this->framerate = frame_rate;
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- /*!
+ By default the first frame index is set to 0.
+ */
+ inline void resetFrameCounter() { frameCount = firstFrame; }
+
+ void saveFrame(vpImage<vpRGBa> &I);
+ void saveFrame(vpImage<unsigned char> &I);
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ inline void setCodec(const int fourcc_codec) { this->fourcc = fourcc_codec; }
+#endif
+
+ void setFileName(const char *filename);
+ void setFileName(const std::string &filename);
+ /*!
+ Enables to set the first frame index.
+
+ \param first_frame : The first frame index.
+ */
+ inline void setFirstFrameIndex(const unsigned int first_frame) { this->firstFrame = first_frame; }
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ /*!
Sets the framerate in Hz of the video when encoding.
\param frame_rate : the expected framerate.
By default the framerate is set to 25Hz.
*/
- inline void setFramerate(const double frame_rate) {
- this->framerate = frame_rate;
- }
+ inline void setFramerate(const double frame_rate) { this->framerate = frame_rate; }
#endif
- private:
- vpVideoFormatType getFormat(const char *filename);
- static std::string getExtension(const std::string &filename);
+private:
+ vpVideoFormatType getFormat(const char *filename);
+ static std::string getExtension(const std::string &filename);
};
#endif
diff --git a/modules/io/src/image/vpImageIo.cpp b/modules/io/src/image/vpImageIo.cpp
index 1b86d56..007b239 100644
--- a/modules/io/src/image/vpImageIo.cpp
+++ b/modules/io/src/image/vpImageIo.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,12 +42,12 @@
*/
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h> //image conversion
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
-void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic,
- unsigned int &w, unsigned int &h, unsigned int &maxval);
+void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w,
+ unsigned int &h, unsigned int &maxval);
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
@@ -58,63 +59,64 @@ void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const st
* \param h[out] : Image height.
* \param maxval[out] : Maximum pixel value.
*/
-void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w, unsigned int &h, unsigned int &maxval)
+void vp_decodeHeaderPNM(const std::string &filename, std::ifstream &fd, const std::string &magic, unsigned int &w,
+ unsigned int &h, unsigned int &maxval)
{
std::string line;
- unsigned int nb_elt = 4, cpt_elt=0;
- while(cpt_elt != nb_elt) {
+ 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<std::string> header = vpIoTools::splitChain(line, std::string(" "));
- if(header.size() == 0) {
+ 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
+ cpt_elt++;
+ header.erase(header.begin(),
+ header.begin() + 1); // erase first element that is processed
}
- while(header.size()) {
+ while (header.size()) {
if (cpt_elt == 1) { // decode width
std::istringstream ss(header[0]);
ss >> w;
- cpt_elt ++;
- header.erase(header.begin(), header.begin()+1); // erase first element that is processed
- }
- else if (cpt_elt == 2) { // decode height
+ cpt_elt++;
+ header.erase(header.begin(),
+ header.begin() + 1); // erase first element that is processed
+ } else if (cpt_elt == 2) { // decode height
std::istringstream ss(header[0]);
ss >> h;
- cpt_elt ++;
- header.erase(header.begin(), header.begin()+1); // erase first element that is processed
- }
- else if (cpt_elt == 3) { // decode maxval
+ cpt_elt++;
+ header.erase(header.begin(),
+ header.begin() + 1); // erase first element that is processed
+ } else if (cpt_elt == 3) { // decode maxval
std::istringstream ss(header[0]);
ss >> maxval;
- cpt_elt ++;
- header.erase(header.begin(), header.begin()+1); // erase first element that is processed
+ cpt_elt++;
+ header.erase(header.begin(),
+ header.begin() + 1); // erase first element that is processed
}
}
}
}
#endif
-vpImageIo::vpImageFormatType
-vpImageIo::getFormat(const std::string &filename)
+vpImageIo::vpImageFormatType vpImageIo::getFormat(const std::string &filename)
{
std::string ext = vpImageIo::getExtension(filename);
@@ -180,14 +182,14 @@ std::string vpImageIo::getExtension(const std::string &filename)
{
// extract the extension
size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
+ std::string ext = filename.substr(dot, filename.size() - 1);
return ext;
}
-
/*!
- Read the contents of the image filename, allocate memory for the corresponding
- greyscale image, update its content, and return a reference to the image.
+ Read the contents of the image filename, allocate memory for the
+ corresponding greyscale image, update its content, and return a reference to
+ the image.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -196,82 +198,85 @@ std::string vpImageIo::getExtension(const std::string &filename)
Always supported formats are *.pgm and *.ppm.
If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras,
+ *.tiff, *.tif, *.png, *.bmp, *.pbm files.
\param I : Image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::read(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::read(vpImage<unsigned char> &I, const std::string &filename)
{
bool exist = vpIoTools::checkFilename(filename);
if (!exist) {
std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
- throw (vpImageException(vpImageException::ioError, message));
+ throw(vpImageException(vpImageException::ioError, message));
}
- //Allows to use ~ symbol or env variables in path
+ // Allows to use ~ symbol or env variables in path
std::string final_filename = vpIoTools::path(filename);
bool try_opencv_reader = false;
- switch(getFormat(final_filename.c_str())){
- case FORMAT_PGM :
- readPGM(I,final_filename); break;
- case FORMAT_PPM :
- readPPM(I,final_filename); break;
- case FORMAT_JPEG :
+ switch (getFormat(final_filename)) {
+ case FORMAT_PGM:
+ readPGM(I, final_filename);
+ break;
+ case FORMAT_PPM:
+ readPPM(I, final_filename);
+ break;
+ case FORMAT_JPEG:
#ifdef VISP_HAVE_JPEG
- readJPEG(I,final_filename);
+ readJPEG(I, final_filename);
#else
try_opencv_reader = true;
#endif
break;
- case FORMAT_PNG :
+ case FORMAT_PNG:
#if defined(VISP_HAVE_PNG)
- readPNG(I,final_filename);
+ readPNG(I, final_filename);
#else
try_opencv_reader = true;
#endif
break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
+ case FORMAT_TIFF:
+ case FORMAT_BMP:
+ case FORMAT_DIB:
+ case FORMAT_PBM:
+ case FORMAT_RASTER:
+ case FORMAT_JPEG2000:
+ case FORMAT_UNKNOWN:
try_opencv_reader = true;
break;
}
if (try_opencv_reader) {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- //std::cout << "Use opencv to read the image" << std::endl;
+ // 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";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
}
vpImageConvert::convert(cvI, I);
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- //std::cout << "Use opencv to read the image" << std::endl;
+ // 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";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
}
vpImageConvert::convert(cvI, I);
#else
std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
#endif
}
}
/*!
- Read the contents of the image filename, allocate memory for the corresponding
- color image, update its content, and return a reference to the image.
+ Read the contents of the image filename, allocate memory for the
+ corresponding color image, update its content, and return a reference to the
+ image.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -280,50 +285,52 @@ vpImageIo::read(vpImage<unsigned char> &I, const std::string &filename)
Always supported formats are *.pgm and *.ppm.
If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras,
+ *.tiff, *.tif, *.png, *.bmp, *.pbm files.
\param I : Image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::read(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::read(vpImage<vpRGBa> &I, const std::string &filename)
{
bool exist = vpIoTools::checkFilename(filename);
if (!exist) {
std::string message = "Cannot read file: \"" + std::string(filename) + "\" doesn't exist";
- throw (vpImageException(vpImageException::ioError, message));
+ throw(vpImageException(vpImageException::ioError, message));
}
- //Allows to use ~ symbol or env variables in path
+ // Allows to use ~ symbol or env variables in path
std::string final_filename = vpIoTools::path(filename);
bool try_opencv_reader = false;
- switch(getFormat(final_filename.c_str())){
- case FORMAT_PGM :
- readPGM(I,final_filename); break;
- case FORMAT_PPM :
- readPPM(I,final_filename); break;
- case FORMAT_JPEG :
+ switch (getFormat(final_filename)) {
+ case FORMAT_PGM:
+ readPGM(I, final_filename);
+ break;
+ case FORMAT_PPM:
+ readPPM(I, final_filename);
+ break;
+ case FORMAT_JPEG:
#ifdef VISP_HAVE_JPEG
- readJPEG(I,final_filename);
+ readJPEG(I, final_filename);
#else
try_opencv_reader = true;
#endif
break;
- case FORMAT_PNG :
+ case FORMAT_PNG:
#if defined(VISP_HAVE_PNG)
- readPNG(I,final_filename);
+ readPNG(I, final_filename);
#else
try_opencv_reader = true;
#endif
break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
+ case FORMAT_TIFF:
+ case FORMAT_BMP:
+ case FORMAT_DIB:
+ case FORMAT_PBM:
+ case FORMAT_RASTER:
+ case FORMAT_JPEG2000:
+ case FORMAT_UNKNOWN:
try_opencv_reader = true;
break;
}
@@ -334,7 +341,7 @@ vpImageIo::read(vpImage<vpRGBa> &I, const std::string &filename)
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";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
}
vpImageConvert::convert(cvI, I);
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
@@ -342,12 +349,12 @@ vpImageIo::read(vpImage<vpRGBa> &I, const std::string &filename)
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";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
}
vpImageConvert::convert(cvI, I);
#else
std::string message = "Cannot read file \"" + std::string(final_filename) + "\": Image format not supported";
- throw (vpImageException(vpImageException::ioError, message)) ;
+ throw(vpImageException(vpImageException::ioError, message));
#endif
}
}
@@ -359,42 +366,44 @@ vpImageIo::read(vpImage<vpRGBa> &I, const std::string &filename)
Always supported formats are *.pgm and *.ppm.
If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras,
+ *.tiff, *.tif, *.png, *.bmp, *.pbm files.
\param I : Image to write.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::write(const vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::write(const vpImage<unsigned char> &I, const std::string &filename)
{
bool try_opencv_writer = false;
- switch(getFormat(filename)){
- case FORMAT_PGM :
- writePGM(I,filename); break;
- case FORMAT_PPM :
- writePPM(I,filename); break;
- case FORMAT_JPEG :
+ switch (getFormat(filename)) {
+ case FORMAT_PGM:
+ writePGM(I, filename);
+ break;
+ case FORMAT_PPM:
+ writePPM(I, filename);
+ break;
+ case FORMAT_JPEG:
#ifdef VISP_HAVE_JPEG
- writeJPEG(I,filename);
+ writeJPEG(I, filename);
#else
try_opencv_writer = true;
#endif
break;
- case FORMAT_PNG :
+ case FORMAT_PNG:
#ifdef VISP_HAVE_PNG
- writePNG(I,filename);
+ writePNG(I, filename);
#else
try_opencv_writer = true;
#endif
break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
+ case FORMAT_TIFF:
+ case FORMAT_BMP:
+ case FORMAT_DIB:
+ case FORMAT_PBM:
+ case FORMAT_RASTER:
+ case FORMAT_JPEG2000:
+ case FORMAT_UNKNOWN:
try_opencv_writer = true;
break;
}
@@ -407,8 +416,7 @@ vpImageIo::write(const vpImage<unsigned char> &I, const std::string &filename)
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")) ;
+ throw(vpImageException(vpImageException::ioError, "Cannot write file: Image format not supported"));
#endif
}
}
@@ -420,42 +428,44 @@ vpImageIo::write(const vpImage<unsigned char> &I, const std::string &filename)
Always supported formats are *.pgm and *.ppm.
If \c libjpeg 3rd party is used, we support also *.jpg and *.jpeg files.
If \c libpng 3rd party is used, we support also *.png files.
- If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras, *.tiff, *.tif, *.png, *.bmp, *.pbm files.
+ If OpenCV 3rd party is used, we support *.jpg, *.jpeg, *.jp2, *.rs, *.ras,
+ *.tiff, *.tif, *.png, *.bmp, *.pbm files.
\param I : Image to write.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::write(const vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::write(const vpImage<vpRGBa> &I, const std::string &filename)
{
bool try_opencv_writer = false;
- switch(getFormat(filename)){
- case FORMAT_PGM :
- writePGM(I,filename); break;
- case FORMAT_PPM :
- writePPM(I,filename); break;
- case FORMAT_JPEG :
+ switch (getFormat(filename)) {
+ case FORMAT_PGM:
+ writePGM(I, filename);
+ break;
+ case FORMAT_PPM:
+ writePPM(I, filename);
+ break;
+ case FORMAT_JPEG:
#ifdef VISP_HAVE_JPEG
- writeJPEG(I,filename);
+ writeJPEG(I, filename);
#else
try_opencv_writer = true;
#endif
break;
- case FORMAT_PNG :
+ case FORMAT_PNG:
#ifdef VISP_HAVE_PNG
- writePNG(I,filename);
+ writePNG(I, filename);
#else
try_opencv_writer = true;
#endif
break;
- case FORMAT_TIFF :
- case FORMAT_BMP :
- case FORMAT_DIB :
- case FORMAT_PBM :
- case FORMAT_RASTER :
- case FORMAT_JPEG2000 :
- case FORMAT_UNKNOWN :
+ case FORMAT_TIFF:
+ case FORMAT_BMP:
+ case FORMAT_DIB:
+ case FORMAT_PBM:
+ case FORMAT_RASTER:
+ case FORMAT_JPEG2000:
+ case FORMAT_UNKNOWN:
try_opencv_writer = true;
break;
}
@@ -468,8 +478,7 @@ vpImageIo::write(const vpImage<vpRGBa> &I, const std::string &filename)
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")) ;
+ throw(vpImageException(vpImageException::ioError, "Cannot write file: Image format not supported"));
#endif
}
}
@@ -487,43 +496,39 @@ vpImageIo::write(const vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writePFM(const vpImage<float> &I, const std::string &filename)
+void vpImageIo::writePFM(const vpImage<float> &I, const std::string &filename)
{
- FILE* fd;
+ FILE *fd;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot write PFM image: filename empty")) ;
+ if (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
- fprintf(fd, "P8\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
+ fprintf(fd, "P8\n"); // Magic number
+ fprintf(fd, "%u %u\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
// Write the bitmap
size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
+ size_t nbyte = I.getWidth() * I.getHeight();
- ierr = fwrite(I.bitmap, sizeof(float), nbyte, fd) ;
+ 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);
fclose(fd);
-
}
//--------------------------------------------------------------------------
// PGM
@@ -537,39 +542,36 @@ vpImageIo::writePFM(const vpImage<float> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writePGM(const vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::writePGM(const vpImage<unsigned char> &I, const std::string &filename)
{
- FILE* fd;
+ FILE *fd;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create PGM file: filename empty")) ;
+ if (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
- fprintf(fd, "P5\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
+ fprintf(fd, "P5\n"); // Magic number
+ fprintf(fd, "%u %u\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
// Write the bitmap
size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
+ size_t nbyte = I.getWidth() * I.getHeight();
- ierr = fwrite(I.bitmap, sizeof(unsigned char), nbyte, fd) ;
+ 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);
@@ -583,19 +585,18 @@ vpImageIo::writePGM(const vpImage<unsigned char> &I, const std::string &filename
\param I : Image to save as a (PGM P5) file.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writePGM(const vpImage<short> &I, const std::string &filename)
+void vpImageIo::writePGM(const vpImage<short> &I, const std::string &filename)
{
- vpImage<unsigned char> Iuc ;
+ vpImage<unsigned char> Iuc;
unsigned int nrows = I.getHeight();
unsigned int ncols = I.getWidth();
Iuc.resize(nrows, ncols);
- for (unsigned int i=0 ; i < nrows * ncols ; i++)
- Iuc.bitmap[i] = (unsigned char)I.bitmap[i] ;
+ for (unsigned int i = 0; i < nrows * ncols; i++)
+ Iuc.bitmap[i] = (unsigned char)I.bitmap[i];
- vpImageIo::writePGM(Iuc, filename) ;
+ vpImageIo::writePGM(Iuc, filename);
}
/*!
Write the content of the image bitmap in the file which name is given by \e
@@ -606,42 +607,39 @@ vpImageIo::writePGM(const vpImage<short> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writePGM(const vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writePGM(const vpImage<vpRGBa> &I, const std::string &filename)
{
- FILE* fd;
+ FILE *fd;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create PGM file: filename empty")) ;
+ if (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
- fprintf(fd, "P5\n"); // Magic number
- fprintf(fd, "%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(fd, "255\n"); // Max level
+ fprintf(fd, "P5\n"); // Magic number
+ fprintf(fd, "%u %u\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(fd, "255\n"); // Max level
// Write the bitmap
size_t ierr;
- size_t nbyte = I.getWidth()*I.getHeight();
+ size_t nbyte = I.getWidth() * I.getHeight();
- vpImage<unsigned char> Itmp ;
- vpImageConvert::convert(I,Itmp) ;
+ vpImage<unsigned char> Itmp;
+ vpImageConvert::convert(I, Itmp);
- ierr = fwrite(Itmp.bitmap, sizeof(unsigned char), nbyte, fd) ;
+ 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);
@@ -664,49 +662,45 @@ vpImageIo::writePGM(const vpImage<vpRGBa> &I, const std::string &filename)
*/
-void
-vpImageIo::readPFM(vpImage<float> &I, const std::string &filename)
+void vpImageIo::readPFM(vpImage<float> &I, const std::string &filename)
{
- unsigned int w=0, h=0, maxval=0;
+ unsigned int w = 0, h = 0, maxval = 0;
unsigned int w_max = 100000, h_max = 100000, maxval_max = 255;
std::string magic("P8");
std::ifstream fd(filename.c_str(), std::ios::binary);
// Open the filename
- if(! fd.is_open()) {
- throw (vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())) ;
+ if (!fd.is_open()) {
+ 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)
- {
+ 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())) {
- I.resize(h,w) ;
+ if ((h != I.getHeight()) || (w != I.getWidth())) {
+ I.resize(h, w);
}
- unsigned int nbyte = I.getHeight()*I.getWidth();
- fd.read ((char *)I.bitmap, sizeof(float) * nbyte);
- if (! fd) {
+ unsigned int nbyte = I.getHeight() * I.getWidth();
+ 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();
}
-
/*!
Read a PGM P5 file and initialize a scalar image.
@@ -722,43 +716,40 @@ vpImageIo::readPFM(vpImage<float> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPGM(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readPGM(vpImage<unsigned char> &I, const std::string &filename)
{
- unsigned int w=0, h=0, maxval=0;
+ unsigned int w = 0, h = 0, maxval = 0;
unsigned int w_max = 100000, h_max = 100000, maxval_max = 255;
std::string magic("P5");
std::ifstream fd(filename.c_str(), std::ios::binary);
// Open the filename
- if(! fd.is_open()) {
- throw (vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())) ;
+ if (!fd.is_open()) {
+ 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)
- {
+ 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())) {
- I.resize(h,w) ;
+ if ((h != I.getHeight()) || (w != I.getWidth())) {
+ I.resize(h, w);
}
- unsigned int nbyte = I.getHeight()*I.getWidth();
- fd.read ((char *)I.bitmap, nbyte);
- if (! fd) {
+ unsigned int nbyte = I.getHeight() * I.getWidth();
+ 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();
@@ -782,17 +773,15 @@ vpImageIo::readPGM(vpImage<unsigned char> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string &filename)
{
- vpImage<unsigned char> Itmp ;
+ vpImage<unsigned char> Itmp;
- vpImageIo::readPGM(Itmp, filename) ;
+ vpImageIo::readPGM(Itmp, filename);
- vpImageConvert::convert(Itmp, I) ;
+ vpImageConvert::convert(Itmp, I);
}
-
//--------------------------------------------------------------------------
// PPM
//--------------------------------------------------------------------------
@@ -800,8 +789,8 @@ vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string &filename)
/*!
Read the contents of the portable pixmap (PPM P6) filename, allocate memory
for the corresponding gray level image, convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
+ set the bitmap whith the gray level data. That means that the image \e I is
+ a "black and white" rendering of the original image in \e filename, as in a
black and white photograph. The quantization formula used is \f$0,299 r +
0,587 g + 0,114 b\f$.
@@ -813,17 +802,15 @@ vpImageIo::readPGM(vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPPM(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readPPM(vpImage<unsigned char> &I, const std::string &filename)
{
- vpImage<vpRGBa> Itmp ;
+ vpImage<vpRGBa> Itmp;
- vpImageIo::readPPM(Itmp, filename) ;
+ vpImageIo::readPPM(Itmp, filename);
- vpImageConvert::convert(Itmp, I) ;
+ vpImageConvert::convert(Itmp, I);
}
-
/*!
Read the contents of the portable pixmap (PPM P6) filename,
allocate memory for the corresponding vpRGBa image.
@@ -835,47 +822,43 @@ vpImageIo::readPPM(vpImage<unsigned char> &I, const std::string &filename)
\param I : Image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPPM(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readPPM(vpImage<vpRGBa> &I, const std::string &filename)
{
- unsigned int w=0, h=0, maxval=0;
+ unsigned int w = 0, h = 0, maxval = 0;
unsigned int w_max = 100000, h_max = 100000, maxval_max = 255;
std::string magic("P6");
std::ifstream fd(filename.c_str(), std::ios::binary);
// Open the filename
- if(! fd.is_open()) {
- throw (vpImageException(vpImageException::ioError, "Cannot open file \"%s\"", filename.c_str())) ;
+ if (!fd.is_open()) {
+ 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)
- {
+ 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())) {
- I.resize(h,w) ;
+ if ((h != I.getHeight()) || (w != I.getWidth())) {
+ I.resize(h, w);
}
- for(unsigned int i=0;i<I.getHeight();i++) {
- for(unsigned int j=0;j<I.getWidth();j++) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
unsigned char rgb[3];
fd.read((char *)&rgb, 3);
- if (! fd) {
+ 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];
@@ -898,17 +881,15 @@ vpImageIo::readPPM(vpImage<vpRGBa> &I, const std::string &filename)
*/
-void
-vpImageIo::writePPM(const vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::writePPM(const vpImage<unsigned char> &I, const std::string &filename)
{
- vpImage<vpRGBa> Itmp ;
+ vpImage<vpRGBa> Itmp;
- vpImageConvert::convert(I, Itmp) ;
+ vpImageConvert::convert(I, Itmp);
- vpImageIo::writePPM(Itmp, filename) ;
+ vpImageIo::writePPM(Itmp, filename);
}
-
/*!
Write the content of the bitmap in the file which name is given by \e
filename. This function writes a portable gray pixmap (PPM P6) file.
@@ -916,32 +897,27 @@ vpImageIo::writePPM(const vpImage<unsigned char> &I, const std::string &filename
\param I : Image to save as a (PPM P6) file.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writePPM(const vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writePPM(const vpImage<vpRGBa> &I, const std::string &filename)
{
- FILE* f;
+ FILE *f;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create PPM file: filename empty")) ;
+ if (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
- fprintf(f,"%d %d\n", I.getWidth(), I.getHeight()); // Image size
- fprintf(f,"%d\n", 255); // Max level
+ fprintf(f, "P6\n"); // Magic number
+ fprintf(f, "%u %u\n", I.getWidth(), I.getHeight()); // Image size
+ fprintf(f, "%d\n", 255); // Max level
- for(unsigned int i=0;i<I.getHeight();i++)
- {
- for(unsigned int j=0;j<I.getWidth();j++)
- {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
vpRGBa v = I[i][j];
unsigned char rgb[3];
rgb[0] = v.R;
@@ -951,8 +927,7 @@ vpImageIo::writePPM(const vpImage<vpRGBa> &I, const std::string &filename)
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()));
}
}
}
@@ -974,8 +949,7 @@ vpImageIo::writePPM(const vpImage<vpRGBa> &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<unsigned char> &I, const std::string &filename)
+void vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filename)
{
struct jpeg_compress_struct cinfo;
struct jpeg_error_mgr jerr;
@@ -985,16 +959,14 @@ vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filenam
jpeg_create_compress(&cinfo);
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create JPEG file: filename empty")) ;
+ if (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();
@@ -1008,15 +980,13 @@ vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filenam
cinfo.in_color_space = JCS_GRAYSCALE;
jpeg_set_defaults(&cinfo);
- jpeg_start_compress(&cinfo,TRUE);
+ jpeg_start_compress(&cinfo, TRUE);
unsigned char *line;
line = new unsigned char[width];
- unsigned char* input = (unsigned char*)I.bitmap;
- while (cinfo.next_scanline < cinfo.image_height)
- {
- for (unsigned int i = 0; i < width; i++)
- {
+ unsigned char *input = (unsigned char *)I.bitmap;
+ while (cinfo.next_scanline < cinfo.image_height) {
+ for (unsigned int i = 0; i < width; i++) {
line[i] = *(input);
input++;
}
@@ -1025,7 +995,7 @@ vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filenam
jpeg_finish_compress(&cinfo);
jpeg_destroy_compress(&cinfo);
- delete [] line;
+ delete[] line;
fclose(file);
}
@@ -1036,8 +1006,7 @@ vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filenam
\param I : Image to save as a JPEG file.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
{
struct jpeg_compress_struct cinfo;
struct jpeg_error_mgr jerr;
@@ -1047,16 +1016,14 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
jpeg_create_compress(&cinfo);
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create JPEG file: filename empty")) ;
+ if (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();
@@ -1070,18 +1037,19 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
cinfo.in_color_space = JCS_RGB;
jpeg_set_defaults(&cinfo);
- jpeg_start_compress(&cinfo,TRUE);
+ jpeg_start_compress(&cinfo, TRUE);
unsigned char *line;
- line = new unsigned char[3*width];
- unsigned char* input = (unsigned char*)I.bitmap;
- while (cinfo.next_scanline < cinfo.image_height)
- {
- for (unsigned int i = 0; i < width; i++)
- {
- line[i*3] = *(input); input++;
- line[i*3+1] = *(input); input++;
- line[i*3+2] = *(input); input++;
+ line = new unsigned char[3 * width];
+ unsigned char *input = (unsigned char *)I.bitmap;
+ while (cinfo.next_scanline < cinfo.image_height) {
+ for (unsigned int i = 0; i < width; i++) {
+ line[i * 3] = *(input);
+ input++;
+ line[i * 3 + 1] = *(input);
+ input++;
+ line[i * 3 + 2] = *(input);
+ input++;
input++;
}
jpeg_write_scanlines(&cinfo, &line, 1);
@@ -1089,17 +1057,17 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
jpeg_finish_compress(&cinfo);
jpeg_destroy_compress(&cinfo);
- delete [] line;
+ delete[] line;
fclose(file);
}
/*!
Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
+ for the corresponding gray level image, if necessary convert the data in
+ gray level, and set the bitmap whith the gray level data. That means that
+ the image \e I is a "black and white" rendering of the original image in \e
+ filename, as in a black and white photograph. If necessary, the quantization
+ formula used is \f$0,299 r + 0,587 g + 0,114 b\f$.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -1109,8 +1077,7 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
{
struct jpeg_decompress_struct cinfo;
struct jpeg_error_mgr jerr;
@@ -1120,16 +1087,14 @@ vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
jpeg_create_decompress(&cinfo);
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot read JPEG image: filename empty")) ;
+ if (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);
@@ -1138,36 +1103,33 @@ vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
unsigned int width = cinfo.image_width;
unsigned int height = cinfo.image_height;
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
+ if ((width != I.getWidth()) || (height != I.getHeight()))
+ I.resize(height, width);
jpeg_start_decompress(&cinfo);
unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
- JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
- ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
+ JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE, rowbytes, 1);
if (cinfo.out_color_space == JCS_RGB) {
- vpImage<vpRGBa> Ic(height,width);
- unsigned char* output = (unsigned char*)Ic.bitmap;
- while (cinfo.output_scanline<cinfo.output_height) {
- jpeg_read_scanlines(&cinfo,buffer,1);
+ vpImage<vpRGBa> Ic(height, width);
+ unsigned char *output = (unsigned char *)Ic.bitmap;
+ while (cinfo.output_scanline < cinfo.output_height) {
+ jpeg_read_scanlines(&cinfo, buffer, 1);
for (unsigned int i = 0; i < width; i++) {
- *(output++) = buffer[0][i*3];
- *(output++) = buffer[0][i*3+1];
- *(output++) = buffer[0][i*3+2];
+ *(output++) = buffer[0][i * 3];
+ *(output++) = buffer[0][i * 3 + 1];
+ *(output++) = buffer[0][i * 3 + 2];
*(output++) = vpRGBa::alpha_default;
}
}
- vpImageConvert::convert(Ic,I) ;
+ vpImageConvert::convert(Ic, I);
}
- else if (cinfo.out_color_space == JCS_GRAYSCALE)
- {
- while (cinfo.output_scanline<cinfo.output_height)
- {
+ else if (cinfo.out_color_space == JCS_GRAYSCALE) {
+ while (cinfo.output_scanline < cinfo.output_height) {
unsigned int row = cinfo.output_scanline;
- jpeg_read_scanlines(&cinfo,buffer,1);
+ jpeg_read_scanlines(&cinfo, buffer, 1);
memcpy(I[row], buffer[0], rowbytes);
}
}
@@ -1195,8 +1157,7 @@ vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
\param I : Color image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string &filename)
{
struct jpeg_decompress_struct cinfo;
struct jpeg_error_mgr jerr;
@@ -1206,16 +1167,14 @@ vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string &filename)
jpeg_create_decompress(&cinfo);
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot read JPEG image: filename empty")) ;
+ if (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);
@@ -1225,42 +1184,37 @@ vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string &filename)
unsigned int width = cinfo.image_width;
unsigned int height = cinfo.image_height;
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
+ if ((width != I.getWidth()) || (height != I.getHeight()))
+ I.resize(height, width);
jpeg_start_decompress(&cinfo);
unsigned int rowbytes = cinfo.output_width * (unsigned int)(cinfo.output_components);
- JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)
- ((j_common_ptr) &cinfo, JPOOL_IMAGE, rowbytes, 1);
-
- if (cinfo.out_color_space == JCS_RGB)
- {
- unsigned char* output = (unsigned char*)I.bitmap;
- while (cinfo.output_scanline<cinfo.output_height)
- {
- jpeg_read_scanlines(&cinfo,buffer,1);
+ JSAMPARRAY buffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE, rowbytes, 1);
+
+ if (cinfo.out_color_space == JCS_RGB) {
+ unsigned char *output = (unsigned char *)I.bitmap;
+ while (cinfo.output_scanline < cinfo.output_height) {
+ jpeg_read_scanlines(&cinfo, buffer, 1);
for (unsigned int i = 0; i < width; i++) {
- *(output++) = buffer[0][i*3];
- *(output++) = buffer[0][i*3+1];
- *(output++) = buffer[0][i*3+2];
+ *(output++) = buffer[0][i * 3];
+ *(output++) = buffer[0][i * 3 + 1];
+ *(output++) = buffer[0][i * 3 + 2];
*(output++) = vpRGBa::alpha_default;
}
}
}
- else if (cinfo.out_color_space == JCS_GRAYSCALE)
- {
- vpImage<unsigned char> Ig(height,width);
+ else if (cinfo.out_color_space == JCS_GRAYSCALE) {
+ vpImage<unsigned char> Ig(height, width);
- while (cinfo.output_scanline<cinfo.output_height)
- {
+ while (cinfo.output_scanline < cinfo.output_height) {
unsigned int row = cinfo.output_scanline;
- jpeg_read_scanlines(&cinfo,buffer,1);
+ jpeg_read_scanlines(&cinfo, buffer, 1);
memcpy(Ig[row], buffer[0], rowbytes);
}
- vpImageConvert::convert(Ig,I) ;
+ vpImageConvert::convert(Ig, I);
}
jpeg_finish_decompress(&cinfo);
@@ -1277,15 +1231,14 @@ vpImageIo::readJPEG(vpImage<vpRGBa> &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<unsigned char> &I, const std::string &filename)
+void vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filename)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat Ip;
vpImageConvert::convert(I, Ip);
cv::imwrite(filename.c_str(), Ip);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(I, Ip);
cvSaveImage(filename.c_str(), Ip);
@@ -1301,15 +1254,14 @@ vpImageIo::writeJPEG(const vpImage<unsigned char> &I, const std::string &filenam
\param I : Image to save as a JPEG file.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat Ip;
vpImageConvert::convert(I, Ip);
cv::imwrite(filename.c_str(), Ip);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(I, Ip);
cvSaveImage(filename.c_str(), Ip);
@@ -1320,11 +1272,11 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
/*!
Read the contents of the JPEG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
+ for the corresponding gray level image, if necessary convert the data in
+ gray level, and set the bitmap whith the gray level data. That means that
+ the image \e I is a "black and white" rendering of the original image in \e
+ filename, as in a black and white photograph. If necessary, the quantization
+ formula used is \f$0,299 r + 0,587 g + 0,114 b\f$.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -1334,29 +1286,27 @@ vpImageIo::writeJPEG(const vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readJPEG(vpImage<unsigned char> &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())
+ 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())
+ 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;
+ 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
}
@@ -1379,39 +1329,33 @@ vpImageIo::readJPEG(vpImage<unsigned char> &I, const std::string &filename)
\param I : Color image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readJPEG(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readJPEG(vpImage<vpRGBa> &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())
+ 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())
+ 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;
+ 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
}
#endif
-
-
-
-
-
//--------------------------------------------------------------------------
// PNG
//--------------------------------------------------------------------------
@@ -1425,56 +1369,49 @@ vpImageIo::readJPEG(vpImage<vpRGBa> &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<unsigned char> &I, const std::string &filename)
+void vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string &filename)
{
FILE *file;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create PNG file: filename empty")) ;
+ if (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);
- if (!png_ptr)
- {
- fclose (file);
+ png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ if (!png_ptr) {
+ fclose(file);
vpERROR_TRACE("Error during png_create_write_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
png_infop info_ptr = png_create_info_struct(png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, NULL);
+ if (!info_ptr) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, NULL);
vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ /* initialize the setjmp for returning properly after a libpng error occured
+ */
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
vpERROR_TRACE("Error during init_io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- /* setup libpng for using standard C fwrite() function with our FILE pointer */
- png_init_io (png_ptr, file);
+ /* setup libpng for using standard C fwrite() function with our FILE pointer
+ */
+ png_init_io(png_ptr, file);
unsigned int width = I.getWidth();
unsigned int height = I.getHeight();
@@ -1482,32 +1419,27 @@ vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string &filename
int color_type = PNG_COLOR_TYPE_GRAY;
/* set some useful information from header */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
vpERROR_TRACE("Error during write header\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- png_set_IHDR(png_ptr, info_ptr, width, height,
- bit_depth, color_type, PNG_INTERLACE_NONE,
- PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
+ png_set_IHDR(png_ptr, info_ptr, width, height, bit_depth, color_type, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE,
+ PNG_FILTER_TYPE_BASE);
png_write_info(png_ptr, info_ptr);
- png_bytep* row_ptrs = new png_bytep[height];
+ png_bytep *row_ptrs = new png_bytep[height];
for (unsigned int i = 0; i < height; i++)
row_ptrs[i] = new png_byte[width];
- unsigned char* input = (unsigned char*)I.bitmap;
+ unsigned char *input = (unsigned char *)I.bitmap;
- for (unsigned int i = 0; i < height; i++)
- {
- png_byte* row = row_ptrs[i];
- for(unsigned int j = 0; j < width; j++)
- {
+ for (unsigned int i = 0; i < height; i++) {
+ png_byte *row = row_ptrs[i];
+ for (unsigned int j = 0; j < width; j++) {
row[j] = *(input);
input++;
}
@@ -1517,12 +1449,12 @@ vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string &filename
png_write_end(png_ptr, NULL);
- for(unsigned int j = 0; j < height; j++)
+ for (unsigned int j = 0; j < height; j++)
delete[] row_ptrs[j];
delete[] row_ptrs;
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
fclose(file);
}
@@ -1534,56 +1466,49 @@ vpImageIo::writePNG(const vpImage<unsigned char> &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<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
{
FILE *file;
// Test the filename
- if (filename.empty()) {
- throw (vpImageException(vpImageException::ioError,
- "Cannot create PNG file: filename empty")) ;
+ if (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);
- if (!png_ptr)
- {
- fclose (file);
+ png_structp png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ if (!png_ptr) {
+ fclose(file);
vpERROR_TRACE("Error during png_create_write_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
png_infop info_ptr = png_create_info_struct(png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, NULL);
+ if (!info_ptr) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, NULL);
vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ /* initialize the setjmp for returning properly after a libpng error occured
+ */
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
vpERROR_TRACE("Error during init_io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- /* setup libpng for using standard C fwrite() function with our FILE pointer */
- png_init_io (png_ptr, file);
+ /* setup libpng for using standard C fwrite() function with our FILE pointer
+ */
+ png_init_io(png_ptr, file);
unsigned int width = I.getWidth();
unsigned int height = I.getHeight();
@@ -1591,35 +1516,34 @@ vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
int color_type = PNG_COLOR_TYPE_RGB;
/* set some useful information from header */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
vpERROR_TRACE("Error during write header\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG write error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG write error"));
}
- png_set_IHDR(png_ptr, info_ptr, width, height,
- bit_depth, color_type, PNG_INTERLACE_NONE,
- PNG_COMPRESSION_TYPE_BASE, PNG_FILTER_TYPE_BASE);
+ png_set_IHDR(png_ptr, info_ptr, width, height, bit_depth, color_type, PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_BASE,
+ PNG_FILTER_TYPE_BASE);
png_write_info(png_ptr, info_ptr);
- png_bytep* row_ptrs = new png_bytep[height];
+ png_bytep *row_ptrs = new png_bytep[height];
for (unsigned int i = 0; i < height; i++)
- row_ptrs[i] = new png_byte[3*width];
+ row_ptrs[i] = new png_byte[3 * width];
- unsigned char* input = (unsigned char*)I.bitmap;;
+ unsigned char *input = (unsigned char *)I.bitmap;
+ ;
- for (unsigned int i = 0; i < height; i++)
- {
- png_byte* row = row_ptrs[i];
- for(unsigned int j = 0; j < width; j++)
- {
- row[3*j] = *(input);input++;
- row[3*j+1] = *(input);input++;
- row[3*j+2] = *(input);input++;
+ for (unsigned int i = 0; i < height; i++) {
+ png_byte *row = row_ptrs[i];
+ for (unsigned int j = 0; j < width; j++) {
+ row[3 * j] = *(input);
+ input++;
+ row[3 * j + 1] = *(input);
+ input++;
+ row[3 * j + 2] = *(input);
+ input++;
input++;
}
}
@@ -1628,23 +1552,23 @@ vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
png_write_end(png_ptr, NULL);
- for(unsigned int j = 0; j < height; j++)
+ for (unsigned int j = 0; j < height; j++)
delete[] row_ptrs[j];
delete[] row_ptrs;
- png_destroy_write_struct (&png_ptr, &info_ptr);
+ png_destroy_write_struct(&png_ptr, &info_ptr);
fclose(file);
}
/*!
Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
+ for the corresponding gray level image, if necessary convert the data in
+ gray level, and set the bitmap whith the gray level data. That means that
+ the image \e I is a "black and white" rendering of the original image in \e
+ filename, as in a black and white photograph. If necessary, the quantization
+ formula used is \f$0,299 r + 0,587 g + 0,114 b\f$.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -1654,175 +1578,162 @@ vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readPNG(vpImage<unsigned char> &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")) ;
+ if (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())) ;
+ 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()));
}
/* 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())) ;
+ 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()));
}
/* create a png read struct */
- //printf("version %s\n", PNG_LIBPNG_VER_STRING);
+ // printf("version %s\n", PNG_LIBPNG_VER_STRING);
png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
- if (png_ptr == NULL)
- {
- fprintf (stderr, "error: can't create a png read structure!\n");
- fclose (file);
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
+ if (png_ptr == NULL) {
+ fprintf(stderr, "error: can't create a png read structure!\n");
+ fclose(file);
+ throw(vpImageException(vpImageException::ioError, "error reading png file"));
}
/* create a png info struct */
- png_infop info_ptr = png_create_info_struct (png_ptr);
- if (info_ptr == NULL)
- {
- fprintf (stderr, "error: can't create a png info structure!\n");
- fclose (file);
- png_destroy_read_struct (&png_ptr, NULL, NULL);
- throw (vpImageException(vpImageException::ioError,
- "error reading png file")) ;
- }
-
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ png_infop info_ptr = png_create_info_struct(png_ptr);
+ if (info_ptr == NULL) {
+ fprintf(stderr, "error: can't create a png info structure!\n");
+ fclose(file);
+ png_destroy_read_struct(&png_ptr, NULL, NULL);
+ throw(vpImageException(vpImageException::ioError, "error reading png file"));
+ }
+
+ /* initialize the setjmp for returning properly after a libpng error occured
+ */
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_read_struct(&png_ptr, &info_ptr, NULL);
vpERROR_TRACE("Error during init io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG read error"));
}
- /* setup libpng for using standard C fread() function with our FILE pointer */
- png_init_io (png_ptr, file);
+ /* setup libpng for using standard C fread() function with our FILE pointer
+ */
+ png_init_io(png_ptr, file);
/* tell libpng that we have already read the magic number */
- png_set_sig_bytes (png_ptr, sizeof (magic));
+ png_set_sig_bytes(png_ptr, sizeof(magic));
/* read png info */
- png_read_info (png_ptr, info_ptr);
+ png_read_info(png_ptr, info_ptr);
unsigned int width = png_get_image_width(png_ptr, info_ptr);
unsigned int height = png_get_image_height(png_ptr, info_ptr);
unsigned int bit_depth, channels, color_type;
/* get some useful information from header */
- bit_depth = png_get_bit_depth (png_ptr, info_ptr);
+ bit_depth = png_get_bit_depth(png_ptr, info_ptr);
channels = png_get_channels(png_ptr, info_ptr);
- color_type = png_get_color_type (png_ptr, info_ptr);
+ color_type = png_get_color_type(png_ptr, info_ptr);
/* convert index color images to RGB images */
if (color_type == PNG_COLOR_TYPE_PALETTE)
- png_set_palette_to_rgb (png_ptr);
+ png_set_palette_to_rgb(png_ptr);
/* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
- png_set_expand (png_ptr);
+ png_set_expand(png_ptr);
-// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
-// png_set_tRNS_to_alpha (png_ptr);
+ // if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
+ // png_set_tRNS_to_alpha (png_ptr);
if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
png_set_strip_alpha(png_ptr);
if (bit_depth == 16)
- png_set_strip_16 (png_ptr);
+ png_set_strip_16(png_ptr);
else if (bit_depth < 8)
- png_set_packing (png_ptr);
+ png_set_packing(png_ptr);
/* update info structure to apply transformations */
- png_read_update_info (png_ptr, info_ptr);
+ png_read_update_info(png_ptr, info_ptr);
channels = png_get_channels(png_ptr, info_ptr);
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
+ if ((width != I.getWidth()) || (height != I.getHeight()))
+ I.resize(height, width);
- png_bytep* rowPtrs = new png_bytep[height];
+ png_bytep *rowPtrs = new png_bytep[height];
- unsigned int stride = width * bit_depth * channels / 8;
- unsigned char* data = new unsigned char[stride * height];
+ unsigned int stride = png_get_rowbytes(png_ptr, info_ptr);
+ unsigned char *data = new unsigned char[stride * height];
- for (unsigned int i =0; i < height; i++)
+ for (unsigned int i = 0; i < height; i++)
rowPtrs[i] = (png_bytep)data + (i * stride);
png_read_image(png_ptr, rowPtrs);
- vpImage<vpRGBa> Ic(height,width);
- unsigned char* output;
+ vpImage<vpRGBa> Ic(height, width);
+ unsigned char *output;
- switch (channels)
- {
+ switch (channels) {
case 1:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i];
- }
+ output = (unsigned char *)I.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i];
+ }
break;
+
case 2:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*2];
- }
+ output = (unsigned char *)I.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 2];
+ }
break;
- case 3:
- output = (unsigned char*)Ic.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*3];
- *(output++) = data[i*3+1];
- *(output++) = data[i*3+2];
- *(output++) = vpRGBa::alpha_default;
- }
- vpImageConvert::convert(Ic,I) ;
+ case 3:
+ output = (unsigned char *)Ic.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 3];
+ *(output++) = data[i * 3 + 1];
+ *(output++) = data[i * 3 + 2];
+ *(output++) = vpRGBa::alpha_default;
+ }
+ vpImageConvert::convert(Ic, I);
break;
+
case 4:
- output = (unsigned char*)Ic.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*4];
- *(output++) = data[i*4+1];
- *(output++) = data[i*4+2];
- *(output++) = data[i*4+3];
- }
- vpImageConvert::convert(Ic,I) ;
+ output = (unsigned char *)Ic.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 4];
+ *(output++) = data[i * 4 + 1];
+ *(output++) = data[i * 4 + 2];
+ *(output++) = data[i * 4 + 3];
+ }
+ vpImageConvert::convert(Ic, I);
break;
}
- delete [] (png_bytep)rowPtrs;
- delete [] data;
- png_read_end (png_ptr, NULL);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ delete[](png_bytep) rowPtrs;
+ delete[] data;
+ png_read_end(png_ptr, NULL);
+ png_destroy_read_struct(&png_ptr, &info_ptr, NULL);
fclose(file);
}
@@ -1844,176 +1755,162 @@ vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string &filename)
\param I : Color image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readPNG(vpImage<vpRGBa> &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")) ;
+ if (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())) ;
+ 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()));
}
/* 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())) ;
+ 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()));
}
/* create a png read struct */
png_structp png_ptr = png_create_read_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
- if (!png_ptr)
- {
- fclose (file);
+ if (!png_ptr) {
+ fclose(file);
vpERROR_TRACE("Error during png_create_read_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG read error"));
}
/* create a png info struct */
- png_infop info_ptr = png_create_info_struct (png_ptr);
- if (!info_ptr)
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, NULL, NULL);
+ png_infop info_ptr = png_create_info_struct(png_ptr);
+ if (!info_ptr) {
+ fclose(file);
+ png_destroy_read_struct(&png_ptr, NULL, NULL);
vpERROR_TRACE("Error during png_create_info_struct()\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG read error"));
}
- /* initialize the setjmp for returning properly after a libpng error occured */
- if (setjmp (png_jmpbuf (png_ptr)))
- {
- fclose (file);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ /* initialize the setjmp for returning properly after a libpng error occured
+ */
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ fclose(file);
+ png_destroy_read_struct(&png_ptr, &info_ptr, NULL);
vpERROR_TRACE("Error during init io\n");
- throw (vpImageException(vpImageException::ioError,
- "PNG read error")) ;
+ throw(vpImageException(vpImageException::ioError, "PNG read error"));
}
- /* setup libpng for using standard C fread() function with our FILE pointer */
- png_init_io (png_ptr, file);
+ /* setup libpng for using standard C fread() function with our FILE pointer
+ */
+ png_init_io(png_ptr, file);
/* tell libpng that we have already read the magic number */
- png_set_sig_bytes (png_ptr, sizeof (magic));
+ png_set_sig_bytes(png_ptr, sizeof(magic));
/* read png info */
- png_read_info (png_ptr, info_ptr);
+ png_read_info(png_ptr, info_ptr);
unsigned int width = png_get_image_width(png_ptr, info_ptr);
unsigned int height = png_get_image_height(png_ptr, info_ptr);
unsigned int bit_depth, channels, color_type;
/* get some useful information from header */
- bit_depth = png_get_bit_depth (png_ptr, info_ptr);
+ bit_depth = png_get_bit_depth(png_ptr, info_ptr);
channels = png_get_channels(png_ptr, info_ptr);
- color_type = png_get_color_type (png_ptr, info_ptr);
+ color_type = png_get_color_type(png_ptr, info_ptr);
/* convert index color images to RGB images */
if (color_type == PNG_COLOR_TYPE_PALETTE)
- png_set_palette_to_rgb (png_ptr);
+ png_set_palette_to_rgb(png_ptr);
/* convert 1-2-4 bits grayscale images to 8 bits grayscale. */
if (color_type == PNG_COLOR_TYPE_GRAY && bit_depth < 8)
- png_set_expand (png_ptr);
+ png_set_expand(png_ptr);
-// if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
-// png_set_tRNS_to_alpha (png_ptr);
+ // if (png_get_valid (png_ptr, info_ptr, PNG_INFO_tRNS))
+ // png_set_tRNS_to_alpha (png_ptr);
if (color_type == PNG_COLOR_TYPE_GRAY_ALPHA)
png_set_strip_alpha(png_ptr);
if (bit_depth == 16)
- png_set_strip_16 (png_ptr);
+ png_set_strip_16(png_ptr);
else if (bit_depth < 8)
- png_set_packing (png_ptr);
+ png_set_packing(png_ptr);
/* update info structure to apply transformations */
- png_read_update_info (png_ptr, info_ptr);
+ png_read_update_info(png_ptr, info_ptr);
channels = png_get_channels(png_ptr, info_ptr);
- if ( (width != I.getWidth()) || (height != I.getHeight()) )
- I.resize(height,width);
-
- png_bytep* rowPtrs = new png_bytep[height];
+ if ((width != I.getWidth()) || (height != I.getHeight()))
+ I.resize(height, width);
- unsigned int stride = width * bit_depth * channels / 8;
- unsigned char* data = new unsigned char[stride * height];
+ png_bytep *rowPtrs = new png_bytep[height];
+ unsigned int stride = png_get_rowbytes(png_ptr, info_ptr);
+ unsigned char *data = new unsigned char[stride * height];
- for (unsigned int i =0; i < height; i++)
+ for (unsigned int i = 0; i < height; i++)
rowPtrs[i] = (png_bytep)data + (i * stride);
png_read_image(png_ptr, rowPtrs);
- vpImage<unsigned char> Ig(height,width);
- unsigned char* output;
+ vpImage<unsigned char> Ig(height, width);
+ unsigned char *output;
- switch (channels)
- {
+ switch (channels) {
case 1:
- output = (unsigned char*)Ig.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i];
- }
- vpImageConvert::convert(Ig,I) ;
+ output = (unsigned char *)Ig.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i];
+ }
+ vpImageConvert::convert(Ig, I);
break;
+
case 2:
- output = (unsigned char*)Ig.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*2];
- }
- vpImageConvert::convert(Ig,I) ;
+ output = (unsigned char *)Ig.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 2];
+ }
+ vpImageConvert::convert(Ig, I);
break;
- case 3:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*3];
- *(output++) = data[i*3+1];
- *(output++) = data[i*3+2];
- *(output++) = vpRGBa::alpha_default;
- }
+ case 3:
+ output = (unsigned char *)I.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 3];
+ *(output++) = data[i * 3 + 1];
+ *(output++) = data[i * 3 + 2];
+ *(output++) = vpRGBa::alpha_default;
+ }
break;
+
case 4:
- output = (unsigned char*)I.bitmap;
- for (unsigned int i = 0; i < width*height; i++)
- {
- *(output++) = data[i*4];
- *(output++) = data[i*4+1];
- *(output++) = data[i*4+2];
- *(output++) = data[i*4+3];
- }
+ output = (unsigned char *)I.bitmap;
+ for (unsigned int i = 0; i < width * height; i++) {
+ *(output++) = data[i * 4];
+ *(output++) = data[i * 4 + 1];
+ *(output++) = data[i * 4 + 2];
+ *(output++) = data[i * 4 + 3];
+ }
break;
}
- delete [] (png_bytep)rowPtrs;
- delete [] data;
- png_read_end (png_ptr, NULL);
- png_destroy_read_struct (&png_ptr, &info_ptr, NULL);
+ delete[](png_bytep) rowPtrs;
+ delete[] data;
+ png_read_end(png_ptr, NULL);
+ png_destroy_read_struct(&png_ptr, &info_ptr, NULL);
fclose(file);
}
@@ -2026,15 +1923,14 @@ vpImageIo::readPNG(vpImage<vpRGBa> &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<unsigned char> &I, const std::string &filename)
+void vpImageIo::writePNG(const vpImage<unsigned char> &I, const std::string &filename)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat Ip;
vpImageConvert::convert(I, Ip);
cv::imwrite(filename.c_str(), Ip);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(I, Ip);
cvSaveImage(filename.c_str(), Ip);
@@ -2050,15 +1946,14 @@ vpImageIo::writePNG(const vpImage<unsigned char> &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<vpRGBa> &I, const std::string &filename)
+void vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat Ip;
vpImageConvert::convert(I, Ip);
cv::imwrite(filename.c_str(), Ip);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(I, Ip);
cvSaveImage(filename.c_str(), Ip);
@@ -2069,11 +1964,11 @@ vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
/*!
Read the contents of the PNG file, allocate memory
- for the corresponding gray level image, if necessary convert the data in gray level, and
- set the bitmap whith the gray level data. That means that the image \e I is a
- "black and white" rendering of the original image in \e filename, as in a
- black and white photograph. If necessary, the quantization formula used is \f$0,299 r +
- 0,587 g + 0,114 b\f$.
+ for the corresponding gray level image, if necessary convert the data in
+ gray level, and set the bitmap whith the gray level data. That means that
+ the image \e I is a "black and white" rendering of the original image in \e
+ filename, as in a black and white photograph. If necessary, the quantization
+ formula used is \f$0,299 r + 0,587 g + 0,114 b\f$.
If the image has been already initialized, memory allocation is done
only if the new image size is different, else we re-use the same
@@ -2083,29 +1978,27 @@ vpImageIo::writePNG(const vpImage<vpRGBa> &I, const std::string &filename)
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string &filename)
+void vpImageIo::readPNG(vpImage<unsigned char> &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())
+ 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())
+ 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;
+ 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
}
@@ -2128,29 +2021,27 @@ vpImageIo::readPNG(vpImage<unsigned char> &I, const std::string &filename)
\param I : Color image to set with the \e filename content.
\param filename : Name of the file containing the image.
*/
-void
-vpImageIo::readPNG(vpImage<vpRGBa> &I, const std::string &filename)
+void vpImageIo::readPNG(vpImage<vpRGBa> &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())
+ 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())
+ 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;
+ 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 80698f6..5536e1e 100644
--- a/modules/io/src/parallel-port/vpParallelPort.cpp
+++ b/modules/io/src/parallel-port/vpParallelPort.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,26 +36,23 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_PARPORT
-# include <sys/types.h>
-# include <sys/stat.h>
-# include <fcntl.h>
-# include <sys/ioctl.h>
-# include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
-# include <visp3/io/vpParallelPort.h>
+#include <visp3/io/vpParallelPort.h>
/*!
\file vpParallelPort.cpp
\brief Parallel port management under unix.
*/
-
-
static unsigned char vpParallelPortData;
/*!
@@ -86,10 +84,7 @@ vpParallelPort::vpParallelPort() : fd(0), device()
\exception vpParallelPortException::closing If the device used to access to
the parallel port can't be closed.
*/
-vpParallelPort::~vpParallelPort()
-{
- this->close();
-}
+vpParallelPort::~vpParallelPort() { this->close(); }
/*!
Open the parallel port with write access and initialise it.
@@ -107,17 +102,16 @@ 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;
ioctl(fd, PPCLAIM);
i = PARPORT_MODE_COMPAT;
- ioctl(fd, PPSETMODE, & i);
+ ioctl(fd, PPSETMODE, &i);
i = IEEE1284_MODE_COMPAT;
- ioctl(fd, PPNEGOT, & i);
+ ioctl(fd, PPNEGOT, &i);
}
/*!
@@ -139,7 +133,7 @@ void vpParallelPort::open()
*/
void vpParallelPort::sendData(unsigned char &data)
{
- ioctl(fd, PPWDATA, & data);
+ ioctl(fd, PPWDATA, &data);
// Memorise the last sended data to the static variable
vpParallelPortData = data;
@@ -150,10 +144,7 @@ void vpParallelPort::sendData(unsigned char &data)
Get the last data sent to the parallel port.
*/
-unsigned char vpParallelPort::getData()
-{
- return vpParallelPortData;
-}
+unsigned char vpParallelPort::getData() { return vpParallelPortData; }
/*!
Close the parallel port.
@@ -169,17 +160,14 @@ void vpParallelPort::close()
int err;
err = ::close(fd);
- if (err !=0) {
+ 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"));
}
-
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpParallelPort.cpp.o) has no symbols
-void dummy_vpParallelPort() {};
+// Work arround to avoid warning: libvisp_core.a(vpParallelPort.cpp.o) has no
+// symbols
+void dummy_vpParallelPort(){};
#endif
-
diff --git a/modules/io/src/tools/vpKeyboard.cpp b/modules/io/src/tools/vpKeyboard.cpp
index 5ae89da..f11ba6d 100644
--- a/modules/io/src/tools/vpKeyboard.cpp
+++ b/modules/io/src/tools/vpKeyboard.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,40 +36,30 @@
*
*****************************************************************************/
-
-
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-# include <stdio.h>
-# include <visp3/io/vpKeyboard.h>
+#include <stdio.h>
+#include <visp3/io/vpKeyboard.h>
/*!
\file vpKeyboard.cpp
\brief Keybord management under unix.
*/
-
/*!
Activates the raw mode to read keys in an non blocking way.
*/
-vpKeyboard::vpKeyboard() : initial_settings(), new_settings()
-{
- init();
-}
+vpKeyboard::vpKeyboard() : initial_settings(), new_settings() { init(); }
/*!
Stops the raw mode.
*/
-vpKeyboard::~vpKeyboard()
-{
- end();
-}
+vpKeyboard::~vpKeyboard() { end(); }
/*!
Get the hit key. kbhit() indicates if a key was hitten.
*/
-int
-vpKeyboard::getchar()
+int vpKeyboard::getchar()
{
int c;
c = ::getchar();
@@ -80,10 +71,9 @@ vpKeyboard::getchar()
\return 1 : if a key was hit.
*/
-int
-vpKeyboard::kbhit()
+int vpKeyboard::kbhit()
{
- struct timeval tv = { 0, 0 };
+ struct timeval tv = {0, 0};
fd_set readfds;
FD_ZERO(&readfds);
@@ -96,21 +86,13 @@ vpKeyboard::kbhit()
Activates the raw mode to read keys in an non blocking way.
*/
-void
-vpKeyboard::init()
-{
- setRawMode(true);
-}
+void vpKeyboard::init() { setRawMode(true); }
/*!
Stops the raw mode.
*/
-void
-vpKeyboard::end()
-{
- setRawMode(false);
-}
+void vpKeyboard::end() { setRawMode(false); }
/*!
Turns on/off the 'raw' mode.
@@ -119,29 +101,27 @@ vpKeyboard::end()
If raw mode is active, there is no need to press return to get a key.
*/
-void
-vpKeyboard::setRawMode(bool active)
+void vpKeyboard::setRawMode(bool active)
{
if (active) {
tcgetattr(STDIN_FILENO, &initial_settings);
- new_settings = initial_settings;
+ // new_settings = initial_settings;
// cfmakeraw(&new_settings);
new_settings = initial_settings;
new_settings.c_lflag &= (unsigned int)~ICANON;
new_settings.c_lflag &= (unsigned int)~ECHO;
new_settings.c_lflag &= (unsigned int)~ISIG;
- //new_settings.c_oflag &= (unsigned int)~NL0;
- //new_settings.c_oflag &= (unsigned int)~CR0;
+ // new_settings.c_oflag &= (unsigned int)~NL0;
+ // new_settings.c_oflag &= (unsigned int)~CR0;
new_settings.c_oflag &= (unsigned int)~TAB0;
- //new_settings.c_oflag &= (unsigned int)~BS0;
+ // new_settings.c_oflag &= (unsigned int)~BS0;
new_settings.c_cc[VMIN] = 1;
new_settings.c_cc[VTIME] = 0;
tcsetattr(STDIN_FILENO, TCSANOW, &new_settings);
- }
- else {
+ } else {
tcsetattr(STDIN_FILENO, TCSANOW, &initial_settings);
}
}
diff --git a/modules/io/src/tools/vpParseArgv.cpp b/modules/io/src/tools/vpParseArgv.cpp
index ccfa5e1..025009c 100644
--- a/modules/io/src/tools/vpParseArgv.cpp
+++ b/modules/io/src/tools/vpParseArgv.cpp
@@ -24,15 +24,13 @@
\brief Command line argument parsing.
*/
-
-
-#include <visp3/io/vpParseArgv.h>
+#include <ctype.h>
+#include <math.h>
+#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
-#include <stdint.h>
#include <string.h>
-#include <math.h>
-#include <ctype.h>
+#include <visp3/io/vpParseArgv.h>
/*
* Default table of argument descriptors. These are normally available
@@ -40,26 +38,22 @@
*/
vpParseArgv::vpArgvInfo vpParseArgv::defaultTable[2] = {
- {"-help", ARGV_HELP, (char *) NULL, (char *) NULL,
- "Print summary of command-line options and abort.\n"},
- {NULL, ARGV_END, (char *) NULL, (char *) NULL,
- (char *) NULL}
-};
+ {"-help", ARGV_HELP, (char *)NULL, (char *)NULL, "Print summary of command-line options and abort.\n"},
+ {NULL, ARGV_END, (char *)NULL, (char *)NULL, (char *)NULL}};
int (*handlerProc1)(const char *dst, const char *key, const char *argument);
int (*handlerProc2)(const char *dst, const char *key, int valargc, const char **argument);
-
/*!
- Process an argv array according to a table of expectedvcommand-line options.
-
+ Process an argv array according to a table of expectedvcommand-line options.
+
The return value is a boolean value with true indicating an
error. If an error occurs then an error message is printed on
stderr. Under normal conditions, both *argcPtr and *argv are
modified to return the arguments that couldn't be processed here
(they didn't match the option table, or followed an
vpParseArgv::ARGV_REST argument).
-
+
\param argcPtr: Pointer to the count of command line arguments.
\param argv: Array of command line argument strings.
@@ -72,403 +66,405 @@ int (*handlerProc2)(const char *dst, const char *key, int valargc, const char **
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
- * table of argument descriptions. */
- vpArgvInfo *matchPtr; /* Descriptor that matches current argument. */
- const char *curArg; /* Current argument */
- char c; /* Second character of current arg (used for
- * quick check for matching; use 2nd char.
- * because first char. will almost always
- * be '-'). */
- int srcIndex; /* Location from which to read next argument
- * from argv. */
- int dstIndex; /* Index into argv to which next unused
- * argument should be copied (never greater
- * than srcIndex). */
- int argc; /* # arguments in argv still to process. */
- size_t length; /* Number of characters in current argument. */
- unsigned long long nargs; /* Number of following arguments to get. */
+ vpArgvInfo *infoPtr; /* Pointer to the current entry in the
+ * table of argument descriptions. */
+ vpArgvInfo *matchPtr; /* Descriptor that matches current argument. */
+ const char *curArg; /* Current argument */
+ char c; /* Second character of current arg (used for
+ * quick check for matching; use 2nd char.
+ * because first char. will almost always
+ * be '-'). */
+ int srcIndex; /* Location from which to read next argument
+ * from argv. */
+ int dstIndex; /* Index into argv to which next unused
+ * argument should be copied (never greater
+ * than srcIndex). */
+ int argc; /* # arguments in argv still to process. */
+ size_t length; /* Number of characters in current argument. */
+ unsigned long long nargs; /* Number of following arguments to get. */
/* Macro to optionally print errors */
-#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
-
- if (flags & ARGV_DONT_SKIP_FIRST_ARG) {
- srcIndex = dstIndex = 0;
- argc = *argcPtr;
- } else {
- srcIndex = dstIndex = 1;
- argc = *argcPtr-1;
- }
-
- while (argc > 0) {
- curArg = argv[srcIndex];
- srcIndex++;
- argc--;
- c = curArg[1];
- length = strlen(curArg);
-
- /*
- * Loop throught the argument descriptors searching for one with
- * the matching key string. If found, leave a pointer to it in
- * matchPtr.
- */
+#define FPRINTF \
+ if (!(flags & ARGV_NO_PRINT)) \
+ (void)fprintf
+
+ if (flags & ARGV_DONT_SKIP_FIRST_ARG) {
+ srcIndex = dstIndex = 0;
+ argc = *argcPtr;
+ } else {
+ srcIndex = dstIndex = 1;
+ argc = *argcPtr - 1;
+ }
- matchPtr = NULL;
- for (unsigned int i = 0; i < 2; i++) {
- if (i == 0) {
- infoPtr = argTable;
- } else {
- infoPtr = defaultTable;
- }
- for (; infoPtr->type != ARGV_END; infoPtr++) {
- if (infoPtr->key == NULL) {
- continue;
- }
- if ((infoPtr->key[1] != c)
- || (strncmp(infoPtr->key, curArg, length) != 0)) {
- continue;
- }
- if (infoPtr->key[length] == 0) {
- matchPtr = infoPtr;
- goto gotMatch;
- }
- if (flags & ARGV_NO_ABBREV) {
- continue;
- }
- if (matchPtr != NULL) {
- FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg);
- return true;
- }
- matchPtr = infoPtr;
- }
+ while (argc > 0) {
+ curArg = argv[srcIndex];
+ srcIndex++;
+ argc--;
+ c = curArg[1];
+ length = strlen(curArg);
+
+ /*
+ * Loop throught the argument descriptors searching for one with
+ * the matching key string. If found, leave a pointer to it in
+ * matchPtr.
+ */
+
+ matchPtr = NULL;
+ for (unsigned int i = 0; i < 2; i++) {
+ if (i == 0) {
+ infoPtr = argTable;
+ } else {
+ infoPtr = defaultTable;
}
- if (matchPtr == NULL) {
-
- /*
- * Unrecognized argument. Just copy it down, unless the caller
- * prefers an error to be registered.
- */
-
- if (flags & ARGV_NO_LEFTOVERS) {
- FPRINTF(stderr, "unrecognized argument \"%s\"\n", curArg);
- }
- argv[dstIndex] = curArg;
- dstIndex++;
- continue;
+ for (; infoPtr->type != ARGV_END; infoPtr++) {
+ if (infoPtr->key == NULL) {
+ continue;
+ }
+ if ((infoPtr->key[1] != c) || (strncmp(infoPtr->key, curArg, length) != 0)) {
+ continue;
+ }
+ if (infoPtr->key[length] == 0) {
+ matchPtr = infoPtr;
+ goto gotMatch;
+ }
+ if (flags & ARGV_NO_ABBREV) {
+ continue;
+ }
+ if (matchPtr != NULL) {
+ FPRINTF(stderr, "ambiguous option \"%s\"\n", curArg);
+ return true;
+ }
+ matchPtr = infoPtr;
}
+ }
+ if (matchPtr == NULL) {
/*
- * Take the appropriate action based on the option type
+ * Unrecognized argument. Just copy it down, unless the caller
+ * prefers an error to be registered.
*/
- gotMatch:
- infoPtr = matchPtr;
- switch (infoPtr->type) {
- case ARGV_CONSTANT:
- *((int *) infoPtr->dst) = 1;
- break;
- case ARGV_INT:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr=NULL;
-
- *(((int *) infoPtr->dst)+i) =
- (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]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_LONG:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr=NULL;
-
- *(((long *) infoPtr->dst)+i) =
- strtol(argv[srcIndex], &endPtr, 0);
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected long argument for \"%s\" but got \"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_STRING:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- *(((const char **)infoPtr->dst)+i) = argv[srcIndex];
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_REST:
- *((int *) infoPtr->dst) = dstIndex;
- goto argsDone;
- case ARGV_FLOAT:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr;
-
- *(((float *) infoPtr->dst)+i) =
- (float)strtod(argv[srcIndex], &endPtr); // Here we use strtod
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected floating-point argument for \"%s\" but got\"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
- case ARGV_DOUBLE:
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long i=0; i<nargs; i++) {
- if (argc == 0) {
- goto missingArg;
- } else {
- char *endPtr;
-
- *(((double *) infoPtr->dst)+i) =
- strtod(argv[srcIndex], &endPtr);
- if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
- FPRINTF(stderr,
- "expected double-point argument for \"%s\" but got\"%s\"\n",
- infoPtr->key, argv[srcIndex]);
- return true;
- }
- srcIndex++;
- argc--;
- }
- }
- break;
-
- case ARGV_FUNC: {
- handlerProc1 = (int (*)(const char *dst, const char *key, const char *argument))infoPtr->src;
-
- if ((*handlerProc1)(infoPtr->dst, infoPtr->key, argv[srcIndex]))
- {
- srcIndex += 1;
- argc -= 1;
- }
- break;
+
+ if (flags & ARGV_NO_LEFTOVERS) {
+ FPRINTF(stderr, "unrecognized argument \"%s\"\n", curArg);
}
- case ARGV_GENFUNC: {
- handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, const char **argument))infoPtr->src;
+ argv[dstIndex] = curArg;
+ dstIndex++;
+ continue;
+ }
- argc = (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv+srcIndex);
- if (argc < 0) {
+ /*
+ * Take the appropriate action based on the option type
+ */
+ gotMatch:
+ infoPtr = matchPtr;
+ switch (infoPtr->type) {
+ case ARGV_CONSTANT:
+ case ARGV_CONSTANT_INT:
+ *((int *)infoPtr->dst) = 1;
+ break;
+ case ARGV_CONSTANT_BOOL:
+ *((bool *)infoPtr->dst) = true;
+ break;
+ case ARGV_INT:
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long i = 0; i < nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr = NULL;
+
+ *(((int *)infoPtr->dst) + i) = (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]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_LONG:
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long i = 0; i < nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr = NULL;
+
+ *(((long *)infoPtr->dst) + i) = strtol(argv[srcIndex], &endPtr, 0);
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr, "expected long argument for \"%s\" but got \"%s\"\n", infoPtr->key, argv[srcIndex]);
return true;
- }
- break;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_STRING:
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long i = 0; i < nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ *(((const char **)infoPtr->dst) + i) = argv[srcIndex];
+ srcIndex++;
+ argc--;
+ }
}
+ break;
+ case ARGV_REST:
+ *((int *)infoPtr->dst) = dstIndex;
+ goto argsDone;
+ case ARGV_FLOAT:
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long i = 0; i < nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr;
+
+ *(((float *)infoPtr->dst) + i) = (float)strtod(argv[srcIndex], &endPtr); // Here we use strtod
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr, "expected floating-point argument for \"%s\" but got\"%s\"\n", infoPtr->key,
+ argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
+ case ARGV_DOUBLE:
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long i = 0; i < nargs; i++) {
+ if (argc == 0) {
+ goto missingArg;
+ } else {
+ char *endPtr;
+
+ *(((double *)infoPtr->dst) + i) = strtod(argv[srcIndex], &endPtr);
+ if ((endPtr == argv[srcIndex]) || (*endPtr != 0)) {
+ FPRINTF(stderr, "expected double-point argument for \"%s\" but got\"%s\"\n", infoPtr->key, argv[srcIndex]);
+ return true;
+ }
+ srcIndex++;
+ argc--;
+ }
+ }
+ break;
- case ARGV_HELP:
- printUsage (argTable, flags);
- return true;
- case ARGV_END:
- default:
- FPRINTF(stderr, "bad argument type %d in vpArgvInfo",
- infoPtr->type);
- return true;
+ case ARGV_FUNC: {
+ handlerProc1 = (int (*)(const char *dst, const char *key, const char *argument))infoPtr->src;
+
+ if ((*handlerProc1)(infoPtr->dst, infoPtr->key, argv[srcIndex])) {
+ srcIndex += 1;
+ argc -= 1;
}
- }
+ break;
+ }
+ case ARGV_GENFUNC: {
+ handlerProc2 = (int (*)(const char *dst, const char *key, int valargc, const char **argument))infoPtr->src;
- /*
- * If we broke out of the loop because of an OPT_REST argument,
- * copy the remaining arguments down.
- */
+ argc = (*handlerProc2)(infoPtr->dst, infoPtr->key, argc, argv + srcIndex);
+ if (argc < 0) {
+ return true;
+ }
+ break;
+ }
+
+ case ARGV_HELP:
+ printUsage(argTable, flags);
+ return true;
+ case ARGV_END:
+ default:
+ FPRINTF(stderr, "bad argument type %d in vpArgvInfo", infoPtr->type);
+ return true;
+ }
+ }
+
+/*
+ * If we broke out of the loop because of an OPT_REST argument,
+ * copy the remaining arguments down.
+ */
+
+argsDone:
+ while (argc) {
+ argv[dstIndex] = argv[srcIndex];
+ srcIndex++;
+ dstIndex++;
+ argc--;
+ }
+ argv[dstIndex] = (char *)NULL;
+ *argcPtr = dstIndex;
+ return false;
+
+missingArg:
+ FPRINTF(stderr, "\"%s\" option requires an additional argument\n", curArg);
+ return true;
- argsDone:
- while (argc) {
- argv[dstIndex] = argv[srcIndex];
- srcIndex++;
- dstIndex++;
- argc--;
- }
- argv[dstIndex] = (char *) NULL;
- *argcPtr = dstIndex;
- return false;
-
- missingArg:
- FPRINTF(stderr, "\"%s\" option requires an additional argument\n", curArg);
- return true;
-
#undef FPRINTF
}
/*!
Generate a help string describing command-line options.
-
+
Prints on stderr (unless vpParseArgv::ARGV_NO_PRINT is specified in
flags) a help string describing all the options in argTable, plus
all those in the default table unless vpParseArgv::ARGV_NO_DEFAULTS
is specified in flags.
-
+
\param argTable: Array of command-specific argument.descriptions.
\param flags: If the vpParseArgv::ARGV_NO_DEFAULTS bit is set in
this word, then don't generate information for default options.
*/
-void
-vpParseArgv::printUsage(vpArgvInfo * argTable, int flags)
+void vpParseArgv::printUsage(vpArgvInfo *argTable, int flags)
{
- vpArgvInfo *infoPtr;
- int width;
- int numSpaces;
+ vpArgvInfo *infoPtr;
+ int width;
+ int numSpaces;
#define NUM_SPACES 20
- static char spaces[] = " ";
-/* char tmp[30]; */
- unsigned long long nargs;
+ static char spaces[] = " ";
+ /* char tmp[30]; */
+ unsigned long long nargs;
/* Macro to optionally print errors */
-#define FPRINTF if (!(flags&ARGV_NO_PRINT)) (void) fprintf
-
- /*
- * First, compute the width of the widest option key, so that we
- * can make everything line up.
- */
-
- width = 4;
- for (unsigned int i = 0; i < 2; i++) {
- for (infoPtr = i ? defaultTable : argTable;
- infoPtr->type != ARGV_END; infoPtr++) {
- int length;
- if (infoPtr->key == NULL) {
- continue;
- }
- length = (int)strlen(infoPtr->key);
- if (length > width) {
- width = length;
- }
+#define FPRINTF \
+ if (!(flags & ARGV_NO_PRINT)) \
+ (void)fprintf
+
+ /*
+ * First, compute the width of the widest option key, so that we
+ * can make everything line up.
+ */
+
+ width = 4;
+ for (unsigned int i = 0; i < 2; i++) {
+ for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; infoPtr++) {
+ int length;
+ if (infoPtr->key == NULL) {
+ continue;
}
- }
-
- FPRINTF(stderr, "Command-specific options:");
- for (unsigned int i = 0; ; i++) {
- for (infoPtr = i ? defaultTable : argTable;
- infoPtr->type != ARGV_END; infoPtr++) {
- if ((infoPtr->type == ARGV_HELP) && (infoPtr->key == NULL)) {
- FPRINTF(stderr, "\n%s", infoPtr->help);
- continue;
- }
- FPRINTF(stderr, "\n %s:", infoPtr->key);
- numSpaces = width + 1 - (int)strlen(infoPtr->key);
- while (numSpaces > 0) {
- if (numSpaces >= NUM_SPACES) {
- FPRINTF(stderr, "%s",spaces);
- } else {
- FPRINTF(stderr, "%s",spaces+NUM_SPACES-numSpaces);
- }
- numSpaces -= NUM_SPACES;
- }
- FPRINTF(stderr, "%s",infoPtr->help);
- switch (infoPtr->type) {
- case ARGV_INT: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %d", *(((int *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_LONG: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %ld", *(((long *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_FLOAT: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %f", *(((float *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_DOUBLE: {
- FPRINTF(stderr, "\n\t\tDefault value:");
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- for (unsigned long j=0; j<nargs; j++) {
- FPRINTF(stderr, " %g", *(((double *) infoPtr->dst)+j));
- }
- break;
- }
- case ARGV_STRING: {
- const char *string;
-
- nargs = (uintptr_t) infoPtr->src;
- if (nargs<1) nargs=1;
- string = *((const char **) infoPtr->dst);
- if ((nargs==1) && (string == NULL)) break;
- for (unsigned long j=0; j<nargs; j++) {
- string = *(((const char **) infoPtr->dst)+j);
- if (string != NULL) {
- FPRINTF(stderr, " \"%s\"", string);
- }
- }
+ length = (int)strlen(infoPtr->key);
+ if (length > width) {
+ width = length;
+ }
+ }
+ }
- break;
- }
- case ARGV_END:
- case ARGV_HELP:
- case ARGV_GENFUNC:
- case ARGV_FUNC:
- case ARGV_REST:
- case ARGV_CONSTANT:
- default: {
- break;
- }
- }
+ FPRINTF(stderr, "Command-specific options:");
+ for (unsigned int i = 0;; i++) {
+ for (infoPtr = i ? defaultTable : argTable; infoPtr->type != ARGV_END; infoPtr++) {
+ if ((infoPtr->type == ARGV_HELP) && (infoPtr->key == NULL)) {
+ FPRINTF(stderr, "\n%s", infoPtr->help);
+ continue;
}
+ FPRINTF(stderr, "\n %s:", infoPtr->key);
+ numSpaces = width + 1 - (int)strlen(infoPtr->key);
+ while (numSpaces > 0) {
+ if (numSpaces >= NUM_SPACES) {
+ FPRINTF(stderr, "%s", spaces);
+ } else {
+ FPRINTF(stderr, "%s", spaces + NUM_SPACES - numSpaces);
+ }
+ numSpaces -= NUM_SPACES;
+ }
+ FPRINTF(stderr, "%s", infoPtr->help);
+ switch (infoPtr->type) {
+ case ARGV_INT: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long j = 0; j < nargs; j++) {
+ FPRINTF(stderr, " %d", *(((int *)infoPtr->dst) + j));
+ }
+ break;
+ }
+ case ARGV_LONG: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long j = 0; j < nargs; j++) {
+ FPRINTF(stderr, " %ld", *(((long *)infoPtr->dst) + j));
+ }
+ break;
+ }
+ case ARGV_FLOAT: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long j = 0; j < nargs; j++) {
+ FPRINTF(stderr, " %f", *(((float *)infoPtr->dst) + j));
+ }
+ break;
+ }
+ case ARGV_DOUBLE: {
+ FPRINTF(stderr, "\n\t\tDefault value:");
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ for (unsigned long j = 0; j < nargs; j++) {
+ FPRINTF(stderr, " %g", *(((double *)infoPtr->dst) + j));
+ }
+ break;
+ }
+ case ARGV_STRING: {
+ const char *string;
+
+ nargs = (uintptr_t)infoPtr->src;
+ if (nargs < 1)
+ nargs = 1;
+ string = *((const char **)infoPtr->dst);
+ if ((nargs == 1) && (string == NULL))
+ break;
+ for (unsigned long j = 0; j < nargs; j++) {
+ string = *(((const char **)infoPtr->dst) + j);
+ if (string != NULL) {
+ FPRINTF(stderr, " \"%s\"", string);
+ }
+ }
- if ((flags & ARGV_NO_DEFAULTS) || (i > 0)) {
- break;
+ break;
}
- FPRINTF(stderr, "\nGeneric options for all commands:");
- }
+ case ARGV_END:
+ case ARGV_HELP:
+ case ARGV_GENFUNC:
+ case ARGV_FUNC:
+ case ARGV_REST:
+ case ARGV_CONSTANT:
+ case ARGV_CONSTANT_INT:
+ case ARGV_CONSTANT_BOOL:
+ default: {
+ break;
+ }
+ }
+ }
+
+ if ((flags & ARGV_NO_DEFAULTS) || (i > 0)) {
+ break;
+ }
+ FPRINTF(stderr, "\nGeneric options for all commands:");
+ }
- FPRINTF(stderr, "\n");
+ FPRINTF(stderr, "\n");
#undef FPRINTF
}
@@ -489,24 +485,22 @@ vpParseArgv::printUsage(vpArgvInfo * argTable, int flags)
returned, and *param points to the parameter if given, or is NULL if no
param.
- \return If standalone parameter (with no option) is found, 1 is returned, and
- *param points to the standalone parameter
+ \return If standalone parameter (with no option) is found, 1 is returned,
+ and *param points to the standalone parameter
- \return If option is found, but it is not in the list of valid options, -1 is
- returned, and *param points to the invalid argument.
+ \return If option is found, but it is not in the list of valid options, -1
+ is returned, and *param points to the invalid argument.
\return When end of argument list is reached, 0 is returned, and *param
is NULL.
*/
-int
-vpParseArgv::parse(int argc, const char** argv, const char* validOpts,
- const char** param)
+int vpParseArgv::parse(int argc, const char **argv, const char *validOpts, const char **param)
{
static int iArg = 1;
int chOpt;
- const char* psz = NULL;
- const char* pszParam = NULL;
+ const char *psz = NULL;
+ const char *pszParam = NULL;
if (iArg < argc) {
psz = &(argv[iArg][0]);
@@ -514,58 +508,51 @@ vpParseArgv::parse(int argc, const char** argv, const char* validOpts,
// we have an option specifier
chOpt = argv[iArg][1];
if (isalnum(chOpt) || ispunct(chOpt)) {
- // we have an option character
- psz = strchr(validOpts, chOpt);
- if (psz != NULL) {
- // option is valid, we want to return chOpt
- if (psz[1] == ':') {
- // option can have a parameter
- psz = &(argv[iArg][2]);
- if (*psz == '\0') {
- // must look at next argv for param
- if (iArg+1 < argc) {
- psz = &(argv[iArg+1][0]);
- // next argv is the param
- iArg++;
- pszParam = psz;
- }
- else {
- // reached end of args looking for param
- // option specified without parameter
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
- }
-
- }
- else {
- // param is attached to option
- pszParam = psz;
- }
- }
- else {
- // option is alone, has no parameter
- }
- }
- else {
- // option specified is not in list of valid options
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
- }
- }
- else {
- // though option specifier was given, option character
- // is not alpha or was was not specified
- chOpt = -1;
- pszParam = &(argv[iArg][0]);
+ // we have an option character
+ psz = strchr(validOpts, chOpt);
+ if (psz != NULL) {
+ // option is valid, we want to return chOpt
+ if (psz[1] == ':') {
+ // option can have a parameter
+ psz = &(argv[iArg][2]);
+ if (*psz == '\0') {
+ // must look at next argv for param
+ if (iArg + 1 < argc) {
+ psz = &(argv[iArg + 1][0]);
+ // next argv is the param
+ iArg++;
+ pszParam = psz;
+ } else {
+ // reached end of args looking for param
+ // option specified without parameter
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
+ }
+
+ } else {
+ // param is attached to option
+ pszParam = psz;
+ }
+ } else {
+ // option is alone, has no parameter
+ }
+ } else {
+ // option specified is not in list of valid options
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
+ }
+ } else {
+ // though option specifier was given, option character
+ // is not alpha or was was not specified
+ chOpt = -1;
+ pszParam = &(argv[iArg][0]);
}
- }
- else {
+ } else {
// standalone arg given with no option specifier
chOpt = 1;
pszParam = &(argv[iArg][0]);
}
- }
- else {
+ } else {
// end of argument list
chOpt = 0;
}
diff --git a/modules/io/src/video/vpDiskGrabber.cpp b/modules/io/src/video/vpDiskGrabber.cpp
index 7e0e308..ca2d7bb 100644
--- a/modules/io/src/video/vpDiskGrabber.cpp
+++ b/modules/io/src/video/vpDiskGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,42 +36,28 @@
*
*****************************************************************************/
-
#include <visp3/io/vpDiskGrabber.h>
-
/*!
Elementary constructor.
*/
vpDiskGrabber::vpDiskGrabber()
- : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
+ : 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")
{
- setDirectory("/tmp");
- setBaseName("I");
- setExtension("pgm");
-
init = false;
}
-
-vpDiskGrabber::vpDiskGrabber(const char *generic_name)
- : image_number(0), image_step(1), number_of_zero(0), useGenericName(false)
+/*!
+ 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)
{
- setDirectory("/tmp");
- setBaseName("I");
- setExtension("pgm");
-
init = false;
- if (strlen( generic_name ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the generic name"));
- }
-
- strcpy(this->genericName, generic_name);
- useGenericName = true;
}
-
/*!
Constructor.
@@ -82,26 +69,22 @@ vpDiskGrabber::vpDiskGrabber(const char *generic_name)
\param ext : Extension of the image file.
*/
-vpDiskGrabber::vpDiskGrabber(const char *dir, const char *basename,
- long number,
- int step, unsigned int noz,
- const char *ext)
- : image_number(number), image_step(step), number_of_zero(noz), useGenericName(false)
+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")
{
- setDirectory(dir);
- setBaseName(basename);
- setExtension(ext);
-
init = false;
}
-void
-vpDiskGrabber::open(vpImage<unsigned char> &I)
+/*!
+ Read the first image of the sequence.
+ The image number is not incremented.
+*/
+void vpDiskGrabber::open(vpImage<unsigned char> &I)
{
long first_number = getImageNumber();
- vpDEBUG_TRACE(2, "first %ld", first_number);
-
acquire(I);
setImageNumber(first_number);
@@ -113,17 +96,14 @@ vpDiskGrabber::open(vpImage<unsigned char> &I)
}
/*!
- Read the fist image of the sequence.
+ Read the first image of the sequence.
The image number is not incremented.
-
*/
-void
-vpDiskGrabber::open(vpImage<vpRGBa> &I)
+void vpDiskGrabber::open(vpImage<vpRGBa> &I)
{
// First we save the image number, so that it can be reaffected after the
// acquisition. That means that the first image is readed twice
long first_number = getImageNumber();
- vpDEBUG_TRACE(2, "first %ld", first_number);
acquire(I);
@@ -136,17 +116,14 @@ vpDiskGrabber::open(vpImage<vpRGBa> &I)
}
/*!
- Read the fist image of the sequence.
+ Read the first image of the sequence.
The image number is not incremented.
-
*/
-void
-vpDiskGrabber::open(vpImage<float> &I)
+void vpDiskGrabber::open(vpImage<float> &I)
{
// First we save the image number, so that it can be reaffected after the
// acquisition. That means that the first image is readed twice
long first_number = getImageNumber();
- vpDEBUG_TRACE(2, "first %ld", first_number);
acquire(I);
@@ -159,254 +136,223 @@ vpDiskGrabber::open(vpImage<float> &I)
}
/*!
- Acquire an image: read a pgm image from the disk.
+ Acquire an image reading the next image from the disk.
After this call, the image number is incremented considering the step.
- \param I the read image
+ \param I : The image read from a file.
*/
-void
-vpDiskGrabber::acquire(vpImage<unsigned char> &I)
+void vpDiskGrabber::acquire(vpImage<unsigned char> &I)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
-
- vpImageIo::read(I, name) ;
+ m_image_number_next += m_image_step;
+ vpImageIo::read(I, ss.str());
width = I.getWidth();
height = I.getHeight();
}
/*!
- Acquire an image: read a ppm image from the disk.
+ Acquire an image reading the next image from the disk.
After this call, the image number is incremented considering the step.
- \param I the read image
+ \param I : The image read from a file.
*/
-void
-vpDiskGrabber::acquire(vpImage<vpRGBa> &I)
+void vpDiskGrabber::acquire(vpImage<vpRGBa> &I)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
+ m_image_number_next += m_image_step;
- vpImageIo::read(I, name) ;
+ vpImageIo::read(I, ss.str());
width = I.getWidth();
height = I.getHeight();
-
}
/*!
- Acquire an image: read a pfm image from the disk.
+ Acquire an image reading the next pfm image from the disk.
After this call, the image number is incremented considering the step.
- \param I the read image
+ \param I : The image read from a file.
*/
-void
-vpDiskGrabber::acquire(vpImage<float> &I)
+void vpDiskGrabber::acquire(vpImage<float> &I)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,image_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,image_number,extension) ;
-
- image_number += image_step ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
+ m_image_number_next += m_image_step;
- vpImageIo::readPFM(I, name) ;
+ vpImageIo::readPFM(I, ss.str());
width = I.getWidth();
height = I.getHeight();
-
}
/*!
- Acquire an image: read a pgm image from the disk.
+ Acquire an image reading the image with number \e img_number from the disk.
After this call, the image number is incremented considering the step.
\param I : The image read from a file.
\param img_number : The number of the desired image.
*/
-void
-vpDiskGrabber::acquire(vpImage<unsigned char> &I, long img_number)
+void vpDiskGrabber::acquire(vpImage<unsigned char> &I, long img_number)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
+ m_image_number_next += m_image_step;
- vpImageIo::read(I, name) ;
+ vpImageIo::read(I, ss.str());
width = I.getWidth();
height = I.getHeight();
}
/*!
- Acquire an image: read a ppm image from the disk.
+ Acquire an image reading the image with number \e img_number from the disk.
After this call, the image number is incremented considering the step.
\param I : The image read from a file.
\param img_number : The number of the desired image.
*/
-void
-vpDiskGrabber::acquire(vpImage<vpRGBa> &I, long img_number)
+void vpDiskGrabber::acquire(vpImage<vpRGBa> &I, long img_number)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
+ m_image_number_next += m_image_step;
- vpImageIo::read(I, name) ;
+ vpImageIo::read(I, ss.str());
width = I.getWidth();
height = I.getHeight();
-
}
-
/*!
- Acquire an image: read a pfm image from the disk.
- After this call, the image number is incremented considering the step.
+ Acquire an image reading the pfm image with number \e img_number from the
+ disk. After this call, the image number is incremented considering the step.
\param I : The image read from a file.
\param img_number : The number of the desired image.
*/
-void
-vpDiskGrabber::acquire(vpImage<float> &I, long img_number)
+void vpDiskGrabber::acquire(vpImage<float> &I, long img_number)
{
+ m_image_number = m_image_number_next;
+ std::stringstream ss;
+ if (m_use_generic_name) {
+ char filename[FILENAME_MAX];
+ 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;
+ }
- char name[FILENAME_MAX] ;
-
- if(useGenericName)
- sprintf(name,genericName,img_number) ;
- else
- sprintf(name,"%s/%s%0*ld.%s",directory,base_name,number_of_zero,img_number,extension) ;
-
- vpDEBUG_TRACE(2, "load: %s\n", name);
+ m_image_number_next += m_image_step;
- vpImageIo::readPFM(I, name) ;
+ vpImageIo::readPFM(I, ss.str());
width = I.getWidth();
height = I.getHeight();
-
}
/*!
- Not useful
+ Not useful.
- Here for compatibility issue with the vpFrameGrabber class
+ Here for compatibility issue with the vpFrameGrabber class.
*/
-void
-vpDiskGrabber::close()
+void vpDiskGrabber::close()
{
// Nothing do do here...
}
-
/*!
Destructor
In fact nothing to destroy...
*/
-vpDiskGrabber::~vpDiskGrabber()
-{
-}
-
+vpDiskGrabber::~vpDiskGrabber() {}
/*!
Set the main directory name (ie location of the image sequence)
*/
-void
-vpDiskGrabber::setDirectory(const char *dir)
-{
- sprintf(directory, "%s", dir) ;
-}
+void vpDiskGrabber::setDirectory(const std::string &dir) { m_directory = dir; }
/*!
Set the image base name.
*/
-void
-vpDiskGrabber::setBaseName(const char *name)
-{
- sprintf(base_name, "%s", name) ;
-}
+void vpDiskGrabber::setBaseName(const std::string &name) { m_base_name = name; }
/*!
Set the image extension.
*/
-void
-vpDiskGrabber::setExtension(const char *ext)
-{
- sprintf(extension, "%s", ext) ;
-}
+void vpDiskGrabber::setExtension(const std::string &ext) { m_extension = ext; }
/*!
Set the number of the image to be read.
*/
-void
-vpDiskGrabber::setImageNumber(long number)
+void vpDiskGrabber::setImageNumber(long number)
{
- image_number = number ;
- vpDEBUG_TRACE(2, "image number %ld", image_number);
-
+ m_image_number = number;
+ m_image_number_next = number;
}
/*!
Set the step between two images.
*/
-void
-vpDiskGrabber::setStep(int step)
-{
- image_step = step;
-}
+void vpDiskGrabber::setStep(long step) { m_image_step = step; }
/*!
Set the step between two images.
*/
-void
-vpDiskGrabber::setNumberOfZero(unsigned int noz)
-{
- number_of_zero = noz ;
-}
+void vpDiskGrabber::setNumberOfZero(unsigned int noz) { m_number_of_zero = noz; }
-void
-vpDiskGrabber::setGenericName(const char *generic_name)
+void vpDiskGrabber::setGenericName(const std::string &generic_name)
{
- if (strlen( generic_name ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the generic name"));
- }
-
- strcpy(this->genericName, generic_name) ;
- useGenericName = true;
+ m_generic_name = generic_name;
+ m_use_generic_name = true;
}
diff --git a/modules/io/src/video/vpFFMPEG.cpp b/modules/io/src/video/vpFFMPEG.cpp
deleted file mode 100644
index 9d48d45..0000000
--- a/modules/io/src/video/vpFFMPEG.cpp
+++ /dev/null
@@ -1,928 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.txt at the root directory of this source
- * distribution for additional information about the GNU GPL.
- *
- * For using ViSP with software that can not be combined with the GNU
- * GPL, please contact Inria about acquiring a ViSP Professional
- * Edition License.
- *
- * See http://visp.inria.fr for more information.
- *
- * This software was developed at:
- * Inria Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- *
- * If you have questions regarding the use of this file, please contact
- * Inria at visp at inria.fr
- *
- * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
- * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
- *
- * Description:
- * Class that manages the FFMPEG library.
- *
- * Authors:
- * Nicolas Melchior
- * Fabien Spindler
- *
- *****************************************************************************/
-
-/*!
- \file vpFFMPEG.cpp
- \brief Class that manages the FFMPEG library
-*/
-
-#include <stdio.h>
-
-#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/io/vpFFMPEG.h>
-#include <visp3/core/vpImageConvert.h>
-
-#ifdef VISP_HAVE_FFMPEG
-
-extern "C"
-{
-//#include <libavcodec/avcodec.h>
-#include <libavformat/avformat.h>
-#include <libswscale/swscale.h>
-}
-
-/*!
- Basic constructor.
-*/
-vpFFMPEG::vpFFMPEG()
- : width(-1), height(-1), frameNumber(0), pFormatCtx(NULL), pCodecCtx(NULL),
- pCodec(NULL), pFrame(NULL), pFrameRGB(NULL), pFrameGRAY(NULL), packet(NULL),
- img_convert_ctx(NULL), videoStream(0), numBytes(0), buffer(NULL), index(),
- streamWasOpen(false), streamWasInitialized(false), color_type(COLORED),
- f(NULL), outbuf(NULL), picture_buf(NULL), outbuf_size(0), out_size(0),
- bit_rate(500000), encoderWasOpened(false),
- framerate_stream(-1), framerate_encoder(25)
-{
- packet = new AVPacket;
-}
-
-/*!
- Basic destructor.
-*/
-vpFFMPEG::~vpFFMPEG()
-{
- closeStream();
- delete packet;
-}
-
-/*!
- Allocates and initializes the parameters depending on the video and the desired color type.
- One the stream is opened, it is possible to get the video encoding framerate getFramerate(),
- and the dimension of the images using getWidth() and getHeight().
-
- \param filename : Path to the video which has to be read.
- \param colortype : Desired color map used to open the video.
- The parameter can take two values : COLORED and GRAY_SCALED.
-
- \return It returns true if the paramters could be initialized. Else it returns false.
-*/
-bool vpFFMPEG::openStream(const char *filename, vpFFMPEGColorType colortype)
-{
- this->color_type = colortype;
-
- av_register_all();
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,0,0) // libavformat 52.84.0
- if (av_open_input_file (&pFormatCtx, filename, NULL, 0, NULL) != 0)
-#else
- if (avformat_open_input (&pFormatCtx, filename, NULL, NULL) != 0) // libavformat 53.4.0
-#endif
- {
- vpTRACE("Couldn't open file ");
- return false;
- }
-
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,21,0) // libavformat 53.21.0
- if (av_find_stream_info (pFormatCtx) < 0)
-#else
- if (avformat_find_stream_info (pFormatCtx, NULL) < 0)
-#endif
- return false;
-
- videoStream = 0;
- bool found_codec = false;
-
- /*
- * Detect streams types
- */
- for (unsigned int i = 0; i < pFormatCtx->nb_streams; i++)
- {
-#if LIBAVUTIL_VERSION_INT < AV_VERSION_INT(51,0,0)
- if(pFormatCtx->streams[i]->codec->codec_type==CODEC_TYPE_VIDEO) // avutil 50.33.0
-#else
- if(pFormatCtx->streams[i]->codec->codec_type==AVMEDIA_TYPE_VIDEO) // avutil 51.9.1
-#endif
- {
- videoStream = i;
- //std::cout << "rate: " << pFormatCtx->streams[i]->r_frame_rate.num << " " << pFormatCtx->streams[i]->r_frame_rate.den << std::endl;
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(55,12,0)
- framerate_stream = pFormatCtx->streams[i]->r_frame_rate.num;
- framerate_stream /= pFormatCtx->streams[i]->r_frame_rate.den;
-#else
- framerate_stream = pFormatCtx->streams[i]->avg_frame_rate.num;
- framerate_stream /= pFormatCtx->streams[i]->avg_frame_rate.den;
-#endif
- found_codec= true;
- break;
- }
- }
-
- if (found_codec)
- {
- pCodecCtx = pFormatCtx->streams[videoStream]->codec;
- pCodec = avcodec_find_decoder(pCodecCtx->codec_id);
-
- if (pCodec == NULL)
- {
- vpTRACE("unsuported codec");
- return false; // Codec not found
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
- if (avcodec_open (pCodecCtx, pCodec) < 0)
-#else
- if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0)
-#endif
- {
- vpTRACE("Could not open codec");
- return false; // Could not open codec
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrame = avcodec_alloc_frame();
-#else
- pFrame = av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (color_type == vpFFMPEG::COLORED)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrameRGB=avcodec_alloc_frame();
-#else
- pFrameRGB=av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (pFrameRGB == NULL)
- return false;
-
- numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
- }
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrameGRAY=avcodec_alloc_frame();
-#else
- pFrameGRAY=av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- if (pFrameGRAY == NULL)
- return false;
-
- numBytes = avpicture_get_size (PIX_FMT_GRAY8,pCodecCtx->width,pCodecCtx->height);
- }
-
- /*
- * Determine required buffer size and allocate buffer
- */
- width = pCodecCtx->width ;
- height = pCodecCtx->height ;
- buffer = (uint8_t *) malloc ((unsigned int)(sizeof (uint8_t)) * (unsigned int)numBytes);
- }
- else
- {
- vpTRACE("Didn't find a video stream");
- return false;
- }
-
- if (color_type == vpFFMPEG::COLORED)
- avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- avpicture_fill((AVPicture *)pFrameGRAY, buffer, PIX_FMT_GRAY8, pCodecCtx->width, pCodecCtx->height);
-
- streamWasOpen = true;
-
- return true;
-}
-
-/*!
- This method initializes the conversion parameters.
-
- It browses the video and lists all the frame. It sets the number of frame in the video.
-
- \returns It returns true if the method was executed without any problem. Else it returns false.
-*/
-bool vpFFMPEG::initStream()
-{
- if (color_type == vpFFMPEG::COLORED)
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_RGB24, SWS_BICUBIC, NULL, NULL, NULL);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, pCodecCtx->pix_fmt, pCodecCtx->width,pCodecCtx->height,PIX_FMT_GRAY8, SWS_BICUBIC, NULL, NULL, NULL);
-
- int ret = av_seek_frame(pFormatCtx, (int)videoStream, 0, AVSEEK_FLAG_ANY) ;
- if (ret < 0 )
- {
- vpTRACE("Error rewinding stream for full indexing") ;
- return false ;
- }
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frame_no = 0 ;
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- ret = avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- ret = avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (ret < 0 )
- {
- vpTRACE("Unable to decode video picture");
- }
- index.push_back(packet->dts);
- frame_no++ ;
- }
- }
- }
-
- frameNumber = index.size();
- av_free_packet(packet);
-
- streamWasInitialized = true;
-
- return true;
-}
-
-
-/*!
- Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
-
- \param I : The vpImage used to stored the video's frame.
- \param frame : The index of the frame which has to be read.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::getFrame(vpImage<vpRGBa> &I, unsigned int frame)
-{
-
- if (frame < frameNumber && streamWasInitialized== true)
- {
- int64_t targetPts = index[frame];
- av_seek_frame(pFormatCtx, (int)videoStream,targetPts, AVSEEK_FLAG_ANY);
- }
- else
- {
- vpTRACE("Couldn't get a frame");
- return false;
- }
-
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
-
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- Gets the Next frame in the video.
-
- \param I : The vpImage used to stored the video's frame.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::acquire(vpImage<vpRGBa> &I)
-{
- int frameFinished ;
-
- if (streamWasInitialized == false)
- {
- vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
- return false;
- }
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
- av_free_packet(packet);
- return true;
-}
-
-/*!
- Gets the \f$ frame \f$ th frame from the video and stores it in the image \f$ I \f$.
-
- \param I : The vpImage used to stored the video's frame.
- \param frame : The index of the frame which has to be read.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::getFrame(vpImage<unsigned char> &I, unsigned int frame)
-{
-
- if (frame < frameNumber && streamWasInitialized== true)
- {
- int64_t targetPts = index[frame];
- av_seek_frame(pFormatCtx,(int)videoStream,targetPts, AVSEEK_FLAG_ANY);
- }
- else
- {
- vpTRACE("Couldn't get a frame");
- return false;
- }
-
- avcodec_flush_buffers(pCodecCtx) ;
-
- int frameFinished ;
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
-
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- Gets the Next frame in the video.
-
- \param I : The vpImage used to stored the video's frame.
-
- \return It returns true if the frame could be read. Else it returns false.
-*/
-bool vpFFMPEG::acquire(vpImage<unsigned char> &I)
-{
- int frameFinished ;
-
- if (streamWasInitialized == false)
- {
- vpTRACE("Couldn't get a frame. The parameters have to be initialized before ");
- return false;
- }
-
- av_init_packet(packet);
- while (av_read_frame (pFormatCtx, packet) >= 0)
- {
- if (packet->stream_index == (int)videoStream)
- {
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(52,72,2)
- avcodec_decode_video(pCodecCtx, pFrame,
- &frameFinished, packet->data, packet->size);
-#else
- avcodec_decode_video2(pCodecCtx, pFrame, &frameFinished, packet); // libavcodec >= 52.72.2 (0.6)
-#endif
- if (frameFinished)
- {
- if (color_type == vpFFMPEG::COLORED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameRGB->data, pFrameRGB->linesize);
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pCodecCtx->height, pFrameGRAY->data, pFrameGRAY->linesize);
-
- copyBitmap(I);
- break;
- }
- }
- }
- av_free_packet(packet);
- return true;
-}
-
-
-/*!
- This method enable to fill the vpImage bitmap thanks to the selected frame.
-
- \throw vpException::dimensionError if either the height or the width
- associated to the class is negative.
-
- \param I : the image to fill.
-*/
-void vpFFMPEG::copyBitmap(vpImage<vpRGBa> &I)
-{
- if(height < 0 || width < 0){
- throw vpException(vpException::dimensionError, "width or height negative.");
- }
- I.resize((unsigned int)height, (unsigned int)width);
-
- unsigned char* line;
- unsigned char* beginOutput = (unsigned char*)I.bitmap;
- unsigned char* output = NULL;
-
- if (color_type == COLORED)
- {
- unsigned char* input = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line);
- *(output++) = *(line+1);
- *(output++) = *(line+2);
- *(output++) = 0;
-
- line+=3;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-
- else if (color_type == GRAY_SCALED)
- {
- unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
- int widthStep = pFrameGRAY->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- line = input;
- output = beginOutput + 4 * width * i;
- for(int j=0 ; j < width ; j++)
- {
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);
- *output++ = *(line);;
-
- line++;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-}
-
-/*!
- This method enable to fill the vpImage bitmap thanks to the selected frame.
-
- \throw vpException::dimensionError if either the height or the width
- associated to the class is negative.
-
- \param I : the image to fill.
-*/
-void vpFFMPEG::copyBitmap(vpImage<unsigned char> &I)
-{
- if(height < 0 || width < 0){
- throw vpException(vpException::dimensionError, "width or height negative.");
- }
- I.resize((unsigned int)height, (unsigned int)width);
-
- unsigned char* beginOutput = (unsigned char*)I.bitmap;
-
- if (color_type == GRAY_SCALED)
- {
- unsigned char* input = (unsigned char*)pFrameGRAY->data[0];
- int widthStep = pFrameGRAY->linesize[0];
- for(int i=0 ; i < height ; i++)
- {
- unsigned char *line = input;
- unsigned char *output = beginOutput + width * i;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(line);
-
- line++;
- }
- //go to the next line
- input+=widthStep;
- }
- }
-
- if (color_type == COLORED)
- {
- unsigned char* input = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
- for (int i = 0 ; i < height ; i++)
- {
- vpImageConvert::RGBToGrey(input + i*widthStep, beginOutput + i*width, (unsigned int)width, 1, false);
- }
- }
-}
-
-/*!
- Deallocates all the FFMPEG parameters.
-*/
-void vpFFMPEG::closeStream()
-{
- if (streamWasOpen)
- {
- if (buffer != NULL) {
- free(buffer);
- buffer = NULL;
- }
-
- if (color_type == vpFFMPEG::COLORED)
- av_free(pFrameRGB);
-
- else if (color_type == vpFFMPEG::GRAY_SCALED)
- av_free(pFrameGRAY);
-
- // Free the YUV frame
- av_free(pFrame);
-
- // Close the codec
- if (pCodecCtx) avcodec_close(pCodecCtx);
-
- // Close the video file
-#if LIBAVFORMAT_VERSION_INT < AV_VERSION_INT(53,17,0) // libavformat 53.17.0
- av_close_input_file(pFormatCtx);
-#else
- avformat_close_input(&pFormatCtx);
-#endif
- }
- streamWasOpen = false;
-
- if (encoderWasOpened)
- {
- if(f!=NULL) endWrite();
-
- if(buffer!=NULL) delete[] buffer;
-
- if(outbuf != NULL) delete[] outbuf;
-
- if(picture_buf != NULL) delete[] picture_buf;
-
- av_free(pFrameRGB);
- av_free(pFrame);
- if (pCodecCtx) avcodec_close(pCodecCtx);
- }
-
- encoderWasOpened = false;
-
- if(streamWasInitialized || encoderWasOpened){
- sws_freeContext (img_convert_ctx);
- }
- streamWasInitialized = false;
-}
-
-/*!
- Allocates and initializes the parameters depending on the video to write.
-
- \param filename : Path to the video which has to be writen.
- \param w,h : Width and height of the image which will be saved.
- \param codec : Type of codec used to encode the video.
-
- By default codec is set to AV_CODEC_ID_MPEG1VIDEO. But if installed, you can use one of the
- AVCodecID proposed by ffmpeg such as : AV_CODEC_ID_MPEG2VIDEO, AV_CODEC_ID_MPEG2VIDEO_XVMC,
- AV_CODEC_ID_MPEG4, AV_CODEC_ID_H264, ... (More AVCodecID can be found in the ffmpeg documentation).
-
- Of course to use the codec it must be installed on your computer.
-
- \return It returns true if the paramters could be initialized. Else it returns false.
-*/
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
-bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, CodecID codec)
-#else
-bool vpFFMPEG::openEncoder(const char *filename, unsigned int w, unsigned int h, AVCodecID codec)
-#endif
-{
- av_register_all();
-
- /* find the mpeg1 video encoder */
- pCodec = avcodec_find_encoder(codec);
- if (pCodec == NULL) {
- fprintf(stderr, "codec not found\n");
- return false;
- }
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,5,0) // libavcodec 53.5.0
- pCodecCtx = avcodec_alloc_context();
-#else
- pCodecCtx = avcodec_alloc_context3(NULL);
-#endif
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,34,0)
- pFrame = avcodec_alloc_frame();
- pFrameRGB = avcodec_alloc_frame();
-#else
- pFrame = av_frame_alloc(); // libavcodec 55.34.1
- pFrameRGB = av_frame_alloc(); // libavcodec 55.34.1
-#endif
-
- /* put sample parameters */
- pCodecCtx->bit_rate = (int)bit_rate;
- /* resolution must be a multiple of two */
- pCodecCtx->width = (int)w;
- pCodecCtx->height = (int)h;
- this->width = (int)w;
- this->height = (int)h;
- /* frames per second */
- pCodecCtx->time_base.num = 1;
- pCodecCtx->time_base.den = framerate_encoder;
- pCodecCtx->gop_size = 10; /* emit one intra frame every ten frames */
- pCodecCtx->max_b_frames=1;
- pCodecCtx->pix_fmt = PIX_FMT_YUV420P;
-
- /* open it */
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(53,35,0) // libavcodec 53.35.0
- if (avcodec_open (pCodecCtx, pCodec) < 0) {
-#else
- if (avcodec_open2 (pCodecCtx, pCodec, NULL) < 0) {
-#endif
- fprintf(stderr, "could not open codec\n");
- return false;
- }
-
- /* the codec gives us the frame size, in samples */
-
- f = fopen(filename, "wb");
- if (!f) {
- fprintf(stderr, "could not open %s\n", filename);
- return false;
- }
-
- outbuf_size = 100000;
- outbuf = new uint8_t[outbuf_size];
-
- numBytes = avpicture_get_size (PIX_FMT_YUV420P,pCodecCtx->width,pCodecCtx->height);
- picture_buf = new uint8_t[numBytes];
- avpicture_fill((AVPicture *)pFrame, picture_buf, PIX_FMT_YUV420P, pCodecCtx->width, pCodecCtx->height);
-
- numBytes = avpicture_get_size (PIX_FMT_RGB24,pCodecCtx->width,pCodecCtx->height);
- buffer = new uint8_t[numBytes];
- avpicture_fill((AVPicture *)pFrameRGB, buffer, PIX_FMT_RGB24, pCodecCtx->width, pCodecCtx->height);
-
- img_convert_ctx= sws_getContext(pCodecCtx->width, pCodecCtx->height, PIX_FMT_RGB24, pCodecCtx->width,pCodecCtx->height,PIX_FMT_YUV420P, SWS_BICUBIC, NULL, NULL, NULL);
-
- encoderWasOpened = true;
-
- return true;
-}
-
-
-/*!
- Saves the image I as frame of the video.
-
- \param I : the image to save.
-
- \return It returns true if the image could be saved.
-*/
-bool vpFFMPEG::saveFrame(vpImage<vpRGBa> &I)
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- writeBitmap(I);
- sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
-
- int got_output;
- int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
- fflush(stdout);
- return true;
-}
-
-
-/*!
- Saves the image I as frame of the video.
-
- \param I : the image to save.
-
- \return It returns true if the image could be saved.
-*/
-bool vpFFMPEG::saveFrame(vpImage<unsigned char> &I)
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- writeBitmap(I);
- sws_scale(img_convert_ctx, pFrameRGB->data, pFrameRGB->linesize, 0, pCodecCtx->height, pFrame->data, pFrame->linesize);
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- out_size = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, pFrame);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
-
- int got_output;
- int ret = avcodec_encode_video2(pCodecCtx, &pkt, pFrame, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
-
- fflush(stdout);
- return true;
-}
-
-/*!
- Ends the writing of the video and close the file.
-
- \return It returns true if the file was closed without problem
-*/
-bool vpFFMPEG::endWrite()
-{
- if (encoderWasOpened == false)
- {
- vpTRACE("Couldn't save a frame. The parameters have to be initialized before ");
- return false;
- }
-
- int ret = 1;
- while (ret != 0)
- {
-
-#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,2,100) // libavcodec 54.2.100
- ret = avcodec_encode_video(pCodecCtx, outbuf, outbuf_size, NULL);
- fwrite(outbuf, 1, (size_t)out_size, f);
-#else
- AVPacket pkt;
- av_init_packet(&pkt);
- pkt.data = NULL; // packet data will be allocated by the encoder
- pkt.size = 0;
- int got_output;
- ret = avcodec_encode_video2(pCodecCtx, &pkt, NULL, &got_output);
- if (ret < 0) {
- std::cerr << "Error encoding frame in " << __FILE__ << " " << __LINE__ << " " << __FUNCTION__ << std::endl;
- return false;
- }
- if (got_output) {
- fwrite(pkt.data, 1, pkt.size, f);
- av_free_packet(&pkt);
- }
-#endif
- }
-
- /*The end of a mpeg file*/
- outbuf[0] = 0x00;
- outbuf[1] = 0x00;
- outbuf[2] = 0x01;
- outbuf[3] = 0xb7;
- fwrite(outbuf, 1, 4, f);
- fclose(f);
- f = NULL;
- return true;
-}
-
-/*!
- This method enables to fill the frame bitmap thanks to the vpImage bitmap.
-*/
-void vpFFMPEG::writeBitmap(vpImage<vpRGBa> &I)
-{
- unsigned char* beginInput = (unsigned char*)I.bitmap;
- unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
-
- for(int i=0 ; i < height ; i++)
- {
- unsigned char *input = beginInput + 4 * i * width;
- unsigned char *output = beginOutput + i * widthStep;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(input);
- *(output++) = *(input+1);
- *(output++) = *(input+2);
-
- input+=4;
- }
- }
-}
-
-
-/*!
- This method enables to fill the frame bitmap thanks to the vpImage bitmap.
-*/
-void vpFFMPEG::writeBitmap(vpImage<unsigned char> &I)
-{
- unsigned char* beginInput = (unsigned char*)I.bitmap;
- unsigned char* beginOutput = (unsigned char*)pFrameRGB->data[0];
- int widthStep = pFrameRGB->linesize[0];
-
- for(int i=0 ; i < height ; i++)
- {
- unsigned char *input = beginInput + i * width;
- unsigned char *output = beginOutput + i * widthStep;
- for(int j=0 ; j < width ; j++)
- {
- *(output++) = *(input);
- *(output++) = *(input);
- *(output++) = *(input);
-
- input++;
- }
- }
-}
-
-#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_core.a(vpFFMPEG.cpp.o) has no symbols
-void dummy_vpFFMPEG() {};
-#endif
diff --git a/modules/io/src/video/vpVideoReader.cpp b/modules/io/src/video/vpVideoReader.cpp
index 3547e33..37da578 100644
--- a/modules/io/src/video/vpVideoReader.cpp
+++ b/modules/io/src/video/vpVideoReader.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,24 +43,24 @@
*/
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/io/vpVideoReader.h>
-#include <iostream>
+#include <cctype>
#include <fstream>
-#include <limits> // numeric_limits
+#include <iostream>
+#include <limits> // numeric_limits
/*!
Basic constructor.
*/
vpVideoReader::vpVideoReader()
: vpFrameGrabber(), imSequence(NULL),
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg(NULL),
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture(), frame(),
+#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)
+ formatType(FORMAT_UNKNOWN), initFileName(false), isOpen(false), frameCount(0), firstFrame(0), lastFrame(0),
+ firstFrameIndexIsSet(false), lastFrameIndexIsSet(false), frameStep(1), frameRate(0.)
{
}
@@ -68,64 +69,122 @@ Basic destructor.
*/
vpVideoReader::~vpVideoReader()
{
- if (imSequence != NULL)
- {
- delete imSequence;
- }
-#ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- delete ffmpeg;
- }
-#endif
+ if (imSequence != NULL) {
+ delete imSequence;
+ }
}
-
/*!
-It enables to set the path and the name of the file(s) which as/have to be read.
+It enables to set the path and the name of the file(s) which as/have to be
+read.
-If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
+If you want to read a video file, \f$ filename \f$ corresponds to the path to
+the file (example : /local/video.mpeg).
-If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+If you want to read a sequence of images, \f$ filename \f$ corresponds to the
+path followed by the image name template. For example, if you want to read
+different images named image0001.jpeg, image0002.jpg, ... and located in the
+folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-\param filename : Path to a video file or file name template of a image sequence.
+\param filename : Path to a video file or file name template of a image
+sequence.
*/
void vpVideoReader::setFileName(const char *filename)
{
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
- }
+ if ((!filename) || (*filename == '\0')) {
+ vpERROR_TRACE("filename empty ");
+ throw(vpImageException(vpImageException::noFileNameError, "filename empty "));
+ }
- if (strlen( filename ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the file name"));
- }
+ if (strlen(filename) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to initialize the file name"));
+ }
- strcpy(this->fileName,filename);
+ strcpy(this->fileName, filename);
- formatType = getFormat(fileName);
+ formatType = getFormat(fileName);
if (formatType == FORMAT_UNKNOWN) {
throw(vpException(vpException::badValue, "Filename extension not supported"));
}
- initFileName = true;
+ // 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()));
+ }
+ }
+
+ initFileName = true;
}
/*!
-It enables to set the path and the name of the file(s) which as/have to be read.
+It enables to set the path and the name of the file(s) which as/have to be
+read.
-If you want to read a video file, \f$ filename \f$ corresponds to the path to the file (example : /local/video.mpeg).
+If you want to read a video file, \f$ filename \f$ corresponds to the path to
+the file (example : /local/video.mpeg).
-If you want to read a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to read different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+If you want to read a sequence of images, \f$ filename \f$ corresponds to the
+path followed by the image name template. For example, if you want to read
+different images named image0001.jpeg, image0002.jpg, ... and located in the
+folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-\param filename : Path to a video file or file name template of a image sequence.
+\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()); }
+
+/*!
+ Open video stream and get first and last frame indexes.
*/
-void vpVideoReader::setFileName(const std::string &filename)
+void vpVideoReader::getProperties()
{
- setFileName(filename.c_str());
+ if (!initFileName) {
+ throw(vpImageException(vpImageException::noFileNameError, "The generic filename has to be set"));
+ }
+
+ if (isImageExtensionSupported()) {
+ imSequence = new vpDiskGrabber;
+ imSequence->setGenericName(fileName);
+ imSequence->setStep(frameStep);
+ if (firstFrameIndexIsSet) {
+ imSequence->setImageNumber(firstFrame);
+ }
+ frameRate = -1.;
+ } else if (isVideoExtensionSupported()) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ capture.open(fileName);
+
+ if (!capture.isOpened()) {
+ 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);
+ height = (unsigned int)capture.get(cv::CAP_PROP_FRAME_HEIGHT);
+ frameRate = (double)capture.get(cv::CAP_PROP_FPS);
+#else
+ width = (unsigned int)capture.get(CV_CAP_PROP_FRAME_WIDTH);
+ height = (unsigned int)capture.get(CV_CAP_PROP_FRAME_HEIGHT);
+ frameRate = capture.get(CV_CAP_PROP_FPS);
+#endif
+
+#else
+ 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
+ // format.");
+ throw(vpException(vpException::fatalError, "The format of the file does "
+ "not correspond to a readable "
+ "format supported by ViSP."));
+ }
+
+ findFirstFrameIndex();
+ isOpen = true;
+ findLastFrameIndex();
}
/*!
@@ -135,63 +194,32 @@ Grab the first frame and stores it in the image \f$ I \f$.
\param I : The image where the frame is stored.
*/
-void vpVideoReader::open(vpImage< vpRGBa > &I)
+void vpVideoReader::open(vpImage<vpRGBa> &I)
{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (isImageExtensionSupported())
- {
- imSequence = new vpDiskGrabber;
- imSequence->setGenericName(fileName);
- if (firstFrameIndexIsSet)
- imSequence->setImageNumber(firstFrame);
- }
- else if (isVideoExtensionSupported())
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- if(!ffmpeg->openStream(fileName, vpFFMPEG::COLORED))
- throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
- ffmpeg->initStream();
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture.open(fileName);
+ getProperties();
+
+ frameCount = firstFrame;
+ if (!getFrame(I, firstFrame)) {
+ throw(vpException(vpException::ioError, "Could not read the video first frame"));
+ }
+
+ // Rewind to the first frame since open() should not increase the frame
+ // counter
+ frameCount = firstFrame;
+
+ if (isVideoExtensionSupported()) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
- if(!capture.isOpened())
- {
- throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
- }
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ capture.set(cv::CAP_PROP_POS_FRAMES, firstFrame - 1);
#else
- //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+ capture.set(CV_CAP_PROP_POS_FRAMES, firstFrame - 1);
+#endif
+ frameCount--;
#endif
- }
- else if (formatType == FORMAT_UNKNOWN)
- {
- //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
- throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
- }
-
- findFirstFrameIndex();
- frameCount = firstFrame;
- if(!getFrame(I, firstFrame))
- {
- //vpERROR_TRACE("Could not read the video first frame");
- throw (vpException(vpException::ioError ,"Could not read the video first frame"));
- }
-
- height = I.getHeight();
- width = I.getWidth();
-
- isOpen = true;
- findLastFrameIndex();
- frameCount = firstFrame; // open() should not increase the frame counter
+ }
}
-
/*!
Sets all the parameters needed to read the video or the image sequence.
@@ -201,156 +229,182 @@ Grab the first frame and stores it in the image \f$ I \f$.
*/
void vpVideoReader::open(vpImage<unsigned char> &I)
{
- if (!initFileName)
- {
- vpERROR_TRACE("The generic filename has to be set");
- throw (vpImageException(vpImageException::noFileNameError,"filename empty"));
- }
-
- if (isImageExtensionSupported())
- {
- imSequence = new vpDiskGrabber;
- imSequence->setGenericName(fileName);
- if (firstFrameIndexIsSet)
- imSequence->setImageNumber(firstFrame);
- }
- else if (isVideoExtensionSupported())
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- if (!ffmpeg->openStream(fileName, vpFFMPEG::GRAY_SCALED))
- throw (vpException(vpException::ioError ,"Could not open the video with ffmpeg"));
- ffmpeg->initStream();
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- capture.open(fileName);
+ getProperties();
- if(!capture.isOpened())
- {
- throw (vpException(vpException::ioError ,"Could not open the video with opencv"));
- }
-#else
- //vpERROR_TRACE("To read video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To read video files ViSP should be build with ffmpeg or opencv >= 2.1.0 3rd party libraries."));
-#endif
- }
- else if (formatType == FORMAT_UNKNOWN)
- {
- //vpERROR_TRACE("The format of the file does not correspond to a readable format.");
- throw (vpException(vpException::fatalError ,"The format of the file does not correspond to a readable format supported by ViSP."));
+ frameCount = firstFrame;
+ if (!getFrame(I, firstFrame)) {
+ throw(vpException(vpException::ioError, "Could not read the video first frame"));
}
- findFirstFrameIndex();
- frameCount = firstFrame;
- if(!getFrame(I,firstFrame))
- {
- //vpERROR_TRACE("Could not read the video first frame");
- throw (vpException(vpException::ioError ,"Could not read the video first frame"));
- }
+ // Rewind to the first frame since open() should not increase the frame
+ // counter
+ frameCount = firstFrame;
- height = I.getHeight();
- width = I.getWidth();
+ if (isVideoExtensionSupported()) {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
- isOpen = true;
- findLastFrameIndex();
- frameCount = firstFrame; // open() should not increase the frame counter
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ capture.set(cv::CAP_PROP_POS_FRAMES, firstFrame - 1);
+#else
+ capture.set(CV_CAP_PROP_POS_FRAMES, firstFrame - 1);
+#endif
+ frameCount--;
+#endif
+ }
}
-
/*!
-Grabs the current (k) image in the stack of frames and increments the frame counter
-in order to grab the next image (k+1) during the next use of the method. If open()
-was not called previously, this method opens the video reader.
+Grabs the current (k) image in the stack of frames and increments the frame
+counter in order to grab the next image (k+1) during the next use of the
+method. If open() was not called previously, this method opens the video
+reader.
This method enables to use the class as frame grabber.
\param I : The image where the frame is stored.
*/
-void vpVideoReader::acquire(vpImage< vpRGBa > &I)
+void vpVideoReader::acquire(vpImage<vpRGBa> &I)
{
- if (!isOpen) {
- open(I);
- }
-
- //getFrame(I,frameCount);
- if (imSequence != NULL)
- {
- imSequence->acquire(I);
- frameCount++; // next index
+ if (!isOpen) {
+ open(I);
}
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg !=NULL)
- {
- ffmpeg->acquire(I);
- frameCount++; // next index
+
+ // getFrame(I,frameCount);
+ if (imSequence != NULL) {
+ imSequence->setStep(frameStep);
+ imSequence->acquire(I);
+ frameCount = imSequence->getImageNumber();
+ if (frameCount + frameStep > lastFrame) {
+ imSequence->setImageNumber(frameCount);
+ } else if (frameCount + frameStep < firstFrame) {
+ imSequence->setImageNumber(frameCount);
+ }
}
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else
- {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else {
capture >> frame;
+ if (frameStep == 1) {
+ frameCount++;
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+ frameCount = (long)capture.get(cv::CAP_PROP_POS_FRAMES);
+ if (frameStep > 0) {
+ if (frameCount + frameStep <= lastFrame) {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount - 1);
+ }
+ } else if (frameStep < 0) {
+ if (frameCount + frameStep >= firstFrame) {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(cv::CAP_PROP_POS_FRAMES, firstFrame - 1);
+ }
+ }
#else
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
+ frameCount = (long)capture.get(CV_CAP_PROP_POS_FRAMES);
+ if (frameStep > 0) {
+ if (frameCount + frameStep <= lastFrame) {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount - 1);
+ }
+ } else if (frameStep < 0) {
+ if (frameCount + frameStep >= firstFrame) {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(CV_CAP_PROP_POS_FRAMES, firstFrame - 1);
+ }
+ }
#endif
+ }
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
+ if (frame.empty())
+ setLastFrameIndex(frameCount - frameStep);
else
vpImageConvert::convert(frame, I);
- }
+ }
#endif
}
-
/*!
-Grabs the kth image in the stack of frames and increments the frame counter in order to grab the next image (k+1) during the next use of the method.
+Grabs the kth image in the stack of frames and increments the frame counter in
+order to grab the next image (k+1) during the next use of the method.
This method enables to use the class as frame grabber.
\param I : The image where the frame is stored.
*/
-void vpVideoReader::acquire(vpImage< unsigned char > &I)
+void vpVideoReader::acquire(vpImage<unsigned char> &I)
{
- if (!isOpen) {
- open(I);
- }
-
- if (imSequence != NULL)
- {
- imSequence->acquire(I);
- frameCount++; // next index
+ if (!isOpen) {
+ open(I);
}
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL)
- {
- ffmpeg->acquire(I);
- frameCount++; // next index
+
+ if (imSequence != NULL) {
+ imSequence->setStep(frameStep);
+ imSequence->acquire(I);
+ frameCount = imSequence->getImageNumber();
+ if (frameCount + frameStep > lastFrame) {
+ imSequence->setImageNumber(frameCount);
+ } else if (frameCount + frameStep < firstFrame) {
+ imSequence->setImageNumber(frameCount);
+ }
}
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else
- {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else {
capture >> frame;
+ if (frameStep == 1) {
+ frameCount++;
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x030000
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
+ frameCount = (long)capture.get(cv::CAP_PROP_POS_FRAMES);
+ if (frameStep > 0) {
+ if (frameCount + frameStep <= lastFrame) {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount - 1);
+ }
+ } else if (frameStep < 0) {
+ if (frameCount + frameStep >= firstFrame) {
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(cv::CAP_PROP_POS_FRAMES, firstFrame - 1);
+ }
+ }
#else
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
+ frameCount = (long)capture.get(CV_CAP_PROP_POS_FRAMES);
+ if (frameStep > 0) {
+ if (frameCount + frameStep <= lastFrame) {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount - 1);
+ }
+ } else if (frameStep < 0) {
+ if (frameCount + frameStep >= firstFrame) {
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount + frameStep - 1);
+ } else {
+ capture.set(CV_CAP_PROP_POS_FRAMES, firstFrame - 1);
+ }
+ }
#endif
+ }
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
+ if (frame.empty())
+ setLastFrameIndex(frameCount - frameStep);
else
vpImageConvert::convert(frame, I);
}
#endif
}
-
/*!
Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
-\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
-But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
-one after one.
+\warning For the video files this method is not precise, and returns the
+nearest key frame from the expected frame. But this method enables to position
+the reader where you want. Then, use the acquire method to grab the following
+images one after one.
\param I : The vpImage used to stored the frame.
\param frame_index : The index of the frame which has to be read.
@@ -359,76 +413,68 @@ one after one.
*/
bool vpVideoReader::getFrame(vpImage<vpRGBa> &I, long frame_index)
{
- if (imSequence != NULL)
- {
- try
- {
+ if (imSequence != NULL) {
+ try {
imSequence->acquire(I, frame_index);
- frameCount = frame_index + 1; // next index
- }
- catch(...)
- {
- vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
- return false;
- }
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ width = I.getWidth();
+ height = I.getHeight();
+ frameCount = imSequence->getImageNumber();
+ imSequence->setImageNumber(frameCount); // to not increment vpDiskGrabber next image
+ if (frameCount + frameStep > lastFrame) {
+ imSequence->setImageNumber(frameCount);
+ } else if (frameCount + frameStep < firstFrame) {
+ imSequence->setImageNumber(frameCount);
+ }
+ } catch (...) {
+ vpERROR_TRACE("Couldn't find the %u th frame", frame_index);
return false;
}
- frameCount = frame_index + 1; // next index
-#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ } else {
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ if (!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index)) {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index);
return false;
}
capture >> frame;
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
- if(frame.empty()) {
+ frameCount = frame_index + frameStep; // next index
+ capture.set(cv::CAP_PROP_POS_FRAMES, frameCount);
+ if (frame.empty()) {
// New trial that makes things working with opencv 3.0.0
capture >> frame;
- if(frame.empty()) {
- setLastFrameIndex(frameCount-1);
+ if (frame.empty()) {
+ setLastFrameIndex(frameCount - frameStep);
return false;
- }
- else {
+ } else {
vpImageConvert::convert(frame, I);
}
- }
- else
+ } else
vpImageConvert::convert(frame, I);
#elif defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ if (!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index)) {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index);
return false;
}
capture >> frame;
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES); // next index
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
+ frameCount = frame_index + frameStep; // next index
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount);
+ if (frame.empty())
+ setLastFrameIndex(frameCount - frameStep);
else
vpImageConvert::convert(frame, I);
#endif
}
-
- return true;
+ return true;
}
-
/*!
Gets the \f$ frame \f$ th frame and stores it in the image \f$ I \f$.
-\warning For the video files this method is not precise, and returns the nearest key frame from the expected frame.
-But this method enables to position the reader where you want. Then, use the acquire method to grab the following images
-one after one.
+\warning For the video files this method is not precise, and returns the
+nearest key frame from the expected frame. But this method enables to position
+the reader where you want. Then, use the acquire method to grab the following
+images one after one.
\param I : The vpImage used to stored the frame.
\param frame_index : The index of the frame which has to be read.
@@ -437,100 +483,96 @@ one after one.
*/
bool vpVideoReader::getFrame(vpImage<unsigned char> &I, long frame_index)
{
- if (imSequence != NULL)
- {
- try
- {
+ if (imSequence != NULL) {
+ try {
imSequence->acquire(I, frame_index);
- frameCount = frame_index + 1;
- }
- catch(...)
- {
- vpERROR_TRACE("Couldn't find the %u th frame", frame_index) ;
- return false;
- }
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- if(!ffmpeg->getFrame(I, (unsigned int)frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ width = I.getWidth();
+ height = I.getHeight();
+ frameCount = imSequence->getImageNumber();
+ imSequence->setImageNumber(frameCount); // to not increment vpDiskGrabber next image
+ if (frameCount + frameStep > lastFrame) {
+ imSequence->setImageNumber(frameCount);
+ } else if (frameCount + frameStep < firstFrame) {
+ imSequence->setImageNumber(frameCount);
+ }
+ } catch (...) {
+ vpERROR_TRACE("Couldn't find the %u th frame", frame_index);
return false;
}
- frameCount = frame_index + 1; // next index
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- if(!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index) ;
+ } else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ if (!capture.set(cv::CAP_PROP_POS_FRAMES, frame_index)) {
+ vpERROR_TRACE("Couldn't find the %ld th frame", frame_index);
return false;
}
capture >> frame;
- frameCount = (long) capture.get(cv::CAP_PROP_POS_FRAMES); // next index
- if(frame.empty()) {
+ if (frame.empty()) {
// New trial that makes things working with opencv 3.0.0
capture >> frame;
- if(frame.empty()) {
- setLastFrameIndex(frameCount-1);
+ if (frame.empty()) {
+ setLastFrameIndex(frameCount - frameStep);
return false;
- }
- else {
+ } else {
vpImageConvert::convert(frame, I);
}
- }
- else
+ } else {
vpImageConvert::convert(frame, I);
+ }
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- if(!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index))
- {
- vpERROR_TRACE("Couldn't find the %ld th frame", frame_index); // next index
+ if (!capture.set(CV_CAP_PROP_POS_FRAMES, frame_index)) {
+ vpERROR_TRACE("Couldn't find the %ld th frame",
+ frame_index); // next index
return false;
}
capture >> frame;
- frameCount = (long) capture.get(CV_CAP_PROP_POS_FRAMES);
- if(frame.empty())
- setLastFrameIndex(frameCount-1);
+ frameCount = (long)capture.get(CV_CAP_PROP_POS_FRAMES);
+ if (frameStep > 1) {
+ frameCount += frameStep - 1; // next index
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount);
+ } else if (frameStep < -1) {
+ frameCount += frameStep - 1; // next index
+ capture.set(CV_CAP_PROP_POS_FRAMES, frameCount);
+ }
+ if (frame.empty())
+ setLastFrameIndex(frameCount - frameStep);
else
vpImageConvert::convert(frame, I);
#endif
}
-
- return true;
+ return true;
}
-
/*!
Gets the format of the file(s) which has/have to be read.
\return Returns the format.
*/
-vpVideoReader::vpVideoFormatType
- vpVideoReader::getFormat(const char *filename)
-{
- std::string sfilename(filename);
-
- std::string ext = vpVideoReader::getExtension(sfilename);
-
- if (ext.compare(".PGM") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".pgm") == 0)
- return FORMAT_PGM;
- else if (ext.compare(".PPM") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".ppm") == 0)
- return FORMAT_PPM;
- else if (ext.compare(".JPG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".JPEG") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".jpeg") == 0)
- return FORMAT_JPEG;
- else if (ext.compare(".PNG") == 0)
- return FORMAT_PNG;
- else if (ext.compare(".png") == 0)
- return FORMAT_PNG;
+vpVideoReader::vpVideoFormatType vpVideoReader::getFormat(const char *filename)
+{
+ std::string sfilename(filename);
+
+ std::string ext = vpVideoReader::getExtension(sfilename);
+
+ if (ext.compare(".PGM") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".pgm") == 0)
+ return FORMAT_PGM;
+ else if (ext.compare(".PPM") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".ppm") == 0)
+ return FORMAT_PPM;
+ else if (ext.compare(".JPG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".JPEG") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".jpeg") == 0)
+ return FORMAT_JPEG;
+ else if (ext.compare(".PNG") == 0)
+ return FORMAT_PNG;
+ else if (ext.compare(".png") == 0)
+ return FORMAT_PNG;
else if (ext.compare(".TIFF") == 0)
return FORMAT_TIFF;
else if (ext.compare(".tiff") == 0)
@@ -560,59 +602,58 @@ vpVideoReader::vpVideoFormatType
else if (ext.compare(".jp2") == 0)
return FORMAT_JPEG2000;
else if (ext.compare(".AVI") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".avi") == 0)
- return FORMAT_AVI;
- else if (ext.compare(".MPEG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpeg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPG") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".mpg") == 0)
- return FORMAT_MPEG;
- else if (ext.compare(".MPEG4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mpeg4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MP4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".mp4") == 0)
- return FORMAT_MPEG4;
- else if (ext.compare(".MOV") == 0)
- return FORMAT_MOV;
- else if (ext.compare(".mov") == 0)
- return FORMAT_MOV;
- else if (ext.compare(".OGV") == 0)
- return FORMAT_OGV;
- else if (ext.compare(".ogv") == 0)
- return FORMAT_OGV;
- else if (ext.compare(".WMV") == 0)
- return FORMAT_WMV;
- else if (ext.compare(".wmv") == 0)
- return FORMAT_WMV;
- else if (ext.compare(".FLV") == 0)
- return FORMAT_FLV;
- else if (ext.compare(".flv") == 0)
- return FORMAT_FLV;
- else if (ext.compare(".MKV") == 0)
- return FORMAT_MKV;
- else if (ext.compare(".mkv") == 0)
- return FORMAT_MKV;
- else
- return FORMAT_UNKNOWN;
+ return FORMAT_AVI;
+ else if (ext.compare(".avi") == 0)
+ return FORMAT_AVI;
+ else if (ext.compare(".MPEG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpeg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPG") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".mpg") == 0)
+ return FORMAT_MPEG;
+ else if (ext.compare(".MPEG4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mpeg4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MP4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".mp4") == 0)
+ return FORMAT_MPEG4;
+ else if (ext.compare(".MOV") == 0)
+ return FORMAT_MOV;
+ else if (ext.compare(".mov") == 0)
+ return FORMAT_MOV;
+ else if (ext.compare(".OGV") == 0)
+ return FORMAT_OGV;
+ else if (ext.compare(".ogv") == 0)
+ return FORMAT_OGV;
+ else if (ext.compare(".WMV") == 0)
+ return FORMAT_WMV;
+ else if (ext.compare(".wmv") == 0)
+ return FORMAT_WMV;
+ else if (ext.compare(".FLV") == 0)
+ return FORMAT_FLV;
+ else if (ext.compare(".flv") == 0)
+ return FORMAT_FLV;
+ else if (ext.compare(".MKV") == 0)
+ return FORMAT_MKV;
+ else if (ext.compare(".mkv") == 0)
+ return FORMAT_MKV;
+ else
+ return FORMAT_UNKNOWN;
}
// return the extension of the file including the dot
std::string vpVideoReader::getExtension(const std::string &filename)
{
- // extract the extension
- size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
- return ext;
+ // extract the extension
+ size_t dot = filename.find_last_of(".");
+ std::string ext = filename.substr(dot, filename.size() - 1);
+ return ext;
}
-
/*!
Get the last frame index (update the lastFrame attribute).
*/
@@ -620,128 +661,84 @@ void vpVideoReader::findLastFrameIndex()
{
if (!isOpen) {
vpERROR_TRACE("Use the open method before");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
+ throw(vpException(vpException::notInitialized, "file not yet opened"));
}
-
+
if (imSequence != NULL) {
- if (! lastFrameIndexIsSet) {
- char name[FILENAME_MAX];
- long image_number = firstFrame;
- bool failed;
- do {
- std::fstream file;
- sprintf(name,fileName,image_number) ;
- file.open(name, std::ios::in);
- failed = file.fail();
- if (!failed) {
- file.close();
- image_number++;
+ if (!lastFrameIndexIsSet) {
+ std::string imageNameFormat = vpIoTools::getName(std::string(fileName));
+ std::string dirName = vpIoTools::getParent(std::string(fileName));
+ if (dirName == "") {
+ dirName = ".";
+ }
+ std::vector<std::string> files = vpIoTools::getDirFiles(dirName);
+ lastFrame = 0;
+ for (size_t i = 0; i < files.size(); i++) {
+ // Checking that file name satisfies image format,
+ // specified by imageNameFormat, and extracting imageIndex
+ long imageIndex = extractImageIndex(files[i], imageNameFormat);
+ if ((imageIndex != -1) && (imageIndex > lastFrame)) {
+ lastFrame = imageIndex;
}
- } while(!failed);
-
- lastFrame = image_number -1;
+ }
}
}
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL) {
- if (! lastFrameIndexIsSet) {
- lastFrame = (long)(ffmpeg->getFrameNumber());
- }
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- else if (! lastFrameIndexIsSet)
- {
- lastFrame = (long) capture.get(cv::CAP_PROP_FRAME_COUNT);
- if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 3.0.0
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ else if (!lastFrameIndexIsSet) {
+ lastFrame = (long)capture.get(cv::CAP_PROP_FRAME_COUNT);
+ if (lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it
+ // return 2 with OpenCV 3.0.0
{
- //std::cout << "Warning: Problem with cv::CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
+ // std::cout << "Warning: Problem with cv::CAP_PROP_FRAME_COUNT. We set
+ // video last frame to an arbitrary value (1000)." << std::endl;
lastFrame = 100000; // Set lastFrame to an arbitrary value
}
- lastFrame--; //Last frame index = total frame count - 1
}
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else if (! lastFrameIndexIsSet)
- {
- lastFrame = (long) capture.get(CV_CAP_PROP_FRAME_COUNT);
- if(lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it return 2 with OpenCV 2.4.10
+ else if (!lastFrameIndexIsSet) {
+ lastFrame = (long)capture.get(CV_CAP_PROP_FRAME_COUNT);
+ if (lastFrame <= 2) // with tutorial/matching/video-postcard.mpeg it
+ // return 2 with OpenCV 2.4.10
{
- //std::cout << "Warning: Problem with CV_CAP_PROP_FRAME_COUNT. We set video last frame to an arbitrary value (1000)." << std::endl;
+ // std::cout << "Warning: Problem with CV_CAP_PROP_FRAME_COUNT. We set
+ // video last frame to an arbitrary value (1000)." << std::endl;
lastFrame = 100000; // Set lastFrame to an arbitrary value
}
- lastFrame--; //Last frame index = total frame count - 1
}
#endif
}
-/*!
-Get the first frame index (update the firstFrame attribute).
-*/
-void
- vpVideoReader::findFirstFrameIndex()
-{
- if (imSequence != NULL)
- {
- if (! firstFrameIndexIsSet) {
- char name[FILENAME_MAX];
- int image_number = 0;
- bool failed;
- do {
- std::fstream file;
- sprintf(name, fileName, image_number) ;
- file.open(name, std::ios::in);
- failed = file.fail();
- if (!failed) file.close();
- image_number++;
- } while(failed);
-
- firstFrame = image_number - 1;
- imSequence->setImageNumber(firstFrame);
- }
- }
-#ifdef VISP_HAVE_FFMPEG
- else if (ffmpeg != NULL) {
- if (! firstFrameIndexIsSet) {
- firstFrame = (long)(0);
- }
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- else if (! firstFrameIndexIsSet)
- {
- firstFrame = (long) (0);
- }
-#endif
-}
/*!
-Return the framerate in Hz used to encode the video stream.
-
-If the video is a sequence of images, return -1.
+Get the first frame index (update the firstFrame attribute).
*/
-double vpVideoReader::getFramerate()
+void vpVideoReader::findFirstFrameIndex()
{
- double framerate = -1.;
-
-#ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- framerate = ffmpeg->getFramerate();
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- framerate = capture.get(cv::CAP_PROP_FPS);
- // if(framerate == 0)
- if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("Problem with cv::CAP_PROP_FPS") ;
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- framerate = capture.get(CV_CAP_PROP_FPS);
- // if(framerate == 0)
- if(std::fabs(framerate) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("Problem with CV_CAP_PROP_FPS") ;
- }
+ if (imSequence != NULL) {
+ if (!firstFrameIndexIsSet) {
+ std::string imageNameFormat = vpIoTools::getName(std::string(fileName));
+ std::string dirName = vpIoTools::getParent(std::string(fileName));
+ if (dirName == "") {
+ dirName = ".";
+ }
+ std::vector<std::string> files = vpIoTools::getDirFiles(dirName);
+ firstFrame = -1;
+ for (size_t i = 0; i < files.size(); i++) {
+ // 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)) {
+ firstFrame = imageIndex;
+ }
+ }
+ imSequence->setImageNumber(firstFrame);
+ }
+ }
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ else if (!firstFrameIndexIsSet) {
+ firstFrame = 1L;
+ }
#endif
- return framerate;
}
/*!
@@ -749,15 +746,9 @@ 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 ||
+ 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);
}
@@ -766,14 +757,9 @@ Return true if the video file extension is supported, false otherwise.
*/
bool vpVideoReader::isVideoExtensionSupported()
{
- return (formatType == FORMAT_AVI ||
- formatType == FORMAT_MPEG ||
- formatType == FORMAT_MPEG4 ||
- formatType == FORMAT_MOV ||
- formatType == FORMAT_OGV ||
- formatType == FORMAT_WMV ||
- formatType == FORMAT_FLV ||
- formatType == FORMAT_MKV);
+ 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);
}
/*!
@@ -796,7 +782,7 @@ int main()
reader.open(I);
while (! reader.end() )
- reader >> I;
+ reader >> I;
}
\endcode
*/
@@ -826,7 +812,7 @@ int main()
reader.open(I);
while (! reader.end() )
- reader >> I;
+ reader >> I;
}
\endcode
*/
@@ -835,3 +821,61 @@ vpVideoReader &vpVideoReader::operator>>(vpImage<vpRGBa> &I)
this->acquire(I);
return *this;
}
+
+/*!
+ Checks imageName format and extracts its index.
+
+ Format must contain substring "%0xd", defining the length of image index.
+ For example, format can be "img%04d.jpg". Then "img0001.jpg" and
+ "img0000.jpg" satisfy it, while "picture001.jpg" and "img001.jpg" don't.
+
+ \param imageName : name from which to extract
+ \param format : format of image name
+ \return extracted index on success, -1 otherwise.
+*/
+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);
+ size_t suffixLength = format.length() - indexEnd - 1;
+
+ // Extracting index
+ if (imageName.length() <= suffixLength + indexBegin) {
+ return -1;
+ }
+ size_t indexLength = imageName.length() - suffixLength - indexBegin;
+ std::string indexSubstr = imageName.substr(indexBegin, indexLength);
+ std::istringstream ss(indexSubstr);
+ long index = 0;
+ ss >> index;
+ if (ss.fail() || index < 0 || !ss.eof()) {
+ return -1;
+ }
+
+ // Checking that format with inserted index equals imageName
+ char nameByFormat[FILENAME_MAX];
+ sprintf(nameByFormat, format.c_str(), index);
+ if (std::string(nameByFormat) != imageName) {
+ return -1;
+ }
+ return index;
+}
+
+/*!
+ Checks image name template, for example "img%04d.jpg"
+ \return true if it is correct, false otherwise
+*/
+bool vpVideoReader::checkImageNameFormat(const std::string &format)
+{
+ size_t indexBegin = format.find_last_of('%');
+ size_t indexEnd = format.find_first_of('d', indexBegin);
+ if (indexBegin == std::string::npos || indexEnd == std::string::npos) {
+ return false;
+ }
+ for (size_t i = indexBegin + 1; i < indexEnd; i++) {
+ if (!std::isdigit(format[i])) {
+ return false;
+ }
+ }
+ return true;
+}
diff --git a/modules/io/src/video/vpVideoWriter.cpp b/modules/io/src/video/vpVideoWriter.cpp
index 8b2b0a8..e081b33 100644
--- a/modules/io/src/video/vpVideoWriter.cpp
+++ b/modules/io/src/video/vpVideoWriter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,287 +46,211 @@
#include <visp3/io/vpVideoWriter.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020200
-# include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
#endif
-
/*!
Basic constructor.
*/
-vpVideoWriter::vpVideoWriter() :
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg(NULL),
-# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- codec(CODEC_ID_MPEG1VIDEO),
-# else
- codec(AV_CODEC_ID_MPEG1VIDEO),
-# endif
- bit_rate(500000),
- framerate(25),
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+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;
frameCount = 0;
isOpen = false;
width = height = 0;
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = NULL;
-# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- codec = CODEC_ID_MPEG1VIDEO;
-# else
- codec = AV_CODEC_ID_MPEG1VIDEO;
-# endif
- bit_rate = 500000;
- framerate = 25;
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
framerate = 25.0;
- fourcc = cv::VideoWriter::fourcc('P','I','M','1');
+ fourcc = cv::VideoWriter::fourcc('P', 'I', 'M', '1');
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
framerate = 25.0;
- fourcc = CV_FOURCC('P','I','M','1'); // default is a MPEG-1 codec
+ fourcc = CV_FOURCC('P', 'I', 'M', '1'); // default is a MPEG-1 codec
#endif
}
-
/*!
Basic destructor.
*/
-vpVideoWriter::~vpVideoWriter()
-{
- #ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- delete ffmpeg;
- #endif
-}
-
+vpVideoWriter::~vpVideoWriter() {}
/*!
It enables to set the path and the name of the files which will be saved.
-
- If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
-
+
+ If you want to write a sequence of images, \f$ filename \f$ corresponds to
+ the path followed by the image name template. For exemple, if you want to
+ write different images named image0001.jpeg, image0002.jpg, ... and located
+ in the folder /local/image, \f$ filename \f$ will be
+ "/local/image/image%04d.jpg".
+
\param filename : filename template of an image sequence.
*/
void vpVideoWriter::setFileName(const char *filename)
{
- if (!filename || *filename == '\0')
- {
- vpERROR_TRACE("filename empty ") ;
- throw (vpImageException(vpImageException::noFileNameError,"filename empty ")) ;
+ if (!filename || *filename == '\0') {
+ vpERROR_TRACE("filename empty ");
+ throw(vpImageException(vpImageException::noFileNameError, "filename empty "));
}
- if (strlen( filename ) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the file name"));
+ if (strlen(filename) >= FILENAME_MAX) {
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the file name"));
}
- strcpy(this->fileName,filename);
-
+ strcpy(this->fileName, filename);
+
formatType = getFormat(fileName);
if (formatType == FORMAT_UNKNOWN) {
throw(vpException(vpException::badValue, "Filename extension not supported"));
}
-
+
initFileName = true;
}
/*!
It enables to set the path and the name of the files which will be saved.
- If you want to write a sequence of images, \f$ filename \f$ corresponds to the path followed by the image name template. For exemple, if you want to write different images named image0001.jpeg, image0002.jpg, ... and located in the folder /local/image, \f$ filename \f$ will be "/local/image/image%04d.jpg".
+ If you want to write a sequence of images, \f$ filename \f$ corresponds to
+ the path followed by the image name template. For exemple, if you want to
+ write different images named image0001.jpeg, image0002.jpg, ... and located
+ in the folder /local/image, \f$ filename \f$ will be
+ "/local/image/image%04d.jpg".
\param filename : filename template of an image sequence.
*/
-void vpVideoWriter::setFileName(const std::string &filename)
-{
- setFileName(filename.c_str());
-}
+void vpVideoWriter::setFileName(const std::string &filename) { setFileName(filename.c_str()); }
/*!
Sets all the parameters needed to write the video or the image sequence.
-
+
\param I : One image with the right dimensions.
*/
-void vpVideoWriter::open(vpImage< vpRGBa > &I)
+void vpVideoWriter::open(vpImage<vpRGBa> &I)
{
- if (!initFileName)
- {
+ 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)
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- ffmpeg->setFramerate(framerate);
- ffmpeg->setBitRate(bit_rate);
- if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
- throw (vpException(vpException::ioError ,"Could not open encode the video with ffmpeg"));
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ } 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()));
- 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"));
+ if (!writer.isOpened()) {
+ // vpERROR_TRACE("Could not open encode the video with opencv");
+ throw(vpException(vpException::fatalError, "Could not open encode the video with opencv"));
}
#else
- //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+ throw(vpException(vpException::fatalError, "To encode video files ViSP should be build with "
+ "opencv 3rd >= 2.1.0 party libraries."));
#endif
}
-
+
frameCount = firstFrame;
-
+
isOpen = true;
}
-
/*!
Sets all the parameters needed to write the video or the image sequence.
-
+
\param I : One image with the right dimensions.
*/
-void vpVideoWriter::open(vpImage< unsigned char > &I)
+void vpVideoWriter::open(vpImage<unsigned char> &I)
{
- if (!initFileName)
- {
+ 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)
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg = new vpFFMPEG;
- ffmpeg->setFramerate(framerate);
- ffmpeg->setBitRate(bit_rate);
- if(!ffmpeg->openEncoder(fileName, I.getWidth(), I.getHeight(), codec)) {
- throw (vpException(vpException::ioError ,"Could not encode the video with ffmpeg"));
- }
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
+ } 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()));
- if(!writer.isOpened())
- {
+ 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
- //vpERROR_TRACE("To encode video files ViSP should be build with ffmpeg or opencv 3rd party libraries.");
- throw (vpException(vpException::fatalError ,"To encode video files ViSP should be build with ffmpeg or opencv 3rd >= 2.1.0 party libraries."));
+ throw(vpException(vpException::fatalError, "To encode video files ViSP should be build with "
+ "opencv 3rd >= 2.1.0 party libraries."));
#endif
}
-
+
frameCount = firstFrame;
-
+
isOpen = true;
}
-
/*!
- Saves the image as a frame of the video or as an image belonging to the image sequence.
-
- Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
-
+ Saves the image as a frame of the video or as an image belonging to the
+ image sequence.
+
+ Each time this method is used, the frame counter is incremented and thus the
+ file name change for the case of an image sequence.
+
\param I : The image which has to be saved
*/
-void vpVideoWriter::saveFrame (vpImage< vpRGBa > &I)
+void vpVideoWriter::saveFrame(vpImage<vpRGBa> &I)
{
- if (!isOpen)
- {
+ if (!isOpen) {
vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
+ 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);
+ sprintf(name, fileName, frameCount);
vpImageIo::write(I, name);
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg->saveFrame(I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- cv::Mat matFrame;
- vpImageConvert::convert(I, matFrame);
- writer << matFrame;
+ } else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x020100
+ cv::Mat matFrame;
+ vpImageConvert::convert(I, matFrame);
+ writer << matFrame;
#endif
}
frameCount++;
}
-
/*!
- Saves the image as a frame of the video or as an image belonging to the image sequence.
-
- Each time this method is used, the frame counter is incremented and thus the file name change for the case of an image sequence.
-
+ Saves the image as a frame of the video or as an image belonging to the
+ image sequence.
+
+ Each time this method is used, the frame counter is incremented and thus the
+ file name change for the case of an image sequence.
+
\param I : The image which has to be saved
*/
-void vpVideoWriter::saveFrame (vpImage< unsigned char > &I)
+void vpVideoWriter::saveFrame(vpImage<unsigned char> &I)
{
- if (!isOpen)
- {
+ if (!isOpen) {
vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
+ 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);
+ sprintf(name, fileName, frameCount);
vpImageIo::write(I, name);
- }
- else
- {
-#ifdef VISP_HAVE_FFMPEG
- ffmpeg->saveFrame(I);
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
+ } else {
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
cv::Mat matFrame, rgbMatFrame;
vpImageConvert::convert(I, matFrame);
cv::cvtColor(matFrame, rgbMatFrame, cv::COLOR_GRAY2BGR);
@@ -341,33 +266,23 @@ void vpVideoWriter::saveFrame (vpImage< unsigned char > &I)
frameCount++;
}
-
/*!
Deallocates parameters use to write the video or the image sequence.
*/
void vpVideoWriter::close()
{
- if (!isOpen)
- {
+ if (!isOpen) {
vpERROR_TRACE("The video has to be open first with the open method");
- throw (vpException(vpException::notInitialized,"file not yet opened"));
- }
- #ifdef VISP_HAVE_FFMPEG
- if (ffmpeg != NULL)
- {
- ffmpeg->endWrite();
+ throw(vpException(vpException::notInitialized, "file not yet opened"));
}
- #endif
}
-
/*!
Gets the format of the file(s) which has/have to be written.
-
+
\return Returns the format.
*/
-vpVideoWriter::vpVideoFormatType
-vpVideoWriter::getFormat(const char *filename)
+vpVideoWriter::vpVideoFormatType vpVideoWriter::getFormat(const char *filename)
{
std::string sfilename(filename);
@@ -426,6 +341,6 @@ std::string vpVideoWriter::getExtension(const std::string &filename)
{
// extract the extension
size_t dot = filename.find_last_of(".");
- std::string ext = filename.substr(dot, filename.size()-1);
+ std::string ext = filename.substr(dot, filename.size() - 1);
return ext;
}
diff --git a/modules/robot/CMakeLists.txt b/modules/robot/CMakeLists.txt
index fcc3cc3..87f33fd 100644
--- a/modules/robot/CMakeLists.txt
+++ b/modules/robot/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -62,13 +63,13 @@ if(USE_VIRTUOSE)
set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /ignore:4099")
# Disable Visual C++ C4312 warning occuring on 3rd party VirtuoseAPI.h file
# VirtuoseAPI.h(280): warning C4312: 'type cast': conversion from 'int' to 'char *' of greater size
- # VirtuoseAPI.h(302): warning C4312: 'type cast': conversion from 'int' to 'VirtContext' of greater size
- vp_set_source_file_compile_flag(src/haptic-device/virtuose/vpVirtuose.cpp /wd4312)
- vp_set_source_file_compile_flag(test/virtuose/testVirtuose.cpp /wd4312)
- vp_set_source_file_compile_flag(test/virtuose/testVirtuoseAfma6.cpp /wd4312)
- vp_set_source_file_compile_flag(test/virtuose/testVirtuoseHapticBox.cpp /wd4312)
- vp_set_source_file_compile_flag(test/virtuose/testVirtuoseJointLimits.cpp /wd4312)
- vp_set_source_file_compile_flag(test/virtuose/testVirtuosePeriodicFunction.cpp /wd4312)
+ # VirtuoseAPI.h(302): warning C4312: 'type cast': conversion from 'int' to 'VirtContext' of greater size
+ vp_set_source_file_compile_flag(src/haptic-device/virtuose/vpVirtuose.cpp /wd"4312")
+ vp_set_source_file_compile_flag(test/virtuose/testVirtuose.cpp /wd"4312")
+ vp_set_source_file_compile_flag(test/virtuose/testVirtuoseAfma6.cpp /wd"4312")
+ vp_set_source_file_compile_flag(test/virtuose/testVirtuoseHapticBox.cpp /wd"4312")
+ vp_set_source_file_compile_flag(test/virtuose/testVirtuoseJointLimits.cpp /wd"4312")
+ vp_set_source_file_compile_flag(test/virtuose/testVirtuosePeriodicFunction.cpp /wd"4312")
endif()
endif()
@@ -105,6 +106,21 @@ elseif(USE_ARIA AND NOT UNIX)
list(APPEND opt_libs ${ARIA_LIBRARIES})
endif()
+if(USE_FRANKA)
+ get_target_property(Franka_inc_dirs_ ${Franka_LIBRARIES} INTERFACE_INCLUDE_DIRECTORIES)
+ list(APPEND opt_incs ${Franka_inc_dirs_})
+
+ set(config_ "release" "debug" "releasewithdebinfo" "relwithdebinfo" "none")
+ foreach(imp_config_ ${config_})
+ string(TOUPPER "${imp_config_}" imp_config_up_)
+ get_target_property(Franka_lib ${Franka_LIBRARIES} IMPORTED_LOCATION_${imp_config_up_})
+ if(Franka_lib)
+ list(APPEND opt_libs ${Franka_lib})
+ break()
+ endif()
+ endforeach()
+endif()
+
# for the wireframe simulator we can use optional opengl and coin
if(USE_COIN3D)
if(WIN32)
@@ -134,7 +150,7 @@ endif()
if(USE_ARIA)
# Add specific build flag to turn off warnings coming from libaria 3rd party
- vp_set_source_file_compile_flag(src/real-robot/pioneer/vpRobotPioneer.cpp -Wno-unused-parameter)
+ vp_set_source_file_compile_flag(src/real-robot/pioneer/vpRobotPioneer.cpp -Wno-unused-parameter -Wno-type-limits)
endif()
if(USE_COIN3D)
diff --git a/modules/robot/include/visp3/robot/vpAfma4.h b/modules/robot/include/visp3/robot/vpAfma4.h
index cbe837c..276d4ec 100644
--- a/modules/robot/include/visp3/robot/vpAfma4.h
+++ b/modules/robot/include/visp3/robot/vpAfma4.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -77,12 +78,14 @@
\f]
The forward kinematics of the robot is given by the homogeneous
- matrix \f${^f}M_e\f$ which is implemented in get_fMe().
+ matrix \f${^f}M_e\f$ which is implemented in get_fMe().
\f[
{^f}M_e = \left[\begin{array}{cccc}
- c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
- s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
+ c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1
+ \\
+ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1
+ \\
-s_5 & -c_5 & d_4+q_2 \\
0 & 0 & 0 & 1 \\
\end{array}
@@ -98,59 +101,57 @@
*/
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-
class VISP_EXPORT vpAfma4
{
- public:
+public:
vpAfma4();
/*! Destructor that does nothing. */
- virtual ~vpAfma4() {};
+ virtual ~vpAfma4(){};
/** @name Inherited functionalities from vpAfma4 */
//@{
- void init (void);
+ void init(void);
- vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
-/* int getInverseKinematics(const vpHomogeneousMatrix & fMc, */
-/* vpColVector & q, const bool &nearest=true); */
- vpHomogeneousMatrix get_fMc (const vpColVector & q) const;
- void get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const;
- void get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const;
+ vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const;
+ /* int getInverseKinematics(const vpHomogeneousMatrix & fMc, */
+ /* vpColVector & q, const bool &nearest=true); */
+ vpHomogeneousMatrix get_fMc(const vpColVector &q) const;
+ void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const;
+ void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
void get_cMe(vpHomogeneousMatrix &cMe) const;
void get_cVe(vpVelocityTwistMatrix &cVe) const;
- void get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const;
+ void get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const;
void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
void get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const;
- friend VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpAfma4 & afma4);
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4);
vpColVector getJointMin() const;
vpColVector getJointMax() const;
//@}
- public:
-
+public:
static const unsigned int njoint; ///< Number of joint.
- protected:
+protected:
// Denavit Hartenberg parameters
- double _a1; // distance along x2
- double _d3; // distance along z2
- double _d4; // distance along z3
+ double _a1; // distance along x2
+ double _d3; // distance along z2
+ double _d4; // distance along z3
double _joint_max[4]; // Maximal value of the joints
double _joint_min[4]; // Minimal value of the joints
// Minimal representation of _eMc
vpTranslationVector _etc; // meters
- vpRxyzVector _erc; // radian
+ vpRxyzVector _erc; // radian
vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
};
@@ -162,4 +163,3 @@ class VISP_EXPORT vpAfma4
*/
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpAfma6.h b/modules/robot/include/visp3/robot/vpAfma6.h
index 9672ea6..733ee88 100644
--- a/modules/robot/include/visp3/robot/vpAfma6.h
+++ b/modules/robot/include/visp3/robot/vpAfma6.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -58,28 +59,29 @@
- \f$ {\cal F}_f \f$: the reference frame, also called world frame
- - \f$ {\cal F}_e \f$: the end-effector frame located at the intersection of the 3 rotations.
+ - \f$ {\cal F}_e \f$: the end-effector frame located at the intersection of
+ the 3 rotations.
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool TOOL_CUSTOM and set this
- tool during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool TOOL_CUSTOM and
+ set this tool during robot initialisation or using set_eMc().
*/
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
class VISP_EXPORT vpAfma6
{
- public:
+public:
#ifdef VISP_HAVE_AFMA6_DATA
//! File where constant parameters in relation with the robot are stored:
- //! joint max, min, coupling factor between 4 ant 5 joint, distance between 5
- //! and 6 joint, tranformation eMc between end-effector and camera frame.
+ //! joint max, min, coupling factor between 4 ant 5 joint, distance between
+ //! 5 and 6 joint, tranformation eMc between end-effector and camera frame.
static const std::string CONST_AFMA6_FILENAME;
static const std::string CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
static const std::string CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
@@ -92,62 +94,62 @@ class VISP_EXPORT vpAfma6
static const std::string CONST_CAMERA_AFMA6_FILENAME;
#endif
/*!
- Name of the camera attached to the CCMOP tool (vpAfma6ToolType::TOOL_CCMOP).
+ Name of the camera attached to the CCMOP tool
+ (vpAfma6ToolType::TOOL_CCMOP).
*/
- static const char * const CONST_CCMOP_CAMERA_NAME;
+ static const char *const CONST_CCMOP_CAMERA_NAME;
/*!
Name of the camera attached to the 2 fingers gripper tool
(vpAfma6ToolType::TOOL_GRIPPER).
*/
- static const char * const CONST_GRIPPER_CAMERA_NAME;
+ static const char *const CONST_GRIPPER_CAMERA_NAME;
/*!
Name of the camera attached to the vacuum gripper tool
(vpAfma6ToolType::TOOL_VACUUM).
*/
- static const char * const CONST_VACUUM_CAMERA_NAME;
+ static const char *const CONST_VACUUM_CAMERA_NAME;
/*!
Name of the generic camera attached to the robot hand
(vpAfma6ToolType::TOOL_GENERIC_CAMERA).
*/
- static const char * const CONST_GENERIC_CAMERA_NAME;
+ static const char *const CONST_GENERIC_CAMERA_NAME;
//! List of possible tools that can be attached to the robot end-effector.
- typedef enum
- {
- TOOL_CCMOP, /*!< Pneumatic CCMOP gripper. */
- TOOL_GRIPPER, /*!< Pneumatic gripper with 2 fingers. */
- TOOL_VACUUM, /*!< Pneumatic vaccum gripper. */
- TOOL_GENERIC_CAMERA, /*!< A generic camera. */
- TOOL_CUSTOM /*!< A user defined tool. */
- } vpAfma6ToolType;
+ typedef enum {
+ TOOL_CCMOP, /*!< Pneumatic CCMOP gripper. */
+ TOOL_GRIPPER, /*!< Pneumatic gripper with 2 fingers. */
+ TOOL_VACUUM, /*!< Pneumatic vaccum gripper. */
+ TOOL_GENERIC_CAMERA, /*!< A generic camera. */
+ TOOL_CUSTOM /*!< A user defined tool. */
+ } vpAfma6ToolType;
//! Default tool attached to the robot end effector
static const vpAfma6ToolType defaultTool;
- public:
+public:
vpAfma6();
/*! Destructor that does nothing. */
- virtual ~vpAfma6() {};
+ virtual ~vpAfma6(){};
/** @name Inherited functionalities from vpAfma6 */
//@{
void init(void);
- void init (const std::string &camera_extrinsic_parameters);
+ void init(const std::string &camera_extrinsic_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,
- const bool &verbose=false) const;
+ vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const;
+ int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest = true,
+ const bool &verbose = false) const;
vpHomogeneousMatrix get_eMc() const;
- vpHomogeneousMatrix get_fMc(const vpColVector & q) const;
- void get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const;
- void get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const;
+ vpHomogeneousMatrix get_fMc(const vpColVector &q) const;
+ void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const;
+ void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
void get_cMe(vpHomogeneousMatrix &cMe) const;
void get_cVe(vpVelocityTwistMatrix &cVe) const;
@@ -155,19 +157,13 @@ class VISP_EXPORT vpAfma6
void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
//! Get the current tool type
- vpAfma6ToolType getToolType() const {
- return tool_current;
- };
+ vpAfma6ToolType getToolType() const { return tool_current; };
//! Get the current camera model projection type
- vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const{
- return projModel;
- };
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const { return projModel; };
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
+ void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const;
void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
vpColVector getJointMin() const;
@@ -175,45 +171,39 @@ class VISP_EXPORT vpAfma6
double getCoupl56() const;
double getLong56() const;
- void parseConfigFile (const std::string &filename);
+ void parseConfigFile(const std::string &filename);
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:
+protected:
/** @name Protected Member Functions Inherited from vpAfma6 */
//@{
//! Set the current tool type
- void setToolType(vpAfma6::vpAfma6ToolType tool){
- tool_current = tool;
- };
+ void setToolType(vpAfma6::vpAfma6ToolType tool) { tool_current = tool; };
//@}
- public:
-
+public:
static const unsigned int njoint; ///< Number of joint.
-
- protected:
- double _coupl_56; // coupling between join 5 and 6
- double _long_56; // distance between join 5 and 6
+protected:
+ double _coupl_56; // coupling between join 5 and 6
+ double _long_56; // distance between join 5 and 6
double _joint_max[6]; // Maximal value of the joints
double _joint_min[6]; // Minimal value of the joints
// Minimal representation of _eMc
vpTranslationVector _etc; // meters
- vpRxyzVector _erc; // radian
+ vpRxyzVector _erc; // radian
vpHomogeneousMatrix _eMc; // Camera extrinsic parameters: effector to camera
- protected:
+protected:
//! Current tool in use
vpAfma6ToolType tool_current;
// Used projection model
vpCameraParameters::vpCameraParametersProjType projModel;
-
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpBiclops.h b/modules/robot/include/visp3/robot/vpBiclops.h
index 739d5a8..54c8e02 100644
--- a/modules/robot/include/visp3/robot/vpBiclops.h
+++ b/modules/robot/include/visp3/robot/vpBiclops.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,10 +44,10 @@
/* --------------------------------------------------------------------- */
/* --- ViSP --- */
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpPoseVector.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpPoseVector.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
@@ -63,11 +64,13 @@
\brief Jacobian, geometric model functionnalities... for biclops, pan, tilt
head.
- Two different Denavit Hartenberg representations of the robot are implemented.
- As mentionned in vpBiclops::DenavitHartenbergModel they differ in the orientation
- of the tilt axis. Use setDenavitHartenbergModel() to select the representation.
+ Two different Denavit Hartenberg representations of the robot are
+ implemented. As mentionned in vpBiclops::DenavitHartenbergModel they differ
+ in the orientation of the tilt axis. Use setDenavitHartenbergModel() to
+ select the representation.
- See http://www.traclabs.com/tracbiclops.htm for more details concerning the hardware.
+ See http://www.traclabs.com/tracbiclops.htm for more details concerning the
+ hardware.
*/
@@ -75,8 +78,8 @@ class VISP_EXPORT vpBiclops
{
public:
/*!
- Two different Denavit Hartenberg representations of the robot are implemented.
- They differ in the orientation of the tilt axis.
+ Two different Denavit Hartenberg representations of the robot are
+ implemented. They differ in the orientation of the tilt axis.
The first representation, vpBiclops::DH1 is given by:
@@ -104,23 +107,23 @@ public:
\end{tabular}
\f]
- where \f$q_1^*, q_2^*\f$ are respectively the pan and tilt joint positions.
+ where \f$q_1^*, q_2^*\f$ are respectively the pan and tilt joint
+ positions.
- In those representations, the pan is oriented from left to right, while the tilt is oriented
+ In those representations, the pan is oriented from left to right, while
+ the tilt is oriented
- in vpBiclops::DH1 from down to top,
- in vpBiclops::DH2 from top to down.
*/
- typedef enum
- {
+ typedef enum {
DH1, /*!< First Denavit Hartenberg representation. */
DH2 /*!< Second Denavit Hartenberg representation. */
} DenavitHartenbergModel;
-public: /* Constants */
-
- static const unsigned int ndof; /*!< Number of dof */
+public: /* Constants */
+ static const unsigned int ndof; /*!< Number of dof */
/* Geometric model */
static const float h;
@@ -129,39 +132,35 @@ public: /* Constants */
static const float tiltJointLimit;
static const float speedLimit;
-
protected:
DenavitHartenbergModel dh_model_;
vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
public:
- vpBiclops (void);
+ vpBiclops(void);
/*! Destructor that does nothing. */
- virtual ~vpBiclops() {};
+ virtual ~vpBiclops(){};
/** @name Inherited functionalities from vpBiclops */
//@{
- void init (void);
+ void init(void);
- void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
- vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
- void computeMGD (const vpColVector &q, vpPoseVector &fvc) const;
+ void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
+ vpHomogeneousMatrix computeMGD(const vpColVector &q) const;
+ void computeMGD(const vpColVector &q, vpPoseVector &fvc) const;
/*!
- Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame and the end
- effector frame.
+ Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame and
+ the end effector frame.
*/
- vpHomogeneousMatrix get_cMe() const
- {
- return cMe_;
- }
+ vpHomogeneousMatrix get_cMe() const { return cMe_; }
void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const;
- void get_fMc (const vpColVector &q, vpPoseVector &fvc) const;
- vpHomogeneousMatrix get_fMc (const vpColVector &q) const;
- vpHomogeneousMatrix get_fMe (const vpColVector &q) const;
+ void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
+ void get_fMc(const vpColVector &q, vpPoseVector &fvc) const;
+ vpHomogeneousMatrix get_fMc(const vpColVector &q) const;
+ vpHomogeneousMatrix get_fMe(const vpColVector &q) const;
void get_eJe(const vpColVector &q, vpMatrix &eJe) const;
void get_fJe(const vpColVector &q, vpMatrix &fJe) const;
@@ -170,32 +169,23 @@ public:
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() ;
+ void set_cMe();
/*!
- Set the transformation between the camera frame and the end effector frame.
+ Set the transformation between the camera frame and the end effector
+ frame.
*/
- void set_cMe(const vpHomogeneousMatrix &cMe)
- {
- cMe_ = cMe;
- }
+ void set_cMe(const vpHomogeneousMatrix &cMe) { cMe_ = cMe; }
/*!
Set the Denavit Hartenberg representation used to model the head.
\sa vpBiclops::DenavitHartenbergModel
*/
- inline void setDenavitHartenbergModel(vpBiclops::DenavitHartenbergModel m=vpBiclops::DH1)
- {
- dh_model_ = m;
- }
+ 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 674d9ec..5e1e1f9 100644
--- a/modules/robot/include/visp3/robot/vpImageSimulator.h
+++ b/modules/robot/include/visp3/robot/vpImageSimulator.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -27,9 +28,9 @@
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
- * Description: Class which enables to project an image in the 3D space
+ * Description: Class which enables to project an image in the 3D space
* and get the view of a virtual camera.
- *
+ *
*
* Authors:
* Amaury Dame
@@ -45,29 +46,37 @@
\ingroup group_robot_image_simu
- \brief Class which enables to project an image in the 3D space and get the view of a virtual camera.
-
- The image is represented by a rectangular image whose corners coordinates are known in the 3D frame linked to the 3D rectangle.
-
- The 3D rectangle is positionned relative to a virtual camera (represented by its intrinsic parameters). Indeed, the pose \f$ cMt \f$ has to be given by the user of the class.
-
- And finally, the view of the virtual camera is given by the geImage() method.
-
+ \brief Class which enables to project an image in the 3D space and get the
+ view of a virtual camera.
+
+ The image is represented by a rectangular image whose corners coordinates
+ are known in the 3D frame linked to the 3D rectangle.
+
+ The 3D rectangle is positionned relative to a virtual camera (represented by
+ its intrinsic parameters). Indeed, the pose \f$ cMt \f$ has to be given by
+ the user of the class.
+
+ And finally, the view of the virtual camera is given by the geImage()
+ method.
+
You can use a colored or a gray scaled image.
-
- To avoid the aliasing especially when the camera is very near from the image plane, a bilinear interpolation can be done for every pixels which have to be filled in. By default this functionality is not used because it consumes lot of time.
-
+
+ To avoid the aliasing especially when the camera is very near from the image
+ plane, a bilinear interpolation can be done for every pixels which have to
+ be filled in. By default this functionality is not used because it consumes
+ lot of time.
+
The following example explain how to use the class.
-
+
\code
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpImageSimulator.h>
-
+
int main()
{
vpImage<vpRGBa> Icamera(480,640,0);
vpImage<vpRGBa> Iimage(60,60);
-
+
// Initialise the image which will be projected into the image Icamera
vpRGBa colorb(0,0,255);
vpRGBa colorw(255,255,255);
@@ -81,7 +90,7 @@
for(int j = 40; j < 60; j++)
Iimage[i][j] = colorr;
}
-
+
// Initialise the 3D coordinates of the Iimage corners
vpColVector X[4];
for (int i = 0; i < 4; i++) X[i].resize(3);
@@ -89,218 +98,211 @@
X[0][0] = -1;
X[0][1] = -1;
X[0][2] = 0;
-
+
// Top right corner
X[1][0] = 1;
X[1][1] = -1;
X[1][2] = 0;
-
+
// Bottom right corner
X[2][0] = 1;
X[2][1] = 1;
X[2][2] = 0;
-
+
//Bottom left corner
X[3][0] = -1;
X[3][1] = 1;
X[3][2] = 0;
-
+
vpImageSimulator sim;
sim.init(Iimage, X);
-
+
sim.setCameraPosition(vpHomogeneousMatrix(0,0,5,vpMath::rad(60),vpMath::rad(0),0));
-
+
vpCameraParameters cam(868.0, 869.0, 320, 240);
-
+
sim.getImage(Icamera,cam);
-
+
return 0;
}
\endcode
*/
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTriangle.h>
-#include <visp3/core/vpRect.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpTriangle.h>
-#include <vector>
#include <list>
+#include <vector>
class VISP_EXPORT vpImageSimulator
{
- public:
- typedef enum {
- COLORED,
- GRAY_SCALED
- } vpColorPlan;
-
- typedef enum {
- SIMPLE,
- BILINEAR_INTERPOLATION
- } vpInterpolationType;
-
- private:
- vpColVector X[4];
- vpHomogeneousMatrix cMt;
- vpColVector X2[4];
- std::vector<vpPoint> pt;
- std::vector<vpPoint> ptClipped;
-
- vpInterpolationType interp;
-
- //normal repere objet
- vpColVector normal_obj;
- //normal repere cam
- vpColVector normal_Cam;
- //normal repere cam sous forme de pointeur sinon vpColVector prend trop de temps
- double *normal_Cam_optim;
-
- //distance du plan au centre de camera
- double distance;
- double visible_result;
- bool visible;
-
-
- //point 3D du plan definissant ou sera le pixel (0,0) de l'image
- double *X0_2_optim;
-
- //variable pour intersection algebre : vecteurs de la base 2D et leur norme
- double euclideanNorm_u,euclideanNorm_v;
-
- //2 vecteur 3D (typiquement deux cotes du plan) qui definissent le repere 2D u,v de l'image
- vpColVector vbase_u,vbase_v;
- //version pointeur des vecteurs de base
- double *vbase_u_optim;
- double *vbase_v_optim;
-
- //allocation memoire du point 3D d'intersection entre le vecteur (centre_cam - point_plan_image) et le plan
- double *Xinter_optim;
-
- //triangles de projection du plan
- std::vector<vpTriangle> listTriangle;
-
- //image de texture
- vpColorPlan colorI;
- vpImage<unsigned char> Ig;
- vpImage<vpRGBa> Ic;
-
- vpRect rect;
- bool cleanPrevImage;
- bool setBackgroundTexture; // flag set when the background is to a texture using setBackGroundTexture()
- vpColor bgColor;
-
- vpColVector focal;
-
- //boolean to tell if the points in the camera frame have to be clipped
- bool needClipping;
-
- public:
- vpImageSimulator(const vpColorPlan &col = COLORED);
- vpImageSimulator(const vpImageSimulator &text);
- virtual ~vpImageSimulator();
-
- vpImageSimulator& operator=(const vpImageSimulator& sim);
-
-
- //creation du plan a partir de ses coordonnees 3D ds repere objet et de son image texture
- void init(const vpImage<unsigned char> &I,vpColVector* X);
- void init(const vpImage<vpRGBa> &I,vpColVector* X);
+public:
+ typedef enum { COLORED, GRAY_SCALED } vpColorPlan;
+
+ typedef enum { SIMPLE, BILINEAR_INTERPOLATION } vpInterpolationType;
+
+private:
+ vpColVector X[4];
+ vpHomogeneousMatrix cMt;
+ vpColVector X2[4];
+ std::vector<vpPoint> pt;
+ std::vector<vpPoint> ptClipped;
+
+ vpInterpolationType interp;
+
+ // normal repere objet
+ vpColVector normal_obj;
+ // normal repere cam
+ vpColVector normal_Cam;
+ // normal repere cam sous forme de pointeur sinon vpColVector prend trop de
+ // temps
+ double *normal_Cam_optim;
+
+ // distance du plan au centre de camera
+ double distance;
+ double visible_result;
+ bool visible;
+
+ // point 3D du plan definissant ou sera le pixel (0,0) de l'image
+ double *X0_2_optim;
+
+ // variable pour intersection algebre : vecteurs de la base 2D et leur norme
+ double euclideanNorm_u, euclideanNorm_v;
+
+ // 2 vecteur 3D (typiquement deux cotes du plan) qui definissent le repere
+ // 2D u,v de l'image
+ vpColVector vbase_u, vbase_v;
+ // version pointeur des vecteurs de base
+ double *vbase_u_optim;
+ double *vbase_v_optim;
+
+ // allocation memoire du point 3D d'intersection entre le vecteur
+ // (centre_cam - point_plan_image) et le plan
+ double *Xinter_optim;
+
+ // triangles de projection du plan
+ std::vector<vpTriangle> listTriangle;
+
+ // image de texture
+ vpColorPlan colorI;
+ vpImage<unsigned char> Ig;
+ vpImage<vpRGBa> Ic;
+
+ vpRect rect;
+ bool cleanPrevImage;
+ bool setBackgroundTexture; // flag set when the background is to a texture
+ // using setBackGroundTexture()
+ vpColor bgColor;
+
+ vpColVector focal;
+
+ // boolean to tell if the points in the camera frame have to be clipped
+ bool needClipping;
+
+public:
+ explicit vpImageSimulator(const vpColorPlan &col = COLORED);
+ vpImageSimulator(const vpImageSimulator &text);
+ virtual ~vpImageSimulator();
+
+ vpImageSimulator &operator=(const vpImageSimulator &sim);
+
+ // creation du plan a partir de ses coordonnees 3D ds repere objet et de son
+ // image texture
+ void init(const vpImage<unsigned char> &I, vpColVector *X);
+ void init(const vpImage<vpRGBa> &I, vpColVector *X);
#ifdef VISP_HAVE_MODULE_IO
- void init(const char* file_image,vpColVector* X);
+ void init(const char *file_image, vpColVector *X);
#endif
- void init(const vpImage<unsigned char> &I, const std::vector<vpPoint>& X);
- void init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X);
+ void init(const vpImage<unsigned char> &I, const std::vector<vpPoint> &X);
+ void init(const vpImage<vpRGBa> &I, const std::vector<vpPoint> &X);
#ifdef VISP_HAVE_MODULE_IO
- void init(const char* file_image, const std::vector<vpPoint>& X);
+ void init(const char *file_image, const std::vector<vpPoint> &X);
#endif
- //projection du plan par cMo => creation des deux triangles definissant projection du plan sur plan image (coord en metre)
- void setCameraPosition(const vpHomogeneousMatrix &cMt);
-
- void setInterpolationType (const vpInterpolationType interplt) {this->interp = interplt;}
-
- void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam);
- void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam);
-
- void getImage(vpImage<unsigned char> &I, vpImage<unsigned char> &Isrc,
- const vpCameraParameters &cam);
- void getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
- const vpCameraParameters &cam);
-
- void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer);
- void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer);
-
- static void getImage(vpImage<unsigned char> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam);
- static void getImage(vpImage<vpRGBa> &I,
- std::list <vpImageSimulator> &list,
- const vpCameraParameters &cam);
-
- std::vector<vpColVector> get3DcornersTextureRectangle();
-
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/);
-
- /*!
- As it can be time consuming to reset all the image to a default baground value, this function enable to reset only the pixel which changed the previous time.
-
- By default this functionality is disabled. and the background color is white.
-
- \param clean : Enable the reset method.
- \param color : Color of the back ground.
- */
- void setCleanPreviousImage(const bool &clean, const vpColor &color = vpColor::white) {
- cleanPrevImage = clean;
- bgColor = color;
- }
+ // projection du plan par cMo => creation des deux triangles definissant
+ // projection du plan sur plan image (coord en metre)
+ void setCameraPosition(const vpHomogeneousMatrix &cMt);
- /*!
- This function allows to set the background to a texture instead of the default black background.
+ void setInterpolationType(const vpInterpolationType interplt) { this->interp = interplt; }
- \param Iback : Image/Texture for the background
- */
- inline void
- setBackGroundTexture(const vpImage<unsigned char>& Iback) {
- setBackgroundTexture = true;
- Ig = Iback;
- }
-
- private:
- void initPlan(vpColVector* X);
-
- //result = plan est visible.
- //ie: un plan est oriente dans si normal_plan.focal < 0 => plan est visible sinon invisible.
- bool isVisible() {return visible;}
-
- //function that project a point x,y on the plane, return true if the projection is on the limited plane
- // and in this case return the corresponding image pixel Ipixelplan
- bool getPixel(const vpImagePoint &iP,unsigned char &Ipixelplan);
- bool getPixel(const vpImagePoint &iP,vpRGBa &Ipixelplan);
- bool getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP,
- unsigned char &Ipixelplan);
- bool getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
- vpRGBa &Ipixelplan);
- bool getPixelDepth(const vpImagePoint &iP, double &Zpixelplan);
- bool getPixelVisibility(const vpImagePoint &iP, double &Zpixelplan);
-
- //operation 3D de base :
- void project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt,
- vpColVector &_vout);
- //donne coordonnes homogenes de _v;
- void getHomogCoord(const vpColVector &_v, vpColVector &_vH);
- //donne coordonnes _v en fction coord homogenes _vH;
- void getCoordFromHomog(const vpColVector &_vH, vpColVector &_v);
-
- void getRoi(const unsigned int &Iwidth, const unsigned int &Iheight,
- const vpCameraParameters &cam, const std::vector<vpPoint> &point, vpRect &rect);
-};
+ void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam);
+ void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam);
+
+ void getImage(vpImage<unsigned char> &I, vpImage<unsigned char> &Isrc, const vpCameraParameters &cam);
+ void getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc, const vpCameraParameters &cam);
+
+ void getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam, vpMatrix &zBuffer);
+ void getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam, vpMatrix &zBuffer);
+
+ static void getImage(vpImage<unsigned char> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam);
+ static void getImage(vpImage<vpRGBa> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam);
+
+ std::vector<vpColVector> get3DcornersTextureRectangle();
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/);
+
+ /*!
+ As it can be time consuming to reset all the image to a default baground
+ value, this function enable to reset only the pixel which changed the
+ previous time.
+
+ By default this functionality is disabled. and the background color is
+ white.
+
+ \param clean : Enable the reset method.
+ \param color : Color of the back ground.
+ */
+ void setCleanPreviousImage(const bool &clean, const vpColor &color = vpColor::white)
+ {
+ cleanPrevImage = clean;
+ bgColor = color;
+ }
+
+ /*!
+ This function allows to set the background to a texture instead of the
+ default black background.
+
+ \param Iback : Image/Texture for the background
+ */
+ inline void setBackGroundTexture(const vpImage<unsigned char> &Iback)
+ {
+ setBackgroundTexture = true;
+ Ig = Iback;
+ }
+
+private:
+ void initPlan(vpColVector *X);
+
+ // result = plan est visible.
+ // ie: un plan est oriente dans si normal_plan.focal < 0 => plan est visible
+ // sinon invisible.
+ bool isVisible() { return visible; }
+
+ // function that project a point x,y on the plane, return true if the
+ // projection is on the limited plane
+ // and in this case return the corresponding image pixel Ipixelplan
+ bool getPixel(const vpImagePoint &iP, unsigned char &Ipixelplan);
+ bool getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan);
+ bool getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan);
+ bool getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan);
+ bool getPixelDepth(const vpImagePoint &iP, double &Zpixelplan);
+ bool getPixelVisibility(const vpImagePoint &iP, double &Zpixelplan);
+
+ // operation 3D de base :
+ void project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout);
+ // donne coordonnes homogenes de _v;
+ void getHomogCoord(const vpColVector &_v, vpColVector &_vH);
+ // donne coordonnes _v en fction coord homogenes _vH;
+ void getCoordFromHomog(const vpColVector &_vH, vpColVector &_v);
+
+ void getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam,
+ const std::vector<vpPoint> &point, vpRect &rect);
+};
#endif
@@ -309,5 +311,3 @@ class VISP_EXPORT vpImageSimulator
* c-basic-offset: 2
* End:
*/
-
-
diff --git a/modules/robot/include/visp3/robot/vpPioneer.h b/modules/robot/include/visp3/robot/vpPioneer.h
index 95c28a8..631e7c5 100644
--- a/modules/robot/include/visp3/robot/vpPioneer.h
+++ b/modules/robot/include/visp3/robot/vpPioneer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
#include <visp3/core/vpTranslationVector.h>
#include <visp3/robot/vpUnicycle.h>
-
/*!
\class vpPioneer
@@ -51,11 +51,12 @@
\brief Generic functions for Pioneer mobile robots.
This class provides common features for Pioneer mobile robots.
- This robot has two control velocities \f$(v_x, w_z)\f$, the translational and
- rotational velocities of the mobile platform respectively.
+ This robot has two control velocities \f$(v_x, w_z)\f$, the translational
+ and rotational velocities of the mobile platform respectively.
- The figure below shows the position of the frames that are used to model the robot.
- The end effector frame is here located at the middle point between the two wheels.
+ The figure below shows the position of the frames that are used to model the
+ robot. The end effector frame is here located at the middle point between
+ the two wheels.
\image html pioneer.png
@@ -87,7 +88,7 @@
\f].
*/
-class VISP_EXPORT vpPioneer: public vpUnicycle
+class VISP_EXPORT vpPioneer : public vpUnicycle
{
public:
/*!
@@ -102,7 +103,7 @@ public:
/*!
Destructor that does nothing.
*/
- virtual ~vpPioneer() {};
+ virtual ~vpPioneer(){};
private:
/*!
@@ -112,9 +113,9 @@ private:
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
+ vpRxyzVector cre; // radian
cte.set(0, 0, -l);
cre.buildFrom(vpMath::rad(90.), 0, vpMath::rad(90.));
cMe_.buildFrom(cte, vpRotationMatrix(cre));
@@ -125,9 +126,9 @@ private:
middle between the two wheels.
Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z]\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile robot and \f$\bf v\f$ the six dimention velocity skew expressed at the
- end effector frame, the robot jacobian is given by:
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control
+ velocities of the mobile robot and \f$\bf v\f$ the six dimention velocity
+ skew expressed at the end effector frame, the robot jacobian is given by:
\f[
{^e}{\bf J}_e = \left(\begin{array}{cc}
@@ -152,4 +153,3 @@ private:
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpPioneerPan.h b/modules/robot/include/visp3/robot/vpPioneerPan.h
index 86f4c58..25956ce 100644
--- a/modules/robot/include/visp3/robot/vpPioneerPan.h
+++ b/modules/robot/include/visp3/robot/vpPioneerPan.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,12 +38,11 @@
#ifndef VPPIONEERPAN_H
#define VPPIONEERPAN_H
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpUnicycle.h>
-
/*!
\class vpPioneerPan
@@ -51,13 +51,15 @@
\brief Generic functions for Pioneer mobile robots equiped with a pan head.
- This class provides common features for Pioneer mobile robots equiped with a pan head.
+ This class provides common features for Pioneer mobile robots equiped with a
+ pan head.
- This robot has three control velocities \f$(v_x, w_z, \dot{q_1})\f$, the translational and
- rotational velocities of the mobile platform, the pan head velocity respectively.
+ This robot has three control velocities \f$(v_x, w_z, \dot{q_1})\f$, the
+ translational and rotational velocities of the mobile platform, the pan head
+ velocity respectively.
- The figure below shows the position of the frames that are used to model the robot.
- The end effector frame is here located at the pan axis.
+ The figure below shows the position of the frames that are used to model the
+ robot. The end effector frame is here located at the pan axis.
\image html pioneer-pan.png
@@ -71,10 +73,10 @@
\right)
\f]
with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
- the six dimention velocity skew expressed at point E in frame E,
- the robot jacobian is given by:
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control
+ velocities of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the
+ pan head and \f$\bf v\f$ the six dimention velocity skew expressed at point
+ E in frame E, the robot jacobian is given by:
\f[
{^e}{\bf J}_e = \left(\begin{array}{ccc}
@@ -88,11 +90,11 @@
\right)
\f]
- with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
- located at the middle point between the two weels.
+ with \f$p_x, p_y\f$ the position of the head base frame in the mobile
+ platform frame located at the middle point between the two weels.
*/
-class VISP_EXPORT vpPioneerPan: public vpUnicycle
+class VISP_EXPORT vpPioneerPan : public vpUnicycle
{
public:
/*!
@@ -102,27 +104,28 @@ public:
{
double q = 0; // Initial position of the pan axis
set_mMp();
- set_pMe( q );
+ set_pMe(q);
set_cMe();
- set_eJe( q );
+ set_eJe(q);
}
/*!
Destructor that does nothing.
*/
- virtual ~vpPioneerPan() {};
+ virtual ~vpPioneerPan(){};
/** @name Inherited functionalities from vpPioneerPan */
//@{
/*!
- Set the robot jacobian expressed at point E the end effector frame located on the pan head.
+ Set the robot jacobian expressed at point E the end effector frame located
+ on the pan head.
Considering \f${\bf v} = {^e}{\bf J}_e \; [v_x, w_z, \dot{q_1}]\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of the pan head and \f$\bf v\f$
- the six dimention velocity skew expressed at point E in frame E,
- the robot jacobian is given by:
+ \f$(v_x, w_z)\f$ respectively the translational and rotational control
+ velocities of the mobile platform, \f$\dot{q_1}\f$ the joint velocity of
+ the pan head and \f$\bf v\f$ the six dimention velocity skew expressed at
+ point E in frame E, the robot jacobian is given by:
\f[
{^e}{\bf J}_e = \left(\begin{array}{ccc}
@@ -136,8 +139,8 @@ public:
\right)
\f]
- with \f$p_x, p_y\f$ the position of the head base frame in the mobile platform frame
- located at the middle point between the two weels.
+ with \f$p_x, p_y\f$ the position of the head base frame in the mobile
+ platform frame located at the middle point between the two weels.
*/
void set_eJe(double q_pan)
@@ -147,14 +150,15 @@ public:
double c1 = cos(q_pan);
double s1 = sin(q_pan);
- eJe_.resize(6,3); // robot jacobian expressed at the pan head end effector
+ eJe_.resize(6,
+ 3); // robot jacobian expressed at the pan head end effector
eJe_ = 0;
eJe_[0][0] = c1;
- eJe_[0][1] = -c1*py - s1*px;
+ eJe_[0][1] = -c1 * py - s1 * px;
eJe_[2][0] = s1;
- eJe_[2][1] = -s1*py + c1*px;
+ eJe_[2][1] = -s1 * py + c1 * px;
eJe_[4][1] = -1;
eJe_[4][2] = 1;
@@ -165,7 +169,8 @@ protected:
/** @name Protected Member Functions Inherited from vpPioneerPan */
//@{
/*!
- Set the transformation between the camera frame and the pan head end effector frame.
+ Set the transformation between the camera frame and the pan head end
+ effector frame.
*/
void set_cMe()
{
@@ -176,8 +181,8 @@ protected:
vpTranslationVector etc(cx, cy, cz);
vpRotationMatrix eRc;
eRc[0][0] = eRc[1][1] = eRc[2][2] = 0;
- eRc[0][2] = 1;
- eRc[1][1] = 1;
+ eRc[0][2] = 1;
+ eRc[1][1] = 1;
eRc[2][0] = -1;
vpHomogeneousMatrix eMc;
@@ -188,7 +193,8 @@ protected:
/*!
Set the transformation between the mobile platform frame
- located at the middle point between the two weels and the base frame of the pan head.
+ located at the middle point between the two weels and the base frame of
+ the pan head.
*/
void set_mMp()
{
@@ -220,7 +226,7 @@ protected:
pRe[0][2] = pRe[1][0] = sin(q);
pRe[1][1] = pRe[2][2] = 0.;
pRe[2][1] = 1.;
- pRe[1][2] = - pRe[0][0];
+ pRe[1][2] = -pRe[0][0];
pMe_.insert(pRe);
}
@@ -229,8 +235,6 @@ protected:
protected:
vpHomogeneousMatrix mMp_; // constant
vpHomogeneousMatrix pMe_; // depends on q pan
-
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpPtu46.h b/modules/robot/include/visp3/robot/vpPtu46.h
index c3a05c8..d98361f 100644
--- a/modules/robot/include/visp3/robot/vpPtu46.h
+++ b/modules/robot/include/visp3/robot/vpPtu46.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,7 +41,6 @@
#ifndef __vpPtu46_H
#define __vpPtu46_H
-
/* ----------------------------------------------------------------------- */
/* --- INCLUDES -------------------------------------------------------- */
/* --------------------------------------------------------------------- */
@@ -50,8 +50,8 @@
/* --- ViSP --- */
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoseVector.h>
#include <visp3/core/vpMath.h>
@@ -72,33 +72,29 @@
*/
-
class VISP_EXPORT vpPtu46
{
public: /* Constants */
-
/** Nombre d'articulations du robot. */
- static const unsigned int ndof; /*!< Number of dof */
+ static const unsigned int ndof; /*!< Number of dof */
/** Geometric model */
static const float L;
static const float h;
-
public: /* Methodes publiques */
-
- vpPtu46 (void);
+ vpPtu46(void);
/*! Destructor that does nothing. */
- virtual ~vpPtu46() {};
+ virtual ~vpPtu46(){};
/** @name Inherited functionalities from vpPtu46 */
//@{
- void init (void);
+ void init(void);
- void computeMGD (const vpColVector &q, vpHomogeneousMatrix & fMc) const;
- vpHomogeneousMatrix computeMGD (const vpColVector & q) const;
- void computeMGD (const vpColVector & q, vpPoseVector & r) const;
+ void computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
+ vpHomogeneousMatrix computeMGD(const vpColVector &q) const;
+ void computeMGD(const vpColVector &q, vpPoseVector &r) const;
void get_cMe(vpHomogeneousMatrix &_cMe) const;
void get_cVe(vpVelocityTwistMatrix &_cVe) const;
@@ -106,7 +102,7 @@ public: /* Methodes publiques */
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 dacd63d..7ede0b0 100644
--- a/modules/robot/include/visp3/robot/vpRingLight.h
+++ b/modules/robot/include/visp3/robot/vpRingLight.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpRingLight_h
#define vpRingLight_h
@@ -44,16 +44,14 @@
\brief Ring light management under unix.
*/
-
-
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_MODULE_IO) && defined(VISP_HAVE_PARPORT)
#include <iostream>
-#include <visp3/robot/vpRingLight.h>
#include <visp3/io/vpParallelPort.h>
+#include <visp3/robot/vpRingLight.h>
/*!
\class vpRingLight
@@ -68,8 +66,8 @@
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/robot/vpRingLight.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -90,8 +88,8 @@ int main()
Here is an example showing how to turn on the lighting during 10 seconds.
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRingLight.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRingLight.h>
int main()
{
@@ -123,8 +121,7 @@ public:
private:
vpParallelPort parport;
-
-} ;
+};
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobot.h b/modules/robot/include/visp3/robot/vpRobot.h
index 4ae6a7c..f898601 100644
--- a/modules/robot/include/visp3/robot/vpRobot.h
+++ b/modules/robot/include/visp3/robot/vpRobot.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,17 +44,16 @@
\brief class that defines a generic virtual robot
*/
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpColVector.h>
#include <visp3/core/vpPoseVector.h>
-
/*!
\class vpRobot
- \ingroup group_robot_real_gantry group_robot_real_cylindrical group_robot_real_arm
- \ingroup group_robot_real_ptu group_robot_real_unicycle group_robot_real_template
- \brief Class that defines a generic virtual robot.
+ \ingroup group_robot_real_gantry group_robot_real_cylindrical
+ group_robot_real_arm \ingroup group_robot_real_ptu group_robot_real_unicycle
+ group_robot_real_template \brief Class that defines a generic virtual robot.
*/
class VISP_EXPORT vpRobot
{
@@ -61,60 +61,58 @@ public:
/*!
Robot control states.
*/
- typedef enum
- {
- STATE_STOP, /*!< Stops robot motion especially in velocity and
- acceleration control. */
- STATE_VELOCITY_CONTROL, //!< Initialize the velocity controller.
- STATE_POSITION_CONTROL, //!< Initialize the position controller.
+ typedef enum {
+ STATE_STOP, /*!< Stops robot motion especially in velocity and
+ acceleration control. */
+ STATE_VELOCITY_CONTROL, //!< Initialize the velocity controller.
+ STATE_POSITION_CONTROL, //!< Initialize the position controller.
STATE_ACCELERATION_CONTROL //!< Initialize the acceleration controller.
- } vpRobotStateType ;
+ } vpRobotStateType;
/*!
Robot control frames.
*/
- typedef enum
- {
+ typedef enum {
REFERENCE_FRAME, /*!< Corresponds to a fixed reference frame
attached to the robot structure. */
ARTICULAR_FRAME, /*!< Corresponds to the joint space. */
CAMERA_FRAME, /*!< Corresponds to a frame attached to the
camera mounted on the robot end-effector. */
- MIXT_FRAME /*!< Corresponds to a "virtual" frame where
- translations are expressed in the reference frame, and
- rotations in the camera frame.*/
- } vpControlFrameType ;
+ MIXT_FRAME /*!< Corresponds to a "virtual" frame where
+ translations are expressed in the reference frame, and
+ rotations in the camera frame.*/
+ } vpControlFrameType;
-private: /* Membres privees */
- vpRobot::vpRobotStateType stateRobot;
- vpRobot::vpControlFrameType frameRobot;
+private: /* Membres privees */
+ vpRobot::vpRobotStateType stateRobot;
+ vpRobot::vpControlFrameType frameRobot;
protected:
double maxTranslationVelocity;
- static const double maxTranslationVelocityDefault;// = 0.2;
+ static const double maxTranslationVelocityDefault; // = 0.2;
double maxRotationVelocity;
- static const double maxRotationVelocityDefault;// = 0.7;
+ static const double maxRotationVelocityDefault; // = 0.7;
//! number of degrees of freedom
- int nDof ;
+ int nDof;
//! robot Jacobian expressed in the end-effector frame
- vpMatrix eJe ;
+ vpMatrix eJe;
//! is the robot Jacobian expressed in the end-effector frame available
- int eJeAvailable ;
+ int eJeAvailable;
//! robot Jacobian expressed in the robot reference frame available
- vpMatrix fJe ;
+ vpMatrix fJe;
//! is the robot Jacobian expressed in the robot reference frame available
- int fJeAvailable ;
+ int fJeAvailable;
- int areJointLimitsAvailable ;
+ int areJointLimitsAvailable;
double *qmin;
- double *qmax ;
+ double *qmax;
bool verbose_;
public:
- vpRobot (void);
- vpRobot (const vpRobot &robot);
+ vpRobot(void);
+ vpRobot(const vpRobot &robot);
virtual ~vpRobot();
/** @name Inherited functionalities from vpRobot */
@@ -122,53 +120,52 @@ public:
//---------- Jacobian -----------------------------
//! Get the robot Jacobian expressed in the end-effector frame
- virtual void get_eJe(vpMatrix &_eJe) = 0 ;
- //! Get the robot Jacobian expressed in the robot reference (or world) frame.
- virtual void get_fJe(vpMatrix &_fJe) = 0 ;
+ virtual void get_eJe(vpMatrix &_eJe) = 0;
+ //! Get the robot Jacobian expressed in the robot reference (or world)
+ //! frame.
+ virtual void get_fJe(vpMatrix &_fJe) = 0;
- //! Get a displacement (frame as to ve specified) between two successive position control.
- virtual void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &q) = 0 ;
+ //! Get a displacement (frame as to ve specified) between two successive
+ //! position control.
+ virtual void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &q) = 0;
- double getMaxTranslationVelocity (void) const ;
- double getMaxRotationVelocity (void) const;
+ 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);
- virtual vpRobotStateType getRobotState (void) const { return stateRobot ; }
+ vpColVector getPosition(const vpRobot::vpControlFrameType frame);
+ virtual vpRobotStateType getRobotState(void) const { return stateRobot; }
- virtual void init() = 0 ;
+ virtual void init() = 0;
- vpRobot & operator=(const vpRobot &robot);
+ vpRobot &operator=(const vpRobot &robot);
- void setMaxRotationVelocity (const double maxVr);
- void setMaxTranslationVelocity (const double maxVt);
+ 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 ;
+ //! Set the velocity (frame has to be specified) that will be applied to the
+ //! velocity controller.
+ virtual void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) = 0;
inline void setVerbose(bool verbose) { verbose_ = verbose; };
//@}
/** @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:
/** @name Protected Member Functions Inherited from vpRobot */
//@{
- vpControlFrameType setRobotFrame (vpRobot::vpControlFrameType newFrame);
- vpControlFrameType getRobotFrame (void) const { return frameRobot ; }
+ vpControlFrameType setRobotFrame(vpRobot::vpControlFrameType newFrame);
+ vpControlFrameType getRobotFrame(void) const { return frameRobot; }
//@}
-} ;
+};
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotAfma4.h b/modules/robot/include/visp3/robot/vpRobotAfma4.h
index 12cba10..5f6ea9e 100644
--- a/modules/robot/include/visp3/robot/vpRobotAfma4.h
+++ b/modules/robot/include/visp3/robot/vpRobotAfma4.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,18 +46,17 @@
#include <iostream>
#include <stdio.h>
-#include <visp3/robot/vpRobot.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
#include <visp3/robot/vpAfma4.h>
+#include <visp3/robot/vpRobot.h>
// low level controller api
extern "C" {
-# include "irisa_Afma4.h"
-# include "trycatch.h"
+#include "irisa_Afma4.h"
+#include "trycatch.h"
}
-
/*!
\class vpRobotAfma4
@@ -78,11 +78,11 @@ extern "C" {
This class allows to control the Afma4 cylindrical robot in position
and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- in the camera frame (vpRobot::CAMERA_FRAME),
- Mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ Mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
in the reference frame and rotations in the camera frame is not implemented.
All the translations are expressed in meters for positions and m/s
@@ -174,21 +174,16 @@ extern "C" {
positions from a position file with readPosFile() and writePosFile()
methods.
*/
-class VISP_EXPORT vpRobotAfma4
- :
- public vpAfma4,
- public vpRobot
+class VISP_EXPORT vpRobotAfma4 : public vpAfma4, public vpRobot
{
private: /* Not allowed functions. */
-
/*!
Copy constructor not allowed.
*/
- vpRobotAfma4 (const vpRobotAfma4 & robot);
+ vpRobotAfma4(const vpRobotAfma4 &robot);
private: /* Attributs prives. */
-
/** \brief Vrai ssi aucun objet de la classe vpRobotAfma4 n'existe.
*
* Il ne peut exister simultanement qu'un seul objet de la classe
@@ -213,8 +208,7 @@ private: /* Attributs prives. */
vpColVector q_prev_getdis;
bool first_time_getdis;
-public: /* Constantes */
-
+public: /* Constantes */
/* Vitesse maximale par default lors du positionnement du robot.
* C'est la valeur a la construction de l'attribut prive \a
* positioningVelocity. Cette valeur peut etre changee par la fonction
@@ -222,30 +216,24 @@ public: /* Constantes */
*/
static const double defaultPositioningVelocity; // = 20.0;
-public: /* Methode publiques */
+public: /* Methode publiques */
+ explicit vpRobotAfma4(bool verbose = true);
+ virtual ~vpRobotAfma4(void);
- 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);
+ 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);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
void get_cMe(vpHomogeneousMatrix &cMe) const;
void get_cVe(vpVelocityTwistMatrix &cVe) const;
@@ -253,39 +241,32 @@ public: /* Methode publiques */
void get_eJe(vpMatrix &eJe);
void get_fJe(vpMatrix &fJe);
- void init (void);
+ void init(void);
- void move(const char *filename) ;
+ void move(const char *filename);
- void powerOn() ;
- void powerOff() ;
+ void powerOn();
+ void powerOff();
- static bool readPosFile(const std::string &filename, vpColVector &q) ;
- static bool savePosFile(const std::string &filename, const vpColVector &q) ;
+ static bool readPosFile(const std::string &filename, vpColVector &q);
+ 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 char *filename) ;
- void setPositioningVelocity (const double velocity);
+ void setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &position);
+ void setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2, const double q4,
+ const double q5);
+ void setPosition(const char *filename);
+ void setPositioningVelocity(const double velocity);
/* --- ETAT ------------------------------------------------------------- */
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+ vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState);
/* --- VITESSE ---------------------------------------------------------- */
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
-
- void stopMotion() ;
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
+ void stopMotion();
};
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotAfma6.h b/modules/robot/include/visp3/robot/vpRobotAfma6.h
index 98f84e2..7d56b27 100644
--- a/modules/robot/include/visp3/robot/vpRobotAfma6.h
+++ b/modules/robot/include/visp3/robot/vpRobotAfma6.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,19 +46,18 @@
#include <iostream>
#include <stdio.h>
-#include <visp3/robot/vpRobot.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpPoseVector.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpPoseVector.h>
#include <visp3/robot/vpAfma6.h>
+#include <visp3/robot/vpRobot.h>
// low level controller api
extern "C" {
-# include "irisa_Afma6.h"
-# include "trycatch.h"
+#include "irisa_Afma6.h"
+#include "trycatch.h"
}
-
/*!
\class vpRobotAfma6
@@ -80,10 +80,10 @@ extern "C" {
This class allows to control the Afma6 gantry robot in position
and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
in the reference frame and rotations in the camera frame.
All the translations are expressed in meters for positions and m/s
@@ -120,18 +120,18 @@ int main() {}
robot with:
\code
- // Set the extrinsic camera parameters obtained with a perpective
+ // Set the extrinsic camera parameters obtained with a perpective
// projection model including a distortion parameter
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
\endcode
-
+
You can get the intrinsic camera parameters of the image I
acquired with the camera, with:
\code
vpCameraParameters cam;
robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
+ // In cam, you get the intrinsic parameters of the projection model
// with distortion.
\endcode
@@ -207,21 +207,16 @@ int main() {}
positions from a position file with readPosFile() and writePosFile()
methods.
*/
-class VISP_EXPORT vpRobotAfma6
- :
- public vpAfma6,
- public vpRobot
+class VISP_EXPORT vpRobotAfma6 : public vpAfma6, public vpRobot
{
private: /* Not allowed functions. */
-
/*!
Copy constructor not allowed.
*/
- vpRobotAfma6 (const vpRobotAfma6 & robot);
+ vpRobotAfma6(const vpRobotAfma6 &robot);
private: /* Attributs prives. */
-
/** \brief Vrai ssi aucun objet de la classe vpRobotAfma6 n'existe.
*
* Il ne peut exister simultanement qu'un seul objet de la classe
@@ -247,9 +242,7 @@ private: /* Attributs prives. */
bool first_time_getdis;
vpHomogeneousMatrix fMc_prev_getdis;
-
-public: /* Constantes */
-
+public: /* Constantes */
/* Vitesse maximale par default lors du positionnement du robot.
* C'est la valeur a la construction de l'attribut prive \a
* positioningVelocity. Cette valeur peut etre changee par la fonction
@@ -257,38 +250,30 @@ public: /* Constantes */
*/
static const double defaultPositioningVelocity; // = 20.0;
-public: /* Methode publiques */
-
- vpRobotAfma6 (bool verbose=true);
- virtual ~vpRobotAfma6 (void);
+public: /* Methode publiques */
+ explicit vpRobotAfma6(bool verbose = true);
+ virtual ~vpRobotAfma6(void);
- bool checkJointLimits(vpColVector& jointsStatus);
+ bool checkJointLimits(vpColVector &jointsStatus);
- void closeGripper() ;
+ 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);
+ 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);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
void get_cMe(vpHomogeneousMatrix &_cMe) const;
void get_cVe(vpVelocityTwistMatrix &_cVe) const;
@@ -298,59 +283,40 @@ public: /* Methode publiques */
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) ;
+ void move(const std::string &filename);
+ void move(const std::string &filename, const double velocity);
- void openGripper() ;
+ void openGripper();
- void powerOn() ;
- void powerOff() ;
+ void powerOn();
+ void powerOff();
- static bool readPosFile(const std::string &filename, vpColVector &q) ;
- static bool savePosFile(const std::string &filename, const vpColVector &q) ;
+ static bool readPosFile(const std::string &filename, vpColVector &q);
+ 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 std::string &filename) ;
- void setPositioningVelocity (const double velocity);
+ 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);
/* --- ETAT ------------------------------------------------------------- */
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
-
+ vpRobot::vpRobotStateType setRobotState(vpRobot::vpRobotStateType newState);
/* --- VITESSE ---------------------------------------------------------- */
- void setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & velocity);
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
- void stopMotion() ;
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
+ void stopMotion();
};
-
-
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
-
#endif
#endif /* #ifndef vpRobotAfma6_h */
diff --git a/modules/robot/include/visp3/robot/vpRobotBiclops.h b/modules/robot/include/visp3/robot/vpRobotBiclops.h
index f9cf49e..8b279fe 100644
--- a/modules/robot/include/visp3/robot/vpRobotBiclops.h
+++ b/modules/robot/include/visp3/robot/vpRobotBiclops.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,31 +43,26 @@
#ifndef __vpROBOT_BICLOPS_H
#define __vpROBOT_BICLOPS_H
-
/* ------------------------------------------------------------------------ */
/* --- INCLUDES ----------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
/* --- GENERAL --- */
#include <iostream>
-#include <stdio.h>
#include <pthread.h>
-
+#include <stdio.h>
/* --- ViSP --- */
-#include <visp3/robot/vpRobot.h>
-#include <visp3/robot/vpBiclops.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpBiclops.h>
+#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpRobotBiclopsController.h>
-
/* ------------------------------------------------------------------------ */
/* --- CLASS -------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
-
/*!
\class vpRobotBiclops
@@ -82,12 +78,12 @@
The control of the head is done by vpRobotBiclopsController class.
- \warning Velocity control mode is not exported from the top-level Biclops API
- class provided by Traclabs. That means that there is no protection in this
- mode to prevent an axis from striking its hard limit. In position mode,
+ \warning Velocity control mode is not exported from the top-level Biclops
+ API class provided by Traclabs. That means that there is no protection in
+ this mode to prevent an axis from striking its hard limit. In position mode,
Traclabs put soft limits in that keep any command from driving to a position
- too close to the hard limits. In velocity mode this protection does not exist
- in the current API.
+ too close to the hard limits. In velocity mode this protection does not
+ exist in the current API.
\warning With the understanding that hitting the hard limits at full
speed/power can damage the unit, damage due to velocity mode commanding is
@@ -95,17 +91,8 @@
*/
-class VISP_EXPORT vpRobotBiclops
- :
- public vpBiclops,
- public vpRobot
+class VISP_EXPORT vpRobotBiclops : public vpBiclops, public vpRobot
{
-
-private:
-
- void getCameraDisplacement(vpColVector &d);
- void getArticularDisplacement(vpColVector &d);
-
private:
static bool robotAlreadyCreated;
pthread_t control_thread;
@@ -118,59 +105,57 @@ private:
vpColVector q_previous;
bool controlThreadCreated;
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// /*! \brief No copy constructor allowed. */
-// vpRobotBiclops(const vpRobotBiclops &)
-// : vpBiclops(), vpRobot(), control_thread(), controller(),
-// positioningVelocity(0), q_previous(), controlThreadCreated(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpRobotBiclops &operator=(const vpRobotBiclops &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // /*! \brief No copy constructor allowed. */
+ // vpRobotBiclops(const vpRobotBiclops &)
+ // : vpBiclops(), vpRobot(), control_thread(), controller(),
+ // positioningVelocity(0), q_previous(), controlThreadCreated(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpRobotBiclops &operator=(const vpRobotBiclops &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
+ static const double defaultPositioningVelocity;
- static const double defaultPositioningVelocity;
-
- vpRobotBiclops (void);
- vpRobotBiclops (const std::string &filename);
- virtual ~vpRobotBiclops (void);
+ vpRobotBiclops(void);
+ explicit vpRobotBiclops(const std::string &filename);
+ virtual ~vpRobotBiclops(void);
- void init (void);
+ void init(void);
void get_cMe(vpHomogeneousMatrix &_cMe) const;
void get_cVe(vpVelocityTwistMatrix &_cVe) const;
- void get_eJe(vpMatrix &_eJe) ;
- void get_fJe(vpMatrix &_fJe) ;
+ void get_eJe(vpMatrix &_eJe);
+ void get_fJe(vpMatrix &_fJe);
void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &d);
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
- double getPositioningVelocity (void);
- void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ double getPositioningVelocity(void);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
- bool readPositionFile(const std::string &filename, vpColVector &q) ;
+ 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 setPosition(const char *filename) ;
- void setPositioningVelocity (const double velocity);
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & q_dot);
+ void 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);
- void stopMotion() ;
+ void stopMotion();
- static void * vpRobotBiclopsSpeedControlLoop (void * arg);
+ static void *vpRobotBiclopsSpeedControlLoop(void *arg);
};
-
-
#endif /* #ifndef __vpROBOT_BICLOPS_H */
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h
index 5dd4d32..a1e7625 100644
--- a/modules/robot/include/visp3/robot/vpRobotBiclopsController.h
+++ b/modules/robot/include/visp3/robot/vpRobotBiclopsController.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,23 +44,22 @@
#ifndef __vpROBOT_BICLOPS_CONTROLLER_H
#define __vpROBOT_BICLOPS_CONTROLLER_H
-
/* ------------------------------------------------------------------------ */
/* --- INCLUDES ----------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
/* --- GENERAL --- */
-#include <iostream> /* Classe std::ostream. */
-#include <stdio.h> /* Classe std::ostream. */
-#include <pthread.h> /* Classe std::ostream. */
+#include <iostream> /* Classe std::ostream. */
+#include <pthread.h> /* Classe std::ostream. */
+#include <stdio.h> /* Classe std::ostream. */
#include <visp3/core/vpConfig.h>
-#include "Biclops.h" // Contrib for Biclops robot
-#include "PMDUtils.h" // Contrib for Biclops robot
+#include "Biclops.h" // Contrib for Biclops robot
+#include "PMDUtils.h" // Contrib for Biclops robot
#if defined(_WIN32)
-class VISP_EXPORT Biclops; //needed for dll creation
+class VISP_EXPORT Biclops; // needed for dll creation
#endif
/* ------------------------------------------------------------------------ */
@@ -94,51 +94,48 @@ public:
// SHM
typedef struct /* ControllerShm_struct */ {
vpControllerStatusType status[2];
- double q_dot[2]; /*!< Desired speed. */
- double actual_q[2]; /*!< Current measured position of each axes. */
+ double q_dot[2]; /*!< Desired speed. */
+ double actual_q[2]; /*!< Current measured position of each axes. */
double actual_q_dot[2]; /*!< Current measured velocity of each axes. */
- bool jointLimit[2]; /*!< Indicates if an axe is in joint limit. */
+ bool jointLimit[2]; /*!< Indicates if an axe is in joint limit. */
} shmType;
#endif /* DOXYGEN_SHOULD_SKIP_THIS */
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpRobotBiclopsController(const vpRobotBiclopsController &)
-// : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL), vergeAxis(NULL),
-// panProfile(), tiltProfile(), vergeProfile(), shm(), stopControllerThread_(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpRobotBiclopsController &operator=(const vpRobotBiclopsController &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpRobotBiclopsController(const vpRobotBiclopsController &)
+ // : biclops(), axisMask(0), panAxis(NULL), tiltAxis(NULL),
+ // vergeAxis(NULL),
+ // panProfile(), tiltProfile(), vergeProfile(), shm(),
+ // stopControllerThread_(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpRobotBiclopsController &operator=(const vpRobotBiclopsController &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
vpRobotBiclopsController();
virtual ~vpRobotBiclopsController();
void init(const std::string &configfile);
- void setPosition(const vpColVector & q, const double percentVelocity);
- void setVelocity(const vpColVector & q_dot);
+ void setPosition(const vpColVector &q, const double percentVelocity);
+ void setVelocity(const vpColVector &q_dot);
vpColVector getPosition();
vpColVector getActualPosition();
vpColVector getVelocity();
vpColVector getActualVelocity();
- PMDAxisControl * getPanAxis() { return panAxis; };
- PMDAxisControl * getTiltAxis() { return tiltAxis; };
- PMDAxisControl * getVergeAxis() { return vergeAxis; };
+ PMDAxisControl *getPanAxis() { return panAxis; };
+ PMDAxisControl *getTiltAxis() { return tiltAxis; };
+ PMDAxisControl *getVergeAxis() { return vergeAxis; };
void writeShm(shmType &shm);
shmType readShm();
- bool isStopRequested()
- {
- return stopControllerThread_;
- }
+ bool isStopRequested() { return stopControllerThread_; }
- void stopRequest(bool stop)
- {
- stopControllerThread_ = stop;
- }
+ void stopRequest(bool stop) { stopControllerThread_ = stop; }
private:
Biclops biclops; // THE interface to Biclops.
@@ -155,7 +152,6 @@ private:
shmType shm;
bool stopControllerThread_;
-
};
#endif /* #ifndef __vpROBOT_BICLOPS_CONTROLLER_H */
diff --git a/modules/robot/include/visp3/robot/vpRobotCamera.h b/modules/robot/include/visp3/robot/vpRobotCamera.h
index ba1486e..83c1430 100644
--- a/modules/robot/include/visp3/robot/vpRobotCamera.h
+++ b/modules/robot/include/visp3/robot/vpRobotCamera.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpRobotCamera_H
#define vpRobotCamera_H
@@ -59,19 +59,19 @@
\deprecated This class is deprecated since ViSP 3.0.0.
- \brief Class that defines the simplest robot: a free flying camera. We recommend to use
- vpSimulatorCamera instead.
+ \brief Class that defines the simplest robot: a free flying camera. We
+recommend to use vpSimulatorCamera instead.
This free flying camera has 6 dof; 3 in translation and 3 in rotation.
It evolves as a gentry robot with respect to a world frame. This class
- is similar to vpSimulatorCamera class except that here the position of the robot
- is provided as the transformation from camera frame to world frame; cMw. Since
- the position of the camera frame evolves, this representation is less intuitive
- than the one implemented in vpSimulatorCamera where the transformation from world
- to camera frame is considered; wMc.
+ is similar to vpSimulatorCamera class except that here the position of the
+robot is provided as the transformation from camera frame to world frame; cMw.
+Since the position of the camera frame evolves, this representation is less
+intuitive than the one implemented in vpSimulatorCamera where the
+transformation from world to camera frame is considered; wMc.
- For this particular simulated robot, the end-effector and camera frame are confused.
- That means that the cMe transformation is equal to identity.
+ For this particular simulated robot, the end-effector and camera frame are
+confused. That means that the cMe transformation is equal to identity.
The robot jacobian expressed in the end-effector frame
\f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
@@ -113,29 +113,29 @@ protected:
vpHomogeneousMatrix cMw_; // camera to world
public:
- vpRobotCamera() ;
- virtual ~vpRobotCamera() ;
+ vpRobotCamera();
+ virtual ~vpRobotCamera();
/** @name Inherited functionalities from vpRobotCamera */
//@{
void get_cVe(vpVelocityTwistMatrix &cVe) const;
void get_eJe(vpMatrix &eJe);
- void getPosition(vpHomogeneousMatrix &cMw) const ;
+ void getPosition(vpHomogeneousMatrix &cMw) const;
void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void setPosition(const vpHomogeneousMatrix &cMw) ;
- void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v) ;
+ void setPosition(const vpHomogeneousMatrix &cMw);
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v);
//@}
private:
- void init() ;
+ void init();
// Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
+ void get_fJe(vpMatrix & /*_fJe */){};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */){};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */){};
+};
#endif
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotException.h b/modules/robot/include/visp3/robot/vpRobotException.h
index d1311a7..b1240a0 100644
--- a/modules/robot/include/visp3/robot/vpRobotException.h
+++ b/modules/robot/include/visp3/robot/vpRobotException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,15 +37,15 @@
*
*****************************************************************************/
-
#ifndef __vpRobotException_H
#define __vpRobotException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpRobotException.h
\brief error that can be emited by the vpRobot class and its derivates
@@ -53,12 +54,15 @@
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpRobotException
@@ -66,78 +70,70 @@
*/
class VISP_EXPORT vpRobotException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpRobot member
- */
- enum errorRobotCodeEnum
- {
-
- /** Erreur lancee par le constructor. */
- constructionError,
-
- /** Erreur lancee lors de la construction d'un objet CRobot
- * correspondant a un robot reel si l'objet de la classe en
- * question doit etre unique. */
- uniqueRobotError,
-
- /** Erreur lancee par les fonctions de commande si le
- * robot n'est pas dans le bon etat au moment du passage
- * d'ordre. */
- wrongStateError,
-
- /** Erreur lancee par les fonctions de changement d'etat
- * si le changement demandee n'est pas possible. */
- stateModificationError,
-
- /** Erreur generee lors d'un retour non nulle d'une fonction
- * de communication de la lib Afma6. */
- communicationError,
-
- /** Erreur lancee apres un appel a une fonction de la lib
- * bas-niveau de control de l'afma6 ayant renvoye une erreur. */
- lowLevelError,
-
- /** Erreur lancee par la fonction de parsing des parametres du
- * robot, si le fichier donne en entree n'est pas valide.
- */
- readingParametersError,
-
- /** Erreur lancee par les methodes d'une classe qui necessite
- * un appel a une fonction d'initialisation apres la
- * construction si l'init n'a pas ete fait. */
- notInitializedError,
-
- /** Erreur lancee par les fonctions decrites dans lAPI mais
- * pas completement implementee. Dans ce cas, la fonction
- * affiche simplement un message d'erreur avant de sortir
- * par le 'throw'.
- */
- notImplementedError,
- /** Position is out of range.
- */
- positionOutOfRangeError
- } ;
-
- public:
- vpRobotException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpRobotException (const int id, const std::string & msg)
- : vpException(id, msg)
- {
- }
- vpRobotException (const int id)
- : vpException(id)
- {
- }
-
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpRobot member
+ */
+ enum errorRobotCodeEnum {
+
+ /** Erreur lancee par le constructor. */
+ constructionError,
+
+ /** Erreur lancee lors de la construction d'un objet CRobot
+ * correspondant a un robot reel si l'objet de la classe en
+ * question doit etre unique. */
+ uniqueRobotError,
+
+ /** Erreur lancee par les fonctions de commande si le
+ * robot n'est pas dans le bon etat au moment du passage
+ * d'ordre. */
+ wrongStateError,
+
+ /** Erreur lancee par les fonctions de changement d'etat
+ * si le changement demandee n'est pas possible. */
+ stateModificationError,
+
+ /** Erreur generee lors d'un retour non nulle d'une fonction
+ * de communication de la lib Afma6. */
+ communicationError,
+
+ /** Erreur lancee apres un appel a une fonction de la lib
+ * bas-niveau de control de l'afma6 ayant renvoye une erreur. */
+ lowLevelError,
+
+ /** Erreur lancee par la fonction de parsing des parametres du
+ * robot, si le fichier donne en entree n'est pas valide.
+ */
+ readingParametersError,
+
+ /** Erreur lancee par les methodes d'une classe qui necessite
+ * un appel a une fonction d'initialisation apres la
+ * construction si l'init n'a pas ete fait. */
+ notInitializedError,
+
+ /** Erreur lancee par les fonctions decrites dans lAPI mais
+ * pas completement implementee. Dans ce cas, la fonction
+ * affiche simplement un message d'erreur avant de sortir
+ * par le 'throw'.
+ */
+ notImplementedError,
+ /** Position is out of range.
+ */
+ positionOutOfRangeError
+ };
+
+public:
+ vpRobotException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpRobotException(const int id, const std::string &msg) : vpException(id, msg) {}
+ explicit vpRobotException(const int id) : vpException(id) {}
};
#endif
diff --git a/modules/robot/include/visp3/robot/vpRobotPioneer.h b/modules/robot/include/visp3/robot/vpRobotPioneer.h
index 7e33b1d..8ab6945 100644
--- a/modules/robot/include/visp3/robot/vpRobotPioneer.h
+++ b/modules/robot/include/visp3/robot/vpRobotPioneer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,14 +44,14 @@
#include <Aria.h>
-// Warning: vpMath.h included from vpRobot.h or vpPioneer.h should be
+// Warning: vpMath.h included from vpRobot.h or vpPioneer.h should be
// included after Aria.h to avoid the build issue:
-// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
+// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
// in this scope"
-// This error is due to cmath header included from vpMath.h that makes
+// This error is due to cmath header included from vpMath.h that makes
// isfinite() ambiguous between ::isfinite() and std::isfinite()
-#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpPioneer.h>
+#include <visp3/robot/vpRobot.h>
/*!
@@ -60,15 +61,14 @@
\brief Interface for Pioneer mobile robots based on Aria 3rd party library.
- This class provides a position and speed control interface for Pioneer mobile robots.
- It inherits from the Aria ArRobot class. For more information about the model of the robot,
- see vpPioneer documentation.
+ This class provides a position and speed control interface for Pioneer
+ mobile robots. It inherits from the Aria ArRobot class. For more information
+ about the model of the robot, see vpPioneer documentation.
*/
-class VISP_EXPORT vpRobotPioneer: public vpRobot, public vpPioneer, public ArRobot
+class VISP_EXPORT vpRobotPioneer : public vpRobot, public vpPioneer, public ArRobot
{
private: /* Not allowed functions. */
-
/*!
Copy constructor not allowed.
*/
@@ -82,41 +82,39 @@ public:
Get the robot Jacobian expressed at point E, the point located at the
middle between the two wheels.
- \param eJe : Robot jacobian such as \f$(v_x, w_z) = {^e}{\bf J}e \; {\bf v}\f$ with
- \f$(v_x, w_z)\f$ respectively the translational and rotational control velocities
- of the mobile robot, \f$\bf v\f$ the six dimention velocity skew, and where
+ \param eJe : Robot jacobian such as \f$(v_x, w_z) = {^e}{\bf J}e \; {\bf
+ v}\f$ with \f$(v_x, w_z)\f$ respectively the translational and rotational
+ control velocities of the mobile robot, \f$\bf v\f$ the six dimention
+ velocity skew, and where
\sa get_eJe()
*/
- void get_eJe(vpMatrix & eJe)
- {
- eJe = vpUnicycle::get_eJe();
- }
+ void get_eJe(vpMatrix &eJe) { eJe = vpUnicycle::get_eJe(); }
private: // Set as private since not implemented
/*!
Get the robot Jacobian expressed in the robot reference (or world) frame.
\warning Not implemented.
*/
- void get_fJe(vpMatrix & /*fJe*/) {} ;
+ void get_fJe(vpMatrix & /*fJe*/){};
/*!
- Get a displacement (frame as to ve specified) between two successive position control.
- \warning Not implemented.
+ 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);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
private: // Set as private since not implemented
/*!
Get the robot position (frame has to be specified).
\warning Not implemented.
*/
- void getPosition(const vpRobot::vpControlFrameType /*frame*/, vpColVector &/*q*/) {};
+ void getPosition(const vpRobot::vpControlFrameType /*frame*/, vpColVector & /*q*/){};
public:
void init();
@@ -126,7 +124,7 @@ private: // Set as private since not implemented
Set a displacement (frame has to be specified) in position control.
\warning Not implemented.
*/
- void setPosition(const vpRobot::vpControlFrameType /*frame*/, const vpColVector &/*q*/) {};
+ void setPosition(const vpRobot::vpControlFrameType /*frame*/, const vpColVector & /*q*/){};
public:
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel);
@@ -134,10 +132,7 @@ public:
/*!
Enable or disable sonar device usage.
*/
- void useSonar(bool usage)
- {
- this->comInt(ArCommands::SONAR, usage);
- }
+ void useSonar(bool usage) { this->comInt(ArCommands::SONAR, usage); }
protected:
bool isInitialized;
@@ -146,4 +141,3 @@ protected:
#endif
#endif // VPROBOTPIONEER_H
-
diff --git a/modules/robot/include/visp3/robot/vpRobotPtu46.h b/modules/robot/include/visp3/robot/vpRobotPtu46.h
index 4585c05..93c1d27 100644
--- a/modules/robot/include/visp3/robot/vpRobotPtu46.h
+++ b/modules/robot/include/visp3/robot/vpRobotPtu46.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,12 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_PTU46
-
#ifndef __vpROBOT_PTU46_H
#define __vpROBOT_PTU46_H
-
/* ------------------------------------------------------------------------ */
/* --- INCLUDES ----------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -52,13 +50,12 @@
#include <iostream>
#include <stdio.h>
-
/* --- ViSP --- */
-#include <visp3/robot/vpRobot.h>
-#include <visp3/robot/vpPtu46.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpPtu46.h>
+#include <visp3/robot/vpRobot.h>
#include <ptu.h> // Contrib for Ptu-46 robot
@@ -66,7 +63,6 @@
/* --- CLASS ------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
/*!
\class vpRobotPtu46
@@ -81,68 +77,57 @@
head.
*/
-class VISP_EXPORT vpRobotPtu46
- :
- public vpPtu46,
- public vpRobot
+class VISP_EXPORT vpRobotPtu46 : public vpPtu46, public vpRobot
{
private:
-
/*! \brief No copy constructor allowed. */
- vpRobotPtu46 (const vpRobotPtu46 & ass);
+ vpRobotPtu46(const vpRobotPtu46 &ass);
/*! Object to control. This is a contribution. */
Ptu ptu;
private:
static bool robotAlreadyCreated;
- double positioningVelocity;
- int velocityMesureTempo;
- char *device;
+ double positioningVelocity;
+ int velocityMesureTempo;
+ char *device;
public:
- static const double defaultPositioningVelocity;
+ static const double defaultPositioningVelocity;
- vpRobotPtu46 (const char *device="/dev/ttyS0");
- vpRobotPtu46 (vpRobotPtu46 * pub);
- virtual ~vpRobotPtu46 (void);
+ explicit vpRobotPtu46(const char *device = "/dev/ttyS0");
+ explicit vpRobotPtu46(vpRobotPtu46 *pub);
+ virtual ~vpRobotPtu46(void);
void get_cMe(vpHomogeneousMatrix &_cMe) const;
void get_cVe(vpVelocityTwistMatrix &_cVe) const;
void get_eJe(vpMatrix &_eJe);
void get_fJe(vpMatrix &_fJe);
- void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &q);
- double getPositioningVelocity (void);
- void getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & q_dot);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
+ void getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ double getPositioningVelocity(void);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q_dot);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
- void init (void);
+ 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 char *filename) ;
- void setPositioningVelocity (const double velocity);
- vpRobot::vpRobotStateType setRobotState (vpRobot::vpRobotStateType newState);
+ 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() ;
-
-private:
- void getCameraDisplacement(vpColVector &d);
- void getArticularDisplacement(vpColVector &d);
+ void stopMotion();
};
-
-
#endif /* #ifndef __vpROBOT_PTU46_H */
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/robot/include/visp3/robot/vpRobotSimulator.h b/modules/robot/include/visp3/robot/vpRobotSimulator.h
index fae4122..dbbe6b5 100644
--- a/modules/robot/include/visp3/robot/vpRobotSimulator.h
+++ b/modules/robot/include/visp3/robot/vpRobotSimulator.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,20 +45,19 @@
*/
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobot.h>
#include <visp3/core/vpTime.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-
+#include <visp3/robot/vpRobot.h>
/*!
\class vpRobotSimulator
- \ingroup group_robot_simu_gantry group_robot_simu_arm group_robot_simu_unicycle
- \ingroup group_robot_simu_camera
+ \ingroup group_robot_simu_gantry group_robot_simu_arm
+ group_robot_simu_unicycle \ingroup group_robot_simu_camera
\brief This class aims to be a basis used to create all the
robot simulators.
-
+
*/
class VISP_EXPORT vpRobotSimulator : public vpRobot
{
@@ -69,32 +69,26 @@ public:
/*!
Basic destructor
*/
- virtual ~vpRobotSimulator() {};
+ virtual ~vpRobotSimulator(){};
/** @name Inherited functionalities from vpRobotSimulator */
//@{
/*!
Return the sampling time.
- \return Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
+ \return Sampling time in second used to compute the robot displacement
+ from the velocity applied to the robot during this time.
*/
- inline double getSamplingTime() const
- {
- return(this->delta_t_);
- }
+ inline double getSamplingTime() const { return (this->delta_t_); }
/*!
Set the sampling time.
- \param delta_t : Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
+ \param delta_t : Sampling time in second used to compute the robot
+ displacement from the velocity applied to the robot during this time.
*/
- virtual inline void setSamplingTime(const double &delta_t)
- {
- this->delta_t_ = delta_t;
- }
+ 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 d17496c..bfbb4e0 100644
--- a/modules/robot/include/visp3/robot/vpRobotTemplate.h
+++ b/modules/robot/include/visp3/robot/vpRobotTemplate.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,13 @@
*
*****************************************************************************/
-
#ifndef vpRobotTemplate_H
#define vpRobotTemplate_H
/*!
\file vpRobotTemplate.h
- \brief class that defines a robot just to show which function you must implement
+ \brief class that defines a robot just to show which function you must
+ implement
*/
#include <visp3/robot/vpRobot.h>
@@ -49,51 +50,46 @@
/*!
\class vpRobotTemplate
\ingroup group_robot_real_template
- \brief class that defines a robot just to show which function you must implement
+ \brief class that defines a robot just to show which function you must
+ implement
*/
class VISP_EXPORT vpRobotTemplate : public vpRobot
{
public:
-
//! basic initialization
- void init() ;
+ void init();
//! constructor
- vpRobotTemplate() ;
+ vpRobotTemplate();
//! destructor
- virtual ~vpRobotTemplate() ;
-
+ virtual ~vpRobotTemplate();
//! get the robot Jacobian expressed in the end-effector frame
- void get_eJe(vpMatrix &_eJe) ;
+ void get_eJe(vpMatrix &_eJe);
//! get the robot Jacobian expressed in the robot reference frame
- void get_fJe(vpMatrix &_fJe) ;
+ void get_fJe(vpMatrix &_fJe);
//! send to the controller a velocity expressed in the camera frame
- void sendCameraVelocity(const vpColVector &v) ;
+ void sendCameraVelocity(const vpColVector &v);
//! send to the controller a velocity expressed in the articular frame
- void sendArticularVelocity(const vpColVector &qdot) ;
+ 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) ;
+ void getPosition(vpPoseVector &q);
//! get a position expressed in the articular frame
- void getArticularPosition(vpColVector &q) ;
+ void getArticularPosition(vpColVector &q);
//! get a displacement (frame as to be specified)
- void getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &q) ;
+ 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 8ca3a64..41f873c 100644
--- a/modules/robot/include/visp3/robot/vpRobotViper650.h
+++ b/modules/robot/include/visp3/robot/vpRobotViper650.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Interface for the Irisa's Viper S650 robot controlled by an Adept MotionBlox.
+ * Interface for the Irisa's Viper S650 robot controlled by an Adept
+ *MotionBlox.
*
* Authors:
* Fabien Spindler
@@ -45,18 +47,17 @@
#include <iostream>
#include <stdio.h>
-#include <visp3/robot/vpRobot.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpViper650.h>
// low level controller api
extern "C" {
-# include "irisa_Viper650.h"
-# include "trycatch.h"
+#include "irisa_Viper650.h"
+#include "trycatch.h"
}
-
/*!
\class vpRobotViper650
@@ -105,18 +106,21 @@ extern "C" {
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool vpViper650::TOOL_CUSTOM and set this
- during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool
+vpViper650::TOOL_CUSTOM and set this during robot initialisation or using
+set_eMc().
- \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
This class allows to control the Viper650 arm robot in position
and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame \f$ {\cal F}_f \f$ (vpRobot::REFERENCE_FRAME),
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame \f$ {\cal F}_f \f$
+(vpRobot::REFERENCE_FRAME),
- in the camera or tool frame \f$ {\cal F}_c \f$ (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame \f$ {\cal F}_f \f$ and rotations in the camera or tool frame \f$ {\cal F}_c \f$ .
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ in the reference frame \f$ {\cal F}_f \f$ and rotations in the camera or
+tool frame \f$ {\cal F}_c \f$ .
All the translations are expressed in meters for positions and m/s
for the velocities. Rotations are expressed in radians for the
@@ -158,10 +162,10 @@ int main()
#ifdef VISP_HAVE_VIPER650
vpRobotViper650 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // Set the extrinsic camera parameters obtained with a perpective
// projection model including a distortion parameter
robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
+ vpCameraParameters::perspectiveProjWithDistortion);
#endif
}
\endcode
@@ -170,11 +174,11 @@ int main()
acquired by the camera attached to the robot, with:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper650.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
@@ -189,7 +193,7 @@ int main()
vpCameraParameters cam;
robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
+ // In cam, you get the intrinsic parameters of the projection model
// with distortion.
#endif
}
@@ -200,10 +204,10 @@ int main()
frame like here in the joint space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -234,10 +238,10 @@ int main()
velocity used to reach the desired position.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -266,10 +270,10 @@ int main()
space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -301,13 +305,13 @@ int main()
}
\endcode
- It is also possible to specify the position of a custom tool cartesian 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
+ It is also possible to specify the position of a custom tool cartesian
+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 <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpRobotViper650.h>
@@ -333,19 +337,16 @@ int main()
positions from a position file with readPosFile() and savePosFile()
methods.
*/
-class VISP_EXPORT vpRobotViper650
- :
- public vpViper650,
- public vpRobot
+class VISP_EXPORT vpRobotViper650 : public vpViper650, public vpRobot
{
-public: /* Constantes */
-
+public: /* Constantes */
/*! \enum vpControlModeType Control mode. */
typedef enum {
AUTO, //!< Automatic control mode (default).
- MANUAL, //!< Manual control mode activated when the dead man switch is in use.
- ESTOP //!< Emergency stop activated.
+ MANUAL, //!< Manual control mode activated when the dead man switch is in
+ //!< use.
+ ESTOP //!< Emergency stop activated.
} vpControlModeType;
/* Vitesse maximale par default lors du positionnement du robot.
@@ -356,14 +357,12 @@ public: /* Constantes */
static const double defaultPositioningVelocity; // = 20.0;
private: /* Not allowed functions. */
-
/*!
Copy constructor not allowed.
*/
- vpRobotViper650 (const vpRobotViper650 & robot);
+ vpRobotViper650(const vpRobotViper650 &robot);
private: /* Attributs prives. */
-
/** \brief Vrai ssi aucun objet de la classe vpRobotViper650 n'existe.
*
* Il ne peut exister simultanement qu'un seul objet de la classe
@@ -389,11 +388,9 @@ private: /* Attributs prives. */
bool first_time_getdis;
vpControlModeType controlMode;
-
-public: /* Methode publiques */
-
- vpRobotViper650 (bool verbose=true);
- virtual ~vpRobotViper650 (void);
+public: /* Methode publiques */
+ explicit vpRobotViper650(bool verbose = true);
+ virtual ~vpRobotViper650(void);
// Force/Torque control
void biasForceTorqueSensor() const;
@@ -407,39 +404,28 @@ public: /* Methode publiques */
\return The control mode indicating if the robot is in automatic,
manual (usage of the dead man switch) or emergnecy stop mode.
*/
- vpControlModeType getControlMode() const {
- return controlMode;
- }
+ 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);
-
- double getPositioningVelocity (void) const;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+
+ double getPositioningVelocity(void) const;
bool getPowerState() const;
- double getTime () const;
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
+ double getTime() const;
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity, double ×tamp);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
void get_cMe(vpHomogeneousMatrix &cMe) const;
void get_cVe(vpVelocityTwistMatrix &cVe) const;
@@ -447,13 +433,12 @@ public: /* Methode publiques */
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_);
-
void move(const std::string &filename);
void openGripper();
@@ -471,25 +456,20 @@ public: /* Methode publiques */
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 std::string &filename) ;
- void setPositioningVelocity (const double velocity);
+ 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);
+ 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();
private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
double maxRotationVelocity_joint6;
};
diff --git a/modules/robot/include/visp3/robot/vpRobotViper850.h b/modules/robot/include/visp3/robot/vpRobotViper850.h
index 19cfb63..ce89d08 100644
--- a/modules/robot/include/visp3/robot/vpRobotViper850.h
+++ b/modules/robot/include/visp3/robot/vpRobotViper850.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Interface for the Irisa's Viper S850 robot controlled by an Adept MotionBlox.
+ * Interface for the Irisa's Viper S850 robot controlled by an Adept
+ *MotionBlox.
*
* Authors:
* Fabien Spindler
@@ -45,22 +47,23 @@
#include <iostream>
#include <stdio.h>
-#include <visp3/robot/vpRobot.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpViper850.h>
// low level controller api
extern "C" {
-# include "irisa_Viper850.h"
-# include "trycatch.h"
+#include "irisa_Viper850.h"
+#include "trycatch.h"
}
-// If USE_ATI_DAQ defined, use DAQ board instead of serial connexion to acquire data using comedi
+// If USE_ATI_DAQ defined, use DAQ board instead of serial connexion to
+// acquire data using comedi
#define USE_ATI_DAQ
#ifdef USE_ATI_DAQ
-# include <visp3/sensor/vpForceTorqueAtiSensor.h>
+#include <visp3/sensor/vpForceTorqueAtiSensor.h>
#endif
/*!
@@ -111,18 +114,21 @@ extern "C" {
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool vpViper850::TOOL_CUSTOM and set this
- during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool
+vpViper850::TOOL_CUSTOM and set this during robot initialisation or using
+set_eMc().
- \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
This class allows to control the Viper650 arm robot in position
and velocity:
- in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame \f$ {\cal F}_f \f$ (vpRobot::REFERENCE_FRAME),
+ - in the fixed reference frame \f$ {\cal F}_f \f$
+(vpRobot::REFERENCE_FRAME),
- in the camera or tool frame \f$ {\cal F}_c \f$ (vpRobot::CAMERA_FRAME),
- or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
- in the reference frame \f$ {\cal F}_f \f$ and rotations in the camera or tool frame \f$ {\cal F}_c \f$ .
+ in the reference frame \f$ {\cal F}_f \f$ and rotations in the camera or
+tool frame \f$ {\cal F}_c \f$ .
All the translations are expressed in meters for positions and m/s
for the velocities. Rotations are expressed in radians for the
@@ -164,10 +170,9 @@ int main()
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // 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
@@ -176,11 +181,11 @@ int main()
acquired by the camera attached to the robot, with:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
@@ -195,7 +200,7 @@ int main()
vpCameraParameters cam;
robot.getCameraParameters(cam, I);
- // In cam, you get the intrinsic parameters of the projection model
+ // In cam, you get the intrinsic parameters of the projection model
// with distortion.
#endif
}
@@ -206,10 +211,10 @@ int main()
frame like here in the joint space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -240,10 +245,10 @@ int main()
velocity used to reach the desired position.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -272,10 +277,10 @@ int main()
space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -307,15 +312,13 @@ int main()
}
\endcode
- It is also possible to specify the position of a custom tool cartesian 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(vpViper850::vpToolType, const vpHomogeneousMatrix &) or
- init(vpViper850::vpToolType, const std::string &) or using set_eMc(). The following example illustrates
- this usecase:
- \code
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/robot/vpRobotViper850.h>
+ It is also possible to specify the position of a custom tool cartesian
+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(vpViper850::vpToolType, const vpHomogeneousMatrix &)
+or init(vpViper850::vpToolType, const std::string &) or using set_eMc(). The
+following example illustrates this usecase: \code #include
+<visp3/core/vpHomogeneousMatrix.h> #include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -339,19 +342,16 @@ int main()
positions from a position file with readPosFile() and savePosFile()
methods.
*/
-class VISP_EXPORT vpRobotViper850
- :
- public vpViper850,
- public vpRobot
+class VISP_EXPORT vpRobotViper850 : public vpViper850, public vpRobot
{
-public: /* Constantes */
-
+public: /* Constantes */
/*! \enum vpControlModeType Control mode. */
typedef enum {
AUTO, //!< Automatic control mode (default).
- MANUAL, //!< Manual control mode activated when the dead man switch is in use.
- ESTOP //!< Emergency stop activated.
+ MANUAL, //!< Manual control mode activated when the dead man switch is in
+ //!< use.
+ ESTOP //!< Emergency stop activated.
} vpControlModeType;
/* Vitesse maximale par default lors du positionnement du robot.
@@ -362,14 +362,12 @@ public: /* Constantes */
static const double defaultPositioningVelocity; // = 20.0;
private: /* Not allowed functions. */
-
/*!
Copy constructor not allowed.
*/
- vpRobotViper850 (const vpRobotViper850 & robot);
+ vpRobotViper850(const vpRobotViper850 &robot);
private: /* Attributs prives. */
-
/** \brief Vrai ssi aucun objet de la classe vpRobotViper850 n'existe.
*
* Il ne peut exister simultanement qu'un seul objet de la classe
@@ -399,10 +397,9 @@ private: /* Attributs prives. */
vpForceTorqueAtiSensor ati;
#endif
-public: /* Methode publiques */
-
- vpRobotViper850 (bool verbose=true);
- virtual ~vpRobotViper850 (void);
+public: /* Methode publiques */
+ explicit vpRobotViper850(bool verbose = true);
+ virtual ~vpRobotViper850(void);
// Force/Torque control
void biasForceTorqueSensor();
@@ -412,39 +409,30 @@ public: /* Methode publiques */
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,
+ \return The control mode indicating if the robot is in automatic,
manual (usage of the dead man switch) or emergnecy stop mode.
*/
- vpControlModeType getControlMode() const {
- return controlMode;
- }
+ vpControlModeType getControlMode() const { return controlMode; }
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);
-
- double getPositioningVelocity (void) const;
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+
+ double getPositioningVelocity(void) const;
bool getPowerState() const;
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity);
- void getVelocity (const vpRobot::vpControlFrameType frame,
- vpColVector & velocity, double ×tamp);
+ 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);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
double getTime() const;
@@ -454,21 +442,21 @@ public: /* Methode publiques */
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_);
- void move(const std::string &filename) ;
+ void move(const std::string &filename);
void openGripper();
- void powerOn() ;
- void powerOff() ;
+ void powerOn();
+ void powerOff();
- static bool readPosFile(const std::string &filename, vpColVector &q) ;
- static bool savePosFile(const std::string &filename, const vpColVector &q) ;
+ static bool readPosFile(const std::string &filename, vpColVector &q);
+ static bool savePosFile(const std::string &filename, const vpColVector &q);
void set_eMc(const vpHomogeneousMatrix &eMc_);
void set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_);
@@ -477,28 +465,22 @@ public: /* Methode publiques */
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 std::string &filename) ;
- void setPositioningVelocity (const double velocity);
+ 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);
+ 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();
private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
-
double maxRotationVelocity_joint6;
};
diff --git a/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h
index 5330196..0ca8c39 100644
--- a/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h
+++ b/modules/robot/include/visp3/robot/vpRobotWireFrameSimulator.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,22 +48,25 @@
#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#if defined(_WIN32)
-# include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
+#include <WinSock2.h>
+#include <windows.h>
#elif defined(VISP_HAVE_PTHREAD)
-# include <pthread.h>
+#include <pthread.h>
#endif
-#include <visp3/robot/vpWireFrameSimulator.h>
-#include <visp3/robot/vpRobot.h>
-#include <visp3/robot/vpRobotSimulator.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotSimulator.h>
+#include <visp3/robot/vpWireFrameSimulator.h>
/*!
\class vpRobotWireFrameSimulator
@@ -71,7 +75,7 @@
\brief This class aims to be a basis used to create all the
simulators of robots.
-
+
Thus in this class you will find all the parameters and methods
which are necessary to create a simulator. Several methods are pure
virtual. In this case it means that they are specific to the each
@@ -83,428 +87,477 @@
*/
class VISP_EXPORT vpRobotWireFrameSimulator : protected vpWireFrameSimulator, public vpRobotSimulator
{
- public:
- vpImage<vpRGBa> I;
-
- typedef enum
- {
- MODEL_3D,
- MODEL_DH
- } vpDisplayRobotType;
-
-
- protected:
- /*! cpu time at the begining of the robot's movement*/
- double tcur;
- /*! cpu time at the end of the last robot's movement*/
- double tprev;
-
- /*! Contains the 3D model of the robot's arms*/
- Bound_scene* robotArms;
-
- /*! Size of the fMi table*/
- unsigned int size_fMi;
- /*! Table containing all the homogeneous matrices between the reference frame of the robot and the frames you used to compute the Denavit-Hartenberg representation
-
- If you use a camera at the end of the effector, the last homogeneous matrix has to be the one between the reference frame and the camera frame (fMc)*/
- vpHomogeneousMatrix* fMi;
-
- /*! The articular coordinates*/
- vpColVector artCoord;
- /*! The articular velocity*/
- vpColVector artVel;
- /*! The velocity in the current frame (articular, camera or reference)*/
- vpColVector velocity;
-
-#if defined(_WIN32)
- HANDLE hThread;
- HANDLE mutex_fMi;
- HANDLE mutex_artVel;
- HANDLE mutex_artCoord;
- HANDLE mutex_velocity;
- HANDLE mutex_display;
-#elif defined(VISP_HAVE_PTHREAD)
- pthread_t thread;
- pthread_attr_t attr;
- pthread_mutex_t mutex_fMi;
- pthread_mutex_t mutex_artVel;
- pthread_mutex_t mutex_artCoord;
- pthread_mutex_t mutex_velocity;
- pthread_mutex_t mutex_display;
-#endif
-
- bool displayBusy;
-
- /*! True if the robot has to be stopped*/
- bool robotStop;
- /*! True if one of the joint reach the limit*/
- bool jointLimit;
- /*! Index of the joint which is in limit*/
- unsigned int jointLimitArt;
- /*! True if the singularity are automatically managed */
- bool singularityManagement;
-
- /*! External camera parameters*/
- vpCameraParameters cameraParam;
-
- #if defined VISP_HAVE_X11
- vpDisplayX display;
- #elif defined VISP_HAVE_GDI
- vpDisplayGDI display;
- #elif defined VISP_HAVE_OPENCV
- vpDisplayOpenCV display;
- #elif defined VISP_HAVE_D3D9
- vpDisplayD3D display;
- #elif defined VISP_HAVE_GTK
- vpDisplayGTK display;
- #endif
-
- vpDisplayRobotType displayType;
-
- bool displayAllowed;
- //! Flag used to force the sampling time in the thread computing the robot's displacement to a constant value (\e samplingTime). It may be useful if the main thread (computing the features) is very time consumming. False by default.
- bool constantSamplingTimeMode;
-
- //! Flag used to specify to the thread managing the robot displacements that the setVelocity() method has been called.
- bool setVelocityCalled;
-
- bool verbose_;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpRobotWireFrameSimulator(const vpRobotWireFrameSimulator &)
-// : vpWireFrameSimulator(), vpRobotSimulator(),
-// I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL), artCoord(), artVel(), velocity(),
-// #if defined(_WIN32)
-// #elif defined(VISP_HAVE_PTHREAD)
-// thread(), attr(),
-// #endif
-// mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(), mutex_display(),
-// displayBusy(false), robotStop(false), jointLimit(false), jointLimitArt(false), singularityManagement(true),
-// cameraParam(),
-// #if defined(VISP_HAVE_DISPLAY)
-// display(),
-// #endif
-// displayType(MODEL_3D), displayAllowed(true), constantSamplingTimeMode(false),
-// setVelocityCalled(false), verbose_(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpRobotWireFrameSimulator &operator=(const vpRobotWireFrameSimulator &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
-
- public:
- vpRobotWireFrameSimulator();
- vpRobotWireFrameSimulator(bool display);
- virtual ~vpRobotWireFrameSimulator();
-
- /** @name Inherited functionalities from vpRobotWireFrameSimulator */
- //@{
- /*!
- Get the parameters of the virtual external camera.
-
- \return It returns the camera parameters.
- */
- vpCameraParameters getExternalCameraParameters() const {
- //if(px_ext != 1 && py_ext != 1)
- // we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
- }
- }
- /*!
- Get the external camera's position relative to the the world reference frame.
+public:
+ vpImage<vpRGBa> I;
- \return the main external camera position relative to the the world reference frame.
- */
- vpHomogeneousMatrix getExternalCameraPosition() const
- {
- return this->vpWireFrameSimulator::getExternalCameraPosition();
- }
+ typedef enum { MODEL_3D, MODEL_DH } vpDisplayRobotType;
- void getInternalView(vpImage<vpRGBa> &I);
- void getInternalView(vpImage<unsigned char> &I);
+protected:
+ /*! cpu time at the begining of the robot's movement*/
+ double tcur;
+ /*! cpu time at the end of the last robot's movement*/
+ double tprev;
- vpHomogeneousMatrix get_cMo();
- /*!
- Get the pose between the object and the fixed world frame.
+ /*! Contains the 3D model of the robot's arms*/
+ Bound_scene *robotArms;
- \return The pose between the object and the fixed world frame.
- */
- vpHomogeneousMatrix get_fMo() const {return fMo;}
+ /*! Size of the fMi table*/
+ unsigned int size_fMi;
+ /*! Table containing all the homogeneous matrices between the reference
+ frame of the robot and the frames you used to compute the Denavit-Hartenberg
+ representation
- /* Display functions */
- void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
- void initScene(const char* obj, const char* desiredObject);
- void initScene (const vpSceneObject &obj);
- void initScene(const char* obj);
+ If you use a camera at the end of the effector, the last homogeneous matrix
+ has to be the one between the reference frame and the camera frame (fMc)*/
+ vpHomogeneousMatrix *fMi;
- /*!
- Set the color used to display the camera in the external view.
+ /*! The articular coordinates*/
+ vpColVector artCoord;
+ /*! The articular velocity*/
+ vpColVector artVel;
+ /*! The velocity in the current frame (articular, camera or reference)*/
+ vpColVector velocity;
- \param col : The desired color.
- */
- void setCameraColor(const vpColor col) {camColor = col;}
+#if defined(_WIN32)
+ HANDLE hThread;
+ HANDLE mutex_fMi;
+ HANDLE mutex_artVel;
+ HANDLE mutex_artCoord;
+ HANDLE mutex_velocity;
+ HANDLE mutex_display;
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_t thread;
+ pthread_attr_t attr;
+ pthread_mutex_t mutex_fMi;
+ pthread_mutex_t mutex_artVel;
+ pthread_mutex_t mutex_artCoord;
+ pthread_mutex_t mutex_velocity;
+ pthread_mutex_t mutex_display;
+#endif
- /*!
- Set the flag used to force the sampling time in the thread computing the
- robot's displacement to a constant value; see setSamplingTime(). It may be
- useful if the main thread (computing the features) is very time consuming.
- False by default.
+ bool displayBusy;
+
+ /*! True if the robot has to be stopped*/
+ bool robotStop;
+ /*! True if one of the joint reach the limit*/
+ bool jointLimit;
+ /*! Index of the joint which is in limit*/
+ unsigned int jointLimitArt;
+ /*! True if the singularity are automatically managed */
+ bool singularityManagement;
+
+ /*! External camera parameters*/
+ vpCameraParameters cameraParam;
+
+#if defined VISP_HAVE_X11
+ vpDisplayX display;
+#elif defined VISP_HAVE_GDI
+ vpDisplayGDI display;
+#elif defined VISP_HAVE_OPENCV
+ vpDisplayOpenCV display;
+#elif defined VISP_HAVE_D3D9
+ vpDisplayD3D display;
+#elif defined VISP_HAVE_GTK
+ vpDisplayGTK display;
+#endif
- \param _constantSamplingTimeMode : The new value of the constantSamplingTimeMode flag.
- */
- inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode){
- constantSamplingTimeMode = _constantSamplingTimeMode;
+ vpDisplayRobotType displayType;
+
+ bool displayAllowed;
+ //! Flag used to force the sampling time in the thread computing the robot's
+ //! displacement to a constant value (\e samplingTime). It may be useful if
+ //! the main thread (computing the features) is very time consumming. False
+ //! by default.
+ bool constantSamplingTimeMode;
+
+ //! Flag used to specify to the thread managing the robot displacements that
+ //! the setVelocity() method has been called.
+ bool setVelocityCalled;
+
+ bool verbose_;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpRobotWireFrameSimulator(const vpRobotWireFrameSimulator &)
+ // : vpWireFrameSimulator(), vpRobotSimulator(),
+ // I(), tcur(0), tprev(0), robotArms(NULL), size_fMi(8), fMi(NULL),
+ // artCoord(), artVel(), velocity(),
+ // #if defined(_WIN32)
+ // #elif defined(VISP_HAVE_PTHREAD)
+ // thread(), attr(),
+ // #endif
+ // mutex_fMi(), mutex_artVel(), mutex_artCoord(), mutex_velocity(),
+ // mutex_display(), displayBusy(false), robotStop(false),
+ // jointLimit(false), jointLimitArt(false),
+ // singularityManagement(true), cameraParam(),
+ // #if defined(VISP_HAVE_DISPLAY)
+ // display(),
+ // #endif
+ // displayType(MODEL_3D), displayAllowed(true),
+ // constantSamplingTimeMode(false), setVelocityCalled(false),
+ // verbose_(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpRobotWireFrameSimulator &operator=(const vpRobotWireFrameSimulator
+ // &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
+
+public:
+ vpRobotWireFrameSimulator();
+ explicit vpRobotWireFrameSimulator(bool display);
+ virtual ~vpRobotWireFrameSimulator();
+
+ /** @name Inherited functionalities from vpRobotWireFrameSimulator */
+ //@{
+ /*!
+ Get the parameters of the virtual external camera.
+
+ \return It returns the camera parameters.
+ */
+ vpCameraParameters getExternalCameraParameters() const
+ {
+ // if(px_ext != 1 && py_ext != 1)
+ // we assume px_ext and py_ext > 0
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
+ else {
+ unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
+ return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
}
-
- /*!
- Set the color used to display the object at the current position in the robot's camera view.
-
- \param col : The desired color.
+ }
+ /*!
+ Get the external camera's position relative to the the world reference
+ frame.
+
+ \return the main external camera position relative to the the world
+ reference frame.
+ */
+ vpHomogeneousMatrix getExternalCameraPosition() const
+ {
+ return this->vpWireFrameSimulator::getExternalCameraPosition();
+ }
+
+ void getInternalView(vpImage<vpRGBa> &I);
+ void getInternalView(vpImage<unsigned char> &I);
+
+ vpHomogeneousMatrix get_cMo();
+ /*!
+ Get the pose between the object and the fixed world frame.
+
+ \return The pose between the object and the fixed world frame.
+ */
+ vpHomogeneousMatrix get_fMo() const { return fMo; }
+
+ /* Display functions */
+ void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
+ void initScene(const char *obj, const char *desiredObject);
+ void initScene(const vpSceneObject &obj);
+ void initScene(const char *obj);
+
+ /*!
+ Set the color used to display the camera in the external view.
+
+ \param col : The desired color.
+ */
+ void setCameraColor(const vpColor &col) { camColor = col; }
+
+ /*!
+ Set the flag used to force the sampling time in the thread computing the
+ robot's displacement to a constant value; see setSamplingTime(). It may be
+ useful if the main thread (computing the features) is very time consuming.
+ False by default.
+
+ \param _constantSamplingTimeMode : The new value of the
+ constantSamplingTimeMode flag.
+ */
+ inline void setConstantSamplingTimeMode(const bool _constantSamplingTimeMode)
+ {
+ constantSamplingTimeMode = _constantSamplingTimeMode;
+ }
+
+ /*!
+ Set the color used to display the object at the current position in the
+ robot's camera view.
+
+ \param col : The desired color.
+ */
+ void setCurrentViewColor(const vpColor &col) { curColor = col; }
+
+ /*!
+ Set the color used to display the object at the desired position in the
+ robot's camera view.
+
+ \param col : The desired color.
+ */
+ void setDesiredViewColor(const vpColor &col) { desColor = col; }
+
+ /*!
+ Set the desired position of the robot's camera relative to the object.
+
+ \param cdMo_ : The desired pose of the camera.
+ */
+ void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
+ {
+ this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
+ }
+
+ /*!
+ Set the way to draw the robot. Depending on what you choose you can
+ display a 3D wireframe model or a set of lines linking the frames used to
+ compute the geometrical model.
+
+ \param dispType : Type of display. Can be MODEL_3D or MODEL_DH.
+ */
+ inline void setDisplayRobotType(const vpDisplayRobotType dispType) { displayType = dispType; }
+ /*!
+ Set the external camera point of view.
+
+ \param camMf_ : The pose of the external camera relative to the world
+ reference frame.
+ */
+ void setExternalCameraPosition(const vpHomogeneousMatrix &camMf_)
+ {
+ this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
+ }
+ /*!
+ Specify the thickness of the graphics drawings.
*/
- void setCurrentViewColor(const vpColor col) {curColor = col;}
+ void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
- /*!
- Set the color used to display the object at the desired position in the robot's camera view.
+ /*!
+ Set the sampling time.
- \param col : The desired color.
- */
- void setDesiredViewColor(const vpColor col) {desColor = col;}
+ \param delta_t : Sampling time in second used to compute the robot
+ displacement from the velocity applied to the robot during this time.
- /*!
- Set the desired position of the robot's camera relative to the object.
+ Since the wireframe simulator is threaded, the sampling time is set to
+ vpTime::getMinTimeForUsleepCall() / 1000 seconds.
- \param cdMo_ : The desired pose of the camera.
- */
- void setDesiredCameraPosition(const vpHomogeneousMatrix cdMo_)
- {
- this->vpWireFrameSimulator::setDesiredCameraPosition(cdMo_);
+ */
+ inline void setSamplingTime(const double &delta_t)
+ {
+ if (delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)) {
+ this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
+ } else {
+ this->delta_t_ = delta_t;
}
+ }
+ /*! Set the parameter which enable or disable the singularity mangement */
+ void setSingularityManagement(const bool sm) { singularityManagement = sm; }
- /*!
- Set the way to draw the robot. Depending on what you choose you can display a 3D wireframe model or a set of lines linking the frames used to compute the geometrical model.
-
- \param dispType : Type of display. Can be MODEL_3D or MODEL_DH.
- */
- inline void setDisplayRobotType (const vpDisplayRobotType dispType) {displayType = dispType;}
- /*!
- Set the external camera point of view.
-
- \param camMf_ : The pose of the external camera relative to the world reference frame.
+ /*!
+ Activates extra printings when the robot reaches joint limits...
*/
- void setExternalCameraPosition(const vpHomogeneousMatrix camMf_)
- {
- this->vpWireFrameSimulator::setExternalCameraPosition(camMf_);
- }
- /*!
- Specify the thickness of the graphics drawings.
- */
- void setGraphicsThickness(unsigned int thickness)
- {
- this->thickness_ = thickness;
- }
+ void setVerbose(bool verbose) { this->verbose_ = verbose; }
- /*!
- Set the sampling time.
+ /*!
+ Set the pose between the object and the fixed world frame.
- \param delta_t : Sampling time in second used to compute the robot displacement from
- the velocity applied to the robot during this time.
+ \param fMo_ : The pose between the object and the fixed world frame.
+ */
+ void set_fMo(const vpHomogeneousMatrix &fMo_) { this->fMo = fMo_; }
+ //@}
- Since the wireframe simulator is threaded, the sampling time is set to vpTime::getMinTimeForUsleepCall() / 1000 seconds.
+protected:
+/** @name Protected Member Functions Inherited from vpRobotWireFrameSimulator
+ */
+//@{
+/*!
+ Function used to launch the thread which moves the robot.
+*/
+#if defined(_WIN32)
+ static DWORD WINAPI launcher(LPVOID lpParam)
+ {
+ (static_cast<vpRobotWireFrameSimulator *>(lpParam))->updateArticularPosition();
+ return 0;
+ }
+#elif defined(VISP_HAVE_PTHREAD)
+ static void *launcher(void *arg)
+ {
+ (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
+ // pthread_exit((void*) 0);
+ return NULL;
+ }
+#endif
- */
- inline void setSamplingTime(const double &delta_t)
- {
- if(delta_t < static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3)){
- this->delta_t_ = static_cast<float>(vpTime::getMinTimeForUsleepCall() * 1e-3);
- } else {
- this->delta_t_ = delta_t;
- }
- }
- /*! Set the parameter which enable or disable the singularity mangement */
- void setSingularityManagement (const bool sm) {singularityManagement = sm;}
-
- /*!
- Activates extra printings when the robot reaches joint limits...
- */
- void setVerbose(bool verbose)
- {
- this->verbose_ = verbose;
- }
+ /* Robot functions */
+ void init() { ; }
+ /*! Method lauched by the thread to compute the position of the robot in the
+ * articular frame. */
+ virtual void updateArticularPosition() = 0;
+ /*! Method used to check if the robot reached a joint limit. */
+ virtual int isInJointLimit() = 0;
+ /*! Compute the articular velocity relative to the velocity in another
+ * frame. */
+ virtual void computeArticularVelocity() = 0;
+
+ /* Display functions */
+ void initDisplay() { ; }
+ virtual void initArms() = 0;
- /*!
- Set the pose between the object and the fixed world frame.
-
- \param fMo_ : The pose between the object and the fixed world frame.
- */
- void set_fMo(const vpHomogeneousMatrix &fMo_) {this->fMo = fMo_;}
- //@}
-
- protected:
- /** @name Protected Member Functions Inherited from vpRobotWireFrameSimulator */
- //@{
- /*!
- Function used to launch the thread which moves the robot.
- */
#if defined(_WIN32)
- static DWORD WINAPI launcher( LPVOID lpParam )
- {
- ((vpRobotWireFrameSimulator *)lpParam)->updateArticularPosition();
- return 0;
- }
+ vpColVector get_artCoord() const
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_artCoord, INFINITE);
+#endif
+ vpColVector artCoordTmp(6);
+ artCoordTmp = artCoord;
+ ReleaseMutex(mutex_artCoord);
+ return artCoordTmp;
+ }
+ void set_artCoord(const vpColVector &coord)
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_artCoord, INFINITE);
+#endif
+ artCoord = coord;
+ ReleaseMutex(mutex_artCoord);
+ }
+
+ vpColVector get_artVel() const
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_artVel, INFINITE);
+#endif
+ vpColVector artVelTmp(artVel);
+ ReleaseMutex(mutex_artVel);
+ return artVelTmp;
+ }
+ void set_artVel(const vpColVector &vel)
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_artVel, INFINITE);
+#endif
+ artVel = vel;
+ ReleaseMutex(mutex_artVel);
+ }
+
+ vpColVector get_velocity()
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_velocity, INFINITE);
+#endif
+ vpColVector velocityTmp = velocity;
+ ReleaseMutex(mutex_velocity);
+ return velocityTmp;
+ }
+ void set_velocity(const vpColVector &vel)
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_velocity, INFINITE);
+#endif
+ velocity = vel;
+ ReleaseMutex(mutex_velocity);
+ }
+
+ void set_displayBusy(const bool &status)
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_display, INFINITE);
+#endif
+ displayBusy = status;
+ ReleaseMutex(mutex_display);
+ }
+ bool get_displayBusy()
+ {
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_display, INFINITE);
+#endif
+ bool status = displayBusy;
+ if (!displayBusy)
+ displayBusy = true;
+ ReleaseMutex(mutex_display);
+ return status;
+ }
+
#elif defined(VISP_HAVE_PTHREAD)
- static void* launcher(void *arg)
- {
- (reinterpret_cast<vpRobotWireFrameSimulator *>(arg))->updateArticularPosition();
- // pthread_exit((void*) 0);
- return NULL;
- }
+ vpColVector get_artCoord()
+ {
+ pthread_mutex_lock(&mutex_artCoord);
+ vpColVector artCoordTmp(6);
+ artCoordTmp = artCoord;
+ pthread_mutex_unlock(&mutex_artCoord);
+ return artCoordTmp;
+ }
+ void set_artCoord(const vpColVector &coord)
+ {
+ pthread_mutex_lock(&mutex_artCoord);
+ artCoord = coord;
+ pthread_mutex_unlock(&mutex_artCoord);
+ }
+
+ vpColVector get_artVel()
+ {
+ pthread_mutex_lock(&mutex_artVel);
+ vpColVector artVelTmp(artVel);
+ pthread_mutex_unlock(&mutex_artVel);
+ return artVelTmp;
+ }
+ void set_artVel(const vpColVector &vel)
+ {
+ pthread_mutex_lock(&mutex_artVel);
+ artVel = vel;
+ pthread_mutex_unlock(&mutex_artVel);
+ }
+
+ vpColVector get_velocity()
+ {
+ pthread_mutex_lock(&mutex_velocity);
+ vpColVector velocityTmp = velocity;
+ pthread_mutex_unlock(&mutex_velocity);
+ return velocityTmp;
+ }
+ void set_velocity(const vpColVector &vel)
+ {
+ pthread_mutex_lock(&mutex_velocity);
+ velocity = vel;
+ pthread_mutex_unlock(&mutex_velocity);
+ }
+
+ void set_displayBusy(const bool &status)
+ {
+ pthread_mutex_lock(&mutex_display);
+ displayBusy = status;
+ pthread_mutex_unlock(&mutex_display);
+ }
+ bool get_displayBusy()
+ {
+ pthread_mutex_lock(&mutex_display);
+ bool status = displayBusy;
+ if (!displayBusy)
+ displayBusy = true;
+ pthread_mutex_unlock(&mutex_display);
+ return status;
+ }
#endif
-
- /* Robot functions */
- void init() {;}
- /*! Method lauched by the thread to compute the position of the robot in the articular frame. */
- virtual void updateArticularPosition() = 0;
- /*! Method used to check if the robot reached a joint limit. */
- virtual int isInJointLimit () = 0;
- /*! Compute the articular velocity relative to the velocity in another frame. */
- virtual void computeArticularVelocity() = 0;
-
- /* Display functions */
- void initDisplay() {;}
- virtual void initArms() = 0;
-
- #if defined(_WIN32)
- vpColVector get_artCoord() const {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_artCoord,INFINITE);
-# endif
- vpColVector artCoordTmp (6);
- artCoordTmp = artCoord;
- ReleaseMutex(mutex_artCoord);
- return artCoordTmp;}
- void set_artCoord(const vpColVector &coord) {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_artCoord, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_artCoord, INFINITE);
-# endif
- artCoord = coord;
- ReleaseMutex(mutex_artCoord);}
-
- vpColVector get_artVel() const {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_artVel, INFINITE);
-# endif
- vpColVector artVelTmp (artVel);
- ReleaseMutex(mutex_artVel);
- return artVelTmp;}
- void set_artVel(const vpColVector &vel) {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_artVel, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_artVel, INFINITE);
-# endif
- artVel = vel;
- ReleaseMutex(mutex_artVel);}
-
- vpColVector get_velocity() {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_velocity, INFINITE);
-# endif
- vpColVector velocityTmp = velocity;
- ReleaseMutex(mutex_velocity);
- return velocityTmp;}
- void set_velocity(const vpColVector &vel) {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_velocity, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_velocity, INFINITE);
-# endif
- velocity = vel;
- ReleaseMutex(mutex_velocity);}
-
- void set_displayBusy (const bool &status) {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_display, INFINITE);
-# endif
- displayBusy = status;
- ReleaseMutex(mutex_display);}
- bool get_displayBusy () {
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_display, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_display, INFINITE);
-# endif
- bool status = displayBusy;
- if (!displayBusy) displayBusy = true;
- ReleaseMutex(mutex_display);
- return status;}
-
- #elif defined(VISP_HAVE_PTHREAD)
- vpColVector get_artCoord() {
- pthread_mutex_lock (&mutex_artCoord);
- vpColVector artCoordTmp (6);
- artCoordTmp = artCoord;
- pthread_mutex_unlock (&mutex_artCoord);
- return artCoordTmp;}
- void set_artCoord(const vpColVector &coord) {
- pthread_mutex_lock (&mutex_artCoord);
- artCoord = coord;
- pthread_mutex_unlock (&mutex_artCoord);}
-
- vpColVector get_artVel() {
- pthread_mutex_lock (&mutex_artVel);
- vpColVector artVelTmp (artVel);
- pthread_mutex_unlock (&mutex_artVel);
- return artVelTmp;}
- void set_artVel(const vpColVector &vel) {
- pthread_mutex_lock (&mutex_artVel);
- artVel = vel;
- pthread_mutex_unlock (&mutex_artVel);}
-
- vpColVector get_velocity() {
- pthread_mutex_lock (&mutex_velocity);
- vpColVector velocityTmp = velocity;
- pthread_mutex_unlock (&mutex_velocity);
- return velocityTmp;}
- void set_velocity(const vpColVector &vel) {
- pthread_mutex_lock (&mutex_velocity);
- velocity = vel;
- pthread_mutex_unlock (&mutex_velocity);}
-
- void set_displayBusy (const bool &status) {
- pthread_mutex_lock (&mutex_display);
- displayBusy = status;
- pthread_mutex_unlock (&mutex_display);}
- bool get_displayBusy () {
- pthread_mutex_lock (&mutex_display);
- bool status = displayBusy;
- if (!displayBusy) displayBusy = true;
- pthread_mutex_unlock (&mutex_display);
- return status;}
- #endif
-
- /*! Get a table of poses between the reference frame and the frames you used to compute the Denavit-Hartenberg representation */
- virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
- //@}
+
+ /*! Get a table of poses between the reference frame and the frames you used
+ * to compute the Denavit-Hartenberg representation */
+ virtual void get_fMi(vpHomogeneousMatrix *fMit) = 0;
+ //@}
};
#endif
diff --git a/modules/robot/include/visp3/robot/vpServolens.h b/modules/robot/include/visp3/robot/vpServolens.h
index d41bd96..0f487fd 100644
--- a/modules/robot/include/visp3/robot/vpServolens.h
+++ b/modules/robot/include/visp3/robot/vpServolens.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,7 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Interface for the Servolens lens attached to the camera fixed on the
+ * Interface for the Servolens lens attached to the camera fixed on the
* Afma4 robot.
*
* Authors:
@@ -47,7 +48,7 @@
\file vpServolens.h
- Interface for the Servolens lens attached to the camera fixed on the
+ Interface for the Servolens lens attached to the camera fixed on the
Afma4 robot.
*/
@@ -61,7 +62,7 @@
\ingroup group_robot_real_cylindrical
- \brief Interface for the Servolens lens attached to the camera fixed on the
+ \brief Interface for the Servolens lens attached to the camera fixed on the
Afma4 robot.
The code below shows how to manipulate this class to get and modify
@@ -74,7 +75,7 @@
int main()
{
// Open the serial device to communicate with the Servolens lens
- vpServolens servolens("/dev/ttyS0");
+ vpServolens servolens("/dev/ttyS0");
// Get the current zoom position
unsigned zoom;
@@ -82,7 +83,7 @@ int main()
std::cout << "Actual zoom value: " << zoom << std::endl;
// Set a new zoom value
- servolens.setPosition(vpServolens::ZOOM, zoom+1000);
+ servolens.setPosition(vpServolens::ZOOM, zoom+1000);
}
\endcode
@@ -90,31 +91,23 @@ int main()
class VISP_EXPORT vpServolens
{
- public:
+public:
+ typedef enum { ZOOM = 1, FOCUS = 2, IRIS = 3 } vpServoType;
typedef enum {
- ZOOM = 1,
- FOCUS= 2,
- IRIS = 3
- } vpServoType;
- typedef enum {
- ZOOM_MAX = 10000, // Valeur maxi zoom (mm/100)
- ZOOM_MIN = 1000, // Valeur mini zoom (mm/100)
- FOCUS_MAX = 1500, // Valeur maxi focus (metres/100)
- FOCUS_MIN = 100, // Valeur mini focus (metres/100)
- IRIS_MAX = 1000, // Valeur maxi diaph (ouverture/100)
- IRIS_MIN = 160 // Valeur mini disph (ouverture/100)
+ ZOOM_MAX = 10000, // Valeur maxi zoom (mm/100)
+ ZOOM_MIN = 1000, // Valeur mini zoom (mm/100)
+ FOCUS_MAX = 1500, // Valeur maxi focus (metres/100)
+ FOCUS_MIN = 100, // Valeur mini focus (metres/100)
+ IRIS_MAX = 1000, // Valeur maxi diaph (ouverture/100)
+ IRIS_MIN = 160 // Valeur mini disph (ouverture/100)
} vpLimitsType;
- typedef enum {
- AUTO = 1,
- CONTROLLED = 2,
- RELEASED = 3
- } vpControllerType;
+ typedef enum { AUTO = 1, CONTROLLED = 2, RELEASED = 3 } vpControllerType;
vpServolens();
- vpServolens(const char *port);
+ explicit vpServolens(const char *port);
~vpServolens();
- void open(const char *port="/dev/ttyS0");
+ void open(const char *port = "/dev/ttyS0");
void close();
void reset() const;
@@ -126,12 +119,9 @@ class VISP_EXPORT vpServolens
void enablePrompt(bool active) const;
- private:
-
+private:
void init() const;
- void enableCmdComplete(vpServoType servo, bool active) const;
-
char wait() const;
void wait(vpServoType servo) const;
@@ -146,4 +136,3 @@ class VISP_EXPORT vpServolens
#endif
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpSimulatorAfma6.h b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h
index e3e4c9c..704c143 100644
--- a/modules/robot/include/visp3/robot/vpSimulatorAfma6.h
+++ b/modules/robot/include/visp3/robot/vpSimulatorAfma6.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,8 +44,8 @@
\brief Class which provides a simulator for the robot Afma6.
*/
-#include <visp3/robot/vpRobotWireFrameSimulator.h>
#include <visp3/robot/vpAfma6.h>
+#include <visp3/robot/vpRobotWireFrameSimulator.h>
#include <string>
@@ -57,9 +58,9 @@
\brief Simulator of Irisa's gantry robot named Afma6.
- Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
- Afma6 robot. This robot is a gantry robot with six degrees of
- freedom manufactured in 1992 by the french Afma-Robots company.
+ Implementation of the vpRobotWireFrameSimulator class in order to simulate
+Irisa's Afma6 robot. This robot is a gantry robot with six degrees of freedom
+manufactured in 1992 by the french Afma-Robots company.
\warning This class uses threading capabilities. Thus on Unix-like
platforms, the libpthread third-party library need to be
@@ -67,10 +68,10 @@
This class allows to control the Afma6 gantry robot in position
and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
in the reference frame and rotations in the camera frame.
All the translations are expressed in meters for positions and m/s
@@ -85,15 +86,15 @@
frame like here in the joint space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
int main()
{
vpSimulatorAfma6 robot;
-
+
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
vpColVector q(6);
@@ -110,7 +111,7 @@ int main()
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
+
return 0;
}
\endcode
@@ -122,15 +123,15 @@ int main()
space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
int main()
{
vpSimulatorAfma6 robot;
-
+
robot.init(vpAfma6::TOOL_GRIPPER, vpCameraParameters::perspectiveProjWithoutDistortion);
vpColVector qvel(6);
@@ -154,7 +155,7 @@ int main()
// Stop the robot
robot.setRobotState(vpRobot::STATE_STOP);
-
+
return 0;
}
\endcode
@@ -172,115 +173,104 @@ int main()
*/
-
class VISP_EXPORT vpSimulatorAfma6 : public vpRobotWireFrameSimulator, public vpAfma6
{
- public:
- static const double defaultPositioningVelocity;
-
- private:
- vpColVector q_prev_getdis;
- bool first_time_getdis;
-
- double positioningVelocity;
-
- vpColVector zeroPos;
- vpColVector reposPos;
-
- bool toolCustom;
- std::string arm_dir;
-
public:
- vpSimulatorAfma6();
- vpSimulatorAfma6(bool display);
- virtual ~vpSimulatorAfma6();
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height);
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I);
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
- void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
- double getPositioningVelocity (void){return positioningVelocity;}
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe);
- void get_cVe(vpVelocityTwistMatrix &cVe);
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (vpAfma6::vpAfma6ToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
- bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
- void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
-
- void move(const char *filename) ;
-
- static bool readPosFile(const 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 setPosition(const char *filename);
- void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
- bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint=NULL, const double &errMax = 0.001);
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
-
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
-
- void stopMotion();
-
+ static const double defaultPositioningVelocity;
+
+private:
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+
+ double positioningVelocity;
+
+ vpColVector zeroPos;
+ vpColVector reposPos;
+
+ bool toolCustom;
+ std::string arm_dir;
+
+public:
+ vpSimulatorAfma6();
+ 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<unsigned char> &I);
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
+ void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+ double getPositioningVelocity(void) { return positioningVelocity; }
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe);
+ void get_cVe(vpVelocityTwistMatrix &cVe);
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void
+ init(vpAfma6::vpAfma6ToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
+ bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
+ void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
+
+ void move(const char *filename);
+
+ static bool readPosFile(const 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 setPosition(const char *filename);
+ void setPositioningVelocity(const double vel) { positioningVelocity = vel; }
+ bool setPosition(const vpHomogeneousMatrix &cdMo, vpImage<unsigned char> *Iint = NULL, const double &errMax = 0.001);
+ vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState);
+
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
+
+ void stopMotion();
+
protected:
- /** @name Protected Member Functions Inherited from vpSimulatorAfma6 */
- //@{
- void computeArticularVelocity();
- void compute_fMi();
- void findHighestPositioningSpeed(vpColVector &q);
- void getExternalImage(vpImage<vpRGBa> &I);
- inline void get_fMi(vpHomogeneousMatrix *fMit) {
+ /** @name Protected Member Functions Inherited from vpSimulatorAfma6 */
+ //@{
+ void computeArticularVelocity();
+ void compute_fMi();
+ void findHighestPositioningSpeed(vpColVector &q);
+ void getExternalImage(vpImage<vpRGBa> &I);
+ inline void get_fMi(vpHomogeneousMatrix *fMit)
+ {
#if defined(_WIN32)
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_fMi, INFINITE);
-# endif
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- ReleaseMutex(mutex_fMi);
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_fMi, INFINITE);
+#endif
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ ReleaseMutex(mutex_fMi);
#elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- pthread_mutex_unlock (&mutex_fMi);
+ pthread_mutex_lock(&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ pthread_mutex_unlock(&mutex_fMi);
#endif
- }
- void init();
- void initArms();
- void initDisplay();
- int isInJointLimit (void);
- bool singularityTest(const vpColVector q, vpMatrix &J);
- void updateArticularPosition();
- //@}
-
-private:
- void getCameraDisplacement(vpColVector &displacement);
- void getArticularDisplacement(vpColVector &displacement);
+ }
+ void init();
+ void initArms();
+ void initDisplay();
+ int isInJointLimit(void);
+ bool singularityTest(const vpColVector &q, vpMatrix &J);
+ void updateArticularPosition();
+ //@}
};
#endif
diff --git a/modules/robot/include/visp3/robot/vpSimulatorCamera.h b/modules/robot/include/visp3/robot/vpSimulatorCamera.h
index d852e91..5f6f812 100644
--- a/modules/robot/include/visp3/robot/vpSimulatorCamera.h
+++ b/modules/robot/include/visp3/robot/vpSimulatorCamera.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpSimulatorCamera_H
#define vpSimulatorCamera_H
@@ -60,12 +60,13 @@
This free flying camera has 6 dof; 3 in translation and 3 in rotation.
It evolves as a gentry robot with respect to a world frame. This class
is similar to vpRobotCamera class except that here the position of the robot
- is provided as the transformation from world frame to camera frame; wMc. This
- representation is more intuitive than the one implemented in vpRobotCamera
- where the transformation from camera to world frame is considered; cMw.
+ is provided as the transformation from world frame to camera frame; wMc.
+This representation is more intuitive than the one implemented in
+vpRobotCamera where the transformation from camera to world frame is
+considered; cMw.
- For this particular simulated robot, the end-effector and camera frame are confused.
- That means that the cMe transformation is equal to identity.
+ For this particular simulated robot, the end-effector and camera frame are
+confused. That means that the cMe transformation is equal to identity.
The robot jacobian expressed in the end-effector frame
\f$ {^e}{\bf J}_e \f$ is also set to identity (see get_eJe()).
@@ -109,8 +110,8 @@ protected:
vpHomogeneousMatrix wMc_; // world to camera
public:
- vpSimulatorCamera() ;
- virtual ~vpSimulatorCamera() ;
+ vpSimulatorCamera();
+ virtual ~vpSimulatorCamera();
public:
/** @name Inherited functionalities from vpSimulatorCamera */
@@ -122,17 +123,16 @@ public:
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:
- void init() ;
+ void init();
// Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
+ void get_fJe(vpMatrix & /*_fJe */){};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */){};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */){};
+};
#endif
diff --git a/modules/robot/include/visp3/robot/vpSimulatorPioneer.h b/modules/robot/include/visp3/robot/vpSimulatorPioneer.h
index f222e97..15cedce 100644
--- a/modules/robot/include/visp3/robot/vpSimulatorPioneer.h
+++ b/modules/robot/include/visp3/robot/vpSimulatorPioneer.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,13 @@
*
*****************************************************************************/
-
#ifndef vpSimulatorPioneer_H
#define vpSimulatorPioneer_H
/*!
\file vpSimulatorPioneer.h
- \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
+ \brief class that defines the Pioneer mobile robot simulator equipped with a
+ static camera.
*/
#include <visp3/core/vpColVector.h>
@@ -56,15 +57,16 @@
\ingroup group_robot_simu_unicycle
- \brief Class that defines the Pioneer mobile robot simulator equipped with a static camera.
+ \brief Class that defines the Pioneer mobile robot simulator equipped with a
+static camera.
It intends to simulate the mobile robot described in vpPioneer class.
This robot has 2 dof: \f$(v_x, w_z)\f$, the translational and
rotational velocities that are applied at point E.
- The robot position evolves with respect to a world frame; wMc. When a new joint velocity
- is applied to the robot using setVelocity(), the position of the camera wrt the world frame
- is updated.
+ The robot position evolves with respect to a world frame; wMc. When a new
+joint velocity is applied to the robot using setVelocity(), the position of
+the camera wrt the world frame is updated.
\image html pioneer.png
@@ -94,7 +96,8 @@ int main()
}
\endcode
- The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
+ The usage of this class is also highlighted in \ref
+tutorial-simu-robot-pioneer.
*/
class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, public vpRobotSimulator
@@ -102,10 +105,10 @@ class VISP_EXPORT vpSimulatorPioneer : public vpPioneer, public vpRobotSimulator
protected:
// world to camera
- vpHomogeneousMatrix wMc_ ;
+ vpHomogeneousMatrix wMc_;
// world to end effector frame which is also the mobile
// robot frame located between the two wheels
- vpHomogeneousMatrix wMe_ ;
+ vpHomogeneousMatrix wMe_;
// cMe_ is a protected member of vpUnicycle
double xm_;
@@ -113,7 +116,7 @@ protected:
double theta_;
public:
- vpSimulatorPioneer() ;
+ vpSimulatorPioneer();
virtual ~vpSimulatorPioneer();
public:
@@ -123,19 +126,16 @@ public:
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:
- void init() ;
+ void init();
// Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-
-} ;
+ void get_fJe(vpMatrix & /*_fJe */){};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */){};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */){};
+};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h b/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h
index 9813bd0..e7e53a4 100644
--- a/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h
+++ b/modules/robot/include/visp3/robot/vpSimulatorPioneerPan.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpSimulatorPioneerPan_H
#define vpSimulatorPioneerPan_H
@@ -62,11 +62,12 @@
It intends to simulate the mobile robot described in vpPioneerPan class.
This robot has 3 dof: \f$(v_x, w_z, \dot{q_1})\f$, the translational and
- rotational velocities of the mobile platform, the pan head velocity respectively.
+ rotational velocities of the mobile platform, the pan head velocity
+respectively.
- The robot position evolves with respect to a world frame; wMc. When a new joint velocity
- is applied to the robot using setVelocity(), the position of the camera wrt the world frame
- is updated.
+ The robot position evolves with respect to a world frame; wMc. When a new
+joint velocity is applied to the robot using setVelocity(), the position of
+the camera wrt the world frame is updated.
\image html pioneer-pan.png
@@ -96,7 +97,8 @@ int main()
}
\endcode
- The usage of this class is also highlighted in \ref tutorial-simu-robot-pioneer.
+ The usage of this class is also highlighted in \ref
+tutorial-simu-robot-pioneer.
*/
class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, public vpRobotSimulator
@@ -104,8 +106,8 @@ class VISP_EXPORT vpSimulatorPioneerPan : public vpPioneerPan, public vpRobotSim
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 wMc_; // world to camera
+ vpHomogeneousMatrix wMm_; // world to mobile robot frame located between the two weels
// mMp_ mobile robot to pan frame is a protected member of vpPioneerPan
// pMe_ pan head to end effector frame is a protected member of vpPioneerPan
// cMe_ is a protected member of vpUnicycle
@@ -116,7 +118,7 @@ protected:
double q_pan_;
public:
- vpSimulatorPioneerPan() ;
+ vpSimulatorPioneerPan();
virtual ~vpSimulatorPioneerPan();
public:
@@ -126,18 +128,16 @@ public:
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:
- void init() ;
+ void init();
// Non implemented virtual pure functions
- void get_fJe(vpMatrix & /*_fJe */) {};
- void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */) {};
- void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */) {};
-} ;
+ void get_fJe(vpMatrix & /*_fJe */){};
+ void getDisplacement(const vpRobot::vpControlFrameType /* frame */, vpColVector & /* q */){};
+ void setPosition(const vpRobot::vpControlFrameType /* frame */, const vpColVector & /* q */){};
+};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpSimulatorViper850.h b/modules/robot/include/visp3/robot/vpSimulatorViper850.h
index 5870d63..ff5ba5b 100644
--- a/modules/robot/include/visp3/robot/vpSimulatorViper850.h
+++ b/modules/robot/include/visp3/robot/vpSimulatorViper850.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -57,19 +58,19 @@
\brief Simulator of Irisa's Viper S850 robot named Viper850.
- Implementation of the vpRobotWireFrameSimulator class in order to simulate Irisa's
- Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
+ Implementation of the vpRobotWireFrameSimulator class in order to simulate
+Irisa's Viper850 robot. This robot is an ADEPT six degrees of freedom arm.
\warning This class uses threading capabilities. Thus on Unix-like
platforms, the libpthread third-party library need to be
installed. On Windows, we use the native threading capabilities.
-
+
This class allows to control the Viper850 arm robot in position
and velocity:
- - in the joint space (vpRobot::ARTICULAR_FRAME),
- - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
+ - in the joint space (vpRobot::ARTICULAR_FRAME),
+ - in the fixed reference frame (vpRobot::REFERENCE_FRAME),
- in the camera frame (vpRobot::CAMERA_FRAME),
- - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
+ - or in a mixed frame (vpRobot::MIXT_FRAME) where translations are expressed
in the reference frame and rotations in the camera frame.
All the translations are expressed in meters for positions and m/s
@@ -84,10 +85,10 @@
frame like here in the joint space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorViper850.h>
int main()
{
@@ -107,7 +108,7 @@ int main()
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
+
return 0;
}
\endcode
@@ -118,10 +119,10 @@ int main()
velocity used to reach the desired position.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorViper850.h>
int main()
{
@@ -138,7 +139,7 @@ int main()
// Moves the robot in the joint space
robot.setPosition(vpRobot::ARTICULAR_FRAME, q);
-
+
return 0;
}
\endcode
@@ -150,10 +151,10 @@ int main()
space:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorViper850.h>
int main()
{
@@ -180,7 +181,7 @@ int main()
// Stop the robot
robot.setRobotState(vpRobot::STATE_STOP);
-
+
return 0;
}
\endcode
@@ -198,118 +199,107 @@ int main()
*/
-
class VISP_EXPORT vpSimulatorViper850 : public vpRobotWireFrameSimulator, public vpViper850
{
- public:
- static const double defaultPositioningVelocity;
-
- private:
- vpColVector q_prev_getdis;
- bool first_time_getdis;
-
- double positioningVelocity;
-
- vpColVector zeroPos;
- vpColVector reposPos;
-
- bool toolCustom;
- std::string arm_dir;
-
- public:
- vpSimulatorViper850();
- vpSimulatorViper850(bool display);
- virtual ~vpSimulatorViper850();
-
- void getCameraParameters(vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height);
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I);
- void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
-
- void getDisplacement(const vpRobot::vpControlFrameType frame,
- vpColVector &displacement);
-
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
- void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
- double getPositioningVelocity (void){return positioningVelocity;}
-
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
- void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame);
- vpColVector getVelocity (const vpRobot::vpControlFrameType frame, double ×tamp);
-
- void get_cMe(vpHomogeneousMatrix &cMe);
- void get_cVe(vpVelocityTwistMatrix &cVe);
- void get_eJe(vpMatrix &eJe);
- void get_fJe(vpMatrix &fJe);
-
- void init (vpViper850::vpToolType tool, vpCameraParameters::vpCameraParametersProjType projModel=vpCameraParameters::perspectiveProjWithoutDistortion);
- bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
- void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
-
- void move(const char *filename) ;
-
- static bool readPosFile(const 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 setPosition(const char *filename);
- void setPositioningVelocity (const double vel) {positioningVelocity = vel;}
- vpRobot::vpRobotStateType setRobotState (const vpRobot::vpRobotStateType newState);
-
- void setVelocity (const vpRobot::vpControlFrameType frame, const vpColVector & velocity);
-
- void stopMotion();
-
+public:
+ static const double defaultPositioningVelocity;
+
+private:
+ vpColVector q_prev_getdis;
+ bool first_time_getdis;
+
+ double positioningVelocity;
+
+ vpColVector zeroPos;
+ vpColVector reposPos;
+
+ bool toolCustom;
+ std::string arm_dir;
+
+public:
+ vpSimulatorViper850();
+ 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<unsigned char> &I);
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I);
+
+ void getDisplacement(const vpRobot::vpControlFrameType frame, vpColVector &displacement);
+
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position);
+ void getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp);
+ double getPositioningVelocity(void) { return positioningVelocity; }
+
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q);
+ void getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame);
+ vpColVector getVelocity(const vpRobot::vpControlFrameType frame, double ×tamp);
+
+ void get_cMe(vpHomogeneousMatrix &cMe);
+ void get_cVe(vpVelocityTwistMatrix &cVe);
+ void get_eJe(vpMatrix &eJe);
+ void get_fJe(vpMatrix &fJe);
+
+ void
+ init(vpViper850::vpToolType tool,
+ vpCameraParameters::vpCameraParametersProjType projModel = vpCameraParameters::perspectiveProjWithoutDistortion);
+ bool initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo);
+ void initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo);
+
+ void move(const char *filename);
+
+ static bool readPosFile(const 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 setPosition(const char *filename);
+ void setPositioningVelocity(const double vel) { positioningVelocity = vel; }
+ vpRobot::vpRobotStateType setRobotState(const vpRobot::vpRobotStateType newState);
+
+ void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &velocity);
+
+ void stopMotion();
+
protected:
- /** @name Protected Member Functions Inherited from vpSimulatorViper850 */
- //@{
- void computeArticularVelocity();
- void compute_fMi();
- void findHighestPositioningSpeed(vpColVector &q);
- void getExternalImage(vpImage<vpRGBa> &I);
-
- inline void get_fMi(vpHomogeneousMatrix *fMit) {
+ /** @name Protected Member Functions Inherited from vpSimulatorViper850 */
+ //@{
+ void computeArticularVelocity();
+ void compute_fMi();
+ void findHighestPositioningSpeed(vpColVector &q);
+ void getExternalImage(vpImage<vpRGBa> &I);
+
+ inline void get_fMi(vpHomogeneousMatrix *fMit)
+ {
#if defined(_WIN32)
-# if defined(WINRT_8_1)
- WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
-# else // pure win32
- WaitForSingleObject(mutex_fMi, INFINITE);
-# endif
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- ReleaseMutex(mutex_fMi);
+#if defined(WINRT_8_1)
+ WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
+#else // pure win32
+ WaitForSingleObject(mutex_fMi, INFINITE);
+#endif
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ ReleaseMutex(mutex_fMi);
#elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
- for (int i = 0; i < 8; i++)
- fMit[i] = fMi[i];
- pthread_mutex_unlock (&mutex_fMi);
+ pthread_mutex_lock(&mutex_fMi);
+ for (int i = 0; i < 8; i++)
+ fMit[i] = fMi[i];
+ pthread_mutex_unlock(&mutex_fMi);
#endif
- }
- void init();
- void initArms();
- void initDisplay();
- int isInJointLimit (void);
- bool singularityTest(const vpColVector q, vpMatrix &J);
- void updateArticularPosition();
- //@}
-
-private:
- void getArticularDisplacement(vpColVector &displacement);
- void getCameraDisplacement(vpColVector &displacement);
+ }
+ void init();
+ void initArms();
+ void initDisplay();
+ int isInJointLimit(void);
+ bool singularityTest(const vpColVector &q, vpMatrix &J);
+ void updateArticularPosition();
+ //@}
};
#endif
diff --git a/modules/robot/include/visp3/robot/vpUnicycle.h b/modules/robot/include/visp3/robot/vpUnicycle.h
index 7b4cfbf..efe8bae 100644
--- a/modules/robot/include/visp3/robot/vpUnicycle.h
+++ b/modules/robot/include/visp3/robot/vpUnicycle.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
#include <visp3/core/vpTranslationVector.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-
/*!
\class vpUnicycle
@@ -59,13 +59,11 @@ public:
/*!
Default constructor that does nothing.
*/
- vpUnicycle() : cMe_(), eJe_()
- {
- };
+ vpUnicycle() : cMe_(), eJe_(){};
/*!
Destructor that does nothing.
*/
- virtual ~vpUnicycle() {};
+ virtual ~vpUnicycle(){};
/** @name Inherited functionalities from vpUnicycle */
//@{
@@ -73,73 +71,61 @@ public:
Return the tranformation \f${^c}{\bf M}_e\f$ between the camera frame
and the mobile robot end effector frame.
*/
- vpHomogeneousMatrix get_cMe() const
- {
- return cMe_;
- }
+ vpHomogeneousMatrix get_cMe() const { return cMe_; }
/*!
Return the twist transformation from camera frame to the mobile robot
- end effector frame. This transformation allows to compute a velocity expressed
- in the end effector frame into the camera frame.
+ end effector frame. This transformation allows to compute a velocity
+ expressed in the end effector frame into the camera frame.
*/
vpVelocityTwistMatrix get_cVe() const
{
vpVelocityTwistMatrix cVe;
- cVe.buildFrom(cMe_) ;
+ cVe.buildFrom(cMe_);
return cVe;
}
/*!
Return the twist transformation from camera frame to the mobile robot
- end effector frame. This transformation allows to compute a velocity expressed
- in the end effector frame into the camera frame.
+ end effector frame. This transformation allows to compute a velocity
+ expressed in the end effector frame into the camera frame.
\sa get_cVe()
*/
- void get_cVe(vpVelocityTwistMatrix &cVe) const
- {
- cVe = vpUnicycle::get_cVe();
- }
+ void get_cVe(vpVelocityTwistMatrix &cVe) const { cVe = get_cVe(); }
/*!
- Return the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
+ Return the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end
+ effector frame.
- \return The robot jacobian such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
- \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
+ \return The robot jacobian such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf
+ q}\f$ with \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and
+ \f$\bf v\f$ the six dimention velocity skew.
*/
- vpMatrix get_eJe() const
- {
- return eJe_;
- }
+ vpMatrix get_eJe() const { return eJe_; }
/*!
- Set the transformation between the camera frame and the end effector frame.
+ Set the transformation between the camera frame and the end effector
+ frame.
*/
- void set_cMe(const vpHomogeneousMatrix &cMe)
- {
- cMe_ = cMe;
- }
+ void set_cMe(const vpHomogeneousMatrix &cMe) { cMe_ = cMe; }
/*!
- Set the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector frame.
+ Set the robot jacobian \f${^e}{\bf J}_e\f$ expressed in the end effector
+ frame.
- \param eJe : The robot jacobian to set such as \f${\bf v} = {^e}{\bf J}_e \; \dot{\bf q}\f$ with
- \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control velocities and \f$\bf v\f$ the six dimention velocity skew.
+ \param eJe : The robot jacobian to set such as \f${\bf v} = {^e}{\bf J}_e
+ \; \dot{\bf q}\f$ with \f$\dot{\bf q} = (v_x, w_z)\f$ the robot control
+ velocities and \f$\bf v\f$ the six dimention velocity skew.
*/
- void set_eJe(const vpMatrix &eJe)
- {
- eJe_ = eJe;
- }
+ void set_eJe(const vpMatrix &eJe) { eJe_ = eJe; }
//@}
protected:
vpHomogeneousMatrix cMe_; // Camera frame to mobile platform frame
- vpMatrix eJe_; // Robot jacobian
+ vpMatrix eJe_; // Robot jacobian
};
#endif
-
-
diff --git a/modules/robot/include/visp3/robot/vpViper.h b/modules/robot/include/visp3/robot/vpViper.h
index 909fc89..d315f58 100644
--- a/modules/robot/include/visp3/robot/vpViper.h
+++ b/modules/robot/include/visp3/robot/vpViper.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,10 +47,10 @@
*/
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobotException.h>
@@ -59,10 +60,10 @@
\ingroup group_robot_real_arm group_robot_simu_arm
- \brief Modelisation of the ADEPT Viper robot
+ \brief Modelisation of the ADEPT Viper robot
- This robot has six degrees of freedom. The model of the robot is the following:
- \image html model-viper.png Model of the Viper 850 robot.
+ This robot has six degrees of freedom. The model of the robot is the
+ following: \image html model-viper.png Model of the Viper 850 robot.
The non modified Denavit-Hartenberg representation of the robot is
given in the table below, where \f$q_1^*, \ldots, q_6^*\f$
@@ -96,11 +97,11 @@
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool TOOL_CUSTOM and set this
- during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool TOOL_CUSTOM and
+ set this during robot initialisation or using set_eMc().
- \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
-
+
The forward kinematics of the robot is implemented in get_fMw(),
get_fMe() and get_fMc().
@@ -111,22 +112,23 @@
*/
class VISP_EXPORT vpViper
{
- public:
+public:
vpViper();
- virtual ~vpViper() {};
+ virtual ~vpViper(){};
/** @name Inherited functionalities from vpViper */
//@{
- vpHomogeneousMatrix getForwardKinematics(const vpColVector & q) const;
- unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector & q, const bool &verbose=false) const;
- unsigned int getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q, const bool &verbose=false) const;
- vpHomogeneousMatrix get_fMc (const vpColVector & q) const;
- void get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const;
- void get_wMe(vpHomogeneousMatrix & wMe) const;
- void get_eMc(vpHomogeneousMatrix & eMc) const;
- void get_eMs(vpHomogeneousMatrix & eMs) const;
- void get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const;
- void get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const;
+ vpHomogeneousMatrix getForwardKinematics(const vpColVector &q) const;
+ unsigned int getInverseKinematicsWrist(const vpHomogeneousMatrix &fMw, vpColVector &q,
+ const bool &verbose = false) const;
+ unsigned int getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &verbose = false) const;
+ vpHomogeneousMatrix get_fMc(const vpColVector &q) const;
+ void get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const;
+ void get_wMe(vpHomogeneousMatrix &wMe) const;
+ void get_eMc(vpHomogeneousMatrix &eMc) const;
+ void get_eMs(vpHomogeneousMatrix &eMs) const;
+ void get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const;
+ void get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const;
void get_cMe(vpHomogeneousMatrix &cMe) const;
void get_cVe(vpVelocityTwistMatrix &cVe) const;
@@ -142,19 +144,20 @@ class VISP_EXPORT vpViper
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, const bool &verbose=false) const;
+private:
+ bool convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod,
+ const bool &verbose = false) const;
- public:
+public:
static const unsigned int njoint; ///< Number of joint.
- protected:
+protected:
vpHomogeneousMatrix eMc; //!< End effector to camera transformation
// Minimal representation of eMc
vpTranslationVector etc; // meters
- vpRxyzVector erc; // radian
+ vpRxyzVector erc; // radian
// Denavit-Hartenberg parameters
double a1, d1; //!< for joint 1
@@ -164,11 +167,10 @@ class VISP_EXPORT vpViper
double d6; //!< for joint 6
double d7; //!< for force/torque location
double c56; //!< Mechanical coupling between joint 5 and joint 6
-
+
// Software joint limits in radians
vpColVector joint_max; // Maximal value of the joints
vpColVector joint_min; // Minimal value of the joints
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpViper650.h b/modules/robot/include/visp3/robot/vpViper650.h
index 8bf006d..619b4fd 100644
--- a/modules/robot/include/visp3/robot/vpViper650.h
+++ b/modules/robot/include/visp3/robot/vpViper650.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -89,8 +90,8 @@
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool TOOL_CUSTOM and set this
- tool during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool TOOL_CUSTOM and
+ set this tool during robot initialisation or using set_eMc().
- \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
@@ -98,10 +99,9 @@
#include <visp3/robot/vpViper.h>
-
-class VISP_EXPORT vpViper650: public vpViper
+class VISP_EXPORT vpViper650 : public vpViper
{
- public:
+public:
#ifdef VISP_HAVE_VIPER650_DATA
//! Files where constant tranformation between end-effector and camera frame
//! are stored.
@@ -118,10 +118,10 @@ class VISP_EXPORT vpViper650: public vpViper
/*!
Name of the camera attached to the end-effector.
*/
- static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
- static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
- static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
- static const char * const CONST_GENERIC_CAMERA_NAME;
+ static const char *const CONST_MARLIN_F033C_CAMERA_NAME;
+ static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME;
+ static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
+ static const char *const CONST_GENERIC_CAMERA_NAME;
//! List of possible tools that can be attached to the robot end-effector.
typedef enum {
@@ -136,49 +136,40 @@ class VISP_EXPORT vpViper650: public vpViper
static const vpToolType defaultTool;
vpViper650();
- virtual ~vpViper650() {};
+ virtual ~vpViper650(){};
/** @name Inherited functionalities from vpViper650 */
//@{
- void init (void);
+ 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,
- const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const { return projModel; };
+
+ void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
+ const unsigned int &image_height) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const;
void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
//! Get the current tool type
- vpToolType getToolType() const{
- return tool_current;
- };
+ vpToolType getToolType() const { return tool_current; };
- void parseConfigFile (const std::string &filename);
+ void parseConfigFile(const std::string &filename);
//@}
- protected:
+protected:
/** @name Protected Member Functions Inherited from vpViper650 */
//@{
//! Set the current tool type
- void setToolType(vpViper650::vpToolType tool){
- tool_current = tool;
- };
+ void setToolType(vpViper650::vpToolType tool) { tool_current = tool; };
//@}
- protected:
+protected:
//! Current tool in use
vpToolType tool_current;
// Used projection model
@@ -186,4 +177,3 @@ class VISP_EXPORT vpViper650: public vpViper
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpViper850.h b/modules/robot/include/visp3/robot/vpViper850.h
index a9d3e97..82ff869 100644
--- a/modules/robot/include/visp3/robot/vpViper850.h
+++ b/modules/robot/include/visp3/robot/vpViper850.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,7 +53,7 @@
\ingroup group_robot_real_arm group_robot_simu_arm
- \brief Modelisation of the ADEPT Viper 850 robot.
+ \brief Modelisation of the ADEPT Viper 850 robot.
The model of the robot is the following:
\image html model-viper.png Model of the Viper 850 robot.
@@ -89,8 +90,8 @@
- \f$ {\cal F}_c \f$: the camera or tool frame, with \f$^f{\bf M}_c = ^f{\bf
M}_e \; ^e{\bf M}_c \f$ where \f$ ^e{\bf M}_c \f$ is the result of
- a calibration stage. We can also consider a custom tool TOOL_CUSTOM and set this
- during robot initialisation or using set_eMc().
+ a calibration stage. We can also consider a custom tool TOOL_CUSTOM and
+ set this during robot initialisation or using set_eMc().
- \f$ {\cal F}_s \f$: the force/torque sensor frame, with \f$d7=0.0666\f$.
@@ -99,10 +100,9 @@
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpViper.h>
-
-class VISP_EXPORT vpViper850: public vpViper
+class VISP_EXPORT vpViper850 : public vpViper
{
- public:
+public:
#ifdef VISP_HAVE_VIPER850_DATA
//! Files where constant tranformation between end-effector and camera frame
//! are stored.
@@ -119,10 +119,10 @@ class VISP_EXPORT vpViper850: public vpViper
/*!
Name of the camera attached to the end-effector.
*/
- static const char * const CONST_MARLIN_F033C_CAMERA_NAME;
- static const char * const CONST_PTGREY_FLEA2_CAMERA_NAME;
- static const char * const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
- static const char * const CONST_GENERIC_CAMERA_NAME;
+ static const char *const CONST_MARLIN_F033C_CAMERA_NAME;
+ static const char *const CONST_PTGREY_FLEA2_CAMERA_NAME;
+ static const char *const CONST_SCHUNK_GRIPPER_CAMERA_NAME;
+ static const char *const CONST_GENERIC_CAMERA_NAME;
//! List of possible tools that can be attached to the robot end-effector.
typedef enum {
@@ -137,54 +137,44 @@ class VISP_EXPORT vpViper850: public vpViper
static const vpToolType defaultTool;
vpViper850();
- virtual ~vpViper850() {};
+ virtual ~vpViper850(){};
/** @name Inherited functionalities from vpViper850 */
//@{
- void init (void);
+ 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,
- const unsigned int &image_height) const;
- void getCameraParameters(vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const;
+ vpCameraParameters::vpCameraParametersProjType getCameraParametersProjType() const { return projModel; };
+
+ void getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
+ const unsigned int &image_height) const;
+ void getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const;
void getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const;
//! Get the current tool type
- vpToolType getToolType() const {
- return tool_current;
- };
+ vpToolType getToolType() const { return tool_current; };
- void parseConfigFile (const std::string &filename);
+ void parseConfigFile(const std::string &filename);
//@}
- protected:
+protected:
/** @name Protected Member Functions Inherited from vpViper650 */
//@{
//! Set the current tool type
- void setToolType(vpViper850::vpToolType tool){
- tool_current = tool;
- };
+ void setToolType(vpViper850::vpToolType tool) { tool_current = tool; };
//@}
- protected:
+protected:
//! Current tool in use
vpToolType tool_current;
// Used projection model
vpCameraParameters::vpCameraParametersProjType projModel;
-
};
#endif
-
diff --git a/modules/robot/include/visp3/robot/vpVirtuose.h b/modules/robot/include/visp3/robot/vpVirtuose.h
index 45a0b99..78168fc 100644
--- a/modules/robot/include/visp3/robot/vpVirtuose.h
+++ b/modules/robot/include/visp3/robot/vpVirtuose.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,7 +48,6 @@
#include <VirtuoseAPI.h>
-
/*!
\file vpVirtuose.h
\brief Wrapper over Haption Virtuose SDK to control haptic devices.
@@ -56,10 +56,11 @@
\class vpVirtuose
\ingroup group_robot_haptic
- This class was tested with Haption (http://www.haption.com) Virtuose 6D haptic device.
+ This class was tested with Haption (http://www.haption.com) Virtuose 6D
+haptic device.
- The class vpVirtuose allows to work with the original Virtuose API inside ViSP.
- The Virtuose API supports the following devices:
+ The class vpVirtuose allows to work with the original Virtuose API inside
+ViSP. The Virtuose API supports the following devices:
- Virtuose 6D35-45
- Virtuose 3D35-40
- Virtuose 3D10-20
@@ -67,45 +68,54 @@
- Virtuose Inca
Not all Virtuose API function are implemented in the class.
- Original Virtuose API functions need to be called with a VirtContext object, provided by the function getHandler().
-
- The Virtuose library implements different control modes that could be set using setCommandType().
- The choice of the control mode depends on the application.
- The following is the description of the main control modes as described in the Virtuose API documentation.
-
- 1. Force/position control (impedance mode): the application sends forces and torques to the device and
- reads the position and speed of the end-effector frame.
- 2. Position/force control (admittance mode): this advanced control mode allows direct coupling with virtual
- objects; in that case, the application sends the position and speed of the center of the
- object to the device, and reads the forces and torques to be applied to the object for
- dynamic integration. Stiffness and damping are calculated by the embedded
- software, knowing the mass and inertia of the object, in order to ensure control
+ Original Virtuose API functions need to be called with a VirtContext object,
+provided by the function getHandler().
+
+ The Virtuose library implements different control modes that could be set
+using setCommandType(). The choice of the control mode depends on the
+application. The following is the description of the main control modes as
+described in the Virtuose API documentation.
+
+ 1. Force/position control (impedance mode): the application sends forces and
+torques to the device and reads the position and speed of the end-effector
+frame.
+ 2. Position/force control (admittance mode): this advanced control mode
+allows direct coupling with virtual objects; in that case, the application
+sends the position and speed of the center of the object to the device, and
+reads the forces and torques to be applied to the object for dynamic
+integration. Stiffness and damping are calculated by the embedded software,
+knowing the mass and inertia of the object, in order to ensure control
stability.
- 3. Position/force with virtual guides: this is the same as above, with addition of virtual
- guides (e.g. fixed translation, fixed rotation, etc.).
+ 3. Position/force with virtual guides: this is the same as above, with
+addition of virtual guides (e.g. fixed translation, fixed rotation, etc.).
The Virtuose library defines the following reference frames:
- 1. The environment frame, corresponding to the origin of the virtual scene; it is specified by the
- software application independently of the Virtuose API.
- 2. The observation frame, corresponding generally to the position of the camera; it is defined with
- respect to environment frame. This frame location could be set using setObservationFrame().
- 3. The base frame, representing the center of the haptic device; it is defined with respect to
- the observation frame. This frame location could be set using setBaseFrame().
- 4. The tool frame corresponds to the base of the tool fixed at the end of the haptic device,
- and is defined with respect to the environment frame.
- 5. The end-effector (avatar) frame corresponds to the position of the user hand on the device,
- taking into account the geometry of the tool, and is defined with respect to tool frame.
+ 1. The environment frame, corresponding to the origin of the virtual scene;
+it is specified by the software application independently of the Virtuose API.
+ 2. The observation frame, corresponding generally to the position of the
+camera; it is defined with respect to environment frame. This frame location
+could be set using setObservationFrame().
+ 3. The base frame, representing the center of the haptic device; it is
+defined with respect to the observation frame. This frame location could be
+set using setBaseFrame().
+ 4. The tool frame corresponds to the base of the tool fixed at the end of
+the haptic device, and is defined with respect to the environment frame.
+ 5. The end-effector (avatar) frame corresponds to the position of the user
+hand on the device, taking into account the geometry of the tool, and is
+defined with respect to tool frame.
The position of the following frames can be defined only once using the API:
- base frame (with respect to the observation frame) thanks to setBaseFrame() and
- end-effector frame (with respect to the tool frame).
+ base frame (with respect to the observation frame) thanks to setBaseFrame()
+and end-effector frame (with respect to the tool frame).
- The position of the observation frame (with respect to the environment frame) can be modified
- dynamically using setObservationFrame().
+ The position of the observation frame (with respect to the environment
+frame) can be modified dynamically using setObservationFrame().
- The position of the tool frame (with respect to the environment frame) cannot be modified.
+ The position of the tool frame (with respect to the environment frame)
+cannot be modified.
- All values used in the Virtuose API are expressed in physical units using metric conventions:
+ All values used in the Virtuose API are expressed in physical units using
+metric conventions:
- Durations in seconds (s)
- Dimensions in meters (m)
- Angles in radians (rad)
@@ -116,9 +126,9 @@
- Masses in kilogrammes (kg)
- Inertia components in kg.m2
- The following sample code shows how to connect to the haptic device to get its current
- joint position:
- \code
+ The following sample code shows how to connect to the haptic device to get
+its current joint position:
+\code
#include <visp3/robot/vpVirtuose.h>
int main()
@@ -136,12 +146,12 @@ public:
vpVirtuose();
~vpVirtuose();
- void addForce (vpColVector &force);
- void enableForceFeedback (int enable);
+ void addForce(vpColVector &force);
+ void enableForceFeedback(int enable);
vpColVector getArticularPosition() const;
vpColVector getArticularVelocity() const;
- vpPoseVector getAvatarPosition() const ;
+ vpPoseVector getAvatarPosition() const;
vpPoseVector getBaseFrame() const;
VirtCommandType getCommandType() const;
bool getDeadMan() const;
@@ -160,14 +170,15 @@ public:
void setArticularForce(const vpColVector &articularForce);
void setArticularPosition(const vpColVector &articularPosition);
void setArticularVelocity(const vpColVector &articularVelocity);
- void setBaseFrame (const vpPoseVector &position);
+ void setBaseFrame(const vpPoseVector &position);
void setCommandType(const VirtCommandType &type);
void setForce(const vpColVector &force);
- void setForceFactor (const float &forceFactor);
- void setIndexingMode (const VirtIndexingType &type) ;
- /*! Set haptic device ip address and port. Default value is "localhost#5000".*/
- inline void setIpAddress(const std::string &ip) {m_ip = ip;}
- void setObservationFrame (const vpPoseVector &position);
+ void setForceFactor(const float &forceFactor);
+ void setIndexingMode(const VirtIndexingType &type);
+ /*! Set haptic device ip address and port. Default value is
+ * "localhost#5000".*/
+ inline void setIpAddress(const std::string &ip) { m_ip = ip; }
+ void setObservationFrame(const vpPoseVector &position);
void setPeriodicFunction(VirtPeriodicFunction CallBackVirt);
void setPosition(vpPoseVector &position);
void setPowerOff();
@@ -177,10 +188,10 @@ public:
void setVelocity(vpColVector &velocity);
void setVelocityFactor(const float &velocityFactor);
/*!
- * Enable/disable verbose mode.
- * \param mode : true to enable, false to disable verbose.
- */
- void setVerbose(bool mode) {m_verbose = mode;}
+ * Enable/disable verbose mode.
+ * \param mode : true to enable, false to disable verbose.
+ */
+ void setVerbose(bool mode) { m_verbose = mode; }
void startPeriodicFunction();
void stopPeriodicFunction();
diff --git a/modules/robot/include/visp3/robot/vpWireFrameSimulator.h b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h
index b4fe496..5876bd0 100644
--- a/modules/robot/include/visp3/robot/vpWireFrameSimulator.h
+++ b/modules/robot/include/visp3/robot/vpWireFrameSimulator.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpWireFrameSimulator_HH
#define vpWireFrameSimulator_HH
@@ -43,20 +43,20 @@
\file vpWireFrameSimulator.h
\brief Implementation of a wire frame simulator.
*/
-#include <visp3/core/vpConfig.h>
-#include <stdio.h>
+#include <cmath> // std::fabs
#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
#include <list>
+#include <stdio.h>
#include <string>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp3/core/vpList.h>
+#include <visp3/core/vpList.h>
#endif
#include <visp3/core/vpImagePoint.h>
#include <visp3/robot/vpImageSimulator.h>
@@ -67,73 +67,86 @@
\ingroup group_robot_simu_gantry group_robot_simu_arm
- \brief Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not
- require third party libraries to be used.
-
- The simulator uses several frames to display the scene in the image. There are:
-
- - The world frame : This is a fixed frame used to compute the position of the other frames against each other. By default this frame is positionned at the same location as the initial object frame position.
-
+ \brief Implementation of a wire frame simulator. Compared to the vpSimulator
+ class, it does not require third party libraries to be used.
+
+ The simulator uses several frames to display the scene in the image. There
+ are:
+
+ - The world frame : This is a fixed frame used to compute the position of
+ the other frames against each other. By default this frame is positionned at
+ the same location as the initial object frame position.
+
- The object frame : It is the frame linked to the object.
-
+
- The camera frame : It is the frame linked to the main camera.
-
- - The external camera frame : It is the frame which corresponds to one external point of view.
-
- The most used homogeneous matrices which describes the displacement between two frames are :
-
- - fMo which is the displacement between the world frame and the object frame.
-
- - cMo which is the displacement between the main camera frame and the object frame.
-
- - cextMf which is the displacement between one external camera frame and the world frame.
-
+
+ - The external camera frame : It is the frame which corresponds to one
+ external point of view.
+
+ The most used homogeneous matrices which describes the displacement between
+ two frames are :
+
+ - fMo which is the displacement between the world frame and the object
+ frame.
+
+ - cMo which is the displacement between the main camera frame and the object
+ frame.
+
+ - cextMf which is the displacement between one external camera frame and the
+ world frame.
+
The following picture summarize all the previous information.
-
+
\image html vpWireFrameSimulator.jpeg
\image latex vpWireFrameSimulator.ps
-
- The simulator uses .bnd or .wrl files as 3D scene descriptors. Several scenes can be found in the data folder which is in the directory where you build ViSP.
-
- You can move the main external view while clicking in the image. The left click enables to turn, the middle button enables to zoom and the left to translate along x and y.
-
- The simulator is able to take into account to camera parameters. You can set the internal and external cameras parameters thanks to a vpCameraParameters.
-
+
+ The simulator uses .bnd or .wrl files as 3D scene descriptors. Several
+ scenes can be found in the data folder which is in the directory where you
+ build ViSP.
+
+ You can move the main external view while clicking in the image. The left
+ click enables to turn, the middle button enables to zoom and the left to
+ translate along x and y.
+
+ The simulator is able to take into account to camera parameters. You can set
+ the internal and external cameras parameters thanks to a vpCameraParameters.
+
The following example shows how it is easy to use.
-
+
\code
-
+
#include <visp3/core/vpImage.h>
#include <visp3/robot/vpWireFrameSimulator.h>
-
+
int main()
{
vpWireFrameSimulator sim;
-
+
vpImage<vpRGBa> Iint(480,640,255);
vpImage<vpRGBa> Iext(480,640,255);
-
+
//Set the type of scene to use
sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
-
+
//Set the initial pose of the camera
sim.setCameraPositionRelObj(vpHomogeneousMatrix(0,0,0.5,vpMath::rad(0),vpMath::rad(10),0));
//Set the desired pose of the camera (for the internal view)
sim.setDesiredCameraPosition(vpHomogeneousMatrix(0.0,0,0.5,0,0,0));
//Set the pose of the reference frame (for the external view)
sim.setExternalCameraPosition(vpHomogeneousMatrix(0.1,0,0.2,0,0,0));
-
+
//Set the camera parameters
vpCameraParameters camera(1000,1000,320,240);
sim.setInternalCameraParameters(camera);
sim.setExternalCameraParameters(camera);
-
+
//Get the internal view
sim.getInternalImage(Iint);
-
+
//Get the external view
sim.getExternalImage(Iext);
-
+
return 0;
}
\endcode
@@ -142,49 +155,68 @@
class VISP_EXPORT vpWireFrameSimulator
{
public:
-
/*!
Type of scene used to display the object at the current position.
*/
- typedef enum
- {
- THREE_PTS, //!< A 40cm by 40cm plate with 3 points at coordinates (0,0,0), (0.1,0,0), (0,0.1,0). Each point is represented by a circle with 2cm radius.
- CUBE, //!< A 12.5cm size cube.
- PLATE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each point is represented by a circle with 2cm radius.
- SMALL_PLATE, //!< 4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0), (0.03,0.03,0), (0.03,0.03,0). Each point is represented by a circle with 1cm radius.
- RECTANGLE, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.07,-0.05,0), (0.07,0.05,0), (0.07,-0.05,0), (-0.07,-0.05,0). Each point is represented by a circle with 2cm radius.
- SQUARE_10CM, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0), (-0.05,-0.05,0). Each point is represented by a circle with 2cm radius.
- DIAMOND, //!< A 40cm by 40cm plate with 4 points at coordinates (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is represented by a circle with 2cm radius.
- TRAPEZOID, //!< A 40cm by 40cm plate with 4 points at coordinates (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0), (0.025,-0.05,0). Each point is represented by a circle with 2cm radius.
+ typedef enum {
+ THREE_PTS, //!< A 40cm by 40cm plate with 3 points at coordinates (0,0,0),
+ //!< (0.1,0,0), (0,0.1,0). Each point is represented by a
+ //!< circle with 2cm radius.
+ CUBE, //!< A 12.5cm size cube.
+ PLATE, //!< A 40cm by 40cm plate with 4 points at coordinates
+ //!< (-0.1,-0.1,0), (0.1,-0.1,0), (0.1,0.1,0), (0.1,0.1,0). Each
+ //!< point is represented by a circle with 2cm radius.
+ SMALL_PLATE, //!< 4 points at coordinates (-0.03,-0.03,0), (0.03,-0.03,0),
+ //!< (0.03,0.03,0), (0.03,0.03,0). Each point is represented
+ //!< by a circle with 1cm radius.
+ RECTANGLE, //!< A 40cm by 40cm plate with 4 points at coordinates
+ //!< (-0.07,-0.05,0), (0.07,0.05,0), (0.07,-0.05,0),
+ //!< (-0.07,-0.05,0). Each point is represented by a circle
+ //!< with 2cm radius.
+ SQUARE_10CM, //!< A 40cm by 40cm plate with 4 points at coordinates
+ //!< (-0.05,0.05,0), (0.05,0.05,0), (0.05,-0.05,0),
+ //!< (-0.05,-0.05,0). Each point is represented by a circle
+ //!< with 2cm radius.
+ DIAMOND, //!< A 40cm by 40cm plate with 4 points at coordinates
+ //!< (0,-0.1,0), (0.1,0,0), (0,0.1,0), (-0.1,0,0). Each point is
+ //!< represented by a circle with 2cm radius.
+ TRAPEZOID, //!< A 40cm by 40cm plate with 4 points at coordinates
+ //!< (-0.025,-0.05,0), (-0.075,0.05,0), (0.075,0.05,0),
+ //!< (0.025,-0.05,0). Each point is represented by a circle
+ //!< with 2cm radius.
THREE_LINES, //!< Three parallel lines with equation y=-5, y=0, y=5.
- ROAD, //!< Three parallel lines representing a road.
- TIRE, //!< A tire represented by 2 circles with radius 10cm and 15cm.
- PIPE, //!< A pipe represented by a cylinder of 25 cm length and 15cm radius.
- CIRCLE, //!< A 10cm radius circle.
- SPHERE, //!< A 15cm radius sphere.
- CYLINDER, //!< A cylinder of 80cm length and 10cm radius.
- PLAN, //!< A plane represented by a 56cm by 56cm plate with a grid of 49 squares inside.
- POINT_CLOUD, //!< A plate with 8 points at coordinates (0.05,0,0), (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0), (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and (0.5,0,0). ach point is represented by a circle with 2cm radius.
+ ROAD, //!< Three parallel lines representing a road.
+ TIRE, //!< A tire represented by 2 circles with radius 10cm and 15cm.
+ PIPE, //!< A pipe represented by a cylinder of 25 cm length and 15cm
+ //!< radius.
+ CIRCLE, //!< A 10cm radius circle.
+ SPHERE, //!< A 15cm radius sphere.
+ CYLINDER, //!< A cylinder of 80cm length and 10cm radius.
+ PLAN, //!< A plane represented by a 56cm by 56cm plate with a grid of 49
+ //!< squares inside.
+ POINT_CLOUD, //!< A plate with 8 points at coordinates (0.05,0,0),
+ //!< (0.15,0.05,0), (0.2,0.2,0), (-0.05,0.2,0),
+ //!< (-0.15,-0.1,0), (-0.1,-0.1,0), (-0.05,0.05,0) and
+ //!< (0.5,0,0). ach point is represented by a circle with 2cm
+ //!< radius.
} vpSceneObject;
/*!
- Type of scene used to display the object at the desired pose (in the internal view).
-
- - D_STANDARD will use the vpSceneObject used to be the object at the current position.
+ Type of scene used to display the object at the desired pose (in the
+ internal view).
+
+ - D_STANDARD will use the vpSceneObject used to be the object at the
+ current position.
- D_OUTIL will display a tool which is attached to the camera.
*/
- typedef enum
- {
- D_STANDARD, //!< The object displayed at the desired position is the same than the scene object defined in vpSceneObject.
- D_CIRCLE, //!< The object displayed at the desired position is a circle.
- D_TOOL //!< A cylindrical tool is attached to the camera.
+ typedef enum {
+ D_STANDARD, //!< The object displayed at the desired position is the same
+ //!< than the scene object defined in vpSceneObject.
+ D_CIRCLE, //!< The object displayed at the desired position is a circle.
+ D_TOOL //!< A cylindrical tool is attached to the camera.
} vpSceneDesiredObject;
- typedef enum
- {
- CT_LINE,
- CT_POINT
- } vpCameraTrajectoryDisplayType;
+ typedef enum { CT_LINE, CT_POINT } vpCameraTrajectoryDisplayType;
protected:
Bound_scene scene;
@@ -224,7 +256,7 @@ protected:
bool blocked;
vpHomogeneousMatrix camMf2;
- vpHomogeneousMatrix f2Mf;
+ vpHomogeneousMatrix f2Mf;
double px_int;
double py_int;
@@ -248,7 +280,7 @@ protected:
private:
std::string scene_dir;
-
+
public:
vpWireFrameSimulator();
virtual ~vpWireFrameSimulator();
@@ -256,15 +288,20 @@ public:
/** @name Inherited functionalities from vpWireFrameSimulator */
//@{
/*!
- Delete the history of the main camera position which are displayed in the external views.
+ Delete the history of the main camera position which are displayed in
+ the external views.
*/
- inline void deleteCameraPositionHistory() {
+ inline void deleteCameraPositionHistory()
+ {
cameraTrajectory.clear();
poseList.clear();
- fMoList.clear();}
+ fMoList.clear();
+ }
- void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
- void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo, const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
+ void displayTrajectory(const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
+ void displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &camMf);
/*!
Get the parameters of the virtual external camera.
@@ -273,16 +310,16 @@ public:
\return It returns the camera parameters.
*/
- vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const {
- //if(px_ext != 1 && py_ext != 1)
+ vpCameraParameters getExternalCameraParameters(const vpImage<unsigned char> &I) const
+ {
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
+ else {
+ unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
+ return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
}
}
/*!
@@ -292,24 +329,26 @@ public:
\return It returns the camera parameters.
*/
- vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const {
- //if(px_ext != 1 && py_ext != 1)
+ vpCameraParameters getExternalCameraParameters(const vpImage<vpRGBa> &I) const
+ {
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_ext,py_ext,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_ext, py_ext, I.getWidth() / 2, I.getHeight() / 2);
+ else {
+ unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
+ return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
}
}
/*!
- Get the main external camera's position relative to the the world reference frame.
+ Get the main external camera's position relative to the the world
+ reference frame.
- \return the main external camera position relative to the the world reference frame.
+ \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<unsigned char> &I);
void getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &camMf);
@@ -323,16 +362,16 @@ public:
\return It returns the camera parameters.
*/
- vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const {
- //if(px_int != 1 && py_int != 1)
+ vpCameraParameters getInternalCameraParameters(const vpImage<unsigned char> &I) const
+ {
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
+ else {
+ unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
+ return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
}
}
/*!
@@ -342,16 +381,16 @@ public:
\return It returns the camera parameters.
*/
- vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const {
- //if(px_int != 1 && py_int != 1)
+ vpCameraParameters getInternalCameraParameters(const vpImage<vpRGBa> &I) const
+ {
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- return vpCameraParameters(px_int,py_int,I.getWidth()/2,I.getHeight()/2);
- else
- {
- unsigned int size = vpMath::minimum(I.getWidth(),I.getHeight())/2;
- return vpCameraParameters(size,size,I.getWidth()/2,I.getHeight()/2);
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon()))
+ return vpCameraParameters(px_int, py_int, I.getWidth() / 2, I.getHeight() / 2);
+ else {
+ unsigned int size = vpMath::minimum(I.getWidth(), I.getHeight()) / 2;
+ return vpCameraParameters(size, size, I.getWidth() / 2, I.getHeight() / 2);
}
}
@@ -363,17 +402,19 @@ public:
\return The pose between between the object and the camera.
*/
- vpHomogeneousMatrix get_cMo() const {return rotz*cMo;}
+ vpHomogeneousMatrix get_cMo() const { return rotz * cMo; }
/*!
- Get the homogeneous matrices cMo stored to display the camera trajectory.
+ Get the homogeneous matrices cMo stored to display the camera
+ trajectory.
\param cMo_history : The list of the homogeneous matrices cMo.
*/
- void get_cMo_History(std::list<vpHomogeneousMatrix>& cMo_history) {
+ void get_cMo_History(std::list<vpHomogeneousMatrix> &cMo_history)
+ {
cMo_history.clear();
- for(std::list<vpHomogeneousMatrix>::const_iterator it=poseList.begin(); it!=poseList.end(); ++it){
- cMo_history.push_back(rotz*(*it));
+ for (std::list<vpHomogeneousMatrix>::const_iterator it = poseList.begin(); it != poseList.end(); ++it) {
+ cMo_history.push_back(rotz * (*it));
}
}
@@ -382,148 +423,169 @@ public:
\return The pose between the object and the fixed world frame.
*/
- vpHomogeneousMatrix get_fMo() const {return fMo;}
+ vpHomogeneousMatrix get_fMo() const { return fMo; }
/*!
- Get the homogeneous matrices fMo stored to display the camera trajectory.
+ Get the homogeneous matrices fMo stored to display the camera
+ trajectory.
\param fMo_history : The list of the homogeneous matrices fMo.
*/
- void get_fMo_History(std::list<vpHomogeneousMatrix>& fMo_history) {fMo_history = fMoList;}
+ void get_fMo_History(std::list<vpHomogeneousMatrix> &fMo_history) { fMo_history = fMoList; }
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject);
- void initScene(const char* obj, const char* desiredObject);
+ void initScene(const char *obj, const char *desiredObject);
void initScene(const vpSceneObject &obj);
- void initScene(const char* obj);
+ void initScene(const char *obj);
- void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject, const std::list<vpImageSimulator> &imObj);
- void initScene(const char* obj, const char* desiredObject, const std::list<vpImageSimulator> &imObj);
+ void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject,
+ const std::list<vpImageSimulator> &imObj);
+ void initScene(const char *obj, const char *desiredObject, const std::list<vpImageSimulator> &imObj);
void initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj);
- void initScene(const char* obj, const std::list<vpImageSimulator> &imObj);
+ void initScene(const char *obj, const std::list<vpImageSimulator> &imObj);
/*!
Set the color used to display the camera in the external view.
\param col : The desired color.
*/
- void setCameraColor(const vpColor &col) {camColor = col;}
+ void setCameraColor(const vpColor &col) { camColor = col; }
/*!
Set the position of the camera relative to the object.
-
+
\param cMo_ : The pose of the camera.
*/
- void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_) {this->cMo = rotz * cMo_; fMc = fMo*this->cMo.inverse();}
+ void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
+ {
+ this->cMo = rotz * cMo_;
+ fMc = fMo * this->cMo.inverse();
+ }
/*!
- Set the position of the the world reference frame relative to the camera.
-
+ Set the position of the the world reference frame relative to the
+ camera.
+
\param fMc_ : The pose of the camera.
*/
- void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_) {this->fMc = fMc_*rotz; cMo = this->fMc.inverse()*fMo;}
+ void setCameraPositionRelWorld(const vpHomogeneousMatrix &fMc_)
+ {
+ this->fMc = fMc_ * rotz;
+ cMo = this->fMc.inverse() * fMo;
+ }
/*!
- Set the parameter which enables to choose the size of the main camera in the external camera views. By default this parameter is set to 1.
+ Set the parameter which enables to choose the size of the main camera in
+ the external camera views. By default this parameter is set to 1.
\param factor : The ration for the camera size.
*/
- inline void setCameraSizeFactor (const float factor) {cameraFactor = factor;}
+ inline void setCameraSizeFactor(const float factor) { cameraFactor = factor; }
/*!
- Set the color used to display the camera trajectory in the external view.
+ Set the color used to display the camera trajectory in the external
+ view.
\param col : The desired color.
*/
- void setCameraTrajectoryColor(const vpColor &col) {camTrajColor = col;}
+ void setCameraTrajectoryColor(const vpColor &col) { camTrajColor = col; }
/*!
- Set the way to display the history of the main camera trajectory in the main external view. The choice is given between displaying lines and points.
+ Set the way to display the history of the main camera trajectory in the
+ main external view. The choice is given between displaying lines and
+ points.
\param camTraj_type : The chosen way to display the camera trajectory.
*/
- inline void setCameraTrajectoryDisplayType (const vpCameraTrajectoryDisplayType &camTraj_type) {this->camTrajType = camTraj_type;}
+ inline void setCameraTrajectoryDisplayType(const vpCameraTrajectoryDisplayType &camTraj_type)
+ {
+ this->camTrajType = camTraj_type;
+ }
/*!
Set the color used to display the object at the current position.
\param col : The desired color.
*/
- void setCurrentViewColor(const vpColor &col) {curColor = col;}
+ void setCurrentViewColor(const vpColor &col) { curColor = col; }
/*!
Set the desired position of the camera relative to the object.
-
+
\param cdMo_ : The desired pose of the camera.
*/
- void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) {this->cdMo = rotz * cdMo_;}
+ void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_) { this->cdMo = rotz * cdMo_; }
/*!
Set the color used to display the object at the desired position.
\param col : The desired color.
*/
- void setDesiredViewColor(const vpColor &col) {desColor = col;}
+ void setDesiredViewColor(const vpColor &col) { desColor = col; }
/*!
- Enable or disable the displaying of the camera trajectory in the main external camera view.
+ Enable or disable the displaying of the camera trajectory in the main
+ external camera view.
By default the trajectory is displayed.
\param do_display : Set to true to display the camera trajectory.
*/
- void setDisplayCameraTrajectory (const bool &do_display) {this->displayCameraTrajectory = do_display;}
+ void setDisplayCameraTrajectory(const bool &do_display) { this->displayCameraTrajectory = do_display; }
/*!
Set the internal camera parameters.
\param cam : The desired camera parameters.
*/
- inline void setExternalCameraParameters(const vpCameraParameters &cam) {
+ inline void setExternalCameraParameters(const vpCameraParameters &cam)
+ {
px_ext = cam.get_px();
py_ext = cam.get_py();
}
/*!
Set the external camera point of view.
-
- \param cam_Mf : The pose of the external camera relative to the world reference frame.
+
+ \param cam_Mf : The pose of the external camera relative to the world
+ reference frame.
*/
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
{
this->camMf = rotz * cam_Mf;
vpTranslationVector T;
- this->camMf.extract (T);
- this->camMf2.buildFrom(0,0,T[2],0,0,0);
- f2Mf = camMf2.inverse()*this->camMf;
+ this->camMf.extract(T);
+ this->camMf2.buildFrom(0, 0, T[2], 0, 0, 0);
+ f2Mf = camMf2.inverse() * this->camMf;
extCamChanged = true;
}
/*!
Specify the thickness of the graphics drawings.
*/
- void setGraphicsThickness(unsigned int thickness)
- {
- this->thickness_ = thickness;
- }
+ void setGraphicsThickness(unsigned int thickness) { this->thickness_ = thickness; }
/*!
Set the internal camera parameters.
\param cam : The desired camera parameters.
*/
- inline void setInternalCameraParameters(const vpCameraParameters &cam) {
+ inline void setInternalCameraParameters(const vpCameraParameters &cam)
+ {
px_int = cam.get_px();
py_int = cam.get_py();
}
/*!
- Set the maximum number of main camera's positions which are stored. Those position can be displayed in the external camera field of view. By default this parameter is set to 1000.
+ Set the maximum number of main camera's positions which are stored.
+ Those position can be displayed in the external camera field of view. By
+ default this parameter is set to 1000.
\param nbPt : The desired number of position which are saved.
*/
- inline void setNbPtTrajectory(const unsigned int nbPt) {nbrPtLimit = nbPt;}
+ 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:
@@ -533,10 +595,14 @@ protected:
void display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color);
vpHomogeneousMatrix navigation(const vpImage<vpRGBa> &I, bool &changed);
vpHomogeneousMatrix navigation(const vpImage<unsigned char> &I, bool &changed);
- vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
- vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo);
- vpImagePoint projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
- vpImagePoint projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
+ vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpHomogeneousMatrix &fMo);
+ vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpHomogeneousMatrix &fMo);
+ vpImagePoint projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
+ vpImagePoint projectCameraTrajectory(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpHomogeneousMatrix &fMo, const vpHomogeneousMatrix &cMf);
//@}
};
diff --git a/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h b/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h
index bc186df..fdbb3c0 100644
--- a/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h
+++ b/modules/robot/include/visp3/robot/vpWireFrameSimulatorTypes.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpWireFrameSimulatorTypes_h
#define vpWireFrameSimulatorTypes_h
@@ -43,8 +43,10 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-typedef unsigned short Index;
-typedef char Type;
+typedef unsigned short Index;
+typedef char Type;
+typedef float Matrix[4][4];
+
/*
* MATRIX
* ______
@@ -55,72 +57,70 @@ typedef char Type;
* |-------------| 0 |
* | Translation | 1 |
*/
-typedef float Matrix[4][4];
+typedef float Matrix[4][4];
-#define DEFAULT_VSIZE 4
+#define DEFAULT_VSIZE 4
/*
* Vertex_list :
- * Pour optimiser l'allocation et la liberation memoire d'une liste de sommets:
- * si (nbr > DEFAULT_VSIZE)
- * | alors ptr est alloue et libere dynamiquement
- * | sinon ptr = tbl
- * fsi;
+ * Pour optimiser l'allocation et la liberation memoire d'une liste de
+ * sommets: si (nbr > DEFAULT_VSIZE) | alors ptr est alloue et libere
+ * dynamiquement | sinon ptr = tbl fsi;
*/
-typedef struct {
- Index nbr; /* nombre de sommets */
- Index *ptr; /* liste dynamique */
- Index tbl[DEFAULT_VSIZE];
+typedef struct {
+ Index nbr; /* nombre de sommets */
+ Index *ptr; /* liste dynamique */
+ Index tbl[DEFAULT_VSIZE];
} Vertex_list;
-typedef struct {
- unsigned is_polygonal:1; /* face polygonale */
- unsigned is_visible :1; /* face affichable */
-#ifdef face_edge
- Edge_list edge; /* liste d'aretes */
-#endif //face_edge
- Vertex_list vertex; /* liste de sommets */
-#ifdef face_normal
- Vector normal; /* vecteur normal */
-#endif //face_normal
+typedef struct {
+ unsigned is_polygonal : 1; /* face polygonale */
+ unsigned is_visible : 1; /* face affichable */
+#ifdef face_edge
+ Edge_list edge; /* liste d'aretes */
+#endif // face_edge
+ Vertex_list vertex; /* liste de sommets */
+#ifdef face_normal
+ Vector normal; /* vecteur normal */
+#endif // face_normal
} Face;
-typedef struct {
- Index nbr; /* nombre de faces */
- Face *ptr; /* liste dynamique */
+typedef struct {
+ Index nbr; /* nombre de faces */
+ Face *ptr; /* liste dynamique */
} Face_list;
-typedef struct {
- float x, y, z;
+typedef struct {
+ float x, y, z;
} Point3f;
-typedef struct {
- Index nbr; /* nombre de points */
- Point3f *ptr; /* liste dynamique */
+typedef struct {
+ Index nbr; /* nombre de points */
+ Point3f *ptr; /* liste dynamique */
} Point3f_list;
-typedef struct {
- unsigned is_display :1; /* surface affichable */
- unsigned is_polygonal:1; /* surface polyedrique */
- Type type; /* type de la primitive */
-#ifdef face_edge
- Edge_list edge; /* liste d'aretes */
-#endif //face_edge
- Face_list face; /* liste de faces */
- Point3f_list point; /* points aux sommets */
-#ifdef face_normal
- Vector_list normal; /* normales aux sommets */
-#endif //face_normal
+typedef struct {
+ unsigned is_display : 1; /* surface affichable */
+ unsigned is_polygonal : 1; /* surface polyedrique */
+ Type type; /* type de la primitive */
+#ifdef face_edge
+ Edge_list edge; /* liste d'aretes */
+#endif // face_edge
+ Face_list face; /* liste de faces */
+ Point3f_list point; /* points aux sommets */
+#ifdef face_normal
+ Vector_list normal; /* normales aux sommets */
+#endif // face_normal
} Bound;
-typedef struct {
- Index nbr; /* nombre de surfaces */
- Bound *ptr; /* liste dynamique */
+typedef struct {
+ Index nbr; /* nombre de surfaces */
+ Bound *ptr; /* liste dynamique */
} Bound_list;
-typedef struct {
- char *name; /* nom de la scene */
- Bound_list bound; /* liste de surfaces */
+typedef struct {
+ char *name; /* nom de la scene */
+ Bound_list bound; /* liste de surfaces */
} Bound_scene;
#endif
diff --git a/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp b/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp
index 32b218b..38677b5 100644
--- a/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp
+++ b/modules/robot/src/haptic-device/virtuose/vpVirtuose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,10 +52,8 @@
* 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_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);
@@ -75,27 +74,28 @@ vpVirtuose::~vpVirtuose()
/*!
* Add a force to be applied to the virtuose (impedance effort).
* This function works in every mode.
- * \param force : Is 6 component dynamic tensor (three forces and three torques) wrt virtuose end-effector
- * and is expressed in the coordinates of the base frame.
+ * \param force : Is 6 component dynamic tensor (three forces and three
+ * torques) wrt virtuose end-effector and is expressed in the coordinates of
+ * the base frame.
*/
void vpVirtuose::addForce(vpColVector &force)
{
if (force.size() != 6) {
throw(vpException(vpException::dimensionError,
- "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
+ "Cannot apply a force feedback (dim %d) to the haptic "
+ "device that is not 6-dimension",
force.size()));
}
init();
float virtforce[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
virtforce[i] = (float)force[i];
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));
}
}
@@ -109,8 +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));
}
}
@@ -123,51 +122,48 @@ vpColVector vpVirtuose::getArticularPosition() const
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
- vpColVector articularPosition(6,0);
+ vpColVector articularPosition(6, 0);
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++)
+ for (unsigned int i = 0; i < 6; i++)
articularPosition[i] = articular_position_[i];
return articularPosition;
}
-
/*!
* Return the 6 joint velocities of the virtuose.
- */
+ */
vpColVector vpVirtuose::getArticularVelocity() const
{
if (!m_is_init) {
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
- vpColVector articularVelocity(6,0);
+ 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++)
+ for (unsigned int i = 0; i < 6; i++)
articularVelocity[i] = articular_velocity_[i];
return articularVelocity;
-
}
/*!
- * Return the indexed position of the end-effector, expressed in the coordinates of the environment reference frame.
- * With respect to the function getPosition(), getAvatarPosition() takes into account current offsets
- * (indexing) and motor scale factors.
- * \sa getPosition(), getPhysicalPosition()
+ * Return the indexed position of the end-effector, expressed in the
+ * coordinates of the environment reference frame. With respect to the
+ * function getPosition(), getAvatarPosition() takes into account current
+ * offsets (indexing) and motor scale factors. \sa getPosition(),
+ * getPhysicalPosition()
*/
vpPoseVector vpVirtuose::getAvatarPosition() const
{
@@ -182,15 +178,12 @@ 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));
- }
- else
- {
- for (int i=0; i<3; i++)
+ throw(vpException(vpException::fatalError, "Error calling virtGetAvatarPosition: error code %d", err));
+ } else {
+ for (int i = 0; i < 3; i++)
translation[i] = position_[i];
- for (int i=0; i<4; i++)
- quaternion[i] = position_[3+i];
+ for (int i = 0; i < 4; i++)
+ quaternion[i] = position_[3 + i];
vpThetaUVector thetau(quaternion);
@@ -219,15 +212,12 @@ 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));
- }
- else
- {
- for (int i=0; i<3; i++)
+ throw(vpException(vpException::fatalError, "Error calling virtGetBaseFrame: error code %d", err));
+ } else {
+ for (int i = 0; i < 3; i++)
translation[i] = position_[i];
- for (int i=0; i<4; i++)
- quaternion[i] = position_[3+i];
+ for (int i = 0; i < 4; i++)
+ quaternion[i] = position_[3 + i];
vpThetaUVector thetau(quaternion);
@@ -250,15 +240,14 @@ VirtCommandType vpVirtuose::getCommandType() const
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;
}
/*!
- * Return the status of DeadMan sensor : true if the sensor is ON (a user is holding the handle)
- * and false if the sensor is OFF (no user detected).
+ * Return the status of DeadMan sensor : true if the sensor is ON (a user is
+ * holding the handle) and false if the sensor is OFF (no user detected).
*/
bool vpVirtuose::getDeadMan() const
{
@@ -269,15 +258,15 @@ bool vpVirtuose::getDeadMan() const
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);
}
/*!
- * Return the status of the emergency stop button : true if the system is operational (button correctly plugged and not triggered)
- * and false if the system is not operational (button not plugged or triggered).
+ * Return the status of the emergency stop button : true if the system is
+ * operational (button correctly plugged and not triggered) and false if the
+ * system is not operational (button not plugged or triggered).
*/
bool vpVirtuose::getEmergencyStop() const
{
@@ -288,15 +277,14 @@ bool vpVirtuose::getEmergencyStop() const
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);
}
/*!
- * Return the 6-dimension force tensor to be applied to the object attached to the Virtuose,
- * allowing the dynamic simulation of the scene.
+ * Return the 6-dimension force tensor to be applied to the object attached to
+ * the Virtuose, allowing the dynamic simulation of the scene.
*/
vpColVector vpVirtuose::getForce() const
{
@@ -304,15 +292,14 @@ vpColVector vpVirtuose::getForce() const
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
- vpColVector force(6,0);
+ 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++)
+ for (unsigned int i = 0; i < 6; i++)
force[i] = force_[i];
return force;
}
@@ -349,9 +336,7 @@ int main()
\sa getArticularPosition()
*/
-VirtContext vpVirtuose::getHandler() {
- return m_virtContext;
-}
+VirtContext vpVirtuose::getHandler() { return m_virtContext; }
/*!
* Return the cartesian current position of the observation reference frame
@@ -359,7 +344,7 @@ VirtContext vpVirtuose::getHandler() {
*
* \sa setObservationFrame(), getBaseFrame()
*/
-vpPoseVector vpVirtuose::getObservationFrame () const
+vpPoseVector vpVirtuose::getObservationFrame() const
{
if (!m_is_init) {
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
@@ -372,15 +357,12 @@ 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));
- }
- else
- {
- for (int i=0; i<3; i++)
+ throw(vpException(vpException::fatalError, "Error calling virtGetObservationFrame: error code %d", err));
+ } else {
+ for (int i = 0; i < 3; i++)
translation[i] = position_[i];
- for (int i=0; i<4; i++)
- quaternion[i] = position_[3+i];
+ for (int i = 0; i < 4; i++)
+ quaternion[i] = position_[3 + i];
vpThetaUVector thetau(quaternion);
@@ -390,8 +372,9 @@ vpPoseVector vpVirtuose::getObservationFrame () const
}
/*!
- * Return the cartesian physical position of the Virtuose expressed in the coordinates of the base reference frame.
- * \sa getAvatarPosition(), getPosition()
+ * Return the cartesian physical position of the Virtuose expressed in the
+ * coordinates of the base reference frame. \sa getAvatarPosition(),
+ * getPosition()
*/
vpPoseVector vpVirtuose::getPhysicalPosition() const
{
@@ -406,15 +389,12 @@ 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));
- }
- else
- {
- for (int i=0; i<3; i++)
+ throw(vpException(vpException::fatalError, "Error calling virtGetPhysicalPosition: error code %d", err));
+ } else {
+ for (int i = 0; i < 3; i++)
translation[i] = position_[i];
- for (int i=0; i<4; i++)
- quaternion[i] = position_[3+i];
+ for (int i = 0; i < 4; i++)
+ quaternion[i] = position_[3 + i];
vpThetaUVector thetau(quaternion);
@@ -424,10 +404,10 @@ vpPoseVector vpVirtuose::getPhysicalPosition() const
}
/*!
- * Return the physical cartesian velocity twist of the Virtuose expressed in the coordinates of
- * the base reference frame.
- * \return A 6-dimension velocity twist with 3 translation velocities and 3 rotation velocities.
- * \sa getVelocity()
+ * Return the physical cartesian velocity twist of the Virtuose expressed in
+ * the coordinates of the base reference frame. \return A 6-dimension velocity
+ * twist with 3 translation velocities and 3 rotation velocities. \sa
+ * getVelocity()
*/
vpColVector vpVirtuose::getPhysicalVelocity() const
{
@@ -435,22 +415,22 @@ vpColVector vpVirtuose::getPhysicalVelocity() const
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
- vpColVector vel(6,0);
+ 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",
virtGetErrorMessage(err)));
}
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
vel[i] = speed[i];
return vel;
}
/*!
- * Return the cartesian position of the virtuose (or the object attached to it, if any) expressed
- * in the coordinates of the environment reference frame.
- * \sa getAvatarPosition(), getPhysicalPosition()
+ * Return the cartesian position of the virtuose (or the object attached to
+ * it, if any) expressed in the coordinates of the environment reference
+ * frame. \sa getAvatarPosition(), getPhysicalPosition()
*/
vpPoseVector vpVirtuose::getPosition() const
{
@@ -465,15 +445,12 @@ 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));
- }
- else
- {
- for (int i=0; i<3; i++)
+ throw(vpException(vpException::fatalError, "Error calling virtGetPosition: error code %d", err));
+ } else {
+ for (int i = 0; i < 3; i++)
translation[i] = position_[i];
- for (int i=0; i<4; i++)
- quaternion[i] = position_[3+i];
+ for (int i = 0; i < 4; i++)
+ quaternion[i] = position_[3 + i];
vpThetaUVector thetau(quaternion);
@@ -485,7 +462,7 @@ vpPoseVector vpVirtuose::getPosition() const
/*!
* Return status of the motors : true if motors are ON, false otherwise.
*/
-bool vpVirtuose::getPower () const
+bool vpVirtuose::getPower() const
{
if (!m_is_init) {
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
@@ -497,10 +474,10 @@ bool vpVirtuose::getPower () const
}
/*!
- * Return the cartesian velocity twist of the virtuose (or the object attached to it, if any)
- * expressed in the coordinates of the environment reference frame.
- * \return A 6-dimension velocity twist with 3 translation velocities and 3 rotation velocities.
- * \sa getPhysicalVelocity()
+ * Return the cartesian velocity twist of the virtuose (or the object attached
+ * to it, if any) expressed in the coordinates of the environment reference
+ * frame. \return A 6-dimension velocity twist with 3 translation velocities
+ * and 3 rotation velocities. \sa getPhysicalVelocity()
*/
vpColVector vpVirtuose::getVelocity() const
{
@@ -508,31 +485,30 @@ vpColVector vpVirtuose::getVelocity() const
throw(vpException(vpException::fatalError, "Device not initialized. Call init()."));
}
- vpColVector vel(6,0);
+ 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++)
+ for (unsigned int i = 0; i < 6; i++)
vel[i] = speed[i];
return vel;
}
/*!
- * Initialize virtuose device opening the connection to the device and setting the default command type.
- * If the device is already initialized, a call to init() does nothing.
+ * Initialize virtuose device opening the connection to the device and setting
+ * the default command type. If the device is already initialized, a call to
+ * init() does nothing.
*/
void vpVirtuose::init()
{
- if (! m_is_init) {
+ if (!m_is_init) {
m_virtContext = virtOpen(m_ip.c_str());
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)) {
@@ -547,14 +523,13 @@ void vpVirtuose::init()
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;
@@ -563,79 +538,82 @@ void vpVirtuose::init()
/*!
* Send a command of articular force to the Virtuose.
- * setArticularForce() only works in mode COMMAND_TYPE_ARTICULAR_IMPEDANCE, to be set with setCommandType().
- * \param articularForce : Six dimension torque vector.
+ * setArticularForce() only works in mode COMMAND_TYPE_ARTICULAR_IMPEDANCE, to
+ * be set with setCommandType(). \param articularForce : Six dimension torque
+ * vector.
*/
-void vpVirtuose::setArticularForce (const vpColVector &articularForce)
+void vpVirtuose::setArticularForce(const vpColVector &articularForce)
{
init();
if (articularForce.size() != 6) {
throw(vpException(vpException::dimensionError,
- "Cannot apply an articular force feedback (dim %d) to the haptic device that is not 6-dimension",
+ "Cannot apply an articular force feedback (dim %d) to "
+ "the haptic device that is not 6-dimension",
articularForce.size()));
}
float articular_force[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
articular_force[i] = (float)articularForce[i];
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));
}
}
/*!
* Send a command of articular (joint) position to the virtuose.
- * This function works only in COMMAND_TYPE_ARTICULAR mode, to be set with setCommandType().
- * \param articularPosition : Six dimension joint position vector.
+ * This function works only in COMMAND_TYPE_ARTICULAR mode, to be set with
+ * setCommandType(). \param articularPosition : Six dimension joint position
+ * vector.
*/
-void vpVirtuose::setArticularPosition (const vpColVector &articularPosition)
+void vpVirtuose::setArticularPosition(const vpColVector &articularPosition)
{
init();
if (articularPosition.size() != 6) {
throw(vpException(vpException::dimensionError,
- "Cannot send an articular position command (dim %d) to the haptic device that is not 6-dimension",
+ "Cannot send an articular position command (dim %d) to "
+ "the haptic device that is not 6-dimension",
articularPosition.size()));
}
float articular_position[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
articular_position[i] = (float)articularPosition[i];
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));
}
}
/*!
* Send a command of articular (joint) velocity to the virtuose.
- * This function works only in COMMAND_TYPE_ARTICULAR mode, to be set with setCommandType().
- * \param articularVelocity : Six dimension joint velocity vector.
+ * This function works only in COMMAND_TYPE_ARTICULAR mode, to be set with
+ * setCommandType(). \param articularVelocity : Six dimension joint velocity
+ * vector.
*/
-void vpVirtuose::setArticularVelocity (const vpColVector &articularVelocity)
+void vpVirtuose::setArticularVelocity(const vpColVector &articularVelocity)
{
init();
if (articularVelocity.size() != 6) {
throw(vpException(vpException::dimensionError,
- "Cannot send an articular velocity command (dim %d) to the haptic device that is not 6-dimension",
+ "Cannot send an articular velocity command (dim %d) to "
+ "the haptic device that is not 6-dimension",
articularVelocity.size()));
}
float articular_velocity[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
articular_velocity[i] = (float)articularVelocity[i];
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));
}
}
@@ -646,7 +624,7 @@ void vpVirtuose::setArticularVelocity (const vpColVector &articularVelocity)
*
* \sa getBaseFrame(), setObservationFrame()
*/
-void vpVirtuose::setBaseFrame (const vpPoseVector &position)
+void vpVirtuose::setBaseFrame(const vpPoseVector &position)
{
init();
@@ -657,15 +635,14 @@ void vpVirtuose::setBaseFrame (const vpPoseVector &position)
position.extract(translation);
position.extract(quaternion);
- for (int i=0; i<3; i++)
+ for (int i = 0; i < 3; i++)
position_[i] = (float)translation[i];
- for (int i=0; i<4; i++)
+ for (int i = 0; i < 4; i++)
position_[3 + i] = (float)quaternion[i];
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));
}
}
@@ -676,7 +653,8 @@ void vpVirtuose::setBaseFrame (const vpPoseVector &position)
* - COMMAND_TYPE_IMPEDANCE : Force/position control.
* - COMMAND_TYPE_VIRTMECH : Position/force control with virtual mechanism.
* - COMMAND_TYPE_ARTICULAR : Joint control.
- * - COMMAND_TYPE_ARTICULAR_IMPEDANCE : Force/Position control in the joint space.
+ * - COMMAND_TYPE_ARTICULAR_IMPEDANCE : Force/Position control in the joint
+ * space.
*/
void vpVirtuose::setCommandType(const VirtCommandType &type)
{
@@ -687,50 +665,50 @@ 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));
}
}
}
/*!
* Set the force to be applied by the Virtuose.
- * setForce() only works in COMMAND_TYPE_IMPEDANCE mode, to be set with setCommandType().
- * \param force : Force vector that represents a dynamic tensor with 6 components.
+ * setForce() only works in COMMAND_TYPE_IMPEDANCE mode, to be set with
+ * setCommandType(). \param force : Force vector that represents a dynamic
+ * tensor with 6 components.
*/
-void vpVirtuose::setForce (const vpColVector &force)
+void vpVirtuose::setForce(const vpColVector &force)
{
init();
if (force.size() != 6) {
throw(vpException(vpException::dimensionError,
- "Cannot apply a force feedback (dim %d) to the haptic device that is not 6-dimension",
+ "Cannot apply a force feedback (dim %d) to the haptic "
+ "device that is not 6-dimension",
force.size()));
}
float virtforce[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
virtforce[i] = (float)force[i];
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));
}
}
/*!
* Set the force scale factor.
- * \param forceFactor : Force scale factor applied to the force torque tensor set by setForce().
+ * \param forceFactor : Force scale factor applied to the force torque tensor
+ * set by setForce().
*/
-void vpVirtuose::setForceFactor (const float &forceFactor)
+void vpVirtuose::setForceFactor(const float &forceFactor)
{
init();
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));
}
}
@@ -738,22 +716,26 @@ void vpVirtuose::setForceFactor (const float &forceFactor)
* Set indexing (offset) mode.
* \param type : Possible choices are:
* - INDEXING_ALL : authorize indexing on all movements.
- * - INDEXING_TRANS : authorize indexing on translation, i.e., the orientation of the object is always identical to the orientation of the device end-effector.
+ * - INDEXING_TRANS : authorize indexing on translation, i.e., the orientation
+ * of the object is always identical to the orientation of the device
+ * end-effector.
* - INDEXING_NONE : forbids indexing on all movements.
- * - The following values are also implemented: INDEXING_ALL_FORCE_FEEDBACK_INHIBITION, INDEXING_TRANS_FORCE_FEEDBACK_INHIBITION.
- * These values correspond to the same modes listed before, but the force feedback is inhibited during indexing.
+ * - The following values are also implemented:
+ * INDEXING_ALL_FORCE_FEEDBACK_INHIBITION,
+ * INDEXING_TRANS_FORCE_FEEDBACK_INHIBITION. These values correspond to the
+ * same modes listed before, but the force feedback is inhibited during
+ * indexing.
*/
-void vpVirtuose::setIndexingMode (const VirtIndexingType &type)
+void vpVirtuose::setIndexingMode(const VirtIndexingType &type)
{
init();
- if (m_indexType != type){
+ if (m_indexType != type) {
m_indexType = 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));
}
}
}
@@ -765,7 +747,7 @@ void vpVirtuose::setIndexingMode (const VirtIndexingType &type)
*
* \sa getObservationFrame(), setBaseFrame()
*/
-void vpVirtuose::setObservationFrame (const vpPoseVector &position)
+void vpVirtuose::setObservationFrame(const vpPoseVector &position)
{
init();
@@ -776,25 +758,29 @@ void vpVirtuose::setObservationFrame (const vpPoseVector &position)
position.extract(translation);
position.extract(quaternion);
- for (int i=0; i<3; i++)
+ for (int i = 0; i < 3; i++)
position_[i] = (float)translation[i];
- for (int i=0; i<4; i++)
+ for (int i = 0; i < 4; i++)
position_[3 + i] = (float)quaternion[i];
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));
}
}
/*!
* Register the periodic function.
- * setPeriodicFunction() defines a callback function to be called at a fixed period of time, as timing for the simulation.
- * The callback function is synchronized with the Virtuose controller (messages arrive at very constant time intervals from it)
- * and generates hardware interrupts to be taken into account by the operating system.
- * In practice, this function is much more efficient for timing the simulation than common software timers.
- * This function is started using startPeriodicFunction() and stopped using stopPeriodicFunction().
+ * setPeriodicFunction() defines a callback function to be called at a fixed
+period of time, as timing for the simulation.
+ * The callback function is synchronized with the Virtuose controller
+(messages arrive at very constant time intervals from it)
+ * and generates hardware interrupts to be taken into account by the operating
+system.
+ * In practice, this function is much more efficient for timing the simulation
+than common software timers.
+ * This function is started using startPeriodicFunction() and stopped using
+stopPeriodicFunction().
* \param CallBackVirt : Callback function.
*
* Example of the use of the periodic function:
@@ -828,16 +814,16 @@ 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));
}
}
/*!
- * Modify the current value of the control position and send it to the Virtuose.
- * \param position : Position of the end-effector (or the attached object, if any).
+ * Modify the current value of the control position and send it to the
+ * Virtuose. \param position : Position of the end-effector (or the attached
+ * object, if any).
*/
-void vpVirtuose::setPosition (vpPoseVector &position)
+void vpVirtuose::setPosition(vpPoseVector &position)
{
init();
@@ -848,44 +834,40 @@ void vpVirtuose::setPosition (vpPoseVector &position)
position.extract(translation);
position.extract(quaternion);
- for (int i=0; i<3; i++)
+ for (int i = 0; i < 3; i++)
position_[i] = (float)translation[i];
- for (int i=0; i<4; i++)
- position_[3+i] = (float)quaternion[i];
+ for (int i = 0; i < 4; i++)
+ position_[3 + i] = (float)quaternion[i];
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));
}
}
-
/*!
* Turn the motor power OFF.
*/
-void vpVirtuose::setPowerOff ()
+void vpVirtuose::setPowerOff()
{
init();
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));
}
}
/*!
* Turn the motor power ON.
*/
-void vpVirtuose::setPowerOn ()
+void vpVirtuose::setPowerOn()
{
init();
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));
}
}
@@ -900,69 +882,65 @@ void vpVirtuose::setSaturation(const float &forceLimit, const float &torqueLimit
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));
}
}
/*!
* Set the the simulation time step.
- * The function must be called before the selection of the type of control mode.
- * \param timeStep : Simulation time step (seconds).
+ * The function must be called before the selection of the type of control
+ * mode. \param timeStep : Simulation time step (seconds).
*/
-void vpVirtuose::setTimeStep (const float &timeStep)
+void vpVirtuose::setTimeStep(const float &timeStep)
{
init();
- if (m_period != timeStep){
+ if (m_period != timeStep) {
m_period = 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));
}
}
}
/*!
- * Modify the current value of the control velocity and send it to the Virtuose.
- * \param velocity : Velocity twist vector, where translations velocities are expressed in
- * m/s and rotation velocities in rad/s.
+ * Modify the current value of the control velocity and send it to the
+ * Virtuose. \param velocity : Velocity twist vector, where translations
+ * velocities are expressed in m/s and rotation velocities in rad/s.
*/
-void vpVirtuose::setVelocity (vpColVector &velocity)
+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",
+ throw(vpException(vpException::dimensionError, "Cannot set a velocity vector (dim %d) that is not 6-dimension",
velocity.size()));
}
float speed[6];
- for(unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
speed[i] = (float)velocity[i];
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));
}
}
/*!
* Set the speed factor.
- * \param velocityFactor : Scale factor applied to the velocities set using setVelocity().
+ * \param velocityFactor : Scale factor applied to the velocities set using
+ * setVelocity().
*/
-void vpVirtuose::setVelocityFactor (const float &velocityFactor)
+void vpVirtuose::setVelocityFactor(const float &velocityFactor)
{
init();
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));
}
}
@@ -977,10 +955,8 @@ void vpVirtuose::startPeriodicFunction()
if (virtStartLoop(m_virtContext)) {
int err = virtGetErrorCode(m_virtContext);
- throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d",
- err));
- }
- else
+ throw(vpException(vpException::fatalError, "Error calling startLoop: error code %d", err));
+ } else
std::cout << "Haptic loop open." << std::endl;
}
@@ -995,16 +971,12 @@ void vpVirtuose::stopPeriodicFunction()
if (virtStopLoop(m_virtContext)) {
int err = virtGetErrorCode(m_virtContext);
- throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d",
- err));
- }
- else
+ throw(vpException(vpException::fatalError, "Error calling stopLoop: error code %d", err));
+ } else
std::cout << "Haptic loop closed." << std::endl;
}
#else
// Work around to avoid warning
-void dummy_vpVirtuose() {};
+void dummy_vpVirtuose(){};
#endif
-
-
diff --git a/modules/robot/src/image-simulator/vpImageSimulator.cpp b/modules/robot/src/image-simulator/vpImageSimulator.cpp
index ee7b03d..ef3f16e 100644
--- a/modules/robot/src/image-simulator/vpImageSimulator.cpp
+++ b/modules/robot/src/image-simulator/vpImageSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -27,62 +28,61 @@
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
- * Description: Class which enables to project an image in the 3D space
+ * Description: Class which enables to project an image in the 3D space
* and get the view of a virtual camera.
- *
+ *
* Authors:
* Amaury Dame
* Nicolas Melchior
*
*****************************************************************************/
-#include <visp3/robot/vpImageSimulator.h>
-#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPolygon3D.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/robot/vpImageSimulator.h>
#ifdef VISP_HAVE_MODULE_IO
-# include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
#endif
/*!
Basic constructor.
-
+
You can choose if you want to use a colored or gray scaled image.
-
- \param col : Enable to choose the color space to use for the image which is projected.
-
+
+ \param col : Enable to choose the color space to use for the image which is
+ projected.
+
By default the class uses colored images.
*/
vpImageSimulator::vpImageSimulator(const vpColorPlan &col)
- : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
- distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
- euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
- vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
- colorI(col), Ig(), Ic(), rect(), cleanPrevImage(false),
- setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
+ : 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++)
+ for (int i = 0; i < 4; i++)
X[i].resize(3);
- for(int i=0;i<4;i++)
+ for (int i = 0; i < 4; i++)
X2[i].resize(3);
normal_obj.resize(3);
- visible=false;
+ visible = false;
normal_Cam.resize(3);
- //Xinter.resize(3);
+ // Xinter.resize(3);
vbase_u.resize(3);
vbase_v.resize(3);
-
+
focal.resize(3);
- focal=0;
- focal[2]=1;
+ focal = 0;
+ focal[2] = 1;
normal_Cam_optim = new double[3];
X0_2_optim = new double[3];
@@ -93,56 +93,52 @@ vpImageSimulator::vpImageSimulator(const vpColorPlan &col)
pt.resize(4);
}
-
/*!
Copy constructor
*/
vpImageSimulator::vpImageSimulator(const vpImageSimulator &text)
- : cMt(), pt(), ptClipped(), interp(SIMPLE), normal_obj(), normal_Cam(), normal_Cam_optim(),
- distance(1.), visible_result(1.), visible(false), X0_2_optim(NULL),
- euclideanNorm_u(0.), euclideanNorm_v(0.), vbase_u(), vbase_v(),
- vbase_u_optim(NULL), vbase_v_optim(NULL), Xinter_optim(NULL), listTriangle(),
- colorI(GRAY_SCALED), Ig(), Ic(), rect(), cleanPrevImage(false),
- setBackgroundTexture(false), bgColor(vpColor::white), focal(), needClipping(false)
+ : 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++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
X[i] = text.X[i];
pt[i] = text.pt[i];
}
- for(int i=0;i<4;i++)
+ for (int i = 0; i < 4; i++)
X2[i].resize(3);
-
+
Ic = text.Ic;
Ig = text.Ig;
-
+
focal.resize(3);
- focal=0;
- focal[2]=1;
+ focal = 0;
+ focal[2] = 1;
normal_obj = text.normal_obj;
euclideanNorm_u = text.euclideanNorm_u;
euclideanNorm_v = text.euclideanNorm_v;
-
+
normal_Cam.resize(3);
vbase_u.resize(3);
vbase_v.resize(3);
-
-
+
normal_Cam_optim = new double[3];
X0_2_optim = new double[3];
vbase_u_optim = new double[3];
vbase_v_optim = new double[3];
Xinter_optim = new double[3];
-
+
colorI = text.colorI;
interp = text.interp;
bgColor = text.bgColor;
cleanPrevImage = text.cleanPrevImage;
setBackgroundTexture = false;
-
+
setCameraPosition(text.cMt);
}
@@ -158,105 +154,88 @@ vpImageSimulator::~vpImageSimulator()
delete[] Xinter_optim;
}
-
-vpImageSimulator&
-vpImageSimulator::operator=(const vpImageSimulator& sim)
+vpImageSimulator &vpImageSimulator::operator=(const vpImageSimulator &sim)
{
- for(unsigned int i=0;i<4;i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
X[i] = sim.X[i];
pt[i] = sim.pt[i];
}
-
+
Ic = sim.Ic;
Ig = sim.Ig;
-
+
bgColor = sim.bgColor;
cleanPrevImage = sim.cleanPrevImage;
-
+
focal = sim.focal;
-
+
normal_obj = sim.normal_obj;
euclideanNorm_u = sim.euclideanNorm_u;
euclideanNorm_v = sim.euclideanNorm_v;
-
+
colorI = sim.colorI;
interp = sim.interp;
-
+
setCameraPosition(sim.cMt);
-
+
return *this;
}
/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
- \param I : The image used to store the result.
- \param cam : The parameters of the virtual camera.
+ Get the view of the virtual camera. Be careful, the image I is modified. The
+ projected image is not added as an overlay! \param I : The image used to
+ store the result. \param cam : The parameters of the virtual camera.
*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<unsigned char> &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);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ else {
+ if (cleanPrevImage) {
+ unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = col;
}
}
}
}
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
+
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
-
+ double right = rect.getRight();
+
unsigned char *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ if (colorI == GRAY_SCALED) {
unsigned char Ipixelplan = 0;
- if(getPixel(ip,Ipixelplan))
- {
- *(bitmap+i*width+j)=Ipixelplan;
+ if (getPixel(ip, Ipixelplan)) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
}
- }
- else if (colorI == COLORED)
- {
+ } else if (colorI == COLORED) {
vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
+ if (getPixel(ip, Ipixelplan)) {
+ unsigned char pixelgrey =
+ (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap + i * width + j) = pixelgrey;
nb_point_dessine++;
}
}
@@ -265,59 +244,50 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
}
}
-
/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
+ Get the view of the virtual camera. Be careful, the image I is modified. The
+ projected image is not added as an overlay! In this method you specify
+ directly the image which is projected.
\param I : The image used to store the result.
\param Isrc : The image which is projected into \f$ I \f$.
\param cam : The parameters of the virtual camera.
*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- vpImage<unsigned char> &Isrc,
- const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<unsigned char> &I, vpImage<unsigned char> &Isrc, const vpCameraParameters &cam)
{
- if (cleanPrevImage)
- {
+ if (cleanPrevImage) {
unsigned char col = (unsigned char)(0.2126 * bgColor.R + 0.7152 * bgColor.G + 0.0722 * bgColor.B);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = col;
}
}
}
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
+ double right = rect.getRight();
unsigned char *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
unsigned char Ipixelplan = 0;
- if(getPixel(Isrc,ip,Ipixelplan))
- {
- *(bitmap+i*width+j)=Ipixelplan;
+ if (getPixel(Isrc, ip, Ipixelplan)) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
}
}
@@ -326,79 +296,72 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
}
/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
-
- To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
-
+ Get the view of the virtual camera. Be careful, the image I is modified. The
+ projected image is not added as an overlay!
+
+ To take into account the projection of several images, a matrix \f$ zBuffer
+ \f$ is given as argument. This matrix contains the z coordinates of all the
+ pixel of the image \f$ I \f$ in the camera frame. During the projection, the
+ pixels are updated if there is no other plan between the camera and the
+ projected image. The matrix \f$ zBuffer \f$ is updated in this case.
+
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
- \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
+ \param zBuffer : A matrix containing the z coordinates of the pixels of the
+ image \f$ I \f$
*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- const vpCameraParameters &cam, vpMatrix &zBuffer)
+void vpImageSimulator::getImage(vpImage<unsigned char> &I, const vpCameraParameters &cam, vpMatrix &zBuffer)
{
if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
-
- if (cleanPrevImage)
- {
+ 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);
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = col;
}
}
}
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
+
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
-
+ double right = rect.getRight();
+
unsigned char *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ if (colorI == GRAY_SCALED) {
unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- *(bitmap+i*width+j)=Ipixelplan;
+ if (getPixel(ip, Ipixelplan)) {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
}
- }
- else if (colorI == COLORED)
- {
+ } else if (colorI == COLORED) {
vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
+ if (getPixel(ip, Ipixelplan)) {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
+ unsigned char pixelgrey =
+ (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap + i * width + j) = pixelgrey;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
@@ -410,69 +373,58 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
}
/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
-
+ Get the view of the virtual camera. Be careful, the image I is modified. The
+ projected image is not added as an overlay!
+
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
{
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ if (cleanPrevImage) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = bgColor;
}
}
}
-
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
+
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
-
+ double right = rect.getRight();
+
vpRGBa *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ if (colorI == GRAY_SCALED) {
unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
+ if (getPixel(ip, Ipixelplan)) {
vpRGBa pixelcolor;
pixelcolor.R = Ipixelplan;
pixelcolor.G = Ipixelplan;
pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
+ *(bitmap + i * width + j) = pixelcolor;
nb_point_dessine++;
}
- }
- else if (colorI == COLORED)
- {
+ } else if (colorI == COLORED) {
vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- *(bitmap+i*width+j) = Ipixelplan;
+ if (getPixel(ip, Ipixelplan)) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
}
}
@@ -481,58 +433,50 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam)
}
}
-
/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay! In this method you specify directly the image which is projected.
-
+ Get the view of the virtual camera. Be carefull, the image I is modified.
+ The projected image is not added as an overlay! In this method you specify
+ directly the image which is projected.
+
\param I : The image used to store the result.
\param Isrc : The image which is projected into \f$ I \f$.
\param cam : The parameters of the virtual camera.
*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
- const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc, const vpCameraParameters &cam)
{
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ if (cleanPrevImage) {
+ for (unsigned int i = 0; i < I.getHeight(); i++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = bgColor;
}
}
}
-
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
+
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
-
+ double right = rect.getRight();
+
vpRGBa *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
vpRGBa Ipixelplan;
- if(getPixel(Isrc,ip,Ipixelplan))
- {
- *(bitmap+i*width+j) = Ipixelplan;
+ if (getPixel(Isrc, ip, Ipixelplan)) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
}
}
@@ -541,81 +485,73 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I, vpImage<vpRGBa> &Isrc,
}
/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
-
- To take into account the projection of several images, a matrix \f$ zBuffer \f$ is given as argument. This matrix contains the z coordinates of all the pixel of the image \f$ I \f$ in the camera frame. During the projection, the pixels are updated if there is no other plan between the camera and the projected image. The matrix \f$ zBuffer \f$ is updated in this case.
-
+ Get the view of the virtual camera. Be carefull, the image I is modified.
+ The projected image is not added as an overlay!
+
+ To take into account the projection of several images, a matrix \f$ zBuffer
+ \f$ is given as argument. This matrix contains the z coordinates of all the
+ pixel of the image \f$ I \f$ in the camera frame. During the projection, the
+ pixels are updated if there is no other plan between the camera and the
+ projected image. The matrix \f$ zBuffer \f$ is updated in this case.
+
\param I : The image used to store the result.
\param cam : The parameters of the virtual camera.
- \param zBuffer : A matrix containing the z coordinates of the pixels of the image \f$ I \f$
+ \param zBuffer : A matrix containing the z coordinates of the pixels of the
+ image \f$ I \f$
*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
- vpMatrix &zBuffer)
+void vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam, vpMatrix &zBuffer)
{
if (I.getWidth() != (unsigned int)zBuffer.getCols() || I.getHeight() != (unsigned int)zBuffer.getRows())
- throw (vpMatrixException(vpMatrixException::incorrectMatrixSizeError, " zBuffer must have the same size as the image I ! "));
-
- if (cleanPrevImage)
- {
- for (unsigned int i = 0; i < I.getHeight(); i++)
- {
- for (unsigned int j = 0; j < I.getWidth(); j++)
- {
+ 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++) {
+ for (unsigned int j = 0; j < I.getWidth(); j++) {
I[i][j] = bgColor;
}
}
}
- if(visible)
- {
- if(!needClipping)
- getRoi(I.getWidth(),I.getHeight(),cam,pt,rect);
+ if (visible) {
+ if (!needClipping)
+ getRoi(I.getWidth(), I.getHeight(), cam, pt, rect);
else
- getRoi(I.getWidth(),I.getHeight(),cam,ptClipped,rect);
-
+ getRoi(I.getWidth(), I.getHeight(), cam, ptClipped, rect);
+
double top = rect.getTop();
double bottom = rect.getBottom();
double left = rect.getLeft();
- double right= rect.getRight();
-
+ double right = rect.getRight();
+
vpRGBa *bitmap = I.bitmap;
unsigned int width = I.getWidth();
vpImagePoint ip;
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++)
- {
- double x=0,y=0;
- ip.set_ij(i,j);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- if (colorI == GRAY_SCALED)
- {
+ for (unsigned int i = (unsigned int)top; i < (unsigned int)bottom; i++) {
+ for (unsigned int j = (unsigned int)left; j < (unsigned int)right; j++) {
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ if (colorI == GRAY_SCALED) {
unsigned char Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
+ if (getPixel(ip, Ipixelplan)) {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
vpRGBa pixelcolor;
pixelcolor.R = Ipixelplan;
pixelcolor.G = Ipixelplan;
pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
+ *(bitmap + i * width + j) = pixelcolor;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
}
- }
- else if (colorI == COLORED)
- {
+ } else if (colorI == COLORED) {
vpRGBa Ipixelplan;
- if(getPixel(ip,Ipixelplan))
- {
- if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0)
- {
- *(bitmap+i*width+j) = Ipixelplan;
+ if (getPixel(ip, Ipixelplan)) {
+ if (Xinter_optim[2] < zBuffer[i][j] || zBuffer[i][j] < 0) {
+ *(bitmap + i * width + j) = Ipixelplan;
nb_point_dessine++;
zBuffer[i][j] = Xinter_optim[2];
}
@@ -627,8 +563,12 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
}
/*!
- Get the view of the virtual camera. Be careful, the image I is modified. The projected image is not added as an overlay!
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
+ Get the view of the virtual camera. Be careful, the image I is modified. The
+ projected image is not added as an overlay! With this method, a list of
+ image is projected into the image. Thus, you have to initialise a list of
+ vpImageSimulator. Then you store them into a vpList. And finally with this
+ method you project them into the image \f$ I \f$. The depth of the 3D scene
+ is managed such as an image in foreground hides an image background.
The following example shows how to use the method:
@@ -726,10 +666,8 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I, const vpCameraParameters &cam,
\param list : List of vpImageSimulator to project
\param cam : The parameters of the virtual camera
*/
-void
-vpImageSimulator::getImage(vpImage<unsigned char> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<unsigned char> &I, std::list<vpImageSimulator> &list,
+ const vpCameraParameters &cam)
{
unsigned int width = I.getWidth();
@@ -740,17 +678,18 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
if (nbsimList < 1)
return;
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
+ vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
- double topFinal = height+1;;
+ double topFinal = height + 1;
+ ;
double bottomFinal = -1;
- double leftFinal = width+1;
+ double leftFinal = width + 1;
double rightFinal = -1;
unsigned int unvisible = 0;
- unsigned int indexSimu=0;
- for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
- vpImageSimulator* sim = &(*it);
+ unsigned int indexSimu = 0;
+ for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
+ vpImageSimulator *sim = &(*it);
if (sim->visible)
simList[indexSimu] = sim;
else
@@ -758,24 +697,25 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
}
nbsimList = nbsimList - unvisible;
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
-
+ if (nbsimList < 1) {
+ delete[] simList;
+ return;
+ }
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
+ for (unsigned int i = 0; i < nbsimList; i++) {
+ if (!simList[i]->needClipping)
+ simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
+ simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
+
+ if (topFinal > simList[i]->rect.getTop())
+ topFinal = simList[i]->rect.getTop();
+ if (bottomFinal < simList[i]->rect.getBottom())
+ bottomFinal = simList[i]->rect.getBottom();
+ if (leftFinal > simList[i]->rect.getLeft())
+ leftFinal = simList[i]->rect.getLeft();
+ if (rightFinal < simList[i]->rect.getRight())
+ rightFinal = simList[i]->rect.getRight();
}
double zmin = -1;
@@ -783,41 +723,33 @@ vpImageSimulator::getImage(vpImage<unsigned char> &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);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- *(bitmap+i*width+j)=Ipixelplan;
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ for (int k = 0; k < (int)nbsimList; k++) {
+ double z = 0;
+ if (simList[k]->getPixelDepth(ip, z)) {
+ if (z < zmin || zmin < 0) {
+ zmin = z;
+ indice = k;
+ }
}
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- unsigned char pixelgrey = (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
- *(bitmap+i*width+j)=pixelgrey;
+ }
+ if (indice >= 0) {
+ if (simList[indice]->colorI == GRAY_SCALED) {
+ unsigned char Ipixelplan = 255;
+ simList[indice]->getPixel(ip, Ipixelplan);
+ *(bitmap + i * width + j) = Ipixelplan;
+ } else if (simList[indice]->colorI == COLORED) {
+ vpRGBa Ipixelplan(255, 255, 255);
+ simList[indice]->getPixel(ip, Ipixelplan);
+ unsigned char pixelgrey =
+ (unsigned char)(0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B);
+ *(bitmap + i * width + j) = pixelgrey;
}
}
}
@@ -826,11 +758,15 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
delete[] simList;
}
-
/*!
- Get the view of the virtual camera. Be carefull, the image I is modified. The projected image is not added as an overlay!
+ Get the view of the virtual camera. Be carefull, the image I is modified.
+ The projected image is not added as an overlay!
- With this method, a list of image is projected into the image. Thus, you have to initialise a list of vpImageSimulator. Then you store them into a vpList. And finally with this method you project them into the image \f$ I \f$. The depth of the 3D scene is managed such as an image in foreground hides an image background.
+ With this method, a list of image is projected into the image. Thus, you
+ have to initialise a list of vpImageSimulator. Then you store them into a
+ vpList. And finally with this method you project them into the image \f$ I
+ \f$. The depth of the 3D scene is managed such as an image in foreground
+ hides an image background.
The following example shows how to use the method:
@@ -928,10 +864,7 @@ vpImageSimulator::getImage(vpImage<unsigned char> &I,
\param list : List of vpImageSimulator to project
\param cam : The parameters of the virtual camera
*/
-void
-vpImageSimulator::getImage(vpImage<vpRGBa> &I,
- std::list<vpImageSimulator> &list,
- const vpCameraParameters &cam)
+void vpImageSimulator::getImage(vpImage<vpRGBa> &I, std::list<vpImageSimulator> &list, const vpCameraParameters &cam)
{
unsigned int width = I.getWidth();
@@ -942,17 +875,18 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I,
if (nbsimList < 1)
return;
- vpImageSimulator** simList = new vpImageSimulator* [nbsimList];
+ vpImageSimulator **simList = new vpImageSimulator *[nbsimList];
- double topFinal = height+1;;
+ double topFinal = height + 1;
+ ;
double bottomFinal = -1;
- double leftFinal = width+1;
+ double leftFinal = width + 1;
double rightFinal = -1;
unsigned int unvisible = 0;
unsigned int indexSimu = 0;
- for(std::list<vpImageSimulator>::iterator it=list.begin(); it!=list.end(); ++it, ++indexSimu){
- vpImageSimulator* sim = &(*it);
+ for (std::list<vpImageSimulator>::iterator it = list.begin(); it != list.end(); ++it, ++indexSimu) {
+ vpImageSimulator *sim = &(*it);
if (sim->visible)
simList[indexSimu] = sim;
else
@@ -961,23 +895,25 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I,
nbsimList = nbsimList - unvisible;
- if (nbsimList < 1)
- {
- delete[] simList;
- return;
- }
+ if (nbsimList < 1) {
+ delete[] simList;
+ return;
+ }
- for (unsigned int i = 0; i < nbsimList; i++)
- {
- if(!simList[i]->needClipping)
- simList[i]->getRoi(width,height,cam,simList[i]->pt,simList[i]->rect);
+ for (unsigned int i = 0; i < nbsimList; i++) {
+ if (!simList[i]->needClipping)
+ simList[i]->getRoi(width, height, cam, simList[i]->pt, simList[i]->rect);
else
- simList[i]->getRoi(width,height,cam,simList[i]->ptClipped,simList[i]->rect);
-
- if (topFinal > simList[i]->rect.getTop()) topFinal = simList[i]->rect.getTop();
- if (bottomFinal < simList[i]->rect.getBottom()) bottomFinal = simList[i]->rect.getBottom();
- if (leftFinal > simList[i]->rect.getLeft()) leftFinal = simList[i]->rect.getLeft();
- if (rightFinal < simList[i]->rect.getRight()) rightFinal = simList[i]->rect.getRight();
+ simList[i]->getRoi(width, height, cam, simList[i]->ptClipped, simList[i]->rect);
+
+ if (topFinal > simList[i]->rect.getTop())
+ topFinal = simList[i]->rect.getTop();
+ if (bottomFinal < simList[i]->rect.getBottom())
+ bottomFinal = simList[i]->rect.getBottom();
+ if (leftFinal > simList[i]->rect.getLeft())
+ leftFinal = simList[i]->rect.getLeft();
+ if (rightFinal < simList[i]->rect.getRight())
+ rightFinal = simList[i]->rect.getRight();
}
double zmin = -1;
@@ -985,45 +921,37 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &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);
- vpPixelMeterConversion::convertPoint(cam,ip, x,y);
- ip.set_ij(y,x);
- for (int k = 0; k < (int)nbsimList; k++)
- {
- double z = 0;
- if(simList[k]->getPixelDepth(ip,z))
- {
- if (z < zmin || zmin < 0)
- {
- zmin = z;
- indice = k;
- }
- }
- }
- if (indice >= 0)
- {
- if (simList[indice]->colorI == GRAY_SCALED)
- {
- unsigned char Ipixelplan = 255;
- simList[indice]->getPixel(ip,Ipixelplan);
- vpRGBa pixelcolor;
- pixelcolor.R = Ipixelplan;
- pixelcolor.G = Ipixelplan;
- pixelcolor.B = Ipixelplan;
- *(bitmap+i*width+j) = pixelcolor;
+ double x = 0, y = 0;
+ ip.set_ij(i, j);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ ip.set_ij(y, x);
+ for (int k = 0; k < (int)nbsimList; k++) {
+ double z = 0;
+ if (simList[k]->getPixelDepth(ip, z)) {
+ if (z < zmin || zmin < 0) {
+ zmin = z;
+ indice = k;
+ }
}
- else if (simList[indice]->colorI == COLORED)
- {
- vpRGBa Ipixelplan(255,255,255);
- simList[indice]->getPixel(ip,Ipixelplan);
- //unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 * Ipixelplan.G + 0.0722 * Ipixelplan.B;
- *(bitmap+i*width+j)=Ipixelplan;
+ }
+ if (indice >= 0) {
+ if (simList[indice]->colorI == GRAY_SCALED) {
+ unsigned char Ipixelplan = 255;
+ simList[indice]->getPixel(ip, Ipixelplan);
+ vpRGBa pixelcolor;
+ pixelcolor.R = Ipixelplan;
+ pixelcolor.G = Ipixelplan;
+ pixelcolor.B = Ipixelplan;
+ *(bitmap + i * width + j) = pixelcolor;
+ } else if (simList[indice]->colorI == COLORED) {
+ vpRGBa Ipixelplan(255, 255, 255);
+ simList[indice]->getPixel(ip, Ipixelplan);
+ // unsigned char pixelgrey = 0.2126 * Ipixelplan.R + 0.7152 *
+ // Ipixelplan.G + 0.0722 * Ipixelplan.B;
+ *(bitmap + i * width + j) = Ipixelplan;
}
}
}
@@ -1034,71 +962,64 @@ vpImageSimulator::getImage(vpImage<vpRGBa> &I,
/*!
Enable to set the position of the 3D plane relative to the virtual camera.
-
+
\param cMt_ : The pose of the plane relative to the virtual camera.
*/
-void
-vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
+void vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
{
cMt = cMt_;
vpRotationMatrix R;
cMt.extract(R);
needClipping = false;
- normal_Cam = R * normal_obj;
-
- visible_result = vpColVector::dotProd(normal_Cam,focal);
-
- for(unsigned int i = 0; i < 4; i++)
+ normal_Cam = R * normal_obj;
+
+ visible_result = vpColVector::dotProd(normal_Cam, focal);
+
+ for (unsigned int i = 0; i < 4; i++)
pt[i].track(cMt);
-
- vpColVector e1(3) ;
- vpColVector e2(3) ;
- vpColVector facenormal(3) ;
- e1[0] = pt[1].get_X() - pt[0].get_X() ;
- e1[1] = pt[1].get_Y() - pt[0].get_Y() ;
- e1[2] = pt[1].get_Z() - pt[0].get_Z() ;
+ vpColVector e1(3);
+ vpColVector e2(3);
+ vpColVector facenormal(3);
- e2[0] = pt[2].get_X() - pt[1].get_X() ;
- e2[1] = pt[2].get_Y() - pt[1].get_Y() ;
- e2[2] = pt[2].get_Z() - pt[1].get_Z() ;
+ e1[0] = pt[1].get_X() - pt[0].get_X();
+ e1[1] = pt[1].get_Y() - pt[0].get_Y();
+ e1[2] = pt[1].get_Z() - pt[0].get_Z();
- facenormal = vpColVector::crossProd(e1,e2) ;
+ e2[0] = pt[2].get_X() - pt[1].get_X();
+ e2[1] = pt[2].get_Y() - pt[1].get_Y();
+ e2[2] = pt[2].get_Z() - pt[1].get_Z();
- double angle = pt[0].get_X()*facenormal[0] + pt[0].get_Y()*facenormal[1] + pt[0].get_Z()*facenormal[2] ;
+ facenormal = vpColVector::crossProd(e1, e2);
- if (angle > 0){
- visible=true;
- }
- else {
- visible=false;
+ double angle = pt[0].get_X() * facenormal[0] + pt[0].get_Y() * facenormal[1] + pt[0].get_Z() * facenormal[2];
+
+ if (angle > 0) {
+ visible = true;
+ } else {
+ visible = false;
}
- if(visible)
- {
- for(unsigned int i = 0; i < 4; i++)
- {
- project(X[i],cMt,X2[i]);
+ if (visible) {
+ for (unsigned int i = 0; i < 4; i++) {
+ project(X[i], cMt, X2[i]);
pt[i].track(cMt);
- if(pt[i].get_Z() < 0)
+ if (pt[i].get_Z() < 0)
needClipping = true;
}
- vbase_u = X2[1]-X2[0];
- vbase_v = X2[3]-X2[0];
+ vbase_u = X2[1] - X2[0];
+ vbase_v = X2[3] - X2[0];
- distance = vpColVector::dotProd(normal_Cam,X2[1]);
-
+ distance = vpColVector::dotProd(normal_Cam, X2[1]);
- if(distance < 0)
- {
+ if (distance < 0) {
visible = false;
return;
}
- for(unsigned int i = 0; i < 3; i++)
- {
+ for (unsigned int i = 0; i < 3; i++) {
normal_Cam_optim[i] = normal_Cam[i];
X0_2_optim[i] = X2[0][i];
vbase_u_optim[i] = vbase_u[i];
@@ -1106,13 +1027,13 @@ vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
}
std::vector<vpPoint> *ptPtr = &pt;
- if(needClipping){
- vpPolygon3D::getClippedPolygon(pt,ptClipped,cMt,vpPolygon3D::NEAR_CLIPPING);
- ptPtr = &ptClipped;
+ if (needClipping) {
+ vpPolygon3D::getClippedPolygon(pt, ptClipped, cMt, vpPolygon3D::NEAR_CLIPPING);
+ ptPtr = &ptClipped;
}
listTriangle.clear();
- for(unsigned int i = 1 ; i < (*ptPtr).size()-1 ; i++){
+ for (unsigned int i = 1; i < (*ptPtr).size() - 1; i++) {
vpImagePoint ip1, ip2, ip3;
ip1.set_j((*ptPtr)[0].get_x());
ip1.set_i((*ptPtr)[0].get_y());
@@ -1120,98 +1041,98 @@ vpImageSimulator::setCameraPosition(const vpHomogeneousMatrix &cMt_)
ip2.set_j((*ptPtr)[i].get_x());
ip2.set_i((*ptPtr)[i].get_y());
- ip3.set_j((*ptPtr)[i+1].get_x());
- ip3.set_i((*ptPtr)[i+1].get_y());
+ ip3.set_j((*ptPtr)[i + 1].get_x());
+ ip3.set_i((*ptPtr)[i + 1].get_y());
- vpTriangle tri(ip1,ip2,ip3);
+ vpTriangle tri(ip1, ip2, ip3);
listTriangle.push_back(tri);
}
}
}
-void
-vpImageSimulator::initPlan(vpColVector* X_)
+void vpImageSimulator::initPlan(vpColVector *X_)
{
- for (unsigned int i = 0; i < 4; i++)
- {
- X[i]=X_[i];
- pt[i].setWorldCoordinates(X_[i][0],X_[i][1],X_[i][2]);
+ for (unsigned int i = 0; i < 4; i++) {
+ X[i] = X_[i];
+ pt[i].setWorldCoordinates(X_[i][0], X_[i][1], X_[i][2]);
}
- normal_obj=vpColVector::crossProd(X[1]-X[0],X[3]-X[0]);
- normal_obj=normal_obj/normal_obj.euclideanNorm();
+ normal_obj = vpColVector::crossProd(X[1] - X[0], X[3] - X[0]);
+ normal_obj = normal_obj / normal_obj.euclideanNorm();
- euclideanNorm_u=(X[1]-X[0]).euclideanNorm();
- euclideanNorm_v=(X[3]-X[0]).euclideanNorm();
+ euclideanNorm_u = (X[1] - X[0]).euclideanNorm();
+ euclideanNorm_v = (X[3] - X[0]).euclideanNorm();
}
/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector
+ containing the 3D coordinates of the image's corners.
+
The table must have a size of 4!
-
+
- \f$ X[0] \f$ :Top left corner.
- \f$ X[1] \f$ :Top right corner.
- \f$ X[2] \f$ :Bottom right corner.
- \f$ X[3] \f$ :Bottom left corner.
-
+
\param I : The image which is projected.
\param X_ : table of the 3D coordinates corresponding to the image corners.
*/
-void
-vpImageSimulator::init(const vpImage<unsigned char> &I,vpColVector* X_)
+void vpImageSimulator::init(const vpImage<unsigned char> &I, vpColVector *X_)
{
Ig = I;
- vpImageConvert::convert(I,Ic);
+ vpImageConvert::convert(I, Ic);
initPlan(X_);
}
/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector
+ containing the 3D coordinates of the image's corners.
+
The table must have a size of 4!
-
+
- \f$ X[0] \f$ :Top left corner.
- \f$ X[1] \f$ :Top right corner.
- \f$ X[2] \f$ :Bottom right corner.
- \f$ X[3] \f$ :Bottom left corner.
-
+
\param I : The image which is projected.
\param X_ : table of the 3D coordinates corresponding to the image corners.
*/
-void
-vpImageSimulator::init(const vpImage<vpRGBa> &I,vpColVector* X_)
+void vpImageSimulator::init(const vpImage<vpRGBa> &I, vpColVector *X_)
{
Ic = I;
- vpImageConvert::convert(I,Ig);
+ vpImageConvert::convert(I, Ig);
initPlan(X_);
}
#ifdef VISP_HAVE_MODULE_IO
/*!
- Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
-
+ Initialise the image thanks to an image whose adress is given by \f$
+ file_image \f$ and a table of vector containing the 3D coordinates of the
+ image's corners.
+
The table must have a size of 4!
-
+
- \f$ X[0] \f$ :Top left corner.
- \f$ X[1] \f$ :Top right corner.
- \f$ X[2] \f$ :Bottom right corner.
- \f$ X[3] \f$ :Bottom left corner.
-
+
\param file_image : The adress of an image file.
\param X_ : table of the 3D coordinates corresponding to the image corners.
*/
-void
-vpImageSimulator::init(const char* file_image,vpColVector* X_)
+void vpImageSimulator::init(const char *file_image, vpColVector *X_)
{
- vpImageIo::read(Ig,file_image);
- vpImageIo::read(Ic,file_image);
+ vpImageIo::read(Ig, file_image);
+ vpImageIo::read(Ic, file_image);
initPlan(X_);
}
#endif
/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector
+ containing the 3D coordinates of the image's corners.
\throw vpException::dimensionError if the _X vector is not of size 4.
@@ -1224,14 +1145,13 @@ 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<unsigned char> &I, const std::vector<vpPoint>& X_)
+void vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoint> &X_)
{
- if(X_.size() != 4){
+ if (X_.size() != 4) {
throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
}
vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
+ for (unsigned int i = 0; i < 4; ++i) {
Xvec[i].resize(3);
Xvec[i][0] = X_[i].get_oX();
Xvec[i][1] = X_[i].get_oY();
@@ -1239,11 +1159,12 @@ vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoin
}
Ig = I;
- vpImageConvert::convert(I,Ic);
+ vpImageConvert::convert(I, Ic);
initPlan(Xvec);
}
/*!
- Initialise the image thanks to an image \f$ I \f$ and a table of vector containing the 3D coordinates of the image's corners.
+ Initialise the image thanks to an image \f$ I \f$ and a table of vector
+ containing the 3D coordinates of the image's corners.
\throw vpException::dimensionError if the X_ vector is not of size 4.
@@ -1256,14 +1177,13 @@ vpImageSimulator::init(const vpImage<unsigned char> &I, const std::vector<vpPoin
\param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
corresponding to the image corners.
*/
-void
-vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X_)
+void vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint> &X_)
{
- if(X_.size() != 4){
+ if (X_.size() != 4) {
throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
}
vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
+ for (unsigned int i = 0; i < 4; ++i) {
Xvec[i].resize(3);
Xvec[i][0] = X_[i].get_oX();
Xvec[i][1] = X_[i].get_oY();
@@ -1271,13 +1191,15 @@ vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X_)
}
Ic = I;
- vpImageConvert::convert(I,Ig);
+ vpImageConvert::convert(I, Ig);
initPlan(Xvec);
}
#ifdef VISP_HAVE_MODULE_IO
/*!
- Initialise the image thanks to an image whose adress is given by \f$ file_image \f$ and a table of vector containing the 3D coordinates of the image's corners.
+ Initialise the image thanks to an image whose adress is given by \f$
+ file_image \f$ and a table of vector containing the 3D coordinates of the
+ image's corners.
\throw vpException::dimensionError if the X_ vector is not of size 4.
@@ -1290,357 +1212,346 @@ vpImageSimulator::init(const vpImage<vpRGBa> &I, const std::vector<vpPoint>& X_)
\param X_ : Vector of the 3D coordinates in the object frame (oX, oY, oZ)
corresponding to the image corners.
*/
-void
-vpImageSimulator::init(const char* file_image, const std::vector<vpPoint>& X_)
+void vpImageSimulator::init(const char *file_image, const std::vector<vpPoint> &X_)
{
- if(X_.size() != 4){
+ if (X_.size() != 4) {
throw vpException(vpException::dimensionError, "the vector must contains 4 points to initialise the simulator");
}
vpColVector Xvec[4];
- for(unsigned int i=0; i<4; ++i){
+ for (unsigned int i = 0; i < 4; ++i) {
Xvec[i].resize(3);
Xvec[i][0] = X_[i].get_oX();
Xvec[i][1] = X_[i].get_oY();
Xvec[i][2] = X_[i].get_oZ();
}
- vpImageIo::read(Ig,file_image);
- vpImageIo::read(Ic,file_image);
+ vpImageIo::read(Ig, file_image);
+ vpImageIo::read(Ic, file_image);
initPlan(Xvec);
}
#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
+ // std::cout << "In get Pixel" << std::endl;
+ // test si pixel dans zone projetee
bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
-//// std::cout << "The pixel is inside the projected area" << std::endl;
-// return false;}
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP)){
+ //// std::cout << "The pixel is inside the projected area" <<
+ /// std::endl;
+ // return false;}
- //methoed algebrique
+ // methoed algebrique
double z;
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
+ // calcul de la profondeur de l'intersection
+ z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
+ // calcul coordonnees 3D intersection
+ Xinter_optim[0] = iP.get_u() * z;
+ Xinter_optim[1] = iP.get_v() * z;
+ Xinter_optim[2] = z;
- //recuperation des coordonnes de l'intersection dans le plan objet
- //repere plan object :
+ // recuperation des coordonnes de l'intersection dans le plan objet
+ // repere plan object :
// centre = X0_2_optim[i] (premier point definissant le plan)
// base = u:(X[1]-X[0]) et v:(X[3]-X[0])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ // ici j'ai considere que le plan est un rectangle => coordonnees sont
+ // simplement obtenu par un produit scalaire
double u = 0, v = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- double diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ double diff = (Xinter_optim[i] - X0_2_optim[i]);
+ u += diff * vbase_u_optim[i];
+ v += diff * vbase_v_optim[i];
}
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
+ u = u / (euclideanNorm_u * euclideanNorm_u);
+ v = v / (euclideanNorm_v * euclideanNorm_v);
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Ig.getHeight()-1);
- j2=u*(Ig.getWidth()-1);
+ if (u > 0 && v > 0 && u < 1. && v < 1.) {
+ double i2, j2;
+ i2 = v * (Ig.getHeight() - 1);
+ j2 = u * (Ig.getWidth() - 1);
if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Ig.getValue(i2,j2);
+ Ipixelplan = Ig.getValue(i2, j2);
else if (interp == SIMPLE)
Ipixelplan = Ig[(unsigned int)i2][(unsigned int)j2];
return true;
- }
- else
+ } else
return false;
}
-bool
-vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc,
- const vpImagePoint &iP, unsigned char &Ipixelplan)
+bool vpImageSimulator::getPixel(vpImage<unsigned char> &Isrc, const vpImagePoint &iP, unsigned char &Ipixelplan)
{
- //test si pixel dans zone projetee
+ // test si pixel dans zone projetee
bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+ // return false;
- //methoed algebrique
+ // methoed algebrique
double z;
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
+ // calcul de la profondeur de l'intersection
+ z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
+ // calcul coordonnees 3D intersection
+ Xinter_optim[0] = iP.get_u() * z;
+ Xinter_optim[1] = iP.get_v() * z;
+ Xinter_optim[2] = z;
- //recuperation des coordonnes de l'intersection dans le plan objet
- //repere plan object :
+ // recuperation des coordonnes de l'intersection dans le plan objet
+ // repere plan object :
// centre = X0_2_optim[i] (premier point definissant le plan)
// base = u:(X[1]-X[0]) et v:(X[3]-X[0])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ // ici j'ai considere que le plan est un rectangle => coordonnees sont
+ // simplement obtenu par un produit scalaire
double u = 0, v = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- double diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ double diff = (Xinter_optim[i] - X0_2_optim[i]);
+ u += diff * vbase_u_optim[i];
+ v += diff * vbase_v_optim[i];
}
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
+ u = u / (euclideanNorm_u * euclideanNorm_u);
+ v = v / (euclideanNorm_v * euclideanNorm_v);
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Isrc.getHeight()-1);
- j2=u*(Isrc.getWidth()-1);
+ if (u > 0 && v > 0 && u < 1. && v < 1.) {
+ double i2, j2;
+ i2 = v * (Isrc.getHeight() - 1);
+ j2 = u * (Isrc.getWidth() - 1);
if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Isrc.getValue(i2,j2);
+ Ipixelplan = Isrc.getValue(i2, j2);
else if (interp == SIMPLE)
Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
return true;
- }
- else
+ } else
return false;
}
-
-bool
-vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
+bool vpImageSimulator::getPixel(const vpImagePoint &iP, vpRGBa &Ipixelplan)
{
- //test si pixel dans zone projetee
+ // test si pixel dans zone projetee
bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+ // return false;
- //methoed algebrique
+ // methoed algebrique
double z;
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
+ // calcul de la profondeur de l'intersection
+ z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
+ // calcul coordonnees 3D intersection
+ Xinter_optim[0] = iP.get_u() * z;
+ Xinter_optim[1] = iP.get_v() * z;
+ Xinter_optim[2] = z;
- //recuperation des coordonnes de l'intersection dans le plan objet
- //repere plan object :
+ // recuperation des coordonnes de l'intersection dans le plan objet
+ // repere plan object :
// centre = X0_2_optim[i] (premier point definissant le plan)
// base = u:(X[1]-X[0]) et v:(X[3]-X[0])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ // ici j'ai considere que le plan est un rectangle => coordonnees sont
+ // simplement obtenu par un produit scalaire
double u = 0, v = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- double diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ double diff = (Xinter_optim[i] - X0_2_optim[i]);
+ u += diff * vbase_u_optim[i];
+ v += diff * vbase_v_optim[i];
}
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
+ u = u / (euclideanNorm_u * euclideanNorm_u);
+ v = v / (euclideanNorm_v * euclideanNorm_v);
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Ic.getHeight()-1);
- j2=u*(Ic.getWidth()-1);
+ if (u > 0 && v > 0 && u < 1. && v < 1.) {
+ double i2, j2;
+ i2 = v * (Ic.getHeight() - 1);
+ j2 = u * (Ic.getWidth() - 1);
if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Ic.getValue(i2,j2);
+ Ipixelplan = Ic.getValue(i2, j2);
else if (interp == SIMPLE)
Ipixelplan = Ic[(unsigned int)i2][(unsigned int)j2];
return true;
- }
- else
+ } else
return false;
}
-bool
-vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP,
- vpRGBa &Ipixelplan)
+bool vpImageSimulator::getPixel(vpImage<vpRGBa> &Isrc, const vpImagePoint &iP, vpRGBa &Ipixelplan)
{
- //test si pixel dans zone projetee
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
+ // test si pixel dans zone projetee
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+ // return false;
bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
- //methoed algebrique
+ // methoed algebrique
double z;
- //calcul de la profondeur de l'intersection
- z = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
- //calcul coordonnees 3D intersection
- Xinter_optim[0]=iP.get_u()*z;
- Xinter_optim[1]=iP.get_v()*z;
- Xinter_optim[2]=z;
+ // calcul de la profondeur de l'intersection
+ z = distance / (normal_Cam_optim[0] * iP.get_u() + normal_Cam_optim[1] * iP.get_v() + normal_Cam_optim[2]);
+ // calcul coordonnees 3D intersection
+ Xinter_optim[0] = iP.get_u() * z;
+ Xinter_optim[1] = iP.get_v() * z;
+ Xinter_optim[2] = z;
- //recuperation des coordonnes de l'intersection dans le plan objet
- //repere plan object :
+ // recuperation des coordonnes de l'intersection dans le plan objet
+ // repere plan object :
// centre = X0_2_optim[i] (premier point definissant le plan)
// base = u:(X[1]-X[0]) et v:(X[3]-X[0])
- //ici j'ai considere que le plan est un rectangle => coordonnees sont simplement obtenu par un produit scalaire
+ // ici j'ai considere que le plan est un rectangle => coordonnees sont
+ // simplement obtenu par un produit scalaire
double u = 0, v = 0;
- for(unsigned int i = 0; i < 3; i++)
- {
- double diff = (Xinter_optim[i]-X0_2_optim[i]);
- u += diff*vbase_u_optim[i];
- v += diff*vbase_v_optim[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ double diff = (Xinter_optim[i] - X0_2_optim[i]);
+ u += diff * vbase_u_optim[i];
+ v += diff * vbase_v_optim[i];
}
- u = u/(euclideanNorm_u*euclideanNorm_u);
- v = v/(euclideanNorm_v*euclideanNorm_v);
+ u = u / (euclideanNorm_u * euclideanNorm_u);
+ v = v / (euclideanNorm_v * euclideanNorm_v);
- if( u > 0 && v > 0 && u < 1. && v < 1.)
- {
- double i2,j2;
- i2=v*(Isrc.getHeight()-1);
- j2=u*(Isrc.getWidth()-1);
+ if (u > 0 && v > 0 && u < 1. && v < 1.) {
+ double i2, j2;
+ i2 = v * (Isrc.getHeight() - 1);
+ j2 = u * (Isrc.getWidth() - 1);
if (interp == BILINEAR_INTERPOLATION)
- Ipixelplan = Isrc.getValue(i2,j2);
+ Ipixelplan = Isrc.getValue(i2, j2);
else if (interp == SIMPLE)
Ipixelplan = Isrc[(unsigned int)i2][(unsigned int)j2];
return true;
- }
- else
+ } else
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;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
+ // test si pixel dans zone projetee
+ bool inside = false;
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+ // return false;
- Zpixelplan = distance/(normal_Cam_optim[0]*iP.get_u()+normal_Cam_optim[1]*iP.get_v()+normal_Cam_optim[2]);
+ 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
+ // test si pixel dans zone projetee
bool inside = false;
- for(unsigned int i = 0 ; i < listTriangle.size() ; i++)
- if(listTriangle[i].inTriangle(iP)){
- inside = true;
- break;
- }
- if(!inside) return false;
-// if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
-// return false;
-
+ for (unsigned int i = 0; i < listTriangle.size(); i++)
+ if (listTriangle[i].inTriangle(iP)) {
+ inside = true;
+ break;
+ }
+ if (!inside)
+ return false;
+ // if(!T1.inTriangle(iP) && !T2.inTriangle(iP))
+ // return false;
+
Visipixelplan = visible_result;
return true;
}
-void
-vpImageSimulator::project(const vpColVector &_vin,
- const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
+void vpImageSimulator::project(const vpColVector &_vin, const vpHomogeneousMatrix &_cMt, vpColVector &_vout)
{
vpColVector XH(4);
- getHomogCoord(_vin,XH);
- getCoordFromHomog(_cMt*XH,_vout);
+ getHomogCoord(_vin, XH);
+ getCoordFromHomog(_cMt * XH, _vout);
}
-void
-vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
+void vpImageSimulator::getHomogCoord(const vpColVector &_v, vpColVector &_vH)
{
- for(unsigned int i=0;i<3;i++)
- _vH[i]=_v[i];
- _vH[3]=1.;
+ for (unsigned int i = 0; i < 3; i++)
+ _vH[i] = _v[i];
+ _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];
+ for (unsigned int i = 0; i < 3; i++)
+ _v[i] = _vH[i] / _vH[3];
}
-
-void
-vpImageSimulator::getRoi(const unsigned int &Iwidth,
- const unsigned int &Iheight,
- const vpCameraParameters &cam,
- const std::vector<vpPoint> &point,
- vpRect &rectangle)
+void vpImageSimulator::getRoi(const unsigned int &Iwidth, const unsigned int &Iheight, const vpCameraParameters &cam,
+ const std::vector<vpPoint> &point, vpRect &rectangle)
{
- double top = Iheight+1;
+ double top = Iheight + 1;
double bottom = -1;
double right = -1;
- double left= Iwidth+1;
-
- for( unsigned int i = 0; i < point.size(); i++)
- {
- double u=0,v=0;
- vpMeterPixelConversion::convertPoint(cam,point[i].get_x(),point[i].get_y(),u,v);
- if (v < top) top = v;
- if (v > bottom) bottom = v;
- if (u < left) left = u;
- if (u > right) right = u;
+ double left = Iwidth + 1;
+
+ for (unsigned int i = 0; i < point.size(); i++) {
+ double u = 0, v = 0;
+ vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), u, v);
+ if (v < top)
+ top = v;
+ if (v > bottom)
+ bottom = v;
+ if (u < left)
+ left = u;
+ if (u > right)
+ right = u;
}
- if (top < 0) top = 0;
- if(top >= Iheight) top = Iheight-1;
- if (bottom < 0) bottom = 0;
- if(bottom >= Iheight) bottom = Iheight-1;
- if(left < 0) left = 0;
- if(left >= Iwidth) left = Iwidth-1;
- if(right < 0) right = 0;
- if(right >= Iwidth) right = Iwidth-1;
-
+ if (top < 0)
+ top = 0;
+ if (top >= Iheight)
+ top = Iheight - 1;
+ if (bottom < 0)
+ bottom = 0;
+ if (bottom >= Iheight)
+ bottom = Iheight - 1;
+ if (left < 0)
+ left = 0;
+ if (left >= Iwidth)
+ left = Iwidth - 1;
+ if (right < 0)
+ right = 0;
+ if (right >= Iwidth)
+ right = Iwidth - 1;
+
rectangle.setTop(top);
rectangle.setBottom(bottom);
rectangle.setLeft(left);
rectangle.setRight(right);
}
-std::vector<vpColVector>
-vpImageSimulator::get3DcornersTextureRectangle()
+std::vector<vpColVector> vpImageSimulator::get3DcornersTextureRectangle()
{
std::vector<vpColVector> X_;
- for (int i=0; i<4; i++)
+ for (int i = 0; i < 4; i++)
X_.push_back(X[i]);
return X_;
}
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpImageSimulator& /*ip*/)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpImageSimulator & /*ip*/)
{
os << "";
return os;
diff --git a/modules/robot/src/light/vpRingLight.cpp b/modules/robot/src/light/vpRingLight.cpp
index e79d09b..c01ce66 100644
--- a/modules/robot/src/light/vpRingLight.cpp
+++ b/modules/robot/src/light/vpRingLight.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,31 +36,30 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_MODULE_IO) && defined(VISP_HAVE_PARPORT)
-# include <sys/types.h>
-# include <sys/stat.h>
-# include <fcntl.h>
-# include <sys/ioctl.h>
-# include <sys/time.h>
-# include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
-# include <visp3/robot/vpRingLight.h>
-# include <visp3/core/vpDebug.h>
-# include <visp3/core/vpTime.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRingLight.h>
/*!
\file vpRingLight.cpp
\brief Ring light management under unix.
*/
-
/*!
- Constructor to acces to the ring light device connected to the parallel port.
+ Constructor to acces to the ring light device connected to the parallel
+ port.
Open and initialise the default parallel port device "/dev/parport0" to
communicate with the ring light.
@@ -70,10 +70,7 @@
Turn the ring light off.
*/
-vpRingLight::vpRingLight() : parport()
-{
- off();
-}
+vpRingLight::vpRingLight() : parport() { off(); }
/*!
Destructor to close the device.
@@ -83,16 +80,13 @@ vpRingLight::vpRingLight() : parport()
\exception vpParallelPortException::closing If the device used to access to
the parallel port can't be closed.
*/
-vpRingLight::~vpRingLight()
-{
- off();
-}
+vpRingLight::~vpRingLight() { off(); }
/*!
Activates the ring light by sending a pulse throw the parallel port.
- The pulse width is 500 us. This pulse activates a NE555 which turns on on the
- light during 10ms.
+ The pulse width is 500 us. This pulse activates a NE555 which turns on on
+ the light during 10ms.
*/
void vpRingLight::pulse()
@@ -112,20 +106,20 @@ void vpRingLight::pulse()
// vpTRACE("Actual data 0x%x = %d\n", data, data);
data = data | mask_pulse_d1 | mask_mode_pulse_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data); // send a 0-1 pulse
// Wait 500 micro seconds
struct timeval ti, tc; // Initial and current time
struct timeval tempo;
tempo.tv_usec = 500;
- gettimeofday(&ti,0L);
+ gettimeofday(&ti, 0L);
do {
- gettimeofday(&tc,0L);
+ gettimeofday(&tc, 0L);
} while (tc.tv_usec < ti.tv_usec + tempo.tv_usec);
data = data & (~mask_pulse_d1);
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data); // send a 1-0 pulse
}
@@ -155,18 +149,18 @@ void vpRingLight::pulse(double time)
// vpTRACE("Actual data 0x%x = %d\n", data, data);
data = data | mask_pulse_d1 | mask_mode_pulse_d3;
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data); // send a 0-1 pulse
// Wait 500 micro seconds
struct timeval ti, tc; // Initial and current time
- gettimeofday(&ti,0);
+ gettimeofday(&ti, 0);
do {
- gettimeofday(&tc,0);
- } while (tc.tv_usec < ti.tv_usec + time*1000);
+ gettimeofday(&tc, 0);
+ } while (tc.tv_usec < ti.tv_usec + time * 1000);
data = data & (~mask_pulse_d1);
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data); // send a 1-0 pulse
}
@@ -187,12 +181,12 @@ void vpRingLight::on()
// To activates the light we send a pulse
int mask_mode_onoff_d2 = 0x04; // D2 is Hight
- int mask_on_d1 = 0x02; // D1 is Hight to turn the light on
+ int mask_on_d1 = 0x02; // D1 is Hight to turn the light on
unsigned char data = 0x00;
- //data = parport.getData(); // actual value of the data bus
+ // data = parport.getData(); // actual value of the data bus
data = data | mask_on_d1 | mask_mode_onoff_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data);
}
@@ -213,16 +207,17 @@ void vpRingLight::off()
// To activates the light we send a pulse
int mask_mode_onoff_d2 = 0x04; // D2 is Hight
- int mask_off_d1 = 0x00; // D1 is Low to turn the light off
+ int mask_off_d1 = 0x00; // D1 is Low to turn the light off
unsigned char data = 0x00;
- //data = parport.getData(); // actual value of the data bus
+ // data = parport.getData(); // actual value of the data bus
data = data | mask_off_d1 | mask_mode_onoff_d2;
- //vpTRACE("Send 0x%x = %d\n", data, data);
+ // vpTRACE("Send 0x%x = %d\n", data, data);
parport.sendData(data);
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRingLight.cpp.o) has no symbols
-void dummy_vpRingLight() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRingLight.cpp.o) has no
+// symbols
+void dummy_vpRingLight(){};
#endif
diff --git a/modules/robot/src/real-robot/afma4/vpAfma4.cpp b/modules/robot/src/real-robot/afma4/vpAfma4.cpp
index c90727a..f95af8d 100644
--- a/modules/robot/src/real-robot/afma4/vpAfma4.cpp
+++ b/modules/robot/src/real-robot/afma4/vpAfma4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,29 +44,26 @@
*/
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpAfma4.h>
-
+#include <visp3/robot/vpRobotException.h>
/* ----------------------------------------------------------------------- */
/* --- STATIC ------------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
const unsigned int vpAfma4::njoint = 4;
-
/*!
Default constructor.
*/
-vpAfma4::vpAfma4()
- : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
+vpAfma4::vpAfma4() : _a1(0), _d3(0), _d4(0), _etc(), _erc(), _eMc()
{
// Set the default parameters in case of the config files are not available.
@@ -74,7 +72,7 @@ vpAfma4::vpAfma4()
//
this->_a1 = 0.205; // distance along x2
this->_d3 = 0.403; // distance along z2
- this->_d4 = 0.14; // distance along z3
+ this->_d4 = 0.14; // distance along z3
// Maximal value of the joints
this->_joint_max[0] = 1.8; // rad
@@ -82,17 +80,17 @@ vpAfma4::vpAfma4()
this->_joint_max[2] = 0.9; // rad
this->_joint_max[3] = 0.76; // rad
// Minimal value of the joints
- this->_joint_min[0] = -1.5; // rad
- this->_joint_min[1] = -0.9; // meter
- this->_joint_min[2] = -3.5; // rad
- this->_joint_min[3] = -0.76;// rad
+ this->_joint_min[0] = -1.5; // rad
+ this->_joint_min[1] = -0.9; // meter
+ this->_joint_min[2] = -3.5; // rad
+ this->_joint_min[3] = -0.76; // rad
// Camera extrinsic parameters: effector to camera frame
this->_etc[0] = 0.; // Translation
this->_etc[1] = 0.;
this->_etc[2] = 0.;
this->_erc[0] = 0.; // Rotation
- this->_erc[1] = -M_PI/2.;
+ this->_erc[1] = -M_PI / 2.;
this->_erc[2] = 0;
vpRotationMatrix eRc(_erc);
@@ -101,16 +99,11 @@ vpAfma4::vpAfma4()
init();
}
-
/*!
Does nothing for the moment.
*/
-void
-vpAfma4::init (void)
-{
- return;
-}
+void vpAfma4::init(void) { return; }
/*!
@@ -118,8 +111,8 @@ vpAfma4::init (void)
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
+ of the camera relative to the base frame given the articular positions of
+ all the four joints.
This method is the same than get_fMc(const vpColVector & q).
@@ -136,15 +129,14 @@ vpAfma4::init (void)
model which expresses the transformation between the fix frame and the
camera frame (\f${^f}M_c\f$) with:
\f[
- {^f}M_c = {^f}M_e * {^e}M_c
+ {^f}M_c = {^f}M_e * {^e}M_c
\f]
\sa get_fMc(const vpColVector & q)
\sa getInverseKinematics()
*/
-vpHomogeneousMatrix
-vpAfma4::getForwardKinematics(const vpColVector & q) const
+vpHomogeneousMatrix vpAfma4::getForwardKinematics(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
fMc = get_fMc(q);
@@ -158,8 +150,8 @@ vpAfma4::getForwardKinematics(const vpColVector & q) const
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
+ of the camera relative to the base frame given the articular positions of
+ all the four joints.
This method is the same than getForwardKinematics(const vpColVector & q).
@@ -176,13 +168,12 @@ vpAfma4::getForwardKinematics(const vpColVector & q) const
model which expresses the transformation between the fix frame and the
camera frame (\f${^f}M_c\f$) with:
\f[
- {^f}M_c = {^f}M_e * {^e}M_c
+ {^f}M_c = {^f}M_e * {^e}M_c
\f]
\sa getForwardKinematics(const vpColVector & q)
*/
-vpHomogeneousMatrix
-vpAfma4::get_fMc (const vpColVector & q) const
+vpHomogeneousMatrix vpAfma4::get_fMc(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
get_fMc(q, fMc);
@@ -196,8 +187,8 @@ vpAfma4::get_fMc (const vpColVector & q) const
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the four joints.
+ of the camera relative to the base frame given the articular positions of
+ all the four joints.
\param q : Articular position of the four joints: q[0] corresponds to
the first rotation (joint 1 with value \f$q_1\f$) of the turret
@@ -212,12 +203,11 @@ vpAfma4::get_fMc (const vpColVector & q) const
model which expresses the transformation between the fix frame and the
camera frame (\f${^f}M_c\f$) with:
\f[
- {^f}M_c = {^f}M_e * {^e}M_c
+ {^f}M_c = {^f}M_e * {^e}M_c
\f]
*/
-void
-vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+void vpAfma4::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
// Compute the direct geometric model: fMe = transformation between
@@ -255,8 +245,10 @@ vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
\f[
{^f}M_e = \left[\begin{array}{cccc}
- c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1 \\
- s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1 \\
+ c_1s_4c_5+s_1c_4c_5 & -c_1s_4s_5-s_1c_4s_5 & c_1c_4-s_1s_4 &a_1c_1-d_3s_1
+ \\
+ s_1s_4c_5-c_1c_4c_5 & -s_1s_4s_5+c_1c_4s_5 & s_1c_4+c_1s_4 &a_1s_1+d_3c_1
+ \\
-s_5 & -c_5 & d_4+q_2 \\
0 & 0 & 0 & 1 \\
\end{array}
@@ -264,31 +256,30 @@ vpAfma4::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
\f]
*/
-void
-vpAfma4::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+void vpAfma4::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
{
- double q1 = q[0]; // rot touret
- double q2 = q[1]; // vertical translation
- double q4 = q[2]; // pan
- double q5 = q[3]; // tilt
-
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
+ double q1 = q[0]; // rot touret
+ double q2 = q[1]; // vertical translation
+ double q4 = q[2]; // pan
+ double q5 = q[3]; // tilt
+
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
/* Calcul du modele d'apres les angles. */
- fMe[0][0] = c1*s4*c5 + s1*c4*c5;
- fMe[0][1] = -c1*s4*s5 - s1*c4*s5;
- fMe[0][2] = c1*c4 - s1*s4;
- fMe[0][3] = c1*this->_a1 - s1*(this->_d3);
+ fMe[0][0] = c1 * s4 * c5 + s1 * c4 * c5;
+ fMe[0][1] = -c1 * s4 * s5 - s1 * c4 * s5;
+ fMe[0][2] = c1 * c4 - s1 * s4;
+ fMe[0][3] = c1 * this->_a1 - s1 * (this->_d3);
- fMe[1][0] = s1*s4*c5 - c1*c4*c5;
- fMe[1][1] = -s1*s4*s5 + c1*c4*s5;
- fMe[1][2] = s1*c4+c1*s4;
- fMe[1][3] = s1*this->_a1 + c1*(this->_d3);
+ fMe[1][0] = s1 * s4 * c5 - c1 * c4 * c5;
+ fMe[1][1] = -s1 * s4 * s5 + c1 * c4 * s5;
+ fMe[1][2] = s1 * c4 + c1 * s4;
+ fMe[1][3] = s1 * this->_a1 + c1 * (this->_d3);
fMe[2][0] = -s5;
fMe[2][1] = -c5;
@@ -315,11 +306,7 @@ 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(); }
/*!
@@ -330,13 +317,12 @@ vpAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
\param cVe : Twist transformation.
*/
-void
-vpAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
return;
}
@@ -359,14 +345,13 @@ vpAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
\param cVf : Twist transformation.
*/
-void
-vpAfma4::get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const
+void vpAfma4::get_cVf(const vpColVector &q, vpVelocityTwistMatrix &cVf) const
{
- vpHomogeneousMatrix fMc, cMf ;
- get_fMc(q, fMc) ;
+ vpHomogeneousMatrix fMc, cMf;
+ get_fMc(q, fMc);
cMf = fMc.inverse();
- cVf.buildFrom(cMf) ;
+ cVf.buildFrom(cMf);
return;
}
@@ -407,24 +392,25 @@ vpAfma4::get_cVf(const vpColVector & q, vpVelocityTwistMatrix &cVf) const
\sa get_fJe()
*/
-void
-vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+void vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
- double q4 = q[2]; // pan
- double q5 = q[3]; // tilt
+ double q4 = q[2]; // pan
+ double q5 = q[3]; // tilt
- double c4 = cos(q4);
- double s4 = sin(q4);
- double c5 = cos(q5);
- double s5 = sin(q5);
+ double c4 = cos(q4);
+ double s4 = sin(q4);
+ double c5 = cos(q5);
+ double s5 = sin(q5);
eJe.resize(6, 4);
eJe = 0;
- eJe[0][0] = -(this->_a1*c4 + this->_d3*s4)*c5; eJe[0][1] = -s5;
- eJe[1][0] = (this->_a1*c4 + this->_d3*s4)*s5; eJe[1][1] = -c5;
- eJe[2][0] = (this->_a1*s4 - this->_d3*c4);
+ eJe[0][0] = -(this->_a1 * c4 + this->_d3 * s4) * c5;
+ eJe[0][1] = -s5;
+ eJe[1][0] = (this->_a1 * c4 + this->_d3 * s4) * s5;
+ eJe[1][1] = -c5;
+ eJe[2][0] = (this->_a1 * s4 - this->_d3 * c4);
eJe[3][0] = eJe[3][2] = -s5;
eJe[4][0] = eJe[4][2] = -c5;
eJe[5][3] = 1.;
@@ -461,10 +447,9 @@ vpAfma4::get_eJe(const vpColVector &q, vpMatrix &eJe) const
\sa get_eJe() and get_fJe_inverse()
*/
-void
-vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+void vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
- fJe.resize(6,4) ;
+ fJe.resize(6, 4);
double q1 = q[0]; // rot touret
double q4 = q[2]; // pan
@@ -476,12 +461,12 @@ vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
fJe = 0;
- fJe[0][0] = -s1*this->_a1 - c1*this->_d3;
+ fJe[0][0] = -s1 * this->_a1 - c1 * this->_d3;
+
+ fJe[1][0] = c1 * this->_a1 - s1 * this->_d3;
- fJe[1][0] = c1*this->_a1 - s1*this->_d3;
-
fJe[2][1] = 1.0;
-
+
fJe[3][3] = c14;
fJe[4][3] = s14;
@@ -489,7 +474,7 @@ vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
fJe[5][0] = fJe[5][2] = 1.0;
}
-/*!
+/*!
Get the inverse jacobian.
@@ -520,12 +505,12 @@ vpAfma4::get_fJe(const vpColVector &q, vpMatrix &fJe) const
*/
void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
{
- fJe_inverse.resize(4, 6) ;
+ fJe_inverse.resize(4, 6);
fJe_inverse = 0;
double q1 = q[0]; // rot touret
double q4 = q[2]; // pan
-
+
double c1 = cos(q1);
double s1 = sin(q1);
double c14 = cos(q1 + q4);
@@ -533,13 +518,13 @@ void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
double det = this->_a1 * this->_a1 + this->_d3 * this->_d3;
- fJe_inverse[0][0] = (-s1*this->_a1 - c1*this->_d3)/det;
- fJe_inverse[0][1] = (c1*this->_a1 - s1*this->_d3)/det;
+ fJe_inverse[0][0] = (-s1 * this->_a1 - c1 * this->_d3) / det;
+ fJe_inverse[0][1] = (c1 * this->_a1 - s1 * this->_d3) / det;
fJe_inverse[1][2] = fJe_inverse[2][5] = 1.;
- fJe_inverse[2][0] = - fJe_inverse[0][0];
- fJe_inverse[2][1] = - fJe_inverse[0][1];
+ fJe_inverse[2][0] = -fJe_inverse[0][0];
+ fJe_inverse[2][1] = -fJe_inverse[0][1];
fJe_inverse[3][3] = c14;
fJe_inverse[3][4] = s14;
@@ -548,16 +533,15 @@ void vpAfma4::get_fJe_inverse(const vpColVector &q, vpMatrix &fJe_inverse) const
/*!
Get min joint values.
- \return Minimal joint values for the 4 dof
+ \return Minimal joint values for the 4 dof
X, Y, A, B. Translation Y is expressed in meters. Rotations
X,A and B in radians.
*/
-vpColVector
-vpAfma4::getJointMin() const
+vpColVector vpAfma4::getJointMin() const
{
vpColVector qmin(4);
- for (unsigned int i=0; i < 4; i ++)
+ for (unsigned int i = 0; i < 4; i++)
qmin[i] = this->_joint_min[i];
return qmin;
}
@@ -570,17 +554,14 @@ vpAfma4::getJointMin() const
X, A and B in radians.
*/
-vpColVector
-vpAfma4::getJointMax() const
+vpColVector vpAfma4::getJointMax() const
{
vpColVector qmax(4);
- for (unsigned int i=0; i < 4; i ++)
+ for (unsigned int i = 0; i < 4; i++)
qmax[i] = this->_joint_max[i];
return qmax;
}
-
-
/*!
Print on the output stream \e os the robot parameters (joint
@@ -590,56 +571,35 @@ vpAfma4::getJointMax() const
\param os : Output stream.
\param afma4 : Robot parameters.
*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os,
- const vpAfma4 & afma4)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma4 &afma4)
{
vpRotationMatrix eRc;
afma4._eMc.extract(eRc);
vpRxyzVector rxyz(eRc);
- os
- << "Joint Max:" << std::endl
- << "\t" << afma4._joint_max[0]
- << "\t" << afma4._joint_max[1]
- << "\t" << afma4._joint_max[2]
- << "\t" << afma4._joint_max[3]
- << "\t" << std::endl
-
- << "Joint Min: " << std::endl
- << "\t" << afma4._joint_min[0]
- << "\t" << afma4._joint_min[1]
- << "\t" << afma4._joint_min[2]
- << "\t" << afma4._joint_min[3]
- << "\t" << std::endl
-
- << "a1: " << std::endl
- << "\t" << afma4._a1
- << "\t" << std::endl
-
- << "d3: " << std::endl
- << "\t" << afma4._d3
- << "\t" << std::endl
-
- << "d4: " << std::endl
- << "\t" << afma4._d4
- << "\t" << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << afma4._eMc[0][3] << " "
- << afma4._eMc[1][3] << " "
- << afma4._eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
+ os << "Joint Max:" << std::endl
+ << "\t" << afma4._joint_max[0] << "\t" << afma4._joint_max[1] << "\t" << afma4._joint_max[2] << "\t"
+ << afma4._joint_max[3] << "\t" << std::endl
+
+ << "Joint Min: " << std::endl
+ << "\t" << afma4._joint_min[0] << "\t" << afma4._joint_min[1] << "\t" << afma4._joint_min[2] << "\t"
+ << afma4._joint_min[3] << "\t" << std::endl
+
+ << "a1: " << std::endl
+ << "\t" << afma4._a1 << "\t" << std::endl
+
+ << "d3: " << std::endl
+ << "\t" << afma4._d3 << "\t" << std::endl
+
+ << "d4: " << std::endl
+ << "\t" << afma4._d4 << "\t" << std::endl
+
+ << "eMc: " << std::endl
+ << "\tTranslation (m): " << afma4._eMc[0][3] << " " << afma4._eMc[1][3] << " " << afma4._eMc[2][3] << "\t"
+ << std::endl
+ << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
return os;
}
diff --git a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp
index 61ce1d9..6ebb575 100644
--- a/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp
+++ b/modules/robot/src/real-robot/afma4/vpRobotAfma4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,13 +45,13 @@
#include <sys/types.h>
#include <unistd.h>
-#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobotAfma4.h>
+#include <visp3/robot/vpRobotException.h>
/* ---------------------------------------------------------------------- */
/* --- STATIC ----------------------------------------------------------- */
@@ -67,7 +68,6 @@ bool vpRobotAfma4::robotAlreadyCreated = false;
*/
const double vpRobotAfma4::defaultPositioningVelocity = 15.0;
-
/* ---------------------------------------------------------------------- */
/* --- EMERGENCY STOP --------------------------------------------------- */
/* ---------------------------------------------------------------------- */
@@ -81,23 +81,26 @@ const double vpRobotAfma4::defaultPositioningVelocity = 15.0;
*/
void emergencyStopAfma4(int signo)
{
- std::cout << "Stop the Afma4 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
- }
+ std::cout << "Stop the Afma4 application by signal (" << signo << "): " << (char)7;
+ switch (signo) {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl;
+ break;
+ case SIGBUS:
+ std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
+ break;
+ case SIGSEGV:
+ std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
+ break;
+ case SIGKILL:
+ std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
+ break;
+ case SIGQUIT:
+ std::cout << "SIGQUIT " << std::endl;
+ break;
+ default:
+ std::cout << signo << std::endl;
+ }
// std::cout << "Emergency stop called\n";
// PrimitiveESTOP_Afma4();
PrimitiveSTOP_Afma4();
@@ -109,10 +112,9 @@ void emergencyStopAfma4(int signo)
fprintf(stdout, "Application ");
fflush(stdout);
kill(getpid(), SIGKILL);
- exit(1) ;
+ exit(1);
}
-
/* ---------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
@@ -128,10 +130,7 @@ void emergencyStopAfma4(int signo)
It also set the robot state to vpRobot::STATE_STOP.
*/
-vpRobotAfma4::vpRobotAfma4 (bool verbose)
- :
- vpAfma4 (),
- vpRobot ()
+vpRobotAfma4::vpRobotAfma4(bool verbose) : vpAfma4(), vpRobot()
{
/*
@@ -154,8 +153,8 @@ vpRobotAfma4::vpRobotAfma4 (bool verbose)
*/
signal(SIGINT, emergencyStopAfma4);
- signal(SIGBUS, emergencyStopAfma4) ;
- signal(SIGSEGV, emergencyStopAfma4) ;
+ signal(SIGBUS, emergencyStopAfma4);
+ signal(SIGSEGV, emergencyStopAfma4);
signal(SIGKILL, emergencyStopAfma4);
signal(SIGQUIT, emergencyStopAfma4);
@@ -164,20 +163,18 @@ vpRobotAfma4::vpRobotAfma4 (bool verbose)
std::cout << "Open communication with MotionBlox.\n";
try {
this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
+ this->setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
// vpERROR_TRACE("Error caught") ;
- throw ;
+ throw;
}
- positioningVelocity = defaultPositioningVelocity ;
+ positioningVelocity = defaultPositioningVelocity;
vpRobotAfma4::robotAlreadyCreated = true;
- return ;
+ return;
}
-
/* ------------------------------------------------------------------------ */
/* --- INITIALISATION ----------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -190,8 +187,7 @@ vpRobotAfma4::vpRobotAfma4 (bool verbose)
is done.
*/
-void
-vpRobotAfma4::init (void)
+void vpRobotAfma4::init(void)
{
int stt;
InitTry;
@@ -208,35 +204,32 @@ vpRobotAfma4::init (void)
first_time_getdis = true;
// Initialize the firewire connection
- Try( stt = InitializeConnection(verbose_) );
+ Try(stt = InitializeConnection(verbose_));
if (stt != SUCCESS) {
- vpERROR_TRACE ("Cannot open connection with the motionblox.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connection with the motionblox");
+ vpERROR_TRACE("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
- Try( stt = InitializeNode_Afma4() );
+ Try(stt = InitializeNode_Afma4());
if (stt != SUCCESS) {
- vpERROR_TRACE ("Cannot open connection with the motionblox.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connection with the motionblox");
+ vpERROR_TRACE("Cannot open connection with the motionblox.");
+ throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with the motionblox");
}
- Try( PrimitiveRESET_Afma4() );
+ 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
if (verbose_) {
std::cout << "Robot status: ";
- switch(EStopStatus) {
+ switch (EStopStatus) {
case ESTOP_AUTO:
case ESTOP_MANUAL:
if (HIPowerStatus == 0)
@@ -255,10 +248,11 @@ vpRobotAfma4::init (void)
std::cout << std::endl;
}
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
+ Try(PrimitiveJOINT_MINMAX_Afma4(_joint_min, _joint_max));
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
+ // _joint_max[i]);
+ // }
// If an error occur in the low level controller, goto here
// CatchPrint();
@@ -267,8 +261,9 @@ vpRobotAfma4::init (void)
// Test if an error occurs
if (TryStt == -20001)
printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
- else if (TryStt == -675)
+ "and if the firewire link exist between the MotionBlox and this "
+ "computer.\n");
+ else if (TryStt == -675)
printf(" Timeout enabling power...\n");
if (TryStt < 0) {
@@ -278,13 +273,11 @@ 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 ;
+ return;
}
-
/* ------------------------------------------------------------------------ */
/* --- DESTRUCTOR --------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -295,24 +288,23 @@ vpRobotAfma4::init (void)
Free allocated resources.
*/
-vpRobotAfma4::~vpRobotAfma4 (void)
+vpRobotAfma4::~vpRobotAfma4(void)
{
InitTry;
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
// 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) {
-// fprintf(stdout, "Power OFF the robot\n");
-// fflush(stdout);
+ // if (HIPowerStatus == 1) {
+ // fprintf(stdout, "Power OFF the robot\n");
+ // fflush(stdout);
-// Try( PrimitivePOWEROFF_Afma4() );
-// }
+ // Try( PrimitivePOWEROFF_Afma4() );
+ // }
// Free allocated resources
ShutDownConnection();
@@ -323,53 +315,48 @@ vpRobotAfma4::~vpRobotAfma4 (void)
return;
}
-
-
-
-
/*!
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;
switch (newState) {
case vpRobot::STATE_STOP: {
- if (vpRobot::STATE_STOP != getRobotState ()) {
- Try( PrimitiveSTOP_Afma4() );
+ if (vpRobot::STATE_STOP != getRobotState()) {
+ Try(PrimitiveSTOP_Afma4());
}
break;
}
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Afma4() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
+ Try(PrimitiveSTOP_Afma4());
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
this->powerOn();
break;
}
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
std::cout << "Change the control mode from stop to velocity control.\n";
}
this->powerOn();
break;
}
default:
- break ;
+ break;
}
CatchPrint();
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
/* ------------------------------------------------------------------------ */
@@ -383,22 +370,19 @@ vpRobotAfma4::setRobotState(vpRobot::vpRobotStateType newState)
\exception vpRobotException::lowLevelError : If the low level
controller returns an error during robot stopping.
*/
-void
-vpRobotAfma4::stopMotion(void)
+void vpRobotAfma4::stopMotion(void)
{
InitTry;
- Try( PrimitiveSTOP_Afma4() );
- setRobotState (vpRobot::STATE_STOP);
+ Try(PrimitiveSTOP_Afma4());
+ setRobotState(vpRobot::STATE_STOP);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
+ vpERROR_TRACE("Cannot stop robot motion");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
}
}
-
/*!
Power on the robot.
@@ -408,8 +392,7 @@ vpRobotAfma4::stopMotion(void)
\sa powerOff(), getPowerState()
*/
-void
-vpRobotAfma4::powerOn(void)
+void vpRobotAfma4::powerOn(void)
{
InitTry;
@@ -419,26 +402,24 @@ vpRobotAfma4::powerOn(void)
bool firsttime = true;
unsigned int nitermax = 10;
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
+ for (unsigned int i = 0; i < nitermax; i++) {
+ Try(PrimitiveSTATUS_Afma4(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
if (EStopStatus == ESTOP_AUTO) {
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
+ } else if (EStopStatus == ESTOP_MANUAL) {
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
+ } else if (EStopStatus == ESTOP_ACTIVATED) {
if (firsttime) {
std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ << "Check the emergency stop button and push the yellow "
+ "button before continuing."
+ << std::endl;
firsttime = false;
}
- fprintf(stdout, "Remaining time %us \r", nitermax-i);
+ fprintf(stdout, "Remaining time %us \r", nitermax - i);
fflush(stdout);
CAL_Wait(1);
- }
- else {
+ } else {
std::cout << "Sorry there is an error on the emergency chain." << std::endl;
std::cout << "You have to call Adept for maintenance..." << std::endl;
// Free allocated resources
@@ -452,22 +433,20 @@ 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) {
fprintf(stdout, "Power ON the Afma4 robot\n");
fflush(stdout);
- Try( PrimitivePOWERON_Afma4() );
+ Try(PrimitivePOWERON_Afma4());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power on the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -480,29 +459,26 @@ vpRobotAfma4::powerOn(void)
\sa powerOn(), getPowerState()
*/
-void
-vpRobotAfma4::powerOff(void)
+void vpRobotAfma4::powerOff(void)
{
InitTry;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
fprintf(stdout, "Power OFF the Afma4 robot\n");
fflush(stdout);
- Try( PrimitivePOWEROFF_Afma4() );
+ Try(PrimitivePOWEROFF_Afma4());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power off the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -517,15 +493,13 @@ vpRobotAfma4::powerOff(void)
\sa powerOn(), powerOff()
*/
-bool
-vpRobotAfma4::getPowerState(void)
+bool vpRobotAfma4::getPowerState(void)
{
InitTry;
bool status = false;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
@@ -534,9 +508,8 @@ vpRobotAfma4::getPowerState(void)
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
+ vpERROR_TRACE("Cannot get the power status");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
}
return status;
}
@@ -550,13 +523,12 @@ vpRobotAfma4::getPowerState(void)
\param cVe : Twist transformation.
*/
-void
-vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- vpAfma4::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpAfma4::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -568,27 +540,26 @@ vpRobotAfma4::get_cVe(vpVelocityTwistMatrix &cVe) const
\param cVf : Twist transformation.
*/
-void
-vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const
+void vpRobotAfma4::get_cVf(vpVelocityTwistMatrix &cVf) const
{
double position[this->njoint];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
CatchPrint();
vpColVector q(this->njoint);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
try {
- vpAfma4::get_cVf(q, cVf) ;
+ vpAfma4::get_cVf(q, cVf);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
- catch(...) {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }}
+}
/*!
@@ -600,11 +571,7 @@ 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); }
/*!
@@ -618,27 +585,25 @@ vpRobotAfma4::get_cMe(vpHomogeneousMatrix &cMe) const
\sa vpAfma4::get_eJe()
*/
-void
-vpRobotAfma4::get_eJe(vpMatrix &eJe)
+void vpRobotAfma4::get_eJe(vpMatrix &eJe)
{
double position[this->njoint];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
CatchPrint();
vpColVector q(this->njoint);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
try {
- vpAfma4::get_eJe(q, eJe) ;
- }
- catch(...) {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ vpAfma4::get_eJe(q, eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
@@ -662,19 +627,18 @@ void vpRobotAfma4::get_fJe(vpMatrix &fJe)
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Afma4(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma4(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
try {
- vpAfma4::get_fJe(q, fJe) ;
- }
- catch(...) {
+ vpAfma4::get_fJe(q, fJe);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
/*!
@@ -705,23 +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; }
/*!
@@ -774,13 +729,14 @@ vpRobotAfma4::getPositioningVelocity (void)
\endcode
\exception vpRobotException::lowLevelError : If the requested frame
- (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, or vpRobot::MIXT_FRAME) are
- requested since they are not implemented.
+ (vpRobot::REFERENCE_FRAME, vpRobot::CAMERA_FRAME, or vpRobot::MIXT_FRAME)
+ are requested since they are not implemented.
\exception vpRobotException::positionOutOfRangeError : The requested
position is out of range.
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+ like:
\code
try {
@@ -794,54 +750,47 @@ 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 ()) {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
-
+
int error = 0;
-
- switch(frame) {
+
+ switch (frame) {
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Positionning error. Reference frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Reference frame not implemented.");
- break ;
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Positionning error. Camera frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Camera frame not implemented.");
- break ;
+ vpERROR_TRACE("Positionning error. Reference frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Reference frame not implemented.");
+ break;
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE("Positionning error. Camera frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Camera frame not implemented.");
+ break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- break ;
-
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ break;
+
case vpRobot::ARTICULAR_FRAME: {
- break ;
-
+ break;
}
}
if (position.getRows() != this->njoint) {
- vpERROR_TRACE ("Positionning error: bad vector dimension.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Positionning error: bad vector dimension.");
+ vpERROR_TRACE("Positionning error: bad vector dimension.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Positionning error: bad vector dimension.");
}
InitTry;
- Try( PrimitiveMOVE_Afma4(position.data, positioningVelocity) );
- Try( WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000) );
+ Try(PrimitiveMOVE_Afma4(position.data, positioningVelocity));
+ Try(WaitState_Afma4(ETAT_ATTENTE_AFMA4, 1000));
CatchPrint();
if (TryStt == InvalidPosition || TryStt == -1023)
@@ -850,16 +799,14 @@ vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
std::cout << " : Unknown error (see Fabien).\n";
else if (error == -1)
std::cout << "Position out of range.\n";
-
+
if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
}
-
- return ;
-}
+ return;
+}
/*!
Move to an absolute position with a given percent of max velocity.
@@ -911,24 +858,20 @@ vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
\sa setPosition()
*/
-void vpRobotAfma4::setPosition (const vpRobot::vpControlFrameType frame,
- const double q1,
- const double q2,
- const double q4,
- const double q5)
+void vpRobotAfma4::setPosition(const vpRobot::vpControlFrameType frame, const double q1, const double q2,
+ const double q4, const double q5)
{
try {
- vpColVector position(this->njoint) ;
- position[0] = q1 ;
- position[1] = q2 ;
- position[2] = q4 ;
- position[3] = q5 ;
-
- setPosition(frame, position) ;
- }
- catch(...) {
+ vpColVector position(this->njoint);
+ position[0] = q1;
+ position[1] = q2;
+ position[2] = q4;
+ position[3] = q5;
+
+ setPosition(frame, position);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -969,16 +912,16 @@ void vpRobotAfma4::setPosition(const char *filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename);
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
- Returns the robot controller current time (in second) since last robot power on.
+ Returns the robot controller current time (in second) since last robot power
+ on.
*/
double vpRobotAfma4::getTime() const
{
@@ -1033,7 +976,8 @@ double vpRobotAfma4::getTime() const
}
// 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 pose in terms of a homogeneous matrix
vpHomogeneousMatrix fMc(fRc, ftc);
@@ -1046,35 +990,33 @@ 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;
- position.resize (this->njoint);
+ position.resize(this->njoint);
switch (frame) {
- case vpRobot::CAMERA_FRAME : {
+ case vpRobot::CAMERA_FRAME: {
position = 0;
return;
}
- case vpRobot::ARTICULAR_FRAME : {
+ case vpRobot::ARTICULAR_FRAME: {
double _q[njoint];
- Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
- for (unsigned int i=0; i < this->njoint; i ++) {
+ Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
+ for (unsigned int i = 0; i < this->njoint; i++) {
position[i] = _q[i];
}
return;
}
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::REFERENCE_FRAME: {
double _q[njoint];
- Try( PrimitiveACQ_POS_Afma4(_q, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma4(_q, ×tamp));
vpColVector q(this->njoint);
- for (unsigned int i=0; i < this->njoint; i++)
+ for (unsigned int i = 0; i < this->njoint; i++)
q[i] = _q[i];
// Compute fMc
@@ -1087,25 +1029,23 @@ vpRobotAfma4::getPosition (const vpRobot::vpControlFrameType frame,
vpRxyzVector rxyz;
rxyz.buildFrom(fRc);
- for (unsigned int i=0; i < 3; i++) {
- position[i] = fMc[i][3]; // translation x,y,z
- position[i+3] = rxyz[i]; // Euler rotation x,y,z
+ for (unsigned int i = 0; i < 3; i++) {
+ position[i] = fMc[i][3]; // translation x,y,z
+ position[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
- break ;
+ break;
}
case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("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.");
+ vpERROR_TRACE("Cannot get position.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
}
return;
@@ -1115,13 +1055,13 @@ vpRobotAfma4::getPosition (const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
+void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
@@ -1140,12 +1080,19 @@ void vpRobotAfma4::getPosition(const vpRobot::vpControlFrameType frame,
vector is 2 when the control is in the camera frame (frame is than set to
vpRobot::CAMERA_FRAME).
- - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t \f$ correspond to joint velocities.
+ - In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4]^t \f$
+ where \f$\dot{q}_1, \dot{q}_3, \dot{q}_4 \f$ correspond to joint
+ velocities in rad/s and \f$\dot{q}_2\f$ to the vertical translation velocity
+ in m/s.
- In camera frame, \f$ vel = [^{c} t_x, ^{c} t_y, ^{c} t_z,^{c}
- \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t\f$ is expressed in the
- camera frame. Since only four dof are available, in camera frame we
- control control only the camera pan and tilt.
+ \omega_x, ^{c} \omega_y]^t, ^{c} \omega_z]^t\f$ is a velocity twist
+ expressed in the camera frame, with translations velocities
+ \f$ ^{c} v_x, ^{c} v_y, ^{c} v_z \f$ in m/s
+ and rotation velocities \f$ ^{c}\omega_x, ^{c} \omega_y, ^{c} \omega_z \f$
+ in rad/s. Since only four dof are available, in camera frame we control only
+ the camera pan and tilt. In other words we apply only \f$ ^{c}\omega_x, ^{c}
+ \omega_y\f$ to the robot, even if \f$ vel \f$ is a 6-dimension vector.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
@@ -1173,68 +1120,59 @@ 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) ");
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
}
// Check the dimension of the velocity vector to see if it is
// compatible with the requested frame
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- //if (vel.getRows() != 2) {
- if (vel.getRows() != 6) {
- vpERROR_TRACE ("Bad dimension of the velocity vector in camera frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension of the velocity vector "
- "in camera frame");
- }
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- if (vel.getRows() != this->njoint) {
- vpERROR_TRACE ("Bad dimension of the articular velocity vector");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension of the articular "
- "velocity vector ");
- }
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- }
- 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");
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // if (vel.getRows() != 2) {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE("Bad dimension of the velocity vector in camera frame");
+ throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the velocity vector "
+ "in camera frame");
+ }
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ if (vel.getRows() != this->njoint) {
+ vpERROR_TRACE("Bad dimension of the articular velocity vector");
+ throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension of the articular "
+ "velocity vector ");
+ }
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ }
+ 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");
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
+ }
}
-
//
// Velocities saturation with normalization
@@ -1245,14 +1183,14 @@ vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
if (frame == vpRobot::CAMERA_FRAME) {
vpColVector vel_max(6);
- for (unsigned int i=0; i<3; i++) {
- vel_max[i] = getMaxTranslationVelocity();
- vel_max[i+3] = getMaxRotationVelocity();
+ for (unsigned int i = 0; i < 3; i++) {
+ vel_max[i] = getMaxTranslationVelocity();
+ vel_max[i + 3] = getMaxRotationVelocity();
}
vpColVector velocity = vpRobot::saturateVelocities(vel, vel_max, true);
-#if 0 // ok
+#if 0 // ok
vpMatrix eJe(4,6);
eJe = 0;
eJe[2][4] = -1;
@@ -1267,7 +1205,7 @@ vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
vpHomogeneousMatrix fMe;
get_fMe(q, fMe);
vpTranslationVector t;
- t=0;
+ t = 0;
vpRotationMatrix fRe;
fMe.extract(fRe);
vpVelocityTwistMatrix fVe(t, fRe);
@@ -1299,26 +1237,23 @@ vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
InitTry;
// Send a joint velocity to the low level controller
- Try( PrimitiveMOVESPEED_Afma4(joint_vel.data) );
+ Try(PrimitiveMOVESPEED_Afma4(joint_vel.data));
Catch();
if (TryStt < 0) {
if (TryStt == VelStopOnJoint) {
UInt32 axisInJoint[njoint];
PrimitiveSTATUS_Afma4(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
+ 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 {
+ } else {
printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
if (TryString != NULL) {
// The statement is in TryString, but we need to check the validity
printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
+ } else {
printf("\n");
}
}
@@ -1331,7 +1266,6 @@ vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
/* --- GET ---------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
/*!
Get the robot velocities.
@@ -1344,8 +1278,8 @@ vpRobotAfma4::setVelocity (const vpRobot::vpControlFrameType frame,
\param timestamp : Time in second since last robot power on.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+ \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\warning The first time this method is called, \e velocity is set to 0. The
first call is used to intialise the velocity computation for the next call.
@@ -1379,17 +1313,15 @@ 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) {
- case vpRobot::ARTICULAR_FRAME:
- velocity.resize (this->njoint);
+ case vpRobot::ARTICULAR_FRAME:
+ velocity.resize(this->njoint);
break;
default:
- velocity.resize (6);
+ velocity.resize(6);
}
velocity = 0;
@@ -1403,17 +1335,17 @@ vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
time_cur = timestamp;
- for (unsigned int i=0; i < this->njoint; i ++) {
+ for (unsigned int i = 0; i < this->njoint; i++) {
q_cur[i] = q[i];
}
// Get the camera pose from the direct kinematics
vpAfma4::get_fMc(q_cur, fMc_cur);
- if ( ! first_time_getvel ) {
+ if (!first_time_getvel) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
@@ -1423,12 +1355,12 @@ vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
// Compute the velocity of the camera from this displacement
velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- break ;
+ break;
}
case vpRobot::ARTICULAR_FRAME: {
velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
+ break;
}
case vpRobot::REFERENCE_FRAME: {
@@ -1443,21 +1375,19 @@ vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
vpVelocityTwistMatrix fVc(fMc_cur);
velocity = fVc * v;
- break ;
+ break;
}
case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a displacement in the mixt frame:"
- "functionality not implemented");
+ vpERROR_TRACE("Cannot get a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the mixt frame:"
+ "functionality not implemented");
- break ;
+ break;
}
}
- }
- else {
+ } else {
first_time_getvel = false;
}
@@ -1470,12 +1400,10 @@ vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
// Memorize the time associated to the joint position for the next call
time_prev_getvel = time_cur;
-
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
@@ -1483,19 +1411,16 @@ vpRobotAfma4::getVelocity (const vpRobot::vpControlFrameType frame,
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ vpColVector &, double &) except that the timestamp is not returned.
*/
-void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
+void vpRobotAfma4::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
double timestamp;
getVelocity(frame, velocity, timestamp);
}
-
-
/*!
Get the robot velocities.
@@ -1535,11 +1460,10 @@ 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);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -1548,15 +1472,15 @@ vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ double &) except that the timestamp is not returned.
*/
-vpColVector vpRobotAfma4::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotAfma4::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector velocity;
double timestamp;
- getVelocity (frame, velocity, timestamp);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -1597,11 +1521,12 @@ expressed in meters, while joint rotations X,A,B in radians.
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+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
+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%
@@ -1611,12 +1536,11 @@ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
\sa savePosFile()
*/
-bool
-vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
+bool vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
{
std::ifstream fd(filename.c_str(), std::ios::in);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -1628,29 +1552,29 @@ vpRobotAfma4::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Afma4 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -1695,12 +1619,11 @@ 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 ;
- fd = fopen(filename.c_str(), "w") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -1715,13 +1638,9 @@ vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
#\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) ;
+ fclose(fd);
return (true);
}
@@ -1735,50 +1654,14 @@ vpRobotAfma4::savePosFile(const std::string &filename, const vpColVector &q)
\sa readPosFile
*/
-void
-vpRobotAfma4::move(const char *filename)
+void vpRobotAfma4::move(const char *filename)
{
vpColVector q;
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
-}
-
-/*!
-
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotAfma4::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param displacement : The measured articular displacement. The
- dimension of \e displacement is 6 (the number of axis of the
- robot). Translations are expressed in meters, rotations in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma4::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
@@ -1798,14 +1681,10 @@ vpRobotAfma4::getArticularDisplacement(vpColVector &displacement)
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
-
*/
-void
-vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
double q[6];
@@ -1815,12 +1694,12 @@ vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Afma4(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
+ Try(PrimitiveACQ_POS_Afma4(q, ×tamp));
+ for (unsigned int i = 0; i < njoint; i++) {
q_cur[i] = q[i];
}
- if ( ! first_time_getdis ) {
+ if (!first_time_getdis) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
@@ -1829,7 +1708,7 @@ vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
case vpRobot::ARTICULAR_FRAME: {
displacement = q_cur - q_prev_getdis;
- break ;
+ break;
}
case vpRobot::REFERENCE_FRAME: {
@@ -1842,8 +1721,7 @@ vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
return;
}
}
- }
- else {
+ } else {
first_time_getdis = false;
}
@@ -1852,14 +1730,13 @@ vpRobotAfma4::getDisplacement(vpRobot::vpControlFrameType frame,
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no symbols
-void dummy_vpRobotAfma4() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma4.cpp.o) has no
+// symbols
+void dummy_vpRobotAfma4(){};
#endif
-
diff --git a/modules/robot/src/real-robot/afma4/vpServolens.cpp b/modules/robot/src/real-robot/afma4/vpServolens.cpp
index 668176a..de583d9 100644
--- a/modules/robot/src/real-robot/afma4/vpServolens.cpp
+++ b/modules/robot/src/real-robot/afma4/vpServolens.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,7 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Interface for the Servolens lens attached to the camera fixed on the
+ * Interface for the Servolens lens attached to the camera fixed on the
* Afma4 robot.
*
* Authors:
@@ -40,38 +41,36 @@
\file vpServolens.cpp
- Interface for the Servolens lens attached to the camera fixed on the
+ Interface for the Servolens lens attached to the camera fixed on the
Afma4 robot.
*/
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-#include <unistd.h>
-#include <termios.h>
-#include <stdio.h>
-#include <sys/types.h>
-#include <sys/stat.h>
#include <fcntl.h>
+#include <stdio.h>
#include <stdlib.h>
#include <string.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <termios.h>
+#include <unistd.h>
-#include <visp3/robot/vpServolens.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpServolens.h>
/*!
Default constructor. Does nothing.
-
+
\sa open()
*/
-vpServolens::vpServolens() : remfd(0), isinit(false)
-{
-}
+vpServolens::vpServolens() : remfd(0), isinit(false) {}
-/*!
+/*!
Open and initialize the Servolens serial link at 9600 bauds, 7
data bits, even parity, 1 stop bit. The cariage return mode is not
active, that means that each character is directly read without
@@ -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.
@@ -91,10 +87,7 @@ vpServolens::vpServolens(const char *port) : remfd(0), isinit(false)
\sa close()
*/
-vpServolens::~vpServolens()
-{
- this->close();
-}
+vpServolens::~vpServolens() { this->close(); }
/*!
@@ -110,26 +103,23 @@ vpServolens::~vpServolens()
\sa close()
*/
-void
-vpServolens::open(const char *port)
+void vpServolens::open(const char *port)
{
- if (! isinit) {
+ if (!isinit) {
struct termios info;
printf("\nOpen the Servolens serial port \"%s\"\n", port);
- if ((this->remfd=::open(port, O_RDWR|O_NONBLOCK)) < 0) {
- vpERROR_TRACE ("Cannot open Servolens serial port.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot open Servolens serial port.");
+ if ((this->remfd = ::open(port, O_RDWR | O_NONBLOCK)) < 0) {
+ vpERROR_TRACE("Cannot open Servolens serial port.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot open Servolens serial port.");
}
// Lecture des parametres courants de la liaison serie.
if (tcgetattr(this->remfd, &info) < 0) {
::close(this->remfd);
- vpERROR_TRACE ("Error using TCGETS in ioctl.");
- throw vpRobotException (vpRobotException::communicationError,
- "Error using TCGETS in ioctl");
+ vpERROR_TRACE("Error using TCGETS in ioctl.");
+ throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
}
//
@@ -142,14 +132,14 @@ vpServolens::open(const char *port)
info.c_iflag |= INLCR;
// Traitement sur les caracteres envoyes sur la RS232.
- info.c_oflag = 0; // idem
+ info.c_oflag = 0; // idem
// Traitement des lignes
info.c_lflag = 0;
// Controle materiel de la liaison
info.c_cflag = 0;
- info.c_cflag |= CREAD; // Validation reception
+ info.c_cflag |= CREAD; // Validation reception
info.c_cflag |= B9600 | CS7 | PARENB; // 9600 baus, 7 data, parite paire
// Caracteres immediatement disponibles.
@@ -158,9 +148,8 @@ 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");
+ vpERROR_TRACE("Error using TCGETS in ioctl.");
+ throw vpRobotException(vpRobotException::communicationError, "Error using TCGETS in ioctl");
}
// Supprime tous les caracteres recus mais non encore lus par read()
@@ -170,15 +159,15 @@ vpServolens::open(const char *port)
this->init();
- // Try to get the position of the zoom to check if the lens is really connected
+ // Try to get the position of the zoom to check if the lens is really
+ // connected
unsigned int izoom;
if (this->getPosition(vpServolens::ZOOM, izoom) == false) {
- vpERROR_TRACE ("Cannot dial with the servolens. Check if the serial link is connected.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with the servolens. Check if the serial link is connected.");
-
+ 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.");
}
-
}
}
@@ -186,8 +175,7 @@ vpServolens::open(const char *port)
Close the Servolens serial link.
\sa open()
*/
-void
-vpServolens::close()
+void vpServolens::close()
{
if (isinit) {
printf("\nClose the serial connection with Servolens\n");
@@ -202,13 +190,11 @@ vpServolens::close()
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
-void
-vpServolens::reset() const
+void vpServolens::reset() const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
@@ -240,13 +226,11 @@ vpServolens::reset() const
\sa open()
*/
-void
-vpServolens::init() const
+void vpServolens::init() const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
@@ -264,56 +248,6 @@ vpServolens::init() const
}
/*!
- Set or remove the Servolens command complete status at the end of servoing.
-
- \param servo : Servolens servo motor.
-
- \param active : true to activate the emission of a command complete
- flag at the end of motion. false to disable this functionality.
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
-
-*/
-void
-vpServolens::enableCmdComplete(vpServoType servo, bool active) const
-{
- if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
- }
- char commande[10];
-
- /* Envoie une commande pour qu'en fin de mouvement servolens renvoie
- * une information de fin de mouvement (ex: ZF, FF, DF).
- */
- switch(servo) {
- case ZOOM:
- if (active)
- sprintf(commande, "ZF1");
- else
- sprintf(commande, "ZF0");
- break;
- case FOCUS:
- if (active)
- sprintf(commande, "FF1");
- else
- sprintf(commande, "FF0");
- break;
- case IRIS:
- if (active)
- sprintf(commande, "DF1");
- else
- sprintf(commande, "DF0");
- break;
- }
-
- /* envoie de la commande */
- this->write(commande); /* a la fin du mouvement envoie de ZF, FF, DF */
-}
-
-/*!
Enable or disable the emission of the Servolens prompt "SERVOLENS>".
\param active : true to activate the emission of the prompy. false
@@ -323,13 +257,11 @@ vpServolens::enableCmdComplete(vpServoType servo, bool active) const
with Servolens.
*/
-void
-vpServolens::enablePrompt(bool active) const
+void vpServolens::enablePrompt(bool active) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
@@ -338,7 +270,7 @@ vpServolens::enablePrompt(bool active) const
sprintf(commande, "SE1");
else
sprintf(commande, "SE0");
-
+
this->write(commande);
}
@@ -350,17 +282,15 @@ vpServolens::enablePrompt(bool active) const
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
-void
-vpServolens::setController(vpControllerType controller) const
+void vpServolens::setController(vpControllerType controller) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
- switch(controller) {
+ switch (controller) {
case AUTO:
/* Valide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW1");
@@ -368,21 +298,20 @@ vpServolens::setController(vpControllerType controller) const
break;
case CONTROLLED:
/* nettoyage : mot d'etat vide 0000 */
- sprintf(commande,"SX0842");
+ sprintf(commande, "SX0842");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
break;
case RELEASED:
- sprintf(commande,"SX1084");
+ sprintf(commande, "SX1084");
this->write(commande);
/* devalide l'incrustation de la fenetre sur l'ecran du moniteur */
sprintf(commande, "VW0");
this->write(commande);
break;
}
-
}
/*!
@@ -391,13 +320,11 @@ vpServolens::setController(vpControllerType controller) const
\param enable : true to activate the auto iris.
*/
-void
-vpServolens::setAutoIris(bool enable) const
+void vpServolens::setAutoIris(bool enable) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
@@ -419,27 +346,25 @@ vpServolens::setAutoIris(bool enable) const
with Servolens.
*/
-void
-vpServolens::setPosition(vpServoType servo, unsigned int position) const
+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.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
- /* attente du prompt pour envoyer une commande */
- /*
- printf("attente prompt\n");
- this->wait();
- */
+/* attente du prompt pour envoyer une commande */
+/*
+printf("attente prompt\n");
+this->wait();
+*/
#ifdef FINSERVO
/* envoie des commandes pour qu'en fin de mouvement servolens renvoie */
/* une commande de fin de mouvement (ex: ZF, FF, DF). */
this->enableCommandComplete();
-#endif /* FINSERVO */
+#endif /* FINSERVO */
// 08/08/00 Fabien S. - Correction de la consigne demandee
// pour prendre en compte l'erreur entre la consigne demandee
@@ -449,38 +374,41 @@ vpServolens::setPosition(vpServoType servo, unsigned int position) const
// A la consigne du iris on ajoute 1.
switch (servo) {
case ZOOM:
- //printf("zoom demande: %d ", position);
- position --;
- if (position < ZOOM_MIN) position = ZOOM_MIN;
- //printf("zoom corrige: %d \n", position);
+ // printf("zoom demande: %d ", position);
+ position--;
+ if (position < ZOOM_MIN)
+ position = ZOOM_MIN;
+ // printf("zoom corrige: %d \n", position);
break;
case FOCUS:
- //printf("focus demande: %d ", position);
- position ++;
- if (position > FOCUS_MAX) position = FOCUS_MAX;
- //printf("focus corrige: %d \n", position);
+ // printf("focus demande: %d ", position);
+ position++;
+ if (position > FOCUS_MAX)
+ position = FOCUS_MAX;
+ // printf("focus corrige: %d \n", position);
break;
case IRIS:
// printf("iris demande: %d ", position);
- position ++;
- if (position > IRIS_MAX) position = IRIS_MAX;
- //printf("iris corrige: %s \n", position);
+ position++;
+ if (position > IRIS_MAX)
+ position = IRIS_MAX;
+ // printf("iris corrige: %s \n", position);
break;
}
/* commande a envoyer aux servomoteurs */
- switch(servo) {
- case ZOOM:
- sprintf(commande, "ZD%d", position);
- break;
- case FOCUS:
- sprintf(commande, "FD%d", position);
- break;
- case IRIS:
- sprintf(commande, "DD%d", position);
- break;
- }
- /* envoie de la commande */
+ switch (servo) {
+ case ZOOM:
+ sprintf(commande, "ZD%u", position);
+ break;
+ case FOCUS:
+ sprintf(commande, "FD%u", position);
+ break;
+ case IRIS:
+ sprintf(commande, "DD%u", position);
+ break;
+ }
+/* envoie de la commande */
#ifdef PRINT
printf("\ncommande: %s", commande);
#endif
@@ -489,7 +417,7 @@ vpServolens::setPosition(vpServoType servo, unsigned int position) const
#ifdef FINSERVO
/* on attend la fin du mouvement des objectifs */
- this->wait(servo); /* on attend les codes ZF, FF, DF */
+ this->wait(servo); /* on attend les codes ZF, FF, DF */
#endif
}
@@ -503,19 +431,17 @@ vpServolens::setPosition(vpServoType servo, unsigned int position) const
with Servolens.
*/
-bool
-vpServolens::getPosition(vpServoType servo, unsigned int &position) const
+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.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char commande[10];
char posit[10], *pt_posit;
char c;
- short fin_lect_posit; /* indique si on a lu la position du servo-moteur */
- short posit_car; /* donne la position du caractere lu */
+ short 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 */
@@ -552,56 +478,56 @@ vpServolens::getPosition(vpServoType servo, unsigned int &position) const
if (this->read(&c, 1) == true) {
// printf("caractere lu: %c\n", c);
- switch (posit_car){
- /* on lit le 1er caractere; (soit Z, soit F, soit D) */
+ switch (posit_car) {
+ /* on lit le 1er caractere; (soit Z, soit F, soit D) */
case 0:
- /* sauvegarde du pointeur */
- pt_posit = posit;
-
- switch (servo) {
- case ZOOM:
- if( c == 'Z') posit_car = 1;
- break;
- case FOCUS:
- if( c == 'F') posit_car = 1;
- break;
- case IRIS:
- if( c == 'D') posit_car = 1;
- break;
- }
- break;
-
- /* si le 1er caractere est correct, on lit le 2eme caractere */
- /* (toujours D) */
+ /* sauvegarde du pointeur */
+ pt_posit = posit;
+
+ switch (servo) {
+ case ZOOM:
+ if (c == 'Z')
+ posit_car = 1;
+ break;
+ case FOCUS:
+ if (c == 'F')
+ posit_car = 1;
+ break;
+ case IRIS:
+ if (c == 'D')
+ posit_car = 1;
+ break;
+ }
+ break;
+
+ /* si le 1er caractere est correct, on lit le 2eme caractere */
+ /* (toujours D) */
case 1:
- if( c == 'D') posit_car = 2;
- else posit_car = 0; /* le 2eme caractere n'est pas correct */
- break;
-
- /* si on a lu les 2 premiers caracteres, on peut lire la */
- /* position du servo-moteur */
+ if (c == 'D')
+ posit_car = 2;
+ else
+ posit_car = 0; /* le 2eme caractere n'est pas correct */
+ break;
+
+ /* si on a lu les 2 premiers caracteres, on peut lire la */
+ /* position du servo-moteur */
case 2:
- if (c >= '0' && c <= '9')
- {
- *pt_posit++ = c; /* sauvegarde de la position */
- lecture_posit_en_cours = 1;
- }
- else if (lecture_posit_en_cours)
- {
- fin_lect_posit = 1;
- *pt_posit = '\0';
- }
- else posit_car = 0;
- break;
+ if (c >= '0' && c <= '9') {
+ *pt_posit++ = c; /* sauvegarde de la position */
+ lecture_posit_en_cours = 1;
+ } else if (lecture_posit_en_cours) {
+ fin_lect_posit = 1;
+ *pt_posit = '\0';
+ } else
+ posit_car = 0;
+ break;
}
- }
- else {
+ } else {
// Timout sur la lecture, on retoure FALSE
return false;
}
- }
- while ( !fin_lect_posit );
+ } while (!fin_lect_posit);
// printf("\nChaine lue: posit: %s", posit);
@@ -611,53 +537,49 @@ vpServolens::getPosition(vpServoType servo, unsigned int &position) const
// printf("\nChaine toilettee: posit: %s", posit);
position = (unsigned int)atoi(posit);
- return(true);
+ return (true);
}
/*!
- These parameters are computed from the Dragonfly2 DR2-COL camera sensor pixel
- size (7.4 um) and from the servolens zoom position.
+ These parameters are computed from the Dragonfly2 DR2-COL camera sensor
+pixel size (7.4 um) and from the servolens zoom position.
- \param I : An image coming from the Dragonfly2 camera attached to the
+ \param I : An image coming from the Dragonfly2 camera attached to the
servolens.
\code
- #include <visp3/vs/vpServolens.h>
-
- int main()
- {
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- vpServolens servolens("/dev/ttyS0");
-
- vpImage<unsigned char> I(240, 320);
- vpCameraParameters cam = servolens.getCameraParameters(I);
- std::cout << "Camera parameters: " << cam << std::endl;
+#include <visp3/vs/vpServolens.h>
+
+int main()
+{
+ // UNIX vpServolens servolens("/dev/ttyS0");
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
+
+ vpImage<unsigned char> I(240, 320);
+ vpCameraParameters cam = servolens.getCameraParameters(I);
+ std::cout << "Camera parameters: " << cam << std::endl;
#endif
}
-
- \exception vpRobotException::communicationError : If cannot dial
- with Servolens.
\endcode
-
+
+ \exception vpRobotException::communicationError : If cannot dial with Servolens.
+
*/
-vpCameraParameters
-vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
+vpCameraParameters vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
vpCameraParameters cam;
double pix_size = 7.4e-6; // Specific to the Dragonfly2 camera
- double px=1000, py=1000, u0=320, v0=240;
+ double px = 1000, py = 1000, u0 = 320, v0 = 240;
// Determine if the image is subsampled.
// Dragonfly2 native images are 640 by 480
double subsample_factor = 1.;
double width = I.getWidth();
- double height= I.getHeight();
-
+ double height = I.getHeight();
if (width > 300 && width < 340 && height > 220 && height < 260)
subsample_factor = 2;
@@ -666,12 +588,13 @@ vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
unsigned zoom;
getPosition(vpServolens::ZOOM, zoom);
- //std::cout << "Actual zoom value: " << zoom << std::endl;
+ // std::cout << "Actual zoom value: " << zoom << std::endl;
- // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale correspondent
- // aux parametres de calibration de la camera (donnees constructeur) pour des
- // tailles d'images CCIR (768x576), donc avec scale = 1.
- double focale = zoom * 1.0e-5; // Transformation en metres
+ // XSIZE_PIX_CAM_AFMA4 / focale et YSIZE_PIX_CAM_AFMA4 / focale
+ // correspondent aux parametres de calibration de la camera (donnees
+ // constructeur) pour des tailles d'images CCIR (768x576), donc avec scale
+ // = 1.
+ double focale = zoom * 1.0e-5; // Transformation en metres
px = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
py = focale / (double)(subsample_factor * pix_size); // Taille des pixels en metres.
u0 = I.getWidth() / 2.;
@@ -689,33 +612,27 @@ vpServolens::getCameraParameters(vpImage<unsigned char> &I) const
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
-char
-vpServolens::wait() const
+char vpServolens::wait() const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
-
+
ssize_t r;
r = ::write(this->remfd, "\r\n", strlen("\r\n"));
if (r != (ssize_t)(strlen("\r\n"))) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot write on Servolens.");
+ 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 != '>');
+ } while (c != '>');
return c;
-
}
/*!
@@ -728,13 +645,11 @@ vpServolens::wait() const
with Servolens.
*/
-void
-vpServolens::wait(vpServoType servo) const
+void vpServolens::wait(vpServoType servo) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
char c;
@@ -752,33 +667,29 @@ vpServolens::wait(vpServoType servo) const
default:
sprintf(fin_mvt, "DF");
break;
-
}
/* lecture des caracteres recus */
do {
/* lecture des caracteres */
- if (::read(this->remfd,&c,1) != 1) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot read on Servolens.");
+ if (::read(this->remfd, &c, 1) != 1) {
+ throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
}
c &= 0x7f;
/* tests si fin de mouvement */
if (c == fin_mvt[0]) {
/* lecture du caractere suivant */
- if (::read(this->remfd,&c,1) != 1) {
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot read on Servolens.");
+ if (::read(this->remfd, &c, 1) != 1) {
+ throw vpRobotException(vpRobotException::communicationError, "Cannot read on Servolens.");
}
c &= 0x7f;
if (c == fin_mvt[1]) {
- sortie = true;
+ sortie = true;
}
}
- }
- while ( !sortie);
+ } while (!sortie);
/* printf("\nmouvement fini: chaine lue = %s", chaine); */
}
@@ -788,35 +699,32 @@ vpServolens::wait(vpServoType servo) const
\param c : The character that was read.
\param timeout_s : Timeout in seconds.
-
+
\return true if the reading was successfully achieved, false otherwise.
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
-bool
-vpServolens::read(char *c, long timeout_s) const
+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.");
+ vpERROR_TRACE("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_ZERO(&readfds);
FD_SET(static_cast<unsigned int>(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;
*c &= 0x7f;
- //printf("lecture 1 car: %c\n", *c);
- return(true);
+ // printf("lecture 1 car: %c\n", *c);
+ return (true);
}
return (false);
@@ -830,24 +738,20 @@ vpServolens::read(char *c, long timeout_s) const
\exception vpRobotException::communicationError : If cannot dial
with Servolens.
*/
-void
-vpServolens::write(const char *s) const
+void vpServolens::write(const char *s) const
{
if (!isinit) {
- vpERROR_TRACE ("Cannot dial with Servolens.");
- throw vpRobotException (vpRobotException::communicationError,
- "Cannot dial with Servolens.");
+ vpERROR_TRACE("Cannot dial with Servolens.");
+ throw vpRobotException(vpRobotException::communicationError, "Cannot dial with Servolens.");
}
ssize_t r = 0;
- r = ::write(this->remfd,"\r", strlen("\r"));
+ r = ::write(this->remfd, "\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.");
+ r += ::write(this->remfd, "\r", strlen("\r"));
+ if (r != (ssize_t)(2 * strlen("\r") + strlen(s))) {
+ throw vpRobotException(vpRobotException::communicationError, "Cannot write on Servolens.");
}
-
/*
* Une petite tempo pour laisser le temps a la liaison serie de
* digerer la commande envoyee. En fait, la liaison serie fonctionne
@@ -869,26 +773,24 @@ vpServolens::write(const char *s) const
\param out : Output string without zero characters on the left.
*/
-bool
-vpServolens::clean(const char *in, char *out) const
+bool vpServolens::clean(const char *in, char *out) const
{
- short nb_car, i=0;
+ short nb_car, i = 0;
bool error = false;
nb_car = strlen(in);
/* on se positionne sur le 1er caractere different de zero */
- while( *(in) == '0' && i++ < nb_car ) {
+ while (*(in) == '0' && i++ < nb_car) {
in++;
- if (i == nb_car)
- {
- error = true; /* la chaine ne contient pas une position */
- *(out++) = '0'; /* mise a zero de la position */
- }
+ if (i == nb_car) {
+ error = true; /* la chaine ne contient pas une position */
+ *(out++) = '0'; /* mise a zero de la position */
+ }
}
/* copie de la position epuree des zeros de gauche */
- while( i++ <= nb_car ) { /* on a mis le = pour copier le caractere de fin */
+ while (i++ <= nb_car) { /* on a mis le = pour copier le caractere de fin */
/* de chaine \0 */
*(out++) = *(in++);
}
diff --git a/modules/robot/src/real-robot/afma6/vpAfma6.cpp b/modules/robot/src/real-robot/afma6/vpAfma6.cpp
index 1fe8e18..dae8a8d 100644
--- a/modules/robot/src/real-robot/afma6/vpAfma6.cpp
+++ b/modules/robot/src/real-robot/afma6/vpAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,26 +44,25 @@
*/
-#include <sstream>
#include <iostream>
+#include <sstream>
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/robot/vpAfma6.h>
+#include <visp3/robot/vpRobotException.h>
/* ----------------------------------------------------------------------- */
/* --- 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 =
@@ -97,10 +97,10 @@ const std::string vpAfma6::CONST_CAMERA_AFMA6_FILENAME =
#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_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
-const char * const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
+const char *const vpAfma6::CONST_CCMOP_CAMERA_NAME = "Dragonfly2-8mm-ccmop";
+const char *const vpAfma6::CONST_GRIPPER_CAMERA_NAME = "Dragonfly2-6mm-gripper";
+const char *const vpAfma6::CONST_VACUUM_CAMERA_NAME = "Dragonfly2-6mm-vacuum";
+const char *const vpAfma6::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
const vpAfma6::vpAfma6ToolType vpAfma6::defaultTool = TOOL_CCMOP;
@@ -112,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.
@@ -124,7 +123,7 @@ vpAfma6::vpAfma6()
// coupling between join 5 and 6
this->_coupl_56 = 0.009091;
// distance between join 5 and 6
- this->_long_56 = -0.06924;
+ this->_long_56 = -0.06924;
// Camera extrinsic parameters: effector to camera frame
this->_eMc.eye(); // Default values are initialized ...
// ... in init (vpAfma6::vpAfma6ToolType tool,
@@ -145,17 +144,15 @@ vpAfma6::vpAfma6()
this->_joint_min[5] = -1.5901;
init();
-
}
/*!
Initialize the robot with the default tool vpAfma6::defaultTool.
*/
-void
-vpAfma6::init (void)
+void vpAfma6::init(void)
{
- this->init ( vpAfma6::defaultTool);
+ this->init(vpAfma6::defaultTool);
return;
}
@@ -164,19 +161,18 @@ vpAfma6::init (void)
Read files containing the constant parameters related to the robot
kinematics and to the end-effector to camera transformation.
- \param camera_extrinsic_parameters : Filename containing the constant parameters of
- the robot kinematics \f$^e{\bf M}_c \f$ transformation.
+ \param camera_extrinsic_parameters : Filename containing the constant
+ parameters of the robot kinematics \f$^e{\bf M}_c \f$ transformation.
- \param camera_intrinsic_parameters : Filename containing the camera extrinsic parameters.
+ \param camera_intrinsic_parameters : Filename containing the camera
+ 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);
+ this->parseConfigFile(camera_extrinsic_parameters);
- this->parseConfigFile (camera_intrinsic_parameters);
+ this->parseConfigFile(camera_intrinsic_parameters);
}
/*!
@@ -205,11 +201,11 @@ eMc_ROT_XYZ 10.0 -90.0 20.0
eMc_TRANS_XYZ 0.05 0.01 0.06
\endcode
- \sa init(vpAfma6::vpToolType, vpCameraParameters::vpCameraParametersProjType),
- init(vpAfma6::vpToolType, const vpHomogeneousMatrix&)
+ \sa init(vpAfma6::vpToolType,
+vpCameraParameters::vpCameraParametersProjType), init(vpAfma6::vpToolType,
+const vpHomogeneousMatrix&)
*/
-void
-vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
+void vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
{
this->setToolType(tool);
this->parseConfigFile(filename.c_str());
@@ -224,10 +220,9 @@ vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const std::string &filename)
extrinsic parameters.
*/
-void
-vpAfma6::init (const std::string &camera_extrinsic_parameters)
+void vpAfma6::init(const std::string &camera_extrinsic_parameters)
{
- this->parseConfigFile (camera_extrinsic_parameters);
+ this->parseConfigFile(camera_extrinsic_parameters);
}
/*!
@@ -240,15 +235,15 @@ vpAfma6::init (const std::string &camera_extrinsic_parameters)
\param eMc : Homogeneous matrix representation of the transformation
between the end-effector frame and the tool frame.
- \sa init(vpAfma6::vpAfma6ToolType, vpCameraParameters::vpCameraParametersProjType),
+ \sa init(vpAfma6::vpAfma6ToolType,
+ vpCameraParameters::vpCameraParametersProjType),
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);
+ this->setToolType(tool);
+ this->set_eMc(eMc);
}
/*!
@@ -268,63 +263,61 @@ vpAfma6::init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc)
\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;
-
+
#ifdef VISP_HAVE_AFMA6_DATA
// Read the robot parameters from files
std::string filename_eMc;
switch (tool) {
case vpAfma6::TOOL_CCMOP: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_CCMOP_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_CCMOP_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpAfma6::TOOL_GRIPPER: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_GRIPPER_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_GRIPPER_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpAfma6::TOOL_VACUUM: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_VACUUM_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_VACUUM_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpAfma6::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
break;
}
break;
}
default: {
- vpERROR_TRACE ("This error should not occur!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -336,81 +329,81 @@ vpAfma6::init (vpAfma6::vpAfma6ToolType tool,
break;
}
}
-
- this->init (vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
-#else // VISP_HAVE_AFMA6_DATA
+ this->init(vpAfma6::CONST_AFMA6_FILENAME, filename_eMc);
+
+#else // VISP_HAVE_AFMA6_DATA
// Use here default values of the robot constant parameters.
switch (tool) {
case vpAfma6::TOOL_CCMOP: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
_erc[0] = vpMath::rad(164.35); // rx
- _erc[1] = vpMath::rad( 89.64); // ry
+ _erc[1] = vpMath::rad(89.64); // ry
_erc[2] = vpMath::rad(-73.05); // rz
- _etc[0] = 0.0117; // tx
- _etc[1] = 0.0033; // ty
- _etc[2] = 0.2272; // tz
+ _etc[0] = 0.0117; // tx
+ _etc[1] = 0.0033; // ty
+ _etc[2] = 0.2272; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion:
_erc[0] = vpMath::rad(33.54); // rx
_erc[1] = vpMath::rad(89.34); // ry
_erc[2] = vpMath::rad(57.83); // rz
- _etc[0] = 0.0373; // tx
- _etc[1] = 0.0024; // ty
- _etc[2] = 0.2286; // tz
+ _etc[0] = 0.0373; // tx
+ _etc[1] = 0.0024; // ty
+ _etc[2] = 0.2286; // tz
break;
}
break;
}
case vpAfma6::TOOL_GRIPPER: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- _erc[0] = vpMath::rad( 88.33); // rx
- _erc[1] = vpMath::rad( 72.07); // ry
- _erc[2] = vpMath::rad( 2.53); // rz
- _etc[0] = 0.0783; // tx
- _etc[1] = 0.1234; // ty
- _etc[2] = 0.1638; // tz
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ _erc[0] = vpMath::rad(88.33); // rx
+ _erc[1] = vpMath::rad(72.07); // ry
+ _erc[2] = vpMath::rad(2.53); // rz
+ _etc[0] = 0.0783; // tx
+ _etc[1] = 0.1234; // ty
+ _etc[2] = 0.1638; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion:
_erc[0] = vpMath::rad(86.69); // rx
_erc[1] = vpMath::rad(71.93); // ry
- _erc[2] = vpMath::rad( 4.17); // rz
- _etc[0] = 0.1034; // tx
- _etc[1] = 0.1142; // ty
- _etc[2] = 0.1642; // tz
+ _erc[2] = vpMath::rad(4.17); // rz
+ _etc[0] = 0.1034; // tx
+ _etc[1] = 0.1142; // ty
+ _etc[2] = 0.1642; // tz
break;
}
break;
}
case vpAfma6::TOOL_VACUUM: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- _erc[0] = vpMath::rad( 90.40); // rx
- _erc[1] = vpMath::rad( 75.11); // ry
- _erc[2] = vpMath::rad( 0.18); // rz
- _etc[0] = 0.0038; // tx
- _etc[1] = 0.1281; // ty
- _etc[2] = 0.1658; // tz
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ _erc[0] = vpMath::rad(90.40); // rx
+ _erc[1] = vpMath::rad(75.11); // ry
+ _erc[2] = vpMath::rad(0.18); // rz
+ _etc[0] = 0.0038; // tx
+ _etc[1] = 0.1281; // ty
+ _etc[2] = 0.1658; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
+ case vpCameraParameters::perspectiveProjWithDistortion:
_erc[0] = vpMath::rad(91.61); // rx
_erc[1] = vpMath::rad(76.17); // ry
_erc[2] = vpMath::rad(-0.98); // rz
- _etc[0] = 0.0815; // tx
- _etc[1] = 0.1162; // ty
- _etc[2] = 0.1658; // tz
+ _etc[0] = 0.0815; // tx
+ _etc[1] = 0.1162; // ty
+ _etc[2] = 0.1658; // tz
break;
}
break;
}
case vpAfma6::TOOL_CUSTOM:
case vpAfma6::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ case vpCameraParameters::perspectiveProjWithDistortion:
// set eMc to identity
_erc[0] = 0; // rx
_erc[1] = 0; // ry
@@ -428,18 +421,17 @@ vpAfma6::init (vpAfma6::vpAfma6ToolType tool,
#endif // VISP_HAVE_AFMA6_DATA
setToolType(tool);
- return ;
+ return;
}
-
/*!
Compute the forward kinematics (direct geometric model) as an
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
+ of the camera relative to the base frame given the articular positions of
+ all the six joints.
This method is the same than get_fMc(const vpColVector & q).
@@ -456,8 +448,7 @@ vpAfma6::init (vpAfma6::vpAfma6ToolType tool,
\sa getInverseKinematics()
*/
-vpHomogeneousMatrix
-vpAfma6::getForwardKinematics(const vpColVector & q) const
+vpHomogeneousMatrix vpAfma6::getForwardKinematics(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
fMc = get_fMc(q);
@@ -493,8 +484,8 @@ vpAfma6::getForwardKinematics(const vpColVector & q) const
The code below shows how to compute the inverse geometric model:
\code
-#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpRobotAfma6.h>
@@ -538,12 +529,11 @@ int main()
\sa getForwardKinematics()
*/
-int
-vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
- vpColVector & q, const bool &nearest, const bool &verbose) const
+int vpAfma6::getInverseKinematics(const vpHomogeneousMatrix &fMc, vpColVector &q, const bool &nearest,
+ const bool &verbose) const
{
vpHomogeneousMatrix fMe;
- double q_[2][6],d[2],t;
+ double q_[2][6], d[2], t;
int ok[2];
double cord[6];
@@ -552,7 +542,6 @@ vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
if (q.getRows() != njoint)
q.resize(6);
-
// for(unsigned int i=0;i<3;i++) {
// fMe[i][3] = fMc[i][3];
// for(int j=0;j<3;j++) {
@@ -568,131 +557,127 @@ vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
fMe = fMc * this->_eMc.inverse();
// std::cout << "\n\nfMe: " << fMe;
- if (fMe[2][2] >= .99999f)
- {
+ if (fMe[2][2] >= .99999f) {
vpTRACE("singularity\n");
- q_[0][4] = q_[1][4] = M_PI/2.f;
- t = atan2(fMe[0][0],fMe[0][1]);
+ q_[0][4] = q_[1][4] = M_PI / 2.f;
+ t = atan2(fMe[0][0], fMe[0][1]);
q_[1][3] = q_[0][3] = q[3];
q_[1][5] = q_[0][5] = t - q_[0][3];
- while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
- /* -> a cause du couplage 4/5 */
+ while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
+ /* -> a cause du couplage 4/5 */
{
q_[1][5] -= vpMath::rad(10);
q_[1][3] += vpMath::rad(10);
}
- while (q_[1][5] <= this->_joint_min[5])
- {
+ while (q_[1][5] <= this->_joint_min[5]) {
q_[1][5] += vpMath::rad(10);
q_[1][3] -= vpMath::rad(10);
}
- }
- else if (fMe[2][2] <= -.99999)
- {
+ } else if (fMe[2][2] <= -.99999) {
vpTRACE("singularity\n");
- q_[0][4] = q_[1][4] = -M_PI/2;
- t = atan2(fMe[1][1],fMe[1][0]);
+ q_[0][4] = q_[1][4] = -M_PI / 2;
+ t = atan2(fMe[1][1], fMe[1][0]);
q_[1][3] = q_[0][3] = q[3];
q_[1][5] = q_[0][5] = q_[0][3] - t;
- while ((q_[1][5]+vpMath::rad(2)) >= this->_joint_max[5])
- /* -> a cause du couplage 4/5 */
+ while ((q_[1][5] + vpMath::rad(2)) >= this->_joint_max[5])
+ /* -> a cause du couplage 4/5 */
{
q_[1][5] -= vpMath::rad(10);
q_[1][3] -= vpMath::rad(10);
}
- while (q_[1][5] <= this->_joint_min[5])
- {
+ while (q_[1][5] <= this->_joint_min[5]) {
q_[1][5] += vpMath::rad(10);
q_[1][3] += vpMath::rad(10);
}
- }
- else
- {
- q_[0][3] = atan2(-fMe[0][2],fMe[1][2]);
- if (q_[0][3] >= 0.0) q_[1][3] = q_[0][3] - M_PI;
- else q_[1][3] = q_[0][3] + M_PI;
+ } else {
+ q_[0][3] = atan2(-fMe[0][2], fMe[1][2]);
+ if (q_[0][3] >= 0.0)
+ q_[1][3] = q_[0][3] - M_PI;
+ else
+ q_[1][3] = q_[0][3] + M_PI;
q_[0][4] = asin(fMe[2][2]);
- if (q_[0][4] >= 0.0) q_[1][4] = M_PI - q_[0][4];
- else q_[1][4] = -M_PI - q_[0][4];
+ if (q_[0][4] >= 0.0)
+ q_[1][4] = M_PI - q_[0][4];
+ else
+ q_[1][4] = -M_PI - q_[0][4];
- q_[0][5] = atan2(-fMe[2][1],fMe[2][0]);
- if (q_[0][5] >= 0.0) q_[1][5] = q_[0][5] - M_PI;
- else q_[1][5] = q_[0][5] + M_PI;
+ q_[0][5] = atan2(-fMe[2][1], fMe[2][0]);
+ if (q_[0][5] >= 0.0)
+ q_[1][5] = q_[0][5] - M_PI;
+ else
+ q_[1][5] = q_[0][5] + M_PI;
}
- q_[0][0] = fMe[0][3] - this->_long_56*cos(q_[0][3]);
- q_[1][0] = fMe[0][3] - this->_long_56*cos(q_[1][3]);
- q_[0][1] = fMe[1][3] - this->_long_56*sin(q_[0][3]);
- q_[1][1] = fMe[1][3] - this->_long_56*sin(q_[1][3]);
+ q_[0][0] = fMe[0][3] - this->_long_56 * cos(q_[0][3]);
+ q_[1][0] = fMe[0][3] - this->_long_56 * cos(q_[1][3]);
+ q_[0][1] = fMe[1][3] - this->_long_56 * sin(q_[0][3]);
+ q_[1][1] = fMe[1][3] - this->_long_56 * sin(q_[1][3]);
q_[0][2] = q_[1][2] = fMe[2][3];
/* prise en compte du couplage axes 5/6 */
- q_[0][5] += this->_coupl_56*q_[0][4];
- q_[1][5] += this->_coupl_56*q_[1][4];
+ q_[0][5] += this->_coupl_56 * q_[0][4];
+ q_[1][5] += this->_coupl_56 * q_[1][4];
- for (int j=0;j<2;j++)
- {
+ for (int j = 0; j < 2; j++) {
ok[j] = 1;
// test is position is reachable
- for (unsigned int i=0;i<6;i++) {
+ for (unsigned int i = 0; i < 6; i++) {
if (q_[j][i] < this->_joint_min[i] || q_[j][i] > this->_joint_max[i]) {
if (verbose) {
if (i < 3)
- std::cout << "Joint " << i << " not in limits: " << this->_joint_min[i] << " < " << q_[j][i] << " < " << this->_joint_max[i] << std::endl;
+ 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;
}
}
}
- if (ok[0] == 0)
- {
+ if (ok[0] == 0) {
if (ok[1] == 0) {
std::cout << "No solution..." << std::endl;
nbsol = 0;
return nbsol;
- }
- else if (ok[1] == 1) {
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
+ } else if (ok[1] == 1) {
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[1][i];
nbsol = 1;
}
- }
- else
- {
+ } else {
if (ok[1] == 0) {
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[0][i];
nbsol = 1;
- }
- else
- {
+ } else {
nbsol = 2;
- //vpTRACE("2 solutions\n");
- for (int j=0;j<2;j++)
- {
+ // vpTRACE("2 solutions\n");
+ for (int j = 0; j < 2; j++) {
d[j] = 0.0;
- for (unsigned int i=3;i<6;i++)
+ for (unsigned int i = 3; i < 6; i++)
d[j] += (q_[j][i] - q[i]) * (q_[j][i] - q[i]);
}
- if (nearest == true)
- {
+ if (nearest == true) {
if (d[0] <= d[1])
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[0][i];
else
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
- }
- else
- {
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[1][i];
+ } else {
if (d[0] <= d[1])
- for (unsigned int i=0;i<6;i++) cord[i] = q_[1][i];
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[1][i];
else
- for (unsigned int i=0;i<6;i++) cord[i] = q_[0][i];
+ for (unsigned int i = 0; i < 6; i++)
+ cord[i] = q_[0][i];
}
}
}
- for(unsigned int i=0; i<6; i++)
- q[i] = cord[i] ;
+ for (unsigned int i = 0; i < 6; i++)
+ q[i] = cord[i];
return nbsol;
}
@@ -703,8 +688,8 @@ vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
+ of the camera relative to the base frame given the articular positions of
+ all the six joints.
This method is the same than getForwardKinematics(const vpColVector & q).
@@ -719,8 +704,7 @@ vpAfma6::getInverseKinematics(const vpHomogeneousMatrix & fMc,
\sa getForwardKinematics(const vpColVector & q)
*/
-vpHomogeneousMatrix
-vpAfma6::get_fMc (const vpColVector & q) const
+vpHomogeneousMatrix vpAfma6::get_fMc(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
get_fMc(q, fMc);
@@ -734,8 +718,8 @@ vpAfma6::get_fMc (const vpColVector & q) const
homogeneous matrix.
By forward kinematics we mean here the position and the orientation
- of the camera relative to the base frame given the articular positions of all
- the six joints.
+ of the camera relative to the base frame given the articular positions of
+ all the six joints.
\param q : Articular joint position of the robot. q[0], q[1], q[2]
correspond to the first 3 translations expressed in meter, while
@@ -747,8 +731,7 @@ vpAfma6::get_fMc (const vpColVector & q) const
camera frame (fMc).
*/
-void
-vpAfma6::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+void vpAfma6::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
// Compute the direct geometric model: fMe = transformation between
@@ -781,37 +764,36 @@ vpAfma6::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
end effector frame (fMe).
*/
-void
-vpAfma6::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+void vpAfma6::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
{
- double q0 = q[0]; // meter
- double q1 = q[1]; // meter
- double q2 = q[2]; // meter
+ double q0 = q[0]; // meter
+ double q1 = q[1]; // meter
+ double q2 = q[2]; // meter
/* Decouplage liaisons 2 et 3. */
- double q5 = q[5] - this->_coupl_56 * q[4];
+ double q5 = q[5] - this->_coupl_56 * q[4];
- double c1 = cos(q[3]);
- double s1 = sin(q[3]);
- double c2 = cos(q[4]);
- double s2 = sin(q[4]);
- double c3 = cos(q5);
- double s3 = sin(q5);
+ double c1 = cos(q[3]);
+ double s1 = sin(q[3]);
+ double c2 = cos(q[4]);
+ double s2 = sin(q[4]);
+ double c3 = cos(q5);
+ double s3 = sin(q5);
// Compute the direct geometric model: fMe = transformation betwee
// fix and end effector frame.
- fMe[0][0] = s1*s2*c3 + c1*s3;
- fMe[0][1] = -s1*s2*s3 + c1*c3;
- fMe[0][2] = -s1*c2;
- fMe[0][3] = q0 + this->_long_56*c1;
-
- fMe[1][0] = -c1*s2*c3 + s1*s3;
- fMe[1][1] = c1*s2*s3 + s1*c3;
- fMe[1][2] = c1*c2;
- fMe[1][3] = q1 + this->_long_56*s1;
-
- fMe[2][0] = c2*c3;
- fMe[2][1] = -c2*s3;
+ fMe[0][0] = s1 * s2 * c3 + c1 * s3;
+ fMe[0][1] = -s1 * s2 * s3 + c1 * c3;
+ fMe[0][2] = -s1 * c2;
+ fMe[0][3] = q0 + this->_long_56 * c1;
+
+ fMe[1][0] = -c1 * s2 * c3 + s1 * s3;
+ fMe[1][1] = c1 * s2 * s3 + s1 * c3;
+ fMe[1][2] = c1 * c2;
+ fMe[1][3] = q1 + this->_long_56 * s1;
+
+ fMe[2][0] = c2 * c3;
+ fMe[2][1] = -c2 * s3;
fMe[2][2] = s2;
fMe[2][3] = q2;
@@ -835,11 +817,7 @@ 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
@@ -850,11 +828,7 @@ vpAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
camera frame.
*/
-vpHomogeneousMatrix
-vpAfma6::get_eMc() const
-{
- return(this->_eMc);
-}
+vpHomogeneousMatrix vpAfma6::get_eMc() const { return (this->_eMc); }
/*!
@@ -865,13 +839,12 @@ vpAfma6::get_eMc() const
\param cVe : Twist transformation.
*/
-void
-vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
return;
}
@@ -888,38 +861,40 @@ vpAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
\param eJe : Robot jacobian expressed in the end-effector frame.
*/
-void
-vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+void vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
- eJe.resize(6,6) ;
+ eJe.resize(6, 6);
- double s4,c4,s5,c5,s6,c6 ;
+ double s4, c4, s5, c5, s6, c6;
- s4=sin(q[3]); c4=cos(q[3]);
- s5=sin(q[4]); c5=cos(q[4]);
- s6=sin(q[5]-this->_coupl_56*q[4]); c6=cos(q[5]-this->_coupl_56*q[4]);
+ s4 = sin(q[3]);
+ c4 = cos(q[3]);
+ s5 = sin(q[4]);
+ c5 = cos(q[4]);
+ s6 = sin(q[5] - this->_coupl_56 * q[4]);
+ c6 = cos(q[5] - this->_coupl_56 * q[4]);
eJe = 0;
- eJe[0][0] = s4*s5*c6+c4*s6;
- eJe[0][1] = -c4*s5*c6+s4*s6;
- eJe[0][2] = c5*c6;
- eJe[0][3] = - this->_long_56*s5*c6;
-
- eJe[1][0] = -s4*s5*s6+c4*c6;
- eJe[1][1] = c4*s5*s6+s4*c6;
- eJe[1][2] = -c5*s6;
- eJe[1][3] = this->_long_56*s5*s6;
-
- eJe[2][0] = -s4*c5;
- eJe[2][1] = c4*c5;
+ eJe[0][0] = s4 * s5 * c6 + c4 * s6;
+ eJe[0][1] = -c4 * s5 * c6 + s4 * s6;
+ eJe[0][2] = c5 * c6;
+ eJe[0][3] = -this->_long_56 * s5 * c6;
+
+ eJe[1][0] = -s4 * s5 * s6 + c4 * c6;
+ eJe[1][1] = c4 * s5 * s6 + s4 * c6;
+ eJe[1][2] = -c5 * s6;
+ eJe[1][3] = this->_long_56 * s5 * s6;
+
+ eJe[2][0] = -s4 * c5;
+ eJe[2][1] = c4 * c5;
eJe[2][2] = s5;
- eJe[2][3] = this->_long_56*c5;
+ eJe[2][3] = this->_long_56 * c5;
- eJe[3][3] = c5*c6;
+ eJe[3][3] = c5 * c6;
eJe[3][4] = s6;
- eJe[4][3] = -c5*s6;
+ eJe[4][3] = -c5 * s6;
eJe[4][4] = c6;
eJe[5][3] = s5;
@@ -929,7 +904,6 @@ vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
return;
}
-
/*!
Get the robot jacobian expressed in the robot reference frame also
@@ -957,40 +931,39 @@ vpAfma6::get_eJe(const vpColVector &q, vpMatrix &eJe) const
*/
-void
-vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+void vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
- fJe.resize(6,6) ;
+ fJe.resize(6, 6);
// block superieur gauche
- fJe[0][0] = fJe[1][1] = fJe[2][2] = 1 ;
-
- double s4 = sin(q[3]) ;
- double c4 = cos(q[3]) ;
+ fJe[0][0] = fJe[1][1] = fJe[2][2] = 1;
+ double s4 = sin(q[3]);
+ double c4 = cos(q[3]);
// block superieur droit
- fJe[0][3] = - this->_long_56*s4 ;
- fJe[1][3] = this->_long_56*c4 ;
-
+ fJe[0][3] = -this->_long_56 * s4;
+ fJe[1][3] = this->_long_56 * c4;
- double s5 = sin(q[4]) ;
- double c5 = cos(q[4]) ;
+ double s5 = sin(q[4]);
+ double c5 = cos(q[4]);
// block inferieur droit
- fJe[3][4] = c4 ; fJe[3][5] = -s4*c5 ;
- fJe[4][4] = s4 ; fJe[4][5] = c4*c5 ;
- fJe[5][3] = 1 ; fJe[5][5] = s5 ;
+ fJe[3][4] = c4;
+ fJe[3][5] = -s4 * c5;
+ fJe[4][4] = s4;
+ fJe[4][5] = c4 * c5;
+ fJe[5][3] = 1;
+ fJe[5][5] = s5;
// coupling between joint 5 and 6
- fJe[3][4] += this->_coupl_56*s4*c5;
- fJe[4][4] += -this->_coupl_56*c4*c5;
- fJe[5][4] += -this->_coupl_56*s5;
+ fJe[3][4] += this->_coupl_56 * s4 * c5;
+ fJe[4][4] += -this->_coupl_56 * c4 * c5;
+ fJe[5][4] += -this->_coupl_56 * s5;
return;
}
-
/*!
Get min joint values.
@@ -999,11 +972,10 @@ vpAfma6::get_fJe(const vpColVector &q, vpMatrix &fJe) const
A,B,C in radians.
*/
-vpColVector
-vpAfma6::getJointMin() const
+vpColVector vpAfma6::getJointMin() const
{
vpColVector qmin(6);
- for (unsigned int i=0; i < 6; i ++)
+ for (unsigned int i = 0; i < 6; i++)
qmin[i] = this->_joint_min[i];
return qmin;
}
@@ -1016,11 +988,10 @@ vpAfma6::getJointMin() const
A,B,C in radians.
*/
-vpColVector
-vpAfma6::getJointMax() const
+vpColVector vpAfma6::getJointMax() const
{
vpColVector qmax(6);
- for (unsigned int i=0; i < 6; i ++)
+ for (unsigned int i = 0; i < 6; i++)
qmax[i] = this->_joint_max[i];
return qmax;
}
@@ -1031,11 +1002,7 @@ vpAfma6::getJointMax() const
\return Coupling factor between join 5 and 6.
*/
-double
-vpAfma6::getCoupl56() const
-{
- return _coupl_56;
-}
+double vpAfma6::getCoupl56() const { return _coupl_56; }
/*!
@@ -1043,12 +1010,7 @@ vpAfma6::getCoupl56() const
\return Distance between join 5 and 6.
*/
-double
-vpAfma6::getLong56() const
-{
- return _long_56;
-}
-
+double vpAfma6::getLong56() const { return _long_56; }
/*!
@@ -1056,100 +1018,94 @@ vpAfma6::getLong56() const
\param filename : File name containing the robot constant
parameters, like max/min joint values, distance between 5 and 6 axis,
- coupling factor between axis 5 and 6, and the hand-to-eye homogeneous matrix.
+ coupling factor between axis 5 and 6, and the hand-to-eye homogeneous
+ matrix.
*/
-void
-vpAfma6::parseConfigFile (const std::string &filename)
+void vpAfma6::parseConfigFile(const std::string &filename)
{
- double rot_eMc[3]; // rotation
- double trans_eMc[3]; // translation
+ vpRxyzVector erc; // eMc rotation
+ vpTranslationVector etc; // eMc translation
std::ifstream fdconfig(filename.c_str(), std::ios::in);
- if(! fdconfig.is_open()) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file: %s", filename.c_str());
+ if (!fdconfig.is_open()) {
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
+ filename.c_str());
}
std::string line;
int lineNum = 0;
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
+ bool get_erc = false;
+ bool get_etc = false;
int code;
- while(std::getline(fdconfig, line)) {
- lineNum ++;
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ while (std::getline(fdconfig, line)) {
+ lineNum++;
+ if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
continue;
}
std::istringstream ss(line);
std::string key;
ss >> key;
- for (code = 0; NULL != opt_Afma6[code]; ++ code) {
+ for (code = 0; NULL != opt_Afma6[code]; ++code) {
if (key.compare(opt_Afma6[code]) == 0) {
break;
}
}
- 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];
- break;
+ 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];
+ 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];
- 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];
+ break;
- case 2:
- ss >> this->_long_56;
- break;
+ case 2:
+ ss >> this->_long_56;
+ break;
- case 3:
- ss >> this->_coupl_56;
- break;
+ case 3:
+ ss >> this->_coupl_56;
+ break;
- case 4:
- break; // Nothing to do: camera name
+ case 4:
+ break; // Nothing to do: camera name
- case 5:
- ss >> rot_eMc[0] >> rot_eMc[1] >> rot_eMc[2];
+ case 5:
+ ss >> erc[0] >> erc[1] >> erc[2];
- // Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
- break;
+ // Convert rotation from degrees to radians
+ erc = erc * M_PI / 180.0;
+ get_erc = true;
+ break;
- case 6:
- ss >> trans_eMc[0] >> trans_eMc[1] >> trans_eMc[2];
- get_trans_eMc = true;
- break;
+ case 6:
+ ss >> etc[0] >> etc[1] >> etc[2];
+ get_etc = true;
+ break;
- default:
- throw(vpRobotException(vpRobotException::readingParametersError,
- "Bad configuration file %s line #%d",
- filename.c_str(), lineNum));
- } /* SWITCH */
- } /* WHILE */
+ default:
+ throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
+ filename.c_str(), lineNum));
+ }
+ }
+
+ fdconfig.close();
// Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- for (unsigned int i=0; i < 3; i ++) {
- _erc[i] = rot_eMc[i];
- _etc[i] = trans_eMc[i];
- }
+ if (get_etc && get_erc) {
+ _erc = erc;
+ _etc = etc;
vpRotationMatrix eRc(_erc);
this->_eMc.buildFrom(_etc, eRc);
}
-
- fdconfig.close();
}
/*!
@@ -1160,8 +1116,7 @@ vpAfma6::parseConfigFile (const std::string &filename)
\param eMc : Transformation between the end-effector frame
and the tool frame.
*/
-void
-vpAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
+void vpAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
{
this->_eMc = eMc;
this->_etc = eMc.getTranslationVector();
@@ -1197,10 +1152,10 @@ vpAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
attached to the robot.
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -1228,79 +1183,54 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
+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)
+#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,
+ 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.");
+ 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,
+ 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.");
+ 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,
+ 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.");
+ 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,
+ 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.");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
default: {
- vpERROR_TRACE ("This error should not occur!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -1309,8 +1239,7 @@ 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
@@ -1319,89 +1248,85 @@ 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;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(1090.6, 1090.0, 310.1, 260.8, -0.2114, 0.2217);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpAfma6::TOOL_GRIPPER: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(837.0, 837.5, 308.7, 251.6, -0.1455, 0.1511);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpAfma6::TOOL_VACUUM: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_VACUUM_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpAfma6::TOOL_GENERIC_CAMERA: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(828.5, 829.0, 322.5, 232.9, -0.1921, 0.2057);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
default:
- vpERROR_TRACE ("This error should not occur!");
+ vpERROR_TRACE("This error should not occur!");
break;
}
#endif
@@ -1418,10 +1343,10 @@ vpAfma6::getCameraParameters (vpCameraParameters &cam,
\param I : A B&W image send by the current camera in use.
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -1446,14 +1371,13 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
+void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ getCameraParameters(cam, I.getWidth(), I.getHeight());
}
/*!
\brief Get the current intrinsic camera parameters obtained by calibration.
@@ -1465,11 +1389,11 @@ vpAfma6::getCameraParameters (vpCameraParameters &cam,
\param I : A color image send by the current camera in use.
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -1494,18 +1418,16 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
+void vpAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ getCameraParameters(cam, I.getWidth(), I.getHeight());
}
-
/*!
Print on the output stream \e os the robot parameters (joint
@@ -1515,57 +1437,32 @@ vpAfma6::getCameraParameters (vpCameraParameters &cam,
\param os : Output stream.
\param afma6 : Robot parameters.
*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os,
- const vpAfma6 & afma6)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAfma6 &afma6)
{
vpRotationMatrix eRc;
afma6._eMc.extract(eRc);
vpRxyzVector rxyz(eRc);
- os
- << "Joint Max:" << std::endl
- << "\t" << afma6._joint_max[0]
- << "\t" << afma6._joint_max[1]
- << "\t" << afma6._joint_max[2]
- << "\t" << afma6._joint_max[3]
- << "\t" << afma6._joint_max[4]
- << "\t" << afma6._joint_max[5]
- << "\t" << std::endl
-
- << "Joint Min: " << std::endl
- << "\t" << afma6._joint_min[0]
- << "\t" << afma6._joint_min[1]
- << "\t" << afma6._joint_min[2]
- << "\t" << afma6._joint_min[3]
- << "\t" << afma6._joint_min[4]
- << "\t" << afma6._joint_min[5]
- << "\t" << std::endl
-
- << "Long 5-6: " << std::endl
- << "\t" << afma6._long_56
- << "\t" << std::endl
-
- << "Coupling 5-6:" << std::endl
- << "\t" << afma6._coupl_56
- << "\t" << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << afma6._eMc[0][3] << " "
- << afma6._eMc[1][3] << " "
- << afma6._eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
+ os << "Joint Max:" << std::endl
+ << "\t" << afma6._joint_max[0] << "\t" << afma6._joint_max[1] << "\t" << afma6._joint_max[2] << "\t"
+ << afma6._joint_max[3] << "\t" << afma6._joint_max[4] << "\t" << afma6._joint_max[5] << "\t" << std::endl
+
+ << "Joint Min: " << std::endl
+ << "\t" << afma6._joint_min[0] << "\t" << afma6._joint_min[1] << "\t" << afma6._joint_min[2] << "\t"
+ << afma6._joint_min[3] << "\t" << afma6._joint_min[4] << "\t" << afma6._joint_min[5] << "\t" << std::endl
+
+ << "Long 5-6: " << std::endl
+ << "\t" << afma6._long_56 << "\t" << std::endl
+
+ << "Coupling 5-6:" << std::endl
+ << "\t" << afma6._coupl_56 << "\t" << std::endl
+
+ << "eMc: " << std::endl
+ << "\tTranslation (m): " << afma6._eMc[0][3] << " " << afma6._eMc[1][3] << " " << afma6._eMc[2][3] << "\t"
+ << std::endl
+ << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
return os;
}
-
diff --git a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp
index 0d40cde..2993501 100644
--- a/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp
+++ b/modules/robot/src/real-robot/afma6/vpRobotAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,12 +45,12 @@
#include <sys/types.h>
#include <unistd.h>
-#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/robot/vpRobotException.h>
@@ -81,24 +82,27 @@ const double vpRobotAfma6::defaultPositioningVelocity = 15.0;
*/
void emergencyStopAfma6(int signo)
{
- std::cout << "Stop the Afma6 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
- case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
- case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
- case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
- case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
- case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
- }
- //std::cout << "Emergency stop called\n";
+ std::cout << "Stop the Afma6 application by signal (" << signo << "): " << (char)7;
+ switch (signo) {
+ case SIGINT:
+ std::cout << "SIGINT (stop by ^C) " << std::endl;
+ break;
+ case SIGBUS:
+ std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
+ break;
+ case SIGSEGV:
+ std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
+ break;
+ case SIGKILL:
+ std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
+ break;
+ case SIGQUIT:
+ std::cout << "SIGQUIT " << std::endl;
+ break;
+ default:
+ std::cout << signo << std::endl;
+ }
+ // std::cout << "Emergency stop called\n";
// PrimitiveESTOP_Afma6();
PrimitiveSTOP_Afma6();
std::cout << "Robot was stopped\n";
@@ -109,7 +113,7 @@ void emergencyStopAfma6(int signo)
fprintf(stdout, "Application ");
fflush(stdout);
kill(getpid(), SIGKILL);
- exit(1) ;
+ exit(1);
}
/* ---------------------------------------------------------------------- */
@@ -135,8 +139,8 @@ void emergencyStopAfma6(int signo)
vpRobotAfma6 robot;
// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distorsion parameter
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
+ robot.init(vpAfma6::TOOL_CCMOP,
+ vpCameraParameters::perspectiveProjWithDistortion); \endcode
Now, you can get the intrinsic camera parameters of the image I
acquired with the camera, with:
@@ -152,10 +156,7 @@ void emergencyStopAfma6(int signo)
vpCameraParameters::vpCameraParametersProjType)
*/
-vpRobotAfma6::vpRobotAfma6 (bool verbose)
- :
- vpAfma6 (),
- vpRobot ()
+vpRobotAfma6::vpRobotAfma6(bool verbose) : vpAfma6(), vpRobot()
{
/*
@@ -178,8 +179,8 @@ vpRobotAfma6::vpRobotAfma6 (bool verbose)
*/
signal(SIGINT, emergencyStopAfma6);
- signal(SIGBUS, emergencyStopAfma6) ;
- signal(SIGSEGV, emergencyStopAfma6) ;
+ signal(SIGBUS, emergencyStopAfma6);
+ signal(SIGSEGV, emergencyStopAfma6);
signal(SIGKILL, emergencyStopAfma6);
signal(SIGQUIT, emergencyStopAfma6);
@@ -188,20 +189,18 @@ vpRobotAfma6::vpRobotAfma6 (bool verbose)
std::cout << "Open communication with MotionBlox.\n";
try {
this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
+ this->setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
// vpERROR_TRACE("Error caught") ;
- throw ;
+ throw;
}
- positioningVelocity = defaultPositioningVelocity ;
+ positioningVelocity = defaultPositioningVelocity;
vpRobotAfma6::robotAlreadyCreated = true;
- return ;
+ return;
}
-
/* ------------------------------------------------------------------------ */
/* --- INITIALISATION ----------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -225,8 +224,7 @@ vpRobotAfma6::vpRobotAfma6 (bool verbose)
\sa vpCameraParameters, init(vpAfma6::vpAfma6CameraRobotType,
vpCameraParameters::vpCameraParametersProjType)
*/
-void
-vpRobotAfma6::init (void)
+void vpRobotAfma6::init(void)
{
InitTry;
@@ -242,12 +240,12 @@ vpRobotAfma6::init (void)
first_time_getdis = true;
// Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
+ Try(InitializeConnection(verbose_));
// Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Afma6() );
+ Try(InitializeNode_Afma6());
- Try( PrimitiveRESET_Afma6() );
+ Try(PrimitiveRESET_Afma6());
// Update the eMc matrix in the low level controller
init(vpAfma6::defaultTool);
@@ -255,14 +253,13 @@ 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
if (verbose_) {
std::cout << "Robot status: ";
- switch(EStopStatus) {
+ switch (EStopStatus) {
case ESTOP_AUTO:
case ESTOP_MANUAL:
if (HIPowerStatus == 0)
@@ -281,19 +278,21 @@ vpRobotAfma6::init (void)
std::cout << std::endl;
}
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
+ Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
+ // _joint_max[i]);
+ // }
// If an error occur in the low level controller, goto here
- //CatchPrint();
+ // CatchPrint();
Catch();
// Test if an error occurs
if (TryStt == -20001)
printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
+ "and if the firewire link exist between the MotionBlox and this "
+ "computer.\n");
else if (TryStt == -675)
printf(" Timeout enabling power...\n");
@@ -304,10 +303,9 @@ 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 ;
+ return;
}
/*!
@@ -331,8 +329,8 @@ vpRobotAfma6::init (void)
vpRobotAfma6 robot;
// Set the extrinsic camera parameters obtained with a perpective
// projection model including a distorsion parameter
- robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithDistortion);
- \endcode
+ robot.init(vpAfma6::TOOL_CCMOP,
+ vpCameraParameters::perspectiveProjWithDistortion); \endcode
Now, you can get the intrinsic camera parameters of the image \e I
acquired with the camera, with:
@@ -346,9 +344,7 @@ 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
@@ -357,27 +353,28 @@ vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool,
vpAfma6::init(tool, projModel);
// Set the robot constant (coupl_56, long_56) in the MotionBlox
- Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
+ Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = _etc[i]; // translation in meters
- eMc_pose[i+3] = _erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = _etc[i]; // translation in meters
+ eMc_pose[i + 3] = _erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
+ Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
-// for (unsigned int i=0; i < njoint; i++) {
-// printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i], _joint_max[i]);
-// }
+ Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, _joint_min[i],
+ // _joint_max[i]);
+ // }
setToolType(tool);
CatchPrint();
- return ;
+ return;
}
/*!
@@ -391,8 +388,7 @@ vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool,
\param eMc : Transformation between the end-effector frame
and the tool frame.
*/
-void
-vpRobotAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
+void vpRobotAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
{
InitTry;
// Set camera extrinsic parameters equal to eMc
@@ -400,12 +396,12 @@ vpRobotAfma6::set_eMc(const vpHomogeneousMatrix &eMc)
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = _etc[i]; // translation in meters
- eMc_pose[i+3] = _erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = _etc[i]; // translation in meters
+ eMc_pose[i + 3] = _erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
+ Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
CatchPrint();
}
@@ -445,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
@@ -455,19 +450,19 @@ vpRobotAfma6::init(vpAfma6::vpAfma6ToolType tool, const vpHomogeneousMatrix &eMc
vpAfma6::init(tool, eMc);
// Set the robot constant (coupl_56, long_56) in the MotionBlox
- Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
+ Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = _etc[i]; // translation in meters
- eMc_pose[i+3] = _erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = _etc[i]; // translation in meters
+ eMc_pose[i + 3] = _erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
+ Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
+ Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
setToolType(tool);
@@ -524,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
@@ -534,19 +528,19 @@ vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool, const std::string &filename)
vpAfma6::init(tool, filename);
// Set the robot constant (coupl_56, long_56) in the MotionBlox
- Try( PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56) );
+ Try(PrimitiveROBOT_CONST_Afma6(_coupl_56, _long_56));
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = _etc[i]; // translation in meters
- eMc_pose[i+3] = _erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = _etc[i]; // translation in meters
+ eMc_pose[i + 3] = _erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCAMERA_CONST_Afma6(eMc_pose) );
+ Try(PrimitiveCAMERA_CONST_Afma6(eMc_pose));
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max) );
+ Try(PrimitiveJOINT_MINMAX_Afma6(_joint_min, _joint_max));
setToolType(tool);
@@ -563,24 +557,23 @@ vpRobotAfma6::init (vpAfma6::vpAfma6ToolType tool, const std::string &filename)
Free allocated resources.
*/
-vpRobotAfma6::~vpRobotAfma6 (void)
+vpRobotAfma6::~vpRobotAfma6(void)
{
InitTry;
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
// 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) {
-// fprintf(stdout, "Power OFF the robot\n");
-// fflush(stdout);
+ // if (HIPowerStatus == 1) {
+ // fprintf(stdout, "Power OFF the robot\n");
+ // fflush(stdout);
-// Try( PrimitivePOWEROFF_Afma6() );
-// }
+ // Try( PrimitivePOWEROFF_Afma6() );
+ // }
// Free allocated resources
ShutDownConnection();
@@ -591,55 +584,50 @@ vpRobotAfma6::~vpRobotAfma6 (void)
return;
}
-
-
-
/*!
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;
switch (newState) {
case vpRobot::STATE_STOP: {
- if (vpRobot::STATE_STOP != getRobotState ()) {
- Try( PrimitiveSTOP_Afma6() );
+ if (vpRobot::STATE_STOP != getRobotState()) {
+ Try(PrimitiveSTOP_Afma6());
}
break;
}
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Afma6() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
+ Try(PrimitiveSTOP_Afma6());
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
this->powerOn();
break;
}
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
std::cout << "Change the control mode from stop to velocity control.\n";
}
this->powerOn();
break;
}
default:
- break ;
+ break;
}
CatchPrint();
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
-
/* ------------------------------------------------------------------------ */
/* --- STOP --------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -651,18 +639,16 @@ vpRobotAfma6::setRobotState(vpRobot::vpRobotStateType newState)
\exception vpRobotException::lowLevelError : If the low level
controller returns an error during robot stopping.
*/
-void
-vpRobotAfma6::stopMotion(void)
+void vpRobotAfma6::stopMotion(void)
{
InitTry;
- Try( PrimitiveSTOP_Afma6() );
- setRobotState (vpRobot::STATE_STOP);
+ Try(PrimitiveSTOP_Afma6());
+ setRobotState(vpRobot::STATE_STOP);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
+ vpERROR_TRACE("Cannot stop robot motion");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
}
}
@@ -675,8 +661,7 @@ vpRobotAfma6::stopMotion(void)
\sa powerOff(), getPowerState()
*/
-void
-vpRobotAfma6::powerOn(void)
+void vpRobotAfma6::powerOn(void)
{
InitTry;
@@ -686,26 +671,24 @@ vpRobotAfma6::powerOn(void)
bool firsttime = true;
unsigned int nitermax = 10;
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
+ for (unsigned int i = 0; i < nitermax; i++) {
+ Try(PrimitiveSTATUS_Afma6(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
if (EStopStatus == ESTOP_AUTO) {
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
+ } else if (EStopStatus == ESTOP_MANUAL) {
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
+ } else if (EStopStatus == ESTOP_ACTIVATED) {
if (firsttime) {
std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ << "Check the emergency stop button and push the yellow "
+ "button before continuing."
+ << std::endl;
firsttime = false;
}
- fprintf(stdout, "Remaining time %us \r", nitermax-i);
+ fprintf(stdout, "Remaining time %us \r", nitermax - i);
fflush(stdout);
CAL_Wait(1);
- }
- else {
+ } else {
std::cout << "Sorry there is an error on the emergency chain." << std::endl;
std::cout << "You have to call Adept for maintenance..." << std::endl;
// Free allocated resources
@@ -719,22 +702,20 @@ 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) {
fprintf(stdout, "Power ON the Afma6 robot\n");
fflush(stdout);
- Try( PrimitivePOWERON_Afma6() );
+ Try(PrimitivePOWERON_Afma6());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power on the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -747,29 +728,26 @@ vpRobotAfma6::powerOn(void)
\sa powerOn(), getPowerState()
*/
-void
-vpRobotAfma6::powerOff(void)
+void vpRobotAfma6::powerOff(void)
{
InitTry;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
fprintf(stdout, "Power OFF the Afma6 robot\n");
fflush(stdout);
- Try( PrimitivePOWEROFF_Afma6() );
+ Try(PrimitivePOWEROFF_Afma6());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power off the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -784,15 +762,13 @@ vpRobotAfma6::powerOff(void)
\sa powerOn(), powerOff()
*/
-bool
-vpRobotAfma6::getPowerState(void)
+bool vpRobotAfma6::getPowerState(void)
{
InitTry;
bool status = false;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
@@ -801,9 +777,8 @@ vpRobotAfma6::getPowerState(void)
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
+ vpERROR_TRACE("Cannot get the power status");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
}
return status;
}
@@ -817,13 +792,12 @@ vpRobotAfma6::getPowerState(void)
\param cVe : Twist transformation.
*/
-void
-vpRobotAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotAfma6::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- vpAfma6::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpAfma6::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -836,12 +810,7 @@ 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); }
/*!
@@ -853,30 +822,26 @@ vpRobotAfma6::get_cMe(vpHomogeneousMatrix &cMe) const
\param eJe : Robot jacobian expressed in the end-effector frame.
*/
-void
-vpRobotAfma6::get_eJe(vpMatrix &eJe)
+void vpRobotAfma6::get_eJe(vpMatrix &eJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
- try
- {
- vpAfma6::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
- }
+ try {
+ vpAfma6::get_eJe(q, eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
+ }
}
/*!
@@ -901,30 +866,26 @@ vpRobotAfma6::get_eJe(vpMatrix &eJe)
\param fJe : Robot jacobian expressed in the reference frame.
*/
- void
- vpRobotAfma6::get_fJe(vpMatrix &fJe)
+void vpRobotAfma6::get_fJe(vpMatrix &fJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Afma6(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma6(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
- try
- {
- vpAfma6::get_fJe(q, fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
+ try {
+ vpAfma6::get_fJe(q, fJe);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
+ }
}
/*!
@@ -955,22 +916,14 @@ vpRobotAfma6::get_eJe(vpMatrix &eJe)
\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; }
/*!
@@ -983,8 +936,8 @@ vpRobotAfma6::getPositioningVelocity (void)
is reached by the robot.
\param pose : A six dimension pose vector corresponding to the position
- to reach. The three first parameters are the translations in meter, the three
- last parameters are the rotations expressed as a theta u vector in
+ to reach. The three first parameters are the translations in meter, the
+three last parameters are the rotations expressed as a theta u vector in
radians. If the position is out of range, an exception is provided.
\param frame : Frame in which the position is expressed.
@@ -992,9 +945,9 @@ vpRobotAfma6::getPositioningVelocity (void)
- In the camera and the reference frame, rotations are
represented by a vpThetaUVector.
- - Mixt frame of joint frame is not implemented.
+ - Mixt frame of joint frame is not implemented.
- \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
+ \exception vpRobotException::lowLevelError : vpRobot::MIXT_FRAME
and vpRobot::ARTICULAR_FRAME not implemented.
\exception vpRobotException::positionOutOfRangeError : The requested
@@ -1002,8 +955,8 @@ vpRobotAfma6::getPositioningVelocity (void)
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotAfma6.h>
#include <visp3/core/vpPoseVector.h>
+#include <visp3/robot/vpRobotAfma6.h>
int main()
{
@@ -1032,7 +985,8 @@ int main()
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1046,28 +1000,25 @@ 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);
vpRxyzVector rxyz;
vpRotationMatrix R;
- R.buildFrom(pose[3], pose[4], pose[5]); //thetau
+ R.buildFrom(pose[3], pose[4], pose[5]); // thetau
rxyz.buildFrom(R);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
position[i] = pose[i];
- position[i+3] = rxyz[i];
+ 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);
+ setPosition(frame, position);
}
/*!
@@ -1104,9 +1055,9 @@ vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
position is out of range.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpColVector.h>
#include <visp3/robot/vpRobotException.h>
int main()
@@ -1130,13 +1081,14 @@ int main()
// Moves the robot in the camera frame
robot.setPosition(vpRobot::CAMERA_FRAME, position);
-
+
return 0;
#endif
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1151,30 +1103,27 @@ 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 ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
- }
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
+ }
double _destination[6];
int error = 0;
double timestamp;
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
vpColVector q(njoint);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = _q[i];
// Get fMc from the inverse kinematics
@@ -1184,9 +1133,9 @@ vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
// Set cMc from the input position
vpTranslationVector txyz;
vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
txyz[i] = position[i];
- rxyz[i] = position[i+3];
+ rxyz[i] = position[i + 3];
}
// Compute cMc2
@@ -1200,35 +1149,33 @@ vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
bool nearest = true;
int solution = this->getInverseKinematics(fMc2, q, nearest);
if (solution) { // Position is reachable
- for (unsigned int i=0; i < njoint; i ++) {
- _destination[i] = q[i];
+ for (unsigned int i = 0; i < njoint; i++) {
+ _destination[i] = q[i];
}
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
+ Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
+ Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
+ } else {
// Cartesian position is out of range
error = -1;
}
- break ;
+ break;
}
case vpRobot::ARTICULAR_FRAME: {
- for (unsigned int i=0; i < njoint; i ++) {
+ for (unsigned int i = 0; i < njoint; i++) {
_destination[i] = position[i];
}
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- break ;
-
+ Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
+ Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
+ break;
}
case vpRobot::REFERENCE_FRAME: {
// Set fMc from the input position
vpTranslationVector txyz;
vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
txyz[i] = position[i];
- rxyz[i] = position[i+3];
+ rxyz[i] = position[i + 3];
}
// Compute fMc from the input position
vpRotationMatrix fRc(rxyz);
@@ -1239,26 +1186,23 @@ vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
bool nearest = true;
int solution = this->getInverseKinematics(fMc, q, nearest);
if (solution) { // Position is reachable
- for (unsigned int i=0; i < njoint; i ++) {
- _destination[i] = q[i];
+ for (unsigned int i = 0; i < njoint; i++) {
+ _destination[i] = q[i];
}
- Try( PrimitiveMOVE_Afma6(_destination, positioningVelocity) );
- Try( WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000) );
- }
- else {
+ Try(PrimitiveMOVE_Afma6(_destination, positioningVelocity));
+ Try(WaitState_Afma6(ETAT_ATTENTE_AFMA6, 1000));
+ } else {
// Cartesian position is out of range
error = -1;
}
- break ;
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
}
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- }
}
CatchPrint();
@@ -1267,15 +1211,14 @@ vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
else if (TryStt < 0)
std::cout << " : Unknown error (see Fabien).\n";
else if (error == -1)
- std::cout << "Position out of range.\n";
+ std::cout << "Position out of range.\n";
if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
}
- return ;
+ return;
}
/*!
@@ -1345,30 +1288,23 @@ int main()
\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) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
+ try {
+ vpColVector position(6);
+ position[0] = pos1;
+ position[1] = pos2;
+ position[2] = pos3;
+ position[3] = pos4;
+ position[4] = pos5;
+ position[5] = pos6;
+
+ setPosition(frame, position);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
+ }
}
/*!
@@ -1384,9 +1320,9 @@ void vpRobotAfma6::setPosition (const vpRobot::vpControlFrameType frame,
This method has the same behavior than the sample code given below;
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1420,9 +1356,8 @@ void vpRobotAfma6::setPosition(const std::string &filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename.c_str());
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -1468,7 +1403,8 @@ void vpRobotAfma6::setPosition(const std::string &filename)
}
// 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 pose in terms of a homogeneous matrix
vpHomogeneousMatrix fMc(ftc, fRc);
@@ -1481,35 +1417,33 @@ 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;
- position.resize (6);
+ position.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME : {
+ case vpRobot::CAMERA_FRAME: {
position = 0;
return;
}
- case vpRobot::ARTICULAR_FRAME : {
+ case vpRobot::ARTICULAR_FRAME: {
double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
+ Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
+ for (unsigned int i = 0; i < njoint; i++) {
position[i] = _q[i];
}
return;
}
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::REFERENCE_FRAME: {
double _q[njoint];
- Try( PrimitiveACQ_POS_Afma6(_q, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma6(_q, ×tamp));
vpColVector q(njoint);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = _q[i];
// Compute fMc
@@ -1522,31 +1456,30 @@ vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
vpRxyzVector rxyz;
rxyz.buildFrom(fRc);
- for (unsigned int i=0; i < 3; i++) {
- position[i] = fMc[i][3]; // translation x,y,z
- position[i+3] = rxyz[i]; // Euler rotation x,y,z
+ for (unsigned int i = 0; i < 3; i++) {
+ position[i] = fMc[i][3]; // translation x,y,z
+ position[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
- break ;
+ break;
}
case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("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.");
+ vpERROR_TRACE("Cannot get position.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
}
return;
}
/*!
- Returns the robot controller current time (in second) since last robot power on.
+ Returns the robot controller current time (in second) since last robot power
+ on.
*/
double vpRobotAfma6::getTime() const
{
@@ -1559,13 +1492,13 @@ double vpRobotAfma6::getTime() const
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
+void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
@@ -1575,28 +1508,26 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
Similar as :
- void getPosition (const vpRobot::vpControlFrameType frame, vpColVector &position)
- as the difference the position is returned using a ThetaU representation.
+ void getPosition (const vpRobot::vpControlFrameType frame, vpColVector
+ &position) as the difference the position is returned using a ThetaU
+ representation.
*/
-void
-vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
+void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp)
{
vpColVector posRxyz;
- //recupere position en Rxyz
+ // recupere position en Rxyz
this->getPosition(frame, posRxyz, timestamp);
vpRxyzVector RxyzVect;
- for(unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
+ for (unsigned int j = 0; j < 3; j++)
+ RxyzVect[j] = posRxyz[j + 3];
+ // recupere le vecteur thetaU correspondant
vpThetaUVector RtuVect(RxyzVect);
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1604,13 +1535,13 @@ vpRobotAfma6::getPosition (const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
+void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
@@ -1620,27 +1551,36 @@ void vpRobotAfma6::getPosition(const vpRobot::vpControlFrameType frame,
Apply a velocity to the robot.
\param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
+ could be expressed in articular, camera frame, reference frame or mixt
+ frame.
\param vel : Velocity vector. Translation velocities are expressed
in m/s while rotation velocities in rad/s. The size of this vector
is always 6.
- In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities, where
+ \f$\dot{q}_1, \dot{q}_2, \dot{q}_3 \f$ are the translation velocities in
+ m/s and \f$\dot{q}_4, \dot{q}_5, \dot{q}_6\f$ are the rotation velocities in
+ rad/s.
- In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
+ expressed in the camera frame, with translations velocities \f$ ^{c} v_x,
+ ^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+ \omega_y, ^{c} \omega_z \f$ in rad/s.
- In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
+ expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
+ ^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+ \omega_y, ^{c} \omega_z \f$ in rad/s.
- In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
+ translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
+ reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
+ \omega_z \f$ in the camera frame in rad/s.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
@@ -1670,53 +1610,50 @@ 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) ");
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
}
vpColVector vel_max(6);
- for (unsigned int i=0; i<3; i++)
+ for (unsigned int i = 0; i < 3; i++)
vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
+ for (unsigned int i = 3; i < 6; i++)
vel_max[i] = getMaxRotationVelocity();
vpColVector vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
- Try( PrimitiveMOVESPEED_Afma6(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- Try( PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6) );
- break ;
- }
- default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPCAM_AFMA6));
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_AFMA6) );
+ Try(PrimitiveMOVESPEED_Afma6(vel_sat.data));
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPFIX_AFMA6));
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ Try(PrimitiveMOVESPEED_CART_Afma6(vel_sat.data, REPMIX_AFMA6));
+ break;
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
}
Catch();
@@ -1724,19 +1661,16 @@ vpRobotAfma6::setVelocity (const vpRobot::vpControlFrameType frame,
if (TryStt == VelStopOnJoint) {
Int32 axisInJoint[njoint];
PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
+ 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 {
+ } else {
printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
if (TryString != NULL) {
// The statement is in TryString, but we need to check the validity
printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
+ } else {
printf("\n");
}
}
@@ -1745,16 +1679,10 @@ vpRobotAfma6::setVelocity (const vpRobot::vpControlFrameType frame,
return;
}
-
-
-
-
-
/* ------------------------------------------------------------------------ */
/* --- GET ---------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
/*!
Get the robot velocities.
@@ -1767,8 +1695,8 @@ vpRobotAfma6::setVelocity (const vpRobot::vpControlFrameType frame,
\param timestamp : Time in second since last robot power on.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+ \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\warning The first time this method is called, \e velocity is set to 0. The
first call is used to intialise the velocity computation for the next call.
@@ -1804,11 +1732,9 @@ 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.resize(6);
velocity = 0;
double q[6];
@@ -1820,16 +1746,16 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
+ Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
time_cur = timestamp;
- for (unsigned int i=0; i < njoint; i ++) {
+ for (unsigned int i = 0; i < njoint; i++) {
q_cur[i] = q[i];
}
// Get the camera pose from the direct kinematics
vpAfma6::get_fMc(q_cur, fMc_cur);
- if ( ! first_time_getvel ) {
+ if (!first_time_getvel) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
@@ -1839,12 +1765,12 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
// Compute the velocity of the camera from this displacement
velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- break ;
+ break;
}
case vpRobot::ARTICULAR_FRAME: {
velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
+ break;
}
case vpRobot::REFERENCE_FRAME: {
@@ -1859,7 +1785,7 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
vpVelocityTwistMatrix fVc(fMc_cur);
velocity = fVc * v;
- break ;
+ break;
}
case vpRobot::MIXT_FRAME: {
@@ -1872,20 +1798,19 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
vpThetaUVector thetaU;
thetaU.buildFrom(cRc);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
// Compute the translation displacement in the reference frame
velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
// Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
+ velocity[i + 3] = thetaU[i];
}
// Compute the velocity
velocity /= (time_cur - time_prev_getvel);
- break ;
+ break;
}
}
- }
- else {
+ } else {
first_time_getvel = false;
}
@@ -1898,12 +1823,10 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
// Memorize the time associated to the joint position for the next call
time_prev_getvel = time_cur;
-
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
@@ -1911,18 +1834,16 @@ vpRobotAfma6::getVelocity (const vpRobot::vpControlFrameType frame,
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ vpColVector &, double &) except that the timestamp is not returned.
*/
-void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
+void vpRobotAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
double timestamp;
getVelocity(frame, velocity, timestamp);
}
-
/*!
Get the robot velocities.
@@ -1964,11 +1885,10 @@ 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);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -1977,15 +1897,15 @@ vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame, double ×tamp)
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ double &) except that the timestamp is not returned.
*/
-vpColVector vpRobotAfma6::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotAfma6::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector velocity;
double timestamp;
- getVelocity (frame, velocity, timestamp);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -2024,7 +1944,8 @@ expressed in meters, while joint rotations A,B,C in radians.
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+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
@@ -2038,12 +1959,11 @@ robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
\sa savePosFile()
*/
-bool
-vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
+bool vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
{
std::ifstream fd(filename.c_str(), std::ios::in);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -2055,29 +1975,29 @@ vpRobotAfma6::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -2123,12 +2043,11 @@ 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 ;
- fd = fopen(filename.c_str(), "w") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -2142,15 +2061,10 @@ vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
#\n\n");
// Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- q[0],
- q[1],
- q[2],
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
+ 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);
}
@@ -2164,15 +2078,14 @@ vpRobotAfma6::savePosFile(const std::string &filename, const vpColVector &q)
\sa readPosFile(), move(const char *, const double)
*/
-void
-vpRobotAfma6::move(const std::string &filename)
+void vpRobotAfma6::move(const std::string &filename)
{
vpColVector q;
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
@@ -2187,15 +2100,14 @@ vpRobotAfma6::move(const std::string &filename)
\sa readPosFile(), move(const char *)
*/
-void
-vpRobotAfma6::move(const std::string &filename, const double velocity)
+void vpRobotAfma6::move(const std::string &filename, const double velocity)
{
vpColVector q;
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPositioningVelocity(velocity);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
@@ -2204,17 +2116,15 @@ vpRobotAfma6::move(const std::string &filename, const double velocity)
\sa closeGripper()
*/
-void
-vpRobotAfma6::openGripper()
+void vpRobotAfma6::openGripper()
{
InitTry;
- Try( PrimitiveGripper_Afma6(1) );
- std::cout << "Open the gripper..." << std::endl;
+ Try(PrimitiveGripper_Afma6(1));
+ std::cout << "Open the gripper..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot open the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot open the gripper.");
+ vpERROR_TRACE("Cannot open the gripper");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot open the gripper.");
}
}
@@ -2225,57 +2135,20 @@ vpRobotAfma6::openGripper()
\sa openGripper()
*/
-void
-vpRobotAfma6::closeGripper()
+void vpRobotAfma6::closeGripper()
{
InitTry;
- Try( PrimitiveGripper_Afma6(0) );
- std::cout << "Close the gripper..." << std::endl;
+ Try(PrimitiveGripper_Afma6(0));
+ std::cout << "Close the gripper..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot close the gripper");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot close the gripper.");
+ vpERROR_TRACE("Cannot close the gripper");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot close the gripper.");
}
}
/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotAfma6::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param displacement : The measured articular displacement. The
- dimension of \e displacement is 6 (the number of axis of the
- robot). Translations are expressed in meters, rotations in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
-vpRobotAfma6::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
Get the robot displacement since the last call of this method.
\warning This functionnality is not implemented for the moment in the
@@ -2291,33 +2164,29 @@ vpRobotAfma6::getArticularDisplacement(vpColVector &displacement)
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
-
*/
-void
-vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
double q[6];
vpColVector q_cur(6);
- vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
+ vpHomogeneousMatrix fMc_cur, c_prevMc_cur;
double timestamp;
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Afma6(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
+ Try(PrimitiveACQ_POS_Afma6(q, ×tamp));
+ for (unsigned int i = 0; i < njoint; i++) {
q_cur[i] = q[i];
}
// Compute the camera pose in the reference frame
fMc_cur = get_fMc(q_cur);
- if ( ! first_time_getdis ) {
+ if (!first_time_getdis) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
// Compute the camera dispacement from the previous pose
@@ -2327,20 +2196,20 @@ vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
vpRotationMatrix R;
c_prevMc_cur.extract(t);
c_prevMc_cur.extract(R);
-
+
vpRxyzVector rxyz;
rxyz.buildFrom(R);
-
- for (unsigned int i=0; i<3; i++) {
- displacement[i] = t[i];
- displacement[i+3] = rxyz[i];
+
+ for (unsigned int i = 0; i < 3; i++) {
+ displacement[i] = t[i];
+ displacement[i + 3] = rxyz[i];
}
- break ;
+ break;
}
case vpRobot::ARTICULAR_FRAME: {
displacement = q_cur - q_prev_getdis;
- break ;
+ break;
}
case vpRobot::REFERENCE_FRAME: {
@@ -2353,8 +2222,7 @@ vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
return;
}
}
- }
- else {
+ } else {
first_time_getdis = false;
}
@@ -2366,9 +2234,8 @@ vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
@@ -2376,47 +2243,41 @@ vpRobotAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
Test the joints of the robot to detect if one or more is at its limit.
- \param jointsStatus : A vector (size 6) of the status of the joints. For each
- joint, the value is equal to 1 if the joint is at its maximal limit, -1 if the
- joint is at its minimal value and 0 otherwise.
-
+ \param jointsStatus : A vector (size 6) of the status of the joints. For
+ each joint, the value is equal to 1 if the joint is at its maximal limit, -1
+ if the joint is at its minimal value and 0 otherwise.
+
\return false if at least one joint is at one of its limit.
*/
-bool
-vpRobotAfma6::checkJointLimits(vpColVector& jointsStatus)
+bool vpRobotAfma6::checkJointLimits(vpColVector &jointsStatus)
{
Int32 axisInJoint[njoint];
bool status = true;
jointsStatus.resize(6);
InitTry;
- Try (PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint,
-NULL));
- for (unsigned int i=0; i < njoint; i ++) {
- if (axisInJoint[i]){
- std::cout << "\nWarning: Velocity control stopped: axis "
- << i+1 << " on joint limit!" <<std::endl;
- jointsStatus[i] = axisInJoint[i];
- status = false;
- }
- else{
- jointsStatus[i] = 0;
- }
+ Try(PrimitiveSTATUS_Afma6(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL));
+ for (unsigned int i = 0; i < njoint; i++) {
+ if (axisInJoint[i]) {
+ std::cout << "\nWarning: Velocity control stopped: axis " << i + 1 << " on joint limit!" << std::endl;
+ jointsStatus[i] = axisInJoint[i];
+ status = false;
+ } else {
+ jointsStatus[i] = 0;
+ }
}
Catch();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot check joint limits.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot check joint limits.");
+ vpERROR_TRACE("Cannot check joint limits.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot check joint limits.");
}
return status;
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no symbols
-void dummy_vpRobotAfma6() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotAfma6.cpp.o) has no
+// symbols
+void dummy_vpRobotAfma6(){};
#endif
-
diff --git a/modules/robot/src/real-robot/biclops/vpBiclops.cpp b/modules/robot/src/real-robot/biclops/vpBiclops.cpp
index 0c1236f..910f9aa 100644
--- a/modules/robot/src/real-robot/biclops/vpBiclops.cpp
+++ b/modules/robot/src/real-robot/biclops/vpBiclops.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,24 +40,23 @@
/* --- INCLUDE ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
-
-
-#include <visp3/robot/vpBiclops.h>
+#include <math.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpMath.h>
-#include <math.h>
-
+#include <visp3/robot/vpBiclops.h>
+#include <visp3/robot/vpRobotException.h>
/* ------------------------------------------------------------------------ */
/* --- COMPUTE ------------------------------------------------------------ */
/* ------------------------------------------------------------------------ */
-const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
-const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
-const float vpBiclops::panJointLimit = (float)(M_PI); /*!< Pan range (in rad): from -panJointLimit to + panJointLimit */
-const float vpBiclops::tiltJointLimit = (float)(M_PI/4.5); /*!< Tilt range (in rad): from -tiltJointLimit to + tiltJointLimit */
+const unsigned int vpBiclops::ndof = 2; /*<! Only pan and tilt are considered. */
+const float vpBiclops::h = 0.048f; /*<! Vertical offset from last joint to camera frame. */
+const float vpBiclops::panJointLimit = (float)(M_PI); /*!< Pan range (in rad): from -panJointLimit to +
+ panJointLimit */
+const float vpBiclops::tiltJointLimit = (float)(M_PI / 4.5); /*!< Tilt range (in rad): from -tiltJointLimit to +
+ tiltJointLimit */
-const float vpBiclops::speedLimit = (float)(M_PI/3.0); /*!< Maximum speed (in rad/s) to perform a displacement */
+const float vpBiclops::speedLimit = (float)(M_PI / 3.0); /*!< Maximum speed (in rad/s) to perform a displacement */
/*!
Compute the direct geometric model of the camera: fMc
@@ -67,20 +67,19 @@ const float vpBiclops::speedLimit = (float)(M_PI/3.0); /*!< Maximum speed (in ra
\param q : Articular position for pan and tilt axis.
\param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
+ of the camera. Describes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
\sa get_fMc(const vpColVector &, vpHomogeneousMatrix &)
*/
-void
-vpBiclops::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
+void vpBiclops::computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
vpHomogeneousMatrix fMe = get_fMe(q);
fMc = fMe * cMe_.inverse();
- vpCDEBUG (6) << "camera position: " << std::endl << fMc;
+ vpCDEBUG(6) << "camera position: " << std::endl << fMc;
- return ;
+ return;
}
/*!
@@ -89,19 +88,18 @@ vpBiclops::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
\param q : Articular position for pan and tilt axis.
\param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
+ of the camera. Describes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
*/
-void
-vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
+void vpBiclops::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
vpHomogeneousMatrix fMe = get_fMe(q);
fMc = fMe * cMe_.inverse();
- vpCDEBUG (6) << "camera position: " << std::endl << fMc;
+ vpCDEBUG(6) << "camera position: " << std::endl << fMc;
- return ;
+ return;
}
/*!
@@ -113,17 +111,16 @@ vpBiclops::get_fMc (const vpColVector &q, vpHomogeneousMatrix &fMc) const
\param q : Articular position for pan and tilt axis.
\return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Describes the transformation between the robot reference
- frame (called fixed) and the camera frame.
+ model of the camera. Describes the transformation between the robot
+ reference frame (called fixed) and the camera frame.
\sa get_fMc(const vpColVector &)
*/
-vpHomogeneousMatrix
-vpBiclops::computeMGD (const vpColVector & q) const
+vpHomogeneousMatrix vpBiclops::computeMGD(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
- computeMGD (q, fMc);
+ computeMGD(q, fMc);
return fMc;
}
@@ -134,16 +131,15 @@ vpBiclops::computeMGD (const vpColVector & q) const
\param q : Articular position for pan and tilt axis.
\return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Discribes the transformation between the robot reference
- frame (called fixed) and the camera frame.
+ model of the camera. Discribes the transformation between the robot
+ reference frame (called fixed) and the camera frame.
*/
-vpHomogeneousMatrix
-vpBiclops::get_fMc (const vpColVector & q) const
+vpHomogeneousMatrix vpBiclops::get_fMc(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
- get_fMc (q, fMc);
+ get_fMc(q, fMc);
return fMc;
}
@@ -154,12 +150,11 @@ vpBiclops::get_fMc (const vpColVector & q) const
\param q : Articular position for pan and tilt axis.
\return fMe, the homogeneous matrix corresponding to the direct geometric
- model of the end effector. Describes the transformation between the robot reference
- frame (called fixed) and the end effector frame.
+ model of the end effector. Describes the transformation between the robot
+ reference frame (called fixed) and the end effector frame.
*/
-vpHomogeneousMatrix
-vpBiclops::get_fMe (const vpColVector & q) const
+vpHomogeneousMatrix vpBiclops::get_fMe(const vpColVector &q) const
{
vpHomogeneousMatrix fMe;
@@ -168,24 +163,23 @@ vpBiclops::get_fMe (const vpColVector & q) const
throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
}
- double q1 = q[0]; // pan
- double q2 = q[1]; // tilt
+ double q1 = q[0]; // pan
+ double q2 = q[1]; // tilt
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
- if (dh_model_ == DH1)
- {
- fMe[0][0] = -c1*s2;
+ if (dh_model_ == DH1) {
+ fMe[0][0] = -c1 * s2;
fMe[0][1] = -s1;
- fMe[0][2] = c1*c2;
+ fMe[0][2] = c1 * c2;
fMe[0][3] = 0;
- fMe[1][0] = -s1*s2;
+ fMe[1][0] = -s1 * s2;
fMe[1][1] = c1;
- fMe[1][2] = s1*c2;
+ fMe[1][2] = s1 * c2;
fMe[1][3] = 0;
fMe[2][0] = -c2;
@@ -197,17 +191,15 @@ vpBiclops::get_fMe (const vpColVector & q) const
fMe[3][1] = 0;
fMe[3][2] = 0;
fMe[3][3] = 1;
- }
- else
- {
- fMe[0][0] = c1*s2;
+ } else {
+ fMe[0][0] = c1 * s2;
fMe[0][1] = -s1;
- fMe[0][2] = c1*c2;
+ fMe[0][2] = c1 * c2;
fMe[0][3] = 0;
- fMe[1][0] = s1*s2;
+ fMe[1][0] = s1 * s2;
fMe[1][1] = c1;
- fMe[1][2] = s1*c2;
+ fMe[1][2] = s1 * c2;
fMe[1][3] = 0;
fMe[2][0] = -c2;
@@ -232,21 +224,20 @@ vpBiclops::get_fMe (const vpColVector & q) const
\param q : Articular position for pan and tilt axis.
- \param fvc : Pose vector corresponding to the transformation between the robot
- reference frame (called fixed) and the camera frame.
+ \param fvc : Pose vector corresponding to the transformation between the
+ robot reference frame (called fixed) and the camera frame.
\sa get_fMc(const vpColVector &, vpPoseVector &)
*/
-void
-vpBiclops::computeMGD (const vpColVector &q, vpPoseVector &fvc) const
+void vpBiclops::computeMGD(const vpColVector &q, vpPoseVector &fvc) const
{
vpHomogeneousMatrix fMc;
- get_fMc (q, fMc);
+ get_fMc(q, fMc);
fvc.buildFrom(fMc.inverse());
- return ;
+ return;
}
/*!
@@ -254,23 +245,21 @@ vpBiclops::computeMGD (const vpColVector &q, vpPoseVector &fvc) const
\param q : Articular position for pan and tilt axis.
- \param fvc : Pose vector corresponding to the transformation between the robot
- reference frame (called fixed) and the camera frame.
+ \param fvc : Pose vector corresponding to the transformation between the
+ robot reference frame (called fixed) and the camera frame.
*/
-void
-vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc) const
+void vpBiclops::get_fMc(const vpColVector &q, vpPoseVector &fvc) const
{
vpHomogeneousMatrix fMc;
- get_fMc (q, fMc);
+ get_fMc(q, fMc);
fvc.buildFrom(fMc.inverse());
- return ;
+ return;
}
-
/* ---------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
@@ -280,11 +269,7 @@ vpBiclops::get_fMc (const vpColVector &q, vpPoseVector &fvc) const
Default construtor. Call init().
*/
-vpBiclops::vpBiclops (void)
- : dh_model_(DH1), cMe_()
-{
- init();
-}
+vpBiclops::vpBiclops(void) : dh_model_(DH1), cMe_() { init(); }
/* ---------------------------------------------------------------------- */
/* --- PRIVATE ---------------------------------------------------------- */
/* ---------------------------------------------------------------------- */
@@ -294,57 +279,48 @@ vpBiclops::vpBiclops (void)
Set the default \f${^c}{\bf M}_e\f$ transformation.
*/
-void
-vpBiclops::init ()
+void vpBiclops::init()
{
dh_model_ = DH1;
set_cMe();
- return ;
+ return;
}
-
/* ----------------------------------------------------------------------- */
/* --- DISPLAY ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpBiclops & /*constant*/)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpBiclops & /*constant*/)
{
- os
- << "Geometric parameters: " << std::endl
- << "h: "
- << "\t" << vpBiclops::h << std::endl;
+ os << "Geometric parameters: " << std::endl
+ << "h: "
+ << "\t" << vpBiclops::h << std::endl;
return os;
}
-
/*!
Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cVe : Twist transformation between camera and end effector frame to
expess a velocity skew from end effector frame in camera frame.
*/
-void
-vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
-{
- cVe.buildFrom(cMe_) ;
-}
+void vpBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const { cVe.buildFrom(cMe_); }
/*!
- Set the default homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ Set the default homogeneous matrix corresponding to the transformation
+ between the camera frame and the end effector frame. The end effector frame
+ is located on the tilt axis.
*/
-void
-vpBiclops::set_cMe()
+void vpBiclops::set_cMe()
{
- vpHomogeneousMatrix eMc ;
+ vpHomogeneousMatrix eMc;
eMc[0][0] = 0;
eMc[0][1] = -1;
@@ -366,7 +342,7 @@ vpBiclops::set_cMe()
eMc[3][2] = 0;
eMc[3][3] = 1;
- cMe_ = eMc.inverse() ;
+ cMe_ = eMc.inverse();
}
/*!
@@ -381,69 +357,59 @@ vpBiclops::set_cMe()
tilt axis).
*/
-void
-vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+void vpBiclops::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
- eJe.resize(6,2) ;
+ eJe.resize(6, 2);
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for biclops articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
}
- double s2 = sin(q[1]) ;
- double c2 = cos(q[1]) ;
+ double s2 = sin(q[1]);
+ double c2 = cos(q[1]);
eJe = 0;
- if (dh_model_ == DH1)
- {
+ if (dh_model_ == DH1) {
eJe[3][0] = -c2;
eJe[4][1] = 1;
eJe[5][0] = -s2;
- }
- else
- {
+ } else {
eJe[3][0] = -c2;
eJe[4][1] = -1;
eJe[5][0] = s2;
-
}
-
}
/*!
Get the robot jacobian expressed in the robot reference frame
\param q : Articular position for pan and tilt axis.
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
+ \param fJe : Jacobian between reference frame (or fix frame) and end
+ effector frame (on tilt axis).
*/
-void
-vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+void vpBiclops::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for biclops articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for biclops articular vector"));
}
- fJe.resize(6,2) ;
+ fJe.resize(6, 2);
- double s1 = sin(q[0]) ;
- double c1 = cos(q[0]) ;
+ double s1 = sin(q[0]);
+ double c1 = cos(q[0]);
fJe = 0;
- if (dh_model_ == DH1)
- {
+ if (dh_model_ == DH1) {
fJe[3][1] = -s1;
fJe[4][1] = c1;
fJe[5][0] = 1;
- }
- else
- {
+ } else {
fJe[3][1] = s1;
fJe[4][1] = -c1;
fJe[5][0] = 1;
diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp
index 2be93eb..0e66f39 100644
--- a/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp
+++ b/modules/robot/src/real-robot/biclops/vpRobotBiclops.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,11 +36,11 @@
*
*****************************************************************************/
-#include <signal.h>
+#include <cmath> // std::fabs
#include <errno.h>
+#include <limits> // numeric_limits
+#include <signal.h>
#include <string.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
#include <visp3/core/vpTime.h>
@@ -47,11 +48,11 @@
#ifdef VISP_HAVE_BICLOPS
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/robot/vpBiclops.h>
#include <visp3/robot/vpRobotBiclops.h>
#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpExponentialMap.h>
-#include <visp3/core/vpIoTools.h>
//#define VP_DEBUG // Activate the debug mode
//#define VP_DEBUG_MODE 10 // Activate debug level 1 and 2
@@ -72,15 +73,14 @@ static pthread_mutex_t vpMeasure_mutex;
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
-
/*!
Default constructor.
- Does nothing more than setting the default configuration file
+ Does nothing more than setting the default configuration file
to /usr/share/BiclopsDefault.cfg.
- As shown in the following example,the turret need to be initialized
+ As shown in the following example,the turret need to be initialized
using init() function.
\code
@@ -90,11 +90,12 @@ static pthread_mutex_t vpMeasure_mutex;
int main()
{
#ifdef VISP_HAVE_BICLOPS
- vpRobotBiclops robot; // Use the default config file in /usr/share/BiclopsDefault.cfg"
+ vpRobotBiclops robot; // Use the default config file in
+/usr/share/BiclopsDefault.cfg"
// Specify the config file location
robot.setConfigFile("/usr/share/BiclopsDefault.cfg"); // Not mandatory since the file is the default one
-
+
// Initialize the head
robot.init();
@@ -110,19 +111,19 @@ int main()
\endcode
*/
-vpRobotBiclops::vpRobotBiclops ()
- : vpBiclops(), vpRobot(), control_thread(), controller(),
- positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
+vpRobotBiclops::vpRobotBiclops()
+ : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
+ q_previous(), controlThreadCreated(false)
{
- vpDEBUG_TRACE (12, "Begin default constructor.");
+ vpDEBUG_TRACE(12, "Begin default constructor.");
vpRobotBiclops::robotAlreadyCreated = false;
setConfigFile("/usr/share/BiclopsDefault.cfg");
// Initialize the mutex dedicated to she shm protection
- pthread_mutex_init (&vpShm_mutex, NULL);
- pthread_mutex_init (&vpEndThread_mutex, NULL);
- pthread_mutex_init (&vpMeasure_mutex, NULL);
+ pthread_mutex_init(&vpShm_mutex, NULL);
+ pthread_mutex_init(&vpEndThread_mutex, NULL);
+ pthread_mutex_init(&vpMeasure_mutex, NULL);
control_thread = 0;
}
@@ -145,7 +146,7 @@ int main()
{
#ifdef VISP_HAVE_BICLOPS
// Specify the config file location and initialize the turret
- vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg");
+ vpRobotBiclops robot("/usr/share/BiclopsDefault.cfg");
// Move the robot to a specified pan and tilt
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
@@ -160,23 +161,23 @@ int main()
\endcode
*/
-vpRobotBiclops::vpRobotBiclops (const std::string &filename)
- : vpBiclops(), vpRobot(), control_thread(), controller(),
- positioningVelocity(defaultPositioningVelocity), q_previous(), controlThreadCreated(false)
+vpRobotBiclops::vpRobotBiclops(const std::string &filename)
+ : vpBiclops(), vpRobot(), control_thread(), controller(), positioningVelocity(defaultPositioningVelocity),
+ q_previous(), controlThreadCreated(false)
{
- vpDEBUG_TRACE (12, "Begin default constructor.");
+ vpDEBUG_TRACE(12, "Begin default constructor.");
vpRobotBiclops::robotAlreadyCreated = false;
setConfigFile(filename);
// Initialize the mutex dedicated to she shm protection
- pthread_mutex_init (&vpShm_mutex, NULL);
- pthread_mutex_init (&vpEndThread_mutex, NULL);
- pthread_mutex_init (&vpMeasure_mutex, NULL);
+ pthread_mutex_init(&vpShm_mutex, NULL);
+ pthread_mutex_init(&vpEndThread_mutex, NULL);
+ pthread_mutex_init(&vpMeasure_mutex, NULL);
init();
- return ;
+ return;
}
/*!
@@ -186,31 +187,29 @@ vpRobotBiclops::vpRobotBiclops (const std::string &filename)
*/
-vpRobotBiclops::~vpRobotBiclops (void)
+vpRobotBiclops::~vpRobotBiclops(void)
{
vpDEBUG_TRACE(12, "Start vpRobotBiclops::~vpRobotBiclops()");
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
- vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+ vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
/* wait the end of the control thread */
- vpDEBUG_TRACE (12, "Wait end of control thread");
+ vpDEBUG_TRACE(12, "Wait end of control thread");
if (controlThreadCreated == true) {
int code = pthread_join(control_thread, NULL);
if (code != 0) {
- vpCERROR << "Cannot terminate the control thread: " << code
- << " strErr=" << strerror(errno)
- << " strCode=" << strerror(code)
- << std::endl;
+ vpCERROR << "Cannot terminate the control thread: " << code << " strErr=" << strerror(errno)
+ << " strCode=" << strerror(code) << std::endl;
}
}
- pthread_mutex_destroy (&vpShm_mutex);
- pthread_mutex_destroy (&vpEndThread_mutex);
- pthread_mutex_destroy (&vpMeasure_mutex);
+ pthread_mutex_destroy(&vpShm_mutex);
+ pthread_mutex_destroy(&vpEndThread_mutex);
+ pthread_mutex_destroy(&vpMeasure_mutex);
vpRobotBiclops::robotAlreadyCreated = false;
@@ -218,21 +217,19 @@ vpRobotBiclops::~vpRobotBiclops (void)
return;
}
-
-/* ------------------------------------------------------------------------- */
-/* --- INITIALISATION ------------------------------------------------------ */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- INITIALISATION ------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
Set the Biclops config filename.
*/
-void
-vpRobotBiclops::setConfigFile(const std::string &filename)
-{
- this->configfile = filename;
-}
+void vpRobotBiclops::setConfigFile(const std::string &filename) { this->configfile = filename; }
/*!
@@ -243,29 +240,24 @@ vpRobotBiclops::setConfigFile(const std::string &filename)
oppened.
*/
-void
-vpRobotBiclops::init ()
+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");
+ throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
}
fclose(fd);
// Initialize the controller
controller.init(configfile);
- try
- {
- setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
vpRobotBiclops::robotAlreadyCreated = true;
@@ -276,7 +268,7 @@ vpRobotBiclops::init ()
controlThreadCreated = false;
- return ;
+ return;
}
/*
@@ -286,24 +278,24 @@ vpRobotBiclops::init ()
ms joint limits during the speed control. If a joint limit is detected the
axis should be halted.
- \warning Velocity control mode is not exported from the top-level Biclops API
- class provided by Traclabs. That means that there is no protection in this
- mode to prevent an axis from striking its hard limit. In position mode,
+ \warning Velocity control mode is not exported from the top-level Biclops
+ API class provided by Traclabs. That means that there is no protection in
+ this mode to prevent an axis from striking its hard limit. In position mode,
Traclabs put soft limits in that keep any command from driving to a position
- too close to the hard limits. In velocity mode this protection does not exist
- in the current API.
+ too close to the hard limits. In velocity mode this protection does not
+ exist in the current API.
\warning With the understanding that hitting the hard limits at full
speed/power can damage the unit, damage due to velocity mode commanding is
under user responsibility.
*/
-void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
+void *vpRobotBiclops::vpRobotBiclopsSpeedControlLoop(void *arg)
{
- vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
+ vpRobotBiclopsController *controller = static_cast<vpRobotBiclopsController *>(arg);
int iter = 0;
-// PMDAxisControl *panAxis = controller->getPanAxis();
-// PMDAxisControl *tiltAxis = controller->getTiltAxis();
+ // PMDAxisControl *panAxis = controller->getPanAxis();
+ // PMDAxisControl *tiltAxis = controller->getTiltAxis();
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE(10, "Start control loop");
@@ -311,121 +303,107 @@ void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
vpColVector mes_q_dot;
vpColVector softLimit(vpBiclops::ndof);
vpColVector q_dot(vpBiclops::ndof);
- bool *new_q_dot = new bool [ vpBiclops::ndof ];
- bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
- bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
- bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
- vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
- double secure = vpMath::rad(2); // add a security angle before joint limit
-
+ bool *new_q_dot = new bool[vpBiclops::ndof];
+ bool *change_dir = new bool[vpBiclops::ndof]; // change of direction
+ bool *force_halt = new bool[vpBiclops::ndof]; // force an axis to halt
+ bool *enable_limit = new bool[vpBiclops::ndof]; // enable soft limit
+ vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
+ double secure = vpMath::rad(2); // add a security angle before joint limit
// Set the soft limits
- softLimit[0] = vpBiclops::panJointLimit - secure;
+ softLimit[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");
+ vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- prev_q_dot [i] = shm.q_dot[i];
- new_q_dot [i] = false;
- change_dir [i] = false;
- force_halt [i] = false;
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
+ prev_q_dot[i] = shm.q_dot[i];
+ new_q_dot[i] = false;
+ change_dir[i] = false;
+ force_halt[i] = false;
enable_limit[i] = true;
}
// Initialize actual position and velocity
- mes_q = controller->getActualPosition();
+ mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
- vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
+ vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
// Updates the shm
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- shm.actual_q[i] = mes_q[i];
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
+ shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
// Update the actuals positions
controller->writeShm(shm);
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
- vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
+ vpDEBUG_TRACE(11, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
- while (! controller->isStopRequested()) {
+ while (!controller->isStopRequested()) {
// Get actual position and velocity
- mes_q = controller->getActualPosition();
+ mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
- vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
+ vpDEBUG_TRACE(11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
-
shm = controller->readShm();
// Updates the shm
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
- shm.actual_q[i] = mes_q[i];
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
+ shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
- vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
- vpMath::deg(mes_q[0]),
- vpMath::deg(mes_q[1]));
- vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
- vpMath::deg(mes_q_dot[0]),
- vpMath::deg(mes_q_dot[1]));
- vpDEBUG_TRACE(12, "desired q_dot : %f %f",
- vpMath::deg(shm.q_dot[0]),
- vpMath::deg(shm.q_dot[1]));
- vpDEBUG_TRACE(13, "previous q_dot : %f %f",
- vpMath::deg(prev_q_dot[0]),
- vpMath::deg(prev_q_dot[1]));
-
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ vpDEBUG_TRACE(12, "mes pan: %f tilt: %f", vpMath::deg(mes_q[0]), vpMath::deg(mes_q[1]));
+ vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f", vpMath::deg(mes_q_dot[0]), vpMath::deg(mes_q_dot[1]));
+ vpDEBUG_TRACE(12, "desired q_dot : %f %f", vpMath::deg(shm.q_dot[0]), vpMath::deg(shm.q_dot[1]));
+ vpDEBUG_TRACE(13, "previous q_dot : %f %f", vpMath::deg(prev_q_dot[0]), vpMath::deg(prev_q_dot[1]));
+
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
// test if joint limits are reached
if (mes_q[i] < -softLimit[i]) {
- vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
- shm.status[i] = vpRobotBiclopsController::STOP;
- shm.jointLimit[i] = true;
- }
- else if (mes_q[i] > softLimit[i]) {
- vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
- shm.status[i] = vpRobotBiclopsController::STOP;
- shm.jointLimit[i] = true;
- }
- else {
- shm.status[i] = vpRobotBiclopsController::SPEED;
- shm.jointLimit[i] = false;
+ vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
+ shm.status[i] = vpRobotBiclopsController::STOP;
+ shm.jointLimit[i] = true;
+ } else if (mes_q[i] > softLimit[i]) {
+ vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
+ shm.status[i] = vpRobotBiclopsController::STOP;
+ shm.jointLimit[i] = true;
+ } else {
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ shm.jointLimit[i] = false;
}
// Test if new a speed is demanded
- //if (shm.q_dot[i] != prev_q_dot[i])
- if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
- new_q_dot[i] = true;
+ // if (shm.q_dot[i] != prev_q_dot[i])
+ if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) >
+ std::fabs(vpMath::maximum(shm.q_dot[i], prev_q_dot[i])) * std::numeric_limits<double>::epsilon())
+ new_q_dot[i] = true;
else
- new_q_dot[i] = false;
+ new_q_dot[i] = false;
// Test if desired speed change of sign
if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
- change_dir[i] = true;
+ change_dir[i] = true;
else
- change_dir[i] = false;
-
+ 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]);
@@ -434,258 +412,227 @@ void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
-
bool updateVelocity = false;
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
// Test if a new desired speed is to apply
if (new_q_dot[i]) {
- // A new desired speed is to apply
- if (shm.status[i] == vpRobotBiclopsController::STOP) {
- // Axis in joint limit
- if (change_dir[i] == false) {
- // New desired speed without change of direction
- // We go in the joint limit
- if (enable_limit[i] == true) { // limit detection active
- // We have to stop this axis
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // We have to apply the desired speed to go away the joint
- // Update the desired speed
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- force_halt[i] = false;
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // New desired speed and change of direction.
- if (enable_limit[i] == true) { // limit detection active
- // Update the desired speed to go away the joint limit
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- force_halt[i] = false;
- enable_limit[i] = false; // Disable joint limit detection
- updateVelocity = true; // We have to send this new speed
- }
- else {
- // We have to stop this axis
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- enable_limit[i] = true; // Joint limit detection must be active
- updateVelocity = true; // We have to send this new speed
- }
- }
- }
- }
- else {
- // Axis not in joint limit
-
- // Update the desired speed
- q_dot[i] = shm.q_dot[i];
- shm.status[i] = vpRobotBiclopsController::SPEED;
- enable_limit[i] = true; // Joint limit detection must be active
- updateVelocity = true; // We have to send this new speed
- }
- }
- else {
- // No change of the desired speed. We have to stop the robot in case of
- // joint limit
- if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
- if (enable_limit[i] == true) { // limit detection active
-
- // Test if this axis was stopped before
- if (force_halt[i] == false) {
- // We have to stop this axis
- q_dot[i] = 0.;
- force_halt[i] = true; // indicate that it will be stopped
- updateVelocity = true; // We have to send this new speed
- }
- }
- }
- else {
- // No need to stop the robot
- enable_limit[i] = true; // Normal situation, activate limit detection
- }
+ // A new desired speed is to apply
+ if (shm.status[i] == vpRobotBiclopsController::STOP) {
+ // Axis in joint limit
+ if (change_dir[i] == false) {
+ // New desired speed without change of direction
+ // We go in the joint limit
+ if (enable_limit[i] == true) { // limit detection active
+ // We have to stop this axis
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ updateVelocity = true; // We have to send this new speed
+ }
+ } else {
+ // We have to apply the desired speed to go away the joint
+ // Update the desired speed
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ force_halt[i] = false;
+ updateVelocity = true; // We have to send this new speed
+ }
+ } else {
+ // New desired speed and change of direction.
+ if (enable_limit[i] == true) { // limit detection active
+ // Update the desired speed to go away the joint limit
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ force_halt[i] = false;
+ enable_limit[i] = false; // Disable joint limit detection
+ updateVelocity = true; // We have to send this new speed
+ } else {
+ // We have to stop this axis
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ enable_limit[i] = true; // Joint limit detection must be active
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ }
+ } else {
+ // Axis not in joint limit
+
+ // Update the desired speed
+ q_dot[i] = shm.q_dot[i];
+ shm.status[i] = vpRobotBiclopsController::SPEED;
+ enable_limit[i] = true; // Joint limit detection must be active
+ updateVelocity = true; // We have to send this new speed
+ }
+ } else {
+ // No change of the desired speed. We have to stop the robot in case
+ // of joint limit
+ if (shm.status[i] == vpRobotBiclopsController::STOP) { // axis limit
+ if (enable_limit[i] == true) { // limit detection active
+
+ // Test if this axis was stopped before
+ if (force_halt[i] == false) {
+ // We have to stop this axis
+ q_dot[i] = 0.;
+ force_halt[i] = true; // indicate that it will be stopped
+ updateVelocity = true; // We have to send this new speed
+ }
+ }
+ } else {
+ // No need to stop the robot
+ enable_limit[i] = true; // Normal situation, activate limit detection
+ }
}
}
// Update the actuals positions
controller->writeShm(shm);
- vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
if (updateVelocity) {
- vpDEBUG_TRACE(12, "apply q_dot : %f %f",
- vpMath::deg(q_dot[0]),
- vpMath::deg(q_dot[1]));
+ 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 );
+ controller->setVelocity(q_dot);
}
-
// Update the previous speed for next iteration
- for (unsigned int i=0; i < vpBiclops::ndof; i ++)
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++)
prev_q_dot[i] = shm.q_dot[i];
vpDEBUG_TRACE(12, "iter: %d", iter);
- //wait 5 ms
+ // wait 5 ms
vpTime::wait(5.0);
-// if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
-// vpDEBUG_TRACE (12, "Calling thread will end");
-// vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
-// std::cout << "Calling thread will end" << std::endl;
-// std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
-//
-// pthread_mutex_unlock(&vpEndThread_mutex);
-// break;
-// }
-
- iter ++;
+ // if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
+ // vpDEBUG_TRACE (12, "Calling thread will end");
+ // vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+ // std::cout << "Calling thread will end" << std::endl;
+ // std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
+ //
+ // pthread_mutex_unlock(&vpEndThread_mutex);
+ // break;
+ // }
+
+ iter++;
}
controller->stopRequest(false);
// Stop the robot
vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
q_dot = 0;
- controller -> setVelocity( q_dot );
+ controller->setVelocity(q_dot);
- delete [] new_q_dot;
- delete [] change_dir;
- delete [] force_halt;
- delete [] enable_limit;
+ delete[] new_q_dot;
+ delete[] change_dir;
+ delete[] force_halt;
+ delete[] enable_limit;
vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
- vpDEBUG_TRACE (10, "Exit control thread ");
+ vpDEBUG_TRACE(10, "Exit control thread ");
// pthread_exit(0);
return NULL;
}
-
/*!
Change the state of the robot either to stop them, or to set position or
speed control.
*/
-vpRobot::vpRobotStateType
-vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
+vpRobot::vpRobotStateType vpRobotBiclops::setRobotState(vpRobot::vpRobotStateType newState)
{
- switch (newState)
- {
- case vpRobot::STATE_STOP:
- {
- if (vpRobot::STATE_STOP != getRobotState ())
- {
- stopMotion();
- }
- break;
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ if (vpRobot::STATE_STOP != getRobotState()) {
+ stopMotion();
}
- case vpRobot::STATE_POSITION_CONTROL:
- {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
- {
- vpDEBUG_TRACE (12, "Speed to position control.");
- stopMotion();
- }
-
- break;
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
+ vpDEBUG_TRACE(12, "Speed to position control.");
+ stopMotion();
}
- case vpRobot::STATE_VELOCITY_CONTROL:
- {
-
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
- pthread_mutex_lock(&vpEndThread_mutex);
-
- vpDEBUG_TRACE (12, "Create speed control thread");
- int code;
- code = pthread_create(&control_thread, NULL,
- &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop,
- &controller);
- if (code != 0) {
- vpCERROR << "Cannot create speed biclops control thread: " << code
- << " strErr=" << strerror(errno)
- << " strCode=" << strerror(code)
- << std::endl;
- }
-
- controlThreadCreated = true;
-
- vpDEBUG_TRACE (12, "Speed control thread created");
+
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
+ pthread_mutex_lock(&vpEndThread_mutex);
+
+ vpDEBUG_TRACE(12, "Create speed control thread");
+ int code;
+ code = pthread_create(&control_thread, NULL, &vpRobotBiclops::vpRobotBiclopsSpeedControlLoop, &controller);
+ if (code != 0) {
+ vpCERROR << "Cannot create speed biclops control thread: " << code << " strErr=" << strerror(errno)
+ << " strCode=" << strerror(code) << std::endl;
}
- break;
+
+ controlThreadCreated = true;
+
+ vpDEBUG_TRACE(12, "Speed control thread created");
}
+ break;
+ }
default:
- break ;
+ break;
}
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
-
-
/*!
Halt all the axis.
*/
-void
-vpRobotBiclops::stopMotion(void)
+void vpRobotBiclops::stopMotion(void)
{
vpColVector q_dot(vpBiclops::ndof);
q_dot = 0;
controller.setVelocity(q_dot);
- //std::cout << "Request to stop the velocity controller thread...."<< std::endl;
+ // std::cout << "Request to stop the velocity controller thread...."<<
+ // std::endl;
controller.stopRequest(true);
}
/*!
Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cVe : Twist transformation between camera and end effector frame to
expess a velocity skew from end effector frame in camera frame.
*/
-void
-vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotBiclops::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- cMe = vpBiclops::get_cMe() ;
+ vpHomogeneousMatrix cMe;
+ cMe = vpBiclops::get_cMe();
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cMe : Homogeneous matrix between camera and end effector frame.
*/
-void
-vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- cMe = vpBiclops::get_cMe() ;
-}
-
+void vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const { cMe = vpBiclops::get_cMe(); }
/*!
Get the robot jacobian expressed in the end-effector frame.
@@ -697,46 +644,37 @@ vpRobotBiclops::get_cMe(vpHomogeneousMatrix &cMe) const
tilt axis).
*/
-void
-vpRobotBiclops::get_eJe(vpMatrix &_eJe)
+void vpRobotBiclops::get_eJe(vpMatrix &_eJe)
{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ vpColVector q(2);
+ getPosition(vpRobot::ARTICULAR_FRAME, q);
- try
- {
- vpBiclops::get_eJe(q, _eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ try {
+ vpBiclops::get_eJe(q, _eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
/*!
Get the robot jacobian expressed in the robot reference frame
- \param _fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
+ \param _fJe : Jacobian between reference frame (or fix frame) and end
+ effector frame (on tilt axis).
*/
-void
-vpRobotBiclops::get_fJe(vpMatrix &_fJe)
+void vpRobotBiclops::get_fJe(vpMatrix &_fJe)
{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
+ vpColVector q(2);
+ getPosition(vpRobot::ARTICULAR_FRAME, q);
- try
- {
- vpBiclops::get_fJe(q,_fJe) ;
- }
- catch(...)
- {
+ try {
+ vpBiclops::get_fJe(q, _fJe);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
-
}
/*!
@@ -746,13 +684,11 @@ vpRobotBiclops::get_fJe(vpMatrix &_fJe)
\param velocity : Velocity in % of the maximum velocity between [0,100]. The
maximum velocity is given vpBiclops::speedLimit.
*/
-void
-vpRobotBiclops::setPositioningVelocity (const double velocity)
+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;
@@ -764,12 +700,7 @@ 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.
@@ -782,63 +713,56 @@ vpRobotBiclops::getPositioningVelocity (void)
\param q : The position to set for each axis in radians.
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
+ \exception vpRobotException::wrongStateError : If a not supported frame
+ 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 ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
- switch(frame)
- {
+ switch (frame) {
case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot move the robot in camera frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in camera frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in camera frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
+ "not implemented");
break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot move the robot in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in reference frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in reference frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
+ "not implemented");
break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot move the robot in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in mixt frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
+ "not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
- break ;
+ break;
}
// test if position reachable
-// if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
-// (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
-// vpERROR_TRACE ("Positionning error.");
-// throw vpRobotException (vpRobotException::wrongStateError,
-// "Positionning error.");
-// }
-
- vpDEBUG_TRACE (12, "Lock mutex vpEndThread_mutex");
+ // if ( (fabs(q[0]) > vpBiclops::panJointLimit) ||
+ // (fabs(q[1]) > vpBiclops::tiltJointLimit) ) {
+ // vpERROR_TRACE ("Positionning error.");
+ // throw vpRobotException (vpRobotException::wrongStateError,
+ // "Positionning error.");
+ // }
+
+ vpDEBUG_TRACE(12, "Lock mutex vpEndThread_mutex");
pthread_mutex_lock(&vpEndThread_mutex);
- controller.setPosition( q, positioningVelocity );
- vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
+ controller.setPosition(q, positioningVelocity);
+ vpDEBUG_TRACE(12, "Unlock mutex vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
- return ;
+ return;
}
/*!
@@ -853,24 +777,21 @@ vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
\param q1 : The pan position to set in radians.
\param q2 : The tilt position to set in radians.
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
+ \exception vpRobotException::wrongStateError : If a not supported 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) ;
- q[0] = q1 ;
- q[1] = q2 ;
+ try {
+ vpColVector q(2);
+ q[0] = q1;
+ q[1] = q2;
- setPosition(frame,q) ;
- }
- catch(...)
- {
+ setPosition(frame, q);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -887,16 +808,14 @@ void vpRobotBiclops::setPosition (const vpRobot::vpControlFrameType frame,
\sa readPositionFile()
*/
-void
-vpRobotBiclops::setPosition(const char *filename)
+void vpRobotBiclops::setPosition(const char *filename)
{
- vpColVector q ;
+ 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");
+ vpERROR_TRACE("Cannot get biclops position from file");
+ throw vpRobotException(vpRobotException::readingParametersError, "Cannot get biclops position from file");
}
- setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
@@ -914,34 +833,28 @@ 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");
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE("Cannot get position in camera frame: not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
+ "not implemented");
break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in reference frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
+ "not implemented");
break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
+ "not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
- break ;
+ break;
}
vpRobot::vpRobotStateType state;
@@ -958,42 +871,40 @@ vpRobotBiclops::getPosition (const vpRobot::vpControlFrameType frame,
default:
q.resize(vpBiclops::ndof);
- vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
+ vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
vpRobotBiclopsController::shmType shm;
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
q[i] = shm.actual_q[i];
}
vpCDEBUG(11) << "++++++++ Measure actuals: " << q.t();
- vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
+ vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
break;
-
}
}
-
/*!
Send a velocity on each axis.
\param frame : Control frame. This biclops head can only be controlled in
articular. Be aware, the camera frame (vpRobot::CAMERA_FRAME), the reference
- frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME) are
- not implemented.
+ frame (vpRobot::REFERENCE_FRAME) and the mixt frame (vpRobot::MIXT_FRAME)
+ are not implemented.
\param q_dot : The desired articular velocity of the axis in rad/s. \f$ \dot
{r} = [\dot{q}_1, \dot{q}_2]^t \f$ with \f$ \dot{q}_1 \f$ the pan of the
@@ -1012,71 +923,57 @@ 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) ");
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
}
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the camera frame:"
- "functionality not implemented");
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- if ( q_dot.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in articular frame");
- }
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- }
- 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");
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
+ "in the camera frame:"
+ "functionality not implemented");
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ if (q_dot.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
+ throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
+ "in articular frame");
}
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ }
+ 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");
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
+ }
}
- vpDEBUG_TRACE (12, "Velocity limitation.");
+ vpDEBUG_TRACE(12, "Velocity limitation.");
bool norm = false; // Flag to indicate when velocities need to be nomalized
// Saturate articular speed
@@ -1086,14 +983,14 @@ vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
// init q_dot_saturated
q_dot_sat = q_dot;
- for (unsigned int i = 0 ; i < vpBiclops::ndof; ++ i) // q1 and q2
+ for (unsigned int i = 0; i < vpBiclops::ndof; ++i) // q1 and q2
{
- if (fabs (q_dot[i]) > max)
- {
+ if (fabs(q_dot[i]) > max) {
norm = true;
- max = fabs (q_dot[i]);
- vpERROR_TRACE ("Excess velocity: ROTATION "
- "(axe nr.%d).", i);
+ max = fabs(q_dot[i]);
+ vpERROR_TRACE("Excess velocity: ROTATION "
+ "(axe nr.%d).",
+ i);
}
}
// Rotations velocities normalisation
@@ -1106,27 +1003,28 @@ vpRobotBiclops::setVelocity (const vpRobot::vpControlFrameType frame,
vpRobotBiclopsController::shmType shm;
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
- for (unsigned int i=0; i < vpBiclops::ndof; i ++)
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++)
shm.q_dot[i] = q_dot[i];
controller.writeShm(shm);
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
return;
}
-
-/* ------------------------------------------------------------------------- */
-/* --- GET ----------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- GET -----------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -1139,34 +1037,28 @@ 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");
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE("Cannot get position in camera frame: not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in camera frame: "
+ "not implemented");
break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in reference frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in reference frame: "
+ "not implemented");
break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get position in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get position in mixt frame: "
+ "not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
- break ;
+ break;
}
vpRobot::vpRobotStateType state;
@@ -1183,34 +1075,32 @@ vpRobotBiclops::getVelocity (const vpRobot::vpControlFrameType frame,
default:
q_dot.resize(vpBiclops::ndof);
- vpDEBUG_TRACE (12, "Lock mutex vpMeasure_mutex");
+ vpDEBUG_TRACE(12, "Lock mutex vpMeasure_mutex");
pthread_mutex_lock(&vpMeasure_mutex); // Wait until a position is available
vpRobotBiclopsController::shmType shm;
- vpDEBUG_TRACE (12, "Lock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller.readShm();
- vpDEBUG_TRACE (12, "unlock mutex vpShm_mutex");
+ vpDEBUG_TRACE(12, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
q_dot[i] = shm.actual_q_dot[i];
}
vpCDEBUG(11) << "++++++++ Velocity actuals: " << q_dot.t();
- vpDEBUG_TRACE (12, "unlock mutex vpMeasure_mutex");
+ vpDEBUG_TRACE(12, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
break;
-
}
}
-
/*!
Return the articular velocity.
@@ -1222,11 +1112,10 @@ vpRobotBiclops::getVelocity (const vpRobot::vpControlFrameType frame,
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
-vpColVector
-vpRobotBiclops::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotBiclops::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector q_dot;
- getVelocity (frame, q_dot);
+ getVelocity(frame, q_dot);
return q_dot;
}
@@ -1250,12 +1139,11 @@ 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);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -1267,29 +1155,29 @@ vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
q.resize(vpBiclops::ndof);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Biclops position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < vpBiclops::ndof+1) // try to split with tab separator
+ 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)
+ if (chain.size() < vpBiclops::ndof + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< vpBiclops::ndof; i++)
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++)
ss >> q[i];
pos_found = true;
break;
@@ -1311,40 +1199,6 @@ vpRobotBiclops::readPositionFile(const std::string &filename, vpColVector &q)
/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param d The measured displacement in camera frame. The dimension of d is 6
- (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
- radians.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotBiclops::getCameraDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, d);
-
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param d The measured articular displacement. The dimension of d is 2 (the
- number of axis of the robot) with respectively d[0] (pan displacement),
- d[1] (tilt displacement)
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, d);
-}
-
-/*!
-
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
@@ -1361,31 +1215,27 @@ void vpRobotBiclops::getArticularDisplacement(vpColVector &d)
Translations are expressed in meters, rotations in radians with the theta U
representation.
- \exception vpRobotException::wrongStateError If a not supported frame type is
- given.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
+ \exception vpRobotException::wrongStateError If a not supported frame type
+ is given.
*/
-void
-vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &d)
+void vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d)
{
vpColVector q_current; // current position
getPosition(vpRobot::ARTICULAR_FRAME, q_current);
- switch(frame) {
+ switch (frame) {
case vpRobot::ARTICULAR_FRAME:
d.resize(vpBiclops::ndof);
d = q_current - q_previous;
- break ;
+ break;
case vpRobot::CAMERA_FRAME: {
d.resize(6);
vpHomogeneousMatrix fMc_current;
vpHomogeneousMatrix fMc_previous;
- fMc_current = vpBiclops::get_fMc(q_current);
+ fMc_current = vpBiclops::get_fMc(q_current);
fMc_previous = vpBiclops::get_fMc(q_previous);
vpHomogeneousMatrix c_previousMc_current;
// fMc_c = fMc_p * c_pMc_c
@@ -1393,33 +1243,29 @@ vpRobotBiclops::getDisplacement(vpRobot::vpControlFrameType frame,
c_previousMc_current = fMc_previous.inverse() * fMc_current;
// Compute the instantaneous velocity from this homogeneous matrix.
- d = vpExponentialMap::inverse( c_previousMc_current );
- break ;
+ d = vpExponentialMap::inverse(c_previousMc_current);
+ break;
}
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the reference frame:"
- "functionality not implemented");
- break ;
+ vpERROR_TRACE("Cannot get a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
+ "functionality not implemented");
+ break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the mixt frame:"
- "functionality not implemented");
- break ;
+ vpERROR_TRACE("Cannot get a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the mixt frame:"
+ "functionality not implemented");
+ break;
}
-
q_previous = q_current; // Update for next call of this method
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no symbols
-void dummy_vpRobotBiclops() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclops.cpp.o) has no
+// symbols
+void dummy_vpRobotBiclops(){};
#endif
-
diff --git a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp
index f42870a..1d0cbf9 100644
--- a/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp
+++ b/modules/robot/src/real-robot/biclops/vpRobotBiclopsController.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,7 +39,7 @@
#include <signal.h>
#include <string.h>
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
-# include <unistd.h>
+#include <unistd.h>
#endif
#include <visp3/core/vpConfig.h>
@@ -55,8 +56,6 @@
#include <visp3/core/vpDebug.h>
-
-
/* ----------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
@@ -65,13 +64,11 @@
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.
-
+ axisMask = Biclops::PanMask + Biclops::TiltMask
+ /*+ Biclops::VergeMask*/; // add this if you want verge.
// Set Debug level depending on how much info you want to see about
// the inner workings of the API. Level 2 is highest with 0 being
@@ -79,7 +76,7 @@ vpRobotBiclopsController::vpRobotBiclopsController()
biclops.SetDebugLevel(0);
// initialize the shared data structure
- for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
shm.status[i] = STOP;
shm.q_dot[i] = 0.;
shm.actual_q[i] = 0.;
@@ -93,9 +90,7 @@ vpRobotBiclopsController::vpRobotBiclopsController()
Destructor.
*/
-vpRobotBiclopsController::~vpRobotBiclopsController()
-{
-}
+vpRobotBiclopsController::~vpRobotBiclopsController() {}
/*!
@@ -110,12 +105,11 @@ vpRobotBiclopsController::~vpRobotBiclopsController()
- if you try to open a bad serial port. Check you config file to verify
which is the used serial port.
*/
-void
-vpRobotBiclopsController::init(const std::string &configfile)
+void vpRobotBiclopsController::init(const std::string &configfile)
{
- vpDEBUG_TRACE (12, "Initialize biclops.");
+ vpDEBUG_TRACE(12, "Initialize biclops.");
bool binit = false;
- for (int i=0; i<1; i++) {
+ for (int i = 0; i < 1; i++) {
try {
std::cout << "Try to initialize biclops head " << std::endl;
binit = biclops.Initialize(configfile.c_str());
@@ -124,30 +118,24 @@ vpRobotBiclopsController::init(const std::string &configfile)
// Initialization completed successfully. Close the config file.
std::cout << "Initialization succeed...\n";
break;
- }
- else {
+ } else {
std::cout << "Initialization failed...\n";
}
- }
- catch(...)
- {
- std::cout << "Initialization failed..."<< std::endl;
+ } catch (...) {
+ std::cout << "Initialization failed..." << std::endl;
}
}
- if (! binit) {
+ if (!binit) {
std::cout << "Cannot initialize biclops head. " << std::endl;
std::cout << "Check if the serial cable is connected." << std::endl;
std::cout << "Check if the robot is powered on." << std::endl;
std::cout << "Check if you try to open the good serial port." << std::endl;
std::cout << "Try to power off/on and restart..." << std::endl;
- throw vpRobotException (vpRobotException::notInitializedError,
- "Cannot initialize biclops head.");
-
+ throw vpRobotException(vpRobotException::notInitializedError, "Cannot initialize biclops head.");
}
-
vpDEBUG_TRACE(12, "Biclops initialized");
// Get shortcut references to each axis.
@@ -157,24 +145,23 @@ vpRobotBiclopsController::init(const std::string &configfile)
vergeAxis = biclops.GetAxis(Biclops::Verge);
#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
- if (!panAxis -> GetHomedState() || !tiltAxis -> GetHomedState()) {
+ if (!panAxis->GetHomedState() || !tiltAxis->GetHomedState()) {
vpDEBUG_TRACE(12, "Biclops is not homed");
}
#else // old API
- if (!panAxis -> IsAlreadyHomed() || !tiltAxis -> IsAlreadyHomed()) {
+ if (!panAxis->IsAlreadyHomed() || !tiltAxis->IsAlreadyHomed()) {
vpDEBUG_TRACE(12, "Biclops is not homed");
}
#endif
- //Execute the homing sequence for all axes.
+ // Execute the homing sequence for all axes.
vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
vpDEBUG_TRACE(12, "Execute the homing sequence for all axes");
- if ( biclops.HomeAxes(axisMask))
+ if (biclops.HomeAxes(axisMask))
vpDEBUG_TRACE(12, "Homing sequence succeed.");
else {
vpERROR_TRACE("Homing sequence failed. Program is stopped");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connection with biclops");
+ throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with biclops");
}
// Get the currently defined (default) motion profiles.
@@ -200,28 +187,24 @@ 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.");
+ if (q.getRows() != vpBiclops::ndof) {
+ vpERROR_TRACE("Bad dimension for positioning vector.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for positioning vector.");
}
- panAxis -> SetProfileMode(PMDTrapezoidalProfile);
- tiltAxis -> SetProfileMode(PMDTrapezoidalProfile);
+ panAxis->SetProfileMode(PMDTrapezoidalProfile);
+ tiltAxis->SetProfileMode(PMDTrapezoidalProfile);
// Create the list of axes we want to coordinate
PMDUtils::AxisList axisList;
axisList.push_back(panAxis);
axisList.push_back(tiltAxis);
-#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
+#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
// Get the currently defined (default) motion profiles.
- //PMDAxisControl::Profile panProfile,tiltProfile;
+ // PMDAxisControl::Profile panProfile,tiltProfile;
panAxis->GetProfile(panProfile);
tiltAxis->GetProfile(tiltProfile);
@@ -229,12 +212,10 @@ 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);
@@ -248,36 +229,32 @@ 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);
+ panAxis->ProfileToCounts(panProfile, desired_profile);
vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
- panAxis -> SetProfile(desired_profile);
+ panAxis->SetProfile(desired_profile);
// Set a position to move to by modifying the respective profiles.
// NOTE: profile values are in revolutions, so here we convert
// from degrees (divide by 360) for readability.
tiltProfile.pos = PMDUtils::RadsToRevs(q[1]);
- tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit
- * percentVelocity / 100.);
+ tiltProfile.vel = PMDUtils::RadsToRevs(vpBiclops::speedLimit * percentVelocity / 100.);
- tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
+ tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
vpCDEBUG(12) << "desired_profile.pos: " << desired_profile.pos << std::endl;
vpCDEBUG(12) << "desired_profile.vel: " << desired_profile.vel << std::endl;
- tiltAxis -> SetProfile(desired_profile);
+ tiltAxis->SetProfile(desired_profile);
#endif
// Coordinate motion
PMDUtils::Coordinate(axisList);
- biclops.Move(Biclops::PanMask + Biclops::TiltMask/*, 0*/); //
-
+ biclops.Move(Biclops::PanMask + Biclops::TiltMask /*, 0*/); //
}
/*!
@@ -289,39 +266,35 @@ vpRobotBiclopsController::setPosition(const vpColVector & q,
\param q_dot : Velocity to apply.
*/
-void
-vpRobotBiclopsController::setVelocity(const vpColVector & q_dot)
+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.");
+ if (q_dot.getRows() != vpBiclops::ndof) {
+ vpERROR_TRACE("Bad dimension for velocity vector.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad dimension for velocity vector.");
}
-
-#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
+#ifdef VISP_HAVE_BICLOPS_AND_GET_HOMED_STATE_FUNCTION // new API
// Get the currently defined (default) motion profiles.
- //PMDAxisControl::Profile panProfile, tiltProfile;
+ // PMDAxisControl::Profile panProfile, tiltProfile;
panAxis->GetProfile(panProfile);
tiltAxis->GetProfile(tiltProfile);
// Set a position to move to by modifying the respective profiles.
// NOTE: profile values are in revolutions, so here we convert
// from degrees (divide by 360) for readability.
- panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
+ panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
// Inform the controller of the new desired position.
panAxis->SetProfile(panProfile);
tiltAxis->SetProfile(tiltProfile);
- panAxis -> SetProfileMode(PMDVelocityContouringProfile);
- tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
+ panAxis->SetProfileMode(PMDVelocityContouringProfile);
+ tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
#else // old API
- panAxis -> SetProfileMode(PMDVelocityContouringProfile);
- tiltAxis -> SetProfileMode(PMDVelocityContouringProfile);
-\
+ panAxis->SetProfileMode(PMDVelocityContouringProfile);
+ tiltAxis->SetProfileMode(PMDVelocityContouringProfile);
+
PMDAxisControl::CountsProfile desired_profile;
// Set a position to move to by modifying the respective profiles.
@@ -329,16 +302,16 @@ vpRobotBiclopsController::setVelocity(const vpColVector & q_dot)
// from degrees (divide by 360) for readability.
panProfile.vel = PMDUtils::RadsToRevs(q_dot[0]);
- panAxis -> ProfileToCounts(panProfile, desired_profile);
- panAxis -> SetProfile(desired_profile);
+ panAxis->ProfileToCounts(panProfile, desired_profile);
+ panAxis->SetProfile(desired_profile);
// Set a position to move to by modifying the respective profiles.
// NOTE: profile values are in revolutions, so here we convert
// from degrees (divide by 360) for readability.
tiltProfile.vel = PMDUtils::RadsToRevs(q_dot[1]);
- tiltAxis -> ProfileToCounts(tiltProfile, desired_profile);
- tiltAxis -> SetProfile(desired_profile);
+ tiltAxis->ProfileToCounts(tiltProfile, desired_profile);
+ tiltAxis->SetProfile(desired_profile);
#endif
// Coordinate motion
biclops.Move(Biclops::PanMask + Biclops::TiltMask, 0); //
@@ -351,21 +324,20 @@ vpRobotBiclopsController::setVelocity(const vpColVector & q_dot)
\return The axis articular position in radians.
*/
-vpColVector
-vpRobotBiclopsController::getPosition()
+vpColVector vpRobotBiclopsController::getPosition()
{
- vpDEBUG_TRACE (12, "Start vpRobotBiclopsController::getPosition() ");
+ vpDEBUG_TRACE(12, "Start vpRobotBiclopsController::getPosition() ");
vpColVector q(vpBiclops::ndof);
PMDint32 panpos, tiltpos;
- panAxis -> GetPosition(panpos);
- tiltAxis -> GetPosition(tiltpos);
+ panAxis->GetPosition(panpos);
+ tiltAxis->GetPosition(tiltpos);
- q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
+ q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
vpCDEBUG(11) << "++++++++ Mesure : " << q.t();
- vpDEBUG_TRACE (12, "End vpRobotBiclopsController::getPosition()");
+ vpDEBUG_TRACE(12, "End vpRobotBiclopsController::getPosition()");
return q;
}
@@ -377,16 +349,15 @@ vpRobotBiclopsController::getPosition()
\return The axis actual articular position in radians.
*/
-vpColVector
-vpRobotBiclopsController::getActualPosition()
+vpColVector vpRobotBiclopsController::getActualPosition()
{
vpColVector q(vpBiclops::ndof);
PMDint32 panpos, tiltpos;
- panAxis -> GetActualPosition(panpos);
- tiltAxis -> GetActualPosition(tiltpos);
+ panAxis->GetActualPosition(panpos);
+ tiltAxis->GetActualPosition(tiltpos);
- q[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(panpos));
+ q[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(panpos));
q[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tiltpos));
return q;
@@ -399,16 +370,15 @@ vpRobotBiclopsController::getActualPosition()
\return The axis articular velocity in rad/s.
*/
-vpColVector
-vpRobotBiclopsController::getVelocity()
+vpColVector vpRobotBiclopsController::getVelocity()
{
vpColVector q_dot(vpBiclops::ndof);
PMDint32 pan_vel, tilt_vel;
- panAxis -> GetVelocity(pan_vel);
- tiltAxis -> GetVelocity(tilt_vel);
+ panAxis->GetVelocity(pan_vel);
+ tiltAxis->GetVelocity(tilt_vel);
- q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
+ q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
return q_dot;
@@ -421,16 +391,15 @@ vpRobotBiclopsController::getVelocity()
\return The axis actual articular velocity in rad/s.
*/
-vpColVector
-vpRobotBiclopsController::getActualVelocity()
+vpColVector vpRobotBiclopsController::getActualVelocity()
{
vpColVector q_dot(vpBiclops::ndof);
PMDint32 pan_vel, tilt_vel;
- panAxis -> GetActualVelocity(pan_vel);
- tiltAxis -> GetActualVelocity(tilt_vel);
+ panAxis->GetActualVelocity(pan_vel);
+ tiltAxis->GetActualVelocity(tilt_vel);
- q_dot[0] = PMDUtils::RevsToRads(panAxis ->CountsToUnits(pan_vel));
+ q_dot[0] = PMDUtils::RevsToRads(panAxis->CountsToUnits(pan_vel));
q_dot[1] = PMDUtils::RevsToRads(tiltAxis->CountsToUnits(tilt_vel));
return q_dot;
@@ -442,15 +411,14 @@ vpRobotBiclopsController::getActualVelocity()
\param shm_ : Content to write in the shared memory.
*/
-void
-vpRobotBiclopsController::writeShm(shmType &shm_)
+void vpRobotBiclopsController::writeShm(shmType &shm_)
{
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
vpDEBUG_TRACE(13, "q_dot[%d]=%f", i, shm_.q_dot[i]);
}
memcpy(&this->shm, &shm_, sizeof(shmType));
- //this->shm = shm_;
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ // this->shm = shm_;
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
}
}
@@ -461,17 +429,16 @@ vpRobotBiclopsController::writeShm(shmType &shm_)
\return A copy of the shared memory.
*/
-vpRobotBiclopsController::shmType
-vpRobotBiclopsController::readShm()
+vpRobotBiclopsController::shmType vpRobotBiclopsController::readShm()
{
shmType tmp_shm;
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
vpDEBUG_TRACE(13, "shm.q_dot[%d]=%f", i, shm.q_dot[i]);
}
memcpy(&tmp_shm, &this->shm, sizeof(shmType));
- //tmp_shm = shm;
- for(unsigned int i=0; i < vpBiclops::ndof; i ++) {
+ // tmp_shm = shm;
+ for (unsigned int i = 0; i < vpBiclops::ndof; i++) {
vpDEBUG_TRACE(13, "tmp_shm.q_dot[%d]=%f", i, tmp_shm.q_dot[i]);
}
@@ -479,9 +446,9 @@ vpRobotBiclopsController::readShm()
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotBiclopsController.cpp.o) has no symbols
-void dummy_vpRobotBiclopsController() {};
+// Work arround to avoid warning:
+// libvisp_robot.a(vpRobotBiclopsController.cpp.o) has no symbols
+void dummy_vpRobotBiclopsController(){};
#endif
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
diff --git a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp
index 27f068c..b5b8457 100644
--- a/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp
+++ b/modules/robot/src/real-robot/pioneer/vpRobotPioneer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,9 +40,9 @@
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotPioneer.h>
// Warning: vpMath.h should be included after Aria.h to avoid the build issue:
-// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
+// "/usr/include/Aria/ariaUtil.h:732:21: error: ‘isfinite’ was not declared
// in this scope"
-// This error is due to cmath header included from vpMath.h that makes
+// This error is due to cmath header included from vpMath.h that makes
// isfinite() ambiguous between ::isfinite() and std::isfinite()
#include <visp3/core/vpMath.h>
@@ -72,25 +73,30 @@ vpRobotPioneer::~vpRobotPioneer()
}
/*!
- Set the velocity (frame has to be specified) that will be applied to the robot.
-
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to control left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to control translational and
- rotational velocities are implemented.
-
- \param vel : A two dimension vector that corresponds to the velocities to apply to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s. In that case sets the velocity of the wheels
- independently.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
-
- Note that to secure the usage of the robot, velocities are saturated to the maximum allowed
- which can be obtained by getMaxTranslationVelocity() and getMaxRotationVelocity(). To change
- the default values, use setMaxTranslationVelocity() and setMaxRotationVelocity().
-
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+ Set the velocity (frame has to be specified) that will be applied to the
+ robot.
+
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME
+ to control left and right wheel velocities and vpRobot::REFERENCE_FRAME to
+ control translational and rotational velocities are implemented.
+
+ \param vel : A two dimension vector that corresponds to the velocities to
+ apply to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of
+ the left wheel and second value is the velocity of the right wheel in m/s.
+ In that case sets the velocity of the wheels independently.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation
+ velocity in m/s. Second value is the rotational velocity in rad/s along the
+ vertical axis.
+
+ Note that to secure the usage of the robot, velocities are saturated to the
+ maximum allowed which can be obtained by getMaxTranslationVelocity() and
+ getMaxRotationVelocity(). To change the default values, use
+ setMaxTranslationVelocity() and setMaxRotationVelocity().
+
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2
+ dimension vector. \exception vpRobotException::wrongStateError : If the
+ specified control frame is not supported.
*/
void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
{
@@ -105,54 +111,49 @@ void vpRobotPioneer::setVelocity(const vpRobot::vpControlFrameType frame, const
"use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
} */
- if (vel.size() != 2)
- {
+ if (vel.size() != 2) {
throw(vpRobotException(vpRobotException::dimensionError, "Velocity vector is not a 2 dimension vector"));
}
vpColVector vel_max(2);
vpColVector vel_sat;
- if (frame == vpRobot::REFERENCE_FRAME)
- {
+ if (frame == vpRobot::REFERENCE_FRAME) {
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxRotationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
- this->setVel(vel_sat[0]*1000.); // convert velocity in mm/s
- this->setRotVel( vpMath::deg(vel_sat[1]) ); // convert velocity in deg/s
+ this->setVel(vel_sat[0] * 1000.); // convert velocity in mm/s
+ this->setRotVel(vpMath::deg(vel_sat[1])); // convert velocity in deg/s
this->unlock();
- }
- else if (frame == vpRobot::ARTICULAR_FRAME)
- {
+ } else if (frame == vpRobot::ARTICULAR_FRAME) {
vel_max[0] = getMaxTranslationVelocity();
vel_max[1] = getMaxTranslationVelocity();
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
this->lock();
- //std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
- this->setVel2(vel_sat[0]*1000., vel_sat[1]*1000.); // convert velocity in mm/s
+ // std::cout << "v: " << (vel*1000).t() << " mm/s" << std::endl;
+ this->setVel2(vel_sat[0] * 1000.,
+ vel_sat[1] * 1000.); // convert velocity in mm/s
this->unlock();
- }
- else
- {
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send the robot velocity in the specified control frame");
+ } else {
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send the robot velocity in the specified control frame");
}
}
/*!
Initialize the robot.
- - Sets the robot in asynchronous mode by starting a low level thread. The robot will be stopped
- if there is no connection to the robot at any given point.
+ - Sets the robot in asynchronous mode by starting a low level thread. The
+ robot will be stopped if there is no connection to the robot at any given
+ point.
- Enables the motors on the robot, if it is connected.
*/
void vpRobotPioneer::init()
{
- if ( ! isInitialized )
- {
+ if (!isInitialized) {
// Start the robot processing cycle running in the background.
// True parameter means that if the connection is lost, then the
// run loop ends.
@@ -168,65 +169,65 @@ void vpRobotPioneer::init()
/*!
Gets the current translational velocity of the robot.
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
- rotational velocities are implemented.
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME
+ to get left and right wheel velocities and vpRobot::REFERENCE_FRAME to get
+ translational and rotational velocities are implemented.
- \param velocity : A two dimension vector that corresponds to the current velocities applied to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
+ \param velocity : A two dimension vector that corresponds to the current
+ velocities applied to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of
+ the left wheel and second value is the velocity of the right wheel in m/s.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation
+ velocity in m/s. Second value is the rotational velocity in rad/s.
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2
+ dimension vector. \exception vpRobotException::wrongStateError : If the
+ specified control frame is not supported.
\sa getVelocity(const vpRobot::vpControlFrameType)
*/
-void vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & velocity)
+void vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
init();
velocity.resize(2);
- if (frame == vpRobot::ARTICULAR_FRAME)
- {
+ if (frame == vpRobot::ARTICULAR_FRAME) {
this->lock();
- velocity[0] = this->getLeftVel() / 1000.;
- velocity[1] = this->getRightVel() / 1000;
+ velocity[0] = this->getLeftVel() / 1000.;
+ velocity[1] = this->getRightVel() / 1000;
this->unlock();
- }
- else if (frame == vpRobot::REFERENCE_FRAME)
- {
+ } else if (frame == vpRobot::REFERENCE_FRAME) {
this->lock();
velocity[0] = this->getVel() / 1000.;
- velocity[1] = vpMath::rad( this->getRotVel() );
+ velocity[1] = vpMath::rad(this->getRotVel());
this->unlock();
- }
- else {
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get the robot volocity in the specified control frame");
+ } else {
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot get the robot volocity in the specified control frame");
}
}
/*!
Gets the current translational velocity of the robot.
- \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME to get left
- and right wheel velocities and vpRobot::REFERENCE_FRAME to get translational and
- rotational velocities are implemented.
+ \param frame : Control frame. For the moment, only vpRobot::ARTICULAR_FRAME
+ to get left and right wheel velocities and vpRobot::REFERENCE_FRAME to get
+ translational and rotational velocities are implemented.
- \return A two dimension vector that corresponds to the current velocities applied to the robot.
- - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of the left wheel and
- second value is the velocity of the right wheel in m/s.
- - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation velocity in m/s.
- Second value is the rotational velocity in rad/s.
+ \return A two dimension vector that corresponds to the current velocities
+ applied to the robot.
+ - If the frame is vpRobot::ARTICULAR_FRAME, first value is the velocity of
+ the left wheel and second value is the velocity of the right wheel in m/s.
+ - If the frame is vpRobot::REFERENCE_FRAME, first value is the translation
+ velocity in m/s. Second value is the rotational velocity in rad/s.
- \exception vpRobotException::dimensionError : Velocity vector is not a 2 dimension vector.
- \exception vpRobotException::wrongStateError : If the specified control frame is not supported.
+ \exception vpRobotException::dimensionError : Velocity vector is not a 2
+ dimension vector. \exception vpRobotException::wrongStateError : If the
+ specified control frame is not supported.
\sa getVelocity(const vpRobot::vpControlFrameType, vpColVector &)
*/
-vpColVector vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame)
+vpColVector vpRobotPioneer::getVelocity(const vpRobot::vpControlFrameType frame)
{
vpColVector velocity;
getVelocity(frame, velocity);
@@ -234,6 +235,7 @@ vpColVector vpRobotPioneer::getVelocity (const vpRobot::vpControlFrameType frame
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no symbols
-void dummy_vpRobotPioneer() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotPioneer.cpp.o) has no
+// symbols
+void dummy_vpRobotPioneer(){};
#endif
diff --git a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp
index 4e79656..8b158a2 100644
--- a/modules/robot/src/real-robot/ptu46/vpPtu46.cpp
+++ b/modules/robot/src/real-robot/ptu46/vpPtu46.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,18 +36,14 @@
*
*****************************************************************************/
-
-
/* ----------------------------------------------------------------------- */
/* --- INCLUDE ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
-
-#include <visp3/robot/vpPtu46.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpPtu46.h>
#include <visp3/robot/vpRobotException.h>
-
/* Inclusion des fichiers standards. */
#include <math.h>
#include <visp3/core/vpMath.h>
@@ -55,11 +52,10 @@
/* --- COMPUTE ------------------------------------------------------------ */
/* ------------------------------------------------------------------------ */
const unsigned int vpPtu46::ndof = 2; /*<! Pan and tilt are considered. */
-const float vpPtu46::L = 0.0765f; /*! Horizontal offset along the last joint,
- from last joint to camera frame. */
-const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
- camera frame. */
-
+const float vpPtu46::L = 0.0765f; /*! Horizontal offset along the last joint,
+ from last joint to camera frame. */
+const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
+ camera frame. */
/*!
Compute the direct geometric model of the camera: fMc
@@ -67,49 +63,48 @@ const float vpPtu46::h = 0.068f; /*<! Vertical offset from last joint to
\param q : Articular position for pan and tilt axis.
\param fMc : Homogeneous matrix corresponding to the direct geometric model
- of the camera. Describes the transformation between the robot reference frame
- (called fixed) and the camera frame.
+ of the camera. Describes the transformation between the robot reference
+ frame (called fixed) and the camera frame.
*/
-void
-vpPtu46::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
+void vpPtu46::computeMGD(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
}
- double q1 = q[0]; // pan
- double q2 = q[1]; // tilt
+ double q1 = q[0]; // pan
+ double q2 = q[1]; // tilt
- double c1 = cos(q1);
- double s1 = sin(q1);
- double c2 = cos(q2);
- double s2 = sin(q2);
+ double c1 = cos(q1);
+ double s1 = sin(q1);
+ double c2 = cos(q2);
+ double s2 = sin(q2);
fMc[0][0] = s1;
- fMc[0][1] = c1*s2;
- fMc[0][2] = c1*c2;
- fMc[0][3] = -h*c1*s2 - L*s1;
+ fMc[0][1] = c1 * s2;
+ fMc[0][2] = c1 * c2;
+ fMc[0][3] = -h * c1 * s2 - L * s1;
fMc[1][0] = -c1;
- fMc[1][1] = s1*s2;
- fMc[1][2] = s1*c2;
- fMc[1][3] = -h*s1*s2 + L*c1;
+ fMc[1][1] = s1 * s2;
+ fMc[1][2] = s1 * c2;
+ fMc[1][3] = -h * s1 * s2 + L * c1;
fMc[2][0] = 0;
fMc[2][1] = -c2;
fMc[2][2] = s2;
- fMc[2][3] = h*c2;
+ fMc[2][3] = h * c2;
fMc[3][0] = 0;
fMc[3][1] = 0;
fMc[3][2] = 0;
fMc[3][3] = 1;
- vpCDEBUG (6) << "Position de la camera: " << std::endl << fMc;
+ vpCDEBUG(6) << "Position de la camera: " << std::endl << fMc;
- return ;
+ return;
}
/*!
@@ -118,16 +113,15 @@ vpPtu46::computeMGD (const vpColVector & q, vpHomogeneousMatrix & fMc) const
\param q : Articular position for pan and tilt axis.
\return fMc, the homogeneous matrix corresponding to the direct geometric
- model of the camera. Describes the transformation between the robot reference
- frame (called fixed) and the camera frame.
+ model of the camera. Describes the transformation between the robot
+ reference frame (called fixed) and the camera frame.
*/
-vpHomogeneousMatrix
-vpPtu46::computeMGD (const vpColVector & q) const
+vpHomogeneousMatrix vpPtu46::computeMGD(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
- computeMGD (q, fMc);
+ computeMGD(q, fMc);
return fMc;
}
@@ -141,19 +135,16 @@ vpPtu46::computeMGD (const vpColVector & q) const
reference frame (called fixed) and the camera frame.
*/
-void
-vpPtu46::computeMGD (const vpColVector & q, vpPoseVector & r) const
+void vpPtu46::computeMGD(const vpColVector &q, vpPoseVector &r) const
{
vpHomogeneousMatrix fMc;
- computeMGD (q, fMc);
+ computeMGD(q, fMc);
r.buildFrom(fMc.inverse());
- return ;
+ return;
}
-
-
/* ---------------------------------------------------------------------- */
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
@@ -163,75 +154,62 @@ vpPtu46::computeMGD (const vpColVector & q, vpPoseVector & r) const
Default construtor. Call init().
*/
-vpPtu46::vpPtu46 (void)
-{
- init();
-}
+vpPtu46::vpPtu46(void) { init(); }
/* ---------------------------------------------------------------------- */
/* --- PRIVATE ---------------------------------------------------------- */
/* ---------------------------------------------------------------------- */
-
/*!
Initialization. Here nothing to do.
*/
-void
-vpPtu46::init ()
-{
- return ;
-}
-
+void vpPtu46::init() { return; }
/* ----------------------------------------------------------------------- */
/* --- DISPLAY ----------------------------------------------------------- */
/* ----------------------------------------------------------------------- */
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpPtu46 & /* constant */)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpPtu46 & /* constant */)
{
- os
- << "Geometric parameters: " << std::endl
- << "L: "
- << "\t" << vpPtu46::L << std::endl
- << "h: "
- << "\t" << vpPtu46::h << std::endl;
+ os << "Geometric parameters: " << std::endl
+ << "L: "
+ << "\t" << vpPtu46::L << std::endl
+ << "h: "
+ << "\t" << vpPtu46::h << std::endl;
return os;
}
-
/*!
Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cVe : Twist transformation between camera and end effector frame to
expess a velocity skew from end effector frame in camera frame.
*/
-void
-vpPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cMe : Homogeneous matrix between camera and end effector frame.
*/
-void
-vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
+void vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
{
- vpHomogeneousMatrix eMc ;
+ vpHomogeneousMatrix eMc;
eMc[0][0] = 0;
eMc[0][1] = -1;
@@ -253,7 +231,7 @@ vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
eMc[3][2] = 0;
eMc[3][3] = 1;
- cMe = eMc.inverse() ;
+ cMe = eMc.inverse();
}
/*!
@@ -268,20 +246,18 @@ vpPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
tilt axis).
*/
-void
-vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+void vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
-
- eJe.resize(6,2) ;
+ eJe.resize(6, 2);
if (q.getRows() != 2) {
vpERROR_TRACE("Bad dimension for ptu-46 articular vector");
throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
}
- double s2 = sin(q[1]) ;
- double c2 = cos(q[1]) ;
+ double s2 = sin(q[1]);
+ double c2 = cos(q[1]);
eJe = 0;
@@ -295,12 +271,11 @@ vpPtu46::get_eJe(const vpColVector &q, vpMatrix &eJe) const
\param q : Articular position for pan and tilt axis.
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
+ \param fJe : Jacobian between reference frame (or fix frame) and end
+ effector frame (on tilt axis).
*/
-void
-vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+void vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
if (q.getRows() != 2) {
@@ -308,10 +283,10 @@ vpPtu46::get_fJe(const vpColVector &q, vpMatrix &fJe) const
throw(vpException(vpException::dimensionError, "Bad dimension for ptu-46 articular vector"));
}
- fJe.resize(6,2) ;
+ fJe.resize(6, 2);
- double s1 = sin(q[0]) ;
- double c1 = cos(q[0]) ;
+ double s1 = sin(q[0]);
+ double c1 = cos(q[0]);
fJe = 0;
diff --git a/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp
index bc00a59..5359917 100644
--- a/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp
+++ b/modules/robot/src/real-robot/ptu46/vpRobotPtu46.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,11 +43,11 @@
#ifdef VISP_HAVE_PTU46
/* Headers des fonctions implementees. */
-#include <visp3/robot/vpPtu46.h>
-#include <visp3/robot/vpRobotPtu46.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/robot/vpPtu46.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpRobotPtu46.h>
/* ---------------------------------------------------------------------- */
/* --- STATIC ------------------------------------------------------------ */
@@ -59,7 +60,6 @@ const double vpRobotPtu46::defaultPositioningVelocity = 10.0;
/* --- CONSTRUCTOR ------------------------------------------------------ */
/* ---------------------------------------------------------------------- */
-
/*!
Default constructor.
@@ -69,38 +69,30 @@ const double vpRobotPtu46::defaultPositioningVelocity = 10.0;
\sa init()
*/
-vpRobotPtu46::vpRobotPtu46 (const char *device)
- :
- vpRobot ()
+vpRobotPtu46::vpRobotPtu46(const char *device) : vpRobot()
{
- this->device = new char [FILENAME_MAX];
+ this->device = new char[FILENAME_MAX];
sprintf(this->device, "%s", device);
- vpDEBUG_TRACE (12, "Open communication with Ptu-46.");
- try
- {
+ vpDEBUG_TRACE(12, "Open communication with Ptu-46.");
+ try {
init();
- }
- catch(...)
- {
- delete [] this->device;
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ delete[] this->device;
+ vpERROR_TRACE("Error caught");
+ throw;
}
- try
- {
- setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...)
- {
- delete [] this->device;
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
+ delete[] this->device;
+ vpERROR_TRACE("Error caught");
+ throw;
}
- positioningVelocity = defaultPositioningVelocity ;
- return ;
+ positioningVelocity = defaultPositioningVelocity;
+ return;
}
/* ------------------------------------------------------------------------ */
@@ -113,27 +105,28 @@ vpRobotPtu46::vpRobotPtu46 (const char *device)
Close the serial connection with the head.
*/
-vpRobotPtu46::~vpRobotPtu46 (void)
+vpRobotPtu46::~vpRobotPtu46(void)
{
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
- if (0 != ptu.close())
- {
- vpERROR_TRACE ("Error while closing communications with the robot ptu-46.");
+ if (0 != ptu.close()) {
+ vpERROR_TRACE("Error while closing communications with the robot ptu-46.");
}
vpRobotPtu46::robotAlreadyCreated = false;
- delete [] device;
+ delete[] device;
return;
}
-
-/* -------------------------------------------------------------------------- */
-/* --- INITIALISATION ------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- INITIALISATION -------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
@@ -144,68 +137,54 @@ vpRobotPtu46::~vpRobotPtu46 (void)
oppened.
*/
-void
-vpRobotPtu46::init ()
+void vpRobotPtu46::init()
{
- vpDEBUG_TRACE (12, "Open connection Ptu-46.");
- if (0 != ptu.init(device) )
- {
- vpERROR_TRACE ("Cannot open connection with ptu-46.");
- throw vpRobotException (vpRobotException::constructionError,
- "Cannot open connection with ptu-46");
+ vpDEBUG_TRACE(12, "Open connection Ptu-46.");
+ if (0 != ptu.init(device)) {
+ vpERROR_TRACE("Cannot open connection with ptu-46.");
+ throw vpRobotException(vpRobotException::constructionError, "Cannot open connection with ptu-46");
}
- return ;
+ return;
}
-
/*!
Change the state of the robot either to stop them, or to set position or
speed control.
*/
-vpRobot::vpRobotStateType
-vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
+vpRobot::vpRobotStateType vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
{
- switch (newState)
- {
- case vpRobot::STATE_STOP:
- {
- if (vpRobot::STATE_STOP != getRobotState ())
- {
- ptu.stop();
- }
- break;
+ switch (newState) {
+ case vpRobot::STATE_STOP: {
+ if (vpRobot::STATE_STOP != getRobotState()) {
+ ptu.stop();
}
- case vpRobot::STATE_POSITION_CONTROL:
- {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ())
- {
- vpDEBUG_TRACE (12, "Passage vitesse -> position.");
- ptu.stop();
- }
- else
- {
- vpDEBUG_TRACE (1, "Passage arret -> position.");
- }
- break;
+ break;
+ }
+ case vpRobot::STATE_POSITION_CONTROL: {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
+ vpDEBUG_TRACE(12, "Passage vitesse -> position.");
+ ptu.stop();
+ } else {
+ vpDEBUG_TRACE(1, "Passage arret -> position.");
}
- case vpRobot::STATE_VELOCITY_CONTROL:
- {
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpDEBUG_TRACE (10, "Arret du robot...");
- ptu.stop();
- }
- break;
+ break;
+ }
+ case vpRobot::STATE_VELOCITY_CONTROL: {
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpDEBUG_TRACE(10, "Arret du robot...");
+ ptu.stop();
}
+ break;
+ }
default:
- break ;
+ break;
}
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
/*!
@@ -213,47 +192,40 @@ vpRobotPtu46::setRobotState(vpRobot::vpRobotStateType newState)
Halt all the axis.
*/
-void
-vpRobotPtu46::stopMotion(void)
+void vpRobotPtu46::stopMotion(void)
{
ptu.stop();
- setRobotState (vpRobot::STATE_STOP);
+ setRobotState(vpRobot::STATE_STOP);
}
-
/*!
Get the twist matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cVe : Twist transformation between camera and end effector frame to
expess a velocity skew from end effector frame in camera frame.
*/
-void
-vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotPtu46::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- vpPtu46::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpPtu46::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
Get the homogeneous matrix corresponding to the transformation between the
- camera frame and the end effector frame. The end effector frame is located on
- the tilt axis.
+ camera frame and the end effector frame. The end effector frame is located
+ on the tilt axis.
\param cMe : Homogeneous matrix between camera and end effector frame.
*/
-void
-vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
-{
- vpPtu46::get_cMe(cMe) ;
-}
+void vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const { vpPtu46::get_cMe(cMe); }
/*!
Get the robot jacobian expressed in the end-effector frame.
@@ -265,72 +237,53 @@ vpRobotPtu46::get_cMe(vpHomogeneousMatrix &cMe) const
tilt axis).
*/
-void
-vpRobotPtu46::get_eJe(vpMatrix &eJe)
+void vpRobotPtu46::get_eJe(vpMatrix &eJe)
{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpPtu46::get_eJe(q,eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ vpColVector q(2);
+ getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ try {
+ vpPtu46::get_eJe(q, eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
/*!
Get the robot jacobian expressed in the robot reference frame
- \param fJe : Jacobian between reference frame (or fix frame) and end effector
- frame (on tilt axis).
+ \param fJe : Jacobian between reference frame (or fix frame) and end
+ effector frame (on tilt axis).
*/
-void
-vpRobotPtu46::get_fJe(vpMatrix &fJe)
+void vpRobotPtu46::get_fJe(vpMatrix &fJe)
{
- vpColVector q(2) ;
- getPosition(vpRobot::ARTICULAR_FRAME, q) ;
-
- try
- {
- vpPtu46::get_fJe(q,fJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
+ vpColVector q(2);
+ getPosition(vpRobot::ARTICULAR_FRAME, q);
+
+ try {
+ vpPtu46::get_fJe(q, fJe);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
}
-
/*!
Set the velocity used for a position control.
\param velocity : Velocity in % of the maximum velocity between [0,100].
*/
-void
-vpRobotPtu46::setPositioningVelocity (const double velocity)
-{
- positioningVelocity = velocity;
-}
+void vpRobotPtu46::setPositioningVelocity(const double velocity) { positioningVelocity = velocity; }
/*!
Get the velocity in % used for a position control.
\return Positionning velocity in [0, 100]
*/
-double
-vpRobotPtu46::getPositioningVelocity (void)
-{
- return positioningVelocity;
-}
-
+double vpRobotPtu46::getPositioningVelocity(void) { return positioningVelocity; }
/*!
Move the robot in position control.
@@ -343,48 +296,41 @@ vpRobotPtu46::getPositioningVelocity (void)
\param q : The position to set for each axis.
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
+ \exception vpRobotException::wrongStateError : If a not supported frame
+ type is given.
*/
-void
-vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
- const vpColVector & q )
+void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const vpColVector &q)
{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
- switch(frame)
- {
+ switch (frame) {
case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot move the robot in camera frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in camera frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in camera frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in camera frame: "
+ "not implemented");
break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot move the robot in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in reference frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in reference frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in reference frame: "
+ "not implemented");
break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot move the robot in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot move the robot in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot move the robot in mixt frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot move the robot in mixt frame: "
+ "not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
- break ;
+ break;
}
// Interface for the controller
@@ -393,17 +339,14 @@ vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
artpos[0] = q[0];
artpos[1] = q[1];
- if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE) )
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error.");
+ if (0 != ptu.move(artpos, positioningVelocity, PTU_ABSOLUTE_MODE)) {
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error.");
}
- return ;
+ return;
}
-
/*!
Move the robot in position control.
@@ -416,25 +359,21 @@ vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
\param q1 : The pan position to set.
\param q2 : The tilt position to set.
- \exception vpRobotException::wrongStateError : If a not supported frame type
- is given.
+ \exception vpRobotException::wrongStateError : If a not supported frame
+ type is given.
*/
-void
-vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
- const double &q1, const double &q2)
+void vpRobotPtu46::setPosition(const vpRobot::vpControlFrameType frame, const double &q1, const double &q2)
{
- try{
- vpColVector q(2) ;
- q[0] = q1 ;
- q[1] = q2 ;
-
- setPosition(frame,q) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
+ try {
+ vpColVector q(2);
+ q[0] = q1;
+ q[1] = q2;
+
+ setPosition(frame, q);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -451,16 +390,14 @@ vpRobotPtu46::setPosition (const vpRobot::vpControlFrameType frame,
\sa readPositionFile()
*/
-void
-vpRobotPtu46::setPosition(const char *filename)
+void vpRobotPtu46::setPosition(const char *filename)
{
- vpColVector q ;
+ vpColVector q;
if (readPositionFile(filename, q) == false) {
- vpERROR_TRACE ("Cannot get ptu-46 position from file");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Cannot get ptu-46 position from file");
+ vpERROR_TRACE("Cannot get ptu-46 position from file");
+ throw vpRobotException(vpRobotException::readingParametersError, "Cannot get ptu-46 position from file");
}
- setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
+ setPosition(vpRobot::ARTICULAR_FRAME, q);
}
/*!
@@ -476,54 +413,45 @@ vpRobotPtu46::setPosition(const char *filename)
is given.
*/
-void
-vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
- vpColVector & q)
+void vpRobotPtu46::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
- vpDEBUG_TRACE (9, "# Entree.");
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- vpERROR_TRACE ("Cannot get position in camera frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in camera frame: "
- "not implemented");
+ vpDEBUG_TRACE(9, "# Entree.");
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME:
+ vpERROR_TRACE("Cannot get position in camera frame: not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in camera frame: "
+ "not implemented");
break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot get position in reference frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in reference frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in reference frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in reference frame: "
+ "not implemented");
break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot get position in mixt frame: "
- "not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
+ vpERROR_TRACE("Cannot get position in mixt frame: "
+ "not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position in mixt frame: "
+ "not implemented");
break;
case vpRobot::ARTICULAR_FRAME:
- break ;
+ break;
}
double artpos[2];
- if (0 != ptu.getCurrentPosition( artpos ) )
- {
- vpERROR_TRACE ("Error when calling recup_posit_Afma4.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Error when calling recup_posit_Afma4.");
+ if (0 != ptu.getCurrentPosition(artpos)) {
+ vpERROR_TRACE("Error when calling recup_posit_Afma4.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Error when calling recup_posit_Afma4.");
}
- q.resize (vpPtu46::ndof);
+ q.resize(vpPtu46::ndof);
q[0] = artpos[0];
q[1] = artpos[1];
}
-
/*!
Send a velocity on each axis.
@@ -533,13 +461,13 @@ vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
and the mixt frame (vpRobot::MIXT_FRAME) are not implemented.
\param v : The desired velocity of the axis. The size of this vector is
- always 2. Velocitoes are expressed in rad/s.
+ always 2. Velocities are expressed in rad/s.
- - In camera frame, \f$ v = [\omega_x, \omega_y]^t \f$.
+ - In camera frame, \f$ v = [\omega_x, \omega_y]^t \f$ expressed in rad/s.
- In articular, we control the 2 dof, \f$ v = [\dot{q}_1, \dot{q}_2]^t \f$
- with \f$ \dot{q}_1 \f$ the pan of the camera and \f$ \dot{q}_2\f$ the tilt of
- the camera.
+ with \f$ \dot{q}_1 \f$ the pan of the camera and \f$ \dot{q}_2\f$ the tilt
+ of the camera in rad/s.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
@@ -553,117 +481,102 @@ vpRobotPtu46::getPosition (const vpRobot::vpControlFrameType frame,
autorized speed (see vpRobot::maxRotationVelocity).
*/
-void
-vpRobotPtu46::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & v)
+void vpRobotPtu46::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
{
TPtuFrame ptuFrameInterface;
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ())
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
}
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- ptuFrameInterface = PTU_CAMERA_FRAME;
- if ( v.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in camera frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in camera frame");
- }
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- ptuFrameInterface = PTU_ARTICULAR_FRAME;
- if ( v.getRows() != 2) {
- vpERROR_TRACE ("Bad dimension fo speed vector in articular frame");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Bad dimension for speed vector "
- "in articular frame");
- }
- break ;
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ ptuFrameInterface = PTU_CAMERA_FRAME;
+ if (v.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension fo speed vector in camera frame");
+ throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
+ "in camera frame");
}
- case vpRobot::REFERENCE_FRAME :
- {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "in the reference frame:"
- "functionality not implemented");
- }
- 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");
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot ");
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ ptuFrameInterface = PTU_ARTICULAR_FRAME;
+ if (v.getRows() != 2) {
+ vpERROR_TRACE("Bad dimension fo speed vector in articular frame");
+ throw vpRobotException(vpRobotException::wrongStateError, "Bad dimension for speed vector "
+ "in articular frame");
}
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot "
+ "in the reference frame:"
+ "functionality not implemented");
+ }
+ 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");
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot send a velocity to the robot ");
+ }
}
- vpDEBUG_TRACE (12, "Velocity limitation.");
+ vpDEBUG_TRACE(12, "Velocity limitation.");
double ptuSpeedInterface[2];
- switch(frame) {
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::CAMERA_FRAME : {
- double max = this ->maxRotationVelocity;
- 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
+ switch (frame) {
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::CAMERA_FRAME: {
+ double max = this->maxRotationVelocity;
+ 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)
- {
+ if (fabs(v[i]) > max) {
norm = true;
- max = fabs (v[i]);
- vpERROR_TRACE ("Excess velocity: ROTATION "
- "(axe nr.%d).", i);
+ max = fabs(v[i]);
+ vpERROR_TRACE("Excess velocity: ROTATION "
+ "(axe nr.%d).",
+ i);
}
}
// Rotations velocities normalisation
if (norm == true) {
- max = this ->maxRotationVelocity / max;
- for (unsigned int i = 0 ; i < 2; ++ i)
- ptuSpeedInterface [i] = v[i]*max;
+ max = this->maxRotationVelocity / max;
+ for (unsigned int i = 0; i < 2; ++i)
+ ptuSpeedInterface[i] = v[i] * max;
}
break;
}
default:
// Should never occur
break;
-
}
- vpCDEBUG(12) << "v: " << ptuSpeedInterface[0]
- << " " << ptuSpeedInterface[1] << std::endl;
+ vpCDEBUG(12) << "v: " << ptuSpeedInterface[0] << " " << ptuSpeedInterface[1] << std::endl;
ptu.move(ptuSpeedInterface, ptuFrameInterface);
return;
}
-
-/* ------------------------------------------------------------------------- */
-/* --- GET ----------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- GET -----------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -676,45 +589,35 @@ 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;
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- vpERROR_TRACE ("Cannot get a velocity in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the camera frame:"
- "functionality not implemented");
- }
- case vpRobot::ARTICULAR_FRAME:
- {
- ptuFrameInterface = PTU_ARTICULAR_FRAME;
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- {
- vpERROR_TRACE ("Cannot get a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a velocity in the reference frame:"
- "functionality not implemented");
- }
- case vpRobot::MIXT_FRAME:
- {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpERROR_TRACE("Cannot get a velocity in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the camera frame:"
+ "functionality not implemented");
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ ptuFrameInterface = PTU_ARTICULAR_FRAME;
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpERROR_TRACE("Cannot get a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a velocity in the reference frame:"
+ "functionality not implemented");
+ }
+ 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");
- }
+ 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");
+ }
}
q_dot.resize(vpPtu46::ndof);
@@ -724,10 +627,8 @@ vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
q_dot[0] = ptuSpeedInterface[0];
q_dot[1] = ptuSpeedInterface[1];
-
}
-
/*!
Return the articular velocity.
@@ -739,11 +640,10 @@ vpRobotPtu46::getVelocity (const vpRobot::vpControlFrameType frame,
\exception vpRobotException::wrongStateError : If a not supported frame type
is given.
*/
-vpColVector
-vpRobotPtu46::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotPtu46::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector q_dot;
- getVelocity (frame, q_dot);
+ getVelocity(frame, q_dot);
return q_dot;
}
@@ -767,12 +667,11 @@ 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);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -784,29 +683,29 @@ vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q)
q.resize(vpPtu46::ndof);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Ptu-46 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < vpPtu46::ndof+1) // try to split with tab separator
+ 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)
+ if (chain.size() < vpPtu46::ndof + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< vpPtu46::ndof; i++)
+ for (unsigned int i = 0; i < vpPtu46::ndof; i++)
ss >> q[i];
pos_found = true;
break;
@@ -828,40 +727,6 @@ vpRobotPtu46::readPositionFile(const std::string &filename, vpColVector &q)
/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param v The measured displacement in camera frame. The dimension of v is 6
- (tx, ty, ty, rx, ry, rz). Translations are expressed in meters, rotations in
- radians.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
-vpRobotPtu46::getCameraDisplacement(vpColVector &v)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, v);
-
-}
-/*!
-
- Get the robot articular displacement since the last call of this method.
-
- \param d The measured articular displacement. The dimension of d is 2 (the
- number of axis of the robot) with respectively d[0] (pan displacement),
- d[1] (tilt displacement)
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void vpRobotPtu46::getArticularDisplacement(vpColVector &d)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, d);
-}
-
-/*!
-
Get the robot displacement since the last call of this method.
\warning The first call of this method gives not a good value for the
@@ -875,61 +740,50 @@ void vpRobotPtu46::getArticularDisplacement(vpColVector &d)
- In camera frame, the dimension of d is 6 (tx, ty, ty, rx, ry,
rz). Translations are expressed in meters, rotations in radians.
- \exception vpRobotException::wrongStateError If a not supported frame type is
- given.
-
- \sa getArticularDisplacement(), getCameraDisplacement()
+ \exception vpRobotException::wrongStateError If a not supported frame type
+ is given.
*/
-void
-vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &d)
+void vpRobotPtu46::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &d)
{
double d_[6];
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- d.resize (6);
- ptu.measureDpl(d_, PTU_CAMERA_FRAME);
- d[0]=d_[0];
- d[1]=d_[1];
- d[2]=d_[2];
- d[3]=d_[3];
- d[4]=d_[4];
- d[5]=d_[5];
- break ;
- }
- case vpRobot::ARTICULAR_FRAME:
- {
- ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
- d.resize (vpPtu46::ndof);
- d[0]=d_[0]; // pan
- d[1]=d_[1]; // tilt
- break ;
- }
- case vpRobot::REFERENCE_FRAME:
- {
- vpERROR_TRACE ("Cannot get a displacement in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot get a displacement in the reference frame:"
- "functionality not implemented");
- }
- 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");
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ d.resize(6);
+ ptu.measureDpl(d_, PTU_CAMERA_FRAME);
+ d[0] = d_[0];
+ d[1] = d_[1];
+ d[2] = d_[2];
+ d[3] = d_[3];
+ d[4] = d_[4];
+ d[5] = d_[5];
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ ptu.measureDpl(d_, PTU_ARTICULAR_FRAME);
+ d.resize(vpPtu46::ndof);
+ d[0] = d_[0]; // pan
+ d[1] = d_[1]; // tilt
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpERROR_TRACE("Cannot get a displacement in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot get a displacement in the reference frame:"
+ "functionality not implemented");
+ }
+ 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");
+ }
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no symbols
-void dummy_vpRobotPtu46() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotPtu46.cpp.o) has no
+// symbols
+void dummy_vpRobotPtu46(){};
#endif
-
diff --git a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp
index 0901fed..ac7a6fc 100644
--- a/modules/robot/src/real-robot/viper/vpRobotViper650.cpp
+++ b/modules/robot/src/real-robot/viper/vpRobotViper650.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,13 +45,13 @@
#include <sys/types.h>
#include <unistd.h>
-#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotViper650.h>
/* ---------------------------------------------------------------------- */
@@ -81,24 +82,27 @@ const double vpRobotViper650::defaultPositioningVelocity = 15.0;
*/
void emergencyStopViper650(int signo)
{
- std::cout << "Stop the Viper650 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
+ std::cout << "Stop the Viper650 application by signal (" << signo << "): " << (char)7;
+ switch (signo) {
case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ std::cout << "SIGINT (stop by ^C) " << std::endl;
+ break;
case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
+ break;
case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
+ break;
case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
+ break;
case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
-}
- //std::cout << "Emergency stop called\n";
+ std::cout << "SIGQUIT " << std::endl;
+ break;
+ default:
+ std::cout << signo << std::endl;
+ }
+ // std::cout << "Emergency stop called\n";
// PrimitiveESTOP_Viper650();
PrimitiveSTOP_Viper650();
std::cout << "Robot was stopped\n";
@@ -109,7 +113,7 @@ void emergencyStopViper650(int signo)
fprintf(stdout, "Application ");
fflush(stdout);
kill(getpid(), SIGKILL);
- exit(1) ;
+ exit(1);
}
/* ---------------------------------------------------------------------- */
@@ -131,21 +135,21 @@ void emergencyStopViper650(int signo)
including the distorsion, use the code below:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper650.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
#ifdef VISP_HAVE_VIPER650
vpRobotViper650 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // Set the extrinsic camera parameters obtained with a perpective
// projection model including a distorsion parameter
robot.init(vpViper650::TOOL_MARLIN_F033C_CAMERA,
- vpCameraParameters::perspectiveProjWithDistortion);
+ vpCameraParameters::perspectiveProjWithDistortion);
\endcode
Now, you can get the intrinsic camera parameters associated to an
@@ -171,10 +175,7 @@ int main()
vpCameraParameters::vpCameraParametersProjType)
*/
-vpRobotViper650::vpRobotViper650 (bool verbose)
- :
- vpViper650 (),
- vpRobot ()
+vpRobotViper650::vpRobotViper650(bool verbose) : vpViper650(), vpRobot()
{
/*
@@ -197,8 +198,8 @@ vpRobotViper650::vpRobotViper650 (bool verbose)
*/
signal(SIGINT, emergencyStopViper650);
- signal(SIGBUS, emergencyStopViper650) ;
- signal(SIGSEGV, emergencyStopViper650) ;
+ signal(SIGBUS, emergencyStopViper650);
+ signal(SIGSEGV, emergencyStopViper650);
signal(SIGKILL, emergencyStopViper650);
signal(SIGQUIT, emergencyStopViper650);
@@ -207,22 +208,20 @@ vpRobotViper650::vpRobotViper650 (bool verbose)
std::cout << "Open communication with MotionBlox.\n";
try {
this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
+ this->setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
// vpERROR_TRACE("Error caught") ;
- throw ;
+ throw;
}
- positioningVelocity = defaultPositioningVelocity ;
+ positioningVelocity = defaultPositioningVelocity;
maxRotationVelocity_joint6 = maxRotationVelocity;
vpRobotViper650::robotAlreadyCreated = true;
- return ;
+ return;
}
-
/* ------------------------------------------------------------------------ */
/* --- INITIALISATION ----------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -240,16 +239,16 @@ vpRobotViper650::vpRobotViper650 (bool verbose)
init(vpViper650::defaultCameraRobot). If you want to set the extrinsic
camera parameters to those obtained with a camera perspective model
including the distorsion you have to call the
- init(vpViper650::vpToolType, vpCameraParameters::vpCameraParametersProjType) method.
- If you want to set custom extrinsic camera parameters you have to call
- the init(vpViper650::vpToolType, const vpHomogeneousMatrix&) method.
+ init(vpViper650::vpToolType, vpCameraParameters::vpCameraParametersProjType)
+ method. If you want to set custom extrinsic camera parameters you have to
+ call the init(vpViper650::vpToolType, const vpHomogeneousMatrix&) method.
- \sa vpCameraParameters, init(vpViper650::vpToolType, vpCameraParameters::vpCameraParametersProjType),
+ \sa vpCameraParameters, init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
init(vpViper650::vpToolType, const vpHomogeneousMatrix&),
init(vpViper650::vpToolType, const std::string&)
*/
-void
-vpRobotViper650::init (void)
+void vpRobotViper650::init(void)
{
InitTry;
@@ -265,15 +264,15 @@ vpRobotViper650::init (void)
first_time_getdis = true;
// Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
+ Try(InitializeConnection(verbose_));
// Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Viper650() );
+ Try(InitializeNode_Viper650());
- Try( PrimitiveRESET_Viper650() );
+ Try(PrimitiveRESET_Viper650());
// Enable the joint limits on axis 6
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
// Update the eMc matrix in the low level controller
init(vpViper650::defaultTool);
@@ -281,14 +280,13 @@ 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
if (verbose_) {
std::cout << "Robot status: ";
- switch(EStopStatus) {
+ switch (EStopStatus) {
case ESTOP_AUTO:
controlMode = AUTO;
if (HIPowerStatus == 0)
@@ -316,23 +314,25 @@ vpRobotViper650::init (void)
std::cout << std::endl;
}
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// If an error occur in the low level controller, goto here
- //CatchPrint();
+ // CatchPrint();
Catch();
// Test if an error occurs
if (TryStt == -20001)
printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
+ "and if the firewire link exist between the MotionBlox and this "
+ "computer.\n");
else if (TryStt == -675)
printf(" Timeout enabling power...\n");
@@ -343,10 +343,9 @@ 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 ;
+ return;
}
/*!
@@ -367,21 +366,20 @@ vpRobotViper650::init (void)
including the distorsion, use the code below:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper650.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
#ifdef VISP_HAVE_VIPER650
vpRobotViper650 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // 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
@@ -407,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
@@ -419,26 +415,27 @@ vpRobotViper650::init (vpViper650::vpToolType tool,
InitTry;
// Get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
+ Try(PrimitiveCONST_Viper650(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/*!
@@ -491,34 +488,34 @@ 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);
+ vpViper650::init(tool, filename);
- InitTry;
+ InitTry;
- // Get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
+ // Get real joint min/max from the MotionBlox
+ Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
+ // }
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try(PrimitiveCONST_Viper650(eMc_pose));
- CatchPrint();
- return ;
+ CatchPrint();
+ return;
}
/*!
@@ -557,34 +554,34 @@ 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_);
+ vpViper650::init(tool, eMc_);
- InitTry;
+ InitTry;
- // Get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data) );
- // Convert units from degrees to radians
- joint_min.deg2rad();
- joint_max.deg2rad();
+ // Get real joint min/max from the MotionBlox
+ Try(PrimitiveJOINT_MINMAX_Viper650(joint_min.data, joint_max.data));
+ // Convert units from degrees to radians
+ joint_min.deg2rad();
+ joint_max.deg2rad();
- // for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
- // }
+ // for (unsigned int i=0; i < njoint; i++) {
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
+ // }
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try(PrimitiveCONST_Viper650(eMc_pose));
- CatchPrint();
- return ;
+ CatchPrint();
+ return;
}
/*!
@@ -598,25 +595,24 @@ vpRobotViper650::init (vpViper650::vpToolType tool, const vpHomogeneousMatrix &e
\param eMc_ : Transformation between the end-effector frame
and the tool frame.
*/
-void
-vpRobotViper650::set_eMc(const vpHomogeneousMatrix &eMc_)
+void vpRobotViper650::set_eMc(const vpHomogeneousMatrix &eMc_)
{
- this->vpViper650::set_eMc(eMc_);
+ this->vpViper650::set_eMc(eMc_);
- InitTry;
+ InitTry;
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try(PrimitiveCONST_Viper650(eMc_pose));
- CatchPrint();
+ CatchPrint();
- return ;
+ return;
}
/*!
@@ -631,25 +627,24 @@ 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_);
+ this->vpViper650::set_eMc(etc_, erc_);
- InitTry;
+ InitTry;
- // Set the camera constant (eMc pose) in the MotionBlox
- double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
- }
- // Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper650(eMc_pose) );
+ // Set the camera constant (eMc pose) in the MotionBlox
+ double eMc_pose[6];
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
+ }
+ // Update the eMc pose in the low level controller
+ Try(PrimitiveCONST_Viper650(eMc_pose));
- CatchPrint();
+ CatchPrint();
- return ;
+ return;
}
/* ------------------------------------------------------------------------ */
@@ -662,16 +657,15 @@ vpRobotViper650::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &er
Free allocated resources.
*/
-vpRobotViper650::~vpRobotViper650 (void)
+vpRobotViper650::~vpRobotViper650(void)
{
InitTry;
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
// 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) {
@@ -690,57 +684,52 @@ vpRobotViper650::~vpRobotViper650 (void)
return;
}
-
-
-
/*!
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;
switch (newState) {
case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- Try( PrimitiveSTOP_Viper650() );
- vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
- }
- break;
+ // 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
}
+ break;
+ }
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Viper650() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try(PrimitiveSTOP_Viper650());
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
+ this->powerOn();
+ break;
+ }
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
}
+ this->powerOn();
+ break;
+ }
default:
- break ;
+ break;
}
CatchPrint();
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
-
/* ------------------------------------------------------------------------ */
/* --- STOP --------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -752,21 +741,19 @@ vpRobot::vpRobotStateType
\exception vpRobotException::lowLevelError : If the low level
controller returns an error during robot stopping.
*/
-void
- vpRobotViper650::stopMotion(void)
+void vpRobotViper650::stopMotion(void)
{
if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
return;
InitTry;
- Try( PrimitiveSTOP_Viper650() );
- setRobotState (vpRobot::STATE_STOP);
+ Try(PrimitiveSTOP_Viper650());
+ setRobotState(vpRobot::STATE_STOP);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
+ vpERROR_TRACE("Cannot stop robot motion");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
}
}
@@ -779,8 +766,7 @@ void
\sa powerOff(), getPowerState()
*/
-void
- vpRobotViper650::powerOn(void)
+void vpRobotViper650::powerOn(void)
{
InitTry;
@@ -790,29 +776,27 @@ void
bool firsttime = true;
unsigned int nitermax = 10;
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
+ for (unsigned int i = 0; i < nitermax; i++) {
+ Try(PrimitiveSTATUS_Viper650(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
if (EStopStatus == ESTOP_AUTO) {
controlMode = AUTO;
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
+ } else if (EStopStatus == ESTOP_MANUAL) {
controlMode = MANUAL;
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
+ } else if (EStopStatus == ESTOP_ACTIVATED) {
controlMode = ESTOP;
if (firsttime) {
std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ << "Check the emergency stop button and push the yellow "
+ "button before continuing."
+ << std::endl;
firsttime = false;
}
- fprintf(stdout, "Remaining time %us \r", nitermax-i);
+ fprintf(stdout, "Remaining time %us \r", nitermax - i);
fflush(stdout);
CAL_Wait(1);
- }
- else {
+ } else {
std::cout << "Sorry there is an error on the emergency chain." << std::endl;
std::cout << "You have to call Adept for maintenance..." << std::endl;
// Free allocated resources
@@ -826,22 +810,20 @@ 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) {
fprintf(stdout, "Power ON the Viper650 robot\n");
fflush(stdout);
-
- Try( PrimitivePOWERON_Viper650() );
+
+ Try(PrimitivePOWERON_Viper650());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power on the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -854,29 +836,26 @@ void
\sa powerOn(), getPowerState()
*/
-void
- vpRobotViper650::powerOff(void)
+void vpRobotViper650::powerOff(void)
{
InitTry;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
fprintf(stdout, "Power OFF the Viper650 robot\n");
fflush(stdout);
- Try( PrimitivePOWEROFF_Viper650() );
+ Try(PrimitivePOWEROFF_Viper650());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power off the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -891,15 +870,13 @@ void
\sa powerOn(), powerOff()
*/
-bool
- vpRobotViper650::getPowerState(void) const
+bool vpRobotViper650::getPowerState(void) const
{
InitTry;
bool status = false;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
@@ -908,9 +885,8 @@ bool
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
+ vpERROR_TRACE("Cannot get the power status");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
}
return status;
}
@@ -924,13 +900,12 @@ bool
\param cVe : Twist transformation.
*/
-void
- vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotViper650::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- vpViper650::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpViper650::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -944,12 +919,7 @@ void
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); }
/*!
@@ -962,29 +932,25 @@ void
end-effector frame.
*/
-void
- vpRobotViper650::get_eJe(vpMatrix &eJe)
+void vpRobotViper650::get_eJe(vpMatrix &eJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_J_Viper650(position, ×tamp) );
+ Try(PrimitiveACQ_POS_J_Viper650(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = vpMath::rad(position[i]);
- try
- {
- vpViper650::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ try {
+ vpViper650::get_eJe(q, eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
/*!
@@ -999,29 +965,25 @@ void
reference frame.
*/
-void
- vpRobotViper650::get_fJe(vpMatrix &fJe)
+void vpRobotViper650::get_fJe(vpMatrix &fJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Viper650(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Viper650(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
- try
- {
- vpViper650::get_fJe(q, fJe) ;
- }
- catch(...)
- {
+ try {
+ vpViper650::get_fJe(q, fJe);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -1037,9 +999,9 @@ void
be in ]0:100].
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1062,23 +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; }
/*!
@@ -1115,9 +1068,9 @@ double
position is out of range.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1144,7 +1097,8 @@ int main()
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1157,16 +1111,13 @@ 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 ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
vpColVector destination(njoint);
@@ -1174,86 +1125,83 @@ void
double timestamp;
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- vpColVector q(njoint);
- Try( PrimitiveACQ_POS_Viper650(q.data, ×tamp) );
-
- // Convert degrees into rad
- q.deg2rad();
-
- // Get fMc from the inverse kinematics
- vpHomogeneousMatrix fMc;
- vpViper650::get_fMc(q, fMc);
-
- // Set cMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpColVector q(njoint);
+ Try(PrimitiveACQ_POS_Viper650(q.data, ×tamp));
+
+ // Convert degrees into rad
+ q.deg2rad();
+
+ // Get fMc from the inverse kinematics
+ vpHomogeneousMatrix fMc;
+ vpViper650::get_fMc(q, fMc);
+
+ // Set cMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i + 3];
+ }
- // Compute cMc2
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- // Compute the new position to reach: fMc*cMc2
- vpHomogeneousMatrix fMc2 = fMc * cMc2;
-
- // Compute the corresponding joint position from the inverse kinematics
- unsigned int solution = this->getInverseKinematics(fMc2, q);
- if (solution) { // Position is reachable
- destination = q;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
- Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
+ // Compute cMc2
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
- break ;
- }
- case vpRobot::ARTICULAR_FRAME: {
- destination = position;
+ // Compute the new position to reach: fMc*cMc2
+ vpHomogeneousMatrix fMc2 = fMc * cMc2;
+
+ // Compute the corresponding joint position from the inverse kinematics
+ unsigned int solution = this->getInverseKinematics(fMc2, q);
+ if (solution) { // Position is reachable
+ destination = q;
// convert rad to deg requested for the low level controller
destination.rad2deg();
-
- //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
- break ;
-
+ Try(PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity));
+ Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
+ } else {
+ // Cartesian position is out of range
+ error = -1;
}
+
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ destination = position;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
+
+ // std::cout << "Joint destination (deg): " << destination.t() <<
+ // std::endl;
+ Try(PrimitiveMOVE_J_Viper650(destination.data, positioningVelocity));
+ Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- // Convert angles from Rxyz representation to Rzyz representation
- vpRxyzVector rxyz(position[3],position[4],position[5]);
- vpRotationMatrix R(rxyz);
- vpRzyzVector rzyz(R);
-
- for (unsigned int i=0; i <3; i++) {
- destination[i] = position[i];
- destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
- }
- int configuration = 0; // keep the actual configuration
+ // Convert angles from Rxyz representation to Rzyz representation
+ vpRxyzVector rxyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rxyz);
+ vpRzyzVector rzyz(R);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ destination[i] = position[i];
+ destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
+ }
+ int configuration = 0; // keep the actual configuration
- //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_C_Viper650(destination.data, configuration,
- positioningVelocity) );
- Try( WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000) );
+ // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
+ // << std::endl;
+ Try(PrimitiveMOVE_C_Viper650(destination.data, configuration, positioningVelocity));
+ Try(WaitState_Viper650(ETAT_ATTENTE_VIPER650, 1000));
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- }
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
CatchPrint();
@@ -1262,22 +1210,21 @@ void
else if (TryStt == -3019) {
if (frame == vpRobot::ARTICULAR_FRAME)
- std::cout << " : Joint position out of range.\n";
+ std::cout << " : Joint position out of range.\n";
else
- std::cout << " : Cartesian position leads to a joint position out of range.\n";
- }
- else if (TryStt < 0)
+ std::cout << " : Cartesian position leads to a joint position out of "
+ "range.\n";
+ } else if (TryStt < 0)
std::cout << " : Unknown error (see Fabien).\n";
else if (error == -1)
std::cout << "Position out of range.\n";
if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
}
- return ;
+ return;
}
/*!
@@ -1345,29 +1292,22 @@ int main()
\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) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
+ try {
+ vpColVector position(6);
+ position[0] = pos1;
+ position[1] = pos2;
+ position[2] = pos3;
+ position[3] = pos4;
+ position[4] = pos5;
+ position[5] = pos6;
+
+ setPosition(frame, position);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -1384,9 +1324,9 @@ void vpRobotViper650::setPosition (const vpRobot::vpControlFrameType frame,
This method has the same behavior than the sample code given below;
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1418,9 +1358,8 @@ void vpRobotViper650::setPosition(const std::string &filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename.c_str());
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -1451,13 +1390,13 @@ void vpRobotViper650::setPosition(const std::string &filename)
\param timestamp : Time in second since last robot power on.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -1494,63 +1433,59 @@ int main()
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;
- position.resize (6);
+ position.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- Try( PrimitiveACQ_POS_J_Viper650(position.data, ×tamp) );
- //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
- position.deg2rad();
+ case vpRobot::CAMERA_FRAME: {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ Try(PrimitiveACQ_POS_J_Viper650(position.data, ×tamp));
+ // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
+ position.deg2rad();
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveACQ_POS_C_Viper650(position.data, ×tamp) );
- // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
- // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
- // Convert Euler Rzyz angles from deg to rad
- for (unsigned int i=3; i <6; i++)
- position[i] = vpMath::rad(position[i]);
- // Convert Rzyz angles into Rxyz representation
- vpRzyzVector rzyz(position[3], position[4], position[5]);
- vpRotationMatrix R(rzyz);
- vpRxyzVector rxyz(R);
-
- // Update the position using Rxyz representation
- for (unsigned int i=0; i <3; i++)
- position[i+3] = rxyz[i];
- // vpCTRACE << "Cartesian position Rxyz (deg)"
- // << position[0] << " " << position[1] << " " << position[2] << " "
- // << vpMath::deg(position[3]) << " "
- // << vpMath::deg(position[4]) << " "
- // << vpMath::deg(position[5]) << std::endl;
-
- break ;
- }
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ Try(PrimitiveACQ_POS_C_Viper650(position.data, ×tamp));
+ // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
+ // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
+ // Convert Euler Rzyz angles from deg to rad
+ for (unsigned int i = 3; i < 6; i++)
+ position[i] = vpMath::rad(position[i]);
+ // Convert Rzyz angles into Rxyz representation
+ vpRzyzVector rzyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rzyz);
+ vpRxyzVector rxyz(R);
+
+ // Update the position using Rxyz representation
+ for (unsigned int i = 0; i < 3; i++)
+ position[i + 3] = rxyz[i];
+ // vpCTRACE << "Cartesian position Rxyz (deg)"
+ // << position[0] << " " << position[1] << " " << position[2] << " "
+ // << vpMath::deg(position[3]) << " "
+ // << vpMath::deg(position[4]) << " "
+ // << vpMath::deg(position[5]) << std::endl;
+
+ break;
+ }
case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- }
+ vpERROR_TRACE("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.");
+ vpERROR_TRACE("Cannot get position.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
}
return;
@@ -1560,13 +1495,13 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector &position)
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
@@ -1576,31 +1511,30 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &)
- The difference is here that the position is returned using a \f$ \theta {\bf u}\f$
- representation for the rotation.
+ The difference is here that the position is returned using a \f$ \theta {\bf
+ u}\f$ representation for the rotation.
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+ \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
+ // recupere position en Rxyz
this->getPosition(frame, posRxyz, timestamp);
vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
+ for (unsigned int j = 0; j < 3; j++)
+ RxyzVect[j] = posRxyz[j + 3];
+ // recupere le vecteur thetaU correspondant
vpThetaUVector RtuVect(RxyzVect);
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1608,20 +1542,21 @@ void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector
+ &, double &).
The difference is here that the timestamp is not returned.
*/
-void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
+void vpRobotViper650::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
}
/*!
- Returns the robot controller current time (in second) since last robot power on.
+ Returns the robot controller current time (in second) since last robot power
+ on.
*/
double vpRobotViper650::getTime() const
{
@@ -1634,27 +1569,33 @@ double vpRobotViper650::getTime() const
Apply a velocity to the robot.
\param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
+ could be expressed in articular, camera frame, reference frame or mixt
+frame.
\param vel : Velocity vector. Translation velocities are expressed
in m/s while rotation velocities in rad/s. The size of this vector
is always 6.
- In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities in rad/s.
- In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
+expressed in the camera frame, with translations velocities \f$ ^{c} v_x, ^{c}
+v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
+expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
+^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
+translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
+reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
+\omega_z \f$ in the camera frame in rad/s.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
@@ -1667,10 +1608,10 @@ double vpRobotViper650::getTime() const
setMaxTranslationVelocity() and setMaxRotationVelocity().
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -1702,89 +1643,85 @@ int main()
}
\endcode
*/
-void
- vpRobotViper650::setVelocity (const vpRobot::vpControlFrameType frame,
- const vpColVector & vel)
-{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- vpERROR_TRACE ("Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot send a velocity to the robot "
- "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
- }
-
+void vpRobotViper650::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel)
+{
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
vpColVector vel_sat(6);
// Velocity saturation
- switch(frame) {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- case vpRobot::MIXT_FRAME : {
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++)
- vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
+ switch (frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME:
+ case vpRobot::MIXT_FRAME: {
+ vpColVector vel_max(6);
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ for (unsigned int i = 0; i < 3; i++)
+ vel_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i = 3; i < 6; i++)
+ vel_max[i] = getMaxRotationVelocity();
- break;
- }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME : {
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+
+ break;
+ }
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME: {
vpColVector vel_max(6);
- //if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
+ // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
- for (unsigned int i=0; i<6; i++)
+ for (unsigned int i = 0; i < 6; i++)
vel_max[i] = getMaxRotationVelocity();
- }
- else {
- for (unsigned int i=0; i<5; i++)
+ } else {
+ for (unsigned int i = 0; i < 5; i++)
vel_max[i] = getMaxRotationVelocity();
vel_max[5] = getMaxRotationVelocityJoint6();
}
vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
-
}
}
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- // Send velocities in m/s and rad/s
- // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- // Convert all the velocities from rad/s into deg/s
- vel_sat.rad2deg();
- //std::cout << "Vitesse appliquee: " << vel_sat.t();
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
- Try( PrimitiveMOVESPEED_Viper650(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- // Send velocities in m/s and rad/s
- std::cout << "Vitesse ref appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- //Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
- break ;
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Send velocities in m/s and rad/s
+ // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
+ Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPCAM_VIPER650));
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ // Convert all the velocities from rad/s into deg/s
+ vel_sat.rad2deg();
+ // std::cout << "Vitesse appliquee: " << vel_sat.t();
+ // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER650) );
+ Try(PrimitiveMOVESPEED_Viper650(vel_sat.data));
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ // Send velocities in m/s and rad/s
+ std::cout << "Vitesse ref appliquee: " << vel_sat.t();
+ Try(PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPFIX_VIPER650));
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ // Try( PrimitiveMOVESPEED_CART_Viper650(vel_sat.data, REPMIX_VIPER650) );
+ break;
+ }
default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
}
Catch();
@@ -1792,19 +1729,16 @@ void
if (TryStt == VelStopOnJoint) {
UInt32 axisInJoint[njoint];
PrimitiveSTATUS_Viper650(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
+ 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 {
+ } else {
printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
if (TryString != NULL) {
// The statement is in TryString, but we need to check the validity
printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
+ } else {
printf("\n");
}
}
@@ -1813,16 +1747,10 @@ void
return;
}
-
-
-
-
-
/* ------------------------------------------------------------------------ */
/* --- GET ---------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
/*!
Get the robot velocities.
@@ -1835,16 +1763,17 @@ void
\param timestamp : Time in second since last robot power on.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is \f$ \theta {\bf u}\f$. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta u}_x, \dot {\theta u}_y, \dot {\theta u}_z]\f$.
+ of the rotation is \f$ \theta {\bf u}\f$. In that cases, \f$velocity = [\dot
+x, \dot y, \dot z, \dot {\theta u}_x, \dot {\theta u}_y, \dot {\theta
+u}_z]\f$.
\warning The first time this method is called, \e velocity is set to 0. The
first call is used to intialise the velocity computation for the next call.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1879,10 +1808,9 @@ 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.resize(6);
velocity = 0;
vpColVector q_cur(6);
@@ -1893,70 +1821,69 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_J_Viper650(q_cur.data, ×tamp) );
+ Try(PrimitiveACQ_POS_J_Viper650(q_cur.data, ×tamp));
time_cur = timestamp;
q_cur.deg2rad();
// Get the camera pose from the direct kinematics
vpViper650::get_fMc(q_cur, fMc_cur);
- if ( ! first_time_getvel ) {
+ if (!first_time_getvel) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- break ;
- }
+ break;
+ }
case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
- break ;
- }
+ velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
- break ;
- }
+ break;
+ }
case vpRobot::MIXT_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the ThetaU representation for the rotation
- vpRotationMatrix cRc;
- cMc.extract(cRc);
- vpThetaUVector thetaU;
- thetaU.buildFrom(cRc);
-
- for (unsigned int i=0; i < 3; i++) {
- // Compute the translation displacement in the reference frame
- velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
- // Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
- }
-
- // Compute the velocity
- velocity /= (time_cur - time_prev_getvel);
- break ;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the ThetaU representation for the rotation
+ vpRotationMatrix cRc;
+ cMc.extract(cRc);
+ vpThetaUVector thetaU;
+ thetaU.buildFrom(cRc);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ // Compute the translation displacement in the reference frame
+ velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
+ // Update the rotation displacement in the camera frame
+ velocity[i + 3] = thetaU[i];
}
+
+ // Compute the velocity
+ velocity /= (time_cur - time_prev_getvel);
+ break;
}
- }
- else {
+ }
+ } else {
first_time_getvel = false;
}
@@ -1969,12 +1896,10 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
// Memorize the time associated to the joint position for the next call
time_prev_getvel = time_cur;
-
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
@@ -1982,12 +1907,11 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ vpColVector &, double &) except that the timestamp is not returned.
*/
-void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
+void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
double timestamp;
getVelocity(frame, velocity, timestamp);
@@ -2005,9 +1929,9 @@ void vpRobotViper650::getVelocity(const vpRobot::vpControlFrameType frame,
and rotations in rad/s.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -2043,10 +1967,10 @@ 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);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -2055,15 +1979,15 @@ vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame, dou
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ double &) except that the timestamp is not returned.
*/
-vpColVector vpRobotViper650::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotViper650::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector velocity;
double timestamp;
- getVelocity (frame, velocity, timestamp);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -2100,11 +2024,12 @@ R: 0.1 0.3 -0.25 -80.5 80 0
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+The code below shows how to read a position from a file and move the robot to
+this position.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper650.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -2116,14 +2041,14 @@ int main()
// Get the current robot joint positions
vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
+ robot.savePosFile("current.pos", q);
// Get the position from a file and move to the registered position
robot.readPosFile("current.pos", q); // Set the joint position from the file
-
+
robot.setPositioningVelocity(5); // Positioning velocity set to 5%
robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
#endif
@@ -2137,7 +2062,7 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
{
std::ifstream fd(filename.c_str(), std::ios::in);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -2149,29 +2074,29 @@ bool vpRobotViper650::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Viper650 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -2214,12 +2139,11 @@ 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 ;
- fd = fopen(filename.c_str(), "w") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -2232,15 +2156,10 @@ bool
#\n\n");
// Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
+ 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);
}
@@ -2254,59 +2173,22 @@ bool
\sa readPosFile
*/
-void
-vpRobotViper650::move(const std::string &filename)
+void vpRobotViper650::move(const std::string &filename)
{
vpColVector q;
try {
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
- }
- catch(...) {
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+ } catch (...) {
throw;
}
}
/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
- vpRobotViper650::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper650::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
Get the robot displacement since the last call of this method.
\warning This functionnality is not implemented for the moment in the
@@ -2322,14 +2204,10 @@ void
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
-
*/
-void
- vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpRobotViper650::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
double q[6];
@@ -2339,35 +2217,34 @@ void
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Viper650(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
+ Try(PrimitiveACQ_POS_Viper650(q, ×tamp));
+ for (unsigned int i = 0; i < njoint; i++) {
q_cur[i] = q[i];
}
- if ( ! first_time_getdis ) {
+ if (!first_time_getdis) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ }
case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
+ displacement = q_cur - q_prev_getdis;
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ }
case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
}
- }
- else {
+ }
+ } else {
first_time_getdis = false;
}
@@ -2376,56 +2253,54 @@ void
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
/*!
-
+
Bias the force/torque sensor.
\warning This function waits 500 ms after the bias request to be sure the
next measures take into account the bias.
-
+
\exception vpRobotException::lowLevelError : If the force/torque sensor bias
cannot be done on the low level controller.
\sa getForceTorque()
*/
-void
- vpRobotViper650::biasForceTorqueSensor() const
+void vpRobotViper650::biasForceTorqueSensor() const
{
InitTry;
- Try( PrimitiveTFS_BIAS_Viper650() );
+ Try(PrimitiveTFS_BIAS_Viper650());
// Wait 500 ms to be sure the next measures take into account the bias
- vpTime::wait(500);
+ vpTime::wait(500);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot bias the force/torque sensor.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot bias the force/torque sensor.");
+ vpERROR_TRACE("Cannot bias the force/torque sensor.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
}
}
/*!
-
+
Get the rough force/torque sensor measures.
\param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
- The code below shows how to get the force/torque measures after a sensor bias.
+ The code below shows how to get the force/torque measures after a sensor
+bias.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -2445,7 +2320,7 @@ int main()
#endif
}
\endcode
-
+
\exception vpRobotException::lowLevelError : If the force/torque measures
cannot be get from the low level controller.
@@ -2456,15 +2331,14 @@ void vpRobotViper650::getForceTorque(vpColVector &H) const
{
InitTry;
- H.resize (6);
+ H.resize(6);
- Try( PrimitiveTFS_ACQ_Viper650(H.data) );
+ Try(PrimitiveTFS_ACQ_Viper650(H.data));
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
+ vpERROR_TRACE("Cannot get the force/torque measures.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
}
}
@@ -2474,13 +2348,14 @@ void vpRobotViper650::getForceTorque(vpColVector &H) const
\return [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
- The code below shows how to get the force/torque measures after a sensor bias.
+ The code below shows how to get the force/torque measures after a sensor
+bias.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper650.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotViper650.h>
int main()
{
@@ -2511,15 +2386,14 @@ vpColVector vpRobotViper650::getForceTorque() const
vpColVector H(6);
- Try( PrimitiveTFS_ACQ_Viper650(H.data) );
+ Try(PrimitiveTFS_ACQ_Viper650(H.data));
return H;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
+ vpERROR_TRACE("Cannot get the force/torque measures.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
}
return H; // Here to avoid a warning, but should never be called
}
@@ -2532,47 +2406,46 @@ vpColVector vpRobotViper650::getForceTorque() const
void vpRobotViper650::enableJoint6Limits() const
{
InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(0));
std::cout << "Enable joint limits on axis 6..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot enable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot enable joint limits on axis 6.");
+ vpERROR_TRACE("Cannot enable joint limits on axis 6");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
}
}
/*!
\warning Each call to this function should be done carefully.
- Disable the joint limits on axis number 6. When joint 6 is outside the limits,
- a call to this function allows to bring the robot to a position inside the limits.
- Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
+ Disable the joint limits on axis number 6. When joint 6 is outside the
+ limits, a call to this function allows to bring the robot to a position
+ inside the limits. Don't forget then to call enableJoint6Limits() to reduce
+ the working space for joint 6.
\sa enableJoint6Limits()
*/
void vpRobotViper650::disableJoint6Limits() const
{
InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper650(1));
std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot disable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot disable joint limits on axis 6.");
+ vpERROR_TRACE("Cannot disable joint limits on axis 6");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
}
}
/*!
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+ Set the maximal rotation velocity that can be sent to the robot during a
+ velocity control.
\param w_max : Maximum rotation velocity expressed in rad/s.
*/
-void
-vpRobotViper650::setMaxRotationVelocity (double w_max)
+void vpRobotViper650::setMaxRotationVelocity(double w_max)
{
vpRobot::setMaxRotationVelocity(w_max);
setMaxRotationVelocityJoint6(w_max);
@@ -2582,7 +2455,8 @@ vpRobotViper650::setMaxRotationVelocity (double w_max)
/*!
- Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+ Set the maximal rotation velocity on joint 6 that is used only during
+ velocity joint control.
This function affects only the velocities that are sent as joint velocities.
@@ -2595,12 +2469,10 @@ vpRobotViper650::setMaxRotationVelocity (double w_max)
robot.setVelocity(ARTICULAR_FRAME, v);
\endcode
-
\param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
*/
-void
-vpRobotViper650::setMaxRotationVelocityJoint6 (const double w6_max)
+void vpRobotViper650::setMaxRotationVelocityJoint6(const double w6_max)
{
maxRotationVelocity_joint6 = w6_max;
return;
@@ -2608,15 +2480,12 @@ vpRobotViper650::setMaxRotationVelocityJoint6 (const double w6_max)
/*!
- Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+ Get the maximal rotation velocity on joint 6 that is used only during
+ velocity joint control.
\return Maximum rotation velocity on joint 6 expressed in rad/s.
*/
-double
-vpRobotViper650::getMaxRotationVelocityJoint6() const
-{
- return maxRotationVelocity_joint6;
-}
+double vpRobotViper650::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
/*!
@@ -2627,12 +2496,11 @@ vpRobotViper650::getMaxRotationVelocityJoint6() const
void vpRobotViper650::openGripper()
{
InitTry;
- Try( PrimitivePneumaticGripper_Viper650(1) );
+ Try(PrimitivePneumaticGripper_Viper650(1));
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.");
}
}
@@ -2646,16 +2514,16 @@ void vpRobotViper650::openGripper()
void vpRobotViper650::closeGripper() const
{
InitTry;
- Try( PrimitivePneumaticGripper_Viper650(0) );
+ Try(PrimitivePneumaticGripper_Viper650(0));
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.");
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has no symbols
-void dummy_vpRobotViper650() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotViper650.cpp.o) has
+// no symbols
+void dummy_vpRobotViper650(){};
#endif
diff --git a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp
index 428dbfb..fc3abb1 100644
--- a/modules/robot/src/real-robot/viper/vpRobotViper850.cpp
+++ b/modules/robot/src/real-robot/viper/vpRobotViper850.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,13 +45,13 @@
#include <sys/types.h>
#include <unistd.h>
-#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/core/vpThetaUVector.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/robot/vpRobot.h>
+#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpRobotViper850.h>
/* ---------------------------------------------------------------------- */
@@ -81,24 +82,27 @@ const double vpRobotViper850::defaultPositioningVelocity = 15.0;
*/
void emergencyStopViper850(int signo)
{
- std::cout << "Stop the Viper850 application by signal ("
- << signo << "): " << (char)7 ;
- switch(signo)
- {
+ std::cout << "Stop the Viper850 application by signal (" << signo << "): " << (char)7;
+ switch (signo) {
case SIGINT:
- std::cout << "SIGINT (stop by ^C) " << std::endl ; break ;
+ std::cout << "SIGINT (stop by ^C) " << std::endl;
+ break;
case SIGBUS:
- std::cout <<"SIGBUS (stop due to a bus error) " << std::endl ; break ;
+ std::cout << "SIGBUS (stop due to a bus error) " << std::endl;
+ break;
case SIGSEGV:
- std::cout <<"SIGSEGV (stop due to a segmentation fault) " << std::endl ; break ;
+ std::cout << "SIGSEGV (stop due to a segmentation fault) " << std::endl;
+ break;
case SIGKILL:
- std::cout <<"SIGKILL (stop by CTRL \\) " << std::endl ; break ;
+ std::cout << "SIGKILL (stop by CTRL \\) " << std::endl;
+ break;
case SIGQUIT:
- std::cout <<"SIGQUIT " << std::endl ; break ;
- default :
- std::cout << signo << std::endl ;
-}
- //std::cout << "Emergency stop called\n";
+ std::cout << "SIGQUIT " << std::endl;
+ break;
+ default:
+ std::cout << signo << std::endl;
+ }
+ // std::cout << "Emergency stop called\n";
// PrimitiveESTOP_Viper850();
PrimitiveSTOP_Viper850();
std::cout << "Robot was stopped\n";
@@ -109,7 +113,7 @@ void emergencyStopViper850(int signo)
fprintf(stdout, "Application ");
fflush(stdout);
kill(getpid(), SIGKILL);
- exit(1) ;
+ exit(1);
}
/* ---------------------------------------------------------------------- */
@@ -131,21 +135,20 @@ void emergencyStopViper850(int signo)
including the distorsion, use the code below:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // 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
@@ -171,10 +174,7 @@ int main()
vpCameraParameters::vpCameraParametersProjType)
*/
-vpRobotViper850::vpRobotViper850 (bool verbose)
- :
- vpViper850 (),
- vpRobot ()
+vpRobotViper850::vpRobotViper850(bool verbose) : vpViper850(), vpRobot()
{
/*
@@ -197,8 +197,8 @@ vpRobotViper850::vpRobotViper850 (bool verbose)
*/
signal(SIGINT, emergencyStopViper850);
- signal(SIGBUS, emergencyStopViper850) ;
- signal(SIGSEGV, emergencyStopViper850) ;
+ signal(SIGBUS, emergencyStopViper850);
+ signal(SIGSEGV, emergencyStopViper850);
signal(SIGKILL, emergencyStopViper850);
signal(SIGQUIT, emergencyStopViper850);
@@ -207,22 +207,20 @@ vpRobotViper850::vpRobotViper850 (bool verbose)
std::cout << "Open communication with MotionBlox.\n";
try {
this->init();
- this->setRobotState(vpRobot::STATE_STOP) ;
- }
- catch(...) {
+ this->setRobotState(vpRobot::STATE_STOP);
+ } catch (...) {
// vpERROR_TRACE("Error caught") ;
- throw ;
+ throw;
}
- positioningVelocity = defaultPositioningVelocity ;
+ positioningVelocity = defaultPositioningVelocity;
maxRotationVelocity_joint6 = maxRotationVelocity;
vpRobotViper850::robotAlreadyCreated = true;
- return ;
+ return;
}
-
/* ------------------------------------------------------------------------ */
/* --- INITIALISATION ----------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -240,17 +238,18 @@ vpRobotViper850::vpRobotViper850 (bool verbose)
init(vpViper850::defaultCameraRobot). If you want to set the extrinsic
camera parameters to those obtained with a camera perspective model
including the distorsion you have to call the
- init(vpViper850::vpViper850CameraRobotType, vpCameraParameters::vpCameraParametersProjType) method.
- If you want to set custom extrinsic camera parameters you have to call
- the init(vpViper850::vpToolType, const vpHomogeneousMatrix&) method.
+ init(vpViper850::vpViper850CameraRobotType,
+ vpCameraParameters::vpCameraParametersProjType) method. If you want to set
+ custom extrinsic camera parameters you have to call the
+ init(vpViper850::vpToolType, const vpHomogeneousMatrix&) method.
- \sa vpCameraParameters, init(vpViper850::vpToolType, vpCameraParameters::vpCameraParametersProjType),
+ \sa vpCameraParameters, init(vpViper850::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
init(vpViper850::vpToolType, const vpHomogeneousMatrix&),
init(vpViper850::vpToolType, const std::string&)
*/
-void
-vpRobotViper850::init (void)
+void vpRobotViper850::init(void)
{
InitTry;
@@ -267,27 +266,29 @@ vpRobotViper850::init (void)
#if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
std::string calibfile;
-# ifdef VISP_HAVE_VIPER850_DATA
+#ifdef VISP_HAVE_VIPER850_DATA
calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
- if (! vpIoTools::checkFilename(calibfile))
+ if (!vpIoTools::checkFilename(calibfile))
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 file"));
-# endif
+#else
+ throw(vpException(vpException::ioError, "You don't have access to Viper850 "
+ "data to retrive ATI F/T calib "
+ "file"));
+#endif
ati.setCalibrationFile(calibfile);
ati.open();
#endif
// Initialize the firewire connection
- Try( InitializeConnection(verbose_) );
+ Try(InitializeConnection(verbose_));
// Connect to the servoboard using the servo board GUID
- Try( InitializeNode_Viper850() );
+ Try(InitializeNode_Viper850());
- Try( PrimitiveRESET_Viper850() );
+ Try(PrimitiveRESET_Viper850());
// Enable the joint limits on axis 6
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
// Update the eMc matrix in the low level controller
init(vpViper850::defaultTool);
@@ -295,14 +296,13 @@ 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
if (verbose_) {
std::cout << "Robot status: ";
- switch(EStopStatus) {
+ switch (EStopStatus) {
case ESTOP_AUTO:
controlMode = AUTO;
if (HIPowerStatus == 0)
@@ -330,23 +330,25 @@ vpRobotViper850::init (void)
std::cout << std::endl;
}
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// If an error occur in the low level controller, goto here
- //CatchPrint();
+ // CatchPrint();
Catch();
// Test if an error occurs
if (TryStt == -20001)
printf("No connection detected. Check if the robot is powered on \n"
- "and if the firewire link exist between the MotionBlox and this computer.\n");
+ "and if the firewire link exist between the MotionBlox and this "
+ "computer.\n");
else if (TryStt == -675)
printf(" Timeout enabling power...\n");
@@ -357,10 +359,9 @@ 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 ;
+ return;
}
/*!
@@ -381,21 +382,20 @@ vpRobotViper850::init (void)
including the distorsion, use the code below:
\code
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpImage.h>
+#include <visp3/robot/vpRobotViper850.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpCameraParameters.h>
int main()
{
#ifdef VISP_HAVE_VIPER850
vpRobotViper850 robot;
- // Set the extrinsic camera parameters obtained with a perpective
+ // 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
@@ -421,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
@@ -433,26 +431,27 @@ vpRobotViper850::init (vpViper850::vpToolType tool,
InitTry;
// get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
+ Try(PrimitiveCONST_Viper850(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/*!
@@ -505,34 +504,34 @@ 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);
InitTry;
// Get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
+ Try(PrimitiveCONST_Viper850(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/*!
@@ -571,34 +570,34 @@ 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_);
InitTry;
// Get real joint min/max from the MotionBlox
- Try( PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data) );
+ Try(PrimitiveJOINT_MINMAX_Viper850(joint_min.data, joint_max.data));
// Convert units from degrees to radians
joint_min.deg2rad();
joint_max.deg2rad();
// for (unsigned int i=0; i < njoint; i++) {
- // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i], joint_max[i]);
+ // printf("axis %d: joint min %lf, max %lf\n", i, joint_min[i],
+ // joint_max[i]);
// }
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
+ Try(PrimitiveCONST_Viper850(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/*!
@@ -612,8 +611,7 @@ vpRobotViper850::init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eM
\param eMc_ : Transformation between the end-effector frame
and the tool frame.
*/
-void
-vpRobotViper850::set_eMc(const vpHomogeneousMatrix &eMc_)
+void vpRobotViper850::set_eMc(const vpHomogeneousMatrix &eMc_)
{
this->vpViper850::set_eMc(eMc_);
@@ -621,16 +619,16 @@ vpRobotViper850::set_eMc(const vpHomogeneousMatrix &eMc_)
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
+ Try(PrimitiveCONST_Viper850(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/*!
@@ -645,25 +643,24 @@ 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_);
+ this->vpViper850::set_eMc(etc_, erc_);
InitTry;
// Set the camera constant (eMc pose) in the MotionBlox
double eMc_pose[6];
- for (unsigned int i=0; i < 3; i ++) {
- eMc_pose[i] = etc[i]; // translation in meters
- eMc_pose[i+3] = erc[i]; // rotation in rad
+ for (unsigned int i = 0; i < 3; i++) {
+ eMc_pose[i] = etc[i]; // translation in meters
+ eMc_pose[i + 3] = erc[i]; // rotation in rad
}
// Update the eMc pose in the low level controller
- Try( PrimitiveCONST_Viper850(eMc_pose) );
+ Try(PrimitiveCONST_Viper850(eMc_pose));
CatchPrint();
- return ;
+ return;
}
/* ------------------------------------------------------------------------ */
@@ -676,7 +673,7 @@ vpRobotViper850::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &er
Free allocated resources.
*/
-vpRobotViper850::~vpRobotViper850 (void)
+vpRobotViper850::~vpRobotViper850(void)
{
#if defined(USE_ATI_DAQ) && defined(VISP_HAVE_COMEDI)
ati.close();
@@ -684,12 +681,11 @@ vpRobotViper850::~vpRobotViper850 (void)
InitTry;
- setRobotState(vpRobot::STATE_STOP) ;
+ setRobotState(vpRobot::STATE_STOP);
// 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) {
@@ -708,57 +704,52 @@ vpRobotViper850::~vpRobotViper850 (void)
return;
}
-
-
-
/*!
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;
switch (newState) {
case vpRobot::STATE_STOP: {
- // Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- Try( PrimitiveSTOP_Viper850() );
- vpTime::sleepMs(100); // needed to ensure velocity task ends up on low level
- }
- break;
+ // 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
}
+ break;
+ }
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
- std::cout << "Change the control mode from velocity to position control.\n";
- Try( PrimitiveSTOP_Viper850() );
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
- }
- this->powerOn();
- break;
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
+ std::cout << "Change the control mode from velocity to position control.\n";
+ Try(PrimitiveSTOP_Viper850());
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
+ this->powerOn();
+ break;
+ }
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- std::cout << "Change the control mode from stop to velocity control.\n";
- }
- this->powerOn();
- break;
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ std::cout << "Change the control mode from stop to velocity control.\n";
}
+ this->powerOn();
+ break;
+ }
default:
- break ;
+ break;
}
CatchPrint();
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
-
/* ------------------------------------------------------------------------ */
/* --- STOP --------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
@@ -770,21 +761,19 @@ vpRobot::vpRobotStateType
\exception vpRobotException::lowLevelError : If the low level
controller returns an error during robot stopping.
*/
-void
- vpRobotViper850::stopMotion(void)
+void vpRobotViper850::stopMotion(void)
{
if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
return;
InitTry;
- Try( PrimitiveSTOP_Viper850() );
- setRobotState (vpRobot::STATE_STOP);
+ Try(PrimitiveSTOP_Viper850());
+ setRobotState(vpRobot::STATE_STOP);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot stop robot motion");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot stop robot motion.");
+ vpERROR_TRACE("Cannot stop robot motion");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot stop robot motion.");
}
}
@@ -797,8 +786,7 @@ void
\sa powerOff(), getPowerState()
*/
-void
- vpRobotViper850::powerOn(void)
+void vpRobotViper850::powerOn(void)
{
InitTry;
@@ -808,29 +796,27 @@ void
bool firsttime = true;
unsigned int nitermax = 10;
- for (unsigned int i=0; i<nitermax; i++) {
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL,
- &HIPowerStatus));
+ for (unsigned int i = 0; i < nitermax; i++) {
+ Try(PrimitiveSTATUS_Viper850(NULL, NULL, &EStopStatus, NULL, NULL, NULL, &HIPowerStatus));
if (EStopStatus == ESTOP_AUTO) {
controlMode = AUTO;
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_MANUAL) {
+ } else if (EStopStatus == ESTOP_MANUAL) {
controlMode = MANUAL;
break; // exit for loop
- }
- else if (EStopStatus == ESTOP_ACTIVATED) {
+ } else if (EStopStatus == ESTOP_ACTIVATED) {
controlMode = ESTOP;
if (firsttime) {
std::cout << "Emergency stop is activated! \n"
- << "Check the emergency stop button and push the yellow button before continuing." << std::endl;
+ << "Check the emergency stop button and push the yellow "
+ "button before continuing."
+ << std::endl;
firsttime = false;
}
- fprintf(stdout, "Remaining time %us \r", nitermax-i);
+ fprintf(stdout, "Remaining time %us \r", nitermax - i);
fflush(stdout);
CAL_Wait(1);
- }
- else {
+ } else {
std::cout << "Sorry there is an error on the emergency chain." << std::endl;
std::cout << "You have to call Adept for maintenance..." << std::endl;
// Free allocated resources
@@ -844,22 +830,20 @@ 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) {
fprintf(stdout, "Power ON the Viper850 robot\n");
fflush(stdout);
-
- Try( PrimitivePOWERON_Viper850() );
+
+ Try(PrimitivePOWERON_Viper850());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power on the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power on the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -872,29 +856,26 @@ void
\sa powerOn(), getPowerState()
*/
-void
- vpRobotViper850::powerOff(void)
+void vpRobotViper850::powerOff(void)
{
InitTry;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
fprintf(stdout, "Power OFF the Viper850 robot\n");
fflush(stdout);
- Try( PrimitivePOWEROFF_Viper850() );
+ Try(PrimitivePOWEROFF_Viper850());
}
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot power off the robot");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot power off the robot.");
+ vpERROR_TRACE("Cannot power off the robot");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot power off the robot.");
}
}
@@ -909,15 +890,13 @@ void
\sa powerOn(), powerOff()
*/
-bool
- vpRobotViper850::getPowerState(void) const
+bool vpRobotViper850::getPowerState(void) const
{
InitTry;
bool status = false;
// Look if the power is on or off
UInt32 HIPowerStatus;
- Try( PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL,
- &HIPowerStatus));
+ Try(PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, NULL, &HIPowerStatus));
CAL_Wait(0.1);
if (HIPowerStatus == 1) {
@@ -926,9 +905,8 @@ bool
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the power status");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get the power status.");
+ vpERROR_TRACE("Cannot get the power status");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get the power status.");
}
return status;
}
@@ -942,13 +920,12 @@ bool
\param cVe : Twist transformation.
*/
-void
- vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotViper850::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- vpViper850::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpViper850::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -962,12 +939,7 @@ void
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); }
/*!
@@ -980,29 +952,25 @@ void
end-effector frame.
*/
-void
- vpRobotViper850::get_eJe(vpMatrix &eJe)
+void vpRobotViper850::get_eJe(vpMatrix &eJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_J_Viper850(position, ×tamp) );
+ Try(PrimitiveACQ_POS_J_Viper850(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = vpMath::rad(position[i]);
- try
- {
- vpViper850::get_eJe(q, eJe) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ try {
+ vpViper850::get_eJe(q, eJe);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
/*!
@@ -1017,29 +985,25 @@ void
reference frame.
*/
-void
- vpRobotViper850::get_fJe(vpMatrix &fJe)
+void vpRobotViper850::get_fJe(vpMatrix &fJe)
{
double position[6];
double timestamp;
InitTry;
- Try( PrimitiveACQ_POS_Viper850(position, ×tamp) );
+ Try(PrimitiveACQ_POS_Viper850(position, ×tamp));
CatchPrint();
vpColVector q(6);
- for (unsigned int i=0; i < njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
q[i] = position[i];
- try
- {
- vpViper850::get_fJe(q, fJe) ;
- }
- catch(...)
- {
+ try {
+ vpViper850::get_fJe(q, fJe);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -1055,9 +1019,9 @@ void
be in ]0:100].
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1080,23 +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; }
/*!
@@ -1133,9 +1088,9 @@ double
position is out of range.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1162,7 +1117,8 @@ int main()
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1175,16 +1131,13 @@ 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 ())
- {
- vpERROR_TRACE ("Robot was not in position-based control\n"
- "Modification of the robot state");
- setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
vpColVector destination(njoint);
@@ -1192,86 +1145,83 @@ void
double timestamp;
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- vpColVector q(njoint);
- Try( PrimitiveACQ_POS_Viper850(q.data, ×tamp) );
-
- // Convert degrees into rad
- q.deg2rad();
-
- // Get fMc from the inverse kinematics
- vpHomogeneousMatrix fMc;
- vpViper850::get_fMc(q, fMc);
-
- // Set cMc from the input position
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++) {
- txyz[i] = position[i];
- rxyz[i] = position[i+3];
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpColVector q(njoint);
+ Try(PrimitiveACQ_POS_Viper850(q.data, ×tamp));
+
+ // Convert degrees into rad
+ q.deg2rad();
+
+ // Get fMc from the inverse kinematics
+ vpHomogeneousMatrix fMc;
+ vpViper850::get_fMc(q, fMc);
+
+ // Set cMc from the input position
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = position[i];
+ rxyz[i] = position[i + 3];
+ }
- // Compute cMc2
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- // Compute the new position to reach: fMc*cMc2
- vpHomogeneousMatrix fMc2 = fMc * cMc2;
-
- // Compute the corresponding joint position from the inverse kinematics
- unsigned int solution = this->getInverseKinematics(fMc2, q);
- if (solution) { // Position is reachable
- destination = q;
- // convert rad to deg requested for the low level controller
- destination.rad2deg();
- Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000) );
- }
- else {
- // Cartesian position is out of range
- error = -1;
- }
+ // Compute cMc2
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
- break ;
- }
- case vpRobot::ARTICULAR_FRAME: {
- destination = position;
+ // Compute the new position to reach: fMc*cMc2
+ vpHomogeneousMatrix fMc2 = fMc * cMc2;
+
+ // Compute the corresponding joint position from the inverse kinematics
+ unsigned int solution = this->getInverseKinematics(fMc2, q);
+ if (solution) { // Position is reachable
+ destination = q;
// convert rad to deg requested for the low level controller
destination.rad2deg();
+ Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
+ Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
+ } else {
+ // Cartesian position is out of range
+ error = -1;
+ }
- //std::cout << "Joint destination (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000) );
- break ;
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ destination = position;
+ // convert rad to deg requested for the low level controller
+ destination.rad2deg();
- }
+ // std::cout << "Joint destination (deg): " << destination.t() <<
+ // std::endl;
+ Try(PrimitiveMOVE_J_Viper850(destination.data, positioningVelocity));
+ Try(WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000));
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- // Convert angles from Rxyz representation to Rzyz representation
- vpRxyzVector rxyz(position[3],position[4],position[5]);
- vpRotationMatrix R(rxyz);
- vpRzyzVector rzyz(R);
-
- for (unsigned int i=0; i <3; i++) {
- destination[i] = position[i];
- destination[i+3] = vpMath::deg(rzyz[i]); // convert also angles in deg
- }
- int configuration = 0; // keep the actual configuration
+ // Convert angles from Rxyz representation to Rzyz representation
+ vpRxyzVector rxyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rxyz);
+ vpRzyzVector rzyz(R);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ destination[i] = position[i];
+ destination[i + 3] = vpMath::deg(rzyz[i]); // convert also angles in deg
+ }
+ int configuration = 0; // keep the actual configuration
- //std::cout << "Base frame destination Rzyz (deg): " << destination.t() << std::endl;
- Try( PrimitiveMOVE_C_Viper850(destination.data, configuration,
- positioningVelocity) );
- Try( WaitState_Viper850(ETAT_ATTENTE_VIPER850, 1000) );
+ // std::cout << "Base frame destination Rzyz (deg): " << destination.t()
+ // << std::endl;
+ 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.");
- }
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
CatchPrint();
@@ -1279,22 +1229,21 @@ void
std::cout << " : Position out of range.\n";
else if (TryStt == -3019) {
if (frame == vpRobot::ARTICULAR_FRAME)
- std::cout << " : Joint position out of range.\n";
+ std::cout << " : Joint position out of range.\n";
else
- std::cout << " : Cartesian position leads to a joint position out of range.\n";
- }
- else if (TryStt < 0)
+ std::cout << " : Cartesian position leads to a joint position out of "
+ "range.\n";
+ } else if (TryStt < 0)
std::cout << " : Unknown error (see Fabien).\n";
else if (error == -1)
std::cout << "Position out of range.\n";
if (TryStt < 0 || error < 0) {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
}
- return ;
+ return;
}
/*!
@@ -1362,29 +1311,22 @@ int main()
\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) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
+ try {
+ vpColVector position(6);
+ position[0] = pos1;
+ position[1] = pos2;
+ position[2] = pos3;
+ position[3] = pos4;
+ position[4] = pos5;
+ position[5] = pos6;
+
+ setPosition(frame, position);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
@@ -1401,9 +1343,9 @@ void vpRobotViper850::setPosition (const vpRobot::vpControlFrameType frame,
This method has the same behavior than the sample code given below;
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1435,9 +1377,8 @@ void vpRobotViper850::setPosition(const std::string &filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename.c_str());
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename.c_str());
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -1468,13 +1409,13 @@ void vpRobotViper850::setPosition(const std::string &filename)
\param timestamp : Time in second since last robot power on.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -1511,69 +1452,67 @@ int main()
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;
- position.resize (6);
+ position.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME : {
- position = 0;
- return;
- }
- case vpRobot::ARTICULAR_FRAME : {
- Try( PrimitiveACQ_POS_J_Viper850(position.data, ×tamp) );
- //vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
- position.deg2rad();
+ case vpRobot::CAMERA_FRAME: {
+ position = 0;
+ return;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ Try(PrimitiveACQ_POS_J_Viper850(position.data, ×tamp));
+ // vpCTRACE << "Get joint position (deg)" << position.t() << std::endl;
+ position.deg2rad();
- return;
- }
- case vpRobot::REFERENCE_FRAME : {
- Try( PrimitiveACQ_POS_C_Viper850(position.data, ×tamp) );
- // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
- // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
- // Convert Euler Rzyz angles from deg to rad
- for (unsigned int i=3; i <6; i++)
- position[i] = vpMath::rad(position[i]);
- // Convert Rzyz angles into Rxyz representation
- vpRzyzVector rzyz(position[3], position[4], position[5]);
- vpRotationMatrix R(rzyz);
- vpRxyzVector rxyz(R);
-
- // Update the position using Rxyz representation
- for (unsigned int i=0; i <3; i++)
- position[i+3] = rxyz[i];
- // vpCTRACE << "Cartesian position Rxyz (deg)"
- // << position[0] << " " << position[1] << " " << position[2] << " "
- // << vpMath::deg(position[3]) << " "
- // << vpMath::deg(position[4]) << " "
- // << vpMath::deg(position[5]) << std::endl;
-
- break ;
- }
+ return;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ Try(PrimitiveACQ_POS_C_Viper850(position.data, ×tamp));
+ // vpCTRACE << "Get cartesian position " << position.t() << std::endl;
+ // 1=tx, 2=ty, 3=tz in meters; 4=Rz 5=Ry 6=Rz in deg
+ // Convert Euler Rzyz angles from deg to rad
+ for (unsigned int i = 3; i < 6; i++)
+ position[i] = vpMath::rad(position[i]);
+ // Convert Rzyz angles into Rxyz representation
+ vpRzyzVector rzyz(position[3], position[4], position[5]);
+ vpRotationMatrix R(rzyz);
+ vpRxyzVector rxyz(R);
+
+ // Update the position using Rxyz representation
+ for (unsigned int i = 0; i < 3; i++)
+ position[i + 3] = rxyz[i];
+ // vpCTRACE << "Cartesian position Rxyz (deg)"
+ // << position[0] << " " << position[1] << " " << position[2] << " "
+ // << vpMath::deg(position[3]) << " "
+ // << vpMath::deg(position[4]) << " "
+ // << vpMath::deg(position[5]) << std::endl;
+
+ break;
+ }
case vpRobot::MIXT_FRAME: {
- vpERROR_TRACE ("Cannot get position in mixt frame: not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get position in mixt frame: "
- "not implemented");
- }
+ vpERROR_TRACE("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.");
+ vpERROR_TRACE("Cannot get position.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get position.");
}
return;
}
/*!
- Returns the robot controller current time (in second) since last robot power on.
+ Returns the robot controller current time (in second) since last robot power
+ on.
*/
double vpRobotViper850::getTime() const
{
@@ -1586,13 +1525,13 @@ double vpRobotViper850::getTime() const
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpColVector & position)
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &position)
{
double timestamp;
getPosition(frame, position, timestamp);
@@ -1602,30 +1541,28 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &).
- The difference is here that the position is returned using a \f$ \theta {\bf u}\f$
- representation.
+ The difference is here that the position is returned using a \f$ \theta {\bf
+ 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
+ // recupere position en Rxyz
this->getPosition(frame, posRxyz, timestamp);
vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
+ for (unsigned int j = 0; j < 3; j++)
+ RxyzVect[j] = posRxyz[j + 3];
+ // recupere le vecteur thetaU correspondant
vpThetaUVector RtuVect(RxyzVect);
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1633,29 +1570,28 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &, double &).
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector
+ &, double &).
The difference is here that the timestamp is not used.
*/
-void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
+void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
vpColVector posRxyz;
double timestamp;
- //recupere position en Rxyz
+ // recupere position en Rxyz
this->getPosition(frame, posRxyz, timestamp);
vpRxyzVector RxyzVect;
- for (unsigned int j=0;j<3;j++)
- RxyzVect[j]=posRxyz[j+3];
- //recupere le vecteur thetaU correspondant
+ for (unsigned int j = 0; j < 3; j++)
+ RxyzVect[j] = posRxyz[j + 3];
+ // recupere le vecteur thetaU correspondant
vpThetaUVector RtuVect(RxyzVect);
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for (unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1663,27 +1599,33 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
Apply a velocity to the robot.
\param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
+ could be expressed in articular, camera frame, reference frame or mixt
+frame.
\param vel : Velocity vector. Translation velocities are expressed
in m/s while rotation velocities in rad/s. The size of this vector
is always 6.
- In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities in rad/s.
- In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
+expressed in the camera frame, with translations velocities \f$ ^{c} v_x, ^{c}
+v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
+expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
+^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
+translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
+reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
+\omega_z \f$ in the camera frame in rad/s.
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
@@ -1696,10 +1638,10 @@ void vpRobotViper850::getPosition(const vpRobot::vpControlFrameType frame,
setMaxTranslationVelocity() and setMaxRotationVelocity().
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -1731,88 +1673,86 @@ 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) ");
- }
-
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
vpColVector vel_sat(6);
// Velocity saturation
- switch(frame) {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- case vpRobot::MIXT_FRAME : {
- vpColVector vel_max(6);
-
- for (unsigned int i=0; i<3; i++)
- vel_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
+ switch (frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME:
+ case vpRobot::MIXT_FRAME: {
+ vpColVector vel_max(6);
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ for (unsigned int i = 0; i < 3; i++)
+ vel_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i = 3; i < 6; i++)
+ vel_max[i] = getMaxRotationVelocity();
- break;
- }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME : {
- vpColVector vel_max(6);
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
- //if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
- if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
+ break;
+ }
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME: {
+ vpColVector vel_max(6);
- for (unsigned int i=0; i<6; i++)
- vel_max[i] = getMaxRotationVelocity();
- }
- else {
- for (unsigned int i=0; i<5; i++)
- vel_max[i] = getMaxRotationVelocity();
- vel_max[5] = getMaxRotationVelocityJoint6();
- }
+ // if (getMaxRotationVelocity() == getMaxRotationVelocityJoint6()) {
+ if (std::fabs(getMaxRotationVelocity() - getMaxRotationVelocityJoint6()) < std::numeric_limits<double>::epsilon()) {
- vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ for (unsigned int i = 0; i < 6; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ } else {
+ for (unsigned int i = 0; i < 5; i++)
+ vel_max[i] = getMaxRotationVelocity();
+ vel_max[5] = getMaxRotationVelocityJoint6();
}
+
+ vel_sat = vpRobot::saturateVelocities(vel, vel_max, true);
+ }
}
InitTry;
- switch(frame) {
- case vpRobot::CAMERA_FRAME : {
- // Send velocities in m/s and rad/s
- // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850) );
- break ;
- }
- case vpRobot::ARTICULAR_FRAME : {
- // Convert all the velocities from rad/s into deg/s
- vel_sat.rad2deg();
- //std::cout << "Vitesse appliquee: " << vel_sat.t();
- //Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
- Try( PrimitiveMOVESPEED_Viper850(vel_sat.data) );
- break ;
- }
- case vpRobot::REFERENCE_FRAME : {
- // Send velocities in m/s and rad/s
- std::cout << "Vitesse ref appliquee: " << vel_sat.t();
- Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850) );
- break ;
- }
- case vpRobot::MIXT_FRAME : {
- //Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
- break ;
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ // Send velocities in m/s and rad/s
+ // std::cout << "Vitesse cam appliquee: " << vel_sat.t();
+ Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPCAM_VIPER850));
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ // Convert all the velocities from rad/s into deg/s
+ vel_sat.rad2deg();
+ // std::cout << "Vitesse appliquee: " << vel_sat.t();
+ // Try( PrimitiveMOVESPEED_CART(vel_sat.data, REPART_VIPER850) );
+ Try(PrimitiveMOVESPEED_Viper850(vel_sat.data));
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ // Send velocities in m/s and rad/s
+ std::cout << "Vitesse ref appliquee: " << vel_sat.t();
+ Try(PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPFIX_VIPER850));
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ // Try( PrimitiveMOVESPEED_CART_Viper850(vel_sat.data, REPMIX_VIPER850) );
+ break;
+ }
default: {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
}
Catch();
@@ -1820,19 +1760,16 @@ void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame,
if (TryStt == VelStopOnJoint) {
UInt32 axisInJoint[njoint];
PrimitiveSTATUS_Viper850(NULL, NULL, NULL, NULL, NULL, axisInJoint, NULL);
- for (unsigned int i=0; i < njoint; i ++) {
+ 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 {
+ } else {
printf("\n%s(%d): Error %d", __FUNCTION__, TryLine, TryStt);
if (TryString != NULL) {
// The statement is in TryString, but we need to check the validity
printf(" Error sentence %s\n", TryString); // Print the TryString
- }
- else {
+ } else {
printf("\n");
}
}
@@ -1841,16 +1778,10 @@ void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame,
return;
}
-
-
-
-
-
/* ------------------------------------------------------------------------ */
/* --- GET ---------------------------------------------------------------- */
/* ------------------------------------------------------------------------ */
-
/*!
Get the robot velocities.
@@ -1863,16 +1794,17 @@ void vpRobotViper850::setVelocity(const vpRobot::vpControlFrameType frame,
\param timestamp : Time in second since last robot power on.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is \f$ \theta {\bf u}\f$. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta u}_x, \dot {\theta u}_y, \dot {\theta u}_z]\f$.
+ of the rotation is \f$ \theta {\bf u}\f$. In that cases, \f$velocity = [\dot
+x, \dot y, \dot z, \dot {\theta u}_x, \dot {\theta u}_y, \dot {\theta
+u}_z]\f$.
\warning The first time this method is called, \e velocity is set to 0. The
first call is used to intialise the velocity computation for the next call.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1907,11 +1839,10 @@ 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);
+ velocity.resize(6);
velocity = 0;
vpColVector q_cur(6);
@@ -1922,71 +1853,69 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp) );
+ Try(PrimitiveACQ_POS_J_Viper850(q_cur.data, ×tamp));
time_cur = timestamp;
q_cur.deg2rad();
// Get the camera pose from the direct kinematics
vpViper850::get_fMc(q_cur, fMc_cur);
- if ( ! first_time_getvel ) {
+ if (!first_time_getvel) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
- // Compute the velocity of the camera from this displacement
- velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+ // Compute the velocity of the camera from this displacement
+ velocity = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- break ;
- }
+ break;
+ }
case vpRobot::ARTICULAR_FRAME: {
- velocity = (q_cur - q_prev_getvel)
- / (time_cur - time_prev_getvel);
- break ;
- }
+ velocity = (q_cur - q_prev_getvel) / (time_cur - time_prev_getvel);
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
- // Compute the velocity of the camera from this displacement
- vpColVector v;
- v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
+ // Compute the velocity of the camera from this displacement
+ vpColVector v;
+ v = vpExponentialMap::inverse(cMc, time_cur - time_prev_getvel);
- // Express this velocity in the reference frame
- vpVelocityTwistMatrix fVc(fMc_cur);
- velocity = fVc * v;
+ // Express this velocity in the reference frame
+ vpVelocityTwistMatrix fVc(fMc_cur);
+ velocity = fVc * v;
- break ;
- }
+ break;
+ }
case vpRobot::MIXT_FRAME: {
- // Compute the displacement of the camera since the previous call
- cMc = fMc_prev_getvel.inverse() * fMc_cur;
-
- // Compute the ThetaU representation for the rotation
- vpRotationMatrix cRc;
- cMc.extract(cRc);
- vpThetaUVector thetaU;
- thetaU.buildFrom(cRc);
-
- for (unsigned int i=0; i < 3; i++) {
- // Compute the translation displacement in the reference frame
- velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
- // Update the rotation displacement in the camera frame
- velocity[i+3] = thetaU[i];
- }
-
- // Compute the velocity
- velocity /= (time_cur - time_prev_getvel);
- break ;
+ // Compute the displacement of the camera since the previous call
+ cMc = fMc_prev_getvel.inverse() * fMc_cur;
+
+ // Compute the ThetaU representation for the rotation
+ vpRotationMatrix cRc;
+ cMc.extract(cRc);
+ vpThetaUVector thetaU;
+ thetaU.buildFrom(cRc);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ // Compute the translation displacement in the reference frame
+ velocity[i] = fMc_prev_getvel[i][3] - fMc_cur[i][3];
+ // Update the rotation displacement in the camera frame
+ velocity[i + 3] = thetaU[i];
}
+
+ // Compute the velocity
+ velocity /= (time_cur - time_prev_getvel);
+ break;
}
- }
- else {
+ }
+ } else {
first_time_getvel = false;
}
@@ -1999,12 +1928,10 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
// Memorize the time associated to the joint position for the next call
time_prev_getvel = time_cur;
-
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
@@ -2012,12 +1939,11 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, vpColVector &, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ vpColVector &, double &) except that the timestamp is not returned.
*/
-void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
- vpColVector & velocity)
+void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &velocity)
{
double timestamp;
getVelocity(frame, velocity, timestamp);
@@ -2035,9 +1961,9 @@ void vpRobotViper850::getVelocity(const vpRobot::vpControlFrameType frame,
and rotations in rad/s.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -2073,10 +1999,10 @@ 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);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -2085,15 +2011,15 @@ vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame, dou
Get robot velocities.
- The behavior is the same than getVelocity(const vpRobot::vpControlFrameType, double &)
- except that the timestamp is not returned.
+ The behavior is the same than getVelocity(const vpRobot::vpControlFrameType,
+ double &) except that the timestamp is not returned.
*/
-vpColVector vpRobotViper850::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpRobotViper850::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector velocity;
double timestamp;
- getVelocity (frame, velocity, timestamp);
+ getVelocity(frame, velocity, timestamp);
return velocity;
}
@@ -2130,11 +2056,12 @@ R: 0.1 0.3 -0.25 -80.5 80 0
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+The code below shows how to read a position from a file and move the robot to
+this position.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -2146,14 +2073,14 @@ int main()
// Get the current robot joint positions
vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
+ robot.savePosFile("current.pos", q);
// Get the position from a file and move to the registered position
robot.readPosFile("current.pos", q); // Set the joint position from the file
-
+
robot.setPositioningVelocity(5); // Positioning velocity set to 5%
robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
#endif
@@ -2167,7 +2094,7 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
{
std::ifstream fd(filename.c_str(), std::ios::in);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -2179,29 +2106,29 @@ bool vpRobotViper850::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -2244,12 +2171,11 @@ 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 ;
- fd = fopen(filename.c_str(), "w") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -2262,15 +2188,10 @@ bool
#\n\n");
// Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
+ 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);
}
@@ -2284,59 +2205,22 @@ bool
\sa readPosFile
*/
-void
- vpRobotViper850::move(const std::string &filename)
+void vpRobotViper850::move(const std::string &filename)
{
vpColVector q;
try {
- this->readPosFile(filename, q) ;
- this->setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ this->readPosFile(filename, q);
+ this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPositioningVelocity(10);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q) ;
- }
- catch(...) {
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+ } catch (...) {
throw;
}
}
/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-
-*/
-void
- vpRobotViper850::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-/*!
-
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
-
- \sa getDisplacement(), getCameraDisplacement()
-
-*/
-void
- vpRobotViper850::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
-}
-
-/*!
-
Get the robot displacement since the last call of this method.
\warning This functionnality is not implemented for the moment in the
@@ -2352,14 +2236,10 @@ void
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
-
*/
-void
- vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpRobotViper850::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
double q[6];
@@ -2369,35 +2249,34 @@ void
InitTry;
// Get the current joint position
- Try( PrimitiveACQ_POS_Viper850(q, ×tamp) );
- for (unsigned int i=0; i < njoint; i ++) {
+ Try(PrimitiveACQ_POS_Viper850(q, ×tamp));
+ for (unsigned int i = 0; i < njoint; i++) {
q_cur[i] = q[i];
}
- if ( ! first_time_getdis ) {
+ if (!first_time_getdis) {
switch (frame) {
case vpRobot::CAMERA_FRAME: {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ }
case vpRobot::ARTICULAR_FRAME: {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
+ displacement = q_cur - q_prev_getdis;
+ break;
+ }
case vpRobot::REFERENCE_FRAME: {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ }
case vpRobot::MIXT_FRAME: {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- }
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
}
- }
- else {
+ }
+ } else {
first_time_getdis = false;
}
@@ -2406,14 +2285,13 @@ void
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get velocity.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get velocity.");
+ vpERROR_TRACE("Cannot get velocity.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get velocity.");
}
}
/*!
-
+
Bias the force/torque sensor.
\sa unbiasForceTorqueSensor(), getForceTorque()
@@ -2422,24 +2300,24 @@ void
void vpRobotViper850::biasForceTorqueSensor()
{
#if defined(USE_ATI_DAQ)
-# if defined(VISP_HAVE_COMEDI)
+#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"));
-# endif
+#else
+ 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;
- Try( PrimitiveTFS_BIAS_Viper850() );
+ Try(PrimitiveTFS_BIAS_Viper850());
// Wait 500 ms to be sure the next measures take into account the bias
- vpTime::wait(500);
+ vpTime::wait(500);
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot bias the force/torque sensor.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot bias the force/torque sensor.");
+ vpERROR_TRACE("Cannot bias the force/torque sensor.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot bias the force/torque sensor.");
}
#endif
}
@@ -2454,29 +2332,31 @@ void vpRobotViper850::biasForceTorqueSensor()
void vpRobotViper850::unbiasForceTorqueSensor()
{
#if defined(USE_ATI_DAQ)
-# if defined(VISP_HAVE_COMEDI)
+#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"));
-# endif
+#else
+ 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
+// Not implemented
#endif
}
/*!
-
+
Get the rough force/torque sensor measures.
\param H: [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
- The code below shows how to get the force/torque measures after a sensor bias.
+ The code below shows how to get the force/torque measures after a sensor
+bias.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -2496,7 +2376,7 @@ int main()
#endif
}
\endcode
-
+
\exception vpRobotException::lowLevelError : If the force/torque measures
cannot be get from the low level controller.
@@ -2506,24 +2386,24 @@ int main()
void vpRobotViper850::getForceTorque(vpColVector &H) const
{
#if defined(USE_ATI_DAQ)
-# if defined(VISP_HAVE_COMEDI)
+#if defined(VISP_HAVE_COMEDI)
H = ati.getForceTorque();
-# else
+#else
(void)H;
- throw(vpException(vpException::fatalError, "Cannot use ATI F/T if comedi is not installed. Try sudo apt-get install libcomedi-dev"));
-# endif
+ 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;
- H.resize (6);
+ H.resize(6);
- Try( PrimitiveTFS_ACQ_Viper850(H.data) );
+ Try(PrimitiveTFS_ACQ_Viper850(H.data));
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
+ vpERROR_TRACE("Cannot get the force/torque measures.");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot get force/torque measures.");
}
#endif
}
@@ -2534,13 +2414,14 @@ void vpRobotViper850::getForceTorque(vpColVector &H) const
\return [Fx, Fy, Fz, Tx, Ty, Tz] Forces/torques measured by the sensor.
- The code below shows how to get the force/torque measures after a sensor bias.
+ The code below shows how to get the force/torque measures after a sensor
+bias.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpTime.h>
+#include <visp3/robot/vpRobotViper850.h>
int main()
{
@@ -2568,25 +2449,25 @@ int main()
vpColVector vpRobotViper850::getForceTorque() const
{
#if defined(USE_ATI_DAQ)
-# if defined(VISP_HAVE_COMEDI)
+#if defined(VISP_HAVE_COMEDI)
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"));
-# endif
+#else
+ 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;
vpColVector H(6);
- Try( PrimitiveTFS_ACQ_Viper850(H.data) );
+ Try(PrimitiveTFS_ACQ_Viper850(H.data));
return H;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot get the force/torque measures.");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot get force/torque measures.");
+ vpERROR_TRACE("Cannot get the 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
@@ -2601,12 +2482,11 @@ vpColVector vpRobotViper850::getForceTorque() const
void vpRobotViper850::openGripper()
{
InitTry;
- Try( PrimitivePneumaticGripper_Viper850(1) );
+ Try(PrimitivePneumaticGripper_Viper850(1));
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.");
}
}
@@ -2620,12 +2500,11 @@ void vpRobotViper850::openGripper()
void vpRobotViper850::closeGripper() const
{
InitTry;
- Try( PrimitivePneumaticGripper_Viper850(0) );
+ Try(PrimitivePneumaticGripper_Viper850(0));
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.");
}
}
@@ -2637,47 +2516,46 @@ void vpRobotViper850::closeGripper() const
void vpRobotViper850::enableJoint6Limits() const
{
InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(0));
std::cout << "Enable joint limits on axis 6..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot enable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot enable joint limits on axis 6.");
+ vpERROR_TRACE("Cannot enable joint limits on axis 6");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot enable joint limits on axis 6.");
}
}
/*!
\warning Each call to this function should be done carefully.
- Disable the joint limits on axis number 6. When joint 6 is outside the limits,
- a call to this function allows to bring the robot to a position inside the limits.
- Don't forget then to call enableJoint6Limits() to reduce the working space for joint 6.
+ Disable the joint limits on axis number 6. When joint 6 is outside the
+ limits, a call to this function allows to bring the robot to a position
+ inside the limits. Don't forget then to call enableJoint6Limits() to reduce
+ the working space for joint 6.
\sa enableJoint6Limits()
*/
void vpRobotViper850::disableJoint6Limits() const
{
InitTry;
- Try( PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1) );
+ Try(PrimitiveREMOVE_JOINT6_LIMITS_Viper850(1));
std::cout << "Warning: Disable joint limits on axis 6..." << std::endl;
CatchPrint();
if (TryStt < 0) {
- vpERROR_TRACE ("Cannot disable joint limits on axis 6");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Cannot disable joint limits on axis 6.");
+ vpERROR_TRACE("Cannot disable joint limits on axis 6");
+ throw vpRobotException(vpRobotException::lowLevelError, "Cannot disable joint limits on axis 6.");
}
}
/*!
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+ Set the maximal rotation velocity that can be sent to the robot during a
+ velocity control.
\param w_max : Maximum rotation velocity expressed in rad/s.
*/
-void
-vpRobotViper850::setMaxRotationVelocity (double w_max)
+void vpRobotViper850::setMaxRotationVelocity(double w_max)
{
vpRobot::setMaxRotationVelocity(w_max);
setMaxRotationVelocityJoint6(w_max);
@@ -2687,7 +2565,8 @@ vpRobotViper850::setMaxRotationVelocity (double w_max)
/*!
- Set the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+ Set the maximal rotation velocity on joint 6 that is used only during
+ velocity joint control.
This function affects only the velocities that are sent as joint velocities.
@@ -2704,8 +2583,7 @@ vpRobotViper850::setMaxRotationVelocity (double w_max)
\param w6_max : Maximum rotation velocity expressed in rad/s on joint 6.
*/
-void
-vpRobotViper850::setMaxRotationVelocityJoint6 (const double w6_max)
+void vpRobotViper850::setMaxRotationVelocityJoint6(const double w6_max)
{
maxRotationVelocity_joint6 = w6_max;
return;
@@ -2713,18 +2591,15 @@ vpRobotViper850::setMaxRotationVelocityJoint6 (const double w6_max)
/*!
- Get the maximal rotation velocity on joint 6 that is used only during velocity joint control.
+ Get the maximal rotation velocity on joint 6 that is used only during
+ velocity joint control.
\return Maximum rotation velocity on joint 6 expressed in rad/s.
*/
-double
-vpRobotViper850::getMaxRotationVelocityJoint6() const
-{
- return maxRotationVelocity_joint6;
-}
+double vpRobotViper850::getMaxRotationVelocityJoint6() const { return maxRotationVelocity_joint6; }
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has no symbols
-void dummy_vpRobotViper850() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotViper850.cpp.o) has
+// no symbols
+void dummy_vpRobotViper850(){};
#endif
-
diff --git a/modules/robot/src/real-robot/viper/vpViper.cpp b/modules/robot/src/real-robot/viper/vpViper.cpp
index e095125..29157c8 100644
--- a/modules/robot/src/real-robot/viper/vpViper.cpp
+++ b/modules/robot/src/real-robot/viper/vpViper.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,18 +44,16 @@
*/
-
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
-#include <visp3/robot/vpRobotException.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpRxyzVector.h>
#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpViper.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
const unsigned int vpViper::njoint = 6;
@@ -98,9 +97,6 @@ vpViper::vpViper()
eMc.eye();
}
-
-
-
/*!
Compute the forward kinematics (direct geometric model) as an
@@ -122,8 +118,7 @@ vpViper::vpViper()
\sa getInverseKinematics()
*/
-vpHomogeneousMatrix
-vpViper::getForwardKinematics(const vpColVector & q) const
+vpHomogeneousMatrix vpViper::getForwardKinematics(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
fMc = get_fMc(q);
@@ -138,36 +133,35 @@ vpViper::getForwardKinematics(const vpColVector & q) const
\param joint : Joint to consider.
- \param q : A joint position.
+ \param q : A joint position.
\param q_mod : The joint position modified by considering modulo
\f$2 \pi\f$ to be in the joint limits.
-
- \return true if the joint position is in the joint limits. false otherwise.
+
+ \return true if the joint position is in the joint limits. false otherwise.
*/
-bool
-vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod, const bool &verbose) const
+bool vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, double &q_mod,
+ const bool &verbose) const
{
double eps = 0.01;
- if (q >= joint_min[joint]-eps && q <= joint_max[joint]+eps ) {
+ if (q >= joint_min[joint] - eps && q <= joint_max[joint] + eps) {
q_mod = q;
return true;
}
- q_mod = q + 2*M_PI;
- if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
+ q_mod = q + 2 * M_PI;
+ if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
return true;
- }
+ }
- q_mod = q - 2*M_PI;
- if (q_mod >= joint_min[joint]-eps && q_mod <= joint_max[joint]+eps ) {
+ q_mod = q - 2 * M_PI;
+ if (q_mod >= joint_min[joint] - eps && q_mod <= joint_max[joint] + eps) {
return true;
- }
+ }
if (verbose) {
- std::cout << "Joint " << joint << " not in limits: "
- << this->joint_min[joint] << " < " << q << " < " << this->joint_max[joint] << std::endl;
-
+ std::cout << "Joint " << joint << " not in limits: " << this->joint_min[joint] << " < " << q << " < "
+ << this->joint_max[joint] << std::endl;
}
return false;
@@ -225,33 +219,33 @@ vpViper::convertJointPositionInLimits(unsigned int joint, const double &q, doubl
\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];
- for (unsigned int i=0; i<8; i++)
+ for (unsigned int i = 0; i < 8; i++)
q_sol[i].resize(6);
- double c1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s1[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s3[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s23[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s4[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s5[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double c6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
- double s6[8]={0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0};
+ double c1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s1[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double c3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s3[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double c23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s23[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double c4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s4[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double c5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s5[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double c6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+ double s6[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
bool ok[8];
if (q.getRows() != njoint)
q.resize(6);
- for (unsigned int i=0; i< 8; i++)
+ for (unsigned int i = 0; i < 8; i++)
ok[i] = true;
double px = fMw[0][3]; // a*c1
@@ -259,66 +253,63 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
double pz = fMw[2][3];
// Compute q1
- double a_2 = px*px+py*py;
- //if (a_2 == 0) {// singularity
- if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) {// singularity
+ double a_2 = px * px + py * py;
+ // if (a_2 == 0) {// singularity
+ if (std::fabs(a_2) <= std::numeric_limits<double>::epsilon()) { // singularity
c1[0] = cos(q[0]);
s1[0] = sin(q[0]);
- c1[4] = cos(q[0]+M_PI);
- s1[4] = sin(q[0]+M_PI);
- }
- else {
+ c1[4] = cos(q[0] + M_PI);
+ s1[4] = sin(q[0] + M_PI);
+ } else {
double a = sqrt(a_2);
- c1[0] = px/a;
- s1[0] = py/a;
- c1[4] = -px/a;
- s1[4] = -py/a;
+ c1[0] = px / a;
+ s1[0] = py / a;
+ c1[4] = -px / a;
+ s1[4] = -py / a;
}
double q1_mod;
- for(unsigned int i=0;i<8;i+=4) {
- q_sol[i][0] = atan2(s1[i],c1[i]);
+ for (unsigned int i = 0; i < 8; i += 4) {
+ q_sol[i][0] = atan2(s1[i], c1[i]);
if (convertJointPositionInLimits(0, q_sol[i][0], q1_mod, verbose) == true) {
q_sol[i][0] = q1_mod;
- for(unsigned int j=1;j<4;j++) {
- c1[i+j] = c1[i];
- s1[i+j] = s1[i];
- q_sol[i+j][0] = q_sol[i][0];
+ for (unsigned int j = 1; j < 4; j++) {
+ c1[i + j] = c1[i];
+ s1[i + j] = s1[i];
+ q_sol[i + j][0] = q_sol[i][0];
}
- }
- else {
- for(unsigned int j=1;j<4;j++)
- ok[i+j] = false;
+ } else {
+ for (unsigned int j = 1; j < 4; j++)
+ ok[i + j] = false;
}
}
// Compute q3
double K, q3_mod;
- for(unsigned int i=0; i<8; i+=4) {
- if(ok[i] == true) {
- K = (px*px+py*py+pz*pz+a1*a1-a2*a2-a3*a3+d1*d1-d4*d4
- - 2*(a1*c1[i]*px + a1*s1[i]*py + d1*pz)) / (2*a2);
- double d4_a3_K = d4*d4+a3*a3-K*K;
+ for (unsigned int i = 0; i < 8; i += 4) {
+ if (ok[i] == true) {
+ K = (px * px + py * py + pz * pz + a1 * a1 - a2 * a2 - a3 * a3 + d1 * d1 - d4 * d4 -
+ 2 * (a1 * c1[i] * px + a1 * s1[i] * py + d1 * pz)) /
+ (2 * a2);
+ double d4_a3_K = d4 * d4 + a3 * a3 - K * K;
- q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
- q_sol[i+2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
+ q_sol[i][2] = atan2(a3, d4) + atan2(K, sqrt(d4_a3_K));
+ q_sol[i + 2][2] = atan2(a3, d4) + atan2(K, -sqrt(d4_a3_K));
- for (unsigned int j=0; j<4; j+=2) {
+ for (unsigned int j = 0; j < 4; j += 2) {
if (d4_a3_K < 0) {
- for(unsigned int k=0; k<2; k++)
- ok[i+j+k] = false;
- }
- else {
- if (convertJointPositionInLimits(2, q_sol[i+j][2], q3_mod, verbose) == true) {
- for(unsigned int k=0; k<2; k++) {
- q_sol[i+j+k][2] = q3_mod;
- c3[i+j+k] = cos(q3_mod);
- s3[i+j+k] = sin(q3_mod);
+ for (unsigned int k = 0; k < 2; k++)
+ ok[i + j + k] = false;
+ } else {
+ if (convertJointPositionInLimits(2, q_sol[i + j][2], q3_mod, verbose) == true) {
+ for (unsigned int k = 0; k < 2; k++) {
+ q_sol[i + j + k][2] = q3_mod;
+ c3[i + j + k] = cos(q3_mod);
+ s3[i + j + k] = sin(q3_mod);
}
- }
- else {
- for(unsigned int k=0; k<2; k++)
- ok[i+j+k] = false;
+ } else {
+ for (unsigned int k = 0; k < 2; k++)
+ ok[i + j + k] = false;
}
}
}
@@ -331,28 +322,28 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
// Compute q2
double q23[8], q2_mod;
- for (unsigned int i=0; i<8; i+=2) {
+ for (unsigned int i = 0; i < 8; i += 2) {
if (ok[i] == true) {
// Compute q23 = q2+q3
- c23[i] = (-(a3-a2*c3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(d4+a2*s3[i]))
- / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
- s23[i] = ((d4+a2*s3[i])*(c1[i]*px+s1[i]*py-a1)-(d1-pz)*(a3-a2*c3[i]))
- / ( (c1[i]*px+s1[i]*py-a1)*(c1[i]*px+s1[i]*py-a1) +(d1-pz)*(d1-pz) );
- q23[i] = atan2(s23[i],c23[i]);
- //std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] << std::endl;
+ c23[i] = (-(a3 - a2 * c3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (d4 + a2 * s3[i])) /
+ ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
+ s23[i] = ((d4 + a2 * s3[i]) * (c1[i] * px + s1[i] * py - a1) - (d1 - pz) * (a3 - a2 * c3[i])) /
+ ((c1[i] * px + s1[i] * py - a1) * (c1[i] * px + s1[i] * py - a1) + (d1 - pz) * (d1 - pz));
+ q23[i] = atan2(s23[i], c23[i]);
+ // std::cout << i << " c23 = " << c23[i] << " s23 = " << s23[i] <<
+ // std::endl;
// q2 = q23 - q3
q_sol[i][1] = q23[i] - q_sol[i][2];
if (convertJointPositionInLimits(1, q_sol[i][1], q2_mod, verbose) == true) {
- for(unsigned int j=0; j<2; j++) {
- q_sol[i+j][1] = q2_mod;
- c23[i+j] = c23[i];
- s23[i+j] = s23[i];
+ for (unsigned int j = 0; j < 2; j++) {
+ q_sol[i + j][1] = q2_mod;
+ c23[i + j] = c23[i];
+ s23[i + j] = s23[i];
}
- }
- else {
- for(unsigned int j=0; j<2; j++)
- ok[i+j] = false;
+ } else {
+ for (unsigned int j = 0; j < 2; j++)
+ ok[i + j] = false;
}
}
}
@@ -366,36 +357,34 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
double r23 = fMw[1][2];
double r33 = fMw[2][2];
double s4s5, c4s5, q4_mod, q5_mod;
- for (unsigned int i=0; i<8; i+=2) {
+ for (unsigned int i = 0; i < 8; i += 2) {
if (ok[i] == true) {
- s4s5 = -s1[i]*r13+c1[i]*r23;
- c4s5 = c1[i]*c23[i]*r13+s1[i]*c23[i]*r23-s23[i]*r33;
+ s4s5 = -s1[i] * r13 + c1[i] * r23;
+ c4s5 = c1[i] * c23[i] * r13 + s1[i] * c23[i] * r23 - s23[i] * r33;
if (fabs(s4s5) < vpMath::rad(0.5) && fabs(c4s5) < vpMath::rad(0.5)) {
// s5 = 0
- c5[i] = c1[i]*s23[i]*r13+s1[i]*s23[i]*r23+c23[i]*r33;
- //std::cout << "Singularity: s5 near 0: ";
+ c5[i] = c1[i] * s23[i] * r13 + s1[i] * s23[i] * r23 + c23[i] * r33;
+ // std::cout << "Singularity: s5 near 0: ";
if (c5[i] > 0.)
q_sol[i][4] = 0.0;
else
q_sol[i][4] = M_PI;
if (convertJointPositionInLimits(4, q_sol[i][4], q5_mod, verbose) == true) {
- for(unsigned int j=0; j<2; j++) {
- q_sol[i+j][3] = q[3]; // keep current q4
- q_sol[i+j][4] = q5_mod;
- c4[i] = cos(q_sol[i+j][3]);
- s4[i] = sin(q_sol[i+j][3]);
+ for (unsigned int j = 0; j < 2; j++) {
+ q_sol[i + j][3] = q[3]; // keep current q4
+ q_sol[i + j][4] = q5_mod;
+ c4[i] = cos(q_sol[i + j][3]);
+ s4[i] = sin(q_sol[i + j][3]);
}
+ } else {
+ for (unsigned int j = 0; j < 2; j++)
+ ok[i + j] = false;
}
- else {
- for(unsigned int j=0; j<2; j++)
- ok[i+j] = false;
- }
- }
- else {
- // s5 != 0
+ } else {
+// s5 != 0
#if 0 // Modified 2016/03/10 since if and else are the same
- //if (c4s5 == 0) {
+ // if (c4s5 == 0) {
if (std::fabs(c4s5) <= std::numeric_limits<double>::epsilon()) {
// c4 = 0
// vpTRACE("c4 = 0");
@@ -412,37 +401,35 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
q_sol[i][3] = q4_mod;
c4[i] = cos(q4_mod);
s4[i] = sin(q4_mod);
- }
- else {
+ } else {
ok[i] = false;
}
if (q_sol[i][3] > 0.)
- q_sol[i+1][3] = q_sol[i][3] + M_PI;
+ q_sol[i + 1][3] = q_sol[i][3] + M_PI;
else
- q_sol[i+1][3] = q_sol[i][3] - M_PI;
- if (convertJointPositionInLimits(3, q_sol[i+1][3], q4_mod, verbose) == true) {
- q_sol[i+1][3] = q4_mod;
- c4[i+1] = cos(q4_mod);
- s4[i+1] = sin(q4_mod);
- }
- else {
- ok[i+1] = false;
+ q_sol[i + 1][3] = q_sol[i][3] - M_PI;
+ if (convertJointPositionInLimits(3, q_sol[i + 1][3], q4_mod, verbose) == true) {
+ q_sol[i + 1][3] = q4_mod;
+ c4[i + 1] = cos(q4_mod);
+ s4[i + 1] = sin(q4_mod);
+ } else {
+ ok[i + 1] = false;
}
// Compute q5
- for (unsigned int j=0; j<2; j++) {
- if (ok[i+j] == true) {
- c5[i+j] = c1[i+j]*s23[i+j]*r13+s1[i+j]*s23[i+j]*r23+c23[i+j]*r33;
- s5[i+j] = (c1[i+j]*c23[i+j]*c4[i+j]-s1[i+j]*s4[i+j])*r13
- +(s1[i+j]*c23[i+j]*c4[i+j]+c1[i+j]*s4[i+j])*r23-s23[i+j]*c4[i+j]*r33;
-
- q_sol[i+j][4] = atan2(s5[i+j], c5[i+j]);
- if (convertJointPositionInLimits(4, q_sol[i+j][4], q5_mod, verbose) == true) {
- q_sol[i+j][4] = q5_mod;
- }
- else {
-
- ok[i+j] = false;
+ for (unsigned int j = 0; j < 2; j++) {
+ if (ok[i + j] == true) {
+ c5[i + j] = c1[i + j] * s23[i + j] * r13 + s1[i + j] * s23[i + j] * r23 + c23[i + j] * r33;
+ s5[i + j] = (c1[i + j] * c23[i + j] * c4[i + j] - s1[i + j] * s4[i + j]) * r13 +
+ (s1[i + j] * c23[i + j] * c4[i + j] + c1[i + j] * s4[i + j]) * r23 -
+ s23[i + j] * c4[i + j] * r33;
+
+ q_sol[i + j][4] = atan2(s5[i + j], c5[i + j]);
+ if (convertJointPositionInLimits(4, q_sol[i + j][4], q5_mod, verbose) == true) {
+ q_sol[i + j][4] = q5_mod;
+ } else {
+
+ ok[i + j] = false;
}
}
}
@@ -456,19 +443,17 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
double r22 = fMw[1][1];
double r32 = fMw[2][1];
double q6_mod;
- for (unsigned int i=0; i<8; i++) {
- c6[i] = -(c1[i]*c23[i]*s4[i]+s1[i]*c4[i])*r12
- +(c1[i]*c4[i]-s1[i]*c23[i]*s4[i])*r22+s23[i]*s4[i]*r32;
- s6[i] = -(c1[i]*c23[i]*c4[i]*c5[i]-c1[i]*s23[i]*s5[i]
- -s1[i]*s4[i]*c5[i])*r12
- -(s1[i]*c23[i]*c4[i]*c5[i]-s1[i]*s23[i]*s5[i]+c1[i]*s4[i]*c5[i])*r22
- +(c23[i]*s5[i]+s23[i]*c4[i]*c5[i])*r32;
+ for (unsigned int i = 0; i < 8; i++) {
+ c6[i] = -(c1[i] * c23[i] * s4[i] + s1[i] * c4[i]) * r12 + (c1[i] * c4[i] - s1[i] * c23[i] * s4[i]) * r22 +
+ s23[i] * s4[i] * r32;
+ s6[i] = -(c1[i] * c23[i] * c4[i] * c5[i] - c1[i] * s23[i] * s5[i] - s1[i] * s4[i] * c5[i]) * r12 -
+ (s1[i] * c23[i] * c4[i] * c5[i] - s1[i] * s23[i] * s5[i] + c1[i] * s4[i] * c5[i]) * r22 +
+ (c23[i] * s5[i] + s23[i] * c4[i] * c5[i]) * r32;
q_sol[i][5] = atan2(s6[i], c6[i]);
if (convertJointPositionInLimits(5, q_sol[i][5], q6_mod, verbose) == true) {
q_sol[i][5] = q6_mod;
- }
- else {
+ } else {
ok[i] = false;
}
}
@@ -477,9 +462,9 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
unsigned int nbsol = 0;
unsigned int sol = 0;
vpColVector dist(8);
- for (unsigned int i=0; i<8; i++) {
+ for (unsigned int i = 0; i < 8; i++) {
if (ok[i] == true) {
- nbsol ++;
+ nbsol++;
sol = i;
// dist[i] = vpColVector::distance(q, q_sol[i]);
vpColVector weight(6);
@@ -487,28 +472,30 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
weight[0] = 8;
weight[1] = weight[2] = 4;
dist[i] = 0;
- for (unsigned int j=0; j< 6; j++) {
- double rought_dist = q[j]- q_sol[i][j];
+ for (unsigned int j = 0; j < 6; j++) {
+ double rought_dist = q[j] - q_sol[i][j];
double modulo_dist = rought_dist;
if (rought_dist > 0) {
- if (fabs(rought_dist - 2*M_PI) < fabs(rought_dist))
- modulo_dist = rought_dist - 2*M_PI;
- }
- else {
- if (fabs(rought_dist + 2*M_PI) < fabs(rought_dist))
- modulo_dist = rought_dist + 2*M_PI;
+ if (fabs(rought_dist - 2 * M_PI) < fabs(rought_dist))
+ modulo_dist = rought_dist - 2 * M_PI;
+ } else {
+ if (fabs(rought_dist + 2 * M_PI) < fabs(rought_dist))
+ modulo_dist = rought_dist + 2 * M_PI;
}
- //std::cout << "dist " << i << ": " << rought_dist << " modulo: " << modulo_dist << std::endl;
- dist[i] += weight[j]*vpMath::sqr(modulo_dist);
+ // std::cout << "dist " << i << ": " << rought_dist << " modulo: " <<
+ // modulo_dist << std::endl;
+ dist[i] += weight[j] * vpMath::sqr(modulo_dist);
}
}
- // std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] << " q: " << q_sol[i].t() << std::endl;
+ // std::cout << "sol " << i << " [" << ok[i] << "] dist: " << dist[i] <<
+ // " q: " << q_sol[i].t() << std::endl;
}
- //std::cout << "dist: " << dist.t() << std::endl;
+ // std::cout << "dist: " << dist.t() << std::endl;
if (nbsol) {
- for (unsigned int i=0; i<8; i++) {
+ for (unsigned int i = 0; i < 8; i++) {
if (ok[i] == true)
- if (dist[i] < dist[sol]) sol = i;
+ if (dist[i] < dist[sol])
+ sol = i;
}
// Update the inverse kinematics solution
q = q_sol[sol];
@@ -517,7 +504,6 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
// << dist[sol] << "): " << q_sol[sol].t() << std::endl;
}
return nbsol;
-
}
/*!
@@ -574,8 +560,7 @@ vpViper::getInverseKinematicsWrist(const vpHomogeneousMatrix & fMw, vpColVector
\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;
@@ -583,7 +568,7 @@ vpViper::getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q,
this->get_wMe(wMe);
this->get_eMc(eMc_);
fMw = fMc * eMc_.inverse() * wMe.inverse();
-
+
return (getInverseKinematicsWrist(fMw, q, verbose));
}
@@ -612,8 +597,7 @@ vpViper::getInverseKinematics(const vpHomogeneousMatrix & fMc, vpColVector & q,
\sa getForwardKinematics(const vpColVector & q), get_fMe(), get_eMc()
*/
-vpHomogeneousMatrix
-vpViper::get_fMc (const vpColVector & q) const
+vpHomogeneousMatrix vpViper::get_fMc(const vpColVector &q) const
{
vpHomogeneousMatrix fMc;
get_fMc(q, fMc);
@@ -642,8 +626,7 @@ vpViper::get_fMc (const vpColVector & q) const
\sa get_fMe(), get_eMc()
*/
-void
-vpViper::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
+void vpViper::get_fMc(const vpColVector &q, vpHomogeneousMatrix &fMc) const
{
// Compute the direct geometric model: fMe = transformation between
@@ -699,9 +682,9 @@ vpViper::get_fMc(const vpColVector & q, vpHomogeneousMatrix & fMc) const
\param q : A 6-dimension vector that contains the 6 joint positions
expressed in radians.
- \param fMe The homogeneous matrix \f${^f}{\bf M}_e\f$ corresponding to the direct geometric
- model which expresses the transformation between the fix frame and the
- end effector frame.
+ \param fMe The homogeneous matrix \f${^f}{\bf M}_e\f$ corresponding to the
+direct geometric model which expresses the transformation between the fix
+frame and the end effector frame.
Note that this transformation can also be computed by considering the wrist
frame \f${^f}{\bf M}_e = {^f}{\bf M}_w *{^w}{\bf M}_e\f$.
@@ -729,8 +712,7 @@ int main()
\endcode
*/
-void
-vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
+void vpViper::get_fMe(const vpColVector &q, vpHomogeneousMatrix &fMe) const
{
double q1 = q[0];
double q2 = q[1];
@@ -743,41 +725,41 @@ vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
// if positions are motor position.
// double q6 = q[5] + c56 * q[4];
-// std::cout << "q6 motor: " << q[5] << " rad "
-// << vpMath::deg(q[5]) << " deg" << std::endl;
-// std::cout << "q6 joint: " << q6 << " rad "
-// << vpMath::deg(q6) << " deg" << std::endl;
+ // std::cout << "q6 motor: " << q[5] << " rad "
+ // << vpMath::deg(q[5]) << " deg" << std::endl;
+ // std::cout << "q6 joint: " << q6 << " rad "
+ // << vpMath::deg(q6) << " deg" << std::endl;
double c1 = cos(q1);
double s1 = sin(q1);
double c2 = cos(q2);
double s2 = sin(q2);
- //double c3 = cos(q3);
- //double s3 = sin(q3);
+ // double c3 = cos(q3);
+ // double s3 = sin(q3);
double c4 = cos(q4);
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
double c6 = cos(q6);
double s6 = sin(q6);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
+ double c23 = cos(q2 + q3);
+ double s23 = sin(q2 + q3);
- fMe[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMe[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMe[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+ fMe[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
+ fMe[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
+ fMe[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
- fMe[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMe[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMe[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+ fMe[0][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][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
+ fMe[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
+ fMe[2][2] = -s23 * c4 * s5 + c23 * c5;
- fMe[0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
- fMe[1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
- fMe[2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
+ 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;
@@ -791,8 +773,8 @@ vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
\param q : A 6-dimension vector that contains the 6 joint positions
expressed in radians.
- \param fMw The homogeneous matrix corresponding to the transformation between
- the fix frame and the wrist frame (fMw).
+ \param fMw The homogeneous matrix corresponding to the transformation
+ between the fix frame and the wrist frame (fMw).
\f[
{^f}M_w = \left(\begin{array}{cccc}
@@ -823,10 +805,9 @@ vpViper::get_fMe(const vpColVector & q, vpHomogeneousMatrix & fMe) const
t_z = s23a3+c23d4-a2s2+d1\\
\end{array}
\f]
-
+
*/
-void
-vpViper::get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const
+void vpViper::get_fMw(const vpColVector &q, vpHomogeneousMatrix &fMw) const
{
double q1 = q[0];
double q2 = q[1];
@@ -839,43 +820,43 @@ vpViper::get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const
// if positions are motor position.
// double q6 = q[5] + c56 * q[4];
-// std::cout << "q6 motor: " << q[5] << " rad "
-// << vpMath::deg(q[5]) << " deg" << std::endl;
-// std::cout << "q6 joint: " << q6 << " rad "
-// << vpMath::deg(q6) << " deg" << std::endl;
+ // std::cout << "q6 motor: " << q[5] << " rad "
+ // << vpMath::deg(q[5]) << " deg" << std::endl;
+ // std::cout << "q6 joint: " << q6 << " rad "
+ // << vpMath::deg(q6) << " deg" << std::endl;
double c1 = cos(q1);
double s1 = sin(q1);
double c2 = cos(q2);
double s2 = sin(q2);
// double c3 = cos(q3);
- //double s3 = sin(q3);
+ // double s3 = sin(q3);
double c4 = cos(q4);
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
double c6 = cos(q6);
double s6 = sin(q6);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
+ double c23 = cos(q2 + q3);
+ double s23 = sin(q2 + q3);
- fMw[0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMw[1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMw[2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
+ fMw[0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
+ fMw[1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
+ fMw[2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
- fMw[0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMw[1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMw[2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
+ fMw[0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
+ fMw[1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
+ fMw[2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
- fMw[0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMw[1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMw[2][2] = -s23*c4*s5+c23*c5;
+ fMw[0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
+ fMw[1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
+ fMw[2][2] = -s23 * c4 * s5 + c23 * c5;
- fMw[0][3] = c1*(-c23*a3+s23*d4+a1+a2*c2);
- fMw[1][3] = s1*(-c23*a3+s23*d4+a1+a2*c2);
- fMw[2][3] = s23*a3+c23*d4-a2*s2+d1;
+ fMw[0][3] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
+ fMw[1][3] = s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
+ fMw[2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
- //std::cout << "Wrist position fMw: " << std::endl << fMw;
+ // std::cout << "Wrist position fMw: " << std::endl << fMw;
return;
}
@@ -886,12 +867,11 @@ vpViper::get_fMw(const vpColVector & q, vpHomogeneousMatrix & fMw) const
wrist frame is located on the intersection of the 3 last rotations.
- \param wMe The homogeneous matrix corresponding to the transformation between
- the wrist frame and the end-effector frame (wMe).
+ \param wMe The homogeneous matrix corresponding to the transformation
+ between the wrist frame and the end-effector frame (wMe).
*/
-void
-vpViper::get_wMe(vpHomogeneousMatrix & wMe) const
+void vpViper::get_wMe(vpHomogeneousMatrix &wMe) const
{
// Set the rotation as identity
wMe.eye();
@@ -911,11 +891,7 @@ vpViper::get_wMe(vpHomogeneousMatrix & wMe) const
\sa get_cMe()
*/
-void
-vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const
-{
- eMc_ = this->eMc;
-}
+void vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const { eMc_ = this->eMc; }
/*!
@@ -926,8 +902,7 @@ vpViper::get_eMc(vpHomogeneousMatrix &eMc_) const
end-effector frame and the force/torque sensor frame.
*/
-void
-vpViper::get_eMs(vpHomogeneousMatrix &eMs) const
+void vpViper::get_eMs(vpHomogeneousMatrix &eMs) const
{
eMs.eye();
eMs[2][3] = -d7; // tz = -d7
@@ -944,19 +919,14 @@ 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(); }
/*!
- Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to end-effector
- frame. This transformation allows to compute a velocity expressed
- in the end-effector frame into the camera frame.
- \f[
- ^c{\bf V}_e = \left(\begin{array}{cc}
+ Get the twist transformation \f$^c{\bf V}_e\f$ from camera frame to
+ end-effector frame. This transformation allows to compute a velocity
+ expressed in the end-effector frame into the camera frame. \f[ ^c{\bf V}_e =
+ \left(\begin{array}{cc}
^c{\bf R}_e & [^c{\bf t}_e]_\times ^c{\bf R}_e\\
{\bf 0}_{3\times 3} & ^c{\bf R}_e
\end{array}
@@ -965,13 +935,12 @@ vpViper::get_cMe(vpHomogeneousMatrix &cMe) const
\param cVe : Twist transformation \f$^c{\bf V}_e\f$.
*/
-void
-vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
{
- vpHomogeneousMatrix cMe ;
- get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
return;
}
@@ -998,10 +967,9 @@ vpViper::get_cVe(vpVelocityTwistMatrix &cVe) const
\sa get_fJw()
*/
-void
-vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
+void vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
{
- vpMatrix V(6,6);
+ vpMatrix V(6, 6);
V = 0;
// Compute the first and last block of V
vpHomogeneousMatrix fMw;
@@ -1010,9 +978,9 @@ vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
fMw.extract(fRw);
vpRotationMatrix wRf;
wRf = fRw.inverse();
- for (unsigned int i=0; i<3; i++ ) {
- for (unsigned int j=0; j<3; j++ ) {
- V[i][j] = V[i+3][j+3] = wRf[i][j];
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ V[i][j] = V[i + 3][j + 3] = wRf[i][j];
}
}
// Compute the second block of V
@@ -1022,21 +990,20 @@ vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
eMw = wMe.inverse();
vpTranslationVector etw;
eMw.extract(etw);
- vpMatrix block2 = etw.skew()*wRf;
- for (unsigned int i=0; i<3; i++ ) {
- for (unsigned int j=0; j<3; j++ ) {
- V[i][j+3] = block2[i][j];
+ vpMatrix block2 = etw.skew() * wRf;
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
+ V[i][j + 3] = block2[i][j];
}
}
// Compute eJe
vpMatrix fJw;
- get_fJw(q, fJw);
+ get_fJw(q, fJw);
eJe = V * fJw;
return;
}
-
/*!
Get the robot jacobian \f${^f}{\bf J}_w\f$ which express the
@@ -1084,8 +1051,7 @@ vpViper::get_eJe(const vpColVector &q, vpMatrix &eJe) const
\sa get_fJe(), get_eJe()
*/
-void
-vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
+void vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
{
double q1 = q[0];
double q2 = q[1];
@@ -1103,8 +1069,8 @@ vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
+ double c23 = cos(q2 + q3);
+ double s23 = sin(q2 + q3);
vpColVector J1(6);
vpColVector J2(6);
@@ -1114,23 +1080,23 @@ vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
vpColVector J6(6);
// Jacobian when d6 is set to zero
- J1[0] = -s1*(-c23*a3+s23*d4+a1+a2*c2);
- J1[1] = c1*(-c23*a3+s23*d4+a1+a2*c2);
+ J1[0] = -s1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
+ J1[1] = c1 * (-c23 * a3 + s23 * d4 + a1 + a2 * c2);
J1[2] = 0;
J1[3] = 0;
J1[4] = 0;
J1[5] = 1;
- J2[0] = c1*(s23*a3+c23*d4-a2*s2);
- J2[1] = s1*(s23*a3+c23*d4-a2*s2);
- J2[2] = c23*a3-s23*d4-a2*c2;
+ J2[0] = c1 * (s23 * a3 + c23 * d4 - a2 * s2);
+ J2[1] = s1 * (s23 * a3 + c23 * d4 - a2 * s2);
+ J2[2] = c23 * a3 - s23 * d4 - a2 * c2;
J2[3] = -s1;
J2[4] = c1;
J2[5] = 0;
- J3[0] = c1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
- J3[1] = s1*(a3*(s2*c3+c2*s3)+(-s2*s3+c2*c3)*d4);
- J3[2] = -a3*(s2*s3-c2*c3)-d4*(s2*c3+c2*s3);
+ J3[0] = c1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
+ J3[1] = s1 * (a3 * (s2 * c3 + c2 * s3) + (-s2 * s3 + c2 * c3) * d4);
+ J3[2] = -a3 * (s2 * s3 - c2 * c3) - d4 * (s2 * c3 + c2 * s3);
J3[3] = -s1;
J3[4] = c1;
J3[5] = 0;
@@ -1138,26 +1104,26 @@ vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
J4[0] = 0;
J4[1] = 0;
J4[2] = 0;
- J4[3] = c1*s23;
- J4[4] = s1*s23;
+ J4[3] = c1 * s23;
+ J4[4] = s1 * s23;
J4[5] = c23;
J5[0] = 0;
J5[1] = 0;
J5[2] = 0;
- J5[3] = -c23*c1*s4-s1*c4;
- J5[4] = c1*c4-c23*s1*s4;
- J5[5] = s23*s4;
+ J5[3] = -c23 * c1 * s4 - s1 * c4;
+ J5[4] = c1 * c4 - c23 * s1 * s4;
+ J5[5] = s23 * s4;
J6[0] = 0;
J6[1] = 0;
J6[2] = 0;
- J6[3] = (c1*c23*c4-s1*s4)*s5+c1*s23*c5;
- J6[4] = (s1*c23*c4+c1*s4)*s5+s1*s23*c5;
- J6[5] = -s23*c4*s5+c23*c5;
+ J6[3] = (c1 * c23 * c4 - s1 * s4) * s5 + c1 * s23 * c5;
+ J6[4] = (s1 * c23 * c4 + c1 * s4) * s5 + s1 * s23 * c5;
+ J6[5] = -s23 * c4 * s5 + c23 * c5;
- fJw.resize(6,6) ;
- for (unsigned int i=0;i<6;i++) {
+ fJw.resize(6, 6);
+ for (unsigned int i = 0; i < 6; i++) {
fJw[i][0] = J1[i];
fJw[i][1] = J2[i];
fJw[i][2] = J3[i];
@@ -1176,7 +1142,7 @@ vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
\f[
{^f}{\bf J}_e = \left[\begin{array}{cc}
I_{3\times3} & [{^f}{\bf R}_w \; {^e}{\bf t}_w]_\times \\
- 0_{3\times3} & I_{3\times3}
+ 0_{3\times3} & I_{3\times3}
\end{array}
\right]
{^f}{\bf J}_w
@@ -1190,15 +1156,14 @@ vpViper::get_fJw(const vpColVector &q, vpMatrix &fJw) const
\sa get_fJw
*/
-void
-vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
+void vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
{
- vpMatrix V(6,6);
+ vpMatrix V(6, 6);
V = 0;
// Set the first and last block to identity
- for (unsigned int i=0; i<6; i++ )
+ for (unsigned int i = 0; i < 6; i++)
V[i][i] = 1;
-
+
// Compute the second block of V
vpHomogeneousMatrix fMw;
get_fMw(q, fMw);
@@ -1210,21 +1175,20 @@ vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
eMw = wMe.inverse();
vpTranslationVector etw;
eMw.extract(etw);
- vpMatrix block2 = (fRw*etw).skew();
+ vpMatrix block2 = (fRw * etw).skew();
// Set the second block
- for (unsigned int i=0; i<3; i++ )
- for (unsigned int j=0; j<3; j++ )
- V[i][j+3] = block2[i][j];
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ V[i][j + 3] = block2[i][j];
// Compute fJe
vpMatrix fJw;
- get_fJw(q, fJw);
+ get_fJw(q, fJw);
fJe = V * fJw;
return;
}
-
/*!
Get minimal joint values.
@@ -1232,11 +1196,7 @@ vpViper::get_fJe(const vpColVector &q, vpMatrix &fJe) const
for the 6 dof. All the values are expressed in radians.
*/
-vpColVector
-vpViper::getJointMin() const
-{
- return joint_min;
-}
+vpColVector vpViper::getJointMin() const { return joint_min; }
/*!
Get maximal joint values.
@@ -1245,11 +1205,7 @@ vpViper::getJointMin() const
for the 6 dof. All the values are expressed in radians.
*/
-vpColVector
-vpViper::getJointMax() const
-{
- return joint_max;
-}
+vpColVector vpViper::getJointMax() const { return joint_max; }
/*!
@@ -1261,11 +1217,7 @@ vpViper::getJointMax() const
be considered in the modelization of the robot.
*/
-double
-vpViper::getCoupl56() const
-{
- return c56;
-}
+double vpViper::getCoupl56() const { return c56; }
/*!
@@ -1275,8 +1227,7 @@ vpViper::getCoupl56() const
\param eMc_ : Transformation between the end-effector frame
and the tool frame.
*/
-void
-vpViper::set_eMc(const vpHomogeneousMatrix &eMc_)
+void vpViper::set_eMc(const vpHomogeneousMatrix &eMc_)
{
this->eMc = eMc_;
this->eMc.extract(etc);
@@ -1294,13 +1245,12 @@ 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_;
vpRotationMatrix eRc(erc);
- this->eMc.buildFrom(etc,eRc);
+ this->eMc.buildFrom(etc, eRc);
}
/*!
@@ -1312,7 +1262,7 @@ vpViper::set_eMc(const vpTranslationVector &etc_, const vpRxyzVector &erc_)
\param os : Output stream.
\param viper : Robot parameters.
*/
-VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpViper &viper)
{
vpRotationMatrix eRc;
viper.eMc.extract(eRc);
@@ -1324,32 +1274,21 @@ VISP_EXPORT std::ostream & operator << (std::ostream & os, const vpViper & viper
jmax.rad2deg();
jmin.rad2deg();
- os
- << "Joint Max (deg):" << std::endl
- << "\t" << jmax.t() << std::endl
-
- << "Joint Min (deg): " << std::endl
- << "\t" << jmin.t() << std::endl
-
- << "Coupling 5-6:" << std::endl
- << "\t" << viper.c56 << std::endl
-
- << "eMc: "<< std::endl
- << "\tTranslation (m): "
- << viper.eMc[0][3] << " "
- << viper.eMc[1][3] << " "
- << viper.eMc[2][3]
- << "\t" << std::endl
- << "\tRotation Rxyz (rad) : "
- << rxyz[0] << " "
- << rxyz[1] << " "
- << rxyz[2]
- << "\t" << std::endl
- << "\tRotation Rxyz (deg) : "
- << vpMath::deg(rxyz[0]) << " "
- << vpMath::deg(rxyz[1]) << " "
- << vpMath::deg(rxyz[2])
- << "\t" << std::endl;
+ os << "Joint Max (deg):" << std::endl
+ << "\t" << jmax.t() << std::endl
+
+ << "Joint Min (deg): " << std::endl
+ << "\t" << jmin.t() << std::endl
+
+ << "Coupling 5-6:" << std::endl
+ << "\t" << viper.c56 << std::endl
+
+ << "eMc: " << std::endl
+ << "\tTranslation (m): " << viper.eMc[0][3] << " " << viper.eMc[1][3] << " " << viper.eMc[2][3] << "\t"
+ << std::endl
+ << "\tRotation Rxyz (rad) : " << rxyz[0] << " " << rxyz[1] << " " << rxyz[2] << "\t" << std::endl
+ << "\tRotation Rxyz (deg) : " << vpMath::deg(rxyz[0]) << " " << vpMath::deg(rxyz[1]) << " " << vpMath::deg(rxyz[2])
+ << "\t" << std::endl;
return os;
}
diff --git a/modules/robot/src/real-robot/viper/vpViper650.cpp b/modules/robot/src/real-robot/viper/vpViper650.cpp
index 033ea1b..46c5e37 100644
--- a/modules/robot/src/real-robot/viper/vpViper650.cpp
+++ b/modules/robot/src/real-robot/viper/vpViper650.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,30 +45,35 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/robot/vpViper650.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/robot/vpViper650.h>
-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");
+ 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");
+ 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");
+ 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");
+ std::string(VISP_VIPER650_DATA_PATH) +
+ 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");
@@ -80,15 +86,13 @@ const std::string vpViper650::CONST_CAMERA_FILENAME =
#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_GENERIC_CAMERA_NAME = "Generic-camera";
+const char *const vpViper650::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
+const char *const vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
+const char *const vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
+const char *const vpViper650::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
const vpViper650::vpToolType vpViper650::defaultTool = vpViper650::TOOL_PTGREY_FLEA2_CAMERA;
-
-
/*!
Default constructor.
@@ -128,8 +132,7 @@ vpViper650::vpViper650()
Initialize the robot with the default tool vpViper650::defaultTool.
*/
-void
-vpViper650::init (void)
+void vpViper650::init(void)
{
this->init(vpViper650::defaultTool);
return;
@@ -144,13 +147,12 @@ vpViper650::init (void)
extrinsic parameters.
*/
-void
-vpViper650::init (const std::string &camera_extrinsic_parameters)
+void vpViper650::init(const std::string &camera_extrinsic_parameters)
{
- //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
- this->parseConfigFile (camera_extrinsic_parameters);
+ // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
+ this->parseConfigFile(camera_extrinsic_parameters);
- return ;
+ return;
}
/*!
@@ -174,69 +176,67 @@ 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;
-
+
#ifdef VISP_HAVE_VIPER650_DATA
// Read the robot parameters from files
std::string filename_eMc;
switch (tool) {
case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper650::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
break;
}
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!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -248,61 +248,61 @@ vpViper650::init (vpViper650::vpToolType tool,
break;
}
}
-
- this->init (filename_eMc);
-#else // VISP_HAVE_VIPER650_DATA
+ this->init(filename_eMc);
+
+#else // VISP_HAVE_VIPER650_DATA
// Use here default values of the robot constant parameters.
switch (tool) {
case vpViper650::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.26); // rx
- erc[1] = vpMath::rad(2.12); // ry
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ erc[0] = vpMath::rad(0.26); // rx
+ erc[1] = vpMath::rad(2.12); // ry
erc[2] = vpMath::rad(-91.31); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0005; // ty
- etc[2] = 0.1022; // tz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0005; // ty
+ etc[2] = 0.1022; // tz
break;
}
break;
}
case vpViper650::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.15); // rx
- erc[1] = vpMath::rad(1.28); // ry
+ case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ erc[0] = vpMath::rad(0.15); // rx
+ erc[1] = vpMath::rad(1.28); // ry
erc[2] = vpMath::rad(-90.8); // rz
- etc[0] = -0.0456; // tx
- etc[1] = -0.0013; // ty
- etc[2] = 0.001; // tz
+ etc[0] = -0.0456; // tx
+ etc[1] = -0.0013; // ty
+ etc[2] = 0.001; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.72); // rx
- erc[1] = vpMath::rad(2.10); // ry
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ erc[0] = vpMath::rad(0.72); // rx
+ erc[1] = vpMath::rad(2.10); // ry
erc[2] = vpMath::rad(-90.5); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0012; // ty
- etc[2] = 0.078; // tz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0012; // ty
+ etc[2] = 0.078; // tz
break;
}
break;
}
case vpViper650::TOOL_GENERIC_CAMERA: {
// Set eMc to identity
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ case vpCameraParameters::perspectiveProjWithDistortion:
erc[0] = 0; // rx
erc[1] = 0; // ry
erc[2] = 0; // rz
@@ -314,10 +314,10 @@ vpViper650::init (vpViper650::vpToolType tool,
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);
@@ -325,7 +325,7 @@ vpViper650::init (vpViper650::vpToolType tool,
#endif // VISP_HAVE_VIPER650_DATA
setToolType(tool);
- return ;
+ return;
}
/*!
@@ -354,15 +354,14 @@ eMc_ROT_XYZ 10.0 -90.0 20.0
eMc_TRANS_XYZ 0.05 0.01 0.06
\endcode
- \sa init(vpViper650::vpToolType, vpCameraParameters::vpCameraParametersProjType),
- init(vpViper650::vpToolType, const vpHomogeneousMatrix&)
+ \sa init(vpViper650::vpToolType,
+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());
+ this->setToolType(tool);
+ this->parseConfigFile(filename.c_str());
}
/*!
@@ -375,16 +374,15 @@ vpViper650::init(vpViper650::vpToolType tool,
\param eMc_ : Homogeneous matrix representation of the transformation
between the end-effector frame and the tool frame.
- \sa init(vpViper650::vpToolType, vpCameraParameters::vpCameraParametersProjType),
+ \sa init(vpViper650::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
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_);
+ this->setToolType(tool);
+ this->set_eMc(eMc_);
}
/*!
@@ -395,102 +393,77 @@ vpViper650::init(vpViper650::vpToolType tool,
parameters, like the hand-to-eye transformation.
*/
-void
-vpViper650::parseConfigFile (const std::string &filename)
+void vpViper650::parseConfigFile(const std::string &filename)
{
- size_t dim;
- int code;
- char Ligne[FILENAME_MAX];
- char namoption[100];
- FILE * fdtask;
- int numLn = 0;
- vpRxyzVector rot_eMc; // rotation
- vpTranslationVector trans_eMc; // translation
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
-
- //vpTRACE("Read the config file for constant parameters %s.", filename.c_str());
- if ((fdtask = fopen(filename.c_str(), "r" )) == NULL) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file %s.", filename.c_str());
- }
-
- while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
- numLn ++;
- if ('#' == Ligne[0]) { continue; }
- {
- std::stringstream ss(Ligne);
- ss >> namoption;
-
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve option name"));
- }
- }
+ vpRxyzVector erc; // eMc rotation
+ vpTranslationVector etc; // eMc translation
+
+ std::ifstream fdconfig(filename.c_str(), std::ios::in);
+
+ if (!fdconfig.is_open()) {
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
+ filename.c_str());
+ }
- dim = strlen(namoption);
+ std::string line;
+ int lineNum = 0;
+ bool get_erc = false;
+ bool get_etc = false;
+ int code;
- for (code = 0; NULL != opt_viper650[code]; ++ code) {
- if (strncmp(opt_viper650[code], namoption, dim) == 0) {
+ while (std::getline(fdconfig, line)) {
+ lineNum++;
+ if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
+ continue;
+ }
+ std::istringstream ss(line);
+ std::string key;
+ ss >> key;
+
+ for (code = 0; NULL != opt_viper650[code]; ++code) {
+ if (key.compare(opt_viper650[code]) == 0) {
break;
}
}
- switch(code) {
+ switch (code) {
case 0:
break; // Nothing to do: camera name
case 1: {
- std::stringstream ss(Ligne);
- ss >> namoption >> rot_eMc[0] >> rot_eMc[1] >> rot_eMc[2];
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve rotation", filename.c_str()));
- }
+ ss >> erc[0] >> erc[1] >> erc[2];
// Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
+ erc = erc * M_PI / 180.0;
+ get_erc = true;
break;
}
case 2: {
- std::stringstream ss(Ligne);
- ss >> namoption >> trans_eMc[0] >> trans_eMc[1] >> trans_eMc[2];
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve translation", filename.c_str()));
- }
- get_trans_eMc = true;
+ ss >> etc[0] >> etc[1] >> etc[2];
+ get_etc = true;
break;
}
default:
- vpERROR_TRACE ("Bad configuration file %s "
- "line #%d.", filename.c_str(), numLn);
- } /* SWITCH */
- } /* WHILE */
+ throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
+ filename.c_str(), lineNum));
+ }
+ }
- fclose (fdtask);
+ fdconfig.close();
// Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- this->set_eMc(trans_eMc,rot_eMc);
- }
- else {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Could not read translation and rotation parameters from config file %s", filename.c_str());
+ if (get_etc && get_erc) {
+ this->set_eMc(etc, erc);
+ } else {
+ throw vpRobotException(vpRobotException::readingParametersError,
+ "Could not read translation and rotation "
+ "parameters from config file %s",
+ filename.c_str());
}
-
- return;
}
-
/*!
Get the current intrinsic camera parameters obtained by calibration.
@@ -501,7 +474,8 @@ vpViper650::parseConfigFile (const std::string &filename)
\warning Thid method needs also an access to the files containing the
camera parameters in XML format. This access is available if
- VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h file.
+ VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER650_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper650.xml
@@ -520,9 +494,9 @@ vpViper650::parseConfigFile (const std::string &filename)
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper650.h>
#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -557,84 +531,61 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
+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)
+#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::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,
+ 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.");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper650::TOOL_PTGREY_FLEA2_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_PTGREY_FLEA2_CAMERA_NAME,
- projModel,
+ 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.");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper650::TOOL_SCHUNK_GRIPPER_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper650::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper650::CONST_CAMERA_FILENAME,
- vpViper650::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
- projModel,
+ 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.");
+ 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,
+ 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.");
+ 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!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -643,8 +594,7 @@ 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
@@ -653,21 +603,20 @@ 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::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
@@ -675,53 +624,49 @@ 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::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper650::TOOL_GENERIC_CAMERA: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper650::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper650::TOOL_CUSTOM: {
- 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.");
+ vpERROR_TRACE("This error should not occur!");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
break;
}
#endif
@@ -738,7 +683,8 @@ vpViper650::getCameraParameters (vpCameraParameters &cam,
\warning Thid method needs also an access to the files containing the
camera parameters in XML format. This access is available if
- VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h file.
+ VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER650_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper650.xml
@@ -753,9 +699,9 @@ vpViper650::getCameraParameters (vpCameraParameters &cam,
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper650.h>
#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -786,14 +732,13 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
+void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ getCameraParameters(cam, I.getWidth(), I.getHeight());
}
/*!
\brief Get the current intrinsic camera parameters obtained by calibration.
@@ -804,8 +749,9 @@ vpViper650::getCameraParameters (vpCameraParameters &cam,
defined in include/visp3/core/vpConfig.h file.
\warning Thid method needs also an access to the files containing the camera
- parameters in XML format. This access is available if VISP_HAVE_VIPER650_DATA
- macro is defined in include/visp3/core/vpConfig.h file.
+ parameters in XML format. This access is available if
+VISP_HAVE_VIPER650_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER650_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper650.xml
@@ -820,9 +766,9 @@ vpViper650::getCameraParameters (vpCameraParameters &cam,
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper650.h>
#include <visp3/robot/vpRobotViper650.h>
+#include <visp3/robot/vpViper650.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -853,13 +799,12 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper650::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
+void vpViper650::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ 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 9932061..8d0506d 100644
--- a/modules/robot/src/real-robot/viper/vpViper850.cpp
+++ b/modules/robot/src/real-robot/viper/vpViper850.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,31 +45,35 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/robot/vpViper850.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/robot/vpViper850.h>
-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");
+ 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");
+ 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");
+ 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");
+ std::string(VISP_VIPER850_DATA_PATH) +
+ 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");
@@ -81,15 +86,13 @@ const std::string vpViper850::CONST_CAMERA_FILENAME =
#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_GENERIC_CAMERA_NAME = "Generic-camera";
+const char *const vpViper850::CONST_MARLIN_F033C_CAMERA_NAME = "Marlin-F033C-12mm";
+const char *const vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME = "PTGrey-Flea2-6mm";
+const char *const vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME = "Schunk-Gripper-PTGrey-Flea2-6mm";
+const char *const vpViper850::CONST_GENERIC_CAMERA_NAME = "Generic-camera";
const vpViper850::vpToolType vpViper850::defaultTool = vpViper850::TOOL_PTGREY_FLEA2_CAMERA;
-
-
/*!
Default constructor.
@@ -130,8 +133,7 @@ vpViper850::vpViper850()
Initialize the robot with the default tool vpViper850::defaultTool.
*/
-void
-vpViper850::init (void)
+void vpViper850::init(void)
{
this->init(vpViper850::defaultTool);
return;
@@ -146,13 +148,12 @@ vpViper850::init (void)
extrinsic parameters.
*/
-void
-vpViper850::init (const std::string &camera_extrinsic_parameters)
+void vpViper850::init(const std::string &camera_extrinsic_parameters)
{
- //vpTRACE ("Parse camera file \""%s\"".", camera_filename);
- this->parseConfigFile (camera_extrinsic_parameters);
+ // vpTRACE ("Parse camera file \""%s\"".", camera_filename);
+ this->parseConfigFile(camera_extrinsic_parameters);
- return ;
+ return;
}
/*!
@@ -176,69 +177,67 @@ 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;
-
+
#ifdef VISP_HAVE_VIPER850_DATA
// Read the robot parameters from files
std::string filename_eMc;
switch (tool) {
case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_MARLIN_F033C_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_MARLIN_F033C_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_PTGREY_FLEA2_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_PTGREY_FLEA2_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_SCHUNK_GRIPPER_WITH_DISTORTION_FILENAME;
break;
}
break;
}
case vpViper850::TOOL_GENERIC_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITHOUT_DISTORTION_FILENAME;
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ filename_eMc = CONST_EMC_GENERIC_WITH_DISTORTION_FILENAME;
break;
}
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!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -250,61 +249,61 @@ vpViper850::init (vpViper850::vpToolType tool,
break;
}
}
-
- this->init (filename_eMc);
-#else // VISP_HAVE_VIPER850_DATA
+ this->init(filename_eMc);
+
+#else // VISP_HAVE_VIPER850_DATA
// Use here default values of the robot constant parameters.
switch (tool) {
case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.26); // rx
- erc[1] = vpMath::rad(2.12); // ry
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ erc[0] = vpMath::rad(0.26); // rx
+ erc[1] = vpMath::rad(2.12); // ry
erc[2] = vpMath::rad(-91.31); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0005; // ty
- etc[2] = 0.1022; // tz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0005; // ty
+ etc[2] = 0.1022; // tz
break;
}
break;
}
case vpViper850::TOOL_PTGREY_FLEA2_CAMERA:
- case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- erc[0] = vpMath::rad(0.15); // rx
- erc[1] = vpMath::rad(1.28); // ry
+ case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ erc[0] = vpMath::rad(0.15); // rx
+ erc[1] = vpMath::rad(1.28); // ry
erc[2] = vpMath::rad(-90.8); // rz
- etc[0] = -0.0456; // tx
- etc[1] = -0.0013; // ty
- etc[2] = 0.001; // tz
+ etc[0] = -0.0456; // tx
+ etc[1] = -0.0013; // ty
+ etc[2] = 0.001; // tz
break;
- case vpCameraParameters::perspectiveProjWithDistortion :
- erc[0] = vpMath::rad(0.72); // rx
- erc[1] = vpMath::rad(2.10); // ry
+ case vpCameraParameters::perspectiveProjWithDistortion:
+ erc[0] = vpMath::rad(0.72); // rx
+ erc[1] = vpMath::rad(2.10); // ry
erc[2] = vpMath::rad(-90.5); // rz
- etc[0] = -0.0444; // tx
- etc[1] = -0.0012; // ty
- etc[2] = 0.078; // tz
+ etc[0] = -0.0444; // tx
+ etc[1] = -0.0012; // ty
+ etc[2] = 0.078; // tz
break;
}
break;
}
case vpViper850::TOOL_GENERIC_CAMERA: {
// Set eMc to identity
- switch(projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
- case vpCameraParameters::perspectiveProjWithDistortion :
+ switch (projModel) {
+ case vpCameraParameters::perspectiveProjWithoutDistortion:
+ case vpCameraParameters::perspectiveProjWithDistortion:
erc[0] = 0; // rx
erc[1] = 0; // ry
erc[2] = 0; // rz
@@ -316,10 +315,10 @@ vpViper850::init (vpViper850::vpToolType tool,
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);
@@ -327,7 +326,7 @@ vpViper850::init (vpViper850::vpToolType tool,
#endif // VISP_HAVE_VIPER850_DATA
setToolType(tool);
- return ;
+ return;
}
/*!
@@ -356,11 +355,11 @@ eMc_ROT_XYZ 10.0 -90.0 20.0
eMc_TRANS_XYZ 0.05 0.01 0.06
\endcode
- \sa init(vpViper850::vpToolType, vpCameraParameters::vpCameraParametersProjType),
- init(vpViper850::vpToolType, const vpHomogeneousMatrix&)
+ \sa init(vpViper850::vpToolType,
+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());
@@ -376,12 +375,12 @@ vpViper850::init(vpViper850::vpToolType tool, const std::string &filename)
\param eMc_ : Homogeneous matrix representation of the transformation
between the end-effector frame and the tool frame.
- \sa init(vpViper850::vpToolType, vpCameraParameters::vpCameraParametersProjType),
+ \sa init(vpViper850::vpToolType,
+ vpCameraParameters::vpCameraParametersProjType),
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_);
@@ -395,99 +394,75 @@ vpViper850::init(vpViper850::vpToolType tool, const vpHomogeneousMatrix &eMc_)
parameters, like the hand-to-eye transformation.
*/
-void
-vpViper850::parseConfigFile (const std::string &filename)
+void vpViper850::parseConfigFile(const std::string &filename)
{
- size_t dim;
- int code;
- char Ligne[FILENAME_MAX];
- char namoption[100];
- FILE * fdtask;
- int numLn = 0;
- vpRxyzVector rot_eMc; // rotation
- vpTranslationVector trans_eMc; // translation
- bool get_rot_eMc = false;
- bool get_trans_eMc = false;
-
- //vpTRACE("Read the config file for constant parameters %s.", filename.c_str());
- if ((fdtask = fopen(filename.c_str(), "r" )) == NULL) {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the config file %s.", filename.c_str());
- }
-
-
- while (fgets(Ligne, FILENAME_MAX, fdtask) != NULL) {
- numLn ++;
- if ('#' == Ligne[0]) { continue; }
- {
- std::stringstream ss(Ligne);
- ss >> namoption;
-
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve option name", filename.c_str()));
- }
- }
+ vpRxyzVector erc; // eMc rotation
+ vpTranslationVector etc; // eMc translation
+
+ std::ifstream fdconfig(filename.c_str(), std::ios::in);
+
+ if (!fdconfig.is_open()) {
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the config file: %s",
+ filename.c_str());
+ }
- dim = strlen(namoption);
+ std::string line;
+ int lineNum = 0;
+ bool get_erc = false;
+ bool get_etc = false;
+ int code;
- for (code = 0; NULL != opt_viper850[code]; ++ code) {
- if (strncmp(opt_viper850[code], namoption, dim) == 0) {
+ while (std::getline(fdconfig, line)) {
+ lineNum++;
+ if ((line.compare(0, 1, "#") == 0) || line.empty()) { // skip comment or empty line
+ continue;
+ }
+ std::istringstream ss(line);
+ std::string key;
+ ss >> key;
+
+ for (code = 0; NULL != opt_viper850[code]; ++code) {
+ if (key.compare(opt_viper850[code]) == 0) {
break;
}
}
- switch(code) {
+ switch (code) {
case 0:
break; // Nothing to do: camera name
case 1: {
- std::stringstream ss(Ligne);
- ss >> namoption >> rot_eMc[0] >> rot_eMc[1] >> rot_eMc[2];
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve rotation", filename.c_str()));
- }
+ ss >> erc[0] >> erc[1] >> erc[2];
// Convert rotation from degrees to radians
- rot_eMc[0] *= M_PI / 180.0;
- rot_eMc[1] *= M_PI / 180.0;
- rot_eMc[2] *= M_PI / 180.0;
- get_rot_eMc = true;
+ erc = erc * M_PI / 180.0;
+ get_erc = true;
break;
}
case 2: {
- std::stringstream ss(Ligne);
- ss >> namoption >> trans_eMc[0] >> trans_eMc[1] >> trans_eMc[2];
- if (ss.fail()) {
- fclose (fdtask);
- throw(vpException(vpException::badValue,
- "Cannot parse configuration file %s to retrieve translation", filename.c_str()));
- }
- get_trans_eMc = true;
+ ss >> etc[0] >> etc[1] >> etc[2];
+ get_etc = true;
break;
}
+
default:
- vpERROR_TRACE ("Bad configuration file %s "
- "ligne #%d.", filename.c_str(), numLn);
- } /* SWITCH */
- } /* WHILE */
+ throw(vpRobotException(vpRobotException::readingParametersError, "Bad configuration file %s line #%d",
+ filename.c_str(), lineNum));
+ }
+ }
- fclose (fdtask);
+ fdconfig.close();
// Compute the eMc matrix from the translations and rotations
- if (get_rot_eMc && get_trans_eMc) {
- this->set_eMc(trans_eMc,rot_eMc);
- }
- else {
- throw vpRobotException (vpRobotException::readingParametersError,
- "Could not read translation and rotation parameters from config file %s", filename.c_str());
+ if (get_etc && get_erc) {
+ this->set_eMc(etc, erc);
+ } else {
+ throw vpRobotException(vpRobotException::readingParametersError,
+ "Could not read translation and rotation "
+ "parameters from config file %s",
+ filename.c_str());
}
-
- return;
}
/*!
@@ -500,7 +475,8 @@ vpViper850::parseConfigFile (const std::string &filename)
\warning Thid method needs also an access to the files containing the
camera parameters in XML format. This access is available if
- VISP_HAVE_VIPER850_DATA macro is defined in include/visp3/core/vpConfig.h file.
+ VISP_HAVE_VIPER850_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER850_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper850.xml
@@ -519,9 +495,9 @@ vpViper850::parseConfigFile (const std::string &filename)
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper850.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -556,84 +532,61 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height) const
+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)
+#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::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,
+ 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.");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME,
- projModel,
+ 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.");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA: {
- std::cout << "Get camera parameters for camera \""
- << vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME << "\"" << std::endl
- << "from the XML file: \""
- << vpViper850::CONST_CAMERA_FILENAME << "\""<< std::endl;
- if (parser.parse(cam,
- vpViper850::CONST_CAMERA_FILENAME,
- vpViper850::CONST_SCHUNK_GRIPPER_CAMERA_NAME,
- projModel,
+ 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.");
+ 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,
+ 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.");
+ 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!");
+ vpERROR_TRACE("This error should not occur!");
// vpERROR_TRACE ("Si elle survient malgre tout, c'est sans doute "
// "que les specs de la classe ont ete modifiee, "
// "et que le code n'a pas ete mis a jour "
@@ -642,9 +595,8 @@ 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
// Set default parameters
@@ -652,21 +604,20 @@ 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::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(1214.0, 1213.0, 323.1, 240.0, -0.1824, 0.1881);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
@@ -674,53 +625,49 @@ 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::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper850::TOOL_GENERIC_CAMERA: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_GENERIC_CAMERA_NAME << "\"" << std::endl;
- switch(this->projModel) {
- case vpCameraParameters::perspectiveProjWithoutDistortion :
+ 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 :
+ case vpCameraParameters::perspectiveProjWithDistortion:
cam.initPersProjWithDistortion(831.3, 831.6, 322.7, 265.8, -0.1955, 0.2047);
break;
}
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
- throw vpRobotException (vpRobotException::readingParametersError,
- "Impossible to read the camera parameters.");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
break;
}
case vpViper850::TOOL_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.");
+ vpERROR_TRACE("This error should not occur!");
+ throw vpRobotException(vpRobotException::readingParametersError, "Impossible to read the camera parameters.");
}
#endif
return;
@@ -736,7 +683,8 @@ vpViper850::getCameraParameters (vpCameraParameters &cam,
\warning Thid method needs also an access to the files containing the
camera parameters in XML format. This access is available if
- VISP_HAVE_VIPER850_DATA macro is defined in include/visp3/core/vpConfig.h file.
+ VISP_HAVE_VIPER850_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER850_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper850.xml
@@ -751,9 +699,9 @@ vpViper850::getCameraParameters (vpCameraParameters &cam,
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper850.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -784,14 +732,13 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I) const
+void vpViper850::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ getCameraParameters(cam, I.getWidth(), I.getHeight());
}
/*!
\brief Get the current intrinsic camera parameters obtained by calibration.
@@ -802,8 +749,9 @@ vpViper850::getCameraParameters (vpCameraParameters &cam,
defined in include/visp3/core/vpConfig.h file.
\warning Thid method needs also an access to the files containing the camera
- parameters in XML format. This access is available if VISP_HAVE_VIPER850_DATA
- macro is defined in include/visp3/core/vpConfig.h file.
+ parameters in XML format. This access is available if
+VISP_HAVE_VIPER850_DATA macro is defined in include/visp3/core/vpConfig.h
+file.
- If VISP_HAVE_VIPER850_DATA and VISP_HAVE_XML2 macros are defined,
this method gets the camera parameters from const_camera_Viper850.xml
@@ -818,9 +766,9 @@ vpViper850::getCameraParameters (vpCameraParameters &cam,
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/robot/vpViper850.h>
#include <visp3/robot/vpRobotViper850.h>
+#include <visp3/robot/vpViper850.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -851,13 +799,12 @@ int main()
}
\endcode
- \exception vpRobotException::readingParametersError : If the camera parameters are not found.
+ \exception vpRobotException::readingParametersError : If the camera
+parameters are not found.
*/
-void
-vpViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I) const
+void vpViper850::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I) const
{
- getCameraParameters(cam,I.getWidth(),I.getHeight());
+ 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 43fb086..49e61cd 100644
--- a/modules/robot/src/robot-simulator/vpRobotCamera.cpp
+++ b/modules/robot/src/robot-simulator/vpRobotCamera.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpRobotCamera.cpp
\brief class that defines the simplest robot : a free flying camera
@@ -45,10 +45,10 @@
#if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotException.h>
/*!
Constructor.
@@ -69,11 +69,7 @@
\endcode
*/
-vpRobotCamera::vpRobotCamera()
- : cMw_()
-{
- init() ;
-}
+vpRobotCamera::vpRobotCamera() : cMw_() { init(); }
/*!
Robot initialisation.
@@ -85,25 +81,22 @@ vpRobotCamera::vpRobotCamera()
void vpRobotCamera::init()
{
nDof = 6;
- eJe.eye(6,6) ;
+ eJe.eye(6, 6);
eJeAvailable = true;
fJeAvailable = false;
areJointLimitsAvailable = false;
qmin = NULL;
qmax = NULL;
- setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
}
-
/*!
Destructor.
*/
-vpRobotCamera::~vpRobotCamera()
-{
-}
+vpRobotCamera::~vpRobotCamera() {}
/*!
@@ -111,12 +104,11 @@ vpRobotCamera::~vpRobotCamera()
frame. This transformation allows to compute a velocity expressed
in the end-effector frame into the camera frame.
- \param cVe : Twist transformation. Here this transformation is equal to identity
- since camera frame and end-effector frame are at the same location.
+ \param cVe : Twist transformation. Here this transformation is equal to
+ identity since camera frame and end-effector frame are at the same location.
*/
-void
-vpRobotCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpRobotCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
{
vpVelocityTwistMatrix cVe_;
cVe = cVe_;
@@ -129,11 +121,7 @@ vpRobotCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
\param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
J}_e\f$ expressed in the end-effector frame.
*/
-void
-vpRobotCamera::get_eJe(vpMatrix &eJe_)
-{
- eJe_ = this->eJe ;
-}
+void vpRobotCamera::get_eJe(vpMatrix &eJe_) { eJe_ = this->eJe; }
/*!
Send to the controller a velocity.
@@ -141,79 +129,64 @@ vpRobotCamera::get_eJe(vpMatrix &eJe_)
\param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
and camera frame (vpRobot::CAMERA_FRAME) are implemented.
- \param v : Velocity to apply to the robot.
+ \param v : Velocity twist to apply to the robot.
- - In the camera frame, this velocity is represented by a vector of dimension 6
- \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+ - In the camera frame, this velocity is represented by a twist vector of
+ dimension 6 \f$ {\bf v} = [v_x v_y v_z w_x w_y w_z]^t \f$ where \f$ v_x,
+ v_y, v_z \f$ are the translation velocities in m/s and \f$ w_x, w_y, w_z
+ \f$ the rotation velocities in rad/s applied in the camera frame.
- - In articular, this velocity is represented by a 6 dimension vector \f$
- \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
- u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
- robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
- here set to identity.
+ - In articular, the behavior is the same as in camera frame.
- We use the exponential map (vpExponentialMap) to update the camera location.
- Sampling time can be set using setSamplingTime().
+ Internally, the exponential map (vpExponentialMap) is used to update the
+ camera position from its velocity after applying the velocity during a
+ sampling time. This sampling time can be set using setSamplingTime().
\sa setSamplingTime()
*/
-void
-vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame,
- const vpColVector &v)
+void vpRobotCamera::setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &v)
{
- switch (frame)
- {
+ switch (frame) {
case vpRobot::ARTICULAR_FRAME:
case vpRobot::CAMERA_FRAME: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
- vpColVector v_max(6);
+ vpColVector v_max(6);
- for (unsigned int i=0; i<3; i++)
- v_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- v_max[i] = getMaxRotationVelocity();
+ for (unsigned int i = 0; i < 3; i++)
+ v_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i = 3; i < 6; i++)
+ v_max[i] = getMaxRotationVelocity();
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
- this->cMw_ = vpExponentialMap::direct(v_sat, delta_t_).inverse()*this->cMw_ ;
- break ;
- }
+ this->cMw_ = vpExponentialMap::direct(v_sat, delta_t_).inverse() * this->cMw_;
+ break;
+ }
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the reference frame:"
- "functionality not implemented");
- break ;
+ vpERROR_TRACE("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the reference frame:"
+ "functionality not implemented");
+ break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
+ vpERROR_TRACE("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
- break ;
+ break;
}
}
-
/*!
- Get the robot position as the transformation from camera frame to world 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.
@@ -222,7 +195,8 @@ vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const
- in the camera cartesien frame,
- joint (articular) coordinates of each axes
- in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+ - in a mixt cartesien frame (translation in reference frame, and rotation in
+ camera frame)
\param position : Measured position of the robot:
- in camera cartesien frame, a 6 dimension vector, set to 0.
@@ -237,15 +211,15 @@ vpRobotCamera::getPosition(vpHomogeneousMatrix &cMw) const
*/
void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
- q.resize (6);
+ q.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME :
+ case vpRobot::CAMERA_FRAME:
q = 0;
break;
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
// Convert wMc_ to a position
// From fMc extract the pose
vpRotationMatrix cRw;
@@ -253,32 +227,33 @@ void vpRobotCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVe
vpRxyzVector rxyz;
rxyz.buildFrom(cRw);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
q[i] = this->cMw_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ q[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
break;
- }
- case vpRobot::MIXT_FRAME :
+ }
+ case vpRobot::MIXT_FRAME:
std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
}
}
/*!
- Set the robot position as the transformation from camera frame to world frame.
+ Set the robot position as the transformation from camera frame to world
+ frame.
*/
-void
-vpRobotCamera::setPosition(const vpHomogeneousMatrix &cMw)
+void vpRobotCamera::setPosition(const vpHomogeneousMatrix &cMw)
{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
- this->cMw_ = cMw ;
+ this->cMw_ = cMw;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotCamera.cpp.o) has no symbols
-void dummy_vpRobotCamera() {};
+// Work arround to avoid warning: libvisp_robot.a(vpRobotCamera.cpp.o) has no
+// symbols
+void dummy_vpRobotCamera(){};
#endif
diff --git a/modules/robot/src/robot-simulator/vpRobotSimulator.cpp b/modules/robot/src/robot-simulator/vpRobotSimulator.cpp
index c783ffa..272c198 100644
--- a/modules/robot/src/robot-simulator/vpRobotSimulator.cpp
+++ b/modules/robot/src/robot-simulator/vpRobotSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,6 +43,4 @@
/*!
Basic constructor that sets the sampling time by default to 40ms.
*/
-vpRobotSimulator::vpRobotSimulator() : vpRobot(), delta_t_(0.040f)
-{
-}
+vpRobotSimulator::vpRobotSimulator() : vpRobot(), delta_t_(0.040f) {}
diff --git a/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp
index fa1e772..c04de6d 100644
--- a/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp
+++ b/modules/robot/src/robot-simulator/vpRobotWireFrameSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,38 +43,37 @@
#include <visp3/robot/vpSimulatorViper850.h>
#include "../wireframe-simulator/vpBound.h"
-#include "../wireframe-simulator/vpVwstack.h"
#include "../wireframe-simulator/vpScene.h"
+#include "../wireframe-simulator/vpVwstack.h"
/*!
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);
- I.resize(480,640);
+ I.resize(480, 640);
I = 255;
#if defined(VISP_HAVE_DISPLAY)
- display.init(I, 0, 0,"The External view");
+ display.init(I, 0, 0, "The External view");
#endif
- //pid_t pid = getpid();
- // setpriority (PRIO_PROCESS, pid, 19);
+ // pid_t pid = getpid();
+ // setpriority (PRIO_PROCESS, pid, 19);
}
/*!
@@ -81,8 +81,8 @@ 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(),
@@ -93,187 +93,191 @@ vpRobotWireFrameSimulator::vpRobotWireFrameSimulator(bool do_display)
#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);
- I.resize(480,640);
+ I.resize(480, 640);
I = 255;
-
+
#if defined(VISP_HAVE_DISPLAY)
if (do_display)
- this->display.init(I, 0, 0,"The External view");
-#endif
-
- //pid_t pid = getpid();
- // setpriority (PRIO_PROCESS, pid, 19);
-}
-
+ this->display.init(I, 0, 0, "The External view");
+#endif
+ // pid_t pid = getpid();
+ // setpriority (PRIO_PROCESS, pid, 19);
+}
/*!
Basic destructor
*/
-vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator()
-{
-}
+vpRobotWireFrameSimulator::~vpRobotWireFrameSimulator() {}
/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+ Initialize the display. It enables to choose the type of scene which will be
+ used to display the object at the current position and at the desired
+ position.
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
+ It exists several default scenes you can use. Use the vpSceneObject and the
+ vpSceneDesiredObject attributes to use them in this method. The
+ corresponding files are stored in the "data" folder which is in the ViSP
+ build directory.
+
+ \param obj : Type of scene used to display the object at the current
+ position. \param desired_object : Type of scene used to display the object
+ at the desired pose (in the internal view).
*/
-void
-vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
+void vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
vpWireFrameSimulator::initScene(obj, desired_object);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
displayCamera = false;
}
/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file.
- It is also possible to use a vrml (.wrl) file.
+ Initialize the display. It enables to choose the type of scene which will be
+ used to display the object at the current position and at the desired
+ position.
+
+ Here you can use the scene you want. You have to set the path to the .bnd
+ file which is a scene file. It is also possible to use a vrml (.wrl) file.
\param obj : Path to the scene file you want to use.
\param desired_object : Path to the scene file you want to use.
*/
-void
-vpRobotWireFrameSimulator::initScene(const char* obj, const char* desired_object)
+void vpRobotWireFrameSimulator::initScene(const char *obj, const char *desired_object)
{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
vpWireFrameSimulator::initScene(obj, desired_object);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
displayCamera = false;
}
/*!
- Initialize the display. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+ Initialize the display. It enables to choose the type of object which will
+ be used to display the object at the current position. The object at the
+ desired position is not displayed.
- \param obj : Type of scene used to display the object at the current position.
+ It exists several default scenes you can use. Use the vpSceneObject
+ attributes to use them in this method. The corresponding files are stored in
+ the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current
+ position.
*/
-void
-vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
+void vpRobotWireFrameSimulator::initScene(const vpSceneObject &obj)
{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
vpWireFrameSimulator::initScene(obj);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
displayCamera = false;
}
/*!
- Initialize the display. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to the .bnd file which is a scene file, or the vrml file.
+ Initialize the display. It enables to choose the type of scene which will be
+ used to display the object at the current position. The object at the
+ desired position is not displayed.
+
+ Here you can use the scene you want. You have to set the path to the .bnd
+ file which is a scene file, or the vrml file.
\param obj : Path to the scene file you want to use.
*/
-void
-vpRobotWireFrameSimulator::initScene(const char* obj)
+void vpRobotWireFrameSimulator::initScene(const char *obj)
{
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
vpWireFrameSimulator::initScene(obj);
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
displayCamera = false;
}
/*!
Get the view of the camera's robot.
-
- According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
-
+
+ According to the initialisation method you used, the current position and
+ maybee the desired position of the object are displayed.
+
\param I_ : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
+void vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
{
-
+
if (!sceneInitialized)
throw;
double u;
double v;
- //if(px_int != 1 && py_int != 1)
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_int);
- v = (double)I_.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I_.getWidth() / (2 * px_int);
+ v = (double)I_.getHeight() / (2 * py_int);
+ } else {
+ u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
+ v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
}
- float o44c[4][4],o44cd[4][4],x,y,z;
+ float o44c[4][4], o44cd[4][4], x, y, z;
Matrix id = IDENTITY_MATRIX;
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
get_fMi(fMit);
- this->cMo = fMit[size_fMi-1].inverse()*fMo;
- this->cMo = rotz*cMo;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- while (get_displayBusy()) vpTime::wait(2);
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ this->cMo = fMit[size_fMi - 1].inverse() * fMo;
+ this->cMo = rotz * cMo;
+
+ vp2jlc_matrix(cMo.inverse(), o44c);
+ vp2jlc_matrix(cdMo.inverse(), o44cd);
+
+ while (get_displayBusy())
+ vpTime::wait(2);
+
+ add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
x = o44c[2][0] + o44c[3][0];
y = o44c[2][1] + o44c[3][1];
z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
+ add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
if (displayObject)
- display_scene(id,this->scene,I_, curColor);
+ display_scene(id, this->scene, I_, curColor);
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
x = o44cd[2][0] + o44cd[3][0];
y = o44cd[2][1] + o44cd[3][1];
z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
- else display_scene(id,desiredScene,I_, desColor);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
+ add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if (displayDesiredObject) {
+ if (desiredObject == D_TOOL)
+ display_scene(o44cd, desiredScene, I_, vpColor::red);
+ else
+ display_scene(id, desiredScene, I_, desColor);
}
delete[] fMit;
set_displayBusy(false);
@@ -281,74 +285,73 @@ vpRobotWireFrameSimulator::getInternalView(vpImage<vpRGBa> &I_)
/*!
Get the view of the camera's robot.
-
- According to the initialisation method you used, the current position and maybee the desired position of the object are displayed.
-
+
+ According to the initialisation method you used, the current position and
+ maybee the desired position of the object are displayed.
+
\param I_ : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
+void vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
{
-
+
if (!sceneInitialized)
throw;
double u;
double v;
- //if(px_int != 1 && py_int != 1)
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_int);
+ v = (double)I.getHeight() / (2 * py_int);
+ } else {
+ u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
+ v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
}
- float o44c[4][4],o44cd[4][4],x,y,z;
+ float o44c[4][4], o44cd[4][4], x, y, z;
Matrix id = IDENTITY_MATRIX;
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
get_fMi(fMit);
- this->cMo = fMit[size_fMi-1].inverse()*fMo;
- this->cMo = rotz*cMo;
-
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- while (get_displayBusy()) vpTime::wait(2);
-
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ this->cMo = fMit[size_fMi - 1].inverse() * fMo;
+ this->cMo = rotz * cMo;
+
+ vp2jlc_matrix(cMo.inverse(), o44c);
+ vp2jlc_matrix(cdMo.inverse(), o44cd);
+
+ while (get_displayBusy())
+ vpTime::wait(2);
+
+ add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
x = o44c[2][0] + o44c[3][0];
y = o44c[2][1] + o44c[3][1];
z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayObject)
- {
- display_scene(id,this->scene,I_, curColor);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
+ add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if (displayObject) {
+ display_scene(id, this->scene, I_, curColor);
}
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
x = o44cd[2][0] + o44cd[3][0];
y = o44cd[2][1] + o44cd[3][1];
z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I_, vpColor::red);
- else display_scene(id,desiredScene,I_, desColor);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
+ add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if (displayDesiredObject) {
+ if (desiredObject == D_TOOL)
+ display_scene(o44cd, desiredScene, I_, vpColor::red);
+ else
+ display_scene(id, desiredScene, I_, desColor);
}
delete[] fMit;
set_displayBusy(false);
@@ -356,21 +359,21 @@ vpRobotWireFrameSimulator::getInternalView(vpImage<unsigned char> &I_)
/*!
Get the pose between the object and the robot's camera.
-
+
\return The pose between the object and the fixed world frame.
*/
-vpHomogeneousMatrix
-vpRobotWireFrameSimulator::get_cMo()
+vpHomogeneousMatrix vpRobotWireFrameSimulator::get_cMo()
{
vpHomogeneousMatrix cMoTemp;
- vpHomogeneousMatrix* fMit = new vpHomogeneousMatrix[size_fMi];
+ vpHomogeneousMatrix *fMit = new vpHomogeneousMatrix[size_fMi];
get_fMi(fMit);
- cMoTemp = fMit[size_fMi-1].inverse()*fMo;
+ cMoTemp = fMit[size_fMi - 1].inverse() * fMo;
delete[] fMit;
return cMoTemp;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
-void dummy_vpRobotWireFrameSimulator() {};
+// Work arround to avoid warning:
+// libvisp_robot.a(vpRobotWireFrameSimulator.cpp.o) has no symbols
+void dummy_vpRobotWireFrameSimulator(){};
#endif
diff --git a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp
index 2d8d6da..cbf2e03 100644
--- a/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp
+++ b/modules/robot/src/robot-simulator/vpSimulatorAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,25 +36,23 @@
*
*****************************************************************************/
-
-
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
-#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpTime.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <string>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpTime.h>
#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpIoTools.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <string>
+#include <visp3/robot/vpSimulatorAfma6.h>
#include "../wireframe-simulator/vpBound.h"
-#include "../wireframe-simulator/vpVwstack.h"
#include "../wireframe-simulator/vpRfstack.h"
#include "../wireframe-simulator/vpScene.h"
+#include "../wireframe-simulator/vpVwstack.h"
const double vpSimulatorAfma6::defaultPositioningVelocity = 25.0;
@@ -61,51 +60,49 @@ 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();
-
+
tcur = vpTime::measureTimeMs();
- #if defined(_WIN32)
-# ifdef WINRT_8_1
+#if defined(_WIN32)
+#ifdef WINRT_8_1
mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
-# else
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
+#else
+ mutex_fMi = CreateMutex(NULL, FALSE, NULL);
+ mutex_artVel = CreateMutex(NULL, FALSE, NULL);
+ mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
+ mutex_velocity = CreateMutex(NULL, FALSE, NULL);
+ mutex_display = CreateMutex(NULL, FALSE, NULL);
#endif
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined (VISP_HAVE_PTHREAD)
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+#elif defined(VISP_HAVE_PTHREAD)
pthread_mutex_init(&mutex_fMi, NULL);
pthread_mutex_init(&mutex_artVel, NULL);
pthread_mutex_init(&mutex_artCoord, NULL);
pthread_mutex_init(&mutex_velocity, NULL);
pthread_mutex_init(&mutex_display, NULL);
-
+
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
+
pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
+#endif
+
compute_fMi();
}
@@ -116,16 +113,15 @@ 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();
-
+
tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
+
+#if defined(_WIN32)
#ifdef WINRT_8_1
mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
@@ -140,27 +136,26 @@ vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
mutex_display = CreateMutex(NULL, FALSE, NULL);
#endif
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined(VISP_HAVE_PTHREAD)
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+#elif defined(VISP_HAVE_PTHREAD)
pthread_mutex_init(&mutex_fMi, NULL);
pthread_mutex_init(&mutex_artVel, NULL);
pthread_mutex_init(&mutex_artCoord, NULL);
pthread_mutex_init(&mutex_velocity, NULL);
pthread_mutex_init(&mutex_display, NULL);
-
+
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
+
pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
+#endif
+
compute_fMi();
}
@@ -170,20 +165,20 @@ vpSimulatorAfma6::vpSimulatorAfma6(bool do_display)
vpSimulatorAfma6::~vpSimulatorAfma6()
{
robotStop = true;
-
- #if defined(_WIN32)
-# if defined(WINRT_8_1)
+
+#if defined(_WIN32)
+#if defined(WINRT_8_1)
WaitForSingleObjectEx(hThread, INFINITE, FALSE);
-# else // pure win32
+#else // pure win32
WaitForSingleObject(hThread, INFINITE);
-# endif
+#endif
CloseHandle(hThread);
CloseHandle(mutex_fMi);
CloseHandle(mutex_artVel);
CloseHandle(mutex_artCoord);
CloseHandle(mutex_velocity);
CloseHandle(mutex_display);
- #elif defined(VISP_HAVE_PTHREAD)
+#elif defined(VISP_HAVE_PTHREAD)
pthread_attr_destroy(&attr);
pthread_join(thread, NULL);
pthread_mutex_destroy(&mutex_fMi);
@@ -191,12 +186,11 @@ vpSimulatorAfma6::~vpSimulatorAfma6()
pthread_mutex_destroy(&mutex_artCoord);
pthread_mutex_destroy(&mutex_velocity);
pthread_mutex_destroy(&mutex_display);
- #endif
-
- if (robotArms != NULL)
- {
- for(int i = 0; i < 6; i++)
- free_Bound_scene (&(robotArms[i]));
+#endif
+
+ if (robotArms != NULL) {
+ for (int i = 0; i < 6; i++)
+ free_Bound_scene(&(robotArms[i]));
}
delete[] robotArms;
@@ -211,25 +205,23 @@ vpSimulatorAfma6::~vpSimulatorAfma6()
not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
variable that the user has to set.
*/
-void
-vpSimulatorAfma6::init()
+void vpSimulatorAfma6::init()
{
// set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
// VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
bool armDirExists = false;
- for(size_t i=0; i < arm_dirs.size(); i++)
- if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
- arm_dir = arm_dirs[i];
- armDirExists = true;
- break;
- }
- if (! armDirExists) {
+ for (size_t i = 0; i < arm_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
+ arm_dir = arm_dirs[i];
+ armDirExists = true;
+ break;
+ }
+ if (!armDirExists) {
try {
arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
}
}
@@ -246,20 +238,22 @@ vpSimulatorAfma6::init()
zeroPos = 0;
reposPos.resize(njoint);
reposPos = 0;
- reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
-
+ reposPos[1] = -M_PI / 2;
+ reposPos[2] = M_PI;
+ reposPos[4] = M_PI / 2;
+
artCoord = zeroPos;
artVel = 0;
-
+
q_prev_getdis.resize(njoint);
q_prev_getdis = 0;
first_time_getdis = true;
-
- positioningVelocity = defaultPositioningVelocity ;
-
- setRobotFrame (vpRobot::ARTICULAR_FRAME);
+
+ positioningVelocity = defaultPositioningVelocity;
+
+ setRobotFrame(vpRobot::ARTICULAR_FRAME);
this->setRobotState(vpRobot::STATE_STOP);
-
+
// Software joint limits in radians
//_joint_min.resize(njoint);
_joint_min[0] = -0.6501;
@@ -280,30 +274,29 @@ vpSimulatorAfma6::init()
/*!
Method which initialises the parameters linked to the display part.
*/
-void
-vpSimulatorAfma6::initDisplay()
+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);
+ getCameraParameters(tmp, 640, 480);
px_int = tmp.get_px();
py_int = tmp.get_py();
sceneInitialized = true;
}
-
/*!
Initialize the robot kinematics with the extrinsic calibration
parameters associated to a specific camera.
The eMc parameters depend on the camera.
-
+
\warning Only perspective projection without distortion is available!
\param tool : Tool to use. Note that the generic camera is not handled.
@@ -312,102 +305,100 @@ 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"
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) {
case vpAfma6::TOOL_CCMOP: {
- _erc[0] = vpMath::rad(164.35); // rx
- _erc[1] = vpMath::rad( 89.64); // ry
- _erc[2] = vpMath::rad(-73.05); // rz
- _etc[0] = 0.0117; // tx
- _etc[1] = 0.0033; // ty
- _etc[2] = 0.2272; // tz
-
- setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_ccmop.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
+ _erc[0] = vpMath::rad(164.35); // rx
+ _erc[1] = vpMath::rad(89.64); // ry
+ _erc[2] = vpMath::rad(-73.05); // rz
+ _etc[0] = 0.0117; // tx
+ _etc[1] = 0.0033; // ty
+ _etc[2] = 0.2272; // tz
+
+ setCameraParameters(vpCameraParameters(1109.5735473989, 1112.1520168160, 320, 240));
+
+ if (robotArms != NULL) {
+ while (get_displayBusy())
+ vpTime::wait(2);
+ free_Bound_scene(&(robotArms[5]));
+ char *name_arm = new char[full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm, "/afma6_tool_ccmop.bnd");
+ set_scene(name_arm, robotArms + 5, 1.0);
+ set_displayBusy(false);
+ delete[] name_arm;
}
+ break;
+ }
case vpAfma6::TOOL_GRIPPER: {
- _erc[0] = vpMath::rad( 88.33); // rx
- _erc[1] = vpMath::rad( 72.07); // ry
- _erc[2] = vpMath::rad( 2.53); // rz
- _etc[0] = 0.0783; // tx
- _etc[1] = 0.1234; // ty
- _etc[2] = 0.1638; // tz
-
- setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
- char *name_arm = new char [full_length];
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_gripper.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
+ _erc[0] = vpMath::rad(88.33); // rx
+ _erc[1] = vpMath::rad(72.07); // ry
+ _erc[2] = vpMath::rad(2.53); // rz
+ _etc[0] = 0.0783; // tx
+ _etc[1] = 0.1234; // ty
+ _etc[2] = 0.1638; // tz
+
+ setCameraParameters(vpCameraParameters(852.6583228197, 854.8084224761, 320, 240));
+
+ if (robotArms != NULL) {
+ while (get_displayBusy())
+ vpTime::wait(2);
+ free_Bound_scene(&(robotArms[5]));
+ char *name_arm = new char[full_length];
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm, "/afma6_tool_gripper.bnd");
+ set_scene(name_arm, robotArms + 5, 1.0);
+ set_displayBusy(false);
+ delete[] name_arm;
}
+ break;
+ }
case vpAfma6::TOOL_VACUUM: {
- _erc[0] = vpMath::rad( 90.40); // rx
- _erc[1] = vpMath::rad( 75.11); // ry
- _erc[2] = vpMath::rad( 0.18); // rz
- _etc[0] = 0.0038; // tx
- _etc[1] = 0.1281; // ty
- _etc[2] = 0.1658; // tz
-
- setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
-
- if (robotArms != NULL)
- {
- while (get_displayBusy()) vpTime::wait(2);
- free_Bound_scene (&(robotArms[5]));
-
- char *name_arm = new char [full_length];
-
- strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_tool_vacuum.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
- set_displayBusy(false);
- delete [] name_arm;
- }
- break;
+ _erc[0] = vpMath::rad(90.40); // rx
+ _erc[1] = vpMath::rad(75.11); // ry
+ _erc[2] = vpMath::rad(0.18); // rz
+ _etc[0] = 0.0038; // tx
+ _etc[1] = 0.1281; // ty
+ _etc[2] = 0.1658; // tz
+
+ setCameraParameters(vpCameraParameters(853.4876600807, 856.0339170706, 320, 240));
+
+ if (robotArms != NULL) {
+ while (get_displayBusy())
+ vpTime::wait(2);
+ free_Bound_scene(&(robotArms[5]));
+
+ char *name_arm = new char[full_length];
+
+ strcpy(name_arm, arm_dir.c_str());
+ strcat(name_arm, "/afma6_tool_vacuum.bnd");
+ set_scene(name_arm, robotArms + 5, 1.0);
+ set_displayBusy(false);
+ delete[] name_arm;
}
+ break;
+ }
case vpAfma6::TOOL_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;
+ }
}
-
+
vpRotationMatrix eRc(_erc);
this->_eMc.buildFrom(_etc, eRc);
-
+
setToolType(tool);
- return ;
+ return;
}
/*!
@@ -416,54 +407,50 @@ vpSimulatorAfma6::init (vpAfma6::vpAfma6ToolType tool,
\param cam : In output, camera parameters to fill.
\param image_width : Image width used to compute camera calibration.
\param image_height : Image height used to compute camera calibration.
-
+
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height)
+void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
+ const unsigned int &image_height)
{
- if (toolCustom)
- {
- vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
+ if (toolCustom) {
+ 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);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_CCMOP_CAMERA_NAME << "\""
+ << std::endl;
+ cam.initPersProjWithoutDistortion(1109.5735473989, 1112.1520168160, 320, 240);
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
}
break;
}
case vpAfma6::TOOL_GRIPPER: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\"" << std::endl;
- cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ std::cout << "Get default camera parameters for camera \"" << vpAfma6::CONST_GRIPPER_CAMERA_NAME << "\""
+ << std::endl;
+ cam.initPersProjWithoutDistortion(852.6583228197, 854.8084224761, 320, 240);
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
}
break;
}
case vpAfma6::TOOL_CUSTOM:
case vpAfma6::TOOL_GENERIC_CAMERA:
case vpAfma6::TOOL_VACUUM: {
- std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
- break;
+ std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
+ break;
}
- default:
- vpERROR_TRACE ("This error should not occur!");
+ default:
+ vpERROR_TRACE("This error should not occur!");
break;
}
return;
@@ -477,11 +464,9 @@ vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I_)
+void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I_)
{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+ getCameraParameters(cam, I_.getWidth(), I_.getHeight());
}
/*!
@@ -492,183 +477,181 @@ vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorAfma6::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I_)
+void vpSimulatorAfma6::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I_)
{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+ getCameraParameters(cam, I_.getWidth(), I_.getHeight());
}
-
/*!
Set the intrinsic camera parameters.
-
+
\param cam : The desired camera parameters.
*/
-void
-vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
+void vpSimulatorAfma6::setCameraParameters(const vpCameraParameters &cam)
{
px_int = cam.get_px();
py_int = cam.get_py();
toolCustom = true;
}
-
/*!
- Method lauched by the thread to compute the position of the robot in the articular frame.
+ Method lauched by the thread to compute the position of the robot in the
+ articular frame.
*/
-void
-vpSimulatorAfma6::updateArticularPosition()
+void vpSimulatorAfma6::updateArticularPosition()
{
- double tcur_1 = tcur;// temporary variable used to store the last time since the last command
+ double tcur_1 = tcur; // temporary variable used to store the last time
+ // since the last command
- while (!robotStop)
- {
- //Get current time
+ while (!robotStop) {
+ // Get current time
tprev = tcur_1;
tcur = vpTime::measureTimeMs();
-
- if(setVelocityCalled || !constantSamplingTimeMode){
+
+ if (setVelocityCalled || !constantSamplingTimeMode) {
setVelocityCalled = false;
-
+
computeArticularVelocity();
-
+
double ellapsedTime = (tcur - tprev) * 1e-3;
- if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
+ if (constantSamplingTimeMode) { // if we want a constant velocity, we
+ // force the ellapsed time to the given
+ // samplingTime
ellapsedTime = getSamplingTime(); // in second
}
-
+
vpColVector articularCoordinates = get_artCoord();
vpColVector articularVelocities = get_artVel();
-
- if (jointLimit)
- {
- double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
- if (art <= _joint_min[jointLimitArt-1] || art >= _joint_max[jointLimitArt-1]) {
+
+ if (jointLimit) {
+ double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
+ if (art <= _joint_min[jointLimitArt - 1] || art >= _joint_max[jointLimitArt - 1]) {
if (verbose_) {
- std::cout << "Joint " << jointLimitArt-1
- << " reaches a limit: " << vpMath::deg(_joint_min[jointLimitArt-1]) << " < "
- << vpMath::deg(art) << " < " << vpMath::deg(_joint_max[jointLimitArt-1]) << std::endl;
+ 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
+ } 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 && jointLimit == false) {
if (jl < 0)
- ellapsedTime = (_joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(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)])/(articularVelocities[(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);
}
set_artCoord(articularCoordinates);
set_artVel(articularVelocities);
-
+
compute_fMi();
-
- if (displayAllowed)
- {
+
+ 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)
- {
- while (get_displayBusy()) vpTime::wait(2);
+
+ if (displayType == MODEL_3D && displayAllowed) {
+ while (get_displayBusy())
+ vpTime::wait(2);
vpSimulatorAfma6::getExternalImage(I);
set_displayBusy(false);
}
-
-
- if (0/*displayType == MODEL_DH && displayAllowed*/)
- {
+
+ if (0 /*displayType == MODEL_DH && displayAllowed*/) {
vpHomogeneousMatrix fMit[8];
get_fMi(fMit);
-
- //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
+
+ // vpDisplay::displayFrame(I,getExternalCameraPosition
+ // ()*fMi[6],cameraParam,0.2,vpColor::none);
vpImagePoint iP, iP_1;
- vpPoint pt(0,0,0);
-
- pt.track(getExternalCameraPosition ());
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
- pt.track(getExternalCameraPosition ()*fMit[0]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
- for (unsigned int k = 1; k < 7; k++)
- {
- pt.track(getExternalCameraPosition ()*fMit[k-1]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
-
- pt.track(getExternalCameraPosition ()*fMit[k]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
-
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
+ vpPoint pt(0, 0, 0);
+
+ pt.track(getExternalCameraPosition());
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
+ pt.track(getExternalCameraPosition() * fMit[0]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
+ vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
+ for (unsigned int k = 1; k < 7; k++) {
+ pt.track(getExternalCameraPosition() * fMit[k - 1]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
+
+ pt.track(getExternalCameraPosition() * fMit[k]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
+
+ vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
}
- vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
+ vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green,
+ thickness_);
}
-
+
vpDisplay::flush(I);
-
-
- vpTime::wait( tcur, 1000*getSamplingTime() );
+
+ vpTime::wait(tcur, 1000 * getSamplingTime());
tcur_1 = tcur;
- }else{
+ } else {
vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
}
}
}
/*!
- Compute the pose between the robot reference frame and the frames used used to compute the Denavit-Hartenberg representation. The last element of the table corresponds to the pose between the reference frame and the camera frame.
-
- To compute the diferent homogeneous matrices, this function needs the articular coordinates as input.
-
- Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,\f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
+ Compute the pose between the robot reference frame and the frames used used
+ to compute the Denavit-Hartenberg representation. The last element of the
+ table corresponds to the pose between the reference frame and the camera
+ frame.
+
+ To compute the diferent homogeneous matrices, this function needs the
+ articular coordinates as input.
+
+ Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$
+ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,\f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$
+ fMc \f$ - where w is for wrist and e for effector-.
*/
-void
-vpSimulatorAfma6::compute_fMi()
+void vpSimulatorAfma6::compute_fMi()
{
- //vpColVector q = get_artCoord();
- vpColVector q(6);//; = get_artCoord();
+ // vpColVector q = get_artCoord();
+ vpColVector q(6); //; = get_artCoord();
q = get_artCoord();
-
+
vpHomogeneousMatrix fMit[8];
-
+
double q1 = q[0];
double q2 = q[1];
double q3 = q[2];
double q4 = q[3];
double q5 = q[4];
double q6 = q[5];
-
+
double c4 = cos(q4);
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
double c6 = cos(q6);
double s6 = sin(q6);
-
+
fMit[0][0][0] = 1;
fMit[0][1][0] = 0;
fMit[0][2][0] = 0;
@@ -681,7 +664,7 @@ vpSimulatorAfma6::compute_fMi()
fMit[0][0][3] = q1;
fMit[0][1][3] = 0;
fMit[0][2][3] = 0;
-
+
fMit[1][0][0] = 1;
fMit[1][1][0] = 0;
fMit[1][2][0] = 0;
@@ -694,7 +677,7 @@ vpSimulatorAfma6::compute_fMi()
fMit[1][0][3] = q1;
fMit[1][1][3] = q2;
fMit[1][2][3] = 0;
-
+
fMit[2][0][0] = 1;
fMit[2][1][0] = 0;
fMit[2][2][0] = 0;
@@ -707,7 +690,7 @@ vpSimulatorAfma6::compute_fMi()
fMit[2][0][3] = q1;
fMit[2][1][3] = q2;
fMit[2][2][3] = q3;
-
+
fMit[3][0][0] = s4;
fMit[3][1][0] = -c4;
fMit[3][2][0] = 0;
@@ -720,33 +703,33 @@ vpSimulatorAfma6::compute_fMi()
fMit[3][0][3] = q1;
fMit[3][1][3] = q2;
fMit[3][2][3] = q3;
-
- fMit[4][0][0] = s4*s5;
- fMit[4][1][0] = -c4*s5;
+
+ fMit[4][0][0] = s4 * s5;
+ fMit[4][1][0] = -c4 * s5;
fMit[4][2][0] = c5;
- fMit[4][0][1] = s4*c5;
- fMit[4][1][1] = -c4*c5;
+ fMit[4][0][1] = s4 * c5;
+ fMit[4][1][1] = -c4 * c5;
fMit[4][2][1] = -s5;
fMit[4][0][2] = c4;
fMit[4][1][2] = s4;
fMit[4][2][2] = 0;
- fMit[4][0][3] = c4*this->_long_56+q1;
- fMit[4][1][3] = s4*this->_long_56+q2;
+ fMit[4][0][3] = c4 * this->_long_56 + q1;
+ fMit[4][1][3] = s4 * this->_long_56 + q2;
fMit[4][2][3] = q3;
-
- fMit[5][0][0] = s4*s5*c6+c4*s6;
- fMit[5][1][0] = -c4*s5*c6+s4*s6;
- fMit[5][2][0] = c5*c6;
- fMit[5][0][1] = -s4*s5*s6+c4*c6;
- fMit[5][1][1] = c4*s5*s6+s4*c6;
- fMit[5][2][1] = -c5*s6;
- fMit[5][0][2] = -s4*c5;
- fMit[5][1][2] = c4*c5;
+
+ fMit[5][0][0] = s4 * s5 * c6 + c4 * s6;
+ fMit[5][1][0] = -c4 * s5 * c6 + s4 * s6;
+ fMit[5][2][0] = c5 * c6;
+ fMit[5][0][1] = -s4 * s5 * s6 + c4 * c6;
+ fMit[5][1][1] = c4 * s5 * s6 + s4 * c6;
+ fMit[5][2][1] = -c5 * s6;
+ fMit[5][0][2] = -s4 * c5;
+ fMit[5][1][2] = c4 * c5;
fMit[5][2][2] = s5;
- fMit[5][0][3] = c4*this->_long_56+q1;
- fMit[5][1][3] = s4*this->_long_56+q2;
+ fMit[5][0][3] = c4 * this->_long_56 + q1;
+ fMit[5][1][3] = s4 * this->_long_56 + q2;
fMit[5][2][3] = q3;
-
+
fMit[6][0][0] = fMit[5][0][0];
fMit[6][1][0] = fMit[5][1][0];
fMit[6][2][0] = fMit[5][2][0];
@@ -759,97 +742,100 @@ vpSimulatorAfma6::compute_fMi()
fMit[6][0][3] = fMit[5][0][3];
fMit[6][1][3] = fMit[5][1][3];
fMit[6][2][3] = fMit[5][2][3];
-
-// vpHomogeneousMatrix cMe;
-// get_cMe(cMe);
-// cMe = cMe.inverse();
-// fMit[7] = fMit[6] * cMe;
- vpAfma6::get_fMc(q,fMit[7]);
-
- #if defined(_WIN32)
-# if defined(WINRT_8_1)
+
+ // vpHomogeneousMatrix cMe;
+ // get_cMe(cMe);
+ // cMe = cMe.inverse();
+ // fMit[7] = fMit[6] * cMe;
+ vpAfma6::get_fMc(q, fMit[7]);
+
+#if defined(_WIN32)
+#if defined(WINRT_8_1)
WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
-# else // pure win32
+#else // pure win32
WaitForSingleObject(mutex_fMi, INFINITE);
-# endif
+#endif
for (int i = 0; i < 8; i++)
fMi[i] = fMit[i];
ReleaseMutex(mutex_fMi);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock(&mutex_fMi);
for (int i = 0; i < 8; i++)
fMi[i] = fMit[i];
- pthread_mutex_unlock (&mutex_fMi);
- #endif
+ pthread_mutex_unlock(&mutex_fMi);
+#endif
}
-
/*!
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: {
// Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
stopMotion();
}
break;
}
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
std::cout << "Change the control mode from velocity to position control.\n";
stopMotion();
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
break;
}
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
std::cout << "Change the control mode from stop to velocity control.\n";
}
break;
}
case vpRobot::STATE_ACCELERATION_CONTROL:
default:
- break ;
+ break;
}
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
/*!
Apply a velocity to the robot.
\param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
+ could be expressed in articular, camera frame, reference frame or mixt
+frame.
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
+ \param vel : Velocity vector. The size of this vector
is always 6.
- In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities in rad/s.
- In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
+expressed in the camera frame, with translations velocities \f$ ^{c} v_x, ^{c}
+v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
+expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
+^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
+translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
+reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
+\omega_z \f$ in the camera frame in rad/s.
+
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
velocity control mode is set. For that, call setRobotState(
@@ -861,10 +847,10 @@ vpSimulatorAfma6::setRobotState(vpRobot::vpRobotStateType newState)
setMaxTranslationVelocity() and setMaxRotationVelocity().
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
int main()
{
@@ -878,7 +864,7 @@ int main()
qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
qvel[4] = 0; // Joint 5 velocity in rad/s
qvel[5] = 0; // Joint 6 velocity in rad/s
-
+
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
@@ -893,196 +879,175 @@ 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) ");
- }
-
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
vpColVector vel_sat(6);
- double scale_sat = 1;
+ double scale_sat = 1;
double vel_trans_max = getMaxTranslationVelocity();
- double vel_rot_max = getMaxRotationVelocity();
-
+ double vel_rot_max = getMaxRotationVelocity();
+
double vel_abs; // Absolute value
-
+
// Velocity saturation
- switch(frame)
- {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
+ switch (frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
- for (unsigned int i = 0 ; i < 3; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_trans_max && !jointLimit)
- {
- vel_trans_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
- "(axis nr. %d).", vel[i], i+1);
- }
+ for (unsigned int i = 0; i < 3; ++i) {
+ vel_abs = fabs(vel[i]);
+ if (vel_abs > vel_trans_max && !jointLimit) {
+ vel_trans_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
+ "(axis nr. %d).",
+ vel[i], i + 1);
+ }
- vel_abs = fabs (vel[i+3]);
- if (vel_abs > vel_rot_max && !jointLimit) {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i+3], i+4);
- }
+ vel_abs = fabs(vel[i + 3]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ vel[i + 3], i + 4);
}
+ }
- double scale_trans_sat = 1;
- double scale_rot_sat = 1;
- if (vel_trans_max > getMaxTranslationVelocity())
- scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
+ double scale_trans_sat = 1;
+ double scale_rot_sat = 1;
+ if (vel_trans_max > getMaxTranslationVelocity())
+ scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
- {
- if (scale_trans_sat < scale_rot_sat)
- scale_sat = scale_trans_sat;
- else
- scale_sat = scale_rot_sat;
- }
- break;
+ if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
+ if (scale_trans_sat < scale_rot_sat)
+ scale_sat = scale_trans_sat;
+ else
+ scale_sat = scale_rot_sat;
}
+ break;
+ }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i], i+1);
- }
- }
- double scale_rot_sat = 1;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
- break;
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME: {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
+ throw;
}
- case vpRobot::MIXT_FRAME :
- {
- break;
+ for (unsigned int i = 0; i < 6; ++i) {
+ vel_abs = fabs(vel[i]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ vel[i], i + 1);
+ }
}
+ double scale_rot_sat = 1;
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (scale_rot_sat < 1)
+ scale_sat = scale_rot_sat;
+ break;
}
-
- set_velocity (vel * scale_sat);
- setRobotFrame (frame);
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
+ }
+
+ set_velocity(vel * scale_sat);
+ setRobotFrame(frame);
setVelocityCalled = true;
}
-
/*!
Compute the articular velocity relative to the velocity in another frame.
*/
-void
-vpSimulatorAfma6::computeArticularVelocity()
+void vpSimulatorAfma6::computeArticularVelocity()
{
vpRobot::vpControlFrameType frame = getRobotFrame();
-
- double vel_rot_max = getMaxRotationVelocity();
-
+
+ double vel_rot_max = getMaxRotationVelocity();
+
vpColVector articularCoordinates = get_artCoord();
vpColVector velocityframe = get_velocity();
vpColVector articularVelocity;
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix eVc(_eMc);
- vpAfma6::get_eJe(articularCoordinates,eJe_);
- eJe_ = eJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,eJe_);
- articularVelocity = eJe_*eVc*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpAfma6::get_fJe(articularCoordinates,fJe_);
- fJe_ = fJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,fJe_);
- articularVelocity = fJe_*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- articularVelocity = velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix eVc(_eMc);
+ vpAfma6::get_eJe(articularCoordinates, eJe_);
+ eJe_ = eJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates, eJe_);
+ articularVelocity = eJe_ * eVc * velocityframe;
+ set_artVel(articularVelocity);
+ break;
}
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- double vel_abs = fabs (articularVelocity[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", articularVelocity[i], i+1);
- }
+ case vpRobot::REFERENCE_FRAME: {
+ vpMatrix fJe_;
+ vpAfma6::get_fJe(articularCoordinates, fJe_);
+ fJe_ = fJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates, fJe_);
+ articularVelocity = fJe_ * velocityframe;
+ set_artVel(articularVelocity);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ articularVelocity = velocityframe;
+ set_artVel(articularVelocity);
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
+ }
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
+ for (unsigned int i = 0; i < 6; ++i) {
+ double vel_abs = fabs(articularVelocity[i]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ articularVelocity[i], i + 1);
}
- double scale_rot_sat = 1;
- double scale_sat = 1;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
-
- set_artVel(articularVelocity * scale_sat);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::MIXT_FRAME :
- {
- break;
}
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (scale_rot_sat < 1)
+ scale_sat = scale_rot_sat;
+
+ set_artVel(articularVelocity * scale_sat);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
}
}
-
/*!
Get the robot velocities.
@@ -1092,13 +1057,13 @@ vpSimulatorAfma6::computeArticularVelocity()
and rotations in rad/s.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+\dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1112,7 +1077,7 @@ int main()
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
vpSimulatorAfma6 robot;
-
+
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
@@ -1129,46 +1094,39 @@ int main()
}
\endcode
*/
-void
-vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
+void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel)
{
vel.resize(6);
-
+
vpColVector articularCoordinates = get_artCoord();
vpColVector articularVelocity = get_artVel();
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix cVe(_eMc);
- vpAfma6::get_eJe(articularCoordinates,eJe_);
- vel = cVe*eJe_*articularVelocity;
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- vel = articularVelocity;
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpAfma6::get_fJe(articularCoordinates,fJe_);
- vel = fJe_*articularVelocity;
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix cVe(_eMc);
+ vpAfma6::get_eJe(articularCoordinates, eJe_);
+ vel = cVe * eJe_ * articularVelocity;
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ vel = articularVelocity;
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpMatrix fJe_;
+ vpAfma6::get_fJe(articularCoordinates, fJe_);
+ vel = fJe_ * articularVelocity;
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
}
}
@@ -1183,13 +1141,12 @@ vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVec
\param timestamp : Unix time in second since January 1st 1970.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+ \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
*/
-void
-vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
+void vpSimulatorAfma6::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double ×tamp)
{
timestamp = vpTime::measureTimeSecond();
getVelocity(frame, vel);
@@ -1204,9 +1161,9 @@ vpSimulatorAfma6::getVelocity (const vpRobot::vpControlFrameType frame, vpColVec
and rotations in rad/s.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1220,7 +1177,7 @@ int main()
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
vpSimulatorAfma6 robot;
-
+
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
@@ -1237,11 +1194,10 @@ int main()
}
\endcode
*/
-vpColVector
-vpSimulatorAfma6::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpSimulatorAfma6::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector vel(6);
- getVelocity (frame, vel);
+ getVelocity(frame, vel);
return vel;
}
@@ -1258,28 +1214,25 @@ 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);
- getVelocity (frame, vel);
+ getVelocity(frame, vel);
return vel;
}
-void
-vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
+void vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
{
- double vel_rot_max = getMaxRotationVelocity();
- double velmax = fabs(q[0]);
- for (unsigned int i = 1; i < 6; i++)
- {
+ double vel_rot_max = getMaxRotationVelocity();
+ double velmax = fabs(q[0]);
+ for (unsigned int i = 1; i < 6; i++) {
if (velmax < fabs(q[i]))
- velmax = fabs(q[i]);
+ velmax = fabs(q[i]);
}
-
- double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
+
+ double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
q = q * alpha;
}
@@ -1317,9 +1270,9 @@ vpSimulatorAfma6::findHighestPositioningSpeed(vpColVector &q)
position is out of range.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1333,7 +1286,7 @@ int main()
position[5] = M_PI; // rotation around z axis, in rad
vpSimulatorAfma6 robot;
-
+
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Set the max velocity to 20%
@@ -1344,7 +1297,8 @@ int main()
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1357,146 +1311,124 @@ 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"
- "Modification of the robot state");
- // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
}
-
+
vpColVector articularCoordinates = get_artCoord();
-
+
vpColVector error(6);
double errsqr = 0;
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = q[i];
+ rxyz[i] = q[i + 3];
+ }
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- vpHomogeneousMatrix fMc_;
- vpAfma6::get_fMc(articularCoordinates, fMc_);
-
- vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
- }while (errsqr > 1e-8 && nbSol > 0);
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- do
- {
- articularCoordinates = get_artCoord();
- error = q - articularCoordinates;
+ vpHomogeneousMatrix fMc_;
+ vpAfma6::get_fMc(articularCoordinates, fMc_);
+
+ vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
+
+ do {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc2, qdes, true, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0) {
+ error = qdes - articularCoordinates;
errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
+ // findHighestPositioningSpeed(error);
set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (q);
+ if (errsqr < 1e-4) {
+ set_artCoord(qdes);
error = 0;
set_artVel(error);
set_velocity(error);
break;
}
- }while (errsqr > 1e-8);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
+ } else {
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
+ }
+ } while (errsqr > 1e-8 && nbSol > 0);
+
+ break;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ do {
+ articularCoordinates = get_artCoord();
+ error = q - articularCoordinates;
+ errsqr = error.sumSquare();
+ // findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4) {
+ set_artCoord(q);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
}
+ } while (errsqr > 1e-8);
+ break;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = q[i];
+ rxyz[i] = q[i + 3];
+ }
- vpRotationMatrix fRc(rxyz);
- vpHomogeneousMatrix fMc_(txyz, fRc);
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
+ vpRotationMatrix fRc(rxyz);
+ vpHomogeneousMatrix fMc_(txyz, fRc);
+
+ do {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc_, qdes, true, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0) {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ // findHighestPositioningSpeed(error);
+ set_artVel(error);
+ if (errsqr < 1e-4) {
+ set_artCoord(qdes);
+ error = 0;
set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
+ set_velocity(error);
+ break;
}
- else
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }while (errsqr > 1e-8 && nbSol > 0);
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- }
+ } else
+ vpERROR_TRACE("Positionning error. Position unreachable");
+ } while (errsqr > 1e-8 && nbSol > 0);
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
}
@@ -1563,30 +1495,23 @@ int main()
\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) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
+ try {
+ vpColVector position(6);
+ position[0] = pos1;
+ position[1] = pos2;
+ position[2] = pos3;
+ position[3] = pos4;
+ position[4] = pos5;
+ position[5] = pos6;
+
+ setPosition(frame, position);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
+ }
}
/*!
@@ -1601,9 +1526,9 @@ void vpSimulatorAfma6::setPosition (const vpRobot::vpControlFrameType frame,
This method has the same behavior than the sample code given below;
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorAfma6.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1632,9 +1557,8 @@ void vpSimulatorAfma6::setPosition(const char *filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename);
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -1662,13 +1586,13 @@ void vpSimulatorAfma6::setPosition(const char *filename)
below show how to convert this position into a vpHomogeneousMatrix:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
int main()
{
@@ -1695,56 +1619,49 @@ int main()
}
\endcode
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
- \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q,
+double ×tamp) \sa setPosition(const vpRobot::vpControlFrameType frame,
+const vpColVector & r)
*/
-void
-vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
q.resize(6);
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- q = 0;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- q = get_artCoord();
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- vpHomogeneousMatrix fMc_;
- vpAfma6::get_fMc (get_artCoord(), fMc_);
-
- vpRotationMatrix fRc;
- fMc_.extract(fRc);
- vpRxyzVector rxyz(fRc);
-
- vpTranslationVector txyz;
- fMc_.extract(txyz);
-
- for (unsigned int i=0; i <3; i++)
- {
- q[i] = txyz[i];
- q[i+3] = rxyz[i];
- }
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ q = 0;
+ break;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ q = get_artCoord();
+ break;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ vpHomogeneousMatrix fMc_;
+ vpAfma6::get_fMc(get_artCoord(), fMc_);
+
+ vpRotationMatrix fRc;
+ fMc_.extract(fRc);
+ vpRxyzVector rxyz(fRc);
+
+ vpTranslationVector txyz;
+ fMc_.extract(txyz);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ q[i] = txyz[i];
+ q[i + 3] = rxyz[i];
}
+ break;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
}
@@ -1774,39 +1691,36 @@ vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpColVect
\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);
}
-
/*!
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &)
The difference is here that the position is returned using a ThetaU
representation.
\sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
*/
-void
-vpSimulatorAfma6::getPosition (const vpRobot::vpControlFrameType frame, vpPoseVector &position)
+void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame,posRxyz);
-
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // recupere position en Rxyz
+ this->getPosition(frame, posRxyz);
+
+ // recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
+
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1814,35 +1728,36 @@ vpSimulatorAfma6::getPosition (const vpRobot::vpControlFrameType frame, vpPoseVe
Get the current time stamped position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &)
The difference is here that the position is returned using a ThetaU
representation.
*/
-void
-vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
+void vpSimulatorAfma6::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position, double ×tamp)
{
timestamp = vpTime::measureTimeSecond();
getPosition(frame, position);
}
/*!
- This method enables to set the minimum and maximum joint limits for all the six axis of the robot. The three first values have to be given in meter and the others in radian.
-
- \param limitMin : The minimum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
- \param limitMax : The maximum joint limits are given in a vector of size 6. The three first values have to be given in meter and the others in radian.
+ This method enables to set the minimum and maximum joint limits for all the
+ six axis of the robot. The three first values have to be given in meter and
+ the others in radian.
+
+ \param limitMin : The minimum joint limits are given in a vector of size 6.
+ The three first values have to be given in meter and the others in radian.
+ \param limitMax : The maximum joint limits are given in a vector of size 6.
+ The three first values have to be given in meter and the others in radian.
*/
-void
-vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
+void vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
{
- if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
- {
+ if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
vpTRACE("Joint limit vector has not a size of 6 !");
return;
}
-
+
_joint_min[0] = limitMin[0];
_joint_min[1] = limitMin[1];
_joint_min[2] = limitMin[2];
@@ -1856,23 +1771,20 @@ vpSimulatorAfma6::setJointLimit(const vpColVector &limitMin, const vpColVector &
_joint_max[3] = limitMax[3];
_joint_max[4] = limitMax[4];
_joint_max[5] = limitMax[5];
-
}
-/*!
+/*!
Test to detect if the robot is near one of its singularities.
-
+
The goal is to avoid the problems du to such configurations.
*/
-bool
-vpSimulatorAfma6::singularityTest(const vpColVector q, vpMatrix &J)
+bool vpSimulatorAfma6::singularityTest(const vpColVector &q, vpMatrix &J)
{
double q5 = q[4];
-
- bool cond = fabs(q5-M_PI/2) < 1e-1;
-
- if(cond)
- {
+
+ bool cond = fabs(q5 - M_PI / 2) < 1e-1;
+
+ if (cond) {
J[0][3] = 0;
J[0][4] = 0;
J[1][3] = 0;
@@ -1883,84 +1795,46 @@ vpSimulatorAfma6::singularityTest(const vpColVector q, vpMatrix &J)
J[5][4] = 0;
return true;
}
-
+
return false;
}
-/*!
+/*!
Method used to check if the robot reached a joint limit.
*/
-int
-vpSimulatorAfma6::isInJointLimit ()
+int vpSimulatorAfma6::isInJointLimit()
{
int artNumb = 0;
double diff = 0;
double difft = 0;
-
+
vpColVector articularCoordinates = get_artCoord();
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] <= _joint_min[i])
- {
+
+ for (unsigned int i = 0; i < 6; i++) {
+ if (articularCoordinates[i] <= _joint_min[i]) {
difft = _joint_min[i] - articularCoordinates[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = -(int)i-1;
+ if (difft > diff) {
+ diff = difft;
+ artNumb = -(int)i - 1;
}
}
}
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] >= _joint_max[i])
- {
+
+ for (unsigned int i = 0; i < 6; i++) {
+ if (articularCoordinates[i] >= _joint_max[i]) {
difft = articularCoordinates[i] - _joint_max[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = (int)(i+1);
+ if (difft > diff) {
+ diff = difft;
+ artNumb = (int)(i + 1);
}
}
}
-
- if (artNumb != 0)
- std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
-
- return artNumb;
-}
-
-/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
- \sa getDisplacement(), getArticularDisplacement()
-*/
-void
-vpSimulatorAfma6::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-
-/*!
- Get the robot joint displacement since the last call of this method.
-
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
+ if (artNumb != 0)
+ std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
+ << std::endl;
- \sa getDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+ return artNumb;
}
/*!
@@ -1979,49 +1853,38 @@ vpSimulatorAfma6::getArticularDisplacement(vpColVector &displacement)
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
*/
-void
-vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpSimulatorAfma6::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
vpColVector q_cur(6);
q_cur = get_artCoord();
- if ( ! first_time_getdis )
- {
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- }
+ if (!first_time_getdis) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ }
- case vpRobot::ARTICULAR_FRAME:
- {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break;
+ }
- case vpRobot::REFERENCE_FRAME:
- {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- }
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
}
- }
- else
- {
+
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
+ }
+ }
+ } else {
first_time_getdis = false;
}
@@ -2062,7 +1925,8 @@ expressed in meters, while joint rotations A,B,C in radians.
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+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
@@ -2075,12 +1939,11 @@ 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);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -2092,29 +1955,29 @@ vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Afma6 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -2158,11 +2021,10 @@ vpSimulatorAfma6::readPosFile(const std::string &filename, vpColVector &q)
\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") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -2176,15 +2038,10 @@ vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
#\n\n");
// Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- q[0],
- q[1],
- q[2],
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
+ 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);
}
@@ -2195,19 +2052,15 @@ vpSimulatorAfma6::savePosFile(const std::string &filename, const vpColVector &q)
\sa readPosFile
*/
-void
-vpSimulatorAfma6::move(const char *filename)
+void vpSimulatorAfma6::move(const char *filename)
{
vpColVector q;
- try
- {
+ try {
this->readPosFile(filename, q);
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
- }
- catch(...)
- {
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+ } catch (...) {
throw;
}
}
@@ -2221,11 +2074,7 @@ 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
@@ -2234,13 +2083,12 @@ vpSimulatorAfma6::get_cMe(vpHomogeneousMatrix &cMe)
\param cVe : Twist transformation.
*/
-void
-vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
+void vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
{
- vpHomogeneousMatrix cMe ;
- vpAfma6::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpAfma6::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -2252,17 +2100,13 @@ vpSimulatorAfma6::get_cVe(vpVelocityTwistMatrix &cVe)
\param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
end-effector frame.
*/
-void
-vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
+void vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
{
- try
- {
- vpAfma6::get_eJe(get_artCoord(), eJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ try {
+ vpAfma6::get_eJe(get_artCoord(), eJe_);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
@@ -2276,39 +2120,32 @@ vpSimulatorAfma6::get_eJe(vpMatrix &eJe_)
\param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
reference frame.
*/
-void
-vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
+void vpSimulatorAfma6::get_fJe(vpMatrix &fJe_)
{
- try
- {
- vpColVector articularCoordinates = get_artCoord();
- vpAfma6::get_fJe(articularCoordinates, fJe_) ;
- }
- catch(...)
- {
+ try {
+ vpColVector articularCoordinates = get_artCoord();
+ vpAfma6::get_fJe(articularCoordinates, fJe_);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
/*!
Stop the robot.
*/
-void
-vpSimulatorAfma6::stopMotion()
+void vpSimulatorAfma6::stopMotion()
{
if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
return;
-
+
vpColVector stop(6);
stop = 0;
set_artVel(stop);
set_velocity(stop);
- vpRobot::setRobotState (vpRobot::STATE_STOP);
+ vpRobot::setRobotState(vpRobot::STATE_STOP);
}
-
-
/**********************************************************************************/
/**********************************************************************************/
/**********************************************************************************/
@@ -2322,209 +2159,199 @@ vpSimulatorAfma6::stopMotion()
not valid, the path is set from the VISP_SCENES_DIR environment
variable that the user has to set.
*/
-void
-vpSimulatorAfma6::initArms()
+void vpSimulatorAfma6::initArms()
{
// set scene_dir from #define VISP_SCENE_DIR if it exists
// VISP_SCENES_DIR may contain multiple locations separated by ";"
std::string scene_dir_;
std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
bool sceneDirExists = false;
- for(size_t i=0; i < scene_dirs.size(); i++)
- if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
- scene_dir_ = scene_dirs[i];
- sceneDirExists = true;
- break;
- }
- if (! sceneDirExists) {
+ for (size_t i = 0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir_ = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (!sceneDirExists) {
try {
scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
}
}
-
unsigned int name_length = 30; // the size of this kind of string "/afma6_arm2.bnd"
if (scene_dir_.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+ 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];
+ char *name_cam = new char[full_length];
strcpy(name_cam, scene_dir_.c_str());
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
+
if (arm_dir.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Afma6 simulator");
+ 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];
+ char *name_arm = new char[full_length];
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_gate.bnd");
- std::cout <<"name arm: " << name_arm << std::endl;
+ strcat(name_arm, "/afma6_gate.bnd");
+ std::cout << "name arm: " << name_arm << std::endl;
set_scene(name_arm, robotArms, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm1.bnd");
- set_scene(name_arm, robotArms+1, 1.0);
+ strcat(name_arm, "/afma6_arm1.bnd");
+ set_scene(name_arm, robotArms + 1, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm2.bnd");
- set_scene(name_arm, robotArms+2, 1.0);
+ strcat(name_arm, "/afma6_arm2.bnd");
+ set_scene(name_arm, robotArms + 2, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm3.bnd");
- set_scene(name_arm, robotArms+3, 1.0);
+ strcat(name_arm, "/afma6_arm3.bnd");
+ set_scene(name_arm, robotArms + 3, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/afma6_arm4.bnd");
- set_scene(name_arm, robotArms+4, 1.0);
-
+ strcat(name_arm, "/afma6_arm4.bnd");
+ set_scene(name_arm, robotArms + 4, 1.0);
+
vpAfma6::vpAfma6ToolType tool;
tool = getToolType();
strcpy(name_arm, arm_dir.c_str());
switch (tool) {
case vpAfma6::TOOL_CCMOP: {
- strcat(name_arm,"/afma6_tool_ccmop.bnd");
- break;
- }
+ strcat(name_arm, "/afma6_tool_ccmop.bnd");
+ break;
+ }
case vpAfma6::TOOL_GRIPPER: {
- strcat(name_arm,"/afma6_tool_gripper.bnd");
- break;
- }
+ strcat(name_arm, "/afma6_tool_gripper.bnd");
+ break;
+ }
case vpAfma6::TOOL_VACUUM: {
- strcat(name_arm,"/afma6_tool_vacuum.bnd");
- break;
- }
+ strcat(name_arm, "/afma6_tool_vacuum.bnd");
+ break;
+ }
case vpAfma6::TOOL_CUSTOM: {
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;
- break;
- }
+ std::cout << "The generic camera is not handled in vpSimulatorAfma6.cpp" << std::endl;
+ break;
+ }
}
- set_scene(name_arm, robotArms+5, 1.0);
-
+ set_scene(name_arm, robotArms + 5, 1.0);
+
add_rfstack(IS_BACK);
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-//
-// sceneInitialized = true;
-// displayObject = true;
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
+ //
+ // sceneInitialized = true;
+ // displayObject = true;
displayCamera = true;
- delete [] name_cam;
- delete [] name_arm;
+ delete[] name_cam;
+ delete[] name_arm;
}
-
-void
-vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
+void vpSimulatorAfma6::getExternalImage(vpImage<vpRGBa> &I_)
{
bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I_,changed);
+ vpHomogeneousMatrix displacement = navigation(I_, changed);
- //if (displacement[2][3] != 0)
+ // if (displacement[2][3] != 0)
if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- camMf2 = camMf2*displacement;
+ camMf2 = camMf2 * displacement;
- f2Mf = camMf2.inverse()*camMf;
+ f2Mf = camMf2.inverse() * camMf;
- camMf = camMf2* displacement * f2Mf;
+ camMf = camMf2 * displacement * f2Mf;
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_ext);
- v = (double)I_.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I_.getWidth() / (2 * px_ext);
+ v = (double)I_.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
+ v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
}
- float w44o[4][4],w44cext[4][4],x,y,z;
+ float w44o[4][4], w44cext[4][4], x, y, z;
- vp2jlc_matrix(camMf.inverse(),w44cext);
+ vp2jlc_matrix(camMf.inverse(), w44cext);
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+
vpHomogeneousMatrix fMit[8];
get_fMi(fMit);
-
- vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
- display_scene(w44o,robotArms[0],I_, curColor);
-
- vp2jlc_matrix(fMit[0],w44o);
- display_scene(w44o,robotArms[1],I_, curColor);
-
- vp2jlc_matrix(fMit[2],w44o);
- display_scene(w44o,robotArms[2],I_, curColor);
-
- vp2jlc_matrix(fMit[3],w44o);
- display_scene(w44o,robotArms[3],I_, curColor);
-
- vp2jlc_matrix(fMit[4],w44o);
- display_scene(w44o,robotArms[4],I_, curColor);
-
- vp2jlc_matrix(fMit[5],w44o);
- display_scene(w44o,robotArms[5],I_, curColor);
-
- if (displayCamera)
- {
+
+ vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
+ display_scene(w44o, robotArms[0], I_, curColor);
+
+ vp2jlc_matrix(fMit[0], w44o);
+ display_scene(w44o, robotArms[1], I_, curColor);
+
+ vp2jlc_matrix(fMit[2], w44o);
+ display_scene(w44o, robotArms[2], I_, curColor);
+
+ vp2jlc_matrix(fMit[3], w44o);
+ display_scene(w44o, robotArms[3], I_, curColor);
+
+ vp2jlc_matrix(fMit[4], w44o);
+ display_scene(w44o, robotArms[4], I_, curColor);
+
+ vp2jlc_matrix(fMit[5], w44o);
+ display_scene(w44o, robotArms[5], I_, curColor);
+
+ if (displayCamera) {
vpHomogeneousMatrix cMe;
get_cMe(cMe);
cMe = cMe.inverse();
cMe = fMit[6] * cMe;
- vp2jlc_matrix(cMe,w44o);
- display_scene(w44o,camera, I_, camColor);
+ vp2jlc_matrix(cMe, w44o);
+ display_scene(w44o, camera, I_, camColor);
}
-
- if (displayObject)
- {
- vp2jlc_matrix(fMo,w44o);
- display_scene(w44o,scene,I_, curColor);
+
+ if (displayObject) {
+ vp2jlc_matrix(fMo, w44o);
+ display_scene(w44o, scene, I_, curColor);
}
}
/*!
- This method enables to initialise the joint coordinates of the robot in order
- to position the camera relative to the object.
-
- Before using this method it is advised to set the position of the object thanks
- to the set_fMo() method.
-
+ This method enables to initialise the joint coordinates of the robot in
+ order to position the camera relative to the object.
+
+ Before using this method it is advised to set the position of the object
+ thanks to the set_fMo() method.
+
In other terms, set the world to camera transformation
- \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}\f$, and from the inverse kinematics
- set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
+ \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}_o)^{-1}\f$, and from the
+ inverse kinematics set the joint positions \f${\bf q}\f$ that corresponds to
+ the \f${^f}{\bf M}_c\f$ transformation.
\param cMo_ : the desired pose of the camera.
\return false if the robot kinematics is not able to reach the cMo position.
*/
-bool
-vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
+bool vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
{
vpColVector stop(6);
bool status = true;
@@ -2533,39 +2360,39 @@ vpSimulatorAfma6::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cM
set_velocity(stop);
vpHomogeneousMatrix fMc_;
fMc_ = fMo * cMo_.inverse();
-
+
vpColVector articularCoordinates = get_artCoord();
int nbSol = getInverseKinematics(fMc_, articularCoordinates, true, verbose_);
-
+
if (nbSol == 0) {
status = false;
- vpERROR_TRACE ("Positionning error. Position unreachable");
+ vpERROR_TRACE("Positionning error. Position unreachable");
}
-
+
if (verbose_)
std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
set_artCoord(articularCoordinates);
-
+
compute_fMi();
return status;
}
/*!
- This method enables to initialise the pose between the object and the reference frame,
- in order to position the object relative to the camera.
-
- Before using this method it is advised to set the articular coordinates of the robot.
+ This method enables to initialise the pose between the object and the
+ reference frame, in order to position the object relative to the camera.
+
+ Before using this method it is advised to set the articular coordinates of
+ the robot.
In other terms, set the world to object transformation
- \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
- with \f${\bf q}\f$ the robot joint position
+ \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c
+ = f({\bf q})\f$ with \f${\bf q}\f$ the robot joint position
\param cMo_ : the desired pose of the camera.
*/
-void
-vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
+void vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
{
vpColVector stop(6);
stop = 0;
@@ -2578,7 +2405,8 @@ vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cM
/*!
This method enable to move the robot with respect to the initialized object.
- The robot trajectory is a straight line from the current position to the one corresponding to the desired pose (3D visual servoing).
+ The robot trajectory is a straight line from the current position to the one
+ corresponding to the desired pose (3D visual servoing).
\param cdMo_ : the desired pose of the camera wrt. the object
\param Iint : pointer to the image where the internal view is displayed
@@ -2586,70 +2414,70 @@ vpSimulatorAfma6::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cM
\return True is the pose is reached, False else
*/
-bool
-vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
+bool vpSimulatorAfma6::setPosition(const vpHomogeneousMatrix &cdMo_, vpImage<unsigned char> *Iint, const double &errMax)
{
- // get rid of max velocity
- double vMax = getMaxTranslationVelocity();
- double wMax = getMaxRotationVelocity();
- setMaxTranslationVelocity(10.*vMax);
- setMaxRotationVelocity(10.*wMax);
-
- vpColVector v(3),w(3),vel(6);
- vpHomogeneousMatrix cdMc;
- vpTranslationVector cdTc;vpRotationMatrix cdRc;vpThetaUVector cdTUc;
- vpColVector err(6);err=1.;
- const double lambda = 5.;
-
- vpVelocityTwistMatrix cVe;
-
- unsigned int i,iter=0;
- while((iter++<300) & (err.euclideanNorm()>errMax))
- {
+ // get rid of max velocity
+ double vMax = getMaxTranslationVelocity();
+ double wMax = getMaxRotationVelocity();
+ setMaxTranslationVelocity(10. * vMax);
+ setMaxRotationVelocity(10. * wMax);
+
+ vpColVector v(3), w(3), vel(6);
+ vpHomogeneousMatrix cdMc;
+ vpTranslationVector cdTc;
+ vpRotationMatrix cdRc;
+ vpThetaUVector cdTUc;
+ vpColVector err(6);
+ err = 1.;
+ const double lambda = 5.;
+
+ vpVelocityTwistMatrix cVe;
+
+ unsigned int i, iter = 0;
+ while ((iter++ < 300) & (err.euclideanNorm() > errMax)) {
double t = vpTime::measureTimeMs();
- // update image
- if(Iint != NULL)
- {
- vpDisplay::display(*Iint);
- getInternalView(*Iint);
- vpDisplay::flush(*Iint);
- }
-
- // update pose error
- cdMc = cdMo_*get_cMo().inverse();
- cdMc.extract(cdRc);
- cdMc.extract(cdTc);
- cdTUc.buildFrom(cdRc);
-
- // compute v,w and velocity
- v = -lambda*cdRc.t()*cdTc;
- w = -lambda*cdTUc;
- for(i=0;i<3;++i)
- {
- vel[i] = v[i];
- vel[i+3] = w[i];
- err[i] = cdTc[i];
- err[i+3] = cdTUc[i];
- }
-
- // update feat
- setVelocity(vpRobot::CAMERA_FRAME,vel);
-
- // wait for it
- vpTime::wait(t,10);
- }
- vel=0.;
- set_velocity(vel);
- set_artVel(vel);
- setMaxTranslationVelocity(vMax);
- setMaxRotationVelocity(wMax);
-
- //std::cout << "setPosition: final error " << err.t() << std::endl;
- return(err.euclideanNorm()<= errMax);
+ // update image
+ if (Iint != NULL) {
+ vpDisplay::display(*Iint);
+ getInternalView(*Iint);
+ vpDisplay::flush(*Iint);
+ }
+
+ // update pose error
+ cdMc = cdMo_ * get_cMo().inverse();
+ cdMc.extract(cdRc);
+ cdMc.extract(cdTc);
+ cdTUc.buildFrom(cdRc);
+
+ // compute v,w and velocity
+ v = -lambda * cdRc.t() * cdTc;
+ w = -lambda * cdTUc;
+ for (i = 0; i < 3; ++i) {
+ vel[i] = v[i];
+ vel[i + 3] = w[i];
+ err[i] = cdTc[i];
+ err[i + 3] = cdTUc[i];
+ }
+
+ // update feat
+ setVelocity(vpRobot::CAMERA_FRAME, vel);
+
+ // wait for it
+ vpTime::wait(t, 10);
+ }
+ vel = 0.;
+ set_velocity(vel);
+ set_artVel(vel);
+ setMaxTranslationVelocity(vMax);
+ setMaxRotationVelocity(wMax);
+
+ // std::cout << "setPosition: final error " << err.t() << std::endl;
+ return (err.euclideanNorm() <= errMax);
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has no symbols
-void dummy_vpSimulatorAfma6() {};
+// Work arround to avoid warning: libvisp_robot.a(vpSimulatorAfma6.cpp.o) has
+// no symbols
+void dummy_vpSimulatorAfma6(){};
#endif
diff --git a/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp
index f682003..04fa744 100644
--- a/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp
+++ b/modules/robot/src/robot-simulator/vpSimulatorCamera.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpSimulatorCamera.cpp
\brief class that defines the simplest robot : a free flying camera
@@ -47,16 +47,12 @@
#include <visp3/robot/vpRobotException.h>
#include <visp3/robot/vpSimulatorCamera.h>
-
/*!
Default constructor that sets the transformation between
world frame and camera frame to identity.
*/
-vpSimulatorCamera::vpSimulatorCamera() : wMc_()
-{
- init() ;
-}
+vpSimulatorCamera::vpSimulatorCamera() : wMc_() { init(); }
/*!
Robot initialisation.
@@ -68,25 +64,22 @@ vpSimulatorCamera::vpSimulatorCamera() : wMc_()
void vpSimulatorCamera::init()
{
nDof = 6;
- eJe.eye(6,6) ;
+ eJe.eye(6, 6);
eJeAvailable = true;
fJeAvailable = false;
areJointLimitsAvailable = false;
qmin = NULL;
qmax = NULL;
- setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
+ setMaxTranslationVelocity(1.); // vx, vy and vz max set to 1 m/s
setMaxRotationVelocity(vpMath::rad(90)); // wx, wy and wz max set to 90 deg/s
}
-
/*!
Destructor.
*/
-vpSimulatorCamera::~vpSimulatorCamera()
-{
-}
+vpSimulatorCamera::~vpSimulatorCamera() {}
/*!
@@ -94,12 +87,11 @@ vpSimulatorCamera::~vpSimulatorCamera()
frame. This transformation allows to compute a velocity expressed
in the end-effector frame into the camera frame.
- \param cVe : Twist transformation. Here this transformation is equal to identity
- since camera frame and end-effector frame are at the same location.
+ \param cVe : Twist transformation. Here this transformation is equal to
+ identity since camera frame and end-effector frame are at the same location.
*/
-void
-vpSimulatorCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
+void vpSimulatorCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
{
vpVelocityTwistMatrix cVe_;
cVe = cVe_;
@@ -110,32 +102,21 @@ vpSimulatorCamera::get_cVe(vpVelocityTwistMatrix &cVe) const
For that simple robot the Jacobian is the identity.
\param eJe_ : A 6 by 6 matrix representing the robot jacobian \f$ {^e}{\bf
- J}_e\f$ expressed in the end-effector frame. Yhis matrix is equal to identity.
+ J}_e\f$ expressed in the end-effector frame. Yhis matrix is equal to
+ identity.
*/
-void
-vpSimulatorCamera::get_eJe(vpMatrix &eJe_)
-{
- eJe_ = this->eJe ;
-}
+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.
@@ -144,7 +125,8 @@ vpSimulatorCamera::getPosition() const
- in the camera cartesien frame,
- joint (articular) coordinates of each axes
- in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+ - in a mixt cartesien frame (translation in reference frame, and rotation in
+ camera frame)
\param position : Measured position of the robot:
- in camera cartesien frame, a 6 dimension vector, set to 0.
@@ -159,15 +141,15 @@ vpSimulatorCamera::getPosition() const
*/
void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
- q.resize (6);
+ q.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME :
+ case vpRobot::CAMERA_FRAME:
q = 0;
break;
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
// Convert wMc_ to a position
// From fMc extract the pose
vpRotationMatrix wRc;
@@ -175,14 +157,14 @@ void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpC
vpRxyzVector rxyz;
rxyz.buildFrom(wRc);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ q[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
break;
- }
- case vpRobot::MIXT_FRAME :
+ }
+ case vpRobot::MIXT_FRAME:
std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
}
}
@@ -193,68 +175,57 @@ void vpSimulatorCamera::getPosition(const vpRobot::vpControlFrameType frame, vpC
\param frame : Control frame type. Only articular (vpRobot::ARTICULAR_FRAME)
and camera frame (vpRobot::CAMERA_FRAME) are implemented.
- \param v : Velocity to apply to the robot.
+ \param v : Velocity twist to apply to the robot.
- - In the camera frame, this velocity is represented by a vector of dimension 6
- \f$ {\bf v} = [{\bf t}, {\bf \theta u }]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ {\bf v} = [t_x, t_y, t_z, {\theta u}_x, {\theta u}_y,
- {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector).
+ - In the camera frame, this velocity is represented by a twist vector of
+ dimension 6 \f$ {\bf v} = [v_x v_y v_z w_x w_y w_z]^t \f$ where \f$ v_x,
+ v_y, v_z \f$ are the translation velocities in m/s and \f$ w_x, w_y, w_z
+ \f$ the rotation velocities in rad/s applied in the camera frame.
- - In articular, this velocity is represented by a 6 dimension vector \f$
- \dot{{\bf q}} = [{\bf t}, {\bf \theta u}]^t \f$ where \f$ \bf t \f$ is a
- translation vector and \f$ {\bf \theta u} \f$ is a rotation vector (see
- vpThetaUVector): \f$ \dot{{\bf q}} = [t_x, t_y, t_z, {\theta u}_x, {\theta
- u}_y, {\theta u}_z] \f$ (see vpTranslationVector and vpThetaUVector). The
- robot jacobian \f$ {^e}{\bf J}_e\f$ expressed in the end-effector frame is
- here set to identity.
+ - In articular, the behavior is the same as in camera frame.
- We use the exponential map (vpExponentialMap) to update the camera location.
- Sampling time can be set using setSamplingTime().
+ Internally, the exponential map (vpExponentialMap) is used to update the
+ camera position from its velocity after applying the velocity during a
+ sampling time. This sampling time can be set using setSamplingTime().
\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 ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
}
- switch (frame)
- {
+ switch (frame) {
case vpRobot::ARTICULAR_FRAME:
case vpRobot::CAMERA_FRAME: {
- vpColVector v_max(6);
+ vpColVector v_max(6);
- for (unsigned int i=0; i<3; i++)
- v_max[i] = getMaxTranslationVelocity();
- for (unsigned int i=3; i<6; i++)
- v_max[i] = getMaxRotationVelocity();
+ for (unsigned int i = 0; i < 3; i++)
+ v_max[i] = getMaxTranslationVelocity();
+ for (unsigned int i = 3; i < 6; i++)
+ v_max[i] = getMaxRotationVelocity();
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
- wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
- setRobotFrame(frame);
- break ;
- }
+ wMc_ = wMc_ * vpExponentialMap::direct(v_sat, delta_t_);
+ setRobotFrame(frame);
+ break;
+ }
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the reference frame:"
- "functionality not implemented");
- break ;
+ vpERROR_TRACE("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the reference frame:"
+ "functionality not implemented");
+ break;
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
+ vpERROR_TRACE("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
- break ;
+ break;
}
}
@@ -265,7 +236,7 @@ vpSimulatorCamera::setVelocity(const vpRobot::vpControlFrameType frame,
*/
void vpSimulatorCamera::setPosition(const vpHomogeneousMatrix &wMc)
{
- if (vpRobot::STATE_POSITION_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
setRobotState(vpRobot::STATE_POSITION_CONTROL);
}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp
index ada3e11..e0c0f78 100644
--- a/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp
+++ b/modules/robot/src/robot-simulator/vpSimulatorPioneer.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,29 +36,24 @@
*
*****************************************************************************/
-
/*!
\file vpSimulatorPioneer.cpp
- \brief class that defines the Pioneer mobile robot simulator equipped with a static camera.
+ \brief class that defines the Pioneer mobile robot simulator equipped with a
+ static camera.
*/
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/robot/vpSimulatorPioneer.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpExponentialMap.h>
-
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
/*!
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.
@@ -82,14 +78,11 @@ void vpSimulatorPioneer::init()
wMc_ = wMe_ * cMe_.inverse();
}
-
/*!
Destructor.
*/
-vpSimulatorPioneer::~vpSimulatorPioneer()
-{
-}
+vpSimulatorPioneer::~vpSimulatorPioneer() {}
/*!
Get the robot jacobian expressed in the end-effector frame.
@@ -98,19 +91,18 @@ 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.
- \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
+ \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is
+ implemented.
- \param v : Velocity vector \f$(v_x, w_z)\f$ to apply to the robot.
+ \param v : Velocity vector \f$(v_x, \omega_z)\f$ to apply to the robot,
+ where \f$v_x\f$ is the linear translational velocity in m/s and
+ \f$\omega_z\f$ is the rotational velocity in rad/s arround the vertical
+ axis.
Depending on the velocity specified as input, the robot position is updated
using the sampling time that can be modified using setSamplingTime().
@@ -118,65 +110,57 @@ 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)
- {
+ switch (frame) {
case vpRobot::ARTICULAR_FRAME: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
- setRobotFrame(frame);
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
+ setRobotFrame(frame);
- // v is a 2 dimension vector that contains v,w
- if (v.size() != 2) {
- vpERROR_TRACE ("Bad dimension of the control vector");
- throw vpRobotException (vpRobotException::dimensionError,
- "Bad dimension of the control vector");
- }
+ // v is a 2 dimension vector that contains v,w
+ if (v.size() != 2) {
+ vpERROR_TRACE("Bad dimension of the control vector");
+ throw vpRobotException(vpRobotException::dimensionError, "Bad dimension of the control vector");
+ }
- vpColVector v_max(2);
+ vpColVector v_max(2);
- v_max[0] = getMaxTranslationVelocity();
- v_max[1] = getMaxRotationVelocity();
+ v_max[0] = getMaxTranslationVelocity();
+ v_max[1] = getMaxRotationVelocity();
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
- xm_ += delta_t_ * v_sat[0] * cos(theta_);
- ym_ += delta_t_ * v_sat[0] * sin(theta_);
- theta_ += delta_t_ * v_sat[1];
+ xm_ += delta_t_ * v_sat[0] * cos(theta_);
+ ym_ += delta_t_ * v_sat[0] * sin(theta_);
+ theta_ += delta_t_ * v_sat[1];
- vpRotationMatrix wRe(0, 0, theta_);
- vpTranslationVector wte(xm_, ym_, 0);
- wMe_.buildFrom(wte, wRe);
- wMc_ = wMe_ * cMe_.inverse();
+ vpRotationMatrix wRe(0, 0, theta_);
+ vpTranslationVector wte(xm_, ym_, 0);
+ wMe_.buildFrom(wte, wRe);
+ wMc_ = wMe_ * cMe_.inverse();
- break ;
- }
- break ;
+ break;
+ } break;
case vpRobot::CAMERA_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the camera frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the camera frame:"
- "functionality not implemented");
- break ;
+ vpERROR_TRACE("Cannot set a velocity in the camera frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the camera frame:"
+ "functionality not implemented");
+ break;
case vpRobot::REFERENCE_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the reference frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the articular frame:"
- "functionality not implemented");
+ vpERROR_TRACE("Cannot set a velocity in the reference frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the articular frame:"
+ "functionality not implemented");
case vpRobot::MIXT_FRAME:
- vpERROR_TRACE ("Cannot set a velocity in the mixt frame: "
- "functionality not implemented");
- throw vpRobotException (vpRobotException::wrongStateError,
- "Cannot set a velocity in the mixt frame:"
- "functionality not implemented");
+ vpERROR_TRACE("Cannot set a velocity in the mixt frame: "
+ "functionality not implemented");
+ throw vpRobotException(vpRobotException::wrongStateError, "Cannot set a velocity in the mixt frame:"
+ "functionality not implemented");
- break ;
+ break;
}
}
@@ -184,11 +168,7 @@ 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.
@@ -197,7 +177,8 @@ vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const
- in the camera cartesien frame,
- joint (articular) coordinates of each axes (not implemented)
- in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+ - in a mixt cartesien frame (translation in reference frame, and rotation in
+ camera frame)
\param position : Measured position of the robot:
- in camera cartesien frame, a 6 dimension vector, set to 0.
@@ -210,17 +191,19 @@ vpSimulatorPioneer::getPosition(vpHomogeneousMatrix &wMc) const
*/
void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
- q.resize (6);
+ q.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME :
+ case vpRobot::CAMERA_FRAME:
q = 0;
break;
- case vpRobot::ARTICULAR_FRAME :
- std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
+ case vpRobot::ARTICULAR_FRAME:
+ std::cout << "ARTICULAR_FRAME is not implemented in "
+ "vpSimulatorPioneer::getPosition()"
+ << std::endl;
break;
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::REFERENCE_FRAME: {
// Convert wMc_ to a position
// From fMc extract the pose
vpRotationMatrix wRc;
@@ -228,14 +211,14 @@ void vpSimulatorPioneer::getPosition(const vpRobot::vpControlFrameType frame, vp
vpRxyzVector rxyz;
rxyz.buildFrom(wRc);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ q[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
break;
- }
- case vpRobot::MIXT_FRAME :
+ }
+ case vpRobot::MIXT_FRAME:
std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
}
}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp b/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp
index b9a2333..6e8b2a6 100644
--- a/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp
+++ b/modules/robot/src/robot-simulator/vpSimulatorPioneerPan.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,19 +36,17 @@
*
*****************************************************************************/
-
/*!
\file vpSimulatorPioneerPan.cpp
\brief class that defines the Pioneer mobile robot simulator equipped
with a camera able to move in pan.
*/
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/robot/vpSimulatorPioneerPan.h>
-#include <visp3/robot/vpRobotException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpExponentialMap.h>
-
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/robot/vpRobotException.h>
+#include <visp3/robot/vpSimulatorPioneerPan.h>
/*!
Constructor.
@@ -58,10 +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.
@@ -90,14 +86,11 @@ void vpSimulatorPioneerPan::init()
wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
}
-
/*!
Destructor.
*/
-vpSimulatorPioneerPan::~vpSimulatorPioneerPan()
-{
-}
+vpSimulatorPioneerPan::~vpSimulatorPioneerPan() {}
/*!
Get the robot jacobian expressed in the end-effector frame.
@@ -106,18 +99,19 @@ 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.
- \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is implemented.
+ \param frame : Control frame type. Only vpRobot::ARTICULAR_FRAME is
+ implemented.
- \param v : Velocity vector \f$(v_x, w_z, \dot q)\f$ to apply to the robot.
+ \param v : Velocity vector \f$(v_x, \omega_z, \dot q)\f$ to apply to the
+ robot, where \f$v_x\f$ is the linear translational velocity in m/s and
+ \f$\omega_z\f$ is the rotational velocity in rad/s arround the vertical axis
+ of the mobile base, and, \f$\dot q\f$ is the pan velocity in rad/s of the
+ camera.
Depending on the velocity specified as input, the robot position is updated
using the sampling time that can be modified using setSamplingTime().
@@ -125,66 +119,59 @@ 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:{
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
- setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- }
+ switch (frame) {
+ case vpRobot::ARTICULAR_FRAME: {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
+ }
- setRobotFrame(frame);
+ setRobotFrame(frame);
- // v is a 3 dimension vector that contains vx, wz, qpan
- if (v.size() != 3) {
- vpERROR_TRACE ("Bad dimension of the control vector");
- throw vpRobotException (vpRobotException::dimensionError,
- "Bad dimension of the control vector");
- }
+ // v is a 3 dimension vector that contains vx, wz, qpan
+ if (v.size() != 3) {
+ vpERROR_TRACE("Bad dimension of the control vector");
+ throw vpRobotException(vpRobotException::dimensionError, "Bad dimension of the control vector");
+ }
- vpColVector v_max(3);
+ vpColVector v_max(3);
- v_max[0] = getMaxTranslationVelocity();
- v_max[1] = getMaxRotationVelocity();
- v_max[2] = getMaxRotationVelocity();
+ v_max[0] = getMaxTranslationVelocity();
+ v_max[1] = getMaxRotationVelocity();
+ v_max[2] = getMaxRotationVelocity();
- vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
+ vpColVector v_sat = vpRobot::saturateVelocities(v, v_max, true);
- xm_ += delta_t_ * v_sat[0] * cos(theta_);
- ym_ += delta_t_ * v_sat[0] * sin(theta_);
- theta_ += delta_t_ * v_sat[1];
- q_pan_ += delta_t_ * v_sat[2];
+ xm_ += delta_t_ * v_sat[0] * cos(theta_);
+ ym_ += delta_t_ * v_sat[0] * sin(theta_);
+ theta_ += delta_t_ * v_sat[1];
+ q_pan_ += delta_t_ * v_sat[2];
- vpRotationMatrix wRm(0, 0, theta_);
- vpTranslationVector wtm(xm_, ym_, 0);
- wMm_.buildFrom(wtm, wRm);
+ vpRotationMatrix wRm(0, 0, theta_);
+ vpTranslationVector wtm(xm_, ym_, 0);
+ wMm_.buildFrom(wtm, wRm);
- // Update the end effector pose
- set_pMe(q_pan_);
+ // Update the end effector pose
+ set_pMe(q_pan_);
- // Update the camera pose
- wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
+ // Update the camera pose
+ wMc_ = wMm_ * mMp_ * pMe_ * cMe_.inverse();
- // Update the jacobian
- set_eJe(q_pan_);
+ // Update the jacobian
+ set_eJe(q_pan_);
- break ;
- }
+ 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");
}
}
@@ -192,11 +179,7 @@ vpSimulatorPioneerPan::setVelocity(const vpRobot::vpControlFrameType frame,
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.
@@ -205,7 +188,8 @@ vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const
- in the camera cartesien frame,
- joint (articular) coordinates of each axes (not implemented)
- in a reference or fixed cartesien frame attached to the robot base
- - in a mixt cartesien frame (translation in reference frame, and rotation in camera frame)
+ - in a mixt cartesien frame (translation in reference frame, and rotation in
+ camera frame)
\param position : Measured position of the robot:
- in camera cartesien frame, a 6 dimension vector, set to 0.
@@ -218,17 +202,19 @@ vpSimulatorPioneerPan::getPosition(vpHomogeneousMatrix &wMc) const
*/
void vpSimulatorPioneerPan::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
- q.resize (6);
+ q.resize(6);
switch (frame) {
- case vpRobot::CAMERA_FRAME :
+ case vpRobot::CAMERA_FRAME:
q = 0;
break;
- case vpRobot::ARTICULAR_FRAME :
- std::cout << "ARTICULAR_FRAME is not implemented in vpSimulatorPioneer::getPosition()" << std::endl;
+ case vpRobot::ARTICULAR_FRAME:
+ std::cout << "ARTICULAR_FRAME is not implemented in "
+ "vpSimulatorPioneer::getPosition()"
+ << std::endl;
break;
- case vpRobot::REFERENCE_FRAME : {
+ case vpRobot::REFERENCE_FRAME: {
// Convert wMc_ to a position
// From fMc extract the pose
vpRotationMatrix wRc;
@@ -236,14 +222,14 @@ void vpSimulatorPioneerPan::getPosition(const vpRobot::vpControlFrameType frame,
vpRxyzVector rxyz;
rxyz.buildFrom(wRc);
- for (unsigned int i=0; i < 3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
q[i] = this->wMc_[i][3]; // translation x,y,z
- q[i+3] = rxyz[i]; // Euler rotation x,y,z
+ q[i + 3] = rxyz[i]; // Euler rotation x,y,z
}
break;
- }
- case vpRobot::MIXT_FRAME :
+ }
+ case vpRobot::MIXT_FRAME:
std::cout << "MIXT_FRAME is not implemented in vpSimulatorCamera::getPosition()" << std::endl;
}
}
diff --git a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp
index ea208ba..1670db0 100644
--- a/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp
+++ b/modules/robot/src/robot-simulator/vpSimulatorViper850.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,25 +36,23 @@
*
*****************************************************************************/
-
-
#include <visp3/robot/vpSimulatorViper850.h>
#if defined(VISP_HAVE_MODULE_GUI) && ((defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_PTHREAD))
-#include <visp3/core/vpTime.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <string>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpIoTools.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <string>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpTime.h>
#include "../wireframe-simulator/vpBound.h"
-#include "../wireframe-simulator/vpVwstack.h"
#include "../wireframe-simulator/vpRfstack.h"
#include "../wireframe-simulator/vpScene.h"
+#include "../wireframe-simulator/vpVwstack.h"
const double vpSimulatorViper850::defaultPositioningVelocity = 25.0;
@@ -61,52 +60,49 @@ 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();
-
+
tcur = vpTime::measureTimeMs();
- #if defined(_WIN32)
-# ifdef WINRT_8_1
+#if defined(_WIN32)
+#ifdef WINRT_8_1
mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
-# else
+#else
mutex_fMi = CreateMutex(NULL, FALSE, NULL);
mutex_artVel = CreateMutex(NULL, FALSE, NULL);
mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
mutex_velocity = CreateMutex(NULL, FALSE, NULL);
mutex_display = CreateMutex(NULL, FALSE, NULL);
-# endif
-
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined (VISP_HAVE_PTHREAD)
+#endif
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+#elif defined(VISP_HAVE_PTHREAD)
pthread_mutex_init(&mutex_fMi, NULL);
pthread_mutex_init(&mutex_artVel, NULL);
pthread_mutex_init(&mutex_artCoord, NULL);
pthread_mutex_init(&mutex_velocity, NULL);
pthread_mutex_init(&mutex_display, NULL);
-
+
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
+
pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
+#endif
+
compute_fMi();
}
@@ -117,51 +113,49 @@ 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();
-
+
tcur = vpTime::measureTimeMs();
-
- #if defined(_WIN32)
-# ifdef WINRT_8_1
+
+#if defined(_WIN32)
+#ifdef WINRT_8_1
mutex_fMi = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artVel = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_artCoord = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_velocity = CreateMutexEx(NULL, NULL, 0, NULL);
mutex_display = CreateMutexEx(NULL, NULL, 0, NULL);
-# else
- mutex_fMi = CreateMutex(NULL,FALSE,NULL);
- mutex_artVel = CreateMutex(NULL,FALSE,NULL);
- mutex_artCoord = CreateMutex(NULL,FALSE,NULL);
- mutex_velocity = CreateMutex(NULL,FALSE,NULL);
- mutex_display = CreateMutex(NULL,FALSE,NULL);
-# endif
-
- DWORD dwThreadIdArray;
- hThread = CreateThread(
- NULL, // default security attributes
- 0, // use default stack size
- launcher, // thread function name
- this, // argument to thread function
- 0, // use default creation flags
- &dwThreadIdArray); // returns the thread identifier
- #elif defined(VISP_HAVE_PTHREAD)
+#else
+ mutex_fMi = CreateMutex(NULL, FALSE, NULL);
+ mutex_artVel = CreateMutex(NULL, FALSE, NULL);
+ mutex_artCoord = CreateMutex(NULL, FALSE, NULL);
+ mutex_velocity = CreateMutex(NULL, FALSE, NULL);
+ mutex_display = CreateMutex(NULL, FALSE, NULL);
+#endif
+
+ DWORD dwThreadIdArray;
+ hThread = CreateThread(NULL, // default security attributes
+ 0, // use default stack size
+ launcher, // thread function name
+ this, // argument to thread function
+ 0, // use default creation flags
+ &dwThreadIdArray); // returns the thread identifier
+#elif defined(VISP_HAVE_PTHREAD)
pthread_mutex_init(&mutex_fMi, NULL);
pthread_mutex_init(&mutex_artVel, NULL);
pthread_mutex_init(&mutex_artCoord, NULL);
pthread_mutex_init(&mutex_velocity, NULL);
pthread_mutex_init(&mutex_display, NULL);
-
+
pthread_attr_init(&attr);
pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_JOINABLE);
-
+
pthread_create(&thread, NULL, launcher, (void *)this);
- #endif
-
+#endif
+
compute_fMi();
}
@@ -171,20 +165,20 @@ vpSimulatorViper850::vpSimulatorViper850(bool do_display)
vpSimulatorViper850::~vpSimulatorViper850()
{
robotStop = true;
-
- #if defined(_WIN32)
-# if defined(WINRT_8_1)
+
+#if defined(_WIN32)
+#if defined(WINRT_8_1)
WaitForSingleObjectEx(hThread, INFINITE, FALSE);
-# else // pure win32
+#else // pure win32
WaitForSingleObject(hThread, INFINITE);
-# endif
+#endif
CloseHandle(hThread);
CloseHandle(mutex_fMi);
CloseHandle(mutex_artVel);
CloseHandle(mutex_artCoord);
CloseHandle(mutex_velocity);
CloseHandle(mutex_display);
- #elif defined(VISP_HAVE_PTHREAD)
+#elif defined(VISP_HAVE_PTHREAD)
pthread_attr_destroy(&attr);
pthread_join(thread, NULL);
pthread_mutex_destroy(&mutex_fMi);
@@ -192,13 +186,12 @@ vpSimulatorViper850::~vpSimulatorViper850()
pthread_mutex_destroy(&mutex_artCoord);
pthread_mutex_destroy(&mutex_velocity);
pthread_mutex_destroy(&mutex_display);
- #endif
-
- if (robotArms != NULL)
- {
- // free_Bound_scene (&(camera));
- for(int i = 0; i < 6; i++)
- free_Bound_scene (&(robotArms[i]));
+#endif
+
+ if (robotArms != NULL) {
+ // free_Bound_scene (&(camera));
+ for (int i = 0; i < 6; i++)
+ free_Bound_scene(&(robotArms[i]));
}
delete[] robotArms;
@@ -213,25 +206,23 @@ vpSimulatorViper850::~vpSimulatorViper850()
not valid, the path is set from the VISP_ROBOT_ARMS_DIR environment
variable that the user has to set.
*/
-void
-vpSimulatorViper850::init()
+void vpSimulatorViper850::init()
{
// set arm_dir from #define VISP_ROBOT_ARMS_DIR if it exists
// VISP_ROBOT_ARMS_DIR may contain multiple locations separated by ";"
std::vector<std::string> arm_dirs = vpIoTools::splitChain(std::string(VISP_ROBOT_ARMS_DIR), std::string(";"));
bool armDirExists = false;
- for(size_t i=0; i < arm_dirs.size(); i++)
- if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
- arm_dir = arm_dirs[i];
- armDirExists = true;
- break;
- }
- if (! armDirExists) {
+ for (size_t i = 0; i < arm_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(arm_dirs[i]) == true) { // directory exists
+ arm_dir = arm_dirs[i];
+ armDirExists = true;
+ break;
+ }
+ if (!armDirExists) {
try {
arm_dir = vpIoTools::getenv("VISP_ROBOT_ARMS_DIR");
std::cout << "The simulator uses data from VISP_ROBOT_ARMS_DIR=" << arm_dir << std::endl;
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot get VISP_ROBOT_ARMS_DIR environment variable" << std::endl;
}
}
@@ -246,32 +237,35 @@ vpSimulatorViper850::init()
zeroPos.resize(njoint);
zeroPos = 0;
- zeroPos[1] = -M_PI/2; zeroPos[2] = M_PI;
+ zeroPos[1] = -M_PI / 2;
+ zeroPos[2] = M_PI;
reposPos.resize(njoint);
reposPos = 0;
- reposPos[1] = -M_PI/2; reposPos[2] = M_PI; reposPos[4] = M_PI/2;
-
+ reposPos[1] = -M_PI / 2;
+ reposPos[2] = M_PI;
+ reposPos[4] = M_PI / 2;
+
artCoord = reposPos;
artVel = 0;
-
+
q_prev_getdis.resize(njoint);
q_prev_getdis = 0;
first_time_getdis = true;
-
- positioningVelocity = defaultPositioningVelocity ;
-
- setRobotFrame (vpRobot::ARTICULAR_FRAME);
+
+ positioningVelocity = defaultPositioningVelocity;
+
+ setRobotFrame(vpRobot::ARTICULAR_FRAME);
this->setRobotState(vpRobot::STATE_STOP);
-
+
// Software joint limits in radians
- //joint_min.resize(njoint);
+ // joint_min.resize(njoint);
joint_min[0] = vpMath::rad(-50);
joint_min[1] = vpMath::rad(-135);
joint_min[2] = vpMath::rad(-40);
joint_min[3] = vpMath::rad(-190);
joint_min[4] = vpMath::rad(-110);
joint_min[5] = vpMath::rad(-184);
- //joint_max.resize(njoint);
+ // joint_max.resize(njoint);
joint_max[0] = vpMath::rad(50);
joint_max[1] = vpMath::rad(-40);
joint_max[2] = vpMath::rad(215);
@@ -283,30 +277,28 @@ vpSimulatorViper850::init()
/*!
Method which initialises the parameters linked to the display part.
*/
-void
-vpSimulatorViper850::initDisplay()
+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);
+ getCameraParameters(tmp, 640, 480);
px_int = tmp.get_px();
py_int = tmp.get_py();
sceneInitialized = true;
}
-
/*!
Initialize the robot kinematics with the extrinsic calibration
parameters associated to a specific camera.
The eMc parameters depend on the camera.
-
+
\warning Only perspective projection without distortion is available!
\param tool : Tool to use.
@@ -315,49 +307,47 @@ 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;
-
+
// Use here default values of the robot constant parameters.
switch (tool) {
case vpViper850::TOOL_MARLIN_F033C_CAMERA: {
- erc[0] = vpMath::rad(0.07); // rx
- erc[1] = vpMath::rad(2.76); // ry
- erc[2] = vpMath::rad(-91.50); // rz
- etc[0] = -0.0453; // tx
- etc[1] = 0.0005; // ty
- etc[2] = 0.0728; // tz
-
- setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
- break;
- }
+ erc[0] = vpMath::rad(0.07); // rx
+ erc[1] = vpMath::rad(2.76); // ry
+ erc[2] = vpMath::rad(-91.50); // rz
+ etc[0] = -0.0453; // tx
+ etc[1] = 0.0005; // ty
+ etc[2] = 0.0728; // tz
+
+ setCameraParameters(vpCameraParameters(1232.0, 1233.0, 320, 240));
+ break;
+ }
case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
- erc[0] = vpMath::rad(0.1527764261); // rx
- erc[1] = vpMath::rad(1.285092455); // ry
- erc[2] = vpMath::rad(-90.75777848); // rz
- etc[0] = -0.04558630174; // tx
- etc[1] = -0.00134326752; // ty
- etc[2] = 0.001000828017; // tz
-
- setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
- break;
- }
+ erc[0] = vpMath::rad(0.1527764261); // rx
+ erc[1] = vpMath::rad(1.285092455); // ry
+ erc[2] = vpMath::rad(-90.75777848); // rz
+ etc[0] = -0.04558630174; // tx
+ etc[1] = -0.00134326752; // ty
+ etc[2] = 0.001000828017; // tz
+
+ setCameraParameters(vpCameraParameters(868.0, 869.0, 320, 240));
+ break;
+ }
case vpViper850::TOOL_SCHUNK_GRIPPER_CAMERA:
case vpViper850::TOOL_CUSTOM:
case vpViper850::TOOL_GENERIC_CAMERA: {
- std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
- break;
- }
+ std::cout << "This tool is not handled in vpSimulatorViper850.cpp" << std::endl;
+ break;
+ }
}
-
+
vpRotationMatrix eRc(erc);
this->eMc.buildFrom(etc, eRc);
-
+
setToolType(tool);
- return ;
+ return;
}
/*!
@@ -366,43 +356,39 @@ vpSimulatorViper850::init(vpViper850::vpToolType tool,
\param cam : In output, camera parameters to fill.
\param image_width : Image width used to compute camera calibration.
\param image_height : Image height used to compute camera calibration.
-
+
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const unsigned int &image_width,
- const unsigned int &image_height)
+void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const unsigned int &image_width,
+ const unsigned int &image_height)
{
- if (toolCustom)
- {
- vpCameraParameters(px_int,py_int,image_width/2,image_height/2);
+ if (toolCustom) {
+ 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::endl;
+ if (image_width == 640 && image_height == 480) {
+ std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_MARLIN_F033C_CAMERA_NAME << "\""
+ << std::endl;
cam.initPersProjWithoutDistortion(1232.0, 1233.0, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
}
break;
}
case vpViper850::TOOL_PTGREY_FLEA2_CAMERA: {
// Set default intrinsic camera parameters for 640x480 images
if (image_width == 640 && image_height == 480) {
- std::cout << "Get default camera parameters for camera \""
- << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\"" << std::endl;
+ std::cout << "Get default camera parameters for camera \"" << vpViper850::CONST_PTGREY_FLEA2_CAMERA_NAME << "\""
+ << std::endl;
cam.initPersProjWithoutDistortion(868.0, 869.0, 320, 240);
- }
- else {
- vpTRACE("Cannot get default intrinsic camera parameters for this image resolution");
+ } else {
+ vpTRACE("Cannot get default intrinsic camera parameters for this image "
+ "resolution");
}
break;
}
@@ -424,11 +410,9 @@ vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<unsigned char> &I_)
+void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const vpImage<unsigned char> &I_)
{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+ getCameraParameters(cam, I_.getWidth(), I_.getHeight());
}
/*!
@@ -439,189 +423,184 @@ vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
\warning The image size must be : 640x480 !
*/
-void
-vpSimulatorViper850::getCameraParameters (vpCameraParameters &cam,
- const vpImage<vpRGBa> &I_)
+void vpSimulatorViper850::getCameraParameters(vpCameraParameters &cam, const vpImage<vpRGBa> &I_)
{
- getCameraParameters(cam,I_.getWidth(),I_.getHeight());
+ getCameraParameters(cam, I_.getWidth(), I_.getHeight());
}
-
/*!
Set the intrinsic camera parameters.
-
+
\param cam : The desired camera parameters.
*/
-void
-vpSimulatorViper850::setCameraParameters(const vpCameraParameters &cam)
+void vpSimulatorViper850::setCameraParameters(const vpCameraParameters &cam)
{
px_int = cam.get_px();
py_int = cam.get_py();
toolCustom = true;
}
-
/*!
- Method lauched by the thread to compute the position of the robot in the articular frame.
+ Method lauched by the thread to compute the position of the robot in the
+ articular frame.
*/
-void
-vpSimulatorViper850::updateArticularPosition()
+void vpSimulatorViper850::updateArticularPosition()
{
- double tcur_1 = tcur;// temporary variable used to store the last time since the last command
+ double tcur_1 = tcur; // temporary variable used to store the last time
+ // since the last command
- while (!robotStop)
- {
- //Get current time
+ while (!robotStop) {
+ // Get current time
tprev = tcur_1;
tcur = vpTime::measureTimeMs();
-
-
- if(setVelocityCalled || !constantSamplingTimeMode){
+
+ if (setVelocityCalled || !constantSamplingTimeMode) {
setVelocityCalled = false;
computeArticularVelocity();
-
+
double ellapsedTime = (tcur - tprev) * 1e-3;
- if(constantSamplingTimeMode){//if we want a constant velocity, we force the ellapsed time to the given samplingTime
+ if (constantSamplingTimeMode) { // if we want a constant velocity, we
+ // force the ellapsed time to the given
+ // samplingTime
ellapsedTime = getSamplingTime(); // in second
}
-
+
vpColVector articularCoordinates = get_artCoord();
vpColVector articularVelocities = get_artVel();
-
- if (jointLimit)
- {
- double art = articularCoordinates[jointLimitArt-1] + ellapsedTime*articularVelocities[jointLimitArt-1];
- if (art <= joint_min[jointLimitArt-1] || art >= joint_max[jointLimitArt-1]) {
+
+ if (jointLimit) {
+ double art = articularCoordinates[jointLimitArt - 1] + ellapsedTime * articularVelocities[jointLimitArt - 1];
+ if (art <= joint_min[jointLimitArt - 1] || art >= joint_max[jointLimitArt - 1]) {
if (verbose_) {
- std::cout << "Joint " << jointLimitArt-1
- << " reaches a limit: " << vpMath::deg(joint_min[jointLimitArt-1]) << " < " << vpMath::deg(art) << " < " << vpMath::deg(joint_max[jointLimitArt-1]) << std::endl;
+ 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
+ } 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 && jointLimit == false) {
if (jl < 0)
- ellapsedTime = (joint_min[(unsigned int)(-jl-1)] - articularCoordinates[(unsigned int)(-jl-1)])/(articularVelocities[(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)])/(articularVelocities[(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);
}
set_artCoord(articularCoordinates);
set_artVel(articularVelocities);
-
+
compute_fMi();
-
- if (displayAllowed)
- {
+
+ 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)
- {
- while (get_displayBusy()) vpTime::wait(2);
+
+ if (displayType == MODEL_3D && displayAllowed) {
+ while (get_displayBusy())
+ vpTime::wait(2);
vpSimulatorViper850::getExternalImage(I);
set_displayBusy(false);
}
-
-
- if (displayType == MODEL_DH && displayAllowed)
- {
+
+ if (displayType == MODEL_DH && displayAllowed) {
vpHomogeneousMatrix fMit[8];
get_fMi(fMit);
-
- //vpDisplay::displayFrame(I,getExternalCameraPosition ()*fMi[6],cameraParam,0.2,vpColor::none);
+
+ // vpDisplay::displayFrame(I,getExternalCameraPosition
+ // ()*fMi[6],cameraParam,0.2,vpColor::none);
vpImagePoint iP, iP_1;
- vpPoint pt(0,0,0);
-
- pt.track(getExternalCameraPosition ());
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
- pt.track(getExternalCameraPosition ()*fMit[0]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
+ vpPoint pt(0, 0, 0);
+
+ pt.track(getExternalCameraPosition());
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
+ pt.track(getExternalCameraPosition() * fMit[0]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
- for (int k = 1; k < 7; k++)
- {
- pt.track(getExternalCameraPosition ()*fMit[k-1]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP_1);
-
- pt.track(getExternalCameraPosition ()*fMit[k]);
- vpMeterPixelConversion::convertPoint (cameraParam, pt.get_x(), pt.get_y(), iP);
-
- vpDisplay::displayLine(I,iP_1,iP,vpColor::green, thickness_);
+ for (int k = 1; k < 7; k++) {
+ pt.track(getExternalCameraPosition() * fMit[k - 1]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP_1);
+
+ pt.track(getExternalCameraPosition() * fMit[k]);
+ vpMeterPixelConversion::convertPoint(cameraParam, pt.get_x(), pt.get_y(), iP);
+
+ vpDisplay::displayLine(I, iP_1, iP, vpColor::green, thickness_);
}
- vpDisplay::displayCamera(I,getExternalCameraPosition ()*fMit[7],cameraParam,0.1,vpColor::green, thickness_);
+ vpDisplay::displayCamera(I, getExternalCameraPosition() * fMit[7], cameraParam, 0.1, vpColor::green,
+ thickness_);
}
-
- vpDisplay::flush(I);
-
- vpTime::wait( tcur, 1000 * getSamplingTime() );
+
+ vpDisplay::flush(I);
+
+ vpTime::wait(tcur, 1000 * getSamplingTime());
tcur_1 = tcur;
- }else{
+ } else {
vpTime::wait(tcur, vpTime::getMinTimeForUsleepCall());
}
}
}
/*!
- Compute the pose between the robot reference frame and the frames used to compute the Denavit-Hartenberg
- representation. The last element of the table corresponds to the pose between the reference frame and
- the camera frame.
-
- To compute the different homogeneous matrices, this function needs the articular coordinates as input.
-
- Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$,
- \f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$ fMc \f$ - where w is for wrist and e for effector-.
+ Compute the pose between the robot reference frame and the frames used to
+ compute the Denavit-Hartenberg representation. The last element of the table
+ corresponds to the pose between the reference frame and the camera frame.
+
+ To compute the different homogeneous matrices, this function needs the
+ articular coordinates as input.
+
+ Finally the output is a table of 8 elements : \f$ fM1 \f$,\f$ fM2 \f$,\f$
+ fM3 \f$,\f$ fM4 \f$,\f$ fM5 \f$, \f$ fM6 = fMw \f$,\f$ fM7 = fMe \f$ and \f$
+ fMc \f$ - where w is for wrist and e for effector-.
*/
-void
-vpSimulatorViper850::compute_fMi()
+void vpSimulatorViper850::compute_fMi()
{
- //vpColVector q = get_artCoord();
- vpColVector q(6);//; = get_artCoord();
+ // vpColVector q = get_artCoord();
+ vpColVector q(6); //; = get_artCoord();
q = get_artCoord();
-
+
vpHomogeneousMatrix fMit[8];
-
+
double q1 = q[0];
double q2 = q[1];
double q3 = q[2];
double q4 = q[3];
double q5 = q[4];
double q6 = q[5];
-
+
double c1 = cos(q1);
double s1 = sin(q1);
double c2 = cos(q2);
double s2 = sin(q2);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
+ double c23 = cos(q2 + q3);
+ double s23 = sin(q2 + q3);
double c4 = cos(q4);
double s4 = sin(q4);
double c5 = cos(q5);
double s5 = sin(q5);
double c6 = cos(q6);
double s6 = sin(q6);
-
+
fMit[0][0][0] = c1;
fMit[0][1][0] = s1;
fMit[0][2][0] = 0;
@@ -631,183 +610,186 @@ vpSimulatorViper850::compute_fMi()
fMit[0][0][2] = -s1;
fMit[0][1][2] = c1;
fMit[0][2][2] = 0;
- fMit[0][0][3] = a1*c1;
- fMit[0][1][3] = a1*s1;
+ fMit[0][0][3] = a1 * c1;
+ fMit[0][1][3] = a1 * s1;
fMit[0][2][3] = d1;
-
- fMit[1][0][0] = c1*c2;
- fMit[1][1][0] = s1*c2;
+
+ fMit[1][0][0] = c1 * c2;
+ fMit[1][1][0] = s1 * c2;
fMit[1][2][0] = -s2;
- fMit[1][0][1] = -c1*s2;
- fMit[1][1][1] = -s1*s2;
+ fMit[1][0][1] = -c1 * s2;
+ fMit[1][1][1] = -s1 * s2;
fMit[1][2][1] = -c2;
fMit[1][0][2] = -s1;
fMit[1][1][2] = c1;
fMit[1][2][2] = 0;
- fMit[1][0][3] = c1*(a2*c2+a1);
- fMit[1][1][3] = s1*(a2*c2+a1);
- fMit[1][2][3] = d1-a2*s2;
-
- double quickcomp1 = a3*c23-a2*c2-a1;
-
- fMit[2][0][0] = -c1*c23;
- fMit[2][1][0] = -s1*c23;
+ fMit[1][0][3] = c1 * (a2 * c2 + a1);
+ fMit[1][1][3] = s1 * (a2 * c2 + a1);
+ fMit[1][2][3] = d1 - a2 * s2;
+
+ double quickcomp1 = a3 * c23 - a2 * c2 - a1;
+
+ fMit[2][0][0] = -c1 * c23;
+ fMit[2][1][0] = -s1 * c23;
fMit[2][2][0] = s23;
fMit[2][0][1] = s1;
fMit[2][1][1] = -c1;
fMit[2][2][1] = 0;
- fMit[2][0][2] = c1*s23;
- fMit[2][1][2] = s1*s23;
+ fMit[2][0][2] = c1 * s23;
+ fMit[2][1][2] = s1 * s23;
fMit[2][2][2] = c23;
- fMit[2][0][3] = -c1*quickcomp1;
- fMit[2][1][3] = -s1*quickcomp1;
- fMit[2][2][3] = a3*s23-a2*s2+d1;
-
- double quickcomp2 = c1*(s23*d4 - quickcomp1);
- double quickcomp3 = s1*(s23*d4 - quickcomp1);
-
- fMit[3][0][0] = -c1*c23*c4+s1*s4;
- fMit[3][1][0] = -s1*c23*c4-c1*s4;
- fMit[3][2][0] = s23*c4;
- fMit[3][0][1] = c1*s23;
- fMit[3][1][1] = s1*s23;
+ fMit[2][0][3] = -c1 * quickcomp1;
+ fMit[2][1][3] = -s1 * quickcomp1;
+ fMit[2][2][3] = a3 * s23 - a2 * s2 + d1;
+
+ double quickcomp2 = c1 * (s23 * d4 - quickcomp1);
+ double quickcomp3 = s1 * (s23 * d4 - quickcomp1);
+
+ fMit[3][0][0] = -c1 * c23 * c4 + s1 * s4;
+ fMit[3][1][0] = -s1 * c23 * c4 - c1 * s4;
+ fMit[3][2][0] = s23 * c4;
+ fMit[3][0][1] = c1 * s23;
+ fMit[3][1][1] = s1 * s23;
fMit[3][2][1] = c23;
- fMit[3][0][2] = -c1*c23*s4-s1*c4;
- fMit[3][1][2] = -s1*c23*s4+c1*c4;
- fMit[3][2][2] = s23*s4;
+ fMit[3][0][2] = -c1 * c23 * s4 - s1 * c4;
+ fMit[3][1][2] = -s1 * c23 * s4 + c1 * c4;
+ fMit[3][2][2] = s23 * s4;
fMit[3][0][3] = quickcomp2;
fMit[3][1][3] = quickcomp3;
- fMit[3][2][3] = c23*d4+a3*s23-a2*s2+d1;
-
- fMit[4][0][0] = c1*(s23*s5-c5*c23*c4)+s1*c5*s4;
- fMit[4][1][0] = s1*(s23*s5-c5*c23*c4)-c1*c5*s4;
- fMit[4][2][0] = s23*c4*c5+c23*s5;
- fMit[4][0][1] = c1*c23*s4+s1*c4;
- fMit[4][1][1] = s1*c23*s4-c1*c4;
- fMit[4][2][1] = -s23*s4;
- fMit[4][0][2] = c1*(s23*c5+s5*c23*c4)-s1*s5*s4;
- fMit[4][1][2] = s1*(s23*c5+s5*c23*c4)+c1*s5*s4;
- fMit[4][2][2] = -s23*c4*s5+c23*c5;
+ fMit[3][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
+
+ fMit[4][0][0] = c1 * (s23 * s5 - c5 * c23 * c4) + s1 * c5 * s4;
+ fMit[4][1][0] = s1 * (s23 * s5 - c5 * c23 * c4) - c1 * c5 * s4;
+ fMit[4][2][0] = s23 * c4 * c5 + c23 * s5;
+ fMit[4][0][1] = c1 * c23 * s4 + s1 * c4;
+ fMit[4][1][1] = s1 * c23 * s4 - c1 * c4;
+ fMit[4][2][1] = -s23 * s4;
+ fMit[4][0][2] = c1 * (s23 * c5 + s5 * c23 * c4) - s1 * s5 * s4;
+ fMit[4][1][2] = s1 * (s23 * c5 + s5 * c23 * c4) + c1 * s5 * s4;
+ fMit[4][2][2] = -s23 * c4 * s5 + c23 * c5;
fMit[4][0][3] = quickcomp2;
fMit[4][1][3] = quickcomp3;
- fMit[4][2][3] = c23*d4+a3*s23-a2*s2+d1;
-
- fMit[5][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMit[5][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMit[5][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
- fMit[5][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMit[5][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMit[5][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
- fMit[5][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMit[5][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMit[5][2][2] = -s23*c4*s5+c23*c5;
+ fMit[4][2][3] = c23 * d4 + a3 * s23 - a2 * s2 + d1;
+
+ fMit[5][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
+ fMit[5][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
+ fMit[5][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
+ fMit[5][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
+ fMit[5][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
+ fMit[5][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
+ fMit[5][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
+ fMit[5][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
+ fMit[5][2][2] = -s23 * c4 * s5 + c23 * c5;
fMit[5][0][3] = quickcomp2;
fMit[5][1][3] = quickcomp3;
- fMit[5][2][3] = s23*a3+c23*d4-a2*s2+d1;
-
- fMit[6][0][0] = c1*(c23*(c4*c5*c6-s4*s6)-s23*s5*c6)-s1*(s4*c5*c6+c4*s6);
- fMit[6][1][0] = -s1*(c23*(-c4*c5*c6+s4*s6)+s23*s5*c6)+c1*(s4*c5*c6+c4*s6);
- fMit[6][2][0] = s23*(s4*s6-c4*c5*c6)-c23*s5*c6;
- fMit[6][0][1] = -c1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)+s1*(s4*c5*s6-c4*c6);
- fMit[6][1][1] = -s1*(c23*(c4*c5*s6+s4*c6)-s23*s5*s6)-c1*(s4*c5*s6-c4*c6);
- fMit[6][2][1] = s23*(c4*c5*s6+s4*c6)+c23*s5*s6;
- fMit[6][0][2] = c1*(c23*c4*s5+s23*c5)-s1*s4*s5;
- fMit[6][1][2] = s1*(c23*c4*s5+s23*c5)+c1*s4*s5;
- fMit[6][2][2] = -s23*c4*s5+c23*c5;
- fMit[6][0][3] = c1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)-s1*s4*s5*d6;
- fMit[6][1][3] = s1*(c23*(c4*s5*d6-a3)+s23*(c5*d6+d4)+a1+a2*c2)+c1*s4*s5*d6;
- fMit[6][2][3] = s23*(a3-c4*s5*d6)+c23*(c5*d6+d4)-a2*s2+d1;
-
+ fMit[5][2][3] = s23 * a3 + c23 * d4 - a2 * s2 + d1;
+
+ fMit[6][0][0] = c1 * (c23 * (c4 * c5 * c6 - s4 * s6) - s23 * s5 * c6) - s1 * (s4 * c5 * c6 + c4 * s6);
+ fMit[6][1][0] = -s1 * (c23 * (-c4 * c5 * c6 + s4 * s6) + s23 * s5 * c6) + c1 * (s4 * c5 * c6 + c4 * s6);
+ fMit[6][2][0] = s23 * (s4 * s6 - c4 * c5 * c6) - c23 * s5 * c6;
+ fMit[6][0][1] = -c1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) + s1 * (s4 * c5 * s6 - c4 * c6);
+ fMit[6][1][1] = -s1 * (c23 * (c4 * c5 * s6 + s4 * c6) - s23 * s5 * s6) - c1 * (s4 * c5 * s6 - c4 * c6);
+ fMit[6][2][1] = s23 * (c4 * c5 * s6 + s4 * c6) + c23 * s5 * s6;
+ fMit[6][0][2] = c1 * (c23 * c4 * s5 + s23 * c5) - s1 * s4 * s5;
+ fMit[6][1][2] = s1 * (c23 * c4 * s5 + s23 * c5) + c1 * s4 * s5;
+ fMit[6][2][2] = -s23 * c4 * s5 + c23 * c5;
+ fMit[6][0][3] = c1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) - s1 * s4 * s5 * d6;
+ fMit[6][1][3] = s1 * (c23 * (c4 * s5 * d6 - a3) + s23 * (c5 * d6 + d4) + a1 + a2 * c2) + c1 * s4 * s5 * d6;
+ fMit[6][2][3] = s23 * (a3 - c4 * s5 * d6) + c23 * (c5 * d6 + d4) - a2 * s2 + d1;
+
vpHomogeneousMatrix cMe;
get_cMe(cMe);
cMe = cMe.inverse();
-// fMit[7] = fMit[6] * cMe;
- vpViper::get_fMc(q,fMit[7]);
-
- #if defined(_WIN32)
-# if defined(WINRT_8_1)
+ // fMit[7] = fMit[6] * cMe;
+ vpViper::get_fMc(q, fMit[7]);
+
+#if defined(_WIN32)
+#if defined(WINRT_8_1)
WaitForSingleObjectEx(mutex_fMi, INFINITE, FALSE);
-# else // pure win32
+#else // pure win32
WaitForSingleObject(mutex_fMi, INFINITE);
-# endif
+#endif
for (int i = 0; i < 8; i++)
fMi[i] = fMit[i];
ReleaseMutex(mutex_fMi);
- #elif defined(VISP_HAVE_PTHREAD)
- pthread_mutex_lock (&mutex_fMi);
+#elif defined(VISP_HAVE_PTHREAD)
+ pthread_mutex_lock(&mutex_fMi);
for (int i = 0; i < 8; i++)
fMi[i] = fMit[i];
- pthread_mutex_unlock (&mutex_fMi);
- #endif
+ pthread_mutex_unlock(&mutex_fMi);
+#endif
}
-
/*!
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: {
// Start primitive STOP only if the current state is Velocity
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
stopMotion();
}
break;
}
case vpRobot::STATE_POSITION_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL == getRobotState()) {
std::cout << "Change the control mode from velocity to position control.\n";
stopMotion();
- }
- else {
- //std::cout << "Change the control mode from stop to position control.\n";
+ } else {
+ // std::cout << "Change the control mode from stop to position
+ // control.\n";
}
break;
}
case vpRobot::STATE_VELOCITY_CONTROL: {
- if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState ()) {
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
std::cout << "Change the control mode from stop to velocity control.\n";
}
break;
}
case vpRobot::STATE_ACCELERATION_CONTROL:
default:
- break ;
+ break;
}
- return vpRobot::setRobotState (newState);
+ return vpRobot::setRobotState(newState);
}
/*!
Apply a velocity to the robot.
\param frame : Control frame in which the velocity is expressed. Velocities
- could be expressed in articular, camera frame, reference frame or mixt frame.
+ could be expressed in articular, camera frame, reference frame or mixt
+frame.
- \param vel : Velocity vector. Translation velocities are expressed
- in m/s while rotation velocities in rad/s. The size of this vector
+ \param vel : Velocity vector. The size of this vector
is always 6.
- In articular, \f$ vel = [\dot{q}_1, \dot{q}_2, \dot{q}_3, \dot{q}_4,
- \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities.
+ \dot{q}_5, \dot{q}_6]^t \f$ correspond to joint velocities in rad/s.
- In camera frame, \f$ vel = [^{c} v_x, ^{c} v_y, ^{c} v_z, ^{c}
- \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is expressed in the
- camera frame.
+ \omega_x, ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector
+expressed in the camera frame, with translations velocities \f$ ^{c} v_x, ^{c}
+v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In reference frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{r}
- \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is expressed in the
- reference frame.
+ \omega_x, ^{r} \omega_y, ^{r} \omega_z]^t \f$ is a velocity twist vector
+expressed in the reference frame, with translations velocities \f$ ^{c} v_x,
+^{c} v_y, ^{c} v_z \f$ in m/s and rotation velocities \f$ ^{c}\omega_x, ^{c}
+\omega_y, ^{c} \omega_z \f$ in rad/s.
- In mixt frame, \f$ vel = [^{r} v_x, ^{r} v_y, ^{r} v_z, ^{c} \omega_x,
- ^{c} \omega_y, ^{c} \omega_z]^t \f$. In mixt frame, translations \f$ v_x,
- v_y, v_z \f$ are expressed in the reference frame and rotations \f$
- \omega_x, \omega_y, \omega_z \f$ in the camera frame.
-
+ ^{c} \omega_y, ^{c} \omega_z]^t \f$ is a velocity twist vector where,
+translations \f$ ^{r} v_x, ^{r} v_y, ^{r} v_z \f$ are expressed in the
+reference frame in m/s and rotations \f$ ^{c} \omega_x, ^{c} \omega_y, ^{c}
+\omega_z \f$ in the camera frame in rad/s.
+
\exception vpRobotException::wrongStateError : If a the robot is not
configured to handle a velocity. The robot can handle a velocity only if the
velocity control mode is set. For that, call setRobotState(
@@ -819,10 +801,10 @@ vpSimulatorViper850::setRobotState(vpRobot::vpRobotStateType newState)
setMaxTranslationVelocity() and setMaxRotationVelocity().
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
+#include <visp3/robot/vpSimulatorViper850.h>
int main()
{
@@ -836,7 +818,7 @@ int main()
qvel[3] = M_PI/8; // Joint 4 velocity in rad/s
qvel[4] = 0; // Joint 5 velocity in rad/s
qvel[5] = 0; // Joint 6 velocity in rad/s
-
+
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
@@ -851,197 +833,176 @@ 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) ");
- }
-
+ if (vpRobot::STATE_VELOCITY_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ throw vpRobotException(vpRobotException::wrongStateError,
+ "Cannot send a velocity to the robot "
+ "use setRobotState(vpRobot::STATE_VELOCITY_CONTROL) first) ");
+ }
+
vpColVector vel_sat(6);
- double scale_sat = 1;
+ double scale_sat = 1;
double vel_trans_max = getMaxTranslationVelocity();
- double vel_rot_max = getMaxRotationVelocity();
-
+ double vel_rot_max = getMaxRotationVelocity();
+
double vel_abs; // Absolute value
-
+
// Velocity saturation
- switch(frame)
- {
- // saturation in cartesian space
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
+ switch (frame) {
+ // saturation in cartesian space
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
+ throw;
+ }
- for (unsigned int i = 0 ; i < 3; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_trans_max && !jointLimit)
- {
- vel_trans_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g m/s in TRANSLATION "
- "(axis nr. %d).", vel[i], i+1);
- }
+ for (unsigned int i = 0; i < 3; ++i) {
+ vel_abs = fabs(vel[i]);
+ if (vel_abs > vel_trans_max && !jointLimit) {
+ vel_trans_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g m/s in TRANSLATION "
+ "(axis nr. %d).",
+ vel[i], i + 1);
+ }
- vel_abs = fabs (vel[i+3]);
- if (vel_abs > vel_rot_max && !jointLimit) {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i+3], i+4);
- }
+ vel_abs = fabs(vel[i + 3]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ vel[i + 3], i + 4);
}
+ }
- double scale_trans_sat = 1;
- double scale_rot_sat = 1;
- if (vel_trans_max > getMaxTranslationVelocity())
- scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
+ double scale_trans_sat = 1;
+ double scale_rot_sat = 1;
+ if (vel_trans_max > getMaxTranslationVelocity())
+ scale_trans_sat = getMaxTranslationVelocity() / vel_trans_max;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( (scale_trans_sat < 1) || (scale_rot_sat < 1) )
- {
- if (scale_trans_sat < scale_rot_sat)
- scale_sat = scale_trans_sat;
- else
- scale_sat = scale_rot_sat;
- }
- break;
+ if ((scale_trans_sat < 1) || (scale_rot_sat < 1)) {
+ if (scale_trans_sat < scale_rot_sat)
+ scale_sat = scale_trans_sat;
+ else
+ scale_sat = scale_rot_sat;
}
+ break;
+ }
- // saturation in joint space
- case vpRobot::ARTICULAR_FRAME :
- {
- if (vel.getRows() != 6)
- {
- vpERROR_TRACE ("The velocity vector must have a size of 6 !!!!");
- throw;
- }
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- vel_abs = fabs (vel[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", vel[i], i+1);
- }
- }
- double scale_rot_sat = 1;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
- break;
+ // saturation in joint space
+ case vpRobot::ARTICULAR_FRAME: {
+ if (vel.getRows() != 6) {
+ vpERROR_TRACE("The velocity vector must have a size of 6 !!!!");
+ throw;
}
- case vpRobot::MIXT_FRAME :
- {
- break;
+ for (unsigned int i = 0; i < 6; ++i) {
+ vel_abs = fabs(vel[i]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ vel[i], i + 1);
+ }
}
+ double scale_rot_sat = 1;
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (scale_rot_sat < 1)
+ scale_sat = scale_rot_sat;
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
}
-
- set_velocity (vel * scale_sat);
- setRobotFrame (frame);
+
+ set_velocity(vel * scale_sat);
+ setRobotFrame(frame);
setVelocityCalled = true;
}
-
/*!
Compute the articular velocity relative to the velocity in another frame.
*/
-void
-vpSimulatorViper850::computeArticularVelocity()
+void vpSimulatorViper850::computeArticularVelocity()
{
vpRobot::vpControlFrameType frame = getRobotFrame();
-
- double vel_rot_max = getMaxRotationVelocity();
-
+
+ double vel_rot_max = getMaxRotationVelocity();
+
vpColVector articularCoordinates = get_artCoord();
vpColVector velocityframe = get_velocity();
vpColVector articularVelocity;
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix eVc(eMc);
- vpViper850::get_eJe(articularCoordinates,eJe_);
- eJe_ = eJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,eJe_);
- articularVelocity = eJe_*eVc*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpViper850::get_fJe(articularCoordinates,fJe_);
- fJe_ = fJe_.pseudoInverse();
- if (singularityManagement)
- singularityTest(articularCoordinates,fJe_);
- articularVelocity = fJe_*velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- articularVelocity = velocityframe;
- set_artVel (articularVelocity);
- break;
- }
- case vpRobot::MIXT_FRAME :
- {
- break;
- }
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix eVc(eMc);
+ vpViper850::get_eJe(articularCoordinates, eJe_);
+ eJe_ = eJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates, eJe_);
+ articularVelocity = eJe_ * eVc * velocityframe;
+ set_artVel(articularVelocity);
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpMatrix fJe_;
+ vpViper850::get_fJe(articularCoordinates, fJe_);
+ fJe_ = fJe_.pseudoInverse();
+ if (singularityManagement)
+ singularityTest(articularCoordinates, fJe_);
+ articularVelocity = fJe_ * velocityframe;
+ set_artVel(articularVelocity);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ articularVelocity = velocityframe;
+ set_artVel(articularVelocity);
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
}
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- case vpRobot::REFERENCE_FRAME :
- {
- for (unsigned int i = 0 ; i < 6; ++ i)
- {
- double vel_abs = fabs (articularVelocity[i]);
- if (vel_abs > vel_rot_max && !jointLimit)
- {
- vel_rot_max = vel_abs;
- vpERROR_TRACE ("Excess velocity %g rad/s in ROTATION "
- "(axis nr. %d).", articularVelocity[i], i+1);
- }
- }
- double scale_rot_sat = 1;
- double scale_sat = 1;
- if (vel_rot_max > getMaxRotationVelocity())
- scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
- if ( scale_rot_sat < 1 )
- scale_sat = scale_rot_sat;
-
- set_artVel(articularVelocity * scale_sat);
- break;
- }
- case vpRobot::ARTICULAR_FRAME :
- case vpRobot::MIXT_FRAME :
- {
- break;
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME:
+ case vpRobot::REFERENCE_FRAME: {
+ for (unsigned int i = 0; i < 6; ++i) {
+ double vel_abs = fabs(articularVelocity[i]);
+ if (vel_abs > vel_rot_max && !jointLimit) {
+ vel_rot_max = vel_abs;
+ vpERROR_TRACE("Excess velocity %g rad/s in ROTATION "
+ "(axis nr. %d).",
+ articularVelocity[i], i + 1);
+ }
}
+ double scale_rot_sat = 1;
+ double scale_sat = 1;
+
+ if (vel_rot_max > getMaxRotationVelocity())
+ scale_rot_sat = getMaxRotationVelocity() / vel_rot_max;
+ if (scale_rot_sat < 1)
+ scale_sat = scale_rot_sat;
+
+ set_artVel(articularVelocity * scale_sat);
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME:
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
}
}
-
/*!
Get the robot velocities.
@@ -1051,13 +1012,13 @@ vpSimulatorViper850::computeArticularVelocity()
and rotations in rad/s.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+\dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1071,7 +1032,7 @@ int main()
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
vpSimulatorViper850 robot;
-
+
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
@@ -1088,46 +1049,39 @@ int main()
}
\endcode
*/
-void
-vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel)
+void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel)
{
vel.resize(6);
-
+
vpColVector articularCoordinates = get_artCoord();
vpColVector articularVelocity = get_artVel();
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- vpMatrix eJe_;
- vpVelocityTwistMatrix cVe(eMc);
- vpViper850::get_eJe(articularCoordinates,eJe_);
- vel = cVe*eJe_*articularVelocity;
- break ;
- }
- case vpRobot::ARTICULAR_FRAME :
- {
- vel = articularVelocity;
- break ;
- }
- case vpRobot::REFERENCE_FRAME :
- {
- vpMatrix fJe_;
- vpViper850::get_fJe(articularCoordinates,fJe_);
- vel = fJe_*articularVelocity;
- break ;
- }
- case vpRobot::MIXT_FRAME :
- {
- break ;
- }
- default:
- {
- vpERROR_TRACE ("Error in spec of vpRobot. "
- "Case not taken in account.");
- return;
- }
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ vpMatrix eJe_;
+ vpVelocityTwistMatrix cVe(eMc);
+ vpViper850::get_eJe(articularCoordinates, eJe_);
+ vel = cVe * eJe_ * articularVelocity;
+ break;
+ }
+ case vpRobot::ARTICULAR_FRAME: {
+ vel = articularVelocity;
+ break;
+ }
+ case vpRobot::REFERENCE_FRAME: {
+ vpMatrix fJe_;
+ vpViper850::get_fJe(articularCoordinates, fJe_);
+ vel = fJe_ * articularVelocity;
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ break;
+ }
+ default: {
+ vpERROR_TRACE("Error in spec of vpRobot. "
+ "Case not taken in account.");
+ return;
+ }
}
}
@@ -1142,13 +1096,12 @@ vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpCol
\param timestamp : Unix time in second since January 1st 1970.
\warning In camera frame, reference frame and mixt frame, the representation
- of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y, \dot
- z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
+ of the rotation is ThetaU. In that cases, \f$velocity = [\dot x, \dot y,
+ \dot z, \dot {\theta U}_x, \dot {\theta U}_y, \dot {\theta U}_z]\f$.
\sa getVelocity(const vpRobot::vpControlFrameType frame, vpColVector & vel)
*/
-void
-vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpColVector & vel, double ×tamp)
+void vpSimulatorViper850::getVelocity(const vpRobot::vpControlFrameType frame, vpColVector &vel, double ×tamp)
{
timestamp = vpTime::measureTimeSecond();
getVelocity(frame, vel);
@@ -1163,9 +1116,9 @@ vpSimulatorViper850::getVelocity (const vpRobot::vpControlFrameType frame, vpCol
and rotations in rad/s.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1179,7 +1132,7 @@ int main()
q_dot[5] = M_PI/16;// Joint 6 velocity in rad/s
vpSimulatorViper850 robot;
-
+
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
// Moves the joint in velocity
@@ -1196,11 +1149,10 @@ int main()
}
\endcode
*/
-vpColVector
-vpSimulatorViper850::getVelocity (vpRobot::vpControlFrameType frame)
+vpColVector vpSimulatorViper850::getVelocity(vpRobot::vpControlFrameType frame)
{
vpColVector vel(6);
- getVelocity (frame, vel);
+ getVelocity(frame, vel);
return vel;
}
@@ -1217,28 +1169,25 @@ 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);
- getVelocity (frame, vel);
+ getVelocity(frame, vel);
return vel;
}
-void
-vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
+void vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
{
- double vel_rot_max = getMaxRotationVelocity();
- double velmax = fabs(q[0]);
- for (unsigned int i = 1; i < 6; i++)
- {
+ double vel_rot_max = getMaxRotationVelocity();
+ double velmax = fabs(q[0]);
+ for (unsigned int i = 1; i < 6; i++) {
if (velmax < fabs(q[i]))
- velmax = fabs(q[i]);
+ velmax = fabs(q[i]);
}
-
- double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax*100);
+
+ double alpha = (getPositioningVelocity() * vel_rot_max) / (velmax * 100);
q = q * alpha;
}
@@ -1276,9 +1225,9 @@ vpSimulatorViper850::findHighestPositioningSpeed(vpColVector &q)
position is out of range.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1292,7 +1241,7 @@ int main()
position[5] = M_PI; // rotation around z axis, in rad
vpSimulatorViper850 robot;
-
+
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Set the max velocity to 20%
@@ -1303,7 +1252,8 @@ int main()
}
\endcode
- To catch the exception if the position is out of range, modify the code like:
+ To catch the exception if the position is out of range, modify the code
+like:
\code
try {
@@ -1316,146 +1266,124 @@ 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"
- "Modification of the robot state");
- // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
+ if (vpRobot::STATE_POSITION_CONTROL != getRobotState()) {
+ vpERROR_TRACE("Robot was not in position-based control\n"
+ "Modification of the robot state");
+ // setRobotState(vpRobot::STATE_POSITION_CONTROL) ;
}
-
+
vpColVector articularCoordinates = get_artCoord();
-
+
vpColVector error(6);
double errsqr = 0;
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- unsigned int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
- }
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ unsigned int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = q[i];
+ rxyz[i] = q[i + 3];
+ }
- vpRotationMatrix cRc2(rxyz);
- vpHomogeneousMatrix cMc2(txyz, cRc2);
-
- vpHomogeneousMatrix fMc_;
- vpViper850::get_fMc(articularCoordinates, fMc_);
-
- vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc2, qdes, verbose_);
- setVelocityCalled = true;
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
- set_artVel(error);
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
- }
- else
- {
- vpERROR_TRACE ("Positionning error.");
- throw vpRobotException (vpRobotException::positionOutOfRangeError,
- "Position out of range.");
- }
- }while (errsqr > 1e-8 && nbSol > 0);
+ vpRotationMatrix cRc2(rxyz);
+ vpHomogeneousMatrix cMc2(txyz, cRc2);
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- do
- {
- articularCoordinates = get_artCoord();
- error = q - articularCoordinates;
+ vpHomogeneousMatrix fMc_;
+ vpViper850::get_fMc(articularCoordinates, fMc_);
+
+ vpHomogeneousMatrix fMc2 = fMc_ * cMc2;
+
+ do {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc2, qdes, verbose_);
+ setVelocityCalled = true;
+ if (nbSol > 0) {
+ error = qdes - articularCoordinates;
errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
+ // findHighestPositioningSpeed(error);
set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (q);
+ if (errsqr < 1e-4) {
+ set_artCoord(qdes);
error = 0;
set_artVel(error);
set_velocity(error);
break;
}
- }while (errsqr > 1e-8);
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- unsigned int nbSol;
- vpColVector qdes(6);
-
- vpTranslationVector txyz;
- vpRxyzVector rxyz;
- for (unsigned int i=0; i < 3; i++)
- {
- txyz[i] = q[i];
- rxyz[i] = q[i+3];
+ } else {
+ vpERROR_TRACE("Positionning error.");
+ throw vpRobotException(vpRobotException::positionOutOfRangeError, "Position out of range.");
+ }
+ } while (errsqr > 1e-8 && nbSol > 0);
+
+ break;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ do {
+ articularCoordinates = get_artCoord();
+ error = q - articularCoordinates;
+ errsqr = error.sumSquare();
+ // findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4) {
+ set_artCoord(q);
+ error = 0;
+ set_artVel(error);
+ set_velocity(error);
+ break;
}
+ } while (errsqr > 1e-8);
+ break;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ unsigned int nbSol;
+ vpColVector qdes(6);
+
+ vpTranslationVector txyz;
+ vpRxyzVector rxyz;
+ for (unsigned int i = 0; i < 3; i++) {
+ txyz[i] = q[i];
+ rxyz[i] = q[i + 3];
+ }
- vpRotationMatrix fRc(rxyz);
- vpHomogeneousMatrix fMc_(txyz, fRc);
-
- do
- {
- articularCoordinates = get_artCoord();
- qdes = articularCoordinates;
- nbSol = getInverseKinematics(fMc_, qdes, verbose_);
- if (nbSol > 0)
- {
- error = qdes - articularCoordinates;
- errsqr = error.sumSquare();
- //findHighestPositioningSpeed(error);
+ vpRotationMatrix fRc(rxyz);
+ vpHomogeneousMatrix fMc_(txyz, fRc);
+
+ do {
+ articularCoordinates = get_artCoord();
+ qdes = articularCoordinates;
+ nbSol = getInverseKinematics(fMc_, qdes, verbose_);
+ if (nbSol > 0) {
+ error = qdes - articularCoordinates;
+ errsqr = error.sumSquare();
+ // findHighestPositioningSpeed(error);
+ set_artVel(error);
+ setVelocityCalled = true;
+ if (errsqr < 1e-4) {
+ set_artCoord(qdes);
+ error = 0;
set_artVel(error);
- setVelocityCalled = true;
- if (errsqr < 1e-4)
- {
- set_artCoord (qdes);
- error = 0;
- set_artVel(error);
- set_velocity(error);
- break;
- }
+ set_velocity(error);
+ break;
}
- else
- vpERROR_TRACE ("Positionning error. Position unreachable");
- }while (errsqr > 1e-8 && nbSol > 0);
- break ;
- }
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
- }
+ } else
+ vpERROR_TRACE("Positionning error. Position unreachable");
+ } while (errsqr > 1e-8 && nbSol > 0);
+ break;
+ }
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
}
@@ -1522,30 +1450,23 @@ int main()
\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) ;
- position[0] = pos1 ;
- position[1] = pos2 ;
- position[2] = pos3 ;
- position[3] = pos4 ;
- position[4] = pos5 ;
- position[5] = pos6 ;
-
- setPosition(frame, position) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught");
- throw ;
- }
+ try {
+ vpColVector position(6);
+ position[0] = pos1;
+ position[1] = pos2;
+ position[2] = pos3;
+ position[3] = pos4;
+ position[4] = pos5;
+ position[5] = pos6;
+
+ setPosition(frame, position);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
+ }
}
/*!
@@ -1560,9 +1481,9 @@ void vpSimulatorViper850::setPosition (const vpRobot::vpControlFrameType frame,
This method has the same behavior than the sample code given below;
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
@@ -1591,9 +1512,8 @@ void vpSimulatorViper850::setPosition(const char *filename)
ret = this->readPosFile(filename, q);
if (ret == false) {
- vpERROR_TRACE ("Bad position in \"%s\"", filename);
- throw vpRobotException (vpRobotException::lowLevelError,
- "Bad position in filename.");
+ vpERROR_TRACE("Bad position in \"%s\"", filename);
+ throw vpRobotException(vpRobotException::lowLevelError, "Bad position in filename.");
}
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
this->setPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -1621,13 +1541,13 @@ void vpSimulatorViper850::setPosition(const char *filename)
below show how to convert this position into a vpHomogeneousMatrix:
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
-#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/robot/vpSimulatorViper850.h>
int main()
{
@@ -1654,56 +1574,49 @@ int main()
}
\endcode
- \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q, double ×tamp)
- \sa setPosition(const vpRobot::vpControlFrameType frame, const vpColVector & r)
+ \sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q,
+double ×tamp) \sa setPosition(const vpRobot::vpControlFrameType frame,
+const vpColVector & r)
*/
-void
-vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
+void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColVector &q)
{
q.resize(6);
-
- switch(frame)
- {
- case vpRobot::CAMERA_FRAME :
- {
- q = 0;
- break ;
- }
-
- case vpRobot::ARTICULAR_FRAME:
- {
- q = get_artCoord();
- break ;
- }
-
- case vpRobot::REFERENCE_FRAME:
- {
- vpHomogeneousMatrix fMc_;
- vpViper::get_fMc (get_artCoord(), fMc_);
-
- vpRotationMatrix fRc;
- fMc_.extract(fRc);
- vpRxyzVector rxyz(fRc);
-
- vpTranslationVector txyz;
- fMc_.extract(txyz);
-
- for (unsigned int i=0; i <3; i++)
- {
- q[i] = txyz[i];
- q[i+3] = rxyz[i];
- }
- break ;
- }
-
- case vpRobot::MIXT_FRAME:
- {
- vpERROR_TRACE ("Positionning error. Mixt frame not implemented");
- throw vpRobotException (vpRobotException::lowLevelError,
- "Positionning error: "
- "Mixt frame not implemented.");
+
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ q = 0;
+ break;
+ }
+
+ case vpRobot::ARTICULAR_FRAME: {
+ q = get_artCoord();
+ break;
+ }
+
+ case vpRobot::REFERENCE_FRAME: {
+ vpHomogeneousMatrix fMc_;
+ vpViper::get_fMc(get_artCoord(), fMc_);
+
+ vpRotationMatrix fRc;
+ fMc_.extract(fRc);
+ vpRxyzVector rxyz(fRc);
+
+ vpTranslationVector txyz;
+ fMc_.extract(txyz);
+
+ for (unsigned int i = 0; i < 3; i++) {
+ q[i] = txyz[i];
+ q[i + 3] = rxyz[i];
}
+ break;
+ }
+
+ case vpRobot::MIXT_FRAME: {
+ vpERROR_TRACE("Positionning error. Mixt frame not implemented");
+ throw vpRobotException(vpRobotException::lowLevelError, "Positionning error: "
+ "Mixt frame not implemented.");
+ }
}
}
@@ -1733,40 +1646,36 @@ vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpColV
\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);
}
-
/*!
Get the current position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &)
The difference is here that the position is returned using a ThetaU
representation.
\sa getPosition(const vpRobot::vpControlFrameType frame, vpColVector &)
*/
-void
-vpSimulatorViper850::getPosition (const vpRobot::vpControlFrameType frame,
- vpPoseVector &position)
+void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position)
{
vpColVector posRxyz;
- //recupere position en Rxyz
- this->getPosition(frame,posRxyz);
-
- //recupere le vecteur thetaU correspondant
- vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3],posRxyz[4],posRxyz[5]));
-
- //remplit le vpPoseVector avec translation et rotation ThetaU
- for(unsigned int j=0;j<3;j++)
- {
- position[j]=posRxyz[j];
- position[j+3]=RtuVect[j];
+ // recupere position en Rxyz
+ this->getPosition(frame, posRxyz);
+
+ // recupere le vecteur thetaU correspondant
+ vpThetaUVector RtuVect(vpRxyzVector(posRxyz[3], posRxyz[4], posRxyz[5]));
+
+ // remplit le vpPoseVector avec translation et rotation ThetaU
+ for (unsigned int j = 0; j < 3; j++) {
+ position[j] = posRxyz[j];
+ position[j + 3] = RtuVect[j];
}
}
@@ -1774,35 +1683,35 @@ vpSimulatorViper850::getPosition (const vpRobot::vpControlFrameType frame,
Get the current time stamped position of the robot.
- Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector &, double &)
+ Similar as getPosition(const vpRobot::vpControlFrameType frame, vpColVector
+ &, double &)
The difference is here that the position is returned using a ThetaU
representation.
*/
-void
-vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame,
- vpPoseVector &position, double ×tamp)
+void vpSimulatorViper850::getPosition(const vpRobot::vpControlFrameType frame, vpPoseVector &position,
+ double ×tamp)
{
timestamp = vpTime::measureTimeSecond();
getPosition(frame, position);
}
/*!
- This method enables to set the minimum and maximum joint limits for all the six axis of the robot. All the values have to be given in radian.
-
- \param limitMin : The minimum joint limits are given in a vector of size 6. All the value must be in radian.
- \param limitMax : The maximum joint limits are given in a vector of size 6. All the value must be in radian.
+ This method enables to set the minimum and maximum joint limits for all the
+ six axis of the robot. All the values have to be given in radian.
+
+ \param limitMin : The minimum joint limits are given in a vector of size 6.
+ All the value must be in radian. \param limitMax : The maximum joint limits
+ are given in a vector of size 6. All the value must be in radian.
*/
-void
-vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
+void vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVector &limitMax)
{
- if (limitMin.getRows() != 6 || limitMax.getRows() != 6)
- {
+ if (limitMin.getRows() != 6 || limitMax.getRows() != 6) {
vpTRACE("Joint limit vector has not a size of 6 !");
return;
}
-
+
joint_min[0] = limitMin[0];
joint_min[1] = limitMin[1];
joint_min[2] = limitMin[2];
@@ -1816,16 +1725,14 @@ vpSimulatorViper850::setJointLimit(const vpColVector &limitMin, const vpColVecto
joint_max[3] = limitMax[3];
joint_max[4] = limitMax[4];
joint_max[5] = limitMax[5];
-
}
-/*!
+/*!
Test to detect if the robot is near one of its singularities.
-
+
The goal is to avoid the problems du to such configurations.
*/
-bool
-vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
+bool vpSimulatorViper850::singularityTest(const vpColVector &q, vpMatrix &J)
{
double q2 = q[1];
double q3 = q[2];
@@ -1834,16 +1741,15 @@ vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
double c2 = cos(q2);
double c3 = cos(q3);
double s3 = sin(q3);
- double c23 = cos(q2+q3);
- double s23 = sin(q2+q3);
+ double c23 = cos(q2 + q3);
+ double s23 = sin(q2 + q3);
double s5 = sin(q5);
-
+
bool cond1 = fabs(s5) < 1e-1;
- bool cond2 = fabs(a3*s3+c3*d4) < 1e-1;
- bool cond3 = fabs(a2*c2-a3*c23+s23*d4+a1) < 1e-1;
-
- if(cond1)
- {
+ bool cond2 = fabs(a3 * s3 + c3 * d4) < 1e-1;
+ bool cond3 = fabs(a2 * c2 - a3 * c23 + s23 * d4 + a1) < 1e-1;
+
+ if (cond1) {
J[3][0] = 0;
J[5][0] = 0;
J[3][1] = 0;
@@ -1858,98 +1764,76 @@ vpSimulatorViper850::singularityTest(const vpColVector q, vpMatrix &J)
J[5][5] = 0;
return true;
}
-
- if(cond2)
- {
- J[1][0] = 0; J[2][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
- J[1][1] = 0; J[2][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
- J[1][2] = 0; J[2][2] = 0; J[3][2] = 0; J[4][2] = 0; J[5][2] = 0;
+
+ if (cond2) {
+ J[1][0] = 0;
+ J[2][0] = 0;
+ J[3][0] = 0;
+ J[4][0] = 0;
+ J[5][0] = 0;
+ J[1][1] = 0;
+ J[2][1] = 0;
+ J[3][1] = 0;
+ J[4][1] = 0;
+ J[5][1] = 0;
+ J[1][2] = 0;
+ J[2][2] = 0;
+ J[3][2] = 0;
+ J[4][2] = 0;
+ J[5][2] = 0;
return true;
}
-
- if(cond3)
- {
- J[0][0] = 0; J[3][0] = 0; J[4][0] = 0; J[5][0] = 0;
- J[0][1] = 0; J[3][1] = 0; J[4][1] = 0; J[5][1] = 0;
+
+ if (cond3) {
+ J[0][0] = 0;
+ J[3][0] = 0;
+ J[4][0] = 0;
+ J[5][0] = 0;
+ J[0][1] = 0;
+ J[3][1] = 0;
+ J[4][1] = 0;
+ J[5][1] = 0;
}
-
+
return false;
}
-/*!
+/*!
Method used to check if the robot reached a joint limit.
*/
-int
-vpSimulatorViper850::isInJointLimit ()
+int vpSimulatorViper850::isInJointLimit()
{
int artNumb = 0;
double diff = 0;
double difft = 0;
-
+
vpColVector articularCoordinates = get_artCoord();
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] <= joint_min[i])
- {
+
+ for (unsigned int i = 0; i < 6; i++) {
+ if (articularCoordinates[i] <= joint_min[i]) {
difft = joint_min[i] - articularCoordinates[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = -(int)i-1;
+ if (difft > diff) {
+ diff = difft;
+ artNumb = -(int)i - 1;
}
}
}
-
- for (unsigned int i = 0; i < 6; i++)
- {
- if (articularCoordinates[i] >= joint_max[i])
- {
+
+ for (unsigned int i = 0; i < 6; i++) {
+ if (articularCoordinates[i] >= joint_max[i]) {
difft = articularCoordinates[i] - joint_max[i];
- if (difft > diff)
- {
- diff = difft;
- artNumb = (int)(i+1);
+ if (difft > diff) {
+ diff = difft;
+ artNumb = (int)(i + 1);
}
}
}
-
- if (artNumb != 0)
- std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!" <<std::endl;
-
- return artNumb;
-}
-
-/*!
- Get the robot displacement expressed in the camera frame since the last call
- of this method.
-
- \param displacement : The measured displacement in the camera frame. The
- dimension of \e displacement is 6 (tx, ty, ty, rx, ry,
- rz). Translations are expressed in meters, rotations in radians with
- the Euler Rxyz representation.
-
- \sa getDisplacement(), getArticularDisplacement()
-*/
-void
-vpSimulatorViper850::getCameraDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::CAMERA_FRAME, displacement);
-}
-
-/*!
- Get the robot joint displacement since the last call of this method.
- \param displacement : The measured joint displacement. The dimension
- of \e displacement is 6 (the robot joint number). All the values are
- expressed in radians.
+ if (artNumb != 0)
+ std::cout << "\nWarning: Velocity control stopped: axis " << fabs((float)artNumb) << " on joint limit!"
+ << std::endl;
- \sa getDisplacement(), getCameraDisplacement()
-*/
-void
-vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
-{
- getDisplacement(vpRobot::ARTICULAR_FRAME, displacement);
+ return artNumb;
}
/*!
@@ -1968,49 +1852,38 @@ vpSimulatorViper850::getArticularDisplacement(vpColVector &displacement)
In camera or reference frame, rotations are expressed with the
Euler Rxyz representation.
- \sa getArticularDisplacement(), getCameraDisplacement()
*/
-void
-vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame,
- vpColVector &displacement)
+void vpSimulatorViper850::getDisplacement(vpRobot::vpControlFrameType frame, vpColVector &displacement)
{
- displacement.resize (6);
+ displacement.resize(6);
displacement = 0;
vpColVector q_cur(6);
q_cur = get_artCoord();
- if ( ! first_time_getdis )
- {
- switch (frame)
- {
- case vpRobot::CAMERA_FRAME:
- {
- std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
- return;
- }
+ if (!first_time_getdis) {
+ switch (frame) {
+ case vpRobot::CAMERA_FRAME: {
+ std::cout << "getDisplacement() CAMERA_FRAME not implemented\n";
+ return;
+ }
- case vpRobot::ARTICULAR_FRAME:
- {
- displacement = q_cur - q_prev_getdis;
- break ;
- }
+ case vpRobot::ARTICULAR_FRAME: {
+ displacement = q_cur - q_prev_getdis;
+ break;
+ }
- case vpRobot::REFERENCE_FRAME:
- {
- std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
- return;
- }
+ case vpRobot::REFERENCE_FRAME: {
+ std::cout << "getDisplacement() REFERENCE_FRAME not implemented\n";
+ return;
+ }
- case vpRobot::MIXT_FRAME:
- {
- std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
- return;
- }
+ case vpRobot::MIXT_FRAME: {
+ std::cout << "getDisplacement() MIXT_FRAME not implemented\n";
+ return;
}
- }
- else
- {
+ }
+ } else {
first_time_getdis = false;
}
@@ -2049,29 +1922,30 @@ R: 0.1 0.3 -0.25 -80.5 80 0
\return true if the positions were successfully readen in the file. false, if
an error occurs.
-The code below shows how to read a position from a file and move the robot to this position.
+The code below shows how to read a position from a file and move the robot to
+this position.
\code
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpSimulatorViper850.h>
-#include <visp3/core/vpColVector.h>
int main()
{
vpSimulatorViper850 robot;
-
+
// Enable the position control of the robot
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
// Get the current robot joint positions
vpColVector q; // Current joint position
- robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
+ robot.getPosition(vpRobot::ARTICULAR_FRAME, q);
// Save this position in a file named "current.pos"
- robot.savePosFile("current.pos", q);
+ robot.savePosFile("current.pos", q);
// Get the position from a file and move to the registered position
robot.readPosFile("current.pos", q); // Set the joint position from the file
-
+
robot.setPositioningVelocity(5); // Positioning velocity set to 5%
robot.setPosition(vpRobot::ARTICULAR_FRAME, q); // Move to the joint position
}
@@ -2079,12 +1953,11 @@ int main()
\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);
- if(! fd.is_open()) {
+ if (!fd.is_open()) {
return false;
}
@@ -2096,29 +1969,29 @@ vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
q.resize(njoint);
- while(std::getline(fd, line)) {
- lineNum ++;
+ while (std::getline(fd, line)) {
+ lineNum++;
if (lineNum == 1) {
- if(! (line.compare(0, id.size(), id) == 0)) { // check if Viper850 position file
+ 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;
}
}
- if((line.compare(0, 1, "#") == 0)) { // skip comment
+ if ((line.compare(0, 1, "#") == 0)) { // skip comment
continue;
}
- if((line.compare(0, key.size(), key) == 0)) { // decode position
+ if ((line.compare(0, key.size(), key) == 0)) { // decode position
// check if there are at least njoint values in the line
std::vector<std::string> chain = vpIoTools::splitChain(line, std::string(" "));
- if (chain.size() < njoint+1) // try to split with tab separator
+ if (chain.size() < njoint + 1) // try to split with tab separator
chain = vpIoTools::splitChain(line, std::string("\t"));
- if(chain.size() < njoint+1)
+ if (chain.size() < njoint + 1)
continue;
std::istringstream ss(line);
std::string key_;
ss >> key_;
- for (unsigned int i=0; i< njoint; i++)
+ for (unsigned int i = 0; i < njoint; i++)
ss >> q[i];
pos_found = true;
break;
@@ -2159,12 +2032,11 @@ vpSimulatorViper850::readPosFile(const std::string &filename, vpColVector &q)
\sa readPosFile()
*/
-bool
-vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
+bool vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector &q)
{
- FILE * fd ;
- fd = fopen(filename.c_str(), "w") ;
+ FILE *fd;
+ fd = fopen(filename.c_str(), "w");
if (fd == NULL)
return false;
@@ -2177,15 +2049,10 @@ vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector
#\n\n");
// Save positions in mm and deg
- fprintf(fd, "R: %lf %lf %lf %lf %lf %lf\n",
- vpMath::deg(q[0]),
- vpMath::deg(q[1]),
- vpMath::deg(q[2]),
- vpMath::deg(q[3]),
- vpMath::deg(q[4]),
- vpMath::deg(q[5]));
-
- fclose(fd) ;
+ 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);
}
@@ -2196,19 +2063,15 @@ vpSimulatorViper850::savePosFile(const std::string &filename, const vpColVector
\sa readPosFile
*/
-void
-vpSimulatorViper850::move(const char *filename)
+void vpSimulatorViper850::move(const char *filename)
{
vpColVector q;
- try
- {
+ try {
this->readPosFile(filename, q);
this->setRobotState(vpRobot::STATE_POSITION_CONTROL);
- this->setPosition ( vpRobot::ARTICULAR_FRAME, q);
- }
- catch(...)
- {
+ this->setPosition(vpRobot::ARTICULAR_FRAME, q);
+ } catch (...) {
throw;
}
}
@@ -2222,11 +2085,7 @@ 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
@@ -2235,13 +2094,12 @@ vpSimulatorViper850::get_cMe(vpHomogeneousMatrix &cMe)
\param cVe : Twist transformation.
*/
-void
-vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
+void vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
{
- vpHomogeneousMatrix cMe ;
- vpViper850::get_cMe(cMe) ;
+ vpHomogeneousMatrix cMe;
+ vpViper850::get_cMe(cMe);
- cVe.buildFrom(cMe) ;
+ cVe.buildFrom(cMe);
}
/*!
@@ -2253,17 +2111,13 @@ vpSimulatorViper850::get_cVe(vpVelocityTwistMatrix &cVe)
\param eJe_ : Robot jacobian \f$^e{\bf J}_e\f$ expressed in the
end-effector frame.
*/
-void
-vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
+void vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
{
- try
- {
- vpViper850::get_eJe(get_artCoord(), eJe_) ;
- }
- catch(...)
- {
- vpERROR_TRACE("catch exception ") ;
- throw ;
+ try {
+ vpViper850::get_eJe(get_artCoord(), eJe_);
+ } catch (...) {
+ vpERROR_TRACE("catch exception ");
+ throw;
}
}
@@ -2277,39 +2131,32 @@ vpSimulatorViper850::get_eJe(vpMatrix &eJe_)
\param fJe_ : Robot jacobian \f$^f{\bf J}_e\f$ expressed in the
reference frame.
*/
-void
-vpSimulatorViper850::get_fJe(vpMatrix &fJe_)
+void vpSimulatorViper850::get_fJe(vpMatrix &fJe_)
{
- try
- {
+ try {
vpColVector articularCoordinates = get_artCoord();
- vpViper850::get_fJe(articularCoordinates, fJe_) ;
- }
- catch(...)
- {
+ vpViper850::get_fJe(articularCoordinates, fJe_);
+ } catch (...) {
vpERROR_TRACE("Error caught");
- throw ;
+ throw;
}
}
/*!
Stop the robot.
*/
-void
-vpSimulatorViper850::stopMotion()
+void vpSimulatorViper850::stopMotion()
{
if (getRobotState() != vpRobot::STATE_VELOCITY_CONTROL)
return;
-
+
vpColVector stop(6);
stop = 0;
set_artVel(stop);
set_velocity(stop);
- vpRobot::setRobotState (vpRobot::STATE_STOP);
+ vpRobot::setRobotState(vpRobot::STATE_STOP);
}
-
-
/**********************************************************************************/
/**********************************************************************************/
/**********************************************************************************/
@@ -2323,189 +2170,180 @@ vpSimulatorViper850::stopMotion()
not valid, the path is set from the VISP_SCENES_DIR environment
variable that the user has to set.
*/
-void
-vpSimulatorViper850::initArms()
+void vpSimulatorViper850::initArms()
{
// set scene_dir from #define VISP_SCENE_DIR if it exists
// VISP_SCENES_DIR may contain multiple locations separated by ";"
std::string scene_dir_;
std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
bool sceneDirExists = false;
- for(size_t i=0; i < scene_dirs.size(); i++)
- if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
- scene_dir_ = scene_dirs[i];
- sceneDirExists = true;
- break;
- }
- if (! sceneDirExists) {
+ for (size_t i = 0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir_ = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (!sceneDirExists) {
try {
scene_dir_ = vpIoTools::getenv("VISP_SCENES_DIR");
std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir_ << std::endl;
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
}
}
unsigned int name_length = 30; // the size of this kind of string "/viper850_arm2.bnd"
if (scene_dir_.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+ 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];
+ char *name_cam = new char[full_length];
strcpy(name_cam, scene_dir_.c_str());
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
-
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
+
if (arm_dir.size() > FILENAME_MAX)
- throw vpException (vpException::dimensionError, "Cannot initialize Viper850 simulator");
+ 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];
+ char *name_arm = new char[full_length];
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm1.bnd");
+ strcat(name_arm, "/viper850_arm1.bnd");
set_scene(name_arm, robotArms, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm2.bnd");
- set_scene(name_arm, robotArms+1, 1.0);
+ strcat(name_arm, "/viper850_arm2.bnd");
+ set_scene(name_arm, robotArms + 1, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm3.bnd");
- set_scene(name_arm, robotArms+2, 1.0);
+ strcat(name_arm, "/viper850_arm3.bnd");
+ set_scene(name_arm, robotArms + 2, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm4.bnd");
- set_scene(name_arm, robotArms+3, 1.0);
+ strcat(name_arm, "/viper850_arm4.bnd");
+ set_scene(name_arm, robotArms + 3, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm5.bnd");
- set_scene(name_arm, robotArms+4, 1.0);
+ strcat(name_arm, "/viper850_arm5.bnd");
+ set_scene(name_arm, robotArms + 4, 1.0);
strcpy(name_arm, arm_dir.c_str());
- strcat(name_arm,"/viper850_arm6.bnd");
- set_scene(name_arm, robotArms+5, 1.0);
-
-// set_scene("./arm2.bnd", robotArms+1, 1.0);
-// set_scene("./arm3.bnd", robotArms+2, 1.0);
-// set_scene("./arm4.bnd", robotArms+3, 1.0);
-// set_scene("./arm5.bnd", robotArms+4, 1.0);
-// set_scene("./arm6.bnd", robotArms+5, 1.0);
-
+ strcat(name_arm, "/viper850_arm6.bnd");
+ set_scene(name_arm, robotArms + 5, 1.0);
+
+ // set_scene("./arm2.bnd", robotArms+1, 1.0);
+ // set_scene("./arm3.bnd", robotArms+2, 1.0);
+ // set_scene("./arm4.bnd", robotArms+3, 1.0);
+ // set_scene("./arm5.bnd", robotArms+4, 1.0);
+ // set_scene("./arm6.bnd", robotArms+5, 1.0);
+
add_rfstack(IS_BACK);
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
-//
-// sceneInitialized = true;
-// displayObject = true;
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
+ //
+ // sceneInitialized = true;
+ // displayObject = true;
displayCamera = true;
- delete [] name_cam;
- delete [] name_arm;
+ delete[] name_cam;
+ delete[] name_arm;
}
-
-void
-vpSimulatorViper850::getExternalImage(vpImage<vpRGBa> &I_)
+void vpSimulatorViper850::getExternalImage(vpImage<vpRGBa> &I_)
{
bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I_,changed);
+ vpHomogeneousMatrix displacement = navigation(I_, changed);
- //if (displacement[2][3] != 0)
+ // if (displacement[2][3] != 0)
if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon())
- camMf2 = camMf2*displacement;
+ camMf2 = camMf2 * displacement;
- f2Mf = camMf2.inverse()*camMf;
+ f2Mf = camMf2.inverse() * camMf;
- camMf = camMf2* displacement * f2Mf;
+ camMf = camMf2 * displacement * f2Mf;
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I_.getWidth()/(2*px_ext);
- v = (double)I_.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I_.getWidth()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
- v = (double)I_.getHeight()/(vpMath::minimum(I_.getWidth(),I_.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I_.getWidth() / (2 * px_ext);
+ v = (double)I_.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I_.getWidth() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
+ v = (double)I_.getHeight() / (vpMath::minimum(I_.getWidth(), I_.getHeight()));
}
- float w44o[4][4],w44cext[4][4],x,y,z;
+ float w44o[4][4], w44cext[4][4], x, y, z;
- vp2jlc_matrix(camMf.inverse(),w44cext);
+ vp2jlc_matrix(camMf.inverse(), w44cext);
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+
vpHomogeneousMatrix fMit[8];
get_fMi(fMit);
-
- vp2jlc_matrix(vpHomogeneousMatrix(0,0,0,0,0,0),w44o);
- display_scene(w44o,robotArms[0],I_, curColor);
-
- vp2jlc_matrix(fMit[0],w44o);
- display_scene(w44o,robotArms[1],I_, curColor);
-
- vp2jlc_matrix(fMit[1],w44o);
- display_scene(w44o,robotArms[2],I_, curColor);
-
- vp2jlc_matrix(fMit[2],w44o);
- display_scene(w44o,robotArms[3],I_, curColor);
-
- vp2jlc_matrix(fMit[3],w44o);
- display_scene(w44o,robotArms[4],I_, curColor);
-
- vp2jlc_matrix(fMit[6],w44o);
- display_scene(w44o,robotArms[5],I_, curColor);
-
- if (displayCamera)
- {
+
+ vp2jlc_matrix(vpHomogeneousMatrix(0, 0, 0, 0, 0, 0), w44o);
+ display_scene(w44o, robotArms[0], I_, curColor);
+
+ vp2jlc_matrix(fMit[0], w44o);
+ display_scene(w44o, robotArms[1], I_, curColor);
+
+ vp2jlc_matrix(fMit[1], w44o);
+ display_scene(w44o, robotArms[2], I_, curColor);
+
+ vp2jlc_matrix(fMit[2], w44o);
+ display_scene(w44o, robotArms[3], I_, curColor);
+
+ vp2jlc_matrix(fMit[3], w44o);
+ display_scene(w44o, robotArms[4], I_, curColor);
+
+ vp2jlc_matrix(fMit[6], w44o);
+ display_scene(w44o, robotArms[5], I_, curColor);
+
+ if (displayCamera) {
vpHomogeneousMatrix cMe;
get_cMe(cMe);
cMe = cMe.inverse();
cMe = fMit[6] * cMe;
- vp2jlc_matrix(cMe,w44o);
- display_scene(w44o,camera, I_, camColor);
+ vp2jlc_matrix(cMe, w44o);
+ display_scene(w44o, camera, I_, camColor);
}
-
- if (displayObject)
- {
- vp2jlc_matrix(fMo,w44o);
- display_scene(w44o,scene,I_, curColor);
+
+ if (displayObject) {
+ vp2jlc_matrix(fMo, w44o);
+ display_scene(w44o, scene, I_, curColor);
}
}
/*!
- This method enables to initialise the joint coordinates of the robot in order to
- position the camera relative to the object.
-
- Before using this method it is advised to set the position of the object thanks
- to the set_fMo() method.
+ This method enables to initialise the joint coordinates of the robot in
+ order to position the camera relative to the object.
+
+ Before using this method it is advised to set the position of the object
+ thanks to the set_fMo() method.
In other terms, set the world to camera transformation
- \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}{_o})^{-1}\f$, and from the inverse kinematics
- set the joint positions \f${\bf q}\f$ that corresponds to the \f${^f}{\bf M}_c\f$ transformation.
-
+ \f${^f}{\bf M}_c = {^f}{\bf M}_o \; ({^c}{\bf M}{_o})^{-1}\f$, and from the
+ inverse kinematics set the joint positions \f${\bf q}\f$ that corresponds to
+ the \f${^f}{\bf M}_c\f$ transformation.
+
\param cMo_ : the desired pose of the camera.
\return false if the robot kinematics is not able to reach the cMo position.
*/
-bool
-vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
+bool vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix &cMo_)
{
vpColVector stop(6);
bool status = true;
@@ -2514,39 +2352,39 @@ vpSimulatorViper850::initialiseCameraRelativeToObject(const vpHomogeneousMatrix
set_velocity(stop);
vpHomogeneousMatrix fMc_;
fMc_ = fMo * cMo_.inverse();
-
+
vpColVector articularCoordinates = get_artCoord();
unsigned int nbSol = getInverseKinematics(fMc_, articularCoordinates, verbose_);
-
+
if (nbSol == 0) {
status = false;
- vpERROR_TRACE ("Positionning error. Position unreachable");
+ vpERROR_TRACE("Positionning error. Position unreachable");
}
if (verbose_)
std::cout << "Used joint coordinates (rad): " << articularCoordinates.t() << std::endl;
set_artCoord(articularCoordinates);
-
+
compute_fMi();
return status;
}
/*!
- This method enables to initialise the pose between the object and the reference frame,
- in order to position the object relative to the camera.
-
- Before using this method it is advised to set the articular coordinates of the robot.
+ This method enables to initialise the pose between the object and the
+ reference frame, in order to position the object relative to the camera.
+
+ Before using this method it is advised to set the articular coordinates of
+ the robot.
In other terms, set the world to object transformation
- \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c = f({\bf q})\f$
- with \f${\bf q}\f$ the robot joint position
-
+ \f${^f}{\bf M}_o = {^f}{\bf M}_c \; {^c}{\bf M}_o\f$ where \f$ {^f}{\bf M}_c
+ = f({\bf q})\f$ with \f${\bf q}\f$ the robot joint position
+
\param cMo_ : the desired pose of the camera.
*/
-void
-vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
+void vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix &cMo_)
{
vpColVector stop(6);
stop = 0;
@@ -2558,6 +2396,7 @@ vpSimulatorViper850::initialiseObjectRelativeToCamera(const vpHomogeneousMatrix
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o) has no symbols
-void dummy_vpSimulatorViper850() {};
+// Work arround to avoid warning: libvisp_robot.a(vpSimulatorViper850.cpp.o)
+// has no symbols
+void dummy_vpSimulatorViper850(){};
#endif
diff --git a/modules/robot/src/vpRobot.cpp b/modules/robot/src/vpRobot.cpp
index 11e151a..2191d88 100644
--- a/modules/robot/src/vpRobot.cpp
+++ b/modules/robot/src/vpRobot.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,37 +36,33 @@
*
*****************************************************************************/
+#include <visp3/core/vpDebug.h>
#include <visp3/robot/vpRobot.h>
#include <visp3/robot/vpRobotException.h>
-#include <visp3/core/vpDebug.h>
-
const double vpRobot::maxTranslationVelocityDefault = 0.2;
const double vpRobot::maxRotationVelocityDefault = 0.7;
-/* ------------------------------------------------------------------------- */
-/* --- CONSTRUCTEUR -------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
-vpRobot::vpRobot (void)
- :
- stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
- maxTranslationVelocity (maxTranslationVelocityDefault),
- maxRotationVelocity (maxRotationVelocityDefault),
- nDof(0),
- eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
- qmin(NULL), qmax(NULL), verbose_(true)
+/* -------------------------------------------------------------------------
+ */
+/* --- CONSTRUCTEUR --------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
+
+vpRobot::vpRobot(void)
+ : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
+ maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
+ eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
+ verbose_(true)
{
}
-vpRobot::vpRobot (const vpRobot &robot)
- :
- stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
- maxTranslationVelocity (maxTranslationVelocityDefault),
- maxRotationVelocity (maxRotationVelocityDefault),
- nDof(0),
- eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false),
- qmin(NULL), qmax(NULL), verbose_(true)
+vpRobot::vpRobot(const vpRobot &robot)
+ : stateRobot(vpRobot::STATE_STOP), frameRobot(vpRobot::CAMERA_FRAME),
+ maxTranslationVelocity(maxTranslationVelocityDefault), maxRotationVelocity(maxRotationVelocityDefault), nDof(0),
+ eJe(), eJeAvailable(false), fJe(), fJeAvailable(false), areJointLimitsAvailable(false), qmin(NULL), qmax(NULL),
+ verbose_(true)
{
*this = robot;
}
@@ -76,17 +73,17 @@ vpRobot::vpRobot (const vpRobot &robot)
vpRobot::~vpRobot()
{
if (qmin != NULL) {
- delete [] qmin;
+ delete[] qmin;
qmin = NULL;
}
if (qmax != NULL) {
- delete [] qmax;
+ delete[] qmax;
qmax = NULL;
}
}
/*! Copy operator. */
-vpRobot & vpRobot::operator=(const vpRobot &robot)
+vpRobot &vpRobot::operator=(const vpRobot &robot)
{
stateRobot = robot.stateRobot;
frameRobot = robot.frameRobot;
@@ -95,12 +92,12 @@ vpRobot & vpRobot::operator=(const vpRobot &robot)
nDof = robot.nDof;
eJe = robot.eJe;
eJeAvailable = robot.eJeAvailable;
- fJe= robot.fJe;
+ fJe = robot.fJe;
fJeAvailable = robot.fJeAvailable;
areJointLimitsAvailable = robot.areJointLimitsAvailable;
- qmin = new double [nDof];
- qmax = new double [nDof];
- for (int i = 0; i< nDof; i++) {
+ qmin = new double[nDof];
+ qmax = new double[nDof];
+ for (int i = 0; i < nDof; i++) {
qmin[i] = robot.qmin[i];
qmax[i] = robot.qmax[i];
}
@@ -111,19 +108,23 @@ vpRobot & vpRobot::operator=(const vpRobot &robot)
/*!
Saturate velocities.
- \param v_in : Vector of input velocities to saturate. Translation velocities should
- be expressed in m/s while rotation velocities in rad/s.
+ \param v_in : Vector of input velocities to saturate. Translation velocities
+should be expressed in m/s while rotation velocities in rad/s.
- \param v_max : Vector of maximal allowed velocities. Maximal translation velocities
- should be expressed in m/s while maximal rotation velocities in rad/s.
+ \param v_max : Vector of maximal allowed velocities. Maximal translation
+velocities should be expressed in m/s while maximal rotation velocities in
+rad/s.
- \param verbose : Print a message indicating which axis causes the saturation.
+ \param verbose : Print a message indicating which axis causes the
+saturation.
\return Saturated velocities.
- \exception vpRobotException::dimensionError : If the input vectors have different dimensions.
+ \exception vpRobotException::dimensionError : If the input vectors have
+different dimensions.
- The code below shows how to use this static method in order to saturate a velocity skew vector.
+ The code below shows how to use this static method in order to saturate a
+velocity skew vector.
\code
#include <iostream>
@@ -159,21 +160,19 @@ 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++)
- {
+ double scale = 1; // global scale factor to saturate all the axis
+ for (unsigned int i = 0; i < size; i++) {
double v_i = fabs(v_in[i]);
double v_max_i = fabs(v_max[i]);
- if ( v_i > v_max_i ) // Test if we should saturate the axis
+ if (v_i > v_max_i) // Test if we should saturate the axis
{
- double scale_i = v_max_i/v_i;
+ double scale_i = v_max_i / v_i;
if (scale_i < scale)
scale = scale_i;
@@ -188,92 +187,87 @@ vpRobot::saturateVelocities(const vpColVector &v_in, const vpColVector &v_max, b
return v_sat;
}
-
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
\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 ;
+ stateRobot = newState;
+ return newState;
}
-vpRobot::vpControlFrameType
-vpRobot::setRobotFrame (vpRobot::vpControlFrameType newFrame)
+vpRobot::vpControlFrameType vpRobot::setRobotFrame(vpRobot::vpControlFrameType newFrame)
{
- frameRobot = newFrame ;
- return newFrame ;
+ frameRobot = newFrame;
+ return newFrame;
}
/*!
Return the current robot position in the specified frame.
*/
-vpColVector
-vpRobot::getPosition (vpRobot::vpControlFrameType frame)
+vpColVector vpRobot::getPosition(vpRobot::vpControlFrameType frame)
{
vpColVector r;
- this ->getPosition (frame, r);
+ this->getPosition(frame, r);
return r;
}
-/* ------------------------------------------------------------------------- */
-/* --- VELOCITY CONTROL ---------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- VELOCITY CONTROL ----------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
-/*!
+/*!
- Set the maximal translation velocity that can be sent to the robot during a velocity control.
+ Set the maximal translation velocity that can be sent to the robot during a
+ velocity control.
\param v_max : Maximum translation velocity expressed in m/s.
*/
-void
-vpRobot::setMaxTranslationVelocity (const double v_max)
+void vpRobot::setMaxTranslationVelocity(const double v_max)
{
- this ->maxTranslationVelocity = v_max;
+ this->maxTranslationVelocity = v_max;
return;
}
/*!
- Get the maximal translation velocity that can be sent to the robot during a velocity control.
+ Get the maximal translation velocity that can be sent to the robot during a
+ velocity control.
\return Maximum translation velocity expressed in m/s.
*/
-double
-vpRobot::getMaxTranslationVelocity (void) const
-{
- return this ->maxTranslationVelocity;
-}
-/*!
+double vpRobot::getMaxTranslationVelocity(void) const { return this->maxTranslationVelocity; }
+/*!
- Set the maximal rotation velocity that can be sent to the robot during a velocity control.
+ Set the maximal rotation velocity that can be sent to the robot during a
+ velocity control.
\param w_max : Maximum rotation velocity expressed in rad/s.
*/
-void
-vpRobot::setMaxRotationVelocity (const double w_max)
+void vpRobot::setMaxRotationVelocity(const double w_max)
{
- this ->maxRotationVelocity = w_max;
+ this->maxRotationVelocity = w_max;
return;
}
-/*!
+/*!
- Get the maximal rotation velocity that can be sent to the robot during a velocity control.
+ Get the maximal rotation velocity that can be sent to the robot during a
+ velocity control.
\return Maximum rotation velocity expressed in rad/s.
*/
-double
-vpRobot::getMaxRotationVelocity (void) const
-{
- return this ->maxRotationVelocity;
-}
-
+double vpRobot::getMaxRotationVelocity(void) const { return this->maxRotationVelocity; }
diff --git a/modules/robot/src/vpRobotTemplate.cpp b/modules/robot/src/vpRobotTemplate.cpp
index b3bb060..4264f61 100644
--- a/modules/robot/src/vpRobotTemplate.cpp
+++ b/modules/robot/src/vpRobotTemplate.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,35 +36,28 @@
*
*****************************************************************************/
-
/*!
\file vpRobotTemplate.cpp
- \brief class that defines a robot just to show which function you must implement
+ \brief class that defines a robot just to show which function you must
+ implement
*/
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpRobotTemplate.h>
-#include <visp3/core/vpDebug.h>
//! basic initialization
void vpRobotTemplate::init()
{
- vpTRACE(" Get the joint limits " ) ;
+ vpTRACE(" Get the joint limits ");
std::cout << "Not implemented ! " << std::endl;
}
//! constructor
-vpRobotTemplate::vpRobotTemplate()
-{
- init() ;
-}
-
+vpRobotTemplate::vpRobotTemplate() { init(); }
//! constructor
-vpRobotTemplate::~vpRobotTemplate()
-{
- std::cout << "Not implemented ! " << std::endl;
-}
+vpRobotTemplate::~vpRobotTemplate() { std::cout << "Not implemented ! " << std::endl; }
/*
@@ -75,19 +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; }
/*
@@ -99,31 +84,27 @@ sendArticularVelocity
*/
//! send to the controller a velocity expressed in the camera frame
-void vpRobotTemplate::sendCameraVelocity(const vpColVector &/* v */)
+void vpRobotTemplate::sendCameraVelocity(const vpColVector & /* v */)
{
std::cout << "Not implemented ! " << std::endl;
- std::cout << "To implement me you need : " << std::endl ;
- std::cout << "\t to known the robot jacobian expressed in " ;
- std::cout << "the end-effector frame (eJe) " <<std::endl ;
- std::cout << "\t the frame transformation between camera frame " ;
- std::cout << "and end-effector frame (cMe)" << std::endl ;
+ std::cout << "To implement me you need : " << std::endl;
+ std::cout << "\t to known the robot jacobian expressed in ";
+ std::cout << "the end-effector frame (eJe) " << std::endl;
+ std::cout << "\t the frame transformation between camera frame ";
+ std::cout << "and end-effector frame (cMe)" << std::endl;
}
//! send to the controller a velocity expressed in the articular frame
-void
-vpRobotTemplate::sendArticularVelocity(const vpColVector &/* qdot */)
+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;
}
-
/*
THESE FUNCTIONS ARE NOT MENDATORY BUT ARE USUALLY USEFUL
@@ -131,40 +112,25 @@ 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;
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/robot/src/wireframe-simulator/vpArit.cpp b/modules/robot/src/wireframe-simulator/vpArit.cpp
index 9221e85..6ea5095 100644
--- a/modules/robot/src/wireframe-simulator/vpArit.cpp
+++ b/modules/robot/src/wireframe-simulator/vpArit.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,12 @@
*
*****************************************************************************/
-#include "vpMy.h"
#include "vpArit.h"
-#include <visp3/core/vpMath.h>
-#include <stdio.h>
+#include "vpMy.h"
#include <math.h>
+#include <stdio.h>
#include <string.h>
+#include <visp3/core/vpMath.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/*
@@ -49,39 +50,36 @@
* fp Fichier en sortie.
* m Matrice a ecrire.
*/
-void
-fprintf_matrix (FILE *fp, Matrix m)
+void fprintf_matrix(FILE *fp, Matrix m)
{
- int i;
+ int i;
- fprintf (fp, "(matrix\n");
- for (i = 0; i < 4; i++)
- fprintf (fp, "\t%.4f\t%.4f\t%.4f\t%.4f\n",
- m[i][0], m[i][1], m[i][2], m[i][3]);
- fprintf (fp, ")\n");
+ fprintf(fp, "(matrix\n");
+ for (i = 0; i < 4; i++)
+ fprintf(fp, "\t%.4f\t%.4f\t%.4f\t%.4f\n", m[i][0], m[i][1], m[i][2], m[i][3]);
+ fprintf(fp, ")\n");
}
-/*
+/*
* La procedure "ident_matrix" initialise la matrice par la matrice identite.
* Entree :
* m Matrice a initialiser.
*/
-void
-ident_matrix (Matrix m)
+void ident_matrix(Matrix m)
{
- static Matrix identity = IDENTITY_MATRIX;
-
- //bcopy ((char *) identity, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) identity, sizeof (Matrix));
-/*
- * Version moins rapide.
- *
- * int i, j;
- *
- * for (i = 0; i < 4; i++)
- * for (j = 0; j < 4; j++)
- * m[i][j] = (i == j) ? 1.0 : 0.0;
- */
+ static Matrix identity = IDENTITY_MATRIX;
+
+ // bcopy ((char *) identity, (char *) m, sizeof (Matrix));
+ memmove((char *)m, (char *)identity, sizeof(Matrix));
+ /*
+ * Version moins rapide.
+ *
+ * int i, j;
+ *
+ * for (i = 0; i < 4; i++)
+ * for (j = 0; j < 4; j++)
+ * m[i][j] = (i == j) ? 1.0 : 0.0;
+ */
}
/*
@@ -90,20 +88,16 @@ ident_matrix (Matrix m)
* a Premiere matrice du produit a = b * a.
* b Seconde matrice du produit.
*/
-void
-premult_matrix (Matrix a, Matrix b)
+void premult_matrix(Matrix a, Matrix b)
{
- Matrix m;
- int i, j;
-
- for (i = 0; i < 4; i++)
- for (j = 0; j < 4; j++)
- m[i][j] = b[i][0] * a[0][j] +
- b[i][1] * a[1][j] +
- b[i][2] * a[2][j] +
- b[i][3] * a[3][j];
- //bcopy ((char *) m, (char *) a, sizeof (Matrix));
- memmove ((char *) a, (char *) m, sizeof (Matrix));
+ Matrix m;
+ int i, j;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ m[i][j] = b[i][0] * a[0][j] + b[i][1] * a[1][j] + b[i][2] * a[2][j] + b[i][3] * a[3][j];
+ // bcopy ((char *) m, (char *) a, sizeof (Matrix));
+ memmove((char *)a, (char *)m, sizeof(Matrix));
}
/*
@@ -113,19 +107,16 @@ premult_matrix (Matrix a, Matrix b)
* a Premiere matrice du produit a = b * a.
* b Seconde matrice du produit 3x3.
*/
-void
-premult3_matrix (Matrix a, Matrix b)
+void premult3_matrix(Matrix a, Matrix b)
{
- Matrix m;
- int i, j;
-
- //bcopy ((char *) a, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) a, sizeof (Matrix));
- for (i = 0; i < 3; i++)
- for (j = 0; j < 4; j++)
- a[i][j] = b[i][0] * m[0][j] +
- b[i][1] * m[1][j] +
- b[i][2] * m[2][j];
+ Matrix m;
+ int i, j;
+
+ // bcopy ((char *) a, (char *) m, sizeof (Matrix));
+ memmove((char *)m, (char *)a, sizeof(Matrix));
+ for (i = 0; i < 3; i++)
+ for (j = 0; j < 4; j++)
+ a[i][j] = b[i][0] * m[0][j] + b[i][1] * m[1][j] + b[i][2] * m[2][j];
}
/*
@@ -134,16 +125,15 @@ premult3_matrix (Matrix a, Matrix b)
* m Matrice a multiplier m = vp * m.
* vp Vecteur d'homothetie.
*/
-void
-prescale_matrix (Matrix m, Vector *vp)
+void prescale_matrix(Matrix m, Vector *vp)
{
- int i;
+ int i;
- for (i = 0; i < 4; i++) {
- m[0][i] *= vp->x;
- m[1][i] *= vp->y;
- m[2][i] *= vp->z;
- }
+ for (i = 0; i < 4; i++) {
+ m[0][i] *= vp->x;
+ m[1][i] *= vp->y;
+ m[2][i] *= vp->z;
+ }
}
/*
@@ -152,13 +142,12 @@ prescale_matrix (Matrix m, Vector *vp)
* m Matrice a multiplier m = vp * m.
* vp Vecteur de translation.
*/
-void
-pretrans_matrix (Matrix m, Vector *vp)
+void pretrans_matrix(Matrix m, Vector *vp)
{
- int i;
+ int i;
- for (i = 0; i < 4; i++)
- m[3][i] += vp->x * m[0][i] + vp->y * m[1][i] + vp->z * m[2][i];
+ for (i = 0; i < 4; i++)
+ m[3][i] += vp->x * m[0][i] + vp->y * m[1][i] + vp->z * m[2][i];
}
/*
@@ -168,25 +157,27 @@ pretrans_matrix (Matrix m, Vector *vp)
* m Matrice a rendre gauche m = m * left.
* axis Axe de la matrice gauche 'x', 'y' ou 'z'.
*/
-void
-postleft_matrix (Matrix m, char axis)
+void postleft_matrix(Matrix m, char axis)
{
- int i;
-
- switch (axis) {
- case 'x' :
- for (i = 0; i < 4; i++) m[i][0] = - m[i][0];
- break;
- case 'y' :
- for (i = 0; i < 4; i++) m[i][1] = - m[i][1];
- break;
- case 'z' :
- for (i = 0; i < 4; i++) m[i][2] = - m[i][2];
- break;
- default : {
- static char proc_name[] = "postleft_matrix";
- fprintf (stderr, "%s: axis unknown\n", proc_name);
+ int i;
+
+ switch (axis) {
+ case 'x':
+ for (i = 0; i < 4; i++)
+ m[i][0] = -m[i][0];
+ break;
+ case 'y':
+ for (i = 0; i < 4; i++)
+ m[i][1] = -m[i][1];
+ break;
+ case 'z':
+ for (i = 0; i < 4; i++)
+ m[i][2] = -m[i][2];
+ break;
+ default: {
+ static char proc_name[] = "postleft_matrix";
+ fprintf(stderr, "%s: axis unknown\n", proc_name);
break;
}
}
@@ -198,42 +189,35 @@ postleft_matrix (Matrix m, char axis)
* a Premiere matrice du produit a = a * b.
* b Seconde matrice du produit.
*/
-void
-postmult_matrix (Matrix a, Matrix b)
+void postmult_matrix(Matrix a, Matrix b)
{
- Matrix m;
- int i, j;
-
- for (i = 0; i < 4; i++)
- for (j = 0; j < 4; j++)
- m[i][j] = a[i][0] * b[0][j] +
- a[i][1] * b[1][j] +
- a[i][2] * b[2][j] +
- a[i][3] * b[3][j];
- //bcopy ((char *) m, (char *) a, sizeof (Matrix));
- memmove ((char *) a, (char *) m, sizeof (Matrix));
+ Matrix m;
+ int i, j;
+
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 4; j++)
+ m[i][j] = a[i][0] * b[0][j] + a[i][1] * b[1][j] + a[i][2] * b[2][j] + a[i][3] * b[3][j];
+ // bcopy ((char *) m, (char *) a, sizeof (Matrix));
+ memmove((char *)a, (char *)m, sizeof(Matrix));
}
/*
- * La procedure "postmult3_matrix" postmultiplie la matrice par une matrice 3x3.
- * Note : La procedure "postmult3_matrix" optimise "postmutl_matrix".
+ * La procedure "postmult3_matrix" postmultiplie la matrice par une matrice
+ * 3x3. Note : La procedure "postmult3_matrix" optimise "postmutl_matrix".
* Entree :
* a Premiere matrice du produit a = a * b.
* b Seconde matrice du produit 3x3.
*/
-void
-postmult3_matrix (Matrix a, Matrix b)
+void postmult3_matrix(Matrix a, Matrix b)
{
- Matrix m;
- int i, j;
-
- //bcopy ((char *) a, (char *) m, sizeof (Matrix));
- memmove ((char *) m, (char *) a, sizeof (Matrix));
- for (i = 0; i < 4; i++)
- for (j = 0; j < 3; j++)
- a[i][j] = m[i][0] * b[0][j] +
- m[i][1] * b[1][j] +
- m[i][2] * b[2][j];
+ Matrix m;
+ int i, j;
+
+ // bcopy ((char *) a, (char *) m, sizeof (Matrix));
+ memmove((char *)m, (char *)a, sizeof(Matrix));
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < 3; j++)
+ a[i][j] = m[i][0] * b[0][j] + m[i][1] * b[1][j] + m[i][2] * b[2][j];
}
/*
@@ -242,34 +226,31 @@ postmult3_matrix (Matrix a, Matrix b)
* m Matrice a multiplier m = m * vp.
* vp Vecteur d'homothetie.
*/
-void
-postscale_matrix (Matrix m, Vector *vp)
+void postscale_matrix(Matrix m, Vector *vp)
{
- int i;
+ int i;
- for (i = 0; i < 4; i++) {
- m[i][0] *= vp->x;
- m[i][1] *= vp->y;
- m[i][2] *= vp->z;
- }
+ for (i = 0; i < 4; i++) {
+ m[i][0] *= vp->x;
+ m[i][1] *= vp->y;
+ m[i][2] *= vp->z;
+ }
}
/*
- * La procedure "posttrans_matrix" post mutiplie la matrice par la translation.
- * Entree :
- * m Matrice a multiplier m = m * vp.
- * vp Vecteur de translation.
+ * La procedure "posttrans_matrix" post mutiplie la matrice par la
+ * translation. Entree : m Matrice a multiplier m = m * vp. vp
+ * Vecteur de translation.
*/
-void
-posttrans_matrix (Matrix m, Vector *vp)
+void posttrans_matrix(Matrix m, Vector *vp)
{
- int i;
+ int i;
- for (i = 0; i < 4; i++) {
- m[i][0] += m[i][3] * vp->x;
- m[i][1] += m[i][3] * vp->y;
- m[i][2] += m[i][3] * vp->z;
- }
+ for (i = 0; i < 4; i++) {
+ m[i][0] += m[i][3] * vp->x;
+ m[i][1] += m[i][3] * vp->y;
+ m[i][2] += m[i][3] * vp->z;
+ }
}
/*
@@ -277,15 +258,14 @@ posttrans_matrix (Matrix m, Vector *vp)
* Entree :
* m Matrice a transposer.
*/
-void
-transpose_matrix (Matrix m)
+void transpose_matrix(Matrix m)
{
- unsigned int i, j;
- float t;
+ unsigned int i, j;
+ float t;
- for (i = 0; i < 4; i++)
- for (j = 0; j < i; j++)
- SWAP(m[i][j],m[j][i],t);
+ for (i = 0; i < 4; i++)
+ for (j = 0; j < i; j++)
+ SWAP(m[i][j], m[j][i], t);
}
/*
@@ -296,19 +276,18 @@ transpose_matrix (Matrix m)
* Sortie :
* Angle en radians.
*/
-float
-cosin_to_angle (float ca, float sa)
+float cosin_to_angle(float ca, float sa)
{
- float a; /* angle a calculer */
-
- if (FABS(ca) < M_EPSILON) {
- a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2);
- }
- else {
- a = (float) atan((double) (sa / ca));
- if (ca < (float)0.0) a += (sa > (float)0.0) ? (float)M_PI : (float)(- M_PI);
- }
- return (a);
+ float a; /* angle a calculer */
+
+ if (FABS(ca) < M_EPSILON) {
+ a = (sa > (float)0.0) ? (float)M_PI_2 : (float)(-M_PI_2);
+ } else {
+ a = (float)atan((double)(sa / ca));
+ if (ca < (float)0.0)
+ a += (sa > (float)0.0) ? (float)M_PI : (float)(-M_PI);
+ }
+ return (a);
}
/*
@@ -319,40 +298,43 @@ cosin_to_angle (float ca, float sa)
* coslut Table pour la fonction "cosinus".
* sinlut Table pour la fonction "sinus".
*/
-void
-cosin_to_lut (Index level, float *coslut, float *sinlut)
+void cosin_to_lut(Index level, float *coslut, float *sinlut)
{
- int i;
- int i_pi_2 = TWO_POWER(level);
- int quad; /* quadrant courant */
- double a; /* angle courant */
- double step = M_PI_2 / (double) i_pi_2;
-
- quad = 0;
- coslut[quad] = 1.0; sinlut[quad] = 0.0; /* 0 */
- quad += i_pi_2;
- coslut[quad] = 0.0; sinlut[quad] = 1.0; /* PI/2 */
- quad += i_pi_2;
- coslut[quad] = -1.0; sinlut[quad] = 0.0; /* PI */
- quad += i_pi_2;
- coslut[quad] = 0.0; sinlut[quad] = -1.0; /* 3PI/2*/
-
- for (i = 1, a = step; i < i_pi_2; i++, a += step) {
- float ca = (float) cos (a);
- quad = 0;
- coslut[quad + i] = ca; /* cos(a) */
- quad += i_pi_2;
- sinlut[quad - i] = ca; /* sin(PI/2-a) */
- sinlut[quad + i] = ca; /* sin(PI/2+a) */
- quad += i_pi_2;
- coslut[quad - i] = - ca; /* cos(PI-a) */
- coslut[quad + i] = - ca; /* cos(PI+a) */
- quad += i_pi_2;
- sinlut[quad - i] = - ca; /* sin(3PI/2-a) */
- sinlut[quad + i] = - ca; /* sin(3PI/2+a) */
- quad += i_pi_2;
- coslut[quad - i] = ca; /* cos(2PI-a) */
- }
+ int i;
+ int i_pi_2 = TWO_POWER(level);
+ int quad; /* quadrant courant */
+ double a; /* angle courant */
+ double step = M_PI_2 / (double)i_pi_2;
+
+ quad = 0;
+ coslut[quad] = 1.0;
+ sinlut[quad] = 0.0; /* 0 */
+ quad += i_pi_2;
+ coslut[quad] = 0.0;
+ sinlut[quad] = 1.0; /* PI/2 */
+ quad += i_pi_2;
+ coslut[quad] = -1.0;
+ sinlut[quad] = 0.0; /* PI */
+ quad += i_pi_2;
+ coslut[quad] = 0.0;
+ sinlut[quad] = -1.0; /* 3PI/2*/
+
+ for (i = 1, a = step; i < i_pi_2; i++, a += step) {
+ float ca = (float)cos(a);
+ quad = 0;
+ coslut[quad + i] = ca; /* cos(a) */
+ quad += i_pi_2;
+ sinlut[quad - i] = ca; /* sin(PI/2-a) */
+ sinlut[quad + i] = ca; /* sin(PI/2+a) */
+ quad += i_pi_2;
+ coslut[quad - i] = -ca; /* cos(PI-a) */
+ coslut[quad + i] = -ca; /* cos(PI+a) */
+ quad += i_pi_2;
+ sinlut[quad - i] = -ca; /* sin(3PI/2-a) */
+ sinlut[quad + i] = -ca; /* sin(3PI/2+a) */
+ quad += i_pi_2;
+ coslut[quad - i] = ca; /* cos(2PI-a) */
+ }
}
/*
@@ -363,21 +345,19 @@ cosin_to_lut (Index level, float *coslut, float *sinlut)
* Sortie :
* La norme du vecteur.
*/
-float
-norm_vector (Vector *vp)
+float norm_vector(Vector *vp)
{
- float norm; /* norme du vecteur */
-
- if ((norm = (float) sqrt ((double) DOT_PRODUCT(*vp,*vp))) > M_EPSILON) {
- vp->x /= norm;
- vp->y /= norm;
- vp->z /= norm;
- }
- else {
- static char proc_name[] = "norm_vector";
- fprintf (stderr, "%s: nul vector\n", proc_name);
+ float norm; /* norme du vecteur */
+
+ if ((norm = (float)sqrt((double)DOT_PRODUCT(*vp, *vp))) > M_EPSILON) {
+ vp->x /= norm;
+ vp->y /= norm;
+ vp->z /= norm;
+ } else {
+ static char proc_name[] = "norm_vector";
+ fprintf(stderr, "%s: nul vector\n", proc_name);
}
- return (norm);
+ return (norm);
}
/*
@@ -387,19 +367,18 @@ norm_vector (Vector *vp)
* np Le vecteur norme orthogonal au plan.
* ap, bp, cp Points formant un repere du plan.
*/
-void
-plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
+void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
{
- Vector u, v;
+ Vector u, v;
- DIF_COORD3(u,*bp,*ap); /* base orthonorme (ap, u, v) */
- DIF_COORD3(v,*cp,*ap);
- norm_vector (&u);
- norm_vector (&v);
- CROSS_PRODUCT (*np,u,v);
+ DIF_COORD3(u, *bp, *ap); /* base orthonorme (ap, u, v) */
+ DIF_COORD3(v, *cp, *ap);
+ norm_vector(&u);
+ norm_vector(&v);
+ CROSS_PRODUCT(*np, u, v);
}
-/*
+/*
* La procedure "point_matrix" deplace un point 3D dans un espace 4D.
* Une matrice homogene 4x4 effectue le changement de repere.
* Entree :
@@ -407,69 +386,60 @@ plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp)
* p3 Point a deplacer.
* m Matrice de changement de repere.
*/
-void
-point_matrix (Point4f *p4, Point3f *p3, Matrix m)
+void point_matrix(Point4f *p4, Point3f *p3, Matrix m)
{
- float x = p3->x, y = p3->y, z = p3->z;
+ float x = p3->x, y = p3->y, z = p3->z;
- p4->x = COORD3_COL(x,y,z,m,0);
- p4->y = COORD3_COL(x,y,z,m,1);
- p4->z = COORD3_COL(x,y,z,m,2);
- p4->w = COORD3_COL(x,y,z,m,3);
+ p4->x = COORD3_COL(x, y, z, m, 0);
+ p4->y = COORD3_COL(x, y, z, m, 1);
+ p4->z = COORD3_COL(x, y, z, m, 2);
+ p4->w = COORD3_COL(x, y, z, m, 3);
}
/*
- * La procedure "point_3D_3D" deplace un tableau de points 3D dans un espace 3D.
- * Une matrice 4x3 effectue le changement de repere.
- * La quatrieme colonne de la matrice vaut [0, 0, 0, 1] et n'est pas utilisee.
- * Entree :
- * ip Tableau de points 3D a deplacer.
- * size Taille du tableau "ip".
- * m Matrice de changement de repere.
- * Entree/Sortie :
- * op Tableau de points 3D resultat.
+ * La procedure "point_3D_3D" deplace un tableau de points 3D dans un espace
+ * 3D. Une matrice 4x3 effectue le changement de repere. La quatrieme colonne
+ * de la matrice vaut [0, 0, 0, 1] et n'est pas utilisee. Entree : ip
+ * Tableau de points 3D a deplacer. size Taille du tableau
+ * "ip". m Matrice de changement de repere. Entree/Sortie : op
+ * Tableau de points 3D resultat.
*/
-void
-point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op)
+void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op)
{
- Point3f *pend = ip + size; /* borne de ip */
+ Point3f *pend = ip + size; /* borne de ip */
- for (; ip < pend; ip++, op++) {
+ for (; ip < pend; ip++, op++) {
float x = ip->x;
float y = ip->y;
float z = ip->z;
- op->x = COORD3_COL(x,y,z,m,0);
- op->y = COORD3_COL(x,y,z,m,1);
- op->z = COORD3_COL(x,y,z,m,2);
- }
+ op->x = COORD3_COL(x, y, z, m, 0);
+ op->y = COORD3_COL(x, y, z, m, 1);
+ op->z = COORD3_COL(x, y, z, m, 2);
+ }
}
/*
- * La procedure "point_3D_4D" deplace un tableau de points 3D dans un espace 4D.
- * Une matrice homogene 4x4 effectue le changement de repere.
- * Entree :
- * p3 Tableau de points 3D a deplacer.
- * size Taille du tableau "p3".
- * m Matrice de changement de repere.
- * Entree/Sortie :
- * p4 Tableau de points 4D resultat.
+ * La procedure "point_3D_4D" deplace un tableau de points 3D dans un espace
+ * 4D. Une matrice homogene 4x4 effectue le changement de repere. Entree : p3
+ * Tableau de points 3D a deplacer. size Taille du tableau
+ * "p3". m Matrice de changement de repere. Entree/Sortie : p4
+ * Tableau de points 4D resultat.
*/
-void
-point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4)
+void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4)
{
- Point3f *pend = p3 + size; /* borne de p3 */
+ Point3f *pend = p3 + size; /* borne de p3 */
- for (; p3 < pend; p3++, p4++) {
+ for (; p3 < pend; p3++, p4++) {
float x = p3->x;
float y = p3->y;
float z = p3->z;
- p4->x = COORD3_COL(x,y,z,m,0);
- p4->y = COORD3_COL(x,y,z,m,1);
- p4->z = COORD3_COL(x,y,z,m,2);
- p4->w = COORD3_COL(x,y,z,m,3);
- }
+ p4->x = COORD3_COL(x, y, z, m, 0);
+ p4->y = COORD3_COL(x, y, z, m, 1);
+ p4->z = COORD3_COL(x, y, z, m, 2);
+ p4->w = COORD3_COL(x, y, z, m, 3);
+ }
}
/*
@@ -480,44 +450,40 @@ point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4)
* a Angle de rotation en degres.
* axis Vecteur directeur de l'axe de rotation.
*/
-void
-rotate_vector (Vector *vp, float a, Vector *axis)
+void rotate_vector(Vector *vp, float a, Vector *axis)
{
- Vector n, u, v, cross;
- float f;
-
- a *= (float)M_PI / (float)180.0; /* passage en radians */
-
- n = *axis; /* norme le vecteur directeur */
- norm_vector (&n);
-
- /*
- * Avant rotation, vp vaut :
- * u + v
- * Apres rotation, vp vaut :
- * u + cos(a) * v + sin(a) * (n^vp)
- * = u + cos(a) * v + sin(a) * (n^v)
- * avec u = (vp.n) * n, v = vp-u;
- * ou "u" est la projection de "vp" sur l'axe "axis",
- * et "v" est la composante de "vp" perpendiculaire a "axis".
- */
- f = DOT_PRODUCT(*vp, n);
- u = n;
- MUL_COORD3(u,f,f,f); /* (vp.n) * n */
-
- DIF_COORD3(v,*vp,u); /* calcule "v" */
-
- f = (float) cos ((double) a);
- MUL_COORD3(v,f,f,f); /* v * cos(a) */
-
- CROSS_PRODUCT(cross,n,*vp);
- f = (float) sin ((double) a);
- MUL_COORD3(cross,f,f,f); /* (n^v) * sin(a) */
-
- SET_COORD3(*vp,
- u.x + v.x + cross.x,
- u.y + v.y + cross.y,
- u.z + v.z + cross.z);
+ Vector n, u, v, cross;
+ float f;
+
+ a *= (float)M_PI / (float)180.0; /* passage en radians */
+
+ n = *axis; /* norme le vecteur directeur */
+ norm_vector(&n);
+
+ /*
+ * Avant rotation, vp vaut :
+ * u + v
+ * Apres rotation, vp vaut :
+ * u + cos(a) * v + sin(a) * (n^vp)
+ * = u + cos(a) * v + sin(a) * (n^v)
+ * avec u = (vp.n) * n, v = vp-u;
+ * ou "u" est la projection de "vp" sur l'axe "axis",
+ * et "v" est la composante de "vp" perpendiculaire a "axis".
+ */
+ f = DOT_PRODUCT(*vp, n);
+ u = n;
+ MUL_COORD3(u, f, f, f); /* (vp.n) * n */
+
+ DIF_COORD3(v, *vp, u); /* calcule "v" */
+
+ f = (float)cos((double)a);
+ MUL_COORD3(v, f, f, f); /* v * cos(a) */
+
+ CROSS_PRODUCT(cross, n, *vp);
+ f = (float)sin((double)a);
+ MUL_COORD3(cross, f, f, f); /* (n^v) * sin(a) */
+
+ SET_COORD3(*vp, u.x + v.x + cross.x, u.y + v.y + cross.y, u.z + v.z + cross.z);
}
/*
@@ -528,27 +494,23 @@ rotate_vector (Vector *vp, float a, Vector *axis)
* Entree/Sortie :
* up Vecteur perpendiculaire a vp.
*/
-void
-upright_vector (Vector *vp, Vector *up)
+void upright_vector(Vector *vp, Vector *up)
{
- if (FABS(vp->z) > M_EPSILON) { /* x et y sont fixes */
- up->z = - (vp->x + vp->y) / vp->z;
- up->x = up->y = 1.0;
- }
- else if (FABS(vp->y) > M_EPSILON) { /* x et z sont fixes */
- up->y = - (vp->x + vp->z) / vp->y;
- up->x = up->z = 1.0;
- }
- else if (FABS(vp->x) > M_EPSILON) { /* y et z sont fixes */
- up->x = - (vp->y + vp->z) / vp->x;
- up->y = up->z = 1.0;
- }
- else {
- static char proc_name[] = "upright_vector";
+ if (FABS(vp->z) > M_EPSILON) { /* x et y sont fixes */
+ up->z = -(vp->x + vp->y) / vp->z;
+ up->x = up->y = 1.0;
+ } else if (FABS(vp->y) > M_EPSILON) { /* x et z sont fixes */
+ up->y = -(vp->x + vp->z) / vp->y;
+ up->x = up->z = 1.0;
+ } else if (FABS(vp->x) > M_EPSILON) { /* y et z sont fixes */
+ up->x = -(vp->y + vp->z) / vp->x;
+ up->y = up->z = 1.0;
+ } else {
+ static char proc_name[] = "upright_vector";
up->x = up->y = up->z = 0.0;
- fprintf (stderr, "%s: nul vector\n", proc_name);
- return;
- }
+ fprintf(stderr, "%s: nul vector\n", proc_name);
+ return;
+ }
}
/*
@@ -559,12 +521,11 @@ upright_vector (Vector *vp, Vector *up)
* m Matrice de rotation et de translation.
* pp Position a initialiser.
*/
-void
-Matrix_to_Position (Matrix m, AritPosition *pp)
+void Matrix_to_Position(Matrix m, AritPosition *pp)
{
- Matrix_to_Rotate (m, &pp->rotate);
- SET_COORD3(pp->scale,1.0,1.0,1.0);
- SET_COORD3(pp->translate,m[3][0],m[3][1],m[3][2]);
+ Matrix_to_Rotate(m, &pp->rotate);
+ SET_COORD3(pp->scale, 1.0, 1.0, 1.0);
+ SET_COORD3(pp->translate, m[3][0], m[3][1], m[3][2]);
}
/*
@@ -593,37 +554,29 @@ Matrix_to_Position (Matrix m, AritPosition *pp)
* m Matrice contenant la composition des rotations.
* vp Rotations par rapport aux axes d'un repere droit en degres.
*/
-void
-Matrix_to_Rotate (Matrix m, Vector *vp)
+void Matrix_to_Rotate(Matrix m, Vector *vp)
{
- float sy = - m[0][2];
- float cy = (float) sqrt (1.0 - (double) (sy * sy));
+ float sy = -m[0][2];
+ float cy = (float)sqrt(1.0 - (double)(sy * sy));
float cx, sx;
- if (FABS(cy) > M_EPSILON) {
+ if (FABS(cy) > M_EPSILON) {
float sz = m[0][1] / cy;
float cz = m[0][0] / cy;
sx = m[1][2] / cy;
cx = m[2][2] / cy;
- SET_COORD3(*vp,
- cosin_to_angle (cx, sx),
- cosin_to_angle (cy, sy),
- cosin_to_angle (cz, sz));
- }
- else { /* RZ = 0 => Ry = +/- 90 degres */
- sx = m[1][1];
- cx = - m[2][1];
-
- SET_COORD3(*vp,
- cosin_to_angle (cx, sx),
- (sy > (float)0.0) ? (float)M_PI_2 : (float)(- M_PI_2),
- (float)0.0);
- }
- vp->x *= (float)180.0 / (float)M_PI; /* passage en degres */
- vp->y *= (float)180.0 / (float)M_PI;
- vp->z *= (float)180.0 / (float)M_PI;
+ SET_COORD3(*vp, cosin_to_angle(cx, sx), cosin_to_angle(cy, sy), cosin_to_angle(cz, sz));
+ } else { /* RZ = 0 => Ry = +/- 90 degres */
+ sx = m[1][1];
+ cx = -m[2][1];
+
+ SET_COORD3(*vp, cosin_to_angle(cx, sx), (sy > (float)0.0) ? (float)M_PI_2 : (float)(-M_PI_2), (float)0.0);
+ }
+ vp->x *= (float)180.0 / (float)M_PI; /* passage en degres */
+ vp->y *= (float)180.0 / (float)M_PI;
+ vp->z *= (float)180.0 / (float)M_PI;
}
/*
@@ -633,14 +586,13 @@ Matrix_to_Rotate (Matrix m, Vector *vp)
* pp Position de reference.
* m Matrice a initialiser.
*/
-void
-Position_to_Matrix (AritPosition *pp, Matrix m)
+void Position_to_Matrix(AritPosition *pp, Matrix m)
{
- Rotate_to_Matrix (&pp->rotate, m); /* rotation */
- prescale_matrix (m, &pp->scale); /* homothetie */
- m[3][0] = pp->translate.x; /* translation */
- m[3][1] = pp->translate.y;
- m[3][2] = pp->translate.z;
+ Rotate_to_Matrix(&pp->rotate, m); /* rotation */
+ prescale_matrix(m, &pp->scale); /* homothetie */
+ m[3][0] = pp->translate.x; /* translation */
+ m[3][1] = pp->translate.y;
+ m[3][2] = pp->translate.z;
}
/*
@@ -663,34 +615,28 @@ Position_to_Matrix (AritPosition *pp, Matrix m)
* vp Rotations par rapport aux axes d'un repere droit en degres.
* m Matrice a initialiser.
*/
-void
-Rotate_to_Matrix (Vector *vp, 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);
-
- m[0][0] = cy * cz;
- m[1][0] = (sx * sy * cz) - (cx * sz);
- m[2][0] = (cx * sy * cz) + (sx * sz);
-
- m[0][1] = cy * sz;
- m[1][1] = (sx * sy * sz) + (cx * cz);
- m[2][1] = (cx * sy * sz) - (sx * cz);
-
- m[0][2] = - sy;
- m[1][2] = sx * cy;
- m[2][2] = cx * cy;
-
- m[0][3] = m[1][3] = m[2][3] = 0.0;
- m[3][0] = m[3][1] = m[3][2] = 0.0;
- m[3][3] = 1.0;
+ float rx = vp->x * (float)M_PI / (float)180.0, /* passage en radians */
+ ry = vp->y * (float)M_PI / (float)180.0, rz = vp->z * (float)M_PI / (float)180.0;
+ float cx = (float)cos((double)rx), sx = (float)sin((double)rx), cy = (float)cos((double)ry),
+ sy = (float)sin((double)ry), cz = (float)cos((double)rz), sz = (float)sin((double)rz);
+
+ m[0][0] = cy * cz;
+ m[1][0] = (sx * sy * cz) - (cx * sz);
+ m[2][0] = (cx * sy * cz) + (sx * sz);
+
+ m[0][1] = cy * sz;
+ m[1][1] = (sx * sy * sz) + (cx * cz);
+ m[2][1] = (cx * sy * sz) - (sx * cz);
+
+ m[0][2] = -sy;
+ m[1][2] = sx * cy;
+ m[2][2] = cx * cy;
+
+ m[0][3] = m[1][3] = m[2][3] = 0.0;
+ m[3][0] = m[3][1] = m[3][2] = 0.0;
+ m[3][3] = 1.0;
}
/*
@@ -716,43 +662,42 @@ Rotate_to_Matrix (Vector *vp, Matrix m)
* axis Vecteur directeur de l'axe de la rotation.
* m Matrice a initialiser.
*/
-void
-Rotaxis_to_Matrix (float a, Vector *axis, Matrix m)
+void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m)
{
- float cosa;
- float sina;
- float vera; /* 1 - cosa */
- Vector n; /* vecteur norme*/
- Vector conv; /* verO n */
- Vector tilde; /* sinO n */
-
- a *= (float)M_PI / (float)180.0; /* passage en radians */
-
- cosa = (float) cos ((double) a);
- sina = (float) sin ((double) a);
- vera = (float)1.0 - cosa;
-
- n = *axis; /* norme le vecteur directeur */
- norm_vector (&n);
- tilde = conv = n;
- MUL_COORD3(conv,vera,vera,vera);
- MUL_COORD3(tilde,sina,sina,sina);
-
- m[0][0] = conv.x * n.x + cosa;
- m[0][1] = conv.x * n.y + tilde.z;
- m[0][2] = conv.x * n.z - tilde.y;
-
- m[1][0] = conv.y * n.x - tilde.z;
- m[1][1] = conv.y * n.y + cosa;
- m[1][2] = conv.y * n.z + tilde.x;
-
- m[2][0] = conv.z * n.x + tilde.y;
- m[2][1] = conv.z * n.y - tilde.x;
- m[2][2] = conv.z * n.z + cosa;
-
- m[0][3] = m[2][3] = m[1][3] = 0.0;
- m[3][0] = m[3][1] = m[3][2] = 0.0;
- m[3][3] = 1.0;
+ float cosa;
+ float sina;
+ float vera; /* 1 - cosa */
+ Vector n; /* vecteur norme*/
+ Vector conv; /* verO n */
+ Vector tilde; /* sinO n */
+
+ a *= (float)M_PI / (float)180.0; /* passage en radians */
+
+ cosa = (float)cos((double)a);
+ sina = (float)sin((double)a);
+ vera = (float)1.0 - cosa;
+
+ n = *axis; /* norme le vecteur directeur */
+ norm_vector(&n);
+ tilde = conv = n;
+ MUL_COORD3(conv, vera, vera, vera);
+ MUL_COORD3(tilde, sina, sina, sina);
+
+ m[0][0] = conv.x * n.x + cosa;
+ m[0][1] = conv.x * n.y + tilde.z;
+ m[0][2] = conv.x * n.z - tilde.y;
+
+ m[1][0] = conv.y * n.x - tilde.z;
+ m[1][1] = conv.y * n.y + cosa;
+ m[1][2] = conv.y * n.z + tilde.x;
+
+ m[2][0] = conv.z * n.x + tilde.y;
+ m[2][1] = conv.z * n.y - tilde.x;
+ m[2][2] = conv.z * n.z + cosa;
+
+ m[0][3] = m[2][3] = m[1][3] = 0.0;
+ m[3][0] = m[3][1] = m[3][2] = 0.0;
+ m[3][3] = 1.0;
}
/*
@@ -763,13 +708,12 @@ Rotaxis_to_Matrix (float a, Vector *axis, Matrix m)
* tp Vecteur des coordonnees de translation.
* m Matrice a initialiser.
*/
-void
-Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m)
+void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m)
{
- Rotate_to_Matrix (rp, m); /* matrice de rotation */
- m[3][0] = tp->x; /* matrice de translation */
- m[3][1] = tp->y;
- m[3][2] = tp->z;
+ Rotate_to_Matrix(rp, m); /* matrice de rotation */
+ m[3][0] = tp->x; /* matrice de translation */
+ m[3][1] = tp->y;
+ m[3][2] = tp->z;
}
/*
@@ -778,28 +722,25 @@ Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m)
* vp Vecteur des coordonnees d'homothetie.
* m Matrice a initialiser.
*/
-void
-Scale_to_Matrix (Vector *vp, Matrix m)
+void Scale_to_Matrix(Vector *vp, Matrix m)
{
- ident_matrix (m);
- m[0][0] = vp->x;
- m[1][1] = vp->y;
- m[2][2] = vp->z;
+ ident_matrix(m);
+ m[0][0] = vp->x;
+ m[1][1] = vp->y;
+ m[2][2] = vp->z;
}
/*
- * La procedure "Translate_to_Matrix" initialise la matrice par la translation.
- * Entree :
- * vp Vecteur des coordonnees de translation.
- * m Matrice a initialiser.
+ * La procedure "Translate_to_Matrix" initialise la matrice par la
+ * translation. Entree : vp Vecteur des coordonnees de
+ * translation. m Matrice a initialiser.
*/
-void
-Translate_to_Matrix (Vector *vp, Matrix m)
+void Translate_to_Matrix(Vector *vp, Matrix m)
{
- ident_matrix (m);
- m[3][0] = vp->x;
- m[3][1] = vp->y;
- m[3][2] = vp->z;
+ ident_matrix(m);
+ m[3][0] = vp->x;
+ m[3][1] = vp->y;
+ m[3][2] = vp->z;
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpArit.h b/modules/robot/src/wireframe-simulator/vpArit.h
index d07d8ff..71c5df0 100644
--- a/modules/robot/src/wireframe-simulator/vpArit.h
+++ b/modules/robot/src/wireframe-simulator/vpArit.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,106 +38,146 @@
#ifndef vpArit_h
#define vpArit_h
+#include <stdio.h>
#include <visp3/core/vpConfig.h>
#include <visp3/robot/vpWireFrameSimulatorTypes.h>
-#include <stdio.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#define ADD_COORD2(r,a,b) { (r).x = (a).x + (b).x;\
- (r).y = (a).y + (b).y; }
-
-#define ADD_COORD3(r,a,b) { (r).x = (a).x + (b).x;\
- (r).y = (a).y + (b).y;\
- (r).z = (a).z + (b).z; }
-
-#define INC_COORD2(r,a) { (r).x += (a).x; (r).y += (a).y; }
-
-#define INC_COORD3(r,a) { (r).x += (a).x; (r).y += (a).y;\
- (r).z += (a).z; }
-
-#define CROSS_PRODUCT(r,a,b) { (r).x = (a).y * (b).z - (a).z * (b).y;\
- (r).y = (a).z * (b).x - (a).x * (b).z;\
- (r).z = (a).x * (b).y - (a).y * (b).x; }
-
-#define DIF_COORD2(r,a,b) { (r).x = (a).x - (b).x;\
- (r).y = (a).y - (b).y; }
-
-#define DIF_COORD3(r,a,b) { (r).x = (a).x - (b).x;\
- (r).y = (a).y - (b).y;\
- (r).z = (a).z - (b).z; }
-
-#define DOT_PRODUCT(a,b) ( ((a).x * (b).x) +\
- ((a).y * (b).y) +\
- ((a).z * (b).z) )
-
-#define LENGTH3(a) (sqrt((double) DOT_PRODUCT((a),(a))))
-
-#define MID_COORD3(r,a,b) { (r).x = ((a).x + (b).x) / 2.0;\
- (r).y = ((a).y + (b).y) / 2.0;\
- (r).z = ((a).z + (b).z) / 2.0; }
-
-#define MUL_COORD3(r,a,b,c) { (r).x *= (a); (r).y *= (b); (r).z *= (c); }
-
-#define PAR_COORD3(r,t,a,b) { (r).x = ((b).x - (a).x) * (t) + (a).x;\
- (r).y = ((b).y - (a).y) * (t) + (a).y;\
- (r).z = ((b).z - (a).z) * (t) + (a).z; }
-
-#define SET_COORD2(r,a,b) { (r).x = (a); (r).y = (b); }
-
-#define SET_COORD3(r,a,b,c) { (r).x = (a); (r).y = (b); (r).z = (c); }
-
-#define SUB_COORD2(r,a) { (r).x -= (a).x; (r).y -= (a).y; }
-
-#define SUB_COORD3(r,a) { (r).x -= (a).x; (r).y -= (a).y;\
- (r).z -= (a).z; }
-
-#define COORD3_COL(x,y,z,m,i) ( ((x) * (m)[0][i]) +\
- ((y) * (m)[1][i]) +\
- ((z) * (m)[2][i]) +\
- (m)[3][i] )
-
-#define COORD4_COL(x,y,z,w,m,i) ( ((x) * (m)[0][i]) +\
- ((y) * (m)[1][i]) +\
- ((z) * (m)[2][i]) +\
- ((w) * (m)[3][i]) )
-
-#define M_POLY1(x,a,b) ((a) * (x) + (b))
-#define M_POLY2(x,a,b,c) (M_POLY1((x),(a),(b)) * (x) + (c))
-#define M_POLY3(x,a,b,c,d) (M_POLY2((x),(a),(b),(c)) * (x) + (d))
-
-
-typedef struct {
- int x, y;
+#define ADD_COORD2(r, a, b) \
+ { \
+ (r).x = (a).x + (b).x; \
+ (r).y = (a).y + (b).y; \
+ }
+
+#define ADD_COORD3(r, a, b) \
+ { \
+ (r).x = (a).x + (b).x; \
+ (r).y = (a).y + (b).y; \
+ (r).z = (a).z + (b).z; \
+ }
+
+#define INC_COORD2(r, a) \
+ { \
+ (r).x += (a).x; \
+ (r).y += (a).y; \
+ }
+
+#define INC_COORD3(r, a) \
+ { \
+ (r).x += (a).x; \
+ (r).y += (a).y; \
+ (r).z += (a).z; \
+ }
+
+#define CROSS_PRODUCT(r, a, b) \
+ { \
+ (r).x = (a).y * (b).z - (a).z * (b).y; \
+ (r).y = (a).z * (b).x - (a).x * (b).z; \
+ (r).z = (a).x * (b).y - (a).y * (b).x; \
+ }
+
+#define DIF_COORD2(r, a, b) \
+ { \
+ (r).x = (a).x - (b).x; \
+ (r).y = (a).y - (b).y; \
+ }
+
+#define DIF_COORD3(r, a, b) \
+ { \
+ (r).x = (a).x - (b).x; \
+ (r).y = (a).y - (b).y; \
+ (r).z = (a).z - (b).z; \
+ }
+
+#define DOT_PRODUCT(a, b) (((a).x * (b).x) + ((a).y * (b).y) + ((a).z * (b).z))
+
+#define LENGTH3(a) (sqrt((double)DOT_PRODUCT((a), (a))))
+
+#define MID_COORD3(r, a, b) \
+ { \
+ (r).x = ((a).x + (b).x) / 2.0; \
+ (r).y = ((a).y + (b).y) / 2.0; \
+ (r).z = ((a).z + (b).z) / 2.0; \
+ }
+
+#define MUL_COORD3(r, a, b, c) \
+ { \
+ (r).x *= (a); \
+ (r).y *= (b); \
+ (r).z *= (c); \
+ }
+
+#define PAR_COORD3(r, t, a, b) \
+ { \
+ (r).x = ((b).x - (a).x) * (t) + (a).x; \
+ (r).y = ((b).y - (a).y) * (t) + (a).y; \
+ (r).z = ((b).z - (a).z) * (t) + (a).z; \
+ }
+
+#define SET_COORD2(r, a, b) \
+ { \
+ (r).x = (a); \
+ (r).y = (b); \
+ }
+
+#define SET_COORD3(r, a, b, c) \
+ { \
+ (r).x = (a); \
+ (r).y = (b); \
+ (r).z = (c); \
+ }
+
+#define SUB_COORD2(r, a) \
+ { \
+ (r).x -= (a).x; \
+ (r).y -= (a).y; \
+ }
+
+#define SUB_COORD3(r, a) \
+ { \
+ (r).x -= (a).x; \
+ (r).y -= (a).y; \
+ (r).z -= (a).z; \
+ }
+
+#define COORD3_COL(x, y, z, m, i) (((x) * (m)[0][i]) + ((y) * (m)[1][i]) + ((z) * (m)[2][i]) + (m)[3][i])
+
+#define COORD4_COL(x, y, z, w, m, i) (((x) * (m)[0][i]) + ((y) * (m)[1][i]) + ((z) * (m)[2][i]) + ((w) * (m)[3][i]))
+
+#define M_POLY1(x, a, b) ((a) * (x) + (b))
+#define M_POLY2(x, a, b, c) (M_POLY1((x), (a), (b)) * (x) + (c))
+#define M_POLY3(x, a, b, c, d) (M_POLY2((x), (a), (b), (c)) * (x) + (d))
+
+typedef struct {
+ int x, y;
} Point2i;
-typedef struct {
- short x, y;
+typedef struct {
+ short x, y;
} Point2s;
-typedef struct {
- int x, y, z;
+typedef struct {
+ int x, y, z;
} Point3i;
-typedef struct {
- float x,y,z,w;
+typedef struct {
+ float x, y, z, w;
} Point4f;
-typedef struct {
- float x,y,z;
+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 } \
+ }
/*
* POSITION
* ________
- *
+ *
* La structure "Position" definit le positionnement d'un objet.
* Matrice de positionnement = R.S.T
* avec R = Rx.Ry.Rz Matrice de rotation autour des axes (Ox,Oy,Oz),
@@ -144,53 +185,63 @@ typedef struct {
* S = Sx.Sy.Sz Matrice d'homothetie sur les axes;
* T = Tx.Ty.Tz Matrice de translation sur les axes.
*/
-typedef struct {
- Vector rotate; /* vecteur rotation */
- Vector scale; /* vecteur homothetie */
- Vector translate; /* vecteur translation */
+typedef struct {
+ Vector rotate; /* vecteur rotation */
+ Vector scale; /* vecteur homothetie */
+ Vector translate; /* vecteur translation */
} AritPosition;
-#define IDENTITY_ROTATE { 0.0, 0.0, 0.0 }
-#define IDENTITY_SCALE { 1.0, 1.0, 1.0 }
-#define IDENTITY_TRANSLATE { 0.0, 0.0, 0.0 }
-
-#define IDENTITY_POSITION { IDENTITY_ROTATE,\
- IDENTITY_SCALE,\
- IDENTITY_TRANSLATE }
-
-void fprintf_matrix (FILE *fp, Matrix m);
-void ident_matrix (Matrix m);
-void premult_matrix (Matrix a, Matrix b);
-void premult3_matrix (Matrix a, Matrix b);
-void prescale_matrix (Matrix m, Vector *vp);
-void pretrans_matrix (Matrix m, Vector *vp);
-void postleft_matrix (Matrix m, char axis);
-void postmult_matrix (Matrix a, Matrix b);
-void postmult3_matrix (Matrix a, Matrix b);
-void postscale_matrix (Matrix m, Vector *vp);
-void posttrans_matrix (Matrix m, Vector *vp);
-void transpose_matrix (Matrix m);
-
-float cosin_to_angle (float ca, float sa);
-float norm_vector (Vector *vp);
-void point_matrix (Point4f *p4, Point3f *p3, Matrix m);
-void plane_norme (Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
-void point_3D_3D (Point3f *ip, int size, Matrix m, Point3f *op);
-void point_3D_4D (Point3f *p3, int size, Matrix m, Point4f *p4);
-void rotate_vector (Vector *vp, float a, Vector *axis);
-void upright_vector (Vector *vp, Vector *up);
-
-void Matrix_to_Position (Matrix m, AritPosition *pp);
-void Matrix_to_Rotate (Matrix m, Vector *vp);
-void Position_to_Matrix (AritPosition *pp, Matrix m);
-void Rotate_to_Matrix (Vector *vp, Matrix m);
-void Rotaxis_to_Matrix (float a, Vector *axis, Matrix m);
-void Rotrans_to_Matrix (Vector *rp, Vector *tp, Matrix m);
-void Scale_to_Matrix (Vector *vp, Matrix m);
-void Translate_to_Matrix (Vector *vp, Matrix m);
-
-void fscanf_Point3f (Point3f *pp);
-void fscanf_Vector (Vector *vp);
+#define IDENTITY_ROTATE \
+ { \
+ 0.0, 0.0, 0.0 \
+ }
+#define IDENTITY_SCALE \
+ { \
+ 1.0, 1.0, 1.0 \
+ }
+#define IDENTITY_TRANSLATE \
+ { \
+ 0.0, 0.0, 0.0 \
+ }
+
+#define IDENTITY_POSITION \
+ { \
+ IDENTITY_ROTATE, IDENTITY_SCALE, IDENTITY_TRANSLATE \
+ }
+
+void fprintf_matrix(FILE *fp, Matrix m);
+void ident_matrix(Matrix m);
+void premult_matrix(Matrix a, Matrix b);
+void premult3_matrix(Matrix a, Matrix b);
+void prescale_matrix(Matrix m, Vector *vp);
+void pretrans_matrix(Matrix m, Vector *vp);
+void postleft_matrix(Matrix m, char axis);
+void postmult_matrix(Matrix a, Matrix b);
+void postmult3_matrix(Matrix a, Matrix b);
+void postscale_matrix(Matrix m, Vector *vp);
+void posttrans_matrix(Matrix m, Vector *vp);
+void transpose_matrix(Matrix m);
+
+float cosin_to_angle(float ca, float sa);
+float norm_vector(Vector *vp);
+void point_matrix(Point4f *p4, Point3f *p3, Matrix m);
+void plane_norme(Vector *np, Point3f *ap, Point3f *bp, Point3f *cp);
+void point_3D_3D(Point3f *ip, int size, Matrix m, Point3f *op);
+void point_3D_4D(Point3f *p3, int size, Matrix m, Point4f *p4);
+void rotate_vector(Vector *vp, float a, Vector *axis);
+void upright_vector(Vector *vp, Vector *up);
+
+void Matrix_to_Position(Matrix m, AritPosition *pp);
+void Matrix_to_Rotate(Matrix m, Vector *vp);
+void Position_to_Matrix(AritPosition *pp, Matrix m);
+void Rotate_to_Matrix(Vector *vp, Matrix m);
+void Rotaxis_to_Matrix(float a, Vector *axis, Matrix m);
+void Rotrans_to_Matrix(Vector *rp, Vector *tp, Matrix m);
+void Scale_to_Matrix(Vector *vp, Matrix m);
+void Translate_to_Matrix(Vector *vp, Matrix m);
+
+void fscanf_Point3f(Point3f *pp);
+void fscanf_Vector(Vector *vp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpAritio.cpp b/modules/robot/src/wireframe-simulator/vpAritio.cpp
index ddbed66..67cd5ce 100644
--- a/modules/robot/src/wireframe-simulator/vpAritio.cpp
+++ b/modules/robot/src/wireframe-simulator/vpAritio.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,29 +40,25 @@
*
*****************************************************************************/
-
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
-#include "vpToken.h"
#include "vpLex.h"
+#include "vpMy.h"
+#include "vpToken.h"
-#include <stdio.h>
+#include <stdio.h>
/*
* La procedure "fprintf_Position" ecrit en ascii un positionnement.
* Entree :
* f Fichier en sortie.
* pp Positionnement a ecrite.
*/
-void fprintf_Position (FILE *f, AritPosition *pp)
+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,31 +66,28 @@ void fprintf_Position (FILE *f, AritPosition *pp)
* Entree :
* pp Point flottant 3D a lire.
*/
-void fscanf_Point3f (Point3f *pp)
+void fscanf_Point3f(Point3f *pp)
{
-static const char *err_tbl[] = {
-"float expected (coordinate ",
-" of point)"
-};
- int t;
+ static const char *err_tbl[] = {"float expected (coordinate ", " of point)"};
+ int t;
- /* Lecture de la premiere coordonnee du point. */
+ /* Lecture de la premiere coordonnee du point. */
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "X", err_tbl[1], NULL);
- pp->x = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "X", err_tbl[1], NULL);
+ pp->x = (t == T_INT) ? (float)myint : myfloat;
- /* Lecture de la seconde coordonnee du point. */
+ /* Lecture de la seconde coordonnee du point. */
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Y", err_tbl[1], NULL);
- pp->y = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Y", err_tbl[1], NULL);
+ pp->y = (t == T_INT) ? (float)myint : myfloat;
- /* Lecture de la troisieme coordonnee du point. */
+ /* Lecture de la troisieme coordonnee du point. */
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
- pp->z = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Z", err_tbl[1], NULL);
+ pp->z = (t == T_INT) ? (float)myint : myfloat;
}
/*
@@ -101,32 +95,29 @@ static const char *err_tbl[] = {
* Entree :
* vp Vecteur a lire.
*/
-void fscanf_Vector (Vector *vp)
+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;
+ int t;
- /* Lecture de la premiere coordonnee du vecteur. */
+ /* Lecture de la premiere coordonnee du vecteur. */
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "X", err_tbl[1], NULL);
- vp->x = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "X", err_tbl[1], NULL);
+ vp->x = (t == T_INT) ? (float)myint : myfloat;
- /* Lecture de la seconde coordonnee du vecteur. */
+ /* Lecture de la seconde coordonnee du vecteur. */
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Y", err_tbl[1], NULL);
- vp->y = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Y", err_tbl[1], NULL);
+ vp->y = (t == T_INT) ? (float)myint : myfloat;
- /* Lecture de la troisieme coordonnee du vecteur. */
+ /* Lecture de la troisieme coordonnee du vecteur. */
- if ((t= lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start",err_tbl[0], "Z", err_tbl[1], NULL);
- vp->z = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Z", err_tbl[1], NULL);
+ vp->z = (t == T_INT) ? (float)myint : myfloat;
}
/*
@@ -134,15 +125,15 @@ static const char *err_tbl[] = {
* Entree :
* pp Positionnement a lire.
*/
-void fscanf_Position (AritPosition *pp)
+void fscanf_Position(AritPosition *pp)
{
- pusherr ("rotate: ");
- fscanf_Vector (&pp->rotate);
- popuperr ("scale: ");
- fscanf_Vector (&pp->scale);
- popuperr ("translate: ");
- fscanf_Vector (&pp->translate);
- poperr ();
+ pusherr("rotate: ");
+ fscanf_Vector(&pp->rotate);
+ popuperr("scale: ");
+ fscanf_Vector(&pp->scale);
+ popuperr("translate: ");
+ fscanf_Vector(&pp->translate);
+ poperr();
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpAritio.h b/modules/robot/src/wireframe-simulator/vpAritio.h
index a1b300f..4e87900 100644
--- a/modules/robot/src/wireframe-simulator/vpAritio.h
+++ b/modules/robot/src/wireframe-simulator/vpAritio.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,16 +45,15 @@
#include <visp3/core/vpConfig.h>
-#include <stdio.h>
+#include <stdio.h>
#include "vpArit.h"
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void fprintf_Position (FILE *f, AritPosition *pp);
-void fscanf_Point3f (Point3f *pp);
-void fscanf_Vector (Vector *vp);
-void fscanf_Position (AritPosition *pp);
+void fprintf_Position(FILE *f, AritPosition *pp);
+void fscanf_Point3f(Point3f *pp);
+void fscanf_Vector(Vector *vp);
+void fscanf_Position(AritPosition *pp);
#endif
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpBound.cpp b/modules/robot/src/wireframe-simulator/vpBound.cpp
index 6c7f42d..2c03f98 100644
--- a/modules/robot/src/wireframe-simulator/vpBound.cpp
+++ b/modules/robot/src/wireframe-simulator/vpBound.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Le fichier "bound.c" contient les procedures de gestion des scenes de modele geometrique surfacique.
+ * Le fichier "bound.c" contient les procedures de gestion des scenes de
+ *modele geometrique surfacique.
*
* Authors:
* Jean-Luc CORRE
@@ -38,9 +40,9 @@
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
#include "vpBound.h"
+#include "vpMy.h"
#include <errno.h>
#include <math.h>
#include <stdio.h>
@@ -53,30 +55,30 @@
* Entree :
* bp Surface a liberer.
*/
-void free_Bound (Bound *bp)
+void free_Bound(Bound *bp)
{
- Face *fp = bp->face.ptr;
- Face *fend = fp + bp->face.nbr;
-
- for (; fp < fend; fp++) { /* libere les polygones */
- if (fp->vertex.ptr != fp->vertex.tbl)
- free ((char *) fp->vertex.ptr);
- }
- if (bp->face.ptr != NULL) { /* libere les faces */
- free ((char *) bp->face.ptr);
- bp->face.ptr = NULL;
- }
- if (bp->point.ptr != NULL) { /* libere les points */
- free ((char *) bp->point.ptr);
- bp->point.ptr = NULL;
- }
-#ifdef face_normal
- if (bp->normal.ptr != NULL) { /* libere les vecteurs */
- free ((char *) bp->normal.ptr);
- bp->normal.ptr = NULL;
- }
-#endif /* face_normal */
- bp->is_display = FALSE;
+ Face *fp = bp->face.ptr;
+ Face *fend = fp + bp->face.nbr;
+
+ for (; fp < fend; fp++) { /* libere les polygones */
+ if (fp->vertex.ptr != fp->vertex.tbl)
+ free((char *)fp->vertex.ptr);
+ }
+ if (bp->face.ptr != NULL) { /* libere les faces */
+ free((char *)bp->face.ptr);
+ bp->face.ptr = NULL;
+ }
+ if (bp->point.ptr != NULL) { /* libere les points */
+ free((char *)bp->point.ptr);
+ bp->point.ptr = NULL;
+ }
+#ifdef face_normal
+ if (bp->normal.ptr != NULL) { /* libere les vecteurs */
+ free((char *)bp->normal.ptr);
+ bp->normal.ptr = NULL;
+ }
+#endif /* face_normal */
+ bp->is_display = FALSE;
}
/*
@@ -85,10 +87,10 @@ void free_Bound (Bound *bp)
* Entree :
* bp Surface a liberer.
*/
-void free_huge_Bound (Bound *bp)
+void free_huge_Bound(Bound *bp)
{
- bp->face.nbr = 1; /* pour la liberation en une fois */
- free_Bound (bp);
+ bp->face.nbr = 1; /* pour la liberation en une fois */
+ free_Bound(bp);
}
/*
@@ -96,67 +98,64 @@ void free_huge_Bound (Bound *bp)
* Entree :
* bsp Scene a liberer.
*/
-void free_Bound_scene (Bound_scene *bsp)
+void free_Bound_scene(Bound_scene *bsp)
{
- Bound *bp = bsp->bound.ptr;
- Bound *bend = bp + bsp->bound.nbr;
-
- for (; bp < bend; bp++) { /* libere les surfaces */
- free_Bound (bp);
- }
- if (bsp->name != NULL) { /* libere le nom */
- free ((char *) bsp->name);
- bsp->name = NULL;
- }
- if (bsp->bound.ptr != NULL) { /* libere le tableau */
- free ((char *) bsp->bound.ptr);
- bsp->bound.ptr = NULL;
- }
+ Bound *bp = bsp->bound.ptr;
+ Bound *bend = bp + bsp->bound.nbr;
+
+ for (; bp < bend; bp++) { /* libere les surfaces */
+ free_Bound(bp);
+ }
+ if (bsp->name != NULL) { /* libere le nom */
+ free((char *)bsp->name);
+ bsp->name = NULL;
+ }
+ if (bsp->bound.ptr != NULL) { /* libere le tableau */
+ free((char *)bsp->bound.ptr);
+ bsp->bound.ptr = NULL;
+ }
}
/*
* La procedure "malloc_Bound" alloue une surface.
- * Les champs "bound.face.edge" ne sont pas utilises.
+ * Les champs "bound.face.edge" ne sont pas utilises.
* Entree :
* bp Surface a allouer.
* type Type de la surface.
* polygonal Booleen indiquant si la surface est polygonale.
* fn Nombre de faces de la surface.
- * pn Nombre de points de la surface.
+ * pn Nombre de points de la surface.
*/
-void malloc_Bound (Bound *bp, Type type, int polygonal, Index fn, Index pn)
+void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn)
{
- static char proc_name[] = "malloc_Bound";
-
- if ((bp->face.nbr = fn) == 0) /* faces */
- bp->face.ptr = NULL;
- else if ((bp->face.ptr = (Face *) malloc (fn * sizeof (Face)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
- if ((bp->point.nbr = pn) == 0) /* points */
- bp->point.ptr = NULL;
- else if ((bp->point.ptr = (Point3f *) malloc (pn * sizeof (Point3f)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-
-#ifdef face_normal
- /* normales aux sommets */
- if ((bp->normal.nbr = (bp->is_polygonal ? 0 : pn)) == 0)
- bp->normal.ptr = NULL;
- else if ((bp->normal.ptr = (Vector *) malloc (pn * sizeof (Vector)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
-#endif /* face_normal */
-
- bp->type = type;
- bp->is_display = TRUE;
+ static char proc_name[] = "malloc_Bound";
+
+ if ((bp->face.nbr = fn) == 0) /* faces */
+ bp->face.ptr = NULL;
+ else if ((bp->face.ptr = (Face *)malloc(fn * sizeof(Face))) == NULL) {
+ perror(proc_name);
+ exit(1);
+ }
+
+ if ((bp->point.nbr = pn) == 0) /* points */
+ bp->point.ptr = NULL;
+ else if ((bp->point.ptr = (Point3f *)malloc(pn * sizeof(Point3f))) == NULL) {
+ perror(proc_name);
+ exit(1);
+ }
+
+#ifdef face_normal
+ /* normales aux sommets */
+ if ((bp->normal.nbr = (bp->is_polygonal ? 0 : pn)) == 0)
+ bp->normal.ptr = NULL;
+ else if ((bp->normal.ptr = (Vector *)malloc(pn * sizeof(Vector))) == NULL) {
+ perror(proc_name);
+ exit(1);
+ }
+#endif /* face_normal */
+
+ bp->type = type;
+ bp->is_display = TRUE;
bp->is_polygonal = (unsigned)polygonal;
}
@@ -176,49 +175,46 @@ void malloc_Bound (Bound *bp, Type type, int polygonal, Index fn, Index pn)
* Entree :
* bp Surface maximale a allouer.
*/
-void malloc_huge_Bound (Bound *bp)
+void malloc_huge_Bound(Bound *bp)
{
-#ifdef face_normal
- malloc_Bound (bp, (Type) BND_NULL, FALSE, FACE_NBR, POINT_NBR);
+#ifdef face_normal
+ malloc_Bound(bp, (Type)BND_NULL, FALSE, FACE_NBR, POINT_NBR);
#else
- malloc_Bound (bp, (Type) BND_NULL, TRUE, FACE_NBR, POINT_NBR);
-#endif /* face_normal */
- if ((bp->face.ptr->vertex.ptr =
- (Index *) malloc (FACE_NBR * VERTEX_NBR * sizeof (Index))) == NULL) {
- static char proc_name[] = "malloc_Huge_Bound";
- perror (proc_name);
- exit (1);
- }
+ 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) {
+ static char proc_name[] = "malloc_Huge_Bound";
+ perror(proc_name);
+ exit(1);
+ }
}
/*
* La procedure "malloc_Bound_scene" alloue une scene de surfaces.
- * Stocke le nom de la scene et alloue l'espace memoire necessaire.
+ * Stocke le nom de la scene et alloue l'espace memoire necessaire.
* Les champs "bound.face.edge" ne sont pas utilises.
* Entree :
* bsp Scene a allouer.
* name Nom de la scene.
* bn Nombre de surfaces de la scene.
*/
-void malloc_Bound_scene (Bound_scene *bsp, const char *name, Index bn)
+void malloc_Bound_scene(Bound_scene *bsp, const char *name, Index bn)
{
- static char proc_name[] = "malloc_Bound_scene";
-
- if ((bsp->name = (char *) malloc ((strlen (name) + 1) * sizeof (char)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
- if ((bsp->bound.nbr = bn) == 0)
- bsp->bound.ptr = NULL;
- else if ((bsp->bound.ptr = (Bound *) malloc (bn * sizeof (Bound)))
- == NULL) {
- perror (proc_name);
- exit (1);
- }
- strcpy (bsp->name, name);
- bsp->bound.nbr = 0;
+ static char proc_name[] = "malloc_Bound_scene";
+
+ if ((bsp->name = (char *)malloc((strlen(name) + 1) * sizeof(char))) == NULL) {
+ perror(proc_name);
+ exit(1);
+ }
+ if ((bsp->bound.nbr = bn) == 0)
+ bsp->bound.ptr = NULL;
+ else if ((bsp->bound.ptr = (Bound *)malloc(bn * sizeof(Bound))) == NULL) {
+ perror(proc_name);
+ exit(1);
+ }
+ strcpy(bsp->name, name);
+ bsp->bound.nbr = 0;
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpBound.h b/modules/robot/src/wireframe-simulator/vpBound.h
index fc67995..bd75836 100644
--- a/modules/robot/src/wireframe-simulator/vpBound.h
+++ b/modules/robot/src/wireframe-simulator/vpBound.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,84 +39,83 @@
#ifndef vpBound_H
#define vpBound_H
-
+
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
+#include "vpMy.h"
#include <visp3/robot/vpWireFrameSimulatorTypes.h>
-#define START_ARG 0
-#define NEXT_ARG 1
-
-#define BND_NULL (-1)
+#define START_ARG 0
+#define NEXT_ARG 1
-#define BND_BLOCK 0
-#define BND_CARDIOIDE 1
-#define BND_CONE 2
-#define BND_CYLINDER 3
-#define BND_SPHERE 4
-#define BND_TORUS 5
-#define BND_WEDGE 6
+#define BND_NULL (-1)
-#define BND_F3 7
-#define BND_F4 8
+#define BND_BLOCK 0
+#define BND_CARDIOIDE 1
+#define BND_CONE 2
+#define BND_CYLINDER 3
+#define BND_SPHERE 4
+#define BND_TORUS 5
+#define BND_WEDGE 6
-#define BND_GRID 9
-#define BND_PIPE 10
-#define BND_SECTION 11
+#define BND_F3 7
+#define BND_F4 8
-#define BND_NBR 12
+#define BND_GRID 9
+#define BND_PIPE 10
+#define BND_SECTION 11
+#define BND_NBR 12
-#define BOUND_NBR 1024
-#define FACE_NBR 6144 /* Tailles de tableaux */
-#define VERTEX_NBR 16
-#define POINT_NBR 6144
-#ifdef face_normal
-#define VECTOR_NBR 6144
-#endif //face_normal
+#define BOUND_NBR 1024
+#define FACE_NBR 6144 /* Tailles de tableaux */
+#define VERTEX_NBR 16
+#define POINT_NBR 6144
+#ifdef face_normal
+#define VECTOR_NBR 6144
+#endif // face_normal
-#ifdef face_edge
-typedef struct {
- Index v0, v1; /* extremites */
- Index f0, f1; /* faces */
+#ifdef face_edge
+typedef struct {
+ Index v0, v1; /* extremites */
+ Index f0, f1; /* faces */
} Edge;
-#endif //face_edge
+#endif // face_edge
-#ifdef face_edge
-typedef struct {
- Index nbr; /* nombre d'aretes */
- Edge *ptr; /* liste dynamique */
+#ifdef face_edge
+typedef struct {
+ Index nbr; /* nombre d'aretes */
+ Edge *ptr; /* liste dynamique */
} Edge_list;
-#endif //face_edge
+#endif // face_edge
-#ifdef face_normal
-typedef struct {
- Index nbr; /* nombre de vecteurs */
- Vector *ptr; /* liste dynamique */
+#ifdef face_normal
+typedef struct {
+ Index nbr; /* nombre de vecteurs */
+ Vector *ptr; /* liste dynamique */
} Vector_list;
-#endif //face_normal
+#endif // face_normal
-typedef struct {
- float xmin, xmax; /* bornes sur l'axe x */
- float ymin, ymax; /* bornes sur l'axe y */
- float zmin, zmax; /* bornes sur l'axe z */
+typedef struct {
+ float xmin, xmax; /* bornes sur l'axe x */
+ float ymin, ymax; /* bornes sur l'axe y */
+ float zmin, zmax; /* bornes sur l'axe z */
} Bounding_box;
-typedef struct {
- Index nbr; /* nombre de scenes */
- Bound_scene *ptr; /* liste dynamique */
+typedef struct {
+ Index nbr; /* nombre de scenes */
+ Bound_scene *ptr; /* liste dynamique */
} Bound_scene_list;
-void free_Bound (Bound *bp);
-void free_huge_Bound (Bound *bp);
-void free_Bound_scene (Bound_scene *bsp);
-void malloc_Bound (Bound *bp, Type type, int polygonal, Index fn, Index pn);
-void malloc_huge_Bound (Bound *bp);
-void malloc_Bound_scene (Bound_scene *bsp, const char *name, Index bn);
+void free_Bound(Bound *bp);
+void free_huge_Bound(Bound *bp);
+void free_Bound_scene(Bound_scene *bsp);
+void malloc_Bound(Bound *bp, Type type, int polygonal, Index fn, Index pn);
+void malloc_huge_Bound(Bound *bp);
+void malloc_Bound_scene(Bound_scene *bsp, const char *name, Index bn);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpBoundio.cpp b/modules/robot/src/wireframe-simulator/vpBoundio.cpp
index e5a953e..a92faef 100644
--- a/modules/robot/src/wireframe-simulator/vpBoundio.cpp
+++ b/modules/robot/src/wireframe-simulator/vpBoundio.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,172 +40,165 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
#include "vpBoundio.h"
-#include "vpToken.h"
-#include "vpSkipio.h"
#include "vpLex.h"
+#include "vpMy.h"
+#include "vpSkipio.h"
+#include "vpToken.h"
-#include <errno.h>
-#include <stdio.h>
-#include <stdlib.h>
+#include <errno.h>
+#include <stdio.h>
+#include <stdlib.h>
/*
* La procedure "fscanf_Bound" lit en ascii une surface.
* Entree :
* bp Surface a lire.
*/
-void fscanf_Bound (Bound *bp)
+void fscanf_Bound(Bound *bp)
{
- /* Lecture du type polygonale de la surface. */
+ /* Lecture du type polygonale de la surface. */
- skip_keyword (T_TYPE, "bound: keyword \"type\" expected");
- if (lex () != T_INT)
- lexerr ("start","bound_type: boolean expected (0=FALSE|~0=TRUE)", NULL);
- bp->is_polygonal = (myint ? 1 : 0);
+ skip_keyword(T_TYPE, "bound: keyword \"type\" expected");
+ if (lex() != T_INT)
+ lexerr("start", "bound_type: boolean expected (0=FALSE|~0=TRUE)", NULL);
+ bp->is_polygonal = (myint ? 1 : 0);
- /* Lecture de la liste de points de la surface. */
+ /* Lecture de la liste de points de la surface. */
- skip_keyword (T_POINT_LIST, "bound: keyword \"point_list\" expected");
- pusherr ("bound_point_list: ");
- fscanf_Point3f_list (&bp->point);
- poperr ();
+ skip_keyword(T_POINT_LIST, "bound: keyword \"point_list\" expected");
+ pusherr("bound_point_list: ");
+ fscanf_Point3f_list(&bp->point);
+ poperr();
- /* Lecture de la liste de faces de la surface. */
+ /* Lecture de la liste de faces de la surface. */
- skip_keyword (T_FACE_LIST, "bound: keyword \"face_list\" expected");
- pusherr ("bound_face_list: ");
- fscanf_Face_list (&bp->face);
- poperr ();
+ skip_keyword(T_FACE_LIST, "bound: keyword \"face_list\" expected");
+ pusherr("bound_face_list: ");
+ fscanf_Face_list(&bp->face);
+ poperr();
}
/*
* La procedure "fscanf_Face_list" lit en ascii une liste de faces.
* Entree :
* lp Liste de faces a lire.
- */
-void fscanf_Face_list (Face_list *lp)
+ */
+void fscanf_Face_list(Face_list *lp)
{
- Face *fp; /* face courante */
- Face *fend; /* borne de "fp" */
+ Face *fp; /* face courante */
+ Face *fend; /* borne de "fp" */
- /* Lecture du nombre de faces de la liste */
+ /* Lecture du nombre de faces de la liste */
- if (lex () != T_INT)
- lexerr ("start","integer expected (number of faces)", NULL);
- lp->nbr = (Index) myint;
+ if (lex() != T_INT)
+ lexerr("start", "integer expected (number of faces)", NULL);
+ lp->nbr = (Index)myint;
- /* Allocation dynamique de la liste de faces. */
+ /* Allocation dynamique de la liste de faces. */
- if (lp->nbr == 0)
- lp->ptr = NULL;
- else if ((lp->ptr = (Face *) malloc (lp->nbr * sizeof (Face)))
- == NULL) {
- static char proc_name[] = "fscanf_Face_list";
- perror (proc_name);
- exit (1);
- }
+ if (lp->nbr == 0)
+ lp->ptr = NULL;
+ else if ((lp->ptr = (Face *)malloc(lp->nbr * sizeof(Face))) == NULL) {
+ static char proc_name[] = "fscanf_Face_list";
+ perror(proc_name);
+ exit(1);
+ }
- /* Lecture des faces de la liste une a une. */
+ /* Lecture des faces de la liste une a une. */
- fp = lp->ptr;
- fend = fp + lp->nbr;
- for (; fp < fend; fp++) {
- Vertex_list *vlp = &fp->vertex;
- Index *vp; /* sommet courant */
- Index *vend; /* borne de "vp" */
+ fp = lp->ptr;
+ fend = fp + lp->nbr;
+ for (; fp < fend; fp++) {
+ Vertex_list *vlp = &fp->vertex;
+ Index *vp; /* sommet courant */
+ Index *vend; /* borne de "vp" */
- /* Lecture du type polygonale de la face. */
+ /* Lecture du type polygonale de la face. */
- if (lex () != T_INT)
- lexerr ("start", "boolean expected (0=FALSE|~0=TRUE)", NULL);
- fp->is_polygonal = (myint ? 1 : 0);
+ if (lex() != T_INT)
+ lexerr("start", "boolean expected (0=FALSE|~0=TRUE)", NULL);
+ fp->is_polygonal = (myint ? 1 : 0);
- /* Lecture du nombre de sommets de la face. */
+ /* Lecture du nombre de sommets de la face. */
- if (lex () != T_INT)
- lexerr ("start", "integer expected (number of vertices)", NULL);
- vlp->nbr = (Index) myint;
+ if (lex() != T_INT)
+ lexerr("start", "integer expected (number of vertices)", NULL);
+ vlp->nbr = (Index)myint;
- /* Allocation dynamique du polygone de la face. */
+ /* Allocation dynamique du polygone de la face. */
if (vlp->nbr <= DEFAULT_VSIZE)
vlp->ptr = vlp->tbl;
- else if ((vlp->ptr = (Index *) malloc (vlp->nbr * sizeof (Index)))
- == NULL) {
- static char proc_name[] = "fscanf_Face_list";
- perror (proc_name);
- exit (1);
- }
+ else if ((vlp->ptr = (Index *)malloc(vlp->nbr * sizeof(Index))) == NULL) {
+ static char proc_name[] = "fscanf_Face_list";
+ perror(proc_name);
+ exit(1);
+ }
- /* Lecture des sommets de la face un a un. */
+ /* Lecture des sommets de la face un a un. */
- vp = vlp->ptr;
+ vp = vlp->ptr;
vend = vp + vlp->nbr;
- for (; vp < vend; *vp++ = (Index) myint)
- if (lex () != T_INT)
- lexerr ("start", "integer expected (index of points 3D)", NULL);
- }
-}
+ for (; vp < vend; *vp++ = (Index)myint)
+ if (lex() != T_INT)
+ lexerr("start", "integer expected (index of points 3D)", NULL);
+ }
+}
/*
* La procedure "fscanf_Point_list" lit en ascii une liste de points 3D.
* Entree :
* lp Liste de points a lire.
*/
-void fscanf_Point3f_list (Point3f_list *lp)
+void fscanf_Point3f_list(Point3f_list *lp)
{
- static const char *err_tbl[] = {
- "float expected (coordinate ",
- " of point)"
- };
- Point3f *pp; /* point courant */
- Point3f *pend; /* borne de "pp" */
-
- /* Lecture du nombre de points de la liste. */
-
- if (lex () != T_INT)
- lexerr ("start", "integer expected (number of points 3D)", NULL);
- lp->nbr = (Index) myint;
-/* FC printf("nbr %d\n",lp->nbr); */
- /* Allocation dynamique la liste de points. */
-
- if (lp->nbr == 0)
- lp->ptr = NULL;
- else if ((lp->ptr = (Point3f *) malloc (lp->nbr * sizeof (Point3f)))
- == NULL) {
- static const char proc_name[] = "fscanf_Point3f_list";
- perror (proc_name);
- exit (1);
- }
-
- /* Lecture des points de la liste un a un. */
-
- pp = lp->ptr;
- pend = pp + lp->nbr;
- for (; pp < pend; pp++) {
- int t;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "X", err_tbl[1], NULL);
-/* FC printf("X %d %f\n",myint, myfloat); */
- pp->x = (t == T_INT) ? (float) myint : myfloat;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "Y", err_tbl[1], NULL);
-/* FC printf("Y %d %f\n",myint, myfloat); */
- pp->y = (t == T_INT) ? (float) myint : myfloat;
-
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", err_tbl[0], "Z", err_tbl[1], NULL);
-/* FC printf("Z %d %f\n",myint, myfloat); */
- pp->z = (t == T_INT) ? (float) myint : myfloat;
- }
+ static const char *err_tbl[] = {"float expected (coordinate ", " of point)"};
+ Point3f *pp; /* point courant */
+ Point3f *pend; /* borne de "pp" */
+
+ /* Lecture du nombre de points de la liste. */
+
+ if (lex() != T_INT)
+ lexerr("start", "integer expected (number of points 3D)", NULL);
+ lp->nbr = (Index)myint;
+ /* FC printf("nbr %d\n",lp->nbr); */
+ /* Allocation dynamique la liste de points. */
+
+ if (lp->nbr == 0)
+ lp->ptr = NULL;
+ else if ((lp->ptr = (Point3f *)malloc(lp->nbr * sizeof(Point3f))) == NULL) {
+ static const char proc_name[] = "fscanf_Point3f_list";
+ perror(proc_name);
+ exit(1);
+ }
+
+ /* Lecture des points de la liste un a un. */
+
+ pp = lp->ptr;
+ pend = pp + lp->nbr;
+ for (; pp < pend; pp++) {
+ int t;
+
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "X", err_tbl[1], NULL);
+ /* FC printf("X %d %f\n",myint, myfloat); */
+ pp->x = (t == T_INT) ? (float)myint : myfloat;
+
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Y", err_tbl[1], NULL);
+ /* FC printf("Y %d %f\n",myint, myfloat); */
+ pp->y = (t == T_INT) ? (float)myint : myfloat;
+
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", err_tbl[0], "Z", err_tbl[1], NULL);
+ /* FC printf("Z %d %f\n",myint, myfloat); */
+ pp->z = (t == T_INT) ? (float)myint : myfloat;
+ }
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpBoundio.h b/modules/robot/src/wireframe-simulator/vpBoundio.h
index a269f94..affc72b 100644
--- a/modules/robot/src/wireframe-simulator/vpBoundio.h
+++ b/modules/robot/src/wireframe-simulator/vpBoundio.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,9 +49,9 @@
#include "vpBound.h"
-void fscanf_Bound (Bound *bp);
-void fscanf_Face_list (Face_list *lp);
-void fscanf_Point3f_list (Point3f_list *lp);
+void fscanf_Bound(Bound *bp);
+void fscanf_Face_list(Face_list *lp);
+void fscanf_Point3f_list(Point3f_list *lp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpClipping.cpp b/modules/robot/src/wireframe-simulator/vpClipping.cpp
index d6d7698..ec2f6a6 100644
--- a/modules/robot/src/wireframe-simulator/vpClipping.cpp
+++ b/modules/robot/src/wireframe-simulator/vpClipping.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,18 +48,18 @@
#include "vpClipping.h"
#include "vpView.h"
+#include <cmath>
+#include <limits>
#include <stdio.h>
#include <stdlib.h>
-#include <limits>
-#include <cmath>
-static void inter (Byte mask, Index v0, Index v1);
-static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
+static void inter(Byte mask, Index v0, Index v1);
+static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3);
/*
* Variables utilisees par le decoupage :
*
- * CLIP :
+ * CLIP :
* Surface resultat apres le decoupage.
* La surface est adaptee pour la reception de tous les types de surfaces.
* Sa taille est definie par les macros "..._NBR" de "bound.h".
@@ -70,7 +71,7 @@ static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
*
* La surface recoit une a une les faces decoupees.
* La surface recoit en une fois tous les points 3D de la surface decoupee
- * par rapport aux 6 plans de la pyramide tronquee de vision.
+ * par rapport aux 6 plans de la pyramide tronquee de vision.
*
* CODE :
* Tableau de booleens durant le decoupage.
@@ -83,67 +84,65 @@ static void point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3);
* Tableau de points durant le decoupage.
* Le tableau est initialise par les points de la surface en entree apres
* transforation en coordonnees homogenes 4D.
- * Le tableau recoit ensuite un a un les points 4D d'intersection de la surface
- * avec les 6 plans de la pyramide tronquee de vision.
+ * Le tableau recoit ensuite un a un les points 4D d'intersection de la
+ * surface avec les 6 plans de la pyramide tronquee de vision.
*/
-static Bound clip; /* surface a decouper */
-static Byte *code; /* tableau de bits */
-static Point4f *point4f; /* tableau de points 4D */
-static Index point4f_nbr; /* nombre de points 4D */
+static Bound clip; /* surface a decouper */
+static Byte *code; /* tableau de bits */
+static Point4f *point4f; /* tableau de points 4D */
+static Index point4f_nbr; /* nombre de points 4D */
-#if clip_opt
-static Index *poly0, *poly1; /* polygones temporaires*/
+#if clip_opt
+static Index *poly0, *poly1; /* polygones temporaires*/
#else
-static Index *poly_tmp; /* polygone temporaire */
-#endif /* clip_opt */
-
+static Index *poly_tmp; /* polygone temporaire */
+#endif /* clip_opt */
/*
* La procedure "open_clipping" alloue et initialise les variables utilisees
* par le mode "clipping".
*/
-void open_clipping (void)
+void open_clipping(void)
{
- /* alloue la surface de travail */
- malloc_huge_Bound (&clip);
-
- /* alloue les tableaux */
- if ((code = (Byte *) malloc (POINT_NBR * sizeof (Byte))) == NULL
- || (point4f = (Point4f *) malloc (POINT_NBR * sizeof (Point4f))) == NULL
-#ifdef clip_opt
- || (poly0 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL
- || (poly1 = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL) {
- static char proc_name[] = "open_clipping";
- perror (proc_name);
- exit (1);
- }
+ /* alloue la surface de travail */
+ malloc_huge_Bound(&clip);
+
+ /* alloue les tableaux */
+ if ((code = (Byte *)malloc(POINT_NBR * sizeof(Byte))) == NULL ||
+ (point4f = (Point4f *)malloc(POINT_NBR * sizeof(Point4f))) == NULL
+#ifdef clip_opt
+ || (poly0 = (Index *)malloc(VERTEX_NBR * sizeof(Index))) == NULL ||
+ (poly1 = (Index *)malloc(VERTEX_NBR * sizeof(Index))) == NULL) {
+ static char proc_name[] = "open_clipping";
+ perror(proc_name);
+ exit(1);
+ }
#else
- || (poly_tmp = (Index *) malloc (VERTEX_NBR * sizeof (Index))) == NULL){
- static char proc_name[] = "open_clipping";
- perror (proc_name);
- exit (1);
- }
-#endif /* clip_opt */
+ || (poly_tmp = (Index *)malloc(VERTEX_NBR * sizeof(Index))) == NULL) {
+ static char proc_name[] = "open_clipping";
+ perror(proc_name);
+ exit(1);
+ }
+#endif /* clip_opt */
}
/*
* La procedure "close_clipping" libere les variables utilisees par
* le mode "clipping".
*/
-void close_clipping (void)
+void close_clipping(void)
{
- free_huge_Bound (&clip);
- free ((char *) code);
- free ((char *) point4f);
-#ifdef clip_opt
- free ((char *) poly0);
- free ((char *) poly1);
+ free_huge_Bound(&clip);
+ free((char *)code);
+ free((char *)point4f);
+#ifdef clip_opt
+ free((char *)poly0);
+ free((char *)poly1);
#else
- free ((char *) poly_tmp);
-#endif /* clip_opt */
+ free((char *)poly_tmp);
+#endif /* clip_opt */
}
-
/*
* La procedure "clipping" decoupe un polygone par rapport a un plan
* suivant l'algorithme de Sutherland et Hodgman.
@@ -155,52 +154,46 @@ void close_clipping (void)
* Sortie :
* Nombre de sommets du polygone resultat "po".
*/
-static Index
-clipping (Byte mask, Index vni, Index *pi, Index *po)
+static Index clipping(Byte mask, Index vni, Index *pi, Index *po)
{
- /*
- * vno Nombre de sommets du polygone "po".
- * vs Premier sommet de l'arete a decouper.
- * vp Second sommet de l'arete a decouper.
- * ins TRUE si le sommet "vs" est interieur, FALSE sinon.
- * inp TRUE si le sommet "vp" est interieur, FALSE sinon.
- */
- Index vno = vni; /* nombre de sommets */
- Index vs = pi[vni-1]; /* premier sommet */
- Index vp; /* second sommet */
- Byte ins = code[vs] & mask; /* code de "vs" */
-
- while (vni--) { /* pour tous les sommets */
- vp = *pi++; /* second sommet */
- Byte inp = code[vp] & mask; /* code du plan courant */
-
- if (ins == IS_INSIDE) {
- if (inp == IS_INSIDE) { /* arete interieure */
- *po++ = vp;
- }
- else { /* intersection */
- inter (mask, vs, vp);
- *po++ = point4f_nbr++;
- }
- }
- else {
- if (inp == IS_INSIDE) { /* intersection */
- inter (mask, vs, vp);
- *po++ = point4f_nbr++;
- *po++ = vp;
- vno++;
- }
- else { /* arete exterieure */
- vno--;
- }
- }
- vs = vp;
- ins = inp;
- }
- return (vno);
+ /*
+ * vno Nombre de sommets du polygone "po".
+ * vs Premier sommet de l'arete a decouper.
+ * vp Second sommet de l'arete a decouper.
+ * ins TRUE si le sommet "vs" est interieur, FALSE sinon.
+ * inp TRUE si le sommet "vp" est interieur, FALSE sinon.
+ */
+ Index vno = vni; /* nombre de sommets */
+ Index vs = pi[vni - 1]; /* premier sommet */
+ Byte ins = code[vs] & mask; /* code de "vs" */
+
+ while (vni--) { /* pour tous les sommets */
+ Index vp = *pi++; /* second sommet */
+ Byte inp = code[vp] & mask; /* code du plan courant */
+
+ if (ins == IS_INSIDE) {
+ if (inp == IS_INSIDE) { /* arete interieure */
+ *po++ = vp;
+ } else { /* intersection */
+ inter(mask, vs, vp);
+ *po++ = point4f_nbr++;
+ }
+ } else {
+ if (inp == IS_INSIDE) { /* intersection */
+ inter(mask, vs, vp);
+ *po++ = point4f_nbr++;
+ *po++ = vp;
+ vno++;
+ } else { /* arete exterieure */
+ vno--;
+ }
+ }
+ vs = vp;
+ ins = inp;
+ }
+ return (vno);
}
-
/*
* La procedure "clipping_Face" decoupe une face par rapport aux 6 plans
* de decoupage de la pyramide tronquee de vision.
@@ -210,89 +203,86 @@ clipping (Byte mask, Index vni, Index *pi, Index *po)
* Sortie :
* Le nombre de sommets de la face resultat.
*/
-static Index
-clipping_Face (Face *fi, Face *fo)
+static Index clipping_Face(Face *fi, Face *fo)
{
- Index *flip = poly_tmp; /* polygone temporaire */
- Index *flop = fo->vertex.ptr; /* polygone resultat */
- Index vn = fi->vertex.nbr; /* nombre de sommets */
-
- if ((vn = clipping (IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0)
- if ((vn = clipping (IS_BELOW, vn, flip, flop)) != 0)
- if ((vn = clipping (IS_RIGHT, vn, flop, flip)) != 0)
- if ((vn = clipping (IS_LEFT, vn, flip, flop)) != 0)
- if ((vn = clipping (IS_BACK, vn, flop, flip)) != 0)
- if ((vn = clipping (IS_FRONT, vn, flip, flop)) != 0) {
- /* recopie de "fi" dans "fo" */
- /* fo->vertex.ptr == flop */
- fo->vertex.nbr = vn;
- fo->is_polygonal = fi->is_polygonal;
- fo->is_visible = fi->is_visible;
-#ifdef face_normal
- fo->normal = fi->normal;
-#endif /* face_normal */
- return (vn);
- }
- return (0);
+ Index *flip = poly_tmp; /* polygone temporaire */
+ Index *flop = fo->vertex.ptr; /* polygone resultat */
+ Index vn = fi->vertex.nbr; /* nombre de sommets */
+
+ if ((vn = clipping(IS_ABOVE, vn, fi->vertex.ptr, flip)) != 0)
+ if ((vn = clipping(IS_BELOW, vn, flip, flop)) != 0)
+ if ((vn = clipping(IS_RIGHT, vn, flop, flip)) != 0)
+ if ((vn = clipping(IS_LEFT, vn, flip, flop)) != 0)
+ if ((vn = clipping(IS_BACK, vn, flop, flip)) != 0)
+ if ((vn = clipping(IS_FRONT, vn, flip, flop)) != 0) {
+ /* recopie de "fi" dans "fo" */
+ /* fo->vertex.ptr == flop */
+ fo->vertex.nbr = vn;
+ fo->is_polygonal = fi->is_polygonal;
+ fo->is_visible = fi->is_visible;
+#ifdef face_normal
+ fo->normal = fi->normal;
+#endif /* face_normal */
+ return (vn);
+ }
+ return (0);
}
/*
* La procedure "clipping_Bound" decoupe une surface par rapport aux 6 plans
* de decoupage de la pyramide tronquee de vision.
- * Les calculs geometriques sont effectues en coordonnees homogenes.
- * Note : Les points invisibles de la surface "clip" ont une profondeur negative
- * c'est a dire une coordonnee Z negative.
- * Entree :
+ * Les calculs geometriques sont effectues en coordonnees homogenes.
+ * Note : Les points invisibles de la surface "clip" ont une profondeur
+ *negative c'est a dire une coordonnee Z negative. Entree :
* bp Surface a decouper.
* m Matrice de projection dans le volume canonique.
* Sortie :
* Pointeur de la surface resultat "clip" si elle est visible,
* NULL sinon.
*/
-Bound *clipping_Bound (Bound *bp, Matrix m)
+Bound *clipping_Bound(Bound *bp, Matrix m)
{
- Face *fi = bp->face.ptr; /* 1ere face */
- Face *fend = fi + bp->face.nbr; /* borne de "fi"*/
- Face *fo = clip.face.ptr; /* face clippee */
-
- /* recopie de "bp" dans les tableaux intermediaires */
-
- point4f_nbr = bp->point.nbr;
- point_3D_4D (bp->point.ptr, (int) point4f_nbr, m, point4f);
- set_Point4f_code (point4f, (int) point4f_nbr, code);
-#ifdef face_normal
- if (! (clip.is_polygonal = bp->is_polygonal))
- //bcopy (bp->normal.ptr, clip.normal.ptr,
- // bp->normal.nbr * sizeof (Vector));
- memmove (clip.normal.ptr, bp->normal.ptr,
- bp->normal.nbr * sizeof (Vector));
-#endif /* face_normal */
- for (; fi < fend; fi++) { /* pour toutes les faces*/
- if (clipping_Face (fi, fo) != 0) {
- fo++; /* ajoute la face a "clip" */
- /*
- * Construction a la volee du future polygone.
- * dont l'espace memoire est deja alloue (voir
- * la procedure "malloc_huge_Bound").
- */
- fo->vertex.ptr = (fo-1)->vertex.ptr+(fo-1)->vertex.nbr;
- }
- }
-
- if (fo == clip.face.ptr)
- return (NULL); /* Rien a voir, circulez... */
-
- /* recopie des tableaux intermediaires dans "clip" */
-
- point_4D_3D (point4f, (int) point4f_nbr, code, clip.point.ptr);
- clip.type = bp->type;
- clip.face.nbr = (Index)( fo - clip.face.ptr );
- clip.point.nbr = point4f_nbr;
-#ifdef face_normal
- if (! bp->is_polygonal)
- clip.normal.nbr = point4f_nbr;
-#endif /* face_normal */
- return (&clip);
+ Face *fi = bp->face.ptr; /* 1ere face */
+ Face *fend = fi + bp->face.nbr; /* borne de "fi"*/
+ Face *fo = clip.face.ptr; /* face clippee */
+
+ /* recopie de "bp" dans les tableaux intermediaires */
+
+ point4f_nbr = bp->point.nbr;
+ point_3D_4D(bp->point.ptr, (int)point4f_nbr, m, point4f);
+ set_Point4f_code(point4f, (int)point4f_nbr, code);
+#ifdef face_normal
+ if (!(clip.is_polygonal = bp->is_polygonal))
+ // bcopy (bp->normal.ptr, clip.normal.ptr,
+ // bp->normal.nbr * sizeof (Vector));
+ memmove(clip.normal.ptr, bp->normal.ptr, bp->normal.nbr * sizeof(Vector));
+#endif /* face_normal */
+ for (; fi < fend; fi++) { /* pour toutes les faces*/
+ if (clipping_Face(fi, fo) != 0) {
+ fo++; /* ajoute la face a "clip" */
+ /*
+ * Construction a la volee du future polygone.
+ * dont l'espace memoire est deja alloue (voir
+ * la procedure "malloc_huge_Bound").
+ */
+ fo->vertex.ptr = (fo - 1)->vertex.ptr + (fo - 1)->vertex.nbr;
+ }
+ }
+
+ if (fo == clip.face.ptr)
+ return (NULL); /* Rien a voir, circulez... */
+
+ /* recopie des tableaux intermediaires dans "clip" */
+
+ point_4D_3D(point4f, (int)point4f_nbr, code, clip.point.ptr);
+ clip.type = bp->type;
+ clip.face.nbr = (Index)(fo - clip.face.ptr);
+ clip.point.nbr = point4f_nbr;
+#ifdef face_normal
+ if (!bp->is_polygonal)
+ clip.normal.nbr = point4f_nbr;
+#endif /* face_normal */
+ return (&clip);
}
/*
@@ -303,90 +293,86 @@ Bound *clipping_Bound (Bound *bp, Matrix m)
* v0 Permier sommet de l'arete.
* v1 Second sommet de l'arete.
*/
-static void
-inter (Byte mask, Index v0, Index v1)
+static void inter(Byte mask, Index v0, Index v1)
{
- Point4f *p = point4f + point4f_nbr;
- Point4f *p0 = point4f + v0;
- Point4f *p1 = point4f + v1;
- float t; /* parametre entre 0 et 1 */
+ Point4f *p = point4f + point4f_nbr;
+ Point4f *p0 = point4f + v0;
+ Point4f *p1 = point4f + v1;
+ float t; /* parametre entre 0 et 1 */
- /* calcule le point d'intersection */
+ /* calcule le point d'intersection */
- switch (mask) {
+ switch (mask) {
- case IS_ABOVE :
- /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */
- t = (p0->w - p0->y) - (p1->w - p1->y);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t;
+ case IS_ABOVE:
+ /* t = (p0->w - p0->y) / ((p0->w - p0->y) - (p1->w - p1->y)); */
+ t = (p0->w - p0->y) - (p1->w - p1->y);
+ // t = (t == 0) ? (float)1.0 : (p0->w - p0->y) / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->y) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = p->y; /* propriete du point d'intersection */
- break;
-
- case IS_BELOW :
- /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */
- t = (p0->w + p0->y) - (p1->w + p1->y);
- //t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t;
+ PAR_COORD3(*p, t, *p0, *p1);
+ p->w = p->y; /* propriete du point d'intersection */
+ break;
+
+ case IS_BELOW:
+ /* t = (p0->w + p0->y) / ((p0->w + p0->y) - (p1->w + p1->y)); */
+ t = (p0->w + p0->y) - (p1->w + p1->y);
+ // t = (t == 0) ? (float)1.0 : (p0->w + p0->y) / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->y) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = - p->y; /* propriete du point d'intersection */
- break;
-
- case IS_RIGHT :
- /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */
- t = (p0->w - p0->x) - (p1->w - p1->x);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t;
+ PAR_COORD3(*p, t, *p0, *p1);
+ p->w = -p->y; /* propriete du point d'intersection */
+ break;
+
+ case IS_RIGHT:
+ /* t = (p0->w - p0->x) / ((p0->w - p0->x) - (p1->w - p1->x)); */
+ t = (p0->w - p0->x) - (p1->w - p1->x);
+ // t = (t == 0) ? (float)1.0 : (p0->w - p0->x) / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->x) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = p->x; /* propriete du point d'intersection */
- break;
-
- case IS_LEFT :
- /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */
- t = (p0->w + p0->x) - (p1->w + p1->x);
- //t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t;
+ PAR_COORD3(*p, t, *p0, *p1);
+ p->w = p->x; /* propriete du point d'intersection */
+ break;
+
+ case IS_LEFT:
+ /* t = (p0->w + p0->x) / ((p0->w + p0->x) - (p1->w + p1->x)); */
+ t = (p0->w + p0->x) - (p1->w + p1->x);
+ // t = (t == 0) ? (float)1.0 : (p0->w + p0->x) / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w + p0->x) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = - p->x; /* propriete du point d'intersection */
- break;
-
- case IS_BACK :
- /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */
- t = (p0->w - p0->z) - (p1->w - p1->z);
- //t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t;
+ PAR_COORD3(*p, t, *p0, *p1);
+ p->w = -p->x; /* propriete du point d'intersection */
+ break;
+
+ case IS_BACK:
+ /* t = (p0->w - p0->z) / ((p0->w - p0->z) - (p1->w - p1->z)); */
+ t = (p0->w - p0->z) - (p1->w - p1->z);
+ // t = (t == 0) ? (float)1.0 : (p0->w - p0->z) / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : (p0->w - p0->z) / t;
- PAR_COORD3(*p,t,*p0,*p1);
- p->w = p->z; /* propriete du point d'intersection */
- break;
-
- case IS_FRONT :
- /* t = p0->z / (p0->z - p1->z); */
- t = (p0->z - p1->z);
- //t = (t == 0) ? (float)1.0 : p0->z / t;
+ PAR_COORD3(*p, t, *p0, *p1);
+ p->w = p->z; /* propriete du point d'intersection */
+ break;
+
+ case IS_FRONT:
+ /* t = p0->z / (p0->z - p1->z); */
+ t = (p0->z - p1->z);
+ // t = (t == 0) ? (float)1.0 : p0->z / t;
t = (std::fabs(t) <= std::numeric_limits<double>::epsilon()) ? (float)1.0 : p0->z / t;
- p->x = (p1->x - p0->x) * t + p0->x;
- p->y = (p1->y - p0->y) * t + p0->y;
- p->w = (p1->w - p0->w) * t + p0->w;
- p->z = (float)M_EPSILON; /* propriete du point d'intersection */
- break;
- }
- /* resout les problemes d'arrondis pour "where_is_Point4f" */
- /* p->w += (p->w < 0) ? (- M_EPSILON) : M_EPSILON; */
- p->w += (float)M_EPSILON;
- code[point4f_nbr] = where_is_Point4f (p); /* localise "p" */
-#ifdef face_normal
- if (! clip.is_polygonal) {
- Vector *n0 = clip.normal.ptr + v0;
- Vector *n1 = clip.normal.ptr + v1;
- Vector *n = clip.normal.ptr + point4f_nbr;
-
- SET_COORD3(*n,
- (n1->x - n0->x) * t + n0->x,
- (n1->y - n0->y) * t + n0->y,
- (n1->z - n0->z) * t + n0->z);
- }
-#endif /* face_normal */
+ p->x = (p1->x - p0->x) * t + p0->x;
+ p->y = (p1->y - p0->y) * t + p0->y;
+ p->w = (p1->w - p0->w) * t + p0->w;
+ p->z = (float)M_EPSILON; /* propriete du point d'intersection */
+ break;
+ }
+ /* resout les problemes d'arrondis pour "where_is_Point4f" */
+ /* p->w += (p->w < 0) ? (- M_EPSILON) : M_EPSILON; */
+ p->w += (float)M_EPSILON;
+ code[point4f_nbr] = where_is_Point4f(p); /* localise "p" */
+#ifdef face_normal
+ if (!clip.is_polygonal) {
+ Vector *n0 = clip.normal.ptr + v0;
+ Vector *n1 = clip.normal.ptr + v1;
+ Vector *n = clip.normal.ptr + point4f_nbr;
+
+ SET_COORD3(*n, (n1->x - n0->x) * t + n0->x, (n1->y - n0->y) * t + n0->y, (n1->z - n0->z) * t + n0->z);
+ }
+#endif /* face_normal */
}
/*
@@ -399,73 +385,77 @@ inter (Byte mask, Index v0, Index v1)
* cp Tableau de code indiquant la visibilite des points 4D.
* p3 Tableau de points 3D issus de la transformation.
*/
-static void
-point_4D_3D (Point4f *p4, int size, Byte *cp, Point3f *p3)
+static void point_4D_3D(Point4f *p4, int size, Byte *cp, Point3f *p3)
{
- Point4f *pend = p4 + size; /* borne de p4 */
- float w;
-
- for (; p4 < pend; p4++, p3++) {
- if (*cp++ == IS_INSIDE) { /* point visible */
- w = p4->w;
-
- p3->x = p4->x / w; /* projection 4D en 3D */
- p3->y = p4->y / w;
- p3->z = p4->z / w;
- }
- else { /* marque invisible */
- p3->z = -1.0;
- }
- }
+ Point4f *pend = p4 + size; /* borne de p4 */
+ float w;
+
+ for (; p4 < pend; p4++, p3++) {
+ if (*cp++ == IS_INSIDE) { /* point visible */
+ w = p4->w;
+
+ p3->x = p4->x / w; /* projection 4D en 3D */
+ p3->y = p4->y / w;
+ p3->z = p4->z / w;
+ } else { /* marque invisible */
+ p3->z = -1.0;
+ }
+ }
}
/*
* La procedure "set_Point4f_code" initialise la position des points 4D
* par rapport a 6 plans de la pyramide tronquee de vision.
- * A chaque point est associe un code indiquant la position respective du point.
- * Entree :
- * p4 Tableau de points 4D a localiser.
- * size Taille du tableau "p4".
- * cp Tableau de codes de localisation resultat.
+ * A chaque point est associe un code indiquant la position respective du
+ * point. Entree : p4 Tableau de points 4D a localiser. size
+ * Taille du tableau "p4". cp Tableau de codes de localisation
+ * resultat.
*/
-void
-set_Point4f_code (Point4f *p4, int size, Byte *cp)
+void set_Point4f_code(Point4f *p4, int size, Byte *cp)
{
- Point4f *pend = p4 + size; /* borne de p4 */
- Byte b; /* code de p4 */
-
- for (; p4 < pend; p4++, *cp++ = b) {
- b = IS_INSIDE;
- if ( p4->w < p4->y) b |= IS_ABOVE;
- else if (- p4->w > p4->y) b |= IS_BELOW;
- if ( p4->w < p4->x) b |= IS_RIGHT;
- else if (- p4->w > p4->x) b |= IS_LEFT;
- if ( p4->w < p4->z) b |= IS_BACK;
- else if ( -0.9 > p4->z) b |= IS_FRONT;
- }
+ Point4f *pend = p4 + size; /* borne de p4 */
+ Byte b; /* code de p4 */
+
+ for (; p4 < pend; p4++, *cp++ = b) {
+ b = IS_INSIDE;
+ if (p4->w < p4->y)
+ b |= IS_ABOVE;
+ else if (-p4->w > p4->y)
+ b |= IS_BELOW;
+ if (p4->w < p4->x)
+ b |= IS_RIGHT;
+ else if (-p4->w > p4->x)
+ b |= IS_LEFT;
+ if (p4->w < p4->z)
+ b |= IS_BACK;
+ else if (-0.9 > p4->z)
+ b |= IS_FRONT;
+ }
}
-
/*
- * La procedure "where_is_Point4f" localise un point 4D par rapport aux 6 plans
- * de decoupage de la pyramide tronquee de vision.
- * Entree :
- * p4 Point homogene 4D a localiser.
- * Sortie :
- * Code indiquant le position de "p4" par rapport aux 6 plans.
+ * La procedure "where_is_Point4f" localise un point 4D par rapport aux 6
+ * plans de decoupage de la pyramide tronquee de vision. Entree : p4
+ * Point homogene 4D a localiser. Sortie : Code indiquant le position de "p4"
+ * par rapport aux 6 plans.
*/
-Byte
-where_is_Point4f (Point4f *p4)
+Byte where_is_Point4f(Point4f *p4)
{
- Byte b = IS_INSIDE; /* code de "p4" */
-
- if ( p4->w < p4->y) b |= IS_ABOVE;
- else if (- p4->w > p4->y) b |= IS_BELOW;
- if ( p4->w < p4->x) b |= IS_RIGHT;
- else if (- p4->w > p4->x) b |= IS_LEFT;
- if ( p4->w < p4->z) b |= IS_BACK;
- else if ( -0.9 > p4->z) b |= IS_FRONT;
- return (b);
+ Byte b = IS_INSIDE; /* code de "p4" */
+
+ if (p4->w < p4->y)
+ b |= IS_ABOVE;
+ else if (-p4->w > p4->y)
+ b |= IS_BELOW;
+ if (p4->w < p4->x)
+ b |= IS_RIGHT;
+ else if (-p4->w > p4->x)
+ b |= IS_LEFT;
+ if (p4->w < p4->z)
+ b |= IS_BACK;
+ else if (-0.9 > p4->z)
+ b |= IS_FRONT;
+ return (b);
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpClipping.h b/modules/robot/src/wireframe-simulator/vpClipping.h
index 658c233..6c4d12e 100644
--- a/modules/robot/src/wireframe-simulator/vpClipping.h
+++ b/modules/robot/src/wireframe-simulator/vpClipping.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,16 +49,16 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
#include "vpBound.h"
+#include "vpMy.h"
-void open_clipping (void);
-void open_clipping (void);
-void close_clipping (void);
-Bound *clipping_Bound (Bound *bp, Matrix m);
-void set_Point4f_code (Point4f *p4, int size, Byte *cp);
-Byte where_is_Point4f (Point4f *p4);
+void open_clipping(void);
+void open_clipping(void);
+void close_clipping(void);
+Bound *clipping_Bound(Bound *bp, Matrix m);
+void set_Point4f_code(Point4f *p4, int size, Byte *cp);
+Byte where_is_Point4f(Point4f *p4);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp
index 45cd7be..3a12423 100644
--- a/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp
+++ b/modules/robot/src/wireframe-simulator/vpCoreDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,49 +37,43 @@
*
*****************************************************************************/
-
-
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <stdio.h>
-#include <stdlib.h>
+#include <stdio.h>
+#include <stdlib.h>
-
-#include "vpMy.h"
#include "vpCoreDisplay.h"
-#include "vpView.h"
#include "vpImstack.h"
+#include "vpMy.h"
#include "vpRfstack.h"
+#include "vpView.h"
#include "vpVwstack.h"
-
/*
- * POINT2I :
+ * POINT2I :
* Tableau de points 2D dans l'espace ecran servant a l'affichage fil-de-fer.
*
* RENAME :
- * Tableau de renommage des sommets ou tableau de compteurs associes aux points.
+ * Tableau de renommage des sommets ou tableau de compteurs associes aux
+ * points.
*/
-Point2i *point2i = (Point2i *) NULL;
-Point2i *listpoint2i = (Point2i *) NULL;
-static int *rename_jlc = (int *) NULL;
-
+Point2i *point2i = (Point2i *)NULL;
+Point2i *listpoint2i = (Point2i *)NULL;
+static int *rename_jlc = (int *)NULL;
/*
* La procedure "open_display" alloue et initialise les variables utilisees
* par le mode "display".
*/
-void open_display (void)
+void open_display(void)
{
- if ((point2i = (Point2i *) malloc (POINT_NBR*sizeof (Point2i))) == NULL
- || (listpoint2i = (Point2i *) malloc (50*sizeof (Point2i))) == NULL
- || (rename_jlc = (int *) malloc (POINT_NBR * sizeof (int))) == NULL)
- {
- static char proc_name[] = "open_display";
- perror (proc_name);
- exit (1);
+ if ((point2i = (Point2i *)malloc(POINT_NBR * sizeof(Point2i))) == NULL ||
+ (listpoint2i = (Point2i *)malloc(50 * sizeof(Point2i))) == NULL ||
+ (rename_jlc = (int *)malloc(POINT_NBR * sizeof(int))) == NULL) {
+ static char proc_name[] = "open_display";
+ perror(proc_name);
+ exit(1);
}
}
@@ -86,14 +81,14 @@ void open_display (void)
* La procedure "close_display" libere les variables utilisees par le mode
* "display".
*/
-void close_display (void)
+void close_display(void)
{
- free ((char *) point2i);
- free ((char *) listpoint2i);
- free ((char *) rename_jlc);
- point2i = (Point2i *) NULL;
- listpoint2i = (Point2i *) NULL;
- rename_jlc = (int *) NULL;
+ free((char *)point2i);
+ free((char *)listpoint2i);
+ free((char *)rename_jlc);
+ point2i = (Point2i *)NULL;
+ listpoint2i = (Point2i *)NULL;
+ rename_jlc = (int *)NULL;
}
/*
@@ -116,16 +111,16 @@ void close_display (void)
* p2 Tableau de points 2D en sortie.
*/
// static
-void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
+void point_3D_2D(Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
{
- Point3f *pend = p3 + size; /* borne de p3 */
- float xdiv2 = ((float) xsize) / (float)2.0;
- float ydiv2 = ((float) ysize) / (float)2.0;
+ Point3f *pend = p3 + size; /* borne de p3 */
+ float xdiv2 = ((float)xsize) / (float)2.0;
+ float ydiv2 = ((float)ysize) / (float)2.0;
- for (; p3 < pend; p3++, p2++) {
- p2->x = (int) ((1.0 + p3->x) * xdiv2);
- p2->y = (int) ((1.0 - p3->y) * ydiv2);
- }
+ for (; p3 < pend; p3++, p2++) {
+ p2->x = (int)((1.0 + p3->x) * xdiv2);
+ p2->y = (int)((1.0 - p3->y) * ydiv2);
+ }
}
/*
@@ -133,7 +128,7 @@ void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
* de la surface "bp".
* Soit la face comportant le contour oriente suivant : (...,P2,P0,P1...).
* La normale a la face au point P0 est obtenue par le produit vectoriel :
- *
+ *
* | x1 - x0 x2 - x0 | | Nx |
* N = (P1 - P0) ^ (P2 - P0) = | y1 - y0 y2 - y0 | = | Ny |
* | z1 - z0 z2 - z0 | | Nz |
@@ -147,82 +142,82 @@ void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2)
* bp Surface a initialiser.
* b Drapeaux indiquant les faces non affichables.
*/
-void set_Bound_face_display (Bound *bp, Byte b)
+void set_Bound_face_display(Bound *bp, Byte b)
{
- Face *fp = bp->face.ptr;
- Face *fend = fp + bp->face.nbr;
- Point3f *pp = bp->point.ptr;
-
- for (; fp < fend; fp++) {
- Index *vp;
- Point3f *p0; /* premier sommet */
- Point3f *p1; /* second sommet */
- Point3f *p2; /* dernier sommet */
-
- fp->is_visible = TRUE;
- if (b == IS_INSIDE) continue;
- vp = fp->vertex.ptr;
- p0 = pp + *vp;
- p1 = pp + *(vp + 1);
- p2 = pp + *(vp + fp->vertex.nbr - 1);
- if (b & IS_ABOVE) {
- fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
- >= (p1->x - p0->x) * (p2->z - p0->z));
- }
- if (! fp->is_visible) continue;
- if (b & IS_BELOW) {
- fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x)
- <= (p1->x - p0->x) * (p2->z - p0->z));
- }
- if (! fp->is_visible) continue;
- if (b & IS_RIGHT) {
- fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
- >= (p1->z - p0->z) * (p2->y - p0->y));
- }
- if (! fp->is_visible) continue;
- if (b & IS_LEFT) {
- fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z)
- <= (p1->z - p0->z) * (p2->y - p0->y));
- }
- if (! fp->is_visible) continue;
- if (b & IS_BACK) {
- fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
- >= (p1->y - p0->y) * (p2->x - p0->x));
- }
- if (! fp->is_visible) continue;
- if (b & IS_FRONT) {
- fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y)
- <= (p1->y - p0->y) * (p2->x - p0->x));
- }
- }
+ Face *fp = bp->face.ptr;
+ Face *fend = fp + bp->face.nbr;
+ Point3f *pp = bp->point.ptr;
+
+ for (; fp < fend; fp++) {
+ Index *vp;
+ Point3f *p0; /* premier sommet */
+ Point3f *p1; /* second sommet */
+ Point3f *p2; /* dernier sommet */
+
+ fp->is_visible = TRUE;
+ if (b == IS_INSIDE)
+ continue;
+ vp = fp->vertex.ptr;
+ p0 = pp + *vp;
+ p1 = pp + *(vp + 1);
+ p2 = pp + *(vp + fp->vertex.nbr - 1);
+ if (b & IS_ABOVE) {
+ fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x) >= (p1->x - p0->x) * (p2->z - p0->z));
+ }
+ if (!fp->is_visible)
+ continue;
+ if (b & IS_BELOW) {
+ fp->is_visible = ((p1->z - p0->z) * (p2->x - p0->x) <= (p1->x - p0->x) * (p2->z - p0->z));
+ }
+ if (!fp->is_visible)
+ continue;
+ if (b & IS_RIGHT) {
+ fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z) >= (p1->z - p0->z) * (p2->y - p0->y));
+ }
+ if (!fp->is_visible)
+ continue;
+ if (b & IS_LEFT) {
+ fp->is_visible = ((p1->y - p0->y) * (p2->z - p0->z) <= (p1->z - p0->z) * (p2->y - p0->y));
+ }
+ if (!fp->is_visible)
+ continue;
+ if (b & IS_BACK) {
+ fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y) >= (p1->y - p0->y) * (p2->x - p0->x));
+ }
+ if (!fp->is_visible)
+ continue;
+ if (b & IS_FRONT) {
+ fp->is_visible = ((p1->x - p0->x) * (p2->y - p0->y) <= (p1->y - p0->y) * (p2->x - p0->x));
+ }
+ }
}
-
/*
* La procedure "wireframe_Face" affiche une face "fp" en "fil de fer".
* sur la fenetre graphique de "suncgi" sur "SUN".
- * Les points des sommets de la face sont contenu dans les points "pp"
+ * Les points des sommets de la face sont contenu dans les points "pp"
* de la surface contenant la face.
* Entree :
* fp face a afficher.
* pp Points de la surface contenant la face.
*/
-void wireframe_Face (Face *fp, Point2i *pp)
+void wireframe_Face(Face *fp, Point2i *pp)
{
-// extern Window id_window;
+ // extern Window id_window;
- Index *vp = fp->vertex.ptr;
- Index *vend = vp + fp->vertex.nbr;
- Point2i *cp = listpoint2i;
+ Index *vp = fp->vertex.ptr;
+ Index *vend = vp + fp->vertex.nbr;
+ Point2i *cp = listpoint2i;
- if (fp->vertex.nbr < 2) return;
- if (fp->vertex.nbr > 50)
- {
- printf("pb malloc listpoint2i (display.c)\n"); return;
- }
- for (; vp < vend; vp++, cp++) {
- SET_COORD2(*cp, pp[*vp].x, pp[*vp].y);
- }
+ if (fp->vertex.nbr < 2)
+ return;
+ if (fp->vertex.nbr > 50) {
+ printf("pb malloc listpoint2i (display.c)\n");
+ return;
+ }
+ for (; vp < vend; vp++, cp++) {
+ SET_COORD2(*cp, pp[*vp].x, pp[*vp].y);
+ }
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpCoreDisplay.h b/modules/robot/src/wireframe-simulator/vpCoreDisplay.h
index dd9537e..1187c58 100644
--- a/modules/robot/src/wireframe-simulator/vpCoreDisplay.h
+++ b/modules/robot/src/wireframe-simulator/vpCoreDisplay.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,14 +44,14 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpBound.h"
#include "vpArit.h"
+#include "vpBound.h"
-void open_display (void);
-void close_display (void);
-void point_3D_2D (Point3f *p3, Index size, int xsize, int ysize, Point2i *p2);
-void set_Bound_face_display (Bound *bp, Byte b);
-void wireframe_Face (Face *fp, Point2i *pp);
+void open_display(void);
+void close_display(void);
+void point_3D_2D(Point3f *p3, Index size, int xsize, int ysize, Point2i *p2);
+void set_Bound_face_display(Bound *bp, Byte b);
+void wireframe_Face(Face *fp, Point2i *pp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpImstack.h b/modules/robot/src/wireframe-simulator/vpImstack.h
index 1a0b7d7..e5d8ecd 100644
--- a/modules/robot/src/wireframe-simulator/vpImstack.h
+++ b/modules/robot/src/wireframe-simulator/vpImstack.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,12 +44,12 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void fprintf_imstack (void);
-void load_imstack (void);
-void pop_imstack (void);
-void push_imstack (void);
-void swap_imstack (void);
-void add_imstack (void);
+void fprintf_imstack(void);
+void load_imstack(void);
+void pop_imstack(void);
+void push_imstack(void);
+void swap_imstack(void);
+void add_imstack(void);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpKeyword.cpp b/modules/robot/src/wireframe-simulator/vpKeyword.cpp
index 2d935fa..c39663f 100644
--- a/modules/robot/src/wireframe-simulator/vpKeyword.cpp
+++ b/modules/robot/src/wireframe-simulator/vpKeyword.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,42 +37,36 @@
*
*****************************************************************************/
-
-
-
+#include "vpKeyword.h"
#include "vpMy.h"
#include "vpToken.h"
-#include "vpKeyword.h"
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static void open_hash (void);
-static void close_hash (void);
-static int hashpjw (const char *str);
-static void insert_keyword (const char *str, Index token);
-
-#ifdef debug
-static void delete_keyword (void);
-static char *get_keyword (void);
-#endif /* debug */
+static void open_hash(void);
+static void close_hash(void);
+static int hashpjw(const char *str);
+static void insert_keyword(const char *str, Index token);
+#ifdef debug
+static void delete_keyword(void);
+static char *get_keyword(void);
+#endif /* debug */
-#define PRIME 211
-#define NEXT(x) (x) = (x)->next
+#define PRIME 211
+#define NEXT(x) (x) = (x)->next
-typedef struct bucket {
- struct bucket *next; /* element suivant */
- char *ident; /* identifateur */
- Byte length; /* longueur de "ident" */
- Index token; /* code du jeton */
+typedef struct bucket {
+ struct bucket *next; /* element suivant */
+ char *ident; /* identifateur */
+ Byte length; /* longueur de "ident" */
+ Index token; /* code du jeton */
} Bucket;
-
-static Bucket **hash_tbl; /* table de "hash-coding" */
-
+static Bucket **hash_tbl; /* table de "hash-coding" */
/*
* La procedure "open_keyword" alloue et initialise les variables utilisees
@@ -79,58 +74,54 @@ static Bucket **hash_tbl; /* table de "hash-coding" */
* Entree :
* kwp Tableau des mots cles termine par NULL.
*/
-void open_keyword (Keyword *kwp)
+void open_keyword(Keyword *kwp)
{
- open_hash ();
- for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */
- insert_keyword (kwp->ident, kwp->token);
+ open_hash();
+ for (; kwp->ident != NULL; kwp++) /* recopie les mots cles */
+ insert_keyword(kwp->ident, kwp->token);
}
/*
* La procedure "close_keyword" libere les variables utilisees
* par les procedures de gestion des mots cles.
*/
-void close_keyword (void)
-{
- close_hash ();
-}
+void close_keyword(void) { close_hash(); }
/*
* La procedure "open_hash" alloue et initialise la table de codage.
*/
-static void
-open_hash (void)
+static void open_hash(void)
{
- Bucket **head, **bend;
-
- if ((hash_tbl = (Bucket **) malloc (sizeof (Bucket *) * PRIME))==NULL){
- static char proc_name[] = "open_hash";
- perror (proc_name);
- exit (1);
- }
- head = hash_tbl;
- bend = head + PRIME;
- for (; head < bend; *head++ = NULL) {};
+ Bucket **head, **bend;
+
+ if ((hash_tbl = (Bucket **)malloc(sizeof(Bucket *) * PRIME)) == NULL) {
+ static char proc_name[] = "open_hash";
+ perror(proc_name);
+ exit(1);
+ }
+ head = hash_tbl;
+ bend = head + PRIME;
+ for (; head < bend; *head++ = NULL) {
+ };
}
/*
* La procedure "close_hash" libere la table de codage et ses elements.
*/
-static void
-close_hash (void)
+static void close_hash(void)
{
- Bucket **head = hash_tbl;
- Bucket **bend = head + PRIME;
- Bucket *bp; /* element courant */
- Bucket *next; /* element suivant */
-
- for (; head < bend; head++) { /* libere les listes */
- for (bp = *head; bp != NULL; bp = next) {
- next = bp->next;
- free ((char *) bp);
- }
- }
- free ((char *) hash_tbl); /* libere la table */
+ Bucket **head = hash_tbl;
+ Bucket **bend = head + PRIME;
+ Bucket *bp; /* element courant */
+ Bucket *next; /* element suivant */
+
+ for (; head < bend; head++) { /* libere les listes */
+ for (bp = *head; bp != NULL; bp = next) {
+ next = bp->next;
+ free((char *)bp);
+ }
+ }
+ free((char *)hash_tbl); /* libere la table */
}
/*
@@ -145,23 +136,21 @@ close_hash (void)
* Sortie :
* Le code de la chaine.
*/
-static int
-hashpjw (const char *str)
+static int hashpjw(const char *str)
{
- unsigned h = 0; /* "hash value" */
+ unsigned h = 0; /* "hash value" */
- for (; *str != '\0'; str++) {
- unsigned g;
+ for (; *str != '\0'; str++) {
+ unsigned g;
h = (h << 4) + (unsigned)(*str);
if ((g = h & ~0xfffffff) != 0) {
- h ^= g >> 24;
- h ^= g;
- }
- }
+ h ^= g >> 24;
+ h ^= g;
+ }
+ }
return ((int)(h % PRIME));
}
-
/*
* La procedure "insert_keyword" insere en tete d'un point d'entree
* de la table de "hachage" le mot cle ayant pour identificateur
@@ -170,26 +159,25 @@ hashpjw (const char *str)
* str Chaine de caracteres du mot cle.
* token Valeur du jeton associe au mot cle.
*/
-static void
-insert_keyword (const char *str, Index token)
+static void insert_keyword(const char *str, Index token)
{
- Bucket **head = hash_tbl + hashpjw (str);
- Bucket *bp;
- Byte length;
-
- length = (Byte)( strlen(str) ); // Warning! Overflow possible!
- if ((bp = (Bucket *) malloc (sizeof (Bucket) + length + 1)) == NULL) {
- static const char proc_name[] = "insert_keyword";
- perror (proc_name);
- exit (1);
- }
- bp->length = length;
- bp->token = token;
- bp->ident = (char *) (bp + 1);
- strcpy (bp->ident, str);
-
- bp->next = *head; /* insere "b" en tete de "head" */
- *head = bp;
+ Bucket **head = hash_tbl + hashpjw(str);
+ Bucket *bp;
+ Byte length;
+
+ length = (Byte)(strlen(str)); // Warning! Overflow possible!
+ if ((bp = (Bucket *)malloc(sizeof(Bucket) + length + 1)) == NULL) {
+ static const char proc_name[] = "insert_keyword";
+ perror(proc_name);
+ exit(1);
+ }
+ bp->length = length;
+ bp->token = token;
+ bp->ident = (char *)(bp + 1);
+ strcpy(bp->ident, str);
+
+ bp->next = *head; /* insere "b" en tete de "head" */
+ *head = bp;
}
/*
@@ -203,41 +191,42 @@ insert_keyword (const char *str, Index token)
* Sortie :
* Valeur du jeton associe si c'est un mot cle, 0 sinon.
*/
-Index get_symbol (char *ident, int length)
+Index get_symbol(char *ident, int length)
{
- Bucket *bp;
- const char *kwd;
- char *idn = ident;
- int len = length;
+ Bucket *bp;
+ const char *kwd;
+ char *idn = ident;
+ int len = length;
- { /* calcule le code de hachage (voir "hashpjw") */
- unsigned h = 0; /* "hash value" */
+ { /* calcule le code de hachage (voir "hashpjw") */
+ unsigned h = 0; /* "hash value" */
- for (; len != 0; idn++, len--) {
- unsigned g;
+ for (; len != 0; idn++, len--) {
+ unsigned g;
h = (h << 4) + (unsigned)(*idn);
if ((g = h & ~0xfffffff) != 0) {
- h ^= g >> 24;
- h ^= g;
- }
- }
- bp = hash_tbl[h % PRIME];
- }
-
- /* recherche le mot cle */
-
- for (; bp != NULL; NEXT(bp)) {
- if (length == bp->length) {
- idn = ident;
- len = length;
- kwd = bp->ident;
+ h ^= g >> 24;
+ h ^= g;
+ }
+ }
+ bp = hash_tbl[h % PRIME];
+ }
+
+ /* recherche le mot cle */
+
+ for (; bp != NULL; NEXT(bp)) {
+ if (length == bp->length) {
+ idn = ident;
+ len = length;
+ kwd = bp->ident;
for (; *idn == *kwd; idn++, kwd++) {
--len;
- if (len == 0) return (bp->token);
+ if (len == 0)
+ return (bp->token);
}
- }
- }
- return (0); /* identificateur */
+ }
+ }
+ return (0); /* identificateur */
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpKeyword.h b/modules/robot/src/wireframe-simulator/vpKeyword.h
index a101eeb..46c28e7 100644
--- a/modules/robot/src/wireframe-simulator/vpKeyword.h
+++ b/modules/robot/src/wireframe-simulator/vpKeyword.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,15 +40,15 @@
#ifndef vpKeyword_H
#define vpKeyword_H
-#include <visp3/core/vpConfig.h>
#include "vpMy.h"
#include "vpToken.h"
+#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void open_keyword (Keyword *kwp);
-void close_keyword (void);
-Index get_symbol (char *ident, int length);
+void open_keyword(Keyword *kwp);
+void close_keyword(void);
+Index get_symbol(char *ident, int length);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpLex.cpp b/modules/robot/src/wireframe-simulator/vpLex.cpp
index 72b4f25..a91deb2 100644
--- a/modules/robot/src/wireframe-simulator/vpLex.cpp
+++ b/modules/robot/src/wireframe-simulator/vpLex.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,374 +39,405 @@
*
*****************************************************************************/
-
-
-
+#include "vpKeyword.h"
#include "vpMy.h"
#include "vpToken.h"
-#include "vpKeyword.h"
#include <ctype.h>
-#include <math.h>
-#include <stdio.h>
#include <fcntl.h>
+#include <math.h>
#include <stdarg.h>
+#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static void count (void);
-static void next_source (void);
+static void count(void);
+static void next_source(void);
-void lexerr (const char* path, ...);
+void lexerr(const char *path, ...);
/* Codes des symboles terminaux */
-#define NULT 0 /* caractere non valide */
-#define EOBT 1 /* fin de buffer */
-#define EOFT 2 /* fin de fichier */
-#define EOLT 3 /* fin de ligne */
-#define CMTT 4 /* commentaire */
-#define IDNT 5 /* identificateur */
-#define INTT 6 /* nombre entier */
-#define FPTT 7 /* nombre flottant */
-#define SGNT 8 /* signe +/- */
-#define SPCT 9 /* caractere blanc */
-#define STGT 10 /* caractere de chaine */
-#define NBRT 11 /* nombre de codes */
+#define NULT 0 /* caractere non valide */
+#define EOBT 1 /* fin de buffer */
+#define EOFT 2 /* fin de fichier */
+#define EOLT 3 /* fin de ligne */
+#define CMTT 4 /* commentaire */
+#define IDNT 5 /* identificateur */
+#define INTT 6 /* nombre entier */
+#define FPTT 7 /* nombre flottant */
+#define SGNT 8 /* signe +/- */
+#define SPCT 9 /* caractere blanc */
+#define STGT 10 /* caractere de chaine */
+#define NBRT 11 /* nombre de codes */
/* Drapeaux des caracteres */
-#define _NULT 0x00 /* caractere non valide */
-#define _CMTT 0x01 /* commentaire */
-#define _FPTT 0x02 /* nombre flottant */
-#define _IDNT 0x04 /* identificateur */
-#define _INTT 0x08 /* nombre entier */
-#define _SGNT 0x10 /* signe +/- */
-#define _STGT 0x20 /* caractere de chaine */
+#define _NULT 0x00 /* caractere non valide */
+#define _CMTT 0x01 /* commentaire */
+#define _FPTT 0x02 /* nombre flottant */
+#define _IDNT 0x04 /* identificateur */
+#define _INTT 0x08 /* nombre entier */
+#define _SGNT 0x10 /* signe +/- */
+#define _STGT 0x20 /* caractere de chaine */
/* Caracteres sentinelles */
-#define ASCII_NBR 128 /* nombre de codes ASCII*/
+#define ASCII_NBR 128 /* nombre de codes ASCII*/
#ifndef EOB
-#define EOB (-2) /* fin de buffer */
+#define EOB (-2) /* fin de buffer */
#endif
#ifndef EOF
-#define EOF (-1) /* fin de fichier */
+#define EOF (-1) /* fin de fichier */
#endif
#ifndef EOL
-#define EOL 10 /* fin de ligne */
+#define EOL 10 /* fin de ligne */
#endif
-#define CHAR_NBR 130 /* nombre de caracteres */
+#define CHAR_NBR 130 /* nombre de caracteres */
/* Tests des drapeaux */
-#define isnult(c) (scantbl[c] == _NULT)
-#define iscmtt(c) (scantbl[c] & _CMTT)
-#define isfptt(c) (scantbl[c] & _FPTT)
-#define isidnt(c) (scantbl[c] & _IDNT)
-#define isintt(c) (scantbl[c] & _INTT)
-#define issgnt(c) (scantbl[c] & _SGNT)
-#define isstgt(c) (scantbl[c] & _STGT)
+#define isnult(c) (scantbl[c] == _NULT)
+#define iscmtt(c) (scantbl[c] & _CMTT)
+#define isfptt(c) (scantbl[c] & _FPTT)
+#define isidnt(c) (scantbl[c] & _IDNT)
+#define isintt(c) (scantbl[c] & _INTT)
+#define issgnt(c) (scantbl[c] & _SGNT)
+#define isstgt(c) (scantbl[c] & _STGT)
/*
* Codes des messages d'erreur de l'analyseur lexicale.
*/
-#define E_UNKNOWN 0
-#define E_SYMBOL 1
-#define E_CMT_EOF 2
-#define E_FLOAT 3
-#define E_INT 4
-#define E_KEYWORD 5
-#define E_STG_EOF 6
-#define E_STG_EOL 7
-#define E_STRING 8
-#define E_9 9
-
-
-const char *lex_errtbl[] = { /* table des messages d'erreur */
- "error unknown",
- "symbol undefined",
- "unexpected EOF in comment",
- "float expected",
- "int expected",
- "keyword expected",
- "unexpected EOF in string or char constant",
- "newline in string or char constant",
- "string expected",
- ""
-};
-
-char *mytext = NULL;
-int mylength = 0;
-int mylineno = 1;
-unsigned int mycolumno = 0;
-float myfloat = 0.0;
-int myint = 0;
-
-
-static char *mysptr; /* tete de lecture de la ligne courante */
-static char *myline; /* debut de la ligne courante */
-static char *lastline; /* derniere ligne du buffer d'entree */
-
-static Byte *chtbl; /* premiers caracteres des terminaux */
-static Byte *scantbl; /* caracteres suivants des terminaux */
-
+#define E_UNKNOWN 0
+#define E_SYMBOL 1
+#define E_CMT_EOF 2
+#define E_FLOAT 3
+#define E_INT 4
+#define E_KEYWORD 5
+#define E_STG_EOF 6
+#define E_STG_EOL 7
+#define E_STRING 8
+#define E_9 9
+
+const char *lex_errtbl[] = {/* table des messages d'erreur */
+ "error unknown",
+ "symbol undefined",
+ "unexpected EOF in comment",
+ "float expected",
+ "int expected",
+ "keyword expected",
+ "unexpected EOF in string or char constant",
+ "newline in string or char constant",
+ "string expected",
+ ""};
+
+char *mytext = NULL;
+int mylength = 0;
+int mylineno = 1;
+unsigned int mycolumno = 0;
+float myfloat = 0.0;
+int myint = 0;
+
+static char *mysptr; /* tete de lecture de la ligne courante */
+static char *myline; /* debut de la ligne courante */
+static char *lastline; /* derniere ligne du buffer d'entree */
+
+static Byte *chtbl; /* premiers caracteres des terminaux */
+static Byte *scantbl; /* caracteres suivants des terminaux */
/*
* La procedure "open_lex" alloue et initialise les variables utilisees
* par l'analyseur lexical "lex".
*/
-void open_lex (void)
+void open_lex(void)
{
- if ((chtbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL
- || (scantbl = (Byte *) malloc (CHAR_NBR * sizeof (Byte))) == NULL) {
- static char proc_name[] = "open_lex";
- perror (proc_name);
- exit (1);
- }
- chtbl += 2; /* 2 sentinelles non affichables */
- scantbl += 2;
+ if ((chtbl = (Byte *)malloc(CHAR_NBR * sizeof(Byte))) == NULL ||
+ (scantbl = (Byte *)malloc(CHAR_NBR * sizeof(Byte))) == NULL) {
+ static char proc_name[] = "open_lex";
+ perror(proc_name);
+ exit(1);
+ }
+ chtbl += 2; /* 2 sentinelles non affichables */
+ scantbl += 2;
- /* initialise les premiers caracteres des symboles terminaux */
+ /* initialise les premiers caracteres des symboles terminaux */
for (int i = 0; i < ASCII_NBR; i++) {
- if (isalpha(i)) chtbl[i] = IDNT;
- else if (isdigit(i)) chtbl[i] = INTT;
- else if (isspace(i)) chtbl[i] = SPCT;
- else switch (i) {
- case '"' : chtbl[i] = STGT; break;
- case '+' :
- case '-' : chtbl[i] = SGNT; break;
- case '.' : chtbl[i] = FPTT; break;
- case '/' : chtbl[i] = CMTT; break;
- case '_' : chtbl[i] = IDNT; break;
- default : chtbl[i] = NULT; break;
- }
- }
-
- /* Initialise les sentinelles comme des terminaux. */
-
- chtbl[EOB] = EOBT;
- chtbl[EOF] = EOFT;
- chtbl[EOL] = EOLT;
-
- /* Initialise les caracteres suivants des symboles terminaux. */
+ if (isalpha(i))
+ chtbl[i] = IDNT;
+ else if (isdigit(i))
+ chtbl[i] = INTT;
+ else if (isspace(i))
+ chtbl[i] = SPCT;
+ else
+ switch (i) {
+ case '"':
+ chtbl[i] = STGT;
+ break;
+ case '+':
+ case '-':
+ chtbl[i] = SGNT;
+ break;
+ case '.':
+ chtbl[i] = FPTT;
+ break;
+ case '/':
+ chtbl[i] = CMTT;
+ break;
+ case '_':
+ chtbl[i] = IDNT;
+ break;
+ default:
+ chtbl[i] = NULT;
+ break;
+ }
+ }
+
+ /* Initialise les sentinelles comme des terminaux. */
+
+ chtbl[EOB] = EOBT;
+ chtbl[EOF] = EOFT;
+ chtbl[EOL] = EOLT;
+
+ /* Initialise les caracteres suivants des symboles terminaux. */
for (int i = 0; i < ASCII_NBR; i++) {
- if (isalpha(i)) scantbl[i] = _CMTT|_IDNT|_STGT;
- else if (isdigit(i)) scantbl[i] = _CMTT|_IDNT|_INTT|_STGT;
- else switch (i) {
- case '"' : scantbl[i] = _CMTT; break;
- case '+' :
- case '-' : scantbl[i] = _CMTT|_SGNT|_STGT; break;
- case '.' : scantbl[i] = _CMTT|_FPTT|_STGT; break;
- case '/' : scantbl[i] = _STGT; break;
- case '_' : scantbl[i] = _CMTT|_IDNT|_STGT; break;
- default : scantbl[i] = _CMTT|_STGT; break;
- }
- }
-
- /* Initialise les sentinelles comme des terminaux. */
-
- scantbl[EOB] = _NULT;
- scantbl[EOF] = _NULT;
- scantbl[EOL] = _NULT;
+ if (isalpha(i))
+ scantbl[i] = _CMTT | _IDNT | _STGT;
+ else if (isdigit(i))
+ scantbl[i] = _CMTT | _IDNT | _INTT | _STGT;
+ else
+ switch (i) {
+ case '"':
+ scantbl[i] = _CMTT;
+ break;
+ case '+':
+ case '-':
+ scantbl[i] = _CMTT | _SGNT | _STGT;
+ break;
+ case '.':
+ scantbl[i] = _CMTT | _FPTT | _STGT;
+ break;
+ case '/':
+ scantbl[i] = _STGT;
+ break;
+ case '_':
+ scantbl[i] = _CMTT | _IDNT | _STGT;
+ break;
+ default:
+ scantbl[i] = _CMTT | _STGT;
+ break;
+ }
+ }
+
+ /* Initialise les sentinelles comme des terminaux. */
+
+ scantbl[EOB] = _NULT;
+ scantbl[EOF] = _NULT;
+ scantbl[EOL] = _NULT;
}
/*
* La procedure "close_lex" libere les variables utilisees
* par l'analyseur lexical "lex".
*/
-void close_lex (void)
+void close_lex(void)
{
- free ((char *) (chtbl - 2)); /* voir "open_lex" pour "- 2" */
- free ((char *) (scantbl - 2));
+ free((char *)(chtbl - 2)); /* voir "open_lex" pour "- 2" */
+ free((char *)(scantbl - 2));
}
-
-#define ECHO printf ("%c", *(mysptr))
-#define CURC (*((signed char *)mysptr)) /* caractere courant */
-#define NEXTC (*((signed char *)mysptr+1)) /* caractere suivant */
-#define PREVC (*((signed char *)mysptr-1)) /* caractere precedent */
-
+#define ECHO printf("%c", *(mysptr))
+#define CURC (*((signed char *)mysptr)) /* caractere courant */
+#define NEXTC (*((signed char *)mysptr + 1)) /* caractere suivant */
+#define PREVC (*((signed char *)mysptr - 1)) /* caractere precedent */
/*
* La procedure "lex" contient l'analyseur lexical.
* Note :
- * La tete de lecture (mysptr) n'est pas systematiquement avancee apres lecture.
- * Le caractere courant est celui sous la tete de lecture.
- * Ainsi on accede de maniere symetrique aux caracteres precedent et suivant.
- * Sortie :
+ * La tete de lecture (mysptr) n'est pas systematiquement avancee apres
+ *lecture. Le caractere courant est celui sous la tete de lecture. Ainsi on
+ *accede de maniere symetrique aux caracteres precedent et suivant. Sortie :
* Code du symbole terminale analyse.
*/
-int lex (void)
+int lex(void)
{
-lex_loop :
-
- for (; chtbl[(int)CURC] == SPCT; mysptr++) {}; /* saute les espaces */
-
- switch (chtbl[(int)CURC]) {
-
- case NULT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- return (*mytext);
- break;
- case EOBT :
- next_source ();
- goto lex_loop;
- break;
- case EOFT :
- mytext = mysptr; /* sauvegarde le jeton */
- return (T_EOF);
- break;
- case EOLT :
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto lex_loop;
- break;
- case CMTT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (CURC != '*')
- return (*mytext);
- mysptr++;
-comment :
- for (; iscmtt((int)CURC); mysptr++) {};
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto comment;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
- return (T_EOF);
- break;
- case EOLT :
- if (mysptr == lastline) next_source ();
- else mysptr++;
- mylineno++;
- myline = mysptr;
- goto comment;
- break;
- case CMTT :
- if (PREVC == '*') { /* veritable fin */
- mysptr++;
- goto lex_loop;
- }
- mysptr++; /* pseudo fin */
- goto comment;
- break;
- }
- break;
- case IDNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- for (; isidnt((int)CURC); mysptr++) {};
- mylength = (int)(mysptr - mytext);
- return (get_symbol (mytext, mylength));
- break;
- case INTT :
- mytext = mysptr; /* sauvegarde le jeton */
-int_part :
- myint = (int) (CURC - '0');
- mysptr++;
- for (; isintt((int)CURC); mysptr++)
- myint = myint * 10 + (int) (CURC - '0');
- switch (CURC) {
- case '.' : /* lecture fraction */
-float_part :
- mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
- if (CURC != 'E' && CURC != 'e') {
- myfloat = (float) atof (mytext);
-/* FC
-printf("mytext %s, myfloat %f\n",mytext,myfloat);
-*/
- return (T_FLOAT);
- }
+lex_loop:
+
+ for (; chtbl[(int)CURC] == SPCT; mysptr++) {
+ }; /* saute les espaces */
+
+ switch (chtbl[(int)CURC]) {
+
+ case NULT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ return (*mytext);
+ break;
+ case EOBT:
+ next_source();
+ goto lex_loop;
+ break;
+ case EOFT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ return (T_EOF);
+ break;
+ case EOLT:
+ if (mysptr == lastline)
+ next_source();
+ else
+ mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto lex_loop;
+ break;
+ case CMTT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (CURC != '*')
+ return (*mytext);
+ mysptr++;
+ comment:
+ for (; iscmtt((int)CURC); mysptr++) {
+ };
+ switch (chtbl[(int)CURC]) {
+ case EOBT:
+ next_source();
+ goto comment;
+ break;
+ case EOFT:
+ lexerr("start", lex_errtbl[E_CMT_EOF], NULL);
+ return (T_EOF);
+ break;
+ case EOLT:
+ if (mysptr == lastline)
+ next_source();
+ else
+ mysptr++;
+ mylineno++;
+ myline = mysptr;
+ goto comment;
+ break;
+ case CMTT:
+ if (PREVC == '*') { /* veritable fin */
+ mysptr++;
+ goto lex_loop;
+ }
+ mysptr++; /* pseudo fin */
+ goto comment;
+ break;
+ }
+ break;
+ case IDNT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ for (; isidnt((int)CURC); mysptr++) {
+ };
+ mylength = (int)(mysptr - mytext);
+ return (get_symbol(mytext, mylength));
+ break;
+ case INTT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ int_part:
+ myint = (int)(CURC - '0');
+ mysptr++;
+ for (; isintt((int)CURC); mysptr++)
+ myint = myint * 10 + (int)(CURC - '0');
+ switch (CURC) {
+ case '.': /* lecture fraction */
+ float_part:
+ mysptr++;
+ for (; isintt((int)CURC); mysptr++) {
+ };
+ if (CURC != 'E' && CURC != 'e') {
+ myfloat = (float)atof(mytext);
+ /* FC
+ printf("mytext %s, myfloat %f\n",mytext,myfloat);
+ */
+ return (T_FLOAT);
+ }
+ break;
+ case 'E': /* lecture exposant */
+ case 'e':
+ mysptr++;
+ if (isintt((int)CURC))
+ mysptr++;
+ else if (issgnt((int)CURC) && isintt((int)NEXTC))
+ mysptr += 2;
+ else {
+ mysptr--;
+ myfloat = (float)atof(mytext);
+ return (T_FLOAT);
+ }
+ for (; isintt((int)CURC); mysptr++) {
+ };
+ myfloat = (float)atof(mytext);
+ return (T_FLOAT);
+ break;
+ default:
+ if (*mytext == '-')
+ myint = -myint;
+ return (T_INT);
+ break;
+ }
+ break;
+ case FPTT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (!isintt((int)CURC)) /* pas de fraction */
+ return (*mytext);
+ goto float_part;
+ break;
+ case SGNT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ if (isintt((int)CURC))
+ goto int_part;
+ if (isfptt((int)CURC) && isintt((int)NEXTC))
+ goto float_part;
+ return (*mytext);
+ break;
+ case STGT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ mysptr++;
+ string:
+ for (; isstgt((int)CURC); mysptr++) {
+ };
+ switch (chtbl[(int)CURC]) {
+ case EOBT:
+ next_source();
+ goto string;
+ break;
+ case EOFT:
+ lexerr("start", lex_errtbl[E_STG_EOF], NULL);
+ return ('\n');
+ break;
+ case EOLT:
+ lexerr("start", lex_errtbl[E_STG_EOL], NULL);
+ return ('\n');
break;
- case 'E' : /* lecture exposant */
- case 'e' :
- mysptr++;
- if (isintt((int)CURC))
- mysptr++;
- else if (issgnt((int)CURC) && isintt((int)NEXTC))
- mysptr +=2;
- else {
- mysptr--;
- myfloat = (float) atof (mytext);
- return (T_FLOAT);
- }
- for (; isintt((int)CURC); mysptr++) {};
- myfloat = (float) atof (mytext);
- return (T_FLOAT);
- break;
- default :
- if (*mytext == '-')
- myint = - myint;
- return (T_INT);
- break;
- }
- break;
- case FPTT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (! isintt((int)CURC)) /* pas de fraction */
- return (*mytext);
- goto float_part;
- break;
- case SGNT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
- if (isintt((int)CURC))
- goto int_part;
- if (isfptt((int)CURC) && isintt((int)NEXTC))
- goto float_part;
- return (*mytext);
- break;
- case STGT :
- mytext = mysptr; /* sauvegarde le jeton */
- mysptr++;
-string :
- for (; isstgt((int)CURC); mysptr++) {};
- switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
- goto string;
- break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
- return ('\n');
- break;
- case EOLT :
- lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
- return ('\n');
- break;
- case STGT :
- if (PREVC != '\\') { /* veritable fin */
- mytext++;
- mylength = (int)(mysptr - mytext);
- mysptr++;
- return (T_STRING);
- }
- mysptr++; /* pseudo fin */
- goto string;
- break;
- }
- break;
- default :
- ECHO;
- mysptr++;
- goto lex_loop;
- break;
- }
- return (T_EOF);
+ case STGT:
+ if (PREVC != '\\') { /* veritable fin */
+ mytext++;
+ mylength = (int)(mysptr - mytext);
+ mysptr++;
+ return (T_STRING);
+ }
+ mysptr++; /* pseudo fin */
+ goto string;
+ break;
+ }
+ break;
+ default:
+ ECHO;
+ mysptr++;
+ goto lex_loop;
+ break;
+ }
+ return (T_EOF);
}
/*
@@ -414,187 +446,198 @@ string :
* 2 Affiche le fichier source sur le fichier "f",
* 3 Stoppe devant le jeton "token".
* Note :
- * La tete de lecture (mysptr) n'est pas systematiquement avancee apres lecture.
- * Le caractere courant est celui sous la tete de lecture.
- * Ainsi on accede de maniere symetrique aux caracteres precedent et suivant.
- * Entree :
+ * La tete de lecture (mysptr) n'est pas systematiquement avancee apres
+ *lecture. Le caractere courant est celui sous la tete de lecture. Ainsi on
+ *accede de maniere symetrique aux caracteres precedent et suivant. Entree :
* f Fichier en sortie.
* token Jeton de fin de rechercher.
* Sortie :
* Code du symbole terminale analyse.
*/
-int lexecho (FILE *f, int token)
+int lexecho(FILE *f, int token)
{
-lex_loop :
- for (; chtbl[(int)CURC] == SPCT; mysptr++) /* saute les espaces */
- fwrite (mysptr, 1, 1, f);
+lex_loop:
+ for (; chtbl[(int)CURC] == SPCT; mysptr++) /* saute les espaces */
+ fwrite(mysptr, 1, 1, f);
switch (chtbl[(int)CURC]) {
- case NULT :
- mytext = mysptr; /* sauvegarde le jeton */
+ case NULT:
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
if (token != *mytext)
- fwrite (mytext, 1, 1, f);
+ fwrite(mytext, 1, 1, f);
return (*mytext);
break;
- case EOBT :
- next_source ();
+ case EOBT:
+ next_source();
goto lex_loop;
break;
- case EOFT :
- mytext = mysptr; /* sauvegarde le jeton */
+ case EOFT:
+ mytext = mysptr; /* sauvegarde le jeton */
return (T_EOF);
break;
- case EOLT :
- fwrite (mysptr, 1, 1, f);
- if (mysptr == lastline) next_source ();
- else mysptr++;
+ case EOLT:
+ fwrite(mysptr, 1, 1, f);
+ if (mysptr == lastline)
+ next_source();
+ else
+ mysptr++;
mylineno++;
myline = mysptr;
goto lex_loop;
break;
- case CMTT :
- fwrite (mysptr, 1, 1, f);
- mytext = mysptr; /* sauvegarde le jeton */
+ case CMTT:
+ fwrite(mysptr, 1, 1, f);
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
if (CURC != '*')
return (*mytext);
- fwrite (mysptr, 1, 1, f);
+ fwrite(mysptr, 1, 1, f);
mysptr++;
-comment :
+ comment:
for (; iscmtt((int)CURC); mysptr++)
- fwrite (mysptr, 1, 1, f);
+ fwrite(mysptr, 1, 1, f);
switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
+ case EOBT:
+ next_source();
goto comment;
break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_CMT_EOF], NULL);
+ case EOFT:
+ lexerr("start", lex_errtbl[E_CMT_EOF], NULL);
return (T_EOF);
break;
- case EOLT :
- fwrite (mysptr, 1, 1, f);
- if (mysptr == lastline) next_source ();
- else mysptr++;
+ case EOLT:
+ fwrite(mysptr, 1, 1, f);
+ if (mysptr == lastline)
+ next_source();
+ else
+ mysptr++;
mylineno++;
myline = mysptr;
goto comment;
break;
- case CMTT :
- fwrite (mysptr, 1, 1, f);
- if (PREVC == '*') { /* veritable fin */
+ case CMTT:
+ fwrite(mysptr, 1, 1, f);
+ if (PREVC == '*') { /* veritable fin */
mysptr++;
goto lex_loop;
}
- mysptr++; /* pseudo fin */
+ mysptr++; /* pseudo fin */
goto comment;
break;
}
break;
- case IDNT :
- mytext = mysptr; /* sauvegarde le jeton */
+ case IDNT:
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
- for (; isidnt((int)CURC); mysptr++) {};
+ for (; isidnt((int)CURC); mysptr++) {
+ };
mylength = (int)(mysptr - mytext);
- if (token != get_symbol (mytext, mylength))
- fwrite (mytext, (size_t)mylength, 1, f);
- return (get_symbol (mytext, mylength));
+ if (token != get_symbol(mytext, mylength))
+ fwrite(mytext, (size_t)mylength, 1, f);
+ return (get_symbol(mytext, mylength));
break;
- case INTT :
- mytext = mysptr; /* sauvegarde le jeton */
-int_part :
+ case INTT:
+ mytext = mysptr; /* sauvegarde le jeton */
+ int_part:
mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
+ for (; isintt((int)CURC); mysptr++) {
+ };
switch (CURC) {
- case '.' : /* lecture fraction */
-float_part :
+ case '.': /* lecture fraction */
+ float_part:
mysptr++;
- for (; isintt((int)CURC); mysptr++) {};
+ for (; isintt((int)CURC); mysptr++) {
+ };
if (CURC != 'E' && CURC != 'e') {
if (token != T_FLOAT)
- fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ fwrite(mytext, (size_t)(mysptr - mytext), 1, f);
return (T_FLOAT);
}
break;
- case 'E' : /* lecture exposant */
- case 'e' :
+ case 'E': /* lecture exposant */
+ case 'e':
mysptr++;
- if (isintt((int)CURC)) mysptr++;
- else if (issgnt((int)CURC) && isintt((int)NEXTC)) mysptr +=2;
+ if (isintt((int)CURC))
+ mysptr++;
+ else if (issgnt((int)CURC) && isintt((int)NEXTC))
+ mysptr += 2;
else {
mysptr--;
if (token != T_FLOAT)
- fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ fwrite(mytext, (size_t)(mysptr - mytext), 1, f);
return (T_FLOAT);
}
- for (; isintt((int)CURC); mysptr++) {};
+ for (; isintt((int)CURC); mysptr++) {
+ };
if (token != T_FLOAT)
- fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ fwrite(mytext, (size_t)(mysptr - mytext), 1, f);
return (T_FLOAT);
break;
- default :
+ default:
if (token != T_INT)
- fwrite (mytext, (size_t)(mysptr - mytext), 1, f);
+ fwrite(mytext, (size_t)(mysptr - mytext), 1, f);
return (T_INT);
break;
}
break;
- case FPTT :
- mytext = mysptr; /* sauvegarde le jeton */
+ case FPTT:
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
- if (! isintt((int)CURC)) { /* pas de fraction */
+ if (!isintt((int)CURC)) { /* pas de fraction */
if (token != *mytext)
- fwrite (mytext, 1, 1, f);
+ fwrite(mytext, 1, 1, f);
return (*mytext);
}
goto float_part;
break;
- case SGNT :
- mytext = mysptr; /* sauvegarde le jeton */
+ case SGNT:
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
- if (isintt((int)CURC)) goto int_part;
- if (isfptt((int)CURC) && isintt((int)NEXTC)) goto float_part;
+ if (isintt((int)CURC))
+ goto int_part;
+ if (isfptt((int)CURC) && isintt((int)NEXTC))
+ goto float_part;
if (token != *mytext)
- fwrite (mytext, 1, 1, f);
+ fwrite(mytext, 1, 1, f);
return (*mytext);
break;
- case STGT :
- fwrite (mysptr, 1, 1, f);
- mytext = mysptr; /* sauvegarde le jeton */
+ case STGT:
+ fwrite(mysptr, 1, 1, f);
+ mytext = mysptr; /* sauvegarde le jeton */
mysptr++;
-string :
+ string:
for (; isstgt((int)CURC); mysptr++)
- fwrite (mysptr, 1, 1, f);
+ fwrite(mysptr, 1, 1, f);
switch (chtbl[(int)CURC]) {
- case EOBT :
- next_source ();
+ case EOBT:
+ next_source();
goto comment;
break;
- case EOFT :
- lexerr ("start", lex_errtbl[E_STG_EOF], NULL);
+ case EOFT:
+ lexerr("start", lex_errtbl[E_STG_EOF], NULL);
return (T_EOF);
break;
- case EOLT :
- lexerr ("start", lex_errtbl[E_STG_EOL], NULL);
+ case EOLT:
+ lexerr("start", lex_errtbl[E_STG_EOL], NULL);
return ('\n');
break;
- case STGT :
- fwrite (mysptr, 1, 1, f);
- if (PREVC != '\\') { /* veritable fin */
+ case STGT:
+ fwrite(mysptr, 1, 1, f);
+ if (PREVC != '\\') { /* veritable fin */
mytext++;
mylength = (int)(mysptr - mytext);
mysptr++;
return (T_STRING);
}
- mysptr++; /* pseudo fin */
+ mysptr++; /* pseudo fin */
goto string;
break;
}
break;
- default :
- fwrite (mysptr, 1, 1, f);
+ default:
+ fwrite(mysptr, 1, 1, f);
mysptr++;
goto lex_loop;
break;
@@ -602,30 +645,24 @@ string :
return (T_EOF);
}
+#undef BUFSIZE
+#undef LINESIZE
+#undef TEXTSIZE
-#undef BUFSIZE
-#undef LINESIZE
-#undef TEXTSIZE
-
-#define BUFSIZE (BUFSIZ << 5)
-#define LINESIZE (BUFSIZ-1)
-#define TEXTSIZE (1 + LINESIZE + BUFSIZE + 1)
-
-
-static FILE *fds; /* descripteur du fichier source */
-static char *source; /* nom du fichier du programme source */
-static char *botbuf; /* fond du buffer d'entree du fichier */
-static char *buf; /* base du buffer d'entree du fichier */
-static char *topbuf; /* sommet du buffer d'entree du fichier */
+#define BUFSIZE (BUFSIZ << 5)
+#define LINESIZE (BUFSIZ - 1)
+#define TEXTSIZE (1 + LINESIZE + BUFSIZE + 1)
+static FILE *fds; /* descripteur du fichier source */
+static char *source; /* nom du fichier du programme source */
+static char *botbuf; /* fond du buffer d'entree du fichier */
+static char *buf; /* base du buffer d'entree du fichier */
+static char *topbuf; /* sommet du buffer d'entree du fichier */
/*
* La procedure "unlex" recule la tete de lecture devant le dernier jeton.
*/
-void unlex (void)
-{
- mysptr = mytext;
-}
+void unlex(void) { mysptr = mytext; }
/*
* La procedure "open_source" alloue et initialise les variables utilisees
@@ -634,91 +671,89 @@ void unlex (void)
* fd Fichier du programme source.
* sptr Nom du fichier du programme source.
*/
-void open_source (FILE *fd, const char *str)
+void open_source(FILE *fd, const char *str)
{
- if ((source = (char *) malloc((strlen(str)+1)*sizeof(char))) == NULL) {
- static char proc_name[] = "open_source";
- perror (proc_name);
- exit (1);
- }
- strcpy (source, str);
- if ((botbuf = (char *) malloc (TEXTSIZE * sizeof (char))) == NULL) {
- static char proc_name[] = "open_source";
- perror (proc_name);
- exit (1);
- }
- fds = fd;
- buf = botbuf + 1 + LINESIZE;
- topbuf = buf + 1;
- mylineno = 1;
- next_source ();
+ if ((source = (char *)malloc((strlen(str) + 1) * sizeof(char))) == NULL) {
+ static char proc_name[] = "open_source";
+ perror(proc_name);
+ exit(1);
+ }
+ strcpy(source, str);
+ if ((botbuf = (char *)malloc(TEXTSIZE * sizeof(char))) == NULL) {
+ static char proc_name[] = "open_source";
+ perror(proc_name);
+ exit(1);
+ }
+ fds = fd;
+ buf = botbuf + 1 + LINESIZE;
+ topbuf = buf + 1;
+ mylineno = 1;
+ next_source();
}
/*
* La procedure "close_source" libere les variables utilisees pour la gestion
* des entrees du programme source.
*/
-void close_source (void)
+void close_source(void)
{
- free ((char *) source);
- free ((char *) botbuf);
+ free((char *)source);
+ free((char *)botbuf);
}
/*
* La procedure "next_source" remplit le buffer courant.
*/
-static void next_source (void)
+static void next_source(void)
{
- size_t size;
- char *bot = buf;
- char *top = topbuf;
-
- /* recopie la derniere ligne devant "buf" */
-
- *bot = EOL; /* evite le debordement de "buf" */
- while ((*--bot = *--top) != EOL) {};
- myline = mysptr = bot + 1;
-
- size = fread (buf,sizeof (char), BUFSIZE,fds);
- if (size == 0) {
- topbuf = buf + 1;
- *buf = EOF;
- *topbuf = EOB; /* sentinelle de fin de fichier */
- mysptr = buf;
- }
- else {
- topbuf = buf + size;
- *topbuf = EOB; /* sentinelle de fin de buffer */
-
- /* recherche de la derniere ligne */
- top = topbuf;
- while (*--top != EOL) {};
- lastline = top;
- }
+ size_t size;
+ char *bot = buf;
+ char *top = topbuf;
+
+ /* recopie la derniere ligne devant "buf" */
+
+ *bot = EOL; /* evite le debordement de "buf" */
+ while ((*--bot = *--top) != EOL) {
+ };
+ myline = mysptr = bot + 1;
+
+ size = fread(buf, sizeof(char), BUFSIZE, fds);
+ if (size == 0) {
+ topbuf = buf + 1;
+ *buf = EOF;
+ *topbuf = EOB; /* sentinelle de fin de fichier */
+ mysptr = buf;
+ } else {
+ topbuf = buf + size;
+ *topbuf = EOB; /* sentinelle de fin de buffer */
+
+ /* recherche de la derniere ligne */
+ top = topbuf;
+ while (*--top != EOL) {
+ };
+ lastline = top;
+ }
}
-
/*
* ERR_STACK : Pile des messages d'erreur.
* La pile est geree par les procedures "poperr", "popuperr" et "pusherr".
* Les messages sont affiches par les procedures "count" et "lexerr".
*/
-#define ERR_STACK_MAX 32
-
-
-static const char *err_stack[ERR_STACK_MAX];
-static int size_stack = 0;
+#define ERR_STACK_MAX 32
+static const char *err_stack[ERR_STACK_MAX];
+static int size_stack = 0;
/*
* La procedure "count" calcule la distance en espaces entre
* le premier caractere "*mytext" et le caractere de debut de ligne "*myline".
*/
-static void count (void)
+static void count(void)
{
- char *str;
+ char *str;
- mycolumno = 0;
+ mycolumno = 0;
for (str = myline; str <= mytext; str++) {
(*str == '\t') ? mycolumno += 8 - (mycolumno % 8) : mycolumno++;
}
@@ -734,74 +769,73 @@ static void count (void)
* va_list Liste de messages d'erreur terminee par NULL.
*/
-//lexerr (va_alist)
-//va_dcl
+// lexerr (va_alist)
+// va_dcl
-void lexerr (const char* path, ...)
+void lexerr(const char *path, ...)
{
- va_list ap;
- char *cp;
- int i;
+ va_list ap;
+ char *cp;
+ int i;
- /* Pointe sur le caractere fautif. */
+ /* Pointe sur le caractere fautif. */
- count ();
- //write (STDERR, myline, mysptr - myline);
- fprintf (stderr, "\n%*c\n\"%s\", line %d:\n",
- mycolumno, '^', source, mylineno);
+ count();
+ // write (STDERR, myline, mysptr - myline);
+ fprintf(stderr, "\n%*c\n\"%s\", line %d:\n", mycolumno, '^', source, mylineno);
- /* Affiche les messages d'erreur de la pile. */
+ /* Affiche les messages d'erreur de la pile. */
- for (i = 0; i < size_stack; i++)
- fprintf (stderr, "%s", err_stack[i]);
+ for (i = 0; i < size_stack; i++)
+ fprintf(stderr, "%s", err_stack[i]);
- /* Affiche les messages d'erreur en parametres. */
+ /* Affiche les messages d'erreur en parametres. */
- va_start(ap,path);
- while ((cp = (char *) va_arg(ap, char *)) != NULL)
- fprintf (stderr, "%s", cp);
- fprintf (stderr,"\n");
- va_end(ap);
+ va_start(ap, path);
+ while ((cp = (char *)va_arg(ap, char *)) != NULL)
+ fprintf(stderr, "%s", cp);
+ fprintf(stderr, "\n");
+ va_end(ap);
- exit (1);
+ exit(1);
}
/*
* La procedure "poperr" depile le message d'erreur du sommet de pile.
*/
-void poperr (void)
+void poperr(void)
{
- if (--size_stack < 0) {
- static char proc_name[] = "poperr";
- fprintf (stderr, "%s: error stack underflow\n", proc_name);
- exit (1);
- }
+ if (--size_stack < 0) {
+ static char proc_name[] = "poperr";
+ fprintf(stderr, "%s: error stack underflow\n", proc_name);
+ exit(1);
+ }
}
/*
* La procedure "popup_error" remplace le message d'erreur du sommet de pile.
*/
-void popuperr (const char *str)
+void popuperr(const char *str)
{
- if (size_stack <= 0) {
- static const char proc_name[] = "popuerr";
- fprintf (stderr, "%s: error stack underflow\n", proc_name);
- exit (1);
- }
- err_stack[size_stack-1] = str;
+ if (size_stack <= 0) {
+ static const char proc_name[] = "popuerr";
+ fprintf(stderr, "%s: error stack underflow\n", proc_name);
+ exit(1);
+ }
+ err_stack[size_stack - 1] = str;
}
/*
* La procedure "pusherr" empile le message d'erreur.
*/
-void pusherr (const char *str)
+void pusherr(const char *str)
{
- if (size_stack >= ERR_STACK_MAX) {
- static const char proc_name[] = "pusherr";
- fprintf (stderr, "%s: error stack overflow\n", proc_name);
- exit (1);
- }
- err_stack[size_stack++] = str;
+ if (size_stack >= ERR_STACK_MAX) {
+ static const char proc_name[] = "pusherr";
+ fprintf(stderr, "%s: error stack overflow\n", proc_name);
+ exit(1);
+ }
+ err_stack[size_stack++] = str;
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpLex.h b/modules/robot/src/wireframe-simulator/vpLex.h
index 98e78fd..cc5087b 100644
--- a/modules/robot/src/wireframe-simulator/vpLex.h
+++ b/modules/robot/src/wireframe-simulator/vpLex.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,22 +42,22 @@
#ifndef vpLex_h
#define vpLex_h
-#include <visp3/core/vpConfig.h>
#include <stdio.h>
+#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void open_lex (void);
-void close_lex (void);
+void open_lex(void);
+void close_lex(void);
int lex(void);
-void unlex (void);
-void open_source (FILE *fd, const char *str);
-void close_source (void);
-void lexerr (const char* path, ...);
-void pusherr (const char *str);
-void popuperr (const char *str);
-void poperr (void);
-int lexecho (FILE *f, int token);
+void unlex(void);
+void open_source(FILE *fd, const char *str);
+void close_source(void);
+void lexerr(const char *path, ...);
+void pusherr(const char *str);
+void popuperr(const char *str);
+void poperr(void);
+int lexecho(FILE *f, int token);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpMy.h b/modules/robot/src/wireframe-simulator/vpMy.h
index 3a2d90a..6b971a6 100644
--- a/modules/robot/src/wireframe-simulator/vpMy.h
+++ b/modules/robot/src/wireframe-simulator/vpMy.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,46 +44,53 @@
#include <visp3/robot/vpWireFrameSimulatorTypes.h>
-#ifndef NULL
-#define NULL 0
+#ifndef NULL
+#define NULL 0
#endif
-#ifndef FALSE
-#define FALSE 0
+#ifndef FALSE
+#define FALSE 0
#endif
-#ifndef TRUE
-#define TRUE 1
+#ifndef TRUE
+#define TRUE 1
#endif
-#ifndef STDIN
-#define STDIN 0
+#ifndef STDIN
+#define STDIN 0
#endif
-#ifndef STDOUT
-#define STDOUT 1
+#ifndef STDOUT
+#define STDOUT 1
#endif
-#ifndef STDERR
-#define STDERR 2
+#ifndef STDERR
+#define STDERR 2
#endif
-#define NAMESIZE 80
-#define LINESIZE 256
+#define NAMESIZE 80
+#define LINESIZE 256
-
-#define M_EPSILON 1E-06
+#define M_EPSILON 1E-06
//#define ABS(X) (((X) < 0) ? -(X) : (X))
-#define FABS(X) (((X) < 0.0) ? -(X) : (X))
+#define FABS(X) (((X) < 0.0) ? -(X) : (X))
//#define MAX(A,B) (((A) > (B)) ? (A) : (B))
//#define MAX3(A,B,C) (MAX(MAX(A,B),C))
//#define MIN(A,B) (((A) < (B)) ? (A) : (B))
//#define MIN3(A,B,C) (MIN(MIN(A,B),C))
-#define MIN_MAX(M,MIN,MAX) if ((M) < (MIN)) (MIN) = (M);\
- else if ((M) > (MAX)) (MAX) = (M)
+#define 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 TWO_POWER(P) (((P) > 0) ? 1 << (P) : 1)
+#define SWAP(A, B, T) \
+ { \
+ (T) = (A); \
+ (A) = (B); \
+ (B) = (T); \
+ }
-typedef unsigned char Byte;
+typedef unsigned char Byte;
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpMyio.cpp b/modules/robot/src/wireframe-simulator/vpMyio.cpp
index 05f99da..e061eb5 100644
--- a/modules/robot/src/wireframe-simulator/vpMyio.cpp
+++ b/modules/robot/src/wireframe-simulator/vpMyio.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,12 +40,9 @@
*
*****************************************************************************/
-
-
-
#include "vpMyio.h"
-#include "vpToken.h"
#include "vpLex.h"
+#include "vpToken.h"
#include <stdio.h>
#include <stdlib.h>
@@ -52,21 +50,20 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-extern char *mytext; /* chaine du symbole courant */
-
+extern char *mytext; /* chaine du symbole courant */
/*
* La procedure "fscanf_float" lit en ascii un nombre flottant.
* Entree :
* fp Nombre flottant a lire.
*/
-void fscanf_float (float *fp)
+void fscanf_float(float *fp)
{
- int t;
+ int t;
- if ((t = lex ()) != T_FLOAT && t != T_INT)
- lexerr ("start", "float expected", NULL);
- *fp = (t == T_INT) ? (float) myint : myfloat;
+ if ((t = lex()) != T_FLOAT && t != T_INT)
+ lexerr("start", "float expected", NULL);
+ *fp = (t == T_INT) ? (float)myint : myfloat;
}
/*
@@ -74,11 +71,11 @@ void fscanf_float (float *fp)
* Entree :
* ip Indice a lire.
*/
-void fscanf_Index (Index *ip)
+void fscanf_Index(Index *ip)
{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = (Index) myint;
+ if (lex() != T_INT)
+ lexerr("start", "integer expected", NULL);
+ *ip = (Index)myint;
}
/*
@@ -86,11 +83,11 @@ void fscanf_Index (Index *ip)
* Entree :
* ip Nombre entier a lire.
*/
-void fscanf_int (int *ip)
+void fscanf_int(int *ip)
{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = myint;
+ if (lex() != T_INT)
+ lexerr("start", "integer expected", NULL);
+ *ip = myint;
}
/*
@@ -98,21 +95,21 @@ void fscanf_int (int *ip)
* Entree :
* str Chaine a lire.
*/
-void fscanf_string (char **str)
+void fscanf_string(char **str)
{
- if (lex () != T_STRING)
- lexerr ("start", "string expected", NULL);
- if (*str == NULL)
- *str = (char *) malloc ((size_t)(mylength + 1) * sizeof (char));
+ if (lex() != T_STRING)
+ lexerr("start", "string expected", NULL);
+ if (*str == NULL)
+ *str = (char *)malloc((size_t)(mylength + 1) * sizeof(char));
else
- *str = (char *) realloc (*str, (size_t)(mylength + 1) * sizeof (char));
+ *str = (char *)realloc(*str, (size_t)(mylength + 1) * sizeof(char));
if (*str == NULL) {
printf("Unable to read the string: bad memory allocation");
return;
}
- strncpy (*str, mytext, (size_t)mylength);
+ strncpy(*str, mytext, (size_t)mylength);
}
/*
@@ -120,11 +117,11 @@ void fscanf_string (char **str)
* Entree :
* ip Type a lire.
*/
-void fscanf_Type (Type *ip)
+void fscanf_Type(Type *ip)
{
- if (lex () != T_INT)
- lexerr ("start", "integer expected", NULL);
- *ip = (Type ) myint;
+ if (lex() != T_INT)
+ lexerr("start", "integer expected", NULL);
+ *ip = (Type)myint;
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpMyio.h b/modules/robot/src/wireframe-simulator/vpMyio.h
index 730519c..6b22dfc 100644
--- a/modules/robot/src/wireframe-simulator/vpMyio.h
+++ b/modules/robot/src/wireframe-simulator/vpMyio.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,11 +49,11 @@
#include "vpMy.h"
-void fscanf_float (float *fp);
-void fscanf_Index (Index *ip);
-void fscanf_int (int *ip);
-void fscanf_string (char **str);
-void fscanf_Type (Type *ip);
+void fscanf_float(float *fp);
+void fscanf_Index(Index *ip);
+void fscanf_int(int *ip);
+void fscanf_string(char **str);
+void fscanf_Type(Type *ip);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpParser.cpp b/modules/robot/src/wireframe-simulator/vpParser.cpp
index f85a04a..d4a5382 100644
--- a/modules/robot/src/wireframe-simulator/vpParser.cpp
+++ b/modules/robot/src/wireframe-simulator/vpParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,12 +39,12 @@
*****************************************************************************/
#include "vpParser.h"
+#include "vpBoundio.h"
#include "vpLex.h"
-#include "vpToken.h"
#include "vpSkipio.h"
-#include "vpBoundio.h"
+#include "vpToken.h"
-#include <stdio.h>
+#include <stdio.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
/*
@@ -51,47 +52,46 @@
* Entree/Sortie :
* bsp Scene surfacique polygonale a lire.
*/
-void parser (Bound_scene *bsp)
+void parser(Bound_scene *bsp)
{
- int token;
+ int token;
- while ((token = lex ()) != T_EOF)
- switch (token) {
- case '$' :
- switch (lex ()) {
- case T_IDENT : /* saute la commande inconnue */
- skip_cmd (/* stderr */);
- unlex ();
- break;
- case T_EXIT :
- return;
- break;
- case T_BOUND :
- if (bsp->bound.nbr == BOUND_NBR) {
- fprintf (stderr, "mire: too much bound\n");
- return;
- }
- fscanf_Bound (
- &(bsp->bound.ptr[bsp->bound.nbr++]));
- break;
-#ifdef used
- case T_REMOVE :
- fscanf_Remove (get_remove ());
- break;
- case T_VIEW :
- fscanf_View_parameters (get_view_parameters ());
- set_projection (void);
- break;
-#endif /* used */
- default :
- lexerr ("start", "keyword expected", NULL);
- break;
- }
- break;
- default :
- lexerr ("start", "symbol '$' expected", NULL);
- break;
- }
+ while ((token = lex()) != T_EOF)
+ switch (token) {
+ case '$':
+ switch (lex()) {
+ case T_IDENT: /* saute la commande inconnue */
+ skip_cmd(/* stderr */);
+ unlex();
+ break;
+ case T_EXIT:
+ return;
+ break;
+ case T_BOUND:
+ if (bsp->bound.nbr == BOUND_NBR) {
+ fprintf(stderr, "mire: too much bound\n");
+ return;
+ }
+ fscanf_Bound(&(bsp->bound.ptr[bsp->bound.nbr++]));
+ break;
+#ifdef used
+ case T_REMOVE:
+ fscanf_Remove(get_remove());
+ break;
+ case T_VIEW:
+ fscanf_View_parameters(get_view_parameters());
+ set_projection(void);
+ break;
+#endif /* used */
+ default:
+ lexerr("start", "keyword expected", NULL);
+ break;
+ }
+ break;
+ default:
+ lexerr("start", "symbol '$' expected", NULL);
+ break;
+ }
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpParser.h b/modules/robot/src/wireframe-simulator/vpParser.h
index b3fbb68..62c9ae2 100644
--- a/modules/robot/src/wireframe-simulator/vpParser.h
+++ b/modules/robot/src/wireframe-simulator/vpParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,7 +47,7 @@
#include "vpBound.h"
-void parser (Bound_scene *bsp);
+void parser(Bound_scene *bsp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpProjection.cpp b/modules/robot/src/wireframe-simulator/vpProjection.cpp
index a36b8f7..fe42bc2 100644
--- a/modules/robot/src/wireframe-simulator/vpProjection.cpp
+++ b/modules/robot/src/wireframe-simulator/vpProjection.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,13 +37,12 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include "vpProjection.h"
-#include <stdio.h>
#include <math.h>
+#include <stdio.h>
/*
* La procedure "View_to_Matrix" constuit la matrice homogene de projection
@@ -51,25 +51,24 @@
* vp Parametres de la prise de vue.
* m Matrice homogene a construire.
*/
-void View_to_Matrix (View_parameters *vp, Matrix m)
+void View_to_Matrix(View_parameters *vp, Matrix m)
{
- static char proc_name[] = "View_to_Matrix";
+ static char proc_name[] = "View_to_Matrix";
- switch (vp->type) {
- case PARALLEL :
- set_parallel (vp, m);
- break;
- case PERSPECTIVE :
- set_perspective (vp, m);
- break;
- default :
- fprintf (stderr, "%s: bad view type\n", proc_name);
- set_perspective (vp, m);
- break;
- }
+ switch (vp->type) {
+ case PARALLEL:
+ set_parallel(vp, m);
+ break;
+ case PERSPECTIVE:
+ set_perspective(vp, m);
+ break;
+ default:
+ fprintf(stderr, "%s: bad view type\n", proc_name);
+ set_perspective(vp, m);
+ break;
+ }
}
-
/*
* La procedure "set_zy" initialise la matrice par une composition :
* 1 - aligne le premier vecteur sur l'axe Z dans le sens negatif.
@@ -79,20 +78,32 @@ void View_to_Matrix (View_parameters *vp, Matrix m)
* v0 Premier vecteur.
* v1 Second vecteur.
*/
-static void set_zy (Matrix m, Vector *v0, Vector *v1)
+static void set_zy(Matrix m, Vector *v0, Vector *v1)
{
- Vector rx, ry, rz;
+ Vector rx, ry, rz;
- SET_COORD3(rz, - v0->x,- v0->y, - v0->z);
- CROSS_PRODUCT(rx, *v0, *v1);
- norm_vector (&rx);
- norm_vector (&rz);
- CROSS_PRODUCT (ry,rz,rx); /* ry est norme */
+ SET_COORD3(rz, -v0->x, -v0->y, -v0->z);
+ CROSS_PRODUCT(rx, *v0, *v1);
+ norm_vector(&rx);
+ norm_vector(&rz);
+ CROSS_PRODUCT(ry, rz, rx); /* ry est norme */
- m[0][0] = rx.x; m[0][1] = ry.x; m[0][2] = rz.x; m[0][3] = 0.0;
- m[1][0] = rx.y; m[1][1] = ry.y; m[1][2] = rz.y; m[1][3] = 0.0;
- m[2][0] = rx.z; m[2][1] = ry.z; m[2][2] = rz.z; m[2][3] = 0.0;
- m[3][0] = 0.0 ; m[3][1] = 0.0 ; m[3][2] = 0.0 ; m[3][3] = 1.0;
+ m[0][0] = rx.x;
+ m[0][1] = ry.x;
+ m[0][2] = rz.x;
+ m[0][3] = 0.0;
+ m[1][0] = rx.y;
+ m[1][1] = ry.y;
+ m[1][2] = rz.y;
+ m[1][3] = 0.0;
+ m[2][0] = rx.z;
+ m[2][1] = ry.z;
+ m[2][2] = rz.z;
+ m[2][3] = 0.0;
+ m[3][0] = 0.0;
+ m[3][1] = 0.0;
+ m[3][2] = 0.0;
+ m[3][3] = 1.0;
}
/*
@@ -105,64 +116,57 @@ static void set_zy (Matrix m, Vector *v0, Vector *v1)
* vp Parametres de visualisation.
* wc Matrice a initialiser.
*/
-void set_parallel (View_parameters *vp, Matrix wc)
+void set_parallel(View_parameters *vp, Matrix wc)
{
- Matrix m = IDENTITY_MATRIX;
- Point3f cop;
- Point4f doprim;
- Vector dop, v;
+ Matrix m = IDENTITY_MATRIX;
+ Point3f cop;
+ Point4f doprim;
+ Vector dop, v;
- /*
- * 1 : Translation du point de reference VRP a l'origine.
- */
- SET_COORD3(v,- vp->vrp.x, - vp->vrp.y, - vp->vrp.z);
- Translate_to_Matrix (&v, wc);
- /*
- * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
- * 3 : Rotation pour rendre la projection de VUP sur le plan de
- * projection parallele a l'axe Y.
- */
- set_zy (m, &vp->vpn, &vp->vup);
- /*
- * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
- */
- postleft_matrix (m, 'z');
- postmult_matrix (wc, m);
- /*
- * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
- * COP = DOP = Direction of Projection.
- * DOPRIM = DOP * R_TRL
- * Pas de translation dans la matrice R_TRL pour la transformation
- * du vecteur DOP.
- */
- SET_COORD3(dop,
- vp->vrp.x - vp->cop.x,
- vp->vrp.y - vp->cop.y,
- vp->vrp.z - vp->cop.z);
- norm_vector (&dop);
- SET_COORD3(cop,dop.x,dop.y,dop.z);
- point_matrix (&doprim, &cop, m);
- ident_matrix (m);
- m[2][0] = - doprim.x / doprim.z;
- m[2][1] = - doprim.y / doprim.z;
- postmult_matrix (wc, m);
- /*
- * 6 : Translation et Mise a l'echelle de la pyramide.
- * Remarque : contrairement a la reference qui donne
- * 0 < x < 1, 0 < y < 1, 0 < z < 1
- * je prefere, afin de rester coherent avec la projection perspective,
- * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1)
- */
- SET_COORD3(v,
- (float)(-(vp->vwd.umax + vp->vwd.umin) / 2.0),
- (float)(-(vp->vwd.vmax + vp->vwd.vmin) / 2.0),
- (float)(-vp->depth.front));
- posttrans_matrix (wc, &v);
- SET_COORD3(v,
- (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)),
- (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
- (float)(1.0 / (vp->depth.back - vp->depth.front)));
- postscale_matrix (wc, &v);
+ /*
+ * 1 : Translation du point de reference VRP a l'origine.
+ */
+ SET_COORD3(v, -vp->vrp.x, -vp->vrp.y, -vp->vrp.z);
+ Translate_to_Matrix(&v, wc);
+ /*
+ * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
+ * 3 : Rotation pour rendre la projection de VUP sur le plan de
+ * projection parallele a l'axe Y.
+ */
+ set_zy(m, &vp->vpn, &vp->vup);
+ /*
+ * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
+ */
+ postleft_matrix(m, 'z');
+ postmult_matrix(wc, m);
+ /*
+ * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
+ * COP = DOP = Direction of Projection.
+ * DOPRIM = DOP * R_TRL
+ * Pas de translation dans la matrice R_TRL pour la transformation
+ * du vecteur DOP.
+ */
+ SET_COORD3(dop, vp->vrp.x - vp->cop.x, vp->vrp.y - vp->cop.y, vp->vrp.z - vp->cop.z);
+ norm_vector(&dop);
+ SET_COORD3(cop, dop.x, dop.y, dop.z);
+ point_matrix(&doprim, &cop, m);
+ ident_matrix(m);
+ m[2][0] = -doprim.x / doprim.z;
+ m[2][1] = -doprim.y / doprim.z;
+ postmult_matrix(wc, m);
+ /*
+ * 6 : Translation et Mise a l'echelle de la pyramide.
+ * Remarque : contrairement a la reference qui donne
+ * 0 < x < 1, 0 < y < 1, 0 < z < 1
+ * je prefere, afin de rester coherent avec la projection perspective,
+ * -1 < x < 1, -1 < y < 1, 0 < z < 1 (w = 1)
+ */
+ SET_COORD3(v, (float)(-(vp->vwd.umax + vp->vwd.umin) / 2.0), (float)(-(vp->vwd.vmax + vp->vwd.vmin) / 2.0),
+ (float)(-vp->depth.front));
+ posttrans_matrix(wc, &v);
+ SET_COORD3(v, (float)(2.0 / (vp->vwd.umax - vp->vwd.umin)), (float)(2.0 / (vp->vwd.vmax - vp->vwd.vmin)),
+ (float)(1.0 / (vp->depth.back - vp->depth.front)));
+ postscale_matrix(wc, &v);
}
/*
@@ -175,56 +179,57 @@ void set_parallel (View_parameters *vp, Matrix wc)
* vp Parametres de visualisation.
* wc Matrice a initialiser.
*/
-void set_perspective (View_parameters *vp, Matrix wc)
+void set_perspective(View_parameters *vp, Matrix wc)
{
- Matrix m = IDENTITY_MATRIX;
- Point4f vrprim, cw;
- float zmin;
- Vector v;
+ Matrix m = IDENTITY_MATRIX;
+ Point4f vrprim, cw;
+ float zmin;
+ Vector v;
- /*
- * 1 : Translation du centre de projection COP a l'origine.
- */
- SET_COORD3(v,- vp->cop.x, - vp->cop.y, - vp->cop.z);
- Translate_to_Matrix (&v, wc);
- /*
- * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
- * 3 : Rotation pour rendre la projection de VUP sur le plan de
- * projection parallele a l'axe Y.
- */
- set_zy (m, &vp->vpn, &vp->vup);
- postmult_matrix (wc, m);
- /*
- * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
- */
- postleft_matrix (wc, 'z');
- /*
- * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
- */
- point_matrix (&vrprim, &vp->vrp, wc);
- cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
- cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
- cw.z = (float)(vrprim.z);
- ident_matrix (m);
- m[2][0] = - cw.x / cw.z;
- m[2][1] = - cw.y / cw.z;
- postmult_matrix (wc, m);
- /*
- * 6 : Mise a l'echelle de la pyramide.
- */
- SET_COORD3(v,
- (float)((2.0*vrprim.z)/((vp->vwd.umax-vp->vwd.umin)*(vrprim.z+vp->depth.back))),
- (float)((2.0*vrprim.z)/((vp->vwd.vmax-vp->vwd.vmin)*(vrprim.z+vp->depth.back))),
- (float)( 1.0/(vrprim.z+vp->depth.back)));
- postscale_matrix (wc, &v);
- /*
- * 7 : Transformation perspective.
- */
- zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
- ident_matrix (m);
- m[2][2] = (float)(1.0 / (1.0 - zmin)); m[2][3] = 1.0;
- m[3][2] = (float)(- zmin / (1.0 - zmin)); m[3][3] = 0.0;
- postmult_matrix (wc, m);
+ /*
+ * 1 : Translation du centre de projection COP a l'origine.
+ */
+ SET_COORD3(v, -vp->cop.x, -vp->cop.y, -vp->cop.z);
+ Translate_to_Matrix(&v, wc);
+ /*
+ * 2 : Rotation pour rendre VPN parallele a l'axe des Z negatifs.
+ * 3 : Rotation pour rendre la projection de VUP sur le plan de
+ * projection parallele a l'axe Y.
+ */
+ set_zy(m, &vp->vpn, &vp->vup);
+ postmult_matrix(wc, m);
+ /*
+ * 4 : Passer d'un repere droit (absolu) a un repere gauche (vision).
+ */
+ postleft_matrix(wc, 'z');
+ /*
+ * 5 : Alignement de l'axe central du volume de vision sur l'axe Z.
+ */
+ point_matrix(&vrprim, &vp->vrp, wc);
+ cw.x = (float)(vrprim.x + (vp->vwd.umin + vp->vwd.umax) / 2.0);
+ cw.y = (float)(vrprim.y + (vp->vwd.vmin + vp->vwd.vmax) / 2.0);
+ cw.z = (float)(vrprim.z);
+ ident_matrix(m);
+ m[2][0] = -cw.x / cw.z;
+ m[2][1] = -cw.y / cw.z;
+ postmult_matrix(wc, m);
+ /*
+ * 6 : Mise a l'echelle de la pyramide.
+ */
+ SET_COORD3(v, (float)((2.0 * vrprim.z) / ((vp->vwd.umax - vp->vwd.umin) * (vrprim.z + vp->depth.back))),
+ (float)((2.0 * vrprim.z) / ((vp->vwd.vmax - vp->vwd.vmin) * (vrprim.z + vp->depth.back))),
+ (float)(1.0 / (vrprim.z + vp->depth.back)));
+ postscale_matrix(wc, &v);
+ /*
+ * 7 : Transformation perspective.
+ */
+ zmin = (vrprim.z + vp->depth.front) / (vrprim.z + vp->depth.back);
+ ident_matrix(m);
+ m[2][2] = (float)(1.0 / (1.0 - zmin));
+ m[2][3] = 1.0;
+ m[3][2] = (float)(-zmin / (1.0 - zmin));
+ m[3][3] = 0.0;
+ postmult_matrix(wc, m);
}
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpProjection.h b/modules/robot/src/wireframe-simulator/vpProjection.h
index bc6f09e..88f8d40 100644
--- a/modules/robot/src/wireframe-simulator/vpProjection.h
+++ b/modules/robot/src/wireframe-simulator/vpProjection.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,12 +44,12 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpView.h"
#include "vpArit.h"
+#include "vpView.h"
-void View_to_Matrix (View_parameters *vp, Matrix m);
-void set_parallel (View_parameters *vp, Matrix wc);
-void set_perspective (View_parameters *vp, Matrix wc);
+void View_to_Matrix(View_parameters *vp, Matrix m);
+void set_parallel(View_parameters *vp, Matrix wc);
+void set_perspective(View_parameters *vp, Matrix wc);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpRfstack.cpp b/modules/robot/src/wireframe-simulator/vpRfstack.cpp
index ea3ec79..0eca240 100644
--- a/modules/robot/src/wireframe-simulator/vpRfstack.cpp
+++ b/modules/robot/src/wireframe-simulator/vpRfstack.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,25 +37,19 @@
*
*****************************************************************************/
-
-
-
-
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
-#include "vpView.h"
+#include "vpMy.h"
#include "vpRfstack.h"
+#include "vpView.h"
#include <stdio.h>
#include <string.h>
-#define STACKSIZE 32
-
-static int stack[STACKSIZE] = {vpDEFAULT_REMOVE}; /* pile */
-static int *sp = stack; /* sommet */
+#define STACKSIZE 32
+static int stack[STACKSIZE] = {vpDEFAULT_REMOVE}; /* pile */
+static int *sp = stack; /* sommet */
/*
* La procedure "fprintf_rfstack" affiche le sommet
@@ -62,48 +57,51 @@ static int *sp = stack; /* sommet */
* Entree :
* fp Fichier en sortie.
*/
-void
-fprintf_rfstack (FILE *fp)
+void fprintf_rfstack(FILE *fp)
{
- int flg;
- flg = 0; /* nul si element unique */
-
- if (*sp == IS_INSIDE) {
- fprintf (fp, "(null)\n");
- return;
- }
- fprintf (fp, "(");
- if (*sp & IS_ABOVE) {
- //if (flg) fprintf (fp, " "); Removed since if (flg) cannot be true
- flg ++;
- fprintf (fp, "above");
- }
- if (*sp & IS_BELOW) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "below");
- }
- if (*sp & IS_RIGHT) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "right");
- }
- if (*sp & IS_LEFT) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "left");
- }
- if (*sp & IS_BACK) {
- if (flg) fprintf (fp, " ");
- flg ++;
- fprintf (fp, "back");
- }
- if (*sp & IS_FRONT) {
- /*if (flg)*/ fprintf (fp, " ");
- flg ++;
- fprintf (fp, "front");
- }
- fprintf (fp, ")\n");
+ int flg;
+ flg = 0; /* nul si element unique */
+
+ if (*sp == IS_INSIDE) {
+ fprintf(fp, "(null)\n");
+ return;
+ }
+ fprintf(fp, "(");
+ if (*sp & IS_ABOVE) {
+ // if (flg) fprintf (fp, " "); Removed since if (flg) cannot be true
+ flg++;
+ fprintf(fp, "above");
+ }
+ if (*sp & IS_BELOW) {
+ if (flg)
+ fprintf(fp, " ");
+ flg++;
+ fprintf(fp, "below");
+ }
+ if (*sp & IS_RIGHT) {
+ if (flg)
+ fprintf(fp, " ");
+ flg++;
+ fprintf(fp, "right");
+ }
+ if (*sp & IS_LEFT) {
+ if (flg)
+ fprintf(fp, " ");
+ flg++;
+ fprintf(fp, "left");
+ }
+ if (*sp & IS_BACK) {
+ if (flg)
+ fprintf(fp, " ");
+ flg++;
+ fprintf(fp, "back");
+ }
+ if (*sp & IS_FRONT) {
+ /*if (flg)*/ fprintf(fp, " ");
+ flg++;
+ fprintf(fp, "front");
+ }
+ fprintf(fp, ")\n");
}
/*
@@ -112,11 +110,7 @@ fprintf_rfstack (FILE *fp)
* Sortie :
* Pointeur sur les drapeaux d'elimination du sommet de la pile.
*/
-int *
-get_rfstack (void)
-{
- return (sp);
-}
+int *get_rfstack(void) { return (sp); }
/*
* La procedure "load_rfstack" charge des drapeaux au sommet
@@ -124,77 +118,63 @@ get_rfstack (void)
* Entree :
* i Niveau a charger.
*/
-void
-load_rfstack (int i)
-{
- *sp = i;
-}
+void load_rfstack(int i) { *sp = i; }
/*
* La procedure "pop_rfstack" depile les drapeaux au sommet
* de la pile des drapeaux d'elimination de faces.
*/
-void
-pop_rfstack (void)
+void pop_rfstack(void)
{
- if (sp == stack) {
- static char proc_name[] = "pop_rfstack";
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
+ if (sp == stack) {
+ static char proc_name[] = "pop_rfstack";
+ fprintf(stderr, "%s: stack underflow\n", proc_name);
+ return;
+ } else
+ sp--;
}
/*
* La procedure "push_rfstack" empile et duplique les drapeaux du sommet
* de la pile des drapeaux d'elimination de faces.
*/
-void
-push_rfstack (void)
+void push_rfstack(void)
{
- if (sp == stack + STACKSIZE - 1) {
- static char proc_name[] = "push_rfstack";
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- *sp = *(sp - 1);
+ if (sp == stack + STACKSIZE - 1) {
+ static char proc_name[] = "push_rfstack";
+ fprintf(stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ *sp = *(sp - 1);
}
/*
* La procedure "swap_rfstack" echange les deux premiers elements
* de la pile des drapeaux d'elimination de faces.
*/
-void
-swap_rfstack (void)
+void swap_rfstack(void)
{
- int *ip;
+ int *ip;
- ip = (sp == stack) ? sp + 1 : sp - 1;
+ ip = (sp == stack) ? sp + 1 : sp - 1;
int tmp;
// SWAP(*sp, *ip, tmp); // produce a cppcheck warning
- tmp = *sp; *sp = *ip; *ip = tmp;
+ tmp = *sp;
+ *sp = *ip;
+ *ip = tmp;
}
/*
* La procedure "add_rfstack" ajoute des drapeaux au sommet
* de la pile des drapeaux d'elimination de faces.
*/
-void
-add_rfstack (int i)
-{
- *sp |= i;
-}
+void add_rfstack(int i) { *sp |= i; }
/*
* La procedure "sub_rfstack" soustrait des drapeaux au sommet
* de la pile des drapeaux d'elimination de faces.
*/
-void
-sub_rfstack (int i)
-{
- *sp &= ~i;
-}
+void sub_rfstack(int i) { *sp &= ~i; }
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpRfstack.h b/modules/robot/src/wireframe-simulator/vpRfstack.h
index 286a419..91e790b 100644
--- a/modules/robot/src/wireframe-simulator/vpRfstack.h
+++ b/modules/robot/src/wireframe-simulator/vpRfstack.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,19 +40,19 @@
#ifndef vpRfstack_h
#define vpRfstack_h
-#include <visp3/core/vpConfig.h>
#include <stdio.h>
+#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void fprintf_rfstack (FILE *fp);
-int *get_rfstack (void);
-void load_rfstack (int i);
-void pop_rfstack (void);
-void push_rfstack (void);
-void swap_rfstack (void);
-void add_rfstack (int i);
-void sub_rfstack (int i);
+void fprintf_rfstack(FILE *fp);
+int *get_rfstack(void);
+void load_rfstack(int i);
+void pop_rfstack(void);
+void push_rfstack(void);
+void swap_rfstack(void);
+void add_rfstack(int i);
+void sub_rfstack(int i);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpScene.cpp b/modules/robot/src/wireframe-simulator/vpScene.cpp
index 7360af9..8e2c12b 100644
--- a/modules/robot/src/wireframe-simulator/vpScene.cpp
+++ b/modules/robot/src/wireframe-simulator/vpScene.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,7 +35,6 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -42,10 +42,10 @@
#include <cmath>
#include <limits>
-#include "vpScene.h"
#include "vpKeyword.h"
#include "vpLex.h"
#include "vpParser.h"
+#include "vpScene.h"
#include <visp3/core/vpException.h>
#include <visp3/core/vpPoint.h>
@@ -53,8 +53,7 @@
/*
Get the extension of the file and return it
*/
-Model_3D
-getExtension(const char* file)
+Model_3D getExtension(const char *file)
{
std::string sfilename(file);
@@ -65,10 +64,9 @@ getExtension(const char* file)
size_t size = sfilename.size();
- if ((bnd>0 && bnd<size ) || (BND>0 && BND<size))
+ if ((bnd > 0 && bnd < size) || (BND > 0 && BND < size))
return BND_MODEL;
- else if ((wrl>0 && wrl<size) || ( WRL>0 && WRL<size))
- {
+ else if ((wrl > 0 && wrl < size) || (WRL > 0 && WRL < size)) {
#if defined(VISP_HAVE_COIN3D)
return WRL_MODEL;
#else
@@ -82,72 +80,66 @@ getExtension(const char* file)
/*
Enable to initialize the scene
*/
-void set_scene (const char* str, Bound_scene *sc, float factor)
+void set_scene(const char *str, Bound_scene *sc, float factor)
{
- FILE *fd;
+ FILE *fd;
- //if ((fd = fopen (str, 0)) == -1)
- if ((fd = fopen (str, "r")) == NULL)
- {
+ // if ((fd = fopen (str, 0)) == -1)
+ if ((fd = fopen(str, "r")) == NULL) {
std::string error = "The file " + std::string(str) + " can not be opened";
- throw(vpException(vpException::ioError, error.c_str())) ;
+ throw(vpException(vpException::ioError, error.c_str()));
}
- open_keyword (keyword_tbl);
- open_lex ();
- open_source (fd, str);
- malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
- parser (sc);
-
- //if (factor != 1)
- if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
- {
- for (int i = 0; i < sc->bound.nbr; i++)
- {
- for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
- {
- sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
- sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
- sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
+ open_keyword(keyword_tbl);
+ open_lex();
+ open_source(fd, str);
+ malloc_Bound_scene(sc, str, (Index)BOUND_NBR);
+ parser(sc);
+
+ // if (factor != 1)
+ if (std::fabs(factor) > std::numeric_limits<double>::epsilon()) {
+ for (int i = 0; i < sc->bound.nbr; i++) {
+ for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++) {
+ sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x * factor;
+ sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y * factor;
+ sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z * factor;
}
}
}
- close_source ();
- close_lex ();
- close_keyword ();
+ close_source();
+ close_lex();
+ close_keyword();
fclose(fd);
}
#if defined(VISP_HAVE_COIN3D)
-void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
+void set_scene_wrl(const char *str, Bound_scene *sc, float factor)
{
- //Load the sceneGraph
+ // Load the sceneGraph
SoDB::init();
SoInput in;
SbBool ok = in.openFile(str);
- SoVRMLGroup *sceneGraphVRML2;
+ SoVRMLGroup *sceneGraphVRML2;
if (!ok) {
throw(vpException(vpException::fatalError, "Can't open file \"%s\". Please check the Marker_Less.ini file", str));
}
- if(!in.isFileVRML2())
- {
+ if (!in.isFileVRML2()) {
SoSeparator *sceneGraph = SoDB::readAll(&in);
- if (sceneGraph == NULL) { /*return -1;*/ }
+ if (sceneGraph == NULL) { /*return -1;*/
+ }
sceneGraph->ref();
SoToVRML2Action tovrml2;
tovrml2.apply(sceneGraph);
- sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
+ sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
sceneGraphVRML2->ref();
sceneGraph->unref();
- }
- else
- {
- sceneGraphVRML2 = SoDB::readAllVRML(&in);
+ } else {
+ sceneGraphVRML2 = SoDB::readAllVRML(&in);
if (sceneGraphVRML2 == NULL) {
/*return -1;*/
throw(vpException(vpException::notInitialized, "Cannot read VRML file"));
@@ -159,76 +151,68 @@ void set_scene_wrl (const char* str, Bound_scene *sc, float factor)
int nbShapes = sceneGraphVRML2->getNumChildren();
- SoNode * child;
+ SoNode *child;
- malloc_Bound_scene (sc, str,(Index)BOUND_NBR);
+ malloc_Bound_scene(sc, str, (Index)BOUND_NBR);
int iterShapes = 0;
- for (int i = 0; i < nbShapes; i++)
- {
+ for (int i = 0; i < nbShapes; i++) {
child = sceneGraphVRML2->getChild(i);
- if (child->getTypeId() == SoVRMLShape::getClassTypeId())
- {
+ if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
int nbFaces = 0;
- std::list<indexFaceSet*> ifs_list;
- SoChildList * child2list = child->getChildren();
- for (int j = 0; j < child2list->getLength(); j++)
- {
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
- {
+ std::list<indexFaceSet *> ifs_list;
+ SoChildList *child2list = child->getChildren();
+ for (int j = 0; j < child2list->getLength(); j++) {
+ if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
indexFaceSet *ifs = new indexFaceSet;
- SoVRMLIndexedFaceSet * face_set;
- face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
- extractFaces(face_set,ifs);
+ SoVRMLIndexedFaceSet *face_set;
+ face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
+ extractFaces(face_set, ifs);
ifs_list.push_back(ifs);
nbFaces++;
}
-// if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
-// {
-// std::cout << "> We found a line" << std::endl;
-// SoVRMLIndexedLineSet * line_set;
-// line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
-// extractLines(line_set);
-// }
+ // if (((SoNode*)child2list->get(j))->getTypeId() ==
+ // SoVRMLIndexedLineSet::getClassTypeId())
+ // {
+ // std::cout << "> We found a line" << std::endl;
+ // SoVRMLIndexedLineSet * line_set;
+ // line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
+ // extractLines(line_set);
+ // }
}
sc->bound.nbr++;
- ifsToBound (&(sc->bound.ptr[iterShapes]), ifs_list);
+ ifsToBound(&(sc->bound.ptr[iterShapes]), ifs_list);
destroyIfs(ifs_list);
iterShapes++;
}
}
- //if (factor != 1)
- if (std::fabs(factor) > std::numeric_limits<double>::epsilon())
- {
- for (int i = 0; i < sc->bound.nbr; i++)
- {
- for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++)
- {
- sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x*factor;
- sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y*factor;
- sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z*factor;
+ // if (factor != 1)
+ if (std::fabs(factor) > std::numeric_limits<double>::epsilon()) {
+ for (int i = 0; i < sc->bound.nbr; i++) {
+ for (int j = 0; j < sc->bound.ptr[i].point.nbr; j++) {
+ sc->bound.ptr[i].point.ptr[j].x = sc->bound.ptr[i].point.ptr[j].x * factor;
+ sc->bound.ptr[i].point.ptr[j].y = sc->bound.ptr[i].point.ptr[j].y * factor;
+ sc->bound.ptr[i].point.ptr[j].z = sc->bound.ptr[i].point.ptr[j].z * factor;
}
}
}
}
-
-void extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
+void extractFaces(SoVRMLIndexedFaceSet *face_set, indexFaceSet *ifs)
{
-// vpList<vpPoint> pointList;
-// pointList.kill();
+ // vpList<vpPoint> pointList;
+ // pointList.kill();
SoVRMLCoordinate *coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
int coordSize = coord->point.getNum();
ifs->nbPt = coordSize;
- for (int i = 0; i < coordSize; i++)
- {
- SbVec3f point(0,0,0);
- point[0]=coord->point[i].getValue()[0];
- point[1]=coord->point[i].getValue()[1];
- point[2]=coord->point[i].getValue()[2];
- vpPoint pt(point[0],point[1],point[2]);
+ for (int i = 0; i < coordSize; i++) {
+ SbVec3f point(0, 0, 0);
+ point[0] = coord->point[i].getValue()[0];
+ point[1] = coord->point[i].getValue()[1];
+ point[2] = coord->point[i].getValue()[2];
+ vpPoint pt(point[0], point[1], point[2]);
ifs->pt.push_back(pt);
}
@@ -236,29 +220,26 @@ void extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs)
int indexListSize = indexList.getNum();
ifs->nbIndex = indexListSize;
- for (int i = 0; i < indexListSize; i++)
- {
+ for (int i = 0; i < indexListSize; i++) {
int index = face_set->coordIndex[i];
ifs->index.push_back(index);
}
}
-void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
+void ifsToBound(Bound *bptr, std::list<indexFaceSet *> &ifs_list)
{
int nbPt = 0;
- for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
+ for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
nbPt += (*it)->nbPt;
}
bptr->point.nbr = (Index)nbPt;
- bptr->point.ptr = (Point3f *) malloc ((unsigned int)nbPt * sizeof (Point3f));
+ bptr->point.ptr = (Point3f *)malloc((unsigned int)nbPt * sizeof(Point3f));
ifs_list.front();
unsigned int iter = 0;
- for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++)
- {
+ for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
+ indexFaceSet *ifs = *it;
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbPt; j++) {
bptr->point.ptr[iter].x = (float)ifs->pt[j].get_oX();
bptr->point.ptr[iter].y = (float)ifs->pt[j].get_oY();
bptr->point.ptr[iter].z = (float)ifs->pt[j].get_oZ();
@@ -270,47 +251,38 @@ void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
ifs_list.front();
std::list<int> indSize;
int indice = 0;
- for (std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
- {
- if(ifs->index[j] == -1)
- {
+ for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
+ indexFaceSet *ifs = *it;
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++) {
+ if (ifs->index[j] == -1) {
nbFace++;
indSize.push_back(indice);
indice = 0;
- }
- else indice++;
+ } else
+ indice++;
}
}
bptr->face.nbr = (Index)nbFace;
- bptr->face.ptr = (Face *) malloc (nbFace * sizeof (Face));
+ bptr->face.ptr = (Face *)malloc(nbFace * sizeof(Face));
std::list<int>::const_iterator iter_indSize = indSize.begin();
- for (unsigned int i = 0; i < indSize.size(); i++)
- {
+ 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<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it)
- {
- indexFaceSet* ifs = *it;
+ for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
+ indexFaceSet *ifs = *it;
iter = 0;
- for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++)
- {
- if(ifs->index[j] != -1)
- {
+ for (unsigned int j = 0; j < (unsigned int)ifs->nbIndex; j++) {
+ if (ifs->index[j] != -1) {
bptr->face.ptr[indice].vertex.ptr[iter] = (Index)(ifs->index[j] + offset);
iter++;
- }
- else
- {
+ } else {
iter = 0;
indice++;
}
@@ -319,23 +291,21 @@ void ifsToBound (Bound* bptr, std::list<indexFaceSet*> &ifs_list)
}
}
-void destroyIfs(std::list<indexFaceSet*> &ifs_list)
+void destroyIfs(std::list<indexFaceSet *> &ifs_list)
{
- for(std::list<indexFaceSet*>::const_iterator it=ifs_list.begin(); it!=ifs_list.end(); ++it){
+ for (std::list<indexFaceSet *>::const_iterator it = ifs_list.begin(); it != ifs_list.end(); ++it) {
delete *it;
}
ifs_list.clear();
}
#else
-void set_scene_wrl (const char* /*str*/, Bound_scene* /*sc*/, float /*factor*/)
-{
-}
+void set_scene_wrl(const char * /*str*/, Bound_scene * /*sc*/, float /*factor*/) {}
#endif
/*
Convert the matrix format to deal with the one in the simulator
*/
-void vp2jlc_matrix (const vpHomogeneousMatrix vpM, Matrix &jlcM)
+void vp2jlc_matrix(const vpHomogeneousMatrix &vpM, Matrix &jlcM)
{
for (unsigned int i = 0; i < 4; i++) {
for (unsigned int j = 0; j < 4; j++)
diff --git a/modules/robot/src/wireframe-simulator/vpScene.h b/modules/robot/src/wireframe-simulator/vpScene.h
index 78989fe..af2e89f 100644
--- a/modules/robot/src/wireframe-simulator/vpScene.h
+++ b/modules/robot/src/wireframe-simulator/vpScene.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,31 +42,30 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include "vpBound.h"
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/robot/vpWireFrameSimulatorTypes.h>
-#include "vpBound.h"
-//Inventor includes
+// Inventor includes
#if defined(VISP_HAVE_COIN3D)
-#include <Inventor/nodes/SoSeparator.h>
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/nodes/SoSeparator.h>
-#include <vector>
#include <list>
+#include <vector>
-typedef struct indexFaceSet
-{
- indexFaceSet() : nbPt(0), pt(), nbIndex(0), index() {};
+typedef struct indexFaceSet {
+ indexFaceSet() : nbPt(0), pt(), nbIndex(0), index(){};
int nbPt;
std::vector<vpPoint> pt;
int nbIndex;
@@ -74,22 +74,17 @@ typedef struct indexFaceSet
#endif
-typedef enum
-{
- BND_MODEL,
- WRL_MODEL,
- UNKNOWN_MODEL
-} Model_3D;
+typedef enum { BND_MODEL, WRL_MODEL, UNKNOWN_MODEL } Model_3D;
-Model_3D getExtension(const char* file);
-void set_scene_wrl (const char* str, Bound_scene *sc, float factor);
-void set_scene (const char*, Bound_scene *, float);
-void vp2jlc_matrix (const vpHomogeneousMatrix, Matrix&);
+Model_3D getExtension(const char *file);
+void set_scene_wrl(const char *str, Bound_scene *sc, float factor);
+void set_scene(const char *, Bound_scene *, float);
+void vp2jlc_matrix(const vpHomogeneousMatrix &, Matrix &);
#if defined(VISP_HAVE_COIN3D)
-void extractFaces(SoVRMLIndexedFaceSet* face_set, indexFaceSet *ifs);
-void ifsToBound (Bound*, std::list<indexFaceSet*> &);
-void destroyIfs(std::list<indexFaceSet*> &);
+void extractFaces(SoVRMLIndexedFaceSet *face_set, indexFaceSet *ifs);
+void ifsToBound(Bound *, std::list<indexFaceSet *> &);
+void destroyIfs(std::list<indexFaceSet *> &);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpSkipio.cpp b/modules/robot/src/wireframe-simulator/vpSkipio.cpp
index 7b035be..1daff35 100644
--- a/modules/robot/src/wireframe-simulator/vpSkipio.cpp
+++ b/modules/robot/src/wireframe-simulator/vpSkipio.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,18 +38,13 @@
*
*****************************************************************************/
-
-
-
-
-
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
-#include "vpToken.h"
#include "vpLex.h"
+#include "vpMy.h"
#include "vpSkipio.h"
+#include "vpToken.h"
#include <stdio.h>
/*
@@ -57,47 +53,49 @@
* Entree :
* f Fichier en sortie.
*/
-void skip_cmd (void)
+void skip_cmd(void)
{
- int token;
+ int token;
- fprintf (stderr, "\n$ ");
- fwrite (mytext, (size_t)mylength, 1, stderr);
- while ((token = lexecho (stderr, '$')) != T_EOF && token != '$') {};
- unlex ();
+ fprintf(stderr, "\n$ ");
+ fwrite(mytext, (size_t)mylength, 1, stderr);
+ while ((token = lexecho(stderr, '$')) != T_EOF && token != '$') {
+ };
+ unlex();
}
/*
- * La procedure "skip_keyword" saute les structures des articles
+ * La procedure "skip_keyword" saute les structures des articles
* jusqu'a reconnaitre le mot cle de jeton "token".
* Entree :
* token Jeton du mot cle a reconnaitre.
* err Message d'erreur si le mot cle n'est pas reconnu.
*/
-void skip_keyword (int token, const char *err)
+void skip_keyword(int token, const char *err)
{
- int t;
+ int t;
- switch (t = lex ()) {
- case T_IDENT : /* saute le mot cle inconnu */
- while ((t = lex ()) != 0){
- switch (t) {
- case '$' : /* nouvelle commande */
- case T_EOF : /* fin de fichier */
- lexerr ("start", err, NULL);
- break;
- default :
- if (t == token) return;
- break;
- }
+ switch (t = lex()) {
+ case T_IDENT: /* saute le mot cle inconnu */
+ while ((t = lex()) != 0) {
+ switch (t) {
+ case '$': /* nouvelle commande */
+ case T_EOF: /* fin de fichier */
+ lexerr("start", err, NULL);
+ break;
+ default:
+ if (t == token)
+ return;
+ break;
+ }
}
- break;
- default :
- if (t == token) return;
- break;
- }
- lexerr ("start", err, NULL);
+ break;
+ default:
+ if (t == token)
+ return;
+ break;
+ }
+ lexerr("start", err, NULL);
}
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpSkipio.h b/modules/robot/src/wireframe-simulator/vpSkipio.h
index dfe5040..1a7a10d 100644
--- a/modules/robot/src/wireframe-simulator/vpSkipio.h
+++ b/modules/robot/src/wireframe-simulator/vpSkipio.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,8 +45,8 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void skip_cmd (void);
-void skip_keyword (int token, const char *err);
+void skip_cmd(void);
+void skip_keyword(int token, const char *err);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpTmstack.cpp b/modules/robot/src/wireframe-simulator/vpTmstack.cpp
index 2eb4b64..3cb0d3e 100644
--- a/modules/robot/src/wireframe-simulator/vpTmstack.cpp
+++ b/modules/robot/src/wireframe-simulator/vpTmstack.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,12 +38,9 @@
*
*****************************************************************************/
-
-
-
-#include "vpMy.h"
-#include "vpArit.h"
#include "vpTmstack.h"
+#include "vpArit.h"
+#include "vpMy.h"
#include <math.h>
#include <stdio.h>
#include <string.h>
@@ -50,12 +48,10 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#define STACKSIZE 32
-
-
-static Matrix stack[STACKSIZE]/* = IDENTITY_MATRIX*/; /* pile */
-static Matrix *sp = stack; /* sommet */
+#define STACKSIZE 32
+static Matrix stack[STACKSIZE] /* = IDENTITY_MATRIX*/; /* pile */
+static Matrix *sp = stack; /* sommet */
/*
* La procedure "get_tmstack" retourne la matrice au sommet
@@ -63,11 +59,7 @@ static Matrix *sp = stack; /* sommet */
* Sortie :
* Pointeur de la matrice au sommet de la pile.
*/
-Matrix *
-get_tmstack (void)
-{
- return (sp);
-}
+Matrix *get_tmstack(void) { return (sp); }
/*
* La procedure "load_tmstack" charge une matrice au sommet
@@ -75,61 +67,57 @@ get_tmstack (void)
* Entree :
* m Matrice a charger.
*/
-void
-load_tmstack (Matrix m)
+void load_tmstack(Matrix m)
{
- //bcopy ((char *) m, (char *) *sp, sizeof (Matrix));
- memmove ((char *) *sp, (char *) m, sizeof (Matrix));
+ // bcopy ((char *) m, (char *) *sp, sizeof (Matrix));
+ memmove((char *)*sp, (char *)m, sizeof(Matrix));
}
/*
* La procedure "pop_tmstack" depile la matrice au sommet
* de la pile des matrices de transformation.
*/
-void
-pop_tmstack (void)
+void pop_tmstack(void)
{
- if (sp == stack) {
- static char proc_name[] = "pop_tmstack";
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
+ if (sp == stack) {
+ static char proc_name[] = "pop_tmstack";
+ fprintf(stderr, "%s: stack underflow\n", proc_name);
+ return;
+ } else
+ sp--;
}
/*
* La procedure "push_tmstack" empile et duplique le sommet
* de la pile des matrices de transformation.
*/
-void
-push_tmstack (void)
+void push_tmstack(void)
{
- if (sp == stack + STACKSIZE - 1) {
- static char proc_name[] = "push_tmstack";
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- //bcopy ((char *) (sp - 1), (char *) sp, sizeof (Matrix));
- memmove ((char *) sp, (char *) (sp - 1), sizeof (Matrix));
+ if (sp == stack + STACKSIZE - 1) {
+ static char proc_name[] = "push_tmstack";
+ fprintf(stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ // bcopy ((char *) (sp - 1), (char *) sp, sizeof (Matrix));
+ memmove((char *)sp, (char *)(sp - 1), sizeof(Matrix));
}
/*
* La procedure "swap_tmstack" echange les deux premieres matrices
* de la pile des matrices de transformation.
*/
-void
-swap_tmstack (void)
+void swap_tmstack(void)
{
- Matrix *mp, tmp;
-
- mp = (sp == stack) ? sp + 1 : sp - 1;
-// bcopy ((char *) *sp, (char *) tmp, sizeof (Matrix));
-// bcopy ((char *) *mp, (char *) *sp, sizeof (Matrix));
-// bcopy ((char *) tmp, (char *) *mp, sizeof (Matrix));
- memmove ((char *) tmp, (char *) *sp, sizeof (Matrix));
- memmove ((char *) *sp, (char *) *mp, sizeof (Matrix));
- memmove ((char *) *mp, (char *) tmp, sizeof (Matrix));
+ Matrix *mp, tmp;
+
+ mp = (sp == stack) ? sp + 1 : sp - 1;
+ // bcopy ((char *) *sp, (char *) tmp, sizeof (Matrix));
+ // bcopy ((char *) *mp, (char *) *sp, sizeof (Matrix));
+ // bcopy ((char *) tmp, (char *) *mp, sizeof (Matrix));
+ memmove((char *)tmp, (char *)*sp, sizeof(Matrix));
+ memmove((char *)*sp, (char *)*mp, sizeof(Matrix));
+ memmove((char *)*mp, (char *)tmp, sizeof(Matrix));
}
/*
@@ -138,11 +126,7 @@ swap_tmstack (void)
* Entree :
* m Matrice multiplicative.
*/
-void
-postmult_tmstack (Matrix m)
-{
- postmult_matrix (*sp, m);
-}
+void postmult_tmstack(Matrix m) { postmult_matrix(*sp, m); }
/*
* La procedure "postrotate_tmstack" postmultiplie la matrice au sommet
@@ -150,13 +134,12 @@ postmult_tmstack (Matrix m)
* Entree :
* vp Vecteur de rotation.
*/
-void
-postrotate_tmstack (Vector *vp)
+void postrotate_tmstack(Vector *vp)
{
- Matrix m;
+ Matrix m;
- Rotate_to_Matrix (vp, m);
- postmult3_matrix (*sp, m);
+ Rotate_to_Matrix(vp, m);
+ postmult3_matrix(*sp, m);
}
/*
@@ -165,11 +148,7 @@ postrotate_tmstack (Vector *vp)
* Entree :
* vp Vecteur d'homothetie.
*/
-void
-postscale_tmstack (Vector *vp)
-{
- postscale_matrix (*sp, vp);
-}
+void postscale_tmstack(Vector *vp) { postscale_matrix(*sp, vp); }
/*
* La procedure "posttranslate_tmstack" postmultiplie la matrice au sommet
@@ -177,11 +156,7 @@ postscale_tmstack (Vector *vp)
* Entree :
* vp Vecteur de translation.
*/
-void
-posttranslate_tmstack (Vector *vp)
-{
- posttrans_matrix (*sp, vp);
-}
+void posttranslate_tmstack(Vector *vp) { posttrans_matrix(*sp, vp); }
/*
* La procedure "premult_tmstack" premultiplie la matrice au sommet
@@ -189,11 +164,7 @@ posttranslate_tmstack (Vector *vp)
* Entree :
* m Matrice multiplicative.
*/
-void
-premult_tmstack (Matrix m)
-{
- premult_matrix (*sp, m);
-}
+void premult_tmstack(Matrix m) { premult_matrix(*sp, m); }
/*
* La procedure "prerotate_tmstack" premultiplie la matrice au sommet
@@ -201,13 +172,12 @@ premult_tmstack (Matrix m)
* Entree :
* vp Vecteur de rotation.
*/
-void
-prerotate_tmstack (Vector *vp)
+void prerotate_tmstack(Vector *vp)
{
- Matrix m;
+ Matrix m;
- Rotate_to_Matrix (vp, m);
- premult3_matrix (*sp, m);
+ Rotate_to_Matrix(vp, m);
+ premult3_matrix(*sp, m);
}
/*
@@ -216,11 +186,7 @@ prerotate_tmstack (Vector *vp)
* Entree :
* vp Vecteur d'homothetie.
*/
-void
-prescale_tmstack (Vector *vp)
-{
- prescale_matrix (*sp, vp);
-}
+void prescale_tmstack(Vector *vp) { prescale_matrix(*sp, vp); }
/*
* La procedure "pretranslate_tmstack" premultiplie la matrice au sommet
@@ -228,11 +194,6 @@ prescale_tmstack (Vector *vp)
* Entree :
* vp Vecteur de translation.
*/
-void
-pretranslate_tmstack (Vector *vp)
-{
- pretrans_matrix (*sp, vp);
-}
+void pretranslate_tmstack(Vector *vp) { pretrans_matrix(*sp, vp); }
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpTmstack.h b/modules/robot/src/wireframe-simulator/vpTmstack.h
index 01e5c40..70ac331 100644
--- a/modules/robot/src/wireframe-simulator/vpTmstack.h
+++ b/modules/robot/src/wireframe-simulator/vpTmstack.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,20 +44,23 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-Matrix *get_tmstack (void);
-void load_tmstack (Matrix m);
-void pop_tmstack (void);
-void push_tmstack (void);
-void swap_tmstack (void);
+#include "vpArit.h"
+#include "vpMy.h"
-void postmult_tmstack (Matrix m);
-void postrotate_tmstack (Vector *vp);
-void postscale_tmstack (Vector *vp);
-void posttranslate_tmstack (Vector *vp);
-void premult_tmstack (Matrix m);
-void prerotate_tmstack (Vector *vp);
-void prescale_tmstack (Vector *vp);
-void pretranslate_tmstack (Vector *vp);
+Matrix *get_tmstack(void);
+void load_tmstack(Matrix m);
+void pop_tmstack(void);
+void push_tmstack(void);
+void swap_tmstack(void);
+
+void postmult_tmstack(Matrix m);
+void postrotate_tmstack(Vector *vp);
+void postscale_tmstack(Vector *vp);
+void posttranslate_tmstack(Vector *vp);
+void premult_tmstack(Matrix m);
+void prerotate_tmstack(Vector *vp);
+void prescale_tmstack(Vector *vp);
+void pretranslate_tmstack(Vector *vp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpToken.cpp b/modules/robot/src/wireframe-simulator/vpToken.cpp
index d0a815b..9159845 100644
--- a/modules/robot/src/wireframe-simulator/vpToken.cpp
+++ b/modules/robot/src/wireframe-simulator/vpToken.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,39 +37,36 @@
*
*****************************************************************************/
-
-#include "vpMy.h"
#include "vpToken.h"
+#include "vpMy.h"
#include <visp3/core/vpConfig.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-
-Keyword keyword_tbl[] = { /* tableau des mots cles termine par NULL*/
- { "above", T_ABOVE },
- { "back", T_BACK },
- { "below", T_BELOW },
- { "bound", T_BOUND },
- { "cop", T_COP },
- { "depth", T_DEPTH },
- { "exit", T_EXIT },
- { "face_list", T_FACE_LIST },
- { "front", T_FRONT },
- { "left", T_LEFT },
- { "none", T_NONE },
- { "parallel", T_PARALLEL },
- { "perspective",T_PERSPECTIVE },
- { "point_list", T_POINT_LIST },
- { "remove", T_REMOVE },
- { "right", T_RIGHT },
- { "type", T_TYPE },
- { "view", T_VIEW },
- { "vpn", T_VPN },
- { "vrp", T_VRP },
- { "vup", T_VUP },
- { "window", T_WINDOW },
- { NULL, NULL }
-};
+Keyword keyword_tbl[] = {/* tableau des mots cles termine par NULL*/
+ {"above", T_ABOVE},
+ {"back", T_BACK},
+ {"below", T_BELOW},
+ {"bound", T_BOUND},
+ {"cop", T_COP},
+ {"depth", T_DEPTH},
+ {"exit", T_EXIT},
+ {"face_list", T_FACE_LIST},
+ {"front", T_FRONT},
+ {"left", T_LEFT},
+ {"none", T_NONE},
+ {"parallel", T_PARALLEL},
+ {"perspective", T_PERSPECTIVE},
+ {"point_list", T_POINT_LIST},
+ {"remove", T_REMOVE},
+ {"right", T_RIGHT},
+ {"type", T_TYPE},
+ {"view", T_VIEW},
+ {"vpn", T_VPN},
+ {"vrp", T_VRP},
+ {"vup", T_VUP},
+ {"window", T_WINDOW},
+ {NULL, NULL}};
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpToken.h b/modules/robot/src/wireframe-simulator/vpToken.h
index d3b3daf..1be5a8a 100644
--- a/modules/robot/src/wireframe-simulator/vpToken.h
+++ b/modules/robot/src/wireframe-simulator/vpToken.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,55 +43,55 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#include <visp3/robot/vpWireFrameSimulatorTypes.h>
-typedef struct {
- const char *ident; /* identifateur */
- Index token; /* code du jeton */
+typedef struct {
+ const char *ident; /* identifateur */
+ Index token; /* code du jeton */
} Keyword;
-#define T_EOF 256
-#define T_FLOAT 257
-#define T_IDENT 258
-#define T_INT 259
-#define T_STRING 260
-
-extern float myfloat;
-extern int myint;
-extern int mylength;
-extern int mylineno;
-extern char *mytext;
-extern Keyword keyword_tbl[];
+#define T_EOF 256
+#define T_FLOAT 257
+#define T_IDENT 258
+#define T_INT 259
+#define T_STRING 260
+extern float myfloat;
+extern int myint;
+extern int mylength;
+extern int mylineno;
+extern char *mytext;
+extern Keyword keyword_tbl[];
/*
* Jetons superieurs a 270 (voir "../mylex/token.h").
*/
-#define T_ABOVE 270
-#define T_BACK 271
-#define T_BELOW 272
-#define T_BOUND 273
-#define T_COP 274
-#define T_DEPTH 275
-#define T_EXIT 276
-#define T_FACE_LIST 277
-#define T_FILE 278
-#define T_FRONT 279
-#define T_IMAGE 280
-#define T_LEFT 281
-#define T_NONE 282
-#define T_ORIGIN 283
-#define T_PARALLEL 284
-#define T_PERSPECTIVE 285
-#define T_POINT_LIST 286
-#define T_REMOVE 287
-#define T_RIGHT 288
-#define T_SIZE 289
-#define T_TYPE 290
-#define T_VIEW 291
-#define T_VPN 292
-#define T_VRP 293
-#define T_VUP 294
-#define T_WINDOW 295
+#define T_ABOVE 270
+#define T_BACK 271
+#define T_BELOW 272
+#define T_BOUND 273
+#define T_COP 274
+#define T_DEPTH 275
+#define T_EXIT 276
+#define T_FACE_LIST 277
+#define T_FILE 278
+#define T_FRONT 279
+#define T_IMAGE 280
+#define T_LEFT 281
+#define T_NONE 282
+#define T_ORIGIN 283
+#define T_PARALLEL 284
+#define T_PERSPECTIVE 285
+#define T_POINT_LIST 286
+#define T_REMOVE 287
+#define T_RIGHT 288
+#define T_SIZE 289
+#define T_TYPE 290
+#define T_VIEW 291
+#define T_VPN 292
+#define T_VRP 293
+#define T_VUP 294
+#define T_WINDOW 295
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpView.h b/modules/robot/src/wireframe-simulator/vpView.h
index 19d8d2d..4845ec8 100644
--- a/modules/robot/src/wireframe-simulator/vpView.h
+++ b/modules/robot/src/wireframe-simulator/vpView.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,12 +44,12 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include "vpMy.h"
#include "vpArit.h"
+#include "vpMy.h"
/*
* Macros de numerotation des 6 plans de decoupage :
- * - Les 6 plans de clipping definissent le volume canonique
+ * - Les 6 plans de clipping definissent le volume canonique
* de la pyramide de vision dans lequel la scene est visible.
* - les 6 plans ont pour equations :
* Plan dessus : W = Y
@@ -58,13 +59,13 @@
* Plan arriere : W = Z
* Plan avant : W = 0
*/
-#define PLANE_ABOVE 0
-#define PLANE_BELOW 1
-#define PLANE_RIGHT 2
-#define PLANE_LEFT 3
-#define PLANE_BACK 4
-#define PLANE_FRONT 5
-#define PLANE_NBR 6
+#define PLANE_ABOVE 0
+#define PLANE_BELOW 1
+#define PLANE_RIGHT 2
+#define PLANE_LEFT 3
+#define PLANE_BACK 4
+#define PLANE_FRONT 5
+#define PLANE_NBR 6
/*
* Macros de positionnement des points 4D :
@@ -72,47 +73,71 @@
* se fait par rapport aux 6 plans de decoupage.
* A chaque point 4D on associe 6 bits, un par plan de decoupage.
*/
-#define IS_INSIDE 0x00
-#define IS_ABOVE 0x01
-#define IS_BELOW 0x02
-#define IS_RIGHT 0x04
-#define IS_LEFT 0x08
-#define IS_BACK 0x10
-#define IS_FRONT 0x20
-
-#define vpDEFAULT_REMOVE IS_INSIDE
-
-#define PARALLEL 0
-#define PERSPECTIVE 1
-
-#define vpDEFAULT_EYE { 0.0, 0.0, 1.0 }
-#define vpDEFAULT_TARGET { 0.0, 0.0, 0.0 }
-#define vpDEFAULT_FOCAL 1.0
-#define vpDEFAULT_ANGLE 45.0
-#define vpDEFAULT_TWIST 0.0
-#define vpDEFAULT_SPEED 0.0
-#define vpDEFAULT_CAMERA { vpDEFAULT_EYE, vpDEFAULT_TARGET,\
- vpDEFAULT_FOCAL,vpDEFAULT_ANGLE, vpDEFAULT_TWIST,\
- vpDEFAULT_SPEED }
-
-#define vpDEFAULT_COP { 0.0, 0.0, 1.0 }
-#define vpDEFAULT_VRP { 0.0, 0.0, 0.0 }
-#define vpDEFAULT_VPN { 0.0, 0.0,-1.0 }
-#define vpDEFAULT_VUP { 0.0, 1.0, 0.0 }
-#define vpDEFAULT_VWD {-1.0, 1.0,-1.0, 1.0 }
-#define vpDEFAULT_DEPTH { 0.0, 1.0 }
-#define vpDEFAULT_TYPE PERSPECTIVE
-
-#define vpDEFAULT_VIEW { vpDEFAULT_TYPE,\
- vpDEFAULT_COP, vpDEFAULT_VRP,\
- vpDEFAULT_VPN, vpDEFAULT_VUP,\
- vpDEFAULT_VWD, vpDEFAULT_DEPTH }
-
-#define vpDEFAULT_WC { 1.0, 0.0, 0.0, 0.0,\
- 0.0, 1.0, 0.0, 0.0,\
- 0.0, 0.0, 1.0, 0.0,\
- 0.0, 0.0, 0.0, 1.0 }
-
+#define IS_INSIDE 0x00
+#define IS_ABOVE 0x01
+#define IS_BELOW 0x02
+#define IS_RIGHT 0x04
+#define IS_LEFT 0x08
+#define IS_BACK 0x10
+#define IS_FRONT 0x20
+
+#define vpDEFAULT_REMOVE IS_INSIDE
+
+#define PARALLEL 0
+#define PERSPECTIVE 1
+
+#define vpDEFAULT_EYE \
+ { \
+ 0.0, 0.0, 1.0 \
+ }
+#define vpDEFAULT_TARGET \
+ { \
+ 0.0, 0.0, 0.0 \
+ }
+#define vpDEFAULT_FOCAL 1.0
+#define vpDEFAULT_ANGLE 45.0
+#define vpDEFAULT_TWIST 0.0
+#define vpDEFAULT_SPEED 0.0
+#define vpDEFAULT_CAMERA \
+ { \
+ vpDEFAULT_EYE, vpDEFAULT_TARGET, vpDEFAULT_FOCAL, vpDEFAULT_ANGLE, vpDEFAULT_TWIST, vpDEFAULT_SPEED \
+ }
+
+#define vpDEFAULT_COP \
+ { \
+ 0.0, 0.0, 1.0 \
+ }
+#define vpDEFAULT_VRP \
+ { \
+ 0.0, 0.0, 0.0 \
+ }
+#define vpDEFAULT_VPN \
+ { \
+ 0.0, 0.0, -1.0 \
+ }
+#define vpDEFAULT_VUP \
+ { \
+ 0.0, 1.0, 0.0 \
+ }
+#define vpDEFAULT_VWD \
+ { \
+ -1.0, 1.0, -1.0, 1.0 \
+ }
+#define vpDEFAULT_DEPTH \
+ { \
+ 0.0, 1.0 \
+ }
+#define vpDEFAULT_TYPE PERSPECTIVE
+
+#define vpDEFAULT_VIEW \
+ { \
+ vpDEFAULT_TYPE, vpDEFAULT_COP, vpDEFAULT_VRP, vpDEFAULT_VPN, vpDEFAULT_VUP, vpDEFAULT_VWD, vpDEFAULT_DEPTH \
+ }
+
+#define vpDEFAULT_WC \
+ { \
+ 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0 \
+ }
/*
* CAMERA PARAMETERS
@@ -126,35 +151,34 @@
* twist Angle de rotation sur l'axe de visee (eye,target) en degres.
* speed Vitesse sur l'axe de visee (eye,target).
*/
-typedef struct {
- Point3f eye; /* position de l'observateur */
- Point3f target; /* point vise */
- float focal; /* focale de la camera */
- float angle; /* angle d'ouverture */
- float twist; /* rotation sur l'axe de visee */
- float speed; /* vitesse sur l'axe de visee */
+typedef struct {
+ Point3f eye; /* position de l'observateur */
+ Point3f target; /* point vise */
+ float focal; /* focale de la camera */
+ float angle; /* angle d'ouverture */
+ float twist; /* rotation sur l'axe de visee */
+ float speed; /* vitesse sur l'axe de visee */
} Camera_parameters;
-typedef struct {
- float umin,umax; /* bords gauche et droit */
- float vmin,vmax; /* bords inferieur et superieur */
+typedef struct {
+ float umin, umax; /* bords gauche et droit */
+ float vmin, vmax; /* bords inferieur et superieur */
} View_window;
-typedef struct {
- float front; /* plan avant ("hither") */
- float back; /* plan arriere ("yon") */
+typedef struct {
+ float front; /* plan avant ("hither") */
+ float back; /* plan arriere ("yon") */
} View_depth;
-typedef struct {
- Type type; /* type de la projection */
- Point3f cop; /* centre de projection */
- Point3f vrp; /* point de reference de visee */
- Vector vpn; /* vecteur nomal au plan */
- Vector vup; /* vecteur indiquant le "haut" */
- View_window vwd; /* fenetre de projection */
- View_depth depth; /* profondeurs de decoupages */
+typedef struct {
+ Type type; /* type de la projection */
+ Point3f cop; /* centre de projection */
+ Point3f vrp; /* point de reference de visee */
+ Vector vpn; /* vecteur nomal au plan */
+ Vector vup; /* vecteur indiquant le "haut" */
+ View_window vwd; /* fenetre de projection */
+ View_depth depth; /* profondeurs de decoupages */
} View_parameters;
#endif
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpViewio.cpp b/modules/robot/src/wireframe-simulator/vpViewio.cpp
index 0d7d2ee..82e2611 100644
--- a/modules/robot/src/wireframe-simulator/vpViewio.cpp
+++ b/modules/robot/src/wireframe-simulator/vpViewio.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,15 +44,14 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <stdio.h>
+#include <stdio.h>
-#include "vpMyio.h"
#include "vpArit.h"
-#include "vpViewio.h"
-#include "vpToken.h"
#include "vpLex.h"
+#include "vpMyio.h"
#include "vpSkipio.h"
-
+#include "vpToken.h"
+#include "vpViewio.h"
/*
* La procedure "fscanf_Remove" lit en ascii les parametres d'elimination
@@ -59,20 +59,35 @@
* Entree :
* bp Parametres a lire.
*/
-void fscanf_Remove (Byte *bp)
+void fscanf_Remove(Byte *bp)
{
- switch (lex ()) {
- case T_NONE : *bp = IS_INSIDE; break;
- case T_ABOVE : *bp |= IS_ABOVE; break;
- case T_BACK : *bp |= IS_BACK; break;
- case T_BELOW : *bp |= IS_BELOW; break;
- case T_FRONT : *bp |= IS_FRONT; break;
- case T_LEFT : *bp |= IS_LEFT; break;
- case T_RIGHT : *bp |= IS_RIGHT; break;
- default :
- lexerr ("start", "remove: keyword \"none|above|back|below|front|left|right\" expected");
- break;
- }
+ switch (lex()) {
+ case T_NONE:
+ *bp = IS_INSIDE;
+ break;
+ case T_ABOVE:
+ *bp |= IS_ABOVE;
+ break;
+ case T_BACK:
+ *bp |= IS_BACK;
+ break;
+ case T_BELOW:
+ *bp |= IS_BELOW;
+ break;
+ case T_FRONT:
+ *bp |= IS_FRONT;
+ break;
+ case T_LEFT:
+ *bp |= IS_LEFT;
+ break;
+ case T_RIGHT:
+ *bp |= IS_RIGHT;
+ break;
+ default:
+ lexerr("start", "remove: keyword "
+ "\"none|above|back|below|front|left|right\" expected");
+ break;
+ }
}
/*
@@ -81,74 +96,72 @@ void fscanf_Remove (Byte *bp)
* Entree :
* vp Parametres de visualisation a lire.
*/
-void fscanf_View_parameters (View_parameters *vp)
+void fscanf_View_parameters(View_parameters *vp)
{
- /* Lecture du type de projection lors de la prise de vue. */
-
- skip_keyword (T_TYPE, "view: keyword \"type\" expected");
- switch (lex ()) {
- case T_PARALLEL :
- vp->type = PARALLEL;
- break;
- case T_PERSPECTIVE :
- vp->type = PERSPECTIVE;
- break;
- default :
- lexerr ("start", "view_type: keyword \"parallel|perspective\" expected");
- break;
- }
-
- /* Lecture du centre de projection (oeil) de la prise de vue. */
-
- skip_keyword (T_COP, "view: keyword \"cop\" expected");
- pusherr ("view_cop: ");
- fscanf_Point3f (&vp->cop);
- poperr ();
-
- /* Lecture du point de reference (cible) a la prise de vue. */
-
- skip_keyword (T_VRP, "view: keyword \"vrp\" expected");
- pusherr ("view_vrp: ");
- fscanf_Point3f (&vp->vrp);
- poperr ();
-
- /* Lecture de la direction normale au plan de projection. */
-
- skip_keyword (T_VPN, "view: keyword \"vpn\" expected");
- pusherr ("view_vpn: ");
- fscanf_Vector (&vp->vpn);
- poperr ();
-
- /* Lecture de la direction indiquant le haut de la projection. */
-
- skip_keyword (T_VUP, "view: keyword \"vup\" expected");
- pusherr ("view_vup: ");
- fscanf_Vector (&vp->vup);
- poperr ();
-
- /* Lecture de la fenetre de projection de la prise de vue. */
-
- skip_keyword (T_WINDOW, "view: keyword \"window\" expected");
- pusherr ("view_window_umin: ");
- fscanf_float (&vp->vwd.umin);
- popuperr ("view_window_umax: ");
- fscanf_float (&vp->vwd.umax);
- popuperr ("view_window_vmin: ");
- fscanf_float (&vp->vwd.vmin);
- popuperr ("view_window_vmax: ");
- fscanf_float (&vp->vwd.vmax);
- poperr ();
-
- /* Lecture des profondeurs de decoupage avant et arriere. */
-
- skip_keyword (T_DEPTH, "view: keyword \"depth\" expected");
- pusherr ("view_depth_front: ");
- fscanf_float (&vp->depth.front);
- popuperr ("view_depth_back: ");
- fscanf_float (&vp->depth.back);
- poperr ();
+ /* Lecture du type de projection lors de la prise de vue. */
+
+ skip_keyword(T_TYPE, "view: keyword \"type\" expected");
+ switch (lex()) {
+ case T_PARALLEL:
+ vp->type = PARALLEL;
+ break;
+ case T_PERSPECTIVE:
+ vp->type = PERSPECTIVE;
+ break;
+ default:
+ lexerr("start", "view_type: keyword \"parallel|perspective\" expected");
+ break;
+ }
+
+ /* Lecture du centre de projection (oeil) de la prise de vue. */
+
+ skip_keyword(T_COP, "view: keyword \"cop\" expected");
+ pusherr("view_cop: ");
+ fscanf_Point3f(&vp->cop);
+ poperr();
+
+ /* Lecture du point de reference (cible) a la prise de vue. */
+
+ skip_keyword(T_VRP, "view: keyword \"vrp\" expected");
+ pusherr("view_vrp: ");
+ fscanf_Point3f(&vp->vrp);
+ poperr();
+
+ /* Lecture de la direction normale au plan de projection. */
+
+ skip_keyword(T_VPN, "view: keyword \"vpn\" expected");
+ pusherr("view_vpn: ");
+ fscanf_Vector(&vp->vpn);
+ poperr();
+
+ /* Lecture de la direction indiquant le haut de la projection. */
+
+ skip_keyword(T_VUP, "view: keyword \"vup\" expected");
+ pusherr("view_vup: ");
+ fscanf_Vector(&vp->vup);
+ poperr();
+
+ /* Lecture de la fenetre de projection de la prise de vue. */
+
+ skip_keyword(T_WINDOW, "view: keyword \"window\" expected");
+ pusherr("view_window_umin: ");
+ fscanf_float(&vp->vwd.umin);
+ popuperr("view_window_umax: ");
+ fscanf_float(&vp->vwd.umax);
+ popuperr("view_window_vmin: ");
+ fscanf_float(&vp->vwd.vmin);
+ popuperr("view_window_vmax: ");
+ fscanf_float(&vp->vwd.vmax);
+ poperr();
+
+ /* Lecture des profondeurs de decoupage avant et arriere. */
+
+ skip_keyword(T_DEPTH, "view: keyword \"depth\" expected");
+ pusherr("view_depth_front: ");
+ fscanf_float(&vp->depth.front);
+ popuperr("view_depth_back: ");
+ fscanf_float(&vp->depth.back);
+ poperr();
}
#endif
-
-
diff --git a/modules/robot/src/wireframe-simulator/vpViewio.h b/modules/robot/src/wireframe-simulator/vpViewio.h
index fc62fd2..0cec25a 100644
--- a/modules/robot/src/wireframe-simulator/vpViewio.h
+++ b/modules/robot/src/wireframe-simulator/vpViewio.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,8 +50,8 @@
#include "vpMy.h"
#include "vpView.h"
-void fscanf_Remove (Byte *bp);
-void fscanf_View_parameters (View_parameters *vp);
+void fscanf_Remove(Byte *bp);
+void fscanf_View_parameters(View_parameters *vp);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpVwstack.cpp b/modules/robot/src/wireframe-simulator/vpVwstack.cpp
index 02e99e3..64cb472 100644
--- a/modules/robot/src/wireframe-simulator/vpVwstack.cpp
+++ b/modules/robot/src/wireframe-simulator/vpVwstack.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,23 +41,21 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <stdio.h>
-#include <string.h>
-#include <limits>
-#include <cmath> // std::fabs()
-#include <stdarg.h>
+#include <cmath> // std::fabs()
+#include <limits>
+#include <stdarg.h>
+#include <stdio.h>
+#include <string.h>
-#include "vpMy.h"
#include "vpArit.h"
+#include "vpMy.h"
#include "vpView.h"
#include "vpVwstack.h"
+#define STACKSIZE 4
-#define STACKSIZE 4
-
-
-static View_parameters stack[STACKSIZE]= { vpDEFAULT_VIEW };
-static View_parameters *sp = stack;
+static View_parameters stack[STACKSIZE] = {vpDEFAULT_VIEW};
+static View_parameters *sp = stack;
/*
* La procedure "fprintf_vwstack" affiche un parametre du sommet
@@ -66,59 +65,59 @@ static View_parameters *sp = stack;
* argv Argument a afficher.
* Si argv est nul, tous les parametres sont affiches.
*/
-void
-fprintf_vwstack (FILE *fp, char *argv)
+void fprintf_vwstack(FILE *fp, char *argv)
{
- if (argv == NULL || strcmp (argv, "type") == 0) {
- const char *typetoa;
-
- switch (sp->type) {
- case PARALLEL :
- typetoa = "parallel";
- break;
- case PERSPECTIVE :
- typetoa = "perspective";
- break;
- default :
- typetoa = "unknown";
- break;
- }
- fprintf (fp, "(type\t%s)\n", typetoa);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "cop") == 0) {
- fprintf (fp, "(cop\t%.3f\t%.3f\t%.3f)\n",
- sp->cop.x, sp->cop.y, sp->cop.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vrp") == 0) {
- fprintf (fp, "(vrp\t%.3f\t%.3f\t%.3f)\n",
- sp->vrp.x, sp->vrp.y, sp->vrp.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vpn") == 0) {
- fprintf (fp, "(vpn\t%.3f\t%.3f\t%.3f)\n",
- sp->vpn.x, sp->vpn.y, sp->vpn.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "vup") == 0) {
- fprintf (fp, "(vup\t%.3f\t%.3f\t%.3f)\n",
- sp->vup.x, sp->vup.y, sp->vup.z);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "window") == 0) {
- fprintf (fp, "(window\t%.3f\t%.3f\t%.3f\t%.3f)\n",
- sp->vwd.umin,sp->vwd.umax,sp->vwd.vmin,sp->vwd.vmax);
- if (argv != NULL) return;
- }
- if (argv == NULL || strcmp (argv, "depth") == 0) {
- fprintf (fp, "(depth\t%.3f\t%.3f)\n",
- sp->depth.front, sp->depth.back);
- if (argv != NULL) return;
- }
+ if (argv == NULL || strcmp(argv, "type") == 0) {
+ const char *typetoa;
+
+ switch (sp->type) {
+ case PARALLEL:
+ typetoa = "parallel";
+ break;
+ case PERSPECTIVE:
+ typetoa = "perspective";
+ break;
+ default:
+ typetoa = "unknown";
+ break;
+ }
+ fprintf(fp, "(type\t%s)\n", typetoa);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "cop") == 0) {
+ fprintf(fp, "(cop\t%.3f\t%.3f\t%.3f)\n", sp->cop.x, sp->cop.y, sp->cop.z);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "vrp") == 0) {
+ fprintf(fp, "(vrp\t%.3f\t%.3f\t%.3f)\n", sp->vrp.x, sp->vrp.y, sp->vrp.z);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "vpn") == 0) {
+ fprintf(fp, "(vpn\t%.3f\t%.3f\t%.3f)\n", sp->vpn.x, sp->vpn.y, sp->vpn.z);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "vup") == 0) {
+ fprintf(fp, "(vup\t%.3f\t%.3f\t%.3f)\n", sp->vup.x, sp->vup.y, sp->vup.z);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "window") == 0) {
+ fprintf(fp, "(window\t%.3f\t%.3f\t%.3f\t%.3f)\n", sp->vwd.umin, sp->vwd.umax, sp->vwd.vmin, sp->vwd.vmax);
+ if (argv != NULL)
+ return;
+ }
+ if (argv == NULL || strcmp(argv, "depth") == 0) {
+ fprintf(fp, "(depth\t%.3f\t%.3f)\n", sp->depth.front, sp->depth.back);
+ if (argv != NULL)
+ return;
+ }
if (argv != NULL) {
- static char proc_name[] = "fprintf_vwstack";
- fprintf (stderr, "%s: argument unknown\n", proc_name);
+ static char proc_name[] = "fprintf_vwstack";
+ fprintf(stderr, "%s: argument unknown\n", proc_name);
}
}
@@ -128,11 +127,7 @@ fprintf_vwstack (FILE *fp, char *argv)
* Sortie :
* Pointeur sur le point de vue du sommet de la pile.
*/
-View_parameters *
-get_vwstack (void)
-{
- return (sp);
-}
+View_parameters *get_vwstack(void) { return (sp); }
/*
* La procedure "load_vwstack" charge un point de vue au sommet
@@ -140,54 +135,47 @@ get_vwstack (void)
* Entree :
* vp Point de vue a charger.
*/
-void
-load_vwstack (View_parameters *vp)
-{
- *sp = *vp;
-}
+void load_vwstack(View_parameters *vp) { *sp = *vp; }
/*
* La procedure "pop_vwstack" depile le point de vue au sommet
* de la pile des points de vue.
*/
-void
-pop_vwstack (void)
+void pop_vwstack(void)
{
- if (sp == stack) {
- static char proc_name[] = "pop_vwstack";
- fprintf (stderr, "%s: stack underflow\n", proc_name);
- return;
- }
- else sp--;
+ if (sp == stack) {
+ static char proc_name[] = "pop_vwstack";
+ fprintf(stderr, "%s: stack underflow\n", proc_name);
+ return;
+ } else
+ sp--;
}
/*
* La procedure "push_vwstack" empile et duplique le point de vue au sommet
* de la pile des points de vue.
*/
-void
-push_vwstack (void)
+void push_vwstack(void)
{
- if (sp == stack + STACKSIZE - 1) {
- static char proc_name[] = "push_vwstack";
- fprintf (stderr, "%s: stack overflow\n", proc_name);
- return;
- }
- sp++;
- *sp = *(sp - 1);
+ if (sp == stack + STACKSIZE - 1) {
+ static char proc_name[] = "push_vwstack";
+ fprintf(stderr, "%s: stack overflow\n", proc_name);
+ return;
+ }
+ sp++;
+ *sp = *(sp - 1);
}
/*
* La procedure "swap_vwstack" echange les deux premiers elements
* de la pile des points de vue.
*/
-void
-swap_vwstack (void)
+void swap_vwstack(void)
{
- View_parameters *vp, tmp;
+ View_parameters *vp, tmp;
- vp = (sp == stack) ? sp + 1 : sp - 1;
- SWAP(*sp, *vp, tmp);
+ vp = (sp == stack) ? sp + 1 : sp - 1;
+ SWAP(*sp, *vp, tmp);
}
/*
@@ -197,82 +185,69 @@ swap_vwstack (void)
* va_alist Nom de l'argument a modifier suivi de ses parametres.
*/
-void
-add_vwstack (const char* path, ... )
-//add_vwstack (va_alist)
+void add_vwstack(const char *path, ...)
+// add_vwstack (va_alist)
// va_dcl
{
- va_list ap;
- char *argv;
-
- va_start (ap,path);
- argv = va_arg (ap, char *);
- if (strcmp (argv, "cop") == 0) {
- /* initialise le centre de projection */
- SET_COORD3(sp->cop,
- (float) va_arg (ap, double),
- (float) va_arg (ap, double),
- (float) va_arg (ap, double));
- }
- else if (strcmp (argv, "depth") == 0) {
- /* initialise les distances des plans de decoupage */
- sp->depth.front = (float) va_arg (ap, double);
- sp->depth.back = (float) va_arg (ap, double);
- }
- else if (strcmp (argv, "type") == 0) {
- /* initialise le type de projection */
- sp->type = (Type) va_arg (ap, int);
- }
- else if (strcmp (argv, "vpn") == 0) {
- /* initialise le vecteur normal au plan */
- float x = (float) va_arg (ap, double);
- float y = (float) va_arg (ap, double);
- float z = (float) va_arg (ap, double);
-
- //if (x == 0 && y == 0 && z == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
- static char proc_name[] = "add_vwstack";
- fprintf (stderr, "%s: bad vpn\n", proc_name);
+ va_list ap;
+ char *argv;
+
+ va_start(ap, path);
+ argv = va_arg(ap, char *);
+ if (strcmp(argv, "cop") == 0) {
+ /* initialise le centre de projection */
+ SET_COORD3(sp->cop, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double));
+ } else if (strcmp(argv, "depth") == 0) {
+ /* initialise les distances des plans de decoupage */
+ sp->depth.front = (float)va_arg(ap, double);
+ sp->depth.back = (float)va_arg(ap, double);
+ } else if (strcmp(argv, "type") == 0) {
+ /* initialise le type de projection */
+ sp->type = (Type)va_arg(ap, int);
+ } else if (strcmp(argv, "vpn") == 0) {
+ /* initialise le vecteur normal au plan */
+ float x = (float)va_arg(ap, double);
+ float y = (float)va_arg(ap, double);
+ float z = (float)va_arg(ap, double);
+
+ // if (x == 0 && y == 0 && z == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(y) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
+ static char proc_name[] = "add_vwstack";
+ fprintf(stderr, "%s: bad vpn\n", proc_name);
+ } else {
+ SET_COORD3(sp->vpn, x, y, z);
}
- else {
- SET_COORD3(sp->vpn,x,y,z);
- }
- }
- else if (strcmp (argv, "vrp") == 0) {
- /* initialise le vecteur de reference */
- SET_COORD3(sp->vrp,
- (float) va_arg (ap, double),
- (float) va_arg (ap, double),
- (float) va_arg (ap, double));
- }
- else if (strcmp (argv, "vup") == 0) {
- /* initialise le vecteur haut du plan */
- float x = (float) va_arg (ap, double);
- float y = (float) va_arg (ap, double);
- float z = (float) va_arg (ap, double);
-
- //if (x == 0 && y == 0 && z == 0)
- if (std::fabs(x) <= std::numeric_limits<double>::epsilon() && std::fabs(y) <= std::numeric_limits<double>::epsilon() && std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
- static char proc_name[] = "add_vwstack";
- fprintf (stderr, "%s: bad vup\n", proc_name);
+ } else if (strcmp(argv, "vrp") == 0) {
+ /* initialise le vecteur de reference */
+ SET_COORD3(sp->vrp, (float)va_arg(ap, double), (float)va_arg(ap, double), (float)va_arg(ap, double));
+ } else if (strcmp(argv, "vup") == 0) {
+ /* initialise le vecteur haut du plan */
+ float x = (float)va_arg(ap, double);
+ float y = (float)va_arg(ap, double);
+ float z = (float)va_arg(ap, double);
+
+ // if (x == 0 && y == 0 && z == 0)
+ if (std::fabs(x) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(y) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(z) <= std::numeric_limits<double>::epsilon()) {
+ static char proc_name[] = "add_vwstack";
+ fprintf(stderr, "%s: bad vup\n", proc_name);
+ } else {
+ SET_COORD3(sp->vup, x, y, z);
}
-else {
- SET_COORD3(sp->vup,x,y,z);
- }
- }
- else if (strcmp (argv, "window") == 0) {
- /* initialise la fenetre de projection */
- sp->vwd.umin = (float) va_arg (ap, double);
- sp->vwd.umax = (float) va_arg (ap, double);
- sp->vwd.vmin = (float) va_arg (ap, double);
- sp->vwd.vmax = (float) va_arg (ap, double);
- }
- else {
- static char proc_name[] = "add_vwstack";
- fprintf (stderr, "%s: bad argument\n", proc_name);
+ } else if (strcmp(argv, "window") == 0) {
+ /* initialise la fenetre de projection */
+ sp->vwd.umin = (float)va_arg(ap, double);
+ sp->vwd.umax = (float)va_arg(ap, double);
+ sp->vwd.vmin = (float)va_arg(ap, double);
+ sp->vwd.vmax = (float)va_arg(ap, double);
+ } else {
+ static char proc_name[] = "add_vwstack";
+ fprintf(stderr, "%s: bad argument\n", proc_name);
}
- va_end (ap);
+ va_end(ap);
}
#endif
-
diff --git a/modules/robot/src/wireframe-simulator/vpVwstack.h b/modules/robot/src/wireframe-simulator/vpVwstack.h
index 9b14a97..911801f 100644
--- a/modules/robot/src/wireframe-simulator/vpVwstack.h
+++ b/modules/robot/src/wireframe-simulator/vpVwstack.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,13 +47,13 @@
#include "vpView.h"
-void fprintf_vwstack (FILE *fp, char *argv);
-View_parameters *get_vwstack (void);
-void load_vwstack (View_parameters *vp);
-void pop_vwstack (void);
-void push_vwstack (void);
-void swap_vwstack (void);
-void add_vwstack (const char* path, ...);
+void fprintf_vwstack(FILE *fp, char *argv);
+View_parameters *get_vwstack(void);
+void load_vwstack(View_parameters *vp);
+void pop_vwstack(void);
+void push_vwstack(void);
+void swap_vwstack(void);
+void add_vwstack(const char *path, ...);
#endif
#endif
diff --git a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp
index 7090a9b..87ffb70 100644
--- a/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp
+++ b/modules/robot/src/wireframe-simulator/vpWireFrameSimulator.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,83 +41,74 @@
\brief Implementation of a wire frame simulator.
*/
-#include <visp3/robot/vpWireFrameSimulator.h>
#include <fcntl.h>
-#include <string.h>
#include <stdio.h>
+#include <string.h>
#include <vector>
+#include <visp3/robot/vpWireFrameSimulator.h>
-#include "vpClipping.h"
-#include "vpToken.h"
-#include "vpMy.h"
-#include "vpLex.h"
+#include "vpArit.h"
#include "vpBound.h"
+#include "vpClipping.h"
+#include "vpCoreDisplay.h"
#include "vpKeyword.h"
+#include "vpLex.h"
+#include "vpMy.h"
#include "vpParser.h"
-#include "vpArit.h"
-#include "vpView.h"
-#include "vpRfstack.h"
-#include "vpVwstack.h"
-#include "vpTmstack.h"
-#include "vpCoreDisplay.h"
-#include "vpBound.h"
#include "vpProjection.h"
+#include "vpRfstack.h"
#include "vpScene.h"
+#include "vpTmstack.h"
+#include "vpToken.h"
+#include "vpView.h"
+#include "vpVwstack.h"
-#include <visp3/core/vpException.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpIoTools.h>
extern Point2i *point2i;
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<vpRGBa> &I, const vpColor &color)
+void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<vpRGBa> &I, const vpColor &color)
{
- // extern Bound *clipping_Bound ();
+ // extern Bound *clipping_Bound ();
Bound *bp, *bend;
Bound *clip; /* surface apres clipping */
- Byte b = (Byte) *get_rfstack ();
+ Byte b = (Byte)*get_rfstack();
Matrix m;
- //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
- memmove((char *) m, (char *) mat, sizeof (Matrix));
- View_to_Matrix (get_vwstack (), *(get_tmstack ()));
- postmult_matrix (m, *(get_tmstack ()));
- bp = sc.bound.ptr;
+ // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
+ memmove((char *)m, (char *)mat, sizeof(Matrix));
+ View_to_Matrix(get_vwstack(), *(get_tmstack()));
+ postmult_matrix(m, *(get_tmstack()));
+ bp = sc.bound.ptr;
bend = bp + sc.bound.nbr;
- for (; bp < bend; bp++)
- {
- if ((clip = clipping_Bound (bp, m)) != NULL)
- {
- Face *fp = clip->face.ptr;
+ for (; bp < bend; bp++) {
+ if ((clip = clipping_Bound(bp, m)) != NULL) {
+ Face *fp = clip->face.ptr;
Face *fend = fp + clip->face.nbr;
- set_Bound_face_display (clip, b); //regarde si is_visible
+ set_Bound_face_display(clip, b); // regarde si is_visible
- point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
- for (; fp < fend; fp++)
- {
- if (fp->is_visible)
- {
- wireframe_Face (fp, point2i);
+ 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_);
+ for (int i = 1; i < fp->vertex.nbr; i++) {
+ vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
+ thickness_);
pt++;
}
- if (fp->vertex.nbr > 2)
- {
- vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
+ if (fp->vertex.nbr > 2) {
+ vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
+ color, thickness_);
}
}
}
@@ -127,46 +119,42 @@ vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<v
/*
Copy the scene corresponding to the registeresd parameters in the image.
*/
-void
-vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I, const vpColor &color)
+void vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<unsigned char> &I,
+ const vpColor &color)
{
- //extern Bound *clipping_Bound ();
+ // extern Bound *clipping_Bound ();
Bound *bp, *bend;
Bound *clip; /* surface apres clipping */
- Byte b = (Byte) *get_rfstack ();
+ Byte b = (Byte)*get_rfstack();
Matrix m;
- //bcopy ((char *) mat, (char *) m, sizeof (Matrix));
- memmove((char *) m, (char *) mat, sizeof (Matrix));
- View_to_Matrix (get_vwstack (), *(get_tmstack ()));
- postmult_matrix (m, *(get_tmstack ()));
- bp = sc.bound.ptr;
+ // bcopy ((char *) mat, (char *) m, sizeof (Matrix));
+ memmove((char *)m, (char *)mat, sizeof(Matrix));
+ View_to_Matrix(get_vwstack(), *(get_tmstack()));
+ postmult_matrix(m, *(get_tmstack()));
+ bp = sc.bound.ptr;
bend = bp + sc.bound.nbr;
- for (; bp < bend; bp++)
- {
- if ((clip = clipping_Bound (bp, m)) != NULL)
- {
- Face *fp = clip->face.ptr;
+ for (; bp < bend; bp++) {
+ if ((clip = clipping_Bound(bp, m)) != NULL) {
+ Face *fp = clip->face.ptr;
Face *fend = fp + clip->face.nbr;
- set_Bound_face_display (clip, b); //regarde si is_visible
+ set_Bound_face_display(clip, b); // regarde si is_visible
- point_3D_2D (clip->point.ptr, clip->point.nbr,(int)I.getWidth(),(int)I.getHeight(),point2i);
- for (; fp < fend; fp++)
- {
- if (fp->is_visible)
- {
- wireframe_Face (fp, point2i);
+ 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_);
+ for (int i = 1; i < fp->vertex.nbr; i++) {
+ vpDisplay::displayLine(I, vpImagePoint((pt)->y, (pt)->x), vpImagePoint((pt + 1)->y, (pt + 1)->x), color,
+ thickness_);
pt++;
}
- if (fp->vertex.nbr > 2)
- {
- vpDisplay::displayLine(I,vpImagePoint((listpoint2i)->y,(listpoint2i)->x),vpImagePoint((pt)->y,(pt)->x),color,thickness_);
+ if (fp->vertex.nbr > 2) {
+ vpDisplay::displayLine(I, vpImagePoint((listpoint2i)->y, (listpoint2i)->x), vpImagePoint((pt)->y, (pt)->x),
+ color, thickness_);
}
}
}
@@ -178,52 +166,49 @@ vpWireFrameSimulator::display_scene(Matrix mat, Bound_scene &sc, const vpImage<u
/*!
Basic constructor.
-
+
Set the path to the scene files (*.bnd and *.sln) used by the
simulator. If the path set in vpConfig.h in VISP_SCENES_DIR macro is
not valid, the path is set from the VISP_SCENES_DIR environment
variable that the user has to set.
*/
vpWireFrameSimulator::vpWireFrameSimulator()
- : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(),
- refMo(), cMo(), cdMo(), object(PLATE), desiredObject(D_STANDARD),
- camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
- desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true),
- cameraTrajectory(), poseList(), fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(),
- old_iPt(), blockedr(false), blockedz(false), blockedt(false), blocked(false),
- camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
- displayDesiredObject(false), displayCamera(false), displayImageSimulator(false),
- cameraFactor(1.), camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
+ : scene(), desiredScene(), camera(), objectImage(), fMo(), fMc(), camMf(), refMo(), cMo(), cdMo(), object(PLATE),
+ desiredObject(D_STANDARD), camColor(vpColor::green), camTrajColor(vpColor::green), curColor(vpColor::blue),
+ desColor(vpColor::red), sceneInitialized(false), displayCameraTrajectory(true), cameraTrajectory(), poseList(),
+ fMoList(), nbrPtLimit(1000), old_iPr(), old_iPz(), old_iPt(), blockedr(false), blockedz(false), blockedt(false),
+ blocked(false), camMf2(), f2Mf(), px_int(1), py_int(1), px_ext(1), py_ext(1), displayObject(false),
+ displayDesiredObject(false), displayCamera(false), displayImageSimulator(false), cameraFactor(1.),
+ camTrajType(CT_LINE), extCamChanged(false), rotz(), thickness_(1), scene_dir()
{
// set scene_dir from #define VISP_SCENE_DIR if it exists
// VISP_SCENES_DIR may contain multiple locations separated by ";"
std::vector<std::string> scene_dirs = vpIoTools::splitChain(std::string(VISP_SCENES_DIR), std::string(";"));
bool sceneDirExists = false;
- for(size_t i=0; i < scene_dirs.size(); i++)
- if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
- scene_dir = scene_dirs[i];
- sceneDirExists = true;
- break;
- }
- if (! sceneDirExists) {
+ for (size_t i = 0; i < scene_dirs.size(); i++)
+ if (vpIoTools::checkDirectory(scene_dirs[i]) == true) { // directory exists
+ scene_dir = scene_dirs[i];
+ sceneDirExists = true;
+ break;
+ }
+ if (!sceneDirExists) {
try {
scene_dir = vpIoTools::getenv("VISP_SCENES_DIR");
std::cout << "The simulator uses data from VISP_SCENES_DIR=" << scene_dir << std::endl;
- }
- catch (...) {
+ } catch (...) {
std::cout << "Cannot get VISP_SCENES_DIR environment variable" << std::endl;
}
}
-
+
open_display();
open_clipping();
- old_iPr = vpImagePoint(-1,-1);
- old_iPz = vpImagePoint(-1,-1);
- old_iPt = vpImagePoint(-1,-1);
-
- rotz.buildFrom(0,0,0,0,0,vpMath::rad(180));
-
+ old_iPr = vpImagePoint(-1, -1);
+ old_iPz = vpImagePoint(-1, -1);
+ old_iPt = vpImagePoint(-1, -1);
+
+ rotz.buildFrom(0, 0, 0, 0, 0, vpMath::rad(180));
+
scene.name = NULL;
scene.bound.ptr = NULL;
scene.bound.nbr = 0;
@@ -237,42 +222,43 @@ vpWireFrameSimulator::vpWireFrameSimulator()
camera.bound.nbr = 0;
}
-
/*!
Basic destructor
*/
vpWireFrameSimulator::~vpWireFrameSimulator()
{
- if(sceneInitialized)
- {
- if(displayObject)
- free_Bound_scene (&(this->scene));
- if(displayCamera){
- free_Bound_scene (&(this->camera));
+ if (sceneInitialized) {
+ if (displayObject)
+ free_Bound_scene(&(this->scene));
+ if (displayCamera) {
+ free_Bound_scene(&(this->camera));
}
- if(displayDesiredObject)
- free_Bound_scene (&(this->desiredScene));
+ if (displayDesiredObject)
+ free_Bound_scene(&(this->desiredScene));
}
close_clipping();
- close_display ();
+ close_display();
cameraTrajectory.clear();
poseList.clear();
fMoList.clear();
}
-
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position and at the desired
+ position.
+
+ It exists several default scenes you can use. Use the vpSceneObject and the
+ vpSceneDesiredObject attributes to use them in this method. The
+ corresponding files are stored in the "data" folder which is in the ViSP
+ build directory.
+
+ \param obj : Type of scene used to display the object at the current
+ position. \param desired_object : Type of scene used to display the object
+ at the desired pose (in the internal view).
*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
+void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object)
{
char name_cam[FILENAME_MAX];
char name[FILENAME_MAX];
@@ -282,71 +268,121 @@ vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredOb
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_);
- if (desiredObject != D_TOOL)
- {
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
- }
- else
- {
- strcat(name_cam,"/tool.bnd");
- set_scene(name_cam,&(this->camera),1.0);
+ if (desiredObject != D_TOOL) {
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
+ } else {
+ strcat(name_cam, "/tool.bnd");
+ set_scene(name_cam, &(this->camera), 1.0);
}
strcpy(name, scene_dir_);
- switch (obj)
- {
- case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
- case CUBE : { strcat(name, "/cube.bnd"); break; }
- case PLATE : { strcat(name, "/plate.bnd"); break; }
- case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
- case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
- case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
- case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
- case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
- case THREE_LINES : { strcat(name, "/line.bnd"); break; }
- case ROAD : { strcat(name, "/road.bnd"); break; }
- case TIRE : { strcat(name, "/circles2.bnd"); break; }
- case PIPE : { strcat(name, "/pipe.bnd"); break; }
- case CIRCLE : { strcat(name, "/circle.bnd"); break; }
- case SPHERE : { strcat(name, "/sphere.bnd"); break; }
- case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
- case PLAN: { strcat(name, "/plan.bnd"); break; }
- case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
- }
- set_scene(name,&(this->scene),1.0);
+ switch (obj) {
+ case THREE_PTS: {
+ strcat(name, "/3pts.bnd");
+ break;
+ }
+ case CUBE: {
+ strcat(name, "/cube.bnd");
+ break;
+ }
+ case PLATE: {
+ strcat(name, "/plate.bnd");
+ break;
+ }
+ case SMALL_PLATE: {
+ strcat(name, "/plate_6cm.bnd");
+ break;
+ }
+ case RECTANGLE: {
+ strcat(name, "/rectangle.bnd");
+ break;
+ }
+ case SQUARE_10CM: {
+ strcat(name, "/square10cm.bnd");
+ break;
+ }
+ case DIAMOND: {
+ strcat(name, "/diamond.bnd");
+ break;
+ }
+ case TRAPEZOID: {
+ strcat(name, "/trapezoid.bnd");
+ break;
+ }
+ case THREE_LINES: {
+ strcat(name, "/line.bnd");
+ break;
+ }
+ case ROAD: {
+ strcat(name, "/road.bnd");
+ break;
+ }
+ case TIRE: {
+ strcat(name, "/circles2.bnd");
+ break;
+ }
+ case PIPE: {
+ strcat(name, "/pipe.bnd");
+ break;
+ }
+ case CIRCLE: {
+ strcat(name, "/circle.bnd");
+ break;
+ }
+ case SPHERE: {
+ strcat(name, "/sphere.bnd");
+ break;
+ }
+ case CYLINDER: {
+ strcat(name, "/cylinder.bnd");
+ break;
+ }
+ case PLAN: {
+ strcat(name, "/plan.bnd");
+ break;
+ }
+ case POINT_CLOUD: {
+ strcat(name, "/point_cloud.bnd");
+ break;
+ }
+ }
+ set_scene(name, &(this->scene), 1.0);
scene_dir_ = scene_dir.c_str();
if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the desired object name"));
- }
-
- switch (desiredObject)
- {
- case D_STANDARD : { break; }
- case D_CIRCLE : {
- strcpy(name, scene_dir_);
- strcat(name, "/circle_sq2.bnd");
- break; }
- case D_TOOL : {
- strcpy(name, scene_dir_);
- strcat(name, "/tool.bnd");
- break; }
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the desired object name"));
+ }
+
+ switch (desiredObject) {
+ case D_STANDARD: {
+ break;
+ }
+ case D_CIRCLE: {
+ strcpy(name, scene_dir_);
+ strcat(name, "/circle_sq2.bnd");
+ break;
+ }
+ case D_TOOL: {
+ strcpy(name, scene_dir_);
+ strcat(name, "/tool.bnd");
+ break;
+ }
}
set_scene(name, &(this->desiredScene), 1.0);
- if (obj == PIPE) load_rfstack(IS_INSIDE);
- else add_rfstack(IS_BACK);
+ if (obj == PIPE)
+ load_rfstack(IS_INSIDE);
+ else
+ add_rfstack(IS_BACK);
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
sceneInitialized = true;
displayObject = true;
@@ -356,92 +392,94 @@ vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredOb
}
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- It exists several default scenes you can use. Use the vpSceneObject and the vpSceneDesiredObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
-
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
-
- \param obj : Type of scene used to display the object at the current position.
- \param desired_object : Type of scene used to display the object at the desired pose (in the internal view).
- \param imObj : A list of vpImageSimulator instances.
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position and at the desired
+ position.
+
+ It exists several default scenes you can use. Use the vpSceneObject and the
+ vpSceneDesiredObject attributes to use them in this method. The
+ corresponding files are stored in the "data" folder which is in the ViSP
+ build directory.
+
+ It is also possible to add a list of vpImageSimulator instances. They will
+ be automatically projected into the image. The position of the four corners
+ have to be given in the object frame.
+
+ \param obj : Type of scene used to display the object at the current
+ position. \param desired_object : Type of scene used to display the object
+ at the desired pose (in the internal view). \param imObj : A list of
+ vpImageSimulator instances.
*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object, const std::list<vpImageSimulator> &imObj)
+void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desired_object,
+ const std::list<vpImageSimulator> &imObj)
{
initScene(obj, desired_object);
objectImage = imObj;
displayImageSimulator = true;
}
-
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position and at the desired
+ position.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a
+ .wrl file which is a 3D model file.
\param obj : Path to the scene file you want to use.
\param desired_object : Path to the scene file you want to use.
*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
+void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object)
{
char name_cam[FILENAME_MAX];
char name[FILENAME_MAX];
object = THREE_PTS;
this->desiredObject = D_STANDARD;
-
+
const char *scene_dir_ = scene_dir.c_str();
if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
}
strcpy(name_cam, scene_dir_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
if (strlen(obj) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the name"));
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name"));
}
- strcpy(name,obj);
+ strcpy(name, obj);
Model_3D model;
model = getExtension(obj);
if (model == BND_MODEL)
- set_scene(name,&(this->scene),1.0);
+ set_scene(name, &(this->scene), 1.0);
else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->scene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
+ set_scene_wrl(name, &(this->scene), 1.0);
+ else if (model == UNKNOWN_MODEL) {
vpERROR_TRACE("Unknown file extension for the 3D model");
}
if (strlen(desired_object) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
}
- strcpy(name,desired_object);
+ strcpy(name, desired_object);
model = getExtension(desired_object);
if (model == BND_MODEL)
- set_scene(name,&(this->desiredScene),1.0);
+ set_scene(name, &(this->desiredScene), 1.0);
else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->desiredScene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
+ set_scene_wrl(name, &(this->desiredScene), 1.0);
+ else if (model == UNKNOWN_MODEL) {
vpERROR_TRACE("Unknown file extension for the 3D model");
}
add_rfstack(IS_BACK);
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
sceneInitialized = true;
displayObject = true;
@@ -450,19 +488,23 @@ vpWireFrameSimulator::initScene(const char* obj, const char* desired_object)
}
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position and at the desired position.
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position and at the desired
+ position.
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+ Here you can use the scene you want. You have to set the path to a .bnd or a
+ .wrl file which is a 3D model file.
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+ It is also possible to add a list of vpImageSimulator instances. They will
+ be automatically projected into the image. The position of the four corners
+ have to be given in the object frame.
\param obj : Path to the scene file you want to use.
\param desired_object : Path to the scene file you want to use.
\param imObj : A list of vpImageSimulator instances.
*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, const std::list<vpImageSimulator> &imObj)
+void vpWireFrameSimulator::initScene(const char *obj, const char *desired_object,
+ const std::list<vpImageSimulator> &imObj)
{
initScene(obj, desired_object);
objectImage = imObj;
@@ -470,15 +512,18 @@ vpWireFrameSimulator::initScene(const char* obj, const char* desired_object, con
}
/*!
- Initialize the simulator. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+ Initialize the simulator. It enables to choose the type of object which will
+ be used to display the object at the current position. The object at the
+ desired position is not displayed.
- \param obj : Type of scene used to display the object at the current position.
+ It exists several default scenes you can use. Use the vpSceneObject
+ attributes to use them in this method. The corresponding files are stored in
+ the "data" folder which is in the ViSP build directory.
+
+ \param obj : Type of scene used to display the object at the current
+ position.
*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj)
+void vpWireFrameSimulator::initScene(const vpSceneObject &obj)
{
char name_cam[FILENAME_MAX];
char name[FILENAME_MAX];
@@ -487,43 +532,94 @@ 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_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
strcpy(name, scene_dir_);
- switch (obj)
- {
- case THREE_PTS : {strcat(name,"/3pts.bnd"); break; }
- case CUBE : { strcat(name, "/cube.bnd"); break; }
- case PLATE : { strcat(name, "/plate.bnd"); break; }
- case SMALL_PLATE : { strcat(name, "/plate_6cm.bnd"); break; }
- case RECTANGLE : { strcat(name, "/rectangle.bnd"); break; }
- case SQUARE_10CM : { strcat(name, "/square10cm.bnd"); break; }
- case DIAMOND : { strcat(name, "/diamond.bnd"); break; }
- case TRAPEZOID : { strcat(name, "/trapezoid.bnd"); break; }
- case THREE_LINES : { strcat(name, "/line.bnd"); break; }
- case ROAD : { strcat(name, "/road.bnd"); break; }
- case TIRE : { strcat(name, "/circles2.bnd"); break; }
- case PIPE : { strcat(name, "/pipe.bnd"); break; }
- case CIRCLE : { strcat(name, "/circle.bnd"); break; }
- case SPHERE : { strcat(name, "/sphere.bnd"); break; }
- case CYLINDER : { strcat(name, "/cylinder.bnd"); break; }
- case PLAN: { strcat(name, "/plan.bnd"); break; }
- case POINT_CLOUD: { strcat(name, "/point_cloud.bnd"); break; }
- }
- set_scene(name,&(this->scene),1.0);
-
- if (obj == PIPE) load_rfstack(IS_INSIDE);
- else add_rfstack(IS_BACK);
-
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
+ switch (obj) {
+ case THREE_PTS: {
+ strcat(name, "/3pts.bnd");
+ break;
+ }
+ case CUBE: {
+ strcat(name, "/cube.bnd");
+ break;
+ }
+ case PLATE: {
+ strcat(name, "/plate.bnd");
+ break;
+ }
+ case SMALL_PLATE: {
+ strcat(name, "/plate_6cm.bnd");
+ break;
+ }
+ case RECTANGLE: {
+ strcat(name, "/rectangle.bnd");
+ break;
+ }
+ case SQUARE_10CM: {
+ strcat(name, "/square10cm.bnd");
+ break;
+ }
+ case DIAMOND: {
+ strcat(name, "/diamond.bnd");
+ break;
+ }
+ case TRAPEZOID: {
+ strcat(name, "/trapezoid.bnd");
+ break;
+ }
+ case THREE_LINES: {
+ strcat(name, "/line.bnd");
+ break;
+ }
+ case ROAD: {
+ strcat(name, "/road.bnd");
+ break;
+ }
+ case TIRE: {
+ strcat(name, "/circles2.bnd");
+ break;
+ }
+ case PIPE: {
+ strcat(name, "/pipe.bnd");
+ break;
+ }
+ case CIRCLE: {
+ strcat(name, "/circle.bnd");
+ break;
+ }
+ case SPHERE: {
+ strcat(name, "/sphere.bnd");
+ break;
+ }
+ case CYLINDER: {
+ strcat(name, "/cylinder.bnd");
+ break;
+ }
+ case PLAN: {
+ strcat(name, "/plan.bnd");
+ break;
+ }
+ case POINT_CLOUD: {
+ strcat(name, "/point_cloud.bnd");
+ break;
+ }
+ }
+ set_scene(name, &(this->scene), 1.0);
+
+ if (obj == PIPE)
+ load_rfstack(IS_INSIDE);
+ else
+ add_rfstack(IS_BACK);
+
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
sceneInitialized = true;
displayObject = true;
@@ -534,18 +630,22 @@ vpWireFrameSimulator::initScene(const vpSceneObject &obj)
}
/*!
- Initialize the simulator. It enables to choose the type of object which will be used to display the object
- at the current position. The object at the desired position is not displayed.
+ Initialize the simulator. It enables to choose the type of object which will
+ be used to display the object at the current position. The object at the
+ desired position is not displayed.
- It exists several default scenes you can use. Use the vpSceneObject attributes to use them in this method. The corresponding files are stored in the "data" folder which is in the ViSP build directory.
+ It exists several default scenes you can use. Use the vpSceneObject
+ attributes to use them in this method. The corresponding files are stored in
+ the "data" folder which is in the ViSP build directory.
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+ It is also possible to add a list of vpImageSimulator instances. They will
+ be automatically projected into the image. The position of the four corners
+ have to be given in the object frame.
- \param obj : Type of scene used to display the object at the current position.
- \param imObj : A list of vpImageSimulator instances.
+ \param obj : Type of scene used to display the object at the current
+ position. \param imObj : A list of vpImageSimulator instances.
*/
-void
-vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
+void vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImageSimulator> &imObj)
{
initScene(obj);
objectImage = imObj;
@@ -553,53 +653,51 @@ vpWireFrameSimulator::initScene(const vpSceneObject &obj, const std::list<vpImag
}
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
-
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position. The object at the
+ desired position is not displayed.
+
+ Here you can use the scene you want. You have to set the path to a .bnd or a
+ .wrl file which is a 3D model file.
\param obj : Path to the scene file you want to use.
*/
-void
-vpWireFrameSimulator::initScene(const char* obj)
+void vpWireFrameSimulator::initScene(const char *obj)
{
char name_cam[FILENAME_MAX];
char name[FILENAME_MAX];
object = THREE_PTS;
-
+
const char *scene_dir_ = scene_dir.c_str();
if (strlen(scene_dir_) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the camera name"));
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the camera name"));
}
strcpy(name_cam, scene_dir_);
- strcat(name_cam,"/camera.bnd");
- set_scene(name_cam,&camera,cameraFactor);
+ strcat(name_cam, "/camera.bnd");
+ set_scene(name_cam, &camera, cameraFactor);
if (strlen(obj) >= FILENAME_MAX) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the name"));
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the name"));
}
- strcpy(name,obj);
+ strcpy(name, obj);
Model_3D model;
model = getExtension(obj);
if (model == BND_MODEL)
- set_scene(name,&(this->scene),1.0);
+ set_scene(name, &(this->scene), 1.0);
else if (model == WRL_MODEL)
- set_scene_wrl(name,&(this->scene),1.0);
- else if (model == UNKNOWN_MODEL)
- {
+ set_scene_wrl(name, &(this->scene), 1.0);
+ else if (model == UNKNOWN_MODEL) {
vpERROR_TRACE("Unknown file extension for the 3D model");
}
add_rfstack(IS_BACK);
- add_vwstack ("start","depth", 0.0, 100.0);
- add_vwstack ("start","window", -0.1,0.1,-0.1,0.1);
- add_vwstack ("start","type", PERSPECTIVE);
+ add_vwstack("start", "depth", 0.0, 100.0);
+ add_vwstack("start", "window", -0.1, 0.1, -0.1, 0.1);
+ add_vwstack("start", "type", PERSPECTIVE);
sceneInitialized = true;
displayObject = true;
@@ -607,18 +705,21 @@ vpWireFrameSimulator::initScene(const char* obj)
}
/*!
- Initialize the simulator. It enables to choose the type of scene which will be used to display the object
- at the current position. The object at the desired position is not displayed.
+ Initialize the simulator. It enables to choose the type of scene which will
+ be used to display the object at the current position. The object at the
+ desired position is not displayed.
- Here you can use the scene you want. You have to set the path to a .bnd or a .wrl file which is a 3D model file.
+ Here you can use the scene you want. You have to set the path to a .bnd or a
+ .wrl file which is a 3D model file.
- It is also possible to add a list of vpImageSimulator instances. They will be automatically projected into the image. The position of the four corners have to be given in the object frame.
+ It is also possible to add a list of vpImageSimulator instances. They will
+ be automatically projected into the image. The position of the four corners
+ have to be given in the object frame.
\param obj : Path to the scene file you want to use.
\param imObj : A list of vpImageSimulator instances.
*/
-void
-vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulator> &imObj)
+void vpWireFrameSimulator::initScene(const char *obj, const std::list<vpImageSimulator> &imObj)
{
initScene(obj);
objectImage = imObj;
@@ -629,583 +730,549 @@ vpWireFrameSimulator::initScene(const char* obj, const std::list<vpImageSimulato
Get the internal view ie the view of the camera.
\param I : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &I)
+void vpWireFrameSimulator::getInternalImage(vpImage<vpRGBa> &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)
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_int);
+ v = (double)I.getHeight() / (2 * py_int);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- float o44c[4][4],o44cd[4][4],x,y,z;
+ float o44c[4][4], o44cd[4][4], x, y, z;
Matrix id = IDENTITY_MATRIX;
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- if (displayImageSimulator)
- {
+ vp2jlc_matrix(cMo.inverse(), o44c);
+ vp2jlc_matrix(cdMo.inverse(), o44cd);
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * cMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
x = o44c[2][0] + o44c[3][0];
y = o44c[2][1] + o44c[3][1];
z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
+ add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
if (displayObject)
- display_scene(id,this->scene,I, curColor);
-
+ display_scene(id, this->scene, I, curColor);
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
x = o44cd[2][0] + o44cd[3][0];
y = o44cd[2][1] + o44cd[3][1];
z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
- else display_scene(id,desiredScene,I, desColor);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
+ add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if (displayDesiredObject) {
+ if (desiredObject == D_TOOL)
+ display_scene(o44cd, desiredScene, I, vpColor::red);
+ else
+ display_scene(id, desiredScene, I, desColor);
}
}
-
/*!
- Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
+ Get the external view. It corresponds to the view of the scene from a
+ reference frame you have to set.
\param I : The image where the external view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
+void vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I)
{
bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I,changed);
+ vpHomogeneousMatrix displacement = navigation(I, changed);
- //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- camMf2 = camMf2*displacement;
+ // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
+ // 0*/)
+ if (std::fabs(displacement[2][3]) >
+ std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ camMf2 = camMf2 * displacement;
- f2Mf = camMf2.inverse()*camMf;
+ f2Mf = camMf2.inverse() * camMf;
- camMf = camMf2* displacement * f2Mf;
+ camMf = camMf2 * displacement * f2Mf;
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_ext);
+ v = (double)I.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+ float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
- vp2jlc_matrix(camMf.inverse(),w44cext);
- vp2jlc_matrix(fMc,w44c);
- vp2jlc_matrix(fMo,w44o);
+ vp2jlc_matrix(camMf.inverse(), w44cext);
+ vp2jlc_matrix(fMc, w44c);
+ vp2jlc_matrix(fMo, w44o);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if ((object == CUBE) || (object == SPHERE))
- {
- add_vwstack ("start","type", PERSPECTIVE);
- }
-
- if (displayImageSimulator)
- {
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if ((object == CUBE) || (object == SPHERE)) {
+ add_vwstack("start", "type", PERSPECTIVE);
+ }
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * camMf * fMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
-
+
if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
+ display_scene(w44o, this->scene, I, curColor);
if (displayCamera)
- display_scene(w44c,camera, I, camColor);
+ display_scene(w44c, camera, I, camColor);
- if (displayCameraTrajectory)
- {
+ if (displayCameraTrajectory) {
vpImagePoint iP;
vpImagePoint iP_1;
poseList.push_back(cMo);
fMoList.push_back(fMo);
-
+
int iter = 0;
- if (changed || extCamChanged)
- {
+ if (changed || extCamChanged) {
cameraTrajectory.clear();
std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
- while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end()))
- {
+ while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
cameraTrajectory.push_back(iP);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, iP, camTrajColor);
++iter_poseList;
++iter_fMoList;
iter++;
iP_1 = iP;
}
extCamChanged = false;
- }
- else
- {
+ } else {
iP = projectCameraTrajectory(I, cMo, fMo);
cameraTrajectory.push_back(iP);
- for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
+ for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
vpDisplay::displayPoint(I, *it, camTrajColor);
iter++;
iP_1 = *it;
}
}
- if (poseList.size() > nbrPtLimit)
- {
+ if (poseList.size() > nbrPtLimit) {
poseList.pop_front();
}
- if (fMoList.size() > nbrPtLimit)
- {
+ if (fMoList.size() > nbrPtLimit) {
fMoList.pop_front();
}
- if (cameraTrajectory.size() > nbrPtLimit)
- {
+ if (cameraTrajectory.size() > nbrPtLimit) {
cameraTrajectory.pop_front();
}
}
}
-
/*!
- Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
+ Get an external view. The point of view is set thanks to the pose between
+ the camera camMf and the fixed world frame.
\param I : The image where the external view is displayed.
- \param cam_Mf : The pose between the point of view and the fixed world frame.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+ \param cam_Mf : The pose between the point of view and the fixed world
+ frame.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
+void vpWireFrameSimulator::getExternalImage(vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cam_Mf)
{
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
-
+ float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
+
vpHomogeneousMatrix camMft = rotz * cam_Mf;
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_ext);
+ v = (double)I.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- vp2jlc_matrix(camMft.inverse(),w44cext);
- vp2jlc_matrix(fMo*cMo.inverse(),w44c);
- vp2jlc_matrix(fMo,w44o);
+ vp2jlc_matrix(camMft.inverse(), w44cext);
+ vp2jlc_matrix(fMo * cMo.inverse(), w44c);
+ vp2jlc_matrix(fMo, w44o);
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- if (displayImageSimulator)
- {
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cam_Mf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * cam_Mf * fMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
-
+
if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
+ display_scene(w44o, this->scene, I, curColor);
if (displayCamera)
- display_scene(w44c,camera, I, camColor);
+ display_scene(w44c, camera, I, camColor);
}
-
/*!
Get the internal view ie the view of the camera.
\param I : The image where the internal view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &I)
+void vpWireFrameSimulator::getInternalImage(vpImage<unsigned char> &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)
+ // if(px_int != 1 && py_int != 1)
// we assume px_int and py_int > 0
- if( (std::fabs(px_int-1.) > vpMath::maximum(px_int,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_int-1) > vpMath::maximum(py_int,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_int);
- v = (double)I.getHeight()/(2*py_int);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_int - 1.) > vpMath::maximum(px_int, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_int - 1) > vpMath::maximum(py_int, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_int);
+ v = (double)I.getHeight() / (2 * py_int);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- float o44c[4][4],o44cd[4][4],x,y,z;
+ float o44c[4][4], o44cd[4][4], x, y, z;
Matrix id = IDENTITY_MATRIX;
- vp2jlc_matrix(cMo.inverse(),o44c);
- vp2jlc_matrix(cdMo.inverse(),o44cd);
-
- if (displayImageSimulator)
- {
+ vp2jlc_matrix(cMo.inverse(), o44c);
+ vp2jlc_matrix(cdMo.inverse(), o44cd);
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * camMf * fMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
- add_vwstack ("start","cop", o44c[3][0],o44c[3][1],o44c[3][2]);
+ add_vwstack("start", "cop", o44c[3][0], o44c[3][1], o44c[3][2]);
x = o44c[2][0] + o44c[3][0];
y = o44c[2][1] + o44c[3][1];
z = o44c[2][2] + o44c[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44c[2][0],o44c[2][1],o44c[2][2]);
- add_vwstack ("start","vup", o44c[1][0],o44c[1][1],o44c[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44c[2][0], o44c[2][1], o44c[2][2]);
+ add_vwstack("start", "vup", o44c[1][0], o44c[1][1], o44c[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
if (displayObject)
- display_scene(id,this->scene,I, curColor);
-
+ display_scene(id, this->scene, I, curColor);
- add_vwstack ("start","cop", o44cd[3][0],o44cd[3][1],o44cd[3][2]);
+ add_vwstack("start", "cop", o44cd[3][0], o44cd[3][1], o44cd[3][2]);
x = o44cd[2][0] + o44cd[3][0];
y = o44cd[2][1] + o44cd[3][1];
z = o44cd[2][2] + o44cd[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", o44cd[2][0],o44cd[2][1],o44cd[2][2]);
- add_vwstack ("start","vup", o44cd[1][0],o44cd[1][1],o44cd[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if (displayDesiredObject)
- {
- if (desiredObject == D_TOOL) display_scene(o44cd,desiredScene,I, vpColor::red);
- else display_scene(id,desiredScene,I, desColor);
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", o44cd[2][0], o44cd[2][1], o44cd[2][2]);
+ add_vwstack("start", "vup", o44cd[1][0], o44cd[1][1], o44cd[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if (displayDesiredObject) {
+ if (desiredObject == D_TOOL)
+ display_scene(o44cd, desiredScene, I, vpColor::red);
+ else
+ display_scene(id, desiredScene, I, desColor);
}
}
-
/*!
- Get the external view. It corresponds to the view of the scene from a reference frame you have to set.
+ Get the external view. It corresponds to the view of the scene from a
+ reference frame you have to set.
\param I : The image where the external view is displayed.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
+void vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I)
{
bool changed = false;
- vpHomogeneousMatrix displacement = navigation(I,changed);
+ vpHomogeneousMatrix displacement = navigation(I, changed);
- //if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- if (std::fabs(displacement[2][3]) > std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
- camMf2 = camMf2*displacement;
+ // if (displacement[2][3] != 0 /*|| rotation[0][3] != 0 || rotation[1][3] !=
+ // 0*/)
+ if (std::fabs(displacement[2][3]) >
+ std::numeric_limits<double>::epsilon() /*|| rotation[0][3] != 0 || rotation[1][3] != 0*/)
+ camMf2 = camMf2 * displacement;
- f2Mf = camMf2.inverse()*camMf;
+ f2Mf = camMf2.inverse() * camMf;
- camMf = camMf2* displacement * f2Mf;
+ camMf = camMf2 * displacement * f2Mf;
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_ext);
+ v = (double)I.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+ float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
- vp2jlc_matrix(camMf.inverse(),w44cext);
- vp2jlc_matrix(fMc,w44c);
- vp2jlc_matrix(fMo,w44o);
+ vp2jlc_matrix(camMf.inverse(), w44cext);
+ vp2jlc_matrix(fMc, w44c);
+ vp2jlc_matrix(fMo, w44o);
-
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
- if ((object == CUBE) || (object == SPHERE))
- {
- add_vwstack ("start","type", PERSPECTIVE);
- }
-
- if (displayImageSimulator)
- {
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+ if ((object == CUBE) || (object == SPHERE)) {
+ add_vwstack("start", "type", PERSPECTIVE);
+ }
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*camMf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * camMf * fMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
-
+
if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
+ display_scene(w44o, this->scene, I, curColor);
if (displayCamera)
- display_scene(w44c,camera, I, camColor);
+ display_scene(w44c, camera, I, camColor);
- if (displayCameraTrajectory)
- {
+ if (displayCameraTrajectory) {
vpImagePoint iP;
vpImagePoint iP_1;
poseList.push_back(cMo);
fMoList.push_back(fMo);
-
+
int iter = 0;
- if (changed || extCamChanged)
- {
+ if (changed || extCamChanged) {
cameraTrajectory.clear();
std::list<vpHomogeneousMatrix>::const_iterator iter_poseList = poseList.begin();
std::list<vpHomogeneousMatrix>::const_iterator iter_fMoList = fMoList.begin();
- while ((iter_poseList!=poseList.end()) && (iter_fMoList!=fMoList.end()) )
- {
+ while ((iter_poseList != poseList.end()) && (iter_fMoList != fMoList.end())) {
iP = projectCameraTrajectory(I, *iter_poseList, *iter_fMoList);
cameraTrajectory.push_back(iP);
- //vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor, thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
+ // vpDisplay::displayPoint(I,cameraTrajectory.value(),vpColor::green);
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, iP, camTrajColor);
++iter_poseList;
++iter_fMoList;
iter++;
iP_1 = iP;
}
extCamChanged = false;
- }
- else
- {
- iP = projectCameraTrajectory(I, cMo,fMo);
+ } else {
+ iP = projectCameraTrajectory(I, cMo, fMo);
cameraTrajectory.push_back(iP);
- for(std::list<vpImagePoint>::const_iterator it=cameraTrajectory.begin(); it!=cameraTrajectory.end(); ++it){
- if (camTrajType == CT_LINE){
- if (iter != 0) vpDisplay::displayLine(I,iP_1,*it,camTrajColor, thickness_);
- }
- else if(camTrajType == CT_POINT)
+ for (std::list<vpImagePoint>::const_iterator it = cameraTrajectory.begin(); it != cameraTrajectory.end(); ++it) {
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, *it, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
vpDisplay::displayPoint(I, *it, camTrajColor);
iter++;
iP_1 = *it;
}
}
- if (poseList.size() > nbrPtLimit)
- {
+ if (poseList.size() > nbrPtLimit) {
poseList.pop_front();
}
- if (fMoList.size() > nbrPtLimit)
- {
+ if (fMoList.size() > nbrPtLimit) {
fMoList.pop_front();
}
- if (cameraTrajectory.size() > nbrPtLimit)
- {
+ if (cameraTrajectory.size() > nbrPtLimit) {
cameraTrajectory.pop_front();
}
}
}
-
/*!
- Get an external view. The point of view is set thanks to the pose between the camera camMf and the fixed world frame.
+ Get an external view. The point of view is set thanks to the pose between
+ the camera camMf and the fixed world frame.
\param I : The image where the external view is displayed.
- \param cam_Mf : The pose between the point of view and the fixed world frame.
-
- \warning : The objects are displayed thanks to overlays. The image I is not modified.
+ \param cam_Mf : The pose between the point of view and the fixed world
+ frame.
+
+ \warning : The objects are displayed thanks to overlays. The image I is not
+ modified.
*/
-void
-vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
+void vpWireFrameSimulator::getExternalImage(vpImage<unsigned char> &I, const vpHomogeneousMatrix &cam_Mf)
{
- float w44o[4][4],w44cext[4][4],w44c[4][4],x,y,z;
+ float w44o[4][4], w44cext[4][4], w44c[4][4], x, y, z;
vpHomogeneousMatrix camMft = rotz * cam_Mf;
-
+
double u;
double v;
- //if(px_ext != 1 && py_ext != 1)
+ // if(px_ext != 1 && py_ext != 1)
// we assume px_ext and py_ext > 0
- if( (std::fabs(px_ext-1.) > vpMath::maximum(px_ext,1.)*std::numeric_limits<double>::epsilon())
- && (std::fabs(py_ext-1) > vpMath::maximum(py_ext,1.)*std::numeric_limits<double>::epsilon()))
- {
- u = (double)I.getWidth()/(2*px_ext);
- v = (double)I.getHeight()/(2*py_ext);
- }
- else
- {
- u = (double)I.getWidth()/(vpMath::minimum(I.getWidth(),I.getHeight()));
- v = (double)I.getHeight()/(vpMath::minimum(I.getWidth(),I.getHeight()));
+ if ((std::fabs(px_ext - 1.) > vpMath::maximum(px_ext, 1.) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(py_ext - 1) > vpMath::maximum(py_ext, 1.) * std::numeric_limits<double>::epsilon())) {
+ u = (double)I.getWidth() / (2 * px_ext);
+ v = (double)I.getHeight() / (2 * py_ext);
+ } else {
+ u = (double)I.getWidth() / (vpMath::minimum(I.getWidth(), I.getHeight()));
+ v = (double)I.getHeight() / (vpMath::minimum(I.getWidth(), I.getHeight()));
}
- vp2jlc_matrix(camMft.inverse(),w44cext);
- vp2jlc_matrix(fMo*cMo.inverse(),w44c);
- vp2jlc_matrix(fMo,w44o);
+ vp2jlc_matrix(camMft.inverse(), w44cext);
+ vp2jlc_matrix(fMo * cMo.inverse(), w44c);
+ vp2jlc_matrix(fMo, w44o);
- add_vwstack ("start","cop", w44cext[3][0],w44cext[3][1],w44cext[3][2]);
+ add_vwstack("start", "cop", w44cext[3][0], w44cext[3][1], w44cext[3][2]);
x = w44cext[2][0] + w44cext[3][0];
y = w44cext[2][1] + w44cext[3][1];
z = w44cext[2][2] + w44cext[3][2];
- add_vwstack ("start","vrp", x,y,z);
- add_vwstack ("start","vpn", w44cext[2][0],w44cext[2][1],w44cext[2][2]);
- add_vwstack ("start","vup", w44cext[1][0],w44cext[1][1],w44cext[1][2]);
- add_vwstack ("start","window", -u, u, -v, v);
-
- if (displayImageSimulator)
- {
+ add_vwstack("start", "vrp", x, y, z);
+ add_vwstack("start", "vpn", w44cext[2][0], w44cext[2][1], w44cext[2][2]);
+ add_vwstack("start", "vup", w44cext[1][0], w44cext[1][1], w44cext[1][2]);
+ add_vwstack("start", "window", -u, u, -v, v);
+
+ if (displayImageSimulator) {
I = 255;
- for(std::list<vpImageSimulator>::iterator it=objectImage.begin(); it!=objectImage.end(); ++it){
- vpImageSimulator* imSim = &(*it);
- imSim->setCameraPosition(rotz*cam_Mf*fMo);
- imSim->getImage(I,getInternalCameraParameters(I));
+ for (std::list<vpImageSimulator>::iterator it = objectImage.begin(); it != objectImage.end(); ++it) {
+ vpImageSimulator *imSim = &(*it);
+ imSim->setCameraPosition(rotz * cam_Mf * fMo);
+ imSim->getImage(I, getInternalCameraParameters(I));
}
if (I.display != NULL)
vpDisplay::display(I);
}
-
+
if (displayObject)
- display_scene(w44o,this->scene,I, curColor);
+ display_scene(w44o, this->scene, I, curColor);
if (displayCamera)
- display_scene(w44c,camera, I, camColor);
+ display_scene(w44c, camera, I, camColor);
}
/*!
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
+ Display a trajectory thanks to a list of homogeneous matrices which give the
+ position of the camera relative to the object and the position of the object
+ relative to the world reference frame. The trajectory is projected into the
+ view of an external camera whose position is given in parameter.
- The two lists must have the same size of homogeneous matrices must have the same size.
+ The two lists must have the same size of homogeneous matrices must have the
+ same size.
\param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
+ \param list_cMo : The homogeneous matrices list containing the position of
+ the camera relative to the object. \param list_fMo : The homogeneous
+ matrices list containing the position of the object relative to the world
+ reference frame. \param cMf : A homogeneous matrix which gives the position
+ of the external camera (used to project the trajectory) relative to the
+ world reference frame.
*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
- const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
+void vpWireFrameSimulator::displayTrajectory(const vpImage<unsigned char> &I,
+ const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo,
+ const vpHomogeneousMatrix &cMf)
{
if (list_cMo.size() != list_fMo.size())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
+ throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
vpImagePoint iP;
vpImagePoint iP_1;
@@ -1214,15 +1281,13 @@ vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const
std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
- while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
- {
+ while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, iP, camTrajColor);
++it_cMo;
++it_fMo;
iter++;
@@ -1231,21 +1296,28 @@ vpWireFrameSimulator::displayTrajectory (const vpImage<unsigned char> &I, const
}
/*!
- Display a trajectory thanks to a list of homogeneous matrices which give the position of the camera relative to the object and the position of the object relative to the world reference frame. The trajectory is projected into the view of an external camera whose position is given in parameter.
+ Display a trajectory thanks to a list of homogeneous matrices which give the
+ position of the camera relative to the object and the position of the object
+ relative to the world reference frame. The trajectory is projected into the
+ view of an external camera whose position is given in parameter.
- The two lists must have the same size of homogeneous matrices must have the same size.
+ The two lists must have the same size of homogeneous matrices must have the
+ same size.
\param I : The image where the trajectory is displayed.
- \param list_cMo : The homogeneous matrices list containing the position of the camera relative to the object.
- \param list_fMo : The homogeneous matrices list containing the position of the object relative to the world reference frame.
- \param cMf : A homogeneous matrix which gives the position of the external camera (used to project the trajectory) relative to the world reference frame.
+ \param list_cMo : The homogeneous matrices list containing the position of
+ the camera relative to the object. \param list_fMo : The homogeneous
+ matrices list containing the position of the object relative to the world
+ reference frame. \param cMf : A homogeneous matrix which gives the position
+ of the external camera (used to project the trajectory) relative to the
+ world reference frame.
*/
-void
-vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
- const std::list<vpHomogeneousMatrix> &list_fMo, const vpHomogeneousMatrix &cMf)
+void vpWireFrameSimulator::displayTrajectory(const vpImage<vpRGBa> &I, const std::list<vpHomogeneousMatrix> &list_cMo,
+ const std::list<vpHomogeneousMatrix> &list_fMo,
+ const vpHomogeneousMatrix &cMf)
{
if (list_cMo.size() != list_fMo.size())
- throw(vpException(vpException::dimensionError ,"The two lists must have the same size")) ;
+ throw(vpException(vpException::dimensionError, "The two lists must have the same size"));
vpImagePoint iP;
vpImagePoint iP_1;
@@ -1254,15 +1326,13 @@ vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::li
std::list<vpHomogeneousMatrix>::const_iterator it_cMo = list_cMo.begin();
std::list<vpHomogeneousMatrix>::const_iterator it_fMo = list_fMo.begin();
- while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end()))
- {
+ while ((it_cMo != list_cMo.end()) && (it_fMo != list_fMo.end())) {
iP = projectCameraTrajectory(I, rotz * (*it_cMo), (*it_fMo), rotz * cMf);
- if (camTrajType == CT_LINE)
- {
- if (iter != 0) vpDisplay::displayLine(I,iP_1,iP,camTrajColor,thickness_);
- }
- else if (camTrajType == CT_POINT)
- vpDisplay::displayPoint(I,iP,camTrajColor);
+ if (camTrajType == CT_LINE) {
+ if (iter != 0)
+ vpDisplay::displayLine(I, iP_1, iP, camTrajColor, thickness_);
+ } else if (camTrajType == CT_POINT)
+ vpDisplay::displayPoint(I, iP, camTrajColor);
++it_cMo;
++it_fMo;
iter++;
@@ -1270,186 +1340,182 @@ vpWireFrameSimulator::displayTrajectory (const vpImage<vpRGBa> &I, const std::li
}
}
-
/*!
Enables to change the external camera position.
*/
-vpHomogeneousMatrix
-vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
+vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage<vpRGBa> &I, bool &changed)
{
- double width = vpMath::minimum(I.getWidth(),I.getHeight());
+ double width = vpMath::minimum(I.getWidth(), I.getHeight());
vpImagePoint iP;
vpImagePoint trash;
bool clicked = false;
bool clickedUp = false;
vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
changed = false;
-
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
- if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
+ // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
- if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
+ if (!blocked)
+ clicked = vpDisplay::getClick(I, trash, b, false);
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
- if (b == vpMouseButton::button3) blockedt = true;
+ if (blocked)
+ clickedUp = vpDisplay::getClickUp(I, trash, b, false);
+
+ if (clicked) {
+ if (b == vpMouseButton::button1)
+ blockedr = true;
+ if (b == vpMouseButton::button2)
+ blockedz = true;
+ if (b == vpMouseButton::button3)
+ blockedt = true;
blocked = true;
}
- if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
+ if (clickedUp) {
+ if (b == vpMouseButton::button1) {
+ old_iPr = vpImagePoint(-1, -1);
blockedr = false;
}
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
+ if (b == vpMouseButton::button2) {
+ old_iPz = vpImagePoint(-1, -1);
blockedz = false;
}
- if (b == vpMouseButton::button3)
- {
- old_iPt = vpImagePoint(-1,-1);
+ if (b == vpMouseButton::button3) {
+ old_iPt = vpImagePoint(-1, -1);
blockedt = false;
}
- if (!(blockedr || blockedz || blockedt))
- {
+ if (!(blockedr || blockedz || blockedt)) {
blocked = false;
- while (vpDisplay::getClick(I,trash,b,false)) {};
+ while (vpDisplay::getClick(I, trash, b, false)) {
+ };
}
}
-
- vpDisplay::getPointerPosition(I,iP);
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
+ vpDisplay::getPointerPosition(I, iP);
+
+ if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
double diffi = iP.get_i() - old_iPr.get_i();
double diffj = iP.get_j() - old_iPr.get_j();
- //cout << "delta :" << diffj << endl;;
- double anglei = diffi*360/width;
- double anglej = diffj*360/width;
- mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
+ // cout << "delta :" << diffj << endl;;
+ double anglei = diffi * 360 / width;
+ double anglej = diffj * 360 / width;
+ mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
changed = true;
}
- if (blockedr) old_iPr = iP;
+ if (blockedr)
+ old_iPr = iP;
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
+ if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
changed = true;
}
- if (blockedz) old_iPz = iP;
+ if (blockedz)
+ old_iPz = iP;
- if (old_iPt != vpImagePoint(-1,-1) && blockedt)
- {
+ if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
double diffi = iP.get_i() - old_iPt.get_i();
double diffj = iP.get_j() - old_iPt.get_j();
- mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
+ mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
changed = true;
}
- if (blockedt) old_iPt = iP;
+ if (blockedt)
+ old_iPt = iP;
return mov;
}
-
/*!
Enables to change the external camera position.
*/
-vpHomogeneousMatrix
-vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
+vpHomogeneousMatrix vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
{
- double width = vpMath::minimum(I.getWidth(),I.getHeight());
+ double width = vpMath::minimum(I.getWidth(), I.getHeight());
vpImagePoint iP;
vpImagePoint trash;
bool clicked = false;
bool clickedUp = false;
vpMouseButton::vpMouseButtonType b = vpMouseButton::button1;
- vpHomogeneousMatrix mov(0,0,0,0,0,0);
+ vpHomogeneousMatrix mov(0, 0, 0, 0, 0, 0);
changed = false;
- //if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
-
- if(!blocked)clicked = vpDisplay::getClick(I,trash,b,false);
+ // if(!blocked) vpDisplay::getClickUp(I,trash, b,false);
+
+ if (!blocked)
+ clicked = vpDisplay::getClick(I, trash, b, false);
- if(blocked)clickedUp = vpDisplay::getClickUp(I,trash, b,false);
+ if (blocked)
+ clickedUp = vpDisplay::getClickUp(I, trash, b, false);
- if(clicked)
- {
- if (b == vpMouseButton::button1) blockedr = true;
- if (b == vpMouseButton::button2) blockedz = true;
- if (b == vpMouseButton::button3) blockedt = true;
+ if (clicked) {
+ if (b == vpMouseButton::button1)
+ blockedr = true;
+ if (b == vpMouseButton::button2)
+ blockedz = true;
+ if (b == vpMouseButton::button3)
+ blockedt = true;
blocked = true;
}
- if(clickedUp)
- {
- if (b == vpMouseButton::button1)
- {
- old_iPr = vpImagePoint(-1,-1);
+ if (clickedUp) {
+ if (b == vpMouseButton::button1) {
+ old_iPr = vpImagePoint(-1, -1);
blockedr = false;
}
- if (b == vpMouseButton::button2)
- {
- old_iPz = vpImagePoint(-1,-1);
+ if (b == vpMouseButton::button2) {
+ old_iPz = vpImagePoint(-1, -1);
blockedz = false;
}
- if (b == vpMouseButton::button3)
- {
- old_iPt = vpImagePoint(-1,-1);
+ if (b == vpMouseButton::button3) {
+ old_iPt = vpImagePoint(-1, -1);
blockedt = false;
}
- if (!(blockedr || blockedz || blockedt))
- {
+ if (!(blockedr || blockedz || blockedt)) {
blocked = false;
- while (vpDisplay::getClick(I,trash,b,false)) {};
+ while (vpDisplay::getClick(I, trash, b, false)) {
+ };
}
}
-
- vpDisplay::getPointerPosition(I,iP);
-
- //std::cout << "point : " << iP << std::endl;
- if (old_iPr != vpImagePoint(-1,-1) && blockedr)
- {
+ vpDisplay::getPointerPosition(I, iP);
+
+ // std::cout << "point : " << iP << std::endl;
+
+ if (old_iPr != vpImagePoint(-1, -1) && blockedr) {
double diffi = iP.get_i() - old_iPr.get_i();
double diffj = iP.get_j() - old_iPr.get_j();
- //cout << "delta :" << diffj << endl;;
- double anglei = diffi*360/width;
- double anglej = diffj*360/width;
- mov.buildFrom(0,0,0,vpMath::rad(-anglei),vpMath::rad(anglej),0);
+ // cout << "delta :" << diffj << endl;;
+ double anglei = diffi * 360 / width;
+ double anglej = diffj * 360 / width;
+ mov.buildFrom(0, 0, 0, vpMath::rad(-anglei), vpMath::rad(anglej), 0);
changed = true;
}
- if (blockedr) old_iPr = iP;
+ if (blockedr)
+ old_iPr = iP;
- if (old_iPz != vpImagePoint(-1,-1) && blockedz)
- {
+ if (old_iPz != vpImagePoint(-1, -1) && blockedz) {
double diffi = iP.get_i() - old_iPz.get_i();
- mov.buildFrom(0,0,diffi*0.01,0,0,0);
+ mov.buildFrom(0, 0, diffi * 0.01, 0, 0, 0);
changed = true;
}
- if (blockedz) old_iPz = iP;
+ if (blockedz)
+ old_iPz = iP;
- if (old_iPt != vpImagePoint(-1,-1) && blockedt)
- {
+ if (old_iPt != vpImagePoint(-1, -1) && blockedt) {
double diffi = iP.get_i() - old_iPt.get_i();
double diffj = iP.get_j() - old_iPt.get_j();
- mov.buildFrom(diffj*0.01,diffi*0.01,0,0,0,0);
+ mov.buildFrom(diffj * 0.01, diffi * 0.01, 0, 0, 0, 0);
changed = true;
}
- if (blockedt) old_iPt = iP;
+ if (blockedt)
+ old_iPt = iP;
return mov;
}
@@ -1457,18 +1523,17 @@ vpWireFrameSimulator::navigation(const vpImage<unsigned char> &I, bool &changed)
/*!
Project the center of the internal camera into the external camera view.
*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_)
+vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_)
{
vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ point.setWorldCoordinates(0, 0, 0);
- point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
+ point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
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;
}
@@ -1476,19 +1541,18 @@ vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const v
/*!
Project the center of the internal camera into the external camera view.
*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I,
- const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_)
+vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_)
{
vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ point.setWorldCoordinates(0, 0, 0);
- point.track(rotz*(camMf*fMo_*cMo_.inverse())) ;
+ point.track(rotz * (camMf * fMo_ * cMo_.inverse()));
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;
}
@@ -1496,18 +1560,18 @@ vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I,
/*!
Project the center of the internal camera into the external camera view.
*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
+vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_,
+ const vpHomogeneousMatrix &cMf)
{
vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ point.setWorldCoordinates(0, 0, 0);
- point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
+ point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
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;
}
@@ -1515,19 +1579,19 @@ vpWireFrameSimulator::projectCameraTrajectory (const vpImage<vpRGBa> &I, const v
/*!
Project the center of the internal camera into the external camera view.
*/
-vpImagePoint
-vpWireFrameSimulator::projectCameraTrajectory (const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
- const vpHomogeneousMatrix &fMo_, const vpHomogeneousMatrix &cMf)
+vpImagePoint vpWireFrameSimulator::projectCameraTrajectory(const vpImage<unsigned char> &I,
+ const vpHomogeneousMatrix &cMo_,
+ const vpHomogeneousMatrix &fMo_,
+ const vpHomogeneousMatrix &cMf)
{
vpPoint point;
- point.setWorldCoordinates(0,0,0);
+ point.setWorldCoordinates(0, 0, 0);
- point.track(rotz*(cMf*fMo_*cMo_.inverse())) ;
+ point.track(rotz * (cMf * fMo_ * cMo_.inverse()));
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/testAfma4.cpp b/modules/robot/test/servo-afma4/testAfma4.cpp
index 2e957ed..40d1e50 100644
--- a/modules/robot/test/servo-afma4/testAfma4.cpp
+++ b/modules/robot/test/servo-afma4/testAfma4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,15 +43,13 @@
degrees of freedom).
*/
-
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpAfma4.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/robot/vpAfma4.h>
#include <iostream>
-
int main()
{
try {
@@ -62,10 +61,8 @@ int main()
std::cout << afma4 << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/modules/robot/test/servo-afma4/testRobotAfma4.cpp b/modules/robot/test/servo-afma4/testRobotAfma4.cpp
index f9209e6..4b09346 100644
--- a/modules/robot/test/servo-afma4/testRobotAfma4.cpp
+++ b/modules/robot/test/servo-afma4/testRobotAfma4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,13 +43,12 @@
degrees of freedom).
*/
-#include <visp3/robot/vpRobotAfma4.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobotAfma4.h>
#include <iostream>
-
#ifdef VISP_HAVE_AFMA4
int main()
@@ -60,8 +60,7 @@ int main()
vpRobotAfma4 afma4;
std::cout << afma4 << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -70,7 +69,7 @@ int main()
int main()
{
std::cout << "The real Afma4 robot controller is not available." << std::endl;
- return 0;
+ return 0;
}
#endif
diff --git a/modules/robot/test/servo-afma6/testAfma6.cpp b/modules/robot/test/servo-afma6/testAfma6.cpp
index a7d5160..72e1b1a 100644
--- a/modules/robot/test/servo-afma6/testAfma6.cpp
+++ b/modules/robot/test/servo-afma6/testAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,15 +43,13 @@
degrees of freedom).
*/
-
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpAfma6.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/robot/vpAfma6.h>
#include <iostream>
-
int main()
{
try {
@@ -60,49 +59,40 @@ int main()
vpAfma6 afma6;
vpCameraParameters cam;
- std::cout << "-- Default settings for Afma6 ---"
- << std::endl;
+ std::cout << "-- Default settings for Afma6 ---" << std::endl;
std::cout << afma6 << std::endl;
afma6.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP);
+ std::cout << "-- 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;
- afma6.init( vpAfma6::TOOL_GRIPPER);
+ 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;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-
diff --git a/modules/robot/test/servo-afma6/testRobotAfma6.cpp b/modules/robot/test/servo-afma6/testRobotAfma6.cpp
index e032121..c6b4f3b 100644
--- a/modules/robot/test/servo-afma6/testRobotAfma6.cpp
+++ b/modules/robot/test/servo-afma6/testRobotAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,13 +43,12 @@
degrees of freedom).
*/
-#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpRobotAfma6.h>
#include <iostream>
-
#ifdef VISP_HAVE_AFMA6
int main()
@@ -60,45 +60,37 @@ int main()
vpRobotAfma6 afma6;
vpCameraParameters cam;
- std::cout << "-- Default settings for Afma6 ---"
- << std::endl;
+ std::cout << "-- Default settings for Afma6 ---" << std::endl;
std::cout << afma6 << std::endl;
afma6.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the CCMOP tool without distortion ---"
- << std::endl;
- afma6.init( vpAfma6::TOOL_CCMOP);
+ std::cout << "-- 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;
- afma6.init( vpAfma6::TOOL_GRIPPER);
+ 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;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -107,7 +99,7 @@ int main()
int main()
{
std::cout << "The real Afma6 robot controller is not available." << std::endl;
- return 0;
+ return 0;
}
#endif
diff --git a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp
index a0d74e2..e20c42a 100644
--- a/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp
+++ b/modules/robot/test/servo-afma6/testRobotAfma6Pose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,26 +41,24 @@
Example of robot pose usage.
- Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
- rMc from the robot position.
+ Show how to compute rMo = rMc * cMo with cMo obtained by pose computation
+ and rMc from the robot position.
*/
-
-
+#include <iostream>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotAfma6.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/blob/vpDot.h>
#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpDebug.h>
-#include <iostream>
#if defined(VISP_HAVE_AFMA6) && defined(VISP_HAVE_DC1394)
int main()
@@ -74,43 +73,43 @@ int main()
// Grab an image from the firewire camera
g.acquire(I);
- // Create an image viewer for the image
+// Create an image viewer for the image
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
// Display the image
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
// Define a squared target
// The target is made of 4 planar points (square dim = 0.077m)
double sdim = 0.077; // square width and height
- vpPoint target[4] ;
+ vpPoint target[4];
// Set the point world coordinates (x,y,z) in the object frame
// o ----> x
// |
// |
// \/
// y
- target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
- target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
- target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
- target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
+ target[0].setWorldCoordinates(-sdim / 2., -sdim / 2., 0);
+ target[1].setWorldCoordinates(sdim / 2., -sdim / 2., 0);
+ target[2].setWorldCoordinates(sdim / 2., sdim / 2., 0);
+ target[3].setWorldCoordinates(-sdim / 2., sdim / 2., 0);
// Image processing to extract the 2D coordinates in sub-pixels of the 4
// points from the image acquired by the camera
// Creation of 4 trackers
vpDot dot[4];
vpImagePoint cog;
- for (int i=0; i < 4; i ++) {
+ for (int i = 0; i < 4; i++) {
dot[i].setGraphics(true); // to display the tracking results
std::cout << "Click on dot " << i << std::endl;
- dot[i].initTracking( I );
+ dot[i].initTracking(I);
// The tracker computes the sub-pixels coordinates in the image
// i ----> u
// |
@@ -119,7 +118,7 @@ int main()
// v
std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
// Flush the tracking results in the viewer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Create an intrinsic camera parameters structure
@@ -130,27 +129,26 @@ 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);
- // Using the camera parameters, compute the perspective projection (transform
- // the dot sub-pixel coordinates into coordinates in the camera frame in
- // meter)
- for (int i=0; i < 4; i ++) {
- double x=0, y=0 ; // coordinates of the dots in the camera frame
+ // Using the camera parameters, compute the perspective projection
+ // (transform the dot sub-pixel coordinates into coordinates in the camera
+ // frame in meter)
+ for (int i = 0; i < 4; i++) {
+ double x = 0, y = 0; // coordinates of the dots in the camera frame
// c ----> x
// |
// |
// \/
// y
- //pixel to meter conversion
+ // pixel to meter conversion
cog = dot[i].getCog();
vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- target[i].set_x(x) ;
- target[i].set_y(y) ;
+ target[i].set_x(x);
+ target[i].set_y(y);
}
// From now, in target[i], we have the 3D coordinates of a point in the
@@ -158,26 +156,25 @@ int main()
// compute the pose cMo between the camera and the object.
vpPose pose;
// Add the 4 points to compute the pose
- for (int i=0; i < 4; i ++) {
- pose.addPoint(target[i]) ;
+ for (int i = 0; i < 4; i++) {
+ pose.addPoint(target[i]);
}
// Create an homogeneous matrix for the camera to object transformation
// computed just bellow
vpHomogeneousMatrix cMo;
- vpRotationMatrix R;
- vpRxyzVector r;
- // Compute the pose: initialisation is done by Lagrange method, and the final
- // pose is computed by the more accurate Virtual Visual Servoing method.
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
-
+ vpRotationMatrix R;
+ vpRxyzVector r;
+ // Compute the pose: initialisation is done by Lagrange method, and the
+ // final pose is computed by the more accurate Virtual Visual Servoing
+ // method.
+ pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
std::cout << "Pose cMo: " << std::endl << cMo;
cMo.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
// Get the robot position in the reference frame
vpHomogeneousMatrix rMc;
@@ -185,17 +182,20 @@ int main()
robot.getPosition(vpRobotAfma6::REFERENCE_FRAME, p);
std::cout << "Robot pose in reference frame: " << p << std::endl;
vpTranslationVector t;
- t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
- r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
+ t[0] = p[0];
+ t[1] = p[1];
+ t[2] = p[2];
+ r[0] = p[3];
+ r[1] = p[4];
+ r[2] = p[5];
R.buildFrom(r);
rMc.buildFrom(t, R);
std::cout << "Pose rMc: " << std::endl << rMc;
rMc.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
robot.getPosition(vpRobotAfma6::ARTICULAR_FRAME, p);
std::cout << "Robot pose in articular: " << p << std::endl;
@@ -204,23 +204,20 @@ 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::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
vpHomogeneousMatrix rMo;
rMo = rMc * cMo;
std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
rMo.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -228,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 3642d0d..0336510 100644
--- a/modules/robot/test/servo-viper/testRobotViper650-frames.cpp
+++ b/modules/robot/test/servo-viper/testRobotViper650-frames.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,7 +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);
@@ -55,19 +56,19 @@ bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, do
M1.extract(tu1);
M2.extract(tu2);
- for(unsigned int i=0; i<3; i++) {
- if (std::fabs(t1[i]-t2[i]) > epsilon)
+ for (unsigned int i = 0; i < 3; i++) {
+ if (std::fabs(t1[i] - t2[i]) > epsilon)
return false;
- if (std::fabs(tu1[i]-tu2[i]) > epsilon)
+ if (std::fabs(tu1[i] - tu2[i]) > epsilon)
return false;
}
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) {
+ for (unsigned int i = 0; i < q1.size(); i++) {
+ if (std::fabs(q1[i] - q2[i]) > epsilon) {
return false;
}
}
@@ -111,7 +112,7 @@ int main()
robot.init(vpViper650::TOOL_CUSTOM, eMt);
// Move robot to repos position
- vpColVector repos(6); // q1, q4, q6 = 0
+ vpColVector repos(6); // q1, q4, q6 = 0
repos[1] = vpMath::rad(-90); // q2
repos[2] = vpMath::rad(180); // q3
repos[4] = vpMath::rad(90); // q5
@@ -134,26 +135,28 @@ int main()
std::cout << "fMt:\n" << fMt << std::endl;
std::cout << "eMc:\n" << cMe.inverse() << std::endl;
- //********* Check if retrieved eMt transformation is the one that was set during init
+ //********* Check if retrieved eMt transformation is the one that was set
+ // during init
if (1) {
vpHomogeneousMatrix eMt_ = fMe.inverse() * fMt;
std::cout << "eMt_:\n" << eMt_ << std::endl;
// Compare pose
std::cout << "Compare pose eMt and eMt_:" << std::endl;
- if (! pose_equal(eMt, eMt_, 1e-4)) {
+ if (!pose_equal(eMt, eMt_, 1e-4)) {
std::cout << " Error: Pose eMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
- //********* Check if retrieved eMc transformation is the one that was set
+ //********* Check if retrieved eMc transformation is the one that was
+ // set
std::cout << "eMc:\n" << cMe.inverse() << std::endl;
// Compare pose
std::cout << "Compare pose eMt and eMc:" << std::endl;
- if (! pose_equal(eMt, cMe.inverse(), 1e-4)) {
+ if (!pose_equal(eMt, cMe.inverse(), 1e-4)) {
std::cout << " Error: Pose eMc differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -168,15 +171,15 @@ int main()
// Compute homogeneous transformation
vpTranslationVector f_t_t;
vpRxyzVector f_rxyz_t;
- for (unsigned int i=0; i<3; i++) {
- f_t_t[i] = f_pose_t[i];
- f_rxyz_t[i] = f_pose_t[i+3];
+ for (unsigned int i = 0; i < 3; i++) {
+ f_t_t[i] = f_pose_t[i];
+ f_rxyz_t[i] = f_pose_t[i + 3];
}
vpHomogeneousMatrix fMt_(f_t_t, vpRotationMatrix(f_rxyz_t));
std::cout << "fMt_ (from ref frame):\n" << fMt_ << std::endl;
std::cout << "Compare pose fMt and fMt_:" << std::endl;
- if (! pose_equal(fMt, fMt_, 1e-4)) {
+ if (!pose_equal(fMt, fMt_, 1e-4)) {
std::cout << " Error: Pose fMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -198,7 +201,7 @@ int main()
std::cout << "Reach joint position q2: " << q2.t() << std::endl;
std::cout << "Compare joint position q and q2:" << std::endl;
- if (! joint_equal(q, q2, 1e-4)) {
+ if (!joint_equal(q, q2, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -211,9 +214,9 @@ int main()
vpColVector f_pose_t(6);
vpTranslationVector f_t_t = fMt.getTranslationVector();
vpRxyzVector f_rxyz_t(fMt.getRotationMatrix());
- for (unsigned int i=0; i<3; i++) {
- f_pose_t[i] = f_t_t[i];
- f_pose_t[i+3] = f_rxyz_t[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ f_pose_t[i] = f_t_t[i];
+ f_pose_t[i + 3] = f_rxyz_t[i];
}
std::cout << "Move robot in reference frame (the robot should not move)" << std::endl;
@@ -222,7 +225,7 @@ int main()
robot.getPosition(vpRobot::ARTICULAR_FRAME, q3);
std::cout << "Reach joint position q3: " << q3.t() << std::endl;
std::cout << "Compare joint position q and q3:" << std::endl;
- if (! joint_equal(q, q3, 1e-4)) {
+ if (!joint_equal(q, q3, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -234,11 +237,11 @@ int main()
if (1) {
// from the current position move the tool frame
vpHomogeneousMatrix tMt;
- //tMt[0][3] = 0.05; // along x_t
+ // tMt[0][3] = 0.05; // along x_t
tMt[1][3] = 0.05; // along y_t
// tMt[2][3] = 0.05; // along z_t
- vpHomogeneousMatrix fMt_ = fMt*tMt; // New position to reach
+ vpHomogeneousMatrix fMt_ = fMt * tMt; // New position to reach
robot.getInverseKinematics(fMt_, q);
std::cout << "fMt_:\n" << fMt_ << std::endl;
@@ -253,7 +256,7 @@ int main()
std::cout << "fpt_:\n" << vpHomogeneousMatrix(fpt_) << std::endl;
std::cout << "Compare pose fMt_ and fpt_:" << std::endl;
- if (! pose_equal(fMt_, vpHomogeneousMatrix(fpt_), 1e-4)) {
+ if (!pose_equal(fMt_, vpHomogeneousMatrix(fpt_), 1e-4)) {
std::cout << " Error: Pose fMt_ differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -262,24 +265,25 @@ int main()
}
//********* Velocity control in tool frame along z
- if (1){
+ if (1) {
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6);
v_t = 0;
- //v_t[2] = 0.01; // translation velocity along z_t
+ // v_t[2] = 0.01; // translation velocity along z_t
v_t[5] = vpMath::rad(5); // rotation velocity along z_t
std::cout << "Move robot in camera velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
- //std::cout << "send vel: " << v_t() << std::endl;
+ while (vpTime::measureTimeMs() - t_init < 6000) {
+ // std::cout << "send vel: " << v_t() << std::endl;
robot.setVelocity(vpRobotViper650::CAMERA_FRAME, v_t);
}
}
//********* Velocity control in tool frame along z using joint velocity
- if (1){
- // We need to stop the robot before changing velocity control from joint to cartesian
+ if (1) {
+ // We need to stop the robot before changing velocity control from joint
+ // to cartesian
robot.setRobotState(vpRobot::STATE_STOP);
vpVelocityTwistMatrix tVe(eMt.inverse());
vpMatrix eJe;
@@ -287,23 +291,22 @@ int main()
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6), q_dot;
v_t = 0;
- //v_t[2] = -0.01; // translation velocity along z_t
+ // v_t[2] = -0.01; // translation velocity along z_t
v_t[5] = vpMath::rad(-5); // rotation velocity along z_t
std::cout << "Move robot in joint velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
+ while (vpTime::measureTimeMs() - t_init < 6000) {
robot.get_eJe(eJe);
vpMatrix tJt = tVe * eJe;
q_dot = tJt.pseudoInverse() * v_t;
- //std::cout << "send vel: " << q_dot.t() << std::endl;
+ // std::cout << "send vel: " << q_dot.t() << std::endl;
robot.setVelocity(vpRobotViper650::ARTICULAR_FRAME, q_dot);
}
}
-
//********* Velocity control in tool frame along x
- if (1){
+ if (1) {
robot.setRobotState(vpRobot::STATE_STOP);
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6);
@@ -312,15 +315,16 @@ int main()
std::cout << "Move robot in camera velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 3000) {
- //std::cout << "send vel: " << v_t() << std::endl;
+ while (vpTime::measureTimeMs() - t_init < 3000) {
+ // std::cout << "send vel: " << v_t() << std::endl;
robot.setVelocity(vpRobotViper650::CAMERA_FRAME, v_t);
}
}
//********* Velocity control in tool frame along x using joint velocity
- if (1){
- // We need to stop the robot before changing velocity control from joint to cartesian
+ if (1) {
+ // We need to stop the robot before changing velocity control from joint
+ // to cartesian
robot.setRobotState(vpRobot::STATE_STOP);
vpVelocityTwistMatrix tVe(eMt.inverse());
vpMatrix eJe;
@@ -332,11 +336,11 @@ int main()
std::cout << "Move robot in joint velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 3000) {
+ while (vpTime::measureTimeMs() - t_init < 3000) {
robot.get_eJe(eJe);
vpMatrix tJt = tVe * eJe;
q_dot = tJt.pseudoInverse() * v_t;
- //std::cout << "send vel: " << q_dot.t() << std::endl;
+ // std::cout << "send vel: " << q_dot.t() << std::endl;
robot.setVelocity(vpRobotViper650::ARTICULAR_FRAME, q_dot);
}
}
@@ -350,11 +354,11 @@ int main()
robot.get_fMc(q, fMt);
vpHomogeneousMatrix tMt; // initialized to identity
- //tMt[0][3] = -0.05; // along x_t
+ // tMt[0][3] = -0.05; // along x_t
tMt[1][3] = -0.05; // along y_t
// tMt[2][3] = -0.05; // along z_t
- robot.getInverseKinematics(fMt*tMt, q);
+ robot.getInverseKinematics(fMt * tMt, q);
std::cout << "Move robot in joint position" << std::endl;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
@@ -362,8 +366,7 @@ int main()
}
std::cout << "The end" << std::endl;
std::cout << "Test succeed" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Test failed with exception: " << e.getMessage() << std::endl;
}
}
diff --git a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp
index 1ce2b3c..b1d61cf 100644
--- a/modules/robot/test/servo-viper/testRobotViper850-frames.cpp
+++ b/modules/robot/test/servo-viper/testRobotViper850-frames.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,7 +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);
@@ -55,19 +56,19 @@ bool pose_equal(const vpHomogeneousMatrix &M1, const vpHomogeneousMatrix &M2, do
M1.extract(tu1);
M2.extract(tu2);
- for(unsigned int i=0; i<3; i++) {
- if (std::fabs(t1[i]-t2[i]) > epsilon)
+ for (unsigned int i = 0; i < 3; i++) {
+ if (std::fabs(t1[i] - t2[i]) > epsilon)
return false;
- if (std::fabs(tu1[i]-tu2[i]) > epsilon)
+ if (std::fabs(tu1[i] - tu2[i]) > epsilon)
return false;
}
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) {
+ for (unsigned int i = 0; i < q1.size(); i++) {
+ if (std::fabs(q1[i] - q2[i]) > epsilon) {
return false;
}
}
@@ -111,7 +112,7 @@ int main()
robot.init(vpViper850::TOOL_CUSTOM, eMt);
// Move robot to repos position
- vpColVector repos(6); // q1, q4, q6 = 0
+ vpColVector repos(6); // q1, q4, q6 = 0
repos[1] = vpMath::rad(-90); // q2
repos[2] = vpMath::rad(180); // q3
repos[4] = vpMath::rad(90); // q5
@@ -134,26 +135,28 @@ int main()
std::cout << "fMt:\n" << fMt << std::endl;
std::cout << "eMc:\n" << cMe.inverse() << std::endl;
- //********* Check if retrieved eMt transformation is the one that was set during init
+ //********* Check if retrieved eMt transformation is the one that was set
+ // during init
if (1) {
vpHomogeneousMatrix eMt_ = fMe.inverse() * fMt;
std::cout << "eMt_:\n" << eMt_ << std::endl;
// Compare pose
std::cout << "Compare pose eMt and eMt_:" << std::endl;
- if (! pose_equal(eMt, eMt_, 1e-4)) {
+ if (!pose_equal(eMt, eMt_, 1e-4)) {
std::cout << " Error: Pose eMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
}
std::cout << " They are the same, we can continue" << std::endl;
- //********* Check if retrieved eMc transformation is the one that was set
+ //********* Check if retrieved eMc transformation is the one that was
+ // set
std::cout << "eMc:\n" << cMe.inverse() << std::endl;
// Compare pose
std::cout << "Compare pose eMt and eMc:" << std::endl;
- if (! pose_equal(eMt, cMe.inverse(), 1e-4)) {
+ if (!pose_equal(eMt, cMe.inverse(), 1e-4)) {
std::cout << " Error: Pose eMc differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -168,15 +171,15 @@ int main()
// Compute homogeneous transformation
vpTranslationVector f_t_t;
vpRxyzVector f_rxyz_t;
- for (unsigned int i=0; i<3; i++) {
- f_t_t[i] = f_pose_t[i];
- f_rxyz_t[i] = f_pose_t[i+3];
+ for (unsigned int i = 0; i < 3; i++) {
+ f_t_t[i] = f_pose_t[i];
+ f_rxyz_t[i] = f_pose_t[i + 3];
}
vpHomogeneousMatrix fMt_(f_t_t, vpRotationMatrix(f_rxyz_t));
std::cout << "fMt_ (from ref frame):\n" << fMt_ << std::endl;
std::cout << "Compare pose fMt and fMt_:" << std::endl;
- if (! pose_equal(fMt, fMt_, 1e-4)) {
+ if (!pose_equal(fMt, fMt_, 1e-4)) {
std::cout << " Error: Pose fMt differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -198,7 +201,7 @@ int main()
std::cout << "Reach joint position q2: " << q2.t() << std::endl;
std::cout << "Compare joint position q and q2:" << std::endl;
- if (! joint_equal(q, q2, 1e-4)) {
+ if (!joint_equal(q, q2, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -211,9 +214,9 @@ int main()
vpColVector f_pose_t(6);
vpTranslationVector f_t_t = fMt.getTranslationVector();
vpRxyzVector f_rxyz_t(fMt.getRotationMatrix());
- for (unsigned int i=0; i<3; i++) {
- f_pose_t[i] = f_t_t[i];
- f_pose_t[i+3] = f_rxyz_t[i];
+ for (unsigned int i = 0; i < 3; i++) {
+ f_pose_t[i] = f_t_t[i];
+ f_pose_t[i + 3] = f_rxyz_t[i];
}
std::cout << "Move robot in reference frame (the robot should not move)" << std::endl;
@@ -222,7 +225,7 @@ int main()
robot.getPosition(vpRobot::ARTICULAR_FRAME, q3);
std::cout << "Reach joint position q3: " << q3.t() << std::endl;
std::cout << "Compare joint position q and q3:" << std::endl;
- if (! joint_equal(q, q3, 1e-4)) {
+ if (!joint_equal(q, q3, 1e-4)) {
std::cout << " Error: Joint position differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -234,11 +237,11 @@ int main()
if (1) {
// from the current position move the tool frame
vpHomogeneousMatrix tMt;
- //tMt[0][3] = 0.05; // along x_t
+ // tMt[0][3] = 0.05; // along x_t
tMt[1][3] = 0.05; // along y_t
// tMt[2][3] = 0.05; // along z_t
- vpHomogeneousMatrix fMt_ = fMt*tMt; // New position to reach
+ vpHomogeneousMatrix fMt_ = fMt * tMt; // New position to reach
robot.getInverseKinematics(fMt_, q);
std::cout << "fMt_:\n" << fMt_ << std::endl;
@@ -253,7 +256,7 @@ int main()
std::cout << "fpt_:\n" << vpHomogeneousMatrix(fpt_) << std::endl;
std::cout << "Compare pose fMt_ and fpt_:" << std::endl;
- if (! pose_equal(fMt_, vpHomogeneousMatrix(fpt_), 1e-4)) {
+ if (!pose_equal(fMt_, vpHomogeneousMatrix(fpt_), 1e-4)) {
std::cout << " Error: Pose fMt_ differ" << std::endl;
std::cout << "\nTest failed" << std::endl;
return -1;
@@ -262,24 +265,25 @@ int main()
}
//********* Velocity control in tool frame along z
- if (0){
+ if (0) {
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6);
v_t = 0;
- //v_t[2] = 0.01; // translation velocity along z_t
+ // v_t[2] = 0.01; // translation velocity along z_t
v_t[5] = vpMath::rad(5); // rotation velocity along z_t
std::cout << "Move robot in camera velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
- //std::cout << "send vel: " << v_t() << std::endl;
+ while (vpTime::measureTimeMs() - t_init < 6000) {
+ // std::cout << "send vel: " << v_t() << std::endl;
robot.setVelocity(vpRobotViper850::CAMERA_FRAME, v_t);
}
}
//********* Velocity control in tool frame along z using joint velocity
- if (0){
- // We need to stop the robot before changing velocity control from joint to cartesian
+ if (0) {
+ // We need to stop the robot before changing velocity control from joint
+ // to cartesian
robot.setRobotState(vpRobot::STATE_STOP);
vpVelocityTwistMatrix tVe(eMt.inverse());
vpMatrix eJe;
@@ -287,23 +291,22 @@ int main()
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6), q_dot;
v_t = 0;
- //v_t[2] = -0.01; // translation velocity along z_t
+ // v_t[2] = -0.01; // translation velocity along z_t
v_t[5] = vpMath::rad(-5); // rotation velocity along z_t
std::cout << "Move robot in joint velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
+ while (vpTime::measureTimeMs() - t_init < 6000) {
robot.get_eJe(eJe);
vpMatrix tJt = tVe * eJe;
q_dot = tJt.pseudoInverse() * v_t;
- //std::cout << "send vel: " << q_dot.t() << std::endl;
+ // std::cout << "send vel: " << q_dot.t() << std::endl;
robot.setVelocity(vpRobotViper850::ARTICULAR_FRAME, q_dot);
}
}
-
//********* Velocity control in tool frame along x
- if (1){
+ if (1) {
robot.setRobotState(vpRobot::STATE_STOP);
double t_init = vpTime::measureTimeMs();
vpColVector v_t(6);
@@ -312,15 +315,16 @@ int main()
std::cout << "Move robot in camera velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
- //std::cout << "send vel: " << v_t() << std::endl;
+ while (vpTime::measureTimeMs() - t_init < 6000) {
+ // std::cout << "send vel: " << v_t() << std::endl;
robot.setVelocity(vpRobotViper850::CAMERA_FRAME, v_t);
}
}
//********* Velocity control in tool frame along x using joint velocity
- if (1){
- // We need to stop the robot before changing velocity control from joint to cartesian
+ if (1) {
+ // We need to stop the robot before changing velocity control from joint
+ // to cartesian
robot.setRobotState(vpRobot::STATE_STOP);
vpVelocityTwistMatrix tVe(eMt.inverse());
vpMatrix eJe;
@@ -332,11 +336,11 @@ int main()
std::cout << "Move robot in joint velocity" << std::endl;
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- while(vpTime::measureTimeMs() - t_init < 6000) {
+ while (vpTime::measureTimeMs() - t_init < 6000) {
robot.get_eJe(eJe);
vpMatrix tJt = tVe * eJe;
q_dot = tJt.pseudoInverse() * v_t;
- //std::cout << "send vel: " << q_dot.t() << std::endl;
+ // std::cout << "send vel: " << q_dot.t() << std::endl;
robot.setVelocity(vpRobotViper850::ARTICULAR_FRAME, q_dot);
}
}
@@ -350,11 +354,11 @@ int main()
robot.get_fMc(q, fMt);
vpHomogeneousMatrix tMt; // initialized to identity
- //tMt[0][3] = -0.05; // along x_t
+ // tMt[0][3] = -0.05; // along x_t
tMt[1][3] = -0.05; // along y_t
// tMt[2][3] = -0.05; // along z_t
- robot.getInverseKinematics(fMt*tMt, q);
+ robot.getInverseKinematics(fMt * tMt, q);
std::cout << "Move robot in joint position" << std::endl;
robot.setRobotState(vpRobot::STATE_POSITION_CONTROL);
@@ -362,8 +366,7 @@ int main()
}
std::cout << "The end" << std::endl;
std::cout << "Test succeed" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Test failed with exception: " << e.getMessage() << std::endl;
}
}
diff --git a/modules/robot/test/servo-viper/testRobotViper850.cpp b/modules/robot/test/servo-viper/testRobotViper850.cpp
index 909bd4f..dccd30e 100644
--- a/modules/robot/test/servo-viper/testRobotViper850.cpp
+++ b/modules/robot/test/servo-viper/testRobotViper850.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,14 +43,11 @@
degrees of freedom).
*/
-
-
-
+#include <iostream>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpRobotViper850.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <iostream>
+#include <visp3/robot/vpRobotViper850.h>
#ifdef VISP_HAVE_VIPER850
int main()
@@ -61,29 +59,28 @@ int main()
vpRobotViper850 viper850;
vpCameraParameters cam;
- std::cout << "-- Default settings for Viper850 ---"
- << std::endl;
+ std::cout << "-- Default settings for Viper850 ---" << std::endl;
std::cout << viper850 << std::endl;
viper850.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
+ std::cout << "-- Settings associated to the Marlin F033C camera without "
+ "distortion ---"
<< std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
+ viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA);
std::cout << viper850 << std::endl;
viper850.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
+ std::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;
-
std::cout << "-- Current joint position:" << std::endl;
vpColVector q;
viper850.getPosition(vpRobot::ARTICULAR_FRAME, q);
@@ -102,12 +99,10 @@ 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]) << std::endl;
+ << "\trzyz (deg): " << vpMath::deg(rzyz[0]) << " " << vpMath::deg(rzyz[1]) << " " << vpMath::deg(rzyz[2])
+ << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -116,7 +111,7 @@ int main()
int main()
{
std::cout << "The real Viper850 robot controller is not available." << std::endl;
- return 0;
+ return 0;
}
#endif
diff --git a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp
index 173267e..b12e3c7 100644
--- a/modules/robot/test/servo-viper/testRobotViper850Pose.cpp
+++ b/modules/robot/test/servo-viper/testRobotViper850Pose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,28 +41,25 @@
Example of robot pose usage.
- Show how to compute rMo = rMc * cMo with cMo obtained by pose computation and
- rMc from the robot position.
+ Show how to compute rMo = rMc * cMo with cMo obtained by pose computation
+ and rMc from the robot position.
*/
-
-
-
+#include <iostream>
+#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImage.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/robot/vpRobotViper850.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/blob/vpDot.h>
#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpDebug.h>
-#include <iostream>
#if defined(VISP_HAVE_VIPER850) && defined(VISP_HAVE_DC1394)
int main()
@@ -77,43 +75,43 @@ int main()
// Grab an image from the firewire camera
g.acquire(I);
- // Create an image viewer for the image
+// Create an image viewer for the image
#ifdef VISP_HAVE_X11
- vpDisplayX display(I,100,100,"Current image") ;
+ vpDisplayX display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Current image") ;
+ vpDisplayOpenCV display(I, 100, 100, "Current image");
#elif defined(VISP_HAVE_GTK)
- vpDisplayGTK display(I,100,100,"Current image") ;
+ vpDisplayGTK display(I, 100, 100, "Current image");
#endif
// Display the image
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
// Define a squared target
// The target is made of 4 planar points (square dim = 0.077m)
double sdim = 0.077; // square width and height
- vpPoint target[4] ;
+ vpPoint target[4];
// Set the point world coordinates (x,y,z) in the object frame
// o ----> x
// |
// |
// \/
// y
- target[0].setWorldCoordinates(-sdim/2., -sdim/2., 0) ;
- target[1].setWorldCoordinates( sdim/2., -sdim/2., 0) ;
- target[2].setWorldCoordinates( sdim/2., sdim/2., 0) ;
- target[3].setWorldCoordinates(-sdim/2., sdim/2., 0) ;
+ target[0].setWorldCoordinates(-sdim / 2., -sdim / 2., 0);
+ target[1].setWorldCoordinates(sdim / 2., -sdim / 2., 0);
+ target[2].setWorldCoordinates(sdim / 2., sdim / 2., 0);
+ target[3].setWorldCoordinates(-sdim / 2., sdim / 2., 0);
// Image processing to extract the 2D coordinates in sub-pixels of the 4
// points from the image acquired by the camera
// Creation of 4 trackers
vpDot dot[4];
vpImagePoint cog;
- for (int i=0; i < 4; i ++) {
+ for (int i = 0; i < 4; i++) {
dot[i].setGraphics(true); // to display the tracking results
std::cout << "Click on dot " << i << std::endl;
- dot[i].initTracking( I );
+ dot[i].initTracking(I);
// The tracker computes the sub-pixels coordinates in the image
// i ----> u
// |
@@ -122,7 +120,7 @@ int main()
// v
std::cout << " Coordinates: " << dot[i].getCog() << std::endl;
// Flush the tracking results in the viewer
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
}
// Create an intrinsic camera parameters structure
@@ -133,27 +131,26 @@ 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);
- // Using the camera parameters, compute the perspective projection (transform
- // the dot sub-pixel coordinates into coordinates in the camera frame in
- // meter)
- for (int i=0; i < 4; i ++) {
- double x=0, y=0 ; // coordinates of the dots in the camera frame
+ // Using the camera parameters, compute the perspective projection
+ // (transform the dot sub-pixel coordinates into coordinates in the camera
+ // frame in meter)
+ for (int i = 0; i < 4; i++) {
+ double x = 0, y = 0; // coordinates of the dots in the camera frame
// c ----> x
// |
// |
// \/
// y
- //pixel to meter conversion
+ // pixel to meter conversion
cog = dot[i].getCog();
vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- target[i].set_x(x) ;
- target[i].set_y(y) ;
+ target[i].set_x(x);
+ target[i].set_y(y);
}
// From now, in target[i], we have the 3D coordinates of a point in the
@@ -161,26 +158,25 @@ int main()
// compute the pose cMo between the camera and the object.
vpPose pose;
// Add the 4 points to compute the pose
- for (int i=0; i < 4; i ++) {
- pose.addPoint(target[i]) ;
+ for (int i = 0; i < 4; i++) {
+ pose.addPoint(target[i]);
}
// Create an homogeneous matrix for the camera to object transformation
// computed just bellow
vpHomogeneousMatrix cMo;
- vpRotationMatrix R;
- vpRxyzVector r;
- // Compute the pose: initialisation is done by Lagrange method, and the final
- // pose is computed by the more accurate Virtual Visual Servoing method.
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
-
+ vpRotationMatrix R;
+ vpRxyzVector r;
+ // Compute the pose: initialisation is done by Lagrange method, and the
+ // final pose is computed by the more accurate Virtual Visual Servoing
+ // method.
+ pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo);
std::cout << "Pose cMo: " << std::endl << cMo;
cMo.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
// Get the robot position in the reference frame
vpHomogeneousMatrix rMc;
@@ -188,17 +184,20 @@ int main()
robot.getPosition(vpRobotViper850::REFERENCE_FRAME, p);
std::cout << "Robot pose in reference frame: " << p << std::endl;
vpTranslationVector t;
- t[0] = p[0]; t[1] = p[1]; t[2] = p[2];
- r[0] = p[3]; r[1] = p[4]; r[2] = p[5];
+ t[0] = p[0];
+ t[1] = p[1];
+ t[2] = p[2];
+ r[0] = p[3];
+ r[1] = p[4];
+ r[2] = p[5];
R.buildFrom(r);
rMc.buildFrom(t, R);
std::cout << "Pose rMc: " << std::endl << rMc;
rMc.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
robot.getPosition(vpRobotViper850::ARTICULAR_FRAME, p);
std::cout << "Robot pose in articular: " << p << std::endl;
@@ -207,23 +206,20 @@ 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::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
vpHomogeneousMatrix rMo;
rMo = rMc * cMo;
std::cout << "Pose rMo = rMc * cMo: " << std::endl << rMo;
rMo.extract(R);
r.buildFrom(R);
- std::cout << " rotation: "
- << vpMath::deg(r[0]) << " "
- << vpMath::deg(r[1]) << " "
- << vpMath::deg(r[2]) << " deg" << std::endl << std::endl;
+ std::cout << " rotation: " << vpMath::deg(r[0]) << " " << vpMath::deg(r[1]) << " " << vpMath::deg(r[2]) << " deg"
+ << std::endl
+ << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
return 0;
@@ -231,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 8d71704..3ed7428 100644
--- a/modules/robot/test/servo-viper/testViper650.cpp
+++ b/modules/robot/test/servo-viper/testViper650.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,11 +43,9 @@
degrees of freedom).
*/
-
-
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpViper650.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper650.h>
#include <iostream>
@@ -59,24 +58,24 @@ int main()
vpViper650 viper650;
vpCameraParameters cam;
- std::cout << "-- Default settings for Viper 650 ---"
- << std::endl;
+ std::cout << "-- Default settings for Viper 650 ---" << std::endl;
std::cout << viper650 << std::endl;
viper650.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
+
+ std::cout << "-- Settings associated to the Marlin F033C camera without "
+ "distortion ---"
<< std::endl;
- viper650.init( vpViper650::TOOL_MARLIN_F033C_CAMERA);
+ viper650.init(vpViper650::TOOL_MARLIN_F033C_CAMERA);
std::cout << viper650 << std::endl;
viper650.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
+ 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;
@@ -91,7 +90,7 @@ int main()
qmotor[5] = vpMath::rad(13.37);
#else
qmotor[0] = vpMath::rad(0);
- qmotor[1] = vpMath::rad(0-90);
+ qmotor[1] = vpMath::rad(0 - 90);
qmotor[2] = vpMath::rad(0);
qmotor[3] = vpMath::rad(0);
qmotor[4] = vpMath::rad(0);
@@ -110,15 +109,12 @@ 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) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return 1;
}
}
-
diff --git a/modules/robot/test/servo-viper/testViper850.cpp b/modules/robot/test/servo-viper/testViper850.cpp
index bebeed4..9602ef7 100644
--- a/modules/robot/test/servo-viper/testViper850.cpp
+++ b/modules/robot/test/servo-viper/testViper850.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,11 +43,9 @@
degrees of freedom).
*/
-
-
#include <visp3/core/vpConfig.h>
-#include <visp3/robot/vpViper850.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/robot/vpViper850.h>
#include <iostream>
@@ -59,24 +58,24 @@ int main()
vpViper850 viper850;
vpCameraParameters cam;
- std::cout << "-- Default settings for Viper 850 ---"
- << std::endl;
+ std::cout << "-- Default settings for Viper 850 ---" << std::endl;
std::cout << viper850 << std::endl;
viper850.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
-
- std::cout << "-- Settings associated to the Marlin F033C camera without distortion ---"
+
+ std::cout << "-- Settings associated to the Marlin F033C camera without "
+ "distortion ---"
<< std::endl;
- viper850.init( vpViper850::TOOL_MARLIN_F033C_CAMERA);
+ viper850.init(vpViper850::TOOL_MARLIN_F033C_CAMERA);
std::cout << viper850 << std::endl;
viper850.getCameraParameters(cam, 640, 480);
std::cout << cam << std::endl;
- std::cout << "-- Settings associated to the Marlin F033C camera with distortion ------"
+ std::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;
@@ -91,7 +90,7 @@ int main()
qmotor[5] = vpMath::rad(13.37);
#else
qmotor[0] = vpMath::rad(0);
- qmotor[1] = vpMath::rad(0-90);
+ qmotor[1] = vpMath::rad(0 - 90);
qmotor[2] = vpMath::rad(0);
qmotor[3] = vpMath::rad(0);
qmotor[4] = vpMath::rad(0);
@@ -110,15 +109,12 @@ 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) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return 1;
}
}
-
diff --git a/modules/robot/test/virtuose/testVirtuose.cpp b/modules/robot/test/virtuose/testVirtuose.cpp
index ae5b6d5..05feb2c 100644
--- a/modules/robot/test/virtuose/testVirtuose.cpp
+++ b/modules/robot/test/virtuose/testVirtuose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,8 @@
/*!
\example testVirtuose.cpp
- Test for reading the Virtuose's joint values inside the main function after checking the emergency button.
+ Test for reading the Virtuose's joint values inside the main function
+ after checking the emergency button.
*/
#include <visp3/robot/vpVirtuose.h>
@@ -50,20 +52,18 @@ int main()
virtuose.init();
bool emergStop = virtuose.getEmergencyStop();
- if (emergStop)
- {
+ if (emergStop) {
std::cout << "The system is operational." << std::endl;
vpColVector q = virtuose.getArticularPosition();
std::cout << "The current joint values are : " << q.t() << std::endl;
- }
- else
- std::cout << "The system is not operational. \nPlease plug the emergency stop to the system (or untrigger it)." << std::endl;
- }
- catch(vpException &e) {
+ } else
+ std::cout << "The system is not operational. \nPlease plug the "
+ "emergency stop to the system (or untrigger it)."
+ << std::endl;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
#else
std::cout << "You should install Virtuose SDK to use this binary..." << std::endl;
#endif
}
-
diff --git a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp
index 0d7ab07..81a685b 100644
--- a/modules/robot/test/virtuose/testVirtuoseAfma6.cpp
+++ b/modules/robot/test/virtuose/testVirtuoseAfma6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,13 +40,14 @@
/*!
\example testVirtuoseAfma6.cpp
- Test Haption Virtuose SDK wrapper to control Afma6 robot velocity from haptic device velocity.
- Movements are allowed inside a cube of pre-determined side.
+ Test Haption Virtuose SDK wrapper to control Afma6 robot velocity from
+ haptic device velocity. Movements are allowed inside a cube of
+ pre-determined side.
*/
#include <visp3/core/vpTime.h>
-#include <visp3/robot/vpVirtuose.h>
#include <visp3/robot/vpRobotAfma6.h>
+#include <visp3/robot/vpVirtuose.h>
int main()
{
@@ -56,7 +58,7 @@ int main()
virtuose.setVerbose(true);
virtuose.setCommandType(COMMAND_TYPE_IMPEDANCE);
virtuose.setPowerOn();
-// virtuose.setSaturation(1.0f,0.0f);
+ // virtuose.setSaturation(1.0f,0.0f);
vpColVector virt_velocity;
vpColVector robot_velocity;
@@ -89,9 +91,9 @@ int main()
robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position_init);
vpColVector min(3), max(3);
- for (unsigned int i=0; i<3; i++) {
- min[i] = robot_cart_position_init[i] - cube_size/2;
- max[i] = robot_cart_position_init[i] + cube_size/2;
+ for (unsigned int i = 0; i < 3; i++) {
+ min[i] = robot_cart_position_init[i] - cube_size / 2;
+ max[i] = robot_cart_position_init[i] + cube_size / 2;
}
std::cout << "min: " << min.t() << std::endl;
std::cout << "max: " << max.t() << std::endl;
@@ -99,24 +101,22 @@ int main()
// Initialize the controller to position control
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
- for(unsigned int iter=0; iter<10000; iter++) {
+ for (unsigned int iter = 0; iter < 10000; iter++) {
virt_velocity = virtuose.getVelocity();
std::cout << "Virtuose velocity: " << virt_velocity.t() << std::endl;
robot.getPosition(vpRobot::REFERENCE_FRAME, robot_cart_position);
- for (int i=0; i < 3; i++) {
- if (robot_cart_position[i] >= max[i])
- {
+ 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;
- if (force_feedback_robot[i] <= -force_limit) force_feedback_robot[i] = -force_limit;
- }
- else if (robot_cart_position[i] <= min[i])
- {
+ 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;
- if (force_feedback_robot[i] >= force_limit) force_feedback_robot[i] = force_limit;
- }
- else
+ 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;
@@ -138,8 +138,7 @@ int main()
robot.stopMotion();
virtuose.setPowerOff();
std::cout << "The end" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
robot.stopMotion();
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
diff --git a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp
index f82d5d9..0bcef6f 100644
--- a/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp
+++ b/modules/robot/test/virtuose/testVirtuoseHapticBox.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,8 +40,9 @@
/*!
\example testVirtuoseHapticBox.cpp
- Test Haption Virtuose SDK wrapper to constrain movements inside a cube of pre-determined side.
- Hard springs are applied to rotations (only translations are allowed).
+ Test Haption Virtuose SDK wrapper to constrain movements inside a cube of
+ pre-determined side. Hard springs are applied to rotations (only
+ translations are allowed).
*/
#include <visp3/core/vpTime.h>
@@ -48,17 +50,17 @@
#if defined(VISP_HAVE_VIRTUOSE)
-void CallBackVirtuose(VirtContext VC, void* ptr)
+void CallBackVirtuose(VirtContext VC, void *ptr)
{
- (void) VC;
+ (void)VC;
static bool firstIteration = true;
static vpPoseVector localPosition0;
vpPoseVector localPosition;
- vpColVector forceFeedback(6,0);
- vpColVector finalForce(6,0);
- vpColVector forceEe(6,0);
+ vpColVector forceFeedback(6, 0);
+ vpColVector finalForce(6, 0);
+ vpColVector forceEe(6, 0);
int force_limit = 15;
int force_increase_rate = 500;
float cube_size = 0.05f;
@@ -70,34 +72,34 @@ 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 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);
- vpColVector zd(3,0);
- vpColVector xee(3,0);
- vpColVector zee(3,0);
- vpColVector xeed(3,0);
- vpColVector zeed(3,0);
- vpColVector zYZ(3,0);
- vpColVector zXZ(3,0);
- vpColVector xXY(3,0);
+ vpColVector xd(3, 0);
+ vpColVector yd(3, 0);
+ vpColVector zd(3, 0);
+ vpColVector xee(3, 0);
+ vpColVector zee(3, 0);
+ vpColVector xeed(3, 0);
+ vpColVector zeed(3, 0);
+ vpColVector zYZ(3, 0);
+ vpColVector zXZ(3, 0);
+ vpColVector xXY(3, 0);
vpTranslationVector tee;
- vpColVector omegad(3,0);
+ vpColVector omegad(3, 0);
vpRotationMatrix Qd;
vpRotationMatrix Qee;
vpPoseVector pee;
- vpColVector vee(6,0);
- vpColVector veed(6,0);
+ vpColVector vee(6, 0);
+ vpColVector veed(6, 0);
double alpha;
- vpColVector torque1(3,0);
- vpColVector torque2(3,0);
- vpColVector torque3(3,0);
+ vpColVector torque1(3, 0);
+ vpColVector torque2(3, 0);
+ vpColVector torque3(3, 0);
- vpVirtuose* p_virtuose=(vpVirtuose*)ptr;
+ vpVirtuose *p_virtuose = (vpVirtuose *)ptr;
localPosition = p_virtuose->getPhysicalPosition();
if (firstIteration) {
@@ -115,7 +117,7 @@ void CallBackVirtuose(VirtContext VC, void* ptr)
// X axis = [0 0 1]
xd[2] = 1;
// Y axis from cross product
- yd = zd.skew(zd)*xd;
+ yd = zd.skew(zd) * xd;
// Current orientation of the ee frame
pee.extract(Qee);
@@ -136,10 +138,10 @@ void CallBackVirtuose(VirtContext VC, void* ptr)
Qd[2][2] = zd[2];
// X and Z axis of the ee frame expressed in the desired frame
- xeed = Qd.inverse()*xee;
- zeed = Qd.inverse()*zee;
+ xeed = Qd.inverse() * xee;
+ zeed = Qd.inverse() * zee;
- vpHomogeneousMatrix dMb(tee,Qd);
+ vpHomogeneousMatrix dMb(tee, Qd);
// Velocity twist matrix for expressing velocities in the desired frame
vpVelocityTwistMatrix dVMb(dMb.inverse());
// Force twist matrix for expressing forces in the base frame
@@ -152,80 +154,82 @@ void CallBackVirtuose(VirtContext VC, void* ptr)
omegad[1] = veed[4];
omegad[2] = veed[5];
- // Projection of Z axis of the ee frame onto plane YZ (expressed in the desired frame)
+ // Projection of Z axis of the ee frame onto plane YZ (expressed in the
+ // desired frame)
zYZ[1] = zeed[1];
zYZ[2] = zeed[2];
- // Projection of Z axis of the ee frame onto plane XZ (expressed in the desired frame)
+ // Projection of Z axis of the ee frame onto plane XZ (expressed in the
+ // desired frame)
zXZ[0] = zeed[0];
zXZ[2] = zeed[2];
// Hard spring to keep Z axis of the ee frame in the horizontal plane
- // Spring applied to the angle between the Z axis of the ee frame and its projection in the YZ (horizontal) plane
- vpColVector rotzYZ(3,0);
- rotzYZ = zeed.skew(zeed)*zYZ.normalize();
- vpColVector forceStiff1 = virtualStiffnessAng*rotzYZ;
- vpColVector forceDamp1= virtualDamperAng*(omegad*rotzYZ.normalize())*rotzYZ.normalize();
-
- for (unsigned int i=0; i<3; i++)
+ // Spring applied to the angle between the Z axis of the ee frame and its
+ // projection in the YZ (horizontal) plane
+ vpColVector rotzYZ(3, 0);
+ rotzYZ = zeed.skew(zeed) * zYZ.normalize();
+ vpColVector forceStiff1 = virtualStiffnessAng * rotzYZ;
+ vpColVector forceDamp1 = virtualDamperAng * (omegad * rotzYZ.normalize()) * rotzYZ.normalize();
+
+ for (unsigned int i = 0; i < 3; i++)
torque1[i] = forceStiff1[i] - forceDamp1[i];
// Hard spring to keep Z axis of the ee frame pointing at the origin
- // Spring applied to the angle between the Z axis of the ee frame and its projection in the XZ (vertical) plane
- vpColVector rotzXZ(3,0);
- rotzXZ = zeed.skew(zeed)*zXZ.normalize();
- vpColVector forceStiff2 = virtualStiffnessAng*rotzXZ;
- vpColVector forceDamp2 = virtualDamperAng*(omegad*rotzXZ.normalize())*rotzXZ.normalize();
-
- for (unsigned int i=0; i<3; i++)
+ // Spring applied to the angle between the Z axis of the ee frame and its
+ // projection in the XZ (vertical) plane
+ vpColVector rotzXZ(3, 0);
+ rotzXZ = zeed.skew(zeed) * zXZ.normalize();
+ vpColVector forceStiff2 = virtualStiffnessAng * rotzXZ;
+ vpColVector forceDamp2 = virtualDamperAng * (omegad * rotzXZ.normalize()) * rotzXZ.normalize();
+
+ for (unsigned int i = 0; i < 3; i++)
torque2[i] = forceStiff2[i] - forceDamp2[i];
// Hard spring for rotation around z axis of the ee
xXY[0] = xeed[0];
xXY[1] = xeed[1];
- vpColVector xdd(3,0);
- xdd[0]=1;
- vpColVector zdd(3,0);
- zdd[2]=1;
- vpColVector rotxXY(3,0);
- rotxXY = xdd.skew(xdd)*xXY.normalize();
+ vpColVector xdd(3, 0);
+ xdd[0] = 1;
+ vpColVector zdd(3, 0);
+ zdd[2] = 1;
+ vpColVector rotxXY(3, 0);
+ rotxXY = xdd.skew(xdd) * xXY.normalize();
alpha = asin(rotxXY[2]);
- vpColVector forceStiff3 = virtualStiffnessAng*alpha*zdd;
- vpColVector forceDamp3 = virtualDamperAng2*(omegad*zdd)*zdd;
- for (unsigned int i=0; i<3; i++)
+ vpColVector forceStiff3 = virtualStiffnessAng * alpha * zdd;
+ vpColVector forceDamp3 = virtualDamperAng2 * (omegad * zdd) * zdd;
+ for (unsigned int i = 0; i < 3; i++)
torque3[i] = forceStiff3[i] - forceDamp3[i];
- for (unsigned int j=0; j<3; j++)
- forceEe[j+3] = torque1[j] + torque2[j]+torque3[j];
+ for (unsigned int j = 0; j < 3; j++)
+ forceEe[j + 3] = torque1[j] + torque2[j] + torque3[j];
forceEe = dFMb * forceEe;
// ---------------
// Haptic Box
// ---------------
- vpColVector p_min(3,0), p_max(3,0);
- for (unsigned int i=0; i<3; i++) {
+ vpColVector p_min(3, 0), p_max(3, 0);
+ for (unsigned int i = 0; i < 3; i++) {
p_min[i] = localPosition0[i] - cube_size / 2;
p_max[i] = localPosition0[i] + cube_size / 2;
}
- for (int i=0; i < 3; i++) {
- if ((p_min[i] >= localPosition[i]))
- {
+ for (int i = 0; i < 3; i++) {
+ if ((p_min[i] >= localPosition[i])) {
forceFeedback[i] = (p_min[i] - localPosition[i]) * force_increase_rate;
- if (forceFeedback[i] >= force_limit) forceFeedback[i] = force_limit;
- }
- else if ((p_max[i] <= localPosition[i]))
- {
+ if (forceFeedback[i] >= force_limit)
+ forceFeedback[i] = force_limit;
+ } else if ((p_max[i] <= localPosition[i])) {
forceFeedback[i] = (p_max[i] - localPosition[i]) * force_increase_rate;
- if (forceFeedback[i] <= -force_limit) forceFeedback[i] = -force_limit;
- }
- else
+ if (forceFeedback[i] <= -force_limit)
+ forceFeedback[i] = -force_limit;
+ } else
forceFeedback[i] = 0;
}
- for (unsigned int j=0; j<6; j++)
+ for (unsigned int j = 0; j < 6; j++)
finalForce[j] = forceFeedback[j] + forceEe[j];
// Set force feedback
@@ -247,9 +251,8 @@ int main()
int counter = 0;
bool swtch = true;
- while(swtch) {
- if (counter>=10)
- {
+ while (swtch) {
+ if (counter >= 10) {
virtuose.stopPeriodicFunction();
virtuose.setPowerOff();
swtch = false;
@@ -258,8 +261,7 @@ int main()
vpTime::sleepMs(1000);
}
std::cout << "The end" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
}
@@ -267,6 +269,8 @@ int main()
#else
int main()
{
- std::cout << "You should install pthread and/or Virtuose API to use this binary..." << std::endl;
+ std::cout << "You should install pthread and/or Virtuose API to use this "
+ "binary..."
+ << std::endl;
}
#endif
diff --git a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp
index 4015f52..8f424a6 100644
--- a/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp
+++ b/modules/robot/test/virtuose/testVirtuoseJointLimits.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,7 +40,8 @@
\example testVirtuoseJointLimits.cpp
Test Haption Virtuose for testing the force feedback in articular mode.
- A force is felt when approaching to the Virtuose's joint limits (estimated experimentally).
+ A force is felt when approaching to the Virtuose's joint limits (estimated
+ experimentally).
*/
#include <visp3/core/vpTime.h>
@@ -47,40 +49,37 @@
#if defined(VISP_HAVE_VIRTUOSE)
-void CallBackVirtuose(VirtContext VC, void* ptr)
+void CallBackVirtuose(VirtContext VC, void *ptr)
{
- (void) VC;
- vpVirtuose* p_virtuose=(vpVirtuose*)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);
vpColVector forceFeedback(numJoint, 0);
int feedbackRegionFactor = 10;
- float saturationForce[6] = {5,5,5,2.5,2.5,2.5};
+ float saturationForce[6] = {5, 5, 5, 2.5, 2.5, 2.5};
- for (unsigned int iter=0; iter<numJoint; iter++)
- feedbackRegion[iter] = (maxQ[iter] - minQ[iter])/feedbackRegionFactor;
+ for (unsigned int iter = 0; iter < numJoint; iter++)
+ feedbackRegion[iter] = (maxQ[iter] - minQ[iter]) / feedbackRegionFactor;
vpColVector currentQ = p_virtuose->getArticularPosition();
// force feedback definition
- 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);
+ 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;
- }
- else if (currentQ[iter] <= (minQ[iter] + feedbackRegion[iter]))
- {
- forceFeedback[iter] = saturationForce[iter] * pow((minQ[iter] + feedbackRegion[iter] - currentQ[iter]) / feedbackRegion[iter], 2);
+ } 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;
- }
- else
- {
+ } else {
forceFeedback[iter] = 0;
std::cout << "Safe zone" << std::endl;
}
@@ -136,8 +135,10 @@ int main()
std::cout << "Min Joint values: " << min_joint.t() << std::endl;
// Best Result (small errors are to be expected)
- // Max Joint values: 0.7811045051 -0.07668215036 2.481732368 2.819076777 1.044736624 2.687076807
- // Min Joint values: -0.8011951447 -1.648244739 0.7439950705 -3.022218227 -1.260564089 -2.054088593
+ // Max Joint values: 0.7811045051 -0.07668215036 2.481732368
+ 2.819076777 1.044736624 2.687076807
+ // Min Joint values: -0.8011951447 -1.648244739 0.7439950705
+ -3.022218227 -1.260564089 -2.054088593
*/
virtuose.setPeriodicFunction(CallBackVirtuose);
@@ -146,9 +147,8 @@ int main()
int counter = 0;
bool swtch = true;
- while(swtch) {
- if (counter>=10)
- {
+ while (swtch) {
+ if (counter >= 10) {
virtuose.stopPeriodicFunction();
virtuose.setPowerOff();
swtch = false;
@@ -158,15 +158,11 @@ int main()
}
std::cout << "The end" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
}
#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 ed0acd9..c3926dd 100644
--- a/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp
+++ b/modules/robot/test/virtuose/testVirtuosePeriodicFunction.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,13 +47,13 @@
#if defined(VISP_HAVE_VIRTUOSE)
-void CallBackVirtuose(VirtContext VC, void* ptr)
+void CallBackVirtuose(VirtContext VC, void *ptr)
{
- (void) VC;
- vpVirtuose* p_virtuose=(vpVirtuose*)ptr;
+ (void)VC;
+ vpVirtuose *p_virtuose = (vpVirtuose *)ptr;
vpPoseVector localPose = p_virtuose->getPhysicalPosition();
- vpColVector vel = p_virtuose->getPhysicalVelocity();
+ vpColVector vel = p_virtuose->getPhysicalVelocity();
std::cout << "pose: " << localPose.t() << std::endl;
std::cout << "vel: " << vel.t() << std::endl;
@@ -70,9 +71,8 @@ int main()
int counter = 0;
bool swtch = true;
- while(swtch){
- if (counter>=2)
- {
+ while (swtch) {
+ if (counter >= 2) {
virtuose.stopPeriodicFunction();
swtch = false;
}
@@ -80,15 +80,11 @@ int main()
vpTime::sleepMs(1000);
}
std::cout << "The end" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
}
#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/CMakeLists.txt b/modules/sensor/CMakeLists.txt
index a377a6f..b8ab1a1 100644
--- a/modules/sensor/CMakeLists.txt
+++ b/modules/sensor/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -41,7 +42,8 @@
set(opt_incs "")
set(opt_libs "")
-# camera devices: v4l, dc1394, cmu1394, directshow ?, freenect, flycapture
+# camera devices: v4l, dc1394, cmu1394, directshow ?, freenect,
+# flycapture, pylon
if(USE_V4L2)
list(APPEND opt_incs ${V4L2_INCLUDE_DIRS})
list(APPEND opt_libs ${V4L2_LIBRARIES})
@@ -70,6 +72,10 @@ if(USE_FLYCAPTURE)
list(APPEND opt_incs ${FLYCAPTURE_INCLUDE_DIRS})
list(APPEND opt_libs ${FLYCAPTURE_LIBRARIES})
endif()
+if(USE_PYLON)
+ list(APPEND opt_incs ${PYLON_INCLUDE_DIRS})
+ list(APPEND opt_libs ${PYLON_LIBRARIES})
+endif()
if(USE_COMEDI)
list(APPEND opt_incs ${COMEDI_INCLUDE_DIRS})
list(APPEND opt_libs ${COMEDI_LIBRARIES})
@@ -78,6 +84,10 @@ if(USE_REALSENSE)
list(APPEND opt_incs ${REALSENSE_INCLUDE_DIRS})
list(APPEND opt_libs ${REALSENSE_LIBRARIES})
endif()
+if(USE_REALSENSE2)
+ list(APPEND opt_incs ${REALSENSE2_INCLUDE_DIRS})
+ list(APPEND opt_libs ${REALSENSE2_LIBRARIES})
+endif()
if(USE_PCL)
list(APPEND opt_incs ${PCL_INCLUDE_DIRS})
@@ -149,7 +159,7 @@ if(USE_PCL)
endforeach()
if(VTK_VERSION VERSION_EQUAL 6.2.0)
mark_as_advanced(Qt5Core_DIR Qt5Gui_DIR Qt5Network_DIR Qt5WebKit_DIR Qt5Widgets_DIR)
- elseif(VTK_VERSION VERSION_EQUAL 7.1.0)
+ elseif(VTK_VERSION VERSION_EQUAL 7.1.0 OR VTK_VERSION VERSION_EQUAL 8.0.0)
mark_as_advanced(DAVIDSDK_INCLUDE_DIR DAVIDSDK_LIBRARY DSSDK_DIR ENSENSO_INCLUDE_DIR ENSENSO_LIBRARY)
mark_as_advanced(GLEW_INCLUDE_DIR GLEW_GLEW_LIBRARY)
mark_as_advanced(Qt5Core_DIR Qt5Gui_DIR Qt5Sql_DIR Qt5Widgets_DIR RSSDK_DIR)
@@ -157,8 +167,11 @@ if(USE_PCL)
endif()
endif()
-# atidac is private
-include_directories(${ATIDAQ_INCLUDE_DIRS})
+if(WITH_ATIDAQ)
+ # atidac is private
+ include_directories(${ATIDAQ_INCLUDE_DIRS})
+endif()
+
vp_add_module(sensor visp_core PRIVATE_OPTIONAL ${ATIDAQ_LIBRARIES})
vp_glob_module_sources()
@@ -166,9 +179,22 @@ if(USE_FLYCAPTURE)
# Add specific build flag to turn off warnings coming from PointGrey flycapture 3rd party
vp_set_source_file_compile_flag(src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp -Wno-unknown-pragmas -Wno-ignored-qualifiers)
endif()
+if(USE_PYLON)
+ # Add specific build flag to turn off warnings coming from Basler
+ # pylon headers
+ vp_set_source_file_compile_flag(src/framegrabber/pylon/vpPylonFactory.cpp -Wno-unknown-pragmas -Wno-unused-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/framegrabber/pylon/vpPylonGrabberGigE.cpp -Wno-unknown-pragmas -Wno-unused-parameter -Wno-overloaded-virtual -Wno-non-pod-varargs /wd"4244" /wd"4267")
+ vp_set_source_file_compile_flag(src/framegrabber/pylon/vpPylonGrabberUsb.cpp -Wno-unknown-pragmas -Wno-unused-parameter -Wno-overloaded-virtual -Wno-non-pod-varargs /wd"4244" /wd"4267")
+ vp_set_source_file_compile_flag(test/framegrabber/testPylonGrabber.cpp -Wno-unknown-pragmas -Wno-overloaded-virtual -Wno-unused-parameter)
+endif()
if(USE_REALSENSE)
# Add specific build flag to turn off warnings coming from RealSense 3rd party
- vp_set_source_file_compile_flag(src/rgb-depth/realsense/vpRealSense.cpp -Wno-strict-aliasing)
+ vp_set_source_file_compile_flag(src/rgb-depth/realsense/vpRealSense.cpp -Wno-strict-aliasing -Wno-pessimizing-move -Wno-unused-parameter)
+endif()
+if(USE_REALSENSE2)
+ # Add specific build flag to turn off warnings coming from RealSense2 3rd party
+ vp_set_source_file_compile_flag(src/rgb-depth/realsense/vpRealSense2.cpp -Wno-reorder -Wno-unused-function)
+ vp_set_source_file_compile_flag(test/rgb-depth/testRealSense2_SR300.cpp -Wno-reorder -Wno-unused-function)
endif()
vp_module_include_directories(${opt_incs})
@@ -177,5 +203,7 @@ vp_add_tests(CTEST_EXCLUDE_PATH framegrabber force-torque rgb-depth DEPENDS_ON v
if(USE_PCL)
# Add specific build flag to turn off warnings coming from PCL 3rd party
- vp_set_source_file_compile_flag(test/rgb-depth/testRealSense.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal)
+ vp_set_source_file_compile_flag(test/rgb-depth/testRealSense_R200.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal -Wno-pessimizing-move -Wno-unused-parameter)
+ vp_set_source_file_compile_flag(test/rgb-depth/testRealSense_SR300.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal -Wno-pessimizing-move -Wno-unused-parameter)
+ vp_set_source_file_compile_flag(test/rgb-depth/testRealSense2_SR300.cpp -Wno-deprecated-declarations -Wno-inconsistent-missing-override -Wno-sign-conversion -Wno-float-equal -Wno-pessimizing-move -Wno-unused-parameter)
endif()
diff --git a/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h
index 5099895..1eada0b 100644
--- a/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h
+++ b/modules/sensor/include/visp3/sensor/vp1394CMUGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,15 +45,17 @@
#ifdef VISP_HAVE_CMU1394
-#include <windows.h>
+// Include WinSock2.h before windows.h to ensure that winsock.h is not
+// included by windows.h since winsock.h and winsock2.h are incompatible
#include <1394Camera.h> // CMU library
+#include <WinSock2.h>
+#include <windows.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpFrameGrabber.h>
#include <visp3/core/vpFrameGrabberException.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\file vp1394CMUGrabber.h
\brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
@@ -65,27 +68,29 @@
\brief Firewire cameras video capture based on CMU 1394 Digital Camera SDK.
- Thus to be enabled, this class needs the optional CMU 1394 3rd party. Installation instruction
- are provided here https://visp.inria.fr/3rd_cmu1394.
+ Thus to be enabled, this class needs the optional CMU 1394 3rd party.
+Installation instruction are provided here https://visp.inria.fr/3rd_cmu1394.
- This block is based on the CMU 1394 Digital Camera SDK. The CMU 1394 Digital
- Camera Driver must be installed (go to http://www.cs.cmu.edu/~iwan/1394/index.html
- to download it).
+ This block is based on the CMU 1394 Digital Camera SDK. The CMU 1394
+Digital Camera Driver must be installed (go to
+http://www.cs.cmu.edu/~iwan/1394/index.html to download it).
- Parameters:
- Camera index (0, 1, 2, ... or 10). First camera has index 0.
- Image format
- Frame rate. Real frame rate depends on device capacities.
- - Control : shutter speed and gain can be internally set, but it is possible to set manually them.
- * Exposure time register value. Real exposure time depends on device capacities.
+ - Control : shutter speed and gain can be internally set, but it is
+possible to set manually them.
+ * Exposure time register value. Real exposure time depends on device
+capacities.
* Gain register value. Real gain depends on device capacities.
- This first example available in tutorial-grabber-CMU1394.cpp shows how to grab
- and display images from a firewire camera under Windows.
+ This first example available in tutorial-grabber-CMU1394.cpp shows how to
+grab and display images from a firewire camera under Windows.
\include tutorial-grabber-CMU1394.cpp
- This other example shows how to consider more than one firewire camera, and how
- to grab and display images from the first camera found on the bus.
+ This other example shows how to consider more than one firewire camera, and
+how to grab and display images from the first camera found on the bus.
\code
#include <iostream>
@@ -104,10 +109,10 @@ int main()
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;
+ 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);
@@ -149,17 +154,9 @@ public:
/*!
Enumeration of color codings.
*/
- typedef enum {
- YUV444,
- YUV422,
- YUV411,
- RGB8,
- MONO8,
- MONO16,
- UNKNOWN
- } vpColorCodingType;
-
-private :
+ typedef enum { YUV444, YUV422, YUV411, RGB8, MONO8, MONO16, UNKNOWN } vpColorCodingType;
+
+private:
//! Current camera handle
C1394Camera *camera;
//! Camera index on the bus
@@ -167,9 +164,9 @@ private :
//! Current video format
unsigned long _format;
//! Current video mode
- unsigned long _mode ;
+ unsigned long _mode;
//! Current video frame rate
- unsigned long _fps ;
+ unsigned long _fps;
//! Current auto mode
bool _modeauto;
//! Current gain
@@ -180,7 +177,6 @@ private :
vpColorCodingType _color;
public:
-
// Constructor.
vp1394CMUGrabber();
// Destructor.
@@ -198,64 +194,103 @@ public:
// Display information about the camera on the standard output.
void displayCameraDescription(int cam_id);
- // Display camera model on the standard output. Call it after open the grabber.
+ // Display camera model on the standard output. Call it after open the
+ // grabber.
void displayCameraModel();
// Get the video framerate
int getFramerate();
- // Get the gain min and max values.
+ // Get the gain min and max values.
void getGainMinMax(unsigned short &min, unsigned short &max);
// Get the number of connected cameras.
- int getNumberOfConnectedCameras() const ;
+ int getNumberOfConnectedCameras() const;
- // Get the shutter min and max values.
+ // Get the shutter min and max values.
void getShutterMinMax(unsigned short &min, unsigned short &max);
//! Get the video color coding format.
vpColorCodingType getVideoColorCoding() const
{
vpColorCodingType color = vp1394CMUGrabber::UNKNOWN;
- if (_format == 0)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV444; break;
- case 1: color = vp1394CMUGrabber::YUV422; break;
- case 2: color = vp1394CMUGrabber::YUV411; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
+ if (_format == 0) {
+ switch (_mode) {
+ case 0:
+ color = vp1394CMUGrabber::YUV444;
+ break;
+ case 1:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 2:
+ color = vp1394CMUGrabber::YUV411;
+ break;
+ case 3:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 4:
+ color = vp1394CMUGrabber::RGB8;
+ break;
+ case 5:
+ color = vp1394CMUGrabber::MONO8;
+ break;
+ case 6:
+ color = vp1394CMUGrabber::MONO16;
+ break;
}
- }
- else if (_format == 1)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV422; break;
- case 1: color = vp1394CMUGrabber::RGB8; break;
- case 2: color = vp1394CMUGrabber::MONO8; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
- case 7: color = vp1394CMUGrabber::MONO16; break;
+ } else if (_format == 1) {
+ switch (_mode) {
+ case 0:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 1:
+ color = vp1394CMUGrabber::RGB8;
+ break;
+ case 2:
+ color = vp1394CMUGrabber::MONO8;
+ break;
+ case 3:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 4:
+ color = vp1394CMUGrabber::RGB8;
+ break;
+ case 5:
+ color = vp1394CMUGrabber::MONO8;
+ break;
+ case 6:
+ color = vp1394CMUGrabber::MONO16;
+ break;
+ case 7:
+ color = vp1394CMUGrabber::MONO16;
+ break;
}
- }
- else if (_format == 2)
- {
- switch(_mode)
- {
- case 0: color = vp1394CMUGrabber::YUV422; break;
- case 1: color = vp1394CMUGrabber::RGB8; break;
- case 2: color = vp1394CMUGrabber::MONO8; break;
- case 3: color = vp1394CMUGrabber::YUV422; break;
- case 4: color = vp1394CMUGrabber::RGB8; break;
- case 5: color = vp1394CMUGrabber::MONO8; break;
- case 6: color = vp1394CMUGrabber::MONO16; break;
- case 7: color = vp1394CMUGrabber::MONO16; break;
+ } else if (_format == 2) {
+ switch (_mode) {
+ case 0:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 1:
+ color = vp1394CMUGrabber::RGB8;
+ break;
+ case 2:
+ color = vp1394CMUGrabber::MONO8;
+ break;
+ case 3:
+ color = vp1394CMUGrabber::YUV422;
+ break;
+ case 4:
+ color = vp1394CMUGrabber::RGB8;
+ break;
+ case 5:
+ color = vp1394CMUGrabber::MONO8;
+ break;
+ case 6:
+ color = vp1394CMUGrabber::MONO16;
+ break;
+ case 7:
+ color = vp1394CMUGrabber::MONO16;
+ break;
}
}
@@ -268,8 +303,8 @@ public:
// Initialization of the grabber using a color image.
void open(vpImage<vpRGBa> &I);
- vp1394CMUGrabber & operator>>(vpImage<unsigned char> &I);
- vp1394CMUGrabber & operator>>(vpImage<vpRGBa> &I);
+ vp1394CMUGrabber &operator>>(vpImage<unsigned char> &I);
+ vp1394CMUGrabber &operator>>(vpImage<vpRGBa> &I);
// Select the camera on the bus. Call it before open the grabber.
void selectCamera(int cam_id);
@@ -293,12 +328,10 @@ public:
void setGain(unsigned short gain);
// Set the video format and mode. Call it before open the grabber.
- void setVideoMode(unsigned long format, unsigned long mode );
-
-private :
+ void setVideoMode(unsigned long format, unsigned long mode);
+private:
void initCamera();
-
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h
index dfb8418..a154b67 100644
--- a/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h
+++ b/modules/sensor/include/visp3/sensor/vp1394TwoGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,8 +42,8 @@
\warning This class is a wrapper over libdc1394-2 optional 3rd party.
- Thus to be enabled, this class needs the optional libdc1394-2 3rd party. Installation instruction
- are provided here https://visp.inria.fr/3rd_dc1394.
+ Thus to be enabled, this class needs the optional libdc1394-2 3rd party.
+ Installation instruction are provided here https://visp.inria.fr/3rd_dc1394.
vp1394TwoGrabber was tested with the following cameras:
- Marlin F033C, F131B
@@ -60,15 +61,14 @@
#if defined(VISP_HAVE_DC1394)
-
/*!
\class vp1394TwoGrabber
\ingroup group_sensor_camera
-
+
\brief Class for firewire ieee1394 video devices using libdc1394-2.x api.
-
+
Needs libraw1394-1.2.0 and libdc1394-2.0.0 or more recent versions
available on http://sourceforge.net.
@@ -77,17 +77,18 @@
\ingroup libdevice
- This grabber allows single or multi camera acquisition.
+ This grabber allows single or multi camera acquisition.
- - Here you will find an example of single capture from the first camera found
- on the bus. This example is available in tutorial-grabber-1394.cpp:
+ - Here you will find an example of single capture from the first camera
+found on the bus. This example is available in tutorial-grabber-1394.cpp:
\include tutorial-grabber-1394.cpp
- A line by line explanation of this example is provided in \ref tutorial-grabber.
- An other example that shows how to use format 7
- and the auto-shutter is provided in vp1394TwoGrabber() constructor:
-
- - If more than one camera is connected, it is also possible to select a specific camera by its GUID:
- \code
+ A line by line explanation of this example is provided in \ref
+tutorial-grabber. An other example that shows how to use format 7 and the
+auto-shutter is provided in vp1394TwoGrabber() constructor:
+
+ - If more than one camera is connected, it is also possible to select a
+specific camera by its GUID:
+\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
@@ -119,14 +120,15 @@ int main()
printf("Use camera with GUID: 0x%lx\n", (long unsigned int)g.getGuid());
g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
-
+
vpImageIo::write(I, "image.pgm"); // Write image on the disk
#endif
}
\endcode
- - Here an example of multi camera capture. An other example is available in setCamera():
- \code
+ - Here an example of multi camera capture. An other example is available in
+setCamera():
+\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
@@ -177,18 +179,17 @@ int main()
#include <dc1394/utils.h>
#include <dc1394/vendor/avt.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-
class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
{
- public:
- static const char * strVideoMode[DC1394_VIDEO_MODE_NUM];
- static const char * strFramerate[DC1394_FRAMERATE_NUM];
- static const char * strColorCoding[DC1394_COLOR_CODING_NUM];
+public:
+ static const char *strVideoMode[DC1394_VIDEO_MODE_NUM];
+ static const char *strFramerate[DC1394_FRAMERATE_NUM];
+ static const char *strColorCoding[DC1394_COLOR_CODING_NUM];
/*!
Enumeration of iso speed. See libdc1394 2.x header file
@@ -208,30 +209,30 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
dc1394/control.h
*/
typedef enum {
- vpVIDEO_MODE_160x120_YUV444 = DC1394_VIDEO_MODE_160x120_YUV444,
- vpVIDEO_MODE_320x240_YUV422 = DC1394_VIDEO_MODE_320x240_YUV422,
- vpVIDEO_MODE_640x480_YUV411 = DC1394_VIDEO_MODE_640x480_YUV411,
- vpVIDEO_MODE_640x480_YUV422 = DC1394_VIDEO_MODE_640x480_YUV422,
- vpVIDEO_MODE_640x480_RGB8 = DC1394_VIDEO_MODE_640x480_RGB8,
- vpVIDEO_MODE_640x480_MONO8 = DC1394_VIDEO_MODE_640x480_MONO8,
- vpVIDEO_MODE_640x480_MONO16 = DC1394_VIDEO_MODE_640x480_MONO16,
- vpVIDEO_MODE_800x600_YUV422 = DC1394_VIDEO_MODE_800x600_YUV422,
- vpVIDEO_MODE_800x600_RGB8 = DC1394_VIDEO_MODE_800x600_RGB8,
- vpVIDEO_MODE_800x600_MONO8 = DC1394_VIDEO_MODE_800x600_MONO8,
- vpVIDEO_MODE_1024x768_YUV422 = DC1394_VIDEO_MODE_1024x768_YUV422,
- vpVIDEO_MODE_1024x768_RGB8 = DC1394_VIDEO_MODE_1024x768_RGB8,
- vpVIDEO_MODE_1024x768_MONO8 = DC1394_VIDEO_MODE_1024x768_MONO8,
- vpVIDEO_MODE_800x600_MONO16 = DC1394_VIDEO_MODE_800x600_MONO16,
- vpVIDEO_MODE_1024x768_MONO16 = DC1394_VIDEO_MODE_1024x768_MONO16,
- vpVIDEO_MODE_1280x960_YUV422 = DC1394_VIDEO_MODE_1280x960_YUV422,
- vpVIDEO_MODE_1280x960_RGB8 = DC1394_VIDEO_MODE_1280x960_RGB8,
- vpVIDEO_MODE_1280x960_MONO8 = DC1394_VIDEO_MODE_1280x960_MONO8,
+ vpVIDEO_MODE_160x120_YUV444 = DC1394_VIDEO_MODE_160x120_YUV444,
+ vpVIDEO_MODE_320x240_YUV422 = DC1394_VIDEO_MODE_320x240_YUV422,
+ vpVIDEO_MODE_640x480_YUV411 = DC1394_VIDEO_MODE_640x480_YUV411,
+ vpVIDEO_MODE_640x480_YUV422 = DC1394_VIDEO_MODE_640x480_YUV422,
+ vpVIDEO_MODE_640x480_RGB8 = DC1394_VIDEO_MODE_640x480_RGB8,
+ vpVIDEO_MODE_640x480_MONO8 = DC1394_VIDEO_MODE_640x480_MONO8,
+ vpVIDEO_MODE_640x480_MONO16 = DC1394_VIDEO_MODE_640x480_MONO16,
+ vpVIDEO_MODE_800x600_YUV422 = DC1394_VIDEO_MODE_800x600_YUV422,
+ vpVIDEO_MODE_800x600_RGB8 = DC1394_VIDEO_MODE_800x600_RGB8,
+ vpVIDEO_MODE_800x600_MONO8 = DC1394_VIDEO_MODE_800x600_MONO8,
+ vpVIDEO_MODE_1024x768_YUV422 = DC1394_VIDEO_MODE_1024x768_YUV422,
+ vpVIDEO_MODE_1024x768_RGB8 = DC1394_VIDEO_MODE_1024x768_RGB8,
+ vpVIDEO_MODE_1024x768_MONO8 = DC1394_VIDEO_MODE_1024x768_MONO8,
+ vpVIDEO_MODE_800x600_MONO16 = DC1394_VIDEO_MODE_800x600_MONO16,
+ vpVIDEO_MODE_1024x768_MONO16 = DC1394_VIDEO_MODE_1024x768_MONO16,
+ vpVIDEO_MODE_1280x960_YUV422 = DC1394_VIDEO_MODE_1280x960_YUV422,
+ vpVIDEO_MODE_1280x960_RGB8 = DC1394_VIDEO_MODE_1280x960_RGB8,
+ vpVIDEO_MODE_1280x960_MONO8 = DC1394_VIDEO_MODE_1280x960_MONO8,
vpVIDEO_MODE_1600x1200_YUV422 = DC1394_VIDEO_MODE_1600x1200_YUV422,
- vpVIDEO_MODE_1600x1200_RGB8 = DC1394_VIDEO_MODE_1600x1200_RGB8,
- vpVIDEO_MODE_1600x1200_MONO8 = DC1394_VIDEO_MODE_1600x1200_MONO8,
- vpVIDEO_MODE_1280x960_MONO16 = DC1394_VIDEO_MODE_1280x960_MONO16,
+ vpVIDEO_MODE_1600x1200_RGB8 = DC1394_VIDEO_MODE_1600x1200_RGB8,
+ vpVIDEO_MODE_1600x1200_MONO8 = DC1394_VIDEO_MODE_1600x1200_MONO8,
+ vpVIDEO_MODE_1280x960_MONO16 = DC1394_VIDEO_MODE_1280x960_MONO16,
vpVIDEO_MODE_1600x1200_MONO16 = DC1394_VIDEO_MODE_1600x1200_MONO16,
- vpVIDEO_MODE_EXIF = DC1394_VIDEO_MODE_EXIF,
+ vpVIDEO_MODE_EXIF = DC1394_VIDEO_MODE_EXIF,
vpVIDEO_MODE_FORMAT7_0 = DC1394_VIDEO_MODE_FORMAT7_0,
vpVIDEO_MODE_FORMAT7_1 = DC1394_VIDEO_MODE_FORMAT7_1,
vpVIDEO_MODE_FORMAT7_2 = DC1394_VIDEO_MODE_FORMAT7_2,
@@ -248,13 +249,13 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
*/
typedef enum {
vpFRAMERATE_1_875 = DC1394_FRAMERATE_1_875,
- vpFRAMERATE_3_75 = DC1394_FRAMERATE_3_75,
- vpFRAMERATE_7_5 = DC1394_FRAMERATE_7_5,
- vpFRAMERATE_15 = DC1394_FRAMERATE_15,
- vpFRAMERATE_30 = DC1394_FRAMERATE_30,
- vpFRAMERATE_60 = DC1394_FRAMERATE_60,
- vpFRAMERATE_120 = DC1394_FRAMERATE_120,
- vpFRAMERATE_240 = DC1394_FRAMERATE_240
+ vpFRAMERATE_3_75 = DC1394_FRAMERATE_3_75,
+ vpFRAMERATE_7_5 = DC1394_FRAMERATE_7_5,
+ vpFRAMERATE_15 = DC1394_FRAMERATE_15,
+ vpFRAMERATE_30 = DC1394_FRAMERATE_30,
+ vpFRAMERATE_60 = DC1394_FRAMERATE_60,
+ vpFRAMERATE_120 = DC1394_FRAMERATE_120,
+ vpFRAMERATE_240 = DC1394_FRAMERATE_240
} vp1394TwoFramerateType;
/*!
@@ -262,100 +263,100 @@ class VISP_EXPORT vp1394TwoGrabber : public vpFrameGrabber
dc1394/control.h
*/
typedef enum {
- vpCOLOR_CODING_MONO8 = DC1394_COLOR_CODING_MONO8,
- vpCOLOR_CODING_YUV411 = DC1394_COLOR_CODING_YUV411,
- vpCOLOR_CODING_YUV422 = DC1394_COLOR_CODING_YUV422,
- vpCOLOR_CODING_YUV444 = DC1394_COLOR_CODING_YUV444,
- vpCOLOR_CODING_RGB8 = DC1394_COLOR_CODING_RGB8,
- vpCOLOR_CODING_MONO16 = DC1394_COLOR_CODING_MONO16,
- vpCOLOR_CODING_RGB16 = DC1394_COLOR_CODING_RGB16,
+ vpCOLOR_CODING_MONO8 = DC1394_COLOR_CODING_MONO8,
+ vpCOLOR_CODING_YUV411 = DC1394_COLOR_CODING_YUV411,
+ vpCOLOR_CODING_YUV422 = DC1394_COLOR_CODING_YUV422,
+ vpCOLOR_CODING_YUV444 = DC1394_COLOR_CODING_YUV444,
+ vpCOLOR_CODING_RGB8 = DC1394_COLOR_CODING_RGB8,
+ vpCOLOR_CODING_MONO16 = DC1394_COLOR_CODING_MONO16,
+ vpCOLOR_CODING_RGB16 = DC1394_COLOR_CODING_RGB16,
vpCOLOR_CODING_MONO16S = DC1394_COLOR_CODING_MONO16S,
- vpCOLOR_CODING_RGB16S = DC1394_COLOR_CODING_RGB16S,
- vpCOLOR_CODING_RAW8 = DC1394_COLOR_CODING_RAW8,
- vpCOLOR_CODING_RAW16 = DC1394_COLOR_CODING_RAW16
+ vpCOLOR_CODING_RGB16S = DC1394_COLOR_CODING_RGB16S,
+ vpCOLOR_CODING_RAW8 = DC1394_COLOR_CODING_RAW8,
+ vpCOLOR_CODING_RAW16 = DC1394_COLOR_CODING_RAW16
} vp1394TwoColorCodingType;
-
+
/*!
- Enumeration of the parameters that can be modified. See libdc1394 2.x header
- file dc1394/control.h
+ Enumeration of the parameters that can be modified. See libdc1394 2.x
+ header file dc1394/control.h
*/
typedef enum {
- vpFEATURE_BRIGHTNESS = DC1394_FEATURE_BRIGHTNESS,
- vpFEATURE_EXPOSURE = DC1394_FEATURE_EXPOSURE,
- vpFEATURE_SHARPNESS = DC1394_FEATURE_SHARPNESS,
-// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
- vpFEATURE_HUE = DC1394_FEATURE_HUE,
- vpFEATURE_SATURATION = DC1394_FEATURE_SATURATION,
- vpFEATURE_GAMMA = DC1394_FEATURE_GAMMA,
- vpFEATURE_SHUTTER = DC1394_FEATURE_SHUTTER,
- vpFEATURE_GAIN = DC1394_FEATURE_GAIN,
- vpFEATURE_IRIS = DC1394_FEATURE_IRIS,
-// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
-// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
-// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
-// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
-// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
-// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
-// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
-// vpFEATURE_PAN = DC1394_FEATURE_PAN,
-// vpFEATURE_TILT = DC1394_FEATURE_TILT,
-// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
-// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
-// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ vpFEATURE_BRIGHTNESS = DC1394_FEATURE_BRIGHTNESS,
+ vpFEATURE_EXPOSURE = DC1394_FEATURE_EXPOSURE,
+ vpFEATURE_SHARPNESS = DC1394_FEATURE_SHARPNESS,
+ // vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ vpFEATURE_HUE = DC1394_FEATURE_HUE,
+ vpFEATURE_SATURATION = DC1394_FEATURE_SATURATION,
+ vpFEATURE_GAMMA = DC1394_FEATURE_GAMMA,
+ vpFEATURE_SHUTTER = DC1394_FEATURE_SHUTTER,
+ vpFEATURE_GAIN = DC1394_FEATURE_GAIN,
+ vpFEATURE_IRIS = DC1394_FEATURE_IRIS,
+ // vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+ // vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+ // vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+ // vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+ // vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+ // vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+ // vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+ // vpFEATURE_PAN = DC1394_FEATURE_PAN,
+ // vpFEATURE_TILT = DC1394_FEATURE_TILT,
+ // vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+ // vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+ // vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
} vp1394TwoParametersType;
-
-
- private:
-
+
+private:
/*!
- Control structure of the values that can be modified during the execution.
+ Control structure of the values that can be modified during the execution.
*/
- typedef struct{
- uint32_t brightness ;
+ typedef struct {
+ uint32_t brightness;
uint32_t exposure;
uint32_t sharpness;
-// uint32_t whiteBalance;
+ // uint32_t whiteBalance;
uint32_t hue;
uint32_t saturation;
uint32_t gamma;
- uint32_t shutter ;
- uint32_t gain ;
+ uint32_t shutter;
+ uint32_t gain;
uint32_t iris;
-// uint32_t focus;
-// uint32_t temperature ;
-// uint32_t trigger ;
-// uint32_t triggerDelay ;
-// uint32_t whiteShadding ;
-// uint32_t frameRate ;
-// uint32_t zoom;
-// uint32_t pan;
-// uint32_t tilt ;
-// uint32_t opticalFilter ;
-// uint32_t captureSize;
-// uint32_t captureQuality ;
- } vpDc1394TwoCameraParametersData;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vp1394TwoGrabber(const vp1394TwoGrabber &)
-// : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), camIsOpen(NULL),
-// num_buffers(4), // ring buffer size
-// isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
-// #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
-// , d(NULL),
-// list(NULL)
-// #endif
-// {
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// }
-// vp1394TwoGrabber &operator=(const vp1394TwoGrabber &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
-//#endif
+ // uint32_t focus;
+ // uint32_t temperature ;
+ // uint32_t trigger ;
+ // uint32_t triggerDelay ;
+ // uint32_t whiteShadding ;
+ // uint32_t frameRate ;
+ // uint32_t zoom;
+ // uint32_t pan;
+ // uint32_t tilt ;
+ // uint32_t opticalFilter ;
+ // uint32_t captureSize;
+ // uint32_t captureQuality ;
+ } vpDc1394TwoCameraParametersData;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vp1394TwoGrabber(const vp1394TwoGrabber &)
+ // : camera(NULL), cameras(NULL), num_cameras(0), camera_id(0),
+ // verbose(false), camIsOpen(NULL),
+ // num_buffers(4), // ring buffer size
+ // isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
+ // #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API >
+ // libdc1394-2.0.0-rc7 , d(NULL),
+ // list(NULL)
+ // #endif
+ // {
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!");
+ // }
+ // vp1394TwoGrabber &operator=(const vp1394TwoGrabber &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vp1394TwoGrabber(bool reset=true);
+ explicit vp1394TwoGrabber(bool reset = true);
virtual ~vp1394TwoGrabber();
void acquire(vpImage<unsigned char> &I);
@@ -381,12 +382,10 @@ public:
uint64_t getCamera();
void getCamera(uint64_t &camera);
- void getColorCoding(vp1394TwoColorCodingType & coding);
- uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode,
- std::list<vp1394TwoColorCodingType> & codings);
- void getFramerate(vp1394TwoFramerateType & fps);
- uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode,
- std::list<vp1394TwoFramerateType> & fps);
+ void getColorCoding(vp1394TwoColorCodingType &coding);
+ uint32_t getColorCodingSupported(vp1394TwoVideoModeType videomode, std::list<vp1394TwoColorCodingType> &codings);
+ void getFramerate(vp1394TwoFramerateType &fps);
+ uint32_t getFramerateSupported(vp1394TwoVideoModeType videomode, std::list<vp1394TwoFramerateType> &fps);
uint64_t getGuid();
void getGuid(uint64_t &guid);
void getHeight(unsigned int &height);
@@ -395,24 +394,22 @@ public:
unsigned int getNumCameras() const;
unsigned int getParameterValue(vp1394TwoParametersType param);
unsigned int getRingBufferSize() const;
- void getVideoMode(vp1394TwoVideoModeType & videomode);
- uint32_t getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes);
+ void getVideoMode(vp1394TwoVideoModeType &videomode);
+ uint32_t getVideoModeSupported(std::list<vp1394TwoVideoModeType> &videomodes);
void getWidth(unsigned int &width);
unsigned int getWidth();
bool isColor();
- bool isColorCodingSupported(vp1394TwoVideoModeType videomode,
- vp1394TwoColorCodingType coding);
- bool isFramerateSupported(vp1394TwoVideoModeType videomode,
- vp1394TwoFramerateType fps);
- bool isVideoModeSupported(vp1394TwoVideoModeType videomode) ;
+ bool isColorCodingSupported(vp1394TwoVideoModeType videomode, vp1394TwoColorCodingType coding);
+ bool isFramerateSupported(vp1394TwoVideoModeType videomode, vp1394TwoFramerateType fps);
+ bool isVideoModeSupported(vp1394TwoVideoModeType videomode);
bool isVideoModeFormat7(vp1394TwoVideoModeType videomode);
void open(vpImage<unsigned char> &I);
void open(vpImage<vpRGBa> &I);
- vp1394TwoGrabber & operator>>(vpImage<unsigned char> &I);
- vp1394TwoGrabber & operator>>(vpImage<vpRGBa> &I);
+ vp1394TwoGrabber &operator>>(vpImage<unsigned char> &I);
+ vp1394TwoGrabber &operator>>(vpImage<vpRGBa> &I);
void printCameraInfo();
@@ -424,8 +421,7 @@ public:
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);
@@ -439,15 +435,15 @@ public:
static std::string videoMode2string(vp1394TwoVideoModeType videomode);
- private:
+private:
void open();
- void initialize(bool reset);
+ void initialize(bool reset);
void setCapture(dc1394switch_t _switch);
void setTransmission(dc1394switch_t _switch);
inline void updateDataCamToStruct();
inline void updateDataStructToCam();
- private:
+private:
dc1394camera_t *camera, **cameras;
unsigned int num_cameras;
unsigned int camera_id;
@@ -455,19 +451,17 @@ public:
bool verbose;
bool *camIsOpen;
unsigned int num_buffers;
-
+
/* parameters for the cameras */
- bool* isDataModified;
- dc1394feature_mode_t* initialShutterMode;
- vpDc1394TwoCameraParametersData* dataCam;
+ bool *isDataModified;
+ dc1394feature_mode_t *initialShutterMode;
+ vpDc1394TwoCameraParametersData *dataCam;
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE
- dc1394_t * d;
- dc1394camera_list_t * list;
+ dc1394_t *d;
+ dc1394camera_list_t *list;
#endif
};
-
#endif
#endif
-
diff --git a/modules/sensor/include/visp3/sensor/vpComedi.h b/modules/sensor/include/visp3/sensor/vpComedi.h
index ce963e2..b49b98a 100644
--- a/modules/sensor/include/visp3/sensor/vpComedi.h
+++ b/modules/sensor/include/visp3/sensor/vpComedi.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,8 +53,9 @@
\ingroup group_sensor_ft
- Interface for data acquisition devices supported by Comedi. Comedi is a linux control and
- measurement device interface. For more information see http://www.comedi.org.
+ Interface for data acquisition devices supported by Comedi. Comedi is a
+linux control and measurement device interface. For more information see
+http://www.comedi.org.
This class was tested with ATI Gamma 65-SI FT sensor connected to a
National Instrument NI DAQmx PCI-6220 board.
@@ -62,11 +64,12 @@
\code
Could not open device /dev/comedi0
\endcode
- you may set up udev permissions for Comedi device files. Once Comedi is installed you also
- need to make sure that the user has appropriate permissions for accessing the Comedi
- device files. That is you need to be able to have read and write access to the /dev/comedi* files.
- One elegant way to achieve this to create a new group and tell udev to add the Comedi device
- files to this group. To this end:
+ you may set up udev permissions for Comedi device files. Once Comedi is
+installed you also need to make sure that the user has appropriate permissions
+for accessing the Comedi device files. That is you need to be able to have
+read and write access to the /dev/comedi* files. One elegant way to achieve
+this to create a new group and tell udev to add the Comedi device files to
+this group. To this end:
1. Login as root
2. Create a new group "iocard":
\code
@@ -74,16 +77,18 @@ $ addgroup --system iocard
\endcode
3. Add udev rules to the /etc/udev/rules.d directory:
\code
-$ echo 'KERNEL=="comedi*", MODE="0660", GROUP="iocard"' > /etc/udev/rules.d/95-comedi.rules
- \endcode
+$ echo 'KERNEL=="comedi*", MODE="0660", GROUP="iocard"' >
+/etc/udev/rules.d/95-comedi.rules \endcode
4. Add users to the "iocard" group:
\code
$ adduser <username> iocard
\endcode
5. Reboot
- The following example shows how to run an synchronous data acquisition at 500 Hz, calling getPhyData() each 2 ms:
- \code
+ The following example shows how to run an synchronous data acquisition at
+500 Hz, calling getPhyData() each 2 ms:
+
+\code
#include <visp3/sensor/vpComedi.h>
int main()
@@ -111,68 +116,48 @@ public:
//@{
void close();
//! Get native Comedi handler.
- comedi_t * getHandler() const
- {
- return m_handler;
- }
- //! Get max data per channel. The returned vector is of dimension the number of channels.
- std::vector<lsampl_t> getMaxData() const
- {
- return m_maxdata;
- }
+ comedi_t *getHandler() const { return m_handler; }
+ //! Get max data per channel. The returned vector is of dimension the number
+ //! of channels.
+ std::vector<lsampl_t> getMaxData() const { return m_maxdata; }
//! Get number of channels.
- unsigned int getNChannel() const
- {
- return m_nchannel;
- }
+ unsigned int getNChannel() const { return m_nchannel; }
vpColVector getPhyData() const;
std::string getPhyDataUnits() const;
- //! Get data range information per channel. The returned vector is of dimension the number of channels.
- std::vector<comedi_range *> getRangeInfo() const
- {
- return m_range_info;
- }
+ //! Get data range information per channel. The returned vector is of
+ //! dimension the number of channels.
+ std::vector<comedi_range *> getRangeInfo() const { return m_range_info; }
//! Get comedi subdevice.
- unsigned int getSubDevice() const {
- return m_subdevice;
- }
+ unsigned int getSubDevice() const { return m_subdevice; }
void open();
/*!
Set analog reference type.
- \param aref : The aref parameter specifies an analog reference to use: AREF_GROUND, AREF_COMMON, AREF_DIFF,
- or AREF_OTHER.
+ \param aref : The aref parameter specifies an analog reference to use:
+ AREF_GROUND, AREF_COMMON, AREF_DIFF, or AREF_OTHER.
*/
- void setAnalogReference(const unsigned int &aref) {
- m_aref = aref;
- }
+ void setAnalogReference(const unsigned int &aref) { m_aref = aref; }
/*!
- Number of channels to read from sensor. For a 6-dim force/torque sensor use 6.
+ 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;
- }
+ void setDevice(const std::string &device) { m_device = device; }
/*!
- Set the range parameter that is the zero-based index of one of the gain ranges supported by the channel.
- This is a number from 0 to N-1 where N is the number of ranges supported by the channel.
+ Set the range parameter that is the zero-based index of one of the gain
+ ranges supported by the channel. This is a number from 0 to N-1 where N
+ is the number of ranges supported by the channel.
*/
- void setRange(const unsigned int &range) {
- m_range = range;
- }
+ 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:
@@ -181,15 +166,15 @@ protected:
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::vector<comedi_range *>m_range_info; /*!< Range information */
- std::vector<lsampl_t> m_maxdata; /*!< Max data value */
- std::vector<unsigned int> m_chanlist; /*!< Channel list */
+ 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<comedi_range *> m_range_info; /*!< Range information */
+ std::vector<lsampl_t> m_maxdata; /*!< Max data value */
+ std::vector<unsigned int> m_chanlist; /*!< Channel list */
//@}
};
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h
index 5e863bd..4f821e0 100644
--- a/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowDevice.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,39 +42,39 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
#include <atlbase.h>
+#include <dshow.h>
#include <iostream>
#include <string>
-#include <dshow.h>
class VISP_EXPORT vpDirectShowDevice
{
- std::string name; //the device's name
- std::string desc; //the device's description
- std::string devPath; //the device's device path (unique)
+ std::string name; // the device's name
+ std::string desc; // the device's description
+ std::string devPath; // the device's device path (unique)
- bool inUse; //true if the device is already used by a grabber
+ bool inUse; // true if the device is already used by a grabber
public:
- vpDirectShowDevice() : inUse(false){}
- vpDirectShowDevice(const CComPtr<IMoniker>& moniker) : inUse(false){ init(moniker); }
+ vpDirectShowDevice() : inUse(false) {}
+ explicit vpDirectShowDevice(const CComPtr<IMoniker> &moniker) : inUse(false) { init(moniker); }
- bool init(const CComPtr<IMoniker>& moniker);
+ bool init(const CComPtr<IMoniker> &moniker);
- bool getState(){ return inUse; }
- void setInUse(){ inUse=true; }
- void resetInUse() { inUse=false; }
+ bool getState() { return inUse; }
+ void setInUse() { inUse = true; }
+ void resetInUse() { inUse = false; }
- std::string& getName(){ return name; }
- std::string& getDesc(){ return desc; }
- std::string& getDevPath(){ return devPath; }
+ std::string &getName() { return name; }
+ std::string &getDesc() { return desc; }
+ std::string &getDevPath() { return devPath; }
- bool operator==(vpDirectShowDevice& dev);
+ 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 2f5ec3d..583fb2a 100644
--- a/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,16 +37,15 @@
*
*****************************************************************************/
-
#ifndef vpDirectShowGrabber_hh
#define vpDirectShowGrabber_hh
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
+#include <visp3/core/vpFrameGrabber.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpFrameGrabber.h>
class vpDirectShowGrabberImpl;
@@ -62,57 +62,53 @@ class vpDirectShowGrabberImpl;
class VISP_EXPORT vpDirectShowGrabber : public vpFrameGrabber
{
- vpDirectShowGrabberImpl * grabber;
-
-
- public:
-
- unsigned int getHeight();
- unsigned int getWidth();
-
- vpDirectShowGrabber();
- virtual ~vpDirectShowGrabber();
+ vpDirectShowGrabberImpl *grabber;
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
+public:
+ unsigned int getHeight();
+ unsigned int getWidth();
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
+ vpDirectShowGrabber();
+ virtual ~vpDirectShowGrabber();
- void close();
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
- //get the number of capture devices
- unsigned int getDeviceNumber();
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
- //change the capture device
- bool setDevice(unsigned int id);
+ void close();
- //displays a list of available devices
- void displayDevices();
+ // get the number of capture devices
+ unsigned int getDeviceNumber();
- //set image size
- bool setImageSize(unsigned int width,unsigned int height);
+ // change the capture device
+ bool setDevice(unsigned int id);
- //set source framerate
- bool setFramerate(double framerate);
+ // displays a list of available devices
+ void displayDevices();
+ // set image size
+ bool setImageSize(unsigned int width, unsigned int height);
- //set capture format
- bool setFormat(unsigned int width,unsigned int height, double framerate);
+ // set source framerate
+ bool setFramerate(double framerate);
- //get capture format
- void getFormat(unsigned int &width,unsigned int &height, double &framerate);
+ // set capture format
+ bool setFormat(unsigned int width, unsigned int height, double framerate);
- //Get the available capture formats
- bool getStreamCapabilities();
+ // get capture format
+ void getFormat(unsigned int &width, unsigned int &height, double &framerate);
- //Set capture MediaType
- bool setMediaType(int mediaTypeID);
+ // Get the available capture formats
+ bool getStreamCapabilities();
- //Get current capture MediaType
- int getMediaType();
+ // Set capture MediaType
+ bool setMediaType(int mediaTypeID);
+ // Get current capture MediaType
+ int getMediaType();
};
#endif
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h
index 61abf43..27efe7a 100644
--- a/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowGrabberImpl.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,178 +43,174 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
#include <atlbase.h>
-#include <qedit.h>
#include <dshow.h>
+#include <qedit.h>
#include <visp3/core/vpFrameGrabber.h>
#include <visp3/core/vpFrameGrabberException.h>
-#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
-#include <visp3/sensor/vpDirectShowDevice.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/sensor/vpDirectShowDevice.h>
+#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
/*!
- \class vpDirectShowGrabberImpl
- \brief class for windows direct show devices - implementation
+ \class vpDirectShowGrabberImpl
+ \brief class for windows direct show devices - implementation
- This class uses CComPtr which is the best way to be sure that all the interfaces
- are released sooner or later.
+ This class uses CComPtr which is the best way to be sure that all the
+ interfaces are released sooner or later.
*/
class VISP_EXPORT vpDirectShowGrabberImpl : public vpFrameGrabber
{
- static const int MAX_DELAY = 10000;
- static const int MAX_DEVICES = 10;
+ static const int MAX_DELAY = 10000;
+ static const int MAX_DEVICES = 10;
-
- public:
-
- /*!
- Enumeration of video subtypes.
-*/
-/*
- typedef enum {
- //Known RGB formats
- vpMEDIASUBTYPE_ARGB32 = MEDIASUBTYPE_ARGB32,
- vpMEDIASUBTYPE_RGB32 = MEDIASUBTYPE_RGB32,
- vpMEDIASUBTYPE_RGB24 = MEDIASUBTYPE_RGB24,
- vpMEDIASUBTYPE_RGB555 = MEDIASUBTYPE_RGB555,
- vpMEDIASUBTYPE_RGB565 = MEDIASUBTYPE_RGB565,
- vpMEDIASUBTYPE_RGB8 = MEDIASUBTYPE_RGB8,
- vpMEDIASUBTYPE_RGB4 = MEDIASUBTYPE_RGB4,
- vpMEDIASUBTYPE_RGB1 = MEDIASUBTYPE_RGB1,
- //Known YUV formats
- vpMEDIASUBTYPE_AYUV = MEDIASUBTYPE_AYUV,
- vpMEDIASUBTYPE_UYVY = MEDIASUBTYPE_UYVY,
- vpMEDIASUBTYPE_Y411 = MEDIASUBTYPE_Y411,
- vpMEDIASUBTYPE_Y41P = MEDIASUBTYPE_Y41P,
- vpMEDIASUBTYPE_Y211 = MEDIASUBTYPE_Y211,
- vpMEDIASUBTYPE_YUY2 = MEDIASUBTYPE_YUY2,
- vpMEDIASUBTYPE_YVYU = MEDIASUBTYPE_YVYU,
- vpMEDIASUBTYPE_YUYV = MEDIASUBTYPE_YUYV,
- vpMEDIASUBTYPE_IF09 = MEDIASUBTYPE_IF09,
- vpMEDIASUBTYPE_IYUV = MEDIASUBTYPE_IYUV,
- vpMEDIASUBTYPE_YV12 = MEDIASUBTYPE_YV12,
- vpMEDIASUBTYPE_YVU9 = MEDIASUBTYPE_YVU9
- } vpDirectShowMediaSubtype;
+public:
+ /*!
+Enumeration of video subtypes.
*/
+ /*
+ typedef enum {
+ //Known RGB formats
+ vpMEDIASUBTYPE_ARGB32 = MEDIASUBTYPE_ARGB32,
+ vpMEDIASUBTYPE_RGB32 = MEDIASUBTYPE_RGB32,
+ vpMEDIASUBTYPE_RGB24 = MEDIASUBTYPE_RGB24,
+ vpMEDIASUBTYPE_RGB555 = MEDIASUBTYPE_RGB555,
+ vpMEDIASUBTYPE_RGB565 = MEDIASUBTYPE_RGB565,
+ vpMEDIASUBTYPE_RGB8 = MEDIASUBTYPE_RGB8,
+ vpMEDIASUBTYPE_RGB4 = MEDIASUBTYPE_RGB4,
+ vpMEDIASUBTYPE_RGB1 = MEDIASUBTYPE_RGB1,
+ //Known YUV formats
+ vpMEDIASUBTYPE_AYUV = MEDIASUBTYPE_AYUV,
+ vpMEDIASUBTYPE_UYVY = MEDIASUBTYPE_UYVY,
+ vpMEDIASUBTYPE_Y411 = MEDIASUBTYPE_Y411,
+ vpMEDIASUBTYPE_Y41P = MEDIASUBTYPE_Y41P,
+ vpMEDIASUBTYPE_Y211 = MEDIASUBTYPE_Y211,
+ vpMEDIASUBTYPE_YUY2 = MEDIASUBTYPE_YUY2,
+ vpMEDIASUBTYPE_YVYU = MEDIASUBTYPE_YVYU,
+ vpMEDIASUBTYPE_YUYV = MEDIASUBTYPE_YUYV,
+ vpMEDIASUBTYPE_IF09 = MEDIASUBTYPE_IF09,
+ vpMEDIASUBTYPE_IYUV = MEDIASUBTYPE_IYUV,
+ vpMEDIASUBTYPE_YV12 = MEDIASUBTYPE_YV12,
+ vpMEDIASUBTYPE_YVU9 = MEDIASUBTYPE_YVU9
+ } vpDirectShowMediaSubtype;
+ */
- vpDirectShowGrabberImpl();
- virtual ~vpDirectShowGrabberImpl();
-
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
-
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
+ vpDirectShowGrabberImpl();
+ virtual ~vpDirectShowGrabberImpl();
- void close();
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
- /*!
- Gets the number of capture devices
- */
- unsigned int getDeviceNumber() {return nbDevices;}
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
- //change the capture device
- bool setDevice(unsigned int id);
+ void close();
- //displays a list of available devices
- void displayDevices();
+ /*!
+ Gets the number of capture devices
+ */
+ unsigned int getDeviceNumber() { return nbDevices; }
- //set image size
- bool setImageSize(unsigned int width,unsigned int height);
+ // change the capture device
+ bool setDevice(unsigned int id);
- //set capture framerate
- bool setFramerate(double framerate);
+ // displays a list of available devices
+ void displayDevices();
- //set capture format
- bool setFormat(unsigned int width,unsigned int height, double framerate);
+ // set image size
+ bool setImageSize(unsigned int width, unsigned int height);
- //get capture format
- void getFormat(unsigned int &width,unsigned int &height, double &framerate);
+ // set capture framerate
+ bool setFramerate(double framerate);
- //set capture MediaType
- bool setMediaType(int mediaTypeID);
+ // set capture format
+ bool setFormat(unsigned int width, unsigned int height, double framerate);
- //get current capture MediaType
- int getMediaType();
+ // get capture format
+ void getFormat(unsigned int &width, unsigned int &height, double &framerate);
- //Get the available capture formats
- bool getStreamCapabilities();
+ // set capture MediaType
+ bool setMediaType(int mediaTypeID);
+ // get current capture MediaType
+ int getMediaType();
- private:
+ // Get the available capture formats
+ bool getStreamCapabilities();
- CComPtr<IGraphBuilder> pGraph; //our DS filter graph
+private:
+ CComPtr<IGraphBuilder> pGraph; // our DS filter graph
- CComPtr<ICaptureGraphBuilder2> pBuild; //the interface to the capture graph builder
- //used to build the filter graph
+ CComPtr<ICaptureGraphBuilder2> pBuild; // the interface to the capture graph builder
+ // used to build the filter graph
- CComPtr<IBaseFilter> pCapSource; //the capture source filter
+ CComPtr<IBaseFilter> pCapSource; // the capture source filter
- CComPtr<ISampleGrabber> pGrabberI; //the sample grabber's interface and filter
- CComPtr<IBaseFilter> pGrabberFilter;
+ CComPtr<ISampleGrabber> pGrabberI; // the sample grabber's interface and filter
+ CComPtr<IBaseFilter> pGrabberFilter;
- CComPtr<IMediaControl> pControl; //The DS filter graph control interface
- CComPtr<IMediaEvent> pEvent; //The DS filter graph event interface
+ CComPtr<IMediaControl> pControl; // The DS filter graph control interface
+ CComPtr<IMediaEvent> pEvent; // The DS filter graph event interface
- vpDirectShowSampleGrabberI sgCB; //Interface used to implement the frame grabber callback
+ vpDirectShowSampleGrabberI sgCB; // Interface used to implement the frame grabber callback
- HRESULT hr; //contains the result of the last operation
+ 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
+ static unsigned int nbDevices; // the number of available devices
+ int currentDevice; // the number of the current device
- // flag to manage CoInitialize() and CoUnInitialze()
- bool initCo ;
- //setup the directshow filtergraph with the first available device
- bool initDirectShow();
+ // flag to manage CoInitialize() and CoUnInitialze()
+ bool initCo;
+ // setup the directshow filtergraph with the first available device
+ bool initDirectShow();
- //enumerates the different video inputs
- bool enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum);
+ // enumerates the different video inputs
+ bool enumerate(CComPtr<IEnumMoniker> &ppVideoInputEnum);
- //selects a random video input from the enumeration and returns the associated filter
- bool selectRandomSource(CComPtr<IEnumMoniker>& ppVideoInputEnum, CComPtr<IBaseFilter>& pCapSource);
+ // selects a random video input from the enumeration and returns the
+ // associated filter
+ bool selectRandomSource(CComPtr<IEnumMoniker> &ppVideoInputEnum, CComPtr<IBaseFilter> &pCapSource);
- //creates the filter graph
- bool createGraph();
+ // creates the filter graph
+ bool createGraph();
- //creates the sample grabber
- bool createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter);
+ // creates the sample grabber
+ bool createSampleGrabber(CComPtr<IBaseFilter> &ppGrabberFilter);
- //checks the capture filter's media type and sets flags as needed
- bool checkSourceType(CComPtr<IPin>& pCapSourcePin);
+ // checks the capture filter's media type and sets flags as needed
+ bool checkSourceType(CComPtr<IPin> &pCapSourcePin);
- //connects the filters as needed
- bool connectSourceToGrabber(CComPtr<IBaseFilter>& pCapSource, CComPtr<IBaseFilter>& pGrabberFilter);
+ // connects the filters as needed
+ bool connectSourceToGrabber(CComPtr<IBaseFilter> &pCapSource, CComPtr<IBaseFilter> &pGrabberFilter);
- //used to convert HRESULT-associated error message to a string
- void HRtoStr(std::string str);
+ // used to convert HRESULT-associated error message to a string
+ void HRtoStr(std::string str);
- //create the list of the available devices
- bool createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum);
+ // create the list of the available devices
+ bool createDeviceList(CComPtr<IEnumMoniker> &ppVideoInputEnum);
- //get the n-th device if it is available
- bool getDevice(unsigned int n, CComPtr<IBaseFilter>& ppCapSource);
+ // get the n-th device if it is available
+ bool getDevice(unsigned int n, CComPtr<IBaseFilter> &ppCapSource);
- //get the first available device if any
- unsigned int getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice);
+ // get the first available device if any
+ unsigned int getFirstUnusedDevice(CComPtr<IBaseFilter> &ppDevice);
- //removes all the filters in the graph
- bool removeAll();
+ // removes all the filters in the graph
+ bool removeAll();
- //Deletes an allocated AM_MEDIA_TYPE structure, including the format block
- void MyDeleteMediaType(AM_MEDIA_TYPE *pmt);
-
- //Frees the format block in an AM_MEDIA_TYPE structure
- void MyFreeMediaType(AM_MEDIA_TYPE& mt);
+ // Deletes an allocated AM_MEDIA_TYPE structure, including the format block
+ void MyDeleteMediaType(AM_MEDIA_TYPE *pmt);
+ // Frees the format block in an AM_MEDIA_TYPE structure
+ void MyFreeMediaType(AM_MEDIA_TYPE &mt);
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h
index d478ee8..50fa81e 100644
--- a/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h
+++ b/modules/sensor/include/visp3/sensor/vpDirectShowSampleGrabberI.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,60 +42,59 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
+#include <dshow.h>
#include <qedit.h>
#include <stdio.h>
-#include <dshow.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
/*!
- This class is needed in order to implement a callback function
- associated with the grabber
+ This class is needed in order to implement a callback function
+ associated with the grabber
*/
class VISP_EXPORT vpDirectShowSampleGrabberI : public ISampleGrabberCB
{
- vpDirectShowSampleGrabberI();
- virtual ~vpDirectShowSampleGrabberI();
-
- //needed by the interface
- STDMETHODIMP_(ULONG) AddRef() { return 1; }
- STDMETHODIMP_(ULONG) Release() { return 2; }
-
- STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject);
-
- //not implemented
- STDMETHODIMP SampleCB(double Time, IMediaSample *pSample)
- {
- return E_NOTIMPL;
- }
-
- //our callback function
- STDMETHODIMP BufferCB(double Time, BYTE *pBuffer, long BufferLen);
+ vpDirectShowSampleGrabberI();
+ virtual ~vpDirectShowSampleGrabberI();
+
+ // needed by the interface
+ STDMETHODIMP_(ULONG) AddRef() { return 1; }
+ STDMETHODIMP_(ULONG) Release() { return 2; }
+
+ STDMETHODIMP QueryInterface(REFIID riid, void **ppvObject);
+
+ // not implemented
+ STDMETHODIMP SampleCB(double Time, IMediaSample *pSample) { return E_NOTIMPL; }
+
+ // our callback function
+ STDMETHODIMP BufferCB(double Time, BYTE *pBuffer, long BufferLen);
private:
- //the currently connected media type
- AM_MEDIA_TYPE connectedMediaType;
+ // the currently connected media type
+ AM_MEDIA_TYPE connectedMediaType;
- //true if the source media type is not a standard one
- bool specialMediaType;
- //true if the image needs to be flipped (only for special media types)
- bool invertedSource;
+ // true if the source media type is not a standard one
+ bool specialMediaType;
+ // true if the image needs to be flipped (only for special media types)
+ bool invertedSource;
- //booleans used to signal a demand from acquire
- bool acqGrayDemand;
- bool acqRGBaDemand;
+ // booleans used to signal a demand from acquire
+ bool acqGrayDemand;
+ bool acqRGBaDemand;
- //pointer on the image to fill during the next callback if there has been a demand
- vpImage<vpRGBa> * rgbaIm;
- vpImage<unsigned char> * grayIm;
+ // pointer on the image to fill during the next callback if there has been a
+ // demand
+ vpImage<vpRGBa> *rgbaIm;
+ vpImage<unsigned char> *grayIm;
- //semaphore used to synchronize the productor (callback) and the consumer (acquire)
- HANDLE copySem;
+ // semaphore used to synchronize the productor (callback) and the consumer
+ // (acquire)
+ HANDLE copySem;
- friend class vpDirectShowGrabberImpl;
+ friend class vpDirectShowGrabberImpl;
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h b/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h
index bdc9762..091b0ba 100644
--- a/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h
+++ b/modules/sensor/include/visp3/sensor/vpFlyCaptureGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -27,9 +28,9 @@
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
- * Description: Class which enables to project an image in the 3D space
+ * Description: Class which enables to project an image in the 3D space
* and get the view of a virtual camera.
- *
+ *
* Authors:
* Fabien Spindler
*
@@ -48,7 +49,8 @@
/*!
\file vpFlyCaptureGrabber.h
- \brief Wrapper over PointGrey FlyCapture SDK to capture images from PointGrey cameras.
+ \brief Wrapper over PointGrey FlyCapture SDK to capture images from
+ PointGrey cameras.
*/
/*!
\class vpFlyCaptureGrabber
@@ -56,14 +58,15 @@
Allows to grab images from a PointGrey camera using FlyCapture SDK.
- To use this class install first FlyCapture SDK https://www.ptgrey.com/flycapture-sdk.
- Installation instructions are provide here https://visp.inria.fr/3rd_flycapture.
- \note To install FlyCapture SDK on linux follow https://www.ptgrey.com/tan/10548.
- \note For specific details about using FlyCapture and Linux with a USB 3.0 camera,
- see http://www.ptgrey.com/KB/10685.
+ To use this class install first FlyCapture SDK
+https://www.ptgrey.com/flycapture-sdk. Installation instructions are provide
+here https://visp.inria.fr/3rd_flycapture. \note To install FlyCapture SDK on
+linux follow https://www.ptgrey.com/tan/10548. \note For specific details
+about using FlyCapture and Linux with a USB 3.0 camera, see
+http://www.ptgrey.com/KB/10685.
- Once installed configure ViSP using cmake to detect FlyCapture SDK and build ViSP to include
- FlyCapture SDK support.
+ Once installed configure ViSP using cmake to detect FlyCapture SDK and build
+ViSP to include FlyCapture SDK support.
This class was tested under Ubuntu and Windows with the following cameras:
- Flea3 USB 3.0 cameras (FL3-U3-32S2M-CS, FL3-U3-13E4C-C)
@@ -100,11 +103,11 @@ int main()
}
\endcode
- If more than one camera is detected, you can use setCamera(const unsigned int &) to
- select the camera of interest.
+ If more than one camera is detected, you can use setCamera(const unsigned
+int &) to select the camera of interest.
- It is also possible to capture images from multiple cameras. The following example
- shows how to capture simultaneously images from multiple cameras.
+ It is also possible to capture images from multiple cameras. The following
+example shows how to capture simultaneously images from multiple cameras.
\code
#include <visp3/core/vpImage.h>
@@ -157,12 +160,12 @@ public:
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;
- };
+ unsigned int getCameraIndex() const { return m_index; };
bool getCameraPower();
static unsigned int getCameraSerial(unsigned int index);
float getExposure();
@@ -174,35 +177,29 @@ public:
bool isCameraPowerAvailable();
//! Return true if the camera is connected.
- bool isConnected() const {
- return m_connected;
- }
+ bool isConnected() const { return m_connected; }
//! Return true if the camera capture is started.
- bool isCaptureStarted() const {
- return m_capture;
- }
+ bool isCaptureStarted() const { return m_capture; }
bool isFormat7Supported(FlyCapture2::Mode format7_mode);
bool isVideoModeAndFrameRateSupported(FlyCapture2::VideoMode video_mode, FlyCapture2::FrameRate frame_rate);
void open(vpImage<unsigned char> &I);
void open(vpImage<vpRGBa> &I);
- vpFlyCaptureGrabber & operator>>(vpImage<unsigned char> &I);
- vpFlyCaptureGrabber & operator>>(vpImage<vpRGBa> &I);
+ vpFlyCaptureGrabber &operator>>(vpImage<unsigned char> &I);
+ vpFlyCaptureGrabber &operator>>(vpImage<vpRGBa> &I);
- float setBrightness(bool brightness_auto, float brightness_value=0);
+ float setBrightness(bool brightness_auto, float brightness_value = 0);
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 setGain(bool gain_auto, float gain_value=0);
- void setFormat7VideoMode(FlyCapture2::Mode format7_mode,
- FlyCapture2::PixelFormat pixel_format,
- unsigned int width, unsigned int height);
+ 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);
float setFrameRate(float frame_rate);
- 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);
+ 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 startCapture();
void stopCapture();
@@ -216,18 +213,17 @@ protected:
FlyCapture2::Property getProperty(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,
- PropertyValue prop_value=ABS_VALUE);
+ void setProperty(const FlyCapture2::PropertyType &prop_type, bool on, bool auto_on, float value,
+ PropertyValue prop_value = ABS_VALUE);
protected:
- FlyCapture2::Camera m_camera; //!< Pointer to each camera
- FlyCapture2::PGRGuid m_guid; //!< Active camera guid
- unsigned int m_index; //!< Active camera index
- unsigned int m_numCameras; //!< Number of connected cameras
+ FlyCapture2::Camera m_camera; //!< Pointer to each camera
+ FlyCapture2::PGRGuid m_guid; //!< Active camera guid
+ unsigned int m_index; //!< Active camera index
+ unsigned int m_numCameras; //!< Number of connected cameras
FlyCapture2::Image m_rawImage; //!< Image buffer
- bool m_connected; //!< true if camera connected
- bool m_capture; //!< true is capture started
+ bool m_connected; //!< true if camera connected
+ bool m_capture; //!< true is capture started
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h b/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h
index 3067cf5..b3c06d6 100644
--- a/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h
+++ b/modules/sensor/include/visp3/sensor/vpForceTorqueAtiSensor.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,19 +52,22 @@
\ingroup group_sensor_ft
- Interface for ATI force/torque sensor. This class works only under linux-like OS.
- It requires Comedi 3rd party. Installation instructions are provided here https://visp.inria.fr/3rd_comedi.
+ Interface for ATI force/torque sensor. This class works only under
+linux-like OS. It requires Comedi 3rd party. Installation instructions are
+provided here https://visp.inria.fr/3rd_comedi.
- Comedi is the linux control and measurement device interface. For more information see http://www.comedi.org.
+ Comedi is the linux control and measurement device interface. For more
+information see http://www.comedi.org.
This class was tested with ATI Gamma 65-SI FT sensor connected to a
National Instrument NI DAQmx PCI-6220 board.
- Synchronous F/T data acquisition is performed using getForceTorque(). The call to the function
- blocks until the whole acquisition has finished.
+ Synchronous F/T data acquisition is performed using getForceTorque(). The
+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
+ The following example shows how to get single measures from an ATI F/T
+device each 10 ms (100 Hz).
+\code
#include <visp3/core/vpTime.h>
#include <visp3/sensor/vpForceTorqueAtiSensor.h>
@@ -93,12 +97,10 @@ public:
void close();
/*!
- Return the calibration file location specified using setCalibrationFile().
- \sa setCalibrationFile()
+ Return the calibration file location specified using
+ setCalibrationFile(). \sa setCalibrationFile()
*/
- std::string getCalibrationFile() const {
- return m_calibfile;
- }
+ std::string getCalibrationFile() const { return m_calibfile; }
vpColVector getForceTorque() const;
vpColVector getForceTorqueAsync() const;
std::string getForceUnits() const;
@@ -106,10 +108,10 @@ public:
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
diff --git a/modules/sensor/include/visp3/sensor/vpKinect.h b/modules/sensor/include/visp3/sensor/vpKinect.h
index 45eafd5..f062e58 100644
--- a/modules/sensor/include/visp3/sensor/vpKinect.h
+++ b/modules/sensor/include/visp3/sensor/vpKinect.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,23 +37,22 @@
*
*****************************************************************************/
-
#ifndef __VP_KINECT__
#define __VP_KINECT__
#include <visp3/core/vpConfig.h>
-// Note that libfreenect needs libusb-1.0 and libpthread
+// Note that libfreenect needs libusb-1.0 and libpthread
#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
#include <iostream>
#include <libfreenect.hpp>
-#include <visp3/core/vpMutex.h> // need pthread
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpMutex.h> // need pthread
+#include <visp3/core/vpPixelMeterConversion.h>
/*!
@@ -62,8 +62,8 @@
\brief Driver for the Kinect-1 device.
- To be enabled this class requires libfreenect 3rd party. Installation instructions are provided here
- https://visp.inria.fr/3rd_freenect.
+ To be enabled this class requires libfreenect 3rd party. Installation
+instructions are provided here https://visp.inria.fr/3rd_freenect.
The following example shows how to use this class to acquire data
(depth map and color image) from a Kinect.
@@ -109,17 +109,17 @@ int main() {
*/
class VISP_EXPORT vpKinect : public Freenect::FreenectDevice
{
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpKinect(const vpKinect &); // Not implemented!
-// vpKinect &operator=(const vpKinect &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpKinect(const vpKinect &); // Not implemented!
+ // vpKinect &operator=(const vpKinect &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- /*!
+ /*!
Depth map resolution.
*/
typedef enum {
@@ -133,54 +133,48 @@ public:
void start(vpKinect::vpDMResolution res = DMAP_LOW_RES);
void stop();
- bool getDepthMap(vpImage<float>& map);
- bool getDepthMap(vpImage<float>& map, vpImage<unsigned char>& Imap);
- bool getRGB(vpImage<vpRGBa>& IRGB);
+ bool getDepthMap(vpImage<float> &map);
+ bool getDepthMap(vpImage<float> &map, vpImage<unsigned char> &Imap);
+ bool getRGB(vpImage<vpRGBa> &IRGB);
+ inline void getIRCamParameters(vpCameraParameters &cam) const { cam = IRcam; }
+ inline void getRGBCamParameters(vpCameraParameters &cam) const { cam = RGBcam; }
+ inline void setIRCamParameters(const vpCameraParameters &cam) { IRcam = cam; }
+ inline void setRGBCamParameters(const vpCameraParameters &cam) { RGBcam = cam; }
- inline void getIRCamParameters(vpCameraParameters &cam) const {
- cam = IRcam;
- }
- inline void getRGBCamParameters(vpCameraParameters &cam) const {
- cam = RGBcam;
- }
- inline void setIRCamParameters(const vpCameraParameters &cam) {
- IRcam = cam;
- }
- inline void setRGBCamParameters(const vpCameraParameters &cam) {
- RGBcam = cam;
- }
-
- void warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped);//warp the RGB image into the Depth camera frame
+ void warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth,
+ vpImage<vpRGBa> &IrgbWarped); // warp the RGB image into
+ // the Depth camera frame
- private:
- //!Instantiation of Freenect virtual functions
+private:
+ //! Instantiation of Freenect virtual functions
// Do not call directly even in child
- void VideoCallback(void* rgb, uint32_t timestamp);
+ void VideoCallback(void *rgb, uint32_t timestamp);
// Do not call directly even in child
- void DepthCallback(void* depth, uint32_t timestamp);
+ void DepthCallback(void *depth, uint32_t timestamp);
- private:
+private:
vpMutex m_rgb_mutex;
vpMutex m_depth_mutex;
- vpCameraParameters RGBcam, IRcam;//intrinsic parameters of the two cameras
- vpHomogeneousMatrix rgbMir;//Transformation from IRcam coordinate frame to RGBcam coordinate frame.
- vpHomogeneousMatrix irMrgb;//Transformation from RGBcam coordinate frame to IRcam coordinate frame .
+ vpCameraParameters RGBcam, IRcam; // intrinsic parameters of the two cameras
+ vpHomogeneousMatrix rgbMir; // Transformation from IRcam coordinate frame to
+ // RGBcam coordinate frame.
+ vpHomogeneousMatrix irMrgb; // Transformation from RGBcam coordinate frame
+ // to IRcam coordinate frame .
vpDMResolution DMres;
- unsigned int hd;//height of the depth map
- unsigned int wd;//width of the depth map
+ unsigned int hd; // height of the depth map
+ unsigned int wd; // width of the depth map
- //Access protected by a mutex:
+ // Access protected by a mutex:
vpImage<float> dmap;
vpImage<vpRGBa> IRGB;
bool m_new_rgb_frame;
bool m_new_depth_map;
bool m_new_depth_image;
- unsigned int height;//height of the rgb image
- unsigned int width;//width of the rgb image
-
+ unsigned int height; // height of the rgb image
+ unsigned int width; // width of the rgb image
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpLaserScan.h b/modules/sensor/include/visp3/sensor/vpLaserScan.h
index 296ec4b..19d6395 100644
--- a/modules/sensor/include/visp3/sensor/vpLaserScan.h
+++ b/modules/sensor/include/visp3/sensor/vpLaserScan.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -62,17 +63,18 @@
*/
class VISP_EXPORT vpLaserScan
{
- public:
- /*! Default constructor that initialize all the internal variable to zero. */
- vpLaserScan()
- : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
- numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
+public:
+ /*! Default constructor that initialize all the internal variable to zero.
+ */
+ vpLaserScan()
+ : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0), numSteps(0), startAngle(0), stopAngle(0),
+ numPoints(0)
{
}
/*! Copy constructor. */
vpLaserScan(const vpLaserScan &scan)
- : listScanPoints(), startTimestamp(0), endTimestamp(0), measurementId(0),
- numSteps(0), startAngle(0), stopAngle(0), numPoints(0)
+ : listScanPoints(scan.listScanPoints), startTimestamp(0), endTimestamp(0), measurementId(0), numSteps(0),
+ startAngle(0), stopAngle(0), numPoints(0)
{
startTimestamp = scan.startTimestamp;
endTimestamp = scan.endTimestamp;
@@ -81,61 +83,36 @@ class VISP_EXPORT vpLaserScan
startAngle = scan.startAngle;
stopAngle = scan.stopAngle;
numPoints = scan.numPoints;
- listScanPoints = scan.listScanPoints;
}
/*! Default destructor that does nothing. */
- virtual ~vpLaserScan() {};
+ virtual ~vpLaserScan(){};
/*! Add the scan point at the end of the list. */
- inline void addPoint(const vpScanPoint &p) {
- listScanPoints.push_back( p );
- }
+ inline void addPoint(const vpScanPoint &p) { listScanPoints.push_back(p); }
/*! Drop the list of points. */
- inline void clear() {
- listScanPoints.clear( );
- }
+ inline void clear() { listScanPoints.clear(); }
/*! Get the list of points. */
- inline std::vector<vpScanPoint> getScanPoints() {
- return listScanPoints;
- }
+ inline std::vector<vpScanPoint> getScanPoints() { return listScanPoints; }
/*! Specifies the id of former measurements and increases with
every measurement. */
- inline void setMeasurementId(const unsigned short &id) {
- this->measurementId = id;
- }
+ 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;
- }
+ inline double getStartTimestamp() { return startTimestamp; }
/*! Return the measurement end time. */
- inline double getEndTimestamp() {
- return endTimestamp;
- }
+ inline double getEndTimestamp() { return endTimestamp; }
- private:
+private:
std::vector<vpScanPoint> listScanPoints;
double startTimestamp;
double endTimestamp;
@@ -144,7 +121,6 @@ class VISP_EXPORT vpLaserScan
short startAngle;
short stopAngle;
unsigned short numPoints;
-
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpLaserScanner.h b/modules/sensor/include/visp3/sensor/vpLaserScanner.h
index 1959caf..98f58c5 100644
--- a/modules/sensor/include/visp3/sensor/vpLaserScanner.h
+++ b/modules/sensor/include/visp3/sensor/vpLaserScanner.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
#ifndef vpLaserScanner_h
#define vpLaserScanner_h
-
#include "visp3/core/vpConfig.h"
/*!
@@ -56,30 +56,22 @@
*/
class VISP_EXPORT vpLaserScanner
{
- public:
- /*! Default constructor that initialize all the internal variable to zero. */
- vpLaserScanner() : ip("null"), port(0) {};
+public:
+ /*! Default constructor that initialize all the internal variable to zero.
+ */
+ vpLaserScanner() : ip("null"), port(0){};
/*! Copy constructor. */
- vpLaserScanner(const vpLaserScanner &scanner)
- : ip("null"), port(0)
- {
- ip = scanner.ip;
- port = scanner.port;
- };
+ vpLaserScanner(const vpLaserScanner &scanner) : ip(scanner.ip), port(scanner.port){};
/*! Default destructor that does nothing. */
- virtual ~vpLaserScanner() {};
+ virtual ~vpLaserScanner(){};
/*! Set the Ethernet address of the laser. */
- void setIpAddress(std::string ip_address) {
- this->ip = ip_address;
- };
+ void setIpAddress(std::string ip_address) { this->ip = ip_address; };
/*! Set the communication port. */
- void setPort(int com_port) {
- this->port = com_port;
- };
+ void setPort(int com_port) { this->port = com_port; };
- protected:
+protected:
std::string ip;
int port;
};
diff --git a/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h
index af35d5f..aa21eed 100644
--- a/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h
+++ b/modules/sensor/include/visp3/sensor/vpOpenCVGrabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,16 +46,16 @@
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
#if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui.hpp>
#else
-# include <highgui.h>
+#include <highgui.h>
#endif
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
/*!
@@ -64,13 +65,12 @@
\brief Class for cameras video capture using OpenCV library.
\deprecated This class is deprecated can only be used with OpenCV < 2.4.8.
-
- \note Instead of using this class, it is also possible to grab images using OpenCV library by using
- directly OpenCV cv::VideoCapture class. The following code corresponding to
- tutorial-grabber-opencv.cpp shows how to grab images in a cv::Mat structure
- and then convert OpenCV images in ViSP images.
- \include tutorial-grabber-opencv.cpp
- The one in grabOpencv.cpp gives an other example.
+
+ \note Instead of using this class, it is also possible to grab images using
+OpenCV library by using directly OpenCV cv::VideoCapture class. The following
+code corresponding to tutorial-grabber-opencv.cpp shows how to grab images in
+a cv::Mat structure and then convert OpenCV images in ViSP images. \include
+tutorial-grabber-opencv.cpp The one in grabOpencv.cpp gives an other example.
The code below shows how to grab and
display images using this class.
@@ -107,36 +107,34 @@ int main()
*/
class VISP_EXPORT vpOpenCVGrabber : public vpFrameGrabber
{
- private:
-
- CvCapture *capture;
- int DeviceType;
- bool flip;
+private:
+ CvCapture *capture;
+ int DeviceType;
+ bool flip;
- public:
+public:
+ vpOpenCVGrabber();
+ ~vpOpenCVGrabber();
- vpOpenCVGrabber();
- ~vpOpenCVGrabber();
+ void open();
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
- void open();
- void open(vpImage<unsigned char> &I);
- void open(vpImage<vpRGBa> &I);
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+ IplImage *acquire();
- void acquire(vpImage<unsigned char> &I);
- void acquire(vpImage<vpRGBa> &I);
- IplImage* acquire();
+ void close();
- void close();
+ void getFramerate(double &framerate);
+ void setFramerate(const double framerate);
- void getFramerate(double & framerate);
- void setFramerate(const double framerate);
+ void setWidth(const unsigned int width);
+ void setHeight(const unsigned int height);
- void setWidth(const unsigned int width);
- void setHeight(const unsigned int height);
-
- void setDeviceType(int type);
+ void setDeviceType(int type);
- void setFlip(bool flipType);
+ void setFlip(bool flipType);
};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpPylonFactory.h b/modules/sensor/include/visp3/sensor/vpPylonFactory.h
new file mode 100644
index 0000000..382a5d7
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpPylonFactory.h
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Factory class used to create vpPylonGrabber instances.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+#ifndef __vpPylonFactory_h_
+#define __vpPylonFactory_h_
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/sensor/vpPylonGrabber.h>
+
+#ifdef VISP_HAVE_PYLON
+
+/*!
+ \file vpPylonFactory.h
+ \brief Description: Factory class used to create vpPylonGrabber
+ instances.
+*/
+
+/*!
+ \brief Factory singleton class to create vpPylonGrabber subclass
+ instances.
+
+ \ingroup group_sensor_camera
+
+ Use vpPylonFactory::instance() to get the singleton instance. This
+ class can also help to initialize and terminate pylon runtime system.
+
+ Example code.
+ \code
+ vpPylonFactory &factory = vpPylonFactory::instance();
+ vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE);
+ \endcode
+ */
+class VISP_EXPORT vpPylonFactory
+{
+public:
+ static vpPylonFactory &instance();
+
+ /*! Device class of cameras.
+ */
+ enum DeviceClass {
+ BASLER_GIGE, //!< Basler GigE camera.
+ BASLER_USB //!< Basler USB camera.
+ };
+
+ vpPylonGrabber *createPylonGrabber(DeviceClass dev_class);
+
+private:
+ //! Default constructor.
+ vpPylonFactory(){};
+ vpPylonFactory(vpPylonFactory const &);
+ void operator=(vpPylonFactory const &);
+
+ Pylon::PylonAutoInitTerm m_autoInitTerm; //!< Auto initialize and terminate object for pylon SDK.
+};
+
+#endif // #ifdef VISP_HAVE_PYLON
+#endif // #ifndef __vpPylonFactory_h_
diff --git a/modules/sensor/include/visp3/sensor/vpPylonGrabber.h b/modules/sensor/include/visp3/sensor/vpPylonGrabber.h
new file mode 100644
index 0000000..7bfb686
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpPylonGrabber.h
@@ -0,0 +1,395 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Classes allow to grab images from a Basler camera using
+ * Pylon SDK.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+#ifndef __vpPylonGrabber_h_
+#define __vpPylonGrabber_h_
+
+#include <ostream>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpFrameGrabber.h>
+
+#ifdef VISP_HAVE_PYLON
+
+// Work arround: /usr/X11R6/include/X11/X.h:115:30: note: expanded from macro
+// 'None'
+#ifdef None
+#undef None
+#endif
+// Work arround: /usr/X11R6/include/X11/Xlib.h:83:16: note: expanded from
+// macro 'Status'
+#ifdef Status
+#undef Status
+#endif
+
+#include <pylon/PylonIncludes.h>
+
+/*!
+ \file vpPylonGrabber.h
+ \brief Wrapper over Basler Pylon SDK to capture images from Basler
+ cameras.
+*/
+
+/*!
+ \class vpPylonGrabber
+ \ingroup group_sensor_camera
+
+ Allows to grab images from a Basler camera using Pylon SDK. This is an
+ abstract base class which can't be instantiated. Use vpPylonFactory
+ functions to create class instances instead. Currently only GigE and
+ USB3 cameras are supported.
+
+ To get a vpPylonGrabber, use the following code.
+ \code
+ vpPylonFactory &factory = vpPylonFactory::instance();
+ vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE);
+ \endcode
+
+ To use this class install first Pylon SDK\n
+ https://www.baslerweb.com/en/support/downloads/software-downloads/#type=pylonsoftware;version=all
+ \n Installation instructions included.
+
+ Once installed, configure ViSP using cmake to detect Pylon SDK and
+ build ViSP to include Pylon SDK support.
+
+ This class was tested under Ubuntu with pylon 5.0.9 and the following
+ cameras:
+ - acA640-90gm
+ - acA1600-60gm
+
+ This class is inspired by vpFlyCaptureGrabber with much simplified methods.
+ */
+class VISP_EXPORT vpPylonGrabber : public vpFrameGrabber
+{
+public:
+ /*!
+ Default destructor.
+ */
+ virtual ~vpPylonGrabber(){};
+
+ /*! Valid values for user set names.
+ */
+ enum UserSetName {
+ USERSET_DEFAULT, //!< The default user set.
+ USERSET_USERSET1, //!< User set 1.
+ USERSET_USERSET2, //!< User set 2.
+ USERSET_USERSET3, //!< User set 3.
+ USERSET_UNKNOWN //!< User set not supported.
+ };
+
+ /*!
+ Acquire a gray level image from the active camera.
+
+ \param I : Image data structure (8 bits image).
+ */
+ virtual void acquire(vpImage<unsigned char> &I) = 0;
+ /*!
+ Acquire a color image from the active camera.
+
+ \param I : Image data structure (RGBa image).
+ */
+ virtual void acquire(vpImage<vpRGBa> &I) = 0;
+
+ /*!
+ \brief Stop active camera capturing images and disconnect the active
+ camera.
+
+ If you want to use again this camera, you may call
+ setCameraIndex(const unsigned int &) and open(vpImage<unsigned char>
+ &) or open(vpImage<vpRGBa> &) to connect again the camera.
+ */
+ virtual void close() = 0;
+ /*!
+ Connect the active camera.
+
+ \sa disconnect()
+ */
+ virtual void connect() = 0;
+ /*!
+ Disconnect the active camera.
+
+ \sa connect()
+ */
+ virtual void disconnect() = 0;
+
+ /*!
+ Return blacklevel value in % or raw value.
+ If the camera doesn't support blacklevel property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setBlackLevel()
+ */
+ virtual float getBlackLevel() = 0;
+ /*!
+ Print to the output stream active camera information (serial number,
+ camera model, camera vendor, sensor, resolution, firmware version,
+ ...).
+ */
+ virtual std::ostream &getCameraInfo(std::ostream &os) = 0;
+ /*!
+ Return the handler to the active camera or NULL if the camera is not
+ connected. This function was designed to provide a direct access to
+ the Pylon SDK to get access to advanced functionalities that are not
+ implemented in this class.
+ */
+ virtual Pylon::CInstantCamera *getCameraHandler() = 0;
+ /*! Return the index of the active camera. */
+ virtual unsigned int getCameraIndex() const = 0;
+ /*!
+ Return the serial id of a camera with \e index.
+ \param index : Camera index.
+
+ \sa setCameraSerial()
+ */
+ virtual std::string getCameraSerial(unsigned int index) = 0;
+ /*!
+ Return exposure value in ms.
+ If the camera doesn't support exposure property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setExposure()
+ */
+ virtual float getExposure() = 0;
+ /*!
+ Return camera capture framerate.
+ If the camera doesn't support framerate property, return an exception.
+
+ \sa setFrameRate()
+ */
+ virtual float getFrameRate() = 0;
+ /*!
+ Return camera gain value in dB or raw value.
+ If the camera doesn't support gain property, return an exception.
+
+ \sa setGain()
+ */
+ virtual float getGain() = 0;
+ /*!
+ \brief Get the number of cameras of specific subclasses. GigE, USB,
+ etc.
+ \return Return the number of cameras connected on the bus GigE or USB.
+ */
+ virtual unsigned int getNumCameras() = 0;
+ /*!
+ Return gamma correction value.
+ If the camera doesn't support gamma property, return an exception.
+
+ \sa setGamma()
+ */
+ virtual float getGamma() = 0;
+ /*!
+ \brief Loads the selected configuration into the camera's volatile
+ memory and makes it the active configuration set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa saveUserSet()
+ */
+ virtual bool loadUserSet(UserSetName user_set) = 0;
+ /*!
+ \brief Gets the configuration set being used as the default startup
+ set.
+
+ \sa setUserSetDefault()
+ */
+ virtual UserSetName getUserSetDefault() = 0;
+
+ //! Return true if the camera is connected.
+ virtual bool isConnected() const = 0;
+ //! Return true if the camera capture is started.
+ virtual bool isCaptureStarted() const = 0;
+ /*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+ virtual void open(vpImage<unsigned char> &I) = 0;
+ /*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+ virtual void open(vpImage<vpRGBa> &I) = 0;
+
+ /*!
+ Operator that allows to capture a grey level image.
+ \param I : The captured image.
+ */
+ virtual vpPylonGrabber &operator>>(vpImage<unsigned char> &I) = 0;
+ /*!
+ Operator that allows to capture a color image.
+ \param I : The captured image.
+ */
+ virtual vpPylonGrabber &operator>>(vpImage<vpRGBa> &I) = 0;
+
+ /*!
+ Set camera blacklevel mode and parameter.
+ \param blacklevel_value : This is the level of black in an image. A
+ high blacklevel will result in a low amount of black in the image.
+
+ \return The measured blacklevel after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getBlackLevel()
+ */
+ virtual float setBlackLevel(float blacklevel_value) = 0;
+ /*!
+ If multiples cameras are connected on the bus, select the camera to
+ dial with.
+
+ \param index : Current camera index, a value comprised between 0 (the
+ first camera found on the bus) and the number of cameras found and
+ returned by getNumCameras() minus 1.
+
+ If two cameras are connected on the bus, setting \e index to one
+ allows to communicate with the second one. This identifier is not
+ unique. That is why, it is also possible to select a camera by its
+ serial number, which is unique using setCameraSerial().
+
+ \exception vpException::badValue : If the index is greater or equal to
+ the number of cameras connected to the bus.
+ */
+ virtual void setCameraIndex(unsigned int index) = 0;
+ /*!
+ Set the current camera from its unique serial identifier.
+ \param serial : Camera serial string.
+
+ \sa getCameraSerial()
+ */
+ virtual void setCameraSerial(const std::string &serial) = 0;
+ /*!
+ Set camera exposure mode and parameter.
+
+ \param exposure_on : If true turn exposure on, otherwise turn off.
+ \param exposure_auto : If true set auto exposure, if false set manual
+ exposure applying \e exposure_value parameter.
+ \param exposure_value : This is the exposure value in ms.
+
+ \return The measured exposure time in ms after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getExposure()
+ */
+ 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
+ applying \e gain_value parameter.
+ \param gain_value : The amount of amplification that is applied to a
+ pixel in manual mode. An increase in gain can result in an increase in
+ noise.
+
+ \return The measured gain after applying the new setting.
+
+ \sa getGain()
+ */
+ virtual float setGain(bool gain_auto, float gain_value) = 0;
+ /*!
+ Set camera frame rate.
+ \param frame_rate : Camera frame rate (fps) to set.
+ \return The camera current framerate.
+
+ \sa getFramerate()
+ */
+ virtual float setFrameRate(float frame_rate) = 0;
+ /*!
+ Set camera gamma correction mode and parameter.
+
+ \param gamma_on : If true turn gamma correction on, otherwise turn off.
+ \param gamma_value : Parameter used to perform gamma correction of
+ pixel intensity.
+
+ \return The measured gamma correction value after applying the new
+ setting.
+
+ \sa getGamma()
+ */
+ virtual float setGamma(bool gamma_on, float gamma_value = 1) = 0;
+ /*!
+ \brief Saves the current active configuration set into the selected
+ user set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \param set_default Whether to set the configuration set to be used as
+ the default startup set.
+ \return true for finished, false otherwise.
+
+ \sa loadUserSet()
+ */
+ virtual bool saveUserSet(UserSetName user_set, bool set_default = false) = 0;
+ /*!
+ \brief Sets the configuration set to be used as the default startup
+ set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa getUserSetDefault()
+ */
+ virtual bool setUserSetDefault(UserSetName user_set) = 0;
+
+ /*!
+ Start active camera capturing images.
+
+ \sa stopCapture()
+ */
+ virtual void startCapture() = 0;
+ /*!
+ Stop active camera capturing images.
+
+ \sa startCapture()
+ */
+ virtual void stopCapture() = 0;
+};
+
+#endif // #ifdef VISP_HAVE_PYLON
+#endif // #ifndef __vpPylonGrabber_h_
diff --git a/modules/sensor/include/visp3/sensor/vpRealSense.h b/modules/sensor/include/visp3/sensor/vpRealSense.h
index a901dff..617e472 100644
--- a/modules/sensor/include/visp3/sensor/vpRealSense.h
+++ b/modules/sensor/include/visp3/sensor/vpRealSense.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,8 +39,8 @@
#ifndef __vpRealSense_h_
#define __vpRealSense_h_
-#include <stdint.h>
#include <map>
+#include <stdint.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColVector.h>
@@ -53,8 +54,8 @@
#include <librealsense/rs.hpp>
#ifdef VISP_HAVE_PCL
-# include <pcl/point_types.h>
-# include <pcl/common/projection_matrix.h>
+#include <pcl/common/projection_matrix.h>
+#include <pcl/point_types.h>
#endif
/*!
@@ -62,18 +63,22 @@
\ingroup group_sensor_rgbd
- This class is a wrapper over the Intel librealsense library https://github.com/IntelRealSense/librealsense.
- It allows to capture data from the Intel RealSense F200, SR300 and R200 cameras.
+ This class is a wrapper over the Intel librealsense library
+https://github.com/IntelRealSense/librealsense. It allows to capture data from
+the Intel RealSense cameras.
- The usage of vpRealSense class is enabled when librealsense 3rd party is successfully installed. Installation
- instructions are provided following https://github.com/IntelRealSense/librealsense#installation-guide.
+ The usage of vpRealSense class is enabled when librealsense 3rd party is
+successfully installed. Installation instructions are provided following
+https://github.com/IntelRealSense/librealsense#installation-guide.
- Moreover, if Point Cloud Library (PCL) 3rd party is installed we also propose interfaces to retrieve point cloud as
- pcl::PointCloud<pcl::PointXYZ> or pcl::PointCloud<pcl::PointXYZRGB> data structures.
+ Moreover, if Point Cloud Library (PCL) 3rd party is installed we also
+propose interfaces to retrieve point cloud as pcl::PointCloud<pcl::PointXYZ>
+or pcl::PointCloud<pcl::PointXYZRGB> data structures.
- \warning Notice that the usage of this class requires compiler and library support for the ISO C++ 2011 standard.
- This support must be enabled with the -std=c++11 compiler option. Hereafter we give an example of
- a CMakeLists.txt file that allows to build sample-realsense.cpp that uses vpRealSense class.
+ \warning Notice that the usage of this class requires compiler and library
+support for the ISO C++ 2011 standard. This support must be enabled with the
+-std=c++11 compiler option. Hereafter we give an example of a CMakeLists.txt
+file that allows to build sample-realsense.cpp that uses vpRealSense class.
\code
project(sample)
cmake_minimum_required(VERSION 2.6)
@@ -91,12 +96,13 @@ add_executable(sample-realsense sample-realsense.cpp)
target_link_libraries(sample-realsense ${VISP_LIBRARIES})
\endcode
- 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 <visp3/sensor/vpRealSense.h>
+ 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 <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense.h>
int main()
{
@@ -117,6 +123,7 @@ int main()
if (vpDisplay::getClick(I, false))
break;
}
+ return 0;
}
\endcode
@@ -129,9 +136,11 @@ int main()
vpImage<vpRGBa> 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
+ 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 <visp3/sensor/vpRealSense.h>
#include <pcl/visualization/cloud_viewer.h>
@@ -160,7 +169,7 @@ int main()
static bool update = false;
if (! update) {
viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
update = true;
}
else {
@@ -169,22 +178,26 @@ int main()
viewer->spinOnce (100);
}
+ return 0;
}
\endcode
- 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 <visp3/sensor/vpRealSense.h>
+ 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 <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense.h>
int main() {
vpRealSense rs;
rs.setEnableStream(rs::stream::color, true);
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();
@@ -216,15 +229,16 @@ int main() {
This example shows how to get depth stream aligned on color stream:
\code
#include <visp3/core/vpImageConvert.h>
-#include <visp3/sensor/vpRealSense.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense.h>
int main() {
vpRealSense rs;
rs.setEnableStream(rs::stream::color, true);
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();
@@ -242,7 +256,8 @@ int main() {
#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);
@@ -255,8 +270,10 @@ int main() {
}
\endcode
- This is how you get intrinsics for non native stream (the native stream has to be enabled!):
- \code
+ This is how you get intrinsics for non native stream (the native stream has
+ to be enabled!):
+
+\code
#include <visp3/sensor/vpRealSense.h>
int main() {
@@ -264,6 +281,7 @@ int main() {
rs.setEnableStream(rs::stream::color, true);
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();
@@ -271,15 +289,14 @@ int main() {
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;
+ << " ; " << depth_aligned_intrinsic.ppx << " ; " << depth_aligned_intrinsic.ppy << std::endl; return 0;
}
\endcode
Useful information can be retrieved using \p getHandler():
\code
-#include <visp3/sensor/vpRealSense.h>
#include <librealsense/rs.hpp>
+#include <visp3/sensor/vpRealSense.h>
int main() {
vpRealSense rs;
@@ -299,7 +316,15 @@ int main() {
}
\endcode
- \note This class has been tested with the Intel RealSense SR300. The following streams are enabled by default:
+ 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
+
+ \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
+version: 1.12.01). The following streams are enabled by default:
- Color stream with preset: best quality
- Depth stream with preset: best quality
- Infrared stream with preset: best quality
@@ -318,70 +343,77 @@ public:
#endif
void acquire(vpImage<unsigned char> &grey); // tested
void acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud);
- void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, std::vector<vpColVector> &pointcloud);
+ void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ std::vector<vpColVector> &pointcloud);
#ifdef VISP_HAVE_PCL
void acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
- void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
- void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
+ void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
+ void acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
#endif
- void acquire(vpImage<vpRGBa> &color); // tested
+ void acquire(vpImage<vpRGBa> &color); // tested
void acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud);
- void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, std::vector<vpColVector> &pointcloud);
+ void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ std::vector<vpColVector> &pointcloud);
- void acquire(unsigned char * const data_image, unsigned char * const data_depth, std::vector<vpColVector> * 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);
+ void acquire(unsigned char *const data_image, unsigned char *const data_depth,
+ std::vector<vpColVector> *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<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
- void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
- void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
-
- void acquire(unsigned char * const data_image, unsigned char * const data_depth, std::vector<vpColVector> * const data_pointCloud,
- pcl::PointCloud<pcl::PointXYZ>::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<vpColVector> * const data_pointCloud,
- pcl::PointCloud<pcl::PointXYZRGB>::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(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
+ void acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
+
+ void acquire(unsigned char *const data_image, unsigned char *const data_depth,
+ std::vector<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::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<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::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
void close();
- vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const;
+ vpCameraParameters getCameraParameters(
+ const rs::stream &stream,
+ vpCameraParameters::vpCameraParametersProjType type = vpCameraParameters::perspectiveProjWithDistortion) const;
//! Get access to device handler
- rs::device *getHandler() const {
- return m_device;
- }
+ rs::device *getHandler() const { return m_device; }
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 invalid for the point cloud.
- //! For instance, the Point Cloud Library (PCL) uses NAN values for points where the depth is invalid.
- inline float getInvalidDepthValue() const {
- return m_invalidDepthValue;
- }
+ //! Get 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 float getInvalidDepthValue() const { return m_invalidDepthValue; }
//! Get number of devices that are detected
- int getNumDevices() const {
- return m_context.get_device_count();
- }
+ int getNumDevices() const { return m_context.get_device_count(); }
//! Get device serial number.
//! \sa setDeviceBySerialNumber()
- std::string getSerialNumber() const {
- return m_serial_no;
- }
+ std::string getSerialNumber() const { return m_serial_no; }
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;
@@ -389,21 +421,25 @@ public:
rs::format m_streamFormat;
int m_streamFramerate;
- vpRsStreamParams() : m_streamWidth(640), m_streamHeight(480), m_streamFormat(rs::format::rgba8), m_streamFramerate(30) {
+ vpRsStreamParams()
+ : 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, const int streamFramerate)
- : m_streamWidth(streamWidth), m_streamHeight(streamHeight), m_streamFormat(streamFormat), m_streamFramerate(streamFramerate) {
+ 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)
+ {
}
};
void setEnableStream(const rs::stream &stream, const bool status);
- //! 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;
- }
+ //! 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; }
void setStreamSettings(const rs::stream &stream, const rs::preset &preset);
void setStreamSettings(const rs::stream &stream, const vpRsStreamParams ¶ms);
@@ -413,7 +449,7 @@ protected:
rs::device *m_device;
int m_num_devices;
std::string m_serial_no;
- std::map <rs::stream, rs::intrinsics> m_intrinsics;
+ std::map<rs::stream, rs::intrinsics> m_intrinsics;
float m_max_Z; //!< Maximal Z depth in meter
std::map<rs::stream, bool> m_enableStreams;
std::map<rs::stream, bool> m_useStreamPresets;
@@ -426,4 +462,3 @@ protected:
#endif
#endif
-
diff --git a/modules/sensor/include/visp3/sensor/vpRealSense2.h b/modules/sensor/include/visp3/sensor/vpRealSense2.h
new file mode 100644
index 0000000..08ed117
--- /dev/null
+++ b/modules/sensor/include/visp3/sensor/vpRealSense2.h
@@ -0,0 +1,361 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * librealSense2 interface.
+ *
+ *****************************************************************************/
+
+#ifndef __vpRealSense2_h_
+#define __vpRealSense2_h_
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
+
+#include <librealsense2/rs.hpp>
+#include <librealsense2/rsutil.h>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/common/common_headers.h>
+#endif
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+/*!
+ \class vpRealSense2
+
+ \ingroup group_sensor_rgbd
+
+ This class provides a lightweight wrapper over the Intel librealsense2
+library https://github.com/IntelRealSense/librealsense. It allows to capture
+data from the Intel RealSense cameras.
+
+ \note Supported devices for Intel® RealSense™ SDK 2.0 (build 2.8.3):
+ - Intel® RealSense™ Camera D400-Series (not tested)
+ - Intel® RealSense™ Developer Kit SR300 (vpRealSense2 is ok)
+
+ The usage of vpRealSense2 class is enabled when librealsense2 3rd party is
+successfully installed.
+
+ Moreover, if Point Cloud Library (PCL) 3rd party is installed, we also
+propose interfaces to retrieve point cloud as pcl::PointCloud<pcl::PointXYZ>
+or pcl::PointCloud<pcl::PointXYZRGB> data structures.
+
+ \warning Notice that the usage of this class requires compiler and library
+support for the ISO C++ 2011 standard. This support must be enabled with the
+-std=c++11 compiler option. Hereafter we give an example of a CMakeLists.txt
+file that allows to build sample-realsense.cpp that uses vpRealSense2 class.
+ \code
+project(sample)
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED)
+include_directories(${VISP_INCLUDE_DIRS})
+
+include(CheckCXXCompilerFlag)
+check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
+if(COMPILER_SUPPORTS_CXX11)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+endif()
+
+add_executable(sample-realsense sample-realsense.cpp)
+target_link_libraries(sample-realsense ${VISP_LIBRARIES})
+ \endcode
+
+ 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 <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+int main()
+{
+ vpRealSense2 rs;
+ rs.open();
+
+ vpImage<unsigned char> I(rs.getIntrinsics(RS2_STREAM_COLOR).height,
+rs.getIntrinsics(RS2_STREAM_COLOR).width); #ifdef VISP_HAVE_X11 vpDisplayX
+d(I); #elif defined(VISP_HAVE_GDI) vpDisplayGDI d(I); #endif
+
+ while (true) {
+ rs.acquire(I);
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+ return 0;
+}
+ \endcode
+
+ If you want to acquire color images, in the previous sample replace:
+ \code
+ vpImage<unsigned char> I(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
+ \endcode
+ by
+ \code
+ vpImage<vpRGBa> 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 <visp3/sensor/vpRealSense2.h>
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+
+int main()
+{
+ vpRealSense2 rs;
+ rs.open();
+
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ rs.acquire(NULL, NULL, NULL, pointcloud);
+
+ pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setCameraPosition(0, 0, -0.5, 0, -1, 0);
+
+ while (true) {
+ rs.acquire(NULL, NULL, NULL, pointcloud);
+
+ static bool update = false;
+ if (!update) {
+ viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
+ viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ update = true;
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
+ }
+
+ viewer->spinOnce(30);
+ }
+ return 0;
+}
+ \endcode
+
+ 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 <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+int main() {
+ vpRealSense2 rs;
+ rs2::config config;
+ config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_RGBA8, 30);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+ config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
+ rs.open(config);
+
+ vpImage<vpRGBa> Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
+ vpImage<unsigned char> Ii(rs.getIntrinsics(RS2_STREAM_INFRARED).height,
+ rs.getIntrinsics(RS2_STREAM_INFRARED).width);
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX dc(Ic, 0, 0, "Color");
+ vpDisplayX di(Ii, 100, 100, "Infrared");
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI dc(Ic, 0, 0, "Color");
+ vpDisplayGDI di(Ii, 100, 100, "Infrared");
+#endif
+
+ while (true) {
+ rs.acquire((unsigned char *) Ic.bitmap, NULL, NULL, Ii.bitmap);
+ vpDisplay::display(Ic);
+ vpDisplay::display(Ii);
+ vpDisplay::flush(Ic);
+ vpDisplay::flush(Ii);
+ if (vpDisplay::getClick(Ic, false) || vpDisplay::getClick(Ii, false))
+ break;
+ }
+ return 0;
+}
+ \endcode
+
+ This example shows how to get depth stream aligned on color stream:
+ \code
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+int main() {
+ vpRealSense2 rs;
+ rs2::config config;
+ config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 30);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+ config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 30);
+ rs.open(config);
+
+ vpImage<vpRGBa> Ic(rs.getIntrinsics(RS2_STREAM_COLOR).height, rs.getIntrinsics(RS2_STREAM_COLOR).width);
+ vpImage<uint16_t> Id_raw(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width);
+ vpImage<vpRGBa> Id(rs.getIntrinsics(RS2_STREAM_DEPTH).height, rs.getIntrinsics(RS2_STREAM_DEPTH).width);
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX dc(Ic, 0, 0, "Color");
+ vpDisplayX dd(Id, 100, 100, "Depth aligned to color");
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI dc(Ic, 0, 0, "Color");
+ vpDisplayGDI dd(Id, 100, 100, "Depth aligned to color");
+#endif
+
+ 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);
+ vpDisplay::display(Ic);
+ vpDisplay::display(Id);
+ vpDisplay::flush(Ic);
+ vpDisplay::flush(Id);
+ if (vpDisplay::getClick(Ic, false) || vpDisplay::getClick(Id, false))
+ break;
+ }
+ return 0;
+}
+ \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();
+ \endcode
+
+ Information about the sensor can be printed with:
+ \code
+#include <visp3/sensor/vpRealSense2.h>
+
+int main() {
+ vpRealSense2 rs;
+ rs.open();
+ std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
+
+ return 0;
+}
+ \endcode
+
+ \note This class has been tested with the Intel RealSense SR300
+(Firmware: 3.21.0.0) using librealsense (API version: 2.8.3). Refer to the
+librealsense2 documentation or [API how
+to](https://github.com/IntelRealSense/librealsense/wiki/API-How-To) for
+additional information.
+*/
+class VISP_EXPORT vpRealSense2
+{
+public:
+ vpRealSense2();
+ virtual ~vpRealSense2();
+
+ void acquire(vpImage<unsigned char> &grey);
+ void acquire(vpImage<vpRGBa> &color);
+ void acquire(unsigned char *const data_image, unsigned char *const data_depth,
+ std::vector<vpColVector> *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<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZ>::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<vpColVector> *const data_pointCloud, pcl::PointCloud<pcl::PointXYZRGB>::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;
+
+ rs2_intrinsics getIntrinsics(const rs2_stream &stream) const;
+
+ //! Get 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 float getInvalidDepthValue() const { return m_invalidDepthValue; }
+
+ //! Get the maximum Z value (used to discard bad reconstructed depth for
+ //! pointcloud).
+ inline float getMaxZ() const { return m_max_Z; }
+
+ //! Get a reference to `rs2::pipeline`.
+ rs2::pipeline &getPipeline() { return m_pipe; }
+
+ //! 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;
+
+ void open(const rs2::config &cfg = rs2::config());
+
+ 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; }
+
+ //! Set the maximum Z value (used to discard bad reconstructed depth for
+ //! pointcloud).
+ inline void setMaxZ(const float maxZ) { m_max_Z = maxZ; }
+
+protected:
+ rs2_intrinsics m_colorIntrinsics;
+ rs2_extrinsics m_depth2ColorExtrinsics;
+ rs2_intrinsics m_depthIntrinsics;
+ float m_depthScale;
+ float m_invalidDepthValue;
+ float m_max_Z;
+ rs2::pipeline m_pipe;
+ rs2::pipeline_profile m_pipelineProfile;
+ rs2::pointcloud m_pointcloud;
+ rs2::points m_points;
+
+ void getColorFrame(const rs2::frame &frame, vpImage<vpRGBa> &color);
+ void getGreyFrame(const rs2::frame &frame, vpImage<unsigned char> &grey);
+ void getNativeFrameData(const rs2::frame &frame, unsigned char *const data);
+ void getPointcloud(const rs2::depth_frame &depth_frame, std::vector<vpColVector> &pointcloud);
+#ifdef VISP_HAVE_PCL
+ void getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud);
+ void getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud);
+#endif
+};
+
+#endif
+#endif
diff --git a/modules/sensor/include/visp3/sensor/vpScanPoint.h b/modules/sensor/include/visp3/sensor/vpScanPoint.h
index 236da2b..a73ed61 100644
--- a/modules/sensor/include/visp3/sensor/vpScanPoint.h
+++ b/modules/sensor/include/visp3/sensor/vpScanPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,11 +40,11 @@
#include <visp3/core/vpMath.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <math.h>
#include <ostream>
#include <sstream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <math.h>
/*!
\file vpScanPoint.h
@@ -69,144 +70,131 @@
vertical angle that gives the orientation of the layer.
*/
-class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not be added since this class is complete inline
+class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not
+ // be added since this class is complete
+ // inline
{
- public:
+public:
/*! Default constructor. */
- inline vpScanPoint() : rDist(0), hAngle(0), vAngle(0) {}
+ 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;
this->vAngle = scanpoint.vAngle;
}
- /*!
- Set the polar point coordinates.
+ /*!
+ Set the polar point coordinates.
\param r_dist : Radial distance in meter.
\param h_angle : Horizontal angle in radian.
\param v_angle : Vertical angle in radian.
*/
- inline vpScanPoint(double r_dist, double h_angle, double v_angle)
- : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
+ inline vpScanPoint(double r_dist, double h_angle, double v_angle) : rDist(r_dist), hAngle(h_angle), vAngle(v_angle)
{
this->rDist = r_dist;
this->hAngle = h_angle;
this->vAngle = v_angle;
}
/*! Destructor that does nothing. */
- inline virtual ~vpScanPoint() {};
- /*!
- Set the polar point coordinates.
+ inline virtual ~vpScanPoint(){};
+ /*!
+ Set the polar point coordinates.
\param r_dist : Radial distance in meter.
\param h_angle : Horizontal angle in radian.
\param v_angle : Vertical angle in radian.
*/
- inline void setPolar(double r_dist, double h_angle, double v_angle) {
+ inline void setPolar(double r_dist, double h_angle, double v_angle)
+ {
this->rDist = r_dist;
this->hAngle = h_angle;
this->vAngle = v_angle;
}
- /*!
+ /*!
Return the radial distance in meter.
*/
- inline double getRadialDist() const {
- return ( this->rDist );
- }
- /*!
+ inline double getRadialDist() const { return (this->rDist); }
+ /*!
Returns the polar elevation (vertical) angle in radian.
*/
- inline double getVAngle() const {
- return ( this->vAngle );
- }
- /*!
+ inline double getVAngle() const { return (this->vAngle); }
+ /*!
Returns the polar elevation (vertical) angle in radian.
*/
- inline double getHAngle() const {
- return ( this->hAngle );
- }
- /*!
+ inline double getHAngle() const { return (this->hAngle); }
+ /*!
Returns the cartesian x coordinate.
The x and y axis define an horizontal plane, where x is oriented
positive in front of the laser while y on the left side.
-
+
*/
- inline double getX() const {
- return ( rDist * cos(this->hAngle) * cos(this->vAngle) );
- }
- /*!
+ inline double getX() const { return (rDist * cos(this->hAngle) * cos(this->vAngle)); }
+ /*!
Returns the cartesian y coordinate.
The x and y axis define an horizontal plane, where x is oriented
positive in front of the laser while y on the left side.
-
+
*/
- inline double getY() const {
- return ( rDist * sin(this->hAngle) );
- }
- /*!
+ inline double getY() const { return (rDist * sin(this->hAngle)); }
+ /*!
Returns the cartesian z coordinate.
The z axis is vertical and oriented in direction of the sky.
-
+
+ */
+ inline double getZ() const { return (rDist * cos(this->hAngle) * sin(this->vAngle)); }
+
+ friend inline std::ostream &operator<<(std::ostream &s, const vpScanPoint &p);
+
+ /*!
+
+ Returns true if sp1 and sp2 are equal; otherwire returns false.
+
*/
- inline double getZ() const {
- return ( rDist * cos(this->hAngle) * sin(this->vAngle) );
+ friend inline bool operator==(const vpScanPoint &sp1, const vpScanPoint &sp2)
+ {
+ // return ( ( sp1.getRadialDist() == sp2.getRadialDist() )
+ // && ( sp1.getHAngle() == sp2.getHAngle() )
+ // && ( sp1.getVAngle() == sp2.getVAngle() ) );
+ double rd1 = sp1.getRadialDist();
+ double ha1 = sp1.getHAngle();
+ double va1 = sp1.getVAngle();
+ double rd2 = sp2.getRadialDist();
+ double ha2 = sp2.getHAngle();
+ double va2 = sp2.getVAngle();
+
+ return ((std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
}
-
- friend inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p);
-
- /*!
-
- Returns true if sp1 and sp2 are equal; otherwire returns false.
-
- */
- friend inline bool operator==( const vpScanPoint &sp1,
- const vpScanPoint &sp2 ) {
- //return ( ( sp1.getRadialDist() == sp2.getRadialDist() )
- // && ( sp1.getHAngle() == sp2.getHAngle() )
- // && ( sp1.getVAngle() == sp2.getVAngle() ) );
- double rd1 = sp1.getRadialDist();
- double ha1 = sp1.getHAngle();
- double va1 = sp1.getVAngle();
- double rd2 = sp2.getRadialDist();
- double ha2 = sp2.getHAngle();
- double va2 = sp2.getVAngle();
-
- return ( ( std::fabs(rd1 - rd2) <= std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
- &&
- ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
- }
-
- /*!
-
- Returns true if sp1 and sp2 are different; otherwire returns false.
-
- */
- friend inline bool operator!=( const vpScanPoint &sp1,
- const vpScanPoint &sp2 ) {
- //return ( ( sp1.getRadialDist() != sp2.getRadialDist() )
- // || ( sp1.getHAngle() != sp2.getHAngle() )
- // || ( sp1.getVAngle() != sp2.getVAngle() ) );
- double rd1 = sp1.getRadialDist();
- double ha1 = sp1.getHAngle();
- double va1 = sp1.getVAngle();
- double rd2 = sp2.getRadialDist();
- double ha2 = sp2.getHAngle();
- double va2 = sp2.getVAngle();
- return ( ( std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1,rd2)) * std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1,ha2)) * std::numeric_limits<double>::epsilon() )
- ||
- ( std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1,va2)) * std::numeric_limits<double>::epsilon() ) );
- }
-
- private:
- double rDist;
- double hAngle;
- double vAngle;
+
+ /*!
+
+ Returns true if sp1 and sp2 are different; otherwire returns false.
+
+ */
+ friend inline bool operator!=(const vpScanPoint &sp1, const vpScanPoint &sp2)
+ {
+ // return ( ( sp1.getRadialDist() != sp2.getRadialDist() )
+ // || ( sp1.getHAngle() != sp2.getHAngle() )
+ // || ( sp1.getVAngle() != sp2.getVAngle() ) );
+ double rd1 = sp1.getRadialDist();
+ double ha1 = sp1.getHAngle();
+ double va1 = sp1.getVAngle();
+ double rd2 = sp2.getRadialDist();
+ double ha2 = sp2.getHAngle();
+ double va2 = sp2.getVAngle();
+ return ((std::fabs(rd1 - rd2) > std::fabs(vpMath::maximum(rd1, rd2)) * std::numeric_limits<double>::epsilon()) ||
+ (std::fabs(ha1 - ha2) <= std::fabs(vpMath::maximum(ha1, ha2)) * std::numeric_limits<double>::epsilon()) ||
+ (std::fabs(va1 - va2) <= std::fabs(vpMath::maximum(va1, va2)) * std::numeric_limits<double>::epsilon()));
+ }
+
+private:
+ double rDist;
+ double hAngle;
+ double vAngle;
};
/*!
@@ -230,14 +218,14 @@ class /* VISP_EXPORT */ vpScanPoint // Note that here VISP_EXPORT should not be
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
@@ -246,15 +234,13 @@ std::cout << p << std::endl;
\endcode
*/
-inline std::ostream &operator << (std::ostream &s, const vpScanPoint &p) {
+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 0771d49..1cdc7fc 100644
--- a/modules/sensor/include/visp3/sensor/vpSickLDMRS.h
+++ b/modules/sensor/include/visp3/sensor/vpSickLDMRS.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,14 +44,14 @@
#include <arpa/inet.h>
#include <iostream>
-#include <vector>
#include <string.h>
+#include <vector>
-#include <visp3/sensor/vpScanPoint.h>
-#include <visp3/sensor/vpLaserScan.h>
-#include <visp3/sensor/vpLaserScanner.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpException.h>
+#include <visp3/sensor/vpLaserScan.h>
+#include <visp3/sensor/vpLaserScanner.h>
+#include <visp3/sensor/vpScanPoint.h>
/*!
@@ -77,23 +78,23 @@
int main()
{
-#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
- std::string ip = "131.254.12.119";
+#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) ||
+(defined(__APPLE__) && defined(__MACH__))) // UNIX std::string ip =
+"131.254.12.119";
vpSickLDMRS laser;
laser.setIpAddress(ip);
laser.setup();
-
+
vpLaserScan laserscan[4];
for ( ; ; ) {
// Get the measured points in the four layers
laser.measure(laserscan);
- // Prints all the measured points
+ // Prints all the measured points
for (int layer=0; layer<4; layer++) {
- std::vector<vpScanPoint> pointsInLayer = laserscan[layer].getScanPoints();
- vpScanPoint p;
-
+ std::vector<vpScanPoint> pointsInLayer = laserscan[layer].getScanPoints(); vpScanPoint p;
+
for (unsigned int i=0; i < pointsInLayer.size(); i++) {
std::cout << pointsInLayer[i] << std::endl;
}
@@ -105,52 +106,57 @@ int main()
*/
class VISP_EXPORT vpSickLDMRS : public vpLaserScanner
{
- public:
+public:
enum MagicWord {
- MagicWordC2 = 0xAFFEC0C2 ///< The magic word that allows to identify the messages that are sent by the Sick LD-MRS.
+ MagicWordC2 = 0xAFFEC0C2 ///< The magic word that allows to identify the
+ ///< messages that are sent by the Sick LD-MRS.
};
enum DataType {
- MeasuredData = 0x2202 ///< Flag to indicate that the body of a message contains measured data.
+ MeasuredData = 0x2202 ///< Flag to indicate that the body of a message
+ ///< contains measured data.
};
vpSickLDMRS();
/*! Copy constructor. */
vpSickLDMRS(const vpSickLDMRS &sick)
- : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), time_offset(0),
- isFirstMeasure(true), maxlen_body(104000)
- {
+ : vpLaserScanner(sick), socket_fd(-1), body(NULL), vAngle(), time_offset(0), isFirstMeasure(true),
+ maxlen_body(104000)
+ {
*this = sick;
};
virtual ~vpSickLDMRS();
/*! Copy constructor. */
vpSickLDMRS &operator=(const vpSickLDMRS &sick)
{
- socket_fd = sick.socket_fd;
- vAngle = sick.vAngle;
- time_offset = sick.time_offset;
- isFirstMeasure = sick.isFirstMeasure;
- maxlen_body = sick.maxlen_body;
- if (body) delete [] body;
- body = new unsigned char [104000];
- memcpy(body, sick.body, maxlen_body);
+ if (this != &sick) {
+ socket_fd = sick.socket_fd;
+ vAngle = sick.vAngle;
+ time_offset = sick.time_offset;
+ isFirstMeasure = sick.isFirstMeasure;
+ maxlen_body = sick.maxlen_body;
+ if (body)
+ delete[] body;
+ body = new unsigned char[104000];
+ memcpy(body, sick.body, maxlen_body);
+ }
return (*this);
};
- bool setup(std::string ip, int port);
+ bool setup(const std::string &ip, int port);
bool setup();
bool measure(vpLaserScan laserscan[4]);
- protected:
+protected:
#if defined(_WIN32)
SOCKET socket_fd;
#else
- int socket_fd;
+ int socket_fd;
#endif
unsigned char *body;
vpColVector vAngle; // constant vertical angle for each layer
double time_offset;
bool isFirstMeasure;
size_t maxlen_body;
- };
+};
#endif
diff --git a/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h
index d259a0c..ad1c62b 100644
--- a/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h
+++ b/modules/sensor/include/visp3/sensor/vpV4l2Grabber.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpV4l2Grabber.h
\brief class for the Video For Linux 2 video device framegrabbing.
@@ -49,13 +49,13 @@
#ifdef VISP_HAVE_V4L2
-#include <linux/types.h>
+#include <libv4l2.h> // Video For Linux Two interface
#include <linux/kernel.h>
+#include <linux/types.h>
#include <linux/videodev2.h> // Video For Linux Two interface
-#include <libv4l2.h> // Video For Linux Two interface
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpFrameGrabber.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
#include <visp3/core/vpRect.h>
@@ -66,15 +66,16 @@
\brief Class that is a wrapper over the Video4Linux2 (V4L2) driver.
- Thus to be enabled, this class needs the optional V4L2 3rd party. Installation instruction
- are provided here https://visp.inria.fr/3rd_v4l2.
+ Thus to be enabled, this class needs the optional V4L2 3rd party.
+Installation instruction are provided here https://visp.inria.fr/3rd_v4l2.
Information about Video4Linux can be found on
http://linuxtv.org/v4lwiki/index.php/Main_Page
This class was tested with a Pinnacle PCTV Studio/Rave board but
also with the following webcams (Logitech QuickCam Vision Pro 9000,
- Logitech QuickCam Orbit AF, Logitech QuickCam IM (V-USB39), Dell latitude E6400 internal webcam).
+ Logitech QuickCam Orbit AF, Logitech QuickCam IM (V-USB39), Dell latitude
+E6400 internal webcam).
If the grabbing fail with a webcam, it means probably that you don't
have the read/write permission on the /dev/video%%d device. You can
@@ -83,10 +84,11 @@
If the grabbing fails when the camera is attached to a bttv PCI
card, it means potentially that you have not configured the linux
bttv kernel module according to your board.
-
+
For that, depending on your linux distribution check the card id in
- /usr/share/doc/kernel-doc-2.4.20/video4linux/bttv/CARDLIST
- - or /usr/share/doc/kernel-doc-2.6.20/Documentation/video4linux/CARDLIST.bttv
+ - or
+/usr/share/doc/kernel-doc-2.6.20/Documentation/video4linux/CARDLIST.bttv
For example, the card id of a Pinnacle PCTV Studio/Rave board is 39.
Once this id is determined, you have to set the bttv driver with, by adding
@@ -104,8 +106,8 @@
This other example shows how to use this grabber with an analogic camera
attached to a bttv PCI card.
\code
-#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
int main()
{
@@ -124,7 +126,7 @@ int main()
#endif
}
\endcode
-
+
\author Fabien Spindler (Fabien.Spindler at irisa.fr), Irisa / Inria Rennes
@@ -146,89 +148,87 @@ public:
/*! \enum vpV4l2FramerateType
Frame rate type for capture.
*/
- typedef enum
- {
- framerate_50fps, //!< 50 frames per second
- framerate_25fps //!< 25 frames per second
- } vpV4l2FramerateType;
+ typedef enum {
+ framerate_50fps, //!< 50 frames per second
+ framerate_25fps //!< 25 frames per second
+ } vpV4l2FramerateType;
/*! \enum vpV4l2FrameFormatType
Frame format type for capture.
*/
- typedef enum
- {
- V4L2_FRAME_FORMAT, /*!< a field only */
- V4L2_IMAGE_FORMAT /*!< an interlaced image */
- } vpV4l2FrameFormatType;
+ typedef enum {
+ V4L2_FRAME_FORMAT, /*!< a field only */
+ V4L2_IMAGE_FORMAT /*!< an interlaced image */
+ } vpV4l2FrameFormatType;
/*! \enum vpV4l2PixelFormatType
Pixel format type for capture.
*/
typedef enum {
- V4L2_GREY_FORMAT, /*!< 8 Greyscale */
+ V4L2_GREY_FORMAT, /*!< 8 Greyscale */
V4L2_RGB24_FORMAT, /*!< 24 RGB-8-8-8 */
V4L2_RGB32_FORMAT, /*!< 32 RGB-8-8-8-8 */
V4L2_BGR24_FORMAT, /*!< 24 BGR-8-8-8 */
- V4L2_YUYV_FORMAT, /*!< 16 YUYV 4:2:2 */
+ V4L2_YUYV_FORMAT, /*!< 16 YUYV 4:2:2 */
V4L2_MAX_FORMAT
} vpV4l2PixelFormatType;
#ifndef DOXYGEN_SHOULD_SKIP_THIS
struct ng_video_fmt {
- unsigned int pixelformat; /* VIDEO_* */
- unsigned int width;
- unsigned int height;
- unsigned int bytesperline; /* zero for compressed formats */
+ unsigned int pixelformat; /* VIDEO_* */
+ unsigned int width;
+ unsigned int height;
+ unsigned int bytesperline; /* zero for compressed formats */
};
-
struct ng_video_buf {
- struct ng_video_fmt fmt;
- size_t size;
- unsigned char *data;
- int refcount;
+ struct ng_video_fmt fmt;
+ size_t size;
+ unsigned char *data;
+ int refcount;
};
#endif
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpV4l2Grabber(const vpV4l2Grabber &)
-// : fd(-1), device(), cap(), streamparm(), inp(NULL), std(NULL), fmt(NULL), ctl(NULL),
-// fmt_v4l2(), fmt_me(), reqbufs(), buf_v4l2(NULL), buf_me(NULL), queue(0),
-// waiton_cpt(0), index_buffer(0), m_verbose(false), m_nbuffers(3), field(0), streaming(false),
-// m_input(vpV4l2Grabber::DEFAULT_INPUT),
-// m_framerate(vpV4l2Grabber::framerate_25fps),
-// m_frameformat(vpV4l2Grabber::V4L2_FRAME_FORMAT),
-// m_pixelformat(vpV4l2Grabber::V4L2_YUYV_FORMAT)
-// {
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// }
-// vpV4l2Grabber &operator=(const vpV4l2Grabber &){
-// throw vpException(vpException::functionNotImplementedError,"Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpV4l2Grabber(const 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)
+ // {
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!");
+ // }
+ // vpV4l2Grabber &operator=(const vpV4l2Grabber &){
+ // throw vpException(vpException::functionNotImplementedError,"Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
vpV4l2Grabber();
- vpV4l2Grabber(bool verbose);
+ explicit vpV4l2Grabber(bool verbose);
vpV4l2Grabber(unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- vpV4l2Grabber(vpImage<unsigned char> &I,
- unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- vpV4l2Grabber(vpImage<vpRGBa> &I,
- unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
- virtual ~vpV4l2Grabber() ;
+ vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
+ vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
+ virtual ~vpV4l2Grabber();
public:
- void open(vpImage<unsigned char> &I) ;
- void open(vpImage<vpRGBa> &I) ;
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
void acquire(vpImage<unsigned char> &I);
void acquire(vpImage<unsigned char> &I, const vpRect &roi);
- void acquire(vpImage<unsigned char> &I, struct timeval ×tamp, const vpRect &roi=vpRect());
+ void acquire(vpImage<unsigned char> &I, struct timeval ×tamp, const vpRect &roi = vpRect());
void acquire(vpImage<vpRGBa> &I);
void acquire(vpImage<vpRGBa> &I, const vpRect &roi);
- void acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRect &roi=vpRect());
+ void acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRect &roi = vpRect());
bool getField();
vpV4l2FramerateType getFramerate();
/*!
@@ -237,44 +237,33 @@ public:
\return Camera pixel format coding.
- */
- inline vpV4l2PixelFormatType getPixelFormat()
- {
- return (this->m_pixelformat);
- }
+ */
+ inline vpV4l2PixelFormatType getPixelFormat() { return (this->m_pixelformat); }
- vpV4l2Grabber & operator>>(vpImage<unsigned char> &I);
- vpV4l2Grabber & operator>>(vpImage<vpRGBa> &I);
+ vpV4l2Grabber &operator>>(vpImage<unsigned char> &I);
+ vpV4l2Grabber &operator>>(vpImage<vpRGBa> &I);
/*!
Activates the verbose mode to print additional information on stdout.
\param verbose : If true activates the verbose mode.
*/
- void setVerboseMode(bool verbose) {
- this->m_verbose = verbose;
- };
+ void setVerboseMode(bool verbose) { this->m_verbose = verbose; };
void setFramerate(vpV4l2FramerateType framerate);
- void setInput(unsigned input = vpV4l2Grabber::DEFAULT_INPUT) ;
+ void setInput(unsigned input = vpV4l2Grabber::DEFAULT_INPUT);
/*!
Set image width to acquire.
*/
- inline void setWidth(unsigned w)
- {
- this->width = w;
- }
+ inline void setWidth(unsigned w) { this->width = w; }
/*!
Set image height to acquire.
*/
- inline void setHeight(unsigned h)
- {
- this->height = h;
- }
+ inline void setHeight(unsigned h) { this->height = h; }
- void setScale(unsigned scale = vpV4l2Grabber::DEFAULT_SCALE) ;
+ void setScale(unsigned scale = vpV4l2Grabber::DEFAULT_SCALE);
/*!
@@ -287,10 +276,7 @@ public:
\param nbuffers : Number of ring buffers.
*/
- inline void setNBuffers(unsigned nbuffers)
- {
- this->m_nbuffers = nbuffers;
- }
+ inline void setNBuffers(unsigned nbuffers) { this->m_nbuffers = nbuffers; }
/*!
Set the device name.
@@ -298,10 +284,7 @@ public:
\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
@@ -309,8 +292,8 @@ public:
\param pixelformat : Camera pixel format coding.
- */
- inline void setPixelFormat(vpV4l2PixelFormatType pixelformat)
+ */
+ inline void setPixelFormat(vpV4l2PixelFormatType pixelformat)
{
this->m_pixelformat = pixelformat;
if (this->m_pixelformat >= V4L2_MAX_FORMAT)
@@ -320,7 +303,6 @@ public:
void close();
private:
-
void setFormat();
/*!
Set the frame format.
@@ -329,50 +311,46 @@ private:
- 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();
void stopStreaming();
- unsigned char * waiton(__u32 &index, struct timeval ×tamp);
- int queueBuffer();
+ unsigned char *waiton(__u32 &index, struct timeval ×tamp);
+ int queueBuffer();
void queueAll();
void printBufInfo(struct v4l2_buffer buf);
- int fd;
- char device[FILENAME_MAX];
+ int fd;
+ char device[FILENAME_MAX];
/* device descriptions */
- struct v4l2_capability cap;
- struct v4l2_streamparm streamparm;
- struct v4l2_input *inp; //[vpV4l2Grabber::MAX_INPUTS];
- struct v4l2_standard *std; //[vpV4l2Grabber::MAX_NORM];
- struct v4l2_fmtdesc *fmt; //[vpV4l2Grabber::MAX_FORMAT];
- struct v4l2_queryctrl *ctl; //[vpV4l2Grabber::MAX_CTRL*2];
+ struct v4l2_capability cap;
+ struct v4l2_streamparm streamparm;
+ struct v4l2_input *inp; //[vpV4l2Grabber::MAX_INPUTS];
+ struct v4l2_standard *std; //[vpV4l2Grabber::MAX_NORM];
+ struct v4l2_fmtdesc *fmt; //[vpV4l2Grabber::MAX_FORMAT];
+ struct v4l2_queryctrl *ctl; //[vpV4l2Grabber::MAX_CTRL*2];
/* capture */
- struct v4l2_format fmt_v4l2;
- struct ng_video_fmt fmt_me;
- struct v4l2_requestbuffers reqbufs;
- struct v4l2_buffer *buf_v4l2; //[vpV4l2Grabber::MAX_BUFFERS];
- struct ng_video_buf *buf_me; //[vpV4l2Grabber::MAX_BUFFERS];
- unsigned int queue;
- unsigned int waiton_cpt;
- __u32 index_buffer; //!< index of the buffer in use
-
- bool m_verbose;
- unsigned m_nbuffers;
+ struct v4l2_format fmt_v4l2;
+ struct ng_video_fmt fmt_me;
+ struct v4l2_requestbuffers reqbufs;
+ struct v4l2_buffer *buf_v4l2; //[vpV4l2Grabber::MAX_BUFFERS];
+ struct ng_video_buf *buf_me; //[vpV4l2Grabber::MAX_BUFFERS];
+ unsigned int queue;
+ unsigned int waiton_cpt;
+ __u32 index_buffer; //!< index of the buffer in use
+
+ bool m_verbose;
+ unsigned m_nbuffers;
unsigned int field;
- bool streaming;
+ bool streaming;
- unsigned m_input;
+ unsigned m_input;
vpV4l2FramerateType m_framerate;
vpV4l2FrameFormatType m_frameformat;
vpV4l2PixelFormatType m_pixelformat;
-} ;
+};
#endif
#endif
-
diff --git a/modules/sensor/src/force-torque/vpComedi.cpp b/modules/sensor/src/force-torque/vpComedi.cpp
index 97d1351..c9a2c85 100644
--- a/modules/sensor/src/force-torque/vpComedi.cpp
+++ b/modules/sensor/src/force-torque/vpComedi.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,26 +49,23 @@
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)
{
}
/*!
- Destructor that closes the connection to the device if it is not already done calling close().
- \sa close()
+ Destructor that closes the connection to the device if it is not already
+ done calling close(). \sa close()
*/
-vpComedi::~vpComedi()
-{
- close();
-}
+vpComedi::~vpComedi() { close(); }
/*!
Open the connection to the device.
*/
void vpComedi::open()
{
- if (! m_handler) {
+ if (!m_handler) {
m_handler = comedi_open(m_device.c_str());
if (!m_handler) {
@@ -81,7 +79,7 @@ void vpComedi::open()
m_range_info.resize(m_nchannel);
m_maxdata.resize(m_nchannel);
m_chanlist.resize(m_nchannel);
- for(unsigned int channel=0; channel < m_nchannel; channel++) {
+ 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);
@@ -102,8 +100,9 @@ void vpComedi::close()
/*!
Get raw data from device.
- If you selected an analog input subdevice, the output is an unsigned number, for example between 0 and 65535
- for a 16 bit analog input, with 0 representing the lowest voltage of the ADC, and a hardware-dependent
+ If you selected an analog input subdevice, the output is an unsigned
+ number, for example between 0 and 65535 for a 16 bit analog input, with 0
+ representing the lowest voltage of the ADC, and a hardware-dependent
maximum value representing the highest voltage.
*/
std::vector<lsampl_t> vpComedi::getRawData() const
@@ -114,12 +113,15 @@ std::vector<lsampl_t> vpComedi::getRawData() const
// Get raw data
std::vector<lsampl_t> raw_data(m_nchannel);
- for(unsigned int channel=0; channel < m_nchannel; channel++) {
- // 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
+ for (unsigned int channel = 0; channel < m_nchannel; channel++) {
+ // 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);
- if(ret < 0){
- throw vpException(vpException::fatalError, "Cannot get %d data from device=%s subdevice=%d channel=%d range=%d analog reference=%d",
+ 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);
}
}
@@ -128,7 +130,8 @@ std::vector<lsampl_t> vpComedi::getRawData() const
}
/*!
- Get physical data from device with units in Volts or mA. To know which unit is used, call getPhyDataUnits().
+ Get physical data from device with units in Volts or mA. To know which unit
+ is used, call getPhyDataUnits().
*/
vpColVector vpComedi::getPhyData() const
{
@@ -140,7 +143,7 @@ vpColVector vpComedi::getPhyData() const
vpColVector phy_data(m_nchannel);
// Convert data to physical data
- for(unsigned int channel=0; channel < m_nchannel; channel++) {
+ 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]);
if (vpMath::isNaN(phy_data[channel])) {
throw vpException(vpException::fatalError, "Comedi DAQ get NaN value. Check the connection with your device");
@@ -150,7 +153,8 @@ vpColVector vpComedi::getPhyData() const
return phy_data;
}
-//! Get units (V or mA) of the physical data acquired by getPhyData() or getPhyDataAsync().
+//! Get units (V or mA) of the physical data acquired by getPhyData() or
+//! getPhyDataAsync().
std::string vpComedi::getPhyDataUnits() const
{
if (m_handler == NULL) {
@@ -158,15 +162,21 @@ std::string vpComedi::getPhyDataUnits() const
}
std::string units;
unsigned int channel = 0;
- switch(m_range_info[channel]->unit) {
- case UNIT_volt: units = "V"; break;
- case UNIT_mA: units = "mA"; break;
- case UNIT_none: break;
+ switch (m_range_info[channel]->unit) {
+ case UNIT_volt:
+ units = "V";
+ break;
+ case UNIT_mA:
+ units = "mA";
+ break;
+ case UNIT_none:
+ break;
}
return units;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpComedi.cpp.o) has no symbols
-void dummy_vpComedi() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpComedi.cpp.o) has no
+// symbols
+void dummy_vpComedi(){};
#endif
diff --git a/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp b/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp
index 12e5729..722322b 100644
--- a/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp
+++ b/modules/sensor/src/force-torque/vpForceTorqueAtiSensor.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,8 +45,7 @@
#include <visp3/core/vpException.h>
#include <visp3/sensor/vpForceTorqueAtiSensor.h>
-static Calibration *s_calibinfo = NULL; //!< Struct containing calibration information
-
+static Calibration *s_calibinfo = NULL; //!< Struct containing calibration information
/*!
* Default constructor.
@@ -67,8 +67,8 @@ void vpForceTorqueAtiSensor::open()
}
/*!
- Bias the sensor storing an unloaded measurement; this removes the effect of tooling weight.
- \sa unbias()
+ Bias the sensor storing an unloaded measurement; this removes the effect of
+ tooling weight. \sa unbias()
*/
void vpForceTorqueAtiSensor::bias()
{
@@ -78,15 +78,16 @@ 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++)
+ float *sample_bias = new float[m_num_channels];
+ for (unsigned int i = 0; i < m_num_channels; i++)
sample_bias[i] = m_sample_bias[i];
Bias(s_calibinfo, sample_bias);
- delete [] sample_bias;
+ delete[] sample_bias;
}
/*!
@@ -104,23 +105,25 @@ 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++)
+ float *sample_bias = new float[m_num_channels];
+ for (unsigned int i = 0; i < m_num_channels; i++)
sample_bias[i] = m_sample_bias[i];
Bias(s_calibinfo, sample_bias);
- delete [] sample_bias;
+ delete[] sample_bias;
}
/*!
- * Close the calibration structure opened using setCalibrationFile() and close the connection to the device.
+ * Close the calibration structure opened using setCalibrationFile() and close
+ * the connection to the device.
*/
void vpForceTorqueAtiSensor::close()
{
- if(s_calibinfo != NULL) {
+ if (s_calibinfo != NULL) {
// free memory allocated to calibration structure
destroyCalibration(s_calibinfo);
s_calibinfo = NULL;
@@ -129,11 +132,12 @@ void vpForceTorqueAtiSensor::close()
}
/*!
- Get a 6-dimension force/torque vector from device. This function performs synchronously one single data acquisition.
- "Synchronous" means that the calling process blocks until the data acquisition has finished.
+ Get a 6-dimension force/torque vector from device. This function performs
+ synchronously one single data acquisition. "Synchronous" means that the
+ calling process blocks until the data acquisition has finished.
- \return A sampled measure from device with forces and torques. Forces units are given by
- getForceUnits(), while torque units by getTorqueUnits().
+ \return A sampled measure from device with forces and torques. Forces units
+ are given by getForceUnits(), while torque units by getTorqueUnits().
*/
vpColVector vpForceTorqueAtiSensor::getForceTorque() const
{
@@ -143,10 +147,10 @@ vpColVector vpForceTorqueAtiSensor::getForceTorque() const
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];
+ float *voltage = new float[m_num_channels];
+ float *ft = new float[m_num_axes];
- for(unsigned int i=0; i<m_num_channels; i++) {
+ for (unsigned int i = 0; i < m_num_channels; i++) {
voltage[i] = phydata[i];
}
@@ -154,11 +158,11 @@ vpColVector vpForceTorqueAtiSensor::getForceTorque() const
ConvertToFT(s_calibinfo, voltage, ft);
vpColVector sample(m_num_axes);
- for(unsigned int i=0; i<m_num_axes; i++)
+ for (unsigned int i = 0; i < m_num_axes; i++)
sample[i] = ft[i];
- delete [] voltage;
- delete [] ft;
+ delete[] voltage;
+ delete[] ft;
return sample;
}
@@ -183,16 +187,13 @@ std::string vpForceTorqueAtiSensor::getTorqueUnits() const
/*!
* Destructor that closes the connection to the device.
*/
-vpForceTorqueAtiSensor::~vpForceTorqueAtiSensor()
-{
- close();
-}
+vpForceTorqueAtiSensor::~vpForceTorqueAtiSensor() { close(); }
/*!
Open ATI calibration file that should correspond to your F/T sensor.
- \param calibfile : ATI calibration file. This file has the following pattern: FT*.cal.
- \param index : Index of calibration in file (default = 1).
- \sa getCalibrationFile(), close()
+ \param calibfile : ATI calibration file. This file has the following
+ 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)
{
@@ -207,7 +208,7 @@ void vpForceTorqueAtiSensor::setCalibrationFile(const std::string &calibfile, un
// Create calibration struct
s_calibinfo = createCalibration(file, m_index);
- if (s_calibinfo==NULL) {
+ if (s_calibinfo == NULL) {
throw vpException(vpException::fatalError, "Calibration file %s couldn't be loaded", m_calibfile.c_str());
}
@@ -234,9 +235,9 @@ int main()
}
\endcode
*/
-std::ostream & operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
+std::ostream &operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
{
- if (s_calibinfo==NULL) {
+ if (s_calibinfo == NULL) {
os << "Calibration Information is not available" << std::endl;
return os;
}
@@ -256,11 +257,12 @@ std::ostream & operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
// print maximum loads of axes
os << "\nRated Loads:" << std::endl;
- for (unsigned short i=0;i<s_calibinfo->rt.NumAxes;i++) {
+ for (unsigned short i = 0; i < s_calibinfo->rt.NumAxes; i++) {
char *units;
- if ((s_calibinfo->AxisNames[i])[0]=='F') {
- units=s_calibinfo->ForceUnits;
- } else units=s_calibinfo->TorqueUnits;
+ if ((s_calibinfo->AxisNames[i])[0] == 'F') {
+ units = s_calibinfo->ForceUnits;
+ } else
+ units = s_calibinfo->TorqueUnits;
os << s_calibinfo->AxisNames[i] << ": " << s_calibinfo->MaxLoads[i] << " " << units << std::endl;
}
@@ -268,11 +270,11 @@ std::ostream & operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
if (s_calibinfo->TempCompAvailable) {
os << "\nTemperature Compensation Information:" << std::endl;
os << "BS: ";
- for (unsigned short i=0;i<s_calibinfo->rt.NumChannels-1;i++) {
+ for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
os << s_calibinfo->rt.bias_slopes[i] << " ";
}
os << "\nGS: ";
- for (unsigned short i=0;i<s_calibinfo->rt.NumChannels-1;i++) {
+ for (unsigned short i = 0; i < s_calibinfo->rt.NumChannels - 1; i++) {
os << s_calibinfo->rt.gain_slopes[i] << " ";
}
os << "\nTherm: " << s_calibinfo->rt.thermistor << std::endl;
@@ -281,8 +283,8 @@ std::ostream & operator<<(std::ostream &os, const vpForceTorqueAtiSensor &ati)
return os;
}
-
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpForceTorqueAtiSensor.cpp.o) has no symbols
-void dummy_vpForceTorqueAtiSensor() {};
+// Work arround to avoid warning:
+// libvisp_sensor.a(vpForceTorqueAtiSensor.cpp.o) has no symbols
+void dummy_vpForceTorqueAtiSensor(){};
#endif
diff --git a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp
index eb89cf1..55bf33b 100644
--- a/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp
+++ b/modules/sensor/src/framegrabber/1394/vp1394CMUGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,7 +51,10 @@
Basic constructor.
*/
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)
+{
// public members
init = false;
@@ -59,15 +63,12 @@ vp1394CMUGrabber::vp1394CMUGrabber()
// private members
camera = new C1394Camera;
- index = 0; // If a camera was not selected the first one (index = 0) will be used
- _format = _mode = _fps = -1;
- _modeauto=true;
}
/*!
Destructor.
*/
-vp1394CMUGrabber::~vp1394CMUGrabber( )
+vp1394CMUGrabber::~vp1394CMUGrabber()
{
close();
// delete camera instance
@@ -78,33 +79,34 @@ vp1394CMUGrabber::~vp1394CMUGrabber( )
}
/*!
- Select the camera on the bus from its index. The first camera found on the bus has index 0.
- \param cam_id : Camera index.
+ Select the camera on the bus from its index. The first camera found on the
+ bus has index 0. \param cam_id : Camera index.
*/
-void
-vp1394CMUGrabber::selectCamera(int cam_id)
+void vp1394CMUGrabber::selectCamera(int cam_id)
{
int camerror;
- index = cam_id ;
+ index = cam_id;
camerror = camera->SelectCamera(index);
- if ( camerror!= CAM_SUCCESS)
- {
- switch (camerror)
- {
- case CAM_ERROR_PARAM_OUT_OF_RANGE:
- vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is not present") );
- break;
- case CAM_ERROR_BUSY:
- vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
- break;
- case CAM_ERROR:
- vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
- break;
+ if (camerror != CAM_SUCCESS) {
+ switch (camerror) {
+ case CAM_ERROR_PARAM_OUT_OF_RANGE:
+ vpERROR_TRACE("vp1394CMUGrabber error: Found no camera number %i", index);
+ throw(
+ vpFrameGrabberException(vpFrameGrabberException::initializationError, "The required camera is not present"));
+ break;
+ case CAM_ERROR_BUSY:
+ vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "The required camera is in use by other application"));
+ break;
+ case CAM_ERROR:
+ vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
+ "selecting camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
+ break;
}
close();
}
@@ -113,36 +115,34 @@ vp1394CMUGrabber::selectCamera(int cam_id)
/*!
Init the selected camera.
*/
-void
-vp1394CMUGrabber::initCamera()
+void vp1394CMUGrabber::initCamera()
{
- if (init == false)
- {
+ if (init == false) {
int camerror;
- if (camera->CheckLink() != CAM_SUCCESS)
- {
+ 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();
- if ( camerror != CAM_SUCCESS )
- {
- switch (camerror)
- {
- case CAM_ERROR_NOT_INITIALIZED:
- vpERROR_TRACE("vp1394CMUGrabber error: No camera selected",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The is no selected camera") );
- break;
- case CAM_ERROR_BUSY:
- vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"The required camera is in use by other application") );
- break;
- case CAM_ERROR:
- vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when selecting camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,"Resolve camera can not be used") );
- break;
+ if (camerror != CAM_SUCCESS) {
+ switch (camerror) {
+ case CAM_ERROR_NOT_INITIALIZED:
+ vpERROR_TRACE("vp1394CMUGrabber error: No camera selected", index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "The is no selected camera"));
+ break;
+ case CAM_ERROR_BUSY:
+ vpERROR_TRACE("vp1394CMUGrabber error: The camera %i is busy", index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "The required camera is in use by other application"));
+ break;
+ case CAM_ERROR:
+ vpERROR_TRACE("vp1394CMUGrabber error: General I/O error when "
+ "selecting camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Resolve camera can not be used"));
+ break;
}
close();
}
@@ -154,9 +154,9 @@ vp1394CMUGrabber::initCamera()
_format = camera->GetVideoFormat();
_mode = camera->GetVideoMode();
_color = getVideoColorCoding();
- //std::cout << "format: " << _format << std::endl;
- //std::cout << "mode: " << _mode << std::endl;
- //std::cout << "color coding: " << _color << std::endl;
+ // std::cout << "format: " << _format << std::endl;
+ // std::cout << "mode: " << _mode << std::endl;
+ // std::cout << "color coding: " << _color << std::endl;
// Set trigger off
camera->GetCameraControlTrigger()->SetOnOff(false);
@@ -165,14 +165,14 @@ vp1394CMUGrabber::initCamera()
camera->GetVideoFrameDimensions(&w, &h);
this->width = w;
this->height = h;
-
+
// start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StartImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
+ "from IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
}
init = true;
@@ -180,13 +180,11 @@ vp1394CMUGrabber::initCamera()
} // end camera init
-
/*!
Initialization of the grabber using a greyscale image.
\param I : gray level image.
*/
-void
-vp1394CMUGrabber::open(vpImage<unsigned char> &I)
+void vp1394CMUGrabber::open(vpImage<unsigned char> &I)
{
initCamera();
I.resize(this->height, this->width);
@@ -196,8 +194,7 @@ vp1394CMUGrabber::open(vpImage<unsigned char> &I)
Initialization of the grabber using a color image.
\param I : color image.
*/
-void
-vp1394CMUGrabber::open(vpImage<vpRGBa> &I)
+void vp1394CMUGrabber::open(vpImage<vpRGBa> &I)
{
initCamera();
I.resize(this->height, this->width);
@@ -210,61 +207,58 @@ vp1394CMUGrabber::open(vpImage<vpRGBa> &I)
\param I : Acquired gray level image.
*/
-void
-vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
+void vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
{
// get image data
unsigned long length;
- unsigned char *rawdata = NULL ;
+ unsigned char *rawdata = NULL;
int dropped;
unsigned int size;
open(I);
- camera->AcquireImageEx(TRUE,&dropped);
+ camera->AcquireImageEx(TRUE, &dropped);
rawdata = camera->GetRawData(&length);
size = I.getSize();
- switch(_color) {
- case vp1394CMUGrabber::MONO8:
- memcpy(I.bitmap, (unsigned char *) rawdata, size);
- break;
- case vp1394CMUGrabber::MONO16:
- vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV411:
- vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV422:
- vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::YUV444:
- vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
- break;
-
- case vp1394CMUGrabber::RGB8:
- vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
- break;
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
+ switch (_color) {
+ case vp1394CMUGrabber::MONO8:
+ memcpy(I.bitmap, (unsigned char *)rawdata, size);
+ break;
+ case vp1394CMUGrabber::MONO16:
+ vpImageConvert::MONO16ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV411:
+ vpImageConvert::YUV411ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV422:
+ vpImageConvert::YUV422ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::YUV444:
+ vpImageConvert::YUV444ToGrey(rawdata, I.bitmap, size);
+ break;
+
+ case vp1394CMUGrabber::RGB8:
+ vpImageConvert::RGBToGrey(rawdata, I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
+ "Acquisition failed."));
+ break;
};
- //unsigned short depth = 0;
- //camera->GetVideoDataDepth(&depth);
- //std::cout << "depth: " << depth << " computed: " << (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
-
-
- //memcpy(I.bitmap,rawdata,length);
+ // unsigned short depth = 0;
+ // camera->GetVideoDataDepth(&depth);
+ // std::cout << "depth: " << depth << " computed: " <<
+ // (float)(length/(I.getHeight() * I.getWidth())) << std::endl;
+ // memcpy(I.bitmap,rawdata,length);
}
/*!
@@ -275,8 +269,7 @@ vp1394CMUGrabber::acquire(vpImage<unsigned char> &I)
\param I : Acquired color image in RGBa format.
*/
-void
-vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I)
+void vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I)
{
// get image data
unsigned long length;
@@ -286,61 +279,59 @@ vp1394CMUGrabber::acquire(vpImage<vpRGBa> &I)
open(I);
- camera->AcquireImageEx(TRUE,&dropped);
+ camera->AcquireImageEx(TRUE, &dropped);
rawdata = camera->GetRawData(&length);
size = I.getWidth() * I.getHeight();
switch (_color) {
- case vp1394CMUGrabber::MONO8:
- vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::MONO8:
+ vpImageConvert::GreyToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- case vp1394CMUGrabber::MONO16:
- vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::MONO16:
+ vpImageConvert::MONO16ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- case vp1394CMUGrabber::YUV411:
- vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::YUV411:
+ vpImageConvert::YUV411ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- case vp1394CMUGrabber::YUV422:
- vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::YUV422:
+ vpImageConvert::YUV422ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- case vp1394CMUGrabber::YUV444:
- vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::YUV444:
+ vpImageConvert::YUV444ToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- case vp1394CMUGrabber::RGB8:
- size = length / 3;
- vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
- break;
+ case vp1394CMUGrabber::RGB8:
+ size = length / 3;
+ vpImageConvert::RGBToRGBa(rawdata, (unsigned char *)I.bitmap, size);
+ break;
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
+ "Acquisition failed."));
+ break;
};
}
/*!
Stop the acquisition of images and free the camera.
*/
-void
-vp1394CMUGrabber::close()
+void vp1394CMUGrabber::close()
{
// stop acquisition
if (camera->IsAcquiring()) {
// stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StopImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
+ 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"));
}
}
@@ -351,8 +342,7 @@ vp1394CMUGrabber::close()
Set the gain and the shutter values.
\sa setGain(), setShutter()
*/
-void
-vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
+void vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
{
setShutter(shutter);
setGain(gain);
@@ -361,16 +351,15 @@ vp1394CMUGrabber::setControl(unsigned short gain, unsigned short shutter)
/*!
Get the number of connected cameras.
*/
-int
-vp1394CMUGrabber::getNumberOfConnectedCameras() const
+int vp1394CMUGrabber::getNumberOfConnectedCameras() const
{
int n_cam = camera->RefreshCameraList();
return n_cam;
}
-/*!
- Get the gain min and max values.
+/*!
+ Get the gain min and max values.
\sa setAutoGain(), setGain()
*/
@@ -383,8 +372,8 @@ void vp1394CMUGrabber::getGainMinMax(unsigned short &min, unsigned short &max)
Control->Inquire();
Control->GetRange(&min, &max);
}
-/*!
- Enable auto gain.
+/*!
+ Enable auto gain.
\sa setGain()
*/
@@ -393,8 +382,8 @@ void vp1394CMUGrabber::setAutoGain()
initCamera();
camera->GetCameraControl(FEATURE_GAIN)->SetAutoMode(true);
}
-/*!
- Disable auto gain and set the gain to the requested value.
+/*!
+ Disable auto gain and set the gain to the requested value.
\sa setAutoGain()
*/
@@ -403,33 +392,35 @@ void vp1394CMUGrabber::setGain(unsigned short gain)
initCamera();
_gain = gain;
- unsigned short min,max;
+ unsigned short min, max;
C1394CameraControl *Control;
Control = camera->GetCameraControl(FEATURE_GAIN);
Control->Inquire();
- Control->GetRange(&min,&max);
+ Control->GetRange(&min, &max);
- if (_gain < min)
- {
+ if (_gain < min) {
_gain = min;
- std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be less than " << _gain << std::endl;
- }
- else if (_gain > max)
- {
+ std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
+ "IEEE 1394 camera number "
+ << index << " can't be less than " << _gain << std::endl;
+ } else if (_gain > max) {
_gain = max;
- std::cout << "vp1394CMUGrabber warning: Desired gain register value of IEEE 1394 camera number " << index << " can't be greater than " << _gain << std::endl;
+ std::cout << "vp1394CMUGrabber warning: Desired gain register value of "
+ "IEEE 1394 camera number "
+ << index << " can't be greater than " << _gain << std::endl;
}
-
+
Control->SetAutoMode(false);
- if(Control->SetValue(_gain) != CAM_SUCCESS)
- {
- std::cout << "vp1394CMUGrabber warning: Can't set gain register value of IEEE 1394 camera number " << index << std::endl;
+ if (Control->SetValue(_gain) != CAM_SUCCESS) {
+ std::cout << "vp1394CMUGrabber warning: Can't set gain register value of "
+ "IEEE 1394 camera number "
+ << index << std::endl;
}
}
-/*!
- Get the shutter min and max values.
+/*!
+ Get the shutter min and max values.
\sa setAutoShutter(), setShutter()
*/
@@ -443,8 +434,8 @@ void vp1394CMUGrabber::getShutterMinMax(unsigned short &min, unsigned short &max
Control->GetRange(&min, &max);
}
-/*!
- Enable auto shutter.
+/*!
+ Enable auto shutter.
\sa setShutter()
*/
@@ -453,8 +444,8 @@ void vp1394CMUGrabber::setAutoShutter()
initCamera();
camera->GetCameraControl(FEATURE_SHUTTER)->SetAutoMode(true);
}
-/*!
- Disable auto shutter and set the shutter to the requested value.
+/*!
+ Disable auto shutter and set the shutter to the requested value.
\sa setAutoShutter()
*/
@@ -464,71 +455,66 @@ void vp1394CMUGrabber::setShutter(unsigned short shutter)
_shutter = shutter;
- unsigned short min,max;
+ unsigned short min, max;
C1394CameraControl *Control;
Control = camera->GetCameraControl(FEATURE_SHUTTER);
Control->Inquire();
- Control->GetRange(&min,&max);
+ Control->GetRange(&min, &max);
- if (_shutter < min)
- {
+ if (_shutter < min) {
_shutter = min;
- std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be less than " << _shutter << std::endl;
- }
- else if (_shutter > max)
- {
+ std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
+ "value of IEEE 1394 camera number "
+ << index << " can't be less than " << _shutter << std::endl;
+ } else if (_shutter > max) {
_shutter = max;
- std::cout << "vp1394CMUGrabber warning: Desired exposure time register value of IEEE 1394 camera number " << index << " can't be greater than " << _shutter << std::endl;
+ std::cout << "vp1394CMUGrabber warning: Desired exposure time register "
+ "value of IEEE 1394 camera number "
+ << index << " can't be greater than " << _shutter << std::endl;
}
Control->SetAutoMode(false);
- if(Control->SetValue(_shutter) != CAM_SUCCESS)
- {
- std::cout << "vp1394CMUGrabber warning: Can't set exposure time register value of IEEE 1394 camera number " << index << std::endl;
+ if (Control->SetValue(_shutter) != CAM_SUCCESS) {
+ std::cout << "vp1394CMUGrabber warning: Can't set exposure time register "
+ "value of IEEE 1394 camera number "
+ << index << std::endl;
}
}
/*!
Display information about the camera on the standard output.
*/
-void
-vp1394CMUGrabber::displayCameraDescription(int cam_id)
+void vp1394CMUGrabber::displayCameraDescription(int cam_id)
{
- if( camera->GetNumberCameras() > cam_id )
- {
+ if (camera->GetNumberCameras() > cam_id) {
char buf[512];
- camera->GetNodeDescription(cam_id,buf,512);
- std::cout << "Camera " << cam_id << ": " << buf << std::endl ;
+ camera->GetNodeDescription(cam_id, buf, 512);
+ std::cout << "Camera " << cam_id << ": " << buf << std::endl;
- }
- else {
- std::cout << "Camera " << cam_id << ": camera not found" << std::endl ;
+ } else {
+ std::cout << "Camera " << cam_id << ": camera not found" << std::endl;
}
}
-
/*!
Display camera model on the standard output. Call it after open the grabber.
*/
-void
-vp1394CMUGrabber::displayCameraModel()
+void vp1394CMUGrabber::displayCameraModel()
{
- char vendor[256] , model[256] , buf[256];
+ char vendor[256], model[256], buf[256];
LARGE_INTEGER ID;
- camera->GetCameraName(model,sizeof(model));
- camera->GetCameraVendor(vendor,sizeof(vendor));
+ camera->GetCameraName(model, sizeof(model));
+ camera->GetCameraVendor(vendor, sizeof(vendor));
camera->GetCameraUniqueID(&ID);
std::cout << "Vendor: " << vendor << std::endl;
std::cout << "Model: " << model << std::endl;
- sprintf(buf,"%08X%08X",ID.HighPart,ID.LowPart);
+ sprintf(buf, "%08X%08X", ID.HighPart, ID.LowPart);
std::cout << "UniqueID: " << buf << std::endl;
-
}
-
/*!
Set the camera format and video mode.
This method has to be called before open().
@@ -540,83 +526,85 @@ vp1394CMUGrabber::displayCameraModel()
format and mode and the resulting video color coding.
<TABLE BORDER="1">
- <TR><TH> Format </TH><TH> Mode </TH><TH> (H) x (W) </TH><TH> Color </TH></TR>
- <TR><TD> 0 </TD><TD> 0 </TD><TD> 160 x 120 </TD><TD> YUV444 </TD></TR>
- <TR><TD> 0 </TD><TD> 1 </TD><TD> 320 x 240 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 0 </TD><TD> 2 </TD><TD> 640 x 480 </TD><TD> YUV411 </TD></TR>
- <TR><TD> 0 </TD><TD> 3 </TD><TD> 640 x 480 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 0 </TD><TD> 4 </TD><TD> 640 x 480 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 0 </TD><TD> 5 </TD><TD> 640 x 480 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 0 </TD><TD> 6 </TD><TD> 640 x 480 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 1 </TD><TD> 0 </TD><TD> 800 x 600 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 1 </TD><TD> 1 </TD><TD> 800 x 600 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 1 </TD><TD> 2 </TD><TD> 800 x 600 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 1 </TD><TD> 3 </TD><TD>1024 x 768 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 1 </TD><TD> 4 </TD><TD>1024 x 768 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 1 </TD><TD> 5 </TD><TD>1024 x 768 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 1 </TD><TD> 6 </TD><TD> 800 x 600 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 1 </TD><TD> 7 </TD><TD>1024 x 768 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 2 </TD><TD> 0 </TD><TD>1280 x 960 </TD><TD> YUV422 </TD></TR>
- <TR><TD> 2 </TD><TD> 1 </TD><TD>1280 x 960 </TD><TD> RGB8 </TD></TR>
- <TR><TD> 2 </TD><TD> 2 </TD><TD>1280 x 960 </TD><TD> MONO8 </TD></TR>
- <TR><TD> 2 </TD><TD> 3 </TD><TD>1600 x 1200</TD><TD> YUV422 </TD></TR>
- <TR><TD> 2 </TD><TD> 4 </TD><TD>1600 x 1200</TD><TD> RGB8 </TD></TR>
- <TR><TD> 2 </TD><TD> 5 </TD><TD>1600 x 1200</TD><TD> MONO8 </TD></TR>
- <TR><TD> 2 </TD><TD> 6 </TD><TD>1280 x 960 </TD><TD> MONO16 </TD></TR>
- <TR><TD> 2 </TD><TD> 7 </TD><TD>1600 x 1200</TD><TD> MONO16 </TD></TR>
+ <TR><TH> Format </TH><TH> Mode </TH><TH> (H) x (W) </TH><TH> Color
+ </TH></TR> <TR><TD> 0 </TD><TD> 0 </TD><TD> 160 x 120 </TD><TD>
+ YUV444 </TD></TR> <TR><TD> 0 </TD><TD> 1 </TD><TD> 320 x 240
+ </TD><TD> YUV422 </TD></TR> <TR><TD> 0 </TD><TD> 2 </TD><TD> 640 x
+ 480 </TD><TD> YUV411 </TD></TR> <TR><TD> 0 </TD><TD> 3 </TD><TD> 640
+ x 480 </TD><TD> YUV422 </TD></TR> <TR><TD> 0 </TD><TD> 4 </TD><TD>
+ 640 x 480 </TD><TD> RGB8 </TD></TR> <TR><TD> 0 </TD><TD> 5
+ </TD><TD> 640 x 480 </TD><TD> MONO8 </TD></TR> <TR><TD> 0 </TD><TD> 6
+ </TD><TD> 640 x 480 </TD><TD> MONO16 </TD></TR> <TR><TD> 1 </TD><TD> 0
+ </TD><TD> 800 x 600 </TD><TD> YUV422 </TD></TR> <TR><TD> 1 </TD><TD> 1
+ </TD><TD> 800 x 600 </TD><TD> RGB8 </TD></TR> <TR><TD> 1 </TD><TD> 2
+ </TD><TD> 800 x 600 </TD><TD> MONO8 </TD></TR> <TR><TD> 1 </TD><TD> 3
+ </TD><TD>1024 x 768 </TD><TD> YUV422 </TD></TR> <TR><TD> 1 </TD><TD> 4
+ </TD><TD>1024 x 768 </TD><TD> RGB8 </TD></TR> <TR><TD> 1 </TD><TD> 5
+ </TD><TD>1024 x 768 </TD><TD> MONO8 </TD></TR> <TR><TD> 1 </TD><TD> 6
+ </TD><TD> 800 x 600 </TD><TD> MONO16 </TD></TR> <TR><TD> 1 </TD><TD> 7
+ </TD><TD>1024 x 768 </TD><TD> MONO16 </TD></TR> <TR><TD> 2 </TD><TD> 0
+ </TD><TD>1280 x 960 </TD><TD> YUV422 </TD></TR> <TR><TD> 2 </TD><TD> 1
+ </TD><TD>1280 x 960 </TD><TD> RGB8 </TD></TR> <TR><TD> 2 </TD><TD> 2
+ </TD><TD>1280 x 960 </TD><TD> MONO8 </TD></TR> <TR><TD> 2 </TD><TD> 3
+ </TD><TD>1600 x 1200</TD><TD> YUV422 </TD></TR> <TR><TD> 2 </TD><TD> 4
+ </TD><TD>1600 x 1200</TD><TD> RGB8 </TD></TR> <TR><TD> 2 </TD><TD> 5
+ </TD><TD>1600 x 1200</TD><TD> MONO8 </TD></TR> <TR><TD> 2 </TD><TD> 6
+ </TD><TD>1280 x 960 </TD><TD> MONO16 </TD></TR> <TR><TD> 2 </TD><TD> 7
+ </TD><TD>1600 x 1200</TD><TD> MONO16 </TD></TR>
</TABLE>
*/
-void
-vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode )
+void vp1394CMUGrabber::setVideoMode(unsigned long format, unsigned long mode)
{
initCamera();
- _format = format ;
- _mode = mode ;
+ _format = format;
+ _mode = mode;
// Set format and mode
- if ((_format != -1) && (_mode != -1))
- {
- if (!camera->HasVideoMode(_format, _mode))
- {
+ if ((_format != -1) && (_mode != -1)) {
+ if (!camera->HasVideoMode(_format, _mode)) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: The image format is not supported by the IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Video mode not supported") );
+ vpERROR_TRACE("vp1394CMUGrabber error: The image format is not "
+ "supported by the IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Video mode not supported"));
}
if (camera->IsAcquiring()) {
// stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StopImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
+ "from IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
}
}
- if (camera->SetVideoFormat(_format) != CAM_SUCCESS)
- {
+ if (camera->SetVideoFormat(_format) != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video format") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video format of IEEE "
+ "1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video format"));
}
- if (camera->SetVideoMode(_mode) != CAM_SUCCESS)
- {
+ if (camera->SetVideoMode(_mode) != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video mode") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video mode of IEEE "
+ "1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video mode"));
}
-
+
// start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StartImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
+ "from IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
}
// Update Image dimension
@@ -633,8 +621,8 @@ vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode )
/*!
Set camera framerate rate. This method has to be called before open().
- \param fps : Value between 0 to 7 used to select a specific camera framerate.
- See the following table for the correspondances between the input
+ \param fps : Value between 0 to 7 used to select a specific camera
+ framerate. See the following table for the correspondances between the input
value and the framerate.
<TABLE BORDER="1">
@@ -651,53 +639,53 @@ vp1394CMUGrabber::setVideoMode( unsigned long format, unsigned long mode )
\sa getFramerate()
*/
-void
-vp1394CMUGrabber::setFramerate(unsigned long fps)
+void vp1394CMUGrabber::setFramerate(unsigned long fps)
{
initCamera();
_fps = fps;
-
+
// Set fps
- if (_fps!=-1)
- {
- if (!camera->HasVideoFrameRate(_format,_mode,_fps))
- {
+ if (_fps != -1) {
+ if (!camera->HasVideoFrameRate(_format, _mode, _fps)) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported by the IEEE 1394 camera number %i for the selected image format",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"The frame rate is not supported") );
+ vpERROR_TRACE("vp1394CMUGrabber error: The frame rate is not supported "
+ "by the IEEE 1394 camera number %i for the selected "
+ "image format",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The frame rate is not supported"));
}
if (camera->IsAcquiring()) {
// stop acquisition
- if (camera->StopImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StopImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while stopping image acquisition") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't stop image acquisition "
+ "from IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while stopping image acquisition"));
}
}
- if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS)
- {
+ if (camera->SetVideoFrameRate(_fps) != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,"Can't set video frame rate") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't set video frame rate of "
+ "IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Can't set video frame rate"));
}
// start acquisition
- if (camera->StartImageAcquisition() != CAM_SUCCESS)
- {
+ if (camera->StartImageAcquisition() != CAM_SUCCESS) {
close();
- vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition from IEEE 1394 camera number %i",index);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Error while starting image acquisition") );
+ vpERROR_TRACE("vp1394CMUGrabber error: Can't start image acquisition "
+ "from IEEE 1394 camera number %i",
+ index);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Error while starting image acquisition"));
}
-
}
}
/*!
Get the video framerate.
-
+
\return Value between 0 to 7 corresponding to a specific camera framerate.
See the following table for the correspondances between the returned
value and the framerate.
@@ -716,8 +704,7 @@ vp1394CMUGrabber::setFramerate(unsigned long fps)
\sa setFramerate()
*/
-int
-vp1394CMUGrabber::getFramerate()
+int vp1394CMUGrabber::getFramerate()
{
initCamera();
int fps = camera->GetVideoFrameRate();
@@ -769,7 +756,7 @@ vp1394CMUGrabber &vp1394CMUGrabber::operator>>(vpImage<vpRGBa> &I)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vp1394CMUGrabber.cpp.o) has no symbols
-void dummy_vp1394CMUGrabber() {};
+// Work arround to avoid warning: libvisp_sensor.a(vp1394CMUGrabber.cpp.o) has
+// no symbols
+void dummy_vp1394CMUGrabber(){};
#endif
-
diff --git a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp
index 9410c96..e063067 100644
--- a/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp
+++ b/modules/sensor/src/framegrabber/1394/vp1394TwoGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vp1394TwoGrabber.cpp
\brief member functions for firewire cameras
@@ -51,73 +51,31 @@
#if defined(VISP_HAVE_DC1394)
#include <unistd.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/core/vpFrameGrabberException.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpTime.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
-const char * vp1394TwoGrabber::strVideoMode[DC1394_VIDEO_MODE_NUM]= {
- "MODE_160x120_YUV444",
- "MODE_320x240_YUV422",
- "MODE_640x480_YUV411",
- "MODE_640x480_YUV422",
- "MODE_640x480_RGB8",
- "MODE_640x480_MONO8",
- "MODE_640x480_MONO16",
- "MODE_800x600_YUV422",
- "MODE_800x600_RGB8",
- "MODE_800x600_MONO8",
- "MODE_1024x768_YUV422",
- "MODE_1024x768_RGB8",
- "MODE_1024x768_MONO8",
- "MODE_800x600_MONO16",
- "MODE_1024x768_MONO16",
- "MODE_1280x960_YUV422",
- "MODE_1280x960_RGB8",
- "MODE_1280x960_MONO8",
- "MODE_1600x1200_YUV422",
- "MODE_1600x1200_RGB8",
- "MODE_1600x1200_MONO8",
- "MODE_1280x960_MONO16",
- "MODE_1600x1200_MONO16",
- "MODE_EXIF",
- "MODE_FORMAT7_0",
- "MODE_FORMAT7_1",
- "MODE_FORMAT7_2",
- "MODE_FORMAT7_3",
- "MODE_FORMAT7_4",
- "MODE_FORMAT7_5",
- "MODE_FORMAT7_6",
- "MODE_FORMAT7_7"
-};
-
-const char * vp1394TwoGrabber::strFramerate[DC1394_FRAMERATE_NUM]= {
- "FRAMERATE_1_875",
- "FRAMERATE_3_75",
- "FRAMERATE_7_5",
- "FRAMERATE_15",
- "FRAMERATE_30",
- "FRAMERATE_60",
- "FRAMERATE_120",
- "FRAMERATE_240"
-};
-
-const char * vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM]= {
- "COLOR_CODING_MONO8",
- "COLOR_CODING_YUV411",
- "COLOR_CODING_YUV422",
- "COLOR_CODING_YUV444",
- "COLOR_CODING_RGB8",
- "COLOR_CODING_MONO16",
- "COLOR_CODING_RGB16",
- "COLOR_CODING_MONO16S",
- "COLOR_CODING_RGB16S",
- "COLOR_CODING_RAW8",
- "COLOR_CODING_RAW16",
+const char *vp1394TwoGrabber::strVideoMode[DC1394_VIDEO_MODE_NUM] = {
+ "MODE_160x120_YUV444", "MODE_320x240_YUV422", "MODE_640x480_YUV411", "MODE_640x480_YUV422",
+ "MODE_640x480_RGB8", "MODE_640x480_MONO8", "MODE_640x480_MONO16", "MODE_800x600_YUV422",
+ "MODE_800x600_RGB8", "MODE_800x600_MONO8", "MODE_1024x768_YUV422", "MODE_1024x768_RGB8",
+ "MODE_1024x768_MONO8", "MODE_800x600_MONO16", "MODE_1024x768_MONO16", "MODE_1280x960_YUV422",
+ "MODE_1280x960_RGB8", "MODE_1280x960_MONO8", "MODE_1600x1200_YUV422", "MODE_1600x1200_RGB8",
+ "MODE_1600x1200_MONO8", "MODE_1280x960_MONO16", "MODE_1600x1200_MONO16", "MODE_EXIF",
+ "MODE_FORMAT7_0", "MODE_FORMAT7_1", "MODE_FORMAT7_2", "MODE_FORMAT7_3",
+ "MODE_FORMAT7_4", "MODE_FORMAT7_5", "MODE_FORMAT7_6", "MODE_FORMAT7_7"};
+
+const char *vp1394TwoGrabber::strFramerate[DC1394_FRAMERATE_NUM] = {
+ "FRAMERATE_1_875", "FRAMERATE_3_75", "FRAMERATE_7_5", "FRAMERATE_15",
+ "FRAMERATE_30", "FRAMERATE_60", "FRAMERATE_120", "FRAMERATE_240"};
+
+const char *vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM] = {
+ "COLOR_CODING_MONO8", "COLOR_CODING_YUV411", "COLOR_CODING_YUV422", "COLOR_CODING_YUV444",
+ "COLOR_CODING_RGB8", "COLOR_CODING_MONO16", "COLOR_CODING_RGB16", "COLOR_CODING_MONO16S",
+ "COLOR_CODING_RGB16S", "COLOR_CODING_RAW8", "COLOR_CODING_RAW16",
};
-
-
/*!
Default constructor.
@@ -126,11 +84,12 @@ const char * vp1394TwoGrabber::strColorCoding[DC1394_COLOR_CODING_NUM]= {
- the ring buffer size is set to 4,
- reset the bus attached to the first camera found on the bus.
- Current camera settings can be changed using setCamera() to select the active
- camera on the bus and than setVideoMode() or setFramerate() to fix the active
- camera settings. The list of supported video modes and framerates is
+ Current camera settings can be changed using setCamera() to select the
+active camera on the bus and than setVideoMode() or setFramerate() to fix the
+active camera settings. The list of supported video modes and framerates is
available using respectively getVideoModeSupported() and
- getFramerateSupported(). To change the ring buffer size use setRingBufferSize().
+ getFramerateSupported(). To change the ring buffer size use
+setRingBufferSize().
\param reset : If "true", reset the bus attached to the first
camera found. Bus reset may help to make firewire working if the
@@ -166,10 +125,10 @@ vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
: camera(NULL), cameras(NULL), num_cameras(0), camera_id(0), verbose(false), camIsOpen(NULL),
num_buffers(4), // ring buffer size
isDataModified(NULL), initialShutterMode(NULL), dataCam(NULL)
- #ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
- , d(NULL),
- list(NULL)
- #endif
+#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ ,
+ d(NULL), list(NULL)
+#endif
{
// protected members
width = height = 0;
@@ -179,7 +138,7 @@ vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
reset = false;
initialize(reset);
-
+
// open();
}
@@ -194,11 +153,11 @@ vp1394TwoGrabber::vp1394TwoGrabber(bool reset)
*/
vp1394TwoGrabber::~vp1394TwoGrabber()
{
-/* if(num_cameras >= 1){
- delete[] isDataModified;
- delete[] initialShutterMode;
- delete[] dataCam;
- }*/
+ /* if(num_cameras >= 1){
+ delete[] isDataModified;
+ delete[] initialShutterMode;
+ delete[] dataCam;
+ }*/
close();
}
@@ -234,7 +193,7 @@ int main()
ncameras = g.getNumCameras();
int last = 0;
- if (ncameras > 1)
+ if (ncameras > 1)
last = ncameras-1;
g.setCamera(last); // To dial with the last camera on the bus
@@ -244,8 +203,9 @@ int main()
}
\endcode
- If more than one camera is connected, it is also possible to select a specific camera by its GUID:
- \code
+ If more than one camera is connected, it is also possible to select a
+specific camera by its GUID:
+\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
@@ -278,12 +238,12 @@ int main()
printf("Use camera with GUID: 0x%lx\n", g.getGuid());
g.acquire(I); // Acquire an image from the camera with GUID 0xb09d01009b329c
-
+
vpImageIo::write(I, "image.pgm"); // Write image on the disk
#endif
}
\endcode
-
+
Here an example of multi camera capture:
\code
#include <visp3/core/vpConfig.h>
@@ -322,8 +282,7 @@ int main()
\sa setFormat(), setVideoMode(), setFramerate(), getNumCameras()
*/
-void
-vp1394TwoGrabber::setCamera(uint64_t cam_id)
+void vp1394TwoGrabber::setCamera(uint64_t cam_id)
{
// Suppose that if camera_id is a camera GUID, this value is greater
// than the number of cameras connected to the bus
@@ -331,7 +290,7 @@ vp1394TwoGrabber::setCamera(uint64_t cam_id)
// Check if camera_id is a camera guid
bool is_guid = false;
// check if the camera_id is a guid
- for (unsigned int i=0; i< num_cameras; i++) {
+ for (unsigned int i = 0; i < num_cameras; i++) {
if (cameras[i]->guid == cam_id) {
this->camera_id = i;
is_guid = true;
@@ -341,23 +300,22 @@ 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 << num_cameras << " camera(s) connected" << std::endl;
- for (unsigned int i=0; i< num_cameras; i++) {
+ for (unsigned int i = 0; i < num_cameras; i++) {
std::cout << " - camera " << i << " with guid 0x" << std::hex << cameras[i]->guid << std::endl;
}
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required camera is not present") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required camera is not present"));
}
- }
- else {
- this->camera_id = (unsigned int) cam_id; // The input cam_id is not a uint64_t guid, but the index of the camera
+ } else {
+ this->camera_id = (unsigned int)cam_id; // The input cam_id is not a
+ // uint64_t guid, but the index of
+ // the camera
}
// create a pointer to the working camera
camera = cameras[this->camera_id];
}
-
/*!
Get the active camera identifier on the bus.
@@ -372,17 +330,14 @@ vp1394TwoGrabber::setCamera(uint64_t cam_id)
\sa setCamera(), getNumCameras()
*/
-void
-vp1394TwoGrabber::getCamera(uint64_t &cam_id)
+void vp1394TwoGrabber::getCamera(uint64_t &cam_id)
{
if (num_cameras) {
cam_id = this->camera_id;
- }
- else {
+ } else {
close();
vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
}
}
@@ -400,17 +355,14 @@ vp1394TwoGrabber::getCamera(uint64_t &cam_id)
\sa setCamera(), getNumCameras()
*/
-uint64_t
-vp1394TwoGrabber::getCamera()
+uint64_t vp1394TwoGrabber::getCamera()
{
if (num_cameras) {
return this->camera_id;
- }
- else {
+ } else {
close();
vpERROR_TRACE("No cameras found");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No cameras found") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No cameras found"));
}
}
@@ -421,11 +373,10 @@ vp1394TwoGrabber::getCamera()
\param ncameras : The number of cameras found on the bus.
*/
-void
-vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
+void vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
{
- if (! num_cameras) {
- vpCTRACE << "No camera found..."<< std::endl;
+ if (!num_cameras) {
+ vpCTRACE << "No camera found..." << std::endl;
ncameras = 0;
}
@@ -439,12 +390,11 @@ vp1394TwoGrabber::getNumCameras(unsigned int &ncameras) const
\return The number of cameras found on the bus.
*/
-unsigned int
-vp1394TwoGrabber::getNumCameras() const
+unsigned int vp1394TwoGrabber::getNumCameras() const
{
unsigned int ncameras = 0;
- if (! num_cameras) {
- vpCTRACE << "No camera found..."<< std::endl;
+ if (!num_cameras) {
+ vpCTRACE << "No camera found..." << std::endl;
ncameras = 0;
}
@@ -454,8 +404,8 @@ vp1394TwoGrabber::getNumCameras() const
/*!
- Set the camera video capture mode. Image size is than updated with respect to
- the new video capture mode.
+ Set the camera video capture mode. Image size is than updated with respect
+to the new video capture mode.
The iso transmission (setTransmission()) and the dma capture (see
setCapture()) are first stopped. Then, the camera video capture mode is
@@ -497,41 +447,35 @@ int main()
\sa getVideoMode(), getVideoModeSupported(), setCamera()
*/
-void
-vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
+void vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
{
open();
- if (! num_cameras) {
+ 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",camera_id) );
+ if (!isVideoModeSupported(videomode)) {
+ 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"));
}
}
@@ -551,14 +495,12 @@ vp1394TwoGrabber::setVideoMode(vp1394TwoVideoModeType videomode)
\sa setVideoMode(), getVideoModeSupported(), setCamera()
*/
-void
-vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType & videomode)
+void vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType &videomode)
{
- if (! num_cameras) {
+ 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;
@@ -566,15 +508,11 @@ 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;
-
+ videomode = (vp1394TwoVideoModeType)_videomode;
}
-
-
/*!
Query the available active camera video modes.
@@ -592,33 +530,30 @@ vp1394TwoGrabber::getVideoMode(vp1394TwoVideoModeType & videomode)
\sa setVideoMode(), getVideoMode(), getCamera()
*/
-uint32_t
-vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> & videomodes)
+uint32_t vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> &videomodes)
{
// Refresh the list of supported modes
videomodes.clear();
- if (! num_cameras) {
+ 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];
- videomodes.push_back( _mode );
+ for (unsigned i = 0; i < _videomodes.num; i++) {
+ vp1394TwoVideoModeType _mode = (vp1394TwoVideoModeType)_videomodes.modes[i];
+ videomodes.push_back(_mode);
}
// return the number of available video modes
@@ -639,29 +574,26 @@ vp1394TwoGrabber::getVideoModeSupported(std::list<vp1394TwoVideoModeType> & vide
\sa setVideoMode(), getVideoMode(), getCamera()
*/
-bool
-vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
+bool vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
{
- if (! num_cameras) {
+ 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
- for (unsigned i=0; i < _videomodes.num; i++) {
- if ((vp1394TwoVideoModeType) _videomodes.modes[i] == videomode)
+ for (unsigned i = 0; i < _videomodes.num; i++) {
+ if ((vp1394TwoVideoModeType)_videomodes.modes[i] == videomode)
return true;
}
return false;
@@ -672,18 +604,17 @@ vp1394TwoGrabber::isVideoModeSupported(vp1394TwoVideoModeType videomode)
Indicates if the video mode is format 7.
\param videomode : The video mode to check for.
-
+
\return true : If the video mode is scalable (Format 7).
\return false : If the video mode is not Format 7 like.
\sa setVideoMode(), getVideoModeSupported(), setCamera()
*/
-bool
-vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
+bool vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
{
- if (dc1394_is_video_mode_scalable((dc1394video_mode_t) videomode))
+ if (dc1394_is_video_mode_scalable((dc1394video_mode_t)videomode))
return true;
return false;
@@ -693,10 +624,10 @@ vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
Indicates if the active camera is grabbing color or grey images.
- We consider color images if the color coding is either YUV (411, 422, 444) or
- RGB (8, 16, 16S). We consider grey images if the color coding is MONO (8,
- 16, 16S) or RAW (8, 16). vp1394TwoColorCodingType gives the supported color
- codings.
+ We consider color images if the color coding is either YUV (411, 422, 444)
+ or RGB (8, 16, 16S). We consider grey images if the color coding is MONO
+ (8, 16, 16S) or RAW (8, 16). vp1394TwoColorCodingType gives the supported
+ color codings.
\return true : If color images are acquired.
\return false : If grey images are acquired.
@@ -704,26 +635,25 @@ vp1394TwoGrabber::isVideoModeFormat7(vp1394TwoVideoModeType videomode)
\sa getColorCoding(), setCamera()
*/
-bool
-vp1394TwoGrabber::isColor()
+bool vp1394TwoGrabber::isColor()
{
vp1394TwoColorCodingType coding;
getColorCoding(coding);
switch (coding) {
- case vpCOLOR_CODING_MONO8:
- case vpCOLOR_CODING_MONO16:
- case vpCOLOR_CODING_MONO16S:
- case vpCOLOR_CODING_RAW8:
- case vpCOLOR_CODING_RAW16:
- return false;
- case vpCOLOR_CODING_YUV411:
- case vpCOLOR_CODING_YUV422:
- case vpCOLOR_CODING_YUV444:
- case vpCOLOR_CODING_RGB8:
- case vpCOLOR_CODING_RGB16:
- case vpCOLOR_CODING_RGB16S:
- return true;
+ case vpCOLOR_CODING_MONO8:
+ case vpCOLOR_CODING_MONO16:
+ case vpCOLOR_CODING_MONO16S:
+ case vpCOLOR_CODING_RAW8:
+ case vpCOLOR_CODING_RAW16:
+ return false;
+ case vpCOLOR_CODING_YUV411:
+ case vpCOLOR_CODING_YUV422:
+ case vpCOLOR_CODING_YUV444:
+ case vpCOLOR_CODING_RGB8:
+ case vpCOLOR_CODING_RGB16:
+ case vpCOLOR_CODING_RGB16S:
+ return true;
}
return false;
}
@@ -752,14 +682,12 @@ vp1394TwoGrabber::isColor()
\sa getFramerate(), getFramerateSupported() , setCamera()
*/
-void
-vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
+void vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
{
open();
- if (! num_cameras) {
+ if (!num_cameras) {
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No camera found") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No camera found"));
}
vp1394TwoVideoModeType cur_videomode;
@@ -767,20 +695,19 @@ vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
if (isVideoModeFormat7(cur_videomode))
return;
- if (!isFramerateSupported(cur_videomode,fps)){
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Framerate not supported by camera %d",camera_id) );
+ if (!isFramerateSupported(cur_videomode, fps)) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Framerate not supported by camera %d",
+ camera_id));
}
// Stop dma capture if started
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);
@@ -803,25 +730,21 @@ vp1394TwoGrabber::setFramerate(vp1394TwoFramerateType fps)
\sa setFramerate(), getFramerateSupported(), setCamera()
*/
-void
-vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType & fps)
+void vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType &fps)
{
- if (! num_cameras) {
+ 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;
-
+ fps = (vp1394TwoFramerateType)_fps;
}
/*!
@@ -855,56 +778,47 @@ vp1394TwoGrabber::getFramerate(vp1394TwoFramerateType & fps)
\sa setFramerate(), getFramerate(), setCamera()
*/
-uint32_t
-vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
- std::list<vp1394TwoFramerateType> & fps)
+uint32_t vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode, std::list<vp1394TwoFramerateType> &fps)
{
- if (! num_cameras) {
+ 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
fps.clear();
switch (mode) {
- // Framerate not available for:
- // - vpVIDEO_MODE_EXIF ie Format_6
- // - vpVIDEO_MODE_FORMAT7... ie the Format_7
- case vpVIDEO_MODE_EXIF:
- case vpVIDEO_MODE_FORMAT7_0:
- case vpVIDEO_MODE_FORMAT7_1:
- case vpVIDEO_MODE_FORMAT7_2:
- case vpVIDEO_MODE_FORMAT7_3:
- case vpVIDEO_MODE_FORMAT7_4:
- case vpVIDEO_MODE_FORMAT7_5:
- case vpVIDEO_MODE_FORMAT7_6:
- case vpVIDEO_MODE_FORMAT7_7:
+ // Framerate not available for:
+ // - vpVIDEO_MODE_EXIF ie Format_6
+ // - vpVIDEO_MODE_FORMAT7... ie the Format_7
+ case vpVIDEO_MODE_EXIF:
+ case vpVIDEO_MODE_FORMAT7_0:
+ case vpVIDEO_MODE_FORMAT7_1:
+ case vpVIDEO_MODE_FORMAT7_2:
+ case vpVIDEO_MODE_FORMAT7_3:
+ case vpVIDEO_MODE_FORMAT7_4:
+ case vpVIDEO_MODE_FORMAT7_5:
+ case vpVIDEO_MODE_FORMAT7_6:
+ case vpVIDEO_MODE_FORMAT7_7:
+ return 0;
+ break;
+ default: {
+ dc1394framerates_t _fps;
+ if (dc1394_video_get_supported_framerates(camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported frametates for mode %d\n", mode);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported framerates"));
+ }
+ if (_fps.num == 0)
return 0;
- break;
- default:
- {
- dc1394framerates_t _fps;
- if (dc1394_video_get_supported_framerates(camera,
- (dc1394video_mode_t)mode,
- &_fps) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported frametates for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported framerates") );
- }
- if (_fps.num == 0)
- return 0;
- for (unsigned int i = 0; i < _fps.num; i ++)
- fps.push_back((vp1394TwoFramerateType)_fps.framerates[i]);
+ for (unsigned int i = 0; i < _fps.num; i++)
+ fps.push_back((vp1394TwoFramerateType)_fps.framerates[i]);
- return _fps.num;
- }
- break;
+ return _fps.num;
+ } break;
}
}
/*!
@@ -940,55 +854,46 @@ vp1394TwoGrabber::getFramerateSupported(vp1394TwoVideoModeType mode,
supported.
\sa setFramerate(), getFramerate(), setCamera()
*/
-bool
-vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode,
- vp1394TwoFramerateType fps)
+bool vp1394TwoGrabber::isFramerateSupported(vp1394TwoVideoModeType mode, vp1394TwoFramerateType fps)
{
- if (! num_cameras) {
+ 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) {
- // Framerate not available for:
- // - vpVIDEO_MODE_EXIF ie Format_6
- // - vpVIDEO_MODE_FORMAT7... ie the Format_7
- case vpVIDEO_MODE_EXIF:
- case vpVIDEO_MODE_FORMAT7_0:
- case vpVIDEO_MODE_FORMAT7_1:
- case vpVIDEO_MODE_FORMAT7_2:
- case vpVIDEO_MODE_FORMAT7_3:
- case vpVIDEO_MODE_FORMAT7_4:
- case vpVIDEO_MODE_FORMAT7_5:
- case vpVIDEO_MODE_FORMAT7_6:
- case vpVIDEO_MODE_FORMAT7_7:
+ // Framerate not available for:
+ // - vpVIDEO_MODE_EXIF ie Format_6
+ // - vpVIDEO_MODE_FORMAT7... ie the Format_7
+ case vpVIDEO_MODE_EXIF:
+ case vpVIDEO_MODE_FORMAT7_0:
+ case vpVIDEO_MODE_FORMAT7_1:
+ case vpVIDEO_MODE_FORMAT7_2:
+ case vpVIDEO_MODE_FORMAT7_3:
+ case vpVIDEO_MODE_FORMAT7_4:
+ case vpVIDEO_MODE_FORMAT7_5:
+ case vpVIDEO_MODE_FORMAT7_6:
+ case vpVIDEO_MODE_FORMAT7_7:
+ return 0;
+ break;
+ default: {
+ dc1394framerates_t _fps;
+ if (dc1394_video_get_supported_framerates(camera, (dc1394video_mode_t)mode, &_fps) != DC1394_SUCCESS) {
+ close();
+ vpERROR_TRACE("Could not query supported frametates for mode %d\n", mode);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not query supported framerates"));
+ }
+ if (_fps.num == 0)
return 0;
- break;
- default:
- {
- dc1394framerates_t _fps;
- if (dc1394_video_get_supported_framerates(camera,
- (dc1394video_mode_t)mode,
- &_fps) != DC1394_SUCCESS) {
- close();
- vpERROR_TRACE("Could not query supported frametates for mode %d\n",
- mode);
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not query supported framerates") );
- }
- if (_fps.num == 0)
- return 0;
- for (unsigned int i = 0; i < _fps.num; i ++){
- if (fps==(vp1394TwoFramerateType)_fps.framerates[i]){
- return true;
- }
+ for (unsigned int i = 0; i < _fps.num; i++) {
+ if (fps == (vp1394TwoFramerateType)_fps.framerates[i]) {
+ return true;
}
- return false;
}
- break;
+ return false;
+ } break;
}
}
@@ -1040,26 +945,23 @@ int main()
\sa getColorCoding(), getColorCodingSupported() , setCamera()
*/
-void
-vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
+void vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
{
- if (! num_cameras) {
+ 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", camera_id) );
+ if (!isColorCodingSupported((vp1394TwoVideoModeType)_videomode, coding)) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Color coding not supported by camera %d",
+ camera_id));
}
// Format 7 video mode
@@ -1067,13 +969,10 @@ 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);
@@ -1098,53 +997,41 @@ vp1394TwoGrabber::setColorCoding(vp1394TwoColorCodingType coding)
\sa setColorCoding(), getColorCodingSupported(), setCamera()
*/
-void
-vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType & coding)
+void vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType &coding)
{
- if (! num_cameras) {
+ 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 {
+ } 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;
+ coding = (vp1394TwoColorCodingType)_coding;
}
/*!
@@ -1168,15 +1055,13 @@ vp1394TwoGrabber::getColorCoding(vp1394TwoColorCodingType & coding)
\sa setColorCoding(), getColorCoding(), setCamera()
*/
-uint32_t
-vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
- std::list<vp1394TwoColorCodingType> & codings)
+uint32_t vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
+ std::list<vp1394TwoColorCodingType> &codings)
{
- if (! num_cameras) {
+ 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
@@ -1185,38 +1070,28 @@ vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
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;
- for (unsigned int i = 0; i < _codings.num; i ++)
+ for (unsigned int i = 0; i < _codings.num; i++)
codings.push_back((vp1394TwoColorCodingType)_codings.codings[i]);
return _codings.num;
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
+ } else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
// Format 6 video mode
return 0;
- }
- else {
+ } 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;
@@ -1243,64 +1118,49 @@ vp1394TwoGrabber::getColorCodingSupported(vp1394TwoVideoModeType mode,
not supported.
\sa setColorCoding(), getColorCoding(), setCamera()
*/
-bool
-vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode,
- vp1394TwoColorCodingType coding)
+bool vp1394TwoGrabber::isColorCodingSupported(vp1394TwoVideoModeType mode, vp1394TwoColorCodingType coding)
{
- if (! num_cameras) {
+ 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;
- for (unsigned int i = 0; i < _codings.num; i ++){
- if (coding==(vp1394TwoColorCodingType)_codings.codings[i])
+ for (unsigned int i = 0; i < _codings.num; i++) {
+ if (coding == (vp1394TwoColorCodingType)_codings.codings[i])
return true;
}
return false;
- }
- else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
+ } else if (dc1394_is_video_mode_still_image((dc1394video_mode_t)mode)) {
// Format 6 video mode
return false;
- }
- else {
+ } 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)
+ if (coding == (vp1394TwoColorCodingType)_coding)
return true;
return false;
}
}
-
/*!
Set the grabbed region of interest ie roi position and size for format 7
@@ -1332,16 +1192,13 @@ 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) {
+ 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;
@@ -1349,8 +1206,7 @@ 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
@@ -1358,14 +1214,11 @@ 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,
@@ -1376,13 +1229,11 @@ 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;
@@ -1393,8 +1244,7 @@ vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top,
if (w > (max_width - left))
w = (max_width - left);
roi_width = (int32_t)w;
- }
- else {
+ } else {
roi_width = DC1394_USE_MAX_AVAIL;
}
@@ -1403,33 +1253,26 @@ vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top,
if (h > (max_height - top))
h = (max_height - top);
roi_height = (int32_t)h;
- }
- else {
+ } else {
roi_height = DC1394_USE_MAX_AVAIL;
}
if (dc1394_format7_set_roi(camera, _videomode,
- (dc1394color_coding_t) DC1394_QUERY_FROM_CAMERA, // color_coding
- DC1394_USE_MAX_AVAIL/*DC1394_QUERY_FROM_CAMERA*/, // bytes_per_packet
- (int32_t)left, // left
- (int32_t)top, // top
- roi_width,
- roi_height)
- != DC1394_SUCCESS) {
+ (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);
@@ -1450,59 +1293,55 @@ vp1394TwoGrabber::setFormat7ROI(unsigned int left, unsigned int top,
\sa close()
*/
-void
-vp1394TwoGrabber::initialize(bool reset)
+void vp1394TwoGrabber::initialize(bool reset)
{
- if (init == false){
- // Find cameras
+ if (init == false) {
+// Find cameras
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
if (d != NULL)
- dc1394_free (d);
+ dc1394_free(d);
- d = dc1394_new ();
- if (dc1394_camera_enumerate (d, &list) != DC1394_SUCCESS) {
- dc1394_camera_free_list (list);
+ d = dc1394_new();
+ if (dc1394_camera_enumerate(d, &list) != DC1394_SUCCESS) {
+ dc1394_camera_free_list(list);
close();
vpERROR_TRACE("Failed to enumerate cameras\n");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Failed to enumerate cameras") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Failed to enumerate cameras"));
}
if (list->num == 0) {
- dc1394_camera_free_list (list);
+ 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)
- delete [] cameras;
+ delete[] cameras;
- cameras = new dc1394camera_t * [list->num];
+ cameras = new dc1394camera_t *[list->num];
num_cameras = 0;
-
- for (unsigned int i=0; i < list->num; i ++) {
- cameras[i] = dc1394_camera_new (d, list->ids[i].guid);
+
+ 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
- num_cameras ++;
+ num_cameras++;
}
if (reset) {
- // Reset the bus to make firewire working if the program was not properly
- // stopped by a CTRL-C. We reset here only the bus attached to the first
- // camera
+ // Reset the bus to make firewire working if the program was not
+ // properly stopped by a CTRL-C. We reset here only the bus attached to
+ // the first camera
dc1394_reset_bus(cameras[0]);
}
// if (list != NULL)
- dc1394_camera_free_list (list);
+ dc1394_camera_free_list(list);
list = NULL;
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
@@ -1511,28 +1350,26 @@ vp1394TwoGrabber::initialize(bool reset)
cameras = NULL;
int err = dc1394_find_cameras(&cameras, &num_cameras);
- if (err!=DC1394_SUCCESS && err != DC1394_NO_CAMERA) {
+ if (err != DC1394_SUCCESS && err != DC1394_NO_CAMERA) {
close();
vpERROR_TRACE("Unable to look for cameras\n\n"
"Please check \n"
- " - if the kernel modules `ieee1394',`raw1394' and `ohci1394' are loaded \n"
+ " - if 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
isDataModified = new bool[num_cameras];
- for(unsigned int i=0; i<num_cameras; i++)
+ for (unsigned int i = 0; i < num_cameras; i++)
isDataModified[i] = false;
initialShutterMode = new dc1394feature_mode_t[num_cameras];
dataCam = new vpDc1394TwoCameraParametersData[num_cameras];
@@ -1542,20 +1379,20 @@ 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;
}
- if (camIsOpen != NULL) delete [] camIsOpen;
- camIsOpen = new bool [num_cameras];
- for (unsigned int i = 0;i<num_cameras;i++){
- camIsOpen[i]=false;
+ if (camIsOpen != NULL)
+ delete[] camIsOpen;
+ camIsOpen = new bool[num_cameras];
+ for (unsigned int i = 0; i < num_cameras; i++) {
+ camIsOpen[i] = false;
}
init = true;
@@ -1570,38 +1407,40 @@ vp1394TwoGrabber::initialize(bool reset)
\sa close()
*/
-void
-vp1394TwoGrabber::open()
+void vp1394TwoGrabber::open()
{
- if (init == false) initialize(false);
- if (camIsOpen[camera_id] == false){
+ if (init == false)
+ initialize(false);
+ if (camIsOpen[camera_id] == false) {
dc1394switch_t status = DC1394_OFF;
- //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API >
+ // libdc1394-2.0.0-rc7
dc1394_video_get_transmission(cameras[camera_id], &status);
- if (status != DC1394_OFF){
+ 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) {
+ if (status == DC1394_ON) {
vpTRACE("ISO transmission refuses to stop");
}
#ifdef VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
// No yet in the new API
- cameras[camera_id]->is_iso_on=status;
+ cameras[camera_id]->is_iso_on = status;
#endif
}
- //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
+ //#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API >
+ // libdc1394-2.0.0-rc7
}
//#endif
}
setCamera(camera_id);
- //setIsoSpeed(DC1394_ISO_SPEED_400);
+ // setIsoSpeed(DC1394_ISO_SPEED_400);
setCapture(DC1394_ON);
setTransmission(DC1394_ON);
camIsOpen[camera_id] = true;
@@ -1615,38 +1454,35 @@ vp1394TwoGrabber::open()
releases all the cameras.
*/
-void
-vp1394TwoGrabber::close()
+void vp1394TwoGrabber::close()
{
- if (init){
+ if (init) {
if (num_cameras) {
- for (unsigned int i = 0; i < num_cameras;i++) {
+ 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]){
+ if (isDataModified[i]) {
// reset values
- try{
+ try {
updateDataStructToCam();
- }
- catch(...){
+ } 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])){
-
+ 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)
@@ -1660,17 +1496,17 @@ vp1394TwoGrabber::close()
}
}
if (camIsOpen != NULL) {
- delete [] camIsOpen;
+ delete[] camIsOpen;
camIsOpen = NULL;
}
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
if (cameras != NULL) {
- delete [] cameras;
+ delete[] cameras;
cameras = NULL;
}
if (d != NULL) {
- dc1394_free (d);
+ dc1394_free(d);
d = NULL;
}
@@ -1683,20 +1519,20 @@ vp1394TwoGrabber::close()
camIsOpen = NULL;
num_cameras = 0;
-
- // remove data for the parameters
- if(isDataModified != NULL){
+
+ // remove data for the parameters
+ if (isDataModified != NULL) {
delete[] isDataModified;
isDataModified = NULL;
}
- if(initialShutterMode != NULL){
+ if (initialShutterMode != NULL) {
delete[] initialShutterMode;
initialShutterMode = NULL;
}
- if(dataCam != NULL){
+ if (dataCam != NULL) {
delete[] dataCam;
dataCam = NULL;
- }
+ }
init = false;
}
@@ -1709,26 +1545,25 @@ vp1394TwoGrabber::close()
\param size : Ring buffer size.
- \exception vpFrameGrabberException::settingError : If ring buffer size is not valid.
+ \exception vpFrameGrabberException::settingError : If ring buffer size is
+ not valid.
\sa getRingBufferSize()
*/
-void
-vp1394TwoGrabber::setRingBufferSize(unsigned int size)
+void vp1394TwoGrabber::setRingBufferSize(unsigned int size)
{
- if (size < 1) {
+ 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) {
// We need to change the ring buffer size
num_buffers = size;
- if(camIsOpen[camera_id]){
+ if (camIsOpen[camera_id]) {
setCapture(DC1394_OFF);
setCapture(DC1394_ON);
- }
+ }
}
}
@@ -1741,19 +1576,15 @@ 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
and max exposure time, but only for AVT cameras. In that case
use setAutoShutter(unsigned int, unsigned int).
- \param enable : Flag to enable or disable the auto shutter. If false, set the
- shutter as manual.
+ \param enable : Flag to enable or disable the auto shutter. If false, set
+the shutter as manual.
\exception vpFrameGrabberException::initializationError : If no
camera found on the bus.
@@ -1786,45 +1617,36 @@ int main()
\sa setAutoShutter(unsigned int, unsigned int), getAutoShutter()
*/
-void
-vp1394TwoGrabber::setAutoShutter(bool enable)
+void vp1394TwoGrabber::setAutoShutter(bool enable)
{
- if (! num_cameras) {
+ 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;
if (enable) {
mode = DC1394_FEATURE_MODE_AUTO;
- }
- else {
+ } else {
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"));
}
}
/*!
Set auto shutter. If available set min and max exposure time.
-
+
\warning Setting min and max exposure time feature is only available
for AVT cameras.
@@ -1862,17 +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) {
- // vpERROR_TRACE("Cannot set auto shutter min and max values. Is the camera an AVT one?\n");
+ 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"));
}
}
@@ -1888,22 +1708,19 @@ vp1394TwoGrabber::setAutoShutter(unsigned int minvalue, unsigned int maxvalue)
\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) {
+ 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) {
- // vpERROR_TRACE("Cannot get auto shutter min and max values. Is the camera an AVT one?\n");
+ 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"));
}
}
@@ -1946,40 +1763,31 @@ int main()
\sa setAutoGain(unsigned int, unsigned int), getAutoGain()
*/
-void
-vp1394TwoGrabber::setAutoGain(bool enable)
+void vp1394TwoGrabber::setAutoGain(bool enable)
{
- if (! num_cameras) {
+ 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;
if (enable) {
mode = DC1394_FEATURE_MODE_AUTO;
- }
- else {
+ } else {
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"));
}
}
/*!
@@ -2022,17 +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) {
- // vpERROR_TRACE("Cannot set auto gain min and max values. Is the camera an AVT one?\n");
+ 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"));
}
}
@@ -2048,26 +1854,22 @@ vp1394TwoGrabber::setAutoGain(unsigned int minvalue, unsigned int maxvalue)
\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) {
+ 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) {
- // vpERROR_TRACE("Cannot get auto gain min and max values. Is the camera an AVT one?\n");
+ 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"));
}
}
-
/*!
Setup camera capture using dma. A ring buffer is used for the
@@ -2085,42 +1887,35 @@ vp1394TwoGrabber::getAutoGain(unsigned int &minvalue, unsigned int &maxvalue)
\sa setRingBufferSize(), setVideoMode(), setFramerate()
*/
-void
-vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
+void vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
{
- if (! num_cameras) {
+ 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) {
+ // 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
+ } else { // _switch == DC1394_OFF
dc1394error_t code = dc1394_capture_stop(camera);
if (code != DC1394_SUCCESS && code != DC1394_CAPTURE_IS_NOT_SET) {
vpERROR_TRACE("Unable to stop camera capture\n");
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Could not setup dma capture") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Could not setup dma capture"));
}
}
}
-
/*!
Setup camera transmission.
@@ -2135,23 +1930,20 @@ vp1394TwoGrabber::setCapture(dc1394switch_t _switch)
\exception vpFrameGrabberException::settingError : If we can't set
the video mode.
*/
-void
-vp1394TwoGrabber::setTransmission(dc1394switch_t _switch)
+void vp1394TwoGrabber::setTransmission(dc1394switch_t _switch)
{
- if (! num_cameras) {
+ 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;
- if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
+ 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){
@@ -2161,21 +1953,19 @@ 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) {
status = DC1394_OFF;
int i = 0;
- while ( status == DC1394_OFF && i++ < 5 ) {
+ while (status == DC1394_OFF && i++ < 5) {
usleep(50000);
- if (dc1394_video_get_transmission(camera, &status)!=DC1394_SUCCESS) {
+ 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"));
}
}
}
@@ -2216,77 +2006,62 @@ int main()
}
\endcode
*/
-void
-vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
+void vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
{
- if (! num_cameras) {
+ 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;
+ dc1394operation_mode_t op_mode;
dc1394speed_t speed;
// Check the speed to configure in B-mode or A-mode
if (isospeed >= vpISO_SPEED_800) {
if (camera->bmode_capable != DC1394_TRUE) {
-// vpERROR_TRACE("Camera is not 1394B mode capable. \n"
-// "Set the iso speed lower or equal to 400Mbps");
+ // 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) {
-// vpERROR_TRACE("Cannot set camera to 1394B mode. \n");
+ 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");
+ // 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) {
-// vpERROR_TRACE("Cannot set camera to 1394A mode. \n");
+ } else {
+ if (dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_LEGACY) != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set camera to 1394A mode. \n");
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set camera to 1394A mode") );
+ 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");
+ // 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) {
-// vpERROR_TRACE("Cannot set requested iso speed. \n");
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Cannot set requested iso speed") );
+ if (dc1394_video_set_iso_speed(camera, (dc1394speed_t)isospeed) != DC1394_SUCCESS) {
+ // vpERROR_TRACE("Cannot set requested iso speed. \n");
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Cannot set requested iso speed"));
}
if (dc1394_video_get_iso_speed(camera, &speed) != DC1394_SUCCESS) {
-// vpERROR_TRACE("Failed to set iso speed. \n");
+ // 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"));
+ }
}
/*!
@@ -2299,8 +2074,7 @@ vp1394TwoGrabber::setIsoTransmissionSpeed(vp1394TwoIsoSpeedType isospeed)
\sa acquire(vpImage<unsigned char> &I)
*/
-void
-vp1394TwoGrabber::open(vpImage<unsigned char> &I)
+void vp1394TwoGrabber::open(vpImage<unsigned char> &I)
{
open();
acquire(I);
@@ -2316,14 +2090,12 @@ vp1394TwoGrabber::open(vpImage<unsigned char> &I)
\sa acquire(vpImage<vpRGBa> &I)
*/
-void
-vp1394TwoGrabber::open(vpImage<vpRGBa> &I)
+void vp1394TwoGrabber::open(vpImage<vpRGBa> &I)
{
open();
acquire(I);
}
-
/*!
Get an image from the active camera frame buffer. This buffer neads to be
@@ -2362,22 +2134,19 @@ int main()
\sa enqueue()
*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue()
+dc1394video_frame_t *vp1394TwoGrabber::dequeue()
{
- if (! num_cameras) {
+ 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) {
- vpERROR_TRACE ("Error: Failed to capture from camera %d\n", camera_id);
+ if (dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_WAIT, &frame) != DC1394_SUCCESS) {
+ vpERROR_TRACE("Error: Failed to capture from camera %d\n", camera_id);
}
return frame;
@@ -2424,12 +2193,11 @@ int main()
\sa enqueue()
*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I)
+dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I)
{
uint64_t timestamp;
uint32_t id;
-
+
dc1394video_frame_t *frame;
frame = dequeue(I, timestamp, id);
@@ -2485,10 +2253,7 @@ int main()
\sa enqueue()
*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I,
- uint64_t ×tamp,
- uint32_t &id)
+dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id)
{
open();
@@ -2502,52 +2267,44 @@ vp1394TwoGrabber::dequeue(vpImage<unsigned char> &I,
timestamp = frame->timestamp;
id = frame->id;
- this->width = frame->size[0];
+ this->width = frame->size[0];
this->height = frame->size[1];
unsigned int size = this->width * this->height;
- if ((I.getWidth() != this->width)||(I.getHeight() != this->height))
+ if ((I.getWidth() != this->width) || (I.getHeight() != this->height))
I.resize(this->height, this->width);
- switch(frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- memcpy(I.bitmap, (unsigned char *) frame->image,
- size*sizeof(unsigned char));
- break;
- case DC1394_COLOR_CODING_MONO16:
- case DC1394_COLOR_CODING_RAW16:
- vpImageConvert::MONO16ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToGrey( (unsigned char *) frame->image,
- I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToGrey((unsigned char *) frame->image, I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
+ switch (frame->color_coding) {
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ memcpy(I.bitmap, (unsigned char *)frame->image, size * sizeof(unsigned char));
+ break;
+ case DC1394_COLOR_CODING_MONO16:
+ case DC1394_COLOR_CODING_RAW16:
+ vpImageConvert::MONO16ToGrey((unsigned char *)frame->image, I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToGrey((unsigned char *)frame->image, I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToGrey((unsigned char *)frame->image, I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToGrey((unsigned char *)frame->image, I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToGrey((unsigned char *)frame->image, I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
+ "Acquisition failed."));
};
return frame;
@@ -2593,12 +2350,11 @@ int main()
\sa enqueue()
*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I)
+dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I)
{
uint64_t timestamp;
uint32_t id;
-
+
dc1394video_frame_t *frame;
frame = dequeue(I, timestamp, id);
@@ -2654,10 +2410,7 @@ int main()
\sa enqueue()
*/
-dc1394video_frame_t *
-vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I,
- uint64_t ×tamp,
- uint32_t &id)
+dc1394video_frame_t *vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id)
{
open();
@@ -2671,54 +2424,45 @@ vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I,
timestamp = frame->timestamp;
id = frame->id;
- this->width = frame->size[0];
+ this->width = frame->size[0];
this->height = frame->size[1];
unsigned int size = this->width * this->height;
- if ((I.getWidth() != width)||(I.getHeight() != height))
+ if ((I.getWidth() != width) || (I.getHeight() != height))
I.resize(height, width);
switch (frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_MONO16:
- case DC1394_COLOR_CODING_RAW16:
- vpImageConvert::MONO16ToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ vpImageConvert::GreyToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_MONO16:
+ case DC1394_COLOR_CODING_RAW16:
+ vpImageConvert::MONO16ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
+ "Acquisition failed."));
};
return frame;
@@ -2734,22 +2478,19 @@ vp1394TwoGrabber::dequeue(vpImage<vpRGBa> &I,
\sa dequeue()
*/
-void
-vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
+void vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
{
- if (! num_cameras) {
+ 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)
dc1394_capture_enqueue(camera, frame);
}
-
/*!
Acquire a grey level image from the active camera.
@@ -2763,12 +2504,11 @@ vp1394TwoGrabber::enqueue(dc1394video_frame_t *frame)
\sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
*/
-void
-vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
+void vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
{
uint64_t timestamp;
uint32_t id;
-
+
dc1394video_frame_t *frame;
frame = dequeue(I, timestamp, id);
@@ -2793,10 +2533,7 @@ vp1394TwoGrabber::acquire(vpImage<unsigned char> &I)
\sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
*/
-void
-vp1394TwoGrabber::acquire(vpImage<unsigned char> &I,
- uint64_t ×tamp,
- uint32_t &id)
+void vp1394TwoGrabber::acquire(vpImage<unsigned char> &I, uint64_t ×tamp, uint32_t &id)
{
dc1394video_frame_t *frame;
@@ -2805,8 +2542,6 @@ vp1394TwoGrabber::acquire(vpImage<unsigned char> &I,
enqueue(frame);
}
-
-
/*!
Acquire a color image from the active camera.
@@ -2820,13 +2555,12 @@ vp1394TwoGrabber::acquire(vpImage<unsigned char> &I,
\sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
*/
-void
-vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
+void vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
{
uint64_t timestamp;
uint32_t id;
dc1394video_frame_t *frame;
-
+
open();
frame = dequeue(I, timestamp, id);
enqueue(frame);
@@ -2850,10 +2584,7 @@ vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I)
\sa setCamera(), setVideoMode(), setFramerate(), dequeue(), enqueue()
*/
-void
-vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I,
- uint64_t ×tamp,
- uint32_t &id)
+void vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I, uint64_t ×tamp, uint32_t &id)
{
dc1394video_frame_t *frame;
@@ -2864,52 +2595,43 @@ vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I,
timestamp = frame->timestamp;
id = frame->id;
- this->width = frame->size[0];
+ this->width = frame->size[0];
this->height = frame->size[1];
unsigned int size = this->width * this->height;
- if ((I.getWidth() != width)||(I.getHeight() != height))
+ if ((I.getWidth() != width) || (I.getHeight() != height))
I.resize(height, width);
switch (frame->color_coding) {
- case DC1394_COLOR_CODING_MONO8:
- case DC1394_COLOR_CODING_RAW8:
- vpImageConvert::GreyToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV411:
- vpImageConvert::YUV411ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV422:
- vpImageConvert::YUV422ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_YUV444:
- vpImageConvert::YUV444ToRGBa( (unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
- case DC1394_COLOR_CODING_RGB8:
- vpImageConvert::RGBToRGBa((unsigned char *) frame->image,
- (unsigned char *) I.bitmap, size);
- break;
-
-
- default:
- close();
- vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Format conversion not implemented. "
- "Acquisition failed.") );
- break;
+ case DC1394_COLOR_CODING_MONO8:
+ case DC1394_COLOR_CODING_RAW8:
+ vpImageConvert::GreyToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV411:
+ vpImageConvert::YUV411ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV422:
+ vpImageConvert::YUV422ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_YUV444:
+ vpImageConvert::YUV444ToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ case DC1394_COLOR_CODING_RGB8:
+ vpImageConvert::RGBToRGBa((unsigned char *)frame->image, (unsigned char *)I.bitmap, size);
+ break;
+
+ default:
+ close();
+ vpERROR_TRACE("Format conversion not implemented. Acquisition failed.");
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Format conversion not implemented. "
+ "Acquisition failed."));
};
enqueue(frame);
-
}
/*!
@@ -2930,11 +2652,10 @@ vp1394TwoGrabber::acquire(vpImage<vpRGBa> &I,
*/
void vp1394TwoGrabber::getWidth(unsigned int &w)
{
- if (! num_cameras) {
+ 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;
@@ -2945,7 +2666,8 @@ void vp1394TwoGrabber::getWidth(unsigned int &w)
Get the image width. It depends on the camera video mode setVideoMode(). The
image size is only available after a call to open() or acquire().
- \return width : The image width, zero if the required camera is not available.
+ \return width : The image width, zero if the required camera is not
+ available.
\exception vpFrameGrabberException::initializationError : If no
camera found on the bus.
@@ -2958,11 +2680,10 @@ void vp1394TwoGrabber::getWidth(unsigned int &w)
*/
unsigned int vp1394TwoGrabber::getWidth()
{
- if (! num_cameras) {
+ 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;
@@ -2987,11 +2708,10 @@ unsigned int vp1394TwoGrabber::getWidth()
*/
void vp1394TwoGrabber::getHeight(unsigned int &h)
{
- if (! num_cameras) {
+ 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;
@@ -3015,11 +2735,10 @@ void vp1394TwoGrabber::getHeight(unsigned int &h)
*/
unsigned int vp1394TwoGrabber::getHeight()
{
- if (! num_cameras) {
+ 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;
@@ -3030,19 +2749,16 @@ unsigned int vp1394TwoGrabber::getHeight()
\sa setCamera()
*/
-void
-vp1394TwoGrabber::printCameraInfo()
+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);
+ dc1394_camera_print_info(camera, stdout);
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
- dc1394_print_camera_info( camera);
+ dc1394_print_camera_info(camera);
#endif
dc1394featureset_t features;
@@ -3054,8 +2770,7 @@ 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
@@ -3082,15 +2797,12 @@ vp1394TwoGrabber::printCameraInfo()
std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode)
{
std::string _str = "";
- dc1394video_mode_t _videomode = (dc1394video_mode_t) videomode;
+ 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;
+ } else {
+ vpCERROR << "The video mode " << (int)videomode << " is not supported by the camera" << std::endl;
}
return _str;
@@ -3111,15 +2823,12 @@ std::string vp1394TwoGrabber::videoMode2string(vp1394TwoVideoModeType videomode)
std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
{
std::string _str = "";
- dc1394framerate_t _fps = (dc1394framerate_t) fps;
+ dc1394framerate_t _fps = (dc1394framerate_t)fps;
- if ((_fps >= DC1394_FRAMERATE_MIN)
- && (_fps <= DC1394_FRAMERATE_MAX)) {
+ 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;
+ } else {
+ vpCERROR << "The framerate " << (int)fps << " is not supported by the camera" << std::endl;
}
return _str;
@@ -3127,8 +2836,8 @@ std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
/*!
- Converts the color coding identifier into a string containing the description
- of the color coding.
+ Converts the color coding identifier into a string containing the
+ description of the color coding.
\param colorcoding : The color coding format.
@@ -3140,16 +2849,13 @@ std::string vp1394TwoGrabber::framerate2string(vp1394TwoFramerateType fps)
std::string vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorcoding)
{
std::string _str = "";
- dc1394color_coding_t _coding = (dc1394color_coding_t) colorcoding;
+ 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;
+ } else {
+ vpCERROR << "The color coding " << (int)colorcoding << " is not supported by the camera" << std::endl;
}
return _str;
@@ -3172,24 +2878,21 @@ std::string vp1394TwoGrabber::colorCoding2string(vp1394TwoColorCodingType colorc
\sa videoMode2string()
*/
-vp1394TwoGrabber::vp1394TwoVideoModeType
-vp1394TwoGrabber::string2videoMode(std::string videomode)
+vp1394TwoGrabber::vp1394TwoVideoModeType vp1394TwoGrabber::string2videoMode(std::string videomode)
{
vp1394TwoVideoModeType _id;
- for (int i = DC1394_VIDEO_MODE_MIN; i <= DC1394_VIDEO_MODE_MAX; i ++) {
- _id = (vp1394TwoVideoModeType) i;
+ for (int i = DC1394_VIDEO_MODE_MIN; i <= DC1394_VIDEO_MODE_MAX; i++) {
+ _id = (vp1394TwoVideoModeType)i;
if (videomode.compare(videoMode2string(_id)) == 0)
return _id;
};
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required videomode is not valid") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required videomode is not valid"));
- return (vp1394TwoVideoModeType) 0;
+ return (vp1394TwoVideoModeType)0;
}
-
/*!
Converts the string containing the description of the framerate into the
@@ -3207,21 +2910,19 @@ vp1394TwoGrabber::string2videoMode(std::string videomode)
\sa framerate2string()
*/
-vp1394TwoGrabber::vp1394TwoFramerateType
-vp1394TwoGrabber::string2framerate(std::string framerate)
+vp1394TwoGrabber::vp1394TwoFramerateType vp1394TwoGrabber::string2framerate(std::string framerate)
{
vp1394TwoFramerateType _id;
- for (int i = DC1394_FRAMERATE_MIN; i <= DC1394_FRAMERATE_MAX; i ++) {
- _id = (vp1394TwoFramerateType) i;
+ for (int i = DC1394_FRAMERATE_MIN; i <= DC1394_FRAMERATE_MAX; i++) {
+ _id = (vp1394TwoFramerateType)i;
if (framerate.compare(framerate2string(_id)) == 0)
return _id;
};
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required framerate is not valid") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required framerate is not valid"));
- return (vp1394TwoFramerateType) 0;
+ return (vp1394TwoFramerateType)0;
}
/*!
@@ -3241,30 +2942,28 @@ vp1394TwoGrabber::string2framerate(std::string framerate)
\sa colorCoding2string()
*/
-vp1394TwoGrabber::vp1394TwoColorCodingType
-vp1394TwoGrabber::string2colorCoding(std::string colorcoding)
+vp1394TwoGrabber::vp1394TwoColorCodingType vp1394TwoGrabber::string2colorCoding(std::string colorcoding)
{
vp1394TwoColorCodingType _id;
- for (int i = DC1394_COLOR_CODING_MIN; i <= DC1394_COLOR_CODING_MAX; i ++) {
- _id = (vp1394TwoColorCodingType) i;
+ for (int i = DC1394_COLOR_CODING_MIN; i <= DC1394_COLOR_CODING_MAX; i++) {
+ _id = (vp1394TwoColorCodingType)i;
if (colorcoding.compare(colorCoding2string(_id)) == 0)
return _id;
};
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "The required color coding is not valid") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The required color coding is not valid"));
- return (vp1394TwoColorCodingType) 0;
+ return (vp1394TwoColorCodingType)0;
}
/*!
- Resets the IEEE1394 bus which camera is attached to. Calling this function is
- "rude" to other devices because it causes them to re-enumerate on the bus and
- may cause a temporary disruption in their current activities. Thus, use it
- sparingly. Its primary use is if a program shuts down uncleanly and needs to
- free leftover ISO channels or bandwidth. A bus reset will free those things
- as a side effect.
+ Resets the IEEE1394 bus which camera is attached to. Calling this function
+is "rude" to other devices because it causes them to re-enumerate on the bus
+and may cause a temporary disruption in their current activities. Thus, use
+it sparingly. Its primary use is if a program shuts down uncleanly and needs
+to free leftover ISO channels or bandwidth. A bus reset will free those
+things as a side effect.
The example below shows how to reset the bus attached to the last
camera found.
@@ -3292,7 +2991,7 @@ int main()
*/
void vp1394TwoGrabber::resetBus()
{
- for (unsigned int i = 0; i < num_cameras;i++) {
+ for (unsigned int i = 0; i < num_cameras; i++) {
if (camIsOpen[i]) {
camera = cameras[i];
setTransmission(DC1394_OFF);
@@ -3302,25 +3001,27 @@ void vp1394TwoGrabber::resetBus()
#ifdef VISP_HAVE_DC1394_CAMERA_ENUMERATE // new API > libdc1394-2.0.0-rc7
setCamera(camera_id);
// free the other cameras
- for (unsigned int i=0;i<num_cameras;i++){
- if (i!=camera_id) dc1394_camera_free(cameras[i]);
+ for (unsigned int i = 0; i < num_cameras; i++) {
+ if (i != camera_id)
+ dc1394_camera_free(cameras[i]);
}
- printf ("Reseting bus...\n");
- dc1394_reset_bus (camera);
+ printf("Reseting bus...\n");
+ dc1394_reset_bus(camera);
- dc1394_camera_free (camera);
- dc1394_free (d);
+ dc1394_camera_free(camera);
+ dc1394_free(d);
d = NULL;
- //if (cameras != NULL)
- delete [] cameras;
- cameras = NULL ;
+ // if (cameras != NULL)
+ delete[] cameras;
+ cameras = NULL;
#elif defined VISP_HAVE_DC1394_FIND_CAMERAS // old API <= libdc1394-2.0.0-rc7
setCamera(camera_id);
// free the other cameras
- for (unsigned int i=0;i<num_cameras;i++){
- if (i!=camera_id) dc1394_free_camera(cameras[i]);
+ for (unsigned int i = 0; i < num_cameras; i++) {
+ if (i != camera_id)
+ dc1394_free_camera(cameras[i]);
}
free(cameras);
cameras = NULL;
@@ -3330,8 +3031,8 @@ void vp1394TwoGrabber::resetBus()
#endif
if (camIsOpen != NULL)
- delete [] camIsOpen;
- camIsOpen = NULL ;
+ delete[] camIsOpen;
+ camIsOpen = NULL;
num_cameras = 0;
@@ -3362,21 +3063,21 @@ void vp1394TwoGrabber::resetBus()
sensors are capturing images. When \e panControlValue is set to 0,
transmit right-left images, when set to 1, transmit right-center
images.
-
+
\exception vpFrameGrabberException::initializationError : If no
camera found on the bus.
- \exception vpFrameGrabberException::settingError : If the register was not set.
+ \exception vpFrameGrabberException::settingError : If the register was not
+ set.
*/
void vp1394TwoGrabber::setPanControl(unsigned int panControlValue)
{
open();
- if (! num_cameras) {
+ 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;
@@ -3385,62 +3086,77 @@ 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"));
}
}
-
/*!
This method get the value of one of the parameters of the camera.
-
- \param param : The parameter expressing the value to return
-
+
+ \param param : The parameter expressing the value to return
+
\return the parameter's value
-
+
\exception vpFrameGrabberException::initializationError : If no
camera found on the bus.
- \exception vpFrameGrabberException::settingError : if the parameter cannot be
- retrieved.
-
+ \exception vpFrameGrabberException::settingError : if the parameter cannot
+ be retrieved.
+
\sa setParameterValue()
-*/
+*/
unsigned int vp1394TwoGrabber::getParameterValue(vp1394TwoParametersType param)
{
- if (! num_cameras) {
+ 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;
- switch(param) {
- case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; break;
- case vpFEATURE_EXPOSURE: feature = DC1394_FEATURE_EXPOSURE; break;
- case vpFEATURE_SHARPNESS: feature = DC1394_FEATURE_SHARPNESS; break;
-// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
- case vpFEATURE_HUE: feature = DC1394_FEATURE_HUE; break;
- case vpFEATURE_SATURATION: feature = DC1394_FEATURE_SATURATION; break;
- case vpFEATURE_GAMMA: feature = DC1394_FEATURE_GAMMA; break;
- case vpFEATURE_SHUTTER: feature = DC1394_FEATURE_SHUTTER; break;
- case vpFEATURE_GAIN: feature = DC1394_FEATURE_GAIN; break;
- case vpFEATURE_IRIS: feature = DC1394_FEATURE_IRIS; break;
-// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
-// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
-// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
-// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
-// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
-// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
-// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
-// vpFEATURE_PAN = DC1394_FEATURE_PAN,
-// vpFEATURE_TILT = DC1394_FEATURE_TILT,
-// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
-// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
-// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param;
+ switch (param) {
+ case vpFEATURE_BRIGHTNESS:
+ feature = DC1394_FEATURE_BRIGHTNESS;
+ break;
+ case vpFEATURE_EXPOSURE:
+ feature = DC1394_FEATURE_EXPOSURE;
+ break;
+ case vpFEATURE_SHARPNESS:
+ feature = DC1394_FEATURE_SHARPNESS;
+ break;
+ // vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ case vpFEATURE_HUE:
+ feature = DC1394_FEATURE_HUE;
+ break;
+ case vpFEATURE_SATURATION:
+ feature = DC1394_FEATURE_SATURATION;
+ break;
+ case vpFEATURE_GAMMA:
+ feature = DC1394_FEATURE_GAMMA;
+ break;
+ case vpFEATURE_SHUTTER:
+ feature = DC1394_FEATURE_SHUTTER;
+ break;
+ case vpFEATURE_GAIN:
+ feature = DC1394_FEATURE_GAIN;
+ break;
+ case vpFEATURE_IRIS:
+ feature = DC1394_FEATURE_IRIS;
+ break;
+ // vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+ // vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+ // vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+ // vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+ // vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+ // vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+ // vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+ // vpFEATURE_PAN = DC1394_FEATURE_PAN,
+ // vpFEATURE_TILT = DC1394_FEATURE_TILT,
+ // vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+ // vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+ // vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
}
dc1394error_t err;
@@ -3448,124 +3164,135 @@ unsigned int 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;
}
-
/*!
- This method set the value of one of the parameters of the camera. The initial
- value of the parameter is recorded and reset when the destructor is called.
-
- \warning If the program crashes and the destructor is not called, then the
+ This method set the value of one of the parameters of the camera. The
+ initial value of the parameter is recorded and reset when the destructor is
+ called.
+
+ \warning If the program crashes and the destructor is not called, then the
modified parameters will remain in the camera.
-
+
\param param : The parameters to modify
\param val : the new value of this parameter
\exception vpFrameGrabberException::initializationError : If no
camera found on the bus.
- \exception vpFrameGrabberException::settingError if a manual mode is not
+ \exception vpFrameGrabberException::settingError if a manual mode is not
available for the parameter, an exception is thrown.
-
+
\sa getParameterValue()
-*/
-void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param,
- unsigned int val)
+*/
+void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param, unsigned int val)
{
- if (! num_cameras) {
+ 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;
- switch(param) {
- case vpFEATURE_BRIGHTNESS: feature = DC1394_FEATURE_BRIGHTNESS; break;
- case vpFEATURE_EXPOSURE: feature = DC1394_FEATURE_EXPOSURE; break;
- case vpFEATURE_SHARPNESS: feature = DC1394_FEATURE_SHARPNESS; break;
-// vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
- case vpFEATURE_HUE: feature = DC1394_FEATURE_HUE; break;
- case vpFEATURE_SATURATION: feature = DC1394_FEATURE_SATURATION; break;
- case vpFEATURE_GAMMA: feature = DC1394_FEATURE_GAMMA; break;
- case vpFEATURE_SHUTTER: feature = DC1394_FEATURE_SHUTTER; break;
- case vpFEATURE_GAIN: feature = DC1394_FEATURE_GAIN; break;
- case vpFEATURE_IRIS: feature = DC1394_FEATURE_IRIS; break;
-// vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
-// vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
-// vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
-// vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
-// vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
-// vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
-// vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
-// vpFEATURE_PAN = DC1394_FEATURE_PAN,
-// vpFEATURE_TILT = DC1394_FEATURE_TILT,
-// vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
-// vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
-// vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
+ dc1394feature_t feature = DC1394_FEATURE_BRIGHTNESS; // = (dc1394feature_t)param;
+ switch (param) {
+ case vpFEATURE_BRIGHTNESS:
+ feature = DC1394_FEATURE_BRIGHTNESS;
+ break;
+ case vpFEATURE_EXPOSURE:
+ feature = DC1394_FEATURE_EXPOSURE;
+ break;
+ case vpFEATURE_SHARPNESS:
+ feature = DC1394_FEATURE_SHARPNESS;
+ break;
+ // vpFEATURE_WHITE_BALANCE = DC1394_FEATURE_WHITE_BALANCE,
+ case vpFEATURE_HUE:
+ feature = DC1394_FEATURE_HUE;
+ break;
+ case vpFEATURE_SATURATION:
+ feature = DC1394_FEATURE_SATURATION;
+ break;
+ case vpFEATURE_GAMMA:
+ feature = DC1394_FEATURE_GAMMA;
+ break;
+ case vpFEATURE_SHUTTER:
+ feature = DC1394_FEATURE_SHUTTER;
+ break;
+ case vpFEATURE_GAIN:
+ feature = DC1394_FEATURE_GAIN;
+ break;
+ case vpFEATURE_IRIS:
+ feature = DC1394_FEATURE_IRIS;
+ break;
+ // vpFEATURE_FOCUS = DC1394_FEATURE_FOCUS,
+ // vpFEATURE_TEMPERATURE = DC1394_FEATURE_TEMPERATURE,
+ // vpFEATURE_TRIGGER = DC1394_FEATURE_TRIGGER,
+ // vpFEATURE_TRIGGER_DELAY = DC1394_FEATURE_TRIGGER_DELAY,
+ // vpFEATURE_WHITE_SHADING = DC1394_FEATURE_WHITE_SHADING,
+ // vpFEATURE_FRAME_RATE = DC1394_FEATURE_FRAME_RATE,
+ // vpFEATURE_ZOOM = DC1394_FEATURE_ZOOM,
+ // vpFEATURE_PAN = DC1394_FEATURE_PAN,
+ // vpFEATURE_TILT = DC1394_FEATURE_TILT,
+ // vpFEATURE_OPTICAL_FILTER = DC1394_FEATURE_OPTICAL_FILTER,
+ // vpFEATURE_CAPTURE_SIZE = DC1394_FEATURE_CAPTURE_SIZE,
+ // vpFEATURE_CAPTURE_QUALITY = DC1394_FEATURE_CAPTURE_QUALITY
}
dc1394error_t err;
dc1394bool_t hasManualMode = DC1394_FALSE;
dc1394feature_modes_t modesAvailable;
-
+
// test wether we can set the shutter value (manual mode available or not)
- err = dc1394_feature_get_modes(camera, feature, &modesAvailable);
+ err = dc1394_feature_get_modes(camera, feature, &modesAvailable);
if (err != DC1394_SUCCESS) {
vpERROR_TRACE("Unable to detect the manual mode information");
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to detect the manual mode information"));
+ throw(
+ vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to detect the manual mode information"));
}
-
- for(unsigned int i=0; i<modesAvailable.num; i++){
- if(modesAvailable.modes[i] == DC1394_FEATURE_MODE_MANUAL){
+
+ for (unsigned int i = 0; i < modesAvailable.num; i++) {
+ if (modesAvailable.modes[i] == DC1394_FEATURE_MODE_MANUAL) {
hasManualMode = DC1394_TRUE;
}
- }
-
- if(hasManualMode == DC1394_TRUE){
-
- if(!isDataModified[camera_id]){// to ensure we save the first mode even after several set
+ }
+
+ if (hasManualMode == DC1394_TRUE) {
+
+ if (!isDataModified[camera_id]) { // to ensure we save the first mode
+ // even after several set
/* we update the structure */
updateDataCamToStruct();
err = dc1394_feature_get_mode(camera, feature, &(initialShutterMode[camera_id]));
if (err != DC1394_SUCCESS) {
vpERROR_TRACE("Unable to get the initial mode");
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to get the initial mode"));
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Unable to get the initial mode"));
}
isDataModified[camera_id] = true;
}
-
+
dc1394feature_mode_t manualMode = DC1394_FEATURE_MODE_MANUAL;
err = dc1394_feature_set_mode(camera, feature, manualMode);
if (err != DC1394_SUCCESS) {
vpERROR_TRACE("Unable to set the muanual mode");
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Unable to set the manual mode") );
- }
+ 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{
+ } 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"));
- }
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "The camera does not have a manual mode"));
+ }
}
/*!
@@ -3574,14 +3301,12 @@ void vp1394TwoGrabber::setParameterValue(vp1394TwoParametersType param,
\sa setCamera(), getCamera()
*/
-void
-vp1394TwoGrabber::getGuid(uint64_t & guid)
+void vp1394TwoGrabber::getGuid(uint64_t &guid)
{
- if (! num_cameras) {
+ 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;
@@ -3594,26 +3319,22 @@ vp1394TwoGrabber::getGuid(uint64_t & guid)
\sa setCamera(), getCamera()
*/
-uint64_t
-vp1394TwoGrabber::getGuid()
+uint64_t vp1394TwoGrabber::getGuid()
{
- if (! num_cameras) {
+ 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;
}
-
/*!
update the data structure used to record the value of the current camera.
-
+
*/
-inline void
-vp1394TwoGrabber::updateDataCamToStruct()
+inline void vp1394TwoGrabber::updateDataCamToStruct()
{
dataCam[camera_id].brightness = getParameterValue(vpFEATURE_BRIGHTNESS);
dataCam[camera_id].exposure = getParameterValue(vpFEATURE_EXPOSURE);
@@ -3627,11 +3348,10 @@ vp1394TwoGrabber::updateDataCamToStruct()
}
/*!
- set the values of several parameters of the current camera with the value
+ set the values of several parameters of the current camera with the value
previously recorded.
*/
-inline void
-vp1394TwoGrabber::updateDataStructToCam()
+inline void vp1394TwoGrabber::updateDataStructToCam()
{
setParameterValue(vpFEATURE_BRIGHTNESS, dataCam[camera_id].brightness);
setParameterValue(vpFEATURE_EXPOSURE, dataCam[camera_id].exposure);
@@ -3689,7 +3409,7 @@ vp1394TwoGrabber &vp1394TwoGrabber::operator>>(vpImage<vpRGBa> &I)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vp1394TwoGrabber.cpp.o) has no symbols
-void dummy_vp1394TwoGrabber() {};
+// Work arround to avoid warning: libvisp_sensor.a(vp1394TwoGrabber.cpp.o) has
+// no symbols
+void dummy_vp1394TwoGrabber(){};
#endif
-
diff --git a/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp
index 97cd54d..93c5638 100644
--- a/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp
+++ b/modules/sensor/src/framegrabber/OpenCV/vpOpenCVGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,135 +43,114 @@
#include <visp3/sensor/vpOpenCVGrabber.h>
-#if ( defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
+#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
-#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpFrameGrabberException.h>
+#include <visp3/core/vpImageConvert.h>
#include <iostream>
#include <math.h>
/*!
- Basic Constructor.
+ Basic Constructor.
*/
-vpOpenCVGrabber::vpOpenCVGrabber()
- : capture(NULL), DeviceType(0), flip(false)
+vpOpenCVGrabber::vpOpenCVGrabber() : capture(NULL), DeviceType(0), flip(false)
{
- // public memebers
- init = false;
+ // public memebers
+ init = false;
- // protected members
- width = height = 0;
+ // protected members
+ width = height = 0;
}
-
/*!
- Basic destructor that calls the close() method.
+ Basic destructor that calls the close() method.
- \sa close()
+ \sa close()
*/
-vpOpenCVGrabber::~vpOpenCVGrabber( )
-{
- close();
-}
-
+vpOpenCVGrabber::~vpOpenCVGrabber() { close(); }
/*!
- Generic initialization of the grabber.
+ Generic initialization of the grabber.
*/
void vpOpenCVGrabber::open()
{
capture = cvCreateCameraCapture(DeviceType);
-
- if (capture != NULL)
- {
- init = true;
- }
-
- else
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done : camera already used or no camera found") );
- }
-}
+ if (capture != NULL) {
+ init = true;
+ }
+
+ else {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Initialization not done : camera already used or no camera found"));
+ }
+}
/*!
- Initialization of the grabber.
- Call the generic initialization method.
+ Initialization of the grabber.
+ Call the generic initialization method.
- \param I : Gray level image. This parameter is not used.
+ \param I : Gray level image. This parameter is not used.
- \sa open()
+ \sa open()
*/
-void vpOpenCVGrabber::open(vpImage<unsigned char> &/*I*/)
-{
- open();
-}
-
+void vpOpenCVGrabber::open(vpImage<unsigned char> & /*I*/) { open(); }
/*!
- Initialization of the grabber.
- Call the generic initialization method.
+ Initialization of the grabber.
+ Call the generic initialization method.
- \param I : Color image. This parameter is not used.
+ \param I : Color image. This parameter is not used.
- \sa open()
+ \sa open()
*/
-void vpOpenCVGrabber::open(vpImage<vpRGBa> &/*I*/)
-{
- open();
-}
-
+void vpOpenCVGrabber::open(vpImage<vpRGBa> & /*I*/) { open(); }
/*!
- Grab a gray level image.
+ Grab a gray level image.
- \param I : Acquired gray level image.
+ \param I : Acquired gray level image.
- \exception vpFrameGrabberException::initializationError If the
- initialization of the grabber was not done previously.
+ \exception vpFrameGrabberException::initializationError If the
+ initialization of the grabber was not done previously.
*/
void vpOpenCVGrabber::acquire(vpImage<unsigned char> &I)
{
- IplImage *im;
-
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- cvGrabFrame(capture);
- im = cvRetrieveFrame(capture);
- vpImageConvert::convert(im, I, flip);
+ IplImage *im;
+
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
+
+ cvGrabFrame(capture);
+ im = cvRetrieveFrame(capture);
+ vpImageConvert::convert(im, I, flip);
}
/*!
- Grab a color image.
+ Grab a color image.
- \param I : Acquired color image.
+ \param I : Acquired color image.
- \exception vpFrameGrabberException::initializationError If the
- initialization of the grabber was not done previously.
+ \exception vpFrameGrabberException::initializationError If the
+ initialization of the grabber was not done previously.
*/
void vpOpenCVGrabber::acquire(vpImage<vpRGBa> &I)
{
- IplImage *im;
-
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- cvGrabFrame(capture);
- im = cvRetrieveFrame(capture);
- vpImageConvert::convert(im, I, flip);
+ IplImage *im;
+
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
+
+ cvGrabFrame(capture);
+ im = cvRetrieveFrame(capture);
+ vpImageConvert::convert(im, I, flip);
}
/*!
@@ -181,15 +161,13 @@ void vpOpenCVGrabber::acquire(vpImage<vpRGBa> &I)
\exception vpFrameGrabberException::initializationError If the
initialization of the grabber was not done previously.
*/
-IplImage* vpOpenCVGrabber::acquire()
+IplImage *vpOpenCVGrabber::acquire()
{
IplImage *im;
- if (init==false)
- {
+ if (init == false) {
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
}
cvGrabFrame(capture);
@@ -198,126 +176,116 @@ IplImage* vpOpenCVGrabber::acquire()
}
/*!
- Stop the acquisition of images and free the camera.
+ Stop the acquisition of images and free the camera.
*/
void vpOpenCVGrabber::close()
{
- init = false;
- cvReleaseCapture( &capture );
- capture = NULL;
+ init = false;
+ cvReleaseCapture(&capture);
+ capture = NULL;
}
-
/*!
- Gets the capture frame rate.
+ Gets the capture frame rate.
- \param framerate : The value of the framerate is returned here.
+ \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
+ Sets the capture frame rate
- \param framerate : The requested value of the capture framerate.
+ \param framerate : The requested value of the capture framerate.
*/
void vpOpenCVGrabber::setFramerate(const double framerate)
{
- cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, framerate);
+ cvSetCaptureProperty(capture, CV_CAP_PROP_FPS, framerate);
}
-
/*!
- Sets the captured image width.
-
+ Sets the captured image width.
+
\warning This function must be called after open() method.
\param w : The requested value of the captured image width.
- \exception vpFrameGrabberException::initializationError If no
- camera was found.
+ \exception vpFrameGrabberException::initializationError If no
+ camera was found.
*/
void vpOpenCVGrabber::setWidth(const unsigned int w)
{
- if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w))
- {
- close();
- vpERROR_TRACE("Impossible to set the size of the grabber");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Impossible to set the size of the grabber") );
- }
+ if (cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, w)) {
+ close();
+ vpERROR_TRACE("Impossible to set the size of the grabber");
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Impossible to set the size of the grabber"));
+ }
this->width = w;
}
/*!
- Sets the captured image height.
+ Sets the captured image height.
\warning This function must be called after open() method.
\param h : The requested value of the captured image height.
- \exception vpFrameGrabberException::initializationError If no
- camera was found.
+ \exception vpFrameGrabberException::initializationError If no
+ camera was found.
*/
void vpOpenCVGrabber::setHeight(const unsigned int h)
{
- if ( cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h))
- {
- close();
- vpERROR_TRACE("Impossible to set the size of the grabber");
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Impossible to set the size of the grabber") );
- }
+ if (cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, h)) {
+ close();
+ vpERROR_TRACE("Impossible to set the size of the grabber");
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError,
+ "Impossible to set the size of the grabber"));
+ }
this->height = h;
}
/*!
- Set the expected type of device.
-
- \param type : expected type of device
- - CV_CAP_ANY
- - CV_CAP_MIL
- - CV_CAP_VFW
- - CV_CAP_V4L
- - CV_CAP_V4L2
- - CV_CAP_FIREWIRE
- - CV_CAP_IEEE1394
- - CV_CAP_DC1394
- - CV_CAP_CMU_1394
+ Set the expected type of device.
+
+ \param type : expected type of device
+ - CV_CAP_ANY
+ - CV_CAP_MIL
+ - CV_CAP_VFW
+ - CV_CAP_V4L
+ - CV_CAP_V4L2
+ - CV_CAP_FIREWIRE
+ - CV_CAP_IEEE1394
+ - CV_CAP_DC1394
+ - CV_CAP_CMU_1394
*/
void vpOpenCVGrabber::setDeviceType(int type)
{
- DeviceType = type;
-
- if ( DeviceType != 0 && DeviceType != 100 &&DeviceType != 200 && DeviceType != 300)
- {
- vpTRACE("The expected type of device may be unknown.");
- }
-}
+ DeviceType = type;
+ if (DeviceType != 0 && DeviceType != 100 && DeviceType != 200 && DeviceType != 300) {
+ vpTRACE("The expected type of device may be unknown.");
+ }
+}
/*!
- Set the boolean variable flip to the expected value.
+ Set the boolean variable flip to the expected value.
- \param flipType : Expected value of the variable flip. True means that the image is flipped during each image acquisition.
-
- \warning This function is only useful under Windows.
+ \param flipType : Expected value of the variable flip. True means that
+ the image is flipped during each image acquisition.
- \note The aim of this function is to fix a problem which appears under Windows. Indeed with several cameras the aquired images are flipped.
+ \warning This function is only useful under Windows.
+
+ \note The aim of this function is to fix a problem which appears under
+ Windows. Indeed with several cameras the aquired images are flipped.
*/
-void vpOpenCVGrabber::setFlip(bool flipType)
-{
- flip = flipType;
-}
+void vpOpenCVGrabber::setFlip(bool flipType) { flip = flipType; }
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpOpenCVGrabber.cpp.o) has no symbols
-void dummy_vpOpenCVGrabber() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpOpenCVGrabber.cpp.o) has
+// no symbols
+void dummy_vpOpenCVGrabber(){};
#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp
index 5d36f24..71fdd57 100644
--- a/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowDevice.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,89 +40,84 @@
#include <stdio.h>
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
#include <visp3/sensor/vpDirectShowDevice.h>
-
/*!
- Initialize the vpDirectShowDevice with the moniker's information
- \param pMoniker The moniker that contains the device's information
- \return Was the operation successfull
+ Initialize the vpDirectShowDevice with the moniker's information
+ \param pMoniker The moniker that contains the device's information
+ \return Was the operation successfull
*/
-bool vpDirectShowDevice::init(const CComPtr<IMoniker>& pMoniker)
+bool vpDirectShowDevice::init(const CComPtr<IMoniker> &pMoniker)
{
- HRESULT hr;
+ HRESULT hr;
- //Get the properties
- CComPtr<IPropertyBag> pPropBag;
- pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void**)(&pPropBag));
+ // Get the properties
+ CComPtr<IPropertyBag> pPropBag;
+ pMoniker->BindToStorage(0, 0, IID_IPropertyBag, (void **)(&pPropBag));
- //get the name of the input
- VARIANT varName;
- VARIANT varDesc;
- VARIANT varDevPath;
- VariantInit(&varName);
- VariantInit(&varDesc);
- VariantInit(&varDevPath);
- char tmp[FILENAME_MAX];
+ // get the name of the input
+ VARIANT varName;
+ VARIANT varDesc;
+ VARIANT varDevPath;
+ VariantInit(&varName);
+ VariantInit(&varDesc);
+ VariantInit(&varDevPath);
+ char tmp[FILENAME_MAX];
- hr = pPropBag->Read(L"FriendlyName", &varName, 0);
+ hr = pPropBag->Read(L"FriendlyName", &varName, 0);
- //successfully got the name
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S", varName.bstrVal);
- name = tmp;
- }
+ // successfully got the name
+ if (SUCCEEDED(hr)) {
+ sprintf(tmp, "%S", varName.bstrVal);
+ name = tmp;
+ }
- VariantClear(&varName);
+ VariantClear(&varName);
- hr = pPropBag->Read(L"Description", &varDesc, 0);
+ hr = pPropBag->Read(L"Description", &varDesc, 0);
- //successfully got the description
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S", varDesc.bstrVal);
- desc = tmp;
- }
+ // successfully got the description
+ if (SUCCEEDED(hr)) {
+ sprintf(tmp, "%S", varDesc.bstrVal);
+ desc = tmp;
+ }
- VariantClear(&varDesc);
+ VariantClear(&varDesc);
- hr = pPropBag->Read(L"DevicePath", &varDevPath, 0);
+ hr = pPropBag->Read(L"DevicePath", &varDevPath, 0);
- //successfully got the device path
- if (SUCCEEDED(hr))
- {
- sprintf(tmp, "%S",varDevPath.bstrVal);
- devPath = tmp;
- }
+ // successfully got the device path
+ if (SUCCEEDED(hr)) {
+ sprintf(tmp, "%S", varDevPath.bstrVal);
+ devPath = tmp;
+ }
- VariantClear(&varDevPath);
+ VariantClear(&varDevPath);
- inUse=false;
+ inUse = false;
- return true;
+ return true;
}
/*!
- Compares the two vpDirectShowDevice.
- \return true if they are equal
+ Compares the two vpDirectShowDevice.
+ \return true if they are equal
*/
-bool vpDirectShowDevice::operator==(vpDirectShowDevice& dev)
+bool vpDirectShowDevice::operator==(vpDirectShowDevice &dev)
{
- return name==dev.name
- && desc==dev.desc
- && devPath==dev.devPath;
+ 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;
+ return os << dev.name << std::endl << dev.desc << std::endl << dev.devPath;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowDevice.cpp.o) has no symbols
-void dummy_vpDirectShowDevice() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowDevice.cpp.o)
+// has no symbols
+void dummy_vpDirectShowDevice(){};
#endif
#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp
index 6579e83..1d811d9 100644
--- a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,203 +37,160 @@
*
*****************************************************************************/
-
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
#include <visp3/sensor/vpDirectShowGrabber.h>
#include <visp3/sensor/vpDirectShowGrabberImpl.h>
/*!
- Returns the number of rows in the grabbed image
+ Returns the number of rows in the grabbed image
*/
-unsigned int vpDirectShowGrabber::getHeight(){ return grabber->getHeight(); }
+unsigned int vpDirectShowGrabber::getHeight() { return grabber->getHeight(); }
/*!
- Returns the number of colunms in the grabbed image
+ Returns the number of colunms in the grabbed image
*/
-unsigned int vpDirectShowGrabber::getWidth(){ return grabber->getWidth(); }
-
+unsigned int vpDirectShowGrabber::getWidth() { return grabber->getWidth(); }
/*!
- Constructor.
- Initializes COM.
+ Constructor.
+ Initializes COM.
*/
-vpDirectShowGrabber::vpDirectShowGrabber()
-{
- grabber = new vpDirectShowGrabberImpl();
-}
+vpDirectShowGrabber::vpDirectShowGrabber() { grabber = new vpDirectShowGrabberImpl(); }
/*!
- Destructor
+ Destructor
*/
-vpDirectShowGrabber::~vpDirectShowGrabber()
-{
- delete grabber;
-}
+vpDirectShowGrabber::~vpDirectShowGrabber() { delete grabber; }
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
-void vpDirectShowGrabber::open()
-{
- grabber->open();
-}
+void vpDirectShowGrabber::open() { grabber->open(); }
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
-void vpDirectShowGrabber::open(vpImage<unsigned char> &I)
-{
- grabber->open();
-}
+void vpDirectShowGrabber::open(vpImage<unsigned char> &I) { grabber->open(); }
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
-void vpDirectShowGrabber::open(vpImage<vpRGBa> &I)
-{
- grabber->open();
-}
-
+void vpDirectShowGrabber::open(vpImage<vpRGBa> &I) { grabber->open(); }
/*!
- Grabs a grayscale image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
+ Grabs a grayscale image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
- \param I The image to fill with the grabbed frame
+ \param I The image to fill with the grabbed frame
- \exception otherError Can't grab the frame
+ \exception otherError Can't grab the frame
*/
-void vpDirectShowGrabber::acquire(vpImage<unsigned char> &I)
-{
- grabber->acquire(I);
-}
+void vpDirectShowGrabber::acquire(vpImage<unsigned char> &I) { grabber->acquire(I); }
/*!
- Grabs a rgba image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
+ Grabs a rgba image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
- \param I The image to fill with the grabbed frame
+ \param I The image to fill with the grabbed frame
- \exception otherError Can't grab the frame
+ \exception otherError Can't grab the frame
*/
-void vpDirectShowGrabber::acquire(vpImage<vpRGBa> &I)
-{
- grabber->acquire(I);
-}
-
+void vpDirectShowGrabber::acquire(vpImage<vpRGBa> &I) { grabber->acquire(I); }
/*!
- Stops the framegrabber
+ Stops the framegrabber
*/
void vpDirectShowGrabber::close() { grabber->close(); }
/*!
- Gets the number of capture devices
- \return the number of capture devices
+ 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.
- \param id : Identifier of the device to use.
+ Set the device (or camera) from which images will be grabbed.
+ \param id : Identifier of the device to use.
- \return true if the device is set successfully, false otherwise.
+ \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
+ Displays the list of devices on the standard output
*/
-void vpDirectShowGrabber::displayDevices()
-{
- grabber->displayDevices();
-}
+void vpDirectShowGrabber::displayDevices() { grabber->displayDevices(); }
/*!
- Set image Size
- \param width : Image width
- \param height : Image height
+ Set image Size
+ \param width : Image width
+ \param height : Image height
- \return true if successful
+ \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);
+ return grabber->setImageSize(width, height);
}
/*!
- Set capture framerate
- \param framerate : Framerate in fps
+ Set capture framerate
+ \param framerate : Framerate in fps
- \return true if successful
+ \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
- \param height : Image height in pixel
- \param framerate : Framerate in fps
+ Set capture format
+ \param width : Image width in pixel
+ \param height : Image height in pixel
+ \param framerate : Framerate in fps
- \return true if successful
+ \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);
+ return grabber->setFormat(width, height, framerate);
}
/*
- Get capture format
- \param width : Pointer to a variable that receives the width in pixel
- \param height : Pointer to a variable that receives the height in pixel
- \param framerate : Pointer to a variable that receives the framerate in fps
+ Get capture format
+ \param width : Pointer to a variable that receives the width in pixel
+ \param height : Pointer to a variable that receives the height in
+ pixel \param framerate : Pointer to a variable that receives the framerate
+ in fps
*/
-void vpDirectShowGrabber::getFormat(unsigned int &width,unsigned int &height, double &framerate)
+void vpDirectShowGrabber::getFormat(unsigned int &width, unsigned int &height, double &framerate)
{
- grabber->getFormat(width, height, framerate);
+ grabber->getFormat(width, height, framerate);
}
/*!
- Get the available capture formats
+ Get the available capture formats
- \return true if successful
+ \return true if successful
*/
-bool vpDirectShowGrabber::getStreamCapabilities()
-{
- return grabber->getStreamCapabilities();
-}
+bool vpDirectShowGrabber::getStreamCapabilities() { return grabber->getStreamCapabilities(); }
/*!
- Set capture MediaType
- \param mediaTypeID : mediaTypeID (available in calling getStreamCapabilities)
+ Set capture MediaType
+ \param mediaTypeID : mediaTypeID (available in calling
+ getStreamCapabilities)
- \return true if successful
+ \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
+ Get current capture MediaType
- \return the current mediaTypeID
+ \return the current mediaTypeID
*/
-int vpDirectShowGrabber::getMediaType()
-{
- return grabber->getMediaType();
-}
+int vpDirectShowGrabber::getMediaType() { return grabber->getMediaType(); }
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowGrabber.cpp.o) has no symbols
-void dummy_vpDirectShowGrabber() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowGrabber.cpp.o)
+// has no symbols
+void dummy_vpDirectShowGrabber(){};
#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
index 17c3541..191fe1b 100644
--- a/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowGrabberImpl.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,1200 +40,1106 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
+#if (defined(VISP_HAVE_DIRECTSHOW))
#include <visp3/sensor/vpDirectShowGrabberImpl.h>
-vpDirectShowDevice * vpDirectShowGrabberImpl::deviceList = NULL;
-unsigned int vpDirectShowGrabberImpl::nbDevices ;
+vpDirectShowDevice *vpDirectShowGrabberImpl::deviceList = NULL;
+unsigned int vpDirectShowGrabberImpl::nbDevices;
/*!
- Converts a HRESULT into the corresponding error message
+ Converts a HRESULT into the corresponding error message
*/
void vpDirectShowGrabberImpl::HRtoStr(std::string str)
{
- TCHAR szErr[MAX_ERROR_TEXT_LEN];
- DWORD res = AMGetErrorText(hr, szErr, MAX_ERROR_TEXT_LEN);
+ TCHAR szErr[MAX_ERROR_TEXT_LEN];
+ DWORD res = AMGetErrorText(hr, szErr, MAX_ERROR_TEXT_LEN);
- if (res == 0) str = "Unknown Error: 0x%2x";
+ if (res == 0)
+ str = "Unknown Error: 0x%2x";
- char msg[MAX_ERROR_TEXT_LEN];
- sprintf(msg,"%s",szErr);
- str = msg;
+ char msg[MAX_ERROR_TEXT_LEN];
+ sprintf(msg, "%s", szErr);
+ str = msg;
}
-
/*!
- Constructor.
- Initializes COM.
+ Constructor.
+ Initializes COM.
*/
vpDirectShowGrabberImpl::vpDirectShowGrabberImpl()
{
- init = false ;
- initCo = false ;
- //COM initialization
- if (FAILED(hr = CoInitializeEx(NULL,COINIT_MULTITHREADED)))
- {
- std::string err;
- HRtoStr(err);
- throw(vpFrameGrabberException(
- vpFrameGrabberException::initializationError,
- "Can't initialize COM\n"+ err));
- }
- initCo = true ;
+ init = false;
+ initCo = false;
+ // COM initialization
+ if (FAILED(hr = CoInitializeEx(NULL, COINIT_MULTITHREADED))) {
+ std::string err;
+ HRtoStr(err);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't initialize COM\n" + err));
+ }
+ initCo = true;
- //create the device list
- if(deviceList == NULL) {
+ // create the device list
+ if (deviceList == NULL) {
CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
- if(enumerate(pVideoInputEnum)) {
+ if (enumerate(pVideoInputEnum)) {
createDeviceList(pVideoInputEnum);
}
- //not used anymore, so we release it
- pVideoInputEnum.Release();
+ // not used anymore, so we release it
+ pVideoInputEnum.Release();
}
}
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
void vpDirectShowGrabberImpl::open()
{
- //create the device list
- if(deviceList == NULL) {
+ // create the device list
+ if (deviceList == NULL) {
CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
- if(enumerate(pVideoInputEnum)) {
+ if (enumerate(pVideoInputEnum)) {
createDeviceList(pVideoInputEnum);
}
- //not used anymore, so we release it
- pVideoInputEnum.Release();
+ // not used anymore, so we release it
+ pVideoInputEnum.Release();
}
init = initDirectShow();
- if(! init )
- {
- std::string err;
- HRtoStr(err);
- throw (vpFrameGrabberException(
- vpFrameGrabberException::initializationError, err));
- }
+ if (!init) {
+ std::string err;
+ HRtoStr(err);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, err));
+ }
}
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
-void vpDirectShowGrabberImpl::open(vpImage<unsigned char> &I)
-{
- open();
-}
+void vpDirectShowGrabberImpl::open(vpImage<unsigned char> &I) { open(); }
/*!
- Creates the filter graph and select the first available device.
- \exception initializationError
+ Creates the filter graph and select the first available device.
+ \exception initializationError
*/
-void vpDirectShowGrabberImpl::open(vpImage<vpRGBa> &I)
-{
- open();
-}
+void vpDirectShowGrabberImpl::open(vpImage<vpRGBa> &I) { open(); }
/*!
- Initialization method
- Creates the capture filter graph
- \return Was the operation successful
+ Initialization method
+ Creates the capture filter graph
+ \return Was the operation successful
*/
bool vpDirectShowGrabberImpl::initDirectShow()
{
+ // get the first working device's filter (unused and getdevice works on it)
+ currentDevice = getFirstUnusedDevice(pCapSource);
- //get the first working device's filter (unused and getdevice works on it)
- currentDevice = getFirstUnusedDevice(pCapSource);
-
- if(currentDevice == nbDevices)
- return false;
-
- //create the filter graph
- if(!createGraph())
- return false;
+ if (currentDevice == nbDevices)
+ return false;
- //we add the capture source to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
- return false;
+ // create the filter graph
+ if (!createGraph())
+ return false;
- //we create a sample grabber
- if(!createSampleGrabber(pGrabberFilter))
- return false;
+ // we add the capture source to the filter graph
+ if (FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
+ return false;
- //we add the grabber to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pGrabberFilter, L"SampleGrabber")))
- return false;
+ // we create a sample grabber
+ if (!createSampleGrabber(pGrabberFilter))
+ return false;
- //we connect the pins
- if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
- return false;
+ // we add the grabber to the filter graph
+ if (FAILED(hr = pGraph->AddFilter(pGrabberFilter, L"SampleGrabber")))
+ return false;
+ // we connect the pins
+ if (!connectSourceToGrabber(pCapSource, pGrabberFilter))
+ return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
+ // get the current connected media type (needed by the callback)
+ if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
- //Gets the various graph's interfaces
- CComPtr<IMediaFilter> pMediaFilter;
+ // Gets the various graph's interfaces
+ CComPtr<IMediaFilter> pMediaFilter;
- pGraph->QueryInterface(IID_IMediaFilter, (void **)&pMediaFilter);
- pGraph->QueryInterface(IID_IMediaControl, reinterpret_cast<void**>(&pControl));
- pGraph->QueryInterface(IID_IMediaEvent, (void **)&pEvent);
+ pGraph->QueryInterface(IID_IMediaFilter, (void **)&pMediaFilter);
+ pGraph->QueryInterface(IID_IMediaControl, reinterpret_cast<void **>(&pControl));
+ pGraph->QueryInterface(IID_IMediaEvent, (void **)&pEvent);
- pMediaFilter->SetSyncSource(NULL);
- pMediaFilter.Release();
+ pMediaFilter->SetSyncSource(NULL);
+ pMediaFilter.Release();
- return true;
+ return true;
}
/*!
- Destructor
+ Destructor
*/
-vpDirectShowGrabberImpl::~vpDirectShowGrabberImpl()
-{
- close();
-}
+vpDirectShowGrabberImpl::~vpDirectShowGrabberImpl() { close(); }
/*!
- Create a video device enumerator.
- \param ppVideoInputEnum The video device enumerator
- \return Was the operation successful
+ Create a video device enumerator.
+ \param ppVideoInputEnum The video device enumerator
+ \return Was the operation successful
*/
-bool vpDirectShowGrabberImpl::enumerate(CComPtr<IEnumMoniker>& ppVideoInputEnum)
+bool vpDirectShowGrabberImpl::enumerate(CComPtr<IEnumMoniker> &ppVideoInputEnum)
{
- CComPtr<ICreateDevEnum> pDevEnum = NULL;
- bool res = false;
-
- //Enumerate system devices
- hr = pDevEnum.CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER);
+ CComPtr<ICreateDevEnum> pDevEnum = NULL;
+ bool res = false;
- //if it is a success
- if (SUCCEEDED(hr))
- {
- //Create a video input device enumerator
- hr = pDevEnum->CreateClassEnumerator(
- CLSID_VideoInputDeviceCategory,
- &ppVideoInputEnum, 0);
+ // Enumerate system devices
+ hr = pDevEnum.CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER);
- if(hr == S_OK) res = true;
- }
+ // if it is a success
+ if (SUCCEEDED(hr)) {
+ // Create a video input device enumerator
+ hr = pDevEnum->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &ppVideoInputEnum, 0);
+ if (hr == S_OK)
+ res = true;
+ }
- pDevEnum.Release();
- return res;
+ pDevEnum.Release();
+ return res;
}
-
/*!
- Create the device list by enumerating the video input devices
- \param ppVideoInputEnum A video device enumerator
- \return Was the operation successful
+ Create the device list by enumerating the video input devices
+ \param ppVideoInputEnum A video device enumerator
+ \return Was the operation successful
*/
-bool vpDirectShowGrabberImpl::createDeviceList(CComPtr<IEnumMoniker>& ppVideoInputEnum)
+bool vpDirectShowGrabberImpl::createDeviceList(CComPtr<IEnumMoniker> &ppVideoInputEnum)
{
- CComPtr<IMoniker> pMoniker[10]; //const max devices
- unsigned long nbMoniker;
-
- ppVideoInputEnum->Reset();
-
- //Enumerates the different inputs
- ppVideoInputEnum->Next(10, reinterpret_cast<IMoniker **>(&pMoniker), &nbMoniker);
-
- //if no input device
- if(nbMoniker == 0) return false;
-
- deviceList = new vpDirectShowDevice[nbMoniker];
-
- nbDevices = (unsigned int)nbMoniker;
-
- //we try to get the properties of each moniker, if it fails, we skip to the next one and
- //decrement the number of valid devices
- unsigned int i=0;
- unsigned int j=0;
- while(i<nbDevices)
- {
- if(!deviceList[i].init(pMoniker[j]))
- {
- //if we can't get the device properties, skip to the next device
- j++;
- nbDevices--;
- }
- else
- {
- i++;
- j++;
- }
- }
-
- //if no working input device
- if(nbDevices == 0) return false;
-
- //we release the monikers
- for(unsigned int i=0 ; i<nbMoniker ;i++)
- {
- pMoniker[i].Release();
- }
-
- return true;
-}
+ CComPtr<IMoniker> pMoniker[10]; // const max devices
+ unsigned long nbMoniker;
+
+ ppVideoInputEnum->Reset();
+
+ // Enumerates the different inputs
+ ppVideoInputEnum->Next(10, reinterpret_cast<IMoniker **>(&pMoniker), &nbMoniker);
+
+ // if no input device
+ if (nbMoniker == 0)
+ return false;
+
+ deviceList = new vpDirectShowDevice[nbMoniker];
+
+ nbDevices = (unsigned int)nbMoniker;
+
+ // we try to get the properties of each moniker, if it fails, we skip to the
+ // next one and decrement the number of valid devices
+ unsigned int i = 0;
+ unsigned int j = 0;
+ while (i < nbDevices) {
+ if (!deviceList[i].init(pMoniker[j])) {
+ // if we can't get the device properties, skip to the next device
+ j++;
+ nbDevices--;
+ } else {
+ i++;
+ j++;
+ }
+ }
+
+ // if no working input device
+ if (nbDevices == 0)
+ return false;
+ // we release the monikers
+ for (unsigned int i = 0; i < nbMoniker; i++) {
+ pMoniker[i].Release();
+ }
+
+ return true;
+}
/*!
- Gets the filter associated with device n if it exists.
- \param n Number of the device
- \param ppDevice The n-th device
- \return Was the operation successful
+ Gets the filter associated with device n if it exists.
+ \param n Number of the device
+ \param ppDevice The n-th device
+ \return Was the operation successful
*/
-bool vpDirectShowGrabberImpl::getDevice(unsigned int n, CComPtr<IBaseFilter>& ppDevice)
+bool vpDirectShowGrabberImpl::getDevice(unsigned int n, CComPtr<IBaseFilter> &ppDevice)
{
- //if n is invalid, quit
- if(n>=nbDevices)
- return false;
-
- //if the device is already in use, quit
- if(deviceList[n].getState() == true)
- return false;
-
- //if we can't enumerate the devices, quit
- CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
- if(!enumerate(pVideoInputEnum))
- return false;
-
- CComPtr<IMoniker> pMoniker = NULL;
- bool deviceFound = false;
-
- //Enumerates the different inputs
- while (pVideoInputEnum->Next(1, &pMoniker, NULL) == S_OK
- && !deviceFound)
- {
- //implicit conversion should work ...
- if(deviceList[n] == vpDirectShowDevice(pMoniker))
- {
- //we get the filter
- if(SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, (void**)&ppDevice)))
- {
- //now the device is in use
- deviceList[n].setInUse();
- deviceFound = true;
- }
- else{ break; } //we can't get the device's filter, quit
- }
- pMoniker.Release();
- }
-
- pVideoInputEnum.Release();
-
- return deviceFound;
+ // if n is invalid, quit
+ if (n >= nbDevices)
+ return false;
+
+ // if the device is already in use, quit
+ if (deviceList[n].getState() == true)
+ return false;
+
+ // if we can't enumerate the devices, quit
+ CComPtr<IEnumMoniker> pVideoInputEnum = NULL;
+ if (!enumerate(pVideoInputEnum))
+ return false;
+
+ CComPtr<IMoniker> pMoniker = NULL;
+ bool deviceFound = false;
+
+ // Enumerates the different inputs
+ while (pVideoInputEnum->Next(1, &pMoniker, NULL) == S_OK && !deviceFound) {
+ // implicit conversion should work ...
+ if (deviceList[n] == vpDirectShowDevice(pMoniker)) {
+ // we get the filter
+ if (SUCCEEDED(pMoniker->BindToObject(0, 0, IID_IBaseFilter, (void **)&ppDevice))) {
+ // now the device is in use
+ deviceList[n].setInUse();
+ deviceFound = true;
+ } else {
+ break;
+ } // we can't get the device's filter, quit
+ }
+ pMoniker.Release();
+ }
+
+ pVideoInputEnum.Release();
+
+ return deviceFound;
}
/*!
- Searches for the first unused device.
- \param ppDevice The first device filter's interface
- \return number of the found device. If the operation wasn't successfull, n=nbDevices.
+ Searches for the first unused device.
+ \param ppDevice The first device filter's interface
+ \return number of the found device. If the operation wasn't
+ successfull, n=nbDevices.
*/
-unsigned int vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr<IBaseFilter>& ppDevice)
+unsigned int vpDirectShowGrabberImpl::getFirstUnusedDevice(CComPtr<IBaseFilter> &ppDevice)
{
- unsigned int n=0;
- bool found=false;
-
- for(n=0;n<nbDevices && !found; n++)
- {
- //if the device is not being used
- if(!deviceList[n].getState())
- {
- if(getDevice(n,ppDevice))
- {
- found = true;
- deviceList[n].setInUse();
- return n;
- }
- }
- }
-
- return n;
+ unsigned int n = 0;
+ bool found = false;
+
+ for (n = 0; n < nbDevices && !found; n++) {
+ // if the device is not being used
+ if (!deviceList[n].getState()) {
+ if (getDevice(n, ppDevice)) {
+ found = true;
+ deviceList[n].setInUse();
+ return n;
+ }
+ }
+ }
+
+ return n;
}
/*!
- Create the capture graph
- \return Was the operation successful
+ Create the capture graph
+ \return Was the operation successful
*/
bool vpDirectShowGrabberImpl::createGraph()
{
- // Create the Capture Graph Builder.
- hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, CLSCTX_INPROC_SERVER);
+ // Create the Capture Graph Builder.
+ hr = pBuild.CoCreateInstance(CLSID_CaptureGraphBuilder2, 0, CLSCTX_INPROC_SERVER);
- if (SUCCEEDED(hr))
- {
- // Create the Filter Graph Manager.
- hr = pGraph.CoCreateInstance(CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER);
+ if (SUCCEEDED(hr)) {
+ // Create the Filter Graph Manager.
+ hr = pGraph.CoCreateInstance(CLSID_FilterGraph, 0, CLSCTX_INPROC_SERVER);
- if (SUCCEEDED(hr))
- {
- // Initialize the Capture Graph Builder.
- pBuild->SetFiltergraph(pGraph);
+ if (SUCCEEDED(hr)) {
+ // Initialize the Capture Graph Builder.
+ pBuild->SetFiltergraph(pGraph);
- return true;
- }
- }
+ return true;
+ }
+ }
- return false;
+ return false;
}
/*!
- Creates the grabber.
- \param ppGrabberFilter The created grabber filter's interface
- \return Was the operation successful
+ Creates the grabber.
+ \param ppGrabberFilter The created grabber filter's interface
+ \return Was the operation successful
*/
-bool vpDirectShowGrabberImpl::createSampleGrabber(CComPtr<IBaseFilter>& ppGrabberFilter)
+bool vpDirectShowGrabberImpl::createSampleGrabber(CComPtr<IBaseFilter> &ppGrabberFilter)
{
- //Creates the sample grabber
- hr = ppGrabberFilter.CoCreateInstance(CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER);
+ // Creates the sample grabber
+ hr = ppGrabberFilter.CoCreateInstance(CLSID_SampleGrabber, NULL, CLSCTX_INPROC_SERVER);
- if (FAILED(hr))
- return false;
+ if (FAILED(hr))
+ return false;
- //gets the SampleGrabber interface in order to configure it later
- hr = ppGrabberFilter->QueryInterface(IID_ISampleGrabber,
- reinterpret_cast<void**>(&pGrabberI));
+ // gets the SampleGrabber interface in order to configure it later
+ hr = ppGrabberFilter->QueryInterface(IID_ISampleGrabber, reinterpret_cast<void **>(&pGrabberI));
- if (FAILED(hr))
- return false;
+ if (FAILED(hr))
+ return false;
- //configure the grabber
- AM_MEDIA_TYPE mt;
- ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
+ // configure the grabber
+ AM_MEDIA_TYPE mt;
+ ZeroMemory(&mt, sizeof(AM_MEDIA_TYPE));
- mt.majortype = MEDIATYPE_Video;
+ mt.majortype = MEDIATYPE_Video;
- //ask for a connection
- mt.subtype = MEDIATYPE_NULL;
+ // ask for a connection
+ mt.subtype = MEDIATYPE_NULL;
- if(FAILED(hr = pGrabberI->SetMediaType(&mt)))
- return false;
+ if (FAILED(hr = pGrabberI->SetMediaType(&mt)))
+ return false;
- //configure the callback of the grabber
- pGrabberI->SetCallback(&sgCB,1);
+ // configure the callback of the grabber
+ pGrabberI->SetCallback(&sgCB, 1);
- //grab only one frame at a time
- pGrabberI->SetOneShot(TRUE);
+ // grab only one frame at a time
+ pGrabberI->SetOneShot(TRUE);
- //no need to bufferize the sample in the grabber
- pGrabberI->SetBufferSamples(false);
+ // no need to bufferize the sample in the grabber
+ pGrabberI->SetBufferSamples(false);
- return true;
+ return true;
}
/*!
Checks the capture filter's ouput pin media type.
- Some formats' orientation is automatically handled by directshow but this isn't the
- case for all the existing formats.
- If the grabbed image is inverted, the source format's orientation may not be
- known by directshow so consider adding an additional handler for this FourCC format.
+ Some formats' orientation is automatically handled by directshow but this
+ isn't the case for all the existing formats. If the grabbed image is
+ inverted, the source format's orientation may not be known by directshow so
+ consider adding an additional handler for this FourCC format.
\return True if the connection media type was a video media type.
*/
-bool vpDirectShowGrabberImpl::checkSourceType(CComPtr<IPin>& pCapSourcePin)
+bool vpDirectShowGrabberImpl::checkSourceType(CComPtr<IPin> &pCapSourcePin)
{
- //retrieves the connected media type
- AM_MEDIA_TYPE mt;
- if(FAILED (pCapSourcePin->ConnectionMediaType(&mt)))
- return false;
-
- if(mt.majortype != MEDIATYPE_Video)
- return false;
-
- //Known RGB formats
- if(mt.subtype == MEDIASUBTYPE_ARGB32 ||
- mt.subtype == MEDIASUBTYPE_RGB32 ||
- mt.subtype == MEDIASUBTYPE_RGB24 ||
- mt.subtype == MEDIASUBTYPE_RGB555 ||
- mt.subtype == MEDIASUBTYPE_RGB565 ||
- mt.subtype == MEDIASUBTYPE_RGB8 ||
- mt.subtype == MEDIASUBTYPE_RGB4 ||
- mt.subtype == MEDIASUBTYPE_RGB1 )
- {
- //image orientation will be handled "automatically"
- sgCB.specialMediaType = false;
- }
- //Known YUV formats
- else if(mt.subtype == MEDIASUBTYPE_AYUV ||
- mt.subtype == MEDIASUBTYPE_UYVY ||
- mt.subtype == MEDIASUBTYPE_Y411 ||
- mt.subtype == MEDIASUBTYPE_Y41P ||
- mt.subtype == MEDIASUBTYPE_Y211 ||
- mt.subtype == MEDIASUBTYPE_YUY2 ||
- mt.subtype == MEDIASUBTYPE_YVYU ||
- mt.subtype == MEDIASUBTYPE_YUYV ||
- mt.subtype == MEDIASUBTYPE_IF09 ||
- mt.subtype == MEDIASUBTYPE_IYUV ||
- mt.subtype == MEDIASUBTYPE_YV12 ||
- mt.subtype == MEDIASUBTYPE_YVU9 )
- {
- //image orientation will be handled "automatically"
- sgCB.specialMediaType = false;
- }
- //FOURCC formats
- else
- {
- //invertedSource boolean will decide the bitmap orientation
- sgCB.specialMediaType = true;
-
- DWORD format;
- VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(mt.pbFormat);
- BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
-
- //get the fourcc code
- format = ((bmpInfo.biCompression&0xFF000000)>>24) |
- ((bmpInfo.biCompression&0x00FF0000)>>8) |
- ((bmpInfo.biCompression&0x0000FF00)<<8) |
- (bmpInfo.biCompression&0x000000FF)<<24;
-
- std::cout<<"This format is not one of the standard YUV or RGB format supported by DirectShow.\n"
- <<"FourCC : "
- <<(char)(bmpInfo.biCompression&0x000000FF)
- <<(char)((bmpInfo.biCompression&0x0000FF00)>>8)
- <<(char)((bmpInfo.biCompression&0x00FF0000)>>16)
- <<(char)((bmpInfo.biCompression&0xFF000000)>>24)<<std::endl;
-
- //Y800 is top-down oriented so the image doesn't have to be flipped vertically
- if(format == 'Y800')
- {
- sgCB.invertedSource = false;
- }
- //cyuv seems to be the only yuv bottom-up oriented format (image has to be flipped)
- else if(format == 'cyuv')
- {
- sgCB.invertedSource = true;
- }
- //insert code for other fourcc formats here
- //see fourcc.org to know which format is bottom-up oriented and thus needs invertedSource sets to true
- else
- {
- std::cout<<"Unknown FourCC compression type, assuming top-down orientation. Image may be inverted."<<std::endl;
- sgCB.invertedSource = false; //consider that the image is topdown oriented by default
- }
- }
-
- return true;
+ // retrieves the connected media type
+ AM_MEDIA_TYPE mt;
+ if (FAILED(pCapSourcePin->ConnectionMediaType(&mt)))
+ return false;
+
+ if (mt.majortype != MEDIATYPE_Video)
+ return false;
+
+ // Known RGB formats
+ if (mt.subtype == MEDIASUBTYPE_ARGB32 || mt.subtype == MEDIASUBTYPE_RGB32 || mt.subtype == MEDIASUBTYPE_RGB24 ||
+ mt.subtype == MEDIASUBTYPE_RGB555 || mt.subtype == MEDIASUBTYPE_RGB565 || mt.subtype == MEDIASUBTYPE_RGB8 ||
+ mt.subtype == MEDIASUBTYPE_RGB4 || mt.subtype == MEDIASUBTYPE_RGB1) {
+ // image orientation will be handled "automatically"
+ sgCB.specialMediaType = false;
+ }
+ // Known YUV formats
+ else if (mt.subtype == MEDIASUBTYPE_AYUV || mt.subtype == MEDIASUBTYPE_UYVY || mt.subtype == MEDIASUBTYPE_Y411 ||
+ mt.subtype == MEDIASUBTYPE_Y41P || mt.subtype == MEDIASUBTYPE_Y211 || mt.subtype == MEDIASUBTYPE_YUY2 ||
+ mt.subtype == MEDIASUBTYPE_YVYU || mt.subtype == MEDIASUBTYPE_YUYV || mt.subtype == MEDIASUBTYPE_IF09 ||
+ mt.subtype == MEDIASUBTYPE_IYUV || mt.subtype == MEDIASUBTYPE_YV12 || mt.subtype == MEDIASUBTYPE_YVU9) {
+ // image orientation will be handled "automatically"
+ sgCB.specialMediaType = false;
+ }
+ // FOURCC formats
+ else {
+ // invertedSource boolean will decide the bitmap orientation
+ sgCB.specialMediaType = true;
+
+ DWORD format;
+ VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER *>(mt.pbFormat);
+ BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
+
+ // get the fourcc code
+ format = ((bmpInfo.biCompression & 0xFF000000) >> 24) | ((bmpInfo.biCompression & 0x00FF0000) >> 8) |
+ ((bmpInfo.biCompression & 0x0000FF00) << 8) | (bmpInfo.biCompression & 0x000000FF) << 24;
+
+ std::cout << "This format is not one of the standard YUV or RGB format "
+ "supported by DirectShow.\n"
+ << "FourCC : " << (char)(bmpInfo.biCompression & 0x000000FF)
+ << (char)((bmpInfo.biCompression & 0x0000FF00) >> 8) << (char)((bmpInfo.biCompression & 0x00FF0000) >> 16)
+ << (char)((bmpInfo.biCompression & 0xFF000000) >> 24) << std::endl;
+
+ // Y800 is top-down oriented so the image doesn't have to be flipped
+ // vertically
+ if (format == 'Y800') {
+ sgCB.invertedSource = false;
+ }
+ // cyuv seems to be the only yuv bottom-up oriented format (image has to
+ // be flipped)
+ else if (format == 'cyuv') {
+ sgCB.invertedSource = true;
+ }
+ // insert code for other fourcc formats here
+ // see fourcc.org to know which format is bottom-up oriented and thus
+ // needs invertedSource sets to true
+ else {
+ std::cout << "Unknown FourCC compression type, assuming top-down "
+ "orientation. Image may be inverted."
+ << std::endl;
+ sgCB.invertedSource = false; // consider that the image is topdown oriented by default
+ }
+ }
+
+ return true;
}
/*!
- Connects the capture device's output pin to the grabber's input pin
- \param pCapSource The capture device
- \param pGrabber The grabber
- \return Was the operation successful
+ Connects the capture device's output pin to the grabber's input pin
+ \param pCapSource The capture device
+ \param pGrabber The grabber
+ \return Was the operation successful
*/
-bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr<IBaseFilter>& _pCapSource, CComPtr<IBaseFilter>& _pGrabberFilter)
+bool vpDirectShowGrabberImpl::connectSourceToGrabber(CComPtr<IBaseFilter> &_pCapSource,
+ CComPtr<IBaseFilter> &_pGrabberFilter)
{
- /*
- //get the capture source's output pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
-
- //get the grabber's input pin
- CComPtr<IPin> pGrabberInputPin;
- if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_INPUT, NULL, NULL, false, 0, &pGrabberInputPin)))
- return false;
-
- //connect the two of them
- if(FAILED(pGraph->Connect(pCapSourcePin, pGrabberInputPin)))
- return false;
-
- //not used anymore, we can release it
- pGrabberInputPin.Release();
- */
- if(FAILED(hr = pBuild->RenderStream(NULL,NULL, _pCapSource, NULL, _pGrabberFilter)))
- return false;
-
- /*
- //get the grabber's output pin
- CComPtr<IPin> pGrabberOutputPin;
- if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_OUTPUT, NULL, NULL, false, 0, &pGrabberOutputPin)))
- return false;
- */
- //get the Null renderer
- CComPtr<IBaseFilter> pNull = NULL;
- if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER)))
- return false;
-/*
- //get the null renderer's input pin
- CComPtr<IPin> pNullInputPin;
- if(FAILED(pBuild->FindPin(pNull, PINDIR_INPUT, NULL, NULL, false, 0, &pNullInputPin)))
- return false;
-
- //connect the grabber's output to the null renderer
- if( FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) ||
- FAILED(pGraph->Connect(pGrabberOutputPin, pNullInputPin)))
- return false;
-*/
-
- if( FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) ||
- FAILED(pBuild->RenderStream(NULL,NULL, _pGrabberFilter, NULL, pNull)))
- return false;
-
- //get the capture source's output pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
- //checks the media type of the capture filter
- //and if the image needs to be inverted
- if(!checkSourceType(pCapSourcePin))
- return false;
-
- //release the remaining interfaces
- pCapSourcePin.Release();
- pNull.Release();
-// pGrabberOutputPin.Release();
-// pNullInputPin.Release();
-
- return true;
+ /*
+ //get the capture source's output pin
+ CComPtr<IPin> pCapSourcePin;
+ if(FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0,
+ &pCapSourcePin))) return false;
+
+ //get the grabber's input pin
+ CComPtr<IPin> pGrabberInputPin;
+ if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_INPUT, NULL, NULL, false,
+ 0, &pGrabberInputPin))) return false;
+
+ //connect the two of them
+ if(FAILED(pGraph->Connect(pCapSourcePin, pGrabberInputPin)))
+ return false;
+
+ //not used anymore, we can release it
+ pGrabberInputPin.Release();
+ */
+ if (FAILED(hr = pBuild->RenderStream(NULL, NULL, _pCapSource, NULL, _pGrabberFilter)))
+ return false;
+
+ /*
+ //get the grabber's output pin
+ CComPtr<IPin> pGrabberOutputPin;
+ if(FAILED(pBuild->FindPin(_pGrabberFilter, PINDIR_OUTPUT, NULL, NULL, false,
+ 0, &pGrabberOutputPin))) return false;
+ */
+ // get the Null renderer
+ CComPtr<IBaseFilter> pNull = NULL;
+ if (FAILED(pNull.CoCreateInstance(CLSID_NullRenderer, NULL, CLSCTX_INPROC_SERVER)))
+ return false;
+ /*
+ //get the null renderer's input pin
+ CComPtr<IPin> pNullInputPin;
+ if(FAILED(pBuild->FindPin(pNull, PINDIR_INPUT, NULL, NULL, false, 0,
+ &pNullInputPin))) return false;
+
+ //connect the grabber's output to the null renderer
+ if( FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) ||
+ FAILED(pGraph->Connect(pGrabberOutputPin, pNullInputPin)))
+ return false;
+ */
+
+ if (FAILED(pGraph->AddFilter(pNull, L"NullRenderer")) ||
+ FAILED(pBuild->RenderStream(NULL, NULL, _pGrabberFilter, NULL, pNull)))
+ return false;
+
+ // get the capture source's output pin
+ CComPtr<IPin> pCapSourcePin;
+ if (FAILED(pBuild->FindPin(_pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
+ return false;
+ // checks the media type of the capture filter
+ // and if the image needs to be inverted
+ if (!checkSourceType(pCapSourcePin))
+ return false;
+
+ // release the remaining interfaces
+ pCapSourcePin.Release();
+ pNull.Release();
+ // pGrabberOutputPin.Release();
+ // pNullInputPin.Release();
+
+ return true;
}
-
/*!
- Removes all the filters from the filter graph
+ Removes all the filters from the filter graph
- \return Was the operation successful
+ \return Was the operation successful
*/
bool vpDirectShowGrabberImpl::removeAll()
{
- CComPtr<IEnumFilters> pEnum = NULL;
- CComPtr<IBaseFilter> pFilter;
- ULONG cFetched;
-
- if (FAILED(hr = pGraph->EnumFilters(&pEnum)))
- return false;
-
- while(pEnum->Next(1, &pFilter, &cFetched) == S_OK)
- {
- if(FAILED(hr = pGraph->RemoveFilter(pFilter))) return false;
- pFilter.Release();
- pEnum->Reset();
- }
+ CComPtr<IEnumFilters> pEnum = NULL;
+ CComPtr<IBaseFilter> pFilter;
+ ULONG cFetched;
+
+ if (FAILED(hr = pGraph->EnumFilters(&pEnum)))
+ return false;
+
+ while (pEnum->Next(1, &pFilter, &cFetched) == S_OK) {
+ if (FAILED(hr = pGraph->RemoveFilter(pFilter)))
+ return false;
+ pFilter.Release();
+ pEnum->Reset();
+ }
- pEnum.Release();
- return true;
+ pEnum.Release();
+ return true;
}
-
/*!
- Grabs a rgba image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
+ Grabs a rgba image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
- \param I The image to fill with the grabbed frame
+ \param I The image to fill with the grabbed frame
- \exception otherError Can't grab the frame
+ \exception otherError Can't grab the frame
*/
void vpDirectShowGrabberImpl::acquire(vpImage<vpRGBa> &I)
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //set the rgbaIm pointer on I (will be filled on the next framegrabber callback)
- sgCB.rgbaIm = &I;
- //there is an acquire demand (execute copy during callback)
- sgCB.acqRGBaDemand = true;
-
- //Run the graph to grab a frame
- pControl->Run();
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- // Wait untill it's done
- long ev;
- hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
+ // set the rgbaIm pointer on I (will be filled on the next framegrabber
+ // callback)
+ sgCB.rgbaIm = &I;
+ // there is an acquire demand (execute copy during callback)
+ sgCB.acqRGBaDemand = true;
- width = I.getWidth();
- height = I.getHeight();
+ // Run the graph to grab a frame
+ pControl->Run();
- //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") );
+ // Wait untill it's done
+ long ev;
+ hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
+ width = I.getWidth();
+ height = I.getHeight();
+ // wait for the end of the next callback (copy)
+ if (WaitForSingleObject(sgCB.copySem, MAX_DELAY) != WAIT_OBJECT_0)
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't grab the frame, callback timeout"));
}
/*!
- Grabs a grayscale image from the video stream.
- Uses a semaphore to synchronize with the framegrabber callback.
+ Grabs a grayscale image from the video stream.
+ Uses a semaphore to synchronize with the framegrabber callback.
- \param I The image to fill with the grabbed frame
+ \param I The image to fill with the grabbed frame
- \exception otherError Can't grab the frame
+ \exception otherError Can't grab the frame
*/
void vpDirectShowGrabberImpl::acquire(vpImage<unsigned char> &I)
{
- if (init==false)
- {
+ 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 callback)
- sgCB.grayIm = &I;
- //there is an acquire demand (execute copy during callback)
- sgCB.acqGrayDemand = true;
+ // set the grayIm pointer on I (will be filled on the next framegrabber
+ // callback)
+ sgCB.grayIm = &I;
+ // there is an acquire demand (execute copy during callback)
+ sgCB.acqGrayDemand = true;
- //Run the graph to grab a frame
- pControl->Run();
+ // Run the graph to grab a frame
+ pControl->Run();
- // Wait untill it's done
- long ev;
- hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
+ // Wait untill it's done
+ long ev;
+ hr = pEvent->WaitForCompletion(MAX_DELAY, &ev);
- width = I.getWidth();
- height = I.getHeight();
+ width = I.getWidth();
+ height = I.getHeight();
- //wait for the end of the next callback (copy)
- if( WaitForSingleObject(sgCB.copySem,MAX_DELAY) != WAIT_OBJECT_0)
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't grab the frame, callback timeout") );
+ // wait for the end of the next callback (copy)
+ if (WaitForSingleObject(sgCB.copySem, MAX_DELAY) != WAIT_OBJECT_0)
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't grab the frame, callback timeout"));
}
/*!
- Set the device (or camera) used by the grabber
- \param id : Identifier of the device to use.
+ Set the device (or camera) used by the grabber
+ \param id : Identifier of the device to use.
- \return true id the device is set successfully, false otherwise.
+ \return true id the device is set successfully, false otherwise.
*/
bool vpDirectShowGrabberImpl::setDevice(unsigned int id)
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
-
- //if n is invalid, or the device is already in use, quit
- if(id>=nbDevices || deviceList[id].getState()==true)
- return false;
-
- //we stop the graph
- pControl->Stop();
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- //then we can safely remove all the filters
- if(!removeAll()) return false;
+ // if n is invalid, or the device is already in use, quit
+ if (id >= nbDevices || deviceList[id].getState() == true)
+ return false;
+ // we stop the graph
+ pControl->Stop();
- //we release the previous source's interface
- pCapSource.Release();
+ // then we can safely remove all the filters
+ if (!removeAll())
+ return false;
- //here reset inUse in the old DSDevice
- deviceList[currentDevice].resetInUse();
+ // we release the previous source's interface
+ pCapSource.Release();
- //we add the grabber back in the graph
- pGraph->AddFilter(pGrabberFilter,L"SampleGrabber");
+ // here reset inUse in the old DSDevice
+ deviceList[currentDevice].resetInUse();
- //get the n-th device's filter
- if(!getDevice(id, pCapSource))
- return false;
+ // we add the grabber back in the graph
+ pGraph->AddFilter(pGrabberFilter, L"SampleGrabber");
- //we add the capture source to the filter graph
- if(FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
- return false;
+ // get the n-th device's filter
+ if (!getDevice(id, pCapSource))
+ return false;
- //we connect the pins
- if(!connectSourceToGrabber(pCapSource, pGrabberFilter))
- return false;
+ // we add the capture source to the filter graph
+ if (FAILED(hr = pGraph->AddFilter(pCapSource, L"Capture Filter")))
+ return false;
+ // we connect the pins
+ if (!connectSourceToGrabber(pCapSource, pGrabberFilter))
+ return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- {
- return false;
- }
+ // get the current connected media type (needed by the callback)
+ if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType)))) {
+ return false;
+ }
- //the device is now in use
- deviceList[id].setInUse();
- currentDevice=id;
+ // the device is now in use
+ deviceList[id].setInUse();
+ currentDevice = id;
- return true;
+ return true;
}
/*!
- Displays the list of devices on the standard output
+ Displays the list of devices on the standard output
*/
void vpDirectShowGrabberImpl::displayDevices()
{
- if(deviceList == NULL)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
+ if (deviceList == NULL) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- for(unsigned int i=0 ; i<nbDevices ; i++)
- std::cout<<i<<" : "<< deviceList[i].getName() <<std::endl;
+ for (unsigned int i = 0; i < nbDevices; i++)
+ std::cout << i << " : " << deviceList[i].getName() << std::endl;
- std::cout<<"Current device : "<<currentDevice<<std::endl<<std::endl;
+ std::cout << "Current device : " << currentDevice << std::endl << std::endl;
}
-
/*!
- Stops the framegrabber
+ Stops the framegrabber
*/
void vpDirectShowGrabberImpl::close()
{
- //the current device isn't being used anymore
- if (init) {
- deviceList[currentDevice].resetInUse();
- init = false ;
- }
- if (initCo) {
- //uninstalls COM
- CoUninitialize();
- initCo = false;
- }
+ // the current device isn't being used anymore
+ if (init) {
+ deviceList[currentDevice].resetInUse();
+ init = false;
+ }
+ if (initCo) {
+ // uninstalls COM
+ CoUninitialize();
+ initCo = false;
+ }
}
/*!
- Set image size
+ 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") );
- }
-
- return setFormat(width, height,NULL);
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
+ return setFormat(width, height, NULL);
}
/*!
- Set capture framerate
+ Set capture framerate
*/
bool vpDirectShowGrabberImpl::setFramerate(double framerate)
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- return setFormat(pVih->bmiHeader.biWidth,pVih->bmiHeader.biHeight,framerate);
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat;
+ return setFormat(pVih->bmiHeader.biWidth, pVih->bmiHeader.biHeight, framerate);
}
/*!
- Set the capture format
+ 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") );
- }
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- bool found=false;
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- //gets the video control interface
- IAMVideoControl *pVideoControl = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMVideoControl, (void**)&pVideoControl)))
- return false;
-
- //get the grabber's input pin
- CComPtr<IPin> pCapSourcePin;
- if(FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
- return false;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return false;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- // Use the video capabilities structure.
-
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
-// VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
-
-// pVih->bmiHeader.biWidth;
-// pVih->bmiHeader.biHeight;
-// 10000000 /pVih->AvgTimePerFrame;
-// std::cout<<"available image size : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<" at "<<10000000 /pVih->AvgTimePerFrame<<std::endl;
-// std::cout<<"compression : "<<pVih->bmiHeader.biCompression<<std::endl;
- if (SUCCEEDED(hr)&& found==false)
- {
- /* Examine the format, and possibly use it. */
- if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
- (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
- (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
- (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
- (pmtConfig->pbFormat != NULL))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
-
- LONG lWidth = pVih->bmiHeader.biWidth;
- LONG lHeight = pVih->bmiHeader.biHeight;
- if(framerate != NULL)
- {
- if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
- {
-
- pVih->AvgTimePerFrame = (LONGLONG)(10000000 / framerate);
- //set the capture media type and the grabber media type
- if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
- FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
- return false;
- //Run the graph to grab a frame
- pControl->Run();
-
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- LONGLONG ActualFrameDuration;
- if(FAILED(hr = pVideoControl->GetCurrentActualFrameRate(pCapSourcePin,&ActualFrameDuration)))
- std::cout<<"Current format (not sure): "<<width <<" x "<< height <<" at "<< 10000000/pVih->AvgTimePerFrame <<" fps"<<std::endl<<std::endl;
+ bool found = false;
+
+ // gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if (FAILED(hr = pBuild->FindInterface(&LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void **)&pConfig)))
+ return false;
+
+ // gets the video control interface
+ IAMVideoControl *pVideoControl = NULL;
+
+ if (FAILED(hr = pBuild->FindInterface(&LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMVideoControl, (void **)&pVideoControl)))
+ return false;
+
+ // get the grabber's input pin
+ CComPtr<IPin> pCapSourcePin;
+ if (FAILED(pBuild->FindPin(pCapSource, PINDIR_OUTPUT, NULL, NULL, false, 0, &pCapSourcePin)))
+ return false;
+
+ int iCount = 0, iSize = 0;
+ if (FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return false;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS)) {
+ // Use the video capabilities structure.
+
+ for (int iFormat = 0; iFormat < iCount; iFormat++) {
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE *)&scc);
+ // VIDEOINFOHEADER *pVih =
+ //(VIDEOINFOHEADER*)pmtConfig->pbFormat;
+
+ // pVih->bmiHeader.biWidth;
+ // pVih->bmiHeader.biHeight;
+ // 10000000 /pVih->AvgTimePerFrame;
+ // std::cout<<"available image size :
+ //"<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<" at
+ //"<<10000000 /pVih->AvgTimePerFrame<<std::endl;
+ // std::cout<<"compression :
+ //"<<pVih->bmiHeader.biCompression<<std::endl;
+ if (SUCCEEDED(hr) && found == false) {
+ /* Examine the format, and possibly use it. */
+ if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
+ (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
+ (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
+ (pmtConfig->cbFormat >= sizeof(VIDEOINFOHEADER)) && (pmtConfig->pbFormat != NULL)) {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)pmtConfig->pbFormat;
+
+ LONG lWidth = pVih->bmiHeader.biWidth;
+ LONG lHeight = pVih->bmiHeader.biHeight;
+ if (framerate != NULL) {
+ if ((unsigned int)lWidth == width && (unsigned int)lHeight == height) {
+
+ pVih->AvgTimePerFrame = (LONGLONG)(10000000 / framerate);
+ // set the capture media type and the grabber media type
+ if (FAILED(hr = pConfig->SetFormat(pmtConfig)) || FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
+ return false;
+ // Run the graph to grab a frame
+ pControl->Run();
+
+ // get the current connected media type (needed by the callback)
+ if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat;
+ LONGLONG ActualFrameDuration;
+ if (FAILED(hr = pVideoControl->GetCurrentActualFrameRate(pCapSourcePin, &ActualFrameDuration)))
+ std::cout << "Current format (not sure): " << width << " x " << height << " at "
+ << 10000000 / pVih->AvgTimePerFrame << " fps" << std::endl
+ << std::endl;
else {
- std::cout<<"Current format : "<<width <<" x "<< height <<" at "<< 10000000/ActualFrameDuration <<" fps"<<std::endl<<std::endl;
+ std::cout << "Current format : " << width << " x " << height << " at " << 10000000 / ActualFrameDuration
+ << " fps" << std::endl
+ << std::endl;
pVih->AvgTimePerFrame = ActualFrameDuration;
}
- found=true;
- }
- }
- else
- {
- if((unsigned int)lWidth == width && (unsigned int)lHeight == height)
- {
- pVih->AvgTimePerFrame = scc.MinFrameInterval;
- //set the capture media type and the grabber media type
- if(FAILED(hr = pConfig->SetFormat(pmtConfig))||
- FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
- return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- found=true;
- std::cout<<"Current format : "<<width <<" x "<< height <<" at "<<(10000000 /pVih->AvgTimePerFrame) <<" fps"<<std::endl<<std::endl;
- }
-
- }
- }
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- if(!found)
- if(framerate != NULL)
- std::cout << "The "<<width <<" x "<< height <<" at " <<framerate<<" fps source image format is not available. "<<std::endl<<std::endl;
- else
- std::cout << "The "<<width <<" x "<< height <<"source image size is not available. "<<std::endl<<std::endl;
-
- return found;
+ found = true;
+ }
+ } else {
+ if ((unsigned int)lWidth == width && (unsigned int)lHeight == height) {
+ pVih->AvgTimePerFrame = scc.MinFrameInterval;
+ // set the capture media type and the grabber media type
+ if (FAILED(hr = pConfig->SetFormat(pmtConfig)) || FAILED(hr = pGrabberI->SetMediaType(pmtConfig)))
+ return false;
+ // get the current connected media type (needed by the callback)
+ if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat;
+ found = true;
+ std::cout << "Current format : " << width << " x " << height << " at "
+ << (10000000 / pVih->AvgTimePerFrame) << " fps" << std::endl
+ << std::endl;
+ }
+ }
+ }
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ if (!found)
+ if (framerate != NULL)
+ std::cout << "The " << width << " x " << height << " at " << framerate
+ << " fps source image format is not available. " << std::endl
+ << std::endl;
+ else
+ std::cout << "The " << width << " x " << height << "source image size is not available. " << std::endl
+ << std::endl;
+
+ return found;
}
/*!
- Get the current capture format and framerate.
- \param width : Image width.
- \param height : Image height.
- \param framerate : Framerate acquisition.
+ Get the current capture format and framerate.
+ \param width : Image width.
+ \param height : Image height.
+ \param framerate : Framerate acquisition.
*/
-void vpDirectShowGrabberImpl::getFormat(unsigned int &width,unsigned int &height, double &framerate)
+void vpDirectShowGrabberImpl::getFormat(unsigned int &width, unsigned int &height, double &framerate)
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
- width = (unsigned int)pVih->bmiHeader.biWidth;
- height = (unsigned int)pVih->bmiHeader.biHeight;
- framerate = (double)(10000000/pVih->AvgTimePerFrame);
-
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat;
+ width = (unsigned int)pVih->bmiHeader.biWidth;
+ height = (unsigned int)pVih->bmiHeader.biHeight;
+ framerate = (double)(10000000 / pVih->AvgTimePerFrame);
}
/*!
- Get the available capture formats
+ Get the available capture formats
*/
bool vpDirectShowGrabberImpl::getStreamCapabilities()
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return false;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- std::cout<<"Available MediaTypes : "<<std::endl<<std::endl;
- // Use the video capabilities structure.
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
-
- if (SUCCEEDED(hr))
- {
- /* Examine the format, and possibly use it. */
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
-
-// LONG lWidth = pVih->bmiHeader.biWidth;
-// LONG lHeight = pVih->bmiHeader.biHeight;
-// SIZE dimensions={lWidth,lHeight};
-// LONGLONG lAvgTimePerFrame = pVih->AvgTimePerFrame;
- std::cout<<"MediaType : "<<iFormat<<std::endl;
-
- if(pmtConfig->subtype==MEDIASUBTYPE_ARGB32)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_ARGB32"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB32)
- std::cout<<"subtype : MEDIASUBTYPE_RGB32"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB24)
- std::cout<<"subtype : MEDIASUBTYPE_RGB24"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB555)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB555"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB565)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB565"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB8)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB8"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB4)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB4"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_RGB1)
- std::cout<<"subtype (not supported): MEDIASUBTYPE_RGB1"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YV12)
- std::cout<<"subtype : MEDIASUBTYPE_YV12"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YVU9)
- std::cout<<"subtype : MEDIASUBTYPE_YVU9"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YUY2)
- std::cout<<"subtype : MEDIASUBTYPE_YUY2"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YUYV)
- std::cout<<"subtype : MEDIASUBTYPE_YUYV"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_YVYU)
- std::cout<<"subtype : MEDIASUBTYPE_YVYU"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_IYUV)
- std::cout<<"subtype : MEDIASUBTYPE_IYUV"<<std::endl;
- else if(pmtConfig->subtype==MEDIASUBTYPE_UYVY)
- std::cout<<"subtype : MEDIASUBTYPE_UYVY"<<std::endl;
- else if((((pVih->bmiHeader.biCompression&0xFF000000)>>24) |
- ((pVih->bmiHeader.biCompression&0x00FF0000)>>8) |
- ((pVih->bmiHeader.biCompression&0x0000FF00)<<8) |
- ((pVih->bmiHeader.biCompression&0x000000FF)<<24)) == 'I420')
- std::cout<<"subtype : I420"<<std::endl;
- else std::cout<<"subtype (not supported) :"
- <<(char)(pVih->bmiHeader.biCompression&0x000000FF)
- <<(char)((pVih->bmiHeader.biCompression&0x0000FF00)>>8)
- <<(char)((pVih->bmiHeader.biCompression&0x00FF0000)>>16)
- <<(char)((pVih->bmiHeader.biCompression&0xFF000000)>>24)<<std::endl;
-
- std::cout<<"image size : "<<pVih->bmiHeader.biWidth<<" x "<<pVih->bmiHeader.biHeight<<std::endl;
- std::cout<<"framerate range: ["<< 10000000/scc.MaxFrameInterval<<","<<10000000/scc.MinFrameInterval<<"]"<<std::endl<<std::endl;
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
-/*
- long frameRateNum;
- LONGLONG *frameRateList;
- if(FAILED(hr = pVideoControl->GetFrameRateList(pCapSourcePin,iFormat,dimensions, //inputs
- &frameRateNum, &frameRateList))) //outputs
- return false;
- for(int i=0; i<(int)frameRateNum ; i++)
- {
- std::cout<<(float)(10000000/frameRateList[i])<<" fps"<<std::endl;
- }
- std::cout<<std::endl;
-*/
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- return true;
+ // gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if (FAILED(hr = pBuild->FindInterface(&LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void **)&pConfig)))
+ return false;
+
+ int iCount = 0, iSize = 0;
+ if (FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return false;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS)) {
+ std::cout << "Available MediaTypes : " << std::endl << std::endl;
+ // Use the video capabilities structure.
+ for (int iFormat = 0; iFormat < iCount; iFormat++) {
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE *)&scc);
+
+ if (SUCCEEDED(hr)) {
+ /* Examine the format, and possibly use it. */
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)pmtConfig->pbFormat;
+
+ // LONG lWidth = pVih->bmiHeader.biWidth;
+ // LONG lHeight =
+ // pVih->bmiHeader.biHeight; SIZE
+ // dimensions={lWidth,lHeight};
+ // LONGLONG lAvgTimePerFrame =
+ // pVih->AvgTimePerFrame;
+ std::cout << "MediaType : " << iFormat << std::endl;
+
+ if (pmtConfig->subtype == MEDIASUBTYPE_ARGB32)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_ARGB32" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB32)
+ std::cout << "subtype : MEDIASUBTYPE_RGB32" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB24)
+ std::cout << "subtype : MEDIASUBTYPE_RGB24" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB555)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_RGB555" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB565)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_RGB565" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB8)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_RGB8" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB4)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_RGB4" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_RGB1)
+ std::cout << "subtype (not supported): MEDIASUBTYPE_RGB1" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_YV12)
+ std::cout << "subtype : MEDIASUBTYPE_YV12" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_YVU9)
+ std::cout << "subtype : MEDIASUBTYPE_YVU9" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_YUY2)
+ std::cout << "subtype : MEDIASUBTYPE_YUY2" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_YUYV)
+ std::cout << "subtype : MEDIASUBTYPE_YUYV" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_YVYU)
+ std::cout << "subtype : MEDIASUBTYPE_YVYU" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_IYUV)
+ std::cout << "subtype : MEDIASUBTYPE_IYUV" << std::endl;
+ else if (pmtConfig->subtype == MEDIASUBTYPE_UYVY)
+ std::cout << "subtype : MEDIASUBTYPE_UYVY" << std::endl;
+ else if ((((pVih->bmiHeader.biCompression & 0xFF000000) >> 24) |
+ ((pVih->bmiHeader.biCompression & 0x00FF0000) >> 8) |
+ ((pVih->bmiHeader.biCompression & 0x0000FF00) << 8) |
+ ((pVih->bmiHeader.biCompression & 0x000000FF) << 24)) == 'I420')
+ std::cout << "subtype : I420" << std::endl;
+ else
+ std::cout << "subtype (not supported) :" << (char)(pVih->bmiHeader.biCompression & 0x000000FF)
+ << (char)((pVih->bmiHeader.biCompression & 0x0000FF00) >> 8)
+ << (char)((pVih->bmiHeader.biCompression & 0x00FF0000) >> 16)
+ << (char)((pVih->bmiHeader.biCompression & 0xFF000000) >> 24) << std::endl;
+
+ std::cout << "image size : " << pVih->bmiHeader.biWidth << " x " << pVih->bmiHeader.biHeight << std::endl;
+ std::cout << "framerate range: [" << 10000000 / scc.MaxFrameInterval << "," << 10000000 / scc.MinFrameInterval
+ << "]" << std::endl
+ << std::endl;
+
+ /*
+ long frameRateNum;
+ LONGLONG *frameRateList;
+ if(FAILED(hr =
+ pVideoControl->GetFrameRateList(pCapSourcePin,iFormat,dimensions,
+ //inputs &frameRateNum, &frameRateList))) //outputs return false;
+ for(int i=0; i<(int)frameRateNum ;
+ i++)
+ {
+ std::cout<<(float)(10000000/frameRateList[i])<<"
+ fps"<<std::endl;
+ }
+ std::cout<<std::endl;
+ */
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ return true;
}
/*!
- Set capture Mediatype
+ Set capture Mediatype
*/
bool vpDirectShowGrabberImpl::setMediaType(int mediaTypeID)
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return false;
-
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(mediaTypeID, &pmtConfig, (BYTE*)&scc);
-
-
- if (SUCCEEDED(hr))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
- pVih->AvgTimePerFrame = scc.MinFrameInterval ;
- //set the capture media type and the grabber media type
- if(FAILED(hr = pGrabberI->SetMediaType(pmtConfig))||
- FAILED(hr = pConfig->SetFormat(pmtConfig)))
- return false;
- //get the current connected media type (needed by the callback)
- if(FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
- return false;
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- return true;
+ // gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if (FAILED(hr = pBuild->FindInterface(&LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void **)&pConfig)))
+ return false;
+
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(mediaTypeID, &pmtConfig, (BYTE *)&scc);
+
+ if (SUCCEEDED(hr)) {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)pmtConfig->pbFormat;
+ pVih->AvgTimePerFrame = scc.MinFrameInterval;
+ // set the capture media type and the grabber media type
+ if (FAILED(hr = pGrabberI->SetMediaType(pmtConfig)) || FAILED(hr = pConfig->SetFormat(pmtConfig)))
+ return false;
+ // get the current connected media type (needed by the callback)
+ if (FAILED(hr = pGrabberI->GetConnectedMediaType(&(sgCB.connectedMediaType))))
+ return false;
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ return true;
}
/*
- Get current capture MediaType
- \return mediaTypeID (-1 if failed)
+ Get current capture MediaType
+ \return mediaTypeID (-1 if failed)
*/
int vpDirectShowGrabberImpl::getMediaType()
{
- if (init==false)
- {
- close();
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Initialization not done") );
- }
+ if (init == false) {
+ close();
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Initialization not done"));
+ }
- int mediaTypeID = -1;
- VIDEOINFOHEADER *pVihConnected = (VIDEOINFOHEADER*)sgCB.connectedMediaType.pbFormat;
-
- //gets the stream config interface
- IAMStreamConfig *pConfig = NULL;
-
- if(FAILED(hr = pBuild->FindInterface(
- &LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
- 0, // Any media type.
- pGrabberFilter, // Pointer to the grabber filter.
- IID_IAMStreamConfig, (void**)&pConfig)))
- return -1;
-
- int iCount = 0, iSize = 0;
- if(FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
- return -1;
-
- // Check the size to make sure we pass in the correct structure.
- if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS))
- {
- // Use the video capabilities structure.
- for (int iFormat = 0; iFormat < iCount; iFormat++)
- {
- VIDEO_STREAM_CONFIG_CAPS scc;
- AM_MEDIA_TYPE *pmtConfig;
- hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE*)&scc);
-
- if (SUCCEEDED(hr))
- {
- /* Examine the format, and possibly use it. */
- if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
- (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
- (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
- (pmtConfig->cbFormat >= sizeof (VIDEOINFOHEADER)) &&
- (pmtConfig->pbFormat != NULL))
- {
- VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER*)pmtConfig->pbFormat;
- if(pVih->bmiHeader.biWidth == pVihConnected->bmiHeader.biWidth &&
- pVih->bmiHeader.biHeight == pVihConnected->bmiHeader.biHeight)
- mediaTypeID = iFormat ;
- }
- }
- // Delete the media type when you are done.
- MyDeleteMediaType(pmtConfig);
- }
- }
- return mediaTypeID;
+ int mediaTypeID = -1;
+ VIDEOINFOHEADER *pVihConnected = (VIDEOINFOHEADER *)sgCB.connectedMediaType.pbFormat;
+
+ // gets the stream config interface
+ IAMStreamConfig *pConfig = NULL;
+
+ if (FAILED(hr = pBuild->FindInterface(&LOOK_UPSTREAM_ONLY, // Capture pin. / Preview pin
+ 0, // Any media type.
+ pGrabberFilter, // Pointer to the grabber filter.
+ IID_IAMStreamConfig, (void **)&pConfig)))
+ return -1;
+
+ int iCount = 0, iSize = 0;
+ if (FAILED(hr = pConfig->GetNumberOfCapabilities(&iCount, &iSize)))
+ return -1;
+
+ // Check the size to make sure we pass in the correct structure.
+ if (iSize == sizeof(VIDEO_STREAM_CONFIG_CAPS)) {
+ // Use the video capabilities structure.
+ for (int iFormat = 0; iFormat < iCount; iFormat++) {
+ VIDEO_STREAM_CONFIG_CAPS scc;
+ AM_MEDIA_TYPE *pmtConfig;
+ hr = pConfig->GetStreamCaps(iFormat, &pmtConfig, (BYTE *)&scc);
+
+ if (SUCCEEDED(hr)) {
+ /* Examine the format, and possibly use it. */
+ if ((pmtConfig->majortype == sgCB.connectedMediaType.majortype) &&
+ (pmtConfig->subtype == sgCB.connectedMediaType.subtype) &&
+ (pmtConfig->formattype == sgCB.connectedMediaType.formattype) &&
+ (pmtConfig->cbFormat >= sizeof(VIDEOINFOHEADER)) && (pmtConfig->pbFormat != NULL)) {
+ VIDEOINFOHEADER *pVih = (VIDEOINFOHEADER *)pmtConfig->pbFormat;
+ if (pVih->bmiHeader.biWidth == pVihConnected->bmiHeader.biWidth &&
+ pVih->bmiHeader.biHeight == pVihConnected->bmiHeader.biHeight)
+ mediaTypeID = iFormat;
+ }
+ }
+ // Delete the media type when you are done.
+ MyDeleteMediaType(pmtConfig);
+ }
+ }
+ return mediaTypeID;
}
-
/*!
- Deletes an allocated AM_MEDIA_TYPE structure, including the format block
+ Deletes an allocated AM_MEDIA_TYPE structure, including the format
+ block
*/
void vpDirectShowGrabberImpl::MyDeleteMediaType(AM_MEDIA_TYPE *pmt)
{
- if (pmt != NULL)
- {
- MyFreeMediaType(*pmt); // See FreeMediaType for the implementation.
- CoTaskMemFree(pmt);
- }
+ if (pmt != NULL) {
+ MyFreeMediaType(*pmt); // See FreeMediaType for the implementation.
+ CoTaskMemFree(pmt);
+ }
}
-
/*!
- Frees the format block in an AM_MEDIA_TYPE structure.
+ Frees the format block in an AM_MEDIA_TYPE structure.
*/
-void vpDirectShowGrabberImpl::MyFreeMediaType(AM_MEDIA_TYPE& mt)
+void vpDirectShowGrabberImpl::MyFreeMediaType(AM_MEDIA_TYPE &mt)
{
- if (mt.cbFormat != 0)
- {
- CoTaskMemFree((PVOID)mt.pbFormat);
- mt.cbFormat = 0;
- mt.pbFormat = NULL;
- }
- if (mt.pUnk != NULL)
- {
- // Unecessary because pUnk should not be used, but safest.
- mt.pUnk->Release();
- mt.pUnk = NULL;
- }
+ if (mt.cbFormat != 0) {
+ CoTaskMemFree((PVOID)mt.pbFormat);
+ mt.cbFormat = 0;
+ mt.pbFormat = NULL;
+ }
+ if (mt.pUnk != NULL) {
+ // Unecessary because pUnk should not be used, but safest.
+ mt.pUnk->Release();
+ mt.pUnk = NULL;
+ }
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowGrabberImpl.cpp.o) has no symbols
-void dummy_vpDirectShowGrabberImpl() {};
+// Work arround to avoid warning:
+// libvisp_sensor.a(vpDirectShowGrabberImpl.cpp.o) has no symbols
+void dummy_vpDirectShowGrabberImpl(){};
#endif
#endif
diff --git a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
index ae0b89d..562d159 100644
--- a/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
+++ b/modules/sensor/src/framegrabber/directshow/vpDirectShowSampleGrabberI.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,294 +38,247 @@
*****************************************************************************/
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
#include <visp3/core/vpConfig.h>
-#if ( defined(VISP_HAVE_DIRECTSHOW) )
-
-
+#if (defined(VISP_HAVE_DIRECTSHOW))
/*!
- Constructor - creates the semaphore
+ Constructor - creates the semaphore
*/
vpDirectShowSampleGrabberI::vpDirectShowSampleGrabberI()
: acqGrayDemand(false), acqRGBaDemand(false), specialMediaType(false), invertedSource(false)
{
- //semaphore(0), max value = 1
- copySem = CreateSemaphore (NULL,0,1,NULL);
+ // semaphore(0), max value = 1
+ copySem = CreateSemaphore(NULL, 0, 1, NULL);
}
/*!
- Destructor - destroys the semaphore
+ Destructor - destroys the semaphore
*/
vpDirectShowSampleGrabberI::~vpDirectShowSampleGrabberI()
{
- //destroys the semaphore
- CloseHandle(copySem);
+ // destroys the semaphore
+ CloseHandle(copySem);
}
-
STDMETHODIMP vpDirectShowSampleGrabberI::QueryInterface(REFIID riid, void **ppvObject)
{
- if (NULL == ppvObject) return E_POINTER;
- if (riid == __uuidof(IUnknown))
- {
- *ppvObject = static_cast<IUnknown*>(this);
- return S_OK;
- }
- if (riid == __uuidof(ISampleGrabberCB))
- {
- *ppvObject = static_cast<ISampleGrabberCB*>(this);
- return S_OK;
- }
- return E_NOTIMPL;
+ if (NULL == ppvObject)
+ return E_POINTER;
+ if (riid == __uuidof(IUnknown)) {
+ *ppvObject = static_cast<IUnknown *>(this);
+ return S_OK;
+ }
+ if (riid == __uuidof(ISampleGrabberCB)) {
+ *ppvObject = static_cast<ISampleGrabberCB *>(this);
+ return S_OK;
+ }
+ return E_NOTIMPL;
}
/*!
- The frame grabber callback -
- Called when the input buffer is full.
- Rq : BufferLen == bmpInfo.biWidth*bmpInfo.biHeight*sizeof(vpRGBa)
+ The frame grabber callback -
+ Called when the input buffer is full.
+ Rq : BufferLen == bmpInfo.biWidth*bmpInfo.biHeight*sizeof(vpRGBa)
*/
STDMETHODIMP vpDirectShowSampleGrabberI::BufferCB(double Time, BYTE *pBuffer, long BufferLen)
-{
- //if there has been a frame demand
- if(acqGrayDemand || acqRGBaDemand)
- {
- //check if the connected media is compatible
- if(connectedMediaType.formattype==FORMAT_VideoInfo)
- {
- //retrieve the image information
- VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER*>(connectedMediaType.pbFormat);
- BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
-
- //if biHeight > 0 and the source is not special
- //then the image needs to be verticaly flipped
- bool flip;
- if(!specialMediaType)
- flip = bmpInfo.biHeight>=0;
- //the source is fourcc and the image is inverted with this compression
- else if(invertedSource)
- flip = true;
- //fourcc and the image doesn't need to be flipped
- else
- flip = false;
+{
+ // if there has been a frame demand
+ if (acqGrayDemand || acqRGBaDemand) {
+ // check if the connected media is compatible
+ if (connectedMediaType.formattype == FORMAT_VideoInfo) {
+ // retrieve the image information
+ VIDEOINFOHEADER *pVih = reinterpret_cast<VIDEOINFOHEADER *>(connectedMediaType.pbFormat);
+ BITMAPINFOHEADER bmpInfo = pVih->bmiHeader;
+
+ // if biHeight > 0 and the source is not special
+ // then the image needs to be verticaly flipped
+ bool flip;
+ if (!specialMediaType)
+ flip = bmpInfo.biHeight >= 0;
+ // the source is fourcc and the image is inverted with this compression
+ else if (invertedSource)
+ flip = true;
+ // fourcc and the image doesn't need to be flipped
+ else
+ flip = false;
+
+ // if the buffer contains a RGB24 image (DS RGB24 <=> BGR)
+ if (connectedMediaType.subtype == MEDIASUBTYPE_RGB24) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::BGRToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap, rgbaIm->getWidth(), rgbaIm->getHeight(),
+ flip);
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::BGRToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth(), grayIm->getHeight(), flip);
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else {
+ unsigned long FourCC;
+ FourCC = ((bmpInfo.biCompression & 0xFF000000) >> 24) | ((bmpInfo.biCompression & 0x00FF0000) >> 8) |
+ ((bmpInfo.biCompression & 0x0000FF00) << 8) | (bmpInfo.biCompression & 0x000000FF) << 24;
+ // if the buffer contains a like YUV420 image
+ if (connectedMediaType.subtype == MEDIASUBTYPE_IYUV || FourCC == 'I420') {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV420ToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap, rgbaIm->getWidth(),
+ rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_YV12) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YV12ToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap, rgbaIm->getWidth(),
+ rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_YVU9) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YVU9ToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap, rgbaIm->getWidth(),
+ rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_YUY2 || connectedMediaType.subtype == MEDIASUBTYPE_YUYV) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YCbCrToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap,
+ rgbaIm->getWidth() * rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_YVYU) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YCrCbToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap,
+ rgbaIm->getWidth() * rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_UYVY) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV422ToRGBa(pBuffer, (unsigned char *)rgbaIm->bitmap,
+ rgbaIm->getWidth() * rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::YUV422ToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ } else if (connectedMediaType.subtype == MEDIASUBTYPE_RGB32) {
+ // if it was an RGBa image demand
+ if (acqRGBaDemand) {
+ // first, resizes the image as needed
+ rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ // copy(pBuffer ,pBuffer +
+ // 4*rgbaIm->getWidth()*rgbaIm->getHeight(),rgbaIm->bitmap);
+ memcpy(rgbaIm->bitmap, pBuffer, 4 * rgbaIm->getWidth() * rgbaIm->getHeight());
+ // reset the demand boolean
+ acqRGBaDemand = false;
+ } else // if it was a grayscale image demand
+ {
+ // first, resizes the image as needed
+ grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
+ // copy and convert the image
+ vpImageConvert::RGBaToGrey(pBuffer, grayIm->bitmap, grayIm->getWidth() * grayIm->getHeight());
+ // reset the demand boolean
+ acqGrayDemand = false;
+ }
+ }
+ }
+ }
- //if the buffer contains a RGB24 image (DS RGB24 <=> BGR)
- if(connectedMediaType.subtype==MEDIASUBTYPE_RGB24)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::BGRToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight(), flip);
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::BGRToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth(), grayIm->getHeight(), flip);
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else
- {
- unsigned long FourCC;
- FourCC = ((bmpInfo.biCompression&0xFF000000)>>24) |
- ((bmpInfo.biCompression&0x00FF0000)>>8) |
- ((bmpInfo.biCompression&0x0000FF00)<<8) |
- (bmpInfo.biCompression&0x000000FF)<<24;
- //if the buffer contains a like YUV420 image
- if(connectedMediaType.subtype==MEDIASUBTYPE_IYUV || FourCC == 'I420')
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
-
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YV12)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YV12ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YVU9)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YVU9ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth() , rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV420ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YUY2 ||
- connectedMediaType.subtype==MEDIASUBTYPE_YUYV)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_YVYU)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCrCbToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YCbCrToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_UYVY)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV422ToRGBa(pBuffer,(unsigned char*) rgbaIm->bitmap,
- rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::YUV422ToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- else if(connectedMediaType.subtype==MEDIASUBTYPE_RGB32)
- {
- //if it was an RGBa image demand
- if(acqRGBaDemand)
- {
- //first, resizes the image as needed
- rgbaIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- //copy(pBuffer ,pBuffer + 4*rgbaIm->getWidth()*rgbaIm->getHeight(),rgbaIm->bitmap);
- memcpy(rgbaIm->bitmap,pBuffer ,4*rgbaIm->getWidth()*rgbaIm->getHeight());
- //reset the demand boolean
- acqRGBaDemand = false;
- }
- else//if it was a grayscale image demand
- {
- //first, resizes the image as needed
- grayIm->resize(abs(bmpInfo.biHeight), bmpInfo.biWidth);
- //copy and convert the image
- vpImageConvert::RGBaToGrey(pBuffer, grayIm->bitmap,
- grayIm->getWidth() * grayIm->getHeight());
- //reset the demand boolean
- acqGrayDemand = false;
- }
- }
- }
- }
-
- //increment the semaphore - allows acquire to continue execution
- ReleaseSemaphore(copySem, 1, NULL);
- }
- return S_OK;
+ // increment the semaphore - allows acquire to continue execution
+ ReleaseSemaphore(copySem, 1, NULL);
+ }
+ return S_OK;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpDirectShowSampleGrabberI.cpp.o) has no symbols
-void dummy_vpDirectShowSampleGrabberI() {};
+// Work arround to avoid warning:
+// libvisp_sensor.a(vpDirectShowSampleGrabberI.cpp.o) has no symbols
+void dummy_vpDirectShowSampleGrabberI(){};
#endif
#endif
diff --git a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp
index bcabfae..024c269 100644
--- a/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp
+++ b/modules/sensor/src/framegrabber/flycapture/vpFlyCaptureGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,8 @@
/*!
\file vpFlyCaptureGrabber.cpp
- \brief Wrapper over PointGrey FlyCapture SDK to capture images from PointGrey cameras.
+ \brief Wrapper over PointGrey FlyCapture SDK to capture images from
+ PointGrey cameras.
*/
#include <visp3/core/vpException.h>
@@ -48,7 +50,8 @@
#include <visp3/core/vpTime.h>
/*!
- Default constructor that consider the first camera found on the bus as active.
+ Default constructor that consider the first camera found on the bus as
+ active.
*/
vpFlyCaptureGrabber::vpFlyCaptureGrabber()
: m_camera(), m_guid(), m_index(0), m_numCameras(0), m_rawImage(), m_connected(false), m_capture(false)
@@ -59,10 +62,7 @@ vpFlyCaptureGrabber::vpFlyCaptureGrabber()
/*!
Default destructor that closes the connection with the camera.
*/
-vpFlyCaptureGrabber::~vpFlyCaptureGrabber()
-{
- close();
-}
+vpFlyCaptureGrabber::~vpFlyCaptureGrabber() { close(); }
/*!
\return Return the number of cameras connected on the bus.
*/
@@ -78,10 +78,10 @@ unsigned int vpFlyCaptureGrabber::getNumCameras()
}
/*!
- Print to the output stream active camera information (serial number, camera model,
- camera vendor, sensor, resolution, firmaware version, ...).
+ Print to the output stream active camera information (serial number, camera
+ model, camera vendor, sensor, resolution, firmaware version, ...).
*/
-std::ostream &vpFlyCaptureGrabber::getCameraInfo(std::ostream & os)
+std::ostream &vpFlyCaptureGrabber::getCameraInfo(std::ostream &os)
{
this->connect();
@@ -102,15 +102,15 @@ std::ostream &vpFlyCaptureGrabber::getCameraInfo(std::ostream & os)
return os;
}
-
/*!
- Return the handler to the active camera or NULL if the camera is not connected.
- This function was designed to provide a direct access to the FlyCapture SDK to
- get access to advanced functionalities that are not implemented in this class.
+ Return the handler to the active camera or NULL if the camera is not
+connected. This function was designed to provide a direct access to the
+FlyCapture SDK to get access to advanced functionalities that are not
+implemented in this class.
- We provide here after and example that shows how to use this function to access
- to the camera and check if a given video mode and framerate are supported by the
- camera.
+ We provide here after and example that shows how to use this function to
+access to the camera and check if a given video mode and framerate are
+supported by the camera.
\code
#include <visp3/core/vpImage.h>
@@ -128,8 +128,7 @@ int main()
bool supported = false;
handler->GetVideoModeAndFrameRateInfo(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60, &supported);
if (supported)
- g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60);
- g.startCapture();
+ g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60); g.startCapture();
for(int i=0; i< nframes; i++) {
g.acquire(I);
@@ -139,9 +138,9 @@ int main()
\endcode
- 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
+ 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
#include <visp3/sensor/vpFlyCaptureGrabber.h>
int main()
@@ -165,9 +164,11 @@ int main()
}
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 << "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!
@@ -186,8 +187,7 @@ FlyCapture2::Camera *vpFlyCaptureGrabber::getCameraHandler()
if (m_connected == true) {
return &m_camera;
- }
- else {
+ } else {
return NULL;
}
}
@@ -316,9 +316,9 @@ float vpFlyCaptureGrabber::getExposure()
Return the serial id of a camera with \e index.
\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
+ The following code shows how to retrieve the serial id of all the cameras
+that are connected on the bus.
+\code
#include <visp3/sensor/vpFlyCaptureGrabber.h>
int main()
@@ -334,21 +334,21 @@ int main()
}
\endcode
- When two cameras are connected (PGR Flea3 in our case), we get the following:
+ When two cameras are connected (PGR Flea3 in our case), we get the
+ following:
\code
Camera with index 0 has serial id: 15372913
Camera with index 1 has serial id: 15290004
- \endcode
+\endcode
\sa setCameraSerial()
*/
unsigned int vpFlyCaptureGrabber::getCameraSerial(unsigned int index)
{
unsigned int num_cameras = vpFlyCaptureGrabber::getNumCameras();
- if(index >= num_cameras) {
- throw (vpException(vpException::badValue,
- "The camera with index %u is not present. Only %d cameras connected.",
- index, num_cameras) );
+ if (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;
@@ -356,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;
}
@@ -368,22 +367,20 @@ unsigned int vpFlyCaptureGrabber::getCameraSerial(unsigned int index)
with.
\param index : Current camera index, a value comprised between 0 (the first
- camera found on the bus) and the number of cameras found on the bus and returned by
- getNumCameras() minus 1. If two cameras are connected on the bus,
- setting \e index to one allows to communicate with the second
- one. This identifier is not unique. That is why, it is also possible
- to select a camera by its serial number, which is unique using
- setCameraSerial().
-
- \exception vpException::badValue : If the index is greater or equal to the number of
- cameras connected to the bus.
+ camera found on the bus) and the number of cameras found on the bus and
+ returned by getNumCameras() minus 1. If two cameras are connected on the
+ bus, setting \e index to one allows to communicate with the second one. This
+ identifier is not unique. That is why, it is also possible to select a
+ camera by its serial number, which is unique using setCameraSerial().
+
+ \exception vpException::badValue : If the index is greater or equal to the
+ number of cameras connected to the bus.
*/
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) );
+ if (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;
@@ -393,8 +390,9 @@ void vpFlyCaptureGrabber::setCameraIndex(unsigned int index)
Set the current camera from its unique serial identifier.
\param serial_id : Camera serial id.
- The following example shows how to capture images from a camera that has seial id 15290004.
- \code
+ The following example shows how to capture images from a camera that has
+seial id 15290004.
+\code
#include <visp3/sensor/vpFlyCaptureGrabber.h>
int main()
@@ -422,15 +420,13 @@ void vpFlyCaptureGrabber::setCameraSerial(unsigned int serial_id)
FlyCapture2::BusManager busMgr;
FlyCapture2::Error error;
m_numCameras = this->getNumCameras();
- for (unsigned int i=0; i<m_numCameras; i++) {
+ for (unsigned int i = 0; i < m_numCameras; i++) {
if (vpFlyCaptureGrabber::getCameraSerial(i) == serial_id) {
m_index = i;
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();
@@ -457,14 +452,15 @@ void vpFlyCaptureGrabber::setProperty(const FlyCapture2::PropertyType &prop_type
prop.onOff = on && propInfo.onOffSupported;
prop.autoManualMode = auto_on && propInfo.autoSupported;
prop.absControl = propInfo.absValSupported;
- switch(prop_value) {
+ switch (prop_value) {
case ABS_VALUE: {
- float value_ = std::max<float>(std::min<float>(value, propInfo.absMax), 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<unsigned int>(std::min<unsigned int>((unsigned int)value, propInfo.max), propInfo.min);
+ unsigned int value_ =
+ (std::max)((std::min)((unsigned int)value, (unsigned int)propInfo.max), (unsigned int)propInfo.min);
prop.valueA = value_;
break;
}
@@ -474,9 +470,7 @@ void vpFlyCaptureGrabber::setProperty(const FlyCapture2::PropertyType &prop_type
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));
}
}
}
@@ -524,9 +518,10 @@ float vpFlyCaptureGrabber::setFrameRate(float frame_rate)
/*!
Set camera shutter mode and parameter.
- \param auto_shutter : If true set auto shutter, if false set manual shutter applying \e shutter_ms parameter.
- \param shutter_ms : This is the speed at which the camera shutter opens and closes in manual mode.
- \return The measured shutter after applying the new setting.
+ \param auto_shutter : If true set auto shutter, if false set manual shutter
+applying \e shutter_ms parameter. \param shutter_ms : This is the speed at
+which the camera shutter opens and closes in manual mode. \return The measured
+shutter after applying the new setting.
The following example shows how to use this function:
\code
@@ -566,10 +561,10 @@ float vpFlyCaptureGrabber::setShutter(bool auto_shutter, float shutter_ms)
/*!
Set camera gain mode and value.
- \param gain_auto : If true set auto gain, if false set manual gain applying \e gain_value parameter.
- \param gain_value : The amount of amplification that is applied to a pixel in manual mode. An increase in gain
- can result in an increase in noise.
- \return The measured gain after applying the new setting.
+ \param gain_auto : If true set auto gain, if false set manual gain applying
+\e gain_value parameter. \param gain_value : The amount of amplification that
+is applied to a pixel in manual mode. An increase in gain can result in an
+increase in noise. \return The measured gain after applying the new setting.
The following example shows how to use this function:
\code
@@ -610,9 +605,11 @@ float vpFlyCaptureGrabber::setGain(bool gain_auto, float gain_value)
/*!
Set camera brightness mode and parameter.
- \param brightness_auto : If true set auto brightness, if false set manual brightness applying \e brightness_value parameter.
- \param brightness_value : This is the level of black in an image. A high brightness will result in a low amount of black in the image.
- \return The measured brightness after applying the new setting.
+ \param brightness_auto : If true set auto brightness, if false set manual
+brightness applying \e brightness_value parameter. \param brightness_value :
+This is the level of black in an image. A high brightness will result in a low
+amount of black in the image. \return The measured brightness after applying
+the new setting.
The following example shows how to use this function:
\code
@@ -654,15 +651,16 @@ float vpFlyCaptureGrabber::setBrightness(bool brightness_auto, float brightness_
Set camera exposure mode and parameter.
\param exposure_on : If true turn exposure on, otherwise turn off.
- \param exposure_auto : If true set auto exposure, if false set manual exposure applying \e exposure_value parameter.
- \param exposure_value : This is the average intensity of the image. It will use
- other available (non-manually adjustable) controls to adjust the image. Specifically,
- when shutter and gain are both in auto mode, manually adjusting the exposure is
- actually adjusting the auto-exposure, which tries to make the average intensity of
- the image 1/4 of the auto-exposure value e.g. exposure is 400, the camera will try
- to adjust shutter and gain so that the average image intensity is 100. When the
- auto-exposure mode is enabled for exposure, the camera tries to manipulate shutter
- and gain such that 5% of the image is saturated (pixel value of 255).
+ \param exposure_auto : If true set auto exposure, if false set manual
+exposure applying \e exposure_value parameter. \param exposure_value : This is
+the average intensity of the image. It will use other available (non-manually
+adjustable) controls to adjust the image. Specifically, when shutter and gain
+are both in auto mode, manually adjusting the exposure is actually adjusting
+the auto-exposure, which tries to make the average intensity of the image 1/4
+of the auto-exposure value e.g. exposure is 400, the camera will try to adjust
+shutter and gain so that the average image intensity is 100. When the
+ auto-exposure mode is enabled for exposure, the camera tries to manipulate
+shutter and gain such that 5% of the image is saturated (pixel value of 255).
\return The measured brightness after applying the new setting.
@@ -706,8 +704,10 @@ float vpFlyCaptureGrabber::setExposure(bool exposure_on, bool exposure_auto, flo
Set camera sharpness mode and parameter.
\param sharpness_on : If true turn sharpness on, otherwise turn off.
- \param sharpness_auto : If true set auto sharpness, if false set manual sharpness applying \e sharpness_value parameter.
- \param sharpness_value : Parameter used to tune the filter applyed on the image to reduce blurred edges in an image.
+ \param sharpness_auto : If true set auto sharpness, if false set manual
+sharpness applying \e sharpness_value parameter. \param sharpness_value :
+Parameter used to tune the filter applyed on the image to reduce blurred edges
+in an image.
\return The measured sharpness after applying the new setting.
@@ -758,11 +758,10 @@ FlyCapture2::Property vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType
FlyCapture2::Property prop;
prop.type = prop_type;
FlyCapture2::Error error;
- error = m_camera.GetProperty( &prop );
+ 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;
}
@@ -772,8 +771,7 @@ FlyCapture2::Property vpFlyCaptureGrabber::getProperty(FlyCapture2::PropertyType
\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();
@@ -784,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;
}
@@ -795,8 +792,8 @@ vpFlyCaptureGrabber::getPropertyInfo(FlyCapture2::PropertyType prop_type)
\param video_mode : Camera video mode.
\param frame_rate : Camera frame rate.
- The following example shows how to use this function to set the camera image resolution to
- 1280 x 960, pixel format to Y8 and capture framerate to 60 fps.
+ The following example shows how to use this function to set the camera image
+resolution to 1280 x 960, pixel format to Y8 and capture framerate to 60 fps.
\code
#include <visp3/core/vpImage.h>
#include <visp3/sensor/vpFlyCaptureGrabber.h>
@@ -820,8 +817,7 @@ int main()
}
\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();
@@ -829,7 +825,7 @@ void vpFlyCaptureGrabber::setVideoModeAndFrameRate(FlyCapture2::VideoMode video_
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."));
}
}
@@ -846,21 +842,21 @@ bool vpFlyCaptureGrabber::isVideoModeAndFrameRateSupported(FlyCapture2::VideoMod
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;
}
/*!
Return size and offset corresponding to a centered roi.
- \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.
+ \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<unsigned int, unsigned int>
-vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size, unsigned int step)
+std::pair<unsigned int, unsigned int> vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size,
+ unsigned int step)
{
- if (size == 0 || size > max_size) size = max_size;
+ if (size == 0 || size > max_size)
+ size = max_size;
// size should be a multiple of step
size = size / step * step;
const unsigned int offset = (max_size - size) / 2;
@@ -872,11 +868,14 @@ vpFlyCaptureGrabber::centerRoi(unsigned int size, unsigned int max_size, unsigne
Set format7 video mode.
\param format7_mode : Format 7 mode.
\param pixel_format : Pixel format.
- \param width,height : Size of the centered roi. If set to 0, use the max allowed size.
+ \param width,height : Size of the centered roi. If set to 0, use the max
+allowed size.
- If the format7 video mode and pixel format are not supported, return an exception.
+ If the format7 video mode and pixel format are not supported, return an
+exception.
- The following example shows how to use this fonction to capture a 640x480 roi:
+ The following example shows how to use this fonction to capture a 640x480
+roi:
\code
#include <iomanip>
@@ -898,8 +897,7 @@ int main()
}
\endcode
*/
-void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode,
- FlyCapture2::PixelFormat pixel_format,
+void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode, FlyCapture2::PixelFormat pixel_format,
unsigned int width, unsigned int height)
{
this->connect();
@@ -912,10 +910,10 @@ void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode,
error = m_camera.GetFormat7Info(&fmt7_info, &fmt7_supported);
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
- throw (vpException(vpException::fatalError, "Cannot get format7 info.") );
+ throw(vpException(vpException::fatalError, "Cannot get format7 info."));
}
- if (! fmt7_supported) {
- throw (vpException(vpException::fatalError, "Format7 mode %d not supported.", (int)format7_mode) );
+ if (!fmt7_supported) {
+ throw(vpException(vpException::fatalError, "Format7 mode %d not supported.", (int)format7_mode));
}
FlyCapture2::Format7ImageSettings fmt7_settings;
@@ -924,9 +922,9 @@ void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode,
// Set centered roi
std::pair<unsigned int, unsigned int> roi_w = this->centerRoi(width, fmt7_info.maxWidth, fmt7_info.imageHStepSize);
std::pair<unsigned int, unsigned int> roi_h = this->centerRoi(height, fmt7_info.maxHeight, fmt7_info.imageVStepSize);
- fmt7_settings.width = roi_w.first;
+ fmt7_settings.width = roi_w.first;
fmt7_settings.offsetX = roi_w.second;
- fmt7_settings.height = roi_h.first;
+ fmt7_settings.height = roi_h.first;
fmt7_settings.offsetY = roi_h.second;
// Validate the settings
@@ -935,15 +933,15 @@ void vpFlyCaptureGrabber::setFormat7VideoMode(FlyCapture2::Mode format7_mode,
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.") );
+ if (!valid) {
+ throw(vpException(vpException::fatalError, "Format7 settings are not valid."));
}
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."));
}
}
@@ -962,7 +960,7 @@ bool vpFlyCaptureGrabber::isFormat7Supported(FlyCapture2::Mode format7_mode)
error = m_camera.GetFormat7Info(&fmt7_info, &supported);
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
- throw (vpException(vpException::fatalError, "Cannot get format7 info.") );
+ throw(vpException(vpException::fatalError, "Cannot get format7 info."));
}
return supported;
@@ -982,8 +980,8 @@ 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", getCameraSerial(m_index)));
+ throw(vpException(vpException::fatalError, "Cannot start capture for camera with serial %u",
+ getCameraSerial(m_index)));
}
m_capture = true;
}
@@ -1006,7 +1004,7 @@ void vpFlyCaptureGrabber::stopCapture()
error = m_camera.StopCapture();
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
- throw (vpException(vpException::fatalError, "Cannot stop capture.") );
+ throw(vpException(vpException::fatalError, "Cannot stop capture."));
}
m_capture = false;
}
@@ -1027,7 +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;
@@ -1035,16 +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;
}
@@ -1067,7 +1062,7 @@ void vpFlyCaptureGrabber::disconnect()
error = m_camera.Disconnect();
if (error != FlyCapture2::PGRERROR_OK) {
error.PrintErrorTrace();
- throw (vpException(vpException::fatalError, "Cannot stop capture.") );
+ throw(vpException(vpException::fatalError, "Cannot stop capture."));
}
m_connected = false;
}
@@ -1079,8 +1074,9 @@ void vpFlyCaptureGrabber::disconnect()
/*!
Stop active camera capturing images and disconnect the active camera.
- If you want to use again this camera, you may call setCamera(const unsigned int &)
- and open(vpImage<unsigned char> &) or open(vpImage<vpRGBa> &) to connect again the camera.
+ If you want to use again this camera, you may call setCamera(const unsigned
+ int &) and open(vpImage<unsigned char> &) or open(vpImage<vpRGBa> &) to
+ connect again the camera.
Similar then calling stopCapture() and disconnect():
\code
@@ -1120,29 +1116,32 @@ void vpFlyCaptureGrabber::acquire(vpImage<unsigned char> &I, FlyCapture2::TimeSt
FlyCapture2::Error error;
// Retrieve an image
- error = m_camera.RetrieveBuffer( &m_rawImage );
+ 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", getCameraSerial(m_index)));
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %u",
+ getCameraSerial(m_index)));
}
timestamp = m_rawImage.GetTimeStamp();
- // Create a converted image
- FlyCapture2::Image convertedImage;
+ height = m_rawImage.GetRows();
+ width = m_rawImage.GetCols();
+ I.resize(height, width);
+
+ // Create a converted image using a stride equals to `sizeof(unsigned
+ // 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);
// 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", getCameraSerial(m_index)));
+ throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u",
+ getCameraSerial(m_index)));
}
- height = convertedImage.GetRows();
- width = convertedImage.GetCols();
- unsigned char *data = convertedImage.GetData();
- I.resize(height, width);
- memcpy(I.bitmap, data, height*width);
}
/*!
@@ -1169,11 +1168,11 @@ void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I, FlyCapture2::TimeStamp &ti
FlyCapture2::Error error;
// Retrieve an image
- error = m_camera.RetrieveBuffer( &m_rawImage );
+ 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", getCameraSerial(m_index)));
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %u",
+ getCameraSerial(m_index)));
}
timestamp = m_rawImage.GetTimeStamp();
@@ -1181,20 +1180,32 @@ void vpFlyCaptureGrabber::acquire(vpImage<vpRGBa> &I, FlyCapture2::TimeStamp &ti
FlyCapture2::Image convertedImage;
// Convert the raw image
- error = m_rawImage.Convert( FlyCapture2::PIXEL_FORMAT_RGBU, &convertedImage );
+ 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", getCameraSerial(m_index)));
+ throw(vpException(vpException::fatalError, "Cannot convert image from camera with serial %u",
+ getCameraSerial(m_index)));
}
height = convertedImage.GetRows();
width = convertedImage.GetCols();
- unsigned char *data = convertedImage.GetData();
I.resize(height, width);
- unsigned int bps = convertedImage.GetBitsPerPixel();
- memcpy(I.bitmap, data, width*height*bps/8);
-}
+ unsigned char *data = convertedImage.GetData();
+ unsigned int stride = convertedImage.GetStride();
+ unsigned int Bps = convertedImage.GetBitsPerPixel() / 8; // Bytes per pixel
+ // `I.bitmap` and `I[i]` are pointers to `vpRGBa` objects. While
+ // `data` is a pointer to an array of 32-bit RGBU values with each
+ // value a byte in the order of R, G, B and U and goes on.
+ for (unsigned int i = 0; i < height; ++i) {
+ for (unsigned int j = 0; j < width; ++j) {
+ unsigned char *pp = data + i * stride + j * Bps;
+ I[i][j].R = pp[0];
+ I[i][j].G = pp[1];
+ I[i][j].B = pp[2];
+ I[i][j].A = pp[3];
+ }
+ }
+}
/*!
Connect to the active camera, start capture and retrieve an image.
@@ -1246,12 +1257,12 @@ bool vpFlyCaptureGrabber::isCameraPowerAvailable()
unsigned int powerRegVal = 0;
FlyCapture2::Error error;
- error = m_camera.ReadRegister( powerReg, &powerRegVal );
- if ( error != FlyCapture2::PGRERROR_OK ) {
+ error = m_camera.ReadRegister(powerReg, &powerRegVal);
+ if (error != FlyCapture2::PGRERROR_OK) {
return false;
}
- return ( (powerRegVal & 0x00008000 ) != 0 );
+ return ((powerRegVal & 0x00008000) != 0);
}
/*!
@@ -1261,18 +1272,18 @@ bool vpFlyCaptureGrabber::isCameraPowerAvailable()
*/
bool vpFlyCaptureGrabber::getCameraPower()
{
- if ( ! isCameraPowerAvailable() )
+ if (!isCameraPowerAvailable())
return false;
const unsigned int powerReg = 0x610;
- unsigned int powerRegVal = 0 ;
+ unsigned int powerRegVal = 0;
FlyCapture2::Error error;
- error = m_camera.ReadRegister( powerReg, &powerRegVal );
- if ( error != FlyCapture2::PGRERROR_OK ) {
+ error = m_camera.ReadRegister(powerReg, &powerRegVal);
+ if (error != FlyCapture2::PGRERROR_OK) {
return false;
}
- return ( (powerRegVal & (0x1 << 31)) != 0 );
+ return ((powerRegVal & (0x1 << 31)) != 0);
}
/*!
@@ -1307,9 +1318,8 @@ void vpFlyCaptureGrabber::setCameraPower(bool on)
{
this->connect();
- if ( ! isCameraPowerAvailable() ) {
- throw (vpException(vpException::badValue,
- "Cannot power on camera. Feature not available") );
+ if (!isCameraPowerAvailable()) {
+ throw(vpException(vpException::badValue, "Cannot power on camera. Feature not available"));
}
// Power on the camera
@@ -1319,10 +1329,10 @@ void vpFlyCaptureGrabber::setCameraPower(bool on)
powerRegVal = (on == true) ? 0x80000000 : 0x0;
FlyCapture2::Error error;
- error = m_camera.WriteRegister( powerReg, powerRegVal );
+ 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;
@@ -1330,17 +1340,15 @@ void vpFlyCaptureGrabber::setCameraPower(bool on)
unsigned int retries = 10;
// Wait for camera to complete power-up
- do
- {
+ do {
vpTime::wait(millisecondsToSleep);
error = m_camera.ReadRegister(powerReg, ®Val);
if (error == FlyCapture2::PGRERROR_TIMEOUT) {
// ignore timeout errors, camera may not be responding to
// register reads during power-up
- }
- else if (error != FlyCapture2::PGRERROR_OK) {
+ } 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--;
@@ -1349,7 +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"));
}
}
@@ -1398,8 +1406,7 @@ vpFlyCaptureGrabber &vpFlyCaptureGrabber::operator>>(vpImage<vpRGBa> &I)
}
#else
-// Work arround to avoid warning: libvisp_flycapture.a(vpFlyCaptureGrabber.cpp.o) has no symbols
-void dummy_vpFlyCaptureGrabber() {};
+// Work arround to avoid warning:
+// libvisp_flycapture.a(vpFlyCaptureGrabber.cpp.o) has no symbols
+void dummy_vpFlyCaptureGrabber(){};
#endif
-
-
diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonFactory.cpp b/modules/sensor/src/framegrabber/pylon/vpPylonFactory.cpp
new file mode 100644
index 0000000..6445376
--- /dev/null
+++ b/modules/sensor/src/framegrabber/pylon/vpPylonFactory.cpp
@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Factory class used to create vpPylonGrabber instances.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPylonFactory.cpp
+ \brief Implementation file for factory class used to create
+ vpPylonGrabber instances.
+*/
+
+#include <visp3/sensor/vpPylonFactory.h>
+
+#ifdef VISP_HAVE_PYLON
+
+#include "vpPylonGrabberGigE.h"
+#include "vpPylonGrabberUsb.h"
+
+/*!
+ \brief Get the vpPylonFactory singleton.
+ */
+vpPylonFactory &vpPylonFactory::instance()
+{
+ static vpPylonFactory instance;
+
+ return instance;
+}
+
+/*!
+ \brief Create an object of vpPylonGrabber.
+
+ \param dev_class The device class. See vpPylonFactory::DeviceClass
+ for valid values.
+ \return The pointer towards the vpPylonGrabber object. It's the
+ caller's responsibility to destroy the object. NULL pointer will be
+ returned if requested object can't be properly created.
+ */
+vpPylonGrabber *vpPylonFactory::createPylonGrabber(DeviceClass dev_class)
+{
+ switch (dev_class) {
+ case BASLER_GIGE:
+ return new vpPylonGrabberGigE();
+ break;
+ case BASLER_USB:
+ return new vpPylonGrabberUsb();
+ break;
+ default:
+ return NULL;
+ break;
+ }
+}
+
+#else
+// Work arround to avoid warning:
+// libvisp_pylon.a(vpPylonFactory.cpp.o) has no symbols
+void dummy_vpPylonFactory(){};
+#endif // #ifdef VISP_HAVE_PYLON
diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp
new file mode 100644
index 0000000..8a9b14c
--- /dev/null
+++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.cpp
@@ -0,0 +1,810 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Implementation of vpPylonGrabberGigE class.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPylonGrabberGigE.cpp
+ \brief Subclass of vpPylonGrabber, implements Basler GigE cameras
+ supporting.
+*/
+
+#include "vpPylonGrabberGigE.h"
+
+#ifdef VISP_HAVE_PYLON
+
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpTime.h>
+
+/*!
+ 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)
+{
+ getNumCameras();
+}
+
+/*!
+ Default destructor that closes the connection with the camera.
+ */
+vpPylonGrabberGigE::~vpPylonGrabberGigE() { close(); }
+
+/*!
+ \return Return the number of cameras connected on the bus.
+*/
+unsigned int vpPylonGrabberGigE::getNumCameras()
+{
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices;
+ Pylon::DeviceInfoList_t filter; // Filter for GigE cameras.
+ Pylon::CBaslerGigEDeviceInfo gige_devinfo;
+ filter.push_back(gige_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ m_numCameras = lstDevices.size();
+ return m_numCameras;
+}
+
+/*!
+ Print to the output stream active camera information (serial number,
+ camera model, camera vendor, sensor, resolution, firmware version,
+ ...).
+ */
+std::ostream &vpPylonGrabberGigE::getCameraInfo(std::ostream &os)
+{
+ connect();
+
+ Pylon::CDeviceInfo deviceInfo = m_camera.GetDeviceInfo();
+ // Get the camera control object.
+ GenApi::INodeMap &control = m_camera.GetNodeMap();
+
+ GenApi::CIntegerPtr widthMax = control.GetNode("WidthMax");
+ GenApi::CIntegerPtr heightMax = control.GetNode("HeightMax");
+
+ os << "Camera information: " << std::endl;
+ 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;
+
+ return os;
+}
+
+/*!
+ Return the handler to the active camera or NULL if the camera is not
+ connected. This function was designed to provide a direct access to
+ the Pylon SDK to get access to advanced functionalities that are not
+ implemented in this class.
+*/
+Pylon::CInstantCamera *vpPylonGrabberGigE::getCameraHandler()
+{
+ connect();
+
+ if (m_connected == true) {
+ return &m_camera;
+ } else {
+ return NULL;
+ }
+}
+
+/*!
+ Return camera capture framerate.
+ If the camera doesn't support framerate property, return an exception.
+
+ \sa setFrameRate()
+ */
+float vpPylonGrabberGigE::getFrameRate()
+{
+ connect();
+
+ float frame_rate = m_camera.AcquisitionFrameRateAbs.GetValue();
+ return frame_rate;
+}
+
+/*!
+ Return camera gain value in dB or raw value.
+ If the camera doesn't support gain property, return an exception.
+
+ \sa setGain()
+ */
+float vpPylonGrabberGigE::getGain()
+{
+ connect();
+
+ if (GenApi::IsReadable(m_camera.GainAbs))
+ return m_camera.GainAbs.GetValue();
+ else if (GenApi::IsReadable(m_camera.GainRaw))
+ return m_camera.GainRaw.GetValue();
+ else
+ throw vpException(vpException::notImplementedError, "Don't know how to get gain.");
+}
+
+/*!
+ Return blacklevel value in % or raw value.
+ If the camera doesn't support blacklevel property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setBlackLevel()
+ */
+float vpPylonGrabberGigE::getBlackLevel()
+{
+ connect();
+
+ if (GenApi::IsReadable(m_camera.BlackLevelAbs))
+ return m_camera.BlackLevelAbs.GetValue();
+ else if (GenApi::IsReadable(m_camera.BlackLevelRaw))
+ return m_camera.BlackLevelRaw.GetValue();
+ else
+ throw vpException(vpException::notImplementedError, "Don't know how to get blacklevel.");
+}
+
+/*!
+ Return exposure value in ms.
+ If the camera doesn't support exposure property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setExposure()
+ */
+float vpPylonGrabberGigE::getExposure()
+{
+ connect();
+
+ if (GenApi::IsReadable(m_camera.ExposureTimeAbs))
+ return m_camera.ExposureTimeAbs.GetValue() * 0.001;
+ else if (GenApi::IsReadable(m_camera.ExposureTimeRaw))
+ return m_camera.ExposureTimeRaw.GetValue();
+ else
+ throw vpException(vpException::notImplementedError, "Don't know how to get exposure.");
+}
+
+/*!
+ Return gamma correction value.
+ If the camera doesn't support gamma property, return an exception.
+
+ \sa setGamma()
+ */
+float vpPylonGrabberGigE::getGamma()
+{
+ connect();
+
+ float gamma = m_camera.Gamma.GetValue();
+ return gamma;
+}
+
+/*!
+ Return the serial id of a camera with \e index.
+ \param index : Camera index.
+
+ \sa setCameraSerial()
+ */
+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));
+ }
+
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices; // List of connected cameras
+ Pylon::DeviceInfoList_t filter; // Filter for GigE cameras.
+ Pylon::CBaslerGigEDeviceInfo gige_devinfo;
+ filter.push_back(gige_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ std::ostringstream os;
+ os << lstDevices[index].GetSerialNumber();
+ return os.str();
+}
+
+/*!
+ \brief Loads the selected configuration into the camera's volatile
+ memory and makes it the active configuration set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa saveUserSet()
+ */
+bool vpPylonGrabberGigE::loadUserSet(UserSetName user_set)
+{
+ connect();
+
+ bool success = selectUserSet(user_set);
+
+ if (success) {
+ m_camera.UserSetLoad.Execute();
+ vpTime::wait(200); // How long you have to wait?
+ success = m_camera.UserSetLoad.IsDone();
+ }
+
+ return success;
+}
+
+/*!
+ \brief Gets the configuration set being used as the default startup
+ set.
+
+ \sa setUserSetDefault()
+ */
+vpPylonGrabber::UserSetName vpPylonGrabberGigE::getUserSetDefault()
+{
+ connect();
+
+ Basler_GigECamera::UserSetDefaultSelectorEnums user_set = m_camera.UserSetDefaultSelector.GetValue();
+
+ switch (user_set) {
+ case Basler_GigECamera::UserSetDefaultSelector_Default:
+ return USERSET_DEFAULT;
+ break;
+ case Basler_GigECamera::UserSetDefaultSelector_UserSet1:
+ return USERSET_USERSET1;
+ break;
+ case Basler_GigECamera::UserSetDefaultSelector_UserSet2:
+ return USERSET_USERSET2;
+ break;
+ case Basler_GigECamera::UserSetDefaultSelector_UserSet3:
+ return USERSET_USERSET3;
+ break;
+ default:
+ return USERSET_UNKNOWN;
+ }
+}
+
+/*!
+ If multiples cameras are connected on the bus, select the camera to
+ dial with.
+
+ \param index : Current camera index, a value comprised between 0 (the
+ first camera found on the bus) and the number of cameras found and
+ returned by getNumCameras() minus 1.
+
+ If two cameras are connected on the bus, setting \e index to one
+ allows to communicate with the second one. This identifier is not
+ unique. That is why, it is also possible to select a camera by its
+ serial number, which is unique using setCameraSerial().
+
+ \exception vpException::badValue : If the index is greater or equal to
+ the number of cameras connected to the bus.
+ */
+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));
+ }
+
+ m_index = index;
+}
+
+/*!
+ Set the current camera from its unique serial identifier.
+ \param serial : Camera serial string.
+
+ \sa getCameraSerial()
+ */
+void vpPylonGrabberGigE::setCameraSerial(const std::string &serial)
+{
+ m_numCameras = getNumCameras();
+ for (unsigned int i = 0; i < m_numCameras; i++) {
+ if (getCameraSerial(i) == serial) {
+ m_index = i;
+ return;
+ }
+ }
+ throw(vpException(vpException::badValue, "The camera with serial id %s is not present.", serial.c_str()));
+}
+
+/*!
+ Set camera frame rate.
+ \param frame_rate : Camera frame rate (fps) to set.
+ \return The camera current framerate.
+
+ \sa getFramerate()
+ */
+float vpPylonGrabberGigE::setFrameRate(float frame_rate)
+{
+ connect();
+
+ m_camera.AcquisitionFrameRateAbs.SetValue(frame_rate);
+
+ return m_camera.AcquisitionFrameRateAbs.GetValue();
+}
+
+/*!
+ Set camera gain mode and value.
+ \param gain_auto : If true set auto gain, if false set manual gain
+ applying \e gain_value parameter.
+ \param gain_value : The amount of amplification that is applied to a
+ pixel in manual mode. An increase in gain can result in an increase in
+ noise.
+
+ \return The measured gain after applying the new setting.
+
+ \sa getGain()
+
+ */
+float vpPylonGrabberGigE::setGain(bool gain_auto, float gain_value)
+{
+ connect();
+
+ if (gain_auto)
+ m_camera.GainAuto.SetValue(Basler_GigECamera::GainAuto_Continuous);
+ else
+ m_camera.GainAuto.SetValue(Basler_GigECamera::GainAuto_Off);
+
+ if (GenApi::IsWritable(m_camera.GainAbs)) {
+ m_camera.GainAbs.SetValue(gain_value);
+ return m_camera.GainAbs.GetValue();
+ } else if (GenApi::IsWritable(m_camera.GainRaw)) {
+ m_camera.GainRaw.SetValue(gain_value);
+ return m_camera.GainRaw.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set gain.");
+}
+
+/*!
+ Set camera blacklevel mode and parameter.
+ \param blacklevel_value : This is the level of black in an image. A
+ high blacklevel will result in a low amount of black in the image.
+
+ \return The measured blacklevel after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getBlackLevel()
+ */
+float vpPylonGrabberGigE::setBlackLevel(float blacklevel_value)
+{
+ connect();
+
+ if (GenApi::IsWritable(m_camera.BlackLevelAbs)) {
+ m_camera.BlackLevelAbs.SetValue(blacklevel_value);
+ return m_camera.BlackLevelAbs.GetValue();
+ } else if (GenApi::IsWritable(m_camera.BlackLevelRaw)) {
+ m_camera.BlackLevelRaw.SetValue(blacklevel_value);
+ return m_camera.BlackLevelRaw.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set blacklevel.");
+}
+
+/*!
+ Set camera exposure mode and parameter.
+
+ \param exposure_on : If true turn exposure on, otherwise turn off.
+ \param exposure_auto : If true set auto exposure, if false set manual
+exposure applying \e exposure_value parameter.
+ \param exposure_value : This is the exposure value in ms.
+
+ \return The measured exposure time in ms after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getExposure()
+ */
+float vpPylonGrabberGigE::setExposure(bool exposure_on, bool exposure_auto, float exposure_value)
+{
+ connect();
+
+ if (exposure_on)
+ m_camera.ExposureMode.SetValue(Basler_GigECamera::ExposureMode_Timed);
+ else
+ m_camera.ExposureMode.SetValue(Basler_GigECamera::ExposureMode_Off);
+
+ if (exposure_auto)
+ m_camera.ExposureAuto.SetValue(Basler_GigECamera::ExposureAuto_Continuous);
+ else
+ m_camera.ExposureAuto.SetValue(Basler_GigECamera::ExposureAuto_Off);
+
+ if (GenApi::IsWritable(m_camera.ExposureTimeAbs)) {
+ m_camera.ExposureTimeAbs.SetValue(exposure_value * 1000);
+ return m_camera.ExposureTimeAbs.GetValue() * 0.001;
+ } else if (GenApi::IsWritable(m_camera.ExposureTimeRaw)) {
+ m_camera.ExposureTimeRaw.SetValue(exposure_value);
+ return m_camera.ExposureTimeRaw.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set exposure.");
+}
+
+/*!
+ Set camera gamma correction mode and parameter.
+
+ \param gamma_on : If true turn gamma correction on, otherwise turn off.
+ \param gamma_value : Parameter used to perform gamma correction of
+ pixel intensity.
+
+ \return The measured gamma correction value after applying the new
+ setting.
+
+ \sa getGamma()
+ */
+float vpPylonGrabberGigE::setGamma(bool gamma_on, float gamma_value)
+{
+ connect();
+
+ if (GenApi::IsWritable(m_camera.GammaEnable))
+ m_camera.GammaEnable.SetValue(gamma_on);
+
+ if (GenApi::IsWritable(m_camera.Gamma)) {
+ m_camera.Gamma.SetValue(gamma_value);
+ return m_camera.Gamma.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set gamma.");
+}
+
+/*!
+ \brief Saves the current active configuration set into the selected
+ user set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \param set_default Whether to set the configuration set to be used as
+ the default startup set.
+ \return true for finished, false otherwise.
+
+ \sa loadUserSet()
+ */
+bool vpPylonGrabberGigE::saveUserSet(UserSetName user_set, bool set_default)
+{
+ connect();
+
+ bool success = selectUserSet(user_set);
+
+ if (success) {
+ m_camera.UserSetSave.Execute();
+ vpTime::wait(200); // How long you have to wait?
+ success = m_camera.UserSetSave.IsDone();
+ }
+
+ if (success && set_default)
+ success = setUserSetDefault(user_set);
+
+ return success;
+}
+
+/*!
+ \brief Sets the configuration set to be used as the default startup
+ set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa getUserSetDefault()
+ */
+bool vpPylonGrabberGigE::setUserSetDefault(UserSetName user_set)
+{
+ connect();
+
+ switch (user_set) {
+ case USERSET_DEFAULT:
+ m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_Default);
+ return true;
+ break;
+ case USERSET_USERSET1:
+ m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet1);
+ return true;
+ break;
+ case USERSET_USERSET2:
+ m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet2);
+ return true;
+ break;
+ case USERSET_USERSET3:
+ m_camera.UserSetDefaultSelector.SetValue(Basler_GigECamera::UserSetDefaultSelector_UserSet3);
+ return true;
+ break;
+ default:
+ return false;
+ }
+}
+
+/*!
+ Start active camera capturing images.
+
+ \sa stopCapture()
+ */
+void vpPylonGrabberGigE::startCapture()
+{
+ connect();
+
+ if (!m_camera.IsGrabbing()) {
+ m_camera.StartGrabbing(1);
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Stop active camera capturing images.
+
+ \sa startCapture()
+ */
+void vpPylonGrabberGigE::stopCapture()
+{
+ if (m_camera.IsGrabbing()) {
+ m_camera.StopGrabbing();
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Connect the active camera.
+
+ \sa disconnect()
+ */
+void vpPylonGrabberGigE::connect()
+{
+ if (m_connected == false) {
+ m_numCameras = getNumCameras();
+ if (m_numCameras == 0) {
+ throw(vpException(vpException::fatalError, "No camera found"));
+ }
+
+ if (!m_camera.IsPylonDeviceAttached()) {
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices;
+ Pylon::DeviceInfoList_t filter; // Filter for GigE cameras.
+ Pylon::CBaslerGigEDeviceInfo gige_devinfo;
+ filter.push_back(gige_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ m_camera.Attach(TlFactory.CreateDevice(lstDevices[m_index]));
+ }
+ // Connect to a camera
+ m_camera.Open();
+ m_connected = true;
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Disconnect the active camera.
+
+ \sa connect()
+ */
+void vpPylonGrabberGigE::disconnect()
+{
+ if (m_connected == true) {
+ m_camera.Close();
+ m_connected = false;
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ \brief Stop active camera capturing images and disconnect the active
+ camera.
+
+ If you want to use again this camera, you may call
+ setCameraIndex(const unsigned int &) and open(vpImage<unsigned char>
+ &) or open(vpImage<vpRGBa> &) to connect again the camera.
+ */
+void vpPylonGrabberGigE::close()
+{
+ stopCapture();
+ disconnect();
+}
+
+/*!
+ Acquire a gray level image from the active camera.
+
+ \param I : Image data structure (8 bits image).
+*/
+void vpPylonGrabberGigE::acquire(vpImage<unsigned char> &I)
+{
+ open();
+
+ Pylon::CGrabResultPtr grabResult;
+ // Retrieve an image
+ if (!m_camera.RetrieveResult(2000, grabResult)) {
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s",
+ getCameraSerial(m_index).c_str()));
+ }
+
+ if (grabResult->GrabSucceeded()) {
+ height = grabResult->GetHeight();
+ width = grabResult->GetWidth();
+ I.resize(height, width);
+
+ Pylon::CImageFormatConverter imageConvert;
+ imageConvert.OutputPixelFormat = Pylon::PixelType_Mono8;
+ imageConvert.OutputPaddingX = 0;
+ // Create a converted image
+ imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, (Pylon::IImage &)grabResult);
+ }
+}
+
+/*!
+ Acquire a color image from the active camera.
+
+ \param I : Image data structure (RGBa image).
+*/
+void vpPylonGrabberGigE::acquire(vpImage<vpRGBa> &I)
+{
+ open();
+
+ Pylon::CGrabResultPtr grabResult;
+ // Retrieve an image
+ if (!m_camera.RetrieveResult(2000, grabResult)) {
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s",
+ getCameraSerial(m_index).c_str()));
+ }
+
+ if (grabResult->GrabSucceeded()) {
+ height = grabResult->GetHeight();
+ width = grabResult->GetWidth();
+ I.resize(height, width);
+
+ Pylon::CImageFormatConverter imageConvert;
+ imageConvert.OutputPixelFormat = Pylon::PixelType_BGRA8packed;
+ imageConvert.OutputPaddingX = 0;
+ // Create a converted image
+ Pylon::CPylonImage destImage;
+ imageConvert.Convert(destImage, (Pylon::IImage &)grabResult);
+ Pylon::SBGRA8Pixel *pixel = (Pylon::SBGRA8Pixel *)destImage.GetBuffer();
+ for (unsigned int i = 0; i < height; i++) {
+ for (unsigned int j = 0; j < width; j++) {
+ unsigned int p_index = i * width + j;
+ I[i][j].R = pixel[p_index].R;
+ I[i][j].G = pixel[p_index].G;
+ I[i][j].B = pixel[p_index].B;
+ I[i][j].A = pixel[p_index].A;
+ }
+ }
+ }
+}
+
+/*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+void vpPylonGrabberGigE::open(vpImage<unsigned char> &I)
+{
+ open();
+ acquire(I);
+}
+
+/*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+void vpPylonGrabberGigE::open(vpImage<vpRGBa> &I)
+{
+ open();
+ acquire(I);
+}
+
+/*!
+ Connect to the active camera and start capture.
+
+ Similar then calling:
+ \code
+ vpPylonGrabberGigE g;
+ ...
+ g.connect();
+ g.startCapture();
+ \endcode
+ */
+void vpPylonGrabberGigE::open()
+{
+ connect();
+ startCapture();
+}
+
+/*!
+ \brief Selects the configuration set to load, save, or configure.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for success, false for failure.
+
+ Default User Set is read-only and cannot be modified.
+ */
+bool vpPylonGrabberGigE::selectUserSet(UserSetName user_set)
+{
+ connect();
+
+ switch (user_set) {
+ case USERSET_DEFAULT:
+ m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_Default);
+ return true;
+ break;
+ case USERSET_USERSET1:
+ m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet1);
+ return true;
+ break;
+ case USERSET_USERSET2:
+ m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet2);
+ return true;
+ break;
+ case USERSET_USERSET3:
+ m_camera.UserSetSelector.SetValue(Basler_GigECamera::UserSetSelector_UserSet3);
+ return true;
+ break;
+ default:
+ return false;
+ }
+}
+
+/*!
+ Operator that allows to capture a grey level image.
+ \param I : The captured image.
+ */
+vpPylonGrabber &vpPylonGrabberGigE::operator>>(vpImage<unsigned char> &I)
+{
+ acquire(I);
+ return *this;
+}
+
+/*!
+ Operator that allows to capture a color image.
+ \param I : The captured image.
+ */
+vpPylonGrabber &vpPylonGrabberGigE::operator>>(vpImage<vpRGBa> &I)
+{
+ acquire(I);
+ return *this;
+}
+
+#else
+// Work arround to avoid warning:
+// libvisp_pylon.a(vpPylonGrabberGigE.cpp.o) has no symbols
+void dummy_vpPylonGrabberGigE(){};
+#endif // #ifdef VISP_HAVE_PYLON
diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h
new file mode 100644
index 0000000..b746693
--- /dev/null
+++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberGigE.h
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Header of vpPylonGrabberGigE class.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPylonGrabberGigE.h
+ \brief Subclass of vpPylonGrabber, implements Basler GigE cameras
+ supporting.
+*/
+
+#ifndef __vpPylonGrabberGigE_h_
+#define __vpPylonGrabberGigE_h_
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/sensor/vpPylonGrabber.h>
+
+#ifdef VISP_HAVE_PYLON
+
+#include <pylon/gige/BaslerGigEInstantCamera.h>
+
+/*!
+ \class vpPylonGrabberGigE
+
+ Allows to grab images from a Basler GigE camera using Pylon SDK.
+
+ This class should not be instantiated directly. Use
+ vpPylonFactory::createPylonGrabber() instead.
+
+ \headerfile vpPylonGrabberGigE.h ""
+ */
+class VISP_EXPORT vpPylonGrabberGigE : public vpPylonGrabber
+{
+public:
+ vpPylonGrabberGigE();
+ ~vpPylonGrabberGigE();
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+
+ void close();
+ void connect();
+ void disconnect();
+
+ float getBlackLevel();
+ std::ostream &getCameraInfo(std::ostream &os);
+ Pylon::CInstantCamera *getCameraHandler();
+ /*! Return the index of the active camera. */
+ unsigned int getCameraIndex() const { return m_index; };
+ std::string getCameraSerial(unsigned int index);
+ float getExposure();
+ float getFrameRate();
+ float getGain();
+ unsigned int getNumCameras();
+ float getGamma();
+ bool loadUserSet(UserSetName user_set);
+ UserSetName getUserSetDefault();
+
+ //! Return true if the camera is connected.
+ bool isConnected() const { return m_connected; }
+ //! Return true if the camera capture is started.
+ bool isCaptureStarted() const { return m_camera.IsGrabbing(); }
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ vpPylonGrabber &operator>>(vpImage<unsigned char> &I);
+ vpPylonGrabber &operator>>(vpImage<vpRGBa> &I);
+
+ 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 setGain(bool gain_auto, float gain_value = 0);
+ float setFrameRate(float frame_rate);
+ float setGamma(bool gamma_on, float gamma_value = 1);
+ bool saveUserSet(UserSetName user_set, bool set_default = false);
+ bool setUserSetDefault(UserSetName user_set);
+
+ void startCapture();
+ void stopCapture();
+
+protected:
+ void open();
+ bool selectUserSet(UserSetName user_set);
+
+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
+};
+
+#endif // #ifdef VISP_HAVE_PYLON
+#endif // #ifndef __vpPylonGrabberGigE_h_
diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp
new file mode 100644
index 0000000..c84226b
--- /dev/null
+++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.cpp
@@ -0,0 +1,795 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Implementation of vpPylonGrabberUsb class.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPylonGrabberUsb.cpp
+ \brief Subclass of vpPylonGrabber, implements Basler USB3 cameras
+ supporting.
+*/
+
+#include "vpPylonGrabberUsb.h"
+
+#ifdef VISP_HAVE_PYLON
+
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpTime.h>
+
+/*!
+ 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)
+{
+ getNumCameras();
+}
+
+/*!
+ Default destructor that closes the connection with the camera.
+ */
+vpPylonGrabberUsb::~vpPylonGrabberUsb() { close(); }
+
+/*!
+ \return Return the number of cameras connected on the bus.
+*/
+unsigned int vpPylonGrabberUsb::getNumCameras()
+{
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices;
+ Pylon::DeviceInfoList_t filter; // Filter for USB cameras.
+ Pylon::CBaslerUsbDeviceInfo usb_devinfo;
+ filter.push_back(usb_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ m_numCameras = lstDevices.size();
+ return m_numCameras;
+}
+
+/*!
+ Print to the output stream active camera information (serial number,
+ camera model, camera vendor, sensor, resolution, firmware version,
+ ...).
+ */
+std::ostream &vpPylonGrabberUsb::getCameraInfo(std::ostream &os)
+{
+ connect();
+
+ Pylon::CDeviceInfo deviceInfo = m_camera.GetDeviceInfo();
+ // Get the camera control object.
+ GenApi::INodeMap &control = m_camera.GetNodeMap();
+
+ GenApi::CIntegerPtr widthMax = control.GetNode("WidthMax");
+ GenApi::CIntegerPtr heightMax = control.GetNode("HeightMax");
+
+ os << "Camera information: " << std::endl;
+ 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;
+
+ return os;
+}
+
+/*!
+ Return the handler to the active camera or NULL if the camera is not
+ connected. This function was designed to provide a direct access to
+ the Pylon SDK to get access to advanced functionalities that are not
+ implemented in this class.
+*/
+Pylon::CInstantCamera *vpPylonGrabberUsb::getCameraHandler()
+{
+ connect();
+
+ if (m_connected == true) {
+ return &m_camera;
+ } else {
+ return NULL;
+ }
+}
+
+/*!
+ Return camera capture framerate.
+ If the camera doesn't support framerate property, return an exception.
+
+ \sa setFrameRate()
+ */
+float vpPylonGrabberUsb::getFrameRate()
+{
+ connect();
+
+ float frame_rate = m_camera.AcquisitionFrameRate.GetValue();
+ return frame_rate;
+}
+
+/*!
+ Return camera gain value in dB or raw value.
+ If the camera doesn't support gain property, return an exception.
+
+ \sa setGain()
+ */
+float vpPylonGrabberUsb::getGain()
+{
+ connect();
+
+ if (GenApi::IsReadable(m_camera.Gain))
+ return m_camera.Gain.GetValue();
+ else
+ throw vpException(vpException::notImplementedError, "Don't know how to get gain.");
+}
+
+/*!
+ Return blacklevel value in % or raw value.
+ If the camera doesn't support blacklevel property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setBlackLevel()
+ */
+float vpPylonGrabberUsb::getBlackLevel()
+{
+ connect();
+
+ if (GenApi::IsReadable(m_camera.BlackLevel))
+ return m_camera.BlackLevel.GetValue();
+ else
+ throw vpException(vpException::notImplementedError, "Don't know how to get blacklevel.");
+}
+
+/*!
+ Return exposure value in ms.
+ If the camera doesn't support exposure property, return an exception.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa setExposure()
+ */
+float vpPylonGrabberUsb::getExposure()
+{
+ connect();
+
+ 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.");
+}
+
+/*!
+ Return gamma correction value.
+ If the camera doesn't support gamma property, return an exception.
+
+ \sa setGamma()
+ */
+float vpPylonGrabberUsb::getGamma()
+{
+ connect();
+
+ float gamma = m_camera.Gamma.GetValue();
+ return gamma;
+}
+
+/*!
+ Return the serial id of a camera with \e index.
+ \param index : Camera index.
+
+ \sa setCameraSerial()
+ */
+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));
+ }
+
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices; // List of connected cameras
+ Pylon::DeviceInfoList_t filter; // Filter for USB cameras.
+ Pylon::CBaslerUsbDeviceInfo usb_devinfo;
+ filter.push_back(usb_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ std::ostringstream os;
+ os << lstDevices[index].GetSerialNumber();
+ return os.str();
+}
+
+/*!
+ \brief Loads the selected configuration into the camera's volatile
+ memory and makes it the active configuration set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa saveUserSet()
+ */
+bool vpPylonGrabberUsb::loadUserSet(UserSetName user_set)
+{
+ connect();
+
+ bool success = selectUserSet(user_set);
+
+ if (success) {
+ m_camera.UserSetLoad.Execute();
+ vpTime::wait(200); // How long you have to wait?
+ success = m_camera.UserSetLoad.IsDone();
+ }
+
+ return success;
+}
+
+/*!
+ \brief Gets the configuration set being used as the default startup
+ set.
+
+ \sa setUserSetDefault()
+ */
+vpPylonGrabber::UserSetName vpPylonGrabberUsb::getUserSetDefault()
+{
+ connect();
+
+ Basler_UsbCameraParams::UserSetDefaultEnums user_set = m_camera.UserSetDefault.GetValue();
+
+ switch (user_set) {
+ case Basler_UsbCameraParams::UserSetDefault_Default:
+ return USERSET_DEFAULT;
+ break;
+ case Basler_UsbCameraParams::UserSetDefault_UserSet1:
+ return USERSET_USERSET1;
+ break;
+ case Basler_UsbCameraParams::UserSetDefault_UserSet2:
+ return USERSET_USERSET2;
+ break;
+ case Basler_UsbCameraParams::UserSetDefault_UserSet3:
+ return USERSET_USERSET3;
+ break;
+ default:
+ return USERSET_UNKNOWN;
+ }
+}
+
+/*!
+ If multiples cameras are connected on the bus, select the camera to
+ dial with.
+
+ \param index : Current camera index, a value comprised between 0 (the
+ first camera found on the bus) and the number of cameras found and
+ returned by getNumCameras() minus 1.
+
+ If two cameras are connected on the bus, setting \e index to one
+ allows to communicate with the second one. This identifier is not
+ unique. That is why, it is also possible to select a camera by its
+ serial number, which is unique using setCameraSerial().
+
+ \exception vpException::badValue : If the index is greater or equal to
+ the number of cameras connected to the bus.
+ */
+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));
+ }
+
+ m_index = index;
+}
+
+/*!
+ Set the current camera from its unique serial identifier.
+ \param serial : Camera serial string.
+
+ \sa getCameraSerial()
+ */
+void vpPylonGrabberUsb::setCameraSerial(const std::string &serial)
+{
+ m_numCameras = getNumCameras();
+ for (unsigned int i = 0; i < m_numCameras; i++) {
+ if (getCameraSerial(i) == serial) {
+ m_index = i;
+ return;
+ }
+ }
+ throw(vpException(vpException::badValue, "The camera with serial id %s is not present.", serial.c_str()));
+}
+
+/*!
+ Set camera frame rate.
+ \param frame_rate : Camera frame rate (fps) to set.
+ \return The camera current framerate.
+
+ \sa getFramerate()
+ */
+float vpPylonGrabberUsb::setFrameRate(float frame_rate)
+{
+ connect();
+
+ m_camera.AcquisitionFrameRate.SetValue(frame_rate);
+
+ return m_camera.AcquisitionFrameRate.GetValue();
+}
+
+/*!
+ Set camera gain mode and value.
+ \param gain_auto : If true set auto gain, if false set manual gain
+ applying \e gain_value parameter.
+ \param gain_value : The amount of amplification that is applied to a
+ pixel in manual mode. An increase in gain can result in an increase in
+ noise.
+
+ \return The measured gain after applying the new setting.
+
+ \sa getGain()
+
+ */
+float vpPylonGrabberUsb::setGain(bool gain_auto, float gain_value)
+{
+ connect();
+
+ if (gain_auto)
+ m_camera.GainAuto.SetValue(Basler_UsbCameraParams::GainAuto_Continuous);
+ else
+ m_camera.GainAuto.SetValue(Basler_UsbCameraParams::GainAuto_Off);
+
+ if (GenApi::IsWritable(m_camera.Gain)) {
+ m_camera.Gain.SetValue(gain_value);
+ return m_camera.Gain.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set gain.");
+}
+
+/*!
+ Set camera blacklevel mode and parameter.
+ \param blacklevel_value : This is the level of black in an image. A
+ high blacklevel will result in a low amount of black in the image.
+
+ \return The measured blacklevel after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Black level is used instead of brightness.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getBlackLevel()
+ */
+float vpPylonGrabberUsb::setBlackLevel(float blacklevel_value)
+{
+ connect();
+
+ if (GenApi::IsWritable(m_camera.BlackLevel)) {
+ m_camera.BlackLevel.SetValue(blacklevel_value);
+ return m_camera.BlackLevel.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set blacklevel.");
+}
+
+/*!
+ Set camera exposure mode and parameter.
+
+ \param exposure_on : If true turn exposure on, otherwise turn off.
+ \param exposure_auto : If true set auto exposure, if false set manual
+exposure applying \e exposure_value parameter.
+ \param exposure_value : This is the exposure value in ms.
+
+ \return The measured exposure time in ms after applying the new setting.
+
+ According to SFNC (standard feature naming convention) of GenICam
+ standard, Exposure is used and deprecates shutter.
+
+ See "Terminology Changes" section of the page:
+ https://www.ptgrey.com/kb/11020?countryid=237
+ \sa getExposure()
+ */
+float vpPylonGrabberUsb::setExposure(bool exposure_on, bool exposure_auto, float exposure_value)
+{
+ connect();
+
+ if (exposure_on)
+ m_camera.ExposureMode.SetValue(Basler_UsbCameraParams::ExposureMode_Timed);
+ else
+ m_camera.ExposureMode.SetValue(Basler_UsbCameraParams::ExposureMode_TriggerWidth);
+
+ if (exposure_auto)
+ m_camera.ExposureAuto.SetValue(Basler_UsbCameraParams::ExposureAuto_Continuous);
+ else
+ m_camera.ExposureAuto.SetValue(Basler_UsbCameraParams::ExposureAuto_Off);
+
+ if (GenApi::IsWritable(m_camera.ExposureTime)) {
+ 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.");
+}
+
+/*!
+ Set camera gamma correction mode and parameter.
+
+ \param gamma_on : If true turn gamma correction on, otherwise turn off.
+ \param gamma_value : Parameter used to perform gamma correction of
+ pixel intensity.
+
+ \return The measured gamma correction value after applying the new
+ setting.
+
+ \sa getGamma()
+ */
+float vpPylonGrabberUsb::setGamma(bool gamma_on, float gamma_value)
+{
+ connect();
+
+ if (GenApi::IsWritable(m_camera.Gamma)) {
+ if (gamma_on)
+ m_camera.Gamma.SetValue(gamma_value);
+ else
+ m_camera.Gamma.SetValue(1);
+ return m_camera.Gamma.GetValue();
+ } else
+ throw vpException(vpException::notImplementedError, "Don't know how to set gamma.");
+}
+
+/*!
+ \brief Saves the current active configuration set into the selected
+ user set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \param set_default Whether to set the configuration set to be used as
+ the default startup set.
+ \return true for finished, false otherwise.
+
+ \sa loadUserSet()
+ */
+bool vpPylonGrabberUsb::saveUserSet(UserSetName user_set, bool set_default)
+{
+ connect();
+
+ bool success = selectUserSet(user_set);
+
+ if (success) {
+ m_camera.UserSetSave.Execute();
+ vpTime::wait(200); // How long you have to wait?
+ success = m_camera.UserSetSave.IsDone();
+ }
+
+ if (success && set_default)
+ success = setUserSetDefault(user_set);
+
+ return success;
+}
+
+/*!
+ \brief Sets the configuration set to be used as the default startup
+ set.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for finished, false otherwise.
+
+ \sa getUserSetDefault()
+ */
+bool vpPylonGrabberUsb::setUserSetDefault(UserSetName user_set)
+{
+ connect();
+
+ switch (user_set) {
+ case USERSET_DEFAULT:
+ m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_Default);
+ return true;
+ break;
+ case USERSET_USERSET1:
+ m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet1);
+ return true;
+ break;
+ case USERSET_USERSET2:
+ m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet2);
+ return true;
+ break;
+ case USERSET_USERSET3:
+ m_camera.UserSetDefault.SetValue(Basler_UsbCameraParams::UserSetDefault_UserSet3);
+ return true;
+ break;
+ default:
+ return false;
+ }
+}
+
+/*!
+ Start active camera capturing images.
+
+ \sa stopCapture()
+ */
+void vpPylonGrabberUsb::startCapture()
+{
+ connect();
+
+ if (!m_camera.IsGrabbing()) {
+ m_camera.StartGrabbing(1);
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Stop active camera capturing images.
+
+ \sa startCapture()
+ */
+void vpPylonGrabberUsb::stopCapture()
+{
+ if (m_camera.IsGrabbing()) {
+ m_camera.StopGrabbing();
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Connect the active camera.
+
+ \sa disconnect()
+ */
+void vpPylonGrabberUsb::connect()
+{
+ if (m_connected == false) {
+ m_numCameras = getNumCameras();
+ if (m_numCameras == 0) {
+ throw(vpException(vpException::fatalError, "No camera found"));
+ }
+
+ if (!m_camera.IsPylonDeviceAttached()) {
+ Pylon::CTlFactory &TlFactory = Pylon::CTlFactory::GetInstance();
+ Pylon::DeviceInfoList_t lstDevices;
+ Pylon::DeviceInfoList_t filter; // Filter for USB cameras.
+ Pylon::CBaslerUsbDeviceInfo usb_devinfo;
+ filter.push_back(usb_devinfo);
+ TlFactory.EnumerateDevices(lstDevices, filter);
+
+ m_camera.Attach(TlFactory.CreateDevice(lstDevices[m_index]));
+ }
+ // Connect to a camera
+ m_camera.Open();
+ m_connected = true;
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ Disconnect the active camera.
+
+ \sa connect()
+ */
+void vpPylonGrabberUsb::disconnect()
+{
+ if (m_connected == true) {
+ m_camera.Close();
+ m_connected = false;
+ }
+ if (m_connected && m_camera.IsGrabbing())
+ init = true;
+ else
+ init = false;
+}
+
+/*!
+ \brief Stop active camera capturing images and disconnect the active
+ camera.
+
+ If you want to use again this camera, you may call
+ setCameraIndex(const unsigned int &) and open(vpImage<unsigned char>
+ &) or open(vpImage<vpRGBa> &) to connect again the camera.
+ */
+void vpPylonGrabberUsb::close()
+{
+ stopCapture();
+ disconnect();
+}
+
+/*!
+ Acquire a gray level image from the active camera.
+
+ \param I : Image data structure (8 bits image).
+*/
+void vpPylonGrabberUsb::acquire(vpImage<unsigned char> &I)
+{
+ open();
+
+ Pylon::CGrabResultPtr grabResult;
+ // Retrieve an image
+ if (!m_camera.RetrieveResult(2000, grabResult)) {
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s",
+ getCameraSerial(m_index).c_str()));
+ }
+
+ if (grabResult->GrabSucceeded()) {
+ height = grabResult->GetHeight();
+ width = grabResult->GetWidth();
+ I.resize(height, width);
+
+ Pylon::CImageFormatConverter imageConvert;
+ imageConvert.OutputPixelFormat = Pylon::PixelType_Mono8;
+ imageConvert.OutputPaddingX = 0;
+ // Create a converted image
+ imageConvert.Convert(I.bitmap, sizeof(unsigned char) * width * height, (Pylon::IImage &)grabResult);
+ }
+}
+
+/*!
+ Acquire a color image from the active camera.
+
+ \param I : Image data structure (RGBa image).
+*/
+void vpPylonGrabberUsb::acquire(vpImage<vpRGBa> &I)
+{
+ open();
+
+ Pylon::CGrabResultPtr grabResult;
+ // Retrieve an image
+ if (!m_camera.RetrieveResult(2000, grabResult)) {
+ throw(vpException(vpException::fatalError, "Cannot retrieve image from camera with serial %s",
+ getCameraSerial(m_index).c_str()));
+ }
+
+ if (grabResult->GrabSucceeded()) {
+ height = grabResult->GetHeight();
+ width = grabResult->GetWidth();
+ I.resize(height, width);
+
+ Pylon::CImageFormatConverter imageConvert;
+ imageConvert.OutputPixelFormat = Pylon::PixelType_BGRA8packed;
+ imageConvert.OutputPaddingX = 0;
+ // Create a converted image
+ Pylon::CPylonImage destImage;
+ imageConvert.Convert(destImage, (Pylon::IImage &)grabResult);
+ Pylon::SBGRA8Pixel *pixel = (Pylon::SBGRA8Pixel *)destImage.GetBuffer();
+ for (unsigned int i = 0; i < height; i++) {
+ for (unsigned int j = 0; j < width; j++) {
+ unsigned int p_index = i * width + j;
+ I[i][j].R = pixel[p_index].R;
+ I[i][j].G = pixel[p_index].G;
+ I[i][j].B = pixel[p_index].B;
+ I[i][j].A = pixel[p_index].A;
+ }
+ }
+ }
+}
+
+/*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+void vpPylonGrabberUsb::open(vpImage<unsigned char> &I)
+{
+ open();
+ acquire(I);
+}
+
+/*!
+ Connect to the active camera, start capture and retrieve an image.
+ \param I : Captured image.
+ */
+void vpPylonGrabberUsb::open(vpImage<vpRGBa> &I)
+{
+ open();
+ acquire(I);
+}
+
+/*!
+ Connect to the active camera and start capture.
+
+ Similar then calling:
+ \code
+ vpPylonGrabberUsb g;
+ ...
+ g.connect();
+ g.startCapture();
+ \endcode
+ */
+void vpPylonGrabberUsb::open()
+{
+ connect();
+ startCapture();
+}
+
+/*!
+ \brief Selects the configuration set to load, save, or configure.
+
+ \param user_set See vpPylonGrabber::UserSetName for valid values.
+ \return true for success, false for failure.
+
+ Default User Set is read-only and cannot be modified.
+ */
+bool vpPylonGrabberUsb::selectUserSet(UserSetName user_set)
+{
+ connect();
+
+ switch (user_set) {
+ case USERSET_DEFAULT:
+ m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_Default);
+ return true;
+ break;
+ case USERSET_USERSET1:
+ m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet1);
+ return true;
+ break;
+ case USERSET_USERSET2:
+ m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet2);
+ return true;
+ break;
+ case USERSET_USERSET3:
+ m_camera.UserSetSelector.SetValue(Basler_UsbCameraParams::UserSetSelector_UserSet3);
+ return true;
+ break;
+ default:
+ return false;
+ }
+}
+
+/*!
+ Operator that allows to capture a grey level image.
+ \param I : The captured image.
+ */
+vpPylonGrabber &vpPylonGrabberUsb::operator>>(vpImage<unsigned char> &I)
+{
+ acquire(I);
+ return *this;
+}
+
+/*!
+ Operator that allows to capture a color image.
+ \param I : The captured image.
+ */
+vpPylonGrabber &vpPylonGrabberUsb::operator>>(vpImage<vpRGBa> &I)
+{
+ acquire(I);
+ return *this;
+}
+
+#else
+// Work arround to avoid warning:
+// libvisp_pylon.a(vpPylonGrabberUsb.cpp.o) has no symbols
+void dummy_vpPylonGrabberUsb(){};
+#endif // #ifdef VISP_HAVE_PYLON
diff --git a/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h
new file mode 100644
index 0000000..c275bdc
--- /dev/null
+++ b/modules/sensor/src/framegrabber/pylon/vpPylonGrabberUsb.h
@@ -0,0 +1,127 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description: Header of vpPylonGrabberUsb class.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file vpPylonGrabberUsb.h
+ \brief Subclass of vpPylonGrabber, implements Basler USB3 cameras
+ supporting.
+*/
+
+#ifndef __vpPylonGrabberUsb_h_
+#define __vpPylonGrabberUsb_h_
+
+#include <visp3/core/vpConfig.h>
+
+#include <visp3/sensor/vpPylonGrabber.h>
+
+#ifdef VISP_HAVE_PYLON
+
+#include <pylon/usb/BaslerUsbInstantCamera.h>
+
+/*!
+ \class vpPylonGrabberUsb
+
+ Allows to grab images from a Basler USB camera using Pylon SDK.
+
+ This class should not be instantiated directly. Use
+ vpPylonFactory::createPylonGrabber() instead.
+
+ \headerfile vpPylonGrabberUsb.h ""
+ */
+class VISP_EXPORT vpPylonGrabberUsb : public vpPylonGrabber
+{
+public:
+ vpPylonGrabberUsb();
+ ~vpPylonGrabberUsb();
+
+ void acquire(vpImage<unsigned char> &I);
+ void acquire(vpImage<vpRGBa> &I);
+
+ void close();
+ void connect();
+ void disconnect();
+
+ float getBlackLevel();
+ std::ostream &getCameraInfo(std::ostream &os);
+ Pylon::CInstantCamera *getCameraHandler();
+ /*! Return the index of the active camera. */
+ unsigned int getCameraIndex() const { return m_index; };
+ std::string getCameraSerial(unsigned int index);
+ float getExposure();
+ float getFrameRate();
+ float getGain();
+ unsigned int getNumCameras();
+ float getGamma();
+ bool loadUserSet(UserSetName user_set);
+ UserSetName getUserSetDefault();
+
+ //! Return true if the camera is connected.
+ bool isConnected() const { return m_connected; }
+ //! Return true if the camera capture is started.
+ bool isCaptureStarted() const { return m_camera.IsGrabbing(); }
+ void open(vpImage<unsigned char> &I);
+ void open(vpImage<vpRGBa> &I);
+
+ vpPylonGrabber &operator>>(vpImage<unsigned char> &I);
+ vpPylonGrabber &operator>>(vpImage<vpRGBa> &I);
+
+ 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 setGain(bool gain_auto, float gain_value = 0);
+ float setFrameRate(float frame_rate);
+ float setGamma(bool gamma_on, float gamma_value = 1);
+ bool saveUserSet(UserSetName user_set, bool set_default = false);
+ bool setUserSetDefault(UserSetName user_set);
+
+ void startCapture();
+ void stopCapture();
+
+protected:
+ void open();
+ bool selectUserSet(UserSetName user_set);
+
+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
+};
+
+#endif // #ifdef VISP_HAVE_PYLON
+#endif // #ifndef __vpPylonGrabberUsb_h_
diff --git a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp
index 2a7c31b..6b45f6f 100644
--- a/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp
+++ b/modules/sensor/src/framegrabber/v4l2/vpV4l2Grabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,34 +46,33 @@
#ifdef VISP_HAVE_V4L2
+#include <errno.h>
+#include <fcntl.h>
+#include <iostream>
#include <stdio.h>
-#include <unistd.h>
#include <stdlib.h>
-#include <sys/time.h>
#include <sys/ioctl.h>
-#include <sys/types.h>
-#include <sys/stat.h>
-#include <fcntl.h>
-#include <stdio.h>
#include <sys/mman.h>
-#include <errno.h>
-#include <iostream>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
-#include <visp3/sensor/vpV4l2Grabber.h>
#include <visp3/core/vpFrameGrabberException.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
//#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImageTools.h>
const unsigned int vpV4l2Grabber::DEFAULT_INPUT = 2;
const unsigned int vpV4l2Grabber::DEFAULT_SCALE = 2;
-const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
-const __u32 vpV4l2Grabber::MAX_NORM = 16;
-const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
-const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
-const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
-const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
-#define vpCLEAR(x) memset (&(x), 0, sizeof (x))
+const __u32 vpV4l2Grabber::MAX_INPUTS = 16;
+const __u32 vpV4l2Grabber::MAX_NORM = 16;
+const __u32 vpV4l2Grabber::MAX_FORMAT = 32;
+const unsigned int vpV4l2Grabber::MAX_CTRL = 32;
+const unsigned int vpV4l2Grabber::MAX_BUFFERS = 32;
+const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
+#define vpCLEAR(x) memset(&(x), 0, sizeof(x))
/*!
Default constructor.
@@ -91,11 +91,11 @@ const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
- Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
fps. These framerates are reachable only if enought buffers are set.
- - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
- to change it.
+ - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use
+setInput() to change it.
- - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
- or setWidth() and setHeight() to change it.
+ - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either
+setScale() or setWidth() and setHeight() to change it.
\code
vpImage<unsigned char> I; // Grey level image
@@ -110,14 +110,14 @@ const unsigned int vpV4l2Grabber::FRAME_SIZE = 288;
g.acquire(I); // Acquire a 768x576 grey image
\endcode
- The grabber allows also to grab a portion of an image using a region of interest.
- The following example shows how to grab a 320x240 region located on the top/left
- corner of the image that has a higher resolution (ie 640x480).
+ The grabber allows also to grab a portion of an image using a region of
+interest. The following example shows how to grab a 320x240 region located on
+the top/left corner of the image that has a higher resolution (ie 640x480).
\code
-#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
int main()
{
@@ -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);
@@ -184,11 +181,11 @@ vpV4l2Grabber::vpV4l2Grabber()
- Framerate acquisition: 25 fps. Use setFramerate() to set 25 fps or 50
fps. These framerates are reachable only if enought buffers are set.
- - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use setInput()
- to change it.
+ - Input board: vpV4l2Grabber::DEFAULT_INPUT. Video input port. Use
+ setInput() to change it.
- - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either setScale()
- or setWidth() and setHeight to change it.
+ - Image size acquisition: vpV4l2Grabber::DEFAULT_SCALE. Use either
+ setScale() or setWidth() and setHeight to change it.
\code
vpImage<unsigned char> I; // Grey level image
@@ -206,13 +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);
@@ -224,7 +218,6 @@ vpV4l2Grabber::vpV4l2Grabber(bool verbose)
init = false;
}
-
/*!
Constructor.
@@ -256,13 +249,10 @@ vpV4l2Grabber::vpV4l2Grabber(bool verbose)
\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,14 +294,11 @@ vpV4l2Grabber::vpV4l2Grabber(unsigned input, unsigned scale)
\endcode
*/
-vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &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<unsigned char> &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);
@@ -356,14 +343,11 @@ vpV4l2Grabber::vpV4l2Grabber(vpImage<unsigned char> &I, unsigned input, unsigned
\endcode
*/
-vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &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<vpRGBa> &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);
@@ -381,19 +365,12 @@ vpV4l2Grabber::vpV4l2Grabber(vpImage<vpRGBa> &I, unsigned input, unsigned scale
\sa close()
*/
-vpV4l2Grabber::~vpV4l2Grabber()
-{
- close() ;
-}
+vpV4l2Grabber::~vpV4l2Grabber() { close(); }
/*!
Set the video input port on the board.
*/
-void
-vpV4l2Grabber::setInput(unsigned input)
-{
- this->m_input = input;
-}
+void vpV4l2Grabber::setInput(unsigned input) { this->m_input = input; }
/*!
Set the decimation factor applied to full resolution images (768x576).
@@ -407,20 +384,17 @@ vpV4l2Grabber::setInput(unsigned input)
An other way to specify the image size is to use setWidth() and setHeight().
*/
-void
-vpV4l2Grabber::setScale(unsigned scale)
+void vpV4l2Grabber::setScale(unsigned scale)
{
- if ((scale <1) || (scale >16))
- {
+ if ((scale < 1) || (scale > 16)) {
close();
- vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16",scale) ;
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Wrong scale") );
+ vpERROR_TRACE("Wrong scale %d, scale should be between 1 and 16", scale);
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Wrong scale"));
}
- setWidth(640/scale);
- setHeight(480/scale);
+ setWidth(640 / scale);
+ setHeight(480 / scale);
}
/*!
@@ -433,32 +407,28 @@ vpV4l2Grabber::setScale(unsigned scale)
\exception vpFrameGrabberException::settingError : Wrong input channel.
*/
-void
-vpV4l2Grabber::open(vpImage<unsigned char> &I)
+void vpV4l2Grabber::open(vpImage<unsigned char> &I)
{
open();
- if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
- {
+ if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
}
-
+
vpV4l2PixelFormatType req_pixelformat = getPixelFormat();
try {
setFormat();
startStreaming();
- }
- catch(...) {
+ } 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;
}
-
+
// try to fing a compatible format
- for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
+ for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
if (format == req_pixelformat) {
continue;
}
@@ -467,27 +437,24 @@ vpV4l2Grabber::open(vpImage<unsigned char> &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 (...) {
+ } 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;
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"));
}
}
}
}
- I.resize(height, width) ;
+ I.resize(height, width);
init = true;
}
@@ -501,13 +468,11 @@ vpV4l2Grabber::open(vpImage<unsigned char> &I)
the image is resized to the current acquisition size.
*/
-void
-vpV4l2Grabber::open(vpImage<vpRGBa> &I)
+void vpV4l2Grabber::open(vpImage<vpRGBa> &I)
{
open();
- if( v4l2_ioctl (fd, VIDIOC_S_INPUT, &m_input) == -1 )
- {
+ if (v4l2_ioctl(fd, VIDIOC_S_INPUT, &m_input) == -1) {
std::cout << "Warning: cannot set input channel to " << m_input << std::endl;
}
@@ -517,16 +482,14 @@ vpV4l2Grabber::open(vpImage<vpRGBa> &I)
setFormat();
startStreaming();
- }
- catch(...) {
+ } 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;
}
-
+
// try to fing a compatible format
- for (int format=0; format< (int)V4L2_MAX_FORMAT; format ++) {
+ for (int format = 0; format < (int)V4L2_MAX_FORMAT; format++) {
if (format == req_pixelformat) {
continue;
}
@@ -535,21 +498,17 @@ vpV4l2Grabber::open(vpImage<vpRGBa> &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 (...) {
+ } 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;
}
}
}
- I.resize(height, width) ;
+ I.resize(height, width);
init = true;
}
@@ -564,8 +523,7 @@ vpV4l2Grabber::open(vpImage<vpRGBa> &I)
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<unsigned char> &I)
+void vpV4l2Grabber::acquire(vpImage<unsigned char> &I)
{
struct timeval timestamp;
vpRect roi;
@@ -584,8 +542,7 @@ vpV4l2Grabber::acquire(vpImage<unsigned char> &I)
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<unsigned char> &I, const vpRect &roi)
+void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, const vpRect &roi)
{
struct timeval timestamp;
@@ -598,81 +555,79 @@ vpV4l2Grabber::acquire(vpImage<unsigned char> &I, const vpRect &roi)
\param I : Image data structure (8 bits image).
\param timestamp : Timeval data structure providing the unix time
- at which the frame was captured in the ringbuffer. \b Warning: some v4l2 drivers do not return
- the time since 1970 (the one returned by gettimeofday() or vpTime) but rather a time that counts
- from the boot time (i.e. uptime).
+ at which the frame was captured in the ringbuffer. \b Warning: some v4l2
+ drivers do not return the time since 1970 (the one returned by
+ gettimeofday() or vpTime) but rather a time that counts from the boot time
+ (i.e. uptime).
- \param roi : Region of interest to grab from the full resolution image. By default acquire the whole image.
+ \param roi : Region of interest to grab from the full resolution image. By
+ default acquire the whole image.
\exception vpFrameGrabberException::initializationError : Frame grabber not
initialized.
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval ×tamp, const vpRect &roi)
+void vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval ×tamp, const vpRect &roi)
{
- if (init==false)
- {
+ if (init == false) {
open(I);
}
- if (init==false)
- {
+ 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 ;
+ unsigned char *bitmap;
bitmap = waiton(index_buffer, timestamp);
if (roi == vpRect())
I.resize(height, width);
else
I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
- switch(m_pixelformat) {
+ switch (m_pixelformat) {
case V4L2_GREY_FORMAT:
if (roi == vpRect())
- memcpy(I.bitmap, bitmap, height * width*sizeof(unsigned char));
+ memcpy(I.bitmap, bitmap, height * width * sizeof(unsigned char));
else
vpImageTools::crop(bitmap, width, height, roi, 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<unsigned char> 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<unsigned char> 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<unsigned char> 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<unsigned char> 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;
@@ -694,8 +649,7 @@ vpV4l2Grabber::acquire(vpImage<unsigned char> &I, struct timeval ×tamp, con
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<vpRGBa> &I)
+void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I)
{
struct timeval timestamp;
vpRect roi;
@@ -714,8 +668,7 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I)
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, const vpRect &roi)
+void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, const vpRect &roi)
{
struct timeval timestamp;
@@ -728,34 +681,32 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, const vpRect &roi)
\param I : Image data structure (32 bits image).
\param timestamp : Timeval data structure providing the unix time
- at which the frame was captured in the ringbuffer. \b Warning: some v4l2 drivers do not return
- the time since 1970 (the one returned by gettimeofday() or vpTime) but rather a time that counts
- from the boot time (i.e. uptime).
+ at which the frame was captured in the ringbuffer. \b Warning: some v4l2
+ drivers do not return the time since 1970 (the one returned by
+ gettimeofday() or vpTime) but rather a time that counts from the boot time
+ (i.e. uptime).
- \param roi : Region of interest to grab from the full resolution image. By default acquire the whole image.
+ \param roi : Region of interest to grab from the full resolution image. By
+ default acquire the whole image.
\exception vpFrameGrabberException::initializationError : Frame grabber not
initialized.
\sa getField()
*/
-void
-vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRect &roi)
+void vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRect &roi)
{
- if (init==false)
- {
+ if (init == false) {
open(I);
}
- if (init==false)
- {
+ 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 ;
+ unsigned char *bitmap;
bitmap = waiton(index_buffer, timestamp);
if (roi == vpRect())
@@ -763,21 +714,22 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRe
else
I.resize((unsigned int)roi.getHeight(), (unsigned int)roi.getWidth());
- // The framegrabber acquire aRGB format. We just shift the data from 1 byte all the data and initialize the last byte
+ // The framegrabber acquire aRGB format. We just shift the data from 1 byte
+ // all the data and initialize the last byte
- switch(m_pixelformat) {
+ 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<vpRGBa> 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;
@@ -786,30 +738,30 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRe
// The framegrabber acquire aRGB format. We just shift the data
// from 1 byte all the data and initialize the last byte
memcpy(I.bitmap, bitmap + 1, height * width * sizeof(vpRGBa) - 1);
- I[height-1][width-1].A = 0;
- }
- else {
- for(unsigned int i=0; i<I.getHeight(); i++) {
- memcpy(I.bitmap, bitmap + 1 + (unsigned int)(roi.getTop()*width + roi.getLeft()), I.getWidth() * sizeof(vpRGBa) - 1);
- I[i][I.getWidth()-1].A = 0;
+ 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()),
+ I.getWidth() * sizeof(vpRGBa) - 1);
+ I[i][I.getWidth() - 1].A = 0;
}
}
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<vpRGBa> 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<vpRGBa> 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;
@@ -825,8 +777,8 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRe
Return the field (odd or even) corresponding to the last acquired
frame.
- This method is to call after acquire() and has only a mean if the acquisition
- framerate is set to 50 fps.
+ This method is to call after acquire() and has only a mean if the
+ acquisition framerate is set to 50 fps.
\return Field of the acquired frame (0 if odd field, 1 if even field).
@@ -836,16 +788,16 @@ vpV4l2Grabber::acquire(vpImage<vpRGBa> &I, struct timeval ×tamp, const vpRe
\sa acquire(), setFramerate()
*/
-bool
-vpV4l2Grabber::getField()
+bool vpV4l2Grabber::getField()
{
- if(field == 2) return 0; //top field
- else if (field == 3) return 1; //bottom field;
+ if (field == 2)
+ return 0; // top field
+ else if (field == 3)
+ return 1; // bottom field;
else {
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "V4l2 returns a bad frame field") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "V4l2 returns a bad frame field"));
return false;
}
}
@@ -855,16 +807,15 @@ vpV4l2Grabber::getField()
\param framerate : The framerate for the acquisition.
- If vpV4l2Grabber::framerate_25fps use vpV4l2Grabber::V4L2_IMAGE_FORMAT,
- - else if vpV4l2Grabber::framerate_50fps use vpV4l2Grabber::V4L2_FRAME_FORMAT.
- \warning If you want to acquire frames at 25 fps or 50 fps, you have to be
- aware of the number of buffers required for the streaming. A typical value
- could be 3 (see setNBuffers()).
+ - else if vpV4l2Grabber::framerate_50fps use
+ vpV4l2Grabber::V4L2_FRAME_FORMAT. \warning If you want to acquire frames at
+ 25 fps or 50 fps, you have to be aware of the number of buffers required for
+ the streaming. A typical value could be 3 (see setNBuffers()).
\sa getFramerate(), setNBuffers()
*/
-void
-vpV4l2Grabber::setFramerate(vpV4l2Grabber::vpV4l2FramerateType framerate)
+void vpV4l2Grabber::setFramerate(vpV4l2Grabber::vpV4l2FramerateType framerate)
{
this->m_framerate = framerate;
@@ -883,35 +834,46 @@ 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.
*/
-void
-vpV4l2Grabber::close()
+void vpV4l2Grabber::close()
{
stopStreaming();
streaming = false;
- if (fd >= 0){
- //vpTRACE("v4l2_close()");
- v4l2_close (fd);
+ if (fd >= 0) {
+ // vpTRACE("v4l2_close()");
+ v4l2_close(fd);
fd = -1;
}
- if (inp != NULL) { delete [] inp; inp = NULL; }
- if (std != NULL) { delete [] std; std = NULL; }
- if (fmt != NULL) { delete [] fmt; fmt = NULL; }
- if (ctl != NULL) { delete [] ctl; ctl = NULL; }
- if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
- if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
+ if (inp != NULL) {
+ delete[] inp;
+ inp = NULL;
+ }
+ if (std != NULL) {
+ delete[] std;
+ std = NULL;
+ }
+ if (fmt != NULL) {
+ delete[] fmt;
+ fmt = NULL;
+ }
+ if (ctl != NULL) {
+ delete[] ctl;
+ ctl = NULL;
+ }
+ if (buf_v4l2 != NULL) {
+ delete[] buf_v4l2;
+ buf_v4l2 = NULL;
+ }
+ if (buf_me != NULL) {
+ delete[] buf_me;
+ buf_me = NULL;
+ }
}
/*!
@@ -925,67 +887,73 @@ vpV4l2Grabber::close()
capabilities.
*/
-void
-vpV4l2Grabber::open()
+void vpV4l2Grabber::open()
{
/* Open Video Device */
struct stat st;
-
- if (-1 == stat (device, &st)) {
- fprintf (stderr, "Cannot identify '%s': %d, %s\n",
- device, errno, strerror (errno));
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "Cannot identify video device") );
+ if (-1 == stat(device, &st)) {
+ fprintf(stderr, "Cannot identify '%s': %d, %s\n", device, errno, strerror(errno));
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Cannot identify video device"));
}
-
- if (!S_ISCHR (st.st_mode)) {
- fprintf (stderr, "%s is no device\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::initializationError,
- "No device") );
-
+
+ if (!S_ISCHR(st.st_mode)) {
+ fprintf(stderr, "%s is no device\n", device);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "No device"));
}
- fd = v4l2_open (device, O_RDWR | O_NONBLOCK, 0);
+ 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") );
-
+ vpERROR_TRACE("No video device \"%s\"\n", device);
+ throw(vpFrameGrabberException(vpFrameGrabberException::initializationError, "Can't access to video device"));
}
- if (inp != NULL) { delete [] inp; inp = NULL; }
- if (std != NULL) { delete [] std; std = NULL; }
- if (fmt != NULL) { delete [] fmt; fmt = NULL; }
- if (ctl != NULL) { delete [] ctl; ctl = NULL; }
- if (buf_v4l2 != NULL) { delete [] buf_v4l2; buf_v4l2 = NULL; }
- if (buf_me != NULL) { delete [] buf_me; buf_me = NULL; }
+ if (inp != NULL) {
+ delete[] inp;
+ inp = NULL;
+ }
+ if (std != NULL) {
+ delete[] std;
+ std = NULL;
+ }
+ if (fmt != NULL) {
+ delete[] fmt;
+ fmt = NULL;
+ }
+ if (ctl != NULL) {
+ delete[] ctl;
+ ctl = NULL;
+ }
+ if (buf_v4l2 != NULL) {
+ delete[] buf_v4l2;
+ buf_v4l2 = NULL;
+ }
+ if (buf_me != NULL) {
+ delete[] buf_me;
+ buf_me = NULL;
+ }
- inp = new struct v4l2_input [vpV4l2Grabber::MAX_INPUTS];
- std = new struct v4l2_standard [vpV4l2Grabber::MAX_NORM];
- fmt = new struct v4l2_fmtdesc [vpV4l2Grabber::MAX_FORMAT];
- ctl = new struct v4l2_queryctrl [vpV4l2Grabber::MAX_CTRL*2];
- buf_v4l2 = new struct v4l2_buffer [vpV4l2Grabber::MAX_BUFFERS];
- buf_me = new struct ng_video_buf [vpV4l2Grabber::MAX_BUFFERS];
+ inp = new struct v4l2_input[vpV4l2Grabber::MAX_INPUTS];
+ std = new struct v4l2_standard[vpV4l2Grabber::MAX_NORM];
+ fmt = new struct v4l2_fmtdesc[vpV4l2Grabber::MAX_FORMAT];
+ ctl = new struct v4l2_queryctrl[vpV4l2Grabber::MAX_CTRL * 2];
+ buf_v4l2 = new struct v4l2_buffer[vpV4l2Grabber::MAX_BUFFERS];
+ buf_me = new struct ng_video_buf[vpV4l2Grabber::MAX_BUFFERS];
/* Querry Video Device Capabilities */
- if ( v4l2_ioctl (fd, VIDIOC_QUERYCAP, &cap) == -1 ) {
+ 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") );
+ fprintf(stderr, "%s is no V4L2 device\n", device);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Is not a V4L2 device"));
}
if (m_verbose) {
- fprintf(stdout, "v4l2 info:\n"
- " device: %s\n"
- " %s %d.%d.%d / %s @ %s\n",
- device,
- cap.driver,
- (cap.version >> 16) & 0xff,
- (cap.version >> 8) & 0xff,
- cap.version & 0xff,
- cap.card, cap.bus_info);
+ fprintf(stdout,
+ "v4l2 info:\n"
+ " device: %s\n"
+ " %s %d.%d.%d / %s @ %s\n",
+ device, cap.driver, (cap.version >> 16) & 0xff, (cap.version >> 8) & 0xff, cap.version & 0xff, cap.card,
+ cap.bus_info);
if (cap.capabilities & V4L2_CAP_VIDEO_OVERLAY)
fprintf(stdout, " Support overlay\n");
else
@@ -1002,19 +970,18 @@ vpV4l2Grabber::open()
fprintf(stdout, " Support streaming capture.\n");
else
fprintf(stdout, " Does not support streaming capture\n");
- if(cap.capabilities & V4L2_CAP_ASYNCIO)
+ if (cap.capabilities & V4L2_CAP_ASYNCIO)
fprintf(stdout, " Support asynchronous I/O methods\n");
else
fprintf(stdout, " Does not support asynchronous I/O methods\n");
- if(cap.capabilities & V4L2_CAP_TIMEPERFRAME)
+ if (cap.capabilities & V4L2_CAP_TIMEPERFRAME)
fprintf(stdout, " Support time per frame field\n");
else
fprintf(stdout, " Does not support time per frame field\n");
// Get framerate
streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) != -1) {
- fprintf(stdout, " Current acquisition framerate: %d fps\n",
- streamparm.parm.output.timeperframe.denominator);
+ fprintf(stdout, " Current acquisition framerate: %d fps\n", streamparm.parm.output.timeperframe.denominator);
}
}
@@ -1027,8 +994,7 @@ vpV4l2Grabber::open()
\exception vpFrameGrabberException::otherError : Can't get video parameters.
*/
-void
-vpV4l2Grabber::getCapabilities()
+void vpV4l2Grabber::getCapabilities()
{
for (__u32 ninputs = 0; ninputs < MAX_INPUTS; ninputs++) {
inp[ninputs].index = ninputs;
@@ -1039,22 +1005,19 @@ vpV4l2Grabber::getCapabilities()
std[nstds].index = nstds;
if (v4l2_ioctl(fd, VIDIOC_ENUMSTD, &std[nstds]))
break;
-
}
for (__u32 nfmts = 0; nfmts < MAX_FORMAT; nfmts++) {
fmt[nfmts].index = nfmts;
- fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt[nfmts].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
if (v4l2_ioctl(fd, VIDIOC_ENUM_FMT, &fmt[nfmts]))
break;
}
streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1)
- {
+ 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"));
}
}
@@ -1066,98 +1029,100 @@ vpV4l2Grabber::getCapabilities()
- pixel format : specified by setPixelFormat().
- \exception vpFrameGrabberException::settingError : Bad format, probably do to
- a wrong scale.
+ \exception vpFrameGrabberException::settingError : Bad format, probably do
+ to a wrong scale.
\exception vpFrameGrabberException::otherError : Can't get video format.
*/
-void
-vpV4l2Grabber::setFormat()
+void vpV4l2Grabber::setFormat()
{
- fmt_me.width = width;
+ fmt_me.width = width;
fmt_me.height = height;
- //fmt_me.bytesperline = width; // bad (normally width * depth / 8), but works
+ // fmt_me.bytesperline = width; // bad (normally width * depth / 8), but
+ // works
// because initialized later by an ioctl call to VIDIOC_S_FMT
- switch(m_pixelformat) {
- case V4L2_GREY_FORMAT : fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
+ switch (m_pixelformat) {
+ case V4L2_GREY_FORMAT:
+ fmt_me.pixelformat = V4L2_PIX_FMT_GREY;
if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_GREY)\n");
break;
- case V4L2_RGB24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
+ case V4L2_RGB24_FORMAT:
+ fmt_me.pixelformat = V4L2_PIX_FMT_RGB24;
if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB24)\n");
break;
- case V4L2_RGB32_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
+ case V4L2_RGB32_FORMAT:
+ fmt_me.pixelformat = V4L2_PIX_FMT_RGB32;
if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_RGB32)\n");
break;
- case V4L2_BGR24_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
+ case V4L2_BGR24_FORMAT:
+ fmt_me.pixelformat = V4L2_PIX_FMT_BGR24;
if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_BGR24)\n");
break;
- case V4L2_YUYV_FORMAT: fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
+ case V4L2_YUYV_FORMAT:
+ fmt_me.pixelformat = V4L2_PIX_FMT_YUYV;
if (m_verbose)
- fprintf(stdout,"v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_PIX_FMT_YUYV)\n");
break;
default:
close();
- throw (vpFrameGrabberException(vpFrameGrabberException::settingError,
- "Bad format, probably do to a wrong scale"));
+ throw(vpFrameGrabberException(vpFrameGrabberException::settingError, "Bad format, probably do to a wrong scale"));
}
/* Get Video Format */
- vpCLEAR (fmt_v4l2);
+ vpCLEAR(fmt_v4l2);
- fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl (fd, VIDIOC_G_FMT, &fmt_v4l2) == -1 ) {
+ 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;
- fmt_v4l2.fmt.pix.height = fmt_me.height;
- //printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
-
+ fmt_v4l2.fmt.pix.pixelformat = fmt_me.pixelformat;
+ fmt_v4l2.fmt.pix.width = fmt_me.width;
+ fmt_v4l2.fmt.pix.height = fmt_me.height;
+ // printf("1 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
+ // fmt_v4l2.fmt.pix.height);
switch (m_frameformat) {
- case V4L2_FRAME_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
+ case V4L2_FRAME_FORMAT:
+ fmt_v4l2.fmt.pix.field = V4L2_FIELD_ALTERNATE;
if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_ALTERNATE)\n");
}
break;
- case V4L2_IMAGE_FORMAT: fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ case V4L2_IMAGE_FORMAT:
+ fmt_v4l2.fmt.pix.field = V4L2_FIELD_INTERLACED;
if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
+ fprintf(stdout, "v4l2: new capture params (V4L2_FIELD_INTERLACED)\n");
}
break;
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
- if( m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE )
- {
+ // height and width of the captured image or frame
+ if (m_frameformat == V4L2_FRAME_FORMAT && height > FRAME_SIZE) {
height = FRAME_SIZE;
}
- //printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width, fmt_v4l2.fmt.pix.height);
+ // printf("2 - w: %d h: %d\n", fmt_v4l2.fmt.pix.width,
+ // fmt_v4l2.fmt.pix.height);
if (v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt_v4l2) == -1) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't set video format") );
+ 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. */
@@ -1168,21 +1133,18 @@ vpV4l2Grabber::setFormat()
if (fmt_v4l2.fmt.pix.sizeimage < min)
fmt_v4l2.fmt.pix.sizeimage = min;
- fmt_me.width = fmt_v4l2.fmt.pix.width;
- fmt_me.height = fmt_v4l2.fmt.pix.height;
+ fmt_me.width = fmt_v4l2.fmt.pix.width;
+ fmt_me.height = fmt_v4l2.fmt.pix.height;
fmt_me.bytesperline = fmt_v4l2.fmt.pix.bytesperline;
if (m_verbose) {
- fprintf(stdout,"v4l2: new capture params (%dx%d, %c%c%c%c, %d byte, %d bytes per line)\n",
- fmt_me.width, fmt_me.height,
- fmt_v4l2.fmt.pix.pixelformat & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 8) & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 16) & 0xff,
- (fmt_v4l2.fmt.pix.pixelformat >> 24) & 0xff,
- fmt_v4l2.fmt.pix.sizeimage,
- fmt_v4l2.fmt.pix.bytesperline);
+ fprintf(stdout,
+ "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);
}
-
}
/*!
@@ -1192,8 +1154,7 @@ vpV4l2Grabber::setFormat()
\exception vpFrameGrabberException::otherError : If a problem occurs.
*/
-void
-vpV4l2Grabber::startStreaming()
+void vpV4l2Grabber::startStreaming()
{
if (streaming == true) { // Acquisition in process.
stopStreaming();
@@ -1201,64 +1162,58 @@ vpV4l2Grabber::startStreaming()
}
/* setup buffers */
- memset (&(reqbufs), 0, sizeof (reqbufs));
- reqbufs.count = m_nbuffers;
- reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ memset(&(reqbufs), 0, sizeof(reqbufs));
+ reqbufs.count = m_nbuffers;
+ reqbufs.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
reqbufs.memory = V4L2_MEMORY_MMAP;
-
- if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1)
- {
+ if (v4l2_ioctl(fd, VIDIOC_REQBUFS, &reqbufs) == -1) {
if (EINVAL == errno) {
- fprintf (stderr, "%s does not support "
- "memory mapping\n", device);
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Does not support memory mapping") );
+ fprintf(stderr,
+ "%s does not support "
+ "memory mapping\n",
+ device);
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Does not support memory mapping"));
}
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't require video buffers") );
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't require video buffers"));
}
for (unsigned i = 0; i < reqbufs.count; i++) {
// Clear the buffer
- memset (&(buf_v4l2[i]), 0, sizeof (buf_v4l2[i]));
- buf_v4l2[i].index = i;
- buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ memset(&(buf_v4l2[i]), 0, sizeof(buf_v4l2[i]));
+ buf_v4l2[i].index = i;
+ buf_v4l2[i].type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf_v4l2[i].memory = V4L2_MEMORY_MMAP;
buf_v4l2[i].length = 0;
- if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't query video buffers") );
+ if (v4l2_ioctl(fd, VIDIOC_QUERYBUF, &buf_v4l2[i]) == -1) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't query video buffers"));
}
memcpy(&buf_me[i].fmt, &fmt_me, sizeof(ng_video_fmt));
buf_me[i].size = buf_me[i].fmt.bytesperline * buf_me[i].fmt.height;
// if (m_verbose)
- // std::cout << "1: buf_v4l2[" << i << "].length: " << buf_v4l2[i].length
+ // std::cout << "1: buf_v4l2[" << i << "].length: " <<
+ // buf_v4l2[i].length
// << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
// << std::endl;
+ buf_me[i].data = (unsigned char *)v4l2_mmap(NULL, buf_v4l2[i].length, PROT_READ | PROT_WRITE, MAP_SHARED, fd,
+ (off_t)buf_v4l2[i].m.offset);
- 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") );
+ if (buf_me[i].data == MAP_FAILED) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't map memory"));
}
buf_me[i].refcount = 0;
// if (m_verbose)
// {
- // std::cout << "2: buf_v4l2[" << i << "].length: " << buf_v4l2[i].length
+ // std::cout << "2: buf_v4l2[" << i << "].length: " <<
+ // buf_v4l2[i].length
// << " buf_v4l2[" << i << "].offset: " << buf_v4l2[i].m.offset
// << std::endl;
- // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size << std::endl;
+ // std::cout << "2: buf_me[" << i << "].size: " << buf_me[i].size <<
+ // std::endl;
// }
if (m_verbose)
@@ -1269,10 +1224,8 @@ vpV4l2Grabber::startStreaming()
queueAll();
/* Set video stream capture on */
- if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type)<0)
- {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't start streaming") );
+ if (v4l2_ioctl(fd, VIDIOC_STREAMON, &fmt_v4l2.type) < 0) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't start streaming"));
}
streaming = true;
@@ -1284,28 +1237,25 @@ vpV4l2Grabber::startStreaming()
\exception vpFrameGrabberException::otherError : if can't stop streaming.
*/
-void
-vpV4l2Grabber::stopStreaming()
+void vpV4l2Grabber::stopStreaming()
{
- //nothing to do if (fd < 0) or if (streaming == false)
+ // nothing to do if (fd < 0) or if (streaming == false)
if ((fd >= 0) && (streaming == true)) {
- //vpTRACE(" Stop the streaming...");
+ // vpTRACE(" Stop the streaming...");
/* stop capture */
fmt_v4l2.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- if (v4l2_ioctl(fd, VIDIOC_STREAMOFF,&fmt_v4l2.type)) {
- throw (vpFrameGrabberException(vpFrameGrabberException::otherError,
- "Can't stop streaming") );
+ if (v4l2_ioctl(fd, VIDIOC_STREAMOFF, &fmt_v4l2.type)) {
+ throw(vpFrameGrabberException(vpFrameGrabberException::otherError, "Can't stop streaming"));
}
/* free buffers */
for (unsigned int i = 0; i < reqbufs.count; i++) {
if (m_verbose)
printBufInfo(buf_v4l2[i]);
- //vpTRACE("v4l2_munmap()");
+ // 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;
@@ -1327,17 +1277,16 @@ vpV4l2Grabber::stopStreaming()
\exception vpFrameGrabberException::otherError : If can't access to the
frame.
*/
-unsigned char *
-vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp)
+unsigned char *vpV4l2Grabber::waiton(__u32 &index, struct timeval ×tamp)
{
struct v4l2_buffer buf;
struct timeval tv;
fd_set rdset;
- /* wait for the next frame */
+/* wait for the next frame */
again:
- tv.tv_sec = 30;
+ tv.tv_sec = 30;
tv.tv_usec = 0;
FD_ZERO(&rdset);
FD_SET(static_cast<unsigned int>(fd), &rdset);
@@ -1346,40 +1295,32 @@ again:
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:
+ 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;
}
-
/* get it */
memset(&buf, 0, sizeof(buf));
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP; // Fabien manquait
- if (-1 == v4l2_ioctl(fd,VIDIOC_DQBUF, &buf)) {
+ if (-1 == v4l2_ioctl(fd, VIDIOC_DQBUF, &buf)) {
index = 0;
- switch(errno)
- {
+ 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;
@@ -1408,18 +1349,16 @@ again:
Capture helpers.
*/
-int
-vpV4l2Grabber::queueBuffer()
+int vpV4l2Grabber::queueBuffer()
{
unsigned int frame = queue % reqbufs.count;
int rc;
-
if (0 != buf_me[frame].refcount) {
if (0 != queue - waiton_cpt)
return -1;
- fprintf(stderr,"v4l2: waiting for a free buffer..............\n");
- //ng_waiton_video_buf(h->buf_me+frame);
+ 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";
}
@@ -1427,25 +1366,19 @@ vpV4l2Grabber::queueBuffer()
rc = v4l2_ioctl(fd, VIDIOC_QBUF, &buf_v4l2[frame]);
if (0 == rc)
queue++;
- else
- {
- switch(errno)
- {
+ 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;
}
}
@@ -1457,8 +1390,7 @@ vpV4l2Grabber::queueBuffer()
Call the queue buffer private method if needed
*/
-void
-vpV4l2Grabber::queueAll()
+void vpV4l2Grabber::queueAll()
{
for (;;) {
if (queue - waiton_cpt >= reqbufs.count) {
@@ -1475,24 +1407,33 @@ vpV4l2Grabber::queueAll()
Get device capabilities.
*/
-void
-vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
+void vpV4l2Grabber::printBufInfo(struct v4l2_buffer buf)
{
char type[40];
- switch(buf.type) {
- case V4L2_BUF_TYPE_VIDEO_CAPTURE: sprintf(type, "video-cap"); break;
- case V4L2_BUF_TYPE_VIDEO_OVERLAY: sprintf(type, "video-over"); break;
- case V4L2_BUF_TYPE_VIDEO_OUTPUT: sprintf(type, "video-out"); break;
- case V4L2_BUF_TYPE_VBI_CAPTURE: sprintf(type, "vbi-cap"); break;
- case V4L2_BUF_TYPE_VBI_OUTPUT: sprintf(type, "vbi-out"); break;
- default: sprintf(type, "unknown"); break;
+ switch (buf.type) {
+ case V4L2_BUF_TYPE_VIDEO_CAPTURE:
+ sprintf(type, "video-cap");
+ break;
+ case V4L2_BUF_TYPE_VIDEO_OVERLAY:
+ sprintf(type, "video-over");
+ break;
+ case V4L2_BUF_TYPE_VIDEO_OUTPUT:
+ sprintf(type, "video-out");
+ break;
+ case V4L2_BUF_TYPE_VBI_CAPTURE:
+ sprintf(type, "vbi-cap");
+ break;
+ case V4L2_BUF_TYPE_VBI_OUTPUT:
+ sprintf(type, "vbi-out");
+ break;
+ default:
+ sprintf(type, "unknown");
+ break;
}
- fprintf(stdout,"v4l2: buf %d: %d ad: 0x%lx offset 0x%x+%d (=0x%x),used %d\n",
- buf.index, buf.type, buf.m.userptr, buf.m.offset,
- buf.length, buf.length, buf.bytesused);
-
+ 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);
}
/*!
@@ -1540,6 +1481,7 @@ vpV4l2Grabber &vpV4l2Grabber::operator>>(vpImage<vpRGBa> &I)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no symbols
-void dummy_vpV4l2Grabber() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpV4l2Grabber.cpp.o) has no
+// symbols
+void dummy_vpV4l2Grabber(){};
#endif
diff --git a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp
index 2e74781..2e7ef82 100644
--- a/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp
+++ b/modules/sensor/src/laserscanner/sick/vpSickLDMRS.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,51 +38,48 @@
#if !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__)))
-#include <visp3/sensor/vpSickLDMRS.h>
-#include <visp3/core/vpMath.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpTime.h>
-#include <sys/socket.h>
-#include <sys/select.h>
-#include <netinet/in.h>
-#include <fcntl.h>
#include <errno.h>
+#include <fcntl.h>
#include <netdb.h>
+#include <netinet/in.h>
#include <string.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTime.h>
+#include <visp3/sensor/vpSickLDMRS.h>
//#include <strings.h>
-#include <math.h>
#include <assert.h>
-#include <stdlib.h>
#include <limits.h>
-
-
+#include <math.h>
+#include <stdlib.h>
/*!
\file vpSickLDMRS.cpp
- \brief Driver for the Sick LD-MRS laser scanner.
+ \brief Driver for the Sick LD-MRS laser scanner.
*/
-/*!
-
+/*!
+
Default constructor that initialize the Ethernet address to
"131.254.12.119", set the port to 12002 and allocates memory for the
body messages.
*/
vpSickLDMRS::vpSickLDMRS()
- : socket_fd(-1), body(NULL), vAngle(), time_offset(0),
- isFirstMeasure(true), maxlen_body(104000)
+ : socket_fd(-1), body(NULL), vAngle(), time_offset(0), isFirstMeasure(true), maxlen_body(104000)
{
ip = "131.254.12.119";
port = 12002;
- body = new unsigned char [maxlen_body];
+ body = new unsigned char[maxlen_body];
vAngle.resize(4); // Vertical angle of the 4 layers
vAngle[0] = vpMath::rad(-1.2);
- vAngle[1] = vpMath::rad(-0.4);
- vAngle[2] = vpMath::rad( 0.4);
- vAngle[3] = vpMath::rad( 1.2);
+ vAngle[1] = vpMath::rad(-0.4);
+ vAngle[2] = vpMath::rad(0.4);
+ vAngle[3] = vpMath::rad(1.2);
}
/*!
@@ -89,27 +87,27 @@ vpSickLDMRS::vpSickLDMRS()
*/
vpSickLDMRS::~vpSickLDMRS()
{
- if (body)
- delete [] body;
+ if (body)
+ delete[] body;
}
-/*!
+/*!
Initialize the connection with the Sick LD-MRS laser scanner.
\param ip_address : Ethernet address of the laser.
\param com_port : Ethernet port of the laser.
\return true if the device was initialized, false otherwise.
-
+
*/
-bool vpSickLDMRS::setup(std::string ip_address, int com_port)
+bool vpSickLDMRS::setup(const std::string &ip_address, int com_port)
{
- setIpAddress( ip_address );
- setPort( com_port );
- return ( this->setup() );
+ setIpAddress(ip_address);
+ setPort(com_port);
+ return (this->setup());
}
-/*!
+/*!
Initialize the connection with the Sick LD-MRS laser scanner.
\return true if the device was initialized, false otherwise.
@@ -121,35 +119,33 @@ bool vpSickLDMRS::setup()
struct timeval tv;
fd_set myset;
- // Create the TCP socket
- socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
+ // Create the TCP socket
+ socket_fd = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
if (socket_fd < 0) {
- fprintf(stderr, "Failed to create socket\n");
- return false;
+ fprintf(stderr, "Failed to create socket\n");
+ return false;
}
- //bzero(&serv_addr, sizeof(serv_addr));
+ // bzero(&serv_addr, sizeof(serv_addr));
memset(&serv_addr, 0, sizeof(serv_addr));
- serv_addr.sin_family = AF_INET; // Internet/IP
- serv_addr.sin_addr.s_addr = inet_addr(ip.c_str()); // IP address
- serv_addr.sin_port = htons(port); // server port
+ serv_addr.sin_family = AF_INET; // Internet/IP
+ serv_addr.sin_addr.s_addr = inet_addr(ip.c_str()); // IP address
+ serv_addr.sin_port = htons(port); // server port
// Establish connection
- res = connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr)) ;
- if (errno == EINPROGRESS) {
- tv.tv_sec = 3;
- tv.tv_usec = 0;
- FD_ZERO(&myset);
- FD_SET(static_cast<unsigned int>(socket_fd), &myset);
- res = select(socket_fd+1, NULL, &myset, NULL, &tv);
- if (res < 0 && errno != EINTR) {
- fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno));
- return false;
- }
- else if (res > 0) {
- fprintf(stderr,"ok");
- }
- else {
- fprintf(stderr, "Timeout in select() - Cancelling!\n");
+ res = connect(socket_fd, (struct sockaddr *)&serv_addr, sizeof(serv_addr));
+ if (errno == EINPROGRESS) {
+ tv.tv_sec = 3;
+ tv.tv_usec = 0;
+ FD_ZERO(&myset);
+ FD_SET(static_cast<unsigned int>(socket_fd), &myset);
+ res = select(socket_fd + 1, NULL, &myset, NULL, &tv);
+ if (res < 0 && errno != EINTR) {
+ fprintf(stderr, "Error connecting to server %d - %s\n", errno, strerror(errno));
+ return false;
+ } else if (res > 0) {
+ fprintf(stderr, "ok");
+ } else {
+ fprintf(stderr, "Timeout in select() - Cancelling!\n");
return false;
}
}
@@ -168,11 +164,11 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
unsigned int *uintptr;
unsigned short *ushortptr;
static unsigned char header[24];
- ushortptr=(unsigned short *)header;
- uintptr=(unsigned int *)header;
+ ushortptr = (unsigned short *)header;
+ uintptr = (unsigned int *)header;
- assert (sizeof(header) == 24);
- //std::cout << "size " << sizeof(header) << std::endl;
+ assert(sizeof(header) == 24);
+ // std::cout << "size " << sizeof(header) << std::endl;
double time_second = 0;
@@ -197,13 +193,13 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
uint32_t msgLength = ntohl(uintptr[2]);
ssize_t len = recv(socket_fd, body, msgLength, MSG_WAITALL);
- if (len != (ssize_t)msgLength){
+ if (len != (ssize_t)msgLength) {
printf("Error, wrong msg length: %d of %d bytes.\n", (int)len, msgLength);
return false;
}
- if (msgtype!=vpSickLDMRS::MeasuredData){
- //printf("The message in not relative to measured data !!!\n");
+ if (msgtype != vpSickLDMRS::MeasuredData) {
+ // printf("The message in not relative to measured data !!!\n");
return true;
}
@@ -211,19 +207,19 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
// get the measurement number
unsigned short measurementId;
- ushortptr = (unsigned short *) body;
+ ushortptr = (unsigned short *)body;
measurementId = ushortptr[0];
// get the start timestamp
- uintptr=(unsigned int *) (body+6);
+ uintptr = (unsigned int *)(body + 6);
unsigned int seconds = uintptr[1];
- unsigned int fractional=uintptr[0];
+ unsigned int fractional = uintptr[0];
double startTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
// get the end timestamp
- uintptr=(unsigned int *) (body+14);
+ uintptr = (unsigned int *)(body + 14);
seconds = uintptr[1];
- fractional=uintptr[0];
+ fractional = uintptr[0];
double endTimestamp = seconds + fractional / 4294967296.; // 4294967296. = 2^32
// compute the time offset to bring the measures in the Unix time reference
@@ -241,18 +237,18 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
// get the start/stop angle
short startAngle = (short)ushortptr[12];
short stopAngle = (short)ushortptr[13];
-// std::cout << "angle in [" << startAngle << "; " << stopAngle
-// << "]" << std::endl;
-
+ // std::cout << "angle in [" << startAngle << "; " << stopAngle
+ // << "]" << std::endl;
+
// get the number of points of this measurement
unsigned short numPoints = ushortptr[14];
int nlayers = 4;
- for (int i=0; i < nlayers; i++) {
+ for (int i = 0; i < nlayers; i++) {
laserscan[i].clear();
- laserscan[i].setMeasurementId(measurementId);
- laserscan[i].setStartTimestamp(startTimestamp);
- laserscan[i].setEndTimestamp(endTimestamp);
+ laserscan[i].setMeasurementId(measurementId);
+ laserscan[i].setStartTimestamp(startTimestamp);
+ laserscan[i].setEndTimestamp(endTimestamp);
laserscan[i].setNumSteps(numSteps);
laserscan[i].setStartAngle(startAngle);
laserscan[i].setStopAngle(stopAngle);
@@ -261,23 +257,23 @@ bool vpSickLDMRS::measure(vpLaserScan laserscan[4])
// decode the measured points
double hAngle; // horizontal angle in rad
- double rDist; // radial distance in meters
+ double rDist; // radial distance in meters
vpScanPoint scanPoint;
- if (numPoints > USHRT_MAX-2)
- throw(vpException (vpException::ioError, "Out of range number of point"));
-
- for (int i=0; i < numPoints; i++) {
- ushortptr = (unsigned short *) (body+44+i*10);
- unsigned char layer = ((unsigned char) body[44+i*10])&0x0F;
- unsigned char echo = ((unsigned char) body[44+i*10])>>4;
- //unsigned char flags = (unsigned char) body[44+i*10+1];
-
- if (echo==0) {
- hAngle = (2.f * M_PI / numSteps)*(short) ushortptr[1];
+ if (numPoints > USHRT_MAX - 2)
+ throw(vpException(vpException::ioError, "Out of range number of point"));
+
+ for (int i = 0; i < numPoints; i++) {
+ ushortptr = (unsigned short *)(body + 44 + i * 10);
+ unsigned char layer = ((unsigned char)body[44 + i * 10]) & 0x0F;
+ unsigned char echo = ((unsigned char)body[44 + i * 10]) >> 4;
+ // unsigned char flags = (unsigned char) body[44+i*10+1];
+
+ if (echo == 0) {
+ hAngle = (2.f * M_PI / numSteps) * (short)ushortptr[1];
rDist = 0.01 * ushortptr[2]; // cm to meters conversion
-
- //vpTRACE("layer: %d d: %f hangle: %f", layer, rDist, hAngle);
+
+ // vpTRACE("layer: %d d: %f hangle: %f", layer, rDist, hAngle);
scanPoint.setPolar(rDist, hAngle, vAngle[layer]);
laserscan[layer].addPoint(scanPoint);
}
diff --git a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp
index d8901fd..e7d306d 100644
--- a/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp
+++ b/modules/sensor/src/rgb-depth/kinect/vpKinect.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,31 +39,28 @@
#include <visp3/core/vpConfig.h>
-// Note that libfreenect needs libusb-1.0 and libpthread
+// Note that libfreenect needs libusb-1.0 and libpthread
#if defined(VISP_HAVE_LIBFREENECT_AND_DEPENDENCIES)
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
-#include <visp3/sensor/vpKinect.h>
#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/sensor/vpKinect.h>
/*!
Default constructor.
*/
vpKinect::vpKinect(freenect_context *ctx, int index)
- : Freenect::FreenectDevice(ctx, index),
- m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(),
- rgbMir(), irMrgb(), DMres(DMAP_LOW_RES),
- hd(240), wd(320),
- dmap(), IRGB(),
- m_new_rgb_frame(false),
- m_new_depth_map(false),
- m_new_depth_image(false),
- height(480), width(640)
+ : Freenect::FreenectDevice(ctx, index), m_rgb_mutex(), m_depth_mutex(), RGBcam(), IRcam(), rgbMir(), irMrgb(),
+ DMres(DMAP_LOW_RES), hd(240), wd(320), dmap(), IRGB(), m_new_rgb_frame(false), m_new_depth_map(false),
+ m_new_depth_image(false), height(480), width(640)
{
dmap.resize(height, width);
IRGB.resize(height, width);
- vpPoseVector r(-0.0266,-0.0047,-0.0055,0.0320578,0.0169041,-0.0076519 );//!Those are the parameters found for our Kinect device. Note that they can differ from one device to another.
+ vpPoseVector r(-0.0266, -0.0047, -0.0055, 0.0320578, 0.0169041,
+ -0.0076519); //! Those are the parameters found for our
+ //! Kinect device. Note that they can differ from
+ //! one device to another.
rgbMir.buildFrom(r);
irMrgb = rgbMir.inverse();
}
@@ -70,45 +68,45 @@ vpKinect::vpKinect(freenect_context *ctx, int index)
/*!
Destructor.
*/
-vpKinect::~vpKinect()
-{
-
-}
+vpKinect::~vpKinect() {}
void vpKinect::start(vpKinect::vpDMResolution res)
{
DMres = res;
- height = 480;
- width = 640;
- //!Calibration parameters are the parameters found for our Kinect device. Note that they can differ from one device to another.
- if (DMres == DMAP_LOW_RES){
- std::cout << "vpKinect::start LOW depth map resolution 240x320" << std::endl;
- // IRcam.setparameters(IRcam.get_px()/2, IRcam.get_py()/2, IRcam.get_u0()/2, IRcam.get_v0()/2);
- //IRcam.initPersProjWithoutDistortion(303.06,297.89,160.75,117.9);
- IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
- hd = 240;
- wd = 320;
- }
- else
- {
- std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
-
- //IRcam.initPersProjWithoutDistortion(606.12,595.78,321.5,235.8);
- IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
- // Idmap.resize(height, width);
- hd = 480;
- wd = 640;
- }
+ height = 480;
+ width = 640;
+ //! Calibration parameters are the parameters found for our Kinect device.
+ //! Note that they can differ from one device to another.
+ if (DMres == DMAP_LOW_RES) {
+ std::cout << "vpKinect::start LOW depth map resolution 240x320" << std::endl;
+ // IRcam.setparameters(IRcam.get_px()/2, IRcam.get_py()/2,
+ // IRcam.get_u0()/2, IRcam.get_v0()/2);
+ // IRcam.initPersProjWithoutDistortion(303.06,297.89,160.75,117.9);
+ IRcam.initPersProjWithDistortion(303.06, 297.89, 160.75, 117.9, -0.27, 0);
+ hd = 240;
+ wd = 320;
+ } else {
+ std::cout << "vpKinect::start MEDIUM depth map resolution 480x640" << std::endl;
+
+ // IRcam.initPersProjWithoutDistortion(606.12,595.78,321.5,235.8);
+ IRcam.initPersProjWithDistortion(606.12, 595.78, 321.5, 235.8, -0.27, 0);
+ // Idmap.resize(height, width);
+ hd = 480;
+ wd = 640;
+ }
#if defined(VISP_HAVE_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, height);
+ 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,
+ 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.initPersProjWithoutDistortion(525.53, 524.94, 309.9, 282.8);//old
+ // RGBcam.initPersProjWithDistortion(536.76, 537.25, 313.45,
+ // 273.27,0.04,-0.04);//old
+ // RGBcam.initPersProjWithoutDistortion(512.0559503505,511.9352058050,310.6693938678,267.0673901049);//new
+ RGBcam.initPersProjWithDistortion(522.5431816996, 522.7191431808, 311.4001982614, 267.4283562142, 0.0477365207,
+ -0.0462326418); // new
#endif
this->startVideo();
@@ -121,21 +119,19 @@ void vpKinect::stop()
this->stopDepth();
}
-
/*!
Acquire a new RGB image.
*/
-void vpKinect::VideoCallback(void* rgb, uint32_t /* timestamp */)
+void vpKinect::VideoCallback(void *rgb, uint32_t /* timestamp */)
{
-// std::cout << "vpKinect Video callback" << std::endl;
+ // std::cout << "vpKinect Video callback" << std::endl;
vpMutex::vpScopedLock lock(m_rgb_mutex);
- uint8_t* rgb_ = static_cast<uint8_t*>(rgb);
- for (unsigned i = 0; i< height;i++){
- for (unsigned j = 0 ; j < width ; j++)
- {
- IRGB[i][j].R = rgb_[3*(width*i +j)+0];
- IRGB[i][j].G = rgb_[3*(width*i +j)+1];
- IRGB[i][j].B = rgb_[3*(width*i +j)+2];
+ uint8_t *rgb_ = static_cast<uint8_t *>(rgb);
+ for (unsigned i = 0; i < height; i++) {
+ for (unsigned j = 0; j < width; j++) {
+ IRGB[i][j].R = rgb_[3 * (width * i + j) + 0];
+ IRGB[i][j].G = rgb_[3 * (width * i + j) + 1];
+ IRGB[i][j].B = rgb_[3 * (width * i + j) + 2];
}
}
@@ -152,16 +148,17 @@ void vpKinect::VideoCallback(void* rgb, uint32_t /* timestamp */)
stored in dmap. (range : 0.3 - 5m).
*/
-void vpKinect::DepthCallback(void* depth, uint32_t /* timestamp */)
+void vpKinect::DepthCallback(void *depth, uint32_t /* timestamp */)
{
-// std::cout << "vpKinect Depth callback" << std::endl;
+ // std::cout << "vpKinect Depth callback" << std::endl;
vpMutex::vpScopedLock lock(m_depth_mutex);
- uint16_t* depth_ = static_cast<uint16_t*>(depth);
- for (unsigned i = 0; i< height;i++){
- for (unsigned j = 0 ; j < width ; j++)
- {
- dmap[i][j] = 0.1236f * tan(depth_[width*i +j] / 2842.5f + 1.1863f);//formula from http://openkinect.org/wiki/Imaging_Information
- if(depth_[width*i +j]>1023){//Depth cannot be computed
+ uint16_t *depth_ = static_cast<uint16_t *>(depth);
+ for (unsigned i = 0; i < height; i++) {
+ for (unsigned j = 0; j < width; j++) {
+ dmap[i][j] =
+ 0.1236f * tan(depth_[width * i + j] / 2842.5f + 1.1863f); // formula from
+ // http://openkinect.org/wiki/Imaging_Information
+ if (depth_[width * i + j] > 1023) { // Depth cannot be computed
dmap[i][j] = -1;
}
}
@@ -170,11 +167,10 @@ void vpKinect::DepthCallback(void* depth, uint32_t /* timestamp */)
m_new_depth_image = true;
}
-
/*!
Get metric depth map (float).
*/
-bool vpKinect::getDepthMap(vpImage<float>& map)
+bool vpKinect::getDepthMap(vpImage<float> &map)
{
vpMutex::vpScopedLock lock(m_depth_mutex);
if (!m_new_depth_map)
@@ -184,60 +180,55 @@ bool vpKinect::getDepthMap(vpImage<float>& map)
return true;
}
-
/*!
* Get metric depth map (float) and corresponding image.
*/
-bool vpKinect::getDepthMap(vpImage<float>& map,vpImage<unsigned char>& Imap)
+bool vpKinect::getDepthMap(vpImage<float> &map, vpImage<unsigned char> &Imap)
{
- // vpMutex::vpScopedLock lock(m_depth_mutex);
- vpImage<float> tempMap;
- m_depth_mutex.lock();
- if (!m_new_depth_map && !m_new_depth_image)
- {
- m_depth_mutex.unlock();
- return false;
- }
- tempMap = dmap;
-
- m_new_depth_map = false;
- m_new_depth_image = false;
- m_depth_mutex.unlock();
+ // vpMutex::vpScopedLock lock(m_depth_mutex);
+ vpImage<float> tempMap;
+ m_depth_mutex.lock();
+ if (!m_new_depth_map && !m_new_depth_image) {
+ m_depth_mutex.unlock();
+ return false;
+ }
+ tempMap = dmap;
- if ((Imap.getHeight()!=hd )||(map.getHeight()!=hd))
- vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
- if (DMres == DMAP_LOW_RES){
- for(unsigned int i = 0; i < hd; i++)
- for(unsigned int j = 0; j < wd; j++){
- map[i][j] = tempMap[i<<1][j<<1];
- //if (map[i][j] != -1)
- if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
- Imap[i][j] = (unsigned char)(255*map[i][j]/5);
- else
- Imap[i][j] = 255;
- }
- }
- else
- {
- for (unsigned i = 0; i< height;i++)
- for (unsigned j = 0 ; j < width ; j++){
- map[i][j] = tempMap[i][j];
- //if (map[i][j] != -1)
- if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
- Imap[i][j] = (unsigned char)(255*map[i][j]/5);
- else
- Imap[i][j] = 255;
- }
- }
+ m_new_depth_map = false;
+ m_new_depth_image = false;
+ m_depth_mutex.unlock();
+
+ if ((Imap.getHeight() != hd) || (map.getHeight() != hd))
+ vpERROR_TRACE(1, "Image size does not match vpKinect DM resolution");
+ if (DMres == DMAP_LOW_RES) {
+ for (unsigned int i = 0; i < hd; i++)
+ for (unsigned int j = 0; j < wd; j++) {
+ map[i][j] = tempMap[i << 1][j << 1];
+ // if (map[i][j] != -1)
+ if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
+ Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
+ else
+ Imap[i][j] = 255;
+ }
+ } else {
+ for (unsigned i = 0; i < height; i++)
+ for (unsigned j = 0; j < width; j++) {
+ map[i][j] = tempMap[i][j];
+ // if (map[i][j] != -1)
+ if (fabs(map[i][j] + 1.f) > std::numeric_limits<float>::epsilon())
+ Imap[i][j] = (unsigned char)(255 * map[i][j] / 5);
+ else
+ Imap[i][j] = 255;
+ }
+ }
- return true;
+ return true;
}
-
/*!
Get RGB image
*/
-bool vpKinect::getRGB(vpImage<vpRGBa>& I_RGB)
+bool vpKinect::getRGB(vpImage<vpRGBa> &I_RGB)
{
vpMutex::vpScopedLock lock(m_rgb_mutex);
if (!m_new_rgb_frame)
@@ -248,64 +239,64 @@ bool vpKinect::getRGB(vpImage<vpRGBa>& 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
+ Warp the RGB frame to the depth camera frame. The size of the resulting
+ IrgbWarped frame is the same as the size of the depth map Idepth
*/
-void vpKinect::warpRGBFrame(const vpImage<vpRGBa> & Irgb, const vpImage<float> & Idepth, vpImage<vpRGBa> & IrgbWarped)
+void vpKinect::warpRGBFrame(const vpImage<vpRGBa> &Irgb, const vpImage<float> &Idepth, vpImage<vpRGBa> &IrgbWarped)
{
- if ((Idepth.getHeight()!=hd )||(Idepth.getWidth()!=wd)){
- vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
- }
- else{
- if((IrgbWarped.getHeight()!=hd )||(IrgbWarped.getWidth()!=wd))
- IrgbWarped.resize(hd, wd);
- IrgbWarped=0;
- double x1=0., y1=0., x2=0., y2=0., Z1, Z2;
- vpImagePoint imgPoint(0,0);
- double u=0., v=0.;
- vpColVector P1(4),P2(4);
-
-// std::cout <<"rgbMir : "<<rgbMir<<std::endl;
-
- for (unsigned int i = 0; i< hd;i++)
- for (unsigned int j = 0 ; j < wd ; j++){
- //! Compute metric coordinates in the ir camera Frame :
- vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
- Z1 = Idepth[i][j];
- //if (Z1!=-1){
- if (std::fabs(Z1+1) <= std::numeric_limits<double>::epsilon()){
- P1[0]=x1*Z1;
- P1[1]=y1*Z1;
- P1[2]=Z1;
- P1[3]=1;
-
- //! Change frame :
- P2 = rgbMir*P1;
- Z2 = P2[2];
- //if (Z2!= 0){
- if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()){
- x2 = P2[0]/P2[2];
- y2 = P2[1]/P2[2];
- }
- else
- std::cout<<"Z2 = 0 !!"<<std::endl;
-
- //! compute pixel coordinates of the corresponding point in the depth image
- vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
-
- unsigned int u_ = (unsigned int)u;
- unsigned int v_ = (unsigned int)v;
- //!Fill warped image value
- if ((u_<width)&&(v_<height)){
- IrgbWarped[i][j] = Irgb[v_][u_];
- }
- else
- IrgbWarped[i][j] = 0;
- }
- }
- }
+ if ((Idepth.getHeight() != hd) || (Idepth.getWidth() != wd)) {
+ vpERROR_TRACE(1, "Idepth image size does not match vpKinect DM resolution");
+ } else {
+ if ((IrgbWarped.getHeight() != hd) || (IrgbWarped.getWidth() != wd))
+ IrgbWarped.resize(hd, wd);
+ IrgbWarped = 0;
+ double x1 = 0., y1 = 0., x2 = 0., y2 = 0., Z1, Z2;
+ vpImagePoint imgPoint(0, 0);
+ double u = 0., v = 0.;
+ vpColVector P1(4), P2(4);
+
+ // std::cout <<"rgbMir : "<<rgbMir<<std::endl;
+
+ for (unsigned int i = 0; i < hd; i++)
+ for (unsigned int j = 0; j < wd; j++) {
+ //! Compute metric coordinates in the ir camera Frame :
+ vpPixelMeterConversion::convertPoint(IRcam, j, i, x1, y1);
+ Z1 = Idepth[i][j];
+ // if (Z1!=-1){
+ if (std::fabs(Z1 + 1) <= std::numeric_limits<double>::epsilon()) {
+ P1[0] = x1 * Z1;
+ P1[1] = y1 * Z1;
+ P1[2] = Z1;
+ P1[3] = 1;
+
+ //! Change frame :
+ P2 = rgbMir * P1;
+ Z2 = P2[2];
+ // if (Z2!= 0){
+ if (std::fabs(Z2) > std::numeric_limits<double>::epsilon()) {
+ x2 = P2[0] / P2[2];
+ y2 = P2[1] / P2[2];
+ } else
+ std::cout << "Z2 = 0 !!" << std::endl;
+
+ //! compute pixel coordinates of the corresponding point in the
+ //! depth image
+ vpMeterPixelConversion::convertPoint(RGBcam, x2, y2, u, v);
+
+ unsigned int u_ = (unsigned int)u;
+ unsigned int v_ = (unsigned int)v;
+ //! Fill warped image value
+ if ((u_ < width) && (v_ < height)) {
+ IrgbWarped[i][j] = Irgb[v_][u_];
+ } else
+ IrgbWarped[i][j] = 0;
+ }
+ }
+ }
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no symbols
-void dummy_vpKinect() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpKinect.cpp.o) has no
+// symbols
+void dummy_vpKinect(){};
#endif // VISP_HAVE_LIBFREENECT
diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp
index 8820b21..754329a 100644
--- a/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp
+++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,8 @@
*
*****************************************************************************/
-#include <iostream>
#include <iomanip>
+#include <iostream>
#include <visp3/core/vpImageConvert.h>
#include <visp3/sensor/vpRealSense.h>
@@ -49,8 +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();
}
@@ -59,34 +60,32 @@ vpRealSense::vpRealSense()
* Default destructor that stops the streaming.
* \sa stop()
*/
-vpRealSense::~vpRealSense()
-{
- close();
-}
+vpRealSense::~vpRealSense() { close(); }
-void vpRealSense::initStream() {
- //General default presets
- //Color stream
+void vpRealSense::initStream()
+{
+ // General default presets
+ // 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);
- //Depth stream
+ // 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);
- //Infrared stream
+ // 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);
- //Infrared stream 2
+ // 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);
- //Enable all streams
+ // Enable all streams
m_enableStreams[rs::stream::color] = true;
m_enableStreams[rs::stream::depth] = true;
m_enableStreams[rs::stream::infrared] = true;
@@ -100,7 +99,7 @@ void vpRealSense::open()
{
m_num_devices = m_context.get_device_count();
- if(m_num_devices == 0)
+ if (m_num_devices == 0)
throw vpException(vpException::fatalError, "RealSense Camera - No device detected.");
std::vector<rs::device *> detected_devices;
@@ -114,32 +113,42 @@ void vpRealSense::open()
detected_devices.push_back(device);
}
- // Exit with error if no serial number is specified and multiple cameras are detected.
+ // Exit with error if no serial number is specified and multiple cameras are
+ // detected.
if ((m_serial_no.empty()) && (m_num_devices > 1)) {
- throw vpException(vpException::fatalError, "RealSense Camera - Multiple cameras detected (%d) but no input serial number specified. Exiting!", m_num_devices);
- }
- // Exit with error if no camera is detected that matches the input serial number.
- if ((! m_serial_no.empty()) && (m_device == NULL)) {
- throw vpException(vpException::fatalError, "RealSense Camera - No camera detected with input serial_no \"%s\" Exiting!", m_serial_no.c_str());
- }
-
- // At this point, m_device will be null if no input serial number was specified and only one camera is connected.
- // This is a valid use case and the code will proceed.
+ throw vpException(vpException::fatalError,
+ "RealSense Camera - Multiple cameras detected (%d) but "
+ "no input serial number specified. Exiting!",
+ m_num_devices);
+ }
+ // Exit with error if no camera is detected that matches the input serial
+ // number.
+ if ((!m_serial_no.empty()) && (m_device == NULL)) {
+ throw vpException(vpException::fatalError,
+ "RealSense Camera - No camera detected with input "
+ "serial_no \"%s\" Exiting!",
+ m_serial_no.c_str());
+ }
+
+ // At this point, m_device will be null if no input serial number was
+ // specified and only one camera is connected. This is a valid use case and
+ // 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;
- //Enable only infrared2 stream if supported
- m_enableStreams[rs::stream::infrared2] = m_device->supports(rs::capabilities::infrared2);
-
+ // 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];
if (m_device->is_streaming()) {
m_device->stop();
}
for (int j = 0; j < 4; j++) {
- auto s = (rs::stream) j;
- auto capabilities = (rs::capabilities) j;
+ auto s = (rs::stream)j;
+ auto capabilities = (rs::capabilities)j;
if (m_device->supports(capabilities) && m_device->is_stream_enabled(s)) {
m_device->disable_stream(s);
}
@@ -149,8 +158,10 @@ void vpRealSense::open()
if (m_useStreamPresets[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);
}
}
@@ -158,8 +169,10 @@ void vpRealSense::open()
if (m_useStreamPresets[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);
}
}
@@ -167,8 +180,10 @@ void vpRealSense::open()
if (m_useStreamPresets[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);
}
}
@@ -176,16 +191,19 @@ void vpRealSense::open()
if (m_useStreamPresets[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);
}
}
// Compute field of view for each enabled stream
m_intrinsics.clear();
- for(int i = 0; i < 4; ++i) {
+ for (int i = 0; i < 4; ++i) {
auto stream = rs::stream(i);
- if(!m_device->is_stream_enabled(stream)) continue;
+ if (!m_device->is_stream_enabled(stream))
+ continue;
auto intrin = m_device->get_stream_intrinsics(stream);
m_intrinsics[stream] = intrin;
@@ -196,15 +214,20 @@ void vpRealSense::open()
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] = m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
- 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_intrinsics[rs::stream::color_aligned_to_depth] =
+ m_device->get_stream_intrinsics(rs::stream::color_aligned_to_depth);
+ 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);
}
}
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_intrinsics[rs::stream::infrared2_aligned_to_depth] = m_device->get_stream_intrinsics(rs::stream::infrared2_aligned_to_depth);
+ m_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);
}
// Start device
@@ -217,7 +240,7 @@ void vpRealSense::open()
void vpRealSense::close()
{
if (m_device) {
- if(m_device->is_streaming()) {
+ if (m_device->is_streaming()) {
m_device->stop();
}
m_device = NULL;
@@ -242,51 +265,60 @@ int main()
void vpRealSense::setDeviceBySerialNumber(const std::string &serial_no)
{
if (m_serial_no.empty()) {
- throw vpException(vpException::fatalError, "RealSense Camera - Multiple cameras detected (%d) but no input serial number specified. Exiting!", m_num_devices);
+ throw vpException(vpException::fatalError,
+ "RealSense Camera - Multiple cameras detected (%d) but "
+ "no input serial number specified. Exiting!",
+ m_num_devices);
}
m_serial_no = serial_no;
}
/*!
- Return camera parameters corresponding to a specific stream. This function has to be called after open().
- \param stream : color, depth, infrared or infrared2 stream for which camera intrinsic parameters are returned.
- \param type : Indicate if the model should include distorsion paramater or not.
+ Return camera parameters corresponding to a specific stream. This function
+ has to be called after open(). \param stream : color, depth, infrared or
+ infrared2 stream for which camera intrinsic parameters are returned. \param
+ type : Indicate if the model should include distorsion paramater or not.
\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);
vpCameraParameters cam;
- double px = intrinsics.ppx;
- double py = intrinsics.ppy;
- double u0 = intrinsics.fx;
- double v0 = intrinsics.fy;
+ double u0 = intrinsics.ppx;
+ double v0 = intrinsics.ppy;
+ double px = intrinsics.fx;
+ double py = intrinsics.fy;
if (type == vpCameraParameters::perspectiveProjWithDistortion) {
double kdu = intrinsics.coeffs[0];
cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
- }
- else {
+ } else {
cam.initPersProjWithoutDistortion(px, py, u0, v0);
}
return cam;
}
/*!
- Get intrinsic parameters corresponding to the stream. This function has to be called after open().
- \param stream : color, depth, infrared or infrared2 stream for which camera intrinsic parameters are returned.
- \sa getCameraParameters()
+ Get intrinsic parameters corresponding to the stream. This function has to
+ be called after open(). \param stream : color, depth, infrared or infrared2
+ stream for which camera intrinsic parameters are returned. \sa
+ 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!");
+ if (!m_device) {
+ 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, "RealSense Camera - stream (%d) is not enabled to retrieve intrinsics. Exiting!", (int)stream);
+ if (!m_device->is_stream_enabled(stream)) {
+ throw vpException(vpException::fatalError,
+ "RealSense Camera - stream (%d) is not enabled to "
+ "retrieve intrinsics. Exiting!",
+ (int)stream);
}
std::map<rs::stream, rs::intrinsics>::const_iterator it_intrin = m_intrinsics.find(stream);
@@ -298,28 +330,37 @@ rs::intrinsics vpRealSense::getIntrinsics(const rs::stream &stream) const
}
/*!
- Get extrinsic transformation from one stream to an other. This function has to be called after open().
- \param from, to : color, depth, infrared or infrared2 stream between which camera extrinsic parameters are returned.
+ Get extrinsic transformation from one stream to an other. This function has
+ to be called after open(). \param from, to : color, depth, infrared or
+ infrared2 stream between which camera extrinsic parameters are returned.
\sa getTransformation()
*/
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!");
- }
- if(!m_device->is_stream_enabled(from)) {
- throw vpException(vpException::fatalError, "RealSense Camera - stream (%d) is not enabled to retrieve extrinsics. Exiting!", (int)from);
- }
- if(!m_device->is_stream_enabled(to)) {
- throw vpException(vpException::fatalError, "RealSense Camera - stream (%d) is not enabled to retrieve extrinsics. Exiting!", (int)to);
+ if (!m_device) {
+ 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,
+ "RealSense Camera - stream (%d) is not enabled to "
+ "retrieve extrinsics. Exiting!",
+ (int)from);
+ }
+ if (!m_device->is_stream_enabled(to)) {
+ throw vpException(vpException::fatalError,
+ "RealSense Camera - stream (%d) is not enabled to "
+ "retrieve extrinsics. Exiting!",
+ (int)to);
}
return m_device->get_extrinsics(from, to);
}
/*!
- Get extrinsic transformation from one stream to an other. This function has to be called after open().
- \param from, to : color, depth, infrared or infrared2 stream between which camera extrinsic parameters are returned.
+ Get extrinsic transformation from one stream to an other. This function has
+ to be called after open(). \param from, to : color, depth, infrared or
+ infrared2 stream between which camera extrinsic parameters are returned.
\sa getExtrinsics()
*/
@@ -328,10 +369,10 @@ vpHomogeneousMatrix vpRealSense::getTransformation(const rs::stream &from, const
rs::extrinsics extrinsics = this->getExtrinsics(from, to);
vpTranslationVector t;
vpRotationMatrix R;
- for(unsigned int i=0; i<3; i++) {
+ for (unsigned int i = 0; i < 3; i++) {
t[i] = extrinsics.translation[i];
- for(unsigned int j=0; j<3; j++)
- R[i][j] = extrinsics.rotation[i*3+j];
+ for (unsigned int j = 0; j < 3; j++)
+ R[i][j] = extrinsics.rotation[i * 3 + j];
}
vpHomogeneousMatrix fMt(t, R);
return fMt;
@@ -346,7 +387,7 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey)
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -359,14 +400,16 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey)
/*!
Acquire data from RealSense device.
\param grey : Grey level image.
- \param pointcloud : Point cloud data as a vector of column vectors. Each column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point.
+ \param pointcloud : Point cloud data as a vector of column vectors. Each
+ column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of
+ a point.
*/
void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector> &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -381,14 +424,16 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey, std::vector<vpColVector>
/*!
Acquire data from RealSense device.
- \param pointcloud : Point cloud data as a vector of column vectors. Each column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point.
+ \param pointcloud : Point cloud data as a vector of column vectors. Each
+ column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of
+ a point.
*/
void vpRealSense::acquire(std::vector<vpColVector> &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -407,7 +452,7 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color)
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -422,14 +467,17 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color)
\param color : Color image.
\param infrared : Infrared image.
\param depth : Depth image.
- \param pointcloud : Point cloud data as a vector of column vectors. Each column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point.
+ \param pointcloud : Point cloud data as a vector of column vectors. Each
+ column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of
+ a point.
*/
-void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, std::vector<vpColVector> &pointcloud)
+void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ std::vector<vpColVector> &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -453,14 +501,17 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, v
\param grey : Grey level image.
\param infrared : Infrared image.
\param depth : Depth image.
- \param pointcloud : Point cloud data as a vector of column vectors. Each column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point.
+ \param pointcloud : Point cloud data as a vector of column vectors. Each
+ column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of
+ a point.
*/
-void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, std::vector<vpColVector> &pointcloud)
+void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ std::vector<vpColVector> &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -482,14 +533,16 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infra
/*!
Acquire data from RealSense device.
\param color : Color image.
- \param pointcloud : Point cloud data as a vector of column vectors. Each column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of a point.
+ \param pointcloud : Point cloud data as a vector of column vectors. Each
+ column vector is 4-dimension and contains X,Y,Z,1 normalized coordinates of
+ a point.
*/
void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -508,20 +561,28 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, std::vector<vpColVector> &poin
\param data_depth : Depth image buffer or NULL if not wanted.
\param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
\param data_infrared : Infrared image buffer or NULL if not wanted.
- \param data_infrared2 : Infrared (for the second IR camera if available) image buffer or NULL if not wanted.
- \param stream_color : Type of color stream (e.g. rs::stream::color, rs::stream::rectified_color, rs::stream::color_aligned_to_depth).
- \param stream_depth : Type of depth stream (e.g. rs::stream::depth, rs::stream::depth_aligned_to_color, rs::stream::depth_aligned_to_rectified_color, rs::stream::depth_aligned_to_infrared2).
- \param stream_infrared : Type of infrared stream (only rs::stream::infrared should be possible).
- \param stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera.
+ \param data_infrared2 : Infrared (for the second IR camera if available)
+ image buffer or NULL if not wanted. \param stream_color : Type of color
+ stream (e.g. rs::stream::color, rs::stream::rectified_color,
+ rs::stream::color_aligned_to_depth). \param stream_depth : Type of depth
+ stream (e.g. rs::stream::depth, rs::stream::depth_aligned_to_color,
+ rs::stream::depth_aligned_to_rectified_color,
+ rs::stream::depth_aligned_to_infrared2). \param stream_infrared : Type of
+ infrared stream (only rs::stream::infrared should be possible). \param
+ 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<vpColVector> * 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<vpColVector> *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!");
}
- if ( !m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -553,7 +614,8 @@ void vpRealSense::acquire(unsigned char * const data_image, unsigned char * cons
\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;
}
@@ -563,8 +625,9 @@ void vpRealSense::setStreamSettings(const rs::stream &stream, const rs::preset &
\param stream : Stream type (color / depth / infrared).
\param params : Stream parameters to use.
- \note You can find the stream settings of the different Intel RealSense cameras at this
- <a href=https://github.com/IntelRealSense/librealsense/blob/v1.11.0/doc/supported_video_formats.pdf>url</a>.
+ \note You can find the stream settings of the different Intel RealSense
+cameras at this <a
+href=https://github.com/IntelRealSense/librealsense/blob/v1.11.0/doc/supported_video_formats.pdf>url</a>.
\code
#include <visp3/sensor/vpRealSense.h>
@@ -584,7 +647,8 @@ 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;
}
@@ -612,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
/*!
@@ -627,7 +688,7 @@ void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -646,7 +707,7 @@ void vpRealSense::acquire(pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -666,7 +727,7 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey, pcl::PointCloud<pcl::Poi
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -689,7 +750,7 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -709,12 +770,13 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, pcl::PointCloud<pcl::PointXYZ>
\param depth : Depth image.
\param pointcloud : Point cloud data information.
*/
-void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -740,12 +802,13 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, v
\param depth : Depth image.
\param pointcloud : Point cloud data information.
*/
-void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -771,12 +834,13 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infra
\param depth : Depth image.
\param pointcloud : Point cloud data with texture information.
*/
-void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
+void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -802,12 +866,13 @@ void vpRealSense::acquire(vpImage<vpRGBa> &color, vpImage<uint16_t> &infrared, v
\param depth : Depth image.
\param pointcloud : Point cloud data with texture information.
*/
-void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
+void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infrared, vpImage<uint16_t> &depth,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
{
if (m_device == NULL) {
throw vpException(vpException::fatalError, "RealSense Camera - Device not opened!");
}
- if (! m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -831,23 +896,32 @@ void vpRealSense::acquire(vpImage<unsigned char> &grey, vpImage<uint16_t> &infra
\param data_image : Color image buffer or NULL if not wanted.
\param data_depth : Depth image buffer or NULL if not wanted.
\param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
- \param pointcloud : Point cloud (in PCL format and without texture information) pointer or NULL if not wanted.
- \param data_infrared : Infrared image buffer or NULL if not wanted.
- \param data_infrared2 : Infrared (for the second IR camera if available) image buffer or NULL if not wanted.
- \param stream_color : Type of color stream (e.g. rs::stream::color, rs::stream::rectified_color, rs::stream::color_aligned_to_depth).
- \param stream_depth : Type of depth stream (e.g. rs::stream::depth, rs::stream::depth_aligned_to_color, rs::stream::depth_aligned_to_rectified_color, rs::stream::depth_aligned_to_infrared2).
- \param stream_infrared : Type of infrared stream (only rs::stream::infrared should be possible).
- \param stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera.
+ \param pointcloud : Point cloud (in PCL format and without texture
+ information) pointer or NULL if not wanted. \param data_infrared : Infrared
+ image buffer or NULL if not wanted. \param data_infrared2 : Infrared (for
+ the second IR camera if available) image buffer or NULL if not wanted.
+ \param stream_color : Type of color stream (e.g. rs::stream::color,
+ rs::stream::rectified_color, rs::stream::color_aligned_to_depth). \param
+ stream_depth : Type of depth stream (e.g. rs::stream::depth,
+ rs::stream::depth_aligned_to_color,
+ rs::stream::depth_aligned_to_rectified_color,
+ rs::stream::depth_aligned_to_infrared2). \param stream_infrared : Type of
+ infrared stream (only rs::stream::infrared should be possible). \param
+ 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<vpColVector> * const data_pointCloud,
- pcl::PointCloud<pcl::PointXYZ>::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<vpColVector> *const data_pointCloud,
+ pcl::PointCloud<pcl::PointXYZ>::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!");
}
- if ( !m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -883,23 +957,31 @@ void vpRealSense::acquire(unsigned char * const data_image, unsigned char * cons
\param data_image : Color image buffer or NULL if not wanted.
\param data_depth : Depth image buffer or NULL if not wanted.
\param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
- \param pointcloud : Point cloud (in PCL format and with texture information) pointer or NULL if not wanted.
- \param data_infrared : Infrared image buffer or NULL if not wanted.
- \param data_infrared2 : Infrared (for the second IR camera if available) image buffer or NULL if not wanted.
- \param stream_color : Type of color stream (e.g. rs::stream::color, rs::stream::rectified_color, rs::stream::color_aligned_to_depth).
- \param stream_depth : Type of depth stream (e.g. rs::stream::depth, rs::stream::depth_aligned_to_color, rs::stream::depth_aligned_to_rectified_color, rs::stream::depth_aligned_to_infrared2).
- \param stream_infrared : Type of infrared stream (only rs::stream::infrared should be possible).
- \param stream_infrared2 : Type of infrared2 stream (e.g. rs::stream::infrared2, rs::stream::infrared2_aligned_to_depth) if supported by the camera.
+ \param pointcloud : Point cloud (in PCL format and with texture information)
+ pointer or NULL if not wanted. \param data_infrared : Infrared image buffer
+ or NULL if not wanted. \param data_infrared2 : Infrared (for the second IR
+ camera if available) image buffer or NULL if not wanted. \param stream_color
+ : Type of color stream (e.g. rs::stream::color, rs::stream::rectified_color,
+ rs::stream::color_aligned_to_depth). \param stream_depth : Type of depth
+ stream (e.g. rs::stream::depth, rs::stream::depth_aligned_to_color,
+ rs::stream::depth_aligned_to_rectified_color,
+ rs::stream::depth_aligned_to_infrared2). \param stream_infrared : Type of
+ infrared stream (only rs::stream::infrared should be possible). \param
+ 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<vpColVector> * const data_pointCloud,
- pcl::PointCloud<pcl::PointXYZRGB>::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<vpColVector> *const data_pointCloud,
+ pcl::PointCloud<pcl::PointXYZRGB>::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!");
}
- if ( !m_device->is_streaming()) {
+ if (!m_device->is_streaming()) {
open();
}
@@ -918,7 +1000,8 @@ void vpRealSense::acquire(unsigned char * const data_image, unsigned char * cons
}
if (pointcloud != NULL) {
- vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color, stream_depth);
+ vp_rs_get_pointcloud_impl(m_device, m_intrinsics, m_max_Z, pointcloud, m_invalidDepthValue, stream_color,
+ stream_depth);
}
if (data_infrared != NULL) {
@@ -947,20 +1030,22 @@ int main()
vpRealSense rs;
rs.open();
std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
+ return 0;
}
\endcode
*/
-std::ostream & operator<<(std::ostream &os, const vpRealSense &rs)
+std::ostream &operator<<(std::ostream &os, const vpRealSense &rs)
{
os << "Device name: " << rs.getHandler()->get_name() << std::endl;
std::streamsize ss = std::cout.precision();
- for(int i = 0; i < 4; ++i)
- {
+ for (int i = 0; i < 4; ++i) {
auto stream = rs::stream(i);
- if(!rs.getHandler()->is_stream_enabled(stream)) continue;
+ 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() << ", distortion = " << intrin.model() << std::endl;
+ std::cout << std::setprecision(1) << std::fixed << ", fov = " << intrin.hfov() << " x " << intrin.vfov()
+ << ", distortion = " << intrin.model() << std::endl;
}
std::cout.precision(ss);
@@ -968,6 +1053,7 @@ std::ostream & operator<<(std::ostream &os, const vpRealSense &rs)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has no symbols
-void dummy_vpRealSense() {};
+// Work arround to avoid warning: libvisp_sensor.a(vpRealSense.cpp.o) has no
+// symbols
+void dummy_vpRealSense(){};
#endif
diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp
new file mode 100644
index 0000000..ccafcd7
--- /dev/null
+++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense2.cpp
@@ -0,0 +1,794 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * librealSense2 interface.
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
+#include <iomanip>
+#include <map>
+#include <set>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+#define MANUAL_POINTCLOUD 1
+
+/*!
+ * 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()
+{
+}
+
+/*!
+ * Default destructor that stops the streaming.
+ * \sa stop()
+ */
+vpRealSense2::~vpRealSense2() { close(); }
+
+/*!
+ Acquire greyscale image from RealSense device.
+ \param grey : Greyscale image.
+ */
+void vpRealSense2::acquire(vpImage<unsigned char> &grey)
+{
+ auto data = m_pipe.wait_for_frames();
+ auto color_frame = data.get_color_frame();
+ getGreyFrame(color_frame, grey);
+}
+
+/*!
+ Acquire color image from RealSense device.
+ \param color : Color image.
+ */
+void vpRealSense2::acquire(vpImage<vpRGBa> &color)
+{
+ auto data = m_pipe.wait_for_frames();
+ auto color_frame = data.get_color_frame();
+ getColorFrame(color_frame, color);
+}
+
+/*!
+ Acquire data from RealSense device.
+ \param data_image : Color image buffer or NULL if not wanted.
+ \param data_depth : Depth image buffer or NULL if not wanted.
+ \param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
+ \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<vpColVector> *const data_pointCloud, unsigned char *const data_infrared,
+ rs2::align *const align_to)
+{
+ auto data = m_pipe.wait_for_frames();
+ if (align_to != NULL)
+ data = align_to->proccess(data);
+
+ if (data_image != NULL) {
+ auto color_frame = data.get_color_frame();
+ getNativeFrameData(color_frame, data_image);
+ }
+
+ if (data_depth != NULL || data_pointCloud != NULL) {
+ auto depth_frame = data.get_depth_frame();
+ if (data_depth != NULL)
+ getNativeFrameData(depth_frame, data_depth);
+
+ if (data_pointCloud != NULL)
+ getPointcloud(depth_frame, *data_pointCloud);
+ }
+
+ if (data_infrared != NULL) {
+ auto infrared_frame = data.first(RS2_STREAM_INFRARED);
+ getNativeFrameData(infrared_frame, data_infrared);
+ }
+}
+
+#ifdef VISP_HAVE_PCL
+/*!
+ Acquire data from RealSense device.
+ \param data_image : Color image buffer or NULL if not wanted.
+ \param data_depth : Depth image buffer or NULL if not wanted.
+ \param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
+ \param pointcloud : Point cloud (in PCL format and without texture
+ information) pointer or NULL if not wanted. \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<vpColVector> *const data_pointCloud,
+ pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud, unsigned char *const data_infrared,
+ rs2::align *const align_to)
+{
+ auto data = m_pipe.wait_for_frames();
+ if (align_to != NULL)
+ data = align_to->proccess(data);
+
+ if (data_image != NULL) {
+ auto color_frame = data.get_color_frame();
+ getNativeFrameData(color_frame, data_image);
+ }
+
+ if (data_depth != NULL || data_pointCloud != NULL || pointcloud != NULL) {
+ auto depth_frame = data.get_depth_frame();
+ if (data_depth != NULL)
+ getNativeFrameData(depth_frame, data_depth);
+
+ if (data_pointCloud != NULL)
+ getPointcloud(depth_frame, *data_pointCloud);
+
+ if (pointcloud != NULL)
+ getPointcloud(depth_frame, pointcloud);
+ }
+
+ if (data_infrared != NULL) {
+ auto infrared_frame = data.first(RS2_STREAM_INFRARED);
+ getNativeFrameData(infrared_frame, data_infrared);
+ }
+}
+
+/*!
+ Acquire data from RealSense device.
+ \param data_image : Color image buffer or NULL if not wanted.
+ \param data_depth : Depth image buffer or NULL if not wanted.
+ \param data_pointCloud : Point cloud vector pointer or NULL if not wanted.
+ \param pointcloud : Point cloud (in PCL format and with texture information)
+ pointer or NULL if not wanted. \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<vpColVector> *const data_pointCloud,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud, unsigned char *const data_infrared,
+ rs2::align *const align_to)
+{
+ auto data = m_pipe.wait_for_frames();
+ if (align_to != NULL)
+ data = align_to->proccess(data);
+
+ auto color_frame = data.get_color_frame();
+ if (data_image != NULL) {
+ getNativeFrameData(color_frame, data_image);
+ }
+
+ if (data_depth != NULL || data_pointCloud != NULL || pointcloud != NULL) {
+ auto depth_frame = data.get_depth_frame();
+ if (data_depth != NULL)
+ getNativeFrameData(depth_frame, data_depth);
+
+ if (data_pointCloud != NULL)
+ getPointcloud(depth_frame, *data_pointCloud);
+
+ if (pointcloud != NULL)
+ getPointcloud(depth_frame, color_frame, pointcloud);
+ }
+
+ if (data_infrared != NULL) {
+ auto infrared_frame = data.first(RS2_STREAM_INFRARED);
+ getNativeFrameData(infrared_frame, data_infrared);
+ }
+}
+#endif
+
+/*!
+ librealsense documentation:
+ <blockquote>
+ Stop the pipeline streaming.
+ The pipeline stops delivering samples to the attached computer vision
+ modules and processing blocks, stops the device streaming and releases the
+ device resources used by the pipeline. It is the application's
+ responsibility to release any frame reference it owns. The method takes
+ effect only after \c start() was called, otherwise an exception is raised.
+ </blockquote>
+ */
+void vpRealSense2::close() { m_pipe.stop(); }
+
+/*!
+ Return the camera parameters corresponding to a specific stream. This
+ function has to be called after open(). \param stream : stream for which
+ camera intrinsic parameters are returned. \param type : Indicate if the
+ model should include distorsion parameters or not.
+
+ \sa getIntrinsics()
+ */
+vpCameraParameters vpRealSense2::getCameraParameters(const rs2_stream &stream,
+ vpCameraParameters::vpCameraParametersProjType type) const
+{
+ auto rs_stream = m_pipelineProfile.get_stream(stream).as<rs2::video_stream_profile>();
+ auto intrinsics = rs_stream.get_intrinsics();
+
+ vpCameraParameters cam;
+ double u0 = intrinsics.ppx;
+ double v0 = intrinsics.ppy;
+ double px = intrinsics.fx;
+ double py = intrinsics.fy;
+
+ if (type == vpCameraParameters::perspectiveProjWithDistortion) {
+ double kdu = intrinsics.coeffs[0];
+ cam.initPersProjWithDistortion(px, py, u0, v0, -kdu, kdu);
+ } else {
+ cam.initPersProjWithoutDistortion(px, py, u0, v0);
+ }
+
+ return cam;
+}
+
+/*!
+ Get intrinsic parameters corresponding to the stream. This function has to
+ be called after open(). \param stream : stream for which the camera
+ intrinsic parameters are returned.
+
+ \sa getCameraParameters()
+ */
+rs2_intrinsics vpRealSense2::getIntrinsics(const rs2_stream &stream) const
+{
+ auto vsp = m_pipelineProfile.get_stream(stream).as<rs2::video_stream_profile>();
+ return vsp.get_intrinsics();
+}
+
+void vpRealSense2::getColorFrame(const rs2::frame &frame, vpImage<vpRGBa> &color)
+{
+ auto vf = frame.as<rs2::video_frame>();
+ const unsigned int width = (unsigned int)vf.get_width();
+ const unsigned int height = (unsigned int)vf.get_height();
+ 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);
+ } else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
+ 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);
+ } else {
+ throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!");
+ }
+}
+
+void vpRealSense2::getGreyFrame(const rs2::frame &frame, vpImage<unsigned char> &grey)
+{
+ auto vf = frame.as<rs2::video_frame>();
+ const unsigned int width = (unsigned int)vf.get_width();
+ const unsigned int height = (unsigned int)vf.get_height();
+ 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);
+ } else if (frame.get_profile().format() == RS2_FORMAT_RGBA8) {
+ 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);
+ } else {
+ throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!");
+ }
+}
+
+void vpRealSense2::getNativeFrameData(const rs2::frame &frame, unsigned char *const data)
+{
+ auto vf = frame.as<rs2::video_frame>();
+ int size = vf.get_width() * vf.get_height();
+
+ switch (frame.get_profile().format()) {
+ case RS2_FORMAT_RGB8:
+ case RS2_FORMAT_BGR8:
+ memcpy(data, frame.get_data(), size * 3);
+ break;
+
+ case RS2_FORMAT_RGBA8:
+ case RS2_FORMAT_BGRA8:
+ memcpy(data, frame.get_data(), size * 4);
+ break;
+
+ case RS2_FORMAT_Y16:
+ case RS2_FORMAT_Z16:
+ memcpy(data, frame.get_data(), size * 2);
+ break;
+
+ case RS2_FORMAT_Y8:
+ memcpy(data, frame.get_data(), size);
+ break;
+
+ default:
+ break;
+ }
+}
+
+void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, std::vector<vpColVector> &pointcloud)
+{
+ auto vf = depth_frame.as<rs2::video_frame>();
+ 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<const uint16_t *>(depth_frame.get_data());
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0; i < height; i++) {
+ auto depth_pixel_index = i * width;
+
+ for (int j = 0; j < width; j++, depth_pixel_index++) {
+ // Get the depth value of the current pixel
+ auto pixels_distance = m_depthScale * p_depth_frame[depth_pixel_index];
+
+ float points[3];
+ const float pixel[] = {(float)j, (float)i};
+ 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;
+
+ vpColVector v(4);
+ v[0] = points[0];
+ v[1] = points[1];
+ v[2] = points[2];
+ v[3] = 1.0;
+ pointcloud[(size_t)depth_pixel_index] = v;
+ }
+ }
+}
+
+#ifdef VISP_HAVE_PCL
+void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, pcl::PointCloud<pcl::PointXYZ>::Ptr &pointcloud)
+{
+ auto vf = depth_frame.as<rs2::video_frame>();
+ const int width = vf.get_width();
+ const int height = vf.get_height();
+ pointcloud->width = (uint32_t)width;
+ pointcloud->height = (uint32_t)height;
+ pointcloud->resize((size_t)(width * height));
+
+#if MANUAL_POINTCLOUD // faster when tested
+ const uint16_t *p_depth_frame = reinterpret_cast<const uint16_t *>(depth_frame.get_data());
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0; i < height; i++) {
+ auto depth_pixel_index = i * width;
+
+ for (int j = 0; j < width; j++, depth_pixel_index++) {
+ // Get the depth value of the current pixel
+ auto pixels_distance = m_depthScale * p_depth_frame[depth_pixel_index];
+
+ float points[3];
+ const float pixel[] = {(float)j, (float)i};
+ 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;
+
+ pointcloud->points[(size_t)(depth_pixel_index)].x = points[0];
+ pointcloud->points[(size_t)(depth_pixel_index)].y = points[1];
+ pointcloud->points[(size_t)(depth_pixel_index)].z = points[2];
+ }
+ }
+#else
+ m_points = m_pointcloud.calculate(depth_frame);
+ auto vertices = m_points.get_vertices();
+
+ for (size_t i = 0; i < m_points.size(); i++) {
+ if (vertices[i].z <= 0.0f || vertices[i].z > m_max_Z) {
+ pointcloud->points[i].x = m_invalidDepthValue;
+ pointcloud->points[i].y = m_invalidDepthValue;
+ pointcloud->points[i].z = m_invalidDepthValue;
+ } else {
+ pointcloud->points[i].x = vertices[i].x;
+ pointcloud->points[i].y = vertices[i].y;
+ pointcloud->points[i].z = vertices[i].z;
+ }
+ }
+#endif
+}
+
+void vpRealSense2::getPointcloud(const rs2::depth_frame &depth_frame, const rs2::frame &color_frame,
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr &pointcloud)
+{
+ auto vf = depth_frame.as<rs2::video_frame>();
+ const int width = vf.get_width();
+ const int height = vf.get_height();
+ pointcloud->width = (uint32_t)width;
+ pointcloud->height = (uint32_t)height;
+ pointcloud->resize((size_t)(width * height));
+
+ const uint16_t *p_depth_frame = reinterpret_cast<const uint16_t *>(depth_frame.get_data());
+ const int color_width = m_colorIntrinsics.width;
+ const int color_height = m_colorIntrinsics.height;
+ auto color_format = color_frame.as<rs2::video_frame>().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<const unsigned char *>(color_frame.get_data());
+
+#pragma omp parallel for schedule(dynamic)
+ for (int i = 0; i < height; i++) {
+ auto depth_pixel_index = i * width;
+
+ for (int j = 0; j < width; j++, depth_pixel_index++) {
+ // Get the depth value of the current pixel
+ auto pixels_distance = m_depthScale * p_depth_frame[depth_pixel_index];
+
+ float depth_point[3];
+ const float pixel[] = {(float)j, (float)i};
+ 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;
+
+ 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);
+ float color_pixel[2];
+ 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) {
+// 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<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
+
+ pointcloud->points[(size_t)depth_pixel_index].rgb = *reinterpret_cast<float *>(&rgb);
+#else
+ pointcloud->points[(size_t)depth_pixel_index].r = (uint8_t)96;
+ pointcloud->points[(size_t)depth_pixel_index].g = (uint8_t)157;
+ pointcloud->points[(size_t)depth_pixel_index].b = (uint8_t)198;
+#endif
+ } else {
+ unsigned int i_ = (unsigned int)color_pixel[1];
+ unsigned int j_ = (unsigned int)color_pixel[0];
+
+#if PCL_VERSION_COMPARE(<, 1, 1, 0)
+ uint32_t rgb = 0;
+ if (swap_rgb) {
+ rgb =
+ (static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) |
+ static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
+ static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16);
+ } else {
+ rgb = (static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) << 16 |
+ static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
+ static_cast<uint32_t>(p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]));
+ }
+
+ pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast<float *>(&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];
+ pointcloud->points[(size_t)depth_pixel_index].g =
+ (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];
+ } else {
+ pointcloud->points[(size_t)depth_pixel_index].r =
+ (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];
+ pointcloud->points[(size_t)depth_pixel_index].b =
+ (uint32_t)p_color_frame[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2];
+ }
+#endif
+ }
+ }
+ }
+}
+
+#endif
+
+/*!
+ Get the extrinsic transformation from one stream to another. This function
+ 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
+{
+ auto from_stream = m_pipelineProfile.get_stream(from);
+ auto to_stream = m_pipelineProfile.get_stream(to);
+ rs2_extrinsics extrinsics = from_stream.get_extrinsics_to(to_stream);
+
+ vpTranslationVector t;
+ vpRotationMatrix R;
+ for (unsigned int i = 0; i < 3; i++) {
+ t[i] = extrinsics.translation[i];
+ for (unsigned int j = 0; j < 3; j++)
+ R[i][j] = extrinsics.rotation[i * 3 + j];
+ }
+
+ vpHomogeneousMatrix to_M_from(t, R);
+ return to_M_from;
+}
+
+/*!
+ Open access to the RealSense device and start the streaming.
+ */
+void vpRealSense2::open(const rs2::config &cfg)
+{
+ m_pipelineProfile = m_pipe.start(cfg);
+
+ rs2::device dev = m_pipelineProfile.get_device();
+
+ // Go over the device's sensors
+ for (rs2::sensor &sensor : dev.query_sensors()) {
+ // Check if the sensor if a depth sensor
+ if (rs2::depth_sensor dpt = sensor.as<rs2::depth_sensor>()) {
+ m_depthScale = dpt.get_depth_scale();
+ }
+ }
+
+ auto depth_stream = m_pipelineProfile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
+ 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<rs2::video_stream_profile>().get_intrinsics();
+ m_depth2ColorExtrinsics = depth_stream.get_extrinsics_to(color_stream);
+}
+
+namespace
+{
+// Helper functions to print information about the RealSense device
+void print(const rs2_extrinsics &extrinsics, std::ostream &os)
+{
+ std::stringstream ss;
+ ss << "Rotation Matrix:\n";
+
+ 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::endl;
+ }
+
+ ss << "\nTranslation Vector: ";
+ 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";
+}
+
+void print(const rs2_intrinsics &intrinsics, std::ostream &os)
+{
+ std::stringstream ss;
+ ss << std::left << std::setw(14) << "Width: "
+ << "\t" << intrinsics.width << "\n"
+ << std::left << std::setw(14) << "Height: "
+ << "\t" << intrinsics.height << "\n"
+ << std::left << std::setw(14) << "PPX: "
+ << "\t" << std::setprecision(15) << intrinsics.ppx << "\n"
+ << std::left << std::setw(14) << "PPY: "
+ << "\t" << std::setprecision(15) << intrinsics.ppy << "\n"
+ << std::left << std::setw(14) << "Fx: "
+ << "\t" << std::setprecision(15) << intrinsics.fx << "\n"
+ << std::left << std::setw(14) << "Fy: "
+ << "\t" << std::setprecision(15) << intrinsics.fy << "\n"
+ << std::left << std::setw(14) << "Distortion: "
+ << "\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)
+ 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)
+{
+ try {
+ intrinsics = profile.get_intrinsics();
+ } catch (...) {
+ }
+}
+
+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 &&
+ !std::memcmp(lhs.coeffs, rhs.coeffs, sizeof(rhs.coeffs));
+}
+
+std::string get_str_formats(const std::set<rs2_format> &formats)
+{
+ std::stringstream ss;
+ for (auto format = formats.begin(); format != formats.end(); ++format) {
+ ss << *format << ((format != formats.end()) && (next(format) == formats.end()) ? "" : "/");
+ }
+ return ss.str();
+}
+
+struct stream_and_resolution {
+ rs2_stream stream;
+ int stream_index;
+ int width;
+ int height;
+ std::string stream_name;
+
+ 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));
+ }
+};
+
+struct stream_and_index {
+ rs2_stream stream;
+ int stream_index;
+
+ bool operator<(const stream_and_index &obj) const
+ {
+ return (std::make_tuple(stream, stream_index) < std::make_tuple(obj.stream, obj.stream_index));
+ }
+};
+} // anonymous namespace
+
+/*!
+ \relates vpRealSense2
+
+ Return information from the sensor.
+ \param os : Input stream.
+ \param rs : RealSense object.
+
+ The following example shows how to use this method.
+ \code
+#include <visp3/sensor/vpRealSense2.h>
+
+int main()
+{
+ vpRealSense2 rs;
+ rs.open();
+ std::cout << "RealSense sensor information:\n" << rs << std::endl;
+ return 0;
+}
+ \endcode
+ */
+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;
+
+ 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)
+ << std::endl;
+
+ // Show which options are supported by this device
+ os << " Device info: \n";
+ for (auto j = 0; j < RS2_CAMERA_INFO_COUNT; ++j) {
+ auto param = static_cast<rs2_camera_info>(j);
+ if (dev.supports(param))
+ 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 << 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<rs2_option>(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 << "\n";
+ }
+
+ for (auto &&sensor : dev.query_sensors()) {
+ 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"
+ << "\n";
+ // Show which streams are supported by this device
+ for (auto &&profile : sensor.get_stream_profiles()) {
+ if (auto video = profile.as<rs2::video_stream_profile>()) {
+ 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 << "\n";
+ }
+
+ std::map<stream_and_index, rs2::stream_profile> streams;
+ std::map<stream_and_resolution, std::vector<std::pair<std::set<rs2_format>, rs2_intrinsics> > > intrinsics_map;
+ for (auto &&sensor : dev.query_sensors()) {
+ // Intrinsics
+ for (auto &&profile : sensor.get_stream_profiles()) {
+ if (auto video = profile.as<rs2::video_stream_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()};
+ safe_get_intrinsics(video, intrinsics);
+ auto it = std::find_if(
+ (intrinsics_map[stream_res]).begin(), (intrinsics_map[stream_res]).end(),
+ [&](const std::pair<std::set<rs2_format>, rs2_intrinsics> &kvp) { return intrinsics == kvp.second; });
+ if (it == (intrinsics_map[stream_res]).end()) {
+ (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
+ // format set
+ }
+ }
+ }
+ }
+
+ os << "Provided Intrinsic:\n";
+ for (auto &kvp : intrinsics_map) {
+ 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";
+ if (intrinsics.second == rs2_intrinsics{}) {
+ os << "Intrinsic NOT available!\n\n";
+ } else {
+ print(intrinsics.second, os);
+ }
+ }
+ }
+
+ // Print Extrinsics
+ os << "\nProvided Extrinsic:\n";
+ for (auto kvp1 = streams.begin(); kvp1 != streams.end(); ++kvp1) {
+ for (auto kvp2 = streams.begin(); kvp2 != streams.end(); ++kvp2) {
+ os << "Extrinsic from \"" << kvp1->second.stream_name() << "\"\t "
+ << "To"
+ << "\t \"" << kvp2->second.stream_name() << "\"\n";
+ auto extrinsics = kvp1->second.get_extrinsics_to(kvp2->second);
+ print(extrinsics, os);
+ }
+ }
+
+ return os;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_sensor.a(vpRealSense2.cpp.o) has no
+// symbols
+void dummy_vpRealSense2(){};
+#endif
diff --git a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h
index 1636b09..8a4f6d2 100644
--- a/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h
+++ b/modules/sensor/src/rgb-depth/realsense/vpRealSense_impl.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,12 +39,17 @@
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_REALSENSE)
-
#include <librealsense/rs.hpp>
+#include <thread>
#include <visp3/core/vpImage.h>
+#ifdef VISP_HAVE_PCL
+#include <pcl/pcl_config.h>
+#endif
+
template <class Type>
-void vp_rs_get_frame_data_impl(const rs::device *m_device, const std::map <rs::stream, rs::intrinsics> &m_intrinsics, const rs::stream &stream, vpImage<Type> &data)
+void vp_rs_get_frame_data_impl(const rs::device *m_device, const std::map<rs::stream, rs::intrinsics> &m_intrinsics,
+ const rs::stream &stream, vpImage<Type> &data)
{
// Retrieve data from stream
if (m_device->is_stream_enabled(stream)) {
@@ -54,20 +60,22 @@ void vp_rs_get_frame_data_impl(const rs::device *m_device, const std::map <rs::s
throw vpException(vpException::fatalError, ss.str());
}
- unsigned int width = (unsigned int) it_intrinsics->second.width;
- unsigned int height = (unsigned int) it_intrinsics->second.height;
+ unsigned int width = (unsigned int)it_intrinsics->second.width;
+ 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), width*height*sizeof(Type));
- }
- else {
+ 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);
}
}
// Retrieve data from native stream
-void vp_rs_get_native_frame_data_impl(const rs::device *m_device, const std::map <rs::stream, rs::intrinsics> &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<rs::stream, rs::intrinsics> &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<rs::stream, rs::intrinsics>::const_iterator it_intrinsics = m_intrinsics.find(native_stream);
@@ -77,71 +85,71 @@ void vp_rs_get_native_frame_data_impl(const rs::device *m_device, const std::map
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
- case rs::format::raw8:
- std::cout << "Stream: raw8 not tested!" << std::endl;
- case rs::format::y8:
- memcpy( data, (unsigned char *) m_device->get_frame_data(stream), size );
- break;
-
- //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)) );
- 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 );
- 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 );
- break;
- case rs::format::y16:
- case rs::format::z16:
- 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 );
- 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 );
- break;
- case rs::format::bgra8:
- case rs::format::rgba8:
- 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 );
- break;
-
- case rs::format::any:
- default:
- break;
+ // 8 bits
+ case rs::format::raw8:
+ std::cout << "Stream: raw8 not tested!" << std::endl;
+ case rs::format::y8:
+ memcpy(data, (unsigned char *)m_device->get_frame_data(stream), size);
+ break;
+
+ // 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)));
+ 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);
+ 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);
+ break;
+ case rs::format::y16:
+ case rs::format::z16:
+ 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);
+ 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);
+ break;
+ case rs::format::bgra8:
+ case rs::format::rgba8:
+ 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);
+ break;
+
+ case rs::format::any:
+ default:
+ break;
}
- }
- else {
- throw vpException(vpException::fatalError, "RealSense Camera - stream %d not enabled!", (rs_stream) native_stream);
+ } else {
+ 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 <rs::stream, rs::intrinsics> &m_intrinsics, vpImage<vpRGBa> &color)
+void vp_rs_get_color_impl(const rs::device *m_device, const std::map<rs::stream, rs::intrinsics> &m_intrinsics,
+ vpImage<vpRGBa> &color)
{
if (m_device->is_stream_enabled(rs::stream::color)) {
std::map<rs::stream, rs::intrinsics>::const_iterator it_intrinsics = m_intrinsics.find(rs::stream::color);
@@ -149,24 +157,30 @@ void vp_rs_get_color_impl(const rs::device *m_device, const std::map <rs::stream
throw vpException(vpException::fatalError, "Cannot find intrinsics for color stream!");
}
- unsigned int width = (unsigned int) it_intrinsics->second.width;
- unsigned int height = (unsigned int) it_intrinsics->second.height;
+ unsigned int width = (unsigned int)it_intrinsics->second.width;
+ unsigned int height = (unsigned int)it_intrinsics->second.height;
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), width*height*sizeof(vpRGBa) );
- else
+ 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),
+ 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 {
throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!");
- }
- else {
+ }
+ } else {
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 <rs::stream, rs::intrinsics> &m_intrinsics, vpImage<unsigned char> &grey)
+void vp_rs_get_grey_impl(const rs::device *m_device, const std::map<rs::stream, rs::intrinsics> &m_intrinsics,
+ vpImage<unsigned char> &grey)
{
if (m_device->is_stream_enabled(rs::stream::color)) {
std::map<rs::stream, rs::intrinsics>::const_iterator it_intrinsics = m_intrinsics.find(rs::stream::color);
@@ -174,25 +188,31 @@ void vp_rs_get_grey_impl(const rs::device *m_device, const std::map <rs::stream,
throw vpException(vpException::fatalError, "Cannot find intrinsics for color stream!");
}
- unsigned int width = (unsigned int) it_intrinsics->second.width;
- unsigned int height = (unsigned int) it_intrinsics->second.height;
+ unsigned int width = (unsigned int)it_intrinsics->second.width;
+ unsigned int height = (unsigned int)it_intrinsics->second.height;
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::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);
+ } else {
throw vpException(vpException::fatalError, "RealSense Camera - color stream not supported!");
- }
- else {
+ }
+ } else {
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 <rs::stream, rs::intrinsics> &m_intrinsics, float max_Z, std::vector<vpColVector> &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<rs::stream, rs::intrinsics> &m_intrinsics,
+ float max_Z, std::vector<vpColVector> &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<rs::stream, rs::intrinsics>::const_iterator it_intrinsics = m_intrinsics.find(stream_depth);
@@ -204,16 +224,16 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map <rs::s
vpColVector p3d(4); // X,Y,Z coordinates
rs::float3 depth_point;
- uint16_t * depth = (uint16_t *)m_device->get_frame_data(stream_depth);
+ uint16_t *depth = (uint16_t *)m_device->get_frame_data(stream_depth);
int width = it_intrinsics->second.width;
int height = it_intrinsics->second.height;
- pointcloud.resize((size_t) (width*height));
+ pointcloud.resize((size_t)(width * height));
for (int i = 0; i < height; i++) {
for (int j = 0; j < width; j++) {
- float scaled_depth = depth[i*width + j] * depth_scale;
+ float scaled_depth = depth[i * width + j] * depth_scale;
- rs::float2 depth_pixel = { (float) j, (float) i};
+ rs::float2 depth_pixel = {(float)j, (float)i};
depth_point = it_intrinsics->second.deproject(depth_pixel, scaled_depth);
if (depth_point.z <= 0 || depth_point.z > max_Z) {
@@ -224,19 +244,19 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map <rs::s
p3d[2] = depth_point.z;
p3d[3] = 1;
- pointcloud[(size_t) (i*width + j)] = p3d;
+ pointcloud[(size_t)(i * width + j)] = p3d;
}
}
- }
- else {
+ } else {
pointcloud.clear();
}
}
#ifdef VISP_HAVE_PCL
// Retrieve point cloud
-void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map<rs::stream, rs::intrinsics> &m_intrinsics, float max_Z, pcl::PointCloud<pcl::PointXYZ>::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<rs::stream, rs::intrinsics> &m_intrinsics,
+ float max_Z, pcl::PointCloud<pcl::PointXYZ>::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<rs::stream, rs::intrinsics>::const_iterator it_intrinsics = m_intrinsics.find(stream_depth);
@@ -246,40 +266,41 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map<rs::st
int width = it_intrinsics->second.width;
int height = it_intrinsics->second.height;
- pointcloud->width = (uint32_t) width;
- pointcloud->height = (uint32_t) height;
- pointcloud->resize((size_t) (width * height));
+ pointcloud->width = (uint32_t)width;
+ pointcloud->height = (uint32_t)height;
+ pointcloud->resize((size_t)(width * height));
const float depth_scale = m_device->get_depth_scale();
// Fill the PointCloud2 fields.
rs::float3 depth_point;
- uint16_t * depth = (uint16_t *)m_device->get_frame_data(stream_depth);
+ uint16_t *depth = (uint16_t *)m_device->get_frame_data(stream_depth);
for (int i = 0; i < height; i++) {
for (int j = 0; j < width; j++) {
- float scaled_depth = depth[i*width + j] * depth_scale;
+ float scaled_depth = depth[i * width + j] * depth_scale;
- rs::float2 depth_pixel = { (float) j, (float) i};
+ rs::float2 depth_pixel = {(float)j, (float)i};
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;
}
- pointcloud->points[(size_t) (i*width + j)].x = depth_point.x;
- pointcloud->points[(size_t) (i*width + j)].y = depth_point.y;
- pointcloud->points[(size_t) (i*width + j)].z = depth_point.z;
+ pointcloud->points[(size_t)(i * width + j)].x = depth_point.x;
+ pointcloud->points[(size_t)(i * width + j)].y = depth_point.y;
+ pointcloud->points[(size_t)(i * width + j)].z = depth_point.z;
}
}
- }
- else {
+ } else {
pointcloud->clear();
}
}
// Retrieve point cloud
-void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map <rs::stream, rs::intrinsics> &m_intrinsics, float max_Z, pcl::PointCloud<pcl::PointXYZRGB>::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<rs::stream, rs::intrinsics> &m_intrinsics,
+ float max_Z, pcl::PointCloud<pcl::PointXYZRGB>::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<rs::stream, rs::intrinsics>::const_iterator it_intrinsics_depth = m_intrinsics.find(stream_depth);
@@ -292,75 +313,118 @@ void vp_rs_get_pointcloud_impl(const rs::device *m_device, const std::map <rs::s
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!");
+ }
+
int depth_width = it_intrinsics_depth->second.width;
int depth_height = it_intrinsics_depth->second.height;
- pointcloud->width = (uint32_t) depth_width;
- pointcloud->height = (uint32_t) depth_height;
- pointcloud->resize((size_t) (depth_width * depth_height));
+ pointcloud->width = (uint32_t)depth_width;
+ pointcloud->height = (uint32_t)depth_height;
+ 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);
- // Fill the PointCloud2 fields.
- rs::float3 depth_point, color_point;
- rs::float2 color_pixel;
- uint16_t * depth = (uint16_t *)m_device->get_frame_data(stream_depth);
- 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;
- 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;
- for (int i = 0; i < depth_height; i++) {
- for (int j = 0; j < depth_width; j++) {
- float scaled_depth = depth[i*depth_width + j] * depth_scale;
-
- rs::float2 depth_pixel = { (float) j, (float) i};
- 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;
- }
- 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;
-
- color_point = depth_2_color_extrinsic.transform(depth_point);
- 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.
- unsigned int r = 96, g = 157, b = 198;
- uint32_t rgb = (static_cast<uint32_t>(r) << 16 |
- static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
-
- pointcloud->points[(size_t) (i*depth_width + j)].rgb = *reinterpret_cast<float*>(&rgb);
- }
- else
- {
- unsigned int i_ = (unsigned int) color_pixel.y;
- unsigned int j_ = (unsigned int) color_pixel.x;
-
- uint32_t rgb = 0;
- if (m_device->get_stream_format(rs::stream::color) == rs::format::rgb8 || m_device->get_stream_format(rs::stream::color) == rs::format::rgba8) {
- rgb = (static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel]) << 16 |
- static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel + 1]) << 8 |
- static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel + 2]));
- } else if (m_device->get_stream_format(rs::stream::color) == rs::format::bgr8 || m_device->get_stream_format(rs::stream::color) == rs::format::bgra8) {
- rgb = (static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel]) |
- static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel + 1]) << 8 |
- static_cast<uint32_t>(color[(i_* (unsigned int) color_width + j_) * nb_color_pixel + 2]) << 16);
+ 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;
+
+ std::vector<std::thread> 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;
+
+ workers.push_back(std::thread([&, start_index, end_index]() {
+ for (int i = start_index; i < end_index; i++) {
+ for (int j = 0; j < depth_width; j++) {
+ 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);
+
+ if (depth_point.z <= 0 || depth_point.z > max_Z) {
+ 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) {
+// 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<uint32_t>(r) << 16 | static_cast<uint32_t>(g) << 8 | static_cast<uint32_t>(b));
+
+ pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast<float *>(&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;
+#endif
+ } else {
+ unsigned int i_ = (unsigned int)color_pixel.y;
+ unsigned int j_ = (unsigned int)color_pixel.x;
+
+#if PCL_VERSION_COMPARE(<, 1, 1, 0)
+ uint32_t rgb = 0;
+ if (swap_rgb) {
+ rgb = (static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) |
+ static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
+ static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]) << 16);
+ } else {
+ rgb = (static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel]) << 16 |
+ static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 1]) << 8 |
+ static_cast<uint32_t>(color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2]));
+ }
+
+ pointcloud->points[(size_t)(i * depth_width + j)].rgb = *reinterpret_cast<float *>(&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];
+ pointcloud->points[(size_t)(i * depth_width + j)].g =
+ (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];
+ } else {
+ pointcloud->points[(size_t)(i * depth_width + j)].r =
+ (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];
+ pointcloud->points[(size_t)(i * depth_width + j)].b =
+ (uint32_t)color[(i_ * (unsigned int)color_width + j_) * nb_color_pixel + 2];
+ }
+#endif
+ }
}
-
- pointcloud->points[(size_t) (i*depth_width + j)].rgb = *reinterpret_cast<float*>(&rgb);
}
- }
+ }));
}
- }
- else {
+
+ 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 82fefbe..2b81d40 100644
--- a/modules/sensor/test/force-torque/testComedi.cpp
+++ b/modules/sensor/test/force-torque/testComedi.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,15 +38,16 @@
/*!
\example testComedi.cpp
- This example shows how to retrieve data from a sensor connected to a DAQ board.
- He we have 1 signe main threads that acquires physical values at 100 Hz (10 ms) and records data in recorded-physical-data-sync.txt file.
+ This example shows how to retrieve data from a sensor connected to a DAQ
+ board. He we have 1 signe main threads that acquires physical values at 100
+ Hz (10 ms) and records data in recorded-physical-data-sync.txt file.
*/
#include <iostream>
#include <sstream>
-#include <visp3/sensor/vpComedi.h>
#include <visp3/gui/vpPlot.h>
+#include <visp3/sensor/vpComedi.h>
int main()
{
@@ -56,10 +58,11 @@ int main()
comedi.open();
#ifdef VISP_HAVE_DISPLAY
- vpPlot scope(1, 700, 700, 100, 200, std::string("ATI physical sensor data (") + comedi.getPhyDataUnits() + std::string(")"));
+ vpPlot scope(1, 700, 700, 100, 200,
+ 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 & >((std::ostringstream() << i)).str());
+ for (unsigned int i = 0; i < comedi.getNChannel(); i++)
+ scope.setLegend(0, i, "G" + dynamic_cast<std::ostringstream &>((std::ostringstream() << i)).str());
#endif
std::string file("recorded-physical-data-sync.txt");
@@ -67,10 +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();
diff --git a/modules/sensor/test/force-torque/testForceTorqueAti.cpp b/modules/sensor/test/force-torque/testForceTorqueAti.cpp
index 864de30..5bae55e 100644
--- a/modules/sensor/test/force-torque/testForceTorqueAti.cpp
+++ b/modules/sensor/test/force-torque/testForceTorqueAti.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,9 +38,10 @@
/*!
\example testForceTorqueAti.cpp
- This example shows how to retrieve data from an ATI F/T sensor connected to a DAQ board.
- He we have 2 threads:
- - the main thread running at 1KHz that get the measures from an ATI F/T sensor and records data in recorded-ft-sync.txt file
+ This example shows how to retrieve data from an ATI F/T sensor connected to
+ a DAQ board. He we have 2 threads:
+ - the main thread running at 1KHz that get the measures from an ATI F/T
+ sensor and records data in recorded-ft-sync.txt file
- the scope thread that plots the F/T values in real-time.
*/
@@ -47,19 +49,14 @@
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpMutex.h>
-#include <visp3/core/vpTime.h>
#include <visp3/core/vpThread.h>
-#include <visp3/sensor/vpForceTorqueAtiSensor.h>
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpPlot.h>
+#include <visp3/sensor/vpForceTorqueAtiSensor.h>
#if defined(VISP_HAVE_PTHREAD)
-typedef enum {
- BIAS_DONE,
- UNBIAS_DONE,
- TO_BIAS,
- TO_UNBIAS
-} BiasState;
+typedef enum { BIAS_DONE, UNBIAS_DONE, TO_BIAS, TO_UNBIAS } BiasState;
vpMutex s_mutex_data;
@@ -82,8 +79,8 @@ vpThread::Return scopeFunction(vpThread::Args args)
#ifdef VISP_HAVE_DISPLAY
vpPlot scope(2, 700, 700, 100, 200, "ATI F/T sensor data");
- scope.initGraph(0,3);
- scope.initGraph(1,3);
+ scope.initGraph(0, 3);
+ scope.initGraph(1, 3);
scope.setTitle(0, "Forces (N)");
scope.setTitle(1, "Torques (Nm)");
scope.setLegend(0, 0, "x");
@@ -128,8 +125,10 @@ vpThread::Return scopeFunction(vpThread::Args args)
vpDisplay::flush(scope.I);
click = vpDisplay::getClick(scope.I, button, false);
if (click && button == vpMouseButton::button3) {
- if (shared_data.bias_state == BIAS_DONE) shared_data.bias_state = TO_UNBIAS;
- else if (shared_data.bias_state == UNBIAS_DONE) shared_data.bias_state = TO_BIAS;
+ if (shared_data.bias_state == BIAS_DONE)
+ shared_data.bias_state = TO_UNBIAS;
+ else if (shared_data.bias_state == UNBIAS_DONE)
+ shared_data.bias_state = TO_BIAS;
{ // Set new bias state
vpMutex::vpScopedLock lock(s_mutex_data);
s_shared_data.bias_state = shared_data.bias_state;
@@ -139,9 +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
@@ -153,7 +152,7 @@ vpThread::Return scopeFunction(vpThread::Args args)
return 0;
}
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_ATIDAQ) && defined(VISP_HAVE_COMEDI)
@@ -161,12 +160,12 @@ int main(int argc, char** argv)
(void)argc;
(void)argv;
std::string calibfile = std::string(VISP_VIPER850_DATA_PATH) + std::string("/ati/FT17824.cal");
- if (! vpIoTools::checkFilename(calibfile)) {
+ if (!vpIoTools::checkFilename(calibfile)) {
std::cout << "ATI F/T calib file \"" << calibfile << "\" doesn't exist";
return 0;
}
#else
- if(argc != 2) {
+ if (argc != 2) {
std::cout << "Usage: " << argv[0] << " <ATI calibration file FT*.cal]>" << std::endl;
return -1;
}
@@ -205,8 +204,7 @@ int main(int argc, char** argv)
ati.bias();
std::cout << "Unbias sensor" << std::endl;
shared_data.bias_state = BIAS_DONE;
- }
- else if (shared_data.bias_state == TO_UNBIAS) {
+ } else if (shared_data.bias_state == TO_UNBIAS) {
ati.unbias();
shared_data.bias_state = UNBIAS_DONE;
}
@@ -224,8 +222,7 @@ int main(int argc, char** argv)
f << timestamp << " " << ft.t() << std::endl;
vpTime::wait(loop_time, 1); // Get a new data each 1 millisecond
- }
- while(! state_stop);
+ } while (!state_stop);
// Wait until thread ends up
thread_scope.join();
@@ -241,8 +238,5 @@ int main(int argc, char** argv)
}
#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 030b092..e4a8814 100644
--- a/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp
+++ b/modules/sensor/test/framegrabber/test1394TwoGrabber.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file test1394TwoGrabber.cpp
@@ -43,7 +43,6 @@
*/
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
@@ -52,19 +51,18 @@
#if defined(VISP_HAVE_DC1394)
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
/*!
\example test1394TwoGrabber.cpp
*/
-int
-main()
+int main()
{
- try {
+ try {
std::cout << "IEEE1394 test in progress..." << std::endl;
// Get the user name
@@ -83,50 +81,49 @@ main()
// Get the number of cameras connected on the bus
unsigned int ncameras; // Number of cameras on the bus
ncameras = g.getNumCameras();
- for (unsigned int i=0; i < ncameras; i++) {
+ 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) {
- g.setCamera(ncameras -1);
+ g.setCamera(ncameras - 1);
g.getGuid(guid);
std::cout << "Use camera with GUID 0x" << std::hex << guid << std::endl;
- g.setCamera(0); // to be sure that the setCamera() in the next line with guid as parameter works
+ g.setCamera(0); // to be sure that the setCamera() in the next line with
+ // guid as parameter works
g.setCamera(guid);
}
- for (int i=0;i<10;i++) g.acquire(I);
+ for (int i = 0; i < 10; i++)
+ g.acquire(I);
g.close();
std::string filename = outputpath + "/imagetest1.pgm";
std::cout << "Write image: " << filename << std::endl;
vpImageIo::write(I, filename);
- std::cout << "New connection..."<< std::endl;
+ std::cout << "New connection..." << std::endl;
g.open(I);
g.close();
- std::cout << "New connection..."<< std::endl;
+ std::cout << "New connection..." << std::endl;
g.open(I);
- //g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_800x600_YUV422);
+ // g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_800x600_YUV422);
g.acquire(I);
g.close();
filename = outputpath + "/imagetest2.pgm";
std::cout << "Write image: " << filename << std::endl;
vpImageIo::write(I, filename);
- }
- catch (...) {
+ } catch (...) {
vpCERROR << "Failure: exit" << std::endl;
}
}
#else
-int
-main()
+int main()
{
vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
- "You should install libdc1394-2 to use this binary.") ;
+ "You should install libdc1394-2 to use this binary.");
}
#endif
diff --git a/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp
index 25dfffd..666819d 100644
--- a/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp
+++ b/modules/sensor/test/framegrabber/test1394TwoResetBus.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file test1394TwoResetBus.cpp
@@ -43,8 +43,6 @@
*/
-
-
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
@@ -52,10 +50,9 @@
#if defined(VISP_HAVE_DC1394)
-#include <visp3/sensor/vp1394TwoGrabber.h>
#include <visp3/core/vpImage.h>
#include <visp3/io/vpImageIo.h>
-
+#include <visp3/sensor/vp1394TwoGrabber.h>
/*!
\example test1394TwoResetBus.cpp
@@ -69,30 +66,27 @@
effect.
*/
-int
-main()
+int main()
{
- try {
+ try {
std::cout << "IEEE1394 bus reset in progress..." << std::endl;
vp1394TwoGrabber g;
g.resetBus(); // Reset the bus attached to the first camera found
std::cout << "IEEE1394 bus was reset." << std::endl;
vpImage<unsigned char> I;
- g.acquire(I);
+ g.acquire(I);
// std::cout << "write /tmp/test.pgm" << std::endl;
// vpImageIo::write(I, "/tmp/test.pgm");
- }
- catch (...) {
+ } catch (...) {
vpCERROR << "Failure: exit" << std::endl;
}
}
#else
-int
-main()
+int main()
{
vpTRACE("Ieee 1394 grabber capabilities are not available...\n"
- "You should install libdc1394-2 to use this binary.") ;
+ "You should install libdc1394-2 to use this binary.");
}
#endif
diff --git a/modules/sensor/test/framegrabber/testPylonGrabber.cpp b/modules/sensor/test/framegrabber/testPylonGrabber.cpp
new file mode 100644
index 0000000..bbf5155
--- /dev/null
+++ b/modules/sensor/test/framegrabber/testPylonGrabber.cpp
@@ -0,0 +1,139 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Basler cameras video capture using Pylon SDK.
+ *
+ * Authors:
+ * Wenfeng CAI
+ *
+ *****************************************************************************/
+
+/*!
+ \file testPylonGrabber.cpp
+
+ \brief Acquire images using Pylon library.
+*/
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+
+#include <iostream>
+#include <string>
+
+#if defined(VISP_HAVE_PYLON)
+
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpPylonFactory.h>
+/*!
+ \example testPylonGrabber.cpp
+*/
+int main()
+{
+ try {
+ std::cout << "Basler camera test with Pylon in progress..." << std::endl;
+
+ // Get the user name
+ std::string username;
+ vpIoTools::getUserName(username);
+ std::string outputpath = "/tmp/" + username;
+ vpIoTools::makeDirectory(outputpath);
+
+ // Creation of an empty image container
+ vpImage<unsigned char> I;
+
+ vpPylonFactory &factory = vpPylonFactory::instance();
+ // Creation of a framegrabber
+ vpPylonGrabber *g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE);
+ std::string guid;
+
+ // Get the number of cameras connected on the bus
+ unsigned int ncameras; // Number of cameras on the bus
+ ncameras = g->getNumCameras();
+ for (unsigned int i = 0; i < ncameras; i++) {
+ g->setCameraIndex(i);
+ guid = g->getCameraSerial(i);
+ std::cout << "Detected camera with serial: " << guid << std::endl;
+ }
+ // If more than one camera connected, use the first one
+ if (ncameras > 1) {
+ g->setCameraIndex(0);
+ guid = g->getCameraSerial(0);
+ std::cout << "Use camera with serial: " << guid << std::endl;
+ // to be sure that the setCamera() in the next line with
+ // guid as parameter works
+ g->setCameraIndex(0);
+ g->setCameraSerial(guid);
+ }
+ g->getCameraInfo(std::cout);
+
+ std::cout << "Frame rate: " << g->getFrameRate() << std::endl;
+ std::cout << "Gain: " << g->getGain() << std::endl;
+ std::cout << "Gamma: " << g->getGamma() << std::endl;
+ std::cout << "Exposure time (ms): " << g->getExposure() << std::endl;
+ float blackLevel = g->getBlackLevel();
+ std::cout << "Black level: " << blackLevel << std::endl;
+
+ 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::string filename = outputpath + "/imagetest1.pgm";
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename);
+
+ std::cout << "New connection..." << std::endl;
+ g->open(I);
+ g->close();
+
+ std::cout << "New connection..." << std::endl;
+ g->open(I);
+ g->close();
+ filename = outputpath + "/imagetest2.pgm";
+ std::cout << "Write image: " << filename << std::endl;
+ vpImageIo::write(I, filename);
+ } catch (const GenericException &e) {
+ vpCERROR << e.what() << std::endl;
+ } catch (const std::exception &e) {
+ vpCERROR << e.what() << std::endl;
+ } catch (...) {
+ vpCERROR << "Failure: exit" << std::endl;
+ }
+}
+#else
+int main()
+{
+ vpTRACE("Basler Pylon grabber capabilities are not available...\n"
+ "You should install pylon SDK to use this binary.");
+}
+
+#endif
diff --git a/modules/sensor/test/rgb-depth/testRealSense.cpp b/modules/sensor/test/rgb-depth/testRealSense.cpp
deleted file mode 100644
index 6525d5c..0000000
--- a/modules/sensor/test/rgb-depth/testRealSense.cpp
+++ /dev/null
@@ -1,574 +0,0 @@
-/****************************************************************************
- *
- * This file is part of the ViSP software.
- * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
- * See the file LICENSE.txt at the root directory of this source
- * distribution for additional information about the GNU GPL.
- *
- * For using ViSP with software that can not be combined with the GNU
- * GPL, please contact Inria about acquiring a ViSP Professional
- * Edition License.
- *
- * See http://visp.inria.fr for more information.
- *
- * This software was developed at:
- * Inria Rennes - Bretagne Atlantique
- * Campus Universitaire de Beaulieu
- * 35042 Rennes Cedex
- * France
- *
- * If you have questions regarding the use of this file, please contact
- * Inria at visp at inria.fr
- *
- * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
- * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
- *
- * Description:
- * Test Intel RealSense acquisition.
- *
- *****************************************************************************/
-
-/*!
- \example testRealSense.cpp
- Test SR300 Intel RealSense acquisition.
-*/
-
-#include <iostream>
-
-#include <visp3/sensor/vpRealSense.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpMutex.h>
-#include <visp3/core/vpThread.h>
-
-#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY)
-# include <librealsense/rs.h>
-
-// Using a thread to display the pointcloud with PCL produces a segfault on OSX
-#if( ! defined(__APPLE__) && ! defined(__MACH__) ) // Not OSX
-# if (defined(VISP_HAVE_PTHREAD) || defined(_WIN32)) // Threading available
-# define USE_THREAD
-# endif
-#endif
-
-#ifdef VISP_HAVE_PCL
-# include <pcl/visualization/cloud_viewer.h>
-# include <pcl/visualization/pcl_visualizer.h>
-#endif
-
-#ifdef VISP_HAVE_PCL
-#ifdef USE_THREAD
-// Shared vars
-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<pcl::PointXYZRGB>::Ptr pointcloud_ = *((pcl::PointCloud<pcl::PointXYZRGB>::Ptr *) args);
-
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_);
- viewer->setBackgroundColor (0, 0, 0);
- viewer->addCoordinateSystem (0.1);
- viewer->initCameraParameters ();
- viewer->setPosition(2*640+80, 480+80);
- viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
- viewer->setSize(640, 480);
-
- t_CaptureState capture_state_;
-
- do {
- s_mutex_capture.lock();
- capture_state_ = s_capture_state;
- s_mutex_capture.unlock();
-
- if (capture_state_ == capture_started) {
- static bool update = false;
- if (! update) {
- viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
- update = true;
- }
- else {
- viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud_, rgb, "sample cloud");
- }
-
- viewer->spinOnce (10);
- }
- } while(capture_state_ != capture_stopped);
-
- std::cout << "End of point cloud display thread" << std::endl;
- return 0;
-}
-#endif
-#endif
-
-#endif
-
-
-int main(
-#if defined(VISP_HAVE_PCL)
- int argc, char *argv[]
-#endif
- )
-{
-#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && ( defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) )
- try {
- vpRealSense rs;
-
- 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;
- return EXIT_SUCCESS;
- }
- rs.close();
-
-
- rs.setEnableStream(rs::stream::color, false);
- rs.setEnableStream(rs::stream::depth, true);
- rs.setEnableStream(rs::stream::infrared, false);
-
- rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 240, rs::format::z16, 110));
-
- rs.open();
- 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::endl;
- std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
-
- vpImage<uint16_t> depth((unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width);
- vpImage<vpRGBa> I_display_depth(depth.getHeight(), depth.getWidth());
-
- std::vector<double> time_vector;
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX dd(I_display_depth, 0, 0, "Depth image");
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI dd(I_display_depth, 0, 0, "Depth image");
-#endif
-
- //Test depth stream during 10 s
- double t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, NULL );
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
- vpDisplay::display(I_display_depth);
- vpDisplay::flush(I_display_depth);
-
- if (vpDisplay::getClick(I_display_depth, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_DEPTH_Z16_640x240_110FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
- << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
- dd.close(I_display_depth);
-
-
- 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.open();
-
- vpImage<vpRGBa> I_color((unsigned int) rs.getIntrinsics(rs::stream::color).height, (unsigned int) rs.getIntrinsics(rs::stream::color).width);
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX dc(I_color, 0, 0, "Color image");
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI dc(I_color, 0, 0, "Color image");
-#endif
-
- time_vector.clear();
- //Test color stream during 10 s
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( (unsigned char *) I_color.bitmap, NULL, NULL, NULL );
-
- vpDisplay::display(I_color);
- vpDisplay::flush(I_color);
-
- if (vpDisplay::getClick(I_color, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_COLOR_RGBA8_1920x1080_30FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
- << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
- dc.close(I_color);
-
-
- rs.setEnableStream(rs::stream::color, false);
- rs.setEnableStream(rs::stream::depth, false);
- rs.setEnableStream(rs::stream::infrared, true);
- rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 200));
- rs.open();
-
- vpImage<uint16_t> infrared((unsigned int) rs.getIntrinsics(rs::stream::infrared).height, (unsigned int) rs.getIntrinsics(rs::stream::infrared).width);
- vpImage<unsigned char> I_display_infrared(infrared.getHeight(), infrared.getWidth());
-
-#if defined(VISP_HAVE_X11)
- vpDisplayX di(I_display_infrared, 0, 0, "Infrared image");
-#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI di(I_display_infrared, 0, 0, "Infrared image");
-#endif
-
- time_vector.clear();
- //Test infrared stream during 10 s
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( NULL, NULL, NULL, (unsigned char *) infrared.bitmap );
- vpImageConvert::convert(infrared, I_display_infrared);
-
- vpDisplay::display(I_display_infrared);
- vpDisplay::flush(I_display_infrared);
-
- if (vpDisplay::getClick(I_display_infrared, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_INFRARED_Y16_640x480_200FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
- << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
- di.close(I_display_infrared);
-
-
- rs.setEnableStream(rs::stream::color, false);
- rs.setEnableStream(rs::stream::depth, true);
- rs.setEnableStream(rs::stream::infrared, false);
- rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
- rs.open();
-
- depth.resize( (unsigned int) rs.getIntrinsics(rs::stream::depth).height, (unsigned int) rs.getIntrinsics(rs::stream::depth).width );
- I_display_depth.resize( depth.getHeight(), depth.getWidth() );
-
-#if defined(VISP_HAVE_X11)
- dd.init(I_display_depth, 0, 0, "Depth image");
-#elif defined(VISP_HAVE_GDI)
- dd.init(I_display_depth, 0, 0, "Depth image");
-#endif
-
- time_vector.clear();
- //Test depth stream during 10 s
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, NULL );
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
- vpDisplay::display(I_display_depth);
- vpDisplay::flush(I_display_depth);
-
- if (vpDisplay::getClick(I_display_depth, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_DEPTH_Z16_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector) << " ms ; Median time: "
- << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
- dd.close(I_display_depth);
-
-
- rs.setEnableStream(rs::stream::color, false);
- rs.setEnableStream(rs::stream::depth, true);
- rs.setEnableStream(rs::stream::infrared, true);
- rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
- rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60));
- rs.open();
-
-#if defined(VISP_HAVE_X11)
- dd.init(I_display_depth, 0, 0, "Depth image");
- di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
-#elif defined(VISP_HAVE_GDI)
- dd.init(I_display_depth, 0, 0, "Depth image");
- di.init(I_display_infrared, I_display_depth.getWidth(), 0, "Infrared image");
-#endif
-
- time_vector.clear();
- //Test depth stream during 10 s
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( NULL, (unsigned char *) depth.bitmap, NULL, (unsigned char *) I_display_infrared.bitmap );
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
- vpDisplay::display(I_display_depth);
- vpDisplay::display(I_display_infrared);
- vpDisplay::flush(I_display_depth);
- vpDisplay::flush(I_display_infrared);
-
- if (vpDisplay::getClick(I_display_depth, false) || vpDisplay::getClick(I_display_infrared, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector)
- << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
-
- dd.close(I_display_depth);
- di.close(I_display_infrared);
-
-
-
-#ifdef VISP_HAVE_PCL
- pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-
-#ifdef USE_THREAD
- vpThread thread_display_pointcloud(displayPointcloudFunction, (vpThread::Args)&pointcloud);
-#else
- pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
- pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud);
- viewer->setBackgroundColor (0, 0, 0);
- viewer->addCoordinateSystem (1.0);
- viewer->initCameraParameters ();
- viewer->setPosition(2*640+80, 480+80);
- viewer->setCameraPosition(0,0,-0.5, 0,-1,0);
- viewer->setSize(640, 480);
-#endif
-
- rs.setEnableStream(rs::stream::color, true);
- rs.setEnableStream(rs::stream::depth, true);
- rs.setEnableStream(rs::stream::infrared, true);
- rs.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(640, 480, rs::format::rgba8, 60));
- rs.setStreamSettings(rs::stream::depth, vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60));
- rs.setStreamSettings(rs::stream::infrared, vpRealSense::vpRsStreamParams(640, 480, rs::format::y8, 60));
- rs.open();
-
- I_color.resize(480, 640);
-
-#if defined(VISP_HAVE_X11)
- dc.init(I_color, 0, 0, "Color image");
- dd.init(I_display_depth, 0, (int) I_color.getHeight()+80, "Depth image");
- di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
-#elif defined(VISP_HAVE_GDI)
- dc.init(I_color, 0, 0, "Color image");
- dd.init(I_display_depth, 0, (int) I_color.getHeight()+80, "Depth image");
- di.init(I_display_infrared, (int) I_display_depth.getWidth(), 0, "Infrared image");
-#endif
-
- //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;
- std::cout << "color_stream: " << color_stream << std::endl;
- //argv[2] <==> depth stream
- rs::stream depth_stream = argc > 2 ? (rs::stream) atoi(argv[2]) : rs::stream::depth;
- std::cout << "depth_stream: " << depth_stream << std::endl;
-
- time_vector.clear();
- //Test depth stream during 10 s
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, pointcloud, (unsigned char *) I_display_infrared.bitmap,
- NULL, color_stream, depth_stream);
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
-#ifdef VISP_HAVE_PCL
-#ifdef USE_THREAD
- {
- vpMutex::vpScopedLock lock(s_mutex_capture);
- s_capture_state = capture_started;
- }
-#else
- static bool update = false;
- if (! update) {
- viewer->addPointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
- viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
- viewer->setPosition( (int) I_color.getWidth()+80, (int) I_color.getHeight()+80) ;
- update = true;
- }
- else {
- viewer->updatePointCloud<pcl::PointXYZRGB> (pointcloud, rgb, "sample cloud");
- }
-
- viewer->spinOnce (10);
-#endif
-#endif
-
- vpDisplay::display(I_color);
- vpDisplay::display(I_display_depth);
- vpDisplay::display(I_display_infrared);
- vpDisplay::flush(I_color);
- vpDisplay::flush(I_display_depth);
- vpDisplay::flush(I_display_infrared);
-
- if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false) || vpDisplay::getClick(I_display_infrared, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
- std::cout << "SR300_COLOR_RGBA8_640x480_60FPS + SR300_DEPTH_Z16_640x480_60FPS + SR300_INFRARED_Y8_640x480_60FPS - Mean time: " << vpMath::getMean(time_vector)
- << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
-
-#ifdef VISP_HAVE_PCL
-#ifdef USE_THREAD
- {
- vpMutex::vpScopedLock lock(s_mutex_capture);
- s_capture_state = capture_stopped;
- }
-#endif
-#endif
-
- dc.close(I_color);
- dd.close(I_display_depth);
- di.close(I_display_infrared);
-
-#endif
-
-
- //Color stream aligned to depth
- rs.setEnableStream(rs::stream::color, true);
- rs.setEnableStream(rs::stream::depth, true);
- rs.setEnableStream(rs::stream::infrared, 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();
-
- dc.init(I_color, 0, 0, "Color aligned to depth");
- di.init(I_display_depth, (int) I_color.getWidth(), 0, "Depth image");
-
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color_aligned_to_depth );
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
- vpDisplay::display(I_color);
- vpDisplay::display(I_display_depth);
- vpDisplay::flush(I_color);
- vpDisplay::flush(I_display_depth);
-
- if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
-
- dc.close(I_color);
- dd.close(I_display_depth);
-
-
- //Depth stream aligned to color
- dc.init(I_color, 0, 0, "Color image");
- di.init(I_display_depth, (int) I_color.getWidth(), 0, "Depth aligned to color");
-
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( (unsigned char *) I_color.bitmap, (unsigned char *) depth.bitmap, NULL, NULL, NULL, rs::stream::color, rs::stream::depth_aligned_to_color );
- vpImageConvert::createDepthHistogram(depth, I_display_depth);
-
- vpDisplay::display(I_color);
- vpDisplay::display(I_display_depth);
- vpDisplay::flush(I_color);
- vpDisplay::flush(I_display_depth);
-
- if (vpDisplay::getClick(I_color, false) || vpDisplay::getClick(I_display_depth, false)) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
-
- dc.close(I_color);
- dd.close(I_display_depth);
-
-
-
-#if VISP_HAVE_OPENCV_VERSION >= 0x020409
- 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.open();
-
- cv::Mat color_mat(480, 640, CV_8UC3);
- cv::Mat infrared_mat(480, 640, CV_8U);
-
- t_begin = vpTime::measureTimeMs();
- while (true) {
- double t = vpTime::measureTimeMs();
- rs.acquire( color_mat.data, NULL, NULL, infrared_mat.data );
-
- cv::imshow("Color mat", color_mat);
- cv::imshow("Infrared mat", infrared_mat);
- char c = cv::waitKey(10);
- if (c == 27) {
- break;
- }
-
- time_vector.push_back(vpTime::measureTimeMs() - t);
- if (vpTime::measureTimeMs() - t_begin >= 10000) {
- break;
- }
- }
-#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;
- } catch(const std::exception & e) {
- std::cerr << e.what() << std::endl;
- }
-
-#elif !defined(VISP_HAVE_REALSENSE)
- std::cout << "Install RealSense SDK to make this test working. X11 or GDI are needed also." << std::endl;
-#elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
- std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) to make this test working" << std::endl;
-#endif
- return EXIT_SUCCESS;
-}
diff --git a/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp
new file mode 100644
index 0000000..037a2bd
--- /dev/null
+++ b/modules/sensor/test/rgb-depth/testRealSense2_SR300.cpp
@@ -0,0 +1,537 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test Intel RealSense acquisition with librealsense2.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRealSense2_SR300.cpp
+ Test Intel RealSense SR300 acquisition with librealsense2.
+*/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense2.h>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#include <thread>
+#endif
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+#include <opencv2/core.hpp>
+#include <opencv2/highgui.hpp>
+#endif
+
+namespace
+{
+#ifdef VISP_HAVE_PCL
+// Global variables
+pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+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) {}
+
+ void run()
+ {
+ std::string date = vpTime::getDateTime();
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date));
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setPosition(640 + 80, 480 + 80);
+ viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
+ viewer->setSize(640, 480);
+
+ bool init = true;
+ bool local_update = false, local_cancelled = false;
+ while (!local_cancelled) {
+ {
+ std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
+
+ if (lock.owns_lock()) {
+ local_update = update_pointcloud;
+ update_pointcloud = false;
+ local_cancelled = cancelled;
+
+ if (local_update) {
+ if (m_colorMode) {
+ local_pointcloud_color = pointcloud_color->makeShared();
+ } else {
+ local_pointcloud = pointcloud->makeShared();
+ }
+ }
+ }
+ }
+
+ if (local_update && !local_cancelled) {
+ local_update = false;
+
+ if (init) {
+ if (m_colorMode) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
+ "RGB sample cloud");
+ } else {
+ viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ }
+ init = false;
+ } else {
+ if (m_colorMode) {
+ viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ }
+ }
+ }
+
+ viewer->spinOnce(5);
+ }
+
+ std::cout << "End of point cloud display thread" << std::endl;
+ }
+
+private:
+ bool m_colorMode;
+ std::mutex &m_mutex;
+};
+
+void getPointcloud(const rs2::depth_frame &depth_frame, std::vector<vpColVector> &pointcloud)
+{
+ auto vf = depth_frame.as<rs2::video_frame>();
+ const int width = vf.get_width();
+ const int height = vf.get_height();
+ pointcloud.resize((size_t)(width * height));
+
+ rs2::pointcloud pc;
+ rs2::points points = pc.calculate(depth_frame);
+ auto vertices = points.get_vertices();
+ vpColVector v(4);
+ for (size_t i = 0; i < points.size(); i++) {
+ if (vertices[i].z) {
+ v[0] = vertices[i].x;
+ v[1] = vertices[i].y;
+ v[2] = vertices[i].z;
+ v[3] = 1.0;
+ } else {
+ v[0] = 0.0;
+ v[1] = 0.0;
+ v[2] = 0.0;
+ v[3] = 1.0;
+ }
+
+ pointcloud[i] = v;
+ }
+}
+#endif
+
+void getNativeFrame(const rs2::frame &frame, unsigned char *const data)
+{
+ auto vf = frame.as<rs2::video_frame>();
+ int size = vf.get_width() * vf.get_height();
+
+ switch (frame.get_profile().format()) {
+ case RS2_FORMAT_RGB8:
+ case RS2_FORMAT_BGR8:
+ memcpy(data, (void *)frame.get_data(), size * 3);
+ break;
+
+ case RS2_FORMAT_RGBA8:
+ case RS2_FORMAT_BGRA8:
+ memcpy(data, (void *)frame.get_data(), size * 4);
+ break;
+
+ case RS2_FORMAT_Y16:
+ case RS2_FORMAT_Z16:
+ memcpy(data, (unsigned char *)frame.get_data(), size * 2);
+ break;
+
+ case RS2_FORMAT_Y8:
+ memcpy(data, (unsigned char *)frame.get_data(), size);
+ break;
+
+ default:
+ break;
+ }
+}
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+void frame_to_mat(const rs2::frame &f, cv::Mat &img)
+{
+ auto vf = f.as<rs2::video_frame>();
+ const int w = vf.get_width();
+ const int h = vf.get_height();
+ const int size = w * h;
+
+ if (f.get_profile().format() == RS2_FORMAT_BGR8) {
+ memcpy(img.ptr<cv::Vec3b>(), f.get_data(), size * 3);
+ } else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
+ cv::Mat tmp(h, w, CV_8UC3, (void *)f.get_data(), cv::Mat::AUTO_STEP);
+ cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
+ } else if (f.get_profile().format() == RS2_FORMAT_Y8) {
+ memcpy(img.ptr<uchar>(), f.get_data(), size);
+ }
+}
+#endif
+}
+
+int main(int argc, char *argv[])
+{
+ bool pcl_color = false;
+ bool show_info = false;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--pcl_color") {
+ pcl_color = true;
+ } else if (std::string(argv[i]) == "--show_info") {
+ show_info = true;
+ }
+ }
+
+ if (show_info) {
+ vpRealSense2 rs;
+ rs.open();
+ std::cout << "RealSense:\n" << rs << std::endl;
+ return EXIT_SUCCESS;
+ }
+
+ vpRealSense2 rs;
+ rs2::config config;
+ config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_RGBA8, 30);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
+ config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y16, 30);
+ rs.open(config);
+
+ rs2::pipeline_profile &profile = rs.getPipelineProfile();
+ auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
+ vpImage<vpRGBa> color((unsigned int)color_profile.height(), (unsigned int)color_profile.width());
+
+ auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
+ vpImage<vpRGBa> depth_color((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width());
+ vpImage<uint16_t> depth_raw((unsigned int)depth_profile.height(), (unsigned int)depth_profile.width());
+
+ auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
+ vpImage<unsigned char> infrared((unsigned int)infrared_profile.height(), (unsigned int)infrared_profile.width());
+ vpImage<uint16_t> infrared_raw((unsigned int)infrared_profile.height(), (unsigned int)infrared_profile.width());
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d1, d2, d3;
+#else
+ vpDisplayGDI d1, d2, d3;
+#endif
+ d1.init(color, 0, 0, "Color");
+ d2.init(depth_color, color.getWidth(), 0, "Depth");
+ d3.init(infrared, 0, color.getHeight() + 100, "Infrared");
+
+ std::vector<vpColVector> pointcloud_colvector;
+#ifdef VISP_HAVE_PCL
+ std::mutex mutex;
+ ViewerWorker viewer_colvector(false, mutex);
+ std::thread viewer_colvector_thread(&ViewerWorker::run, &viewer_colvector);
+#endif
+
+ rs2::pipeline &pipe = rs.getPipeline();
+
+ std::cout << "Color intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Color intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
+
+ std::cout << "Depth intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Depth intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
+
+ std::cout << "Infrared intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Infrared intrinsics:\n"
+ << 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::vector<double> time_vector;
+ double t_begin = vpTime::measureTimeMs();
+ while (vpTime::measureTimeMs() - t_begin < 10000) {
+ double t = vpTime::measureTimeMs();
+
+ auto data = pipe.wait_for_frames();
+ auto color_frame = data.get_color_frame();
+ getNativeFrame(color_frame, (unsigned char *)color.bitmap);
+
+ auto depth_frame = data.get_depth_frame();
+ getNativeFrame(depth_frame, (unsigned char *)depth_raw.bitmap);
+
+ auto infrared_frame = data.first(RS2_STREAM_INFRARED);
+ getNativeFrame(infrared_frame, (unsigned char *)infrared_raw.bitmap);
+
+#ifdef VISP_HAVE_PCL
+ getPointcloud(depth_frame, pointcloud_colvector);
+
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+
+ pointcloud->width = depth_profile.width();
+ pointcloud->height = depth_profile.height();
+ pointcloud->points.resize(pointcloud_colvector.size());
+ for (size_t i = 0; i < pointcloud_colvector.size(); i++) {
+ pointcloud->points[(size_t)i].x = pointcloud_colvector[i][0];
+ pointcloud->points[(size_t)i].y = pointcloud_colvector[i][1];
+ pointcloud->points[(size_t)i].z = pointcloud_colvector[i][2];
+ }
+
+ update_pointcloud = true;
+ }
+#endif
+
+ vpImageConvert::createDepthHistogram(depth_raw, depth_color);
+ vpImageConvert::convert(infrared_raw, infrared);
+
+ vpDisplay::display(color);
+ vpDisplay::display(depth_color);
+ vpDisplay::display(infrared);
+
+ vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
+
+ vpDisplay::flush(color);
+ vpDisplay::flush(depth_color);
+ vpDisplay::flush(infrared);
+
+ time_vector.push_back(vpTime::measureTimeMs() - t);
+ if (vpDisplay::getClick(color, false))
+ break;
+ }
+
+ rs.close();
+ d1.close(color);
+ d2.close(depth_color);
+ d3.close(infrared);
+
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ cancelled = true;
+ }
+
+ viewer_colvector_thread.join();
+#endif
+ std::cout << "Acquisition1 - Mean time: " << vpMath::getMean(time_vector)
+ << " 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);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
+ config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
+ rs.open(config);
+
+ color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
+ color.init((unsigned int)color_profile.height(), (unsigned int)color_profile.width());
+
+ depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
+ 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<rs2::video_stream_profile>();
+ 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");
+ d3.init(infrared, 0, color.getHeight() + 100, "Infrared");
+
+#ifdef VISP_HAVE_PCL
+ cancelled = false;
+ ViewerWorker viewer(pcl_color, mutex);
+ std::thread viewer_thread(&ViewerWorker::run, &viewer);
+#endif
+
+ std::cout << "\n" << std::endl;
+ std::cout << "Color intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Color intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
+
+ std::cout << "Depth intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Depth intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_DEPTH, vpCameraParameters::perspectiveProjWithDistortion) << std::endl;
+
+ std::cout << "Infrared intrinsics:\n"
+ << rs.getCameraParameters(RS2_STREAM_INFRARED, vpCameraParameters::perspectiveProjWithoutDistortion)
+ << std::endl;
+ std::cout << "Infrared intrinsics:\n"
+ << 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;
+
+ time_vector.clear();
+ t_begin = vpTime::measureTimeMs();
+ while (vpTime::measureTimeMs() - t_begin < 10000) {
+ double t = vpTime::measureTimeMs();
+
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+
+ if (pcl_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,
+ (unsigned char *)infrared.bitmap);
+ }
+
+ update_pointcloud = true;
+ }
+#else
+ rs.acquire((unsigned char *)color.bitmap, (unsigned char *)depth_raw.bitmap, NULL,
+ (unsigned char *)infrared.bitmap);
+#endif
+
+ vpImageConvert::createDepthHistogram(depth_raw, depth_color);
+
+ vpDisplay::display(color);
+ vpDisplay::display(depth_color);
+ vpDisplay::display(infrared);
+
+ vpDisplay::displayText(color, 20, 20, "Click to quit.", vpColor::red);
+
+ vpDisplay::flush(color);
+ vpDisplay::flush(depth_color);
+ vpDisplay::flush(infrared);
+
+ time_vector.push_back(vpTime::measureTimeMs() - t);
+ if (vpDisplay::getClick(color, false))
+ break;
+ }
+
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ cancelled = true;
+ }
+
+ viewer_thread.join();
+#endif
+
+ d1.close(color);
+ 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;
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ rs.close();
+ config.disable_all_streams();
+ config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 60);
+ config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
+ config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
+ rs.open(config);
+
+ color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
+ cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
+
+ depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
+ 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<rs2::video_stream_profile>();
+ cv::Mat mat_infrared(infrared_profile.height(), infrared_profile.width(), CV_8U);
+
+ time_vector.clear();
+ t_begin = vpTime::measureTimeMs();
+ while (vpTime::measureTimeMs() - t_begin < 10000) {
+ double t = vpTime::measureTimeMs();
+
+ auto data = pipe.wait_for_frames();
+ frame_to_mat(data.get_color_frame(), mat_color);
+ frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
+ frame_to_mat(data.first(RS2_STREAM_INFRARED), mat_infrared);
+
+ cv::imshow("OpenCV color", mat_color);
+ cv::imshow("OpenCV depth", mat_depth);
+ cv::imshow("OpenCV infrared", mat_infrared);
+
+ time_vector.push_back(vpTime::measureTimeMs() - t);
+ if (cv::waitKey(10) == 27)
+ break;
+ }
+
+ std::cout << "Acquisition3 - Mean time: " << vpMath::getMean(time_vector)
+ << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
+#endif
+
+ return EXIT_SUCCESS;
+}
+
+#else
+int main()
+{
+#if !defined(VISP_HAVE_REALSENSE2)
+ std::cout << "Install librealsense2 to make this test work." << std::endl;
+#endif
+#if !defined(VISP_HAVE_CPP11_COMPATIBILITY)
+ std::cout << "Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON) "
+ "to make this test work"
+ << std::endl;
+#endif
+#if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
+ std::cout << "X11 or GDI are needed." << std::endl;
+#endif
+ return 0;
+}
+#endif
diff --git a/modules/sensor/test/rgb-depth/testRealSense_R200.cpp b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp
new file mode 100644
index 0000000..193cd3d
--- /dev/null
+++ b/modules/sensor/test/rgb-depth/testRealSense_R200.cpp
@@ -0,0 +1,580 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test Intel RealSense acquisition.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRealSense_R200.cpp
+ Test Intel RealSense R200 acquisition.
+*/
+
+#include <iostream>
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpRealSense.h>
+
+#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#include <mutex>
+#include <thread>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#endif
+
+namespace
+{
+
+#ifdef VISP_HAVE_PCL
+// Global variables
+pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+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) {}
+
+ void run()
+ {
+ std::string date = vpTime::getDateTime();
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date));
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setPosition(640 + 80, 480 + 80);
+ viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
+ viewer->setSize(640, 480);
+
+ bool init = true;
+ bool local_update = false, local_cancelled = false;
+ while (!local_cancelled) {
+ {
+ std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
+
+ if (lock.owns_lock()) {
+ local_update = update_pointcloud;
+ update_pointcloud = false;
+ local_cancelled = cancelled;
+
+ if (local_update) {
+ if (m_colorMode) {
+ local_pointcloud_color = pointcloud_color->makeShared();
+ } else {
+ local_pointcloud = pointcloud->makeShared();
+ }
+ }
+ }
+ }
+
+ if (local_update && !local_cancelled) {
+ local_update = false;
+
+ if (init) {
+ if (m_colorMode) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
+ "RGB sample cloud");
+ } else {
+ viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ }
+ init = false;
+ } else {
+ if (m_colorMode) {
+ viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ }
+ }
+ }
+
+ viewer->spinOnce(5);
+ }
+
+ std::cout << "End of point cloud display thread" << std::endl;
+ }
+
+private:
+ bool m_colorMode;
+ std::mutex &m_mutex;
+};
+#endif //#ifdef VISP_HAVE_PCL
+
+void test_R200(vpRealSense &rs, const std::map<rs::stream, bool> &enables,
+ const std::map<rs::stream, vpRealSense::vpRsStreamParams> ¶ms,
+ const std::map<rs::option, double> &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<rs::stream, bool>::const_iterator it_enable;
+ std::map<rs::stream, vpRealSense::vpRsStreamParams>::const_iterator it_param;
+
+ 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) {
+ it_param = params.find(it_enable->first);
+
+ if (it_param != params.end()) {
+ rs.setStreamSettings(it_param->first, it_param->second);
+ }
+ }
+ }
+
+ rs.open();
+
+ vpImage<uint16_t> depth;
+ vpImage<unsigned char> I_depth;
+ vpImage<vpRGBa> I_depth_color;
+
+ vpImage<vpRGBa> I_color;
+ vpImage<uint16_t> infrared, infrared2;
+ vpImage<unsigned char> I_infrared, I_infrared2;
+
+ bool direct_infrared_conversion = false;
+ for (it_enable = enables.begin(); it_enable != enables.end(); ++it_enable) {
+ if (it_enable->second) {
+ switch (it_enable->first) {
+ case rs::stream::color:
+ I_color.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ break;
+
+ case rs::stream::depth:
+ depth.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ I_depth.init(depth.getHeight(), depth.getWidth());
+ I_depth_color.init(depth.getHeight(), depth.getWidth());
+ break;
+
+ case rs::stream::infrared:
+ infrared.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ I_infrared.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;
+ }
+ break;
+
+ case rs::stream::infrared2:
+ 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;
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX dc, dd, di, di2;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI dc, dd, di, di2;
+#endif
+
+ for (it_enable = enables.begin(); it_enable != enables.end(); ++it_enable) {
+ if (it_enable->second) {
+ switch (it_enable->first) {
+ case rs::stream::color:
+ dc.init(I_color, 0, 0, "Color frame");
+ break;
+
+ case rs::stream::depth:
+ if (depth_color_visualization) {
+ 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");
+ 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");
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ std::cout << "direct_infrared_conversion=" << direct_infrared_conversion << std::endl;
+
+#ifdef VISP_HAVE_PCL
+ std::mutex mutex;
+ ViewerWorker viewer(pcl_color, mutex);
+ std::thread viewer_thread;
+
+ if (display_pcl) {
+ viewer_thread = std::thread(&ViewerWorker::run, &viewer);
+ }
+#else
+ display_pcl = false;
+#endif
+
+ rs::device *dev = rs.getHandler();
+
+ // Test stream acquisition during 10 s
+ std::vector<double> time_vector;
+ double t_begin = vpTime::measureTimeMs();
+ while (true) {
+ double t = vpTime::measureTimeMs();
+
+ for (std::map<rs::option, double>::const_iterator it = options.begin(); it != options.end(); ++it) {
+ dev->set_option(it->first, it->second);
+ }
+
+ if (display_pcl) {
+#ifdef VISP_HAVE_PCL
+ std::lock_guard<std::mutex> lock(mutex);
+
+ 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,
+ 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,
+ 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);
+ } 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);
+ }
+
+ vpImageConvert::convert(infrared, I_infrared);
+ vpImageConvert::convert(infrared2, I_infrared2);
+ }
+
+ update_pointcloud = true;
+#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);
+ } 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);
+ vpImageConvert::convert(infrared, I_infrared);
+ vpImageConvert::convert(infrared2, I_infrared2);
+ }
+ }
+
+ if (depth_color_visualization) {
+ vpImageConvert::createDepthHistogram(depth, I_depth_color);
+ } else {
+ vpImageConvert::createDepthHistogram(depth, I_depth);
+ }
+
+ vpDisplay::display(I_color);
+ if (depth_color_visualization) {
+ vpDisplay::display(I_depth_color);
+ } else {
+ vpDisplay::display(I_depth);
+ }
+ vpDisplay::display(I_infrared);
+ vpDisplay::display(I_infrared2);
+
+ vpDisplay::flush(I_color);
+ if (depth_color_visualization) {
+ vpDisplay::flush(I_depth_color);
+ } else {
+ vpDisplay::flush(I_depth);
+ }
+ vpDisplay::flush(I_infrared);
+ 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)) {
+ break;
+ }
+
+ time_vector.push_back(vpTime::measureTimeMs() - t);
+ if (vpTime::measureTimeMs() - t_begin >= 10000) {
+ break;
+ }
+ }
+
+ if (display_pcl) {
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ cancelled = true;
+ }
+
+ viewer_thread.join();
+#endif
+ }
+
+ std::cout << title << " - Mean time: " << vpMath::getMean(time_vector)
+ << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
+
+ rs.close();
+}
+
+} // namespace
+
+int main(int argc, char *argv[])
+{
+ try {
+ vpRealSense rs;
+
+ 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;
+ 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::endl;
+ std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
+
+ rs.close();
+
+ std::map<rs::stream, bool> enables;
+ std::map<rs::stream, vpRealSense::vpRsStreamParams> params;
+ std::map<rs::option, double> options;
+
+ enables[rs::stream::color] = false;
+ enables[rs::stream::depth] = true;
+ enables[rs::stream::infrared] = false;
+ enables[rs::stream::infrared2] = false;
+
+ 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);
+
+ 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);
+
+ options[rs::option::r200_lr_auto_exposure_enabled] = 0;
+ options[rs::option::r200_emitter_enabled] = 0;
+
+ test_R200(rs, enables, params, options,
+ "R200_DEPTH_Z16_640x480_90FPS + R200_INFRARED_Y8_640x480_90FPS "
+ "+ R200_INFRARED2_Y8_640x480_90FPS + "
+ "!r200_lr_auto_exposure_enabled + !r200_emitter_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(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;
+
+ test_R200(rs, enables, params, options,
+ "R200_DEPTH_Z16_628x468_90FPS + "
+ "R200_INFRARED_Y16_640x480_90FPS + "
+ "R200_INFRARED2_Y16_640x480_90FPS");
+
+ 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(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();
+
+ test_R200(rs, enables, params, options,
+ "R200_DEPTH_Z16_628x468_90FPS + R200_INFRARED_Y8_640x480_90FPS "
+ "+ R200_INFRARED2_Y8_640x480_90FPS");
+
+ 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, 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 "
+ "+ R200_INFRARED_Y8_640x480_90FPS + "
+ "R200_INFRARED2_Y8_640x480_90FPS",
+ true);
+
+ 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(1920, 1080, rs::format::rgba8, 30);
+
+ 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);
+ test_R200(rs, enables, params, options, "R200_COLOR_RGBA8_640x480_60FPS");
+
+ 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);
+
+ // 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;
+ 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;
+ 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;
+ std::cout << "infrared2_stream: " << infrared2_stream << std::endl;
+
+ test_R200(rs, enables, params, options,
+ "R200_COLOR_ALIGNED_TO_DEPTH_RGBA8_640x480_60FPS + "
+ "R200_DEPTH_ALIGNED_TO_RECTIFIED_COLOR_Z16_640x480_60FPS + "
+ "R200_INFRARED_Y8_640x480_60FPS + "
+ "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
+
+ 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);
+
+ // 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));
+#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;
+ } catch (const std::exception &e) {
+ std::cerr << e.what() << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+}
+
+#else
+int main()
+{
+#if !defined(VISP_HAVE_REALSENSE)
+ std::cout << "Install RealSense SDK to make this test working. X11 or GDI "
+ "are needed also."
+ << std::endl;
+#elif !defined(VISP_HAVE_CPP11_COMPATIBILITY)
+ std::cout << "Build ViSP with c++11 compiler flag (cmake -DUSE_CPP11=ON) "
+ "to make this test working"
+ << std::endl;
+#elif !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
+ std::cout << "X11 or GDI are needed!" << std::endl;
+#endif
+ return EXIT_SUCCESS;
+}
+#endif
diff --git a/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp
new file mode 100644
index 0000000..d9aead1
--- /dev/null
+++ b/modules/sensor/test/rgb-depth/testRealSense_SR300.cpp
@@ -0,0 +1,515 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test Intel RealSense acquisition.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testRealSense_SR300.cpp
+ Test Intel RealSense SR300 acquisition.
+*/
+
+#include <iostream>
+
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/sensor/vpRealSense.h>
+
+#if defined(VISP_HAVE_REALSENSE) && defined(VISP_HAVE_CPP11_COMPATIBILITY) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#include <mutex>
+#include <thread>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/visualization/cloud_viewer.h>
+#include <pcl/visualization/pcl_visualizer.h>
+#endif
+
+namespace
+{
+
+#ifdef VISP_HAVE_PCL
+// Global variables
+pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+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) {}
+
+ void run()
+ {
+ std::string date = vpTime::getDateTime();
+ pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer " + date));
+ pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
+ pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(new pcl::PointCloud<pcl::PointXYZRGB>());
+
+ viewer->setBackgroundColor(0, 0, 0);
+ viewer->initCameraParameters();
+ viewer->setPosition(640 + 80, 480 + 80);
+ viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
+ viewer->setSize(640, 480);
+
+ bool init = true;
+ bool local_update = false, local_cancelled = false;
+ while (!local_cancelled) {
+ {
+ std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
+
+ if (lock.owns_lock()) {
+ local_update = update_pointcloud;
+ update_pointcloud = false;
+ local_cancelled = cancelled;
+
+ if (local_update) {
+ if (m_colorMode) {
+ local_pointcloud_color = pointcloud_color->makeShared();
+ } else {
+ local_pointcloud = pointcloud->makeShared();
+ }
+ }
+ }
+ }
+
+ if (local_update && !local_cancelled) {
+ local_update = false;
+
+ if (init) {
+ if (m_colorMode) {
+ viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
+ "RGB sample cloud");
+ } else {
+ viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
+ }
+ init = false;
+ } else {
+ if (m_colorMode) {
+ viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb, "RGB sample cloud");
+ } else {
+ viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud, "sample cloud");
+ }
+ }
+ }
+
+ viewer->spinOnce(5);
+ }
+
+ std::cout << "End of point cloud display thread" << std::endl;
+ }
+
+private:
+ bool m_colorMode;
+ std::mutex &m_mutex;
+};
+#endif //#ifdef VISP_HAVE_PCL
+
+void test_SR300(vpRealSense &rs, const std::map<rs::stream, bool> &enables,
+ const std::map<rs::stream, vpRealSense::vpRsStreamParams> ¶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<rs::stream, bool>::const_iterator it_enable;
+ std::map<rs::stream, vpRealSense::vpRsStreamParams>::const_iterator it_param;
+
+ 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) {
+ it_param = params.find(it_enable->first);
+
+ if (it_param != params.end()) {
+ rs.setStreamSettings(it_param->first, it_param->second);
+ }
+ }
+ }
+
+ rs.open();
+
+ vpImage<uint16_t> depth;
+ vpImage<unsigned char> I_depth;
+ vpImage<vpRGBa> I_depth_color;
+
+ vpImage<vpRGBa> I_color;
+ vpImage<uint16_t> infrared;
+ vpImage<unsigned char> I_infrared;
+
+ bool direct_infrared_conversion = false;
+ for (it_enable = enables.begin(); it_enable != enables.end(); ++it_enable) {
+ if (it_enable->second) {
+ switch (it_enable->first) {
+ case rs::stream::color:
+ I_color.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ break;
+
+ case rs::stream::depth:
+ depth.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ I_depth.init(depth.getHeight(), depth.getWidth());
+ I_depth_color.init(depth.getHeight(), depth.getWidth());
+ break;
+
+ case rs::stream::infrared:
+ infrared.init((unsigned int)rs.getIntrinsics(it_enable->first).height,
+ (unsigned int)rs.getIntrinsics(it_enable->first).width);
+ I_infrared.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;
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX dc, dd, di;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI dc, dd, di;
+#endif
+
+ for (it_enable = enables.begin(); it_enable != enables.end(); ++it_enable) {
+ if (it_enable->second) {
+ switch (it_enable->first) {
+ case rs::stream::color:
+ dc.init(I_color, 0, 0, "Color frame");
+ break;
+
+ case rs::stream::depth:
+ if (depth_color_visualization) {
+ 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");
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+
+ if (rs.getHandler()->is_stream_enabled(rs::stream::infrared)) {
+ std::cout << "direct_infrared_conversion=" << direct_infrared_conversion << std::endl;
+ }
+
+#ifdef VISP_HAVE_PCL
+ std::mutex mutex;
+ ViewerWorker viewer(pcl_color, mutex);
+ std::thread viewer_thread;
+
+ if (display_pcl) {
+ viewer_thread = std::thread(&ViewerWorker::run, &viewer);
+ }
+#else
+ display_pcl = false;
+#endif
+
+ // Test stream acquisition during 10 s
+ std::vector<double> time_vector;
+ double t_begin = vpTime::measureTimeMs();
+ while (true) {
+ double t = vpTime::measureTimeMs();
+
+ if (display_pcl) {
+#ifdef VISP_HAVE_PCL
+ std::lock_guard<std::mutex> lock(mutex);
+
+ 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);
+ } else {
+ 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);
+ } else {
+ 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);
+ }
+
+ update_pointcloud = true;
+#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);
+ } else {
+ 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);
+ }
+ }
+
+ if (depth_color_visualization) {
+ vpImageConvert::createDepthHistogram(depth, I_depth_color);
+ } else {
+ vpImageConvert::createDepthHistogram(depth, I_depth);
+ }
+
+ vpDisplay::display(I_color);
+ if (depth_color_visualization) {
+ vpDisplay::display(I_depth_color);
+ } else {
+ vpDisplay::display(I_depth);
+ }
+ vpDisplay::display(I_infrared);
+
+ vpDisplay::flush(I_color);
+ if (depth_color_visualization) {
+ vpDisplay::flush(I_depth_color);
+ } else {
+ vpDisplay::flush(I_depth);
+ }
+ vpDisplay::flush(I_infrared);
+
+ 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)) {
+ break;
+ }
+
+ time_vector.push_back(vpTime::measureTimeMs() - t);
+ if (vpTime::measureTimeMs() - t_begin >= 10000) {
+ break;
+ }
+ }
+
+ if (display_pcl) {
+#ifdef VISP_HAVE_PCL
+ {
+ std::lock_guard<std::mutex> lock(mutex);
+ cancelled = true;
+ }
+
+ viewer_thread.join();
+#endif
+ }
+
+ std::cout << title << " - Mean time: " << vpMath::getMean(time_vector)
+ << " ms ; Median time: " << vpMath::getMedian(time_vector) << " ms" << std::endl;
+
+ rs.close();
+}
+
+} // namespace
+
+int main(int argc, char *argv[])
+{
+ try {
+ vpRealSense rs;
+
+ 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;
+ 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::endl;
+ std::cout << "RealSense sensor characteristics: \n" << rs << std::endl;
+
+ rs.close();
+
+ std::map<rs::stream, bool> enables;
+ std::map<rs::stream, vpRealSense::vpRsStreamParams> params;
+
+ enables[rs::stream::color] = false;
+ enables[rs::stream::depth] = true;
+ enables[rs::stream::infrared] = false;
+
+ params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 240, rs::format::z16, 110);
+
+ test_SR300(rs, enables, params, "SR300_DEPTH_Z16_640x240_110FPS");
+
+ enables[rs::stream::color] = true;
+ enables[rs::stream::depth] = false;
+ enables[rs::stream::infrared] = false;
+
+ params[rs::stream::color] = vpRealSense::vpRsStreamParams(1920, 1080, rs::format::rgba8, 30);
+
+ test_SR300(rs, enables, params, "SR300_COLOR_RGBA8_1920x1080_30FPS");
+
+ enables[rs::stream::color] = false;
+ enables[rs::stream::depth] = false;
+ enables[rs::stream::infrared] = true;
+
+ params[rs::stream::infrared] = vpRealSense::vpRsStreamParams(640, 480, rs::format::y16, 200);
+
+ test_SR300(rs, enables, params, "SR300_INFRARED_Y16_640x480_200FPS");
+
+ enables[rs::stream::color] = false;
+ enables[rs::stream::depth] = true;
+ enables[rs::stream::infrared] = false;
+
+ params[rs::stream::depth] = vpRealSense::vpRsStreamParams(640, 480, rs::format::z16, 60);
+
+ test_SR300(rs, enables, params, "SR300_DEPTH_Z16_640x480_60FPS", true);
+
+ enables[rs::stream::color] = false;
+ 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);
+
+ 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
+
+ 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);
+
+ // 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;
+ 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;
+ std::cout << "depth_stream: " << depth_stream << std::endl;
+
+ test_SR300(rs, enables, params,
+ "SR300_COLOR_RGBA8_640x480_60FPS + "
+ "SR300_DEPTH_Z16_640x480_60FPS + "
+ "SR300_INFRARED_Y8_640x480_60FPS",
+ true, color_stream, depth_stream, true, true);
+#endif
+
+ // Color stream aligned to depth
+ enables[rs::stream::color] = true;
+ 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);
+
+ test_SR300(rs, enables, params,
+ "SR300_COLOR_ALIGNED_TO_DEPTH_RGBA8_640x480_30FPS + "
+ "SR300_DEPTH_Z16_640x480_30FPS",
+ true, rs::stream::color_aligned_to_depth);
+
+ // Depth stream aligned to color
+ test_SR300(rs, enables, params,
+ "SR300_COLOR_RGBA8_640x480_30FPS + "
+ "SR300_DEPTH_ALIGNED_TO_COLOR_Z16_640x480_30FPS",
+ true, rs::stream::color, rs::stream::depth_aligned_to_color);
+
+#if VISP_HAVE_OPENCV_VERSION >= 0x020409
+ 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.open();
+
+ cv::Mat color_mat(480, 640, CV_8UC3);
+ cv::Mat infrared_mat(480, 640, CV_8U);
+
+ double t_begin = vpTime::measureTimeMs();
+ while (true) {
+ rs.acquire(color_mat.data, NULL, NULL, infrared_mat.data);
+
+ cv::imshow("Color mat", color_mat);
+ cv::imshow("Infrared mat", infrared_mat);
+ char c = cv::waitKey(1);
+ if (c == 27) {
+ break;
+ }
+
+ if (vpTime::measureTimeMs() - t_begin >= 10000) {
+ break;
+ }
+ }
+#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;
+ } catch (const std::exception &e) {
+ std::cerr << e.what() << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+}
+
+#else
+int main()
+{
+#if !defined(VISP_HAVE_REALSENSE)
+ std::cout << "Install librealsense to make this test work." << std::endl;
+#endif
+#if !defined(VISP_HAVE_CPP11_COMPATIBILITY)
+ std::cout << "Build ViSP with C++11 compiler flag (cmake -DUSE_CPP11=ON) "
+ "to make this test work."
+ << std::endl;
+#endif
+#if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
+ std::cout << "X11 or GDI are needed." << std::endl;
+#endif
+ return EXIT_SUCCESS;
+}
+#endif
diff --git a/modules/tracker/blob/CMakeLists.txt b/modules/tracker/blob/CMakeLists.txt
index 52074f3..6c351a0 100644
--- a/modules/tracker/blob/CMakeLists.txt
+++ b/modules/tracker/blob/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/tracker/blob/include/visp3/blob/vpDot.h b/modules/tracker/blob/include/visp3/blob/vpDot.h
index dbb12cc..ff37806 100644
--- a/modules/tracker/blob/include/visp3/blob/vpDot.h
+++ b/modules/tracker/blob/include/visp3/blob/vpDot.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,20 +46,20 @@
#define vpDot_hh
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/core/vpTracker.h>
-#include <visp3/core/vpRect.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpTracker.h>
-#include <math.h>
#include <fstream>
#include <list>
+#include <math.h>
#include <vector>
#ifdef VISP_USE_MSVC
-# pragma comment(linker, "/STACK:256000000") // Increase max recursion depth
+#pragma comment(linker, "/STACK:256000000") // Increase max recursion depth
#endif
/*!
@@ -77,11 +78,11 @@
track a blob and display the tracking results.
\code
-#include <visp3/core/vpConfig.h>
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/blob/vpDot.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
@@ -115,90 +116,89 @@ int main()
*/
class VISP_EXPORT vpDot : public vpTracker
{
-public :
+public:
/*! \enum vpConnexityType
Type of connexity 4, or 8.
*/
typedef enum {
CONNEXITY_4, /*!< For a given pixel 4 neighbors are considered (left,
- right, up, down) */
- CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
- right, up, down, and the 4 pixels located on the diagonal) */
+ right, up, down) */
+ CONNEXITY_8 /*!< For a given pixel 8 neighbors are considered (left,
+ right, up, down, and the 4 pixels located on the diagonal) */
} vpConnexityType;
static const unsigned int SPIRAL_SEARCH_SIZE; /*!< Spiral size for the dot
- search. */
- double m00; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
- with \f$i = j = 0 \f$.
-
- \sa setComputeMoments()
- */
- double m01; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
- obtained with \f$i = 0 \f$ and \f$ j = 1 \f$.
-
- \sa setComputeMoments()
- */
- double m10; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 0 \f$.
-
- \sa setComputeMoments()
- */
- double m11; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double m20; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
- obtained with \f$i = 2 \f$ and \f$ j = 0 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double m02; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
- obtained with \f$i = 0 \f$ and \f$ j = 2 \f$.
-
- \warning This moment is not computed for the moment.
-
- \sa setComputeMoments()
- */
- double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
- \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
- double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
- \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
-
- \sa setComputeMoments()
- */
- double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
- \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
-
- vpDot() ;
- vpDot(const vpImagePoint &ip) ;
- vpDot(const vpDot& d) ;
- virtual ~vpDot() ;
-
- void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
- unsigned int thickness=1) const;
+ search. */
+ double m00; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
+ with \f$i = j = 0 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m01; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 1 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m10; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 0 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m11; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double m20; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
+ obtained with \f$i = 2 \f$ and \f$ j = 0 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double m02; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 2 \f$.
+
+ \warning This moment is not computed for the moment.
+
+ \sa setComputeMoments()
+ */
+ double mu11; /*!< \f$ \mu_{11} \f$ is a second order central moments defined
+ by: \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu20; /*!< \f$ \mu_{20} \f$ is a second order central moments defined
+ by: \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu02; /*!< \f$ \mu_{02} \f$ is a second order central moments defined
+ by: \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+
+ vpDot();
+ explicit vpDot(const vpImagePoint &ip);
+ vpDot(const vpDot &d);
+ virtual ~vpDot();
+
+ void display(const vpImage<unsigned char> &I, vpColor color = vpColor::red, unsigned int thickness = 1) const;
/*!
@@ -207,13 +207,11 @@ public :
\sa getWidth(), getHeight()
*/
- inline vpRect getBBox() const {
+ inline vpRect getBBox() const
+ {
vpRect bbox;
- bbox.setRect(this->u_min,
- this->v_min,
- this->u_max - this->u_min + 1,
- this->v_max - this->v_min + 1);
+ bbox.setRect(this->u_min, this->v_min, this->u_max - this->u_min + 1, this->v_max - this->v_min + 1);
return (bbox);
};
@@ -222,18 +220,15 @@ public :
\return The coordinates of the center of gravity.
*/
- inline vpImagePoint getCog() const {
- return cog;
- }
+ inline vpImagePoint getCog() const { return cog; }
/*!
Return the list of all the image points on the border of the dot.
- \warning Doesn't return the image points inside the dot anymore. To get those points see getConnexities().
+ \warning Doesn't return the image points inside the dot anymore. To get
+ those points see getConnexities().
*/
- inline std::list<vpImagePoint> getEdges() const {
- return this->ip_edges_list;
- };
+ inline std::list<vpImagePoint> getEdges() const { return this->ip_edges_list; };
/*!
@@ -243,11 +238,9 @@ public :
This list is updated after a call to track().
*/
- inline std::list<vpImagePoint> getConnexities() const {
- return this->ip_connexities_list;
- };
+ inline std::list<vpImagePoint> getConnexities() const { return this->ip_connexities_list; };
- inline double getGamma() const {return this->gamma;};
+ inline double getGamma() const { return this->gamma; };
/*!
Return the precision of the gray level of the dot. It is a double
@@ -255,23 +248,17 @@ public :
values close to 0 show a very bad precision.
*/
- double getGrayLevelPrecision() const {return grayLevelPrecision;}
- double getMaxDotSize() const {
- return this->maxDotSizePercentage;
- }
+ double getGrayLevelPrecision() const { return grayLevelPrecision; }
+ double getMaxDotSize() const { return this->maxDotSizePercentage; }
/*!
Return the mean gray level value of the dot.
*/
- double getMeanGrayLevel() const {
- return (this->mean_gray_level);
- };
+ double getMeanGrayLevel() const { return (this->mean_gray_level); };
/*!
\return a vpPolygon made from the edges of the dot.
*/
- vpPolygon getPolygon() const {
- return (vpPolygon(ip_edges_list));
- };
+ vpPolygon getPolygon() const { return (vpPolygon(ip_edges_list)); };
/*!
@@ -280,9 +267,7 @@ public :
\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); };
/*!
@@ -291,35 +276,31 @@ public :
\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<unsigned char> &I) ;
+ void initTracking(const vpImage<unsigned char> &I);
void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip);
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip,
- unsigned int gray_level_min, unsigned int gray_level_max);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int gray_level_min,
+ unsigned int gray_level_max);
- vpDot& operator =(const vpDot& d) ;
- bool operator ==(const vpDot& d) const;
- bool operator !=(const vpDot& d) const;
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d);
+ vpDot &operator=(const vpDot &d);
+ bool operator==(const vpDot &d) const;
+ bool operator!=(const vpDot &d) const;
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDot &d);
- void print(std::ostream& os) { os << *this << std::endl ; }
+ void print(std::ostream &os) { os << *this << std::endl; }
/*!
Initialize the dot coordinates with \e ip.
*/
- inline void setCog(const vpImagePoint &ip) {
- this->cog = ip;
- }
+ inline void setCog(const vpImagePoint &ip) { this->cog = ip; }
/*!
Activates the dot's moments computation.
- \param activate true, if you want to compute the moments. If false, moments
- are not computed.
+ \param activate true, if you want to compute the moments. If false,
+ moments are not computed.
Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
vpDot::m20, vpDot::m02.
@@ -329,19 +310,15 @@ public :
*/
void setComputeMoments(const bool activate) { compute_moment = activate; }
-
+
/*!
Set the type of connexity: 4 or 8.
*/
- void setConnexity(vpConnexityType type) {this->connexityType = type; };
- void setMaxDotSize(double percentage) ;
- void setGrayLevelMin( const unsigned int &level_min ) {
- this->gray_level_min = level_min;
- };
- void setGrayLevelMax( const unsigned int &level_max ) {
- this->gray_level_max = level_max;
- };
- void setGrayLevelPrecision( const double & grayLevelPrecision );
+ void setConnexity(vpConnexityType type) { this->connexityType = type; };
+ void setMaxDotSize(double percentage);
+ void setGrayLevelMin(const unsigned int &level_min) { this->gray_level_min = level_min; };
+ void setGrayLevelMax(const unsigned int &level_max) { this->gray_level_max = level_max; };
+ void setGrayLevelPrecision(const double &grayLevelPrecision);
/*!
Activates the display of all the pixels of the dot during the tracking.
@@ -356,26 +333,27 @@ public :
\sa setGraphicsThickness()
*/
- void setGraphics(const bool activate) { graphics = activate ; }
+ void setGraphics(const bool activate) { graphics = activate; }
/*!
- Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
+ Modify the default thickness that is set to 1 of the drawings in overlay
+ when setGraphics() is enabled.
\sa setGraphics()
*/
- void setGraphicsThickness(unsigned int t) {this->thickness = t;};
+ void setGraphicsThickness(unsigned int t) { this->thickness = t; };
- void track(const vpImage<unsigned char> & I) ;
- void track(const vpImage<unsigned char> & I, vpImagePoint &ip) ;
+ void track(const vpImage<unsigned char> &I);
+ void track(const vpImage<unsigned char> &I, vpImagePoint &ip);
private:
//! internal use only
std::list<vpImagePoint> ip_connexities_list;
-
+
//! List of border points
std::list<vpImagePoint> ip_edges_list;
/*! Type of connexity
-
+
\warning In previous version this variable was called connexity
*/
vpConnexityType connexityType;
@@ -387,43 +365,40 @@ private:
unsigned int u_min, u_max, v_min, v_max;
// Flag used to allow display
- bool graphics ;
+ bool graphics;
unsigned int thickness; // Graphics thickness
double maxDotSizePercentage;
unsigned char gray_level_out;
-
- double mean_gray_level; // Mean gray level of the dot
+
+ double mean_gray_level; // Mean gray level of the dot
unsigned int gray_level_min; // left threshold for binarisation
unsigned int gray_level_max; // right threshold for binarisation
- double grayLevelPrecision; //precision of the gray level of the dot.
- //It is a double precision float witch value is in ]0,1].
- //1 means full precision, whereas values close to 0 show a very bad precision
- double gamma ;
+ double grayLevelPrecision; // precision of the gray level of the dot.
+ // It is a double precision float witch value is in ]0,1].
+ // 1 means full precision, whereas values close to 0 show a very bad
+ // precision
+ double gamma;
//! flag : true moment are computed
- bool compute_moment ;
+ bool compute_moment;
double nbMaxPoint;
-
- void init() ;
+
+ void init();
void setGrayLevelOut();
- bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n);
- bool connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab);
- void COG(const vpImage<unsigned char> &I,double& u, double& v) ;
-
-//Static Functions
+ bool connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
+ double &v_cog, double &n);
+ bool connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
+ double &v_cog, double &n, std::vector<bool> &checkTab);
+ void COG(const vpImage<unsigned char> &I, double &u, double &v);
+
+ // Static Functions
public:
- static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ static void display(const vpImage<unsigned char> &I, const vpImagePoint &cog,
const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
-} ;
+ unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
+ vpColor color = vpColor::red, unsigned int thickness = 1);
+};
#endif
-
-
-
diff --git a/modules/tracker/blob/include/visp3/blob/vpDot2.h b/modules/tracker/blob/include/visp3/blob/vpDot2.h
index f97b12f..24b46db 100644
--- a/modules/tracker/blob/include/visp3/blob/vpDot2.h
+++ b/modules/tracker/blob/include/visp3/blob/vpDot2.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*
\file vpDot2.h
\brief This tracker is meant to track some zones on a vpImage.
@@ -44,15 +44,15 @@
#ifndef vpDot2_hh
#define vpDot2_hh
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpRect.h>
-#include <visp3/core/vpTracker.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpRect.h>
+#include <visp3/core/vpTracker.h>
-#include <vector>
#include <list>
+#include <vector>
/*!
\class vpDot2
@@ -80,12 +80,13 @@
Along the inner ellipse the sampled points should have gray levels
that are in the gray level minimum and maximum bounds, while
on the outside ellipse, the gray levels should be out of the gray level
- bounds. To set the percentage of the sample points which should have the right
- levels use setEllipsoidBadPointsPercentage(). The distance between the
+ bounds. To set the percentage of the sample points which should have the
+ right levels use setEllipsoidBadPointsPercentage(). The distance between the
inner ellpsoid and the blob contour, as well the distance between the
- blob contour and the outside ellipse is fixed by setEllipsoidShapePrecision().
- If you want to track a non ellipsoid shape, and turn off this validation test,
- you have to call setEllipsoidShapePrecision(0).
+ blob contour and the outside ellipse is fixed by
+ setEllipsoidShapePrecision(). If you want to track a non ellipsoid shape,
+ and turn off this validation test, you have to call
+ setEllipsoidShapePrecision(0).
- The width, height and surface of the blob are compared to the
corresponding values of the previous blob. If they differ to much
the blob could be rejected. To set the admissible distance you can
@@ -103,18 +104,18 @@
is used when there was a problem performing basic tracking of the dot, but
can also be used to find a certain type of dots in the full image.
- The following sample code available in tutorial-blob-tracker-live-firewire.cpp shows how to
- grab images from a firewire camera, track a blob and display the tracking
- results.
+ The following sample code available in
+ tutorial-blob-tracker-live-firewire.cpp shows how to grab images from a
+ firewire camera, track a blob and display the tracking results.
\include tutorial-blob-tracker-live-firewire.cpp
A line by line explanation of the previous example is provided in
\ref tutorial-tracking-blob.
This other example available in tutorial-blob-auto-tracker.cpp shows firstly
- how to detect in the first image all the blobs that match some characteristics
- in terms of size, area, gray level. Secondly, it shows how to track all the
- dots that are detected.
+ how to detect in the first image all the blobs that match some
+ characteristics in terms of size, area, gray level. Secondly, it shows how
+ to track all the dots that are detected.
\include tutorial-blob-auto-tracker.cpp
A line by line explanation of this last example is also provided in
@@ -126,14 +127,14 @@ class VISP_EXPORT vpDot2 : public vpTracker
{
public:
vpDot2();
- vpDot2(const vpImagePoint &ip) ;
- vpDot2(const vpDot2& twinDot );
+ explicit vpDot2(const vpImagePoint &ip);
+ vpDot2(const vpDot2 &twinDot);
virtual ~vpDot2();
- static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col = vpColor::blue, bool trackDot = true);
+ static vpMatrix defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I,
+ vpColor col = vpColor::blue, bool trackDot = true);
- void display(const vpImage<unsigned char>& I, vpColor color = vpColor::red,
- unsigned int thickness=1) const;
+ void display(const vpImage<unsigned char> &I, vpColor color = vpColor::red, unsigned int thickness = 1) const;
double getArea() const;
/*!
@@ -143,13 +144,12 @@ public:
\sa getWidth(), getHeight()
*/
- inline vpRect getBBox() const {
+ inline vpRect getBBox() const
+ {
vpRect bbox;
- bbox.setRect(this->bbox_u_min,
- this->bbox_v_min,
- this->bbox_u_max - this->bbox_u_min + 1,
- this->bbox_v_max - this->bbox_v_min + 1);
+ 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);
};
@@ -158,11 +158,9 @@ public:
\return The coordinates of the center of gravity.
*/
- inline vpImagePoint getCog() const {
- return cog;
- }
+ inline vpImagePoint getCog() const { return cog; }
- double getDistance( const vpDot2& distantDot ) const;
+ double getDistance(const vpDot2 &distantDot) const;
/*!
Return the list of all the image points on the dot
@@ -172,9 +170,7 @@ public:
border. This list is update after a call to track().
*/
- void getEdges(std::list<vpImagePoint> &edges_list) const {
- edges_list = this->ip_edges_list;
- };
+ void getEdges(std::list<vpImagePoint> &edges_list) const { edges_list = this->ip_edges_list; };
/*!
Return the list of all the image points on the dot
@@ -184,40 +180,31 @@ public:
border. This list is update after a call to track().
*/
- std::list<vpImagePoint> getEdges() const {
- return(this->ip_edges_list);
- };
+ std::list<vpImagePoint> getEdges() const { return (this->ip_edges_list); };
/*!
Get the percentage of sampled points that are considered non conform
in terms of the gray level on the inner and the ouside ellipses.
\sa setEllipsoidBadPointsPercentage()
*/
- double getEllipsoidBadPointsPercentage() const
- {
- return allowedBadPointsPercentage_;
- }
+ double getEllipsoidBadPointsPercentage() const { return allowedBadPointsPercentage_; }
double getEllipsoidShapePrecision() const;
void getFreemanChain(std::list<unsigned int> &freeman_chain) const;
- inline double getGamma() const {return this->gamma;};
+ inline double getGamma() const { return this->gamma; };
/*!
Return the color level of pixels inside the dot.
\sa getGrayLevelMax()
*/
- inline unsigned int getGrayLevelMin() const {
- return gray_level_min;
- };
+ inline unsigned int getGrayLevelMin() const { return gray_level_min; };
/*!
Return the color level of pixels inside the dot.
\sa getGrayLevelMin()
*/
- inline unsigned int getGrayLevelMax() const {
- return gray_level_max;
- };
+ inline unsigned int getGrayLevelMax() const { return gray_level_max; };
double getGrayLevelPrecision() const;
double getHeight() const;
@@ -225,48 +212,39 @@ public:
/*!
\return The mean gray level value of the dot.
*/
- double getMeanGrayLevel() const {
- return (this->mean_gray_level);
- };
+ double getMeanGrayLevel() const { return (this->mean_gray_level); };
/*!
\return a vpPolygon made from the edges of the dot.
*/
- vpPolygon getPolygon() const {
- return (vpPolygon(ip_edges_list));
- };
+ vpPolygon getPolygon() const { return (vpPolygon(ip_edges_list)); };
double getSizePrecision() const;
double getWidth() const;
- void initTracking(const vpImage<unsigned char>& I, unsigned int size = 0);
- void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int size = 0);
- void initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int gray_lvl_min, unsigned int gray_lvl_max,
- unsigned int size = 0 );
+ void initTracking(const vpImage<unsigned char> &I, unsigned int size = 0);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size = 0);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int gray_lvl_min,
+ unsigned int gray_lvl_max, unsigned int size = 0);
- vpDot2& operator=(const vpDot2& twinDot );
- friend VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d);
+ vpDot2 &operator=(const vpDot2 &twinDot);
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpDot2 &d);
- void print(std::ostream& os) { os << *this << std::endl ; }
- void searchDotsInArea(const vpImage<unsigned char>& I,
- int area_u, int area_v,
- unsigned int area_w, unsigned int area_h, std::list<vpDot2> &niceDots );
+ void print(std::ostream &os) { os << *this << std::endl; }
+ void searchDotsInArea(const vpImage<unsigned char> &I, int area_u, int area_v, unsigned int area_w,
+ unsigned int area_h, std::list<vpDot2> &niceDots);
- void searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots );
+ void searchDotsInArea(const vpImage<unsigned char> &I, std::list<vpDot2> &niceDots);
- void setArea( const double & area );
+ void setArea(const double &area);
/*!
Initialize the dot coordinates with \e ip.
*/
- inline void setCog(const vpImagePoint &ip) {
- this->cog = ip;
- }
+ inline void setCog(const vpImagePoint &ip) { this->cog = ip; }
/*!
Activates the dot's moments computation.
- \param activate true, if you want to compute the moments. If false, moments
- are not computed.
+ \param activate true, if you want to compute the moments. If false,
+ moments are not computed.
Computed moment are vpDot::m00, vpDot::m10, vpDot::m01, vpDot::m11,
vpDot::m20, vpDot::m02.
@@ -289,7 +267,7 @@ public:
that all the points should have a right level, while a value of 1
means that all the points can have a bad gray level.
*/
- void setEllipsoidBadPointsPercentage(const double &percentage=0.0)
+ void setEllipsoidBadPointsPercentage(const double &percentage = 0.0)
{
if (percentage < 0.)
allowedBadPointsPercentage_ = 0.;
@@ -299,7 +277,7 @@ public:
allowedBadPointsPercentage_ = percentage;
}
- void setEllipsoidShapePrecision(const double & ellipsoidShapePrecision);
+ void setEllipsoidShapePrecision(const double &ellipsoidShapePrecision);
/*!
Activates the display of the border of the dot during the tracking.
The default thickness of the overlayed drawings can be modified using
@@ -313,26 +291,28 @@ public:
\sa setGraphicsThickness()
*/
- void setGraphics(const bool activate) { graphics = activate ; }
+ void setGraphics(const bool activate) { graphics = activate; }
/*!
- Modify the default thickness that is set to 1 of the drawings in overlay when setGraphics() is enabled.
+ Modify the default thickness that is set to 1 of the drawings in overlay
+ when setGraphics() is enabled.
\sa setGraphics()
*/
- void setGraphicsThickness(unsigned int t) {this->thickness = t;};
+ void setGraphicsThickness(unsigned int t) { this->thickness = t; };
/*!
- Set the color level of the dot to search a dot in a region of interest. This level will be
- used to know if a pixel in the image belongs to the dot or not. Only pixels
- with higher level can belong to the dot. If the level is lower than the
- minimum level for a dot, set the level to MIN_IN_LEVEL.
+ Set the color level of the dot to search a dot in a region of interest. This
+ level will be used to know if a pixel in the image belongs to the dot or
+ not. Only pixels with higher level can belong to the dot. If the level is
+ lower than the minimum level for a dot, set the level to MIN_IN_LEVEL.
\param min : Color level of a dot to search in a region of interest.
\sa setGrayLevelMax(), setGrayLevelPrecision()
*/
- inline void setGrayLevelMin( const unsigned int & min ) {
+ inline void setGrayLevelMin(const unsigned int &min)
+ {
if (min > 255)
this->gray_level_min = 255;
else
@@ -348,114 +328,106 @@ public:
\sa setGrayLevelMin(), setGrayLevelPrecision()
*/
- inline void setGrayLevelMax( const unsigned int & max ) {
+ inline void setGrayLevelMax(const unsigned int &max)
+ {
if (max > 255)
this->gray_level_max = 255;
else
this->gray_level_max = max;
};
- void setGrayLevelPrecision( const double & grayLevelPrecision );
- void setHeight( const double & height );
- void setMaxSizeSearchDistancePrecision(const double & maxSizeSearchDistancePrecision);
- void setSizePrecision( const double & sizePrecision );
- void setWidth( const double & width );
+ void setGrayLevelPrecision(const double &grayLevelPrecision);
+ void setHeight(const double &height);
+ void setMaxSizeSearchDistancePrecision(const double &maxSizeSearchDistancePrecision);
+ void setSizePrecision(const double &sizePrecision);
+ void setWidth(const double &width);
void track(const vpImage<unsigned char> &I);
void track(const vpImage<unsigned char> &I, vpImagePoint &cog);
static void trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
- std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar = NULL);
+ std::vector<vpImagePoint> &cogs, vpImagePoint *cogStar = NULL);
public:
- double m00; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
- with \f$i = j = 0 \f$. This moment corresponds to the dot
- surface.
-
- \sa setComputeMoments()
- */
- double m10; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 0 \f$. \f$ m_{10} \f$
- corresponds to the inertia first order moment along the v axis.
-
- \sa setComputeMoments()
- */
- double m01; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
- obtained with \f$i = 0 \f$ and \f$ j = 1 \f$. \f$ m_{01} \f$
- corresponds to the inertia first order moment along the u axis.
-
- \sa setComputeMoments()
- */
- double m11; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
- obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
-
- \sa setComputeMoments()
- */
- double m20; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
- obtained with \f$i = 2 \f$ and \f$ j = 0 \f$. \f$ m_{20} \f$
- corresponds to the inertia second order moment along the v
- axis.
-
- \sa setComputeMoments()
- */
- double m02; /*!< Considering the general distribution moments for \f$ N \f$
- points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
- u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
- obtained with \f$i = 0 \f$ and \f$ j = 2 \f$. \f$ m_{02} \f$
- corresponds to the inertia second order moment along the u
- axis.
-
- \sa setComputeMoments()
- */
- double mu11;/*!< \f$ \mu_{11} \f$ is a second order central moments defined by:
- \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
- double mu20;/*!< \f$ \mu_{20} \f$ is a second order central moments defined by:
- \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
-
- \sa setComputeMoments()
- */
- double mu02;/*!< \f$ \mu_{02} \f$ is a second order central moments defined by:
- \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
-
- \sa setComputeMoments()
- */
+ double m00; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{00} \f$ is a zero order moment obtained
+ with \f$i = j = 0 \f$. This moment corresponds to the dot
+ surface.
+
+ \sa setComputeMoments()
+ */
+ double m10; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{10} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 0 \f$. \f$ m_{10} \f$
+ corresponds to the inertia first order moment along the v axis.
+
+ \sa setComputeMoments()
+ */
+ double m01; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{01} \f$ is a first order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 1 \f$. \f$ m_{01} \f$
+ corresponds to the inertia first order moment along the u axis.
+
+ \sa setComputeMoments()
+ */
+ double m11; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{11} \f$ is a first order moment
+ obtained with \f$i = 1 \f$ and \f$ j = 1 \f$.
+
+ \sa setComputeMoments()
+ */
+ double m20; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{20} \f$ is a second order moment
+ obtained with \f$i = 2 \f$ and \f$ j = 0 \f$. \f$ m_{20} \f$
+ corresponds to the inertia second order moment along the v
+ axis.
+
+ \sa setComputeMoments()
+ */
+ double m02; /*!< Considering the general distribution moments for \f$ N \f$
+ points defined by the relation \f$ m_{ij} = \sum_{h=0}^{N}
+ u_h^i v_h^j \f$, \f$ m_{02} \f$ is a second order moment
+ obtained with \f$i = 0 \f$ and \f$ j = 2 \f$. \f$ m_{02} \f$
+ corresponds to the inertia second order moment along the u
+ axis.
+
+ \sa setComputeMoments()
+ */
+ double mu11; /*!< \f$ \mu_{11} \f$ is a second order central moments defined
+ by: \f$ \mu_{11} = m_{11} - \frac{m_{10}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu20; /*!< \f$ \mu_{20} \f$ is a second order central moments defined
+ by: \f$ \mu_{20} = m_{20} - \frac{m_{10}}{m_{00}}m_{10} \f$
+
+ \sa setComputeMoments()
+ */
+ double mu02; /*!< \f$ \mu_{02} \f$ is a second order central moments defined
+ by: \f$ \mu_{02} = m_{02} - \frac{m_{01}}{m_{00}}m_{01} \f$
+
+ \sa setComputeMoments()
+ */
private:
- virtual bool isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot);
+ virtual bool isValid(const vpImage<unsigned char> &I, const vpDot2 &wantedDot);
- virtual bool hasGoodLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const;
- virtual bool hasReverseLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const;
+ virtual bool hasGoodLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const;
+ virtual bool hasReverseLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const;
- virtual vpDot2* getInstance();
+ virtual vpDot2 *getInstance();
void init();
- bool computeParameters(const vpImage<unsigned char> &I,
- const double &u = -1.0,
- const double &v = -1.0);
-
-
+ bool computeParameters(const vpImage<unsigned char> &I, const double &u = -1.0, const double &v = -1.0);
- bool findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u,
- const unsigned int &v, unsigned int &border_u,
- unsigned int &border_v);
- void computeMeanGrayLevel(const vpImage<unsigned char>& I);
+ bool findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
+ unsigned int &border_u, unsigned int &border_v);
+ void computeMeanGrayLevel(const vpImage<unsigned char> &I);
/*!
@@ -465,9 +437,7 @@ private:
\sa getFirstBorder_v()
*/
- unsigned int getFirstBorder_u() const {
- return this->firstBorder_u;
- }
+ unsigned int getFirstBorder_u() const { return this->firstBorder_u; }
/*!
Get the starting point on a dot border. The dot border is
@@ -476,53 +446,42 @@ private:
\sa getFirstBorder_u()
*/
- unsigned int getFirstBorder_v() const {
- return this->firstBorder_v;
- }
+ unsigned int getFirstBorder_v() const { return this->firstBorder_v; }
- bool computeFreemanChainElement(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &element);
- void computeFreemanParameters(const int &u_p,
- const int &v_p, unsigned int &element,
- int &du, int &dv, float &dS,
- float &dMu, float &dMv,
- float &dMuv,
- float &dMu2, float &dMv2);
- void updateFreemanPosition( unsigned int& u, unsigned int& v,
- const unsigned int &dir );
-
-
- bool isInImage(const vpImage<unsigned char> &I ) const;
+ bool computeFreemanChainElement(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
+ unsigned int &element);
+ void computeFreemanParameters(const int &u_p, const int &v_p, unsigned int &element, int &du, int &dv, float &dS,
+ float &dMu, float &dMv, float &dMuv, float &dMu2, float &dMv2);
+ void updateFreemanPosition(unsigned int &u, unsigned int &v, const unsigned int &dir);
+
+ bool isInImage(const vpImage<unsigned char> &I) const;
bool isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip) const;
bool isInArea(const unsigned int &u, const unsigned int &v) const;
- void getGridSize( unsigned int &gridWidth, unsigned int &gridHeight );
- void setArea(const vpImage<unsigned char> &I,
- int u, int v, unsigned int w, unsigned int h);
+ void getGridSize(unsigned int &gridWidth, unsigned int &gridHeight);
+ void setArea(const vpImage<unsigned char> &I, int u, int v, unsigned int w, unsigned int h);
void setArea(const vpImage<unsigned char> &I);
- void setArea(const vpRect & a);
+ void setArea(const vpRect &a);
- unsigned char getMeanGrayLevel(vpImage<unsigned char>& I) const;
+ unsigned char getMeanGrayLevel(vpImage<unsigned char> &I) const;
//! coordinates (float) of the point center of gravity
vpImagePoint cog;
double width;
double height;
double surface;
- unsigned int gray_level_min; // minumum gray level for the dot.
- // pixel with lower level don't belong
- // to this dot.
-
- unsigned int gray_level_max; // maximum gray level for the dot.
- // pixel with higher level don't belong
- // to this dot.
- double mean_gray_level; // Mean gray level of the dot
- double grayLevelPrecision ;
- double gamma ;
- double sizePrecision ;
+ unsigned int gray_level_min; // minumum gray level for the dot.
+ // pixel with lower level don't belong
+ // to this dot.
+
+ unsigned int gray_level_max; // maximum gray level for the dot.
+ // pixel with higher level don't belong
+ // to this dot.
+ double mean_gray_level; // Mean gray level of the dot
+ double grayLevelPrecision;
+ double gamma;
+ double sizePrecision;
double ellipsoidShapePrecision;
double maxSizeSearchDistancePrecision;
double allowedBadPointsPercentage_;
@@ -534,8 +493,8 @@ private:
std::list<vpImagePoint> ip_edges_list;
// flag
- bool compute_moment ; // true moment are computed
- bool graphics ; // true for graphic overlay display
+ bool compute_moment; // true moment are computed
+ bool graphics; // true for graphic overlay display
unsigned int thickness; // Graphics thickness
@@ -545,18 +504,14 @@ private:
// The first point coodinate on the dot border
unsigned int firstBorder_u;
unsigned int firstBorder_v;
-
-//Static funtions
+
+ // Static funtions
public:
- static void display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
+ static void display(const vpImage<unsigned char> &I, const vpImagePoint &cog,
const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color = vpColor::red,
- unsigned int thickness=1);
-
+ unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
+ vpColor color = vpColor::red, unsigned int thickness = 1);
};
#endif
-
-
diff --git a/modules/tracker/blob/src/dots/vpDot.cpp b/modules/tracker/blob/src/dots/vpDot.cpp
index eaa11f3..ff97114 100644
--- a/modules/tracker/blob/src/dots/vpDot.cpp
+++ b/modules/tracker/blob/src/dots/vpDot.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,8 +44,8 @@
*/
#include <visp3/blob/vpDot.h>
-#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpTrackingException.h>
#include <vector>
@@ -68,18 +69,18 @@ void vpDot::init()
cog.set_u(0);
cog.set_v(0);
- compute_moment = false ;
- graphics = false ;
+ compute_moment = false;
+ graphics = false;
thickness = 1;
- maxDotSizePercentage = 0.25 ; // 25 % of the image size
-
+ maxDotSizePercentage = 0.25; // 25 % of the image size
+
mean_gray_level = 0;
gray_level_min = 128;
gray_level_max = 255;
grayLevelPrecision = 0.85;
- gamma = 1.5 ;
+ gamma = 1.5;
- m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu02 = mu20 = 0 ;
+ m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu02 = mu20 = 0;
connexityType = CONNEXITY_4;
@@ -90,11 +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)
{
}
@@ -104,42 +104,34 @@ 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(), 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)
{
- cog = ip;
}
/*!
\brief Copy constructor.
*/
-vpDot::vpDot(const vpDot& d)
- : vpTracker(d),
- m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4),
- cog(), u_min(0), u_max(0), v_min(0), v_max(0), graphics(false), thickness(1), maxDotSizePercentage(0.25),
- gray_level_out(0), mean_gray_level(0), gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85),
- gamma(1.5), compute_moment(false), nbMaxPoint(0)
+vpDot::vpDot(const vpDot &d)
+ : vpTracker(d), m00(0.), m01(0.), m10(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.),
+ ip_connexities_list(), ip_edges_list(), connexityType(CONNEXITY_4), cog(), u_min(0), u_max(0), v_min(0), v_max(0),
+ graphics(false), thickness(1), maxDotSizePercentage(0.25), gray_level_out(0), mean_gray_level(0),
+ gray_level_min(128), gray_level_max(255), grayLevelPrecision(0.85), gamma(1.5), compute_moment(false), nbMaxPoint(0)
{
- *this = d ;
+ *this = d;
}
/*!
\brief Destructor.
*/
-vpDot::~vpDot()
-{
- ip_connexities_list.clear() ;
-}
+vpDot::~vpDot() { ip_connexities_list.clear(); }
/*!
\brief Copy operator.
*/
-vpDot&
-vpDot::operator=(const vpDot& d)
+vpDot &vpDot::operator=(const vpDot &d)
{
ip_edges_list = d.ip_edges_list;
ip_connexities_list = d.ip_connexities_list;
@@ -151,16 +143,16 @@ vpDot::operator=(const vpDot& d)
u_max = d.u_max;
v_max = d.v_max;
- graphics = d.graphics ;
+ graphics = d.graphics;
thickness = d.thickness;
maxDotSizePercentage = d.maxDotSizePercentage;
gray_level_out = d.gray_level_out;
- mean_gray_level = d.mean_gray_level ;
- gray_level_min = d.gray_level_min ;
- gray_level_max = d.gray_level_max ;
+ mean_gray_level = d.mean_gray_level;
+ gray_level_min = d.gray_level_min;
+ gray_level_max = d.gray_level_max;
grayLevelPrecision = d.grayLevelPrecision;
gamma = d.gamma;
- compute_moment = d.compute_moment ;
+ compute_moment = d.compute_moment;
nbMaxPoint = d.nbMaxPoint;
m00 = d.m00;
@@ -174,20 +166,12 @@ vpDot::operator=(const vpDot& d)
mu20 = d.mu20;
mu02 = d.mu02;
- return *this ;
+ return *this;
}
-bool
-vpDot::operator!=(const vpDot& d) const
-{
- return ( cog != d.getCog() );
-}
+bool vpDot::operator!=(const vpDot &d) const { return (cog != d.getCog()); }
-bool
-vpDot::operator==(const vpDot& d) const
-{
- return ( cog == d.getCog() );
-}
+bool vpDot::operator==(const vpDot &d) const { return (cog == d.getCog()); }
/*!
@@ -200,15 +184,13 @@ vpDot::operator==(const vpDot& d) const
level "out" can be found. This means that the min gray level is set to 0
and the max gray level to 255. This should not occur
*/
-void
-vpDot::setGrayLevelOut()
+void vpDot::setGrayLevelOut()
{
if (gray_level_min == 0) {
if (gray_level_max == 255) {
// gray_level_min = 0 and gray_level_max = 255: this should not occur
- //vpERROR_TRACE("Unable to choose a good \"out\" level") ;
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Unable to choose a good \"out\" level")) ;
+ // vpERROR_TRACE("Unable to choose a good \"out\" level") ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Unable to choose a good \"out\" level"));
}
gray_level_out = static_cast<unsigned char>(gray_level_max + 1u);
}
@@ -231,11 +213,11 @@ vpDot::setGrayLevelOut()
\sa setGrayLevelOut()
*/
-bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n)
+bool vpDot::connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
+ double &v_cog, double &n)
{
- std::vector<bool> checkTab(I.getWidth()*I.getHeight(),false);
- return connexe(I,u,v,mean_value,u_cog,v_cog,n,checkTab);
+ std::vector<bool> checkTab(I.getWidth() * I.getHeight(), false);
+ return connexe(I, u, v, mean_value, u_cog, v_cog, n, checkTab);
}
/*!
Perform the tracking of a dot by connex components.
@@ -254,134 +236,131 @@ bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int
\sa setGrayLevelOut()
*/
-bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int v,
- double &mean_value, double &u_cog, double &v_cog, double &n,std::vector<bool> &checkTab)
+bool vpDot::connexe(const vpImage<unsigned char> &I, unsigned int u, unsigned int v, double &mean_value, double &u_cog,
+ double &v_cog, double &n, std::vector<bool> &checkTab)
{
unsigned int width = I.getWidth();
- unsigned int height= I.getHeight();
+ unsigned int height = I.getHeight();
// Test if we are in the image
- if ( (u >= width) || (v >= height) )
- {
- //std::cout << "out of bound" << std::endl;
+ if ((u >= width) || (v >= height)) {
+ // std::cout << "out of bound" << std::endl;
return false;
}
-
- if(checkTab[u + v*I.getWidth()])
+
+ if (checkTab[u + v * I.getWidth()])
return true;
-
+
vpImagePoint ip;
ip.set_u(u);
- ip.set_v(v);
-
- if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
- {
- checkTab[v*I.getWidth() + u] = true;
+ ip.set_v(v);
+
+ if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max) {
+ checkTab[v * I.getWidth() + u] = true;
ip_connexities_list.push_back(ip);
- u_cog += u ;
- v_cog += v ;
- n+=1 ;
+ u_cog += u;
+ v_cog += v;
+ n += 1;
if (n > nbMaxPoint) {
-// vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
-// "This threshold can be modified using the setMaxDotSize() "
-// "method.",
-// n, n / (I.getWidth() * I.getHeight()),
-// nbMaxPoint, maxDotSizePercentage) ;
+ // vpERROR_TRACE("Too many point %lf (%lf%% of image size). "
+ // "This threshold can be modified using the
+ // setMaxDotSize() " "method.", n, n /
+ //(I.getWidth() * I.getHeight()), nbMaxPoint,
+ // maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
"Too many point %lf (%lf%% of image size). "
"This threshold can be modified using the setMaxDotSize() "
"method.",
- n, n / (I.getWidth() * I.getHeight()),
- nbMaxPoint, maxDotSizePercentage)) ;
+ n, n / (I.getWidth() * I.getHeight()), nbMaxPoint, maxDotSizePercentage));
}
// Bounding box update
- if (u < this->u_min) this->u_min = u;
- if (u > this->u_max) this->u_max = u;
- if (v < this->v_min) this->v_min = v;
- if (v > this->v_max) this->v_max = v;
-
+ if (u < this->u_min)
+ this->u_min = u;
+ if (u > this->u_max)
+ this->u_max = u;
+ if (v < this->v_min)
+ this->v_min = v;
+ if (v > this->v_max)
+ this->v_max = v;
+
// Mean value of the dot intensities
- mean_value = (mean_value *(n-1) + I[v][u]) / n;
- if (compute_moment==true)
- {
- m00++ ;
- m10 += u ;
- m01 += v ;
- m11 += (u*v) ;
- m20 += u*u ;
- m02 += v*v ;
+ mean_value = (mean_value * (n - 1) + I[v][u]) / n;
+ if (compute_moment == true) {
+ m00++;
+ m10 += u;
+ m01 += v;
+ m11 += (u * v);
+ m20 += u * u;
+ m02 += v * v;
}
- }
- else
- {
- //std::cout << "not in" << std::endl;
+ } else {
+ // std::cout << "not in" << std::endl;
return false;
}
-
+
bool edge = false;
-
- //if((int)u-1 >= 0)
- if(u >= 1)
- if(!checkTab[u-1 + v*I.getWidth()])
- if(!connexe(I,u-1,v, mean_value,u_cog,v_cog, n, checkTab))
- edge = true;
-
- if(u+1 < I.getWidth())
- if(!checkTab[u+1+v*I.getWidth()])
- if(!connexe(I,u+1,v,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v >= 1)
- if(!checkTab[u+(v-1)*I.getWidth()])
- if(!connexe(I,u, v-1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
- if(v+1 < I.getHeight())
- if(!checkTab[u+(v+1)*I.getWidth()])
- if(!connexe(I,u,v+1,mean_value,u_cog, v_cog, n, checkTab))
- edge = true;
-
+
+ // if((int)u-1 >= 0)
+ if (u >= 1)
+ if (!checkTab[u - 1 + v * I.getWidth()])
+ if (!connexe(I, u - 1, v, mean_value, u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if (u + 1 < I.getWidth())
+ if (!checkTab[u + 1 + v * I.getWidth()])
+ if (!connexe(I, u + 1, v, mean_value, u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if (v >= 1)
+ if (!checkTab[u + (v - 1) * I.getWidth()])
+ if (!connexe(I, u, v - 1, mean_value, u_cog, v_cog, n, checkTab))
+ edge = true;
+
+ if (v + 1 < I.getHeight())
+ if (!checkTab[u + (v + 1) * I.getWidth()])
+ if (!connexe(I, u, v + 1, mean_value, u_cog, v_cog, n, checkTab))
+ edge = true;
+
if (connexityType == CONNEXITY_8) {
- if(v >= 1 && u >= 1)
- if(!checkTab[u-1+(v-1)*I.getWidth()])
- if(!connexe(I,u-1,v-1,mean_value,u_cog, v_cog, n, checkTab))
+ if (v >= 1 && u >= 1)
+ if (!checkTab[u - 1 + (v - 1) * I.getWidth()])
+ if (!connexe(I, u - 1, v - 1, mean_value, u_cog, v_cog, n, checkTab))
edge = true;
-
- if(v >= 1 && u+1 < I.getWidth())
- if(!checkTab[u+1+(v-1)*I.getWidth()])
- if(!connexe(I,u+1,v-1,mean_value,u_cog, v_cog, n, checkTab))
+
+ if (v >= 1 && u + 1 < I.getWidth())
+ if (!checkTab[u + 1 + (v - 1) * I.getWidth()])
+ if (!connexe(I, u + 1, v - 1, mean_value, u_cog, v_cog, n, checkTab))
edge = true;
-
- if(v+1 < I.getHeight() && u >= 1)
- if(!checkTab[u-1+(v+1)*I.getWidth()])
- if(!connexe(I,u-1,v+1,mean_value, u_cog, v_cog, n, checkTab))
+
+ if (v + 1 < I.getHeight() && u >= 1)
+ if (!checkTab[u - 1 + (v + 1) * I.getWidth()])
+ if (!connexe(I, u - 1, v + 1, mean_value, u_cog, v_cog, n, checkTab))
edge = true;
-
- if(v+1 < I.getHeight() && u+1 < I.getWidth())
- if(!checkTab[u+1+(v+1)*I.getWidth()])
- if(!connexe(I,u+1,v+1,mean_value,u_cog, v_cog, n, checkTab))
+
+ if (v + 1 < I.getHeight() && u + 1 < I.getWidth())
+ if (!checkTab[u + 1 + (v + 1) * I.getWidth()])
+ if (!connexe(I, u + 1, v + 1, mean_value, u_cog, v_cog, n, checkTab))
edge = true;
- }
-
- if(edge){
+ }
+
+ if (edge) {
ip_edges_list.push_back(ip);
- if (graphics==true)
- {
+ if (graphics == true) {
vpImagePoint ip_(ip);
- for(unsigned int t=0; t<thickness; t++) {
+ for (unsigned int t = 0; t < thickness; t++) {
ip_.set_u(ip.get_u() + t);
- vpDisplay::displayPoint(I, ip_, vpColor::red) ;
+ vpDisplay::displayPoint(I, ip_, vpColor::red);
}
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
}
-
+
return true;
}
@@ -404,26 +383,25 @@ bool vpDot::connexe(const vpImage<unsigned char>& I,unsigned int u,unsigned int
\sa connexe()
*/
-void
-vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
+void vpDot::COG(const vpImage<unsigned char> &I, double &u, double &v)
{
// Set the maximal number of points considering the maximal dot size
// image percentage
- nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
+ nbMaxPoint = (I.getWidth() * I.getHeight()) * maxDotSizePercentage;
// segmentation de l'image apres seuillage
// (etiquetage des composante connexe)
if (compute_moment)
m00 = m11 = m02 = m20 = m10 = m01 = mu11 = mu20 = mu02 = 0;
- double u_cog = 0 ;
- double v_cog = 0 ;
- double npoint = 0 ;
- this->mean_gray_level = 0 ;
+ double u_cog = 0;
+ double v_cog = 0;
+ double npoint = 0;
+ this->mean_gray_level = 0;
- ip_connexities_list.clear() ;
+ ip_connexities_list.clear();
ip_edges_list.clear();
-
+
// Initialise the boundig box
this->u_min = I.getWidth();
this->u_max = 0;
@@ -465,9 +443,8 @@ vpDot::COG(const vpImage<unsigned char> &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) )
- {
- bool sol = false ;
+ if (!connexe(I, (unsigned int)u, (unsigned int)v, mean_gray_level, u_cog, v_cog, npoint)) {
+ bool sol = false;
unsigned int right = 1;
unsigned int botom = 1;
@@ -477,128 +454,136 @@ vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
unsigned int k;
// Spiral search from the center to find the nearest dot
- while( (right < SPIRAL_SEARCH_SIZE) && (sol == false) ) {
- for (k=1; k <= right; k++) if(sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
- if ( connexe(I, (unsigned int)u_+k, (unsigned int)(v_),mean_gray_level, u_cog, v_cog, npoint) ) {
- sol = true; u = u_+k; v = v_;
- }
- }
+ while ((right < SPIRAL_SEARCH_SIZE) && (sol == false)) {
+ for (k = 1; k <= right; k++)
+ if (sol == false) {
+ u_cog = 0;
+ v_cog = 0;
+ ip_connexities_list.clear();
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0;
+ if (connexe(I, (unsigned int)u_ + k, (unsigned int)(v_), mean_gray_level, u_cog, v_cog, npoint)) {
+ sol = true;
+ u = u_ + k;
+ v = v_;
+ }
+ }
u_ += k;
right += 2;
- for (k=1; k <= botom; k++) if (sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_+k),mean_gray_level, u_cog, v_cog, npoint) ) {
- sol = true; u = u_; v = v_+k;
- }
- }
+ for (k = 1; k <= botom; k++)
+ if (sol == false) {
+ u_cog = 0;
+ v_cog = 0;
+ ip_connexities_list.clear();
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0;
+
+ if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ + k), mean_gray_level, u_cog, v_cog, npoint)) {
+ sol = true;
+ u = u_;
+ v = v_ + k;
+ }
+ }
v_ += k;
botom += 2;
- for (k=1; k <= left; k++) if (sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_-k), (unsigned int)(v_),mean_gray_level,u_cog, v_cog, npoint) ) {
- sol = true ; u = u_-k; v = v_;
- }
- }
+ for (k = 1; k <= left; k++)
+ if (sol == false) {
+ u_cog = 0;
+ v_cog = 0;
+ ip_connexities_list.clear();
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0;
+
+ if (connexe(I, (unsigned int)(u_ - k), (unsigned int)(v_), mean_gray_level, u_cog, v_cog, npoint)) {
+ sol = true;
+ u = u_ - k;
+ v = v_;
+ }
+ }
u_ -= k;
left += 2;
- for (k=1; k <= up; k++) if(sol==false) {
- u_cog = 0 ;
- v_cog = 0 ;
- ip_connexities_list.clear() ;
- ip_edges_list.clear();
-
- this->mean_gray_level = 0 ;
-
- if ( connexe(I, (unsigned int)(u_), (unsigned int)(v_-k),mean_gray_level,u_cog, v_cog, npoint) ) {
- sol = true ; u = u_; v = v_-k;
- }
- }
+ for (k = 1; k <= up; k++)
+ if (sol == false) {
+ u_cog = 0;
+ v_cog = 0;
+ ip_connexities_list.clear();
+ ip_edges_list.clear();
+
+ this->mean_gray_level = 0;
+
+ if (connexe(I, (unsigned int)(u_), (unsigned int)(v_ - k), mean_gray_level, u_cog, v_cog, npoint)) {
+ sol = true;
+ u = u_;
+ v = v_ - k;
+ }
+ }
v_ -= k;
up += 2;
}
if (sol == false) {
- //vpERROR_TRACE("Dot has been lost") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "Dot has been lost")) ;
+ // vpERROR_TRACE("Dot has been lost") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError, "Dot has been lost"));
}
}
#endif
-/*
- vpImagePoint ip;
- unsigned int i, j;
- std::list<vpImagePoint>::iterator it;
- for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it ++) {
- ip = *it;
- i = (unsigned int) ip.get_i();
- j = (unsigned int) ip.get_j();
- I[i][j] = 255 ;
- }*/
+ /*
+ vpImagePoint ip;
+ unsigned int i, j;
+ std::list<vpImagePoint>::iterator it;
+ for (it = ip_connexities_list.begin(); it != ip_connexities_list.end(); it
+ ++) { ip = *it; i = (unsigned int) ip.get_i(); j = (unsigned int)
+ ip.get_j(); I[i][j] = 255 ;
+ }*/
- u_cog = u_cog/npoint ;
- v_cog = v_cog/npoint ;
+ u_cog = u_cog / npoint;
+ v_cog = v_cog / npoint;
- u = u_cog ;
- v = v_cog ;
+ u = u_cog;
+ v = v_cog;
// Initialize the threshold for the next call to track()
- double Ip = pow((double)this->mean_gray_level/255,1/gamma);
+ double Ip = pow((double)this->mean_gray_level / 255, 1 / gamma);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
- //vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
- //vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
+ // vpCTRACE << "gray_level_min: " << gray_level_min << std::endl;
+ // vpCTRACE << "gray_level_max: " << gray_level_max << std::endl;
- if (npoint < 5)
- {
- //vpERROR_TRACE("Dot to small") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "Dot to small")) ;
+ if (npoint < 5) {
+ // vpERROR_TRACE("Dot to small") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError, "Dot to small"));
}
- if (npoint > nbMaxPoint)
- {
-// vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf (%lf%%). This threshold can be modified using the setMaxDotSize() method.",
-// npoint, npoint / (I.getWidth() * I.getHeight()),
-// nbMaxPoint, maxDotSizePercentage) ;
+ if (npoint > nbMaxPoint) {
+ // vpERROR_TRACE("Too many point %lf (%lf%%). Max allowed is %lf
+ // (%lf%%). This threshold can be modified using the setMaxDotSize()
+ // method.",
+ // npoint, npoint / (I.getWidth() * I.getHeight()),
+ // nbMaxPoint, maxDotSizePercentage) ;
throw(vpTrackingException(vpTrackingException::featureLostError,
- "Too many point %lf (%lf%%). Max allowed is %lf (%lf%%). This threshold can be modified using the setMaxDotSize() method.",
- npoint, npoint / (I.getWidth() * I.getHeight()),
- nbMaxPoint, maxDotSizePercentage)) ;
- }
+ "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));
+ }
}
/*!
@@ -613,16 +598,13 @@ vpDot::COG(const vpImage<unsigned char> &I, double& u, double& v)
vpTrackingException::featureLostError.
*/
-void
-vpDot::setMaxDotSize(double percentage)
+void vpDot::setMaxDotSize(double percentage)
{
if (percentage <= 0.0 || percentage > 1.0) {
// print a warning. We keep the default percentage
vpTRACE("Max dot size percentage is requested to be set to %lf.",
- "Value should be in ]0:1]. Value will be set to %lf.",
- percentage, maxDotSizePercentage);
- }
- else {
+ "Value should be in ]0:1]. Value will be set to %lf.", percentage, maxDotSizePercentage);
+ } else {
maxDotSizePercentage = percentage;
}
}
@@ -650,34 +632,31 @@ vpDot::setMaxDotSize(double percentage)
\sa track(), getCog()
*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I)
+void vpDot::initTracking(const vpImage<unsigned char> &I)
{
- while (vpDisplay::getClick(I, cog) != true) ;
+ while (vpDisplay::getClick(I, cog) != true)
+ ;
unsigned int i = (unsigned int)cog.get_i();
unsigned int j = (unsigned int)cog.get_j();
- double Ip = pow((double)I[i][j]/255, 1/gamma);
+ double Ip = pow((double)I[i][j] / 255, 1 / gamma);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ throw(e);
}
}
@@ -704,34 +683,30 @@ vpDot::initTracking(const vpImage<unsigned char>& I)
\sa track()
*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip)
+void vpDot::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip)
{
- cog = ip ;
+ cog = ip;
unsigned int i = (unsigned int)cog.get_i();
unsigned int j = (unsigned int)cog.get_j();
- double Ip = pow((double)I[i][j]/255, 1/gamma);
+ double Ip = pow((double)I[i][j] / 255, 1 / gamma);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ throw(e);
}
}
@@ -762,26 +737,22 @@ vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip)
\sa track(), getCog()
*/
-void
-vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
- unsigned int level_min, unsigned int level_max)
+void vpDot::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int level_min,
+ unsigned int level_max)
{
- cog = ip ;
+ cog = ip;
this->gray_level_min = level_min;
this->gray_level_max = level_max;
try {
- track( I );
- }
- catch(vpException &e)
- {
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ throw(e);
}
}
-
/*!
Track and compute the dot characteristics.
@@ -796,35 +767,31 @@ vpDot::initTracking(const vpImage<unsigned char>& I, const vpImagePoint &ip,
\sa getCog()
*/
-void
-vpDot::track(const vpImage<unsigned char> &I)
+void vpDot::track(const vpImage<unsigned char> &I)
{
- try{
+ try {
setGrayLevelOut();
double u = this->cog.get_u();
double v = this->cog.get_v();
- COG( I, u, v ) ;
+ COG(I, u, v);
- this->cog.set_u( u );
- this->cog.set_v( v );
-
- if (compute_moment==true)
- {
- mu11 = m11 - u*m01;
- mu02 = m02 - v*m01;
- mu20 = m20 - u*m10;
+ this->cog.set_u(u);
+ this->cog.set_v(v);
+
+ if (compute_moment == true) {
+ mu11 = m11 - u * m01;
+ mu02 = m02 - v * m01;
+ mu20 = m20 - u * m10;
}
if (graphics) {
// display a red cross at the center of gravity's location in the image.
- vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
+ vpDisplay::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness);
}
- }
- catch(vpException &e)
- {
- throw(e) ;
+ } catch (vpException &e) {
+ throw(e);
}
}
@@ -842,10 +809,9 @@ vpDot::track(const vpImage<unsigned char> &I)
\param ip [out] : Sub pixel coordinate of the tracked dot center of gravity.
*/
-void
-vpDot::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
+void vpDot::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
{
- track( I ) ;
+ track(I);
ip = this->cog;
}
@@ -857,13 +823,12 @@ vpDot::track(const vpImage<unsigned char> &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<unsigned char>& I, vpColor color, unsigned int thick) const
+void vpDot::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thick) const
{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thick);
+ vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thick);
std::list<vpImagePoint>::const_iterator it;
- for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
- {
+ for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
@@ -874,7 +839,8 @@ void vpDot::display(const vpImage<unsigned char>& I, vpColor color, unsigned int
\param precision : It is a double precision float which value is
in ]0,1]:
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - 1 means full precision, whereas values close to 0 show a very bad
+ accuracy.
- Values lower or equal to 0 are brought back to an epsion>0
- Values higher than 1 are brought back to 1
If the initial gray level is I, the gray levels of the dot will be between :
@@ -885,19 +851,14 @@ void vpDot::display(const vpImage<unsigned char>& I, vpColor color, unsigned int
\sa setWidth(), setHeight(), setGrayLevelMin(), setGrayLevelMax()
*/
-void vpDot::setGrayLevelPrecision( const double & precision )
+void vpDot::setGrayLevelPrecision(const double &precision)
{
double epsilon = 0.05;
- if( grayLevelPrecision<epsilon )
- {
+ if (grayLevelPrecision < epsilon) {
this->grayLevelPrecision = epsilon;
- }
- else if( grayLevelPrecision>1 )
- {
+ } else if (grayLevelPrecision > 1) {
this->grayLevelPrecision = 1.0;
- }
- else
- {
+ } else {
this->grayLevelPrecision = precision;
}
}
@@ -907,24 +868,22 @@ void vpDot::setGrayLevelPrecision( const double & precision )
Display the dot center of gravity and its list of edges.
\param I : The image used as background.
-
+
\param cog : The center of gravity.
-
+
\param edges_list : The list of edges;
-
+
\param color : Color used to display the dot.
-
+
\param thickness : Thickness of the dot.
*/
-void vpDot::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
+void vpDot::display(const vpImage<unsigned char> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
+ vpColor color, unsigned int thickness)
{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
std::list<vpImagePoint>::const_iterator it;
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
+ for (it = edges_list.begin(); it != edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
@@ -943,26 +902,20 @@ void vpDot::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
\param thickness : Thickness of the dot.
*/
-void vpDot::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
+void vpDot::display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
+ vpColor color, unsigned int thickness)
{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
std::list<vpImagePoint>::const_iterator it;
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
+ for (it = edges_list.begin(); it != edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
/*!
- Writes the dot center of gravity coordinates in the frame (i,j) (For more details
- about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
- and returns a reference to the stream.
+ Writes the dot center of gravity coordinates in the frame (i,j) (For more
+ details about the orientation of the frame see the vpImagePoint
+ documentation) to the stream \e os, and returns a reference to the stream.
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot& d) {
- return (os << "(" << d.getCog() << ")" ) ;
-} ;
-
-
+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 c99713e..4800f8d 100644
--- a/modules/tracker/blob/src/dots/vpDot2.cpp
+++ b/modules/tracker/blob/src/dots/vpDot2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,19 +44,18 @@
//#define DEBUG
-
#include <visp3/core/vpDisplay.h>
// exception handling
-#include <visp3/core/vpTrackingException.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpTrackingException.h>
-#include <visp3/blob/vpDot2.h>
+#include <cmath> // std::fabs
+#include <iostream>
+#include <limits> // numeric_limits
#include <math.h>
-#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <visp3/blob/vpDot2.h>
/******************************************************************************
*
@@ -79,7 +79,7 @@ void vpDot2::init()
gray_level_min = 128;
gray_level_max = 255;
grayLevelPrecision = 0.80;
- gamma = 1.5 ;
+ gamma = 1.5;
sizePrecision = 0.65;
ellipsoidShapePrecision = 0.65;
@@ -93,7 +93,7 @@ void vpDot2::init()
firstBorder_u = 0;
firstBorder_v = 0;
- compute_moment = false ;
+ compute_moment = false;
graphics = false;
thickness = 1;
}
@@ -102,13 +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),
+ : 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()
+ 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()
{
}
@@ -121,28 +119,23 @@ 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(), width(0), height(0), surface(0),
- gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
+ : 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()
+ allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false), graphics(false),
+ thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0), firstBorder_u(0), firstBorder_v()
{
- cog = ip;
}
/*!
Copy constructor.
*/
-vpDot2::vpDot2(const vpDot2& twinDot )
- : vpTracker(twinDot),
- m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.),
- mu11(0.), mu20(0.), mu02(0.), cog(), width(0), height(0), surface(0),
- gray_level_min(128), gray_level_max(255), mean_gray_level(0), grayLevelPrecision(0.8), gamma(1.5),
- sizePrecision(0.65), ellipsoidShapePrecision(0.65), maxSizeSearchDistancePrecision(0.65),
- allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(), compute_moment(false),
- graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
+vpDot2::vpDot2(const vpDot2 &twinDot)
+ : vpTracker(twinDot), m00(0.), m10(0.), m01(0.), m11(0.), m20(0.), m02(0.), mu11(0.), mu20(0.), mu02(0.), cog(),
+ width(0), height(0), surface(0), gray_level_min(128), gray_level_max(255), mean_gray_level(0),
+ grayLevelPrecision(0.8), gamma(1.5), sizePrecision(0.65), ellipsoidShapePrecision(0.65),
+ maxSizeSearchDistancePrecision(0.65), allowedBadPointsPercentage_(0.), area(), direction_list(), ip_edges_list(),
+ compute_moment(false), graphics(false), thickness(1), bbox_u_min(0), bbox_u_max(0), bbox_v_min(0), bbox_v_max(0),
firstBorder_u(0), firstBorder_v()
{
*this = twinDot;
@@ -151,26 +144,27 @@ vpDot2::vpDot2(const vpDot2& twinDot )
/*!
= operator.
*/
-vpDot2& vpDot2::operator=(const vpDot2& twinDot )
+vpDot2 &vpDot2::operator=(const vpDot2 &twinDot)
{
cog = twinDot.cog;
- width = twinDot.width;
- height = twinDot.height;
- surface = twinDot.surface;
+ width = twinDot.width;
+ height = twinDot.height;
+ surface = twinDot.surface;
gray_level_min = twinDot.gray_level_min;
gray_level_max = twinDot.gray_level_max;
mean_gray_level = twinDot.mean_gray_level;
grayLevelPrecision = twinDot.grayLevelPrecision;
- gamma = twinDot.gamma; ;
+ gamma = twinDot.gamma;
+ ;
sizePrecision = twinDot.sizePrecision;
- ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision ;
+ ellipsoidShapePrecision = twinDot.ellipsoidShapePrecision;
maxSizeSearchDistancePrecision = twinDot.maxSizeSearchDistancePrecision;
allowedBadPointsPercentage_ = twinDot.allowedBadPointsPercentage_;
area = twinDot.area;
direction_list = twinDot.direction_list;
- ip_edges_list = twinDot.ip_edges_list;
+ ip_edges_list = twinDot.ip_edges_list;
compute_moment = twinDot.compute_moment;
graphics = twinDot.graphics;
@@ -201,8 +195,7 @@ vpDot2& vpDot2::operator=(const vpDot2& twinDot )
/*!
Destructor... do nothing for the moment.
*/
-vpDot2::~vpDot2(){}
-
+vpDot2::~vpDot2() {}
/******************************************************************************
*
@@ -216,18 +209,16 @@ 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<unsigned char>& I, vpColor color, unsigned int t) const
+void vpDot2::display(const vpImage<unsigned char> &I, vpColor color, unsigned int t) const
{
- vpDisplay::displayCross(I, cog, 3*t+8, color, t);
+ vpDisplay::displayCross(I, cog, 3 * t + 8, color, t);
std::list<vpImagePoint>::const_iterator it;
- for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it)
- {
+ for (it = ip_edges_list.begin(); it != ip_edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
-
/*!
Initialize the tracking with a mouse click on the image and update the dot
@@ -259,24 +250,24 @@ void vpDot2::display(const vpImage<unsigned char>& I, vpColor color, unsigned in
\sa track()
*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I, unsigned int size)
+void vpDot2::initTracking(const vpImage<unsigned char> &I, unsigned int size)
{
- while ( vpDisplay::getClick(I, cog) != true) ;
+ while (vpDisplay::getClick(I, cog) != true)
+ ;
unsigned int i = (unsigned int)cog.get_i();
unsigned int j = (unsigned int)cog.get_j();
- double Ip = pow((double)I[i][j]/255,1/gamma);
+ double Ip = pow((double)I[i][j] / 255, 1 / gamma);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
@@ -284,12 +275,10 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I, unsigned int size)
setHeight(size);
try {
- track( I );
- }
- catch(vpException &e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ // vpERROR_TRACE("Error caught") ;
+ throw(e);
}
}
@@ -320,25 +309,23 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I, unsigned int size)
This is the default case. To track a non ellipsoid shape use
setEllipsoidShapePrecision(0).
*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I,
- const vpImagePoint &ip, unsigned int size)
+void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int size)
{
- cog = ip ;
+ cog = ip;
unsigned int i = (unsigned int)cog.get_i();
unsigned int j = (unsigned int)cog.get_j();
- double Ip = pow((double)I[i][j]/255,1/gamma);
+ double Ip = pow((double)I[i][j] / 255, 1 / gamma);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
@@ -346,12 +333,10 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I,
setHeight(size);
try {
- track( I );
- }
- catch(vpException &e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ // vpERROR_TRACE("Error caught") ;
+ throw(e);
}
}
@@ -394,13 +379,10 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I,
\sa track(), getCog()
*/
-void vpDot2::initTracking(const vpImage<unsigned char>& I,
- const vpImagePoint &ip,
- unsigned int gray_lvl_min,
- unsigned int gray_lvl_max,
- unsigned int size)
+void vpDot2::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip, unsigned int gray_lvl_min,
+ unsigned int gray_lvl_max, unsigned int size)
{
- cog = ip ;
+ cog = ip;
this->gray_level_min = gray_lvl_min;
this->gray_level_max = gray_lvl_max;
@@ -409,17 +391,13 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I,
setHeight(size);
try {
- track( I );
- }
- catch(vpException &e)
- {
- //vpERROR_TRACE("Error caught") ;
- throw(e) ;
+ track(I);
+ } catch (vpException &e) {
+ // vpERROR_TRACE("Error caught") ;
+ throw(e);
}
}
-
-
/*!
Try to locate the dot in the image:
@@ -427,8 +405,8 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I,
- First, estimate the new position of the dot, using its previous position.
- Then compute the center of gravity (surface, width height) of the
tracked entity from the Freeman chain elements.
- - If the dot is lost (estimated point too dark, too much surface change,...),
- search the dot in a window around the previous position.
+ - If the dot is lost (estimated point too dark, too much surface
+ change,...), search the dot in a window around the previous position.
- If no valid dot was found in the window, return an exception.
\param I : Image.
@@ -460,7 +438,7 @@ void vpDot2::initTracking(const vpImage<unsigned char>& I,
*/
void vpDot2::track(const vpImage<unsigned char> &I)
{
- m00 = m11 = m02 = m20 = m10 = m01 = 0 ;
+ m00 = m11 = m02 = m20 = m10 = m01 = 0;
// First, we will estimate the position of the tracked point
@@ -468,11 +446,10 @@ void vpDot2::track(const vpImage<unsigned char> &I)
setArea(I);
// create a copy of the dot to search
- // This copy can be saw as the previous dot used to check if the current one
- // found with computeParameters() is similar to the previous one (see isValid()
- // function).
- // If the found dot is not similar (or valid), we use this copy to set the current
- // found dot to the previous one (see below).
+ // This copy can be saw as the previous dot used to check if the current one
+ // found with computeParameters() is similar to the previous one (see
+ // isValid() function). If the found dot is not similar (or valid), we use
+ // this copy to set the current found dot to the previous one (see below).
vpDot2 wantedDot(*this);
// vpDEBUG_TRACE(0, "Previous dot: ");
@@ -482,61 +459,54 @@ void vpDot2::track(const vpImage<unsigned char> &I)
if (found) {
// test if the found dot is valid (ie similar to the previous one)
- found = isValid( I, wantedDot);
- if (! found) {
+ found = isValid(I, wantedDot);
+ if (!found) {
*this = wantedDot;
- //std::cout << "The found dot is not valid" << std::endl;
+ // std::cout << "The found dot is not valid" << std::endl;
}
}
- if (! found) {
- // vpDEBUG_TRACE(0, "Search the dot in a biggest window around the last position");
- // vpDEBUG_TRACE(0, "Bad computed dot: ");
+ if (!found) {
+ // vpDEBUG_TRACE(0, "Search the dot in a biggest window around the
+ // last position"); vpDEBUG_TRACE(0, "Bad computed dot: ");
// vpDEBUG_TRACE(0, "u: %f v: %f", get_u(), get_v());
// vpDEBUG_TRACE(0, "w: %f h: %f", getWidth(), getHeight());
// if estimation was wrong (get an error tracking), look for the dot
// closest from the estimation,
- // i.e. search for dots in an a region of interest around the this dot and get the first
- // element in the area.
+ // i.e. search for dots in an a region of interest around the this dot and
+ // get the first element in the area.
// first get the size of the search window from the dot size
double searchWindowWidth, searchWindowHeight;
- //if( getWidth() == 0 || getHeight() == 0 )
- if( std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon() || std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon() )
- {
+ // if( getWidth() == 0 || getHeight() == 0 )
+ if (std::fabs(getWidth()) <= std::numeric_limits<double>::epsilon() ||
+ std::fabs(getHeight()) <= std::numeric_limits<double>::epsilon()) {
searchWindowWidth = 80.;
searchWindowHeight = 80.;
- }
- else
- {
- searchWindowWidth = getWidth() * 5;
+ } else {
+ searchWindowWidth = getWidth() * 5;
searchWindowHeight = getHeight() * 5;
}
std::list<vpDot2> candidates;
- searchDotsInArea( I,
- (int)(this->cog.get_u()-searchWindowWidth /2.0),
- (int)(this->cog.get_v()-searchWindowHeight/2.0),
- (unsigned int)searchWindowWidth,
- (unsigned int)searchWindowHeight,
- candidates);
+ searchDotsInArea(I, (int)(this->cog.get_u() - searchWindowWidth / 2.0),
+ (int)(this->cog.get_v() - searchWindowHeight / 2.0), (unsigned int)searchWindowWidth,
+ (unsigned int)searchWindowHeight, candidates);
// if the vector is empty, that mean we didn't find any candidate
// in the area, return an error tracking.
- if( candidates.empty() )
- {
- //vpERROR_TRACE("No dot was found") ;
- throw(vpTrackingException(vpTrackingException::featureLostError,
- "No dot was found")) ;
+ if (candidates.empty()) {
+ // vpERROR_TRACE("No dot was found") ;
+ throw(vpTrackingException(vpTrackingException::featureLostError, "No dot was found"));
}
// otherwise we've got our dot, update this dot's parameters
vpDot2 movingDot = candidates.front();
- setCog( movingDot.getCog() );
- setArea( movingDot.getArea() );
- setWidth( movingDot.getWidth() );
- setHeight( movingDot.getHeight() );
+ setCog(movingDot.getCog());
+ setArea(movingDot.getArea());
+ setWidth(movingDot.getWidth());
+ setHeight(movingDot.getHeight());
// Update the moments
m00 = movingDot.m00;
@@ -561,20 +531,21 @@ void vpDot2::track(const vpImage<unsigned char> &I)
// " surface (number of pixels) which differ too much "
// " to the previous one "
// "- or a problem of the shape which is not ellipsoid if "
- // " use setEllipsoidShapePrecision(double ellipsoidShapePrecision) "
+ // " use setEllipsoidShapePrecision(double
+ // ellipsoidShapePrecision) "
// " which is the default case. "
- // " To track a non ellipsoid shape use setEllipsoidShapePrecision(0)") ;
+ // " To track a non ellipsoid shape use
+ // setEllipsoidShapePrecision(0)") ;
// throw(vpTrackingException(vpTrackingException::featureLostError,
// "The found dot is invalid")) ;
// }
// }
// if this dot is partially out of the image, return an error tracking.
- if( !isInImage( I ) )
- {
- //vpERROR_TRACE("The center of gravity of the dot is not in the image") ;
+ 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")) ;
+ "The center of gravity of the dot is not in the image"));
}
// Get dots center of gravity
@@ -582,28 +553,27 @@ void vpDot2::track(const vpImage<unsigned char> &I)
// unsigned int v = (unsigned int) this->cog.get_v();
// Updates the min and max gray levels for the next iteration
// double Ip = pow((double)I[v][u]/255,1/gamma);
- double Ip = pow(getMeanGrayLevel()/255,1/gamma);
- //printf("current value of gray level center : %i\n", I[v][u]);
-
- //getMeanGrayLevel(I);
- if(Ip - (1 - grayLevelPrecision)<0){
- gray_level_min = 0 ;
- }
- else{
- gray_level_min = (unsigned int) (255*pow(Ip - (1 - grayLevelPrecision),gamma));
+ double Ip = pow(getMeanGrayLevel() / 255, 1 / gamma);
+ // printf("current value of gray level center : %i\n", I[v][u]);
+
+ // getMeanGrayLevel(I);
+ if (Ip - (1 - grayLevelPrecision) < 0) {
+ gray_level_min = 0;
+ } else {
+ gray_level_min = (unsigned int)(255 * pow(Ip - (1 - grayLevelPrecision), gamma));
if (gray_level_min > 255)
gray_level_min = 255;
}
- gray_level_max = (unsigned int) (255*pow(Ip + (1 - grayLevelPrecision),gamma));
+ gray_level_max = (unsigned int)(255 * pow(Ip + (1 - grayLevelPrecision), gamma));
if (gray_level_max > 255)
gray_level_max = 255;
- //printf("%i %i \n",gray_level_max,gray_level_min);
+ // printf("%i %i \n",gray_level_max,gray_level_min);
if (graphics) {
// display a red cross at the center of gravity's location in the image.
- vpDisplay::displayCross(I, this->cog, 3*thickness+8, vpColor::red, thickness);
- //vpDisplay::flush(I);
+ vpDisplay::displayCross(I, this->cog, 3 * thickness + 8, vpColor::red, thickness);
+ // vpDisplay::flush(I);
}
}
@@ -625,65 +595,50 @@ void vpDot2::track(const vpImage<unsigned char> &I)
\sa track()
*/
-void
-vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
+void vpDot2::track(const vpImage<unsigned char> &I, vpImagePoint &ip)
{
track(I);
ip = this->cog;
}
-///// GET METHODS /////////////////////////////////////////////////////////////
+///// GET METHODS
+////////////////////////////////////////////////////////////////
/*!
Return the width of the dot.
\sa getHeight()
*/
-double vpDot2::getWidth() const
-{
- return width;
-}
+double vpDot2::getWidth() const { return width; }
/*!
Return the height of the dot.
\sa getWidth()
*/
-double vpDot2::getHeight() const
-{
- return height;
-}
+double vpDot2::getHeight() const { return height; }
/*!
Return the area of the dot.
The area of the dot is also given by \f$|m00|\f$.
*/
-double vpDot2::getArea() const
-{
- return fabs(surface);
-}
+double vpDot2::getArea() const { return fabs(surface); }
/*!
Return the precision of the gray level of the dot. It is a double
precision float witch value is in ]0,1]. 1 means full precision, whereas
values close to 0 show a very bad precision.
*/
-double vpDot2::getGrayLevelPrecision() const
-{
- return grayLevelPrecision;
-}
+double vpDot2::getGrayLevelPrecision() const { return grayLevelPrecision; }
/*!
Return the precision of the size of the dot. It is a double
precision float witch value is in ]0,1]. 1 means full precision, whereas
values close to 0 show a very bad precision.
*/
-double vpDot2::getSizePrecision() const
-{
- return sizePrecision;
-}
+double vpDot2::getSizePrecision() const { return sizePrecision; }
/*!
Return the precision of the ellipsoid shape of the dot. It is a double
@@ -692,35 +647,29 @@ double vpDot2::getSizePrecision() const
\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 point on a dot border. It is a double
- precision float witch value is in [0.05,1]. 1 means full precision, whereas
- values close to 0 show a very bad precision.
+ Return the precision of the search maximum distance to get the starting
+ point on a dot border. It is a double precision float witch value is in
+ [0.05,1]. 1 means full precision, whereas values close to 0 show a very bad
+ precision.
*/
-double vpDot2::getMaxSizeSearchDistancePrecision() const{
- return maxSizeSearchDistancePrecision;
-}
+double vpDot2::getMaxSizeSearchDistancePrecision() const { return maxSizeSearchDistancePrecision; }
/*!
Return the distance between the two center of dots.
*/
-double vpDot2::getDistance( const vpDot2& distantDot ) const
+double vpDot2::getDistance(const vpDot2 &distantDot) const
{
vpImagePoint cogDistantDot = distantDot.getCog();
double diff_u = this->cog.get_u() - cogDistantDot.get_u();
double diff_v = this->cog.get_v() - cogDistantDot.get_v();
- return sqrt( diff_u*diff_u + diff_v*diff_v );
+ return sqrt(diff_u * diff_u + diff_v * diff_v);
}
-
///// SET METHODS ////////////////////////////////////////////////////////////
-
/*!
Set the width of the dot. This is meant to be used to search a dot in an
@@ -730,10 +679,7 @@ double vpDot2::getDistance( const vpDot2& distantDot ) const
\sa setHeight(), setArea(), setSizePrecision()
*/
-void vpDot2::setWidth( const double & w )
-{
- this->width = w;
-}
+void vpDot2::setWidth(const double &w) { this->width = w; }
/*!
@@ -745,31 +691,27 @@ void vpDot2::setWidth( const double & w )
\sa setWidth(), setArea(), setSizePrecision()
*/
-void vpDot2::setHeight( const double & h )
-{
- this->height = h;
-}
+void vpDot2::setHeight(const double &h) { this->height = h; }
/*!
- Set the area of the dot. This is meant to be used to search a dot in a region of interest.
+ Set the area of the dot. This is meant to be used to search a dot in a
+ region of interest.
\param a : Area of a dot to search in a region of interest.
\sa setWidth(), setHeight(), setSizePrecision()
*/
-void vpDot2::setArea( const double & a )
-{
- this->surface = a;
-}
+void vpDot2::setArea(const double &a) { this->surface = a; }
/*!
Set the precision of the gray level of the dot.
\param precision : It is a double precision float which value is in ]0,1]:
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - 1 means full precision, whereas values close to 0 show a very bad
+ accuracy.
- Values lower or equal to 0 are brought back to an epsilon>0
- Values higher than 1 are brought back to 1
If the initial gray level is I, the gray levels of the dot will be between :
@@ -780,48 +722,41 @@ void vpDot2::setArea( const double & a )
\sa setGrayLevelMin(), setGrayLevelMax()
*/
-void vpDot2::setGrayLevelPrecision( const double & precision )
+void vpDot2::setGrayLevelPrecision(const double &precision)
{
double epsilon = 0.05;
- if( grayLevelPrecision<epsilon )
- {
+ if (grayLevelPrecision < epsilon) {
this->grayLevelPrecision = epsilon;
- }
- else if( grayLevelPrecision>1 )
- {
+ } else if (grayLevelPrecision > 1) {
this->grayLevelPrecision = 1.0;
- }
- else
- {
+ } else {
this->grayLevelPrecision = precision;
}
}
/*!
- Set the precision of the size of the dot. Used to test the validity of the dot
+ Set the precision of the size of the dot. Used to test the validity of the
+ dot
\param precision : It is a double precision float which value is in [0,1]:
- this is the limit ratio between the tested parameter and the measured one.
- minSize = sizePrecision*originalSize ; maxSize = originalSize/sizePrecision ;
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ minSize = sizePrecision*originalSize ; maxSize =
+ originalSize/sizePrecision ;
+ - 1 means full precision, whereas values close to 0 show a very bad
+ accuracy.
- Values lower or equal to 0 are brought back to 0.
- Values higher than 1 are brought back to 1.
- To desactivate validity test set sizePrecision to 0
\sa setWidth(), setHeight(), setArea()
*/
-void vpDot2::setSizePrecision( const double & precision )
+void vpDot2::setSizePrecision(const double &precision)
{
- if( sizePrecision<0 )
- {
+ if (sizePrecision < 0) {
this->sizePrecision = 0;
- }
- else if( sizePrecision>1 )
- {
+ } else if (sizePrecision > 1) {
this->sizePrecision = 1.0;
- }
- else
- {
+ } else {
this->sizePrecision = precision;
}
}
@@ -830,22 +765,25 @@ void vpDot2::setSizePrecision( const double & precision )
Indicates if the dot should have an ellipsoid shape to be valid.
\param precision : It is a double precision float which value is in [0,1]:
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - 1 means full precision, whereas values close to 0 show a very bad
+ accuracy.
- Values lower or equal to 0 are brought back to 0.
- Values higher than 1 are brought back to 1.
To track a non ellipsoid shape use setEllipsoidShapePrecision(0).
- The following example show how to track a blob with a height constraint on an ellipsoid shape.
- The tracking will fail if the shape is not ellipsoid.
+ The following example show how to track a blob with a height constraint on
+ an ellipsoid shape. The tracking will fail if the shape is not ellipsoid.
\code
vpDot2 dot;
- dot.setEllipsoidShapePrecision(0.9); // to track a blob with a height constraint attendee on a circle shape
+ dot.setEllipsoidShapePrecision(0.9); // to track a blob with a height
+ constraint attendee on a circle shape
...
dot.track();
\endcode
- This other example shows how to remove any constraint on the shape. Here the tracker will be able to track
- any shape, including square or rectangular shapes.
+ 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
@@ -855,48 +793,41 @@ void vpDot2::setSizePrecision( const double & precision )
\sa getEllipsoidShapePrecision()
*/
-void vpDot2::setEllipsoidShapePrecision(const double & precision) {
+void vpDot2::setEllipsoidShapePrecision(const double &precision)
+{
- if( ellipsoidShapePrecision<0 )
- {
+ if (ellipsoidShapePrecision < 0) {
this->ellipsoidShapePrecision = 0;
- }
- else if( ellipsoidShapePrecision>1 )
- {
+ } else if (ellipsoidShapePrecision > 1) {
this->ellipsoidShapePrecision = 1.0;
- }
- else
- {
+ } else {
this->ellipsoidShapePrecision = precision;
}
}
/*!
- Set the precision of the search maximum distance to get the starting point on a dot border. A too low value
- mean a large search area.
+ Set the precision of the search maximum distance to get the starting point
+ on a dot border. A too low value mean a large search area.
- \param precision : It is a double precision float which value is in [0.05,1]:
+ \param precision : It is a double precision float which value is in
+ [0.05,1]:
- this is the limit ratio between the tested parameter and the measured one.
distance < getWidth()/(getSizePrecision()+epsilon);
- - 1 means full precision, whereas values close to 0 show a very bad accuracy.
+ - 1 means full precision, whereas values close to 0 show a very bad
+ accuracy.
- Values lower or equal to 0.05 are brought back to 0.05
- Values higher than 1 are brought back to 1.
*/
-void vpDot2::setMaxSizeSearchDistancePrecision( const double & precision )
+void vpDot2::setMaxSizeSearchDistancePrecision(const double &precision)
{
double epsilon = 0.05;
- if( maxSizeSearchDistancePrecision<epsilon )
- {
- this-> maxSizeSearchDistancePrecision = epsilon;
- }
- else if( maxSizeSearchDistancePrecision >1 )
- {
+ if (maxSizeSearchDistancePrecision < epsilon) {
+ this->maxSizeSearchDistancePrecision = epsilon;
+ } else if (maxSizeSearchDistancePrecision > 1) {
this->maxSizeSearchDistancePrecision = 1.0;
- }
- else
- {
+ } else {
this->maxSizeSearchDistancePrecision = precision;
}
}
@@ -909,11 +840,7 @@ void vpDot2::setMaxSizeSearchDistancePrecision( const double & precision )
\param I : Image.
*/
-void
-vpDot2::setArea(const vpImage<unsigned char> &I)
-{
- setArea(I, 0, 0, I.getWidth(), I.getHeight());
-}
+void vpDot2::setArea(const vpImage<unsigned char> &I) { setArea(I, 0, 0, I.getWidth(), I.getHeight()); }
/*!
@@ -927,22 +854,25 @@ vpDot2::setArea(const vpImage<unsigned char> &I)
\param h : Area height.
*/
-void
-vpDot2::setArea(const vpImage<unsigned char> &I,
- int u, int v,
- unsigned int w, unsigned int h)
+void vpDot2::setArea(const vpImage<unsigned char> &I, int u, int v, unsigned int w, unsigned int h)
{
unsigned int image_w = I.getWidth();
unsigned int image_h = I.getHeight();
// Bounds the area to the image
- if (u < 0) u = 0;
- else if (u >= (int)image_w) u = (int)image_w - 1;
- if (v < 0) v = 0;
- else if (v >= (int)image_h) v = (int)image_h - 1;
-
- if (((unsigned int)u + w) > image_w) w = image_w - (unsigned int)u - 1;
- if (((unsigned int)v + h) > image_h) h = image_h - (unsigned int)v - 1;
+ if (u < 0)
+ u = 0;
+ else if (u >= (int)image_w)
+ u = (int)image_w - 1;
+ if (v < 0)
+ v = 0;
+ else if (v >= (int)image_h)
+ v = (int)image_h - 1;
+
+ if (((unsigned int)u + w) > image_w)
+ w = image_w - (unsigned int)u - 1;
+ if (((unsigned int)v + h) > image_h)
+ h = image_h - (unsigned int)v - 1;
area.setRect(u, v, w, h);
}
@@ -954,11 +884,7 @@ vpDot2::setArea(const vpImage<unsigned char> &I,
\param a : Area.
*/
-void
- vpDot2::setArea(const vpRect & a)
-{
- area = a;
-}
+void vpDot2::setArea(const vpRect &a) { area = a; }
///// CLASS FUNCTIONALITY ////////////////////////////////////////////////////
@@ -974,7 +900,8 @@ void
\param niceDots: List of the dots that are found.
- Before calling this method, dot characteristics to found have to be set like:
+ Before calling this method, dot characteristics to found have to be set
+ like:
\code
vpDot2 d;
@@ -1009,17 +936,19 @@ void
}
\endcode
- \sa searchDotsInArea(vpImage<unsigned char>&, int, int, unsigned int, unsigned int, std::list<vpDot2> &)
+ \sa searchDotsInArea(vpImage<unsigned char>&, int, int, unsigned int,
+ unsigned int, std::list<vpDot2> &)
*/
-void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2> &niceDots)
+void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, std::list<vpDot2> &niceDots)
{
- searchDotsInArea( I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
+ searchDotsInArea(I, 0, 0, I.getWidth(), I.getHeight(), niceDots);
}
/*!
- Look for a list of dot matching this dot parameters within a region of interest
- defined by a rectangle in the image. The rectangle upper-left coordinates are given by
+ Look for a list of dot matching this dot parameters within a region of
+ interest defined by a rectangle in the image. The rectangle upper-left
+ coordinates are given by
(\e area_u, \e area_v). The size of the rectangle is given by \e area_w and
\e area_h.
@@ -1037,30 +966,26 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I, std::list<vpDot2>
\sa searchDotsInArea(vpImage<unsigned char>& I, std::list<vpDot2> &)
*/
-void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
- int area_u,
- int area_v,
- unsigned int area_w,
- unsigned int area_h,
- std::list<vpDot2> &niceDots)
+void vpDot2::searchDotsInArea(const vpImage<unsigned char> &I, int area_u, int area_v, unsigned int area_w,
+ unsigned int area_h, std::list<vpDot2> &niceDots)
{
// clear the list of nice dots
niceDots.clear();
- // Fit the input area in the image; we keep only the common part between this
- // area and the image.
+ // Fit the input area in the image; we keep only the common part between
+ // this area and the image.
setArea(I, area_u, area_v, area_w, area_h);
// compute the size of the search grid
unsigned int gridWidth;
unsigned int gridHeight;
- getGridSize( gridWidth, gridHeight );
+ getGridSize(gridWidth, gridHeight);
if (graphics) {
// Display the area were the dot is search
vpDisplay::displayRectangle(I, area, vpColor::blue, false, thickness);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
#ifdef DEBUG
@@ -1074,57 +999,56 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
std::list<vpDot2>::iterator itnice;
std::list<vpDot2>::iterator itbad;
- vpDot2* dotToTest = NULL;
+ vpDot2 *dotToTest = NULL;
vpDot2 tmpDot;
- unsigned int area_u_min = (unsigned int) area.getLeft();
- unsigned int area_u_max = (unsigned int) area.getRight();
- unsigned int area_v_min = (unsigned int) area.getTop();
- unsigned int area_v_max = (unsigned int) area.getBottom();
+ unsigned int area_u_min = (unsigned int)area.getLeft();
+ unsigned int area_u_max = (unsigned int)area.getRight();
+ unsigned int area_v_min = (unsigned int)area.getTop();
+ unsigned int area_v_max = (unsigned int)area.getBottom();
unsigned int u, v;
vpImagePoint cogTmpDot;
- for( v=area_v_min ; v<area_v_max ; v=v+gridHeight )
- {
- for( u=area_u_min ; u<area_u_max ; u=u+gridWidth )
- {
+ for (v = area_v_min; v < area_v_max; v = v + gridHeight) {
+ for (u = area_u_min; u < area_u_max; u = u + gridWidth) {
// if the pixel we're in doesn't have the right color (outside the
// graylevel interval), no need to check further, just get to the
// next grid intersection.
- if( !hasGoodLevel(I, u, v) ) continue;
+ if (!hasGoodLevel(I, u, v))
+ continue;
// Test if an other germ is inside the bounding box of a dot previously
// detected
bool good_germ = true;
itnice = niceDots.begin();
- while( itnice != niceDots.end() && good_germ == true) {
+ while (itnice != niceDots.end() && good_germ == true) {
tmpDot = *itnice;
cogTmpDot = tmpDot.getCog();
double u0 = cogTmpDot.get_u();
double v0 = cogTmpDot.get_v();
- double half_w = tmpDot.getWidth() / 2.;
+ double half_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;
}
- ++ itnice;
+ ++itnice;
}
- if (! good_germ)
+ if (!good_germ)
continue;
// Compute the right border position for this possible germ
unsigned int border_u;
unsigned int border_v;
- if(findFirstBorder(I, u, v, border_u, border_v) == false){
+ if (findFirstBorder(I, u, v, border_u, border_v) == false) {
// germ is not good.
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ // Jump all the pixels between v,u and v,
+ // dotToTest->getFirstBorder_u()
u = border_u;
v = border_v;
continue;
@@ -1134,32 +1058,34 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
#define vpBAD_DOT_VALUE (*itbad)
vpImagePoint cogBadDot;
- while( itbad != badDotsVector.end() && good_germ == true) {
- if( (double)u >= vpBAD_DOT_VALUE.bbox_u_min
- && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
- (double)v >= vpBAD_DOT_VALUE.bbox_v_min
- && (double)v <= vpBAD_DOT_VALUE.bbox_v_max){
+ while (itbad != badDotsVector.end() && good_germ == true) {
+ if ((double)u >= vpBAD_DOT_VALUE.bbox_u_min && (double)u <= vpBAD_DOT_VALUE.bbox_u_max &&
+ (double)v >= vpBAD_DOT_VALUE.bbox_v_min && (double)v <= vpBAD_DOT_VALUE.bbox_v_max) {
std::list<vpImagePoint>::const_iterator it_edges = ip_edges_list.begin();
- while (it_edges != ip_edges_list.end() && good_germ == true){
+ while (it_edges != ip_edges_list.end() && good_germ == true) {
// Test if the germ belong to a previously detected dot:
// - from the germ go right to the border and compare this
// position to the list of pixels of previously detected dots
cogBadDot = *it_edges;
- //if( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) {
- if( (std::fabs(border_u - cogBadDot.get_u()) <= vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u()))*std::numeric_limits<double>::epsilon() )
- &&
- (std::fabs(v - cogBadDot.get_v()) <= vpMath::maximum(std::fabs((double)v), std::fabs(cogBadDot.get_v()))*std::numeric_limits<double>::epsilon() )) {
+ // if( border_u == cogBadDot.get_u() && v == cogBadDot.get_v()) {
+ if ((std::fabs(border_u - cogBadDot.get_u()) <=
+ vpMath::maximum(std::fabs((double)border_u), std::fabs(cogBadDot.get_u())) *
+ std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(v - cogBadDot.get_v()) <=
+ vpMath::maximum(std::fabs((double)v), std::fabs(cogBadDot.get_v())) *
+ std::numeric_limits<double>::epsilon())) {
good_germ = false;
}
- ++ it_edges;
+ ++it_edges;
}
}
++itbad;
}
#undef vpBAD_DOT_VALUE
- if (! good_germ) {
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ if (!good_germ) {
+ // Jump all the pixels between v,u and v,
+ // dotToTest->getFirstBorder_u()
u = border_u;
v = border_v;
continue;
@@ -1168,68 +1094,68 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
vpTRACE(4, "Try germ (%d, %d)", u, v);
vpImagePoint germ;
- germ.set_u( u );
- germ.set_v( v );
+ germ.set_u(u);
+ germ.set_v(v);
// otherwise estimate the width, height and surface of the dot we
// created, and test it.
- if( dotToTest != NULL ) delete dotToTest;
+ if (dotToTest != NULL)
+ delete dotToTest;
dotToTest = getInstance();
- dotToTest->setCog( germ );
- dotToTest->setGrayLevelMin ( getGrayLevelMin() );
- dotToTest->setGrayLevelMax ( getGrayLevelMax() );
- dotToTest->setGrayLevelPrecision( getGrayLevelPrecision() );
- dotToTest->setSizePrecision( getSizePrecision() );
- dotToTest->setGraphics( graphics );
- dotToTest->setGraphicsThickness( thickness );
- dotToTest->setComputeMoments( true );
- dotToTest->setArea( area );
- dotToTest->setEllipsoidShapePrecision( ellipsoidShapePrecision );
- dotToTest->setEllipsoidBadPointsPercentage( allowedBadPointsPercentage_ );
+ dotToTest->setCog(germ);
+ dotToTest->setGrayLevelMin(getGrayLevelMin());
+ dotToTest->setGrayLevelMax(getGrayLevelMax());
+ dotToTest->setGrayLevelPrecision(getGrayLevelPrecision());
+ dotToTest->setSizePrecision(getSizePrecision());
+ dotToTest->setGraphics(graphics);
+ dotToTest->setGraphicsThickness(thickness);
+ dotToTest->setComputeMoments(true);
+ dotToTest->setArea(area);
+ dotToTest->setEllipsoidShapePrecision(ellipsoidShapePrecision);
+ dotToTest->setEllipsoidBadPointsPercentage(allowedBadPointsPercentage_);
// first compute the parameters of the dot.
// if for some reasons this caused an error tracking
// (dot partially out of the image...), check the next intersection
- if( dotToTest->computeParameters( I ) == false ) {
- // Jump all the pixels between v,u and v, dotToTest->getFirstBorder_u()
+ if (dotToTest->computeParameters(I) == false) {
+ // Jump all the pixels between v,u and v,
+ // dotToTest->getFirstBorder_u()
u = border_u;
v = border_v;
continue;
}
// if the dot to test is valid,
- if( dotToTest->isValid( I, *this ) )
- {
+ if (dotToTest->isValid(I, *this)) {
vpImagePoint cogDotToTest = dotToTest->getCog();
// Compute the distance to the center. The center used here is not the
// area center available by area.getCenter(area_center_u,
// area_center_v) but the center of the input area which may be
// partially outside the image.
- double area_center_u = area_u + area_w/2.0 - 0.5;
- double area_center_v = area_v + area_h/2.0 - 0.5;
+ double area_center_u = area_u + area_w / 2.0 - 0.5;
+ double area_center_v = area_v + area_h / 2.0 - 0.5;
double thisDiff_u = cogDotToTest.get_u() - area_center_u;
double thisDiff_v = cogDotToTest.get_v() - area_center_v;
- double thisDist = sqrt( thisDiff_u*thisDiff_u + thisDiff_v*thisDiff_v);
+ double thisDist = sqrt(thisDiff_u * thisDiff_u + thisDiff_v * thisDiff_v);
bool stopLoop = false;
itnice = niceDots.begin();
- while( itnice != niceDots.end() && stopLoop == false )
- {
+ while (itnice != niceDots.end() && stopLoop == false) {
tmpDot = *itnice;
- //double epsilon = 0.001; // detecte +sieurs points
+ // double epsilon = 0.001; // detecte +sieurs points
double epsilon = 3.0;
// if the center of the dot is the same than the current
// don't add it, test the next point of the grid
cogTmpDot = tmpDot.getCog();
- if( fabs( cogTmpDot.get_u() - cogDotToTest.get_u() ) < epsilon &&
- fabs( cogTmpDot.get_v() - cogDotToTest.get_v() ) < epsilon )
- {
+ if (fabs(cogTmpDot.get_u() - cogDotToTest.get_u()) < epsilon &&
+ fabs(cogTmpDot.get_v() - cogDotToTest.get_v()) < epsilon) {
stopLoop = true;
- // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
+ // Jump all the pixels between v,u and v,
+ // tmpDot->getFirstBorder_u()
u = border_u;
v = border_v;
continue;
@@ -1237,19 +1163,17 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
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,
// then add this dot before the current vector element.
- if( otherDist > thisDist )
- {
- niceDots.insert(itnice, *dotToTest );
- ++ itnice;
+ if (otherDist > thisDist) {
+ niceDots.insert(itnice, *dotToTest);
+ ++itnice;
stopLoop = true;
- // Jump all the pixels between v,u and v, tmpDot->getFirstBorder_u()
+ // Jump all the pixels between v,u and v,
+ // tmpDot->getFirstBorder_u()
u = border_u;
v = border_v;
continue;
@@ -1260,18 +1184,17 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
// if we reached the end of the vector without finding the dot
// or inserting it, insert it now.
- if( itnice == niceDots.end() && stopLoop == false )
- {
- niceDots.push_back( *dotToTest );
+ if (itnice == niceDots.end() && stopLoop == false) {
+ niceDots.push_back(*dotToTest);
}
- }
- else {
+ } else {
// Store bad dots
- badDotsVector.push_front( *dotToTest );
+ badDotsVector.push_front(*dotToTest);
}
}
}
- if( dotToTest != NULL ) delete dotToTest;
+ if (dotToTest != NULL)
+ delete dotToTest;
}
/*!
@@ -1294,7 +1217,7 @@ void vpDot2::searchDotsInArea(const vpImage<unsigned char>& I,
\param wantedDot : Wanted dot passed in.
*/
-bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
+bool vpDot2::isValid(const vpImage<unsigned char> &I, const vpDot2 &wantedDot)
{
double size_precision = wantedDot.getSizePrecision();
double ellipsoidShape_precision = wantedDot.getEllipsoidShapePrecision();
@@ -1303,97 +1226,74 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
// First, check the width, height and surface of the dot. Those parameters
// must be the same.
//
- //if ( (wantedDot.getWidth() != 0)
+ // if ( (wantedDot.getWidth() != 0)
// && (wantedDot.getHeight() != 0)
// && (wantedDot.getArea() != 0) )
- if ( (std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon()) )
- // if (size_precision!=0){
+ if ((std::fabs(wantedDot.getWidth()) > std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(wantedDot.getHeight()) > std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(wantedDot.getArea()) > std::numeric_limits<double>::epsilon()))
+ // if (size_precision!=0){
{
- if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()){
+ if (std::fabs(size_precision) > std::numeric_limits<double>::epsilon()) {
double epsilon = 0.001;
#ifdef DEBUG
std::cout << "test size precision......................\n";
- std::cout << "wanted dot: " << "w=" << wantedDot.getWidth()
- << " h=" << wantedDot.getHeight()
- << " s=" << wantedDot.getArea()
- << " precision=" << size_precision
- << " epsilon=" << epsilon << std::endl;
- std::cout << "dot found: " << "w=" << getWidth()
- << " h=" << getHeight()
- << " s=" << getArea() << std::endl;
+ std::cout << "wanted dot: "
+ << "w=" << wantedDot.getWidth() << " h=" << wantedDot.getHeight() << " s=" << wantedDot.getArea()
+ << " precision=" << size_precision << " epsilon=" << epsilon << std::endl;
+ std::cout << "dot found: "
+ << "w=" << getWidth() << " h=" << getHeight() << " s=" << getArea() << std::endl;
#endif
- if( ( wantedDot.getWidth()*size_precision-epsilon < getWidth() ) == false )
- {
- vpDEBUG_TRACE(3, "Bad width > for dot (%g, %g)",
- cog.get_u(), cog.get_v());
+ 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),
+ 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());
+ printf("Bad surface %g < %g for dot (%g, %g)\n", getArea(),
+ wantedDot.getArea() / (size_precision * size_precision + epsilon), cog.get_u(), cog.get_v());
#endif
return false;
}
@@ -1406,14 +1306,15 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
//
int nb_point_to_test = 20; // Nb points to test on inner and outside ellipsoid
int nb_bad_points = 0;
- int nb_max_bad_points = (int)(nb_point_to_test*allowedBadPointsPercentage_);
- double step_angle = 2*M_PI / nb_point_to_test;
+ int nb_max_bad_points = (int)(nb_point_to_test * allowedBadPointsPercentage_);
+ double step_angle = 2 * M_PI / nb_point_to_test;
// if (ellipsoidShape_precision != 0 && compute_moment) {
if (std::fabs(ellipsoidShape_precision) > std::numeric_limits<double>::epsilon() && compute_moment) {
// std::cout << "test shape precision......................\n";
// See F. Chaumette. Image moments: a general and useful set of features
- // for visual servoing. IEEE Trans. on Robotics, 20(4):713-723, August 2004.
+ // for visual servoing. IEEE Trans. on Robotics, 20(4):713-723, August
+ // 2004.
// mu11 = m11 - m00 * xg * yg = m11 - m00 * m10/m00 * m01/m00
// = m11 - m10 * m01 / m00
@@ -1426,57 +1327,55 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
//
// a2^2 = 2 / m00 * (mu02 + mu20 - sqrt( (mu20 - mu02)^2 + 4mu11^2) )
- //we compute parameters of the estimated ellipse
- double tmp1 = (m01*m01 -m10*m10)/m00+(m20-m02);
- double tmp2 = m11 -m10*m01/m00 ;
- double Sqrt = sqrt(tmp1*tmp1 + 4*tmp2*tmp2);
- double a1 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 + Sqrt));
- double a2 = sqrt(2/m00*((m20+m02)-(m10*m10+m01*m01)/m00 - Sqrt));
- double alpha = 0.5*atan2(2*(m11*m00-m10*m01),
- ((m20-m02)*m00-m10*m10+m01*m01));
+ // we compute parameters of the estimated ellipse
+ double tmp1 = (m01 * m01 - m10 * m10) / m00 + (m20 - m02);
+ double tmp2 = m11 - m10 * m01 / m00;
+ double Sqrt = sqrt(tmp1 * tmp1 + 4 * tmp2 * tmp2);
+ double a1 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 + Sqrt));
+ double a2 = sqrt(2 / m00 * ((m20 + m02) - (m10 * m10 + m01 * m01) / m00 - Sqrt));
+ double alpha = 0.5 * atan2(2 * (m11 * m00 - m10 * m01), ((m20 - m02) * m00 - m10 * m10 + m01 * m01));
// to be able to track small dots, minorize the ellipsoid radius for the
// inner test
a1 -= 1.0;
a2 -= 1.0;
- double innerCoef = ellipsoidShape_precision ;
+ double innerCoef = ellipsoidShape_precision;
unsigned int u, v;
double cog_u = this->cog.get_u();
double cog_v = this->cog.get_v();
vpImagePoint ip;
nb_bad_points = 0;
- for( double theta = 0. ; theta<2*M_PI ; theta+= step_angle ) {
- u = (unsigned int) (cog_u + innerCoef*(a1*cos(alpha)*cos(theta)-a2*sin(alpha)*sin(theta)));
- v = (unsigned int) (cog_v + innerCoef*(a1*sin(alpha)*cos(theta)+a2*cos(alpha)*sin(theta)));
- if( ! this->hasGoodLevel( I, u, v) ) {
- // vpTRACE("Inner cercle pixel (%d, %d) has bad level for dot (%g, %g)",
- // u, v, cog_u, cog_v);
+ for (double theta = 0.; theta < 2 * M_PI; theta += step_angle) {
+ u = (unsigned int)(cog_u + innerCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta)));
+ v = (unsigned int)(cog_v + innerCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta)));
+ if (!this->hasGoodLevel(I, u, v)) {
+// vpTRACE("Inner cercle pixel (%d, %d) has bad level for dot (%g, %g)",
+// u, v, cog_u, cog_v);
#ifdef DEBUG
- printf("Inner cercle pixel (%u, %u) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
+ printf("Inner cercle pixel (%u, %u) has bad level for dot (%g, %g): "
+ "%d not in [%u, %u]\n",
u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
#endif
- //return false;
- nb_bad_points ++;
+ // return false;
+ nb_bad_points++;
}
if (graphics) {
- for (unsigned int t=0; t< thickness; t++) {
- ip.set_u( u + t );
- ip.set_v( v );
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ for (unsigned int t = 0; t < thickness; t++) {
+ ip.set_u(u + t);
+ ip.set_v(v);
+ vpDisplay::displayPoint(I, ip, vpColor::green);
}
}
#ifdef DEBUG
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ vpDisplay::displayPoint(I, ip, vpColor::green);
vpDisplay::flush(I);
#endif
}
- if (nb_bad_points > nb_max_bad_points)
- {
+ 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;
}
@@ -1485,46 +1384,45 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
a1 += 2.0;
a2 += 2.0;
- double outCoef = 2-ellipsoidShape_precision; //1.6;
+ 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)));
+ for (double theta = 0.; theta < 2 * M_PI; theta += step_angle) {
+ u = (unsigned int)(cog_u + outCoef * (a1 * cos(alpha) * cos(theta) - a2 * sin(alpha) * sin(theta)));
+ v = (unsigned int)(cog_v + outCoef * (a1 * sin(alpha) * cos(theta) + a2 * cos(alpha) * sin(theta)));
#ifdef DEBUG
- //vpDisplay::displayRectangle(I, area, vpColor::yellow);
- vpDisplay::displayCross( I, (int) v, (int) u, 7, vpColor::purple ) ;
+ // 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 ) ) {
- // vpTRACE("Outside cercle pixel (%d, %d) has bad level for dot (%g, %g)",
- // u, v, cog_u, cog_v);
+ if (!this->hasReverseLevel(I, u, v)) {
+// vpTRACE("Outside cercle pixel (%d, %d) has bad level for dot (%g,
+// %g)", u, v, cog_u, cog_v);
#ifdef DEBUG
- printf("Outside cercle pixel (%u, %u) has bad level for dot (%g, %g): %d not in [%d, %d]\n",
+ printf("Outside cercle pixel (%u, %u) has bad level for dot (%g, "
+ "%g): %d not in [%u, %u]\n",
u, v, cog_u, cog_v, I[v][u], gray_level_min, gray_level_max);
#endif
- nb_bad_points ++;
- //return false;
+ nb_bad_points++;
+ // return false;
}
if (graphics) {
- for(unsigned int t=0; t<thickness; t++) {
- ip.set_u( u + t);
- ip.set_v( v );
+ for (unsigned int t = 0; t < thickness; t++) {
+ ip.set_u(u + t);
+ ip.set_v(v);
- vpDisplay::displayPoint( I, ip, vpColor::green ) ;
+ vpDisplay::displayPoint(I, ip, vpColor::green);
}
}
}
}
- if (nb_bad_points > nb_max_bad_points)
- {
+ 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;
}
@@ -1532,8 +1430,6 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
return true;
}
-
-
/*!
Check if a the pixel of coordinates (u, v) is in the image and has
@@ -1544,31 +1440,26 @@ bool vpDot2::isValid(const vpImage<unsigned char>& I, const vpDot2& wantedDot )
\param v : Pixel to test.
\return true : If the pixel of coordinates (u, v) is in the area and
- has a value between the min and max levels fixed by setGrayLevelMin() and setGrayLevelMax().
+ has a value between the min and max levels fixed by setGrayLevelMin() and
+ setGrayLevelMax().
\return false : Otherwise
\sa setGrayLevelMin(), setGrayLevelMax()
*/
-bool vpDot2::hasGoodLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const
+bool vpDot2::hasGoodLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
{
- if( !isInArea( u, v ) )
+ if (!isInArea(u, v))
return false;
- if( I[v][u] >= gray_level_min && I[v][u] <= gray_level_max)
- {
+ if (I[v][u] >= gray_level_min && I[v][u] <= gray_level_max) {
return true;
- }
- else
- {
+ } else {
return false;
}
}
-
/*!
Check if a the pixel of coordinates (u, v) in the image has a good level to
@@ -1581,25 +1472,19 @@ bool vpDot2::hasGoodLevel(const vpImage<unsigned char>& I,
\return true if it is so, and false otherwise.
*/
-bool vpDot2::hasReverseLevel(const vpImage<unsigned char>& I,
- const unsigned int &u,
- const unsigned int &v) const
+bool vpDot2::hasReverseLevel(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v) const
{
- if( !isInArea( u, v ) )
+ if (!isInArea(u, v))
return false;
- if( I[v][u] < gray_level_min || I[v][u] > gray_level_max)
- {
+ if (I[v][u] < gray_level_min || I[v][u] > gray_level_max) {
return true;
- }
- else
- {
+ } else {
return false;
}
}
-
/*!
Return a new instance of vpDot2.
@@ -1608,10 +1493,7 @@ bool vpDot2::hasReverseLevel(const vpImage<unsigned char>& I,
\return An instance of vpDot2.
*/
-vpDot2* vpDot2::getInstance()
-{
- return new vpDot2();
-}
+vpDot2 *vpDot2::getInstance() { return new vpDot2(); }
/*!
@@ -1628,12 +1510,7 @@ vpDot2* vpDot2::getInstance()
- 6 : down
- 7 : down right
*/
-void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const
-{
- freeman_chain = direction_list;
-}
-
-
+void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const { freeman_chain = direction_list; }
/******************************************************************************
*
@@ -1641,8 +1518,6 @@ void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const
*
******************************************************************************/
-
-
/*!
Compute all the parameters of the dot (center, width, height, surface,
@@ -1674,9 +1549,7 @@ void vpDot2::getFreemanChain(std::list<unsigned int> &freeman_chain) const
\sa getFirstBorder_u(), getFirstBorder_v()
*/
-bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
- const double &_u,
- const double &_v)
+bool vpDot2::computeParameters(const vpImage<unsigned char> &I, const double &_u, const double &_v)
{
direction_list.clear();
ip_edges_list.clear();
@@ -1685,25 +1558,24 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
double est_v = _v;
// if u has default value, set it to the actual center value
- //if( est_u == -1.0 )
- if( std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u),1.)*std::numeric_limits<double>::epsilon() )
- {
+ // if( est_u == -1.0 )
+ if (std::fabs(est_u + 1.0) <= vpMath::maximum(std::fabs(est_u), 1.) * std::numeric_limits<double>::epsilon()) {
est_u = this->cog.get_u();
}
// if v has default value, set it to the actual center value
- //if( est_v == -1.0 )
- if( std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v),1.)*std::numeric_limits<double>::epsilon() )
- {
+ // if( est_v == -1.0 )
+ if (std::fabs(est_v + 1.0) <= vpMath::maximum(std::fabs(est_v), 1.) * std::numeric_limits<double>::epsilon()) {
est_v = this->cog.get_v();
}
- // if the estimated position of the dot is out of the image, not need to continue,
- // return an error tracking
- if( !isInArea( (unsigned int) est_u, (unsigned int) est_v ) )
- {
- vpDEBUG_TRACE(3, "Initial pixel coordinates (%d, %d) for dot tracking are not in the area",
- (int) est_u, (int) est_v) ;
+ // if the estimated position of the dot is out of the image, not need to
+ // continue, return an error tracking
+ if (!isInArea((unsigned int)est_u, (unsigned int)est_v)) {
+ vpDEBUG_TRACE(3,
+ "Initial pixel coordinates (%d, %d) for dot tracking are "
+ "not in the area",
+ (int)est_u, (int)est_v);
return false;
}
@@ -1714,20 +1586,16 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &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) ;
+ if (!hasGoodLevel(I, (unsigned int)est_u, (unsigned int)est_v)) {
+ vpDEBUG_TRACE(3, "Can't find a dot from pixel (%d, %d) coordinates", (int)est_u, (int)est_v);
return false;
}
// find the border
- if(!findFirstBorder(I, (unsigned int) est_u, (unsigned int) est_v,
- this->firstBorder_u, this->firstBorder_v)) {
+ 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;
}
@@ -1738,20 +1606,19 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
unsigned int firstDir = dir;
// if we are now out of the image, return an error tracking
- if( !isInArea( this->firstBorder_u, this->firstBorder_v ) )
- {
- vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area",
- this->firstBorder_u, this->firstBorder_v);
+ if (!isInArea(this->firstBorder_u, this->firstBorder_v)) {
+ vpDEBUG_TRACE(3, "Border pixel coordinates (%d, %d) of the dot are not in the area", this->firstBorder_u,
+ this->firstBorder_v);
return false;
}
// store the new direction and dot border coordinates.
- direction_list.push_back( dir );
+ direction_list.push_back(dir);
vpImagePoint ip;
- ip.set_u( this->firstBorder_u );
- ip.set_v( this->firstBorder_v );
+ ip.set_u(this->firstBorder_u);
+ ip.set_v(this->firstBorder_v);
- ip_edges_list.push_back( ip );
+ ip_edges_list.push_back(ip);
int border_u = (int)this->firstBorder_u;
int border_v = (int)this->firstBorder_v;
@@ -1771,13 +1638,13 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
do {
// if it was asked, show the border
if (graphics) {
- for(int t=0; t< (int)thickness; t++) {
- ip.set_u ( border_u + t);
- ip.set_v ( border_v );
-
- vpDisplay::displayPoint(I, ip, vpColor::red) ;
+ for (int t = 0; t < (int)thickness; t++) {
+ ip.set_u(border_u + t);
+ ip.set_v(border_v);
+
+ vpDisplay::displayPoint(I, ip, vpColor::red);
}
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
#ifdef DEBUG
vpDisplay::displayPoint(I, border_v, border_u, vpColor::red);
@@ -1785,14 +1652,14 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
#endif
// Determine the increments for the parameters
computeFreemanParameters(border_u, border_v, dir, du, dv,
- dS, // surface
- dMu, dMv, // first order moments
+ dS, // surface
+ dMu, dMv, // first order moments
dMuv, dMu2, dMv2); // second order moment
// Update the parameters
border_u += du; // Next position on the border
border_v += dv;
- m00 += dS; // enclosed area
+ m00 += dS; // enclosed area
m10 += dMu; // First order moment along v axis
m01 += dMv; // First order moment along u axis
if (compute_moment) {
@@ -1801,7 +1668,7 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
m02 += dMv2; // Second order moment along u axis
}
// if we are now out of the image, return an error tracking
- if( !isInArea( (unsigned int)border_u, (unsigned int)border_v ) ) {
+ if (!isInArea((unsigned int)border_u, (unsigned int)border_v)) {
vpDEBUG_TRACE(3, "Dot (%d, %d) is not in the area", border_u, border_v);
// Can Occur on a single pixel dot located on the top border
@@ -1810,34 +1677,36 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
// store the new direction and dot border coordinates.
- direction_list.push_back( dir );
+ direction_list.push_back(dir);
- ip.set_u( border_u );
- ip.set_v( border_v );
- ip_edges_list.push_back( ip );
+ ip.set_u(border_u);
+ ip.set_v(border_v);
+ ip_edges_list.push_back(ip);
// vpDisplay::getClick(I);
// update the extreme point of the dot.
- if( border_v < bbox_v_min ) bbox_v_min = border_v;
- if( border_v > bbox_v_max ) bbox_v_max = border_v;
- if( border_u < bbox_u_min ) bbox_u_min = border_u;
- if( border_u > bbox_u_max ) bbox_u_max = border_u;
+ if (border_v < bbox_v_min)
+ bbox_v_min = border_v;
+ if (border_v > bbox_v_max)
+ bbox_v_max = border_v;
+ if (border_u < bbox_u_min)
+ bbox_u_min = border_u;
+ if (border_u > bbox_u_max)
+ bbox_u_max = border_u;
// move around the tracked entity by following the border.
if (computeFreemanChainElement(I, (unsigned int)border_u, (unsigned int)border_v, dir) == false) {
- vpDEBUG_TRACE(3, "Can't compute Freeman chain for dot (%d, %d)",
- border_u, border_v);
+ 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);
+ // vpTRACE("border_u: %d border_v: %d dir: %d", border_u, border_v,
+ // dir);
- }
- while( (getFirstBorder_u() != (unsigned int)border_u
- || getFirstBorder_v() != (unsigned int)border_v
- || firstDir != dir) &&
- isInArea( (unsigned int)border_u, (unsigned int)border_v ) );
+ } while ((getFirstBorder_u() != (unsigned int)border_u || getFirstBorder_v() != (unsigned int)border_v ||
+ firstDir != dir) &&
+ isInArea((unsigned int)border_u, (unsigned int)border_v));
#ifdef VP_DEBUG
#if VP_DEBUG_MODE == 3
@@ -1847,49 +1716,44 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
// if the surface is one or zero , the center of gravity wasn't properly
// detected. Return an error tracking.
- //if( m00 == 0 || m00 == 1 )
- if( std::fabs(m00) <= std::numeric_limits<double>::epsilon()
- || std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.)*std::numeric_limits<double>::epsilon() )
- {
+ // if( m00 == 0 || m00 == 1 )
+ if (std::fabs(m00) <= std::numeric_limits<double>::epsilon() ||
+ std::fabs(m00 - 1.) <= vpMath::maximum(std::fabs(m00), 1.) * std::numeric_limits<double>::epsilon()) {
vpDEBUG_TRACE(3, "The center of gravity of the dot wasn't properly detected");
return false;
- }
- else // compute the center
+ } else // compute the center
{
// this magic formula gives the coordinates of the center of gravity
double tmpCenter_u = m10 / m00;
double tmpCenter_v = m01 / m00;
-
- //Updates the central moments
- if (compute_moment)
- {
- mu11 = m11 - tmpCenter_u*m01;
- mu02 = m02 - tmpCenter_v*m01;
- mu20 = m20 - tmpCenter_u*m10;
- }
+ // Updates the central moments
+ if (compute_moment) {
+ mu11 = m11 - tmpCenter_u * m01;
+ mu02 = m02 - tmpCenter_v * m01;
+ mu20 = m20 - tmpCenter_u * m10;
+ }
// check the center is in the image... never know...
// if( !hasGoodLevel( I, (unsigned int)tmpCenter_u,
// (unsigned int)tmpCenter_v ) )
// {
- // vpDEBUG_TRACE(3, "The center of gravity of the dot (%g, %g) has not a good in level", tmpCenter_u, tmpCenter_v);
- // return false;
+ // vpDEBUG_TRACE(3, "The center of gravity of the dot (%g, %g) has
+ // not a good in level", tmpCenter_u, tmpCenter_v); return false;
// }
- cog.set_u( tmpCenter_u );
- cog.set_v( tmpCenter_v );
+ cog.set_u(tmpCenter_u);
+ cog.set_v(tmpCenter_v);
}
- width = bbox_u_max - bbox_u_min + 1;
- height = bbox_v_max - bbox_v_min + 1;
+ width = bbox_u_max - bbox_u_min + 1;
+ height = bbox_v_max - bbox_v_min + 1;
surface = m00;
computeMeanGrayLevel(I);
return true;
}
-
/*!
Find the starting point on a dot border from an other point in the dot.
the dot border is computed from this point.
@@ -1900,17 +1764,13 @@ bool vpDot2::computeParameters(const vpImage<unsigned char> &I,
\param border_u : The row coordinate of the found starting point.
\param border_v : The column coordinate of the found starting point.
- \return false if the width of this dot was initialised and we already crossed
-the dot on more than the max possible width. Return true if success.
+ \return false if the width of this dot was initialised and we already
+crossed the dot on more than the max possible width. Return true if success.
\sa computeParameters()
*/
-bool
- vpDot2::findFirstBorder(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &border_u,
- unsigned int &border_v)
+bool vpDot2::findFirstBorder(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
+ unsigned int &border_u, unsigned int &border_v)
{
// find the border
@@ -1920,22 +1780,24 @@ bool
// work with double when navigating around the dot.
border_u = u;
border_v = v;
- double epsilon =0.001;
+ double epsilon = 0.001;
#ifdef DEBUG
std::cout << "gray level: " << gray_level_min << " " << gray_level_max << std::endl;
#endif
- while( hasGoodLevel( I, border_u+1, border_v ) &&
- border_u < area.getRight()/*I.getWidth()*/ ) {
+ while (hasGoodLevel(I, border_u + 1, border_v) && border_u < area.getRight() /*I.getWidth()*/) {
// if the width of this dot was initialised and we already crossed the dot
// on more than the max possible width, no need to continue, return an
// error tracking
- if( getWidth() > 0 && ( border_u - u ) > getWidth()/(getMaxSizeSearchDistancePrecision()+epsilon) ) {
- vpDEBUG_TRACE(3, "The found dot (%d, %d, %d) has a greater width than the required one", u, v, border_u);
+ if (getWidth() > 0 && (border_u - u) > getWidth() / (getMaxSizeSearchDistancePrecision() + epsilon)) {
+ vpDEBUG_TRACE(3,
+ "The found dot (%d, %d, %d) has a greater width than the "
+ "required one",
+ u, v, border_u);
return false;
}
#ifdef DEBUG
- vpDisplay::displayPoint(I, (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
@@ -1944,97 +1806,85 @@ bool
return true;
}
-
/*!
Considering a pixel (u, v) compute the next element of the Freeman chain
code.
- According to the gray level of pixel (u, v) and his eight neighbors determine
- the next element of the chain in order to turn around the dot
+ According to the gray level of pixel (u, v) and his eight neighbors
+ determine the next element of the chain in order to turn around the dot
counterclockwise.
\param I : The image we are working with.
\param v : The row coordinate of a pixel on a border.
\param u : The column coordinate of the pixel on a border.
- \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
- with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
+ \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6,
+ 7) with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
\return false if an element cannot be found. Occurs for example with an area
constituted by a single pixel. Return true if success.
*/
-bool
- vpDot2::computeFreemanChainElement(const vpImage<unsigned char> &I,
- const unsigned int &u,
- const unsigned int &v,
- unsigned int &element)
+bool vpDot2::computeFreemanChainElement(const vpImage<unsigned char> &I, const unsigned int &u, const unsigned int &v,
+ unsigned int &element)
{
- if (hasGoodLevel( I, u, v )) {
+ if (hasGoodLevel(I, u, v)) {
unsigned int _u = u;
unsigned int _v = v;
// get the point on the right of the point passed in
- updateFreemanPosition( _u, _v, (element + 2) %8 );
- if (hasGoodLevel( I, _u, _v )) {
- element = (element + 2) % 8; // turn right
- }
- else {
+ updateFreemanPosition(_u, _v, (element + 2) % 8);
+ if (hasGoodLevel(I, _u, _v)) {
+ element = (element + 2) % 8; // turn right
+ } else {
unsigned int _u1 = u;
unsigned int _v1 = v;
- updateFreemanPosition( _u1, _v1, (element + 1) %8 );
+ updateFreemanPosition(_u1, _v1, (element + 1) % 8);
- if ( hasGoodLevel( I, _u1, _v1 )) {
- element = (element + 1) % 8; // turn diag right
- }
- else {
+ if (hasGoodLevel(I, _u1, _v1)) {
+ element = (element + 1) % 8; // turn diag right
+ } else {
unsigned int _u2 = u;
unsigned int _v2 = v;
- updateFreemanPosition( _u2, _v2, element ); // same direction
+ updateFreemanPosition(_u2, _v2, element); // same direction
- if ( hasGoodLevel( I, _u2, _v2 )) {
- //element = element; // keep same dir
- }
- else {
+ if (hasGoodLevel(I, _u2, _v2)) {
+ // element = element; // keep same dir
+ } else {
unsigned int _u3 = u;
unsigned int _v3 = v;
- updateFreemanPosition( _u3, _v3, (element + 7) %8 ); // diag left
+ updateFreemanPosition(_u3, _v3, (element + 7) % 8); // diag left
- if ( hasGoodLevel( I, _u3, _v3 )) {
- element = (element + 7) %8; // turn diag left
- }
- else {
+ if (hasGoodLevel(I, _u3, _v3)) {
+ element = (element + 7) % 8; // turn diag left
+ } else {
unsigned int _u4 = u;
unsigned int _v4 = v;
- updateFreemanPosition( _u4, _v4, (element + 6) %8 ); // left
+ updateFreemanPosition(_u4, _v4, (element + 6) % 8); // left
- if ( hasGoodLevel( I, _u4, _v4 )) {
- element = (element + 6) %8 ; // turn left
- }
- else {
+ if (hasGoodLevel(I, _u4, _v4)) {
+ element = (element + 6) % 8; // turn left
+ } else {
unsigned int _u5 = u;
unsigned int _v5 = v;
- updateFreemanPosition( _u5, _v5, (element + 5) %8 ); // left
+ updateFreemanPosition(_u5, _v5, (element + 5) % 8); // left
- if ( hasGoodLevel( I, _u5, _v5 )) {
- element = (element + 5) %8 ; // turn diag down
- }
- else {
+ if (hasGoodLevel(I, _u5, _v5)) {
+ element = (element + 5) % 8; // turn diag down
+ } else {
unsigned int _u6 = u;
unsigned int _v6 = v;
- updateFreemanPosition( _u6, _v6, (element + 4) %8 ); // left
+ updateFreemanPosition(_u6, _v6, (element + 4) % 8); // left
- if ( hasGoodLevel( I, _u6, _v6 )) {
- element = (element + 4) %8 ; // turn down
- }
- else {
+ if (hasGoodLevel(I, _u6, _v6)) {
+ element = (element + 4) % 8; // turn down
+ } else {
unsigned int _u7 = u;
unsigned int _v7 = v;
- updateFreemanPosition( _u7, _v7, (element + 3) %8 ); // diag
+ updateFreemanPosition(_u7, _v7, (element + 3) % 8); // diag
- if ( hasGoodLevel( I, _u7, _v7 )) {
- element = (element + 3) %8 ; // turn diag right down
- }
- else {
+ if (hasGoodLevel(I, _u7, _v7)) {
+ element = (element + 3) % 8; // turn diag right down
+ } else {
// No neighbor with a good level
//
return false;
@@ -2053,7 +1903,6 @@ bool
}
return true;
-
}
/*!
@@ -2076,26 +1925,19 @@ bool
\param dMuv : Moment increases. Cumulated values of dMuv gives m11.
- \param dMu2 : Second order moment along v axis increases. Cumulated values of
- dMu2 gives m20.
+ \param dMu2 : Second order moment along v axis increases. Cumulated values
+ of dMu2 gives m20.
- \param dMv2 : Second order moment along u axis increases. Cumulated values of
- dMv2 gives m02.
+ \param dMv2 : Second order moment along u axis increases. Cumulated values
+ of dMv2 gives m02.
Considering the previous coordinates (u_p, v_p) of a pixel on a border, the
next coordinates (u, v) are given by: u = u_p + du and v = v_p + dv
*/
-void
- vpDot2::computeFreemanParameters(const int &u_p,
- const int &v_p,
- unsigned int &element,
- int &du, int &dv,
- float &dS,
- float &dMu, float &dMv,
- float &dMuv,
- float &dMu2, float &dMv2)
+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;
@@ -2106,22 +1948,22 @@ void
/*
3 2 1
\ | /
- \|/
+ \|/
4 ------- 0
/|\
/ | \
5 6 7
*/
- switch(element) {
+ switch (element) {
case 0: // go right
du = 1;
- dS = (float) v_p;
+ dS = (float)v_p;
dMu = 0.0;
dMv = (float)(0.5 * v_p * v_p);
if (compute_moment) {
dMuv = (float)(0.25 * v_p * v_p * (2 * u_p + 1));
dMu2 = 0;
- dMv2 = (float)(1.0/ 3. * v_p * v_p * v_p);
+ dMv2 = (float)(1.0 / 3. * v_p * v_p * v_p);
}
break;
@@ -2129,24 +1971,24 @@ void
du = 1;
dv = 1;
dS = (float)(v_p + 0.5);
- dMu = - (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
- dMv = (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
+ dMu = -(float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0);
+ dMv = (float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0);
if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25+half_u_p) + v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
- dMu2 = (float)(-1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)( 1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1./12.0);
+ float half_u_p = (float)(0.5 * u_p);
+ dMuv = (float)(v_p * v_p * (0.25 + half_u_p) + v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125);
+ dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) - 1. / 12.0);
+ dMv2 = (float)(1. / 3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) + 1. / 12.0);
}
break;
case 2: // go top
dv = 1;
dS = 0.0;
- dMu = (float)(- 0.5 * u_p * u_p);
+ dMu = (float)(-0.5 * u_p * u_p);
dMv = 0.0;
if (compute_moment) {
dMuv = 0;
- dMu2 = (float)(-1.0/ 3. * u_p * u_p * u_p);
+ dMu2 = (float)(-1.0 / 3. * u_p * u_p * u_p);
dMv2 = 0;
}
break;
@@ -2154,51 +1996,51 @@ void
case 3:
du = -1;
dv = 1;
- dS = (float)(- v_p - 0.5);
- dMu = - (float)(0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
- dMv = - (float)(0.5 * v_p * ( v_p + 1 ) + 1.0 / 6.0);
+ dS = (float)(-v_p - 0.5);
+ dMu = -(float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0);
+ dMv = -(float)(0.5 * v_p * (v_p + 1) + 1.0 / 6.0);
if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25-half_u_p) + v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
- dMu2 = (float)(-1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)(-1./3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1./12.0);
+ float half_u_p = (float)(0.5 * u_p);
+ dMuv = (float)(v_p * v_p * (0.25 - half_u_p) + v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125);
+ dMu2 = (float)(-1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0);
+ dMv2 = (float)(-1. / 3. * v_p * (v_p * v_p + 1.5 * v_p + 1.) - 1. / 12.0);
}
break;
case 4:
du = -1;
- dS = (float)(- v_p);
- dMv = (float)(- 0.5 * v_p * v_p);
+ dS = (float)(-v_p);
+ dMv = (float)(-0.5 * v_p * v_p);
dMu = 0.0;
if (compute_moment) {
dMuv = (float)(-0.25 * v_p * v_p * (2 * u_p - 1));
dMu2 = 0;
- dMv2 = (float)(-1.0/ 3. * v_p * v_p * v_p);
+ dMv2 = (float)(-1.0 / 3. * v_p * v_p * v_p);
}
break;
case 5:
du = -1;
dv = -1;
- dS = (float)(- v_p + 0.5);
- dMu = (float)( 0.5 * u_p * ( u_p - 1 ) + 1.0 / 6.0);
- dMv = (float)(- (0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0));
+ dS = (float)(-v_p + 0.5);
+ dMu = (float)(0.5 * u_p * (u_p - 1) + 1.0 / 6.0);
+ dMv = (float)(-(0.5 * v_p * (v_p - 1) + 1.0 / 6.0));
if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25-half_u_p) - v_p*(1./3.-half_u_p) - 1./6.*u_p +0.125);
- dMu2 = (float)( 1./3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1./12.0);
- dMv2 = (float)(-1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
+ float half_u_p = (float)(0.5 * u_p);
+ dMuv = (float)(v_p * v_p * (0.25 - half_u_p) - v_p * (1. / 3. - half_u_p) - 1. / 6. * u_p + 0.125);
+ dMu2 = (float)(1. / 3. * u_p * (u_p * u_p - 1.5 * u_p + 1.) - 1. / 12.0);
+ dMv2 = (float)(-1. / 3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1. / 12.0);
}
break;
case 6:
dv = -1;
dS = 0.0;
- dMu = (float)(0.5 * u_p * u_p);
+ dMu = (float)(0.5 * u_p * u_p);
dMv = 0.0;
if (compute_moment) {
dMuv = 0;
- dMu2 = (float)(1.0/ 3. * u_p * u_p * u_p);
+ dMu2 = (float)(1.0 / 3. * u_p * u_p * u_p);
dMv2 = 0;
}
break;
@@ -2207,19 +2049,18 @@ void
du = 1;
dv = -1;
dS = (float)(v_p - 0.5);
- dMu = (float)(0.5 * u_p * ( u_p + 1 ) + 1.0 / 6.0);
- dMv = (float)(0.5 * v_p * ( v_p - 1 ) + 1.0 / 6.0);
+ dMu = (float)(0.5 * u_p * (u_p + 1) + 1.0 / 6.0);
+ dMv = (float)(0.5 * v_p * (v_p - 1) + 1.0 / 6.0);
if (compute_moment) {
- float half_u_p = (float)(0.5*u_p);
- dMuv = (float)(v_p*v_p*(0.25+half_u_p) - v_p*(1./3.+half_u_p) + 1./6.*u_p +0.125);
- dMu2 = (float)(1./3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1./12.0);
- dMv2 = (float)(1./3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1./12.0);
+ float half_u_p = (float)(0.5 * u_p);
+ dMuv = (float)(v_p * v_p * (0.25 + half_u_p) - v_p * (1. / 3. + half_u_p) + 1. / 6. * u_p + 0.125);
+ dMu2 = (float)(1. / 3. * u_p * (u_p * u_p + 1.5 * u_p + 1.) + 1. / 12.0);
+ dMv2 = (float)(1. / 3. * v_p * (v_p * v_p - 1.5 * v_p + 1.) - 1. / 12.0);
}
break;
}
}
-
/*!
From a pixel coordinate and a direction, update the pixel coordinates after
@@ -2233,18 +2074,37 @@ void
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: u += 1; break;
- case 1: u += 1; v += 1; break;
- case 2: v += 1; break;
- case 3: u -= 1; v += 1; break;
- case 4: u -= 1; break;
- case 5: u -= 1; v -= 1; break;
- case 6: v -= 1; break;
- case 7: u += 1; v -= 1; break;
+ switch (dir) {
+ case 0:
+ u += 1;
+ break;
+ case 1:
+ u += 1;
+ v += 1;
+ break;
+ case 2:
+ v += 1;
+ break;
+ case 3:
+ u -= 1;
+ v += 1;
+ break;
+ case 4:
+ u -= 1;
+ break;
+ case 5:
+ u -= 1;
+ v -= 1;
+ break;
+ case 6:
+ v -= 1;
+ break;
+ case 7:
+ u += 1;
+ v -= 1;
+ break;
}
}
@@ -2259,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<unsigned char> &I) const
-{
- return isInImage( I, cog);
-}
+bool vpDot2::isInImage(const vpImage<unsigned char> &I) const { return isInImage(I, cog); }
/*!
@@ -2282,15 +2139,17 @@ bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip)
double u = ip.get_u();
double v = ip.get_v();
- if( u < 0 || u >= w ) return false;
- if( v < 0 || v >= h ) return false;
+ if (u < 0 || u >= w)
+ return false;
+ if (v < 0 || v >= h)
+ return false;
return true;
}
/*!
- Test if a pixel is in a region of interest. Points of the border are not considered to
- be in the area.
+ Test if a pixel is in a region of interest. Points of the border are not
+ considered to be in the area.
\param u : The column coordinate of the pixel.
\param v : The row coordinate of the pixel .
@@ -2298,23 +2157,24 @@ bool vpDot2::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &ip)
\return true if the pixel of coordinates (u, v) is in the image and false
otherwise.
*/
-bool vpDot2::isInArea( const unsigned int &u, const unsigned int &v) const
+bool vpDot2::isInArea(const unsigned int &u, const unsigned int &v) const
{
- unsigned int area_u_min = (unsigned int) area.getLeft();
- unsigned int area_u_max = (unsigned int) area.getRight();
- unsigned int area_v_min = (unsigned int) area.getTop();
- unsigned int area_v_max = (unsigned int) area.getBottom();
+ unsigned int area_u_min = (unsigned int)area.getLeft();
+ unsigned int area_u_max = (unsigned int)area.getRight();
+ unsigned int area_v_min = (unsigned int)area.getTop();
+ unsigned int area_v_max = (unsigned int)area.getBottom();
- if( u < area_u_min || u > area_u_max ) return false;
- if( v < area_v_min || v > area_v_max ) return false;
+ if (u < area_u_min || u > area_u_max)
+ return false;
+ if (v < area_v_min || v > area_v_max)
+ return false;
return true;
}
-
/*!
- Get the search grid size used to found a dot in a region of interest. This grid is used to
- parse only some pixels of the search area.
+ Get the search grid size used to found a dot in a region of interest. This
+ grid is used to parse only some pixels of the search area.
\param gridWidth : Number of pixels between to vertical lines of the grid
@@ -2322,22 +2182,22 @@ bool vpDot2::isInArea( const unsigned int &u, const unsigned int &v) const
*/
-void vpDot2::getGridSize( unsigned int &gridWidth, unsigned int &gridHeight )
+void vpDot2::getGridSize(unsigned int &gridWidth, unsigned int &gridHeight)
{
// first get the research grid width and height Note that
// 1/sqrt(2)=cos(pi/4). The grid squares should be small enough to be
// contained in the dot. We gent this here if the dot is a perfect disc.
// More accurate criterium to define the grid should be implemented if
// necessary
- gridWidth = (unsigned int) (getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.));
- gridHeight = (unsigned int) (getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0));
+ gridWidth = (unsigned int)(getWidth() * getMaxSizeSearchDistancePrecision() / sqrt(2.));
+ gridHeight = (unsigned int)(getHeight() * getMaxSizeSearchDistancePrecision() / sqrt(2.0));
- if( gridWidth == 0 ) gridWidth = 1;
- if( gridHeight == 0 ) gridHeight = 1;
+ if (gridWidth == 0)
+ gridWidth = 1;
+ if (gridHeight == 0)
+ gridHeight = 1;
}
-
-
/*!
Compute an approximation of mean gray level of the dot.
@@ -2350,182 +2210,173 @@ void vpDot2::getGridSize( unsigned int &gridWidth, unsigned int &gridHeight )
*/
-void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char>& I)
+void vpDot2::computeMeanGrayLevel(const vpImage<unsigned char> &I)
{
int cog_u = (int)cog.get_u();
int cog_v = (int)cog.get_v();
- unsigned int sum_value =0;
- unsigned int nb_pixels =0;
+ unsigned int sum_value = 0;
+ unsigned int nb_pixels = 0;
- for(unsigned int i=(unsigned int)this->bbox_u_min; i <=(unsigned int)this->bbox_u_max ; i++){
- unsigned int pixel_gray =(unsigned int) I[(unsigned int)cog_v][i];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ 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 ++;
+ nb_pixels++;
}
}
- for(unsigned int i=(unsigned int)this->bbox_v_min; i <=(unsigned int)this->bbox_v_max ; i++){
- unsigned char pixel_gray =I[i][(unsigned int)cog_u];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ 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 ++;
+ nb_pixels++;
}
}
- if(nb_pixels < 10){ //could be good to choose the min nb points from area of dot
- //add diagonals points to have enough point
- int imin,imax;
- if( (cog_u - bbox_u_min) > (cog_v - bbox_v_min)){
- imin=cog_v - bbox_v_min;
+ if (nb_pixels < 10) { // could be good to choose the min nb points from area of dot
+ // add diagonals points to have enough point
+ int imin, imax;
+ if ((cog_u - bbox_u_min) > (cog_v - bbox_v_min)) {
+ imin = cog_v - bbox_v_min;
+ } else {
+ imin = cog_u - bbox_u_min;
}
- else{ imin = cog_u - bbox_u_min;}
- if( (bbox_u_max - cog_u) > (bbox_v_max - cog_v)){
- imax=bbox_v_max - cog_v;
+ if ((bbox_u_max - cog_u) > (bbox_v_max - cog_v)) {
+ imax = bbox_v_max - cog_v;
+ } else {
+ imax = bbox_u_max - cog_u;
}
- else{ imax = bbox_u_max - cog_u;}
- for(int i=-imin; i <=imax ; i++){
- unsigned int pixel_gray =(unsigned int) I[(unsigned int)(cog_v + i)][(unsigned int)(cog_u + i)];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ for (int i = -imin; i <= imax; i++) {
+ unsigned int pixel_gray = (unsigned int)I[(unsigned int)(cog_v + i)][(unsigned int)(cog_u + i)];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) {
sum_value += pixel_gray;
- nb_pixels ++;
+ nb_pixels++;
}
}
- if( (cog_u - bbox_u_min) > (bbox_v_max - cog_v)){
+ if ((cog_u - bbox_u_min) > (bbox_v_max - cog_v)) {
imin = bbox_v_max - cog_v;
+ } else {
+ imin = cog_u - bbox_u_min;
}
- else{ imin = cog_u - bbox_u_min;}
- if( (bbox_u_max - cog_u) > (cog_v - bbox_v_min)){
+ if ((bbox_u_max - cog_u) > (cog_v - bbox_v_min)) {
imax = cog_v - bbox_v_min;
+ } else {
+ imax = bbox_u_max - cog_u;
}
- else{ imax = bbox_u_max - cog_u;}
- for(int i=-imin; i <=imax ; i++){
- unsigned char pixel_gray =I[(unsigned int)(cog_v - i)][(unsigned int)(cog_u + i)];
- if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()){
+ for (int i = -imin; i <= imax; i++) {
+ unsigned char pixel_gray = I[(unsigned int)(cog_v - i)][(unsigned int)(cog_u + i)];
+ if (pixel_gray >= getGrayLevelMin() && pixel_gray <= getGrayLevelMax()) {
sum_value += pixel_gray;
- nb_pixels ++;
+ nb_pixels++;
}
}
}
- if(nb_pixels== 0){
- //should never happen
- throw(vpTrackingException(vpTrackingException::notEnoughPointError,"No point was found"));
- }
- else{
- mean_gray_level = sum_value/nb_pixels;
+ if (nb_pixels == 0) {
+ // should never happen
+ throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No point was found"));
+ } else {
+ mean_gray_level = sum_value / nb_pixels;
}
}
/*!
Define a number of dots from a file.
- If the file does not exist, define it by clicking an image, the dots are then saved into the file.
+ If the file does not exist, define it by clicking an image, the dots are
+ then saved into the file.
- If the dots from the file cannot be tracked in the image, will ask to click them.
+ If the dots from the file cannot be tracked in the image, will ask to click
+ them.
- \param dot : dot2 array
- \param n : number of dots, array dimension
- \param dotFile : path for the file
- \param I : image
- \param col : color to print the dots (default Blue)
- \param trackDot : if true, tracks the dots in the image, if false simply loads the coordinates (default true)
+ \param dot : dot2 array
+ \param n : number of dots, array dimension
+ \param dotFile : path for the file
+ \param I : image
+ \param col : color to print the dots (default Blue)
+ \param trackDot : if true, tracks the dots in the image, if false
+ simply loads the coordinates (default true)
\return an nx2 matrix with the coordinates of the dots
*/
-vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I, vpColor col, bool trackDot)
+vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::string &dotFile, vpImage<unsigned char> &I,
+ vpColor col, bool trackDot)
{
- vpMatrix Cogs(n, 2);
- vpImagePoint cog;
- unsigned int i;
- bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
- if(fromFile)
- {
- vpMatrix::loadMatrix(dotFile, Cogs);
- std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
- }
-
- // test number of cogs in file
- if(Cogs.getRows() < n)
- {
- std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
- fromFile = false;
- }
-
- // read from file and tracks the dots
- if(fromFile)
- {
- try
- {
- for(i=0;i<n;++i)
- {
- cog.set_uv(Cogs[i][0], Cogs[i][1]);
+ vpMatrix Cogs(n, 2);
+ vpImagePoint cog;
+ unsigned int i;
+ bool fromFile = vpIoTools::checkFilename(dotFile.c_str());
+ if (fromFile) {
+ vpMatrix::loadMatrix(dotFile, Cogs);
+ std::cout << Cogs.getRows() << " dots loaded from file " << dotFile << std::endl;
+ }
+
+ // test number of cogs in file
+ if (Cogs.getRows() < n) {
+ std::cout << "Dot file has a wrong number of dots : redefining them" << std::endl;
+ fromFile = false;
+ }
+
+ // read from file and tracks the dots
+ if (fromFile) {
+ try {
+ for (i = 0; i < n; ++i) {
+ cog.set_uv(Cogs[i][0], Cogs[i][1]);
dot[i].setGraphics(true);
dot[i].setCog(cog);
- if(trackDot)
- {
- dot[i].initTracking(I,cog);
- dot[i].track(I);
- vpDisplay::displayCross(I, cog, 10, col);
- }
- }
- }
- catch(...)
- {
- std::cout << "Cannot track dots from file" << std::endl;
- fromFile = false;
- }
- vpDisplay::flush(I);
-
- // check that dots are far away ones from the other
- for(i=0;i<n && fromFile;++i)
- {
- 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)
- {
- fromFile = false;
- std::cout << "Dots from file seem incoherent" << std::endl;
+ if (trackDot) {
+ dot[i].initTracking(I, cog);
+ dot[i].track(I);
+ vpDisplay::displayCross(I, cog, 10, col);
+ }
}
- }
- }
-
- if(!fromFile)
- {
- vpDisplay::display(I);
- vpDisplay::flush(I);
-
- std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
- for (i = 0; i < n; i++)
- {
- if(trackDot)
- {
+ } catch (...) {
+ std::cout << "Cannot track dots from file" << std::endl;
+ fromFile = false;
+ }
+ vpDisplay::flush(I);
+
+ // check that dots are far away ones from the other
+ for (i = 0; i < n && fromFile; ++i) {
+ 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) {
+ fromFile = false;
+ std::cout << "Dots from file seem incoherent" << std::endl;
+ }
+ }
+ }
+
+ if (!fromFile) {
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+
+ std::cout << "Click on the " << n << " dots clockwise starting from upper/left dot..." << std::endl;
+ for (i = 0; i < n; i++) {
+ if (trackDot) {
dot[i].setGraphics(true);
dot[i].initTracking(I);
- cog = dot[i].getCog();
- }
- else
- {
- vpDisplay::getClick(I, cog);
- dot[i].setCog(cog);
- }
- Cogs[i][0] = cog.get_u();
- Cogs[i][1] = cog.get_v();
- vpDisplay::displayCross(I, cog, 10, col);
- vpDisplay::flush(I);
- }
- }
-
- if (!fromFile && (dotFile != ""))
- {
- vpMatrix::saveMatrix(dotFile, Cogs);
- std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
- }
-
- // back to non graphic mode
- for(i=0;i<n;++i)
- dot[i].setGraphics(false);
+ cog = dot[i].getCog();
+ } else {
+ vpDisplay::getClick(I, cog);
+ dot[i].setCog(cog);
+ }
+ Cogs[i][0] = cog.get_u();
+ Cogs[i][1] = cog.get_v();
+ vpDisplay::displayCross(I, cog, 10, col);
+ vpDisplay::flush(I);
+ }
+ }
+
+ if (!fromFile && (dotFile != "")) {
+ vpMatrix::saveMatrix(dotFile, Cogs);
+ std::cout << Cogs.getRows() << " dots written to file " << dotFile << std::endl;
+ }
+
+ // back to non graphic mode
+ for (i = 0; i < n; ++i)
+ dot[i].setGraphics(false);
return Cogs;
}
@@ -2533,35 +2384,41 @@ vpMatrix vpDot2::defineDots(vpDot2 dot[], const unsigned int &n, const std::stri
/*!
Tracks a number of dots in an image and displays their trajectories
- \param dot : dot2 array
- \param n : number of dots, array dimension
- \param I : image
- \param cogs : vector of vpImagePoint that will be updated with the new dots, will be displayed in green
- \param cogStar (optional) : array of vpImagePoint indicating the desired position (default NULL), will be displayed in red
+ \param dot : dot2 array
+
+ \param n : number of dots, array dimension
+
+ \param I : image
+
+ \param cogs : vector of vpImagePoint that will be updated with the new
+ dots, will be displayed in green
+
+ \param cogStar (optional) : array of
+ vpImagePoint indicating the desired position (default NULL), will be
+ displayed in red
*/
-void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I, std::vector<vpImagePoint> &cogs, vpImagePoint* cogStar)
+void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsigned char> &I,
+ std::vector<vpImagePoint> &cogs, vpImagePoint *cogStar)
{
- unsigned int i;
- // tracking
- for(i=0;i<n;++i)
- {
- dot[i].track(I);
- cogs.push_back(dot[i].getCog());
- }
- // trajectories
- for(i=n;i<cogs.size();++i)
- vpDisplay::displayCircle(I,cogs[i],4,vpColor::green,true);
- // initial position
- for(i=0;i<n;++i)
- vpDisplay::displayCircle(I,cogs[i],4,vpColor::blue,true);
- // if exists, desired position
- if(cogStar != NULL)
- for(i=0;i<n;++i)
- {
- vpDisplay::displayDotLine(I,cogStar[i],dot[i].getCog(),vpColor::red);
- vpDisplay::displayCircle(I,cogStar[i],4,vpColor::red,true);
+ unsigned int i;
+ // tracking
+ for (i = 0; i < n; ++i) {
+ dot[i].track(I);
+ cogs.push_back(dot[i].getCog());
}
- vpDisplay::flush(I);
+ // trajectories
+ for (i = n; i < cogs.size(); ++i)
+ vpDisplay::displayCircle(I, cogs[i], 4, vpColor::green, true);
+ // initial position
+ for (i = 0; i < n; ++i)
+ vpDisplay::displayCircle(I, cogs[i], 4, vpColor::blue, true);
+ // if exists, desired position
+ if (cogStar != NULL)
+ for (i = 0; i < n; ++i) {
+ vpDisplay::displayDotLine(I, cogStar[i], dot[i].getCog(), vpColor::red);
+ vpDisplay::displayCircle(I, cogStar[i], 4, vpColor::red, true);
+ }
+ vpDisplay::flush(I);
}
/*!
@@ -2569,24 +2426,22 @@ void vpDot2::trackAndDisplay(vpDot2 dot[], const unsigned int &n, vpImage<unsign
Display the dot center of gravity and its list of edges.
\param I : The image used as background.
-
+
\param cog : The center of gravity.
-
+
\param edges_list : The list of edges;
-
+
\param color : Color used to display the dot.
-
+
\param thickness : Thickness of the dot.
*/
-void vpDot2::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
+void vpDot2::display(const vpImage<unsigned char> &I, const vpImagePoint &cog,
+ const std::list<vpImagePoint> &edges_list, vpColor color, unsigned int thickness)
{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
std::list<vpImagePoint>::const_iterator it;
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
+ for (it = edges_list.begin(); it != edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
@@ -2605,24 +2460,20 @@ void vpDot2::display(const vpImage<unsigned char>& I,const vpImagePoint &cog,
\param thickness : Thickness of the dot.
*/
-void vpDot2::display(const vpImage<vpRGBa>& I,const vpImagePoint &cog,
- const std::list<vpImagePoint> &edges_list, vpColor color,
- unsigned int thickness)
+void vpDot2::display(const vpImage<vpRGBa> &I, const vpImagePoint &cog, const std::list<vpImagePoint> &edges_list,
+ vpColor color, unsigned int thickness)
{
- vpDisplay::displayCross(I, cog, 3*thickness+8, color, thickness);
+ vpDisplay::displayCross(I, cog, 3 * thickness + 8, color, thickness);
std::list<vpImagePoint>::const_iterator it;
- for (it = edges_list.begin(); it != edges_list.end(); ++it)
- {
+ for (it = edges_list.begin(); it != edges_list.end(); ++it) {
vpDisplay::displayPoint(I, *it, color);
}
}
/*!
- Writes the dot center of gravity coordinates in the frame (i,j) (For more details
- about the orientation of the frame see the vpImagePoint documentation) to the stream \e os,
- and returns a reference to the stream.
+ Writes the dot center of gravity coordinates in the frame (i,j) (For more
+ details about the orientation of the frame see the vpImagePoint
+ documentation) to the stream \e os, and returns a reference to the stream.
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream& os, vpDot2& d) {
- return (os << "(" << d.getCog() << ")" ) ;
-}
+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 c915f06..cccd131 100644
--- a/modules/tracker/blob/test/tracking/testTrackDot.cpp
+++ b/modules/tracker/blob/test/tracking/testTrackDot.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,28 +37,26 @@
*
*****************************************************************************/
-
-
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpConfig.h>
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpDebug.h>
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GTK) || defined(VISP_HAVE_GDI))
+#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
-#include <visp3/blob/vpDot2.h>
#ifdef VISP_HAVE_MODULE_FEATURES
-# include <visp3/visual_features/vpFeatureEllipse.h>
-# include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
#endif
/*!
@@ -67,10 +66,9 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
/*!
@@ -95,7 +93,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read image \n\
- \"ViSP-images/ellipse/ellipse.pgm\"\n\
+ \"ellipse/ellipse.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -108,12 +106,10 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
}
/*!
@@ -127,22 +123,31 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -157,9 +162,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -170,18 +173,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
-
// Read the command line options
- if (getOptions(argc, argv, opt_ipath,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -192,8 +194,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -201,25 +202,23 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I ;
+ vpImage<unsigned char> I;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/ellipse");
+ dirname = vpIoTools::createFilePath(ipath, "ellipse");
// Build the name of the image file
filename = vpIoTools::createFilePath(dirname, "ellipse.pgm");
@@ -230,27 +229,23 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
vpCTRACE << "Load: " << filename << std::endl;
- vpImageIo::read(I, filename) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
+ vpImageIo::read(I, filename);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
std::cerr << " Cannot read " << filename << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display;
#elif defined VISP_HAVE_GTK
@@ -261,61 +256,55 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display.init(I, 100, 100,"Display...") ;
+ display.init(I, 100, 100, "Display...");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(I) ;
- //Flush the display
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ // Flush the display
+ vpDisplay::flush(I);
}
- vpDot2 dot ;
+ vpDot2 dot;
std::cout << "debut 1\n";
- //dot.setMaxDotSize(0.50); // dot max size = 50% of the image size
+ // dot.setMaxDotSize(0.50); // dot max size = 50% of the image size
vpImagePoint ip;
- ip.set_i( 140 );
- ip.set_j( 140 );
+ ip.set_i(140);
+ ip.set_j(140);
dot.initTracking(I, ip);
if (opt_display) {
- dot.setGraphics(true) ;
- }
- else {
- dot.setGraphics(false) ;
+ dot.setGraphics(true);
+ } else {
+ dot.setGraphics(false);
}
dot.setComputeMoments(true);
- dot.track(I) ;
+ dot.track(I);
vpCameraParameters cam;
#ifdef VISP_HAVE_MODULE_FEATURES
vpFeatureEllipse e;
- vpFeatureBuilder::create(e,cam,dot);
+ vpFeatureBuilder::create(e, cam, dot);
#endif
if (opt_display) {
#ifdef VISP_HAVE_MODULE_FEATURES
- e.display(cam, I, vpColor::red) ;
+ e.display(cam, I, vpColor::red);
#endif
vpDisplay::flush(I);
if (opt_click_allowed) {
std::cout << "A click to exit..." << std::endl;
- vpDisplay::getClick(I) ;
+ vpDisplay::getClick(I);
}
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
return 1;
}
}
#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/CMakeLists.txt b/modules/tracker/klt/CMakeLists.txt
index 75d015c..96daf0d 100644
--- a/modules/tracker/klt/CMakeLists.txt
+++ b/modules/tracker/klt/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h
index d1f6a9f..2ea14d3 100644
--- a/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h
+++ b/modules/tracker/klt/include/visp3/klt/vpKltOpencv.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,15 +48,15 @@
#ifndef vpKltOpencv_h
#define vpKltOpencv_h
-#include <visp3/core/vpConfig.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020408))
-#include <opencv2/video/tracking.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/video/tracking.hpp>
/*!
\class vpKltOpencv
@@ -63,8 +64,9 @@
\ingroup module_klt
\brief Wrapper for the KLT (Kanade-Lucas-Tomasi) feature tracker
- implemented in OpenCV. Thus to enable this class OpenCV should be installed. Installation
- instructions are provided here https://visp.inria.fr/3rd_opencv.
+ implemented in OpenCV. Thus to enable this class OpenCV should be installed.
+ Installation instructions are provided here
+ https://visp.inria.fr/3rd_opencv.
The following example available in tutorial-klt-tracker.cpp shows how to use
the main functions of the class.
@@ -77,77 +79,80 @@ class VISP_EXPORT vpKltOpencv
{
public:
vpKltOpencv();
- vpKltOpencv(const vpKltOpencv& copy);
+ vpKltOpencv(const vpKltOpencv ©);
virtual ~vpKltOpencv();
void addFeature(const float &x, const float &y);
void addFeature(const long &id, const float &x, const float &y);
void addFeature(const cv::Point2f &f);
- void display(const vpImage<unsigned char> &I,
- const vpColor &color = vpColor::red, unsigned int thickness=1);
+ void display(const vpImage<unsigned char> &I, const vpColor &color = vpColor::red, unsigned int thickness = 1);
static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
static void display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
+ const std::vector<long> &featuresid, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
static void display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
+ const std::vector<long> &featuresid, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
//! Get the size of the averaging block used to track the features.
- int getBlockSize() const {return m_blockSize;}
+ int getBlockSize() const { return m_blockSize; }
void getFeature(const int &index, long &id, float &x, float &y) const;
//! Get the list of current features.
- std::vector<cv::Point2f> getFeatures() const {return m_points[1];}
- //CvPoint2D32f* getFeatures() const {return features;}
+ std::vector<cv::Point2f> getFeatures() const { return m_points[1]; }
+ // CvPoint2D32f* getFeatures() const {return features;}
//! Get the unique id of each feature.
- std::vector<long> getFeaturesId() const {return m_points_id;}
- //long* getFeaturesId() const {return featuresid;}
+ std::vector<long> getFeaturesId() const { return m_points_id; }
+ // long* getFeaturesId() const {return featuresid;}
//! Get the free parameter of the Harris detector.
- double getHarrisFreeParameter() const {return m_harris_k;}
+ double getHarrisFreeParameter() const { return m_harris_k; }
//! Get the list of lost feature
- //bool *getListOfLostFeature() const { return lostDuringTrack; }
+ // bool *getListOfLostFeature() const { return lostDuringTrack; }
//! Get the maximum number of features to track in the image.
- int getMaxFeatures() const {return m_maxCount;}
- //! Get the minimal Euclidean distance between detected corners during initialization.
- double getMinDistance() const {return m_minDistance;}
+ int getMaxFeatures() const { return m_maxCount; }
+ //! Get the minimal Euclidean distance between detected corners during
+ //! initialization.
+ double getMinDistance() const { return m_minDistance; }
//! Get the number of current features
int getNbFeatures() const { return (int)m_points[1].size(); }
//! Get the number of previous features.
int getNbPrevFeatures() const { return (int)m_points[0].size(); }
- //void getPrevFeature(int index, int &id, float &x, float &y) const;
+ // void getPrevFeature(int index, int &id, float &x, float &y) const;
//! Get the list of previous features
- std::vector<cv::Point2f> getPrevFeatures() const {return m_points[0];}
- //CvPoint2D32f* getPrevFeatures() const {return prev_features;}
+ std::vector<cv::Point2f> getPrevFeatures() const { return m_points[0]; }
+ // CvPoint2D32f* getPrevFeatures() const {return prev_features;}
//! Get the list of features id
- //long* getPrevFeaturesId() const {return prev_featuresid;}
+ // long* getPrevFeaturesId() const {return prev_featuresid;}
//! Get the maximal pyramid level.
- int getPyramidLevels() const {return m_pyrMaxLevel;}
- //! Get the parameter characterizing the minimal accepted quality of image corners.
- double getQuality() const {return m_qualityLevel;}
+ int getPyramidLevels() const { return m_pyrMaxLevel; }
+ //! Get the parameter characterizing the minimal accepted quality of image
+ //! corners.
+ double getQuality() const { return m_qualityLevel; }
//! Get the window size used to refine the corner locations.
- int getWindowSize() const {return m_winSize;}
+ int getWindowSize() const { return m_winSize; }
- void initTracking(const cv::Mat &I, const cv::Mat &mask=cv::Mat());
+ void initTracking(const cv::Mat &I, const cv::Mat &mask = cv::Mat());
void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts);
void initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids);
- vpKltOpencv & operator=(const vpKltOpencv& copy);
+ 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<cv::Point2f> &guess_pts);
- void setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid);
+ void setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
+ const std::vector<long> &fid);
void setMaxFeatures(const int maxCount);
void setMinDistance(double minDistance);
void setMinEigThreshold(double minEigThreshold);
void setPyramidLevels(const int pyrMaxLevel);
void setQuality(double qualityLevel);
- //! Does nothing. Just here for compat with previous releases that use OpenCV C api to do the tracking.
- void setTrackerId(int tid) {(void)tid;}
+ //! Does nothing. Just here for compat with previous releases that use
+ //! OpenCV C api to do the tracking.
+ void setTrackerId(int tid) { (void)tid; }
void setUseHarris(const int useHarrisDetector);
void setWindowSize(const int winSize);
void suppressFeature(const int &index);
@@ -155,7 +160,7 @@ public:
protected:
cv::Mat m_gray, m_prevGray;
std::vector<cv::Point2f> m_points[2]; //!< Previous [0] and current [1] keypoint location
- std::vector<long> m_points_id; //!< Keypoint id
+ std::vector<long> m_points_id; //!< Keypoint id
int m_maxCount;
cv::TermCriteria m_termcrit;
int m_winSize;
@@ -168,37 +173,36 @@ protected:
int m_pyrMaxLevel;
long m_next_points_id;
bool m_initial_guess;
-
};
#elif defined(VISP_HAVE_OPENCV)
#ifdef _CH_
-#pragma package <opencv>
+#pragma package < opencv >
#endif
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/video/tracking.hpp>
-# include <opencv2/legacy/legacy.hpp>
-# include <opencv2/highgui/highgui.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/legacy/legacy.hpp>
+#include <opencv2/video/tracking.hpp>
#else
-# ifndef _EiC
-# include <cv.h>
-# include <highgui.h>
-# include <stdio.h>
-# include <ctype.h>
-# endif
+#ifndef _EiC
+#include <ctype.h>
+#include <cv.h>
+#include <highgui.h>
+#include <stdio.h>
+#endif
#endif
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
-#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpTrackingException.h>
-typedef int (*funccheck)(int,double,double);
-typedef void (*funcinfo)(int,int,int,double,double);
+typedef int (*funccheck)(int, double, double);
+typedef void (*funcinfo)(int, int, int, double, double);
typedef void (*funcevent)(int);
/*!
@@ -219,45 +223,52 @@ typedef void (*funcevent)(int);
class VISP_EXPORT vpKltOpencv
{
private:
- int initialized; //Is the tracker ready ?
-
- int maxFeatures; //Maximum number of features to track (Default 50)
- int globalcountFeatures; //Keep over time for ID
-
- int win_size; //Size of search window for tracker (default 10)
- double quality; //Multiplier for the maxmin eigenvalue; specifies minimal accepted quality of image corners (default 0.01)
- double min_distance; //Limit, specifying minimum possible distance between returned corners; Euclidian distance is used. (default 10)
- double harris_free_parameter; //Harris detector free parameter. (default 0.04)
- int block_size; //Size of the averaging block used by the corner detector (default 3)
- int use_harris; //0 use a simple Minimum EigenValue Detector, != 0 use Harris (default 1)
- int pyramid_level; //Number of level for the tracker's gaussian pyramid data (default 3)
- int _tid; //tracker id for multiple trackers
-
- IplImage *image; //Image buffer
- IplImage *prev_image; //Image buffer for the previous iteration
- IplImage *pyramid; //Gaussian pyramid data
- IplImage *prev_pyramid; //Gaussian pyramid data for the previous iteration
- IplImage *swap_temp; //Internal
-
- int countFeatures; //Currently tracked features
- int countPrevFeatures; //Previously tracked features
-
- CvPoint2D32f *features; //List of features
- CvPoint2D32f *prev_features; //List of features for previous iteration
-
- long *featuresid; //Array of ids for features
- long *prev_featuresid; //Array of ids for previous features
-
- int flags; //Flags for tracking (internal)
-
- bool initial_guess; //Bool to precise if the next call to track() uses an initial guess
-
- bool *lostDuringTrack; // Result of the tracker for every feature : 1 = lost, 0 = still present
- char *status; //Result of the tracker for every features : 0 = lost, 1 = found
-
-
-
- //EVENT FUNCTION POINTERS
+ int initialized; // Is the tracker ready ?
+
+ int maxFeatures; // Maximum number of features to track (Default 50)
+ int globalcountFeatures; // Keep over time for ID
+
+ int win_size; // Size of search window for tracker (default 10)
+ double quality; // Multiplier for the maxmin eigenvalue; specifies minimal
+ // accepted quality of image corners (default 0.01)
+ double min_distance; // Limit, specifying minimum possible distance between
+ // returned corners; Euclidian distance is used.
+ // (default 10)
+ double harris_free_parameter; // Harris detector free parameter. (default 0.04)
+ int block_size; // Size of the averaging block used by the corner detector
+ // (default 3)
+ int use_harris; // 0 use a simple Minimum EigenValue Detector, != 0 use
+ // Harris (default 1)
+ int pyramid_level; // Number of level for the tracker's gaussian pyramid
+ // data (default 3)
+ int _tid; // tracker id for multiple trackers
+
+ IplImage *image; // Image buffer
+ IplImage *prev_image; // Image buffer for the previous iteration
+ IplImage *pyramid; // Gaussian pyramid data
+ IplImage *prev_pyramid; // Gaussian pyramid data for the previous iteration
+ IplImage *swap_temp; // Internal
+
+ int countFeatures; // Currently tracked features
+ int countPrevFeatures; // Previously tracked features
+
+ CvPoint2D32f *features; // List of features
+ CvPoint2D32f *prev_features; // List of features for previous iteration
+
+ long *featuresid; // Array of ids for features
+ long *prev_featuresid; // Array of ids for previous features
+
+ int flags; // Flags for tracking (internal)
+
+ bool initial_guess; // Bool to precise if the next call to track() uses an
+ // initial guess
+
+ bool *lostDuringTrack; // Result of the tracker for every feature : 1 =
+ // lost, 0 = still present
+ char *status; // Result of the tracker for every features : 0 = lost, 1 =
+ // found
+
+ // EVENT FUNCTION POINTERS
funcevent OnInitialize;
funcinfo OnFeatureLost;
funcinfo OnNewFeature;
@@ -265,148 +276,174 @@ private:
funccheck IsFeatureValid;
private:
-
- //Internal
+ // Internal
void clean();
void cleanAll();
void reset();
public:
vpKltOpencv();
- vpKltOpencv(const vpKltOpencv& copy);
+ vpKltOpencv(const vpKltOpencv ©);
virtual ~vpKltOpencv();
void addFeature(const int &id, const float &x, const float &y);
- //Draw the tracked features on the given image
- void display(const vpImage<unsigned char> &I,
- vpColor color = vpColor::red, unsigned int thickness=1);
+ // Draw the tracked features on the given image
+ void display(const vpImage<unsigned char> &I, vpColor color = vpColor::red, unsigned int thickness = 1);
//! Get the block size
- int getBlockSize() const {return block_size;}
+ int getBlockSize() const { return block_size; }
void getFeature(int index, long &id, float &x, float &y) const;
//! Get the list of features
- CvPoint2D32f* getFeatures() const {return features;}
+ CvPoint2D32f *getFeatures() const { return features; }
//! Get the list of features id
- long* getFeaturesId() const {return featuresid;}
+ long *getFeaturesId() const { return featuresid; }
//! Get Harris free parameter
- double getHarrisFreeParameter() const {return harris_free_parameter;}
+ double getHarrisFreeParameter() const { return harris_free_parameter; }
//! Get the list of lost feature
bool *getListOfLostFeature() const { return lostDuringTrack; }
//! Get Max number of features
- int getMaxFeatures() const {return maxFeatures;}
+ int getMaxFeatures() const { return maxFeatures; }
//! Get Min Distance
- double getMinDistance() const {return min_distance;}
+ double getMinDistance() const { return min_distance; }
//! Get the current number of features
int getNbFeatures() const { return countFeatures; }
//! Get the previous number of features
int getNbPrevFeatures() const { return countPrevFeatures; }
void getPrevFeature(int index, int &id, float &x, float &y) const;
//! Get the list of features
- CvPoint2D32f* getPrevFeatures() const {return prev_features;}
+ CvPoint2D32f *getPrevFeatures() const { return prev_features; }
//! Get the list of features id
- long* getPrevFeaturesId() const {return prev_featuresid;}
+ long *getPrevFeaturesId() const { return prev_featuresid; }
//! Get the number of pyramid levels
- int getPyramidLevels() const {return pyramid_level;}
+ int getPyramidLevels() const { return pyramid_level; }
//! Get the quality of the tracker
- double getQuality() const {return quality;}
+ double getQuality() const { return quality; }
//! Get Max number of features
- int getWindowSize() const {return win_size;}
+ int getWindowSize() const { return win_size; }
- //Detect corners in the image. Initialize the tracker
+ // Detect corners in the image. Initialize the tracker
void initTracking(const IplImage *I, const IplImage *mask = NULL);
void initTracking(const IplImage *I, CvPoint2D32f *pts, int size);
void initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int size);
- vpKltOpencv & operator=(const vpKltOpencv& copy);
- //Track !
+ vpKltOpencv &operator=(const vpKltOpencv ©);
+ // Track !
void track(const IplImage *I);
-
- //Seters
+ // Seters
/*!
Set the size of the averaging block used to track the features.
- \warning The input is a signed integer to be compatible with OpenCV. However,
- it must be a positive integer.
+ \warning The input is a signed integer to be compatible with OpenCV.
+ However, it must be a positive integer.
\param input : The new size of the block.
*/
- void setBlockSize(const int input) {initialized = 0; block_size=input;}
+ void setBlockSize(const int input)
+ {
+ initialized = 0;
+ block_size = input;
+ }
/*!
Set the Harris parameter (The \e k value).
- \warning The tracker must be re-initialised using the method initTracking().
+ \warning The tracker must be re-initialised using the method
+ initTracking().
\param input : The new Harris parameter.
*/
- void setHarrisFreeParameter(double input) {initialized = 0; harris_free_parameter=input;}
+ void setHarrisFreeParameter(double input)
+ {
+ initialized = 0;
+ harris_free_parameter = input;
+ }
void setInitialGuess(CvPoint2D32f **guess_pts);
void setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size);
/*!
- Is a feature valid (e.g. : test if not too close to borders) -> event(id_tracker, x, y)
+ Is a feature valid (e.g. : test if not too close to borders) ->
+ event(id_tracker, x, y)
*/
- void setIsFeatureValid(funccheck input) {IsFeatureValid = input;}
+ void setIsFeatureValid(funccheck input) { IsFeatureValid = input; }
/* Should be used only before initTracking */
void setMaxFeatures(const int input);
/*!
Set the minimal distance between two points during the initialisation.
- \warning The tracker must be re-initialised using the method initTracking().
+ \warning The tracker must be re-initialised using the method
+ initTracking().
\param input : The new minimal distance between two points.
*/
- void setMinDistance(double input) {initialized = 0; min_distance=input;}
-
- //Functors
-
- //Event when tracker is initialized -> event(id_tracker)
- void setOnInitialize(funcevent input) {OnInitialize = input;}
- //Event when a feature is lost -> event(id_tracker, index, uid, x, y)
- void setOnFeatureLost(funcinfo input) {OnFeatureLost = input;}
- //Event when a new feature is found -> event(id_tracker, index, uid, x, y)
- void setOnNewFeature(funcinfo input) {OnNewFeature = input;}
- //Event when a feature is found while tracking -> event(id_tracker, index, uid, x, y)
- void setOnMeasureFeature(funcinfo input) {OnMeasureFeature = input;}
+ void setMinDistance(double input)
+ {
+ initialized = 0;
+ min_distance = input;
+ }
+
+ // Functors
+
+ // Event when tracker is initialized -> event(id_tracker)
+ void setOnInitialize(funcevent input) { OnInitialize = input; }
+ // Event when a feature is lost -> event(id_tracker, index, uid, x, y)
+ void setOnFeatureLost(funcinfo input) { OnFeatureLost = input; }
+ // Event when a new feature is found -> event(id_tracker, index, uid, x, y)
+ void setOnNewFeature(funcinfo input) { OnNewFeature = input; }
+ // Event when a feature is found while tracking -> event(id_tracker, index,
+ // uid, x, y)
+ void setOnMeasureFeature(funcinfo input) { OnMeasureFeature = input; }
/*!
Set the maximal pyramid level. If the level is zero, then no pyramid is
computed for the optical flow.
- \warning The tracker must be re-initialised using the method initTracking().
+ \warning The tracker must be re-initialised using the method
+ initTracking().
\param input : The new maximal pyramid level.
*/
- void setPyramidLevels(const int input) {initialized = 0; pyramid_level=input;}
- void setQuality(double input) {initialized = 0; quality=input;}
- void setTrackerId(int tid) {_tid = tid;}
+ void setPyramidLevels(const int input)
+ {
+ initialized = 0;
+ pyramid_level = input;
+ }
+ void setQuality(double input)
+ {
+ initialized = 0;
+ quality = input;
+ }
+ void setTrackerId(int tid) { _tid = tid; }
/*!
Set the window size for the sub-pixel computation.
- \warning The tracker must be re-initialised using the method initTracking().
+ \warning The tracker must be re-initialised using the method
+ initTracking().
\param input : The new number of maximum features.
*/
- void setUseHarris(const int input) {initialized = 0; use_harris=input;}
- void setWindowSize(const int input) {initialized = 0; win_size=input;}
+ void setUseHarris(const int input)
+ {
+ initialized = 0;
+ use_harris = input;
+ }
+ void setWindowSize(const int input)
+ {
+ initialized = 0;
+ win_size = input;
+ }
void suppressFeature(int index);
-
- //Static Functions
-public:
- static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color = vpColor::green,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color = vpColor::green,
- unsigned int thickness=1);
-
- static void display(const vpImage<unsigned char>& I, const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I, const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color = vpColor::green, unsigned int thickness=1);
+ // Static Functions
+public:
+ static void display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
+ vpColor color = vpColor::green, unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
+ vpColor color = vpColor::green, unsigned int thickness = 1);
+
+ static void display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
+ const int &nbFeatures, vpColor color = vpColor::green, unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
+ const int &nbFeatures, vpColor color = vpColor::green, unsigned int thickness = 1);
};
#endif
diff --git a/modules/tracker/klt/src/vpKltOpencv.cpp b/modules/tracker/klt/src/vpKltOpencv.cpp
index cf83a75..d6e3398 100644
--- a/modules/tracker/klt/src/vpKltOpencv.cpp
+++ b/modules/tracker/klt/src/vpKltOpencv.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,28 +52,27 @@
#include <string>
#include <visp3/core/vpDisplay.h>
-#include <visp3/klt/vpKltOpencv.h>
#include <visp3/core/vpTrackingException.h>
+#include <visp3/klt/vpKltOpencv.h>
/*!
Default constructor.
*/
vpKltOpencv::vpKltOpencv()
: m_gray(), m_prevGray(), m_points_id(), m_maxCount(500), m_termcrit(), m_winSize(10), m_qualityLevel(0.01),
- m_minDistance(15), m_minEigThreshold(1e-4), m_harris_k(0.04), m_blockSize(3), m_useHarrisDetector(1), m_pyrMaxLevel(3),
- m_next_points_id(0), m_initial_guess(false)
+ m_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& copy)
+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_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;
}
@@ -80,7 +80,7 @@ vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
/*!
Copy operator.
*/
-vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
+vpKltOpencv &vpKltOpencv::operator=(const vpKltOpencv ©)
{
m_gray = copy.m_gray;
m_prevGray = copy.m_prevGray;
@@ -103,16 +103,14 @@ vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
return *this;
}
-vpKltOpencv::~vpKltOpencv()
-{
-}
+vpKltOpencv::~vpKltOpencv() {}
/*!
Initialise the tracking by extracting KLT keypoints on the provided image.
- \param I : Grey level image used as input. This image should have only 1 channel.
- \param mask : Image mask used to restrict the keypoint detection area.
- If mask is NULL, all the image will be considered.
+ \param I : Grey level image used as input. This image should have only 1
+ channel. \param mask : Image mask used to restrict the keypoint detection
+ area. If mask is NULL, all the image will be considered.
\exception vpTrackingException::initializationError : If the image I is not
initialized, or if the image or the mask have bad coding format.
@@ -121,22 +119,23 @@ void vpKltOpencv::initTracking(const cv::Mat &I, const cv::Mat &mask)
{
m_next_points_id = 0;
- //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
+ // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
I.copyTo(m_gray);
- for (size_t i=0; i<2; i++) {
+ for (size_t i = 0; i < 2; i++) {
m_points[i].clear();
}
m_points_id.clear();
- cv::goodFeaturesToTrack(m_gray, m_points[1], m_maxCount, m_qualityLevel, m_minDistance, mask, m_blockSize, 0, m_harris_k);
+ 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);
+ if (m_points[1].size() > 0) {
+ cv::cornerSubPix(m_gray, m_points[1], cv::Size(m_winSize, m_winSize), cv::Size(-1, -1), m_termcrit);
- for (size_t i=0; i < m_points[1].size(); i++)
- m_points_id.push_back(m_next_points_id++);
+ for (size_t i = 0; i < m_points[1].size(); i++)
+ m_points_id.push_back(m_next_points_id++);
}
}
@@ -147,7 +146,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)
+ if (m_points[1].size() == 0)
throw vpTrackingException(vpTrackingException::fatalError, "Not enough key points to track.");
std::vector<float> err;
@@ -158,29 +157,28 @@ void vpKltOpencv::track(const cv::Mat &I)
if (m_initial_guess) {
flags |= cv::OPTFLOW_USE_INITIAL_FLOW;
m_initial_guess = false;
- }
- else {
+ } else {
std::swap(m_points[1], m_points[0]);
}
- //cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
+ // cvtColor(I, m_gray, cv::COLOR_BGR2GRAY);
I.copyTo(m_gray);
- if(m_prevGray.empty()){
+ if (m_prevGray.empty()) {
m_gray.copyTo(m_prevGray);
}
std::vector<uchar> status;
cv::calcOpticalFlowPyrLK(m_prevGray, m_gray, m_points[0], m_points[1], status, err, cv::Size(m_winSize, m_winSize),
- m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);
+ m_pyrMaxLevel, m_termcrit, flags, m_minEigThreshold);
// Remove points that are lost
- for (int i=(int)status.size()-1; i>=0; i--) {
+ for (int i = (int)status.size() - 1; i >= 0; i--) {
if (status[(size_t)i] == 0) { // point is lost
- m_points[0].erase(m_points[0].begin()+i);
- m_points[1].erase(m_points[1].begin()+i);
- m_points_id.erase(m_points_id.begin()+i);
+ m_points[0].erase(m_points[0].begin() + i);
+ m_points[1].erase(m_points[1].begin() + i);
+ m_points_id.erase(m_points_id.begin() + i);
}
}
}
@@ -200,7 +198,7 @@ void vpKltOpencv::track(const cv::Mat &I)
*/
void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) const
{
- if ((size_t)index >= m_points[1].size()){
+ if ((size_t)index >= m_points[1].size()) {
throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
}
@@ -216,8 +214,7 @@ void vpKltOpencv::getFeature(const int &index, long &id, float &x, float &y) con
\param color : Color used to display the features.
\param thickness : Thickness of the drawings.
*/
-void vpKltOpencv::display(const vpImage<unsigned char> &I,
- const vpColor &color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<unsigned char> &I, const vpColor &color, unsigned int thickness)
{
vpKltOpencv::display(I, m_points[1], m_points_id, color, thickness);
}
@@ -238,10 +235,10 @@ void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv:
const vpColor &color, unsigned int thickness)
{
vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
+ for (size_t i = 0; i < features.size(); i++) {
+ ip.set_u(vpMath::round(features[i].x));
+ ip.set_v(vpMath::round(features[i].y));
+ vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
}
}
@@ -257,14 +254,14 @@ void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv:
\param thickness : Thickness of the points.
*/
-void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const vpColor &color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features, const vpColor &color,
+ unsigned int thickness)
{
vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness);
+ for (size_t i = 0; i < features.size(); i++) {
+ ip.set_u(vpMath::round(features[i].x));
+ ip.set_v(vpMath::round(features[i].y));
+ vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
}
}
@@ -283,18 +280,17 @@ void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2
\param thickness : Thickness of the points
*/
void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color, unsigned int thickness)
+ const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
{
vpImagePoint ip;
for (size_t i = 0; i < features.size(); i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
+ ip.set_u(vpMath::round(features[i].x));
+ ip.set_v(vpMath::round(features[i].y));
vpDisplay::displayCross(I, ip, 10, color, thickness);
std::ostringstream id;
id << featuresid[i];
- ip.set_u( vpMath::round( features[i].x + 5 ) );
+ ip.set_u(vpMath::round(features[i].x + 5));
vpDisplay::displayText(I, ip, id.str(), color);
}
}
@@ -314,18 +310,17 @@ void vpKltOpencv::display(const vpImage<unsigned char> &I, const std::vector<cv:
\param thickness : Thickness of the points
*/
void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2f> &features,
- const std::vector<long> &featuresid,
- const vpColor &color, unsigned int thickness)
+ const std::vector<long> &featuresid, const vpColor &color, unsigned int thickness)
{
vpImagePoint ip;
- for (size_t i = 0 ; i < features.size() ; i++) {
- ip.set_u( vpMath::round(features[i].x ) );
- ip.set_v( vpMath::round(features[i].y ) );
+ for (size_t i = 0; i < features.size(); i++) {
+ ip.set_u(vpMath::round(features[i].x));
+ ip.set_v(vpMath::round(features[i].y));
vpDisplay::displayCross(I, ip, 10, color, thickness);
std::ostringstream id;
id << featuresid[i];
- ip.set_u( vpMath::round( features[i].x + 5 ) );
+ ip.set_u(vpMath::round(features[i].x + 5));
vpDisplay::displayText(I, ip, id.str(), color);
}
}
@@ -333,120 +328,104 @@ void vpKltOpencv::display(const vpImage<vpRGBa> &I, const std::vector<cv::Point2
/*!
Set the maximum number of features to track in the image.
- \param maxCount : Maximum number of features to detect and track. Default value is set to 500.
+ \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.
- \param winSize : Half of the side length of the search window. Default value is set to 10.
- For example, if \e winSize=5 , then a 5*2+1 \f$\times\f$ 5*2+1 = 11 \f$\times\f$ 11 search window is used.
+ \param winSize : Half of the side length of the search window. Default value
+ is set to 10. For example, if \e winSize=5 , then a 5*2+1 \f$\times\f$ 5*2+1
+ = 11 \f$\times\f$ 11 search window is used.
*/
-void vpKltOpencv::setWindowSize(const int winSize)
-{
- m_winSize = winSize;
-}
+void vpKltOpencv::setWindowSize(const int winSize) { m_winSize = winSize; }
/*!
- Set the parameter characterizing the minimal accepted quality of image corners.
-
- \param qualityLevel : Quality level parameter. Default value is set to 0.01. The parameter value is multiplied by the
- best corner quality measure, which is the minimal eigenvalue or the Harris function response. The corners with
- the quality measure less than the product are rejected. For example, if the best corner has the quality
- measure = 1500, and the qualityLevel=0.01, then all the corners with the quality measure less than 15 are rejected.
+ Set the parameter characterizing the minimal accepted quality of image
+ corners.
+
+ \param qualityLevel : Quality level parameter. Default value is set to 0.01.
+ The parameter value is multiplied by the best corner quality measure, which
+ is the minimal eigenvalue or the Harris function response. The corners with
+ the quality measure less than the product are rejected. For example, if the
+ best corner has the quality measure = 1500, and the qualityLevel=0.01, then
+ all the corners with the quality measure less than 15 are rejected.
*/
-void vpKltOpencv::setQuality(double qualityLevel)
-{
- m_qualityLevel = qualityLevel;
-}
+void vpKltOpencv::setQuality(double qualityLevel) { m_qualityLevel = qualityLevel; }
/*!
Set the free parameter of the Harris detector.
- \param harris_k : Free parameter of the Harris detector. Default value is set to 0.04.
+ \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
the minimal eigenvalue of gradient matrices for corner detection.
- \param useHarrisDetector : If 1 (default value), use the Harris detector. If 0 use the eigenvalue.
+ \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 initialization.
+ Set the minimal Euclidean distance between detected corners during
+ initialization.
- \param minDistance : Minimal possible Euclidean distance between the detected corners.
- Default value is set to 15.
+ \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.
+ 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.
- \warning The input is a signed integer to be compatible with OpenCV. However,
- it must be a positive integer.
+ \warning The input is a signed integer to be compatible with OpenCV.
+ However, it must be a positive integer.
- \param blockSize : Size of an average block for computing a derivative covariation
- matrix over each pixel neighborhood. Default value is set to 3.
+ \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
computed for the optical flow.
- \param pyrMaxLevel : 0-based maximal pyramid level number; if set to 0, pyramids are not used (single level),
- if set to 1, two levels are used, and so on. Default value is set to 3.
+ \param pyrMaxLevel : 0-based maximal pyramid level number; if set to 0,
+ pyramids are not used (single level), if set to 1, two levels are used, and
+ so on. Default value is set to 3.
*/
-void vpKltOpencv::setPyramidLevels(const int pyrMaxLevel)
-{
- m_pyrMaxLevel = pyrMaxLevel;
-}
+void vpKltOpencv::setPyramidLevels(const int pyrMaxLevel) { m_pyrMaxLevel = pyrMaxLevel; }
/*!
- Set the points that will be used as initial guess during the next call to track().
- A typical usage of this function is to predict the position of the features before the
- next call to track().
+ Set the points that will be used as initial guess during the next call to
+ track(). A typical usage of this function is to predict the position of the
+ features before the next call to track().
\param guess_pts : Vector of points that should be tracked. The size of this
- vector should be the same as the one returned by getFeatures(). If this is not the case,
- an exception is returned. Note also that the id of the points is not modified.
+ vector should be the same as the one returned by getFeatures(). If this is
+ not the case, an exception is returned. Note also that the id of the points
+ is not modified.
\sa initTracking()
*/
-void
-vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
+void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
{
- if(guess_pts.size() != m_points[1].size()){
+ if (guess_pts.size() != m_points[1].size()) {
throw(vpException(vpException::badValue,
- "Cannot set initial guess: size feature vector [%d] and guess vector [%d] doesn't match",
+ "Cannot set initial guess: size feature vector [%d] "
+ "and guess vector [%d] doesn't match",
m_points[1].size(), guess_pts.size()));
}
@@ -455,24 +434,26 @@ vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &guess_pts)
}
/*!
- Set the points that will be used as initial guess during the next call to track().
- A typical usage of this function is to predict the position of the features before the
- next call to track().
+ Set the points that will be used as initial guess during the next call to
+ track(). A typical usage of this function is to predict the position of the
+ features before the next call to track().
- \param init_pts : Initial points (could be obtained from getPrevFeatures() or getFeatures()).
- \param guess_pts : Prediction of the new position of the initial points. The size of this vector must be the same as the size of the vector of initial points.
- \param fid : Identifiers of the initial points.
+ \param init_pts : Initial points (could be obtained from getPrevFeatures()
+ or getFeatures()). \param guess_pts : Prediction of the new position of the
+ initial points. The size of this vector must be the same as the size of the
+ vector of initial points. \param fid : Identifiers of the initial points.
\sa getPrevFeatures(),getPrevFeaturesId
\sa getFeatures(), getFeaturesId
\sa initTracking()
*/
-void
-vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts, const std::vector<long> &fid)
+void vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std::vector<cv::Point2f> &guess_pts,
+ const std::vector<long> &fid)
{
- if(guess_pts.size() != init_pts.size()){
+ if (guess_pts.size() != init_pts.size()) {
throw(vpException(vpException::badValue,
- "Cannot set initial guess: size init vector [%d] and guess vector [%d] doesn't match",
+ "Cannot set initial guess: size init vector [%d] and "
+ "guess vector [%d] doesn't match",
init_pts.size(), guess_pts.size()));
}
@@ -483,43 +464,42 @@ vpKltOpencv::setInitialGuess(const std::vector<cv::Point2f> &init_pts, const std
}
/*!
- Set the points that will be used as initialization during the next call to track().
+ Set the points that will be used as initialization during the next call to
+ track().
\param I : Input image.
\param pts : Vector of points that should be tracked.
*/
-void
-vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
+void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts)
{
m_initial_guess = false;
m_points[1] = pts;
m_next_points_id = 0;
m_points_id.clear();
- for(size_t i=0; i < m_points[1].size(); i++) {
- m_points_id.push_back(m_next_points_id ++);
+ for (size_t i = 0; i < m_points[1].size(); i++) {
+ m_points_id.push_back(m_next_points_id++);
}
I.copyTo(m_gray);
}
-void
-vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
+void vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts, const std::vector<long> &ids)
{
m_initial_guess = false;
m_points[1] = pts;
m_points_id.clear();
- if(ids.size() != pts.size()){
+ if (ids.size() != pts.size()) {
m_next_points_id = 0;
- for(size_t i=0; i < m_points[1].size(); i++)
- m_points_id.push_back(m_next_points_id ++);
- }
- else{
+ for (size_t i = 0; i < m_points[1].size(); i++)
+ m_points_id.push_back(m_next_points_id++);
+ } else {
long max = 0;
- for(size_t i=0; i < m_points[1].size(); i++){
+ for (size_t i = 0; i < m_points[1].size(); i++) {
m_points_id.push_back(ids[i]);
- if(ids[i] > max) max = ids[i];
+ if (ids[i] > max)
+ max = ids[i];
}
m_next_points_id = max + 1;
}
@@ -529,8 +509,9 @@ vpKltOpencv::initTracking(const cv::Mat &I, const std::vector<cv::Point2f> &pts,
/*!
- Add a keypoint at the end of the feature list. The id of the feature is set to ensure that it is unique.
- \param x,y : Coordinates of the feature in the image.
+ Add a keypoint at the end of the feature list. The id of the feature is set
+ to ensure that it is unique. \param x,y : Coordinates of the feature in the
+ image.
*/
void vpKltOpencv::addFeature(const float &x, const float &y)
@@ -545,7 +526,8 @@ void vpKltOpencv::addFeature(const float &x, const float &y)
Add a keypoint at the end of the feature list.
\warning This function doesn't ensure that the id of the feature is unique.
- You should rather use addFeature(const float &, const float &) or addFeature(const cv::Point2f &).
+ You should rather use addFeature(const float &, const float &) or
+ addFeature(const cv::Point2f &).
\param id : Feature id. Should be unique
\param x,y : Coordinates of the feature in the image.
@@ -562,8 +544,9 @@ void vpKltOpencv::addFeature(const long &id, const float &x, const float &y)
/*!
- Add a keypoint at the end of the feature list. The id of the feature is set to ensure that it is unique.
- \param f : Coordinates of the feature in the image.
+ Add a keypoint at the end of the feature list. The id of the feature is set
+ to ensure that it is unique. \param f : Coordinates of the feature in the
+ image.
*/
void vpKltOpencv::addFeature(const cv::Point2f &f)
@@ -578,15 +561,14 @@ void vpKltOpencv::addFeature(const cv::Point2f &f)
*/
void vpKltOpencv::suppressFeature(const int &index)
{
- if ((size_t)index >= m_points[1].size()){
+ if ((size_t)index >= m_points[1].size()) {
throw(vpException(vpException::badValue, "Feature [%d] doesn't exist", index));
}
- m_points[1].erase(m_points[1].begin()+index);
- m_points_id.erase(m_points_id.begin()+index);
+ m_points[1].erase(m_points[1].begin() + index);
+ m_points_id.erase(m_points_id.begin() + index);
}
-
#elif defined(VISP_HAVE_OPENCV)
#include <string>
@@ -595,16 +577,20 @@ void vpKltOpencv::suppressFeature(const int &index)
void vpKltOpencv::clean()
{
- if (image) cvReleaseImage(&image);
- if (prev_image) cvReleaseImage(&prev_image);
- if (pyramid) cvReleaseImage(&pyramid);
- if (prev_pyramid) cvReleaseImage(&prev_pyramid);
-
+ if (image)
+ cvReleaseImage(&image);
+ if (prev_image)
+ cvReleaseImage(&prev_image);
+ if (pyramid)
+ cvReleaseImage(&pyramid);
+ if (prev_pyramid)
+ cvReleaseImage(&prev_pyramid);
+
image = NULL;
prev_image = NULL;
pyramid = NULL;
prev_pyramid = NULL;
-
+
swap_temp = NULL;
countFeatures = 0;
countPrevFeatures = 0;
@@ -616,12 +602,18 @@ void vpKltOpencv::clean()
void vpKltOpencv::cleanAll()
{
clean();
- if (features) cvFree(&features);
- if (prev_features) cvFree(&prev_features);
- if (status) cvFree(&status);
- if (lostDuringTrack) cvFree(&lostDuringTrack);
- if (featuresid) cvFree(&featuresid);
- if (prev_featuresid) cvFree(&prev_featuresid);
+ if (features)
+ cvFree(&features);
+ if (prev_features)
+ cvFree(&prev_features);
+ if (status)
+ cvFree(&status);
+ if (lostDuringTrack)
+ cvFree(&lostDuringTrack);
+ if (featuresid)
+ cvFree(&featuresid);
+ if (prev_featuresid)
+ cvFree(&prev_featuresid);
features = NULL;
prev_features = NULL;
status = NULL;
@@ -630,43 +622,37 @@ void vpKltOpencv::cleanAll()
prev_featuresid = NULL;
}
-void vpKltOpencv::reset()
-{
- clean();
-
-}
+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]));
- status = (char*)cvAlloc((size_t)maxFeatures);
- lostDuringTrack = (bool*)cvAlloc((size_t)maxFeatures);
- featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
- prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+ features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(features[0]));
+ prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(prev_features[0]));
+ status = (char *)cvAlloc((size_t)maxFeatures);
+ lostDuringTrack = (bool *)cvAlloc((size_t)maxFeatures);
+ featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
+ prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
}
/*!
Copy constructor.
*/
-vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
- : initialized(0), maxFeatures(50), globalcountFeatures(0), win_size(10), quality(0.01),
- min_distance(10), harris_free_parameter(0.04), block_size(3), use_harris(1),
- pyramid_level(3), _tid(-1), image(NULL), prev_image(NULL), pyramid(NULL),
- prev_pyramid(NULL), swap_temp(NULL), countFeatures(0), countPrevFeatures(0),
- features(NULL), prev_features(NULL), featuresid(NULL), prev_featuresid(NULL),
- flags(0), initial_guess(false), lostDuringTrack(0), status(0), OnInitialize(0),
- OnFeatureLost(0), OnNewFeature(0), OnMeasureFeature(0), IsFeatureValid(0)
+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)
{
*this = copy;
}
@@ -674,9 +660,9 @@ vpKltOpencv::vpKltOpencv(const vpKltOpencv& copy)
/*!
Copy operator.
*/
-vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
+vpKltOpencv &vpKltOpencv::operator=(const vpKltOpencv ©)
{
- //Shallow copy of primitives
+ // Shallow copy of primitives
initialized = copy.initialized;
maxFeatures = copy.maxFeatures;
countFeatures = copy.countFeatures;
@@ -711,69 +697,63 @@ vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
globalcountFeatures = 0;
}
- if (copy.image)
- {
- image = cvCreateImage(cvGetSize(copy.image), 8, 1);
+ if (copy.image) {
+ image = cvCreateImage(cvGetSize(copy.image), 8, 1);
// /*IplImage **/cvCopyImage(copy.image,image);
cvCopy(copy.image, image, 0);
}
- if (copy.prev_image)
- {
+ if (copy.prev_image) {
prev_image = cvCreateImage(cvGetSize(copy.prev_image), IPL_DEPTH_8U, 1);
// /*IplImage **/ cvCopyImage(copy.prev_image,prev_image);
cvCopy(copy.prev_image, prev_image, 0);
}
- if (copy.pyramid)
- {
+ if (copy.pyramid) {
pyramid = cvCreateImage(cvGetSize(copy.pyramid), IPL_DEPTH_8U, 1);
// /*IplImage **/cvCopyImage(copy.pyramid,pyramid);
cvCopy(copy.pyramid, pyramid, 0);
}
- if (copy.prev_pyramid)
- {
+ if (copy.prev_pyramid) {
prev_pyramid = cvCreateImage(cvGetSize(copy.prev_pyramid), IPL_DEPTH_8U, 1);
// /*IplImage **/cvCopyImage(copy.prev_pyramid,prev_pyramid);
cvCopy(copy.prev_pyramid, prev_pyramid, 0);
}
- //Deep copy of arrays
+ // 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];
}
@@ -781,10 +761,7 @@ vpKltOpencv & vpKltOpencv::operator=(const vpKltOpencv& copy)
return *this;
}
-vpKltOpencv::~vpKltOpencv()
-{
- cleanAll();
-}
+vpKltOpencv::~vpKltOpencv() { cleanAll(); }
/*!
Set the maximum number of features to track in the image.
@@ -793,31 +770,38 @@ vpKltOpencv::~vpKltOpencv()
\param input : The new number of maximum features.
*/
-void vpKltOpencv::setMaxFeatures(const int input) {
- initialized = 0; maxFeatures=input;
-
- if (features) cvFree(&features);
- if (prev_features) cvFree(&prev_features);
- if (status) cvFree(&status);
- if (lostDuringTrack) cvFree(&lostDuringTrack);
- if (featuresid) cvFree(&featuresid);
- if (prev_featuresid) cvFree(&prev_featuresid);
-
-
- features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
- prev_features = (CvPoint2D32f*)cvAlloc((unsigned int)maxFeatures*sizeof(CvPoint2D32f));
- status = (char*)cvAlloc((unsigned int)maxFeatures*sizeof(char));
- lostDuringTrack = (bool*)cvAlloc((unsigned int)maxFeatures*sizeof(bool));
- featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
- prev_featuresid = (long*)cvAlloc((unsigned int)maxFeatures*sizeof(long));
+void vpKltOpencv::setMaxFeatures(const int input)
+{
+ initialized = 0;
+ maxFeatures = input;
+
+ if (features)
+ cvFree(&features);
+ if (prev_features)
+ cvFree(&prev_features);
+ if (status)
+ cvFree(&status);
+ if (lostDuringTrack)
+ cvFree(&lostDuringTrack);
+ if (featuresid)
+ cvFree(&featuresid);
+ if (prev_featuresid)
+ cvFree(&prev_featuresid);
+
+ features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
+ prev_features = (CvPoint2D32f *)cvAlloc((unsigned int)maxFeatures * sizeof(CvPoint2D32f));
+ status = (char *)cvAlloc((unsigned int)maxFeatures * sizeof(char));
+ lostDuringTrack = (bool *)cvAlloc((unsigned int)maxFeatures * sizeof(bool));
+ featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
+ prev_featuresid = (long *)cvAlloc((unsigned int)maxFeatures * sizeof(long));
}
/*!
Initialise the tracking by extracting KLT keypoints on the provided image.
- \param I : Grey level image used as input. This image should have only 1 channel.
- \param mask : Image mask used to restrict the keypoint detection area.
- If mask is NULL, all the image will be considered.
+ \param I : Grey level image used as input. This image should have only 1
+ channel. \param mask : Image mask used to restrict the keypoint detection
+ area. If mask is NULL, all the image will be considered.
\exception vpTrackingException::initializationError : If the image I is not
initialized, or if the image or the mask have bad coding format.
@@ -825,34 +809,36 @@ 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")) ;
+ if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
}
if (mask) {
- if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
- throw(vpException(vpTrackingException::initializationError, "Bad Image format")) ;
+ if (mask->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
}
}
- //Creation des buffers
+ // Creation des buffers
CvSize Sizeim, SizeI;
SizeI = cvGetSize(I);
bool b_imOK = true;
- if(image != NULL){
+ if (image != NULL) {
Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ 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;
+ image = cvCreateImage(cvGetSize(I), 8, 1);
+ image->origin = I->origin;
prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
- }else{
+ } else {
swap_temp = 0;
countFeatures = 0;
countPrevFeatures = 0;
@@ -863,63 +849,61 @@ void vpKltOpencv::initTracking(const IplImage *I, const IplImage *mask)
initialized = 1;
- //Import
+ // Import
cvCopy(I, image, 0);
- //Recherche de points d'interets
+ // Recherche de points d'interets
countFeatures = maxFeatures;
countPrevFeatures = 0;
- IplImage* eig = cvCreateImage(cvGetSize(image), 32, 1);
- IplImage* temp = cvCreateImage(cvGetSize(image), 32, 1);
- cvGoodFeaturesToTrack(image, eig, temp, features,
- &countFeatures, quality, min_distance,
- mask, block_size, use_harris, harris_free_parameter);
- cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size),
- cvSize(-1,-1),cvTermCriteria(CV_TERMCRIT_ITER|
- CV_TERMCRIT_EPS,20,0.03));
+ IplImage *eig = cvCreateImage(cvGetSize(image), 32, 1);
+ IplImage *temp = cvCreateImage(cvGetSize(image), 32, 1);
+ cvGoodFeaturesToTrack(image, eig, temp, features, &countFeatures, quality, min_distance, mask, block_size, use_harris,
+ harris_free_parameter);
+ cvFindCornerSubPix(image, features, countFeatures, cvSize(win_size, win_size), cvSize(-1, -1),
+ cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03));
cvReleaseImage(&eig);
cvReleaseImage(&temp);
if (OnInitialize)
OnInitialize(_tid);
- //printf("Number of features at init: %d\n", countFeatures);
- for (int boucle=0; boucle<countFeatures;boucle++) {
+ // printf("Number of features at init: %d\n", countFeatures);
+ for (int boucle = 0; boucle < countFeatures; boucle++) {
featuresid[boucle] = globalcountFeatures;
globalcountFeatures++;
-
- if (OnNewFeature){
- OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x,
- features[boucle].y);
+
+ if (OnNewFeature) {
+ OnNewFeature(_tid, boucle, featuresid[boucle], features[boucle].x, features[boucle].y);
}
}
}
/*!
- Set the points that will be used as initialization during the next call to track().
+ Set the points that will be used as initialization during the next call to
+ track().
\param I : Input image.
\param pts : Vector of points that should be tracked.
*/
-void
-vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
+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
+ // Creation des buffers
CvSize Sizeim, SizeI;
SizeI = cvGetSize(I);
bool b_imOK = true;
- if(image != NULL){
+ if (image != NULL) {
Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ 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;
+ image = cvCreateImage(cvGetSize(I), 8, 1);
+ image->origin = I->origin;
prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
@@ -928,7 +912,7 @@ vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, int size)
}
// Save current features as previous features
countFeatures = size;
- for (int i=0; i<countFeatures;i++) {
+ for (int i = 0; i < countFeatures; i++) {
features[i] = pts[i];
featuresid[i] = i;
}
@@ -939,24 +923,24 @@ 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
+ // Creation des buffers
CvSize Sizeim, SizeI;
SizeI = cvGetSize(I);
bool b_imOK = true;
- if(image != NULL){
+ if (image != NULL) {
Sizeim = cvGetSize(image);
- if(SizeI.width != Sizeim.width || SizeI.height != Sizeim.height) b_imOK = false;
+ 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;
+ image = cvCreateImage(cvGetSize(I), 8, 1);
+ image->origin = I->origin;
prev_image = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
prev_pyramid = cvCreateImage(cvGetSize(I), IPL_DEPTH_8U, 1);
@@ -966,7 +950,7 @@ vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int s
// Save current features as previous features
countFeatures = size;
long max = 0;
- for (int i=0; i<countFeatures;i++) {
+ for (int i = 0; i < countFeatures; i++) {
features[i] = pts[i];
featuresid[i] = fid[i];
if (fid[i] > max)
@@ -982,67 +966,59 @@ vpKltOpencv::initTracking(const IplImage *I, CvPoint2D32f *pts, long *fid, int s
void vpKltOpencv::track(const IplImage *I)
{
if (!initialized) {
- vpERROR_TRACE("KLT Not initialized") ;
- throw(vpException(vpTrackingException::initializationError,
- "KLT Not initialized")) ;
+ vpERROR_TRACE("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")) ;
+ if (I->depth != IPL_DEPTH_8U || I->nChannels != 1) {
+ throw(vpException(vpTrackingException::initializationError, "Bad Image format"));
}
-
-
CV_SWAP(prev_image, image, swap_temp);
CV_SWAP(prev_pyramid, pyramid, swap_temp);
-
+
cvCopy(I, image, 0);
-
- if(!initial_guess){
+
+ if (!initial_guess) {
// Save current features as previous features
countPrevFeatures = countFeatures;
- for (int boucle=0; boucle<countFeatures;boucle++) {
+ for (int boucle = 0; boucle < countFeatures; boucle++) {
prev_featuresid[boucle] = featuresid[boucle];
}
-
+
CvPoint2D32f *swap_features = 0;
CV_SWAP(prev_features, features, swap_features);
}
-
- if (countFeatures <= 0) return;
-
- cvCalcOpticalFlowPyrLK( prev_image, image, prev_pyramid, pyramid,
- prev_features, features, countFeatures,
- cvSize(win_size, win_size), pyramid_level,
- status, 0, cvTermCriteria(CV_TERMCRIT_ITER
- |CV_TERMCRIT_EPS,20,0.03),
- flags );
-
- if(!initial_guess)
+
+ if (countFeatures <= 0)
+ return;
+
+ cvCalcOpticalFlowPyrLK(prev_image, image, prev_pyramid, pyramid, prev_features, features, countFeatures,
+ cvSize(win_size, win_size), pyramid_level, status, 0,
+ cvTermCriteria(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.03), flags);
+
+ if (!initial_guess)
flags |= CV_LKFLOW_PYR_A_READY;
- else{
+ else {
flags = CV_LKFLOW_PYR_A_READY;
initial_guess = false;
}
- int i,k;
- for (i = k = 0; i < countFeatures ; i++) {
- if (!status[i]) {
+ int i, k;
+ for (i = k = 0; i < countFeatures; i++) {
+ if (!status[i]) {
lostDuringTrack[i] = 1;
if (OnFeatureLost)
- OnFeatureLost(_tid, i, featuresid[i], features[i].x,
- features[i].y);
+ OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
continue;
}
-
- if (IsFeatureValid) {
- if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
+
+ if (IsFeatureValid) {
+ if (!IsFeatureValid(_tid, features[i].x, features[i].y)) {
lostDuringTrack[i] = 1;
if (OnFeatureLost)
OnFeatureLost(_tid, i, featuresid[i], features[i].x, features[i].y);
@@ -1052,8 +1028,9 @@ void vpKltOpencv::track(const IplImage *I)
features[k] = features[i];
featuresid[k] = featuresid[i];
- if (OnMeasureFeature) OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
-
+ if (OnMeasureFeature)
+ OnMeasureFeature(_tid, k, featuresid[k], features[k].x, features[k].y);
+
lostDuringTrack[i] = 0;
k++;
}
@@ -1067,13 +1044,11 @@ 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<unsigned char> &I,
- vpColor color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<unsigned char> &I, vpColor color, unsigned int thickness)
{
- if ((features == 0) || (I.bitmap==0) || (!initialized))
- {
+ if ((features == 0) || (I.bitmap == 0) || (!initialized)) {
vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ throw(vpException(vpException::memoryAllocationError, " Memory problem"));
}
vpKltOpencv::display(I, features, featuresid, countFeatures, color, thickness);
@@ -1085,7 +1060,7 @@ void vpKltOpencv::display(const vpImage<unsigned char> &I,
getFeature(i,...) may not represent the same feature before and
after a tracking iteration (if a feature is lost, features are
shifted in the array).
-
+
\param index : index of feature
\param id : id of the feature
\param x : x coordinate
@@ -1094,10 +1069,9 @@ void vpKltOpencv::display(const vpImage<unsigned char> &I,
*/
void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
{
- if (index >= countFeatures)
- {
+ if (index >= countFeatures) {
vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ throw(vpException(vpException::memoryAllocationError, " Memory problem"));
}
x = features[index].x;
@@ -1105,53 +1079,53 @@ void vpKltOpencv::getFeature(int index, long &id, float &x, float &y) const
id = featuresid[index];
}
-
/*!
- Set the points that will be used as initial guess during the next call to track().
-
+ Set the points that will be used as initial guess during the next call to
+ track().
+
\warning Those points will be used just one time (next track()).
-
- \param guess_pts : Reference on an array of CvPoint2D32f allocated with cvAlloc().
+
+ \param guess_pts : Reference on an array of CvPoint2D32f allocated with
+ cvAlloc().
*/
-void
-vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
+void vpKltOpencv::setInitialGuess(CvPoint2D32f **guess_pts)
{
// Save current features as previous features
countPrevFeatures = countFeatures;
- for (int boucle=0; boucle<countFeatures;boucle++) {
+ for (int boucle = 0; boucle < countFeatures; boucle++) {
prev_featuresid[boucle] = featuresid[boucle];
}
-
+
CvPoint2D32f *swap_features = NULL;
CV_SWAP(prev_features, *guess_pts, swap_features);
-
+
CV_SWAP(features, prev_features, swap_features);
-
+
flags |= CV_LKFLOW_INITIAL_GUESSES;
-
+
initial_guess = true;
}
/*!
- Set the points that will be used as initial guess during the next call to track().
- A typical usage of this function is to predict the position of the features before the
- next call to track().
-
- \param init_pts : Initial points (could be obtained from getPrevFeatures() or getFeatures()).
- \param guess_pts : Prediction of the new position of the initial points. The size of this vector must be the same as the size of the vector of initial points.
- \param fid : Identifiers of the initial points.
+ Set the points that will be used as initial guess during the next call to
+ track(). A typical usage of this function is to predict the position of the
+ features before the next call to track().
+
+ \param init_pts : Initial points (could be obtained from getPrevFeatures()
+ or getFeatures()). \param guess_pts : Prediction of the new position of the
+ initial points. The size of this vector must be the same as the size of the
+ vector of initial points. \param fid : Identifiers of the initial points.
\param size : size of the vectors.
\sa getPrevFeatures(),getPrevFeaturesId
\sa getFeatures(), getFeaturesId
\sa initTracking()
*/
-void
-vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
+void vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts, long *fid, int size)
{
countPrevFeatures = size;
countFeatures = size;
- for (int boucle=0; boucle<size;boucle++) {
+ for (int boucle = 0; boucle < size; boucle++) {
prev_featuresid[boucle] = fid[boucle];
featuresid[boucle] = fid[boucle];
}
@@ -1160,8 +1134,8 @@ vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts,
CvPoint2D32f *swap_features2 = NULL;
CV_SWAP(prev_features, *init_pts, swap_features);
-// if(swap_features) cvFree(&swap_features);
-// swap_features = NULL;
+ // if(swap_features) cvFree(&swap_features);
+ // swap_features = NULL;
CV_SWAP(features, *guess_pts, swap_features2);
@@ -1180,10 +1154,9 @@ vpKltOpencv::setInitialGuess(CvPoint2D32f **init_pts, CvPoint2D32f **guess_pts,
*/
void vpKltOpencv::getPrevFeature(int index, int &id, float &x, float &y) const
{
- if (index >= countPrevFeatures)
- {
+ if (index >= countPrevFeatures) {
vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ throw(vpException(vpException::memoryAllocationError, " Memory problem"));
}
x = prev_features[index].x;
@@ -1197,11 +1170,9 @@ Add at the end of the feauture list.
If there is no space left, the feature is not added (just return)
*/
-void vpKltOpencv::addFeature(const int &id,
- const float &x, const float &y)
+void vpKltOpencv::addFeature(const int &id, const float &x, const float &y)
{
- if (maxFeatures == countFeatures)
- {
+ if (maxFeatures == countFeatures) {
vpERROR_TRACE(" Cannot add the feature ");
return;
}
@@ -1211,22 +1182,21 @@ void vpKltOpencv::addFeature(const int &id,
f.y = y;
features[countFeatures] = f;
featuresid[countFeatures] = id;
- countFeatures ++;
+ countFeatures++;
}
void vpKltOpencv::suppressFeature(int index)
{
- if (index >= countFeatures)
- {
+ if (index >= countFeatures) {
vpERROR_TRACE(" Memory problem ");
- throw(vpException(vpException::memoryAllocationError," Memory problem"));
+ throw(vpException(vpException::memoryAllocationError, " Memory problem"));
}
- countFeatures --;
+ countFeatures--;
- for (int i=index ; i < countFeatures; i ++) {
- features[i] = features[i+1];
- featuresid[i] = featuresid[i+1];
+ for (int i = index; i < countFeatures; i++) {
+ features[i] = features[i + 1];
+ featuresid[i] = featuresid[i + 1];
}
}
@@ -1244,15 +1214,14 @@ void vpKltOpencv::suppressFeature(int index)
\param thickness : Thickness of the points.
*/
-void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
+ vpColor color, unsigned int thickness)
{
vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+ for (int i = 0; i < nbFeatures; i++) {
+ ip.set_u(vpMath::round(features_list[i].x));
+ ip.set_v(vpMath::round(features_list[i].y));
+ vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
}
}
/*!
@@ -1269,15 +1238,14 @@ void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* fe
\param thickness : Thickness of the points.
*/
-void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
- const int &nbFeatures, vpColor color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const int &nbFeatures,
+ vpColor color, unsigned int thickness)
{
vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+ for (int i = 0; i < nbFeatures; i++) {
+ ip.set_u(vpMath::round(features_list[i].x));
+ ip.set_v(vpMath::round(features_list[i].y));
+ vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
}
}
@@ -1297,20 +1265,18 @@ void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_
\param thickness : Thickness of the points
*/
-void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<unsigned char> &I, const CvPoint2D32f *features_list,
+ const long *featuresid_list, const int &nbFeatures, vpColor color, unsigned int thickness)
{
vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10+thickness, color, thickness) ;
+ for (int i = 0; i < nbFeatures; i++) {
+ ip.set_u(vpMath::round(features_list[i].x));
+ ip.set_v(vpMath::round(features_list[i].y));
+ vpDisplay::displayCross(I, ip, 10 + thickness, color, thickness);
char id[10];
sprintf(id, "%ld", featuresid_list[i]);
- ip.set_u( vpMath::round( features_list[i].x + 5 ) );
+ ip.set_u(vpMath::round(features_list[i].x + 5));
vpDisplay::displayText(I, ip, id, color);
}
}
@@ -1331,35 +1297,35 @@ void vpKltOpencv::display(const vpImage<unsigned char>& I,const CvPoint2D32f* fe
\param thickness : Thickness of the points
*/
-void vpKltOpencv::display(const vpImage<vpRGBa>& I,const CvPoint2D32f* features_list,
- const long *featuresid_list, const int &nbFeatures,
- vpColor color, unsigned int thickness)
+void vpKltOpencv::display(const vpImage<vpRGBa> &I, const CvPoint2D32f *features_list, const long *featuresid_list,
+ const int &nbFeatures, vpColor color, unsigned int thickness)
{
vpImagePoint ip;
- for (int i = 0 ; i < nbFeatures ; i++)
- {
- ip.set_u( vpMath::round(features_list[i].x ) );
- ip.set_v( vpMath::round(features_list[i].y ) );
- vpDisplay::displayCross(I, ip, 10, color, thickness) ;
+ for (int i = 0; i < nbFeatures; i++) {
+ ip.set_u(vpMath::round(features_list[i].x));
+ ip.set_v(vpMath::round(features_list[i].y));
+ vpDisplay::displayCross(I, ip, 10, color, thickness);
char id[10];
sprintf(id, "%ld", featuresid_list[i]);
- ip.set_u( vpMath::round( features_list[i].x + 5 ) );
+ ip.set_u(vpMath::round(features_list[i].x + 5));
vpDisplay::displayText(I, ip, id, color);
}
}
#else
-// Work arround to avoid visp_klt library empty when OpenCV is not installed or used
+// Work arround to avoid visp_klt library empty when OpenCV is not installed
+// or used
class VISP_EXPORT dummy_vpKltOpencv
{
public:
- dummy_vpKltOpencv() {};
+ dummy_vpKltOpencv(){};
};
#if !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no symbols
-void dummy_vpKltOpenCV_fct() {};
+// Work arround to avoid warning: libvisp_klt.a(vpKltOpenCV.cpp.o) has no
+// symbols
+void dummy_vpKltOpenCV_fct(){};
#endif
#endif
diff --git a/modules/tracker/mbt/CMakeLists.txt b/modules/tracker/mbt/CMakeLists.txt
index fceb2a8..f706c35 100644
--- a/modules/tracker/mbt/CMakeLists.txt
+++ b/modules/tracker/mbt/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -39,7 +40,6 @@
set(opt_incs "")
set(opt_libs "")
-# If
if(USE_COIN3D AND NOT HAVE_visp_ar)
if(WIN32)
add_definitions("-DCOIN_DLL")
@@ -107,24 +107,118 @@ if(USE_COIN3D AND NOT HAVE_visp_ar)
endif()
endif()
-vp_add_module(mbt visp_vision visp_core visp_me visp_visual_features OPTIONAL visp_ar visp_klt visp_gui)
+if(USE_PCL)
+ list(APPEND opt_incs ${PCL_INCLUDE_DIRS})
+
+ # list(APPEND opt_libs ${PCL_LIBRARIES})
+ # Using PCL_LIBRARIES works to build visp library, examples, demos and test thanks to the components,
+ # but not tutorials when they are build outside ViSP as they are stand alone CMake projects that use
+ # ViSP as a 3rd party.
+ # To be clear PCL_LIBRARIES contains VTK 3rd party such as vtkalglib and not /usr/local/Cellar/vtk/6.3.0/lib/libvtkalglib-6.3.1.dylib
+ # full path as requested to use ViSP as 3rd party. This is the case for all VTK libraries that are PCL dependencies.
+ # The build of ViSP works with PCL_LIBRARIES since in that case thanks to vtkalglib properties, CMake
+ # is able to find the real name and location of the libraries.
+ # But when ViSP is used as a 3rd party where it should import PCL libraries, it doesn't work with
+ # PCL_LIBRARIES and especially with VTK_LIBRARIES.
+ # The solution here is to get the full location of VTK_LIBRARIES libraries thanks to the properties and link
+ # with these names.
+ # An other way could be to include PCLConfig.cmake, but in that case, visp-config and visp.pc
+ # will be not able to give the names of PCL libraries when used without CMake.
+ foreach(lib_ ${PCL_LIBRARIES})
+ # check if ${lib_} is in VTK_LIBRARIES and is a TARGET. If this is the case, we retrieve the full location
+ list(FIND VTK_LIBRARIES ${lib_} lib_is_vtk_)
+ if(NOT ${lib_is_vtk_} EQUAL -1 AND TARGET ${lib_})
+ # This is a VTK library
+ list(APPEND PCL_VTK_LIBRARIES ${lib_})
+ else()
+ # Other libraries sqlite3, boost..., optimized, debug
+ if(EXISTS ${lib_} OR ${lib_} MATCHES "optimized" OR ${lib_} MATCHES "debug")
+ list(APPEND opt_libs ${lib_})
+ else()
+ find_library(${lib_}_LIBRARY ${lib_} QUIET)
+ mark_as_advanced(${lib_}_LIBRARY)
+ if(${lib_}_LIBRARY)
+ list(APPEND opt_libs ${${lib_}_LIBRARY})
+ else()
+ list(APPEND opt_libs ${lib_})
+ endif()
+ endif()
+ endif()
+ endforeach()
+
+ find_package(VTK QUIET)
+ if (VTK_FOUND AND NOT ANDROID)
+ # Fix for Ubuntu 16.04 to add vtkFiltering as dependency. Note that vtkFiltering does't exists on OSX
+ list(FIND VTK_LIBRARIES "vtkFiltering" vtkFiltering_exists_)
+ if(NOT ${vtkFiltering_exists_} EQUAL -1)
+ list(APPEND PCL_VTK_LIBRARIES "vtkFiltering") # seems required on Ubuntu 16.04
+ endif()
+ if(VTK_VERSION VERSION_EQUAL 6.2.0)
+ # Work arround to avoid build issue on ubuntu 16.04 with libvtk6-dev package
+ # cannot find -lvtkproj4
+ # See https://bugs.launchpad.net/ubuntu/+source/vtk6/+bug/1573234
+ list(REMOVE_ITEM opt_libs "vtkproj4")
+ endif()
+ set(config_ "NONE" "RELEASE" "DEBUG" "RELEASEWITHDEBINFO" "RELWITHDEBINFO")
+ foreach(lib_ ${PCL_VTK_LIBRARIES})
+ foreach(imp_config_ ${config_})
+ get_target_property(lib_property_${imp_config_}_ ${lib_} IMPORTED_IMPLIB_${imp_config_})
+ if(NOT EXISTS ${lib_property_${imp_config_}_})
+ get_target_property(lib_property_${imp_config_}_ ${lib_} IMPORTED_LOCATION_${imp_config_})
+ endif()
+ # Under Unix, there is no specific suffix for PCL/VTK libraries. # Under Windows, we add the "optimized", "debug" specific keywords
+ if(WIN32 AND EXISTS "${lib_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "RELEASE") # also valid for RELEASEWITHDEBINFO
+ list(APPEND opt_libs optimized "${lib_property_${imp_config_}_}")
+ elseif(WIN32 AND EXISTS "${lib_property_${imp_config_}_}" AND "${imp_config_}" MATCHES "DEBUG")
+ list(APPEND opt_libs debug "${lib_property_${imp_config_}_}")
+ elseif(EXISTS "${lib_property_${imp_config_}_}")
+ list(APPEND opt_libs "${lib_property_${imp_config_}_}")
+ endif()
+ endforeach()
+ endforeach()
+ if(VTK_VERSION VERSION_EQUAL 6.2.0)
+ mark_as_advanced(Qt5Core_DIR Qt5Gui_DIR Qt5Network_DIR Qt5WebKit_DIR Qt5Widgets_DIR)
+ elseif(VTK_VERSION VERSION_EQUAL 7.1.0 OR VTK_VERSION VERSION_EQUAL 8.0.0)
+ mark_as_advanced(DAVIDSDK_INCLUDE_DIR DAVIDSDK_LIBRARY DSSDK_DIR ENSENSO_INCLUDE_DIR ENSENSO_LIBRARY)
+ mark_as_advanced(GLEW_INCLUDE_DIR GLEW_GLEW_LIBRARY)
+ mark_as_advanced(Qt5Core_DIR Qt5Gui_DIR Qt5Sql_DIR Qt5Widgets_DIR RSSDK_DIR)
+ endif()
+ endif()
+endif()
+
+if(WITH_CLIPPER)
+ # clipper is private
+ include_directories(${CLIPPER_INCLUDE_DIRS})
+endif()
+
+vp_add_module(mbt visp_vision visp_core visp_me visp_visual_features OPTIONAL visp_ar visp_klt visp_gui PRIVATE_OPTIONAL ${CLIPPER_LIBRARIES})
vp_glob_module_sources()
if(USE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- vp_set_source_file_compile_flag(src/edge/vpMbEdgeMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/edge/vpMbEdgeTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/edge/vpMbtDistanceCircle.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/edge/vpMbtDistanceCylinder.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/edge/vpMbtDistanceLine.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/hybrid/vpMbEdgeKltMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/hybrid/vpMbEdgeKltTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/klt/vpMbKltMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/klt/vpMbKltTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/klt/vpMbtDistanceKltCylinder.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/klt/vpMbtDistanceKltPoints.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(src/vpMbTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ vp_set_source_file_compile_flag(src/edge/vpMbEdgeMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/edge/vpMbEdgeTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/edge/vpMbtDistanceCircle.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/edge/vpMbtDistanceCylinder.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/edge/vpMbtDistanceLine.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/hybrid/vpMbEdgeKltMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/hybrid/vpMbEdgeKltTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/klt/vpMbKltMultiTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/klt/vpMbKltTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/klt/vpMbtDistanceKltCylinder.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/klt/vpMbtDistanceKltPoints.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/vpMbTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/depth/vpMbtFaceDepthDense.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/depth/vpMbtFaceDepthNormal.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/depth/vpMbDepthDenseTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/depth/vpMbDepthNormalTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/vpMbtXmlGenericParser.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(src/vpMbGenericTracker.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+endif()
+if(MSVC)
+ vp_set_source_file_compile_flag(src/depth/vpMbtTukeyEstimator.cpp /wd"4244")
endif()
vp_module_include_directories(${opt_incs})
vp_create_module(${opt_libs})
+vp_add_tests(DEPENDS_ON visp_core)
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h
new file mode 100644
index 0000000..8e8d2b1
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthDenseTracker.h
@@ -0,0 +1,168 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Model-based tracker using depth dense features.
+ *
+ *****************************************************************************/
+
+#ifndef __vpMbDepthDenseTracker_h_
+#define __vpMbDepthDenseTracker_h_
+
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/mbt/vpMbtFaceDepthDense.h>
+#include <visp3/mbt/vpMbtTukeyEstimator.h>
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+#include <visp3/core/vpDisplay.h>
+#endif
+
+class VISP_EXPORT vpMbDepthDenseTracker : public virtual vpMbTracker
+{
+public:
+ vpMbDepthDenseTracker();
+ virtual ~vpMbDepthDenseTracker();
+
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual inline vpColVector getError() const { return m_error_depthDense; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w_depthDense; }
+
+ virtual void init(const vpImage<unsigned char> &I);
+
+ virtual void loadConfigFile(const std::string &configFile);
+
+ void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose = false);
+#if defined(VISP_HAVE_PCL)
+ void reInitModel(const pcl::PointCloud<pcl::PointXYZ>::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 setDepthDenseFilteringMaxDistance(const double maxDistance);
+ virtual void setDepthDenseFilteringMethod(const int method);
+ virtual void setDepthDenseFilteringMinDistance(const double minDistance);
+ virtual void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio);
+
+ 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;
+ return;
+ }
+
+ m_depthDenseSamplingStepX = stepX;
+ m_depthDenseSamplingStepY = stepY;
+ }
+
+ virtual void setOgreVisibilityTest(const bool &v);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
+#ifdef VISP_HAVE_PCL
+ virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
+#endif
+
+ virtual void setScanLineVisibilityTest(const bool &v);
+
+ virtual void testTracking();
+
+ virtual void track(const vpImage<unsigned char> &);
+#ifdef VISP_HAVE_PCL
+ virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
+#endif
+ virtual void track(const std::vector<vpColVector> &point_cloud, const unsigned int width, const unsigned int height);
+
+protected:
+ //! Set of faces describing the object used only for display with scan line.
+ vpMbHiddenFaces<vpMbtPolygon> m_depthDenseHiddenFacesDisplay;
+ //! Dummy image used to compute the visibility
+ vpImage<unsigned char> m_depthDenseI_dummyVisibility;
+ //! List of current active (visible and features extracted) faces
+ std::vector<vpMbtFaceDepthDense *> m_depthDenseListOfActiveFaces;
+ //! Nb features
+ unsigned int m_denseDepthNbFeatures;
+ //! List of faces
+ std::vector<vpMbtFaceDepthDense *> m_depthDenseNormalFaces;
+ //! Sampling step in x-direction
+ unsigned int m_depthDenseSamplingStepX;
+ //! Sampling step in y-direction
+ unsigned int m_depthDenseSamplingStepY;
+ //! (s - s*)
+ vpColVector m_error_depthDense;
+ //! Interaction matrix
+ vpMatrix m_L_depthDense;
+ //! Tukey M-Estimator
+ vpMbtTukeyEstimator<double> m_robust_depthDense;
+ //! Robust weights
+ vpColVector m_w_depthDense;
+ //! Weighted error
+ vpColVector m_weightedError_depthDense;
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ vpDisplay *m_debugDisp_depthDense;
+ vpImage<unsigned char> m_debugImage_depthDense;
+#endif
+
+ void addFace(vpMbtPolygon &polygon, const bool alreadyClose);
+
+ void computeVisibility(const unsigned int width, const unsigned int height);
+
+ void computeVVS();
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+ virtual void computeVVSWeights();
+ using vpMbTracker::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 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);
+
+#ifdef VISP_HAVE_PCL
+ void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
+#endif
+ void segmentPointCloud(const std::vector<vpColVector> &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
new file mode 100644
index 0000000..14da14d
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbDepthNormalTracker.h
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Model-based tracker using depth normal features.
+ *
+ *****************************************************************************/
+
+#ifndef __vpMbDepthNormalTracker_h_
+#define __vpMbDepthNormalTracker_h_
+
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/mbt/vpMbtFaceDepthNormal.h>
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+#include <visp3/core/vpDisplay.h>
+#endif
+
+class VISP_EXPORT vpMbDepthNormalTracker : public virtual vpMbTracker
+{
+public:
+ vpMbDepthNormalTracker();
+ virtual ~vpMbDepthNormalTracker();
+
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ 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 void init(const vpImage<unsigned char> &I);
+
+ virtual void loadConfigFile(const std::string &configFile);
+
+ void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose = false);
+#if defined(VISP_HAVE_PCL)
+ void reInitModel(const pcl::PointCloud<pcl::PointXYZ>::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 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 setDepthNormalUseRobust(const bool use);
+
+ virtual void setOgreVisibilityTest(const bool &v);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
+#if defined(VISP_HAVE_PCL)
+ virtual void setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const vpHomogeneousMatrix &cdMo);
+#endif
+
+ virtual void setScanLineVisibilityTest(const bool &v);
+
+ virtual void testTracking();
+
+ virtual void track(const vpImage<unsigned char> &);
+#if defined(VISP_HAVE_PCL)
+ virtual void track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
+#endif
+ virtual void track(const std::vector<vpColVector> &point_cloud, const unsigned int width, const unsigned int height);
+
+protected:
+ //! Method to estimate the desired features
+ vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod;
+ //! Set of faces describing the object used only for display with scan line.
+ vpMbHiddenFaces<vpMbtPolygon> m_depthNormalHiddenFacesDisplay;
+ //! Dummy image used to compute the visibility
+ vpImage<unsigned char> m_depthNormalI_dummyVisibility;
+ //! List of current active (visible and with features extracted) faces
+ std::vector<vpMbtFaceDepthNormal *> m_depthNormalListOfActiveFaces;
+ //! List of desired features
+ std::vector<vpColVector> m_depthNormalListOfDesiredFeatures;
+ //! List of faces
+ std::vector<vpMbtFaceDepthNormal *> m_depthNormalFaces;
+ //! PCL plane estimation method
+ int m_depthNormalPclPlaneEstimationMethod;
+ //! PCL RANSAC maximum number of iterations
+ int m_depthNormalPclPlaneEstimationRansacMaxIter;
+ //! PCL RANSAC threshold
+ double m_depthNormalPclPlaneEstimationRansacThreshold;
+ //! Sampling step in x-direction
+ unsigned int m_depthNormalSamplingStepX;
+ //! Sampling step in y-direction
+ unsigned int m_depthNormalSamplingStepY;
+ //! If true, use Tukey robust M-Estimator
+ bool m_depthNormalUseRobust;
+ //! (s - s*)
+ vpColVector m_error_depthNormal;
+ //! Interaction matrix
+ vpMatrix m_L_depthNormal;
+ //! Robust
+ vpRobust m_robust_depthNormal;
+ //! Robust weights
+ vpColVector m_w_depthNormal;
+ //! Weighted error
+ vpColVector m_weightedError_depthNormal;
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ vpDisplay *m_debugDisp_depthNormal;
+ vpImage<unsigned char> m_debugImage_depthNormal;
+#endif
+
+ void addFace(vpMbtPolygon &polygon, const bool alreadyClose);
+
+ void computeVisibility(const unsigned int width, const unsigned int height);
+
+ void computeVVS();
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+
+ virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace = 0, const std::string &name = "");
+
+ virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
+ const std::string &name = "");
+
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon);
+
+ virtual void initFaceFromLines(vpMbtPolygon &polygon);
+
+#ifdef VISP_HAVE_PCL
+ void segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud);
+#endif
+ void segmentPointCloud(const std::vector<vpColVector> &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 0aea500..689b300 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltMultiTracker.h
@@ -2,19 +2,20 @@
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
- * GPL, please contact INRIA about acquiring a ViSP Professional
+ * GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See https://visp.inria.fr/download/ for more information.
+ *
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
@@ -24,7 +25,7 @@
*
* If you have questions regarding the use of this file, please contact
* INRIA at visp at inria.fr
- *
+ *
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
@@ -50,7 +51,7 @@
#include <visp3/core/vpConfig.h>
-#if (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 <visp3/mbt/vpMbEdgeMultiTracker.h>
#include <visp3/mbt/vpMbKltMultiTracker.h>
@@ -60,23 +61,20 @@
\ingroup group_mbt_trackers
\warning This class is only available if OpenCV is installed, and used.
- \brief Hybrid stereo (or more) tracker based on moving-edges and keypoints tracked using KLT
- tracker.
+ \brief Hybrid stereo (or more) tracker based on moving-edges and keypoints
+ tracked using KLT tracker.
- The \ref tutorial-tracking-mb-stereo is a good starting point to use this class.
+ The \ref tutorial-tracking-mb-stereo is a good starting point to use this
+ class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
+ The tracker requires the knowledge of the 3D model that could be provided in
+ a vrml or in a cao file. The cao format is described in loadCAOModel(). It
+ may also use an xml file used to tune the behavior of the tracker and an
init file used to compute the pose at the very first image.
*/
-class VISP_EXPORT vpMbEdgeKltMultiTracker: public vpMbEdgeMultiTracker, public vpMbKltMultiTracker
+class VISP_EXPORT vpMbEdgeKltMultiTracker : public vpMbEdgeMultiTracker, public vpMbKltMultiTracker
{
protected:
- //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
- bool compute_interaction;
- //! The gain of the virtual visual servoing stage.
- double lambda;
//! Factor for KLT trackers.
double m_factorKLT;
//! Factor for edge trackers.
@@ -85,15 +83,23 @@ protected:
double thresholdKLT;
//! The threshold used in the robust estimation of MBT.
double thresholdMBT;
- //! The maximum iteration of the virtual visual servoing stage.
- unsigned int maxIter;
- //! Map of camera transformation matrix between the current camera frame to the reference camera frame (cCurrent_M_cRef)
+ //! Map of camera transformation matrix between the current camera frame to
+ //! the reference camera frame (cCurrent_M_cRef)
std::map<std::string, vpHomogeneousMatrix> m_mapOfCameraTransformationMatrix;
//! Name of the reference camera
std::string m_referenceCameraName;
-
+ //! Number of features
+ unsigned int m_nbrow;
+ //! Interaction matrix
+ vpMatrix m_L_hybridMulti;
+ //! (s - s*)
+ vpColVector m_error_hybridMulti;
+ //! Robust weights
+ vpColVector m_w_hybridMulti;
+ //! Weighted error
+ vpColVector m_weightedError_hybridMulti;
public:
vpMbEdgeKltMultiTracker();
@@ -102,29 +108,30 @@ public:
virtual ~vpMbEdgeKltMultiTracker();
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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<unsigned char> &I1, const vpImage<unsigned char> &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<vpRGBa>& I1, const vpImage<vpRGBa>& 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<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual void display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual std::vector<std::string> getCameraNames() const;
@@ -136,10 +143,10 @@ public:
using vpMbTracker::getClipping;
virtual unsigned int getClipping(const std::string &cameraName) const;
- virtual vpMbHiddenFaces<vpMbtPolygon>& getFaces();
- vpMbHiddenFaces<vpMbtPolygon>& getEdgeFaces(const std::string &cameraName);
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces();
+ vpMbHiddenFaces<vpMbtPolygon> &getEdgeFaces(const std::string &cameraName);
std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > getEdgeFaces() const;
- vpMbHiddenFaces<vpMbtPolygon>& getKltFaces(const std::string &cameraName);
+ vpMbHiddenFaces<vpMbtPolygon> &getKltFaces(const std::string &cameraName);
std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > getKltFaces() const;
/*!
@@ -147,117 +154,117 @@ public:
\return The factor for the KLT trackers.
*/
- inline double getFactorKLT() const {
- return m_factorKLT;
- }
+ inline double getFactorKLT() const { return m_factorKLT; }
/*!
Return the factor for the MBT trackers in the VVS process.
\return The factor for the MBT trackers.
*/
- inline double getFactorMBT() const {
- return m_factorMBT;
- }
+ inline double getFactorMBT() const { return m_factorMBT; }
virtual unsigned int getNbPolygon() const;
virtual std::map<std::string, unsigned int> getEdgeMultiNbPolygon() const;
virtual std::map<std::string, unsigned int> getKltMultiNbPolygon() const;
-
/*!
Get the number of cameras.
\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<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const;
- virtual void init(const vpImage<unsigned char>& I);
+ virtual inline vpColVector getError() const { return m_error_hybridMulti; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w_hybridMulti; }
+
+ virtual void init(const vpImage<unsigned char> &I);
#ifdef VISP_HAVE_MODULE_GUI
- virtual void initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile="");
+ virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile = "");
- virtual void initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp=false);
+ virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp = false);
- virtual void initClick(const vpImage<unsigned char>& I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp=false,
- const bool firstCameraIsReference=true);
+ virtual void initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false,
+ const bool firstCameraIsReference = true);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp=false);
+ const std::string &initFile, const bool displayHelp = false);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp=false);
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp = false);
#endif
- 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 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<unsigned char>& I, const std::string &initFile);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const std::string &initFile);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);
- virtual void initFromPose(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference=true);
+ virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &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,
- const bool firstCameraIsReference=true);
+ virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2,
+ const bool firstCameraIsReference = true);
virtual void loadConfigFile(const std::map<std::string, std::string> &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<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
virtual void reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose=false, const bool firstCameraIsReference=true);
+ 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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose=false);
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose = false);
virtual void resetTracker();
virtual void setAngleAppear(const double &a);
virtual void setAngleDisappear(const double &a);
- virtual void setCameraParameters(const vpCameraParameters& camera);
+ virtual void setCameraParameters(const vpCameraParameters &camera);
- virtual void setCameraParameters(const vpCameraParameters& camera1, const vpCameraParameters& camera2,
- const bool firstCameraIsReference=true);
+ 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<std::string, vpCameraParameters> &mapOfCameraParameters);
virtual void setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix);
+ const vpHomogeneousMatrix &cameraTransformationMatrix);
- virtual void setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
+ virtual void
+ setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
virtual void setClipping(const unsigned int &flags);
virtual void setClipping(const std::string &cameraName, const unsigned int &flags);
- virtual void setCovarianceComputation(const bool& flag);
+ virtual void setCovarianceComputation(const bool &flag);
virtual void setDisplayFeatures(const bool displayF);
@@ -269,32 +276,31 @@ public:
\param factor : The new factor.
*/
- inline void setFactorKLT(const double factor) {
- m_factorKLT = factor;
- }
+ inline void setFactorKLT(const double factor) { m_factorKLT = factor; }
/*!
Set the factor for MBT tracker in the VVS process.
\param factor : The new factor.
*/
- inline void setFactorMBT(const double factor) {
- m_factorMBT = factor;
- }
+ inline void setFactorMBT(const double factor) { m_factorMBT = factor; }
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider
+ a polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \param ratio : Ratio of succesful attempts that has to be considered.
+ Value has to be between 0.0 (0%) and 1.0 (100%).
*/
void setGoodNbRayCastingAttemptsRatio(const double &ratio);
/*!
- Set the number of rays that will be sent toward each polygon for visibility test.
- Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+ Set the number of rays that will be sent toward each polygon for
+ visibility test. Each ray will go from the optic center of the camera to a
+ random point inside the considered polygon.
\sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
@@ -303,16 +309,16 @@ public:
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts);
#endif
- virtual void setLod(const bool useLod, const std::string &name="");
+ 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 setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="");
+ virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = "");
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
- const std::string &name);
+ 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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name);
+ const std::string &name);
virtual void setNearClippingDistance(const double &dist);
virtual void setNearClippingDistance(const std::string &cameraName, const double &dist);
@@ -325,14 +331,15 @@ public:
virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
- virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, const vpHomogeneousMatrix &c1Mo,
- const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference=true);
+ virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
virtual void setProjectionErrorComputation(const bool &flag);
@@ -345,41 +352,37 @@ public:
virtual void testTracking();
virtual void track(const vpImage<unsigned char> &I);
- virtual void track(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2);
+ virtual void track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2);
virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
protected:
- //Current computeVVS function hides the base computeVVS but it is intended.
- //The two following lines should have been added to remove compiler warning.
+ // Current computeVVS function hides the base computeVVS but it is intended.
+ // The two following lines should have been added to remove compiler
+ // warning.
using vpMbEdgeMultiTracker::computeVVS;
using vpMbKltMultiTracker::computeVVS;
virtual void computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNumberOfRows, std::map<std::string, unsigned int> &mapOfNbInfos,
- vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl=0);
-
- virtual unsigned int initMbtTracking(std::vector<FeatureType> &indexOfFeatures,
- std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders,
- std::map<std::string, unsigned int> &mapOfNumberOfCircles);
-
- //Same thing as computeVVS
+ const unsigned int lvl = 0);
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+ using vpMbEdgeMultiTracker::computeVVSInteractionMatrixAndResidu;
+ using vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu;
+ virtual void computeVVSInteractionMatrixAndResidu(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist);
+ virtual void computeVVSWeights();
+ using vpMbTracker::computeVVSWeights;
+
+ virtual unsigned int initMbtTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ unsigned int lvl);
+
+ // Same thing as computeVVS
using vpMbKltMultiTracker::postTracking;
- virtual void postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- vpColVector &w_mbt, vpColVector &w_klt, std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNbInfos, const unsigned int lvl);
+ virtual void postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages, const unsigned int lvl);
virtual void reinit(/*const vpImage<unsigned char>& I*/);
- virtual unsigned int trackFirstLoop(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages, vpColVector &factor,
- std::vector<FeatureType> &indexOfFeatures,
- std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders,
- std::map<std::string, unsigned int> &mapOfNumberOfCircles, const unsigned int lvl);
-
virtual void trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
};
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h
index 14bdceb..a991bf2 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeKltTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,42 +49,42 @@
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-#include <visp3/core/vpRobust.h>
-#include <visp3/core/vpSubMatrix.h>
-#include <visp3/core/vpSubColVector.h>
#include <visp3/core/vpExponentialMap.h>
-#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/core/vpPoseVector.h>
+#include <visp3/core/vpSubColVector.h>
+#include <visp3/core/vpSubMatrix.h>
#include <visp3/klt/vpKltOpencv.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
-#include <visp3/core/vpPoseVector.h>
-#include <visp3/mbt/vpMbtEdgeKltXmlParser.h>
#include <visp3/mbt/vpMbKltTracker.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/mbt/vpMbtEdgeKltXmlParser.h>
/*!
\class vpMbEdgeKltTracker
\ingroup group_mbt_trackers
\warning This class is only available if OpenCV is installed, and used.
-
- \brief Hybrid tracker based on moving-edges and keypoints tracked using KLT
+
+ \brief Hybrid tracker based on moving-edges and keypoints tracked using KLT
tracker.
-
+
The \ref tutorial-tracking-mb is a good starting point to use this class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
+ The tracker requires the knowledge of the 3D model that could be provided in
+a vrml or in a cao file. The cao format is described in loadCAOModel(). It may
+also use an xml file used to tune the behavior of the tracker and an init file
+used to compute the pose at the very first image.
+
+ The following code shows the simplest way to use the tracker. The \ref
+tutorial-tracking-mb is also a good starting point to use this class.
- The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
-
\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpException.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeKltTracker.h>
int main()
{
@@ -104,9 +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
@@ -126,41 +131,42 @@ int main()
return 0;
#endif
}
-\endcode
+\endcode
The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
+ must be known (object always at the same initial pose for example) or
+computed using another method:
\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeKltTracker.h>
int main()
{
#if defined VISP_HAVE_OPENCV
vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
//acquire an image
vpImageIo::read(I, "cube.pgm"); // Example of acquisition
#if defined VISP_HAVE_XML2
tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ // 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){
// acquire a new image
tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
+ tracker.getPose(cMo); // get the pose
}
-
+
#if defined VISP_HAVE_XML2
// Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeKltTracker::loadConfigFile()
vpXmlParser::cleanup();
@@ -171,28 +177,28 @@ int main()
}
\endcode
- Finally it can be used not to track an object but just to display a model at a
- given pose:
+ Finally it can be used not to track an object but just to display a model at
+a given pose:
\code
-#include <visp/vpMbEdgeKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeKltTracker.h>
int main()
{
#if defined VISP_HAVE_OPENCV
vpMbEdgeKltTracker tracker; // Create an hybrid model based tracker.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
vpCameraParameters cam;
-
+
// Acquire an image
vpImageIo::read(I, "cube.pgm");
-
+
#if defined VISP_HAVE_X11
vpDisplayX display;
display.init(I,100,100,"Mb Hybrid Tracker");
@@ -202,7 +208,8 @@ int main()
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.
+ // 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
@@ -211,10 +218,10 @@ int main()
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();
+ vpXmlParser::cleanup();
#endif
return 0;
@@ -222,70 +229,63 @@ int main()
}
\endcode
*/
-class VISP_EXPORT vpMbEdgeKltTracker: public vpMbKltTracker, public vpMbEdgeTracker
+class VISP_EXPORT vpMbEdgeKltTracker : public vpMbKltTracker, public vpMbEdgeTracker
{
protected:
- //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
- bool compute_interaction;
- //! The gain of the virtual visual servoing stage.
- double lambda;
//! The threshold used in the robust estimation of KLT.
double thresholdKLT;
//! The threshold used in the robust estimation of MBT.
double thresholdMBT;
//! The maximum iteration of the virtual visual servoing stage.
- unsigned int maxIter;
+ unsigned int m_maxIterKlt;
+ //! Robust weights for Edge
+ vpColVector w_mbt;
+ //! Robust weights for KLT
+ vpColVector w_klt;
+ //! (s - s*)
+ vpColVector m_error_hybrid;
+ //! Robust weights
+ vpColVector m_w_hybrid;
public:
-
vpMbEdgeKltTracker();
virtual ~vpMbEdgeKltTracker();
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel = false);
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- /*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
+ virtual inline vpColVector getError() const { return m_error_hybrid; }
- /*!
- Get the maximum iteration of the virtual visual servoing stage.
+ virtual inline vpColVector getRobustWeights() const { return m_w_hybrid; }
- \return the number of iteration
- */
- virtual inline unsigned int getMaxIter() const {return maxIter;}
-
/*!
Get the near distance for clipping.
-
+
\return Near clipping value.
*/
- virtual inline double getNearClippingDistance() const { return vpMbKltTracker::getNearClippingDistance(); }
-
- void loadConfigFile(const char* configFile);
- virtual void loadConfigFile(const std::string& configFile);
-
- void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
+ virtual inline double getNearClippingDistance() const { return vpMbKltTracker::getNearClippingDistance(); }
+
+ void loadConfigFile(const char *configFile);
+ virtual void loadConfigFile(const std::string &configFile);
+
+ void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose = false);
+ void reInitModel(const vpImage<unsigned char> &I, const char *cad_name, const vpHomogeneousMatrix &cMo,
+ const bool verbose = false);
void resetTracker();
- virtual void setCameraParameters(const vpCameraParameters& cam);
-
+ virtual void setCameraParameters(const vpCameraParameters &cam);
+
/*!
Specify which clipping to use.
-
+
\sa vpMbtPolygonClipping
-
+
\param flags : New clipping flags.
*/
- virtual void setClipping(const unsigned int &flags) {vpMbEdgeTracker::setClipping(flags); }
+ virtual void setClipping(const unsigned int &flags) { vpMbEdgeTracker::setClipping(flags); }
/*!
Set the far distance for clipping.
@@ -295,22 +295,8 @@ public:
virtual void setFarClippingDistance(const double &dist) { vpMbEdgeTracker::setFarClippingDistance(dist); }
/*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain; vpMbEdgeTracker::setLambda(lambda); vpMbKltTracker::setLambda(lambda);}
-
- /*!
- Set the maximum iteration of the virtual visual servoing stage.
-
- \param max : the desired number of iteration
- */
- virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
-
- /*!
Set the near distance for clipping.
-
+
\param dist : Near clipping value.
*/
virtual void setNearClippingDistance(const double &dist) { vpMbEdgeTracker::setNearClippingDistance(dist); }
@@ -318,11 +304,13 @@ public:
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
- virtual void setOgreVisibilityTest(const bool &v){
+ virtual void setOgreVisibilityTest(const bool &v)
+ {
vpMbTracker::setOgreVisibilityTest(v);
#ifdef VISP_HAVE_OGRE
faces.getOgreContext()->setWindowName("MBT Hybrid");
@@ -334,46 +322,50 @@ public:
\param v : True to use it, False otherwise
*/
- virtual void setScanLineVisibilityTest(const bool &v){
+ virtual void setScanLineVisibilityTest(const bool &v)
+ {
vpMbEdgeTracker::setScanLineVisibilityTest(v);
vpMbKltTracker::setScanLineVisibilityTest(v);
}
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
/*!
Set if the projection error criteria has to be computed.
- \param flag : True if the projection error criteria has to be computed, false otherwise
+ \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<unsigned char>& I);
+ virtual void testTracking() {}
+ virtual void track(const vpImage<unsigned char> &I);
protected:
- void computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt,
- vpColVector &w_klt, const unsigned int lvl=0);
-
- virtual void init(const vpImage<unsigned char>& I);
- virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double r, const int idFace=0,
- const std::string &name="");
- virtual void initCylinder(const vpPoint&, const vpPoint &, const double r, const int idFace,
- const std::string &name="");
+ virtual void computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
+ const unsigned int lvl = 0);
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+ using vpMbTracker::computeCovarianceMatrixVVS;
+ using vpMbTracker::computeVVSPoseEstimation;
+
+ virtual void init(const vpImage<unsigned char> &I);
+ virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double r, const int idFace = 0,
+ const std::string &name = "");
+ virtual void initCylinder(const vpPoint &, const vpPoint &, const double r, const int idFace,
+ const std::string &name = "");
virtual void initFaceFromCorners(vpMbtPolygon &polygon);
virtual void initFaceFromLines(vpMbtPolygon &polygon);
- unsigned int initMbtTracking(const unsigned int level=0);
+ unsigned int initMbtTracking(const unsigned int level = 0);
- bool postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
- const unsigned int lvl=0);
- void postTrackingMbt(vpColVector &w, const unsigned int level=0);
+ bool postTracking(const vpImage<unsigned char> &I, vpColVector &w_mbt, vpColVector &w_klt,
+ const unsigned int lvl = 0);
+ void postTrackingMbt(vpColVector &w, const unsigned int level = 0);
- unsigned int trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl = 0);
- void trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &_error,
- vpHomogeneousMatrix& cMo, const unsigned int lvl=0);
+ unsigned int trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor, const unsigned int lvl = 0);
+ void trackSecondLoop(const vpImage<unsigned char> &I, vpMatrix &L, vpColVector &_error, vpHomogeneousMatrix &cMo,
+ const unsigned int lvl = 0);
};
#endif
-#endif //VISP_HAVE_OPENCV
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h
index 8be2336..0433738 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeMultiTracker.h
@@ -2,19 +2,20 @@
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
- * GPL, please contact INRIA about acquiring a ViSP Professional
+ * GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See https://visp.inria.fr/download/ for more information.
+ *
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
@@ -24,7 +25,7 @@
*
* If you have questions regarding the use of this file, please contact
* INRIA at visp at inria.fr
- *
+ *
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
@@ -49,72 +50,83 @@
#include <vector>
#include <visp3/mbt/vpMbEdgeTracker.h>
-#include <visp3/core/vpRobust.h>
/*!
\class vpMbEdgeMultiTracker
\ingroup group_mbt_trackers
- \brief Make the complete stereo (or more) tracking of an object by using its CAD model.
+ \brief Make the complete stereo (or more) tracking of an object by using its
+ CAD model.
This class allows to track an object or a scene given its 3D model.
- The \ref tutorial-tracking-mb-stereo is also a good starting point to use this class.
+ The \ref tutorial-tracking-mb-stereo is also a good starting point to use
+ this class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
+ The tracker requires the knowledge of the 3D model that could be provided in
+ a vrml or in a cao file. The cao format is described in loadCAOModel(). It
+ may also use an xml file used to tune the behavior of the tracker and an
init file used to compute the pose at the very first image.
*/
-class VISP_EXPORT vpMbEdgeMultiTracker: public vpMbEdgeTracker
+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)
+ //! Map of camera transformation matrix between the current camera frame to
+ //! the reference camera frame (cCurrent_M_cRef)
std::map<std::string, vpHomogeneousMatrix> m_mapOfCameraTransformationMatrix;
//! Map of Model-based edge trackers
- std::map<std::string, vpMbEdgeTracker*> m_mapOfEdgeTrackers;
+ std::map<std::string, vpMbEdgeTracker *> m_mapOfEdgeTrackers;
//! Map of pyramidal images for each camera
- std::map<std::string, std::vector<const vpImage<unsigned char>* > > m_mapOfPyramidalImages;
+ std::map<std::string, std::vector<const vpImage<unsigned char> *> > m_mapOfPyramidalImages;
//! Name of the reference camera
std::string m_referenceCameraName;
-
+ //! Interaction matrix
+ vpMatrix m_L_edgeMulti;
+ //! (s - s*)
+ vpColVector m_error_edgeMulti;
+ //! Robust weights
+ vpColVector m_w_edgeMulti;
+ //! Weighted error
+ vpColVector m_weightedError_edgeMulti;
public:
// Default constructor <==> equivalent to vpMbEdgeTracker
vpMbEdgeMultiTracker();
- // Constructor with a specified number of cameras, camera names are generated
- vpMbEdgeMultiTracker(const unsigned int nbCameras);
+ // Constructor with a specified number of cameras, camera names are
+ // generated
+ explicit vpMbEdgeMultiTracker(const unsigned int nbCameras);
// Constructor with a specified list of camera names
- vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames);
+ explicit vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames);
virtual ~vpMbEdgeMultiTracker();
/** @name Inherited functionalities from vpMbEdgeMultiTracker */
//@{
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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<unsigned char> &I1, const vpImage<unsigned char> &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<vpRGBa>& I1, const vpImage<vpRGBa>& 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<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual void display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual std::vector<std::string> getCameraNames() const;
@@ -126,30 +138,29 @@ public:
using vpMbTracker::getClipping;
virtual unsigned int getClipping(const std::string &cameraName) const;
- virtual vpMbHiddenFaces<vpMbtPolygon>& getFaces();
- virtual vpMbHiddenFaces<vpMbtPolygon>& getFaces(const std::string &cameraName);
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces();
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces(const std::string &cameraName);
virtual std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > getFaces() const;
- void getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level=0) const;
- virtual void getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *>& circlesList,
- const unsigned int level=0) const;
+ void getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level = 0) const;
+ virtual void getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
+ const unsigned int level = 0) const;
- void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level=0) const;
- virtual void getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *>& cylindersList,
- const unsigned int level=0) const;
+ void getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level = 0) const;
+ virtual void getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
+ const unsigned int level = 0) const;
- void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level=0) const;
- virtual void getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *>& linesList,
- const unsigned int level=0) const;
+ void getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level = 0) const;
+ virtual void getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
+ const unsigned int level = 0) const;
virtual void getMovingEdge(vpMe &p_me) const;
virtual vpMe getMovingEdge() const;
virtual void getMovingEdge(const std::string &cameraName, vpMe &p_me) const;
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 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;
@@ -160,86 +171,92 @@ public:
\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<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const;
- void init(const vpImage<unsigned char>& I);
+ virtual inline vpColVector getError() const { return m_error_edgeMulti; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w_edgeMulti; }
+
+ void init(const vpImage<unsigned char> &I);
#ifdef VISP_HAVE_MODULE_GUI
- virtual void initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile="");
+ virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile = "");
- virtual void initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp=false);
+ virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp = false);
- virtual void initClick(const vpImage<unsigned char>& I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp=false,
- const bool firstCameraIsReference=true);
+ virtual void initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false,
+ const bool firstCameraIsReference = true);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp=false);
+ const std::string &initFile, const bool displayHelp = false);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp=false);
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp = false);
#endif
- virtual void initFromPose(const vpImage<unsigned char>& I, const std::string &initFile);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const std::string &initFile);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);
- virtual void initFromPose(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference=true);
+ virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
virtual void loadConfigFile(const std::string &configFile);
virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2,
- const bool firstCameraIsReference=true);
+ const bool firstCameraIsReference = true);
virtual void loadConfigFile(const std::map<std::string, std::string> &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<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
virtual void reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose=false, const bool firstCameraIsReference=true);
+ 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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose=false);
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose = false);
virtual void resetTracker();
virtual void setAngleAppear(const double &a);
virtual void setAngleDisappear(const double &a);
- virtual void setCameraParameters(const vpCameraParameters& camera);
+ virtual void setCameraParameters(const vpCameraParameters &camera);
- virtual void setCameraParameters(const vpCameraParameters& camera1, const vpCameraParameters& camera2,
- const bool firstCameraIsReference=true);
+ 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<std::string, vpCameraParameters> &mapOfCameraParameters);
virtual void setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix);
+ const vpHomogeneousMatrix &cameraTransformationMatrix);
- virtual void setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
+ virtual void
+ setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
virtual void setClipping(const unsigned int &flags);
virtual void setClipping(const std::string &cameraName, const unsigned int &flags);
- virtual void setCovarianceComputation(const bool& flag);
+ virtual void setCovarianceComputation(const bool &flag);
virtual void setDisplayFeatures(const bool displayF);
@@ -250,17 +267,20 @@ public:
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider
+ a polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \param ratio : Ratio of succesful attempts that has to be considered.
+ Value has to be between 0.0 (0%) and 1.0 (100%).
*/
void setGoodNbRayCastingAttemptsRatio(const double &ratio);
/*!
- Set the number of rays that will be sent toward each polygon for visibility test.
- Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+ Set the number of rays that will be sent toward each polygon for
+ visibility test. Each ray will go from the optic center of the camera to a
+ random point inside the considered polygon.
\sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
@@ -269,16 +289,16 @@ public:
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts);
#endif
- virtual void setLod(const bool useLod, const std::string &name="");
+ 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 setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="");
+ virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = "");
virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
- const std::string &name);
+ 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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name);
+ const std::string &name);
virtual void setMovingEdge(const vpMe &me);
virtual void setMovingEdge(const std::string &cameraName, const vpMe &me);
@@ -289,12 +309,12 @@ public:
/*!
Enable/Disable the appearance of Ogre config dialog on startup.
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
*/
virtual void setOgreShowConfigDialog(const bool showConfigDialog);
@@ -304,20 +324,21 @@ public:
virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
- virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, const vpHomogeneousMatrix &c1Mo,
- const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference=true);
+ virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
virtual void setProjectionErrorComputation(const bool &flag);
virtual void setReferenceCameraName(const std::string &referenceCameraName);
- virtual void setScales(const std::vector<bool>& scales);
+ virtual void setScales(const std::vector<bool> &scales);
virtual void setScanLineVisibilityTest(const bool &v);
@@ -329,33 +350,27 @@ public:
//@}
protected:
- typedef enum FeatureType {
- LINE, CYLINDER, CIRCLE
- } FeatureType;
+ enum FeatureType { LINE, CYLINDER, CIRCLE };
/** @name Protected Member Functions Inherited from vpMbEdgeMultiTracker */
//@{
- virtual void cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char>* > >& pyramid);
+ virtual void cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid);
virtual void computeProjectionError();
- virtual void computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const unsigned int lvl);
-
- virtual void computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
- vpColVector &weighted_error, vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders, std::map<std::string, unsigned int> &mapOfNumberOfCircles,
- std::map<std::string, vpColVector> &mapOfWeightLines, std::map<std::string, vpColVector> &mapOfWeightCylinders,
- std::map<std::string, vpColVector> &mapOfWeightCircles, std::map<std::string, vpColVector> &mapOfErrorLines,
- std::map<std::string, vpColVector> &mapOfErrorCylinders, std::map<std::string, vpColVector> &mapOfErrorCircles,
- std::map<std::string, vpRobust> &mapOfRobustLines, std::map<std::string, vpRobust> &mapOfRobustCylinders,
- std::map<std::string, vpRobust> &mapOfRobustCircles, double threshold);
-
- virtual void initPyramid(const std::map<std::string, const vpImage<unsigned char> * >& mapOfImages,
- std::map<std::string, std::vector<const vpImage<unsigned char>* > >& pyramid);
+ virtual void computeVVS(std::map<std::string, const vpImage<unsigned char> *> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist);
+ virtual void computeVVSWeights();
+ using vpMbTracker::computeVVSWeights;
+
+ virtual void initPyramid(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid);
//@}
};
#endif
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h
index b85bade..de1a13a 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbEdgeTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,44 +47,43 @@
#define vpMbEdgeTracker_HH
#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpXmlParser.h>
#include <visp3/mbt/vpMbTracker.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/mbt/vpMbtMeLine.h>
-#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceCylinder.h>
-#include <visp3/core/vpXmlParser.h>
-#include <visp3/core/vpRobust.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
+#include <visp3/mbt/vpMbtMeLine.h>
+#include <visp3/me/vpMe.h>
-#include <iostream>
#include <fstream>
-#include <vector>
+#include <iostream>
#include <list>
+#include <vector>
#if defined(VISP_HAVE_COIN3D)
-//Inventor includes
-#include <Inventor/nodes/SoSeparator.h>
+// Inventor includes
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/nodes/SoSeparator.h>
#endif
#ifdef VISP_HAVE_OPENCV
-# if VISP_HAVE_OPENCV_VERSION >= 0x020101
-# include <opencv2/core/core.hpp>
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/imgproc/imgproc_c.h>
-# else
-# include <cv.h>
-# endif
+#if VISP_HAVE_OPENCV_VERSION >= 0x020101
+#include <opencv2/core/core.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/imgproc/imgproc_c.h>
+#else
+#include <cv.h>
+#endif
#endif
/*!
@@ -92,34 +92,36 @@
\brief Make the complete tracking of an object by using its CAD model.
This class allows to track an object or a scene given its 3D model. A
- video can be found on YouTube \e https://www.youtube.com/watch?v=UK10KMMJFCI . The \ref tutorial-tracking-mb is also a good starting point to use this class.
+ video can be found on YouTube \e https://www.youtube.com/watch?v=UK10KMMJFCI
+. The \ref tutorial-tracking-mb is also a good starting point to use this
+class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
+ The tracker requires the knowledge of the 3D model that could be provided in
+a vrml or in a cao file. The cao format is described in loadCAOModel(). It may
+also use an xml file used to tune the behavior of the tracker and an init file
+used to compute the pose at the very first image.
The following code shows the simplest way to use the tracker.
\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpException.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeTracker.h>
int main()
{
vpMbEdgeTracker tracker; // Create a model based tracker.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
vpCameraParameters cam;
-
+
// Acquire an image
vpImageIo::read(I, "cube.pgm");
-
+
#if defined VISP_HAVE_X11
vpDisplayX display;
display.init(I,100,100,"Mb Edge Tracker");
@@ -130,16 +132,18 @@ int main()
#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.
+ // 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
vpDisplay::display(I);
tracker.track(I); // Track the object on this image
tracker.getPose(cMo); // Get the pose
-
+
tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
- vpDisplay::flush(I);
+ vpDisplay::flush(I);
}
// Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
@@ -149,8 +153,9 @@ int main()
}
\endcode
- For application with large inter-images displacement, multi-scale tracking is also possible, by setting the number of scales used and by activating (or not) them
- using a vector of booleans, as presented in the following code:
+ For application with large inter-images displacement, multi-scale tracking
+is also possible, by setting the number of scales used and by activating (or
+not) them using a vector of booleans, as presented in the following code:
\code
...
@@ -171,37 +176,38 @@ int main()
\endcode
The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
+ must be known (object always at the same initial pose for example) or
+computed using another method:
\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeTracker.h>
int main()
{
vpMbEdgeTracker tracker; // Create a model based tracker.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
//acquire an image
vpImageIo::read(I, "cube.pgm"); // Example of acquisition
#if defined VISP_HAVE_XML2
tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ // 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){
// acquire a new image
tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
+ tracker.getPose(cMo); // get the pose
}
-
+
// Cleanup memory allocated by xml library used to parse the xml config file in vpMbEdgeTracker::loadConfigFile()
vpXmlParser::cleanup();
@@ -209,27 +215,27 @@ int main()
}
\endcode
- Finally it can be used not to track an object but just to display a model at a
- given pose:
+ Finally it can be used not to track an object but just to display a model at
+a given pose:
\code
-#include <visp/vpMbEdgeTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeTracker.h>
int main()
{
vpMbEdgeTracker tracker; // Create a model based tracker.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
vpCameraParameters cam;
-
+
// Acquire an image
vpImageIo::read(I, "cube.pgm");
-
+
#if defined VISP_HAVE_X11
vpDisplayX display;
display.init(I,100,100,"Mb Edge Tracker");
@@ -239,7 +245,9 @@ int main()
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.
+ // 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
@@ -248,7 +256,7 @@ int main()
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();
@@ -258,141 +266,163 @@ int main()
*/
-class VISP_EXPORT vpMbEdgeTracker: virtual public vpMbTracker
+class VISP_EXPORT vpMbEdgeTracker : public virtual vpMbTracker
{
friend class vpMbEdgeMultiTracker;
friend class vpMbEdgeKltMultiTracker;
- protected :
-
- /*! If this flag is true, the interaction matrix
- extracted from the feature set is computed at each
- iteration in the visual servoing loop.
- */
- int compute_interaction;
- //! The gain of the virtual visual servoing stage.
- double lambda;
-
- //! The moving edges parameters.
- vpMe me;
- //! Vector of list of all the lines tracked (each line is linked to a list of moving edges). Each element of the vector is for a scale (element 0 = level 0 = no subsampling).
- std::vector< std::list< vpMbtDistanceLine*> > lines;
-
- //! Vector of the tracked circles.
- std::vector< std::list< vpMbtDistanceCircle*> > circles;
-
- //! Vector of the tracked cylinders.
- std::vector< std::list< vpMbtDistanceCylinder*> > cylinders;
-
- //! Index of the polygon to add, and total number of polygon extracted so far.
- unsigned int nline;
-
- //! Index of the circle to add, and total number of circles extracted so far.
- unsigned int ncircle;
-
- //! Index of the cylinder to add, and total number of cylinders extracted so far.
- unsigned int ncylinder;
-
- //! Number of polygon (face) currently visible.
- unsigned int nbvisiblepolygone;
-
- //! Percentage of good points over total number of points below which tracking is supposed to have failed.
- double percentageGdPt;
-
- //! Vector of scale level to use for the multi-scale tracking.
- std::vector<bool> scales;
-
- //! Pyramid of image associated to the current image. This pyramid is computed in the init() and in the track() methods.
- std::vector< const vpImage<unsigned char>* > Ipyramid;
-
- //! Current scale level used. This attribute must not be modified outside of the downScale() and upScale() methods, as it used to specify to some methods which set of distanceLine use.
- unsigned int scaleLevel;
-
- //! Number of features used in the computation of the projection error
- unsigned int nbFeaturesForProjErrorComputation;
+protected:
+ //! The moving edges parameters.
+ vpMe me;
+ //! Vector of list of all the lines tracked (each line is linked to a list
+ //! of moving edges). Each element of the vector is for a scale (element 0 =
+ //! level 0 = no subsampling).
+ std::vector<std::list<vpMbtDistanceLine *> > lines;
+
+ //! Vector of the tracked circles.
+ std::vector<std::list<vpMbtDistanceCircle *> > circles;
+
+ //! Vector of the tracked cylinders.
+ std::vector<std::list<vpMbtDistanceCylinder *> > cylinders;
+
+ //! Index of the polygon to add, and total number of polygon extracted so
+ //! far.
+ unsigned int nline;
+
+ //! Index of the circle to add, and total number of circles extracted so
+ //! far.
+ unsigned int ncircle;
+
+ //! Index of the cylinder to add, and total number of cylinders extracted so
+ //! far.
+ unsigned int ncylinder;
+
+ //! Number of polygon (face) currently visible.
+ unsigned int nbvisiblepolygone;
+
+ //! Percentage of good points over total number of points below which
+ //! tracking is supposed to have failed.
+ double percentageGdPt;
+
+ //! Vector of scale level to use for the multi-scale tracking.
+ std::vector<bool> scales;
+
+ //! Pyramid of image associated to the current image. This pyramid is
+ //! computed in the init() and in the track() methods.
+ std::vector<const vpImage<unsigned char> *> Ipyramid;
+
+ //! Current scale level used. This attribute must not be modified outside of
+ //! the downScale() and upScale() methods, as it used to specify to some
+ //! methods which set of distanceLine use.
+ unsigned int scaleLevel;
+
+ //! Number of features used in the computation of the projection error
+ unsigned int nbFeaturesForProjErrorComputation;
+
+ /// Edge VVS variables
+ vpColVector m_factor;
+ vpRobust m_robustLines;
+ vpRobust m_robustCylinders;
+ vpRobust m_robustCircles;
+ vpColVector m_wLines;
+ vpColVector m_wCylinders;
+ vpColVector m_wCircles;
+ vpColVector m_errorLines;
+ vpColVector m_errorCylinders;
+ vpColVector m_errorCircles;
+ //! Interaction matrix
+ vpMatrix m_L_edge;
+ //! (s - s*)
+ vpColVector m_error_edge;
+ //! Robust weights
+ vpColVector m_w_edge;
+ //! Weighted error
+ vpColVector m_weightedError_edge;
+ //! Robust
+ vpRobust m_robust_edge;
public:
-
- vpMbEdgeTracker();
+ vpMbEdgeTracker();
virtual ~vpMbEdgeTracker();
-
+
/** @name Inherited functionalities from vpMbEdgeTracker */
//@{
- void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- /*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
-
- void getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level = 0) const;
- void getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level = 0) const;
- void getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level = 0) const;
+ void getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level = 0) const;
+ void getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level = 0) const;
+ void getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level = 0) const;
/*!
Get the moving edge parameters.
- \param p_me [out] : an instance of the moving edge parameters used by the tracker.
+ \param p_me [out] : an instance of the moving edge parameters used by the
+ tracker.
*/
- virtual inline void getMovingEdge(vpMe &p_me ) const { p_me = this->me;}
+ virtual inline void getMovingEdge(vpMe &p_me) const { p_me = this->me; }
/*!
Get the moving edge parameters.
\return an instance of the moving edge parameters used by the tracker.
*/
- virtual inline vpMe getMovingEdge() const { return this->me;}
+ virtual inline vpMe getMovingEdge() const { return this->me; }
+
+ virtual unsigned int getNbPoints(const unsigned int level = 0) const;
- virtual unsigned int getNbPoints(const unsigned int level=0) const;
-
/*!
- Return the scales levels used for the tracking.
-
- \return The scales levels used for the tracking.
+ Return the scales levels used for the tracking.
+
+ \return The scales levels used for the tracking.
*/
- std::vector<bool> getScales() const {return scales;}
+ std::vector<bool> getScales() const { return scales; }
/*!
- \return The threshold value between 0 and 1 over good moving edges ratio. It allows to
- decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
- moving edges should be considered as good to have a valid pose, while 0.1 means that
- 10% of the moving edge are enough to declare a pose valid.
+ \return The threshold value between 0 and 1 over good moving edges ratio.
+ It allows to decide if the tracker has enough valid moving edges to
+ compute a pose. 1 means that all moving edges should be considered as
+ good to have a valid pose, while 0.1 means that 10% of the moving edge
+ are enough to declare a pose valid.
\sa setGoodMovingEdgesRatioThreshold()
*/
- inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt;}
+ inline double getGoodMovingEdgesRatioThreshold() const { return percentageGdPt; }
+
+ virtual inline vpColVector getError() const { return m_error_edge; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w_edge; }
void loadConfigFile(const std::string &configFile);
- void loadConfigFile(const char* configFile);
- virtual void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
+ void loadConfigFile(const char *configFile);
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
+ void reInitModel(const vpImage<unsigned char> &I, const char *cad_name, const vpHomogeneousMatrix &cMo,
+ const bool verbose = false);
void resetTracker();
-
+
/*!
Set the camera parameters.
\param camera : the new camera parameters
*/
- virtual void setCameraParameters(const vpCameraParameters& camera) {
+ virtual void setCameraParameters(const vpCameraParameters &camera)
+ {
this->cam = camera;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
(*it)->setCameraParameters(cam);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
(*it)->setCameraParameters(cam);
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
(*it)->setCameraParameters(cam);
}
}
@@ -408,14 +438,16 @@ public:
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
- virtual void setOgreVisibilityTest(const bool &v){
- vpMbTracker::setOgreVisibilityTest(v);
+ virtual void setOgreVisibilityTest(const bool &v)
+ {
+ vpMbTracker::setOgreVisibilityTest(v);
#ifdef VISP_HAVE_OGRE
- faces.getOgreContext()->setWindowName("MBT Edge");
+ faces.getOgreContext()->setWindowName("MBT Edge");
#endif
}
@@ -424,12 +456,13 @@ public:
\param v : True to use it, False otherwise
*/
- virtual void setScanLineVisibilityTest(const bool &v){
+ virtual void setScanLineVisibilityTest(const bool &v)
+ {
vpMbTracker::setScanLineVisibilityTest(v);
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
(*it)->useScanLine = v;
}
}
@@ -437,31 +470,25 @@ public:
}
/*!
- Set the threshold value between 0 and 1 over good moving edges ratio. It allows to
- decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
- moving edges should be considered as good to have a valid pose, while 0.1 means that
- 10% of the moving edge are enough to declare a pose valid.
+ Set the threshold value between 0 and 1 over good moving edges ratio. It
+ allows to decide if the tracker has enough valid moving edges to compute
+ a pose. 1 means that all moving edges should be considered as good to
+ have a valid pose, while 0.1 means that 10% of the moving edge are enough
+ to declare a pose valid.
- \param threshold : Value between 0 and 1 that corresponds to the ratio of good
- moving edges that is necessary to consider that the estimated pose is valid.
- Default value is 0.4.
+ \param threshold : Value between 0 and 1 that corresponds to the ratio of
+ good moving edges that is necessary to consider that the estimated pose
+ is valid. Default value is 0.4.
\sa getGoodMovingEdgesRatioThreshold()
*/
- void setGoodMovingEdgesRatioThreshold(const double threshold) {percentageGdPt = threshold;}
+ void setGoodMovingEdgesRatioThreshold(const double threshold) { percentageGdPt = threshold; }
- /*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain;}
-
void setMovingEdge(const vpMe &me);
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
-
- void setScales(const std::vector<bool>& _scales);
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
+
+ void setScales(const std::vector<bool> &_scales);
void setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking);
@@ -472,62 +499,52 @@ protected:
/** @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, const std::string& name = "");
- void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string& name = "");
+ void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace = -1,
+ const std::string &name = "");
+ void addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace = -1, const std::string &name = "");
void addLine(vpPoint &p1, vpPoint &p2, int polygon = -1, std::string name = "");
- void addPolygon(vpMbtPolygon &p) ;
-
- void cleanPyramid(std::vector<const vpImage<unsigned char>* >& _pyramid);
- void computeProjectionError(const vpImage<unsigned char>& _I);
-
- void computeVVS(const vpImage<unsigned char>& _I, const unsigned int lvl);
- void computeVVSFirstPhase(const vpImage<unsigned char>& I, const unsigned int iter,
- vpMatrix &L, vpColVector &factor, double &count, vpColVector &error, vpColVector &w_mbt, const unsigned int lvl = 0);
- void computeVVSFirstPhaseFactor(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl = 0);
- void computeVVSFirstPhasePoseEstimation(const unsigned int nerror, const unsigned int iter, const vpColVector &factor,
- vpColVector &weighted_error, vpMatrix &L, bool &isoJoIdentity_);
- void computeVVSSecondPhase(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &error_lines,
- vpColVector &error_cylinders, vpColVector &error_circles, vpColVector &error, const unsigned int lvl);
- void computeVVSSecondPhaseCheckLevenbergMarquardt(const unsigned int iter, const unsigned int nbrow,
- const vpColVector &m_error_prev, const vpColVector &m_w_prev, const vpHomogeneousMatrix &cMoPrev,
- double &mu, bool &reStartFromLastIncrement);
- void computeVVSSecondPhasePoseEstimation(const unsigned int nerror, vpMatrix &L, vpMatrix &L_true, vpMatrix &LVJ_true,
- vpColVector &W_true, const vpColVector &factor, const unsigned int iter, const bool isoJoIdentity_,
- vpColVector &weighted_error, double &mu, vpColVector &m_error_prev, vpColVector &m_w_prev,
- vpHomogeneousMatrix &cMoPrev, double &residu_1, double &r);
- void computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
- const unsigned int nbrow, vpColVector &weighted_error,
- vpRobust &robust_lines, vpRobust &robust_cylinders, vpRobust &robust_circles,
- vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
- vpColVector &error_lines, vpColVector &error_cylinders, vpColVector &error_circles,
- const unsigned int nberrors_lines, const unsigned int nberrors_cylinders, const unsigned int nberrors_circles);
-
- void displayFeaturesOnImage(const vpImage<unsigned char>& I, const unsigned int lvl);
+ void addPolygon(vpMbtPolygon &p);
+
+ void cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid);
+ void computeProjectionError(const vpImage<unsigned char> &_I);
+
+ void computeVVS(const vpImage<unsigned char> &_I, const unsigned int lvl);
+ void computeVVSFirstPhase(const vpImage<unsigned char> &I, const unsigned int iter, double &count,
+ const unsigned int lvl = 0);
+ void computeVVSFirstPhaseFactor(const vpImage<unsigned char> &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<unsigned char> &I);
+ virtual void computeVVSWeights();
+ using vpMbTracker::computeVVSWeights;
+
+ void displayFeaturesOnImage(const vpImage<unsigned char> &I, const unsigned int lvl);
void downScale(const unsigned int _scale);
- void init(const vpImage<unsigned char>& I);
- virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace=0, const std::string &name="");
- virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
- const std::string &name="");
+ void init(const vpImage<unsigned char> &I);
+ virtual void initCircle(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace = 0, const std::string &name = "");
+ virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
+ const std::string &name = "");
virtual void initFaceFromCorners(vpMbtPolygon &polygon);
virtual void initFaceFromLines(vpMbtPolygon &polygon);
- unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders, unsigned int &nberrors_circles);
- void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo) ;
- void initPyramid(const vpImage<unsigned char>& _I, std::vector<const vpImage<unsigned char>* >& _pyramid);
+ unsigned int initMbtTracking(unsigned int &nberrors_lines, unsigned int &nberrors_cylinders,
+ unsigned int &nberrors_circles);
+ void initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
+ void initPyramid(const vpImage<unsigned char> &_I, std::vector<const vpImage<unsigned char> *> &_pyramid);
void reInitLevel(const unsigned int _lvl);
void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo);
- void removeCircle(const std::string& name);
- void removeCylinder(const std::string& name);
- void removeLine(const std::string& name);
+ void removeCircle(const std::string &name);
+ void removeCylinder(const std::string &name);
+ void removeLine(const std::string &name);
void resetMovingEdge();
void testTracking();
- void trackMovingEdge(const vpImage<unsigned char> &I) ;
- void updateMovingEdge(const vpImage<unsigned char> &I) ;
+ void trackMovingEdge(const vpImage<unsigned char> &I);
+ void updateMovingEdge(const vpImage<unsigned char> &I);
void updateMovingEdgeWeights();
- void upScale(const unsigned int _scale);
- void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline) ;
+ void upScale(const unsigned int _scale);
+ void visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo, bool &newvisibleline);
//@}
};
#endif
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h
new file mode 100644
index 0000000..d7acb28
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbGenericTracker.h
@@ -0,0 +1,494 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Generic model-based tracker.
+ *
+ *****************************************************************************/
+/*!
+ \file vpMbGenericTracker.h
+ \brief Generic model-based tracker
+*/
+
+#ifndef __vpMbGenericTracker_h_
+#define __vpMbGenericTracker_h_
+
+#include <visp3/mbt/vpMbDepthDenseTracker.h>
+#include <visp3/mbt/vpMbDepthNormalTracker.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/mbt/vpMbKltTracker.h>
+
+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))
+ 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. */
+ };
+
+ vpMbGenericTracker();
+ vpMbGenericTracker(const unsigned int nbCameras, const int trackerType = EDGE_TRACKER);
+ explicit vpMbGenericTracker(const std::vector<int> &trackerTypes);
+ vpMbGenericTracker(const std::vector<std::string> &cameraNames, const std::vector<int> &trackerTypes);
+
+ virtual ~vpMbGenericTracker();
+
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual void display(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &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<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
+ virtual void display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual std::vector<std::string> getCameraNames() const;
+
+ using vpMbTracker::getCameraParameters;
+ virtual void getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const;
+ virtual void getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const;
+
+ virtual std::map<std::string, int> getCameraTrackerTypes() const;
+
+ using vpMbTracker::getClipping;
+ virtual void getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const;
+ virtual void getClipping(std::map<std::string, unsigned int> &mapOfClippingFlags) const;
+
+ virtual inline vpColVector getError() const { return m_error; }
+
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces();
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces(const std::string &cameraName);
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ virtual std::list<vpMbtDistanceCircle *> &getFeaturesCircle();
+ virtual std::list<vpMbtDistanceKltCylinder *> &getFeaturesKltCylinder();
+ virtual std::list<vpMbtDistanceKltPoints *> &getFeaturesKlt();
+#endif
+
+ virtual double getGoodMovingEdgesRatioThreshold() const;
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ virtual std::vector<vpImagePoint> getKltImagePoints() const;
+ virtual std::map<int, vpImagePoint> getKltImagePointsWithId() const;
+
+ virtual unsigned int getKltMaskBorder() const;
+ virtual int getKltNbPoints() const;
+
+ virtual vpKltOpencv getKltOpencv() const;
+ virtual void getKltOpencv(vpKltOpencv &klt1, vpKltOpencv &klt2) const;
+ virtual void getKltOpencv(std::map<std::string, vpKltOpencv> &mapOfKlts) const;
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ virtual std::vector<cv::Point2f> getKltPoints() const;
+#endif
+
+ virtual double getKltThresholdAcceptation() const;
+#endif
+
+ virtual void getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
+ const unsigned int level = 0) const;
+ virtual void getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
+ const unsigned int level = 0) const;
+ virtual void getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
+ const unsigned int level = 0) const;
+
+ virtual vpMe getMovingEdge() const;
+ virtual void getMovingEdge(vpMe &me1, vpMe &me2) const;
+ virtual void getMovingEdge(std::map<std::string, vpMe> &mapOfMovingEdges) const;
+
+ virtual unsigned int getNbPoints(const unsigned int level = 0) const;
+ virtual void getNbPoints(std::map<std::string, unsigned int> &mapOfNbPoints, const unsigned int level = 0) const;
+
+ virtual inline unsigned int getNbPolygon() const;
+ virtual void getNbPolygon(std::map<std::string, unsigned int> &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<vpPolygon>, std::vector<std::vector<vpPoint> > >
+ getPolygonFaces(const bool orderPolygons = true, const bool useVisibility = true, const bool clipPolygon = false);
+ virtual void getPolygonFaces(std::map<std::string, std::vector<vpPolygon> > &mapOfPolygons,
+ std::map<std::string, std::vector<std::vector<vpPoint> > > &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<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const;
+
+ virtual inline vpColVector getRobustWeights() const { return m_w; }
+
+ virtual void init(const vpImage<unsigned char> &I);
+
+#ifdef VISP_HAVE_MODULE_GUI
+ using vpMbTracker::initClick;
+ virtual void initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false);
+ virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp = false);
+#endif
+
+ using vpMbTracker::initFromPoints;
+ virtual void initFromPoints(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2);
+ virtual void initFromPoints(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitPoints);
+
+ using vpMbTracker::initFromPose;
+ virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2);
+ virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitPoses);
+
+ virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo);
+ virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &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<std::string, std::string> &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<std::string, std::string> &mapOfModelFiles, const bool verbose = false);
+
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
+ virtual void reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfModelFiles,
+ const std::map<std::string, vpHomogeneousMatrix> &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<std::string, double> &mapOfAngles);
+
+ virtual void setAngleDisappear(const double &a);
+ virtual void setAngleDisappear(const double &a1, const double &a2);
+ virtual void setAngleDisappear(const std::map<std::string, double> &mapOfAngles);
+
+ virtual void setCameraParameters(const vpCameraParameters &camera);
+ virtual void setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2);
+ virtual void setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters);
+
+ virtual void setCameraTransformationMatrix(const std::string &cameraName,
+ const vpHomogeneousMatrix &cameraTransformationMatrix);
+ virtual void
+ setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &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<std::string, unsigned int> &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 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 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<std::string, double> &mapOfClippingDists);
+
+ virtual void setFeatureFactors(const std::map<vpTrackerType, double> &mapOfFeatureFactors);
+
+ virtual void setGoodMovingEdgesRatioThreshold(const double threshold);
+
+#ifdef VISP_HAVE_OGRE
+ virtual void setGoodNbRayCastingAttemptsRatio(const double &ratio);
+ virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts);
+#endif
+
+#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<std::string, unsigned int> &mapOfErosions);
+
+ virtual void setKltOpencv(const vpKltOpencv &t);
+ virtual void setKltOpencv(const vpKltOpencv &t1, const vpKltOpencv &t2);
+ virtual void setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfKlts);
+
+ virtual void setKltThresholdAcceptation(const double th);
+
+#endif
+
+ 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 setMovingEdge(const vpMe &me);
+ virtual void setMovingEdge(const vpMe &me1, const vpMe &me2);
+ virtual void setMovingEdge(const std::map<std::string, vpMe> &mapOfMe);
+
+ virtual void setNearClippingDistance(const double &dist);
+ virtual void setNearClippingDistance(const double &dist1, const double &dist2);
+ virtual void setNearClippingDistance(const std::map<std::string, double> &mapOfDists);
+
+ virtual void setOgreShowConfigDialog(const bool showConfigDialog);
+ virtual void setOgreVisibilityTest(const bool &v);
+
+ virtual void setOptimizationMethod(const vpMbtOptimizationMethod &opt);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
+ virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo);
+ virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+
+ virtual void setProjectionErrorComputation(const bool &flag);
+
+ virtual void setReferenceCameraName(const std::string &referenceCameraName);
+
+ virtual void setScanLineVisibilityTest(const bool &v);
+
+ virtual void setTrackerType(const int type);
+ virtual void setTrackerType(const std::map<std::string, int> &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<unsigned char> &I);
+ virtual void track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2);
+ virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
+#ifdef VISP_HAVE_PCL
+ virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds);
+#endif
+ virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
+ std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+ std::map<std::string, unsigned int> &mapOfPointCloudHeights);
+
+protected:
+ virtual void computeProjectionError();
+
+ virtual void computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
+
+ virtual void computeVVSInit();
+ virtual void computeVVSInit(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
+ virtual void computeVVSInteractionMatrixAndResidu();
+ virtual void computeVVSInteractionMatrixAndResidu(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist);
+ 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 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);
+
+#ifdef VISP_HAVE_PCL
+ virtual void preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds);
+#endif
+ virtual void preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
+ std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+ std::map<std::string, unsigned int> &mapOfPointCloudHeights);
+
+private:
+ class TrackerWrapper : public vpMbEdgeTracker,
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ public vpMbKltTracker,
+#endif
+ public vpMbDepthNormalTracker,
+ public vpMbDepthDenseTracker
+ {
+ friend class vpMbGenericTracker;
+
+ public:
+ //! (s - s*)
+ vpColVector m_error;
+ //! Interaction matrix
+ vpMatrix m_L;
+ //! Type of the tracker (a combination of the above)
+ int m_trackerType;
+ //! Robust weights
+ vpColVector m_w;
+ //! Weighted error
+ vpColVector m_weightedError;
+
+ TrackerWrapper();
+ explicit TrackerWrapper(const int trackerType);
+
+ virtual ~TrackerWrapper();
+
+ virtual inline vpColVector getError() const { return m_error; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w; }
+
+ virtual inline int getTrackerType() const { return m_trackerType; }
+
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ virtual void init(const vpImage<unsigned char> &I);
+
+ virtual void loadConfigFile(const std::string &configFile);
+
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
+
+ virtual void resetTracker();
+
+ virtual void setCameraParameters(const vpCameraParameters &camera);
+
+ virtual void setClipping(const unsigned int &flags);
+
+ virtual void setFarClippingDistance(const double &dist);
+
+ virtual void setNearClippingDistance(const double &dist);
+
+ virtual void setOgreVisibilityTest(const bool &v);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
+
+ virtual void setProjectionErrorComputation(const bool &flag);
+
+ virtual void setScanLineVisibilityTest(const bool &v);
+
+ virtual void setTrackerType(const int type);
+
+ virtual void testTracking();
+
+ virtual void track(const vpImage<unsigned char> &I);
+#ifdef VISP_HAVE_PCL
+ using vpMbDepthNormalTracker::track;
+ using vpMbDepthDenseTracker::track;
+ virtual void track(const vpImage<unsigned char> *const ptr_I = NULL,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud = nullptr);
+#endif
+
+ protected:
+ virtual void computeVVS(const vpImage<unsigned char> *const ptr_I);
+ virtual void computeVVSInit();
+ virtual void computeVVSInit(const vpImage<unsigned char> *const ptr_I);
+ virtual void computeVVSInteractionMatrixAndResidu();
+ using vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu;
+ virtual void computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> *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 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);
+
+ virtual void initMbtTracking(const vpImage<unsigned char> *const ptr_I);
+
+#ifdef VISP_HAVE_PCL
+ virtual void postTracking(const vpImage<unsigned char> *const ptr_I = NULL,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud = nullptr);
+ virtual void preTracking(const vpImage<unsigned char> *const ptr_I = NULL,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud = nullptr);
+#endif
+ virtual void postTracking(const vpImage<unsigned char> *const ptr_I = NULL, const unsigned int pointcloud_width = 0,
+ const unsigned int pointcloud_height = 0);
+ virtual void preTracking(const vpImage<unsigned char> *const ptr_I = NULL,
+ const std::vector<vpColVector> *const point_cloud = NULL,
+ const unsigned int pointcloud_width = 0, const unsigned int pointcloud_height = 0);
+ };
+
+protected:
+ //! (s - s*)
+ vpColVector m_error;
+ //! Interaction matrix
+ vpMatrix m_L;
+ //! Map of camera transformation matrix between the current camera frame to
+ //! the reference camera frame (cCurrent_M_cRef)
+ std::map<std::string, vpHomogeneousMatrix> m_mapOfCameraTransformationMatrix;
+ //! Ponderation between each feature type in the VVS stage
+ std::map<vpTrackerType, double> m_mapOfFeatureFactors;
+ //! Map of Model-based trackers, key is the name of the camera, value is the
+ //! tracker
+ std::map<std::string, TrackerWrapper *> m_mapOfTrackers;
+ //! Percentage of good points over total number of points below which
+ //! tracking is supposed to have failed (only for Edge tracking).
+ double m_percentageGdPt;
+ //! Name of the reference camera
+ std::string m_referenceCameraName;
+ //! Threshold below which the weight associated to a point to consider this
+ //! one as an outlier (only for KLT tracking).
+ double m_thresholdOutlier;
+ //! Robust weights
+ vpColVector m_w;
+ //! Weighted error
+ vpColVector m_weightedError;
+};
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h
index 8319cd2..3f724f7 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbHiddenFaces.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,8 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Generic model based tracker. This class declares the methods to implement in
- * order to have a model based tracker.
+ * Generic model based tracker. This class declares the methods to implement
+ *in order to have a model based tracker.
*
* Authors:
* Romain Tallonneau
@@ -44,222 +45,243 @@
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/mbt/vpMbtPolygon.h>
#include <visp3/mbt/vpMbScanLine.h>
+#include <visp3/mbt/vpMbtPolygon.h>
#ifdef VISP_HAVE_OGRE
- #include <visp3/ar/vpAROgre.h>
+#include <visp3/ar/vpAROgre.h>
#endif
-#include <vector>
#include <limits>
+#include <vector>
+
+template <class PolygonType> class vpMbHiddenFaces;
+
+template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second);
/*!
\class vpMbHiddenFaces
-
- \brief Implementation of the polygons management for the model-based trackers.
- \ingroup group_mbt_faces
+ \brief Implementation of the polygons management for the model-based
+ trackers.
+ \ingroup group_mbt_faces
*/
-template<class PolygonType = vpMbtPolygon>
-class vpMbHiddenFaces
+template <class PolygonType = vpMbtPolygon> class vpMbHiddenFaces
{
- private:
+private:
//! List of polygons
- std::vector<PolygonType *> Lpol ;
+ std::vector<PolygonType *> Lpol;
//! Number of visible polygon
unsigned int nbVisiblePolygon;
vpMbScanLine scanlineRender;
-
+
#ifdef VISP_HAVE_OGRE
vpImage<unsigned char> ogreBackground;
bool ogreInitialised;
unsigned int nbRayAttempts;
double ratioVisibleRay;
vpAROgre *ogre;
- std::vector< Ogre::ManualObject* > lOgrePolygons;
+ std::vector<Ogre::ManualObject *> lOgrePolygons;
bool ogreShowConfigDialog;
#endif
-
- unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
- bool &changed,
- bool useOgre = false, bool not_used = false,
- const vpImage<unsigned char> &I = vpImage<unsigned char>(),
- const vpCameraParameters &cam = vpCameraParameters()) ;
- public :
- vpMbHiddenFaces() ;
- ~vpMbHiddenFaces() ;
-
- void addPolygon(PolygonType *p) ;
+ unsigned int setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed, bool useOgre = false,
+ bool not_used = false, const vpImage<unsigned char> &I = vpImage<unsigned char>(),
+ const vpCameraParameters &cam = vpCameraParameters());
+
+public:
+ vpMbHiddenFaces();
+ ~vpMbHiddenFaces();
+ vpMbHiddenFaces(const vpMbHiddenFaces ©);
+ vpMbHiddenFaces &operator=(vpMbHiddenFaces other);
+ friend void swap<>(vpMbHiddenFaces &first, vpMbHiddenFaces &second);
- bool computeVisibility(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool not_used,
- const vpImage<unsigned char> &I,
- const vpCameraParameters &cam,
- const vpTranslationVector &cameraPos,
- unsigned int index);
+ void addPolygon(PolygonType *p);
- void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+ bool computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed, bool useOgre, bool not_used, const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam, const vpTranslationVector &cameraPos, unsigned int index);
- void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);
+ void computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
- void computeScanLineQuery(const vpPoint &a, const vpPoint &b,
- std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults = false);
+ void computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h);
- vpMbScanLine& getMbScanLineRenderer() { return scanlineRender; }
+ void computeScanLineQuery(const vpPoint &a, const vpPoint &b, std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults = false);
+
+ vpMbScanLine &getMbScanLineRenderer() { return scanlineRender; }
#ifdef VISP_HAVE_OGRE
- void displayOgre(const vpHomogeneousMatrix &cMo);
-#endif
-
- /*!
- Get the list of polygons.
+ void displayOgre(const vpHomogeneousMatrix &cMo);
+#endif
- \return Mbt Klt polygons list.
- */
- std::vector<PolygonType*>& getPolygon() {return Lpol;}
+ /*!
+ Get the list of polygons.
+
+ \return Mbt Klt polygons list.
+ */
+ std::vector<PolygonType *> &getPolygon() { return Lpol; }
#ifdef VISP_HAVE_OGRE
- void initOgre(const vpCameraParameters &cam = vpCameraParameters());
+ void initOgre(const vpCameraParameters &cam = vpCameraParameters());
#endif
-
- /*!
- get the number of visible polygons.
- \return number of visible polygons.
- */
- unsigned int getNbVisiblePolygon() const {return nbVisiblePolygon;}
+ /*!
+ get the number of visible polygons.
+
+ \return number of visible polygons.
+ */
+ unsigned int getNbVisiblePolygon() const { return nbVisiblePolygon; }
#ifdef VISP_HAVE_OGRE
- /*!
- Get the number of rays that will be sent toward each polygon for visibility test.
- Each ray will go from the optic center of the camera to a random point inside the considered polygon.
+ /*!
+ Get the number of rays that will be sent toward each polygon for
+ visibility test. Each ray will go from the optic center of the camera to a
+ random point inside the considered polygon.
- \sa getGoodNbRayCastingAttemptsRatio()
+ \sa getGoodNbRayCastingAttemptsRatio()
- \return Number of rays sent.
- */
- unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }
+ \return Number of rays sent.
+ */
+ unsigned int getNbRayCastingAttemptsForVisibility() { return nbRayAttempts; }
- /*!
- Get the Ogre3D Context.
+ /*!
+ Get the Ogre3D Context.
- \return A pointer on a vpAROgre instance.
- */
- vpAROgre* getOgreContext(){return ogre;}
+ \return A pointer on a vpAROgre instance.
+ */
+ vpAROgre *getOgreContext() { return ogre; }
- /*!
- Get the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ /*!
+ Get the ratio of visibility attempts that has to be successful to consider
+ a polygon as visible.
- \sa getNbRayCastingAttemptsForVisibility()
+ \sa getNbRayCastingAttemptsForVisibility()
- \return Ratio of succesful attempts that has to be considered. Value will be between 0.0 (0%) and 1.0 (100%).
- */
- double getGoodNbRayCastingAttemptsRatio(){ return ratioVisibleRay; }
+ \return Ratio of succesful attempts that has to be considered. Value will
+ be between 0.0 (0%) and 1.0 (100%).
+ */
+ double getGoodNbRayCastingAttemptsRatio() { return ratioVisibleRay; }
#endif
- bool isAppearing(const unsigned int i){ return Lpol[i]->isAppearing(); }
-
-
+ bool isAppearing(const unsigned int i) { return Lpol[i]->isAppearing(); }
+
#ifdef VISP_HAVE_OGRE
/*!
Tell whether if Ogre Context is initialised or not.
\return True if it does, false otherwise.
*/
- bool isOgreInitialised() { return ogreInitialised; }
+ bool isOgreInitialised() { return ogreInitialised; }
#endif
-
- /*!
- Check if the polygon at position i in the list is visible.
-
- \param i : TPosition in the list.
-
- \return Return true if the polygon is visible.
- */
- bool isVisible(const unsigned int i){ return Lpol[i]->isVisible(); }
-
+
+ /*!
+ Check if the polygon at position i in the list is visible.
+
+ \param i : TPosition in the list.
+
+ \return Return true if the polygon is visible.
+*/
+ bool isVisible(const unsigned int i) { return Lpol[i]->isVisible(); }
+
#ifdef VISP_HAVE_OGRE
- bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
+ bool isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index);
#endif
-
- //! operator[] as modifier.
- inline PolygonType* operator[](const unsigned int i) { return Lpol[i];}
- //! operator[] as reader.
- inline const PolygonType* operator[](const unsigned int i) const { return Lpol[i];}
-
- void reset();
-
+
+ //! operator[] as modifier.
+ inline PolygonType *operator[](const unsigned int i) { return Lpol[i]; }
+ //! operator[] as reader.
+ inline const PolygonType *operator[](const unsigned int i) const { return Lpol[i]; }
+
+ void reset();
+
#ifdef VISP_HAVE_OGRE
- /*!
- Set the background size (by default it is 640x480).
- The background size has to match with the size of the image that you are using for the traking.
-
- \warning This function has to be called before initOgre().
-
- \param h : Height of the background
- \param w : Width of the background
- */
- void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w) { ogreBackground = vpImage<unsigned char>(h, w, 0); }
-
- /*!
- Set the number of rays that will be sent toward each polygon for visibility test.
- Each ray will go from the optic center of the camera to a random point inside the considered polygon.
-
- \sa setGoodNbRayCastingAttemptsRatio(const double &)
-
- \param attempts Number of rays to be sent.
- */
- void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }
-
- /*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
-
- \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
-
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
- */
- void setGoodNbRayCastingAttemptsRatio(const double &ratio) {ratioVisibleRay = ratio; if(ratioVisibleRay > 1.0) ratioVisibleRay = 1.0; if(ratioVisibleRay < 0.0) ratioVisibleRay = 0.0;}
- /*!
- Enable/Disable the appearance of Ogre config dialog on startup.
-
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
-
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
- */
- inline void setOgreShowConfigDialog(const bool showConfigDialog){
- ogreShowConfigDialog = showConfigDialog;
- }
+ /*!
+ Set the background size (by default it is 640x480).
+ The background size has to match with the size of the image that you are
+ using for the traking.
+
+ \warning This function has to be called before initOgre().
+
+ \param h : Height of the background
+ \param w : Width of the background
+ */
+ void setBackgroundSizeOgre(const unsigned int &h, const unsigned int &w)
+ {
+ ogreBackground = vpImage<unsigned char>(h, w, 0);
+ }
+
+ /*!
+ Set the number of rays that will be sent toward each polygon for
+ visibility test. Each ray will go from the optic center of the camera to a
+ random point inside the considered polygon.
+
+ \sa setGoodNbRayCastingAttemptsRatio(const double &)
+
+ \param attempts Number of rays to be sent.
+ */
+ void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) { nbRayAttempts = attempts; }
+
+ /*!
+ Set the ratio of visibility attempts that has to be successful to consider
+ a polygon as visible.
+
+ \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
+
+ \param ratio : Ratio of succesful attempts that has to be considered.
+ Value has to be between 0.0 (0%) and 1.0 (100%).
+ */
+ void setGoodNbRayCastingAttemptsRatio(const double &ratio)
+ {
+ ratioVisibleRay = ratio;
+ if (ratioVisibleRay > 1.0)
+ ratioVisibleRay = 1.0;
+ if (ratioVisibleRay < 0.0)
+ ratioVisibleRay = 0.0;
+ }
+ /*!
+ Enable/Disable the appearance of Ogre config dialog on startup.
+
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
+
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
+ */
+ inline void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
#endif
-
- unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed) ;
- unsigned int setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
- unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
-
+
+ unsigned int setVisible(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, const double &angle, bool &changed);
+ unsigned int setVisible(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed);
+ unsigned int setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed);
+
#ifdef VISP_HAVE_OGRE
- unsigned int setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
- unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed) ;
+ unsigned int setVisibleOgre(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed);
+ unsigned int setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears,
+ bool &changed);
#endif
/*!
Get the number of polygons.
-
+
\return Size of the list.
*/
- inline unsigned int size() const { return (unsigned int)Lpol.size(); }
-} ;
+ inline unsigned int size() const { return (unsigned int)Lpol.size(); }
+};
/*!
Basic constructor.
*/
-template<class PolygonType>
-vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces()
- : Lpol(), nbVisiblePolygon(0), scanlineRender()
+template <class PolygonType>
+vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces() : Lpol(), nbVisiblePolygon(0), scanlineRender()
{
#ifdef VISP_HAVE_OGRE
ogreInitialised = false;
@@ -274,43 +296,86 @@ vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces()
/*!
Basic destructor.
*/
-template<class PolygonType>
-vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
+template <class PolygonType> vpMbHiddenFaces<PolygonType>::~vpMbHiddenFaces()
{
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if (Lpol[i]!=NULL){
- delete Lpol[i] ;
+ for (unsigned int i = 0; i < Lpol.size(); i++) {
+ if (Lpol[i] != NULL) {
+ delete Lpol[i];
}
- Lpol[i] = NULL ;
+ Lpol[i] = NULL;
}
Lpol.resize(0);
#ifdef VISP_HAVE_OGRE
- if(ogre != NULL){
+ if (ogre != NULL) {
delete ogre;
ogre = NULL;
}
// This is already done by calling "delete ogre"
-// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
-// if (lOgrePolygons[i]!=NULL){
-// delete lOgrePolygons[i] ;
-// }
-// lOgrePolygons[i] = NULL ;
-// }
+ // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
+ // if (lOgrePolygons[i]!=NULL){
+ // delete lOgrePolygons[i];
+ // }
+ // lOgrePolygons[i] = NULL;
+ // }
lOgrePolygons.resize(0);
#endif
}
/*!
+ Copy constructor.
+*/
+template <class PolygonType>
+vpMbHiddenFaces<PolygonType>::vpMbHiddenFaces(const vpMbHiddenFaces<PolygonType> ©)
+ : 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)
+#endif
+{
+ // Copy the list of polygons
+ for (unsigned int i = 0; i < copy.Lpol.size(); i++) {
+ PolygonType *poly = new PolygonType(*copy.Lpol[i]);
+ Lpol.push_back(poly);
+ }
+}
+
+template <class PolygonType> void swap(vpMbHiddenFaces<PolygonType> &first, vpMbHiddenFaces<PolygonType> &second)
+{
+ using std::swap;
+ swap(first.Lpol, second.Lpol);
+ swap(first.nbVisiblePolygon, second.nbVisiblePolygon);
+ swap(first.scanlineRender, second.scanlineRender);
+#ifdef VISP_HAVE_OGRE
+ swap(first.ogreInitialised, second.ogreInitialised);
+ swap(first.nbRayAttempts, second.nbRayAttempts);
+ swap(first.ratioVisibleRay, second.ratioVisibleRay);
+ swap(first.ogreShowConfigDialog, second.ogreShowConfigDialog);
+ swap(first.ogre, second.ogre);
+ swap(first.ogreBackground, second.ogreBackground);
+#endif
+}
+
+/*!
+ Copy assignment operator.
+*/
+template <class PolygonType>
+vpMbHiddenFaces<PolygonType> &vpMbHiddenFaces<PolygonType>::operator=(vpMbHiddenFaces<PolygonType> other)
+{
+ swap(*this, other);
+
+ return *this;
+}
+
+/*!
Add a polygon to the list of polygons.
-
+
\param p : The polygon to add.
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
+template <class PolygonType> void vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
{
PolygonType *p_new = new PolygonType;
p_new->index = p->index;
@@ -322,40 +387,38 @@ vpMbHiddenFaces<PolygonType>::addPolygon(PolygonType *p)
p_new->setName(p->name);
p_new->hasOrientation = p->hasOrientation;
- for(unsigned int i = 0; i < p->nbpt; i++)
- p_new->p[i]= p->p[i];
+ for (unsigned int i = 0; i < p->nbpt; i++)
+ p_new->p[i] = p->p[i];
Lpol.push_back(p_new);
}
/*!
Reset the Hidden faces (remove the list of PolygonType)
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::reset()
+template <class PolygonType> void vpMbHiddenFaces<PolygonType>::reset()
{
nbVisiblePolygon = 0;
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if (Lpol[i]!=NULL){
- delete Lpol[i] ;
+ for (unsigned int i = 0; i < Lpol.size(); i++) {
+ if (Lpol[i] != NULL) {
+ delete Lpol[i];
}
- Lpol[i] = NULL ;
+ Lpol[i] = NULL;
}
Lpol.resize(0);
#ifdef VISP_HAVE_OGRE
- if(ogre != NULL){
+ if (ogre != NULL) {
delete ogre;
ogre = NULL;
}
// This is already done by calling "delete ogre"
-// for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
-// if (lOgrePolygons[i]!=NULL){
-// delete lOgrePolygons[i] ;
-// }
-// lOgrePolygons[i] = NULL ;
-// }
+ // for(unsigned int i = 0 ; i < lOgrePolygons.size() ; i++){
+ // if (lOgrePolygons[i]!=NULL){
+ // delete lOgrePolygons[i];
+ // }
+ // lOgrePolygons[i] = NULL;
+ // }
lOgrePolygons.resize(0);
@@ -368,19 +431,20 @@ vpMbHiddenFaces<PolygonType>::reset()
}
/*!
- Compute the clipped points of the polygons that have been added via addPolygon().
+ Compute the clipped points of the polygons that have been added via
+ addPolygon().
\param cMo : Pose that will be used to clip the polygons.
\param cam : Camera parameters that will be used to clip the polygons.
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+template <class PolygonType>
+void vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
{
- for (unsigned int i = 0; i < Lpol.size(); i++){
+ for (unsigned int i = 0; i < Lpol.size(); i++) {
// For fast result we could just clip visible polygons.
- // However clipping all of them gives us the possibility to return more information in the scanline visibility results
-// if(Lpol[i]->isVisible())
+ // However clipping all of them gives us the possibility to return more
+ // information in the scanline visibility results
+ // if(Lpol[i]->isVisible())
{
Lpol[i]->changeFrame(cMo);
Lpol[i]->computePolygonClipped(cam);
@@ -389,28 +453,30 @@ vpMbHiddenFaces<PolygonType>::computeClippedPolygons(const vpHomogeneousMatrix &
}
/*!
- Render the scene in order to perform, later via computeScanLineQuery(), visibility tests.
+ Render the scene in order to perform, later via computeScanLineQuery(),
+ visibility tests.
\param cam : Camera parameters that will be used to render the scene.
\param w : Width of the render window.
\param h : Height of the render window.
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w, const unsigned int &h){
+template <class PolygonType>
+void vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &cam, const unsigned int &w,
+ const unsigned int &h)
+{
std::vector<std::vector<std::pair<vpPoint, unsigned int> > > polyClipped(Lpol.size());
- std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > listPolyClipped;
+ std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> listPolyClipped;
std::vector<int> listPolyIndices;
- for (unsigned int i = 0; i < Lpol.size(); i++){
+ for (unsigned int i = 0; i < Lpol.size(); i++) {
// For fast result we could just use visible polygons.
- // However using all of them gives us the possibility to return more information in the scanline visibility results
-// if(Lpol[i]->isVisible())
+ // However using all of them gives us the possibility to return more
+ // information in the scanline visibility results
+ // if(Lpol[i]->isVisible())
{
polyClipped[i].clear();
Lpol[i]->getPolygonClipped(polyClipped[i]);
- if(polyClipped[i].size() != 0)
- {
+ if (polyClipped[i].size() != 0) {
listPolyClipped.push_back(&polyClipped[i]);
listPolyIndices.push_back(Lpol[i]->getIndex());
}
@@ -427,46 +493,44 @@ vpMbHiddenFaces<PolygonType>::computeScanLineRender(const vpCameraParameters &ca
\param a : First point of the line.
\param b : Second point of the line.
- \param lines : Result of the scanline visibility. List of the visible parts of the line.
- \param displayResults : True if the results have to be displayed. False otherwise
+ \param lines : Result of the scanline visibility. List of the visible parts
+ of the line. \param displayResults : True if the results have to be
+ displayed. False otherwise
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::computeScanLineQuery(const vpPoint &a, const vpPoint &b,
- std::vector<std::pair<vpPoint, vpPoint> > &lines,
- const bool &displayResults)
+template <class PolygonType>
+void vpMbHiddenFaces<PolygonType>::computeScanLineQuery(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults)
{
- scanlineRender.queryLineVisibility(a,b,lines,displayResults);
+ scanlineRender.queryLineVisibility(a, b, lines, displayResults);
}
/*!
Compute the number of visible polygons.
-
+
\param cMo : The pose of the camera
\param angleAppears : Angle used to test the appearance of a face
\param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param useOgre : True if a Ogre is used to test the visibility, False otherwise
- \param not_used : Unused parameter.
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise \param useOgre : True if a Ogre is used to
+ test the visibility, False otherwise \param not_used : Unused parameter.
\param I : Image used to test if a face is entirely projected in the image.
\param cam : Camera parameters.
-
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool not_used,
- const vpImage<unsigned char> &I,
- const vpCameraParameters &cam)
-{
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed, bool useOgre,
+ bool not_used, const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam)
+{
nbVisiblePolygon = 0;
changed = false;
-
+
vpTranslationVector cameraPos;
-
- if(useOgre){
+
+ if (useOgre) {
#ifdef VISP_HAVE_OGRE
cMo.inverse().extract(cameraPos);
ogre->renderOneFrame(ogreBackground, cMo);
@@ -474,11 +538,11 @@ vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
vpTRACE("ViSP doesn't have Ogre3D, simple visibility test used");
#endif
}
-
- for (unsigned int i = 0; i < Lpol.size(); i++){
- //std::cout << "Calling poly: " << i << std::endl;
+
+ 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))
- nbVisiblePolygon ++;
+ nbVisiblePolygon++;
}
return nbVisiblePolygon;
}
@@ -489,206 +553,198 @@ vpMbHiddenFaces<PolygonType>::setVisiblePrivate(const vpHomogeneousMatrix &cMo,
\param cMo : The pose of the camera
\param angleAppears : Angle used to test the appearance of a face
\param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param useOgre : True if a Ogre is used to test the visibility, False otherwise.
- \param not_used : Unused parameter.
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise \param useOgre : True if a Ogre is used to
+ test the visibility, False otherwise. \param not_used : Unused parameter.
\param I : Image used to test if a face is entirely projected in the image.
\param cam : Camera parameters.
- \param cameraPos : Position of the camera. Used only when Ogre is used as 3rd party.
- \param index : Index of the face to consider.
+ \param cameraPos : Position of the camera. Used only when Ogre is used as
+ 3rd party. \param index : Index of the face to consider.
\return Return true if the face is visible.
*/
-template<class PolygonType>
-bool
-vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo,
- const double &angleAppears, const double &angleDisappears,
- bool &changed, bool useOgre, bool not_used,
- const vpImage<unsigned char> & I,
- const vpCameraParameters & cam,
- const vpTranslationVector &
- cameraPos,
- unsigned int index)
+template <class PolygonType>
+bool vpMbHiddenFaces<PolygonType>::computeVisibility(const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed, bool useOgre,
+ bool not_used, const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam,
+ const vpTranslationVector &cameraPos, unsigned int index)
{
- (void) not_used;
+ (void)not_used;
unsigned int i = index;
Lpol[i]->changeFrame(cMo);
Lpol[i]->isappearing = false;
- //Commented because we need to compute visibility
+ // Commented because we need to compute visibility
// even when dealing with line in level of detail case
/*if(Lpol[i]->getNbPoint() <= 2)
{
Lpol[i]->isvisible = true;
}
- else*/{
- if(Lpol[i]->isVisible())
- {
- bool testDisappear = false;
- unsigned int nbCornerInsidePrev = 0;
+ else*/ {
+ if (Lpol[i]->isVisible()) {
+ bool testDisappear = false;
+ // unsigned int nbCornerInsidePrev = 0;
- if(!testDisappear){
- if(useOgre)
+ 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));
- }
+ {
+ (void)cameraPos; // Avoid warning
+ testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
+ }
#endif
- else
- testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
- }
-
- // test if the face is still visible
- if(testDisappear){
-// std::cout << "Face " << i << " disappears" << std::endl;
- changed = true;
- Lpol[i]->isvisible = false;
- }
- else {
- //nbVisiblePolygon++;
- Lpol[i]->isvisible = true;
+ else
+ testDisappear = (!Lpol[i]->isVisible(cMo, angleDisappears, false, cam, I));
+ }
- if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
+ // test if the face is still visible
+ if (testDisappear) {
+ // std::cout << "Face " << i << " disappears" <<
+ // std::endl;
changed = true;
- }
- }
- else
- {
- bool testAppear = true;
+ Lpol[i]->isvisible = false;
+ } else {
+ // nbVisiblePolygon++;
+ Lpol[i]->isvisible = true;
+
+ // if(nbCornerInsidePrev > Lpol[i]->getNbCornerInsidePrevImage())
+ // changed = true;
+ }
+ } else {
+ bool testAppear = true;
- if(testAppear){
- if(useOgre)
+ 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));
+ testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
#endif
- else
- testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
- }
+ else
+ testAppear = (Lpol[i]->isVisible(cMo, angleAppears, false, cam, I));
+ }
- if(testAppear){
-// std::cout << "Face " << i << " appears" << std::endl;
- Lpol[i]->isvisible = true;
- changed = true;
- //nbVisiblePolygon++;
- }
- else{
-// std::cout << "Problem" << std::endl;
- Lpol[i]->isvisible = false;
+ if (testAppear) {
+ // std::cout << "Face " << i << " appears" << std::endl;
+ Lpol[i]->isvisible = true;
+ changed = true;
+ // nbVisiblePolygon++;
+ } else {
+ // std::cout << "Problem" << std::endl;
+ Lpol[i]->isvisible = false;
+ }
}
}
- }
- // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon << std::endl;
+ // std::cout << "Nombre de polygones visibles: " << nbVisiblePolygon <<
+ // std::endl;
return Lpol[i]->isvisible;
}
/*!
Compute the number of visible polygons.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera.
- \param angle : Angle used to test the appearance and disappearance of a face.
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
+
+ \param I : Image used to check if the region of interest is inside the
+ image. \param cam : Camera parameters. \param cMo : The pose of the camera.
+ \param angle : Angle used to test the appearance and disappearance of a
+ face. \param changed : True if a face appeared, disappeared or too many
+ points have been lost. False otherwise
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angle, bool &changed)
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, const double &angle,
+ bool &changed)
{
return setVisible(I, cam, cMo, angle, angle, changed);
}
/*!
Compute the number of visible polygons.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
-
+
+ \param I : Image used to check if the region of interest is inside the
+ image. \param cam : Camera parameters. \param cMo : The pose of the camera
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise \param angleAppears : Angle used to test the
+ appearance of a face \param angleDisappears : Angle used to test the
+ disappearance of a face
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisible(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed)
{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false,true,I,cam);
+ return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false, true, I, cam);
}
/*!
Compute the number of visible polygons.
-
+
\param cMo : The pose of the camera
\param angleAppears : Angle used to test the appearance of a face
\param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisible(const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed)
{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,false);
+ return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, false);
}
#ifdef VISP_HAVE_OGRE
/*!
Initialise the ogre context for face visibility tests.
-
+
\param cam : Camera parameters.
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
+template <class PolygonType> void vpMbHiddenFaces<PolygonType>::initOgre(const vpCameraParameters &cam)
{
ogreInitialised = true;
ogre->setCameraParameters(cam);
ogre->setShowConfigDialog(ogreShowConfigDialog);
ogre->init(ogreBackground, false, true);
-
- for(unsigned int n = 0 ; n < Lpol.size(); n++){
- Ogre::ManualObject* manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
+
+ for (unsigned int n = 0; n < Lpol.size(); n++) {
+ Ogre::ManualObject *manual = ogre->getSceneManager()->createManualObject(Ogre::StringConverter::toString(n));
manual->begin("BaseWhiteNoLighting", Ogre::RenderOperation::OT_LINE_STRIP);
- for(unsigned int i = 0; i < Lpol[n]->nbpt; i++){
- manual->position( (Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(), (Ogre::Real)Lpol[n]->p[i].get_oZ());
+ for (unsigned int i = 0; i < Lpol[n]->nbpt; i++) {
+ manual->position((Ogre::Real)Lpol[n]->p[i].get_oX(), (Ogre::Real)Lpol[n]->p[i].get_oY(),
+ (Ogre::Real)Lpol[n]->p[i].get_oZ());
manual->colour(1.0, 1.0, 1.0);
manual->index(i);
}
-
+
manual->index(0);
manual->end();
ogre->getSceneManager()->getRootSceneNode()->createChildSceneNode()->attachObject(manual);
-
+
lOgrePolygons.push_back(manual);
}
}
/*!
Update the display in Ogre Window.
-
+
\param cMo : Pose used to display.
*/
-template<class PolygonType>
-void
-vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
+template <class PolygonType> void vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
{
- if(ogreInitialised && !ogre->isWindowHidden()){
- for(unsigned int i = 0 ; i < Lpol.size() ; i++){
- if(Lpol[i]->isVisible()){
+ if (ogreInitialised && !ogre->isWindowHidden()) {
+ for (unsigned int i = 0; i < Lpol.size(); i++) {
+ if (Lpol[i]->isVisible()) {
lOgrePolygons[i]->setVisible(true);
- }
- else
+ } else
lOgrePolygons[i]->setVisible(false);
}
ogre->display(ogreBackground, cMo);
@@ -697,83 +753,84 @@ vpMbHiddenFaces<PolygonType>::displayOgre(const vpHomogeneousMatrix &cMo)
/*!
Compute the number of visible polygons through Ogre3D.
-
- \param I : Image used to check if the region of interest is inside the image.
- \param cam : Camera parameters.
- \param cMo : The pose of the camera
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
- \param angleAppears : Angle used to test the appearance of a face
- \param angleDisappears : Angle used to test the disappearance of a face
-
+
+ \param I : Image used to check if the region of interest is inside the
+ image. \param cam : Camera parameters. \param cMo : The pose of the camera
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise \param angleAppears : Angle used to test the
+ appearance of a face \param angleDisappears : Angle used to test the
+ disappearance of a face
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpImage<unsigned char>& I, const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpImage<unsigned char> &I,
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo,
+ const double &angleAppears, const double &angleDisappears,
+ bool &changed)
{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true,true,I,cam);
+ return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true, true, I, cam);
}
/*!
Compute the number of visible polygons through Ogre3D.
-
+
\param cMo : The pose of the camera
\param angleAppears : Angle used to test the appearance of a face
\param angleDisappears : Angle used to test the disappearance of a face
- \param changed : True if a face appeared, disappeared or too many points have been lost. False otherwise
-
+ \param changed : True if a face appeared, disappeared or too many points
+ have been lost. False otherwise
+
\return Return the number of visible polygons
*/
-template<class PolygonType>
-unsigned int
-vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears, const double &angleDisappears, bool &changed)
+template <class PolygonType>
+unsigned int vpMbHiddenFaces<PolygonType>::setVisibleOgre(const vpHomogeneousMatrix &cMo, const double &angleAppears,
+ const double &angleDisappears, bool &changed)
{
- return setVisiblePrivate(cMo,angleAppears,angleDisappears,changed,true);
+ return setVisiblePrivate(cMo, angleAppears, angleDisappears, changed, true);
}
/*!
Test the visibility of a polygon through Ogre3D via RayCasting.
-
+
\param cameraPos : Position of the camera in the 3D world.
\param index : Index of the polygon.
-
+
\return Return true if the polygon is visible, False otherwise.
*/
-template<class PolygonType>
-bool
-vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
+template <class PolygonType>
+bool vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos, const unsigned int &index)
{
- Ogre::Vector3 camera((Ogre::Real)cameraPos[0],(Ogre::Real)cameraPos[1],(Ogre::Real)cameraPos[2]);
- if(!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())){
+ Ogre::Vector3 camera((Ogre::Real)cameraPos[0], (Ogre::Real)cameraPos[1], (Ogre::Real)cameraPos[2]);
+ if (!ogre->getCamera()->isVisible(lOgrePolygons[index]->getBoundingBox())) {
lOgrePolygons[index]->setVisible(false);
Lpol[index]->isvisible = false;
return false;
}
- //Get the center of gravity
+ // Get the center of gravity
bool visible = false;
unsigned int nbVisible = 0;
- for(unsigned int i = 0; i < nbRayAttempts; i++)
- {
- Ogre::Vector3 origin(0,0,0);
+ for (unsigned int i = 0; i < nbRayAttempts; i++) {
+ Ogre::Vector3 origin(0, 0, 0);
Ogre::Real totalFactor = 0.0f;
- for(unsigned int j = 0 ; j < Lpol[index]->getNbPoint() ; j++)
- {
- Ogre::Real factor = 1.0f;
+ for (unsigned int j = 0; j < Lpol[index]->getNbPoint(); j++) {
+ Ogre::Real factor = 1.0f;
- if(nbRayAttempts > 1){
- int r = rand() % 101;
+ if (nbRayAttempts > 1) {
+ int r = rand() % 101;
- if(r != 0)
- factor = ((Ogre::Real)r)/100.0f;
- }
+ if (r != 0)
+ factor = ((Ogre::Real)r) / 100.0f;
+ }
- Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(), (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
- tmp *= factor;
- origin += tmp;
- totalFactor += factor;
+ Ogre::Vector3 tmp((Ogre::Real)Lpol[index]->getPoint(j).get_oX(), (Ogre::Real)Lpol[index]->getPoint(j).get_oY(),
+ (Ogre::Real)Lpol[index]->getPoint(j).get_oZ());
+ tmp *= factor;
+ origin += tmp;
+ totalFactor += factor;
}
origin /= totalFactor;
@@ -788,69 +845,68 @@ vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos
Ogre::RaySceneQueryResult &result = mRaySceneQuery->execute();
Ogre::RaySceneQueryResult::iterator it = result.begin();
-// while(it != result.end()){
-// std::cout << it->movable->getName() << "(" << it->distance<< ") : " << std::flush;
-// it++;
-// }
-// std::cout << std::endl;
-// it = result.begin();
-
- if(it != result.end())
- if(it->movable->getName().find("SimpleRenderable") != Ogre::String::npos) //Test if the ogreBackground is intersect in first
- it++;
-
- double distance, distancePrev;
- // In a case of a two-axis aligned segment, ray collision is not always working.
- if(Lpol[index]->getNbPoint() == 2 &&
- (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) < std::numeric_limits<double>::epsilon()) +
- (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) < std::numeric_limits<double>::epsilon()) +
- (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) < std::numeric_limits<double>::epsilon())) >= 2 ))
- {
- if(it != result.end())
- {
- if(it->movable->getName() == Ogre::StringConverter::toString(index)){
+ // while(it != result.end()){
+ // std::cout << it->movable->getName() << "(" << it->distance<< ") :
+ // " << std::flush; it++;
+ // }
+ // std::cout << std::endl;
+ // it = result.begin();
+
+ if (it != result.end())
+ if (it->movable->getName().find("SimpleRenderable") !=
+ Ogre::String::npos) // Test if the ogreBackground is intersect in
+ // first
+ ++it;
+
+ double distance;
+ // In a case of a two-axis aligned segment, ray collision is not always
+ // working.
+ if (Lpol[index]->getNbPoint() == 2 &&
+ (((std::fabs(Lpol[index]->getPoint(0).get_oX() - Lpol[index]->getPoint(1).get_oX()) <
+ std::numeric_limits<double>::epsilon()) +
+ (std::fabs(Lpol[index]->getPoint(0).get_oY() - Lpol[index]->getPoint(1).get_oY()) <
+ std::numeric_limits<double>::epsilon()) +
+ (std::fabs(Lpol[index]->getPoint(0).get_oZ() - Lpol[index]->getPoint(1).get_oZ()) <
+ std::numeric_limits<double>::epsilon())) >= 2)) {
+ if (it != result.end()) {
+ if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
nbVisible++;
- }
- else
- {
+ } else {
distance = it->distance;
- // Cannot use epsilon for comparison as ray lenght is slightly different from the collision distance returned by Ogre::RaySceneQueryResult.
- if(distance > distanceCollision || std::fabs(distance - distanceCollision) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
+ // Cannot use epsilon for comparison as ray lenght is slightly
+ // different from the collision distance returned by
+ // Ogre::RaySceneQueryResult.
+ if (distance > distanceCollision || std::fabs(distance - distanceCollision) <
+ 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/)
nbVisible++;
}
- }
- else
+ } else
nbVisible++; // Collision not detected but present.
- }
- else
- {
- if(it != result.end())
- {
+ } else {
+ if (it != result.end()) {
distance = it->distance;
- distancePrev = distance;
+ double distancePrev = distance;
- //std::cout << "For " << Ogre::StringConverter::toString(index) << ": " << it->movable->getName() << " / " << std::flush;
+ // 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++;
- while(it != result.end())
- {
+ } else {
+ ++it;
+ while (it != result.end()) {
distance = it->distance;
- if(std::fabs(distance - distancePrev) < 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/){
- //std::cout << it->movable->getName() << " / " << std::flush;
- if(it->movable->getName() == Ogre::StringConverter::toString(index)){
+ if (std::fabs(distance - distancePrev) <
+ 1e-6 /*std::fabs(distance) * std::numeric_limits<double>::epsilon()*/) {
+ // std::cout << it->movable->getName() << " / " << std::flush;
+ if (it->movable->getName() == Ogre::StringConverter::toString(index)) {
nbVisible++;
break;
}
- it++;
+ ++it;
distancePrev = distance;
- }
- else
+ } else
break;
}
}
@@ -858,27 +914,25 @@ vpMbHiddenFaces<PolygonType>::isVisibleOgre(const vpTranslationVector &cameraPos
}
ogre->getSceneManager()->destroyQuery(mRaySceneQuery);
-
}
- if(((double)nbVisible)/((double)nbRayAttempts) > ratioVisibleRay ||
- std::fabs(((double)nbVisible)/((double)nbRayAttempts) - ratioVisibleRay) < ratioVisibleRay * std::numeric_limits<double>::epsilon())
+ if (((double)nbVisible) / ((double)nbRayAttempts) > ratioVisibleRay ||
+ std::fabs(((double)nbVisible) / ((double)nbRayAttempts) - ratioVisibleRay) <
+ ratioVisibleRay * std::numeric_limits<double>::epsilon())
visible = true;
else
visible = false;
- if(visible){
+ if (visible) {
lOgrePolygons[index]->setVisible(true);
Lpol[index]->isvisible = true;
- }
- else{
+ } else {
lOgrePolygons[index]->setVisible(false);
Lpol[index]->isvisible = false;
}
return Lpol[index]->isvisible;
}
-#endif //VISP_HAVE_OGRE
+#endif // VISP_HAVE_OGRE
#endif // vpMbHiddenFaces
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h
index b92b58e..1278ec1 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbKltMultiTracker.h
@@ -2,19 +2,20 @@
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
- * GPL, please contact INRIA about acquiring a ViSP Professional
+ * GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See https://visp.inria.fr/download/ for more information.
+ *
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
@@ -24,7 +25,7 @@
*
* If you have questions regarding the use of this file, please contact
* INRIA at visp at inria.fr
- *
+ *
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
@@ -50,11 +51,10 @@
#include <visp3/core/vpConfig.h>
-#if (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 <visp3/mbt/vpMbKltTracker.h>
-
/*!
\class vpMbKltMultiTracker
\ingroup group_mbt_trackers
@@ -62,24 +62,34 @@
\brief Model based stereo (or more) tracker using only KLT.
- The \ref tutorial-tracking-mb-stereo is a good starting point to use this class.
+ The \ref tutorial-tracking-mb-stereo is a good starting point to use this
+ class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
+ The tracker requires the knowledge of the 3D model that could be provided in
+ a vrml or in a cao file. The cao format is described in loadCAOModel(). It
+ may also use an xml file used to tune the behavior of the tracker and an
init file used to compute the pose at the very first image.
*/
-class VISP_EXPORT vpMbKltMultiTracker: public vpMbKltTracker
+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)
+ //! Map of camera transformation matrix between the current camera frame to
+ //! the reference camera frame (cCurrent_M_cRef)
std::map<std::string, vpHomogeneousMatrix> m_mapOfCameraTransformationMatrix;
//! Map of Model-based klt trackers
- std::map<std::string, vpMbKltTracker*> m_mapOfKltTrackers;
+ std::map<std::string, vpMbKltTracker *> m_mapOfKltTrackers;
//! Name of the reference camera
std::string m_referenceCameraName;
+ //! Interaction matrix
+ vpMatrix m_L_kltMulti;
+ //! (s - s*)
+ vpColVector m_error_kltMulti;
+ //! Robust weights
+ vpColVector m_w_kltMulti;
+ //! Weighted error
+ vpColVector m_weightedError_kltMulti;
public:
vpMbKltMultiTracker();
@@ -90,31 +100,33 @@ public:
/** @name Inherited functionalities from vpMbKltMultiTracker */
//@{
- virtual void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name="");
+ virtual void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
+ const std::string &name = "");
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel=false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- virtual void display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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<unsigned char> &I1, const vpImage<unsigned char> &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<vpRGBa>& I1, const vpImage<vpRGBa>& 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<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual void display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel=false);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters, const vpColor &col,
+ const unsigned int thickness = 1, const bool displayFullModel = false);
virtual std::vector<std::string> getCameraNames() const;
@@ -126,18 +138,18 @@ public:
using vpMbTracker::getClipping;
virtual unsigned int getClipping(const std::string &cameraName) const;
- virtual vpMbHiddenFaces<vpMbtPolygon>& getFaces();
- virtual vpMbHiddenFaces<vpMbtPolygon>& getFaces(const std::string &cameraName);
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces();
+ virtual vpMbHiddenFaces<vpMbtPolygon> &getFaces(const std::string &cameraName);
virtual std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > getFaces() const;
- virtual std::list<vpMbtDistanceCircle*>& getFeaturesCircle();
- virtual std::list<vpMbtDistanceCircle*>& getFeaturesCircle(const std::string &cameraName);
+ virtual std::list<vpMbtDistanceCircle *> &getFeaturesCircle();
+ virtual std::list<vpMbtDistanceCircle *> &getFeaturesCircle(const std::string &cameraName);
- virtual std::list<vpMbtDistanceKltPoints*>& getFeaturesKlt();
- virtual std::list<vpMbtDistanceKltPoints*>& getFeaturesKlt(const std::string &cameraName);
+ virtual std::list<vpMbtDistanceKltPoints *> &getFeaturesKlt();
+ virtual std::list<vpMbtDistanceKltPoints *> &getFeaturesKlt(const std::string &cameraName);
- virtual std::list<vpMbtDistanceKltCylinder*>& getFeaturesKltCylinder();
- virtual std::list<vpMbtDistanceKltCylinder*>& getFeaturesKltCylinder(const std::string &cameraName);
+ virtual std::list<vpMbtDistanceKltCylinder *> &getFeaturesKltCylinder();
+ virtual std::list<vpMbtDistanceKltCylinder *> &getFeaturesKltCylinder(const std::string &cameraName);
virtual std::map<std::string, std::vector<vpImagePoint> > getKltImagePoints() const;
@@ -148,10 +160,10 @@ public:
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
virtual std::map<std::string, std::vector<cv::Point2f> > getKltPoints() const;
#else
- virtual std::map<std::string, CvPoint2D32f*> getKltPoints();
+ virtual std::map<std::string, CvPoint2D32f *> getKltPoints();
#endif
- virtual std::map<std::string, int> getNbKltPoints() const;
+ virtual std::map<std::string, int> getKltNbPoints() const;
virtual unsigned int getNbPolygon() const;
virtual std::map<std::string, unsigned int> getMultiNbPolygon() const;
@@ -161,86 +173,92 @@ public:
\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<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const;
- virtual void init(const vpImage<unsigned char>& I);
+ virtual inline vpColVector getError() const { return m_error_kltMulti; }
+
+ virtual inline vpColVector getRobustWeights() const { return m_w_kltMulti; }
+
+ virtual void init(const vpImage<unsigned char> &I);
#ifdef VISP_HAVE_MODULE_GUI
- virtual void initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile="");
+ virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile = "");
- virtual void initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp=false);
+ virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp = false);
- virtual void initClick(const vpImage<unsigned char>& I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp=false,
- const bool firstCameraIsReference=true);
+ virtual void initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp = false,
+ const bool firstCameraIsReference = true);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp=false);
+ const std::string &initFile, const bool displayHelp = false);
virtual void initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp=false);
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp = false);
#endif
- virtual void initFromPose(const vpImage<unsigned char>& I, const std::string &initFile);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const std::string &initFile);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);
- virtual void initFromPose(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference=true);
+ virtual void initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
virtual void loadConfigFile(const std::string &configFile);
- virtual void loadConfigFile(const std::string& configFile1, const std::string& configFile2,
- const bool firstCameraIsReference=true);
+ virtual void loadConfigFile(const std::string &configFile1, const std::string &configFile2,
+ const bool firstCameraIsReference = true);
virtual void loadConfigFile(const std::map<std::string, std::string> &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<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
virtual void reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose=false, const bool firstCameraIsReference=true);
+ 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<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose=false);
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose = false);
virtual void resetTracker();
virtual void setAngleAppear(const double &a);
virtual void setAngleDisappear(const double &a);
- virtual void setCameraParameters(const vpCameraParameters& camera);
+ virtual void setCameraParameters(const vpCameraParameters &camera);
- virtual void setCameraParameters(const vpCameraParameters& camera1, const vpCameraParameters& camera2,
- const bool firstCameraIsReference=true);
+ 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<std::string, vpCameraParameters> &mapOfCameraParameters);
virtual void setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix);
+ const vpHomogeneousMatrix &cameraTransformationMatrix);
- virtual void setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
+ virtual void
+ setCameraTransformationMatrix(const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix);
virtual void setClipping(const unsigned int &flags);
virtual void setClipping(const std::string &cameraName, const unsigned int &flags);
- virtual void setCovarianceComputation(const bool& flag);
+ virtual void setCovarianceComputation(const bool &flag);
virtual void setDisplayFeatures(const bool displayF);
@@ -253,19 +271,21 @@ public:
void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts);
#endif
- virtual void setKltOpencv(const vpKltOpencv& t);
+ virtual void setKltMaskBorder(const unsigned int &e);
+
+ virtual void setKltOpencv(const vpKltOpencv &t);
virtual void setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfOpenCVTrackers);
- 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 setKltThresholdAcceptation(const double th);
- virtual void setMaskBorder(const unsigned int &e);
+ 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 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 setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name);
+ const std::string &name);
virtual void setNearClippingDistance(const double &dist);
virtual void setNearClippingDistance(const std::string &cameraName, const double &dist);
@@ -278,42 +298,50 @@ public:
virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
- virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2, const vpHomogeneousMatrix &c1Mo,
- const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference=true);
+ virtual void setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference = true);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_);
+ const vpHomogeneousMatrix &cMo_);
virtual void setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses);
virtual void setReferenceCameraName(const std::string &referenceCameraName);
virtual void setScanLineVisibilityTest(const bool &v);
- virtual void setThresholdAcceptation(const double th);
-
virtual void setUseKltTracking(const std::string &name, const bool &useKltTracking);
virtual void track(const vpImage<unsigned char> &I);
- virtual void track(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2);
+ virtual void track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2);
virtual void track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
//@}
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+ /* vp_deprecated */ virtual std::map<std::string, int> getNbKltPoints() const;
+ /* vp_deprecated */ virtual void setMaskBorder(const unsigned int &e);
+ /* vp_deprecated */ virtual void setThresholdAcceptation(const double th);
+ //@}
+
protected:
/** @name Protected Member Functions Inherited from vpMbKltMultiTracker */
//@{
- virtual void computeVVS(std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w);
- virtual void computeVVSWeights(const unsigned int iter, const unsigned int nbInfos,
- std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &R, vpColVector &w_true, vpColVector &w,
- std::map<std::string, vpRobust> &mapOfRobusts, double threshold);
+ virtual void computeVVS();
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+ virtual void computeVVSInteractionMatrixAndResidu(std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist);
+ virtual void computeVVSWeights();
+ using vpMbTracker::computeVVSWeights;
+
+ virtual void postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
- virtual void preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNbInfos,
- std::map<std::string, unsigned int> &mapOfNbFaceUsed);
+ virtual void preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages);
- virtual void postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w_klt);
using vpMbKltTracker::reinit;
virtual void reinit(/* const vpImage<unsigned char>& I */);
//@}
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h
index 0191abb..09a7efe 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbKltTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,56 +48,56 @@
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
-#include <visp3/mbt/vpMbTracker.h>
-#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/mbt/vpMbtKltXmlParser.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpRobust.h>
#include <visp3/core/vpSubColVector.h>
#include <visp3/core/vpSubMatrix.h>
-#include <visp3/core/vpExponentialMap.h>
-#include <visp3/mbt/vpMbtDistanceKltPoints.h>
+#include <visp3/klt/vpKltOpencv.h>
+#include <visp3/mbt/vpMbTracker.h>
#include <visp3/mbt/vpMbtDistanceCircle.h>
#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
+#include <visp3/mbt/vpMbtDistanceKltPoints.h>
+#include <visp3/mbt/vpMbtKltXmlParser.h>
+#include <visp3/vision/vpHomography.h>
/*!
\class vpMbKltTracker
\ingroup group_mbt_trackers
\warning This class is only available if OpenCV is installed, and used.
-
+
\brief Model based tracker using only KLT.
The \ref tutorial-tracking-mb is a good starting point to use this class.
- The tracker requires the knowledge of the 3D model that could be provided in a vrml
- or in a cao file. The cao format is described in loadCAOModel().
- It may also use an xml file used to tune the behavior of the tracker and an
- init file used to compute the pose at the very first image.
+ The tracker requires the knowledge of the 3D model that could be provided in
+a vrml or in a cao file. The cao format is described in loadCAOModel(). It may
+also use an xml file used to tune the behavior of the tracker and an init file
+used to compute the pose at the very first image.
+
+ The following code shows the simplest way to use the tracker. The \ref
+tutorial-tracking-mb is also a good starting point to use this class.
- The following code shows the simplest way to use the tracker. The \ref tutorial-tracking-mb is also a good starting point to use this class.
-
\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
-#include <visp/vpException.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpException.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbKltTracker.h>
int main()
{
#if defined VISP_HAVE_OPENCV
vpMbKltTracker tracker; // Create a model based tracker via KLT points.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose computed using the tracker.
+ vpHomogeneousMatrix cMo; // Pose computed using the tracker.
vpCameraParameters cam;
-
+
// Acquire an image
vpImageIo::read(I, "cube.pgm");
-
+
#if defined VISP_HAVE_X11
vpDisplayX display;
display.init(I,100,100,"Mb Klt Tracker");
@@ -107,61 +108,63 @@ int main()
#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.
+ // 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
vpDisplay::display(I);
tracker.track(I); // Track the object on this image
tracker.getPose(cMo); // Get the pose
-
+
tracker.display(I, cMo, cam, vpColor::darkRed, 1); // Display the model at the computed pose.
vpDisplay::flush(I);
}
#if defined VISP_HAVE_XML2
- // Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
- vpXmlParser::cleanup();
-#endif
+ // Cleanup memory allocated by xml library used to parse the xml config file
+in vpMbKltTracker::loadConfigFile() vpXmlParser::cleanup(); #endif
return 0;
#endif
}
-\endcode
+\endcode
The tracker can also be used without display, in that case the initial pose
- must be known (object always at the same initial pose for example) or computed
- using another method:
+ must be known (object always at the same initial pose for example) or
+computed using another method:
\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
#include <visp/vpImageIo.h>
+#include <visp/vpMbKltTracker.h>
int main()
{
#if defined VISP_HAVE_OPENCV
vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
-
+ vpHomogeneousMatrix cMo; // Pose used in entry (has to be defined), then computed using the tracker.
+
//acquire an image
vpImageIo::read(I, "cube.pgm"); // Example of acquisition
#if defined VISP_HAVE_XML2
tracker.loadConfigFile("cube.xml"); // Load the configuration of the tracker
#endif
- tracker.loadModel("cube.cao"); // load the 3d model, to read .wrl model coin is required, if coin is not installed .cao file can be used.
+ // 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){
// acquire a new image
tracker.track(I); // track the object on this image
- tracker.getPose(cMo); // get the pose
+ tracker.getPose(cMo); // get the pose
}
-
+
#if defined VISP_HAVE_XML2
// Cleanup memory allocated by xml library used to parse the xml config file in vpMbKltTracker::loadConfigFile()
vpXmlParser::cleanup();
@@ -172,28 +175,28 @@ int main()
}
\endcode
- Finally it can be used not to track an object but just to display a model at a
- given pose:
+ Finally it can be used not to track an object but just to display a model at
+a given pose:
\code
-#include <visp/vpMbKltTracker.h>
-#include <visp/vpImage.h>
-#include <visp/vpImageIo.h>
-#include <visp/vpHomogeneousMatrix.h>
#include <visp/vpCameraParameters.h>
#include <visp/vpDisplayX.h>
+#include <visp/vpHomogeneousMatrix.h>
+#include <visp/vpImage.h>
+#include <visp/vpImageIo.h>
+#include <visp/vpMbKltTracker.h>
int main()
{
#if defined VISP_HAVE_OPENCV
vpMbKltTracker tracker; // Create a model based tracker via Klt Points.
vpImage<unsigned char> I;
- vpHomogeneousMatrix cMo; // Pose used to display the model.
+ vpHomogeneousMatrix cMo; // Pose used to display the model.
vpCameraParameters cam;
-
+
// Acquire an image
vpImageIo::read(I, "cube.pgm");
-
+
#if defined VISP_HAVE_X11
vpDisplayX display;
display.init(I,100,100,"Mb Klt Tracker");
@@ -203,7 +206,8 @@ int main()
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.
+ // 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
@@ -212,7 +216,7 @@ int main()
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();
@@ -223,13 +227,13 @@ int main()
}
\endcode
*/
-class VISP_EXPORT vpMbKltTracker: virtual public vpMbTracker
+class VISP_EXPORT vpMbKltTracker : public virtual vpMbTracker
{
friend class vpMbKltMultiTracker;
friend class vpMbEdgeKltMultiTracker;
protected:
- //! Temporary OpenCV image for fast conversion.
+//! Temporary OpenCV image for fast conversion.
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat cur;
#else
@@ -237,156 +241,153 @@ protected:
#endif
//! Initial pose.
vpHomogeneousMatrix c0Mo;
- //! If true, compute the interaction matrix at each iteration of the minimization. Otherwise, compute it only on the first iteration.
- bool compute_interaction;
- //! Flag to specify whether the init method is called the first or not (specific calls to realize in this case).
+ //! Flag to specify whether the init method is called the first or not
+ //! (specific calls to realize in this case).
bool firstInitialisation;
//! Erosion of the mask
unsigned int maskBorder;
- //! The gain of the virtual visual servoing stage.
- double lambda;
- //! The maximum iteration of the virtual visual servoing stage.
- unsigned int maxIter;
- //! Threshold below which the weight associated to a point to consider this one as an outlier.
+ //! Threshold below which the weight associated to a point to consider this
+ //! one as an outlier.
double threshold_outlier;
- //! Percentage of good points, according to the initial number, that must have the tracker.
+ //! Percentage of good points, according to the initial number, that must
+ //! have the tracker.
double percentGood;
- //! The estimated displacement of the pose between the current instant and the initial position.
+ //! The estimated displacement of the pose between the current instant and
+ //! the initial position.
vpHomogeneousMatrix ctTc0;
//! Points tracker.
vpKltOpencv tracker;
//!
- std::list<vpMbtDistanceKltPoints*> kltPolygons;
+ std::list<vpMbtDistanceKltPoints *> kltPolygons;
//!
- std::list<vpMbtDistanceKltCylinder*> kltCylinders;
+ std::list<vpMbtDistanceKltCylinder *> kltCylinders;
//! Vector of the circles used here only to display the full model.
- std::list<vpMbtDistanceCircle*> circles_disp;
+ std::list<vpMbtDistanceCircle *> circles_disp;
+ //!
+ unsigned int m_nbInfos;
+ //!
+ unsigned int m_nbFaceUsed;
+ //! Interaction matrix
+ vpMatrix m_L_klt;
+ //! (s - s*)
+ vpColVector m_error_klt;
+ //! Robust weights
+ vpColVector m_w_klt;
+ //! Weighted error
+ vpColVector m_weightedError_klt;
+ //! Robust
+ vpRobust m_robust_klt;
public:
vpMbKltTracker();
virtual ~vpMbKltTracker();
-
+
/** @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<unsigned char>& I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam, const vpColor& col, const unsigned int thickness=1,
- const bool displayFullModel = false);
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col, const unsigned int thickness=1, const bool displayFullModel = false);
+ void addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name = "");
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
/*! Return the address of the circle feature list. */
- virtual std::list<vpMbtDistanceCircle*> &getFeaturesCircle() { return circles_disp; }
+ virtual std::list<vpMbtDistanceCircle *> &getFeaturesCircle() { return circles_disp; }
/*! Return the address of the cylinder feature list. */
- virtual std::list<vpMbtDistanceKltCylinder*> &getFeaturesKltCylinder() { return kltCylinders; }
+ virtual std::list<vpMbtDistanceKltCylinder *> &getFeaturesKltCylinder() { return kltCylinders; }
/*! Return the address of the Klt feature list. */
- virtual std::list<vpMbtDistanceKltPoints*> &getFeaturesKlt() { return kltPolygons; }
+ virtual std::list<vpMbtDistanceKltPoints *> &getFeaturesKlt() { return kltPolygons; }
- /*!
- Get the current list of KLT points.
-
- \return the list of KLT points through vpKltOpencv.
- */
+/*!
+ Get the current list of KLT points.
+
+ \return the list of KLT points through vpKltOpencv.
+ */
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- inline std::vector<cv::Point2f> getKltPoints() const {return tracker.getFeatures();}
+ inline std::vector<cv::Point2f> getKltPoints() const { return tracker.getFeatures(); }
#else
- inline CvPoint2D32f* getKltPoints() {return tracker.getFeatures();}
+ inline CvPoint2D32f *getKltPoints() { return tracker.getFeatures(); }
#endif
-
+
std::vector<vpImagePoint> getKltImagePoints() const;
std::map<int, vpImagePoint> getKltImagePointsWithId() const;
/*!
Get the klt tracker at the current state.
-
+
\return klt tracker.
*/
- inline vpKltOpencv getKltOpencv() const { return tracker; }
+ inline vpKltOpencv getKltOpencv() const { return tracker; }
/*!
- Get the value of the gain used to compute the control law.
-
- \return the value for the gain.
- */
- virtual inline double getLambda() const {return lambda;}
-
- /*!
Get the erosion of the mask used on the Model faces.
\return The erosion.
*/
- inline unsigned int getMaskBorder() const { return maskBorder; }
-
- /*!
- Get the maximum iteration of the virtual visual servoing stage.
-
- \return the number of iteration
- */
- virtual inline unsigned int getMaxIter() const {return maxIter;}
-
+ inline unsigned int getKltMaskBorder() const { return maskBorder; }
+
/*!
Get the current number of klt points.
-
+
\return the number of features
*/
- inline int getNbKltPoints() const {return tracker.getNbFeatures();}
+ inline int getKltNbPoints() const { return tracker.getNbFeatures(); }
/*!
Get the threshold for the acceptation of a point.
- \return threshold_outlier : Threshold for the weight below which a point is rejected.
+ \return threshold_outlier : Threshold for the weight below which a point
+ is rejected.
*/
- inline double getThresholdAcceptation() const { return threshold_outlier;}
-
- virtual void loadConfigFile(const std::string& configFile);
- void loadConfigFile(const char* configFile);
-
- virtual void reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name, const vpHomogeneousMatrix& cMo_,
- const bool verbose=false);
- void reInitModel(const vpImage<unsigned char>& I, const char* cad_name, const vpHomogeneousMatrix& cMo,
- const bool verbose=false);
- void resetTracker();
+ inline double getKltThresholdAcceptation() const { return threshold_outlier; }
- void setCameraParameters(const vpCameraParameters& cam);
+ virtual inline vpColVector getError() const { return m_error_klt; }
- virtual void setKltOpencv(const vpKltOpencv& t);
+ virtual inline vpColVector getRobustWeights() const { return m_w_klt; }
+
+ virtual void loadConfigFile(const std::string &configFile);
+ void loadConfigFile(const char *configFile);
+
+ virtual void reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose = false);
+ void reInitModel(const vpImage<unsigned char> &I, const char *cad_name, const vpHomogeneousMatrix &cMo,
+ const bool verbose = false);
+ void resetTracker();
+
+ void setCameraParameters(const vpCameraParameters &cam);
- /*!
- Set the value of the gain used to compute the control law.
-
- \param gain : the desired value for the gain.
- */
- virtual inline void setLambda(const double gain) {this->lambda = gain;}
-
/*!
Set the erosion of the mask used on the Model faces.
\param e : The desired erosion.
*/
- inline void setMaskBorder(const unsigned int &e)
+ inline void setKltMaskBorder(const unsigned int &e)
{
maskBorder = e;
- //if(useScanLine)
+ // if(useScanLine)
faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
}
-
+
+ virtual void setKltOpencv(const vpKltOpencv &t);
+
/*!
- Set the maximum iteration of the virtual visual servoing stage.
-
- \param max : the desired number of iteration
+ Set the threshold for the acceptation of a point.
+
+ \param th : Threshold for the weight below which a point is rejected.
*/
- virtual inline void setMaxIter(const unsigned int max) {maxIter = max;}
+ inline void setKltThresholdAcceptation(const double th) { threshold_outlier = th; }
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
- virtual void setOgreVisibilityTest(const bool &v){
+ virtual void setOgreVisibilityTest(const bool &v)
+ {
vpMbTracker::setOgreVisibilityTest(v);
#ifdef VISP_HAVE_OGRE
faces.getOgreContext()->setWindowName("MBT Klt");
@@ -398,66 +399,102 @@ public:
\param v : True to use it, False otherwise
*/
- virtual void setScanLineVisibilityTest(const bool &v){
+ virtual void setScanLineVisibilityTest(const bool &v)
+ {
vpMbTracker::setScanLineVisibilityTest(v);
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it)
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it)
(*it)->useScanLine = v;
}
-
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo);
+
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo);
/*!
Set if the projection error criteria has to be computed.
- \param flag : True if the projection error criteria has to be computed, false otherwise
+ \param flag : True if the projection error criteria has to be computed,
+ false otherwise
*/
- virtual void setProjectionErrorComputation(const bool &flag) {
- if(flag)
- vpCTRACE << "This option is not yet implemented in vpMbKltTracker, projection error computation set to false." << std::endl ; }
-
+ virtual void setProjectionErrorComputation(const bool &flag)
+ {
+ if (flag)
+ std::cerr << "This option is not yet implemented in vpMbKltTracker, "
+ "projection error computation set to false."
+ << std::endl;
+ }
+
+ void setUseKltTracking(const std::string &name, const bool &useKltTracking);
+
+ virtual void testTracking();
+ virtual void track(const vpImage<unsigned char> &I);
+
+ /*!
+ @name Deprecated functions
+ */
+ //@{
+
+ /*!
+ Get the erosion of the mask used on the Model faces.
+ \deprecated Use rather getkltMaskBorder()
+
+ \return The erosion.
+ */
+ /* 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(); }
+
+ /*!
+ Get the threshold for the acceptation of a point.
+ \deprecated Use rather getKltThresholdAcceptation()
+
+ \return threshold_outlier : Threshold for the weight below which a point
+ is rejected.
+ */
+ /* vp_deprecated */ inline double getThresholdAcceptation() const { return threshold_outlier; }
+ /*!
+ Set the erosion of the mask used on the Model faces.
+
+ \param e : The desired erosion.
+ */
+ /* vp_deprecated */ inline void setMaskBorder(const unsigned int &e)
+ {
+ maskBorder = e;
+ // if(useScanLine)
+ faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
+ }
+
/*!
Set the threshold for the acceptation of a point.
+ \deprecated Use rather setKltThresholdAcceptation()
\param th : Threshold for the weight below which a point is rejected.
*/
- inline void setThresholdAcceptation(const double th) {threshold_outlier = th;}
+ /* vp_deprecated */ inline void setThresholdAcceptation(const double th) { threshold_outlier = th; }
+
+ //@}
- void setUseKltTracking(const std::string &name, const bool &useKltTracking);
-
- virtual void testTracking();
- virtual void track(const vpImage<unsigned char>& I);
-
protected:
/** @name Protected Member Functions Inherited from vpMbKltTracker */
//@{
- void computeVVS(const unsigned int &nbInfos, vpColVector &w);
- void computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
- const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
- double &mu, bool &reStartFromLastIncrement);
- void computeVVSCovariance(const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev,
- const vpMatrix &L_true, const vpMatrix &LVJ_true);
- void computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H,
- std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
- const vpHomogeneousMatrix &ctTc0_);
- void computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
- const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
- vpColVector &R, vpMatrix <L, vpColVector <R, vpColVector &error_prev, vpColVector &v, double &mu,
- vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev);
- void computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R,
- vpColVector &w_true, vpColVector &w, vpRobust &robust);
-
- virtual void init(const vpImage<unsigned char>& I);
+ void computeVVS();
+ virtual void computeVVSInit();
+ virtual void computeVVSInteractionMatrixAndResidu();
+
+ virtual void init(const vpImage<unsigned char> &I);
virtual void initFaceFromCorners(vpMbtPolygon &polygon);
virtual void initFaceFromLines(vpMbtPolygon &polygon);
- virtual void initCircle(const vpPoint&, const vpPoint &, const vpPoint &, const double, const int,
- const std::string &name="");
- virtual void initCylinder(const vpPoint&, const vpPoint &, const double, const int,
- const std::string &name="");
-
- void preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed);
- bool postTracking(const vpImage<unsigned char>& I, vpColVector &w);
- virtual void reinit(const vpImage<unsigned char>& I);
+ virtual void initCircle(const vpPoint &, const vpPoint &, const vpPoint &, const double, const int,
+ const std::string &name = "");
+ virtual void initCylinder(const vpPoint &, const vpPoint &, const double, const int, const std::string &name = "");
+
+ void preTracking(const vpImage<unsigned char> &I);
+ bool postTracking(const vpImage<unsigned char> &I, vpColVector &w);
+ virtual void reinit(const vpImage<unsigned char> &I);
//@}
};
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h
index 5745755..add5519 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbScanLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Compute the visibility of 3D polygons already transformed in the camera frame
+ * Compute the visibility of 3D polygons already transformed in the camera
+ *frame
*
* Authors:
* Aurelien Yol
@@ -38,19 +40,19 @@
#ifndef vpMbScanLine_HH
#define vpMbScanLine_HH
-#include <vector>
-#include <list>
#include <deque>
-#include <set>
+#include <limits> // numeric_limits
+#include <list>
#include <map>
-#include <limits> // numeric_limits
+#include <set>
+#include <vector>
-#include <visp3/core/vpColVector.h>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpPoint.h>
//#define DEBUG_DISP // Uncomment to get visibility debug display
@@ -69,24 +71,21 @@
class VISP_EXPORT vpMbScanLine
{
public:
- //! Structure to define if a scanline intersection is a starting or an ending point of a polygon, or just a single line intersection.
- typedef enum
- {
- START = 1,
- END = 0,
- POINT = 2
- } vpMbScanLineType ;
-
- //! Structure to define a scanline edge (basically a pair of (X,Y,Z) vectors).
+ //! Structure to define if a scanline intersection is a starting or an
+ //! ending point of a polygon, or just a single line intersection.
+ typedef enum { START = 1, END = 0, POINT = 2 } vpMbScanLineType;
+
+ //! Structure to define a scanline edge (basically a pair of (X,Y,Z)
+ //! vectors).
typedef std::pair<vpColVector, vpColVector> vpMbScanLineEdge;
//! Structure to define a scanline intersection.
- struct vpMbScanLineSegment
- {
- vpMbScanLineSegment() : type(START), edge(), p(0), P1(0), P2(0), Z1(0), Z2(0), ID(0), b_sample_Y(false) {};
+ struct vpMbScanLineSegment {
+ vpMbScanLineSegment() : type(START), edge(), p(0), P1(0), P2(0), Z1(0), Z2(0), ID(0), b_sample_Y(false) {}
vpMbScanLineType type;
vpMbScanLineEdge edge;
- double p; // This value can be either x or y-coordinate value depending if the structure is used in X or Y-axis scanlines computation.
+ double 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;
@@ -94,119 +93,109 @@ public:
};
//! vpMbScanLineEdge Comparator.
- struct vpMbScanLineEdgeComparator
- {
+ struct vpMbScanLineEdgeComparator {
inline bool operator()(const vpMbScanLineEdge &l0, const vpMbScanLineEdge &l1) const
{
- for(unsigned int i = 0 ; i < 3 ; ++i)
+ for (unsigned int i = 0; i < 3; ++i)
if (l0.first[i] < l1.first[i])
return true;
- else if(l0.first[i] > l1.first[i])
+ else if (l0.first[i] > l1.first[i])
return false;
- for(unsigned int i = 0 ; i < 3 ; ++i)
+ for (unsigned int i = 0; i < 3; ++i)
if (l0.second[i] < l1.second[i])
return true;
- else if(l0.second[i] > l1.second[i])
+ else if (l0.second[i] > l1.second[i])
return false;
return false;
}
};
//! vpMbScanLineSegment Comparators.
- struct vpMbScanLineSegmentComparator
- {
+ struct vpMbScanLineSegmentComparator {
inline bool operator()(const vpMbScanLineSegment &a, const vpMbScanLineSegment &b) const
{
- //return a.p == b.p ? a.type < b.type : a.p < b.p;
+ // return a.p == b.p ? a.type < b.type : a.p < b.p;
return (std::fabs(a.p - b.p) <= std::numeric_limits<double>::epsilon()) ? a.type < b.type : a.p < b.p;
}
- inline bool operator()(const std::pair<double,vpMbScanLineSegment> &a, const std::pair<double, vpMbScanLineSegment> &b) const
+ inline bool operator()(const std::pair<double, vpMbScanLineSegment> &a,
+ const std::pair<double, vpMbScanLineSegment> &b) const
{
return a.first < b.first;
}
};
private:
- unsigned int w, h;
- vpCameraParameters K;
- unsigned int maskBorder;
- vpImage<unsigned char> mask;
- vpImage<int> primitive_ids;
+ unsigned int w, h;
+ vpCameraParameters K;
+ unsigned int maskBorder;
+ vpImage<unsigned char> mask;
+ vpImage<int> primitive_ids;
std::map<vpMbScanLineEdge, std::set<int>, vpMbScanLineEdgeComparator> visibility_samples;
- double depthTreshold;
+ double depthTreshold;
public:
#if defined(DEBUG_DISP)
vpDisplay *dispMaskDebug;
vpDisplay *dispLineDebug;
- vpImage<unsigned char> linedebugImg;
+ vpImage<unsigned char> linedebugImg;
#endif
vpMbScanLine();
~vpMbScanLine();
- void drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
- std::vector<int> listPolyIndices,
- const vpCameraParameters &K, unsigned int w, unsigned int h);
+ void drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> &polygons,
+ std::vector<int> listPolyIndices, const vpCameraParameters &K, unsigned int w, unsigned int h);
/*!
If there is one polygon behind another,
- this threshold defines the minimum distance between both polygons to still consider the one behind as visible.
+ this threshold defines the minimum distance between both polygons to still
+ consider the one behind as visible.
\return Current Threshold.
*/
- double getDepthTreshold() { return depthTreshold; }
- unsigned int getMaskBorder() { return maskBorder; }
- const vpImage<unsigned char>& getMask() const { return mask; }
- const vpImage<int>& getPrimitiveIDs() const { return primitive_ids; }
+ double getDepthTreshold() { return depthTreshold; }
+ unsigned int getMaskBorder() { return maskBorder; }
+ const vpImage<unsigned char> &getMask() const { return mask; }
+ const vpImage<int> &getPrimitiveIDs() const { return primitive_ids; }
- void queryLineVisibility(const vpPoint &a, const vpPoint &b,
- std::vector<std::pair<vpPoint, vpPoint> > &lines,
- const bool &displayResults = false);
+ void queryLineVisibility(const vpPoint &a, const vpPoint &b, std::vector<std::pair<vpPoint, vpPoint> > &lines,
+ const bool &displayResults = false);
/*!
If there is one polygon behind another,
- this threshold defines the minimum distance between both polygons to still consider the one behind as visible.
+ this threshold defines the minimum distance between both polygons to still
+ consider the one behind as visible.
\param treshold : New Threshold.
*/
- void setDepthTreshold(const double &treshold) { depthTreshold = treshold; }
- void setMaskBorder(const unsigned int &mb){ maskBorder = mb; }
-
+ void setDepthTreshold(const double &treshold) { depthTreshold = treshold; }
+ void setMaskBorder(const unsigned int &mb) { maskBorder = mb; }
private:
void createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
const unsigned int &size);
- void drawLineY(const vpColVector &a,
- const vpColVector &b,
- const vpMbScanLineEdge &line_ID,
- const int ID,
+ void drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID,
std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
- void drawLineX(const vpColVector &a,
- const vpColVector &b,
- const vpMbScanLineEdge &line_ID,
- const int ID,
+ void drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &line_ID, const int ID,
std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
- void drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
- const int ID,
+ void drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
- void drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
- const int ID,
+ void drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
std::vector<std::vector<vpMbScanLineSegment> > &scanlines);
// Static functions
static vpMbScanLineEdge makeMbScanLineEdge(const vpPoint &a, const vpPoint &b);
- static void createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K);
- static double getAlpha(double x, double X0, double Z0, double X1, double Z1);
- static double mix(double a, double b, double alpha);
- static vpPoint mix(const vpPoint &a, const vpPoint &b, double alpha);
- static double norm(const vpPoint &a, const vpPoint &b);
+ static void createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCameraParameters &K);
+ static double getAlpha(double x, double X0, double Z0, double X1, double Z1);
+ static double mix(double a, double b, double alpha);
+ static vpPoint mix(const vpPoint &a, const vpPoint &b, double alpha);
+ static double norm(const vpPoint &a, const vpPoint &b);
};
#endif // doxygen should skip this
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h
index 7107d4d..090c4d3 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,8 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Generic model based tracker. This class declares the methods to implement in
- * order to have a model based tracker.
+ * Generic model based tracker. This class declares the methods to implement
+ *in order to have a model based tracker.
*
* Authors:
* Romain Tallonneau
@@ -39,50 +40,51 @@
/*!
\file vpMbTracker.h
- \brief Generic model based tracker.
+ \brief Generic model based tracker.
*/
#ifndef vpMbTracker_hh
#define vpMbTracker_hh
-#include <vector>
-#include <string>
-#include <sstream>
-#include <fstream>
-#include <map>
#include <algorithm>
#include <cctype>
+#include <fstream>
+#include <functional> // std::not1
#include <locale>
-#include <functional> // std::not1
-
+#include <map>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpColVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpRGBa.h>
-#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/mbt/vpMbtPolygon.h>
-#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/core/vpRGBa.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
+#include <visp3/mbt/vpMbtPolygon.h>
#ifdef VISP_HAVE_COIN3D
-//Work around to avoid type redefinition int8_t with Coin
-// #if defined(_WIN32) && defined(VISP_HAVE_OGRE) && (_MSC_VER >= 1600) // Visual Studio 2010
+// Work around to avoid type redefinition int8_t with Coin
+// #if defined(_WIN32) && defined(VISP_HAVE_OGRE) && (_MSC_VER >= 1600) //
+// Visual Studio 2010
// #define HAVE_INT8_T 1
// #endif
-//Inventor includes
-# include <Inventor/VRMLnodes/SoVRMLGroup.h>
-# include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
-# include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
+// Inventor includes
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
+#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
#endif
/*!
\class vpMbTracker
\ingroup group_mbt_trackers
- \brief Main methods for a model-based tracker.
+ \brief Main methods for a model-based tracker.
This class provides the main methods for a model based tracker. This pure
virtual class must be used in inheritance for a tracker that compute the
@@ -91,22 +93,20 @@
This class intends to define a common basis for object tracking. This is
realised by implementing the main functions:
- - init() : Initialisation of the tracker (it includes re-initialisation). This
- method is called at the end of the initClick() method.
- - initFaceFromCorners() : Initialisation of the lines that has to be tracked.
+ - init() : Initialisation of the tracker (it includes re-initialisation).
+ This method is called at the end of the initClick() method.
+ - initFaceFromCorners() : Initialisation of the lines that has to be
+ tracked.
- track() : Tracking on the current image
- - testTracking() : Test the tracking. This method throws exception if the
- tracking failed.
+ - testTracking() : Test the tracking. This method throws exception if the
+ tracking failed.
- display() : Display the model and eventually other information.
*/
class VISP_EXPORT vpMbTracker
{
public:
- typedef enum {
- GAUSS_NEWTON_OPT = 0,
- LEVENBERG_MARQUARDT_OPT = 1
- } vpMbtOptimizationMethod;
+ typedef enum { GAUSS_NEWTON_OPT = 0, LEVENBERG_MARQUARDT_OPT = 1 } vpMbtOptimizationMethod;
protected:
//! The camera parameters.
@@ -117,26 +117,27 @@ protected:
vpMatrix oJo;
//! Boolean to know if oJo is identity (for fast computation)
bool isoJoIdentity;
- //! The name of the file containing the model (it is used to create a file name.0.pos used to store the compute pose in the initClick method).
+ //! The name of the file containing the model (it is used to create a file
+ //! name.0.pos used to store the compute pose in the initClick method).
std::string modelFileName;
- //! Flag used to ensure that the CAD model is loaded before the initialisation.
- bool modelInitialised;
- //! Filename used to save the initial pose computed using the initClick() method. It is also used to read a previous pose in the same method.
+ //! Flag used to ensure that the CAD model is loaded before the
+ //! initialisation.
+ bool modelInitialised;
+ //! Filename used to save the initial pose computed using the initClick()
+ //! method. It is also used to read a previous pose in the same method.
std::string poseSavingFilename;
//! Flag used to specify if the covariance matrix has to be computed or not.
bool computeCovariance;
//! Covariance matrix
vpMatrix covarianceMatrix;
- //! Flag used to specify if the gradient error criteria has to be computed or not.
+ //! Flag used to specify if the gradient error criteria has to be computed
+ //! or not.
bool computeProjError;
- //! Error angle between the gradient direction of the model features projected at the resulting pose and their normal.
+ //! Error angle between the gradient direction of the model features
+ //! projected at the resulting pose and their normal.
double projectionError;
- //! If true, the features are displayed.
+ //! If true, the features are displayed.
bool displayFeatures;
- //! Weights used in the robust scheme
- vpColVector m_w;
- //! Error s-s*
- vpColVector m_error;
//! Optimization method used
vpMbtOptimizationMethod m_optimizationMethod;
@@ -171,95 +172,145 @@ protected:
unsigned int nbCircles;
//! True if LOD mode is enabled
bool useLodGeneral;
- //! True if the CAO model is loaded before the call to loadConfigFile, (deduced by the number of polygons)
+ //! True if the CAO model is loaded before the call to loadConfigFile,
+ //! (deduced by the number of polygons)
bool applyLodSettingInConfig;
//! Minimum line length threshold for LOD mode (general setting)
double minLineLengthThresholdGeneral;
//! Minimum polygon area threshold for LOD mode (general setting)
double minPolygonAreaThresholdGeneral;
- //! Map with [map.first]=parameter_names and [map.second]=type (string, number or boolean)
+ //! Map with [map.first]=parameter_names and [map.second]=type (string,
+ //! number or boolean)
std::map<std::string, std::string> mapOfParameterNames;
+ //! If true, compute the interaction matrix at each iteration of the
+ //! minimization. Otherwise, compute it only on the first iteration
+ bool m_computeInteraction;
+ //! Gain of the virtual visual servoing stage
+ double m_lambda;
+ //! Maximum number of iterations of the virtual visual servoing stage
+ unsigned int m_maxIter;
+ //! Epsilon threshold to stop the VVS optimization loop
+ double m_stopCriteriaEpsilon;
+ //! Initial Mu for Levenberg Marquardt optimization loop
+ double m_initialMu;
public:
vpMbTracker();
virtual ~vpMbTracker();
-
+
/** @name Inherited functionalities from vpMbTracker */
//@{
/*! Return the angle used to test polygons appearance. */
- virtual inline double getAngleAppear() const { return angleAppears; }
+ virtual inline double getAngleAppear() const { return angleAppears; }
/*! Return the angle used to test polygons disappearance. */
- virtual inline double getAngleDisappear() const { return angleDisappears; }
+ virtual inline double getAngleDisappear() const { return angleDisappears; }
/*!
Get the camera parameters.
\param camera : copy of the camera parameters used by the tracker.
*/
- virtual void getCameraParameters(vpCameraParameters& camera) const { camera = this->cam;}
-
+ virtual void getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; }
+
/*!
- Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType.
+ Get the clipping used and defined in
+ vpPolygon3D::vpMbtPolygonClippingType.
\return Clipping flags.
*/
- virtual inline unsigned int getClipping() const { return clippingFlag; }
+ virtual inline unsigned int getClipping() const { return clippingFlag; }
/*!
- Get the covariance matrix.
+ Get the covariance matrix. This matrix is only computed if
+ setCovarianceComputation() is turned on.
+
+ \sa setCovarianceComputation()
*/
- virtual vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
+
+ virtual vpMatrix getCovarianceMatrix() const
+ {
+ if (!computeCovariance) {
+ // vpTRACE("Warning : The covariance matrix has not been computed.
+ // See setCovarianceComputation() to do it.");
+ std::cerr << "Warning : The covariance matrix has not been computed. "
+ "See setCovarianceComputation() to do it."
+ << std::endl;
+ }
+
+ return covarianceMatrix;
}
/*!
- Get the error angle between the gradient direction of the model features projected at the resulting pose and their normal.
- The error is expressed in degree between 0 and 90.
+ Get the initial value of mu used in the Levenberg Marquardt optimization
+ loop.
+
+ \return the initial mu value.
+ */
+ virtual inline double getInitialMu() const { return m_initialMu; }
+
+ /*!
+ Get the value of the gain used to compute the control law.
+
+ \return the value for the gain.
+ */
+ virtual inline double getLambda() const { return m_lambda; }
+
+ /*!
+ Get the maximum number of iterations of the virtual visual servoing stage.
+
+ \return the number of iteration
+ */
+ virtual inline unsigned int getMaxIter() const { return m_maxIter; }
+
+ /*!
+ Get the error angle between the gradient direction of the model features
+ projected at the resulting pose and their normal. The error is expressed
+ in degree between 0 and 90. This value is computed if
+ setProjectionErrorComputation() is turned on.
\return the value for the error.
+
+ \sa setProjectionErrorComputation()
*/
virtual double getProjectionError() const { return projectionError; }
virtual vpColVector getEstimatedDoF() const;
/*!
- Return the error vector \f$(s-s^*)\f$ reached after the virtual visual servoing process used to estimate the pose.
+ Return the error vector \f$(s-s^*)\f$ reached after the virtual visual
+ servoing process used to estimate the pose.
- The following example shows how to use this function to compute the norm of the residual and the norm of the residual
- normalized by the number of features that are tracked:
+ 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;
+ tracker.track(I); std::cout << "Residual: " << sqrt( (tracker.getError()).sumSquare()) << std::endl;
+ std::cout << "Residual normalized: "
+ << sqrt( (tracker.getError()).sumSquare())/tracker.getError().size() << std::endl;
\endcode
\sa getRobustWeights()
*/
- virtual vpColVector getError() const {
- return m_error;
- }
+ virtual vpColVector getError() const = 0;
/*! Return a reference to the faces structure. */
- virtual inline vpMbHiddenFaces<vpMbtPolygon>& getFaces() { return faces;}
+ virtual inline vpMbHiddenFaces<vpMbtPolygon> &getFaces() { return faces; }
/*!
Get the far distance for clipping.
\return Far clipping value.
*/
- virtual inline double getFarClippingDistance() const { return distFarClip; }
+ virtual inline double getFarClippingDistance() const { return distFarClip; }
/*!
Return the weights vector \f$w_i\f$ computed by the robust scheme.
- The following example shows how to use this function to compute the norm of the weighted residual
- and the norm of the weighted residual normalized by the sum of the weights associated to the
- features that are tracked:
+ The following example shows how to use this function to compute the norm
+ of the weighted residual and the norm of the weighted residual normalized
+ by the sum of the weights associated to the features that are tracked:
\code
tracker.track(I);
vpColVector w = tracker.getRobustWeights();
@@ -274,25 +325,21 @@ public:
\sa getError()
*/
- virtual vpColVector getRobustWeights() const {
- return m_w;
- }
+ virtual vpColVector getRobustWeights() const = 0;
/*!
Get the number of polygons (faces) representing the object to track.
\return Number of polygons.
*/
- virtual inline unsigned int getNbPolygon() const {
- return static_cast<unsigned int>(faces.size());
- }
+ virtual inline unsigned int getNbPolygon() const { return static_cast<unsigned int>(faces.size()); }
/*!
Get the near distance for clipping.
\return Near clipping value.
*/
- virtual inline double getNearClippingDistance() const { return distNearClip; }
+ virtual inline double getNearClippingDistance() const { return distNearClip; }
/*!
Get the optimization method used during the tracking.
@@ -312,52 +359,56 @@ public:
\param index : Index of the polygon to return.
\return Pointer to the polygon index.
*/
- virtual inline vpMbtPolygon* getPolygon(const unsigned int index) {
- if(index >= static_cast<unsigned int>(faces.size()) ){
+ virtual inline vpMbtPolygon *getPolygon(const unsigned int index)
+ {
+ if (index >= static_cast<unsigned int>(faces.size())) {
throw vpException(vpException::dimensionError, "index out of range");
}
return faces[index];
}
- virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > getPolygonFaces(const bool orderPolygons=true,
- const bool useVisibility=true, const bool clipPolygon=false);
+ virtual std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
+ getPolygonFaces(const bool orderPolygons = true, const bool useVisibility = true, const bool clipPolygon = false);
/*!
Get the current pose between the object and the camera.
- cMo is the matrix which can be used to express
+ cMo is the matrix which can be used to express
coordinates from the object frame to camera frame.
\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.
- cMo is the matrix which can be used to express
+ cMo is the matrix which can be used to express
coordinates from the object frame to camera frame.
\return the current pose
*/
- virtual inline vpHomogeneousMatrix getPose() const {return this->cMo;}
+ virtual inline vpHomogeneousMatrix getPose() const { return this->cMo; }
+
+ virtual inline double getStopCriteriaEpsilon() const { return m_stopCriteriaEpsilon; }
- // Intializer
+// Intializer
#ifdef VISP_HAVE_MODULE_GUI
- virtual void initClick( const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp = false );
- virtual void initClick( const vpImage<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile = "" );
+ virtual void initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp = false);
+ virtual void initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile = "");
#endif
- virtual void initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile );
- virtual void initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list, const std::vector<vpPoint> &points3D_list );
+ virtual void initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile);
+ virtual void initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
+ const std::vector<vpPoint> &points3D_list);
- virtual void initFromPose(const vpImage<unsigned char>& I, const std::string &initFile);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo);
- virtual void initFromPose(const vpImage<unsigned char>& I, const vpPoseVector &cPo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const std::string &initFile);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ virtual void initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo);
- 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 char *modelFile, const bool verbose = false);
+ virtual void loadModel(const std::string &modelFile, const bool verbose = false);
/*!
Set the angle used to test polygons appearance.
@@ -380,45 +431,74 @@ public:
\param a : new angle in radian.
*/
virtual inline void setAngleDisappear(const double &a) { angleDisappears = a; }
-
+
/*!
Set the camera parameters.
\param camera : the new camera parameters
*/
- virtual void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
+ virtual void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; }
virtual void setClipping(const unsigned int &flags);
-
+
/*!
- Set if the covaraince matrix has to be computed.
+ Set if the covariance matrix has to be computed.
- \param flag : True if the covariance has to be computed, false otherwise
+ \param flag : True if the covariance has to be computed, false otherwise.
+ If computed its value is available with getCovarianceMatrix()
+
+ \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 (ME) and the klt points if used.
+ Enable to display the features. By features, we meant the moving edges
+ (ME) and the klt points if used.
- Note that if present, the moving edges can be displayed with different colors:
+ Note that if present, the moving edges can be displayed with different
+ colors:
- If green : The ME is a good point.
- - If blue : The ME is removed because of a contrast problem during the tracking phase.
- - If purple : The ME is removed because of a threshold problem during the tracking phase.
- - If red : The ME is removed because it is rejected by the robust approach in the virtual visual servoing scheme.
+ - If blue : The ME is removed because of a contrast problem during the
+ tracking phase.
+ - If purple : The ME is removed because of a threshold problem during the
+ tracking phase.
+ - If red : The ME is removed because it is rejected by the robust approach
+ in the virtual visual servoing scheme.
\param displayF : set it to true to display the features.
*/
- virtual void setDisplayFeatures(const bool displayF) {displayFeatures = displayF;}
+ virtual void setDisplayFeatures(const bool displayF) { displayFeatures = displayF; }
- virtual void setEstimatedDoF(const vpColVector& v);
+ virtual void setEstimatedDoF(const vpColVector &v);
virtual void setFarClippingDistance(const double &dist);
- virtual void setLod(const bool useLod, const std::string &name="");
+ /*!
+ Set the initial value of mu for the Levenberg Marquardt optimization loop.
+
+ \param mu : initial mu.
+ */
+ virtual inline void setInitialMu(const double mu) { m_initialMu = mu; }
+
+ /*!
+ Set the value of the gain used to compute the control law.
- virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name="");
+ \param gain : the desired value for the gain.
+ */
+ virtual inline void setLambda(const double gain) { m_lambda = gain; }
- virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name="");
+ virtual void setLod(const bool useLod, const std::string &name = "");
+
+ /*!
+ Set the maximum iteration of the virtual visual servoing stage.
+
+ \param max : the desired number of iteration
+ */
+ virtual inline void setMaxIter(const unsigned int max) { m_maxIter = max; }
+
+ virtual void setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name = "");
+
+ virtual void setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name = "");
virtual void setNearClippingDistance(const double &dist);
@@ -428,40 +508,56 @@ public:
\param opt : Optimization method to use.
*/
virtual inline void setOptimizationMethod(const vpMbtOptimizationMethod &opt) { m_optimizationMethod = opt; }
-
+
+ /*!
+ Set the minimal error (previous / current estimation) to determine if
+ there is convergence or not.
+
+ \param eps : Epsilon threshold.
+ */
+ virtual inline void setStopCriteriaEpsilon(const double eps) { m_stopCriteriaEpsilon = eps; }
+
/*!
- Set if the projection error criteria has to be computed.
+ Set if the projection error criteria has to be computed. This criteria
+ could be used to detect the quality of the tracking. It computes an angle
+ between 0 and 90 degrees that is available with getProjectionError().
+ Closer to 0 is the value, better is the tracking.
- \param flag : True if the projection error criteria has to be computed, false otherwise
+ \param flag : True if the projection error criteria has to be computed,
+ false otherwise.
+
+ \sa getProjectionError()
*/
virtual void setProjectionErrorComputation(const bool &flag) { computeProjError = flag; }
- virtual void setScanLineVisibilityTest(const bool &v){ useScanLine = v; }
+ virtual void setScanLineVisibilityTest(const bool &v) { useScanLine = v; }
virtual void setOgreVisibilityTest(const bool &v);
-
+
void savePose(const std::string &filename) const;
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider
+ a polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \param ratio : Ratio of succesful attempts that has to be considered.
+ Value has to be between 0.0 (0%) and 1.0 (100%).
*/
- void setGoodNbRayCastingAttemptsRatio(const double &ratio) {
- faces.setGoodNbRayCastingAttemptsRatio(ratio);
- }
+ 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 random point inside the considered polygon.
+ Set the number of rays that will be sent toward each polygon for
+ visibility test. Each ray will go from the optic center of the camera to a
+ random point inside the considered polygon.
\sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
\param attempts Number of rays to be sent.
*/
- void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) {
+ virtual void setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
+ {
faces.setNbRayCastingAttemptsForVisibility(attempts);
}
#endif
@@ -469,29 +565,26 @@ public:
/*!
Enable/Disable the appearance of Ogre config dialog on startup.
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
*/
- inline void setOgreShowConfigDialog(const bool showConfigDialog){
- ogreShowConfigDialog = showConfigDialog;
- }
+ inline virtual void setOgreShowConfigDialog(const bool showConfigDialog) { ogreShowConfigDialog = showConfigDialog; }
/*!
Set the filename used to save the initial pose computed using the
- initClick() method. It is also used to read a previous pose in the same method.
- If the file is not set then, the initClick() method will create a .0.pos
- file in the root directory. This directory is the path to the file given to
- the method initClick() used to know the coordinates in the object frame.
+ initClick() method. It is also used to read a previous pose in the same
+ method. If the file is not set then, the initClick() method will create a
+ .0.pos file in the root directory. This directory is the path to the file
+ given to the method initClick() used to know the coordinates in the object
+ frame.
\param filename : The new filename.
*/
- inline void setPoseSavingFilename(const std::string& filename){
- poseSavingFilename = filename;
- }
+ inline void setPoseSavingFilename(const std::string &filename) { poseSavingFilename = filename; }
/* PURE VIRTUAL METHODS */
@@ -504,10 +597,11 @@ public:
\param cam : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+ \param displayFullModel : If true, the full model is displayed (even the
+ non visible surfaces).
*/
- virtual void display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
+ virtual void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false) = 0;
/*!
Display the 3D model at a given position using the given camera parameters
on a color (RGBa) image.
@@ -517,17 +611,18 @@ public:
\param cam : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+ \param displayFullModel : If true, the full model is displayed (even the
+ non visible surfaces).
*/
- virtual void display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- const vpColor& col , const unsigned int thickness=1, const bool displayFullModel = false)=0;
+ virtual void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false) = 0;
/*!
Initialise the tracking.
\param I : Input image.
*/
- virtual void init(const vpImage<unsigned char>& I)=0;
+ virtual void init(const vpImage<unsigned char> &I) = 0;
/*!
Load a config file to parameterise the behavior of the tracker.
@@ -536,7 +631,7 @@ public:
\param configFile : An xml config file to parse.
*/
- virtual void loadConfigFile(const std::string& configFile)=0;
+ virtual void loadConfigFile(const std::string &configFile) = 0;
/*!
Reset the tracker.
@@ -547,12 +642,13 @@ public:
Set the pose to be used in entry of the next call to the track() function.
This pose will be just used once.
- \warning This function has to be called after the initialisation of the tracker.
+ \warning This function has to be called after the initialisation of the
+ tracker.
\param I : image corresponding to the desired pose.
\param cdMo : Pose to affect.
*/
- virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo) = 0;
+ virtual void setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo) = 0;
/*!
Test the quality of the tracking.
@@ -566,51 +662,73 @@ public:
\param I : The current image.
*/
- virtual void track(const vpImage<unsigned char>& I)=0;
+ virtual void track(const vpImage<unsigned char> &I) = 0;
//@}
protected:
/** @name Protected Member Functions Inherited from vpMbTracker */
//@{
- void addPolygon(const std::vector<vpPoint>& corners, const int idFace=-1, const std::string &polygonName="",
- const bool useLod=false, const double minPolygonAreaThreshold=2500.0, const double minLineLengthThreshold=50.0);
- void addPolygon(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace=-1,
- const std::string &polygonName="", const bool useLod=false, const double minPolygonAreaThreshold=2500.0);
- void addPolygon(const vpPoint& p1, const vpPoint &p2, const int idFace=-1, const std::string &polygonName="",
- const bool useLod=false, const double minLineLengthThreshold=50);
- void addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace=-1, const std::string &polygonName="",
- const bool useLod=false, const double minLineLengthThreshold=50);
-
- void createCylinderBBox(const vpPoint& p1, const vpPoint &p2, const double &radius, std::vector<std::vector<vpPoint> > &listFaces);
-
- void computeJTR(const vpMatrix& J, const vpColVector& R, vpColVector& JTR) const;
-
+ void addPolygon(const std::vector<vpPoint> &corners, const int idFace = -1, const std::string &polygonName = "",
+ const bool useLod = false, const double minPolygonAreaThreshold = 2500.0,
+ const double minLineLengthThreshold = 50.0);
+ void addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius, const int idFace = -1,
+ const std::string &polygonName = "", const bool useLod = false,
+ const double minPolygonAreaThreshold = 2500.0);
+ void addPolygon(const vpPoint &p1, const vpPoint &p2, const int idFace = -1, const std::string &polygonName = "",
+ const bool useLod = false, const double minLineLengthThreshold = 50);
+ void addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace = -1,
+ const std::string &polygonName = "", const bool useLod = false,
+ const double minLineLengthThreshold = 50);
+
+ void createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
+ std::vector<std::vector<vpPoint> > &listFaces);
+
+ 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);
+
#ifdef VISP_HAVE_COIN3D
virtual void extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace);
- virtual void extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="");
- virtual void extractLines(SoVRMLIndexedLineSet* line_set, int &idFace, const std::string &polygonName="");
- virtual void extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName="");
+ virtual void extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
+ const std::string &polygonName = "");
+ virtual void extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName = "");
+ virtual void extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
+ const std::string &polygonName = "");
#endif
-
- vpPoint getGravityCenter(const std::vector<vpPoint>& _pts) const;
-
+
+ vpPoint getGravityCenter(const std::vector<vpPoint> &_pts) const;
+
/*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
+ Add a circle to track from its center, 3 points (including the center)
+ defining the plane that contain the circle and its radius.
\param p1 : Center of the circle.
\param p2 : A point on the plane containing the circle.
- \param p3 : An other point on the plane containing the circle. With the center of the circle \e p1,
- \e p2 and \e p3 we have 3 points defining the plane that contains the circle.
- \param radius : Radius of the circle.
+ \param p3 : An other point on the plane containing the circle. With the
+ center of the circle \e p1, \e p2 and \e p3 we have 3 points defining the
+ plane that contains the circle. \param radius : Radius of the circle.
\param idFace : Id of the face associated to the circle.
\param name : Name of the circle.
*/
- virtual void initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace=0, const std::string &name="")=0;
+ 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.
+ Add a cylinder to track from two points on the axis (defining the length
+ of the cylinder) and its radius.
\param p1 : First point on the axis.
\param p2 : Second point on the axis.
@@ -618,56 +736,57 @@ protected:
\param idFace : Id of the face associated to the cylinder.
\param name : Name of the cylinder.
*/
- virtual void initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace=0,
- const std::string &name="")=0;
+ virtual void initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace = 0,
+ const std::string &name = "") = 0;
/*!
- Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
+ Add the lines to track from the polygon description. If the polygon has
+ only two points, it defines a single line that is always visible. If it
+ has three or more corners, it defines a face. In that case the visibility
+ of the face is computed in order to track the corresponding lines only if
+ the face is visible.
The id of the polygon is supposed to be set prior calling this function.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
- virtual void initFaceFromCorners(vpMbtPolygon &polygon)=0;
- virtual void initFaceFromLines(vpMbtPolygon &polygon)=0;
+ virtual void initFaceFromCorners(vpMbtPolygon &polygon) = 0;
+ virtual void initFaceFromLines(vpMbtPolygon &polygon) = 0;
- virtual void loadVRMLModel(const std::string& modelFile);
- virtual void loadCAOModel(const std::string& modelFile, std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
- const bool verbose=false, const bool parent=true);
+ virtual void loadVRMLModel(const std::string &modelFile);
+ virtual void loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
+ int &startIdFace, const bool verbose = false, const bool parent = true);
- void removeComment(std::ifstream& fileId);
+ void removeComment(std::ifstream &fileId);
- inline bool parseBoolean(std::string &input) {
+ inline bool parseBoolean(std::string &input)
+ {
std::transform(input.begin(), input.end(), input.begin(), ::tolower);
std::istringstream is(input);
bool b;
- //Parse string to boolean either in the textual representation (True/False)
- //or in numeric representation (1/0)
+ // Parse string to boolean either in the textual representation
+ // (True/False) or in numeric representation (1/0)
is >> (input.size() > 1 ? std::boolalpha : std::noboolalpha) >> b;
return b;
}
- std::map<std::string, std::string> parseParameters(std::string& endLine);
+ std::map<std::string, std::string> parseParameters(std::string &endLine);
- inline std::string <rim(std::string &s) const {
+ inline std::string <rim(std::string &s) const
+ {
s.erase(s.begin(), std::find_if(s.begin(), s.end(), std::not1(std::ptr_fun<int, int>(std::isspace))));
return s;
}
- inline std::string &rtrim(std::string &s) const {
+ inline std::string &rtrim(std::string &s) const
+ {
s.erase(std::find_if(s.rbegin(), s.rend(), std::not1(std::ptr_fun<int, int>(std::isspace))).base(), s.end());
return s;
}
- inline std::string &trim(std::string &s) const {
- return ltrim(rtrim(s));
- }
+ inline std::string &trim(std::string &s) const { return ltrim(rtrim(s)); }
//@}
};
-
#endif
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h
index fc79185..81c5538 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbXmlParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,8 +38,9 @@
/*!
* \file vpMbXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ * object.
+ */
#ifndef vpMbXmlParser_HH
#define vpMbXmlParser_HH
@@ -47,20 +49,20 @@
#ifdef VISP_HAVE_XML2
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-#include <visp3/core/vpXmlParser.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpXmlParser.h>
/*!
\class vpMbXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup group_mbt_xml_parser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ object. \ingroup group_mbt_xml_parser
Data parser for the model based tracker.
*/
-class VISP_EXPORT vpMbXmlParser: public vpXmlParser
+class VISP_EXPORT vpMbXmlParser : public vpXmlParser
{
protected:
//! Camera parameters.
@@ -85,8 +87,8 @@ protected:
double minLineLengthThreshold;
//! Minimum polygon area to track a face when LOD is enabled
double minPolygonAreaThreshold;
-
- typedef enum{
+
+ typedef enum {
conf,
face,
angle_appear,
@@ -108,143 +110,131 @@ protected:
last
} dataToParseMb;
-
public:
-
/** @name Public Member Functions Inherited from vpMbXmlParser */
//@{
- vpMbXmlParser();
- virtual ~vpMbXmlParser();
+ vpMbXmlParser();
+ virtual ~vpMbXmlParser();
/*!
Get the angle to determine if a face appeared.
\return angleAppear
*/
- inline double getAngleAppear() const {return angleAppear;}
-
+ inline double getAngleAppear() const { return angleAppear; }
+
/*!
Get the angle to determine if a face disappeared.
\return angleDisappear
*/
- inline double getAngleDisappear() const {return angleDisappear;}
-
- void getCameraParameters(vpCameraParameters& _cam) const { _cam = this->cam;}
-
+ inline double getAngleDisappear() const { return angleDisappear; }
+
+ void getCameraParameters(vpCameraParameters &_cam) const { _cam = this->cam; }
+
/*!
Get the far clipping distance.
\return farClipping
*/
- inline double getFarClippingDistance() const {return farClipping;}
-
+ inline double getFarClippingDistance() const { return farClipping; }
+
/*!
Use FOV clipping
\return True if yes, False otherwise.
*/
- inline bool getFovClipping() const {return fovClipping;}
+ inline bool getFovClipping() const { return fovClipping; }
/*!
Get the state of LOD setting.
\return True if LOD is enabled, false otherwise.
*/
- inline bool getLodState() const {
- return useLod;
- }
-
+ inline bool getLodState() const { return useLod; }
+
/*!
Get the minimum line length to track a segment when LOD is enabled.
\return The minimum line length.
*/
- inline double getMinLineLengthThreshold() const {
- return minLineLengthThreshold;
- }
+ 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.
\return nearClipping
*/
- inline double getNearClippingDistance() const {return nearClipping;}
-
+ inline double getNearClippingDistance() const { return nearClipping; }
+
/*!
Has Far clipping been specified?
\return True if yes, False otherwise.
*/
- inline bool hasFarClippingDistance() const {return hasFarClipping;}
-
+ inline bool hasFarClippingDistance() const { return hasFarClipping; }
+
/*!
Has Near clipping been specified?
\return True if yes, False otherwise.
*/
- inline bool hasNearClippingDistance() const {return hasNearClipping;}
-
- void parse(const char * filename);
+ inline bool hasNearClippingDistance() const { return hasNearClipping; }
+
+ void parse(const char *filename);
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_camera (xmlDocPtr doc, xmlNodePtr node);
+ void read_camera(xmlDocPtr doc, xmlNodePtr node);
void read_face(xmlDocPtr doc, xmlNodePtr node);
- void read_lod (xmlDocPtr doc, xmlNodePtr node);
-
+ void read_lod(xmlDocPtr doc, xmlNodePtr node);
+
/*!
Set the angle to determine if a face appeared.
\param aappear : New angleAppear
*/
- inline void setAngleAppear(const double &aappear) {angleAppear = aappear;}
-
+ inline void setAngleAppear(const double &aappear) { angleAppear = aappear; }
+
/*!
Set the angle to determine if a face disappeared.
\param adisappear : New angleDisappear
*/
- inline void setAngleDisappear(const double &adisappear) {angleDisappear = adisappear;}
-
- void setCameraParameters(const vpCameraParameters &_cam){ cam = _cam; }
-
+ inline void setAngleDisappear(const double &adisappear) { angleDisappear = adisappear; }
+
+ void setCameraParameters(const vpCameraParameters &_cam) { cam = _cam; }
+
/*!
Set the far clipping distance.
\param fclip : New farClipping
*/
- inline void setFarClippingDistance(const double &fclip) {farClipping = fclip;}
+ 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);
//@}
-
+
protected:
/** @name Protected Member Functions Inherited from vpMbXmlParser */
//@{
void init();
//@}
-
};
#endif
#endif /* NMBXMLPARSER_H_ */
-
-
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h
index 2679ac0..72789de 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCircle.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,208 +44,211 @@
#ifndef vpMbtDistanceCircle_HH
#define vpMbtDistanceCircle_HH
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/mbt/vpMbtMeEllipse.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
-#include <visp3/core/vpCircle.h>
-#include <visp3/mbt/vpMbHiddenFaces.h>
/*!
\class vpMbtDistanceCircle
-
+
\brief Manage a circle used in the model-based tracker.
\ingroup group_mbt_features
*/
class VISP_EXPORT vpMbtDistanceCircle
{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- //double alpha;
- double wmean;
- vpFeatureEllipse featureEllipse ;
- //! Polygon describing the circle bbox
-// vpMbtPolygon poly;
- bool isTrackedCircle;
-
- public:
- //! The moving edge containers
- vpMbtMeEllipse *meEllipse;
-
- //! The circle to track
- vpCircle *circle;
-
- //! The radius of the circle
- double radius;
-
- //! The center of the circle
- vpPoint *p1;
- //! A point on the plane containing the circle
- vpPoint *p2;
- //! An other point on the plane containing the circle
- vpPoint *p3;
-
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- unsigned int nbFeature;
- //! Indicates if the circle has to be reinitialized
- bool Reinit;
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the faces which contain the line
- int index_polygon;
- //! Indicates if the circle is visible or not
- bool isvisible;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMbtDistanceCircle(const vpMbtDistanceCircle &)
-// : name(), index(0), cam(), me(NULL), wmean(1),
-// featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
-// circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
-// L(), error(), nbFeature(0), Reinit(false),
-// hiddenface(NULL), index_polygon(-1), isvisible(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMbtDistanceCircle &operator=(const vpMbtDistanceCircle &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
-
- public:
- vpMbtDistanceCircle() ;
- ~vpMbtDistanceCircle() ;
-
- void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r);
-
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
-
- /*!
- Get the index of the circle.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() {return index ;}
-
- /*!
- Get the mean weight of the circle. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the circle.
- */
- inline double getMeanWeight() const {return wmean;}
-
- /*!
- Get the name of the circle.
-
- \return Return the name of the circle.
- */
- inline std::string getName() const {return name;}
-
- /*!
- Get the polygon associated to the circle.
-
- \return poly.
- */
-// inline vpMbtPolygon& getPolygon() {return poly;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
- /*!
- Return if the circle is used for tracking.
-
- \return True if it is used, False otherwise.
- */
- inline bool isTracked() const {return isTrackedCircle;}
-
- /*!
- Check if the circle is visible in the image or not.
-
- \return Return true if the circle is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set if the circle has to considered during tracking phase.
-
- \param track : True if the circle has to be tracked, False otherwise.
- */
- inline void setTracked(const bool& track) {this->isTrackedCircle = track;}
-
- /*!
- Set the index of the circle.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the circle.
-
- \param _wmean : The mean weight of the circle.
- */
- inline void setMeanWeight(const double _wmean) {this->wmean = _wmean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the circle.
-
- \param circle_name : The name of the circle.
- */
- inline void setName(const std::string& circle_name) {this->name = circle_name;}
-
- /*!
- Set the name of the circle.
-
- \param circle_name : The name of the circle.
- */
- inline void setName(const char* circle_name) {this->name = std::string(circle_name);}
-
- /*!
- Set a boolean parameter to indicates if the circle is visible in the image or not.
-
- \param _isvisible : Set to true if the circle is visible
- */
- inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
-
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- private:
- void project(const vpHomogeneousMatrix &cMo);
-} ;
+private:
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ // double alpha;
+ double wmean;
+ vpFeatureEllipse featureEllipse;
+ //! Polygon describing the circle bbox
+ // vpMbtPolygon poly;
+ bool isTrackedCircle;
+
+public:
+ //! The moving edge containers
+ vpMbtMeEllipse *meEllipse;
+
+ //! The circle to track
+ vpCircle *circle;
+
+ //! The radius of the circle
+ double radius;
+
+ //! The center of the circle
+ vpPoint *p1;
+ //! A point on the plane containing the circle
+ vpPoint *p2;
+ //! An other point on the plane containing the circle
+ vpPoint *p3;
+
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ unsigned int nbFeature;
+ //! Indicates if the circle has to be reinitialized
+ bool Reinit;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the faces which contain the line
+ int index_polygon;
+ //! Indicates if the circle is visible or not
+ bool isvisible;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMbtDistanceCircle(const vpMbtDistanceCircle &)
+ // : name(), index(0), cam(), me(NULL), wmean(1),
+ // featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
+ // circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
+ // L(), error(), nbFeature(0), Reinit(false),
+ // hiddenface(NULL), index_polygon(-1), isvisible(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMbtDistanceCircle &operator=(const vpMbtDistanceCircle &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
+
+public:
+ vpMbtDistanceCircle();
+ ~vpMbtDistanceCircle();
+
+ void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r);
+
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters &camera) { camera = this->cam; }
+
+ /*!
+ Get the index of the circle.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() { return index; }
+
+ /*!
+ Get the mean weight of the circle. The mean weight is computed thanks to
+ the weight of each moving edge. Those weights are computed by the robust
+ estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the circle.
+ */
+ inline double getMeanWeight() const { return wmean; }
+
+ /*!
+ Get the name of the circle.
+
+ \return Return the name of the circle.
+ */
+ inline std::string getName() const { return name; }
+
+ /*!
+ Get the polygon associated to the circle.
+
+ \return poly.
+ */
+ // inline vpMbtPolygon& getPolygon() {return poly;}
+
+ void initInteractionMatrixError();
+
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+ /*!
+ Return if the circle is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const { return isTrackedCircle; }
+
+ /*!
+ Check if the circle is visible in the image or not.
+
+ \return Return true if the circle is visible
+ */
+ inline bool isVisible() const { return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; }
-#endif
+ /*!
+ Set if the circle has to considered during tracking phase.
+
+ \param track : True if the circle has to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool &track) { this->isTrackedCircle = track; }
+
+ /*!
+ Set the index of the circle.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) { index = i; }
+
+ /*!
+ Set the mean weight of the circle.
+ \param _wmean : The mean weight of the circle.
+ */
+ inline void setMeanWeight(const double _wmean) { this->wmean = _wmean; }
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the circle.
+
+ \param circle_name : The name of the circle.
+ */
+ inline void setName(const std::string &circle_name) { this->name = circle_name; }
+
+ /*!
+ Set the name of the circle.
+
+ \param circle_name : The name of the circle.
+ */
+ inline void setName(const char *circle_name) { this->name = std::string(circle_name); }
+
+ /*!
+ Set a boolean parameter to indicates if the circle is visible in the image
+ or not.
+
+ \param _isvisible : Set to true if the circle is visible
+ */
+ inline void setVisible(bool _isvisible) { isvisible = _isvisible; }
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+private:
+ void project(const vpHomogeneousMatrix &cMo);
+};
+
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h
index 335d610..6e03f54 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceCylinder.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,226 +47,232 @@
#ifndef vpMbtDistanceCylinder_HH
#define vpMbtDistanceCylinder_HH
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpLine.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/mbt/vpMbtMeLine.h>
-#include <visp3/core/vpLine.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/visual_features/vpFeatureLine.h>
-#include <visp3/core/vpCylinder.h>
-#include <visp3/core/vpCircle.h>
-#include <visp3/mbt/vpMbHiddenFaces.h>
/*!
\class vpMbtDistanceCylinder
-
+
\brief Manage a cylinder used in the model-based tracker.
\ingroup group_mbt_features
*/
class VISP_EXPORT vpMbtDistanceCylinder
{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- double wmean1;
- double wmean2;
- vpFeatureLine featureline1 ;
- vpFeatureLine featureline2 ;
- bool isTrackedCylinder;
-
- public:
- //! The moving edge containers (first line of the cylinder)
- vpMbtMeLine *meline1;
- //! The moving edge containers (second line of the cylinder)
- vpMbtMeLine *meline2;
-
- //! The upper circle limiting the cylinder
- vpCircle *cercle1;
- //! The lower circle limiting the cylinder
- vpCircle *cercle2;
-
- //! The radius of the cylinder
- double radius;
-
- //! The first extremity on the axe
- vpPoint *p1;
- //! The second extremity on the axe
- vpPoint *p2;
-
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- unsigned int nbFeature;
- //! The number of moving edges on line 1
- unsigned int nbFeaturel1;
- //! The number of moving edges on line 2
- unsigned int nbFeaturel2;
- //! Indicates if the line has to be reinitialized
- bool Reinit;
- //! The cylinder
- vpCylinder *c;
-
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the face which contains the cylinder
- int index_polygon;
- //! Indicates if the cylinder is visible or not
- bool isvisible;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMbtDistanceCylinder(const vpMbtDistanceCylinder &)
-// : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1),
-// featureline1(), featureline2(), isTrackedCylinder(true), meline1(NULL), meline2(NULL),
-// cercle1(NULL), cercle2(NULL), radius(0), p1(NULL), p2(NULL), L(),
-// error(), nbFeature(0), nbFeaturel1(0), nbFeaturel2(0), Reinit(false),
-// c(NULL), hiddenface(NULL), index_polygon(-1), isvisible(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMbtDistanceCylinder &operator=(const vpMbtDistanceCylinder &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
-
- public:
- vpMbtDistanceCylinder() ;
- ~vpMbtDistanceCylinder() ;
-
- void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r);
-
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) {camera = this->cam;}
-
- /*!
- Get the index of the cylinder.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() {return index ;}
-
- /*!
- Get the mean weight of the first line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the first line.
- */
- inline double getMeanWeight1() const {return wmean1;}
-
- /*!
- Get the mean weight of the second line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the second line.
- */
- inline double getMeanWeight2() const {return wmean2;}
-
- /*!
- Get the name of the cylinder.
-
- \return Return the name of the cylinder
- */
- inline std::string getName() const {return name;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Return if the cylinder is used for tracking.
-
- \return True if it is used, False otherwise.
- */
- inline bool isTracked() const {return isTrackedCylinder;}
-
- /*!
- Check if the cylinder is visible in the image or not.
-
- \return Return true if the cylinder is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set if the cylinder has to considered during tracking phase.
-
- \param track : True if the cylinder has to be tracked, False otherwise.
- */
- inline void setTracked(const bool& track) {this->isTrackedCylinder = track;}
-
- /*!
- Set the index of the cylinder.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the first line.
-
- \param wmean : The mean weight of the first line.
- */
- inline void setMeanWeight1(const double wmean) {this->wmean1 = wmean;}
-
- /*!
- Set the mean weight of the second line.
-
- \param wmean : The mean weight of the second line.
- */
- inline void setMeanWeight2(const double wmean) {this->wmean2 = wmean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the cylinder.
-
- \param cyl_name : The name of the cylinder.
- */
- inline void setName(const std::string& cyl_name) {this->name = cyl_name;}
-
- /*!
- Set the name of the cylinder.
-
- \param cyl_name : The name of the cylinder
- */
- inline void setName(const char* cyl_name) {this->name = std::string(cyl_name);}
-
- /*!
- Set a boolean parameter to indicates if the cylinder is visible in the image or not.
-
- \param _isvisible : Set to true if the cylinder is visible
- */
- inline void setVisible(bool _isvisible) {isvisible = _isvisible ;}
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- private:
- void project(const vpHomogeneousMatrix &cMo);
-} ;
+private:
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ double wmean1;
+ double wmean2;
+ vpFeatureLine featureline1;
+ vpFeatureLine featureline2;
+ bool isTrackedCylinder;
-#endif
+public:
+ //! The moving edge containers (first line of the cylinder)
+ vpMbtMeLine *meline1;
+ //! The moving edge containers (second line of the cylinder)
+ vpMbtMeLine *meline2;
+
+ //! The upper circle limiting the cylinder
+ vpCircle *cercle1;
+ //! The lower circle limiting the cylinder
+ vpCircle *cercle2;
+
+ //! The radius of the cylinder
+ double radius;
+
+ //! The first extremity on the axe
+ vpPoint *p1;
+ //! The second extremity on the axe
+ vpPoint *p2;
+
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ unsigned int nbFeature;
+ //! The number of moving edges on line 1
+ unsigned int nbFeaturel1;
+ //! The number of moving edges on line 2
+ unsigned int nbFeaturel2;
+ //! Indicates if the line has to be reinitialized
+ bool Reinit;
+ //! The cylinder
+ vpCylinder *c;
+
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the face which contains the cylinder
+ int index_polygon;
+ //! Indicates if the cylinder is visible or not
+ bool isvisible;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMbtDistanceCylinder(const vpMbtDistanceCylinder &)
+ // : name(), index(0), cam(), me(NULL), wmean1(1), wmean2(1),
+ // featureline1(), featureline2(), isTrackedCylinder(true),
+ // meline1(NULL), meline2(NULL), cercle1(NULL), cercle2(NULL),
+ // radius(0), p1(NULL), p2(NULL), L(), error(), nbFeature(0),
+ // nbFeaturel1(0), nbFeaturel2(0), Reinit(false), c(NULL),
+ // hiddenface(NULL), index_polygon(-1), isvisible(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMbtDistanceCylinder &operator=(const vpMbtDistanceCylinder &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
+
+public:
+ vpMbtDistanceCylinder();
+ ~vpMbtDistanceCylinder();
+
+ void buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r);
+
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters &camera) { camera = this->cam; }
+
+ /*!
+ Get the index of the cylinder.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() { return index; }
+
+ /*!
+ Get the mean weight of the first line. The mean weight is computed thanks
+ to the weight of each moving edge. Those weights are computed by the robust
+ estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the first line.
+ */
+ inline double getMeanWeight1() const { return wmean1; }
+
+ /*!
+ Get the mean weight of the second line. The mean weight is computed thanks
+ to the weight of each moving edge. Those weights are computed by the robust
+ estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the second line.
+ */
+ inline double getMeanWeight2() const { return wmean2; }
+ /*!
+ Get the name of the cylinder.
+
+ \return Return the name of the cylinder
+ */
+ inline std::string getName() const { return name; }
+
+ void initInteractionMatrixError();
+
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Return if the cylinder is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const { return isTrackedCylinder; }
+
+ /*!
+ Check if the cylinder is visible in the image or not.
+
+ \return Return true if the cylinder is visible
+ */
+ inline bool isVisible() const { return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; }
+
+ /*!
+ Set if the cylinder has to considered during tracking phase.
+
+ \param track : True if the cylinder has to be tracked, False otherwise.
+ */
+ inline void setTracked(const bool &track) { this->isTrackedCylinder = track; }
+
+ /*!
+ Set the index of the cylinder.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) { index = i; }
+
+ /*!
+ Set the mean weight of the first line.
+
+ \param wmean : The mean weight of the first line.
+ */
+ inline void setMeanWeight1(const double wmean) { this->wmean1 = wmean; }
+
+ /*!
+ Set the mean weight of the second line.
+
+ \param wmean : The mean weight of the second line.
+ */
+ inline void setMeanWeight2(const double wmean) { this->wmean2 = wmean; }
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the cylinder.
+
+ \param cyl_name : The name of the cylinder.
+ */
+ inline void setName(const std::string &cyl_name) { this->name = cyl_name; }
+
+ /*!
+ Set the name of the cylinder.
+
+ \param cyl_name : The name of the cylinder
+ */
+ inline void setName(const char *cyl_name) { this->name = std::string(cyl_name); }
+
+ /*!
+ Set a boolean parameter to indicates if the cylinder is visible in the
+ image or not.
+
+ \param _isvisible : Set to true if the cylinder is visible
+ */
+ inline void setVisible(bool _isvisible) { isvisible = _isvisible; }
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+private:
+ void project(const vpHomogeneousMatrix &cMo);
+};
+
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h
index 3ccf482..846f81e 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltCylinder.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,20 +45,21 @@
#include <map>
-#include <visp3/core/vpPolygon3D.h>
-#include <visp3/klt/vpKltOpencv.h>
-#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpGEMM.h>
-#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/klt/vpKltOpencv.h>
#include <visp3/mbt/vpMbHiddenFaces.h>
-#include <visp3/core/vpCylinder.h>
-#include <visp3/core/vpCircle.h>
+#include <visp3/vision/vpHomography.h>
/*!
\class vpMbtDistanceKltCylinder
- \brief Implementation of a polygon of the model containing points of interest. It is used by the model-based tracker KLT, and hybrid.
+ \brief Implementation of a polygon of the model containing points of
+ interest. It is used by the model-based tracker KLT, and hybrid.
\warning This class is only available if OpenCV is installed, and used.
@@ -108,106 +110,109 @@ public:
bool useScanLine;
private:
- double computeZ(const double &x, const double &y);
- bool isTrackedFeature(const int id);
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMbtDistanceKltCylinder(const vpMbtDistanceKltCylinder &)
-// : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(),
-// initPoints(), initPoints3D(), curPoints(), curPointsInd(),
-// nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false),
-// cam(), isTrackedKltCylinder(true), listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMbtDistanceKltCylinder &operator=(const vpMbtDistanceKltCylinder &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ double computeZ(const double &x, const double &y);
+ bool isTrackedFeature(const int id);
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMbtDistanceKltCylinder(const vpMbtDistanceKltCylinder &)
+ // : c0Mo(), p1Ext(), p2Ext(), cylinder(), circle1(), circle2(),
+ // initPoints(), initPoints3D(), curPoints(), curPointsInd(),
+ // nbPointsCur(0), nbPointsInit(0), minNbPoint(4),
+ // enoughPoints(false), cam(), isTrackedKltCylinder(true),
+ // listIndicesCylinderBBox(), hiddenface(NULL), useScanLine(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMbtDistanceKltCylinder &operator=(const vpMbtDistanceKltCylinder &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpMbtDistanceKltCylinder();
- virtual ~vpMbtDistanceKltCylinder();
+ vpMbtDistanceKltCylinder();
+ virtual ~vpMbtDistanceKltCylinder();
- void buildFrom(const vpPoint &p1, const vpPoint &p2, const double &r);
+ 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);
+ unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker);
+ void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMc0, vpColVector &_R, vpMatrix &_J);
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayPrimitive(const vpImage<unsigned char>& _I);
- void displayPrimitive(const vpImage<vpRGBa>& _I);
+ void displayPrimitive(const vpImage<unsigned char> &_I);
+ void displayPrimitive(const vpImage<vpRGBa> &_I);
/*!
Get the camera parameters of the face.
\return cam : the camera parameters of the face.
*/
- inline vpCameraParameters& getCameraParameters(){ return cam; }
+ inline vpCameraParameters &getCameraParameters() { return cam; }
- inline std::map<int, vpImagePoint>& getCurrentPoints() {return curPoints; }
+ inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; }
- inline std::map<int, int>& getCurrentPointsInd() {return curPointsInd; }
+ inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; }
- inline vpCylinder getCylinder() const {
- return cylinder;
- }
+ inline vpCylinder getCylinder() const { return cylinder; }
/*!
- Get the number of point that was belonging to the face at the initialisation
+ Get the number of point that was belonging to the face at the
+ initialisation
\return the number of initial point.
\sa getCurrentNumberPoints()
*/
- inline unsigned int getInitialNumberPoint() const { return nbPointsInit;}
+ inline unsigned int getInitialNumberPoint() const { return nbPointsInit; }
/*!
Get the number of points detected in the last image.
- \warning To have the real number of points, the function computeNbDetectedCurrent()
- must be called first.
+ \warning To have the real number of points, the function
+ computeNbDetectedCurrent() must be called first.
\return the number of points detected in the current image.
\sa getInitialNumberPoint()
*/
- inline unsigned int getCurrentNumberPoints() const {return nbPointsCur;}
+ inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; }
- inline bool hasEnoughPoints() const {return enoughPoints;}
+ inline bool hasEnoughPoints() const { return enoughPoints; }
/*!
Return if the klt cylinder is used for tracking.
\return True if it is used, False otherwise.
*/
- inline bool isTracked() const {return isTrackedKltCylinder;}
+ inline bool isTracked() const { return isTrackedKltCylinder; }
- void init(const vpKltOpencv& _tracker, const vpHomogeneousMatrix &cMo);
+ 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);
#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 a8a8174..2de6cfb 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceKltPoints.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,18 +46,19 @@
#include <map>
-#include <visp3/core/vpPolygon3D.h>
-#include <visp3/klt/vpKltOpencv.h>
-#include <visp3/core/vpPlane.h>
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpGEMM.h>
-#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPolygon3D.h>
+#include <visp3/klt/vpKltOpencv.h>
#include <visp3/mbt/vpMbHiddenFaces.h>
+#include <visp3/vision/vpHomography.h>
/*!
\class vpMbtDistanceKltPoints
- \brief Implementation of a polygon of the model containing points of interest. It is used by the model-based tracker KLT, and hybrid.
+ \brief Implementation of a polygon of the model containing points of
+ interest. It is used by the model-based tracker KLT, and hybrid.
\warning This class is only available if OpenCV is installed, and used.
@@ -71,7 +73,8 @@ private:
vpColVector N;
//! current normal
vpColVector N_cur;
- //! inverse of the distance between the plane and the camera at the initial position (speed up computation)
+ //! inverse of the distance between the plane and the camera at the initial
+ //! position (speed up computation)
double invd0;
//! cRc0_0n (temporary variable to speed up the computation)
vpColVector cRc0_0n;
@@ -107,104 +110,110 @@ public:
bool useScanLine;
private:
-
- double compute_1_over_Z(const double x, const double y);
- void computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& cHc0);
- bool isTrackedFeature(const int id);
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
-// : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
-// nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
-// cam(), isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL), useScanLine(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ double compute_1_over_Z(const double x, const double y);
+ void computeP_mu_t(const double x_in, const double y_in, double &x_out, double &y_out, const vpMatrix &cHc0);
+ bool isTrackedFeature(const int id);
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMbtDistanceKltPoints(const vpMbtDistanceKltPoints &)
+ // : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(),
+ // curPoints(), curPointsInd(),
+ // nbPointsCur(0), nbPointsInit(0), minNbPoint(4),
+ // enoughPoints(false), dt(1.), d0(1.), cam(),
+ // isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL),
+ // useScanLine(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMbtDistanceKltPoints &operator=(const vpMbtDistanceKltPoints &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- vpMbtDistanceKltPoints();
- virtual ~vpMbtDistanceKltPoints();
+ vpMbtDistanceKltPoints();
+ virtual ~vpMbtDistanceKltPoints();
- unsigned int computeNbDetectedCurrent(const vpKltOpencv& _tracker);
- void computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& cHc0);
- void computeInteractionMatrixAndResidu(vpColVector& _R, vpMatrix& _J);
+ unsigned int computeNbDetectedCurrent(const vpKltOpencv &_tracker);
+ void computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &cHc0);
+ void computeInteractionMatrixAndResidu(vpColVector &_R, vpMatrix &_J);
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayPrimitive(const vpImage<unsigned char>& _I);
- void displayPrimitive(const vpImage<vpRGBa>& _I);
+ void displayPrimitive(const vpImage<unsigned char> &_I);
+ void displayPrimitive(const vpImage<vpRGBa> &_I);
/*!
Get the camera parameters of the face.
\return cam : the camera parameters of the face.
*/
- inline vpCameraParameters& getCameraParameters(){ return cam; }
+ inline vpCameraParameters &getCameraParameters() { return cam; }
- inline vpColVector getCurrentNormal() const {return N_cur; }
+ inline vpColVector getCurrentNormal() const { return N_cur; }
- inline std::map<int, vpImagePoint>& getCurrentPoints() {return curPoints; }
+ inline std::map<int, vpImagePoint> &getCurrentPoints() { return curPoints; }
- inline std::map<int, int>& getCurrentPointsInd() {return curPointsInd; }
+ inline std::map<int, int> &getCurrentPointsInd() { return curPointsInd; }
/*!
- Get the number of point that was belonging to the face at the initialisation
+ Get the number of point that was belonging to the face at the
+ initialisation
\return the number of initial point.
\sa getCurrentNumberPoints()
*/
- inline unsigned int getInitialNumberPoint() const { return nbPointsInit;}
+ inline unsigned int getInitialNumberPoint() const { return nbPointsInit; }
/*!
Get the number of points detected in the last image.
- \warning To have the real number of points, the function computeNbDetectedCurrent()
- must be called first.
+ \warning To have the real number of points, the function
+ computeNbDetectedCurrent() must be called first.
\return the number of points detected in the current image.
\sa getInitialNumberPoint()
*/
- inline unsigned int getCurrentNumberPoints() const {return nbPointsCur;}
+ inline unsigned int getCurrentNumberPoints() const { return nbPointsCur; }
- inline bool hasEnoughPoints() const {return enoughPoints;}
+ inline bool hasEnoughPoints() const { return enoughPoints; }
- void init(const vpKltOpencv& _tracker);
+ void init(const vpKltOpencv &_tracker);
/*!
Return if the klt points are used for tracking.
\return True if it is used, False otherwise.
*/
- inline bool isTracked() const {return isTrackedKltPoints;}
+ 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);
#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 50c8388..0bbe5fa 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtDistanceLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,18 +46,18 @@
#ifndef vpMbtDistanceLine_HH
#define vpMbtDistanceLine_HH
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpLine.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/mbt/vpMbHiddenFaces.h>
#include <visp3/mbt/vpMbtMeLine.h>
-#include <visp3/core/vpLine.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/visual_features/vpFeatureLine.h>
-#include <visp3/mbt/vpMbHiddenFaces.h>
#include <list>
/*!
\class vpMbtDistanceLine
-
+
\brief Manage the line of a polygon used in the model-based tracker.
\ingroup group_mbt_features
@@ -64,191 +65,198 @@
*/
class VISP_EXPORT vpMbtDistanceLine
{
- private :
- std::string name;
- unsigned int index;
- vpCameraParameters cam;
- vpMe *me;
- bool isTrackedLine;
- bool isTrackedLineWithVisibility;
- double wmean;
- vpFeatureLine featureline ;
- //! Polygon describing the line
- vpMbtPolygon poly;
-
- public:
- //! Use scanline rendering
- bool useScanLine;
- //! The moving edge container
- //vpMbtMeLine *meline;
- std::vector<vpMbtMeLine*> meline;
- //! The 3D line
- vpLine *line;
- //! The first extremity
- vpPoint *p1;
- //! The second extremity
- vpPoint *p2;
- //! The interaction matrix
- vpMatrix L;
- //! The error vector
- vpColVector error;
- //! The number of moving edges
- //unsigned int nbFeature;
- std::vector<unsigned int> nbFeature;
- //! The number of moving edges
- unsigned int nbFeatureTotal;
- //! Indicates if the line has to be reinitialized
- bool Reinit;
- //! Pointer to the list of faces
- vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
- //! Index of the faces which contain the line
- std::list<int> Lindex_polygon;
- //! Vector of bool associated with Lindex_polygon to know if Lindex_polygon[i] is tracked
- std::vector<bool> Lindex_polygon_tracked;
- //! Indicates if the line is visible or not
- bool isvisible;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpMbtDistanceLine(const vpMbtDistanceLine &)
-// : name(), index(0), cam(), me(NULL), isTrackedLine(true), isTrackedLineWithVisibility(true),
-// wmean(1), featureline(), poly(), useScanLine(false), meline(), line(NULL), p1(NULL), p2(NULL), L(),
-// error(), nbFeature(), nbFeatureTotal(0), Reinit(false), hiddenface(NULL), Lindex_polygon(),
-// Lindex_polygon_tracked(), isvisible(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpMbtDistanceLine &operator=(const vpMbtDistanceLine &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
-
- public:
- vpMbtDistanceLine() ;
- ~vpMbtDistanceLine() ;
-
- void addPolygon(const int &index);
-
- void buildFrom(vpPoint &_p1, vpPoint &_p2);
-
- bool closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold);
- void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
-
- void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor col, const unsigned int thickness = 1, const bool displayFullModel = false);
- void displayMovingEdges(const vpImage<unsigned char> &I);
-
- /*!
- Get the camera paramters.
-
- \param camera : The vpCameraParameters used to store the camera parameters.
- */
- inline void getCameraParameters(vpCameraParameters& camera) const {camera = this->cam;}
-
- /*!
- Get the index of the line.
-
- \return Return the index of the line.
- */
- inline unsigned int getIndex() const {return index ;}
-
- /*!
- Get the mean weight of the line. The mean weight is computed thanks to the weight of each moving edge.
- Those weights are computed by the robust estimation method used during the virtual visual servoing.
-
- \return The mean weight of the line.
- */
- inline double getMeanWeight() const {return wmean;}
-
- /*!
- Get the name of the line.
-
- \return Return the name of the line
- */
- inline std::string getName() const {return name;}
-
- /*!
- Get the polygon associated to the line.
-
- \return poly.
- */
- inline vpMbtPolygon& getPolygon() {return poly;}
-
- void initInteractionMatrixError();
-
- bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Return if the line is used for tracking.
-
- \return True if it is used, False otherwise.
- */
- inline bool isTracked() const {return isTrackedLineWithVisibility;}
-
- /*!
- Check if the line is visible in the image or not.
-
- \return Return true if the line is visible
- */
- inline bool isVisible() const {return isvisible; }
-
- void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- /*!
- Set the camera paramters.
- \param camera : The camera parameters.
- */
- inline void setCameraParameters(const vpCameraParameters& camera) {this->cam = camera;}
-
- /*!
- Set the index of the line.
-
- \param i : The index number
- */
- inline void setIndex(const unsigned int i) {index = i;}
-
- /*!
- Set the mean weight of the line.
-
- \param w_mean : The mean weight of the line.
- */
- inline void setMeanWeight(const double w_mean) {this->wmean = w_mean;}
-
- void setMovingEdge(vpMe *Me);
-
- /*!
- Set the name of the line.
-
- \param line_name : The name of the line.
- */
- inline void setName(const std::string line_name) {this->name = line_name;}
-
- /*!
- Set the name of the line.
-
- \param line_name : The name of the line.
- */
- inline void setName(const char* line_name) {this->name = std::string(line_name);}
-
- void setTracked(const std::string &name, const bool &track);
-
- /*!
- Set a boolean parameter to indicates if the line is visible in the image or not.
-
- \param _isvisible : Set to true if the line is visible
- */
- void setVisible(bool _isvisible) {isvisible = _isvisible ; }
-
- void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
-
- void updateTracked();
-
- private:
- void project(const vpHomogeneousMatrix &cMo);
-} ;
+private:
+ std::string name;
+ unsigned int index;
+ vpCameraParameters cam;
+ vpMe *me;
+ bool isTrackedLine;
+ bool isTrackedLineWithVisibility;
+ double wmean;
+ vpFeatureLine featureline;
+ //! Polygon describing the line
+ vpMbtPolygon poly;
-#endif
+public:
+ //! Use scanline rendering
+ bool useScanLine;
+ //! The moving edge container
+ // vpMbtMeLine *meline;
+ std::vector<vpMbtMeLine *> meline;
+ //! The 3D line
+ vpLine *line;
+ //! The first extremity
+ vpPoint *p1;
+ //! The second extremity
+ vpPoint *p2;
+ //! The interaction matrix
+ vpMatrix L;
+ //! The error vector
+ vpColVector error;
+ //! The number of moving edges
+ // unsigned int nbFeature;
+ std::vector<unsigned int> nbFeature;
+ //! The number of moving edges
+ unsigned int nbFeatureTotal;
+ //! Indicates if the line has to be reinitialized
+ bool Reinit;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *hiddenface;
+ //! Index of the faces which contain the line
+ std::list<int> Lindex_polygon;
+ //! Vector of bool associated with Lindex_polygon to know if
+ //! Lindex_polygon[i] is tracked
+ std::vector<bool> Lindex_polygon_tracked;
+ //! Indicates if the line is visible or not
+ bool isvisible;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpMbtDistanceLine(const vpMbtDistanceLine &)
+ // : name(), index(0), cam(), me(NULL), isTrackedLine(true),
+ // isTrackedLineWithVisibility(true),
+ // wmean(1), featureline(), poly(), useScanLine(false), meline(),
+ // line(NULL), p1(NULL), p2(NULL), L(), error(), nbFeature(),
+ // nbFeatureTotal(0), Reinit(false), hiddenface(NULL),
+ // Lindex_polygon(), Lindex_polygon_tracked(), isvisible(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpMbtDistanceLine &operator=(const vpMbtDistanceLine &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
+
+public:
+ vpMbtDistanceLine();
+ ~vpMbtDistanceLine();
+
+ void addPolygon(const int &index);
+
+ void buildFrom(vpPoint &_p1, vpPoint &_p2);
+
+ bool closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold);
+ void computeInteractionMatrixError(const vpHomogeneousMatrix &cMo);
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void displayMovingEdges(const vpImage<unsigned char> &I);
+
+ /*!
+ Get the camera paramters.
+
+ \param camera : The vpCameraParameters used to store the camera parameters.
+ */
+ inline void getCameraParameters(vpCameraParameters &camera) const { camera = this->cam; }
+
+ /*!
+ Get the index of the line.
+
+ \return Return the index of the line.
+ */
+ inline unsigned int getIndex() const { return index; }
+
+ /*!
+ Get the mean weight of the line. The mean weight is computed thanks to the
+ weight of each moving edge. Those weights are computed by the robust
+ estimation method used during the virtual visual servoing.
+
+ \return The mean weight of the line.
+ */
+ inline double getMeanWeight() const { return wmean; }
+
+ /*!
+ Get the name of the line.
+
+ \return Return the name of the line
+ */
+ inline std::string getName() const { return name; }
+
+ /*!
+ Get the polygon associated to the line.
+
+ \return poly.
+ */
+ inline vpMbtPolygon &getPolygon() { return poly; }
+
+ void initInteractionMatrixError();
+ bool initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Return if the line is used for tracking.
+
+ \return True if it is used, False otherwise.
+ */
+ inline bool isTracked() const { return isTrackedLineWithVisibility; }
+
+ /*!
+ Check if the line is visible in the image or not.
+
+ \return Return true if the line is visible
+ */
+ inline bool isVisible() const { return isvisible; }
+
+ void reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ /*!
+ Set the camera paramters.
+ \param camera : The camera parameters.
+ */
+ inline void setCameraParameters(const vpCameraParameters &camera) { this->cam = camera; }
+
+ /*!
+ Set the index of the line.
+
+ \param i : The index number
+ */
+ inline void setIndex(const unsigned int i) { index = i; }
+
+ /*!
+ Set the mean weight of the line.
+
+ \param w_mean : The mean weight of the line.
+ */
+ inline void setMeanWeight(const double w_mean) { this->wmean = w_mean; }
+
+ void setMovingEdge(vpMe *Me);
+
+ /*!
+ Set the name of the line.
+
+ \param line_name : The name of the line.
+ */
+ inline void setName(const std::string &line_name) { this->name = line_name; }
+
+ /*!
+ Set the name of the line.
+
+ \param line_name : The name of the line.
+ */
+ inline void setName(const char *line_name) { this->name = std::string(line_name); }
+
+ void setTracked(const std::string &name, const bool &track);
+
+ /*!
+ Set a boolean parameter to indicates if the line is visible in the image
+ or not.
+
+ \param _isvisible : Set to true if the line is visible
+ */
+ void setVisible(bool _isvisible) { isvisible = _isvisible; }
+
+ void trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo);
+
+ void updateTracked();
+
+private:
+ void project(const vpHomogeneousMatrix &cMo);
+};
+
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h
index 343099f..0cb5bb5 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtEdgeKltXmlParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Load XML parameters of the Model based tracker (using edges and point features).
+ * Load XML parameters of the Model based tracker (using edges and point
+ *features).
*
* Authors:
* Aurelien Yol
@@ -37,8 +39,9 @@
/*!
* \file vpMbtEdgeKltXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ * object.
+ */
#ifndef vpMbtEdgeKltXmlParser_HH
#define vpMbtEdgeKltXmlParser_HH
@@ -47,48 +50,38 @@
#ifdef VISP_HAVE_XML2
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
-#include <visp3/mbt/vpMbtXmlParser.h>
#include <visp3/mbt/vpMbtKltXmlParser.h>
+#include <visp3/mbt/vpMbtXmlParser.h>
/*!
\class vpMbtEdgeKltXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup group_mbt_xml_parser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ object. \ingroup group_mbt_xml_parser
Data parser for the model based tracker.
*/
-class VISP_EXPORT vpMbtEdgeKltXmlParser: public vpMbtXmlParser, public vpMbtKltXmlParser
+class VISP_EXPORT vpMbtEdgeKltXmlParser : public vpMbtXmlParser, public vpMbtKltXmlParser
{
protected:
- typedef enum{
- camera,
- face,
- klt,
- ecm,
- lod
- } dataToParseMbtEdgeKlt;
-public:
+ typedef enum { camera, face, klt, ecm, lod } dataToParseMbtEdgeKlt;
- vpMbtEdgeKltXmlParser();
- virtual ~vpMbtEdgeKltXmlParser();
+public:
+ vpMbtEdgeKltXmlParser();
+ virtual ~vpMbtEdgeKltXmlParser();
- void parse(const char * filename);
+ void parse(const char *filename);
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
void writeMainClass(xmlNodePtr node);
-
+
protected:
void init();
-
};
#endif
#endif /* NMBTEDGEKLTXMLPARSER_H_ */
-
-
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h
new file mode 100644
index 0000000..9040daf
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthDense.h
@@ -0,0 +1,229 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Manage depth dense features for a particular face.
+ *
+ *****************************************************************************/
+
+#ifndef __vpMbtFaceDepthDense_h_
+#define __vpMbtFaceDepthDense_h_
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_PCL
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#endif
+
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
+
+#define DEBUG_DISPLAY_DEPTH_DENSE 0
+
+class VISP_EXPORT vpMbtFaceDepthDense
+{
+public:
+ enum vpDepthDenseFilteringType {
+ NO_FILTERING = 0, ///< Face is used if visible
+ 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
+ };
+
+ //! Camera intrinsic parameters
+ vpCameraParameters m_cam;
+ //! Flags specifying which clipping to used
+ unsigned int m_clippingFlag;
+ //! Distance for near clipping
+ double m_distFarClip;
+ //! Distance for near clipping
+ double m_distNearClip;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *m_hiddenFace;
+ //! Plane equation described in the object frame
+ vpPlane m_planeObject;
+ //! Polygon defining the face
+ vpMbtPolygon *m_polygon;
+ //! Scan line visibility
+ bool m_useScanLine;
+
+ vpMbtFaceDepthDense();
+ virtual ~vpMbtFaceDepthDense();
+
+ void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *const faces, int polygon = -1,
+ std::string name = "");
+
+#ifdef VISP_HAVE_PCL
+ bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud, const unsigned int stepX,
+ const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ );
+#endif
+ bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
+ const std::vector<vpColVector> &point_cloud, const unsigned int stepX,
+ const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ );
+
+ void computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &error);
+
+ void computeVisibility();
+ void computeVisibilityDisplay();
+
+ void display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const double scale = 0.05, const unsigned int thickness = 1);
+ void displayFeature(const vpImage<vpRGBa> &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; }
+
+ void setCameraParameters(const vpCameraParameters &camera);
+
+ void setScanLineVisibilityTest(const bool v);
+
+ inline void setDepthDenseFilteringMaxDistance(const double maxDistance)
+ {
+ m_depthDenseFilteringMaxDist = maxDistance;
+ }
+
+ inline void setDepthDenseFilteringMethod(const int method) { m_depthDenseFilteringMethod = method; }
+
+ inline void setDepthDenseFilteringMinDistance(const double minDistance)
+ {
+ m_depthDenseFilteringMinDist = minDistance;
+ }
+
+ inline void setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
+ {
+ if (occupancyRatio < 0.0 || occupancyRatio > 1.0) {
+ std::cerr << "occupancyRatio < 0.0 || occupancyRatio > 1.0" << std::endl;
+ } else {
+ m_depthDenseFilteringOccupancyRatio = occupancyRatio;
+ }
+ }
+
+private:
+ class PolygonLine
+ {
+ public:
+ //! The first extremity
+ vpPoint *m_p1;
+ //! The second extremity
+ vpPoint *m_p2;
+ //! Polygon describing the line
+ vpMbtPolygon m_poly;
+ //! The first extremity clipped in the image frame
+ vpImagePoint m_imPt1;
+ //! The second extremity clipped in the image frame
+ vpImagePoint m_imPt2;
+
+ 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 = &m_poly.p[0];
+ m_p2 = &m_poly.p[1];
+ }
+
+ PolygonLine &operator=(PolygonLine other)
+ {
+ swap(*this, other);
+
+ return *this;
+ }
+
+ void swap(PolygonLine &first, PolygonLine &second)
+ {
+ using std::swap;
+ swap(first.m_p1, second.m_p1);
+ swap(first.m_p2, second.m_p2);
+ swap(first.m_poly, second.m_poly);
+ swap(first.m_imPt1, second.m_imPt1);
+ swap(first.m_imPt2, second.m_imPt2);
+ }
+ };
+
+protected:
+ //! Method to use to consider or not the face
+ int m_depthDenseFilteringMethod;
+ //! Maximum distance threshold
+ double m_depthDenseFilteringMaxDist;
+ //! Minimum distance threshold
+ double m_depthDenseFilteringMinDist;
+ //! Ratio between available depth points and theoretical number of points
+ double m_depthDenseFilteringOccupancyRatio;
+ //! Flag to define if the face should be tracked or not
+ bool m_isTracked;
+ //! Visibility flag
+ bool m_isVisible;
+ std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
+ //! Plane equation described in the camera frame and updated with the
+ //! current pose
+ vpPlane m_planeCamera;
+ //! List of depth points inside the face
+ std::vector<double> m_pointCloudFace;
+ //! Polygon lines used for scan-line visibility
+ std::vector<PolygonLine> m_polygonLines;
+
+protected:
+ void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
+ std::vector<vpImagePoint> &roiPts
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ ,
+ double &distanceToFace);
+
+ bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
+};
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h
new file mode 100644
index 0000000..9649883
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtFaceDepthNormal.h
@@ -0,0 +1,291 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Manage depth normal features for a particular face.
+ *
+ *****************************************************************************/
+
+#ifndef __vpMbtFaceDepthNormal_h_
+#define __vpMbtFaceDepthNormal_h_
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+#ifdef VISP_HAVE_PCL
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#endif
+
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbTracker.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
+
+#define DEBUG_DISPLAY_DEPTH_NORMAL 0
+
+class VISP_EXPORT vpMbtFaceDepthNormal
+{
+public:
+ enum vpFaceCentroidType {
+ GEOMETRIC_CENTROID, ///< Compute the geometric centroid
+ MEAN_CENTROID ///< Compute the mean centroid
+ };
+
+ enum vpFeatureEstimationType {
+ ROBUST_FEATURE_ESTIMATION = 0,
+ ROBUST_SVD_PLANE_ESTIMATION = 1,
+#ifdef VISP_HAVE_PCL
+ PCL_PLANE_ESTIMATION = 2
+#endif
+ };
+
+ //! Camera intrinsic parameters
+ vpCameraParameters m_cam;
+ //! Flags specifying which clipping to used
+ unsigned int m_clippingFlag;
+ //! Distance for near clipping
+ double m_distFarClip;
+ //! Distance for near clipping
+ double m_distNearClip;
+ //! Pointer to the list of faces
+ vpMbHiddenFaces<vpMbtPolygon> *m_hiddenFace;
+ //! Plane equation described in the object frame
+ vpPlane m_planeObject;
+ //! Polygon defining the face
+ vpMbtPolygon *m_polygon;
+ //! Scan line visibility
+ bool m_useScanLine;
+
+ vpMbtFaceDepthNormal();
+ virtual ~vpMbtFaceDepthNormal();
+
+ void addLine(vpPoint &p1, vpPoint &p2, vpMbHiddenFaces<vpMbtPolygon> *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<pcl::PointXYZ>::ConstPtr &point_cloud,
+ vpColVector &desired_features, const unsigned int stepX, const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ );
+#endif
+ bool computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
+ const std::vector<vpColVector> &point_cloud, vpColVector &desired_features,
+ const unsigned int stepX, const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ vpImage<unsigned char> &debugImage, std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ );
+
+ 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,
+ vpColVector &face_normal);
+#ifdef VISP_HAVE_PCL
+ 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<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+ void display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const vpColor &col, const unsigned int thickness = 1, const bool displayFullModel = false);
+
+ void displayFeature(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ const double scale = 0.05, const unsigned int thickness = 1);
+ void displayFeature(const vpImage<vpRGBa> &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 setFeatureEstimationMethod(const vpFeatureEstimationType &method) { m_featureEstimationMethod = method; }
+
+ inline void setPclPlaneEstimationMethod(const int method) { m_pclPlaneEstimationMethod = method; }
+
+ inline void setPclPlaneEstimationRansacMaxIter(const int maxIter) { m_pclPlaneEstimationRansacMaxIter = maxIter; }
+
+ inline void setPclPlaneEstimationRansacThreshold(const double threshold)
+ {
+ m_pclPlaneEstimationRansacThreshold = threshold;
+ }
+
+ void setScanLineVisibilityTest(const bool v);
+
+private:
+ class PolygonLine
+ {
+ public:
+ //! The first extremity
+ vpPoint *m_p1;
+ //! The second extremity
+ vpPoint *m_p2;
+ //! Polygon describing the line
+ vpMbtPolygon m_poly;
+ //! The first extremity clipped in the image frame
+ vpImagePoint m_imPt1;
+ //! The second extremity clipped in the image frame
+ vpImagePoint m_imPt2;
+
+ 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 = &m_poly.p[0];
+ m_p2 = &m_poly.p[1];
+ }
+
+ PolygonLine &operator=(PolygonLine other)
+ {
+ swap(*this, other);
+
+ return *this;
+ }
+
+ void swap(PolygonLine &first, PolygonLine &second)
+ {
+ using std::swap;
+ swap(first.m_p1, second.m_p1);
+ swap(first.m_p2, second.m_p2);
+ swap(first.m_poly, second.m_poly);
+ swap(first.m_imPt1, second.m_imPt1);
+ swap(first.m_imPt2, second.m_imPt2);
+ }
+ };
+
+ template <class T> class Mat33
+ {
+ public:
+ std::vector<T> data;
+
+ Mat33() : data(9) {}
+
+ inline T operator[](const size_t i) const { return data[i]; }
+
+ inline T &operator[](const size_t i) { return data[i]; }
+
+ 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]) +
+ data[2] * (data[3] * data[7] - data[4] * data[6]);
+ T invdet = 1 / det;
+
+ Mat33<T> minv;
+ minv[0] = (data[4] * data[8] - data[7] * data[5]) * invdet;
+ minv[1] = (data[2] * data[7] - data[1] * data[8]) * invdet;
+ minv[2] = (data[1] * data[5] - data[2] * data[4]) * invdet;
+ minv[3] = (data[5] * data[6] - data[3] * data[8]) * invdet;
+ minv[4] = (data[0] * data[8] - data[2] * data[6]) * invdet;
+ minv[5] = (data[3] * data[2] - data[0] * data[5]) * invdet;
+ minv[6] = (data[3] * data[7] - data[6] * data[4]) * invdet;
+ minv[7] = (data[6] * data[1] - data[0] * data[7]) * invdet;
+ minv[8] = (data[0] * data[4] - data[3] * data[1]) * invdet;
+
+ return minv;
+ }
+ };
+
+protected:
+ //! True if the face should be considered by the tracker
+ bool m_faceActivated;
+ //! Method to compute the face centroid for the current features
+ vpFaceCentroidType m_faceCentroidMethod;
+ //! Desired centroid (computed from the sensor)
+ vpPoint m_faceDesiredCentroid;
+ //! Face (normalized) normal (computed from the sensor)
+ vpPoint m_faceDesiredNormal;
+ //! Method to estimate the desired features
+ vpFeatureEstimationType m_featureEstimationMethod;
+ //!
+ bool m_isTracked;
+ //!
+ bool m_isVisible;
+ //!
+ std::vector<vpMbtDistanceLine *> m_listOfFaceLines;
+ //! Plane equation described in the camera frame and updated with the
+ //! current pose
+ vpPlane m_planeCamera;
+ //! PCL plane estimation method
+ int m_pclPlaneEstimationMethod;
+ //! PCL pane estimation max number of iterations
+ int m_pclPlaneEstimationRansacMaxIter;
+ //! PCL plane estimation RANSAC threshold
+ double m_pclPlaneEstimationRansacThreshold;
+ //!
+ std::vector<PolygonLine> m_polygonLines;
+
+#ifdef VISP_HAVE_PCL
+ bool computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
+ vpColVector &desired_features, vpColVector &desired_normal,
+ vpColVector ¢roid_point);
+#endif
+ void computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
+ const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
+ vpColVector &desired_features, vpColVector &desired_normal,
+ vpColVector ¢roid_point);
+ void computeDesiredFeaturesSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
+ vpColVector &desired_features, vpColVector &desired_normal,
+ vpColVector ¢roid_point);
+ void computeDesiredNormalAndCentroid(const vpHomogeneousMatrix &cMo, const vpColVector &desired_normal,
+ const vpColVector ¢roid_point);
+
+ bool computePolygonCentroid(const std::vector<vpPoint> &points, vpPoint ¢roid);
+
+ void computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width, const unsigned int height,
+ std::vector<vpImagePoint> &roiPts
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ );
+
+ void estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
+ vpColVector &x_estimated, std::vector<double> &weights);
+
+ void estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
+ vpColVector &plane_equation_estimated, vpColVector ¢roid);
+
+ bool samePoint(const vpPoint &P1, const vpPoint &P2) const;
+};
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h
index b84d413..e9d018d 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtKltXmlParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,8 +38,9 @@
/*!
* \file vpMbtKltXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
-*/
+ * \brief Parse an Xml file to extract configuration parameters of a Mbt Klt
+ * object.
+ */
#ifndef vpMbtKltXmlParser_HH
#define vpMbtKltXmlParser_HH
@@ -46,19 +48,19 @@
#ifdef VISP_HAVE_XML2
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbXmlParser.h>
/*!
\class vpMbtKltXmlParser
- \brief Parse an Xml file to extract configuration parameters of a Mbt Klt object.
- \ingroup group_mbt_xml_parser
+ \brief Parse an Xml file to extract configuration parameters of a Mbt Klt
+ object. \ingroup group_mbt_xml_parser
Data parser for the KLT model based tracker.
*/
-class VISP_EXPORT vpMbtKltXmlParser: virtual public vpMbXmlParser
+class VISP_EXPORT vpMbtKltXmlParser : virtual public vpMbXmlParser
{
protected:
//! Border of the mask used on Klt points
@@ -77,8 +79,8 @@ protected:
unsigned int blockSize;
//! Number of pyramid levels
unsigned int pyramidLevels;
-
- typedef enum{
+
+ typedef enum {
klt = vpMbXmlParser::last,
mask_border,
max_features,
@@ -91,130 +93,129 @@ protected:
last
} dataToParseMbKlt;
-
public:
/** @name Public Member Functions Inherited from vpMbtKltXmlParser */
//@{
- vpMbtKltXmlParser();
- virtual ~vpMbtKltXmlParser();
+ vpMbtKltXmlParser();
+ virtual ~vpMbtKltXmlParser();
/*!
Get the size of a block.
\return blockSize
*/
- inline unsigned int getBlockSize() const {return blockSize;}
-
+ inline unsigned int getBlockSize() const { return blockSize; }
+
/*!
Get the Harris free parameter.
\return harrisParam
*/
- inline double getHarrisParam() const {return harrisParam;}
-
- /*!
- Get the Border of the mask.
+ inline double getHarrisParam() const { return harrisParam; }
+
+ /*!
+Get the Border of the mask.
+
+\return faceBorder
+*/
+ inline unsigned int getMaskBorder() const { return maskBorder; }
- \return faceBorder
- */
- inline unsigned int getMaskBorder() const {return maskBorder;}
-
/*!
Get the maximum number of features for the KLT.
\return maxFeatures
*/
- inline unsigned int getMaxFeatures() const {return maxFeatures;}
-
+ inline unsigned int getMaxFeatures() const { return maxFeatures; }
+
/*!
Get the minimum distance between KLT points.
\return minDist
*/
- inline double getMinDistance() const {return minDist;}
-
+ inline double getMinDistance() const { return minDist; }
+
/*!
Get the number of pyramid levels
\return pyramidLevels
*/
- inline unsigned int getPyramidLevels() const {return pyramidLevels;}
-
+ inline unsigned int getPyramidLevels() const { return pyramidLevels; }
+
/*!
Get the quality of the KLT.
\return quality
*/
- inline double getQuality() const {return qualityValue;}
-
+ inline double getQuality() const { return qualityValue; }
+
/*!
Get the size of the window used in the KLT tracker.
\return winSize
*/
- inline unsigned int getWindowSize() const {return winSize;}
-
- void parse(const char * filename);
-
+ inline unsigned int getWindowSize() const { return winSize; }
+
+ void parse(const char *filename);
+
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
void read_klt(xmlDocPtr doc, xmlNodePtr node);
-
+
/*!
Set the size of a block.
\param bs : New blockSize
*/
- inline void setBlockSize(const unsigned int &bs) {blockSize = bs;}
-
+ inline void setBlockSize(const unsigned int &bs) { blockSize = bs; }
+
/*!
Set the Harris free parameter.
\param hp : New harrisParam
*/
- inline void setHarrisParam(const double &hp) {harrisParam = hp;}
-
+ inline void setHarrisParam(const double &hp) { harrisParam = hp; }
+
/*!
Set the Border of the mask.
\param mb = new maskBorder
*/
- inline void setMaskBorder(const unsigned int &mb) {maskBorder = mb;}
-
+ inline void setMaskBorder(const unsigned int &mb) { maskBorder = mb; }
+
/*!
Set the maximum number of features for the KLT.
\param mF : New maxFeatures
*/
- inline void setMaxFeatures(const unsigned int &mF) {maxFeatures = mF;}
-
+ inline void setMaxFeatures(const unsigned int &mF) { maxFeatures = mF; }
+
/*!
Set the minimum distance between KLT points.
\param mD : New minDist
*/
- inline void setMinDistance(const double &mD) {minDist = mD;}
+ inline void setMinDistance(const double &mD) { minDist = mD; }
/*!
Set the number of pyramid levels
\param pL : New pyramidLevels
*/
- inline void setPyramidLevels(const unsigned int &pL) {pyramidLevels = pL;}
-
+ inline void setPyramidLevels(const unsigned int &pL) { pyramidLevels = pL; }
+
/*!
Set the quality of the KLT.
\param q : New quality
*/
- inline void setQuality(const double &q) {qualityValue = q;}
-
+ inline void setQuality(const double &q) { qualityValue = q; }
+
/*!
Set the size of the window used in the KLT tracker.
\param w : New winSize
*/
- inline void setWindowSize(const unsigned int &w) {winSize = w;}
-
+ inline void setWindowSize(const unsigned int &w) { winSize = w; }
+
void writeMainClass(xmlNodePtr node);
//@}
@@ -223,12 +224,8 @@ protected:
//@{
void init();
//@}
-
};
#endif
#endif
-
-
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h
index 6b9e946..6616727 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeEllipse.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,18 +44,18 @@
#ifndef vpMbtMeEllipse_HH
#define vpMbtMeEllipse_HH
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
-#include <visp3/me/vpMeTracker.h>
-#include <visp3/me/vpMeSite.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeTracker.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
-#include <math.h>
#include <list>
+#include <math.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -64,50 +65,58 @@
\brief Class that tracks an ellipse moving edges.
- In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
+ In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of
+ the image frame (For more information about the image frame see the
+ vpImagePoint documentation) that satisfy the implicit equation :
\f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
- If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
+ If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of
+ points \f$ (i,j) \f$ represents a circle.
The five parameters are stored in the public attribute K.
- An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
- major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
+ An ellipse is also defined thanks to three other parameter which are \f$ a
+ \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and
+ \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
+ major axis and the i axis of the image frame \f$ (i,j) \f$. The following
+ figure shows better meaning of those parameters.
\image html vpMeEllipse.gif
\image latex vpMeEllipse.ps width=10cm
- It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
+ It is possible to compute the coordinates \f$ (i,j) \f$ of a point which
+ belongs to the ellipse thanks to the following equations :
\f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
\f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
- Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
+ Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse
+ center in the image frame and \f$ \alpha \f$ is an angle beetween \f$
+ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
\image html vpMeEllipse2.gif
\image latex vpMeEllipse2.ps width=10cm
The example below available in tutorial-me-ellipse-tracker.cpp and described
- in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
+ in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to
+ use this class.
\include tutorial-me-ellipse-tracker.cpp
- */
-/*
The code below shows how to use this class.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/mbt/vpMbtMeEllipse.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/mbt/vpMbtMeEllipse.h>
int main()
{
vpImage<unsigned char> I;
// I is the image containing the ellipse to track
-
+
// Set the moving-edges tracker parameters
vpMe me;
me.setRange(25);
@@ -133,80 +142,88 @@ int main()
}
\endcode
- \note It is possible to display the ellipse as an overlay. For that you
+ \note It is possible to display the ellipse as an overlay. For that you
must use the display function of the class vpMbtMeEllipse.
*/
class VISP_EXPORT vpMbtMeEllipse : public vpMeTracker
{
public:
- vpMbtMeEllipse() ;
- vpMbtMeEllipse(const vpMbtMeEllipse &meellipse) ;
- virtual ~vpMbtMeEllipse() ;
+ vpMbtMeEllipse();
+ vpMbtMeEllipse(const vpMbtMeEllipse &meellipse);
+ virtual ~vpMbtMeEllipse();
- void computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures);
+ void computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad, unsigned int &_nbFeatures);
- void display(const vpImage<unsigned char>&I, vpColor col) ;
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
+ void display(const vpImage<unsigned char> &I, vpColor col);
+ void display(const vpImage<unsigned char> &I)
+ {
+ vpMeTracker::display(I);
+ } // Shouldn't be here since it's already in vpMeTracker
/*!
\return Expected number of moving edges to track along the ellipse.
*/
- int getExpectedDensity() {return (int)expecteddensity;};
+ int getExpectedDensity() { return (int)expecteddensity; }
/*!
Gets the 2 order central moment \f$ \mu_{11} \f$.
-
+
\return the value of \f$ \mu_{11} \f$.
*/
- inline double get_mu11() const {return mu11;}
-
+ inline double get_mu11() const { return mu11; }
+
/*!
Gets the 2 order central moment \f$ \mu_{02} \f$.
-
+
\return the value of \f$ \mu_{02} \f$.
*/
- inline double get_mu02() const {return mu02;}
-
+ inline double get_mu02() const { return mu02; }
+
/*!
Gets the 2 order central moment \f$ \mu_{20} \f$.
-
+
\return the value of \f$ \mu_{20} \f$.
*/
- inline double get_mu20() const {return mu20;}
-
+ inline double get_mu20() const { return mu20; }
+
/*!
Gets the center of the ellipse.
*/
- inline vpImagePoint getCenter() const {return iPc; }
-
+ inline vpImagePoint getCenter() const { return iPc; }
+
/*!
Gets the semiminor axis of the ellipse.
*/
- inline double getA() const {return a; }
-
+ inline double getA() const { return a; }
+
/*!
Gets the semimajor axis of the ellipse.
*/
- inline double getB() const {return b; }
-
+ inline double getB() const { return b; }
+
/*!
- Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+ Gets the angle made by the major axis and the i axis of the image frame
+ \f$ (i,j) \f$
*/
- inline double getE() const {return e; }
-
+ inline double getE() const { return e; }
+
/*!
Gets the parameters a, b, e of the ellipse.
*/
- void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
-
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p) ;
+ void getEquationParam(double &A, double &B, double &E)
+ {
+ A = a;
+ B = b;
+ E = e;
+ }
- void track(const vpImage<unsigned char>& Im);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p,
+ double mu02_p);
- void updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p);
+ void track(const vpImage<unsigned char> &Im);
+ void updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p,
+ double mu02_p);
protected:
//! The coordinates of the ellipse center.
@@ -215,7 +232,8 @@ protected:
double a;
//! \f$ b \f$ is the semimajor axis of the ellipse.
double b;
- //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
+ //! \f$ e \f$ is the angle made by the major axis and the i axis of the
+ //! image frame \f$ (i,j) \f$.
double e;
protected:
@@ -225,17 +243,17 @@ protected:
double se;
//! Second order central moments
- double mu11,mu20, mu02;
+ double mu11, mu20, mu02;
//! Threshold for the robust least square.
double thresholdWeight;
//! Expected number of me to track along the ellipse.
double expecteddensity;
private:
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
+ void sample(const vpImage<unsigned char> &image);
+ void reSample(const vpImage<unsigned char> &I);
void updateTheta();
- void suppressPoints() ;
+ void suppressPoints();
};
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h
index 5df4400..83e5340 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtMeLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -59,69 +60,75 @@
*/
class VISP_EXPORT vpMbtMeLine : public vpMeTracker
{
- private:
- vpMeSite PExt[2] ;
- double rho, theta, theta_1;
- double delta ,delta_1;
- int sign;
- double a,b,c;
-
- public:
- int imin, imax;
- int jmin, jmax;
- double expecteddensity;
-
- public:
- vpMbtMeLine();
- ~vpMbtMeLine();
-
- void computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures);
-
- void display(const vpImage<unsigned char>& /*I*/, vpColor /*col*/) {;}
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The a coefficient of the moving edge
- */
- inline double get_a() const { return this->a;}
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The b coefficient of the moving edge
- */
- inline double get_b() const { return this->b;}
-
- /*!
- Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
-
- \return : The c coefficient of the moving edge
- */
- inline double get_c() const { return this->c;}
-
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho, double theta);
-
- void track(const vpImage<unsigned char> &I);
-
- void updateParameters(const vpImage<unsigned char> &I, double rho, double theta);
- void updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2, double rho, double theta);
-
- private:
- void bubbleSortI();
- void bubbleSortJ();
- void findSignal(const vpImage<unsigned char>& I, const vpMe *me, double *conv);
- void sample(const vpImage<unsigned char>&image);
- void seekExtremities(const vpImage<unsigned char> &I);
- void setExtremities();
- void suppressPoints(const vpImage<unsigned char> &I);
- void reSample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char>&image, vpImagePoint ip1, vpImagePoint ip2);
- void updateDelta();
-} ;
+private:
+ vpMeSite PExt[2];
+ double rho, theta, theta_1;
+ double delta, delta_1;
+ int sign;
+ double a, b, c;
+
+public:
+ int imin, imax;
+ int jmin, jmax;
+ double expecteddensity;
+
+public:
+ vpMbtMeLine();
+ ~vpMbtMeLine();
+
+ void computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad, unsigned int &_nbFeatures);
+
+ void display(const vpImage<unsigned char> & /*I*/, vpColor /*col*/) { ; }
+ void display(const vpImage<unsigned char> &I)
+ {
+ vpMeTracker::display(I);
+ } // Shouldn't be here since it's already in vpMeTracker
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j
+ \; sin(\theta) - \rho = 0 \f$
+
+ \return : The a coefficient of the moving edge
+ */
+ inline double get_a() const { return this->a; }
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j
+ \; sin(\theta) - \rho = 0 \f$
+
+ \return : The b coefficient of the moving edge
+ */
+ inline double get_b() const { return this->b; }
+
+ /*!
+ Get the a coefficient of the line corresponding to \f$ i \; cos(\theta) + j
+ \; sin(\theta) - \rho = 0 \f$
+
+ \return : The c coefficient of the moving edge
+ */
+ inline double get_c() const { return this->c; }
+
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho,
+ double theta);
+
+ void track(const vpImage<unsigned char> &I);
+
+ void updateParameters(const vpImage<unsigned char> &I, double rho, double theta);
+ void updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2, double rho,
+ double theta);
+
+private:
+ void bubbleSortI();
+ void bubbleSortJ();
+ void sample(const vpImage<unsigned char> &image);
+ void seekExtremities(const vpImage<unsigned char> &I);
+ void setExtremities();
+ void suppressPoints(const vpImage<unsigned char> &I);
+ void reSample(const vpImage<unsigned char> &image);
+ void reSample(const vpImage<unsigned char> &image, const vpImagePoint &ip1, const vpImagePoint &ip2);
+ void updateDelta();
+};
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
#endif
-
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h
index 93f82a4..3a5ca3f 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtPolygon.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,17 +47,18 @@
#ifndef vpMbtPolygon_HH
#define vpMbtPolygon_HH
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpPolygon3D.h>
#include <vector>
/*!
\class vpMbtPolygon
-
- \brief Implementation of a polygon of the model used by the model-based tracker.
+
+ \brief Implementation of a polygon of the model used by the model-based
+ tracker.
\ingroup group_mbt_faces
@@ -64,104 +66,104 @@
class VISP_EXPORT vpMbtPolygon : public vpPolygon3D
{
public:
- //! Index of the polygon. Cannot be unsigned int because default value is -1.
+ //! Index of the polygon. Cannot be unsigned int because default value is
+ //! -1.
int index;
//! flag to specify whether the face is visible or not
bool isvisible;
//! flag to specify whether the face is appearing or not
bool isappearing;
- //! Flag to specify if the visibility of the polygon depends also of the current level of detail (LOD)
+ //! Flag to specify if the visibility of the polygon depends also of the
+ //! current level of detail (LOD)
bool useLod;
- //! Threshold for minimum line length in pixel to consider if the line is visible or not in LOD case
+ //! Threshold for minimum line length in pixel to consider if the line is
+ //! visible or not in LOD case
double minLineLengthThresh;
- //! Threshold for minimum polygon area in pixel to consider if the polygon is visible or not in LOD case
+ //! Threshold for minimum polygon area in pixel to consider if the polygon
+ //! is visible or not in LOD case
double minPolygonAreaThresh;
//! Name of the polygon
std::string name;
- //! Boolean that specify if the polygon has an orientation or not (mainly used for cylinders)
+ //! Boolean that specify if the polygon has an orientation or not (mainly
+ //! used for cylinders)
bool hasOrientation;
-public:
- vpMbtPolygon() ;
- vpMbtPolygon(const vpMbtPolygon& mbtp) ;
- virtual ~vpMbtPolygon() ;
-
- /*!
- Get the index of the face.
+public:
+ vpMbtPolygon();
+ vpMbtPolygon(const vpMbtPolygon &mbtp);
+ virtual ~vpMbtPolygon();
- \return index : the index of the face.
- */
- inline int getIndex() const {return index ;}
+ /*!
+ Get the index of the face.
+
+ \return index : the index of the face.
+ */
+ inline int getIndex() const { return index; }
/*!
Get the name of the face.
\return Name of the face.
*/
- inline std::string getName() const {return name;}
+ inline std::string getName() const { return name; }
- inline bool isAppearing() const {return isappearing;}
- inline bool isPolygonOriented() { return hasOrientation; }
- virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo = false,
- const vpCameraParameters &cam = vpCameraParameters(), const vpImage<unsigned char> &I = vpImage<unsigned char>());
- bool isVisible() const {return isvisible;}
+ inline bool isAppearing() const { return isappearing; }
+ inline bool isPolygonOriented() { return hasOrientation; }
+ virtual bool isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo = false,
+ const vpCameraParameters &cam = vpCameraParameters(),
+ const vpImage<unsigned char> &I = vpImage<unsigned char>());
+ bool isVisible() const { return isvisible; }
+
+ vpMbtPolygon &operator=(const vpMbtPolygon &mbtp);
- vpMbtPolygon& operator=(const vpMbtPolygon& mbtp) ;
-
/*!
Set the index of the face.
\param i : the new index of the face.
*/
- virtual inline void setIndex(const int i ) { index = i ; }
+ virtual inline void setIndex(const int i) { index = i; }
- // Due to a doxygen warning include the sample code in the doc, we remove the inline and put the doc in the *.cpp file
- void setLod(const bool use_lod);
+ // Due to a doxygen warning include the sample code in the doc, we remove
+ // the inline and put the doc in the *.cpp file
+ void setLod(const bool use_lod);
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD
- (level of detail) case. This threshold is only used when setLoD() is turned on.
+ Set the threshold for the minimum line length to be considered as visible
+ in the LOD (level of detail) case. This threshold is only used when
+ setLoD() is turned on.
- \param min_line_length : threshold for the minimum line length in pixel. When a single line that doesn't
- belong to a face is considered by the tracker, this line is tracked only if its lenght in pixel is
- greater than \e min_line_length.
+ \param min_line_length : threshold for the minimum line length in pixel.
+ When a single line that doesn't belong to a face is considered by the
+ tracker, this line is tracked only if its lenght in pixel is greater than
+ \e min_line_length.
\sa setLoD()
*/
- inline void setMinLineLengthThresh(const double min_line_length) {
- this->minLineLengthThresh = min_line_length;
- }
+ 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.
+ Set the minimum polygon area to be considered as visible in the LOD (level
+ of detail) case. This threshold is only used when setLoD() is turned on.
- \param min_polygon_area : threshold for the minimum polygon area in pixel. When a face
- is considered by the tracker, this face is tracked only if its area in pixel is
- greater than \e min_polygon_area.
+ \param min_polygon_area : threshold for the minimum polygon area in pixel.
+ When a face is considered by the tracker, this face is tracked only if its
+ area in pixel is greater than \e min_polygon_area.
\sa setLoD()
*/
- inline void setMinPolygonAreaThresh(const double min_polygon_area) {
- this->minPolygonAreaThresh = min_polygon_area;
- }
+ 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
new file mode 100644
index 0000000..c81f495
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtTukeyEstimator.h
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Tukey M-estimator.
+ *
+ *****************************************************************************/
+
+#ifndef __vpMbtTukeyEstimator_h_
+#define __vpMbtTukeyEstimator_h_
+
+#include <vector>
+#include <visp3/core/vpColVector.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+template <typename T> class VISP_EXPORT vpMbtTukeyEstimator
+{
+public:
+ void MEstimator(const std::vector<T> &residues, std::vector<T> &weights, const T NoiseThreshold);
+ void MEstimator(const vpColVector &residues, vpColVector &weights, const double NoiseThreshold);
+
+private:
+ T getMedian(std::vector<T> &vec);
+ void MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights, const T NoiseThreshold);
+ void MEstimator_impl_ssse3(const std::vector<T> &residues, std::vector<T> &weights, const T NoiseThreshold);
+ void psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights);
+ void psiTukey(const T sig, std::vector<T> &x, vpColVector &weights);
+
+ std::vector<T> m_normres;
+ std::vector<T> m_residues;
+};
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h
new file mode 100644
index 0000000..a8597ac
--- /dev/null
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlGenericParser.h
@@ -0,0 +1,563 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Load XML Parameter for Model Based Tracker.
+ *
+ *****************************************************************************/
+
+/*!
+ * \file vpMbtXmlGenericParser.h
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ * object.
+ */
+
+#ifndef __vpMbtXmlGenericParser_h_
+#define __vpMbtXmlGenericParser_h_
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <libxml/xmlmemory.h>
+
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpXmlParser.h>
+#include <visp3/mbt/vpMbtFaceDepthNormal.h>
+#include <visp3/me/vpMe.h>
+
+/*!
+ \class vpMbtXmlGenericParser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ object. \ingroup group_mbt_xml_parser
+
+ Data parser for the model-based tracker.
+
+ */
+class VISP_EXPORT 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. */
+ 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
+ depth dense features. */
+ };
+
+protected:
+ //! Parser type
+ vpParserType m_parserType;
+ //! Camera parameters.
+ vpCameraParameters m_cam;
+ //! Angle to determine if a face appeared
+ double m_angleAppear;
+ //! Angle to determine if a face disappeared
+ double m_angleDisappear;
+ //! Is near clipping distance specified?
+ bool m_hasNearClipping;
+ //! Near clipping distance
+ double m_nearClipping;
+ //! Is far clipping distance specified?
+ bool m_hasFarClipping;
+ //! Near clipping distance
+ double m_farClipping;
+ //! Fov Clipping
+ bool m_fovClipping;
+ // LOD
+ //! If true, the LOD is enabled, otherwise it is not
+ bool m_useLod;
+ //! Minimum line length to track a segment when LOD is enabled
+ double m_minLineLengthThreshold;
+ //! Minimum polygon area to track a face when LOD is enabled
+ double m_minPolygonAreaThreshold;
+ // Edge
+ //! Moving edges parameters.
+ vpMe m_ecm;
+ // KLT
+ //! Border of the mask used on Klt points
+ unsigned int m_kltMaskBorder;
+ //! Maximum of Klt features
+ unsigned int m_kltMaxFeatures;
+ //! Windows size
+ unsigned int m_kltWinSize;
+ //! Quality of the Klt points
+ double m_kltQualityValue;
+ //! Minimum distance between klt points
+ double m_kltMinDist;
+ //! Harris free parameters
+ double m_kltHarrisParam;
+ //! Block size
+ unsigned int m_kltBlockSize;
+ //! Number of pyramid levels
+ unsigned int m_kltPyramidLevels;
+ // Depth normal
+ //! Feature estimation method
+ vpMbtFaceDepthNormal::vpFeatureEstimationType m_depthNormalFeatureEstimationMethod;
+ //! PCL plane estimation method
+ int m_depthNormalPclPlaneEstimationMethod;
+ //! PCL RANSAC maximum number of iterations
+ int m_depthNormalPclPlaneEstimationRansacMaxIter;
+ //! PCL RANSAC threshold
+ double m_depthNormalPclPlaneEstimationRansacThreshold;
+ //! Sampling step in X
+ unsigned int m_depthNormalSamplingStepX;
+ //! Sampling step in Y
+ unsigned int m_depthNormalSamplingStepY;
+ // Depth dense
+ //! Sampling step in X
+ unsigned int m_depthDenseSamplingStepX;
+ //! Sampling step in Y
+ unsigned int m_depthDenseSamplingStepY;
+
+ enum vpDataToParseMb {
+ //<conf>
+ conf,
+ //<face>
+ face,
+ angle_appear,
+ angle_disappear,
+ near_clipping,
+ far_clipping,
+ fov_clipping,
+ //<camera>
+ camera,
+ height,
+ width,
+ u0,
+ v0,
+ px,
+ py,
+ lod,
+ use_lod,
+ min_line_length_threshold,
+ min_polygon_area_threshold,
+ //<ecm>
+ ecm,
+ mask,
+ size,
+ nb_mask,
+ range,
+ tracking,
+ contrast,
+ edge_threshold,
+ mu1,
+ mu2,
+ sample,
+ step,
+ //<klt>
+ klt,
+ mask_border,
+ max_features,
+ window_size,
+ quality,
+ min_distance,
+ harris,
+ size_block,
+ pyramid_lvl,
+ //<depth_normal>
+ depth_normal,
+ feature_estimation_method,
+ PCL_plane_estimation,
+ PCL_plane_estimation_method,
+ PCL_plane_estimation_ransac_max_iter,
+ PCL_plane_estimation_ransac_threshold,
+ depth_sampling_step,
+ depth_sampling_step_X,
+ depth_sampling_step_Y,
+ //<depth_dense>
+ depth_dense,
+ depth_dense_sampling_step,
+ depth_dense_sampling_step_X,
+ depth_dense_sampling_step_Y
+ };
+
+public:
+ /** @name Public Member Functions Inherited from vpMbtXmlGenericParser */
+ //@{
+ explicit vpMbtXmlGenericParser(const vpParserType &type = EDGE_PARSER);
+ virtual ~vpMbtXmlGenericParser();
+
+ /*!
+ Get the angle to determine if a face appeared.
+ */
+ inline double getAngleAppear() const { return m_angleAppear; }
+
+ /*!
+ Get the angle to determine if a face disappeared.
+ */
+ inline double getAngleDisappear() const { return m_angleDisappear; }
+
+ void getCameraParameters(vpCameraParameters &_cam) const { _cam = m_cam; }
+
+ /*!
+ Get moving edge parameters.
+ */
+ void getEdgeMe(vpMe &_ecm) const { _ecm = m_ecm; }
+
+ /*!
+ Get depth dense sampling step in X.
+ */
+ inline unsigned int getDepthDenseSamplingStepX() const { return m_depthDenseSamplingStepX; }
+
+ /*!
+ Get depth dense sampling step in Y.
+ */
+ inline unsigned int getDepthDenseSamplingStepY() const { return m_depthDenseSamplingStepY; }
+
+ /*!
+ Get depth normal feature estimation method.
+ */
+ vpMbtFaceDepthNormal::vpFeatureEstimationType getDepthNormalFeatureEstimationMethod() const
+ {
+ return m_depthNormalFeatureEstimationMethod;
+ }
+
+ /*!
+ Get depth normal PCL plane estimation method.
+ */
+ inline int getDepthNormalPclPlaneEstimationMethod() const { return m_depthNormalPclPlaneEstimationMethod; }
+
+ /*!
+ Get depth normal PCL maximum number of iterations.
+ */
+ inline int getDepthNormalPclPlaneEstimationRansacMaxIter() const
+ {
+ return m_depthNormalPclPlaneEstimationRansacMaxIter;
+ }
+
+ /*!
+ Get depth normal PCL RANSAC threshold.
+ */
+ inline double getDepthNormalPclPlaneEstimationRansacThreshold() const
+ {
+ return m_depthNormalPclPlaneEstimationRansacThreshold;
+ }
+
+ /*!
+ Get depth normal sampling step in X.
+ */
+ inline unsigned int getDepthNormalSamplingStepX() const { return m_depthNormalSamplingStepX; }
+
+ /*!
+ Get depth normal sampling step in Y.
+ */
+ inline unsigned int getDepthNormalSamplingStepY() const { return m_depthNormalSamplingStepY; }
+
+ /*!
+ Get the far clipping distance.
+ */
+ inline double getFarClippingDistance() const { return m_farClipping; }
+
+ /*!
+ Get if FOV clipping should be used or not.
+ */
+ inline bool getFovClipping() const { return m_fovClipping; }
+
+ /*!
+ Get the size of a block.
+ */
+ inline unsigned int getKltBlockSize() const { return m_kltBlockSize; }
+
+ /*!
+ Get the Harris free parameter.
+ */
+ inline double getKltHarrisParam() const { return m_kltHarrisParam; }
+
+ /*!
+ Get the Border of the mask.
+ */
+ inline unsigned int getKltMaskBorder() const { return m_kltMaskBorder; }
+
+ /*!
+ Get the maximum number of features for the KLT.
+ */
+ inline unsigned int getKltMaxFeatures() const { return m_kltMaxFeatures; }
+
+ /*!
+ Get the minimum distance between KLT points.
+ */
+ inline double getKltMinDistance() const { return m_kltMinDist; }
+
+ /*!
+ Get the number of pyramid levels
+ */
+ inline unsigned int getKltPyramidLevels() const { return m_kltPyramidLevels; }
+
+ /*!
+ Get the quality of the KLT.
+ */
+ inline double getKltQuality() const { return m_kltQualityValue; }
+
+ /*!
+ Get the size of the window used in the KLT tracker.
+ */
+ inline unsigned int getKltWindowSize() const { return m_kltWinSize; }
+
+ /*!
+ Get the state of LOD setting.
+ */
+ inline bool getLodState() const { return m_useLod; }
+
+ /*!
+ Get the minimum line length to track a segment when LOD is enabled.
+ */
+ 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; }
+
+ /*!
+ Get the near clipping distance.
+ */
+ inline double getNearClippingDistance() const { return m_nearClipping; }
+
+ /*!
+ Has Far clipping been specified?
+
+ \return True if yes, False otherwise.
+ */
+ inline bool hasFarClippingDistance() const { return m_hasFarClipping; }
+
+ /*!
+ Has Near clipping been specified?
+
+ \return True if yes, False otherwise.
+ */
+ inline bool hasNearClippingDistance() const { return m_hasNearClipping; }
+
+ void parse(const std::string &filename);
+
+ virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
+
+ /*!
+ Set the angle to determine if a face appeared.
+
+ \param aappear : New angleAppear
+ */
+ 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; }
+
+ /*!
+ Set camera parameters.
+
+ \param _cam : New camera parameters
+ */
+ 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; }
+
+ /*!
+ Set depth dense sampling step in Y.
+
+ \param stepY : New sampling step
+ */
+ 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)
+ {
+ m_depthNormalFeatureEstimationMethod = method;
+ }
+
+ /*!
+ Set depth normal PCL plane estimation method.
+
+ \param method : New PCL plane estimation method
+ */
+ inline void setDepthNormalPclPlaneEstimationMethod(const int method)
+ {
+ m_depthNormalPclPlaneEstimationMethod = method;
+ }
+
+ /*!
+ Set depth normal PCL RANSAC maximum number of iterations.
+
+ \param maxIter : New maximum number of iterations
+ */
+ inline void setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
+ {
+ m_depthNormalPclPlaneEstimationRansacMaxIter = maxIter;
+ }
+
+ /*!
+ Set depth normal PCL RANSAC threshold.
+
+ \param threshold : New RANSAC threshold
+ */
+ inline void setDepthNormalPclPlaneEstimationRansacThreshold(const double threshold)
+ {
+ m_depthNormalPclPlaneEstimationRansacThreshold = threshold;
+ }
+
+ /*!
+ Set depth normal sampling step in X.
+
+ \param stepX : New sampling step
+ */
+ 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; }
+
+ /*!
+ Set moving edge parameters.
+
+ \param _ecm : New moving edge parameters
+ */
+ inline void setEdgeMe(const vpMe &_ecm) { m_ecm = _ecm; }
+
+ /*!
+ Set the far clipping distance.
+
+ \param fclip : New farClipping
+ */
+ inline void setFarClippingDistance(const double &fclip) { m_farClipping = fclip; }
+
+ /*!
+ Set the size of a block.
+
+ \param bs : New blockSize
+ */
+ inline void setKltBlockSize(const unsigned int &bs) { m_kltBlockSize = bs; }
+
+ /*!
+ Set the Harris free parameter.
+
+ \param hp : New harrisParam
+ */
+ inline void setKltHarrisParam(const double &hp) { m_kltHarrisParam = hp; }
+
+ /*!
+ Set the Border of the mask.
+
+ \param mb = new maskBorder
+ */
+ 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; }
+
+ /*!
+ Set the minimum distance between KLT points.
+
+ \param mD : New minDist
+ */
+ inline void setKltMinDistance(const double &mD) { m_kltMinDist = mD; }
+
+ /*!
+ Set the number of pyramid levels
+
+ \param pL : New pyramidLevels
+ */
+ inline void setKltPyramidLevels(const unsigned int &pL) { m_kltPyramidLevels = pL; }
+
+ /*!
+ Set the quality of the KLT.
+
+ \param q : New quality
+ */
+ inline void setKltQuality(const double &q) { m_kltQualityValue = q; }
+
+ /*!
+ Set the size of the window used in the KLT tracker.
+
+ \param w : New winSize
+ */
+ inline void setKltWindowSize(const unsigned int &w) { m_kltWinSize = w; }
+
+ /*!
+ Set the near clipping distance.
+
+ \param nclip : New nearClipping
+ */
+ inline void setNearClippingDistance(const double &nclip) { m_nearClipping = nclip; }
+
+ void writeMainClass(xmlNodePtr node);
+ //@}
+
+protected:
+ void init();
+
+ void read_camera(xmlDocPtr doc, xmlNodePtr node);
+ void read_face(xmlDocPtr doc, xmlNodePtr node);
+ void read_lod(xmlDocPtr doc, xmlNodePtr node);
+
+ // Edge
+ void read_ecm(xmlDocPtr doc, xmlNodePtr node);
+ void read_ecm_sample(xmlDocPtr doc, xmlNodePtr node);
+ void read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node);
+ void read_ecm_mask(xmlDocPtr doc, xmlNodePtr node);
+ void read_ecm_range(xmlDocPtr doc, xmlNodePtr node);
+ void read_ecm_contrast(xmlDocPtr doc, xmlNodePtr node);
+
+ // KLT
+ void read_klt(xmlDocPtr doc, xmlNodePtr node);
+
+ // Depth normal
+ void read_depth_normal(xmlDocPtr doc, xmlNodePtr node);
+ void read_depth_normal_PCL(xmlDocPtr doc, xmlNodePtr node);
+ void read_depth_normal_sampling_step(xmlDocPtr doc, xmlNodePtr node);
+
+ // Depth dense
+ void read_depth_dense(xmlDocPtr doc, xmlNodePtr node);
+ void read_depth_dense_sampling_step(xmlDocPtr doc, xmlNodePtr node);
+};
+
+#endif
+
+#endif
diff --git a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h
index 5389371..162e00e 100644
--- a/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h
+++ b/modules/tracker/mbt/include/visp3/mbt/vpMbtXmlParser.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,8 +41,9 @@
/*!
* \file vpMbtXmlParser.h
- * \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
-*/
+ * \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ * object.
+ */
#ifndef vpMbtXmlParser_HH
#define vpMbtXmlParser_HH
@@ -50,23 +52,23 @@
#ifdef VISP_HAVE_XML2
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbXmlParser.h>
#include <visp3/me/vpMe.h>
/*!
\class vpMbtXmlParser
- \brief Parse an Xml file to extract configuration parameters of a mbtConfig object.
- \ingroup group_mbt_xml_parser
+ \brief Parse an Xml file to extract configuration parameters of a mbtConfig
+ object. \ingroup group_mbt_xml_parser
Data parser for the model based tracker.
*/
-class VISP_EXPORT vpMbtXmlParser: virtual public vpMbXmlParser
+class VISP_EXPORT vpMbtXmlParser : virtual public vpMbXmlParser
{
protected:
- typedef enum{
+ typedef enum {
ecm = vpMbXmlParser::last,
mask,
size,
@@ -85,26 +87,25 @@ protected:
//! Moving edges parameters.
vpMe m_ecm;
-
public:
/** @name Public Member Functions Inherited from vpMbtXmlParser */
//@{
vpMbtXmlParser();
virtual ~vpMbtXmlParser();
- void getMe(vpMe& _ecm) const { _ecm = this->m_ecm;}
+ void getMe(vpMe &_ecm) const { _ecm = this->m_ecm; }
- void parse(const char * filename);
+ void parse(const char *filename);
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
- void read_ecm (xmlDocPtr doc, xmlNodePtr node);
- void read_sample (xmlDocPtr doc, xmlNodePtr node);
- void read_sample_deprecated (xmlDocPtr doc, xmlNodePtr node);
- void read_mask (xmlDocPtr doc, xmlNodePtr node);
- void read_range (xmlDocPtr doc, xmlNodePtr node);
- void read_contrast (xmlDocPtr doc, xmlNodePtr node);
-
- void setMovingEdge(const vpMe &_ecm){ m_ecm = _ecm; }
+ void read_ecm(xmlDocPtr doc, xmlNodePtr node);
+ void read_sample(xmlDocPtr doc, xmlNodePtr node);
+ void read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node);
+ void read_mask(xmlDocPtr doc, xmlNodePtr node);
+ void read_range(xmlDocPtr doc, xmlNodePtr node);
+ void read_contrast(xmlDocPtr doc, xmlNodePtr node);
+
+ void setMovingEdge(const vpMe &_ecm) { m_ecm = _ecm; }
void writeMainClass(xmlNodePtr node);
//@}
@@ -114,13 +115,8 @@ protected:
//@{
void init();
//@}
-
-
};
#endif
#endif /* NMBTXMLPARSER_H_ */
-
-
-
diff --git a/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp
new file mode 100644
index 0000000..2e6524d
--- /dev/null
+++ b/modules/tracker/mbt/src/depth/vpMbDepthDenseTracker.cpp
@@ -0,0 +1,722 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Model-based tracker using depth dense features.
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/point_cloud.h>
+#endif
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbDepthDenseTracker.h>
+#include <visp3/mbt/vpMbtXmlGenericParser.h>
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#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()
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ m_debugDisp_depthDense(NULL), m_debugImage_depthDense()
+#endif
+{
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Depth Dense");
+#endif
+
+#if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_DENSE
+ m_debugDisp_depthDense = new vpDisplayX;
+#elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_DENSE
+ m_debugDisp_depthDense = new vpDisplayGDI;
+#endif
+}
+
+vpMbDepthDenseTracker::~vpMbDepthDenseTracker()
+{
+ for (size_t i = 0; i < m_depthDenseNormalFaces.size(); i++) {
+ delete m_depthDenseNormalFaces[i];
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ delete m_debugDisp_depthDense;
+#endif
+}
+
+void vpMbDepthDenseTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose)
+{
+ if (polygon.nbpt < 3) {
+ return;
+ }
+
+ // Copy hidden faces
+ m_depthDenseHiddenFacesDisplay = faces;
+
+ vpMbtFaceDepthDense *normal_face = new vpMbtFaceDepthDense;
+ normal_face->m_hiddenFace = &faces;
+ normal_face->m_polygon = &polygon;
+ normal_face->m_cam = cam;
+ normal_face->m_useScanLine = useScanLine;
+ normal_face->m_clippingFlag = clippingFlag;
+ normal_face->m_distNearClip = distNearClip;
+ normal_face->m_distFarClip = distFarClip;
+
+ // 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_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());
+ }
+ }
+
+ // Construct a vpPlane in object frame
+ vpPoint pts[3];
+ 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);
+
+ m_depthDenseNormalFaces.push_back(normal_face);
+}
+
+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);
+
+ if (useScanLine) {
+ // if (clippingFlag <= 2) {
+ // cam.computeFov(m_depthDenseI_dummyVisibility.getWidth(),
+ // m_depthDenseI_dummyVisibility.getHeight());
+ // }
+
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, m_depthDenseI_dummyVisibility.getWidth(),
+ m_depthDenseI_dummyVisibility.getHeight());
+ }
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face_normal = *it;
+ face_normal->computeVisibility();
+ }
+}
+
+void vpMbDepthDenseTracker::computeVVS()
+{
+ double normRes = 0;
+ double normRes_1 = -1;
+ unsigned int iter = 0;
+
+ computeVVSInit();
+
+ vpColVector error_prev(m_denseDepthNbFeatures);
+ vpMatrix LTL;
+ vpColVector LTR, v;
+
+ double mu = m_initialMu;
+ vpHomogeneousMatrix cMo_prev;
+
+ bool isoJoIdentity_ = true;
+ vpVelocityTwistMatrix cVo;
+ vpMatrix L_true, LVJ_true;
+
+ 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);
+
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
+
+ if (computeCovariance) {
+ L_true = m_L_depthDense;
+ if (!isoJoIdentity_) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_depthDense * cVo * oJo);
+ }
+ }
+
+ // Compute DoF only once
+ if (iter == 0) {
+ isoJoIdentity_ = true;
+ oJo.eye();
+
+ // If all the 6 dof should be estimated, we check if the interaction
+ // matrix is full rank. If not we remove automatically the dof that
+ // cannot be estimated This is particularly useful when consering
+ // circles (rank 5) and cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+
+ vpMatrix K; // kernel
+ unsigned int rank = (m_L_depthDense * cVo).kernel(K);
+ if (rank == 0) {
+ throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
+ }
+
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I - K.AtA();
+
+ isoJoIdentity_ = false;
+ }
+ }
+ }
+
+ 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];
+ num += m_w_depthDense[i] * vpMath::sqr(m_error_depthDense[i]);
+ den += m_w_depthDense[i];
+
+ // weight interaction matrix
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_depthDense[i][j] *= m_w_depthDense[i];
+ }
+ }
+
+ 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;
+
+ normRes_1 = normRes;
+ normRes = sqrt(num / den);
+ }
+
+ iter++;
+ }
+
+ computeCovarianceMatrixVVS(isoJoIdentity_, m_w_depthDense, cMo_prev, L_true, LVJ_true, m_error_depthDense);
+}
+
+void vpMbDepthDenseTracker::computeVVSInit()
+{
+ m_denseDepthNbFeatures = 0;
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
+ it != m_depthDenseListOfActiveFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face = *it;
+ m_denseDepthNbFeatures += face->getNbFeatures();
+ }
+
+ m_L_depthDense.resize(m_denseDepthNbFeatures, 6, false, false);
+ m_error_depthDense.resize(m_denseDepthNbFeatures, false);
+ m_weightedError_depthDense.resize(m_denseDepthNbFeatures, false);
+
+ m_w_depthDense.resize(m_denseDepthNbFeatures, false);
+ m_w_depthDense = 1;
+}
+
+void vpMbDepthDenseTracker::computeVVSInteractionMatrixAndResidu()
+{
+ unsigned int start_index = 0;
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseListOfActiveFaces.begin();
+ it != m_depthDenseListOfActiveFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face = *it;
+
+ vpMatrix L_face;
+ vpColVector error;
+
+ face->computeInteractionMatrixAndResidu(cMo, L_face, error);
+
+ m_error_depthDense.insert(start_index, error);
+ m_L_depthDense.insert(L_face, start_index, 0);
+
+ start_index += error.getRows();
+ }
+}
+
+void vpMbDepthDenseTracker::computeVVSWeights()
+{
+ m_robust_depthDense.MEstimator(m_error_depthDense, m_w_depthDense, 1e-3);
+}
+
+void vpMbDepthDenseTracker::display(const vpImage<unsigned char> &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);
+
+ if (useScanLine) {
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ m_depthDenseHiddenFacesDisplay.computeClippedPolygons(cMo_, c);
+ m_depthDenseHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight());
+ }
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face_normal = *it;
+ face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
+
+ if (displayFeatures) {
+ face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
+ }
+ }
+}
+
+void vpMbDepthDenseTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ vpCameraParameters c = cam_;
+
+ bool changed = false;
+ vpImage<unsigned char> I_dummy;
+ vpImageConvert::convert(I, I_dummy);
+ 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());
+ }
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face_normal = *it;
+ face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
+
+ if (displayFeatures) {
+ face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
+ }
+ }
+}
+
+void vpMbDepthDenseTracker::init(const vpImage<unsigned char> &I)
+{
+ if (!modelInitialised) {
+ throw vpException(vpException::fatalError, "model not initialized");
+ }
+
+ bool reInitialisation = false;
+ if (!useOgre) {
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ } else {
+#ifdef VISP_HAVE_OGRE
+ if (!faces.isOgreInitialised()) {
+ faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
+ faces.setOgreShowConfigDialog(ogreShowConfigDialog);
+ faces.initOgre(cam);
+ // Turn off Ogre config dialog display for the next call to this
+ // function since settings are saved in the ogre.cfg file and used
+ // during the next call
+ ogreShowConfigDialog = false;
+ }
+
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#else
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+
+ if (useScanLine || clippingFlag > 3)
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ computeVisibility(I.getWidth(), I.getHeight());
+}
+
+void vpMbDepthDenseTracker::loadConfigFile(const std::string &configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::DEPTH_DENSE_PARSER);
+
+ xmlp.setCameraParameters(cam);
+ xmlp.setAngleAppear(vpMath::deg(angleAppears));
+ xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
+
+ xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
+ xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
+
+ try {
+ 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());
+ }
+
+ vpCameraParameters camera;
+ xmlp.getCameraParameters(camera);
+ setCameraParameters(camera);
+
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ if (xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if (xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if (xmlp.getFovClipping())
+ setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);
+
+ setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
+#else
+ std::cerr << "You need the libXML2 to read the config file " << configFile << std::endl;
+#endif
+}
+
+void vpMbDepthDenseTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
+{
+ cMo.eye();
+
+ for (size_t i = 0; i < m_depthDenseNormalFaces.size(); i++) {
+ delete m_depthDenseNormalFaces[i];
+ m_depthDenseNormalFaces[i] = NULL;
+ }
+
+ m_depthDenseNormalFaces.clear();
+
+ loadModel(cad_name, verbose);
+ initFromPose(I, cMo_);
+}
+
+#if defined(VISP_HAVE_PCL)
+void vpMbDepthDenseTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose)
+{
+ vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
+ reInitModel(I_dummy, cad_name, cMo_, verbose);
+}
+
+#endif
+
+void vpMbDepthDenseTracker::resetTracker()
+{
+ cMo.eye();
+
+ for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *normal_face = *it;
+ delete normal_face;
+ normal_face = NULL;
+ }
+
+ m_depthDenseNormalFaces.clear();
+
+ m_computeInteraction = true;
+ computeCovariance = false;
+
+ angleAppears = vpMath::rad(89);
+ angleDisappears = vpMath::rad(89);
+
+ clippingFlag = vpPolygon3D::NO_CLIPPING;
+
+ m_lambda = 1.0;
+ m_maxIter = 30;
+
+ faces.reset();
+
+ m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
+
+ useScanLine = false;
+
+#ifdef VISP_HAVE_OGRE
+ useOgre = false;
+#endif
+
+ m_depthDenseListOfActiveFaces.clear();
+}
+
+void vpMbDepthDenseTracker::setOgreVisibilityTest(const bool &v)
+{
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Depth Dense");
+#endif
+}
+
+void vpMbDepthDenseTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
+{
+ cMo = cdMo;
+ init(I);
+}
+
+#if defined(VISP_HAVE_PCL)
+void vpMbDepthDenseTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ const vpHomogeneousMatrix &cdMo)
+{
+ vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
+ cMo = cdMo;
+ init(I_dummy);
+}
+#endif
+
+void vpMbDepthDenseTracker::setScanLineVisibilityTest(const bool &v)
+{
+ vpMbTracker::setScanLineVisibilityTest(v);
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setScanLineVisibilityTest(v);
+ }
+}
+
+void vpMbDepthDenseTracker::testTracking() {}
+
+#ifdef VISP_HAVE_PCL
+void vpMbDepthDenseTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::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_debugImage_depthDense = 0;
+ std::vector<std::vector<vpImagePoint> > roiPts_vec;
+#endif
+
+ for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face = *it;
+
+ if (face->isVisible()) {
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ std::vector<std::vector<vpImagePoint> > roiPts_vec_;
+#endif
+ if (face->computeDesiredFeatures(cMo, point_cloud, m_depthDenseSamplingStepX, m_depthDenseSamplingStepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ m_debugImage_depthDense, roiPts_vec_
+#endif
+ )) {
+ m_depthDenseListOfActiveFaces.push_back(*it);
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
+#endif
+ }
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ vpDisplay::display(m_debugImage_depthDense);
+
+ for (size_t i = 0; i < roiPts_vec.size(); i++) {
+ if (roiPts_vec[i].empty())
+ 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][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
+ vpColor::red, 2);
+ }
+
+ vpDisplay::flush(m_debugImage_depthDense);
+#endif
+}
+#endif
+
+void vpMbDepthDenseTracker::segmentPointCloud(const std::vector<vpColVector> &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_debugImage_depthDense = 0;
+ std::vector<std::vector<vpImagePoint> > roiPts_vec;
+#endif
+
+ for (std::vector<vpMbtFaceDepthDense *>::iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ vpMbtFaceDepthDense *face = *it;
+
+ if (face->isVisible()) {
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ std::vector<std::vector<vpImagePoint> > roiPts_vec_;
+#endif
+ if (face->computeDesiredFeatures(cMo, width, height, point_cloud, m_depthDenseSamplingStepX,
+ m_depthDenseSamplingStepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ m_debugImage_depthDense, roiPts_vec_
+#endif
+ )) {
+ m_depthDenseListOfActiveFaces.push_back(*it);
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ roiPts_vec.insert(roiPts_vec.end(), roiPts_vec_.begin(), roiPts_vec_.end());
+#endif
+ }
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ vpDisplay::display(m_debugImage_depthDense);
+
+ for (size_t i = 0; i < roiPts_vec.size(); i++) {
+ if (roiPts_vec[i].empty())
+ 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][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
+ vpColor::red, 2);
+ }
+
+ vpDisplay::flush(m_debugImage_depthDense);
+#endif
+}
+
+void vpMbDepthDenseTracker::setCameraParameters(const vpCameraParameters &camera)
+{
+ this->cam = camera;
+
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setCameraParameters(camera);
+ }
+}
+
+void vpMbDepthDenseTracker::setDepthDenseFilteringMaxDistance(const double maxDistance)
+{
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setDepthDenseFilteringMaxDistance(maxDistance);
+ }
+}
+
+void vpMbDepthDenseTracker::setDepthDenseFilteringMethod(const int method)
+{
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setDepthDenseFilteringMethod(method);
+ }
+}
+
+void vpMbDepthDenseTracker::setDepthDenseFilteringMinDistance(const double minDistance)
+{
+ for (std::vector<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setDepthDenseFilteringMinDistance(minDistance);
+ }
+}
+
+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<vpMbtFaceDepthDense *>::const_iterator it = m_depthDenseNormalFaces.begin();
+ it != m_depthDenseNormalFaces.end(); ++it) {
+ (*it)->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
+ }
+}
+
+void vpMbDepthDenseTracker::track(const vpImage<unsigned char> &)
+{
+ throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
+}
+
+#ifdef VISP_HAVE_PCL
+void vpMbDepthDenseTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ segmentPointCloud(point_cloud);
+
+ computeVVS();
+
+ computeVisibility(point_cloud->width, point_cloud->height);
+}
+#endif
+
+void vpMbDepthDenseTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
+ const unsigned int height)
+{
+ segmentPointCloud(point_cloud, width, height);
+
+ computeVVS();
+
+ 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*/)
+{
+ 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*/)
+{
+ throw vpException(vpException::fatalError, "vpMbDepthDenseTracker::initCylinder() should not be called!");
+}
+
+void vpMbDepthDenseTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); }
+
+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
new file mode 100644
index 0000000..fba071c
--- /dev/null
+++ b/modules/tracker/mbt/src/depth/vpMbDepthNormalTracker.cpp
@@ -0,0 +1,763 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Model-based tracker using depth normal features.
+ *
+ *****************************************************************************/
+
+#include <iostream>
+
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/point_cloud.h>
+#endif
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbDepthNormalTracker.h>
+#include <visp3/mbt/vpMbtXmlGenericParser.h>
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#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()
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ m_debugDisp_depthNormal(NULL), m_debugImage_depthNormal()
+#endif
+{
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Depth");
+#endif
+
+#if defined(VISP_HAVE_X11) && DEBUG_DISPLAY_DEPTH_NORMAL
+ m_debugDisp_depthNormal = new vpDisplayX;
+#elif defined(VISP_HAVE_GDI) && DEBUG_DISPLAY_DEPTH_NORMAL
+ m_debugDisp_depthNormal = new vpDisplayGDI;
+#endif
+}
+
+vpMbDepthNormalTracker::~vpMbDepthNormalTracker()
+{
+ for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
+ delete m_depthNormalFaces[i];
+ }
+}
+
+void vpMbDepthNormalTracker::addFace(vpMbtPolygon &polygon, const bool alreadyClose)
+{
+ if (polygon.nbpt < 3) {
+ return;
+ }
+
+ // Copy hidden faces
+ m_depthNormalHiddenFacesDisplay = faces;
+
+ vpMbtFaceDepthNormal *normal_face = new vpMbtFaceDepthNormal;
+ normal_face->m_hiddenFace = &faces;
+ normal_face->m_polygon = &polygon;
+ normal_face->m_cam = cam;
+ normal_face->m_useScanLine = useScanLine;
+ 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);
+
+ // 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());
+ }
+
+ 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());
+ }
+ }
+
+ // Construct a vpPlane in object frame
+ vpPoint pts[3];
+ 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);
+
+ m_depthNormalFaces.push_back(normal_face);
+}
+
+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);
+
+ if (useScanLine) {
+ // if (clippingFlag <= 2) {
+ // cam.computeFov(m_depthNormalI_dummyVisibility.getWidth(),
+ // m_depthNormalI_dummyVisibility.getHeight());
+ // }
+
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, m_depthNormalI_dummyVisibility.getWidth(),
+ m_depthNormalI_dummyVisibility.getHeight());
+ }
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ vpMbtFaceDepthNormal *face_normal = *it;
+ face_normal->computeVisibility();
+ }
+}
+
+void vpMbDepthNormalTracker::computeVVS()
+{
+ double normRes = 0;
+ double normRes_1 = -1;
+ unsigned int iter = 0;
+
+ computeVVSInit();
+ unsigned int nb_features = (unsigned int)(3 * m_depthNormalListOfDesiredFeatures.size());
+
+ vpColVector error_prev(nb_features);
+ vpMatrix LTL;
+ vpColVector LTR, v;
+
+ double mu = m_initialMu;
+ vpHomogeneousMatrix cMo_prev;
+
+ bool isoJoIdentity_ = true;
+ vpVelocityTwistMatrix cVo;
+ vpMatrix L_true, LVJ_true;
+
+ 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);
+
+ if (!reStartFromLastIncrement) {
+ if (m_depthNormalUseRobust)
+ computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal);
+
+ if (computeCovariance) {
+ L_true = m_L_depthNormal;
+ if (!isoJoIdentity_) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_depthNormal * (cVo * oJo));
+ }
+ }
+
+ // Compute DoF only once
+ if (iter == 0) {
+ isoJoIdentity_ = true;
+ oJo.eye();
+
+ // If all the 6 dof should be estimated, we check if the interaction
+ // matrix is full rank. If not we remove automatically the dof that
+ // cannot be estimated This is particularly useful when consering
+ // circles (rank 5) and cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+
+ vpMatrix K; // kernel
+ unsigned int rank = (m_L_depthNormal * cVo).kernel(K);
+ if (rank == 0) {
+ throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
+ }
+
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I - K.AtA();
+
+ isoJoIdentity_ = false;
+ }
+ }
+ }
+
+ 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];
+ num += m_w_depthNormal[i] * vpMath::sqr(m_error_depthNormal[i]);
+ den += m_w_depthNormal[i];
+
+ // weight interaction matrix
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_depthNormal[i][j] *= m_w_depthNormal[i];
+ }
+ }
+
+ computeVVSPoseEstimation(isoJoIdentity_, iter, m_L_depthNormal, LTL, m_weightedError_depthNormal,
+ m_error_depthNormal, error_prev, LTR, mu, v);
+
+ cMo_prev = cMo;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+
+ normRes_1 = normRes;
+ normRes = sqrt(num / den);
+ }
+
+ iter++;
+ }
+
+ 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());
+
+ m_L_depthNormal.resize(nb_features, 6, false, false);
+ m_error_depthNormal.resize(nb_features, false);
+ m_weightedError_depthNormal.resize(nb_features, false);
+
+ m_w_depthNormal.resize(nb_features, false);
+ m_w_depthNormal = 1;
+
+ m_robust_depthNormal.resize(nb_features);
+ m_robust_depthNormal.setThreshold(1e-3);
+}
+
+void vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu()
+{
+ unsigned int cpt = 0;
+ for (std::vector<vpMbtFaceDepthNormal *>::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];
+
+ m_error_depthNormal.insert(cpt * 3, face_error);
+ m_L_depthNormal.insert(L_face, cpt * 3, 0);
+
+ cpt++;
+ }
+}
+
+void vpMbDepthNormalTracker::display(const vpImage<unsigned char> &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);
+
+ if (useScanLine) {
+ c.computeFov(I.getWidth(), I.getHeight());
+
+ m_depthNormalHiddenFacesDisplay.computeClippedPolygons(cMo_, c);
+ m_depthNormalHiddenFacesDisplay.computeScanLineRender(c, I.getWidth(), I.getHeight());
+ }
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ vpMbtFaceDepthNormal *face_normal = *it;
+ face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
+
+ if (displayFeatures) {
+ face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
+ }
+ }
+}
+
+void vpMbDepthNormalTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ vpCameraParameters c = cam_;
+
+ bool changed = false;
+ vpImage<unsigned char> I_dummy;
+ vpImageConvert::convert(I, I_dummy);
+ 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());
+ }
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ vpMbtFaceDepthNormal *face_normal = *it;
+ face_normal->display(I, cMo_, c, col, thickness, displayFullModel);
+
+ if (displayFeatures) {
+ face_normal->displayFeature(I, cMo_, c, 0.05, thickness);
+ }
+ }
+}
+
+void vpMbDepthNormalTracker::init(const vpImage<unsigned char> &I)
+{
+ if (!modelInitialised) {
+ throw vpException(vpException::fatalError, "model not initialized");
+ }
+
+ bool reInitialisation = false;
+ if (!useOgre) {
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ } else {
+#ifdef VISP_HAVE_OGRE
+ if (!faces.isOgreInitialised()) {
+ faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
+ faces.setOgreShowConfigDialog(ogreShowConfigDialog);
+ faces.initOgre(cam);
+ // Turn off Ogre config dialog display for the next call to this
+ // function since settings are saved in the ogre.cfg file and used
+ // during the next call
+ ogreShowConfigDialog = false;
+ }
+
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#else
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+
+ if (useScanLine || clippingFlag > 3)
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ computeVisibility(I.getWidth(), I.getHeight());
+}
+
+void vpMbDepthNormalTracker::loadConfigFile(const std::string &configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtXmlGenericParser xmlp(vpMbtXmlGenericParser::DEPTH_NORMAL_PARSER);
+
+ xmlp.setCameraParameters(cam);
+ 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.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
+ xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
+
+ try {
+ 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());
+ }
+
+ vpCameraParameters camera;
+ xmlp.getCameraParameters(camera);
+ setCameraParameters(camera);
+
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ if (xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if (xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if (xmlp.getFovClipping())
+ setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);
+
+ 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;
+#endif
+}
+
+void vpMbDepthNormalTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
+{
+ cMo.eye();
+
+ for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
+ delete m_depthNormalFaces[i];
+ m_depthNormalFaces[i] = NULL;
+ }
+
+ m_depthNormalFaces.clear();
+
+ loadModel(cad_name, verbose);
+ initFromPose(I, cMo_);
+}
+
+#if defined(VISP_HAVE_PCL)
+void vpMbDepthNormalTracker::reInitModel(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ const std::string &cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose)
+{
+ vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
+ reInitModel(I_dummy, cad_name, cMo_, verbose);
+}
+
+#endif
+
+void vpMbDepthNormalTracker::resetTracker()
+{
+ cMo.eye();
+
+ for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
+ ++it) {
+ vpMbtFaceDepthNormal *normal_face = *it;
+ delete normal_face;
+ normal_face = NULL;
+ }
+
+ m_depthNormalFaces.clear();
+
+ m_computeInteraction = true;
+ computeCovariance = false;
+
+ angleAppears = vpMath::rad(89);
+ angleDisappears = vpMath::rad(89);
+
+ clippingFlag = vpPolygon3D::NO_CLIPPING;
+
+ m_lambda = 1.0;
+
+ faces.reset();
+
+ m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
+
+ useScanLine = false;
+
+#ifdef VISP_HAVE_OGRE
+ useOgre = false;
+#endif
+
+ m_depthNormalListOfActiveFaces.clear();
+ m_depthNormalListOfDesiredFeatures.clear();
+}
+
+void vpMbDepthNormalTracker::setOgreVisibilityTest(const bool &v)
+{
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT Depth");
+#endif
+}
+
+void vpMbDepthNormalTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
+{
+ cMo = cdMo;
+ init(I);
+}
+
+#if defined(VISP_HAVE_PCL)
+void vpMbDepthNormalTracker::setPose(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ const vpHomogeneousMatrix &cdMo)
+{
+ vpImage<unsigned char> I_dummy(point_cloud->height, point_cloud->width);
+ cMo = cdMo;
+ init(I_dummy);
+}
+#endif
+
+void vpMbDepthNormalTracker::setScanLineVisibilityTest(const bool &v)
+{
+ vpMbTracker::setScanLineVisibilityTest(v);
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setScanLineVisibilityTest(v);
+ }
+}
+
+void vpMbDepthNormalTracker::testTracking() {}
+
+#ifdef VISP_HAVE_PCL
+void vpMbDepthNormalTracker::segmentPointCloud(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ m_depthNormalListOfActiveFaces.clear();
+ m_depthNormalListOfDesiredFeatures.clear();
+
+#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_debugImage_depthNormal = 0;
+ std::vector<std::vector<vpImagePoint> > roiPts_vec;
+#endif
+
+ for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
+ ++it) {
+ vpMbtFaceDepthNormal *face = *it;
+
+ if (face->isVisible()) {
+ vpColVector desired_features;
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ std::vector<std::vector<vpImagePoint> > roiPts_vec_;
+#endif
+ 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_
+#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());
+#endif
+ }
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ vpDisplay::display(m_debugImage_depthNormal);
+
+ for (size_t i = 0; i < roiPts_vec.size(); i++) {
+ if (roiPts_vec[i].empty())
+ 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][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
+ vpColor::red, 2);
+ }
+
+ vpDisplay::flush(m_debugImage_depthNormal);
+#endif
+}
+#endif
+
+void vpMbDepthNormalTracker::segmentPointCloud(const std::vector<vpColVector> &point_cloud, const unsigned int width,
+ const unsigned int height)
+{
+ m_depthNormalListOfActiveFaces.clear();
+ m_depthNormalListOfDesiredFeatures.clear();
+
+#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_debugImage_depthNormal = 0;
+ std::vector<std::vector<vpImagePoint> > roiPts_vec;
+#endif
+
+ for (std::vector<vpMbtFaceDepthNormal *>::iterator it = m_depthNormalFaces.begin(); it != m_depthNormalFaces.end();
+ ++it) {
+ vpMbtFaceDepthNormal *face = *it;
+
+ if (face->isVisible()) {
+ vpColVector desired_features;
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ std::vector<std::vector<vpImagePoint> > roiPts_vec_;
+#endif
+
+ if (face->computeDesiredFeatures(cMo, width, height, point_cloud, desired_features, m_depthNormalSamplingStepX,
+ m_depthNormalSamplingStepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ 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());
+#endif
+ }
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ vpDisplay::display(m_debugImage_depthNormal);
+
+ for (size_t i = 0; i < roiPts_vec.size(); i++) {
+ if (roiPts_vec[i].empty())
+ 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][0], roiPts_vec[i][roiPts_vec[i].size() - 1],
+ vpColor::red, 2);
+ }
+
+ vpDisplay::flush(m_debugImage_depthNormal);
+#endif
+}
+
+void vpMbDepthNormalTracker::setCameraParameters(const vpCameraParameters &camera)
+{
+ this->cam = camera;
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setCameraParameters(camera);
+ }
+}
+
+void vpMbDepthNormalTracker::setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
+{
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setFaceCentroidMethod(method);
+ }
+}
+
+void vpMbDepthNormalTracker::setDepthNormalFeatureEstimationMethod(
+ const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
+{
+ m_depthNormalFeatureEstimationMethod = method;
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setFeatureEstimationMethod(method);
+ }
+}
+
+void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationMethod(const int method)
+{
+ m_depthNormalPclPlaneEstimationMethod = method;
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setPclPlaneEstimationMethod(method);
+ }
+}
+
+void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
+{
+ m_depthNormalPclPlaneEstimationRansacMaxIter = maxIter;
+
+ for (std::vector<vpMbtFaceDepthNormal *>::const_iterator it = m_depthNormalFaces.begin();
+ it != m_depthNormalFaces.end(); ++it) {
+ (*it)->setPclPlaneEstimationRansacMaxIter(maxIter);
+ }
+}
+
+void vpMbDepthNormalTracker::setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
+{
+ m_depthNormalPclPlaneEstimationRansacThreshold = thresold;
+
+ for (std::vector<vpMbtFaceDepthNormal *>::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)
+{
+ if (stepX == 0 || stepY == 0) {
+ std::cerr << "stepX and stepY must be greater than zero!" << std::endl;
+ return;
+ }
+
+ m_depthNormalSamplingStepX = stepX;
+ m_depthNormalSamplingStepY = stepY;
+}
+
+// void vpMbDepthNormalTracker::setDepthNormalUseRobust(const bool use) {
+// m_depthNormalUseRobust = use;
+//}
+
+void vpMbDepthNormalTracker::track(const vpImage<unsigned char> &)
+{
+ throw vpException(vpException::fatalError, "Cannot track with a grayscale image!");
+}
+
+#ifdef VISP_HAVE_PCL
+void vpMbDepthNormalTracker::track(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ segmentPointCloud(point_cloud);
+
+ computeVVS();
+
+ computeVisibility(point_cloud->width, point_cloud->height);
+}
+#endif
+
+void vpMbDepthNormalTracker::track(const std::vector<vpColVector> &point_cloud, const unsigned int width,
+ const unsigned int height)
+{
+ segmentPointCloud(point_cloud, width, height);
+
+ computeVVS();
+
+ 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*/)
+{
+ 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*/)
+{
+ throw vpException(vpException::fatalError, "vpMbDepthNormalTracker::initCylinder() should not be called!");
+}
+
+void vpMbDepthNormalTracker::initFaceFromCorners(vpMbtPolygon &polygon) { addFace(polygon, false); }
+
+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
new file mode 100644
index 0000000..f70405d
--- /dev/null
+++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthDense.cpp
@@ -0,0 +1,765 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Manage depth dense features for a particular face.
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpCPUFeatures.h>
+#include <visp3/mbt/vpMbtFaceDepthDense.h>
+
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+#endif
+
+#define USE_SSE_CODE 1
+#if VISP_HAVE_SSE2 && USE_SSE_CODE
+#define USE_SSE 1
+#else
+#define USE_SSE 0
+#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()
+{
+}
+
+vpMbtFaceDepthDense::~vpMbtFaceDepthDense()
+{
+ for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
+ delete m_listOfFaceLines[i];
+ }
+}
+
+/*!
+ Add a line belonging to the \f$ index \f$ the polygon to the list of lines.
+ It is defined by its two extremities.
+
+ If the line already exists, the ploygone's index is added to the list of
+ polygon to which it belongs.
+
+ \param P1 : The first extremity of the line.
+ \param P2 : The second extremity of the line.
+ \param faces : Pointer to vpMbHiddenFaces.
+ \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<vpMbtPolygon> *const faces, int polygon,
+ std::string name)
+{
+ // Build a PolygonLine to be able to easily display the lines model
+ PolygonLine polygon_line;
+
+ // Add polygon
+ polygon_line.m_poly.setNbPoint(2);
+ polygon_line.m_poly.addPoint(0, P1);
+ polygon_line.m_poly.addPoint(1, P2);
+
+ polygon_line.m_poly.setClipping(m_clippingFlag);
+ polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
+ polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
+
+ polygon_line.m_p1 = &polygon_line.m_poly.p[0];
+ polygon_line.m_p2 = &polygon_line.m_poly.p[1];
+
+ m_polygonLines.push_back(polygon_line);
+
+ // suppress line already in the model
+ bool already_here = false;
+ vpMbtDistanceLine *l;
+
+ for (std::vector<vpMbtDistanceLine *>::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))) {
+ already_here = true;
+ l->addPolygon(polygon);
+ l->hiddenface = faces;
+ l->useScanLine = m_useScanLine;
+ }
+ }
+
+ if (!already_here) {
+ l = new vpMbtDistanceLine;
+
+ l->setCameraParameters(m_cam);
+ l->buildFrom(P1, P2);
+ l->addPolygon(polygon);
+ l->hiddenface = faces;
+ l->useScanLine = m_useScanLine;
+
+ l->setIndex((unsigned int)m_listOfFaceLines.size());
+ l->setName(name);
+
+ if (m_clippingFlag != vpPolygon3D::NO_CLIPPING)
+ l->getPolygon().setClipping(m_clippingFlag);
+
+ if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ l->getPolygon().setNearClippingDistance(m_distNearClip);
+
+ if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ l->getPolygon().setFarClippingDistance(m_distFarClip);
+
+ m_listOfFaceLines.push_back(l);
+ }
+}
+
+#ifdef VISP_HAVE_PCL
+bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ const unsigned int stepX, const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ vpImage<unsigned char> &debugImage,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+)
+{
+ unsigned int width = point_cloud->width, height = point_cloud->height;
+ m_pointCloudFace.clear();
+
+ if (point_cloud->width == 0 || point_cloud->height == 0)
+ return false;
+
+ std::vector<vpImagePoint> roiPts;
+ double distanceToFace;
+ computeROI(cMo, width, height, roiPts
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ roiPts_vec
+#endif
+ ,
+ distanceToFace);
+
+ if (roiPts.size() <= 2) {
+#ifndef NDEBUG
+ 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)) {
+ return false;
+ }
+
+ vpPolygon polygon_2d(roiPts);
+ 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 left = (unsigned int)std::max(0.0, bb.getLeft());
+ unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
+
+ bb.setTop(top);
+ bb.setBottom(bottom);
+ bb.setLeft(left);
+ bb.setRight(right);
+
+ if (bb.getHeight() < 0 || bb.getWidth() < 0) {
+ return false;
+ }
+
+ m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#else
+ bool push = false;
+ double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
+#endif
+
+ 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)))) {
+ totalTheoreticalPoints++;
+
+ if (pcl::isFinite((*point_cloud)(j, i)) && (*point_cloud)(j, i).z > 0) {
+ totalPoints++;
+
+ if (checkSSE2) {
+#if USE_SSE
+ if (!push) {
+ push = true;
+ prev_x = (*point_cloud)(j, i).x;
+ prev_y = (*point_cloud)(j, i).y;
+ prev_z = (*point_cloud)(j, i).z;
+ } else {
+ push = false;
+ m_pointCloudFace.push_back(prev_x);
+ m_pointCloudFace.push_back((*point_cloud)(j, i).x);
+
+ m_pointCloudFace.push_back(prev_y);
+ m_pointCloudFace.push_back((*point_cloud)(j, i).y);
+
+ m_pointCloudFace.push_back(prev_z);
+ m_pointCloudFace.push_back((*point_cloud)(j, i).z);
+ }
+#endif
+ } else {
+ m_pointCloudFace.push_back((*point_cloud)(j, i).x);
+ m_pointCloudFace.push_back((*point_cloud)(j, i).y);
+ m_pointCloudFace.push_back((*point_cloud)(j, i).z);
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ debugImage[i][j] = 255;
+#endif
+ }
+ }
+ }
+ }
+
+#if USE_SSE
+ if (checkSSE2 && push) {
+ m_pointCloudFace.push_back(prev_x);
+ m_pointCloudFace.push_back(prev_y);
+ m_pointCloudFace.push_back(prev_z);
+ }
+#endif
+
+ if (totalPoints == 0 || ((m_depthDenseFilteringMethod & DEPTH_OCCUPANCY_RATIO_FILTERING) &&
+ totalPoints / (double)totalTheoreticalPoints < m_depthDenseFilteringOccupancyRatio)) {
+ return false;
+ }
+
+ return true;
+}
+#endif
+
+bool vpMbtFaceDepthDense::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width,
+ const unsigned int height, const std::vector<vpColVector> &point_cloud,
+ const unsigned int stepX, const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ vpImage<unsigned char> &debugImage,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+)
+{
+ m_pointCloudFace.clear();
+
+ if (width == 0 || height == 0)
+ return 0;
+
+ std::vector<vpImagePoint> roiPts;
+ double distanceToFace;
+ computeROI(cMo, width, height, roiPts
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ roiPts_vec
+#endif
+ ,
+ distanceToFace);
+
+ if (roiPts.size() <= 2) {
+#ifndef NDEBUG
+ 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)) {
+ return false;
+ }
+
+ vpPolygon polygon_2d(roiPts);
+ 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 left = (unsigned int)std::max(0.0, bb.getLeft());
+ unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
+
+ bb.setTop(top);
+ bb.setBottom(bottom);
+ bb.setLeft(left);
+ bb.setRight(right);
+
+ m_pointCloudFace.reserve((size_t)(bb.getWidth() * bb.getHeight()));
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#else
+ bool push = false;
+ double prev_x = 0.0, prev_y = 0.0, prev_z = 0.0;
+#endif
+
+ 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)))) {
+ totalTheoreticalPoints++;
+
+ if (point_cloud[i * width + j][2] > 0) {
+ totalPoints++;
+
+ if (checkSSE2) {
+#if USE_SSE
+ if (!push) {
+ push = true;
+ prev_x = point_cloud[i * width + j][0];
+ prev_y = point_cloud[i * width + j][1];
+ prev_z = point_cloud[i * width + j][2];
+ } else {
+ push = false;
+ m_pointCloudFace.push_back(prev_x);
+ m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
+
+ m_pointCloudFace.push_back(prev_y);
+ m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
+
+ m_pointCloudFace.push_back(prev_z);
+ m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
+ }
+#endif
+ } else {
+ m_pointCloudFace.push_back(point_cloud[i * width + j][0]);
+ m_pointCloudFace.push_back(point_cloud[i * width + j][1]);
+ m_pointCloudFace.push_back(point_cloud[i * width + j][2]);
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ debugImage[i][j] = 255;
+#endif
+ }
+ }
+ }
+ }
+
+#if USE_SSE
+ if (checkSSE2 && push) {
+ m_pointCloudFace.push_back(prev_x);
+ m_pointCloudFace.push_back(prev_y);
+ m_pointCloudFace.push_back(prev_z);
+ }
+#endif
+
+ 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::computeVisibilityDisplay()
+{
+ // Compute lines visibility, only for display
+ vpMbtDistanceLine *line;
+ for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
+ ++it) {
+ line = *it;
+ bool isvisible = false;
+
+ for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
+ ++itindex) {
+ int index = *itindex;
+ if (index == -1) {
+ isvisible = true;
+ } else {
+ if (line->hiddenface->isVisible((unsigned int)index)) {
+ isvisible = true;
+ }
+ }
+ }
+
+ // Si la ligne n'appartient a aucune face elle est tout le temps visible
+ if (line->Lindex_polygon.empty())
+ isvisible = true; // Not sure that this can occur
+
+ if (isvisible) {
+ line->setVisible(true);
+ } else {
+ line->setVisible(false);
+ }
+ }
+}
+
+void vpMbtFaceDepthDense::computeInteractionMatrixAndResidu(const vpHomogeneousMatrix &cMo, vpMatrix &L,
+ vpColVector &error)
+{
+ if (m_pointCloudFace.empty()) {
+ L.resize(0, 0);
+ error.resize(0);
+ return;
+ }
+
+ L.resize(getNbFeatures(), 6, false, false);
+ error.resize(getNbFeatures(), false);
+
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double nx = m_planeCamera.getA();
+ double ny = m_planeCamera.getB();
+ double nz = m_planeCamera.getC();
+ double D = m_planeCamera.getD();
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#endif
+
+ if (checkSSE2) {
+#if USE_SSE
+ size_t cpt = 0;
+ if (getNbFeatures() >= 2) {
+ double *ptr_point_cloud = &m_pointCloudFace[0];
+ double *ptr_L = L.data;
+ double *ptr_error = error.data;
+
+ const __m128d vnx = _mm_set1_pd(nx);
+ const __m128d vny = _mm_set1_pd(ny);
+ const __m128d vnz = _mm_set1_pd(nz);
+ const __m128d vd = _mm_set1_pd(D);
+
+ double tmp_a1[2], tmp_a2[2], tmp_a3[2];
+
+ 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));
+
+ _mm_storeu_pd(tmp_a1, va1);
+ _mm_storeu_pd(tmp_a2, va2);
+ _mm_storeu_pd(tmp_a3, va3);
+
+ *ptr_L = nx;
+ ptr_L++;
+ *ptr_L = ny;
+ ptr_L++;
+ *ptr_L = nz;
+ ptr_L++;
+ *ptr_L = tmp_a1[0];
+ ptr_L++;
+ *ptr_L = tmp_a2[0];
+ ptr_L++;
+ *ptr_L = tmp_a3[0];
+ ptr_L++;
+
+ *ptr_L = nx;
+ ptr_L++;
+ *ptr_L = ny;
+ ptr_L++;
+ *ptr_L = nz;
+ ptr_L++;
+ *ptr_L = tmp_a1[1];
+ ptr_L++;
+ *ptr_L = tmp_a2[1];
+ ptr_L++;
+ *ptr_L = tmp_a3[1];
+ 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_storeu_pd(ptr_error, verror);
+ ptr_error += 2;
+ }
+ }
+
+ for (; cpt < m_pointCloudFace.size(); cpt += 3) {
+ double x = m_pointCloudFace[cpt];
+ double y = m_pointCloudFace[cpt + 1];
+ double z = m_pointCloudFace[cpt + 2];
+
+ double _a1 = (nz * y) - (ny * z);
+ double _a2 = (nx * z) - (nz * x);
+ double _a3 = (ny * x) - (nx * y);
+
+ // L
+ L[(unsigned int)(cpt / 3)][0] = nx;
+ L[(unsigned int)(cpt / 3)][1] = ny;
+ L[(unsigned int)(cpt / 3)][2] = nz;
+ L[(unsigned int)(cpt / 3)][3] = _a1;
+ L[(unsigned int)(cpt / 3)][4] = _a2;
+ L[(unsigned int)(cpt / 3)][5] = _a3;
+
+ vpColVector normal(3);
+ normal[0] = nx;
+ normal[1] = ny;
+ normal[2] = nz;
+
+ vpColVector pt(3);
+ pt[0] = x;
+ pt[1] = y;
+ pt[2] = z;
+
+ // Error
+ error[(unsigned int)(cpt / 3)] = D + (normal.t() * pt);
+ }
+#endif
+ } else {
+ vpColVector normal(3);
+ normal[0] = nx;
+ normal[1] = ny;
+ normal[2] = nz;
+ vpColVector pt(3);
+
+ unsigned int idx = 0;
+ for (size_t i = 0; i < m_pointCloudFace.size(); i += 3, idx++) {
+ double x = m_pointCloudFace[i];
+ double y = m_pointCloudFace[i + 1];
+ double z = m_pointCloudFace[i + 2];
+
+ double _a1 = (nz * y) - (ny * z);
+ double _a2 = (nx * z) - (nz * x);
+ double _a3 = (ny * x) - (nx * y);
+
+ // L
+ L[idx][0] = nx;
+ L[idx][1] = ny;
+ L[idx][2] = nz;
+ L[idx][3] = _a1;
+ L[idx][4] = _a2;
+ L[idx][5] = _a3;
+
+ pt[0] = x;
+ pt[1] = y;
+ pt[2] = z;
+ // Error
+ error[idx] = D + (normal.t() * pt);
+ }
+ }
+}
+
+void vpMbtFaceDepthDense::computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width,
+ const unsigned int height, std::vector<vpImagePoint> &roiPts
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ ,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+ ,
+ double &distanceToFace)
+{
+ if (m_useScanLine || m_clippingFlag > 2)
+ m_cam.computeFov(width, height);
+
+ if (m_useScanLine) {
+ for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
+ it->m_p1->changeFrame(cMo);
+ it->m_p2->changeFrame(cMo);
+
+ vpImagePoint ip1, ip2;
+
+ it->m_poly.changeFrame(cMo);
+ 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)) {
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+ m_hiddenFace->computeScanLineQuery(it->m_poly.polyClipped[0].first, it->m_poly.polyClipped[1].first, linesLst,
+ true);
+
+ vpPoint faceCentroid;
+
+ 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);
+
+ it->m_imPt1 = ip1;
+ it->m_imPt2 = ip2;
+
+ 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());
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ std::vector<vpImagePoint> roiPts_;
+ roiPts_.push_back(ip1);
+ roiPts_.push_back(ip2);
+ roiPts_vec.push_back(roiPts_);
+#endif
+ }
+
+ if (linesLst.empty()) {
+ distanceToFace = std::numeric_limits<double>::max();
+ } else {
+ faceCentroid.set_X(faceCentroid.get_X() / (2 * linesLst.size()));
+ 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());
+ }
+ }
+ }
+ } else {
+ // Get polygon clipped
+ m_polygon->getRoiClipped(m_cam, roiPts, cMo);
+
+ // Get 3D polygon clipped
+ std::vector<vpPoint> polygonsClipped;
+ m_polygon->getPolygonClipped(polygonsClipped);
+
+ if (polygonsClipped.empty()) {
+ distanceToFace = std::numeric_limits<double>::max();
+ } else {
+ vpPoint faceCentroid;
+
+ for (size_t i = 0; i < polygonsClipped.size(); i++) {
+ faceCentroid.set_X(faceCentroid.get_X() + polygonsClipped[i].get_X());
+ faceCentroid.set_Y(faceCentroid.get_Y() + polygonsClipped[i].get_Y());
+ faceCentroid.set_Z(faceCentroid.get_Z() + polygonsClipped[i].get_Z());
+ }
+
+ faceCentroid.set_X(faceCentroid.get_X() / polygonsClipped.size());
+ 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() +
+ faceCentroid.get_Z() * faceCentroid.get_Z());
+ }
+
+#if DEBUG_DISPLAY_DEPTH_DENSE
+ roiPts_vec.push_back(roiPts);
+#endif
+ }
+}
+
+void vpMbtFaceDepthDense::display(const vpImage<unsigned char> &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<vpMbtDistanceLine *>::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<vpRGBa> &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<vpMbtDistanceLine *>::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<unsigned char> & /*I*/, const vpHomogeneousMatrix & /*cMo*/,
+ const vpCameraParameters & /*cam*/, const double /*scale*/,
+ const unsigned int /*thickness*/)
+{
+}
+
+void vpMbtFaceDepthDense::displayFeature(const vpImage<vpRGBa> & /*I*/, const vpHomogeneousMatrix & /*cMo*/,
+ const vpCameraParameters & /*cam*/, const double /*scale*/,
+ const unsigned int /*thickness*/)
+{
+}
+
+/*!
+ Check if two vpPoints are similar.
+
+ To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon
+ \f$.
+
+ \param P1 : The first point to compare
+ \param P2 : The second point to compare
+*/
+bool 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<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
+ dz <= std::numeric_limits<double>::epsilon())
+ return true;
+ else
+ return false;
+}
+
+void vpMbtFaceDepthDense::setCameraParameters(const vpCameraParameters &camera)
+{
+ m_cam = camera;
+
+ for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
+ ++it) {
+ (*it)->setCameraParameters(camera);
+ }
+}
+
+void vpMbtFaceDepthDense::setScanLineVisibilityTest(const bool v)
+{
+ m_useScanLine = v;
+
+ for (std::vector<vpMbtDistanceLine *>::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
new file mode 100644
index 0000000..2e7955e
--- /dev/null
+++ b/modules/tracker/mbt/src/depth/vpMbtFaceDepthNormal.cpp
@@ -0,0 +1,1524 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Manage depth normal features for a particular face.
+ *
+ *****************************************************************************/
+
+#include <visp3/core/vpCPUFeatures.h>
+#include <visp3/mbt/vpMbtFaceDepthNormal.h>
+#include <visp3/mbt/vpMbtTukeyEstimator.h>
+
+#ifdef VISP_HAVE_PCL
+#include <pcl/common/centroid.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#endif
+
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+#endif
+
+#define USE_SSE_CODE 1
+#if VISP_HAVE_SSE2 && USE_SSE_CODE
+#define USE_SSE 1
+#else
+#define USE_SSE 0
+#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()
+{
+}
+
+vpMbtFaceDepthNormal::~vpMbtFaceDepthNormal()
+{
+ for (size_t i = 0; i < m_listOfFaceLines.size(); i++) {
+ delete m_listOfFaceLines[i];
+ }
+}
+
+/*!
+ Add a line belonging to the \f$ index \f$ the polygon to the list of lines.
+ It is defined by its two extremities.
+
+ If the line already exists, the ploygone's index is added to the list of
+ polygon to which it belongs.
+
+ \param P1 : The first extremity of the line.
+ \param P2 : The second extremity of the line.
+ \param faces : Pointer to vpMbHiddenFaces.
+ \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<vpMbtPolygon> *const faces, int polygon,
+ std::string name)
+{
+ // Build a PolygonLine to be able to easily display the lines model
+ PolygonLine polygon_line;
+
+ // Add polygon
+ polygon_line.m_poly.setNbPoint(2);
+ polygon_line.m_poly.addPoint(0, P1);
+ polygon_line.m_poly.addPoint(1, P2);
+
+ polygon_line.m_poly.setClipping(m_clippingFlag);
+ polygon_line.m_poly.setNearClippingDistance(m_distNearClip);
+ polygon_line.m_poly.setFarClippingDistance(m_distFarClip);
+
+ polygon_line.m_p1 = &polygon_line.m_poly.p[0];
+ polygon_line.m_p2 = &polygon_line.m_poly.p[1];
+
+ m_polygonLines.push_back(polygon_line);
+
+ // suppress line already in the model
+ bool already_here = false;
+ vpMbtDistanceLine *l;
+
+ for (std::vector<vpMbtDistanceLine *>::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))) {
+ already_here = true;
+ l->addPolygon(polygon);
+ l->hiddenface = faces;
+ l->useScanLine = m_useScanLine;
+ }
+ }
+
+ if (!already_here) {
+ l = new vpMbtDistanceLine;
+
+ l->setCameraParameters(m_cam);
+ l->buildFrom(P1, P2);
+ l->addPolygon(polygon);
+ l->hiddenface = faces;
+ l->useScanLine = m_useScanLine;
+
+ l->setIndex((unsigned int)m_listOfFaceLines.size());
+ l->setName(name);
+
+ if (m_clippingFlag != vpPolygon3D::NO_CLIPPING)
+ l->getPolygon().setClipping(m_clippingFlag);
+
+ if ((m_clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ l->getPolygon().setNearClippingDistance(m_distNearClip);
+
+ if ((m_clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ l->getPolygon().setFarClippingDistance(m_distFarClip);
+
+ m_listOfFaceLines.push_back(l);
+ }
+}
+
+#ifdef VISP_HAVE_PCL
+bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width,
+ const unsigned int height,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud,
+ vpColVector &desired_features, const unsigned int stepX,
+ const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ vpImage<unsigned char> &debugImage,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+)
+{
+ m_faceActivated = false;
+
+ if (width == 0 || height == 0)
+ return false;
+
+ std::vector<vpImagePoint> roiPts;
+ vpColVector desired_normal(3);
+
+ computeROI(cMo, width, height, roiPts
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ roiPts_vec
+#endif
+ );
+
+ if (roiPts.size() <= 2) {
+#ifndef NDEBUG
+ std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
+#endif
+ return false;
+ }
+
+ vpPolygon polygon_2d(roiPts);
+ 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 left = (unsigned int)std::max(0.0, bb.getLeft());
+ unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
+
+ bb.setTop(top);
+ bb.setBottom(bottom);
+ bb.setLeft(left);
+ bb.setRight(right);
+
+ // Keep only 3D points inside the projected polygon face
+ pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face(new pcl::PointCloud<pcl::PointXYZ>);
+ std::vector<double> 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()));
+ } else if (m_featureEstimationMethod == ROBUST_SVD_PLANE_ESTIMATION) {
+ 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()));
+ }
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#else
+ bool push = false;
+ double prev_x, prev_y, prev_z;
+#endif
+
+ double x = 0.0, y = 0.0;
+ 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)))) {
+
+ if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
+ point_cloud_face->push_back((*point_cloud)(j, i));
+ } else if (m_featureEstimationMethod == ROBUST_SVD_PLANE_ESTIMATION ||
+ m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) {
+ point_cloud_face_vec.push_back((*point_cloud)(j, i).x);
+ point_cloud_face_vec.push_back((*point_cloud)(j, i).y);
+ point_cloud_face_vec.push_back((*point_cloud)(j, i).z);
+
+ if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) {
+ // Add point for custom method for plane equation estimation
+ vpPixelMeterConversion::convertPoint(m_cam, j, i, x, y);
+
+ if (checkSSE2) {
+#if USE_SSE
+ if (!push) {
+ push = true;
+ prev_x = x;
+ prev_y = y;
+ prev_z = (*point_cloud)(j, i).z;
+ } else {
+ push = false;
+ point_cloud_face_custom.push_back(prev_x);
+ point_cloud_face_custom.push_back(x);
+
+ point_cloud_face_custom.push_back(prev_y);
+ point_cloud_face_custom.push_back(y);
+
+ point_cloud_face_custom.push_back(prev_z);
+ point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
+ }
+#endif
+ } else {
+ point_cloud_face_custom.push_back(x);
+ point_cloud_face_custom.push_back(y);
+ point_cloud_face_custom.push_back((*point_cloud)(j, i).z);
+ }
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ debugImage[i][j] = 255;
+#endif
+ }
+ }
+ }
+
+#if USE_SSE
+ if (checkSSE2 && push) {
+ point_cloud_face_custom.push_back(prev_x);
+ point_cloud_face_custom.push_back(prev_y);
+ point_cloud_face_custom.push_back(prev_z);
+ }
+#endif
+
+ if (point_cloud_face->empty() && point_cloud_face_custom.empty() && point_cloud_face_vec.empty()) {
+ return false;
+ }
+
+ // Face centroid computed by the different methods
+ vpColVector centroid_point(3);
+
+ if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
+ 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);
+ } else if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) {
+ 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!");
+ }
+
+ computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
+
+ m_faceActivated = true;
+
+ return true;
+}
+#endif
+
+bool vpMbtFaceDepthNormal::computeDesiredFeatures(const vpHomogeneousMatrix &cMo, const unsigned int width,
+ const unsigned int height,
+ const std::vector<vpColVector> &point_cloud,
+ vpColVector &desired_features, const unsigned int stepX,
+ const unsigned int stepY
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ vpImage<unsigned char> &debugImage,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+)
+{
+ m_faceActivated = false;
+
+ if (width == 0 || height == 0)
+ return false;
+
+ std::vector<vpImagePoint> roiPts;
+ vpColVector desired_normal(3);
+
+ computeROI(cMo, width, height, roiPts
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ roiPts_vec
+#endif
+ );
+
+ if (roiPts.size() <= 2) {
+#ifndef NDEBUG
+ std::cerr << "Error: roiPts.size() <= 2 in computeDesiredFeatures" << std::endl;
+#endif
+ return false;
+ }
+
+ vpPolygon polygon_2d(roiPts);
+ 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 left = (unsigned int)std::max(0.0, bb.getLeft());
+ unsigned int right = (unsigned int)std::min((double)width, std::max(0.0, bb.getRight()));
+
+ bb.setTop(top);
+ bb.setBottom(bottom);
+ bb.setLeft(left);
+ bb.setRight(right);
+
+ // Keep only 3D points inside the projected polygon face
+ std::vector<double> point_cloud_face, point_cloud_face_custom;
+
+ 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()));
+ }
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#else
+ bool push = false;
+ double prev_x, prev_y, prev_z;
+#endif
+
+ double x = 0.0, y = 0.0;
+ 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)))) {
+ // Add point
+ point_cloud_face.push_back(point_cloud[i * width + j][0]);
+ point_cloud_face.push_back(point_cloud[i * width + j][1]);
+ point_cloud_face.push_back(point_cloud[i * width + j][2]);
+
+ if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) {
+ // Add point for custom method for plane equation estimation
+ vpPixelMeterConversion::convertPoint(m_cam, j, i, x, y);
+
+ if (checkSSE2) {
+#if USE_SSE
+ if (!push) {
+ push = true;
+ prev_x = x;
+ prev_y = y;
+ prev_z = point_cloud[i * width + j][2];
+ } else {
+ push = false;
+ point_cloud_face_custom.push_back(prev_x);
+ point_cloud_face_custom.push_back(x);
+
+ point_cloud_face_custom.push_back(prev_y);
+ 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]);
+ }
+#endif
+ } else {
+ point_cloud_face_custom.push_back(x);
+ point_cloud_face_custom.push_back(y);
+ point_cloud_face_custom.push_back(point_cloud[i * width + j][2]);
+ }
+ }
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ debugImage[i][j] = 255;
+#endif
+ }
+ }
+ }
+
+#if USE_SSE
+ if (checkSSE2 && push) {
+ point_cloud_face_custom.push_back(prev_x);
+ point_cloud_face_custom.push_back(prev_y);
+ point_cloud_face_custom.push_back(prev_z);
+ }
+#endif
+
+ if (point_cloud_face.empty() && point_cloud_face_custom.empty()) {
+ return false;
+ }
+
+ // Face centroid computed by the different methods
+ vpColVector centroid_point(3);
+
+#ifdef VISP_HAVE_PCL
+ if (m_featureEstimationMethod == PCL_PLANE_ESTIMATION) {
+ pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_pcl(new pcl::PointCloud<pcl::PointXYZ>);
+ 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]));
+ }
+
+ 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);
+ } else if (m_featureEstimationMethod == ROBUST_FEATURE_ESTIMATION) {
+ computeDesiredFeaturesRobustFeatures(point_cloud_face_custom, point_cloud_face, cMo, desired_features,
+ desired_normal, centroid_point);
+ } else {
+ throw vpException(vpException::badValue, "Unknown feature estimation method!");
+ }
+
+ computeDesiredNormalAndCentroid(cMo, desired_normal, centroid_point);
+
+ m_faceActivated = true;
+
+ return true;
+}
+
+#ifdef VISP_HAVE_PCL
+bool vpMbtFaceDepthNormal::computeDesiredFeaturesPCL(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud_face,
+ vpColVector &desired_features, vpColVector &desired_normal,
+ vpColVector ¢roid_point)
+{
+ try {
+ // Compute plane equation for this subset of point cloud
+ pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
+ pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
+ // Create the segmentation object
+ pcl::SACSegmentation<pcl::PointXYZ> seg;
+ // Optional
+ seg.setOptimizeCoefficients(true);
+ // Mandatory
+ seg.setModelType(pcl::SACMODEL_PLANE);
+ seg.setMethodType(m_pclPlaneEstimationMethod);
+ seg.setDistanceThreshold(m_pclPlaneEstimationRansacThreshold);
+ seg.setMaxIterations(m_pclPlaneEstimationRansacMaxIter);
+
+ seg.setInputCloud(point_cloud_face);
+ seg.segment(*inliers, *coefficients);
+
+ pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_face_extracted(new pcl::PointCloud<pcl::PointXYZ>);
+ // Create the filtering object
+ pcl::ExtractIndices<pcl::PointXYZ> extract;
+
+ // Extract the inliers
+ extract.setInputCloud(point_cloud_face);
+ extract.setIndices(inliers);
+ extract.setNegative(false);
+ extract.filter(*point_cloud_face_extracted);
+
+ pcl::PointXYZ 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);
+
+ 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_normal[0] = face_normal.x;
+ desired_normal[1] = face_normal.y;
+ desired_normal[2] = face_normal.z;
+
+ centroid_point[0] = centroid_point_pcl.x;
+ centroid_point[1] = centroid_point_pcl.y;
+ centroid_point[2] = centroid_point_pcl.z;
+ } else {
+ std::cerr << "Cannot compute centroid!" << std::endl;
+ return false;
+ }
+ } catch (const pcl::PCLException &e) {
+ std::cerr << "Catch a PCL exception: " << e.what() << std::endl;
+ throw;
+ }
+
+ return true;
+}
+#endif
+
+void vpMbtFaceDepthNormal::computeDesiredFeaturesRobustFeatures(const std::vector<double> &point_cloud_face_custom,
+ const std::vector<double> &point_cloud_face,
+ const vpHomogeneousMatrix &cMo,
+ vpColVector &desired_features,
+ vpColVector &desired_normal,
+ vpColVector ¢roid_point)
+{
+ std::vector<double> weights;
+ double den = 0.0;
+ estimateFeatures(point_cloud_face_custom, cMo, desired_features, weights);
+
+ // Compute face centroid
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ centroid_point[0] += weights[i] * point_cloud_face[3 * i];
+ centroid_point[1] += weights[i] * point_cloud_face[3 * i + 1];
+ centroid_point[2] += weights[i] * point_cloud_face[3 * i + 2];
+
+ den += weights[i];
+ }
+
+ centroid_point[0] /= den;
+ centroid_point[1] /= den;
+ centroid_point[2] /= den;
+
+ computeNormalVisibility(-desired_features[0], -desired_features[1], -desired_features[2], centroid_point,
+ desired_normal);
+}
+
+void vpMbtFaceDepthNormal::computeDesiredFeaturesSVD(const std::vector<double> &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);
+
+ 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,
+ desired_normal);
+}
+
+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);
+ centroid_cam[0] = centroid_point[0];
+ centroid_cam[1] = centroid_point[1];
+ centroid_cam[2] = centroid_point[2];
+ centroid_cam[3] = 1;
+
+ vpColVector centroid_obj = cMo.inverse() * centroid_cam;
+ 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);
+ face_normal_cam[0] = desired_normal[0];
+ face_normal_cam[1] = desired_normal[1];
+ face_normal_cam[2] = desired_normal[2];
+ 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]);
+}
+
+bool vpMbtFaceDepthNormal::computePolygonCentroid(const std::vector<vpPoint> &points_, vpPoint ¢roid)
+{
+ if (points_.empty()) {
+ return false;
+ }
+
+ if (points_.size() < 2) {
+ centroid = points_[0];
+ return true;
+ }
+
+ std::vector<vpPoint> points = points_;
+ points.push_back(points_.front());
+
+ double A1 = 0.0, A2 = 0.0, c_x1 = 0.0, c_x2 = 0.0, c_y = 0.0, c_z = 0.0;
+
+ 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());
+ 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();
+
+ // 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());
+ 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();
+ }
+
+ c_x1 /= 3.0 * A1;
+ c_y /= 3.0 * A1;
+ c_x2 /= 3.0 * A2;
+ c_z /= 3.0 * A2;
+
+ if (A1 > A2) {
+ centroid.set_X(c_x1);
+ } else {
+ centroid.set_X(c_x2);
+ }
+
+ centroid.set_Y(c_y);
+ centroid.set_Z(c_z);
+
+ return true;
+}
+
+void vpMbtFaceDepthNormal::computeROI(const vpHomogeneousMatrix &cMo, const unsigned int width,
+ const unsigned int height, std::vector<vpImagePoint> &roiPts
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ ,
+ std::vector<std::vector<vpImagePoint> > &roiPts_vec
+#endif
+)
+{
+ if (m_useScanLine || m_clippingFlag > 2)
+ m_cam.computeFov(width, height);
+
+ if (m_useScanLine) {
+ for (std::vector<PolygonLine>::iterator it = m_polygonLines.begin(); it != m_polygonLines.end(); ++it) {
+ it->m_p1->changeFrame(cMo);
+ it->m_p2->changeFrame(cMo);
+
+ vpImagePoint ip1, ip2;
+
+ it->m_poly.changeFrame(cMo);
+ 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)) {
+
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+ 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);
+
+ it->m_imPt1 = ip1;
+ it->m_imPt2 = ip2;
+
+ roiPts.push_back(ip1);
+ roiPts.push_back(ip2);
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ std::vector<vpImagePoint> roiPts_;
+ roiPts_.push_back(ip1);
+ roiPts_.push_back(ip2);
+ roiPts_vec.push_back(roiPts_);
+#endif
+ }
+ }
+ }
+ } else {
+ // Get polygon clipped
+ m_polygon->getRoiClipped(m_cam, roiPts, cMo);
+
+#if DEBUG_DISPLAY_DEPTH_NORMAL
+ roiPts_vec.push_back(roiPts);
+#endif
+ }
+}
+
+void vpMbtFaceDepthNormal::computeVisibility() { m_isVisible = m_polygon->isVisible(); }
+
+void vpMbtFaceDepthNormal::computeVisibilityDisplay()
+{
+ // Compute lines visibility, only for display
+ vpMbtDistanceLine *line;
+ for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
+ ++it) {
+ line = *it;
+ bool isvisible = false;
+
+ for (std::list<int>::const_iterator itindex = line->Lindex_polygon.begin(); itindex != line->Lindex_polygon.end();
+ ++itindex) {
+ int index = *itindex;
+ if (index == -1) {
+ isvisible = true;
+ } else {
+ if (line->hiddenface->isVisible((unsigned int)index)) {
+ isvisible = true;
+ }
+ }
+ }
+
+ // Si la ligne n'appartient a aucune face elle est tout le temps visible
+ if (line->Lindex_polygon.empty())
+ isvisible = true; // Not sure that this can occur
+
+ if (isvisible) {
+ line->setVisible(true);
+ } else {
+ line->setVisible(false);
+ }
+ }
+}
+
+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;
+ faceNormal[1] = ny;
+ faceNormal[2] = nz;
+ faceNormal.normalize();
+
+ // Get polygon clipped
+ std::vector<vpImagePoint> roiPts;
+ m_polygon->getRoiClipped(camera, roiPts, cMo);
+
+ std::vector<vpPoint> polyPts;
+ m_polygon->getPolygonClipped(polyPts);
+
+ vpColVector e4(3);
+ if (m_faceCentroidMethod == GEOMETRIC_CENTROID) {
+ computePolygonCentroid(polyPts, centroid);
+ centroid.project();
+
+ e4[0] = -centroid.get_X();
+ e4[1] = -centroid.get_Y();
+ e4[2] = -centroid.get_Z();
+ e4.normalize();
+ } else {
+ double centroid_x = 0.0;
+ double centroid_y = 0.0;
+ double centroid_z = 0.0;
+
+ for (size_t i = 0; i < polyPts.size(); i++) {
+ centroid_x += polyPts[i].get_X();
+ centroid_y += polyPts[i].get_Y();
+ centroid_z += polyPts[i].get_Z();
+ }
+
+ centroid_x /= polyPts.size();
+ centroid_y /= polyPts.size();
+ centroid_z /= polyPts.size();
+
+ e4[0] = -centroid_x;
+ e4[1] = -centroid_y;
+ e4[2] = -centroid_z;
+ e4.normalize();
+
+ centroid.set_X(centroid_x);
+ centroid.set_Y(centroid_y);
+ centroid.set_Z(centroid_z);
+ }
+
+ correct_normal.resize(3, false);
+ double angle = acos(vpColVector::dotProd(e4, faceNormal));
+ if (angle < M_PI_2) {
+ correct_normal = faceNormal;
+ } else {
+ correct_normal[0] = -faceNormal[0];
+ correct_normal[1] = -faceNormal[1];
+ correct_normal[2] = -faceNormal[2];
+ }
+}
+
+#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)
+{
+ vpColVector faceNormal(3);
+ faceNormal[0] = nx;
+ faceNormal[1] = ny;
+ faceNormal[2] = nz;
+ faceNormal.normalize();
+
+ vpColVector e4(3);
+ e4[0] = -centroid_point.x;
+ e4[1] = -centroid_point.y;
+ e4[2] = -centroid_point.z;
+ e4.normalize();
+
+ double angle = acos(vpColVector::dotProd(e4, faceNormal));
+ 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]);
+ }
+}
+#endif
+
+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;
+ face_normal[1] = ny;
+ face_normal[2] = nz;
+ face_normal.normalize();
+
+ vpColVector e4 = -centroid_point;
+ e4.normalize();
+
+ double angle = acos(vpColVector::dotProd(e4, face_normal));
+ if (angle >= M_PI_2) {
+ face_normal[0] = -face_normal[0];
+ face_normal[1] = -face_normal[1];
+ face_normal[2] = -face_normal[2];
+ }
+}
+
+void vpMbtFaceDepthNormal::computeInteractionMatrix(const vpHomogeneousMatrix &cMo, vpMatrix &L, vpColVector &features)
+{
+ L.resize(3, 6, false, false);
+
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double ux = m_planeCamera.getA();
+ double uy = m_planeCamera.getB();
+ double uz = m_planeCamera.getC();
+ double D = m_planeCamera.getD();
+ double D2 = D * D;
+
+ // Features
+ features.resize(3, false);
+ features[0] = -ux / D;
+ features[1] = -uy / D;
+ features[2] = -uz / D;
+
+ // L_A
+ L[0][0] = ux * ux / D2;
+ L[0][1] = ux * uy / D2;
+ L[0][2] = ux * uz / D2;
+ L[0][3] = 0.0;
+ L[0][4] = uz / D;
+ L[0][5] = -uy / D;
+
+ // L_B
+ L[1][0] = ux * uy / D2;
+ L[1][1] = uy * uy / D2;
+ L[1][2] = uy * uz / D2;
+ L[1][3] = -uz / D;
+ L[1][4] = 0.0;
+ L[1][5] = ux / D;
+
+ // L_C
+ L[2][0] = ux * uz / D2;
+ L[2][1] = uy * uz / D2;
+ L[2][2] = uz * uz / D2;
+ L[2][3] = uy / D;
+ L[2][4] = -ux / D;
+ L[2][5] = 0.0;
+}
+
+void vpMbtFaceDepthNormal::display(const vpImage<unsigned char> &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<vpMbtDistanceLine *>::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<vpRGBa> &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<vpMbtDistanceLine *>::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<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, const double scale,
+ const unsigned int thickness)
+{
+ if (m_faceActivated /*&& m_isTracked*/ && m_isVisible) {
+ // Desired feature
+ vpPoint pt_centroid = m_faceDesiredCentroid;
+ pt_centroid.changeFrame(cMo);
+ pt_centroid.project();
+
+ vpImagePoint im_centroid;
+ vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
+
+ vpPoint pt_normal = m_faceDesiredNormal;
+ pt_normal.changeFrame(cMo);
+ pt_normal.project();
+
+ vpPoint pt_extremity;
+ pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
+ pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
+ pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
+ pt_extremity.project();
+
+ vpImagePoint 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);
+
+ // Current feature
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double ux = m_planeCamera.getA();
+ double uy = m_planeCamera.getB();
+ double uz = m_planeCamera.getC();
+
+ 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);
+
+ pt_centroid.project();
+ 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);
+
+ vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
+ }
+}
+
+void vpMbtFaceDepthNormal::displayFeature(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam, const double scale,
+ const unsigned int thickness)
+{
+ if (m_faceActivated /*&& m_isTracked*/ && m_isVisible) {
+ // Desired feature
+ vpPoint pt_centroid = m_faceDesiredCentroid;
+ pt_centroid.changeFrame(cMo);
+ pt_centroid.project();
+
+ vpImagePoint im_centroid;
+ vpMeterPixelConversion::convertPoint(cam, pt_centroid.get_x(), pt_centroid.get_y(), im_centroid);
+
+ vpPoint pt_normal = m_faceDesiredNormal;
+ pt_normal.changeFrame(cMo);
+ pt_normal.project();
+
+ vpPoint pt_extremity;
+ pt_extremity.set_X(pt_centroid.get_X() + pt_normal.get_X() * scale);
+ pt_extremity.set_Y(pt_centroid.get_Y() + pt_normal.get_Y() * scale);
+ pt_extremity.set_Z(pt_centroid.get_Z() + pt_normal.get_Z() * scale);
+ pt_extremity.project();
+
+ vpImagePoint 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);
+
+ // Current feature
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double ux = m_planeCamera.getA();
+ double uy = m_planeCamera.getB();
+ double uz = m_planeCamera.getC();
+
+ 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);
+
+ pt_centroid.project();
+ 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);
+
+ vpDisplay::displayArrow(I, im_centroid, im_extremity, vpColor::red, 4, 2, thickness);
+ }
+}
+
+void vpMbtFaceDepthNormal::estimateFeatures(const std::vector<double> &point_cloud_face, const vpHomogeneousMatrix &cMo,
+ vpColVector &x_estimated, std::vector<double> &w)
+{
+ vpMbtTukeyEstimator<double> tukey_robust;
+ std::vector<double> residues(point_cloud_face.size() / 3);
+
+ w.resize(point_cloud_face.size() / 3, 1.0);
+
+ unsigned int max_iter = 30, iter = 0;
+ double error = 0.0, prev_error = -1.0;
+ double A = 0.0, B = 0.0, C = 0.0;
+
+ Mat33<double> ATA_3x3;
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !USE_SSE
+ checkSSE2 = false;
+#endif
+
+ if (checkSSE2) {
+#if USE_SSE
+ while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
+ if (iter == 0) {
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double ux = m_planeCamera.getA();
+ double uy = m_planeCamera.getB();
+ double uz = m_planeCamera.getC();
+ double D = m_planeCamera.getD();
+
+ // Features
+ A = -ux / D;
+ B = -uy / D;
+ C = -uz / D;
+
+ size_t cpt = 0;
+ if (point_cloud_face.size() / 3 >= 2) {
+ const double *ptr_point_cloud = &point_cloud_face[0];
+ const __m128d vA = _mm_set1_pd(A);
+ const __m128d vB = _mm_set1_pd(B);
+ const __m128d vC = _mm_set1_pd(C);
+ const __m128d vones = _mm_set1_pd(1.0);
+
+ double *ptr_residues = &residues[0];
+
+ 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));
+ _mm_storeu_pd(ptr_residues, tmp);
+ }
+ }
+
+ for (; cpt < point_cloud_face.size(); cpt += 3) {
+ double xi = point_cloud_face[cpt];
+ double yi = point_cloud_face[cpt + 1];
+ double Zi = point_cloud_face[cpt + 2];
+
+ residues[cpt / 3] = (A * xi + B * yi + C - 1 / Zi);
+ }
+ }
+
+ tukey_robust.MEstimator(residues, w, 1e-2);
+
+ __m128d vsum_wi2_xi2 = _mm_setzero_pd();
+ __m128d vsum_wi2_yi2 = _mm_setzero_pd();
+ __m128d vsum_wi2 = _mm_setzero_pd();
+ __m128d vsum_wi2_xi_yi = _mm_setzero_pd();
+ __m128d vsum_wi2_xi = _mm_setzero_pd();
+ __m128d vsum_wi2_yi = _mm_setzero_pd();
+
+ __m128d vsum_wi2_xi_Zi = _mm_setzero_pd();
+ __m128d vsum_wi2_yi_Zi = _mm_setzero_pd();
+ __m128d vsum_wi2_Zi = _mm_setzero_pd();
+
+ // Estimate A, B, C
+ size_t cpt = 0;
+ if (point_cloud_face.size() / 3 >= 2) {
+ const double *ptr_point_cloud = &point_cloud_face[0];
+ double *ptr_w = &w[0];
+
+ 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));
+
+ 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 = _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 = _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_Zi = _mm_add_pd(vsum_wi2_Zi, vwi2_invZi);
+ }
+ }
+
+ double vtmp[2];
+ _mm_storeu_pd(vtmp, vsum_wi2_xi2);
+ double sum_wi2_xi2 = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_yi2);
+ double sum_wi2_yi2 = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2);
+ double sum_wi2 = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_xi_yi);
+ double sum_wi2_xi_yi = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_xi);
+ double sum_wi2_xi = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_yi);
+ double sum_wi2_yi = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_xi_Zi);
+ double sum_wi2_xi_Zi = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_yi_Zi);
+ double sum_wi2_yi_Zi = vtmp[0] + vtmp[1];
+
+ _mm_storeu_pd(vtmp, vsum_wi2_Zi);
+ double sum_wi2_Zi = vtmp[0] + vtmp[1];
+
+ for (; cpt < point_cloud_face.size(); cpt += 3) {
+ double wi2 = w[cpt / 3] * w[cpt / 3];
+
+ double xi = point_cloud_face[cpt];
+ double yi = point_cloud_face[cpt + 1];
+ double Zi = point_cloud_face[cpt + 2];
+ double invZi = 1.0 / Zi;
+
+ sum_wi2_xi2 += wi2 * xi * xi;
+ sum_wi2_yi2 += wi2 * yi * yi;
+ sum_wi2 += wi2;
+ sum_wi2_xi_yi += wi2 * xi * yi;
+ sum_wi2_xi += wi2 * xi;
+ sum_wi2_yi += wi2 * yi;
+
+ sum_wi2_xi_Zi += wi2 * xi * invZi;
+ sum_wi2_yi_Zi += wi2 * yi * invZi;
+ sum_wi2_Zi += wi2 * invZi;
+ }
+
+ ATA_3x3[0] = sum_wi2_xi2;
+ ATA_3x3[1] = sum_wi2_xi_yi;
+ ATA_3x3[2] = sum_wi2_xi;
+ ATA_3x3[3] = sum_wi2_xi_yi;
+ ATA_3x3[4] = sum_wi2_yi2;
+ ATA_3x3[5] = sum_wi2_yi;
+ ATA_3x3[6] = sum_wi2_xi;
+ ATA_3x3[7] = sum_wi2_yi;
+ ATA_3x3[8] = sum_wi2;
+
+ Mat33<double> 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;
+
+ cpt = 0;
+
+ // Compute error
+ prev_error = error;
+ error = 0.0;
+
+ __m128d verror = _mm_set1_pd(0.0);
+ if (point_cloud_face.size() / 3 >= 2) {
+ const double *ptr_point_cloud = &point_cloud_face[0];
+ const __m128d vA = _mm_set1_pd(A);
+ const __m128d vB = _mm_set1_pd(B);
+ const __m128d vC = _mm_set1_pd(C);
+ const __m128d vones = _mm_set1_pd(1.0);
+
+ double *ptr_residues = &residues[0];
+
+ 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));
+ verror = _mm_add_pd(verror, _mm_mul_pd(tmp, tmp));
+
+ _mm_storeu_pd(ptr_residues, tmp);
+ }
+ }
+
+ _mm_storeu_pd(vtmp, verror);
+ error = vtmp[0] + vtmp[1];
+
+ for (size_t idx = cpt; idx < point_cloud_face.size(); idx += 3) {
+ double xi = point_cloud_face[idx];
+ double yi = point_cloud_face[idx + 1];
+ double Zi = point_cloud_face[idx + 2];
+
+ error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
+ residues[idx / 3] = (A * xi + B * yi + C - 1 / Zi);
+ }
+
+ error /= point_cloud_face.size() / 3;
+
+ iter++;
+ } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
+#endif
+ } else {
+ while (std::fabs(error - prev_error) > 1e-6 && (iter < max_iter)) {
+ if (iter == 0) {
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double ux = m_planeCamera.getA();
+ double uy = m_planeCamera.getB();
+ double uz = m_planeCamera.getC();
+ double D = m_planeCamera.getD();
+
+ // Features
+ A = -ux / D;
+ B = -uy / D;
+ C = -uz / D;
+
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ double xi = point_cloud_face[3 * i];
+ double yi = point_cloud_face[3 * i + 1];
+ double Zi = point_cloud_face[3 * i + 2];
+
+ residues[i] = (A * xi + B * yi + C - 1 / Zi);
+ }
+ }
+
+ tukey_robust.MEstimator(residues, w, 1e-2);
+
+ // Estimate A, B, C
+ double sum_wi2_xi2 = 0.0, sum_wi2_yi2 = 0.0, sum_wi2 = 0.0;
+ double sum_wi2_xi_yi = 0.0, sum_wi2_xi = 0.0, sum_wi2_yi = 0.0;
+
+ double sum_wi2_xi_Zi = 0.0, sum_wi2_yi_Zi = 0.0, sum_wi2_Zi = 0.0;
+
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ double wi2 = w[i] * w[i];
+
+ double xi = point_cloud_face[3 * i];
+ double yi = point_cloud_face[3 * i + 1];
+ double Zi = point_cloud_face[3 * i + 2];
+ double invZi = 1 / Zi;
+
+ sum_wi2_xi2 += wi2 * xi * xi;
+ sum_wi2_yi2 += wi2 * yi * yi;
+ sum_wi2 += wi2;
+ sum_wi2_xi_yi += wi2 * xi * yi;
+ sum_wi2_xi += wi2 * xi;
+ sum_wi2_yi += wi2 * yi;
+
+ sum_wi2_xi_Zi += wi2 * xi * invZi;
+ sum_wi2_yi_Zi += wi2 * yi * invZi;
+ sum_wi2_Zi += wi2 * invZi;
+ }
+
+ ATA_3x3[0] = sum_wi2_xi2;
+ ATA_3x3[1] = sum_wi2_xi_yi;
+ ATA_3x3[2] = sum_wi2_xi;
+ ATA_3x3[3] = sum_wi2_xi_yi;
+ ATA_3x3[4] = sum_wi2_yi2;
+ ATA_3x3[5] = sum_wi2_yi;
+ ATA_3x3[6] = sum_wi2_xi;
+ ATA_3x3[7] = sum_wi2_yi;
+ ATA_3x3[8] = sum_wi2;
+
+ Mat33<double> 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;
+
+ prev_error = error;
+ error = 0.0;
+
+ // Compute error
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ double xi = point_cloud_face[3 * i];
+ double yi = point_cloud_face[3 * i + 1];
+ double Zi = point_cloud_face[3 * i + 2];
+
+ error += vpMath::sqr(A * xi + B * yi + C - 1 / Zi);
+ residues[i] = (A * xi + B * yi + C - 1 / Zi);
+ }
+
+ error /= point_cloud_face.size() / 3;
+
+ iter++;
+ } // while ( std::fabs(error - prev_error) > 1e-6 && (iter < max_iter) )
+ }
+
+ x_estimated.resize(3, false);
+ x_estimated[0] = A;
+ x_estimated[1] = B;
+ x_estimated[2] = C;
+}
+
+void vpMbtFaceDepthNormal::estimatePlaneEquationSVD(const std::vector<double> &point_cloud_face,
+ const vpHomogeneousMatrix &cMo,
+ vpColVector &plane_equation_estimated, vpColVector ¢roid)
+{
+ const unsigned int max_iter = 10;
+ double prev_error = 1e3;
+ double error = 1e3 - 1;
+
+ std::vector<double> weights(point_cloud_face.size() / 3, 1.0);
+ std::vector<double> residues(point_cloud_face.size() / 3);
+ vpMatrix M((unsigned int)(point_cloud_face.size() / 3), 3);
+ vpMbtTukeyEstimator<double> tukey;
+ vpColVector normal;
+
+ 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 {
+ // Transform the plane equation for the current pose
+ m_planeCamera = m_planeObject;
+ m_planeCamera.changeFrame(cMo);
+
+ double A = m_planeCamera.getA();
+ double B = m_planeCamera.getB();
+ double C = m_planeCamera.getC();
+ double D = m_planeCamera.getD();
+
+ // 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] +
+ C * point_cloud_face[3 * i + 2] + D) /
+ sqrt(A * A + B * B + C * C);
+ }
+
+ tukey.MEstimator(residues, weights, 1e-4);
+ plane_equation_estimated.resize(4, false);
+ }
+
+ // Compute centroid
+ double centroid_x = 0.0, centroid_y = 0.0, centroid_z = 0.0;
+ double total_w = 0.0;
+
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ centroid_x += weights[i] * point_cloud_face[3 * i];
+ centroid_y += weights[i] * point_cloud_face[3 * i + 1];
+ centroid_z += weights[i] * point_cloud_face[3 * i + 2];
+ total_w += weights[i];
+ }
+
+ centroid_x /= total_w;
+ centroid_y /= total_w;
+ centroid_z /= total_w;
+
+ // 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);
+ }
+
+ vpMatrix J = M.t() * M;
+
+ vpColVector W;
+ vpMatrix V;
+ J.svd(W, V);
+
+ double smallestSv = W[0];
+ unsigned int indexSmallestSv = 0;
+ for (unsigned int i = 1; i < W.size(); i++) {
+ if (W[i] < smallestSv) {
+ smallestSv = W[i];
+ indexSmallestSv = i;
+ }
+ }
+
+ normal = V.getCol(indexSmallestSv);
+
+ // Compute plane equation
+ double A = normal[0], B = normal[1], C = normal[2];
+ double D = -(A * centroid_x + B * centroid_y + C * centroid_z);
+
+ // Update plane equation
+ plane_equation_estimated[0] = A;
+ plane_equation_estimated[1] = B;
+ plane_equation_estimated[2] = C;
+ plane_equation_estimated[3] = D;
+
+ // Compute error points to estimated plane
+ 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] +
+ C * point_cloud_face[3 * i + 2] + D) /
+ sqrt(A * A + B * B + C * C);
+ error += residues[i] * residues[i];
+ }
+ error /= sqrt(error / total_w);
+ }
+
+ // Update final weights
+ tukey.MEstimator(residues, weights, 1e-4);
+
+ // Update final centroid
+ centroid.resize(3, false);
+ double total_w = 0.0;
+
+ for (size_t i = 0; i < point_cloud_face.size() / 3; i++) {
+ centroid[0] += weights[i] * point_cloud_face[3 * i];
+ centroid[1] += weights[i] * point_cloud_face[3 * i + 1];
+ centroid[2] += weights[i] * point_cloud_face[3 * i + 2];
+ total_w += weights[i];
+ }
+
+ centroid[0] /= total_w;
+ centroid[1] /= total_w;
+ centroid[2] /= total_w;
+
+ // Compute final plane equation
+ double A = normal[0], B = normal[1], C = normal[2];
+ double D = -(A * centroid[0] + B * centroid[1] + C * centroid[2]);
+
+ // Update final plane equation
+ plane_equation_estimated[0] = A;
+ plane_equation_estimated[1] = B;
+ plane_equation_estimated[2] = C;
+ plane_equation_estimated[3] = D;
+}
+
+/*!
+ Check if two vpPoints are similar.
+
+ To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon
+ \f$.
+
+ \param P1 : The first point to compare
+ \param P2 : The second point to compare
+*/
+bool 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<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
+ dz <= std::numeric_limits<double>::epsilon())
+ return true;
+ else
+ return false;
+}
+
+void vpMbtFaceDepthNormal::setCameraParameters(const vpCameraParameters &camera)
+{
+ m_cam = camera;
+
+ for (std::vector<vpMbtDistanceLine *>::const_iterator it = m_listOfFaceLines.begin(); it != m_listOfFaceLines.end();
+ ++it) {
+ (*it)->setCameraParameters(camera);
+ }
+}
+
+void vpMbtFaceDepthNormal::setScanLineVisibilityTest(const bool v)
+{
+ m_useScanLine = v;
+
+ for (std::vector<vpMbtDistanceLine *>::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
new file mode 100644
index 0000000..00143d5
--- /dev/null
+++ b/modules/tracker/mbt/src/depth/vpMbtTukeyEstimator.cpp
@@ -0,0 +1,408 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Tukey M-estimator.
+ *
+ *****************************************************************************/
+
+#include <algorithm>
+#include <cmath>
+#include <iostream>
+
+#include <visp3/core/vpCPUFeatures.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/mbt/vpMbtTukeyEstimator.h>
+
+#define USE_TRANSFORM 1
+#if defined(VISP_HAVE_CPP11_COMPATIBILITY) && USE_TRANSFORM
+#define HAVE_TRANSFORM 1
+#endif
+
+#if HAVE_TRANSFORM
+#include <functional>
+#endif
+
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+
+#if defined __SSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <pmmintrin.h>
+#define VISP_HAVE_SSE3 1
+#endif
+#if defined __SSSE3__ || (defined _MSC_VER && _MSC_VER >= 1500)
+#include <tmmintrin.h>
+#define VISP_HAVE_SSSE3 1
+#endif
+#endif
+
+#define USE_ORIGINAL_TUKEY_CODE 1
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#if HAVE_TRANSFORM
+namespace
+{
+template <typename T> struct AbsDiff : public std::binary_function<T, T, T> {
+ double operator()(const T a, const T b) const { return std::fabs(a - b); }
+};
+}
+#endif
+
+template class vpMbtTukeyEstimator<float>;
+template class vpMbtTukeyEstimator<double>;
+
+#if VISP_HAVE_SSSE3
+namespace
+{
+inline __m128 abs_ps(__m128 x)
+{
+ static const __m128 sign_mask = _mm_set1_ps(-0.f); // -0.f = 1 << 31
+ return _mm_andnot_ps(sign_mask, x);
+}
+}
+#endif
+
+template <typename T> T vpMbtTukeyEstimator<T>::getMedian(std::vector<T> &vec)
+{
+ // Not the exact median when even number of elements
+ int index = (int)(ceil(vec.size() / 2.0)) - 1;
+ std::nth_element(vec.begin(), vec.begin() + index, vec.end());
+ return vec[index];
+}
+
+// Without MEstimator_impl, error with g++4.6, ok with gcc 5.4.0
+// Ubuntu-12.04-Linux-i386-g++4.6-Dyn-RelWithDebInfo-dc1394-v4l2-X11-OpenCV2.3.1-lapack-gsl-Coin-jpeg-png-xml-pthread-OpenMP-dmtx-zbar-Wov-Weq-Moment:
+// libvisp_mbt.so.3.1.0: undefined reference to
+// `vpMbtTukeyEstimator<double>::MEstimator(std::vector<double,
+// std::allocator<double> > const&, std::vector<double, std::allocator<double>
+// >&, double)'
+template <typename T>
+void vpMbtTukeyEstimator<T>::MEstimator_impl(const std::vector<T> &residues, std::vector<T> &weights,
+ const T NoiseThreshold)
+{
+ if (residues.empty()) {
+ return;
+ }
+
+ m_residues = residues;
+
+ T med = getMedian(m_residues);
+ m_normres.resize(residues.size());
+
+#if HAVE_TRANSFORM
+ std::transform(residues.begin(), residues.end(), m_normres.begin(),
+ std::bind(AbsDiff<T>(), std::placeholders::_1, med));
+#else
+ for (size_t i = 0; i < m_residues.size(); i++) {
+ m_normres[i] = (std::fabs(residues[i] - med));
+ }
+#endif
+
+ m_residues = m_normres;
+ T normmedian = getMedian(m_residues);
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ T sigma = static_cast<T>(1.4826 * normmedian); // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
+ }
+
+ psiTukey(sigma, m_normres, weights);
+}
+
+template <>
+void vpMbtTukeyEstimator<float>::MEstimator_impl_ssse3(const std::vector<float> &residues, std::vector<float> &weights,
+ const float NoiseThreshold)
+{
+#if VISP_HAVE_SSSE3
+ if (residues.empty()) {
+ return;
+ }
+
+ m_residues = residues;
+
+ float med = getMedian(m_residues);
+ m_normres.resize(residues.size());
+
+ size_t i = 0;
+ __m128 med_128 = _mm_set_ps1(med);
+
+ 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)));
+ }
+ }
+
+ for (; i < m_residues.size(); i++) {
+ m_normres[i] = (std::fabs(residues[i] - med));
+ }
+
+ m_residues = m_normres;
+ float normmedian = getMedian(m_residues);
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ float sigma = 1.4826f * normmedian; // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
+ }
+
+ psiTukey(sigma, m_normres, weights);
+#else
+ (void)residues;
+ (void)weights;
+ (void)NoiseThreshold;
+#endif
+}
+
+template <>
+void vpMbtTukeyEstimator<double>::MEstimator_impl_ssse3(const std::vector<double> &residues,
+ std::vector<double> &weights, const double NoiseThreshold)
+{
+#if VISP_HAVE_SSSE3
+ if (residues.empty()) {
+ return;
+ }
+
+ m_residues = residues;
+
+ double med = getMedian(m_residues);
+ m_normres.resize(residues.size());
+
+#if HAVE_TRANSFORM
+ std::transform(residues.begin(), residues.end(), m_normres.begin(),
+ std::bind(AbsDiff<double>(), std::placeholders::_1, med));
+#else
+ for (size_t i = 0; i < m_residues.size(); i++) {
+ m_normres[i] = (std::fabs(residues[i] - med));
+ }
+#endif
+
+ m_residues = m_normres;
+ double normmedian = getMedian(m_residues);
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ double sigma = 1.4826 * normmedian; // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
+ }
+
+ psiTukey(sigma, m_normres, weights);
+#else
+ (void)residues;
+ (void)weights;
+ (void)NoiseThreshold;
+#endif
+}
+
+template <>
+void vpMbtTukeyEstimator<float>::MEstimator(const std::vector<float> &residues, std::vector<float> &weights,
+ const float NoiseThreshold)
+{
+ bool checkSSSE3 = vpCPUFeatures::checkSSSE3();
+#if !VISP_HAVE_SSSE3
+ checkSSSE3 = false;
+#endif
+
+ if (checkSSSE3)
+ MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
+ else
+ MEstimator_impl(residues, weights, NoiseThreshold);
+}
+
+template <>
+void vpMbtTukeyEstimator<double>::MEstimator(const std::vector<double> &residues, std::vector<double> &weights,
+ const double NoiseThreshold)
+{
+ bool checkSSSE3 = vpCPUFeatures::checkSSSE3();
+#if !VISP_HAVE_SSSE3
+ checkSSSE3 = false;
+#endif
+
+ if (checkSSSE3)
+ MEstimator_impl_ssse3(residues, weights, NoiseThreshold);
+ else
+ MEstimator_impl(residues, weights, NoiseThreshold);
+}
+
+template <typename T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, vpColVector &weights)
+{
+ T cst_const = static_cast<T>(4.6851);
+ T inv_cst_const = 1 / cst_const;
+ T inv_sig = 1 / sig;
+
+ for (unsigned int i = 0; i < (unsigned int)x.size(); i++) {
+#if USE_ORIGINAL_TUKEY_CODE
+ if (std::fabs(sig) <= std::numeric_limits<T>::epsilon() &&
+ std::fabs(weights[i]) > std::numeric_limits<double>::epsilon() // sig should be equal to 0
+ // only if NoiseThreshold
+ // ==
+ // 0
+ ) {
+ weights[i] = 1;
+ continue;
+ }
+#endif
+
+ double xi_sig = x[(size_t)i] * inv_sig;
+
+ if ((std::fabs(xi_sig) <= cst_const)
+#if USE_ORIGINAL_TUKEY_CODE
+ && std::fabs(weights[i]) > std::numeric_limits<double>::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) );
+ } else {
+ // Outlier - could resize list of points tracked here?
+ weights[i] = 0;
+ }
+ }
+}
+
+template <>
+void vpMbtTukeyEstimator<double>::MEstimator(const vpColVector &residues, vpColVector &weights,
+ const double NoiseThreshold)
+{
+ if (residues.size() == 0) {
+ return;
+ }
+
+ m_residues.resize(0);
+ m_residues.reserve(residues.size());
+ m_residues.insert(m_residues.end(), &residues.data[0], &residues.data[residues.size()]);
+
+ double med = getMedian(m_residues);
+
+ m_normres.resize(residues.size());
+ for (size_t i = 0; i < m_residues.size(); i++) {
+ m_normres[i] = std::fabs(residues[(unsigned int)i] - med);
+ }
+
+ m_residues = m_normres;
+ double normmedian = getMedian(m_residues);
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ double sigma = 1.4826 * normmedian; // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if (sigma < NoiseThreshold) {
+ sigma = NoiseThreshold;
+ }
+
+ psiTukey(sigma, m_normres, weights);
+}
+
+template <>
+void vpMbtTukeyEstimator<float>::MEstimator(const vpColVector &residues, vpColVector &weights,
+ const double NoiseThreshold)
+{
+ if (residues.size() == 0) {
+ return;
+ }
+
+ m_residues.resize(0);
+ m_residues.reserve(residues.size());
+ for (unsigned int i = 0; i < residues.size(); i++) {
+ m_residues.push_back((float)residues[i]);
+ }
+
+ float med = getMedian(m_residues);
+
+ m_normres.resize(residues.size());
+ for (size_t i = 0; i < m_residues.size(); i++) {
+ m_normres[i] = (float)std::fabs(residues[(unsigned int)i] - med);
+ }
+
+ m_residues = m_normres;
+ float normmedian = getMedian(m_residues);
+
+ // 1.48 keeps scale estimate consistent for a normal probability dist.
+ float sigma = 1.4826f * normmedian; // median Absolute Deviation
+
+ // Set a minimum threshold for sigma
+ // (when sigma reaches the level of noise in the image)
+ if (sigma < NoiseThreshold) {
+ sigma = (float)NoiseThreshold;
+ }
+
+ psiTukey(sigma, m_normres, weights);
+}
+
+template <class T> void vpMbtTukeyEstimator<T>::psiTukey(const T sig, std::vector<T> &x, std::vector<T> &weights)
+{
+ T cst_const = static_cast<T>(4.6851);
+ T inv_cst_const = 1 / cst_const;
+ T inv_sig = 1 / sig;
+
+ for (size_t i = 0; i < x.size(); i++) {
+#if USE_ORIGINAL_TUKEY_CODE
+ if (std::fabs(sig) <= std::numeric_limits<T>::epsilon() &&
+ std::fabs(weights[i]) > std::numeric_limits<T>::epsilon() // sig should be equal to 0 only
+ // if NoiseThreshold == 0
+ ) {
+ weights[i] = 1;
+ continue;
+ }
+#endif
+
+ T xi_sig = x[i] * inv_sig;
+
+ if ((std::fabs(xi_sig) <= cst_const)
+#if USE_ORIGINAL_TUKEY_CODE
+ && std::fabs(weights[i]) > std::numeric_limits<T>::epsilon() // Consider the previous
+ // weights here
+#endif
+ ) {
+ // vpMath::sqr( 1 - vpMath::sqr(xi_sig * inv_cst_const) );
+ weights[i] = (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const)) *
+ (1 - (xi_sig * inv_cst_const) * (xi_sig * inv_cst_const));
+ } else {
+ // Outlier - could resize list of points tracked here?
+ weights[i] = 0;
+ }
+ }
+}
+#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp
index f8c9f4c..072c667 100644
--- a/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbEdgeMultiTracker.cpp
@@ -2,19 +2,20 @@
*
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
- *
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
- * GPL, please contact INRIA about acquiring a ViSP Professional
+ * GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
- *
+ * See https://visp.inria.fr/download/ for more information.
+ *
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
@@ -24,7 +25,7 @@
*
* If you have questions regarding the use of this file, please contact
* INRIA at visp at inria.fr
- *
+ *
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
@@ -43,20 +44,21 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/mbt/vpMbEdgeMultiTracker.h>
#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
-
+#include <visp3/mbt/vpMbEdgeMultiTracker.h>
/*!
Basic constructor
*/
-vpMbEdgeMultiTracker::vpMbEdgeMultiTracker() : m_mapOfCameraTransformationMatrix(), m_mapOfEdgeTrackers(),
- m_mapOfPyramidalImages(), m_referenceCameraName("Camera") {
+vpMbEdgeMultiTracker::vpMbEdgeMultiTracker()
+ : 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();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
}
@@ -65,68 +67,74 @@ vpMbEdgeMultiTracker::vpMbEdgeMultiTracker() : m_mapOfCameraTransformationMatrix
\param nbCameras : Number of cameras to use.
*/
-vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const unsigned int nbCameras) : m_mapOfCameraTransformationMatrix(),
- m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(), m_referenceCameraName("Camera") {
+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()
+{
- if(nbCameras == 0) {
+ if (nbCameras == 0) {
throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !");
} else if (nbCameras == 1) {
m_mapOfEdgeTrackers["Camera"] = new vpMbEdgeTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
- } else if(nbCameras == 2) {
+ } else if (nbCameras == 2) {
m_mapOfEdgeTrackers["Camera1"] = new vpMbEdgeTracker();
m_mapOfEdgeTrackers["Camera2"] = new vpMbEdgeTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
m_mapOfCameraTransformationMatrix["Camera2"] = vpHomogeneousMatrix();
- //Set by default the reference camera to Camera1
+ // Set by default the reference camera to Camera1
m_referenceCameraName = "Camera1";
} else {
- for(unsigned int i = 1; i <= nbCameras; i++) {
+ for (unsigned int i = 1; i <= nbCameras; i++) {
std::stringstream ss;
ss << "Camera" << i;
m_mapOfEdgeTrackers[ss.str()] = new vpMbEdgeTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
}
- //Set by default the reference camera to the first one
+ // Set by default the reference camera to the first one
m_referenceCameraName = m_mapOfEdgeTrackers.begin()->first;
}
}
/*!
Construct a vpMbEdgeMultiTracker with the specified list of camera names.
- By default, the first camera name by alphabetic order is the reference camera.
+ By default, the first camera name by alphabetic order is the reference
+ camera.
\param cameraNames : List of camera names.
*/
-vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const std::vector<std::string> &cameraNames) : m_mapOfCameraTransformationMatrix(),
- m_mapOfEdgeTrackers(), m_mapOfPyramidalImages(), m_referenceCameraName("Camera") {
+vpMbEdgeMultiTracker::vpMbEdgeMultiTracker(const std::vector<std::string> &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()) {
+ if (cameraNames.empty()) {
throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbEdgeMultiTracker with no camera !");
}
- for(std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
+ for (std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
m_mapOfEdgeTrackers[*it] = new vpMbEdgeTracker();
}
- //Set by default the reference camera
+ // Set by default the reference camera
m_referenceCameraName = cameraNames.front();
}
/*!
Basic destructor useful to deallocate the memory.
*/
-vpMbEdgeMultiTracker::~vpMbEdgeMultiTracker() {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+vpMbEdgeMultiTracker::~vpMbEdgeMultiTracker()
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
delete it->second;
}
@@ -135,13 +143,14 @@ vpMbEdgeMultiTracker::~vpMbEdgeMultiTracker() {
cleanPyramid(m_mapOfPyramidalImages);
}
-void vpMbEdgeMultiTracker::cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char>* > >& pyramid) {
- for(std::map<std::string, std::vector<const vpImage<unsigned char>* > >::iterator it1 = pyramid.begin();
- it1 != pyramid.end(); ++it1) {
- if(it1->second.size() > 0){
+void vpMbEdgeMultiTracker::cleanPyramid(std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
+{
+ for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::iterator it1 = pyramid.begin();
+ it1 != pyramid.end(); ++it1) {
+ if (it1->second.size() > 0) {
it1->second[0] = NULL;
- for(size_t i = 1; i < it1->second.size(); i++) {
- if(it1->second[i] != NULL) {
+ for (size_t i = 1; i < it1->second.size(); i++) {
+ if (it1->second[i] != NULL) {
delete it1->second[i];
it1->second[i] = NULL;
}
@@ -151,22 +160,23 @@ void vpMbEdgeMultiTracker::cleanPyramid(std::map<std::string, std::vector<const
}
}
-void vpMbEdgeMultiTracker::computeProjectionError() {
- if(computeProjError) {
+void vpMbEdgeMultiTracker::computeProjectionError()
+{
+ if (computeProjError) {
double rawTotalProjectionError = 0.0;
unsigned int nbTotalFeaturesUsed = 0;
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
double curProjError = it->second->getProjectionError();
unsigned int nbFeaturesUsed = it->second->nbFeaturesForProjErrorComputation;
- if(nbFeaturesUsed > 0) {
+ if (nbFeaturesUsed > 0) {
nbTotalFeaturesUsed += nbFeaturesUsed;
- rawTotalProjectionError += ( vpMath::rad(curProjError)*nbFeaturesUsed );
+ rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
}
}
- if(nbTotalFeaturesUsed > 0) {
+ if (nbTotalFeaturesUsed > 0) {
nbFeaturesForProjErrorComputation = nbTotalFeaturesUsed;
projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
} else {
@@ -176,431 +186,309 @@ void vpMbEdgeMultiTracker::computeProjectionError() {
}
}
-void vpMbEdgeMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages, const unsigned int lvl) {
- //Number of moving edges
- unsigned int nbrow = 0;
- //unsigned int nberrors_lines = 0;
- //unsigned int nberrors_cylinders = 0;
- //unsigned int nberrors_circles = 0;
-
- std::vector<FeatureType> indexOfFeatures;
- std::map<std::string, unsigned int> mapOfNumberOfRows;
- std::map<std::string, unsigned int> mapOfNumberOfLines;
- std::map<std::string, unsigned int> mapOfNumberOfCylinders;
- std::map<std::string, unsigned int> mapOfNumberOfCircles;
-
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
- it1 != m_mapOfEdgeTrackers.end(); ++it1) {
- unsigned int nrows = 0;
- unsigned int nlines = 0;
- unsigned int ncylinders = 0;
- unsigned int ncircles = 0;
-
- nrows = it1->second->initMbtTracking(nlines, ncylinders, ncircles);
-
- mapOfNumberOfRows[it1->first] = nrows;
- mapOfNumberOfLines[it1->first] = nlines;
- mapOfNumberOfCylinders[it1->first] = ncylinders;
- mapOfNumberOfCircles[it1->first] = ncircles;
-
- nbrow += nrows;
- //nberrors_lines += nlines;
- //nberrors_cylinders += ncylinders;
- //nberrors_circles += ncircles;
-
- for(unsigned int i = 0; i < nlines; i++) {
- indexOfFeatures.push_back(LINE);
- }
-
- for(unsigned int i = 0; i < ncylinders; i++) {
- indexOfFeatures.push_back(CYLINDER);
- }
-
- for(unsigned int i = 0; i < ncircles; i++) {
- indexOfFeatures.push_back(CIRCLE);
- }
- }
-
- if(nbrow < 4) {
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
- }
-
- vpMatrix L;
-
- // compute the error vector
- m_error.resize(nbrow);
- unsigned int nerror = m_error.getRows();
- vpColVector v;
+void vpMbEdgeMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const unsigned int lvl)
+{
+ computeVVSInit();
+ unsigned int nbrow = m_error_edgeMulti.getRows();
-// double limite = 3; //Une limite de 3 pixels
-// limite = limite / cam.get_px(); //Transformation limite pixel en limite metre.
unsigned int iter = 0;
- vpColVector weighted_error;
- vpColVector factor;
- std::map<std::string, vpColVector> mapOfFactors;
-
- //Parametre pour la premiere phase d'asservissement
+ // 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
std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
- for(std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
- it != m_mapOfCameraTransformationMatrix.end(); ++it) {
+ for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
+ it != m_mapOfCameraTransformationMatrix.end(); ++it) {
vpVelocityTwistMatrix cVo;
cVo.buildFrom(it->second);
mapOfVelocityTwist[it->first] = cVo;
}
-// std::cout << "\n\n\ncMo used before the first phase=\n" << cMo << std::endl;
+ // std::cout << "\n\n\ncMo used before the first phase=\n" << cMo <<
+ // std::endl;
/*** First phase ***/
- while(reloop == true && iter < 10)
- {
- m_w.resize(0);
- m_error.resize(0);
-
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge;
+ while (reloop == true && iter < 10) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
}
- if(iter == 0)
- {
- weighted_error.resize(nerror);
-
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->m_w.resize(mapOfNumberOfRows[it->first]);
- it->second->m_w = 0;
-
- it->second->m_error.resize(mapOfNumberOfRows[it->first]);
-
- mapOfFactors[it->first].resize(mapOfNumberOfRows[it->first]);
- mapOfFactors[it->first] = 1;
- }
- }
-
double count = 0;
reloop = false;
- L = vpMatrix();
- factor = vpColVector();
-
-
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- vpMatrix L_tmp(mapOfNumberOfRows[it->first], 6);
-
+ unsigned int start_idx = 0;
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
double count_tmp = 0.0;
- it->second->computeVVSFirstPhase(*mapOfImages[it->first], iter, L_tmp, mapOfFactors[it->first], count_tmp,
- it->second->m_error, it->second->m_w, lvl);
+ edge = it->second;
+ edge->computeVVSFirstPhase(*mapOfImages[it->first], iter, count_tmp, lvl);
count += count_tmp;
- L_tmp = L_tmp*mapOfVelocityTwist[it->first];
+ 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);
- L.stack(L_tmp);
- factor.stack(mapOfFactors[it->first]);
- m_w.stack(it->second->m_w);
- m_error.stack(it->second->m_error);
+ start_idx += edge->m_error_edge.getRows();
}
- count = count / (double) nbrow;
+ count = count / (double)nbrow;
if (count < 0.85) {
reloop = true;
}
- computeVVSFirstPhasePoseEstimation(nerror, iter, factor, weighted_error, L, isoJoIdentity_);
+ computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);
iter++;
}
-// std::cout << "\n\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl;
-// std::cout << "Residual=" << m_error.sum() / m_error.size() << std::endl;
-
+ // std::cout << "\n\t First minimization in " << iter << " iteration give
+ // as initial cMo: \n" << cMo << std::endl; std::cout << "Residual=" <<
+ // m_error.sum() / m_error.size() << std::endl;
/*** Second phase ***/
- //Variables for individual weights
- std::map<std::string, vpRobust> mapOfRobustLines;
- std::map<std::string, vpRobust> mapOfRobustCylinders;
- std::map<std::string, vpRobust> mapOfRobustCircles;
- //Init map of robust
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- mapOfRobustLines[it->first] = vpRobust(mapOfNumberOfLines[it->first]);
- mapOfRobustLines[it->first].setIteration(0);
-
- mapOfRobustCylinders[it->first] = vpRobust(mapOfNumberOfCylinders[it->first]);
- mapOfRobustCylinders[it->first].setIteration(0);
-
- mapOfRobustCircles[it->first] = vpRobust(mapOfNumberOfCircles[it->first]);
- mapOfRobustCircles[it->first].setIteration(0);
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
+ edge->m_w_edge = 1;
}
- vpColVector w_lines;
- vpColVector w_cylinders;
- vpColVector w_circles;
-
- std::map<std::string, vpColVector> mapOfWeightLines;
- std::map<std::string, vpColVector> mapOfWeightCylinders;
- std::map<std::string, vpColVector> mapOfWeightCircles;
-
iter = 0;
- vpColVector error_lines;
- vpColVector error_cylinders;
- vpColVector error_circles;
-
vpHomogeneousMatrix cMoPrev;
- vpColVector W_true;
+ vpColVector W_true(nbrow);
vpMatrix L_true;
vpMatrix LVJ_true;
- double mu = 0.01;
- vpColVector m_error_prev(nbrow);
- vpColVector m_w_prev(nbrow);
+ double mu = m_initialMu;
+ vpColVector m_error_prev;
+ vpColVector m_w_prev;
double residu_1 = 1e3;
- double r =1e3-1;
+ double r = 1e3 - 1;
- //while ( ((int)((residu_1 - r)*1e8) != 0 ) && (iter<30))
- while(std::fabs((residu_1 - r)*1e8) > std::numeric_limits<double>::epsilon() && (iter<30))
- {
- L.resize(0,0);
- m_error.resize(0);
-
- error_lines.resize(0);
- error_cylinders.resize(0);
- error_circles.resize(0);
+ // For computeVVSPoseEstimation
+ vpColVector LTR;
+ vpColVector v;
+ vpMatrix LTL;
- std::map<std::string, vpColVector> mapOfErrorLines;
- std::map<std::string, vpColVector> mapOfErrorCylinders;
- std::map<std::string, vpColVector> mapOfErrorCircles;
+ // while ( ((int)((residu_1 - r)*1e8) != 0 ) && (iter<30))
+ while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
+ computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- vpMatrix L_tmp(mapOfNumberOfRows[it->first], 6);
- vpColVector error_lines_tmp(mapOfNumberOfLines[it->first]);
- vpColVector error_cylinders_tmp(mapOfNumberOfCylinders[it->first]);
- vpColVector error_circles_tmp(mapOfNumberOfCircles[it->first]);
+ bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_edgeMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
+ &m_w_prev);
- it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
- vpColVector error_tmp;
- error_tmp.resize(mapOfNumberOfRows[it->first]);
+ L_true = m_L_edgeMulti;
+ vpVelocityTwistMatrix cVo;
- it->second->computeVVSSecondPhase(*mapOfImages[it->first], L_tmp, error_lines_tmp,
- error_cylinders_tmp, error_circles_tmp, error_tmp, lvl);
- L_tmp = L_tmp*mapOfVelocityTwist[it->first];
+ if (computeCovariance) {
+ L_true = m_L_edgeMulti;
+ if (!isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_edgeMulti * cVo * oJo);
+ }
+ }
- L.stack(L_tmp);
- m_error.stack(error_tmp);
+ double wi = 0.0, eri = 0.0;
+ double num = 0.0, den = 0.0;
+ if ((iter == 0) || m_computeInteraction) {
+ for (unsigned int i = 0; i < nbrow; i++) {
+ wi = m_w_edgeMulti[i] * m_factor[i];
+ W_true[i] = wi;
+ eri = m_error_edgeMulti[i];
+ num += wi * vpMath::sqr(eri);
+ den += wi;
- error_lines.stack(error_lines_tmp);
- error_cylinders.stack(error_cylinders_tmp);
- error_circles.stack(error_circles_tmp);
+ m_weightedError_edgeMulti[i] = wi * eri;
- mapOfErrorLines[it->first] = error_lines_tmp;
- mapOfErrorCylinders[it->first] = error_cylinders_tmp;
- mapOfErrorCircles[it->first] = error_circles_tmp;
- }
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
+ }
+ }
+ } else {
+ for (unsigned int i = 0; i < nbrow; i++) {
+ wi = m_w_edgeMulti[i] * m_factor[i];
+ W_true[i] = wi;
+ eri = m_error_edgeMulti[i];
+ num += wi * vpMath::sqr(eri);
+ den += wi;
+
+ m_weightedError_edgeMulti[i] = wi * eri;
+ }
+ }
- bool reStartFromLastIncrement = false;
- computeVVSSecondPhaseCheckLevenbergMarquardt(iter, nbrow, m_error_prev, m_w_prev, cMoPrev, mu, reStartFromLastIncrement);
+ residu_1 = r;
+ r = sqrt(num / den); // Le critere d'arret prend en compte le poids
- if(!reStartFromLastIncrement) {
- w_lines.resize(0);
- w_cylinders.resize(0);
- w_circles.resize(0);
+ 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);
- computeVVSSecondPhaseWeights(iter, nerror, weighted_error, w_lines, w_cylinders, w_circles, mapOfNumberOfLines,
- mapOfNumberOfCylinders, mapOfNumberOfCircles, mapOfWeightLines, mapOfWeightCylinders, mapOfWeightCircles,
- mapOfErrorLines, mapOfErrorCylinders, mapOfErrorCircles, mapOfRobustLines, mapOfRobustCylinders,
- mapOfRobustCircles, 2.0);
+ cMoPrev = cMo;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ }
- if(iter == 0) {
- m_w.resize(nerror);
- m_w = 1;
- }
+ iter++;
+ }
- for(unsigned int cpt = 0, cpt_lines = 0, cpt_cylinders = 0, cpt_circles = 0; cpt < nbrow; cpt++) {
- switch(indexOfFeatures[cpt]) {
- case LINE:
- m_w[cpt] = w_lines[cpt_lines];
- cpt_lines++;
- break;
-
- case CYLINDER:
- m_w[cpt] = w_cylinders[cpt_cylinders];
- cpt_cylinders++;
- break;
-
- case CIRCLE:
- m_w[cpt] = w_circles[cpt_circles];
- cpt_circles++;
- break;
-
- default:
- std::cerr << "Problem with feature type !" << std::endl;
- break;
- }
- }
+ // std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
- computeVVSSecondPhasePoseEstimation(nerror, L, L_true, LVJ_true, W_true, factor, iter, isoJoIdentity_,
- weighted_error, mu, m_error_prev, m_w_prev, cMoPrev, residu_1, r);
- }
+ computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edgeMulti);
- iter++;
+ for (std::map<std::string, vpMbEdgeTracker *>::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_)
+{
+ unsigned int nerror = m_weightedError_edgeMulti.getRows();
-// std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
+ double wi, eri;
+ if ((iter == 0) || m_computeInteraction) {
+ for (unsigned int i = 0; i < nerror; i++) {
+ wi = m_w_edgeMulti[i] * m_factor[i];
+ eri = m_error_edgeMulti[i];
- if(computeCovariance){
- vpMatrix D;
- D.diag(W_true);
+ m_weightedError_edgeMulti[i] = wi * eri;
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity_){
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, m_error, L_true, D);
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_edgeMulti[i][j] = wi * m_L_edgeMulti[i][j];
+ }
}
- else{
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, m_error, LVJ_true, D);
+ } else {
+ for (unsigned int i = 0; i < nerror; i++) {
+ wi = m_w_edgeMulti[i] * m_factor[i];
+ eri = m_error_edgeMulti[i];
+
+ m_weightedError_edgeMulti[i] = wi * eri;
}
}
- unsigned int cpt = 0;
- for(std::map<std::string, unsigned int>::const_iterator it = mapOfNumberOfRows.begin(); it != mapOfNumberOfRows.end(); ++it) {
- for(unsigned int i = 0; i < mapOfNumberOfRows[it->first]; i++) {
- m_mapOfEdgeTrackers[it->first]->m_w[i] = m_w[i+cpt];
- }
- m_mapOfEdgeTrackers[it->first]->updateMovingEdgeWeights();
- cpt += mapOfNumberOfRows[it->first];
- }
-}
-
-void vpMbEdgeMultiTracker::computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
- vpColVector &weighted_error, vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders, std::map<std::string, unsigned int> &mapOfNumberOfCircles,
- std::map<std::string, vpColVector> &mapOfWeightLines, std::map<std::string, vpColVector> &mapOfWeightCylinders,
- std::map<std::string, vpColVector> &mapOfWeightCircles, std::map<std::string, vpColVector> &mapOfErrorLines,
- std::map<std::string, vpColVector> &mapOfErrorCylinders, std::map<std::string, vpColVector> &mapOfErrorCircles,
- std::map<std::string, vpRobust> &mapOfRobustLines, std::map<std::string, vpRobust> &mapOfRobustCylinders,
- std::map<std::string, vpRobust> &mapOfRobustCircles, double threshold) {
- if(iter == 0)
- {
- weighted_error.resize(nerror);
-
- //Init weight size
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- mapOfWeightLines[it->first].resize(mapOfNumberOfLines[it->first]);
- mapOfWeightLines[it->first] = 1;
-
- mapOfWeightCylinders[it->first].resize(mapOfNumberOfCylinders[it->first]);
- mapOfWeightCylinders[it->first] = 1;
-
- mapOfWeightCircles[it->first].resize(mapOfNumberOfCircles[it->first]);
- mapOfWeightCircles[it->first] = 1;
- }
+ vpVelocityTwistMatrix cVo;
- //Set threshold for each robust
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustLines.begin(); it != mapOfRobustLines.end(); ++it) {
- vpCameraParameters current_cam;
- m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
- it->second.setThreshold(threshold / current_cam.get_px());
- }
+ // If all the 6 dof should be estimated, we check if the interaction matrix
+ // is full rank. If not we remove automatically the dof that cannot be
+ // estimated This is particularly useful when consering circles (rank 5) and
+ // cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCylinders.begin(); it != mapOfRobustCylinders.end(); ++it) {
- vpCameraParameters current_cam;
- m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
- it->second.setThreshold(threshold / current_cam.get_px());
+ 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 !");
}
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I - K.AtA();
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCircles.begin(); it != mapOfRobustCircles.end(); ++it) {
- vpCameraParameters current_cam;
- m_mapOfEdgeTrackers[it->first]->getCameraParameters(current_cam);
- it->second.setThreshold( vpMath::sqr(threshold / current_cam.get_px()) );
+ isoJoIdentity_ = false;
}
+ }
- //Compute weights
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorLines.begin();
- it != mapOfErrorLines.end(); ++it) {
- if(mapOfNumberOfLines[it->first] > 0) {
- mapOfRobustLines[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightLines[it->first]);
+ vpColVector v;
+ vpMatrix LTL;
+ vpColVector LTR;
- //Stack weights
- w_lines.stack(mapOfWeightLines[it->first]);
- }
- }
+ 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<double>::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<double>::epsilon()) * LVJTR;
+ v = cVo * v;
+ }
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCylinders.begin();
- it != mapOfErrorCylinders.end(); ++it) {
- if(mapOfNumberOfCylinders[it->first] > 0) {
- mapOfRobustCylinders[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCylinders[it->first]);
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+}
- //Stack weights
- w_cylinders.stack(mapOfWeightCylinders[it->first]);
- }
- }
+void vpMbEdgeMultiTracker::computeVVSInit()
+{
+ unsigned int nbrow = 0;
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCircles.begin();
- it != mapOfErrorCircles.end(); ++it) {
- if(mapOfNumberOfCircles[it->first] > 0) {
- mapOfRobustCircles[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCircles[it->first]);
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
- //Stack weights
- w_circles.stack(mapOfWeightCircles[it->first]);
- }
+ try {
+ edge->computeVVSInit();
+ nbrow += edge->m_error_edge.getRows();
+ } catch (...) {
}
}
- else
- {
- //Set iteration for each robust
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustLines.begin(); it != mapOfRobustLines.end(); ++it) {
- it->second.setIteration(iter);
- }
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCylinders.begin(); it != mapOfRobustCylinders.end(); ++it) {
- it->second.setIteration(iter);
- }
+ if (nbrow < 4) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError,
+ "No data found to compute the interaction matrix...");
+ }
- for(std::map<std::string, vpRobust>::iterator it = mapOfRobustCircles.begin(); it != mapOfRobustCircles.end(); ++it) {
- it->second.setIteration(iter);
- }
+ // Initialize with correct size
+ m_L_edgeMulti.resize(nbrow, 6, false, false);
+ m_w_edgeMulti.resize(nbrow, false);
+ m_error_edgeMulti.resize(nbrow, false);
+ m_weightedError_edgeMulti.resize(nbrow, false);
+ m_factor.resize(nbrow, false);
+}
+
+void vpMbEdgeMultiTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbEdgeMultiTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
+}
- //Compute weights
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorLines.begin();
- it != mapOfErrorLines.end(); ++it) {
- if(mapOfNumberOfLines[it->first] > 0) {
- mapOfRobustLines[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightLines[it->first]);
+void vpMbEdgeMultiTracker::computeVVSInteractionMatrixAndResidu(
+ std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
+{
+ unsigned int start_idx = 0;
- //Stack weights
- w_lines.stack(mapOfWeightLines[it->first]);
- }
- }
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
+ edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCylinders.begin();
- it != mapOfErrorCylinders.end(); ++it) {
- if(mapOfNumberOfCylinders[it->first] > 0) {
- mapOfRobustCylinders[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCylinders[it->first]);
+ edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]);
- //Stack weights
- w_cylinders.stack(mapOfWeightCylinders[it->first]);
- }
- }
+ m_L_edgeMulti.insert(edge->m_L_edge * mapOfVelocityTwist[it->first], start_idx, 0);
+ m_error_edgeMulti.insert(start_idx, edge->m_error_edge);
- for(std::map<std::string, vpColVector>::const_iterator it = mapOfErrorCircles.begin();
- it != mapOfErrorCircles.end(); ++it) {
- if(mapOfNumberOfCircles[it->first] > 0) {
- mapOfRobustCircles[it->first].MEstimator(vpRobust::TUKEY, it->second, mapOfWeightCircles[it->first]);
+ start_idx += edge->m_error_edge.getRows();
+ }
+}
- //Stack weights
- w_circles.stack(mapOfWeightCircles[it->first]);
- }
- }
+void vpMbEdgeMultiTracker::computeVVSWeights()
+{
+ unsigned int start_idx = 0;
+
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
+
+ // Compute weights
+ edge->computeVVSWeights();
+
+ m_w_edgeMulti.insert(start_idx, edge->m_w_edge);
+ start_idx += edge->m_w_edge.getRows();
}
}
@@ -612,13 +500,16 @@ void vpMbEdgeMultiTracker::computeVVSSecondPhaseWeights(const unsigned int iter,
\param cam_ : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeMultiTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &cam_, const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+void vpMbEdgeMultiTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ 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;
@@ -633,13 +524,16 @@ void vpMbEdgeMultiTracker::display(const vpImage<unsigned char>& I, const vpHomo
\param cam_ : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ 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;
@@ -647,7 +541,8 @@ void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneous
}
/*!
- Display the 3D model from a given position of the camera (stereo cameras configuration).
+ Display the 3D model from a given position of the camera (stereo cameras
+ configuration).
\param I1 : The first grayscale image.
\param I2 : The second grayscale image.
@@ -657,26 +552,30 @@ void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneous
\param cam2 : The second camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeMultiTracker::display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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<unsigned char> &I1, const vpImage<unsigned char> &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) {
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::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;
}
}
/*!
- Display the 3D model from a given position of the camera (stereo cameras configuration).
+ Display the 3D model from a given position of the camera (stereo cameras
+ configuration).
\param I1 : The first color image.
\param I2 : The second color image.
@@ -686,47 +585,54 @@ void vpMbEdgeMultiTracker::display(const vpImage<unsigned char>& I1, const vpIma
\param cam2 : The second camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<vpRGBa>& 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<vpRGBa> &I1, const vpImage<vpRGBa> &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) {
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::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;
}
}
/*!
- Display the 3D model from a given position of the camera (multiple cameras configuration).
+ Display the 3D model from a given position of the camera (multiple cameras
+ configuration).
\param mapOfImages : Map of grayscale images.
\param mapOfCameraPoses : Map of camera poses.
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
- //Display only for the given images
- for(std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
- it_img != mapOfImages.end(); ++it_img) {
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) {
+ 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);
} else {
std::cerr << "Missing elements !" << std::endl;
@@ -735,28 +641,32 @@ void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<uns
}
/*!
- Display the 3D model from a given position of the camera (multiple cameras configuration).
+ Display the 3D model from a given position of the camera (multiple cameras
+ configuration).
\param mapOfImages : Map of color images.
\param mapOfCameraPoses : Map of camera poses.
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
- //Display only for the given images
- for(std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
- it_img != mapOfImages.end(); ++it_img) {
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(it_img->first);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) {
+ 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);
} else {
std::cerr << "Missing elements !" << std::endl;
@@ -769,11 +679,12 @@ void vpMbEdgeMultiTracker::display(const std::map<std::string, const vpImage<vpR
\return The vector of camera names.
*/
-std::vector<std::string> vpMbEdgeMultiTracker::getCameraNames() const {
+std::vector<std::string> vpMbEdgeMultiTracker::getCameraNames() const
+{
std::vector<std::string> cameraNames;
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
cameraNames.push_back(it_edge->first);
}
@@ -785,10 +696,11 @@ std::vector<std::string> vpMbEdgeMultiTracker::getCameraNames() const {
\param camera : Copy of the camera parameters used by the tracker.
*/
-void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &camera) const {
- //Get the reference camera parameters
+void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &camera) const
+{
+ // Get the reference camera parameters
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getCameraParameters(camera);
} else {
std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl;
@@ -801,16 +713,17 @@ void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &camera) const
\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 {
- if(m_mapOfEdgeTrackers.size() == 2) {
+void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
+{
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::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;
}
}
@@ -820,9 +733,10 @@ void vpMbEdgeMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCamer
\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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getCameraParameters(camera);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -834,12 +748,13 @@ void vpMbEdgeMultiTracker::getCameraParameters(const std::string &cameraName, vp
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbEdgeMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const {
- //Clear the input map
+void vpMbEdgeMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
+{
+ // Clear the input map
mapOfCameraParameters.clear();
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
vpCameraParameters cam_;
it->second->getCameraParameters(cam_);
mapOfCameraParameters[it->first] = cam_;
@@ -847,14 +762,16 @@ void vpMbEdgeMultiTracker::getCameraParameters(std::map<std::string, vpCameraPar
}
/*!
- Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType for the given camera name.
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType
+ for the given camera name.
\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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
return it->second->getClipping();
} else {
std::cerr << "Cannot find camera: " << cameraName << std::endl;
@@ -868,9 +785,10 @@ unsigned int vpMbEdgeMultiTracker::getClipping(const std::string &cameraName) co
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeMultiTracker::getFaces() {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeMultiTracker::getFaces()
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
return it->second->getFaces();
}
@@ -883,9 +801,10 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeMultiTracker::getFaces() {
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeMultiTracker::getFaces(const std::string &cameraName) {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeMultiTracker::getFaces(const std::string &cameraName)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
return it->second->getFaces();
}
@@ -898,10 +817,11 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeMultiTracker::getFaces(const std::string
\return Reference a map of the face structure for each camera.
*/
-std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeMultiTracker::getFaces() const {
+std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeMultiTracker::getFaces() const
+{
std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > mapOfFaces;
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
mapOfFaces[it->first] = it->second->faces;
}
@@ -912,16 +832,17 @@ std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeMultiTracker::getF
Get the list of the circles tracked for the specified level. Each circle
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param circlesList : The list of the circles of the model.
*/
-void vpMbEdgeMultiTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->getLcircle(circlesList, level);
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -932,17 +853,18 @@ void vpMbEdgeMultiTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesL
Get the list of the circles tracked for the specified level. Each circle
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
- \param cameraName : Camera name for which we want to get the list of vpMbtDistanceCircle.
- \param level : Level corresponding to the list to return.
- \param circlesList : The list of the circles of the model.
+ \param cameraName : Camera name for which we want to get the list of
+ 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<vpMbtDistanceCircle *>& circlesList,
- const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
+ const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getLcircle(circlesList, level);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -953,16 +875,18 @@ void vpMbEdgeMultiTracker::getLcircle(const std::string &cameraName, std::list<v
Get the list of the cylinders tracked for the specified level. Each cylinder
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param cylindersList : The list of the cylinders of the model.
*/
-void vpMbEdgeMultiTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList,
+ const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->getLcylinder(cylindersList, level);
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -973,17 +897,19 @@ void vpMbEdgeMultiTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cyli
Get the list of the cylinders tracked for the specified level. Each cylinder
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
- \param cameraName : Camera name for which we want to get the list of vpMbtDistanceCylinder.
- \param level : Level corresponding to the list to return.
- \param cylindersList : The list of the cylinders of the model.
+ \param cameraName : Camera name for which we want to get the list of
+ 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<vpMbtDistanceCylinder *>& cylindersList,
- const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLcylinder(const std::string &cameraName,
+ std::list<vpMbtDistanceCylinder *> &cylindersList,
+ const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getLcylinder(cylindersList, level);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -991,19 +917,20 @@ void vpMbEdgeMultiTracker::getLcylinder(const std::string &cameraName, std::list
}
/*!
- Get the list of the lines tracked for the specified level. Each line contains
- the list of the vpMeSite.
+ Get the list of the lines tracked for the specified level. Each line
+ contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param linesList : The list of the lines of the model.
*/
-void vpMbEdgeMultiTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->getLline(linesList, level);
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -1011,20 +938,21 @@ void vpMbEdgeMultiTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, c
}
/*!
- Get the list of the lines tracked for the specified level. Each line contains
- the list of the vpMeSite.
+ Get the list of the lines tracked for the specified level. Each line
+ contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
- \param cameraName : Camera name for which we want to get the list of vpMbtDistanceLine.
- \param level : Level corresponding to the list to return.
+ \param cameraName : Camera name for which we want to get the list of
+ 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<vpMbtDistanceLine *>& linesList,
- const unsigned int level) const {
+void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
+ const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getLline(linesList, level);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -1037,9 +965,10 @@ void vpMbEdgeMultiTracker::getLline(const std::string &cameraName, std::list<vpM
\param p_me : Moving edge parameters for the reference camera.
*/
-void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const {
+void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ 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;
@@ -1051,10 +980,11 @@ void vpMbEdgeMultiTracker::getMovingEdge(vpMe &p_me) const {
\return an instance of the moving edge parameters used by the tracker.
*/
-vpMe vpMbEdgeMultiTracker::getMovingEdge() const {
+vpMe vpMbEdgeMultiTracker::getMovingEdge() const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
vpMe me_tmp;
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ 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;
@@ -1070,9 +1000,10 @@ 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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getMovingEdge(p_me);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -1086,7 +1017,8 @@ void vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName, vpMe &p_
\return an instance of the moving edge parameters used by the tracker.
*/
-vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const {
+vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const
+{
vpMe me_tmp;
getMovingEdge(cameraName, me_tmp);
return me_tmp;
@@ -1104,7 +1036,8 @@ vpMe vpMbEdgeMultiTracker::getMovingEdge(const std::string &cameraName) const {
\return the number of good points for the reference camera.
*/
-unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const {
+unsigned int vpMbEdgeMultiTracker::getNbPoints(const unsigned int level) const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
unsigned int nbGoodPoints = 0;
@@ -1131,14 +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 {
- std::map<std::string, vpMbEdgeTracker *>::const_iterator it =
- m_mapOfEdgeTrackers.find(cameraName);
+unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, const unsigned int level) const
+{
+ std::map<std::string, vpMbEdgeTracker *>::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;
@@ -1149,9 +1081,10 @@ unsigned int vpMbEdgeMultiTracker::getNbPoints(const std::string &cameraName, co
\return Number of polygons.
*/
-unsigned int vpMbEdgeMultiTracker::getNbPolygon() const {
+unsigned int vpMbEdgeMultiTracker::getNbPolygon() const
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
return it->second->getNbPolygon();
}
@@ -1160,13 +1093,15 @@ unsigned int vpMbEdgeMultiTracker::getNbPolygon() const {
}
/*!
- Get the number of polygons (faces) representing the object to track for the specified camera.
+ Get the number of polygons (faces) representing the object to track for the
+ specified camera.
\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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
return it->second->getNbPolygon();
}
@@ -1175,14 +1110,16 @@ unsigned int vpMbEdgeMultiTracker::getNbPolygon(const std::string &cameraName) c
}
/*!
- Get the number of polygons (faces) representing the object to track for all the cameras.
+ Get the number of polygons (faces) representing the object to track for all
+ the cameras.
\return Number of polygons for the specified camera.
*/
-std::map<std::string, unsigned int> vpMbEdgeMultiTracker::getMultiNbPolygon() const {
+std::map<std::string, unsigned int> vpMbEdgeMultiTracker::getMultiNbPolygon() const
+{
std::map<std::string, unsigned int> mapOfNbPolygons;
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
mapOfNbPolygons[it->first] = it->second->getNbPolygon();
}
@@ -1195,17 +1132,16 @@ std::map<std::string, unsigned int> vpMbEdgeMultiTracker::getMultiNbPolygon() co
\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<std::string, vpMbEdgeTracker *>::const_iterator it =
- m_mapOfEdgeTrackers.begin();
+ std::map<std::string, vpMbEdgeTracker *>::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;
}
}
@@ -1217,9 +1153,10 @@ void vpMbEdgeMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatri
\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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getPose(cMo_);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -1231,41 +1168,43 @@ void vpMbEdgeMultiTracker::getPose(const std::string &cameraName, vpHomogeneousM
\param mapOfCameraPoses : The map of camera poses for all the cameras.
*/
-void vpMbEdgeMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const {
- //Clear the map
+void vpMbEdgeMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
+{
+ // Clear the map
mapOfCameraPoses.clear();
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
vpHomogeneousMatrix cMo_;
it->second->getPose(cMo_);
mapOfCameraPoses[it->first] = cMo_;
}
}
-void vpMbEdgeMultiTracker::init(const vpImage<unsigned char>& /*I*/) {
-}
+void vpMbEdgeMultiTracker::init(const vpImage<unsigned char> & /*I*/) {}
#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the list points3D_list.
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are given in \e points3D_list.
- \param I : Input image
- \param points3D_list : List of the 3D points (object frame).
- \param displayFile : Path to the image used to display the help. This functionality
- is only available if visp_io module is used.
+ \param I : Input image where the user has to click.
+ \param points3D_list : List of at least 4 3D points with coordinates
+ expressed in meters in the object frame. \param displayFile : Path to the
+ 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<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile) {
- if(m_mapOfEdgeTrackers.empty()) {
+void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile)
+{
+ if (m_mapOfEdgeTrackers.empty()) {
throw vpException(vpTrackingException::initializationError, "There is no camera !");
- } else if(m_mapOfEdgeTrackers.size() > 1) {
+ } else if (m_mapOfEdgeTrackers.size() > 1) {
throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
} else {
- //Get the vpMbEdgeTracker object for the reference camera name
+ // Get the vpMbEdgeTracker object for the reference camera name
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->initClick(I, points3D_list, displayFile);
it->second->getPose(cMo);
} else {
@@ -1277,34 +1216,45 @@ void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char>& I, const std:
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \param I : Input image.
- \param initFile : File containing the points where to click.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \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
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
-void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp) {
- if(m_mapOfEdgeTrackers.empty()) {
+void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
+ const bool displayHelp)
+{
+ if (m_mapOfEdgeTrackers.empty()) {
throw vpException(vpTrackingException::initializationError, "There is no camera !");
- } else if(m_mapOfEdgeTrackers.size() > 1) {
+ } else if (m_mapOfEdgeTrackers.size() > 1) {
throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
} else {
- //Get the vpMbEdgeTracker object for the reference camera name
+ // Get the vpMbEdgeTracker object for the reference camera name
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->initClick(I, initFile, displayHelp);
it->second->getPose(cMo);
} else {
@@ -1316,39 +1266,53 @@ void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char>& I, const std:
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param I1 : Input image for the first camera.
\param I2 : Input image for the second camera.
- \param initFile1 : File containing the points where to click for the first camera.
- \param initFile2 : File containing the points where to click for the second camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
-
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
+ \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
+ 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
+ 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.
+
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
+
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp, const bool firstCameraIsReference) {
- if(m_mapOfEdgeTrackers.size() == 2) {
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
it->second->initClick(I1, initFile1, displayHelp);
- if(firstCameraIsReference) {
- //Set the reference cMo
+ if (firstCameraIsReference) {
+ // Set the reference cMo
it->second->getPose(cMo);
- //Set the reference camera parameters
+ // Set the reference camera parameters
it->second->getCameraParameters(this->cam);
}
@@ -1356,11 +1320,11 @@ void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpI
it->second->initClick(I2, initFile2, displayHelp);
- if(!firstCameraIsReference) {
- //Set the reference cMo
+ if (!firstCameraIsReference) {
+ // Set the reference cMo
it->second->getPose(cMo);
- //Set the reference camera parameters
+ // Set the reference camera parameters
it->second->getCameraParameters(this->cam);
}
} else {
@@ -1371,47 +1335,57 @@ void vpMbEdgeMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpI
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile for the reference camera.
- The other cameras will be automatically initialized and the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param mapOfImages : Map of images.
- \param initFile : File containing the points where to click for the reference camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \param initFile : File containing the points where to click for the
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
- \sa setPathNamePoseSaving()
*/
void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp) {
+ const std::string &initFile, const bool displayHelp)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
- if(it_img != mapOfImages.end()) {
- //Init click on reference camera
+ if (it_img != mapOfImages.end()) {
+ // Init click on reference camera
it_edge->second->initClick(*it_img->second, initFile, displayHelp);
- //Set the reference cMo
+ // Set the reference cMo
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) {
- if(it_edge->first != m_referenceCameraName) {
+ // Set the pose for the others cameras
+ 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<std::string, vpHomogeneousMatrix>::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 {
@@ -1434,53 +1408,62 @@ void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<u
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile.
- The cameras that have not an init file will be automatically initialized but
- the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \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 ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ The cameras that have not an init file will be automatically initialized but
+ the camera transformation matrices have to be set before.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \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
+ teabox.ppm). This image may be used to show where to click.
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp) {
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
- //InitClick for the reference camera
+ 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);
- //Get reference camera pose
+ // Get reference camera pose
it_edge->second->getPose(cMo);
} else {
throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !");
}
- //Vector of missing pose matrices for cameras
+ // Vector of missing pose matrices for cameras
std::vector<std::string> vectorOfMissingCameraPoses;
- //InitClick for all the cameras that have an initFile
- for(it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- if(it_edge->first != m_referenceCameraName) {
+ // InitClick for all the cameras that have an initFile
+ 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()) {
+ 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);
@@ -1488,18 +1471,21 @@ void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<u
}
}
- //SetPose for cameras that do not have an initFile
- for(std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
- it1 != vectorOfMissingCameraPoses.end(); ++it1) {
+ // SetPose for cameras that do not have an initFile
+ for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
+ it1 != vectorOfMissingCameraPoses.end(); ++it1) {
it_img = mapOfImages.find(*it1);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1);
+ std::map<std::string, vpHomogeneousMatrix>::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 {
std::stringstream ss;
- ss << "Missing image or missing camera transformation matrix ! Cannot set the pose for camera: " << (*it1) << " !";
+ ss << "Missing image or missing camera transformation matrix ! Cannot "
+ "set the pose for camera: "
+ << (*it1) << " !";
throw vpException(vpTrackingException::initializationError, ss.str());
}
}
@@ -1507,8 +1493,8 @@ void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<u
#endif //#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialize the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
- The structure of this file is (without the comments):
+ Initialize the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile. The structure of this file is (without the comments):
\code
// The six value of the pose vector
0.0000 // \
@@ -1519,45 +1505,48 @@ void vpMbEdgeMultiTracker::initClick(const std::map<std::string, const vpImage<u
0.0000 // /
\endcode
- Where the three firsts lines refer to the translation and the three last to the rotation in thetaU
- parameterization (see vpThetaUVector).
+ 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.
*/
-void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char>& 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 !");
+void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &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 !");
}
char s[FILENAME_MAX];
- std::fstream finit ;
+ std::fstream finit;
vpPoseVector init_pos;
std::string ext = ".pos";
- size_t pos = initFile.rfind(ext);
+ size_t pos = initFile.rfind(ext);
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
else
- sprintf(s,"%s.pos", initFile.c_str());
+ sprintf(s, "%s.pos", initFile.c_str());
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
std::cerr << "cannot read " << s << std::endl;
throw vpException(vpException::ioError, "cannot read init file");
}
- for (unsigned int i = 0; i < 6; i += 1){
+ for (unsigned int i = 0; i < 6; i += 1) {
finit >> init_pos[i];
}
- //Set the new pose for the reference camera
+ // Set the new pose for the reference camera
cMo.buildFrom(init_pos);
- //Init for the reference camera
+ // Init for the reference camera
std::map<std::string, vpMbEdgeTracker *>::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_ref == m_mapOfEdgeTrackers.end()) {
+ if (it_ref == m_mapOfEdgeTrackers.end()) {
throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
}
@@ -1571,8 +1560,9 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char>& I, const s
\param I : Input image
\param cMo_ : Pose matrix.
*/
-void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_) {
- if(m_mapOfEdgeTrackers.size() != 1) {
+void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &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 !";
throw vpException(vpTrackingException::initializationError, ss.str());
@@ -1580,9 +1570,9 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char>& I, const v
this->cMo = cMo_;
- //Init for the reference camera
+ // Init for the reference camera
std::map<std::string, vpMbEdgeTracker *>::iterator it_ref = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_ref == m_mapOfEdgeTrackers.end()) {
+ if (it_ref == m_mapOfEdgeTrackers.end()) {
throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
}
@@ -1596,7 +1586,8 @@ void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char>& I, const v
\param I : Input image
\param cPo : Pose vector.
*/
-void vpMbEdgeMultiTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo) {
+void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo)
+{
vpHomogeneousMatrix _cMo(cPo);
vpMbEdgeMultiTracker::initFromPose(I, _cMo);
}
@@ -1608,25 +1599,30 @@ void vpMbEdgeMultiTracker::initFromPose (const vpImage<unsigned char>& I, const
\param I2 : Input image for the second camera.
\param c1Mo : Pose matrix for the first camera.
\param c2Mo : Pose matrix for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference
+ camera, otherwise it is the second one.
*/
void vpMbEdgeMultiTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) {
- //For Edge, initFromPose has the same behavior than setPose
- //So, for convenience we call setPose
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
+ // For Edge, initFromPose has the same behavior than setPose
+ // So, for convenience we call setPose
vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}
/*!
- Initialize the tracking thanks to the pose. The camera transformation matrices have to be set before.
+ Initialize the tracking thanks to the pose. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images.
\param cMo_ : Pose matrix for the reference camera.
*/
void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
- //For Edge, initFromPose has the same behavior than setPose
- //So, for convenience we call setPose
+ const vpHomogeneousMatrix &cMo_)
+{
+ // For Edge, initFromPose has the same behavior than setPose
+ // So, for convenience we call setPose
vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
}
@@ -1637,17 +1633,18 @@ void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImag
\param mapOfCameraPoses : Map of pose matrix.
*/
void vpMbEdgeMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
- //For Edge, initFromPose has the same behavior than setPose
- //So, for convenience we call setPose
+ const std::map<std::string, vpHomogeneousMatrix> &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<std::string, const vpImage<unsigned char> * >& mapOfImages,
- std::map<std::string, std::vector<const vpImage<unsigned char>* > >& pyramid)
+void vpMbEdgeMultiTracker::initPyramid(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, std::vector<const vpImage<unsigned char> *> > &pyramid)
{
- for(std::map<std::string, const vpImage<unsigned char> * >::const_iterator it = mapOfImages.begin();
- it != mapOfImages.end(); ++it) {
+ for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it = mapOfImages.begin();
+ it != mapOfImages.end(); ++it) {
pyramid[it->first].resize(scales.size());
vpMbEdgeTracker::initPyramid(*it->second, pyramid[it->first]);
@@ -1656,14 +1653,14 @@ void vpMbEdgeMultiTracker::initPyramid(const std::map<std::string, const vpImage
/*!
Load the xml configuration file.
- From the configuration file, initialize the parameters corresponding to the objects: moving-edges,
- camera and visibility angles.
+ From the configuration file, initialize the parameters corresponding to the
+objects: moving-edges, camera and visibility angles.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -1704,17 +1701,18 @@ void vpMbEdgeMultiTracker::initPyramid(const std::map<std::string, const vpImage
\sa vpXmlParser::cleanup()
*/
-void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile) {
+void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
- //Load ConfigFile for reference camera
+ if (it != m_mapOfEdgeTrackers.end()) {
+ // Load ConfigFile for reference camera
it->second->loadConfigFile(configFile);
it->second->getCameraParameters(cam);
- //Set Moving Edge parameters
+ // Set Moving Edge parameters
this->me = it->second->getMovingEdge();
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1726,33 +1724,35 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile) {
}
/*!
- Load the xml configuration files for the stereo cameras case. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges,
- camera and visibility angles.
+ Load the xml configuration files for the stereo cameras case. An example of
+ such a file is provided in loadConfigFile(const std::string &)
+ documentation. From the configuration file initialize the parameters
+ corresponding to the objects: moving-edges, camera and visibility angles.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile1 : Full name of the xml file for the first camera.
\param configFile2 : Full name of the xml file for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
\sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
*/
void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
- const bool firstCameraIsReference) {
- if(m_mapOfEdgeTrackers.size() == 2) {
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
it->second->loadConfigFile(configFile1);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
it->second->getCameraParameters(cam);
- //Set Moving Edge parameters
+ // Set Moving Edge parameters
this->me = it->second->getMovingEdge();
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1761,13 +1761,13 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, const
it->second->loadConfigFile(configFile2);
- if(!firstCameraIsReference) {
+ if (!firstCameraIsReference) {
it->second->getCameraParameters(cam);
- //Set Moving Edge parameters
+ // Set Moving Edge parameters
this->me = it->second->getMovingEdge();
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1780,23 +1780,24 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::string &configFile1, const
}
/*!
- Load the xml configuration files for all the cameras. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges,
- camera and visibility angles.
+ Load the xml configuration files for all the cameras. An example of such a
+ file is provided in loadConfigFile(const std::string &) documentation. From
+ the configuration file initialize the parameters corresponding to the
+ objects: moving-edges, camera and visibility angles.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param mapOfConfigFiles : Map of xml files.
\sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
*/
-void vpMbEdgeMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+void vpMbEdgeMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_edge->first);
- if(it_config != mapOfConfigFiles.end()) {
+ if (it_config != mapOfConfigFiles.end()) {
it_edge->second->loadConfigFile(it_config->second);
} else {
std::stringstream ss;
@@ -1805,15 +1806,15 @@ void vpMbEdgeMultiTracker::loadConfigFile(const std::map<std::string, std::strin
}
}
- //Set the reference camera parameters
+ // Set the reference camera parameters
std::map<std::string, vpMbEdgeTracker *>::iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->getCameraParameters(cam);
- //Set Moving Edge parameters
+ // Set Moving Edge parameters
this->me = it->second->getMovingEdge();
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1841,17 +1842,18 @@ int main()
}
\endcode
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
\param modelFile : the file containing the the 3D model description.
The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \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) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, const bool verbose)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->loadModel(modelFile, verbose);
}
@@ -1863,23 +1865,24 @@ void vpMbEdgeMultiTracker::loadModel(const std::string &modelFile, const bool ve
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose) {
- if(m_mapOfEdgeTrackers.size() != 1) {
+void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char> &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 !";
throw vpException(vpTrackingException::fatalError, ss.str());
}
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->reInitModel(I, cad_name, cMo_, verbose);
- //Set reference pose
+ // Set reference pose
it_edge->second->getPose(cMo);
modelInitialised = true;
@@ -1889,25 +1892,28 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char>& I, const st
/*!
Re-initialize the model used by the tracker.
- \param I1 : The image containing the object to initialize for the first camera.
- \param I2 : The image containing the object to initialize for the second camera.
- \param cad_name : Path to the file containing the 3D model description.
- \param c1Mo : The new vpHomogeneousMatrix between the first camera and the new model.
- \param c2Mo : The new vpHomogeneousMatrix between the second camera and the new model.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param I1 : The image containing the object to initialize for the first
+ camera. \param I2 : The image containing the object to initialize for the
+ second camera. \param cad_name : Path to the file containing the 3D model
+ description. \param c1Mo : The new vpHomogeneousMatrix between the first
+ camera and the new model. \param c2Mo : The new vpHomogeneousMatrix between
+ the second camera and the new model. \param verbose : verbose option to
+ print additional information when loading CAO model files which include
+ 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<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose, const bool firstCameraIsReference) {
- if(m_mapOfEdgeTrackers.size() == 2) {
+ const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
+ const vpHomogeneousMatrix &c2Mo, const bool verbose,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose);
- if(firstCameraIsReference) {
- //Set reference pose
+ if (firstCameraIsReference) {
+ // Set reference pose
it_edge->second->getPose(cMo);
}
@@ -1915,8 +1921,8 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const v
it_edge->second->reInitModel(I2, cad_name, c2Mo, verbose);
- if(!firstCameraIsReference) {
- //Set reference pose
+ if (!firstCameraIsReference) {
+ // Set reference pose
it_edge->second->getPose(cMo);
}
} else {
@@ -1929,34 +1935,38 @@ void vpMbEdgeMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const v
\param mapOfImages : Map of images.
\param cad_name : Path to the file containing the 3D model description.
- \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras and the current object position.
- \param verbose : Verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras
+ and the current object position. \param verbose : Verbose option to print
+ additional information when loading CAO model files which include other CAO
+ model files.
*/
void vpMbEdgeMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose) {
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ 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
+ // Set reference pose
it_edge->second->getPose(cMo);
} else {
throw vpException(vpTrackingException::fatalError, "Cannot reInitModel for reference camera !");
}
std::vector<std::string> vectorOfMissingCameras;
- for(it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- if(it_edge->first != m_referenceCameraName) {
+ 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()) {
it_edge->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
} else {
vectorOfMissingCameras.push_back(it_edge->first);
@@ -1964,12 +1974,13 @@ void vpMbEdgeMultiTracker::reInitModel(const std::map<std::string, const vpImage
}
}
- for(std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin();
- it != vectorOfMissingCameras.end(); ++it) {
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
+ ++it) {
it_img = mapOfImages.find(*it);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::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);
}
@@ -1980,12 +1991,13 @@ void vpMbEdgeMultiTracker::reInitModel(const std::map<std::string, const vpImage
Reset the tracker. The model is removed and the pose is set to identity.
The tracker needs to be initialized with a new model and a new pose.
*/
-void vpMbEdgeMultiTracker::resetTracker() {
+void vpMbEdgeMultiTracker::resetTracker()
+{
this->cMo.eye();
- //Reset all internal trackers
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ // Reset all internal trackers
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->resetTracker();
}
@@ -1995,11 +2007,11 @@ void vpMbEdgeMultiTracker::resetTracker() {
useOgre = false;
#endif
- compute_interaction = 1;
-// nline = 0; //Not used in vpMbEdgeMultiTracker class
-// ncylinder = 0; //Not used in vpMbEdgeMultiTracker class
- lambda = 1;
-// nbvisiblepolygone = 0; //Not used in vpMbEdgeMultiTracker class
+ m_computeInteraction = true;
+ // nline = 0; //Not used in vpMbEdgeMultiTracker class
+ // ncylinder = 0; //Not used in vpMbEdgeMultiTracker class
+ m_lambda = 1.0;
+ // nbvisiblepolygone = 0; //Not used in vpMbEdgeMultiTracker class
percentageGdPt = 0.4;
angleAppears = vpMath::rad(89);
@@ -2021,11 +2033,12 @@ void vpMbEdgeMultiTracker::resetTracker() {
\param a : new angle in radian.
*/
-void vpMbEdgeMultiTracker::setAngleAppear(const double &a) {
+void vpMbEdgeMultiTracker::setAngleAppear(const double &a)
+{
vpMbTracker::setAngleAppear(a);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setAngleAppear(a);
}
}
@@ -2039,11 +2052,12 @@ void vpMbEdgeMultiTracker::setAngleAppear(const double &a) {
\param a : new angle in radian.
*/
-void vpMbEdgeMultiTracker::setAngleDisappear(const double &a) {
+void vpMbEdgeMultiTracker::setAngleDisappear(const double &a)
+{
vpMbTracker::setAngleDisappear(a);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setAngleDisappear(a);
}
}
@@ -2053,17 +2067,18 @@ void vpMbEdgeMultiTracker::setAngleDisappear(const double &a) {
\param camera : The new camera parameters.
*/
-void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters& camera) {
- if(m_mapOfEdgeTrackers.empty()) {
+void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera)
+{
+ if (m_mapOfEdgeTrackers.empty()) {
throw vpException(vpTrackingException::fatalError, "There is no camera !");
- } else if(m_mapOfEdgeTrackers.size() > 1) {
+ } else if (m_mapOfEdgeTrackers.size() > 1) {
throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
} else {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setCameraParameters(camera);
- //Set reference camera parameters
+ // Set reference camera parameters
this->cam = camera;
} else {
std::stringstream ss;
@@ -2078,20 +2093,22 @@ void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters& camera)
\param camera1 : The new camera parameters for the first camera.
\param camera2 : The new camera parameters for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \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) {
- if(m_mapOfEdgeTrackers.empty()) {
+void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfEdgeTrackers.empty()) {
throw vpException(vpTrackingException::fatalError, "There is no camera !");
- } else if(m_mapOfEdgeTrackers.size() == 2) {
+ } else if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
it->second->setCameraParameters(camera1);
++it;
it->second->setCameraParameters(camera2);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cam = camera1;
} else {
this->cam = camera2;
@@ -2109,12 +2126,13 @@ void vpMbEdgeMultiTracker::setCameraParameters(const vpCameraParameters& camera1
\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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setCameraParameters(camera);
- if(it->first == m_referenceCameraName) {
+ if (it->first == m_referenceCameraName) {
this->cam = camera;
}
} else {
@@ -2129,14 +2147,15 @@ void vpMbEdgeMultiTracker::setCameraParameters(const std::string &cameraName, co
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbEdgeMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+void vpMbEdgeMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_edge->first);
- if(it_cam != mapOfCameraParameters.end()) {
+ if (it_cam != mapOfCameraParameters.end()) {
it_edge->second->setCameraParameters(it_cam->second);
- if(it_edge->first == m_referenceCameraName) {
+ if (it_edge->first == m_referenceCameraName) {
this->cam = it_cam->second;
}
} else {
@@ -2148,15 +2167,18 @@ void vpMbEdgeMultiTracker::setCameraParameters(const std::map<std::string, vpCam
}
/*!
- Set the camera transformation matrix for the specified camera (\f$ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
+ Set the camera transformation matrix for the specified camera (\f$
+ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
\param cameraName : Camera name.
- \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera.
+ \param cameraTransformationMatrix : Camera transformation matrix between the
+ current and the reference camera.
*/
void vpMbEdgeMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix) {
+ const vpHomogeneousMatrix &cameraTransformationMatrix)
+{
std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
- if(it != m_mapOfCameraTransformationMatrix.end()) {
+ if (it != m_mapOfCameraTransformationMatrix.end()) {
it->second = cameraTransformationMatrix;
} else {
std::stringstream ss;
@@ -2167,18 +2189,21 @@ void vpMbEdgeMultiTracker::setCameraTransformationMatrix(const std::string &came
/*!
Set the map of camera transformation matrices
- (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
+ (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1},
+ _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
\param mapOfTransformationMatrix : map of camera transformation matrices.
*/
void vpMbEdgeMultiTracker::setCameraTransformationMatrix(
- const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix) {
- //Check if all cameras have a transformation matrix
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = mapOfTransformationMatrix.find(it_edge->first);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
+{
+ // Check if all cameras have a transformation matrix
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ mapOfTransformationMatrix.find(it_edge->first);
- if(it_camTrans == mapOfTransformationMatrix.end()) {
+ if (it_camTrans == mapOfTransformationMatrix.end()) {
throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !");
}
}
@@ -2193,12 +2218,13 @@ void vpMbEdgeMultiTracker::setCameraTransformationMatrix(
\param flags : New clipping flags.
*/
-void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags) {
- //Set clipping for vpMbEdgeMultiTracker class
+void vpMbEdgeMultiTracker::setClipping(const unsigned int &flags)
+{
+ // Set clipping for vpMbEdgeMultiTracker class
vpMbTracker::setClipping(flags);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setClipping(flags);
}
}
@@ -2211,10 +2237,12 @@ 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) {
- //Set clipping for the given camera, do not change the clipping for vpMbEdgeMultiTracker class
+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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setClipping(flags);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2222,33 +2250,40 @@ void vpMbEdgeMultiTracker::setClipping(const std::string &cameraName, const unsi
}
/*!
- Set if the covaraince matrix has to be computed.
+ Set if the covariance matrix has to be computed.
\param flag : True if the covariance has to be computed, false otherwise
*/
-void vpMbEdgeMultiTracker::setCovarianceComputation(const bool& flag) {
+void vpMbEdgeMultiTracker::setCovarianceComputation(const bool &flag)
+{
vpMbTracker::setCovarianceComputation(flag);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setCovarianceComputation(flag);
}
}
/*!
- Enable to display the features. By features, we mean the moving edges (ME) and the klt points if used.
+ Enable to display the features. By features, we mean the moving edges (ME)
+ and the klt points if used.
- Note that if present, the moving edges can be displayed with different colors:
+ Note that if present, the moving edges can be displayed with different
+ colors:
- If green : The ME is a good point.
- - If blue : The ME is removed because of a contrast problem during the tracking phase.
- - If purple : The ME is removed because of a threshold problem during the tracking phase.
- - If red : The ME is removed because it is rejected by the robust approach in the virtual visual servoing scheme.
+ - If blue : The ME is removed because of a contrast problem during the
+ tracking phase.
+ - If purple : The ME is removed because of a threshold problem during the
+ tracking phase.
+ - If red : The ME is removed because it is rejected by the robust approach
+ in the virtual visual servoing scheme.
\param displayF : set it to true to display the features.
*/
-void vpMbEdgeMultiTracker::setDisplayFeatures(const bool displayF) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setDisplayFeatures(const bool displayF)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setDisplayFeatures(displayF);
}
@@ -2260,11 +2295,12 @@ void vpMbEdgeMultiTracker::setDisplayFeatures(const bool displayF) {
\param dist : Far clipping value.
*/
-void vpMbEdgeMultiTracker::setFarClippingDistance(const double &dist) {
+void vpMbEdgeMultiTracker::setFarClippingDistance(const double &dist)
+{
vpMbTracker::setFarClippingDistance(dist);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setFarClippingDistance(dist);
}
}
@@ -2275,9 +2311,10 @@ 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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setFarClippingDistance(dist);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2285,20 +2322,22 @@ void vpMbEdgeMultiTracker::setFarClippingDistance(const std::string &cameraName,
}
/*!
- Set the threshold value between 0 and 1 over good moving edges ratio. It allows to
- decide if the tracker has enough valid moving edges to compute a pose. 1 means that all
- moving edges should be considered as good to have a valid pose, while 0.1 means that
- 10% of the moving edge are enough to declare a pose valid.
+ Set the threshold value between 0 and 1 over good moving edges ratio. It
+ allows to decide if the tracker has enough valid moving edges to compute a
+ pose. 1 means that all moving edges should be considered as good to have a
+ valid pose, while 0.1 means that 10% of the moving edge are enough to
+ declare a pose valid.
- \param threshold : Value between 0 and 1 that corresponds to the ratio of good
- moving edges that is necessary to consider that the estimated pose is valid.
- Default value is 0.4.
+ \param threshold : Value between 0 and 1 that corresponds to the ratio of
+ good moving edges that is necessary to consider that the estimated pose is
+ valid. Default value is 0.4.
\sa getGoodMovingEdgesRatioThreshold()
*/
-void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold(const double threshold) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold(const double threshold)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setGoodMovingEdgesRatioThreshold(threshold);
}
@@ -2307,65 +2346,76 @@ void vpMbEdgeMultiTracker::setGoodMovingEdgesRatioThreshold(const double thresho
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider a
+ polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \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) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->setGoodNbRayCastingAttemptsRatio(ratio);
- }
+void vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ it->second->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 random point inside the considered polygon.
+/*!
+ Set the number of rays that will be sent toward each polygon for visibility
+ test. Each ray will go from the optic center of the camera to a random point
+ inside the considered polygon.
- \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
+ \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
- \param attempts Number of rays to be sent.
- */
- void vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->setNbRayCastingAttemptsForVisibility(attempts);
- }
+ \param attempts Number of rays to be sent.
+*/
+void vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ it->second->setNbRayCastingAttemptsForVisibility(attempts);
}
+}
#endif
- /*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+/*!
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param name : name of the face we want to modify the LOD parameter.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param name : name of the face we want to modify
+ the LOD parameter.
- \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
- */
-void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &name) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
+ */
+void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &name)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setLod(useLod, name);
}
}
/*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param cameraName : Name of the camera we want to set the LOD.
- \param name : name of the face we want to modify the LOD parameter, if empty all the faces are considered.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param cameraName : Name of the camera we want to
+ set the LOD. \param name : name of the face we want to modify the LOD
+ parameter, if empty all the faces are considered.
\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<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->setLod(useLod, name);
} else {
std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
@@ -2373,34 +2423,39 @@ void vpMbEdgeMultiTracker::setLod(const bool useLod, const std::string &cameraNa
}
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
+ Set the threshold for the minimum line length to be considered as visible in
+ the LOD case.
\param minLineLengthThresh : threshold for the minimum line length in pixel.
\param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinPolygonAreaThresh()
*/
-void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setMinLineLengthThresh(minLineLengthThresh, name);
}
}
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
+ Set the threshold for the minimum line length to be considered as visible in
+ the LOD case.
\param minLineLengthThresh : threshold for the minimum line length in pixel.
\param cameraName : name of the camera to consider.
- \param name : name of the face we want to modify the LOD threshold, if empty all the faces are considered.
+ \param name : name of the face we want to modify the LOD threshold, if empty
+ all the faces are considered.
\sa setLod(), setMinPolygonAreaThresh()
*/
void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
- const std::string &name) {
+ const std::string &name)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->setMinLineLengthThresh(minLineLengthThresh, name);
} else {
std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
@@ -2410,14 +2465,15 @@ void vpMbEdgeMultiTracker::setMinLineLengthThresh(const double minLineLengthThre
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param name : name of the face we want to modify the LOD threshold.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinLineLengthThresh()
*/
-void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
}
}
@@ -2425,17 +2481,19 @@ void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaTh
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param cameraName : name of the camera to consider.
- \param name : name of the face we want to modify the LOD threshold, if empty all the faces are considered.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param cameraName : name of the camera to consider. \param name :
+ name of the face we want to modify the LOD threshold, if empty all the faces
+ are considered.
\sa setLod(), setMinLineLengthThresh()
*/
void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name) {
+ const std::string &name)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(cameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
it_edge->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
} else {
std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
@@ -2447,9 +2505,10 @@ void vpMbEdgeMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaTh
\param me : an instance of vpMe containing all the desired parameters.
*/
-void vpMbEdgeMultiTracker::setMovingEdge(const vpMe &me) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setMovingEdge(const vpMe &me)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setMovingEdge(me);
}
}
@@ -2460,9 +2519,10 @@ 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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setMovingEdge(me);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2474,11 +2534,12 @@ void vpMbEdgeMultiTracker::setMovingEdge(const std::string &cameraName, const vp
\param dist : Near clipping value.
*/
-void vpMbEdgeMultiTracker::setNearClippingDistance(const double &dist) {
+void vpMbEdgeMultiTracker::setNearClippingDistance(const double &dist)
+{
vpMbTracker::setNearClippingDistance(dist);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setNearClippingDistance(dist);
}
}
@@ -2489,9 +2550,10 @@ 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<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(cameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setNearClippingDistance(dist);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2501,16 +2563,17 @@ void vpMbEdgeMultiTracker::setNearClippingDistance(const std::string &cameraName
/*!
Enable/Disable the appearance of Ogre config dialog on startup.
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
*/
-void vpMbEdgeMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setOgreShowConfigDialog(showConfigDialog);
}
}
@@ -2518,19 +2581,21 @@ void vpMbEdgeMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
-void vpMbEdgeMultiTracker::setOgreVisibilityTest(const bool &v) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setOgreVisibilityTest(const bool &v)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setOgreVisibilityTest(v);
}
#ifdef VISP_HAVE_OGRE
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->faces.getOgreContext()->setWindowName("Multi MBT Edge (" + it->first + ")");
}
#endif
@@ -2543,9 +2608,10 @@ void vpMbEdgeMultiTracker::setOgreVisibilityTest(const bool &v) {
\param opt : Optimization method to use.
*/
-void vpMbEdgeMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setOptimizationMethod(opt);
}
@@ -2559,10 +2625,11 @@ void vpMbEdgeMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &
\param I : image corresponding to the desired pose.
\param cMo_ : Pose to affect.
*/
-void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_) {
- if(m_mapOfEdgeTrackers.size() == 1) {
+void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
+{
+ if (m_mapOfEdgeTrackers.size() == 1) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->setPose(I, cMo_);
this->cMo = cMo_;
} else {
@@ -2572,7 +2639,8 @@ void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomo
}
} else {
std::stringstream ss;
- ss << "You are trying to set the pose with only one image and cMo but there are multiple cameras !";
+ ss << "You are trying to set the pose with only one image and cMo but "
+ "there are multiple cameras !";
throw vpException(vpTrackingException::fatalError, ss.str());
}
}
@@ -2585,11 +2653,14 @@ void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomo
\param I2 : Second image corresponding to the desired pose.
\param c1Mo : First pose to affect.
\param c2Mo : Second pose to affect.
- \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : if true, the first camera is the reference,
+ otherwise it is the second one.
*/
void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference) {
- if(m_mapOfEdgeTrackers.size() == 2) {
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
it->second->setPose(I1, c1Mo);
@@ -2597,7 +2668,7 @@ void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpIma
it->second->setPose(I2, c2Mo);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cMo = c1Mo;
} else {
this->cMo = c2Mo;
@@ -2618,26 +2689,28 @@ void vpMbEdgeMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpIma
\param cMo_ : Pose to affect to the reference camera.
*/
void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
+ const vpHomogeneousMatrix &cMo_)
+{
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end()) {
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ if (it_edge != m_mapOfEdgeTrackers.end()) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
- if(it_img != mapOfImages.end()) {
- //Set pose on reference camera
+ if (it_img != mapOfImages.end()) {
+ // Set pose on reference camera
it_edge->second->setPose(*it_img->second, cMo_);
- //Set the reference cMo
+ // Set the reference cMo
cMo = cMo_;
- //Set the pose for the others cameras
- for(it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- if(it_edge->first != m_referenceCameraName) {
+ // Set the pose for the others cameras
+ 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<std::string, vpHomogeneousMatrix>::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 {
@@ -2660,37 +2733,40 @@ void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<uns
/*!
Set the pose to be used in entry of the next call to the track() function.
This pose will be just used once.
- Cameras that do not have pose will be automatically handled but the pose for the reference has to be passed in parameter.
- The camera transformation matrices have to be set before.
+ Cameras that do not have pose will be automatically handled but the pose for
+ the reference has to be passed in parameter. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images corresponding to the desired pose.
\param mapOfCameraPoses : Map of poses to affect.
*/
void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
- //Set the reference cMo
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
+ // Set the reference cMo
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ 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 !");
}
- //Vector of missing pose matrices for cameras
+ // Vector of missing pose matrices for cameras
std::vector<std::string> vectorOfMissingCameraPoses;
- //Set pose for the specified cameras
- for(it_edge = m_mapOfEdgeTrackers.begin(); it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- if(it_edge->first != m_referenceCameraName) {
+ // Set pose for the specified cameras
+ 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()) {
- //Set pose
+ if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ // Set pose
it_edge->second->setPose(*it_img->second, it_camPose->second);
} else {
vectorOfMissingCameraPoses.push_back(it_edge->first);
@@ -2698,17 +2774,20 @@ void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<uns
}
}
- for(std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
- it1 != vectorOfMissingCameraPoses.end(); ++it1) {
+ for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
+ it1 != vectorOfMissingCameraPoses.end(); ++it1) {
it_img = mapOfImages.find(*it1);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1);
+ std::map<std::string, vpHomogeneousMatrix>::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 {
std::stringstream ss;
- ss << "Missing image or missing camera transformation matrix ! Cannot set the pose for camera: " << (*it1) << " !";
+ ss << "Missing image or missing camera transformation matrix ! Cannot "
+ "set the pose for camera: "
+ << (*it1) << " !";
throw vpException(vpTrackingException::fatalError, ss.str());
}
}
@@ -2717,15 +2796,17 @@ void vpMbEdgeMultiTracker::setPose(const std::map<std::string, const vpImage<uns
/*!
Set if the projection error criteria has to be computed.
- \param flag : True if the projection error criteria has to be computed, false otherwise
+ \param flag : True if the projection error criteria has to be computed,
+ false otherwise
*/
-void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag) {
- //Set the flag in the current class
+void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag)
+{
+ // Set the flag in the current class
vpMbTracker::setProjectionErrorComputation(flag);
- //Set the flag for each camera
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ // Set the flag for each camera
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setProjectionErrorComputation(flag);
}
}
@@ -2735,9 +2816,10 @@ void vpMbEdgeMultiTracker::setProjectionErrorComputation(const bool &flag) {
\param referenceCameraName : Name of the reference camera.
*/
-void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCameraName) {
- std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.find(referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
+{
+ std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(referenceCameraName);
+ if (it != m_mapOfEdgeTrackers.end()) {
m_referenceCameraName = referenceCameraName;
} else {
std::stringstream ss;
@@ -2747,17 +2829,18 @@ void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCa
}
/*!
- Set the scales to use to realize the tracking. The vector of boolean activates
- or not the scales to set for the object tracking. The first element of the list
- correspond to the tracking on the full image, the second element corresponds
- to the tracking on an image subsampled by two.
+ Set the scales to use to realize the tracking. The vector of boolean
+ activates or not the scales to set for the object tracking. The first
+ element of the list correspond to the tracking on the full image, the second
+ element corresponds to the tracking on an image subsampled by two.
Using multi scale tracking allows to track the object with greater moves. It
- requires the computation of a pyramid of images, but the total tracking can be
- faster than a tracking based only on the full scale. The pose is computed from
- the smallest image to the biggest. This may be dangerous if the object to
- track is small in the image, because the subsampled scale(s) will have only
- few points to compute the pose (it could result in a loss of precision).
+ requires the computation of a pyramid of images, but the total tracking can
+ be faster than a tracking based only on the full scale. The pose is computed
+ from the smallest image to the biggest. This may be dangerous if the object
+ to track is small in the image, because the subsampled scale(s) will have
+ only few points to compute the pose (it could result in a loss of
+ precision).
\warning This method must be used before the tracker has been initialized (
before the call of the loadConfigFile() or loadModel() methods).
@@ -2766,10 +2849,11 @@ void vpMbEdgeMultiTracker::setReferenceCameraName(const std::string &referenceCa
\param scale : The vector describing the levels to use.
*/
-void vpMbEdgeMultiTracker::setScales(const std::vector<bool>& scale) {
+void vpMbEdgeMultiTracker::setScales(const std::vector<bool> &scale)
+{
vpMbEdgeTracker::setScales(scale);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setScales(scale);
}
}
@@ -2779,25 +2863,28 @@ void vpMbEdgeMultiTracker::setScales(const std::vector<bool>& scale) {
\param v : True to use it, False otherwise
*/
-void vpMbEdgeMultiTracker::setScanLineVisibilityTest(const bool &v) {
- //Set general setScanLineVisibilityTest
+void vpMbEdgeMultiTracker::setScanLineVisibilityTest(const bool &v)
+{
+ // Set general setScanLineVisibilityTest
vpMbTracker::setScanLineVisibilityTest(v);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setScanLineVisibilityTest(v);
}
}
/*!
- Set if the polygons that have the given name have to be considered during the tracking phase.
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
\param name : name of the polygon(s).
\param useEdgeTracking : True if it has to be considered, false otherwise.
*/
-void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setUseEdgeTracking(name, useEdgeTracking);
}
}
@@ -2809,12 +2896,13 @@ void vpMbEdgeMultiTracker::setUseEdgeTracking(const std::string &name, const boo
\param I : The image.
*/
-void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I) {
- //Track only with reference camera
- //Get the reference camera parameters
+void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I)
+{
+ // Track only with reference camera
+ // Get the reference camera parameters
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.find(m_referenceCameraName);
- if(it != m_mapOfEdgeTrackers.end()) {
+ if (it != m_mapOfEdgeTrackers.end()) {
it->second->track(I);
it->second->getPose(cMo);
} else {
@@ -2823,8 +2911,9 @@ void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I) {
throw vpException(vpTrackingException::fatalError, ss.str());
}
- if(computeProjError) {
- //Use the projection error computed by the single vpMbEdgeTracker in m_mapOfEdgeTrackers
+ if (computeProjError) {
+ // Use the projection error computed by the single vpMbEdgeTracker in
+ // m_mapOfEdgeTrackers
projectionError = it->second->getProjectionError();
}
}
@@ -2837,9 +2926,10 @@ void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I) {
\param I1 : The first image.
\param I2 : The second image.
*/
-void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2) {
- //Track with the special case of stereo cameras
- if(m_mapOfEdgeTrackers.size() == 2) {
+void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
+{
+ // Track with the special case of stereo cameras
+ if (m_mapOfEdgeTrackers.size() == 2) {
std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
mapOfImages[it->first] = &I1;
@@ -2861,123 +2951,122 @@ void vpMbEdgeMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage
\param mapOfImages : Map of images.
*/
-void vpMbEdgeMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages) {
- //Check if there is an image for each camera
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(it_edge->first);
+void vpMbEdgeMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ // Check if there is an image for each camera
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
- if(it_img == mapOfImages.end()) {
+ if (it_img == mapOfImages.end()) {
throw vpException(vpTrackingException::fatalError, "Missing images !");
}
}
-
initPyramid(mapOfImages, m_mapOfPyramidalImages);
- unsigned int lvl = (unsigned int) scales.size();
+ unsigned int lvl = (unsigned int)scales.size();
do {
lvl--;
projectionError = 90.0;
- if(scales[lvl]) {
+ if (scales[lvl]) {
vpHomogeneousMatrix cMo_1 = cMo;
- try
- {
+ try {
downScale(lvl);
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
- it1 != m_mapOfEdgeTrackers.end(); ++it1) {
- //Downscale for each camera
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
+ it1 != m_mapOfEdgeTrackers.end(); ++it1) {
+ // Downscale for each camera
it1->second->downScale(lvl);
- //Track moving edges
+ // Track moving edges
try {
it1->second->trackMovingEdge(*m_mapOfPyramidalImages[it1->first][lvl]);
- } catch(...) {
- vpTRACE("Error in moving edge tracking") ;
- throw ;
+ } catch (...) {
+ vpTRACE("Error in moving edge tracking");
+ throw;
}
}
try {
std::map<std::string, const vpImage<unsigned char> *> mapOfPyramidImages;
- for(std::map<std::string, std::vector<const vpImage<unsigned char>* > >::const_iterator
- it = m_mapOfPyramidalImages.begin(); it != m_mapOfPyramidalImages.end(); ++it) {
+ for (std::map<std::string, std::vector<const vpImage<unsigned char> *> >::const_iterator it =
+ m_mapOfPyramidalImages.begin();
+ it != m_mapOfPyramidalImages.end(); ++it) {
mapOfPyramidImages[it->first] = it->second[lvl];
}
computeVVS(mapOfPyramidImages, lvl);
- } catch(...) {
+ } catch (...) {
covarianceMatrix = -1;
throw; // throw the original exception
}
-
- //Test tracking failed only if all testTracking failed
+ // Test tracking failed only if all testTracking failed
bool isOneTestTrackingOk = false;
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- //Set the camera pose
- it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ // Set the camera pose
+ it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
try {
it->second->testTracking();
isOneTestTrackingOk = true;
- } catch(/*vpException &e*/...) {
- // throw e;
+ } catch (/*vpException &e*/...) {
+ // throw e;
}
}
- if(!isOneTestTrackingOk) {
+ if (!isOneTestTrackingOk) {
std::ostringstream oss;
- oss << "Not enough moving edges to track the object. Try to reduce the threshold="
+ oss << "Not enough moving edges to track the object. Try to reduce "
+ "the threshold="
<< percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
throw vpTrackingException(vpTrackingException::fatalError, oss.str());
}
-
- if(displayFeatures) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ if (displayFeatures) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->displayFeaturesOnImage(*mapOfImages[it->first], lvl);
}
}
// Looking for new visible face
bool newvisibleface = false;
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
}
- if(useScanLine) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ if (useScanLine) {
+ for (std::map<std::string, vpMbEdgeTracker *>::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());
+ mapOfImages[it->first]->getHeight());
}
}
try {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->updateMovingEdge(*mapOfImages[it->first]);
}
- } catch(...) {
+ } catch (...) {
throw; // throw the original exception
}
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
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);
- if(computeProjError) {
- //Compute the projection error
+ if (computeProjError) {
+ // Compute the projection error
it->second->computeProjectionError(*mapOfImages[it->first]);
}
}
@@ -2985,36 +3074,33 @@ void vpMbEdgeMultiTracker::track(std::map<std::string, const vpImage<unsigned ch
computeProjectionError();
upScale(lvl);
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->upScale(lvl);
}
- }
- catch(vpException &e)
- {
- if(lvl != 0){
+ } catch (vpException &e) {
+ if (lvl != 0) {
cMo = cMo_1;
reInitLevel(lvl);
upScale(lvl);
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->cMo = cMo_1;
it->second->reInitLevel(lvl);
it->second->upScale(lvl);
}
- }
- else{
+ } else {
upScale(lvl);
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->upScale(lvl);
}
- throw(e) ;
+ throw(e);
}
}
}
- } while(lvl != 0);
+ } while (lvl != 0);
cleanPyramid(m_mapOfPyramidalImages);
}
diff --git a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp
index 18fa7f3..fa9e351 100644
--- a/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbEdgeTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,40 +44,40 @@
*/
#include <visp3/core/vpDebug.h>
-#include <visp3/vision/vpPose.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpExponentialMap.h>
-#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/core/vpRobust.h>
-#include <visp3/core/vpMatrixException.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPolygon3D.h>
#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/mbt/vpMbtXmlParser.h>
-#include <visp3/core/vpPolygon3D.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/vision/vpPose.h>
-#include <limits>
-#include <string>
-#include <sstream>
#include <float.h>
+#include <limits>
#include <map>
-
+#include <sstream>
+#include <string>
/*!
Basic constructor
*/
vpMbEdgeTracker::vpMbEdgeTracker()
- : compute_interaction(1), lambda(1), me(), lines(1), circles(1), cylinders(1), nline(0), ncircle(0), ncylinder(0),
- nbvisiblepolygone(0), percentageGdPt(0.4), scales(1),
- Ipyramid(0), scaleLevel(0), nbFeaturesForProjErrorComputation(0)
+ : 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);
scales[0] = true;
-
+
#ifdef VISP_HAVE_OGRE
faces.getOgreContext()->setWindowName("MBT Edge");
#endif
@@ -87,70 +88,71 @@ vpMbEdgeTracker::vpMbEdgeTracker()
*/
vpMbEdgeTracker::~vpMbEdgeTracker()
{
- vpMbtDistanceLine *l ;
+ vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
vpMbtDistanceCircle *ci;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if (l!=NULL){
- delete l ;
+ if (l != NULL) {
+ delete l;
}
- l = NULL ;
+ l = NULL;
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
cy = *it;
- if (cy!=NULL){
- delete cy ;
+ if (cy != NULL) {
+ delete cy;
}
- cy = NULL ;
+ cy = NULL;
}
lines[i].clear();
cylinders[i].clear();
}
}
- for (unsigned int i = 0; i < circles.size(); i += 1){
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (unsigned int i = 0; i < circles.size(); i += 1) {
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if (ci!=NULL){
- delete ci ;
+ if (ci != NULL) {
+ delete ci;
}
- ci = NULL ;
+ ci = NULL;
}
circles[i].clear();
}
cleanPyramid(Ipyramid);
}
-/*!
+/*!
Set the moving edge parameters.
-
+
\param p_me : an instance of vpMe containing all the desired parameters
*/
-void
-vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
+void vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
{
this->me = p_me;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
vpMbtDistanceLine *l = *it;
- l->setMovingEdge(&(this->me)) ;
+ l->setMovingEdge(&(this->me));
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
vpMbtDistanceCylinder *cy = *it;
- cy->setMovingEdge(&(this->me)) ;
+ cy->setMovingEdge(&(this->me));
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
vpMbtDistanceCircle *ci = *it;
- ci->setMovingEdge(&(this->me)) ;
+ ci->setMovingEdge(&(this->me));
}
}
}
@@ -158,179 +160,166 @@ vpMbEdgeTracker::setMovingEdge(const vpMe &p_me)
/*!
Compute the visual servoing loop to get the pose of the feature set.
-
- \exception vpTrackingException::notEnoughPointError if the number of detected
- feature is equal to zero.
-
- \param _I : The current image.
+
+ \exception vpTrackingException::notEnoughPointError if the number of
+ detected feature is equal to zero.
+
+ \param _I : The current image.
\param lvl : The level in the pyramid scale.
*/
-void
-vpMbEdgeTracker::computeVVS(const vpImage<unsigned char>& _I, const unsigned int lvl)
+void vpMbEdgeTracker::computeVVS(const vpImage<unsigned char> &_I, const unsigned int lvl)
{
double residu_1 = 1e3;
- double r = 1e3-1;
-
- //vpColVector w;
- vpColVector factor;
- //vpColVector error; // s-s*
- vpColVector weighted_error; // Weighted error vector wi(s-s)*
+ double r = 1e3 - 1;
unsigned int iter = 0;
- //Nombre de moving edges
- unsigned int nbrow = 0;
- unsigned int nberrors_lines = 0;
- unsigned int nberrors_cylinders = 0;
- unsigned int nberrors_circles = 0;
-
- nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
-
- if (nbrow==0){
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "No data found to compute the interaction matrix...");
- }
-
- vpMatrix L(nbrow,6);
-
- // compute the error vector
- m_error.resize(nbrow);
- unsigned int nerror = m_error.getRows();
+ computeVVSInit();
+ unsigned int nbrow = m_error_edge.getRows();
bool reloop = true;
-
+
bool isoJoIdentity_ = isoJoIdentity; // Backup since it can be modified if L is not full rank
if (isoJoIdentity_)
oJo.eye();
/*** First phase ***/
- while ( reloop == true && iter<10)
- {
- if(iter==0)
- {
- weighted_error.resize(nerror);
- m_w.resize(nerror);
- m_w = 0;
- factor.resize(nerror);
- factor = 1;
- }
-
+ while (reloop == true && iter < 10) {
double count = 0;
- reloop = false;
-
- computeVVSFirstPhase(_I, iter, L, factor, count, m_error, m_w, lvl);
+ computeVVSFirstPhase(_I, iter, count, lvl);
count = count / (double)nbrow;
- if (count < 0.85){
- reloop = true;
+ if (count >= 0.85) {
+ reloop = false;
}
- computeVVSFirstPhasePoseEstimation(nerror, iter, factor, weighted_error, L, isoJoIdentity_);
+ computeVVSFirstPhasePoseEstimation(iter, isoJoIdentity_);
iter++;
}
-// std::cout << "\t First minimization in " << iter << " iteration give as initial cMo: \n" << cMo << std::endl ;
-
-
-/*** Second phase ***/
-
- vpRobust robust_lines(nberrors_lines);
- vpRobust robust_cylinders(nberrors_cylinders);
- vpRobust robust_circles(nberrors_circles);
- robust_lines.setIteration(0);
- robust_cylinders.setIteration(0);
- robust_circles.setIteration(0);
- iter = 0;
- vpColVector w_lines(nberrors_lines);
- vpColVector w_cylinders(nberrors_cylinders);
- vpColVector w_circles(nberrors_circles);
- vpColVector error_lines(nberrors_lines);
- vpColVector error_cylinders(nberrors_cylinders);
- vpColVector error_circles(nberrors_circles);
+ // std::cout << "\t First minimization in " << iter << " iteration give as
+ // initial cMo: \n" << cMo << std::endl;
+ /*** Second phase ***/
vpHomogeneousMatrix cMoPrev;
- vpColVector W_true;
+ vpColVector W_true(nbrow);
vpMatrix L_true;
vpMatrix LVJ_true;
- double mu = 0.01;
- vpColVector m_error_prev(nbrow);
- vpColVector m_w_prev(nbrow);
-
- //while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
- while(std::fabs((residu_1 - r)*1e8) > std::numeric_limits<double>::epsilon() && (iter<30))
- {
- computeVVSSecondPhase(_I, L, error_lines, error_cylinders, error_circles, m_error, lvl);
+ double mu = m_initialMu;
+ vpColVector m_error_prev;
+ vpColVector m_w_prev;
+
+ // To avoid to create these matrices each iteration
+ vpMatrix LTL;
+ vpColVector LTR;
+ vpColVector v;
+
+ iter = 0;
+ m_w_edge = 1;
+
+ // while ( ((int)((residu_1 - r)*1e8) !=0 ) && (iter<30))
+ while (std::fabs((residu_1 - r) * 1e8) > std::numeric_limits<double>::epsilon() && (iter < m_maxIter)) {
+ computeVVSInteractionMatrixAndResidu(_I);
bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_edge, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
+ &m_w_edge, &m_w_prev);
+
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
- computeVVSSecondPhaseCheckLevenbergMarquardt(iter, nbrow, m_error_prev, m_w_prev, cMoPrev, mu, reStartFromLastIncrement);
+ L_true = m_L_edge;
+ vpVelocityTwistMatrix cVo;
- if(!reStartFromLastIncrement){
- computeVVSSecondPhaseWeights(iter, nerror, nbrow, weighted_error, robust_lines, robust_cylinders, robust_circles,
- w_lines, w_cylinders, w_circles, error_lines, error_cylinders, error_circles, nberrors_lines, nberrors_cylinders,
- nberrors_circles);
+ if (computeCovariance) {
+ L_true = m_L_edge;
+ if (!isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_edge * cVo * oJo);
+ }
+ }
- computeVVSSecondPhasePoseEstimation(nerror, L, L_true, LVJ_true, W_true, factor, iter, isoJoIdentity_,
- weighted_error, mu, m_error_prev, m_w_prev, cMoPrev, residu_1, r);
+ double wi = 0.0, eri = 0.0;
+ double num = 0.0, den = 0.0;
+ if ((iter == 0) || m_computeInteraction) {
+ for (unsigned int i = 0; i < nbrow; i++) {
+ wi = m_w_edge[i] * m_factor[i];
+ W_true[i] = wi;
+ eri = m_error_edge[i];
+ num += wi * vpMath::sqr(eri);
+ den += wi;
+
+ m_weightedError_edge[i] = wi * eri;
+
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_edge[i][j] = wi * m_L_edge[i][j];
+ }
+ }
+ } else {
+ for (unsigned int i = 0; i < nbrow; i++) {
+ wi = m_w_edge[i] * m_factor[i];
+ W_true[i] = wi;
+ eri = m_error_edge[i];
+ num += wi * vpMath::sqr(eri);
+ den += wi;
+
+ m_weightedError_edge[i] = wi * eri;
+ }
+ }
+
+ 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);
+
+ cMoPrev = cMo;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
} // endif(!restartFromLast)
iter++;
}
-// std::cout << "VVS estimate pose cMo:\n" << cMo << std::endl;
- if(computeCovariance){
- vpMatrix D;
- D.diag(W_true);
-
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity_){
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
- }
- else{
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
- }
- }
+ computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMoPrev, L_true, LVJ_true, m_error_edge);
updateMovingEdgeWeights();
}
-void
-vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char>& _I, const unsigned int iter, vpMatrix &L,
- vpColVector &factor, double &count, vpColVector &error, vpColVector &w_mbt, const unsigned int lvl) {
+void vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char> &_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
+ // Parametre pour la premiere phase d'asservissement
double e_prev = 0, e_cur, e_next;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked())
- {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
l = *it;
l->computeInteractionMatrixError(cMo);
double fac = 1;
- if (iter == 0)
- {
- for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex) {
+ if (iter == 0) {
+ for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
+ ++itindex) {
int index = *itindex;
- if (l->hiddenface->isAppearing((unsigned int)index))
- {
+ if (l->hiddenface->isAppearing((unsigned int)index)) {
fac = 0.2;
break;
}
- if(l->closeToImageBorder(_I, 10))
- {
+ if (l->closeToImageBorder(_I, 10)) {
fac = 0.1;
break;
}
@@ -341,187 +330,165 @@ vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char>& _I, const un
unsigned int indexFeature = 0;
- for(unsigned int a = 0 ; a < l->meline.size() ; a++)
- {
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
if (iter == 0 && l->meline[a] != NULL)
itListLine = l->meline[a]->getMeList().begin();
- for (unsigned int i=0 ; i < l->nbFeature[a] ; i++)
- {
- for (unsigned int j=0; j < 6 ; j++)
- {
- L[n+i][j] = l->L[indexFeature][j]; //On remplit la matrice d'interaction globale
+ 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
}
- error[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 (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+ if (m_error_edge[n + i] <= limite)
+ count = count + 1.0; // Si erreur proche de 0 on incremente cur
- w_mbt[n+i] = 0;
+ m_w_edge[n + i] = 0;
- if (iter == 0)
- {
- factor[n+i] = fac;
+ if (iter == 0) {
+ m_factor[n + i] = fac;
vpMeSite site = *itListLine;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
++itListLine;
}
- //If pour la premiere extremite des moving edges
- if (indexFeature == 0)
- {
+ // If pour la premiere extremite des moving edges
+ if (indexFeature == 0) {
e_cur = l->error[0];
- if (l->nbFeature[a] > 1)
- {
+ 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) )
- {
- w_mbt[n+i] = 1/*0.5*/;
+ 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;
- }
- else w_mbt[n+i] = 1;
+ } else
+ m_w_edge[n + i] = 1;
}
- //If pour la derniere extremite des moving edges
- else if(indexFeature == l->nbFeatureTotal-1)
- {
+ // If pour la derniere extremite des moving edges
+ else if (indexFeature == l->nbFeatureTotal - 1) {
e_cur = l->error[indexFeature];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- w_mbt[n+i] += 1/*0.5*/;
+ if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
+ m_w_edge[n + i] += 1 /*0.5*/;
}
}
- else
- {
+ else {
e_cur = l->error[indexFeature];
- e_next = l->error[indexFeature+1];
- if ( fabs(e_cur - e_prev) < limite )
- {
- w_mbt[n+i] += 0.5;
+ e_next = l->error[indexFeature + 1];
+ if (fabs(e_cur - e_prev) < limite) {
+ m_w_edge[n + i] += 0.5;
}
- if ( fabs(e_cur - e_next) < limite )
- {
- w_mbt[n+i] += 0.5;
+ if (fabs(e_cur - e_next) < limite) {
+ m_w_edge[n + i] += 0.5;
}
e_prev = e_cur;
}
indexFeature++;
}
- n += l->nbFeature[a] ;
+ n += l->nbFeature[a];
}
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it) {
- if((*it)->isTracked())
- {
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
cy->computeInteractionMatrixError(cMo, _I);
double fac = 1.0;
std::list<vpMeSite>::const_iterator itCyl1;
std::list<vpMeSite>::const_iterator itCyl2;
- if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)){
+ if (iter == 0 && (cy->meline1 != NULL || cy->meline2 != NULL)) {
itCyl1 = cy->meline1->getMeList().begin();
itCyl2 = cy->meline2->getMeList().begin();
}
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j]; //On remplit la matrice d'interaction globale
+ 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
}
- error[n+i] = cy->error[i]; //On remplit la matrice d'erreur
+ m_error_edge[n + i] = cy->error[i]; // On remplit la matrice d'erreur
- if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+ if (m_error_edge[n + i] <= limite)
+ count = count + 1.0; // Si erreur proche de 0 on incremente cur
- w_mbt[n+i] = 0;
+ m_w_edge[n + i] = 0;
- if (iter == 0)
- {
- factor[n+i] = fac;
+ if (iter == 0) {
+ m_factor[n + i] = fac;
vpMeSite site;
- if(i<cy->nbFeaturel1) {
- site= *itCyl1;
+ if (i < cy->nbFeaturel1) {
+ site = *itCyl1;
++itCyl1;
- }
- else{
- site= *itCyl2;
+ } else {
+ site = *itCyl2;
++itCyl2;
}
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
}
- //If pour la premiere extremite des moving edges
- if (i == 0)
- {
+ // If pour la premiere extremite des moving edges
+ if (i == 0) {
e_cur = cy->error[0];
- if (cy->nbFeature > 1)
- {
+ if (cy->nbFeature > 1) {
e_next = cy->error[1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- w_mbt[n+i] = 1/*0.5*/;
+ 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;
- }
- else w_mbt[n+i] = 1;
+ } else
+ m_w_edge[n + i] = 1;
}
- if (i == cy->nbFeaturel1)
- {
+ if (i == cy->nbFeaturel1) {
e_cur = cy->error[i];
- if (cy->nbFeaturel2 > 1)
- {
- e_next = cy->error[i+1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- w_mbt[n+i] = 1/*0.5*/;
+ if (cy->nbFeaturel2 > 1) {
+ e_next = cy->error[i + 1];
+ if (fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next)) {
+ m_w_edge[n + i] = 1 /*0.5*/;
}
e_prev = e_cur;
- }
- else w_mbt[n+i] = 1;
+ } else
+ m_w_edge[n + i] = 1;
}
- //If pour la derniere extremite des moving edges
- else if(i == cy->nbFeaturel1-1)
- {
+ // If pour la derniere extremite des moving edges
+ else if (i == cy->nbFeaturel1 - 1) {
e_cur = cy->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- w_mbt[n+i] += 1/*0.5*/;
+ 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)
- {
+ // 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) )
- {
- w_mbt[n+i] += 1/*0.5*/;
+ if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
+ m_w_edge[n + i] += 1 /*0.5*/;
}
}
- else
- {
+ else {
e_cur = cy->error[i];
- e_next = cy->error[i+1];
- if ( fabs(e_cur - e_prev) < limite ){
- w_mbt[n+i] += 0.5;
+ e_next = cy->error[i + 1];
+ if (fabs(e_cur - e_prev) < limite) {
+ m_w_edge[n + i] += 0.5;
}
- if ( fabs(e_cur - e_next) < limite ){
- w_mbt[n+i] += 0.5;
+ if (fabs(e_cur - e_next) < limite) {
+ m_w_edge[n + i] += 0.5;
}
e_prev = e_cur;
}
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it) {
- if((*it)->isTracked())
- {
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
ci->computeInteractionMatrixError(cMo);
double fac = 1.0;
@@ -531,146 +498,143 @@ vpMbEdgeTracker::computeVVSFirstPhase(const vpImage<unsigned char>& _I, const un
itCir = ci->meEllipse->getMeList().begin();
}
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j]; //On remplit la matrice d'interaction globale
+ 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
}
- error[n+i] = ci->error[i]; //On remplit la matrice d'erreur
+ m_error_edge[n + i] = ci->error[i]; // On remplit la matrice d'erreur
- if (error[n+i] <= limite) count = count+1.0; //Si erreur proche de 0 on incremente cur
+ if (m_error_edge[n + i] <= limite)
+ count = count + 1.0; // Si erreur proche de 0 on incremente cur
- w_mbt[n+i] = 0;
+ m_w_edge[n + i] = 0;
- if (iter == 0)
- {
- factor[n+i] = fac;
+ if (iter == 0) {
+ m_factor[n + i] = fac;
vpMeSite site = *itCir;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
++itCir;
}
- //If pour la premiere extremite des moving edges
- if (i == 0)
- {
+ // If pour la premiere extremite des moving edges
+ if (i == 0) {
e_cur = ci->error[0];
- if (ci->nbFeature > 1)
- {
+ if (ci->nbFeature > 1) {
e_next = ci->error[1];
- if ( fabs(e_cur - e_next) < limite && vpMath::sign(e_cur) == vpMath::sign(e_next) )
- {
- w_mbt[n+i] = 1/*0.5*/;
+ 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;
- }
- else w_mbt[n+i] = 1;
+ } else
+ m_w_edge[n + i] = 1;
}
- //If pour la derniere extremite des moving edges
- else if(i == ci->nbFeature-1)
- {
+ // If pour la derniere extremite des moving edges
+ else if (i == ci->nbFeature - 1) {
e_cur = ci->error[i];
- if ( fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev) )
- {
- w_mbt[n+i] += 1/*0.5*/;
+ if (fabs(e_cur - e_prev) < limite && vpMath::sign(e_cur) == vpMath::sign(e_prev)) {
+ m_w_edge[n + i] += 1 /*0.5*/;
}
}
- else
- {
+ else {
e_cur = ci->error[i];
- e_next = ci->error[i+1];
- if ( fabs(e_cur - e_prev) < limite ){
- w_mbt[n+i] += 0.5;
+ e_next = ci->error[i + 1];
+ if (fabs(e_cur - e_prev) < limite) {
+ m_w_edge[n + i] += 0.5;
}
- if ( fabs(e_cur - e_next) < limite ){
- w_mbt[n+i] += 0.5;
+ if (fabs(e_cur - e_next) < limite) {
+ m_w_edge[n + i] += 0.5;
}
e_prev = e_cur;
}
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
}
-void
-vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl) {
+void vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage<unsigned char> &I, const unsigned int lvl)
+{
vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
vpMbtDistanceCircle *ci;
unsigned int n = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
l = *it;
l->computeInteractionMatrixError(cMo);
double fac = 1;
- for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
+ ++itindex) {
int index = *itindex;
if (l->hiddenface->isAppearing((unsigned int)index)) {
fac = 0.2;
break;
}
- if(l->closeToImageBorder(I, 10)){
+ if (l->closeToImageBorder(I, 10)) {
fac = 0.1;
break;
}
}
unsigned int indexFeature = 0;
- for(unsigned int a = 0 ; a < l->meline.size(); a++){
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
std::list<vpMeSite>::const_iterator itListLine;
- if (l->meline[a] != NULL)
- {
+ if (l->meline[a] != NULL) {
itListLine = l->meline[a]->getMeList().begin();
- for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
- factor[n+i] = fac;
- vpMeSite site = *itListLine;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itListLine;
- indexFeature++;
+ for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
+ m_factor[n + i] = fac;
+ vpMeSite site = *itListLine;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
+ ++itListLine;
+ indexFeature++;
}
- n+= l->nbFeature[a] ;
+ n += l->nbFeature[a];
}
}
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
cy->computeInteractionMatrixError(cMo, I);
std::list<vpMeSite>::const_iterator itCyl1;
std::list<vpMeSite>::const_iterator itCyl2;
- if ((cy->meline1 != NULL || cy->meline2 != NULL)){
+ if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
itCyl1 = cy->meline1->getMeList().begin();
itCyl2 = cy->meline2->getMeList().begin();
double fac = 1.0;
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- factor[n+i] = fac;
+ for (unsigned int i = 0; i < cy->nbFeature; i++) {
+ m_factor[n + i] = fac;
vpMeSite site;
- if(i<cy->nbFeaturel1) {
- site= *itCyl1;
+ if (i < cy->nbFeaturel1) {
+ site = *itCyl1;
++itCyl1;
- }
- else{
- site= *itCyl2;
+ } else {
+ site = *itCyl2;
++itCyl2;
}
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
ci->computeInteractionMatrixError(cMo);
@@ -679,60 +643,62 @@ vpMbEdgeTracker::computeVVSFirstPhaseFactor(const vpImage<unsigned char>& I, vpC
itCir = ci->meEllipse->getMeList().begin();
double fac = 1.0;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- factor[n+i] = fac;
+ for (unsigned int i = 0; i < ci->nbFeature; i++) {
+ m_factor[n + i] = fac;
vpMeSite site = *itCir;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ m_factor[n + i] = 0.2;
++itCir;
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
}
}
-void
-vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int nerror, const unsigned int iter, const vpColVector &factor,
- vpColVector &weighted_error, vpMatrix &L, bool &isoJoIdentity_) {
+void vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int iter, bool &isoJoIdentity_)
+{
+ unsigned int nerror = m_weightedError_edge.getRows();
double wi, eri;
- if((iter==0) || compute_interaction) {
+ if ((iter == 0) || m_computeInteraction) {
for (unsigned int i = 0; i < nerror; i++) {
- wi = m_w[i]*factor[i];
- eri = m_error[i];
+ wi = m_w_edge[i] * m_factor[i];
+ eri = m_error_edge[i];
- weighted_error[i] = wi*eri;
+ m_weightedError_edge[i] = wi * eri;
for (unsigned int j = 0; j < 6; j++) {
- L[i][j] = wi*L[i][j];
+ m_L_edge[i][j] = wi * m_L_edge[i][j];
}
}
} else {
- for(unsigned int i = 0; i < nerror; i++) {
- wi = m_w[i]*factor[i];
- eri = m_error[i];
+ for (unsigned int i = 0; i < nerror; i++) {
+ wi = m_w_edge[i] * m_factor[i];
+ eri = m_error_edge[i];
- weighted_error[i] = wi*eri;
+ m_weightedError_edge[i] = wi * eri;
}
}
vpVelocityTwistMatrix cVo;
- // If all the 6 dof should be estimated, we check if the interaction matrix is full rank.
- // If not we remove automatically the dof that cannot be estimated
- // This is particularly useful when consering circles (rank 5) and cylinders (rank 4)
+ // If all the 6 dof should be estimated, we check if the interaction matrix
+ // is full rank. If not we remove automatically the dof that cannot be
+ // estimated This is particularly useful when consering circles (rank 5) and
+ // cylinders (rank 4)
if (isoJoIdentity_) {
cVo.buildFrom(cMo);
vpMatrix K; // kernel
- unsigned int rank = (L*cVo).kernel(K);
- if(rank == 0) {
+ unsigned int rank = (m_L_edge * cVo).kernel(K);
+ if (rank == 0) {
throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
}
if (rank != 6) {
vpMatrix I; // Identity
I.eye(6);
- oJo = I-K.AtA();
+ oJo = I - K.AtA();
isoJoIdentity_ = false;
}
@@ -742,27 +708,75 @@ vpMbEdgeTracker::computeVVSFirstPhasePoseEstimation(const unsigned int nerror, c
vpMatrix LTL;
vpColVector LTR;
- if(isoJoIdentity_){
- LTL = L.AtA();
- computeJTR(L, weighted_error, LTR);
- v = -0.7*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
- }
- else{
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpColVector LVJTR;
- computeJTR(LVJ, weighted_error, LVJTR);
- v = -0.7*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
+ 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<double>::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<double>::epsilon()) * LVJTR;
+ v = cVo * v;
}
- cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
}
-void
-vpMbEdgeTracker::computeVVSSecondPhase(const vpImage<unsigned char>& _I, vpMatrix &L, vpColVector &error_lines,
- vpColVector &error_cylinders, vpColVector &error_circles, vpColVector &error, const unsigned int lvl) {
+void vpMbEdgeTracker::computeVVSInit()
+{
+ // Nombre de moving edges
+ unsigned int nbrow = 0;
+ unsigned int nberrors_lines = 0;
+ unsigned int nberrors_cylinders = 0;
+ unsigned int nberrors_circles = 0;
+
+ nbrow = initMbtTracking(nberrors_lines, nberrors_cylinders, nberrors_circles);
+
+ if (nbrow == 0) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError,
+ "No data found to compute the interaction matrix...");
+ }
+
+ m_L_edge.resize(nbrow, 6, false, false);
+ m_error_edge.resize(nbrow, false);
+
+ m_weightedError_edge.resize(nbrow, false);
+ m_w_edge.resize(nbrow, false);
+ m_w_edge = 1;
+ m_factor.resize(nbrow, false);
+ m_factor = 1;
+
+ m_robustLines.resize(nberrors_lines);
+ m_robustCylinders.resize(nberrors_cylinders);
+ m_robustCircles.resize(nberrors_circles);
+ m_robustLines.setThreshold(2.0 / cam.get_px());
+ m_robustCylinders.setThreshold(2.0 / cam.get_px());
+ m_robustCircles.setThreshold(vpMath::sqr(2.0 / cam.get_px()));
+
+ m_wLines.resize(nberrors_lines, false);
+ m_wLines = 1;
+ m_wCylinders.resize(nberrors_cylinders, false);
+ m_wCylinders = 1;
+ m_wCircles.resize(nberrors_circles, false);
+ m_wCircles = 1;
+
+ m_errorLines.resize(nberrors_lines, false);
+ m_errorCylinders.resize(nberrors_cylinders, false);
+ m_errorCircles.resize(nberrors_circles, false);
+}
+
+void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbEdgeTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
+}
+
+void vpMbEdgeTracker::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> &_I)
+{
vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
vpMbtDistanceCircle *ci;
@@ -772,267 +786,95 @@ vpMbEdgeTracker::computeVVSSecondPhase(const vpImage<unsigned char>& _I, vpMatri
unsigned int ncylinders = 0;
unsigned int ncircles = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ if ((*it)->isTracked()) {
l = *it;
- l->computeInteractionMatrixError(cMo) ;
- for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
- for (unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = l->L[i][j];
- error[n+i] = l->error[i];
- error_lines[nlines+i] = error[n+i];
+ l->computeInteractionMatrixError(cMo);
+ for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_edge[n + i][j] = l->L[i][j];
+ m_error_edge[n + i] = l->error[i];
+ m_errorLines[nlines + i] = m_error_edge[n + i];
}
}
- n+= l->nbFeatureTotal;
- nlines+= l->nbFeatureTotal;
+ n += l->nbFeatureTotal;
+ nlines += l->nbFeatureTotal;
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
- cy->computeInteractionMatrixError(cMo, _I) ;
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j];
- error[n+i] = cy->error[i];
- error_cylinders[ncylinders+i] = error[n+i];
+ cy->computeInteractionMatrixError(cMo, _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];
+ m_error_edge[n + i] = cy->error[i];
+ m_errorCylinders[ncylinders + i] = m_error_edge[n + i];
}
}
- n+= cy->nbFeature ;
- ncylinders+= cy->nbFeature ;
+ n += cy->nbFeature;
+ ncylinders += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
- ci->computeInteractionMatrixError(cMo) ;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j];
- error[n+i] = ci->error[i];
- error_circles[ncircles+i] = error[n+i];
+ ci->computeInteractionMatrixError(cMo);
+ 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];
+ m_error_edge[n + i] = ci->error[i];
+ m_errorCircles[ncircles + i] = m_error_edge[n + i];
}
}
- n+= ci->nbFeature ;
- ncircles+= ci->nbFeature ;
- }
- }
-}
-
-void
-vpMbEdgeTracker::computeVVSSecondPhaseCheckLevenbergMarquardt(const unsigned int iter, const unsigned int nbrow,
- const vpColVector &m_error_prev, const vpColVector &m_w_prev, const vpHomogeneousMatrix &cMoPrev,
- double &mu, bool &reStartFromLastIncrement) {
- if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
- if(m_error.sumSquare()/(double)nbrow > m_error_prev.sumSquare()/(double)nbrow){
- mu *= 10.0;
-
- if(mu > 1.0)
- throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
-
- cMo = cMoPrev;
- m_error = m_error_prev;
- m_w = m_w_prev;
- reStartFromLastIncrement = true;
- }
- }
-}
-
-void
-vpMbEdgeTracker::computeVVSSecondPhasePoseEstimation(const unsigned int nerror, vpMatrix &L, vpMatrix &L_true,
- vpMatrix &LVJ_true, vpColVector &W_true, const vpColVector &factor, const unsigned int iter, const bool isoJoIdentity_,
- vpColVector &weighted_error, double &mu, vpColVector &m_error_prev, vpColVector &m_w_prev,
- vpHomogeneousMatrix &cMoPrev, double &residu_1, double &r) {
- double num=0;
- double den=0;
- double wi;
- double eri;
-
- vpMatrix LTL;
- vpColVector LTR;
-
- L_true = L;
- W_true = vpColVector(nerror);
-
- vpVelocityTwistMatrix cVo;
- if(computeCovariance){
- L_true = L;
- if(!isoJoIdentity_){
- cVo.buildFrom(cMo);
- LVJ_true = (L*cVo*oJo);
- }
- }
-
- if((iter==0)|| compute_interaction) {
- for (unsigned int i = 0; i < nerror; i++) {
- wi = m_w[i]*factor[i];
- W_true[i] = wi;
- eri = m_error[i];
- num += wi*vpMath::sqr(eri);
- den += wi;
-
- weighted_error[i] = wi*eri ;
-
- for (unsigned int j = 0; j < 6; j++) {
- L[i][j] = wi*L[i][j];
- }
- }
- } else {
- for(unsigned int i = 0; i < nerror; i++) {
- wi = m_w[i]*factor[i];
- W_true[i] = wi;
- eri = m_error[i];
- num += wi*vpMath::sqr(eri);
- den += wi;
-
- weighted_error[i] = wi*eri ;
+ n += ci->nbFeature;
+ ncircles += ci->nbFeature;
}
}
-
- vpColVector v;
- if(isoJoIdentity_){
- LTL = L.AtA();
- computeJTR(L, weighted_error, LTR);
-
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LTL.getRows(), LTL.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LTL + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
-
- if(iter != 0)
- mu /= 10.0;
-
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- v = -lambda*LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon())*LTR;
- }
- }
- else{
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpColVector LVJTR;
- computeJTR(LVJ, weighted_error, LVJTR);
-
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
-
- if(iter != 0)
- mu /= 10.0;
-
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- {
- v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- break;
- }
- }
- }
-
- residu_1 = r;
- r = sqrt(num/den); //Le critere d'arret prend en compte le poids
-
- cMoPrev = cMo;
- cMo = vpExponentialMap::direct(v).inverse() * cMo;
}
-void
-vpMbEdgeTracker::computeVVSSecondPhaseWeights(const unsigned int iter, const unsigned int nerror,
- const unsigned int nbrow, vpColVector &weighted_error,
- vpRobust &robust_lines, vpRobust &robust_cylinders, vpRobust &robust_circles,
- vpColVector &w_lines, vpColVector &w_cylinders, vpColVector &w_circles,
- vpColVector &error_lines, vpColVector &error_cylinders, vpColVector &error_circles,
- const unsigned int nberrors_lines, const unsigned int nberrors_cylinders, const unsigned int nberrors_circles) {
- if(iter==0)
- {
- weighted_error.resize(nerror);
- m_w.resize(nerror);
- m_w = 1;
- w_lines.resize(nberrors_lines);
- w_lines = 1;
- w_cylinders.resize(nberrors_cylinders);
- w_cylinders = 1;
- w_circles.resize(nberrors_circles);
- w_circles = 1;
-
- robust_lines.setThreshold(2/cam.get_px());
- robust_cylinders.setThreshold(2/cam.get_px());
- robust_circles.setThreshold(vpMath::sqr(2/cam.get_px()));
- if(nberrors_lines > 0)
- robust_lines.MEstimator(vpRobust::TUKEY, error_lines,w_lines);
- if(nberrors_cylinders > 0)
- robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
- if(nberrors_circles > 0)
- robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
- }
- else
- {
- robust_lines.setIteration(iter);
- robust_cylinders.setIteration(iter);
- robust_circles.setIteration(iter);
- if(nberrors_lines > 0)
- robust_lines.MEstimator(vpRobust::TUKEY, error_lines, w_lines);
- if(nberrors_cylinders > 0)
- robust_cylinders.MEstimator(vpRobust::TUKEY, error_cylinders,w_cylinders);
- if(nberrors_circles > 0)
- robust_circles.MEstimator(vpRobust::TUKEY, error_circles,w_circles);
- }
-
- unsigned int cpt = 0;
- while(cpt<nbrow){
- if(cpt<nberrors_lines){
- m_w[cpt] = w_lines[cpt];
- }
- else if (cpt<nberrors_lines+nberrors_cylinders){
- m_w[cpt] = w_cylinders[cpt-nberrors_lines];
- }
- else {
- m_w[cpt] = w_circles[cpt-nberrors_lines-nberrors_cylinders];
- }
- cpt++;
- }
+void vpMbEdgeTracker::computeVVSWeights()
+{
+ 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);
+ if (nberrors_circles > 0)
+ m_robustCircles.MEstimator(vpRobust::TUKEY, m_errorCircles, m_wCircles);
+
+ m_w_edge.insert(0, m_wLines);
+ m_w_edge.insert(m_wLines.getRows(), m_wCylinders);
+ m_w_edge.insert(m_wLines.getRows() + m_wCylinders.getRows(), m_wCircles);
}
/*!
Compute the projection error of the model.
- This approach compares the gradient direction around samples of each lines of the model with their direction.
- Error is expressed in degrees between 0 and 90.
+ This approach compares the gradient direction around samples of each lines
+ of the model with their direction. Error is expressed in degrees between 0
+ and 90.
\param _I : Image in which the model appears.
*/
-void
-vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
+void vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char> &_I)
{
projectionError = 0.0;
unsigned int nbFeatures = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceLine *>::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){
+ 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);
@@ -1043,12 +885,11 @@ vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
vpMbtDistanceCylinder *cy = *it;
- if (cy->isVisible() && cy->isTracked())
- {
- if(cy->meline1 != NULL)
- {
+ if (cy->isVisible() && cy->isTracked()) {
+ if (cy->meline1 != NULL) {
double cylinderNormGradient = 0;
unsigned int cylinderNbFeatures = 0;
cy->meline1->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
@@ -1056,8 +897,7 @@ vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
nbFeatures += cylinderNbFeatures;
}
- if(cy->meline2 != NULL)
- {
+ if (cy->meline2 != NULL) {
double cylinderNormGradient = 0;
unsigned int cylinderNbFeatures = 0;
cy->meline2->computeProjectionError(_I, cylinderNormGradient, cylinderNbFeatures);
@@ -1067,10 +907,10 @@ vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
vpMbtDistanceCircle *c = *it;
- if (c->isVisible() && c->isTracked() && c->meEllipse != NULL)
- {
+ if (c->isVisible() && c->isTracked() && c->meEllipse != NULL) {
double circleNormGradient = 0;
unsigned int circleNbFeatures = 0;
c->meEllipse->computeProjectionError(_I, circleNormGradient, circleNbFeatures);
@@ -1079,291 +919,268 @@ vpMbEdgeTracker::computeProjectionError(const vpImage<unsigned char>& _I)
}
}
- if(nbFeatures > 0) {
- projectionError = vpMath::deg(projectionError/(double)nbFeatures);
- }
- else {
+ if (nbFeatures > 0) {
+ projectionError = vpMath::deg(projectionError / (double)nbFeatures);
+ } else {
projectionError = 90.0;
}
nbFeaturesForProjErrorComputation = nbFeatures;
-// std::cout << "Norm Gradient = " << errorGradient << std::endl;
+ // std::cout << "Norm Gradient = " << errorGradient << std::endl;
}
/*!
Check if the tracking failed.
-
- \throw vpTrackingException::fatalError if the test fails.
+
+ \throw vpTrackingException::fatalError if the test fails.
*/
-void
-vpMbEdgeTracker::testTracking()
+void vpMbEdgeTracker::testTracking()
{
int nbExpectedPoint = 0;
int nbGoodPoint = 0;
int nbBadPoint = 0;
-
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+
+ for (std::list<vpMbtDistanceLine *>::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){
+ if (l->isVisible() && l->isTracked()) {
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
+ if (l->meline[a] != NULL) {
nbExpectedPoint += (int)l->meline[a]->expecteddensity;
- for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
+ for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
+ itme != l->meline[a]->getMeList().end(); ++itme) {
vpMeSite pix = *itme;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoint++;
+ else
+ nbBadPoint++;
}
}
}
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::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<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
+ for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
+ itme1 != cy->meline1->getMeList().end(); ++itme1) {
vpMeSite pix = *itme1;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoint++;
+ else
+ nbBadPoint++;
}
nbExpectedPoint += (int)cy->meline2->expecteddensity;
- for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
+ for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
+ itme2 != cy->meline2->getMeList().end(); ++itme2) {
vpMeSite pix = *itme2;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoint++;
+ else
+ nbBadPoint++;
}
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
vpMbtDistanceCircle *ci = *it;
- if (ci->isVisible() && ci->isTracked() && ci->meEllipse !=NULL)
- {
+ if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
nbExpectedPoint += ci->meEllipse->getExpectedDensity();
- for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
+ for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
+ itme != ci->meEllipse->getMeList().end(); ++itme) {
vpMeSite pix = *itme;
- if (pix.getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoint++;
- else nbBadPoint++;
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoint++;
+ else
+ nbBadPoint++;
}
}
}
- // Compare the number of good points with the min between the number of expected points and
- // number of points that are tracked
- int nb_min = (int)vpMath::minimum(percentageGdPt *nbExpectedPoint, percentageGdPt *(nbGoodPoint + nbBadPoint) );
- //int nb_min = std::min(val1, val2);
+ // Compare the number of good points with the min between the number of
+ // expected points and number of points that are tracked
+ int nb_min = (int)vpMath::minimum(percentageGdPt * nbExpectedPoint, percentageGdPt * (nbGoodPoint + nbBadPoint));
+ // int nb_min = (std::min)(val1, val2);
if (nbGoodPoint < nb_min || nbExpectedPoint < 2) {
std::ostringstream oss;
- oss << "Not enough moving edges ("
- << nbGoodPoint
- << ") to track the object: expected "
- << nb_min
- << ". Try to reduce the threshold="
- << percentageGdPt
+ oss << "Not enough moving edges (" << nbGoodPoint << ") to track the object: expected " << nb_min
+ << ". Try to reduce the threshold=" << percentageGdPt
<< " using vpMbTracker::setGoodMovingEdgesRatioThreshold()";
throw vpTrackingException(vpTrackingException::fatalError, oss.str());
- }
+ }
}
/*!
Compute each state of the tracking procedure for all the feature sets.
-
+
If the tracking is considered as failed an exception is thrown.
-
+
\param I : The image.
*/
-void
-vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
-{
+void vpMbEdgeTracker::track(const vpImage<unsigned char> &I)
+{
initPyramid(I, Ipyramid);
-
-// for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
+
+ // for (int lvl = ((int)scales.size()-1); lvl >= 0; lvl -= 1)
unsigned int lvl = (unsigned int)scales.size();
- do{
+ do {
lvl--;
projectionError = 90.0;
- if(scales[lvl]){
+ if (scales[lvl]) {
vpHomogeneousMatrix cMo_1 = cMo;
- try
- {
+ try {
downScale(lvl);
- try
- {
+ try {
trackMovingEdge(*Ipyramid[lvl]);
- }
- catch(...)
- {
- vpTRACE("Error in moving edge tracking") ;
- throw ;
+ } catch (...) {
+ vpTRACE("Error in moving edge tracking");
+ throw;
}
- // initialize the vector that contains the error and the matrix that contains
- // the interaction matrix
- // AY: Useless as it is done in coputeVVS()
+ // initialize the vector that contains the error and the matrix that
+ // contains the interaction matrix AY: Useless as it is done in
+ // coputeVVS()
/*
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- if (l->isVisible()){
- l->initInteractionMatrixError() ;
+ for(std::list<vpMbtDistanceLine*>::const_iterator
+ it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){ l = *it; if
+ (l->isVisible()){ l->initInteractionMatrixError();
}
- }
+ }
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
+ for(std::list<vpMbtDistanceCylinder*>::const_iterator
+ it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){ cy = *it;
if(cy->isVisible()) {
cy->initInteractionMatrixError();
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- if (ci->isVisible()){
- ci->initInteractionMatrixError();
+ for(std::list<vpMbtDistanceCircle*>::const_iterator
+ it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){ ci = *it; if
+ (ci->isVisible()){ ci->initInteractionMatrixError();
}
}
*/
- try
- {
+ try {
computeVVS(*Ipyramid[lvl], lvl);
- }
- catch(...)
- {
+ } catch (...) {
covarianceMatrix = -1;
throw; // throw the original exception
}
- try
- {
- testTracking();
- }
- catch(...)
- {
- throw; // throw the original exception
- }
+ testTracking();
- if (displayFeatures)
- {
+ if (displayFeatures) {
displayFeaturesOnImage(I, lvl);
}
// Looking for new visible face
- bool newvisibleface = false ;
- visibleFace(I, cMo, newvisibleface) ;
+ bool newvisibleface = false;
+ visibleFace(I, cMo, newvisibleface);
- //cam.computeFov(I.getWidth(), I.getHeight());
- if(useScanLine){
- faces.computeClippedPolygons(cMo,cam);
+ // cam.computeFov(I.getWidth(), I.getHeight());
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
- try
- {
- updateMovingEdge(I);
- }
- catch(...)
- {
- throw; // throw the original exception
- }
+ updateMovingEdge(I);
- initMovingEdge(I,cMo) ;
+ initMovingEdge(I, cMo);
// Reinit the moving edge for the lines which need it.
- reinitMovingEdge(I,cMo);
+ reinitMovingEdge(I, cMo);
- if(computeProjError)
+ if (computeProjError)
computeProjectionError(I);
upScale(lvl);
- }
- catch(vpException &e)
- {
- if(lvl != 0){
+ } catch (vpException &e) {
+ if (lvl != 0) {
cMo = cMo_1;
reInitLevel(lvl);
upScale(lvl);
- }
- else{
+ } else {
upScale(lvl);
- throw(e) ;
+ throw(e);
}
}
}
- } while(lvl != 0);
-
+ } while (lvl != 0);
+
cleanPyramid(Ipyramid);
}
/*!
Initialize the tracking.
-
+
\param I : The image.
*/
-void vpMbEdgeTracker::init(const vpImage<unsigned char>& I)
+void vpMbEdgeTracker::init(const vpImage<unsigned char> &I)
{
- if(!modelInitialised){
+ if (!modelInitialised) {
throw vpException(vpException::fatalError, "model not initialized");
}
- bool a = false;
+ bool a = false;
-#ifdef VISP_HAVE_OGRE
- if(useOgre){
- if(!faces.isOgreInitialised()){
+#ifdef VISP_HAVE_OGRE
+ if (useOgre) {
+ if (!faces.isOgreInitialised()) {
faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
faces.setOgreShowConfigDialog(ogreShowConfigDialog);
faces.initOgre(cam);
- // Turn off Ogre config dialog display for the next call to this function
- // since settings are saved in the ogre.cfg file and used during the next
- // call
- ogreShowConfigDialog = false;
+ // Turn off Ogre config dialog display for the next call to this
+ // function since settings are saved in the ogre.cfg file and used
+ // during the next call
+ ogreShowConfigDialog = false;
}
}
#endif
-
- if(clippingFlag > 2)
+
+ if (clippingFlag > 2)
cam.computeFov(I.getWidth(), I.getHeight());
-
+
initPyramid(I, Ipyramid);
visibleFace(I, cMo, a);
unsigned int i = (unsigned int)scales.size();
resetMovingEdge();
- if(useScanLine){
- if(clippingFlag <= 2)
+ if (useScanLine) {
+ if (clippingFlag <= 2)
cam.computeFov(I.getWidth(), I.getHeight());
- faces.computeClippedPolygons(cMo,cam);
+ faces.computeClippedPolygons(cMo, cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
do {
i--;
- if(scales[i]){
+ if (scales[i]) {
downScale(i);
initMovingEdge(*Ipyramid[i], cMo);
upScale(i);
}
- } while(i != 0);
-
+ } while (i != 0);
+
cleanPyramid(Ipyramid);
}
/*!
Set the pose to be used in entry of the next call to the track() function.
This pose will be just used once.
-
+
\param I : image corresponding to the desired pose.
\param cdMo : Pose to affect.
*/
-void
-vpMbEdgeTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+void vpMbEdgeTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
cMo = cdMo;
@@ -1371,31 +1188,33 @@ vpMbEdgeTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMa
}
/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
+ Load the xml configuration file. An example of such a file is provided in
+ loadConfigFile(const char*) documentation. From the configuration file
+ initialize the parameters corresponding to the objects: moving-edges, camera
+ and visibility angles.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile : full name of the xml file.
\sa loadConfigFile(const char*), vpXmlParser::cleanup()
*/
-void
-vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
+void vpMbEdgeTracker::loadConfigFile(const std::string &configFile)
{
vpMbEdgeTracker::loadConfigFile(configFile.c_str());
}
/*!
Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, camera and visibility angles.
+ From the configuration file initialize the parameters corresponding to the
+objects: moving-edges, camera and visibility angles.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -1436,42 +1255,40 @@ vpMbEdgeTracker::loadConfigFile(const std::string& configFile)
\sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
*/
-void
-vpMbEdgeTracker::loadConfigFile(const char* configFile)
+void vpMbEdgeTracker::loadConfigFile(const char *configFile)
{
#ifdef VISP_HAVE_XML2
vpMbtXmlParser xmlp;
-
+
xmlp.setCameraParameters(cam);
xmlp.setAngleAppear(vpMath::deg(angleAppears));
xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
xmlp.setMovingEdge(me);
- try{
+ try {
std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
xmlp.parse(configFile);
- }
- catch(...){
+ } catch (...) {
throw vpException(vpException::ioError, "Cannot open XML file \"%s\"", configFile);
}
-
+
vpCameraParameters camera;
vpMe meParser;
xmlp.getCameraParameters(camera);
xmlp.getMe(meParser);
-
+
setCameraParameters(camera);
setMovingEdge(meParser);
angleAppears = vpMath::rad(xmlp.getAngleAppear());
angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
-
- if(xmlp.hasNearClippingDistance())
+
+ if (xmlp.hasNearClippingDistance())
setNearClippingDistance(xmlp.getNearClippingDistance());
-
- if(xmlp.hasFarClippingDistance())
+
+ if (xmlp.hasFarClippingDistance())
setFarClippingDistance(xmlp.getFarClippingDistance());
-
- if(xmlp.getFovClipping())
+
+ if (xmlp.getFovClipping())
setClipping(clippingFlag | vpPolygon3D::FOV_CLIPPING);
useLodGeneral = xmlp.getLodState();
@@ -1479,7 +1296,7 @@ vpMbEdgeTracker::loadConfigFile(const char* configFile)
minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
applyLodSettingInConfig = false;
- if(this->getNbPolygon() > 0) {
+ if (this->getNbPolygon() > 0) {
applyLodSettingInConfig = true;
setLod(useLodGeneral);
setMinLineLengthThresh(minLineLengthThresholdGeneral);
@@ -1491,7 +1308,6 @@ vpMbEdgeTracker::loadConfigFile(const char* configFile)
#endif
}
-
/*!
Display the 3D model from a given position of the camera.
@@ -1500,33 +1316,36 @@ vpMbEdgeTracker::loadConfigFile(const char* configFile)
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void
-vpMbEdgeTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &camera, const vpColor& col,
- const unsigned int thickness, const bool displayFullModel)
+void vpMbEdgeTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- break ; //displaying model on one scale only
+ break; // displaying model on one scale only
}
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
@@ -1539,284 +1358,289 @@ vpMbEdgeTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMat
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible surfaces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible surfaces).
*/
-void
-vpMbEdgeTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &camera, const vpColor& col,
- const unsigned int thickness, const bool displayFullModel)
+void vpMbEdgeTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- (*it)->display(I, cMo_, camera, col, thickness, displayFullModel) ;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- break ; //displaying model on one scale only
+ break; // displaying model on one scale only
}
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
-void
-vpMbEdgeTracker::displayFeaturesOnImage(const vpImage<unsigned char>& I, const unsigned int lvl)
+void vpMbEdgeTracker::displayFeaturesOnImage(const vpImage<unsigned char> &I, const unsigned int lvl)
{
- if(lvl == 0){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
+ if (lvl == 0) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
vpMbtDistanceLine *l = *it;
- if (l->isVisible() && l->isTracked()){
+ if (l->isVisible() && l->isTracked()) {
l->displayMovingEdges(I);
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
vpMbtDistanceCylinder *cy = *it;
- if(cy->isVisible() && cy->isTracked()) {
+ if (cy->isVisible() && cy->isTracked()) {
cy->displayMovingEdges(I);
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
vpMbtDistanceCircle *ci = *it;
- if (ci->isVisible() && ci->isTracked()){
+ if (ci->isVisible() && ci->isTracked()) {
ci->displayMovingEdges(I);
}
}
}
}
-
/*!
Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the lines.
-
+ The 3D model is projected into the image to create moving edges along the
+ lines.
+
\param I : The image.
\param _cMo : The pose of the camera used to initialize the moving edges.
*/
-void
-vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
-{
- vpMbtDistanceLine *l ;
+void vpMbEdgeTracker::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
+{
+ vpMbtDistanceLine *l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
l = *it;
- bool isvisible = false ;
+ bool isvisible = false;
- for(std::list<int>::const_iterator itindex=l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
+ ++itindex) {
int index = *itindex;
- if (index ==-1) isvisible =true ;
- else
- {
- if (l->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
+ if (index == -1)
+ isvisible = true;
+ else {
+ if (l->hiddenface->isVisible((unsigned int)index))
+ isvisible = true;
}
}
- //Si la ligne n'appartient a aucune face elle est tout le temps visible
- if (l->Lindex_polygon.empty()) isvisible = true; // Not sure that this can occur
+ // Si la ligne n'appartient a aucune face elle est tout le temps visible
+ if (l->Lindex_polygon.empty())
+ isvisible = true; // Not sure that this can occur
- if (isvisible)
- {
- l->setVisible(true) ;
+ if (isvisible) {
+ l->setVisible(true);
l->updateTracked();
if (l->meline.size() == 0 && l->isTracked())
- l->initMovingEdge(I, _cMo) ;
- }
- else
- {
- l->setVisible(false) ;
- for(unsigned int a = 0 ; a < l->meline.size() ; a++){
- if (l->meline[a] != NULL) delete l->meline[a] ;
- l->nbFeature[a] = 0;
+ l->initMovingEdge(I, _cMo);
+ } else {
+ l->setVisible(false);
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
+ if (l->meline[a] != NULL)
+ delete l->meline[a];
+ if (a < l->nbFeature.size())
+ l->nbFeature[a] = 0;
}
l->nbFeatureTotal = 0;
l->meline.clear();
+ l->nbFeature.clear();
}
}
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ vpMbtDistanceCylinder *cy;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
cy = *it;
- bool isvisible = false ;
+ bool isvisible = false;
int index = cy->index_polygon;
- if (index ==-1) isvisible =true ;
- else
- {
- if (cy->hiddenface->isVisible((unsigned int)index +1 ) ||
- cy->hiddenface->isVisible((unsigned int)index +2 ) ||
- cy->hiddenface->isVisible((unsigned int)index +3 ) ||
- cy->hiddenface->isVisible((unsigned int)index +4 ))
- isvisible = true ;
- }
-// vpTRACE("cyl with index %d is visible: %d", index, isvisible);
-
- if (isvisible)
- {
- cy->setVisible(true) ;
- if (cy->meline1==NULL || cy->meline2==NULL){
- if(cy->isTracked())
- cy->initMovingEdge(I, _cMo) ;
- }
- }
- else
- {
- cy->setVisible(false) ;
- if (cy->meline1!=NULL) delete cy->meline1;
- if (cy->meline2!=NULL) delete cy->meline2;
- cy->meline1=NULL;
- cy->meline2=NULL;
+ if (index == -1)
+ isvisible = true;
+ else {
+ if (cy->hiddenface->isVisible((unsigned int)index + 1) || cy->hiddenface->isVisible((unsigned int)index + 2) ||
+ cy->hiddenface->isVisible((unsigned int)index + 3) || cy->hiddenface->isVisible((unsigned int)index + 4))
+ isvisible = true;
+ }
+ // vpTRACE("cyl with index %d is visible: %d", index, isvisible);
+
+ if (isvisible) {
+ cy->setVisible(true);
+ if (cy->meline1 == NULL || cy->meline2 == NULL) {
+ if (cy->isTracked())
+ cy->initMovingEdge(I, _cMo);
+ }
+ } else {
+ cy->setVisible(false);
+ if (cy->meline1 != NULL)
+ delete cy->meline1;
+ if (cy->meline2 != NULL)
+ delete cy->meline2;
+ cy->meline1 = NULL;
+ cy->meline2 = NULL;
cy->nbFeature = 0;
cy->nbFeaturel1 = 0;
- cy->nbFeaturel2= 0;
+ cy->nbFeaturel2 = 0;
}
}
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ vpMbtDistanceCircle *ci;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
ci = *it;
- bool isvisible = false ;
+ bool isvisible = false;
int index = ci->index_polygon;
- if (index ==-1) isvisible =true ;
- else
- {
- if (ci->hiddenface->isVisible((unsigned int)index)) isvisible = true ;
- }
-
- if (isvisible)
- {
- ci->setVisible(true) ;
- if (ci->meEllipse==NULL)
- {
- if(ci->isTracked())
- ci->initMovingEdge(I, _cMo) ;
- }
- }
- else
- {
- ci->setVisible(false) ;
- if (ci->meEllipse!=NULL) delete ci->meEllipse;
- ci->meEllipse=NULL;
+ if (index == -1)
+ isvisible = true;
+ else {
+ if (ci->hiddenface->isVisible((unsigned int)index))
+ isvisible = true;
+ }
+
+ if (isvisible) {
+ ci->setVisible(true);
+ if (ci->meEllipse == NULL) {
+ if (ci->isTracked())
+ ci->initMovingEdge(I, _cMo);
+ }
+ } else {
+ ci->setVisible(false);
+ if (ci->meEllipse != NULL)
+ delete ci->meEllipse;
+ ci->meEllipse = NULL;
ci->nbFeature = 0;
}
}
}
-
/*!
Track the moving edges in the image.
-
+
\param I : the image.
*/
-void
-vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
+void vpMbEdgeTracker::trackMovingEdge(const vpImage<unsigned char> &I)
{
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
vpMbtDistanceLine *l = *it;
- if(l->isVisible() && l->isTracked()){
- if(l->meline.size() == 0){
+ if (l->isVisible() && l->isTracked()) {
+ if (l->meline.size() == 0) {
l->initMovingEdge(I, cMo);
}
- l->trackMovingEdge(I, cMo) ;
+ l->trackMovingEdge(I, cMo);
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
vpMbtDistanceCylinder *cy = *it;
- if(cy->isVisible() && cy->isTracked()) {
- if(cy->meline1 == NULL || cy->meline2 == NULL){
- cy->initMovingEdge(I, cMo);
- }
- cy->trackMovingEdge(I, cMo) ;
+ if (cy->isVisible() && cy->isTracked()) {
+ if (cy->meline1 == NULL || cy->meline2 == NULL) {
+ cy->initMovingEdge(I, cMo);
+ }
+ cy->trackMovingEdge(I, cMo);
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
vpMbtDistanceCircle *ci = *it;
- if(ci->isVisible() && ci->isTracked()){
- if(ci->meEllipse == NULL){
+ if (ci->isVisible() && ci->isTracked()) {
+ if (ci->meEllipse == NULL) {
ci->initMovingEdge(I, cMo);
}
- ci->trackMovingEdge(I, cMo) ;
+ ci->trackMovingEdge(I, cMo);
}
}
}
-
/*!
Update the moving edges at the end of the virtual visual servoing.
-
+
\param I : the image.
*/
-void
-vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
+void vpMbEdgeTracker::updateMovingEdge(const vpImage<unsigned char> &I)
{
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceLine *l;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ if ((*it)->isTracked()) {
l = *it;
- l->updateMovingEdge(I, cMo) ;
- if (l->nbFeatureTotal == 0 && l->isVisible()){
+ l->updateMovingEdge(I, cMo);
+ if (l->nbFeatureTotal == 0 && l->isVisible()) {
l->Reinit = true;
}
}
}
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceCylinder *cy;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
- cy->updateMovingEdge(I, cMo) ;
- if((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()){
+ cy->updateMovingEdge(I, cMo);
+ if ((cy->nbFeaturel1 == 0 || cy->nbFeaturel2 == 0) && cy->isVisible()) {
cy->Reinit = true;
}
}
}
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceCircle *ci;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
- ci->updateMovingEdge(I, cMo) ;
- if(ci->nbFeature == 0 && ci->isVisible()){
+ ci->updateMovingEdge(I, cMo);
+ if (ci->nbFeature == 0 && ci->isVisible()) {
ci->Reinit = true;
}
}
}
}
-void
-vpMbEdgeTracker::updateMovingEdgeWeights() {
+void vpMbEdgeTracker::updateMovingEdgeWeights()
+{
unsigned int n = 0;
vpMbtDistanceLine *l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ if ((*it)->isTracked()) {
l = *it;
unsigned int indexLine = 0;
- double wmean = 0 ;
- for(unsigned int a = 0 ; a < l->meline.size() ; a++)
- {
+ double wmean = 0;
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
if (l->nbFeature[a] > 0) {
std::list<vpMeSite>::iterator itListLine;
itListLine = l->meline[a]->getMeList().begin();
- for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
- wmean += m_w[n+indexLine] ;
+ for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
+ wmean += m_w_edge[n + indexLine];
vpMeSite p = *itListLine;
- if (m_w[n+indexLine] < 0.5){
+ if (m_w_edge[n + indexLine] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListLine = p;
@@ -1827,10 +1651,10 @@ vpMbEdgeTracker::updateMovingEdgeWeights() {
}
}
}
- n+= l->nbFeatureTotal ;
+ n += l->nbFeatureTotal;
- if (l->nbFeatureTotal!=0)
- wmean /= l->nbFeatureTotal ;
+ if (l->nbFeatureTotal != 0)
+ wmean /= l->nbFeatureTotal;
else
wmean = 1;
@@ -1843,21 +1667,22 @@ vpMbEdgeTracker::updateMovingEdgeWeights() {
// Same thing with cylinders as with lines
vpMbtDistanceCylinder *cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
- double wmean = 0 ;
+ double wmean = 0;
std::list<vpMeSite>::iterator itListCyl1;
std::list<vpMeSite>::iterator itListCyl2;
- if (cy->nbFeature > 0){
+ if (cy->nbFeature > 0) {
itListCyl1 = cy->meline1->getMeList().begin();
itListCyl2 = cy->meline2->getMeList().begin();
- for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
- wmean += m_w[n+i] ;
+ for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
+ wmean += m_w_edge[n + i];
vpMeSite p = *itListCyl1;
- if (m_w[n+i] < 0.5){
+ if (m_w_edge[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCyl1 = p;
@@ -1867,22 +1692,22 @@ vpMbEdgeTracker::updateMovingEdgeWeights() {
}
}
- if (cy->nbFeaturel1!=0)
- wmean /= cy->nbFeaturel1 ;
+ if (cy->nbFeaturel1 != 0)
+ wmean /= cy->nbFeaturel1;
else
wmean = 1;
cy->setMeanWeight1(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
cy->Reinit = true;
}
wmean = 0;
- for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
- wmean += m_w[n+i] ;
+ for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
+ wmean += m_w_edge[n + i];
vpMeSite p = *itListCyl2;
- if (m_w[n+i] < 0.5){
+ if (m_w_edge[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCyl2 = p;
@@ -1891,38 +1716,39 @@ vpMbEdgeTracker::updateMovingEdgeWeights() {
++itListCyl2;
}
- if (cy->nbFeaturel2!=0)
- wmean /= cy->nbFeaturel2 ;
+ if (cy->nbFeaturel2 != 0)
+ wmean /= cy->nbFeaturel2;
else
wmean = 1;
cy->setMeanWeight2(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
cy->Reinit = true;
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
// Same thing with circles as with lines
vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
- double wmean = 0 ;
+ double wmean = 0;
std::list<vpMeSite>::iterator itListCir;
- if (ci->nbFeature > 0){
+ if (ci->nbFeature > 0) {
itListCir = ci->meEllipse->getMeList().begin();
}
wmean = 0;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- wmean += m_w[n+i] ;
+ for (unsigned int i = 0; i < ci->nbFeature; i++) {
+ wmean += m_w_edge[n + i];
vpMeSite p = *itListCir;
- if (m_w[n+i] < 0.5){
+ if (m_w_edge[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCir = p;
@@ -1931,55 +1757,57 @@ vpMbEdgeTracker::updateMovingEdgeWeights() {
++itListCir;
}
- if (ci->nbFeature!=0)
- wmean /= ci->nbFeature ;
+ if (ci->nbFeature != 0)
+ wmean /= ci->nbFeature;
else
wmean = 1;
ci->setMeanWeight(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
ci->Reinit = true;
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
}
-
/*!
Reinitialize the lines if it is required.
-
- A line is reinitialized if the 2D line do not match enough with the projected 3D line.
-
+
+ A line is reinitialized if the 2D line do not match enough with the
+ projected 3D line.
+
\param I : the image.
\param _cMo : the pose of the used to re-initialize the moving edges
*/
-void
-vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
+void vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &_cMo)
{
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceLine *l;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ if ((*it)->isTracked()) {
l = *it;
if (l->Reinit && l->isVisible())
l->reinitMovingEdge(I, _cMo);
}
}
- vpMbtDistanceCylinder*cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceCylinder *cy;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
if (cy->Reinit && cy->isVisible())
cy->reinitMovingEdge(I, _cMo);
}
}
- vpMbtDistanceCircle*ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceCircle *ci;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
if (ci->Reinit && ci->isVisible())
ci->reinitMovingEdge(I, _cMo);
@@ -1987,13 +1815,13 @@ vpMbEdgeTracker::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomog
}
}
-void
-vpMbEdgeTracker::resetMovingEdge(){
- for (unsigned int i = 0; i < scales.size(); i += 1){
+void vpMbEdgeTracker::resetMovingEdge()
+{
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
if (scales[i]) {
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
- for(unsigned int a = 0 ; a < (*it)->meline.size() ; a++){
- if((*it)->meline[a] != NULL){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
+ for (unsigned int a = 0; a < (*it)->meline.size(); a++) {
+ if ((*it)->meline[a] != NULL) {
delete (*it)->meline[a];
(*it)->meline[a] = NULL;
}
@@ -2003,12 +1831,13 @@ vpMbEdgeTracker::resetMovingEdge(){
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- if((*it)->meline1 != NULL){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
+ if ((*it)->meline1 != NULL) {
delete (*it)->meline1;
(*it)->meline1 = NULL;
}
- if((*it)->meline2 != NULL){
+ if ((*it)->meline2 != NULL) {
delete (*it)->meline2;
(*it)->meline2 = NULL;
}
@@ -2018,8 +1847,8 @@ vpMbEdgeTracker::resetMovingEdge(){
(*it)->nbFeaturel2 = 0;
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- if((*it)->meEllipse != NULL){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
+ if ((*it)->meEllipse != NULL) {
delete (*it)->meEllipse;
(*it)->meEllipse = NULL;
}
@@ -2031,79 +1860,80 @@ vpMbEdgeTracker::resetMovingEdge(){
/*!
Check if two vpPoints are similar.
-
- To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon \f$.
-
+
+ To be similar : \f$ (X_1 - X_2)^2 + (Y_1 - Y_2)^2 + (Z_1 - Z_2)^2 < epsilon
+ \f$.
+
\param P1 : The first point to compare
\param P2 : The second point to compare
*/
-bool
-vpMbEdgeTracker::samePoint(const vpPoint &P1, const vpPoint &P2) const
+bool vpMbEdgeTracker::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<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() && dz <= std::numeric_limits<double>::epsilon())
- return true ;
+ if (dx <= std::numeric_limits<double>::epsilon() && dy <= std::numeric_limits<double>::epsilon() &&
+ dz <= std::numeric_limits<double>::epsilon())
+ return true;
else
- return false ;
+ return false;
}
-
/*!
- Add a line belonging to the \f$ index \f$ the polygon to the list of lines. It is defined by its two extremities.
-
- If the line already exists, the ploygone's index is added to the list of polygon to which it belongs.
-
+ Add a line belonging to the \f$ index \f$ the polygon to the list of lines.
+ It is defined by its two extremities.
+
+ If the line already exists, the ploygone's index is added to the list of
+ polygon to which it belongs.
+
\param P1 : The first extremity of the line.
\param P2 : The second extremity of the line.
\param polygon : The index of the polygon to which the line belongs.
- \param name : the optional name of the line
+ \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 ;
- vpMbtDistanceLine *l ;
-
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
+ // suppress line already in the model
+ bool already_here = false;
+ vpMbtDistanceLine *l;
+
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
downScale(i);
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if((samePoint(*(l->p1),P1) && samePoint(*(l->p2),P2)) ||
- (samePoint(*(l->p1),P2) && samePoint(*(l->p2),P1)) ){
- already_here = true ;
+ 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 ;
+ l->hiddenface = &faces;
}
}
- if (!already_here){
- l = new vpMbtDistanceLine ;
+ if (!already_here) {
+ l = new vpMbtDistanceLine;
- l->setCameraParameters(cam) ;
- l->buildFrom(P1,P2) ;
+ l->setCameraParameters(cam);
+ l->buildFrom(P1, P2);
l->addPolygon(polygon);
- l->setMovingEdge(&me) ;
- l->hiddenface = &faces ;
+ l->setMovingEdge(&me);
+ l->hiddenface = &faces;
l->useScanLine = useScanLine;
- l->setIndex(nline) ;
+ l->setIndex(nline);
l->setName(name);
-
- if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+
+ 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 ;
+
+ nline += 1;
lines[i].push_back(l);
}
upScale(i);
@@ -2112,20 +1942,19 @@ vpMbEdgeTracker::addLine(vpPoint &P1, vpPoint &P2, int polygon, std::string name
}
/*!
- Remove a line using its name.
-
- \param name : The name of the line to remove.
+ Remove a line using its name.
+
+ \param name : The name of the line to remove.
*/
-void
-vpMbEdgeTracker::removeLine(const std::string& name)
+void vpMbEdgeTracker::removeLine(const std::string &name)
{
vpMbtDistanceLine *l;
-
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+
+ for (unsigned int i = 0; i < scales.size(); i++) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if (name.compare(l->getName()) == 0){
+ if (name.compare(l->getName()) == 0) {
lines[i].erase(it);
break;
}
@@ -2138,31 +1967,32 @@ vpMbEdgeTracker::removeLine(const std::string& name)
Add a circle to the list of circles.
\param P1 : Center of the circle.
- \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
- \param idFace : Id of the face that is associated to the circle to handle visibility test.
- \param name : the optional name of the circle.
+ \param P2,P3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param r : Radius of the circle. \param idFace : Id of the face that
+ is associated to the circle to handle visibility test. \param name : the
+ optional name of the circle.
*/
-void
-vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace, const std::string& name)
+void vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, int idFace,
+ const std::string &name)
{
- bool already_here = false ;
- vpMbtDistanceCircle *ci ;
+ bool already_here = false;
+ vpMbtDistanceCircle *ci;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
downScale(i);
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
- (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
- already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
+ if ((samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P2) && samePoint(*(ci->p3), P3)) ||
+ (samePoint(*(ci->p1), P1) && samePoint(*(ci->p2), P3) && samePoint(*(ci->p3), P2))) {
+ already_here =
+ (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
}
}
- if (!already_here){
- ci = new vpMbtDistanceCircle ;
+ if (!already_here) {
+ ci = new vpMbtDistanceCircle;
ci->setCameraParameters(cam);
ci->buildFrom(P1, P2, P3, r);
@@ -2170,18 +2000,20 @@ vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &
ci->setIndex(ncircle);
ci->setName(name);
ci->index_polygon = idFace;
- ci->hiddenface = &faces ;
+ ci->hiddenface = &faces;
-// if(clippingFlag != vpPolygon3D::NO_CLIPPING)
-// ci->getPolygon().setClipping(clippingFlag);
+ // if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ // ci->getPolygon().setClipping(clippingFlag);
-// if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
-// ci->getPolygon().setNearClippingDistance(distNearClip);
+ // if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) ==
+ // vpPolygon3D::NEAR_CLIPPING)
+ // ci->getPolygon().setNearClippingDistance(distNearClip);
-// if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
-// ci->getPolygon().setFarClippingDistance(distFarClip);
+ // if((clippingFlag & vpPolygon3D::FAR_CLIPPING) ==
+ // vpPolygon3D::FAR_CLIPPING)
+ // ci->getPolygon().setFarClippingDistance(distFarClip);
- ncircle +=1;
+ ncircle += 1;
circles[i].push_back(ci);
}
upScale(i);
@@ -2198,25 +2030,27 @@ vpMbEdgeTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &
\param idFace : The index of the face.
\param name : the optional name of the cylinder
*/
-void
-vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace, const std::string& name)
+void vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double r, int idFace,
+ const std::string &name)
{
- bool already_here = false ;
- vpMbtDistanceCylinder *cy ;
+ bool already_here = false;
+ vpMbtDistanceCylinder *cy;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
downScale(i);
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
cy = *it;
- if((samePoint(*(cy->p1),P1) && samePoint(*(cy->p2),P2)) ||
- (samePoint(*(cy->p1),P2) && samePoint(*(cy->p2),P1)) ){
- already_here = (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
+ if ((samePoint(*(cy->p1), P1) && samePoint(*(cy->p2), P2)) ||
+ (samePoint(*(cy->p1), P2) && samePoint(*(cy->p2), P1))) {
+ already_here =
+ (std::fabs(cy->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(cy->radius, r));
}
}
- if (!already_here){
- cy = new vpMbtDistanceCylinder ;
+ if (!already_here) {
+ cy = new vpMbtDistanceCylinder;
cy->setCameraParameters(cam);
cy->buildFrom(P1, P2, r);
@@ -2224,8 +2058,8 @@ vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double
cy->setIndex(ncylinder);
cy->setName(name);
cy->index_polygon = idFace;
- cy->hiddenface = &faces ;
- ncylinder +=1;
+ cy->hiddenface = &faces;
+ ncylinder += 1;
cylinders[i].push_back(cy);
}
upScale(i);
@@ -2238,16 +2072,15 @@ vpMbEdgeTracker::addCylinder(const vpPoint &P1, const vpPoint &P2, const double
\param name : The name of the cylinder to remove.
*/
-void
-vpMbEdgeTracker::removeCylinder(const std::string& name)
+void vpMbEdgeTracker::removeCylinder(const std::string &name)
{
vpMbtDistanceCylinder *cy;
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceCylinder*>::iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i++) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceCylinder *>::iterator it = cylinders[i].begin(); it != cylinders[i].end(); ++it) {
cy = *it;
- if (name.compare(cy->getName()) == 0){
+ if (name.compare(cy->getName()) == 0) {
cylinders[i].erase(it);
break;
}
@@ -2261,16 +2094,15 @@ vpMbEdgeTracker::removeCylinder(const std::string& name)
\param name : The name of the circle to remove.
*/
-void
-vpMbEdgeTracker::removeCircle(const std::string& name)
+void vpMbEdgeTracker::removeCircle(const std::string &name)
{
vpMbtDistanceCircle *ci;
- for(unsigned int i=0; i<scales.size(); i++){
- if(scales[i]){
- for(std::list<vpMbtDistanceCircle*>::iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i++) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceCircle *>::iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if (name.compare(ci->getName()) == 0){
+ if (name.compare(ci->getName()) == 0) {
circles[i].erase(it);
break;
}
@@ -2281,143 +2113,144 @@ vpMbEdgeTracker::removeCircle(const std::string& name)
/*!
Add a polygon to the list of polygons.
-
+
\param p : The polygon to add.
*/
-void
-vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
+void vpMbEdgeTracker::addPolygon(vpMbtPolygon &p)
{
- unsigned int nbpt = p.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(p.p[i], p.p[i+1], p.getIndex()) ;
- addLine(p.p[nbpt-1], p.p[0], p.getIndex()) ;
+ unsigned int nbpt = p.getNbPoint();
+ if (nbpt > 0) {
+ for (unsigned int i = 0; i < nbpt - 1; i++)
+ addLine(p.p[i], p.p[i + 1], p.getIndex());
+ addLine(p.p[nbpt - 1], p.p[0], p.getIndex());
}
}
/*!
Detect the visible faces in the image and says if a new one appeared.
-
- \warning If in one iteration one face appears and one disappears, then the
- function will not detect the new face.
-
+
+ \warning If in one iteration one face appears and one disappears, then the
+ function will not detect the new face.
+
\param _I : Image to test if a face is entirely in the image.
- \param _cMo : The pose of the camera used to project the 3D model into the image.
- \param newvisibleline : This parameter is set to true if a new face appeared.
+ \param _cMo : The pose of the camera used to project the 3D model into the
+ image. \param newvisibleline : This parameter is set to true if a new face
+ appeared.
*/
-void
-vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> & _I,
- const vpHomogeneousMatrix &_cMo, bool &newvisibleline)
+void vpMbEdgeTracker::visibleFace(const vpImage<unsigned char> &_I, const vpHomogeneousMatrix &_cMo,
+ bool &newvisibleline)
{
- unsigned int n ;
+ unsigned int n;
bool changed = false;
- if(!useOgre) {
- //n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89), changed) ;
- n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
- }
- else{
+ if (!useOgre) {
+ // n = faces.setVisible(_I, cam, _cMo, vpMath::rad(89), vpMath::rad(89),
+ // changed);
+ n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
+ } else {
#ifdef VISP_HAVE_OGRE
n = faces.setVisibleOgre(_I, cam, _cMo, angleAppears, angleDisappears, changed);
#else
- n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed) ;
+ n = faces.setVisible(_I, cam, _cMo, angleAppears, angleDisappears, changed);
#endif
- }
-
- if (n > nbvisiblepolygone)
- {
- //cout << "une nouvelle face est visible " << endl ;
- newvisibleline = true ;
}
- else
- newvisibleline = false ;
- nbvisiblepolygone= n ;
+ if (n > nbvisiblepolygone) {
+ // cout << "une nouvelle face est visible " << endl;
+ newvisibleline = true;
+ } else
+ newvisibleline = false;
+
+ nbvisiblepolygone = n;
}
/*!
Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
+ two points, it defines a single line that is always visible. If it has three
+ or more corners, it defines a face. In that case the visibility of the face
+ is computed in order to track the corresponding lines only if the face is
+ visible.
The id of the polygon is supposed to be set prior calling this function.
- This method is called from the loadModel() one to add a face of the object to track.
+ This method is called from the loadModel() one to add a face of the object
+ to track.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+void vpMbEdgeTracker::initFaceFromCorners(vpMbtPolygon &polygon)
{
- unsigned int nbpt = polygon.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
- addLine(polygon.p[nbpt-1], polygon.p[0], polygon.getIndex(), polygon.getName());
+ 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());
}
}
/*!
Add the lines to track from the polygon description. If the polygon has only
- two points, it defines a single line that is always visible. If it has three or
- more corners, it defines a face. In that case the visibility of the face is computed
- in order to track the corresponding lines only if the face is visible.
+ two points, it defines a single line that is always visible. If it has three
+ or more corners, it defines a face. In that case the visibility of the face
+ is computed in order to track the corresponding lines only if the face is
+ visible.
The id of the polygon is supposed to be set prior calling this function.
- This method is called from the loadModel() one to add a face of the object to track.
+ This method is called from the loadModel() one to add a face of the object
+ to track.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
+void vpMbEdgeTracker::initFaceFromLines(vpMbtPolygon &polygon)
{
- unsigned int nbpt = polygon.getNbPoint() ;
- if(nbpt > 0){
- for (unsigned int i=0 ; i < nbpt-1 ; i++)
- addLine(polygon.p[i], polygon.p[i+1], polygon.getIndex(), polygon.getName());
+ 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());
}
}
-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<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
vpMbtDistanceLine *l = *it;
- if(l->isVisible() && l->isTracked())
- {
- nbrow += l->nbFeatureTotal;
- nberrors_lines+=l->nbFeatureTotal;
+ if (l->isTracked()) {
l->initInteractionMatrixError();
+ nbrow += l->nbFeatureTotal;
+ nberrors_lines += l->nbFeatureTotal;
}
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
vpMbtDistanceCylinder *cy = *it;
- if(cy->isVisible() && cy->isTracked())
- {
- nbrow += cy->nbFeature ;
- nberrors_cylinders += cy->nbFeature ;
- cy->initInteractionMatrixError() ;
+ if (cy->isTracked()) {
+ cy->initInteractionMatrixError();
+ nbrow += cy->nbFeature;
+ nberrors_cylinders += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
vpMbtDistanceCircle *ci = *it;
- if(ci->isVisible() && ci->isTracked())
- {
- nbrow += ci->nbFeature ;
- nberrors_circles += ci->nbFeature ;
- ci->initInteractionMatrixError() ;
+ if (ci->isTracked()) {
+ ci->initInteractionMatrixError();
+ nbrow += ci->nbFeature;
+ nberrors_circles += ci->nbFeature;
}
}
@@ -2425,19 +2258,18 @@ vpMbEdgeTracker::initMbtTracking(unsigned int &nberrors_lines,
}
/*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
+ Add a circle to track from its center, 3 points (including the center)
+ defining the plane that contain the circle and its radius.
\param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param idFace : Index of the face associated to the circle to handle visibility test.
- \param name : The optional name of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param radius : Radius of the circle. \param idFace : Index of the
+ face associated to the circle to handle visibility test. \param name : The
+ optional name of the circle.
*/
-void
-vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace, const std::string &name)
+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);
}
@@ -2449,46 +2281,48 @@ vpMbEdgeTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint
\param p1 : First point on the axis.
\param p2 : Second point on the axis.
\param radius : Radius of the cylinder.
- \param idFace : Id of the face that is associated to the cylinder to handle visibility test.
- \param name : The optional name of the cylinder.
+ \param idFace : Id of the face that is associated to the cylinder to handle
+ visibility test. \param name : The optional name of the cylinder.
*/
-void
-vpMbEdgeTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
- const std::string &name)
+void vpMbEdgeTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string &name)
{
addCylinder(p1, p2, radius, (int)idFace, name);
}
/*!
Reset the tracker. The model is removed and the pose is set to identity.
- The tracker needs to be initialized with a new model and a new pose.
-
+ The tracker needs to be initialized with a new model and a new pose.
+
*/
-void
-vpMbEdgeTracker::resetTracker()
+void vpMbEdgeTracker::resetTracker()
{
this->cMo.eye();
vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
vpMbtDistanceCircle *ci;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if (l!=NULL) delete l ;
- l = NULL ;
+ if (l != NULL)
+ delete l;
+ l = NULL;
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
cy = *it;
- if (cy!=NULL) delete cy;
+ if (cy != NULL)
+ delete cy;
cy = NULL;
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if (ci!=NULL) delete ci;
+ if (ci != NULL)
+ delete ci;
ci = NULL;
}
lines[i].clear();
@@ -2504,14 +2338,14 @@ vpMbEdgeTracker::resetTracker()
#ifdef VISP_HAVE_OGRE
useOgre = false;
#endif
-
- compute_interaction=1;
+
+ m_computeInteraction = true;
nline = 0;
ncylinder = 0;
- lambda = 1;
+ m_lambda = 1.0;
nbvisiblepolygone = 0;
percentageGdPt = 0.4;
-
+
angleAppears = vpMath::rad(89);
angleDisappears = vpMath::rad(89);
clippingFlag = vpPolygon3D::NO_CLIPPING;
@@ -2527,52 +2361,54 @@ vpMbEdgeTracker::resetTracker()
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
{
reInitModel(I, cad_name.c_str(), cMo_, verbose);
}
/*!
- Re-initialize the model used by the tracker.
-
+ Re-initialize the model used by the tracker.
+
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbEdgeTracker::reInitModel(const vpImage<unsigned char> &I, const char *cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
{
this->cMo.eye();
vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
vpMbtDistanceCircle *ci;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if (l!=NULL) delete l ;
- l = NULL ;
+ if (l != NULL)
+ delete l;
+ l = NULL;
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
cy = *it;
- if (cy!=NULL) delete cy;
+ if (cy != NULL)
+ delete cy;
cy = NULL;
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if (ci!=NULL) delete ci;
+ if (ci != NULL)
+ delete ci;
ci = NULL;
}
@@ -2584,11 +2420,11 @@ vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_na
faces.reset();
- //compute_interaction=1;
+ // compute_interaction=1;
nline = 0;
ncylinder = 0;
ncircle = 0;
- //lambda = 1;
+ // lambda = 1;
nbvisiblepolygone = 0;
loadModel(cad_name, verbose);
@@ -2596,58 +2432,64 @@ vpMbEdgeTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_na
}
/*!
- Return the number of good points (vpMeSite) tracked. A good point is a
+ Return the number of good points (vpMeSite) tracked. A good point is a
vpMeSite with its flag "state" equal to 0. Only these points are used
during the virtual visual servoing stage.
-
+
\exception vpException::dimensionError if level does not represent a used
level.
-
- \return the number of good points.
+
+ \return the number of good points.
*/
-unsigned int
-vpMbEdgeTracker::getNbPoints(const unsigned int level) const
+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);
+ if ((level > scales.size()) || !scales[level]) {
+ throw vpException(vpException::dimensionError, "Cannot get the number of points for level %d: level is not used",
+ level);
}
-
+
unsigned int nbGoodPoints = 0;
- vpMbtDistanceLine *l ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[level].begin(); it!=lines[level].end(); ++it){
+ vpMbtDistanceLine *l;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[level].begin(); it != lines[level].end(); ++it) {
l = *it;
- if (l->isVisible() && l->isTracked())
- {
- for(unsigned int a = 0 ; a < l->meline.size() ; a++){
- if(l->nbFeature[a] != 0)
- for(std::list<vpMeSite>::const_iterator itme=l->meline[a]->getMeList().begin(); itme!=l->meline[a]->getMeList().end(); ++itme){
- if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ if (l->isVisible() && l->isTracked()) {
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
+ if (l->nbFeature[a] != 0)
+ for (std::list<vpMeSite>::const_iterator itme = l->meline[a]->getMeList().begin();
+ itme != l->meline[a]->getMeList().end(); ++itme) {
+ if (itme->getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoints++;
}
}
}
}
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[level].begin(); it!=cylinders[level].end(); ++it){
+ vpMbtDistanceCylinder *cy;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[level].begin(); it != cylinders[level].end();
+ ++it) {
cy = *it;
- if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL))
- {
- for(std::list<vpMeSite>::const_iterator itme1=cy->meline1->getMeList().begin(); itme1!=cy->meline1->getMeList().end(); ++itme1){
- if (itme1->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ if (cy->isVisible() && cy->isTracked() && (cy->meline1 != NULL || cy->meline2 != NULL)) {
+ for (std::list<vpMeSite>::const_iterator itme1 = cy->meline1->getMeList().begin();
+ itme1 != cy->meline1->getMeList().end(); ++itme1) {
+ if (itme1->getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoints++;
}
- for(std::list<vpMeSite>::const_iterator itme2=cy->meline2->getMeList().begin(); itme2!=cy->meline2->getMeList().end(); ++itme2){
- if (itme2->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ for (std::list<vpMeSite>::const_iterator itme2 = cy->meline2->getMeList().begin();
+ itme2 != cy->meline2->getMeList().end(); ++itme2) {
+ if (itme2->getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoints++;
}
}
}
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[level].begin(); it!=circles[level].end(); ++it){
+ vpMbtDistanceCircle *ci;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[level].begin(); it != circles[level].end(); ++it) {
ci = *it;
- if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL)
- {
- for(std::list<vpMeSite>::const_iterator itme=ci->meEllipse->getMeList().begin(); itme!=ci->meEllipse->getMeList().end(); ++itme){
- if (itme->getState() == vpMeSite::NO_SUPPRESSION) nbGoodPoints++;
+ if (ci->isVisible() && ci->isTracked() && ci->meEllipse != NULL) {
+ for (std::list<vpMeSite>::const_iterator itme = ci->meEllipse->getMeList().begin();
+ itme != ci->meEllipse->getMeList().end(); ++itme) {
+ if (itme->getState() == vpMeSite::NO_SUPPRESSION)
+ nbGoodPoints++;
}
}
}
@@ -2656,100 +2498,116 @@ vpMbEdgeTracker::getNbPoints(const unsigned int level) const
}
/*!
- Set the scales to use to realize the tracking. The vector of boolean activates
- or not the scales to set for the object tracking. The first element of the list
- correspond to the tracking on the full image, the second element corresponds
- to the tracking on an image subsampled by two.
-
- Using multi scale tracking allows to track the object with greater moves. It
- requires the computation of a pyramid of images, but the total tracking can be
- faster than a tracking based only on the full scale. The pose is computed from
- the smallest image to the biggest. This may be dangerous if the object to
- track is small in the image, because the subsampled scale(s) will have only
- few points to compute the pose (it could result in a loss of precision).
-
+ Set the scales to use to realize the tracking. The vector of boolean
+ activates or not the scales to set for the object tracking. The first
+ element of the list correspond to the tracking on the full image, the second
+ element corresponds to the tracking on an image subsampled by two.
+
+ Using multi scale tracking allows to track the object with greater moves. It
+ requires the computation of a pyramid of images, but the total tracking can
+ be faster than a tracking based only on the full scale. The pose is computed
+ from the smallest image to the biggest. This may be dangerous if the object
+ to track is small in the image, because the subsampled scale(s) will have
+ only few points to compute the pose (it could result in a loss of
+ precision).
+
\warning This method must be used before the tracker has been initialized (
- before the call of the loadConfigFile() or loadModel() methods).
-
- \warning At least one level must be activated.
-
+ before the call of the loadConfigFile() or loadModel() methods).
+
+ \warning At least one level must be activated.
+
\param scale : The vector describing the levels to use.
*/
-void
-vpMbEdgeTracker::setScales(const std::vector<bool>& scale)
+void vpMbEdgeTracker::setScales(const std::vector<bool> &scale)
{
unsigned int nbActivatedLevels = 0;
- for (unsigned int i = 0; i < scale.size(); i += 1){
- if(scale[i]){
+ for (unsigned int i = 0; i < scale.size(); i++) {
+ if (scale[i]) {
nbActivatedLevels++;
}
}
- if((scale.size() < 1) || (nbActivatedLevels == 0)){
- vpERROR_TRACE(" !! WARNING : must use at least one level for the tracking. Use the global one");
+
+ if (scale.empty() || (nbActivatedLevels == 0)) {
+ vpERROR_TRACE(" !! WARNING : must use at least one level for the "
+ "tracking. Use the global one");
this->scales.resize(0);
this->scales.push_back(true);
+
lines.resize(1);
lines[0].clear();
+
cylinders.resize(1);
cylinders[0].clear();
- }
- else{
+
+ circles.resize(1);
+ circles[0].clear();
+ } else {
this->scales = scale;
+
lines.resize(scale.size());
cylinders.resize(scale.size());
- for (unsigned int i = 0; i < lines.size(); i += 1){
+ circles.resize(scale.size());
+
+ for (unsigned int i = 0; i < lines.size(); i++) {
lines[i].clear();
cylinders[i].clear();
+ circles[i].clear();
}
}
}
/*!
Set the far distance for clipping.
-
+
\param dist : Far clipping value.
*/
-void
-vpMbEdgeTracker::setFarClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
- vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
- else{
+void vpMbEdgeTracker::setFarClippingDistance(const double &dist)
+{
+ 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;
+ else if (dist < 0)
+ std::cerr << "Far clipping value cannot be inferior than 0. Far clipping "
+ "won't be considered."
+ << std::endl;
+ else {
vpMbTracker::setFarClippingDistance(dist);
vpMbtDistanceLine *l;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
l->getPolygon().setFarClippingDistance(distFarClip);
}
}
- }
+ }
}
}
/*!
Set the near distance for clipping.
-
+
\param dist : Near clipping value.
*/
-void
-vpMbEdgeTracker::setNearClippingDistance(const double &dist)
-{
- if( (clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
- vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
- else{
+void vpMbEdgeTracker::setNearClippingDistance(const double &dist)
+{
+ 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;
+ else if (dist < 0)
+ std::cerr << "Near clipping value cannot be inferior than 0. Near "
+ "clipping won't be considered."
+ << std::endl;
+ else {
vpMbTracker::setNearClippingDistance(dist);
vpMbtDistanceLine *l;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
l->getPolygon().setNearClippingDistance(distNearClip);
}
@@ -2760,21 +2618,20 @@ vpMbEdgeTracker::setNearClippingDistance(const double &dist)
/*!
Specify which clipping to use.
-
+
\sa vpMbtPolygonClipping
-
+
\param flags : New clipping flags.
*/
-void
-vpMbEdgeTracker::setClipping(const unsigned int &flags)
-{
+void vpMbEdgeTracker::setClipping(const unsigned int &flags)
+{
vpMbTracker::setClipping(flags);
-
+
vpMbtDistanceLine *l;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
l->getPolygon().setClipping(clippingFlag);
}
@@ -2783,72 +2640,71 @@ vpMbEdgeTracker::setClipping(const unsigned int &flags)
}
/*!
- Compute the pyramid of image associated to the image in parameter. The scales
- computed are the ones corresponding to the scales attribute of the class. If
- OpenCV is detected, the functions used to computed a smoothed pyramid come
- from OpenCV, otherwise a simple subsampling (no smoothing, no interpolation)
- is realized.
-
- \warning The pyramid contains pointers to vpImage. To properly deallocate the
- pyramid. All the element but the first (which is a pointer to the input image)
- must be freed. A proper cleaning is implemented in the cleanPyramid() method.
-
+ Compute the pyramid of image associated to the image in parameter. The
+ scales computed are the ones corresponding to the scales attribute of the
+ class. If OpenCV is detected, the functions used to computed a smoothed
+ pyramid come from OpenCV, otherwise a simple subsampling (no smoothing, no
+ interpolation) is realized.
+
+ \warning The pyramid contains pointers to vpImage. To properly deallocate
+ the pyramid. All the element but the first (which is a pointer to the input
+ image) must be freed. A proper cleaning is implemented in the cleanPyramid()
+ method.
+
\param _I : The input image.
\param _pyramid : The pyramid of image to build from the input image.
*/
-void
-vpMbEdgeTracker::initPyramid(const vpImage<unsigned char>& _I, std::vector< const vpImage<unsigned char>* >& _pyramid)
+void vpMbEdgeTracker::initPyramid(const vpImage<unsigned char> &_I,
+ std::vector<const vpImage<unsigned char> *> &_pyramid)
{
_pyramid.resize(scales.size());
-
- if(scales[0]){
+
+ if (scales[0]) {
_pyramid[0] = &_I;
- }
- else{
+ } else {
_pyramid[0] = NULL;
}
-
- for(unsigned int i=1; i<_pyramid.size(); i += 1){
- if(scales[i]){
+
+ for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
+ if (scales[i]) {
unsigned int cScale = static_cast<unsigned int>(pow(2., (int)i));
- vpImage<unsigned char>* I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
+ vpImage<unsigned char> *I = new vpImage<unsigned char>(_I.getHeight() / cScale, _I.getWidth() / cScale);
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x020408))
- IplImage* vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
- vpI0->imageData = (char*)(_I.bitmap);
- IplImage* vpI = cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
+ IplImage *vpI0 = cvCreateImageHeader(cvSize((int)_I.getWidth(), (int)_I.getHeight()), IPL_DEPTH_8U, 1);
+ vpI0->imageData = (char *)(_I.bitmap);
+ IplImage *vpI =
+ cvCreateImage(cvSize((int)(_I.getWidth() / cScale), (int)(_I.getHeight() / cScale)), IPL_DEPTH_8U, 1);
cvResize(vpI0, vpI, CV_INTER_NN);
vpImageConvert::convert(vpI, *I);
- cvReleaseImage(&vpI);
+ cvReleaseImage(&vpI);
vpI0->imageData = NULL;
- cvReleaseImageHeader(&vpI0);
+ 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];
}
}
-#endif
+#endif
_pyramid[i] = I;
- }
- else{
+ } else {
_pyramid[i] = NULL;
}
}
}
/*!
- Clean the pyramid of image allocated with the initPyramid() method. The vector
- has a size equal to zero at the end of the method.
-
+ Clean the pyramid of image allocated with the initPyramid() method. The
+ vector has a size equal to zero at the end of the method.
+
\param _pyramid : The pyramid of image to clean.
*/
-void
-vpMbEdgeTracker::cleanPyramid(std::vector< const vpImage<unsigned char>* >& _pyramid)
+void vpMbEdgeTracker::cleanPyramid(std::vector<const vpImage<unsigned char> *> &_pyramid)
{
- if(_pyramid.size() > 0){
+ if (_pyramid.size() > 0) {
_pyramid[0] = NULL;
- for (unsigned int i = 1; i < _pyramid.size(); i += 1){
- if(_pyramid[i] != NULL){
+ for (unsigned int i = 1; i < _pyramid.size(); i += 1) {
+ if (_pyramid[i] != NULL) {
delete _pyramid[i];
_pyramid[i] = NULL;
}
@@ -2858,19 +2714,18 @@ vpMbEdgeTracker::cleanPyramid(std::vector< const vpImage<unsigned char>* >& _pyr
}
/*!
- Get the list of the lines tracked for the specified level. Each line contains
- the list of the vpMeSite.
+ Get the list of the lines tracked for the specified level. Each line
+ contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param linesList : The list of the lines of the model.
*/
-void
-vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsigned int level) const
+void vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *> &linesList, const unsigned int level) const
{
- if(level > scales.size() || !scales[level]){
+ if (level > scales.size() || !scales[level]) {
std::ostringstream oss;
oss << level;
std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
@@ -2880,21 +2735,19 @@ vpMbEdgeTracker::getLline(std::list<vpMbtDistanceLine *>& linesList, const unsig
linesList = lines[level];
}
-
/*!
Get the list of the cylinders tracked for the specified level. Each cylinder
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param cylindersList : The list of the cylinders of the model.
*/
-void
-vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList, const unsigned int level) const
+void vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *> &cylindersList, const unsigned int level) const
{
- if(level > scales.size() || !scales[level]){
+ if (level > scales.size() || !scales[level]) {
std::ostringstream oss;
oss << level;
std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
@@ -2904,21 +2757,19 @@ vpMbEdgeTracker::getLcylinder(std::list<vpMbtDistanceCylinder *>& cylindersList,
cylindersList = cylinders[level];
}
-
/*!
Get the list of the circles tracked for the specified level. Each circle
contains the list of the vpMeSite.
- \throw vpException::dimensionError if the second parameter does not correspond
- to an used level.
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
\param level : Level corresponding to the list to return.
\param circlesList : The list of the circles of the model.
*/
-void
-vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const unsigned int level) const
+void vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *> &circlesList, const unsigned int level) const
{
- if(level > scales.size() || !scales[level]){
+ if (level > scales.size() || !scales[level]) {
std::ostringstream oss;
oss << level;
std::string errorMsg = "level " + oss.str() + " is not used, cannot get its distance lines.";
@@ -2929,19 +2780,18 @@ vpMbEdgeTracker::getLcircle(std::list<vpMbtDistanceCircle *>& circlesList, const
}
/*!
- Modify the camera parameters to have them corresponding to the current scale.
- The new parameters are divided by \f$ 2^{\_scale} \f$.
-
- \param _scale : Scale to use.
+ Modify the camera parameters to have them corresponding to the current
+ scale. The new parameters are divided by \f$ 2^{\_scale} \f$.
+
+ \param _scale : Scale to use.
*/
-void
-vpMbEdgeTracker::downScale(const unsigned int _scale)
+void vpMbEdgeTracker::downScale(const unsigned int _scale)
{
const double ratio = pow(2., (int)_scale);
scaleLevel = _scale;
-
+
vpMatrix K = cam.get_K();
-
+
K[0][0] /= ratio;
K[1][1] /= ratio;
K[0][2] /= ratio;
@@ -2951,60 +2801,60 @@ vpMbEdgeTracker::downScale(const unsigned int _scale)
}
/*!
- Modify the camera parameters to have them corresponding to the current scale.
- The new parameters are multiplied by \f$ 2^{\_scale} \f$.
-
- \param _scale : Scale to use.
+ Modify the camera parameters to have them corresponding to the current
+ scale. The new parameters are multiplied by \f$ 2^{\_scale} \f$.
+
+ \param _scale : Scale to use.
*/
-void
-vpMbEdgeTracker::upScale(const unsigned int _scale)
+void vpMbEdgeTracker::upScale(const unsigned int _scale)
{
const double ratio = pow(2., (int)_scale);
scaleLevel = 0;
-
+
vpMatrix K = cam.get_K();
-
+
K[0][0] *= ratio;
K[1][1] *= ratio;
K[0][2] *= ratio;
K[1][2] *= ratio;
-
cam.initFromCalibrationMatrix(K);
}
/*!
- Re initialize the moving edges associated to a given level. This method is
- used to re-initialize the level if the tracking failed on this level but
- succeeded on the other one.
-
+ Re initialize the moving edges associated to a given level. This method is
+ used to re-initialize the level if the tracking failed on this level but
+ succeeded on the other one.
+
\param _lvl : The level to re-initialize.
*/
-void
-vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
+void vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
{
unsigned int scaleLevel_1 = scaleLevel;
scaleLevel = _lvl;
- vpMbtDistanceLine *l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceLine *l;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ if ((*it)->isTracked()) {
l = *it;
l->reinitMovingEdge(*Ipyramid[_lvl], cMo);
}
}
vpMbtDistanceCylinder *cy;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
cy->reinitMovingEdge(*Ipyramid[_lvl], cMo);
}
}
-
+
vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
ci->reinitMovingEdge(*Ipyramid[_lvl], cMo);
}
@@ -3016,36 +2866,39 @@ vpMbEdgeTracker::reInitLevel(const unsigned int _lvl)
}
/*!
- Set if the polygons that have the given name have to be considered during the tracking phase.
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
\param name : name of the polygon(s).
\param useEdgeTracking : True if it has to be considered, False otherwise.
*/
-void
-vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
+void vpMbEdgeTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
/*(*it)->setTracked(useEdgeTracking);
- for(std::list<int>::const_iterator itpoly=(*it)->Lindex_polygon.begin(); itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
+ for(std::list<int>::const_iterator
+ itpoly=(*it)->Lindex_polygon.begin();
+ itpoly!=(*it)->Lindex_polygon.end(); ++itpoly){
if(faces[(*itpoly)]->getName() != name){
(*it)->setTracked(true);
break;
}
}*/
- (*it)->setTracked(name,useEdgeTracking);
+ (*it)->setTracked(name, useEdgeTracking);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
- if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
+ if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
(*it)->setTracked(useEdgeTracking);
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
- if(faces[(unsigned)(*it)->index_polygon]->getName() == name){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
+ if (faces[(unsigned)(*it)->index_polygon]->getName() == name) {
(*it)->setTracked(useEdgeTracking);
}
}
diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp
index 6b90e50..9cd0295 100644
--- a/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCircle.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,26 +44,24 @@
\brief Make the complete tracking of an object by using its CAD model.
*/
-#include <stdlib.h>
#include <algorithm>
+#include <stdlib.h>
-#include <visp3/mbt/vpMbtDistanceCircle.h>
-#include <visp3/core/vpPlane.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbtDistanceCircle.h>
+#include <visp3/vision/vpPose.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
-#include <visp3/vision/vpPose.h>
/*!
Basic constructor
*/
vpMbtDistanceCircle::vpMbtDistanceCircle()
- : name(), index(0), cam(), me(NULL), wmean(1),
- featureEllipse(), isTrackedCircle(true), meEllipse(NULL),
- circle(NULL), radius(0.), p1(NULL), p2(NULL), p3(NULL),
- L(), error(), nbFeature(0), Reinit(false),
- hiddenface(NULL), index_polygon(-1), isvisible(false)
+ : 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)
{
}
@@ -71,42 +70,42 @@ vpMbtDistanceCircle::vpMbtDistanceCircle()
*/
vpMbtDistanceCircle::~vpMbtDistanceCircle()
{
- if (meEllipse != NULL) delete meEllipse;
- if (circle != NULL) delete circle;
- if (p1 != NULL) delete p1;
- if (p2 != NULL) delete p2;
- if (p3 != NULL) delete p3;
+ if (meEllipse != NULL)
+ delete meEllipse;
+ if (circle != NULL)
+ delete circle;
+ if (p1 != NULL)
+ delete p1;
+ if (p2 != NULL)
+ delete p2;
+ if (p3 != NULL)
+ delete p3;
}
/*!
Project the circle into the image.
-
- \param cMo : The pose of the camera used to project the circle into the image.
-*/
-void
-vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo)
-{
- circle->project(cMo) ;
-}
+ \param cMo : The pose of the camera used to project the circle into the
+ image.
+*/
+void vpMbtDistanceCircle::project(const vpHomogeneousMatrix &cMo) { circle->project(cMo); }
/*!
- Build a vpMbtDistanceCircle thanks to its center, 3 points (including the center) with
- coordinates expressed in the object frame and defining the plane that contain
- the circle and its radius.
+ Build a vpMbtDistanceCircle thanks to its center, 3 points (including the
+ center) with coordinates expressed in the object frame and defining the
+ plane that contain the circle and its radius.
\param _p1 : Center of the circle.
- \param _p2,_p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
+ \param _p2,_p3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param r : Radius of the circle.
*/
-void
-vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
+void vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpPoint &_p3, const double r)
{
- circle = new vpCircle ;
- p1 = new vpPoint ;
- p2 = new vpPoint ;
- p3 = new vpPoint ;
+ circle = new vpCircle;
+ p1 = new vpPoint;
+ p2 = new vpPoint;
+ p3 = new vpPoint;
// Get the points
*p1 = _p1;
@@ -122,62 +121,55 @@ vpMbtDistanceCircle::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const vpP
circle->setWorldCoordinates(plane.getA(), plane.getB(), plane.getC(), _p1.get_oX(), _p1.get_oY(), _p1.get_oZ(), r);
}
-
-/*!
+/*!
Set the moving edge parameters.
-
+
\param _me : an instance of vpMe containing all the desired parameters
*/
-void
-vpMbtDistanceCircle::setMovingEdge(vpMe *_me)
+void vpMbtDistanceCircle::setMovingEdge(vpMe *_me)
{
- me = _me ;
- if (meEllipse != NULL)
- {
- meEllipse->setMe(me) ;
+ me = _me;
+ if (meEllipse != NULL) {
+ meEllipse->setMe(me);
}
}
/*!
Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the circle.
-
+ The 3D model is projected into the image to create moving edges along the
+ circle.
+
\param I : The image.
\param cMo : The pose of the camera used to initialize the moving edges.
\return false if an error occur, true otherwise.
*/
-bool
-vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+bool vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(isvisible){
+ if (isvisible) {
// Perspective projection
circle->changeFrame(cMo);
- try{
+ try {
circle->projection();
- }
- catch(...){
- std::cout<<"Problem when projecting circle\n";
+ } catch (...) {
+ std::cout << "Problem when projecting circle\n";
return false;
}
// Create the moving edges containers
meEllipse = new vpMbtMeEllipse;
- meEllipse->setMe(me) ;
+ meEllipse->setMe(me);
- //meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
+ // meEllipse->setDisplay(vpMeSite::RANGE_RESULT) ; // TODO only for debug
meEllipse->setInitRange(me->getRange()); // TODO: check because set to zero for lines
- try
- {
+ try {
vpImagePoint ic;
double mu20_p, mu11_p, mu02_p;
vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
meEllipse->initTracking(I, ic, mu20_p, mu11_p, mu02_p);
- }
- catch(...)
- {
- //vpTRACE("the circle can't be initialized");
+ } catch (...) {
+ // vpTRACE("the circle can't be initialized");
return false;
}
}
@@ -186,21 +178,17 @@ vpMbtDistanceCircle::initMovingEdge(const vpImage<unsigned char> &I, const vpHom
/*!
Track the moving edges in the image.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+void vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
{
- if(isvisible){
- try
- {
- meEllipse->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meEllipse failed" << std::endl;
+ if (isvisible) {
+ try {
+ meEllipse->track(I);
+ } catch (...) {
+ // std::cout << "Track meEllipse failed" << std::endl;
meEllipse->reset();
Reinit = true;
}
@@ -210,67 +198,61 @@ vpMbtDistanceCircle::trackMovingEdge(const vpImage<unsigned char> &I, const vpHo
}
}
-
/*!
Update the moving edges internal parameters.
\warning : Not implemented.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceCircle::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(isvisible){
+ if (isvisible) {
// Perspective projection
circle->changeFrame(cMo);
- try{
+ try {
circle->projection();
+ } catch (...) {
+ std::cout << "Problem when projecting circle\n";
}
- catch(...){std::cout<<"Problem when projecting circle\n";}
- try
- {
+ try {
vpImagePoint ic;
double mu20_p, mu11_p, mu02_p;
vpMeterPixelConversion::convertEllipse(cam, *circle, ic, mu20_p, mu11_p, mu02_p);
meEllipse->updateParameters(I, ic, mu20_p, mu11_p, mu02_p);
- }
- catch(...)
- {
+ } catch (...) {
Reinit = true;
}
nbFeature = (unsigned int)meEllipse->getMeList().size();
}
}
-
/*!
Reinitialize the circle if it is required.
-
- A circle is reinitialized if the ellipse do not match enough with the projected 3D circle.
-
+
+ A circle is reinitialized if the ellipse do not match enough with the
+ projected 3D circle.
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(meEllipse!= NULL)
+ if (meEllipse != NULL)
delete meEllipse;
-
+
meEllipse = NULL;
- if(initMovingEdge(I,cMo) == false)
+ if (initMovingEdge(I, cMo) == false)
Reinit = true;
Reinit = false;
}
-
/*!
Display the circle. The 3D circle is projected into the image as an ellipse.
@@ -282,19 +264,19 @@ vpMbtDistanceCircle::reinitMovingEdge(const vpImage<unsigned char> &I, const vpH
\param displayFullModel : When true, display the circle even if non visible.
If false, display the circle only if visible.
*/
-void
-vpMbtDistanceCircle::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel )
+void vpMbtDistanceCircle::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedCircle) || displayFullModel){
+ if ((isvisible && isTrackedCircle) || displayFullModel) {
// Perspective projection
circle->changeFrame(cMo);
- try{
+ try {
circle->projection();
+ } catch (...) {
+ std::cout << "Cannot project the circle";
}
- catch(...){std::cout<<"Cannot project the circle";}
vpImagePoint center;
double mu20_p, mu11_p, mu02_p;
@@ -314,19 +296,19 @@ vpMbtDistanceCircle::display(const vpImage<unsigned char>&I, const vpHomogeneous
\param displayFullModel : When true, display the circle even if non visible.
If false, display the circle only if visible.
*/
-void
-vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col,
- const unsigned int thickness, const bool displayFullModel)
+void vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedCircle) || displayFullModel){
+ if ((isvisible && isTrackedCircle) || displayFullModel) {
// Perspective projection
circle->changeFrame(cMo);
- try{
+ try {
circle->projection();
+ } catch (...) {
+ std::cout << "Cannot project the circle";
}
- catch(...){std::cout<<"Cannot project the circle";}
vpImagePoint center;
double mu20_p, mu11_p, mu02_p;
@@ -335,22 +317,23 @@ vpMbtDistanceCircle::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix
}
}
-
/*!
- Enable to display the points along the ellipse with a color corresponding to their state.
-
+ Enable to display the points along the ellipse with a color corresponding
+ to their state.
+
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If blue : The point is removed because of the robust method in the
+ virtual visual servoing.
+
\param I : The image.
*/
-void
-vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
+void vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
{
- if (meEllipse != NULL)
- {
+ if (meEllipse != NULL) {
meEllipse->display(I); // display the me
if (vpDEBUG_ENABLE(3))
vpDisplay::flush(I);
@@ -360,40 +343,37 @@ vpMbtDistanceCircle::displayMovingEdges(const vpImage<unsigned char> &I)
/*!
Initialize the size of the interaction matrix and the error vector.
*/
-void
-vpMbtDistanceCircle::initInteractionMatrixError()
+void vpMbtDistanceCircle::initInteractionMatrixError()
{
- if (isvisible == true)
- {
+ if (isvisible) {
nbFeature = (unsigned int)meEllipse->getMeList().size();
L.resize(nbFeature, 6);
error.resize(nbFeature);
- }
- else
- nbFeature = 0 ;
+ } else
+ nbFeature = 0;
}
/*!
- Compute the interaction matrix and the error vector corresponding to the point to ellipse algebraic distance.
+ 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)
- {
+ if (isvisible) {
// Perspective projection
- circle->changeFrame(cMo) ;
- try{
+ circle->changeFrame(cMo);
+ try {
circle->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle\n";
}
- catch(...){std::cout<<"Problem projection circle\n";}
vpFeatureBuilder::create(featureEllipse, *circle);
vpMatrix H1 = featureEllipse.interaction();
vpRowVector H(5);
- double x=0, y=0;
+ double x = 0, y = 0;
// Get the parameters of the ellipse in the image plane
double xg = circle->p[0];
@@ -404,24 +384,23 @@ vpMbtDistanceCircle::computeInteractionMatrixError(const vpHomogeneousMatrix &cM
unsigned int j = 0;
- for(std::list<vpMeSite>::const_iterator it=meEllipse->getMeList().begin(); it!=meEllipse->getMeList().end(); ++it){
+ for (std::list<vpMeSite>::const_iterator it = meEllipse->getMeList().begin(); it != meEllipse->getMeList().end();
+ ++it) {
vpPixelMeterConversion::convertPoint(cam, it->j, it->i, x, y);
- H[0] = 2*(mu11*(y-yg)+mu02*(xg-x));
- H[1] = 2*(mu20*(yg-y)+mu11*(x-xg));
- H[2] = vpMath::sqr(y-yg)-mu02;
- H[3] = 2*(yg*(x-xg)+y*xg+mu11-x*y);
- H[4] = vpMath::sqr(x-xg)-mu20;
+ H[0] = 2 * (mu11 * (y - yg) + mu02 * (xg - x));
+ H[1] = 2 * (mu20 * (yg - y) + mu11 * (x - xg));
+ H[2] = vpMath::sqr(y - yg) - mu02;
+ H[3] = 2 * (yg * (x - xg) + y * xg + mu11 - x * y);
+ H[4] = vpMath::sqr(x - xg) - mu20;
- for (unsigned int k=0; k<6; k++)
- L[j][k] = H[0]*H1[0][k] + H[1]*H1[1][k] + H[2]*H1[2][k] + H[3]*H1[3][k] + H[4]*H1[4][k];
+ 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;
+ 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 0d0a4dd..81e85d4 100644
--- a/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceCylinder.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,14 +47,14 @@
\brief Make the complete tracking of an object by using its CAD model.
*/
-#include <visp3/mbt/vpMbtDistanceCylinder.h>
-#include <visp3/core/vpPlane.h>
+#include <algorithm>
+#include <stdlib.h>
#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbtDistanceCylinder.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
-#include <stdlib.h>
-#include <algorithm>
#include <visp3/vision/vpPose.h>
@@ -61,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)
{
}
@@ -74,46 +74,53 @@ vpMbtDistanceCylinder::vpMbtDistanceCylinder()
*/
vpMbtDistanceCylinder::~vpMbtDistanceCylinder()
{
-// cout << "Deleting cylinder " << index << endl ;
-
- if (p1 != NULL) delete p1 ;
- if (p2 != NULL) delete p2 ;
- if (c != NULL) delete c ;
- if (meline1 != NULL) delete meline1 ;
- if (meline2 != NULL) delete meline2 ;
- if (cercle1 != NULL) delete cercle1 ;
- if (cercle2 != NULL) delete cercle2 ;
+ // cout << "Deleting cylinder " << index << endl;
+
+ if (p1 != NULL)
+ delete p1;
+ if (p2 != NULL)
+ delete p2;
+ if (c != NULL)
+ delete c;
+ if (meline1 != NULL)
+ delete meline1;
+ if (meline2 != NULL)
+ delete meline2;
+ if (cercle1 != NULL)
+ delete cercle1;
+ if (cercle2 != NULL)
+ delete cercle2;
}
/*!
- Project the cylinder and the two points corresponding to its extremities into the image.
-
- \param cMo : The pose of the camera used to project the cylinder into the image.
+ Project the cylinder and the two points corresponding to its extremities
+ into the image.
+
+ \param cMo : The pose of the camera used to project the cylinder into the
+ image.
*/
-void
-vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceCylinder::project(const vpHomogeneousMatrix &cMo)
{
- c->project(cMo) ;
- p1->project(cMo) ;
- p2->project(cMo) ;
- cercle1->project(cMo) ;
- cercle2->project(cMo) ;
+ c->project(cMo);
+ p1->project(cMo);
+ p2->project(cMo);
+ cercle1->project(cMo);
+ cercle2->project(cMo);
}
-
/*!
- Build a vpMbtDistanceCylinder thanks to two points corresponding to the extremities of its axis and its radius.
-
+ Build a vpMbtDistanceCylinder thanks to two points corresponding to the
+ extremities of its axis and its radius.
+
\param _p1 : The first extremity on the axis.
\param _p2 : The second extremity on the axis.
\param r : Radius of the cylinder.
*/
-void
-vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
+void vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const double r)
{
- c = new vpCylinder ;
- p1 = new vpPoint ;
- p2 = new vpPoint ;
+ c = new vpCylinder;
+ p1 = new vpPoint;
+ p2 = new vpPoint;
cercle1 = new vpCircle;
cercle2 = new vpCircle;
@@ -136,48 +143,45 @@ vpMbtDistanceCylinder::buildFrom(const vpPoint &_p1, const vpPoint &_p2, const d
V2[2] = _p2.get_oZ();
// Get the axis of the cylinder
- ABC = V1-V2;
+ 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);
}
-
-/*!
+/*!
Set the moving edge parameters.
-
+
\param _me : an instance of vpMe containing all the desired parameters
*/
-void
-vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
+void vpMbtDistanceCylinder::setMovingEdge(vpMe *_me)
{
- me = _me ;
- if (meline1 != NULL)
- {
- meline1->setMe(me) ;
+ me = _me;
+ if (meline1 != NULL) {
+ meline1->setMe(me);
}
- if (meline2 != NULL)
- {
- meline2->setMe(me) ;
+ if (meline2 != NULL) {
+ meline2->setMe(me);
}
}
/*!
Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the lines.
-
+ The 3D model is projected into the image to create moving edges along the
+ lines.
+
\param I : The image.
\param cMo : The pose of the camera used to initialize the moving edges.
\return false if an error occur, true otherwise.
*/
-bool
-vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+bool vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(isvisible){
+ if (isvisible) {
// Perspective projection
p1->changeFrame(cMo);
p2->changeFrame(cMo);
@@ -187,125 +191,144 @@ vpMbtDistanceCylinder::initMovingEdge(const vpImage<unsigned char> &I, const vpH
p1->projection();
p2->projection();
- try{
+ try {
cercle1->projection();
- }
- catch(...){
- //std::cout<<"Problem when projecting circle 1\n";
+ } catch (...) {
+ // std::cout<<"Problem when projecting circle 1\n";
return false;
}
- try{
+ try {
cercle2->projection();
- }
- catch(...){
- //std::cout<<"Problem when projecting circle 2\n";
+ } catch (...) {
+ // std::cout<<"Problem when projecting circle 2\n";
return false;
}
c->projection();
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Create the moving edges containers
- meline1 = new vpMbtMeLine ;
- meline1->setMe(me) ;
- meline2 = new vpMbtMeLine ;
- meline2->setMe(me) ;
+ meline1 = new vpMbtMeLine;
+ meline1->setMe(me);
+ meline2 = new vpMbtMeLine;
+ meline2->setMe(me);
- // meline->setDisplay(vpMeSite::RANGE_RESULT) ;
+ // meline->setDisplay(vpMeSite::RANGE_RESULT);
meline1->setInitRange(0);
meline2->setInitRange(0);
// Conversion meter to pixels
- vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
+ 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;
+ double i11, i12, i21, i22, j11, j12, j21, j22;
vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
vpCircle::computeIntersectionPoint(*cercle1, cam, rho2, theta2, i21, j21);
vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// update limits of the melines.
- int marge = /*10*/5; //ou 5 normalement
- if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
- if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
+ int marge = /*10*/ 5; // ou 5 normalement
+ if (ip11.get_j() < ip12.get_j()) {
+ meline1->jmin = (int)ip11.get_j() - marge;
+ meline1->jmax = (int)ip12.get_j() + marge;
+ } else {
+ meline1->jmin = (int)ip12.get_j() - marge;
+ meline1->jmax = (int)ip11.get_j() + marge;
+ }
+ if (ip11.get_i() < ip12.get_i()) {
+ meline1->imin = (int)ip11.get_i() - marge;
+ meline1->imax = (int)ip12.get_i() + marge;
+ } else {
+ meline1->imin = (int)ip12.get_i() - marge;
+ meline1->imax = (int)ip11.get_i() + marge;
+ }
- if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
- if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
+ if (ip21.get_j() < ip22.get_j()) {
+ meline2->jmin = (int)ip21.get_j() - marge;
+ meline2->jmax = (int)ip22.get_j() + marge;
+ } else {
+ meline2->jmin = (int)ip22.get_j() - marge;
+ meline2->jmax = (int)ip21.get_j() + marge;
+ }
+ if (ip21.get_i() < ip22.get_i()) {
+ meline2->imin = (int)ip21.get_i() - marge;
+ meline2->imax = (int)ip22.get_i() + marge;
+ } else {
+ meline2->imin = (int)ip22.get_i() - marge;
+ meline2->imax = (int)ip21.get_i() + marge;
+ }
// Initialize the tracking
- while (theta1 > M_PI) { theta1 -= M_PI ; }
- while (theta1 < -M_PI) { theta1 += M_PI ; }
+ while (theta1 > M_PI) {
+ theta1 -= M_PI;
+ }
+ while (theta1 < -M_PI) {
+ theta1 += M_PI;
+ }
- if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
- else theta1 = M_PI/2.0 - theta1;
+ if (theta1 < -M_PI / 2.0)
+ theta1 = -theta1 - 3 * M_PI / 2.0;
+ else
+ theta1 = M_PI / 2.0 - theta1;
- while (theta2 > M_PI) { theta2 -= M_PI ; }
- while (theta2 < -M_PI) { theta2 += M_PI ; }
+ while (theta2 > M_PI) {
+ theta2 -= M_PI;
+ }
+ while (theta2 < -M_PI) {
+ theta2 += M_PI;
+ }
- if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
- else theta2 = M_PI/2.0 - theta2;
+ if (theta2 < -M_PI / 2.0)
+ theta2 = -theta2 - 3 * M_PI / 2.0;
+ else
+ theta2 = M_PI / 2.0 - theta2;
- try
- {
- meline1->initTracking(I,ip11,ip12,rho1,theta1);
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
+ try {
+ meline1->initTracking(I, ip11, ip12, rho1, theta1);
+ } catch (...) {
+ // vpTRACE("the line can't be initialized");
return false;
}
- try
- {
- meline2->initTracking(I,ip21,ip22,rho2,theta2);
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
+ try {
+ meline2->initTracking(I, ip21, ip22, rho2, theta2);
+ } catch (...) {
+ // vpTRACE("the line can't be initialized");
return false;
}
}
return true;
}
-
-
/*!
Track the moving edges in the image.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+void vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
{
- if(isvisible){
- try
- {
- meline1->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meline1 failed" << std::endl;
+ if (isvisible) {
+ try {
+ meline1->track(I);
+ } catch (...) {
+ // std::cout << "Track meline1 failed" << std::endl;
meline1->reset();
Reinit = true;
}
- try
- {
- meline2->track(I) ;
- }
- catch(...)
- {
- //std::cout << "Track meline2 failed" << std::endl;
+ try {
+ meline2->track(I);
+ } catch (...) {
+ // std::cout << "Track meline2 failed" << std::endl;
meline2->reset();
Reinit = true;
}
@@ -317,17 +340,15 @@ vpMbtDistanceCylinder::trackMovingEdge(const vpImage<unsigned char> &I, const vp
}
}
-
/*!
Update the moving edges internal parameters.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(isvisible){
+ if (isvisible) {
// Perspective projection
p1->changeFrame(cMo);
p2->changeFrame(cMo);
@@ -337,26 +358,28 @@ vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const v
p1->projection();
p2->projection();
- try{
+ try {
cercle1->projection();
+ } catch (...) {
+ std::cout << "Probleme projection cercle 1\n";
}
- catch(...){std::cout<<"Probleme projection cercle 1\n";}
- try{
+ try {
cercle2->projection();
+ } catch (...) {
+ std::cout << "Probleme projection cercle 2\n";
}
- catch(...){std::cout<<"Probleme projection cercle 2\n";}
c->projection();
// Get the limbos
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Conversion meter to pixels
- vpMeterPixelConversion::convertLine(cam,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(cam,c->getRho2(),c->getTheta2(),rho2,theta2);
+ 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;
+ double i11, i12, i21, i22, j11, j12, j21, j22;
vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
@@ -365,49 +388,79 @@ vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const v
vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// update limits of the meline.
- int marge = /*10*/5; //ou 5 normalement
- if (ip11.get_j()<ip12.get_j()) { meline1->jmin = (int)ip11.get_j()-marge ; meline1->jmax = (int)ip12.get_j()+marge ; } else{ meline1->jmin = (int)ip12.get_j()-marge ; meline1->jmax = (int)ip11.get_j()+marge ; }
- if (ip11.get_i()<ip12.get_i()) { meline1->imin = (int)ip11.get_i()-marge ; meline1->imax = (int)ip12.get_i()+marge ; } else{ meline1->imin = (int)ip12.get_i()-marge ; meline1->imax = (int)ip11.get_i()+marge ; }
+ int marge = /*10*/ 5; // ou 5 normalement
+ if (ip11.get_j() < ip12.get_j()) {
+ meline1->jmin = (int)ip11.get_j() - marge;
+ meline1->jmax = (int)ip12.get_j() + marge;
+ } else {
+ meline1->jmin = (int)ip12.get_j() - marge;
+ meline1->jmax = (int)ip11.get_j() + marge;
+ }
+ if (ip11.get_i() < ip12.get_i()) {
+ meline1->imin = (int)ip11.get_i() - marge;
+ meline1->imax = (int)ip12.get_i() + marge;
+ } else {
+ meline1->imin = (int)ip12.get_i() - marge;
+ meline1->imax = (int)ip11.get_i() + marge;
+ }
- if (ip21.get_j()<ip22.get_j()) { meline2->jmin = (int)ip21.get_j()-marge ; meline2->jmax = (int)ip22.get_j()+marge ; } else{ meline2->jmin = (int)ip22.get_j()-marge ; meline2->jmax = (int)ip21.get_j()+marge ; }
- if (ip21.get_i()<ip22.get_i()) { meline2->imin = (int)ip21.get_i()-marge ; meline2->imax = (int)ip22.get_i()+marge ; } else{ meline2->imin = (int)ip22.get_i()-marge ; meline2->imax = (int)ip21.get_i()+marge ; }
+ if (ip21.get_j() < ip22.get_j()) {
+ meline2->jmin = (int)ip21.get_j() - marge;
+ meline2->jmax = (int)ip22.get_j() + marge;
+ } else {
+ meline2->jmin = (int)ip22.get_j() - marge;
+ meline2->jmax = (int)ip21.get_j() + marge;
+ }
+ if (ip21.get_i() < ip22.get_i()) {
+ meline2->imin = (int)ip21.get_i() - marge;
+ meline2->imax = (int)ip22.get_i() + marge;
+ } else {
+ meline2->imin = (int)ip22.get_i() - marge;
+ meline2->imax = (int)ip21.get_i() + marge;
+ }
// Initialize the tracking
- while (theta1 > M_PI) { theta1 -= M_PI ; }
- while (theta1 < -M_PI) { theta1 += M_PI ; }
+ while (theta1 > M_PI) {
+ theta1 -= M_PI;
+ }
+ while (theta1 < -M_PI) {
+ theta1 += M_PI;
+ }
- if (theta1 < -M_PI/2.0) theta1 = -theta1 - 3*M_PI/2.0;
- else theta1 = M_PI/2.0 - theta1;
+ if (theta1 < -M_PI / 2.0)
+ theta1 = -theta1 - 3 * M_PI / 2.0;
+ else
+ theta1 = M_PI / 2.0 - theta1;
- while (theta2 > M_PI) { theta2 -= M_PI ; }
- while (theta2 < -M_PI) { theta2 += M_PI ; }
+ while (theta2 > M_PI) {
+ theta2 -= M_PI;
+ }
+ while (theta2 < -M_PI) {
+ theta2 += M_PI;
+ }
- if (theta2 < -M_PI/2.0) theta2 = -theta2 - 3*M_PI/2.0;
- else theta2 = M_PI/2.0 - theta2;
+ if (theta2 < -M_PI / 2.0)
+ theta2 = -theta2 - 3 * M_PI / 2.0;
+ else
+ theta2 = M_PI / 2.0 - theta2;
- try
- {
- //meline1->updateParameters(I,rho1,theta1) ;
- meline1->updateParameters(I,ip11,ip12,rho1,theta1) ;
- }
- catch(...)
- {
+ try {
+ // meline1->updateParameters(I,rho1,theta1);
+ meline1->updateParameters(I, ip11, ip12, rho1, theta1);
+ } catch (...) {
Reinit = true;
}
- try
- {
- //meline2->updateParameters(I,rho2,theta2) ;
- meline2->updateParameters(I,ip21,ip22,rho2,theta2) ;
- }
- catch(...)
- {
+ try {
+ // meline2->updateParameters(I,rho2,theta2);
+ meline2->updateParameters(I, ip21, ip22, rho2, theta2);
+ } catch (...) {
Reinit = true;
}
@@ -418,33 +471,31 @@ vpMbtDistanceCylinder::updateMovingEdge(const vpImage<unsigned char> &I, const v
}
}
-
/*!
Reinitialize the cylinder if it is required.
-
- A line is reinitialized if the 2D lines do not match enough with the projected 3D lines.
-
+
+ A line is reinitialized if the 2D lines do not match enough with the
+ projected 3D lines.
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(meline1!= NULL)
+ if (meline1 != NULL)
delete meline1;
- if(meline2!= NULL)
+ if (meline2 != NULL)
delete meline2;
-
+
meline1 = NULL;
meline2 = NULL;
- if (initMovingEdge(I,cMo) == false)
+ if (initMovingEdge(I, cMo) == false)
Reinit = true;
Reinit = false;
}
-
/*!
Display the cylinder. The 3D cylinder is projected into the image.
@@ -455,12 +506,11 @@ vpMbtDistanceCylinder::reinitMovingEdge(const vpImage<unsigned char> &I, const v
\param thickness : The thickness of the lines.
\param displayFullModel : When true, display the circle even if non visible.
*/
-void
-vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters&camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel)
+void vpMbtDistanceCylinder::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedCylinder) || displayFullModel){
+ if ((isvisible && isTrackedCylinder) || displayFullModel) {
// Perspective projection
p1->changeFrame(cMo);
p2->changeFrame(cMo);
@@ -470,25 +520,27 @@ vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneo
p1->projection();
p2->projection();
- try{
+ try {
cercle1->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 1";
}
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
+ try {
cercle2->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 2";
}
- catch(...){std::cout<<"Problem projection circle 2";}
c->projection();
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
+ 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;
+ double i11, i12, i21, i22, j11, j12, j21, j22;
vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
@@ -497,15 +549,15 @@ vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneo
vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ vpDisplay::displayLine(I, ip11, ip12, col, thickness);
+ vpDisplay::displayLine(I, ip21, ip22, col, thickness);
}
}
@@ -519,12 +571,11 @@ vpMbtDistanceCylinder::display(const vpImage<unsigned char>&I, const vpHomogeneo
\param thickness : The thickness of the lines.
\param displayFullModel : When true, display the circle even if non visible.
*/
-void
-vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness,
- const bool displayFullModel)
+void vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedCylinder) || displayFullModel){
+ if ((isvisible && isTrackedCylinder) || displayFullModel) {
// Perspective projection
p1->changeFrame(cMo);
p2->changeFrame(cMo);
@@ -534,25 +585,27 @@ vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatr
p1->projection();
p2->projection();
- try{
+ try {
cercle1->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 1";
}
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
+ try {
cercle2->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 2";
}
- catch(...){std::cout<<"Problem projection circle 2";}
c->projection();
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,c->getRho1(),c->getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,c->getRho2(),c->getTheta2(),rho2,theta2);
+ 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;
+ double i11, i12, i21, i22, j11, j12, j21, j22;
vpCircle::computeIntersectionPoint(*cercle1, cam, rho1, theta1, i11, j11);
vpCircle::computeIntersectionPoint(*cercle2, cam, rho1, theta1, i12, j12);
@@ -561,38 +614,38 @@ vpMbtDistanceCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatr
vpCircle::computeIntersectionPoint(*cercle2, cam, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ vpDisplay::displayLine(I, ip11, ip12, col, thickness);
+ vpDisplay::displayLine(I, ip21, ip22, col, thickness);
}
}
-
/*!
- Enable to display the points along the lines with a color corresponding to their state.
-
+ Enable to display the points along the lines with a color corresponding to
+ their state.
+
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If blue : The point is removed because of the robust method in the
+ virtual visual servoing.
+
\param I : The image.
*/
-void
-vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
+void vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
{
- if (meline1 != NULL)
- {
+ if (meline1 != NULL) {
meline1->display(I);
}
- if (meline2 != NULL)
- {
+ if (meline2 != NULL) {
meline2->display(I);
}
}
@@ -600,117 +653,122 @@ vpMbtDistanceCylinder::displayMovingEdges(const vpImage<unsigned char> &I)
/*!
Initialize the size of the interaction matrix and the error vector.
*/
-void
-vpMbtDistanceCylinder::initInteractionMatrixError()
+void vpMbtDistanceCylinder::initInteractionMatrixError()
{
- if (isvisible == true) {
+ if (isvisible) {
nbFeaturel1 = (unsigned int)meline1->getMeList().size();
nbFeaturel2 = (unsigned int)meline2->getMeList().size();
nbFeature = nbFeaturel1 + nbFeaturel2;
L.resize(nbFeature, 6);
error.resize(nbFeature);
- }
- else {
- nbFeature = 0 ;
+ } else {
+ nbFeature = 0;
nbFeaturel1 = 0;
nbFeaturel2 = 0;
}
}
/*!
- Compute the interaction matrix and the error vector corresponding to the cylinder.
+ Compute the interaction matrix and the error vector corresponding to the
+ cylinder.
*/
-void
-vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo, const vpImage<unsigned char> &I)
+void vpMbtDistanceCylinder::computeInteractionMatrixError(const vpHomogeneousMatrix &cMo,
+ const vpImage<unsigned char> &I)
{
if (isvisible) {
// Perspective projection
- c->changeFrame(cMo) ;
- c->projection() ;
- cercle1->changeFrame(cMo) ;
- cercle1->changeFrame(cMo) ;
- try{
+ c->changeFrame(cMo);
+ c->projection();
+ cercle1->changeFrame(cMo);
+ cercle1->changeFrame(cMo);
+ try {
cercle1->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 1\n";
}
- catch(...){std::cout<<"Problem projection circle 1\n";}
- try{
+ try {
cercle2->projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 2\n";
}
- catch(...){std::cout<<"Problem projection circle 2\n";}
bool disp = false;
bool disp2 = false;
- if (disp || disp2) vpDisplay::flush(I);
+ if (disp || disp2)
+ vpDisplay::flush(I);
// Build the lines
- vpFeatureBuilder::create(featureline2,*c,vpCylinder::line2) ;
- vpFeatureBuilder::create(featureline1,*c,vpCylinder::line1) ;
+ vpFeatureBuilder::create(featureline2, *c, vpCylinder::line2);
+ vpFeatureBuilder::create(featureline1, *c, vpCylinder::line1);
- double rho1 = featureline1.getRho() ;
- double theta1 = featureline1.getTheta() ;
- double rho2 = featureline2.getRho() ;
- double theta2 = featureline2.getTheta() ;
+ double rho1 = featureline1.getRho();
+ double theta1 = featureline1.getTheta();
+ double rho2 = featureline2.getRho();
+ double theta2 = featureline2.getTheta();
double co1 = cos(theta1);
double si1 = sin(theta1);
double co2 = cos(theta2);
double si2 = sin(theta2);
- double mx = 1.0/cam.get_px() ;
- double my = 1.0/cam.get_py() ;
- double xc = cam.get_u0() ;
- double yc = cam.get_v0() ;
+ double mx = 1.0 / cam.get_px();
+ double my = 1.0 / cam.get_py();
+ double xc = cam.get_u0();
+ double yc = cam.get_v0();
- vpMatrix H1 ;
- H1 = featureline1.interaction() ;
- vpMatrix H2 ;
- H2 = featureline2.interaction() ;
+ vpMatrix H1;
+ H1 = featureline1.interaction();
+ vpMatrix H2;
+ H2 = featureline2.interaction();
- vpMeSite p ;
- unsigned int j =0 ;
- for(std::list<vpMeSite>::const_iterator it=meline1->getMeList().begin(); it!=meline1->getMeList().end(); ++it){
+ vpMeSite p;
+ unsigned int j = 0;
+ for (std::list<vpMeSite>::const_iterator it = meline1->getMeList().begin(); it != meline1->getMeList().end();
+ ++it) {
double x = (double)it->j;
double y = (double)it->i;
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
+ x = (x - xc) * mx;
+ y = (y - yc) * my;
- double alpha1 = x*si1 - y*co1;
+ double alpha1 = x * si1 - y * co1;
- double *Lrho = H1[0] ;
- double *Ltheta = H1[1] ;
+ double *Lrho = H1[0];
+ double *Ltheta = H1[1];
// Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++){
- L[j][k] = (Lrho[k] + alpha1*Ltheta[k]);
+ for (unsigned int k = 0; k < 6; k++) {
+ L[j][k] = (Lrho[k] + alpha1 * Ltheta[k]);
}
- error[j] = rho1 - ( x*co1 + y*si1) ;
+ error[j] = rho1 - (x * co1 + y * si1);
- if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100), vpColor::orange,1);
+ if (disp)
+ vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::orange, 1);
j++;
}
- for(std::list<vpMeSite>::const_iterator it=meline2->getMeList().begin(); it!=meline2->getMeList().end(); ++it){
+ for (std::list<vpMeSite>::const_iterator it = meline2->getMeList().begin(); it != meline2->getMeList().end();
+ ++it) {
double x = (double)it->j;
double y = (double)it->i;
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
+ x = (x - xc) * mx;
+ y = (y - yc) * my;
- double alpha2 = x*si2 - y*co2;
+ double alpha2 = x * si2 - y * co2;
- double *Lrho = H2[0] ;
- double *Ltheta = H2[1] ;
+ double *Lrho = H2[0];
+ double *Ltheta = H2[1];
// Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++){
- L[j][k] = (Lrho[k] + alpha2*Ltheta[k]);
+ for (unsigned int k = 0; k < 6; k++) {
+ L[j][k] = (Lrho[k] + alpha2 * Ltheta[k]);
}
- error[j] = rho2 - ( x*co2 + y*si2) ;
+ error[j] = rho2 - (x * co2 + y * si2);
- if (disp) vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j]*100),vpColor::red,1);
+ if (disp)
+ vpDisplay::displayCross(I, it->i, it->j, (unsigned int)(error[j] * 100), vpColor::red, 1);
j++;
}
}
}
-
diff --git a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp
index d6dc968..a92db66 100644
--- a/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtDistanceLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,11 +44,11 @@
\brief Make the complete tracking of an object by using its CAD model.
*/
-#include <visp3/mbt/vpMbtDistanceLine.h>
-#include <visp3/core/vpPlane.h>
+#include <stdlib.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/mbt/vpMbtDistanceLine.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <stdlib.h>
void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane);
void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L);
@@ -56,10 +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)
{
}
@@ -68,117 +68,116 @@ vpMbtDistanceLine::vpMbtDistanceLine()
*/
vpMbtDistanceLine::~vpMbtDistanceLine()
{
-// cout << "Deleting line " << index << endl ;
- if (line != NULL) delete line ;
+ // cout << "Deleting line " << index << endl;
+ if (line != NULL)
+ delete line;
- for(unsigned int i = 0 ; i < meline.size() ; i++)
- if (meline[i] != NULL) delete meline[i] ;
+ for (unsigned int i = 0; i < meline.size(); i++)
+ if (meline[i] != NULL)
+ delete meline[i];
meline.clear();
}
/*!
- Project the line and the two points corresponding to its extremities into the image.
-
+ Project the line and the two points corresponding to its extremities into
+ the image.
+
\param cMo : The pose of the camera used to project the line into the image.
*/
-void
-vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceLine::project(const vpHomogeneousMatrix &cMo)
{
- line->project(cMo) ;
- p1->project(cMo) ;
- p2->project(cMo) ;
+ line->project(cMo);
+ p1->project(cMo);
+ p2->project(cMo);
}
/*!
Build a 3D plane thanks to 3 points and stores it in \f$ plane \f$.
-
+
\param P : The first point to define the plane
\param Q : The second point to define the plane
\param R : The third point to define the plane
- \param plane : The vpPlane instance used to store the computed plane equation.
+ \param plane : The vpPlane instance used to store the computed plane
+ equation.
*/
-void
-buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
+void buildPlane(vpPoint &P, vpPoint &Q, vpPoint &R, vpPlane &plane)
{
vpColVector a(3);
vpColVector b(3);
- vpColVector n(3);
- //Calculate vector corresponding to PQ
- a[0]=P.get_oX()-Q.get_oX();
- a[1]=P.get_oY()-Q.get_oY();
- a[2]=P.get_oZ()-Q.get_oZ();
- //Calculate vector corresponding to PR
- b[0]=P.get_oX()-R.get_oX();
- b[1]=P.get_oY()-R.get_oY();
- b[2]=P.get_oZ()-R.get_oZ();
+ // Calculate vector corresponding to PQ
+ a[0] = P.get_oX() - Q.get_oX();
+ a[1] = P.get_oY() - Q.get_oY();
+ a[2] = P.get_oZ() - Q.get_oZ();
+
+ // Calculate vector corresponding to PR
+ b[0] = P.get_oX() - R.get_oX();
+ b[1] = P.get_oY() - R.get_oY();
+ b[2] = P.get_oZ() - R.get_oZ();
- //Calculate normal vector to plane PQ x PR
- n=vpColVector::cross(a,b);
+ // Calculate normal vector to plane PQ x PR
+ vpColVector n = vpColVector::cross(a, b);
- //Equation of the plane is given by:
+ // Equation of the plane is given by:
double A = n[0];
double B = n[1];
double C = n[2];
- double D=-(A*P.get_oX()+B*P.get_oY()+C*P.get_oZ());
+ double D = -(A * P.get_oX() + B * P.get_oY() + C * P.get_oZ());
- double norm = sqrt(A*A+B*B+C*C) ;
- plane.setA(A/norm) ;
- plane.setB(B/norm) ;
- plane.setC(C/norm) ;
- plane.setD(D/norm) ;
+ double norm = sqrt(A * A + B * B + C * C);
+ plane.setA(A / norm);
+ plane.setB(B / norm);
+ plane.setC(C / norm);
+ plane.setD(D / norm);
}
-
/*!
Build a line thanks to 4 points.
-
- The method is the following : Two plane are computed thanks to (P1,P2,P3) and (P1,P2,P4) (see the buildPlane method). Then the line equation is computed thanks to the intersection between the two planes.
-
+
+ The method is the following : Two plane are computed thanks to (P1,P2,P3)
+ and (P1,P2,P4) (see the buildPlane method). Then the line equation is
+ computed thanks to the intersection between the two planes.
+
\param P1 : The first point to compute the line.
\param P2 : The second point to compute the line.
\param P3 : The third point to compute the line.
\param P4 : The fourth point to compute the line.
\param L : The instance of vpLine to store the computed line equation.
*/
-void
-buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
+void buildLine(vpPoint &P1, vpPoint &P2, vpPoint &P3, vpPoint &P4, vpLine &L)
{
vpPlane plane1;
- vpPlane plane2 ;
- buildPlane(P1,P2,P3,plane1) ;
- buildPlane(P1,P2,P4,plane2) ;
+ vpPlane plane2;
+ buildPlane(P1, P2, P3, plane1);
+ buildPlane(P1, P2, P4, plane2);
- L.setWorldCoordinates(plane1.getA(),plane1.getB(), plane1.getC(),plane1.getD(),
- plane2.getA(),plane2.getB(), plane2.getC(),plane2.getD()) ;
+ L.setWorldCoordinates(plane1.getA(), plane1.getB(), plane1.getC(), plane1.getD(), plane2.getA(), plane2.getB(),
+ plane2.getC(), plane2.getD());
}
-
/*!
- Build a vpMbtDistanceLine thanks to two points corresponding to the extremities.
-
+ Build a vpMbtDistanceLine thanks to two points corresponding to the
+ extremities.
+
\param _p1 : The first extremity.
\param _p2 : The second extremity.
*/
-void
-vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
+void vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
{
if (line == NULL) {
- line = new vpLine ;
+ line = new vpLine;
}
poly.setNbPoint(2);
poly.addPoint(0, _p1);
poly.addPoint(1, _p2);
-
+
p1 = &poly.p[0];
p2 = &poly.p[1];
vpColVector V1(3);
vpColVector V2(3);
- vpColVector V3(3);
- vpColVector V4(3);
V1[0] = p1->get_oX();
V1[1] = p1->get_oY();
@@ -187,30 +186,25 @@ vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
V2[1] = p2->get_oY();
V2[2] = p2->get_oZ();
- //if((V1-V2).sumSquare()!=0)
- if(std::fabs((V1-V2).sumSquare()) > std::numeric_limits<double>::epsilon())
- {
- {
- V3[0]=double(rand()%1000)/100;
- V3[1]=double(rand()%1000)/100;
- V3[2]=double(rand()%1000)/100;
-
-
- vpColVector v_tmp1,v_tmp2;
- v_tmp1 = V2-V1;
- v_tmp2 = V3-V1;
- V4=vpColVector::cross(v_tmp1,v_tmp2);
- }
-
- vpPoint P3(V3[0],V3[1],V3[2]);
- vpPoint P4(V4[0],V4[1],V4[2]);
- buildLine(*p1,*p2, P3,P4, *line) ;
- }
- else
- {
- vpPoint P3(V1[0],V1[1],V1[2]);
- vpPoint P4(V2[0],V2[1],V2[2]);
- buildLine(*p1,*p2,P3,P4,*line) ;
+ // if((V1-V2).sumSquare()!=0)
+ if (std::fabs((V1 - V2).sumSquare()) > std::numeric_limits<double>::epsilon()) {
+ vpColVector V3(3);
+ V3[0] = double(rand() % 1000) / 100;
+ V3[1] = double(rand() % 1000) / 100;
+ V3[2] = double(rand() % 1000) / 100;
+
+ vpColVector v_tmp1, v_tmp2;
+ v_tmp1 = V2 - V1;
+ v_tmp2 = V3 - V1;
+ vpColVector V4 = vpColVector::cross(v_tmp1, v_tmp2);
+
+ vpPoint P3(V3[0], V3[1], V3[2]);
+ vpPoint P4(V4[0], V4[1], V4[2]);
+ buildLine(*p1, *p2, P3, P4, *line);
+ } else {
+ vpPoint P3(V1[0], V1[1], V1[2]);
+ vpPoint P4(V2[0], V2[1], V2[2]);
+ buildLine(*p1, *p2, P3, P4, *line);
}
}
@@ -219,8 +213,7 @@ vpMbtDistanceLine::buildFrom(vpPoint &_p1, vpPoint &_p2)
\param idx : Index of the polygon
*/
-void
-vpMbtDistanceLine::addPolygon(const int &idx)
+void vpMbtDistanceLine::addPolygon(const int &idx)
{
Lindex_polygon.push_back(idx);
Lindex_polygon_tracked.push_back(true);
@@ -233,26 +226,24 @@ 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<int>::const_iterator itpoly=Lindex_polygon.begin(); itpoly!=Lindex_polygon.end(); ++itpoly){
- if((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname){
+ for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
+ if ((*hiddenface)[(unsigned)(*itpoly)]->getName() == polyname) {
Lindex_polygon_tracked[ind] = track;
}
ind++;
}
isTrackedLine = false;
- for(unsigned int i = 0 ; i < Lindex_polygon_tracked.size() ; i++)
- if(Lindex_polygon_tracked[i])
- {
+ for (unsigned int i = 0; i < Lindex_polygon_tracked.size(); i++)
+ if (Lindex_polygon_tracked[i]) {
isTrackedLine = true;
break;
}
- if(!isTrackedLine){
+ if (!isTrackedLine) {
isTrackedLineWithVisibility = false;
return;
}
@@ -262,20 +253,20 @@ vpMbtDistanceLine::setTracked(const std::string &polyname, const bool &track)
/*!
Update the boolean specifying if the line has to be tracked.
- It takes into account the desactivated polygons and the visibility of the others.
+ It takes into account the desactivated polygons and the visibility of the
+ others.
*/
-void
-vpMbtDistanceLine::updateTracked()
+void vpMbtDistanceLine::updateTracked()
{
- if(!isTrackedLine){
+ if (!isTrackedLine) {
isTrackedLineWithVisibility = false;
return;
}
unsigned int ind = 0;
isTrackedLineWithVisibility = false;
- for(std::list<int>::const_iterator itpoly=Lindex_polygon.begin(); itpoly!=Lindex_polygon.end(); ++itpoly){
- if((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]){
+ for (std::list<int>::const_iterator itpoly = Lindex_polygon.begin(); itpoly != Lindex_polygon.end(); ++itpoly) {
+ if ((*hiddenface)[(unsigned)(*itpoly)]->isVisible() && Lindex_polygon_tracked[ind]) {
isTrackedLineWithVisibility = true;
break;
}
@@ -283,184 +274,196 @@ vpMbtDistanceLine::updateTracked()
}
}
-/*!
+/*!
Set the moving edge parameters.
-
+
\param _me : an instance of vpMe containing all the desired parameters
*/
-void
-vpMbtDistanceLine::setMovingEdge(vpMe *_me)
+void vpMbtDistanceLine::setMovingEdge(vpMe *_me)
{
- me = _me ;
+ me = _me;
- for(unsigned int i = 0 ; i < meline.size() ; i++)
- if (meline[i] != NULL)
- {
-// nbFeature[i] = 0;
+ for (unsigned int i = 0; i < meline.size(); i++)
+ if (meline[i] != NULL) {
+ // nbFeature[i] = 0;
meline[i]->reset();
- meline[i]->setMe(me) ;
+ meline[i]->setMe(me);
}
-// nbFeatureTotal = 0;
+ // nbFeatureTotal = 0;
}
-
/*!
- Initialize the moving edge thanks to a given pose of the camera.
- The 3D model is projected into the image to create moving edges along the line.
-
+ Initialize the moving edge thanks to a given pose of the camera.
+ The 3D model is projected into the image to create moving edges along the
+ line.
+
\param I : The image.
\param cMo : The pose of the camera used to initialize the moving edges.
\return false if an error occur, true otherwise.
*/
-bool
-vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+bool vpMbtDistanceLine::initMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- if (meline[i] != NULL) delete meline[i] ;
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ if (meline[i] != NULL)
+ delete meline[i];
}
meline.clear();
nbFeature.clear();
nbFeatureTotal = 0;
- if(isvisible)
- {
+ if (isvisible) {
p1->changeFrame(cMo);
p2->changeFrame(cMo);
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ if (poly.getClipping() > 3) // Contains at least one FOV constraint
cam.computeFov(I.getWidth(), I.getHeight());
poly.computePolygonClipped(cam);
- if(poly.polyClipped.size() == 2){ //Les points sont visibles.
+ if (poly.polyClipped.size() == 2) { // Les points sont visibles.
std::vector<std::pair<vpPoint, vpPoint> > linesLst;
- if(useScanLine){
- hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst);
- }
- else{
- linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ if (useScanLine) {
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
+ } else {
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
}
- if(linesLst.size() == 0){
-// isvisible = false;
+ if (linesLst.size() == 0) {
+ // isvisible = false;
return false;
}
- // To have the exact same pose values as the old version (angle or ogre visibility test only), points should be reorganised when using scanline algorithm.
-// if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].first.get_X())) > sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].second.get_X())))
-// {
-// std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
-// for(int i = linesLst.size()-1 ; i >= 0 ; i--)
-// linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
-// linesLst = linesLstTmp;
-// }
+ // To have the exact same pose values as the old version (angle or ogre
+ // visibility test only), points should be reorganised when using
+ // scanline algorithm.
+ // if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() -
+ // linesLst[0].first.get_X())) >
+ // sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() -
+ // linesLst[0].second.get_X())))
+ // {
+ // std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
+ // for(int i = linesLst.size()-1 ; i >= 0 ; i--)
+ // linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
+ // linesLst = linesLstTmp;
+ // }
line->changeFrame(cMo);
line->projection();
- double rho,theta;
- //rho theta uv
- vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
+ double rho, theta;
+ // rho theta uv
+ vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta);
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < -M_PI) { theta += M_PI ; }
-
- if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
- else theta = M_PI/2.0 - theta;
+ while (theta > M_PI) {
+ theta -= M_PI;
+ }
+ while (theta < -M_PI) {
+ theta += M_PI;
+ }
+ if (theta < -M_PI / 2.0)
+ theta = -theta - 3 * M_PI / 2.0;
+ else
+ theta = M_PI / 2.0 - theta;
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ for (unsigned int i = 0; i < linesLst.size(); i++) {
vpImagePoint ip1, ip2;
linesLst[i].first.project();
linesLst[i].second.project();
- vpMeterPixelConversion::convertPoint(cam,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
+ 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) ;
+ vpMbtMeLine *melinePt = new vpMbtMeLine;
+ melinePt->setMe(me);
- // meline[i]->setDisplay(vpMeSite::RANGE_RESULT) ;
+ // meline[i]->setDisplay(vpMeSite::RANGE_RESULT);
melinePt->setInitRange(0);
- int marge = /*10*/5; //ou 5 normalement
- if (ip1.get_j()<ip2.get_j()) { melinePt->jmin = (int)ip1.get_j()-marge ; melinePt->jmax = (int)ip2.get_j()+marge ; } else{ melinePt->jmin = (int)ip2.get_j()-marge ; melinePt->jmax = (int)ip1.get_j()+marge ; }
- if (ip1.get_i()<ip2.get_i()) { melinePt->imin = (int)ip1.get_i()-marge ; melinePt->imax = (int)ip2.get_i()+marge ; } else{ melinePt->imin = (int)ip2.get_i()-marge ; melinePt->imax = (int)ip1.get_i()+marge ; }
+ int marge = /*10*/ 5; // ou 5 normalement
+ if (ip1.get_j() < ip2.get_j()) {
+ melinePt->jmin = (int)ip1.get_j() - marge;
+ melinePt->jmax = (int)ip2.get_j() + marge;
+ } else {
+ melinePt->jmin = (int)ip2.get_j() - marge;
+ melinePt->jmax = (int)ip1.get_j() + marge;
+ }
+ if (ip1.get_i() < ip2.get_i()) {
+ melinePt->imin = (int)ip1.get_i() - marge;
+ melinePt->imax = (int)ip2.get_i() + marge;
+ } else {
+ melinePt->imin = (int)ip2.get_i() - marge;
+ melinePt->imax = (int)ip1.get_i() + marge;
+ }
- try
- {
- melinePt->initTracking(I,ip1,ip2,rho,theta);
+ try {
+ melinePt->initTracking(I, ip1, ip2, rho, theta);
meline.push_back(melinePt);
- // nbFeature.push_back((unsigned int) melinePt->getMeList().size());
- // nbFeatureTotal += nbFeature.back();
- }
- catch(...)
- {
- //vpTRACE("the line can't be initialized");
+ // nbFeature.push_back((unsigned int)
+ // melinePt->getMeList().size()); nbFeatureTotal +=
+ // nbFeature.back();
+ } catch (...) {
+ // vpTRACE("the line can't be initialized");
delete melinePt;
isvisible = false;
return false;
}
}
- }
- else{
+ } else {
isvisible = false;
-// return false;
+ // return false;
}
}
-// trackMovingEdge(I,cMo) ;
+ // trackMovingEdge(I,cMo);
return true;
}
-
-
/*!
Track the moving edges in the image.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
+void vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/)
{
- if (isvisible)
- {
-// p1->changeFrame(cMo) ;
-// p2->changeFrame(cMo) ;
-//
-// p1->projection() ;
-// p2->projection() ;
-//
-// vpImagePoint ip1, ip2;
-//
-// vpMeterPixelConversion::convertPoint(*cam,p1->get_x(),p1->get_y(),ip1) ;
-// vpMeterPixelConversion::convertPoint(*cam,p2->get_x(),p2->get_y(),ip2) ;
-//
-// int marge = /*10*/5; //ou 5 normalement
-// if (ip1.get_j()<ip2.get_j()) { meline->jmin = ip1.get_j()-marge ; meline->jmax = ip2.get_j()+marge ; }
-// else{ meline->jmin = ip2.get_j()-marge ; meline->jmax = ip1.get_j()+marge ; }
-// if (ip1.get_i()<ip2.get_i()) { meline->imin = ip1.get_i()-marge ; meline->imax = ip2.get_i()+marge ; }
-// else{ meline->imin = ip2.get_i()-marge ; meline->imax = ip1.get_i()+marge ; }
-
- try
- {
+ if (isvisible) {
+ // p1->changeFrame(cMo);
+ // p2->changeFrame(cMo);
+ //
+ // p1->projection();
+ // p2->projection();
+ //
+ // vpImagePoint ip1, ip2;
+ //
+ // vpMeterPixelConversion::convertPoint(*cam,p1->get_x(),p1->get_y(),ip1);
+ // vpMeterPixelConversion::convertPoint(*cam,p2->get_x(),p2->get_y(),ip2);
+ //
+ // int marge = /*10*/5; //ou 5 normalement
+ // if (ip1.get_j()<ip2.get_j()) { meline->jmin = ip1.get_j()-marge ;
+ // meline->jmax = ip2.get_j()+marge ; } else{ meline->jmin =
+ // ip2.get_j()-marge ; meline->jmax = ip1.get_j()+marge ; } if
+ // (ip1.get_i()<ip2.get_i()) { meline->imin = ip1.get_i()-marge ;
+ // meline->imax = ip2.get_i()+marge ; } else{ meline->imin =
+ // ip2.get_i()-marge ; meline->imax = ip1.get_i()+marge ; }
+
+ try {
+ nbFeature.clear();
nbFeatureTotal = 0;
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- meline[i]->track(I) ;
- nbFeature.push_back((unsigned int) meline[i]->getMeList().size());
- nbFeatureTotal += (unsigned int) meline[i]->getMeList().size();
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ meline[i]->track(I);
+ nbFeature.push_back((unsigned int)meline[i]->getMeList().size());
+ nbFeatureTotal += (unsigned int)meline[i]->getMeList().size();
}
- }
- catch(...)
- {
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- if (meline[i] != NULL) delete meline[i] ;
+ } catch (...) {
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ if (meline[i] != NULL)
+ delete meline[i];
}
nbFeature.clear();
@@ -472,39 +475,37 @@ vpMbtDistanceLine::trackMovingEdge(const vpImage<unsigned char> &I, const vpHomo
}
}
-
/*!
Update the moving edges internal parameters.
-
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- if(isvisible){
+ if (isvisible) {
p1->changeFrame(cMo);
p2->changeFrame(cMo);
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ if (poly.getClipping() > 3) // Contains at least one FOV constraint
cam.computeFov(I.getWidth(), I.getHeight());
poly.computePolygonClipped(cam);
- if(poly.polyClipped.size() == 2){ //Les points sont visibles.
+ if (poly.polyClipped.size() == 2) { // Les points sont visibles.
std::vector<std::pair<vpPoint, vpPoint> > linesLst;
- if(useScanLine){
- hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst);
- }
- else{
- linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ if (useScanLine) {
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst);
+ } else {
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
}
- if(linesLst.size() != meline.size() || linesLst.size() == 0){
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- if (meline[i] != NULL) delete meline[i] ;
+ if (linesLst.size() != meline.size() || linesLst.size() == 0) {
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ if (meline[i] != NULL)
+ delete meline[i];
}
meline.clear();
@@ -512,55 +513,74 @@ vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHom
nbFeatureTotal = 0;
isvisible = false;
Reinit = true;
- }
- else{
-
- // To have the exact same pose values as the old version (angle or ogre visibility test only), points should be reorganised when using scanline algorithm.
-// if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].first.get_X())) > sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() - linesLst[0].second.get_X())))
-// {
-// std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
-// for(int i = linesLst.size()-1 ; i >= 0 ; i--)
-// linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
-// linesLst = linesLstTmp;
-// }
+ } else {
+
+ // To have the exact same pose values as the old version (angle or
+ // ogre visibility test only), points should be reorganised when using
+ // scanline algorithm.
+ // if(sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() -
+ // linesLst[0].first.get_X())) >
+ // sqrt(vpMath::sqr(poly.polyClipped[0].first.get_X() -
+ // linesLst[0].second.get_X())))
+ // {
+ // std::vector<std::pair<vpPoint, vpPoint> > linesLstTmp;
+ // for(int i = linesLst.size()-1 ; i >= 0 ; i--)
+ // linesLstTmp.push_back(std::make_pair(linesLst[i].second,linesLst[i].first));
+ // linesLst = linesLstTmp;
+ // }
line->changeFrame(cMo);
line->projection();
- double rho,theta;
- //rho theta uv
- vpMeterPixelConversion::convertLine(cam,line->getRho(),line->getTheta(),rho,theta);
-
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < -M_PI) { theta += M_PI ; }
+ double rho, theta;
+ // rho theta uv
+ vpMeterPixelConversion::convertLine(cam, line->getRho(), line->getTheta(), rho, theta);
- if (theta < -M_PI/2.0) theta = -theta - 3*M_PI/2.0;
- else theta = M_PI/2.0 - theta;
+ while (theta > M_PI) {
+ theta -= M_PI;
+ }
+ while (theta < -M_PI) {
+ theta += M_PI;
+ }
+ if (theta < -M_PI / 2.0)
+ theta = -theta - 3 * M_PI / 2.0;
+ else
+ theta = M_PI / 2.0 - theta;
- try
- {
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ try {
+ for (unsigned int i = 0; i < linesLst.size(); i++) {
vpImagePoint ip1, ip2;
linesLst[i].first.project();
linesLst[i].second.project();
- vpMeterPixelConversion::convertPoint(cam,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(cam,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
-
- int marge = /*10*/5; //ou 5 normalement
- if (ip1.get_j()<ip2.get_j()) { meline[i]->jmin = (int)ip1.get_j()-marge ; meline[i]->jmax = (int)ip2.get_j()+marge ; } else{ meline[i]->jmin = (int)ip2.get_j()-marge ; meline[i]->jmax = (int)ip1.get_j()+marge ; }
- if (ip1.get_i()<ip2.get_i()) { meline[i]->imin = (int)ip1.get_i()-marge ; meline[i]->imax = (int)ip2.get_i()+marge ; } else{ meline[i]->imin = (int)ip2.get_i()-marge ; meline[i]->imax = (int)ip1.get_i()+marge ; }
-
- meline[i]->updateParameters(I,ip1,ip2,rho,theta) ;
- nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
- nbFeatureTotal += nbFeature[i];
+ vpMeterPixelConversion::convertPoint(cam, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
+ vpMeterPixelConversion::convertPoint(cam, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
+
+ int marge = /*10*/ 5; // ou 5 normalement
+ if (ip1.get_j() < ip2.get_j()) {
+ meline[i]->jmin = (int)ip1.get_j() - marge;
+ meline[i]->jmax = (int)ip2.get_j() + marge;
+ } else {
+ meline[i]->jmin = (int)ip2.get_j() - marge;
+ meline[i]->jmax = (int)ip1.get_j() + marge;
+ }
+ if (ip1.get_i() < ip2.get_i()) {
+ meline[i]->imin = (int)ip1.get_i() - marge;
+ meline[i]->imax = (int)ip2.get_i() + marge;
+ } else {
+ meline[i]->imin = (int)ip2.get_i() - marge;
+ meline[i]->imax = (int)ip1.get_i() + marge;
+ }
+
+ meline[i]->updateParameters(I, ip1, ip2, rho, theta);
+ nbFeature[i] = (unsigned int)meline[i]->getMeList().size();
+ nbFeatureTotal += nbFeature[i];
}
- }
- catch(...)
- {
- for(unsigned int j = 0 ; j < meline.size() ; j++){
- if (meline[j] != NULL) delete meline[j] ;
+ } catch (...) {
+ for (unsigned int j = 0; j < meline.size(); j++) {
+ if (meline[j] != NULL)
+ delete meline[j];
}
meline.clear();
@@ -570,10 +590,10 @@ vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHom
Reinit = true;
}
}
- }
- else{
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- if (meline[i] != NULL) delete meline[i] ;
+ } else {
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ if (meline[i] != NULL)
+ delete meline[i];
}
nbFeature.clear();
meline.clear();
@@ -583,33 +603,32 @@ vpMbtDistanceLine::updateMovingEdge(const vpImage<unsigned char> &I, const vpHom
}
}
-
/*!
Reinitialize the line if it is required.
-
- A line is reinitialized if the 2D line do not match enough with the projected 3D line.
-
+
+ A line is reinitialized if the 2D line do not match enough with the
+ projected 3D line.
+
\param I : the image.
\param cMo : The pose of the camera.
*/
-void
-vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo)
{
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- if (meline[i] != NULL) delete meline[i] ;
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ if (meline[i] != NULL)
+ delete meline[i];
}
nbFeature.clear();
meline.clear();
nbFeatureTotal = 0;
- if (initMovingEdge(I,cMo) == false)
+ if (initMovingEdge(I, cMo) == false)
Reinit = true;
Reinit = false;
}
-
/*!
Display the line. The 3D line is projected into the image.
@@ -618,53 +637,52 @@ vpMbtDistanceLine::reinitMovingEdge(const vpImage<unsigned char> &I, const vpHom
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the line.
- \param displayFullModel : If true, the line is displayed even if it is not visible.
+ \param displayFullModel : If true, the line is displayed even if it is not
+ visible.
*/
-void
-vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col, const unsigned int thickness, const bool displayFullModel)
+void vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedLine) || displayFullModel){
+ if ((isvisible && isTrackedLine) || displayFullModel) {
p1->changeFrame(cMo);
p2->changeFrame(cMo);
vpImagePoint ip1, ip2;
vpCameraParameters c = camera;
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ if (poly.getClipping() > 3) // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
-
+
poly.computePolygonClipped(c);
-
- if( poly.polyClipped.size() == 2 &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+
+ if (poly.polyClipped.size() == 2 &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
std::vector<std::pair<vpPoint, vpPoint> > linesLst;
- if(useScanLine && !displayFullModel){
- hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst,true);
- }
- else{
- linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ if (useScanLine && !displayFullModel) {
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
+ } else {
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
}
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ 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);
+ vpDisplay::displayLine(I, ip1, ip2, col, thickness);
}
}
}
}
-
/*!
Display the line. The 3D line is projected into the image.
@@ -673,70 +691,70 @@ vpMbtDistanceLine::display(const vpImage<unsigned char> &I, const vpHomogeneousM
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the line.
- \param displayFullModel : If true, the line is displayed even if it is not visible.
+ \param displayFullModel : If true, the line is displayed even if it is not
+ visible.
*/
-void
-vpMbtDistanceLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &camera, const vpColor col,
- const unsigned int thickness, const bool displayFullModel)
+void vpMbtDistanceLine::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (isvisible && isTrackedLine) || displayFullModel){
+ if ((isvisible && isTrackedLine) || displayFullModel) {
p1->changeFrame(cMo);
p2->changeFrame(cMo);
vpImagePoint ip1, ip2;
vpCameraParameters c = camera;
- if(poly.getClipping() > 3) // Contains at least one FOV constraint
+ if (poly.getClipping() > 3) // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
poly.computePolygonClipped(c);
- if( poly.polyClipped.size() == 2 &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
- ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+ if (poly.polyClipped.size() == 2 &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((poly.polyClipped[1].second & poly.polyClipped[0].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
std::vector<std::pair<vpPoint, vpPoint> > linesLst;
- if(useScanLine && !displayFullModel){
- hiddenface->computeScanLineQuery(poly.polyClipped[0].first,poly.polyClipped[1].first,linesLst,true);
- }
- else{
- linesLst.push_back(std::make_pair(poly.polyClipped[0].first,poly.polyClipped[1].first));
+ if (useScanLine && !displayFullModel) {
+ hiddenface->computeScanLineQuery(poly.polyClipped[0].first, poly.polyClipped[1].first, linesLst, true);
+ } else {
+ linesLst.push_back(std::make_pair(poly.polyClipped[0].first, poly.polyClipped[1].first));
}
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ 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);
+ vpDisplay::displayLine(I, ip1, ip2, col, thickness);
}
}
}
}
-
/*!
- Enable to display the points along the line with a color corresponding to their state.
-
+ Enable to display the points along the line with a color corresponding to
+ their state.
+
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If blue : The point is removed because of the robust method in the virtual visual servoing.
-
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If blue : The point is removed because of the robust method in the
+ virtual visual servoing.
+
\param I : The image.
*/
-void
-vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
+void vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
{
- for(unsigned int i = 0 ; i < meline.size() ; i++)
- if (meline[i] != NULL)
- {
+ for (unsigned int i = 0; i < meline.size(); i++)
+ if (meline[i] != NULL) {
meline[i]->display(I);
}
}
@@ -744,104 +762,121 @@ vpMbtDistanceLine::displayMovingEdges(const vpImage<unsigned char> &I)
/*!
Initialize the size of the interaction matrix and the error vector.
*/
-void
-vpMbtDistanceLine::initInteractionMatrixError()
+void vpMbtDistanceLine::initInteractionMatrixError()
{
- if (isvisible == true)
- {
- L.resize(nbFeatureTotal,6) ;
- error.resize(nbFeatureTotal) ;
- }
- else{
- for(unsigned int i = 0 ; i < meline.size() ; i++)
+ if (isvisible == true) {
+ L.resize(nbFeatureTotal, 6);
+ error.resize(nbFeatureTotal);
+ } else {
+ for (unsigned int i = 0; i < meline.size(); i++) {
nbFeature[i] = 0;
- nbFeatureTotal = 0 ;
+ // To be consistent with nbFeature[i] = 0
+ std::list<vpMeSite> &me_site_list = meline[i]->getMeList();
+ me_site_list.clear();
+ }
+ nbFeatureTotal = 0;
}
}
/*!
- Compute the interaction matrix and the error vector corresponding to the line.
+ 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)
- {
- // feature projection
- line->changeFrame(cMo) ;
- line->projection() ;
+ if (isvisible) {
+ try {
+ // feature projection
+ line->changeFrame(cMo);
+ line->projection();
- vpFeatureBuilder::create(featureline,*line) ;
+ vpFeatureBuilder::create(featureline, *line);
- double rho = featureline.getRho() ;
- double theta = featureline.getTheta() ;
+ double rho = featureline.getRho();
+ double theta = featureline.getTheta();
- double co = cos(theta);
- double si = sin(theta);
+ double co = cos(theta);
+ double si = sin(theta);
- double mx = 1.0/cam.get_px() ;
- double my = 1.0/cam.get_py() ;
- double xc = cam.get_u0() ;
- double yc = cam.get_v0() ;
+ double mx = 1.0 / cam.get_px();
+ double my = 1.0 / cam.get_py();
+ double xc = cam.get_u0();
+ double yc = cam.get_v0();
- double alpha_ ;
- vpMatrix H ;
- H = featureline.interaction() ;
+ double alpha_;
+ vpMatrix H = featureline.interaction();
- double x,y ;
- vpMeSite p ;
- unsigned int j =0 ;
+ double x, y;
+ unsigned int j = 0;
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- for(std::list<vpMeSite>::const_iterator it=meline[i]->getMeList().begin(); it!=meline[i]->getMeList().end(); ++it){
- x = (double)it->j ;
- y = (double)it->i ;
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
+ it != meline[i]->getMeList().end(); ++it) {
+ x = (double)it->j;
+ y = (double)it->i;
- x = (x-xc)*mx ;
- y = (y-yc)*my ;
+ x = (x - xc) * mx;
+ y = (y - yc) * my;
- alpha_ = x*si - y*co;
+ alpha_ = x * si - y * co;
- double *Lrho = H[0] ;
- double *Ltheta = H[1] ;
- // Calculate interaction matrix for a distance
- for (unsigned int k=0 ; k < 6 ; k++)
- {
- L[j][k] = (Lrho[k] + alpha_*Ltheta[k]);
+ double *Lrho = H[0];
+ double *Ltheta = H[1];
+ // Calculate interaction matrix for a distance
+ for (unsigned int k = 0; k < 6; k++) {
+ L[j][k] = (Lrho[k] + alpha_ * Ltheta[k]);
+ }
+ error[j] = rho - (x * co + y * si);
+ j++;
+ }
+ }
+ } 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;
+
+ unsigned int j = 0;
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin();
+ it != meline[i]->getMeList().end(); ++it) {
+ for (unsigned int k = 0; k < 6; k++) {
+ L[j][k] = 0.0;
+ }
+
+ error[j] = 0.0;
+ j++;
}
- error[j] = rho - ( x*co + y*si) ;
- j++;
}
}
}
}
/*!
- Test wether the line is close to the border of the image (at a given threshold)
-
+ Test wether the line is close to the border of the image (at a given
+ threshold)
+
\param I : the input image (to know its size)
- \param threshold : the threshold in pixel
+ \param threshold : the threshold in pixel
\return true if the line is near the border of the image
*/
-bool
-vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char>& I, const unsigned int threshold)
+bool vpMbtDistanceLine::closeToImageBorder(const vpImage<unsigned char> &I, const unsigned int threshold)
{
- if(threshold > I.getWidth() || threshold > I.getHeight()){
+ if (threshold > I.getWidth() || threshold > I.getHeight()) {
return true;
}
- if (isvisible){
+ if (isvisible) {
- for(unsigned int i = 0 ; i < meline.size() ; i++){
- for(std::list<vpMeSite>::const_iterator it=meline[i]->getMeList().begin(); it!=meline[i]->getMeList().end(); ++it){
- int i_ = it->i ;
- int j_ = it->j ;
+ for (unsigned int i = 0; i < meline.size(); i++) {
+ for (std::list<vpMeSite>::const_iterator it = meline[i]->getMeList().begin(); it != meline[i]->getMeList().end();
+ ++it) {
+ int i_ = it->i;
+ int j_ = it->j;
- if(i_ < 0 || j_ < 0){ //out of image.
+ if (i_ < 0 || j_ < 0) { // out of image.
return true;
}
- if( ((unsigned int)i_ > (I.getHeight()- threshold) ) || (unsigned int)i_ < threshold ||
- ((unsigned int)j_ > (I.getWidth ()- threshold) ) || (unsigned int)j_ < threshold ) {
+ 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 cee4a0c..e6b5804 100644
--- a/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtMeEllipse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,23 +40,21 @@
#include <visp3/mbt/vpMbtMeEllipse.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/core/vpRobust.h>
-#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/me/vpMe.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <algorithm> // std::min
-
+#include <algorithm> // (std::min)
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
/*!
Basic constructor that calls the constructor of the class vpMeTracker.
*/
vpMbtMeEllipse::vpMbtMeEllipse()
- : iPc(), a(0.), b(0.), e(0.),
- ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
+ : iPc(), a(0.), b(0.), e(0.), ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
{
}
@@ -63,17 +62,16 @@ vpMbtMeEllipse::vpMbtMeEllipse()
Copy constructor.
*/
vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse)
- : vpMeTracker(meellipse), iPc(), a(0.), b(0.), e(0.),
- ce(0.), se(0.), mu11(0.), mu20(0.), mu02(0.), thresholdWeight(0.), expecteddensity(0.)
+ : 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.)
{
- iPc = meellipse.iPc;
a = meellipse.a;
b = meellipse.b;
e = meellipse.e;
ce = meellipse.ce;
se = meellipse.se;
-
+
mu11 = meellipse.mu11;
mu20 = meellipse.mu20;
mu02 = meellipse.mu02;
@@ -84,53 +82,51 @@ vpMbtMeEllipse::vpMbtMeEllipse(const vpMbtMeEllipse &meellipse)
/*!
Basic destructor.
*/
-vpMbtMeEllipse::~vpMbtMeEllipse()
-{
- list.clear();
-}
+vpMbtMeEllipse::~vpMbtMeEllipse() { list.clear(); }
/*!
Compute the projection error of the ellipse.
- Compare the gradient direction around samples of the ellipse to the normal of the tangent of the considered sample.
- Error is expressed in radians between 0 and M_PI/2.0;
+ Compare the gradient direction around samples of the ellipse to the normal
+ of the tangent of the considered sample. Error is expressed in radians
+ between 0 and M_PI/2.0;
\param _I : Image in which the line appears.
\param _sumErrorRad : sum of the error per feature.
\param _nbFeatures : Number of features used to compute _sumErrorRad.
*/
-void
-vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures)
+void vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad,
+ unsigned int &_nbFeatures)
{
_sumErrorRad = 0;
_nbFeatures = 0;
-// vpMatrix filterX(3,3);
-// filterX[0][0] = -1;
-// filterX[1][0] = -2;
-// filterX[2][0] = -1;
+ // vpMatrix filterX(3,3);
+ // filterX[0][0] = -1;
+ // filterX[1][0] = -2;
+ // filterX[2][0] = -1;
-// filterX[0][1] = 0;
-// filterX[1][1] = 0;
-// filterX[2][1] = 0;
+ // filterX[0][1] = 0;
+ // filterX[1][1] = 0;
+ // filterX[2][1] = 0;
-// filterX[0][2] = 1;
-// filterX[1][2] = 2;
-// filterX[2][2] = 1;
+ // filterX[0][2] = 1;
+ // filterX[1][2] = 2;
+ // filterX[2][2] = 1;
-// vpMatrix filterY(3,3);
-// filterY[0][0] = -1;
-// filterY[0][1] = -2;
-// filterY[0][2] = -1;
+ // vpMatrix filterY(3,3);
+ // filterY[0][0] = -1;
+ // filterY[0][1] = -2;
+ // filterY[0][2] = -1;
-// filterY[1][0] = 0;
-// filterY[1][1] = 0;
-// filterY[1][2] = 0;
+ // filterY[1][0] = 0;
+ // filterY[1][1] = 0;
+ // filterY[1][2] = 0;
-// filterY[2][0] = 1;
-// filterY[2][1] = 2;
-// filterY[2][2] = 1;
+ // filterY[2][0] = 1;
+ // filterY[2][1] = 2;
+ // filterY[2][2] = 1;
- vpMatrix filterX(5,5);
+ vpMatrix filterX(5, 5);
filterX[0][0] = -1;
filterX[1][0] = -4;
filterX[2][0] = -6;
@@ -161,7 +157,7 @@ vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double
filterX[3][4] = 4;
filterX[4][4] = 1;
- vpMatrix filterY(5,5);
+ vpMatrix filterY(5, 5);
filterY[0][0] = -1;
filterY[0][1] = -4;
filterY[0][2] = -6;
@@ -193,23 +189,26 @@ vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double
filterY[4][4] = 1;
double offset = std::floor(filterX.getRows() / 2.0f);
-// std::cout << "offset=" << offset << std::endl;
- int height = (int) _I.getHeight() ;
- int width = (int) _I.getWidth() ;
+ // std::cout << "offset=" << offset << std::endl;
+ int height = (int)_I.getHeight();
+ int width = (int)_I.getWidth();
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
double iSite = it->ifloat;
double jSite = it->jfloat;
- if(!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height, width)) { // Check if necessary
+ if (!outOfImage(vpMath::round(iSite), vpMath::round(jSite), 0, height,
+ width)) { // Check if necessary
// The tangent angle to the ellipse at a site
- double theta = atan( (-mu02*jSite + mu02*iPc.get_j() + mu11*iSite - mu11*iPc.get_i())
- / (mu20*iSite - mu11*jSite + mu11*iPc.get_j() - mu20*iPc.get_i()))
- - M_PI/2;
+ double theta = atan((-mu02 * jSite + mu02 * iPc.get_j() + mu11 * iSite - mu11 * iPc.get_i()) /
+ (mu20 * iSite - mu11 * jSite + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
+ M_PI / 2;
double deltaNormalized = theta;
- while (deltaNormalized<0) deltaNormalized += M_PI;
- while (deltaNormalized>M_PI) deltaNormalized -= M_PI;
+ while (deltaNormalized < 0)
+ deltaNormalized += M_PI;
+ while (deltaNormalized > M_PI)
+ deltaNormalized -= M_PI;
vpColVector vecSite(2);
vecSite[0] = cos(deltaNormalized);
@@ -219,49 +218,61 @@ vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double
double gradientX = 0;
double gradientY = 0;
- for(unsigned int i = 0; i<filterX.getRows() ; i++){
- double iImg = iSite + (i - offset);
- for (unsigned int j = 0; j< filterX.getCols(); j++){
- double jImg = jSite + (j-offset);
+ for (unsigned int i = 0; i < filterX.getRows(); i++) {
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j < filterX.getCols(); j++) {
+ double jImg = jSite + (j - offset);
- if(iImg < 0) iImg = 0.0;
- if(jImg < 0) jImg = 0.0;
+ if (iImg < 0)
+ iImg = 0.0;
+ if (jImg < 0)
+ jImg = 0.0;
- if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
- if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+ if (iImg > _I.getHeight() - 1)
+ iImg = _I.getHeight() - 1;
+ if (jImg > _I.getWidth() - 1)
+ jImg = _I.getWidth() - 1;
- gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
}
}
- for(unsigned int i = 0; i<filterY.getRows() ; i++){
- double iImg = iSite + (i - offset);
- for (unsigned int j = 0; j< filterY.getCols(); j++){
- double jImg = jSite + (j-offset);
+ for (unsigned int i = 0; i < filterY.getRows(); i++) {
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j < filterY.getCols(); j++) {
+ double jImg = jSite + (j - offset);
- if(iImg < 0) iImg = 0.0;
- if(jImg < 0) jImg = 0.0;
+ if (iImg < 0)
+ iImg = 0.0;
+ if (jImg < 0)
+ jImg = 0.0;
- if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
- if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+ if (iImg > _I.getHeight() - 1)
+ iImg = _I.getHeight() - 1;
+ if (jImg > _I.getWidth() - 1)
+ jImg = _I.getWidth() - 1;
- gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
}
}
- double angle = atan2(gradientY,gradientX);
- while (angle<0) angle += M_PI;
- while (angle>M_PI) angle -= M_PI;
+ double angle = atan2(gradientY, gradientX);
+ while (angle < 0)
+ angle += M_PI;
+ while (angle > M_PI)
+ angle -= M_PI;
-// if(std::fabs(deltaNormalized-angle) > M_PI / 2)
-// {
-// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI / 2;
-// } else {
-// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
-// }
+ // if(std::fabs(deltaNormalized-angle) > M_PI / 2)
+ // {
+ // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI
+ // / 2;
+ // } else {
+ // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
+ // }
-// double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
-// double angle2 = sqrt(vpMath::sqr(deltaNormalized- (angle-M_PI)));
+ // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
+ // double angle2 = sqrt(vpMath::sqr(deltaNormalized-
+ // (angle-M_PI)));
vpColVector vecGrad(2);
vecGrad[0] = cos(angle);
@@ -271,7 +282,7 @@ vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double
double angle1 = acos(vecSite * vecGrad);
double angle2 = acos(vecSite * (-vecGrad));
- _sumErrorRad += std::min(angle1,angle2);
+ _sumErrorRad += (std::min)(angle1, angle2);
_nbFeatures++;
}
@@ -285,106 +296,100 @@ vpMbtMeEllipse::computeProjectionError(const vpImage<unsigned char>& _I, double
\param I : Image in which the ellipse appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMbtMeEllipse::sample(const vpImage<unsigned char> & I)
+void vpMbtMeEllipse::sample(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
- int height = (int)I.getHeight() ;
- int width = (int)I.getWidth() ;
+ int height = (int)I.getHeight();
+ int width = (int)I.getWidth();
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "In vpMbtMeEllipse::sample: " ;
- std::cout << "function called with sample step = 0" ;
- //return fatalError ;
+ // if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
+ std::cout << "In vpMbtMeEllipse::sample: ";
+ std::cout << "function called with sample step = 0";
+ // return fatalError;
}
// Approximation of the circumference of an ellipse:
// [Ramanujan, S., "Modular Equations and Approximations to ,"
// Quart. J. Pure. Appl. Math., vol. 45 (1913-1914), pp. 350-372]
- double t = (a-b)/(a+b);
- double circumference = M_PI*(a+b)*(1 + 3*vpMath::sqr(t)/(10 + sqrt(4 - 3*vpMath::sqr(t))));
+ double t = (a - b) / (a + b);
+ double circumference = M_PI * (a + b) * (1 + 3 * vpMath::sqr(t) / (10 + sqrt(4 - 3 * vpMath::sqr(t))));
int nb_points_to_track = (int)(circumference / me->getSampleStep());
- double incr = 2*M_PI/nb_points_to_track;
+ double incr = 2 * M_PI / nb_points_to_track;
- expecteddensity = 0;//nb_points_to_track;
+ expecteddensity = 0; // nb_points_to_track;
// Delete old list
list.clear();
// sample positions
- double k = 0 ;
- for (int pt=0; pt < nb_points_to_track; pt++)
- {
- double j = a *cos(k) ; // equation of an ellipse
- double i = b *sin(k) ; // equation of an ellipse
+ double k = 0;
+ for (int pt = 0; pt < nb_points_to_track; pt++) {
+ double j = a * cos(k); // equation of an ellipse
+ double i = b * sin(k); // equation of an ellipse
- double iP_j = iPc.get_j() + ce *j - se *i;
- double iP_i = iPc.get_i() + se *j + ce *i;
+ double iP_j = iPc.get_j() + ce * j - se * i;
+ double iP_i = iPc.get_i() + se * j + ce * i;
- //vpColor col = vpColor::red ;
- //vpDisplay::displayCross(I, vpImagePoint(iP_i, iP_j), 5, col) ; //debug only
+ // vpColor col = vpColor::red;
+ // vpDisplay::displayCross(I, vpImagePoint(iP_i, iP_j), 5, col) ; //debug
+ // only
// If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(iP_i), vpMath::round(iP_j), 0, height, width))
- {
+ if (!outOfImage(vpMath::round(iP_i), vpMath::round(iP_j), 0, height, width)) {
// The tangent angle to the ellipse at a site
- double theta = atan( (-mu02*iP_j + mu02*iPc.get_j() + mu11*iP_i - mu11*iPc.get_i())
- / (mu20*iP_i - mu11*iP_j + mu11*iPc.get_j() - mu20*iPc.get_i()))
- - M_PI/2;
+ double theta = atan((-mu02 * iP_j + mu02 * iPc.get_j() + mu11 * iP_i - mu11 * iPc.get_i()) /
+ (mu20 * iP_i - mu11 * iP_j + mu11 * iPc.get_j() - mu20 * iPc.get_i())) -
+ M_PI / 2;
- vpMeSite pix ;
- pix.init((int)iP_i, (int)iP_j, theta) ;
- pix.setDisplay(selectDisplay) ;
+ vpMeSite pix;
+ pix.init((int)iP_i, (int)iP_j, theta);
+ pix.setDisplay(selectDisplay);
pix.setState(vpMeSite::NO_SUPPRESSION);
list.push_back(pix);
- expecteddensity ++;
+ expecteddensity++;
}
- k += incr ;
-
+ k += incr;
}
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
-
/*!
-
+
Resample the ellipse if the number of sample is less than 90% of the
expected value.
-
- \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
- and the parameter which indicates the number of degrees between
- two points (vpMe::sample_step).
+
+ \note The expected value is computed thanks to the difference between the
+ smallest and the biggest \f$ \alpha \f$ angles and the parameter which
+ indicates the number of degrees between two points (vpMe::sample_step).
\param I : Image in which the ellipse appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
+void vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
- unsigned int n = numberOfSignal() ;
- if ((double)n<0.9*expecteddensity){
- sample(I) ;
- vpMeTracker::track(I) ;
+ unsigned int n = numberOfSignal();
+ if ((double)n < 0.9 * expecteddensity) {
+ sample(I);
+ vpMeTracker::track(I);
}
}
@@ -393,35 +398,34 @@ vpMbtMeEllipse::reSample(const vpImage<unsigned char> &I)
\note The \f$ \theta \f$ angle is useful during the tracking part.
*/
-void
-vpMbtMeEllipse::updateTheta()
+void vpMbtMeEllipse::updateTheta()
{
vpMeSite p_me;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
vpImagePoint iP;
iP.set_i(p_me.ifloat);
iP.set_j(p_me.jfloat);
// The tangent angle to the ellipse at a site
- 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;
+ 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 ;
+ p_me.alpha = theta;
*it = p_me;
}
}
/*!
- Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
+ Suppress the vpMeSite which are no more detected as point which belongs to
+ the ellipse edge.
*/
-void
-vpMbtMeEllipse::suppressPoints()
+void vpMbtMeEllipse::suppressPoints()
{
// Loop through list of sites to track
- for(std::list<vpMeSite>::iterator itList=list.begin(); itList!=list.end();){
- vpMeSite s = *itList;//current reference pixel
+ for (std::list<vpMeSite>::iterator itList = list.begin(); itList != list.end();) {
+ vpMeSite s = *itList; // current reference pixel
if (s.getState() != vpMeSite::NO_SUPPRESSION)
itList = list.erase(itList);
else
@@ -438,15 +442,13 @@ vpMbtMeEllipse::suppressPoints()
\param I : Image in which the ellipse appears.
\param col : Color of the displayed ellipse.
*/
-void
-vpMbtMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
+void vpMbtMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
{
vpDisplay::displayEllipse(I, iPc, mu20, mu11, mu02, true, col);
}
-void
-vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic,
- double mu20_p, double mu11_p, double mu02_p)
+void vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p,
+ double mu02_p)
{
iPc = ic;
mu20 = mu20_p;
@@ -455,13 +457,12 @@ vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint
if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
+ double val_p = sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p) / 2);
+ b = sqrt((mu20_p + mu02_p - val_p) / 2);
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- }
- else {
+ e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
+ } else {
a = sqrt(mu20_p);
b = sqrt(mu02_p);
e = 0.;
@@ -472,16 +473,14 @@ vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint
ce = cos(e);
se = sin(e);
- sample(I) ;
+ sample(I);
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
- try{
- track(I) ;
- }
- catch(vpException &exception)
- {
- throw(exception) ;
+ try {
+ track(I);
+ } catch (vpException &exception) {
+ throw(exception);
}
}
@@ -490,20 +489,17 @@ vpMbtMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint
\param I : Image in which the ellipse appears.
*/
-void
-vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
+void vpMbtMeEllipse::track(const vpImage<unsigned char> &I)
{
- try{
- vpMeTracker::track(I) ;
- }
- catch(vpException &exception)
- {
- throw(exception) ;
+ try {
+ vpMeTracker::track(I);
+ } catch (vpException &exception) {
+ throw(exception);
}
}
-void
-vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p, double mu11_p, double mu02_p)
+void vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ic, double mu20_p,
+ double mu11_p, double mu02_p)
{
iPc = ic;
mu20 = mu20_p;
@@ -512,13 +508,12 @@ vpMbtMeEllipse::updateParameters(const vpImage<unsigned char> &I, const vpImageP
if (std::fabs(mu11_p) > std::numeric_limits<double>::epsilon()) {
- double val_p = sqrt(vpMath::sqr(mu20_p-mu02_p) + 4*vpMath::sqr(mu11_p));
- a = sqrt((mu20_p + mu02_p + val_p)/2);
- b = sqrt((mu20_p + mu02_p - val_p)/2);
+ double val_p = sqrt(vpMath::sqr(mu20_p - mu02_p) + 4 * vpMath::sqr(mu11_p));
+ a = sqrt((mu20_p + mu02_p + val_p) / 2);
+ b = sqrt((mu20_p + mu02_p - val_p) / 2);
- e = (mu02_p - mu20_p + val_p)/(2*mu11_p);
- }
- else {
+ e = (mu02_p - mu20_p + val_p) / (2 * mu11_p);
+ } else {
a = sqrt(mu20_p);
b = sqrt(mu02_p);
e = 0.;
diff --git a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp
index d003294..5a5d680 100644
--- a/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtMeLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,46 +44,46 @@
\file vpMbtMeLine.cpp
\brief Make the complete tracking of an object by using its CAD model.
*/
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <algorithm> // std::min
+#include <algorithm> // (std::min)
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
-#include <visp3/mbt/vpMbtMeLine.h>
-#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbtMeLine.h>
//! Normalize an angle between -Pi and Pi
-static void
-normalizeAngle(double &delta)
+static void normalizeAngle(double &delta)
{
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < -M_PI) { delta += M_PI ; }
+ while (delta > M_PI) {
+ delta -= M_PI;
+ }
+ while (delta < -M_PI) {
+ delta += M_PI;
+ }
}
-
/*!
Basic constructor that calls the constructor of the class vpMeTracker.
*/
vpMbtMeLine::vpMbtMeLine()
- : rho(0.), theta(0.), theta_1(M_PI/2), delta(0.), delta_1(0), sign(1),
- a(0.), b(0.), c(0.), imin(0), imax(0), jmin(0), jmax(0),
- expecteddensity(0.)
+ : rho(0.), theta(0.), theta_1(M_PI / 2), delta(0.), delta_1(0), sign(1), a(0.), b(0.), c(0.), imin(0), imax(0),
+ jmin(0), jmax(0), expecteddensity(0.)
{
}
/*!
Basic destructor.
*/
-vpMbtMeLine::~vpMbtMeLine()
-{
- list.clear();
-}
+vpMbtMeLine::~vpMbtMeLine() { list.clear(); }
/*!
Initialization of the tracking. The line is defined thanks to the
- coordinates of two points corresponding to the extremities and its (\f$\rho \: \theta\f$) parameters.
-
- Remember the equation of a line : \f$ i \; cos(\theta) + j \; sin(\theta) - \rho = 0 \f$
+ coordinates of two points corresponding to the extremities and its (\f$\rho
+ \: \theta\f$) parameters.
+
+ Remember the equation of a line : \f$ i \; cos(\theta) + j \; sin(\theta) -
+ \rho = 0 \f$
\param I : Image in which the line appears.
\param ip1 : Coordinates of the first point.
@@ -90,77 +91,70 @@ vpMbtMeLine::~vpMbtMeLine()
\param rho_ : The \f$\rho\f$ parameter
\param theta_ : The \f$\theta\f$ parameter
*/
-void
-vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
- double rho_, double theta_)
+void vpMbtMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ double rho_, double theta_)
{
- vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
+ vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
- try
- {
+ try {
// 1. On fait ce qui concerne les droites (peut etre vide)
// Points extremites
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
-
+ PExt[0].ifloat = (float)ip1.get_i();
+ PExt[0].jfloat = (float)ip1.get_j();
+ PExt[1].ifloat = (float)ip2.get_i();
+ PExt[1].jfloat = (float)ip2.get_j();
+
this->rho = rho_;
this->theta = theta_;
theta_1 = theta_;
-
+
a = cos(theta);
b = sin(theta);
c = -rho;
-
- delta = - theta + M_PI/2.0;
+
+ delta = -theta + M_PI / 2.0;
normalizeAngle(delta);
delta_1 = delta;
- sample(I) ;
+ sample(I);
expecteddensity = (double)list.size();
vpMeTracker::track(I);
- }
- catch(...)
- {
+ } catch (...) {
throw; // throw the original exception
}
- vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
+ vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
}
-
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities of the line.
\param I : Image in which the line appears.
*/
-void
-vpMbtMeLine::sample(const vpImage<unsigned char>& I)
+void vpMbtMeLine::sample(const vpImage<unsigned char> &I)
{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
double n_sample;
- //if (me->getSampleStep==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- throw(vpTrackingException(vpTrackingException::fatalError,
- "Function vpMbtMeLine::sample() called with moving-edges sample step = 0")) ;
+ // if (me->getSampleStep==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
+ throw(vpTrackingException(vpTrackingException::fatalError, "Function vpMbtMeLine::sample() called with "
+ "moving-edges sample step = 0"));
}
// i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+ double diffsi = PExt[0].ifloat - PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat - PExt[1].jfloat;
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
+ double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
// number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
+ n_sample = length_p / (double)me->getSampleStep();
- double stepi = diffsi/(double)n_sample;
- double stepj = diffsj/(double)n_sample;
+ double stepi = diffsi / (double)n_sample;
+ double stepj = diffsj / (double)n_sample;
// Choose starting point
double is = PExt[1].ifloat;
@@ -173,75 +167,64 @@ vpMbtMeLine::sample(const vpImage<unsigned char>& I)
// line_p, starting at PSiteExt[0]
vpImagePoint ip;
- for(int i=0; i<=vpMath::round(n_sample); i++)
- {
+ for (int i = 0; i <= vpMath::round(n_sample); i++) {
// If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- vpMeSite pix ; //= list.value();
- pix.init((int)is, (int)js, delta, 0, sign) ;
-
+ if (!outOfImage(vpMath::round(is), vpMath::round(js), (int)(me->getRange() + me->getMaskSize() + 1), (int)rows,
+ (int)cols)) {
+ vpMeSite pix; //= list.value();
+ pix.init((int)is, (int)js, delta, 0, sign);
+
pix.track(I, me, false);
-
- pix.setDisplay(selectDisplay) ;
-
- if(vpDEBUG_ENABLE(3))
- {
- ip.set_i( is );
- ip.set_j( js );
- vpDisplay::displayCross(I, ip, 2, vpColor::blue);
+
+ pix.setDisplay(selectDisplay);
+
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i(is);
+ ip.set_j(js);
+ vpDisplay::displayCross(I, ip, 2, vpColor::blue);
}
list.push_back(pix);
}
is += stepi;
js += stepj;
-
}
vpCDEBUG(1) << "end vpMeLine::sample() : ";
- vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl ;
+ vpCDEBUG(1) << list.size() << " point inserted in the list " << std::endl;
}
-
/*!
Suppress the moving which belong no more to the line.
-
+
\param I : The image.
*/
-void
-vpMbtMeLine::suppressPoints(const vpImage<unsigned char> & I)
+void vpMbtMeLine::suppressPoints(const vpImage<unsigned char> &I)
{
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
+ vpMeSite s = *it; // current reference pixel
if (fabs(sin(theta)) > 0.9) // Vertical line management
{
- if ((s.i < imin) ||(s.i > imax))
- {
+ if ((s.i < imin) || (s.i > imax)) {
s.setState(vpMeSite::CONSTRAST);
}
}
else if (fabs(cos(theta)) > 0.9) // Horizontal line management
{
- if ((s.j < jmin) || (s.j > jmax))
- {
+ if ((s.j < jmin) || (s.j > jmax)) {
s.setState(vpMeSite::CONSTRAST);
}
}
- else
- {
- if ((s.i < imin) ||(s.i > imax) || (s.j < jmin) || (s.j > jmax) )
- {
+ else {
+ if ((s.i < imin) || (s.i > imax) || (s.j < jmin) || (s.j > jmax)) {
s.setState(vpMeSite::CONSTRAST);
}
-
}
- if (outOfImage(s.i, s.j, (int)(me->getRange()+me->getMaskSize()+1), (int)I.getHeight(), (int)I.getWidth()))
- {
+ if (outOfImage(s.i, s.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)I.getHeight(), (int)I.getWidth())) {
s.setState(vpMeSite::TOO_NEAR);
}
@@ -252,159 +235,153 @@ vpMbtMeLine::suppressPoints(const vpImage<unsigned char> & I)
}
}
-
/*!
- Seek along the line defined by its equation, the two extremities of the line. This function is useful in case of translation of the line.
-
+ Seek along the line defined by its equation, the two extremities of the line.
+ This function is useful in case of translation of the line.
+
\param I : Image in which the line appears.
*/
-void
-vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
+void vpMbtMeLine::seekExtremities(const vpImage<unsigned char> &I)
{
- vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
+ vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
double n_sample;
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- throw(vpTrackingException(vpTrackingException::fatalError,
- "Function called with sample step = 0")) ;
+ // if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
+ throw(vpTrackingException(vpTrackingException::fatalError, "Function called with sample step = 0"));
}
// i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+ double diffsi = PExt[0].ifloat - PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat - PExt[1].jfloat;
- double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
+ double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
- double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
- double dj = diffsj/sqrt(s) ;
+ double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
+ double dj = diffsj / sqrt(s);
double length_p = sqrt(s); /*(vpMath::sqr(diffsi)+vpMath::sqr(diffsj))*/
// number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
+ n_sample = length_p / (double)me->getSampleStep();
double sample_step = (double)me->getSampleStep();
- vpMeSite P ;
- P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
+ vpMeSite P;
+ P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
+ P.setDisplay(selectDisplay);
- unsigned int memory_range = me->getRange() ;
+ unsigned int memory_range = me->getRange();
me->setRange(1);
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat + di * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat + dj * sample_step;
+ P.j = (int)P.jfloat;
- if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
- {
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
- }
- else
- if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- P.track(I,me,false) ;
+ if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
+ if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::cyan);
+ } else if (!outOfImage(P.i, P.j, (int)(me->getRange() + me->getMaskSize() + 1), (int)rows, (int)cols)) {
+ P.track(I, me, false);
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
- }
- else
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
+ if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::green);
+ } else if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 10, vpColor::blue);
}
}
-
- P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
- if ((P.i < imin) ||(P.i > imax) || (P.j < jmin) || (P.j > jmax) )
- {
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j,5,vpColor::cyan) ;
+ P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
+ P.setDisplay(selectDisplay);
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat - di * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat - dj * sample_step;
+ P.j = (int)P.jfloat;
+
+ if ((P.i < imin) || (P.i > imax) || (P.j < jmin) || (P.j > jmax)) {
+ if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::cyan);
}
- else
- if(!outOfImage(P.i, P.j, (int)(me->getRange()+me->getMaskSize()+1), (int)rows, (int)cols))
- {
- P.track(I,me,false) ;
+ 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)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 5, vpColor::green) ;
- }
- else
- if (vpDEBUG_ENABLE(3)) vpDisplay::displayCross(I,P.i,P.j, 10, vpColor::blue) ;
+ if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 5, vpColor::green);
+ } else if (vpDEBUG_ENABLE(3))
+ vpDisplay::displayCross(I, P.i, P.j, 10, vpColor::blue);
}
}
-
+
me->setRange(memory_range);
-
- vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
- vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
-}
+ vpCDEBUG(1) << "end vpMeLine::sample() : ";
+ vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
+}
/*!
- Compute the projection error of the line. Compare the gradient direction around samples of the line to its direction.
- Error is expressed in radians between 0 and M_PI/2.0;
+ Compute the projection error of the line. Compare the gradient direction
+ around samples of the line to its direction. Error is expressed in radians
+ between 0 and M_PI/2.0;
\param _I : Image in which the line appears.
\param _sumErrorRad : sum of the error per feature.
\param _nbFeatures : Number of features used to compute _sumErrorRad.
*/
-void
-vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_sumErrorRad, unsigned int &_nbFeatures)
+void vpMbtMeLine::computeProjectionError(const vpImage<unsigned char> &_I, double &_sumErrorRad,
+ unsigned int &_nbFeatures)
{
_sumErrorRad = 0;
_nbFeatures = 0;
double deltaNormalized = theta;
unsigned int iter = 0;
- while (deltaNormalized<0) deltaNormalized += M_PI;
- while (deltaNormalized>M_PI) deltaNormalized -= M_PI;
+ while (deltaNormalized < 0)
+ deltaNormalized += M_PI;
+ while (deltaNormalized > M_PI)
+ deltaNormalized -= M_PI;
vpColVector vecLine(2);
vecLine[0] = cos(deltaNormalized);
vecLine[1] = sin(deltaNormalized);
vecLine.normalize();
-// vpMatrix filterX(3,3);
-// filterX[0][0] = -1;
-// filterX[1][0] = -2;
-// filterX[2][0] = -1;
+ // vpMatrix filterX(3,3);
+ // filterX[0][0] = -1;
+ // filterX[1][0] = -2;
+ // filterX[2][0] = -1;
-// filterX[0][1] = 0;
-// filterX[1][1] = 0;
-// filterX[2][1] = 0;
+ // filterX[0][1] = 0;
+ // filterX[1][1] = 0;
+ // filterX[2][1] = 0;
-// filterX[0][2] = 1;
-// filterX[1][2] = 2;
-// filterX[2][2] = 1;
+ // filterX[0][2] = 1;
+ // filterX[1][2] = 2;
+ // filterX[2][2] = 1;
-// vpMatrix filterY(3,3);
-// filterY[0][0] = -1;
-// filterY[0][1] = -2;
-// filterY[0][2] = -1;
+ // vpMatrix filterY(3,3);
+ // filterY[0][0] = -1;
+ // filterY[0][1] = -2;
+ // filterY[0][2] = -1;
-// filterY[1][0] = 0;
-// filterY[1][1] = 0;
-// filterY[1][2] = 0;
+ // filterY[1][0] = 0;
+ // filterY[1][1] = 0;
+ // filterY[1][2] = 0;
-// filterY[2][0] = 1;
-// filterY[2][1] = 2;
-// filterY[2][2] = 1;
+ // filterY[2][0] = 1;
+ // filterY[2][1] = 2;
+ // filterY[2][2] = 1;
- vpMatrix filterX(5,5);
+ vpMatrix filterX(5, 5);
filterX[0][0] = -1;
filterX[1][0] = -4;
filterX[2][0] = -6;
@@ -435,7 +412,7 @@ vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_s
filterX[3][4] = 4;
filterX[4][4] = 1;
- vpMatrix filterY(5,5);
+ vpMatrix filterY(5, 5);
filterY[0][0] = -1;
filterY[0][1] = -4;
filterY[0][2] = -6;
@@ -468,47 +445,57 @@ vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_s
double offset = std::floor(filterX.getRows() / 2.0f);
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- if(iter != 0 && iter+1 != list.size()){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
+ if (iter != 0 && iter + 1 != list.size()) {
double gradientX = 0;
double gradientY = 0;
double iSite = it->ifloat;
double jSite = it->jfloat;
- for(unsigned int i = 0; i<filterX.getRows() ; i++){
- double iImg = iSite + (i - offset);
- for (unsigned int j = 0; j< filterX.getCols(); j++){
- double jImg = jSite + (j-offset);
+ for (unsigned int i = 0; i < filterX.getRows(); i++) {
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j < filterX.getCols(); j++) {
+ double jImg = jSite + (j - offset);
- if(iImg < 0) iImg = 0.0;
- if(jImg < 0) jImg = 0.0;
+ if (iImg < 0)
+ iImg = 0.0;
+ if (jImg < 0)
+ jImg = 0.0;
- if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
- if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+ if (iImg > _I.getHeight() - 1)
+ iImg = _I.getHeight() - 1;
+ if (jImg > _I.getWidth() - 1)
+ jImg = _I.getWidth() - 1;
- gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ gradientX += filterX[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
}
}
- for(unsigned int i = 0; i<filterY.getRows() ; i++){
- double iImg = iSite + (i - offset);
- for (unsigned int j = 0; j< filterY.getCols(); j++){
- double jImg = jSite + (j-offset);
+ for (unsigned int i = 0; i < filterY.getRows(); i++) {
+ double iImg = iSite + (i - offset);
+ for (unsigned int j = 0; j < filterY.getCols(); j++) {
+ double jImg = jSite + (j - offset);
- if(iImg < 0) iImg = 0.0;
- if(jImg < 0) jImg = 0.0;
+ if (iImg < 0)
+ iImg = 0.0;
+ if (jImg < 0)
+ jImg = 0.0;
- if(iImg > _I.getHeight()-1) iImg = _I.getHeight()-1;
- if(jImg > _I.getWidth()-1) jImg = _I.getWidth()-1;
+ if (iImg > _I.getHeight() - 1)
+ iImg = _I.getHeight() - 1;
+ if (jImg > _I.getWidth() - 1)
+ jImg = _I.getWidth() - 1;
- gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
+ gradientY += filterY[i][j] * _I((unsigned int)iImg, (unsigned int)jImg);
}
}
- double angle = atan2(gradientX,gradientY);
- while (angle<0) angle += M_PI;
- while (angle>M_PI) angle -= M_PI;
+ double angle = atan2(gradientX, gradientY);
+ while (angle < 0)
+ angle += M_PI;
+ while (angle > M_PI)
+ angle -= M_PI;
vpColVector vecGrad(2);
vecGrad[0] = cos(angle);
@@ -518,17 +505,19 @@ vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_s
double angle1 = acos(vecLine * vecGrad);
double angle2 = acos(vecLine * (-vecGrad));
-// double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
-// double angle2 = sqrt(vpMath::sqr(deltaNormalized- (angle-M_PI)));
+ // double angle1 = sqrt(vpMath::sqr(deltaNormalized-angle));
+ // double angle2 = sqrt(vpMath::sqr(deltaNormalized-
+ // (angle-M_PI)));
- _sumErrorRad += std::min(angle1,angle2);
+ _sumErrorRad += (std::min)(angle1, angle2);
-// if(std::fabs(deltaNormalized-angle) > M_PI / 2)
-// {
-// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI / 2;
-// } else {
-// sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
-// }
+ // if(std::fabs(deltaNormalized-angle) > M_PI / 2)
+ // {
+ // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle)) - M_PI
+ // / 2;
+ // } else {
+ // sumErrorRad += sqrt(vpMath::sqr(deltaNormalized-angle));
+ // }
_nbFeatures++;
}
@@ -546,26 +535,23 @@ vpMbtMeLine::computeProjectionError(const vpImage<unsigned char>& _I, double &_s
\param I : Image in which the line appears.
*/
-void
-vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
+void vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
{
- unsigned int n = numberOfSignal() ;
+ unsigned int n = numberOfSignal();
- if ((double)n<0.5*expecteddensity && n > 0)
- {
+ if ((double)n < 0.5 * expecteddensity && n > 0) {
double delta_new = delta;
delta = delta_1;
- sample(I) ;
+ sample(I);
expecteddensity = (double)list.size();
delta = delta_new;
// 2. On appelle ce qui n'est pas specifique
{
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
}
}
-
/*!
Resample the line if the number of sample is less than 50% of the
expected value.
@@ -578,54 +564,52 @@ vpMbtMeLine::reSample(const vpImage<unsigned char> &I)
\param ip1 : The first extremity of the line.
\param ip2 : The second extremity of the line.
*/
-void
-vpMbtMeLine::reSample(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2)
+void vpMbtMeLine::reSample(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2)
{
size_t n = list.size();
- if ((double)n<0.5*expecteddensity /*&& n > 0*/) // n is always > 0
+ if ((double)n < 0.5 * expecteddensity /*&& n > 0*/) // n is always > 0
{
double delta_new = delta;
delta = delta_1;
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
- sample(I) ;
+ PExt[0].ifloat = (float)ip1.get_i();
+ PExt[0].jfloat = (float)ip1.get_j();
+ PExt[1].ifloat = (float)ip2.get_i();
+ PExt[1].jfloat = (float)ip2.get_j();
+ sample(I);
expecteddensity = (double)list.size();
delta = delta_new;
- vpMeTracker::track(I) ;
+ vpMeTracker::track(I);
}
}
/*!
Set the alpha value of the different vpMeSite to the value of delta.
*/
-void
-vpMbtMeLine::updateDelta()
+void vpMbtMeLine::updateDelta()
{
- vpMeSite p_me ;
+ vpMeSite p_me;
double diff = 0;
- //if(fabs(theta) == M_PI )
- if(std::fabs(std::fabs(theta) - M_PI) <= vpMath::maximum(std::fabs(theta), (double)M_PI)*std::numeric_limits<double>::epsilon() )
- {
- theta = 0 ;
+ // if(fabs(theta) == M_PI )
+ if (std::fabs(std::fabs(theta) - M_PI) <=
+ vpMath::maximum(std::fabs(theta), (double)M_PI) * std::numeric_limits<double>::epsilon()) {
+ theta = 0;
}
diff = fabs(theta - theta_1);
- if (diff > M_PI/2.0)
- sign *= -1;
+ if (diff > M_PI / 2.0)
+ sign *= -1;
theta_1 = theta;
-
- delta = - theta + M_PI/2.0;
+
+ delta = -theta + M_PI / 2.0;
normalizeAngle(delta);
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- p_me.alpha = delta ;
+ p_me.alpha = delta;
p_me.mask_sign = sign;
*it = p_me;
}
@@ -634,19 +618,15 @@ vpMbtMeLine::updateDelta()
/*!
Track the line in the image I.
-
+
\param I : Image in which the line appears.
*/
-void
-vpMbtMeLine::track(const vpImage<unsigned char> &I)
+void vpMbtMeLine::track(const vpImage<unsigned char> &I)
{
// 2. On appelle ce qui n'est pas specifique
- try
- {
+ try {
vpMeTracker::track(I);
- }
- catch(...)
- {
+ } catch (...) {
throw; // throw the original exception
}
@@ -655,16 +635,15 @@ vpMbtMeLine::track(const vpImage<unsigned char> &I)
// setExtremities();
}
-
/*!
Update the moving edges parameters after the virtual visual servoing.
-
+
\param I : The image.
\param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
- \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
+ \param theta_ : The \f$\theta\f$ parameter used in the line's polar
+ equation.
*/
-void
-vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
+void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, double theta_)
{
this->rho = rho_;
this->theta = theta_;
@@ -677,26 +656,25 @@ vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, double rho_, doub
seekExtremities(I);
suppressPoints(I);
setExtremities();
- //reechantillonage si necessaire
+ // reechantillonage si necessaire
reSample(I);
// remet a jour l'angle delta pour chaque point de la liste
updateDelta();
}
-
/*!
Update the moving edges parameters after the virtual visual servoing.
-
+
\param I : The image.
\param ip1 : The first extremity of the line.
\param ip2 : The second extremity of the line.
\param rho_ : The \f$\rho\f$ parameter used in the line's polar equation.
- \param theta_ : The \f$\theta\f$ parameter used in the line's polar equation.
+ \param theta_ : The \f$\theta\f$ parameter used in the line's polar
+ equation.
*/
-void
-vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1, vpImagePoint ip2,
- double rho_, double theta_)
+void vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2,
+ double rho_, double theta_)
{
this->rho = rho_;
this->theta = theta_;
@@ -709,72 +687,63 @@ vpMbtMeLine::updateParameters(const vpImage<unsigned char> &I, vpImagePoint ip1,
seekExtremities(I);
suppressPoints(I);
setExtremities();
- //reechantillonage si necessaire
- reSample(I,ip1,ip2);
+ // reechantillonage si necessaire
+ reSample(I, ip1, ip2);
// remet a jour l'angle delta pour chaque point de la liste
updateDelta();
}
-
/*!
Seek in the list of available points the two extremities of the line.
*/
-void
-vpMbtMeLine::setExtremities()
+void vpMbtMeLine::setExtremities()
{
- double i_min = +1e6 ;
+ double i_min = +1e6;
double j_min = +1e6;
- double i_max = -1 ;
- double j_max = -1 ;
+ double i_max = -1;
+ double j_max = -1;
// Loop through list of sites to track
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.ifloat < i_min)
- {
- i_min = s.ifloat ;
- j_min = s.jfloat ;
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite s = *it; // current reference pixel
+ if (s.ifloat < i_min) {
+ i_min = s.ifloat;
+ j_min = s.jfloat;
}
- if (s.ifloat > i_max)
- {
- i_max = s.ifloat ;
- j_max = s.jfloat ;
+ if (s.ifloat > i_max) {
+ i_max = s.ifloat;
+ j_max = s.jfloat;
}
}
- if ( ! list.empty() )
- {
- PExt[0].ifloat = i_min ;
- PExt[0].jfloat = j_min ;
- PExt[1].ifloat = i_max ;
- PExt[1].jfloat = j_max ;
+ if (!list.empty()) {
+ PExt[0].ifloat = i_min;
+ PExt[0].jfloat = j_min;
+ PExt[1].ifloat = i_max;
+ PExt[1].jfloat = j_max;
}
- if (fabs(i_min-i_max) < 25)
- {
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.jfloat < j_min)
- {
- i_min = s.ifloat ;
- j_min = s.jfloat ;
+ if (fabs(i_min - i_max) < 25) {
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite s = *it; // current reference pixel
+ if (s.jfloat < j_min) {
+ i_min = s.ifloat;
+ j_min = s.jfloat;
}
- if (s.jfloat > j_max)
- {
- i_max = s.ifloat ;
- j_max = s.jfloat ;
+ if (s.jfloat > j_max) {
+ i_max = s.ifloat;
+ j_max = s.jfloat;
}
}
- if (! list.empty())
- {
- PExt[0].ifloat = i_min ;
- PExt[0].jfloat = j_min ;
- PExt[1].ifloat = i_max ;
- PExt[1].jfloat = j_max ;
+ if (!list.empty()) {
+ PExt[0].ifloat = i_min;
+ PExt[0].jfloat = j_min;
+ PExt[1].ifloat = i_max;
+ PExt[1].jfloat = j_max;
}
bubbleSortJ();
}
@@ -783,13 +752,9 @@ 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()
+void vpMbtMeLine::bubbleSortI()
{
#if 0
unsigned int nbElmt = list.size();
@@ -798,8 +763,8 @@ vpMbtMeLine::bubbleSortI()
list.front();
for (unsigned int i=0; i < nbElmt-pass; i++)
{
- vpMeSite s1 = list.value() ;
- vpMeSite s2 = list.nextValue() ;
+ vpMeSite s1 = list.value();
+ vpMeSite s2 = list.nextValue();
if (s1.ifloat > s2.ifloat)
list.swapRight();
else
@@ -810,13 +775,9 @@ 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()
+void vpMbtMeLine::bubbleSortJ()
{
#if 0
unsigned int nbElmt = list.size();
@@ -825,8 +786,8 @@ vpMbtMeLine::bubbleSortJ()
list.front();
for (unsigned int i=0; i < nbElmt-pass; i++)
{
- vpMeSite s1 = list.value() ;
- vpMeSite s2 = list.nextValue() ;
+ vpMeSite s1 = list.value();
+ vpMeSite s2 = list.nextValue();
if (s1.jfloat > s2.jfloat)
list.swapRight();
else
@@ -837,31 +798,4 @@ vpMbtMeLine::bubbleSortJ()
list.sort(sortByJ);
}
-
-void
-vpMbtMeLine::findSignal(const vpImage<unsigned char>& I, const vpMe *p_me, double *conv)
-{
- vpImagePoint itest(PExt[0].ifloat+(PExt[1].ifloat-PExt[0].ifloat)/2, PExt[0].jfloat+(PExt[1].jfloat-PExt[0].jfloat)/2);
-
- vpMeSite pix ; //= list.value();
- pix.init(itest.get_i(), itest.get_j(), delta, 0, sign);
-
- vpMeSite *list_query_pixels;
-// double convolution = 0;
- unsigned int range = p_me->getRange();
-
- list_query_pixels = pix.getQueryList(I, (int)range);
-
- vpDisplay::displayCross(I,itest,5,vpColor::cyan,3);
- vpDisplay::displayLine(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),vpImagePoint(list_query_pixels[2*range].ifloat,list_query_pixels[2*range].jfloat),vpColor::cyan);
- vpDisplay::displayCross(I,vpImagePoint(list_query_pixels[0].ifloat,list_query_pixels[0].jfloat),5,vpColor::orange,3);
-
- for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
- {
- conv[n] = list_query_pixels[n].convolution(I, p_me);
- }
- delete [] list_query_pixels;
-}
-
#endif
-
diff --git a/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp
index 3352397..b5b281c 100644
--- a/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp
+++ b/modules/tracker/mbt/src/edge/vpMbtXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,38 +40,30 @@
*****************************************************************************/
#include <visp3/core/vpConfig.h>
-
#ifdef VISP_HAVE_XML2
#include <iostream>
#include <map>
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbtXmlParser.h>
-
/*!
- Default constructor.
-
+ Default constructor.
+
*/
-vpMbtXmlParser::vpMbtXmlParser() : m_ecm()
-{
- init();
-}
+vpMbtXmlParser::vpMbtXmlParser() : m_ecm() { init(); }
/*!
Default destructor.
*/
-vpMbtXmlParser::~vpMbtXmlParser()
-{
-}
+vpMbtXmlParser::~vpMbtXmlParser() {}
/*!
Initialise internal variables (including the map).
*/
-void
-vpMbtXmlParser::init()
+void vpMbtXmlParser::init()
{
vpMbXmlParser::init();
@@ -91,11 +84,10 @@ vpMbtXmlParser::init()
/*!
Parse the file in parameters.
This method is deprecated, use parse() instead.
-
+
\param filename : File to parse.
*/
-void
-vpMbtXmlParser::parse(const char * filename)
+void vpMbtXmlParser::parse(const char *filename)
{
std::string file = filename;
vpXmlParser::parse(file);
@@ -103,197 +95,195 @@ vpMbtXmlParser::parse(const char * filename)
/*!
Write info to file.
-
+
\warning Useless, so not yet implemented => Throw exception.
*/
-void
-vpMbtXmlParser::writeMainClass(xmlNodePtr /*node*/)
+void vpMbtXmlParser::writeMainClass(xmlNodePtr /*node*/)
{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
+ throw vpException(vpException::notImplementedError, "Not yet implemented.");
}
/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
\param doc : Document to parse.
- \param node : Root node.
+ \param node : Root node.
*/
-void
-vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
bool camera_node = false;
bool face_node = false;
bool ecm_node = false;
bool lod_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case camera: {
+ this->read_camera(doc, dataNode);
camera_node = true;
- }break;
- case face:{
+ } break;
+ case face: {
this->read_face(doc, dataNode);
face_node = true;
- }break;
- case ecm:{
- this->read_ecm (doc, dataNode);
+ } break;
+ case ecm: {
+ this->read_ecm(doc, dataNode);
ecm_node = true;
- }break;
- case sample:{
- this->read_sample_deprecated (doc, dataNode);
- }break;
- case lod:{
+ } break;
+ case sample: {
+ this->read_sample_deprecated(doc, dataNode);
+ } break;
+ case lod: {
this->read_lod(doc, dataNode);
lod_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+
+ if (!camera_node) {
+ std::cout << "camera : u0 : " << this->cam.get_u0() << " (default)" << std::endl;
+ std::cout << "camera : v0 : " << this->cam.get_v0() << " (default)" << std::endl;
+ std::cout << "camera : px : " << this->cam.get_px() << " (default)" << std::endl;
+ std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl;
}
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ if (!face_node) {
+ std::cout << "face : Angle Appear : " << angleAppear << " (default)" << std::endl;
+ std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl;
}
- if(!ecm_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ if (!ecm_node) {
+ std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << " (default)" << std::endl;
+ std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << " (default)" << std::endl;
+ std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : threshold : " << this->m_ecm.getThreshold() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu1 : " << this->m_ecm.getMu1() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu2 : " << this->m_ecm.getMu2() << " (default)" << std::endl;
+ std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl;
}
- if(!lod_node) {
+ if (!lod_node) {
std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
}
}
-
/*!
Read ecm information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the ecm information.
*/
-void
-vpMbtXmlParser::read_ecm (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_ecm(xmlDocPtr doc, xmlNodePtr node)
{
bool mask_node = false;
bool range_node = false;
bool contrast_node = false;
bool sample_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case mask:{
- this->read_mask (doc, dataNode);
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case mask: {
+ this->read_mask(doc, dataNode);
mask_node = true;
- }break;
- case range:{
- this->read_range (doc, dataNode);
+ } break;
+ case range: {
+ this->read_range(doc, dataNode);
range_node = true;
- }break;
- case contrast:{
- this->read_contrast (doc, dataNode);
+ } break;
+ case contrast: {
+ this->read_contrast(doc, dataNode);
contrast_node = true;
- }break;
- case sample:{
- this->read_sample (doc, dataNode);
+ } break;
+ case sample: {
+ this->read_sample(doc, dataNode);
sample_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_ecm : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_ecm : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!mask_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+
+ if (!mask_node) {
+ std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << " (default)" << std::endl;
+ std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << " (default)" << std::endl;
}
-
- if(!range_node) {
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+
+ if (!range_node) {
+ std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << " (default)" << std::endl;
}
-
- if(!contrast_node) {
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+
+ if (!contrast_node) {
+ std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu1 " << this->m_ecm.getMu1() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu2 " << this->m_ecm.getMu2() << " (default)" << std::endl;
}
- if(!sample_node) {
- std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ if (!sample_node) {
+ std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl;
}
}
/*!
Read sample information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the sample information.
*/
-void
-vpMbtXmlParser::read_sample (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_sample(xmlDocPtr doc, xmlNodePtr node)
{
bool step_node = false;
-
+
// current data values.
double d_stp = this->m_ecm.getSampleStep();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case step:{
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case step: {
d_stp = xmlReadIntChild(doc, dataNode);
step_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
+
this->m_ecm.setSampleStep(d_stp);
-// this->m_ecm.setNbTotalSample(d_nb_sample);
+ // this->m_ecm.setNbTotalSample(d_nb_sample);
- if(!step_node)
- std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ if (!step_node)
+ std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl;
else
- std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<<std::endl;
+ std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << std::endl;
}
/*!
@@ -304,192 +294,195 @@ vpMbtXmlParser::read_sample (xmlDocPtr doc, xmlNodePtr node)
\param doc : Pointer to the document.
\param node : Pointer to the node of the sample information.
*/
-void
-vpMbtXmlParser::read_sample_deprecated (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node)
{
bool step_node = false;
- //bool nb_sample_node = false;
+ // bool nb_sample_node = false;
- // current data values.
+ // current data values.
double d_stp = this->m_ecm.getSampleStep();
-// int d_nb_sample = this->m_ecm.getNbTotalSample();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case step:{
+ // int d_nb_sample = this->m_ecm.getNbTotalSample();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case step: {
d_stp = xmlReadIntChild(doc, dataNode);
step_node = true;
- }break;
-// case nb_sample:{
-// d_nb_sample = xmlReadIntChild(doc, dataNode);
-// nb_sample_node = true;
-// }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ // case nb_sample:{
+ // d_nb_sample = xmlReadIntChild(doc, dataNode);
+ // nb_sample_node = true;
+ // }break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
this->m_ecm.setSampleStep(d_stp);
-// this->m_ecm.setNbTotalSample(d_nb_sample);
+ // this->m_ecm.setNbTotalSample(d_nb_sample);
- if(!step_node)
- std::cout <<"[DEPRECATED] sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ if (!step_node)
+ std::cout << "[DEPRECATED] sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl;
else
- std::cout <<"[DEPRECATED] sample : sample_step : "<< this->m_ecm.getSampleStep()<<std::endl;
+ std::cout << "[DEPRECATED] sample : sample_step : " << this->m_ecm.getSampleStep() << std::endl;
-// if(!nb_sample_node)
-// std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
-// else
-// std::cout <<"sample : n_total_sample : "<< this->m_ecm.getNbTotalSample()<<std::endl;
+ // if(!nb_sample_node)
+ // std::cout <<"sample : n_total_sample : "<<
+ // this->m_ecm.getNbTotalSample()<< " (default)"<<std::endl;
+ // else
+ // std::cout <<"sample : n_total_sample : "<<
+ // this->m_ecm.getNbTotalSample()<<std::endl;
- std::cout <<" WARNING : This node (sample) is deprecated." << std::endl;
+ std::cout << " WARNING : This node (sample) is deprecated." << std::endl;
std::cout << " It should be moved in the ecm node (ecm : sample)." << std::endl;
}
/*!
Read mask information for the vpMeSite.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the mask information.
*/
-void
-vpMbtXmlParser::read_mask (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_mask(xmlDocPtr doc, xmlNodePtr node)
{
bool size_node = false;
bool nb_mask_node = false;
-
- // current data values.
+
+ // current data values.
unsigned int d_size = this->m_ecm.getMaskSize();
unsigned int d_nb_mask = this->m_ecm.getMaskNumber();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case size:{
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case size: {
d_size = xmlReadUnsignedIntChild(doc, dataNode);
size_node = true;
- }break;
- case nb_mask:{
+ } break;
+ case nb_mask: {
d_nb_mask = xmlReadUnsignedIntChild(doc, dataNode);
nb_mask_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_mask : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_mask : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
- this->m_ecm.setMaskSize(d_size) ;
+ this->m_ecm.setMaskSize(d_size);
// Check to ensure that d_nb_mask > 0
- if (! d_nb_mask)
- throw(vpException(vpException::badValue, "Model-based tracker mask size parameter should be different from zero in xml file"));
+ if (!d_nb_mask)
+ throw(vpException(vpException::badValue, "Model-based tracker mask size "
+ "parameter should be different "
+ "from zero in xml file"));
this->m_ecm.setMaskNumber(d_nb_mask);
-
- if(!size_node)
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- else
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() <<std::endl;
-
- if(!nb_mask_node)
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
+
+ if (!size_node)
+ std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << " (default)" << std::endl;
+ else
+ std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << std::endl;
+
+ if (!nb_mask_node)
+ std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << " (default)" << std::endl;
else
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() <<std::endl;
+ std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << std::endl;
}
/*!
Read range information for the vpMeSite.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the range information.
*/
-void
-vpMbtXmlParser::read_range (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_range(xmlDocPtr doc, xmlNodePtr node)
{
bool tracking_node = false;
-
- // current data values.
- unsigned int m_range_tracking = this->m_ecm.getRange();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case tracking:{
+
+ // current data values.
+ unsigned int m_range_tracking = this->m_ecm.getRange();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case tracking: {
m_range_tracking = xmlReadUnsignedIntChild(doc, dataNode);
tracking_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_range : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_range : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
this->m_ecm.setRange(m_range_tracking);
-
- if(!tracking_node)
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
+
+ if (!tracking_node)
+ std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << " (default)" << std::endl;
else
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<<std::endl;
+ std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << std::endl;
}
-
/*!
Read the contrast information from the xml file.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the contrast information.
*/
-void
-vpMbtXmlParser::read_contrast (xmlDocPtr doc, xmlNodePtr node)
+void vpMbtXmlParser::read_contrast(xmlDocPtr doc, xmlNodePtr node)
{
bool edge_threshold_node = false;
bool mu1_node = false;
bool mu2_node = false;
-
- // current data values.
- double d_edge_threshold = this->m_ecm.getThreshold();
- double d_mu1 = this->m_ecm.getMu1();
- double d_mu2 = this->m_ecm.getMu2();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case edge_threshold:{
+
+ // current data values.
+ double d_edge_threshold = this->m_ecm.getThreshold();
+ double d_mu1 = this->m_ecm.getMu1();
+ double d_mu2 = this->m_ecm.getMu2();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case edge_threshold: {
d_edge_threshold = xmlReadDoubleChild(doc, dataNode);
edge_threshold_node = true;
- }break;
- case mu1:{
+ } break;
+ case mu1: {
d_mu1 = xmlReadDoubleChild(doc, dataNode);
mu1_node = true;
- }break;
- case mu2:{
+ } break;
+ case mu2: {
d_mu2 = xmlReadDoubleChild(doc, dataNode);
mu2_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_contrast : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
@@ -498,25 +491,25 @@ vpMbtXmlParser::read_contrast (xmlDocPtr doc, xmlNodePtr node)
this->m_ecm.setMu1(d_mu1);
this->m_ecm.setMu2(d_mu2);
this->m_ecm.setThreshold(d_edge_threshold);
-
- if(!edge_threshold_node)
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
+
+ if (!edge_threshold_node)
+ std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() << " (default)" << std::endl;
else
- std::cout <<"ecm : contrast : threshold " << this->m_ecm.getThreshold()<<std::endl;
-
- if(!mu1_node)
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
+ std::cout << "ecm : contrast : threshold " << this->m_ecm.getThreshold() << std::endl;
+
+ if (!mu1_node)
+ std::cout << "ecm : contrast : mu1 " << this->m_ecm.getMu1() << " (default)" << std::endl;
else
- std::cout <<"ecm : contrast : mu1 " << this->m_ecm.getMu1()<<std::endl;
-
- if(!mu2_node)
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
+ std::cout << "ecm : contrast : mu1 " << this->m_ecm.getMu1() << std::endl;
+
+ if (!mu2_node)
+ std::cout << "ecm : contrast : mu2 " << this->m_ecm.getMu2() << " (default)" << std::endl;
else
- std::cout <<"ecm : contrast : mu2 " << this->m_ecm.getMu2()<<std::endl;
+ std::cout << "ecm : contrast : mu2 " << this->m_ecm.getMu2() << std::endl;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbtXmlParser.cpp.o) has no symbols
-void dummy_vpMbtXmlParser() {};
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtXmlParser.cpp.o) has no
+// symbols
+void dummy_vpMbtXmlParser(){};
#endif
-
diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp
index 27803ba..3194185 100644
--- a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp
+++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltMultiTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -13,7 +14,7 @@
* GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See https://visp.inria.fr/download/ for more information.
*
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
@@ -44,23 +45,25 @@
#include <visp3/core/vpConfig.h>
-#if (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 <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
-
/*!
Basic constructor
*/
vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker()
- : vpMbEdgeMultiTracker(), vpMbKltMultiTracker(),
- compute_interaction(true), lambda(0.8), m_factorKLT(0.65), m_factorMBT(0.35),
- thresholdKLT(2.), thresholdMBT(2.), maxIter(200),
- m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera") {
- //Add default camera transformation matrix
+ : 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();
+
+ m_lambda = 0.8;
+ m_maxIter = 200;
}
/*!
@@ -69,35 +72,38 @@ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker()
\param nbCameras : Number of cameras to use.
*/
vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const unsigned int nbCameras)
- : vpMbEdgeMultiTracker(nbCameras), vpMbKltMultiTracker(nbCameras),
- compute_interaction(true), lambda(0.8), m_factorKLT(0.65), m_factorMBT(0.35),
- thresholdKLT(2.), thresholdMBT(2.), maxIter(200),
- m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera") {
+ : 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) {
+ if (nbCameras == 0) {
throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
- } else if(nbCameras == 1) {
- //Add default camera transformation matrix
+ } else if (nbCameras == 1) {
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
- } else if(nbCameras == 2) {
- //Add default camera transformation matrix
+ } else if (nbCameras == 2) {
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
m_mapOfCameraTransformationMatrix["Camera2"] = vpHomogeneousMatrix();
- //Set by default the reference camera
+ // Set by default the reference camera
m_referenceCameraName = "Camera1";
} else {
- for(unsigned int i = 1; i <= nbCameras; i++) {
+ for (unsigned int i = 1; i <= nbCameras; i++) {
std::stringstream ss;
ss << "Camera" << i;
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
}
- //Set by default the reference camera to the first one
+ // Set by default the reference camera to the first one
m_referenceCameraName = m_mapOfKltTrackers.begin()->first;
}
+
+ m_lambda = 0.8;
+ m_maxIter = 200;
}
/*!
@@ -106,433 +112,239 @@ vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const unsigned int nbCameras)
\param cameraNames : List of camera names.
*/
vpMbEdgeKltMultiTracker::vpMbEdgeKltMultiTracker(const std::vector<std::string> &cameraNames)
- : vpMbEdgeMultiTracker(cameraNames), vpMbKltMultiTracker(cameraNames),
- compute_interaction(true), lambda(0.8), m_factorKLT(0.65), m_factorMBT(0.35),
- thresholdKLT(2.), thresholdMBT(2.), maxIter(200),
- m_mapOfCameraTransformationMatrix(), m_referenceCameraName("Camera") {
- //Set by default the reference camera
- m_referenceCameraName = cameraNames.front();
+ : 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;
}
-vpMbEdgeKltMultiTracker::~vpMbEdgeKltMultiTracker() {
-}
+vpMbEdgeKltMultiTracker::~vpMbEdgeKltMultiTracker() {}
void vpMbEdgeKltMultiTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNumberOfRows, std::map<std::string, unsigned int> &mapOfNbInfos,
- vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl) {
-
- //Factor for MBT, each factor for each camera are appended
- vpColVector factor;
- //Indicate for each row in the final interaction matrix or residual or weight for the MBT part
- //whether the considered features is a line, a cylinder or a circle
- std::vector<FeatureType> indexOfFeatures;
- std::map<std::string, unsigned int> mapOfNumberOfLines;
- std::map<std::string, unsigned int> mapOfNumberOfCylinders;
- std::map<std::string, unsigned int> mapOfNumberOfCircles;
- //Get the number of rows for the MBT part
- //Get also the number of rows, lines, cylinders and circle for each camera
- unsigned int nbrow = trackFirstLoop(mapOfImages, factor, indexOfFeatures,
- mapOfNumberOfRows, mapOfNumberOfLines, mapOfNumberOfCylinders, mapOfNumberOfCircles, lvl);
-
-// unsigned int nbInfos = w_klt.size() / 2;
- unsigned int nbInfos = 0;
- for(std::map<std::string, unsigned int>::const_iterator it = mapOfNbInfos.begin(); it != mapOfNbInfos.end(); ++it) {
- nbInfos += it->second;
- }
+ const unsigned int lvl)
+{
- if(nbInfos < 4 && nbrow < 4) {
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
- } else if(nbrow < 4) {
- nbrow = 0;
- }
+ m_nbrow = initMbtTracking(mapOfImages, lvl);
-// double factorMBT = 1.0;
-// double factorKLT = 1.0;
-//
-// //More efficient weight repartition for hybrid tracker should come soon...
-//// factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
-//// factorKLT = 1.0 - factorMBT;
-// factorMBT = 0.35;
-// factorKLT = 0.65;
+ if (m_nbInfos < 4 && m_nbrow < 4) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
+ } else if (m_nbrow < 4) {
+ m_nbrow = 0;
+ }
double factorMBT = m_factorMBT;
double factorKLT = m_factorKLT;
- if (nbrow < 4) {
+ if (m_nbrow < 4) {
factorKLT = 1.;
std::cerr << "There are less than 4 KLT features, set factorKLT = 1. !" << std::endl;
}
- if (nbInfos < 4) {
+ if (m_nbInfos < 4) {
factorMBT = 1.;
std::cerr << "There are less than 4 moving edges, set factorMBT = 1. !" << std::endl;
- nbInfos = 0;
+ m_nbInfos = 0;
}
+ computeVVSInit();
vpHomogeneousMatrix cMoPrev;
vpHomogeneousMatrix ctTc0_Prev;
- //Error vector for MBT + KLT for the previous iteration
- vpColVector m_error_prev(2*nbInfos + nbrow);
- //Weighting vector for MBT + KLT for the previous iteration
- vpColVector m_w_prev(2*nbInfos + nbrow);
- double mu = 0.01;
+ // Error vector for MBT + KLT for the previous iteration
+ vpColVector m_error_prev;
+ // Weighting vector for MBT + KLT for the previous iteration
+ vpColVector m_w_prev;
+ double mu = m_initialMu;
- //Create the map of VelocityTwistMatrices
+ // Create the map of VelocityTwistMatrices
std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
- for(std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
- it != m_mapOfCameraTransformationMatrix.end(); ++it) {
+ for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
+ it != m_mapOfCameraTransformationMatrix.end(); ++it) {
vpVelocityTwistMatrix cVo;
cVo.buildFrom(it->second);
mapOfVelocityTwist[it->first] = cVo;
}
- //Map of robust for edge trackers
- //Individual weights for each primitives and for each camera
- std::map<std::string, vpRobust> mapOfEdgeRobustLines;
- std::map<std::string, vpRobust> mapOfEdgeRobustCylinders;
- std::map<std::string, vpRobust> mapOfEdgeRobustCircles;
- //Init map of robust
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- mapOfEdgeRobustLines[it->first] = vpRobust(mapOfNumberOfLines[it->first]);
- mapOfEdgeRobustLines[it->first].setIteration(0);
-
- mapOfEdgeRobustCylinders[it->first] = vpRobust(mapOfNumberOfCylinders[it->first]);
- mapOfEdgeRobustCylinders[it->first].setIteration(0);
-
- mapOfEdgeRobustCircles[it->first] = vpRobust(mapOfNumberOfCircles[it->first]);
- mapOfEdgeRobustCircles[it->first].setIteration(0);
- }
-
- //Map of robust for KLT trackers
- std::map<std::string, vpRobust> mapOfKltRobusts;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- mapOfKltRobusts[it->first] = vpRobust(2*mapOfNbInfos[it->first]);
- }
-
- //Individual primitive residuals for MBT for all the cameras
- vpColVector error_lines;
- vpColVector error_cylinders;
- vpColVector error_circles;
-
- //Individual primitive weights for MBT for all the cameras
- vpColVector w_lines;
- vpColVector w_cylinders;
- vpColVector w_circles;
-
- //Map of primitive weights for each camera and for MBT
- std::map<std::string, vpColVector> mapOfWeightLines;
- std::map<std::string, vpColVector> mapOfWeightCylinders;
- std::map<std::string, vpColVector> mapOfWeightCircles;
-
- //Variables used in the minimization process
+ // Variables used in the minimization process
double residu = 0;
double residu_1 = -1;
unsigned int iter = 0;
- vpMatrix *L;
- vpColVector *R;
vpMatrix L_true;
vpMatrix LVJ_true;
- vpColVector w_true;
vpColVector v;
- vpHomography H;
vpMatrix LTL;
vpColVector LTR;
- while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
- L = new vpMatrix();
- R = new vpColVector();
-
- //MBT
- error_lines.resize(0);
- error_cylinders.resize(0);
- error_circles.resize(0);
-
- std::map<std::string, vpColVector> mapOfErrorLines;
- std::map<std::string, vpColVector> mapOfErrorCylinders;
- std::map<std::string, vpColVector> mapOfErrorCircles;
-
- vpColVector R_mbt;
- vpMatrix L_mbt;
- if(nbrow >= 4) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- vpMatrix L_tmp(mapOfNumberOfRows[it->first], 6);
- vpColVector error_lines_tmp(mapOfNumberOfLines[it->first]);
- vpColVector error_cylinders_tmp(mapOfNumberOfCylinders[it->first]);
- vpColVector error_circles_tmp(mapOfNumberOfCircles[it->first]);
-
- //Set the corresponding cMo for the current camera
- it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;
-
- vpColVector R_tmp;
- R_tmp.resize(mapOfNumberOfRows[it->first]);
- it->second->computeVVSSecondPhase(*mapOfImages[it->first], L_tmp, error_lines_tmp,
- error_cylinders_tmp, error_circles_tmp, R_tmp, 0);
- //Set the computed weight
- it->second->m_w = R_tmp;
- L_tmp = L_tmp*mapOfVelocityTwist[it->first];
-
- //Stack interaction matrix for MBT
- L_mbt.stack(L_tmp);
- //Stack residual for MBT
- R_mbt.stack(R_tmp);
-
- error_lines.stack(error_lines_tmp);
- error_cylinders.stack(error_cylinders_tmp);
- error_circles.stack(error_circles_tmp);
-
- mapOfErrorLines[it->first] = error_lines_tmp;
- mapOfErrorCylinders[it->first] = error_cylinders_tmp;
- mapOfErrorCircles[it->first] = error_circles_tmp;
- }
+ while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
+ computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
+
+ bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_hybridMulti, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
+ &m_w_prev);
+ if (reStartFromLastIncrement) {
+ ctTc0 = ctTc0_Prev;
}
- //KLT
- vpColVector R_klt;
- vpMatrix L_klt;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it1 = m_mapOfKltTrackers.begin();
- it1 != m_mapOfKltTrackers.end(); ++it1) {
- if(mapOfNbInfos[it1->first] > 0) {
- unsigned int shift = 0;
- vpColVector R_current; // residu for the current camera for KLT
- vpMatrix L_current; // interaction matrix for the current camera for KLT
- vpHomography H_current;
-
- R_current.resize(2 * mapOfNbInfos[it1->first]);
- L_current.resize(2 * mapOfNbInfos[it1->first], 6, 0);
-
- //Use the ctTc0 variable instead of the formula in the monocular case
- //to ensure that we have the same result than vpMbKltTracker
- //as some slight differences can occur due to numerical imprecision
- if(m_mapOfKltTrackers.size() == 1) {
- computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
- it1->second->kltPolygons, it1->second->kltCylinders, ctTc0);
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
+
+ // 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;
} else {
- vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it1->first] *
- cMo * it1->second->c0Mo.inverse();
- computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
- it1->second->kltPolygons, it1->second->kltCylinders, c_curr_tTc_curr0);
+ m_w_hybridMulti[cpt] *= factorKLT;
}
+ }
- //Transform the current interaction matrix with VelocityTwistMatrix
- L_current = L_current*mapOfVelocityTwist[it1->first];
-
- //Stack residual and interaction matrix
- R_klt.stack(R_current);
- L_klt.stack(L_current);
+ if (computeCovariance) {
+ L_true = m_L_hybridMulti;
+ if (!isoJoIdentity) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_hybridMulti * cVo * oJo);
+ }
}
- }
+ residu_1 = residu;
+ residu = 0;
+ double num = 0;
+ double den = 0;
- bool reStartFromLastIncrement = false;
- if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT) {
- if( m_error.sumSquare()/(double)(2*nbInfos + nbrow) > m_error_prev.sumSquare()/(double)(2*nbInfos + nbrow) ) {
- mu *= 10.0;
+ 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];
- if(mu > 1.0) {
- throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+ 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];
+ }
}
-
- cMo = cMoPrev;
- m_error = m_error_prev;
- m_w = m_w_prev;
- ctTc0 = ctTc0_Prev;
- reStartFromLastIncrement = true;
}
- }
- if(iter == 0) {
- m_w.resize(nbrow + 2*nbInfos);
- m_w = 1;
+ 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);
- w_true.resize(nbrow + 2*nbInfos);
+ cMoPrev = cMo;
+ ctTc0_Prev = ctTc0;
+ ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
+ cMo = ctTc0 * c0Mo;
}
- if(!reStartFromLastIncrement) {
- //MBT
- w_lines.resize(0);
- w_cylinders.resize(0);
- w_circles.resize(0);
-
- //Compute the weights for MBT
- computeVVSSecondPhaseWeights(iter, nbrow, w_mbt, w_lines, w_cylinders, w_circles, mapOfNumberOfLines,
- mapOfNumberOfCylinders, mapOfNumberOfCircles, mapOfWeightLines, mapOfWeightCylinders, mapOfWeightCircles,
- mapOfErrorLines, mapOfErrorCylinders, mapOfErrorCircles, mapOfEdgeRobustLines, mapOfEdgeRobustCylinders,
- mapOfEdgeRobustCircles, thresholdMBT);
-
- for(unsigned int cpt = 0, cpt_lines = 0, cpt_cylinders = 0, cpt_circles = 0; cpt < nbrow; cpt++) {
- switch(indexOfFeatures[cpt]) {
- case LINE:
- w_mbt[cpt] = w_lines[cpt_lines];
- cpt_lines++;
- break;
-
- case CYLINDER:
- w_mbt[cpt] = w_cylinders[cpt_cylinders];
- cpt_cylinders++;
- break;
-
- case CIRCLE:
- w_mbt[cpt] = w_circles[cpt_circles];
- cpt_circles++;
- break;
-
- default:
- std::cerr << "Problem with feature type !" << std::endl;
- break;
- }
- }
+ iter++;
+ }
- //KLT
- vpColVector w_true_klt; //not used
- //Compute the weights for KLT
- computeVVSWeights(iter, nbInfos, mapOfNbInfos, R_klt, w_true_klt, w_klt, mapOfKltRobusts, thresholdKLT);
+ computeCovarianceMatrixVVS(isoJoIdentity, m_w_hybridMulti, cMoPrev, L_true, LVJ_true, m_error_hybridMulti);
+}
+void vpMbEdgeKltMultiTracker::computeVVSInit()
+{
+ unsigned int totalNbRows = 2 * m_nbInfos + m_nbrow;
- //Stack MBT first and then KLT
- /* robust */
- if(nbrow > 3) {
- //Stack interaction matrix and residual from MBT
- L->stack(L_mbt);
- R->stack(R_mbt);
- }
+ m_L_hybridMulti.resize(totalNbRows, 6, false, false);
+ m_error_hybridMulti.resize(totalNbRows, false);
- if(nbInfos > 3) {
- //Stack interaction matrix and residual from KLT
- L->stack(L_klt);
- R->stack(R_klt);
- }
+ m_weightedError_hybridMulti.resize(totalNbRows, false);
+ m_w_hybridMulti.resize(totalNbRows, false);
+ m_w_hybridMulti = 1;
- //Set weight for m_w with the good weighting between MBT and KLT
- unsigned int cpt = 0;
- while( cpt < (nbrow + 2*nbInfos) ) {
- if(cpt < (unsigned int) nbrow) {
- m_w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
- }
- else {
- m_w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
- }
- cpt++;
- }
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
+ klt->computeVVSInit();
+ }
+}
- m_error = (*R);
- if(computeCovariance) {
- L_true = (*L);
- if(!isoJoIdentity) {
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- LVJ_true = ( (*L)*cVo*oJo );
- }
- }
+void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbEdgeKltMultiTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
+}
- residu_1 = residu;
- residu = 0;
- double num = 0;
- double den = 0;
- for (unsigned int i = 0; i < R->getRows(); i++) {
- num += m_w[i]*vpMath::sqr((*R)[i]);
- den += m_w[i];
+void vpMbEdgeKltMultiTracker::computeVVSInteractionMatrixAndResidu(
+ std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
+{
+ unsigned int startIdx = 0;
- w_true[i] = m_w[i];
- (*R)[i] *= m_w[i];
- if(compute_interaction) {
- for (unsigned int j = 0; j < 6; j++) {
- (*L)[i][j] *= m_w[i];
- }
- }
- }
+ if (m_nbrow >= 4) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
- residu = sqrt(num/den);
+ // Set the corresponding cMo for the current camera
+ it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
- if(isoJoIdentity) {
- LTL = L->AtA();
- computeJTR(*L, *R, LTR);
+ edge->computeVVSInteractionMatrixAndResidu(*mapOfImages[it->first]);
- switch(m_optimizationMethod) {
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LTL.getRows(), LTL.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LTL + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+ // Stack interaction matrix for MBT
+ 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);
- if(iter != 0) {
- mu /= 10.0;
- }
+ startIdx += edge->m_error_edge.getRows();
+ }
+ }
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
- break;
- }
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
+
+ if (klt->m_nbInfos > 0) {
+
+ // Use the ctTc0 variable instead of the formula in the monocular case
+ // to ensure that we have the same result than vpMbKltTracker
+ // as some slight differences can occur due to numerical imprecision
+ if (m_mapOfKltTrackers.size() == 1) {
+ klt->ctTc0 = ctTc0;
+ klt->computeVVSInteractionMatrixAndResidu();
+ } else {
+ vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it->first] * cMo * klt->c0Mo.inverse();
+ klt->ctTc0 = c_curr_tTc_curr0;
+ klt->computeVVSInteractionMatrixAndResidu();
}
- else {
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- vpMatrix LVJ = ((*L)*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpColVector LVJTR;
- computeJTR(LVJ, *R, LVJTR);
-
- switch(m_optimizationMethod) {
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
-
- if(iter != 0) {
- mu /= 10.0;
- }
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- {
- v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- break;
- }
- }
- }
+ // 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);
- cMoPrev = cMo;
- ctTc0_Prev = ctTc0;
- ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
- cMo = ctTc0 * c0Mo;
+ startIdx += 2 * klt->m_nbInfos;
}
+ }
+}
- iter++;
+void vpMbEdgeKltMultiTracker::computeVVSWeights()
+{
+ unsigned int startIdx = 0;
+
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
- delete L;
- delete R;
+ // Compute weights
+ edge->computeVVSWeights();
+
+ m_w_hybridMulti.insert(startIdx, edge->m_w_edge);
+ startIdx += edge->m_w_edge.getRows();
}
- if(computeCovariance) {
- vpMatrix D;
- D.diag(w_true);
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity) {
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
- }
- else {
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
- }
+ // Compute weights
+ klt->computeVVSWeights(klt->m_robust_klt, klt->m_error_klt, klt->m_w_klt);
+
+ m_w_hybridMulti.insert(startIdx, klt->m_w_klt);
+ startIdx += klt->m_w_klt.getRows();
}
}
@@ -544,32 +356,31 @@ void vpMbEdgeKltMultiTracker::computeVVS(std::map<std::string, const vpImage<uns
\param cam_ : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &cam_, const vpColor& col, const unsigned int thickness,
- const bool displayFullModel) {
+void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char> &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);
-// vpMbKltMultiTracker::display(I, cMo_, cam_, col, thickness, displayFullModel);
-
- //Display only features for KLT trackers
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts){
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
+
+ // Display only features for KLT trackers
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+
+ for (std::list<vpMbtDistanceKltPoints *>::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()) {
+ kltpoly->displayPrimitive(I);
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl){
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ for (std::list<vpMbtDistanceKltCylinder *>::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())
kltPolyCylinder->displayPrimitive(I);
}
}
@@ -583,32 +394,33 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char>& I, const vpH
\param cam_ : The camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &cam_, const vpColor& color , const unsigned int thickness,
- const bool displayFullModel) {
+void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa> &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);
-// vpMbKltMultiTracker::display(I, cMo_, cam_, color, thickness, displayFullModel);
-
- //Display only features for KLT trackers
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
-
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts){
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
+ // vpMbKltMultiTracker::display(I, cMo_, cam_, color, thickness,
+ // displayFullModel);
+
+ // Display only features for KLT trackers
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+
+ for (std::list<vpMbtDistanceKltPoints *>::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()) {
+ kltpoly->displayPrimitive(I);
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl){
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ for (std::list<vpMbtDistanceKltCylinder *>::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())
kltPolyCylinder->displayPrimitive(I);
}
}
@@ -625,25 +437,27 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogene
\param cam2 : The second camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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<unsigned char> &I1, const vpImage<unsigned char> &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);
-// vpMbKltMultiTracker::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
+ // Display only features for KLT trackers
bool first = true;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts){
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- if(first) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ for (std::list<vpMbtDistanceKltPoints *>::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 (first) {
kltpoly->displayPrimitive(I1);
} else {
kltpoly->displayPrimitive(I2);
@@ -651,12 +465,11 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char>& I1, const vp
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl){
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
- if(first) {
+ for (std::list<vpMbtDistanceKltCylinder *>::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 (first) {
kltPolyCylinder->displayPrimitive(I1);
} else {
kltPolyCylinder->displayPrimitive(I2);
@@ -679,25 +492,27 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<unsigned char>& I1, const vp
\param cam2 : The second camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<vpRGBa>& 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<vpRGBa> &I1, const vpImage<vpRGBa> &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);
-// vpMbKltMultiTracker::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)
+ // Display only features for KLT trackers (not the model)
bool first = true;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts){
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- if(first) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ for (std::list<vpMbtDistanceKltPoints *>::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 (first) {
kltpoly->displayPrimitive(I1);
} else {
kltpoly->displayPrimitive(I2);
@@ -705,12 +520,11 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<v
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl){
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
- if(first) {
+ for (std::list<vpMbtDistanceKltCylinder *>::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 (first) {
kltPolyCylinder->displayPrimitive(I1);
} else {
kltPolyCylinder->displayPrimitive(I2);
@@ -730,36 +544,37 @@ void vpMbEdgeKltMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<v
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel);
-// vpMbKltMultiTracker::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<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ // Display only features for KLT trackers (not the model)
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
- if(it_img != mapOfImages.end()) {
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts) {
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive( *(it_img->second) );
+ if (it_img != mapOfImages.end()) {
+ for (std::list<vpMbtDistanceKltPoints *>::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()) {
+ kltpoly->displayPrimitive(*(it_img->second));
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl) {
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
- kltPolyCylinder->displayPrimitive( *(it_img->second) );
+ for (std::list<vpMbtDistanceKltCylinder *>::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())
+ kltPolyCylinder->displayPrimitive(*(it_img->second));
}
}
}
@@ -773,36 +588,37 @@ void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
vpMbEdgeMultiTracker::display(mapOfImages, mapOfCameraPoses, mapOfCameraParameters, col, thickness, displayFullModel);
-// vpMbKltMultiTracker::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<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ // Display only features for KLT trackers (not the model)
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
- if(it_img != mapOfImages.end()) {
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it_pts = it_klt->second->kltPolygons.begin();
- it_pts != it_klt->second->kltPolygons.end(); ++it_pts){
- kltpoly = *it_pts;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive( *(it_img->second) );
+ if (it_img != mapOfImages.end()) {
+ for (std::list<vpMbtDistanceKltPoints *>::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()) {
+ kltpoly->displayPrimitive(*(it_img->second));
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it_cyl = it_klt->second->kltCylinders.begin();
- it_cyl != it_klt->second->kltCylinders.end(); ++it_cyl){
- kltPolyCylinder = *it_cyl;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
- kltPolyCylinder->displayPrimitive( *(it_img->second) );
+ for (std::list<vpMbtDistanceKltCylinder *>::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())
+ kltPolyCylinder->displayPrimitive(*(it_img->second));
}
}
}
@@ -813,7 +629,8 @@ void vpMbEdgeKltMultiTracker::display(const std::map<std::string, const vpImage<
\return The vector of camera names.
*/
-std::vector<std::string> vpMbEdgeKltMultiTracker::getCameraNames() const {
+std::vector<std::string> vpMbEdgeKltMultiTracker::getCameraNames() const
+{
return vpMbEdgeMultiTracker::getCameraNames();
}
@@ -822,9 +639,7 @@ std::vector<std::string> 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.
@@ -832,8 +647,9 @@ void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &camera) co
\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 {
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
+{
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getCameraParameters(cam1, cam2);
}
@@ -843,8 +659,9 @@ void vpMbEdgeKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCa
\param cameraName : Name of the camera.
\param camera : Copy of the camera parameters.
*/
-void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const {
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName, vpCameraParameters &camera) const
+{
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getCameraParameters(cameraName, camera);
}
@@ -853,25 +670,29 @@ void vpMbEdgeKltMultiTracker::getCameraParameters(const std::string &cameraName,
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbEdgeKltMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const {
- //Clear the input map
+void vpMbEdgeKltMultiTracker::getCameraParameters(
+ std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
+{
+ // Clear the input map
mapOfCameraParameters.clear();
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getCameraParameters(mapOfCameraParameters);
}
/*!
- Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType for the given camera name.
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType
+ for the given camera name.
\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<std::string, vpMbKltTracker *>::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
+ 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
return it->second->getClipping();
} else {
std::cerr << "Cannot find camera: " << cameraName << std::endl;
@@ -880,20 +701,24 @@ unsigned int vpMbEdgeKltMultiTracker::getClipping(const std::string &cameraName)
return vpMbTracker::getClipping();
}
-vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeKltMultiTracker::getFaces() {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getFaces()
+{
std::cerr << "Return the wrong faces reference !" << std::endl;
std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeFaces or "
- "vpMbEdgeKltMultiTracker::getKltFaces instead !" << std::endl;
+ "vpMbEdgeKltMultiTracker::getKltFaces instead !"
+ << std::endl;
return faces;
}
/*!
- Return a reference to the faces structure for the given camera name for edge trackers.
+ Return a reference to the faces structure for the given camera name for edge
+ trackers.
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName) {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getEdgeFaces(const std::string &cameraName)
+{
return vpMbEdgeMultiTracker::getFaces(cameraName);
}
@@ -902,16 +727,19 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeKltMultiTracker::getEdgeFaces(const std::
\return Reference a map of the face structure for each camera.
*/
-std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getEdgeFaces() const {
+std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getEdgeFaces() const
+{
return vpMbEdgeMultiTracker::getFaces();
}
/*!
- Return a reference to the faces structure for the given camera name for KLT trackers.
+ Return a reference to the faces structure for the given camera name for KLT
+ trackers.
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName) {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbEdgeKltMultiTracker::getKltFaces(const std::string &cameraName)
+{
return vpMbKltMultiTracker::getFaces(cameraName);
}
@@ -920,31 +748,38 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbEdgeKltMultiTracker::getKltFaces(const std::s
\return Reference a map of the face structure for each camera.
*/
-std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getKltFaces() const {
+std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbEdgeKltMultiTracker::getKltFaces() const
+{
return vpMbKltMultiTracker::getFaces();
}
-unsigned int vpMbEdgeKltMultiTracker::getNbPolygon() const {
+unsigned int vpMbEdgeKltMultiTracker::getNbPolygon() const
+{
std::cerr << "Use vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon or "
- "vpMbEdgeKltMultiTracker::getKltMultiNbPolygon instead !" << std::endl;
+ "vpMbEdgeKltMultiTracker::getKltMultiNbPolygon instead !"
+ << std::endl;
return 0;
}
/*!
- Get the number of polygons (faces) representing the object to track for the edge trackers.
+ Get the number of polygons (faces) representing the object to track for the
+ edge trackers.
\return Number of polygons.
*/
-std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const {
+std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getEdgeMultiNbPolygon() const
+{
return vpMbEdgeMultiTracker::getMultiNbPolygon();
}
/*!
- Get the number of polygons (faces) representing the object to track for the KLT trackers.
+ Get the number of polygons (faces) representing the object to track for the
+ KLT trackers.
\return Number of polygons.
*/
-std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const {
+std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getKltMultiNbPolygon() const
+{
return vpMbKltMultiTracker::getMultiNbPolygon();
}
@@ -954,8 +789,9 @@ std::map<std::string, unsigned int> vpMbEdgeKltMultiTracker::getKltMultiNbPolygo
\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 {
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const
+{
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getPose(c1Mo, c2Mo);
}
@@ -967,8 +803,9 @@ void vpMbEdgeKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMa
\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 {
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMatrix &cMo_) const
+{
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getPose(cameraName, cMo_);
}
@@ -977,203 +814,256 @@ void vpMbEdgeKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneo
\param mapOfCameraPoses : The map of camera poses for all the cameras.
*/
-void vpMbEdgeKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const {
- //Clear the map
+void vpMbEdgeKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
+{
+ // Clear the map
mapOfCameraPoses.clear();
- //We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
+ // We could use either the vpMbEdgeMultiTracker or vpMbKltMultiTracker class
vpMbEdgeMultiTracker::getPose(mapOfCameraPoses);
}
-void vpMbEdgeKltMultiTracker::init(const vpImage<unsigned char>& /*I*/) {
- if(!modelInitialised){
+void vpMbEdgeKltMultiTracker::init(const vpImage<unsigned char> & /*I*/)
+{
+ if (!modelInitialised) {
throw vpException(vpTrackingException::initializationError, "model not initialized");
}
}
#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the list points3D_list.
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are given in \e points3D_list.
- \param I : Input image
- \param points3D_list : List of the 3D points (object frame).
- \param displayFile : Path to the image used to display the help. This functionality
- is only available if visp_io module is used.
-*/
-void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char>& I, const std::vector<vpPoint> &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
+ \param I : Input image where the user has to click.
+ \param points3D_list : List of at least 4 3D points with coordinates
+ expressed in meters in the object frame. \param displayFile : Path to the
+ 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<unsigned char> &I, const std::vector<vpPoint> &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
vpMbKltMultiTracker::initClick(I, points3D_list, displayFile);
- //Set pose for Edge (setPose or initFromPose is equivalent with vpMbEdgeTracker)
- //And it avoids to click a second time
+ // Set pose for Edge (setPose or initFromPose is equivalent with
+ // vpMbEdgeTracker) And it avoids to click a second time
vpMbEdgeMultiTracker::setPose(I, cMo);
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \param I : Input image.
- \param initFile : File containing the points where to click.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \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
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
-void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp) {
- //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
+void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
+ const bool displayHelp)
+{
+ // 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
vpMbKltMultiTracker::initClick(I, initFile, displayHelp);
- //Set pose for Edge (setPose or initFromPose is equivalent with vpMbEdgeTracker)
- //And it avoids to click a second time
+ // Set pose for Edge (setPose or initFromPose is equivalent with
+ // vpMbEdgeTracker) And it avoids to click a second time
vpMbEdgeMultiTracker::setPose(I, cMo);
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param I1 : Input image for the first camera.
\param I2 : Input image for the second camera.
- \param initFile1 : File containing the points where to click for the first camera.
- \param initFile2 : File containing the points where to click for the second camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
-
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
-*/
-void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char>& I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp, const bool firstCameraIsReference) {
+ \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
+ 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
+ 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.
+
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
+
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
+*/
+void vpMbEdgeKltMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2,
+ const bool displayHelp, const bool firstCameraIsReference)
+{
vpMbKltMultiTracker::initClick(I1, I2, initFile1, initFile2, displayHelp, firstCameraIsReference);
- //Get c2Mo
+ // Get c2Mo
vpHomogeneousMatrix c2Mo;
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
++it_klt;
it_klt->second->getPose(c2Mo);
} else {
it_klt->second->getPose(c2Mo);
}
- //Set pose for Edge (setPose or initFromPose is equivalent with vpMbEdgeTracker)
+ // Set pose for Edge (setPose or initFromPose is equivalent with
+ // vpMbEdgeTracker)
vpMbEdgeMultiTracker::setPose(I1, I2, cMo, c2Mo, firstCameraIsReference);
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile for the reference camera.
- The other cameras will be automatically initialized and the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param mapOfImages : Map of images.
- \param initFile : File containing the points where to click for the reference camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \param initFile : File containing the points where to click for the
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
- \sa setPathNamePoseSaving()
*/
void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp) {
+ const std::string &initFile, const bool displayHelp)
+{
vpMbKltMultiTracker::initClick(mapOfImages, initFile, displayHelp);
- //Get pose for all the cameras
+ // Get pose for all the cameras
std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
vpMbKltMultiTracker::getPose(mapOfCameraPoses);
- //Set pose for Edge for all the cameras (setPose or initFromPose is equivalent with vpMbEdgeTracker)
- //And it avoids to click a second time
+ // Set pose for Edge for all the cameras (setPose or initFromPose is
+ // equivalent with vpMbEdgeTracker) And it avoids to click a second time
vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile.
- The cameras that have not an init file will be automatically initialized but
- the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \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 ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ The cameras that have not an init file will be automatically initialized but
+ the camera transformation matrices have to be set before.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \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
+ teabox.ppm). This image may be used to show where to click.
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
void vpMbEdgeKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp) {
+ const std::map<std::string, std::string> &mapOfInitFiles,
+ const bool displayHelp)
+{
vpMbKltMultiTracker::initClick(mapOfImages, mapOfInitFiles, displayHelp);
- //Get pose for all the cameras
+ // Get pose for all the cameras
std::map<std::string, vpHomogeneousMatrix> mapOfCameraPoses;
vpMbKltMultiTracker::getPose(mapOfCameraPoses);
- //Set pose for Edge for all the cameras (setPose or initFromPose is equivalent with vpMbEdgeTracker)
- //And it avoids to click a second time
+ // Set pose for Edge for all the cameras (setPose or initFromPose is
+ // equivalent with vpMbEdgeTracker) And it avoids to click a second time
vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}
#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;
}
-void vpMbEdgeKltMultiTracker::initCylinder(const vpPoint&, const vpPoint &, const double, const int,
- const std::string &) {
+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;
}
-void vpMbEdgeKltMultiTracker::initFaceFromCorners(vpMbtPolygon &) {
- std::cerr << "The method initFaceFromCorners is not used in vpMbEdgeKltMultiTracker !" << std::endl;
+void vpMbEdgeKltMultiTracker::initFaceFromCorners(vpMbtPolygon &)
+{
+ std::cerr << "The method initFaceFromCorners is not used in "
+ "vpMbEdgeKltMultiTracker !"
+ << std::endl;
}
-void vpMbEdgeKltMultiTracker::initFaceFromLines(vpMbtPolygon &) {
- std::cerr << "The method initFaceFromLines is not used in vpMbEdgeKltMultiTracker !" << std::endl;
+void vpMbEdgeKltMultiTracker::initFaceFromLines(vpMbtPolygon &)
+{
+ std::cerr << "The method initFaceFromLines is not used in "
+ "vpMbEdgeKltMultiTracker !"
+ << std::endl;
}
/*!
- Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
- The structure of this file is (without the comments):
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile. The structure of this file is (without the comments):
\code
// The six value of the pose vector
0.0000 // \
@@ -1184,36 +1074,37 @@ void vpMbEdgeKltMultiTracker::initFaceFromLines(vpMbtPolygon &) {
0.0000 // /
\endcode
- Where the three firsts lines refer to the translation and the three last to the rotation in thetaU
- parametrisation (see vpThetaUVector).
+ Where the three firsts lines refer to the translation and the three last to
+ the rotation in thetaU parametrisation (see vpThetaUVector).
\param I : Input image
\param initFile : Path to the file containing the pose.
*/
-void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile) {
- //Monocular case only !
- if(m_mapOfKltTrackers.size() > 1) {
+void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &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 !");
}
char s[FILENAME_MAX];
- std::fstream finit ;
+ std::fstream finit;
vpPoseVector init_pos;
std::string ext = ".pos";
- size_t pos = initFile.rfind(ext);
+ size_t pos = initFile.rfind(ext);
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
else
- sprintf(s,"%s.pos", initFile.c_str());
+ sprintf(s, "%s.pos", initFile.c_str());
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
std::cerr << "cannot read " << s << std::endl;
throw vpException(vpException::ioError, "cannot read init file");
}
- for (unsigned int i = 0; i < 6; i += 1){
+ for (unsigned int i = 0; i < 6; i += 1) {
finit >> init_pos[i];
}
@@ -1228,9 +1119,10 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, cons
\param I : Input image
\param cMo_ : Pose matrix.
*/
-void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_) {
- //Monocular case only !
- if(m_mapOfKltTrackers.size() > 1) {
+void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &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 !");
}
@@ -1245,7 +1137,8 @@ void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, cons
\param I : Input image
\param cPo : Pose vector.
*/
-void vpMbEdgeKltMultiTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo) {
+void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo)
+{
vpHomogeneousMatrix _cMo(cPo);
initFromPose(I, _cMo);
}
@@ -1257,22 +1150,27 @@ void vpMbEdgeKltMultiTracker::initFromPose (const vpImage<unsigned char>& I, con
\param I2 : Input image for the second camera.
\param c1Mo : Pose matrix for the first camera.
\param c2Mo : Pose matrix for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference
+ camera, otherwise it is the second one.
*/
-void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) {
+void vpMbEdgeKltMultiTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
vpMbEdgeMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
vpMbKltMultiTracker::initFromPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}
/*!
- Initialize the tracking thanks to the pose. The camera transformation matrices have to be set before.
+ Initialize the tracking thanks to the pose. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images.
\param cMo_ : Pose matrix for the reference camera.
*/
void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
+ const vpHomogeneousMatrix &cMo_)
+{
vpMbEdgeMultiTracker::initFromPose(mapOfImages, cMo_);
vpMbKltMultiTracker::initFromPose(mapOfImages, cMo_);
}
@@ -1284,65 +1182,71 @@ void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpI
\param mapOfCameraPoses : Map of pose matrix.
*/
void vpMbEdgeKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
vpMbEdgeMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
vpMbKltMultiTracker::initFromPose(mapOfImages, mapOfCameraPoses);
}
-unsigned int vpMbEdgeKltMultiTracker::initMbtTracking(std::vector<FeatureType> &indexOfFeatures,
- std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders,
- std::map<std::string, unsigned int> &mapOfNumberOfCircles) {
- unsigned int nbrow = 0;
- unsigned int nberrors_lines = 0;
- unsigned int nberrors_cylinders = 0;
- unsigned int nberrors_circles = 0;
-
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
- it1 != m_mapOfEdgeTrackers.end(); ++it1) {
- unsigned int nrows = 0;
- unsigned int nlines = 0;
- unsigned int ncylinders = 0;
- unsigned int ncircles = 0;
-
- nrows = it1->second->initMbtTracking(nlines, ncylinders, ncircles);
-
- mapOfNumberOfRows[it1->first] = nrows;
- mapOfNumberOfLines[it1->first] = nlines;
- mapOfNumberOfCylinders[it1->first] = ncylinders;
- mapOfNumberOfCircles[it1->first] = ncircles;
-
- nbrow += nrows;
- nberrors_lines += nlines;
- nberrors_cylinders += ncylinders;
- nberrors_circles += ncircles;
-
- for(unsigned int i = 0; i < nlines; i++) {
- indexOfFeatures.push_back(LINE);
- }
+unsigned int
+vpMbEdgeKltMultiTracker::initMbtTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ unsigned int lvl)
+{
+ vpMbEdgeTracker *edge = NULL;
+ unsigned int nbrows = 0;
- for(unsigned int i = 0; i < ncylinders; i++) {
- indexOfFeatures.push_back(CYLINDER);
- }
+ m_factor.resize(0, false);
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
+
+ try {
+ edge->computeVVSInit();
+
+ unsigned int nbrow = edge->m_error_edge.getRows();
+ nbrows += nbrow;
+
+ // Set the corresponding cMo for each camera
+ // Used in computeVVSFirstPhaseFactor with computeInteractionMatrixError
+ edge->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
+
+ edge->computeVVSFirstPhaseFactor(*mapOfImages[it->first], lvl);
+ m_factor.stack(edge->m_factor);
+ } catch (...) {
+ edge->m_L_edge.resize(0, 6, false, false);
+ edge->m_error_edge.resize(0, false);
- for(unsigned int i = 0; i < ncircles; i++) {
- indexOfFeatures.push_back(CIRCLE);
+ edge->m_weightedError_edge.resize(0, false);
+ edge->m_w_edge.resize(0, false);
+ edge->m_factor.resize(0, false);
+
+ edge->m_robustLines.resize(0);
+ edge->m_robustCylinders.resize(0);
+ edge->m_robustCircles.resize(0);
+
+ edge->m_wLines.resize(0, false);
+ edge->m_wCylinders.resize(0, false);
+ edge->m_wCircles.resize(0, false);
+
+ edge->m_errorLines.resize(0, false);
+ edge->m_errorCylinders.resize(0, false);
+ edge->m_errorCircles.resize(0, false);
}
}
- return nbrow;
+ return nbrows;
}
/*!
Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ From the configuration file initialize the parameters corresponding to the
+objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -1380,44 +1284,50 @@ unsigned int vpMbEdgeKltMultiTracker::initMbtTracking(std::vector<FeatureType> &
\sa vpXmlParser::cleanup()
*/
-void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile) {
+void vpMbEdgeKltMultiTracker::loadConfigFile(const std::string &configFile)
+{
vpMbEdgeMultiTracker::loadConfigFile(configFile);
vpMbKltMultiTracker::loadConfigFile(configFile);
}
/*!
- Load the xml configuration files for the stereo cameras case. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ Load the xml configuration files for the stereo cameras case. An example of
+ such a file is provided in loadConfigFile(const std::string &)
+ documentation. From the configuration file initialize the parameters
+ corresponding to the objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile1 : Full name of the xml file for the first camera.
\param configFile2 : Full name of the xml file for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
\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);
}
/*!
- Load the xml configuration files for all the cameras. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ Load the xml configuration files for all the cameras. An example of such a
+ file is provided in loadConfigFile(const std::string &) documentation. From
+ the configuration file initialize the parameters corresponding to the
+ objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param mapOfConfigFiles : Map of xml files.
\sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
*/
-void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles) {
+void vpMbEdgeKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
+{
vpMbEdgeMultiTracker::loadConfigFile(mapOfConfigFiles);
vpMbKltMultiTracker::loadConfigFile(mapOfConfigFiles);
}
@@ -1439,15 +1349,16 @@ int main()
}
\endcode
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
\param modelFile : the file containing the the 3D model description.
The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \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);
@@ -1455,69 +1366,66 @@ void vpMbEdgeKltMultiTracker::loadModel(const std::string &modelFile, const bool
}
void vpMbEdgeKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- vpColVector &w_mbt, vpColVector &w_klt, std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNbInfos, const unsigned int lvl) {
- //MBT
- unsigned int cpt = 0;
- for(std::map<std::string, unsigned int>::const_iterator it = mapOfNumberOfRows.begin(); it != mapOfNumberOfRows.end(); ++it) {
- for(unsigned int i = 0; i < mapOfNumberOfRows[it->first]; i++) {
- m_mapOfEdgeTrackers[it->first]->m_w[i] = w_mbt[i+cpt];
- }
+ const unsigned int lvl)
+{
+ // MBT
+ vpMbEdgeTracker *edge = NULL;
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
- m_mapOfEdgeTrackers[it->first]->updateMovingEdgeWeights();
- cpt += mapOfNumberOfRows[it->first];
- }
+ edge->updateMovingEdgeWeights();
- if(displayFeatures) {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->displayFeaturesOnImage(*mapOfImages[it->first], lvl);
+ if (displayFeatures) {
+ edge->displayFeaturesOnImage(*mapOfImages[it->first], lvl);
}
}
- //KLT
- vpMbKltMultiTracker::postTracking(mapOfImages, mapOfNbInfos, w_klt);
+ // KLT
+ vpMbKltMultiTracker::postTracking(mapOfImages);
// Looking for new visible face
bool newvisibleface = false;
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
+ edge->visibleFace(*mapOfImages[it->first], it->second->cMo, newvisibleface);
}
- if(useScanLine) {
- for(std::map<std::string, vpMbEdgeTracker*>::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());
+ if (useScanLine) {
+ for (std::map<std::string, vpMbEdgeTracker *>::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(),
+ mapOfImages[it->first]->getHeight());
}
}
- try {
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->updateMovingEdge(*mapOfImages[it->first]);
- }
- } catch(vpException &e) {
- throw e;
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
+ edge->updateMovingEdge(*mapOfImages[it->first]);
}
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- it->second->initMovingEdge(*mapOfImages[it->first], it->second->cMo);
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ edge = it->second;
+ edge->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);
+ edge->reinitMovingEdge(*mapOfImages[it->first], it->second->cMo);
- if(computeProjError) {
- it->second->computeProjectionError(*mapOfImages[it->first]);
+ if (computeProjError) {
+ edge->computeProjectionError(*mapOfImages[it->first]);
}
}
}
-void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */) {
-// vpMbEdgeMultiTracker::reinit();
+void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */)
+{
+ // vpMbEdgeMultiTracker::reinit();
vpMbKltMultiTracker::reinit();
}
@@ -1526,12 +1434,13 @@ void vpMbEdgeKltMultiTracker::reinit(/*const vpImage<unsigned char>& I */) {
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose) {
+void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &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);
}
@@ -1539,18 +1448,21 @@ void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char>& I, const
/*!
Re-initialize the model used by the tracker.
- \param I1 : The image containing the object to initialize for the first camera.
- \param I2 : The image containing the object to initialize for the second camera.
- \param cad_name : Path to the file containing the 3D model description.
- \param c1Mo : The new vpHomogeneousMatrix between the first camera and the new model.
- \param c2Mo : The new vpHomogeneousMatrix between the second camera and the new model.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param I1 : The image containing the object to initialize for the first
+ camera. \param I2 : The image containing the object to initialize for the
+ second camera. \param cad_name : Path to the file containing the 3D model
+ description. \param c1Mo : The new vpHomogeneousMatrix between the first
+ camera and the new model. \param c2Mo : The new vpHomogeneousMatrix between
+ the second camera and the new model. \param verbose : verbose option to
+ print additional information when loading CAO model files which include
+ 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<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose, const bool firstCameraIsReference) {
+ 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);
}
@@ -1560,13 +1472,16 @@ void vpMbEdgeKltMultiTracker::reInitModel(const vpImage<unsigned char> &I1, cons
\param mapOfImages : Map of images.
\param cad_name : Path to the file containing the 3D model description.
- \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras and the current object position.
- \param verbose : Verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras
+ and the current object position. \param verbose : Verbose option to print
+ additional information when loading CAO model files which include other CAO
+ model files.
*/
void vpMbEdgeKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose) {
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose)
+{
vpMbEdgeMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
vpMbKltMultiTracker::reInitModel(mapOfImages, cad_name, mapOfCameraPoses, verbose);
}
@@ -1575,7 +1490,8 @@ void vpMbEdgeKltMultiTracker::reInitModel(const std::map<std::string, const vpIm
Reset the tracker. The model is removed and the pose is set to identity.
The tracker needs to be initialized with a new model and a new pose.
*/
-void vpMbEdgeKltMultiTracker::resetTracker() {
+void vpMbEdgeKltMultiTracker::resetTracker()
+{
vpMbEdgeMultiTracker::resetTracker();
vpMbKltMultiTracker::resetTracker();
}
@@ -1589,7 +1505,8 @@ void vpMbEdgeKltMultiTracker::resetTracker() {
\param a : new angle in radian.
*/
-void vpMbEdgeKltMultiTracker::setAngleAppear(const double &a) {
+void vpMbEdgeKltMultiTracker::setAngleAppear(const double &a)
+{
vpMbEdgeMultiTracker::setAngleAppear(a);
vpMbKltMultiTracker::setAngleAppear(a);
}
@@ -1603,7 +1520,8 @@ void vpMbEdgeKltMultiTracker::setAngleAppear(const double &a) {
\param a : new angle in radian.
*/
-void vpMbEdgeKltMultiTracker::setAngleDisappear(const double &a) {
+void vpMbEdgeKltMultiTracker::setAngleDisappear(const double &a)
+{
vpMbEdgeMultiTracker::setAngleDisappear(a);
vpMbKltMultiTracker::setAngleDisappear(a);
}
@@ -1613,7 +1531,8 @@ 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);
@@ -1625,14 +1544,16 @@ void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters& came
\param camera1 : The new camera parameters for the first camera.
\param camera2 : The new camera parameters for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \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);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cam = camera1;
} else {
this->cam = camera2;
@@ -1645,11 +1566,12 @@ void vpMbEdgeKltMultiTracker::setCameraParameters(const vpCameraParameters& came
\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);
- if(cameraName == m_referenceCameraName) {
+ if (cameraName == m_referenceCameraName) {
this->cam = camera;
}
}
@@ -1659,31 +1581,36 @@ void vpMbEdgeKltMultiTracker::setCameraParameters(const std::string &cameraName,
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbEdgeKltMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters) {
+void vpMbEdgeKltMultiTracker::setCameraParameters(
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
+{
vpMbEdgeMultiTracker::setCameraParameters(mapOfCameraParameters);
vpMbKltMultiTracker::setCameraParameters(mapOfCameraParameters);
- for(std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
- it != mapOfCameraParameters.end(); ++it) {
- if(it->first == m_referenceCameraName) {
+ for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
+ it != mapOfCameraParameters.end(); ++it) {
+ if (it->first == m_referenceCameraName) {
this->cam = it->second;
}
}
}
/*!
- Set the camera transformation matrix for the specified camera (\f$ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
+ Set the camera transformation matrix for the specified camera (\f$
+ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
\param cameraName : Camera name.
- \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera.
+ \param cameraTransformationMatrix : Camera transformation matrix between the
+ current and the reference camera.
*/
void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix) {
+ const vpHomogeneousMatrix &cameraTransformationMatrix)
+{
vpMbEdgeMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
vpMbKltMultiTracker::setCameraTransformationMatrix(cameraName, cameraTransformationMatrix);
std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
- if(it != m_mapOfCameraTransformationMatrix.end()) {
+ if (it != m_mapOfCameraTransformationMatrix.end()) {
it->second = cameraTransformationMatrix;
} else {
std::cerr << "Cannot find camera: " << cameraName << " !" << std::endl;
@@ -1692,12 +1619,14 @@ void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(const std::string &c
/*!
Set the map of camera transformation matrices
- (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
+ (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1},
+ _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
\param mapOfTransformationMatrix : map of camera transformation matrices.
*/
void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(
- const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
+{
vpMbEdgeMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
vpMbKltMultiTracker::setCameraTransformationMatrix(mapOfTransformationMatrix);
@@ -1711,7 +1640,8 @@ void vpMbEdgeKltMultiTracker::setCameraTransformationMatrix(
\param flags : New clipping flags.
*/
-void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags) {
+void vpMbEdgeKltMultiTracker::setClipping(const unsigned int &flags)
+{
vpMbEdgeMultiTracker::setClipping(flags);
vpMbKltMultiTracker::setClipping(flags);
}
@@ -1724,8 +1654,9 @@ 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) {
- //Here, we do not change the general clipping flag
+void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const unsigned int &flags)
+{
+ // Here, we do not change the general clipping flag
vpMbEdgeMultiTracker::setClipping(cameraName, flags);
vpMbKltMultiTracker::setClipping(cameraName, flags);
}
@@ -1735,7 +1666,8 @@ void vpMbEdgeKltMultiTracker::setClipping(const std::string &cameraName, const u
\param flag : True if the covariance has to be computed, false otherwise
*/
-void vpMbEdgeKltMultiTracker::setCovarianceComputation(const bool& flag) {
+void vpMbEdgeKltMultiTracker::setCovarianceComputation(const bool &flag)
+{
vpMbEdgeMultiTracker::setCovarianceComputation(flag);
vpMbKltMultiTracker::setCovarianceComputation(flag);
}
@@ -1743,15 +1675,20 @@ void vpMbEdgeKltMultiTracker::setCovarianceComputation(const bool& flag) {
/*!
Enable to display the moving edges (ME) and the klt features.
- Note that if present, the moving edges can be displayed with different colors:
+ Note that if present, the moving edges can be displayed with different
+ colors:
- If green : The ME is a good point.
- - If blue : The ME is removed because of a contrast problem during the tracking phase.
- - If purple : The ME is removed because of a threshold problem during the tracking phase.
- - If red : The ME is removed because it is rejected by the robust approach in the virtual visual servoing scheme.
+ - If blue : The ME is removed because of a contrast problem during the
+ tracking phase.
+ - If purple : The ME is removed because of a threshold problem during the
+ tracking phase.
+ - If red : The ME is removed because it is rejected by the robust approach
+ in the virtual visual servoing scheme.
\param displayF : set it to true to display the features.
*/
-void vpMbEdgeKltMultiTracker::setDisplayFeatures(const bool displayF) {
+void vpMbEdgeKltMultiTracker::setDisplayFeatures(const bool displayF)
+{
vpMbEdgeMultiTracker::setDisplayFeatures(displayF);
vpMbKltMultiTracker::setDisplayFeatures(displayF);
}
@@ -1761,7 +1698,8 @@ void vpMbEdgeKltMultiTracker::setDisplayFeatures(const bool displayF) {
\param dist : Far clipping value.
*/
-void vpMbEdgeKltMultiTracker::setFarClippingDistance(const double &dist) {
+void vpMbEdgeKltMultiTracker::setFarClippingDistance(const double &dist)
+{
vpMbEdgeMultiTracker::setFarClippingDistance(dist);
vpMbKltMultiTracker::setFarClippingDistance(dist);
}
@@ -1772,102 +1710,120 @@ 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);
}
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider a
+ polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \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) {
- vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio);
- vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio);
- }
+void vpMbEdgeKltMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
+{
+ vpMbEdgeMultiTracker::setGoodNbRayCastingAttemptsRatio(ratio);
+ vpMbKltMultiTracker::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 random point inside the considered polygon.
+/*!
+ Set the number of rays that will be sent toward each polygon for visibility
+ test. Each ray will go from the optic center of the camera to a random point
+ inside the considered polygon.
- \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
+ \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
- \param attempts Number of rays to be sent.
- */
- void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) {
- vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
- vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
- }
+ \param attempts Number of rays to be sent.
+*/
+void vpMbEdgeKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
+{
+ vpMbEdgeMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
+ vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(attempts);
+}
#endif
/*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param name : name of the face we want to modify the LOD parameter.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param name : name of the face we want to modify
+ the LOD parameter.
\sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
*/
-void 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);
}
/*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param cameraName : Name of the camera we want to set the LOD.
- \param name : name of the face we want to modify the LOD parameter, if empty all the faces are considered.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param cameraName : Name of the camera we want to
+ set the LOD. \param name : name of the face we want to modify the LOD
+ parameter, if empty all the faces are considered.
\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);
}
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
+ Set the threshold for the minimum line length to be considered as visible in
+ the LOD case.
\param minLineLengthThresh : threshold for the minimum line length in pixel.
\param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinPolygonAreaThresh()
*/
-void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name) {
+void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
+{
vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, name);
}
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
+ Set the threshold for the minimum line length to be considered as visible in
+ the LOD case.
\param minLineLengthThresh : threshold for the minimum line length in pixel.
\param cameraName : name of the camera to consider.
- \param name : name of the face we want to modify the LOD threshold, if empty all the faces are considered.
+ \param name : name of the face we want to modify the LOD threshold, if empty
+ all the faces are considered.
\sa setLod(), setMinPolygonAreaThresh()
*/
void vpMbEdgeKltMultiTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &cameraName,
- const std::string &name) {
+ const std::string &name)
+{
vpMbEdgeMultiTracker::setMinLineLengthThresh(minLineLengthThresh, cameraName, name);
}
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param name : name of the face we want to modify the LOD threshold.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinLineLengthThresh()
*/
-void 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);
}
@@ -1875,14 +1831,16 @@ void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAre
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param cameraName : name of the camera to consider.
- \param name : name of the face we want to modify the LOD threshold, if empty all the faces are considered.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param cameraName : name of the camera to consider. \param name :
+ name of the face we want to modify the LOD threshold, if empty all the faces
+ are considered.
\sa setLod(), setMinLineLengthThresh()
*/
void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name) {
+ const std::string &name)
+{
vpMbEdgeMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
vpMbKltMultiTracker::setMinPolygonAreaThresh(minPolygonAreaThresh, cameraName, name);
}
@@ -1892,7 +1850,8 @@ void vpMbEdgeKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAre
\param dist : Near clipping value.
*/
-void vpMbEdgeKltMultiTracker::setNearClippingDistance(const double &dist) {
+void vpMbEdgeKltMultiTracker::setNearClippingDistance(const double &dist)
+{
vpMbEdgeMultiTracker::setNearClippingDistance(dist);
vpMbKltMultiTracker::setNearClippingDistance(dist);
}
@@ -1903,7 +1862,8 @@ 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);
}
@@ -1911,14 +1871,15 @@ void vpMbEdgeKltMultiTracker::setNearClippingDistance(const std::string &cameraN
/*!
Enable/Disable the appearance of Ogre config dialog on startup.
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
*/
-void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) {
+void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
+{
vpMbEdgeMultiTracker::setOgreShowConfigDialog(showConfigDialog);
vpMbKltMultiTracker::setOgreShowConfigDialog(showConfigDialog);
}
@@ -1926,34 +1887,35 @@ void vpMbEdgeKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialo
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
-void vpMbEdgeKltMultiTracker::setOgreVisibilityTest(const bool &v) {
- //Edge
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+void vpMbEdgeKltMultiTracker::setOgreVisibilityTest(const bool &v)
+{
+ // Edge
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setOgreVisibilityTest(v);
}
#ifdef VISP_HAVE_OGRE
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->faces.getOgreContext()->setWindowName("Multi Edge MBT Hybrid (" + it->first + ")");
}
#endif
-
- //KLT
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ // KLT
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setOgreVisibilityTest(v);
}
#ifdef VISP_HAVE_OGRE
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->faces.getOgreContext()->setWindowName("Multi KLT MBT Hybrid (" + it->first + ")");
}
#endif
@@ -1966,7 +1928,8 @@ 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);
}
@@ -1978,13 +1941,14 @@ void vpMbEdgeKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMetho
\param I : image corresponding to the desired pose.
\param cMo_ : Pose to affect.
*/
-void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_) {
- if(m_mapOfEdgeTrackers.size() != 1 || m_mapOfKltTrackers.size() != 1) {
+void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &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<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.find(m_referenceCameraName);
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
+ if (it_edge != m_mapOfEdgeTrackers.end() && it_klt != m_mapOfKltTrackers.end()) {
it_edge->second->setPose(I, cMo_);
it_klt->second->setPose(I, cMo_);
@@ -2005,10 +1969,13 @@ void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpH
\param I2 : Second image corresponding to the desired pose.
\param c1Mo : First pose to affect.
\param c2Mo : Second pose to affect.
- \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : if true, the first camera is the reference,
+ otherwise it is the second one.
*/
void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference) {
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
vpMbEdgeMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
vpMbKltMultiTracker::setPose(I1, I2, c1Mo, c2Mo, firstCameraIsReference);
}
@@ -2022,7 +1989,8 @@ void vpMbEdgeKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vp
\param cMo_ : Pose to affect to the reference camera.
*/
void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
+ const vpHomogeneousMatrix &cMo_)
+{
vpMbEdgeMultiTracker::setPose(mapOfImages, cMo_);
vpMbKltMultiTracker::setPose(mapOfImages, cMo_);
}
@@ -2030,14 +1998,16 @@ void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<
/*!
Set the pose to be used in entry of the next call to the track() function.
This pose will be just used once.
- Cameras that do not have pose will be automatically handled but the pose for the reference has to be passed in parameter.
- The camera transformation matrices have to be set before.
+ Cameras that do not have pose will be automatically handled but the pose for
+ the reference has to be passed in parameter. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images corresponding to the desired pose.
\param mapOfCameraPoses : Map of poses to affect.
*/
void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
vpMbEdgeMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
vpMbKltMultiTracker::setPose(mapOfImages, mapOfCameraPoses);
}
@@ -2045,15 +2015,17 @@ void vpMbEdgeKltMultiTracker::setPose(const std::map<std::string, const vpImage<
/*!
Set if the projection error criteria has to be computed.
- \param flag : True if the projection error criteria has to be computed, false otherwise
+ \param flag : True if the projection error criteria has to be computed,
+ false otherwise
*/
-void vpMbEdgeKltMultiTracker::setProjectionErrorComputation(const bool &flag) {
- //Set the general flag for the current class
+void vpMbEdgeKltMultiTracker::setProjectionErrorComputation(const bool &flag)
+{
+ // Set the general flag for the current class
vpMbTracker::setProjectionErrorComputation(flag);
- //Set the flag for each camera
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
+ // Set the flag for each camera
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
it->second->setProjectionErrorComputation(flag);
}
}
@@ -2063,7 +2035,8 @@ 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);
m_referenceCameraName = referenceCameraName;
@@ -2074,7 +2047,8 @@ void vpMbEdgeKltMultiTracker::setReferenceCameraName(const std::string &referenc
\param v : True to use it, False otherwise
*/
-void vpMbEdgeKltMultiTracker::setScanLineVisibilityTest(const bool &v) {
+void vpMbEdgeKltMultiTracker::setScanLineVisibilityTest(const bool &v)
+{
vpMbEdgeMultiTracker::setScanLineVisibilityTest(v);
vpMbKltMultiTracker::setScanLineVisibilityTest(v);
}
@@ -2084,11 +2058,13 @@ void vpMbEdgeKltMultiTracker::setScanLineVisibilityTest(const bool &v) {
\param th : Threshold for the weight below which a point is rejected.
*/
-void vpMbEdgeKltMultiTracker::setThresholdAcceptation(const double th) {
+void vpMbEdgeKltMultiTracker::setThresholdAcceptation(const double th)
+{
vpMbKltMultiTracker::setThresholdAcceptation(th);
}
-void vpMbEdgeKltMultiTracker::testTracking() {
+void vpMbEdgeKltMultiTracker::testTracking()
+{
std::cerr << "The method vpMbEdgeKltMultiTracker::testTracking is not used !" << std::endl;
}
@@ -2099,13 +2075,14 @@ void vpMbEdgeKltMultiTracker::testTracking() {
\param I : the input image
*/
-void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I) {
- //Track only with reference camera
- //Get the reference camera parameters
+void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I)
+{
+ // Track only with reference camera
+ // Get the reference camera parameters
std::map<std::string, vpMbEdgeTracker *>::const_iterator it_mbt = m_mapOfEdgeTrackers.find(m_referenceCameraName);
std::map<std::string, vpMbKltTracker *>::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<std::string, const vpImage<unsigned char> *> mapOfImages;
mapOfImages[m_referenceCameraName] = &I;
track(mapOfImages);
@@ -2115,8 +2092,8 @@ void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I) {
throw vpException(vpTrackingException::fatalError, ss.str().c_str());
}
- //Set the projection error from the single camera
- if(computeProjError) {
+ // Set the projection error from the single camera
+ if (computeProjError) {
projectionError = it_mbt->second->getProjectionError();
}
}
@@ -2129,11 +2106,12 @@ void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I) {
\param I1 : The first image.
\param I2 : The second image.
*/
-void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2) {
- if(m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) {
+void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
+{
+ if (m_mapOfEdgeTrackers.size() == 2 && m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
- //Assume that the first image is the first name in alphabetic order
+ // Assume that the first image is the first name in alphabetic order
mapOfImages[it->first] = &I1;
++it;
@@ -2153,103 +2131,66 @@ void vpMbEdgeKltMultiTracker::track(const vpImage<unsigned char>& I1, const vpIm
\param mapOfImages : Map of images.
*/
-void vpMbEdgeKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages) {
- //Reset the projectionError
+void vpMbEdgeKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ // Reset the projectionError
projectionError = 90.0;
- //Check if there is an image for each camera
- //mbt
- for(std::map<std::string, vpMbEdgeTracker*>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
- it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(it_edge->first);
+ // Check if there is an image for each camera
+ // mbt
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it_edge = m_mapOfEdgeTrackers.begin();
+ it_edge != m_mapOfEdgeTrackers.end(); ++it_edge) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_edge->first);
- if(it_img == mapOfImages.end()) {
+ if (it_img == mapOfImages.end()) {
throw vpException(vpTrackingException::fatalError, "Missing images for edge trackers !");
}
}
- //klt
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(it_klt->first);
+ // klt
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
- if(it_img == mapOfImages.end()) {
+ if (it_img == mapOfImages.end()) {
throw vpException(vpTrackingException::fatalError, "Missing images for KLT trackers !");
}
}
- std::map<std::string, unsigned int> mapOfNbInfos;
- std::map<std::string, unsigned int> mapOfNbFaceUsed;
-
try {
- vpMbKltMultiTracker::preTracking(mapOfImages, mapOfNbInfos, mapOfNbFaceUsed);
- } catch(/*vpException &e*/...) {
-// std::cerr << "Catch an exception in vpMbKltMultiTracker::preTracking: " << e.what() << std::endl;
+ vpMbKltMultiTracker::preTracking(mapOfImages);
+ } catch (...) {
}
- vpColVector w_klt;
-
- //MBT: track moving edges
+ // MBT: track moving edges
trackMovingEdges(mapOfImages);
- vpColVector w_mbt;
- std::map<std::string, unsigned int> mapOfNumberOfRows;
- computeVVS(mapOfImages, mapOfNumberOfRows, mapOfNbInfos, w_mbt, w_klt);
+ computeVVS(mapOfImages);
- postTracking(mapOfImages, w_mbt, w_klt, mapOfNumberOfRows, mapOfNbInfos, 0);
+ postTracking(mapOfImages, 0);
- if(computeProjError) {
+ if (computeProjError) {
vpMbEdgeMultiTracker::computeProjectionError();
}
}
-unsigned int vpMbEdgeKltMultiTracker::trackFirstLoop(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- vpColVector &factor, std::vector<FeatureType> &indexOfFeatures,
- std::map<std::string, unsigned int> &mapOfNumberOfRows,
- std::map<std::string, unsigned int> &mapOfNumberOfLines,
- std::map<std::string, unsigned int> &mapOfNumberOfCylinders,
- std::map<std::string, unsigned int> &mapOfNumberOfCircles, const unsigned int lvl) {
-
- //Number of moving edges
- unsigned int nbrow = initMbtTracking(indexOfFeatures, mapOfNumberOfRows, mapOfNumberOfLines,
- mapOfNumberOfCylinders, mapOfNumberOfCircles);
-
- if (nbrow == 0) {
- return nbrow;
- }
-
- factor = vpColVector();
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
- it != m_mapOfEdgeTrackers.end(); ++it) {
- //Set the corresponding cMo for each camera
- //Used in computeVVSFirstPhaseFactor with computeInteractionMatrixError
- it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
-
- vpColVector factor_tmp;
- factor_tmp.resize(mapOfNumberOfRows[it->first]);
- factor_tmp = 1;
- it->second->computeVVSFirstPhaseFactor(*mapOfImages[it->first], factor_tmp, lvl);
-
- factor.stack(factor_tmp);
- }
-
- return nbrow;
-}
-
-void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages) {
- for(std::map<std::string, vpMbEdgeTracker *>::const_iterator it1 = m_mapOfEdgeTrackers.begin();
- it1 != m_mapOfEdgeTrackers.end(); ++it1) {
- //Track moving edges
+void vpMbEdgeKltMultiTracker::trackMovingEdges(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ for (std::map<std::string, vpMbEdgeTracker *>::const_iterator it = m_mapOfEdgeTrackers.begin();
+ it != m_mapOfEdgeTrackers.end(); ++it) {
+ vpMbEdgeTracker *edge = it->second;
+ // Track moving edges
try {
- it1->second->trackMovingEdge(*mapOfImages[it1->first]);
- } catch(...) {
+ edge->trackMovingEdge(*mapOfImages[it->first]);
+ } catch (...) {
std::cerr << "Error in moving edge tracking" << std::endl;
- throw ;
+ throw;
}
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(dummy_vpMbEdgeKltMultiTracker.cpp.o) has no symbols
-void dummy_vpMbEdgeKltMultiTracker() {};
-#endif //VISP_HAVE_OPENCV
+// Work arround to avoid warning:
+// libvisp_mbt.a(dummy_vpMbEdgeKltMultiTracker.cpp.o) has no symbols
+void dummy_vpMbEdgeKltMultiTracker(){};
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp
index d74ddf8..0d7c5f3 100644
--- a/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp
+++ b/modules/tracker/mbt/src/hybrid/vpMbEdgeKltTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,34 +40,33 @@
//#define VP_DEBUG_MODE 1 // Activate debug level 1
#include <visp3/core/vpDebug.h>
-#include <visp3/mbt/vpMbEdgeKltTracker.h>
#include <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
vpMbEdgeKltTracker::vpMbEdgeKltTracker()
- : compute_interaction(true), lambda(0.8), thresholdKLT(2.), thresholdMBT(2.), maxIter(200)
+ : thresholdKLT(2.), thresholdMBT(2.), m_maxIterKlt(30), w_mbt(), w_klt(), m_error_hybrid(), m_w_hybrid()
{
computeCovariance = false;
-
- vpMbKltTracker::setMaxIter(30);
angleAppears = vpMath::rad(65);
angleDisappears = vpMath::rad(75);
-
+
#ifdef VISP_HAVE_OGRE
faces.getOgreContext()->setWindowName("MBT Hybrid");
#endif
+
+ m_lambda = 0.8;
+ m_maxIter = 200;
}
/*!
Basic constructor
*/
-vpMbEdgeKltTracker::~vpMbEdgeKltTracker()
-{
-}
+vpMbEdgeKltTracker::~vpMbEdgeKltTracker() {}
/*!
Initialization of the tracker using a known initial pose.
@@ -74,11 +74,10 @@ vpMbEdgeKltTracker::~vpMbEdgeKltTracker()
\param I : Input image.
*/
-void
-vpMbEdgeKltTracker::init(const vpImage<unsigned char>& I)
+void vpMbEdgeKltTracker::init(const vpImage<unsigned char> &I)
{
vpMbKltTracker::init(I);
-
+
initPyramid(I, Ipyramid);
vpMbEdgeTracker::resetMovingEdge();
@@ -86,132 +85,128 @@ vpMbEdgeKltTracker::init(const vpImage<unsigned char>& I)
unsigned int i = (unsigned int)scales.size();
do {
i--;
- if(scales[i]){
+ if (scales[i]) {
downScale(i);
initMovingEdge(*Ipyramid[i], cMo);
upScale(i);
}
- } while(i != 0);
-
+ } while (i != 0);
+
cleanPyramid(Ipyramid);
}
/*!
- Set the pose to be used in entry (as guess) of the next call to the track() function.
- This pose will be just used once.
+ Set the pose to be used in entry (as guess) of the next call to the track()
+ function. This pose will be just used once.
\warning This functionnality is not available when tracking cylinders.
\param I : image corresponding to the desired pose.
\param cdMo : Pose to affect.
*/
-void
-vpMbEdgeKltTracker::setPose( const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+void vpMbEdgeKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
- vpMbKltTracker::setPose(I, cdMo);
+ vpMbKltTracker::setPose(I, cdMo);
- resetMovingEdge();
+ resetMovingEdge();
- if(useScanLine){
- cam.computeFov(I.getWidth(), I.getHeight());
- faces.computeClippedPolygons(cMo,cam);
- faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
- }
+ if (useScanLine) {
+ cam.computeFov(I.getWidth(), I.getHeight());
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
- initPyramid(I, Ipyramid);
+ initPyramid(I, Ipyramid);
- unsigned int i = (unsigned int)scales.size();
- do {
- i--;
- if(scales[i]){
- downScale(i);
- initMovingEdge(*Ipyramid[i], cMo);
- upScale(i);
- }
- } while(i != 0);
-
- cleanPyramid(Ipyramid);
+ unsigned int i = (unsigned int)scales.size();
+ do {
+ i--;
+ if (scales[i]) {
+ downScale(i);
+ initMovingEdge(*Ipyramid[i], cMo);
+ upScale(i);
+ }
+ } while (i != 0);
+
+ cleanPyramid(Ipyramid);
}
/*!
Reset the tracker. The model is removed and the pose is set to identity.
- The tracker needs to be initialized with a new model and a new pose.
+ The tracker needs to be initialized with a new model and a new pose.
*/
-void
-vpMbEdgeKltTracker::resetTracker()
+void vpMbEdgeKltTracker::resetTracker()
{
vpMbEdgeTracker::resetTracker();
vpMbKltTracker::resetTracker();
}
-unsigned int
-vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
+unsigned int vpMbEdgeKltTracker::initMbtTracking(const unsigned int lvl)
{
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
-
- if(lvl >= scales.size() || !scales[lvl]){
+ if (lvl >= scales.size() || !scales[lvl]) {
throw vpException(vpException::dimensionError, "lvl not used.");
}
- unsigned int nbrow = 0;
- for(std::list<vpMbtDistanceLine*>::iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
+ unsigned int nbrow = 0;
+ for (std::list<vpMbtDistanceLine *>::iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ vpMbtDistanceLine *l = *it;
- if(l->isVisible() && l->isTracked()){
- nbrow += l->nbFeatureTotal ;
- l->initInteractionMatrixError() ;
+ if (l->isTracked()) {
+ l->initInteractionMatrixError();
+ nbrow += l->nbFeatureTotal;
}
}
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
- if(cy->isVisible() && cy->isTracked()){
- nbrow += cy->nbFeature ;
- cy->initInteractionMatrixError() ;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ vpMbtDistanceCylinder *cy = *it;
+
+ if (cy->isTracked()) {
+ cy->initInteractionMatrixError();
+ nbrow += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ vpMbtDistanceCircle *ci = *it;
- if(ci->isVisible() && ci ->isTracked()){
- nbrow += ci->nbFeature ;
- ci->initInteractionMatrixError() ;
+ if (ci->isTracked()) {
+ ci->initInteractionMatrixError();
+ nbrow += ci->nbFeature;
}
}
- return nbrow;
+ return nbrow;
}
/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
+ Load the xml configuration file. An example of such a file is provided in
+ loadConfigFile(const char*) documentation. From the configuration file
+ initialize the parameters corresponding to the objects: moving-edges, KLT,
+ camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile : full name of the xml file.
\sa loadConfigFile(const char*), vpXmlParser::cleanup()
*/
-void
-vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
+void vpMbEdgeKltTracker::loadConfigFile(const std::string &configFile)
{
vpMbEdgeKltTracker::loadConfigFile(configFile.c_str());
}
/*!
Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: moving-edges, KLT, camera.
+ From the configuration file initialize the parameters corresponding to the
+objects: moving-edges, KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -266,8 +261,7 @@ vpMbEdgeKltTracker::loadConfigFile(const std::string& configFile)
\sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
*/
-void
-vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
+void vpMbEdgeKltTracker::loadConfigFile(const char *configFile)
{
#ifdef VISP_HAVE_XML2
vpMbtEdgeKltXmlParser xmlp;
@@ -287,11 +281,10 @@ vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
xmlp.setPyramidLevels(3);
xmlp.setMaskBorder(maskBorder);
- try{
+ try {
std::cout << " *********** Parsing XML for Mb Edge Tracker ************ " << std::endl;
xmlp.parse(configFile);
- }
- catch(...){
+ } catch (...) {
vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
throw vpException(vpException::ioError, "problem to parse configuration file.");
}
@@ -303,13 +296,13 @@ vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
angleAppears = vpMath::rad(xmlp.getAngleAppear());
angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
- if(xmlp.hasNearClippingDistance())
+ if (xmlp.hasNearClippingDistance())
setNearClippingDistance(xmlp.getNearClippingDistance());
- if(xmlp.hasFarClippingDistance())
+ if (xmlp.hasFarClippingDistance())
setFarClippingDistance(xmlp.getFarClippingDistance());
- if(xmlp.getFovClipping()){
+ if (xmlp.getFovClipping()) {
setClipping(vpMbEdgeTracker::clippingFlag | vpPolygon3D::FOV_CLIPPING);
}
@@ -318,7 +311,7 @@ vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
applyLodSettingInConfig = false;
- if(this->getNbPolygon() > 0) {
+ if (this->getNbPolygon() > 0) {
applyLodSettingInConfig = true;
setLod(useLodGeneral);
setMinLineLengthThresh(minLineLengthThresholdGeneral);
@@ -338,7 +331,7 @@ vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
tracker.setPyramidLevels((int)xmlp.getPyramidLevels());
maskBorder = xmlp.getMaskBorder();
- //if(useScanLine)
+ // if(useScanLine)
faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
#else
@@ -349,36 +342,31 @@ vpMbEdgeKltTracker::loadConfigFile(const char* configFile)
/*!
Realize the post tracking operations. Mostly visibility tests
*/
-bool
-vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w_mbt, vpColVector &w_klt,
- const unsigned int lvl)
+bool vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char> &I, vpColVector &w_mbt, vpColVector &w_klt,
+ const unsigned int lvl)
{
-
- postTrackingMbt(w_mbt,lvl);
-
- if (displayFeatures)
- {
- if(lvl == 0){
- vpMbtDistanceLine* l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- l = *it;
- if (l->isVisible() && l->isTracked()){
+ postTrackingMbt(w_mbt, lvl);
+
+ if (displayFeatures) {
+ if (lvl == 0) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ vpMbtDistanceLine *l = *it;
+ if (l->isVisible() && l->isTracked()) {
l->displayMovingEdges(I);
}
}
-
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- cy = *it;
+
+ for (std::list<vpMbtDistanceCylinder *>::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())
+ if (cy->isTracked())
cy->displayMovingEdges(I);
}
- vpMbtDistanceCircle *ci ;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- ci = *it;
- if (ci->isVisible() && ci->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ vpMbtDistanceCircle *ci = *it;
+ if (ci->isVisible() && ci->isTracked()) {
ci->displayMovingEdges(I);
}
}
@@ -387,23 +375,23 @@ vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w
bool reInit = vpMbKltTracker::postTracking(I, w_klt);
- if(useScanLine){
+ if (useScanLine) {
cam.computeFov(I.getWidth(), I.getHeight());
- faces.computeClippedPolygons(cMo,cam);
+ faces.computeClippedPolygons(cMo, cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
vpMbEdgeTracker::updateMovingEdge(I);
-
- vpMbEdgeTracker::initMovingEdge(I, cMo) ;
+
+ vpMbEdgeTracker::initMovingEdge(I, cMo);
vpMbEdgeTracker::reinitMovingEdge(I, cMo);
- if(computeProjError)
+ if (computeProjError)
vpMbEdgeTracker::computeProjectionError(I);
- if(reInit)
+ if (reInit)
return true;
-
+
return false;
}
@@ -417,30 +405,29 @@ vpMbEdgeKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w
\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]){
+ if (lvl >= scales.size() || !scales[lvl]) {
throw vpException(vpException::dimensionError, "_lvl not used.");
}
- unsigned int n =0 ;
- vpMbtDistanceLine* l;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked()){
+ unsigned int n = 0;
+ vpMbtDistanceLine *l;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
l = *it;
unsigned int indexLine = 0;
- double wmean = 0 ;
+ double wmean = 0;
- for(unsigned int a = 0 ; a < l->meline.size() ; a++)
- {
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
std::list<vpMeSite>::iterator itListLine;
- if (l->nbFeature[a] > 0) itListLine = l->meline[a]->getMeList().begin();
+ if (l->nbFeature[a] > 0)
+ itListLine = l->meline[a]->getMeList().begin();
- for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
- wmean += w[n+indexLine] ;
+ for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
+ wmean += w[n + indexLine];
vpMeSite p = *itListLine;
- if (w[n+indexLine] < 0.5){
+ if (w[n + indexLine] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListLine = p;
}
@@ -450,10 +437,10 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
}
}
- n+= l->nbFeatureTotal ;
+ n += l->nbFeatureTotal;
- if (l->nbFeatureTotal!=0)
- wmean /= l->nbFeatureTotal ;
+ if (l->nbFeatureTotal != 0)
+ wmean /= l->nbFeatureTotal;
else
wmean = 1;
@@ -463,25 +450,26 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
l->Reinit = true;
}
}
-
+
// Same thing with cylinders as with lines
- vpMbtDistanceCylinder *cy ;
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- if((*it)->isTracked()){
+ vpMbtDistanceCylinder *cy;
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
- double wmean = 0 ;
+ double wmean = 0;
std::list<vpMeSite>::iterator itListCyl1;
std::list<vpMeSite>::iterator itListCyl2;
- if (cy->nbFeature > 0){
+ if (cy->nbFeature > 0) {
itListCyl1 = cy->meline1->getMeList().begin();
itListCyl2 = cy->meline2->getMeList().begin();
}
wmean = 0;
- for(unsigned int i=0 ; i < cy->nbFeaturel1 ; i++){
- wmean += w[n+i] ;
+ for (unsigned int i = 0; i < cy->nbFeaturel1; i++) {
+ wmean += w[n + i];
vpMeSite p = *itListCyl1;
- if (w[n+i] < 0.5){
+ if (w[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCyl1 = p;
@@ -490,22 +478,22 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
++itListCyl1;
}
- if (cy->nbFeaturel1!=0)
- wmean /= cy->nbFeaturel1 ;
+ if (cy->nbFeaturel1 != 0)
+ wmean /= cy->nbFeaturel1;
else
wmean = 1;
cy->setMeanWeight1(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
cy->Reinit = true;
}
wmean = 0;
- for(unsigned int i=cy->nbFeaturel1 ; i < cy->nbFeature ; i++){
- wmean += w[n+i] ;
+ for (unsigned int i = cy->nbFeaturel1; i < cy->nbFeature; i++) {
+ wmean += w[n + i];
vpMeSite p = *itListCyl2;
- if (w[n+i] < 0.5){
+ if (w[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCyl2 = p;
@@ -514,38 +502,38 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
++itListCyl2;
}
- if (cy->nbFeaturel2!=0)
- wmean /= cy->nbFeaturel2 ;
+ if (cy->nbFeaturel2 != 0)
+ wmean /= cy->nbFeaturel2;
else
wmean = 1;
cy->setMeanWeight2(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
cy->Reinit = true;
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
// Same thing with circles as with lines
vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
- double wmean = 0 ;
+ double wmean = 0;
std::list<vpMeSite>::iterator itListCir;
- if (ci->nbFeature > 0){
+ if (ci->nbFeature > 0) {
itListCir = ci->meEllipse->getMeList().begin();
}
wmean = 0;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- wmean += w[n+i] ;
+ for (unsigned int i = 0; i < ci->nbFeature; i++) {
+ wmean += w[n + i];
vpMeSite p = *itListCir;
- if (w[n+i] < 0.5){
+ if (w[n + i] < 0.5) {
p.setState(vpMeSite::M_ESTIMATOR);
*itListCir = p;
@@ -554,18 +542,18 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
++itListCir;
}
- if (ci->nbFeature!=0)
- wmean /= ci->nbFeature ;
+ if (ci->nbFeature != 0)
+ wmean /= ci->nbFeature;
else
wmean = 1;
ci->setMeanWeight(wmean);
- if (wmean < 0.8){
+ if (wmean < 0.8) {
ci->Reinit = true;
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
}
@@ -574,64 +562,59 @@ vpMbEdgeKltTracker::postTrackingMbt(vpColVector &w, const unsigned int lvl)
Realize the VVS loop for the tracking
\param I : current image.
- \param nbInfos : Size of the features.
- \param w_mbt : weight vector for MBT.
- \param w_klt : weight vector for KLT.
+ \param nbInfos : Size of the features (KLT).
+ \param nbrow : Size of the features (Edge).
\param lvl : level of the pyramid.
*/
-void
-vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned int &nbInfos, vpColVector &w_mbt, vpColVector &w_klt, const unsigned int lvl)
+void vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char> &I, const unsigned int &nbInfos, unsigned int &nbrow,
+ const unsigned int lvl)
{
vpColVector factor;
- unsigned int nbrow = trackFirstLoop(I, factor, lvl);
-
- if(nbrow < 4 && nbInfos < 4){
- vpERROR_TRACE("\n\t\t Error-> not enough data") ;
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
- }
- else if(nbrow < 4)
+ nbrow = trackFirstLoop(I, factor, lvl);
+
+ if (nbrow < 4 && nbInfos < 4) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
+ } else if (nbrow < 4)
nbrow = 0;
-
+
+ unsigned int totalNbRows = nbrow + 2 * nbInfos;
double residu = 0;
double residu_1 = -1;
unsigned int iter = 0;
- vpMatrix *L;
- vpMatrix L_mbt, L_klt; // interaction matrix
- vpColVector *R;
- vpColVector R_mbt, R_klt; // residu
+ vpMatrix L(totalNbRows, 6);
+ vpMatrix L_mbt, L_klt; // interaction matrix
+ vpColVector weighted_error(totalNbRows);
+ vpColVector R_mbt, R_klt; // residu
vpMatrix L_true;
vpMatrix LVJ_true;
- //vpColVector R_true;
- vpColVector w_true;
-
- if(nbrow != 0){
- L_mbt.resize(nbrow,6);
- R_mbt.resize(nbrow);
+
+ if (nbrow != 0) {
+ L_mbt.resize(nbrow, 6, false, false);
+ R_mbt.resize(nbrow, false);
}
-
- if(nbInfos != 0){
- L_klt.resize(2*nbInfos,6);
- R_klt.resize(2*nbInfos);
+
+ if (nbInfos != 0) {
+ L_klt.resize(2 * nbInfos, 6, false, false);
+ R_klt.resize(2 * nbInfos, false);
}
-
- //vpColVector w; // weight from MEstimator
- vpColVector v; // "speed" for VVS
+
+ vpColVector v; // "speed" for VVS
vpRobust robust_mbt(0), robust_klt(0);
vpHomography H;
vpMatrix LTL;
vpColVector LTR;
-
- double factorMBT = 1.0;
- double factorKLT = 1.0;
-
- //More efficient weight repartition for hybrid tracker should come soon...
-// factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
-// factorKLT = 1.0 - factorMBT;
+
+ double factorMBT; // = 1.0;
+ double factorKLT; // = 1.0;
+
+ // More efficient weight repartition for hybrid tracker should come soon...
+ // factorMBT = 1.0 - (double)nbrow / (double)(nbrow + nbInfos);
+ // factorKLT = 1.0 - factorMBT;
factorMBT = 0.35;
factorKLT = 0.65;
-
+
if (nbrow < 4)
factorKLT = 1.;
if (nbInfos < 4)
@@ -642,132 +625,132 @@ vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned i
vpHomogeneousMatrix cMoPrev;
vpHomogeneousMatrix ctTc0_Prev;
- vpColVector m_error_prev(2*nbInfos + nbrow);
- vpColVector m_w_prev(2*nbInfos + nbrow);
- double mu = 0.01;
-
- while( ((int)((residu - residu_1)*1e8) !=0 ) && (iter<maxIter) ){
- L = new vpMatrix();
- R = new vpColVector();
-
- if(nbrow >= 4)
- trackSecondLoop(I,L_mbt,R_mbt,cMo,lvl);
-
- if(nbInfos >= 4){
+ vpColVector m_error_prev;
+ vpColVector m_w_prev;
+
+ // Init size
+ m_error_hybrid.resize(totalNbRows, false);
+ m_w_hybrid.resize(totalNbRows, false);
+
+ if (nbrow != 0) {
+ w_mbt.resize(nbrow, false);
+ w_mbt = 1; // needed in vpRobust::psiTukey()
+ robust_mbt.resize(nbrow);
+ }
+
+ if (nbInfos != 0) {
+ w_klt.resize(2 * nbInfos, false);
+ w_klt = 1; // needed in vpRobust::psiTukey()
+ robust_klt.resize(2 * nbInfos);
+ }
+
+ double mu = m_initialMu;
+
+ while (((int)((residu - residu_1) * 1e8) != 0) && (iter < m_maxIter)) {
+ if (nbrow >= 4)
+ trackSecondLoop(I, L_mbt, R_mbt, cMo, lvl);
+
+ if (nbInfos >= 4) {
unsigned int shift = 0;
- vpMbtDistanceKltPoints *kltpoly;
- // for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=vpMbKltTracker::kltPolygons.begin(); it!=vpMbKltTracker::kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->hasEnoughPoints()){
- vpSubColVector subR(R_klt, shift, 2*kltpoly->getCurrentNumberPoints());
- vpSubMatrix subL(L_klt, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
+
+ for (std::list<vpMbtDistanceKltPoints *>::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);
kltpoly->computeHomography(ctTc0, H);
kltpoly->computeInteractionMatrixAndResidu(subR, subL);
- shift += 2*kltpoly->getCurrentNumberPoints();
+ shift += 2 * kltpoly->getCurrentNumberPoints();
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
-
- if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
- {
- vpSubColVector subR(R_klt, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
- vpSubMatrix subL(L_klt, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
- try{
- kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0,subR, subL);
- }catch(...){
+ for (std::list<vpMbtDistanceKltCylinder *>::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);
+ try {
+ kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
+ } catch (...) {
throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
}
- shift += 2*kltPolyCylinder->getCurrentNumberPoints();
+ shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
}
}
}
- bool reStartFromLastIncrement = false;
- if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
- if(m_error.sumSquare()/(double)(2*nbInfos + nbrow) > m_error_prev.sumSquare()/(double)(2*nbInfos + nbrow)){
- mu *= 10.0;
-
- if(mu > 1.0)
- throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
-
- cMo = cMoPrev;
- m_error = m_error_prev;
- m_w = m_w_prev;
- ctTc0 = ctTc0_Prev;
- reStartFromLastIncrement = true;
- }
+ /* residuals */
+ if (nbrow > 3) {
+ m_error_hybrid.insert(0, R_mbt);
}
- if(!reStartFromLastIncrement){
- if(iter == 0){
- m_w.resize(nbrow + 2*nbInfos);
- m_w=1;
-
- if(nbrow != 0){
- w_mbt.resize(nbrow);
- w_mbt = 1;
- robust_mbt.resize(nbrow);
- }
-
- if(nbInfos != 0){
- w_klt.resize(2*nbInfos);
- w_klt = 1;
- robust_klt.resize(2*nbInfos);
- }
+ if (nbInfos > 3) {
+ m_error_hybrid.insert(nbrow, R_klt);
+ }
- w_true.resize(nbrow + 2*nbInfos);
+ unsigned int cpt = 0;
+ while (cpt < (nbrow + 2 * nbInfos)) {
+ if (cpt < (unsigned)nbrow) {
+ m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
+ } else {
+ m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
}
+ cpt++;
+ }
+
+ bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_hybrid, m_error_prev, cMoPrev, mu, reStartFromLastIncrement,
+ &m_w_prev);
+ if (reStartFromLastIncrement) {
+ ctTc0 = ctTc0_Prev;
+ }
- /* robust */
- if(nbrow > 3){
+ if (!reStartFromLastIncrement) {
+ /* robust */
+ if (nbrow > 3) {
residuMBT = 0;
- for(unsigned int i = 0; i < R_mbt.getRows(); i++)
+ for (unsigned int i = 0; i < R_mbt.getRows(); i++)
residuMBT += fabs(R_mbt[i]);
residuMBT /= R_mbt.getRows();
- robust_mbt.setIteration(iter);
- robust_mbt.setThreshold(thresholdMBT/cam.get_px());
- robust_mbt.MEstimator( vpRobust::TUKEY, R_mbt, w_mbt);
- L->stack(L_mbt);
- R->stack(R_mbt);
+ robust_mbt.setThreshold(thresholdMBT / cam.get_px());
+ robust_mbt.MEstimator(vpRobust::TUKEY, R_mbt, w_mbt);
+
+ L.insert(L_mbt, 0, 0);
}
- if(nbInfos > 3){
+ if (nbInfos > 3) {
residuKLT = 0;
- for(unsigned int i = 0; i < R_klt.getRows(); i++)
+ for (unsigned int i = 0; i < R_klt.getRows(); i++)
residuKLT += fabs(R_klt[i]);
residuKLT /= R_klt.getRows();
- robust_klt.setIteration(iter);
- robust_klt.setThreshold(thresholdKLT/cam.get_px());
- robust_klt.MEstimator( vpRobust::TUKEY, R_klt, w_klt);
+ robust_klt.setThreshold(thresholdKLT / cam.get_px());
+ robust_klt.MEstimator(vpRobust::TUKEY, R_klt, w_klt);
- L->stack(L_klt);
- R->stack(R_klt);
+ L.insert(L_klt, nbrow, 0);
}
unsigned int cpt = 0;
- while(cpt< (nbrow+2*nbInfos)){
- if(cpt<(unsigned)nbrow){
- m_w[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT) ;
+ while (cpt < (nbrow + 2 * nbInfos)) {
+ if (cpt < (unsigned)nbrow) {
+ m_w_hybrid[cpt] = ((w_mbt[cpt] * factor[cpt]) * factorMBT);
+ } else {
+ m_w_hybrid[cpt] = (w_klt[cpt - nbrow] * factorKLT);
}
- else
- m_w[cpt] = (w_klt[cpt-nbrow] * factorKLT);
cpt++;
}
- m_error = (*R);
- if(computeCovariance){
- L_true = (*L);
- if(!isoJoIdentity){
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- LVJ_true = ((*L)*cVo*oJo);
+ if (computeCovariance) {
+ L_true = L;
+ if (!isoJoIdentity) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (L * cVo * oJo);
}
}
@@ -775,103 +758,46 @@ vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned i
residu = 0;
double num = 0;
double den = 0;
- for (unsigned int i = 0; i < static_cast<unsigned int>(R->getRows()); i++){
- num += m_w[i]*vpMath::sqr((*R)[i]);
- den += m_w[i];
-
- w_true[i] = m_w[i];
- (*R)[i] *= m_w[i];
- if(compute_interaction){
- for (unsigned int j = 0; j < 6; j += 1){
- (*L)[i][j] *= m_w[i];
+
+ for (unsigned int i = 0; i < weighted_error.getRows(); i++) {
+ num += m_w_hybrid[i] * vpMath::sqr(m_error_hybrid[i]);
+ den += m_w_hybrid[i];
+
+ weighted_error[i] = m_error_hybrid[i] * m_w_hybrid[i];
+ if (m_computeInteraction) {
+ for (unsigned int j = 0; j < 6; j += 1) {
+ L[i][j] *= m_w_hybrid[i];
}
}
}
- residu = sqrt(num/den);
-
- if(isoJoIdentity){
- LTL = L->AtA();
- computeJTR(*L, *R, LTR);
+ residu = sqrt(num / den);
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LTL.getRows(), LTL.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LTL + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
-
- if(iter != 0)
- mu /= 10.0;
-
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
- }
- }
- else{
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- vpMatrix LVJ = ((*L)*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpColVector LVJTR;
- computeJTR(LVJ, *R, LVJTR);
-
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
-
- if(iter != 0)
- mu /= 10.0;
-
- m_error_prev = m_error;
- m_w_prev = m_w;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- {
- v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- break;
- }
- }
- }
+ computeVVSPoseEstimation(isoJoIdentity, iter, L, LTL, weighted_error, m_error_hybrid, m_error_prev, LTR, mu, v,
+ &m_w_hybrid, &m_w_prev);
cMoPrev = cMo;
ctTc0_Prev = ctTc0;
ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
cMo = ctTc0 * c0Mo;
}
-
+
iter++;
-
- delete L;
- delete R;
- }
-
- if(computeCovariance){
- vpMatrix D;
- D.diag(w_true);
-
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity){
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
- }
- else{
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
- }
}
+
+ 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!");
+}
+
+void vpMbEdgeKltTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbEdgeKltTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
}
/*!
@@ -881,145 +807,146 @@ vpMbEdgeKltTracker::computeVVS(const vpImage<unsigned char>& I, const unsigned i
\param I : the input image.
*/
-void
-vpMbEdgeKltTracker::track(const vpImage<unsigned char>& I)
-{
- unsigned int nbInfos = 0;
- unsigned int nbFaceUsed = 0;
- vpColVector w_klt;
-
- try{
- vpMbKltTracker::preTracking(I, nbInfos, nbFaceUsed);
+void vpMbEdgeKltTracker::track(const vpImage<unsigned char> &I)
+{
+ try {
+ vpMbKltTracker::preTracking(I);
+ } catch (...) {
}
- catch(...){}
-
- if(nbInfos >= 4)
- vpMbKltTracker::computeVVS(nbInfos, w_klt);
- else{
- nbInfos = 0;
+
+ if (m_nbInfos >= 4) {
+ unsigned int old_maxIter = m_maxIter;
+ m_maxIter = m_maxIterKlt;
+ vpMbKltTracker::computeVVS();
+ m_maxIter = old_maxIter;
+ } else {
+ m_nbInfos = 0;
// std::cout << "[Warning] Unable to init with KLT" << std::endl;
}
-
+
vpMbEdgeTracker::trackMovingEdge(I);
-
- vpColVector w_mbt;
- computeVVS(I, nbInfos, w_mbt, w_klt);
- if(postTracking(I, w_mbt, w_klt)){
+ unsigned int nbrow = 0;
+ computeVVS(I, m_nbInfos, nbrow);
+
+ if (postTracking(I, w_mbt, w_klt)) {
vpMbKltTracker::reinit(I);
-
- // AY : Removed as edge tracked, if necessary, is reinitialized in postTracking()
-
-// initPyramid(I, Ipyramid);
-
-// unsigned int i = (unsigned int)scales.size();
-// do {
-// i--;
-// if(scales[i]){
-// downScale(i);
-// initMovingEdge(*Ipyramid[i], cMo);
-// upScale(i);
-// }
-// } while(i != 0);
-
-// cleanPyramid(Ipyramid);
+
+ // AY : Removed as edge tracked, if necessary, is reinitialized in
+ // postTracking()
+
+ // initPyramid(I, Ipyramid);
+
+ // unsigned int i = (unsigned int)scales.size();
+ // do {
+ // i--;
+ // if(scales[i]){
+ // downScale(i);
+ // initMovingEdge(*Ipyramid[i], cMo);
+ // upScale(i);
+ // }
+ // } while(i != 0);
+
+ // cleanPyramid(Ipyramid);
}
}
-unsigned int
-vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char>& I, vpColVector &factor, const unsigned int lvl)
+unsigned int vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char> &I, vpColVector &factor,
+ const unsigned int lvl)
{
- vpMbtDistanceLine *l ;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
- if(lvl >= scales.size() || !scales[lvl]){
+ if (lvl >= scales.size() || !scales[lvl]) {
throw vpException(vpException::dimensionError, "_lvl not used.");
}
- unsigned int nbrow = initMbtTracking(lvl);
-
- if (nbrow==0){
-// vpERROR_TRACE("\n\t\t Error-> not enough data in the interaction matrix...") ;
-// throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data in the interaction matrix...");
- return nbrow;
+ unsigned int nbrow = initMbtTracking(lvl);
+
+ if (nbrow == 0) {
+ // throw vpTrackingException(vpTrackingException::notEnoughPointError,
+ // "Error: not enough features in the interaction matrix...");
+ return nbrow;
}
-
- factor.resize(nbrow);
+
+ factor.resize(nbrow, false);
factor = 1;
-
+
unsigned int n = 0;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
l = *it;
l->computeInteractionMatrixError(cMo);
double fac = 1;
- for(std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex!=l->Lindex_polygon.end(); ++itindex){
+ for (std::list<int>::const_iterator itindex = l->Lindex_polygon.begin(); itindex != l->Lindex_polygon.end();
+ ++itindex) {
int index = *itindex;
if (l->hiddenface->isAppearing((unsigned int)index)) {
fac = 0.2;
break;
}
- if(l->closeToImageBorder(I, 10)){
+ if (l->closeToImageBorder(I, 10)) {
fac = 0.1;
break;
}
}
unsigned int indexFeature = 0;
- for(unsigned int a = 0 ; a < l->meline.size(); a++){
+ for (unsigned int a = 0; a < l->meline.size(); a++) {
std::list<vpMeSite>::const_iterator itListLine;
- if (l->meline[a] != NULL)
- {
+ if (l->meline[a] != NULL) {
itListLine = l->meline[a]->getMeList().begin();
- for (unsigned int i=0 ; i < l->nbFeature[a] ; i++){
- factor[n+i] = fac;
- vpMeSite site = *itListLine;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
- ++itListLine;
- indexFeature++;
+ for (unsigned int i = 0; i < l->nbFeature[a]; i++) {
+ factor[n + i] = fac;
+ vpMeSite site = *itListLine;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ factor[n + i] = 0.2;
+ ++itListLine;
+ indexFeature++;
}
- n+= l->nbFeature[a] ;
+ n += l->nbFeature[a];
}
}
}
}
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- if((*it)->isTracked()){
+
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
cy->computeInteractionMatrixError(cMo, I);
double fac = 1.0;
std::list<vpMeSite>::const_iterator itCyl1;
std::list<vpMeSite>::const_iterator itCyl2;
- if ((cy->meline1 != NULL || cy->meline2 != NULL)){
+ if ((cy->meline1 != NULL || cy->meline2 != NULL)) {
itCyl1 = cy->meline1->getMeList().begin();
itCyl2 = cy->meline2->getMeList().begin();
}
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- factor[n+i] = fac;
+ for (unsigned int i = 0; i < cy->nbFeature; i++) {
+ factor[n + i] = fac;
vpMeSite site;
- if(i<cy->nbFeaturel1) {
- site= *itCyl1;
+ if (i < cy->nbFeaturel1) {
+ site = *itCyl1;
++itCyl1;
- }
- else{
- site= *itCyl2;
+ } else {
+ site = *itCyl2;
++itCyl2;
}
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ factor[n + i] = 0.2;
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
ci->computeInteractionMatrixError(cMo);
double fac = 1.0;
@@ -1029,68 +956,69 @@ vpMbEdgeKltTracker::trackFirstLoop(const vpImage<unsigned char>& I, vpColVector
itCir = ci->meEllipse->getMeList().begin();
}
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- factor[n+i] = fac;
+ for (unsigned int i = 0; i < ci->nbFeature; i++) {
+ factor[n + i] = fac;
vpMeSite site = *itCir;
- if (site.getState() != vpMeSite::NO_SUPPRESSION) factor[n+i] = 0.2;
+ if (site.getState() != vpMeSite::NO_SUPPRESSION)
+ factor[n + i] = 0.2;
++itCir;
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
-
+
return nbrow;
}
-void
-vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &L, vpColVector &error,
- vpHomogeneousMatrix& cMo_, const unsigned int lvl)
+void vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char> &I, vpMatrix &L, vpColVector &error,
+ vpHomogeneousMatrix &cMo_, const unsigned int lvl)
{
- vpMbtDistanceLine* l;
- vpMbtDistanceCylinder *cy ;
- vpMbtDistanceCircle *ci ;
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
- unsigned int n = 0 ;
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[lvl].begin(); it!=lines[lvl].end(); ++it){
- if((*it)->isTracked()){
+ unsigned int n = 0;
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[lvl].begin(); it != lines[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
l = *it;
- l->computeInteractionMatrixError(cMo_) ;
- for (unsigned int i=0 ; i < l->nbFeatureTotal ; i++){
- for (unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = l->L[i][j];
- error[n+i] = l->error[i];
+ l->computeInteractionMatrixError(cMo_);
+ for (unsigned int i = 0; i < l->nbFeatureTotal; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ L[n + i][j] = l->L[i][j];
+ error[n + i] = l->error[i];
}
}
- n+= l->nbFeatureTotal;
+ n += l->nbFeatureTotal;
}
}
-
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[lvl].begin(); it!=cylinders[lvl].end(); ++it){
- if((*it)->isTracked()){
+
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[lvl].begin(); it != cylinders[lvl].end();
+ ++it) {
+ if ((*it)->isTracked()) {
cy = *it;
- cy->computeInteractionMatrixError(cMo_, I) ;
- for(unsigned int i=0 ; i < cy->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = cy->L[i][j];
- error[n+i] = cy->error[i];
+ cy->computeInteractionMatrixError(cMo_, I);
+ for (unsigned int i = 0; i < cy->nbFeature; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ L[n + i][j] = cy->L[i][j];
+ error[n + i] = cy->error[i];
}
}
- n+= cy->nbFeature ;
+ n += cy->nbFeature;
}
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[lvl].begin(); it!=circles[lvl].end(); ++it){
- if((*it)->isTracked()){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[lvl].begin(); it != circles[lvl].end(); ++it) {
+ if ((*it)->isTracked()) {
ci = *it;
- ci->computeInteractionMatrixError(cMo_) ;
- for(unsigned int i=0 ; i < ci->nbFeature ; i++){
- for(unsigned int j=0; j < 6 ; j++){
- L[n+i][j] = ci->L[i][j];
- error[n+i] = ci->error[i];
+ ci->computeInteractionMatrixError(cMo_);
+ for (unsigned int i = 0; i < ci->nbFeature; i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ L[n + i][j] = ci->L[i][j];
+ error[n + i] = ci->error[i];
}
}
- n+= ci->nbFeature ;
+ n += ci->nbFeature;
}
}
}
@@ -1100,11 +1028,10 @@ vpMbEdgeKltTracker::trackSecondLoop(const vpImage<unsigned char>& I, vpMatrix &
\param camera : the new camera parameters
*/
-void
-vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters& camera)
+void vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters &camera)
{
this->cam = camera;
-
+
vpMbEdgeTracker::setCameraParameters(cam);
vpMbKltTracker::setCameraParameters(cam);
}
@@ -1112,10 +1039,10 @@ vpMbEdgeKltTracker::setCameraParameters(const vpCameraParameters& camera)
/*!
Initialise a new face from the coordinates given in parameter.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+void vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
{
vpMbEdgeTracker::initFaceFromCorners(polygon);
vpMbKltTracker::initFaceFromCorners(polygon);
@@ -1123,29 +1050,27 @@ vpMbEdgeKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
/*!
Initialise a new face from the coordinates given in parameter.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
+void vpMbEdgeKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
{
vpMbEdgeTracker::initFaceFromLines(polygon);
vpMbKltTracker::initFaceFromLines(polygon);
}
/*!
- Add a circle to track from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
+ Add a circle to track from its center, 3 points (including the center)
+ defining the plane that contain the circle and its radius.
\param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param idFace : Id of the face associated to the circle.
- \param name : The optional name of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param radius : Radius of the circle. \param idFace : Id of the face
+ associated to the circle. \param name : The optional name of the circle.
*/
-void
-vpMbEdgeKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int idFace, const std::string &name)
+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);
}
@@ -1160,9 +1085,8 @@ vpMbEdgeKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoi
\param idFace : Id of the face associated to the cylinder.
\param name : The optional name of the cylinder.
*/
-void
-vpMbEdgeKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
- const std::string &name)
+void vpMbEdgeKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string &name)
{
vpMbEdgeTracker::initCylinder(p1, p2, radius, idFace, name);
vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
@@ -1176,47 +1100,50 @@ vpMbEdgeKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const dou
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
+ \param displayFullModel : boolean to say if all the model has to be
+ displayed, even the faces that are not visible.
*/
-void
-vpMbEdgeKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel)
+void vpMbEdgeKltTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- break ; //displaying model on one scale only
+ break; // displaying model on one scale only
}
}
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
kltPolyCylinder->displayPrimitive(I);
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
@@ -1229,47 +1156,50 @@ vpMbEdgeKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneous
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : boolean to say if all the model has to be displayed, even the faces that are not visible.
+ \param displayFullModel : boolean to say if all the model has to be
+ displayed, even the faces that are not visible.
*/
-void
-vpMbEdgeKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &camera,
- const vpColor& col , const unsigned int thickness, const bool displayFullModel)
+void vpMbEdgeKltTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[scaleLevel].begin(); it!=lines[scaleLevel].end(); ++it){
- (*it)->display(I,cMo_, camera, col, thickness, displayFullModel);
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin(); it != lines[scaleLevel].end();
+ ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[scaleLevel].begin(); it!=cylinders[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[scaleLevel].begin(); it!=circles[scaleLevel].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
- break ; //displaying model on one scale only
+ break; // displaying model on one scale only
}
}
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
- kltpoly->displayPrimitive(I);
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if(displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
kltPolyCylinder->displayPrimitive(I);
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
@@ -1279,13 +1209,12 @@ vpMbEdgeKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
{
reInitModel(I, cad_name.c_str(), cMo_, verbose);
}
@@ -1295,51 +1224,49 @@ vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::stri
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char> &I, const char *cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
{
- // Reinit klt
+// Reinit klt
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
+ if (cur != NULL) {
cvReleaseImage(&cur);
cur = NULL;
}
#endif
// delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly != NULL) {
+ delete kltpoly;
}
- kltpoly = NULL ;
+ kltpoly = NULL;
}
kltPolygons.clear();
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if (kltPolyCylinder!=NULL){
- delete kltPolyCylinder ;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder != NULL) {
+ delete kltPolyCylinder;
}
- kltPolyCylinder = NULL ;
+ kltPolyCylinder = NULL;
}
kltCylinders.clear();
// delete the structures used to display circles
vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
ci = *it;
- if (ci!=NULL){
- delete ci ;
+ if (ci != NULL) {
+ delete ci;
}
- ci = NULL ;
+ ci = NULL;
}
circles_disp.clear();
@@ -1350,23 +1277,27 @@ vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad
vpMbtDistanceLine *l;
vpMbtDistanceCylinder *cy;
- for (unsigned int i = 0; i < scales.size(); i += 1){
- if(scales[i]){
- for(std::list<vpMbtDistanceLine*>::const_iterator it=lines[i].begin(); it!=lines[i].end(); ++it){
+ for (unsigned int i = 0; i < scales.size(); i += 1) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
l = *it;
- if (l!=NULL) delete l ;
- l = NULL ;
+ if (l != NULL)
+ delete l;
+ l = NULL;
}
- for(std::list<vpMbtDistanceCylinder*>::const_iterator it=cylinders[i].begin(); it!=cylinders[i].end(); ++it){
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
cy = *it;
- if (cy!=NULL) delete cy;
+ if (cy != NULL)
+ delete cy;
cy = NULL;
}
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles[i].begin(); it!=circles[i].end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
ci = *it;
- if (ci!=NULL) delete ci;
+ if (ci != NULL)
+ delete ci;
ci = NULL;
}
@@ -1376,11 +1307,11 @@ vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad
}
}
- //compute_interaction=1;
+ // compute_interaction=1;
nline = 0;
ncylinder = 0;
ncircle = 0;
- //lambda = 1;
+ // lambda = 1;
nbvisiblepolygone = 0;
// Reinit common parts
@@ -1393,6 +1324,7 @@ vpMbEdgeKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has no symbols
-void dummy_vpMbEdgeKltTracker() {};
-#endif //VISP_HAVE_OPENCV
+// Work arround to avoid warning: libvisp_mbt.a(vpMbEdgeKltTracker.cpp.o) has
+// no symbols
+void dummy_vpMbEdgeKltTracker(){};
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp
index 46b8ed3..b5c932a 100644
--- a/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp
+++ b/modules/tracker/mbt/src/hybrid/vpMbtEdgeKltXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Load XML parameters of the Model based tracker (using edges and point features).
+ * Load XML parameters of the Model based tracker (using edges and point
+ *features).
*
* Authors:
* Aurelien Yol
@@ -36,38 +38,30 @@
*****************************************************************************/
#include <visp3/core/vpConfig.h>
-
#ifdef VISP_HAVE_XML2
#include <iostream>
#include <map>
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbtEdgeKltXmlParser.h>
-
/*!
- Default constructor.
-
+ Default constructor.
+
*/
-vpMbtEdgeKltXmlParser::vpMbtEdgeKltXmlParser()
-{
- init();
-}
+vpMbtEdgeKltXmlParser::vpMbtEdgeKltXmlParser() { init(); }
/*!
Default destructor.
*/
-vpMbtEdgeKltXmlParser::~vpMbtEdgeKltXmlParser()
-{
-}
+vpMbtEdgeKltXmlParser::~vpMbtEdgeKltXmlParser() {}
/*!
Initialise internal variables (including the map).
*/
-void
-vpMbtEdgeKltXmlParser::init()
+void vpMbtEdgeKltXmlParser::init()
{
vpMbtXmlParser::init();
vpMbtKltXmlParser::init();
@@ -82,11 +76,10 @@ vpMbtEdgeKltXmlParser::init()
/*!
Parse the file in parameters.
This method is deprecated, use parse() instead.
-
+
\param filename : File to parse.
*/
-void
-vpMbtEdgeKltXmlParser::parse(const char * filename)
+void vpMbtEdgeKltXmlParser::parse(const char *filename)
{
std::string file = filename;
vpXmlParser::parse(file);
@@ -94,101 +87,100 @@ vpMbtEdgeKltXmlParser::parse(const char * filename)
/*!
Write info to file.
-
+
\warning Useless, so not yet implemented => Throw exception.
*/
-void
-vpMbtEdgeKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
+void vpMbtEdgeKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
+ throw vpException(vpException::notImplementedError, "Not yet implemented.");
}
/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
\param doc : Document to parse.
- \param node : Root node.
+ \param node : Root node.
*/
-void
-vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+void vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
bool camera_node = false;
bool face_node = false;
bool ecm_node = false;
bool klt_node = false;
bool lod_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case vpMbtEdgeKltXmlParser::camera:{
- this->read_camera (doc, dataNode);
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case vpMbtEdgeKltXmlParser::camera: {
+ this->read_camera(doc, dataNode);
camera_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::face:{
+ } break;
+ case vpMbtEdgeKltXmlParser::face: {
this->read_face(doc, dataNode);
face_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::klt:{
+ } break;
+ case vpMbtEdgeKltXmlParser::klt: {
this->read_klt(doc, dataNode);
klt_node = true;
- }break;
- case vpMbtEdgeKltXmlParser::ecm:{
- this->read_ecm (doc, dataNode);
+ } break;
+ case vpMbtEdgeKltXmlParser::ecm: {
+ this->read_ecm(doc, dataNode);
ecm_node = true;
- }break;
- case sample:{
- this->read_sample_deprecated (doc, dataNode);
- }break;
- case vpMbtEdgeKltXmlParser::lod:{
+ } break;
+ case sample: {
+ this->read_sample_deprecated(doc, dataNode);
+ } break;
+ case vpMbtEdgeKltXmlParser::lod: {
this->read_lod(doc, dataNode);
lod_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+
+ if (!camera_node) {
+ std::cout << "camera : u0 : " << this->cam.get_u0() << " (default)" << std::endl;
+ std::cout << "camera : v0 : " << this->cam.get_v0() << " (default)" << std::endl;
+ std::cout << "camera : px : " << this->cam.get_px() << " (default)" << std::endl;
+ std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl;
}
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+
+ if (!face_node) {
+ std::cout << "face : Angle Appear : " << angleAppear << " (default)" << std::endl;
+ std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl;
}
- if(!klt_node) {
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ if (!klt_node) {
+ std::cout << "klt : Mask Border : " << maskBorder << " (default)" << std::endl;
+ std::cout << "klt : Max Features : " << maxFeatures << " (default)" << std::endl;
+ std::cout << "klt : Windows Size : " << winSize << " (default)" << std::endl;
+ std::cout << "klt : Quality : " << qualityValue << " (default)" << std::endl;
+ std::cout << "klt : Min Distance : " << minDist << " (default)" << std::endl;
+ std::cout << "klt : Harris Parameter : " << harrisParam << " (default)" << std::endl;
+ std::cout << "klt : Block Size : " << blockSize << " (default)" << std::endl;
+ std::cout << "klt : Pyramid Levels : " << pyramidLevels << " (default)" << std::endl;
}
- if(!ecm_node) {
- std::cout << "ecm : mask : size : "<< this->m_ecm.getMaskSize() << " (default)" <<std::endl;
- std::cout << "ecm : mask : nb_mask : "<< this->m_ecm.getMaskNumber() << " (default)" <<std::endl;
- std::cout <<"ecm : range : tracking : "<< this->m_ecm.getRange()<< " (default)" <<std::endl;
- std::cout <<"ecm : contrast : threshold : " << this->m_ecm.getThreshold()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu1 : " << this->m_ecm.getMu1()<<" (default)" <<std::endl;
- std::cout <<"ecm : contrast : mu2 : " << this->m_ecm.getMu2()<<" (default)" <<std::endl;
- std::cout <<"ecm : sample : sample_step : "<< this->m_ecm.getSampleStep()<< " (default)" << std::endl;
+ if (!ecm_node) {
+ std::cout << "ecm : mask : size : " << this->m_ecm.getMaskSize() << " (default)" << std::endl;
+ std::cout << "ecm : mask : nb_mask : " << this->m_ecm.getMaskNumber() << " (default)" << std::endl;
+ std::cout << "ecm : range : tracking : " << this->m_ecm.getRange() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : threshold : " << this->m_ecm.getThreshold() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu1 : " << this->m_ecm.getMu1() << " (default)" << std::endl;
+ std::cout << "ecm : contrast : mu2 : " << this->m_ecm.getMu2() << " (default)" << std::endl;
+ std::cout << "ecm : sample : sample_step : " << this->m_ecm.getSampleStep() << " (default)" << std::endl;
}
- if(!lod_node) {
+ 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;
@@ -196,7 +188,7 @@ vpMbtEdgeKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbtEdgeKltXmlParser.cpp.o) has no symbols
-void dummy_vpMbtEdgeKltXmlParser() {};
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtEdgeKltXmlParser.cpp.o)
+// has no symbols
+void dummy_vpMbtEdgeKltXmlParser(){};
#endif
-
diff --git a/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp
index 55dcd6f..4988282 100644
--- a/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp
+++ b/modules/tracker/mbt/src/klt/vpMbKltMultiTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2016 by INRIA. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -13,7 +14,7 @@
* GPL, please contact INRIA about acquiring a ViSP Professional
* Edition License.
*
- * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
+ * See https://visp.inria.fr/download/ for more information.
*
* This software was developed at:
* INRIA Rennes - Bretagne Atlantique
@@ -44,21 +45,22 @@
#include <visp3/core/vpConfig.h>
-#if (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 <visp3/core/vpTrackingException.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/mbt/vpMbKltMultiTracker.h>
-
/*!
Basic constructor
*/
-vpMbKltMultiTracker::vpMbKltMultiTracker() : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(),
- m_referenceCameraName("Camera") {
+vpMbKltMultiTracker::vpMbKltMultiTracker()
+ : m_mapOfCameraTransformationMatrix(), m_mapOfKltTrackers(), m_referenceCameraName("Camera"), m_L_kltMulti(),
+ m_error_kltMulti(), m_w_kltMulti(), m_weightedError_kltMulti()
+{
m_mapOfKltTrackers["Camera"] = new vpMbKltTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
}
@@ -67,37 +69,39 @@ vpMbKltMultiTracker::vpMbKltMultiTracker() : m_mapOfCameraTransformationMatrix()
\param nbCameras : Number of cameras to use.
*/
-vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras) : m_mapOfCameraTransformationMatrix(),
- m_mapOfKltTrackers(), m_referenceCameraName("Camera") {
+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()
+{
- if(nbCameras == 0) {
+ if (nbCameras == 0) {
throw vpException(vpTrackingException::fatalError, "Cannot construct a vpMbkltMultiTracker with no camera !");
- } else if(nbCameras == 1) {
+ } else if (nbCameras == 1) {
m_mapOfKltTrackers["Camera"] = new vpMbKltTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
- } else if(nbCameras == 2) {
+ } else if (nbCameras == 2) {
m_mapOfKltTrackers["Camera1"] = new vpMbKltTracker();
m_mapOfKltTrackers["Camera2"] = new vpMbKltTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
m_mapOfCameraTransformationMatrix["Camera2"] = vpHomogeneousMatrix();
- //Set by default the reference camera
+ // Set by default the reference camera
m_referenceCameraName = "Camera1";
} else {
- for(unsigned int i = 1; i <= nbCameras; i++) {
+ for (unsigned int i = 1; i <= nbCameras; i++) {
std::stringstream ss;
ss << "Camera" << i;
m_mapOfKltTrackers[ss.str()] = new vpMbKltTracker();
- //Add default camera transformation matrix
+ // Add default camera transformation matrix
m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
}
- //Set by default the reference camera to the first one
+ // Set by default the reference camera to the first one
m_referenceCameraName = m_mapOfKltTrackers.begin()->first;
}
}
@@ -107,26 +111,29 @@ vpMbKltMultiTracker::vpMbKltMultiTracker(const unsigned int nbCameras) : m_mapOf
\param cameraNames : List of camera names.
*/
-vpMbKltMultiTracker::vpMbKltMultiTracker(const std::vector<std::string> &cameraNames) : m_mapOfCameraTransformationMatrix(),
- m_mapOfKltTrackers(), m_referenceCameraName("Camera") {
- if(cameraNames.empty()) {
+vpMbKltMultiTracker::vpMbKltMultiTracker(const std::vector<std::string> &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 !");
}
- for(std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
+ for (std::vector<std::string>::const_iterator it = cameraNames.begin(); it != cameraNames.end(); ++it) {
m_mapOfKltTrackers[*it] = new vpMbKltTracker();
}
- //Set by default the reference camera
+ // Set by default the reference camera
m_referenceCameraName = cameraNames.front();
}
/*!
Basic destructor useful to deallocate the memory.
*/
-vpMbKltMultiTracker::~vpMbKltMultiTracker() {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+vpMbKltMultiTracker::~vpMbKltMultiTracker()
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
delete it->second;
}
}
@@ -135,140 +142,159 @@ vpMbKltMultiTracker::~vpMbKltMultiTracker() {
Add a circle to the list of circles.
\param P1 : Center of the circle.
- \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
- \param name : Name of the circle.
+ \param P2,P3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param r : Radius of the circle. \param name : Name of the circle.
*/
-void
-vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name)
+void vpMbKltMultiTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
+ const std::string &name)
{
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->addCircle(P1, P2, P3, r, name);
}
}
-void vpMbKltMultiTracker::computeVVS(std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w) {
- vpMatrix L; // interaction matrix
- vpColVector R; // residu
- vpMatrix L_true; // interaction matrix
+void vpMbKltMultiTracker::computeVVS()
+{
+ vpMatrix L_true;
vpMatrix LVJ_true;
- //vpColVector R_true; // residu
- vpColVector w_true;
- vpColVector v; // "speed" for VVS
-
- unsigned int nbInfos = 0;
- for(std::map<std::string, unsigned int>::const_iterator it = mapOfNbInfos.begin(); it != mapOfNbInfos.end(); ++it) {
- nbInfos += it->second;
- }
-
- std::map<std::string, vpRobust> mapOfRobusts;
-
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- mapOfRobusts[it->first] = vpRobust(2*mapOfNbInfos[it->first]);
- }
+ vpColVector v;
vpMatrix LTL;
vpColVector LTR;
vpHomogeneousMatrix cMoPrev;
vpHomogeneousMatrix ctTc0_Prev;
- vpColVector error_prev(2*nbInfos);
- double mu = 0.01;
+ vpColVector error_prev;
+ double mu = m_initialMu;
double normRes = 0;
double normRes_1 = -1;
unsigned int iter = 0;
+ computeVVSInit();
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
+ klt->computeVVSInit();
+ }
+
std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
- for(std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
- it != m_mapOfCameraTransformationMatrix.end(); ++it) {
+ for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
+ it != m_mapOfCameraTransformationMatrix.end(); ++it) {
vpVelocityTwistMatrix cVo;
cVo.buildFrom(it->second);
mapOfVelocityTwist[it->first] = cVo;
}
- while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ) {
- L.resize(0,0);
- R.resize(0);
+ while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
+ computeVVSInteractionMatrixAndResidu(mapOfVelocityTwist);
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it1 = m_mapOfKltTrackers.begin();
- it1 != m_mapOfKltTrackers.end(); ++it1) {
- unsigned int shift = 0;
- vpColVector R_current; // residu
- vpMatrix L_current; // interaction matrix
- vpHomography H_current;
+ bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_kltMulti, error_prev, cMoPrev, mu, reStartFromLastIncrement);
+ if (reStartFromLastIncrement) {
+ ctTc0 = ctTc0_Prev;
+ }
- R_current.resize(2 * mapOfNbInfos[it1->first]);
- L_current.resize(2 * mapOfNbInfos[it1->first], 6, 0);
+ if (!reStartFromLastIncrement) {
+ vpMbKltMultiTracker::computeVVSWeights();
- //Use the ctTc0 variable instead of the formula in the monocular case
- //to ensure that we have the same result than vpMbKltTracker
- //as some slight differences can occur due to numerical imprecision
- if(m_mapOfKltTrackers.size() == 1) {
- computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
- it1->second->kltPolygons, it1->second->kltCylinders, ctTc0);
- } else {
- vpHomogeneousMatrix c_curr_tTc_curr0 = m_mapOfCameraTransformationMatrix[it1->first] *
- cMo * it1->second->c0Mo.inverse();
- computeVVSInteractionMatrixAndResidu(shift, R_current, L_current, H_current,
- it1->second->kltPolygons, it1->second->kltCylinders, c_curr_tTc_curr0);
+ if (computeCovariance) {
+ L_true = m_L_kltMulti;
+ if (!isoJoIdentity) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_kltMulti * cVo * oJo);
+ }
}
- //VelocityTwistMatrix
- L_current = L_current*mapOfVelocityTwist[it1->first];
+ normRes_1 = normRes;
+ normRes = 0.0;
- //Stack residu and interaction matrix
- R.stack(R_current);
- L.stack(L_current);
- }
+ for (unsigned int i = 0; i < m_weightedError_kltMulti.getRows(); i++) {
+ m_weightedError_kltMulti[i] = m_error_kltMulti[i] * m_w_kltMulti[i];
+ normRes += m_weightedError_kltMulti[i];
+ }
- bool reStartFromLastIncrement = false;
- computeVVSCheckLevenbergMarquardtKlt(iter, nbInfos, cMoPrev, error_prev, ctTc0_Prev, mu, reStartFromLastIncrement);
+ if ((iter == 0) || m_computeInteraction) {
+ for (unsigned int i = 0; i < m_L_kltMulti.getRows(); i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_kltMulti[i][j] *= m_w_kltMulti[i];
+ }
+ }
+ }
- if(!reStartFromLastIncrement) {
- vpMbKltMultiTracker::computeVVSWeights(iter, nbInfos, mapOfNbInfos, R, w_true, w, mapOfRobusts, 2.0);
+ computeVVSPoseEstimation(isoJoIdentity, iter, m_L_kltMulti, LTL, m_weightedError_kltMulti, m_error_kltMulti,
+ error_prev, LTR, mu, v);
- computeVVSPoseEstimation(iter, L, w, L_true, LVJ_true, normRes, normRes_1, w_true, R, LTL, LTR,
- error_prev, v, mu, cMoPrev, ctTc0_Prev);
+ cMoPrev = cMo;
+ ctTc0_Prev = ctTc0;
+ ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
+ cMo = ctTc0 * c0Mo;
} // endif(!reStartFromLastIncrement)
iter++;
}
- computeVVSCovariance(w_true, cMoPrev, L_true, LVJ_true);
+ computeCovarianceMatrixVVS(isoJoIdentity, m_w_kltMulti, cMoPrev, L_true, LVJ_true, m_error_kltMulti);
}
-void vpMbKltMultiTracker::computeVVSWeights(const unsigned int iter, const unsigned int nbInfos,
- std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &R, vpColVector &w_true, vpColVector &w,
- std::map<std::string, vpRobust> &mapOfRobusts, double threshold) {
- /* robust */
- if(iter == 0) {
- w_true.resize(2*nbInfos);
- w.resize(2*nbInfos);
- w = 1;
- w_true = 1;
+void vpMbKltMultiTracker::computeVVSInit()
+{
+ unsigned int nbFeatures = 2 * m_nbInfos;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- vpCameraParameters camTmp;
- it->second->getCameraParameters(camTmp);
- mapOfRobusts[it->first].setThreshold(threshold / camTmp.get_px());
+ m_L_kltMulti.resize(nbFeatures, 6, false, false);
+ m_w_kltMulti.resize(nbFeatures, false);
+ m_error_kltMulti.resize(nbFeatures, false);
+ m_weightedError_kltMulti.resize(nbFeatures, false);
+}
+
+void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbKltMultiTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
+}
+
+void vpMbKltMultiTracker::computeVVSInteractionMatrixAndResidu(
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
+{
+ unsigned int startIdx = 0;
+
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
+
+ // Use the ctTc0 variable instead of the formula in the monocular case
+ // to ensure that we have the same result than vpMbKltTracker
+ // as some slight differences can occur due to numerical imprecision
+ if (m_mapOfKltTrackers.size() == 1) {
+ klt->ctTc0 = ctTc0;
+ klt->computeVVSInteractionMatrixAndResidu();
+ } else {
+ vpHomogeneousMatrix c_curr_tTc_curr0 =
+ 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);
+
+ startIdx += klt->m_error_klt.getRows();
}
+}
- unsigned int shift = 0;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- if(mapOfNbInfos[it->first] > 0) {
- vpSubColVector sub_w(w, shift, 2*mapOfNbInfos[it->first]);
- vpSubColVector sub_r(R, shift, 2*mapOfNbInfos[it->first]);
- shift += 2*mapOfNbInfos[it->first];
+void vpMbKltMultiTracker::computeVVSWeights()
+{
+ unsigned int startIdx = 0;
+ for (std::map<std::string, vpMbKltTracker *>::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);
- mapOfRobusts[it->first].setIteration(iter);
- mapOfRobusts[it->first].MEstimator(vpRobust::TUKEY, sub_r, sub_w);
- }
+ m_w_kltMulti.insert(startIdx, klt->m_w_klt);
+ startIdx += klt->m_w_klt.getRows();
}
}
@@ -280,12 +306,15 @@ void vpMbKltMultiTracker::computeVVSWeights(const unsigned int iter, const unsig
\param cam_ : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbKltMultiTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_,
- const vpCameraParameters &cam_, const vpColor& col , const unsigned int thickness, const bool displayFullModel) {
+void vpMbKltMultiTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ 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;
@@ -300,12 +329,15 @@ void vpMbKltMultiTracker::display(const vpImage<unsigned char>& I, const vpHomog
\param cam_ : The camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbKltMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters &cam_,
- const vpColor& color , const unsigned int thickness, const bool displayFullModel) {
+void vpMbKltMultiTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &color, const unsigned int thickness,
+ const bool displayFullModel)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ 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;
@@ -323,20 +355,23 @@ void vpMbKltMultiTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousM
\param cam2 : The second camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbKltMultiTracker::display(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& 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) {
+void vpMbKltMultiTracker::display(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &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<std::string, vpMbKltTracker *>::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;
}
}
@@ -351,20 +386,23 @@ void vpMbKltMultiTracker::display(const vpImage<unsigned char>& I1, const vpImag
\param cam2 : The second camera parameters.
\param color : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
-void vpMbKltMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<vpRGBa>& 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) {
+void vpMbKltMultiTracker::display(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, vpMbKltTracker *>::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;
}
}
@@ -376,21 +414,24 @@ void vpMbKltMultiTracker::display(const vpImage<vpRGBa>& I1, const vpImage<vpRGB
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
- //Display only for the given images
- for(std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
- it_img != mapOfImages.end(); ++it_img) {
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
- if(it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) {
+ 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);
} else {
std::cerr << "Missing elements ! " << std::endl;
@@ -406,21 +447,24 @@ void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<unsi
\param mapOfCameraParameters : Map of camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : If true, the full model is displayed (even the non visible faces).
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
*/
void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel) {
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
- //Display only for the given images
- for(std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
- it_img != mapOfImages.end(); ++it_img) {
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(it_img->first);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_img->first);
- if(it_klt != m_mapOfKltTrackers.end() && it_camPose != mapOfCameraPoses.end() && it_cam != mapOfCameraParameters.end()) {
+ 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);
} else {
std::cerr << "Missing elements ! " << std::endl;
@@ -433,11 +477,12 @@ void vpMbKltMultiTracker::display(const std::map<std::string, const vpImage<vpRG
\return The vector of camera names.
*/
-std::vector<std::string> vpMbKltMultiTracker::getCameraNames() const {
+std::vector<std::string> vpMbKltMultiTracker::getCameraNames() const
+{
std::vector<std::string> cameraNames;
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
cameraNames.push_back(it_klt->first);
}
@@ -449,10 +494,11 @@ std::vector<std::string> vpMbKltMultiTracker::getCameraNames() const {
\param camera : Copy of the camera parameters used by the tracker.
*/
-void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const {
- //Get the reference camera parameters
+void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const
+{
+ // Get the reference camera parameters
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->getCameraParameters(camera);
} else {
std::cerr << "The reference camera name: " << m_referenceCameraName << " does not exist !" << std::endl;
@@ -465,16 +511,17 @@ void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &camera) const
\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 {
- if(m_mapOfKltTrackers.size() == 2) {
+void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::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;
}
}
@@ -484,9 +531,10 @@ void vpMbKltMultiTracker::getCameraParameters(vpCameraParameters &cam1, vpCamera
\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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->getCameraParameters(camera);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -498,12 +546,13 @@ void vpMbKltMultiTracker::getCameraParameters(const std::string &cameraName, vpC
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbKltMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const {
- //Clear the input map
+void vpMbKltMultiTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
+{
+ // Clear the input map
mapOfCameraParameters.clear();
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
vpCameraParameters cam_;
it->second->getCameraParameters(cam_);
mapOfCameraParameters[it->first] = cam_;
@@ -511,14 +560,16 @@ void vpMbKltMultiTracker::getCameraParameters(std::map<std::string, vpCameraPara
}
/*!
- Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType for the given camera name.
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType
+ for the given camera name.
\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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getClipping();
} else {
std::cerr << "Cannot find camera: " << cameraName << std::endl;
@@ -532,9 +583,10 @@ unsigned int vpMbKltMultiTracker::getClipping(const std::string &cameraName) con
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbKltMultiTracker::getFaces() {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbKltMultiTracker::getFaces()
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getFaces();
}
@@ -547,9 +599,10 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbKltMultiTracker::getFaces() {
\return Reference to the face structure.
*/
-vpMbHiddenFaces<vpMbtPolygon>& vpMbKltMultiTracker::getFaces(const std::string &cameraName) {
+vpMbHiddenFaces<vpMbtPolygon> &vpMbKltMultiTracker::getFaces(const std::string &cameraName)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getFaces();
}
@@ -562,10 +615,11 @@ vpMbHiddenFaces<vpMbtPolygon>& vpMbKltMultiTracker::getFaces(const std::string &
\return Reference a map of the face structure for each camera.
*/
-std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbKltMultiTracker::getFaces() const {
+std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbKltMultiTracker::getFaces() const
+{
std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > mapOfFaces;
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfFaces[it->first] = it->second->faces;
}
@@ -577,10 +631,11 @@ std::map<std::string, vpMbHiddenFaces<vpMbtPolygon> > vpMbKltMultiTracker::getFa
\return The address of the circle feature list.
*/
-std::list<vpMbtDistanceCircle*>& vpMbKltMultiTracker::getFeaturesCircle() {
- std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
+std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle()
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
return it_klt->second->getFeaturesCircle();
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -595,10 +650,11 @@ std::list<vpMbtDistanceCircle*>& vpMbKltMultiTracker::getFeaturesCircle() {
\param cameraName : Camera name.
\return The address of the circle feature list.
*/
-std::list<vpMbtDistanceCircle*>& vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName) {
- std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
+std::list<vpMbtDistanceCircle *> &vpMbKltMultiTracker::getFeaturesCircle(const std::string &cameraName)
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getFeaturesCircle();
} else {
std::cerr << "The camera: " << cameraName << " does not exist !";
@@ -613,10 +669,11 @@ std::list<vpMbtDistanceCircle*>& vpMbKltMultiTracker::getFeaturesCircle(const st
\return The address of the Klt feature list.
*/
-std::list<vpMbtDistanceKltPoints*>& vpMbKltMultiTracker::getFeaturesKlt() {
- std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
+std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt()
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
return it_klt->second->getFeaturesKlt();
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -632,10 +689,11 @@ std::list<vpMbtDistanceKltPoints*>& vpMbKltMultiTracker::getFeaturesKlt() {
\return The address of the Klt feature list.
*/
-std::list<vpMbtDistanceKltPoints*>& vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName) {
- std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
+std::list<vpMbtDistanceKltPoints *> &vpMbKltMultiTracker::getFeaturesKlt(const std::string &cameraName)
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getFeaturesKlt();
} else {
std::cerr << "The camera: " << cameraName << " does not exist !";
@@ -649,10 +707,11 @@ std::list<vpMbtDistanceKltPoints*>& vpMbKltMultiTracker::getFeaturesKlt(const st
\return The address of the cylinder feature list.
*/
-std::list<vpMbtDistanceKltCylinder*>& vpMbKltMultiTracker::getFeaturesKltCylinder() {
- std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
+std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder()
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
return it_klt->second->getFeaturesKltCylinder();
} else {
std::cerr << "Cannot find reference camera: " << m_referenceCameraName << " !" << std::endl;
@@ -667,10 +726,11 @@ std::list<vpMbtDistanceKltCylinder*>& vpMbKltMultiTracker::getFeaturesKltCylinde
\param cameraName : Camera name.
\return The address of the cylinder feature list.
*/
-std::list<vpMbtDistanceKltCylinder*>& vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName) {
- std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
+std::list<vpMbtDistanceKltCylinder *> &vpMbKltMultiTracker::getFeaturesKltCylinder(const std::string &cameraName)
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getFeaturesKltCylinder();
} else {
std::cerr << "The camera: " << cameraName << " does not exist !";
@@ -687,11 +747,12 @@ std::list<vpMbtDistanceKltCylinder*>& vpMbKltMultiTracker::getFeaturesKltCylinde
\return the list of KLT points through vpKltOpencv for each camera.
*/
-std::map<std::string, std::vector<vpImagePoint> > vpMbKltMultiTracker::getKltImagePoints() const {
+std::map<std::string, std::vector<vpImagePoint> > vpMbKltMultiTracker::getKltImagePoints() const
+{
std::map<std::string, std::vector<vpImagePoint> > mapOfFeatures;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfFeatures[it->first] = it->second->getKltImagePoints();
}
@@ -704,13 +765,15 @@ std::map<std::string, std::vector<vpImagePoint> > vpMbKltMultiTracker::getKltIma
\warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f.
This function convert and copy the openCV KLT points into vpImagePoints.
- \return the list of KLT points and their id through vpKltOpencv for each camera.
+ \return the list of KLT points and their id through vpKltOpencv for each
+ camera.
*/
-std::map<std::string, std::map<int, vpImagePoint> > vpMbKltMultiTracker::getKltImagePointsWithId() const {
+std::map<std::string, std::map<int, vpImagePoint> > vpMbKltMultiTracker::getKltImagePointsWithId() const
+{
std::map<std::string, std::map<int, vpImagePoint> > mapOfFeatures;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfFeatures[it->first] = it->second->getKltImagePointsWithId();
}
@@ -722,11 +785,12 @@ std::map<std::string, std::map<int, vpImagePoint> > vpMbKltMultiTracker::getKltI
\return klt tracker.
*/
-std::map<std::string, vpKltOpencv> vpMbKltMultiTracker::getKltOpencv() const {
+std::map<std::string, vpKltOpencv> vpMbKltMultiTracker::getKltOpencv() const
+{
std::map<std::string, vpKltOpencv> mapOfKltOpenCVTracker;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfKltOpenCVTracker[it->first] = it->second->getKltOpencv();
}
@@ -739,22 +803,24 @@ std::map<std::string, vpKltOpencv> vpMbKltMultiTracker::getKltOpencv() const {
\return The list of KLT points through vpKltOpencv.
*/
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
-std::map<std::string, std::vector<cv::Point2f> > vpMbKltMultiTracker::getKltPoints() const {
+std::map<std::string, std::vector<cv::Point2f> > vpMbKltMultiTracker::getKltPoints() const
+{
std::map<std::string, std::vector<cv::Point2f> > mapOfFeatures;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfFeatures[it->first] = it->second->getKltPoints();
}
return mapOfFeatures;
}
#else
-std::map<std::string, CvPoint2D32f*> vpMbKltMultiTracker::getKltPoints() {
- std::map<std::string, CvPoint2D32f*> mapOfFeatures;
+std::map<std::string, CvPoint2D32f *> vpMbKltMultiTracker::getKltPoints()
+{
+ std::map<std::string, CvPoint2D32f *> mapOfFeatures;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfFeatures[it->first] = it->second->getKltPoints();
}
@@ -767,12 +833,13 @@ std::map<std::string, CvPoint2D32f*> vpMbKltMultiTracker::getKltPoints() {
\return The number of features
*/
-std::map<std::string, int> vpMbKltMultiTracker::getNbKltPoints() const {
+std::map<std::string, int> vpMbKltMultiTracker::getKltNbPoints() const
+{
std::map<std::string, int> mapOfFeatures;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- mapOfFeatures[it->first] = it->second->getNbKltPoints();
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ mapOfFeatures[it->first] = it->second->getKltNbPoints();
}
return mapOfFeatures;
@@ -783,9 +850,10 @@ std::map<std::string, int> vpMbKltMultiTracker::getNbKltPoints() const {
\return Number of polygons.
*/
-unsigned int vpMbKltMultiTracker::getNbPolygon() const {
+unsigned int vpMbKltMultiTracker::getNbPolygon() const
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
return it->second->getNbPolygon();
}
@@ -794,14 +862,16 @@ unsigned int vpMbKltMultiTracker::getNbPolygon() const {
}
/*!
- Get the number of polygons (faces) representing the object to track for all the cameras.
+ Get the number of polygons (faces) representing the object to track for all
+ the cameras.
\return Number of polygons for the specified camera.
*/
-std::map<std::string, unsigned int> vpMbKltMultiTracker::getMultiNbPolygon() const {
+std::map<std::string, unsigned int> vpMbKltMultiTracker::getMultiNbPolygon() const
+{
std::map<std::string, unsigned int> mapOfNbPolygons;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
mapOfNbPolygons[it->first] = it->second->getNbPolygon();
}
@@ -814,16 +884,16 @@ std::map<std::string, unsigned int> vpMbKltMultiTracker::getMultiNbPolygon() con
\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 {
- if(m_mapOfKltTrackers.size() == 2) {
+void vpMbKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::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;
}
}
@@ -835,9 +905,10 @@ void vpMbKltMultiTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix
\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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->getPose(cMo_);
} else {
std::cerr << "The camera: " << cameraName << " does not exist !" << std::endl;
@@ -849,45 +920,47 @@ void vpMbKltMultiTracker::getPose(const std::string &cameraName, vpHomogeneousMa
\param mapOfCameraPoses : The map of camera poses for all the cameras.
*/
-void vpMbKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const {
- //Clear the map
+void vpMbKltMultiTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
+{
+ // Clear the map
mapOfCameraPoses.clear();
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
vpHomogeneousMatrix cMo_;
it->second->getPose(cMo_);
mapOfCameraPoses[it->first] = cMo_;
}
}
-void vpMbKltMultiTracker::init(const vpImage<unsigned char>& /*I*/) {
-}
+void vpMbKltMultiTracker::init(const vpImage<unsigned char> & /*I*/) {}
#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the list points3D_list.
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are given in \e points3D_list.
- \param I : Input image
- \param points3D_list : List of the 3D points (object frame).
- \param displayFile : Path to the image used to display the help. This functionality
- is only available if visp_io module is used.
+ \param I : Input image where the user has to click.
+ \param points3D_list : List of at least 4 3D points with coordinates
+ expressed in meters in the object frame. \param displayFile : Path to the
+ 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<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
- const std::string &displayFile) {
- if(m_mapOfKltTrackers.empty()) {
+void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
+ const std::string &displayFile)
+{
+ if (m_mapOfKltTrackers.empty()) {
throw vpException(vpTrackingException::initializationError, "There is no camera !");
- } else if(m_mapOfKltTrackers.size() > 1) {
+ } else if (m_mapOfKltTrackers.size() > 1) {
throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
} else {
- //Get the vpMbKltTracker object for the reference camera name
+ // Get the vpMbKltTracker object for the reference camera name
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->initClick(I, points3D_list, displayFile);
it->second->getPose(cMo);
- //Init c0Mo
+ // Init c0Mo
c0Mo = cMo;
ctTc0.eye();
} else {
@@ -899,38 +972,48 @@ void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I, const std::
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \param I : Input image.
- \param initFile : File containing the points where to click.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \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
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
-void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp) {
- if(m_mapOfKltTrackers.empty()) {
+void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile,
+ const bool displayHelp)
+{
+ if (m_mapOfKltTrackers.empty()) {
throw vpException(vpTrackingException::initializationError, "There is no camera !");
- } else if(m_mapOfKltTrackers.size() > 1) {
+ } else if (m_mapOfKltTrackers.size() > 1) {
throw vpException(vpTrackingException::initializationError, "There is more than one camera !");
} else {
- //Get the vpMbKltTracker object for the reference camera name
+ // Get the vpMbKltTracker object for the reference camera name
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->initClick(I, initFile, displayHelp);
it->second->getPose(cMo);
- //Init c0Mo
+ // Init c0Mo
c0Mo = cMo;
ctTc0.eye();
} else {
@@ -942,42 +1025,58 @@ void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I, const std::
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param I1 : Input image for the first camera.
\param I2 : Input image for the second camera.
- \param initFile1 : File containing the points where to click for the first camera.
- \param initFile2 : File containing the points where to click for the second camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
-
- \exception vpException::ioError : The file specified in initFile doesn't exist.
-
- \sa setPathNamePoseSaving()
-*/
-void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I1, const vpImage<unsigned char> &I2,
- const std::string& initFile1, const std::string& initFile2, const bool displayHelp, const bool firstCameraIsReference) {
- if(m_mapOfKltTrackers.size() == 2) {
+ \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
+ 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
+ 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.
+
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
+
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
+*/
+void vpMbKltMultiTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
it->second->initClick(I1, initFile1, displayHelp);
- if(firstCameraIsReference) {
- //Set the reference cMo
+ if (firstCameraIsReference) {
+ // Set the reference cMo
it->second->getPose(cMo);
- //Set the reference camera parameters
+ // Set the reference camera parameters
it->second->getCameraParameters(this->cam);
- //Init c0Mo and ctTc0
+ // Init c0Mo and ctTc0
c0Mo = cMo;
ctTc0.eye();
}
@@ -986,14 +1085,14 @@ void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I1, const vpIm
it->second->initClick(I2, initFile2, displayHelp);
- if(!firstCameraIsReference) {
- //Set the reference cMo
+ if (!firstCameraIsReference) {
+ // Set the reference cMo
it->second->getPose(cMo);
- //Set the reference camera parameters
+ // Set the reference camera parameters
it->second->getCameraParameters(this->cam);
- //Init c0Mo and ctTc0
+ // Init c0Mo and ctTc0
c0Mo = cMo;
ctTc0.eye();
}
@@ -1005,50 +1104,60 @@ void vpMbKltMultiTracker::initClick(const vpImage<unsigned char>& I1, const vpIm
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile for the reference camera.
- The other cameras will be automatically initialized and the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
\param mapOfImages : Map of images.
- \param initFile : File containing the points where to click for the reference camera.
- \param displayHelp : Optional display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ \param initFile : File containing the points where to click for the
+ 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.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
- \sa setPathNamePoseSaving()
*/
void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &initFile, const bool displayHelp) {
+ const std::string &initFile, const bool displayHelp)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ if (it_klt != m_mapOfKltTrackers.end()) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
- if(it_img != mapOfImages.end()) {
+ if (it_img != mapOfImages.end()) {
it_klt->second->initClick(*it_img->second, initFile, displayHelp);
- //Set the reference cMo
+ // Set the reference cMo
it_klt->second->getPose(cMo);
- //Init c0Mo
+ // Init c0Mo
c0Mo = cMo;
ctTc0.eye();
- //Set the pose for the others cameras
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ // Set the pose for the others cameras
+ 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<std::string, vpHomogeneousMatrix>::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);
@@ -1072,34 +1181,43 @@ void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<un
}
/*!
- Initialize the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile.
- The cameras that have not an init file will be automatically initialized but
- the camera transformation matrices have to be set before.
- The structure of this file is (without the comments):
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \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 ( 'initFile.ppm' ). This
- image may be used to show where to click.
+ The cameras that have not an init file will be automatically initialized but
+ the camera transformation matrices have to be set before.
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \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
+ teabox.ppm). This image may be used to show where to click.
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp) {
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, std::string>::const_iterator it_initFile = mapOfInitFiles.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_initFile != mapOfInitFiles.end()) {
+ 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);
@@ -1109,17 +1227,17 @@ void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<un
throw vpException(vpTrackingException::initializationError, "Cannot initClick for the reference camera !");
}
- //Vector of missing initFile for cameras
+ // Vector of missing initFile for cameras
std::vector<std::string> vectorOfMissingCameraPoses;
- //Set pose for the specified cameras
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ // Set pose for the specified cameras
+ 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()) {
- //InitClick for the current camera
+ 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);
} else {
vectorOfMissingCameraPoses.push_back(it_klt->first);
@@ -1127,19 +1245,22 @@ void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<un
}
}
- //Set pose for cameras that do not have an initFile
- for(std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
- it1 != vectorOfMissingCameraPoses.end(); ++it1) {
+ // Set pose for cameras that do not have an initFile
+ for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
+ it1 != vectorOfMissingCameraPoses.end(); ++it1) {
it_img = mapOfImages.find(*it1);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1);
+ std::map<std::string, vpHomogeneousMatrix>::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);
} else {
std::stringstream ss;
- ss << "Missing image or missing camera transformation matrix ! Cannot set the pose for camera: " << (*it1) << " !";
+ ss << "Missing image or missing camera transformation matrix ! Cannot "
+ "set the pose for camera: "
+ << (*it1) << " !";
throw vpException(vpTrackingException::initializationError, ss.str().c_str());
}
}
@@ -1147,8 +1268,8 @@ void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<un
#endif //#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
- The structure of this file is (without the comments):
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile. The structure of this file is (without the comments):
\code
// The six value of the pose vector
0.0000 // \
@@ -1159,47 +1280,48 @@ void vpMbKltMultiTracker::initClick(const std::map<std::string, const vpImage<un
0.0000 // /
\endcode
- Where the three firsts lines refer to the translation and the three last to the rotation in thetaU
- parametrisation (see vpThetaUVector).
+ Where the three firsts lines refer to the translation and the three last to
+ the rotation in thetaU parametrisation (see vpThetaUVector).
\param I : Input image
\param initFile : Path to the file containing the pose.
*/
-void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile) {
- //Monocular case only !
- if(m_mapOfKltTrackers.size() > 1) {
+void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &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 !");
}
char s[FILENAME_MAX];
- std::fstream finit ;
+ std::fstream finit;
vpPoseVector init_pos;
std::string ext = ".pos";
- size_t pos = initFile.rfind(ext);
+ size_t pos = initFile.rfind(ext);
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
else
- sprintf(s,"%s.pos", initFile.c_str());
+ sprintf(s, "%s.pos", initFile.c_str());
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
std::cerr << "cannot read " << s << std::endl;
throw vpException(vpException::ioError, "cannot read init file");
}
- for (unsigned int i = 0; i < 6; i += 1){
+ for (unsigned int i = 0; i < 6; i += 1) {
finit >> init_pos[i];
}
- //Set the new pose for the reference camera
+ // Set the new pose for the reference camera
cMo.buildFrom(init_pos);
c0Mo = cMo;
ctTc0.eye();
- //Init for the reference camera
+ // Init for the reference camera
std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_ref == m_mapOfKltTrackers.end()) {
+ if (it_ref == m_mapOfKltTrackers.end()) {
throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
}
@@ -1212,14 +1334,15 @@ void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const st
\param I : Input image
\param cMo_ : Pose matrix.
*/
-void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_) {
+void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
+{
this->cMo = cMo_;
c0Mo = cMo;
ctTc0.eye();
- //Init for the reference camera
+ // Init for the reference camera
std::map<std::string, vpMbKltTracker *>::iterator it_ref = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_ref == m_mapOfKltTrackers.end()) {
+ if (it_ref == m_mapOfKltTrackers.end()) {
throw vpException(vpTrackingException::initializationError, "Cannot find the reference camera !");
}
@@ -1232,7 +1355,8 @@ void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I, const vp
\param I : Input image
\param cPo : Pose vector.
*/
-void vpMbKltMultiTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo) {
+void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo)
+{
vpHomogeneousMatrix _cMo(cPo);
initFromPose(I, _cMo);
}
@@ -1244,11 +1368,14 @@ void vpMbKltMultiTracker::initFromPose (const vpImage<unsigned char>& I, const v
\param I2 : Input image for the second camera.
\param c1Mo : Pose matrix for the first camera.
\param c2Mo : Pose matrix for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference
+ camera, otherwise it is the second one.
*/
-void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo, const bool firstCameraIsReference) {
- if(m_mapOfKltTrackers.size() == 2) {
+void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
it->second->initFromPose(I1, c1Mo);
@@ -1256,7 +1383,7 @@ void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I1, const v
it->second->initFromPose(I2, c2Mo);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cMo = c1Mo;
} else {
this->cMo = c2Mo;
@@ -1272,36 +1399,39 @@ void vpMbKltMultiTracker::initFromPose(const vpImage<unsigned char>& I1, const v
}
/*!
- Initialize the tracking thanks to the pose. The camera transformation matrices have to be set before.
+ Initialize the tracking thanks to the pose. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images.
\param cMo_ : Pose matrix for the reference camera.
*/
void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
+ const vpHomogeneousMatrix &cMo_)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
- if(it_img != mapOfImages.end()) {
+ if (it_img != mapOfImages.end()) {
it_klt->second->initFromPose(*it_img->second, cMo_);
cMo = cMo_;
c0Mo = this->cMo;
ctTc0.eye();
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ if (it_klt->first != m_referenceCameraName) {
std::map<std::string, vpHomogeneousMatrix>::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 !");
}
}
}
@@ -1322,13 +1452,15 @@ void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage
\param mapOfCameraPoses : Map of pose matrix.
*/
void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
- //Set the reference cMo
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
+ // Set the reference cMo
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ 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);
@@ -1338,33 +1470,36 @@ void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage
throw vpException(vpTrackingException::initializationError, "Cannot set pose for the reference camera !");
}
- //Vector of missing pose matrices for cameras
+ // Vector of missing pose matrices for cameras
std::vector<std::string> vectorOfMissingCameraPoses;
- //Set pose for the specified cameras
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ // Set pose for the specified cameras
+ 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);
- if(it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
- //Set pose
+ if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ // Set pose
it_klt->second->initFromPose(*it_img->second, it_camPose->second);
} else {
vectorOfMissingCameraPoses.push_back(it_klt->first);
}
}
- for(std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
- it1 != vectorOfMissingCameraPoses.end(); ++it1) {
+ for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
+ it1 != vectorOfMissingCameraPoses.end(); ++it1) {
it_img = mapOfImages.find(*it1);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1);
+ std::map<std::string, vpHomogeneousMatrix>::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 {
std::stringstream ss;
- ss << "Missing image or missing camera transformation matrix ! Cannot set the pose for camera: " << (*it1) << " !";
+ ss << "Missing image or missing camera transformation matrix ! Cannot "
+ "set the pose for camera: "
+ << (*it1) << " !";
throw vpException(vpTrackingException::initializationError, ss.str().c_str());
}
}
@@ -1372,13 +1507,14 @@ void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage
/*!
Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ From the configuration file initialize the parameters corresponding to the
+objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -1416,14 +1552,15 @@ void vpMbKltMultiTracker::initFromPose(const std::map<std::string, const vpImage
\sa vpXmlParser::cleanup()
*/
-void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile) {
+void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
- //Load ConfigFile for reference camera
+ if (it != m_mapOfKltTrackers.end()) {
+ // Load ConfigFile for reference camera
it->second->loadConfigFile(configFile);
it->second->getCameraParameters(cam);
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1435,29 +1572,32 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile) {
}
/*!
- Load the xml configuration files for the stereo cameras case. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ Load the xml configuration files for the stereo cameras case. An example of
+ such a file is provided in loadConfigFile(const std::string &)
+ documentation. From the configuration file initialize the parameters
+ corresponding to the objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile1 : Full name of the xml file for the first camera.
\param configFile2 : Full name of the xml file for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : If true, the first camera is the reference,
+ otherwise it is the second one.
\sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
*/
-void vpMbKltMultiTracker::loadConfigFile(const std::string& configFile1, const std::string& configFile2,
- const bool firstCameraIsReference) {
- if(m_mapOfKltTrackers.size() == 2) {
+void vpMbKltMultiTracker::loadConfigFile(const std::string &configFile1, const std::string &configFile2,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
it->second->loadConfigFile(configFile1);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
it->second->getCameraParameters(cam);
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1466,10 +1606,10 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string& configFile1, const s
it->second->loadConfigFile(configFile2);
- if(!firstCameraIsReference) {
+ if (!firstCameraIsReference) {
it->second->getCameraParameters(cam);
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1482,22 +1622,24 @@ void vpMbKltMultiTracker::loadConfigFile(const std::string& configFile1, const s
}
/*!
- Load the xml configuration files for all the cameras. An example of such a file is provided in
- loadConfigFile(const std::string &) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ Load the xml configuration files for all the cameras. An example of such a
+ file is provided in loadConfigFile(const std::string &) documentation. From
+ the configuration file initialize the parameters corresponding to the
+ objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param mapOfConfigFiles : Map of xml files.
\sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
*/
-void vpMbKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+void vpMbKltMultiTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
std::map<std::string, std::string>::const_iterator it_config = mapOfConfigFiles.find(it_klt->first);
- if(it_config != mapOfConfigFiles.end()) {
+ if (it_config != mapOfConfigFiles.end()) {
it_klt->second->loadConfigFile(it_config->second);
} else {
std::stringstream ss;
@@ -1506,12 +1648,12 @@ void vpMbKltMultiTracker::loadConfigFile(const std::map<std::string, std::string
}
}
- //Set the reference camera parameters
+ // Set the reference camera parameters
std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->getCameraParameters(cam);
- //Set clipping
+ // Set clipping
this->clippingFlag = it->second->getClipping();
this->angleAppears = it->second->getAngleAppear();
this->angleDisappears = it->second->getAngleDisappear();
@@ -1539,60 +1681,58 @@ int main()
}
\endcode
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
\param modelFile : the file containing the the 3D model description.
The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \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) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::loadModel(const std::string &modelFile, const bool verbose)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->loadModel(modelFile, verbose);
}
modelInitialised = true;
}
-void vpMbKltMultiTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNbInfos,
- std::map<std::string, unsigned int> &mapOfNbFaceUsed) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- mapOfNbInfos[it->first] = 0;
- mapOfNbFaceUsed[it->first] = 0;
- }
+void vpMbKltMultiTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ m_nbInfos = 0;
+ m_nbFaceUsed = 0;
+
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
- for (std::map<std::string, vpMbKltTracker*>::const_iterator it =
- m_mapOfKltTrackers.begin(); it != m_mapOfKltTrackers.end(); ++it) {
try {
- it->second->preTracking(*mapOfImages[it->first], mapOfNbInfos[it->first], mapOfNbFaceUsed[it->first]);
- } catch (/*vpException &e*/...) {
-// throw e;
+ klt->preTracking(*mapOfImages[it->first]);
+ m_nbInfos += klt->m_nbInfos;
+ m_nbFaceUsed += klt->m_nbFaceUsed;
+ } catch (...) {
}
}
}
-void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- std::map<std::string, unsigned int> &mapOfNbInfos, vpColVector &w_klt) {
- unsigned int shift = 0;
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- //Set the camera pose
- it->second->cMo = m_mapOfCameraTransformationMatrix[it->first]*cMo;
+void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ vpMbKltTracker *klt = it->second;
- if(mapOfNbInfos[it->first] > 0) {
- vpSubColVector sub_w(w_klt, shift, 2*mapOfNbInfos[it->first]);
- shift += 2*mapOfNbInfos[it->first];
- if(it->second->postTracking(*mapOfImages[it->first], sub_w)) {
- it->second->reinit(*mapOfImages[it->first]);
+ // Set the camera pose
+ it->second->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
- //set ctTc0 to identity
- if(it->first == m_referenceCameraName) {
- reinit(/*mapOfImages[it->first]*/);
- }
+ if (klt->m_nbInfos > 0 && klt->postTracking(*mapOfImages[it->first], klt->m_w_klt)) {
+ klt->reinit(*mapOfImages[it->first]);
+
+ // set ctTc0 to identity
+ if (it->first == m_referenceCameraName) {
+ reinit(/*mapOfImages[it->first]*/);
}
}
}
@@ -1601,7 +1741,8 @@ void vpMbKltMultiTracker::postTracking(std::map<std::string, const vpImage<unsig
/*!
The parameter is not used.
*/
-void vpMbKltMultiTracker::reinit(/* const vpImage<unsigned char>& I*/) {
+void vpMbKltMultiTracker::reinit(/* const vpImage<unsigned char>& I*/)
+{
c0Mo = cMo;
ctTc0.eye();
}
@@ -1611,13 +1752,14 @@ void vpMbKltMultiTracker::reinit(/* const vpImage<unsigned char>& I*/) {
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose) {
- if(m_mapOfKltTrackers.size() != 1) {
+void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char> &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 !";
throw vpException(vpTrackingException::fatalError, ss.str().c_str());
@@ -1627,10 +1769,10 @@ void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char>& I, const std
modelInitialised = true;
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
it_klt->second->reInitModel(I, cad_name, cMo_, verbose);
- //Set reference pose
+ // Set reference pose
it_klt->second->getPose(cMo);
c0Mo = cMo;
@@ -1643,25 +1785,28 @@ void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char>& I, const std
/*!
Re-initialize the model used by the tracker.
- \param I1 : The image containing the object to initialize for the first camera.
- \param I2 : The image containing the object to initialize for the second camera.
- \param cad_name : Path to the file containing the 3D model description.
- \param c1Mo : The new vpHomogeneousMatrix between the first camera and the new model.
- \param c2Mo : The new vpHomogeneousMatrix between the second camera and the new model.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
- \param firstCameraIsReference : If true, the first camera is the reference camera, otherwise it is the second one.
+ \param I1 : The image containing the object to initialize for the first
+ camera. \param I2 : The image containing the object to initialize for the
+ second camera. \param cad_name : Path to the file containing the 3D model
+ description. \param c1Mo : The new vpHomogeneousMatrix between the first
+ camera and the new model. \param c2Mo : The new vpHomogeneousMatrix between
+ the second camera and the new model. \param verbose : verbose option to
+ print additional information when loading CAO model files which include
+ 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<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const std::string &cad_name, const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
- const bool verbose, const bool firstCameraIsReference) {
- if(m_mapOfKltTrackers.size() == 2) {
+ const std::string &cad_name, const vpHomogeneousMatrix &c1Mo,
+ const vpHomogeneousMatrix &c2Mo, const bool verbose,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it_edge = m_mapOfKltTrackers.begin();
it_edge->second->reInitModel(I1, cad_name, c1Mo, verbose);
- if(firstCameraIsReference) {
- //Set reference pose
+ if (firstCameraIsReference) {
+ // Set reference pose
it_edge->second->getPose(cMo);
}
@@ -1669,8 +1814,8 @@ void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const vp
it_edge->second->reInitModel(I2, cad_name, c2Mo, verbose);
- if(!firstCameraIsReference) {
- //Set reference pose
+ if (!firstCameraIsReference) {
+ // Set reference pose
it_edge->second->getPose(cMo);
}
@@ -1686,22 +1831,26 @@ void vpMbKltMultiTracker::reInitModel(const vpImage<unsigned char> &I1, const vp
\param mapOfImages : Map of images.
\param cad_name : Path to the file containing the 3D model description.
- \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras and the current object position.
- \param verbose : Verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras
+ and the current object position. \param verbose : Verbose option to print
+ additional information when loading CAO model files which include other CAO
+ model files.
*/
void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::string &cad_name, const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
- const bool verbose) {
+ const std::string &cad_name,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ 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
+ // Set reference pose
it_klt->second->getPose(cMo);
c0Mo = cMo;
@@ -1711,12 +1860,12 @@ void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<
}
std::vector<std::string> vectorOfMissingCameras;
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ 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()) {
it_klt->second->reInitModel(*it_img->second, cad_name, it_camPose->second, verbose);
} else {
vectorOfMissingCameras.push_back(it_klt->first);
@@ -1724,12 +1873,13 @@ void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<
}
}
- for(std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin();
- it != vectorOfMissingCameras.end(); ++it) {
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
+ ++it) {
it_img = mapOfImages.find(*it);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::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);
}
@@ -1740,17 +1890,18 @@ void vpMbKltMultiTracker::reInitModel(const std::map<std::string, const vpImage<
Reset the tracker. The model is removed and the pose is set to identity.
The tracker needs to be initialized with a new model and a new pose.
*/
-void vpMbKltMultiTracker::resetTracker() {
+void vpMbKltMultiTracker::resetTracker()
+{
cMo.eye();
ctTc0.eye();
- //Call resetTracker for all the cameras
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ // Call resetTracker for all the cameras
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->resetTracker();
}
- compute_interaction = true;
+ m_computeInteraction = true;
firstInitialisation = true;
computeCovariance = false;
@@ -1763,8 +1914,8 @@ void vpMbKltMultiTracker::resetTracker() {
threshold_outlier = 0.5;
percentGood = 0.7;
- lambda = 0.8;
- maxIter = 200;
+ m_lambda = 0.8;
+ m_maxIter = 200;
m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
@@ -1784,11 +1935,12 @@ void vpMbKltMultiTracker::resetTracker() {
\param a : new angle in radian.
*/
-void vpMbKltMultiTracker::setAngleAppear(const double &a) {
+void vpMbKltMultiTracker::setAngleAppear(const double &a)
+{
vpMbTracker::setAngleAppear(a);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setAngleAppear(a);
}
}
@@ -1802,11 +1954,12 @@ void vpMbKltMultiTracker::setAngleAppear(const double &a) {
\param a : new angle in radian.
*/
-void vpMbKltMultiTracker::setAngleDisappear(const double &a) {
+void vpMbKltMultiTracker::setAngleDisappear(const double &a)
+{
vpMbTracker::setAngleDisappear(a);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setAngleDisappear(a);
}
}
@@ -1816,17 +1969,18 @@ void vpMbKltMultiTracker::setAngleDisappear(const double &a) {
\param camera : The new camera parameters.
*/
-void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters& camera) {
- if(m_mapOfKltTrackers.empty()) {
+void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera)
+{
+ if (m_mapOfKltTrackers.empty()) {
throw vpException(vpTrackingException::fatalError, "There is no camera !");
- } else if(m_mapOfKltTrackers.size() > 1) {
+ } else if (m_mapOfKltTrackers.size() > 1) {
throw vpException(vpTrackingException::fatalError, "There is more than one camera !");
} else {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setCameraParameters(camera);
- //Set reference camera parameters
+ // Set reference camera parameters
this->cam = camera;
} else {
std::stringstream ss;
@@ -1841,20 +1995,22 @@ void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters& camera)
\param camera1 : The new camera parameters for the first camera.
\param camera2 : The new camera parameters for the second camera.
- \param firstCameraIsReference : If true, the first camera is the reference, otherwise it is the second one.
+ \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) {
- if(m_mapOfKltTrackers.empty()) {
+void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.empty()) {
throw vpException(vpTrackingException::fatalError, "There is no camera !");
- } else if(m_mapOfKltTrackers.size() == 2) {
+ } else if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
it->second->setCameraParameters(camera1);
++it;
it->second->setCameraParameters(camera2);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cam = camera1;
} else {
this->cam = camera2;
@@ -1872,12 +2028,13 @@ void vpMbKltMultiTracker::setCameraParameters(const vpCameraParameters& camera1,
\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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setCameraParameters(camera);
- if(it->first == m_referenceCameraName) {
+ if (it->first == m_referenceCameraName) {
this->cam = camera;
}
} else {
@@ -1892,14 +2049,15 @@ void vpMbKltMultiTracker::setCameraParameters(const std::string &cameraName, con
\param mapOfCameraParameters : Map of camera parameters.
*/
-void vpMbKltMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+void vpMbKltMultiTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
std::map<std::string, vpCameraParameters>::const_iterator it_cam = mapOfCameraParameters.find(it_klt->first);
- if(it_cam != mapOfCameraParameters.end()) {
+ if (it_cam != mapOfCameraParameters.end()) {
it_klt->second->setCameraParameters(it_cam->second);
- if(it_klt->first == m_referenceCameraName) {
+ if (it_klt->first == m_referenceCameraName) {
this->cam = it_cam->second;
}
} else {
@@ -1911,15 +2069,18 @@ void vpMbKltMultiTracker::setCameraParameters(const std::map<std::string, vpCame
}
/*!
- Set the camera transformation matrix for the specified camera (\f$ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
+ Set the camera transformation matrix for the specified camera (\f$
+ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
\param cameraName : Camera name.
- \param cameraTransformationMatrix : Camera transformation matrix between the current and the reference camera.
+ \param cameraTransformationMatrix : Camera transformation matrix between the
+ current and the reference camera.
*/
void vpMbKltMultiTracker::setCameraTransformationMatrix(const std::string &cameraName,
- const vpHomogeneousMatrix &cameraTransformationMatrix) {
+ const vpHomogeneousMatrix &cameraTransformationMatrix)
+{
std::map<std::string, vpHomogeneousMatrix>::iterator it = m_mapOfCameraTransformationMatrix.find(cameraName);
- if(it != m_mapOfCameraTransformationMatrix.end()) {
+ if (it != m_mapOfCameraTransformationMatrix.end()) {
it->second = cameraTransformationMatrix;
} else {
std::stringstream ss;
@@ -1930,18 +2091,21 @@ void vpMbKltMultiTracker::setCameraTransformationMatrix(const std::string &camer
/*!
Set the map of camera transformation matrices
- (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1}, _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
+ (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1},
+ _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
\param mapOfTransformationMatrix : map of camera transformation matrices.
*/
void vpMbKltMultiTracker::setCameraTransformationMatrix(
- const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix) {
- //Check if all cameras have a transformation matrix
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = mapOfTransformationMatrix.find(it_klt->first);
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
+{
+ // Check if all cameras have a transformation matrix
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ mapOfTransformationMatrix.find(it_klt->first);
- if(it_camTrans == mapOfTransformationMatrix.end()) {
+ if (it_camTrans == mapOfTransformationMatrix.end()) {
throw vpException(vpTrackingException::initializationError, "Missing camera transformation matrix !");
}
}
@@ -1956,11 +2120,12 @@ void vpMbKltMultiTracker::setCameraTransformationMatrix(
\param flags : New clipping flags.
*/
-void vpMbKltMultiTracker::setClipping(const unsigned int &flags) {
+void vpMbKltMultiTracker::setClipping(const unsigned int &flags)
+{
vpMbTracker::setClipping(flags);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setClipping(flags);
}
}
@@ -1973,9 +2138,10 @@ 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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setClipping(flags);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -1987,11 +2153,12 @@ void vpMbKltMultiTracker::setClipping(const std::string &cameraName, const unsig
\param flag : True if the covariance has to be computed, false otherwise
*/
-void vpMbKltMultiTracker::setCovarianceComputation(const bool& flag) {
+void vpMbKltMultiTracker::setCovarianceComputation(const bool &flag)
+{
vpMbTracker::setCovarianceComputation(flag);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setCovarianceComputation(flag);
}
}
@@ -2001,9 +2168,10 @@ void vpMbKltMultiTracker::setCovarianceComputation(const bool& flag) {
\param displayF : set it to true to display the features.
*/
-void vpMbKltMultiTracker::setDisplayFeatures(const bool displayF) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setDisplayFeatures(const bool displayF)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setDisplayFeatures(displayF);
}
@@ -2015,11 +2183,12 @@ void vpMbKltMultiTracker::setDisplayFeatures(const bool displayF) {
\param dist : Far clipping value.
*/
-void vpMbKltMultiTracker::setFarClippingDistance(const double &dist) {
+void vpMbKltMultiTracker::setFarClippingDistance(const double &dist)
+{
vpMbTracker::setFarClippingDistance(dist);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setFarClippingDistance(dist);
}
}
@@ -2030,9 +2199,10 @@ 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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setFarClippingDistance(dist);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2041,43 +2211,49 @@ void vpMbKltMultiTracker::setFarClippingDistance(const std::string &cameraName,
#ifdef VISP_HAVE_OGRE
/*!
- Set the ratio of visibility attempts that has to be successful to consider a polygon as visible.
+ Set the ratio of visibility attempts that has to be successful to consider a
+ polygon as visible.
\sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
- \param ratio : Ratio of succesful attempts that has to be considered. Value has to be between 0.0 (0%) and 1.0 (100%).
+ \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) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- it->second->setGoodNbRayCastingAttemptsRatio(ratio);
- }
+void vpMbKltMultiTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ it->second->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 random point inside the considered polygon.
+/*!
+ Set the number of rays that will be sent toward each polygon for visibility
+ test. Each ray will go from the optic center of the camera to a random point
+ inside the considered polygon.
- \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
+ \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
- \param attempts Number of rays to be sent.
- */
- void vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- it->second->setNbRayCastingAttemptsForVisibility(attempts);
- }
+ \param attempts Number of rays to be sent.
+*/
+void vpMbKltMultiTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ it->second->setNbRayCastingAttemptsForVisibility(attempts);
}
+}
#endif
- /*!
- Set the new value of the klt tracker.
+/*!
+ Set the new value of the klt tracker.
- \param t : Klt tracker containing the new values.
- */
-void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv& t) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ \param t : Klt tracker containing the new values.
+*/
+void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv &t)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
it_klt->second->setKltOpencv(t);
}
}
@@ -2087,11 +2263,12 @@ void vpMbKltMultiTracker::setKltOpencv(const vpKltOpencv& t) {
\param mapOfOpenCVTrackers : Map of Klt trackers containing the new values.
*/
-void vpMbKltMultiTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfOpenCVTrackers) {
- for(std::map<std::string, vpKltOpencv>::const_iterator it_kltOpenCV = mapOfOpenCVTrackers.begin();
- it_kltOpenCV != mapOfOpenCVTrackers.end(); ++it_kltOpenCV) {
- std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.find(it_kltOpenCV->first);
- if(it_klt != m_mapOfKltTrackers.end()) {
+void vpMbKltMultiTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfOpenCVTrackers)
+{
+ for (std::map<std::string, vpKltOpencv>::const_iterator it_kltOpenCV = mapOfOpenCVTrackers.begin();
+ it_kltOpenCV != mapOfOpenCVTrackers.end(); ++it_kltOpenCV) {
+ std::map<std::string, vpMbKltTracker *>::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;
@@ -2100,35 +2277,41 @@ void vpMbKltMultiTracker::setKltOpencv(const std::map<std::string, vpKltOpencv>
}
/*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param name : name of the face we want to modify the LOD parameter.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param name : name of the face we want to modify
+ the LOD parameter.
\sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
*/
-void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &name) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &name)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setLod(useLod, name);
}
}
/*!
- Set the flag to consider if the level of detail (LOD) is used for all the cameras.
+ Set the flag to consider if the level of detail (LOD) is used for all the
+ cameras.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param cameraName : Name of the camera we want to set the LOD.
- \param name : name of the face we want to modify the LOD parameter, if empty all the faces are considered.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param cameraName : Name of the camera we want to
+ set the LOD. \param name : name of the face we want to modify the LOD
+ parameter, if empty all the faces are considered.
\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<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
it_klt->second->setLod(useLod, name);
} else {
std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
@@ -2140,10 +2323,11 @@ void vpMbKltMultiTracker::setLod(const bool useLod, const std::string &cameraNam
\param e : The desired erosion.
*/
-void vpMbKltMultiTracker::setMaskBorder(const unsigned int &e) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- it->second->setMaskBorder(e);
+void vpMbKltMultiTracker::setKltMaskBorder(const unsigned int &e)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ it->second->setKltMaskBorder(e);
}
maskBorder = e;
@@ -2152,21 +2336,23 @@ void vpMbKltMultiTracker::setMaskBorder(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;
}
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param name : name of the face we want to modify the LOD threshold.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinLineLengthThresh()
*/
-void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
}
}
@@ -2174,17 +2360,19 @@ void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThr
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param cameraName : name of the camera to consider.
- \param name : name of the face we want to modify the LOD threshold, if empty all the faces are considered.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param cameraName : name of the camera to consider. \param name :
+ name of the face we want to modify the LOD threshold, if empty all the faces
+ are considered.
\sa setLod(), setMinLineLengthThresh()
*/
void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &cameraName,
- const std::string &name) {
+ const std::string &name)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(cameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
+ if (it_klt != m_mapOfKltTrackers.end()) {
it_klt->second->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
} else {
std::cerr << "The camera: " << cameraName << " cannot be found !" << std::endl;
@@ -2196,11 +2384,12 @@ void vpMbKltMultiTracker::setMinPolygonAreaThresh(const double minPolygonAreaThr
\param dist : Near clipping value.
*/
-void vpMbKltMultiTracker::setNearClippingDistance(const double &dist) {
+void vpMbKltMultiTracker::setNearClippingDistance(const double &dist)
+{
vpMbTracker::setNearClippingDistance(dist);
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setNearClippingDistance(dist);
}
}
@@ -2208,16 +2397,17 @@ void vpMbKltMultiTracker::setNearClippingDistance(const double &dist) {
/*!
Enable/Disable the appearance of Ogre config dialog on startup.
- \warning This method has only effect when Ogre is used and Ogre visibility test is
- enabled using setOgreVisibilityTest() with true parameter.
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
- \param showConfigDialog : if true, shows Ogre dialog window (used to set Ogre
- rendering options) when Ogre visibility is enabled. By default, this functionality
- is turned off.
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
*/
-void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setOgreShowConfigDialog(showConfigDialog);
}
}
@@ -2225,19 +2415,21 @@ void vpMbKltMultiTracker::setOgreShowConfigDialog(const bool showConfigDialog) {
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
-void vpMbKltMultiTracker::setOgreVisibilityTest(const bool &v) {
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setOgreVisibilityTest(const bool &v)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setOgreVisibilityTest(v);
}
#ifdef VISP_HAVE_OGRE
- for(std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->faces.getOgreContext()->setWindowName("Multi MBT Klt (" + it->first + ")");
}
#endif
@@ -2251,9 +2443,10 @@ 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<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(cameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setNearClippingDistance(dist);
} else {
std::cerr << "Camera: " << cameraName << " does not exist !" << std::endl;
@@ -2265,9 +2458,10 @@ void vpMbKltMultiTracker::setNearClippingDistance(const std::string &cameraName,
\param opt : Optimization method to use.
*/
-void vpMbKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setOptimizationMethod(opt);
}
@@ -2281,10 +2475,11 @@ void vpMbKltMultiTracker::setOptimizationMethod(const vpMbtOptimizationMethod &o
\param I : image corresponding to the desired pose.
\param cMo_ : Pose to affect.
*/
-void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_) {
- if(m_mapOfKltTrackers.size() == 1) {
+void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
+{
+ if (m_mapOfKltTrackers.size() == 1) {
std::map<std::string, vpMbKltTracker *>::iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->setPose(I, cMo_);
this->cMo = cMo_;
@@ -2298,7 +2493,7 @@ void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomog
} else {
std::stringstream ss;
ss << "You are trying to set the pose with only one image and cMo "
- "but there are multiple cameras !";
+ "but there are multiple cameras !";
throw vpException(vpTrackingException::fatalError, ss.str());
}
}
@@ -2311,11 +2506,14 @@ void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I, const vpHomog
\param I2 : Second image corresponding to the desired pose.
\param c1Mo : First pose to affect.
\param c2Mo : Second pose to affect.
- \param firstCameraIsReference : if true, the first camera is the reference, otherwise it is the second one.
+ \param firstCameraIsReference : if true, the first camera is the reference,
+ otherwise it is the second one.
*/
void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
- const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix c2Mo, const bool firstCameraIsReference) {
- if(m_mapOfKltTrackers.size() == 2) {
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo,
+ const bool firstCameraIsReference)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
it->second->setPose(I1, c1Mo);
@@ -2323,7 +2521,7 @@ void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImag
it->second->setPose(I2, c2Mo);
- if(firstCameraIsReference) {
+ if (firstCameraIsReference) {
this->cMo = c1Mo;
} else {
this->cMo = c2Mo;
@@ -2347,29 +2545,31 @@ void vpMbKltMultiTracker::setPose(const vpImage<unsigned char> &I1, const vpImag
\param cMo_ : Pose to affect to the reference camera.
*/
void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const vpHomogeneousMatrix &cMo_) {
+ const vpHomogeneousMatrix &cMo_)
+{
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end()) {
- std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ if (it_klt != m_mapOfKltTrackers.end()) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
- if(it_img != mapOfImages.end()) {
- //Set pose on reference camera
+ if (it_img != mapOfImages.end()) {
+ // Set pose on reference camera
it_klt->second->setPose(*it_img->second, cMo_);
- //Set the reference cMo
+ // Set the reference cMo
cMo = cMo_;
c0Mo = this->cMo;
ctTc0.eye();
- //Set the pose for the others cameras
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ // Set the pose for the others cameras
+ for (it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ if (it_klt->first != m_referenceCameraName) {
std::map<std::string, vpHomogeneousMatrix>::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 {
@@ -2392,20 +2592,23 @@ void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsi
/*!
Set the pose to be used in entry of the next call to the track() function.
This pose will be just used once.
- Cameras that do not have pose will be automatically handled but the pose for the reference has to be passed in parameter.
- The camera transformation matrices have to be set before.
+ Cameras that do not have pose will be automatically handled but the pose for
+ the reference has to be passed in parameter. The camera transformation
+ matrices have to be set before.
\param mapOfImages : Map of images corresponding to the desired pose.
\param mapOfCameraPoses : Map of poses to affect.
*/
void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
- const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) {
- //Set the reference cMo
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
+ // Set the reference cMo
std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.find(m_referenceCameraName);
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(m_referenceCameraName);
- if(it_klt != m_mapOfKltTrackers.end() && it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ 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);
@@ -2415,17 +2618,17 @@ void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsi
throw vpException(vpTrackingException::fatalError, "Cannot set pose for the reference camera !");
}
- //Vector of missing pose matrices for cameras
+ // Vector of missing pose matrices for cameras
std::vector<std::string> vectorOfMissingCameraPoses;
- //Set pose for the specified cameras
- for(it_klt = m_mapOfKltTrackers.begin(); it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- if(it_klt->first != m_referenceCameraName) {
+ // Set pose for the specified cameras
+ 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()) {
- //Set pose
+ if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ // Set pose
it_klt->second->setPose(*it_img->second, it_camPose->second);
} else {
vectorOfMissingCameraPoses.push_back(it_klt->first);
@@ -2433,17 +2636,20 @@ void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsi
}
}
- for(std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
- it1 != vectorOfMissingCameraPoses.end(); ++it1) {
+ for (std::vector<std::string>::const_iterator it1 = vectorOfMissingCameraPoses.begin();
+ it1 != vectorOfMissingCameraPoses.end(); ++it1) {
it_img = mapOfImages.find(*it1);
- std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans = m_mapOfCameraTransformationMatrix.find(*it1);
+ std::map<std::string, vpHomogeneousMatrix>::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 {
std::stringstream ss;
- ss << "Missing image or missing camera transformation matrix ! Cannot set the pose for camera: " << (*it1) << " !";
+ ss << "Missing image or missing camera transformation matrix ! Cannot "
+ "set the pose for camera: "
+ << (*it1) << " !";
throw vpException(vpTrackingException::fatalError, ss.str().c_str());
}
}
@@ -2454,9 +2660,10 @@ void vpMbKltMultiTracker::setPose(const std::map<std::string, const vpImage<unsi
\param referenceCameraName : Name of the reference camera.
*/
-void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName) {
- std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.find(referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCameraName)
+{
+ std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(referenceCameraName);
+ if (it != m_mapOfKltTrackers.end()) {
m_referenceCameraName = referenceCameraName;
} else {
std::stringstream ss;
@@ -2470,12 +2677,13 @@ void vpMbKltMultiTracker::setReferenceCameraName(const std::string &referenceCam
\param v : True to use it, False otherwise
*/
-void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v) {
- //Set general setScanLineVisibilityTest
+void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v)
+{
+ // Set general setScanLineVisibilityTest
vpMbTracker::setScanLineVisibilityTest(v);
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setScanLineVisibilityTest(v);
}
}
@@ -2485,24 +2693,27 @@ void vpMbKltMultiTracker::setScanLineVisibilityTest(const bool &v) {
\param th : Threshold for the weight below which a point is rejected.
*/
-void vpMbKltMultiTracker::setThresholdAcceptation(const double th) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- it->second->setThresholdAcceptation(th);
+void vpMbKltMultiTracker::setKltThresholdAcceptation(const double th)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
+ it->second->setKltThresholdAcceptation(th);
}
threshold_outlier = th;
}
/*!
- Set if the polygons that have the given name have to be considered during the tracking phase.
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
\param name : name of the polygon(s).
\param useKltTracking : True if it has to be considered, False otherwise.
*/
-void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking) {
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
+void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
+{
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
+ it != m_mapOfKltTrackers.end(); ++it) {
it->second->setUseKltTracking(name, useKltTracking);
}
}
@@ -2514,12 +2725,13 @@ void vpMbKltMultiTracker::setUseKltTracking(const std::string &name, const bool
\param I : the input image
*/
-void vpMbKltMultiTracker::track(const vpImage<unsigned char> &I) {
- //Track only with reference camera
- //Get the reference camera parameters
+void vpMbKltMultiTracker::track(const vpImage<unsigned char> &I)
+{
+ // Track only with reference camera
+ // Get the reference camera parameters
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.find(m_referenceCameraName);
- if(it != m_mapOfKltTrackers.end()) {
+ if (it != m_mapOfKltTrackers.end()) {
it->second->track(I);
it->second->getPose(cMo);
} else {
@@ -2537,8 +2749,9 @@ void vpMbKltMultiTracker::track(const vpImage<unsigned char> &I) {
\param I1 : The first image.
\param I2 : The second image.
*/
-void vpMbKltMultiTracker::track(const vpImage<unsigned char>& I1, const vpImage<unsigned char>& I2) {
- if(m_mapOfKltTrackers.size() == 2) {
+void vpMbKltMultiTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
+{
+ if (m_mapOfKltTrackers.size() == 2) {
std::map<std::string, vpMbKltTracker *>::const_iterator it = m_mapOfKltTrackers.begin();
std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
mapOfImages[it->first] = &I1;
@@ -2560,41 +2773,55 @@ void vpMbKltMultiTracker::track(const vpImage<unsigned char>& I1, const vpImage<
\param mapOfImages : Map of images.
*/
-void vpMbKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages) {
- //Check if there is an image for each camera
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it_klt = m_mapOfKltTrackers.begin();
- it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
- std::map<std::string, const vpImage<unsigned char>* >::const_iterator it_img = mapOfImages.find(it_klt->first);
+void vpMbKltMultiTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ // Check if there is an image for each camera
+ for (std::map<std::string, vpMbKltTracker *>::const_iterator it_klt = m_mapOfKltTrackers.begin();
+ it_klt != m_mapOfKltTrackers.end(); ++it_klt) {
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.find(it_klt->first);
- if(it_img == mapOfImages.end()) {
+ if (it_img == mapOfImages.end()) {
throw vpException(vpTrackingException::fatalError, "Missing images !");
}
}
- std::map<std::string, unsigned int> mapOfNbInfos;
- std::map<std::string, unsigned int> mapOfNbFaceUsed;
-
- preTracking(mapOfImages, mapOfNbInfos, mapOfNbFaceUsed);
-
- bool atLeastOneTrackerOk = false;
- for(std::map<std::string, vpMbKltTracker*>::const_iterator it = m_mapOfKltTrackers.begin();
- it != m_mapOfKltTrackers.end(); ++it) {
- if(mapOfNbInfos[it->first] >= 4 && mapOfNbFaceUsed[it->first] != 0) {
- atLeastOneTrackerOk = true;
- }
- }
+ preTracking(mapOfImages);
- if(!atLeastOneTrackerOk) {
- vpERROR_TRACE("\n\t\t Error-> not enough data") ;
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
+ if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
}
- computeVVS(mapOfNbInfos, m_w);
+ computeVVS();
- postTracking(mapOfImages, mapOfNbInfos, m_w);
+ postTracking(mapOfImages);
}
+//////////////// Deprecated ////////////////
+/*!
+ Get the current number of klt points for each camera.
+ \deprecated Use rather getKltNbPoints()
+
+ \return The number of features
+*/
+std::map<std::string, int> vpMbKltMultiTracker::getNbKltPoints() const { return getKltNbPoints(); }
+
+/*!
+ Set the erosion of the mask used on the Model faces.
+ \deprecated Use rather setKltMaskBorder()
+
+ \param e : The desired erosion.
+*/
+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); }
+
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
-void dummy_vpMbKltMultiTracker() {};
-#endif //VISP_HAVE_OPENCV
+// Work arround to avoid warning:
+// libvisp_mbt.a(dummy_vpMbKltMultiTracker.cpp.o) has no symbols
+void dummy_vpMbKltMultiTracker(){};
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp
index 740a692..20af8e2 100644
--- a/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp
+++ b/modules/tracker/mbt/src/klt/vpMbKltTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,14 +38,14 @@
*****************************************************************************/
#include <visp3/core/vpImageConvert.h>
-#include <visp3/mbt/vpMbKltTracker.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/core/vpTrackingException.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/mbt/vpMbKltTracker.h>
#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 <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
#endif
vpMbKltTracker::vpMbKltTracker()
@@ -54,10 +55,10 @@ vpMbKltTracker::vpMbKltTracker()
#else
cur(NULL),
#endif
- c0Mo(), compute_interaction(true),
- firstInitialisation(true), maskBorder(5), lambda(0.8), maxIter(200), threshold_outlier(0.5),
- percentGood(0.6), ctTc0(), tracker(), kltPolygons(), kltCylinders(), circles_disp()
-{
+ 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);
tracker.setMaxFeatures(10000);
@@ -67,13 +68,16 @@ vpMbKltTracker::vpMbKltTracker()
tracker.setHarrisFreeParameter(0.01);
tracker.setBlockSize(3);
tracker.setPyramidLevels(3);
-
+
angleAppears = vpMath::rad(65);
angleDisappears = vpMath::rad(75);
#ifdef VISP_HAVE_OGRE
faces.getOgreContext()->setWindowName("MBT Klt");
#endif
+
+ m_lambda = 0.8;
+ m_maxIter = 200;
}
/*!
@@ -83,70 +87,67 @@ vpMbKltTracker::vpMbKltTracker()
vpMbKltTracker::~vpMbKltTracker()
{
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
+ if (cur != NULL) {
cvReleaseImage(&cur);
cur = NULL;
}
#endif
// delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly != NULL) {
+ delete kltpoly;
}
- kltpoly = NULL ;
+ kltpoly = NULL;
}
kltPolygons.clear();
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if (kltPolyCylinder!=NULL){
- delete kltPolyCylinder ;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder != NULL) {
+ delete kltPolyCylinder;
}
- kltPolyCylinder = NULL ;
+ kltPolyCylinder = NULL;
}
kltCylinders.clear();
// delete the structures used to display circles
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
+ vpMbtDistanceCircle *ci = *it;
+ if (ci != NULL) {
+ delete ci;
}
- ci = NULL ;
+ ci = NULL;
}
circles_disp.clear();
}
-void
-vpMbKltTracker::init(const vpImage<unsigned char>& I)
+void vpMbKltTracker::init(const vpImage<unsigned char> &I)
{
- if(!modelInitialised){
+ if (!modelInitialised) {
throw vpException(vpException::fatalError, "model not initialized");
}
-
- bool reInitialisation = false;
- if(!useOgre)
+
+ bool reInitialisation = false;
+ if (!useOgre)
faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
- else{
-#ifdef VISP_HAVE_OGRE
- if(!faces.isOgreInitialised()){
+ else {
+#ifdef VISP_HAVE_OGRE
+ if (!faces.isOgreInitialised()) {
faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
faces.setOgreShowConfigDialog(ogreShowConfigDialog);
faces.initOgre(cam);
- // Turn off Ogre config dialog display for the next call to this function
- // since settings are saved in the ogre.cfg file and used during the next
- // call
- ogreShowConfigDialog = false;
+ // Turn off Ogre config dialog display for the next call to this
+ // function since settings are saved in the ogre.cfg file and used
+ // during the next call
+ ogreShowConfigDialog = false;
}
-
+
faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
-
+
#else
faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
#endif
@@ -154,8 +155,7 @@ vpMbKltTracker::init(const vpImage<unsigned char>& I)
reinit(I);
}
-void
-vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
+void vpMbKltTracker::reinit(const vpImage<unsigned char> &I)
{
c0Mo = cMo;
ctTc0.eye();
@@ -164,46 +164,44 @@ vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
cam.computeFov(I.getWidth(), I.getHeight());
- if(useScanLine){
- faces.computeClippedPolygons(cMo,cam);
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
-
- // mask
+
+// mask
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat mask((int)I.getRows(), (int)I.getCols(), CV_8UC1, cv::Scalar(0));
#else
- IplImage* mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
+ IplImage *mask = cvCreateImage(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
cvZero(mask);
#endif
vpMbtDistanceKltPoints *kltpoly;
vpMbtDistanceKltCylinder *kltPolyCylinder;
- if(useScanLine){
+ if (useScanLine) {
vpImageConvert::convert(faces.getMbScanLineRenderer().getMask(), mask);
- }
- else{
- unsigned char val = 255/* - i*15*/;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ } else {
+ unsigned char val = 255 /* - i*15*/;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
- //need to changeFrame when reinit() is called by postTracking
- //other solution is
+ 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->updateMask(mask, val, maskBorder);
}
}
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
kltPolyCylinder = *it;
- if(kltPolyCylinder->isTracked())
- {
- for(unsigned int k = 0 ; k < kltPolyCylinder->listIndicesCylinderBBox.size() ; k++)
- {
+ 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){
+ if (faces[indCylBBox]->isVisible() && faces[indCylBBox]->getNbPoint() > 2u) {
faces[indCylBBox]->computePolygonClipped(cam); // Might not be necessary when scanline is activated
}
}
@@ -212,22 +210,25 @@ vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
}
}
}
-
+
tracker.initTracking(cur, mask);
-// tracker.track(cur); // AY: Not sure to be usefull but makes sure that the points are valid for tracking and avoid too fast reinitialisations.
-// vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << " points" << std::endl;
+ // tracker.track(cur); // AY: Not sure to be usefull but makes sure that
+ // the points are valid for tracking and avoid too fast reinitialisations.
+ // vpCTRACE << "init klt. detected " << tracker.getNbFeatures() << "
+ // points" << std::endl;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
kltpoly->init(tracker);
}
}
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
kltPolyCylinder = *it;
- if(kltPolyCylinder->isTracked())
+ if (kltPolyCylinder->isTracked())
kltPolyCylinder->init(tracker, cMo);
}
@@ -240,58 +241,55 @@ vpMbKltTracker::reinit(const vpImage<unsigned char>& I)
Reset the tracker. The model is removed and the pose is set to identity.
The tracker needs to be initialized with a new model and a new pose.
*/
-void
-vpMbKltTracker::resetTracker()
+void vpMbKltTracker::resetTracker()
{
cMo.eye();
-
+
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
+ if (cur != NULL) {
cvReleaseImage(&cur);
cur = NULL;
}
#endif
// delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly != NULL) {
+ delete kltpoly;
}
- kltpoly = NULL ;
+ kltpoly = NULL;
}
kltPolygons.clear();
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if (kltPolyCylinder!=NULL){
- delete kltPolyCylinder ;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder != NULL) {
+ delete kltPolyCylinder;
}
- kltPolyCylinder = NULL ;
+ kltPolyCylinder = NULL;
}
kltCylinders.clear();
// delete the structures used to display circles
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
+ vpMbtDistanceCircle *ci = *it;
+ if (ci != NULL) {
+ delete ci;
}
- ci = NULL ;
+ ci = NULL;
}
circles_disp.clear();
- compute_interaction = true;
+ m_computeInteraction = true;
firstInitialisation = true;
computeCovariance = false;
tracker.setTrackerId(1);
tracker.setUseHarris(1);
-
+
tracker.setMaxFeatures(10000);
tracker.setWindowSize(5);
tracker.setQuality(0.01);
@@ -299,25 +297,25 @@ vpMbKltTracker::resetTracker()
tracker.setHarrisFreeParameter(0.01);
tracker.setBlockSize(3);
tracker.setPyramidLevels(3);
-
+
angleAppears = vpMath::rad(65);
angleDisappears = vpMath::rad(75);
-
+
clippingFlag = vpPolygon3D::NO_CLIPPING;
-
+
maskBorder = 5;
threshold_outlier = 0.5;
- percentGood = 0.7;
-
- lambda = 0.8;
- maxIter = 200;
+ percentGood = 0.6;
+
+ m_lambda = 0.8;
+ m_maxIter = 200;
faces.reset();
m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
useScanLine = false;
-
+
#ifdef VISP_HAVE_OGRE
useOgre = false;
#endif
@@ -325,37 +323,37 @@ vpMbKltTracker::resetTracker()
/*!
Get the current list of KLT points.
-
- \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
-
+
+ \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f.
+ This function convert and copy the openCV KLT points into vpImagePoints.
+
\return the list of KLT points through vpKltOpencv.
*/
-std::vector<vpImagePoint>
-vpMbKltTracker::getKltImagePoints() const
+std::vector<vpImagePoint> vpMbKltTracker::getKltImagePoints() const
{
std::vector<vpImagePoint> kltPoints;
- for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
long id;
float x_tmp, y_tmp;
tracker.getFeature((int)i, id, x_tmp, y_tmp);
kltPoints.push_back(vpImagePoint(y_tmp, x_tmp));
}
-
+
return kltPoints;
}
/*!
Get the current list of KLT points and their id.
-
- \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f. This function convert and copy the openCV KLT points into vpImagePoints.
-
+
+ \warning Contrary to getKltPoints which returns a pointer on CvPoint2D32f.
+ This function convert and copy the openCV KLT points into vpImagePoints.
+
\return the list of KLT points and their id through vpKltOpencv.
*/
-std::map<int, vpImagePoint>
-vpMbKltTracker::getKltImagePointsWithId() const
+std::map<int, vpImagePoint> vpMbKltTracker::getKltImagePointsWithId() const
{
std::map<int, vpImagePoint> kltPoints;
- for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i ++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(tracker.getNbFeatures()); i++) {
long id;
float x_tmp, y_tmp;
tracker.getFeature((int)i, id, x_tmp, y_tmp);
@@ -365,7 +363,7 @@ vpMbKltTracker::getKltImagePointsWithId() const
kltPoints[id] = vpImagePoint(y_tmp, x_tmp);
#endif
}
-
+
return kltPoints;
}
@@ -374,8 +372,8 @@ vpMbKltTracker::getKltImagePointsWithId() const
\param t : Klt tracker containing the new values.
*/
-void
-vpMbKltTracker::setKltOpencv(const vpKltOpencv& t){
+void vpMbKltTracker::setKltOpencv(const vpKltOpencv &t)
+{
tracker.setMaxFeatures(t.getMaxFeatures());
tracker.setWindowSize(t.getWindowSize());
tracker.setQuality(t.getQuality());
@@ -390,22 +388,20 @@ vpMbKltTracker::setKltOpencv(const vpKltOpencv& t){
\param camera : the new camera parameters.
*/
-void
-vpMbKltTracker::setCameraParameters(const vpCameraParameters& camera)
+void vpMbKltTracker::setCameraParameters(const vpCameraParameters &camera)
{
-// for (unsigned int i = 0; i < faces.size(); i += 1){
-// faces[i]->setCameraParameters(camera);
-// }
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ // faces[i]->setCameraParameters(camera);
+ // }
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
kltpoly->setCameraParameters(camera);
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
kltPolyCylinder->setCameraParameters(camera);
}
@@ -413,26 +409,24 @@ vpMbKltTracker::setCameraParameters(const vpCameraParameters& camera)
}
/*!
- Set the pose to be used in entry (as guess) of the next call to the track() function.
- This pose will be just used once.
+ Set the pose to be used in entry (as guess) of the next call to the track()
+ function. This pose will be just used once.
\warning This functionnality is not available when tracking cylinders.
\param I : image corresponding to the desired pose.
\param cdMo : Pose to affect.
*/
-void
-vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix& cdMo)
+void vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
{
- if((int)(kltCylinders.size()) != 0)
- {
- std::cout << "WARNING: Cannot set pose when model contains cylinder(s). This feature is not implemented yet." << std::endl;
+ 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;
cMo = cdMo;
init(I);
- }
- else
- {
+ } else {
vpMbtDistanceKltPoints *kltpoly;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
@@ -441,19 +435,19 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
std::vector<cv::Point2f> guess_pts;
#else
unsigned int nbp = 0;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
+ for (std::list<vpMbtDistanceKltPoints *>::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)
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));
+ CvPoint2D32f *init_pts = NULL;
+ init_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(init_pts[0]));
+ long *init_ids = (long *)cvAlloc((unsigned int)tracker.getMaxFeatures() * sizeof(long));
unsigned int iter_pts = 0;
- CvPoint2D32f* guess_pts = NULL;
- guess_pts = (CvPoint2D32f*)cvAlloc(tracker.getMaxFeatures()*sizeof(guess_pts[0]));
+ CvPoint2D32f *guess_pts = NULL;
+ guess_pts = (CvPoint2D32f *)cvAlloc(tracker.getMaxFeatures() * sizeof(guess_pts[0]));
#endif
vpHomogeneousMatrix cdMc = cdMo * cMo.inverse();
@@ -465,15 +459,15 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
cdMc.extract(cdRc);
cdMc.extract(cdtc);
- unsigned int nbCur = 0;
+ // unsigned int nbCur = 0;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it) {
+ for (std::list<vpMbtDistanceKltPoints *>::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
+ // Get the normal to the face at the current state cMo
vpPlane plan(kltpoly->polygon->p[0], kltpoly->polygon->p[1], kltpoly->polygon->p[2]);
plan.changeFrame(cMcd);
@@ -482,29 +476,46 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
double invDc = 1.0 / plan.getD();
- //Create the homography
+ // Create the homography
vpMatrix cdHc;
vpGEMM(cdtc, Nc, -invDc, cdRc, 1.0, cdHc, VP_GEMM_B_T);
cdHc /= cdHc[2][2];
- //Create the 2D homography
+ // Create the 2D homography
vpMatrix cdGc = cam.get_K() * cdHc * cam.get_K_inverse();
- //Points displacement
+ // Points displacement
std::map<int, vpImagePoint>::const_iterator iter = kltpoly->getCurrentPoints().begin();
- nbCur+= (unsigned int)kltpoly->getCurrentPoints().size();
- for( ; iter != kltpoly->getCurrentPoints().end(); iter++){
+ // 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]) !=
+ init_ids.end())
+#else
+ 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
+ // vpMbtDistanceKltPoints due to possible overlapping faces)
+ continue;
+ }
+#endif
+
vpColVector cdp(3);
- cdp[0] = iter->second.get_j(); cdp[1] = iter->second.get_i(); cdp[2] = 1.0;
+ cdp[0] = iter->second.get_j();
+ cdp[1] = iter->second.get_i();
+ cdp[2] = 1.0;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Point2f p((float)cdp[0], (float)cdp[1]);
init_pts.push_back(p);
-# if TARGET_OS_IPHONE
+#if TARGET_OS_IPHONE
init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(int)iter->first]);
-# else
+#else
init_ids.push_back((size_t)(kltpoly->getCurrentPointsInd())[(size_t)iter->first]);
-# endif
+#endif
#else
init_pts[iter_pts].x = (float)cdp[0];
init_pts[iter_pts].y = (float)cdp[1];
@@ -513,7 +524,7 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
double p_mu_t_2 = cdp[0] * cdGc[2][0] + cdp[1] * cdGc[2][1] + cdGc[2][2];
- if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
+ if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
cdp[0] = 0.0;
cdp[1] = 0.0;
throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
@@ -522,7 +533,7 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
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
+// Set value to the KLT tracker
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Point2f p_guess((float)cdp[0], (float)cdp[1]);
guess_pts.push_back(p_guess);
@@ -541,20 +552,23 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
#else
tracker.setInitialGuess(&init_pts, &guess_pts, init_ids, iter_pts);
- if(init_pts) cvFree(&init_pts);
+ if (init_pts)
+ cvFree(&init_pts);
init_pts = NULL;
- if(guess_pts) cvFree(&guess_pts);
+ if (guess_pts)
+ cvFree(&guess_pts);
guess_pts = NULL;
- if(init_ids)cvFree(&init_ids);
+ if (init_ids)
+ cvFree(&init_ids);
init_ids = NULL;
#endif
bool reInitialisation = false;
- if(!useOgre)
+ if (!useOgre)
faces.setVisible(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
- else{
+ else {
#ifdef VISP_HAVE_OGRE
faces.setVisibleOgre(I, cam, cdMo, angleAppears, angleDisappears, reInitialisation);
#else
@@ -564,14 +578,14 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
cam.computeFov(I.getWidth(), I.getHeight());
- if(useScanLine){
- faces.computeClippedPolygons(cdMo,cam);
+ if (useScanLine) {
+ faces.computeClippedPolygons(cdMo, cam);
faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
}
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ for (std::list<vpMbtDistanceKltPoints *>::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);
}
@@ -586,73 +600,69 @@ vpMbKltTracker::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatr
/*!
Initialise a new face from the coordinates given in parameter.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
+void vpMbKltTracker::initFaceFromCorners(vpMbtPolygon &polygon)
{
- vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
- kltPoly->setCameraParameters(cam) ;
- kltPoly->polygon = &polygon;
- kltPoly->hiddenface = &faces ;
- kltPoly->useScanLine = useScanLine;
- kltPolygons.push_back(kltPoly);
+ vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
+ kltPoly->setCameraParameters(cam);
+ kltPoly->polygon = &polygon;
+ kltPoly->hiddenface = &faces;
+ kltPoly->useScanLine = useScanLine;
+ kltPolygons.push_back(kltPoly);
}
/*!
Initialise a new face from the coordinates given in parameter.
- \param polygon : The polygon describing the set of lines that has to be tracked.
+ \param polygon : The polygon describing the set of lines that has to be
+ tracked.
*/
-void
-vpMbKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
+void vpMbKltTracker::initFaceFromLines(vpMbtPolygon &polygon)
{
- vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
- kltPoly->setCameraParameters(cam) ;
- kltPoly->polygon = &polygon;
- kltPoly->hiddenface = &faces ;
- kltPoly->useScanLine = useScanLine;
- kltPolygons.push_back(kltPoly);
+ vpMbtDistanceKltPoints *kltPoly = new vpMbtDistanceKltPoints();
+ kltPoly->setCameraParameters(cam);
+ kltPoly->polygon = &polygon;
+ kltPoly->hiddenface = &faces;
+ kltPoly->useScanLine = useScanLine;
+ kltPolygons.push_back(kltPoly);
}
/*!
- Achieve the tracking of the KLT features and associate the features to the faces.
+ Achieve the tracking of the KLT features and associate the features to the
+ faces.
\param I : The input image.
- \param nbInfos : Size of the features.
- \param nbFaceUsed : Number of face used for the tracking.
*/
-void
-vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInfos, unsigned int &nbFaceUsed)
+void vpMbKltTracker::preTracking(const vpImage<unsigned char> &I)
{
vpImageConvert::convert(I, cur);
tracker.track(cur);
-
- nbInfos = 0;
- nbFaceUsed = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+
+ m_nbInfos = 0;
+ m_nbFaceUsed = 0;
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
kltpoly->computeNbDetectedCurrent(tracker);
-// faces[i]->ransac();
- if(kltpoly->hasEnoughPoints()){
- nbInfos += kltpoly->getCurrentNumberPoints();
- nbFaceUsed++;
+ // faces[i]->ransac();
+ if (kltpoly->hasEnoughPoints()) {
+ m_nbInfos += kltpoly->getCurrentNumberPoints();
+ m_nbFaceUsed++;
}
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
- if(kltPolyCylinder->isTracked())
- {
+ if (kltPolyCylinder->isTracked()) {
kltPolyCylinder->computeNbDetectedCurrent(tracker);
- if(kltPolyCylinder->hasEnoughPoints()){
- nbInfos += kltPolyCylinder->getCurrentNumberPoints();
- nbFaceUsed++;
+ if (kltPolyCylinder->hasEnoughPoints()) {
+ m_nbInfos += kltPolyCylinder->getCurrentNumberPoints();
+ m_nbFaceUsed++;
}
}
}
@@ -661,319 +671,205 @@ vpMbKltTracker::preTracking(const vpImage<unsigned char>& I, unsigned int &nbInf
/*!
Realize the post tracking operations. Mostly visibility tests
*/
-bool
-vpMbKltTracker::postTracking(const vpImage<unsigned char>& I, vpColVector &w)
+bool vpMbKltTracker::postTracking(const vpImage<unsigned char> &I, vpColVector &w)
{
- // # For a better Post Tracking, tracker should reinitialize if so faces don't have enough points but are visible.
- // # Here we are not doing it for more speed performance.
+ // # For a better Post Tracking, tracker should reinitialize if so faces
+ // don't have enough points but are visible. # Here we are not doing it for
+ // more speed performance.
bool reInitialisation = false;
-
+
unsigned int initialNumber = 0;
unsigned int currentNumber = 0;
unsigned int shift = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2){
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2) {
initialNumber += kltpoly->getInitialNumberPoint();
- if(kltpoly->hasEnoughPoints()){
- vpSubColVector sub_w(w, shift, 2*kltpoly->getCurrentNumberPoints());
- shift += 2*kltpoly->getCurrentNumberPoints();
+ if (kltpoly->hasEnoughPoints()) {
+ vpSubColVector sub_w(w, shift, 2 * kltpoly->getCurrentNumberPoints());
+ shift += 2 * kltpoly->getCurrentNumberPoints();
kltpoly->removeOutliers(sub_w, threshold_outlier);
-
+
currentNumber += kltpoly->getCurrentNumberPoints();
}
-// else{
-// reInitialisation = true;
-// break;
-// }
+ // else{
+ // reInitialisation = true;
+ // break;
+ // }
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
- if(kltPolyCylinder->isTracked())
- {
+ if (kltPolyCylinder->isTracked()) {
initialNumber += kltPolyCylinder->getInitialNumberPoint();
- if(kltPolyCylinder->hasEnoughPoints()){
- vpSubColVector sub_w(w, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
- shift += 2*kltPolyCylinder->getCurrentNumberPoints();
+ if (kltPolyCylinder->hasEnoughPoints()) {
+ vpSubColVector sub_w(w, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
+ shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
kltPolyCylinder->removeOutliers(sub_w, threshold_outlier);
currentNumber += kltPolyCylinder->getCurrentNumberPoints();
}
}
}
-
-// if(!reInitialisation){
- double value = percentGood * (double)initialNumber;
- if((double)currentNumber < value){
-// std::cout << "Too many point disappear : " << initialNumber << "/" << currentNumber << std::endl;
- reInitialisation = true;
- }
- else{
- if(!useOgre)
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
- else{
-#ifdef VISP_HAVE_OGRE
- faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+
+ // if(!reInitialisation){
+ double value = percentGood * (double)initialNumber;
+ if ((double)currentNumber < value) {
+ // std::cout << "Too many point disappear : " << initialNumber << "/"
+ // << currentNumber << std::endl;
+ reInitialisation = true;
+ } else {
+ if (!useOgre)
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ else {
+#ifdef VISP_HAVE_OGRE
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
#else
- faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
#endif
- }
}
-// }
-
- if(reInitialisation)
+ }
+ // }
+
+ if (reInitialisation)
return true;
-
+
return false;
}
/*!
Realize the VVS loop for the tracking
-
- \param nbInfos : Size of the features
- \param w : weight of the features after M-Estimation.
*/
-void
-vpMbKltTracker::computeVVS(const unsigned int &nbInfos, vpColVector &w)
+void vpMbKltTracker::computeVVS()
{
- vpMatrix L; // interaction matrix
- vpColVector R; // residu
- vpMatrix L_true; // interaction matrix
+ vpMatrix L_true; // interaction matrix without weighting
vpMatrix LVJ_true;
- //vpColVector R_true; // residu
- vpColVector v; // "speed" for VVS
- vpHomography H;
- vpColVector w_true;
- vpRobust robust(2*nbInfos);
+ vpColVector v; // "speed" for VVS
vpMatrix LTL;
vpColVector LTR;
vpHomogeneousMatrix cMoPrev;
vpHomogeneousMatrix ctTc0_Prev;
- vpColVector error_prev(2*nbInfos);
- double mu = 0.01;
-
+ vpColVector error_prev;
+ double mu = m_initialMu;
+
double normRes = 0;
double normRes_1 = -1;
unsigned int iter = 0;
- R.resize(2*nbInfos);
- L.resize(2*nbInfos, 6, 0);
-
- while( ((int)((normRes - normRes_1)*1e8) != 0 ) && (iter<maxIter) ){
-
- unsigned int shift = 0;
+ vpMbKltTracker::computeVVSInit();
- computeVVSInteractionMatrixAndResidu(shift, R, L, H, kltPolygons, kltCylinders, ctTc0);
+ while (((int)((normRes - normRes_1) * 1e8) != 0) && (iter < m_maxIter)) {
+ vpMbKltTracker::computeVVSInteractionMatrixAndResidu();
bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error_klt, error_prev, cMoPrev, mu, reStartFromLastIncrement);
+ if (reStartFromLastIncrement) {
+ ctTc0 = ctTc0_Prev;
+ }
- computeVVSCheckLevenbergMarquardtKlt(iter, nbInfos, cMoPrev, error_prev, ctTc0_Prev, mu, reStartFromLastIncrement);
+ if (!reStartFromLastIncrement) {
+ vpMbTracker::computeVVSWeights(m_robust_klt, m_error_klt, m_w_klt);
- if(!reStartFromLastIncrement){
- computeVVSWeights(iter, nbInfos, R, w_true, w, robust);
+ if (computeCovariance) {
+ L_true = m_L_klt;
+ if (!isoJoIdentity) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L_klt * cVo * oJo);
+ }
+ }
- computeVVSPoseEstimation(iter, L, w, L_true, LVJ_true, normRes, normRes_1, w_true, R, LTL, LTR,
- error_prev, v, mu, cMoPrev, ctTc0_Prev);
- } // endif(!reStartFromLastIncrement)
-
- iter++;
- }
-
- if(computeCovariance){
- computeVVSCovariance(w_true, cMoPrev, L_true, LVJ_true);
- }
-}
+ normRes_1 = normRes;
+ normRes = 0.0;
-void
-vpMbKltTracker::computeVVSCheckLevenbergMarquardtKlt(const unsigned int iter, const unsigned int nbInfos,
- const vpHomogeneousMatrix &cMoPrev, const vpColVector &error_prev, const vpHomogeneousMatrix &ctTc0_Prev,
- double &mu, bool &reStartFromLastIncrement) {
- if(iter != 0 && m_optimizationMethod == vpMbTracker::LEVENBERG_MARQUARDT_OPT){
- if(m_error.sumSquare()/(double)(2*nbInfos) > error_prev.sumSquare()/(double)(2*nbInfos)){
- mu *= 10.0;
+ for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
+ m_weightedError_klt[i] = m_error_klt[i] * m_w_klt[i];
+ normRes += m_weightedError_klt[i];
+ }
- if(mu > 1.0)
- throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+ if ((iter == 0) || m_computeInteraction) {
+ for (unsigned int i = 0; i < m_error_klt.getRows(); i++) {
+ for (unsigned int j = 0; j < 6; j++) {
+ m_L_klt[i][j] *= m_w_klt[i];
+ }
+ }
+ }
- cMo = cMoPrev;
- m_error = error_prev;
- ctTc0 = ctTc0_Prev;
- reStartFromLastIncrement = true;
- }
- }
-}
+ computeVVSPoseEstimation(isoJoIdentity, iter, m_L_klt, LTL, m_weightedError_klt, m_error_klt, error_prev, LTR, mu,
+ v);
-void
-vpMbKltTracker::computeVVSCovariance(const vpColVector &w_true, const vpHomogeneousMatrix &cMoPrev,
- const vpMatrix &L_true, const vpMatrix &LVJ_true) {
- if(computeCovariance){
- vpMatrix D;
- D.diag(w_true);
+ cMoPrev = cMo;
+ ctTc0_Prev = ctTc0;
+ ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
+ cMo = ctTc0 * c0Mo;
+ } // endif(!reStartFromLastIncrement)
- // Note that here the covariance is computed on cMoPrev for time computation efficiency
- if(isoJoIdentity){
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,L_true,D);
- }
- else{
- covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev,m_error,LVJ_true,D);
- }
+ iter++;
}
+
+ computeCovarianceMatrixVVS(isoJoIdentity, m_w_klt, cMoPrev, L_true, LVJ_true, m_error_klt);
}
-void
-vpMbKltTracker::computeVVSInteractionMatrixAndResidu(unsigned int shift, vpColVector &R, vpMatrix &L, vpHomography &H,
- std::list<vpMbtDistanceKltPoints*> &kltPolygons_, std::list<vpMbtDistanceKltCylinder*> &kltCylinders_,
- const vpHomogeneousMatrix &ctTc0_) {
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it = kltPolygons_.begin(); it != kltPolygons_.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
- kltpoly->hasEnoughPoints()){
- vpSubColVector subR(R, shift, 2*kltpoly->getCurrentNumberPoints());
- vpSubMatrix subL(L, shift, 0, 2*kltpoly->getCurrentNumberPoints(), 6);
- try{
- kltpoly->computeHomography(ctTc0_, H);
- kltpoly->computeInteractionMatrixAndResidu(subR, subL);
- }catch(...){
- throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
- }
+void vpMbKltTracker::computeVVSInit()
+{
+ unsigned int nbFeatures = 2 * m_nbInfos;
- shift += 2*kltpoly->getCurrentNumberPoints();
- }
- }
+ m_L_klt.resize(nbFeatures, 6, false, false);
+ m_error_klt.resize(nbFeatures, false);
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it = kltCylinders_.begin(); it != kltCylinders_.end(); ++it){
- kltPolyCylinder = *it;
+ m_weightedError_klt.resize(nbFeatures, false);
+ m_w_klt.resize(nbFeatures, false);
+ m_w_klt = 1;
- if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
- {
- vpSubColVector subR(R, shift, 2*kltPolyCylinder->getCurrentNumberPoints());
- vpSubMatrix subL(L, shift, 0, 2*kltPolyCylinder->getCurrentNumberPoints(), 6);
- try{
- kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0_,subR, subL);
- }catch(...){
- throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
- }
-
- shift += 2*kltPolyCylinder->getCurrentNumberPoints();
- }
- }
+ m_robust_klt.resize(nbFeatures);
+ m_robust_klt.setThreshold(2 / cam.get_px());
}
-void
-vpMbKltTracker::computeVVSPoseEstimation(const unsigned int iter, vpMatrix &L,
- const vpColVector &w, vpMatrix &L_true, vpMatrix &LVJ_true, double &normRes, double &normRes_1, vpColVector &w_true,
- vpColVector &R, vpMatrix <L, vpColVector <R, vpColVector &error_prev, vpColVector &v, double &mu,
- vpHomogeneousMatrix &cMoPrev, vpHomogeneousMatrix &ctTc0_Prev) {
- m_error = R;
- if(computeCovariance){
- L_true = L;
- if(!isoJoIdentity){
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- LVJ_true = (L*cVo*oJo);
- }
- }
+void vpMbKltTracker::computeVVSInteractionMatrixAndResidu()
+{
+ unsigned int shift = 0;
+ vpHomography H;
- normRes_1 = normRes;
- normRes = 0;
- for (unsigned int i = 0; i < static_cast<unsigned int>(R.getRows()); i += 1){
- w_true[i] = w[i];
- R[i] = R[i] * w[i];
- normRes += R[i];
- }
+ for (std::list<vpMbtDistanceKltPoints *>::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((iter == 0) || compute_interaction){
- for(unsigned int i=0; i<static_cast<unsigned int>(R.getRows()); i++){
- for(unsigned int j=0; j<6; j++){
- L[i][j] *= w[i];
+ try {
+ kltpoly->computeHomography(ctTc0, H);
+ kltpoly->computeInteractionMatrixAndResidu(subR, subL);
+ } catch (...) {
+ throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
}
+
+ shift += 2 * kltpoly->getCurrentNumberPoints();
}
}
- if(isoJoIdentity){
- LTL = L.AtA();
- computeJTR(L, R, LTR);
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LTL.getRows(), LTL.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LTL + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LTR;
+ if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints()) {
+ vpSubColVector subR(m_error_klt, shift, 2 * kltPolyCylinder->getCurrentNumberPoints());
+ vpSubMatrix subL(m_L_klt, shift, 0, 2 * kltPolyCylinder->getCurrentNumberPoints(), 6);
- if(iter != 0)
- mu /= 10.0;
-
- error_prev = m_error;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- v = -lambda * LTL.pseudoInverse(LTL.getRows()*std::numeric_limits<double>::epsilon()) * LTR;
- }
- }
- else{
- vpVelocityTwistMatrix cVo;
- cVo.buildFrom(cMo);
- vpMatrix LVJ = (L*cVo*oJo);
- vpMatrix LVJTLVJ = (LVJ).AtA();
- vpColVector LVJTR;
- computeJTR(LVJ, R, LVJTR);
-
- switch(m_optimizationMethod){
- case vpMbTracker::LEVENBERG_MARQUARDT_OPT:
- {
- vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
- LMA.eye();
- vpMatrix LTLmuI = LVJTLVJ + (LMA*mu);
- v = -lambda*LTLmuI.pseudoInverse(LTLmuI.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
-
- if(iter != 0)
- mu /= 10.0;
-
- error_prev = m_error;
- break;
- }
- case vpMbTracker::GAUSS_NEWTON_OPT:
- default:
- {
- v = -lambda*LVJTLVJ.pseudoInverse(LVJTLVJ.getRows()*std::numeric_limits<double>::epsilon())*LVJTR;
- v = cVo * v;
- break;
- }
+ try {
+ kltPolyCylinder->computeInteractionMatrixAndResidu(ctTc0, subR, subL);
+ } catch (...) {
+ throw vpTrackingException(vpTrackingException::fatalError, "Cannot compute interaction matrix");
}
- }
-
- cMoPrev = cMo;
- ctTc0_Prev = ctTc0;
- ctTc0 = vpExponentialMap::direct(v).inverse() * ctTc0;
- cMo = ctTc0 * c0Mo;
-}
-void
-vpMbKltTracker::computeVVSWeights(const unsigned int iter, const unsigned int nbInfos, const vpColVector &R,
- vpColVector &w_true, vpColVector &w, vpRobust &robust) {
- if(iter == 0){
- w_true.resize(2*nbInfos);
- w.resize(2*nbInfos);
- w = 1;
- w_true = 1;
+ shift += 2 * kltPolyCylinder->getCurrentNumberPoints();
+ }
}
- robust.setIteration(iter);
- robust.setThreshold(2/cam.get_px());
- robust.MEstimator( vpRobust::TUKEY, R, w);
}
/*!
@@ -983,57 +879,47 @@ vpMbKltTracker::computeVVSWeights(const unsigned int iter, const unsigned int nb
\param I : the input image
*/
-void
-vpMbKltTracker::track(const vpImage<unsigned char>& I)
-{
- unsigned int nbInfos = 0;
- unsigned int nbFaceUsed = 0;
-
- try{
- preTracking(I, nbInfos, nbFaceUsed);
- }
- catch(vpException &e){
- throw e;
- }
-
- if(nbInfos < 4 || nbFaceUsed == 0){
- vpERROR_TRACE("\n\t\t Error-> not enough data") ;
- throw vpTrackingException(vpTrackingException::notEnoughPointError, "\n\t\t Error-> not enough data");
+void vpMbKltTracker::track(const vpImage<unsigned char> &I)
+{
+ preTracking(I);
+
+ if (m_nbInfos < 4 || m_nbFaceUsed == 0) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
}
- //vpColVector w;
- computeVVS(nbInfos, m_w);
+ computeVVS();
- if(postTracking(I, m_w))
+ if (postTracking(I, m_w_klt))
reinit(I);
}
/*!
- Load the xml configuration file. An example of such a file is provided in loadConfigFile(const char*) documentation.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ Load the xml configuration file. An example of such a file is provided in
+ loadConfigFile(const char*) documentation. From the configuration file
+ initialize the parameters corresponding to the objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
\param configFile : full name of the xml file.
\sa loadConfigFile(const char*), vpXmlParser::cleanup()
*/
-void
-vpMbKltTracker::loadConfigFile(const std::string& configFile)
+void vpMbKltTracker::loadConfigFile(const std::string &configFile)
{
vpMbKltTracker::loadConfigFile(configFile.c_str());
}
/*!
Load the xml configuration file.
- From the configuration file initialize the parameters corresponding to the objects: KLT, camera.
+ From the configuration file initialize the parameters corresponding to the
+objects: KLT, camera.
- \warning To clean up memory allocated by the xml library, the user has to call
- vpXmlParser::cleanup() before the exit().
+ \warning To clean up memory allocated by the xml library, the user has to
+call vpXmlParser::cleanup() before the exit().
- \throw vpException::ioError if the file has not been properly parsed (file not
- found or wrong format for the data).
+ \throw vpException::ioError if the file has not been properly parsed (file
+not found or wrong format for the data).
\param configFile : full name of the xml file.
@@ -1071,12 +957,11 @@ vpMbKltTracker::loadConfigFile(const std::string& configFile)
\sa loadConfigFile(const std::string&), vpXmlParser::cleanup()
*/
-void
-vpMbKltTracker::loadConfigFile(const char* configFile)
+void vpMbKltTracker::loadConfigFile(const char *configFile)
{
#ifdef VISP_HAVE_XML2
vpMbtKltXmlParser xmlp;
-
+
xmlp.setMaxFeatures(10000);
xmlp.setWindowSize(5);
xmlp.setQuality(0.01);
@@ -1087,12 +972,11 @@ vpMbKltTracker::loadConfigFile(const char* configFile)
xmlp.setMaskBorder(maskBorder);
xmlp.setAngleAppear(vpMath::deg(angleAppears));
xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
-
- try{
+
+ try {
std::cout << " *********** Parsing XML for MBT KLT Tracker ************ " << std::endl;
xmlp.parse(configFile);
- }
- catch(...){
+ } catch (...) {
vpERROR_TRACE("Can't open XML file \"%s\"\n ", configFile);
throw vpException(vpException::ioError, "problem to parse configuration file.");
}
@@ -1100,7 +984,7 @@ vpMbKltTracker::loadConfigFile(const char* configFile)
vpCameraParameters camera;
xmlp.getCameraParameters(camera);
setCameraParameters(camera);
-
+
tracker.setMaxFeatures((int)xmlp.getMaxFeatures());
tracker.setWindowSize((int)xmlp.getWindowSize());
tracker.setQuality(xmlp.getQuality());
@@ -1112,16 +996,16 @@ vpMbKltTracker::loadConfigFile(const char* configFile)
angleAppears = vpMath::rad(xmlp.getAngleAppear());
angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
- //if(useScanLine)
+ // if(useScanLine)
faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
-
- if(xmlp.hasNearClippingDistance())
+
+ if (xmlp.hasNearClippingDistance())
setNearClippingDistance(xmlp.getNearClippingDistance());
-
- if(xmlp.hasFarClippingDistance())
+
+ if (xmlp.hasFarClippingDistance())
setFarClippingDistance(xmlp.getFarClippingDistance());
-
- if(xmlp.getFovClipping())
+
+ if (xmlp.getFovClipping())
setClipping(clippingFlag = clippingFlag | vpPolygon3D::FOV_CLIPPING);
useLodGeneral = xmlp.getLodState();
@@ -1129,7 +1013,7 @@ vpMbKltTracker::loadConfigFile(const char* configFile)
minPolygonAreaThresholdGeneral = xmlp.getMinPolygonAreaThreshold();
applyLodSettingInConfig = false;
- if(this->getNbPolygon() > 0) {
+ if (this->getNbPolygon() > 0) {
applyLodSettingInConfig = true;
setLod(useLodGeneral);
setMinLineLengthThresh(minLineLengthThresholdGeneral);
@@ -1149,57 +1033,60 @@ vpMbKltTracker::loadConfigFile(const char* configFile)
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are visible.
+ \param displayFullModel : Boolean to say if all the model has to be
+ displayed, even the faces that are visible.
*/
-void
-vpMbKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
- const vpColor& col, const unsigned int thickness, const bool displayFullModel)
+void vpMbKltTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
vpCameraParameters c = camera;
- if(clippingFlag > 3) // Contains at least one FOV constraint
+ if (clippingFlag > 3) // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
- vpMbtDistanceKltPoints *kltpoly;
- vpMbtDistanceKltCylinder *kltPolyCylinder;
+ // vpMbtDistanceKltPoints *kltpoly;
+ // vpMbtDistanceKltCylinder *kltPolyCylinder;
// Previous version 12/08/2015
-// for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
-// kltpoly = *it;
-// kltpoly->polygon->changeFrame(cMo_);
-// kltpoly->polygon->computePolygonClipped(c);
-// }
- faces.computeClippedPolygons(cMo_,c);
+ // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
+ // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ // kltpoly = *it;
+ // kltpoly->polygon->changeFrame(cMo_);
+ // kltpoly->polygon->computePolygonClipped(c);
+ // }
+ faces.computeClippedPolygons(cMo_, c);
+
+ if (useScanLine && !displayFullModel)
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
- if(useScanLine && !displayFullModel)
- faces.computeScanLineRender(cam,I.getWidth(), I.getHeight());
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
+ kltpoly->display(I, cMo_, camera, col, thickness, displayFullModel);
- 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);
+ // faces[i]->displayNormal(I);
}
}
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
+ for (std::list<vpMbtDistanceKltCylinder *>::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<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
@@ -1212,57 +1099,60 @@ vpMbKltTracker::display(const vpImage<unsigned char>& I, const vpHomogeneousMatr
\param camera : The camera parameters.
\param col : The desired color.
\param thickness : The thickness of the lines.
- \param displayFullModel : Boolean to say if all the model has to be displayed, even the faces that are not visible.
+ \param displayFullModel : Boolean to say if all the model has to be
+ displayed, even the faces that are not visible.
*/
-void
-vpMbKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo_, const vpCameraParameters & camera,
- const vpColor& col , const unsigned int thickness, const bool displayFullModel)
+void vpMbKltTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
vpCameraParameters c = camera;
-
- if(clippingFlag > 3) // Contains at least one FOV constraint
+
+ if (clippingFlag > 3) // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
- vpMbtDistanceKltPoints *kltpoly;
- vpMbtDistanceKltCylinder *kltPolyCylinder;
+ // vpMbtDistanceKltPoints *kltpoly;
+ // vpMbtDistanceKltCylinder *kltPolyCylinder;
// Previous version 12/08/2015
-// for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
-// kltpoly = *it;
-// kltpoly->polygon->changeFrame(cMo_);
-// kltpoly->polygon->computePolygonClipped(c);
-// }
- faces.computeClippedPolygons(cMo_,c);
-
- if(useScanLine && !displayFullModel)
- faces.computeScanLineRender(cam,I.getWidth(), I.getHeight());
+ // for(std::list<vpMbtDistanceKltPoints*>::const_iterator
+ // it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
+ // kltpoly = *it;
+ // kltpoly->polygon->changeFrame(cMo_);
+ // kltpoly->polygon->computePolygonClipped(c);
+ // }
+ faces.computeClippedPolygons(cMo_, c);
+
+ if (useScanLine && !displayFullModel)
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
- kltpoly->display(I,cMo_,camera,col,thickness,displayFullModel);
+ 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);
+ // faces[i]->displayNormal(I);
}
}
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
+ for (std::list<vpMbtDistanceKltCylinder *>::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<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
(*it)->display(I, cMo_, camera, col, thickness);
}
#ifdef VISP_HAVE_OGRE
- if(useOgre)
+ if (useOgre)
faces.displayOgre(cMo_);
#endif
}
@@ -1275,76 +1165,74 @@ vpMbKltTracker::display(const vpImage<vpRGBa>& I, const vpHomogeneousMatrix &cMo
\throw vpTrackingException::fatalError if the test fails.
*/
-void
-vpMbKltTracker::testTracking()
+void vpMbKltTracker::testTracking()
{
unsigned int nbTotalPoints = 0;
- vpMbtDistanceKltPoints *kltpoly;
-// for (unsigned int i = 0; i < faces.size(); i += 1){
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 && kltpoly->hasEnoughPoints()){
+ // for (unsigned int i = 0; i < faces.size(); i += 1){
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly->polygon->isVisible() && kltpoly->isTracked() && kltpoly->polygon->getNbPoint() > 2 &&
+ kltpoly->hasEnoughPoints()) {
nbTotalPoints += kltpoly->getCurrentNumberPoints();
}
}
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if(kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
nbTotalPoints += kltPolyCylinder->getCurrentNumberPoints();
}
- if(nbTotalPoints < 10){
+ 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).");
+ "test tracking failed (too few points to realize a good tracking).");
}
}
/*!
- Add a cylinder to display (not for tracking) from two points on the axis (defining the length of
- the cylinder) and its radius.
+ Add a cylinder to display (not for tracking) from two points on the axis
+ (defining the length of the cylinder) and its radius.
\param p1 : First point on the axis.
\param p2 : Second point on the axis.
\param radius : Radius of the cylinder.
- \param idFace : Identifier of the polygon representing the revolution axis of the cylinder.
- \param name : The optional name of the cylinder.
+ \param idFace : Identifier of the polygon representing the revolution axis
+ of the cylinder. \param name : The optional name of the cylinder.
*/
-void
-vpMbKltTracker::initCylinder(const vpPoint& p1, const vpPoint &p2, const double radius, const int idFace,
- const std::string &/*name*/)
+void vpMbKltTracker::initCylinder(const vpPoint &p1, const vpPoint &p2, const double radius, const int idFace,
+ const std::string & /*name*/)
{
vpMbtDistanceKltCylinder *kltPoly = new vpMbtDistanceKltCylinder();
- kltPoly->setCameraParameters(cam) ;
+ kltPoly->setCameraParameters(cam);
- kltPoly->buildFrom(p1,p2,radius);
+ kltPoly->buildFrom(p1, p2, radius);
// Add the Cylinder BBox to the list of polygons
- kltPoly->listIndicesCylinderBBox.push_back(idFace+1);
- kltPoly->listIndicesCylinderBBox.push_back(idFace+2);
- kltPoly->listIndicesCylinderBBox.push_back(idFace+3);
- kltPoly->listIndicesCylinderBBox.push_back(idFace+4);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace + 1);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace + 2);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace + 3);
+ kltPoly->listIndicesCylinderBBox.push_back(idFace + 4);
- kltPoly->hiddenface = &faces ;
+ kltPoly->hiddenface = &faces;
kltPoly->useScanLine = useScanLine;
kltCylinders.push_back(kltPoly);
}
/*!
- Add a circle to display (not for tracking) from its center, 3 points (including the center) defining the plane that contain
- the circle and its radius.
+ Add a circle to display (not for tracking) from its center, 3 points
+ (including the center) defining the plane that contain the circle and its
+ radius.
\param p1 : Center of the circle.
- \param p2,p3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param radius : Radius of the circle.
- \param name : The optional name of the circle.
+ \param p2,p3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param radius : Radius of the circle. \param name : The optional
+ name of the circle.
*/
-void
-vpMbKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &p3, const double radius,
- const int /*idFace*/, const std::string &name)
+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);
}
@@ -1353,27 +1241,30 @@ vpMbKltTracker::initCircle(const vpPoint& p1, const vpPoint &p2, const vpPoint &
Add a circle to the list of circles.
\param P1 : Center of the circle.
- \param P2,P3 : Two points on the plane containing the circle. With the center of the circle we have 3 points
- defining the plane that contains the circle.
- \param r : Radius of the circle.
- \param name : Name of the circle.
+ \param P2,P3 : Two points on the plane containing the circle. With the
+ center of the circle we have 3 points defining the plane that contains the
+ circle. \param r : Radius of the circle. \param name : Name of the circle.
*/
-void
-vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r, const std::string &name)
+void vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P3, const double r,
+ const std::string &name)
{
- bool already_here = false ;
- vpMbtDistanceCircle *ci ;
-
-// for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
-// ci = *it;
-// if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) && samePoint(*(ci->p3),P3)) ||
-// (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) && samePoint(*(ci->p3),P2)) ){
-// already_here = (std::fabs(ci->radius - r) < std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius, r));
-// }
-// }
-
- if (!already_here){
- ci = new vpMbtDistanceCircle ;
+ bool already_here = false;
+
+ // for(std::list<vpMbtDistanceCircle*>::const_iterator
+ // it=circles_disp.begin(); it!=circles_disp[i].end(); ++it){
+ // ci = *it;
+ // if((samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P2) &&
+ // samePoint(*(ci->p3),P3)) ||
+ // (samePoint(*(ci->p1),P1) && samePoint(*(ci->p2),P3) &&
+ // samePoint(*(ci->p3),P2)) ){
+ // already_here = (std::fabs(ci->radius - r) <
+ // std::numeric_limits<double>::epsilon() * vpMath::maximum(ci->radius,
+ // r));
+ // }
+ // }
+
+ if (!already_here) {
+ vpMbtDistanceCircle *ci = new vpMbtDistanceCircle;
ci->setCameraParameters(cam);
ci->setName(name);
@@ -1387,13 +1278,12 @@ vpMbKltTracker::addCircle(const vpPoint &P1, const vpPoint &P2, const vpPoint &P
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
{
reInitModel(I, cad_name.c_str(), cMo_, verbose);
}
@@ -1403,18 +1293,17 @@ vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const std::string &
\param I : The image containing the object to initialize.
\param cad_name : Path to the file containing the 3D model description.
- \param cMo_ : The new vpHomogeneousMatrix between the camera and the new model
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
*/
-void
-vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_name,
- const vpHomogeneousMatrix& cMo_, const bool verbose)
+void vpMbKltTracker::reInitModel(const vpImage<unsigned char> &I, const char *cad_name, const vpHomogeneousMatrix &cMo_,
+ const bool verbose)
{
this->cMo.eye();
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(cur != NULL){
+ if (cur != NULL) {
cvReleaseImage(&cur);
cur = NULL;
}
@@ -1422,39 +1311,35 @@ vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_nam
firstInitialisation = true;
-
// delete the Klt Polygon features
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if (kltpoly!=NULL){
- delete kltpoly ;
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly != NULL) {
+ delete kltpoly;
}
- kltpoly = NULL ;
+ kltpoly = NULL;
}
kltPolygons.clear();
- vpMbtDistanceKltCylinder *kltPolyCylinder;
- for(std::list<vpMbtDistanceKltCylinder*>::const_iterator it=kltCylinders.begin(); it!=kltCylinders.end(); ++it){
- kltPolyCylinder = *it;
- if (kltPolyCylinder!=NULL){
- delete kltPolyCylinder ;
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder != NULL) {
+ delete kltPolyCylinder;
}
- kltPolyCylinder = NULL ;
+ kltPolyCylinder = NULL;
}
kltCylinders.clear();
// delete the structures used to display circles
- vpMbtDistanceCircle *ci;
- for(std::list<vpMbtDistanceCircle*>::const_iterator it=circles_disp.begin(); it!=circles_disp.end(); ++it){
- ci = *it;
- if (ci!=NULL){
- delete ci ;
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
+ vpMbtDistanceCircle *ci = *it;
+ if (ci != NULL) {
+ delete ci;
}
- ci = NULL ;
+ ci = NULL;
}
-
faces.reset();
loadModel(cad_name, verbose);
@@ -1462,24 +1347,24 @@ vpMbKltTracker::reInitModel(const vpImage<unsigned char>& I, const char* cad_nam
}
/*!
- Set if the polygons that have the given name have to be considered during the tracking phase.
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
\param name : name of the polygon(s).
\param useKltTracking : True if it has to be considered, False otherwise.
*/
-void
-vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
+void vpMbKltTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
{
- vpMbtDistanceKltPoints *kltpoly;
- for(std::list<vpMbtDistanceKltPoints*>::const_iterator it=kltPolygons.begin(); it!=kltPolygons.end(); ++it){
- kltpoly = *it;
- if(kltpoly->polygon->getName() == name){
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly->polygon->getName() == name) {
kltpoly->setTracked(useKltTracking);
}
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no symbols
-void dummy_vpMbKltTracker() {};
-#endif //VISP_HAVE_OPENCV
+// Work arround to avoid warning: libvisp_mbt.a(vpMbKltTracker.cpp.o) has no
+// symbols
+void dummy_vpMbKltTracker(){};
+#endif // VISP_HAVE_OPENCV
diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp
index af6c97e..4db4d37 100644
--- a/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp
+++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltCylinder.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,18 @@
*
*****************************************************************************/
+#include <visp3/core/vpPolygon.h>
#include <visp3/mbt/vpMbtDistanceKltCylinder.h>
#include <visp3/mbt/vpMbtDistanceKltPoints.h>
-#include <visp3/core/vpPolygon.h>
-
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+#if defined(VISP_HAVE_CLIPPER)
+#include <clipper.hpp> // clipper private library
+#endif
+
#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
-# include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
#endif
/*!
@@ -51,10 +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)
{
}
@@ -62,13 +65,9 @@ vpMbtDistanceKltCylinder::vpMbtDistanceKltCylinder()
Basic destructor.
*/
-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;
@@ -85,26 +84,26 @@ vpMbtDistanceKltCylinder::buildFrom(const vpPoint &p1, const vpPoint &p2, const
V2[2] = p2.get_oZ();
// Get the axis of the cylinder
- ABC = V1-V2;
+ 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);
}
/*!
- Initialise the cylinder to track. All the points in the map, representing all the
- map detected in the image, are parsed in order to extract the id of the points
- that are indeed in the face.
+ Initialise the cylinder to track. All the points in the map, representing
+ all the map detected in the image, are parsed in order to extract the id of
+ the points that are indeed in the face.
\param _tracker : ViSP OpenCV KLT Tracker.
\param cMo : Pose of the object in the camera frame at initialization.
*/
-void
-vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousMatrix &cMo)
+void vpMbtDistanceKltCylinder::init(const vpKltOpencv &_tracker, const vpHomogeneousMatrix &cMo)
{
c0Mo = cMo;
cylinder.changeFrame(cMo);
@@ -117,34 +116,28 @@ vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousM
curPoints = std::map<int, vpImagePoint>();
curPointsInd = std::map<int, int>();
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
long id;
float x_tmp, y_tmp;
_tracker.getFeature((int)i, id, x_tmp, y_tmp);
bool add = false;
- if(useScanLine)
- {
- if((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
- (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth())
- {
- for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
- if(hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == listIndicesCylinderBBox[kc])
- {
+ if (useScanLine) {
+ if ((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
+ (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth()) {
+ for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++)
+ if (hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] ==
+ listIndicesCylinderBBox[kc]) {
add = true;
break;
}
}
- }
- else
- {
+ } else {
std::vector<vpImagePoint> roi;
- for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
- {
- hiddenface->getPolygon()[(size_t) listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
- if(vpPolygon::isInside(roi, y_tmp, x_tmp))
- {
+ for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
+ hiddenface->getPolygon()[(size_t)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
+ if (vpPolygon::isInside(roi, y_tmp, x_tmp)) {
add = true;
break;
}
@@ -152,12 +145,12 @@ vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousM
}
}
- if(add){
+ if (add) {
- double xm=0, ym=0;
+ double xm = 0, ym = 0;
vpPixelMeterConversion::convertPoint(cam, x_tmp, y_tmp, xm, ym);
- double Z = computeZ(xm,ym);
- if(!vpMath::isNaN(Z)){
+ double Z = computeZ(xm, ym);
+ if (!vpMath::isNaN(Z)) {
#if TARGET_OS_IPHONE
initPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
curPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
@@ -177,26 +170,30 @@ vpMbtDistanceKltCylinder::init(const vpKltOpencv& _tracker, const vpHomogeneousM
#else
initPoints3D[id] = p;
#endif
- //std::cout << "Computed Z for : " << xm << "," << ym << " : " << computeZ(xm,ym) << std::endl;
+ // std::cout << "Computed Z for : " << xm << "," << ym << " : " <<
+ // computeZ(xm,ym) << std::endl;
}
}
}
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
- //std::cout << "Nb detected points in cylinder : " << nbPointsCur << std::endl;
+ // std::cout << "Nb detected points in cylinder : " << nbPointsCur <<
+ // std::endl;
}
/*!
- compute the number of point in this instanciation of the tracker that corresponds
- to the points of the cylinder
+ compute the number of point in this instanciation of the tracker that
+ corresponds to the points of the cylinder
\param _tracker : the KLT tracker
- \return the number of points that are tracked in this face and in this instanciation of the tracker
+ \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;
@@ -204,22 +201,24 @@ vpMbtDistanceKltCylinder::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
curPoints = std::map<int, vpImagePoint>();
curPointsInd = std::map<int, int>();
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
_tracker.getFeature((int)i, id, x, y);
- if(isTrackedFeature((int)id)){
+ if (isTrackedFeature((int)id)) {
#if TARGET_OS_IPHONE
- curPoints[(int)id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPoints[(int)id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
curPointsInd[(int)id] = (int)i;
#else
- curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPoints[id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
curPointsInd[id] = (int)i;
#endif
nbPointsCur++;
}
}
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
return nbPointsCur;
}
@@ -229,10 +228,10 @@ vpMbtDistanceKltCylinder::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
associated weight is below a given threshold (threshold_outlier).
\param _w : Vector containing the weight of all the tracked points.
- \param threshold_outlier : Threshold to specify wether or not a point has to be deleted.
+ \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<int, vpImagePoint> tmp;
std::map<int, int> tmp2;
@@ -241,29 +240,30 @@ vpMbtDistanceKltCylinder::removeOutliers(const vpColVector& _w, const double &th
nbPointsCur = 0;
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
-// if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
+ for (; iter != curPoints.end(); ++iter) {
+ if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
+ // if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
tmp2[iter->first] = curPointsInd[iter->first];
nbPointsCur++;
- }
- else{
+ } else {
nbSupp++;
initPoints.erase(iter->first);
}
- k+=2;
+ k += 2;
}
- if(nbSupp != 0){
+ if (nbSupp != 0) {
curPoints = std::map<int, vpImagePoint>();
curPointsInd = std::map<int, int>();
curPoints = tmp;
curPointsInd = tmp2;
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
}
}
@@ -278,88 +278,87 @@ vpMbtDistanceKltCylinder::removeOutliers(const vpColVector& _w, const double &th
\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;
cylinder.changeFrame(_cMc0 * c0Mo);
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
double x_cur(0), y_cur(0);
vpPixelMeterConversion::convertPoint(cam, j_cur, i_cur, x_cur, y_cur);
- vpPoint p0 = initPoints3D[id];
+ vpPoint p0 = initPoints3D[id];
p0.changeFrame(_cMc0);
p0.project();
- double x0_transform(p0.get_x()), y0_transform(p0.get_y()) ;
+ double x0_transform(p0.get_x()), y0_transform(p0.get_y());
double Z = computeZ(x_cur, y_cur);
- if(vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()){
-// std::cout << "Z is Nan : " << A << " , " << B << " , " << C << " | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
-// std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C << std::endl;
-
- _J[2*index_][0] = 0;
- _J[2*index_][1] = 0;
- _J[2*index_][2] = 0;
- _J[2*index_][3] = 0;
- _J[2*index_][4] = 0;
- _J[2*index_][5] = 0;
-
- _J[2*index_+1][0] = 0;
- _J[2*index_+1][1] = 0;
- _J[2*index_+1][2] = 0;
- _J[2*index_+1][3] = 0;
- _J[2*index_+1][4] = 0;
- _J[2*index_+1][5] = 0;
-
- _R[2*index_] = (x0_transform - x_cur);
- _R[2*index_+1] = (y0_transform - y_cur);
+ if (vpMath::isNaN(Z) || Z < std::numeric_limits<double>::epsilon()) {
+ // std::cout << "Z is Nan : " << A << " , " << B << " , " << C << "
+ // | " << Z << " | " << x_cur << " , " << y_cur << std::endl;
+ // std::cout << std::sqrt(B*B - A*C) << " , " << B*B - A*C <<
+ // std::endl;
+
+ _J[2 * index_][0] = 0;
+ _J[2 * index_][1] = 0;
+ _J[2 * index_][2] = 0;
+ _J[2 * index_][3] = 0;
+ _J[2 * index_][4] = 0;
+ _J[2 * index_][5] = 0;
+
+ _J[2 * index_ + 1][0] = 0;
+ _J[2 * index_ + 1][1] = 0;
+ _J[2 * index_ + 1][2] = 0;
+ _J[2 * index_ + 1][3] = 0;
+ _J[2 * index_ + 1][4] = 0;
+ _J[2 * index_ + 1][5] = 0;
+
+ _R[2 * index_] = (x0_transform - x_cur);
+ _R[2 * index_ + 1] = (y0_transform - y_cur);
index_++;
- }
- else
- {
- double invZ = 1.0/Z;
-
- _J[2*index_][0] = - invZ;
- _J[2*index_][1] = 0;
- _J[2*index_][2] = x_cur * invZ;
- _J[2*index_][3] = x_cur * y_cur;
- _J[2*index_][4] = -(1+x_cur*x_cur);
- _J[2*index_][5] = y_cur;
-
- _J[2*index_+1][0] = 0;
- _J[2*index_+1][1] = - invZ;
- _J[2*index_+1][2] = y_cur * invZ;
- _J[2*index_+1][3] = (1+y_cur*y_cur);
- _J[2*index_+1][4] = - y_cur * x_cur;
- _J[2*index_+1][5] = - x_cur;
-
- _R[2*index_] = (x0_transform - x_cur);
- _R[2*index_+1] = (y0_transform - y_cur);
+ } else {
+ double invZ = 1.0 / Z;
+
+ _J[2 * index_][0] = -invZ;
+ _J[2 * index_][1] = 0;
+ _J[2 * index_][2] = x_cur * invZ;
+ _J[2 * index_][3] = x_cur * y_cur;
+ _J[2 * index_][4] = -(1 + x_cur * x_cur);
+ _J[2 * index_][5] = y_cur;
+
+ _J[2 * index_ + 1][0] = 0;
+ _J[2 * index_ + 1][1] = -invZ;
+ _J[2 * index_ + 1][2] = y_cur * invZ;
+ _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
+ _J[2 * index_ + 1][4] = -y_cur * x_cur;
+ _J[2 * index_ + 1][5] = -x_cur;
+
+ _R[2 * index_] = (x0_transform - x_cur);
+ _R[2 * index_ + 1] = (y0_transform - y_cur);
index_++;
}
}
}
/*!
- Test whether the feature with identifier id in paramters is in the list of tracked
- features.
+ Test whether the feature with identifier id in paramters is in the list of
+ tracked features.
\param _id : the id of the current feature to test
\return true if the id is in the list of tracked feature
*/
-bool
-vpMbtDistanceKltCylinder::isTrackedFeature(const int _id)
+bool vpMbtDistanceKltCylinder::isTrackedFeature(const int _id)
{
std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
- if(iter != initPoints.end())
+ if (iter != initPoints.end())
return true;
return false;
@@ -371,101 +370,156 @@ vpMbtDistanceKltCylinder::isTrackedFeature(const int _id)
\param mask : the mask to update (0, not in the object, _nb otherwise).
\param nb : Optionnal value to set to the pixels included in the face.
- \param shiftBorder : Optionnal shift for the border in pixel (sort of built-in erosion) to avoid to consider pixels near the limits of the face.
+ \param shiftBorder : Optionnal shift for the border in pixel (sort of
+ built-in erosion) to avoid to consider pixels near the limits of the face.
*/
-void
-vpMbtDistanceKltCylinder::updateMask(
+void vpMbtDistanceKltCylinder::updateMask(
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat &mask,
#else
- IplImage* mask,
+ IplImage *mask,
#endif
unsigned char nb, unsigned int shiftBorder)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- int width = mask.cols;
+ int width = mask.cols;
int height = mask.rows;
#else
- int width = mask->width;
+ int width = mask->width;
int height = mask->height;
#endif
- for(unsigned int kc = 0 ; kc < listIndicesCylinderBBox.size() ; kc++)
- {
- 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<vpImagePoint> roi;
- (*hiddenface)[(unsigned int) listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
- vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);
-
- /* check image boundaries */
- if(i_min > height){ //underflow
- i_min = 0;
- }
- if(i_max > height){
- i_max = height;
- }
- if(j_min > width){ //underflow
- j_min = 0;
- }
- if(j_max > width){
- j_max = width;
+ for (unsigned int kc = 0; kc < listIndicesCylinderBBox.size(); kc++) {
+ 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<vpImagePoint> roi;
+ (*hiddenface)[(unsigned int)listIndicesCylinderBBox[kc]]->getRoiClipped(cam, roi);
+
+ double shiftBorder_d = (double)shiftBorder;
+#if defined(VISP_HAVE_CLIPPER)
+ std::vector<vpImagePoint> roi_offset;
+
+ ClipperLib::Path path;
+ for (std::vector<vpImagePoint>::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;
+ ClipperLib::ClipperOffset co;
+ co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
+ co.Execute(solution, -shiftBorder_d);
+
+ // Keep biggest polygon by area
+ if (!solution.empty()) {
+ size_t index_max = 0;
+
+ if (solution.size() > 1) {
+ double max_area = 0;
+ vpPolygon polygon_area;
+
+ for (size_t i = 0; i < solution.size(); i++) {
+ std::vector<vpImagePoint> 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)));
+ }
+
+ polygon_area.buildFrom(corners);
+ if (polygon_area.getArea() > max_area) {
+ max_area = polygon_area.getArea();
+ index_max = i;
+ }
}
+ }
+
+ 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)));
+ }
+ } else {
+ roi_offset = roi;
+ }
- double shiftBorder_d = (double) shiftBorder;
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpPolygon::isInside(roi, i_d, j_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
- mask.at<unsigned char>(i,j) = nb;
- }
- }
- else{
- if(vpPolygon::isInside(roi, i, j)){
- mask.at<unsigned char>(i,j) = nb;
- }
- }
+ vpPolygon polygon_test(roi_offset);
+ vpImagePoint imPt;
+#endif
+
+#if defined(VISP_HAVE_CLIPPER)
+ vpPolygon3D::getMinMaxRoi(roi_offset, i_min, i_max, j_min, j_max);
+#else
+ vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min, j_max);
+#endif
+
+ /* check image boundaries */
+ if (i_min > height) { // underflow
+ i_min = 0;
+ }
+ if (i_max > height) {
+ i_max = height;
+ }
+ if (j_min > width) { // underflow
+ j_min = 0;
+ }
+ if (j_max > width) {
+ j_max = width;
+ }
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ for (int i = i_min; i < i_max; i++) {
+ double i_d = (double)i;
+
+ for (int j = j_min; j < j_max; j++) {
+ double j_d = (double)j;
+
+#if defined(VISP_HAVE_CLIPPER)
+ imPt.set_ij(i_d, j_d);
+ if (polygon_test.isInside(imPt)) {
+ mask.ptr<uchar>(i)[j] = nb;
+ }
+#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)) {
+ mask.at<unsigned char>(i, j) = nb;
+ }
+ } else {
+ if (vpPolygon::isInside(roi, i, j)) {
+ mask.at<unsigned char>(i, j) = nb;
}
}
- #else
- unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpPolygon::isInside(roi, i_d, j_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
- *(ptrData++) = nb;
- }
- else{
- ptrData++;
- }
- }
- else{
- if(vpPolygon::isInside(roi, i, j)){
- *(ptrData++) = nb;
- }
- else{
- ptrData++;
- }
- }
+#endif
+ }
+ }
+#else
+ unsigned char *ptrData = (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min;
+ for (int i = i_min; i < i_max; i++) {
+ double i_d = (double)i;
+ for (int j = j_min; j < j_max; j++) {
+ double j_d = (double)j;
+ if (shiftBorder != 0) {
+ if (vpPolygon::isInside(roi, i_d, j_d) &&
+ vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
+ *(ptrData++) = nb;
+ } else {
+ ptrData++;
+ }
+ } else {
+ if (vpPolygon::isInside(roi, i, j)) {
+ *(ptrData++) = nb;
+ } else {
+ ptrData++;
}
- ptrData += mask->widthStep - j_max + j_min;
}
- #endif
+ }
+ ptrData += mask->widthStep - j_max + j_min;
+ }
+#endif
}
}
}
@@ -475,11 +529,10 @@ vpMbtDistanceKltCylinder::updateMask(
\param _I : The image where to display.
*/
-void
-vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<unsigned char>& _I)
+void vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<unsigned char> &_I)
{
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
vpImagePoint iP;
iP.set_i(static_cast<double>(iter->second.get_i()));
@@ -487,11 +540,11 @@ vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<unsigned char>& _I)
vpDisplay::displayCross(_I, iP, 10, vpColor::red);
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ iP.set_i(vpMath::round(iP.get_i() + 7));
+ iP.set_j(vpMath::round(iP.get_j() + 7));
+ std::stringstream ss;
+ ss << id;
+ vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
}
}
@@ -500,11 +553,10 @@ vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<unsigned char>& _I)
\param _I : The image where to display.
*/
-void
-vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<vpRGBa>& _I)
+void vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<vpRGBa> &_I)
{
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
vpImagePoint iP;
iP.set_i(static_cast<double>(iter->second.get_i()));
@@ -512,45 +564,47 @@ vpMbtDistanceKltCylinder::displayPrimitive(const vpImage<vpRGBa>& _I)
vpDisplay::displayCross(_I, iP, 10, vpColor::red);
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ iP.set_i(vpMath::round(iP.get_i() + 7));
+ iP.set_j(vpMath::round(iP.get_j() + 7));
+ std::stringstream ss;
+ ss << id;
+ vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
}
}
-void
-vpMbtDistanceKltCylinder::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera,
- const vpColor col, const unsigned int thickness, const bool /*displayFullModel*/)
+void vpMbtDistanceKltCylinder::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col,
+ const unsigned int thickness, const bool /*displayFullModel*/)
{
- //if(isvisible || displayFullModel)
+ // if(isvisible || displayFullModel)
{
// Perspective projection
circle1.changeFrame(cMo);
circle2.changeFrame(cMo);
cylinder.changeFrame(cMo);
- try{
+ try {
circle1.projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 1";
}
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
+ try {
circle2.projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 2";
}
- catch(...){std::cout<<"Problem projection circle 2";}
cylinder.projection();
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,cylinder.getRho1(),cylinder.getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,cylinder.getRho2(),cylinder.getTheta2(),rho2,theta2);
+ 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;
+ double i11, i12, i21, i22, j11, j12, j21, j22;
vpCircle::computeIntersectionPoint(circle1, cam, rho1, theta1, i11, j11);
vpCircle::computeIntersectionPoint(circle2, cam, rho1, theta1, i12, j12);
@@ -559,49 +613,51 @@ vpMbtDistanceKltCylinder::display(const vpImage<unsigned char> &I, const vpHomog
vpCircle::computeIntersectionPoint(circle2, cam, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ vpDisplay::displayLine(I, ip11, ip12, col, thickness);
+ vpDisplay::displayLine(I, ip21, ip22, col, thickness);
}
}
-void
-vpMbtDistanceKltCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &camera,
- const vpColor col, const unsigned int thickness, const bool /*displayFullModel*/)
+void vpMbtDistanceKltCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &camera, const vpColor &col,
+ const unsigned int thickness, const bool /*displayFullModel*/)
{
- //if(isvisible || displayFullModel)
+ // if(isvisible || displayFullModel)
{
// Perspective projection
circle1.changeFrame(cMo);
circle2.changeFrame(cMo);
cylinder.changeFrame(cMo);
- try{
+ try {
circle1.projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 1";
}
- catch(...){std::cout<<"Problem projection circle 1";}
- try{
+ try {
circle2.projection();
+ } catch (...) {
+ std::cout << "Problem projection circle 2";
}
- catch(...){std::cout<<"Problem projection circle 2";}
cylinder.projection();
- double rho1,theta1;
- double rho2,theta2;
+ double rho1, theta1;
+ double rho2, theta2;
// Meters to pixels conversion
- vpMeterPixelConversion::convertLine(camera,cylinder.getRho1(),cylinder.getTheta1(),rho1,theta1);
- vpMeterPixelConversion::convertLine(camera,cylinder.getRho2(),cylinder.getTheta2(),rho2,theta2);
+ 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;
+ 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);
@@ -610,35 +666,37 @@ vpMbtDistanceKltCylinder::display(const vpImage<vpRGBa> &I, const vpHomogeneousM
vpCircle::computeIntersectionPoint(circle2, camera, rho2, theta2, i22, j22);
// Create the image points
- vpImagePoint ip11,ip12,ip21,ip22;
- ip11.set_ij(i11,j11);
- ip12.set_ij(i12,j12);
- ip21.set_ij(i21,j21);
- ip22.set_ij(i22,j22);
+ vpImagePoint ip11, ip12, ip21, ip22;
+ ip11.set_ij(i11, j11);
+ ip12.set_ij(i12, j12);
+ ip21.set_ij(i21, j21);
+ ip22.set_ij(i22, j22);
// Display
- vpDisplay::displayLine(I,ip11,ip12,col, thickness);
- vpDisplay::displayLine(I,ip21,ip22,col, thickness);
+ vpDisplay::displayLine(I, ip11, ip12, col, thickness);
+ vpDisplay::displayLine(I, ip21, ip22, col, thickness);
}
}
-
// ######################
// Private Functions
// ######################
-double
-vpMbtDistanceKltCylinder::computeZ(const double &x, const double &y)
+double vpMbtDistanceKltCylinder::computeZ(const double &x, const double &y)
{
-// double A = x*x + y*y + 1 - ((cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()) * (cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()));
-// double B = (x * cylinder.getX() + y * cylinder.getY() + cylinder.getZ());
-// double C = cylinder.getX() * cylinder.getX() + cylinder.getY() * cylinder.getY() + cylinder.getZ() * cylinder.getZ() - cylinder.getR() * cylinder.getR();
-//
-// return (B - std::sqrt(B*B - A*C))/A;
+ // double A = x*x + y*y + 1 -
+ // ((cylinder.getA()*x+cylinder.getB()*y+cylinder.getC()) *
+ // (cylinder.getA()*x+cylinder.getB()*y+cylinder.getC())); double B = (x *
+ // cylinder.getX() + y * cylinder.getY() + cylinder.getZ()); double C =
+ // cylinder.getX() * cylinder.getX() + cylinder.getY() * cylinder.getY() +
+ // cylinder.getZ() * cylinder.getZ() - cylinder.getR() * cylinder.getR();
+ //
+ // return (B - std::sqrt(B*B - A*C))/A;
return cylinder.computeZ(x, y);
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbtDistanceKltCylinder.cpp.o) has no symbols
-void dummy_vpMbtDistanceKltCylinder() {};
+// Work arround to avoid warning:
+// libvisp_mbt.a(vpMbtDistanceKltCylinder.cpp.o) has no symbols
+void dummy_vpMbtDistanceKltCylinder(){};
#endif
diff --git a/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp
index 6df1558..8a2e57e 100644
--- a/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp
+++ b/modules/tracker/mbt/src/klt/vpMbtDistanceKltPoints.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,13 +37,17 @@
*
*****************************************************************************/
-#include <visp3/mbt/vpMbtDistanceKltPoints.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/mbt/vpMbtDistanceKltPoints.h>
#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+#if defined(VISP_HAVE_CLIPPER)
+#include <clipper.hpp> // clipper private library
+#endif
+
#if defined(__APPLE__) && defined(__MACH__) // Apple OSX and iOS (Darwin)
-# include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
+#include <TargetConditionals.h> // To detect OSX or IOS using TARGET_OS_IPHONE or TARGET_OS_IOS macro
#endif
/*!
@@ -50,31 +55,27 @@
*/
vpMbtDistanceKltPoints::vpMbtDistanceKltPoints()
- : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(), curPoints(), curPointsInd(),
- nbPointsCur(0), nbPointsInit(0), minNbPoint(4), enoughPoints(false), dt(1.), d0(1.),
- cam(), isTrackedKltPoints(true), polygon(NULL), hiddenface(NULL), useScanLine(false)
+ : H(), N(), N_cur(), invd0(1.), cRc0_0n(), initPoints(std::map<int, vpImagePoint>()),
+ curPoints(std::map<int, vpImagePoint>()), curPointsInd(std::map<int, int>()), nbPointsCur(0), nbPointsInit(0),
+ minNbPoint(4), enoughPoints(false), dt(1.), d0(1.), cam(), isTrackedKltPoints(true), polygon(NULL),
+ hiddenface(NULL), useScanLine(false)
{
- initPoints = std::map<int, vpImagePoint>();
- curPoints = std::map<int, vpImagePoint>();
- curPointsInd = std::map<int, int>();
}
/*!
Basic destructor.
*/
-vpMbtDistanceKltPoints::~vpMbtDistanceKltPoints()
-{}
+vpMbtDistanceKltPoints::~vpMbtDistanceKltPoints() {}
/*!
- Initialise the face to track. All the points in the map, representing all the
- map detected in the image, are parsed in order to extract the id of the points
- that are indeed in the face.
+ Initialise the face to track. All the points in the map, representing all
+ the map detected in the image, are parsed in order to extract the id of the
+ points that are indeed in the face.
\param _tracker : ViSP OpenCV KLT Tracker.
*/
-void
-vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
+void vpMbtDistanceKltPoints::init(const vpKltOpencv &_tracker)
{
// extract ids of the points in the face
nbPointsInit = 0;
@@ -85,26 +86,24 @@ vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
std::vector<vpImagePoint> roi;
polygon->getRoiClipped(cam, roi);
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i ++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
long id;
float x_tmp, y_tmp;
_tracker.getFeature((int)i, id, x_tmp, y_tmp);
bool add = false;
- if(useScanLine)
- {
- if((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
- (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
- hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] == polygon->getIndex())
+ if (useScanLine) {
+ if ((unsigned int)y_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getHeight() &&
+ (unsigned int)x_tmp < hiddenface->getMbScanLineRenderer().getPrimitiveIDs().getWidth() &&
+ hiddenface->getMbScanLineRenderer().getPrimitiveIDs()[(unsigned int)y_tmp][(unsigned int)x_tmp] ==
+ polygon->getIndex())
add = true;
- }
- else if(vpPolygon::isInside(roi, y_tmp, x_tmp))
- {
+ } else if (vpPolygon::isInside(roi, y_tmp, x_tmp)) {
add = true;
}
- if(add){
+ if (add) {
#if TARGET_OS_IPHONE
initPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
curPoints[(int)id] = vpImagePoint(y_tmp, x_tmp);
@@ -114,13 +113,16 @@ vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
curPoints[id] = vpImagePoint(y_tmp, x_tmp);
curPointsInd[id] = (int)i;
#endif
- nbPointsInit++;
- nbPointsCur++;
}
}
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ nbPointsInit = (unsigned int)initPoints.size();
+ nbPointsCur = (unsigned int)curPoints.size();
+
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
// initialisation of the value for the computation in SE3
vpPlane plan(polygon->getPoint(0), polygon->getPoint(1), polygon->getPoint(2));
@@ -134,14 +136,14 @@ vpMbtDistanceKltPoints::init(const vpKltOpencv& _tracker)
}
/*!
- compute the number of point in this instanciation of the tracker that corresponds
- to the points of the face
+ compute the number of point in this instanciation of the tracker that
+ corresponds to the points of the face
\param _tracker : the KLT tracker
- \return the number of points that are tracked in this face and in this instanciation of the tracker
+ \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;
@@ -149,22 +151,25 @@ vpMbtDistanceKltPoints::computeNbDetectedCurrent(const vpKltOpencv& _tracker)
curPoints = std::map<int, vpImagePoint>();
curPointsInd = std::map<int, int>();
- for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++){
+ for (unsigned int i = 0; i < static_cast<unsigned int>(_tracker.getNbFeatures()); i++) {
_tracker.getFeature((int)i, id, x, y);
- if(isTrackedFeature((int)id)){
+ if (isTrackedFeature((int)id)) {
#if TARGET_OS_IPHONE
- curPoints[(int)id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPoints[(int)id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
curPointsInd[(int)id] = (int)i;
#else
- curPoints[id] = vpImagePoint(static_cast<double>(y),static_cast<double>(x));
+ curPoints[id] = vpImagePoint(static_cast<double>(y), static_cast<double>(x));
curPointsInd[id] = (int)i;
#endif
- nbPointsCur++;
}
}
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ nbPointsCur = (unsigned int)curPoints.size();
+
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
return nbPointsCur;
}
@@ -179,13 +184,12 @@ 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;
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
double i_cur(iter->second.get_i()), j_cur(iter->second.get_j());
@@ -196,37 +200,37 @@ vpMbtDistanceKltPoints::computeInteractionMatrixAndResidu(vpColVector& _R, vpMat
double x0(0), y0(0);
vpPixelMeterConversion::convertPoint(cam, iP0, x0, y0);
- double x0_transform, y0_transform ;// equivalent x and y in the first image (reference)
- computeP_mu_t(x0, y0, x0_transform, y0_transform, H );
+ double x0_transform,
+ y0_transform; // equivalent x and y in the first image (reference)
+ computeP_mu_t(x0, y0, x0_transform, y0_transform, H);
double invZ = compute_1_over_Z(x_cur, y_cur);
- _J[2*index_][0] = - invZ;
- _J[2*index_][1] = 0;
- _J[2*index_][2] = x_cur * invZ;
- _J[2*index_][3] = x_cur * y_cur;
- _J[2*index_][4] = -(1+x_cur*x_cur);
- _J[2*index_][5] = y_cur;
-
- _J[2*index_+1][0] = 0;
- _J[2*index_+1][1] = - invZ;
- _J[2*index_+1][2] = y_cur * invZ;
- _J[2*index_+1][3] = (1+y_cur*y_cur);
- _J[2*index_+1][4] = - y_cur * x_cur;
- _J[2*index_+1][5] = - x_cur;
-
- _R[2*index_] = (x0_transform - x_cur);
- _R[2*index_+1] = (y0_transform - y_cur);
+ _J[2 * index_][0] = -invZ;
+ _J[2 * index_][1] = 0;
+ _J[2 * index_][2] = x_cur * invZ;
+ _J[2 * index_][3] = x_cur * y_cur;
+ _J[2 * index_][4] = -(1 + x_cur * x_cur);
+ _J[2 * index_][5] = y_cur;
+
+ _J[2 * index_ + 1][0] = 0;
+ _J[2 * index_ + 1][1] = -invZ;
+ _J[2 * index_ + 1][2] = y_cur * invZ;
+ _J[2 * index_ + 1][3] = (1 + y_cur * y_cur);
+ _J[2 * index_ + 1][4] = -y_cur * x_cur;
+ _J[2 * index_ + 1][5] = -x_cur;
+
+ _R[2 * index_] = (x0_transform - x_cur);
+ _R[2 * index_ + 1] = (y0_transform - y_cur);
index_++;
}
}
-double
-vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
+double vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
{
double num = cRc0_0n[0] * x + cRc0_0n[1] * y + cRc0_0n[2];
double den = -(d0 - dt);
- return num/den;
+ return num / den;
}
/*!
@@ -241,12 +245,12 @@ vpMbtDistanceKltPoints::compute_1_over_Z(const double x, const double y)
\param y_out : the y coordinates of the output point
\param _cHc0 : the homography used to transfer the point
*/
-inline void
-vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, double& x_out, double& y_out, const vpMatrix& _cHc0)
+inline void vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, double &x_out, double &y_out,
+ const vpMatrix &_cHc0)
{
double p_mu_t_2 = x_in * _cHc0[2][0] + y_in * _cHc0[2][1] + _cHc0[2][2];
- if( fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()){
+ if (fabs(p_mu_t_2) < std::numeric_limits<double>::epsilon()) {
x_out = 0.0;
y_out = 0.0;
throw vpException(vpException::divideByZeroError, "the depth of the point is calculated to zero");
@@ -265,11 +269,11 @@ vpMbtDistanceKltPoints::computeP_mu_t(const double x_in, const double y_in, doub
Several internal variables are computed (dt, cRc0_0n)
- \param _cTc0 : the displacement matrix of the camera between the initial position of the camera and the current camera position
- \param _cHc0 : the homography of the plane
+ \param _cTc0 : the displacement matrix of the camera between the initial
+ position of the camera and the current camera position \param _cHc0 : the
+ homography of the plane
*/
-void
-vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHomography& _cHc0)
+void vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix &_cTc0, vpHomography &_cHc0)
{
vpRotationMatrix cRc0;
vpTranslationVector ctransc0;
@@ -278,55 +282,54 @@ vpMbtDistanceKltPoints::computeHomography(const vpHomogeneousMatrix& _cTc0, vpHo
_cTc0.extract(ctransc0);
vpMatrix cHc0 = _cHc0.convert();
-// vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
+ // vpGEMM(cRc0, 1.0, invd0, cRc0, -1.0, _cHc0, VP_GEMM_A_T);
vpGEMM(ctransc0, N, -invd0, cRc0, 1.0, cHc0, VP_GEMM_B_T);
cHc0 /= cHc0[2][2];
H = cHc0;
-// vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
-// vpQuaternionVector RotQuat(cRc0);
-// vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(), -RotQuat.z(), RotQuat.w());
-// vpQuaternionVector partial = RotQuat * NQuat;
-// vpQuaternionVector resQuat = (partial * RotQuatConj);
-//
-// cRc0_0n = vpColVector(3);
-// cRc0_0n[0] = resQuat.x();
-// cRc0_0n[1] = resQuat.y();
-// cRc0_0n[2] = resQuat.z();
-
- cRc0_0n = cRc0*N;
-
-// vpPlane p(corners[0], corners[1], corners[2]);
-// vpColVector Ncur = p.getNormal();
-// Ncur.normalize();
+ // vpQuaternionVector NQuat(N[0], N[1], N[2], 0.0);
+ // vpQuaternionVector RotQuat(cRc0);
+ // vpQuaternionVector RotQuatConj(-RotQuat.x(), -RotQuat.y(),
+ // -RotQuat.z(), RotQuat.w()); vpQuaternionVector partial = RotQuat *
+ // NQuat; vpQuaternionVector resQuat = (partial * RotQuatConj);
+ //
+ // cRc0_0n = vpColVector(3);
+ // cRc0_0n[0] = resQuat.x();
+ // cRc0_0n[1] = resQuat.y();
+ // cRc0_0n[2] = resQuat.z();
+
+ cRc0_0n = cRc0 * N;
+
+ // vpPlane p(corners[0], corners[1], corners[2]);
+ // vpColVector Ncur = p.getNormal();
+ // Ncur.normalize();
N_cur = cRc0_0n;
dt = 0.0;
- for (unsigned int i = 0; i < 3; i += 1){
+ for (unsigned int i = 0; i < 3; i += 1) {
dt += ctransc0[i] * (N_cur[i]);
}
}
/*!
- Test whether the feature with identifier id in paramters is in the list of tracked
- features.
+ Test whether the feature with identifier id in paramters is in the list of
+ tracked features.
\param _id : the id of the current feature to test
\return true if the id is in the list of tracked feature
*/
-bool
-vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
+bool vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
{
-// std::map<int, vpImagePoint>::const_iterator iter = initPoints.begin();
-// while(iter != initPoints.end()){
-// if(iter->first == _id){
-// return true;
-// }
-// iter++;
-// }
+ // std::map<int, vpImagePoint>::const_iterator iter = initPoints.begin();
+ // while(iter != initPoints.end()){
+ // if(iter->first == _id){
+ // return true;
+ // }
+ // iter++;
+ // }
std::map<int, vpImagePoint>::iterator iter = initPoints.find(_id);
- if(iter != initPoints.end())
+ if (iter != initPoints.end())
return true;
return false;
@@ -338,89 +341,145 @@ vpMbtDistanceKltPoints::isTrackedFeature(const int _id)
\param mask : the mask to update (0, not in the object, _nb otherwise).
\param nb : Optionnal value to set to the pixels included in the face.
- \param shiftBorder : Optionnal shift for the border in pixel (sort of built-in erosion) to avoid to consider pixels near the limits of the face.
+ \param shiftBorder : Optionnal shift for the border in pixel (sort of
+ built-in erosion) to avoid to consider pixels near the limits of the face.
*/
-void
-vpMbtDistanceKltPoints::updateMask(
+void vpMbtDistanceKltPoints::updateMask(
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat &mask,
#else
- IplImage* mask,
+ IplImage *mask,
#endif
unsigned char nb, unsigned int shiftBorder)
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- int width = mask.cols;
+ int width = mask.cols;
int height = mask.rows;
#else
- int width = mask->width;
+ int width = mask->width;
int height = mask->height;
#endif
int i_min, i_max, j_min, j_max;
std::vector<vpImagePoint> roi;
polygon->getRoiClipped(cam, roi);
- vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min,j_max);
+
+ double shiftBorder_d = (double)shiftBorder;
+
+#if defined(VISP_HAVE_CLIPPER)
+ std::vector<vpImagePoint> roi_offset;
+
+ ClipperLib::Path path;
+ for (std::vector<vpImagePoint>::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;
+ ClipperLib::ClipperOffset co;
+ co.AddPath(path, ClipperLib::jtRound, ClipperLib::etClosedPolygon);
+ co.Execute(solution, -shiftBorder_d);
+
+ // Keep biggest polygon by area
+ if (!solution.empty()) {
+ size_t index_max = 0;
+
+ if (solution.size() > 1) {
+ double max_area = 0;
+ vpPolygon polygon_area;
+
+ for (size_t i = 0; i < solution.size(); i++) {
+ std::vector<vpImagePoint> 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)));
+ }
+
+ polygon_area.buildFrom(corners);
+ if (polygon_area.getArea() > max_area) {
+ max_area = polygon_area.getArea();
+ index_max = i;
+ }
+ }
+ }
+
+ 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)));
+ }
+ } else {
+ roi_offset = roi;
+ }
+
+ vpPolygon polygon_test(roi_offset);
+ vpImagePoint imPt;
+#endif
+
+#if defined(VISP_HAVE_CLIPPER)
+ vpPolygon3D::getMinMaxRoi(roi_offset, i_min, i_max, j_min, j_max);
+#else
+ vpPolygon3D::getMinMaxRoi(roi, i_min, i_max, j_min, j_max);
+#endif
/* check image boundaries */
- if(i_min > height){ //underflow
+ if (i_min > height) { // underflow
i_min = 0;
}
- if(i_max > height){
+ if (i_max > height) {
i_max = height;
}
- if(j_min > width){ //underflow
+ if (j_min > width) { // underflow
j_min = 0;
}
- if(j_max > width){
+ if (j_max > width) {
j_max = width;
}
- double shiftBorder_d = (double) shiftBorder;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpPolygon::isInside(roi, i_d, j_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
- mask.at<unsigned char>(i,j) = nb;
- }
+ 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 defined(VISP_HAVE_CLIPPER)
+ imPt.set_ij(i_d, j_d);
+ if (polygon_test.isInside(imPt)) {
+ mask.ptr<uchar>(i)[j] = nb;
}
- else{
- if(vpPolygon::isInside(roi, i, j)){
- mask.at<unsigned char>(i,j) = nb;
+#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)) {
+ mask.at<unsigned char>(i, j) = nb;
+ }
+ } else {
+ if (vpPolygon::isInside(roi, i, j)) {
+ mask.at<unsigned char>(i, j) = nb;
}
}
+#endif
}
}
#else
- unsigned char* ptrData = (unsigned char*)mask->imageData + i_min*mask->widthStep+j_min;
- for(int i=i_min; i< i_max; i++){
- double i_d = (double) i;
- for(int j=j_min; j< j_max; j++){
- double j_d = (double) j;
- if(shiftBorder != 0){
- if( vpPolygon::isInside(roi, i_d, j_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d+shiftBorder_d)
- && vpPolygon::isInside(roi, i_d+shiftBorder_d, j_d-shiftBorder_d)
- && vpPolygon::isInside(roi, i_d-shiftBorder_d, j_d-shiftBorder_d) ){
+ unsigned char *ptrData = (unsigned char *)mask->imageData + i_min * mask->widthStep + j_min;
+ for (int i = i_min; i < i_max; i++) {
+ double i_d = (double)i;
+ for (int j = j_min; j < j_max; j++) {
+ double j_d = (double)j;
+ if (shiftBorder != 0) {
+ if (vpPolygon::isInside(roi, i_d, j_d) && vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d + shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d + shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d + shiftBorder_d, j_d - shiftBorder_d) &&
+ vpPolygon::isInside(roi, i_d - shiftBorder_d, j_d - shiftBorder_d)) {
*(ptrData++) = nb;
- }
- else{
+ } else {
ptrData++;
}
- }
- else{
- if(vpPolygon::isInside(roi, i, j)){
+ } else {
+ if (vpPolygon::isInside(roi, i, j)) {
*(ptrData++) = nb;
- }
- else{
+ } else {
ptrData++;
}
}
@@ -435,10 +494,10 @@ vpMbtDistanceKltPoints::updateMask(
associated weight is below a given threshold (threshold_outlier).
\param _w : Vector containing the weight of all the tracked points.
- \param threshold_outlier : Threshold to specify wether or not a point has to be deleted.
+ \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<int, vpImagePoint> tmp;
std::map<int, int> tmp2;
@@ -447,26 +506,27 @@ vpMbtDistanceKltPoints::removeOutliers(const vpColVector& _w, const double &thre
nbPointsCur = 0;
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
- if(_w[k] > threshold_outlier && _w[k+1] > threshold_outlier){
-// if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
+ for (; iter != curPoints.end(); ++iter) {
+ if (_w[k] > threshold_outlier && _w[k + 1] > threshold_outlier) {
+ // if(_w[k] > threshold_outlier || _w[k+1] > threshold_outlier){
tmp[iter->first] = vpImagePoint(iter->second.get_i(), iter->second.get_j());
tmp2[iter->first] = curPointsInd[iter->first];
nbPointsCur++;
- }
- else{
+ } else {
nbSupp++;
initPoints.erase(iter->first);
}
- k+=2;
+ k += 2;
}
- if(nbSupp != 0){
+ if (nbSupp != 0) {
curPoints = tmp;
curPointsInd = tmp2;
- if(nbPointsCur >= minNbPoint) enoughPoints = true;
- else enoughPoints = false;
+ if (nbPointsCur >= minNbPoint)
+ enoughPoints = true;
+ else
+ enoughPoints = false;
}
}
@@ -475,11 +535,10 @@ vpMbtDistanceKltPoints::removeOutliers(const vpColVector& _w, const double &thre
\param _I : The image where to display.
*/
-void
-vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
+void vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char> &_I)
{
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
vpImagePoint iP;
iP.set_i(static_cast<double>(iter->second.get_i()));
@@ -487,11 +546,11 @@ vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
vpDisplay::displayCross(_I, iP, 10, vpColor::red);
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ iP.set_i(vpMath::round(iP.get_i() + 7));
+ iP.set_j(vpMath::round(iP.get_j() + 7));
+ std::stringstream ss;
+ ss << id;
+ vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
}
}
@@ -500,11 +559,10 @@ vpMbtDistanceKltPoints::displayPrimitive(const vpImage<unsigned char>& _I)
\param _I : The image where to display.
*/
-void
-vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa>& _I)
+void vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa> &_I)
{
std::map<int, vpImagePoint>::const_iterator iter = curPoints.begin();
- for( ; iter != curPoints.end(); iter++){
+ for (; iter != curPoints.end(); ++iter) {
int id(iter->first);
vpImagePoint iP;
iP.set_i(static_cast<double>(iter->second.get_i()));
@@ -512,85 +570,82 @@ vpMbtDistanceKltPoints::displayPrimitive(const vpImage<vpRGBa>& _I)
vpDisplay::displayCross(_I, iP, 10, vpColor::red);
- iP.set_i( vpMath::round( iP.get_i() + 7 ) );
- iP.set_j( vpMath::round( iP.get_j() + 7 ) );
- char ide[10];
- sprintf(ide, "%ld", static_cast<long int>(id));
- vpDisplay::displayText(_I, iP, ide, vpColor::red);
+ iP.set_i(vpMath::round(iP.get_i() + 7));
+ iP.set_j(vpMath::round(iP.get_j() + 7));
+ std::stringstream ss;
+ ss << id;
+ vpDisplay::displayText(_I, iP, ss.str(), vpColor::red);
}
}
-void
-vpMbtDistanceKltPoints::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &/*cMo*/, const vpCameraParameters &camera,
- const vpColor col, const unsigned int thickness, const bool displayFullModel)
+void vpMbtDistanceKltPoints::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix & /*cMo*/,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (polygon->isVisible() && isTrackedKltPoints) || displayFullModel)
- {
- std::vector<std::pair<vpPoint,unsigned int> > roi;
- polygon->getPolygonClipped(roi);
-
- for (unsigned int j = 0; j < roi.size(); j += 1){
- if(((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
-
- vpImagePoint ip1, ip2;
- std::vector<std::pair<vpPoint, vpPoint> > linesLst;
-
- if(useScanLine && !displayFullModel)
- hiddenface->computeScanLineQuery(roi[j].first,roi[(j+1)%roi.size()].first,linesLst, true);
- else
- linesLst.push_back(std::make_pair(roi[j].first,roi[(j+1)%roi.size()].first));
-
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
- linesLst[i].first.project();
- linesLst[i].second.project();
- vpMeterPixelConversion::convertPoint(camera,linesLst[i].first.get_x(),linesLst[i].first.get_y(),ip1);
- vpMeterPixelConversion::convertPoint(camera,linesLst[i].second.get_x(),linesLst[i].second.get_y(),ip2);
-
- vpDisplay::displayLine(I,ip1,ip2,col, thickness);
- }
+ if ((polygon->isVisible() && isTrackedKltPoints) || displayFullModel) {
+ std::vector<std::pair<vpPoint, unsigned int> > roi;
+ polygon->getPolygonClipped(roi);
+
+ for (unsigned int j = 0; j < roi.size(); j += 1) {
+ if (((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
+
+ vpImagePoint ip1, ip2;
+ std::vector<std::pair<vpPoint, vpPoint> > linesLst;
+
+ if (useScanLine && !displayFullModel)
+ hiddenface->computeScanLineQuery(roi[j].first, roi[(j + 1) % roi.size()].first, linesLst, true);
+ else
+ linesLst.push_back(std::make_pair(roi[j].first, roi[(j + 1) % roi.size()].first));
+
+ for (unsigned int i = 0; i < linesLst.size(); i++) {
+ linesLst[i].first.project();
+ linesLst[i].second.project();
+ vpMeterPixelConversion::convertPoint(camera, linesLst[i].first.get_x(), linesLst[i].first.get_y(), ip1);
+ vpMeterPixelConversion::convertPoint(camera, linesLst[i].second.get_x(), linesLst[i].second.get_y(), ip2);
+
+ vpDisplay::displayLine(I, ip1, ip2, col, thickness);
}
}
}
+ }
}
-
-void
-vpMbtDistanceKltPoints::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &/*cMo*/, const vpCameraParameters &camera,
- const vpColor col, const unsigned int thickness, const bool displayFullModel)
+void vpMbtDistanceKltPoints::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix & /*cMo*/,
+ const vpCameraParameters &camera, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
{
- if( (polygon->isVisible() && isTrackedKltPoints) || displayFullModel)
- {
- std::vector<std::pair<vpPoint,unsigned int> > roi;
+ if ((polygon->isVisible() && isTrackedKltPoints) || displayFullModel) {
+ std::vector<std::pair<vpPoint, unsigned int> > roi;
polygon->getPolygonClipped(roi);
- for (unsigned int j = 0; j < roi.size(); j += 1){
- if(((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
- ((roi[(j+1)%roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)){
+ for (unsigned int j = 0; j < roi.size(); j += 1) {
+ if (((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::NEAR_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::FAR_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::DOWN_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::UP_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::LEFT_CLIPPING) == 0) &&
+ ((roi[(j + 1) % roi.size()].second & roi[j].second & vpPolygon3D::RIGHT_CLIPPING) == 0)) {
vpImagePoint ip1, ip2;
std::vector<std::pair<vpPoint, vpPoint> > linesLst;
- if(useScanLine && !displayFullModel)
- hiddenface->computeScanLineQuery(roi[j].first,roi[(j+1)%roi.size()].first,linesLst, true);
+ if (useScanLine && !displayFullModel)
+ hiddenface->computeScanLineQuery(roi[j].first, roi[(j + 1) % roi.size()].first, linesLst, true);
else
- linesLst.push_back(std::make_pair(roi[j].first,roi[(j+1)%roi.size()].first));
+ linesLst.push_back(std::make_pair(roi[j].first, roi[(j + 1) % roi.size()].first));
- for(unsigned int i = 0 ; i < linesLst.size() ; i++){
+ 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);
+ vpDisplay::displayLine(I, ip1, ip2, col, thickness);
}
}
}
@@ -598,6 +653,7 @@ vpMbtDistanceKltPoints::display(const vpImage<vpRGBa> &I, const vpHomogeneousMat
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbtDistanceKltPoints.cpp.o) has no symbols
-void dummy_vpMbtDistanceKltPoints() {};
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtDistanceKltPoints.cpp.o)
+// has no symbols
+void dummy_vpMbtDistanceKltPoints(){};
#endif
diff --git a/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp
index 891df4d..1b664a0 100644
--- a/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp
+++ b/modules/tracker/mbt/src/klt/vpMbtKltXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,18 +42,17 @@
#include <iostream>
#include <map>
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbtKltXmlParser.h>
-
/*!
- Default constructor.
-
+ Default constructor.
+
*/
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();
}
@@ -60,15 +60,12 @@ vpMbtKltXmlParser::vpMbtKltXmlParser()
/*!
Default destructor.
*/
-vpMbtKltXmlParser::~vpMbtKltXmlParser()
-{
-}
+vpMbtKltXmlParser::~vpMbtKltXmlParser() {}
/*!
Initialise internal variables (including the map).
*/
-void
-vpMbtKltXmlParser::init()
+void vpMbtKltXmlParser::init()
{
vpMbXmlParser::init();
@@ -86,11 +83,10 @@ vpMbtKltXmlParser::init()
/*!
Parse the file in parameters.
This method is deprecated, use parse() instead.
-
+
\param filename : File to parse.
*/
-void
-vpMbtKltXmlParser::parse(const char * filename)
+void vpMbtKltXmlParser::parse(const char *filename)
{
std::string file = filename;
vpXmlParser::parse(file);
@@ -98,83 +94,82 @@ vpMbtKltXmlParser::parse(const char * filename)
/*!
Write info to file.
-
+
\warning Useless, so not yet implemented => Throw exception.
*/
-void
-vpMbtKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
+void vpMbtKltXmlParser::writeMainClass(xmlNodePtr /*node*/)
{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
+ throw vpException(vpException::notImplementedError, "Not yet implemented.");
}
/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
\param doc : Document to parse.
- \param node : Root node.
+ \param node : Root node.
*/
-void
-vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+void vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
bool camera_node = false;
bool face_node = false;
bool klt_node = false;
bool lod_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case camera: {
+ this->read_camera(doc, dataNode);
camera_node = true;
- }break;
- case face:{
+ } break;
+ case face: {
this->read_face(doc, dataNode);
face_node = true;
- }break;
- case klt:{
+ } break;
+ case klt: {
this->read_klt(doc, dataNode);
klt_node = true;
- }break;
- case lod:{
+ } break;
+ case lod: {
this->read_lod(doc, dataNode);
lod_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ if (!camera_node) {
+ std::cout << "camera : u0 : " << this->cam.get_u0() << " (default)" << std::endl;
+ std::cout << "camera : v0 : " << this->cam.get_v0() << " (default)" << std::endl;
+ std::cout << "camera : px : " << this->cam.get_px() << " (default)" << std::endl;
+ std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl;
}
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+ if (!face_node) {
+ std::cout << "face : Angle Appear : " << angleAppear << " (default)" << std::endl;
+ std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl;
}
- if(!klt_node) {
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ if (!klt_node) {
+ std::cout << "klt : Mask Border : " << maskBorder << " (default)" << std::endl;
+ std::cout << "klt : Max Features : " << maxFeatures << " (default)" << std::endl;
+ std::cout << "klt : Windows Size : " << winSize << " (default)" << std::endl;
+ std::cout << "klt : Quality : " << qualityValue << " (default)" << std::endl;
+ std::cout << "klt : Min Distance : " << minDist << " (default)" << std::endl;
+ std::cout << "klt : Harris Parameter : " << harrisParam << " (default)" << std::endl;
+ std::cout << "klt : Block Size : " << blockSize << " (default)" << std::endl;
+ std::cout << "klt : Pyramid Levels : " << pyramidLevels << " (default)" << std::endl;
}
- if(!lod_node) {
+ 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;
@@ -183,14 +178,13 @@ vpMbtKltXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
/*!
Read klt information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the camera information.
*/
-void
-vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
+void vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
{
bool mask_border_node = false;
bool max_features_node = false;
@@ -200,95 +194,96 @@ vpMbtKltXmlParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
bool harris_node = false;
bool size_block_node = false;
bool pyramid_lvl_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case mask_border:{
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case mask_border: {
maskBorder = xmlReadUnsignedIntChild(doc, dataNode);
mask_border_node = true;
- }break;
- case max_features:{
+ } break;
+ case max_features: {
maxFeatures = xmlReadUnsignedIntChild(doc, dataNode);
max_features_node = true;
- }break;
- case window_size:{
+ } break;
+ case window_size: {
winSize = xmlReadUnsignedIntChild(doc, dataNode);
window_size_node = true;
- }break;
- case quality:{
+ } break;
+ case quality: {
qualityValue = xmlReadDoubleChild(doc, dataNode);
quality_node = true;
- }break;
- case min_distance:{
+ } break;
+ case min_distance: {
minDist = xmlReadDoubleChild(doc, dataNode);
min_distance_node = true;
- }break;
- case harris:{
+ } break;
+ case harris: {
harrisParam = xmlReadDoubleChild(doc, dataNode);
harris_node = true;
- }break;
- case size_block:{
+ } break;
+ case size_block: {
blockSize = xmlReadUnsignedIntChild(doc, dataNode);
size_block_node = true;
- }break;
- case pyramid_lvl:{
+ } break;
+ case pyramid_lvl: {
pyramidLevels = xmlReadUnsignedIntChild(doc, dataNode);
pyramid_lvl_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_camera : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!mask_border_node)
- std::cout << "klt : Mask Border : "<< maskBorder <<" (default)" <<std::endl;
+
+ if (!mask_border_node)
+ std::cout << "klt : Mask Border : " << maskBorder << " (default)" << std::endl;
else
- std::cout << "klt : Mask Border : "<< maskBorder <<std::endl;
-
- if(!max_features_node)
- std::cout << "klt : Max Features : "<< maxFeatures <<" (default)" <<std::endl;
+ std::cout << "klt : Mask Border : " << maskBorder << std::endl;
+
+ if (!max_features_node)
+ std::cout << "klt : Max Features : " << maxFeatures << " (default)" << std::endl;
else
- std::cout << "klt : Max Features : "<< maxFeatures <<std::endl;
-
- if(!window_size_node)
- std::cout << "klt : Windows Size : "<< winSize <<" (default)" <<std::endl;
+ std::cout << "klt : Max Features : " << maxFeatures << std::endl;
+
+ if (!window_size_node)
+ std::cout << "klt : Windows Size : " << winSize << " (default)" << std::endl;
else
- std::cout << "klt : Windows Size : "<< winSize <<std::endl;
-
- if(!quality_node)
- std::cout << "klt : Quality : "<< qualityValue <<" (default)" <<std::endl;
+ std::cout << "klt : Windows Size : " << winSize << std::endl;
+
+ if (!quality_node)
+ std::cout << "klt : Quality : " << qualityValue << " (default)" << std::endl;
else
- std::cout << "klt : Quality : "<< qualityValue <<std::endl;
-
- if(!min_distance_node)
- std::cout << "klt : Min Distance : "<< minDist <<" (default)" <<std::endl;
+ std::cout << "klt : Quality : " << qualityValue << std::endl;
+
+ if (!min_distance_node)
+ std::cout << "klt : Min Distance : " << minDist << " (default)" << std::endl;
else
- std::cout << "klt : Min Distance : "<< minDist <<std::endl;
-
- if(!harris_node)
- std::cout << "klt : Harris Parameter : "<< harrisParam <<" (default)" <<std::endl;
+ std::cout << "klt : Min Distance : " << minDist << std::endl;
+
+ if (!harris_node)
+ std::cout << "klt : Harris Parameter : " << harrisParam << " (default)" << std::endl;
else
- std::cout << "klt : Harris Parameter : "<< harrisParam <<std::endl;
-
- if(!size_block_node)
- std::cout << "klt : Block Size : "<< blockSize <<" (default)" <<std::endl;
+ std::cout << "klt : Harris Parameter : " << harrisParam << std::endl;
+
+ if (!size_block_node)
+ std::cout << "klt : Block Size : " << blockSize << " (default)" << std::endl;
else
- std::cout << "klt : Block Size : "<< blockSize <<std::endl;
-
- if(!pyramid_lvl_node)
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<" (default)" <<std::endl;
+ std::cout << "klt : Block Size : " << blockSize << std::endl;
+
+ if (!pyramid_lvl_node)
+ std::cout << "klt : Pyramid Levels : " << pyramidLevels << " (default)" << std::endl;
else
- std::cout << "klt : Pyramid Levels : "<< pyramidLevels <<std::endl;
+ std::cout << "klt : Pyramid Levels : " << pyramidLevels << std::endl;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbtKltXmlParser.cpp.o) has no symbols
-void dummy_vpMbtKltXmlParser() {};
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtKltXmlParser.cpp.o) has
+// no symbols
+void dummy_vpMbtKltXmlParser(){};
#endif
-
diff --git a/modules/tracker/mbt/src/vpMbGenericTracker.cpp b/modules/tracker/mbt/src/vpMbGenericTracker.cpp
new file mode 100644
index 0000000..0b09fa4
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbGenericTracker.cpp
@@ -0,0 +1,5244 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Generic model-based tracker.
+ *
+ *****************************************************************************/
+
+#include <visp3/mbt/vpMbGenericTracker.h>
+
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbtXmlGenericParser.h>
+
+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_mapOfTrackers["Camera"] = new TrackerWrapper(EDGE_TRACKER);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
+
+ // 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))
+ m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
+#endif
+
+ m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
+ 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()
+{
+ if (nbCameras == 0) {
+ throw vpException(vpTrackingException::fatalError, "Cannot use no camera!");
+ } else if (nbCameras == 1) {
+ m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerType);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
+ } else {
+ for (unsigned int i = 1; i <= nbCameras; i++) {
+ std::stringstream ss;
+ ss << "Camera" << i;
+ m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerType);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
+ }
+
+ // Set by default the reference camera to the first one
+ m_referenceCameraName = m_mapOfTrackers.begin()->first;
+ }
+
+ // 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))
+ m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
+#endif
+
+ m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
+ m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
+}
+
+vpMbGenericTracker::vpMbGenericTracker(const std::vector<int> &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 (trackerTypes.empty()) {
+ throw vpException(vpException::badValue, "There is no camera!");
+ }
+
+ if (trackerTypes.size() == 1) {
+ m_mapOfTrackers["Camera"] = new TrackerWrapper(trackerTypes[0]);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix["Camera"] = vpHomogeneousMatrix();
+ } else {
+ for (size_t i = 1; i <= trackerTypes.size(); i++) {
+ std::stringstream ss;
+ ss << "Camera" << i;
+ m_mapOfTrackers[ss.str()] = new TrackerWrapper(trackerTypes[i - 1]);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix[ss.str()] = vpHomogeneousMatrix();
+ }
+
+ // Set by default the reference camera to the first one
+ m_referenceCameraName = m_mapOfTrackers.begin()->first;
+ }
+
+ // 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))
+ m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
+#endif
+
+ m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
+ m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
+}
+
+vpMbGenericTracker::vpMbGenericTracker(const std::vector<std::string> &cameraNames,
+ const std::vector<int> &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()");
+ }
+
+ for (size_t i = 0; i < cameraNames.size(); i++) {
+ m_mapOfTrackers[cameraNames[i]] = new TrackerWrapper(trackerTypes[i]);
+
+ // Add default camera transformation matrix
+ m_mapOfCameraTransformationMatrix[cameraNames[i]] = vpHomogeneousMatrix();
+ }
+
+ // Set by default the reference camera to the first one
+ m_referenceCameraName = m_mapOfTrackers.begin()->first;
+
+ // 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))
+ m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
+#endif
+
+ m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
+ m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
+}
+
+vpMbGenericTracker::~vpMbGenericTracker()
+{
+ for (std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.begin(); it != m_mapOfTrackers.end();
+ ++it) {
+ delete it->second;
+ it->second = NULL;
+ }
+}
+
+void vpMbGenericTracker::computeProjectionError()
+{
+ if (computeProjError) {
+ double rawTotalProjectionError = 0.0;
+ unsigned int nbTotalFeaturesUsed = 0;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+
+ double curProjError = tracker->getProjectionError();
+ unsigned int nbFeaturesUsed = tracker->nbFeaturesForProjErrorComputation;
+
+ if (nbFeaturesUsed > 0) {
+ nbTotalFeaturesUsed += nbFeaturesUsed;
+ rawTotalProjectionError += (vpMath::rad(curProjError) * nbFeaturesUsed);
+ }
+ }
+
+ if (nbTotalFeaturesUsed > 0) {
+ projectionError = vpMath::deg(rawTotalProjectionError / (double)nbTotalFeaturesUsed);
+ } else {
+ projectionError = 90.0;
+ }
+ } else {
+ projectionError = 90.0;
+ }
+}
+
+void vpMbGenericTracker::computeVVS(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ computeVVSInit(mapOfImages);
+
+ if (m_error.getRows() < 4) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
+ }
+
+ double normRes = 0;
+ double normRes_1 = -1;
+ unsigned int iter = 0;
+
+ vpMatrix LTL;
+ vpColVector LTR, v;
+ vpColVector error_prev;
+
+ double mu = m_initialMu;
+ vpHomogeneousMatrix cMo_prev;
+
+ bool isoJoIdentity_ = true;
+
+ // Covariance
+ vpColVector W_true(m_error.getRows());
+ vpMatrix L_true, LVJ_true;
+
+ // Create the map of VelocityTwistMatrices
+ std::map<std::string, vpVelocityTwistMatrix> mapOfVelocityTwist;
+ for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = m_mapOfCameraTransformationMatrix.begin();
+ it != m_mapOfCameraTransformationMatrix.end(); ++it) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(it->second);
+ mapOfVelocityTwist[it->first] = cVo;
+ }
+
+ double factorEdge = m_mapOfFeatureFactors[EDGE_TRACKER];
+#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)) {
+ computeVVSInteractionMatrixAndResidu(mapOfImages, mapOfVelocityTwist);
+
+ bool reStartFromLastIncrement = false;
+ computeVVSCheckLevenbergMarquardt(iter, m_error, error_prev, cMo_prev, mu, reStartFromLastIncrement);
+ if (reStartFromLastIncrement) {
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+
+ tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo_prev;
+
+#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();
+ tracker->ctTc0 = c_curr_tTc_curr0;
+#endif
+ }
+ }
+
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
+
+ if (computeCovariance) {
+ L_true = m_L;
+ if (!isoJoIdentity_) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L * (cVo * oJo));
+ }
+ }
+
+ vpVelocityTwistMatrix cVo;
+ if (iter == 0) {
+ isoJoIdentity_ = true;
+ oJo.eye();
+
+ // If all the 6 dof should be estimated, we check if the interaction
+ // matrix is full rank. If not we remove automatically the dof that
+ // cannot be estimated This is particularly useful when consering
+ // circles (rank 5) and cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+
+ vpMatrix K; // kernel
+ unsigned int rank = (m_L * cVo).kernel(K);
+ if (rank == 0) {
+ throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
+ }
+
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I - K.AtA();
+
+ isoJoIdentity_ = false;
+ }
+ }
+ }
+
+ // Weighting
+ double num = 0;
+ double den = 0;
+
+ unsigned int start_index = 0;
+ for (std::map<std::string, TrackerWrapper *>::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;
+ W_true[start_index + i] = wi;
+ m_weightedError[start_index + i] = wi * m_error[start_index + i];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += tracker->m_error_edge.getRows();
+ }
+
+#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;
+ W_true[start_index + i] = wi;
+ m_weightedError[start_index + i] = wi * m_error[start_index + i];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += tracker->m_error_klt.getRows();
+ }
+#endif
+
+ if (tracker->m_trackerType & DEPTH_NORMAL_TRACKER) {
+ 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];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += tracker->m_error_depthNormal.getRows();
+ }
+
+ if (tracker->m_trackerType & DEPTH_DENSE_TRACKER) {
+ 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];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += tracker->m_error_depthDense.getRows();
+ }
+ }
+
+ normRes_1 = normRes;
+ normRes = sqrt(num / den);
+
+ 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<std::string, TrackerWrapper *>::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();
+ tracker->ctTc0 = c_curr_tTc_curr0;
+ }
+#endif
+
+ // Update cMo
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
+ }
+ }
+
+ iter++;
+ }
+
+ computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+
+ if (tracker->m_trackerType & EDGE_TRACKER) {
+ tracker->updateMovingEdgeWeights();
+ }
+ }
+}
+
+void vpMbGenericTracker::computeVVSInit()
+{
+ throw vpException(vpException::fatalError, "vpMbGenericTracker::computeVVSInit() should not be called!");
+}
+
+void vpMbGenericTracker::computeVVSInit(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ unsigned int nbFeatures = 0;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->computeVVSInit(mapOfImages[it->first]);
+
+ nbFeatures += tracker->m_error.getRows();
+ }
+
+ m_L.resize(nbFeatures, 6, false, false);
+ m_error.resize(nbFeatures, false);
+
+ m_weightedError.resize(nbFeatures, false);
+ m_w.resize(nbFeatures, false);
+ m_w = 1;
+}
+
+void vpMbGenericTracker::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbGenericTracker::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called");
+}
+
+void vpMbGenericTracker::computeVVSInteractionMatrixAndResidu(
+ std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, vpVelocityTwistMatrix> &mapOfVelocityTwist)
+{
+ unsigned int start_index = 0;
+
+ for (std::map<std::string, TrackerWrapper *>::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();
+ tracker->ctTc0 = c_curr_tTc_curr0;
+#endif
+
+ tracker->computeVVSInteractionMatrixAndResidu(mapOfImages[it->first]);
+
+ m_L.insert(tracker->m_L * mapOfVelocityTwist[it->first], start_index, 0);
+ m_error.insert(start_index, tracker->m_error);
+
+ start_index += tracker->m_error.getRows();
+ }
+}
+
+void vpMbGenericTracker::computeVVSWeights()
+{
+ unsigned int start_index = 0;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->computeVVSWeights();
+
+ m_w.insert(start_index, tracker->m_w);
+ start_index += tracker->m_w.getRows();
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I : The grayscale image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param cam_ : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+
+ \note This function will display the model only for the reference camera.
+*/
+void vpMbGenericTracker::display(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I : The color image.
+ \param cMo_ : Pose used to project the 3D model into the image.
+ \param cam_ : The camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+
+ \note This function will display the model only for the reference camera.
+*/
+void vpMbGenericTracker::display(const vpImage<vpRGBa> &I, const vpHomogeneousMatrix &cMo_,
+ const vpCameraParameters &cam_, const vpColor &col, const unsigned int thickness,
+ const bool displayFullModel)
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I1 : The first grayscale image.
+ \param I2 : The second grayscale image.
+ \param c1Mo : Pose used to project the 3D model into the first image.
+ \param c2Mo : Pose used to project the 3D model into the second image.
+ \param cam1 : The first camera parameters.
+ \param cam2 : The second camera parameters.
+ \param color : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::display(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &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<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param I1 : The first color image.
+ \param I2 : The second color image.
+ \param c1Mo : Pose used to project the 3D model into the first image.
+ \param c2Mo : Pose used to project the 3D model into the second image.
+ \param cam1 : The first camera parameters.
+ \param cam2 : The second camera parameters.
+ \param color : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::display(const vpImage<vpRGBa> &I1, const vpImage<vpRGBa> &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<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param mapOfImages : Map of grayscale images.
+ \param mapOfCameraPoses : Map of camera poses.
+ \param mapOfCameraParameters : Map of camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+*/
+void vpMbGenericTracker::display(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
+ std::map<std::string, vpCameraParameters>::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);
+ } else {
+ std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
+ }
+ }
+}
+
+/*!
+ Display the 3D model from a given position of the camera.
+
+ \param mapOfImages : Map of color images.
+ \param mapOfCameraPoses : Map of camera poses.
+ \param mapOfCameraParameters : Map of camera parameters.
+ \param col : The desired color.
+ \param thickness : The thickness of the lines.
+ \param displayFullModel : If true, the full model is displayed (even the non
+ visible faces).
+*/
+void vpMbGenericTracker::display(const std::map<std::string, const vpImage<vpRGBa> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const std::map<std::string, vpCameraParameters> &mapOfCameraParameters,
+ const vpColor &col, const unsigned int thickness, const bool displayFullModel)
+{
+ // Display only for the given images
+ for (std::map<std::string, const vpImage<vpRGBa> *>::const_iterator it_img = mapOfImages.begin();
+ it_img != mapOfImages.end(); ++it_img) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it_img->first);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camPose = mapOfCameraPoses.find(it_img->first);
+ std::map<std::string, vpCameraParameters>::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);
+ } else {
+ std::cerr << "Missing elements for image:" << it_img->first << "!" << std::endl;
+ }
+ }
+}
+
+/*!
+ Get the camera names.
+
+ \return The vector of camera names.
+*/
+std::vector<std::string> vpMbGenericTracker::getCameraNames() const
+{
+ std::vector<std::string> cameraNames;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
+ cameraNames.push_back(it_tracker->first);
+ }
+
+ return cameraNames;
+}
+
+/*!
+ Get all the camera parameters.
+
+ \param cam1 : Copy of the camera parameters for the first camera.
+ \param cam2 : Copy of the camera parameters for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::getCameraParameters(vpCameraParameters &cam1, vpCameraParameters &cam2) const
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get all the camera parameters.
+
+ \param mapOfCameraParameters : Map of camera parameters.
+*/
+void vpMbGenericTracker::getCameraParameters(std::map<std::string, vpCameraParameters> &mapOfCameraParameters) const
+{
+ // Clear the input map
+ mapOfCameraParameters.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ vpCameraParameters cam_;
+ it->second->getCameraParameters(cam_);
+ mapOfCameraParameters[it->first] = cam_;
+ }
+}
+
+/*!
+ Get the camera tracker types.
+
+ \return The map of camera tracker types.
+ \sa vpTrackerType
+*/
+std::map<std::string, int> vpMbGenericTracker::getCameraTrackerTypes() const
+{
+ std::map<std::string, int> trackingTypes;
+
+ TrackerWrapper *traker;
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
+ traker = it_tracker->second;
+ trackingTypes[it_tracker->first] = traker->getTrackerType();
+ }
+
+ return trackingTypes;
+}
+
+/*!
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType.
+
+ \param clippingFlag1 : Clipping flags for the first camera.
+ \param clippingFlag2 : Clipping flags for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::getClipping(unsigned int &clippingFlag1, unsigned int &clippingFlag2) const
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the clipping used and defined in vpPolygon3D::vpMbtPolygonClippingType.
+
+ \param mapOfClippingFlags : Map of clipping flags.
+*/
+void vpMbGenericTracker::getClipping(std::map<std::string, unsigned int> &mapOfClippingFlags) const
+{
+ mapOfClippingFlags.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ mapOfClippingFlags[it->first] = tracker->getClipping();
+ }
+}
+
+/*!
+ Return a reference to the faces structure.
+
+ \return Reference to the face structure.
+*/
+vpMbHiddenFaces<vpMbtPolygon> &vpMbGenericTracker::getFaces()
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ return faces;
+}
+
+/*!
+ Return a reference to the faces structure for the given camera name.
+
+ \return Reference to the face structure.
+*/
+vpMbHiddenFaces<vpMbtPolygon> &vpMbGenericTracker::getFaces(const std::string &cameraName)
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ return faces;
+}
+
+#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<vpMbtDistanceCircle *> &vpMbGenericTracker::getFeaturesCircle()
+{
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_referenceCameraName.c_str());
+ }
+}
+
+/*!
+ Return the address of the cylinder feature list for the reference camera.
+*/
+std::list<vpMbtDistanceKltCylinder *> &vpMbGenericTracker::getFeaturesKltCylinder()
+{
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_referenceCameraName.c_str());
+ }
+}
+
+/*!
+ Return the address of the Klt feature list for the reference camera.
+*/
+std::list<vpMbtDistanceKltPoints *> &vpMbGenericTracker::getFeaturesKlt()
+{
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_referenceCameraName.c_str());
+ }
+}
+#endif
+
+/*!
+ \return The threshold value between 0 and 1 over good moving edges ratio.
+ It allows to decide if the tracker has enough valid moving edges to compute
+ a pose. 1 means that all moving edges should be considered as good to have
+ a valid pose, while 0.1 means that 10% of the moving edge are enough to
+ declare a pose valid.
+
+ \sa setGoodMovingEdgesRatioThreshold()
+*/
+double vpMbGenericTracker::getGoodMovingEdgesRatioThreshold() const { return m_percentageGdPt; }
+
+#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.
+
+ \warning This function convert and copy the OpenCV KLT points into
+ vpImagePoints.
+
+ \return the list of KLT points through vpKltOpencv.
+*/
+std::vector<vpImagePoint> vpMbGenericTracker::getKltImagePoints() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return std::vector<vpImagePoint>();
+}
+
+/*!
+ Get the current list of KLT points and their id for the reference camera.
+
+ \warning This function convert and copy the openCV KLT points into
+ vpImagePoints.
+
+ \return the list of KLT points and their id through vpKltOpencv.
+*/
+std::map<int, vpImagePoint> vpMbGenericTracker::getKltImagePointsWithId() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return std::map<int, vpImagePoint>();
+}
+
+/*!
+ Get the erosion of the mask used on the Model faces.
+
+ \return The erosion for the reference camera.
+*/
+unsigned int vpMbGenericTracker::getKltMaskBorder() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return 0;
+}
+
+/*!
+ Get number of KLT points for the reference camera.
+
+ \return Number of KLT points for the reference camera.
+*/
+int vpMbGenericTracker::getKltNbPoints() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return 0;
+}
+
+/*!
+ Get the klt tracker at the current state for the reference camera.
+
+ \return klt tracker.
+*/
+vpKltOpencv vpMbGenericTracker::getKltOpencv() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return vpKltOpencv();
+}
+
+/*!
+ Get the klt tracker at the current state.
+
+ \param klt1 : Klt tracker for the first camera.
+ \param klt2 : Klt tracker for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::getKltOpencv(vpKltOpencv &klt1, vpKltOpencv &klt2) const
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the klt tracker at the current state.
+
+ \param mapOfKlts : Map if klt trackers.
+*/
+void vpMbGenericTracker::getKltOpencv(std::map<std::string, vpKltOpencv> &mapOfKlts) const
+{
+ mapOfKlts.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ mapOfKlts[it->first] = tracker->getKltOpencv();
+ }
+}
+
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+/*!
+ Get the current list of KLT points for the reference camera.
+
+ \return the list of KLT points through vpKltOpencv.
+*/
+std::vector<cv::Point2f> vpMbGenericTracker::getKltPoints() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return std::vector<cv::Point2f>();
+}
+#endif
+
+/*!
+ Get the threshold for the acceptation of a point.
+
+ \return threshold_outlier : Threshold for the weight below which a point is
+ rejected.
+*/
+double vpMbGenericTracker::getKltThresholdAcceptation() const { return m_thresholdOutlier; }
+#endif
+
+/*!
+ Get the list of the circles tracked for the specified level. Each circle
+ contains the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
+
+ \param cameraName : Camera name for which we want to get the list of
+ vpMbtDistanceCircle. \param circlesList : The list of the circles of the
+ model. \param level : Level corresponding to the list to return.
+
+ \note Multi-scale moving edge tracking is not possible, scale level=0 must
+ be used.
+*/
+void vpMbGenericTracker::getLcircle(const std::string &cameraName, std::list<vpMbtDistanceCircle *> &circlesList,
+ const unsigned int level) const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the list of the cylinders tracked for the specified level. Each cylinder
+ contains the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
+
+ \param cameraName : Camera name for which we want to get the list of
+ vpMbtDistanceCylinder. \param cylindersList : The list of the cylinders of
+ the model. \param level : Level corresponding to the list to return.
+
+ \note Multi-scale moving edge tracking is not possible, scale level=0 must
+ be used.
+*/
+void vpMbGenericTracker::getLcylinder(const std::string &cameraName, std::list<vpMbtDistanceCylinder *> &cylindersList,
+ const unsigned int level) const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the list of the lines tracked for the specified level. Each line
+ contains the list of the vpMeSite.
+
+ \throw vpException::dimensionError if the second parameter does not
+ correspond to an used level.
+
+ \param cameraName : Camera name for which we want to get the list of
+ vpMbtDistanceLine. \param linesList : The list of the lines of the model.
+ \param level : Level corresponding to the list to return.
+
+ \note Multi-scale moving edge tracking is not possible, scale level=0 must
+ be used.
+*/
+void vpMbGenericTracker::getLline(const std::string &cameraName, std::list<vpMbtDistanceLine *> &linesList,
+ const unsigned int level) const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the moving edge parameters for the reference camera.
+
+ \return an instance of the moving edge parameters used by the tracker.
+*/
+vpMe vpMbGenericTracker::getMovingEdge() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return vpMe();
+}
+
+/*!
+ Get the moving edge parameters.
+
+ \param me1 : Moving edge parameters for the first camera.
+ \param me2 : Moving edge parameters for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::getMovingEdge(vpMe &me1, vpMe &me2) const
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+
+#endif
+
+/*!
+ Get the moving edge parameters for all the cameras
+
+ \param mapOfMovingEdges : Map of moving edge parameters for all the cameras.
+*/
+void vpMbGenericTracker::getMovingEdge(std::map<std::string, vpMe> &mapOfMovingEdges) const
+{
+ mapOfMovingEdges.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ mapOfMovingEdges[it->first] = tracker->getMovingEdge();
+ }
+}
+
+/*!
+ Return the number of good points (vpMeSite) tracked. A good point is a
+ vpMeSite with its flag "state" equal to 0. Only these points are used
+ during the virtual visual servoing stage.
+
+ \param level : Pyramid level to consider.
+
+ \exception vpException::dimensionError if level does not represent a used
+ level.
+
+ \return the number of good points for the reference camera.
+
+ \note Multi-scale moving edge tracking is not possible, scale level=0 must
+ be used.
+*/
+unsigned int vpMbGenericTracker::getNbPoints(const unsigned int level) const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+
+ return nbGoodPoints;
+}
+
+/*!
+ Return the number of good points (vpMeSite) tracked. A good point is a
+ vpMeSite with its flag "state" equal to 0. Only these points are used
+ during the virtual visual servoing stage.
+
+ \param mapOfNbPoints : Map of number of good points (vpMeSite) tracked for
+ all the cameras. \param level : Pyramid level to consider.
+
+ \exception vpException::dimensionError if level does not represent a used
+ level.
+
+ \note Multi-scale moving edge tracking is not possible, scale level=0 must
+ be used.
+*/
+void vpMbGenericTracker::getNbPoints(std::map<std::string, unsigned int> &mapOfNbPoints, const unsigned int level) const
+{
+ mapOfNbPoints.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ mapOfNbPoints[it->first] = tracker->getNbPoints(level);
+ }
+}
+
+/*!
+ Get the number of polygons (faces) representing the object to track.
+
+ \return Number of polygons for the reference camera.
+*/
+unsigned int vpMbGenericTracker::getNbPolygon() const
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ return 0;
+}
+
+/*!
+ Get the number of polygons (faces) representing the object to track.
+
+ \return Number of polygons for all the cameras.
+*/
+void vpMbGenericTracker::getNbPolygon(std::map<std::string, unsigned int> &mapOfNbPolygons) const
+{
+ mapOfNbPolygons.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ mapOfNbPolygons[it->first] = tracker->getNbPolygon();
+ }
+}
+
+/*!
+ Return the polygon (face) "index" for the reference camera.
+
+ \exception vpException::dimensionError if index does not represent a good
+ polygon.
+
+ \param index : Index of the polygon to return.
+ \return Pointer to the polygon index for the reference camera or NULL in
+ case of problem.
+*/
+vpMbtPolygon *vpMbGenericTracker::getPolygon(const unsigned int index)
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ return NULL;
+}
+
+/*!
+ Return the polygon (face) "index" for the specified camera.
+
+ \exception vpException::dimensionError if index does not represent a good
+ polygon.
+
+ \param cameraName : Name of the camera to return the polygon.
+ \param index : Index of the polygon to return.
+ \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)
+{
+ std::map<std::string, TrackerWrapper *>::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;
+ return NULL;
+}
+
+/*!
+ Get the list of polygons faces (a vpPolygon representing the projection of
+ the face in the image and a list of face corners in 3D), with the
+ possibility to order by distance to the camera or to use the visibility
+ check to consider if the polygon face must be retrieved or not.
+
+ \param orderPolygons : If true, the resulting list is ordered from the
+ nearest polygon faces to the farther. \param useVisibility : If true, only
+ visible faces will be retrieved. \param clipPolygon : If true, the polygons
+ will be clipped according to the clipping flags set in vpMbTracker. \return
+ A pair object containing the list of vpPolygon and the list of face corners.
+
+ \note This function will return the 2D polygons faces and 3D face points
+ only for the reference camera.
+*/
+std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
+vpMbGenericTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
+{
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces;
+
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it->second;
+ polygonFaces = tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+
+ return polygonFaces;
+}
+
+/*!
+ Get the list of polygons faces (a vpPolygon representing the projection of
+ the face in the image and a list of face corners in 3D), with the
+ possibility to order by distance to the camera or to use the visibility
+ check to consider if the polygon face must be retrieved or not.
+
+ \param mapOfPolygons : Map of 2D polygon faces.
+ \param mapOfPoints : Map of face 3D points.
+ \param orderPolygons : If true, the resulting list is ordered from the
+ nearest polygon faces to the farther. \param useVisibility : If true, only
+ visible faces will be retrieved. \param clipPolygon : If true, the polygons
+ will be clipped according to the clipping flags set in vpMbTracker. \return
+ A pair object containing the list of vpPolygon and the list of face corners.
+
+ \note This function will return the 2D polygons faces and 3D face points
+ only for all the cameras.
+*/
+void vpMbGenericTracker::getPolygonFaces(std::map<std::string, std::vector<vpPolygon> > &mapOfPolygons,
+ std::map<std::string, std::vector<std::vector<vpPoint> > > &mapOfPoints,
+ const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
+{
+ mapOfPolygons.clear();
+ mapOfPoints.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > polygonFaces =
+ tracker->getPolygonFaces(orderPolygons, useVisibility, clipPolygon);
+
+ mapOfPolygons[it->first] = polygonFaces.first;
+ mapOfPoints[it->first] = polygonFaces.second;
+ }
+}
+
+/*!
+ Get the current pose between the object and the cameras.
+
+ \param c1Mo : The camera pose for the first camera.
+ \param c2Mo : The camera pose for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::getPose(vpHomogeneousMatrix &c1Mo, vpHomogeneousMatrix &c2Mo) const
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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;
+ }
+}
+
+/*!
+ Get the current pose between the object and the cameras.
+
+ \param mapOfCameraPoses : The map of camera poses for all the cameras.
+*/
+void vpMbGenericTracker::getPose(std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses) const
+{
+ // Clear the map
+ mapOfCameraPoses.clear();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->getPose(mapOfCameraPoses[it->first]);
+ }
+}
+
+void vpMbGenericTracker::init(const vpImage<unsigned char> &I)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->cMo = m_mapOfCameraTransformationMatrix[it->first] * cMo;
+ tracker->init(I);
+ }
+}
+
+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!");
+}
+
+#ifdef VISP_HAVE_MODULE_GUI
+
+/*!
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
+ \code
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
+ \endcode
+
+ \param I1 : Input image for the first camera.
+ \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
+ 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
+ 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.
+
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::initClick(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2, const bool displayHelp)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ TrackerWrapper *tracker = it->second;
+ tracker->initClick(I1, initFile1, displayHelp);
+
+ ++it;
+
+ tracker = it->second;
+ tracker->initClick(I2, initFile2, displayHelp);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ tracker = it->second;
+
+ // Set the reference cMo
+ tracker->getPose(cMo);
+ }
+ } else {
+ throw vpException(vpTrackingException::initializationError,
+ "Cannot initClick()! Require two cameras but there are %d cameras!", m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Initialise the tracker by clicking in the reference image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
+ \code
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
+ \endcode
+
+ The cameras that have not an init file will be automatically initialized but
+ the camera transformation matrices have to be set before.
+
+ \param mapOfImages : Map of images.
+ \param mapOfInitFiles : Map of files containing the points where to click
+ for each 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.
+
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
+
+ \note Image and init file must be supplied for the reference camera. The
+ images for all the cameras must be supplied to correctly initialize the
+ trackers but some init files can be omitted. In this case, they will be
+ initialized using the pose computed from the reference camera pose and using
+ the known geometric transformation between each camera (see
+ setCameraTransformationMatrix()).
+*/
+void vpMbGenericTracker::initClick(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitFiles, const bool displayHelp)
+{
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, std::string>::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!");
+ }
+
+ // Vector of missing initFile for cameras
+ std::vector<std::string> vectorOfMissingCameraPoses;
+
+ // Set pose for the specified cameras
+ 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()) {
+ // InitClick for the current camera
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->initClick(*it_img->second, it_initFile->second, displayHelp);
+ } else {
+ vectorOfMissingCameraPoses.push_back(it_tracker->first);
+ }
+ }
+ }
+
+ // Init for cameras that do not have an initFile
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
+ it != vectorOfMissingCameraPoses.end(); ++it) {
+ it_img = mapOfImages.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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);
+ } else {
+ throw vpException(vpTrackingException::initializationError,
+ "Missing image or missing camera transformation "
+ "matrix! Cannot set the pose for camera: %s!",
+ it->c_str());
+ }
+ }
+}
+#endif
+
+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!");
+}
+
+void vpMbGenericTracker::initFaceFromCorners(vpMbtPolygon & /*polygon*/)
+{
+ 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!");
+}
+
+/*!
+ Initialise the tracker by reading 3D point coordinates and the corresponding
+ 2D image point coordinates from a file. Comments starting with # character
+ 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 # /
+ \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
+ the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::initFromPoints(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ TrackerWrapper *tracker = it->second;
+ tracker->initFromPoints(I1, initFile1);
+
+ ++it;
+
+ tracker = it->second;
+ tracker->initFromPoints(I2, initFile2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ tracker = it->second;
+
+ // Set the reference cMo
+ tracker->getPose(cMo);
+
+ // Set the reference camera parameters
+ tracker->getCameraParameters(cam);
+ }
+ } else {
+ throw vpException(vpTrackingException::initializationError,
+ "Cannot initFromPoints()! Require two cameras but "
+ "there are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+void vpMbGenericTracker::initFromPoints(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitPoints)
+{
+ // Set the reference cMo
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, std::string>::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!");
+ }
+
+ // Vector of missing initPoints for cameras
+ std::vector<std::string> vectorOfMissingCameraPoints;
+
+ // Set pose for the specified cameras
+ 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()) {
+ // Set pose
+ it_tracker->second->initFromPoints(*it_img->second, it_initPoints->second);
+ } else {
+ vectorOfMissingCameraPoints.push_back(it_tracker->first);
+ }
+ }
+
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoints.begin();
+ it != vectorOfMissingCameraPoints.end(); ++it) {
+ it_img = mapOfImages.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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 {
+ throw vpException(vpTrackingException::initializationError,
+ "Missing image or missing camera transformation "
+ "matrix! Cannot init the pose for camera: %s!",
+ it->c_str());
+ }
+ }
+}
+
+/*!
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile.
+
+ \param I1 : Input image for the first camera.
+ \param I2 : Input image for the second camera.
+ \param initFile1 : Init pose file for the first camera.
+ \param initFile2 : Init pose file for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const std::string &initFile1, const std::string &initFile2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ TrackerWrapper *tracker = it->second;
+ tracker->initFromPose(I1, initFile1);
+
+ ++it;
+
+ tracker = it->second;
+ tracker->initFromPose(I2, initFile2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ tracker = it->second;
+
+ // Set the reference cMo
+ tracker->getPose(cMo);
+
+ // Set the reference camera parameters
+ tracker->getCameraParameters(cam);
+ }
+ } else {
+ throw vpException(vpTrackingException::initializationError,
+ "Cannot initFromPose()! Require two cameras but there "
+ "are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile.
+
+ \param mapOfImages : Map of images.
+ \param mapOfInitPoses : Map of init pose files.
+
+ \note Image and init pose file must be supplied for the reference camera.
+ The images for all the cameras must be supplied to correctly initialize the
+ trackers but some init pose files can be omitted. In this case, they will be
+ initialized using the pose computed from the reference camera pose and using
+ the known geometric transformation between each camera (see
+ setCameraTransformationMatrix()).
+*/
+void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfInitPoses)
+{
+ // Set the reference cMo
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, std::string>::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!");
+ }
+
+ // Vector of missing pose matrices for cameras
+ std::vector<std::string> vectorOfMissingCameraPoses;
+
+ // Set pose for the specified cameras
+ 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);
+
+ if (it_img != mapOfImages.end() && it_initPose != mapOfInitPoses.end()) {
+ // Set pose
+ it_tracker->second->initFromPose(*it_img->second, it_initPose->second);
+ } else {
+ vectorOfMissingCameraPoses.push_back(it_tracker->first);
+ }
+ }
+
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
+ it != vectorOfMissingCameraPoses.end(); ++it) {
+ it_img = mapOfImages.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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 {
+ throw vpException(vpTrackingException::initializationError,
+ "Missing image or missing camera transformation "
+ "matrix! Cannot init the pose for camera: %s!",
+ it->c_str());
+ }
+ }
+}
+
+/*!
+ Initialize the tracking thanks to the pose.
+
+ \param I1 : Input image for the first camera.
+ \param I2 : Input image for the second camera.
+ \param c1Mo : Pose matrix for the first camera.
+ \param c2Mo : Pose matrix for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::initFromPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->initFromPose(I1, c1Mo);
+
+ ++it;
+
+ it->second->initFromPose(I2, c2Mo);
+
+ this->cMo = c1Mo;
+ } else {
+ throw vpException(vpTrackingException::initializationError,
+ "This method requires 2 cameras but there are %d cameras!", m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Initialize the tracking thanks to the pose.
+
+ \param mapOfImages : Map of images.
+ \param mapOfCameraPoses : Map of pose matrix.
+
+ \note Image and camera pose must be supplied for the reference camera. The
+ images for all the cameras must be supplied to correctly initialize the
+ trackers but some camera poses can be omitted. In this case, they will be
+ initialized using the pose computed from the reference camera pose and using
+ the known geometric transformation between each camera (see
+ setCameraTransformationMatrix()).
+*/
+void vpMbGenericTracker::initFromPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
+ // Set the reference cMo
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, vpHomogeneousMatrix>::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!");
+ }
+
+ // Vector of missing pose matrices for cameras
+ std::vector<std::string> vectorOfMissingCameraPoses;
+
+ // Set pose for the specified cameras
+ 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);
+
+ if (it_img != mapOfImages.end() && it_camPose != mapOfCameraPoses.end()) {
+ // Set pose
+ it_tracker->second->initFromPose(*it_img->second, it_camPose->second);
+ } else {
+ vectorOfMissingCameraPoses.push_back(it_tracker->first);
+ }
+ }
+
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
+ it != vectorOfMissingCameraPoses.end(); ++it) {
+ it_img = mapOfImages.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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 {
+ throw vpException(vpTrackingException::initializationError,
+ "Missing image or missing camera transformation "
+ "matrix! Cannot set the pose for camera: %s!",
+ it->c_str());
+ }
+ }
+}
+
+/*!
+ Load the xml configuration file.
+ From the configuration file initialize the parameters corresponding to the
+ objects: tracking parameters, camera intrinsic parameters.
+
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
+
+ \throw vpException::ioError if the file has not been properly parsed (file
+ not found or wrong format for the data).
+
+ \param configFile : full name of the xml file.
+
+ \sa vpXmlParser::cleanup()
+*/
+void vpMbGenericTracker::loadConfigFile(const std::string &configFile)
+{
+ for (std::map<std::string, TrackerWrapper *>::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());
+ }
+
+ m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
+ this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
+ this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
+ this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
+}
+
+/*!
+ Load the xml configuration files.
+ From the configuration file initialize the parameters corresponding to the
+ objects: tracking parameters, camera intrinsic parameters.
+
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
+
+ \param configFile1 : Full name of the xml file for the first camera.
+ \param configFile2 : Full name of the xml file for the second camera.
+
+ \sa vpXmlParser::cleanup()
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+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!");
+ }
+
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->loadConfigFile(configFile1);
+
+ ++it_tracker;
+ tracker = it_tracker->second;
+ 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());
+ }
+
+ m_mapOfTrackers[m_referenceCameraName]->getCameraParameters(this->cam);
+ this->angleAppears = m_mapOfTrackers[m_referenceCameraName]->getAngleAppear();
+ this->angleDisappears = m_mapOfTrackers[m_referenceCameraName]->getAngleDisappear();
+ this->clippingFlag = m_mapOfTrackers[m_referenceCameraName]->getClipping();
+}
+
+/*!
+ Load the xml configuration files.
+ From the configuration file initialize the parameters corresponding to the
+ objects: tracking parameters, camera intrinsic parameters.
+
+ \warning To clean up memory allocated by the xml library, the user has to
+ call vpXmlParser::cleanup() before the exit().
+
+ \param mapOfConfigFiles : Map of xml files.
+
+ \sa loadConfigFile(const std::string &), vpXmlParser::cleanup()
+
+ \note Configuration files must be supplied for all the cameras.
+*/
+void vpMbGenericTracker::loadConfigFile(const std::map<std::string, std::string> &mapOfConfigFiles)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
+ TrackerWrapper *tracker = it_tracker->second;
+
+ std::map<std::string, std::string>::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!",
+ it_tracker->first.c_str());
+ }
+ }
+
+ // Set the reference camera parameters
+ std::map<std::string, TrackerWrapper *>::iterator it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it->second;
+ tracker->getCameraParameters(cam);
+
+ // Set clipping
+ this->clippingFlag = tracker->getClipping();
+ this->angleAppears = tracker->getAngleAppear();
+ this->angleDisappears = tracker->getAngleDisappear();
+ } else {
+ throw vpException(vpTrackingException::initializationError, "The reference camera: %s does not exist!",
+ m_referenceCameraName.c_str());
+ }
+}
+
+/*!
+ Load a 3D model from the file in parameter. This file must either be a vrml
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
+
+ \warning When this class is called to load a vrml model, remember that you
+ have to call Call SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
+
+ \param modelFile : the file containing the 3D model description.
+ The extension of this file is either .wrl or .cao.
+ \param verbose : verbose option to print additional information when loading
+CAO model files which include other CAO model files.
+
+ \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)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->loadModel(modelFile, verbose);
+ }
+}
+
+/*!
+ Load a 3D model from the file in parameter. This file must either be a vrml
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
+
+ \warning When this class is called to load a vrml model, remember that you
+ have to call Call SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
+
+ \param modelFile1 : the file containing the 3D model description for the
+first camera. The extension of this file is either .wrl or .cao. \param
+modelFile2 : the file containing the the 3D model description for the second
+camera. The extension of this file is either .wrl or .cao. \param verbose :
+verbose option to print additional information when loading CAO model files
+which include other CAO model files.
+
+ \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)
+{
+ if (m_mapOfTrackers.size() != 2) {
+ throw vpException(vpException::fatalError, "The tracker is not set in a stereo configuration!");
+ }
+
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->loadModel(modelFile1, verbose);
+
+ ++it_tracker;
+ tracker = it_tracker->second;
+ tracker->loadModel(modelFile2, verbose);
+}
+
+/*!
+ Load a 3D model from the file in parameter. This file must either be a vrml
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
+
+ \warning When this class is called to load a vrml model, remember that you
+ have to call Call SoDD::finish() before ending the program.
+ \code
+int main()
+{
+ ...
+#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
+ SoDB::finish();
+#endif
+}
+ \endcode
+
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
+
+ \param mapOfModelFiles : map of files containing the 3D model description.
+ The extension of this file is either .wrl or .cao.
+ \param verbose : verbose option to print additional information when loading
+CAO model files which include other CAO model files.
+
+ \note Each camera must have a model file.
+*/
+void vpMbGenericTracker::loadModel(const std::map<std::string, std::string> &mapOfModelFiles, const bool verbose)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+ it_tracker != m_mapOfTrackers.end(); ++it_tracker) {
+ std::map<std::string, std::string>::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",
+ it_tracker->first.c_str());
+ }
+ }
+}
+
+#ifdef VISP_HAVE_PCL
+void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->preTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
+ }
+}
+#endif
+
+void vpMbGenericTracker::preTracking(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
+ std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+ std::map<std::string, unsigned int> &mapOfPointCloudHeights)
+{
+ for (std::map<std::string, TrackerWrapper *>::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],
+ mapOfPointCloudHeights[it->first]);
+ }
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I : The image containing the object to initialize.
+ \param cad_name : Path to the file containing the 3D model description.
+ \param cMo_ : The new vpHomogeneousMatrix between the camera and the new
+ model. \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
+*/
+void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &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());
+ }
+
+ std::map<std::string, TrackerWrapper *>::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);
+
+ // Set reference pose
+ tracker->getPose(cMo);
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() the reference camera!");
+ }
+
+ modelInitialised = true;
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param I1 : The image containing the object to initialize for the first
+ camera. \param I2 : The image containing the object to initialize for the
+ second camera. \param cad_name1 : Path to the file containing the 3D model
+ description for the first camera. \param cad_name2 : Path to the file
+ containing the 3D model description for the second camera. \param c1Mo : The
+ new vpHomogeneousMatrix between the first camera and the new model. \param
+ c2Mo : The new vpHomogeneousMatrix between the second camera and the new
+ model. \param verbose : verbose option to print additional information when
+ loading CAO model files which include other CAO model files.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::reInitModel(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &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<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.begin();
+
+ it_tracker->second->reInitModel(I1, cad_name1, c1Mo, verbose);
+
+ ++it_tracker;
+
+ it_tracker->second->reInitModel(I2, cad_name2, c2Mo, verbose);
+
+ it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it_tracker != m_mapOfTrackers.end()) {
+ // Set reference pose
+ it_tracker->second->getPose(cMo);
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "This method requires exactly two cameras!");
+ }
+
+ modelInitialised = true;
+}
+
+/*!
+ Re-initialize the model used by the tracker.
+
+ \param mapOfImages : Map of images.
+ \param mapOfModelFiles : Map of model files.
+ \param mapOfCameraPoses : The new vpHomogeneousMatrix between the cameras
+ and the current object position. \param verbose : Verbose option to print
+ additional information when loading CAO model files which include other CAO
+ model files.
+*/
+void vpMbGenericTracker::reInitModel(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, std::string> &mapOfModelFiles,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses,
+ const bool verbose)
+{
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, std::string>::const_iterator it_model = mapOfModelFiles.find(m_referenceCameraName);
+ std::map<std::string, vpHomogeneousMatrix>::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);
+
+ // Set reference pose
+ tracker->getPose(cMo);
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Cannot reInitModel() for reference camera!");
+ }
+
+ std::vector<std::string> vectorOfMissingCameras;
+ 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()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->reInitModel(*it_img->second, it_model->second, it_camPose->second, verbose);
+ } else {
+ vectorOfMissingCameras.push_back(it_tracker->first);
+ }
+ }
+ }
+
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameras.begin(); it != vectorOfMissingCameras.end();
+ ++it) {
+ it_img = mapOfImages.find(*it);
+ it_model = mapOfModelFiles.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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);
+ }
+ }
+
+ modelInitialised = true;
+}
+
+/*!
+ Reset the tracker. The model is removed and the pose is set to identity.
+ The tracker needs to be initialized with a new model and a new pose.
+*/
+void vpMbGenericTracker::resetTracker()
+{
+ cMo.eye();
+
+ useScanLine = false;
+
+#ifdef VISP_HAVE_OGRE
+ useOgre = false;
+#endif
+
+ m_computeInteraction = true;
+ m_lambda = 1.0;
+
+ angleAppears = vpMath::rad(89);
+ angleDisappears = vpMath::rad(89);
+ clippingFlag = vpPolygon3D::NO_CLIPPING;
+ distNearClip = 0.001;
+ distFarClip = 100;
+
+ m_optimizationMethod = vpMbTracker::GAUSS_NEWTON_OPT;
+ m_maxIter = 30;
+ m_stopCriteriaEpsilon = 1e-8;
+ m_initialMu = 0.01;
+
+ // Only for Edge
+ m_percentageGdPt = 0.4;
+
+ // Only for KLT
+ m_thresholdOutlier = 0.5;
+
+ // 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))
+ m_mapOfFeatureFactors[KLT_TRACKER] = 1.0;
+#endif
+
+ m_mapOfFeatureFactors[DEPTH_NORMAL_TRACKER] = 1.0;
+ m_mapOfFeatureFactors[DEPTH_DENSE_TRACKER] = 1.0;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->resetTracker();
+ }
+}
+
+/*!
+ Set the angle used to test polygons appearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value lower than
+ this parameter, the polygon is considered as appearing.
+ The polygon will then be tracked.
+
+ \param a : new angle in radian.
+*/
+void vpMbGenericTracker::setAngleAppear(const double &a)
+{
+ vpMbTracker::setAngleAppear(a);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setAngleAppear(a);
+ }
+}
+
+/*!
+ Set the angle used to test polygons appearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value lower than
+ this parameter, the polygon is considered as appearing.
+ The polygon will then be tracked.
+
+ \param a1 : new angle in radian for the first camera.
+ \param a2 : new angle in radian for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setAngleAppear(const double &a1, const double &a2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setAngleAppear(a1);
+
+ ++it;
+ it->second->setAngleAppear(a2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ angleAppears = it->second->getAngleAppear();
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the angle used to test polygons appearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value lower than
+ this parameter, the polygon is considered as appearing.
+ The polygon will then be tracked.
+
+ \param mapOfAngles : Map of new angles in radian.
+*/
+void vpMbGenericTracker::setAngleAppear(const std::map<std::string, double> &mapOfAngles)
+{
+ for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setAngleAppear(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ angleAppears = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Set the angle used to test polygons disappearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value greater than
+ this parameter, the polygon is considered as disappearing.
+ The tracking of the polygon will then be stopped.
+
+ \param a : new angle in radian.
+*/
+void vpMbGenericTracker::setAngleDisappear(const double &a)
+{
+ vpMbTracker::setAngleDisappear(a);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setAngleDisappear(a);
+ }
+}
+
+/*!
+ Set the angle used to test polygons disappearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value greater than
+ this parameter, the polygon is considered as disappearing.
+ The tracking of the polygon will then be stopped.
+
+ \param a1 : new angle in radian for the first camera.
+ \param a2 : new angle in radian for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setAngleDisappear(const double &a1, const double &a2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setAngleDisappear(a1);
+
+ ++it;
+ it->second->setAngleDisappear(a2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ angleDisappears = it->second->getAngleDisappear();
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the angle used to test polygons disappearance.
+ If the angle between the normal of the polygon and the line going
+ from the camera to the polygon center has a value greater than
+ this parameter, the polygon is considered as disappearing.
+ The tracking of the polygon will then be stopped.
+
+ \param mapOfAngles : Map of new angles in radian.
+*/
+void vpMbGenericTracker::setAngleDisappear(const std::map<std::string, double> &mapOfAngles)
+{
+ for (std::map<std::string, double>::const_iterator it = mapOfAngles.begin(); it != mapOfAngles.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setAngleDisappear(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ angleDisappears = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Set the camera parameters.
+
+ \param camera : the new camera parameters.
+*/
+void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera)
+{
+ vpMbTracker::setCameraParameters(camera);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setCameraParameters(camera);
+ }
+}
+
+/*!
+ Set the camera parameters.
+
+ \param camera1 : the new camera parameters for the first camera.
+ \param camera2 : the new camera parameters for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setCameraParameters(const vpCameraParameters &camera1, const vpCameraParameters &camera2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setCameraParameters(camera1);
+
+ ++it;
+ it->second->setCameraParameters(camera2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ it->second->getCameraParameters(cam);
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the camera parameters.
+
+ \param mapOfCameraParameters : map of new camera parameters.
+
+ \note This function will set the camera parameters only for the supplied
+ camera names.
+*/
+void vpMbGenericTracker::setCameraParameters(const std::map<std::string, vpCameraParameters> &mapOfCameraParameters)
+{
+ for (std::map<std::string, vpCameraParameters>::const_iterator it = mapOfCameraParameters.begin();
+ it != mapOfCameraParameters.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setCameraParameters(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ cam = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Set the camera transformation matrix for the specified camera (\f$
+ _{}^{c_{current}}\textrm{M}_{c_{reference}} \f$).
+
+ \param cameraName : Camera name.
+ \param cameraTransformationMatrix : Camera transformation matrix between the
+ current and the reference camera.
+*/
+void vpMbGenericTracker::setCameraTransformationMatrix(const std::string &cameraName,
+ const vpHomogeneousMatrix &cameraTransformationMatrix)
+{
+ std::map<std::string, vpHomogeneousMatrix>::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());
+ }
+}
+
+/*!
+ Set the map of camera transformation matrices
+ (\f$ _{}^{c_1}\textrm{M}_{c_1}, _{}^{c_2}\textrm{M}_{c_1},
+ _{}^{c_3}\textrm{M}_{c_1}, \cdots, _{}^{c_n}\textrm{M}_{c_1} \f$).
+
+ \param mapOfTransformationMatrix : map of camera transformation matrices.
+*/
+void vpMbGenericTracker::setCameraTransformationMatrix(
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfTransformationMatrix)
+{
+ for (std::map<std::string, vpHomogeneousMatrix>::const_iterator it = mapOfTransformationMatrix.begin();
+ it != mapOfTransformationMatrix.end(); ++it) {
+ std::map<std::string, vpHomogeneousMatrix>::iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(it->first);
+
+ if (it_camTrans != m_mapOfCameraTransformationMatrix.end()) {
+ it_camTrans->second = it->second;
+ }
+ }
+}
+
+/*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param flags : New clipping flags.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setClipping(const unsigned int &flags)
+{
+ vpMbTracker::setClipping(flags);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setClipping(flags);
+ }
+}
+
+/*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param flags1 : New clipping flags for the first camera.
+ \param flags2 : New clipping flags for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setClipping(const unsigned int &flags1, const unsigned int &flags2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setClipping(flags1);
+
+ ++it;
+ it->second->setClipping(flags2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ clippingFlag = it->second->getClipping();
+ } else {
+ 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!";
+ throw vpException(vpTrackingException::fatalError, ss.str());
+ }
+}
+
+/*!
+ Set maximum distance to consider a face.
+ You should use the maximum depth range of the sensor used.
+
+ \param maxDistance : Maximum distance to the face.
+
+ \sa setDepthDenseFilteringMethod
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthDenseFilteringMaxDistance(const double maxDistance)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthDenseFilteringMaxDistance(maxDistance);
+ }
+}
+
+/*!
+ Set method to discard a face, e.g.if outside of the depth range.
+
+ \param method : Depth dense filtering method.
+
+ \sa vpMbtFaceDepthDense::vpDepthDenseFilteringType
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthDenseFilteringMethod(const int method)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthDenseFilteringMethod(method);
+ }
+}
+
+/*!
+ Set minimum distance to consider a face.
+ You should use the minimum depth range of the sensor used.
+
+ \param minDistance : Minimum distance to the face.
+
+ \sa setDepthDenseFilteringMethod
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthDenseFilteringMinDistance(const double minDistance)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthDenseFilteringMinDistance(minDistance);
+ }
+}
+
+/*!
+ Set depth occupancy ratio to consider a face, used to discard faces where
+ the depth map is not well reconstructed.
+
+ \param occupancyRatio : Occupancy ratio, between [0 ; 1].
+
+ \sa setDepthDenseFilteringMethod
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthDenseFilteringOccupancyRatio(const double occupancyRatio)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthDenseFilteringOccupancyRatio(occupancyRatio);
+ }
+}
+
+/*!
+ Set depth dense sampling step.
+
+ \param stepX : Sampling step in x-direction.
+ \param stepY : Sampling step in y-direction.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthDenseSamplingStep(const unsigned int stepX, const unsigned int stepY)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthDenseSamplingStep(stepX, stepY);
+ }
+}
+
+/*!
+ Set method to compute the centroid for display for depth tracker.
+
+ \param method : Centroid computation method.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalFaceCentroidMethod(const vpMbtFaceDepthNormal::vpFaceCentroidType &method)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalFaceCentroidMethod(method);
+ }
+}
+
+/*!
+ Set depth feature estimation method.
+
+ \param method : Depth feature estimation method.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalFeatureEstimationMethod(
+ const vpMbtFaceDepthNormal::vpFeatureEstimationType &method)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalFeatureEstimationMethod(method);
+ }
+}
+
+/*!
+ Set depth PCL plane estimation method.
+
+ \param method : Depth PCL plane estimation method.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalPclPlaneEstimationMethod(const int method)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalPclPlaneEstimationMethod(method);
+ }
+}
+
+/*!
+ Set depth PCL RANSAC maximum number of iterations.
+
+ \param maxIter : Depth PCL RANSAC maximum number of iterations.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacMaxIter(const int maxIter)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalPclPlaneEstimationRansacMaxIter(maxIter);
+ }
+}
+
+/*!
+ Set depth PCL RANSAC threshold.
+
+ \param thresold : Depth PCL RANSAC threshold.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalPclPlaneEstimationRansacThreshold(const double thresold)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalPclPlaneEstimationRansacThreshold(thresold);
+ }
+}
+
+/*!
+ Set depth sampling step.
+
+ \param stepX : Sampling step in x-direction.
+ \param stepY : Sampling step in y-direction.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDepthNormalSamplingStep(const unsigned int stepX, const unsigned int stepY)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDepthNormalSamplingStep(stepX, stepY);
+ }
+}
+
+/*!
+ Specify which clipping to use.
+
+ \sa vpMbtPolygonClipping
+
+ \param mapOfClippingFlags : Map of new clipping flags.
+*/
+void vpMbGenericTracker::setClipping(const std::map<std::string, unsigned int> &mapOfClippingFlags)
+{
+ for (std::map<std::string, unsigned int>::const_iterator it = mapOfClippingFlags.begin();
+ it != mapOfClippingFlags.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setClipping(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ clippingFlag = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Enable to display the features. By features, we meant the moving edges (ME)
+ and the klt points if used.
+
+ Note that if present, the moving edges can be displayed with different
+ colors:
+ - If green : The ME is a good point.
+ - If blue : The ME is removed because of a contrast problem during the
+ tracking phase.
+ - If purple : The ME is removed because of a threshold problem during the
+ tracking phase.
+ - If red : The ME is removed because it is rejected by the robust approach
+ in the virtual visual servoing scheme.
+
+ \param displayF : set it to true to display the features.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setDisplayFeatures(const bool displayF)
+{
+ vpMbTracker::setDisplayFeatures(displayF);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setDisplayFeatures(displayF);
+ }
+}
+
+/*!
+ Set the far distance for clipping.
+
+ \param dist : Far clipping value.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setFarClippingDistance(const double &dist)
+{
+ vpMbTracker::setFarClippingDistance(dist);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setFarClippingDistance(dist);
+ }
+}
+
+/*!
+ Set the far distance for clipping.
+
+ \param dist1 : Far clipping value for the first camera.
+ \param dist2 : Far clipping value for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setFarClippingDistance(const double &dist1, const double &dist2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setFarClippingDistance(dist1);
+
+ ++it;
+ it->second->setFarClippingDistance(dist2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ distFarClip = it->second->getFarClippingDistance();
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the far distance for clipping.
+
+ \param mapOfClippingDists : Map of far clipping values.
+*/
+void vpMbGenericTracker::setFarClippingDistance(const std::map<std::string, double> &mapOfClippingDists)
+{
+ for (std::map<std::string, double>::const_iterator it = mapOfClippingDists.begin(); it != mapOfClippingDists.end();
+ ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setFarClippingDistance(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ distFarClip = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Set the feature factors used in the VVS stage (ponderation between the
+ feature types).
+
+ \param mapOfFeatureFactors : Map of feature factors.
+*/
+void vpMbGenericTracker::setFeatureFactors(const std::map<vpTrackerType, double> &mapOfFeatureFactors)
+{
+ for (std::map<vpTrackerType, double>::iterator it = m_mapOfFeatureFactors.begin(); it != m_mapOfFeatureFactors.end();
+ ++it) {
+ std::map<vpTrackerType, double>::const_iterator it_factor = mapOfFeatureFactors.find(it->first);
+ if (it_factor != mapOfFeatureFactors.end()) {
+ it->second = it_factor->second;
+ }
+ }
+}
+
+/*!
+ Set the threshold value between 0 and 1 over good moving edges ratio. It
+ allows to decide if the tracker has enough valid moving edges to compute a
+ pose. 1 means that all moving edges should be considered as good to have a
+ valid pose, while 0.1 means that 10% of the moving edge are enough to
+ declare a pose valid.
+
+ \param threshold : Value between 0 and 1 that corresponds to the ratio of
+ good moving edges that is necessary to consider that the estimated pose is
+ valid. Default value is 0.4.
+
+ \sa getGoodMovingEdgesRatioThreshold()
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setGoodMovingEdgesRatioThreshold(const double threshold)
+{
+ m_percentageGdPt = threshold;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setGoodMovingEdgesRatioThreshold(threshold);
+ }
+}
+
+#ifdef VISP_HAVE_OGRE
+/*!
+ Set the ratio of visibility attempts that has to be successful to consider a
+ polygon as visible.
+
+ \sa setNbRayCastingAttemptsForVisibility(const unsigned int &)
+
+ \param ratio : Ratio of succesful attempts that has to be considered. Value
+ has to be between 0.0 (0%) and 1.0 (100%).
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setGoodNbRayCastingAttemptsRatio(const double &ratio)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->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 random point
+ inside the considered polygon.
+
+ \sa setGoodNbRayCastingAttemptsRatio(const unsigned int &)
+
+ \param attempts Number of rays to be sent.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setNbRayCastingAttemptsForVisibility(const unsigned int &attempts)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setNbRayCastingAttemptsForVisibility(attempts);
+ }
+}
+#endif
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+/*!
+ Set the new value of the klt tracker.
+
+ \param t : Klt tracker containing the new values.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setKltOpencv(t);
+ }
+}
+
+/*!
+ Set the new value of the klt tracker.
+
+ \param t1 : Klt tracker containing the new values for the first camera.
+ \param t2 : Klt tracker containing the new values for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setKltOpencv(const vpKltOpencv &t1, const vpKltOpencv &t2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the new value of the klt tracker.
+
+ \param mapOfKlts : Map of klt tracker containing the new values.
+*/
+void vpMbGenericTracker::setKltOpencv(const std::map<std::string, vpKltOpencv> &mapOfKlts)
+{
+ for (std::map<std::string, vpKltOpencv>::const_iterator it = mapOfKlts.begin(); it != mapOfKlts.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setKltOpencv(it->second);
+ }
+ }
+}
+
+/*!
+ Set the threshold for the acceptation of a point.
+
+ \param th : Threshold for the weight below which a point is rejected.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setKltThresholdAcceptation(const double th)
+{
+ m_thresholdOutlier = th;
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setKltThresholdAcceptation(th);
+ }
+}
+#endif
+
+/*!
+ Set the flag to consider if the level of detail (LOD) is used.
+
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param name : name of the face we want to modify
+ the LOD parameter.
+
+ \sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setLod(const bool useLod, const std::string &name)
+{
+ for (std::map<std::string, TrackerWrapper *>::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))
+/*!
+ Set the erosion of the mask used on the Model faces.
+
+ \param e : The desired erosion.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setKltMaskBorder(e);
+ }
+}
+
+/*!
+ Set the erosion of the mask used on the Model faces.
+
+ \param e1 : The desired erosion for the first camera.
+ \param e2 : The desired erosion for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setKltMaskBorder(const unsigned int &e1, const unsigned int &e2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the erosion of the mask used on the Model faces.
+
+ \param mapOfErosions : Map of desired erosions.
+*/
+void vpMbGenericTracker::setKltMaskBorder(const std::map<std::string, unsigned int> &mapOfErosions)
+{
+ for (std::map<std::string, unsigned int>::const_iterator it = mapOfErosions.begin(); it != mapOfErosions.end();
+ ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setKltMaskBorder(it->second);
+ }
+ }
+}
+#endif
+
+/*!
+ Set the threshold for the minimum line length to be considered as visible in
+ the LOD case.
+
+ \param minLineLengthThresh : threshold for the minimum line length in pixel.
+ \param name : name of the face we want to modify the LOD threshold.
+
+ \sa setLod(), setMinPolygonAreaThresh()
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setMinLineLengthThresh(minLineLengthThresh, name);
+ }
+}
+
+/*!
+ Set the minimum polygon area to be considered as visible in the LOD case.
+
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
+
+ \sa setLod(), setMinLineLengthThresh()
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setMinPolygonAreaThresh(minPolygonAreaThresh, name);
+ }
+}
+
+/*!
+ Set the moving edge parameters.
+
+ \param me : an instance of vpMe containing all the desired parameters.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setMovingEdge(const vpMe &me)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setMovingEdge(me);
+ }
+}
+
+/*!
+ Set the moving edge parameters.
+
+ \param me1 : an instance of vpMe containing all the desired parameters for
+ the first camera. \param me2 : an instance of vpMe containing all the
+ desired parameters for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setMovingEdge(const vpMe &me1, const vpMe &me2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the moving edge parameters.
+
+ \param mapOfMe : Map of vpMe containing all the desired parameters.
+*/
+void vpMbGenericTracker::setMovingEdge(const std::map<std::string, vpMe> &mapOfMe)
+{
+ for (std::map<std::string, vpMe>::const_iterator it = mapOfMe.begin(); it != mapOfMe.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setMovingEdge(it->second);
+ }
+ }
+}
+
+/*!
+ Set the near distance for clipping.
+
+ \param dist : Near clipping value.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setNearClippingDistance(const double &dist)
+{
+ vpMbTracker::setNearClippingDistance(dist);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setNearClippingDistance(dist);
+ }
+}
+
+/*!
+ Set the near distance for clipping.
+
+ \param dist1 : Near clipping value for the first camera.
+ \param dist2 : Near clipping value for the second camera.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setNearClippingDistance(const double &dist1, const double &dist2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setNearClippingDistance(dist1);
+
+ ++it;
+
+ it->second->setNearClippingDistance(dist2);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ distNearClip = it->second->getNearClippingDistance();
+ } else {
+ std::cerr << "Cannot find the reference camera: " << m_referenceCameraName << "!" << std::endl;
+ }
+ } else {
+ throw vpException(vpTrackingException::fatalError, "Require two cameras! There are %d cameras!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the near distance for clipping.
+
+ \param mapOfDists : Map of near clipping values.
+*/
+void vpMbGenericTracker::setNearClippingDistance(const std::map<std::string, double> &mapOfDists)
+{
+ for (std::map<std::string, double>::const_iterator it = mapOfDists.begin(); it != mapOfDists.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setNearClippingDistance(it->second);
+
+ if (it->first == m_referenceCameraName) {
+ distNearClip = it->second;
+ }
+ }
+ }
+}
+
+/*!
+ Enable/Disable the appearance of Ogre config dialog on startup.
+
+ \warning This method has only effect when Ogre is used and Ogre visibility
+ test is enabled using setOgreVisibilityTest() with true parameter.
+
+ \param showConfigDialog : if true, shows Ogre dialog window (used to set
+ Ogre rendering options) when Ogre visibility is enabled. By default, this
+ functionality is turned off.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setOgreShowConfigDialog(const bool showConfigDialog)
+{
+ vpMbTracker::setOgreShowConfigDialog(showConfigDialog);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setOgreShowConfigDialog(showConfigDialog);
+ }
+}
+
+/*!
+ Use Ogre3D for visibility tests
+
+ \warning This function has to be called before the initialization of the
+ tracker.
+
+ \param v : True to use it, False otherwise
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setOgreVisibilityTest(const bool &v)
+{
+ vpMbTracker::setOgreVisibilityTest(v);
+
+ for (std::map<std::string, TrackerWrapper *>::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<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->faces.getOgreContext()->setWindowName("Multi Generic MBT (" + it->first + ")");
+ }
+#endif
+}
+
+/*!
+ Set the optimization method used during the tracking.
+
+ \param opt : Optimization method to use (see vpMbtOptimizationMethod).
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setOptimizationMethod(const vpMbtOptimizationMethod &opt)
+{
+ vpMbTracker::setOptimizationMethod(opt);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setOptimizationMethod(opt);
+ }
+}
+
+/*!
+ Set the pose to be used in entry (as guess) of the next call to the track()
+ function. This pose will be just used once.
+
+ \warning This functionnality is not available when tracking cylinders with
+ the KLT tracking.
+
+ \param I : image corresponding to the desired pose.
+ \param cdMo : Pose to affect.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setPose(const vpImage<unsigned char> &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!");
+ }
+
+ cMo = cdMo;
+
+ std::map<std::string, TrackerWrapper *>::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!",
+ m_referenceCameraName.c_str());
+ }
+}
+
+/*!
+ Set the pose to be used in entry of the next call to the track() function.
+ This pose will be just used once.
+
+ \param I1 : First image corresponding to the desired pose.
+ \param I2 : Second image corresponding to the desired pose.
+ \param c1Mo : First pose to affect.
+ \param c2Mo : Second pose to affect.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::setPose(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2,
+ const vpHomogeneousMatrix &c1Mo, const vpHomogeneousMatrix &c2Mo)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it->second->setPose(I1, c1Mo);
+
+ ++it;
+
+ it->second->setPose(I2, c2Mo);
+
+ it = m_mapOfTrackers.find(m_referenceCameraName);
+ if (it != m_mapOfTrackers.end()) {
+ // Set reference pose
+ it->second->getPose(cMo);
+ } else {
+ 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!",
+ m_mapOfTrackers.size());
+ }
+}
+
+/*!
+ Set the pose to be used in entry of the next call to the track() function.
+ This pose will be just used once.
+ The camera transformation matrices have to be set before.
+
+ \param mapOfImages : Map of images.
+ \param mapOfCameraPoses : Map of pose to affect to the cameras.
+
+ \note Image and camera pose must be supplied for the reference camera. The
+ images for all the cameras must be supplied to correctly initialize the
+ trackers but some camera poses can be omitted. In this case, they will be
+ initialized using the pose computed from the reference camera pose and using
+ the known geometric transformation between each camera (see
+ setCameraTransformationMatrix()).
+*/
+void vpMbGenericTracker::setPose(const std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ const std::map<std::string, vpHomogeneousMatrix> &mapOfCameraPoses)
+{
+ // Set the reference cMo
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(m_referenceCameraName);
+ std::map<std::string, const vpImage<unsigned char> *>::const_iterator it_img =
+ mapOfImages.find(m_referenceCameraName);
+ std::map<std::string, vpHomogeneousMatrix>::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!");
+ }
+
+ // Vector of missing pose matrices for cameras
+ std::vector<std::string> vectorOfMissingCameraPoses;
+
+ // Set pose for the specified cameras
+ 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()) {
+ // Set pose
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setPose(*it_img->second, it_camPose->second);
+ } else {
+ vectorOfMissingCameraPoses.push_back(it_tracker->first);
+ }
+ }
+ }
+
+ for (std::vector<std::string>::const_iterator it = vectorOfMissingCameraPoses.begin();
+ it != vectorOfMissingCameraPoses.end(); ++it) {
+ it_img = mapOfImages.find(*it);
+ std::map<std::string, vpHomogeneousMatrix>::const_iterator it_camTrans =
+ m_mapOfCameraTransformationMatrix.find(*it);
+
+ 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 {
+ throw vpException(vpTrackingException::fatalError,
+ "Missing image or missing camera transformation "
+ "matrix! Cannot set pose for camera: %s!",
+ it->c_str());
+ }
+ }
+}
+
+/*!
+ Set if the projection error criteria has to be computed. This criteria could
+ be used to detect the quality of the tracking. It computes an angle between
+ 0 and 90 degrees that is available with getProjectionError(). Closer to 0 is
+ the value, better is the tracking.
+
+ \param flag : True if the projection error criteria has to be computed,
+ false otherwise.
+
+ \sa getProjectionError()
+
+ \note Available only if the edge features are used (e.g. Edge tracking or
+ Edge + KLT tracking). Otherwise, the value of 90 degrees will be returned.
+*/
+void vpMbGenericTracker::setProjectionErrorComputation(const bool &flag)
+{
+ vpMbTracker::setProjectionErrorComputation(flag);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setProjectionErrorComputation(flag);
+ }
+}
+
+/*!
+ Set the reference camera name.
+
+ \param referenceCameraName : Name of the reference camera.
+*/
+void vpMbGenericTracker::setReferenceCameraName(const std::string &referenceCameraName)
+{
+ std::map<std::string, TrackerWrapper *>::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!";
+ }
+}
+
+void vpMbGenericTracker::setScanLineVisibilityTest(const bool &v)
+{
+ vpMbTracker::setScanLineVisibilityTest(v);
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setScanLineVisibilityTest(v);
+ }
+}
+
+/*!
+ Set the tracker type.
+
+ \param type : Type of features to used, see vpTrackerType (e.g.
+ vpMbGenericTracker::EDGE_TRACKER or vpMbGenericTracker::EDGE_TRACKER |
+ vpMbGenericTracker::KLT_TRACKER).
+
+ \note This function will set the new parameter for all the cameras.
+
+ \warning This function has to be called before the loading of the CAD model.
+*/
+void vpMbGenericTracker::setTrackerType(const int type)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setTrackerType(type);
+ }
+}
+
+/*!
+ Set the tracker types.
+
+ \param mapOfTrackerTypes : Map of feature types to used, see vpTrackerType
+ (e.g. vpMbGenericTracker::EDGE_TRACKER or vpMbGenericTracker::EDGE_TRACKER |
+ vpMbGenericTracker::KLT_TRACKER).
+
+ \warning This function has to be called before the loading of the CAD model.
+*/
+void vpMbGenericTracker::setTrackerType(const std::map<std::string, int> &mapOfTrackerTypes)
+{
+ for (std::map<std::string, int>::const_iterator it = mapOfTrackerTypes.begin(); it != mapOfTrackerTypes.end(); ++it) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it_tracker = m_mapOfTrackers.find(it->first);
+ if (it_tracker != m_mapOfTrackers.end()) {
+ TrackerWrapper *tracker = it_tracker->second;
+ tracker->setTrackerType(it->second);
+ }
+ }
+}
+
+/*!
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
+
+ \param name : name of the polygon(s).
+ \param useEdgeTracking : True if it has to be considered, False otherwise.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setUseEdgeTracking(const std::string &name, const bool &useEdgeTracking)
+{
+ for (std::map<std::string, TrackerWrapper *>::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))
+/*!
+ Set if the polygons that have the given name have to be considered during
+ the tracking phase.
+
+ \param name : name of the polygon(s).
+ \param useKltTracking : True if it has to be considered, False otherwise.
+
+ \note This function will set the new parameter for all the cameras.
+*/
+void vpMbGenericTracker::setUseKltTracking(const std::string &name, const bool &useKltTracking)
+{
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ tracker->setUseKltTracking(name, useKltTracking);
+ }
+}
+#endif
+
+void vpMbGenericTracker::testTracking()
+{
+ // Test tracking fails only if all testTracking have failed
+ bool isOneTestTrackingOk = false;
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+ try {
+ tracker->testTracking();
+ isOneTestTrackingOk = true;
+ } catch (...) {
+ }
+ }
+
+ if (!isOneTestTrackingOk) {
+ std::ostringstream oss;
+ oss << "Not enough moving edges to track the object. Try to reduce the "
+ "threshold="
+ << m_percentageGdPt << " using setGoodMovingEdgesRatioThreshold()";
+ throw vpTrackingException(vpTrackingException::fatalError, oss.str());
+ }
+}
+
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param I : The current image.
+
+ \note This function will track only for the reference camera.
+*/
+void vpMbGenericTracker::track(const vpImage<unsigned char> &I)
+{
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ mapOfImages[m_referenceCameraName] = &I;
+
+ std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
+ std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
+
+ track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
+}
+
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param I1 : The first image.
+ \param I2 : The second image.
+
+ \note This function assumes a stereo configuration of the generic tracker.
+*/
+void vpMbGenericTracker::track(const vpImage<unsigned char> &I1, const vpImage<unsigned char> &I2)
+{
+ if (m_mapOfTrackers.size() == 2) {
+ std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
+ mapOfImages[it->first] = &I1;
+ ++it;
+
+ mapOfImages[it->first] = &I2;
+
+ std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
+ std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
+
+ track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
+ } else {
+ std::stringstream ss;
+ ss << "Require two cameras! There are " << m_mapOfTrackers.size() << " cameras!";
+ throw vpException(vpTrackingException::fatalError, ss.str().c_str());
+ }
+}
+
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param mapOfImages : Map of images.
+*/
+void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages)
+{
+ std::map<std::string, const std::vector<vpColVector> *> mapOfPointClouds;
+ std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
+
+ track(mapOfImages, mapOfPointClouds, mapOfWidths, mapOfHeights);
+}
+
+#ifdef VISP_HAVE_PCL
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param mapOfImages : Map of images.
+ \param mapOfPointClouds : Map of PCL pointclouds.
+*/
+void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, pcl::PointCloud<pcl::PointXYZ>::ConstPtr> &mapOfPointClouds)
+{
+ for (std::map<std::string, TrackerWrapper *>::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 |
+#endif
+ 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))
+ | KLT_TRACKER
+#endif
+ ) &&
+ mapOfImages[it->first] == NULL) {
+ throw vpException(vpException::fatalError, "Image pointer is NULL!");
+ }
+
+ if (tracker->m_trackerType & (DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER) &&
+ mapOfPointClouds[it->first] == nullptr) {
+ throw vpException(vpException::fatalError, "Pointcloud smart pointer is NULL!");
+ }
+ }
+
+ preTracking(mapOfImages, mapOfPointClouds);
+
+ try {
+ computeVVS(mapOfImages);
+ } catch (...) {
+ covarianceMatrix = -1;
+ throw; // throw the original exception
+ }
+
+ testTracking();
+
+ for (std::map<std::string, TrackerWrapper *>::const_iterator it = m_mapOfTrackers.begin();
+ it != m_mapOfTrackers.end(); ++it) {
+ TrackerWrapper *tracker = it->second;
+
+ tracker->postTracking(mapOfImages[it->first], mapOfPointClouds[it->first]);
+ }
+
+ computeProjectionError();
+}
+#endif
+
+/*!
+ Realize the tracking of the object in the image.
+
+ \throw vpException : if the tracking is supposed to have failed
+
+ \param mapOfImages : Map of images.
+ \param mapOfPointClouds : Map of pointclouds.
+ \param mapOfPointCloudWidths : Map of pointcloud widths.
+ \param mapOfPointCloudHeights : Map of pointcloud heights.
+*/
+void vpMbGenericTracker::track(std::map<std::string, const vpImage<unsigned char> *> &mapOfImages,
+ std::map<std::string, const std::vector<vpColVector> *> &mapOfPointClouds,
+ std::map<std::string, unsigned int> &mapOfPointCloudWidths,
+ std::map<std::string, unsigned int> &mapOfPointCloudHeights)
+{
+ for (std::map<std::string, TrackerWrapper *>::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 |
+#endif
+ 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))
+ | KLT_TRACKER
+#endif
+ ) &&
+ mapOfImages[it->first] == NULL) {
+ throw vpException(vpException::fatalError, "Image pointer is NULL!");
+ }
+
+ 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);
+
+ try {
+ computeVVS(mapOfImages);
+ } catch (...) {
+ covarianceMatrix = -1;
+ throw; // throw the original exception
+ }
+
+ testTracking();
+
+ for (std::map<std::string, TrackerWrapper *>::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]);
+ }
+
+ computeProjectionError();
+}
+
+/** TrackerWrapper **/
+vpMbGenericTracker::TrackerWrapper::TrackerWrapper()
+ : m_error(), m_L(), m_trackerType(EDGE_TRACKER), m_w(), m_weightedError()
+{
+ m_lambda = 1.0;
+ m_maxIter = 30;
+
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
+#endif
+}
+
+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))
+ KLT_TRACKER |
+#endif
+ DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
+ throw vpException(vpTrackingException::badValue, "Bad value for tracker type: %d!", m_trackerType);
+ }
+
+ m_lambda = 1.0;
+ m_maxIter = 30;
+
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("MBT TrackerWrapper");
+#endif
+}
+
+vpMbGenericTracker::TrackerWrapper::~TrackerWrapper() {}
+
+// Implemented only for debugging purposes: use TrackerWrapper as a standalone
+// tracker
+void vpMbGenericTracker::TrackerWrapper::computeVVS(const vpImage<unsigned char> *const ptr_I)
+{
+ computeVVSInit(ptr_I);
+
+ if (m_error.getRows() < 4) {
+ throw vpTrackingException(vpTrackingException::notEnoughPointError, "Error: not enough features");
+ }
+
+ double normRes = 0;
+ double normRes_1 = -1;
+ unsigned int iter = 0;
+
+ double factorEdge = 1.0;
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ double factorKlt = 1.0;
+#endif
+ double factorDepth = 1.0;
+ double factorDepthDense = 1.0;
+
+ vpMatrix LTL;
+ vpColVector LTR, v;
+ vpColVector error_prev;
+
+ double mu = m_initialMu;
+ vpHomogeneousMatrix cMo_prev;
+#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;
+
+ // Covariance
+ vpColVector W_true(m_error.getRows());
+ 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))
+ 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)) {
+ computeVVSInteractionMatrixAndResidu(ptr_I);
+
+ bool reStartFromLastIncrement = false;
+ 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 (reStartFromLastIncrement) {
+ if (m_trackerType & KLT_TRACKER) {
+ ctTc0 = ctTc0_Prev;
+ }
+ }
+#endif
+
+ if (!reStartFromLastIncrement) {
+ computeVVSWeights();
+
+ if (computeCovariance) {
+ L_true = m_L;
+ if (!isoJoIdentity_) {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ LVJ_true = (m_L * cVo * oJo);
+ }
+ }
+
+ vpVelocityTwistMatrix cVo;
+ if (iter == 0) {
+ isoJoIdentity_ = true;
+ oJo.eye();
+
+ // If all the 6 dof should be estimated, we check if the interaction
+ // matrix is full rank. If not we remove automatically the dof that
+ // cannot be estimated This is particularly useful when consering
+ // circles (rank 5) and cylinders (rank 4)
+ if (isoJoIdentity_) {
+ cVo.buildFrom(cMo);
+
+ vpMatrix K; // kernel
+ unsigned int rank = (m_L * cVo).kernel(K);
+ if (rank == 0) {
+ throw vpException(vpException::fatalError, "Rank=0, cannot estimate the pose !");
+ }
+
+ if (rank != 6) {
+ vpMatrix I; // Identity
+ I.eye(6);
+ oJo = I - K.AtA();
+
+ isoJoIdentity_ = false;
+ }
+ }
+ }
+
+ // Weighting
+ double num = 0;
+ double den = 0;
+
+ unsigned int start_index = 0;
+ if (m_trackerType & EDGE_TRACKER) {
+ for (unsigned int i = 0; i < nb_edge_features; i++) {
+ double wi = m_w_edge[i] * m_factor[i] * factorEdge;
+ W_true[i] = wi;
+ m_weightedError[i] = wi * m_error[i];
+
+ num += wi * vpMath::sqr(m_error[i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[i][j] *= wi;
+ }
+ }
+
+ start_index += nb_edge_features;
+ }
+
+#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;
+ W_true[start_index + i] = wi;
+ m_weightedError[start_index + i] = wi * m_error_klt[i];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += nb_klt_features;
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ for (unsigned int i = 0; i < nb_depth_features; i++) {
+ double wi = m_w_depthNormal[i] * factorDepth;
+ m_w[start_index + i] = m_w_depthNormal[i];
+ m_weightedError[start_index + i] = wi * m_error[start_index + i];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ start_index += nb_depth_features;
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ for (unsigned int i = 0; i < nb_depth_dense_features; i++) {
+ double wi = m_w_depthDense[i] * factorDepthDense;
+ m_w[start_index + i] = m_w_depthDense[i];
+ m_weightedError[start_index + i] = wi * m_error[start_index + i];
+
+ num += wi * vpMath::sqr(m_error[start_index + i]);
+ den += wi;
+
+ for (unsigned int j = 0; j < m_L.getCols(); j++) {
+ m_L[start_index + i][j] *= wi;
+ }
+ }
+
+ // start_index += nb_depth_dense_features;
+ }
+
+ 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 (m_trackerType & KLT_TRACKER) {
+ ctTc0_Prev = ctTc0;
+ }
+#endif
+
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+
+#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;
+ }
+#endif
+ normRes_1 = normRes;
+
+ normRes = sqrt(num / den);
+ }
+
+ iter++;
+ }
+
+ computeCovarianceMatrixVVS(isoJoIdentity_, W_true, cMo_prev, L_true, LVJ_true, m_error);
+
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::updateMovingEdgeWeights();
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::computeVVSInit()
+{
+ throw vpException(vpException::fatalError, "vpMbGenericTracker::"
+ "TrackerWrapper::computeVVSInit("
+ ") should not be called!");
+}
+
+void vpMbGenericTracker::TrackerWrapper::computeVVSInit(const vpImage<unsigned char> *const ptr_I)
+{
+ initMbtTracking(ptr_I);
+
+ unsigned int nbFeatures = 0;
+
+ if (m_trackerType & EDGE_TRACKER) {
+ nbFeatures += m_error_edge.getRows();
+ } else {
+ m_error_edge.clear();
+ m_weightedError_edge.clear();
+ m_L_edge.clear();
+ m_w_edge.clear();
+ }
+
+#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();
+ } else {
+ m_error_klt.clear();
+ m_weightedError_klt.clear();
+ m_L_klt.clear();
+ m_w_klt.clear();
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::computeVVSInit();
+ nbFeatures += m_error_depthNormal.getRows();
+ } else {
+ m_error_depthNormal.clear();
+ m_weightedError_depthNormal.clear();
+ m_L_depthNormal.clear();
+ m_w_depthNormal.clear();
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ vpMbDepthDenseTracker::computeVVSInit();
+ nbFeatures += m_error_depthDense.getRows();
+ } else {
+ m_error_depthDense.clear();
+ m_weightedError_depthDense.clear();
+ m_L_depthDense.clear();
+ m_w_depthDense.clear();
+ }
+
+ m_L.resize(nbFeatures, 6, false, false);
+ m_error.resize(nbFeatures, false);
+
+ m_weightedError.resize(nbFeatures, false);
+ m_w.resize(nbFeatures, false);
+ m_w = 1;
+}
+
+void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu()
+{
+ throw vpException(vpException::fatalError, "vpMbGenericTracker::"
+ "TrackerWrapper::"
+ "computeVVSInteractionMatrixAndR"
+ "esidu() should not be called!");
+}
+
+void vpMbGenericTracker::TrackerWrapper::computeVVSInteractionMatrixAndResidu(const vpImage<unsigned char> *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 (m_trackerType & KLT_TRACKER) {
+ vpMbKltTracker::computeVVSInteractionMatrixAndResidu();
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::computeVVSInteractionMatrixAndResidu();
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ vpMbDepthDenseTracker::computeVVSInteractionMatrixAndResidu();
+ }
+
+ unsigned int start_index = 0;
+ if (m_trackerType & EDGE_TRACKER) {
+ m_L.insert(m_L_edge, start_index, 0);
+ m_error.insert(start_index, m_error_edge);
+
+ start_index += m_error_edge.getRows();
+ }
+
+#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);
+
+ start_index += m_error_klt.getRows();
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ m_L.insert(m_L_depthNormal, start_index, 0);
+ m_error.insert(start_index, m_error_depthNormal);
+
+ start_index += m_error_depthNormal.getRows();
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ m_L.insert(m_L_depthDense, start_index, 0);
+ m_error.insert(start_index, m_error_depthDense);
+
+ // start_index += m_error_depthDense.getRows();
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::computeVVSWeights()
+{
+ unsigned int start_index = 0;
+
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::computeVVSWeights();
+ m_w.insert(start_index, m_w_edge);
+
+ start_index += m_w_edge.getRows();
+ }
+
+#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);
+
+ start_index += m_w_klt.getRows();
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ if (m_depthNormalUseRobust) {
+ vpMbTracker::computeVVSWeights(m_robust_depthNormal, m_error_depthNormal, m_w_depthNormal);
+ m_w.insert(start_index, m_w_depthNormal);
+ }
+
+ start_index += m_w_depthNormal.getRows();
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ vpMbDepthDenseTracker::computeVVSWeights();
+ m_w.insert(start_index, m_w_depthDense);
+
+ // start_index += m_w_depthDense.getRows();
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::display(const vpImage<unsigned char> &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))
+ } else if (m_trackerType == KLT_TRACKER) {
+ vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+#endif
+ } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
+ 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<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
+ it != lines[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ break; // display model on one scale only
+ }
+ }
+ }
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ if (m_trackerType & KLT_TRACKER) {
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
+ ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
+ }
+ }
+
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ vpMbDepthDenseTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if (useOgre)
+ faces.displayOgre(cMo_);
+#endif
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::display(const vpImage<vpRGBa> &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))
+ } else if (m_trackerType == KLT_TRACKER) {
+ vpMbKltTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+#endif
+ } else if (m_trackerType == DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ } else if (m_trackerType == DEPTH_DENSE_TRACKER) {
+ 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<vpMbtDistanceLine *>::const_iterator it = lines[scaleLevel].begin();
+ it != lines[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[scaleLevel].begin();
+ it != cylinders[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[scaleLevel].begin();
+ it != circles[scaleLevel].end(); ++it) {
+ (*it)->display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ break; // display model on one scale only
+ }
+ }
+ }
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ if (m_trackerType & KLT_TRACKER) {
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end();
+ ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (displayFeatures && kltpoly->hasEnoughPoints() && kltpoly->isTracked() && kltpoly->polygon->isVisible()) {
+ kltpoly->displayPrimitive(I);
+ }
+ }
+
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (displayFeatures && kltPolyCylinder->isTracked() && kltPolyCylinder->hasEnoughPoints())
+ kltPolyCylinder->displayPrimitive(I);
+ }
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ vpMbDepthNormalTracker::display(I, cMo_, camera, col, thickness, displayFullModel);
+ }
+
+#ifdef VISP_HAVE_OGRE
+ if (useOgre)
+ faces.displayOgre(cMo_);
+#endif
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::init(const vpImage<unsigned char> &I)
+{
+ if (!modelInitialised) {
+ throw vpException(vpException::fatalError, "model not initialized");
+ }
+
+ if (useScanLine || clippingFlag > 3)
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ bool reInitialisation = false;
+ if (!useOgre) {
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+ } else {
+#ifdef VISP_HAVE_OGRE
+ if (!faces.isOgreInitialised()) {
+ faces.setBackgroundSizeOgre(I.getHeight(), I.getWidth());
+
+ faces.setOgreShowConfigDialog(ogreShowConfigDialog);
+ faces.initOgre(cam);
+ // Turn off Ogre config dialog display for the next call to this
+ // function since settings are saved in the ogre.cfg file and used
+ // during the next call
+ ogreShowConfigDialog = false;
+ }
+
+ faces.setVisibleOgre(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#else
+ faces.setVisible(I, cam, cMo, angleAppears, angleDisappears, reInitialisation);
+#endif
+ }
+
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ if (m_trackerType & KLT_TRACKER)
+ vpMbKltTracker::reinit(I);
+#endif
+
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::resetMovingEdge();
+
+ bool a = false;
+ 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);
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ 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)
+{
+ 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)
+{
+ 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 (m_trackerType & KLT_TRACKER)
+ vpMbKltTracker::initCylinder(p1, p2, radius, idFace, name);
+#endif
+}
+
+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 (m_trackerType & KLT_TRACKER)
+ vpMbKltTracker::initFaceFromCorners(polygon);
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER)
+ vpMbDepthNormalTracker::initFaceFromCorners(polygon);
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ vpMbDepthDenseTracker::initFaceFromCorners(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 (m_trackerType & KLT_TRACKER)
+ vpMbKltTracker::initFaceFromLines(polygon);
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER)
+ vpMbDepthNormalTracker::initFaceFromLines(polygon);
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ vpMbDepthDenseTracker::initFaceFromLines(polygon);
+}
+
+void vpMbGenericTracker::TrackerWrapper::initMbtTracking(const vpImage<unsigned char> *const ptr_I)
+{
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::computeVVSInit();
+ vpMbEdgeTracker::computeVVSFirstPhaseFactor(*ptr_I, 0);
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::loadConfigFile(const std::string &configFile)
+{
+#ifdef VISP_HAVE_XML2
+ vpMbtXmlGenericParser xmlp((vpMbtXmlGenericParser::vpParserType)m_trackerType);
+
+ xmlp.setCameraParameters(cam);
+ xmlp.setAngleAppear(vpMath::deg(angleAppears));
+ xmlp.setAngleDisappear(vpMath::deg(angleDisappears));
+
+ // Edge
+ xmlp.setEdgeMe(me);
+
+ // KLT
+ xmlp.setKltMaxFeatures(10000);
+ xmlp.setKltWindowSize(5);
+ xmlp.setKltQuality(0.01);
+ xmlp.setKltMinDistance(5);
+ 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))
+ xmlp.setKltMaskBorder(maskBorder);
+#endif
+
+ // Depth normal
+ xmlp.setDepthNormalFeatureEstimationMethod(m_depthNormalFeatureEstimationMethod);
+ xmlp.setDepthNormalPclPlaneEstimationMethod(m_depthNormalPclPlaneEstimationMethod);
+ xmlp.setDepthNormalPclPlaneEstimationRansacMaxIter(m_depthNormalPclPlaneEstimationRansacMaxIter);
+ xmlp.setDepthNormalPclPlaneEstimationRansacThreshold(m_depthNormalPclPlaneEstimationRansacThreshold);
+ xmlp.setDepthNormalSamplingStepX(m_depthNormalSamplingStepX);
+ xmlp.setDepthNormalSamplingStepY(m_depthNormalSamplingStepY);
+
+ // Depth dense
+ xmlp.setDepthDenseSamplingStepX(m_depthDenseSamplingStepX);
+ xmlp.setDepthDenseSamplingStepY(m_depthDenseSamplingStepY);
+
+ try {
+
+ std::cout << " *********** Parsing XML for";
+
+ std::vector<std::string> 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 (m_trackerType & KLT_TRACKER)
+ tracker_names.push_back("Klt");
+#endif
+ if (m_trackerType & DEPTH_NORMAL_TRACKER)
+ tracker_names.push_back("Depth Normal");
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ tracker_names.push_back("Depth Dense");
+
+ for (size_t i = 0; i < tracker_names.size(); i++) {
+ std::cout << " " << tracker_names[i];
+ if (i == tracker_names.size() - 1) {
+ std::cout << " ";
+ }
+ }
+
+ 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());
+ }
+
+ vpCameraParameters camera;
+ xmlp.getCameraParameters(camera);
+ setCameraParameters(camera);
+
+ angleAppears = vpMath::rad(xmlp.getAngleAppear());
+ angleDisappears = vpMath::rad(xmlp.getAngleDisappear());
+
+ if (xmlp.hasNearClippingDistance())
+ setNearClippingDistance(xmlp.getNearClippingDistance());
+
+ if (xmlp.hasFarClippingDistance())
+ setFarClippingDistance(xmlp.getFarClippingDistance());
+
+ if (xmlp.getFovClipping()) {
+ setClipping(vpMbEdgeTracker::clippingFlag | vpPolygon3D::FOV_CLIPPING);
+ }
+
+ useLodGeneral = xmlp.getLodState();
+ minLineLengthThresholdGeneral = xmlp.getLodMinLineLengthThreshold();
+ minPolygonAreaThresholdGeneral = xmlp.getLodMinPolygonAreaThreshold();
+
+ applyLodSettingInConfig = false;
+ if (this->getNbPolygon() > 0) {
+ applyLodSettingInConfig = true;
+ setLod(useLodGeneral);
+ setMinLineLengthThresh(minLineLengthThresholdGeneral);
+ setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+ }
+
+ // Edge
+ vpMe meParser;
+ xmlp.getEdgeMe(meParser);
+ vpMbEdgeTracker::setMovingEdge(meParser);
+
+// KLT
+#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());
+ tracker.setMinDistance(xmlp.getKltMinDistance());
+ tracker.setHarrisFreeParameter(xmlp.getKltHarrisParam());
+ tracker.setBlockSize((int)xmlp.getKltBlockSize());
+ tracker.setPyramidLevels((int)xmlp.getKltPyramidLevels());
+ maskBorder = xmlp.getKltMaskBorder();
+
+ // if(useScanLine)
+ faces.getMbScanLineRenderer().setMaskBorder(maskBorder);
+#endif
+
+ // Depth normal
+ setDepthNormalFeatureEstimationMethod(xmlp.getDepthNormalFeatureEstimationMethod());
+ setDepthNormalPclPlaneEstimationMethod(xmlp.getDepthNormalPclPlaneEstimationMethod());
+ setDepthNormalPclPlaneEstimationRansacMaxIter(xmlp.getDepthNormalPclPlaneEstimationRansacMaxIter());
+ setDepthNormalPclPlaneEstimationRansacThreshold(xmlp.getDepthNormalPclPlaneEstimationRansacThreshold());
+ setDepthNormalSamplingStep(xmlp.getDepthNormalSamplingStepX(), xmlp.getDepthNormalSamplingStepY());
+
+ // Depth dense
+ setDepthDenseSamplingStep(xmlp.getDepthDenseSamplingStepX(), xmlp.getDepthDenseSamplingStepY());
+#else
+ 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<unsigned char> *const ptr_I,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ if (displayFeatures) {
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::displayFeaturesOnImage(*ptr_I, 0);
+ }
+ }
+
+#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)) {
+ vpMbKltTracker::reinit(*ptr_I);
+ }
+ }
+#endif
+
+ // Looking for new visible face
+ if (m_trackerType & EDGE_TRACKER) {
+ bool newvisibleface = false;
+ vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);
+
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
+ }
+ }
+
+ // Depth normal
+ if (m_trackerType & DEPTH_NORMAL_TRACKER)
+ vpMbDepthNormalTracker::computeVisibility(point_cloud->width, point_cloud->height);
+
+ // Depth dense
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ vpMbDepthDenseTracker::computeVisibility(point_cloud->width, point_cloud->height);
+
+ // Edge
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::updateMovingEdge(*ptr_I);
+
+ vpMbEdgeTracker::initMovingEdge(*ptr_I, cMo);
+ // Reinit the moving edge for the lines which need it.
+ vpMbEdgeTracker::reinitMovingEdge(*ptr_I, cMo);
+
+ if (computeProjError) {
+ vpMbEdgeTracker::computeProjectionError(*ptr_I);
+ }
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ if (m_trackerType & EDGE_TRACKER) {
+ try {
+ vpMbEdgeTracker::trackMovingEdge(*ptr_I);
+ } catch (...) {
+ std::cerr << "Error in moving edge tracking" << std::endl;
+ throw;
+ }
+ }
+
+#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);
+ } catch (vpException &e) {
+ std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
+ throw;
+ }
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ try {
+ vpMbDepthNormalTracker::segmentPointCloud(point_cloud);
+ } catch (...) {
+ std::cerr << "Error in Depth normal tracking" << std::endl;
+ throw;
+ }
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ try {
+ vpMbDepthDenseTracker::segmentPointCloud(point_cloud);
+ } catch (...) {
+ std::cerr << "Error in Depth dense tracking" << std::endl;
+ throw;
+ }
+ }
+}
+#endif
+
+void vpMbGenericTracker::TrackerWrapper::postTracking(const vpImage<unsigned char> *const ptr_I,
+ const unsigned int pointcloud_width,
+ const unsigned int pointcloud_height)
+{
+ if (displayFeatures) {
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::displayFeaturesOnImage(*ptr_I, 0);
+ }
+ }
+
+#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)) {
+ vpMbKltTracker::reinit(*ptr_I);
+ }
+ }
+#endif
+
+ // Looking for new visible face
+ if (m_trackerType & EDGE_TRACKER) {
+ bool newvisibleface = false;
+ vpMbEdgeTracker::visibleFace(*ptr_I, cMo, newvisibleface);
+
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, ptr_I->getWidth(), ptr_I->getHeight());
+ }
+ }
+
+ // Depth normal
+ if (m_trackerType & DEPTH_NORMAL_TRACKER)
+ vpMbDepthNormalTracker::computeVisibility(pointcloud_width, pointcloud_height);
+
+ // Depth dense
+ if (m_trackerType & DEPTH_DENSE_TRACKER)
+ vpMbDepthDenseTracker::computeVisibility(pointcloud_width, pointcloud_height);
+
+ // Edge
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::updateMovingEdge(*ptr_I);
+
+ vpMbEdgeTracker::initMovingEdge(*ptr_I, cMo);
+ // Reinit the moving edge for the lines which need it.
+ vpMbEdgeTracker::reinitMovingEdge(*ptr_I, cMo);
+
+ if (computeProjError) {
+ vpMbEdgeTracker::computeProjectionError(*ptr_I);
+ }
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::preTracking(const vpImage<unsigned char> *const ptr_I,
+ const std::vector<vpColVector> *const point_cloud,
+ const unsigned int pointcloud_width,
+ const unsigned int pointcloud_height)
+{
+ if (m_trackerType & EDGE_TRACKER) {
+ try {
+ vpMbEdgeTracker::trackMovingEdge(*ptr_I);
+ } catch (...) {
+ std::cerr << "Error in moving edge tracking" << std::endl;
+ throw;
+ }
+ }
+
+#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);
+ } catch (vpException &e) {
+ std::cerr << "Error in KLT tracking: " << e.what() << std::endl;
+ throw;
+ }
+ }
+#endif
+
+ if (m_trackerType & DEPTH_NORMAL_TRACKER) {
+ try {
+ vpMbDepthNormalTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
+ } catch (...) {
+ std::cerr << "Error in Depth tracking" << std::endl;
+ throw;
+ }
+ }
+
+ if (m_trackerType & DEPTH_DENSE_TRACKER) {
+ try {
+ vpMbDepthDenseTracker::segmentPointCloud(*point_cloud, pointcloud_width, pointcloud_height);
+ } catch (...) {
+ std::cerr << "Error in Depth dense tracking" << std::endl;
+ throw;
+ }
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::reInitModel(const vpImage<unsigned char> &I, const std::string &cad_name,
+ const vpHomogeneousMatrix &cMo_, const bool verbose)
+{
+ cMo.eye();
+
+ // Edge
+ vpMbtDistanceLine *l;
+ vpMbtDistanceCylinder *cy;
+ vpMbtDistanceCircle *ci;
+
+ for (unsigned int i = 0; i < scales.size(); i++) {
+ if (scales[i]) {
+ for (std::list<vpMbtDistanceLine *>::const_iterator it = lines[i].begin(); it != lines[i].end(); ++it) {
+ l = *it;
+ if (l != NULL)
+ delete l;
+ l = NULL;
+ }
+
+ for (std::list<vpMbtDistanceCylinder *>::const_iterator it = cylinders[i].begin(); it != cylinders[i].end();
+ ++it) {
+ cy = *it;
+ if (cy != NULL)
+ delete cy;
+ cy = NULL;
+ }
+
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles[i].begin(); it != circles[i].end(); ++it) {
+ ci = *it;
+ if (ci != NULL)
+ delete ci;
+ ci = NULL;
+ }
+
+ lines[i].clear();
+ cylinders[i].clear();
+ circles[i].clear();
+ }
+ }
+
+ nline = 0;
+ ncylinder = 0;
+ ncircle = 0;
+ nbvisiblepolygone = 0;
+
+// KLT
+#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);
+ cur = NULL;
+ }
+#endif
+
+ // delete the Klt Polygon features
+ for (std::list<vpMbtDistanceKltPoints *>::const_iterator it = kltPolygons.begin(); it != kltPolygons.end(); ++it) {
+ vpMbtDistanceKltPoints *kltpoly = *it;
+ if (kltpoly != NULL) {
+ delete kltpoly;
+ }
+ kltpoly = NULL;
+ }
+ kltPolygons.clear();
+
+ for (std::list<vpMbtDistanceKltCylinder *>::const_iterator it = kltCylinders.begin(); it != kltCylinders.end();
+ ++it) {
+ vpMbtDistanceKltCylinder *kltPolyCylinder = *it;
+ if (kltPolyCylinder != NULL) {
+ delete kltPolyCylinder;
+ }
+ kltPolyCylinder = NULL;
+ }
+ kltCylinders.clear();
+
+ // delete the structures used to display circles
+ for (std::list<vpMbtDistanceCircle *>::const_iterator it = circles_disp.begin(); it != circles_disp.end(); ++it) {
+ ci = *it;
+ if (ci != NULL) {
+ delete ci;
+ }
+ ci = NULL;
+ }
+ circles_disp.clear();
+
+ firstInitialisation = true;
+
+#endif
+
+ // Depth normal
+ for (size_t i = 0; i < m_depthNormalFaces.size(); i++) {
+ delete m_depthNormalFaces[i];
+ m_depthNormalFaces[i] = NULL;
+ }
+ m_depthNormalFaces.clear();
+
+ // Depth dense
+ for (size_t i = 0; i < m_depthDenseNormalFaces.size(); i++) {
+ delete m_depthDenseNormalFaces[i];
+ m_depthDenseNormalFaces[i] = NULL;
+ }
+ m_depthDenseNormalFaces.clear();
+
+ faces.reset();
+
+ loadModel(cad_name, verbose);
+ initFromPose(I, cMo_);
+}
+
+void vpMbGenericTracker::TrackerWrapper::resetTracker()
+{
+ vpMbEdgeTracker::resetTracker();
+#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)
+{
+ this->cam = camera;
+
+ vpMbEdgeTracker::setCameraParameters(cam);
+#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::setFarClippingDistance(const double &dist)
+{
+ vpMbEdgeTracker::setFarClippingDistance(dist);
+}
+
+void vpMbGenericTracker::TrackerWrapper::setNearClippingDistance(const double &dist)
+{
+ vpMbEdgeTracker::setNearClippingDistance(dist);
+}
+
+void vpMbGenericTracker::TrackerWrapper::setOgreVisibilityTest(const bool &v)
+{
+ vpMbTracker::setOgreVisibilityTest(v);
+#ifdef VISP_HAVE_OGRE
+ faces.getOgreContext()->setWindowName("TrackerWrapper");
+#endif
+}
+
+void vpMbGenericTracker::TrackerWrapper::setPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cdMo)
+{
+ bool performKltSetPose = false;
+
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ if (m_trackerType & KLT_TRACKER) {
+ performKltSetPose = true;
+
+ if (useScanLine || clippingFlag > 3)
+ cam.computeFov(I.getWidth(), I.getHeight());
+
+ vpMbKltTracker::setPose(I, cdMo);
+ }
+#endif
+
+ if (!performKltSetPose) {
+ cMo = cdMo;
+ init(I);
+ return;
+ }
+
+ if (m_trackerType & EDGE_TRACKER)
+ resetMovingEdge();
+
+ if (useScanLine) {
+ faces.computeClippedPolygons(cMo, cam);
+ faces.computeScanLineRender(cam, I.getWidth(), I.getHeight());
+ }
+
+#if 0
+ if (m_trackerType & EDGE_TRACKER) {
+ initPyramid(I, Ipyramid);
+
+ unsigned int i = (unsigned int) scales.size();
+ do {
+ i--;
+ if(scales[i]){
+ downScale(i);
+ initMovingEdge(*Ipyramid[i], cMo);
+ upScale(i);
+ }
+ } while(i != 0);
+
+ cleanPyramid(Ipyramid);
+ }
+#else
+ if (m_trackerType & EDGE_TRACKER)
+ initMovingEdge(I, cMo);
+#endif
+
+ // Depth normal
+ vpMbDepthNormalTracker::computeVisibility(I.getWidth(), I.getHeight());
+
+ // Depth dense
+ vpMbDepthDenseTracker::computeVisibility(I.getWidth(), I.getHeight());
+}
+
+void vpMbGenericTracker::TrackerWrapper::setProjectionErrorComputation(const bool &flag)
+{
+ vpMbEdgeTracker::setProjectionErrorComputation(flag);
+}
+
+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))
+ vpMbKltTracker::setScanLineVisibilityTest(v);
+#endif
+ vpMbDepthNormalTracker::setScanLineVisibilityTest(v);
+ vpMbDepthDenseTracker::setScanLineVisibilityTest(v);
+}
+
+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))
+ KLT_TRACKER |
+#endif
+ DEPTH_NORMAL_TRACKER | DEPTH_DENSE_TRACKER)) == 0) {
+ throw vpException(vpTrackingException::badValue, "bad value for tracker type: !", type);
+ }
+
+ m_trackerType = type;
+}
+
+void vpMbGenericTracker::TrackerWrapper::testTracking()
+{
+ if (m_trackerType & EDGE_TRACKER) {
+ vpMbEdgeTracker::testTracking();
+ }
+}
+
+void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> &
+#ifdef VISP_HAVE_PCL
+ I
+#endif
+)
+{
+ if ((m_trackerType & (EDGE_TRACKER
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ | KLT_TRACKER
+#endif
+ )) == 0) {
+ std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
+ return;
+ }
+
+#ifdef VISP_HAVE_PCL
+ track(&I, nullptr);
+#endif
+}
+
+#ifdef VISP_HAVE_PCL
+void vpMbGenericTracker::TrackerWrapper::track(const vpImage<unsigned char> *const ptr_I,
+ const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &point_cloud)
+{
+ if ((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) {
+ std::cerr << "Bad tracker type: " << m_trackerType << std::endl;
+ return;
+ }
+
+ if (m_trackerType & (EDGE_TRACKER
+#if defined(VISP_HAVE_MODULE_KLT) && (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ | KLT_TRACKER
+#endif
+ ) &&
+ ptr_I == NULL) {
+ 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!");
+ }
+
+ // Back-up cMo in case of exception
+ vpHomogeneousMatrix cMo_1 = cMo;
+ try {
+ preTracking(ptr_I, point_cloud);
+
+ try {
+ computeVVS(ptr_I);
+ } catch (...) {
+ covarianceMatrix = -1;
+ throw; // throw the original exception
+ }
+
+ if (m_trackerType == EDGE_TRACKER)
+ testTracking();
+
+ postTracking(ptr_I, point_cloud);
+
+ } catch (const vpException &e) {
+ std::cerr << "Exception: " << e.what() << std::endl;
+ cMo = cMo_1;
+ throw; // rethrowing the original exception
+ }
+}
+#endif
diff --git a/modules/tracker/mbt/src/vpMbScanLine.cpp b/modules/tracker/mbt/src/vpMbScanLine.cpp
index b2adc00..828bf5c 100644
--- a/modules/tracker/mbt/src/vpMbScanLine.cpp
+++ b/modules/tracker/mbt/src/vpMbScanLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,51 +39,53 @@
#include <visp3/core/vpConfig.h>
#if defined _MSC_VER && _MSC_VER >= 1200
-# define NOMINMAX
+#define NOMINMAX
#endif
-#include <cmath>
#include <algorithm>
+#include <cmath>
#include <cstddef>
#include <iostream>
#include <utility>
-#include <visp3/mbt/vpMbScanLine.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/mbt/vpMbScanLine.h>
#if defined(DEBUG_DISP)
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#endif
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
vpMbScanLine::vpMbScanLine()
- : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(),
- visibility_samples(), depthTreshold(1e-06)
+ : w(0), h(0), K(), maskBorder(0), mask(), primitive_ids(), visibility_samples(), depthTreshold(1e-06)
#if defined(DEBUG_DISP)
- ,dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
+ ,
+ dispMaskDebug(NULL), dispLineDebug(NULL), linedebugImg()
#endif
{
#if defined(VISP_HAVE_X11) && defined(DEBUG_DISP)
- dispLineDebug = new vpDisplayX();
- dispMaskDebug = new vpDisplayX();
+ dispLineDebug = new vpDisplayX();
+ dispMaskDebug = new vpDisplayX();
#elif defined(VISP_HAVE_GDI) && defined(DEBUG_DISP)
- dispLineDebug = new vpDisplayGDI();
- dispMaskDebug = new vpDisplayGDI();
+ dispLineDebug = new vpDisplayGDI();
+ dispMaskDebug = new vpDisplayGDI();
#endif
}
vpMbScanLine::~vpMbScanLine()
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
- if (dispLineDebug != NULL) delete dispLineDebug;
- if (dispMaskDebug != NULL) delete dispMaskDebug;
+ if (dispLineDebug != NULL)
+ delete dispLineDebug;
+ if (dispMaskDebug != NULL)
+ delete dispMaskDebug;
#endif
}
/*!
- Compute the intersections between Y-axis scanlines and a given line (two points polygon).
+ Compute the intersections between Y-axis scanlines and a given line (two
+ points polygon).
\param a : First point of the line.
\param b : Second point of the line.
@@ -90,11 +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<std::vector<vpMbScanLineSegment> > &scanlines)
+void vpMbScanLine::drawLineY(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
double x0 = a[0] / a[2];
double y0 = a[1] / a[2];
@@ -102,40 +102,39 @@ void vpMbScanLine::drawLineY(const vpColVector &a,
double x1 = b[0] / b[2];
double y1 = b[1] / b[2];
double z1 = b[2];
- if (y0 > y1)
- {
+ if (y0 > y1) {
std::swap(x0, x1);
std::swap(y0, y1);
std::swap(z0, z1);
}
- //if (y0 >= h - 1 || y1 < 0 || y1 == y0)
+ // if (y0 >= h - 1 || y1 < 0 || y1 == y0)
if (y0 >= h - 1 || y1 < 0 || std::fabs(y1 - y0) <= std::numeric_limits<double>::epsilon())
- return;
+ return;
- const unsigned int _y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(y0)));
- const double _y1 = std::min<double>(h, y1);
+ 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));
- for(unsigned int y = _y0 ; y < _y1 ; ++y)
- {
- const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
- const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
- vpMbScanLineSegment s;
- s.p = x;
- s.type = POINT;
- s.Z2 = s.Z1 = mix(z0, z1, alpha);
- s.P2 = s.P1 = s.p * s.Z1;
- s.ID = ID;
- s.edge = edge;
- s.b_sample_Y = b_sample_Y;
- scanlines[y].push_back(s);
+ for (unsigned int y = _y0; y < _y1; ++y) {
+ const double x = x0 + (x1 - x0) * (y - y0) / (y1 - y0);
+ const double alpha = getAlpha(y, y0 * z0, z0, y1 * z1, z1);
+ vpMbScanLineSegment s;
+ s.p = x;
+ s.type = POINT;
+ s.Z2 = s.Z1 = mix(z0, z1, alpha);
+ s.P2 = s.P1 = s.p * s.Z1;
+ s.ID = ID;
+ s.edge = edge;
+ s.b_sample_Y = b_sample_Y;
+ scanlines[y].push_back(s);
}
}
/*!
- Compute the intersections between X-axis scanlines and a given line (two points polygon).
+ Compute the intersections between X-axis scanlines and a given line (two
+ points polygon).
\param a : First point of the line.
\param b : Second point of the line.
@@ -143,11 +142,8 @@ void vpMbScanLine::drawLineY(const vpColVector &a,
\param ID : Id of the given line (has to be know when using queries).
\param scanlines : Resulting intersections.
*/
-void vpMbScanLine::drawLineX(const vpColVector &a,
- const vpColVector &b,
- const vpMbScanLineEdge &edge,
- const int ID,
- std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+void vpMbScanLine::drawLineX(const vpColVector &a, const vpColVector &b, const vpMbScanLineEdge &edge, const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
double x0 = a[0] / a[2];
double y0 = a[1] / a[2];
@@ -155,39 +151,36 @@ void vpMbScanLine::drawLineX(const vpColVector &a,
double x1 = b[0] / b[2];
double y1 = b[1] / b[2];
double z1 = b[2];
- if (x0 > x1)
- {
+ if (x0 > x1) {
std::swap(x0, x1);
std::swap(y0, y1);
std::swap(z0, z1);
}
- //if (x0 >= w - 1 || x1 < 0 || x1 == x0)
+ // if (x0 >= w - 1 || x1 < 0 || x1 == x0)
if (x0 >= w - 1 || x1 < 0 || std::fabs(x1 - x0) <= std::numeric_limits<double>::epsilon())
- return;
+ return;
- const unsigned int _x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(x0)));
- const double _x1 = std::min<double>(w, x1);
+ 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));
- for(unsigned int x = _x0 ; x < _x1 ; ++x)
- {
- const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
- const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
- vpMbScanLineSegment s;
- s.p = y;
- s.type = POINT;
- s.Z2 = s.Z1 = mix(z0, z1, alpha);
- s.P2 = s.P1 = s.p * s.Z1;
- s.ID = ID;
- s.edge = edge;
- s.b_sample_Y = b_sample_Y;
- scanlines[x].push_back(s);
+ for (unsigned int x = _x0; x < _x1; ++x) {
+ const double y = y0 + (y1 - y0) * (x - x0) / (x1 - x0);
+ const double alpha = getAlpha(x, x0 * z0, z0, x1 * z1, z1);
+ vpMbScanLineSegment s;
+ s.p = y;
+ s.type = POINT;
+ s.Z2 = s.Z1 = mix(z0, z1, alpha);
+ s.P2 = s.P1 = s.p * s.Z1;
+ s.ID = ID;
+ s.edge = edge;
+ s.b_sample_Y = b_sample_Y;
+ scanlines[x].push_back(s);
}
}
-
/*!
Compute the Y-axis scanlines intersections of a polygon.
@@ -195,41 +188,34 @@ void vpMbScanLine::drawLineX(const vpColVector &a,
\param ID : ID of the polygon (has to be know when using queries).
\param scanlines : Resulting intersections.
*/
-void
-vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
- const int ID,
- std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+void vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
if (polygon.size() < 2)
return;
- if (polygon.size() == 2)
- {
+ if (polygon.size() == 2) {
vpColVector p1, p2;
createVectorFromPoint(polygon.front().first, p1, K);
createVectorFromPoint(polygon.back().first, p2, K);
- drawLineY(p1,
- p2,
- makeMbScanLineEdge(polygon.front().first, polygon.back().first),
- ID,
- scanlines);
+ drawLineY(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
return;
}
std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
local_scanlines.resize(h);
- for(size_t i = 0 ; i < polygon.size() ; ++i)
- {
+ for (size_t i = 0; i < polygon.size(); ++i) {
vpColVector p1, p2;
createVectorFromPoint(polygon[i].first, p1, K);
createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
- drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
+ drawLineY(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
+ local_scanlines);
}
- createScanLinesFromLocals(scanlines,local_scanlines,h);
+ createScanLinesFromLocals(scanlines, local_scanlines, h);
}
/*!
@@ -239,41 +225,34 @@ vpMbScanLine::drawPolygonY(const std::vector<std::pair<vpPoint, unsigned int> >
\param ID : ID of the polygon (has to be know when using queries).
\param scanlines : Resulting intersections.
*/
-void
-vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon,
- const int ID,
- std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
+void vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> > &polygon, const int ID,
+ std::vector<std::vector<vpMbScanLineSegment> > &scanlines)
{
if (polygon.size() < 2)
- return;
+ return;
- if (polygon.size() == 2)
- {
+ if (polygon.size() == 2) {
vpColVector p1, p2;
createVectorFromPoint(polygon.front().first, p1, K);
createVectorFromPoint(polygon.back().first, p2, K);
- drawLineX(p1,
- p2,
- makeMbScanLineEdge(polygon.front().first, polygon.back().first),
- ID,
- scanlines);
+ drawLineX(p1, p2, makeMbScanLineEdge(polygon.front().first, polygon.back().first), ID, scanlines);
return;
}
std::vector<std::vector<vpMbScanLineSegment> > local_scanlines;
local_scanlines.resize(w);
- for(size_t i = 0 ; i < polygon.size() ; ++i)
- {
+ for (size_t i = 0; i < polygon.size(); ++i) {
vpColVector p1, p2;
createVectorFromPoint(polygon[i].first, p1, K);
createVectorFromPoint(polygon[(i + 1) % polygon.size()].first, p2, K);
- drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID, local_scanlines);
+ drawLineX(p1, p2, makeMbScanLineEdge(polygon[i].first, polygon[(i + 1) % polygon.size()].first), ID,
+ local_scanlines);
}
- createScanLinesFromLocals(scanlines,local_scanlines,w);
+ createScanLinesFromLocals(scanlines, local_scanlines, w);
}
/*!
@@ -285,55 +264,49 @@ vpMbScanLine::drawPolygonX(const std::vector<std::pair<vpPoint, unsigned int> >
\param localScanlines : Local scanline vector (X or Y-axis).
\param size : Corresponding size (typically the width or the height).
*/
-void
-vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
- std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
- const unsigned int &size)
+void vpMbScanLine::createScanLinesFromLocals(std::vector<std::vector<vpMbScanLineSegment> > &scanlines,
+ std::vector<std::vector<vpMbScanLineSegment> > &localScanlines,
+ const unsigned int &size)
{
- for(unsigned int j = 0 ; j < size ; ++j)
- {
- std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
- sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator()); // Not sure its necessary
-
- bool b_start = true;
- for(size_t i = 0 ; i < scanline.size() ; ++i)
- {
- vpMbScanLineSegment s = scanline[i];
- if (b_start)
- {
- s.type = START;
- s.P1 = s.p * s.Z1;
- b_start = false;
- }
- else
- {
- vpMbScanLineSegment &prev = scanlines[j].back();
- s.type = END;
- s.P1 = prev.P1;
- s.Z1 = prev.Z1;
- s.P2 = s.p * s.Z2;
- prev.P2 = s.P2;
- prev.Z2 = s.Z2;
- b_start = true;
- }
- scanlines[j].push_back(s);
+ for (unsigned int j = 0; j < size; ++j) {
+ std::vector<vpMbScanLineSegment> &scanline = localScanlines[j];
+ sort(scanline.begin(), scanline.end(),
+ vpMbScanLineSegmentComparator()); // Not sure its necessary
+
+ bool b_start = true;
+ for (size_t i = 0; i < scanline.size(); ++i) {
+ vpMbScanLineSegment s = scanline[i];
+ if (b_start) {
+ s.type = START;
+ s.P1 = s.p * s.Z1;
+ b_start = false;
+ } else {
+ vpMbScanLineSegment &prev = scanlines[j].back();
+ s.type = END;
+ s.P1 = prev.P1;
+ s.Z1 = prev.Z1;
+ s.P2 = s.p * s.Z2;
+ prev.P2 = s.P2;
+ prev.Z2 = s.Z2;
+ b_start = true;
}
+ scanlines[j].push_back(s);
+ }
}
}
/*!
- Render a scene of polygons and compute scanlines intersections in order to use queries.
+ Render a scene of polygons and compute scanlines intersections in order to
+ use queries.
\param polygons : List of polygons composed by arrays of lines.
- \param listPolyIndices : List of polygons IDs (has to be know when using queries).
- \param cam : Camera parameters.
- \param width : Width of the image (render window).
- \param height : Height of the image (render window).
+ \param listPolyIndices : List of polygons IDs (has to be know when using
+ queries). \param cam : Camera parameters. \param width : Width of the image
+ (render window). \param height : Height of the image (render window).
*/
-void
-vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > * > &polygons,
- std::vector<int> listPolyIndices,
- const vpCameraParameters &cam, unsigned int width, unsigned int height)
+void vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigned int> > *> &polygons,
+ std::vector<int> listPolyIndices, const vpCameraParameters &cam, unsigned int width,
+ unsigned int height)
{
this->w = width;
this->h = height;
@@ -346,236 +319,215 @@ vpMbScanLine::drawScene(const std::vector<std::vector<std::pair<vpPoint, unsigne
std::vector<std::vector<vpMbScanLineSegment> > scanlinesX;
scanlinesX.resize(w);
- mask.resize(h,w,0);
+ mask.resize(h, w, 0);
- vpImage<unsigned char> maskY(h,w,0);
- vpImage<unsigned char> maskX(h,w,0);
+ vpImage<unsigned char> maskY(h, w, 0);
+ vpImage<unsigned char> maskX(h, w, 0);
primitive_ids.resize(h, w, -1);
- for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
- {
- drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
- drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
+ for (unsigned int ID = 0; ID < polygons.size(); ++ID) {
+ drawPolygonY(*(polygons[ID]), listPolyIndices[ID], scanlinesY);
+ drawPolygonX(*(polygons[ID]), listPolyIndices[ID], scanlinesX);
}
// Y
int last_ID = -1;
vpMbScanLineSegment last_visible;
- for(unsigned int y = 0 ; y < scanlinesY.size() ; ++y)
- {
- std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
- sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+ for (unsigned int y = 0; y < scanlinesY.size(); ++y) {
+ std::vector<vpMbScanLineSegment> &scanline = scanlinesY[y];
+ sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+
+ std::vector<std::pair<double, vpMbScanLineSegment> > stack;
+ for (size_t i = 0; i < scanline.size(); ++i) {
+ const vpMbScanLineSegment &s = scanline[i];
+
+ switch (s.type) {
+ case START:
+ stack.push_back(std::make_pair(s.Z1, s));
+ break;
+ case END:
+ for (size_t j = 0; j < stack.size(); ++j)
+ if (stack[j].second.ID == s.ID) {
+ if (j != stack.size() - 1)
+ stack[j] = stack.back();
+ stack.pop_back();
+ break;
+ }
+ break;
+ case POINT:
+ break;
+ }
+
+ for (size_t j = 0; j < stack.size(); ++j) {
+ const vpMbScanLineSegment &s0 = stack[j].second;
+ stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
+ }
+ sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
- std::vector<std::pair<double, vpMbScanLineSegment> > stack;
- for(size_t i = 0 ; i < scanline.size() ; ++i)
- {
- const vpMbScanLineSegment &s = scanline[i];
+ int new_ID = stack.empty() ? -1 : stack.front().second.ID;
- switch(s.type)
- {
+ if (new_ID != last_ID || s.type == POINT) {
+ if (s.b_sample_Y)
+ switch (s.type) {
+ case POINT:
+ if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
+ visibility_samples[s.edge].insert((int)y);
+ break;
case START:
- stack.push_back(std::make_pair(s.Z1, s));
- break;
+ if (new_ID == s.ID)
+ visibility_samples[s.edge].insert((int)y);
+ break;
case END:
- for(size_t j = 0 ; j < stack.size() ; ++j)
- if (stack[j].second.ID == s.ID)
- {
- stack[j] = stack.back();
- stack.pop_back();
- break;
- }
- break;
- case POINT:
- break;
+ if (last_ID == s.ID)
+ visibility_samples[s.edge].insert((int)y);
+ break;
}
- for(size_t j = 0 ; j < stack.size() ; ++j)
- {
- const vpMbScanLineSegment &s0 = stack[j].second;
- stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
- }
- sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
-
- int new_ID = stack.empty() ? -1 : stack.front().second.ID;
-
- if (new_ID != last_ID || s.type == POINT)
- {
- if (s.b_sample_Y)
- switch(s.type)
- {
- case POINT:
- if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
- visibility_samples[s.edge].insert((int)y);
- break;
- case START:
- if (new_ID == s.ID)
- visibility_samples[s.edge].insert((int)y);
- break;
- case END:
- if (last_ID == s.ID)
- visibility_samples[s.edge].insert((int)y);
- break;
- }
-
- // This part will only be used for MbKltTracking
- if (last_ID != -1)
- {
- const unsigned int x0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
- const double x1 = std::min<double>(w, s.p);
- for(unsigned int x = x0 + maskBorder ; x < x1 - maskBorder; ++x)
- {
- primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
-
- if(maskBorder != 0)
- maskY[(unsigned int)y][(unsigned int)x] = 255;
- else
- mask[(unsigned int)y][(unsigned int)x] = 255;
- }
- }
-
- last_ID = new_ID;
- if (!stack.empty())
- {
- last_visible = stack.front().second;
- last_visible.p = s.p;
- }
+ // This part will only be used for MbKltTracking
+ if (last_ID != -1) {
+ const unsigned int x0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p)));
+ const double x1 = (std::min)((double)w, (double)s.p);
+ for (unsigned int x = x0 + maskBorder; x < x1 - maskBorder; ++x) {
+ primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
+
+ if (maskBorder != 0)
+ maskY[(unsigned int)y][(unsigned int)x] = 255;
+ else
+ mask[(unsigned int)y][(unsigned int)x] = 255;
}
+ }
+
+ last_ID = new_ID;
+ if (!stack.empty()) {
+ last_visible = stack.front().second;
+ last_visible.p = s.p;
+ }
}
+ }
}
// X
last_ID = -1;
- for(unsigned int x = 0 ; x < scanlinesX.size() ; ++x)
- {
- std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
- sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+ for (unsigned int x = 0; x < scanlinesX.size(); ++x) {
+ std::vector<vpMbScanLineSegment> &scanline = scanlinesX[x];
+ sort(scanline.begin(), scanline.end(), vpMbScanLineSegmentComparator());
+
+ std::vector<std::pair<double, vpMbScanLineSegment> > stack;
+ for (size_t i = 0; i < scanline.size(); ++i) {
+ const vpMbScanLineSegment &s = scanline[i];
+
+ switch (s.type) {
+ case START:
+ stack.push_back(std::make_pair(s.Z1, s));
+ break;
+ case END:
+ for (size_t j = 0; j < stack.size(); ++j)
+ if (stack[j].second.ID == s.ID) {
+ if (j != stack.size() - 1)
+ stack[j] = stack.back();
+ stack.pop_back();
+ break;
+ }
+ break;
+ case POINT:
+ break;
+ }
+
+ for (size_t j = 0; j < stack.size(); ++j) {
+ const vpMbScanLineSegment &s0 = stack[j].second;
+ stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
+ }
+ sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
- std::vector<std::pair<double, vpMbScanLineSegment> > stack;
- for(size_t i = 0 ; i < scanline.size() ; ++i)
- {
- const vpMbScanLineSegment &s = scanline[i];
+ int new_ID = stack.empty() ? -1 : stack.front().second.ID;
- switch(s.type)
- {
+ if (new_ID != last_ID || s.type == POINT) {
+ if (!s.b_sample_Y)
+ switch (s.type) {
+ case POINT:
+ if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
+ visibility_samples[s.edge].insert((int)x);
+ break;
case START:
- stack.push_back(std::make_pair(s.Z1, s));
- break;
+ if (new_ID == s.ID)
+ visibility_samples[s.edge].insert((int)x);
+ break;
case END:
- for(size_t j = 0 ; j < stack.size() ; ++j)
- if (stack[j].second.ID == s.ID)
- {
- stack[j] = stack.back();
- stack.pop_back();
- break;
- }
- break;
- case POINT:
- break;
+ if (last_ID == s.ID)
+ visibility_samples[s.edge].insert((int)x);
+ break;
}
- for(size_t j = 0 ; j < stack.size() ; ++j)
- {
- const vpMbScanLineSegment &s0 = stack[j].second;
- stack[j].first = mix(s0.Z1, s0.Z2, getAlpha(s.type == POINT ? s.p : (s.p + 0.5), s0.P1, s0.Z1, s0.P2, s0.Z2));
- }
- sort(stack.begin(), stack.end(), vpMbScanLineSegmentComparator());
-
- int new_ID = stack.empty() ? -1 : stack.front().second.ID;
-
- if (new_ID != last_ID || s.type == POINT)
- {
- if (!s.b_sample_Y)
- switch(s.type)
- {
- case POINT:
- if (new_ID == -1 || s.Z1 - depthTreshold <= stack.front().first)
- visibility_samples[s.edge].insert((int)x);
- break;
- case START:
- if (new_ID == s.ID)
- visibility_samples[s.edge].insert((int)x);
- break;
- case END:
- if (last_ID == s.ID)
- visibility_samples[s.edge].insert((int)x);
- break;
- }
-
- // This part will only be used for MbKltTracking
- if (maskBorder != 0 && last_ID != -1)
- {
- const unsigned int y0 = std::max<unsigned int>(0, (unsigned int)(std::ceil(last_visible.p)));
- const double y1 = std::min<double>(h, s.p);
- for(unsigned int y = y0 + maskBorder ; y < y1 - maskBorder; ++y)
- {
- //primitive_ids[(unsigned int)y][(unsigned int)x] = last_visible.ID;
- maskX[(unsigned int)y][(unsigned int)x] = 255;
- }
- }
-
- last_ID = new_ID;
- if (!stack.empty())
- {
- last_visible = stack.front().second;
- last_visible.p = s.p;
- }
+ // This part will only be used for MbKltTracking
+ if (maskBorder != 0 && last_ID != -1) {
+ const unsigned int y0 = (std::max)((unsigned int)0, (unsigned int)(std::ceil(last_visible.p)));
+ const double y1 = (std::min)((double)h, (double)s.p);
+ for (unsigned int y = y0 + maskBorder; y < y1 - maskBorder; ++y) {
+ // primitive_ids[(unsigned int)y][(unsigned int)x] =
+ // last_visible.ID;
+ maskX[(unsigned int)y][(unsigned int)x] = 255;
}
+ }
+
+ last_ID = new_ID;
+ if (!stack.empty()) {
+ last_visible = stack.front().second;
+ last_visible.p = s.p;
+ }
}
+ }
}
- if(maskBorder != 0)
- for(unsigned int i = 0 ; i < h ; i++)
- for(unsigned int j = 0 ; j < w ; j++)
- if(maskX[i][j] == 255 && maskY[i][j] == 255)
+ if (maskBorder != 0)
+ for (unsigned int i = 0; i < h; i++)
+ for (unsigned int j = 0; j < w; j++)
+ if (maskX[i][j] == 255 && maskY[i][j] == 255)
mask[i][j] = 255;
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
- if(!dispMaskDebug->isInitialised()){
+ if (!dispMaskDebug->isInitialised()) {
dispMaskDebug->init(mask, 800, 600);
}
vpDisplay::display(mask);
- for(unsigned int ID = 0 ; ID < polygons.size() ; ++ID)
- {
- for(unsigned int i = 0 ; i < polygons[ID]->size() ; i++){
+ for (unsigned int ID = 0; ID < polygons.size(); ++ID) {
+ for (unsigned int i = 0; i < polygons[ID]->size(); i++) {
vpPoint p1 = (*(polygons[ID]))[i].first;
- vpPoint p2 = (*(polygons[ID]))[(i+1)%polygons[ID]->size()].first;
- double i1=0,j1=0,i2=0,j2=0;
+ vpPoint p2 = (*(polygons[ID]))[(i + 1) % polygons[ID]->size()].first;
+ double i1 = 0, j1 = 0, i2 = 0, j2 = 0;
p1.project();
p2.project();
- vpMeterPixelConversion::convertPoint(K,p1.get_x(), p1.get_y(),j1,i1);
- vpMeterPixelConversion::convertPoint(K,p2.get_x(), p2.get_y(),j2,i2);
+ vpMeterPixelConversion::convertPoint(K, p1.get_x(), p1.get_y(), j1, i1);
+ vpMeterPixelConversion::convertPoint(K, p2.get_x(), p2.get_y(), j2, i2);
- vpDisplay::displayLine(mask,i1,j1,i2,j2,vpColor::red,3);
+ vpDisplay::displayLine(mask, i1, j1, i2, j2, vpColor::red, 3);
}
}
vpDisplay::flush(mask);
-
- if(!dispLineDebug->isInitialised()){
- linedebugImg.resize(h,w,0);
+ if (!dispLineDebug->isInitialised()) {
+ linedebugImg.resize(h, w, 0);
dispLineDebug->init(linedebugImg, 800, 100);
}
vpDisplay::display(linedebugImg);
#endif
-
}
/*!
- Test the visibility of a line. As a result, a subsampled line of the given one with all its visible parts.
+ Test the visibility of a line. As a result, a subsampled line of the given
+ one with all its visible parts.
\param a : First point of the line.
\param b : Second point of the line.
- \param lines : List of lines corresponding of the visible parts of the given line.
- \param displayResults : True if the results have to be displayed. False otherwise.
+ \param lines : List of lines corresponding of the visible parts of the given
+ line. \param displayResults : True if the results have to be displayed.
+ False otherwise.
*/
-void
-vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
- std::vector<std::pair<vpPoint, vpPoint> > &lines,
- const bool &displayResults)
+void vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
+ std::vector<std::pair<vpPoint, vpPoint> > &lines, const bool &displayResults)
{
vpColVector _a, _b;
createVectorFromPoint(a, _a, K);
@@ -591,23 +543,24 @@ vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
vpMbScanLineEdge edge = makeMbScanLineEdge(a, b);
lines.clear();
- if(displayResults){
+ if (displayResults) {
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
vpPoint a_(a), b_(b);
a_.project();
b_.project();
- vpMeterPixelConversion::convertPoint(K,a_.get_x(), a_.get_y(),j1,i1);
- vpMeterPixelConversion::convertPoint(K,b_.get_x(), b_.get_y(),j2,i2);
+ vpMeterPixelConversion::convertPoint(K, a_.get_x(), a_.get_y(), j1, i1);
+ vpMeterPixelConversion::convertPoint(K, b_.get_x(), b_.get_y(), j2, i2);
- vpDisplay::displayLine(linedebugImg,i1,j1,i2,j2,vpColor::yellow,3);
+ vpDisplay::displayLine(linedebugImg, i1, j1, i2, j2, vpColor::yellow, 3);
#endif
}
if (!visibility_samples.count(edge))
- return;
+ return;
- // Initialized as the biggest difference between the two points is on the X-axis
+ // Initialized as the biggest difference between the two points is on the
+ // X-axis
double *v0(&x0), *w0(&z0);
double *v1(&x1), *w1(&z1);
unsigned int size(w);
@@ -624,82 +577,73 @@ vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
vpPoint a_;
vpPoint b_;
- if (*v0 > *v1)
- {
- std::swap(v0, v1);
- std::swap(w0, w1);
- //std::swap(a, b);
- // Cannot call swap(a,b) since a and b are const
- // Instead of swap we set the right address of the corresponding pointers
- a_ = b;
- b_ = a;
- }
- else {
+ if (*v0 > *v1) {
+ std::swap(v0, v1);
+ std::swap(w0, w1);
+ // std::swap(a, b);
+ // Cannot call swap(a,b) since a and b are const
+ // Instead of swap we set the right address of the corresponding pointers
+ a_ = b;
+ b_ = a;
+ } else {
a_ = a;
b_ = b;
}
- //if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
+ // if (*v0 >= size - 1 || *v1 < 0 || *v1 == *v0)
if (*v0 >= size - 1 || *v1 < 0 || std::fabs(*v1 - *v0) <= std::numeric_limits<double>::epsilon())
- return;
+ return;
- const int _v0 = std::max(0, int(std::ceil(*v0)));
- const int _v1 = std::min<int>((int)(size - 1), (int)(std::ceil(*v1) - 1));
+ const int _v0 = (std::max)(0, int(std::ceil(*v0)));
+ const int _v1 = (std::min)((int)(size - 1), (int)(std::ceil(*v1) - 1));
const std::set<int> &visible_samples = visibility_samples[edge];
int last = _v0;
vpPoint line_start;
vpPoint line_end;
bool b_line_started = false;
- for(std::set<int>::const_iterator it = visible_samples.begin() ; it != visible_samples.end() ; ++it)
- {
- const int v = *it;
- const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
- //const vpPoint p = mix(a, b, alpha);
- const vpPoint p = mix(a_, b_, alpha);
- if (last + 1 != v)
- {
- if(b_line_started)
- lines.push_back(std::make_pair(line_start, line_end));
- b_line_started = false;
- }
- if (v == _v0)
- {
- //line_start = a;
- line_start = a_;
- line_end = p;
- b_line_started = true;
- }
- else if (v == _v1)
- {
- //line_end = b;
- line_end = b_;
- if (!b_line_started)
- line_start = p;
- b_line_started = true;
- }
- else
- {
- line_end = p;
- if (!b_line_started)
- line_start = p;
- b_line_started = true;
- }
- last = v;
+ for (std::set<int>::const_iterator it = visible_samples.begin(); it != visible_samples.end(); ++it) {
+ const int v = *it;
+ const double alpha = getAlpha(v, (*v0) * (*w0), (*w0), (*v1) * (*w1), (*w1));
+ // const vpPoint p = mix(a, b, alpha);
+ const vpPoint p = mix(a_, b_, alpha);
+ if (last + 1 != v) {
+ if (b_line_started)
+ lines.push_back(std::make_pair(line_start, line_end));
+ b_line_started = false;
+ }
+ if (v == _v0) {
+ // line_start = a;
+ line_start = a_;
+ line_end = p;
+ b_line_started = true;
+ } else if (v == _v1) {
+ // line_end = b;
+ line_end = b_;
+ if (!b_line_started)
+ line_start = p;
+ b_line_started = true;
+ } else {
+ line_end = p;
+ if (!b_line_started)
+ line_start = p;
+ b_line_started = true;
+ }
+ last = v;
}
if (b_line_started)
- lines.push_back(std::make_pair(line_start, line_end));
+ lines.push_back(std::make_pair(line_start, line_end));
- if(displayResults){
+ if (displayResults) {
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(DEBUG_DISP)
double i1(0.0), j1(0.0), i2(0.0), j2(0.0);
- for(unsigned int i = 0 ; i < lines.size() ; i++){
+ 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);
+ vpDisplay::displayLine(linedebugImg, i1, j1, i2, j2, vpColor::red, 3);
}
vpDisplay::flush(linedebugImg);
#endif
@@ -714,8 +658,7 @@ vpMbScanLine::queryLineVisibility(const vpPoint &a, const vpPoint &b,
\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);
@@ -729,13 +672,11 @@ vpMbScanLine::makeMbScanLineEdge(const vpPoint &a, const vpPoint &b)
_b[2] = std::ceil((b.get_Z() * 1e8) * 1e-6);
bool b_comp = false;
- for(unsigned int i = 0 ; i < 3 ; ++i)
- if (_a[i] < _b[i])
- {
+ for (unsigned int i = 0; i < 3; ++i)
+ if (_a[i] < _b[i]) {
b_comp = true;
break;
- }
- else if(_a[i] > _b[i])
+ } else if (_a[i] > _b[i])
break;
if (b_comp)
@@ -751,14 +692,13 @@ 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);
+ v = vpColVector(3);
- v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
- v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
- v[2] = p.get_Z();
+ v[0] = p.get_X() * K.get_px() + K.get_u0() * p.get_Z();
+ v[1] = p.get_Y() * K.get_py() + K.get_v0() * p.get_Z();
+ v[2] = p.get_Z();
}
/*!
@@ -770,8 +710,7 @@ vpMbScanLine::createVectorFromPoint(const vpPoint &p, vpColVector &v, const vpCa
\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);
@@ -779,8 +718,8 @@ vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
if (vpMath::isNaN(alpha) || vpMath::isInf(alpha))
return 0.0;
- alpha = std::min(1.0,alpha);
- alpha = std::max(0.0,alpha);
+ alpha = (std::min)(1.0, alpha);
+ alpha = (std::max)(0.0, alpha);
return alpha;
}
@@ -793,11 +732,7 @@ vpMbScanLine::getAlpha(double x, double X0, double Z0, double X1, double Z1)
\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.
@@ -808,13 +743,12 @@ vpMbScanLine::mix(double a, double b, double alpha)
\return Interpolated vpPoint.
*/
-vpPoint
-vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
+vpPoint vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
{
vpPoint res;
- res.set_X(a.get_X() + ( b.get_X() - a.get_X() ) * alpha);
- res.set_Y(a.get_Y() + ( b.get_Y() - a.get_Y() ) * alpha);
- res.set_Z(a.get_Z() + ( b.get_Z() - a.get_Z() ) * alpha);
+ res.set_X(a.get_X() + (b.get_X() - a.get_X()) * alpha);
+ res.set_Y(a.get_Y() + (b.get_Y() - a.get_Y()) * alpha);
+ res.set_Z(a.get_Z() + (b.get_Z() - a.get_Z()) * alpha);
return res;
}
@@ -827,10 +761,10 @@ vpMbScanLine::mix(const vpPoint &a, const vpPoint &b, double alpha)
\return Resulting norm.
*/
-double
-vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
+double vpMbScanLine::norm(const vpPoint &a, const vpPoint &b)
{
- return sqrt(vpMath::sqr(a.get_X()-b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) + vpMath::sqr(a.get_Z() - b.get_Z()));
+ return sqrt(vpMath::sqr(a.get_X() - b.get_X()) + vpMath::sqr(a.get_Y() - b.get_Y()) +
+ vpMath::sqr(a.get_Z() - b.get_Z()));
}
#endif
diff --git a/modules/tracker/mbt/src/vpMbTracker.cpp b/modules/tracker/mbt/src/vpMbTracker.cpp
index 0c0f008..f79994b 100644
--- a/modules/tracker/mbt/src/vpMbTracker.cpp
+++ b/modules/tracker/mbt/src/vpMbTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,84 +43,90 @@
\brief Generic model based tracker
*/
+#include <algorithm>
#include <iostream>
#include <limits>
-#include <algorithm>
#include <map>
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpDisplay.h>
#ifdef VISP_HAVE_MODULE_GUI
-# include <visp3/gui/vpDisplayOpenCV.h>
-# include <visp3/gui/vpDisplayX.h>
-# include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColor.h>
-#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpPixelMeterConversion.h>
#ifdef VISP_HAVE_MODULE_IO
-# include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
#endif
-#include <visp3/mbt/vpMbTracker.h>
-#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpCPUFeatures.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMatrixException.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/mbt/vpMbTracker.h>
#ifdef VISP_HAVE_COIN3D
-//Inventor includes
-#include <Inventor/nodes/SoSeparator.h>
+// Inventor includes
+#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
+#include <Inventor/VRMLnodes/SoVRMLGroup.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedFaceSet.h>
#include <Inventor/VRMLnodes/SoVRMLIndexedLineSet.h>
-#include <Inventor/VRMLnodes/SoVRMLCoordinate.h>
-#include <Inventor/actions/SoWriteAction.h>
-#include <Inventor/actions/SoSearchAction.h>
-#include <Inventor/misc/SoChildList.h>
+#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#include <Inventor/VRMLnodes/SoVRMLTransform.h>
#include <Inventor/actions/SoGetMatrixAction.h>
#include <Inventor/actions/SoGetPrimitiveCountAction.h>
+#include <Inventor/actions/SoSearchAction.h>
#include <Inventor/actions/SoToVRML2Action.h>
-#include <Inventor/VRMLnodes/SoVRMLGroup.h>
-#include <Inventor/VRMLnodes/SoVRMLTransform.h>
-#include <Inventor/VRMLnodes/SoVRMLShape.h>
+#include <Inventor/actions/SoWriteAction.h>
+#include <Inventor/misc/SoChildList.h>
+#include <Inventor/nodes/SoSeparator.h>
#endif
-
+#if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
+#include <emmintrin.h>
+#define VISP_HAVE_SSE2 1
+#endif
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-namespace {
- /*!
- Structure to store info about segment in CAO model files.
- */
- struct SegmentInfo {
- SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
-
- std::vector<vpPoint> extremities;
- std::string name;
- bool useLod;
- double minLineLengthThresh;
- };
-
- /*!
- Structure to store info about a polygon face represented by a vpPolygon and by a list of vpPoint
- representing the corners of the polygon face in 3D.
- */
- struct PolygonFaceInfo {
- PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
- : distanceToCamera(dist), polygon(poly), faceCorners(corners) {}
-
- bool operator<(const PolygonFaceInfo &pfi) const {
- return distanceToCamera < pfi.distanceToCamera;
- }
-
- double distanceToCamera;
- vpPolygon polygon;
- std::vector<vpPoint> faceCorners;
- };
+namespace
+{
+/*!
+ Structure to store info about segment in CAO model files.
+ */
+struct SegmentInfo {
+ SegmentInfo() : extremities(), name(), useLod(false), minLineLengthThresh(0.) {}
+
+ std::vector<vpPoint> extremities;
+ std::string name;
+ bool useLod;
+ double minLineLengthThresh;
+};
+
+/*!
+ Structure to store info about a polygon face represented by a vpPolygon and
+ by a list of vpPoint representing the corners of the polygon face in 3D.
+ */
+struct PolygonFaceInfo {
+ PolygonFaceInfo(const double dist, const vpPolygon &poly, const std::vector<vpPoint> &corners)
+ : distanceToCamera(dist), polygon(poly), faceCorners(corners)
+ {
+ }
+
+ bool operator<(const PolygonFaceInfo &pfi) const { return distanceToCamera < pfi.distanceToCamera; }
+
+ double distanceToCamera;
+ vpPolygon polygon;
+ std::vector<vpPoint> faceCorners;
+};
}
#endif // DOXYGEN_SHOULD_SKIP_THIS
@@ -129,95 +136,99 @@ namespace {
*/
vpMbTracker::vpMbTracker()
-: cam(), cMo(), oJo(6,6), isoJoIdentity(true), modelFileName(), modelInitialised(false),
- poseSavingFilename(), computeCovariance(false), covarianceMatrix(), computeProjError(false),
- projectionError(90.0), displayFeatures(false), m_w(), m_error(), m_optimizationMethod(vpMbTracker::GAUSS_NEWTON_OPT),
- faces(), angleAppears( vpMath::rad(89) ), angleDisappears( vpMath::rad(89) ), distNearClip(0.001),
- distFarClip(100), clippingFlag(vpPolygon3D::NO_CLIPPING), useOgre(false), ogreShowConfigDialog(false), useScanLine(false),
- nbPoints(0), nbLines(0), nbPolygonLines(0), nbPolygonPoints(0), nbCylinders(0), nbCircles(0),
- useLodGeneral(false), applyLodSettingInConfig(false), minLineLengthThresholdGeneral(50.0),
- minPolygonAreaThresholdGeneral(2500.0), mapOfParameterNames()
+ : 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,
- //like name of faces or LOD setting
- mapOfParameterNames["name"] = "string";
- mapOfParameterNames["minPolygonAreaThreshold"] = "number";
- mapOfParameterNames["minLineLengthThreshold"] = "number";
- mapOfParameterNames["useLod"] = "boolean";
+ oJo.eye();
+ // Map used to parse additional information in CAO model files,
+ // like name of faces or LOD setting
+ mapOfParameterNames["name"] = "string";
+ mapOfParameterNames["minPolygonAreaThreshold"] = "number";
+ mapOfParameterNames["minLineLengthThreshold"] = "number";
+ mapOfParameterNames["useLod"] = "boolean";
}
/*!
Basic destructor that doest nothing.
*/
-vpMbTracker::~vpMbTracker()
-{
-}
+vpMbTracker::~vpMbTracker() {}
#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the file initFile. The structure of this file
- is (without the comments):
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are extracted from a file. In
+ this file, comments starting with # character are allowed. Notice that 3D
+ point coordinates are expressed in meter in the object frame with their X, Y
+ and Z values.
+
+ The structure of this file is the following:
+
\code
- 4 // Number of points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in the object basis
- 0.01 -0.01 -0.01 // /
+ # 3D point coordinates
+ 4 # Number of points in the file (minimum is four)
+ 0.01 0.01 0.01 # \
+ ... # | 3D coordinates in the object frame (X, Y, Z)
+ 0.01 -0.01 -0.01 # /
\endcode
- \param I : Input image
- \param initFile : File containing the points where to click
- \param displayHelp : Optionnal display of an image ( 'initFile.ppm' ). This
- image may be used to show where to click. This functionality is only available
- if visp_io module is used.
-
- \exception vpException::ioError : The file specified in initFile doesn't exist.
+ \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
+ 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.
- \sa setPathNamePoseSaving()
+ \exception vpException::ioError : The file specified in \e initFile doesn't
+ exist.
*/
-void
-vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initFile, const bool displayHelp)
+void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::string &initFile, const bool displayHelp)
{
vpHomogeneousMatrix last_cMo;
vpPoseVector init_pos;
- vpImagePoint ip;
+ vpImagePoint ip;
vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
-
- std::string ext = ".init";
- std::string str_pose = "";
- size_t pos = (unsigned int)initFile.rfind(ext);
+
+ std::string ext = ".init";
+ std::string str_pose = "";
+ size_t pos = (unsigned int)initFile.rfind(ext);
// Load the last poses from files
- std::fstream finitpos ;
- std::fstream finit ;
+ std::fstream finitpos;
+ std::fstream finit;
char s[FILENAME_MAX];
- if(poseSavingFilename.empty()){
- if( pos == initFile.size()-ext.size() && pos != 0)
- str_pose = initFile.substr(0,pos) + ".0.pos";
- else
- str_pose = initFile + ".0.pos";
-
- finitpos.open(str_pose.c_str() ,std::ios::in) ;
- sprintf(s, "%s", str_pose.c_str());
- }else{
- finitpos.open(poseSavingFilename.c_str() ,std::ios::in) ;
+ if (poseSavingFilename.empty()) {
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ str_pose = initFile.substr(0, pos) + ".0.pos";
+ else
+ str_pose = initFile + ".0.pos";
+
+ finitpos.open(str_pose.c_str(), std::ios::in);
+ sprintf(s, "%s", str_pose.c_str());
+ } else {
+ finitpos.open(poseSavingFilename.c_str(), std::ios::in);
sprintf(s, "%s", poseSavingFilename.c_str());
}
- if(finitpos.fail() ){
- std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
+ if (finitpos.fail()) {
+ std::cout << "cannot read " << s << std::endl << "cMo set to identity" << std::endl;
last_cMo.eye();
- }
- else{
- for (unsigned int i = 0; i < 6; i += 1){
+ } else {
+ for (unsigned int i = 0; i < 6; i += 1) {
finitpos >> init_pos[i];
}
finitpos.close();
- last_cMo.buildFrom(init_pos) ;
-
- std::cout <<"last_cMo : "<<std::endl << last_cMo <<std::endl;
+ last_cMo.buildFrom(init_pos);
+
+ std::cout << "last_cMo : " << std::endl << last_cMo << std::endl;
vpDisplay::display(I);
display(I, last_cMo, cam, vpColor::green, 1, true);
@@ -225,89 +236,81 @@ vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initF
vpDisplay::flush(I);
std::cout << "No modification : left click " << std::endl;
- std::cout << "Modify initial pose : right 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) ;
+ vpDisplay::flush(I);
- while (!vpDisplay::getClick(I, ip, button)) ;
+ while (!vpDisplay::getClick(I, ip, button))
+ ;
}
-
- if (!finitpos.fail() && button == vpMouseButton::button1){
- cMo = last_cMo ;
- }
- else
- {
+ if (!finitpos.fail() && button == vpMouseButton::button1) {
+ cMo = last_cMo;
+ } else {
vpDisplay *d_help = NULL;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
- vpPose pose ;
+ vpPose pose;
- pose.clearPoint() ;
+ pose.clearPoint();
// file parser
// number of points
// X Y Z
// X Y Z
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
+ else
+ sprintf(s, "%s.init", initFile.c_str());
- double X,Y,Z ;
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.init", initFile.c_str());
-
- std::cout << "Load 3D points from: " << s << std::endl ;
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
+ std::cout << "Load 3D points from: " << s << std::endl;
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
+ throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
}
#ifdef VISP_HAVE_MODULE_IO
- //Display window creation and initialisation
- try{
- if(displayHelp){
+ // Display window creation and initialisation
+ try {
+ if (displayHelp) {
std::string dispF;
- if( pos == initFile.size()-ext.size() && pos != 0)
- dispF = initFile.substr(0,pos) + ".ppm";
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ dispF = initFile.substr(0, pos) + ".ppm";
else
dispF = initFile + ".ppm";
if (vpIoTools::checkFilename(dispF)) {
std::cout << "Load image to help initialization: " << dispF << std::endl;
#if defined VISP_HAVE_X11
- d_help = new vpDisplayX ;
+ d_help = new vpDisplayX;
#elif defined VISP_HAVE_GDI
d_help = new vpDisplayGDI;
#elif defined VISP_HAVE_OPENCV
d_help = new vpDisplayOpenCV;
#endif
- vpImage<vpRGBa> Iref ;
- vpImageIo::read(Iref, dispF) ;
+ vpImage<vpRGBa> Iref;
+ vpImageIo::read(Iref, dispF);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
- d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
- "Where to initialize...") ;
- vpDisplay::display(Iref) ;
+ d_help->init(Iref, I.display->getWindowXPosition() + (int)I.getWidth() + 80, I.display->getWindowYPosition(),
+ "Where to initialize...");
+ vpDisplay::display(Iref);
vpDisplay::flush(Iref);
#endif
}
}
- }
- catch(...) {
- if(d_help != NULL) {
+ } catch (...) {
+ if (d_help != NULL) {
delete d_help;
d_help = NULL;
}
}
-#else //#ifdef VISP_HAVE_MODULE_IO
+#else //#ifdef VISP_HAVE_MODULE_IO
(void)(displayHelp);
#endif //#ifdef VISP_HAVE_MODULE_IO
char c;
@@ -319,17 +322,16 @@ vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initF
}
finit.unget();
- unsigned int n ;
- finit >> n ;
+ unsigned int n3d;
+ finit >> n3d;
finit.ignore(256, '\n'); // skip the rest of the line
- std::cout << "Number of 3D points " << n << std::endl ;
- if (n > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
+ 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);
}
- vpPoint *P = new vpPoint [n] ;
- for (unsigned int i=0 ; i < n ; i++){
+ vpPoint *P = new vpPoint[n3d];
+ for (unsigned int i = 0; i < n3d; i++) {
// skip lines starting with # as comment
finit.get(c);
while (!finit.fail() && (c == '#')) {
@@ -337,224 +339,207 @@ vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::string& initF
finit.get(c);
}
finit.unget();
+ double X, Y, Z;
- finit >> X ;
- finit >> Y ;
- finit >> Z ;
+ finit >> X;
+ finit >> Y;
+ finit >> Z;
finit.ignore(256, '\n'); // skip the rest of the line
- std::cout << "Point " << i+1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
- P[i].setWorldCoordinates(X,Y,Z) ; // (X,Y,Z)
+ std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
+ P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
}
finit.close();
-////////////////////////////////
bool isWellInit = false;
- while(!isWellInit)
- {
-////////////////////////////////
+ while (!isWellInit) {
std::vector<vpImagePoint> mem_ip;
- for(unsigned int i=0 ; i< n ; i++)
- {
+ for (unsigned int i = 0; i < n3d; i++) {
std::ostringstream text;
- text << "Click on point " << i+1;
+ text << "Click on point " << i + 1;
vpDisplay::display(I);
vpDisplay::displayText(I, 15, 10, text.str(), vpColor::red);
- for (unsigned int k=0; k<mem_ip.size(); k++) {
- vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2) ;
+ for (unsigned int k = 0; k < mem_ip.size(); k++) {
+ vpDisplay::displayCross(I, mem_ip[k], 10, vpColor::green, 2);
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
- std::cout << "Click on point " << i+1 << " ";
- double x=0,y=0;
- vpDisplay::getClick(I, ip) ;
+ std::cout << "Click on point " << i + 1 << " ";
+ double x = 0, y = 0;
+ vpDisplay::getClick(I, ip);
mem_ip.push_back(ip);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
vpPixelMeterConversion::convertPoint(cam, ip, x, y);
P[i].set_x(x);
P[i].set_y(y);
std::cout << "with 2D coordinates: " << ip << std::endl;
- pose.addPoint(P[i]) ; // and added to the pose computation point list
+ pose.addPoint(P[i]); // and added to the pose computation point list
}
- vpDisplay::flush(I) ;
- vpDisplay::display(I) ;
+ vpDisplay::flush(I);
+ vpDisplay::display(I);
vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
+ pose.computePose(vpPose::LAGRANGE, cMo1);
double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
+ pose.computePose(vpPose::DEMENTHON, cMo2);
double d2 = pose.computeResidual(cMo2);
- if(d1 < d2){
+ if (d1 < d2) {
cMo = cMo1;
- }
- else{
+ } else {
cMo = cMo2;
}
pose.computePose(vpPose::VIRTUAL_VS, cMo);
display(I, cMo, cam, vpColor::green, 1, true);
- vpDisplay::displayText(I, 15, 10,
- "left click to validate, right click to re initialize object",
- vpColor::red);
+ vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
button = vpMouseButton::button1;
- while (!vpDisplay::getClick(I, ip, button)) ;
-
+ while (!vpDisplay::getClick(I, ip, button))
+ ;
- if (button == vpMouseButton::button1)
- {
+ if (button == vpMouseButton::button1) {
isWellInit = true;
- }
- else
- {
- pose.clearPoint() ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ } else {
+ pose.clearPoint();
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
}
-////////////////////////////////////
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
- delete [] P;
+ delete[] P;
- //save the pose into file
- if(poseSavingFilename.empty())
- savePose(str_pose);
- else
- savePose(poseSavingFilename);
+ // save the pose into file
+ if (poseSavingFilename.empty())
+ savePose(str_pose);
+ else
+ savePose(poseSavingFilename);
- if(d_help != NULL) {
+ if (d_help != NULL) {
delete d_help;
d_help = NULL;
}
- }
+ }
- std::cout <<"cMo : "<<std::endl << cMo <<std::endl;
+ std::cout << "cMo : " << std::endl << cMo << std::endl;
init(I);
}
/*!
- Initialise the tracking by clicking on the image points corresponding to the
- 3D points (object frame) in the list points3D_list.
-
- \param I : Input image
- \param points3D_list : List of the 3D points (object frame).
- \param displayFile : Path to the image used to display the help. This functionality
- is only available if visp_io module is used.
+ Initialise the tracker by clicking in the image on the pixels that
+ correspond to the 3D points whose coordinates are given in \e points3D_list.
+
+ \param I : Input image where the user has to click.
+ \param points3D_list : List of at least 4 3D points with coordinates
+ expressed in meters in the object frame. \param displayFile : Path to the
+ 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<unsigned char>& I, const std::vector<vpPoint> &points3D_list,
+void vpMbTracker::initClick(const vpImage<unsigned char> &I, const std::vector<vpPoint> &points3D_list,
const std::string &displayFile)
{
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
vpDisplay *d_help = NULL;
- vpPose pose ;
+ vpPose pose;
std::vector<vpPoint> P;
- for (unsigned int i=0 ; i < points3D_list.size() ; i++)
- P.push_back( vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()) );
-
+ for (unsigned int i = 0; i < points3D_list.size(); i++)
+ P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
+
#ifdef VISP_HAVE_MODULE_IO
- vpImage<vpRGBa> Iref ;
- //Display window creation and initialisation
- if(vpIoTools::checkFilename(displayFile)){
- try{
+ vpImage<vpRGBa> Iref;
+ // Display window creation and initialisation
+ if (vpIoTools::checkFilename(displayFile)) {
+ try {
std::cout << "Load image to help initialization: " << displayFile << std::endl;
#if defined VISP_HAVE_X11
- d_help = new vpDisplayX ;
+ d_help = new vpDisplayX;
#elif defined VISP_HAVE_GDI
d_help = new vpDisplayGDI;
#elif defined VISP_HAVE_OPENCV
d_help = new vpDisplayOpenCV;
#endif
- vpImageIo::read(Iref, displayFile) ;
+ vpImageIo::read(Iref, displayFile);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
- d_help->init(Iref, I.display->getWindowXPosition()+(int)I.getWidth()+80, I.display->getWindowYPosition(),
- "Where to initialize...") ;
- vpDisplay::display(Iref) ;
+ d_help->init(Iref, I.display->getWindowXPosition() + (int)I.getWidth() + 80, I.display->getWindowYPosition(),
+ "Where to initialize...");
+ vpDisplay::display(Iref);
vpDisplay::flush(Iref);
#endif
- }
- catch(...) {
- if(d_help != NULL) {
+ } catch (...) {
+ if (d_help != NULL) {
delete d_help;
d_help = NULL;
}
}
}
-#else //#ifdef VISP_HAVE_MODULE_IO
- (void)(displayFile);
+#else //#ifdef VISP_HAVE_MODULE_IO
+ (void)(displayFile);
#endif //#ifdef VISP_HAVE_MODULE_IO
- vpImagePoint ip;
- bool isWellInit = false;
- while(!isWellInit)
- {
- for(unsigned int i=0 ; i< points3D_list.size() ; i++)
- {
- std::cout << "Click on point " << i+1 << std::endl ;
- double x=0,y=0;
- vpDisplay::getClick(I, ip) ;
- vpDisplay::displayCross(I, ip, 5,vpColor::green) ;
- vpDisplay::flush(I) ;
- vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- P[i].set_x(x);
- P[i].set_y(y);
-
- std::cout << "Click on point " << ip << std::endl;
-
- vpDisplay::displayPoint (I, ip, vpColor::green); //display target point
- pose.addPoint(P[i]) ; // and added to the pose computation point list
- }
- vpDisplay::flush(I) ;
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2){
- cMo = cMo1;
- }
- else{
- cMo = cMo2;
- }
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
+ vpImagePoint ip;
+ bool isWellInit = false;
+ while (!isWellInit) {
+ for (unsigned int i = 0; i < points3D_list.size(); i++) {
+ std::cout << "Click on point " << i + 1 << std::endl;
+ double x = 0, y = 0;
+ vpDisplay::getClick(I, ip);
+ vpDisplay::displayCross(I, ip, 5, vpColor::green);
+ vpDisplay::flush(I);
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+
+ std::cout << "Click on point " << ip << std::endl;
+
+ vpDisplay::displayPoint(I, ip, vpColor::green); // display target point
+ pose.addPoint(P[i]); // and added to the pose computation point list
+ }
+ vpDisplay::flush(I);
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1);
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2);
+ double d2 = pose.computeResidual(cMo2);
+
+ if (d1 < d2) {
+ cMo = cMo1;
+ } else {
+ cMo = cMo2;
+ }
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
display(I, cMo, cam, vpColor::green, 1, true);
- vpDisplay::displayText(I, 15, 10,
- "left click to validate, right click to re initialize object",
- vpColor::red);
-
- vpDisplay::flush(I) ;
-
- vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
- while (!vpDisplay::getClick(I, ip, button)) ;
-
- if (button == vpMouseButton::button1)
- {
- isWellInit = true;
- }
- else
- {
- pose.clearPoint() ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
- }
- }
+ vpDisplay::displayText(I, 15, 10, "left click to validate, right click to re initialize object", vpColor::red);
+
+ vpDisplay::flush(I);
+
+ vpMouseButton::vpMouseButtonType button = vpMouseButton::button1;
+ while (!vpDisplay::getClick(I, ip, button)) {
+ };
+
+ if (button == vpMouseButton::button1) {
+ isWellInit = true;
+ } else {
+ pose.clearPoint();
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
+ }
+ }
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::red);
- if(d_help != NULL) {
+ if (d_help != NULL) {
delete d_help;
d_help = NULL;
}
@@ -564,95 +549,154 @@ void vpMbTracker::initClick(const vpImage<unsigned char>& I, const std::vector<v
#endif //#ifdef VISP_HAVE_MODULE_GUI
/*!
- Initialise the tracking by reading the 3D points (object frame) and the image points
- in initFile. The structure of this file is (without the comments):
- \code
- 4 // Number of 3D points in the file (minimum is four)
- 0.01 0.01 0.01 // \
- ... // | 3D coordinates in meters in the object frame
- 0.01 -0.01 -0.01 // /
- 4 // Number of image points in the file (has to be the same as the number of 3D points)
- 100 200 // \
- ... // | 2D coordinates in pixel in the image
- 50 10 // /
+ Initialise the tracker by reading 3D point coordinates and the corresponding
+ 2D image point coordinates from a file. Comments starting with # character
+ 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 # /
\endcode
-
+
\param I : Input image
\param initFile : Path to the file containing all the points.
*/
-void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::string& initFile )
+void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::string &initFile)
{
- char s[FILENAME_MAX];
- std::fstream finit ;
-
- std::string ext = ".init";
+ char s[FILENAME_MAX];
+ std::fstream finit;
+
+ std::string ext = ".init";
size_t pos = initFile.rfind(ext);
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.init", initFile.c_str());
-
- std::cout << "filename " << s << std::endl ;
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
- std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
- }
-
- unsigned int size;
- double X, Y, Z;
- finit >> size ;
- std::cout << "number of points " << size << std::endl ;
-
- if (size > 100000) {
+
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
+ else
+ sprintf(s, "%s.init", initFile.c_str());
+
+ std::cout << "Load 2D/3D points from: " << s << std::endl;
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
+ std::cout << "cannot read " << s << std::endl;
+ throw vpException(vpException::ioError, "Cannot open model-based tracker init file %s", s);
+ }
+
+ //********
+ // Read 3D points coordinates
+ //********
+ char c;
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+
+ unsigned int n3d;
+ finit >> n3d;
+ 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);
+ }
+
+ vpPoint *P = new vpPoint[n3d];
+ for (unsigned int i = 0; i < n3d; i++) {
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+ double X, Y, Z;
+ finit >> X;
+ finit >> Y;
+ finit >> Z;
+ finit.ignore(256, '\n'); // skip the rest of the line
+
+ std::cout << "Point " << i + 1 << " with 3D coordinates: " << X << " " << Y << " " << Z << std::endl;
+ P[i].setWorldCoordinates(X, Y, Z); // (X,Y,Z)
+ }
+
+ //********
+ // Read 3D points coordinates
+ //********
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+
+ unsigned int n2d;
+ finit >> n2d;
+ finit.ignore(256, '\n'); // skip the rest of the line
+ 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);
+ }
+
+ if (n3d != n2d) {
+ delete[] P;
throw vpException(vpException::badValue,
- "Exceed the max number of points.");
+ "In %s file, number of 2D points %d and number of 3D "
+ "points %d are not equal",
+ s, n2d, n3d);
}
- vpPoint *P = new vpPoint [size];
- vpPose pose ;
-
- for(unsigned int i=0 ; i< size ; i++)
- {
- finit >> X ;
- finit >> Y ;
- finit >> Z ;
- P[i].setWorldCoordinates(X,Y,Z) ;
- }
-
- unsigned int size2;
- double x, y;
- vpImagePoint ip;
- finit >> size2 ;
- if(size != size2)
- vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
-
- for(unsigned int i=0 ; i< size ; i++)
- {
- finit >> x;
- finit >> y;
- ip = vpImagePoint(x,y);
- vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- P[i].set_x(x);
- P[i].set_y(y);
- pose.addPoint(P[i]);
- }
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2)
- cMo = cMo1;
- else
- cMo = cMo2;
-
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
-
- delete [] P;
+ vpPose pose;
+ for (unsigned int i = 0; i < n2d; i++) {
+ // skip lines starting with # as comment
+ finit.get(c);
+ while (!finit.fail() && (c == '#')) {
+ finit.ignore(256, '\n');
+ finit.get(c);
+ }
+ finit.unget();
+ double u, v, x = 0, y = 0;
+ finit >> v;
+ finit >> u;
+ 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;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+ pose.addPoint(P[i]);
+ }
+
+ finit.close();
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1);
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2);
+ double d2 = pose.computeResidual(cMo2);
+
+ if (d1 < d2)
+ cMo = cMo1;
+ else
+ cMo = cMo2;
+
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
+
+ delete[] P;
init(I);
}
@@ -660,99 +704,100 @@ void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::st
/*!
Initialise the tracking with the list of image points (points2D_list) and
the list of corresponding 3D points (object frame) (points3D_list).
-
+
\param I : Input image
\param points2D_list : List of image points.
- \param points3D_list : List of 3D points (object frame).
+ \param points3D_list : List of 3D points (object frame).
*/
-void vpMbTracker::initFromPoints( const vpImage<unsigned char>& I, const std::vector<vpImagePoint> &points2D_list,
- const std::vector<vpPoint> &points3D_list )
+void vpMbTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &points2D_list,
+ const std::vector<vpPoint> &points3D_list)
{
- if(points2D_list.size() != points3D_list.size())
- vpERROR_TRACE( "vpMbTracker::initFromPoints(), Number of 2D points different to the number of 3D points." );
-
- size_t size = points3D_list.size();
+ if (points2D_list.size() != points3D_list.size())
+ vpERROR_TRACE("vpMbTracker::initFromPoints(), Number of 2D points "
+ "different to the number of 3D points.");
+
+ size_t size = points3D_list.size();
std::vector<vpPoint> P;
- vpPose pose ;
-
- for(size_t i=0 ; i< size ; i++)
- {
- P.push_back( vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()) );
- double x=0,y=0;
- vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
- P[i].set_x(x);
- P[i].set_y(y);
- pose.addPoint(P[i]);
- }
-
- vpHomogeneousMatrix cMo1, cMo2;
- pose.computePose(vpPose::LAGRANGE, cMo1) ;
- double d1 = pose.computeResidual(cMo1);
- pose.computePose(vpPose::DEMENTHON, cMo2) ;
- double d2 = pose.computeResidual(cMo2);
-
- if(d1 < d2)
- cMo = cMo1;
- else
- cMo = cMo2;
-
- pose.computePose(vpPose::VIRTUAL_VS, cMo);
+ vpPose pose;
+
+ for (size_t i = 0; i < size; i++) {
+ P.push_back(vpPoint(points3D_list[i].get_oX(), points3D_list[i].get_oY(), points3D_list[i].get_oZ()));
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(cam, points2D_list[i], x, y);
+ P[i].set_x(x);
+ P[i].set_y(y);
+ pose.addPoint(P[i]);
+ }
+
+ vpHomogeneousMatrix cMo1, cMo2;
+ pose.computePose(vpPose::LAGRANGE, cMo1);
+ double d1 = pose.computeResidual(cMo1);
+ pose.computePose(vpPose::DEMENTHON, cMo2);
+ double d2 = pose.computeResidual(cMo2);
+
+ if (d1 < d2)
+ cMo = cMo1;
+ else
+ cMo = cMo2;
+
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
init(I);
}
/*!
- Initialise the tracking thanks to the pose in vpPoseVector format, and read in the file initFile.
- The structure of this file is (without the comments):
+ Initialise the tracking thanks to the pose in vpPoseVector format, and read
+ in the file initFile. The structure of this file is (without the comments):
\code
// The six value of the pose vector
0.0000 // \
- 0.0000 // |
+ 0.0000 // |
1.0000 // | Exemple of value for the pose vector where Z = 1 meter
0.0000 // |
- 0.0000 // |
+ 0.0000 // |
0.0000 // /
\endcode
-
- Where the three firsts lines refer to the translation and the three last to the rotation in thetaU parametrisation (see vpThetaUVector).
- \param I : Input image
- \param initFile : Path to the file containing the pose.
+
+ Where the three firsts lines refer to the translation and the three last to
+ the rotation in thetaU parametrisation (see vpThetaUVector). \param I :
+ Input image \param initFile : Path to the file containing the pose.
*/
-void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const std::string &initFile)
+void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const std::string &initFile)
{
- char s[FILENAME_MAX];
- std::fstream finit ;
- vpPoseVector init_pos;
-
- std::string ext = ".pos";
- size_t pos = initFile.rfind(ext);
-
- if( pos == initFile.size()-ext.size() && pos != 0)
- sprintf(s,"%s", initFile.c_str());
- else
- sprintf(s,"%s.pos", initFile.c_str());
-
- finit.open(s,std::ios::in) ;
- if (finit.fail()){
- std::cout << "cannot read " << s << std::endl;
- throw vpException(vpException::ioError, "cannot read init file");
- }
-
- for (unsigned int i = 0; i < 6; i += 1){
- finit >> init_pos[i];
- }
-
- cMo.buildFrom(init_pos);
+ char s[FILENAME_MAX];
+ std::fstream finit;
+ vpPoseVector init_pos;
+
+ std::string ext = ".pos";
+ size_t pos = initFile.rfind(ext);
+
+ if (pos == initFile.size() - ext.size() && pos != 0)
+ sprintf(s, "%s", initFile.c_str());
+ else
+ sprintf(s, "%s.pos", initFile.c_str());
+
+ finit.open(s, std::ios::in);
+ if (finit.fail()) {
+ std::cout << "cannot read " << s << std::endl;
+ throw vpException(vpException::ioError, "cannot read init file");
+ }
+
+ for (unsigned int i = 0; i < 6; i += 1) {
+ finit >> init_pos[i];
+ }
+
+ cMo.buildFrom(init_pos);
+
init(I);
}
/*!
Initialise the tracking thanks to the pose.
-
+
\param I : Input image
\param cMo_ : Pose matrix.
*/
-void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogeneousMatrix &cMo_)
+void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const vpHomogeneousMatrix &cMo_)
{
this->cMo = cMo_;
init(I);
@@ -760,11 +805,11 @@ void vpMbTracker::initFromPose(const vpImage<unsigned char>& I, const vpHomogene
/*!
Initialise the tracking thanks to the pose vector.
-
+
\param I : Input image
- \param cPo : Pose vector.
+ \param cPo : Pose vector.
*/
-void vpMbTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVector &cPo)
+void vpMbTracker::initFromPose(const vpImage<unsigned char> &I, const vpPoseVector &cPo)
{
vpHomogeneousMatrix _cMo(cPo);
initFromPose(I, _cMo);
@@ -772,34 +817,37 @@ void vpMbTracker::initFromPose (const vpImage<unsigned char>& I, const vpPoseVec
/*!
Save the pose in the given filename
-
- \param filename : Path to the file used to save the pose.
+
+ \param filename : Path to the file used to save the pose.
*/
void vpMbTracker::savePose(const std::string &filename) const
{
- vpPoseVector init_pos;
- std::fstream finitpos ;
- char s[FILENAME_MAX];
-
- sprintf(s,"%s", filename.c_str());
- finitpos.open(s, std::ios::out) ;
-
- init_pos.buildFrom(cMo);
- finitpos << init_pos;
- finitpos.close();
-}
+ vpPoseVector init_pos;
+ std::fstream finitpos;
+ char s[FILENAME_MAX];
+
+ sprintf(s, "%s", filename.c_str());
+ finitpos.open(s, std::ios::out);
+ init_pos.buildFrom(cMo);
+ finitpos << init_pos;
+ finitpos.close();
+}
-void vpMbTracker::addPolygon(const std::vector<vpPoint>& corners, const int idFace, const std::string &polygonName,
- const bool useLod, const double minPolygonAreaThreshold, const double minLineLengthThreshold)
+void vpMbTracker::addPolygon(const std::vector<vpPoint> &corners, const int idFace, const std::string &polygonName,
+ const bool useLod, const double minPolygonAreaThreshold,
+ const double minLineLengthThreshold)
{
std::vector<vpPoint> corners_without_duplicates;
corners_without_duplicates.push_back(corners[0]);
- for (unsigned int i=0; i < corners.size()-1; i++) {
- if (std::fabs(corners[i].get_oX() - corners[i+1].get_oX()) > std::fabs(corners[i].get_oX())*std::numeric_limits<double>::epsilon()
- || std::fabs(corners[i].get_oY() - corners[i+1].get_oY()) > std::fabs(corners[i].get_oY())*std::numeric_limits<double>::epsilon()
- || std::fabs(corners[i].get_oZ() - corners[i+1].get_oZ()) > std::fabs(corners[i].get_oZ())*std::numeric_limits<double>::epsilon()) {
- corners_without_duplicates.push_back(corners[i+1]);
+ for (unsigned int i = 0; i < corners.size() - 1; i++) {
+ if (std::fabs(corners[i].get_oX() - corners[i + 1].get_oX()) >
+ std::fabs(corners[i].get_oX()) * std::numeric_limits<double>::epsilon() ||
+ std::fabs(corners[i].get_oY() - corners[i + 1].get_oY()) >
+ std::fabs(corners[i].get_oY()) * std::numeric_limits<double>::epsilon() ||
+ std::fabs(corners[i].get_oZ() - corners[i + 1].get_oZ()) >
+ std::fabs(corners[i].get_oZ()) * std::numeric_limits<double>::epsilon()) {
+ corners_without_duplicates.push_back(corners[i + 1]);
}
}
@@ -809,188 +857,194 @@ void vpMbTracker::addPolygon(const std::vector<vpPoint>& corners, const int idFa
polygon.setName(polygonName);
polygon.setLod(useLod);
-// //if(minPolygonAreaThreshold != -1.0) {
-// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
-// }
-//
-// //if(minLineLengthThreshold != -1.0) {
-// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinLineLengthThresh(minLineLengthThreshold);
-// }
+ // //if(minPolygonAreaThreshold != -1.0) {
+ // if(std::fabs(minPolygonAreaThreshold + 1.0) >
+ // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
+ // {
+ // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+ // }
+ //
+ // //if(minLineLengthThreshold != -1.0) {
+ // if(std::fabs(minLineLengthThreshold + 1.0) >
+ // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
+ // {
+ // polygon.setMinLineLengthThresh(minLineLengthThreshold);
+ // }
polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
polygon.setMinLineLengthThresh(minLineLengthThreshold);
- for(unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
+ for (unsigned int j = 0; j < corners_without_duplicates.size(); j++) {
polygon.addPoint(j, corners_without_duplicates[j]);
}
faces.addPolygon(&polygon);
- if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ 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);
}
-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)
+void vpMbTracker::addPolygon(const vpPoint &p1, const vpPoint &p2, const vpPoint &p3, const double radius,
+ const int idFace, const std::string &polygonName, const bool useLod,
+ const double minPolygonAreaThreshold)
{
- vpMbtPolygon polygon;
- polygon.setNbPoint(4);
- polygon.setName(polygonName);
- polygon.setLod(useLod);
+ vpMbtPolygon polygon;
+ polygon.setNbPoint(4);
+ polygon.setName(polygonName);
+ polygon.setLod(useLod);
-// //if(minPolygonAreaThreshold != -1.0) {
-// if(std::fabs(minPolygonAreaThreshold + 1.0) > std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
-// }
- polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
- //Non sense to set minLineLengthThreshold for circle
- //but used to be coherent when applying LOD settings for all polygons
- polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);
-
- {
- // Create the 4 points of the circle bounding box
- vpPlane plane(p1, p2, p3, vpPlane::object_frame);
-
- // Matrice de passage entre world et circle frame
- double norm_X = sqrt(vpMath::sqr(p2.get_oX()-p1.get_oX())
- + vpMath::sqr(p2.get_oY()-p1.get_oY())
- + vpMath::sqr(p2.get_oZ()-p1.get_oZ()));
- double norm_Y = sqrt(vpMath::sqr(plane.getA())
- + vpMath::sqr(plane.getB())
- + vpMath::sqr(plane.getC()));
- vpRotationMatrix wRc;
- vpColVector x(3),y(3),z(3);
- // X axis is P2-P1
- x[0] = (p2.get_oX()-p1.get_oX()) / norm_X;
- x[1] = (p2.get_oY()-p1.get_oY()) / norm_X;
- x[2] = (p2.get_oZ()-p1.get_oZ()) / norm_X;
- // Y axis is the normal of the plane
- y[0] = plane.getA() / norm_Y;
- y[1] = plane.getB() / norm_Y;
- y[2] = plane.getC() / norm_Y;
- // Z axis = X ^ Y
- z = vpColVector::crossProd(x, y);
- for (unsigned int i=0; i< 3; i++) {
- wRc[i][0] = x[i];
- wRc[i][1] = y[i];
- wRc[i][2] = z[i];
- }
+ // //if(minPolygonAreaThreshold != -1.0) {
+ // if(std::fabs(minPolygonAreaThreshold + 1.0) >
+ // std::fabs(minPolygonAreaThreshold)*std::numeric_limits<double>::epsilon())
+ // {
+ // polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+ // }
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThreshold);
+ // Non sense to set minLineLengthThreshold for circle
+ // but used to be coherent when applying LOD settings for all polygons
+ polygon.setMinLineLengthThresh(minLineLengthThresholdGeneral);
+
+ {
+ // Create the 4 points of the circle bounding box
+ vpPlane plane(p1, p2, p3, vpPlane::object_frame);
+
+ // Matrice de passage entre world et circle frame
+ double norm_X = sqrt(vpMath::sqr(p2.get_oX() - p1.get_oX()) + vpMath::sqr(p2.get_oY() - p1.get_oY()) +
+ vpMath::sqr(p2.get_oZ() - p1.get_oZ()));
+ double norm_Y = sqrt(vpMath::sqr(plane.getA()) + vpMath::sqr(plane.getB()) + vpMath::sqr(plane.getC()));
+ vpRotationMatrix wRc;
+ vpColVector x(3), y(3), z(3);
+ // X axis is P2-P1
+ x[0] = (p2.get_oX() - p1.get_oX()) / norm_X;
+ x[1] = (p2.get_oY() - p1.get_oY()) / norm_X;
+ x[2] = (p2.get_oZ() - p1.get_oZ()) / norm_X;
+ // Y axis is the normal of the plane
+ y[0] = plane.getA() / norm_Y;
+ y[1] = plane.getB() / norm_Y;
+ y[2] = plane.getC() / norm_Y;
+ // Z axis = X ^ Y
+ z = vpColVector::crossProd(x, y);
+ for (unsigned int i = 0; i < 3; i++) {
+ wRc[i][0] = x[i];
+ wRc[i][1] = y[i];
+ wRc[i][2] = z[i];
+ }
- vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
- vpHomogeneousMatrix wMc(wtc, wRc);
+ vpTranslationVector wtc(p1.get_oX(), p1.get_oY(), p1.get_oZ());
+ vpHomogeneousMatrix wMc(wtc, wRc);
- vpColVector c_p(4); // A point in the circle frame that is on the bbox
- c_p[0] = radius;
- c_p[1] = 0;
- c_p[2] = radius;
- c_p[3] = 1;
+ vpColVector c_p(4); // A point in the circle frame that is on the bbox
+ c_p[0] = radius;
+ c_p[1] = 0;
+ c_p[2] = radius;
+ c_p[3] = 1;
- // Matrix to rotate a point by 90 deg around Y in the circle frame
- for(unsigned int i=0; i<4; i++) {
- vpColVector w_p(4); // A point in the word frame
- vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0,vpMath::rad(90*i), 0));
- w_p = wMc * cMc_90 * c_p;
+ // Matrix to rotate a point by 90 deg around Y in the circle frame
+ for (unsigned int i = 0; i < 4; i++) {
+ vpColVector w_p(4); // A point in the word frame
+ vpHomogeneousMatrix cMc_90(vpTranslationVector(), vpRotationMatrix(0, vpMath::rad(90 * i), 0));
+ w_p = wMc * cMc_90 * c_p;
- vpPoint w_P;
- w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
+ vpPoint w_P;
+ w_P.setWorldCoordinates(w_p[0], w_p[1], w_p[2]);
- polygon.addPoint(i,w_P);
- }
+ polygon.addPoint(i, w_P);
}
+ }
- polygon.setIndex(idFace);
- faces.addPolygon(&polygon);
+ polygon.setIndex(idFace);
+ faces.addPolygon(&polygon);
- if(clippingFlag != vpPolygon3D::NO_CLIPPING)
- faces.getPolygon().back()->setClipping(clippingFlag);
+ if (clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
- if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
- faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+ if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
- if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
- faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+ 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
+ // A polygon as a single line that corresponds to the revolution axis of the
+ // cylinder
vpMbtPolygon polygon;
polygon.setNbPoint(2);
polygon.addPoint(0, p1);
polygon.addPoint(1, p2);
- polygon.setIndex(idFace) ;
+ polygon.setIndex(idFace);
polygon.setName(polygonName);
polygon.setLod(useLod);
-// //if(minLineLengthThreshold != -1.0) {
-// if(std::fabs(minLineLengthThreshold + 1.0) > std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon()) {
-// polygon.setMinLineLengthThresh(minLineLengthThreshold);
-// }
+ // //if(minLineLengthThreshold != -1.0) {
+ // if(std::fabs(minLineLengthThreshold + 1.0) >
+ // std::fabs(minLineLengthThreshold)*std::numeric_limits<double>::epsilon())
+ // {
+ // polygon.setMinLineLengthThresh(minLineLengthThreshold);
+ // }
polygon.setMinLineLengthThresh(minLineLengthThreshold);
- //Non sense to set minPolygonAreaThreshold for cylinder
- //but used to be coherent when applying LOD settings for all polygons
+ // Non sense to set minPolygonAreaThreshold for cylinder
+ // but used to be coherent when applying LOD settings for all polygons
polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
- faces.addPolygon(&polygon) ;
+ faces.addPolygon(&polygon);
- if(clippingFlag != vpPolygon3D::NO_CLIPPING)
+ 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);
}
-void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace, const std::string &polygonName,
- const bool useLod, const double minLineLengthThreshold)
+void vpMbTracker::addPolygon(const std::vector<std::vector<vpPoint> > &listFaces, const int idFace,
+ const std::string &polygonName, const bool useLod, const double minLineLengthThreshold)
{
- int id = idFace;
- for(unsigned int i = 0 ; i < listFaces.size() ; i++)
- {
- vpMbtPolygon polygon;
- polygon.setNbPoint((unsigned int)listFaces[i].size());
- for(unsigned int j = 0 ; j < listFaces[i].size() ; j++)
- polygon.addPoint(j, listFaces[i][j]);
+ int id = idFace;
+ for (unsigned int i = 0; i < listFaces.size(); i++) {
+ vpMbtPolygon polygon;
+ polygon.setNbPoint((unsigned int)listFaces[i].size());
+ for (unsigned int j = 0; j < listFaces[i].size(); j++)
+ polygon.addPoint(j, listFaces[i][j]);
- polygon.setIndex(id) ;
- polygon.setName(polygonName);
- polygon.setIsPolygonOriented(false);
- polygon.setLod(useLod);
- polygon.setMinLineLengthThresh(minLineLengthThreshold);
- polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
+ polygon.setIndex(id);
+ polygon.setName(polygonName);
+ polygon.setIsPolygonOriented(false);
+ polygon.setLod(useLod);
+ polygon.setMinLineLengthThresh(minLineLengthThreshold);
+ polygon.setMinPolygonAreaThresh(minPolygonAreaThresholdGeneral);
- faces.addPolygon(&polygon) ;
+ faces.addPolygon(&polygon);
- if(clippingFlag != vpPolygon3D::NO_CLIPPING)
- faces.getPolygon().back()->setClipping(clippingFlag);
+ if (clippingFlag != vpPolygon3D::NO_CLIPPING)
+ faces.getPolygon().back()->setClipping(clippingFlag);
- if((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
- faces.getPolygon().back()->setNearClippingDistance(distNearClip);
+ if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING)
+ faces.getPolygon().back()->setNearClippingDistance(distNearClip);
- if((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
- faces.getPolygon().back()->setFarClippingDistance(distFarClip);
+ if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING)
+ faces.getPolygon().back()->setFarClippingDistance(distFarClip);
- id++;
- }
+ id++;
+ }
}
/*!
Load a 3D model from the file in parameter. This file must either be a vrml
- file (.wrl) or a CAO file (.cao). CAO format is described in the
- loadCAOModel() method.
+ file (.wrl) or a CAO file (.cao). CAO format is described in the
+ loadCAOModel() method.
\warning When this class is called to load a vrml model, remember that you
have to call Call SoDD::finish() before ending the program.
@@ -1004,19 +1058,15 @@ int main()
}
\endcode
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
\param modelFile : the file containing the the 3D model description.
The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \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
@@ -1035,23 +1085,22 @@ int main()
}
\endcode
- \throw vpException::ioError if the file cannot be open, or if its extension is
- not wrl or cao.
+ \throw vpException::ioError if the file cannot be open, or if its extension
+is not wrl or cao.
\param modelFile : the file containing the the 3D model description.
The extension of this file is either .wrl or .cao.
- \param verbose : verbose option to print additional information when loading CAO model files which include other
- CAO model files.
+ \param verbose : verbose option to print additional information when loading
+CAO model files which include other CAO model files.
*/
-void
-vpMbTracker::loadModel(const std::string& modelFile, const bool verbose)
+void vpMbTracker::loadModel(const std::string &modelFile, const bool verbose)
{
std::string::const_iterator it;
-
- if(vpIoTools::checkFilename(modelFile)) {
+
+ 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<std::string> vectorOfModelFilename;
int startIdFace = (int)faces.size();
nbPoints = 0;
@@ -1061,27 +1110,23 @@ vpMbTracker::loadModel(const std::string& modelFile, const bool verbose)
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) == '.') ){
+ } 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{
+ } else {
throw vpException(vpException::ioError, "Error: File %s doesn't contain a cao or wrl model", modelFile.c_str());
}
- }
- else{
+ } else {
throw vpException(vpException::ioError, "Error: File %s doesn't exist", modelFile.c_str());
}
-
+
this->modelInitialised = true;
this->modelFileName = modelFile;
}
-
/*!
- Load the 3D model of the object from a vrml file. Only LineSet and FaceSet are
- extracted from the vrml file.
+ Load the 3D model of the object from a vrml file. Only LineSet and FaceSet
+are extracted from the vrml file.
\warning When this class is called, remember that you have to call Call
SoDD::finish() before ending the program.
@@ -1097,50 +1142,48 @@ int main()
\warning The cylinders extracted using this method do not use the Cylinder
keyword of vrml since vrml exporter such as Blender or AC3D consider a
- cylinder as an IndexedFaceSet. To test whether an indexedFaceSet is a cylinder
- or not, the name of the geometry is read. If the name begins with "cyl" then
- the faceset is supposed to be a cylinder. For example, the line
+ cylinder as an IndexedFaceSet. To test whether an indexedFaceSet is a
+cylinder or not, the name of the geometry is read. If the name begins with
+"cyl" then the faceset is supposed to be a cylinder. For example, the line
\code
geometry DEF cyl_cylinder1 IndexedFaceSet
\endcode
defines a cylinder named cyl_cylinder1.
- \throw vpException::fatalError if the file cannot be open.
-
+ \throw vpException::fatalError if the file cannot be open.
+
\param modelFile : The full name of the file containing the 3D model.
*/
-void
-vpMbTracker::loadVRMLModel(const std::string& modelFile)
+void vpMbTracker::loadVRMLModel(const std::string &modelFile)
{
#ifdef VISP_HAVE_COIN3D
SoDB::init(); // Call SoDB::finish() before ending the program.
SoInput in;
SbBool ok = in.openFile(modelFile.c_str());
- SoVRMLGroup *sceneGraphVRML2;
+ SoVRMLGroup *sceneGraphVRML2;
if (!ok) {
vpERROR_TRACE("can't open file to load model");
throw vpException(vpException::fatalError, "can't open file to load model");
}
- if(!in.isFileVRML2())
- {
- SoSeparator *sceneGraph = SoDB::readAll(&in);
- if (sceneGraph == NULL) { /*return -1;*/ }
+ if (!in.isFileVRML2()) {
+ SoSeparator *sceneGraph = SoDB::readAll(&in);
+ if (sceneGraph == NULL) { /*return -1;*/
+ }
sceneGraph->ref();
-
+
SoToVRML2Action tovrml2;
tovrml2.apply(sceneGraph);
-
- sceneGraphVRML2 =tovrml2.getVRML2SceneGraph();
+
+ sceneGraphVRML2 = tovrml2.getVRML2SceneGraph();
sceneGraphVRML2->ref();
sceneGraph->unref();
- }
- else
- {
- sceneGraphVRML2 = SoDB::readAllVRML(&in);
- if (sceneGraphVRML2 == NULL) { /*return -1;*/ }
+ } else {
+ sceneGraphVRML2 = SoDB::readAllVRML(&in);
+ if (sceneGraphVRML2 == NULL) { /*return -1;*/
+ }
sceneGraphVRML2->ref();
}
@@ -1149,7 +1192,7 @@ vpMbTracker::loadVRMLModel(const std::string& modelFile)
vpHomogeneousMatrix transform;
int indexFace = (int)faces.size();
extractGroup(sceneGraphVRML2, transform, indexFace);
-
+
sceneGraphVRML2->unref();
#else
vpERROR_TRACE("coin not detected with ViSP, cannot load model : %s", modelFile.c_str());
@@ -1157,41 +1200,43 @@ vpMbTracker::loadVRMLModel(const std::string& modelFile)
#endif
}
-void vpMbTracker::removeComment(std::ifstream& fileId) {
- char c;
-
- fileId.get(c);
- while (!fileId.fail() && (c == '#')) {
- fileId.ignore(256, '\n');
- fileId.get(c);
- }
- if (fileId.fail())
- throw(vpException(vpException::ioError, "Reached end of file"));
- fileId.unget();
+void vpMbTracker::removeComment(std::ifstream &fileId)
+{
+ char c;
+
+ fileId.get(c);
+ while (!fileId.fail() && (c == '#')) {
+ fileId.ignore(256, '\n');
+ fileId.get(c);
+ }
+ if (fileId.fail()) {
+ throw(vpException(vpException::ioError, "Reached end of file"));
+ }
+ fileId.unget();
}
-std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& endLine) {
+std::map<std::string, std::string> vpMbTracker::parseParameters(std::string &endLine)
+{
std::map<std::string, std::string> mapOfParams;
bool exit = false;
while (!endLine.empty() && !exit) {
exit = true;
- for (std::map<std::string, std::string>::const_iterator it =
- mapOfParameterNames.begin(); it != mapOfParameterNames.end();
- ++it) {
+ for (std::map<std::string, std::string>::const_iterator it = mapOfParameterNames.begin();
+ it != mapOfParameterNames.end(); ++it) {
endLine = trim(endLine);
// std::cout << "endLine=" << endLine << std::endl;
std::string param(it->first + "=");
- //Compare with a potential parameter
+ // Compare with a potential parameter
if (endLine.compare(0, param.size(), param) == 0) {
exit = false;
endLine = endLine.substr(param.size());
bool parseQuote = false;
if (it->second == "string") {
- //Check if the string is between quotes
+ // Check if the string is between quotes
if (endLine.size() > 2 && endLine[0] == '"') {
parseQuote = true;
endLine = endLine.substr(1);
@@ -1207,7 +1252,7 @@ std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& end
}
if (!parseQuote) {
- //Deal with space or tabulation after parameter value to substring
+ // Deal with space or tabulation after parameter value to substring
// to the next sequence
size_t pos1 = endLine.find_first_of(' ');
size_t pos2 = endLine.find_first_of('\t');
@@ -1220,19 +1265,16 @@ std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& end
}
}
-// for(std::map<std::string, std::string>::const_iterator it = mapOfParams.begin(); it != mapOfParams.end(); ++it) {
-// std::cout << it->first << "=" << it->second << std::endl;
-// }
-
return mapOfParams;
}
/*!
Load a 3D model contained in a *.cao file.
-
- Since ViSP 2.9.1, lines starting with # character are considered as comments.
- It is also possible to add comment at the end of the lines. No specific character is requested before the comment.
- In the following example we use "//" but it could be an other character.
+
+ Since ViSP 2.9.1, lines starting with # character are considered as
+ comments. It is also possible to add comment at the end of the lines. No
+ specific character is requested before the comment. In the following example
+ we use "//" but it could be an other character.
Since ViSP 2.9.1, circles are supported.
@@ -1247,7 +1289,7 @@ std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& end
# 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 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
@@ -1255,8 +1297,7 @@ std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& end
# 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
+ 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.)
@@ -1264,18 +1305,19 @@ std::map<std::string, std::string> vpMbTracker::parseParameters(std::string& end
1
0.5 0 1 2 // radius, index center point, index 2 other points on the plane containing the circle
\endcode
-
+
\param modelFile : Full name of the main *.cao file containing the model.
\param vectorOfModelFilename : A vector of *.cao files.
\param startIdFace : Current Id of the face.
- \param verbose : If true, will print additional information with CAO model files which include other CAO model files.
- \param parent : This parameter is set to true when parsing a parent CAO model file, and false when parsing an included
- CAO model file.
+ \param verbose : If true, will print additional information with CAO model
+ files which include other CAO model files.
+ \param parent : This parameter is
+ set to true when parsing a parent CAO model file, and false when parsing an
+ included CAO model file.
*/
-void
-vpMbTracker::loadCAOModel(const std::string& modelFile,
- std::vector<std::string>& vectorOfModelFilename, int& startIdFace,
- const bool verbose, const bool parent) {
+void vpMbTracker::loadCAOModel(const std::string &modelFile, std::vector<std::string> &vectorOfModelFilename,
+ int &startIdFace, const bool verbose, const bool parent)
+{
std::ifstream fileId;
fileId.exceptions(std::ifstream::failbit | std::ifstream::eofbit);
fileId.open(modelFile.c_str(), std::ifstream::in);
@@ -1284,62 +1326,63 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
throw vpException(vpException::ioError, "cannot read CAO model file");
}
- if(verbose) {
- std::cout << "Model file : " << modelFile << std::endl;
+ if (verbose) {
+ std::cout << "Model file : " << modelFile << std::endl;
}
vectorOfModelFilename.push_back(modelFile);
try {
char c;
- // Extraction of the version (remove empty line and commented ones (comment
- // line begin with the #)).
- //while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
+ // Extraction of the version (remove empty line and commented ones
+ // (comment line begin with the #)).
+ // while ((fileId.get(c) != NULL) && (c == '#')) fileId.ignore(256, '\n');
removeComment(fileId);
- //////////////////////////Read CAO Version (V1, V2, ...)//////////////////////////
+ //////////////////////////Read CAO Version (V1, V2,
+ ///...)//////////////////////////
int caoVersion;
fileId.get(c);
if (c == 'V') {
fileId >> caoVersion;
fileId.ignore(256, '\n'); // skip the rest of the line
} else {
- std::cout
- << "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...";
- throw vpException(vpException::badValue,
- "in vpMbTracker::loadCAOModel() -> Bad parameter header file : use V0, V1, ...");
+ std::cout << "in vpMbTracker::loadCAOModel() -> Bad parameter header "
+ "file : use V0, V1, ...";
+ throw vpException(vpException::badValue, "in vpMbTracker::loadCAOModel() -> Bad parameter "
+ "header file : use V0, V1, ...");
}
removeComment(fileId);
-
- //////////////////////////Read the header part if present//////////////////////////
+ //////////////////////////Read the header part if
+ /// present//////////////////////////
std::string line;
std::string prefix = "load";
fileId.get(c);
fileId.unget();
bool header = false;
- while(c == 'l' || c == 'L') {
+ while (c == 'l' || c == 'L') {
header = true;
getline(fileId, line);
- if(!line.compare(0, prefix.size(), prefix)) {
+ if (!line.compare(0, prefix.size(), prefix)) {
- //Get the loaded model pathname
+ // Get the loaded model pathname
std::string headerPathRead = line.substr(6);
size_t firstIndex = headerPathRead.find_first_of("\")");
headerPathRead = headerPathRead.substr(0, firstIndex);
std::string headerPath = headerPathRead;
- if(!vpIoTools::isAbsolutePathname(headerPathRead)) {
+ if (!vpIoTools::isAbsolutePathname(headerPathRead)) {
std::string parentDirectory = vpIoTools::getParent(modelFile);
headerPath = vpIoTools::createFilePath(parentDirectory, headerPathRead);
}
- //Normalize path
+ // Normalize path
headerPath = vpIoTools::path(headerPath);
- //Get real path
+ // Get real path
headerPath = vpIoTools::getAbsolutePathname(headerPath);
bool cyclic = false;
@@ -1357,8 +1400,8 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
throw vpException(vpException::ioError, "file cannot be open");
}
} else {
- std::cout << "WARNING Cyclic dependency detected with file "
- << headerPath << " declared in " << modelFile << std::endl;
+ std::cout << "WARNING Cyclic dependency detected with file " << headerPath << " declared in " << modelFile
+ << std::endl;
}
} else {
header = false;
@@ -1369,25 +1412,23 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId.unget();
}
-
- //////////////////////////Read the point declaration part//////////////////////////
+ //////////////////////////Read the point declaration
+ /// part//////////////////////////
unsigned int caoNbrPoint;
fileId >> caoNbrPoint;
fileId.ignore(256, '\n'); // skip the rest of the line
nbPoints += caoNbrPoint;
- if(verbose || vectorOfModelFilename.size() == 1) {
+ if (verbose || vectorOfModelFilename.size() == 1) {
std::cout << "> " << caoNbrPoint << " points" << std::endl;
}
if (caoNbrPoint > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points in the CAO model.");
+ 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];
@@ -1395,7 +1436,7 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
double y;
double z;
- int i; // image coordinate (used for matching)
+ int i; // image coordinate (used for matching)
int j;
for (unsigned int k = 0; k < caoNbrPoint; k++) {
@@ -1415,34 +1456,33 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
caoPoints[k].setWorldCoordinates(x, y, z);
}
-
removeComment(fileId);
-
- //////////////////////////Read the segment declaration part//////////////////////////
- //Store in a map the potential segments to add
- std::map<std::pair<unsigned int, unsigned int>, SegmentInfo > segmentTemporaryMap;
+ //////////////////////////Read the segment declaration
+ /// part//////////////////////////
+ // Store in a map the potential segments to add
+ std::map<std::pair<unsigned int, unsigned int>, SegmentInfo> segmentTemporaryMap;
unsigned int caoNbrLine;
fileId >> caoNbrLine;
fileId.ignore(256, '\n'); // skip the rest of the line
nbLines += caoNbrLine;
unsigned int *caoLinePoints = NULL;
- if(verbose || vectorOfModelFilename.size() == 1) {
+ if (verbose || vectorOfModelFilename.size() == 1) {
std::cout << "> " << caoNbrLine << " lines" << std::endl;
}
if (caoNbrLine > 100000) {
delete[] caoPoints;
- throw vpException(vpException::badValue,
- "Exceed the max number of lines in the CAO model.");
+ throw vpException(vpException::badValue, "Exceed the max number of lines in the CAO model.");
}
if (caoNbrLine > 0)
caoLinePoints = new unsigned int[2 * caoNbrLine];
unsigned int index1, index2;
- //Initialization of idFace with startIdFace for dealing with recursive load in header
+ // Initialization of idFace with startIdFace for dealing with recursive
+ // load in header
int idFace = startIdFace;
for (unsigned int k = 0; k < caoNbrLine; k++) {
@@ -1451,8 +1491,9 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId >> index1;
fileId >> index2;
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
+ //////////////////////////Read the parameter value if
+ /// present//////////////////////////
+ // Get the end of the line
char buffer[256];
fileId.getline(buffer, 256);
std::string endLine(buffer);
@@ -1461,13 +1502,13 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
std::string segmentName = "";
double minLineLengthThresh = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
- if(mapOfParams.find("name") != mapOfParams.end()) {
+ if (mapOfParams.find("name") != mapOfParams.end()) {
segmentName = mapOfParams["name"];
}
- if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
+ if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
minLineLengthThresh = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
}
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ if (mapOfParams.find("useLod") != mapOfParams.end()) {
useLod = parseBoolean(mapOfParams["useLod"]);
}
@@ -1495,25 +1536,26 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
removeComment(fileId);
-
- //////////////////////////Read the face segment declaration part//////////////////////////
- /* Load polygon from the lines extracted earlier (the first point of the line is used)*/
- //Store in a vector the indexes of the segments added in the face segment case
+ //////////////////////////Read the face segment declaration
+ /// part//////////////////////////
+ /* Load polygon from the lines extracted earlier (the first point of the
+ * line is used)*/
+ // Store in a vector the indexes of the segments added in the face segment
+ // case
std::vector<std::pair<unsigned int, unsigned int> > faceSegmentKeyVector;
unsigned int caoNbrPolygonLine;
fileId >> caoNbrPolygonLine;
fileId.ignore(256, '\n'); // skip the rest of the line
nbPolygonLines += caoNbrPolygonLine;
- if(verbose || vectorOfModelFilename.size() == 1) {
+ if (verbose || vectorOfModelFilename.size() == 1) {
std::cout << "> " << caoNbrPolygonLine << " polygon lines" << std::endl;
}
if (caoNbrPolygonLine > 100000) {
delete[] caoPoints;
delete[] caoLinePoints;
- throw vpException(vpException::badValue,
- "Exceed the max number of polygon lines.");
+ throw vpException(vpException::badValue, "Exceed the max number of polygon lines.");
}
unsigned int index;
@@ -1530,7 +1572,7 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
for (unsigned int n = 0; n < nbLinePol; n++) {
fileId >> index;
- if(index >= caoNbrLine) {
+ if (index >= caoNbrLine) {
throw vpException(vpException::badValue, "Exceed the max number of lines.");
}
corners.push_back(caoPoints[caoLinePoints[2 * index]]);
@@ -1540,9 +1582,9 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
faceSegmentKeyVector.push_back(key);
}
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
+ //////////////////////////Read the parameter value if
+ /// present//////////////////////////
+ // Get the end of the line
char buffer[256];
fileId.getline(buffer, 256);
std::string endLine(buffer);
@@ -1551,13 +1593,13 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
std::string polygonName = "";
bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
+ if (mapOfParams.find("name") != mapOfParams.end()) {
polygonName = mapOfParams["name"];
}
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
}
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ if (mapOfParams.find("useLod") != mapOfParams.end()) {
useLod = parseBoolean(mapOfParams["useLod"]);
}
@@ -1565,10 +1607,11 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
initFaceFromLines(*(faces.getPolygon().back())); // Init from the last polygon that was added
}
- //Add the segments which were not already added in the face segment case
- for(std::map<std::pair<unsigned int, unsigned int>, SegmentInfo >::const_iterator it =
- segmentTemporaryMap.begin(); it != segmentTemporaryMap.end(); ++it) {
- if(std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) == faceSegmentKeyVector.end()) {
+ // Add the segments which were not already added in the face segment case
+ for (std::map<std::pair<unsigned int, unsigned int>, SegmentInfo>::const_iterator it = segmentTemporaryMap.begin();
+ it != segmentTemporaryMap.end(); ++it) {
+ if (std::find(faceSegmentKeyVector.begin(), faceSegmentKeyVector.end(), it->first) ==
+ faceSegmentKeyVector.end()) {
addPolygon(it->second.extremities, idFace++, it->second.name, it->second.useLod, minPolygonAreaThresholdGeneral,
it->second.minLineLengthThresh);
initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
@@ -1577,22 +1620,20 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
removeComment(fileId);
-
- //////////////////////////Read the face point declaration part//////////////////////////
+ //////////////////////////Read the face point declaration
+ /// part//////////////////////////
/* Extract the polygon using the point coordinates (top of the file) */
unsigned int caoNbrPolygonPoint;
fileId >> caoNbrPolygonPoint;
fileId.ignore(256, '\n'); // skip the rest of the line
nbPolygonPoints += caoNbrPolygonPoint;
- if(verbose || vectorOfModelFilename.size() == 1) {
- std::cout << "> " << caoNbrPolygonPoint << " polygon points"
- << std::endl;
+ if (verbose || vectorOfModelFilename.size() == 1) {
+ std::cout << "> " << caoNbrPolygonPoint << " polygon points" << std::endl;
}
if (caoNbrPolygonPoint > 100000) {
- throw vpException(vpException::badValue,
- "Exceed the max number of polygon point.");
+ throw vpException(vpException::badValue, "Exceed the max number of polygon point.");
}
for (unsigned int k = 0; k < caoNbrPolygonPoint; k++) {
@@ -1601,22 +1642,20 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
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<vpPoint> corners;
for (unsigned int n = 0; n < nbPointPol; n++) {
fileId >> index;
if (index > caoNbrPoint - 1) {
- throw vpException(vpException::badValue,
- "Exceed the max number of points.");
+ throw vpException(vpException::badValue, "Exceed the max number of points.");
}
corners.push_back(caoPoints[index]);
}
-
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
+ //////////////////////////Read the parameter value if
+ /// present//////////////////////////
+ // Get the end of the line
char buffer[256];
fileId.getline(buffer, 256);
std::string endLine(buffer);
@@ -1625,27 +1664,28 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
std::string polygonName = "";
bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
+ if (mapOfParams.find("name") != mapOfParams.end()) {
polygonName = mapOfParams["name"];
}
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
}
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ if (mapOfParams.find("useLod") != mapOfParams.end()) {
useLod = parseBoolean(mapOfParams["useLod"]);
}
-
addPolygon(corners, idFace++, polygonName, useLod, minPolygonAreaThreshold, minLineLengthThresholdGeneral);
initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
}
- //////////////////////////Read the cylinder declaration part//////////////////////////
+ //////////////////////////Read the cylinder declaration
+ /// part//////////////////////////
unsigned int caoNbCylinder;
try {
removeComment(fileId);
- if (fileId.eof()) { // check if not at the end of the file (for old style files)
+ if (fileId.eof()) { // check if not at the end of the file (for old
+ // style files)
delete[] caoPoints;
delete[] caoLinePoints;
return;
@@ -1656,13 +1696,12 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId.ignore(256, '\n'); // skip the rest of the line
nbCylinders += caoNbCylinder;
- if(verbose || vectorOfModelFilename.size() == 1) {
+ if (verbose || vectorOfModelFilename.size() == 1) {
std::cout << "> " << caoNbCylinder << " cylinders" << std::endl;
}
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) {
@@ -1674,8 +1713,9 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId >> indexP2;
fileId >> radius;
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
+ //////////////////////////Read the parameter value if
+ /// present//////////////////////////
+ // Get the end of the line
char buffer[256];
fileId.getline(buffer, 256);
std::string endLine(buffer);
@@ -1684,13 +1724,13 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
std::string polygonName = "";
bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
double minLineLengthThreshold = !applyLodSettingInConfig ? minLineLengthThresholdGeneral : 50.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
+ if (mapOfParams.find("name") != mapOfParams.end()) {
polygonName = mapOfParams["name"];
}
- if(mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
+ if (mapOfParams.find("minLineLengthThreshold") != mapOfParams.end()) {
minLineLengthThreshold = std::atof(mapOfParams["minLineLengthThreshold"].c_str());
}
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ if (mapOfParams.find("useLod") != mapOfParams.end()) {
useLod = parseBoolean(mapOfParams["useLod"]);
}
@@ -1698,26 +1738,26 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
addPolygon(caoPoints[indexP1], caoPoints[indexP2], idFace++, polygonName, useLod, minLineLengthThreshold);
std::vector<std::vector<vpPoint> > listFaces;
- createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius,listFaces);
+ createCylinderBBox(caoPoints[indexP1], caoPoints[indexP2], radius, listFaces);
addPolygon(listFaces, idFace, polygonName, useLod, minLineLengthThreshold);
- idFace+=4;
+ idFace += 4;
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//////////////////////////
+ //////////////////////////Read the circle declaration
+ /// part//////////////////////////
unsigned int caoNbCircle;
try {
removeComment(fileId);
- if (fileId.eof()) { // check if not at the end of the file (for old style files)
+ if (fileId.eof()) { // check if not at the end of the file (for old
+ // style files)
delete[] caoPoints;
delete[] caoLinePoints;
return;
@@ -1728,13 +1768,12 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId.ignore(256, '\n'); // skip the rest of the line
nbCircles += caoNbCircle;
- if(verbose || vectorOfModelFilename.size() == 1) {
+ if (verbose || vectorOfModelFilename.size() == 1) {
std::cout << "> " << caoNbCircle << " circles" << std::endl;
}
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) {
@@ -1747,8 +1786,9 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
fileId >> indexP2;
fileId >> indexP3;
- //////////////////////////Read the parameter value if present//////////////////////////
- //Get the end of the line
+ //////////////////////////Read the parameter value if
+ /// present//////////////////////////
+ // Get the end of the line
char buffer[256];
fileId.getline(buffer, 256);
std::string endLine(buffer);
@@ -1757,26 +1797,24 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
std::string polygonName = "";
bool useLod = !applyLodSettingInConfig ? useLodGeneral : false;
double minPolygonAreaThreshold = !applyLodSettingInConfig ? minPolygonAreaThresholdGeneral : 2500.0;
- if(mapOfParams.find("name") != mapOfParams.end()) {
+ if (mapOfParams.find("name") != mapOfParams.end()) {
polygonName = mapOfParams["name"];
}
- if(mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
+ if (mapOfParams.find("minPolygonAreaThreshold") != mapOfParams.end()) {
minPolygonAreaThreshold = std::atof(mapOfParams["minPolygonAreaThreshold"].c_str());
}
- if(mapOfParams.find("useLod") != mapOfParams.end()) {
+ if (mapOfParams.find("useLod") != mapOfParams.end()) {
useLod = parseBoolean(mapOfParams["useLod"]);
}
- addPolygon(caoPoints[indexP1], caoPoints[indexP2],
- caoPoints[indexP3], radius, idFace, polygonName, useLod, minPolygonAreaThreshold);
+ 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;
}
@@ -1785,8 +1823,8 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
delete[] caoPoints;
delete[] caoLinePoints;
- if(vectorOfModelFilename.size() > 1 && parent) {
- if(verbose) {
+ if (vectorOfModelFilename.size() > 1 && parent) {
+ if (verbose) {
std::cout << "Global information for " << vpIoTools::getName(modelFile) << " :" << std::endl;
std::cout << "Total nb of points : " << nbPoints << std::endl;
std::cout << "Total nb of lines : " << nbLines << std::endl;
@@ -1811,86 +1849,85 @@ vpMbTracker::loadCAOModel(const std::string& modelFile,
#ifdef VISP_HAVE_COIN3D
/*!
- Extract a VRML object Group.
-
+ Extract a VRML object Group.
+
\param sceneGraphVRML2 : Current node (either Transform, or Group node).
\param transform : Transformation matrix for this group.
\param idFace : Index of the face.
*/
-void
-vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
-{
+void vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &transform, int &idFace)
+{
vpHomogeneousMatrix transformCur;
SoVRMLTransform *sceneGraphVRML2Trasnform = dynamic_cast<SoVRMLTransform *>(sceneGraphVRML2);
- if(sceneGraphVRML2Trasnform){
+ if (sceneGraphVRML2Trasnform) {
float rx, ry, rz, rw;
- sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx,ry,rz,rw);
- vpRotationMatrix rotMat(vpQuaternionVector(rx,ry,rz,rw));
-// std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " << rw << std::endl;
-
+ sceneGraphVRML2Trasnform->rotation.getValue().getValue(rx, ry, rz, rw);
+ vpRotationMatrix rotMat(vpQuaternionVector(rx, ry, rz, rw));
+ // std::cout << "Rotation: " << rx << " " << ry << " " << rz << " " <<
+ // rw << std::endl;
+
float tx, ty, tz;
tx = sceneGraphVRML2Trasnform->translation.getValue()[0];
ty = sceneGraphVRML2Trasnform->translation.getValue()[1];
tz = sceneGraphVRML2Trasnform->translation.getValue()[2];
- vpTranslationVector transVec(tx,ty,tz);
-// std::cout << "Translation: " << tx << " " << ty << " " << tz << std::endl;
-
+ vpTranslationVector transVec(tx, ty, tz);
+ // std::cout << "Translation: " << tx << " " << ty << " " << tz <<
+ // std::endl;
+
float sx, sy, sz;
sx = sceneGraphVRML2Trasnform->scale.getValue()[0];
sy = sceneGraphVRML2Trasnform->scale.getValue()[1];
sz = sceneGraphVRML2Trasnform->scale.getValue()[2];
-// std::cout << "Scale: " << sx << " " << sy << " " << sz << std::endl;
-
- for(unsigned int i = 0 ; i < 3 ; i++)
+ // std::cout << "Scale: " << sx << " " << sy << " " << sz <<
+ // std::endl;
+
+ for (unsigned int i = 0; i < 3; i++)
rotMat[0][i] *= sx;
- for(unsigned int i = 0 ; i < 3 ; i++)
+ for (unsigned int i = 0; i < 3; i++)
rotMat[1][i] *= sy;
- for(unsigned int i = 0 ; i < 3 ; i++)
+ for (unsigned int i = 0; i < 3; i++)
rotMat[2][i] *= sz;
-
- transformCur = vpHomogeneousMatrix(transVec,rotMat);
+
+ transformCur = vpHomogeneousMatrix(transVec, rotMat);
transform = transform * transformCur;
}
-
+
int nbShapes = sceneGraphVRML2->getNumChildren();
-// std::cout << sceneGraphVRML2->getTypeId().getName().getString() << std::endl;
-// std::cout << "Nb object in VRML : " << nbShapes << std::endl;
-
- SoNode * child;
-
- for (int i = 0; i < nbShapes; i++)
- {
+ // std::cout << sceneGraphVRML2->getTypeId().getName().getString() <<
+ // std::endl; std::cout << "Nb object in VRML : " << nbShapes <<
+ // std::endl;
+
+ SoNode *child;
+
+ for (int i = 0; i < nbShapes; i++) {
vpHomogeneousMatrix transform_recursive(transform);
child = sceneGraphVRML2->getChild(i);
-
- if (child->getTypeId() == SoVRMLGroup::getClassTypeId()){
- extractGroup((SoVRMLGroup*)child, transform_recursive, idFace);
+
+ if (child->getTypeId() == SoVRMLGroup::getClassTypeId()) {
+ extractGroup((SoVRMLGroup *)child, transform_recursive, idFace);
}
-
- if (child->getTypeId() == SoVRMLTransform::getClassTypeId()){
- extractGroup((SoVRMLTransform*)child, transform_recursive, idFace);
+
+ if (child->getTypeId() == SoVRMLTransform::getClassTypeId()) {
+ extractGroup((SoVRMLTransform *)child, transform_recursive, idFace);
}
-
- if (child->getTypeId() == SoVRMLShape::getClassTypeId()){
- SoChildList * child2list = child->getChildren();
+
+ if (child->getTypeId() == SoVRMLShape::getClassTypeId()) {
+ SoChildList *child2list = child->getChildren();
std::string name = child->getName().getString();
- for (int j = 0; j < child2list->getLength(); j++)
- {
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId())
- {
- SoVRMLIndexedFaceSet * face_set;
- face_set = (SoVRMLIndexedFaceSet*)child2list->get(j);
- if(!strncmp(face_set->getName().getString(),"cyl",3)){
+ for (int j = 0; j < child2list->getLength(); j++) {
+ if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedFaceSet::getClassTypeId()) {
+ SoVRMLIndexedFaceSet *face_set;
+ face_set = (SoVRMLIndexedFaceSet *)child2list->get(j);
+ if (!strncmp(face_set->getName().getString(), "cyl", 3)) {
extractCylinders(face_set, transform, idFace, name);
- }else{
+ } else {
extractFaces(face_set, transform, idFace, name);
}
}
- if (((SoNode*)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId())
- {
- SoVRMLIndexedLineSet * line_set;
- line_set = (SoVRMLIndexedLineSet*)child2list->get(j);
+ if (((SoNode *)child2list->get(j))->getTypeId() == SoVRMLIndexedLineSet::getClassTypeId()) {
+ SoVRMLIndexedLineSet *line_set;
+ line_set = (SoVRMLIndexedLineSet *)child2list->get(j);
extractLines(line_set, idFace, name);
}
}
@@ -1900,50 +1937,45 @@ vpMbTracker::extractGroup(SoVRMLGroup *sceneGraphVRML2, vpHomogeneousMatrix &tra
/*!
Extract a face of the object to track from the VMRL model. This method calls
- the initFaceFromCorners() method implemented in the child class.
+ the initFaceFromCorners() method implemented in the child class.
\param face_set : Pointer to the face in the vrml format.
\param transform : Transformation matrix applied to the face.
\param idFace : Face id.
\param polygonName: Name of the polygon.
*/
-void
-vpMbTracker::extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName)
+void vpMbTracker::extractFaces(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
+ const std::string &polygonName)
{
std::vector<vpPoint> corners;
corners.resize(0);
-// SoMFInt32 indexList = _face_set->coordIndex;
-// int indexListSize = indexList.getNum();
+ // SoMFInt32 indexList = _face_set->coordIndex;
+ // int indexListSize = indexList.getNum();
int indexListSize = face_set->coordIndex.getNum();
-
+
vpColVector pointTransformed(4);
vpPoint pt;
SoVRMLCoordinate *coord;
-
- for (int i = 0; i < indexListSize; i++)
- {
- if (face_set->coordIndex[i] == -1)
- {
- if(corners.size() > 1)
- {
+
+ for (int i = 0; i < indexListSize; i++) {
+ if (face_set->coordIndex[i] == -1) {
+ if (corners.size() > 1) {
addPolygon(corners, idFace++, polygonName);
initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
corners.resize(0);
}
- }
- else
- {
+ } else {
coord = (SoVRMLCoordinate *)(face_set->coord.getValue());
int index = face_set->coordIndex[i];
- pointTransformed[0]=coord->point[index].getValue()[0];
- pointTransformed[1]=coord->point[index].getValue()[1];
- pointTransformed[2]=coord->point[index].getValue()[2];
+ pointTransformed[0] = coord->point[index].getValue()[0];
+ pointTransformed[1] = coord->point[index].getValue()[1];
+ pointTransformed[2] = coord->point[index].getValue()[2];
pointTransformed[3] = 1.0;
-
+
pointTransformed = transform * pointTransformed;
-
- pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
+
+ pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
corners.push_back(pt);
}
}
@@ -1953,25 +1985,27 @@ vpMbTracker::extractFaces(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &t
Extract a cylinder to track from the VMRL model. This method calls
the initCylinder() method implemented in the child class.
- \warning This method extract cylinder described using an indexed face set not
- a cylinder set since software such as AC3D or blender export a cylinder using
- this data type. the object name is used, if it begins with "cyl" then this
- method is called otherwise the extractFaces() is used.
+ \warning This method extract cylinder described using an indexed face set
+ not a cylinder set since software such as AC3D or blender export a cylinder
+ using this data type. the object name is used, if it begins with "cyl" then
+ this method is called otherwise the extractFaces() is used.
\param face_set : Pointer to the cylinder in the vrml format.
\param transform : Transformation matrix applied to the cylinder.
\param idFace : Id of the face.
\param polygonName: Name of the polygon.
*/
-void
-vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatrix &transform, int &idFace, const std::string &polygonName)
+void vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet *face_set, vpHomogeneousMatrix &transform, int &idFace,
+ const std::string &polygonName)
{
- std::vector<vpPoint> corners_c1, corners_c2;//points belonging to the first circle and to the second one.
- SoVRMLCoordinate* coords = (SoVRMLCoordinate *)face_set->coord.getValue();
+ std::vector<vpPoint> corners_c1, corners_c2; // points belonging to the
+ // first circle and to the
+ // second one.
+ SoVRMLCoordinate *coords = (SoVRMLCoordinate *)face_set->coord.getValue();
unsigned int indexListSize = (unsigned int)coords->point.getNum();
- if(indexListSize % 2 == 1){
+ 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.");
}
@@ -1980,23 +2014,22 @@ vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatri
vpColVector pointTransformed(4);
vpPoint pt;
-
// extract all points and fill the two sets.
- for(int i=0; i<coords->point.getNum(); ++i){
- pointTransformed[0]=coords->point[i].getValue()[0];
- pointTransformed[1]=coords->point[i].getValue()[1];
- pointTransformed[2]=coords->point[i].getValue()[2];
+ for (int i = 0; i < coords->point.getNum(); ++i) {
+ pointTransformed[0] = coords->point[i].getValue()[0];
+ pointTransformed[1] = coords->point[i].getValue()[1];
+ pointTransformed[2] = coords->point[i].getValue()[2];
pointTransformed[3] = 1.0;
-
+
pointTransformed = transform * pointTransformed;
-
- pt.setWorldCoordinates(pointTransformed[0],pointTransformed[1],pointTransformed[2]);
- if(i < (int)corners_c1.size()){
+ pt.setWorldCoordinates(pointTransformed[0], pointTransformed[1], pointTransformed[2]);
+
+ if (i < (int)corners_c1.size()) {
corners_c1[(unsigned int)i] = pt;
- }else{
- corners_c2[(unsigned int)i-corners_c1.size()] = pt;
+ } else {
+ corners_c2[(unsigned int)i - corners_c1.size()] = pt;
}
}
@@ -2013,13 +2046,14 @@ vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatri
dist[2] = p2.get_oZ() - corners_c2[0].get_oZ();
double radius_c2 = sqrt(dist.sumSquare());
- if(std::fabs(radius_c1 - radius_c2) > (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))){
+ if (std::fabs(radius_c1 - radius_c2) >
+ (std::numeric_limits<double>::epsilon() * vpMath::maximum(radius_c1, radius_c2))) {
std::cout << "Radius from the two circles of the cylinders are different." << std::endl;
throw vpException(vpException::badValue, "Radius from the two circles of the cylinders are different.");
}
- //addPolygon(p1, p2, idFace, polygonName);
- //initCylinder(p1, p2, radius_c1, idFace++);
+ // addPolygon(p1, p2, idFace, polygonName);
+ // initCylinder(p1, p2, radius_c1, idFace++);
int idRevolutionAxis = idFace;
addPolygon(p1, p2, idFace++, polygonName);
@@ -2027,51 +2061,45 @@ vpMbTracker::extractCylinders(SoVRMLIndexedFaceSet* face_set, vpHomogeneousMatri
std::vector<std::vector<vpPoint> > listFaces;
createCylinderBBox(p1, p2, radius_c1, listFaces);
addPolygon(listFaces, idFace, polygonName);
- idFace+=4;
+ idFace += 4;
initCylinder(p1, p2, radius_c1, idRevolutionAxis, polygonName);
}
/*!
Extract a line of the object to track from the VMRL model. This method calls
- the initFaceFromCorners() method implemented in the child class.
+ the initFaceFromCorners() method implemented in the child class.
\param line_set : Pointer to the line in the vrml format.
\param idFace : Id of the face.
\param polygonName: Name of the polygon.
*/
-void
-vpMbTracker::extractLines(SoVRMLIndexedLineSet* line_set, int &idFace, const std::string &polygonName)
+void vpMbTracker::extractLines(SoVRMLIndexedLineSet *line_set, int &idFace, const std::string &polygonName)
{
std::vector<vpPoint> corners;
corners.resize(0);
int indexListSize = line_set->coordIndex.getNum();
- SbVec3f point(0,0,0);
+ SbVec3f point(0, 0, 0);
vpPoint pt;
SoVRMLCoordinate *coord;
-
- for (int i = 0; i < indexListSize; i++)
- {
- if (line_set->coordIndex[i] == -1)
- {
- if(corners.size() > 1)
- {
+
+ for (int i = 0; i < indexListSize; i++) {
+ if (line_set->coordIndex[i] == -1) {
+ if (corners.size() > 1) {
addPolygon(corners, idFace++, polygonName);
initFaceFromCorners(*(faces.getPolygon().back())); // Init from the last polygon that was added
corners.resize(0);
}
- }
- else
- {
+ } else {
coord = (SoVRMLCoordinate *)(line_set->coord.getValue());
int index = line_set->coordIndex[i];
- point[0]=coord->point[index].getValue()[0];
- point[1]=coord->point[index].getValue()[1];
- point[2]=coord->point[index].getValue()[2];
+ point[0] = coord->point[index].getValue()[0];
+ point[1] = coord->point[index].getValue()[1];
+ point[2] = coord->point[index].getValue()[2];
- pt.setWorldCoordinates(point[0],point[1],point[2]);
+ pt.setWorldCoordinates(point[0], point[1], point[2]);
corners.push_back(pt);
}
}
@@ -2080,18 +2108,17 @@ vpMbTracker::extractLines(SoVRMLIndexedLineSet* line_set, int &idFace, const std
#endif // VISP_HAVE_COIN3D
/*!
- Compute the center of gravity of a set of point. This is used in the cylinder
- extraction to find the center of the circles.
+ Compute the center of gravity of a set of point. This is used in the
+ cylinder extraction to find the center of the circles.
\throw vpException::dimensionError if the set is empty.
\param pts : Set of point to extract the center of gravity.
\return Center of gravity of the set.
*/
-vpPoint
-vpMbTracker::getGravityCenter(const std::vector<vpPoint>& pts) const
+vpPoint vpMbTracker::getGravityCenter(const std::vector<vpPoint> &pts) const
{
- if(pts.empty()){
+ 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.");
}
@@ -2100,40 +2127,42 @@ vpMbTracker::getGravityCenter(const std::vector<vpPoint>& pts) const
double oZ = 0;
vpPoint G;
- for(unsigned int i=0; i<pts.size(); ++i){
+ for (unsigned int i = 0; i < pts.size(); ++i) {
oX += pts[i].get_oX();
oY += pts[i].get_oY();
oZ += pts[i].get_oZ();
}
- G.setWorldCoordinates(oX/pts.size(), oY/pts.size(), oZ/pts.size());
+ G.setWorldCoordinates(oX / pts.size(), oY / pts.size(), oZ / pts.size());
return G;
}
/*!
- Get the list of polygons faces (a vpPolygon representing the projection of the face in the image and a list of face corners
- in 3D), with the possibility to order by distance to the camera or to use the visibility check to consider if the polygon
- face must be retrieved or not.
-
- \param orderPolygons : If true, the resulting list is ordered from the nearest polygon faces to the farther.
- \param useVisibility : If true, only visible faces will be retrieved.
- \param clipPolygon : If true, the polygons will be clipped according to the clipping flags set in vpMbTracker.
- \return A pair object containing the list of vpPolygon and the list of face corners.
+ Get the list of polygons faces (a vpPolygon representing the projection of
+ the face in the image and a list of face corners in 3D), with the
+ possibility to order by distance to the camera or to use the visibility
+ check to consider if the polygon face must be retrieved or not.
+
+ \param orderPolygons : If true, the resulting list is ordered from the
+ nearest polygon faces to the farther. \param useVisibility : If true, only
+ visible faces will be retrieved. \param clipPolygon : If true, the polygons
+ will be clipped according to the clipping flags set in vpMbTracker. \return
+ A pair object containing the list of vpPolygon and the list of face corners.
*/
std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > >
vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility, const bool clipPolygon)
{
- //Temporary variable to permit to order polygons by distance
+ // Temporary variable to permit to order polygons by distance
std::vector<vpPolygon> polygonsTmp;
std::vector<std::vector<vpPoint> > roisPtTmp;
- //Pair containing the list of vpPolygon and the list of face corners
+ // Pair containing the list of vpPolygon and the list of face corners
std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pairOfPolygonFaces;
for (unsigned int i = 0; i < faces.getPolygon().size(); i++) {
- //A face has at least 3 points
+ // 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<vpImagePoint> roiPts;
if (clipPolygon) {
@@ -2161,12 +2190,12 @@ vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility,
}
}
- if(orderPolygons) {
- //Order polygons by distance (near to far)
+ if (orderPolygons) {
+ // Order polygons by distance (near to far)
std::vector<PolygonFaceInfo> listOfPolygonFaces;
- for(unsigned int i = 0; i < polygonsTmp.size(); i++) {
+ for (unsigned int i = 0; i < polygonsTmp.size(); i++) {
double x_centroid = 0.0, y_centroid = 0.0, z_centroid = 0.0;
- for(unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
+ for (unsigned int j = 0; j < roisPtTmp[i].size(); j++) {
x_centroid += roisPtTmp[i][j].get_X();
y_centroid += roisPtTmp[i][j].get_Y();
z_centroid += roisPtTmp[i][j].get_Z();
@@ -2176,19 +2205,19 @@ vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility,
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;
+ 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
+ // Sort the list of polygon faces
std::sort(listOfPolygonFaces.begin(), listOfPolygonFaces.end());
polygonsTmp.resize(listOfPolygonFaces.size());
roisPtTmp.resize(listOfPolygonFaces.size());
size_t cpt = 0;
- for(std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
- ++it, cpt++) {
+ for (std::vector<PolygonFaceInfo>::const_iterator it = listOfPolygonFaces.begin(); it != listOfPolygonFaces.end();
+ ++it, cpt++) {
polygonsTmp[cpt] = it->polygon;
roisPtTmp[cpt] = it->faceCorners;
}
@@ -2206,18 +2235,20 @@ vpMbTracker::getPolygonFaces(const bool orderPolygons, const bool useVisibility,
/*!
Use Ogre3D for visibility tests
- \warning This function has to be called before the initialization of the tracker.
+ \warning This function has to be called before the initialization of the
+ tracker.
\param v : True to use it, False otherwise
*/
-void
-vpMbTracker::setOgreVisibilityTest(const bool &v)
+void vpMbTracker::setOgreVisibilityTest(const bool &v)
{
useOgre = v;
- if(useOgre){
+ if (useOgre) {
#ifndef VISP_HAVE_OGRE
useOgre = false;
- std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test will be used. setOgreVisibilityTest() set to false." << std::endl;
+ std::cout << "WARNING: ViSP doesn't have Ogre3D, basic visibility test "
+ "will be used. setOgreVisibilityTest() set to false."
+ << std::endl;
#endif
}
}
@@ -2227,17 +2258,18 @@ vpMbTracker::setOgreVisibilityTest(const bool &v)
\param dist : Far clipping value.
*/
-void
-vpMbTracker::setFarClippingDistance(const double &dist)
+void vpMbTracker::setFarClippingDistance(const double &dist)
{
- if( (clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
- vpTRACE("Far clipping value cannot be inferior than near clipping value. Far clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Far clipping value cannot be inferior than 0. Far clipping won't be considered.");
- else{
+ if ((clippingFlag & vpPolygon3D::NEAR_CLIPPING) == vpPolygon3D::NEAR_CLIPPING && dist <= distNearClip)
+ vpTRACE("Far clipping value cannot be inferior than near clipping value. "
+ "Far clipping won't be considered.");
+ else if (dist < 0)
+ vpTRACE("Far clipping value cannot be inferior than 0. Far clipping "
+ "won't be considered.");
+ else {
clippingFlag = (clippingFlag | vpPolygon3D::FAR_CLIPPING);
distFarClip = dist;
- for (unsigned int i = 0; i < faces.size(); i ++){
+ for (unsigned int i = 0; i < faces.size(); i++) {
faces[i]->setFarClippingDistance(distFarClip);
}
#ifdef VISP_HAVE_OGRE
@@ -2249,37 +2281,35 @@ vpMbTracker::setFarClippingDistance(const double &dist)
/*!
Set the flag to consider if the level of detail (LOD) is used.
- \param useLod : true if the level of detail must be used, false otherwise. When true,
- two parameters can be set, see setMinLineLengthThresh() and setMinPolygonAreaThresh().
- \param name : name of the face we want to modify the LOD parameter.
+ \param useLod : true if the level of detail must be used, false otherwise.
+ When true, two parameters can be set, see setMinLineLengthThresh() and
+ setMinPolygonAreaThresh(). \param name : name of the face we want to modify
+ the LOD parameter.
\sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
*/
-void
-vpMbTracker::setLod(const bool useLod, const std::string &name)
+void vpMbTracker::setLod(const bool useLod, const std::string &name)
{
- for (unsigned int i = 0; i < faces.size(); i++)
- {
- if(name.empty() || faces[i]->name == name) {
+ for (unsigned int i = 0; i < faces.size(); i++) {
+ if (name.empty() || faces[i]->name == name) {
faces[i]->setLod(useLod);
}
}
}
/*!
- Set the threshold for the minimum line length to be considered as visible in the LOD case.
+ Set the threshold for the minimum line length to be considered as visible
+ in the LOD case.
- \param minLineLengthThresh : threshold for the minimum line length in pixel.
- \param name : name of the face we want to modify the LOD threshold.
+ \param minLineLengthThresh : threshold for the minimum line length in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinPolygonAreaThresh()
*/
-void
-vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std::string &name)
+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) {
+ for (unsigned int i = 0; i < faces.size(); i++) {
+ if (name.empty() || faces[i]->name == name) {
faces[i]->setMinLineLengthThresh(minLineLengthThresh);
}
}
@@ -2288,17 +2318,15 @@ vpMbTracker::setMinLineLengthThresh(const double minLineLengthThresh, const std:
/*!
Set the minimum polygon area to be considered as visible in the LOD case.
- \param minPolygonAreaThresh : threshold for the minimum polygon area in pixel.
- \param name : name of the face we want to modify the LOD threshold.
+ \param minPolygonAreaThresh : threshold for the minimum polygon area in
+ pixel. \param name : name of the face we want to modify the LOD threshold.
\sa setLod(), setMinLineLengthThresh()
*/
-void
-vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const std::string &name)
+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) {
+ for (unsigned int i = 0; i < faces.size(); i++) {
+ if (name.empty() || faces[i]->name == name) {
faces[i]->setMinPolygonAreaThresh(minPolygonAreaThresh);
}
}
@@ -2309,17 +2337,18 @@ vpMbTracker::setMinPolygonAreaThresh(const double minPolygonAreaThresh, const st
\param dist : Near clipping value.
*/
-void
-vpMbTracker::setNearClippingDistance(const double &dist)
+void vpMbTracker::setNearClippingDistance(const double &dist)
{
- if( (clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
- vpTRACE("Near clipping value cannot be superior than far clipping value. Near clipping won't be considered.");
- else if ( dist < 0 )
- vpTRACE("Near clipping value cannot be inferior than 0. Near clipping won't be considered.");
- else{
+ if ((clippingFlag & vpPolygon3D::FAR_CLIPPING) == vpPolygon3D::FAR_CLIPPING && dist >= distFarClip)
+ vpTRACE("Near clipping value cannot be superior than far clipping value. "
+ "Near clipping won't be considered.");
+ else if (dist < 0)
+ vpTRACE("Near clipping value cannot be inferior than 0. Near clipping "
+ "won't be considered.");
+ else {
clippingFlag = (clippingFlag | vpPolygon3D::NEAR_CLIPPING);
distNearClip = dist;
- for (unsigned int i = 0; i < faces.size(); i ++){
+ for (unsigned int i = 0; i < faces.size(); i++) {
faces[i]->setNearClippingDistance(distNearClip);
}
#ifdef VISP_HAVE_OGRE
@@ -2335,48 +2364,184 @@ vpMbTracker::setNearClippingDistance(const double &dist)
\param flags : New clipping flags.
*/
-void
-vpMbTracker::setClipping(const unsigned int &flags)
+void vpMbTracker::setClipping(const unsigned int &flags)
{
clippingFlag = flags;
- for (unsigned int i = 0; i < faces.size(); i ++)
+ for (unsigned int i = 0; i < faces.size(); i++)
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)
+{
+ if (computeCovariance) {
+ vpMatrix D;
+ D.diag(w_true);
+
+ // Note that here the covariance is computed on cMoPrev for time
+ // computation efficiency
+ if (isoJoIdentity_) {
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, L_true, D);
+ } else {
+ covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, error, LVJ_true, D);
+ }
+ }
+}
+
/*!
- Compute \f$ J^T R \f$, with J the interaction matrix and R the vector of
+ Compute \f$ J^T R \f$, with J the interaction matrix and R the vector of
residu.
-
- \throw vpMatrixException::incorrectMatrixSizeError if the sizes of the
+
+ \throw vpMatrixException::incorrectMatrixSizeError if the sizes of the
matrices do not allow the computation.
-
+
\warning The JTR vector is resized.
-
+
\param interaction : The interaction matrix (size Nx6).
\param error : The residu vector (size Nx1).
\param JTR : The resulting JTR column vector (size 6x1).
-
*/
-void
-vpMbTracker::computeJTR(const vpMatrix& interaction, const vpColVector& error, vpColVector& JTR) 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);
- const unsigned int N = interaction.getRows();
+ JTR.resize(6, false);
+
+ bool checkSSE2 = vpCPUFeatures::checkSSE2();
+#if !VISP_HAVE_SSE2
+ checkSSE2 = false;
+#endif
+
+ if (checkSSE2) {
+#if VISP_HAVE_SSE2
+ __m128d v_JTR_0_1 = _mm_setzero_pd();
+ __m128d v_JTR_2_3 = _mm_setzero_pd();
+ __m128d v_JTR_4_5 = _mm_setzero_pd();
+
+ for (unsigned int i = 0; i < interaction.getRows(); i++) {
+ const __m128d v_error = _mm_set1_pd(error[i]);
+
+ __m128d v_interaction = _mm_loadu_pd(&interaction[i][0]);
+ v_JTR_0_1 = _mm_add_pd(v_JTR_0_1, _mm_mul_pd(v_interaction, v_error));
- for (unsigned int i = 0; i < 6; i += 1){
- double ssum = 0;
- for (unsigned int j = 0; j < N; j += 1){
- ssum += interaction[j][i] * error[j];
+ v_interaction = _mm_loadu_pd(&interaction[i][2]);
+ v_JTR_2_3 = _mm_add_pd(v_JTR_2_3, _mm_mul_pd(v_interaction, v_error));
+
+ v_interaction = _mm_loadu_pd(&interaction[i][4]);
+ v_JTR_4_5 = _mm_add_pd(v_JTR_4_5, _mm_mul_pd(v_interaction, v_error));
+ }
+
+ _mm_storeu_pd(JTR.data, v_JTR_0_1);
+ _mm_storeu_pd(JTR.data + 2, v_JTR_2_3);
+ _mm_storeu_pd(JTR.data + 4, v_JTR_4_5);
+#endif
+ } else {
+ const unsigned int N = interaction.getRows();
+
+ for (unsigned int i = 0; i < 6; i += 1) {
+ double ssum = 0;
+ for (unsigned int j = 0; j < N; j += 1) {
+ ssum += interaction[j][i] * error[j];
+ }
+ JTR[i] = ssum;
+ }
+ }
+}
+
+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()) {
+ mu *= 10.0;
+
+ if (mu > 1.0)
+ throw vpTrackingException(vpTrackingException::fatalError, "Optimization diverged");
+
+ cMo = cMoPrev;
+ error = m_error_prev;
+ if (w != NULL && m_w_prev != NULL) {
+ *w = *m_w_prev;
+ }
+ reStartFromLastIncrement = true;
+ }
+ }
+}
+
+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();
+ computeJTR(L, R, LTR);
+
+ switch (m_optimizationMethod) {
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: {
+ vpMatrix LMA(LTL.getRows(), LTL.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LTL + (LMA * mu);
+ v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
+
+ if (iter != 0)
+ mu /= 10.0;
+
+ error_prev = error;
+ if (w != NULL && m_w_prev != NULL)
+ *m_w_prev = *w;
+ break;
+ }
+
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ v = -m_lambda * LTL.pseudoInverse(LTL.getRows() * std::numeric_limits<double>::epsilon()) * LTR;
+ break;
+ }
+ } else {
+ vpVelocityTwistMatrix cVo;
+ cVo.buildFrom(cMo);
+ vpMatrix LVJ = (L * (cVo * oJo));
+ vpMatrix LVJTLVJ = (LVJ).AtA();
+ vpColVector LVJTR;
+ computeJTR(LVJ, R, LVJTR);
+
+ switch (m_optimizationMethod) {
+ case vpMbTracker::LEVENBERG_MARQUARDT_OPT: {
+ vpMatrix LMA(LVJTLVJ.getRows(), LVJTLVJ.getCols());
+ LMA.eye();
+ vpMatrix LTLmuI = LVJTLVJ + (LMA * mu);
+ v = -m_lambda * LTLmuI.pseudoInverse(LTLmuI.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
+ v = cVo * v;
+
+ if (iter != 0)
+ mu /= 10.0;
+
+ error_prev = error;
+ if (w != NULL && m_w_prev != NULL)
+ *m_w_prev = *w;
+ break;
+ }
+ case vpMbTracker::GAUSS_NEWTON_OPT:
+ default:
+ v = -m_lambda * LVJTLVJ.pseudoInverse(LVJTLVJ.getRows() * std::numeric_limits<double>::epsilon()) * LVJTR;
+ v = cVo * v;
+ break;
}
- JTR[i] = ssum;
}
}
+void vpMbTracker::computeVVSWeights(vpRobust &robust, const vpColVector &error, vpColVector &w)
+{
+ if (error.getRows() > 0)
+ robust.MEstimator(vpRobust::TUKEY, error, w);
+}
+
/*!
Get a 1x6 vpColVector representing the estimated degrees of freedom.
vpColVector[0] = 1 if translation on X is estimated, 0 otherwise;
@@ -2388,20 +2553,21 @@ vpMbTracker::computeJTR(const vpMatrix& interaction, const vpColVector& error, v
\return 1x6 vpColVector representing the estimated degrees of freedom.
*/
-vpColVector
-vpMbTracker::getEstimatedDoF() const
+vpColVector vpMbTracker::getEstimatedDoF() const
{
- vpColVector v(6);
- for(unsigned int i = 0 ; i < 6 ; i++)
- v[i] = oJo[i][i];
- return v;
+ vpColVector v(6);
+ for (unsigned int i = 0; i < 6; i++)
+ v[i] = oJo[i][i];
+ return v;
}
/*!
- Set a 6-dim column vector representing the degrees of freedom in the object frame
- that are estimated by the tracker. When set to 1, all the 6 dof are estimated.
+ Set a 6-dim column vector representing the degrees of freedom in the object
+ frame that are estimated by the tracker. When set to 1, all the 6 dof are
+ estimated.
- Below we give the correspondance between the index of the vector and the considered dof:
+ Below we give the correspondance between the index of the vector and the
+ considered dof:
- v[0] = 1 if translation along X is estimated, 0 otherwise;
- v[1] = 1 if translation along Y is estimated, 0 otherwise;
- v[2] = 1 if translation along Z is estimated, 0 otherwise;
@@ -2410,115 +2576,108 @@ vpMbTracker::getEstimatedDoF() const
- v[5] = 1 if rotation along Z is estimated, 0 otherwise;
*/
-void
-vpMbTracker::setEstimatedDoF(const vpColVector& v)
+void vpMbTracker::setEstimatedDoF(const vpColVector &v)
{
- if(v.getRows() == 6)
- {
- isoJoIdentity = true;
- for(unsigned int i = 0 ; i < 6 ; i++){
- // if(v[i] != 0){
- if(std::fabs(v[i]) > std::numeric_limits<double>::epsilon()){
- oJo[i][i] = 1.0;
- }
- else{
- oJo[i][i] = 0.0;
- isoJoIdentity = false;
- }
- }
+ if (v.getRows() == 6) {
+ isoJoIdentity = true;
+ for (unsigned int i = 0; i < 6; i++) {
+ // if(v[i] != 0){
+ if (std::fabs(v[i]) > std::numeric_limits<double>::epsilon()) {
+ oJo[i][i] = 1.0;
+ } else {
+ oJo[i][i] = 0.0;
+ isoJoIdentity = false;
+ }
}
+ }
}
-
-void
-vpMbTracker::createCylinderBBox(const vpPoint& p1, const vpPoint &p2, const double &radius, std::vector<std::vector<vpPoint> > &listFaces)
+void vpMbTracker::createCylinderBBox(const vpPoint &p1, const vpPoint &p2, const double &radius,
+ std::vector<std::vector<vpPoint> > &listFaces)
{
- listFaces.clear();
+ listFaces.clear();
-// std::vector<vpPoint> revolutionAxis;
-// revolutionAxis.push_back(p1);
-// revolutionAxis.push_back(p2);
-// listFaces.push_back(revolutionAxis);
+ // std::vector<vpPoint> revolutionAxis;
+ // revolutionAxis.push_back(p1);
+ // revolutionAxis.push_back(p2);
+ // listFaces.push_back(revolutionAxis);
- vpColVector axis(3);
- axis[0] = p1.get_oX() - p2.get_oX();
- axis[1] = p1.get_oY() - p2.get_oY();
- axis[2] = p1.get_oZ() - p2.get_oZ();
+ vpColVector axis(3);
+ axis[0] = p1.get_oX() - p2.get_oX();
+ axis[1] = p1.get_oY() - p2.get_oY();
+ axis[2] = p1.get_oZ() - p2.get_oZ();
- vpColVector randomVec(3);
- randomVec = 0;
+ vpColVector randomVec(3);
+ randomVec = 0;
- vpColVector axisOrtho(3);
+ vpColVector axisOrtho(3);
- randomVec[0] = 1.0;
- axisOrtho = vpColVector::crossProd(axis, randomVec);
+ randomVec[0] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
- if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
- {
- randomVec = 0;
- randomVec[1] = 1.0;
- axisOrtho = vpColVector::crossProd(axis, randomVec);
- if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
- {
- randomVec = 0;
- randomVec[2] = 1.0;
- axisOrtho = vpColVector::crossProd(axis, randomVec);
- if(axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
- throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
- }
+ if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon()) {
+ randomVec = 0;
+ randomVec[1] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
+ if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon()) {
+ randomVec = 0;
+ randomVec[2] = 1.0;
+ axisOrtho = vpColVector::crossProd(axis, randomVec);
+ if (axisOrtho.euclideanNorm() < std::numeric_limits<double>::epsilon())
+ throw vpMatrixException(vpMatrixException::badValue, "Problem in the cylinder definition");
}
+ }
- axisOrtho.normalize();
-
- vpColVector axisOrthoBis(3);
- axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
- axisOrthoBis.normalize();
-
- //First circle
- vpColVector p1Vec(3);
- p1Vec[0] = p1.get_oX();
- p1Vec[1] = p1.get_oY();
- p1Vec[2] = p1.get_oZ();
- vpColVector fc1 = p1Vec + axisOrtho*radius;
- vpColVector fc2 = p1Vec + axisOrthoBis*radius;
- vpColVector fc3 = p1Vec - axisOrtho*radius;
- vpColVector fc4 = p1Vec - axisOrthoBis*radius;
-
- vpColVector p2Vec(3);
- p2Vec[0] = p2.get_oX();
- p2Vec[1] = p2.get_oY();
- p2Vec[2] = p2.get_oZ();
- vpColVector sc1 = p2Vec + axisOrtho*radius;
- vpColVector sc2 = p2Vec + axisOrthoBis*radius;
- vpColVector sc3 = p2Vec - axisOrtho*radius;
- vpColVector sc4 = p2Vec - axisOrthoBis*radius;
-
- std::vector<vpPoint> pointsFace;
- pointsFace.push_back( vpPoint(fc1[0], fc1[1], fc1[2]) );
- pointsFace.push_back( vpPoint(sc1[0], sc1[1], sc1[2]) );
- pointsFace.push_back( vpPoint(sc2[0], sc2[1], sc2[2]) );
- pointsFace.push_back( vpPoint(fc2[0], fc2[1], fc2[2]) );
- listFaces.push_back(pointsFace);
-
- pointsFace.clear();
- pointsFace.push_back( vpPoint(fc2[0], fc2[1], fc2[2]) );
- pointsFace.push_back( vpPoint(sc2[0], sc2[1], sc2[2]) );
- pointsFace.push_back( vpPoint(sc3[0], sc3[1], sc3[2]) );
- pointsFace.push_back( vpPoint(fc3[0], fc3[1], fc3[2]) );
- listFaces.push_back(pointsFace);
-
- pointsFace.clear();
- pointsFace.push_back( vpPoint(fc3[0], fc3[1], fc3[2]) );
- pointsFace.push_back( vpPoint(sc3[0], sc3[1], sc3[2]) );
- pointsFace.push_back( vpPoint(sc4[0], sc4[1], sc4[2]) );
- pointsFace.push_back( vpPoint(fc4[0], fc4[1], fc4[2]) );
- listFaces.push_back(pointsFace);
-
- pointsFace.clear();
- pointsFace.push_back( vpPoint(fc4[0], fc4[1], fc4[2]) );
- pointsFace.push_back( vpPoint(sc4[0], sc4[1], sc4[2]) );
- pointsFace.push_back( vpPoint(sc1[0], sc1[1], sc1[2]) );
- pointsFace.push_back( vpPoint(fc1[0], fc1[1], fc1[2]) );
- listFaces.push_back(pointsFace);
+ axisOrtho.normalize();
+
+ vpColVector axisOrthoBis(3);
+ axisOrthoBis = vpColVector::crossProd(axis, axisOrtho);
+ axisOrthoBis.normalize();
+
+ // First circle
+ vpColVector p1Vec(3);
+ p1Vec[0] = p1.get_oX();
+ p1Vec[1] = p1.get_oY();
+ p1Vec[2] = p1.get_oZ();
+ vpColVector fc1 = p1Vec + axisOrtho * radius;
+ vpColVector fc2 = p1Vec + axisOrthoBis * radius;
+ vpColVector fc3 = p1Vec - axisOrtho * radius;
+ vpColVector fc4 = p1Vec - axisOrthoBis * radius;
+
+ vpColVector p2Vec(3);
+ p2Vec[0] = p2.get_oX();
+ p2Vec[1] = p2.get_oY();
+ p2Vec[2] = p2.get_oZ();
+ vpColVector sc1 = p2Vec + axisOrtho * radius;
+ vpColVector sc2 = p2Vec + axisOrthoBis * radius;
+ vpColVector sc3 = p2Vec - axisOrtho * radius;
+ vpColVector sc4 = p2Vec - axisOrthoBis * radius;
+
+ std::vector<vpPoint> pointsFace;
+ pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
+ pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
+ pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
+ pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back(vpPoint(fc2[0], fc2[1], fc2[2]));
+ pointsFace.push_back(vpPoint(sc2[0], sc2[1], sc2[2]));
+ pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
+ pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back(vpPoint(fc3[0], fc3[1], fc3[2]));
+ pointsFace.push_back(vpPoint(sc3[0], sc3[1], sc3[2]));
+ pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
+ pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
+ listFaces.push_back(pointsFace);
+
+ pointsFace.clear();
+ pointsFace.push_back(vpPoint(fc4[0], fc4[1], fc4[2]));
+ pointsFace.push_back(vpPoint(sc4[0], sc4[1], sc4[2]));
+ pointsFace.push_back(vpPoint(sc1[0], sc1[1], sc1[2]));
+ pointsFace.push_back(vpPoint(fc1[0], fc1[1], fc1[2]));
+ listFaces.push_back(pointsFace);
}
-
diff --git a/modules/tracker/mbt/src/vpMbXmlParser.cpp b/modules/tracker/mbt/src/vpMbXmlParser.cpp
index d3ea181..3b3ccf0 100644
--- a/modules/tracker/mbt/src/vpMbXmlParser.cpp
+++ b/modules/tracker/mbt/src/vpMbXmlParser.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,26 +37,22 @@
*****************************************************************************/
#include <visp3/core/vpConfig.h>
-
#ifdef VISP_HAVE_XML2
#include <iostream>
#include <map>
-#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
+#include <libxml/xmlmemory.h> /* Fonctions de la lib XML. */
#include <visp3/mbt/vpMbXmlParser.h>
-
/*!
- Default constructor.
-
+ Default constructor.
+
*/
vpMbXmlParser::vpMbXmlParser()
- : cam(), angleAppear(70), angleDisappear(80),
- hasNearClipping(false), nearClipping(false),
- hasFarClipping(false), farClipping(false), fovClipping(false),
- useLod(false), minLineLengthThreshold(50.0), minPolygonAreaThreshold(2500.0)
+ : cam(), angleAppear(70), angleDisappear(80), hasNearClipping(false), nearClipping(false), hasFarClipping(false),
+ farClipping(false), fovClipping(false), useLod(false), minLineLengthThreshold(50.0), minPolygonAreaThreshold(2500.0)
{
init();
@@ -64,15 +61,12 @@ vpMbXmlParser::vpMbXmlParser()
/*!
Default destructor.
*/
-vpMbXmlParser::~vpMbXmlParser()
-{
-}
+vpMbXmlParser::~vpMbXmlParser() {}
/*!
Initialise internal variables (including the map).
*/
-void
-vpMbXmlParser::init()
+void vpMbXmlParser::init()
{
setMainTag("conf");
@@ -99,11 +93,10 @@ vpMbXmlParser::init()
/*!
Parse the file in parameters.
This method is deprecated, use parse() instead.
-
+
\param filename : File to parse.
*/
-void
-vpMbXmlParser::parse(const char * filename)
+void vpMbXmlParser::parse(const char *filename)
{
std::string file = filename;
vpXmlParser::parse(file);
@@ -111,67 +104,66 @@ vpMbXmlParser::parse(const char * filename)
/*!
Write info to file.
-
+
\warning Useless, so not yet implemented => Throw exception.
*/
-void
-vpMbXmlParser::writeMainClass(xmlNodePtr /*node*/)
+void vpMbXmlParser::writeMainClass(xmlNodePtr /*node*/)
{
- throw vpException(vpException::notImplementedError, "Not yet implemented." );
+ throw vpException(vpException::notImplementedError, "Not yet implemented.");
}
/*!
- Read the parameters of the class from the file given by its document pointer
- and by its root node.
-
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
\param doc : Document to parse.
- \param node : Root node.
+ \param node : Root node.
*/
-void
-vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+void vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
bool camera_node = false;
bool face_node = false;
bool lod_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case camera:{
- this->read_camera (doc, dataNode);
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case camera: {
+ this->read_camera(doc, dataNode);
camera_node = true;
- }break;
- case face:{
+ } break;
+ case face: {
this->read_face(doc, dataNode);
face_node = true;
- }break;
- case lod:{
+ } break;
+ case lod: {
this->read_lod(doc, dataNode);
lod_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_sample : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_sample : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!camera_node) {
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+
+ if (!camera_node) {
+ std::cout << "camera : u0 : " << this->cam.get_u0() << " (default)" << std::endl;
+ std::cout << "camera : v0 : " << this->cam.get_v0() << " (default)" << std::endl;
+ std::cout << "camera : px : " << this->cam.get_px() << " (default)" << std::endl;
+ std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl;
}
-
- if(!face_node) {
- std::cout << "face : Angle Appear : "<< angleAppear <<" (default)" <<std::endl;
- std::cout << "face : Angle Disappear : "<< angleDisappear <<" (default)" <<std::endl;
+
+ if (!face_node) {
+ std::cout << "face : Angle Appear : " << angleAppear << " (default)" << std::endl;
+ std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl;
}
- if(!lod_node) {
+ 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;
@@ -180,169 +172,168 @@ vpMbXmlParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
/*!
Read camera information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the camera information.
*/
-void
-vpMbXmlParser::read_camera (xmlDocPtr doc, xmlNodePtr node)
+void vpMbXmlParser::read_camera(xmlDocPtr doc, xmlNodePtr node)
{
bool u0_node = false;
bool v0_node = false;
bool px_node = false;
bool py_node = false;
-
- // current data values.
- double d_u0 = this->cam.get_u0();
- double d_v0 = this->cam.get_v0();
- double d_px = this->cam.get_px();
- double d_py = this->cam.get_py();
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case u0:{
+
+ // current data values.
+ double d_u0 = this->cam.get_u0();
+ double d_v0 = this->cam.get_v0();
+ double d_px = this->cam.get_px();
+ double d_py = this->cam.get_py();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case u0: {
d_u0 = xmlReadDoubleChild(doc, dataNode);
u0_node = true;
- }break;
- case v0:{
+ } break;
+ case v0: {
d_v0 = xmlReadDoubleChild(doc, dataNode);
v0_node = true;
- }break;
- case px:{
+ } break;
+ case px: {
d_px = xmlReadDoubleChild(doc, dataNode);
px_node = true;
- }break;
- case py:{
+ } break;
+ case py: {
d_py = xmlReadDoubleChild(doc, dataNode);
py_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_camera : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- this->cam.initPersProjWithoutDistortion(d_px, d_py, d_u0, d_v0) ;
-
- if(!u0_node)
- std::cout << "camera : u0 : "<< this->cam.get_u0() << " (default)" <<std::endl;
+
+ this->cam.initPersProjWithoutDistortion(d_px, d_py, d_u0, d_v0);
+
+ if (!u0_node)
+ std::cout << "camera : u0 : " << this->cam.get_u0() << " (default)" << std::endl;
else
- std::cout << "camera : u0 : "<< this->cam.get_u0() <<std::endl;
-
- if(!v0_node)
- std::cout << "camera : v0 : "<< this->cam.get_v0() << " (default)" <<std::endl;
+ std::cout << "camera : u0 : " << this->cam.get_u0() << std::endl;
+
+ if (!v0_node)
+ std::cout << "camera : v0 : " << this->cam.get_v0() << " (default)" << std::endl;
else
- std::cout << "camera : v0 : "<< this->cam.get_v0() <<std::endl;
-
- if(!px_node)
- std::cout << "camera : px : "<< this->cam.get_px() << " (default)" <<std::endl;
+ std::cout << "camera : v0 : " << this->cam.get_v0() << std::endl;
+
+ if (!px_node)
+ std::cout << "camera : px : " << this->cam.get_px() << " (default)" << std::endl;
else
- std::cout << "camera : px : "<< this->cam.get_px() <<std::endl;
+ std::cout << "camera : px : " << this->cam.get_px() << std::endl;
- if(!py_node)
- std::cout << "camera : py : "<< this->cam.get_py() << " (default)" <<std::endl;
+ if (!py_node)
+ std::cout << "camera : py : " << this->cam.get_py() << " (default)" << std::endl;
else
- std::cout << "camera : py : "<< this->cam.get_py() <<std::endl;
+ std::cout << "camera : py : " << this->cam.get_py() << std::endl;
}
/*!
Read face information.
-
- \throw vpException::fatalError if there was an unexpected number of data.
-
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
\param doc : Pointer to the document.
\param node : Pointer to the node of the camera information.
*/
-void
-vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node)
+void vpMbXmlParser::read_face(xmlDocPtr doc, xmlNodePtr node)
{
bool angle_appear_node = false;
bool angle_disappear_node = false;
bool near_clipping_node = false;
bool far_clipping_node = false;
bool fov_clipping_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
- case angle_appear:{
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case angle_appear: {
angleAppear = xmlReadDoubleChild(doc, dataNode);
angle_appear_node = true;
- }break;
- case angle_disappear:{
+ } break;
+ case angle_disappear: {
angleDisappear = xmlReadDoubleChild(doc, dataNode);
angle_disappear_node = true;
- }break;
- case near_clipping:{
+ } break;
+ case near_clipping: {
nearClipping = xmlReadDoubleChild(doc, dataNode);
near_clipping_node = true;
hasNearClipping = true;
- }break;
- case far_clipping:{
+ } break;
+ case far_clipping: {
farClipping = xmlReadDoubleChild(doc, dataNode);
far_clipping_node = true;
hasFarClipping = true;
- }break;
- case fov_clipping:{
+ } break;
+ case fov_clipping: {
if (xmlReadIntChild(doc, dataNode))
fovClipping = true;
else
fovClipping = false;
fov_clipping_node = true;
- }break;
- default:{
-// vpTRACE("unknown tag in read_camera : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ } break;
+ default: {
+ // vpTRACE("unknown tag in read_camera : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
-
- if(!angle_appear_node)
- std::cout << "face : Angle Appear : "<< angleAppear << " (default)" <<std::endl;
+
+ if (!angle_appear_node)
+ std::cout << "face : Angle Appear : " << angleAppear << " (default)" << std::endl;
else
- std::cout << "face : Angle Appear : "<< angleAppear <<std::endl;
-
- if(!angle_disappear_node)
- std::cout << "face : Angle Disappear : "<< angleDisappear << " (default)" <<std::endl;
+ std::cout << "face : Angle Appear : " << angleAppear << std::endl;
+
+ if (!angle_disappear_node)
+ std::cout << "face : Angle Disappear : " << angleDisappear << " (default)" << std::endl;
else
- std::cout << "face : Angle Disappear : "<< angleDisappear <<std::endl;
-
- if(near_clipping_node)
- std::cout << "face : Near Clipping : "<< nearClipping <<std::endl;
-
- if(far_clipping_node)
- std::cout << "face : Far Clipping : "<< farClipping <<std::endl;
-
- if(fov_clipping_node) {
- if(fovClipping)
- std::cout << "face : Fov Clipping : True" <<std::endl;
+ std::cout << "face : Angle Disappear : " << angleDisappear << std::endl;
+
+ if (near_clipping_node)
+ std::cout << "face : Near Clipping : " << nearClipping << std::endl;
+
+ if (far_clipping_node)
+ std::cout << "face : Far Clipping : " << farClipping << std::endl;
+
+ if (fov_clipping_node) {
+ if (fovClipping)
+ std::cout << "face : Fov Clipping : True" << std::endl;
else
- std::cout << "face : Fov Clipping : False" <<std::endl;
+ std::cout << "face : Fov Clipping : False" << std::endl;
}
}
-void
-vpMbXmlParser::read_lod (xmlDocPtr doc, xmlNodePtr node) {
+void vpMbXmlParser::read_lod(xmlDocPtr doc, xmlNodePtr node)
+{
bool use_lod_node = false;
bool min_line_length_threshold_node = false;
bool min_polygon_area_threshold_node = false;
-
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
case use_lod:
useLod = (xmlReadIntChild(doc, dataNode) != 0);
use_lod_node = true;
@@ -355,32 +346,33 @@ vpMbXmlParser::read_lod (xmlDocPtr doc, xmlNodePtr node) {
minPolygonAreaThreshold = xmlReadDoubleChild(doc, dataNode);
min_polygon_area_threshold_node = true;
break;
- default:{
-// vpTRACE("unknown tag in read_contrast : %d, %s", iter_data->second, (iter_data->first).c_str());
- }break;
+ default: {
+ // vpTRACE("unknown tag in read_contrast : %d, %s",
+ // iter_data->second, (iter_data->first).c_str());
+ } break;
}
}
}
}
- if(!use_lod_node)
- std::cout << "lod : use lod : " << useLod << " (default)" <<std::endl;
+ if (!use_lod_node)
+ std::cout << "lod : use lod : " << useLod << " (default)" << std::endl;
else
std::cout << "lod : use lod : " << useLod << std::endl;
- if(!min_line_length_threshold_node)
- std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<" (default)" <<std::endl;
+ if (!min_line_length_threshold_node)
+ std::cout << "lod : min line length threshold : " << minLineLengthThreshold << " (default)" << std::endl;
else
- std::cout <<"lod : min line length threshold : " << minLineLengthThreshold <<std::endl;
+ 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;
+ if (!min_polygon_area_threshold_node)
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << " (default)" << std::endl;
else
- std::cout <<"lod : min polygon area threshold : " << minPolygonAreaThreshold <<std::endl;
+ std::cout << "lod : min polygon area threshold : " << minPolygonAreaThreshold << std::endl;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_mbt.a(vpMbXmlParser.cpp.o) has no symbols
-void dummy_vpMbXmlParser() {};
+// Work arround to avoid warning: libvisp_mbt.a(vpMbXmlParser.cpp.o) has no
+// symbols
+void dummy_vpMbXmlParser(){};
#endif
-
diff --git a/modules/tracker/mbt/src/vpMbtPolygon.cpp b/modules/tracker/mbt/src/vpMbtPolygon.cpp
index 58b4bf3..74aeac5 100644
--- a/modules/tracker/mbt/src/vpMbtPolygon.cpp
+++ b/modules/tracker/mbt/src/vpMbtPolygon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,28 +47,28 @@
\brief Implements a polygon of the model used by the model-based tracker.
*/
-#include <visp3/mbt/vpMbtPolygon.h>
#include <visp3/core/vpPolygon.h>
+#include <visp3/mbt/vpMbtPolygon.h>
/*!
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),
+vpMbtPolygon::vpMbtPolygon(const vpMbtPolygon &mbtp)
+ : vpPolygon3D(mbtp), index(mbtp.index), isvisible(mbtp.isvisible), isappearing(mbtp.isappearing), useLod(mbtp.useLod),
+ minLineLengthThresh(mbtp.minLineLengthThresh), minPolygonAreaThresh(mbtp.minPolygonAreaThresh), name(mbtp.name),
hasOrientation(mbtp.hasOrientation)
{
- //*this = mbtp; // Should not be called by copy contructor to avoid multiple assignements.
+ //*this = mbtp; // Should not be called by copy contructor to avoid multiple
+ // assignements.
}
-vpMbtPolygon& vpMbtPolygon::operator=(const vpMbtPolygon& mbtp)
+vpMbtPolygon &vpMbtPolygon::operator=(const vpMbtPolygon &mbtp)
{
vpPolygon3D::operator=(mbtp);
index = mbtp.index;
@@ -85,38 +86,35 @@ vpMbtPolygon& vpMbtPolygon::operator=(const vpMbtPolygon& mbtp)
/*!
Basic destructor.
*/
-vpMbtPolygon::~vpMbtPolygon()
-{
-}
+vpMbtPolygon::~vpMbtPolygon() {}
/*!
- Check if the polygon is visible in the image and if the angle between the normal
- to the face and the line vector going from the optical center to the cog of the face is below
- the given threshold.
- To do that, the polygon is projected into the image thanks to the camera pose.
+ Check if the polygon is visible in the image and if the angle between the
+ normal to the face and the line vector going from the optical center to the
+ cog of the face is below the given threshold. To do that, the polygon is
+ projected into the image thanks to the camera pose.
\param cMo : The pose of the camera.
\param alpha : Maximum angle to detect if the face is visible (in rad).
- \param modulo : Indicates if the test should also consider faces that are not oriented
- counter clockwise. If true, the orientation of the face is without importance.
- \param cam : Camera parameters (intrinsics parameters)
+ \param modulo : Indicates if the test should also consider faces that are
+ not oriented counter clockwise. If true, the orientation of the face is
+ without importance. \param cam : Camera parameters (intrinsics parameters)
\param I : Image used to consider level of detail.
\return Return true if the polygon is visible.
*/
-bool
-vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
- const vpCameraParameters &cam, const vpImage<unsigned char> &I)
+bool vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, const bool &modulo,
+ const vpCameraParameters &cam, const vpImage<unsigned char> &I)
{
// std::cout << "Computing angle from MBT Face (cMo, alpha)" << std::endl;
changeFrame(cMo);
- if(nbpt <= 2) {
- //Level of detail (LOD)
- if(useLod) {
+ if (nbpt <= 2) {
+ // Level of detail (LOD)
+ if (useLod) {
vpCameraParameters c = cam;
- if(clippingFlag > 3) { // Contains at least one FOV constraint
+ if (clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
@@ -129,8 +127,10 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
double x2 = roiImagePoints[1].get_u();
double y2 = roiImagePoints[1].get_v();
double length = std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
-// std::cout << "Index=" << index << " ; Line length=" << length << " ; clippingFlag=" << clippingFlag << std::endl;
-// vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index, length, minLineLengthThresh);
+ // std::cout << "Index=" << index << " ; Line length=" <<
+ // length << " ; clippingFlag=" << clippingFlag << std::endl;
+ // vpTRACE("index=%d lenght=%f minLineLengthThresh=%f", index,
+ // length, minLineLengthThresh);
if (length < minLineLengthThresh) {
isvisible = false;
@@ -143,26 +143,25 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
/* a line is always visible when LOD is not used */
isvisible = true;
isappearing = false;
- return true ;
+ return true;
}
- // If the polygon has no orientation, the angle check visibility is always valid.
- // Feature mainly used for cylinders.
- if(!hasOrientation)
- {
+ // If the polygon has no orientation, the angle check visibility is always
+ // valid. Feature mainly used for cylinders.
+ if (!hasOrientation) {
isvisible = true;
isappearing = false;
return true;
}
- //Check visibility from normal
- //Newell's Method for calculating the normal of an arbitrary 3D polygon
- //https://www.opengl.org/wiki/Calculating_a_Surface_Normal
+ // Check visibility from normal
+ // Newell's Method for calculating the normal of an arbitrary 3D polygon
+ // https://www.opengl.org/wiki/Calculating_a_Surface_Normal
vpColVector faceNormal(3);
vpColVector currentVertex, nextVertex;
- for(unsigned int i = 0; i<nbpt; i++) {
+ for (unsigned int i = 0; i < nbpt; i++) {
currentVertex = p[i].cP;
- nextVertex = p[(i+1) % nbpt].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]);
@@ -170,9 +169,9 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
}
faceNormal.normalize();
- vpColVector e4(3) ;
+ vpColVector e4(3);
vpPoint pt;
- for (unsigned int i = 0; i < nbpt; i += 1){
+ for (unsigned int i = 0; i < nbpt; i += 1) {
pt.set_X(pt.get_X() + p[i].get_X());
pt.set_Y(pt.get_Y() + p[i].get_Y());
pt.set_Z(pt.get_Z() + p[i].get_Z());
@@ -184,15 +183,16 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
double angle = acos(vpColVector::dotProd(e4, faceNormal));
-// vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" << vpMath::deg(alpha) << std::endl;
+ // vpCTRACE << angle << "/" << vpMath::deg(angle) << "/" <<
+ // vpMath::deg(alpha) << std::endl;
- if( angle < alpha || (modulo && (M_PI - angle) < alpha)) {
+ if (angle < alpha || (modulo && (M_PI - angle) < alpha)) {
isvisible = true;
isappearing = false;
if (useLod) {
vpCameraParameters c = cam;
- if(clippingFlag > 3) { // Contains at least one FOV constraint
+ if (clippingFlag > 3) { // Contains at least one FOV constraint
c.computeFov(I.getWidth(), I.getHeight());
}
computePolygonClipped(c);
@@ -201,8 +201,9 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
vpPolygon roiPolygon(roiImagePoints);
double area = roiPolygon.getArea();
-// std::cout << "After normal test ; Index=" << index << " ; area=" << area << " ; clippingFlag="
-// << clippingFlag << std::endl;
+ // std::cout << "After normal test ; Index=" << index << " ; area="
+ // << area << " ; clippingFlag="
+ // << clippingFlag << std::endl;
if (area < minPolygonAreaThresh) {
isappearing = false;
isvisible = false;
@@ -213,18 +214,16 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
return true;
}
- if (angle < alpha+vpMath::rad(1) ){
+ if (angle < alpha + vpMath::rad(1)) {
isappearing = true;
- }
- else if (modulo && (M_PI - angle) < alpha+vpMath::rad(1) ){
+ } else if (modulo && (M_PI - angle) < alpha + vpMath::rad(1)) {
isappearing = true;
- }
- else {
+ } else {
isappearing = false;
}
- isvisible = false ;
- return false ;
+ isvisible = false;
+ return false;
}
//###################################
@@ -233,53 +232,48 @@ vpMbtPolygon::isVisible(const vpHomogeneousMatrix &cMo, const double alpha, cons
/*!
Set the flag to consider if the level of detail (LOD) is used or not.
- When activated, lines and faces of the 3D model are tracked if respectively their
- projected lenght and area in the image are significative enough. By significative, we mean:
- - if the lenght of the projected line in the image is greater that a threshold set by
- setMinLineLengthThresh()
- - if the area of the projected face in the image is greater that a threshold set by
- setMinPolygonAreaThresh().
+ When activated, lines and faces of the 3D model are tracked if respectively
+their projected lenght and area in the image are significative enough. By
+significative, we mean:
+ - if the lenght of the projected line in the image is greater that a
+threshold set by setMinLineLengthThresh()
+ - if the area of the projected face in the image is greater that a threshold
+set by setMinPolygonAreaThresh().
\param use_lod : true if level of detail must be used, false otherwise.
The sample code below shows how to introduce this feature:
\code
-#include <visp/vpMbEdgeTracker.h>
#include <visp/vpImageIo.h>
+#include <visp/vpMbEdgeTracker.h>
int main()
{
-vpImage<unsigned char> I;
+ pImage<unsigned char> 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
\sa setMinLineLengthThresh(), setMinPolygonAreaThresh()
*/
-void
-vpMbtPolygon::setLod(const bool use_lod)
-{
- this->useLod = use_lod;
-}
-
-
+void vpMbtPolygon::setLod(const bool use_lod) { this->useLod = use_lod; }
diff --git a/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp
new file mode 100644
index 0000000..73382ae
--- /dev/null
+++ b/modules/tracker/mbt/src/vpMbtXmlGenericParser.cpp
@@ -0,0 +1,1188 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Load XML Parameter for Model Based Tracker.
+ *
+ *****************************************************************************/
+#include <visp3/core/vpConfig.h>
+
+#ifdef VISP_HAVE_XML2
+
+#include <iostream>
+#include <map>
+
+#include <libxml/xmlmemory.h>
+
+#include <visp3/mbt/vpMbtXmlGenericParser.h>
+
+vpMbtXmlGenericParser::vpMbtXmlGenericParser(const vpParserType &type)
+ : m_parserType(type),
+ //<camera>
+ m_cam(),
+ //<face>
+ m_angleAppear(70), m_angleDisappear(80), m_hasNearClipping(false), m_nearClipping(false), m_hasFarClipping(false),
+ m_farClipping(false), m_fovClipping(false),
+ //<lod>
+ m_useLod(false), m_minLineLengthThreshold(50.0), m_minPolygonAreaThreshold(2500.0),
+ //<ecm>
+ m_ecm(),
+ //<klt>
+ m_kltMaskBorder(0), m_kltMaxFeatures(0), m_kltWinSize(0), m_kltQualityValue(0.), m_kltMinDist(0.),
+ m_kltHarrisParam(0.), m_kltBlockSize(0), m_kltPyramidLevels(0),
+ //<depth_normal>
+ m_depthNormalFeatureEstimationMethod(vpMbtFaceDepthNormal::ROBUST_FEATURE_ESTIMATION),
+ m_depthNormalPclPlaneEstimationMethod(2), m_depthNormalPclPlaneEstimationRansacMaxIter(200),
+ m_depthNormalPclPlaneEstimationRansacThreshold(0.001), m_depthNormalSamplingStepX(2), m_depthNormalSamplingStepY(2),
+ //<depth_dense>
+ m_depthDenseSamplingStepX(2), m_depthDenseSamplingStepY(2)
+{
+ init();
+}
+
+vpMbtXmlGenericParser::~vpMbtXmlGenericParser() {}
+
+/*!
+ Initialise internal variables (including the map).
+*/
+void vpMbtXmlGenericParser::init()
+{
+ setMainTag("conf");
+
+ //<conf>
+ nodeMap["conf"] = conf;
+ //<face>
+ nodeMap["face"] = face;
+ nodeMap["angle_appear"] = angle_appear;
+ nodeMap["angle_disappear"] = angle_disappear;
+ nodeMap["near_clipping"] = near_clipping;
+ nodeMap["far_clipping"] = far_clipping;
+ nodeMap["fov_clipping"] = fov_clipping;
+ //<camera>
+ nodeMap["camera"] = camera;
+ nodeMap["height"] = height;
+ nodeMap["width"] = width;
+ nodeMap["u0"] = u0;
+ nodeMap["v0"] = v0;
+ nodeMap["px"] = px;
+ nodeMap["py"] = py;
+ //<lod>
+ nodeMap["lod"] = lod;
+ nodeMap["use_lod"] = use_lod;
+ nodeMap["min_line_length_threshold"] = min_line_length_threshold;
+ nodeMap["min_polygon_area_threshold"] = min_polygon_area_threshold;
+ //<ecm>
+ nodeMap["ecm"] = ecm;
+ nodeMap["mask"] = mask;
+ nodeMap["size"] = size;
+ nodeMap["nb_mask"] = nb_mask;
+ nodeMap["range"] = range;
+ nodeMap["tracking"] = tracking;
+ nodeMap["contrast"] = contrast;
+ nodeMap["edge_threshold"] = edge_threshold;
+ nodeMap["mu1"] = mu1;
+ nodeMap["mu2"] = mu2;
+ nodeMap["sample"] = sample;
+ nodeMap["step"] = step;
+ //<klt>
+ nodeMap["klt"] = klt;
+ nodeMap["mask_border"] = mask_border;
+ nodeMap["max_features"] = max_features;
+ nodeMap["window_size"] = window_size;
+ nodeMap["quality"] = quality;
+ nodeMap["min_distance"] = min_distance;
+ nodeMap["harris"] = harris;
+ nodeMap["size_block"] = size_block;
+ nodeMap["pyramid_lvl"] = pyramid_lvl;
+ //<depth_normal>
+ nodeMap["depth_normal"] = depth_normal;
+ nodeMap["feature_estimation_method"] = feature_estimation_method;
+ nodeMap["PCL_plane_estimation"] = PCL_plane_estimation;
+ nodeMap["method"] = PCL_plane_estimation_method;
+ nodeMap["ransac_max_iter"] = PCL_plane_estimation_ransac_max_iter;
+ nodeMap["ransac_threshold"] = PCL_plane_estimation_ransac_threshold;
+ nodeMap["sampling_step"] = depth_sampling_step;
+ nodeMap["step_X"] = depth_sampling_step_X;
+ nodeMap["step_Y"] = depth_sampling_step_Y;
+ //<depth_dense>
+ nodeMap["depth_dense"] = depth_dense;
+ nodeMap["sampling_step"] = depth_dense_sampling_step;
+ nodeMap["step_X"] = depth_dense_sampling_step_X;
+ nodeMap["step_Y"] = depth_dense_sampling_step_Y;
+}
+
+/*!
+ Parse the file in parameters.
+ This method is deprecated, use parse() instead.
+
+ \param filename : File to parse.
+*/
+void vpMbtXmlGenericParser::parse(const std::string &filename) { vpXmlParser::parse(filename); }
+
+/*!
+ Write info to file.
+
+ \warning Useless, so not yet implemented => Throw exception.
+*/
+void vpMbtXmlGenericParser::writeMainClass(xmlNodePtr /*node*/)
+{
+ throw vpException(vpException::notImplementedError, "Not implemented.");
+}
+
+/*!
+ Read the parameters of the class from the file given by its document pointer
+ and by its root node.
+
+ \param doc : Document to parse.
+ \param node : Root node.
+*/
+void vpMbtXmlGenericParser::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool camera_node = false;
+ bool face_node = false;
+ bool ecm_node = false;
+ bool klt_node = false;
+ bool lod_node = false;
+ bool depth_normal_node = false;
+ bool depth_dense_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case camera:
+ read_camera(doc, dataNode);
+ camera_node = true;
+ break;
+
+ case face:
+ read_face(doc, dataNode);
+ face_node = true;
+ break;
+
+ case lod:
+ read_lod(doc, dataNode);
+ lod_node = true;
+ break;
+
+ case ecm:
+ if (m_parserType & EDGE_PARSER) {
+ read_ecm(doc, dataNode);
+ ecm_node = true;
+ }
+ break;
+
+ case sample:
+ if (m_parserType & EDGE_PARSER)
+ read_sample_deprecated(doc, dataNode);
+ break;
+
+ case klt:
+ if (m_parserType & KLT_PARSER) {
+ read_klt(doc, dataNode);
+ klt_node = true;
+ }
+ break;
+
+ case depth_normal:
+ if (m_parserType & DEPTH_NORMAL_PARSER) {
+ read_depth_normal(doc, dataNode);
+ depth_normal_node = true;
+ }
+ break;
+
+ case depth_dense:
+ if (m_parserType & DEPTH_DENSE_PARSER) {
+ read_depth_dense(doc, dataNode);
+ depth_dense_node = true;
+ }
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ 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;
+ }
+
+ if (!face_node) {
+ 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;
+ }
+
+ 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;
+ }
+
+ 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;
+ }
+
+ if (!depth_normal_node && (m_parserType & DEPTH_NORMAL_PARSER)) {
+ 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 << " (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;
+ }
+}
+
+/*!
+ Read camera information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void vpMbtXmlGenericParser::read_camera(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool u0_node = false;
+ bool v0_node = false;
+ bool px_node = false;
+ bool py_node = false;
+
+ // current data values.
+ double d_u0 = m_cam.get_u0();
+ double d_v0 = m_cam.get_v0();
+ double d_px = m_cam.get_px();
+ double d_py = m_cam.get_py();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case u0:
+ d_u0 = xmlReadDoubleChild(doc, dataNode);
+ u0_node = true;
+ break;
+
+ case v0:
+ d_v0 = xmlReadDoubleChild(doc, dataNode);
+ v0_node = true;
+ break;
+
+ case px:
+ d_px = xmlReadDoubleChild(doc, dataNode);
+ px_node = true;
+ break;
+
+ case py:
+ d_py = xmlReadDoubleChild(doc, dataNode);
+ py_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ 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;
+ 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;
+ 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;
+ 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;
+ else
+ std::cout << "camera : py : " << m_cam.get_py() << std::endl;
+}
+
+/*!
+ Read depth normal information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node information.
+*/
+void vpMbtXmlGenericParser::read_depth_normal(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool feature_estimation_method_node = false;
+ bool PCL_plane_estimation_node = false;
+ bool sampling_step_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::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);
+ feature_estimation_method_node = true;
+ break;
+
+ case PCL_plane_estimation:
+ read_depth_normal_PCL(doc, dataNode);
+ PCL_plane_estimation_node = true;
+ break;
+
+ case depth_sampling_step:
+ read_depth_normal_sampling_step(doc, dataNode);
+ sampling_step_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!feature_estimation_method_node)
+ std::cout << "depth normal : feature_estimation_method : " << m_depthNormalFeatureEstimationMethod << " (default)"
+ << std::endl;
+ else
+ 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 << " (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;
+ }
+}
+
+/*!
+ Read depth normal PCL properties.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node information.
+*/
+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) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::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);
+ PCL_plane_estimation_method_node = true;
+ break;
+
+ case PCL_plane_estimation_ransac_max_iter:
+ 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);
+ PCL_plane_estimation_ransac_threshold_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!PCL_plane_estimation_method_node)
+ 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;
+
+ if (!PCL_plane_estimation_ransac_max_iter_node)
+ 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;
+
+ if (!PCL_plane_estimation_ransac_threshold_node)
+ std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : "
+ << m_depthNormalPclPlaneEstimationRansacThreshold << " (default)" << std::endl;
+ else
+ std::cout << "depth normal : PCL_plane_estimation : ransac_threshold : "
+ << m_depthNormalPclPlaneEstimationRansacThreshold << std::endl;
+}
+
+/*!
+ Read depth normal sampling step.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node information.
+*/
+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) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case depth_sampling_step_X:
+ m_depthNormalSamplingStepX = xmlReadUnsignedIntChild(doc, dataNode);
+ sampling_step_X_node = true;
+ break;
+
+ case depth_sampling_step_Y:
+ m_depthNormalSamplingStepY = xmlReadUnsignedIntChild(doc, dataNode);
+ sampling_step_Y_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!sampling_step_X_node)
+ 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;
+
+ if (!sampling_step_Y_node)
+ 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;
+}
+
+/*!
+ Read depth dense information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the ecm information.
+*/
+void vpMbtXmlGenericParser::read_depth_dense(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool sampling_step_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case depth_dense_sampling_step:
+ read_depth_dense_sampling_step(doc, dataNode);
+ sampling_step_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ 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;
+ }
+}
+
+/*!
+ Read depth dense sampling step.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the range information.
+*/
+void 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) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case depth_dense_sampling_step_X:
+ m_depthDenseSamplingStepX = xmlReadUnsignedIntChild(doc, dataNode);
+ sampling_step_X_node = true;
+ break;
+
+ case depth_dense_sampling_step_Y:
+ m_depthDenseSamplingStepY = xmlReadUnsignedIntChild(doc, dataNode);
+ sampling_step_Y_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!sampling_step_X_node)
+ 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;
+
+ if (!sampling_step_Y_node)
+ 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;
+}
+
+/*!
+ Read ecm information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the ecm information.
+*/
+void vpMbtXmlGenericParser::read_ecm(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool mask_node = false;
+ bool range_node = false;
+ bool contrast_node = false;
+ bool sample_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case mask:
+ read_ecm_mask(doc, dataNode);
+ mask_node = true;
+ break;
+
+ case range:
+ read_ecm_range(doc, dataNode);
+ range_node = true;
+ break;
+
+ case contrast:
+ read_ecm_contrast(doc, dataNode);
+ contrast_node = true;
+ break;
+
+ case sample:
+ read_ecm_sample(doc, dataNode);
+ sample_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ 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;
+ }
+
+ if (!range_node) {
+ 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;
+ }
+
+ if (!sample_node) {
+ std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl;
+ }
+}
+
+/*!
+ Read the contrast information from the xml file.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the contrast information.
+*/
+void vpMbtXmlGenericParser::read_ecm_contrast(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool edge_threshold_node = false;
+ bool mu1_node = false;
+ bool mu2_node = false;
+
+ // current data values.
+ double d_edge_threshold = m_ecm.getThreshold();
+ double d_mu1 = m_ecm.getMu1();
+ double d_mu2 = m_ecm.getMu2();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case edge_threshold:
+ d_edge_threshold = xmlReadDoubleChild(doc, dataNode);
+ edge_threshold_node = true;
+ break;
+
+ case mu1:
+ d_mu1 = xmlReadDoubleChild(doc, dataNode);
+ mu1_node = true;
+ break;
+
+ case mu2:
+ d_mu2 = xmlReadDoubleChild(doc, dataNode);
+ mu2_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ m_ecm.setMu1(d_mu1);
+ m_ecm.setMu2(d_mu2);
+ m_ecm.setThreshold(d_edge_threshold);
+
+ if (!edge_threshold_node)
+ std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() << " (default)" << std::endl;
+ else
+ std::cout << "ecm : contrast : threshold " << m_ecm.getThreshold() << std::endl;
+
+ if (!mu1_node)
+ 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;
+ else
+ std::cout << "ecm : contrast : mu2 " << m_ecm.getMu2() << std::endl;
+}
+
+/*!
+ Read mask information for the vpMeSite.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the mask information.
+*/
+void vpMbtXmlGenericParser::read_ecm_mask(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool size_node = false;
+ bool nb_mask_node = false;
+
+ // current data values.
+ 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) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case size:
+ d_size = xmlReadUnsignedIntChild(doc, dataNode);
+ size_node = true;
+ break;
+
+ case nb_mask:
+ d_nb_mask = xmlReadUnsignedIntChild(doc, dataNode);
+ nb_mask_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ m_ecm.setMaskSize(d_size);
+
+ // Check to ensure that d_nb_mask > 0
+ if (d_nb_mask == 0)
+ throw(vpException(vpException::badValue, "Model-based tracker mask size "
+ "parameter should be different "
+ "from zero in xml file"));
+ m_ecm.setMaskNumber(d_nb_mask);
+
+ if (!size_node)
+ 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;
+ else
+ std::cout << "ecm : mask : nb_mask : " << m_ecm.getMaskNumber() << std::endl;
+}
+
+/*!
+ Read range information for the vpMeSite.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the range information.
+*/
+void vpMbtXmlGenericParser::read_ecm_range(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool tracking_node = false;
+
+ // current data values.
+ unsigned int m_range_tracking = m_ecm.getRange();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case tracking:
+ m_range_tracking = xmlReadUnsignedIntChild(doc, dataNode);
+ tracking_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ m_ecm.setRange(m_range_tracking);
+
+ if (!tracking_node)
+ std::cout << "ecm : range : tracking : " << m_ecm.getRange() << " (default)" << std::endl;
+ else
+ std::cout << "ecm : range : tracking : " << m_ecm.getRange() << std::endl;
+}
+
+/*!
+ Read sample information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the sample information.
+*/
+void vpMbtXmlGenericParser::read_ecm_sample(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool step_node = false;
+
+ // current data values.
+ double d_stp = m_ecm.getSampleStep();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case step:
+ d_stp = xmlReadIntChild(doc, dataNode);
+ step_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ m_ecm.setSampleStep(d_stp);
+
+ if (!step_node)
+ std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << " (default)" << std::endl;
+ else
+ std::cout << "ecm : sample : sample_step : " << m_ecm.getSampleStep() << std::endl;
+}
+
+/*!
+ Read face information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void vpMbtXmlGenericParser::read_face(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool angle_appear_node = false;
+ bool angle_disappear_node = false;
+ bool near_clipping_node = false;
+ bool far_clipping_node = false;
+ bool fov_clipping_node = false;
+ m_hasNearClipping = false;
+ m_hasFarClipping = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case angle_appear:
+ m_angleAppear = xmlReadDoubleChild(doc, dataNode);
+ angle_appear_node = true;
+ break;
+
+ case angle_disappear:
+ m_angleDisappear = xmlReadDoubleChild(doc, dataNode);
+ angle_disappear_node = true;
+ break;
+
+ case near_clipping:
+ m_nearClipping = xmlReadDoubleChild(doc, dataNode);
+ m_hasNearClipping = true;
+ near_clipping_node = true;
+ break;
+
+ case far_clipping:
+ m_farClipping = xmlReadDoubleChild(doc, dataNode);
+ m_hasFarClipping = true;
+ far_clipping_node = true;
+ break;
+
+ case fov_clipping:
+ if (xmlReadIntChild(doc, dataNode))
+ m_fovClipping = true;
+ else
+ m_fovClipping = false;
+ fov_clipping_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!angle_appear_node)
+ 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;
+ else
+ std::cout << "face : Angle Disappear : " << m_angleDisappear << std::endl;
+
+ if (near_clipping_node)
+ std::cout << "face : Near Clipping : " << m_nearClipping << std::endl;
+
+ if (far_clipping_node)
+ std::cout << "face : Far Clipping : " << m_farClipping << std::endl;
+
+ if (fov_clipping_node) {
+ if (m_fovClipping)
+ std::cout << "face : Fov Clipping : True" << std::endl;
+ else
+ std::cout << "face : Fov Clipping : False" << std::endl;
+ }
+}
+
+/*!
+ Read klt information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the camera information.
+*/
+void vpMbtXmlGenericParser::read_klt(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool mask_border_node = false;
+ bool max_features_node = false;
+ bool window_size_node = false;
+ bool quality_node = false;
+ bool min_distance_node = false;
+ bool harris_node = false;
+ bool size_block_node = false;
+ bool pyramid_lvl_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case mask_border:
+ m_kltMaskBorder = xmlReadUnsignedIntChild(doc, dataNode);
+ mask_border_node = true;
+ break;
+
+ case max_features:
+ m_kltMaxFeatures = xmlReadUnsignedIntChild(doc, dataNode);
+ max_features_node = true;
+ break;
+
+ case window_size:
+ m_kltWinSize = xmlReadUnsignedIntChild(doc, dataNode);
+ window_size_node = true;
+ break;
+
+ case quality:
+ m_kltQualityValue = xmlReadDoubleChild(doc, dataNode);
+ quality_node = true;
+ break;
+
+ case min_distance:
+ m_kltMinDist = xmlReadDoubleChild(doc, dataNode);
+ min_distance_node = true;
+ break;
+
+ case harris:
+ m_kltHarrisParam = xmlReadDoubleChild(doc, dataNode);
+ harris_node = true;
+ break;
+
+ case size_block:
+ m_kltBlockSize = xmlReadUnsignedIntChild(doc, dataNode);
+ size_block_node = true;
+ break;
+
+ case pyramid_lvl:
+ m_kltPyramidLevels = xmlReadUnsignedIntChild(doc, dataNode);
+ pyramid_lvl_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!mask_border_node)
+ 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;
+ else
+ std::cout << "klt : Max Features : " << m_kltMaxFeatures << std::endl;
+
+ if (!window_size_node)
+ 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;
+ else
+ std::cout << "klt : Quality : " << m_kltQualityValue << std::endl;
+
+ if (!min_distance_node)
+ 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;
+ else
+ std::cout << "klt : Harris Parameter : " << m_kltHarrisParam << std::endl;
+
+ if (!size_block_node)
+ 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;
+ else
+ std::cout << "klt : Pyramid Levels : " << m_kltPyramidLevels << std::endl;
+}
+
+void vpMbtXmlGenericParser::read_lod(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool use_lod_node = false;
+ bool min_line_length_threshold_node = false;
+ bool min_polygon_area_threshold_node = false;
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case use_lod:
+ m_useLod = (xmlReadIntChild(doc, dataNode) != 0);
+ use_lod_node = true;
+ break;
+
+ case min_line_length_threshold:
+ m_minLineLengthThreshold = xmlReadDoubleChild(doc, dataNode);
+ min_line_length_threshold_node = true;
+ break;
+
+ case min_polygon_area_threshold:
+ m_minPolygonAreaThreshold = xmlReadDoubleChild(doc, dataNode);
+ min_polygon_area_threshold_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ if (!use_lod_node)
+ std::cout << "lod : use lod : " << m_useLod << " (default)" << std::endl;
+ else
+ 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;
+ else
+ 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;
+ else
+ std::cout << "lod : min polygon area threshold : " << m_minPolygonAreaThreshold << std::endl;
+}
+
+/*!
+ Read sample information.
+
+ \throw vpException::fatalError if there was an unexpected number of data.
+
+ \param doc : Pointer to the document.
+ \param node : Pointer to the node of the sample information.
+*/
+void vpMbtXmlGenericParser::read_sample_deprecated(xmlDocPtr doc, xmlNodePtr node)
+{
+ bool step_node = false;
+ // bool nb_sample_node = false;
+
+ // current data values.
+ double d_stp = m_ecm.getSampleStep();
+
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::const_iterator iter_data = nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
+ case step:
+ d_stp = xmlReadIntChild(doc, dataNode);
+ step_node = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+ }
+
+ m_ecm.setSampleStep(d_stp);
+
+ if (!step_node)
+ 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 << " WARNING : This node (sample) is deprecated." << std::endl;
+ std::cout << " It should be moved in the ecm node (ecm : sample)." << std::endl;
+}
+
+#elif !defined(VISP_BUILD_SHARED_LIBS)
+// Work arround to avoid warning: libvisp_mbt.a(vpMbtXmlGenericParser.cpp.o)
+// has no symbols
+void dummy_vpMbtXmlGenericParser(){};
+#endif
diff --git a/modules/tracker/mbt/test/testTukeyEstimator.cpp b/modules/tracker/mbt/test/testTukeyEstimator.cpp
new file mode 100644
index 0000000..705ba74
--- /dev/null
+++ b/modules/tracker/mbt/test/testTukeyEstimator.cpp
@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * Description:
+ * Test Tukey M-Estimator.
+ *
+ *****************************************************************************/
+
+/*!
+ \example testTukeyEstimator.cpp
+
+ \brief Test Tukey M-Estimator.
+*/
+
+#include <cstdlib>
+#include <iostream>
+#include <time.h>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpGaussRand.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/mbt/vpMbtTukeyEstimator.h>
+
+int main(int /*argc*/, const char ** /*argv*/)
+{
+ size_t nb_elements = 1000;
+ int nb_iterations = 100;
+ double stdev = 0.5, mean = 0.0, noise_threshold = 1e-3;
+
+ vpGaussRand noise(stdev, mean);
+ noise.seed((unsigned int)time(NULL));
+
+ vpColVector residues_col((unsigned int)nb_elements);
+ vpColVector weights_col((unsigned int)nb_elements, 1.0), weights_col_save;
+ for (size_t i = 0; i < nb_elements; i++) {
+ residues_col[(unsigned int)i] = noise();
+ }
+
+ vpRobust robust((unsigned int)nb_elements);
+ robust.setThreshold(noise_threshold);
+ double t_robust = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ robust.MEstimator(vpRobust::TUKEY, residues_col, weights_col);
+ }
+ t_robust = vpTime::measureTimeMs() - t_robust;
+
+ {
+
+ vpMbtTukeyEstimator<double> tukey_estimator;
+ std::vector<double> residues(nb_elements);
+ for (size_t i = 0; i < residues.size(); i++) {
+ residues[i] = residues_col[(unsigned int)i];
+ }
+
+ std::vector<double> weights(nb_elements, 1);
+ double t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ tukey_estimator.MEstimator(residues, weights, noise_threshold);
+ }
+ t = vpTime::measureTimeMs() - t;
+
+ 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)) {
+ 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[" << i << "]=" << weights[i] << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ // Generate again for weights != 1
+ for (size_t i = 0; i < nb_elements; i++) {
+ residues_col[(unsigned int)i] = noise();
+ }
+ weights_col_save = weights_col;
+ t_robust = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ robust.MEstimator(vpRobust::TUKEY, residues_col, weights_col);
+ }
+ t_robust = vpTime::measureTimeMs() - t_robust;
+
+ {
+ vpMbtTukeyEstimator<float> tukey_estimator;
+ std::vector<float> residues(nb_elements);
+ std::vector<float> weights(nb_elements);
+ for (size_t i = 0; i < residues.size(); i++) {
+ residues[i] = (float)residues_col[(unsigned int)i];
+ weights[i] = (float)weights_col_save[(unsigned int)i];
+ }
+
+ double t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ tukey_estimator.MEstimator(residues, weights, (float)noise_threshold);
+ }
+ t = vpTime::measureTimeMs() - t;
+
+ 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)) {
+ 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[i] << std::endl;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ // Generate again for weights != 1 and vpColVector type
+ for (size_t i = 0; i < nb_elements; i++) {
+ residues_col[(unsigned int)i] = noise();
+ }
+ weights_col_save = weights_col;
+ t_robust = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ robust.MEstimator(vpRobust::TUKEY, residues_col, weights_col);
+ }
+ t_robust = vpTime::measureTimeMs() - t_robust;
+
+ {
+ vpMbtTukeyEstimator<double> tukey_estimator;
+ vpColVector residues = residues_col;
+ vpColVector weights = weights_col_save;
+
+ double t = vpTime::measureTimeMs();
+ for (int i = 0; i < nb_iterations; i++) {
+ tukey_estimator.MEstimator(residues, weights, noise_threshold);
+ }
+ t = vpTime::measureTimeMs() - t;
+
+ 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)) {
+ 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;
+ return EXIT_FAILURE;
+ }
+ }
+ }
+
+ std::cout << "vpMbtTukeyEstimator returns the same values than vpRobust::TUKEY." << std::endl;
+ return EXIT_SUCCESS;
+}
diff --git a/modules/tracker/me/CMakeLists.txt b/modules/tracker/me/CMakeLists.txt
index 26bfed2..08465f9 100644
--- a/modules/tracker/me/CMakeLists.txt
+++ b/modules/tracker/me/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/tracker/me/include/visp3/me/vpMe.h b/modules/tracker/me/include/visp3/me/vpMe.h
index ad05ed8..da60229 100644
--- a/modules/tracker/me/include/visp3/me/vpMe.h
+++ b/modules/tracker/me/include/visp3/me/vpMe.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,16 +39,16 @@
*****************************************************************************/
/*!
- \file vpMe.h
- \brief Moving edges
+ \file vpMe.h
+ \brief Moving edges
*/
#ifndef vpMe_H
#define vpMe_H
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
/*!
\class vpMe
@@ -63,9 +64,9 @@ public:
#else
private:
#endif
- double threshold ;//! Likelihood ratio threshold
- double mu1; //! Contrast continuity parameter (left boundary)
- double mu2; //! Contrast continuity parameter (right boundary)
+ double threshold; //! Likelihood ratio threshold
+ double mu1; //! Contrast continuity parameter (left boundary)
+ double mu2; //! Contrast continuity parameter (right boundary)
double min_samplestep;
unsigned int anglestep;
int mask_sign;
@@ -73,29 +74,38 @@ private:
double sample_step; //! Distance between sampled points (in pixels)
int ntotal_sample;
int points_to_track;
- //! convolution masks' size in pixels (masks are square), \warning should not be public, use setMaskSize() and getMaskSize() instead (kept public for compatibility reasons).
+ //! convolution masks' size in pixels (masks are square), \warning should
+ //! not be public, use setMaskSize() and getMaskSize() instead (kept public
+ //! for compatibility reasons).
unsigned int mask_size;
- //! the number of convolution masks available for tracking ; defines resolution. \warning Should not be public, use setMaskNumber() and getMaskNumber() instead (kept public for compatibility reasons).
+ //! the number of convolution masks available for tracking ; defines
+ //! resolution. \warning Should not be public, use setMaskNumber() and
+ //! getMaskNumber() instead (kept public for compatibility reasons).
unsigned int n_mask;
- //strip: defines a "security strip" such that Seek_Extremities()
- //cannot return a new extremity which is too close to the
- //frame borders which may cause Get_Sampling_Grid to refuse
- //the that extremity
+ // strip: defines a "security strip" such that Seek_Extremities()
+ // cannot return a new extremity which is too close to the
+ // frame borders which may cause Get_Sampling_Grid to refuse
+ // the that extremity
int strip;
- //int graph ;
- vpMatrix *mask ; //! Array of matrices defining the different masks (one for every angle step).
+ // int graph ;
+ vpMatrix *mask; //! Array of matrices defining the different masks (one for
+ //! every angle step).
public:
- vpMe() ;
- vpMe(const vpMe &me) ;
- virtual ~vpMe() ;
-
- const vpMe& operator=(const vpMe &me);
+ vpMe();
+ vpMe(const vpMe &me);
+ virtual ~vpMe();
+
+ vpMe &operator=(const vpMe &me);
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- const vpMe& operator=(const vpMe &&me);
+ vpMe &operator=(const vpMe &&me);
#endif
- void checkSamplestep(double &a) { if(a < min_samplestep) a = min_samplestep ; }
+ void checkSamplestep(double &a)
+ {
+ if (a < min_samplestep)
+ a = min_samplestep;
+ }
/*!
Return the angle step.
@@ -107,11 +117,11 @@ public:
\return the value of mask.
*/
- inline vpMatrix* getMask() const { return mask; }
+ inline vpMatrix *getMask() const { return mask; }
/*!
- Return the number of mask applied to determine the object contour. The number of mask determines the precision of
- the normal of the edge for every sample. If precision is 2deg, then there
- are 360/2 = 180 masks.
+ Return the number of mask applied to determine the object contour. The
+ number of mask determines the precision of the normal of the edge for
+ every sample. If precision is 2deg, then there are 360/2 = 180 masks.
\return the current number of mask.
*/
@@ -123,14 +133,16 @@ public:
*/
inline int getMaskSign() const { return mask_sign; }
/*!
- Return the actual mask size (in pixel) used to compute the image gradient and determine the object contour.
- The mask size defines the size of the convolution mask used to detect an edge.
+ Return the actual mask size (in pixel) used to compute the image gradient
+ and determine the object contour. The mask size defines the size of the
+ convolution mask used to detect an edge.
\return the current mask size.
*/
inline unsigned int getMaskSize() const { return mask_size; }
/*!
- Get the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
+ Get the minimum allowed sample step. Useful to specify a lower bound when
+ the sample step is changed.
\return Value of min_samplestep.
*/
@@ -158,7 +170,7 @@ public:
\return Value of points_to_track.
*/
- inline int getPointsToTrack() const { return points_to_track ; }
+ inline int getPointsToTrack() const { return points_to_track; }
/*!
Return the seek range on both sides of the reference pixel.
@@ -173,118 +185,119 @@ public:
inline int getStrip() const { return strip; }
/*!
- Return the likelihood threshold used to determined if the moving edge is valid or not.
+ Return the likelihood threshold used to determined if the moving edge is
+ valid or not.
\return Value of threshold.
*/
inline double getThreshold() const { return threshold; }
- void initMask() ;// convolution masks - offset computation
- void print( ) ;
-
+ void initMask(); // convolution masks - offset computation
+ void print();
+
/*!
Set the angle step.
\param a : new angle step.
*/
- void setAngleStep(const unsigned int &a) { anglestep = a ; }
+ void setAngleStep(const unsigned int &a) { anglestep = a; }
/*!
- Set the number of mask applied to determine the object contour. The number of mask determines the precision of
- the normal of the edge for every sample. If precision is 2deg, then there
- are 360/2 = 180 masks.
+ Set the number of mask applied to determine the object contour. The number
+ of mask determines the precision of the normal of the edge for every
+ sample. If precision is 2deg, then there are 360/2 = 180 masks.
\param a : the number of mask.
*/
- void setMaskNumber(const unsigned int &a) ;
-
+ void setMaskNumber(const unsigned int &a);
+
/*!
Set the mask sign.
-
+
\param a : new mask sign.
*/
- void setMaskSign(const int &a){ mask_sign = a ; }
-
+ void setMaskSign(const int &a) { mask_sign = a; }
+
/*!
- Set the mask size (in pixel) used to compute the image gradient and determine the object contour.
- The mask size defines the size of the convolution mask used to detect an edge.
-
+ Set the mask size (in pixel) used to compute the image gradient and
+ determine the object contour. The mask size defines the size of the
+ convolution mask used to detect an edge.
+
\param a : new mask size.
*/
void setMaskSize(const unsigned int &a);
/*!
- Set the minimum allowed sample step. Useful to specify a lower bound when the sample step is changed.
+ Set the minimum allowed sample step. Useful to specify a lower bound when
+ the sample step is changed.
\param min : new minimum sample step.
*/
- void setMinSampleStep(const double &min) { min_samplestep = min ; }
+ void setMinSampleStep(const double &min) { min_samplestep = min; }
/*!
Set the minimum image contrast allowed to detect a contour.
-
+
\param mu_1 : new mu1.
*/
- void setMu1(const double &mu_1) { this->mu1 = mu_1 ; }
-
+ void setMu1(const double &mu_1) { this->mu1 = mu_1; }
+
/*!
Set the maximum image contrast allowed to detect a contour.
-
+
\param mu_2 : new mu2.
*/
- void setMu2(const double &mu_2) { this->mu2 = mu_2 ; }
-
+ void setMu2(const double &mu_2) { this->mu2 = mu_2; }
+
/*!
Set how many discretizied points are used to track the feature.
-
+
\param nb : new total number of sample.
*/
void setNbTotalSample(const int &nb) { ntotal_sample = nb; }
-
- /*!
+
+ /*!
Set the number of points to track.
-
+
\param n : new number of points to track.
-
- \warning This method is useful only for the vpMeNurbsTracker.
+
+ \warning This method is useful only for the vpMeNurbsTracker.
*/
void setPointsToTrack(const int &n) { points_to_track = n; }
-
+
/*!
Set the seek range on both sides of the reference pixel.
-
+
\param r : new range.
*/
- void setRange(const unsigned int &r) { range = r ; }
-
+ void setRange(const unsigned int &r) { range = r; }
+
/*!
Set the minimum distance in pixel between two discretized points.
-
+
\param s : new sample_step.
*/
- void setSampleStep(const double &s) { sample_step = s ; }
-
+ void setSampleStep(const double &s) { sample_step = s; }
+
/*!
Get the minimum distance in pixel between two discretized points.
\return Value of sample_step.
*/
- inline double getSampleStep() const { return sample_step ; }
-
+ inline double getSampleStep() const { return sample_step; }
+
/*!
Set the number of pixels that are ignored around the image borders.
-
+
\param a : new strip.
*/
- void setStrip(const int &a) { strip = a ; }
-
+ void setStrip(const int &a) { strip = a; }
+
/*!
- Set the likelihood threshold used to determined if the moving edge is valid or not.
-
+ Set the likelihood threshold used to determined if the moving edge is
+ valid or not.
+
\param t : new threshold.
*/
- void setThreshold(const double &t) { threshold = t ; }
- };
-
+ void setThreshold(const double &t) { threshold = t; }
+};
#endif
-
-
diff --git a/modules/tracker/me/include/visp3/me/vpMeEllipse.h b/modules/tracker/me/include/visp3/me/vpMeEllipse.h
index 28234cd..901681c 100644
--- a/modules/tracker/me/include/visp3/me/vpMeEllipse.h
+++ b/modules/tracker/me/include/visp3/me/vpMeEllipse.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,51 +44,61 @@
#ifndef vpMeEllipse_HH
#define vpMeEllipse_HH
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
-#include <visp3/me/vpMeTracker.h>
-#include <visp3/me/vpMeSite.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeTracker.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpImage.h>
-#include <math.h>
#include <list>
+#include <math.h>
/*!
- \class vpMeEllipse
+ \class vpMeEllipse
\ingroup module_me
\brief Class that tracks an ellipse moving edges.
- In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of the image frame (For more information about the image frame see the vpImagePoint documentation) that satisfy the implicit equation :
+ In this class, an ellipse is defined as the set of points \f$ (i,j) \f$ of
+ the image frame (For more information about the image frame see the
+ vpImagePoint documentation) that satisfy the implicit equation :
\f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
- If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of points \f$ (i,j) \f$ represents a circle.
+ If \f$ K_0 \f$ is equal to 1 and \f$ K_1 \f$ is equal to 0 the the set of
+ points \f$ (i,j) \f$ represents a circle.
The five parameters are stored in the public attribute K.
- An ellipse is also defined thanks to three other parameter which are \f$ a \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
- major axis and the i axis of the image frame \f$ (i,j) \f$. The following figure shows better meaning of those parameters.
+ An ellipse is also defined thanks to three other parameter which are \f$ a
+ \f$, \f$ b \f$ and \f$ e \f$. \f$ a \f$ represents the semiminor axis and
+ \f$ b \f$ is the semimajor axis. Here \f$ e \f$ is the angle made by the
+ major axis and the i axis of the image frame \f$ (i,j) \f$. The following
+ figure shows better meaning of those parameters.
\image html vpMeEllipse.gif
\image latex vpMeEllipse.ps width=10cm
- It is possible to compute the coordinates \f$ (i,j) \f$ of a point which belongs to the ellipse thanks to the following equations :
+ It is possible to compute the coordinates \f$ (i,j) \f$ of a point which
+ belongs to the ellipse thanks to the following equations :
\f[ i = i_c + b cos(e) cos(\alpha) - a sin(e) sin(\alpha) \f]
\f[ j = j_c + b sin(e) cos(\alpha) + a cos(e) sin(\alpha) \f]
- Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse center in the image frame and \f$ \alpha \f$ is an angle beetween \f$ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
+ Here the coordinates \f$ (i_c,j_c) \f$ are the coordinates of the ellipse
+ center in the image frame and \f$ \alpha \f$ is an angle beetween \f$
+ [0,2\pi] \f$ and which enables to describe all the points of the ellipse.
\image html vpMeEllipse2.gif
\image latex vpMeEllipse2.ps width=10cm
The example below available in tutorial-me-ellipse-tracker.cpp and described
- in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to use this class.
+ in \ref tutorial-tracking-me, section \ref tracking_me_ellipse shows how to
+ use this class.
\include tutorial-me-ellipse-tracker.cpp
@@ -95,178 +106,199 @@
class VISP_EXPORT vpMeEllipse : public vpMeTracker
{
public:
- vpMeEllipse() ;
- vpMeEllipse(const vpMeEllipse &meellipse) ;
- virtual ~vpMeEllipse() ;
+ vpMeEllipse();
+ vpMeEllipse(const vpMeEllipse &meellipse);
+ virtual ~vpMeEllipse();
/*!
\return Expected number of moving edges to track along the ellipse.
*/
- int getExpectedDensity() {return (int)expecteddensity;};
- void track(const vpImage<unsigned char>& Im);
+ int getExpectedDensity() { return (int)expecteddensity; };
+ void track(const vpImage<unsigned char> &Im);
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint* iP);
+ void initTracking(const vpImage<unsigned char> &I);
+ void initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint *iP);
void initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP);
- void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p, double low_alpha, double high_alpha) ;
- void display(const vpImage<unsigned char>&I, vpColor col) ;
- void display(const vpImage<unsigned char>& I) {vpMeTracker::display(I);} //Shouldn't be here since it's already in vpMeTracker
- void printParameters() ;
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p,
+ double low_alpha, double high_alpha);
+ void display(const vpImage<unsigned char> &I, vpColor col);
+ void display(const vpImage<unsigned char> &I)
+ {
+ vpMeTracker::display(I);
+ } // Shouldn't be here since it's already in vpMeTracker
+ void printParameters();
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
/*!
@name Deprecated functions
*/
//@{
- void initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- unsigned *i, unsigned *j) ;
- //@}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
+ void initTracking(const vpImage<unsigned char> &I, const unsigned int n, unsigned *i, unsigned *j);
+//@}
+#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
+
/*!
- Gets the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
-
+ Gets the 0 order moment \f$ m_{00} \f$ which represents the area of the
+ ellipse.
+
\return the value of \f$ m_{00} \f$.
*/
- inline double get_m00() const {return m00;}
-
+ inline double get_m00() const { return m00; }
+
/*!
- Gets the 1 order raw moment \f$ m_{10} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
+ Gets the 1 order raw moment \f$ m_{10} \f$ with \f$ m_{nm} = \sum_{i,j}i^n
+ j^m \f$.
+
\return the value of \f$ m_{10} \f$.
*/
- inline double get_m10() const {return m10;}
-
+ inline double get_m10() const { return m10; }
+
/*!
- Gets the 1 order raw moment \f$ m_{01} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
+ Gets the 1 order raw moment \f$ m_{01} \f$ with \f$ m_{nm} = \sum_{i,j}i^n
+ j^m \f$.
+
\return the value of \f$ m_{01} \f$.
*/
- inline double get_m01() const {return m01;}
-
+ inline double get_m01() const { return m01; }
+
/*!
- Gets the 2 order raw moment \f$ m_{11} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
+ Gets the 2 order raw moment \f$ m_{11} \f$ with \f$ m_{nm} = \sum_{i,j}i^n
+ j^m \f$.
+
\return the value of \f$ m_{11} \f$.
*/
- inline double get_m11() const {return m11;}
-
+ inline double get_m11() const { return m11; }
+
/*!
- Gets the 2 order raw moment \f$ m_{20} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
+ Gets the 2 order raw moment \f$ m_{20} \f$ with \f$ m_{nm} = \sum_{i,j}i^n
+ j^m \f$.
+
\return the value of \f$ m_{11} \f$.
*/
- inline double get_m20() const {return m20;}
-
+ inline double get_m20() const { return m20; }
+
/*!
- Gets the 2 order raw moment \f$ m_{02} \f$ with \f$ m_{nm} = \sum_{i,j}i^n j^m \f$.
-
+ Gets the 2 order raw moment \f$ m_{02} \f$ with \f$ m_{nm} = \sum_{i,j}i^n
+ j^m \f$.
+
\return the value of \f$ m_{11} \f$.
*/
- inline double get_m02() const {return m02;}
-
+ inline double get_m02() const { return m02; }
+
/*!
Gets the 2 order central moment \f$ \mu_{11} \f$.
-
+
\return the value of \f$ \mu_{11} \f$.
*/
- inline double get_mu11() const {return mu11;}
-
+ inline double get_mu11() const { return mu11; }
+
/*!
Gets the 2 order central moment \f$ \mu_{02} \f$.
-
+
\return the value of \f$ \mu_{02} \f$.
*/
- inline double get_mu02() const {return mu02;}
-
+ inline double get_mu02() const { return mu02; }
+
/*!
Gets the 2 order central moment \f$ \mu_{20} \f$.
-
+
\return the value of \f$ \mu_{20} \f$.
*/
- inline double get_mu20() const {return mu20;}
-
+ inline double get_mu20() const { return mu20; }
+
/*!
Gets the center of the ellipse.
*/
- inline vpImagePoint getCenter() const {return iPc; }
-
+ inline vpImagePoint getCenter() const { return iPc; }
+
/*!
Gets the semiminor axis of the ellipse.
*/
- inline double getA() const {return a; }
-
+ inline double getA() const { return a; }
+
/*!
Gets the semimajor axis of the ellipse.
*/
- inline double getB() const {return b; }
-
+ inline double getB() const { return b; }
+
/*!
- Gets the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+ Gets the angle made by the major axis and the i axis of the image frame
+ \f$ (i,j) \f$
*/
- inline double getE() const {return e; }
-
+ inline double getE() const { return e; }
+
/*!
Gets the equation parameters of the ellipse
*/
- void getEquationParam(double &A, double &B, double &E) { A = a; B = b; E = e; }
-
+ void getEquationParam(double &A, double &B, double &E)
+ {
+ A = a;
+ B = b;
+ E = e;
+ }
+
/*!
Gets the smallest \f$ alpha \f$ angle
*/
inline double getSmallestAngle() { return alpha1; }
-
+
/*!
Gets the highest \f$ alpha \f$ angle
*/
inline double getHighestAngle() { return alpha2; }
-
- /*!
- Set the new threshold for the robust estimation of the parameters of the
- ellipse equation.
- If the weight of a point is below this threshold, this one is removed from
- the list of tracked meSite.
- Value must be between 0 (never rejected) and 1 (always rejected).
-
- \param threshold : The new value of the threshold.
- */
- void setThresholdRobust(const double threshold){
- if(threshold<0){
- thresholdWeight = 0;
- }else if(threshold>1){
- thresholdWeight = 1;
- }else{
- thresholdWeight = threshold;
- }
- }
+
+ /*!
+ Set the new threshold for the robust estimation of the parameters of the
+ ellipse equation.
+ If the weight of a point is below this threshold, this one is removed from
+ the list of tracked meSite.
+ Value must be between 0 (never rejected) and 1 (always rejected).
+
+ \param threshold : The new value of the threshold.
+ */
+ void setThresholdRobust(const double threshold)
+ {
+ if (threshold < 0) {
+ thresholdWeight = 0;
+ } else if (threshold > 1) {
+ thresholdWeight = 1;
+ } else {
+ thresholdWeight = threshold;
+ }
+ }
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
public:
#else
protected:
#endif
- /*! Parameters of the ellipse to define the set of points that satisfy the implicit equation :
- \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
+ /*! Parameters of the ellipse to define the set of points that satisfy the
+ implicit equation : \f[ i^2 + K_0j^2 + 2K_1ij + 2K_2i + 2K_3j + K4 = 0 \f]
*/
- vpColVector K ;
+ vpColVector K;
//! The coordinates of the ellipse center.
vpImagePoint iPc;
//! \f$ a \f$ is the semiminor axis of the ellipse.
double a;
//! \f$ b \f$ is the semimajor axis of the ellipse.
double b;
- //! \f$ e \f$ is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
+ //! \f$ e \f$ is the angle made by the major axis and the i axis of the
+ //! image frame \f$ (i,j) \f$.
double e;
protected:
- //! The coordinates of the point corresponding to the smallest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
+ //! The coordinates of the point corresponding to the smallest \f$ alpha \f$
+ //! angle. More things about the \f$ alpha \f$ are given at the beginning of
+ //! the class description.
vpImagePoint iP1;
- //! The coordinates of the point corresponding to the highest \f$ alpha \f$ angle. More things about the \f$ alpha \f$ are given at the beginning of the class description.
+ //! The coordinates of the point corresponding to the highest \f$ alpha \f$
+ //! angle. More things about the \f$ alpha \f$ are given at the beginning of
+ //! the class description.
vpImagePoint iP2;
//! The smallest \f$ alpha \f$ angle.
- double alpha1 ;
+ double alpha1;
//! The highest \f$ alpha \f$ angle.
- double alpha2 ;
+ double alpha2;
//! Value of cos(e).
double ce;
//! Value of sin(e).
@@ -276,26 +308,26 @@ protected:
//! Surface
double m00;
//! Second order central moments
- double mu11,mu20, mu02;
+ double mu11, mu20, mu02;
//! First order raw moments
- double m10,m01;
+ double m10, m01;
//! Second order raw moments
- double m11,m02,m20;
+ double m11, m02, m20;
//! Threshold for the robust least square.
double thresholdWeight;
//! Expected number of me to track along the ellipse.
double expecteddensity;
private:
- void computeAngle(vpImagePoint pt1, vpImagePoint pt2);
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void leastSquare() ;
+ void computeAngle(const vpImagePoint &pt1, const vpImagePoint &pt);
+ void sample(const vpImage<unsigned char> &image);
+ void reSample(const vpImage<unsigned char> &I);
+ void leastSquare();
void updateTheta();
- void suppressPoints() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
+ void suppressPoints();
+ void seekExtremities(const vpImage<unsigned char> &I);
void setExtremities();
- void getParameters() ;
+ void getParameters();
void computeMoments();
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
@@ -303,32 +335,19 @@ private:
@name Deprecated functions
*/
//@{
- void computeAngle(int ip1, int jp1,int ip2, int jp2) ;
- void computeAngle(int ip1, int jp1, double &alpha1,
- int ip2, int jp2, double &alpha2) ;
- /*!
- \deprecated This function does nothing and shouldn't be used since we modify
- the code to detect automatically if the object to track becomes a circle
- rather an ellipse.
- */
- vp_deprecated void setCircle(bool ) { }
-
- //@}
-#endif //VISP_BUILD_DEPRECATED_FUNCTIONS
-
-//Static Function
-public:
- static void display(const vpImage<unsigned char>& I, const vpImagePoint ¢er,
- const double &A, const double &B, const double &E,
- const double & smallalpha, const double &highalpha,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I, const vpImagePoint ¢er,
- const double &A, const double &B, const double &E,
- const double & smallalpha, const double &highalpha,
- const vpColor &color = vpColor::green,
- unsigned int thickness=1);
+ void computeAngle(int ip1, int jp1, int ip2, int jp2);
+ void computeAngle(int ip1, int jp1, double &alpha1, int ip2, int jp2, double &alpha2);
+//@}
+#endif // VISP_BUILD_DEPRECATED_FUNCTIONS
+ // Static Function
+public:
+ static void display(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &A, const double &B,
+ const double &E, const double &smallalpha, const double &highalpha,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &A, const double &B,
+ const double &E, const double &smallalpha, const double &highalpha,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
};
#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeLine.h b/modules/tracker/me/include/visp3/me/vpMeLine.h
index 043a685..ad2c9d3 100644
--- a/modules/tracker/me/include/visp3/me/vpMeLine.h
+++ b/modules/tracker/me/include/visp3/me/vpMeLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,12 +44,12 @@
#ifndef vpMeLine_HH
#define vpMeLine_HH
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/me/vpMeTracker.h>
-#include <math.h>
#include <iostream>
+#include <math.h>
/*!
\class vpMeLine
@@ -93,15 +94,13 @@
in \ref tutorial-tracking-me shows how to use this class.
\include tutorial-me-line-tracker.cpp
-*/
-/*
The code below shows how to use this class.
\code
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpImage.h>
-#include <visp3/me/vpMeLine.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/me/vpMeLine.h>
int main()
{
@@ -114,7 +113,7 @@ int main()
I[i][j] = 255;
}
}
-
+
// Set the moving-edges tracker parameters
vpMe me;
me.setRange(25);
@@ -126,7 +125,7 @@ int main()
line.setMe(&me);
// Initialize the location of the vertical line to track
- vpImagePoint ip1, ip2; // Two points belonging to the line to track
+ vpImagePoint ip1, ip2; // Two points belonging to the line to track
ip1.set_i( 120 );
ip1.set_j( 119 );
ip2.set_i( 170 );
@@ -145,26 +144,27 @@ int main()
}
\endcode
- \note It is possible to display the line as an overlay. For that you
+ \note It is possible to display the line as an overlay. For that you
must use the display function of the class vpMeLine.
*/
class VISP_EXPORT vpMeLine : public vpMeTracker
{
private:
- static void update_indices(double theta,int incr,int i,int j,int& i1,int& i2,int& j1,int& j2);
-
+ static void update_indices(double theta, int incr, int i, int j, int &i1, int &i2, int &j1, int &j2);
+
protected:
- vpMeSite PExt[2] ;
+ vpMeSite PExt[2];
- double rho, theta ;
- double delta ,delta_1;
+ double rho, theta;
+ double delta, delta_1;
double angle, angle_1;
int sign;
- //! Flag to specify wether the intensity of the image at the middle point is used to compute the sign of rho or not.
+ //! Flag to specify wether the intensity of the image at the middle point is
+ //! used to compute the sign of rho or not.
bool _useIntensityForRho;
-
+
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
public:
#else
@@ -176,85 +176,83 @@ protected:
double c; //!< Parameter c of the line equation a*i + b*j + c = 0
public:
- vpMeLine() ;
+ vpMeLine();
vpMeLine(const vpMeLine &meline);
- virtual ~vpMeLine() ;
+ virtual ~vpMeLine();
- void display(const vpImage<unsigned char>& I, vpColor col) ;
+ void display(const vpImage<unsigned char> &I, vpColor col);
- void track(const vpImage<unsigned char>& Im);
+ void track(const vpImage<unsigned char> &Im);
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void leastSquare() ;
+ void sample(const vpImage<unsigned char> &image);
+ void reSample(const vpImage<unsigned char> &I);
+ void leastSquare();
void updateDelta();
- void setExtremities() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
- void suppressPoints() ;
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2) ;
-
- void computeRhoTheta(const vpImage<unsigned char> &I) ;
- double getRho() const ;
- double getTheta() const ;
- void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2) ;
-
+ void setExtremities();
+ void seekExtremities(const vpImage<unsigned char> &I);
+ void suppressPoints();
+
+ void initTracking(const vpImage<unsigned char> &I);
+ void initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2);
+
+ void computeRhoTheta(const vpImage<unsigned char> &I);
+ double getRho() const;
+ double getTheta() const;
+ void getExtremities(vpImagePoint &ip1, vpImagePoint &ip2);
+
/*!
Gets the equation parameters of the line
*/
- void getEquationParam(double &A, double &B, double &C) { A = a; B = b; C = c; }
-
+ void getEquationParam(double &A, double &B, double &C)
+ {
+ A = a;
+ B = b;
+ C = c;
+ }
+
/*!
Gets parameter a of the line equation a*i + b*j + c = 0
*/
- inline double getA() const {return a; }
-
+ inline double getA() const { return a; }
+
/*!
Gets parameter b of the line equation a*i + b*j + c = 0
*/
- inline double getB() const {return b; }
-
+ inline double getB() const { return b; }
+
/*!
Gets parameter c of the line equation a*i + b*j + c = 0
*/
- inline double getC() const {return c; }
+ 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
- attribute based on the intensity near the middle point of the line. This is
- usually done to distinguish between a black/white and a white/black edge but
- it may be source of problem (ex. for a servoing example) when this point can
- be occluded.
+ attribute based on the intensity near the middle point of the line. This
+ is usually done to distinguish between a black/white and a white/black
+ edge but it may be source of problem (ex. for a servoing example) when
+ this point can be occluded.
\param useIntensityForRho : new value of the flag.
*/
- inline void computeRhoSignFromIntensity(const bool useIntensityForRho){
- _useIntensityForRho = useIntensityForRho;
- }
+ inline void computeRhoSignFromIntensity(const bool useIntensityForRho) { _useIntensityForRho = useIntensityForRho; }
-//Static Functions
-public:
- static void display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
-
- static void display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
- static void display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color = vpColor::green, unsigned int thickness=1);
+ // Static Functions
+public:
+ static void display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
+ const double &B, const double &C, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
+ const double &B, const double &C, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1);
+
+ static void display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
+ static void display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1);
};
#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeNurbs.h b/modules/tracker/me/include/visp3/me/vpMeNurbs.h
index 03acd4a..407aa36 100644
--- a/modules/tracker/me/include/visp3/me/vpMeNurbs.h
+++ b/modules/tracker/me/include/visp3/me/vpMeNurbs.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,20 +38,21 @@
/*!
\file vpMeNurbs.h
- \brief Moving edges on a form reprsented by a NURBS (Non Uniform Rational B-Spline)
+ \brief Moving edges on a form reprsented by a NURBS (Non Uniform Rational
+ B-Spline)
*/
#ifndef vpMeNurbs_HH
#define vpMeNurbs_HH
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/me/vpMeTracker.h>
#include <visp3/me/vpNurbs.h>
-#include <math.h>
#include <iostream>
#include <list>
+#include <math.h>
/*!
\class vpMeNurbs
@@ -59,16 +61,18 @@
\brief Class that tracks in an image a edge defined by a Nurbs.
- The advantage of this class is that it enables to track an edge whose equation is
- not known in advance. At each iteration, the Nurbs corresponding to the edge is computed.
-
- It is possible to have a direct access to the nurbs. It is indeed a public parameter.
+ The advantage of this class is that it enables to track an edge whose
+equation is not known in advance. At each iteration, the Nurbs corresponding
+to the edge is computed.
+
+ It is possible to have a direct access to the nurbs. It is indeed a public
+parameter.
The code below shows how to use this class.
\code
#include <visp3/core/vpImage.h>
-#include <visp3/me/vpMeNurbs.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/me/vpMeNurbs.h>
int main()
{
@@ -114,112 +118,113 @@ int main()
}
\endcode
- \note It is possible to display the nurbs as an overlay. For that you
+ \note It is possible to display the nurbs as an overlay. For that you
must use the display function of the class vpMeNurbs.
-
- \note In case of an edge which is not smooth, it can be interesting to use the
- canny detection to find the extremities. In this case, use the method
- setEnableCannyDetection to enable it. Warning : This function requires OpenCV.
+
+ \note In case of an edge which is not smooth, it can be interesting to use
+the canny detection to find the extremities. In this case, use the method
+ setEnableCannyDetection to enable it. Warning : This function requires
+OpenCV.
*/
class VISP_EXPORT vpMeNurbs : public vpMeTracker
{
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- public:
- #else
- private:
- #endif
- //! The Nurbs which represents the tracked edge.
- vpNurbs nurbs;
-
- private:
- //! Size of the nurbs
- double dist;
- //! Number of control points used to compute the Nurbs.
- unsigned int nbControlPoints;
- //! counter used to trigger the canny edge detection at the beginning of the Nurbs.
- int beginPtFound;
- //! counter used to trigger the canny edge detection at the end of the Nurbs.
- int endPtFound;
- //! True if the canny detection has to be used during the extremities search.
- bool enableCannyDetection;
- //! First canny threshold
- double cannyTh1;
- //! Second canny threshold
- double cannyTh2;
-
- public:
- vpMeNurbs();
- vpMeNurbs(const vpMeNurbs &menurbs);
- virtual ~vpMeNurbs();
-
- /*!
- Sets the number of control points used to compute the Nurbs.
-
- \param nb_point : The number of control points used to compute the Nurbs.
- */
- void setNbControlPoints(const unsigned int nb_point) {this->nbControlPoints = nb_point;}
-
- /*!
- Enables or disables the canny detection used during the extremities search.
-
- \param enable_canny : if true it enables the canny detection.
- */
- void setEnableCannyDetection(const bool enable_canny) {this->enableCannyDetection =enable_canny;}
-
- /*!
- Enables to set the two thresholds use by the canny detection.
-
- \param th1 : The first threshold;
- \param th2 : The second threshold;
- */
- void setCannyThreshold(const double th1, const double th2)
- {
- this->cannyTh1 = th1;
- this->cannyTh2 = th2;
- }
-
- void initTracking(const vpImage<unsigned char> &I) ;
- void initTracking(const vpImage<unsigned char> &I,
- const std::list<vpImagePoint> &ptList) ;
-
- void track(const vpImage<unsigned char>& Im);
-
- void sample(const vpImage<unsigned char>&image);
- void reSample(const vpImage<unsigned char> &I) ;
- void updateDelta();
- void setExtremities() ;
- void seekExtremities(const vpImage<unsigned char> &I) ;
- void seekExtremitiesCanny(const vpImage<unsigned char> &I) ;
- void suppressPoints() ;
-
- void supressNearPoints();
- void localReSample(const vpImage<unsigned char> &I);
-
- /*!
- Gets the nurbs;
- */
- inline vpNurbs getNurbs() const {return nurbs; }
-
- void display(const vpImage<unsigned char>& I, vpColor col) ;
-
- private:
- bool computeFreemanChainElement(const vpImage<unsigned char> &I,
- vpImagePoint &iP,
- unsigned int &element);
-
- bool hasGoodLevel(const vpImage<unsigned char>& I,
- const vpImagePoint& iP) const;
-
- bool isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const;
-
- void computeFreemanParameters( unsigned int element, vpImagePoint &diP);
-
- bool farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP);
-
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+public:
+#else
+private:
+#endif
+ //! The Nurbs which represents the tracked edge.
+ vpNurbs nurbs;
+
+private:
+ //! Size of the nurbs
+ double dist;
+ //! Number of control points used to compute the Nurbs.
+ unsigned int nbControlPoints;
+ //! counter used to trigger the canny edge detection at the beginning of the
+ //! Nurbs.
+ int beginPtFound;
+ //! counter used to trigger the canny edge detection at the end of the
+ //! Nurbs.
+ int endPtFound;
+ //! True if the canny detection has to be used during the extremities
+ //! search.
+ bool enableCannyDetection;
+ //! First canny threshold
+ double cannyTh1;
+ //! Second canny threshold
+ double cannyTh2;
+
+public:
+ vpMeNurbs();
+ vpMeNurbs(const vpMeNurbs &menurbs);
+ virtual ~vpMeNurbs();
+
+ /*!
+ Sets the number of control points used to compute the Nurbs.
+
+ \param nb_point : The number of control points used to compute the Nurbs.
+ */
+ void setNbControlPoints(const unsigned int nb_point) { this->nbControlPoints = nb_point; }
+
+ /*!
+ Enables or disables the canny detection used during the extremities
+ search.
+
+ \param enable_canny : if true it enables the canny detection.
+ */
+ void setEnableCannyDetection(const bool enable_canny) { this->enableCannyDetection = enable_canny; }
+
+ /*!
+ Enables to set the two thresholds use by the canny detection.
+
+ \param th1 : The first threshold;
+ \param th2 : The second threshold;
+ */
+ void setCannyThreshold(const double th1, const double th2)
+ {
+ this->cannyTh1 = th1;
+ this->cannyTh2 = th2;
+ }
+
+ void initTracking(const vpImage<unsigned char> &I);
+ void initTracking(const vpImage<unsigned char> &I, const std::list<vpImagePoint> &ptList);
+
+ void track(const vpImage<unsigned char> &Im);
+
+ void sample(const vpImage<unsigned char> &image);
+ void reSample(const vpImage<unsigned char> &I);
+ void updateDelta();
+ void setExtremities();
+ void seekExtremities(const vpImage<unsigned char> &I);
+ void seekExtremitiesCanny(const vpImage<unsigned char> &I);
+ void suppressPoints();
+
+ void supressNearPoints();
+ void localReSample(const vpImage<unsigned char> &I);
+
+ /*!
+ Gets the nurbs;
+ */
+ inline vpNurbs getNurbs() const { return nurbs; }
+
+ void display(const vpImage<unsigned char> &I, vpColor col);
+
+private:
+ bool computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element);
+
+ bool hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const;
+
+ bool isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const;
+
+ void computeFreemanParameters(unsigned int element, vpImagePoint &diP);
+
+ bool farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP);
+
public:
- static void display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color = vpColor::green);
- static void display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color = vpColor::green);
+ static void display(const vpImage<unsigned char> &I, vpNurbs &n, vpColor color = vpColor::green);
+ static void display(const vpImage<vpRGBa> &I, vpNurbs &n, vpColor color = vpColor::green);
};
#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeSite.h b/modules/tracker/me/include/visp3/me/vpMeSite.h
index ec0f532..92c58fb 100644
--- a/modules/tracker/me/include/visp3/me/vpMeSite.h
+++ b/modules/tracker/me/include/visp3/me/vpMeSite.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,18 +39,16 @@
*****************************************************************************/
/*!
- \file vpMeSite.h
- \brief Moving edges
+ \file vpMeSite.h
+ \brief Moving edges
*/
-
-
#ifndef vpMeSite_H
#define vpMeSite_H
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/me/vpMe.h>
/*!
@@ -63,7 +62,7 @@
- Bug fix: rewrote application of masks to use the temporal
information instead of applying both temporal masks to the same
image. ie: spatial -> spatio/temporal
-
+
- Added new tracking function to choose the most similar edge
amongst all edges found.
@@ -72,16 +71,9 @@
class VISP_EXPORT vpMeSite
{
public:
- typedef enum
- {
- NONE,
- RANGE,
- RESULT,
- RANGE_RESULT
- } vpMeSiteDisplayType;
-
- typedef enum
- {
+ typedef enum { NONE, RANGE, RESULT, RANGE_RESULT } vpMeSiteDisplayType;
+
+ typedef enum {
NO_SUPPRESSION = 0,
CONSTRAST = 1,
THRESHOLD = 2,
@@ -89,135 +81,133 @@ public:
TOO_NEAR = 4,
UNKNOW = 5
} vpMeSiteState;
-
+
public:
- int i,j ;
- int i_1, j_1 ;
- double ifloat, jfloat ;
- unsigned char v ;
- int mask_sign ;
+ int i, j;
+ int i_1, j_1;
+ double ifloat, jfloat;
+ unsigned char v;
+ int mask_sign;
// Angle of tangent at site
double alpha;
// Convolution of Site in previous image
- double convlt ;
- // Convolution of Site in previous image
- double normGradient ;
+ double convlt;
+ // Convolution of Site in previous image
+ double normGradient;
// Uncertainty of point given as a probability between 0 and 1
double weight;
-
+
private:
- vpMeSiteDisplayType selectDisplay ;
+ vpMeSiteDisplayType selectDisplay;
vpMeSiteState state;
public:
- void init() ;
- void init(double ip, double jp, double alphap) ;
- void init(double ip, double jp, double alphap, double convltp) ;
- void init(double ip, double jp, double alphap, double convltp, int sign) ;
-
- vpMeSite () ;
- vpMeSite(double ip, double jp) ;
- vpMeSite (const vpMeSite &mesite) ;
- virtual ~vpMeSite() {} ;
-
- void display(const vpImage<unsigned char>& I);
-
- double convolution(const vpImage<unsigned char>& ima, const vpMe *me) ;
-
- vpMeSite *getQueryList(const vpImage<unsigned char> &I, const int range) ;
-
- void track(const vpImage<unsigned char>& im,
- const vpMe *me,
- const bool test_contraste=true);
-
+ void init();
+ void init(double ip, double jp, double alphap);
+ void init(double ip, double jp, double alphap, double convltp);
+ void init(double ip, double jp, double alphap, double convltp, int sign);
+
+ vpMeSite();
+ vpMeSite(double ip, double jp);
+ vpMeSite(const vpMeSite &mesite);
+ virtual ~vpMeSite(){};
+
+ void display(const vpImage<unsigned char> &I);
+
+ double convolution(const vpImage<unsigned char> &ima, const vpMe *me);
+
+ vpMeSite *getQueryList(const vpImage<unsigned char> &I, const int range);
+
+ void track(const vpImage<unsigned char> &im, const vpMe *me, const bool test_contraste = true);
+
/*!
Set the angle of tangent at site
\param a : new value of alpha
*/
void setAlpha(const double &a) { alpha = a; }
-
+
/*!
Get the angle of tangent at site
\return value of alpha
*/
inline double getAlpha() const { return alpha; }
-
-
- void setDisplay(vpMeSiteDisplayType select) { selectDisplay = select ; }
-
+
+ void setDisplay(vpMeSiteDisplayType select) { selectDisplay = select; }
+
/*!
Get the i coordinate (integer)
\return value of i
*/
inline int get_i() const { return i; }
-
+
/*!
Get the j coordinate (f)
\return value of j
*/
inline int get_j() const { return j; }
-
+
/*!
Get the i coordinate (double)
\return value of i
*/
inline double get_ifloat() const { return ifloat; }
-
+
/*!
Get the j coordinate (double)
\return value of j
*/
inline double get_jfloat() const { return jfloat; }
-
+
/*!
Set the state of the site
\param flag : flag corresponding to vpMeSiteState
-
+
\sa vpMeSiteState
*/
- void setState(const vpMeSiteState &flag){
- state = flag;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ void setState(const vpMeSiteState &flag)
+ {
+ state = flag;
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
suppress = (int)flag;
- #endif
+#endif
}
-
+
/*!
Get the state of the site
\return flag corresponding to vpMeSiteState
*/
inline vpMeSiteState getState() const { return state; }
-
+
/*!
Set the weight of the site
\param w : new value of weight
*/
void setWeight(const double &w) { weight = w; }
-
+
/*!
Get the weight of the site
\return value of weight
*/
inline double getWeight() const { return weight; }
-
-//Operators
- vpMeSite &operator=(const vpMeSite &m) ;
- int operator!=(const vpMeSite &m) ;
-
- friend VISP_EXPORT std::ostream& operator<<(std::ostream& os, vpMeSite& vpMeS);
-
-//Static functions
+
+ // Operators
+ vpMeSite &operator=(const vpMeSite &m);
+ int operator!=(const vpMeSite &m);
+
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, vpMeSite &vpMeS);
+
+ // Static functions
/*!
Compute the distance \f$ |S1 - S2| = \sqrt{(i_1-i_2)^2+(j_1-j_2)^2} \f$
@@ -226,9 +216,11 @@ public:
\return the distance between the two sites.
*/
- static double distance (const vpMeSite S1, const vpMeSite S2) {
- return(sqrt(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat)));}
-
+ static double distance(const vpMeSite &S1, const vpMeSite &S2)
+ {
+ return (sqrt(vpMath::sqr(S1.ifloat - S2.ifloat) + vpMath::sqr(S1.jfloat - S2.jfloat)));
+ }
+
/*!
Compute the distance \f$ |S1 - S2| = (i_1-i_2)^2+(j_1-j_2)^2 \f$
@@ -237,23 +229,25 @@ public:
\return the distance between the two sites.
*/
- static double sqrDistance (const vpMeSite S1, const vpMeSite S2) {
- return(vpMath::sqr(S1.ifloat-S2.ifloat)+vpMath::sqr(S1.jfloat-S2.jfloat));}
-
- static void display(const vpImage<unsigned char>& I, const double &i, const double &j,
+ static double sqrDistance(const vpMeSite &S1, const vpMeSite &S2)
+ {
+ return (vpMath::sqr(S1.ifloat - S2.ifloat) + vpMath::sqr(S1.jfloat - S2.jfloat));
+ }
+
+ static void display(const vpImage<unsigned char> &I, const double &i, const double &j,
const vpMeSiteState &state = NO_SUPPRESSION);
- static void display(const vpImage<vpRGBa>& I, const double &i, const double &j,
+ static void display(const vpImage<vpRGBa> &I, const double &i, const double &j,
const vpMeSiteState &state = NO_SUPPRESSION);
-//Deprecated
+// Deprecated
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
public:
//! Flag to indicate whether point is rejected or not
//! 1 = contrast, 2 = threshold, 3 = M-estimator, 0 = nosupp
int suppress;
-
- vp_deprecated void getSign(const vpImage<unsigned char> &I, const int range) ;
+
+ vp_deprecated void getSign(const vpImage<unsigned char> &I, const int range);
#endif
-} ;
+};
#endif
diff --git a/modules/tracker/me/include/visp3/me/vpMeTracker.h b/modules/tracker/me/include/visp3/me/vpMeTracker.h
index bd48f69..2a58655 100644
--- a/modules/tracker/me/include/visp3/me/vpMeTracker.h
+++ b/modules/tracker/me/include/visp3/me/vpMeTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,13 +46,13 @@
#define vpMeTracker_HH
#include <visp3/core/vpColVector.h>
-#include <visp3/me/vpMeSite.h>
-#include <visp3/me/vpMe.h>
#include <visp3/core/vpTracker.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeSite.h>
-#include <math.h>
#include <iostream>
#include <list>
+#include <math.h>
/*!
\class vpMeTracker
@@ -70,107 +71,101 @@ protected:
#endif
//! Tracking dependent variables/functions
//! List of tracked moving edges points.
- std::list<vpMeSite> list ;
+ std::list<vpMeSite> list;
//! Moving edges initialisation parameters
- vpMe *me ;
+ vpMe *me;
unsigned int init_range;
int nGoodElement;
-
+
protected:
- vpMeSite::vpMeSiteDisplayType selectDisplay ;
+ vpMeSite::vpMeSiteDisplayType selectDisplay;
public:
// Constructor/Destructor
- vpMeTracker() ;
- vpMeTracker(const vpMeTracker& meTracker) ;
- virtual ~vpMeTracker() ;
-
- void init() ;
- void initTracking(const vpImage<unsigned char>& I);
-
+ vpMeTracker();
+ vpMeTracker(const vpMeTracker &meTracker);
+ virtual ~vpMeTracker();
+
+ void init();
+ void initTracking(const vpImage<unsigned char> &I);
+
//! Track sampled pixels.
- void track(const vpImage<unsigned char>& I);
-
- unsigned int numberOfSignal() ;
- unsigned int totalNumberOfSignal() ;
-
- virtual void display(const vpImage<unsigned char> &I, vpColor col)=0;
- virtual void display(const vpImage<unsigned char>& I);
- void display(const vpImage<unsigned char>& I, vpColVector &w, unsigned int &index_w);
-
- void setDisplay(vpMeSite::vpMeSiteDisplayType select) {
- selectDisplay = select ;
- }
-
- vpMeTracker& operator =(vpMeTracker& f);
-
- int outOfImage( int i , int j , int half , int rows , int cols) ;
- int outOfImage( vpImagePoint iP , int half , int rows , int cols) ;
-
+ void track(const vpImage<unsigned char> &I);
+
+ unsigned int numberOfSignal();
+ unsigned int totalNumberOfSignal();
+
+ virtual void display(const vpImage<unsigned char> &I, vpColor col) = 0;
+ virtual void display(const vpImage<unsigned char> &I);
+ void display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w);
+
+ void setDisplay(vpMeSite::vpMeSiteDisplayType select) { selectDisplay = select; }
+
+ vpMeTracker &operator=(vpMeTracker &f);
+
+ int outOfImage(int i, int j, int half, int row, int cols);
+ int outOfImage(const vpImagePoint &iP, int half, int rows, int cols);
+
void reset();
- //!Sample pixels at a given interval
- virtual void sample(const vpImage<unsigned char> &image)=0;
-
-
+ //! Sample pixels at a given interval
+ virtual void sample(const vpImage<unsigned char> &image) = 0;
+
/*!
Set the initial range.
-
+
\param r : initial range.
*/
void setInitRange(const unsigned int &r) { init_range = r; }
-
+
/*!
Return the initial range.
-
+
\return Value of init_range.
*/
inline unsigned int getInitRange() { return init_range; }
-
+
/*!
Set the moving edges initialisation parameters
-
+
\param p_me : Moving Edges.
*/
- void setMe(vpMe *p_me) { this->me = p_me ; }
-
+ void setMe(vpMe *p_me) { this->me = p_me; }
+
/*!
Return the moving edges initialisation parameters
-
+
\return Moving Edges.
*/
- inline vpMe* getMe(){ return me; }
-
+ inline vpMe *getMe() { return me; }
+
/*!
Set the list of moving edges
-
+
\param l : list of Moving Edges.
*/
void setMeList(const std::list<vpMeSite> &l) { list = l; }
-
+
/*!
Return the list of moving edges
-
+
\return List of Moving Edges.
*/
- inline std::list<vpMeSite>& getMeList() { return list; }
+ inline std::list<vpMeSite> &getMeList() { return list; }
inline std::list<vpMeSite> getMeList() const { return list; }
-
+
/*!
Return the number of points that has not been suppressed.
-
+
\return Number of good points.
*/
inline int getNbPoints() const { return nGoodElement; }
-
+
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
public:
int query_range;
- bool display_point;// if 1 (TRUE) displays the line that is being tracked
+ bool display_point; // if 1 (TRUE) displays the line that is being tracked
#endif
};
-
#endif
-
-
diff --git a/modules/tracker/me/include/visp3/me/vpNurbs.h b/modules/tracker/me/include/visp3/me/vpNurbs.h
index f8c7253..ee44beb 100644
--- a/modules/tracker/me/include/visp3/me/vpNurbs.h
+++ b/modules/tracker/me/include/visp3/me/vpNurbs.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -32,7 +33,7 @@
*
* Authors:
* Nicolas Melchior
- *
+ *
*****************************************************************************/
#ifndef vpNurbs_H
@@ -40,15 +41,16 @@
/*!
\file vpNurbs.h
- \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
+ \brief Class that provides tools to compute and manipulate a Non Uniform
+ Rational B-Spline curve.
*/
+#include <visp3/core/vpBSpline.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpList.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/me/vpMeSite.h>
-#include <visp3/core/vpBSpline.h>
-#include <visp3/core/vpList.h>
#include <list>
@@ -56,44 +58,55 @@
\class vpNurbs
\ingroup module_me
- \brief Class that provides tools to compute and manipulate a Non Uniform Rational B-Spline curve.
-
+ \brief Class that provides tools to compute and manipulate a Non Uniform
+ Rational B-Spline curve.
+
The different parameters are :
- - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i = 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1} i = 0, ...,m \f$.
- To define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} , ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real numbers and p is the degree of the B-Spline basis functions.
+ - The knot vector \f$ U = {u_0, ... , u_m} \f$ where the knots \f$ u_i, i =
+ 0, ...,m \f$ are real number such as \f$ u_i < u_{i+1} i = 0, ...,m \f$. To
+ define a curve, the knot vector is such as : \f$ U = {a , ... , a, u_{p+1} ,
+ ... , u_{m-p-1} , b , ... , b} \f$ where \f$ a \f$ and \f$ b \f$ are real
+ numbers and p is the degree of the B-Spline basis functions.
- The B-Spline basis functions \f$ N_{i,p} \f$ defined as :
\f[ N_{i,0}(u) = \left\{\begin{array}{cc}
1 & \mbox{if } u_i \leq u_{i+1} \\ 0 & else
\end{array}\right.\f]
- \f[ N_{i,p}(u) = \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
-
- where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis functions.
+ \f[ N_{i,p}(u) =
+ \frac{u-u_i}{u_{i+p}-u_i}N_{i,p-1}(u)+\frac{u_{i+p+1}-u}{u_{i+p+1}-u_{i+1}}N_{i+1,p-1}(u)\f]
+
+ where \f$ i = 0 , ... , m-1 \f$ and p is the degree of the B-Spline basis
+ functions.
- - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$ (i,j) \f$ of a point in an image.
-
- - The weight \f$ {w_i} \f$ associated to each control points.The wheights value is upper than 0.
+ - The control points \f$ {P_i} \f$ which are defined by the coordinates \f$
+ (i,j) \f$ of a point in an image.
- It is possible to compute the coordinates of a point corresponding to the knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula :
- \f[ C(u) = \frac{\sum_{i=0}^n (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)}\f]
+ - The weight \f$ {w_i} \f$ associated to each control points.The wheights
+ value is upper than 0.
- You can find much more information about the B-Splines and the implementation of all the methods in the Nurbs Book.
+ It is possible to compute the coordinates of a point corresponding to the
+ knots \f$ u \f$ (\f$ u \in [u_0,u_m]\f$) thanks to the formula : \f[ C(u) =
+ \frac{\sum_{i=0}^n (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)}\f]
+
+ You can find much more information about the B-Splines and the
+ implementation of all the methods in the Nurbs Book.
*/
-class VISP_EXPORT vpNurbs : public vpBSpline
+class VISP_EXPORT vpNurbs : public vpBSpline
{
protected:
- std::vector<double> weights; //Vector which contains the weights associated to each control Points
-
+ std::vector<double> weights; // Vector which contains the weights associated
+ // to each control Points
protected:
- static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ static vpMatrix computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights);
vpMatrix computeCurveDers(double u, unsigned int der);
public:
-
vpNurbs();
vpNurbs(const vpNurbs &nurbs);
virtual ~vpNurbs();
@@ -101,12 +114,14 @@ public:
/*!
Gets all the weights relative to the control points.
- \param list [out] : A std::list containing weights relative to the control points.
+ \param list [out] : A std::list containing weights relative to the
+ control points.
*/
- inline void get_weights(std::list<double>& list) const {
+ inline void get_weights(std::list<double> &list) const
+ {
list.clear();
for (unsigned int i = 0; i < weights.size(); i++)
- list.push_back(*(&(weights[0])+i));
+ list.push_back(*(&(weights[0]) + i));
}
/*!
@@ -114,38 +129,51 @@ public:
\param list : A std::list containing the value of the knots.
*/
- inline void set_weights(const std::list<double> &list) {
+ inline void set_weights(const std::list<double> &list)
+ {
weights.clear();
- for(std::list<double>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<double>::const_iterator it = list.begin(); it != list.end(); ++it) {
weights.push_back(*it);
}
}
- static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ static vpImagePoint computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
vpImagePoint computeCurvePoint(double u);
- static vpImagePoint* computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- vpImagePoint* computeCurveDersPoint(double u, unsigned int der);
+ static vpImagePoint *computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights);
+ vpImagePoint *computeCurveDersPoint(double u, unsigned int der);
- static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ static void curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights);
void curveKnotIns(double u, unsigned int s = 0, unsigned int r = 1);
- static void refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void refineKnotVectCurve(double* x, unsigned int r);
+ static void refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ void refineKnotVectCurve(double *x, unsigned int r);
- static unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
+ static unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s,
+ unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
unsigned int removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL);
- static void globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void globalCurveInterp(vpList<vpMeSite>& l_crossingPoints);
- void globalCurveInterp(const std::list<vpImagePoint>& l_crossingPoints);
- void globalCurveInterp(const std::list<vpMeSite>& l_crossingPoints);
+ static void globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights);
+ void globalCurveInterp(vpList<vpMeSite> &l_crossingPoints);
+ void globalCurveInterp(const std::list<vpImagePoint> &l_crossingPoints);
+ void globalCurveInterp(const std::list<vpMeSite> &l_crossingPoints);
void globalCurveInterp();
- static void globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights);
- void globalCurveApprox(vpList<vpMeSite>& l_crossingPoints, unsigned int n);
- void globalCurveApprox(const std::list<vpImagePoint>& l_crossingPoints, unsigned int n);
- void globalCurveApprox(const std::list<vpMeSite>& l_crossingPoints, unsigned int n);
+ static void globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights);
+ void globalCurveApprox(vpList<vpMeSite> &l_crossingPoints, unsigned int n);
+ void globalCurveApprox(const std::list<vpImagePoint> &l_crossingPoints, unsigned int n);
+ void globalCurveApprox(const std::list<vpMeSite> &l_crossingPoints, unsigned int n);
void globalCurveApprox(unsigned int n);
};
diff --git a/modules/tracker/me/src/moving-edges/vpMe.cpp b/modules/tracker/me/src/moving-edges/vpMe.cpp
index d3b3abb..0c2ed74 100644
--- a/modules/tracker/me/src/moving-edges/vpMe.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMe.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,319 +38,296 @@
*****************************************************************************/
/*!
- \file vpMe.cpp
- \brief Moving edges
+ \file vpMe.cpp
+ \brief Moving edges
*/
-
-
-#include <visp3/me/vpMe.h>
+#include <stdlib.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpMath.h>
-#include <stdlib.h>
+#include <visp3/me/vpMe.h>
#ifndef DOXYGEN_SHOULD_SKIP_THIS
+struct point {
+ double x;
+ double y;
+};
-struct point
-{
- double x ;
- double y ;
-} ;
-
-struct droite
-{
- double a ;
- double b ;
- double c ;
-} ;
-
-
-
-
-
-
+struct droite {
+ double a;
+ double b;
+ double c;
+};
-template <class Type>
-inline void
-permute(Type & a, Type & b)
+template <class Type> inline void permute(Type &a, Type &b)
{
Type t = a;
a = b;
b = t;
}
-static droite
-droite_cartesienne(point P, point Q)
+static droite droite_cartesienne(point P, point Q)
{
droite PQ;
- PQ.a = P.y-Q.y;
- PQ.b = Q.x-P.x;
- PQ.c = Q.y*P.x - Q.x*P.y;
+ PQ.a = P.y - Q.y;
+ PQ.b = Q.x - P.x;
+ PQ.c = Q.y * P.x - Q.x * P.y;
- return(PQ);
+ return (PQ);
}
-
-
-static point
-point_intersection(droite D1, droite D2)
+static point point_intersection(droite D1, droite D2)
{
- point I;
- double det; // determinant des 2 vect.normaux
+ point I;
+ double det; // determinant des 2 vect.normaux
- det = (D1.a*D2.b - D2.a*D1.b); // interdit D1,D2 paralleles
- I.x = (D2.c*D1.b - D1.c*D2.b)/det;
- I.y = (D1.c*D2.a - D2.c*D1.a)/det;
+ det = (D1.a * D2.b - D2.a * D1.b); // interdit D1,D2 paralleles
+ I.x = (D2.c * D1.b - D1.c * D2.b) / det;
+ I.y = (D1.c * D2.a - D2.c * D1.a) / det;
- return(I);
+ 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 !
- if(vpMath::equal(P.x,Xmax))
- P.x=Xmax;
-
- if(vpMath::equal(P.y,Ymin))
- P.y=Ymin;
- if(vpMath::equal(P.y,Ymax))
- P.y=Ymax;
+ if (vpMath::equal(P.x, Xmin))
+ P.x = Xmin; // a peu pres => exactement !
+ if (vpMath::equal(P.x, Xmax))
+ P.x = Xmax;
+
+ if (vpMath::equal(P.y, Ymin))
+ P.y = Ymin;
+ if (vpMath::equal(P.y, Ymax))
+ P.y = Ymax;
}
-
-static void
-permute(point &A, point &B)
+static void permute(point &A, point &B)
{
- point C ;
+ point C;
- if (A.x>B.x) // fonction sans doute a tester...
+ if (A.x > B.x) // fonction sans doute a tester...
{
- C = A ;
- A = B ;
- B = C ;
+ C = A;
+ A = B;
+ B = C;
}
}
-
// vrai si partie visible
-static bool
-clipping (point A, point B,
- double Xmin, double Ymin, double Xmax, double Ymax,
- point & Ac , point & Bc )// resultat: A,B clippes
+static bool clipping(point A, point B, double Xmin, double Ymin, double Xmax, double Ymax, point &Ac,
+ point &Bc) // resultat: A,B clippes
{
- droite AB, D[4];
- D[0].a = 1; D[0].b = 0; D[0].c = -Xmin;
- D[1].a = 1; D[1].b = 0; D[1].c = -Xmax;
- D[2].a = 0; D[2].b = 1; D[2].c = -Ymin;
- D[3].a = 0; D[3].b = 1; D[3].c = -Ymax;
-
- point P[2];
- P[0]=A; P[1]=B;
- int code_P[2], // codes de P[n]
- i, bit_i, // i -> (0000100...)
- n;
-
- AB = droite_cartesienne(A,B);
-
- for ( ; ; ) // 2 sorties directes internes
- {
- // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
- // ================================================
- for(n=0; n<2; n++)
- {
- code_P[n] = 0000;
-
- if( P[n].x < Xmin )
- code_P[n] |= 1; // positionne bit0
- if( P[n].x > Xmax )
- code_P[n] |= 2; // .. bit1
- if( P[n].y < Ymin )
- code_P[n] |= 4; // .. bit2
- if( P[n].y > Ymax )
- code_P[n] |= 8; // .. bit3
- }
-
-
- // 2 CAS OU L'ON PEUT CONCLURE => sortie
- // =====================================
- if((code_P[0] | code_P[1])==0000) // Aucun bit a 1
- /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
- { if(P[0].x < P[1].x) // Rend le couple de points
- { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
- else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
- */
+ droite AB, D[4];
+ D[0].a = 1;
+ D[0].b = 0;
+ D[0].c = -Xmin;
+ D[1].a = 1;
+ D[1].b = 0;
+ D[1].c = -Xmax;
+ D[2].a = 0;
+ D[2].b = 1;
+ D[2].c = -Ymin;
+ D[3].a = 0;
+ D[3].b = 1;
+ D[3].c = -Ymax;
+
+ point P[2];
+ P[0] = A;
+ P[1] = B;
+ int code_P[2], // codes de P[n]
+ i, bit_i, // i -> (0000100...)
+ n;
+
+ AB = droite_cartesienne(A, B);
+
+ for (;;) // 2 sorties directes internes
+ {
+ // CALCULE CODE DE VISIBILITE (Sutherland & Sproul)
+ // ================================================
+ for (n = 0; n < 2; n++) {
+ code_P[n] = 0000;
+
+ if (P[n].x < Xmin)
+ code_P[n] |= 1; // positionne bit0
+ if (P[n].x > Xmax)
+ code_P[n] |= 2; // .. bit1
+ if (P[n].y < Ymin)
+ code_P[n] |= 4; // .. bit2
+ if (P[n].y > Ymax)
+ code_P[n] |= 8; // .. bit3
+ }
+
+ // 2 CAS OU L'ON PEUT CONCLURE => sortie
+ // =====================================
+ if ((code_P[0] | code_P[1]) == 0000) // Aucun bit a 1
+ /* NE TRIE PLUS LE RESULTAT ! S_relative() en tient compte
+{ if(P[0].x < P[1].x) // Rend le couple de points
+ { Ac=P[0]; Bc=P[1]; } // clippes (ordonnes selon
+else { Ac=P[1]; Bc=P[0]; } // leur abscisse x)
+ */
{
- Ac=P[0]; Bc=P[1];
- if(vpMath::equal(Ac.x,Bc.x) && vpMath::equal(Ac.y,Bc.y))
- return(false); // AB = 1 point = invisible
+ Ac = P[0];
+ Bc = P[1];
+ if (vpMath::equal(Ac.x, Bc.x) && vpMath::equal(Ac.y, Bc.y))
+ return (false); // AB = 1 point = invisible
else
- return(true); // Partie de AB clippee visible!
+ return (true); // Partie de AB clippee visible!
}
- if((code_P[0] & code_P[1])!=0000) // au moins 1 bit commun
- {
- return(false); // AB completement invisible!
- }
-
-
- // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
- // - clippe le point P[n] qui sort de la fenetre (coupe Droite i)
- // - reboucle avec le nouveau couple de points
- // ================================================================
- if(code_P[0] != 0000)
- {
- n=0; // c'est P[0] qu'on clippera
- for(i=0,bit_i=1; !(code_P[0] & bit_i); i++,bit_i<<=1){;}
- }
- else
- {
- n=1; // c'est P[1] qu'on clippera
- for(i=0,bit_i=1; !(code_P[1] & bit_i); i++,bit_i<<=1){;}
- }
-
- P[n] = point_intersection(AB,D[i]); // clippe le point concerne
-
+ if ((code_P[0] & code_P[1]) != 0000) // au moins 1 bit commun
+ {
+ return (false); // AB completement invisible!
+ }
- // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
- // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
- // =======================================================
- recale(P[n], Xmin,Ymin,Xmax,Ymax);
+ // CAS GENERAL (on sait que code_P[0 ou 1] a au moins un bit a 1
+ // - clippe le point P[n] qui sort de la fenetre (coupe Droite i)
+ // - reboucle avec le nouveau couple de points
+ // ================================================================
+ if (code_P[0] != 0000) {
+ n = 0; // c'est P[0] qu'on clippera
+ for (i = 0, bit_i = 1; !(code_P[0] & bit_i); i++, bit_i <<= 1) {
+ ;
+ }
+ } else {
+ n = 1; // c'est P[1] qu'on clippera
+ for (i = 0, bit_i = 1; !(code_P[1] & bit_i); i++, bit_i <<= 1) {
+ ;
+ }
+ }
- }
+ P[n] = point_intersection(AB, D[i]); // clippe le point concerne
+ // RECALE EXACTEMENT LE POINT (calcul flottant => arrondi)
+ // AFIN QUE LE CALCUL DES CODES NE BOUCLE PAS INDEFINIMENT
+ // =======================================================
+ recale(P[n], Xmin, Ymin, Xmax, Ymax);
+ }
}
-
// calcule la surface relative des 2 portions definies
// par le segment PQ sur le carre Xmin,Ymin,Xmax,Ymax
// Rem : P,Q tries sur x, et donc seulement 6 cas
-static double
-S_relative(point P, point Q,
- double Xmin, double Ymin, double Xmax, double Ymax)
+static double S_relative(point P, point Q, double Xmin, double Ymin, double Xmax, double Ymax)
{
- if(Q.x < P.x) // tri le couple de points
- permute(P,Q); // selon leur abscisse x
-
-
+ if (Q.x < P.x) // tri le couple de points
+ permute(P, Q); // selon leur abscisse x
- recale(P, Xmin,Ymin,Xmax,Ymax); // permet des calculs de S_relative
- recale(Q, Xmin,Ymin,Xmax,Ymax); // moins approximatifs.
+ recale(P, Xmin, Ymin, Xmax, Ymax); // permet des calculs de S_relative
+ recale(Q, Xmin, Ymin, Xmax, Ymax); // moins approximatifs.
- //if(P.x==Xmin && Q.x==Xmax)
- if((std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon()))
- return( fabs(Ymax+Ymin-P.y-Q.y) );
+ // if(P.x==Xmin && Q.x==Xmax)
+ if ((std::fabs(P.x - Xmin) <=
+ vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(Q.x - Xmax) <=
+ vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon()))
+ return (fabs(Ymax + Ymin - P.y - Q.y));
- //if( (P.y==Ymin && Q.y==Ymax) ||
+ // if( (P.y==Ymin && Q.y==Ymax) ||
// (Q.y==Ymin && P.y==Ymax))
- if( ( (std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) )
- ||
- ( (std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon())
- &&
- (std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()) ) )
- return( fabs(Xmax+Xmin-P.x-Q.x) );
-
- //if( P.x==Xmin && Q.y==Ymax )
- if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
- &&
- std::fabs(Q.y-Ymax) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon() )
- return( 1-(Ymax-P.y)*(Q.x-Xmin) );
- //if( P.x==Xmin && Q.y==Ymin )
- if( std::fabs(P.x-Xmin) <= vpMath::maximum(std::fabs(P.x),std::fabs(Xmin))*std::numeric_limits<double>::epsilon()
- &&
- std::fabs(Q.y-Ymin) <= vpMath::maximum(std::fabs(Q.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon() )
- return( 1-(P.y-Ymin)*(Q.x-Xmin) );
- //if( P.y==Ymin && Q.x==Xmax )
- if( std::fabs(P.y-Ymin) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymin))*std::numeric_limits<double>::epsilon()
- && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon() )
- return( 1-(Xmax-P.x)*(Q.y-Ymin) );
- //if( P.y==Ymax && Q.x==Xmax )
- if( std::fabs(P.y-Ymax) <= vpMath::maximum(std::fabs(P.y),std::fabs(Ymax))*std::numeric_limits<double>::epsilon()
- && std::fabs(Q.x-Xmax) <= vpMath::maximum(std::fabs(Q.x),std::fabs(Xmax))*std::numeric_limits<double>::epsilon())
- return( 1-(Xmax-P.x)*(Ymax-Q.y) );
-
-
- printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n",
- P.x,P.y,Q.x,Q.y,Xmin,Ymin,Xmax,Ymax);
- exit(-1); // DEBUG Stoppe net l'execution
+ if (((std::fabs(P.y - Ymin) <=
+ vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(Q.y - Ymax) <=
+ vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())) ||
+ ((std::fabs(Q.y - Ymin) <=
+ vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P.y - Ymax) <=
+ vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())))
+ return (fabs(Xmax + Xmin - P.x - Q.x));
+
+ // if( P.x==Xmin && Q.y==Ymax )
+ if (std::fabs(P.x - Xmin) <=
+ vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(Q.y - Ymax) <=
+ vpMath::maximum(std::fabs(Q.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon())
+ return (1 - (Ymax - P.y) * (Q.x - Xmin));
+ // if( P.x==Xmin && Q.y==Ymin )
+ if (std::fabs(P.x - Xmin) <=
+ vpMath::maximum(std::fabs(P.x), std::fabs(Xmin)) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(Q.y - Ymin) <=
+ vpMath::maximum(std::fabs(Q.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon())
+ return (1 - (P.y - Ymin) * (Q.x - Xmin));
+ // if( P.y==Ymin && Q.x==Xmax )
+ if (std::fabs(P.y - Ymin) <=
+ vpMath::maximum(std::fabs(P.y), std::fabs(Ymin)) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(Q.x - Xmax) <=
+ vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
+ return (1 - (Xmax - P.x) * (Q.y - Ymin));
+ // if( P.y==Ymax && Q.x==Xmax )
+ if (std::fabs(P.y - Ymax) <=
+ vpMath::maximum(std::fabs(P.y), std::fabs(Ymax)) * std::numeric_limits<double>::epsilon() &&
+ std::fabs(Q.x - Xmax) <=
+ vpMath::maximum(std::fabs(Q.x), std::fabs(Xmax)) * std::numeric_limits<double>::epsilon())
+ return (1 - (Xmax - P.x) * (Ymax - Q.y));
+
+ printf("utils_ecm: ERREUR dans S_relative (%f,%f) (%f,%f) %f %f %f %f\n", P.x, P.y, Q.x, Q.y, Xmin, Ymin, Xmax, Ymax);
+ exit(-1); // DEBUG Stoppe net l'execution
}
-
-static void
-calcul_masques(vpColVector &angle, // definitions des angles theta
- unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
- vpMatrix *M) // resultat M[theta](n,n)
+static void calcul_masques(vpColVector &angle, // definitions des angles theta
+ unsigned int n, // taille masques (PAIRE ou IMPAIRE Ok)
+ vpMatrix *M) // resultat M[theta](n,n)
{
// Le coef |a| = |1/2n| n'est pas incorpore dans M(i,j) (=> que des int)
- unsigned int i_theta, // indice (boucle sur les masques)
- i,j; // indices de boucle sur M(i,j)
- double X,Y, // point correspondant/centre du masque
- moitie = ((double)n)/2.0; // moitie REELLE du masque
- point P1,Q1,P,Q; // clippe Droite(theta) P1,Q1 -> P,Q
- int sgn; // signe de M(i,j)
- double v; // ponderation de M(i,j)
-
- unsigned int nb_theta = angle.getRows() ;
-
- for(i_theta=0; i_theta<nb_theta; i_theta++)
- {
- 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<double>::epsilon() ) // => tan(theta) infinie !
- {
- P1.x=0; P1.y=-(int)n;
- Q1.x=0; Q1.y= n;
- }
- else
- {
- double tan_theta = sin_theta/cos_theta; // pente de la droite D(theta)
- P1.x=-(int)n; P1.y=tan_theta*(-(int)n);
- Q1.x=n; Q1.y=tan_theta*n;
- }
-
- // CALCULE MASQUE M(theta)
- // ======================
- M[i_theta].resize(n,n); // allocation (si necessaire)
-
- for(i=0,Y=-moitie+0.5 ; i<n ; i++,Y++)
- {
- for(j=0,X=-moitie+0.5 ; j<n ; j++,X++)
- {
- // produit vectoriel dir_droite*(X,Y)
- sgn = vpMath::sign(cos_theta*Y - sin_theta*X);
-
- // Resultat = P,Q
- if( clipping(P1,Q1, X-0.5,Y-0.5,X+0.5,Y+0.5, P,Q) )
- {
- // v dans [0,1]
- v=S_relative(P,Q, X-0.5,Y-0.5,X+0.5,Y+0.5);
- }
- else
- v=1; // PQ ne coupe pas le pixel(i,j)
-
- M[i_theta][i][j] = vpMath::round(100*sgn*v);
-
- // 2 chiffres significatifs
- // M(i,j) sans incorporer le coef a
- }
- }
- }
+ unsigned int i_theta, // indice (boucle sur les masques)
+ i, j; // indices de boucle sur M(i,j)
+ double X, Y, // point correspondant/centre du masque
+ moitie = ((double)n) / 2.0; // moitie REELLE du masque
+ point P1, Q1, P, Q; // clippe Droite(theta) P1,Q1 -> P,Q
+ int sgn; // signe de M(i,j)
+ double v; // ponderation de M(i,j)
+
+ unsigned int nb_theta = angle.getRows();
+
+ for (i_theta = 0; i_theta < nb_theta; i_theta++) {
+ 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<double>::epsilon()) // => tan(theta) infinie !
+ {
+ P1.x = 0;
+ P1.y = -(int)n;
+ Q1.x = 0;
+ Q1.y = n;
+ } else {
+ double tan_theta = sin_theta / cos_theta; // pente de la droite D(theta)
+ P1.x = -(int)n;
+ P1.y = tan_theta * (-(int)n);
+ Q1.x = n;
+ Q1.y = tan_theta * n;
+ }
+
+ // CALCULE MASQUE M(theta)
+ // ======================
+ M[i_theta].resize(n, n); // allocation (si necessaire)
+
+ for (i = 0, Y = -moitie + 0.5; i < n; i++, Y++) {
+ for (j = 0, X = -moitie + 0.5; j < n; j++, X++) {
+ // produit vectoriel dir_droite*(X,Y)
+ sgn = vpMath::sign(cos_theta * Y - sin_theta * X);
+
+ // Resultat = P,Q
+ if (clipping(P1, Q1, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5, P, Q)) {
+ // v dans [0,1]
+ v = S_relative(P, Q, X - 0.5, Y - 0.5, X + 0.5, Y + 0.5);
+ } else
+ v = 1; // PQ ne coupe pas le pixel(i,j)
+
+ M[i_theta][i][j] = vpMath::round(100 * sgn * v);
+ // 2 chiffres significatifs
+ // M(i,j) sans incorporer le coef a
+ }
+ }
+ }
}
#endif
@@ -359,101 +337,92 @@ calcul_masques(vpColVector &angle, // definitions des angles theta
matrices to create.
*/
-void
-vpMe::initMask()
+void vpMe::initMask()
{
if (mask != NULL)
- delete [] mask;
-
- mask = new vpMatrix[n_mask] ;
+ delete[] mask;
- vpColVector angle(n_mask) ;
+ mask = new vpMatrix[n_mask];
- unsigned int angle_pas ;
- angle_pas = 180 / n_mask ;
+ vpColVector angle(n_mask);
- unsigned int k =0 ;
- for (unsigned int i = 0 ; /* i < 180, */ k < n_mask ; i += angle_pas)
- angle[k++] = i ;
+ unsigned int angle_pas;
+ angle_pas = 180 / n_mask;
- calcul_masques(angle, mask_size, mask ) ;
+ unsigned int k = 0;
+ for (unsigned int i = 0; /* i < 180, */ k < n_mask; i += angle_pas)
+ angle[k++] = i;
+ calcul_masques(angle, mask_size, mask);
}
-
-
-void
-vpMe::print( )
+void vpMe::print()
{
- std::cout<< std::endl ;
- std::cout<<"Moving edges settings " <<std::endl ;
- std::cout<< std::endl ;
- std::cout<<" Size of the convolution masks...."<<mask_size<<"x"<<mask_size<<" pixels"<<std::endl ;
- std::cout<<" Number of masks.................."<<n_mask<<" "<<std::endl ;
- std::cout<<" Query range +/- J................"<<range<<" pixels "<<std::endl ;
- std::cout<<" Likelihood test ratio............"<<threshold<<std::endl ;
- std::cout<<" Contrast tolerance +/-..........."<< mu1 * 100<<"% and "<<mu2 * 100<<"% "<<std::endl ;
- std::cout<<" Sample step......................"<<sample_step<<" pixels"<<std::endl ;
- std::cout<<" Strip............................"<<strip<<" pixels "<<std::endl ;
- std::cout<<" Min_Samplestep..................."<<min_samplestep<<" pixels "<<std::endl ;
+ std::cout << std::endl;
+ std::cout << "Moving edges settings " << std::endl;
+ std::cout << std::endl;
+ std::cout << " Size of the convolution masks...." << mask_size << "x" << mask_size << " pixels" << std::endl;
+ std::cout << " Number of masks.................." << n_mask << " " << std::endl;
+ std::cout << " Query range +/- J................" << range << " pixels " << std::endl;
+ std::cout << " Likelihood test ratio............" << threshold << std::endl;
+ std::cout << " Contrast tolerance +/-..........." << mu1 * 100 << "% and " << mu2 * 100 << "% " << std::endl;
+ std::cout << " Sample step......................" << sample_step << " pixels" << std::endl;
+ std::cout << " Strip............................" << strip << " pixels " << std::endl;
+ std::cout << " Min_Samplestep..................." << min_samplestep << " pixels " << std::endl;
}
vpMe::vpMe()
- : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
- range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
- n_mask(180), strip(2), mask(NULL)
+ : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0), range(4), sample_step(10),
+ ntotal_sample(0), points_to_track(500), mask_size(5), n_mask(180), strip(2), mask(NULL)
{
- //ntotal_sample = 0; // not sure that it is used
- //points_to_track = 500; // not sure that it is used
- anglestep = (180 / n_mask) ;
-
- initMask() ;
+ // ntotal_sample = 0; // not sure that it is used
+ // points_to_track = 500; // not sure that it is used
+ anglestep = (180 / n_mask);
+
+ initMask();
}
vpMe::vpMe(const vpMe &me)
- : threshold(1500), mu1(0.5), mu2(0.5), min_samplestep(4), anglestep(1), mask_sign(0),
- range(4), sample_step(10), ntotal_sample(0), points_to_track(500), mask_size(5),
- n_mask(180), strip(2), mask(NULL)
+ : 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;
}
//! Copy operator.
-const
-vpMe& vpMe::operator=(const vpMe &me)
+vpMe &vpMe::operator=(const vpMe &me)
{
if (mask != NULL) {
- delete [] mask;
- mask = NULL ;
+ delete[] mask;
+ mask = NULL;
}
- threshold = me.threshold ;
- mu1 = me.mu1 ;
- mu2 = me.mu2 ;
- min_samplestep = me.min_samplestep ;
- anglestep = me.anglestep ;
- mask_size = me.mask_size ;
- n_mask = me.n_mask ;
- mask_sign = me.mask_sign ;
- range = me.range ;
- sample_step = me.sample_step ;
+ threshold = me.threshold;
+ mu1 = me.mu1;
+ mu2 = me.mu2;
+ min_samplestep = me.min_samplestep;
+ anglestep = me.anglestep;
+ mask_size = me.mask_size;
+ n_mask = me.n_mask;
+ mask_sign = me.mask_sign;
+ range = me.range;
+ sample_step = me.sample_step;
ntotal_sample = me.ntotal_sample;
points_to_track = me.points_to_track;
- strip = me.strip ;
-
- initMask() ;
+ strip = me.strip;
+
+ initMask();
return *this;
}
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
//! Move operator.
-const
-vpMe& vpMe::operator=(const vpMe &&me)
+vpMe &vpMe::operator=(const vpMe &&me)
{
if (mask != NULL) {
- delete [] mask;
- mask = NULL ;
+ delete[] mask;
+ mask = NULL;
}
threshold = std::move(me.threshold);
mu1 = std::move(me.mu1);
@@ -469,36 +438,28 @@ vpMe& vpMe::operator=(const vpMe &&me)
points_to_track = std::move(me.points_to_track);
strip = std::move(me.strip);
- initMask() ;
+ initMask();
return *this;
}
#endif
vpMe::~vpMe()
{
- if (mask != NULL)
- {
- delete [] mask ;
+ if (mask != NULL) {
+ delete[] mask;
mask = NULL;
}
}
-
-void
-vpMe::setMaskNumber(const unsigned int &n)
+void vpMe::setMaskNumber(const unsigned int &n)
{
- n_mask = n ;
- anglestep = 180 / n_mask ;
- initMask() ;
+ n_mask = n;
+ anglestep = 180 / n_mask;
+ initMask();
}
-void
-vpMe::setMaskSize(const unsigned int &s)
+void vpMe::setMaskSize(const unsigned int &s)
{
- mask_size = s ;
- initMask() ;
+ mask_size = s;
+ initMask();
}
-
-
-
-
diff --git a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp
index b1d6eaa..9c6571a 100644
--- a/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMeEllipse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,60 +36,59 @@
*
*****************************************************************************/
-
-
#include <visp3/me/vpMeEllipse.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/me/vpMe.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <vector>
-void computeTheta(double &theta, vpColVector &K, vpImagePoint iP);
+void computeTheta(double &theta, vpColVector &K, const vpImagePoint &iP);
/*!
- Computes the \f$ \theta \f$ angle which represents the angle between the tangente to the curve and the i axis.
+ Computes the \f$ \theta \f$ angle which represents the angle between the
+ tangente to the curve and the i axis.
\param theta : The computed value.
\param K : The parameters of the ellipse.
\param iP : the point belonging th the ellipse where the angle is computed.
*/
-void
-computeTheta(double &theta, vpColVector &K, vpImagePoint iP)
+void computeTheta(double &theta, vpColVector &K, const vpImagePoint &iP)
{
double i = iP.get_i();
double j = iP.get_j();
- double A = 2*i+2*K[1]*j + 2*K[2] ;
- double B = 2*K[0]*j + 2*K[1]*i + 2*K[3];
+ double A = 2 * i + 2 * K[1] * j + 2 * K[2];
+ double B = 2 * K[0] * j + 2 * K[1] * i + 2 * K[3];
- theta = atan2(A,B) ; //Angle between the tangente and the i axis.
+ theta = atan2(A, B); // Angle between the tangente and the i axis.
- while (theta > M_PI) { theta -= M_PI ; }
- while (theta < 0) { theta += M_PI ; }
+ while (theta > M_PI) {
+ theta -= M_PI;
+ }
+ while (theta < 0) {
+ theta += M_PI;
+ }
}
-
/*!
Basic constructor that calls the constructor of the class vpMeTracker.
*/
vpMeEllipse::vpMeEllipse()
- : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2*M_PI),
- ce(0.), se(0.), angle(), m00(0.), mu11(0.), mu20(0.), mu02(0.),
- m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
- thresholdWeight(0.2), expecteddensity(0.)
+ : K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2 * M_PI), ce(0.), se(0.), angle(), m00(0.),
+ mu11(0.), mu20(0.), mu02(0.), m10(0.), m01(0.), m11(0.), m02(0.), m20(0.), thresholdWeight(0.2), expecteddensity(0.)
{
// redimensionnement du vecteur de parametre
// i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
- K.resize(5) ;
+ K.resize(5);
- //j1 = j2 = i1 = i2 = 0 ;
+ // j1 = j2 = i1 = i2 = 0;
iP1.set_i(0);
iP1.set_j(0);
iP2.set_i(0);
@@ -99,25 +99,18 @@ vpMeEllipse::vpMeEllipse()
Copy constructor.
*/
vpMeEllipse::vpMeEllipse(const vpMeEllipse &meellipse)
- : vpMeTracker(meellipse), K(), iPc(), a(0.), b(0.), e(0.), iP1(), iP2(), alpha1(0), alpha2(2*M_PI),
- ce(0.), se(0.), angle(), m00(0.), mu11(0.), mu20(0.), mu02(0.),
- m10(0.), m01(0.), m11(0.), m02(0.), m20(0.),
- thresholdWeight(0.2), expecteddensity(0.)
+ : 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.)
{
- K = meellipse.K;
- iPc = meellipse.iPc;
a = meellipse.a;
b = meellipse.b;
e = meellipse.e;
- iP1 = meellipse.iP1;
- iP2 = meellipse.iP2;
alpha1 = meellipse.alpha1;
alpha2 = meellipse.alpha2;
ce = meellipse.ce;
se = meellipse.se;
-
- angle = meellipse.angle;
m00 = meellipse.m00;
mu11 = meellipse.mu11;
@@ -127,7 +120,7 @@ vpMeEllipse::vpMeEllipse(const vpMeEllipse &meellipse)
m01 = meellipse.m01;
m11 = meellipse.m11;
m02 = meellipse.m02;
- m20 = meellipse.m20;
+ m20 = meellipse.m20;
thresholdWeight = meellipse.thresholdWeight;
expecteddensity = meellipse.expecteddensity;
@@ -142,7 +135,6 @@ vpMeEllipse::~vpMeEllipse()
angle.clear();
}
-
/*!
Construct a list of vpMeSite moving edges at a particular sampling
step between the two extremities. The two extremities are defined by
@@ -150,35 +142,31 @@ vpMeEllipse::~vpMeEllipse()
\param I : Image in which the ellipse appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeEllipse::sample(const vpImage<unsigned char> & I)
+void vpMeEllipse::sample(const vpImage<unsigned char> &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() ;
+ int height = (int)I.getHeight();
+ int width = (int)I.getWidth();
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- std::cout << "In vpMeEllipse::sample: " ;
- std::cout << "function called with sample step = 0" ;
- //return fatalError ;
+ // if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
+ std::cout << "In vpMeEllipse::sample: ";
+ std::cout << "function called with sample step = 0";
+ // return fatalError;
}
- double j, i;//, j11, i11;
vpImagePoint iP11;
- j = i = 0.0 ;
- double incr = vpMath::rad(me->getSampleStep()) ; // angle increment en degree
- vpColor col = vpColor::red ;
- getParameters() ;
+ double incr = vpMath::rad(me->getSampleStep()); // angle increment en degree
+ vpColor col = vpColor::red;
+ getParameters();
// Delete old list
list.clear();
@@ -186,311 +174,285 @@ vpMeEllipse::sample(const vpImage<unsigned char> & I)
angle.clear();
// sample positions
- double k = alpha1 ;
- while (k<alpha2)
- {
-// j = a *cos(k) ; // equation of an ellipse
-// i = b *sin(k) ; // equation of an ellipse
-
- j = a *sin(k) ; // equation of an ellipse
- i = b *cos(k) ; // equation of an ellipse
+ double k = alpha1;
+ while (k < alpha2) {
+ double j = a * sin(k); // equation of an ellipse
+ double i = b * cos(k); // equation of an ellipse
- // (i,j) are the coordinates on the origin centered ellipse ;
+ // (i,j) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
-// iP11.set_j( iPc.get_j() + ce *j - se *i );
-// iP11.set_i( iPc.get_i() -( se *j + ce *i) );
+ iP11.set_j(iPc.get_j() + ce * j + se * i);
+ iP11.set_i(iPc.get_i() - se * j + ce * i);
- iP11.set_j( iPc.get_j() + ce *j + se *i );
- iP11.set_i( iPc.get_i() - se *j + ce *i );
+ vpDisplay::displayCross(I, iP11, 5, col);
- vpDisplay::displayCross(I, iP11, 5, col) ;
-
- double theta ;
- computeTheta(theta, K, iP11) ;
+ double theta;
+ computeTheta(theta, K, iP11);
// If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width))
- {
- vpMeSite pix ;
- pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta) ;
- pix.setDisplay(selectDisplay) ;
+ if (!outOfImage(vpMath::round(iP11.get_i()), vpMath::round(iP11.get_j()), 0, height, width)) {
+ vpMeSite pix;
+ pix.init((int)iP11.get_i(), (int)iP11.get_j(), theta);
+ pix.setDisplay(selectDisplay);
pix.setState(vpMeSite::NO_SUPPRESSION);
- if(vpDEBUG_ENABLE(3))
- {
- vpDisplay::displayCross(I,iP11, 5, vpColor::blue);
+ if (vpDEBUG_ENABLE(3)) {
+ vpDisplay::displayCross(I, iP11, 5, vpColor::blue);
}
list.push_back(pix);
angle.push_back(k);
}
- k += incr ;
-
+ k += incr;
}
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
-
/*!
-
+
Resample the ellipse if the number of sample is less than 90% of the
expected value.
-
- \note The expected value is computed thanks to the difference between the smallest and the biggest \f$ \alpha \f$ angles
- and the parameter which indicates the number of degrees between
- two points (vpMe::sample_step).
+
+ \note The expected value is computed thanks to the difference between the
+ smallest and the biggest \f$ \alpha \f$ angles and the parameter which
+ indicates the number of degrees between two points (vpMe::sample_step).
\param I : Image in which the ellipse appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeEllipse::reSample(const vpImage<unsigned char> &I)
+void vpMeEllipse::reSample(const vpImage<unsigned char> &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());
- if ((double)n<0.9*expecteddensity){
- sample(I) ;
+ unsigned int n = numberOfSignal();
+ expecteddensity = (alpha2 - alpha1) / vpMath::rad((double)me->getSampleStep());
+ if ((double)n < 0.5 * expecteddensity) {
+ sample(I);
+ vpMeTracker::track(I);
}
}
-
/*!
- Computes the coordinates of the ellipse center, length of the semiminor axis \f$ a \f$, the length of the semimajor axis \f$ b \f$ and,
- \f$ e \f$ which is the angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$.
-
- All those computations are made thanks to the parameters \f$ K = {K_0, ..., K_4} \f$.
+ Computes the coordinates of the ellipse center, length of the semiminor axis
+ \f$ a \f$, the length of the semimajor axis \f$ b \f$ and, \f$ e \f$ which
+ is the angle made by the major axis and the i axis of the image frame \f$
+ (i,j) \f$.
+
+ All those computations are made thanks to the parameters \f$ K = {K_0, ...,
+ K_4} \f$.
*/
-void
-vpMeEllipse::getParameters()
+void vpMeEllipse::getParameters()
{
- double k[6] ;
- for (unsigned int i=0 ; i < 5 ; i++)
- k[i+1] = K[i] ;
- k[0] = 1 ;
+ double k[6];
+ for (unsigned int i = 0; i < 5; i++)
+ k[i + 1] = K[i];
+ k[0] = 1;
- double d = k[2]*k[2] - k[0]*k[1];
+ double d = k[2] * k[2] - k[0] * k[1];
- iPc.set_i( (k[1] * k[3] - k[2] * k[4]) / d );
- iPc.set_j( (k[0] * k[4] - k[2] * k[3]) / d );
+ iPc.set_i((k[1] * k[3] - k[2] * k[4]) / d);
+ iPc.set_j((k[0] * k[4] - k[2] * k[3]) / d);
- double sq = sqrt(vpMath::sqr(k[1]-k[0]) + 4.0*vpMath::sqr(k[2])) ;
+ double sq = sqrt(vpMath::sqr(k[1] - k[0]) + 4.0 * vpMath::sqr(k[2]));
if (std::fabs(k[2]) <= std::numeric_limits<double>::epsilon()) {
e = 0;
- }
- else {
- e = (k[1] - k[0] + sq) / (2.0*k[2]);
- e = (-1/e) ;
+ } else {
+ e = (k[1] - k[0] + sq) / (2.0 * k[2]);
+ e = (-1 / e);
}
- e = atan(e) ;
+ e = atan(e);
- if(e < 0.0) e += M_PI ;
+ if (e < 0.0)
+ e += M_PI;
- ce = cos(e) ;
- se = sin(e) ;
+ ce = cos(e);
+ se = sin(e);
- double num = 2.0*(k[0]*iPc.get_i()*iPc.get_i() + 2.0*k[2]*iPc.get_j()*iPc.get_i() + k[1]*iPc.get_j()*iPc.get_j() - k[5]) ;
- double a2 = num / (k[0] + k[1] + sq ) ;
- double b2 = num / (k[0] + k[1] - sq ) ;
+ double num = 2.0 * (k[0] * iPc.get_i() * iPc.get_i() + 2.0 * k[2] * iPc.get_j() * iPc.get_i() +
+ k[1] * iPc.get_j() * iPc.get_j() - k[5]);
+ double a2 = num / (k[0] + k[1] + sq);
+ double b2 = num / (k[0] + k[1] - sq);
- a = sqrt( a2 ) ;
- b = sqrt( b2 ) ;
+ a = sqrt(a2);
+ b = sqrt(b2);
}
/*!
- Print the parameters \f$ K = {K_0, ..., K_4} \f$ and the coordinates of the ellipse center.
+ Print the parameters \f$ K = {K_0, ..., K_4} \f$ and the coordinates of the
+ ellipse center.
*/
-void
-vpMeEllipse::printParameters()
+void vpMeEllipse::printParameters()
{
- std::cout << "K" << std::endl ;
- std::cout << K.t() ;
- std::cout << iPc << std::endl ;
+ std::cout << "K" << std::endl;
+ std::cout << K.t();
+ std::cout << iPc << std::endl;
}
/*!
- Computes the \f$ alpha \f$ counter clockwise angle of the two points and store them into alpha1 for the
- smallest and alpha2 for the biggest.
+ Computes the \f$ alpha \f$ counter clockwise angle of the two points and
+ store them into alpha1 for the smallest and alpha2 for the biggest.
\note This function is useful only during the initialization.
\param pt1 : First point whose \f$ alpha \f$ angle is computed.
\param pt2 : Second point whose \f$ alpha \f$ angle is computed.
-*/
-void
-vpMeEllipse::computeAngle(vpImagePoint pt1, vpImagePoint pt2)
+*/
+void vpMeEllipse::computeAngle(const vpImagePoint &pt1, const vpImagePoint &pt2)
{
- getParameters() ;
-
- int number_of_points = 2000 ;
- double incr = 2 * M_PI / number_of_points ; // angle increment
+ getParameters();
- double dmin1 = 1e6 ;
- double dmin2 = 1e6 ;
+ int number_of_points = 2000;
+ double incr = 2 * M_PI / number_of_points; // angle increment
- double k = 0 ;
- while(k < 2*M_PI) {
+ double dmin1 = 1e6;
+ double dmin2 = 1e6;
-// j1 = a *cos(k) ; // equation of an ellipse
-// i1 = b *sin(k) ; // equation of an ellipse
+ double k = 0;
+ while (k < 2 * M_PI) {
- double j1 = a *sin(k) ; // equation of an ellipse
- double i1 = b *cos(k) ; // equation of an ellipse
+ double j1 = a * sin(k); // equation of an ellipse
+ double i1 = b * cos(k); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
-// j11 = iPc.get_j() + ce *j1 - se *i1 ;
-// i11 = iPc.get_i() -( se *j1 + ce *i1) ;
- double j11 = iPc.get_j() + ce *j1 + se *i1 ;
- double i11 = iPc.get_i() - se *j1 + ce *i1 ;
+ 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) ;
- if (d < dmin1)
- {
- dmin1 = d ;
- alpha1 = k ;
+ double d = vpMath::sqr(pt1.get_i() - i11) + vpMath::sqr(pt1.get_j() - j11);
+ if (d < dmin1) {
+ dmin1 = d;
+ alpha1 = k;
}
- d = vpMath::sqr(pt2.get_i()-i11) + vpMath::sqr(pt2.get_j()-j11) ;
- if (d < dmin2)
- {
- dmin2 = d ;
- alpha2 = k ;
+ d = vpMath::sqr(pt2.get_i() - i11) + vpMath::sqr(pt2.get_j() - j11);
+ if (d < dmin2) {
+ dmin2 = d;
+ alpha2 = k;
}
- k += incr ;
+ k += incr;
}
if (alpha2 < alpha1)
alpha2 += 2 * M_PI;
- //else if (alpha2 == alpha1)
+ // else if (alpha2 == alpha1)
else if (std::fabs(alpha2 - alpha1) < std::fabs(alpha1) * std::numeric_limits<double>::epsilon())
alpha2 += 2 * M_PI;
}
-
/*!
Compute the \f$ theta \f$ angle for each vpMeSite.
\note The \f$ theta \f$ angle is useful during the tracking part.
*/
-void
-vpMeEllipse::updateTheta()
+void vpMeEllipse::updateTheta()
{
vpMeSite p_me;
double theta;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
vpImagePoint iP;
iP.set_i(p_me.ifloat);
iP.set_j(p_me.jfloat);
- computeTheta(theta, K, iP) ;
- p_me.alpha = theta ;
+ computeTheta(theta, K, iP);
+ p_me.alpha = theta;
*it = p_me;
}
}
/*!
- Suppress the vpMeSite which are no more detected as point which belongs to the ellipse edge.
+ Suppress the vpMeSite which are no more detected as point which belongs to
+ the ellipse edge.
*/
-void
-vpMeEllipse::suppressPoints()
+void vpMeEllipse::suppressPoints()
{
// Loop through list of sites to track
std::list<vpMeSite>::iterator itList = list.begin();
- for(std::list<double>::iterator it=angle.begin(); it!=angle.end(); ){
- vpMeSite s = *itList;//current reference pixel
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- {
- itList = list.erase(itList) ;
+ for (std::list<double>::iterator it = angle.begin(); it != angle.end();) {
+ vpMeSite s = *itList; // current reference pixel
+ if (s.getState() != vpMeSite::NO_SUPPRESSION) {
+ itList = list.erase(itList);
it = angle.erase(it);
- }
- else
- {
+ } else {
++itList;
++it;
}
}
}
-
-/*!
+/*!
Seek along the ellipse edge defined by its equation, the two extremities of
- the ellipse (ie the two points with the smallest and the biggest \f$ \alpha \f$ angle.
+ the ellipse (ie the two points with the smallest and the biggest \f$ \alpha
+ \f$ angle.
\param I : Image in which the ellipse appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
+void vpMeEllipse::seekExtremities(const vpImage<unsigned char> &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() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
vpImagePoint ip;
- unsigned int memory_range = me->getRange() ;
+ unsigned int memory_range = me->getRange();
me->setRange(2);
- double memory_mu1 = me->getMu1();
+ double memory_mu1 = me->getMu1();
me->setMu1(0.5);
- double memory_mu2 = me->getMu2();
+ double memory_mu2 = me->getMu2();
me->setMu2(0.5);
- double incr = vpMath::rad(2.0) ;
+ double incr = vpMath::rad(2.0);
- if (alpha2-alpha1 < 2*M_PI-vpMath::rad(6.0))
- {
+ if (alpha2 - alpha1 < 2 * M_PI - vpMath::rad(6.0)) {
vpMeSite P;
double k = alpha1;
- double i1,j1;
+ double i1, j1;
- for (unsigned int i=0 ; i < 3 ; i++)
- {
+ for (unsigned int i = 0; i < 3; i++) {
k -= incr;
- //while ( k < -M_PI ) { k+=2*M_PI; }
+ // while ( k < -M_PI ) { k+=2*M_PI; }
- i1 = b *cos(k) ; // equation of an ellipse
- j1 = a *sin(k) ; // equation of an ellipse
- P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
- P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
+ i1 = b * cos(k); // equation of an ellipse
+ j1 = a * sin(k); // equation of an ellipse
+ P.ifloat = iPc.get_i() - se * j1 + ce * i1;
+ P.i = (int)P.ifloat;
+ P.jfloat = iPc.get_j() + ce * j1 + se * i1;
+ P.j = (int)P.jfloat;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
+ ip.set_i(P.i);
+ ip.set_j(P.j);
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ vpDisplay::displayCross(I, ip, 5, vpColor::green);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ ip.set_i(P.i);
+ ip.set_j(P.j);
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue);
}
}
}
@@ -498,55 +460,52 @@ vpMeEllipse::seekExtremities(const vpImage<unsigned char> &I)
k = alpha2;
- for (unsigned int i=0 ; i < 3 ; i++)
- {
+ for (unsigned int i = 0; i < 3; i++) {
k += incr;
- //while ( k > M_PI ) { k-=2*M_PI; }
+ // while ( k > M_PI ) { k-=2*M_PI; }
- i1 = b *cos(k) ; // equation of an ellipse
- j1 = a *sin(k) ; // equation of an ellipse
- P.ifloat = iPc.get_i() - se *j1 + ce *i1 ; P.i = (int)P.ifloat ;
- P.jfloat = iPc.get_j() + ce *j1 + se *i1 ; P.j = (int)P.jfloat ;
+ i1 = b * cos(k); // equation of an ellipse
+ j1 = a * sin(k); // equation of an ellipse
+ P.ifloat = iPc.get_i() - se * j1 + ce * i1;
+ P.i = (int)P.ifloat;
+ P.jfloat = iPc.get_j() + ce * j1 + se * i1;
+ P.j = (int)P.jfloat;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
angle.push_back(k);
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
+ ip.set_i(P.i);
+ ip.set_j(P.j);
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ vpDisplay::displayCross(I, ip, 5, vpColor::green);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ ip.set_i(P.i);
+ ip.set_j(P.j);
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue);
}
}
}
}
}
- suppressPoints() ;
+ suppressPoints();
me->setRange(memory_range);
me->setMu1(memory_mu1);
me->setMu2(memory_mu2);
}
-
/*!
- Finds in the list of vpMeSite the two points with the smallest and the biggest \f$ \alpha \f$ angle value, and stores them.
+ Finds in the list of vpMeSite the two points with the smallest and the
+ biggest \f$ \alpha \f$ angle value, and stores them.
*/
-void
-vpMeEllipse::setExtremities()
+void vpMeEllipse::setExtremities()
{
double alphamin = +1e6;
double alphamax = -1e6;
@@ -558,127 +517,115 @@ vpMeEllipse::setExtremities()
// Loop through list of sites to track
std::list<double>::const_iterator itAngle = angle.begin();
- for(std::list<vpMeSite>::const_iterator itList=list.begin(); itList!=list.end(); ++itList){
- vpMeSite s = *itList;//current reference pixel
+ for (std::list<vpMeSite>::const_iterator itList = list.begin(); itList != list.end(); ++itList) {
+ vpMeSite s = *itList; // current reference pixel
double alpha = *itAngle;
- if (alpha < alphamin)
- {
+ if (alpha < alphamin) {
alphamin = alpha;
- imin = s.ifloat ;
- jmin = s.jfloat ;
+ imin = s.ifloat;
+ jmin = s.jfloat;
}
- if (alpha > alphamax)
- {
+ if (alpha > alphamax) {
alphamax = alpha;
- imax = s.ifloat ;
- jmax = s.jfloat ;
+ imax = s.ifloat;
+ jmax = s.jfloat;
}
++itAngle;
}
alpha1 = alphamin;
alpha2 = alphamax;
- iP1.set_ij(imin,jmin);
- iP2.set_ij(imax,jmax);
+ iP1.set_ij(imin, jmin);
+ iP2.set_ij(imax, jmax);
}
-
/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the ellipse.
*/
-void
-vpMeEllipse::leastSquare()
+void vpMeEllipse::leastSquare()
{
// Construction du systeme Ax=b
// i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
- unsigned int i ;
+ unsigned int i;
- vpMeSite p_me ;
+ vpMeSite p_me;
- unsigned int iter =0 ;
- vpColVector b_(numberOfSignal()) ;
- vpRobust r(numberOfSignal()) ;
+ unsigned int iter = 0;
+ vpColVector b_(numberOfSignal());
+ vpRobust r(numberOfSignal());
r.setThreshold(2);
- r.setIteration(0) ;
- vpMatrix D(numberOfSignal(),numberOfSignal()) ;
- D.eye() ;
- vpMatrix DA, DAmemory ;
- vpColVector DAx ;
- vpColVector w(numberOfSignal()) ;
- w =1 ;
- unsigned int nos_1 = numberOfSignal() ;
-
- if (list.size() < 3)
- {
- throw(vpException(vpException::dimensionError,
- "Not enought moving edges to track the ellipse")) ;
+ r.setIteration(0);
+ vpMatrix D(numberOfSignal(), numberOfSignal());
+ D.eye();
+ vpMatrix DA, DAmemory;
+ vpColVector DAx;
+ vpColVector w(numberOfSignal());
+ w = 1;
+ unsigned int nos_1 = numberOfSignal();
+
+ if (list.size() < 3) {
+ throw(vpException(vpException::dimensionError, "Not enought moving edges to track the ellipse"));
}
- vpMatrix A(numberOfSignal(),5) ;
+ vpMatrix A(numberOfSignal(), 5);
vpColVector x(5);
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ unsigned int k = 0;
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = vpMath::sqr(p_me.jfloat) ;
- A[k][1] = 2 * p_me.ifloat * p_me.jfloat ;
- A[k][2] = 2 * p_me.ifloat ;
- A[k][3] = 2 * p_me.jfloat ;
- A[k][4] = 1 ;
-
- b_[k] = - vpMath::sqr(p_me.ifloat) ;
- k++ ;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ A[k][0] = vpMath::sqr(p_me.jfloat);
+ A[k][1] = 2 * p_me.ifloat * p_me.jfloat;
+ A[k][2] = 2 * p_me.ifloat;
+ A[k][3] = 2 * p_me.jfloat;
+ A[k][4] = 1;
+
+ b_[k] = -vpMath::sqr(p_me.ifloat);
+ k++;
}
}
- while (iter < 4 )
- {
- DA = D*A ;
- vpMatrix DAp ;
+ while (iter < 4) {
+ DA = D * A;
+ vpMatrix DAp;
- x = DA.pseudoInverse(1e-26) *D*b_ ;
+ x = DA.pseudoInverse(1e-26) * D * b_;
vpColVector residu(nos_1);
- residu = b_ - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
+ residu = b_ - A * x;
+ r.setIteration(iter);
+ r.MEstimator(vpRobust::TUKEY, residu, w);
k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
+ for (i = 0; i < nos_1; i++) {
+ D[k][k] = w[k];
k++;
}
iter++;
}
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ k = 0;
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < thresholdWeight)
- {
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ if (w[k] < thresholdWeight) {
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
- k++ ;
+ k++;
}
}
- for(i = 0; i < 5; i ++)
+ for (i = 0; i < 5; i++)
K[i] = x[i];
- getParameters() ;
+ getParameters();
}
-
/*!
Display the ellipse.
@@ -688,31 +635,27 @@ vpMeEllipse::leastSquare()
\param I : Image in which the ellipse appears.
\param col : Color of the displayed ellipse.
*/
-void
-vpMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
+void vpMeEllipse::display(const vpImage<unsigned char> &I, vpColor col)
{
- vpMeEllipse::display(I,iPc,a,b,e,alpha1,alpha2,col);
+ vpMeEllipse::display(I, iPc, a, b, e, alpha1, alpha2, col);
}
-
/*!
- Initilization of the tracking. Ask the user to click counter clockwise on five points
- located on the ellipse edge to track.
+ Initilization of the tracking. Ask the user to click counter clockwise on
+ five points located on the ellipse edge to track.
\warning The points should be selected counter clockwise.
\param I : Image in which the ellipse appears.
*/
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I)
+void vpMeEllipse::initTracking(const vpImage<unsigned char> &I)
{
- const unsigned int n=5 ;
+ const unsigned int n = 5;
vpImagePoint iP[n];
- for (unsigned int k =0 ; k < n ; k++)
- {
- std::cout << "Click points "<< k+1 <<"/" << n ;
- std::cout << " on the ellipse in the trigonometric order" <<std::endl ;
+ for (unsigned int k = 0; k < n; k++) {
+ std::cout << "Click points " << k + 1 << "/" << n;
+ std::cout << " on the ellipse in the trigonometric order" << std::endl;
vpDisplay::getClick(I, iP[k], true);
vpDisplay::displayCross(I, iP[k], 7, vpColor::red);
vpDisplay::flush(I);
@@ -720,126 +663,122 @@ vpMeEllipse::initTracking(const vpImage<unsigned char> &I)
}
iP1 = iP[0];
- iP2 = iP[n-1];
+ iP2 = iP[n - 1];
- initTracking(I, n, iP) ;
+ initTracking(I, n, iP);
}
-
/*!
Initialization of the tracking. The ellipse is defined thanks to the
coordinates of n points.
- \warning It is better to use at least five points to well estimate the ellipse parameters.
- \warning The n points should be selected counter clockwise.
+ \warning It is better to use at least five points to well estimate the
+ ellipse parameters. \warning The n points should be selected counter
+ clockwise.
\param I : Image in which the ellipse appears.
\param n : The number of points in the list.
\param iP : A pointer to a list of points belonging to the ellipse edge.
*/
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- vpImagePoint *iP)
+void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, vpImagePoint *iP)
{
- vpMatrix A(n,5) ;
- vpColVector b_(n) ;
- vpColVector x(5) ;
+ vpMatrix A(n, 5);
+ vpColVector b_(n);
+ vpColVector x(5);
// Construction du systeme Ax=b
// i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = vpMath::sqr(iP[k].get_j()) ;
- A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
- A[k][2] = 2* iP[k].get_i() ;
- A[k][3] = 2* iP[k].get_j() ;
- A[k][4] = 1 ;
+ for (unsigned int k = 0; k < n; k++) {
+ A[k][0] = vpMath::sqr(iP[k].get_j());
+ A[k][1] = 2 * iP[k].get_i() * iP[k].get_j();
+ A[k][2] = 2 * iP[k].get_i();
+ A[k][3] = 2 * iP[k].get_j();
+ A[k][4] = 1;
- b_[k] = - vpMath::sqr(iP[k].get_i()) ;
+ b_[k] = -vpMath::sqr(iP[k].get_i());
}
- K = A.pseudoInverse(1e-26)*b_ ;
+ K = A.pseudoInverse(1e-26) * b_;
iP1 = iP[0];
- iP2 = iP[n-1];
+ iP2 = iP[n - 1];
- getParameters() ;
+ getParameters();
- computeAngle(iP1, iP2) ;
+ 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) ;
+ display(I, vpColor::green);
+ sample(I);
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
- track(I) ;
+ track(I);
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
/*!
Initialization of the tracking. The ellipse is defined thanks to a vector of
n points.
- \warning It is better to use at least five points to well estimate the ellipse parameters.
- \warning The n points should be selected counter clockwise.
+ \warning It is better to use at least five points to well estimate the
+ ellipse parameters. \warning The n points should be selected counter
+ clockwise.
\param I : Image in which the ellipse appears.
- \param iP : A vector of points belonging to the ellipse edge used to initialize the tracking.
+ \param iP : A vector of points belonging to the ellipse edge used to
+ initialize the tracking.
*/
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
+void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &iP)
{
unsigned int n = (unsigned int)(iP.size());
- vpMatrix A(n,5) ;
- vpColVector b_(n) ;
- vpColVector x(5) ;
+ vpMatrix A(n, 5);
+ vpColVector b_(n);
+ vpColVector x(5);
// Construction du systeme Ax=b
// i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
// A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = vpMath::sqr(iP[k].get_j()) ;
- A[k][1] = 2* iP[k].get_i() * iP[k].get_j() ;
- A[k][2] = 2* iP[k].get_i() ;
- A[k][3] = 2* iP[k].get_j() ;
- A[k][4] = 1 ;
+ for (unsigned int k = 0; k < n; k++) {
+ A[k][0] = vpMath::sqr(iP[k].get_j());
+ A[k][1] = 2 * iP[k].get_i() * iP[k].get_j();
+ A[k][2] = 2 * iP[k].get_i();
+ A[k][3] = 2 * iP[k].get_j();
+ A[k][4] = 1;
- b_[k] = - vpMath::sqr(iP[k].get_i()) ;
+ b_[k] = -vpMath::sqr(iP[k].get_i());
}
- K = A.pseudoInverse(1e-26)*b_ ;
+ K = A.pseudoInverse(1e-26) * b_;
iP1 = iP[0];
- iP2 = iP[n-1];
+ iP2 = iP[n - 1];
- getParameters() ;
+ getParameters();
- computeAngle(iP1, iP2) ;
+ 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) ;
+ display(I, vpColor::green);
+ sample(I);
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
- track(I) ;
+ track(I);
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p, double e_p,
- double low_alpha, double high_alpha)
+void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ic, double a_p, double b_p,
+ double e_p, double low_alpha, double high_alpha)
{
iPc = ic;
a = a_p;
@@ -848,21 +787,21 @@ vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &i
alpha1 = low_alpha;
alpha2 = high_alpha;
- if (alpha2 <alpha1)
+ if (alpha2 < alpha1)
alpha2 += 2 * M_PI;
ce = cos(e);
se = sin(e);
- display(I, vpColor::green) ;
- sample(I) ;
+ display(I, vpColor::green);
+ sample(I);
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
- track(I) ;
+ track(I);
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
/*!
@@ -870,60 +809,59 @@ vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &i
\param I : Image in which the ellipse appears.
*/
-void
-vpMeEllipse::track(const vpImage<unsigned char> &I)
+void vpMeEllipse::track(const vpImage<unsigned char> &I)
{
- vpMeTracker::track(I) ;
+ vpMeTracker::track(I);
// Estimation des parametres de la droite aux moindres carre
- suppressPoints() ;
- setExtremities() ;
+ suppressPoints();
+ setExtremities();
- leastSquare() ;
- seekExtremities(I) ;
- setExtremities() ;
- leastSquare() ;
+ leastSquare();
+ seekExtremities(I);
+ setExtremities();
+ leastSquare();
// suppression des points rejetes par la regression robuste
- suppressPoints() ;
- setExtremities() ;
+ suppressPoints();
+ setExtremities();
- //reechantillonage si necessaire
- reSample(I) ;
+ // reechantillonage si necessaire
+ reSample(I);
// remet a jour l'angle delta pour chaque point de la liste
- updateTheta() ;
+ updateTheta();
computeMoments();
// Remise a jour de delta dans la liste de site me
- if (vpDEBUG_ENABLE(2))
- {
- display(I,vpColor::red) ;
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ if (vpDEBUG_ENABLE(2)) {
+ display(I, vpColor::red);
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
}
/*!
- Computes the 0 order moment \f$ m_{00} \f$ which represents the area of the ellipse.
-
- Computes the second central moments \f$ \mu_{20} \f$, \f$ \mu_{02} \f$ and \f$ \mu_{11} \f$
+ Computes the 0 order moment \f$ m_{00} \f$ which represents the area of the
+ ellipse.
+
+ Computes the second central moments \f$ \mu_{20} \f$, \f$ \mu_{02} \f$ and
+ \f$ \mu_{11} \f$
*/
-void
-vpMeEllipse::computeMoments()
+void vpMeEllipse::computeMoments()
{
- double tane = tan(-1/e);
- m00 = M_PI*a*b;
- m10 = m00*iPc.get_i();
- m01 = m00*iPc.get_j();
- m20 = m00*(a*a+b*b*tane*tane)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_i();
- m02 = m00*(a*a*tane*tane+b*b)/(4*(1+tane*tane))+m00*iPc.get_j()*iPc.get_j();
- m11 = m00*tane*(a*a-b*b)/(4*(1+tane*tane))+m00*iPc.get_i()*iPc.get_j();
- mu11 = m11 - iPc.get_j()*m10;
- mu02 = m02 - iPc.get_j()*m01;
- mu20 = m20 - iPc.get_i()*m10;
+ double tane = tan(-1 / e);
+ m00 = M_PI * a * b;
+ m10 = m00 * iPc.get_i();
+ m01 = m00 * iPc.get_j();
+ m20 = m00 * (a * a + b * b * tane * tane) / (4 * (1 + tane * tane)) + m00 * iPc.get_i() * iPc.get_i();
+ m02 = m00 * (a * a * tane * tane + b * b) / (4 * (1 + tane * tane)) + m00 * iPc.get_j() * iPc.get_j();
+ m11 = m00 * tane * (a * a - b * b) / (4 * (1 + tane * tane)) + m00 * iPc.get_i() * iPc.get_j();
+ mu11 = m11 - iPc.get_j() * m10;
+ mu02 = m02 - iPc.get_j() * m01;
+ mu20 = m20 - iPc.get_i() * m10;
}
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
@@ -931,229 +869,181 @@ 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() ;
+ getParameters();
- int number_of_points = 2000 ;
- double incr = 2 * M_PI / number_of_points ; // angle increment
+ int number_of_points = 2000;
+ double incr = 2 * M_PI / number_of_points; // angle increment
- double dmin1 = 1e6 ;
- double dmin2 = 1e6 ;
+ double dmin1 = 1e6;
+ double dmin2 = 1e6;
- double k = -M_PI ;
- while(k < M_PI) {
+ double k = -M_PI;
+ while (k < M_PI) {
- double j1 = a *cos(k) ; // equation of an ellipse
- double i1 = b *sin(k) ; // equation of an ellipse
+ double j1 = a * cos(k); // equation of an ellipse
+ double i1 = b * sin(k); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- double j11 = iPc.get_j() + ce *j1 - se *i1 ;
- double i11 = iPc.get_i() -( se *j1 + ce *i1) ;
-
- double d = vpMath::sqr(ip1-i11) + vpMath::sqr(jp1-j11) ;
- if (d < dmin1)
- {
- dmin1 = d ;
- alpha1 = k ;
- _alpha1 = k ;
+ double j11 = iPc.get_j() + ce * j1 - se * i1;
+ double i11 = iPc.get_i() - (se * j1 + ce * i1);
+
+ double d = vpMath::sqr(ip1 - i11) + vpMath::sqr(jp1 - j11);
+ if (d < dmin1) {
+ dmin1 = d;
+ alpha1 = k;
+ _alpha1 = k;
}
- d = vpMath::sqr(ip2-i11) + vpMath::sqr(jp2-j11) ;
- if (d < dmin2)
- {
- dmin2 = d ;
- alpha2 = k ;
- _alpha2 = k ;
+ d = vpMath::sqr(ip2 - i11) + vpMath::sqr(jp2 - j11);
+ if (d < dmin2) {
+ dmin2 = d;
+ alpha2 = k;
+ _alpha2 = k;
}
- k += incr ;
+ k += incr;
}
- if (alpha2 <alpha1) alpha2 += 2*M_PI ;
-
- vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl ;
+ if (alpha2 < alpha1)
+ alpha2 += 2 * M_PI;
+ vpCDEBUG(1) << "end vpMeEllipse::computeAngle(..)" << alpha1 << " " << alpha2 << std::endl;
}
-
/*!
* \brief computeAngle
*/
-void
-vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
+void vpMeEllipse::computeAngle(int ip1, int jp1, int ip2, int jp2)
{
- double a1, a2 ;
- computeAngle(ip1,jp1,a1, ip2, jp2,a2) ;
+ double a1, a2;
+ computeAngle(ip1, jp1, a1, ip2, jp2, a2);
}
-
-void
-vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n,
- unsigned *i, unsigned *j)
+void vpMeEllipse::initTracking(const vpImage<unsigned char> &I, const unsigned int n, unsigned *i, unsigned *j)
{
- vpCDEBUG(1) <<" begin vpMeEllipse::initTracking()"<<std::endl ;
+ vpMatrix A(n, 5);
+ vpColVector b_(n);
+ vpColVector x(5);
- //if (circle==false)
- if (1)
- {
- vpMatrix A(n,5) ;
- vpColVector b_(n) ;
- vpColVector x(5) ;
-
- // Construction du systeme Ax=b
- //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
- // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
-
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = vpMath::sqr(j[k]) ;
- A[k][1] = 2* i[k] * j[k] ;
- A[k][2] = 2* i[k] ;
- A[k][3] = 2* j[k] ;
- A[k][4] = 1 ;
-
- b_[k] = - vpMath::sqr(i[k]) ;
- }
+ // Construction du systeme Ax=b
+ //! i^2 + K0 j^2 + 2 K1 i j + 2 K2 i + 2 K3 j + K4
+ // A = (j^2 2ij 2i 2j 1) x = (K0 K1 K2 K3 K4)^T b = (-i^2 )
+
+ for (unsigned int k = 0; k < n; k++) {
+ A[k][0] = vpMath::sqr(j[k]);
+ A[k][1] = 2 * i[k] * j[k];
+ A[k][2] = 2 * i[k];
+ A[k][3] = 2 * j[k];
+ A[k][4] = 1;
- K = A.pseudoInverse(1e-26)*b_ ;
- std::cout << K << std::endl;
+ b_[k] = -vpMath::sqr(i[k]);
}
- else
- {
- vpMatrix A(n,3) ;
- vpColVector b_(n) ;
- vpColVector x(3) ;
-
- vpColVector Kc(3) ;
- for (unsigned int k =0 ; k < n ; k++)
- {
- A[k][0] = 2* i[k] ;
- A[k][1] = 2* j[k] ;
-
- A[k][2] = 1 ;
- b_[k] = - vpMath::sqr(i[k]) - vpMath::sqr(j[k]) ;
- }
- Kc = A.pseudoInverse(1e-26)*b_ ;
- K[0] = 1 ;
- K[1] = 0 ;
- K[2] = Kc[0] ;
- K[3] = Kc[1] ;
- K[4] = Kc[2] ;
+ K = A.pseudoInverse(1e-26) * b_;
+ // std::cout << K << std::endl;
- std::cout << K << std::endl;
- }
- iP1.set_i( i[0] );
- iP1.set_j( j[0] );
- iP2.set_i( i[n-1] );
- iP2.set_j( j[n-1] );
+ iP1.set_i(i[0]);
+ iP1.set_j(j[0]);
+ iP2.set_i(i[n - 1]);
+ iP2.set_j(j[n - 1]);
- getParameters() ;
- computeAngle(iP1, iP2) ;
- display(I, vpColor::green) ;
+ getParameters();
+ computeAngle(iP1, iP2);
+ display(I, vpColor::green);
- sample(I) ;
+ sample(I);
// 2. On appelle ce qui n'est pas specifique
{
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
- try{
- track(I) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ track(I);
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
#endif // Deprecated
/*!
Display of the ellipse thanks to the equation parameters.
-
+
\param I : The image used as background.
-
+
\param center : Center of the ellipse
\param A : Semiminor axis of the ellipse.
-
+
\param B : Semimajor axis of the ellipse.
-
- \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
-
+
+ \param E : Angle made by the major axis and the i axis of the image frame
+ \f$ (i,j) \f$
+
\param smallalpha : Smallest \f$ alpha \f$ angle in rad.
\param highalpha : Highest \f$ alpha \f$ angle in rad.
-
+
\param color : Color used to display th lines.
\param thickness : Thickness of the drawings.
*/
-void vpMeEllipse::display(const vpImage<unsigned char>& I, const vpImagePoint ¢er,
- const double &A, const double &B, const double &E,
- const double & smallalpha, const double &highalpha,
- const vpColor &color, unsigned int thickness)
+void vpMeEllipse::display(const vpImage<unsigned char> &I, const vpImagePoint ¢er, const double &A, const double &B,
+ const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
+ unsigned int thickness)
{
double j1, i1;
vpImagePoint iP11;
double j2, i2;
vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
+ j1 = j2 = i1 = i2 = 0;
- double incr = vpMath::rad(2) ; // angle increment
+ double incr = vpMath::rad(2); // angle increment
- vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
+ vpDisplay::displayCross(I, center, 20, vpColor::red, thickness);
- double k = smallalpha ;
- while (k+incr<highalpha)
- {
- j1 = A *cos(k) ; // equation of an ellipse
- i1 = B *sin(k) ; // equation of an ellipse
+ double k = smallalpha;
+ while (k + incr < highalpha) {
+ j1 = A * cos(k); // equation of an ellipse
+ i1 = B * sin(k); // equation of an ellipse
- j2 = A *cos(k+incr) ; // equation of an ellipse
- i2 = B *sin(k+incr) ; // equation of an ellipse
+ j2 = A * cos(k + incr); // equation of an ellipse
+ i2 = B * sin(k + incr); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
+ iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
// to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+ iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
+ iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
- vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
+ vpDisplay::displayLine(I, iP11, iP22, color, thickness);
- k += incr ;
+ k += incr;
}
- j1 = A *cos(smallalpha) ; // equation of an ellipse
- i1 = B *sin(smallalpha) ; // equation of an ellipse
+ j1 = A * cos(smallalpha); // equation of an ellipse
+ i1 = B * sin(smallalpha); // equation of an ellipse
- j2 = A *cos(highalpha) ; // equation of an ellipse
- i2 = B *sin(highalpha) ; // equation of an ellipse
+ j2 = A * cos(highalpha); // equation of an ellipse
+ i2 = B * sin(highalpha); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
+ iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
// to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+ iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
+ iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
- vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
- vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
+ vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness);
+ vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness);
}
/*!
@@ -1168,7 +1058,8 @@ void vpMeEllipse::display(const vpImage<unsigned char>& I, const vpImagePoint &c
\param B : Semimajor axis of the ellipse.
- \param E : Angle made by the major axis and the i axis of the image frame \f$ (i,j) \f$
+ \param E : Angle made by the major axis and the i axis of the image frame
+ \f$ (i,j) \f$
\param smallalpha : Smallest \f$ alpha \f$ angle in rad.
@@ -1178,59 +1069,57 @@ void vpMeEllipse::display(const vpImage<unsigned char>& I, const vpImagePoint &c
\param thickness : Thickness of the drawings.
*/
-void vpMeEllipse::display(const vpImage<vpRGBa>& I, const vpImagePoint ¢er,
- const double &A, const double &B, const double &E,
- const double & smallalpha, const double &highalpha,
- const vpColor &color, unsigned int thickness)
+void vpMeEllipse::display(const vpImage<vpRGBa> &I, const vpImagePoint ¢er, const double &A, const double &B,
+ const double &E, const double &smallalpha, const double &highalpha, const vpColor &color,
+ unsigned int thickness)
{
double j1, i1;
vpImagePoint iP11;
double j2, i2;
vpImagePoint iP22;
- j1 = j2 = i1 = i2 = 0 ;
+ j1 = j2 = i1 = i2 = 0;
- double incr = vpMath::rad(2) ; // angle increment
+ double incr = vpMath::rad(2); // angle increment
- vpDisplay::displayCross(I,center,20, vpColor::red, thickness) ;
+ vpDisplay::displayCross(I, center, 20, vpColor::red, thickness);
- double k = smallalpha ;
- while (k+incr<highalpha)
- {
- j1 = A *cos(k) ; // equation of an ellipse
- i1 = B *sin(k) ; // equation of an ellipse
+ double k = smallalpha;
+ while (k + incr < highalpha) {
+ j1 = A * cos(k); // equation of an ellipse
+ i1 = B * sin(k); // equation of an ellipse
- j2 = A *cos(k+incr) ; // equation of an ellipse
- i2 = B *sin(k+incr) ; // equation of an ellipse
+ j2 = A * cos(k + incr); // equation of an ellipse
+ i2 = B * sin(k + incr); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
+ iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
// to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+ iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
+ iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
- vpDisplay::displayLine(I, iP11, iP22, color, thickness) ;
+ vpDisplay::displayLine(I, iP11, iP22, color, thickness);
- k += incr ;
+ k += incr;
}
- j1 = A *cos(smallalpha) ; // equation of an ellipse
- i1 = B *sin(smallalpha) ; // equation of an ellipse
+ j1 = A * cos(smallalpha); // equation of an ellipse
+ i1 = B * sin(smallalpha); // equation of an ellipse
- j2 = A *cos(highalpha) ; // equation of an ellipse
- i2 = B *sin(highalpha) ; // equation of an ellipse
+ j2 = A * cos(highalpha); // equation of an ellipse
+ i2 = B * sin(highalpha); // equation of an ellipse
- // (i1,j1) are the coordinates on the origin centered ellipse ;
+ // (i1,j1) are the coordinates on the origin centered ellipse;
// a rotation by "e" and a translation by (xci,jc) are done
// to get the coordinates of the point on the shifted ellipse
- iP11.set_j ( center.get_j() + cos(E) *j1 - sin(E) *i1 );
- iP11.set_i ( center.get_i() -( sin(E) *j1 + cos(E) *i1) );
+ iP11.set_j(center.get_j() + cos(E) * j1 - sin(E) * i1);
+ iP11.set_i(center.get_i() - (sin(E) * j1 + cos(E) * i1));
// to get the coordinates of the point on the shifted ellipse
- iP22.set_j ( center.get_j() + cos(E) *j2 - sin(E) *i2 );
- iP22.set_i ( center.get_i() -( sin(E) *j2 + cos(E) *i2) );
+ iP22.set_j(center.get_j() + cos(E) * j2 - sin(E) * i2);
+ iP22.set_i(center.get_i() - (sin(E) * j2 + cos(E) * i2));
- vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness) ;
- vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness) ;
+ vpDisplay::displayLine(I, center, iP11, vpColor::red, thickness);
+ vpDisplay::displayLine(I, center, iP22, vpColor::blue, thickness);
}
diff --git a/modules/tracker/me/src/moving-edges/vpMeLine.cpp b/modules/tracker/me/src/moving-edges/vpMeLine.cpp
index 0ce1dd8..9f46c93 100644
--- a/modules/tracker/me/src/moving-edges/vpMeLine.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMeLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,64 +36,57 @@
*
*****************************************************************************/
-
/*!
\file vpMeLine.cpp
\brief Moving edges
*/
-
-
-#include <visp3/me/vpMeTracker.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/me/vpMeSite.h>
-#include <visp3/me/vpMeLine.h>
-#include <visp3/core/vpRobust.h>
-#include <visp3/core/vpTrackingException.h>
+#include <algorithm> // (std::min)
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpMath.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <algorithm> // std::min
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeLine.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeTracker.h>
#define INCR_MIN 1
void computeDelta(double &delta, int i1, int j1, int i2, int j2);
-static void
-normalizeAngle(double &delta)
+static void normalizeAngle(double &delta)
{
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < -M_PI) { delta += M_PI ; }
+ while (delta > M_PI) {
+ delta -= M_PI;
+ }
+ while (delta < -M_PI) {
+ delta += M_PI;
+ }
}
-void
-computeDelta(double &delta, int i1, int j1, int i2, int j2)
+void computeDelta(double &delta, int i1, int j1, int i2, int j2)
{
- double B = double(i1-i2) ;
- double A = double(j1-j2) ;
-
- delta = atan2(B,A) ;
- delta -= M_PI/2.0 ;
- normalizeAngle(delta) ;
+ double B = double(i1 - i2);
+ double A = double(j1 - j2);
+ delta = atan2(B, A);
+ delta -= M_PI / 2.0;
+ normalizeAngle(delta);
}
-static void
-project(double a, double b, double c,
- double i, double j, double &ip,double &jp)
+static void project(double a, double b, double c, double i, double j, double &ip, double &jp)
{
- if (fabs(a)>fabs(b))
- {
- jp = (vpMath::sqr(a)*j - a*b*i - c*b)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
- ip = (-c-b*jp)/a;
- }
- else
- {
- ip = (vpMath::sqr(b)*i-a*b*j-c*a)/(vpMath::sqr(a)+vpMath::sqr(b)) ;
- jp = (-c-a*ip)/b;
+ if (fabs(a) > fabs(b)) {
+ jp = (vpMath::sqr(a) * j - a * b * i - c * b) / (vpMath::sqr(a) + vpMath::sqr(b));
+ ip = (-c - b * jp) / a;
+ } else {
+ ip = (vpMath::sqr(b) * i - a * b * j - c * a) / (vpMath::sqr(a) + vpMath::sqr(b));
+ jp = (-c - a * ip) / b;
}
}
@@ -102,8 +96,8 @@ project(double a, double b, double c,
*/
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.)
{
}
/*!
@@ -112,8 +106,7 @@ 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),
+ : 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.)
{
@@ -138,10 +131,7 @@ vpMeLine::vpMeLine(const vpMeLine &meline)
Basic destructor.
*/
-vpMeLine::~vpMeLine()
-{
- list.clear();
-}
+vpMeLine::~vpMeLine() { list.clear(); }
/*!
@@ -150,41 +140,38 @@ vpMeLine::~vpMeLine()
\param I : Image in which the line appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeLine::sample(const vpImage<unsigned char>& I)
+void vpMeLine::sample(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
double n_sample;
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
- vpERROR_TRACE("function called with sample step = 0") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "sample step = 0")) ;
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
+ vpERROR_TRACE("function called with sample step = 0");
+ throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
}
// i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+ double diffsi = PExt[0].ifloat - PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat - PExt[1].jfloat;
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
- if(std::fabs(length_p)<=std::numeric_limits<double>::epsilon())
- throw(vpTrackingException(vpTrackingException::fatalError,"points too close of each other to define a line")) ;
+ double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
+ if (std::fabs(length_p) <= std::numeric_limits<double>::epsilon())
+ throw(vpTrackingException(vpTrackingException::fatalError, "points too close of each other to define a line"));
// number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
+ n_sample = length_p / (double)me->getSampleStep();
- double stepi = diffsi/(double)n_sample;
- double stepj = diffsj/(double)n_sample;
+ double stepi = diffsi / (double)n_sample;
+ double stepj = diffsj / (double)n_sample;
// Choose starting point
double is = PExt[1].ifloat;
@@ -197,19 +184,16 @@ vpMeLine::sample(const vpImage<unsigned char>& I)
// line_p, starting at PSiteExt[0]
vpImagePoint ip;
- for(int i=0; i<=vpMath::round(n_sample); i++)
- {
+ for (int i = 0; i <= vpMath::round(n_sample); i++) {
// If point is in the image, add to the sample list
- if(!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols))
- {
- vpMeSite pix ; //= list.value();
- pix.init((int)is, (int)js, delta, 0, sign) ;
- pix.setDisplay(selectDisplay) ;
-
- if(vpDEBUG_ENABLE(3))
- {
- ip.set_i( is );
- ip.set_j( js );
+ if (!outOfImage(vpMath::round(is), vpMath::round(js), 0, rows, cols)) {
+ vpMeSite pix; //= list.value();
+ pix.init((int)is, (int)js, delta, 0, sign);
+ pix.setDisplay(selectDisplay);
+
+ if (vpDEBUG_ENABLE(3)) {
+ ip.set_i(is);
+ ip.set_j(js);
vpDisplay::displayCross(I, ip, 2, vpColor::blue);
}
@@ -217,14 +201,12 @@ vpMeLine::sample(const vpImage<unsigned char>& I)
}
is += stepi;
js += stepj;
-
}
vpCDEBUG(1) << "end vpMeLine::sample() : ";
- vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
+ vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
}
-
/*!
Display line.
@@ -237,13 +219,11 @@ vpMeLine::sample(const vpImage<unsigned char>& I)
that is considered as an outlier is displayed in green.
*/
-void
-vpMeLine::display(const vpImage<unsigned char>&I, vpColor col)
+void vpMeLine::display(const vpImage<unsigned char> &I, vpColor col)
{
- vpMeLine::display(I,PExt[0],PExt[1],list,a,b,c,col);
+ vpMeLine::display(I, PExt[0], PExt[1], list, a, b, c, col);
}
-
/*!
Initilization of the tracking. Ask the user to click on two points
@@ -251,208 +231,188 @@ vpMeLine::display(const vpImage<unsigned char>&I, vpColor col)
\param I : Image in which the line appears.
*/
-void
-vpMeLine::initTracking(const vpImage<unsigned char> &I)
+void vpMeLine::initTracking(const vpImage<unsigned char> &I)
{
vpImagePoint ip1, ip2;
- std::cout << "Click on the line first point..." <<std::endl ;
- while (vpDisplay::getClick(I, ip1)!=true) ;
+ std::cout << "Click on the line first point..." << std::endl;
+ while (vpDisplay::getClick(I, ip1) != true)
+ ;
vpDisplay::displayCross(I, ip1, 7, vpColor::red);
vpDisplay::flush(I);
- std::cout << "Click on the line second point..." <<std::endl ;
- while (vpDisplay::getClick(I, ip2)!=true) ;
+ std::cout << "Click on the line second point..." << std::endl;
+ while (vpDisplay::getClick(I, ip2) != true)
+ ;
vpDisplay::displayCross(I, ip2, 7, vpColor::red);
vpDisplay::flush(I);
- try
- {
- initTracking(I, ip1, ip2) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ initTracking(I, ip1, ip2);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
}
-
/*!
-
+
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the line.
*/
-void
-vpMeLine::leastSquare()
+void vpMeLine::leastSquare()
{
- vpMatrix A(numberOfSignal(),2) ;
- vpColVector x(2), x_1(2) ;
+ vpMatrix A(numberOfSignal(), 2);
+ vpColVector x(2), x_1(2);
x_1 = 0;
- unsigned int i ;
+ unsigned int i;
- vpRobust r(numberOfSignal()) ;
+ vpRobust r(numberOfSignal());
r.setThreshold(2);
- r.setIteration(0) ;
- vpMatrix D(numberOfSignal(),numberOfSignal()) ;
- D.eye() ;
- vpMatrix DA, DAmemory ;
- vpColVector DAx ;
- vpColVector w(numberOfSignal()) ;
- vpColVector B(numberOfSignal()) ;
- w =1 ;
- vpMeSite p_me ;
- unsigned int iter =0 ;
- unsigned int nos_1 = 0 ;
+ r.setIteration(0);
+ vpMatrix D(numberOfSignal(), numberOfSignal());
+ D.eye();
+ vpMatrix DA, DAmemory;
+ vpColVector DAx;
+ vpColVector w(numberOfSignal());
+ vpColVector B(numberOfSignal());
+ w = 1;
+ vpMeSite p_me;
+ unsigned int iter = 0;
+ unsigned int nos_1 = 0;
double distance = 100;
- if (list.size() <= 2 || numberOfSignal() <= 2)
- {
- //vpERROR_TRACE("Not enough point") ;
+ 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
- // a i + j + c = 0
- // A = (i 1) B = (-j)
+ if ((fabs(b) >= 0.9)) // Construction du systeme Ax=B
+ // a i + j + c = 0
+ // A = (i 1) B = (-j)
{
- nos_1 = numberOfSignal() ;
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ nos_1 = numberOfSignal();
+ unsigned int k = 0;
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = p_me.ifloat ;
- A[k][1] = 1 ;
- B[k] = -p_me.jfloat ;
- k++ ;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ A[k][0] = p_me.ifloat;
+ A[k][1] = 1;
+ B[k] = -p_me.jfloat;
+ k++;
}
}
- while (iter < 4 && distance > 0.05)
- {
- DA = D*A ;
- x = DA.pseudoInverse(1e-26) *D*B ;
+ while (iter < 4 && distance > 0.05) {
+ DA = D * A;
+ x = DA.pseudoInverse(1e-26) * D * B;
vpColVector residu(nos_1);
- residu = B - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
+ residu = B - A * x;
+ r.setIteration(iter);
+ r.MEstimator(vpRobust::TUKEY, residu, w);
k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
+ for (i = 0; i < nos_1; i++) {
+ D[k][k] = w[k];
k++;
}
- iter++ ;
- distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
+ iter++;
+ distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
x_1 = x;
}
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ k = 0;
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < 0.2)
- {
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ if (w[k] < 0.2) {
p_me.setState(vpMeSite::M_ESTIMATOR);
-
+
*it = p_me;
}
- k++ ;
+ k++;
}
}
// mise a jour de l'equation de la droite
- a = x[0] ;
- b = 1 ;
- c = x[1] ;
-
- double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
- a /= s ;
- b /= s ;
- c /= s ;
+ a = x[0];
+ b = 1;
+ c = x[1];
+
+ double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
+ a /= s;
+ b /= s;
+ c /= s;
}
-
- else // Construction du systeme Ax=B
- // i + bj + c = 0
- // A = (j 1) B = (-i)
+ else // Construction du systeme Ax=B
+ // i + bj + c = 0
+ // A = (j 1) B = (-i)
{
- nos_1 = numberOfSignal() ;
- unsigned int k =0 ;
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ nos_1 = numberOfSignal();
+ unsigned int k = 0;
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- A[k][0] = p_me.jfloat ;
- A[k][1] = 1 ;
- B[k] = -p_me.ifloat ;
- k++ ;
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ A[k][0] = p_me.jfloat;
+ A[k][1] = 1;
+ B[k] = -p_me.ifloat;
+ k++;
}
}
- while (iter < 4 && distance > 0.05)
- {
- DA = D*A ;
- x = DA.pseudoInverse(1e-26) *D*B ;
+ while (iter < 4 && distance > 0.05) {
+ DA = D * A;
+ x = DA.pseudoInverse(1e-26) * D * B;
vpColVector residu(nos_1);
- residu = B - A*x;
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY,residu,w) ;
+ residu = B - A * x;
+ r.setIteration(iter);
+ r.MEstimator(vpRobust::TUKEY, residu, w);
k = 0;
- for (i=0 ; i < nos_1 ; i++)
- {
- D[k][k] =w[k] ;
+ for (i = 0; i < nos_1; i++) {
+ D[k][k] = w[k];
k++;
}
- iter++ ;
- distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
+ iter++;
+ distance = fabs(x[0] - x_1[0]) + fabs(x[1] - x_1[1]);
x_1 = x;
}
- k =0 ;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ k = 0;
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
- {
- if (w[k] < 0.2)
- {
+ if (p_me.getState() == vpMeSite::NO_SUPPRESSION) {
+ if (w[k] < 0.2) {
p_me.setState(vpMeSite::M_ESTIMATOR);
-
+
*it = p_me;
}
- k++ ;
+ k++;
}
}
- a = 1 ;
- b = x[0] ;
- c = x[1] ;
-
- double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
- a /= s ;
- b /= s ;
- c /= s ;
+ a = 1;
+ b = x[0];
+ c = x[1];
+
+ double s = sqrt(vpMath::sqr(a) + vpMath::sqr(b));
+ a /= s;
+ b /= s;
+ c /= s;
}
// mise a jour du delta
- delta = atan2(a,b) ;
+ delta = atan2(a, b);
- normalizeAngle(delta) ;
+ normalizeAngle(delta);
}
-
-
/*!
-
+
Initialization of the tracking. The line is defined thanks to the
coordinates of two points.
@@ -460,70 +420,64 @@ vpMeLine::leastSquare()
\param ip1 : Coordinates of the first point.
\param ip2 : Coordinates of the second point.
*/
-void
-vpMeLine::initTracking(const vpImage<unsigned char> &I,
- const vpImagePoint &ip1,
- const vpImagePoint &ip2)
+void vpMeLine::initTracking(const vpImage<unsigned char> &I, const vpImagePoint &ip1, const vpImagePoint &ip2)
{
- vpCDEBUG(1) <<" begin vpMeLine::initTracking()"<<std::endl ;
+ vpCDEBUG(1) << " begin vpMeLine::initTracking()" << std::endl;
int i1s, j1s, i2s, j2s;
- i1s = vpMath::round( ip1.get_i() );
- i2s = vpMath::round( ip2.get_i() );
- j1s = vpMath::round( ip1.get_j() );
- j2s = vpMath::round( ip2.get_j() );
+ i1s = vpMath::round(ip1.get_i());
+ i2s = vpMath::round(ip2.get_i());
+ j1s = vpMath::round(ip1.get_j());
+ j2s = vpMath::round(ip2.get_j());
- try{
+ try {
// 1. On fait ce qui concerne les droites (peut etre vide)
{
// Points extremites
- PExt[0].ifloat = (float)ip1.get_i() ;
- PExt[0].jfloat = (float)ip1.get_j() ;
- PExt[1].ifloat = (float)ip2.get_i() ;
- PExt[1].jfloat = (float)ip2.get_j() ;
+ PExt[0].ifloat = (float)ip1.get_i();
+ PExt[0].jfloat = (float)ip1.get_j();
+ PExt[1].ifloat = (float)ip2.get_i();
+ PExt[1].jfloat = (float)ip2.get_j();
- double angle_ = atan2((double)(i1s-i2s),(double)(j1s-j2s)) ;
- a = cos(angle_) ;
- b = sin(angle_) ;
+ double angle_ = atan2((double)(i1s - i2s), (double)(j1s - j2s));
+ a = cos(angle_);
+ b = sin(angle_);
// Real values of a, b can have an other sign. So to get the good values
// of a and b in order to initialise then c, we call track(I) just below
- computeDelta(delta,i1s,j1s,i2s,j2s) ;
+ computeDelta(delta, i1s, j1s, i2s, j2s);
delta_1 = delta;
- // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a), delta);
-
- sample(I) ;
+ // vpTRACE("a: %f b: %f c: %f -b/a: %f delta: %f", a, b, c, -(b/a),
+ // delta);
+ sample(I);
}
// 2. On appelle ce qui n'est pas specifique
{
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
- // Call track(I) to give the good sign to a and b and to initialise c which can be used for the display
+ // Call track(I) to give the good sign to a and b and to initialise c
+ // which can be used for the display
track(I);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
- vpCDEBUG(1) <<" end vpMeLine::initTracking()"<<std::endl ;
+ vpCDEBUG(1) << " end vpMeLine::initTracking()" << std::endl;
}
-
/*!
Suppression of the points which belong no more to the line.
*/
-void
-vpMeLine::suppressPoints()
+void vpMeLine::suppressPoints()
{
// Loop through list of sites to track
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
+ vpMeSite s = *it; // current reference pixel
if (s.getState() != vpMeSite::NO_SUPPRESSION)
it = list.erase(it);
@@ -532,64 +486,56 @@ vpMeLine::suppressPoints()
}
}
-
/*!
Seek in the list of available points the two extremities of the line.
*/
-void
-vpMeLine::setExtremities()
+void vpMeLine::setExtremities()
{
- double imin = +1e6 ;
+ double imin = +1e6;
double jmin = +1e6;
- double imax = -1 ;
- double jmax = -1 ;
-
+ double imax = -1;
+ double jmax = -1;
// Loop through list of sites to track
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.ifloat < imin)
- {
- imin = s.ifloat ;
- jmin = s.jfloat ;
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite s = *it; // current reference pixel
+ if (s.ifloat < imin) {
+ imin = s.ifloat;
+ jmin = s.jfloat;
}
- if (s.ifloat > imax)
- {
- imax = s.ifloat ;
- jmax = s.jfloat ;
+ if (s.ifloat > imax) {
+ imax = s.ifloat;
+ jmax = s.jfloat;
}
}
- PExt[0].ifloat = imin ;
- PExt[0].jfloat = jmin ;
- PExt[1].ifloat = imax ;
- PExt[1].jfloat = jmax ;
-
- if (fabs(imin-imax) < 25)
- {
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
- if (s.jfloat < jmin)
- {
- imin = s.ifloat ;
- jmin = s.jfloat ;
+ PExt[0].ifloat = imin;
+ PExt[0].jfloat = jmin;
+ PExt[1].ifloat = imax;
+ PExt[1].jfloat = jmax;
+
+ if (fabs(imin - imax) < 25) {
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite s = *it; // current reference pixel
+ if (s.jfloat < jmin) {
+ imin = s.ifloat;
+ jmin = s.jfloat;
}
- if (s.jfloat > jmax)
- {
- imax = s.ifloat ;
- jmax = s.jfloat ;
+ if (s.jfloat > jmax) {
+ imax = s.ifloat;
+ jmax = s.jfloat;
}
}
- PExt[0].ifloat = imin ;
- PExt[0].jfloat = jmin ;
- PExt[1].ifloat = imax ;
- PExt[1].jfloat = jmax ;
+ PExt[0].ifloat = imin;
+ PExt[0].jfloat = jmin;
+ PExt[1].ifloat = imax;
+ PExt[1].jfloat = jmax;
}
}
-/*!
+/*!
Seek along the line defined by its equation, the two extremities of
the line. This function is useful in case of translation of the
@@ -597,110 +543,103 @@ vpMeLine::setExtremities()
\param I : Image in which the line appears.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
+void vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
{
- vpCDEBUG(1) <<"begin vpMeLine::sample() : "<<std::endl ;
+ vpCDEBUG(1) << "begin vpMeLine::sample() : " << std::endl;
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
double n_sample;
- //if (me->getSampleStep()==0)
- if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon())
- {
+ // if (me->getSampleStep()==0)
+ if (std::fabs(me->getSampleStep()) <= std::numeric_limits<double>::epsilon()) {
- vpERROR_TRACE("function called with sample step = 0") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "sample step = 0")) ;
+ vpERROR_TRACE("function called with sample step = 0");
+ throw(vpTrackingException(vpTrackingException::fatalError, "sample step = 0"));
}
// i, j portions of the line_p
- double diffsi = PExt[0].ifloat-PExt[1].ifloat;
- double diffsj = PExt[0].jfloat-PExt[1].jfloat;
+ double diffsi = PExt[0].ifloat - PExt[1].ifloat;
+ double diffsj = PExt[0].jfloat - PExt[1].jfloat;
- double s = vpMath::sqr(diffsi)+vpMath::sqr(diffsj) ;
+ double s = vpMath::sqr(diffsi) + vpMath::sqr(diffsj);
- double di = diffsi/sqrt(s) ; // pas de risque de /0 car d(P1,P2) >0
- double dj = diffsj/sqrt(s) ;
+ double di = diffsi / sqrt(s); // pas de risque de /0 car d(P1,P2) >0
+ double dj = diffsj / sqrt(s);
- double length_p = sqrt((vpMath::sqr(diffsi)+vpMath::sqr(diffsj)));
+ double length_p = sqrt((vpMath::sqr(diffsi) + vpMath::sqr(diffsj)));
// number of samples along line_p
- n_sample = length_p/(double)me->getSampleStep();
+ n_sample = length_p / (double)me->getSampleStep();
double sample_step = (double)me->getSampleStep();
- vpMeSite P ;
- P.init((int) PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
+ vpMeSite P;
+ P.init((int)PExt[0].ifloat, (int)PExt[0].jfloat, delta_1, 0, sign);
+ P.setDisplay(selectDisplay);
- unsigned int memory_range = me->getRange() ;
+ unsigned int memory_range = me->getRange();
me->setRange(1);
vpImagePoint ip;
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + dj*sample_step ; P.j = (int)P.jfloat ;
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat + di * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat + dj * sample_step;
+ P.j = (int)P.jfloat;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
+ ip.set_i(P.i);
+ ip.set_j(P.j);
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ vpDisplay::displayCross(I, ip, 5, vpColor::green);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ ip.set_i(P.i);
+ ip.set_j(P.j);
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue);
}
}
}
}
- P.init((int) PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign) ;
- P.setDisplay(selectDisplay) ;
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - di*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - dj*sample_step ; P.j = (int)P.jfloat ;
+ P.init((int)PExt[1].ifloat, (int)PExt[1].jfloat, delta_1, 0, sign);
+ P.setDisplay(selectDisplay);
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat - di * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat - dj * sample_step;
+ P.j = (int)P.jfloat;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false) ;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
list.push_back(P);
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 5, vpColor::green) ;
+ ip.set_i(P.i);
+ ip.set_j(P.j);
+ vpDisplay::displayCross(I, ip, 5, vpColor::green);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- ip.set_i( P.i );
- ip.set_j( P.j );
- vpDisplay::displayCross(I, ip, 10, vpColor::blue) ;
+ ip.set_i(P.i);
+ ip.set_j(P.j);
+ vpDisplay::displayCross(I, ip, 10, vpColor::blue);
}
}
}
@@ -708,206 +647,189 @@ vpMeLine::seekExtremities(const vpImage<unsigned char> &I)
me->setRange(memory_range);
- vpCDEBUG(1) <<"end vpMeLine::sample() : " ;
- vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl ;
+ vpCDEBUG(1) << "end vpMeLine::sample() : ";
+ vpCDEBUG(1) << n_sample << " point inserted in the list " << std::endl;
}
-
/*!
-
+
Resample the line if the number of sample is less than 80% of the
expected value.
-
+
\note The expected value is computed thanks to the length of the
line and the parameter which indicates the number of pixel between
two points (vpMe::sample_step).
\param I : Image in which the line appears.
*/
-void
-vpMeLine::reSample(const vpImage<unsigned char> &I)
+void vpMeLine::reSample(const vpImage<unsigned char> &I)
{
- double i1,j1,i2,j2 ;
+ double i1, j1, i2, j2;
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) ;
- project(a,b,c,PExt[1].ifloat,PExt[1].jfloat,i2,j2) ;
+ project(a, b, c, PExt[0].ifloat, PExt[0].jfloat, i1, j1);
+ project(a, b, c, PExt[1].ifloat, PExt[1].jfloat, i2, j2);
// Points extremites
- PExt[0].ifloat = i1 ;
- PExt[0].jfloat = j1 ;
- PExt[1].ifloat = i2 ;
- PExt[1].jfloat = j2 ;
+ PExt[0].ifloat = i1;
+ PExt[0].jfloat = j1;
+ PExt[1].ifloat = i2;
+ PExt[1].jfloat = j2;
- double d = sqrt(vpMath::sqr(i1-i2)+vpMath::sqr(j1-j2)) ;
+ double d = sqrt(vpMath::sqr(i1 - i2) + vpMath::sqr(j1 - j2));
- unsigned int n = numberOfSignal() ;
+ unsigned int n = numberOfSignal();
double expecteddensity = d / (double)me->getSampleStep();
- if ((double)n<0.9*expecteddensity)
- {
+ if ((double)n < 0.9 * expecteddensity) {
double delta_new = delta;
delta = delta_1;
- sample(I) ;
+ sample(I);
delta = delta_new;
// 2. On appelle ce qui n'est pas specifique
{
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
}
}
}
/*!
-
+
Set the alpha value of the different vpMeSite to the value of delta.
*/
-void
-vpMeLine::updateDelta()
+void vpMeLine::updateDelta()
{
- vpMeSite p_me ;
+ vpMeSite p_me;
- double angle_ = delta + M_PI/2;
+ double angle_ = delta + M_PI / 2;
double diff = 0;
- while (angle_<0) angle_ += M_PI;
- while (angle_>M_PI) angle_ -= M_PI;
+ while (angle_ < 0)
+ angle_ += M_PI;
+ while (angle_ > M_PI)
+ angle_ -= M_PI;
- angle_ = vpMath::round(angle_ * 180 / M_PI) ;
+ angle_ = vpMath::round(angle_ * 180 / M_PI);
- //if(fabs(angle_) == 180 )
- if(std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon())
- {
- angle_= 0 ;
+ // if(fabs(angle_) == 180 )
+ if (std::fabs(std::fabs(angle_) - 180) <= std::numeric_limits<double>::epsilon()) {
+ angle_ = 0;
}
- //std::cout << "angle theta : " << theta << std::endl ;
+ // std::cout << "angle theta : " << theta << std::endl ;
diff = fabs(angle_ - angle_1);
if (diff > 90)
sign *= -1;
angle_1 = angle_;
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
p_me = *it;
- p_me.alpha = delta ;
+ p_me.alpha = delta;
p_me.mask_sign = sign;
*it = p_me;
}
delta_1 = delta;
}
-
/*!
Track the line in the image I.
\param I : Image in which the line appears.
*/
-void
-vpMeLine::track(const vpImage<unsigned char> &I)
+void vpMeLine::track(const vpImage<unsigned char> &I)
{
- vpCDEBUG(1) <<"begin vpMeLine::track()"<<std::endl ;
+ vpCDEBUG(1) << "begin vpMeLine::track()" << std::endl;
// 1. On fait ce qui concerne les droites (peut etre vide)
+ {} // 2. On appelle ce qui n'est pas specifique
{
- }
- // 2. On appelle ce qui n'est pas specifique
- {
- vpMeTracker::track(I) ;
+ vpMeTracker::track(I);
}
// 3. On revient aux droites
{
// supression des points rejetes par les ME
- suppressPoints() ;
- setExtremities() ;
-
+ suppressPoints();
+ setExtremities();
// Estimation des parametres de la droite aux moindres carre
- try
- {
- leastSquare() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ try {
+ leastSquare();
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
// recherche de point aux extremite de la droites
// dans le cas d'un glissement
- seekExtremities(I) ;
-
- setExtremities() ;
- try
- {
- leastSquare() ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ seekExtremities(I);
+
+ setExtremities();
+ try {
+ leastSquare();
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
// suppression des points rejetes par la regression robuste
- suppressPoints() ;
- setExtremities() ;
+ suppressPoints();
+ setExtremities();
- //reechantillonage si necessaire
- reSample(I) ;
+ // reechantillonage si necessaire
+ reSample(I);
// remet a jour l'angle delta pour chaque point de la liste
- updateDelta() ;
+ updateDelta();
// Remise a jour de delta dans la liste de site me
- if (vpDEBUG_ENABLE(2))
- {
- display(I,vpColor::red) ;
- vpMeTracker::display(I) ;
- vpDisplay::flush(I) ;
+ if (vpDEBUG_ENABLE(2)) {
+ display(I, vpColor::red);
+ vpMeTracker::display(I);
+ vpDisplay::flush(I);
}
-
-
}
- computeRhoTheta(I) ;
+ computeRhoTheta(I);
- vpCDEBUG(1) <<"end vpMeLine::track()"<<std::endl ;
+ vpCDEBUG(1) << "end vpMeLine::track()" << std::endl;
}
-void vpMeLine::update_indices(double theta,int i,int j,int incr,int& i1,int& i2,int& j1,int& j2){
- i1 = (int)(i + cos(theta) *incr) ;
- j1 = (int)(j + sin(theta) *incr) ;
+void vpMeLine::update_indices(double theta, int i, int j, int incr, int &i1, int &i2, int &j1, int &j2)
+{
+ i1 = (int)(i + cos(theta) * incr);
+ j1 = (int)(j + sin(theta) * incr);
- i2 = (int)(i - cos(theta) *incr) ;
- j2 = (int)(j - sin(theta) *incr) ;
+ i2 = (int)(i - cos(theta) * incr);
+ j2 = (int)(j - sin(theta) * incr);
}
/*!
-
+
Compute the two parameters \f$(\rho, \theta)\f$ of the line.
\param I : Image in which the line appears.
*/
-void
-vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
+void vpMeLine::computeRhoTheta(const vpImage<unsigned char> &I)
{
- //rho = -c ;
- //theta = atan2(a,b) ;
+ // rho = -c ;
+ // theta = atan2(a,b) ;
rho = fabs(c);
- theta = atan2(b,a) ;
+ theta = atan2(b, a);
- while (theta >= M_PI) theta -=M_PI ;
- while (theta < 0) theta +=M_PI ;
-
- if(_useIntensityForRho){
+ while (theta >= M_PI)
+ theta -= M_PI;
+ while (theta < 0)
+ theta += M_PI;
+
+ if (_useIntensityForRho) {
/* while(theta < -M_PI) theta += 2*M_PI ;
while(theta >= M_PI) theta -= 2*M_PI ;
@@ -928,108 +850,95 @@ vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
}
*/
// convention pour choisir le signe de rho
- int i,j ;
- i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat )/2) ;
- j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat )/2) ;
+ int i, j;
+ i = vpMath::round((PExt[0].ifloat + PExt[1].ifloat) / 2);
+ j = vpMath::round((PExt[0].jfloat + PExt[1].jfloat) / 2);
- int end = false ;
- int incr = 10 ;
+ int end = false;
+ int incr = 10;
- int i1=0,i2=0,j1=0,j2=0 ;
- unsigned char v1=0,v2=0 ;
+ int i1 = 0, i2 = 0, j1 = 0, j2 = 0;
+ unsigned char v1 = 0, v2 = 0;
int width_ = (int)I.getWidth();
int height_ = (int)I.getHeight();
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
-
- if(i1<0 || i1>=height_ || i2<0 || i2>=height_ ||
- j1<0 || j1>=width_ || j2<0 || j2>=width_){
- double rho_lim1 = fabs((double)i/cos(theta));
- double rho_lim2 = fabs((double)j/sin(theta));
-
- double co_rho_lim1 = fabs(((double)(height_-i))/cos(theta));
- double co_rho_lim2 = fabs(((double)(width_-j))/sin(theta));
-
- double rho_lim = std::min(rho_lim1,rho_lim2);
- double co_rho_lim = std::min(co_rho_lim1,co_rho_lim2);
- incr = (int)std::floor(std::min(rho_lim,co_rho_lim));
- if(incr<INCR_MIN){
- vpERROR_TRACE("increment is too small") ;
- throw(vpTrackingException(vpTrackingException::fatalError,
- "increment is too small")) ;
- }
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
+ update_indices(theta, i, j, incr, i1, i2, j1, j2);
+
+ if (i1 < 0 || i1 >= height_ || i2 < 0 || i2 >= height_ || j1 < 0 || j1 >= width_ || j2 < 0 || j2 >= width_) {
+ double rho_lim1 = fabs((double)i / cos(theta));
+ double rho_lim2 = fabs((double)j / sin(theta));
+
+ double co_rho_lim1 = fabs(((double)(height_ - i)) / cos(theta));
+ double co_rho_lim2 = fabs(((double)(width_ - j)) / sin(theta));
+
+ double rho_lim = (std::min)(rho_lim1, rho_lim2);
+ double co_rho_lim = (std::min)(co_rho_lim1, co_rho_lim2);
+ incr = (int)std::floor((std::min)(rho_lim, co_rho_lim));
+ if (incr < INCR_MIN) {
+ vpERROR_TRACE("increment is too small");
+ throw(vpTrackingException(vpTrackingException::fatalError, "increment is too small"));
+ }
+ update_indices(theta, i, j, incr, i1, i2, j1, j2);
}
- while (!end)
- {
+ while (!end) {
end = true;
unsigned int i1_ = static_cast<unsigned int>(i1);
unsigned int j1_ = static_cast<unsigned int>(j1);
unsigned int i2_ = static_cast<unsigned int>(i2);
unsigned int j2_ = static_cast<unsigned int>(j2);
- v1=I[i1_][j1_];
- v2=I[i2_][j2_];
- if (abs(v1-v2) < 1)
- {
- incr-- ;
- end = false ;
- if (incr==1)
- {
- throw(vpException(vpException::fatalError,
- "In vpMeLine cannot determine rho sign, since there is no gray level difference between both sides of the line"));
+ v1 = I[i1_][j1_];
+ v2 = I[i2_][j2_];
+ if (abs(v1 - v2) < 1) {
+ 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"));
}
}
- update_indices(theta,i,j,incr,i1,i2,j1,j2);
+ update_indices(theta, i, j, incr, i1, i2, j1, j2);
}
- if (theta >=0 && theta <= M_PI/2)
- {
- if (v2 < v1)
- {
+ if (theta >= 0 && theta <= M_PI / 2) {
+ if (v2 < v1) {
theta += M_PI;
rho *= -1;
}
}
- else
- {
+ else {
double jinter;
- jinter = -c/b;
- if (v2 < v1)
- {
+ jinter = -c / b;
+ if (v2 < v1) {
theta += M_PI;
- if (jinter > 0)
- {
+ if (jinter > 0) {
rho *= -1;
}
}
- else
- {
- if (jinter < 0)
- {
+ else {
+ if (jinter < 0) {
rho *= -1;
}
}
}
- if (vpDEBUG_ENABLE(2))
- {
+ if (vpDEBUG_ENABLE(2)) {
vpImagePoint ip, ip1, ip2;
- ip.set_i( i );
- ip.set_j( j );
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip.set_i(i);
+ ip.set_j(j);
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
- vpDisplay::displayArrow(I, ip, ip1, vpColor::green) ;
- vpDisplay::displayArrow(I, ip, ip2, vpColor::red) ;
+ vpDisplay::displayArrow(I, ip, ip1, vpColor::green);
+ vpDisplay::displayArrow(I, ip, ip2, vpColor::red);
}
}
-
}
/*!
@@ -1042,41 +951,31 @@ vpMeLine::computeRhoTheta(const vpImage<unsigned char>& I)
class, \f$\rho\f$ is signed.
*/
-double
-vpMeLine::getRho() const
-{
- return rho ;
-}
+double vpMeLine::getRho() const { return rho; }
/*!
Get the value of the angle \f$\theta\f$.
*/
-double
-vpMeLine::getTheta() const
-{
- return theta ;
-}
+double vpMeLine::getTheta() const { return theta; }
/*!
-
+
Get the extremities of the line.
\param ip1 : Coordinates of the first extremity.
\param ip2 : Coordinates of the second extremity.
*/
-void
-vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
+void vpMeLine::getExtremities(vpImagePoint &ip1, vpImagePoint &ip2)
{
/*Return the coordinates of the extremities of the line*/
- ip1.set_i( PExt[0].ifloat );
- ip1.set_j( PExt[0].jfloat );
- ip2.set_i( PExt[1].ifloat );
- ip2.set_j( PExt[1].jfloat );
+ ip1.set_i(PExt[0].ifloat);
+ ip1.set_j(PExt[0].jfloat);
+ ip2.set_i(PExt[1].ifloat);
+ ip2.set_j(PExt[1].jfloat);
}
-
/*!
-
+
Computes the intersection point of two lines. The result is given in
the (i,j) frame.
@@ -1087,9 +986,7 @@ 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;
@@ -1098,124 +995,116 @@ vpMeLine::intersection(const vpMeLine &line1, const vpMeLine &line2,
double b2 = line2.b;
double c2 = line2.c;
- try{
- double i=0, j=0;
+ try {
+ double i = 0, j = 0;
double denom = 0;
- if (a1 > 0.1)
- {
- denom = (-(a2/a1) * b1 + b2);
+ if (a1 > 0.1) {
+ denom = (-(a2 / a1) * b1 + b2);
- //if (denom == 0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
+ // if (denom == 0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
return (false);
}
- //if (denom != 0 )
- if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
- {
- j = ( (a2/a1)*c1 - c2 ) / denom;
- i = (-b1*j - c1) / a1;
+ // if (denom != 0 )
+ if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
+ j = ((a2 / a1) * c1 - c2) / denom;
+ i = (-b1 * j - c1) / a1;
}
}
- else
- {
- denom = (-(b2/b1) * a1 + a2);
+ else {
+ denom = (-(b2 / b1) * a1 + a2);
- //if (denom == 0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
+ // if (denom == 0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
std::cout << "!!!!!!!!!!!!! Problem : Lines are parallel !!!!!!!!!!!!!" << std::endl;
return (false);
}
- //if (denom != 0 )
- if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
- {
- i = ( (b2/b1)*c1 - c2 ) / denom;
- j = (-a1*i - c1) / b1;
+ // if (denom != 0 )
+ if (std::fabs(denom) > std::numeric_limits<double>::epsilon()) {
+ i = ((b2 / b1) * c1 - c2) / denom;
+ j = (-a1 * i - c1) / b1;
}
}
- ip.set_i( i );
- ip.set_j( j );
-
+ ip.set_i(i);
+ ip.set_j(j);
+
return (true);
- }
- catch(...)
- {
+ } catch (...) {
return (false);
}
}
/*!
- Display of a moving line thanks to its equation parameters and its extremities.
-
+ Display of a moving line thanks to its equation parameters and its
+ extremities.
+
\param I : The image used as background.
\param PExt1 : First extrimity
-
+
\param PExt2 : Second extrimity
-
+
\param A : Parameter a of the line equation a*i + b*j + c = 0
-
+
\param B : Parameter b of the line equation a*i + b*j + c = 0
-
+
\param C : Parameter c of the line equation a*i + b*j + c = 0
-
+
\param color : Color used to display the line.
-
+
\param thickness : Thickness of the line.
*/
-void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
+void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
+ const double &B, const double &C, const vpColor &color, unsigned int thickness)
{
vpImagePoint ip1, ip2;
if (fabs(A) < fabs(B)) {
double i1, j1, i2, j2;
i1 = 0;
- j1 = (-A*i1 -C) / B;
+ j1 = (-A * i1 - C) / B;
i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
+ j2 = (-A * i2 - C) / B;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
- }
- else {
+ } else {
double i1, j1, i2, j2;
j1 = 0;
i1 = -(B * j1 + C) / A;
j2 = I.getWidth() - 1.0;
i2 = -(B * j2 + C) / A;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt1.ifloat);
+ ip1.set_j(PExt1.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt2.ifloat);
+ ip1.set_j(PExt2.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
}
/*!
- Display of a moving line thanks to its equation parameters and its extremities.
+ Display of a moving line thanks to its equation parameters and its
+ extremities.
\param I : The image used as background.
@@ -1233,135 +1122,133 @@ void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, co
\param thickness : Thickness of the line.
*/
-void vpMeLine::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
+void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2, const double &A,
+ const double &B, const double &C, const vpColor &color, unsigned int thickness)
{
vpImagePoint ip1, ip2;
if (fabs(A) < fabs(B)) {
double i1, j1, i2, j2;
i1 = 0;
- j1 = (-A*i1 -C) / B;
+ j1 = (-A * i1 - C) / B;
i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
+ j2 = (-A * i2 - C) / B;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
- }
- else {
+ } else {
double i1, j1, i2, j2;
j1 = 0;
i1 = -(B * j1 + C) / A;
j2 = I.getWidth() - 1.0;
i2 = -(B * j2 + C) / A;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt1.ifloat);
+ ip1.set_j(PExt1.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt2.ifloat);
+ ip1.set_j(PExt2.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
}
/*!
- Display of a moving line thanks to its equation parameters and its extremities with all the site list.
-
+ Display of a moving line thanks to its equation parameters and its
+ extremities with all the site list.
+
\param I : The image used as background.
\param PExt1 : First extrimity
-
+
\param PExt2 : Second extrimity
-
+
\param site_list : vpMeSite list
-
+
\param A : Parameter a of the line equation a*i + b*j + c = 0
-
+
\param B : Parameter b of the line equation a*i + b*j + c = 0
-
+
\param C : Parameter c of the line equation a*i + b*j + c = 0
-
+
\param color : Color used to display the line.
-
+
\param thickness : Thickness of the line.
*/
-void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
+void vpMeLine::display(const vpImage<unsigned char> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
{
vpImagePoint ip;
-
- for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
+
+ for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
vpMeSite pix = *it;
- ip.set_i( pix.ifloat );
- ip.set_j( pix.jfloat );
+ ip.set_i(pix.ifloat);
+ ip.set_j(pix.jfloat);
if (pix.getState() == vpMeSite::M_ESTIMATOR)
- vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
+ vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
else
- vpDisplay::displayCross(I, ip, 5, color,thickness);
+ vpDisplay::displayCross(I, ip, 5, color, thickness);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
-
+
vpImagePoint ip1, ip2;
if (fabs(A) < fabs(B)) {
double i1, j1, i2, j2;
i1 = 0;
- j1 = (-A*i1 -C) / B;
+ j1 = (-A * i1 - C) / B;
i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
+ j2 = (-A * i2 - C) / B;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
- }
- else {
+ } else {
double i1, j1, i2, j2;
j1 = 0;
i1 = -(B * j1 + C) / A;
j2 = I.getWidth() - 1.0;
i2 = -(B * j2 + C) / A;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt1.ifloat);
+ ip1.set_j(PExt1.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt2.ifloat);
+ ip1.set_j(PExt2.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
}
/*!
- Display of a moving line thanks to its equation parameters and its extremities with all the site list.
+ Display of a moving line thanks to its equation parameters and its
+ extremities with all the site list.
\param I : The image used as background.
@@ -1381,24 +1268,23 @@ void vpMeLine::display(const vpImage<unsigned char>& I,const vpMeSite &PExt1, co
\param thickness : Thickness of the line.
*/
-void vpMeLine::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpMeSite &PExt2,
- const std::list<vpMeSite> &site_list,
- const double &A, const double &B, const double &C,
- const vpColor &color, unsigned int thickness)
+void vpMeLine::display(const vpImage<vpRGBa> &I, const vpMeSite &PExt1, const vpMeSite &PExt2,
+ const std::list<vpMeSite> &site_list, const double &A, const double &B, const double &C,
+ const vpColor &color, unsigned int thickness)
{
vpImagePoint ip;
- for(std::list<vpMeSite>::const_iterator it=site_list.begin(); it!=site_list.end(); ++it){
+ for (std::list<vpMeSite>::const_iterator it = site_list.begin(); it != site_list.end(); ++it) {
vpMeSite pix = *it;
- ip.set_i( pix.ifloat );
- ip.set_j( pix.jfloat );
+ ip.set_i(pix.ifloat);
+ ip.set_j(pix.jfloat);
if (pix.getState() == vpMeSite::M_ESTIMATOR)
- vpDisplay::displayCross(I, ip, 5, vpColor::green,thickness);
+ vpDisplay::displayCross(I, ip, 5, vpColor::green, thickness);
else
- vpDisplay::displayCross(I, ip, 5, color,thickness);
+ vpDisplay::displayCross(I, ip, 5, color, thickness);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
vpImagePoint ip1, ip2;
@@ -1406,39 +1292,37 @@ void vpMeLine::display(const vpImage<vpRGBa>& I,const vpMeSite &PExt1, const vpM
if (fabs(A) < fabs(B)) {
double i1, j1, i2, j2;
i1 = 0;
- j1 = (-A*i1 -C) / B;
+ j1 = (-A * i1 - C) / B;
i2 = I.getHeight() - 1.0;
- j2 = (-A*i2 -C) / B;
+ j2 = (-A * i2 - C) / B;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
- }
- else {
+ } else {
double i1, j1, i2, j2;
j1 = 0;
i1 = -(B * j1 + C) / A;
j2 = I.getWidth() - 1.0;
i2 = -(B * j2 + C) / A;
- ip1.set_i( i1 );
- ip1.set_j( j1 );
- ip2.set_i( i2 );
- ip2.set_j( j2 );
+ ip1.set_i(i1);
+ ip1.set_j(j1);
+ ip2.set_i(i2);
+ ip2.set_j(j2);
vpDisplay::displayLine(I, ip1, ip2, color);
- //vpDisplay::flush(I);
+ // vpDisplay::flush(I);
}
- ip1.set_i( PExt1.ifloat );
- ip1.set_j( PExt1.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt1.ifloat);
+ ip1.set_j(PExt1.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
- ip1.set_i( PExt2.ifloat );
- ip1.set_j( PExt2.jfloat );
- vpDisplay::displayCross(I, ip1, 10, vpColor::green,thickness);
+ ip1.set_i(PExt2.ifloat);
+ ip1.set_j(PExt2.jfloat);
+ vpDisplay::displayCross(I, ip1, 10, vpColor::green, thickness);
}
-
diff --git a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp
index 16bea22..b36f5b7 100644
--- a/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMeNurbs.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,146 +36,137 @@
*
*****************************************************************************/
-
/*!
\file vpMeNurbs.cpp
\brief Moving edges
*/
-#include <visp3/me/vpMeTracker.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/me/vpMeSite.h>
-#include <visp3/me/vpMeNurbs.h>
-#include <visp3/core/vpRobust.h>
-#include <visp3/core/vpTrackingException.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
+#include <stdlib.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRect.h>
-#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpImageFilter.h>
-#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <visp3/core/vpRobust.h>
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/me/vpMe.h>
+#include <visp3/me/vpMeNurbs.h>
+#include <visp3/me/vpMeSite.h>
+#include <visp3/me/vpMeTracker.h>
#ifdef VISP_HAVE_OPENCV
-# if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
//# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/imgproc/imgproc_c.h>
-# else
-# include <cv.h>
-# endif
+#include <opencv2/imgproc/imgproc_c.h>
+#else
+#include <cv.h>
+#endif
#endif
double computeDelta(double deltai, double deltaj);
-void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP,
- vpMe* me, double &angle, double &convlt);
-vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP);
+void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt);
+vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP);
bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list);
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list);
#endif
-//Compute the angle delta = arctan(deltai/deltaj)
-//and normalize it between 0 and pi
-double
-computeDelta(double deltai, double deltaj)
+// Compute the angle delta = arctan(deltai/deltaj)
+// and normalize it between 0 and pi
+double computeDelta(double deltai, double deltaj)
{
double delta;
- delta = atan2(deltai,deltaj) ;
- delta -= M_PI/2.0 ;
- while (delta > M_PI) { delta -= M_PI ; }
- while (delta < 0) { delta += M_PI ; }
- return(delta);
+ delta = atan2(deltai, deltaj);
+ delta -= M_PI / 2.0;
+ while (delta > M_PI) {
+ delta -= M_PI;
+ }
+ while (delta < 0) {
+ delta += M_PI;
+ }
+ return (delta);
}
-//Check if the image point is in the image and not to close to
-//its edge to enable the computation of a convolution whith a mask.
-static
-bool outOfImage( vpImagePoint iP , int half , int rows , int cols)
+// Check if the image point is in the image and not to close to
+// its edge to enable the computation of a convolution whith a mask.
+static bool outOfImage(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<unsigned char> &I, const vpImagePoint &iP,
- vpMe* me, double &angle, double &convlt)
+// if iP is a edge point, it computes the angle corresponding to the
+// highest convolution result. the angle is between 0 an 179.
+// The result gives the angle in RADIAN + pi/2 (to deal with the moving edeg
+// alpha angle) and the corresponding convolution result.
+void findAngle(const vpImage<unsigned char> &I, const vpImagePoint &iP, vpMe *me, double &angle, double &convlt)
{
int Iheight = (int)I.getHeight();
int Iwidth = (int)I.getWidth();
angle = 0.0;
convlt = 0.0;
- for (int i = 0; i < 180; i++)
- {
+ for (int i = 0; i < 180; i++) {
double conv = 0.0;
unsigned int half;
- half = (me->getMaskSize() - 1) >> 1 ;
+ half = (me->getMaskSize() - 1) >> 1;
- if(outOfImage( iP , (int)half + me->getStrip() , Iheight, Iwidth))
- {
- conv = 0.0 ;
- }
- else
- {
+ if (outOfImage(iP, (int)half + me->getStrip(), Iheight, Iwidth)) {
+ conv = 0.0;
+ } else {
int index_mask;
- if (me->getAngleStep() !=0)
- index_mask = (int)(i/(double)me->getAngleStep());
+ if (me->getAngleStep() != 0)
+ index_mask = (int)(i / (double)me->getAngleStep());
else
- throw (vpException(vpException::divideByZeroError,"angle step = 0"));
-
- unsigned int ihalf = (unsigned int)(iP.get_i()-half) ;
- unsigned int jhalf = (unsigned int)(iP.get_j()-half) ;
- unsigned int a ;
- unsigned int b ;
- for( a = 0 ; a < me->getMaskSize() ; a++ )
- {
- unsigned int ihalfa = ihalf+a ;
- for( b = 0 ; b < me->getMaskSize() ; b++ )
- {
- conv += me->getMask()[index_mask][a][b] *
- I(ihalfa,jhalf+b) ;
+ throw(vpException(vpException::divideByZeroError, "angle step = 0"));
+
+ unsigned int ihalf = (unsigned int)(iP.get_i() - half);
+ unsigned int jhalf = (unsigned int)(iP.get_j() - half);
+ unsigned int a;
+ unsigned int b;
+ for (a = 0; a < me->getMaskSize(); a++) {
+ unsigned int ihalfa = ihalf + a;
+ for (b = 0; b < me->getMaskSize(); b++) {
+ conv += me->getMask()[index_mask][a][b] * I(ihalfa, jhalf + b);
}
}
}
conv = fabs(conv);
- if (conv > convlt)
- {
+ if (conv > convlt) {
convlt = conv;
angle = vpMath::rad(i);
- angle += M_PI/2;
- while (angle > M_PI) { angle -= M_PI ; }
- while (angle < 0) { angle += M_PI ; }
+ angle += M_PI / 2;
+ while (angle > M_PI) {
+ angle -= M_PI;
+ }
+ while (angle < 0) {
+ angle += M_PI;
+ }
}
}
}
-//Find the point belonging to the edge of the sub image which respects the following hypotheses:
+// Find the point belonging to the edge of the sub image which respects the
+// following hypotheses:
//- the value of the pixel is upper than zero.
//- the distantce between the point and iP is less than 4 pixels.
-//The function returns the nearest point of iP which respect the hypotheses
-//If no point is found the returned point is (-1,-1)
-vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePoint &iP)
+// The function returns the nearest point of iP which respect the hypotheses
+// If no point is found the returned point is (-1,-1)
+vpImagePoint findFirstBorder(const vpImage<unsigned char> &Isub, const vpImagePoint &iP)
{
double dist = 1e6;
double dist_1 = 1e6;
- vpImagePoint index(-1,-1);
- for (unsigned int i = 0; i <= Isub.getHeight(); i++)
- {
- for (unsigned int j = 0; j <= Isub.getWidth(); j++)
- {
- if(i == 0 || i == Isub.getHeight()-1 || j == 0 || j == Isub.getWidth()-1)
- {
- if (Isub(i,j) > 0)
- {
- dist = vpImagePoint::sqrDistance(vpImagePoint(iP),vpImagePoint(i,j));
- if (dist <= 16 && dist < dist_1)
- {
+ vpImagePoint index(-1, -1);
+ for (unsigned int i = 0; i <= Isub.getHeight(); i++) {
+ for (unsigned int j = 0; j <= Isub.getWidth(); j++) {
+ if (i == 0 || i == Isub.getHeight() - 1 || j == 0 || j == Isub.getWidth() - 1) {
+ if (Isub(i, j) > 0) {
+ dist = vpImagePoint::sqrDistance(vpImagePoint(iP), vpImagePoint(i, j));
+ if (dist <= 16 && dist < dist_1) {
dist_1 = dist;
- index.set_ij(i,j);
+ index.set_ij(i, j);
}
}
}
@@ -184,17 +176,15 @@ vpImagePoint findFirstBorder(const vpImage<unsigned char>& Isub, const vpImagePo
}
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
-//from the center of the sub image (ie the point (15,15).
+// Check if the list of vpImagePoint contains a distant point of less tha 4
+// pixels from the center of the sub image (ie the point (15,15).
vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
{
ip_edges_list->front();
- while (!ip_edges_list->outside())
- {
+ while (!ip_edges_list->outside()) {
vpImagePoint iP = ip_edges_list->value();
- double dist = vpImagePoint::sqrDistance(iP,vpImagePoint(15,15));
- if (dist <= 16)
- {
+ double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
+ if (dist <= 16) {
return true;
}
ip_edges_list->next();
@@ -203,15 +193,14 @@ vp_deprecated bool findCenterPoint(vpList<vpImagePoint> *ip_edges_list)
}
#endif
-//Check if the list of vpImagePoint contains a distant point of less tha 4 pixels
-//from the center of the sub image (ie the point (15,15).
+// Check if the list of vpImagePoint contains a distant point of less tha 4
+// pixels from the center of the sub image (ie the point (15,15).
bool findCenterPoint(std::list<vpImagePoint> *ip_edges_list)
{
- for(std::list<vpImagePoint>::const_iterator it=ip_edges_list->begin(); it!=ip_edges_list->end(); ++it){
+ for (std::list<vpImagePoint>::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)
- {
+ double dist = vpImagePoint::sqrDistance(iP, vpImagePoint(15, 15));
+ if (dist <= 16) {
return true;
}
}
@@ -224,8 +213,8 @@ bool findCenterPoint(std::list<vpImagePoint> *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.)
{
}
@@ -233,11 +222,9 @@ vpMeNurbs::vpMeNurbs()
Copy constructor.
*/
vpMeNurbs::vpMeNurbs(const vpMeNurbs &menurbs)
- : vpMeTracker(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.)
{
- nurbs = menurbs.nurbs;
dist = menurbs.dist;
nbControlPoints = menurbs.nbControlPoints;
beginPtFound = menurbs.beginPtFound;
@@ -250,10 +237,7 @@ vpMeNurbs::vpMeNurbs(const vpMeNurbs &menurbs)
/*!
Basic destructor.
*/
-vpMeNurbs::~vpMeNurbs()
-{
-}
-
+vpMeNurbs::~vpMeNurbs() {}
/*!
Initialization of the tracking. Ask the user to click left on several points
@@ -261,28 +245,26 @@ vpMeNurbs::~vpMeNurbs()
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::initTracking(const vpImage<unsigned char> &I)
+void vpMeNurbs::initTracking(const vpImage<unsigned char> &I)
{
std::list<vpImagePoint> ptList;
vpImagePoint pt;
vpMouseButton::vpMouseButtonType b;
- while (vpDisplay::getClick(I, pt, b))
- {
- if (b == vpMouseButton::button1)
- {
- //std::cout<<pt<<std::endl;
+ while (vpDisplay::getClick(I, pt, b)) {
+ if (b == vpMouseButton::button1) {
+ // std::cout<<pt<<std::endl;
ptList.push_back(pt);
- vpDisplay::displayCross(I,pt,10,vpColor::green);
+ vpDisplay::displayCross(I, pt, 10, vpColor::green);
vpDisplay::flush(I);
}
- if (b == vpMouseButton::button3) break;
+ if (b == vpMouseButton::button3)
+ break;
}
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"));
}
/*!
@@ -292,15 +274,13 @@ vpMeNurbs::initTracking(const vpImage<unsigned char> &I)
\param I : Image in which the edge appears.
\param ptList : List of point to initialize the Nurbs.
*/
-void
-vpMeNurbs::initTracking(const vpImage<unsigned char> &I,
- const std::list<vpImagePoint> &ptList)
+void vpMeNurbs::initTracking(const vpImage<unsigned char> &I, const std::list<vpImagePoint> &ptList)
{
nurbs.globalCurveInterp(ptList);
sample(I);
- vpMeTracker::initTracking(I) ;
+ vpMeTracker::initTracking(I);
track(I);
}
@@ -310,11 +290,10 @@ vpMeNurbs::initTracking(const vpImage<unsigned char> &I,
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::sample(const vpImage<unsigned char>& I)
+void vpMeNurbs::sample(const vpImage<unsigned char> &I)
{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
double step = 1.0 / (double)me->getPointsToTrack();
// Delete old list
@@ -323,344 +302,331 @@ vpMeNurbs::sample(const vpImage<unsigned char>& I)
vpImagePoint ip;
double u = 0.0;
vpImagePoint *pt = NULL;
- vpImagePoint pt_1(-rows,-cols);
- while (u <= 1.0)
- {
- if (pt!=NULL) delete[] pt;
+ vpImagePoint pt_1(-rows, -cols);
+ while (u <= 1.0) {
+ if (pt != NULL)
+ delete[] pt;
pt = nurbs.computeCurveDersPoint(u, 1);
- double delta = computeDelta(pt[1].get_i(),pt[1].get_j());
+ double delta = computeDelta(pt[1].get_i(), pt[1].get_j());
// If point is in the image, add to the sample list
- if(!outOfImage(pt[0], 0, rows, cols) && vpImagePoint::sqrDistance(pt[0],pt_1) >= vpMath::sqr(me->getSampleStep()))
- {
- vpMeSite pix ; //= list.value();
- pix.init(pt[0].get_i(), pt[0].get_j(), delta) ;
- pix.setDisplay(selectDisplay) ;
+ if (!outOfImage(pt[0], 0, rows, cols) &&
+ vpImagePoint::sqrDistance(pt[0], pt_1) >= vpMath::sqr(me->getSampleStep())) {
+ vpMeSite pix; //= list.value();
+ pix.init(pt[0].get_i(), pt[0].get_j(), delta);
+ pix.setDisplay(selectDisplay);
list.push_back(pix);
pt_1 = pt[0];
}
- u = u+step;
+ u = u + step;
}
- if (pt!=NULL) delete[] pt;
+ if (pt != NULL)
+ delete[] pt;
}
-
/*!
Suppression of the points which:
-
+
- belong no more to the edge.
- which are to closed to another point.
*/
-void
-vpMeNurbs::suppressPoints()
+void vpMeNurbs::suppressPoints()
{
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ){
- vpMeSite s = *it;//current reference pixel
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();) {
+ vpMeSite s = *it; // current reference pixel
- if (s.getState() != vpMeSite::NO_SUPPRESSION)
- {
+ if (s.getState() != vpMeSite::NO_SUPPRESSION) {
it = list.erase(it);
- }
- else
+ } else
++it;
}
}
-
/*!
Set the alpha value (normal to the edge at this point)
of the different vpMeSite to a value computed thanks to the nurbs.
*/
-void
-vpMeNurbs::updateDelta()
+void vpMeNurbs::updateDelta()
{
double u = 0.0;
double d = 1e6;
double d_1 = 1e6;
- std::list<vpMeSite>::iterator it=list.begin();
-
+ std::list<vpMeSite>::iterator it = list.begin();
+
vpImagePoint Cu;
- vpImagePoint* der = NULL;
+ vpImagePoint *der = NULL;
double step = 0.01;
- while (u < 1 && it!=list.end())
- {
+ while (u < 1 && it != list.end()) {
vpMeSite s = *it;
- vpImagePoint pt(s.i,s.j);
- while (d <= d_1 && u<1)
- {
+ vpImagePoint pt(s.i, s.j);
+ while (d <= d_1 && u < 1) {
Cu = nurbs.computeCurvePoint(u);
- d_1=d;
- d = vpImagePoint::distance(pt,Cu);
- u +=step;
+ d_1 = d;
+ d = vpImagePoint::distance(pt, Cu);
+ u += step;
}
-
- u-=step;
- if (der != NULL) delete[] der;
+
+ u -= step;
+ if (der != NULL)
+ delete[] der;
der = nurbs.computeCurveDersPoint(u, 1);
- //vpImagePoint toto(der[0].get_i(),der[0].get_j());
- //vpDisplay::displayCross(I,toto,4,vpColor::red);
-
- s.alpha = computeDelta(der[1].get_i(),der[1].get_j());
+ // vpImagePoint toto(der[0].get_i(),der[0].get_j());
+ // vpDisplay::displayCross(I,toto,4,vpColor::red);
+
+ s.alpha = computeDelta(der[1].get_i(), der[1].get_j());
*it = s;
++it;
d = 1e6;
d_1 = 1.5e6;
}
- if (der != NULL) delete[] der;
+ if (der != NULL)
+ delete[] der;
}
-
-/*!
+/*!
Seek along the edge defined by the nurbs, the two extremities of
the edge. This function is useful in case of translation of the
edge.
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::seekExtremities(const vpImage<unsigned char> &I)
+void vpMeNurbs::seekExtremities(const vpImage<unsigned char> &I)
{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
- vpImagePoint* begin = NULL;
- vpImagePoint* end = NULL;
+ vpImagePoint *begin = NULL;
+ vpImagePoint *end = NULL;
- begin = nurbs.computeCurveDersPoint(0.0,1);
- end = nurbs.computeCurveDersPoint(1.0,1);
+ begin = nurbs.computeCurveDersPoint(0.0, 1);
+ end = nurbs.computeCurveDersPoint(1.0, 1);
- //Check if the two extremities are not to close to eachother.
- double d = vpImagePoint::distance(begin[0],end[0]);
- double threshold = 3*me->getSampleStep();
+ // Check if the two extremities are not to close to eachother.
+ double d = vpImagePoint::distance(begin[0], end[0]);
+ double threshold = 3 * me->getSampleStep();
double sample_step = me->getSampleStep();
vpImagePoint pt;
- if ( d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/)
- {
- vpMeSite P ;
-
- //Init vpMeSite
- P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign) ;
- P.setDisplay(selectDisplay) ;
-
- //Set the range
- unsigned int memory_range = me->getRange() ;
+ if (d > threshold /*|| (list.firstValue()).mask_sign != (list.lastValue()).mask_sign*/) {
+ vpMeSite P;
+
+ // Init vpMeSite
+ P.init(begin[0].get_i(), begin[0].get_j(), (list.front()).alpha, 0, (list.front()).mask_sign);
+ P.setDisplay(selectDisplay);
+
+ // Set the range
+ unsigned int memory_range = me->getRange();
me->setRange(2);
-
- //Point at the beginning of the list
+
+ // Point at the beginning of the list
bool beginPtAdded = false;
vpImagePoint pt_max = begin[0];
- double angle = atan2(begin[1].get_i(),begin[1].get_j());
+ double angle = atan2(begin[1].get_i(), begin[1].get_j());
double co = vpMath::abs(cos(angle));
co = co * vpMath::sign(begin[1].get_j());
double si = vpMath::abs(sin(angle));
si = si * vpMath::sign(begin[1].get_i());
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat - si*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat - co*sample_step ; P.j = (int)P.jfloat ;
- pt.set_ij(P.ifloat,P.jfloat);
- if (vpImagePoint::distance(end[0],pt) < threshold) break;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false);
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_front(P) ;
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat - si * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat - co * sample_step;
+ P.j = (int)P.jfloat;
+ pt.set_ij(P.ifloat, P.jfloat);
+ if (vpImagePoint::distance(end[0], pt) < threshold)
+ break;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
+ list.push_front(P);
beginPtAdded = true;
pt_max = pt;
if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
+ vpDisplay::displayCross(I, pt, 5, vpColor::blue);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, pt, 10, vpColor::blue);
}
}
}
}
-
- if (!beginPtAdded) beginPtFound++;
+
+ if (!beginPtAdded)
+ beginPtFound++;
P.init(end[0].get_i(), end[0].get_j(), (list.back()).alpha, 0, (list.back()).mask_sign);
P.setDisplay(selectDisplay);
-
+
bool endPtAdded = false;
- angle = atan2(end[1].get_i(),end[1].get_j());
+ angle = atan2(end[1].get_i(), end[1].get_j());
co = vpMath::abs(cos(angle));
co = co * vpMath::sign(end[1].get_j());
si = vpMath::abs(sin(angle));
si = si * vpMath::sign(end[1].get_i());
- for (int i=0 ; i < 3 ; i++)
- {
- P.ifloat = P.ifloat + si*sample_step ; P.i = (int)P.ifloat ;
- P.jfloat = P.jfloat + co*sample_step ; P.j = (int)P.jfloat ;
- pt.set_ij(P.ifloat,P.jfloat);
- if (vpImagePoint::distance(begin[0],pt) < threshold) break;
- if(!outOfImage(P.i, P.j, 5, rows, cols))
- {
- P.track(I,me,false);
-
- if (P.getState() == vpMeSite::NO_SUPPRESSION)
- {
- list.push_back(P) ;
+ for (int i = 0; i < 3; i++) {
+ P.ifloat = P.ifloat + si * sample_step;
+ P.i = (int)P.ifloat;
+ P.jfloat = P.jfloat + co * sample_step;
+ P.j = (int)P.jfloat;
+ pt.set_ij(P.ifloat, P.jfloat);
+ if (vpImagePoint::distance(begin[0], pt) < threshold)
+ break;
+ if (!outOfImage(P.i, P.j, 5, rows, cols)) {
+ P.track(I, me, false);
+
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
+ list.push_back(P);
endPtAdded = true;
if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 5, vpColor::blue) ;
+ vpDisplay::displayCross(I, pt, 5, vpColor::blue);
}
- }
- else {
+ } else {
if (vpDEBUG_ENABLE(3)) {
- vpDisplay::displayCross(I, pt, 10, vpColor::blue) ;
+ vpDisplay::displayCross(I, pt, 10, vpColor::blue);
}
}
}
}
- if (!endPtAdded) endPtFound++;
+ if (!endPtAdded)
+ endPtFound++;
me->setRange(memory_range);
- }
- else
- {
+ } else {
list.pop_front();
}
/*if(begin != NULL)*/ delete[] begin;
/*if(end != NULL) */ delete[] end;
}
-
-/*!
+/*!
Seek the extremities of the edge thanks to a canny edge detection.
The edge detection enable to find the points belonging to the edge.
The any vpMesite are initialize at this points.
-
+
This method is practicle when the edge is not smooth.
-
+
\note To use the canny detection, OpenCV has to be installed.
\param I : Image in which the edge appears.
*/
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
-void
-vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> &I)
+void vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> &I)
#else
-void
-vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
+void vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
#endif
{
#if (defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION < 0x030000))
vpMeSite pt = list.front();
- vpImagePoint firstPoint(pt.ifloat,pt.jfloat);
+ vpImagePoint firstPoint(pt.ifloat, pt.jfloat);
pt = list.back();
- vpImagePoint lastPoint(pt.ifloat,pt.jfloat);
- if (beginPtFound >=3 && farFromImageEdge(I, firstPoint))
- {
+ vpImagePoint lastPoint(pt.ifloat, pt.jfloat);
+ if (beginPtFound >= 3 && farFromImageEdge(I, firstPoint)) {
vpImagePoint *begin = NULL;
- begin = nurbs.computeCurveDersPoint(0.0,1);
- vpImage<unsigned char> Isub(32,32); //Sub image.
- vpImagePoint topLeft(begin[0].get_i()-15,begin[0].get_j()-15);
- vpRect rect(topLeft,32,32);
-
- vpDisplay::displayRectangle(I,rect,vpColor::green);
-
- vpImageTools::crop(I,rect,Isub);
-
+ begin = nurbs.computeCurveDersPoint(0.0, 1);
+ vpImage<unsigned char> Isub(32, 32); // Sub image.
+ vpImagePoint topLeft(begin[0].get_i() - 15, begin[0].get_j() - 15);
+ vpRect rect(topLeft, 32, 32);
+
+ vpDisplay::displayRectangle(I, rect, vpColor::green);
+
+ vpImageTools::crop(I, rect, Isub);
+
vpImagePoint lastPtInSubIm(begin[0]);
double u = 0.0;
- double step =0.0001;
- //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
- while (inRectangle(lastPtInSubIm,rect) && u < 1)
- {
+ double step = 0.0001;
+ // Find the point of the nurbs closest from the edge of the subImage and
+ // in the subImage.
+ while (inRectangle(lastPtInSubIm, rect) && u < 1) {
u += step;
lastPtInSubIm = nurbs.computeCurvePoint(u);
}
u -= step;
- if( u > 0)
+ if (u > 0)
lastPtInSubIm = nurbs.computeCurvePoint(u);
-
+
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(Isub, Ip);
-
- IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
- cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
-
+
+ IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
+ cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
+
vpImageConvert::convert(dst, Isub);
#endif
-
- vpImagePoint firstBorder(-1,-1);
-
- firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
-
+
+ vpImagePoint firstBorder(-1, -1);
+
+ firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
+
std::list<vpImagePoint> ip_edges_list;
- if (firstBorder != vpImagePoint(-1, -1))
- {
+ if (firstBorder != vpImagePoint(-1, -1)) {
unsigned int dir;
double fi = firstBorder.get_i();
double fj = firstBorder.get_j();
- double w = Isub.getWidth()-1;
- double h = Isub.getHeight()-1;
- //if (firstBorder.get_i() == 0) dir = 4;
- if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
- //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
- else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
- //else if (firstBorder.get_j() == 0) dir = 2;
- else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
- //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
- else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
- computeFreemanChainElement(Isub, firstBorder , dir);
+ double w = Isub.getWidth() - 1;
+ double h = Isub.getHeight() - 1;
+ // if (firstBorder.get_i() == 0) dir = 4;
+ if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
+ dir = 4;
+ // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
+ else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
+ dir = 0;
+ // else if (firstBorder.get_j() == 0) dir = 2;
+ else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
+ dir = 2;
+ // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
+ else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
+ dir = 6;
+ computeFreemanChainElement(Isub, firstBorder, dir);
unsigned int firstDir = dir;
- ip_edges_list.push_back( firstBorder );
+ ip_edges_list.push_back(firstBorder);
vpImagePoint border(firstBorder);
vpImagePoint dBorder;
- do
- {
+ do {
computeFreemanParameters(dir, dBorder);
border = border + dBorder;
- vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
+ vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
- ip_edges_list.push_back( border );
+ ip_edges_list.push_back(border);
- computeFreemanChainElement(Isub, border , dir);
- } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
+ computeFreemanChainElement(Isub, border, dir);
+ } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
}
-
- if (findCenterPoint(&ip_edges_list))
- {
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); /*++it*/){
+
+ if (findCenterPoint(&ip_edges_list)) {
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end();
+ /*++it*/) {
vpMeSite s = *it;
- vpImagePoint iP(s.ifloat,s.jfloat);
- if (inRectangle(iP,rect))
- it = list.erase(it) ;
+ vpImagePoint iP(s.ifloat, s.jfloat);
+ if (inRectangle(iP, rect))
+ it = list.erase(it);
else
break;
}
- std::list<vpMeSite>::iterator itList=list.begin();
+ std::list<vpMeSite>::iterator itList = list.begin();
double convlt;
double delta = 0;
int nbr = 0;
std::list<vpMeSite> addedPt;
- for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
+ for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
+ ++itEdges) {
vpMeSite s = *itList;
vpImagePoint iPtemp = *itEdges + topLeft;
vpMeSite pix;
pix.init(iPtemp.get_i(), iPtemp.get_j(), delta);
- dist = vpMeSite::sqrDistance(s,pix);
- if (dist >= vpMath::sqr(me->getSampleStep())/*25*/)
- {
+ dist = vpMeSite::sqrDistance(s, pix);
+ if (dist >= vpMath::sqr(me->getSampleStep()) /*25*/) {
bool exist = false;
- for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
+ for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
dist = vpMeSite::sqrDistance(pix, *itAdd);
- if (dist < vpMath::sqr(me->getSampleStep())/*25*/)
+ if (dist < vpMath::sqr(me->getSampleStep()) /*25*/)
exist = true;
}
- if (!exist)
- {
+ if (!exist) {
findAngle(I, iPtemp, me, delta, convlt);
pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
pix.setDisplay(selectDisplay);
@@ -673,115 +639,111 @@ vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
}
}
- unsigned int memory_range = me->getRange();
+ unsigned int memory_range = me->getRange();
me->setRange(3);
- std::list<vpMeSite>::iterator itList2=list.begin();
- for (int j = 0; j < nbr; j++)
- {
+ std::list<vpMeSite>::iterator itList2 = list.begin();
+ for (int j = 0; j < nbr; j++) {
vpMeSite s = *itList2;
- s.track(I,me,false);
+ s.track(I, me, false);
*itList2 = s;
++itList2;
}
me->setRange(memory_range);
}
-
+
/* if (begin != NULL) */ delete[] begin;
beginPtFound = 0;
}
- if(endPtFound >= 3 && farFromImageEdge(I, lastPoint))
- {
+ if (endPtFound >= 3 && farFromImageEdge(I, lastPoint)) {
vpImagePoint *end = NULL;
- end = nurbs.computeCurveDersPoint(1.0,1);
-
- vpImage<unsigned char> Isub(32,32); //Sub image.
- vpImagePoint topLeft(end[0].get_i()-15,end[0].get_j()-15);
- vpRect rect(topLeft,32,32);
-
- vpDisplay::displayRectangle(I,rect,vpColor::green);
-
- vpImageTools::crop(I,rect,Isub);
-
+ end = nurbs.computeCurveDersPoint(1.0, 1);
+
+ vpImage<unsigned char> Isub(32, 32); // Sub image.
+ vpImagePoint topLeft(end[0].get_i() - 15, end[0].get_j() - 15);
+ vpRect rect(topLeft, 32, 32);
+
+ vpDisplay::displayRectangle(I, rect, vpColor::green);
+
+ vpImageTools::crop(I, rect, Isub);
+
vpImagePoint lastPtInSubIm(end[0]);
double u = 1.0;
- double step =0.0001;
- //Find the point of the nurbs closest from the edge of the subImage and in the subImage.
- while (inRectangle(lastPtInSubIm,rect) && u > 0)
- {
+ double step = 0.0001;
+ // Find the point of the nurbs closest from the edge of the subImage and
+ // in the subImage.
+ while (inRectangle(lastPtInSubIm, rect) && u > 0) {
u -= step;
lastPtInSubIm = nurbs.computeCurvePoint(u);
}
u += step;
- if( u < 1.0)
+ if (u < 1.0)
lastPtInSubIm = nurbs.computeCurvePoint(u);
-
+
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
vpImageFilter::canny(Isub, Isub, 3, cannyTh1, 3);
#else
- IplImage* Ip = NULL;
+ IplImage *Ip = NULL;
vpImageConvert::convert(Isub, Ip);
- IplImage* dst = cvCreateImage( cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1 );
- cvCanny( Ip, dst, cannyTh1, cannyTh2, 3 );
+ IplImage *dst = cvCreateImage(cvSize((int)Isub.getWidth(), (int)Isub.getHeight()), 8, 1);
+ cvCanny(Ip, dst, cannyTh1, cannyTh2, 3);
vpImageConvert::convert(dst, Isub);
#endif
-
- vpImagePoint firstBorder(-1,-1);
-
- firstBorder = findFirstBorder(Isub, lastPtInSubIm-topLeft);
-
+
+ vpImagePoint firstBorder(-1, -1);
+
+ firstBorder = findFirstBorder(Isub, lastPtInSubIm - topLeft);
+
std::list<vpImagePoint> ip_edges_list;
- if (firstBorder != vpImagePoint(-1, -1))
- {
+ if (firstBorder != vpImagePoint(-1, -1)) {
unsigned int dir;
double fi = firstBorder.get_i();
double fj = firstBorder.get_j();
- double w = Isub.getWidth()-1;
- double h = Isub.getHeight()-1;
- //if (firstBorder.get_i() == 0) dir = 4;
- if (std::fabs(fi) <= std::numeric_limits<double>::epsilon()) dir = 4;
- //else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
- else if (std::fabs(fi-h) <= std::fabs(vpMath::maximum(fi,h))*std::numeric_limits<double>::epsilon()) dir = 0;
- //else if (firstBorder.get_j() == 0) dir = 2;
- else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon()) dir = 2;
- //else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
- else if (std::fabs(fj-w) <= std::fabs(vpMath::maximum(fj,w))*std::numeric_limits<double>::epsilon()) dir = 6;
-
-
- computeFreemanChainElement(Isub, firstBorder , dir);
+ double w = Isub.getWidth() - 1;
+ double h = Isub.getHeight() - 1;
+ // if (firstBorder.get_i() == 0) dir = 4;
+ if (std::fabs(fi) <= std::numeric_limits<double>::epsilon())
+ dir = 4;
+ // else if (firstBorder.get_i() == Isub.getHeight()-1) dir = 0;
+ else if (std::fabs(fi - h) <= std::fabs(vpMath::maximum(fi, h)) * std::numeric_limits<double>::epsilon())
+ dir = 0;
+ // else if (firstBorder.get_j() == 0) dir = 2;
+ else if (std::fabs(fj) <= std::numeric_limits<double>::epsilon())
+ dir = 2;
+ // else if (firstBorder.get_j() == Isub.getWidth()-1) dir = 6;
+ else if (std::fabs(fj - w) <= std::fabs(vpMath::maximum(fj, w)) * std::numeric_limits<double>::epsilon())
+ dir = 6;
+
+ computeFreemanChainElement(Isub, firstBorder, dir);
unsigned int firstDir = dir;
- ip_edges_list.push_back( firstBorder );
+ ip_edges_list.push_back(firstBorder);
vpImagePoint border(firstBorder);
vpImagePoint dBorder;
- do
- {
+ do {
computeFreemanParameters(dir, dBorder);
border = border + dBorder;
- vpDisplay::displayPoint(I, border+topLeft, vpColor::orange) ;
+ vpDisplay::displayPoint(I, border + topLeft, vpColor::orange);
- ip_edges_list.push_back( border );
+ ip_edges_list.push_back(border);
- computeFreemanChainElement(Isub, border , dir);
- } while( (border != firstBorder || dir != firstDir) && isInImage(Isub,border) );
+ computeFreemanChainElement(Isub, border, dir);
+ } while ((border != firstBorder || dir != firstDir) && isInImage(Isub, border));
}
-
- if (findCenterPoint(&ip_edges_list))
- {
-// list.end();
+
+ if (findCenterPoint(&ip_edges_list)) {
+ // list.end();
vpMeSite s;
- while(true)//{//!list.outside())
+ while (true) //{//!list.outside())
{
- s = list.back();//list.value() ;
- vpImagePoint iP(s.ifloat,s.jfloat);
- if (inRectangle(iP,rect))
- {
- list.erase(list.end()) ;
-// list.end();
- }
- else
+ s = list.back(); // list.value() ;
+ vpImagePoint iP(s.ifloat, s.jfloat);
+ if (inRectangle(iP, rect)) {
+ list.erase(list.end());
+ // list.end();
+ } else
break;
}
@@ -791,22 +753,21 @@ vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
double delta;
int nbr = 0;
std::list<vpMeSite> addedPt;
- for(std::list<vpImagePoint>::const_iterator itEdges=ip_edges_list.begin(); itEdges!=ip_edges_list.end(); ++itEdges){
+ for (std::list<vpImagePoint>::const_iterator itEdges = ip_edges_list.begin(); itEdges != ip_edges_list.end();
+ ++itEdges) {
s = *itList;
vpImagePoint iPtemp = *itEdges + topLeft;
vpMeSite pix;
pix.init(iPtemp.get_i(), iPtemp.get_j(), 0);
- dist = vpMeSite::sqrDistance(s,pix);
- if (dist >= vpMath::sqr(me->getSampleStep()))
- {
+ dist = vpMeSite::sqrDistance(s, pix);
+ if (dist >= vpMath::sqr(me->getSampleStep())) {
bool exist = false;
- for(std::list<vpMeSite>::const_iterator itAdd=addedPt.begin(); itAdd!=addedPt.end(); ++itAdd){
+ for (std::list<vpMeSite>::const_iterator itAdd = addedPt.begin(); itAdd != addedPt.end(); ++itAdd) {
dist = vpMeSite::sqrDistance(pix, *itAdd);
if (dist < vpMath::sqr(me->getSampleStep()))
exist = true;
}
- if (!exist)
- {
+ if (!exist) {
findAngle(I, iPtemp, me, delta, convlt);
pix.init(iPtemp.get_i(), iPtemp.get_j(), delta, convlt);
pix.setDisplay(selectDisplay);
@@ -816,21 +777,20 @@ vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
}
}
}
-
- unsigned int memory_range = me->getRange();
+
+ unsigned int memory_range = me->getRange();
me->setRange(3);
std::list<vpMeSite>::iterator itList2 = list.end();
--itList2; // Move to the last element
- for (int j = 0; j < nbr; j++)
- {
+ for (int j = 0; j < nbr; j++) {
vpMeSite me_s = *itList2;
- me_s.track(I,me,false);
+ me_s.track(I, me, false);
*itList2 = me_s;
--itList2;
}
me->setRange(memory_range);
}
-
+
/* if (end != NULL) */ delete[] end;
endPtFound = 0;
}
@@ -839,114 +799,102 @@ vpMeNurbs::seekExtremitiesCanny(const vpImage<unsigned char> & /* I */)
#endif
}
-
/*!
Resample the edge if the number of sample is less than 70% of the
expected value.
-
+
\note The expected value is computed thanks to the length of the
nurbs and the parameter which indicates the number of pixel between
two points (vpMe::sample_step).
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::reSample(const vpImage<unsigned char> &I)
+void vpMeNurbs::reSample(const vpImage<unsigned char> &I)
{
unsigned int n = numberOfSignal();
double nbPt = floor(dist / me->getSampleStep());
- if ((double)n < 0.7*nbPt)
- {
+ if ((double)n < 0.7 * nbPt) {
sample(I);
vpMeTracker::initTracking(I);
}
}
-
/*!
Resample a part of the edge if two vpMeSite are too far from eachother.
- In this case the method try to initialize any vpMeSite between the two points.
+ In this case the method try to initialize any vpMeSite between the two
+ points.
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::localReSample(const vpImage<unsigned char> &I)
+void vpMeNurbs::localReSample(const vpImage<unsigned char> &I)
{
- int rows = (int)I.getHeight() ;
- int cols = (int)I.getWidth() ;
- vpImagePoint* iP = NULL;
-
+ int rows = (int)I.getHeight();
+ int cols = (int)I.getWidth();
+ vpImagePoint *iP = NULL;
+
int n = (int)numberOfSignal();
-
-// list.front();
- std::list<vpMeSite>::iterator it=list.begin();
- std::list<vpMeSite>::iterator itNext=list.begin();
+
+ // list.front();
+ std::list<vpMeSite>::iterator it = list.begin();
+ std::list<vpMeSite>::iterator itNext = list.begin();
++itNext;
unsigned int range_tmp = me->getRange();
me->setRange(2);
- while(itNext!=list.end() && n <= me->getPointsToTrack())
- {
- vpMeSite s = *it;//current reference pixel
- vpMeSite s_next = *itNext;//current reference pixel
-
- double d = vpMeSite::sqrDistance(s,s_next);
- if(d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600)
- {
- vpImagePoint iP0(s.ifloat,s.jfloat);
- vpImagePoint iPend(s_next.ifloat,s_next.jfloat);
- vpImagePoint iP_1(s.ifloat,s.jfloat);
+ while (itNext != list.end() && n <= me->getPointsToTrack()) {
+ vpMeSite s = *it; // current reference pixel
+ vpMeSite s_next = *itNext; // current reference pixel
+
+ double d = vpMeSite::sqrDistance(s, s_next);
+ if (d > 4 * vpMath::sqr(me->getSampleStep()) && d < 1600) {
+ vpImagePoint iP0(s.ifloat, s.jfloat);
+ vpImagePoint iPend(s_next.ifloat, s_next.jfloat);
+ vpImagePoint iP_1(s.ifloat, s.jfloat);
double u = 0.0;
double ubegin = 0.0;
double uend = 0.0;
double dmin1_1 = 1e6;
double dmin2_1 = 1e6;
- while(u < 1)
- {
- u+=0.01;
- double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iP0);
- double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u),iPend);
+ while (u < 1) {
+ u += 0.01;
+ double dmin1 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iP0);
+ double dmin2 = vpImagePoint::sqrDistance(nurbs.computeCurvePoint(u), iPend);
- if (dmin1 < dmin1_1)
- {
+ if (dmin1 < dmin1_1) {
dmin1_1 = dmin1;
ubegin = u;
}
- if (dmin2 < dmin2_1)
- {
+ if (dmin2 < dmin2_1) {
dmin2_1 = dmin2;
uend = u;
}
}
u = ubegin;
-
- //if(( u != 1.0 || uend != 1.0)
- if( (std::fabs(u-1.0) > std::fabs(vpMath::maximum(u, 1.0))*std::numeric_limits<double>::epsilon())
- || (std::fabs(uend-1.0) > std::fabs(vpMath::maximum(uend, 1.0))*std::numeric_limits<double>::epsilon()))
- {
+
+ // if(( u != 1.0 || uend != 1.0)
+ if ((std::fabs(u - 1.0) > std::fabs(vpMath::maximum(u, 1.0)) * std::numeric_limits<double>::epsilon()) ||
+ (std::fabs(uend - 1.0) > std::fabs(vpMath::maximum(uend, 1.0)) * std::numeric_limits<double>::epsilon())) {
iP = nurbs.computeCurveDersPoint(u, 1);
- while (vpImagePoint::sqrDistance(iP[0],iPend) > vpMath::sqr(me->getSampleStep()) && u < uend)
- {
- u+=0.01;
+ while (vpImagePoint::sqrDistance(iP[0], iPend) > vpMath::sqr(me->getSampleStep()) && u < uend) {
+ u += 0.01;
/*if (iP!=NULL)*/ {
delete[] iP;
iP = NULL;
}
iP = nurbs.computeCurveDersPoint(u, 1);
- if ( vpImagePoint::sqrDistance(iP[0],iP_1) > vpMath::sqr(me->getSampleStep()) && !outOfImage(iP[0], 0, rows, cols))
- {
- double delta = computeDelta(iP[1].get_i(),iP[1].get_j());
- vpMeSite pix ; //= list.value();
- pix.init(iP[0].get_i(), iP[0].get_j(), delta) ;
- pix.setDisplay(selectDisplay) ;
- pix.track(I,me,false);
- if (pix.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (vpImagePoint::sqrDistance(iP[0], iP_1) > vpMath::sqr(me->getSampleStep()) &&
+ !outOfImage(iP[0], 0, rows, cols)) {
+ double delta = computeDelta(iP[1].get_i(), iP[1].get_j());
+ vpMeSite pix; //= list.value();
+ pix.init(iP[0].get_i(), iP[0].get_j(), delta);
+ pix.setDisplay(selectDisplay);
+ pix.track(I, me, false);
+ if (pix.getState() == vpMeSite::NO_SUPPRESSION) {
list.insert(it, pix);
iP_1 = iP[0];
}
@@ -964,14 +912,12 @@ vpMeNurbs::localReSample(const vpImage<unsigned char> &I)
me->setRange(range_tmp);
}
-
/*!
Suppress vpMeSites if they are too close to each other.
-
+
The goal is to keep the order of the vpMeSites in the list.
*/
-void
-vpMeNurbs::supressNearPoints()
+void vpMeNurbs::supressNearPoints()
{
#if 0
// Loop through list of sites to track
@@ -980,11 +926,11 @@ vpMeNurbs::supressNearPoints()
{
vpMeSite s = list.value() ;//current reference pixel
vpMeSite s_next = list.nextValue() ;//current reference pixel
-
+
if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep()))
{
s_next.setState(vpMeSite::TOO_NEAR);
-
+
list.next();
list.modify(s_next);
if (!list.nextOutside()) list.next();
@@ -993,78 +939,74 @@ vpMeNurbs::supressNearPoints()
list.next() ;
}
#endif
- std::list<vpMeSite>::const_iterator it=list.begin();
- std::list<vpMeSite>::iterator itNext=list.begin();
+ std::list<vpMeSite>::const_iterator it = list.begin();
+ std::list<vpMeSite>::iterator itNext = list.begin();
++itNext;
- for(;itNext!=list.end();){
- vpMeSite s = *it;//current reference pixel
- vpMeSite s_next = *itNext;//current reference pixel
+ for (; itNext != list.end();) {
+ vpMeSite s = *it; // current reference pixel
+ vpMeSite s_next = *itNext; // current reference pixel
- if(vpMeSite::sqrDistance(s,s_next) < vpMath::sqr(me->getSampleStep())){
+ if (vpMeSite::sqrDistance(s, s_next) < vpMath::sqr(me->getSampleStep())) {
s_next.setState(vpMeSite::TOO_NEAR);
-
+
*itNext = s_next;
++it;
++itNext;
- if(itNext!=list.end()){
+ if (itNext != list.end()) {
++it;
++itNext;
}
- }
- else{
+ } else {
++it;
++itNext;
}
}
}
-
/*!
Track the edge in the image I.
\param I : Image in which the edge appears.
*/
-void
-vpMeNurbs::track(const vpImage<unsigned char> &I)
+void vpMeNurbs::track(const vpImage<unsigned char> &I)
{
- //Tracking des vpMeSites
+ // Tracking des vpMeSites
vpMeTracker::track(I);
- //Suppress points which are too close to each other
+ // Suppress points which are too close to each other
supressNearPoints();
-
- //Suppressions des points ejectes par le tracking
+
+ // Suppressions des points ejectes par le tracking
suppressPoints();
if (list.size() == 1)
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "Not enough valid me to track"));
- //Recalcule les parametres
-// nurbs.globalCurveInterp(list);
- nurbs.globalCurveApprox(list,nbControlPoints);
-
- //On resample localement
+ // Recalcule les parametres
+ // nurbs.globalCurveInterp(list);
+ nurbs.globalCurveApprox(list, nbControlPoints);
+
+ // On resample localement
localReSample(I);
seekExtremities(I);
- if(enableCannyDetection)
+ if (enableCannyDetection)
seekExtremitiesCanny(I);
-// nurbs.globalCurveInterp(list);
- nurbs.globalCurveApprox(list,nbControlPoints);
-
+ // nurbs.globalCurveInterp(list);
+ nurbs.globalCurveApprox(list, nbControlPoints);
+
double u = 0.0;
vpImagePoint pt;
vpImagePoint pt_1;
dist = 0;
- while (u<=1.0)
- {
+ while (u <= 1.0) {
pt = nurbs.computeCurvePoint(u);
- //if(u!=0)
- if(std::fabs(u) > std::numeric_limits<double>::epsilon())
- dist = dist + vpImagePoint::distance(pt,pt_1);
+ // if(u!=0)
+ if (std::fabs(u) > std::numeric_limits<double>::epsilon())
+ dist = dist + vpImagePoint::distance(pt, pt_1);
pt_1 = pt;
- u=u+0.01;
+ u = u + 0.01;
}
updateDelta();
@@ -1072,7 +1014,6 @@ vpMeNurbs::track(const vpImage<unsigned char> &I)
reSample(I);
}
-
/*!
Display edge.
@@ -1083,12 +1024,7 @@ vpMeNurbs::track(const vpImage<unsigned char> &I)
\param col : Color of the displayed line.
*/
-void
-vpMeNurbs::display(const vpImage<unsigned char>&I, vpColor col)
-{
- vpMeNurbs::display(I,nurbs,col);
-}
-
+void vpMeNurbs::display(const vpImage<unsigned char> &I, vpColor col) { vpMeNurbs::display(I, nurbs, col); }
/*!
Considering a pixel iP compute the next element of the Freeman chain
@@ -1100,95 +1036,82 @@ vpMeNurbs::display(const vpImage<unsigned char>&I, vpColor col)
\param I : The image we are working with.
\param iP : The current pixel.
- \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6, 7)
- with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
+ \param element : The next freeman element chain code (0, 1, 2, 3, 4, 5, 6,
+ 7) with 0 for right moving, 2 for down, 4 for left and 6 for up moving.
\return false if an element cannot be found. Occurs for example with an area
constituted by a single pixel. Return true if success.
*/
-bool
-vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I,
- vpImagePoint &iP,
- unsigned int &element)
+bool vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I, vpImagePoint &iP, unsigned int &element)
{
vpImagePoint diP;
vpImagePoint iPtemp;
- if (hasGoodLevel( I, iP )) {
+ if (hasGoodLevel(I, iP)) {
// get the point on the right of the point passed in
- computeFreemanParameters((element + 2) %8, diP);
+ computeFreemanParameters((element + 2) % 8, diP);
iPtemp = iP + diP;
- if (hasGoodLevel( I, iPtemp )) {
- element = (element + 2) % 8; // turn right
- }
- else {
- computeFreemanParameters((element + 1) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 2) % 8; // turn right
+ } else {
+ computeFreemanParameters((element + 1) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 1) % 8; // turn diag right
- }
- else {
- computeFreemanParameters(element, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 1) % 8; // turn diag right
+ } else {
+ computeFreemanParameters(element, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- //element = element; // keep same dir
- }
- else {
- computeFreemanParameters((element + 7) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ // element = element; // keep same dir
+ } else {
+ computeFreemanParameters((element + 7) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 7) %8; // turn diag left
- }
- else {
- computeFreemanParameters((element + 6) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 7) % 8; // turn diag left
+ } else {
+ computeFreemanParameters((element + 6) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 6) %8 ; // turn left
- }
- else {
- computeFreemanParameters((element + 5) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 6) % 8; // turn left
+ } else {
+ computeFreemanParameters((element + 5) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 5) %8 ; // turn diag down
- }
- else {
- computeFreemanParameters((element + 4) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 5) % 8; // turn diag down
+ } else {
+ computeFreemanParameters((element + 4) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 4) %8 ; // turn down
- }
- else {
- computeFreemanParameters((element + 3) %8, diP);
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 4) % 8; // turn down
+ } else {
+ computeFreemanParameters((element + 3) % 8, diP);
iPtemp = iP + diP;
- if ( hasGoodLevel( I, iPtemp )) {
- element = (element + 3) %8 ; // turn diag right down
- }
- else {
- // No neighbor with a good level
- //
- return false;
- }
- }
- }
- }
- }
- }
+ if (hasGoodLevel(I, iPtemp)) {
+ element = (element + 3) % 8; // turn diag right down
+ } else {
+ // No neighbor with a good level
+ //
+ return false;
+ }
+ }
+ }
+ }
+ }
+ }
}
}
- }
- else {
+ } else {
return false;
}
return true;
}
-
/*!
Check if the pixel iP is in the image and has
a good level to belong to the edge.
@@ -1201,23 +1124,18 @@ vpMeNurbs::computeFreemanChainElement(const vpImage<unsigned char> &I,
\return false : Otherwise
*/
-bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char>& I,
- const vpImagePoint& iP) const
+bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
{
- if( !isInImage( I, iP ) )
+ 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
- {
+ } else {
return false;
}
}
-
/*!
Test if a pixel is in the image. Points of the border are not considered to
be in the image.
@@ -1228,18 +1146,15 @@ bool vpMeNurbs::hasGoodLevel(const vpImage<unsigned char>& I,
\return true if the image point \e iP is in the image and false
otherwise.
*/
-bool vpMeNurbs::isInImage(const vpImage<unsigned char>& I, const vpImagePoint &iP) const
+bool vpMeNurbs::isInImage(const vpImage<unsigned char> &I, const vpImagePoint &iP) const
{
- return (iP.get_i() >= 0
- && iP.get_j() >= 0
- && iP.get_i() < I.getHeight()
- && iP.get_j() < I.getWidth());
+ return (iP.get_i() >= 0 && iP.get_j() >= 0 && iP.get_i() < I.getHeight() && iP.get_j() < I.getWidth());
}
-
/*!
- Gives the displacement corresponding to the value of the parameter \e element.
-
+ Gives the displacement corresponding to the value of the parameter \e
+ element.
+
- If element = 0 diP = (0,1).
- If element = 1 diP = (1,1).
- If element = 2 diP = (1,0).
@@ -1248,75 +1163,71 @@ bool vpMeNurbs::isInImage(const vpImage<unsigned char>& I, const vpImagePoint &i
- If element = 5 diP = (-1,-1).
- If element = 6 diP = (-1,0).
- If element = 7 diP = (-1,1).
-
- \param element : the element value(typically given by the method computeFreemanChainElement).
- \param diP : the output parameter which contains the displacement cooresponding to the value of \e element.
+
+ \param element : the element value(typically given by the method
+ computeFreemanChainElement). \param diP : the output parameter which
+ contains the displacement cooresponding to the value of \e element.
*/
-void
-vpMeNurbs::computeFreemanParameters( unsigned int element, vpImagePoint &diP)
+void vpMeNurbs::computeFreemanParameters(unsigned int element, vpImagePoint &diP)
{
/*
5 6 7
\ | /
- \|/
+ \|/
4 ------- 0
/|\
/ | \
3 2 1
*/
- switch(element) {
+ switch (element) {
case 0: // go right
- diP.set_ij(0,1);
+ diP.set_ij(0, 1);
break;
case 1: // go right top
- diP.set_ij(1,1);
+ diP.set_ij(1, 1);
break;
case 2: // go top
- diP.set_ij(1,0);
+ diP.set_ij(1, 0);
break;
case 3:
- diP.set_ij(1,-1);
+ diP.set_ij(1, -1);
break;
case 4:
- diP.set_ij(0,-1);
+ diP.set_ij(0, -1);
break;
case 5:
- diP.set_ij(-1,-1);
+ diP.set_ij(-1, -1);
break;
case 6:
- diP.set_ij(-1,0);
+ diP.set_ij(-1, 0);
break;
case 7:
- diP.set_ij(-1,1);
+ diP.set_ij(-1, 1);
break;
}
}
-
/*!
Check if the point is far enough from the image edges
-
+
\param I : The image.
\param iP : An image point.
-
- \return true if the point iP is at least 20 pixels far from the image edeges.
+
+ \return true if the point iP is at least 20 pixels far from the image
+ edeges.
*/
-bool
-vpMeNurbs::farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint& iP)
+bool vpMeNurbs::farFromImageEdge(const vpImage<unsigned char> &I, const vpImagePoint &iP)
{
unsigned int height = I.getHeight();
- unsigned int width = I.getWidth();
- return (iP.get_i() < height - 20
- && iP.get_j() < width - 20
- && iP.get_i() > 20
- && iP.get_j() > 20);
+ unsigned int width = I.getWidth();
+ return (iP.get_i() < height - 20 && iP.get_j() < width - 20 && iP.get_i() > 20 && iP.get_j() > 20);
}
/*!
@@ -1329,15 +1240,14 @@ vpMeNurbs::farFromImageEdge(const vpImage<unsigned char>& I, const vpImagePoint&
\param color : Color used to display the nurbs.
*/
-void vpMeNurbs::display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor color)
+void vpMeNurbs::display(const vpImage<unsigned char> &I, vpNurbs &n, vpColor color)
{
double u = 0.0;
vpImagePoint pt;
- while (u <= 1)
- {
+ while (u <= 1) {
pt = n.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,color);
- u+=0.01;
+ vpDisplay::displayCross(I, pt, 4, color);
+ u += 0.01;
}
}
@@ -1351,15 +1261,13 @@ void vpMeNurbs::display(const vpImage<unsigned char>& I, vpNurbs &n, vpColor col
\param color : Color used to display the nurbs.
*/
-void vpMeNurbs::display(const vpImage<vpRGBa>& I, vpNurbs &n, vpColor color)
+void vpMeNurbs::display(const vpImage<vpRGBa> &I, vpNurbs &n, vpColor color)
{
double u = 0.0;
vpImagePoint pt;
- while (u <= 1)
- {
+ while (u <= 1) {
pt = n.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,color);
- u+=0.01;
+ vpDisplay::displayCross(I, pt, 4, color);
+ u += 0.01;
}
}
-
diff --git a/modules/tracker/me/src/moving-edges/vpMeSite.cpp b/modules/tracker/me/src/moving-edges/vpMeSite.cpp
index c9c8a10..276d2f5 100644
--- a/modules/tracker/me/src/moving-edges/vpMeSite.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMeSite.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,89 +43,88 @@
\brief Moving edges
*/
-
-
-#include <visp3/me/vpMeSite.h>
-#include <visp3/me/vpMe.h>
-#include <visp3/core/vpTrackingException.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <stdlib.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <visp3/core/vpTrackingException.h>
+#include <visp3/me/vpMe.h>
#include <visp3/me/vpMeSite.h>
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static
-bool horsImage(int i , int j, int half, int rows, int cols)
+static bool horsImage(int i, int j, int half, int rows, int cols)
{
- //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
+ // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
+ // > (cols - half - 3) )) ;
int half_1 = half + 1;
int half_3 = half + 3;
- //return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j > (cols - half - 3) )) ;
- return( (0 < (half_1 - i) ) || ( (i - rows + half_3) > 0 ) || ( 0 < (half_1 -j) ) || ( (j - cols + half_3) > 0 ) ) ;
+ // return((i < half + 1) || ( i > (rows - half - 3) )||(j < half + 1) || (j
+ // > (cols - half - 3) )) ;
+ return ((0 < (half_1 - i)) || ((i - rows + half_3) > 0) || (0 < (half_1 - j)) || ((j - cols + half_3) > 0));
}
#endif
-void
-vpMeSite::init()
+void vpMeSite::init()
{
// Site components
- alpha = 0.0 ;
- convlt = 0.0 ;
- weight=-1;
+ alpha = 0.0;
+ convlt = 0.0;
+ weight = -1;
- selectDisplay = NONE ;
+ selectDisplay = NONE;
// Pixel components
- i = 0 ;
- j = 0 ;
- v = 0 ;
- ifloat =i ;
- jfloat = j ;
- i_1 = 0 ;
- j_1 = 0 ;
+ i = 0;
+ j = 0;
+ v = 0;
+ ifloat = i;
+ jfloat = j;
+ i_1 = 0;
+ j_1 = 0;
- mask_sign = 1 ;
+ mask_sign = 1;
normGradient = 0;
-
+
state = NO_SUPPRESSION;
-
+
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
suppress = 0;
#endif
}
vpMeSite::vpMeSite()
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+ : 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)
+ ,
+ suppress(0)
#endif
{
}
vpMeSite::vpMeSite(double ip, double jp)
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+ : 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)
+ ,
+ suppress(0)
#endif
{
- i = vpMath::round(ip) ;
- j = vpMath::round(jp) ;
- ifloat = ip ;
- jfloat = jp ;
+ i = vpMath::round(ip);
+ j = vpMath::round(jp);
+ ifloat = ip;
+ jfloat = jp;
}
/*!
Copy constructor.
*/
-vpMeSite::vpMeSite (const vpMeSite &mesite)
- : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.),
- convlt(0.), normGradient(0), weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
+vpMeSite::vpMeSite(const vpMeSite &mesite)
+ : i(0), j(0), i_1(0), j_1(0), ifloat(0), jfloat(0), v(0), mask_sign(1), alpha(0.), convlt(0.), normGradient(0),
+ weight(1), selectDisplay(NONE), state(NO_SUPPRESSION)
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , suppress(0)
+ ,
+ suppress(0)
#endif
{
*this = mesite;
@@ -132,58 +132,54 @@ vpMeSite::vpMeSite (const vpMeSite &mesite)
// More an Update than init
// For points in meter form (to avoid approximations)
-void
-vpMeSite::init(double ip, double jp, double alphap)
+void vpMeSite::init(double ip, double jp, double alphap)
{
// Note: keep old value of convlt, suppress and contrast
- selectDisplay = NONE ;
+ selectDisplay = NONE;
ifloat = ip;
- i= vpMath::round(ip);
- jfloat = jp ;
+ i = vpMath::round(ip);
+ jfloat = jp;
j = vpMath::round(jp);
- alpha = alphap ;
- mask_sign =1 ;
-
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
+ alpha = alphap;
+ mask_sign = 1;
+ v = 0;
+ i_1 = 0;
+ j_1 = 0;
}
// initialise with convolution
-void
-vpMeSite::init(double ip, double jp, double alphap, double convltp)
+void vpMeSite::init(double ip, double jp, double alphap, double convltp)
{
- selectDisplay = NONE ;
- ifloat = ip ;
- i= (int)ip ;
- jfloat = jp ;
- j =(int)jp ;
- alpha = alphap ;
+ selectDisplay = NONE;
+ ifloat = ip;
+ i = (int)ip;
+ jfloat = jp;
+ j = (int)jp;
+ alpha = alphap;
convlt = convltp;
- mask_sign =1 ;
+ mask_sign = 1;
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
+ v = 0;
+ i_1 = 0;
+ j_1 = 0;
}
// initialise with convolution and sign
-void
-vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
+void vpMeSite::init(double ip, double jp, double alphap, double convltp, int sign)
{
- selectDisplay = NONE ;
- ifloat = ip ;
- i= (int)ip ;
- jfloat = jp ;
- j =(int)jp ;
- alpha = alphap ;
+ selectDisplay = NONE;
+ ifloat = ip;
+ i = (int)ip;
+ jfloat = jp;
+ j = (int)jp;
+ alpha = alphap;
convlt = convltp;
- mask_sign = sign ;
+ mask_sign = sign;
- v = 0 ;
- i_1 = 0 ;
- j_1 = 0 ;
+ v = 0;
+ i_1 = 0;
+ j_1 = 0;
}
vpMeSite &vpMeSite::operator=(const vpMeSite &m)
@@ -202,166 +198,159 @@ vpMeSite &vpMeSite::operator=(const vpMeSite &m)
weight = m.weight;
selectDisplay = m.selectDisplay;
state = m.state;
-
+
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
suppress = m.suppress;
#endif
-
- return *this ;
-}
+ return *this;
+}
// ===================================================================
/*!
- * Construct and return the list of vpMeSite along the normal to the contour, in the given range.
- * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set.
- * \param I : Image in which the display is performed.
- * \param range : +/- the range within which the pixel's correspondent will be sought
- * \return Pointer to the list of query sites
+ * Construct and return the list of vpMeSite along the normal to the contour,
+ * in the given range. \pre : ifloat, jfloat, and the direction of the normal
+ * (alpha) have to be set. \param I : Image in which the display is performed.
+ * \param range : +/- the range within which the pixel's correspondent will
+ * be sought \return Pointer to the list of query sites
*/
// ===================================================================
-vpMeSite*
-vpMeSite::getQueryList(const vpImage<unsigned char> &I, const int range)
+vpMeSite *vpMeSite::getQueryList(const vpImage<unsigned char> &I, const int range)
{
- int k ;
+ int k;
int n;
- vpMeSite *list_query_pixels ;
- list_query_pixels = NULL ;
+ vpMeSite *list_query_pixels;
+ list_query_pixels = NULL;
unsigned int range_ = static_cast<unsigned int>(range);
// Size of query list includes the point on the line
- list_query_pixels = new vpMeSite[2 * range_ + 1] ;
+ list_query_pixels = new vpMeSite[2 * range_ + 1];
// range : +/- the range within which the pixel's
- //correspondent will be sought
+ // correspondent will be sought
double salpha = sin(alpha);
double calpha = cos(alpha);
- n = 0 ;
+ n = 0;
vpImagePoint ip;
-
- for(k = -range ; k <= range ; k++)
- {
- double ii = (ifloat+k*salpha);
- double jj = (jfloat+k*calpha);
+
+ for (k = -range; k <= range; k++) {
+ double ii = (ifloat + k * salpha);
+ double jj = (jfloat + k * calpha);
// Display
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RANGE)) {
- ip.set_i( ii );
- ip.set_j( jj );
- vpDisplay::displayCross(I, ip, 1, vpColor::yellow) ;
+ if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RANGE)) {
+ ip.set_i(ii);
+ ip.set_j(jj);
+ vpDisplay::displayCross(I, ip, 1, vpColor::yellow);
}
// Copy parent's convolution
- vpMeSite pel ;
- pel.init(ii, jj, alpha, convlt,mask_sign) ;
- pel.setDisplay(selectDisplay) ;// Display
+ vpMeSite pel;
+ pel.init(ii, jj, alpha, convlt, mask_sign);
+ pel.setDisplay(selectDisplay); // Display
// Add site to the query list
- list_query_pixels[n] = pel ;
- n++ ;
+ list_query_pixels[n] = pel;
+ n++;
}
- return(list_query_pixels) ;
+ return (list_query_pixels);
}
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
// ===================================================================
/*!
- * get the sign (according to the difference of values of the intensities of the extremities).
- * \pre : ifloat, jfloat, and the direction of the normal (alpha) have to be set.
- * \param I : Image in which the sign is computed.
- * \param range : +/- the range within which the pixel's correspondent is sought
- * \post : mask_sign is computed
+ * get the sign (according to the difference of values of the intensities of
+ * the extremities). \pre : ifloat, jfloat, and the direction of the normal
+ * (alpha) have to be set. \param I : Image in which the sign is computed.
+ * \param range : +/- the range within which the pixel's correspondent is
+ * sought \post : mask_sign is computed
*/
// ===================================================================
-void
-vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
+void vpMeSite::getSign(const vpImage<unsigned char> &I, const int range)
{
- int k ;
+ int k;
double salpha = sin(alpha);
double calpha = cos(alpha);
- //First extremity
- k = -range ;
- unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
- unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
+ // First extremity
+ k = -range;
+ unsigned int i1 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
+ unsigned int j1 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
- //Second extremity
- k = range ;
- unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat+k*salpha));
- unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat+k*calpha));
+ // Second extremity
+ k = range;
+ unsigned int i2 = static_cast<unsigned int>(vpMath::round(ifloat + k * salpha));
+ unsigned int j2 = static_cast<unsigned int>(vpMath::round(jfloat + k * calpha));
- // TODO: Here check if i1,j1,i2,j2 > 0 else ??
- if (I[i1][j1] > I[i2][j2]) mask_sign = 1 ; else mask_sign = -1 ;
+ // TODO: Here check if i1,j1,i2,j2 > 0 else ??
+ if (I[i1][j1] > I[i2][j2])
+ mask_sign = 1;
+ else
+ mask_sign = -1;
}
#endif
// Specific function for ME
-double
-vpMeSite::convolution(const vpImage<unsigned char>&I, const vpMe *me)
+double vpMeSite::convolution(const vpImage<unsigned char> &I, const vpMe *me)
{
int half;
int height_ = static_cast<int>(I.getHeight());
- int width_ = static_cast<int>(I.getWidth());
+ int width_ = static_cast<int>(I.getWidth());
- double conv = 0.0 ;
+ double conv = 0.0;
unsigned int msize = me->getMaskSize();
- half = (static_cast<int>(msize) - 1) >> 1 ;
+ half = (static_cast<int>(msize) - 1) >> 1;
- if(horsImage( i , j , half + me->getStrip() , height_, width_))
- {
- conv = 0.0 ;
- i = 0 ; j = 0 ;
- }
- else
- {
+ if (horsImage(i, j, half + me->getStrip(), height_, width_)) {
+ conv = 0.0;
+ i = 0;
+ j = 0;
+ } else {
// Calculate tangent angle from normal
- double theta = alpha+M_PI/2;
+ double theta = alpha + M_PI / 2;
// Move tangent angle to within 0->M_PI for a positive
// mask index
- while (theta<0) theta += M_PI;
- while (theta>M_PI) theta -= M_PI;
+ while (theta < 0)
+ theta += M_PI;
+ while (theta > M_PI)
+ theta -= M_PI;
// Convert radians to degrees
- int thetadeg = vpMath::round(theta * 180 / M_PI) ;
+ int thetadeg = vpMath::round(theta * 180 / M_PI);
- if(abs(thetadeg) == 180 )
- {
- thetadeg= 0 ;
+ if (abs(thetadeg) == 180) {
+ 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<unsigned int>(i);
unsigned int j_ = static_cast<unsigned int>(j);
unsigned int half_ = static_cast<unsigned int>(half);
- unsigned int ihalf = i_-half_ ;
- unsigned int jhalf = j_-half_ ;
-
- for(unsigned int a = 0 ; a < msize ; a++ )
- {
- unsigned int ihalfa = ihalf+a ;
- for(unsigned int b = 0 ; b < msize ; b++ )
- {
- conv += mask_sign* me->getMask()[index_mask][a][b] *
- // I(i-half+a,j-half+b) ;
- I(ihalfa,jhalf+b) ;
+ unsigned int ihalf = i_ - half_;
+ unsigned int jhalf = j_ - half_;
+
+ for (unsigned int a = 0; a < msize; a++) {
+ unsigned int ihalfa = ihalf + a;
+ for (unsigned int b = 0; b < msize; b++) {
+ conv += mask_sign * me->getMask()[index_mask][a][b] *
+ // I(i-half+a,j-half+b) ;
+ I(ihalfa, jhalf + b);
}
}
-
}
- return(conv) ;
+ return (conv);
}
-
/*!
Specific function for ME.
@@ -370,10 +359,7 @@ vpMeSite::convolution(const vpImage<unsigned char>&I, const vpMe *me)
is needed.
*/
-void
-vpMeSite::track(const vpImage<unsigned char>& I,
- const vpMe *me,
- const bool test_contraste)
+void vpMeSite::track(const vpImage<unsigned char> &I, const vpMe *me, const bool test_contraste)
{
// vpMeSite *list_query_pixels ;
// int max_rank =0 ;
@@ -391,8 +377,8 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// // of the current pixel will be sought
// int range = me->getRange() ;
//
- // // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
- // list_query_pixels = getQueryList(I, range) ;
+ // // std::cout << i << " " << j<<" " << range << " " << suppress <<
+ // std::endl ; list_query_pixels = getQueryList(I, range) ;
//
// double contraste_max = 1 + me->getMu2();
// double contraste_min = 1 - me->getMu1();
@@ -414,7 +400,8 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// // convolution results
// convolution = list_query_pixels[n].convolution(I, me) ;
//
- // // luminance ratio of reference pixel to potential correspondent pixel
+ // // luminance ratio of reference pixel to potential correspondent
+ // pixel
// // the luminance must be similar, hence the ratio value should
// // lay between, for instance, 0.5 and 1.5 (parameter tolerance)
// if( test_contraste )
@@ -422,9 +409,9 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// // Include this to eliminate temporal calculation
// if (convlt==0)
// {
- // std::cout << "vpMeSite::track : Division by zero convlt = 0" << std::endl ;
- // delete []list_query_pixels ;
- // delete []likelihood;
+ // std::cout << "vpMeSite::track : Division by zero convlt = 0" <<
+ // std::endl ; delete []list_query_pixels ; delete
+ // []likelihood;
// throw(vpTrackingException(vpTrackingException::initializationError,
// "Division by zero")) ;
// }
@@ -470,8 +457,9 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// vpDisplay::displayPoint(I, ip, vpColor::red);
// }
//
- // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by the vpMeSite of max likelihood
- // normGradient = vpMath::sqr(max_convolution);
+ // *this = list_query_pixels[max_rank] ;//The vpMeSite is replaced by
+ // the vpMeSite of max likelihood normGradient =
+ // vpMath::sqr(max_convolution);
//
// convlt = max_convolution;
// i_1 = ii_1; //list_query_pixels[max_rank].i ;
@@ -497,12 +485,12 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// delete []likelihood; // modif portage
// }
- vpMeSite *list_query_pixels ;
- int max_rank =-1 ;
+ vpMeSite *list_query_pixels;
+ int max_rank = -1;
// int max_rank1=-1 ;
// int max_rank2 = -1;
- double max_convolution = 0 ;
- double max = 0 ;
+ double max_convolution = 0;
+ double max = 0;
double contraste = 0;
// vpDisplay::display(I) ;
// vpERROR_TRACE("getclcik %d",me->range) ;
@@ -510,144 +498,128 @@ vpMeSite::track(const vpImage<unsigned char>& I,
// range = +/- range of pixels within which the correspondent
// of the current pixel will be sought
- unsigned int range = me->getRange() ;
+ unsigned int range = me->getRange();
- // std::cout << i << " " << j<<" " << range << " " << suppress << std::endl ;
- list_query_pixels = getQueryList(I, (int)range) ;
+ // std::cout << i << " " << j<<" " << range << " " << suppress <<
+ // std::endl ;
+ list_query_pixels = getQueryList(I, (int)range);
- double contraste_max = 1 + me->getMu2();
- double contraste_min = 1 - me->getMu1();
+ double contraste_max = 1 + me->getMu2();
+ double contraste_min = 1 - me->getMu1();
// array in which likelihood ratios will be stored
- double *likelihood= new double[ 2 * range + 1 ] ;
+ double *likelihood = new double[2 * range + 1];
- int ii_1 = i ;
- int jj_1 = j ;
- i_1 = i ;
- j_1 = j ;
+ int ii_1 = i;
+ int jj_1 = j;
+ i_1 = i;
+ j_1 = j;
double threshold;
- threshold = me->getThreshold() ;
+ threshold = me->getThreshold();
double diff = 1e6;
// std::cout <<"---------------------"<<std::endl ;
- for(unsigned int n = 0 ; n < 2 * range + 1 ; n++)
- {
+ for (unsigned int n = 0; n < 2 * range + 1; n++) {
// convolution results
- double convolution_ = list_query_pixels[n].convolution(I, me) ;
+ double convolution_ = list_query_pixels[n].convolution(I, me);
// luminance ratio of reference pixel to potential correspondent pixel
// the luminance must be similar, hence the ratio value should
// lay between, for instance, 0.5 and 1.5 (parameter tolerance)
- if( test_contraste )
- {
- likelihood[n] = fabs(convolution_ + convlt );
- if (likelihood[n]> threshold)
- {
+ if (test_contraste) {
+ likelihood[n] = fabs(convolution_ + convlt);
+ if (likelihood[n] > threshold) {
contraste = convolution_ / convlt;
- if((contraste > contraste_min) && (contraste < contraste_max) && fabs(1-contraste) < diff)
- {
- diff = fabs(1-contraste);
- max_convolution= convolution_;
- max = likelihood[n] ;
- max_rank = (int)n ;
+ if ((contraste > contraste_min) && (contraste < contraste_max) && fabs(1 - contraste) < diff) {
+ diff = fabs(1 - contraste);
+ max_convolution = convolution_;
+ max = likelihood[n];
+ max_rank = (int)n;
// max_rank2 = max_rank1;
// max_rank1 = max_rank;
}
}
}
- else
- {
- likelihood[n] = fabs(2*convolution_) ;
- if (likelihood[n] > max && likelihood[n] > threshold)
- {
- max_convolution= convolution_;
- max = likelihood[n] ;
- max_rank = (int)n ;
+ else {
+ likelihood[n] = fabs(2 * convolution_);
+ if (likelihood[n] > max && likelihood[n] > threshold) {
+ max_convolution = convolution_;
+ max = likelihood[n];
+ max_rank = (int)n;
// max_rank2 = max_rank1;
// max_rank1 = max_rank;
}
}
}
-
+
// test on the likelihood threshold if threshold==-1 then
// the me->threshold is selected
vpImagePoint ip;
// if (test_contrast)
- if(max_rank >= 0)
- {
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- {
- ip.set_i( list_query_pixels[max_rank].i );
- ip.set_j( list_query_pixels[max_rank].j );
+ if (max_rank >= 0) {
+ if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
+ ip.set_i(list_query_pixels[max_rank].i);
+ ip.set_j(list_query_pixels[max_rank].j);
vpDisplay::displayPoint(I, ip, vpColor::red);
}
-
- *this = list_query_pixels[max_rank] ;//The vpMeSite2 is replaced by the vpMeSite2 of max likelihood
- normGradient = vpMath::sqr(max_convolution);
+
+ *this = list_query_pixels[max_rank]; // The vpMeSite2 is replaced by the
+ // vpMeSite2 of max likelihood
+ normGradient = vpMath::sqr(max_convolution);
convlt = max_convolution;
- i_1 = ii_1; //list_query_pixels[max_rank].i ;
- j_1 = jj_1; //list_query_pixels[max_rank].j ;
- delete []list_query_pixels ;
- delete []likelihood;
- }
- else //none of the query sites is better than the threshold
+ i_1 = ii_1; // list_query_pixels[max_rank].i ;
+ j_1 = jj_1; // list_query_pixels[max_rank].j ;
+ delete[] list_query_pixels;
+ delete[] likelihood;
+ } else // none of the query sites is better than the threshold
{
- if ((selectDisplay==RANGE_RESULT)||(selectDisplay==RESULT))
- {
- ip.set_i( list_query_pixels[0].i );
- ip.set_j( list_query_pixels[0].j );
+ if ((selectDisplay == RANGE_RESULT) || (selectDisplay == RESULT)) {
+ ip.set_i(list_query_pixels[0].i);
+ ip.set_j(list_query_pixels[0].j);
vpDisplay::displayPoint(I, ip, vpColor::green);
}
- normGradient = 0 ;
- //if(contraste != 0)
- if(std::fabs(contraste) > std::numeric_limits<double>::epsilon())
+ normGradient = 0;
+ // if(contraste != 0)
+ if (std::fabs(contraste) > std::numeric_limits<double>::epsilon())
state = CONSTRAST; // contrast suppression
else
state = THRESHOLD; // threshold suppression
- delete []list_query_pixels ;
- delete []likelihood; // modif portage
+ delete[] list_query_pixels;
+ delete[] likelihood; // modif portage
}
}
-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)
+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 );
+ 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 );
+ return (os << "Alpha: " << vpMeS.alpha << " Convolution: " << vpMeS.convlt << " Weight: " << vpMeS.weight);
#endif
}
-void vpMeSite::display(const vpImage<unsigned char>& I)
-{
- vpMeSite::display(I,ifloat,jfloat,state);
-}
+void vpMeSite::display(const vpImage<unsigned char> &I) { vpMeSite::display(I, ifloat, jfloat, state); }
-//Static functions
+// Static functions
/*!
Display the moving edge site with a color corresponding to their state.
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem).
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If red : The point is removed because of the robust method in the
+ virtual visual servoing (M-Estimator problem).
- If cyan : The point is removed because it's too close to another.
- Yellow otherwise
@@ -656,32 +628,31 @@ void vpMeSite::display(const vpImage<unsigned char>& I)
\param j : Pixel j of the site
\param state : state of the site
*/
-void vpMeSite::display(const vpImage<unsigned char>& I, const double &i, const double &j, const vpMeSiteState &state)
+void vpMeSite::display(const vpImage<unsigned char> &I, const double &i, const double &j, const vpMeSiteState &state)
{
- switch(state)
- {
- case NO_SUPPRESSION:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
- break;
+ switch (state) {
+ case NO_SUPPRESSION:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::green, 1);
+ break;
- case CONSTRAST:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
- break;
+ case CONSTRAST:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::blue, 1);
+ break;
- case THRESHOLD:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
- break;
+ case THRESHOLD:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::purple, 1);
+ break;
- case M_ESTIMATOR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
- break;
+ case M_ESTIMATOR:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::red, 1);
+ break;
- case TOO_NEAR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
- break;
+ case TOO_NEAR:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::cyan, 1);
+ break;
- default:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
+ default:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::yellow, 1);
}
}
@@ -689,9 +660,12 @@ void vpMeSite::display(const vpImage<unsigned char>& I, const double &i, const d
Display the moving edge site with a color corresponding to their state.
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem).
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If red : The point is removed because of the robust method in the
+ virtual visual servoing (M-Estimator problem).
- If cyan : The point is removed because it's too close to another.
- Yellow otherwise
@@ -700,31 +674,30 @@ void vpMeSite::display(const vpImage<unsigned char>& I, const double &i, const d
\param j : Pixel j of the site
\param state : state of the site
*/
-void vpMeSite::display(const vpImage<vpRGBa>& I, const double &i, const double &j, const vpMeSiteState &state)
+void vpMeSite::display(const vpImage<vpRGBa> &I, const double &i, const double &j, const vpMeSiteState &state)
{
- switch(state)
- {
- case NO_SUPPRESSION:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::green,1);
- break;
+ switch (state) {
+ case NO_SUPPRESSION:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::green, 1);
+ break;
- case CONSTRAST:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::blue,1);
- break;
+ case CONSTRAST:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::blue, 1);
+ break;
- case THRESHOLD:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::purple,1);
- break;
+ case THRESHOLD:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::purple, 1);
+ break;
- case M_ESTIMATOR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::red,1);
- break;
+ case M_ESTIMATOR:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::red, 1);
+ break;
- case TOO_NEAR:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::cyan,1);
- break;
+ case TOO_NEAR:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::cyan, 1);
+ break;
- default:
- vpDisplay::displayCross(I,vpImagePoint(i,j),3,vpColor::yellow,1);
+ default:
+ vpDisplay::displayCross(I, vpImagePoint(i, j), 3, vpColor::yellow, 1);
}
}
diff --git a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp
index 02e52f4..cf9774a 100644
--- a/modules/tracker/me/src/moving-edges/vpMeTracker.cpp
+++ b/modules/tracker/me/src/moving-edges/vpMeTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,41 +41,39 @@
\brief Contains abstract elements for a Distance to Feature type feature.
*/
-
-
-#include <visp3/me/vpMeTracker.h>
-#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
+#include <visp3/me/vpMeTracker.h>
-#include <visp3/core/vpTrackingException.h>
-#include <visp3/core/vpDebug.h>
#include <algorithm>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpTrackingException.h>
#define DEBUG_LEVEL1 0
#define DEBUG_LEVEL2 0
-void
-vpMeTracker::init()
+void vpMeTracker::init()
{
- vpTracker::init() ;
- p.resize(2) ;
- selectDisplay = vpMeSite::NONE ;
+ vpTracker::init();
+ p.resize(2);
+ selectDisplay = vpMeSite::NONE;
}
vpMeTracker::vpMeTracker()
: list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , query_range (0), display_point(false)
+ ,
+ query_range(0), display_point(false)
#endif
{
init();
}
-vpMeTracker::vpMeTracker(const vpMeTracker& meTracker)
- : vpTracker(meTracker),
- list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
+vpMeTracker::vpMeTracker(const vpMeTracker &meTracker)
+ : vpTracker(meTracker), list(), me(NULL), init_range(1), nGoodElement(0), selectDisplay(vpMeSite::NONE)
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
- , query_range (0), display_point(false)
+ ,
+ query_range(0), display_point(false)
#endif
{
init();
@@ -84,11 +83,11 @@ vpMeTracker::vpMeTracker(const vpMeTracker& meTracker)
nGoodElement = meTracker.nGoodElement;
init_range = meTracker.init_range;
selectDisplay = meTracker.selectDisplay;
-
- #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
display_point = meTracker.display_point;
query_range = meTracker.query_range;
- #endif
+#endif
}
/*!
@@ -100,120 +99,99 @@ void vpMeTracker::reset()
list.clear();
}
-vpMeTracker::~vpMeTracker()
-{
- reset();
-}
+vpMeTracker::~vpMeTracker() { reset(); }
-vpMeTracker&
-vpMeTracker::operator = (vpMeTracker& p_me)
+vpMeTracker &vpMeTracker::operator=(vpMeTracker &p_me)
{
list = p_me.list;
me = p_me.me;
- selectDisplay = p_me.selectDisplay ;
-
+ selectDisplay = p_me.selectDisplay;
+ init_range = p_me.init_range;
+ nGoodElement = p_me.nGoodElement;
+#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
+ query_range = p_me.query_range;
+ display_point = p_me.display_point;
+#endif
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 vpMeTracker::numberOfSignal()
{
- unsigned int number_signal=0;
+ unsigned int number_signal = 0;
// Loop through all the points tracked from the contour
number_signal = static_cast<unsigned int>(std::count_if(list.begin(), list.end(), isSuppressZero));
return number_signal;
}
-unsigned int
-vpMeTracker::totalNumberOfSignal()
-{
- return (unsigned int)list.size();
-}
+unsigned int vpMeTracker::totalNumberOfSignal() { return (unsigned int)list.size(); }
-int
-vpMeTracker::outOfImage(int i, int j, int half, int rows, int cols)
+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(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))));
}
-
/*!
Virtual function that is called by lower classes vpMeEllipse, vpMeLine
and vpMeNurbs.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeTracker::initTracking(const vpImage<unsigned char>& I)
+void vpMeTracker::initTracking(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
// Must set range to 0
unsigned int range_tmp = me->getRange();
me->setRange(init_range);
- nGoodElement=0;
+ nGoodElement = 0;
int d = 0;
vpImagePoint ip1, ip2;
// Loop through list of sites to track
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite refp = *it;//current reference pixel
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite refp = *it; // current reference pixel
- d++ ;
+ d++;
// If element hasn't been suppressed
- if(refp.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
try {
- refp.track(I,me,false);
- }
- catch(...)
- {
+ refp.track(I, me, false);
+ } catch (...) {
// EM verifier quel signal est de sortie !!!
- vpERROR_TRACE("Error caught") ;
- throw ;
+ vpERROR_TRACE("Error caught");
+ throw;
}
- if(refp.getState() == vpMeSite::NO_SUPPRESSION) nGoodElement++;
+ if (refp.getState() == vpMeSite::NO_SUPPRESSION)
+ nGoodElement++;
}
-
#if (DEBUG_LEVEL2)
{
- double a,b ;
- a = refp.i_1 - refp.i ;
- b = refp.j_1 - refp.j ;
- if(refp.getState() == vpMeSite::NO_SUPPRESSION) {
- ip1.set_i( refp.i );
- ip1.set_j( refp.j );
- ip2.set_i( refp.i+a );
- ip2.set_j( refp.j+b );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
+ double a, b;
+ a = refp.i_1 - refp.i;
+ b = refp.j_1 - refp.j;
+ if (refp.getState() == vpMeSite::NO_SUPPRESSION) {
+ ip1.set_i(refp.i);
+ ip1.set_j(refp.j);
+ ip2.set_i(refp.i + a);
+ ip2.set_j(refp.j + b);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::green);
}
}
#endif
@@ -227,13 +205,12 @@ vpMeTracker::initTracking(const vpImage<unsigned char>& I)
switch (res)
{
case ERR_TRACKING:
- std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " << std::endl ;
- break ;
- case fatalError:
- std::cout << "vpMeTracker::initTracking:Track return fatalError" << std::endl ;
- break ;
+ std::cout << "vpMeTracker::initTracking:Track return ERR_TRACKING " <<
+ std::endl ; break ; case fatalError: std::cout <<
+ "vpMeTracker::initTracking:Track return fatalError" << std::endl ; break ;
default:
- std::cout << "vpMeTracker::initTracking:Track return error " << res << std::endl ;
+ std::cout << "vpMeTracker::initTracking:Track return error " << res <<
+ std::endl ;
}
return res ;
}
@@ -247,68 +224,59 @@ vpMeTracker::initTracking(const vpImage<unsigned char>& I)
\param I : Image.
- \exception vpTrackingException::initializationError : Moving edges not initialized.
+ \exception vpTrackingException::initializationError : Moving edges not
+ initialized.
*/
-void
-vpMeTracker::track(const vpImage<unsigned char>& I)
+void vpMeTracker::track(const vpImage<unsigned char> &I)
{
if (!me) {
vpDERROR_TRACE(2, "Tracking error: Moving edges not initialized");
- throw(vpTrackingException(vpTrackingException::initializationError,
- "Moving edges not initialized")) ;
+ throw(vpTrackingException(vpTrackingException::initializationError, "Moving edges not initialized"));
}
- if (list.empty())
- {
+ 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;
+ nGoodElement = 0;
// int d =0;
// Loop through list of sites to track
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
- vpMeSite s = *it;//current reference pixel
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
+ vpMeSite s = *it; // current reference pixel
// d++ ;
// If element hasn't been suppressed
- if(s.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (s.getState() == vpMeSite::NO_SUPPRESSION) {
- try{
+ try {
// vpERROR_TRACE("%d",d ) ;
// vpERROR_TRACE("range %d",me->range) ;
- s.track(I,me,true);
- }
- catch(vpTrackingException)
- {
- vpERROR_TRACE("catch exception ") ;
+ s.track(I, me, true);
+ } catch (vpTrackingException) {
+ vpERROR_TRACE("catch exception ");
s.setState(vpMeSite::THRESHOLD);
}
- if(s.getState() != vpMeSite::THRESHOLD)
- {
+ if (s.getState() != vpMeSite::THRESHOLD) {
nGoodElement++;
#if (DEBUG_LEVEL2)
{
- double a,b ;
- a = s.i_1 - s.i ;
- b = s.j_1 - s.j ;
- if(s.getState() == vpMeSite::NO_SUPPRESSION) {
- ip1.set_i( s.i );
- ip1.set_j( s.j );
- ip2.set_i( s.i+a*5 );
- ip2.set_j( s.j+b*5 );
- vpDisplay::displayArrow(I, ip1, ip2, vpColor::green) ;
+ double a, b;
+ a = s.i_1 - s.i;
+ b = s.j_1 - s.j;
+ if (s.getState() == vpMeSite::NO_SUPPRESSION) {
+ ip1.set_i(s.i);
+ ip1.set_j(s.j);
+ ip2.set_i(s.i + a * 5);
+ ip2.set_j(s.j + b * 5);
+ vpDisplay::displayArrow(I, ip1, ip2, vpColor::green);
}
}
#endif
-
}
*it = s;
}
@@ -317,45 +285,45 @@ vpMeTracker::track(const vpImage<unsigned char>& I)
/*!
Display the moving edge sites with a color corresponding to their state.
-
+
- If green : The vpMeSite is a good point.
- - If blue : The point is removed because of the vpMeSite tracking phase (constrast problem).
- - If purple : The point is removed because of the vpMeSite tracking phase (threshold problem).
- - If red : The point is removed because of the robust method in the virtual visual servoing (M-Estimator problem).
+ - If blue : The point is removed because of the vpMeSite tracking phase
+ (constrast problem).
+ - If purple : The point is removed because of the vpMeSite tracking phase
+ (threshold problem).
+ - If red : The point is removed because of the robust method in the virtual
+ visual servoing (M-Estimator problem).
- If cyan : The point is removed because it's too close to another.
- Yellow otherwise
-
+
\param I : The image.
*/
-void
-vpMeTracker::display(const vpImage<unsigned char>& I)
+void vpMeTracker::display(const vpImage<unsigned char> &I)
{
#if (DEBUG_LEVEL1)
{
- std::cout <<"begin vpMeTracker::displayList() " << std::endl ;
- std::cout<<" There are "<<list.size()<< " sites in the list " << std::endl ;
+ std::cout << "begin vpMeTracker::displayList() " << std::endl;
+ std::cout << " There are " << list.size() << " sites in the list " << std::endl;
}
#endif
- for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::const_iterator it = list.begin(); it != list.end(); ++it) {
vpMeSite p_me = *it;
p_me.display(I);
}
}
/*! Displays the status of moving edge sites
-
+
\param I : The image.
\param w : vector
\param index_w : index
*/
-void
-vpMeTracker::display(const vpImage<unsigned char>& I,vpColVector &w, unsigned int &index_w)
+void vpMeTracker::display(const vpImage<unsigned char> &I, vpColVector &w, unsigned int &index_w)
{
- for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
+ for (std::list<vpMeSite>::iterator it = list.begin(); it != list.end(); ++it) {
vpMeSite P = *it;
- if(P.getState() == vpMeSite::NO_SUPPRESSION)
- {
+ if (P.getState() == vpMeSite::NO_SUPPRESSION) {
P.weight = w[index_w];
index_w++;
}
@@ -367,5 +335,3 @@ vpMeTracker::display(const vpImage<unsigned char>& I,vpColVector &w, unsigned in
#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2
-
-
diff --git a/modules/tracker/me/src/moving-edges/vpNurbs.cpp b/modules/tracker/me/src/moving-edges/vpNurbs.cpp
index fa85610..8409148 100644
--- a/modules/tracker/me/src/moving-edges/vpNurbs.cpp
+++ b/modules/tracker/me/src/moving-edges/vpNurbs.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,10 @@
*
*****************************************************************************/
-
-
-#include <visp3/me/vpNurbs.h>
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#include <visp3/core/vpColVector.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <visp3/me/vpNurbs.h>
/*
Compute the distance d = |Pw1-Pw2|
*/
@@ -48,159 +47,149 @@ inline double distance(const vpImagePoint &iP1, const double w1, const vpImagePo
{
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));
+ double distancew = w1 - w2;
+ return sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
}
-
/*!
Basic constructor.
-
+
The degree \f$ p \f$ of the NURBS basis functions is set to 3 to
compute cubic NURBS.
*/
-vpNurbs::vpNurbs()
- : weights()
-{
- p = 3;
-}
+vpNurbs::vpNurbs() : weights() { p = 3; }
/*!
Copy constructor.
*/
-vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights()
-{
- weights = nurbs.weights;
-}
+vpNurbs::vpNurbs(const vpNurbs &nurbs) : vpBSpline(nurbs), weights(nurbs.weights) {}
/*!
Basic destructor
*/
-vpNurbs::~vpNurbs()
-{
-}
+vpNurbs::~vpNurbs() {}
/*!
- Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the knot \f$ u \f$.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the NURBS basis functions.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
- \param l_weights : the list of weights.
-
+ Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n
+ (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the
+ knot \f$ u \f$.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the NURBS basis functions. \param l_knots : The
+ knot vector \param l_controlPoints : the list of control points. \param
+ l_weights : the list of weights.
+
return the coordinates of a point corresponding to the knot \f$ u \f$.
*/
-vpImagePoint
-vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p,
- std::vector<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
+vpImagePoint vpNurbs::computeCurvePoint(double l_u, unsigned int l_i, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
{
- vpBasisFunction* N = NULL;
+ vpBasisFunction *N = NULL;
N = computeBasisFuns(l_u, l_i, l_p, l_knots);
vpImagePoint pt;
double ic = 0;
double jc = 0;
double wc = 0;
- for(unsigned int j = 0; j <= l_p; j++)
- {
- ic = ic + N[j].value * (l_controlPoints[l_i-l_p+j]).get_i() * l_weights[l_i-l_p+j];
- jc = jc + N[j].value * (l_controlPoints[l_i-l_p+j]).get_j() * l_weights[l_i-l_p+j];
- wc = wc + N[j].value * l_weights[l_i-l_p+j];
+ for (unsigned int j = 0; j <= l_p; j++) {
+ ic = ic + N[j].value * (l_controlPoints[l_i - l_p + j]).get_i() * l_weights[l_i - l_p + j];
+ jc = jc + N[j].value * (l_controlPoints[l_i - l_p + j]).get_j() * l_weights[l_i - l_p + j];
+ wc = wc + N[j].value * l_weights[l_i - l_p + j];
}
- pt.set_i(ic/wc);
- pt.set_j(jc/wc);
-
- if (N != NULL) delete[] N;
+ pt.set_i(ic / wc);
+ pt.set_j(jc / wc);
+
+ if (N != NULL)
+ delete[] N;
return pt;
}
/*!
- Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the knot \f$ u \f$.
-
+ Compute the coordinates of a point \f$ C(u) = \frac{\sum_{i=0}^n
+ (N_{i,p}(u)w_iP_i)}{\sum_{i=0}^n (N_{i,p}(u)w_i)} \f$ corresponding to the
+ knot \f$ u \f$.
+
\param u : A real number which is between the extrimities of the knot vector
-
+
return the coordinates of a point corresponding to the knot \f$ u \f$.
*/
vpImagePoint vpNurbs::computeCurvePoint(double u)
{
- vpBasisFunction* N = NULL;
+ vpBasisFunction *N = NULL;
N = computeBasisFuns(u);
vpImagePoint pt;
double ic = 0;
double jc = 0;
double wc = 0;
- for(unsigned int j = 0; j <= p; j++)
- {
- ic = ic + N[j].value * (controlPoints[N[0].i+j]).get_i() * weights[N[0].i+j]; //N[0].i = findSpan(u)-p
- jc = jc + N[j].value * (controlPoints[N[0].i+j]).get_j() * weights[N[0].i+j];
- wc = wc + N[j].value * weights[N[0].i+j];
+ for (unsigned int j = 0; j <= p; j++) {
+ ic = ic + N[j].value * (controlPoints[N[0].i + j]).get_i() * weights[N[0].i + j]; // N[0].i = findSpan(u)-p
+ jc = jc + N[j].value * (controlPoints[N[0].i + j]).get_j() * weights[N[0].i + j];
+ wc = wc + N[j].value * weights[N[0].i + j];
}
- pt.set_i(ic/wc);
- pt.set_j(jc/wc);
-
- if (N != NULL) delete[] N;
+ pt.set_i(ic / wc);
+ pt.set_j(jc / wc);
+
+ if (N != NULL)
+ delete[] N;
return pt;
}
-
/*!
This function is used in the computeCurveDersPoint method.
-
- Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} \f$.
-
+
+ Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der}
+ \f$.
+
The formula used is the following :
-
+
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ contains the control points and the associatede weights.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the NURBS basis functions.
- \param l_der : The last derivative to be computed.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
- \param l_weights : the list of weights.
-
- \return a matrix of size (l_der+1)x3 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth line of the matrix. For each lines the first and the second column coresponds to the coordinates (i,j) of the point and the third column corresponds to the associated weight.
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p
+ \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$
+ contains the control points and the associatede weights.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the NURBS basis functions. \param l_der : The
+ last derivative to be computed. \param l_knots : The knot vector \param
+ l_controlPoints : the list of control points. \param l_weights : the list of
+ weights.
+
+ \return a matrix of size (l_der+1)x3 containing the coordinates \f$
+ C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in
+ the kth line of the matrix. For each lines the first and the second column
+ coresponds to the coordinates (i,j) of the point and the third column
+ corresponds to the associated weight.
*/
-vpMatrix
-vpNurbs::computeCurveDers(double l_u, unsigned int l_i,
- unsigned int l_p, unsigned int l_der,
- std::vector<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
+vpMatrix vpNurbs::computeCurveDers(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
{
- vpMatrix derivate(l_der+1,3);
- vpBasisFunction** N = NULL;
+ vpMatrix derivate(l_der + 1, 3);
+ vpBasisFunction **N = NULL;
N = computeDersBasisFuns(l_u, l_i, l_p, l_der, l_knots);
- for(unsigned int k = 0; k <= l_der; k++)
- {
+ for (unsigned int k = 0; k <= l_der; k++) {
derivate[k][0] = 0.0;
- derivate[k][1] = 0.0;
+ derivate[k][1] = 0.0;
derivate[k][2] = 0.0;
- for(unsigned int j = 0; j<= l_p; j++)
- {
- derivate[k][0] = derivate[k][0] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_i();
- derivate[k][1] = derivate[k][1] + N[k][j].value*(l_controlPoints[l_i-l_p+j]).get_j();
- derivate[k][2] = derivate[k][2] + N[k][j].value*(l_weights[l_i-l_p+j]);
+ for (unsigned int j = 0; j <= l_p; j++) {
+ derivate[k][0] = derivate[k][0] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_i();
+ derivate[k][1] = derivate[k][1] + N[k][j].value * (l_controlPoints[l_i - l_p + j]).get_j();
+ derivate[k][2] = derivate[k][2] + N[k][j].value * (l_weights[l_i - l_p + j]);
}
}
-
- if (N!=NULL) {
- for(unsigned int i = 0; i <= l_der; i++)
- delete [] N[i];
+
+ if (N != NULL) {
+ for (unsigned int i = 0; i <= l_der; i++)
+ delete[] N[i];
delete[] N;
}
return derivate;
@@ -208,191 +197,193 @@ vpNurbs::computeCurveDers(double l_u, unsigned int l_i,
/*!
This function is used in the computeCurveDersPoint method.
-
+
Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , der \f$.
-
+
The formula used is the following :
-
+
\f[ C^{(k)}(u) = \sum_{i=0}^n (N_{i,p}^{(k)}(u)Pw_i) \f]
-
- where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$ contains the control points and the associatede weights.
-
+
+ where \f$ i \f$ is the knot interval number in which \f$ u \f$ lies, \f$ p
+ \f$ is the degree of the NURBS basis function and \f$ Pw_i = (P_i w_i) \f$
+ contains the control points and the associatede weights.
+
\param u : A real number which is between the extrimities of the knot vector
\param der : The last derivative to be computed.
-
- \return a matrix of size (l_der+1)x3 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth line of the matrix. For each lines the first and the second column coresponds to the coordinates (i,j) of the point and the third column corresponds to the associated weight.
+
+ \return a matrix of size (l_der+1)x3 containing the coordinates \f$
+ C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the
+ kth line of the matrix. For each lines the first and the second column
+ coresponds to the coordinates (i,j) of the point and the third column
+ corresponds to the associated weight.
*/
vpMatrix vpNurbs::computeCurveDers(double u, unsigned int der)
{
- vpMatrix derivate(der+1,3);
- vpBasisFunction** N = NULL;
+ vpMatrix derivate(der + 1, 3);
+ vpBasisFunction **N = NULL;
N = computeDersBasisFuns(u, der);
- for(unsigned int k = 0; k <= der; k++)
- {
+ for (unsigned int k = 0; k <= der; k++) {
derivate[k][0] = 0.0;
- derivate[k][1] = 0.0;
+ derivate[k][1] = 0.0;
derivate[k][2] = 0.0;
- for(unsigned int j = 0; j<= p; j++)
- {
- derivate[k][0] = derivate[k][0] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_i();
- derivate[k][1] = derivate[k][1] + N[k][j].value*(controlPoints[N[0][0].i-p+j]).get_j();
- derivate[k][2] = derivate[k][2] + N[k][j].value*(weights[N[0][0].i-p+j]);
+ for (unsigned int j = 0; j <= p; j++) {
+ derivate[k][0] = derivate[k][0] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_i();
+ derivate[k][1] = derivate[k][1] + N[k][j].value * (controlPoints[N[0][0].i - p + j]).get_j();
+ derivate[k][2] = derivate[k][2] + N[k][j].value * (weights[N[0][0].i - p + j]);
}
}
-
- if (N!=NULL) delete[] N;
+
+ if (N != NULL)
+ delete[] N;
return derivate;
}
-
/*!
- Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} \f$.
-
+ Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der}
+ \f$.
+
To see how the derivatives are computed refers to the Nurbs book.
-
- \param l_u : A real number which is between the extrimities of the knot vector
- \param l_i : the number of the knot interval in which \f$ l_u \f$ lies
- \param l_p : Degree of the NURBS basis functions.
- \param l_der : The last derivative to be computed.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
- \param l_weights : the list of weights.
-
- \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth cell of the array.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector \param l_i : the number of the knot interval in which \f$ l_u \f$
+ lies \param l_p : Degree of the NURBS basis functions. \param l_der : The
+ last derivative to be computed. \param l_knots : The knot vector \param
+ l_controlPoints : the list of control points. \param l_weights : the list of
+ weights.
+
+ \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u)
+ \f$ for \f$ k = 0, ... , l_{der} \f$. The kth derivative is in the kth cell
+ of the array.
*/
-vpImagePoint*
-vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i,
- unsigned int l_p, unsigned int l_der,
- std::vector<double> &l_knots,
- std::vector<vpImagePoint> &l_controlPoints,
- std::vector<double> &l_weights)
+vpImagePoint *vpNurbs::computeCurveDersPoint(double l_u, unsigned int l_i, unsigned int l_p, unsigned int l_der,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
{
std::vector<vpImagePoint> A;
vpImagePoint pt;
- for(unsigned int j = 0; j < l_controlPoints.size(); j++)
- {
+ for (unsigned int j = 0; j < l_controlPoints.size(); j++) {
pt = l_controlPoints[j];
- pt.set_i(pt.get_i()*l_weights[j]);
- pt.set_j(pt.get_j()*l_weights[j]);
+ pt.set_i(pt.get_i() * l_weights[j]);
+ pt.set_j(pt.get_j() * l_weights[j]);
A.push_back(pt);
}
vpMatrix Awders = computeCurveDers(l_u, l_i, l_p, l_der, l_knots, A, l_weights);
- vpImagePoint* CK = new vpImagePoint[l_der+1];
+ vpImagePoint *CK = new vpImagePoint[l_der + 1];
- for(unsigned int k = 0; k <= l_der; k++)
- {
+ for (unsigned int k = 0; k <= l_der; k++) {
double ic = Awders[k][0];
double jc = Awders[k][1];
- for(unsigned int j = 1; j <= k; j++)
- {
- double tmpComb = static_cast<double>( vpMath::comb(k,j) );
- ic = ic - tmpComb*Awders[k][2]*(CK[k-j].get_i());
- jc = jc - tmpComb*Awders[j][2]*(CK[k-j].get_j());
+ for (unsigned int j = 1; j <= k; j++) {
+ double tmpComb = static_cast<double>(vpMath::comb(k, j));
+ ic = ic - tmpComb * Awders[k][2] * (CK[k - j].get_i());
+ jc = jc - tmpComb * Awders[j][2] * (CK[k - j].get_j());
}
- CK[k].set_ij(ic/Awders[0][2],jc/Awders[0][2]);
+ CK[k].set_ij(ic / Awders[0][2], jc / Awders[0][2]);
}
return CK;
}
-
/*!
- Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der} \f$.
-
+ Compute the kth derivatives of \f$ C(u) \f$ for \f$ k = 0, ... , l_{der}
+ \f$.
+
To see how the derivatives are computed refers to the Nurbs book.
-
+
\param u : A real number which is between the extrimities of the knot vector
\param der : The last derivative to be computed.
-
- \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u) \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of the array.
+
+ \return an array of size l_der+1 containing the coordinates \f$ C^{(k)}(u)
+ \f$ for \f$ k = 0, ... , der \f$. The kth derivative is in the kth cell of
+ the array.
*/
-vpImagePoint* vpNurbs::computeCurveDersPoint(double u, unsigned int der)
+vpImagePoint *vpNurbs::computeCurveDersPoint(double u, unsigned int der)
{
unsigned int i = findSpan(u);
return computeCurveDersPoint(u, i, p, der, knots, controlPoints, weights);
}
-
/*!
- Insert \f$ l_r \f$ times a knot in the \f$ l_k \f$ th interval of the knot vector. The inserted knot \f$ l_u \f$ has multiplicity \f$ l_s \f$.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param l_u : A real number which is between the extrimities of the knot vector and which has to be inserted.
- \param l_k : The number of the knot interval in which \f$ l_u \f$ lies.
- \param l_s : Multiplicity of \f$ l_u \f$
+ Insert \f$ l_r \f$ times a knot in the \f$ l_k \f$ th interval of the knot
+ vector. The inserted knot \f$ l_u \f$ has multiplicity \f$ l_s \f$.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector and which has to be inserted. \param l_k : The number of the knot
+ interval in which \f$ l_u \f$ lies. \param l_s : Multiplicity of \f$ l_u \f$
\param l_r : Number of times \f$ l_u \f$ has to be inserted.
\param l_p : Degree of the NURBS basis functions.
\param l_knots : The knot vector
\param l_controlPoints : the list of control points.
\param l_weights : the list of weights.
*/
-void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+void vpNurbs::curveKnotIns(double l_u, unsigned int l_k, unsigned int l_s, unsigned int l_r, unsigned int l_p,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
{
- vpMatrix Rw(l_p+1,3);
+ vpMatrix Rw(l_p + 1, 3);
std::vector<vpImagePoint>::iterator it1;
std::vector<double>::iterator it2;
vpImagePoint pt;
double w = 0;
- for (unsigned int j = 0; j <= l_p-l_s; j++)
- {
- Rw[j][0] = (l_controlPoints[l_k-l_p+j]).get_i() * l_weights[l_k-l_p+j];
- Rw[j][1] = (l_controlPoints[l_k-l_p+j]).get_j() * l_weights[l_k-l_p+j];
- Rw[j][2] = l_weights[l_k-l_p+j];
+ for (unsigned int j = 0; j <= l_p - l_s; j++) {
+ Rw[j][0] = (l_controlPoints[l_k - l_p + j]).get_i() * l_weights[l_k - l_p + j];
+ Rw[j][1] = (l_controlPoints[l_k - l_p + j]).get_j() * l_weights[l_k - l_p + j];
+ Rw[j][2] = l_weights[l_k - l_p + j];
}
it1 = l_controlPoints.begin();
- l_controlPoints.insert(it1+(int)l_k-(int)l_s, l_r, pt);
+ l_controlPoints.insert(it1 + (int)l_k - (int)l_s, l_r, pt);
it2 = l_weights.begin();
- l_weights.insert(it2+(int)l_k-(int)l_s, l_r, w);
+ l_weights.insert(it2 + (int)l_k - (int)l_s, l_r, w);
- unsigned int L=0;
+ unsigned int L = 0;
double alpha;
- for (unsigned int j = 1; j <= l_r; j++)
- {
- L = l_k - l_p +j;
-
- for (unsigned int i = 0; i <=l_p-j-l_s; i++)
- {
- alpha = (l_u - l_knots[L+i])/(l_knots[i+l_k+1] - l_knots[L+i]);
- Rw[i][0]= alpha*Rw[i+1][0]+(1.0-alpha)*Rw[i][0];
- Rw[i][1]= alpha*Rw[i+1][1]+(1.0-alpha)*Rw[i][1];
- Rw[i][2]= alpha*Rw[i+1][2]+(1.0-alpha)*Rw[i][2];
+ for (unsigned int j = 1; j <= l_r; j++) {
+ L = l_k - l_p + j;
+
+ for (unsigned int i = 0; i <= l_p - j - l_s; i++) {
+ alpha = (l_u - l_knots[L + i]) / (l_knots[i + l_k + 1] - l_knots[L + i]);
+ Rw[i][0] = alpha * Rw[i + 1][0] + (1.0 - alpha) * Rw[i][0];
+ Rw[i][1] = alpha * Rw[i + 1][1] + (1.0 - alpha) * Rw[i][1];
+ Rw[i][2] = alpha * Rw[i + 1][2] + (1.0 - alpha) * Rw[i][2];
}
- pt.set_ij(Rw[0][0]/Rw[0][2],Rw[0][1]/Rw[0][2]);
+ pt.set_ij(Rw[0][0] / Rw[0][2], Rw[0][1] / Rw[0][2]);
l_controlPoints[L] = pt;
l_weights[L] = Rw[0][2];
- pt.set_ij(Rw[l_p-j-l_s][0]/Rw[l_p-j-l_s][2],Rw[l_p-j-l_s][1]/Rw[l_p-j-l_s][2]);
- l_controlPoints[l_k+l_r-j-l_s] = pt;
- l_weights[l_k+l_r-j-l_s] = Rw[l_p-j-l_s][2];
+ pt.set_ij(Rw[l_p - j - l_s][0] / Rw[l_p - j - l_s][2], Rw[l_p - j - l_s][1] / Rw[l_p - j - l_s][2]);
+ l_controlPoints[l_k + l_r - j - l_s] = pt;
+ l_weights[l_k + l_r - j - l_s] = Rw[l_p - j - l_s][2];
}
- for(unsigned int j = L+1; j < l_k-l_s; j++)
- {
- pt.set_ij(Rw[j-L][0]/Rw[j-L][2],Rw[j-L][1]/Rw[j-L][2]);
+ for (unsigned int j = L + 1; j < l_k - l_s; j++) {
+ pt.set_ij(Rw[j - L][0] / Rw[j - L][2], Rw[j - L][1] / Rw[j - L][2]);
l_controlPoints[j] = pt;
- l_weights[j] = Rw[j-L][2];
+ l_weights[j] = Rw[j - L][2];
}
it2 = l_knots.begin();
- l_knots.insert(it2+(int)l_k, l_r, l_u);
+ l_knots.insert(it2 + (int)l_k, l_r, l_u);
}
-
/*!
- Insert \f$ r \f$ times a knot in the \f$ k \f$ th interval of the knot vector. The inserted knot \f$ u \f$ has multiplicity \f$ s \f$.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param u : A real number which is between the extrimities of the knot vector and which has to be inserted.
- \param s : Multiplicity of \f$ l_u \f$
- \param r : Number of times \f$ l_u \f$ has to be inserted.
+ Insert \f$ r \f$ times a knot in the \f$ k \f$ th interval of the knot
+ vector. The inserted knot \f$ u \f$ has multiplicity \f$ s \f$.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param u : A real number which is between the extrimities of the knot vector
+ and which has to be inserted. \param s : Multiplicity of \f$ l_u \f$ \param
+ r : Number of times \f$ l_u \f$ has to be inserted.
*/
void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r)
{
@@ -400,20 +391,21 @@ void vpNurbs::curveKnotIns(double u, unsigned int s, unsigned int r)
curveKnotIns(u, i, s, r, p, knots, controlPoints, weights);
}
-
/*!
Insert \f$ l_r \f$ knots in the knot vector.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param l_x : Several real numbers which are between the extrimities of the knot vector and which have to be inserted.
- \param l_r : Number of knot in the array \f$ l_x \f$.
- \param l_p : Degree of the NURBS basis functions.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param l_x : Several real numbers which are between the extrimities of the
+ knot vector and which have to be inserted. \param l_r : Number of knot in
+ the array \f$ l_x \f$. \param l_p : Degree of the NURBS basis functions.
\param l_knots : The knot vector
\param l_controlPoints : the list of control points.
\param l_weights : the list of weights.
*/
-void vpNurbs::refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+void vpNurbs::refineKnotVectCurve(double *l_x, unsigned int l_r, unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
{
unsigned int a = findSpan(l_x[0], l_p, l_knots);
unsigned int b = findSpan(l_x[l_r], l_p, l_knots);
@@ -422,9 +414,8 @@ void vpNurbs::refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_
unsigned int n = (unsigned int)l_controlPoints.size();
unsigned int m = (unsigned int)l_knots.size();
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
+ for (unsigned int j = 0; j < n; j++) {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
}
std::vector<double> l_knots_tmp(l_knots);
@@ -434,121 +425,120 @@ void vpNurbs::refineKnotVectCurve(double* l_x, unsigned int l_r, unsigned int l_
vpImagePoint pt;
double w = 0;
- for (unsigned int j = 0; j <= l_r; j++)
- {
+ for (unsigned int j = 0; j <= l_r; j++) {
l_controlPoints.push_back(pt);
l_weights.push_back(w);
l_knots.push_back(w);
}
- for(unsigned int j = b+l_p; j <= m-1; j++) l_knots[j+l_r+1] = l_knots_tmp[j];
+ for (unsigned int j = b + l_p; j <= m - 1; j++)
+ l_knots[j + l_r + 1] = l_knots_tmp[j];
- for (unsigned int j = b-1; j <= n-1; j++)
- {
- l_controlPoints[j+l_r+1] = l_controlPoints_tmp[j];
- l_weights[j+l_r+1] = l_weights_tmp[j];
- }
+ for (unsigned int j = b - 1; j <= n - 1; j++) {
+ l_controlPoints[j + l_r + 1] = l_controlPoints_tmp[j];
+ l_weights[j + l_r + 1] = l_weights_tmp[j];
+ }
- unsigned int i = b+l_p-1;
- unsigned int k = b+l_p+l_r;
+ unsigned int i = b + l_p - 1;
+ unsigned int k = b + l_p + l_r;
{
unsigned int j = l_r + 1;
- do{
+ do {
j--;
- while(l_x[j] <= l_knots[i] && i > a)
- {
- l_controlPoints[k-l_p-1] = l_controlPoints_tmp[i-l_p-1];
- l_weights[k-l_p-1] = l_weights_tmp[i-l_p-1];
+ while (l_x[j] <= l_knots[i] && i > a) {
+ l_controlPoints[k - l_p - 1] = l_controlPoints_tmp[i - l_p - 1];
+ l_weights[k - l_p - 1] = l_weights_tmp[i - l_p - 1];
l_knots[k] = l_knots_tmp[i];
k--;
i--;
}
- l_controlPoints[k-l_p-1] = l_controlPoints[k-l_p];
- l_weights[k-l_p-1] = l_weights[k-l_p];
-
- for (unsigned int l = 1; l <= l_p; l++)
- {
- unsigned int ind = k-l_p+l;
- double alpha = l_knots[k+l] - l_x[j];
- //if (vpMath::abs(alpha) == 0.0)
- if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon())
- {
- l_controlPoints[ind-1] = l_controlPoints[ind];
- l_weights[ind-1] = l_weights[ind];
- }
- else
- {
- alpha = alpha/(l_knots[k+l]-l_knots_tmp[i-l_p+l]);
- l_controlPoints[ind-1].set_i( alpha * l_controlPoints[ind-1].get_i() + (1.0-alpha) * l_controlPoints[ind].get_i());
- l_controlPoints[ind-1].set_j( alpha * l_controlPoints[ind-1].get_j() + (1.0-alpha) * l_controlPoints[ind].get_j());
- l_weights[ind-1] = alpha * l_weights[ind-1] + (1.0-alpha) * l_weights[ind];
+ l_controlPoints[k - l_p - 1] = l_controlPoints[k - l_p];
+ l_weights[k - l_p - 1] = l_weights[k - l_p];
+
+ for (unsigned int l = 1; l <= l_p; l++) {
+ unsigned int ind = k - l_p + l;
+ double alpha = l_knots[k + l] - l_x[j];
+ // if (vpMath::abs(alpha) == 0.0)
+ if (std::fabs(alpha) <= std::numeric_limits<double>::epsilon()) {
+ l_controlPoints[ind - 1] = l_controlPoints[ind];
+ l_weights[ind - 1] = l_weights[ind];
+ } else {
+ alpha = alpha / (l_knots[k + l] - l_knots_tmp[i - l_p + l]);
+ l_controlPoints[ind - 1].set_i(alpha * l_controlPoints[ind - 1].get_i() +
+ (1.0 - alpha) * l_controlPoints[ind].get_i());
+ l_controlPoints[ind - 1].set_j(alpha * l_controlPoints[ind - 1].get_j() +
+ (1.0 - alpha) * l_controlPoints[ind].get_j());
+ l_weights[ind - 1] = alpha * l_weights[ind - 1] + (1.0 - alpha) * l_weights[ind];
}
}
l_knots[k] = l_x[j];
k--;
- }while(j != 0);
+ } while (j != 0);
}
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()/l_weights[j],l_controlPoints[j].get_j()/l_weights[j]);
+ for (unsigned int j = 0; j < n; j++) {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i() / l_weights[j], l_controlPoints[j].get_j() / l_weights[j]);
}
}
-
/*!
Insert \f$ r \f$ knots in the knot vector.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param x : Several real numbers which are between the extrimities of the knot vector and which have to be inserted.
- \param r : Number of knot in the array \f$ l_x \f$.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param x : Several real numbers which are between the extrimities of the
+ knot vector and which have to be inserted. \param r : Number of knot in the
+ array \f$ l_x \f$.
*/
-void vpNurbs::refineKnotVectCurve(double* x, unsigned int r)
+void vpNurbs::refineKnotVectCurve(double *x, unsigned int r)
{
refineKnotVectCurve(x, r, p, knots, controlPoints, weights);
}
-
/*!
- Remove \f$ l_num \f$ times the knot \f$ l_u \f$ from the knot vector. The removed knot \f$ l_u \f$ is the \f$ l_r \f$ th vector in the knot vector.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param l_u : A real number which is between the extrimities of the knot vector and which has to be removed.
- \param l_r : Index of \f$ l_u \f$ in the knot vector.
- \param l_num : Number of times \f$ l_u \f$ has to be removed.
+ Remove \f$ l_num \f$ times the knot \f$ l_u \f$ from the knot vector. The
+ removed knot \f$ l_u \f$ is the \f$ l_r \f$ th vector in the knot vector.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param l_u : A real number which is between the extrimities of the knot
+ vector and which has to be removed. \param l_r : Index of \f$ l_u \f$ in the
+ knot vector. \param l_num : Number of times \f$ l_u \f$ has to be removed.
\param l_TOL : A parameter which has to be computed.
\param l_s : Multiplicity of \f$ l_u \f$.
\param l_p : Degree of the NURBS basis functions.
\param l_knots : The knot vector
\param l_controlPoints : the list of control points.
\param l_weights : the list of weights.
-
+
\return The number of time that l_u was removed.
-
+
\f$ l_{TOL} = \frac{dw_{min}}{1+|P|_{max}} \f$
-
- where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ |P|_{max} \f$ is the maximum distance of any point on the original curve from the origin and \f$ d \f$ is the desired bound on deviation.
+
+ where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$
+ |P|_{max} \f$ is the maximum distance of any point on the original curve
+ from the origin and \f$ d \f$ is the desired bound on deviation.
*/
-unsigned int
-vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+unsigned int vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, double l_TOL, unsigned int l_s,
+ unsigned int l_p, std::vector<double> &l_knots,
+ std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
{
unsigned int n = (unsigned int)l_controlPoints.size();
unsigned int m = n + l_p + 1;
- for (unsigned int j = 0; j < n; j++)
- {
- l_controlPoints[j].set_ij(l_controlPoints[j].get_i()*l_weights[j],l_controlPoints[j].get_j()*l_weights[j]);
+ for (unsigned int j = 0; j < n; j++) {
+ l_controlPoints[j].set_ij(l_controlPoints[j].get_i() * l_weights[j], l_controlPoints[j].get_j() * l_weights[j]);
}
unsigned int ord = l_p + 1;
- double fout = (2*l_r-l_s-l_p)/2.;
+ double fout = (2 * l_r - l_s - l_p) / 2.;
unsigned int last = l_r - l_s;
unsigned int first = l_r - l_p;
- unsigned int tblSize = 2*l_p+1;
+ unsigned int tblSize = 2 * l_p + 1;
vpImagePoint *tempP = new vpImagePoint[tblSize];
double *tempW = new double[tblSize];
vpImagePoint pt;
@@ -557,65 +547,63 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, doubl
double alfj = 0;
unsigned int i, j;
- for(t = 0; t < l_num; t++)
- {
+ for (t = 0; t < l_num; t++) {
unsigned int off = first - 1;
tempP[0] = l_controlPoints[off];
tempW[0] = l_weights[off];
- tempP[last+1-off] = l_controlPoints[last+1];
- tempW[last+1-off] = l_weights[last+1];
+ tempP[last + 1 - off] = l_controlPoints[last + 1];
+ tempW[last + 1 - off] = l_weights[last + 1];
i = first;
j = last;
unsigned int ii = 1;
- unsigned int jj = last -off;
+ unsigned int jj = last - off;
int remflag = 0;
- while (j-i > t)
- {
- alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
- alfj = (l_u - l_knots[j-t])/(l_knots[j+ord]-l_knots[j-t]);
- pt.set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
- tempP[ii].set_i((l_controlPoints[i].get_i()-(1.0-alfi)*tempP[ii-1].get_i())/alfi);
- tempP[ii].set_j((l_controlPoints[i].get_j()-(1.0-alfi)*tempP[ii-1].get_j())/alfi);
- tempW[ii] = ((l_weights[i]-(1.0-alfi)*tempW[ii-1])/alfi);
- tempP[jj].set_i((l_controlPoints[j].get_i()-alfj*tempP[jj+1].get_i())/(1.0-alfj));
- tempP[jj].set_j((l_controlPoints[j].get_j()-alfj*tempP[jj+1].get_j())/(1.0-alfj));
- tempW[jj] = ((l_weights[j]-alfj*tempW[jj+1])/(1.0-alfj));
+ while (j - i > t) {
+ alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
+ alfj = (l_u - l_knots[j - t]) / (l_knots[j + ord] - l_knots[j - t]);
+ pt.set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
+ tempP[ii].set_i((l_controlPoints[i].get_i() - (1.0 - alfi) * tempP[ii - 1].get_i()) / alfi);
+ tempP[ii].set_j((l_controlPoints[i].get_j() - (1.0 - alfi) * tempP[ii - 1].get_j()) / alfi);
+ tempW[ii] = ((l_weights[i] - (1.0 - alfi) * tempW[ii - 1]) / alfi);
+ tempP[jj].set_i((l_controlPoints[j].get_i() - alfj * tempP[jj + 1].get_i()) / (1.0 - alfj));
+ tempP[jj].set_j((l_controlPoints[j].get_j() - alfj * tempP[jj + 1].get_j()) / (1.0 - alfj));
+ tempW[jj] = ((l_weights[j] - alfj * tempW[jj + 1]) / (1.0 - alfj));
i++;
j--;
ii++;
jj--;
}
- if(j-i < t)
- {
- double distancei = tempP[ii-1].get_i() - tempP[jj+1].get_i();
- double distancej = tempP[ii-1].get_j() - tempP[jj+1].get_j();
- double distancew = tempW[ii-1] - tempW[jj+1];
- double distance = sqrt(vpMath::sqr(distancei)+vpMath::sqr(distancej)+vpMath::sqr(distancew));
- if(distance <= l_TOL) remflag = 1;
+ if (j - i < t) {
+ double distancei = tempP[ii - 1].get_i() - tempP[jj + 1].get_i();
+ double distancej = tempP[ii - 1].get_j() - tempP[jj + 1].get_j();
+ double distancew = tempW[ii - 1] - tempW[jj + 1];
+ double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
+ if (distance <= l_TOL)
+ remflag = 1;
+ } else {
+ alfi = (l_u - l_knots[i]) / (l_knots[i + ord + t] - l_knots[i]);
+ double distancei =
+ l_controlPoints[i].get_i() - (alfi * tempP[ii + t + 1].get_i() + (1.0 - alfi) * tempP[ii - 1].get_i());
+ double distancej =
+ l_controlPoints[i].get_j() - (alfi * tempP[ii + t + 1].get_j() + (1.0 - alfi) * tempP[ii - 1].get_j());
+ double distancew = l_weights[i] - (alfi * tempW[ii + t + 1] + (1.0 - alfi) * tempW[ii - 1]);
+ double distance = sqrt(vpMath::sqr(distancei) + vpMath::sqr(distancej) + vpMath::sqr(distancew));
+ if (distance <= l_TOL)
+ remflag = 1;
}
- else
- {
- alfi = (l_u - l_knots[i])/(l_knots[i+ord+t]-l_knots[i]);
- double distancei = l_controlPoints[i].get_i() - (alfi*tempP[ii+t+1].get_i()+(1.0-alfi)*tempP[ii-1].get_i());
- double distancej = l_controlPoints[i].get_j() - (alfi*tempP[ii+t+1].get_j()+(1.0-alfi)*tempP[ii-1].get_j());
- double distancew = l_weights[i] - (alfi*tempW[ii+t+1]+(1.0-alfi)*tempW[ii-1]);
- double distance = sqrt(vpMath::sqr(distancei)+vpMath::sqr(distancej)+vpMath::sqr(distancew));
- if(distance <= l_TOL) remflag = 1;
- }
- if (remflag == 0) break;
- else
- {
+ if (remflag == 0)
+ break;
+ else {
i = first;
j = last;
- while (j-i>t)
- {
- l_controlPoints[i].set_i(tempP[i-off].get_i());
- l_controlPoints[i].set_j(tempP[i-off].get_j());
- l_weights[i] = tempW[i-off];
- l_controlPoints[j].set_i(tempP[j-off].get_i());
- l_controlPoints[j].set_j(tempP[j-off].get_j());
- l_weights[j] = tempW[j-off];
+ while (j - i > t) {
+ l_controlPoints[i].set_i(tempP[i - off].get_i());
+ l_controlPoints[i].set_j(tempP[i - off].get_j());
+ l_weights[i] = tempW[i - off];
+ l_controlPoints[j].set_i(tempP[j - off].get_i());
+ l_controlPoints[j].set_j(tempP[j - off].get_j());
+ l_weights[j] = tempW[j - off];
i++;
j--;
}
@@ -628,171 +616,172 @@ vpNurbs::removeCurveKnot(double l_u, unsigned int l_r, unsigned int l_num, doubl
delete[] tempW;
return t;
}
- for(unsigned int k = l_r+1; k <= m; k++) l_knots[k-t] = l_knots[k];
+ for (unsigned int k = l_r + 1; k <= m; k++)
+ l_knots[k - t] = l_knots[k];
j = (unsigned int)fout;
i = j;
- for(unsigned int k = 1; k< t; k++)
- {
- if (k%2) i++;
- else j--;
+ for (unsigned int k = 1; k < t; k++) {
+ if (k % 2)
+ i++;
+ else
+ j--;
}
- for(unsigned int k = i+1; k <= n; k++)
- {
+ for (unsigned int k = i + 1; k <= n; k++) {
l_controlPoints[j].set_i(l_controlPoints[k].get_i());
l_controlPoints[j].set_j(l_controlPoints[k].get_j());
l_weights[j] = l_weights[k];
j++;
}
- for(unsigned int k = 0; k < t; k++)
- {
- l_knots.erase(l_knots.end()-1);
- l_controlPoints.erase(l_controlPoints.end()-1);
+ for (unsigned int k = 0; k < t; k++) {
+ l_knots.erase(l_knots.end() - 1);
+ l_controlPoints.erase(l_controlPoints.end() - 1);
}
- for(unsigned int k = 0; k < l_controlPoints.size(); k++)
- l_controlPoints[k].set_ij(l_controlPoints[k].get_i()/l_weights[k],l_controlPoints[k].get_j()/l_weights[k]);
+ for (unsigned int k = 0; k < l_controlPoints.size(); k++)
+ l_controlPoints[k].set_ij(l_controlPoints[k].get_i() / l_weights[k], l_controlPoints[k].get_j() / l_weights[k]);
delete[] tempP;
delete[] tempW;
return t;
}
-
/*!
- Remove \f$ num \f$ times the knot \f$ u \f$ from the knot vector. The removed knot \f$ u \f$ is the \f$ r \f$ th vector in the knot vector.
-
- Of course the knot vector changes. But The list of control points and the list of the associated weights change too.
-
- \param u : A real number which is between the extrimities of the knot vector and which has to be removed.
- \param r : Index of \f$ l_u \f$ in the knot vector.
- \param num : Number of times \f$ l_u \f$ has to be removed.
- \param TOL : A parameter which has to be computed.
-
+ Remove \f$ num \f$ times the knot \f$ u \f$ from the knot vector. The
+ removed knot \f$ u \f$ is the \f$ r \f$ th vector in the knot vector.
+
+ Of course the knot vector changes. But The list of control points and the
+ list of the associated weights change too.
+
+ \param u : A real number which is between the extrimities of the knot vector
+ and which has to be removed. \param r : Index of \f$ l_u \f$ in the knot
+ vector. \param num : Number of times \f$ l_u \f$ has to be removed. \param
+ TOL : A parameter which has to be computed.
+
\return The number of time that l_u was removed.
-
+
\f$ TOL = \frac{dw_{min}}{1+|P|_{max}} \f$
-
- where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$ |P|_{max} \f$ is the maximum distance of any point on the original curve from the origin and \f$ d \f$ is the desired bound on deviation.
+
+ where \f$ w_{min} \f$ is the minimal weight on the original curve, \f$
+ |P|_{max} \f$ is the maximum distance of any point on the original curve
+ from the origin and \f$ d \f$ is the desired bound on deviation.
*/
-unsigned int
-vpNurbs::removeCurveKnot(double u, unsigned int r, unsigned int num, double TOL)
+unsigned int vpNurbs::removeCurveKnot(double u, unsigned int r, unsigned int num, double TOL)
{
return removeCurveKnot(u, r, num, TOL, 0, p, knots, controlPoints, weights);
}
-
/*!
- Method which enables to compute a NURBS curve passing through a set of data points.
-
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
-
- \param l_crossingPoints : The list of data points which have to be interpolated.
- \param l_p : Degree of the NURBS basis functions. This value need to be > 0.
- \param l_knots : The knot vector
- \param l_controlPoints : the list of control points.
- \param l_weights : the list of weights.
+ Method which enables to compute a NURBS curve passing through a set of data
+ points.
+
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
+
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated. \param l_p : Degree of the NURBS basis functions. This value
+ need to be > 0. \param l_knots : The knot vector \param l_controlPoints :
+ the list of control points. \param l_weights : the list of weights.
*/
-void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+void vpNurbs::globalCurveInterp(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
{
if (l_p == 0) {
- //vpERROR_TRACE("Bad degree of the NURBS basis functions");
- throw(vpException(vpException::badValue,
- "Bad degree of the NURBS basis functions")) ;
+ // vpERROR_TRACE("Bad degree of the NURBS basis functions");
+ throw(vpException(vpException::badValue, "Bad degree of the NURBS basis functions"));
}
l_knots.clear();
l_controlPoints.clear();
l_weights.clear();
- unsigned int n = (unsigned int)l_crossingPoints.size()-1;
- unsigned int m = n+l_p+1;
+ unsigned int n = (unsigned int)l_crossingPoints.size() - 1;
+ unsigned int m = n + l_p + 1;
double d = 0;
- for(unsigned int k=1; k<=n; k++)
- d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
-
- //Compute ubar
+ for (unsigned int k = 1; k <= n; k++)
+ d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
+
+ // Compute ubar
std::vector<double> ubar;
ubar.push_back(0.0);
- for(unsigned int k=1; k<n; k++) {
- ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
+ for (unsigned int k = 1; k < n; k++) {
+ ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
}
ubar.push_back(1.0);
- //Compute the knot vector
- for(unsigned int k = 0; k <= l_p; k++)
+ // Compute the knot vector
+ for (unsigned int k = 0; k <= l_p; k++)
l_knots.push_back(0.0);
- double sum = 0;
- for(unsigned int k = 1; k <= l_p; k++)
+ double sum = 0;
+ for (unsigned int k = 1; k <= l_p; k++)
sum = sum + ubar[k];
- //Centripetal method
- for(unsigned int k = 1; k <= n-l_p; k++)
- {
- l_knots.push_back(sum/l_p);
- sum = sum - ubar[k-1] + ubar[l_p+k-1];
+ // Centripetal method
+ for (unsigned int k = 1; k <= n - l_p; k++) {
+ l_knots.push_back(sum / l_p);
+ sum = sum - ubar[k - 1] + ubar[l_p + k - 1];
}
- for(unsigned int k = m-l_p; k <= m; k++)
+ for (unsigned int k = m - l_p; k <= m; k++)
l_knots.push_back(1.0);
-
- vpMatrix A(n+1,n+1);
- vpBasisFunction* N;
- for(unsigned int i = 0; i <= n; i++)
- {
+ vpMatrix A(n + 1, n + 1);
+ vpBasisFunction *N;
+
+ for (unsigned int i = 0; i <= n; i++) {
unsigned int span = findSpan(ubar[i], l_p, l_knots);
N = computeBasisFuns(ubar[i], span, l_p, l_knots);
- for (unsigned int k = 0; k <= l_p; k++) A[i][span-l_p+k] = N[k].value;
+ for (unsigned int k = 0; k <= l_p; k++)
+ A[i][span - l_p + k] = N[k].value;
delete[] N;
}
- //vpMatrix Ainv = A.inverseByLU();
+ // vpMatrix Ainv = A.inverseByLU();
vpMatrix Ainv;
A.pseudoInverse(Ainv);
- vpColVector Qi(n+1);
- vpColVector Qj(n+1);
- vpColVector Qw(n+1);
- for (unsigned int k = 0; k <= n; k++)
- {
+ vpColVector Qi(n + 1);
+ vpColVector Qj(n + 1);
+ vpColVector Qw(n + 1);
+ for (unsigned int k = 0; k <= n; k++) {
Qi[k] = l_crossingPoints[k].get_i();
Qj[k] = l_crossingPoints[k].get_j();
}
Qw = 1;
- vpColVector Pi = Ainv*Qi;
- vpColVector Pj = Ainv*Qj;
- vpColVector Pw = Ainv*Qw;
+ vpColVector Pi = Ainv * Qi;
+ vpColVector Pj = Ainv * Qj;
+ vpColVector Pw = Ainv * Qw;
vpImagePoint pt;
- for (unsigned int k = 0; k <= n; k++)
- {
- pt.set_ij(Pi[k],Pj[k]);
+ for (unsigned int k = 0; k <= n; k++) {
+ pt.set_ij(Pi[k], Pj[k]);
l_controlPoints.push_back(pt);
l_weights.push_back(Pw[k]);
}
}
/*!
- Method which enables to compute a NURBS curve passing through a set of data points.
-
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
-
- \param l_crossingPoints : The list of data points which have to be interpolated.
+ Method which enables to compute a NURBS curve passing through a set of data
+ points.
+
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
+
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated.
*/
void vpNurbs::globalCurveInterp(vpList<vpMeSite> &l_crossingPoints)
{
std::vector<vpImagePoint> v_crossingPoints;
l_crossingPoints.front();
vpMeSite s = l_crossingPoints.value();
- vpImagePoint pt(s.ifloat,s.jfloat);
+ vpImagePoint pt(s.ifloat, s.jfloat);
vpImagePoint pt_1 = pt;
v_crossingPoints.push_back(pt);
l_crossingPoints.next();
- while(!l_crossingPoints.outside())
- {
+ while (!l_crossingPoints.outside()) {
s = l_crossingPoints.value();
- pt.set_ij(s.ifloat,s.jfloat);
- if (vpImagePoint::distance(pt_1,pt) >= 10)
- {
+ pt.set_ij(s.ifloat, s.jfloat);
+ if (vpImagePoint::distance(pt_1, pt) >= 10) {
v_crossingPoints.push_back(pt);
pt_1 = pt;
}
@@ -802,41 +791,46 @@ void vpNurbs::globalCurveInterp(vpList<vpMeSite> &l_crossingPoints)
}
/*!
- Method which enables to compute a NURBS curve passing through a set of data points.
+ Method which enables to compute a NURBS curve passing through a set of data
+ points.
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
- \param l_crossingPoints : The list of data points which have to be interpolated.
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated.
*/
void vpNurbs::globalCurveInterp(const std::list<vpImagePoint> &l_crossingPoints)
{
std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
v_crossingPoints.push_back(*it);
}
globalCurveInterp(v_crossingPoints, p, knots, controlPoints, weights);
}
-
/*!
- Method which enables to compute a NURBS curve passing through a set of data points.
+ Method which enables to compute a NURBS curve passing through a set of data
+ points.
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
- \param l_crossingPoints : The list of data points which have to be interpolated.
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated.
*/
void vpNurbs::globalCurveInterp(const std::list<vpMeSite> &l_crossingPoints)
{
std::vector<vpImagePoint> v_crossingPoints;
vpMeSite s = l_crossingPoints.front();
- vpImagePoint pt(s.ifloat,s.jfloat);
+ vpImagePoint pt(s.ifloat, s.jfloat);
vpImagePoint pt_1 = pt;
v_crossingPoints.push_back(pt);
std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin();
++it;
- for(; it!=l_crossingPoints.end(); ++it){
+ for (; it != l_crossingPoints.end(); ++it) {
vpImagePoint pt_tmp(it->ifloat, it->jfloat);
- if (vpImagePoint::distance(pt_1, pt_tmp) >= 10){
+ if (vpImagePoint::distance(pt_1, pt_tmp) >= 10) {
v_crossingPoints.push_back(pt_tmp);
pt_1 = pt_tmp;
}
@@ -845,144 +839,138 @@ void vpNurbs::globalCurveInterp(const std::list<vpMeSite> &l_crossingPoints)
}
/*!
- Method which enables to compute a NURBS curve passing through a set of data points.
-
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
-*/
-void vpNurbs::globalCurveInterp()
-{
- globalCurveInterp(crossingPoints, p, knots, controlPoints, weights);
-}
+ Method which enables to compute a NURBS curve passing through a set of data
+ points.
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
+*/
+void vpNurbs::globalCurveInterp() { globalCurveInterp(crossingPoints, p, knots, controlPoints, weights); }
/*!
- Method which enables to compute a NURBS curve approximating a set of data points.
-
+ Method which enables to compute a NURBS curve approximating a set of data
+ points.
+
The data points are approximated thanks to a least square method.
-
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
-
- \param l_crossingPoints : The list of data points which have to be interpolated.
- \param l_p : Degree of the NURBS basis functions.
- \param l_n : The desired number of control points. l_n must be under or equal to the number of data points.
- \param l_knots : The knot vector.
- \param l_controlPoints : the list of control points.
- \param l_weights : the list of weights.
+
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
+
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated. \param l_p : Degree of the NURBS basis functions. \param l_n :
+ The desired number of control points. l_n must be under or equal to the
+ number of data points. \param l_knots : The knot vector. \param
+ l_controlPoints : the list of control points. \param l_weights : the list of
+ weights.
*/
-void vpNurbs::globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n, std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints, std::vector<double> &l_weights)
+void vpNurbs::globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, unsigned int l_p, unsigned int l_n,
+ std::vector<double> &l_knots, std::vector<vpImagePoint> &l_controlPoints,
+ std::vector<double> &l_weights)
{
l_knots.clear();
l_controlPoints.clear();
l_weights.clear();
- unsigned int m = (unsigned int)l_crossingPoints.size()-1;
+ unsigned int m = (unsigned int)l_crossingPoints.size() - 1;
double d = 0;
- for(unsigned int k=1; k<=m; k++)
- d = d + distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1);
-
- //Compute ubar
+ for (unsigned int k = 1; k <= m; k++)
+ d = d + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1);
+
+ // Compute ubar
std::vector<double> ubar;
ubar.push_back(0.0);
- for(unsigned int k=1; k<m; k++)
- ubar.push_back(ubar[k-1]+distance(l_crossingPoints[k],1,l_crossingPoints[k-1],1)/d);
- ubar.push_back(1.0);
+ for (unsigned int k = 1; k < m; k++)
+ ubar.push_back(ubar[k - 1] + distance(l_crossingPoints[k], 1, l_crossingPoints[k - 1], 1) / d);
+ ubar.push_back(1.0);
- //Compute the knot vector
- for(unsigned int k = 0; k <= l_p; k++)
+ // Compute the knot vector
+ for (unsigned int k = 0; k <= l_p; k++)
l_knots.push_back(0.0);
-
- d = (double)(m+1)/(double)(l_n-l_p+1);
-
- 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]);
+
+ d = (double)(m + 1) / (double)(l_n - l_p + 1);
+
+ 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]);
}
- for(unsigned int k = 0; k <= l_p ; k++)
+ for (unsigned int k = 0; k <= l_p; k++)
l_knots.push_back(1.0);
- //Compute Rk
+ // Compute Rk
std::vector<vpImagePoint> Rk;
- vpBasisFunction* N;
- for(unsigned int k = 1; k <= m-1; k++)
- {
+ vpBasisFunction *N;
+ for (unsigned int k = 1; k <= m - 1; k++) {
unsigned int span = findSpan(ubar[k], l_p, l_knots);
- if (span == l_p && span == l_n)
- {
+ if (span == l_p && span == l_n) {
N = computeBasisFuns(ubar[k], span, l_p, l_knots);
- vpImagePoint pt(l_crossingPoints[k].get_i()-N[0].value*l_crossingPoints[0].get_i()-N[l_p].value*l_crossingPoints[m].get_i(),
- l_crossingPoints[k].get_j()-N[0].value*l_crossingPoints[0].get_j()-N[l_p].value*l_crossingPoints[m].get_j());
+ vpImagePoint pt(l_crossingPoints[k].get_i() - N[0].value * l_crossingPoints[0].get_i() -
+ N[l_p].value * l_crossingPoints[m].get_i(),
+ l_crossingPoints[k].get_j() - N[0].value * l_crossingPoints[0].get_j() -
+ N[l_p].value * l_crossingPoints[m].get_j());
Rk.push_back(pt);
delete[] N;
- }
- else if (span == l_p)
- {
+ } 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)
- {
+ } 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
- {
+ } else {
Rk.push_back(l_crossingPoints[k]);
}
}
- vpMatrix A(m-1,l_n-1);
- //Compute A
- for(unsigned int i = 1; i <= m-1; i++)
- {
+ vpMatrix A(m - 1, l_n - 1);
+ // Compute A
+ for (unsigned int i = 1; i <= m - 1; i++) {
unsigned int span = findSpan(ubar[i], l_p, l_knots);
N = computeBasisFuns(ubar[i], span, l_p, l_knots);
- for (unsigned int k = 0; k <= l_p; k++)
- {
+ for (unsigned int k = 0; k <= l_p; k++) {
if (N[k].i > 0 && N[k].i < l_n)
- A[i-1][N[k].i-1] = N[k].value;
+ A[i - 1][N[k].i - 1] = N[k].value;
}
delete[] N;
}
-
- vpColVector Ri(l_n-1);
- vpColVector Rj(l_n-1);
- vpColVector Rw(l_n-1);
- for (unsigned int i = 0; i < l_n-1; i++)
- {
- double sum =0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_i();
+
+ vpColVector Ri(l_n - 1);
+ vpColVector Rj(l_n - 1);
+ vpColVector Rw(l_n - 1);
+ for (unsigned int i = 0; i < l_n - 1; i++) {
+ double sum = 0;
+ for (unsigned int k = 0; k < m - 1; k++)
+ sum = sum + A[k][i] * Rk[k].get_i();
Ri[i] = sum;
sum = 0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]*Rk[k].get_j();
+ for (unsigned int k = 0; k < m - 1; k++)
+ sum = sum + A[k][i] * Rk[k].get_j();
Rj[i] = sum;
sum = 0;
- for (unsigned int k = 0; k < m-1; k++) sum = sum + A[k][i]; //The crossing points weigths are equal to 1.
+ for (unsigned int k = 0; k < m - 1; k++)
+ sum = sum + A[k][i]; // The crossing points weigths are equal to 1.
Rw[i] = sum;
}
-
+
vpMatrix AtA = A.AtA();
vpMatrix AtAinv;
AtA.pseudoInverse(AtAinv);
-
- vpColVector Pi = AtAinv*Ri;
- vpColVector Pj = AtAinv*Rj;
- vpColVector Pw = AtAinv*Rw;
-
+
+ vpColVector Pi = AtAinv * Ri;
+ vpColVector Pj = AtAinv * Rj;
+ vpColVector Pw = AtAinv * Rw;
+
vpImagePoint pt;
l_controlPoints.push_back(l_crossingPoints[0]);
l_weights.push_back(1.0);
- for (unsigned int k = 0; k < l_n-1; k++)
- {
- pt.set_ij(Pi[k],Pj[k]);
+ for (unsigned int k = 0; k < l_n - 1; k++) {
+ pt.set_ij(Pi[k], Pj[k]);
l_controlPoints.push_back(pt);
l_weights.push_back(Pw[k]);
}
@@ -994,12 +982,12 @@ void vpNurbs::globalCurveApprox(std::vector<vpImagePoint> &l_crossingPoints, uns
Method which enables to compute a NURBS curve approximating a set of
data points.
-
+
The data points are approximated thanks to a least square method.
-
+
The result of the method is composed by a knot vector, a set of
control points and a set of associated weights.
-
+
\param l_crossingPoints : The list of data points which have to be
interpolated.
@@ -1010,10 +998,9 @@ void vpNurbs::globalCurveApprox(vpList<vpMeSite> &l_crossingPoints, unsigned int
{
std::vector<vpImagePoint> v_crossingPoints;
l_crossingPoints.front();
- while(!l_crossingPoints.outside())
- {
+ while (!l_crossingPoints.outside()) {
vpMeSite s = l_crossingPoints.value();
- vpImagePoint pt(s.ifloat,s.jfloat);
+ vpImagePoint pt(s.ifloat, s.jfloat);
v_crossingPoints.push_back(pt);
l_crossingPoints.next();
}
@@ -1021,19 +1008,22 @@ void vpNurbs::globalCurveApprox(vpList<vpMeSite> &l_crossingPoints, unsigned int
}
/*!
- Method which enables to compute a NURBS curve approximating a set of data points.
+ Method which enables to compute a NURBS curve approximating a set of data
+ points.
The data points are approximated thanks to a least square method.
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
- \param l_crossingPoints : The list of data points which have to be interpolated.
- \param n : The desired number of control points. The parameter \e n must be under or equal to the number of data points.
+ \param l_crossingPoints : The list of data points which have to be
+ interpolated. \param n : The desired number of control points. The parameter
+ \e n must be under or equal to the number of data points.
*/
void vpNurbs::globalCurveApprox(const std::list<vpImagePoint> &l_crossingPoints, unsigned int n)
{
std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpImagePoint>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
v_crossingPoints.push_back(*it);
}
globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
@@ -1058,23 +1048,23 @@ void vpNurbs::globalCurveApprox(const std::list<vpImagePoint> &l_crossingPoints,
void vpNurbs::globalCurveApprox(const std::list<vpMeSite> &l_crossingPoints, unsigned int n)
{
std::vector<vpImagePoint> v_crossingPoints;
- for(std::list<vpMeSite>::const_iterator it=l_crossingPoints.begin(); it!=l_crossingPoints.end(); ++it){
+ for (std::list<vpMeSite>::const_iterator it = l_crossingPoints.begin(); it != l_crossingPoints.end(); ++it) {
vpImagePoint pt(it->ifloat, it->jfloat);
v_crossingPoints.push_back(pt);
}
globalCurveApprox(v_crossingPoints, p, n, knots, controlPoints, weights);
}
-
/*!
- Method which enables to compute a NURBS curve approximating a set of data points.
-
+ Method which enables to compute a NURBS curve approximating a set of data
+ points.
+
The data points are approximated thanks to a least square method.
-
- The result of the method is composed by a knot vector, a set of control points and a set of associated weights.
+
+ The result of the method is composed by a knot vector, a set of control
+ points and a set of associated weights.
*/
void vpNurbs::globalCurveApprox(unsigned int n)
{
globalCurveApprox(crossingPoints, p, n, knots, controlPoints, weights);
}
-
diff --git a/modules/tracker/me/test/testNurbs.cpp b/modules/tracker/me/test/testNurbs.cpp
index dc61dce..896cc5e 100644
--- a/modules/tracker/me/test/testNurbs.cpp
+++ b/modules/tracker/me/test/testNurbs.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,23 +46,23 @@
#include <visp3/me/vpNurbs.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/io/vpImageIo.h>
#ifdef VISP_HAVE_MODULE_GUI
-# include <visp3/gui/vpDisplayGTK.h>
-# include <visp3/gui/vpDisplayGDI.h>
-# include <visp3/gui/vpDisplayOpenCV.h>
-# include <visp3/gui/vpDisplayD3D.h>
-# include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpDisplayD3D.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#endif
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/core/vpIoTools.h>
#include <cstdlib>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpParseArgv.h>
#if defined(VISP_HAVE_DISPLAY)
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -98,7 +99,6 @@ OPTIONS: Default\n\
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
}
-
/*!
Set the program options.
@@ -114,17 +114,25 @@ OPTIONS: Default\n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -139,28 +147,25 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
bool opt_click_allowed = true;
bool opt_display = true;
// Read the command line options
- if (getOptions(argc, argv, opt_click_allowed,
- opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> I(540,480);
- vpImage<unsigned char> I2(540,480);
- vpImage<unsigned char> I3(540,480);
+ vpImage<unsigned char> I(540, 480);
+ vpImage<unsigned char> I2(540, 480);
+ vpImage<unsigned char> I3(540, 480);
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display[3];
#elif defined VISP_HAVE_GDI
@@ -173,9 +178,9 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(I, 100, 100,"Points as control points") ;
- vpDisplay::display(I) ;
- vpDisplay::flush(I) ;
+ display[0].init(I, 100, 100, "Points as control points");
+ vpDisplay::display(I);
+ vpDisplay::flush(I);
}
vpNurbs Nurbs;
@@ -195,28 +200,28 @@ main(int argc, const char ** argv)
std::list<vpImagePoint> controlPoints;
std::list<double> weights;
vpImagePoint pt;
- pt.set_ij(50,300);
+ pt.set_ij(50, 300);
controlPoints.push_back(pt);
weights.push_back(1);
- pt.set_ij(100,130);
+ pt.set_ij(100, 130);
controlPoints.push_back(pt);
weights.push_back(5);
- pt.set_ij(150,400);
+ pt.set_ij(150, 400);
controlPoints.push_back(pt);
weights.push_back(0.2);
- pt.set_ij(200,370);
+ pt.set_ij(200, 370);
controlPoints.push_back(pt);
weights.push_back(10);
- pt.set_ij(250,120);
+ pt.set_ij(250, 120);
controlPoints.push_back(pt);
weights.push_back(1);
- pt.set_ij(300,250);
+ pt.set_ij(300, 250);
controlPoints.push_back(pt);
weights.push_back(2);
- pt.set_ij(350,200);
+ pt.set_ij(350, 200);
controlPoints.push_back(pt);
weights.push_back(3);
- pt.set_ij(400,300);
+ pt.set_ij(400, 300);
controlPoints.push_back(pt);
weights.push_back(1);
@@ -225,146 +230,135 @@ main(int argc, const char ** argv)
Nurbs.set_controlPoints(controlPoints);
Nurbs.set_weights(weights);
- std::cout << "The parameters are :" <<std::endl;
- std::cout << "p : " << Nurbs.get_p() <<std::endl;
- std::cout << "" <<std::endl;
- std::cout << "The knot vector :" <<std::endl;
+ std::cout << "The parameters are :" << std::endl;
+ std::cout << "p : " << Nurbs.get_p() << std::endl;
+ std::cout << "" << std::endl;
+ std::cout << "The knot vector :" << std::endl;
std::list<double> knots_cur;
Nurbs.get_knots(knots_cur);
- unsigned int i_display=0;
- for(std::list<double>::const_iterator it=knots_cur.begin(); it!=knots_cur.end(); ++it, ++i_display){
+ unsigned int i_display = 0;
+ for (std::list<double>::const_iterator it = knots_cur.begin(); it != knots_cur.end(); ++it, ++i_display) {
std::cout << i_display << " ---> " << *it << std::endl;
}
- std::cout << "The control points are :" <<std::endl;
+ std::cout << "The control points are :" << std::endl;
std::list<vpImagePoint> controlPoints_cur;
Nurbs.get_controlPoints(controlPoints_cur);
- i_display=0;
- for(std::list<vpImagePoint>::const_iterator it=controlPoints_cur.begin(); it!=controlPoints_cur.end(); ++it, ++i_display){
+ i_display = 0;
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints_cur.begin(); it != controlPoints_cur.end();
+ ++it, ++i_display) {
std::cout << i_display << " ---> " << *it << std::endl;
}
- std::cout << "The associated weights are :" <<std::endl;
+ std::cout << "The associated weights are :" << std::endl;
std::list<double> weights_cur;
Nurbs.get_weights(weights_cur);
- i_display=0;
- for(std::list<double>::const_iterator it=weights_cur.begin(); it!=weights_cur.end(); ++it, ++i_display){
+ i_display = 0;
+ for (std::list<double>::const_iterator it = weights_cur.begin(); it != weights_cur.end(); ++it, ++i_display) {
std::cout << i_display << " ---> " << *it << std::endl;
}
- unsigned int i = Nurbs.findSpan(5/2.0);
- std::cout << "The knot interval number for the value u = 5/2 is : " << i <<std::endl;
+ unsigned int i = Nurbs.findSpan(5 / 2.0);
+ std::cout << "The knot interval number for the value u = 5/2 is : " << i << std::endl;
vpBasisFunction *N = NULL;
- N = Nurbs.computeBasisFuns(5/2.0);
+ N = Nurbs.computeBasisFuns(5 / 2.0);
std::cout << "The nonvanishing basis functions N(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ 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);
+ N2 = Nurbs.computeDersBasisFuns(5 / 2.0, 2);
std::cout << "The first derivatives of the basis functions N'(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ for (unsigned int j = 0; j < Nurbs.get_p() + 1; j++)
std::cout << N2[1][j].value << std::endl;
std::cout << "The second derivatives of the basis functions N''(u=5/2) are :" << std::endl;
- for (unsigned int j = 0; j < Nurbs.get_p()+1; j++)
+ for (unsigned int j = 0; j < Nurbs.get_p() + 1; j++)
std::cout << N2[2][j].value << std::endl;
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
double u = 0.0;
- while (u <= 5)
- {
+ while (u <= 5) {
pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I,pt,4,vpColor::red);
- u+=0.01;
+ vpDisplay::displayCross(I, pt, 4, vpColor::red);
+ u += 0.01;
}
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) {
vpDisplay::displayCross(I, *it, 4, vpColor::green);
}
- vpDisplay::flush(I) ;
+ vpDisplay::flush(I);
vpDisplay::getClick(I);
}
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display[1].init(I2, 100, 100,"Points interpolation") ;
- vpDisplay::display(I2) ;
- vpDisplay::flush(I2) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ display[1].init(I2, 100, 100, "Points interpolation");
+ vpDisplay::display(I2);
+ vpDisplay::flush(I2);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
Nurbs.globalCurveInterp(controlPoints);
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
double u = 0.0;
- while (u <= 1)
- {
+ while (u <= 1) {
pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I2,pt,4,vpColor::red);
- u+=0.01;
+ vpDisplay::displayCross(I2, pt, 4, vpColor::red);
+ u += 0.01;
}
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) {
vpDisplay::displayCross(I2, *it, 4, vpColor::green);
}
- vpDisplay::flush(I2) ;
+ vpDisplay::flush(I2);
vpDisplay::getClick(I2);
}
-
if (opt_display) {
- try{
+ try {
// Display size is automatically defined by the image (I) size
- display[2].init(I3, 100, 100,"Points approximation") ;
- vpDisplay::display(I3) ;
- vpDisplay::flush(I3) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error while displaying the image") ;
+ display[2].init(I3, 100, 100, "Points approximation");
+ vpDisplay::display(I3);
+ vpDisplay::flush(I3);
+ } catch (...) {
+ vpERROR_TRACE("Error while displaying the image");
exit(-1);
}
}
- Nurbs.globalCurveApprox(controlPoints,5);
+ Nurbs.globalCurveApprox(controlPoints, 5);
- if (opt_display && opt_click_allowed)
- {
+ if (opt_display && opt_click_allowed) {
double u = 0.0;
- while (u <= 1)
- {
+ while (u <= 1) {
pt = Nurbs.computeCurvePoint(u);
- vpDisplay::displayCross(I3,pt,4,vpColor::red);
- u+=0.01;
+ vpDisplay::displayCross(I3, pt, 4, vpColor::red);
+ u += 0.01;
}
- for(std::list<vpImagePoint>::const_iterator it=controlPoints.begin(); it!=controlPoints.end(); ++it){
+ for (std::list<vpImagePoint>::const_iterator it = controlPoints.begin(); it != controlPoints.end(); ++it) {
vpDisplay::displayCross(I3, *it, 4, vpColor::green);
}
- vpDisplay::flush(I3) ;
+ vpDisplay::flush(I3);
vpDisplay::getClick(I3);
}
- if (N != NULL) delete[] N;
- if (N2 != NULL)
- {
+ if (N != NULL)
+ delete[] N;
+ if (N2 != NULL) {
for (int j = 0; j <= 2; j++)
delete[] N2[j];
delete[] N2;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
@@ -373,12 +367,9 @@ main(int argc, const char ** argv)
#else
int main()
{
- std::cout << "This example requires a video device. "
- << std::endl
- << "You should install X11, GTK, OpenCV, GDI or Direct3D"
- << std::endl
- << "to be able to execute this example."
- << std::endl;
+ std::cout << "This example requires a video device. " << std::endl
+ << "You should install X11, GTK, OpenCV, GDI or Direct3D" << std::endl
+ << "to be able to execute this example." << std::endl;
return 0;
}
#endif
diff --git a/modules/tracker/tt/CMakeLists.txt b/modules/tracker/tt/CMakeLists.txt
index 40fa7b3..22f4e04 100644
--- a/modules/tracker/tt/CMakeLists.txt
+++ b/modules/tracker/tt/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h
index 5a125c3..8795711 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTracker.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,227 +47,254 @@
#include <math.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/tt/vpTemplateTrackerZone.h>
#include <visp3/tt/vpTemplateTrackerWarp.h>
-#include <visp3/core/vpImageFilter.h>
+#include <visp3/tt/vpTemplateTrackerZone.h>
/*!
\class vpTemplateTracker
\ingroup group_tt_tracker
- This class allows to instanciate a template tracker using image registration algorithms \cite Dame10c \cite Dame11c.
+ This class allows to instanciate a template tracker using image registration
+ algorithms \cite Dame10c \cite Dame11c.
*/
class VISP_EXPORT vpTemplateTracker
{
- protected:
- //traitement pyramidal
- unsigned int nbLvlPyr; // If = 1, disable pyramidal usage
- unsigned int l0Pyr;
- bool pyrInitialised;
-
- vpTemplateTrackerPoint *ptTemplate;
- vpTemplateTrackerPoint **ptTemplatePyr;
- bool ptTemplateInit;
- unsigned int templateSize;
- unsigned int *templateSizePyr;
- bool *ptTemplateSelect;
- bool **ptTemplateSelectPyr;
- bool ptTemplateSelectInit;
- unsigned int templateSelectSize;
+protected:
+ // traitement pyramidal
+ unsigned int nbLvlPyr; // If = 1, disable pyramidal usage
+ unsigned int l0Pyr;
+ bool pyrInitialised;
+
+ vpTemplateTrackerPoint *ptTemplate;
+ vpTemplateTrackerPoint **ptTemplatePyr;
+ bool ptTemplateInit;
+ unsigned int templateSize;
+ unsigned int *templateSizePyr;
+ bool *ptTemplateSelect;
+ bool **ptTemplateSelectPyr;
+ bool ptTemplateSelectInit;
+ unsigned int templateSelectSize;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; // pour inverse et compo
+ vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; // pour inverse et
+ // compo
+#endif
+
+ vpTemplateTrackerPointCompo *ptTemplateCompo; // pour ESM
+ vpTemplateTrackerPointCompo **ptTemplateCompoPyr; // pour ESM
+ vpTemplateTrackerZone *zoneTracked;
+ vpTemplateTrackerZone *zoneTrackedPyr;
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- vpTemplateTrackerPointSuppMIInv *ptTemplateSupp; //pour inverse et compo
- vpTemplateTrackerPointSuppMIInv **ptTemplateSuppPyr; //pour inverse et compo
- #endif
+ vpImage<unsigned char> *pyr_IDes;
- vpTemplateTrackerPointCompo *ptTemplateCompo; //pour ESM
- vpTemplateTrackerPointCompo **ptTemplateCompoPyr; //pour ESM
- vpTemplateTrackerZone *zoneTracked;
- vpTemplateTrackerZone *zoneTrackedPyr;
-
- vpImage<unsigned char> *pyr_IDes;
-
- vpMatrix H;
- vpMatrix Hdesire;
- vpMatrix *HdesirePyr;
- vpMatrix HLM;
- vpMatrix HLMdesire;
- vpMatrix *HLMdesirePyr;
- vpMatrix HLMdesireInverse;
- vpMatrix *HLMdesireInversePyr;
- vpColVector G;
-
- double gain;
- double thresholdGradient;
- bool costFunctionVerification;
- bool blur;
- bool useBrent;
- unsigned int nbIterBrent;
- unsigned int taillef;
- double *fgG;
- double *fgdG;
- double ratioPixelIn;
- int mod_i;
- int mod_j;//variable de sampling de zone de reference
- unsigned int nbParam ;
- double lambdaDep ;
- unsigned int iterationMax ;
- //pour BFGS
- unsigned int iterationGlobale;
- //diverge is set to true if there is no more point in the tracked area
- bool diverge;
- unsigned int nbIteration;
- bool useCompositionnal;
- bool useInverse;
+ vpMatrix H;
+ vpMatrix Hdesire;
+ vpMatrix *HdesirePyr;
+ vpMatrix HLM;
+ vpMatrix HLMdesire;
+ vpMatrix *HLMdesirePyr;
+ vpMatrix HLMdesireInverse;
+ vpMatrix *HLMdesireInversePyr;
+ vpColVector G;
- vpTemplateTrackerWarp *Warp;
- //Parametre de deplacement
- vpColVector p;
- vpColVector dp;
+ double gain;
+ double thresholdGradient;
+ bool costFunctionVerification;
+ bool blur;
+ bool useBrent;
+ unsigned int nbIterBrent;
+ unsigned int taillef;
+ double *fgG;
+ double *fgdG;
+ double ratioPixelIn;
+ int mod_i;
+ int mod_j; // variable de sampling de zone de reference
+ unsigned int nbParam;
+ double lambdaDep;
+ unsigned int iterationMax;
+ // pour BFGS
+ unsigned int iterationGlobale;
+ // diverge is set to true if there is no more point in the tracked area
+ bool diverge;
+ unsigned int nbIteration;
+ bool useCompositionnal;
+ bool useInverse;
- //temporary values for warping
- vpColVector X1;
- vpColVector X2;
- //temporary derivative matrix
- vpMatrix dW;
+ vpTemplateTrackerWarp *Warp;
+ // Parametre de deplacement
+ vpColVector p;
+ vpColVector dp;
- vpImage<double> BI;
- vpImage<double> dIx ;
- vpImage<double> dIy ;
- vpTemplateTrackerZone zoneRef_; // Reference zone
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpTemplateTracker(const vpTemplateTracker &)
-// : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
-// ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
-// ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0),
-// ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
-// zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(NULL),
-// HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
-// G(), gain(0), thresholdGradient(0), costFunctionVerification(false),
-// blur(false), useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
-// ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0), iterationMax(0),
-// iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(false),
-// useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpTemplateTracker &operator=(const vpTemplateTracker &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // temporary values for warping
+ vpColVector X1;
+ vpColVector X2;
+ // temporary derivative matrix
+ vpMatrix dW;
- public:
- //! Default constructor.
- vpTemplateTracker()
- : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
- ptTemplateInit(false), templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL),
- ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false), templateSelectSize(0),
- ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL), ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
- zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(NULL),
- HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
- G(), gain(0), thresholdGradient(0), costFunctionVerification(false),
- blur(false), useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
- ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0), iterationMax(0),
- iterationGlobale(0), diverge(false), nbIteration(0), useCompositionnal(false),
- useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(), zoneRef_()
- {}
- vpTemplateTracker(vpTemplateTrackerWarp *_warp);
- virtual ~vpTemplateTracker();
-
- void display(const vpImage<unsigned char> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
- void display(const vpImage<vpRGBa> &I, const vpColor& col = vpColor::green, const unsigned int thickness=3);
+ vpImage<double> BI;
+ vpImage<double> dIx;
+ vpImage<double> dIy;
+ vpTemplateTrackerZone zoneRef_; // Reference zone
- bool getDiverge() const {return diverge;}
- vpColVector getdp(){ return dp; }
- vpColVector getG() const { return G; }
- vpMatrix getH() const { return H; }
- unsigned int getNbParam() const { return nbParam ; }
- unsigned int getNbIteration() const { return nbIteration; }
- vpColVector getp() const { return p;}
- double getRatioPixelIn() const {return ratioPixelIn;}
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpTemplateTracker(const vpTemplateTracker &)
+ // : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL),
+ // ptTemplatePyr(NULL),
+ // ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
+ // ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
+ // ptTemplateSelectInit(false), templateSelectSize(0),
+ // ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
+ // ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL),
+ // zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
+ // Hdesire(), HdesirePyr(NULL), HLM(), HLMdesire(),
+ // HLMdesirePyr(NULL), HLMdesireInverse(), HLMdesireInversePyr(NULL),
+ // G(), gain(0), thresholdGradient(0),
+ // costFunctionVerification(false), blur(false), useBrent(false),
+ // nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL),
+ // ratioPixelIn(0), mod_i(0), mod_j(0), nbParam(), lambdaDep(0),
+ // iterationMax(0), iterationGlobale(0), diverge(false),
+ // nbIteration(0), useCompositionnal(false), useInverse(false),
+ // Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(),
+ // zoneRef_()
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpTemplateTracker &operator=(const vpTemplateTracker &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
- /*!
+public:
+ //! Default constructor.
+ vpTemplateTracker()
+ : nbLvlPyr(0), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL), ptTemplateInit(false),
+ templateSize(0), templateSizePyr(NULL), ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL),
+ ptTemplateSelectInit(false), templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
+ ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL), pyr_IDes(NULL), H(),
+ Hdesire(), HdesirePyr(NULL), HLM(), HLMdesire(), HLMdesirePyr(NULL), HLMdesireInverse(),
+ HLMdesireInversePyr(NULL), G(), gain(0), thresholdGradient(0), costFunctionVerification(false), blur(false),
+ useBrent(false), nbIterBrent(0), taillef(0), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(0), mod_j(0),
+ nbParam(), lambdaDep(0), iterationMax(0), iterationGlobale(0), diverge(false), nbIteration(0),
+ useCompositionnal(false), useInverse(false), Warp(NULL), p(), dp(), X1(), X2(), dW(), BI(), dIx(), dIy(),
+ zoneRef_()
+ {
+ }
+ explicit vpTemplateTracker(vpTemplateTrackerWarp *_warp);
+ virtual ~vpTemplateTracker();
- \return The pointer to the warper.
- */
- vpTemplateTrackerWarp *getWarp() const {return Warp;}
+ void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
+ void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
- /*!
- Return the reference template zone.
- */
- vpTemplateTrackerZone getZoneRef() const { return zoneRef_; }
+ bool getDiverge() const { return diverge; }
+ vpColVector getdp() { return dp; }
+ vpColVector getG() const { return G; }
+ vpMatrix getH() const { return H; }
+ unsigned int getNbParam() const { return nbParam; }
+ unsigned int getNbIteration() const { return nbIteration; }
+ vpColVector getp() const { return p; }
+ double getRatioPixelIn() const { return ratioPixelIn; }
- void initClick(const vpImage<unsigned char> &I, bool delaunay=false);
- void initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay=false);
- void initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone);
-
- void resetTracker();
-
- void setBlur(bool b){blur = b;}
- void setCostFunctionVerification(bool b){costFunctionVerification = b;}
- void setGain(double g){gain=g;}
- void setGaussianFilterSize(unsigned int new_taill);
- void setHDes(vpMatrix &tH){ Hdesire=tH; vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire); HLMdesireInverse = HLMdesire.inverseByLU();}
- /*!
- Set the maximum number of iteration of the estimation scheme.
- \param n : Maximum number of iterations to stop the estimation scheme. A typical value is arround 100.
- */
- void setIterationMax(const unsigned int &n) { iterationMax = n ; }
- /*!
- Set the convergence gain used in the estimation scheme.
- \param l : Gain. A typical value is 0.001.
- */
- void setLambda(double l) { lambdaDep = l ; }
- void setNbIterBrent(const unsigned int &b){nbIterBrent=b;}
- void setp(const vpColVector &tp){ p=tp; diverge=false; iterationGlobale=0; }
- /*!
- Set the number of pyramid levels used in the multi-resolution scheme.
- If \e nlevels > 1, the tracker uses a pyramidal approach.
+ /*!
- \param nlevels : Number of pyramid levels. Algorithm starts at level nlevels-1.
- \param level_to_stop : Last level of the pyramid that will be considered. Lowest level is zero.
- */
- void setPyramidal(unsigned int nlevels=2, unsigned int level_to_stop=1) {
- nbLvlPyr = nlevels;
- l0Pyr = level_to_stop;
- if(l0Pyr >= nlevels){
- std::cout << "Warning: level_to_stop: " << level_to_stop << " higher than level_to_start: " << nlevels-1 << " (nlevels-1)" <<std::endl;
- std::cout << "Level to stop put to: " << nlevels-1 << std::endl;
- l0Pyr = nlevels-1;
- }
+ \return The pointer to the warper.
+ */
+ vpTemplateTrackerWarp *getWarp() const { return Warp; }
+
+ /*!
+ Return the reference template zone.
+ */
+ vpTemplateTrackerZone getZoneRef() const { return zoneRef_; }
+
+ void initClick(const vpImage<unsigned char> &I, bool delaunay = false);
+ void initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &v_ip, bool delaunay = false);
+ void initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone &zone);
+
+ void resetTracker();
+
+ void setBlur(bool b) { blur = b; }
+ void setCostFunctionVerification(bool b) { costFunctionVerification = b; }
+ void setGain(double g) { gain = g; }
+ void setGaussianFilterSize(unsigned int new_taill);
+ void setHDes(vpMatrix &tH)
+ {
+ Hdesire = tH;
+ vpMatrix::computeHLM(Hdesire, lambdaDep, HLMdesire);
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ }
+ /*!
+ Set the maximum number of iteration of the estimation scheme.
+ \param n : Maximum number of iterations to stop the estimation scheme. A
+ typical value is arround 100.
+ */
+ void setIterationMax(const unsigned int &n) { iterationMax = n; }
+ /*!
+ Set the convergence gain used in the estimation scheme.
+ \param l : Gain. A typical value is 0.001.
+ */
+ void setLambda(double l) { lambdaDep = l; }
+ void setNbIterBrent(const unsigned int &b) { nbIterBrent = b; }
+ void setp(const vpColVector &tp)
+ {
+ p = tp;
+ diverge = false;
+ iterationGlobale = 0;
+ }
+ /*!
+ Set the number of pyramid levels used in the multi-resolution scheme.
+ If \e nlevels > 1, the tracker uses a pyramidal approach.
+
+ \param nlevels : Number of pyramid levels. Algorithm starts at level
+ nlevels-1. \param level_to_stop : Last level of the pyramid that will be
+ considered. Lowest level is zero.
+ */
+ void setPyramidal(unsigned int nlevels = 2, unsigned int level_to_stop = 1)
+ {
+ nbLvlPyr = nlevels;
+ l0Pyr = level_to_stop;
+ if (l0Pyr >= nlevels) {
+ std::cout << "Warning: level_to_stop: " << level_to_stop << " higher than level_to_start: " << nlevels - 1
+ << " (nlevels-1)" << std::endl;
+ std::cout << "Level to stop put to: " << nlevels - 1 << std::endl;
+ l0Pyr = nlevels - 1;
}
- /*!
- Set the pixel sampling parameters along the rows and the columns.
- \param sample_i : Sampling factor along the rows.
- If 1 all the lines are considered. If 2, consider one line over two.
+ }
+ /*!
+ Set the pixel sampling parameters along the rows and the columns.
+ \param sample_i : Sampling factor along the rows.
+ If 1 all the lines are considered. If 2, consider one line over two.
+
+ \param sample_j : Sampling factor along the columns.
+ If 1 all the columns are considered. If 2, consider one column over two.
+ */
+ void setSampling(int sample_i, int sample_j)
+ {
+ mod_i = sample_i;
+ mod_j = sample_j;
+ }
+ void setThresholdGradient(double threshold) { thresholdGradient = threshold; }
+ /*! By default Brent usage is disabled. */
+ void setUseBrent(bool b) { useBrent = b; }
- \param sample_j : Sampling factor along the columns.
- If 1 all the columns are considered. If 2, consider one column over two.
- */
- void setSampling(int sample_i,int sample_j){mod_i=sample_i; mod_j=sample_j;}
- void setThresholdGradient(double threshold){thresholdGradient=threshold;}
- /*! By default Brent usage is disabled. */
- void setUseBrent(bool b){useBrent = b;}
-
- void track(const vpImage<unsigned char> &I);
- void trackRobust(const vpImage<unsigned char> &I);
-
- protected:
+ void track(const vpImage<unsigned char> &I);
+ void trackRobust(const vpImage<unsigned char> &I);
- void computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha);
- virtual double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) = 0;
- void getGaussianBluredImage(const vpImage<unsigned char> &I){ vpImageFilter::filter(I, BI,fgG,taillef); }
- virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
- virtual void initHessienDesiredPyr(const vpImage<unsigned char> &I);
- virtual void initPyramidal(unsigned int nbLvl,unsigned int l0);
- void initTracking(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
- virtual void initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone);
- virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
- virtual void trackPyr(const vpImage<unsigned char> &I);
+protected:
+ void computeOptimalBrentGain(const vpImage<unsigned char> &I, vpColVector &tp, double tMI, vpColVector &direction,
+ double &alpha);
+ virtual double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) = 0;
+ void getGaussianBluredImage(const vpImage<unsigned char> &I) { vpImageFilter::filter(I, BI, fgG, taillef); }
+ virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
+ virtual void initHessienDesiredPyr(const vpImage<unsigned char> &I);
+ virtual void initPyramidal(unsigned int nbLvl, unsigned int l0);
+ void initTracking(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone);
+ virtual void initTrackingPyr(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone);
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
+ virtual void trackPyr(const vpImage<unsigned char> &I);
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h
index d95b403..cfef5f2 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerBSpline.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h
index b4f36e0..f86352e 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerHeader.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,53 +52,54 @@
\ingroup group_tt_tools
*/
struct vpTemplateTrackerZPoint {
- int x,y;
+ int x, y;
- vpTemplateTrackerZPoint() : x(0), y(0) {}
+ vpTemplateTrackerZPoint() : x(0), y(0) {}
};
/*!
\struct vpTemplateTrackerDPoint
\ingroup group_tt_tools
*/
struct vpTemplateTrackerDPoint {
- double x,y;
+ double x, y;
- vpTemplateTrackerDPoint() : x(0), y(0) {}
+ vpTemplateTrackerDPoint() : x(0), y(0) {}
};
/*!
\struct vpTemplateTrackerPoint
\ingroup group_tt_tools
*/
struct vpTemplateTrackerPoint {
- int x,y;
- double dx,dy;
- double val;
- double *dW;
- double *HiG;
+ int x, y;
+ double dx, dy;
+ double val;
+ double *dW;
+ double *HiG;
- vpTemplateTrackerPoint() : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) {}
+ vpTemplateTrackerPoint() : x(0), y(0), dx(0), dy(0), val(0), dW(NULL), HiG(NULL) {}
};
/*!
\struct vpTemplateTrackerPointCompo
\ingroup group_tt_tools
*/
struct vpTemplateTrackerPointCompo {
- double *dW;
- vpTemplateTrackerPointCompo() : dW(NULL) {}
+ double *dW;
+ vpTemplateTrackerPointCompo() : dW(NULL) {}
};
#ifndef DOXYGEN_SHOULD_SKIP_THIS
struct vpTemplateTrackerPointSuppMIInv {
- double et;
- int ct;
- double *BtInit;
- double *Bt;
- double *dBt;
- double *d2W;
- double *d2Wx;
- double *d2Wy;
- vpTemplateTrackerPointSuppMIInv() : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL),
- d2W(NULL), d2Wx(NULL), d2Wy(NULL) {}
+ double et;
+ int ct;
+ double *BtInit;
+ double *Bt;
+ double *dBt;
+ double *d2W;
+ double *d2Wx;
+ double *d2Wy;
+ vpTemplateTrackerPointSuppMIInv() : et(0), ct(0), BtInit(NULL), Bt(NULL), dBt(NULL), d2W(NULL), d2Wx(NULL), d2Wy(NULL)
+ {
+ }
};
#endif // DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h
index 84969c6..e1f20e1 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSD.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,38 +47,36 @@
#include <math.h>
-#include <visp3/tt/vpTemplateTracker.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpMath.h>
+#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/vision/vpHomography.h>
/*!
\class vpTemplateTrackerSSD
\ingroup group_tt_tracker
*/
-class VISP_EXPORT vpTemplateTrackerSSD: public vpTemplateTracker
+class VISP_EXPORT vpTemplateTrackerSSD : public vpTemplateTracker
{
- protected:
- vpRowVector DI;
- vpRowVector temp;
+protected:
+ vpRowVector DI;
+ vpRowVector temp;
- protected:
- double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
- double getCost(const vpImage<unsigned char> &I){ return getCost(I,p); }
- virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
- virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
+protected:
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getCost(const vpImage<unsigned char> &I) { return getCost(I, p); }
+ virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
- public:
- vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp);
+public:
+ explicit vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp);
- double getSSD(const vpImage<unsigned char> &I, const vpColVector &tp);
- void setGain(double g){ gain=g; }
+ double getSSD(const vpImage<unsigned char> &I, const vpColVector &tp);
+ void setGain(double g) { gain = g; }
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h
index ae085c6..63e65f8 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDESM.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,23 +51,23 @@
\class vpTemplateTrackerSSDESM
\ingroup group_tt_tracker
*/
-class VISP_EXPORT vpTemplateTrackerSSDESM: public vpTemplateTrackerSSD
+class VISP_EXPORT vpTemplateTrackerSSDESM : public vpTemplateTrackerSSD
{
- protected:
- bool compoInitialised;
- vpMatrix HDir;
- vpMatrix HInv;
- vpMatrix HLMDir;
- vpMatrix HLMInv;
- vpColVector GDir;
- vpColVector GInv;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompInverse(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
+protected:
+ bool compoInitialised;
+ vpMatrix HDir;
+ vpMatrix HInv;
+ vpMatrix HLMDir;
+ vpMatrix HLMInv;
+ vpColVector GDir;
+ vpColVector GInv;
- public:
- vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp);
+protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+
+public:
+ explicit vpTemplateTrackerSSDESM(vpTemplateTrackerWarp *warp);
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h
index 9d4f556..9b60e2e 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardAdditional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,34 +49,29 @@
/*!
\ingroup group_tt_tracker
- The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a.
+ 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:
+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;
- //valeur pour calculer Quasi_Newton
- vpColVector p_prec;
- vpColVector G_prec;
- vpMatrix KQuasiNewton;
+private:
+ vpMinimizationTypeSSDForwardAdditional minimizationMethod;
+ // valeur pour calculer Quasi_Newton
+ vpColVector p_prec;
+ vpColVector G_prec;
+ vpMatrix KQuasiNewton;
- protected:
- void initHessienDesired(const vpImage<unsigned char> &/*I*/){}
- void trackNoPyr(const vpImage<unsigned char> &I);
+protected:
+ void initHessienDesired(const vpImage<unsigned char> & /*I*/) {}
+ void trackNoPyr(const vpImage<unsigned char> &I);
- public:
- vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp);
+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 b606224..4848f4b 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDForwardCompositional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,19 +49,20 @@
/*!
\ingroup group_tt_tracker
- The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a.
+ 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;
+protected:
+ bool compoInitialised;
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompo(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
+protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompo(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
- public:
- vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp);
+public:
+ 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 2cfcffe..01cba4c 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerSSDInverseCompositional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,35 +50,37 @@
/*!
\ingroup group_tt_tracker
- The algorithm implemented in this class is described in \cite Baker04a and \cite Marchand16a.
+ 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;
- vpMatrix HInv;
- vpMatrix HCompInverse;
- bool useTemplateSelect;//use only the strong gradient pixels to compute the Jabocian
- //pour eval evolRMS
- double evolRMS;
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- double threshold_RMS;
-
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void initCompInverse(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
- void deletePosEvalRMS();
- void computeEvalRMS(const vpColVector &p);
- void initPosEvalRMS(const vpColVector &p);
+protected:
+ bool compoInitialised;
+ vpMatrix HInv;
+ vpMatrix HCompInverse;
+ bool useTemplateSelect; // use only the strong gradient pixels to compute
+ // the Jabocian
+ // pour eval evolRMS
+ double evolRMS;
+ std::vector<double> x_pos;
+ std::vector<double> y_pos;
+ double threshold_RMS;
- public:
- vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp);
+protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
- /*! 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;}
+public:
+ explicit vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp);
+
+ /*! Use only the strong gradient pixels to compute the Jabobian. By default
+ * this feature is disabled. */
+ void setUseTemplateSelect(bool b) { useTemplateSelect = b; }
+ void setThresholdRMS(double threshold) { threshold_RMS = threshold; }
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h
index 6df8c7e..10618da 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerTriangle.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,10 +48,10 @@
#include <assert.h>
#include <vector>
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
/*!
@@ -59,109 +60,109 @@
*/
class VISP_EXPORT vpTemplateTrackerTriangle
{
- protected:
- double minx_temp;
- double miny_temp;
- vpTemplateTrackerDPoint C1; //! Corner 1
- vpTemplateTrackerDPoint C2; //! Corner 2
- vpTemplateTrackerDPoint C3; //! Corner 3
-
- double l_t;
- double h_t;
-
- bool not_good;
- double uvinv00;
- double uvinv01;
- double uvinv10;
- double uvinv11;
- double marge_triangle;
- double area;
-
- private:
- vpColVector getCorner1() const;
- vpColVector getCorner2() const;
- vpColVector getCorner3() const;
-
- public:
- vpTemplateTrackerTriangle();
- vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle& T);
- vpTemplateTrackerTriangle(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3);
- vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
- vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
- vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3);
-
- /*!
- Return the area of the triangle.
-
- \return The area of the triangle.
- */
- inline double getArea() const{
- return this->area;
+protected:
+ double minx_temp;
+ double miny_temp;
+ vpTemplateTrackerDPoint C1; //! Corner 1
+ vpTemplateTrackerDPoint C2; //! Corner 2
+ vpTemplateTrackerDPoint C3; //! Corner 3
+
+ double l_t;
+ double h_t;
+
+ bool not_good;
+ double uvinv00;
+ double uvinv01;
+ double uvinv10;
+ double uvinv11;
+ double marge_triangle;
+ double area;
+
+private:
+ vpColVector getCorner1() const;
+ vpColVector getCorner2() const;
+ vpColVector getCorner3() const;
+
+public:
+ vpTemplateTrackerTriangle();
+ vpTemplateTrackerTriangle(const vpTemplateTrackerTriangle &T);
+ vpTemplateTrackerTriangle(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3);
+ vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
+ vpTemplateTrackerTriangle(int x1, int y1, int x2, int y2, int x3, int y3);
+ vpTemplateTrackerTriangle(double x1, double y1, double x2, double y2, double x3, double y3);
+
+ /*!
+ Return the area of the triangle.
+
+ \return The area of the triangle.
+ */
+ inline double getArea() const { return this->area; }
+
+ vpTemplateTrackerTriangle getPyramidDown() const;
+ void getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const;
+ void getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const;
+ void getCorners(std::vector<vpImagePoint> &c) const;
+
+ /*!
+ Returns the coordinates of a triangle corner.
+ \param i : Allowed values are 0, 1 or 2.
+ \return
+ - if i = 0, return corner 1 coordinates,
+ - if i = 1, return corner 2 coordinates,
+ - if i = 2, return corner 3 coordinates.
+
+ The coordinates are returned as a 2 dimension vector (x,y).
+ */
+ vpColVector getCorner(unsigned int i) const
+ {
+ assert(i < 3);
+ if (i == 0)
+ return getCorner1();
+ else if (i == 1)
+ return getCorner2();
+ else /*if(i==2)*/
+ return getCorner3();
+ };
+ /*!
+ Returns the coordinates of a triangle corner.
+ \param i : Allowed values are 0, 1 or 2.
+ - if i = 0, return corner 1 coordinates,
+ - if i = 1, return corner 2 coordinates,
+ - if i = 2, return corner 3 coordinates.
+ \param x : The coordinate of the corner along horizontal axis.
+ \param y : The coordinate of the corner along vertical axis.
+ */
+ void getCorner(unsigned int i, double &x, double &y) const
+ {
+ assert(i < 3);
+ if (i == 0) {
+ x = C1.x;
+ y = C1.y;
+ } else if (i == 1) {
+ x = C2.x;
+ y = C2.y;
+ } else /*if(i==2)*/ {
+ x = C3.x;
+ y = C3.y;
}
+ };
- vpTemplateTrackerTriangle getPyramidDown() const;
- void getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const;
- void getCorners(vpImagePoint &c1, vpImagePoint &c2, vpImagePoint &c3) const;
- void getCorners(std::vector<vpImagePoint> &c) const;
-
- /*!
- Returns the coordinates of a triangle corner.
- \param i : Allowed values are 0, 1 or 2.
- \return
- - if i = 0, return corner 1 coordinates,
- - if i = 1, return corner 2 coordinates,
- - if i = 2, return corner 3 coordinates.
-
- The coordinates are returned as a 2 dimension vector (x,y).
- */
- vpColVector getCorner(unsigned int i) const {
- assert(i<3);
- if(i==0) return getCorner1();
- else if(i==1) return getCorner2();
- else /*if(i==2)*/ return getCorner3();
- };
- /*!
- Returns the coordinates of a triangle corner.
- \param i : Allowed values are 0, 1 or 2.
- - if i = 0, return corner 1 coordinates,
- - if i = 1, return corner 2 coordinates,
- - if i = 2, return corner 3 coordinates.
- \param x : The coordinate of the corner along horizontal axis.
- \param y : The coordinate of the corner along vertical axis.
- */
- void getCorner(unsigned int i, double &x, double &y) const {
- assert(i<3);
- if(i==0) {
- x = C1.x;
- y = C1.y;
- }
- else if(i==1) {
- x = C2.x;
- y = C2.y;
- }
- else /*if(i==2)*/ {
- x = C3.x;
- y = C3.y;
- }
- };
-
- double getMaxx() const;
- double getMaxy() const;
- double getMinx() const;
- double getMiny() const;
-
- void getSize(double &w, double &h) const;
- void getSize(int &w, int &h) const;
-
- void init(const vpColVector &c1,const vpColVector &c2,const vpColVector &c3);
- void init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
- void init(int x1,int y1, int x2,int y2, int x3,int y3);
- void init(double x1,double y1, double x2,double y2, double x3,double y3);
- bool inTriangle(const vpImagePoint &ip) const;
- bool inTriangle(const int &i, const int &j) const;
- bool inTriangle(const double &i,const double &j) const;
-
- vpTemplateTrackerTriangle & operator=(const vpTemplateTrackerTriangle& T);
+ double getMaxx() const;
+ double getMaxy() const;
+ double getMinx() const;
+ double getMiny() const;
+
+ void getSize(double &w, double &h) const;
+ void getSize(int &w, int &h) const;
+
+ void init(const vpColVector &c1, const vpColVector &c2, const vpColVector &c3);
+ void init(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3);
+ void init(int x1, int y1, int x2, int y2, int x3, int y3);
+ void init(double x1, double y1, double x2, double y2, double x3, double y3);
+ bool inTriangle(const vpImagePoint &ip) const;
+ bool inTriangle(const int &i, const int &j) const;
+ bool inTriangle(const double &i, const double &j) const;
+
+ vpTemplateTrackerTriangle &operator=(const vpTemplateTrackerTriangle &T);
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h
index 79de224..272a1b8 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarp.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,15 +42,14 @@
\brief
*/
-
#ifndef vpTemplateTrackerWarp_hh
#define vpTemplateTrackerWarp_hh
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpTrackingException.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
#include <visp3/tt/vpTemplateTrackerTriangle.h>
#include <visp3/tt/vpTemplateTrackerZone.h>
-#include <visp3/core/vpTrackingException.h>
/*!
\class vpTemplateTrackerWarp
@@ -57,190 +57,201 @@
*/
class VISP_EXPORT vpTemplateTrackerWarp
{
- protected:
- double denom;
- vpMatrix dW;
- unsigned int nbParam ;
-
- public:
- //constructor;
- vpTemplateTrackerWarp() : denom(1.), dW(), nbParam(0) {}
- virtual ~vpTemplateTrackerWarp(){}
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- virtual void computeCoeff(const vpColVector &p)=0;
- virtual void computeDenom(vpColVector &vX, const vpColVector &ParamM)=0;
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- virtual void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW) = 0;
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- virtual void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW) = 0;
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
- #endif
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- virtual void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW) = 0;
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- virtual void getdWdp0(const int &i,const int &j,double *dIdW) = 0;
-
- /*!
- Compute the distance between a zone and its associated warped zone.
-
- \param Z : Zone to consider.
- \param p : Parameters of the warping function.
- */
- double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z,const vpColVector &p);
-
- /*!
- Get the number of parameters of the warping function.
-
- \return Number of parameters.
- */
- unsigned int getNbParam() const {return nbParam;}
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- virtual void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const = 0;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- virtual void getParamPyramidDown(const vpColVector &p,vpColVector &pdown) =0;
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- virtual void getParamPyramidUp(const vpColVector &p,vpColVector &pup) =0;
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- virtual bool isESMcompatible() const =0;
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- virtual void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const = 0;
-
- /*!
- Set the number of parameters of the warping function.
-
- \param nb : New number of parameters.
- */
- void setNbParam(unsigned int nb){nbParam=nb;dW.resize(2,nbParam);}
-
- /*!
- Warp a list of points.
-
- \param ut0 : List of u coordinates of the points.
- \param vt0 : List of v coordinates of the points.
- \param nb_pt : Number of points to consider.
- \param p : Parameters of the warp.
- \param u : Resulting u coordinates.
- \param v : resulting v coordinates.
- */
- void warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warp.
- */
- virtual void warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM) = 0;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- virtual void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- virtual void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM) = 0;
-
- /*!
- Warp a triangle and store the result in a new zone.
-
- \param in : Triangle to warp.
- \param p : Parameters of the warping function. These parameters are estimated by the template
- tracker and returned using vpTemplateTracker::getp().
- \param out : Resulting triangle.
- */
- void warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out);
-
- /*!
- Warp a zone and store the result in a new zone.
-
- \param in : Zone to warp.
- \param p : Parameters of the warping function. These parameters are estimated by the template
- tracker and returned using vpTemplateTracker::getp().
- \param out : Resulting zone.
- */
- void warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out);
+protected:
+ double denom;
+ vpMatrix dW;
+ unsigned int nbParam;
+
+public:
+ // constructor;
+ vpTemplateTrackerWarp() : denom(1.), dW(), nbParam(0) {}
+ virtual ~vpTemplateTrackerWarp() {}
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ virtual void computeCoeff(const vpColVector &p) = 0;
+ virtual void computeDenom(vpColVector &vX, const vpColVector &ParamM) = 0;
+#endif
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ virtual void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW) = 0;
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ virtual void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW) = 0;
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p);
+#endif
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
+ */
+ virtual void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW) = 0;
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ virtual void getdWdp0(const int &i, const int &j, double *dIdW) = 0;
+
+ /*!
+ Compute the distance between a zone and its associated warped zone.
+
+ \param Z : Zone to consider.
+ \param p : Parameters of the warping function.
+ */
+ double getDistanceBetweenZoneAndWarpedZone(const vpTemplateTrackerZone &Z, const vpColVector &p);
+
+ /*!
+ Get the number of parameters of the warping function.
+
+ \return Number of parameters.
+ */
+ unsigned int getNbParam() const { return nbParam; }
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ virtual void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const = 0;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ virtual void getParamPyramidDown(const vpColVector &p, vpColVector &pdown) = 0;
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ virtual void getParamPyramidUp(const vpColVector &p, vpColVector &pup) = 0;
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ virtual bool isESMcompatible() const = 0;
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ virtual void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const = 0;
+
+ /*!
+ Set the number of parameters of the warping function.
+
+ \param nb : New number of parameters.
+ */
+ void setNbParam(unsigned int nb)
+ {
+ nbParam = nb;
+ dW.resize(2, nbParam);
+ }
+
+ /*!
+ Warp a list of points.
+
+ \param ut0 : List of u coordinates of the points.
+ \param vt0 : List of v coordinates of the points.
+ \param nb_pt : Number of points to consider.
+ \param p : Parameters of the warp.
+ \param u : Resulting u coordinates.
+ \param v : resulting v coordinates.
+ */
+ void warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u, double *v);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warp.
+ */
+ virtual void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM) = 0;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ virtual void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM) = 0;
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ virtual void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM) = 0;
+
+ /*!
+ Warp a triangle and store the result in a new zone.
+
+ \param in : Triangle to warp.
+ \param p : Parameters of the warping function. These parameters are
+ estimated by the template tracker and returned using
+ vpTemplateTracker::getp(). \param out : Resulting triangle.
+ */
+ void warpTriangle(const vpTemplateTrackerTriangle &in, const vpColVector &p, vpTemplateTrackerTriangle &out);
+
+ /*!
+ Warp a zone and store the result in a new zone.
+
+ \param in : Zone to warp.
+ \param p : Parameters of the warping function. These parameters are
+ estimated by the template tracker and returned using
+ vpTemplateTracker::getp(). \param out : Resulting zone.
+ */
+ void warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out);
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h
index 21dc7fc..3f12678 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpAffine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,10 +39,10 @@
*****************************************************************************/
/*!
\file vpTemplateTrackerWarpAffine.h
- \brief Affine warping function: w(X)=AX+b with: A=[[1+p0, p2], [p1, 1+p3]] and b= [p4, p5]]
+ \brief Affine warping function: w(X)=AX+b with: A=[[1+p0, p2], [p1, 1+p3]]
+ and b= [p4, p5]]
*/
-
#ifndef vpTemplateTrackerWarpAffine_hh
#define vpTemplateTrackerWarpAffine_hh
@@ -51,125 +52,134 @@
\class vpTemplateTrackerWarpAffine
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpAffine: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpAffine : public vpTemplateTrackerWarp
{
- public:
- //constructor;
- vpTemplateTrackerWarpAffine();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the columns) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+public:
+ // constructor;
+ vpTemplateTrackerWarpAffine();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector & /*p*/) {}
+ void computeDenom(vpColVector & /*vX*/, const vpColVector & /*ParamM*/) {}
+#endif
+
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis of the
+ point (i,j). \param dIdW : Resulting derivative matrix (Image according to
+ the warping function).
+ */
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const int &i, const int &j, double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const { return false; }
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the columns) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h
index b0d66a9..4bd2431 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomography.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,10 +39,10 @@
*****************************************************************************/
/*!
\file vpTemplateTrackerWarpHomography.h
- \brief warping function of an homography: the homography is directly defined by the diplacement parameter: H=[[1+p0, p3, p6], [p1, 1+p4, p7], [p2, p5, 1]]
+ \brief warping function of an homography: the homography is directly defined
+ by the diplacement parameter: H=[[1+p0, p3, p6], [p1, 1+p4, p7], [p2, p5, 1]]
*/
-
#ifndef vpTemplateTrackerWarpHomography_hh
#define vpTemplateTrackerWarpHomography_hh
@@ -52,149 +53,159 @@
\class vpTemplateTrackerWarpHomography
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpHomography: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpHomography : public vpTemplateTrackerWarp
{
- public:
- //constructor;
- vpTemplateTrackerWarpHomography();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- #endif
-
- /*!
- Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
-
- \param vX : Point to consider
- \param ParamM : parameters of the warping function.
- */
- void computeDenom(vpColVector &vX, const vpColVector &ParamM);
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Return the homography defined by the given parameters
-
- \param ParamM : Parameters of the homography.
-
- \return An Homography via vpHomography.
- */
- vpHomography getHomography(const vpColVector &ParamM) const;
-
- /*!
- Compute the parameters of the homography warping function according to the given vpHomography
-
- \param H : Homography used to compute the parameters.
- \param par : Resulting warping function parameters.
- */
- void getParam(const vpHomography &H,vpColVector &par) const;
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+public:
+ // constructor;
+ vpTemplateTrackerWarpHomography();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector & /*p*/) {}
+#endif
+
+ /*!
+ Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
+
+ \param vX : Point to consider
+ \param ParamM : parameters of the warping function.
+ */
+ void computeDenom(vpColVector &vX, const vpColVector &ParamM);
+
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
+ */
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const int &i, const int &j, double *dIdW);
+
+ /*!
+ Return the homography defined by the given parameters
+
+ \param ParamM : Parameters of the homography.
+
+ \return An Homography via vpHomography.
+ */
+ vpHomography getHomography(const vpColVector &ParamM) const;
+
+ /*!
+ Compute the parameters of the homography warping function according to the
+ given vpHomography
+
+ \param H : Homography used to compute the parameters.
+ \param par : Resulting warping function parameters.
+ */
+ void getParam(const vpHomography &H, vpColVector &par) const;
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const { return false; }
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h
index c39bdd2..eb724a3 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpHomographySL3.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,10 +39,10 @@
*****************************************************************************/
/*!
\file vpTemplateTrackerWarpHomographySL3.h
- \brief warping function of an homography: the homography is defined on the sl3 lie algebra H=exp(Sum(p[i]* A_i)) A_i is the basis of the SL3 Algebra
+ \brief warping function of an homography: the homography is defined on the
+ sl3 lie algebra H=exp(Sum(p[i]* A_i)) A_i is the basis of the SL3 Algebra
*/
-
#ifndef vpTemplateTrackerWarpHomographySL3_hh
#define vpTemplateTrackerWarpHomographySL3_hh
@@ -54,164 +55,176 @@
\class vpTemplateTrackerWarpHomographySL3
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpHomographySL3: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpHomographySL3 : public vpTemplateTrackerWarp
{
- protected:
- vpMatrix G;
- vpMatrix dGx;
- std::vector<vpMatrix> A;
-
- public:
- //constructor;
- vpTemplateTrackerWarpHomographySL3();
- ~vpTemplateTrackerWarpHomographySL3();
-
- /*!
- Compute the exponential of the homography matrix defined by the given parameters
-
- \param p : Parameters of the SL3 homography warping function.
- */
- void computeCoeff(const vpColVector &p);
-
- /*!
- Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
-
- \param vX : Point to consider
- \param ParamM : parameters of the warping function.
- */
- void computeDenom(vpColVector &vX, const vpColVector &ParamM);
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Find the displacement/warping function parameters from a list of points.
-
- \param ut0 : Original u coordinates.
- \param vt0 : Original v coordinates.
- \param u : Warped u coordinates.
- \param v : Warped v coordinates.
- \param nb_pt : Number of points.
- \param p : Resulting warping function parameters.
- */
- void findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const double &i, const double &j,double *dIdW);
-
- /*!
- Return the homography defined by the warping function
-
- \return An Homography via vpHomography.
- */
- vpHomography getHomography() const;
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return true;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void warpXInv(const vpColVector &/*vX*/,vpColVector &/*vXres*/,const vpColVector &/*ParamM*/) {}
- #endif
+protected:
+ vpMatrix G;
+ vpMatrix dGx;
+ std::vector<vpMatrix> A;
+
+public:
+ // constructor;
+ vpTemplateTrackerWarpHomographySL3();
+ ~vpTemplateTrackerWarpHomographySL3();
+
+ /*!
+ Compute the exponential of the homography matrix defined by the given
+ parameters
+
+ \param p : Parameters of the SL3 homography warping function.
+ */
+ void computeCoeff(const vpColVector &p);
+
+ /*!
+ Compute the projection denominator (Z) used in x = X/Z and y = Y/Z.
+
+ \param vX : Point to consider
+ \param ParamM : parameters of the warping function.
+ */
+ void computeDenom(vpColVector &vX, const vpColVector &ParamM);
+
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
+
+ /*!
+ Find the displacement/warping function parameters from a list of points.
+
+ \param ut0 : Original u coordinates.
+ \param vt0 : Original v coordinates.
+ \param u : Warped u coordinates.
+ \param v : Warped v coordinates.
+ \param nb_pt : Number of points.
+ \param p : Resulting warping function parameters.
+ */
+ void findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt, vpColVector &p);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
+ */
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const int &i, const int &j, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const double &i, const double &j, double *dIdW);
+
+ /*!
+ Return the homography defined by the warping function
+
+ \return An Homography via vpHomography.
+ */
+ vpHomography getHomography() const;
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const { return true; }
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM);
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void warpXInv(const vpColVector & /*vX*/, vpColVector & /*vXres*/, const vpColVector & /*ParamM*/) {}
+#endif
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h
index f8ccf60..88a8694 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpRT.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
\brief sRt warping function
*/
-
#ifndef vpTemplateTrackerWarpRT_hh
#define vpTemplateTrackerWarpRT_hh
@@ -51,57 +51,65 @@
\class vpTemplateTrackerWarpRT
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpRT: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpRT : public vpTemplateTrackerWarp
{
public:
- //constructor;
+ // constructor;
vpTemplateTrackerWarpRT();
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
+ void computeCoeff(const vpColVector & /*p*/) {}
+ void computeDenom(vpColVector & /*vX*/, const vpColVector & /*ParamM*/) {}
#endif
/*!
- Compute the derivative of the warping function according to its parameters.
+ Compute the derivative of the warping function according to its
+ parameters.
\param X1 : Point to consider in the derivative computation.
\param X2 : Point to consider in the derivative computation.
\param ParamM : Parameters of the warping function.
\param dW : Resulting derivative matrix.
*/
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
/*!
- Compute the compositionnal derivative of the warping function according to its parameters.
+ Compute the compositionnal derivative of the warping function according
+ to its parameters.
\param X1 : Point to consider in the derivative computation.
\param X2 : Point to consider in the derivative computation.
\param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
+ \param dwdp0 : Derivative matrix of the warping function according to
+ the initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
*/
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
/*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ Compute the derivative of the image with relation to the warping
+ function parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
*/
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
/*!
- Compute the derivative of the warping function according to the initial parameters.
+ Compute the derivative of the warping function according to the initial
+ parameters.
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
*/
- void getdWdp0(const int &i,const int &j,double *dIdW);
+ void getdWdp0(const int &i, const int &j, double *dIdW);
/*!
Get the inverse of the warping function parameters.
@@ -109,7 +117,7 @@ public:
\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.
@@ -117,7 +125,7 @@ public:
\param p : Current parameters of the warping function.
\param pdown : Resulting parameters on level down.
*/
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
/*!
Get the parameters of the warping function one level up.
@@ -125,23 +133,24 @@ public:
\param p : Current parameters of the warping function.
\param pup : Resulting parameters one level up.
*/
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
/*!
Tells if the warping function is ESM compatible.
\return True if it is ESM compatible, False otherwise.
*/
- bool isESMcompatible() const {return false;}
+ bool isESMcompatible() const { return false; }
/*!
- Get the displacement resulting from the composition of two other displacements.
+ Get the displacement resulting from the composition of two other
+ displacements.
\param p1 : First displacement.
\param p2 : Second displacement.
\param pres : Displacement resulting from the composition of p1 and p2.
*/
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
/*!
Warp a point.
@@ -150,7 +159,7 @@ public:
\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.
@@ -161,7 +170,7 @@ public:
\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.
@@ -170,6 +179,6 @@ public:
\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 aafb1ba..79471aa 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpSRT.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
\brief sRt warping function
*/
-
#ifndef vpTemplateTrackerWarpSRT_hh
#define vpTemplateTrackerWarpSRT_hh
@@ -51,125 +51,134 @@
\class vpTemplateTrackerWarpSRT
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpSRT: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpSRT : public vpTemplateTrackerWarp
{
- public:
- //constructor;
- vpTemplateTrackerWarpSRT();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return false;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+public:
+ // constructor;
+ vpTemplateTrackerWarpSRT();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector & /*p*/) {}
+ void computeDenom(vpColVector & /*vX*/, const vpColVector & /*ParamM*/) {}
+#endif
+
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
+ */
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const int &i, const int &j, double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const { return false; }
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h
index cd81165..73d2a84 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerWarpTranslation.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,7 +42,6 @@
\brief Translation warping function: w(X)=X+b with: b= [p0, p1]]
*/
-
#ifndef vpTemplateTrackerWarpTranslation_hh
#define vpTemplateTrackerWarpTranslation_hh
@@ -51,125 +51,134 @@
\class vpTemplateTrackerWarpTranslation
\ingroup group_tt_warp
*/
-class VISP_EXPORT vpTemplateTrackerWarpTranslation: public vpTemplateTrackerWarp
+class VISP_EXPORT vpTemplateTrackerWarpTranslation : public vpTemplateTrackerWarp
{
- public:
- //constructor;
- vpTemplateTrackerWarpTranslation();
-
- #ifndef DOXYGEN_SHOULD_SKIP_THIS
- void computeCoeff(const vpColVector &/*p*/){}
- void computeDenom(vpColVector &/*vX*/, const vpColVector &/*ParamM*/){}
- #endif
-
- /*!
- Compute the derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warping function.
- \param dW : Resulting derivative matrix.
- */
- void dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,vpMatrix &dW);
-
- /*!
- Compute the compositionnal derivative of the warping function according to its parameters.
-
- \param X1 : Point to consider in the derivative computation.
- \param X2 : Point to consider in the derivative computation.
- \param ParamM : Parameters of the warp.
- \param dwdp0 : Derivative matrix of the warping function according to the initial warping function parameters (p=0).
- \param dW : Resulting compositionnal derivative matrix.
- */
- void dWarpCompo(const vpColVector &X1,const vpColVector &X2,const vpColVector &ParamM,const double *dwdp0,vpMatrix &dW);
-
- /*!
- Compute the derivative of the image with relation to the warping function parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dy : Derivative on the y-axis (along the rows) of the point (i,j).
- \param dx : Derivative on the x-axis (along the columns) of the point (i,j).
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW);
-
- /*!
- Compute the derivative of the warping function according to the initial parameters.
-
- \param i : i coordinate (along the rows) of the point to consider in the image.
- \param j : j coordinate (along the columns) of the point to consider in the image.
- \param dIdW : Resulting derivative matrix (Image according to the warping function).
- */
- void getdWdp0(const int &i,const int &j,double *dIdW);
-
- /*!
- Get the inverse of the warping function parameters.
-
- \param ParamM : Parameters of the warping function.
- \param ParamMinv : Inverse parameters.
- */
- void getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const ;
-
- /*!
- Get the parameters of the warping function one level down.
-
- \param p : Current parameters of the warping function.
- \param pdown : Resulting parameters on level down.
- */
- void getParamPyramidDown(const vpColVector &p,vpColVector &pdown);
-
- /*!
- Get the parameters of the warping function one level up.
-
- \param p : Current parameters of the warping function.
- \param pup : Resulting parameters one level up.
- */
- void getParamPyramidUp(const vpColVector &p,vpColVector &pup);
-
- /*!
- Tells if the warping function is ESM compatible.
-
- \return True if it is ESM compatible, False otherwise.
- */
- bool isESMcompatible() const {return true;}
-
- /*!
- Get the displacement resulting from the composition of two other displacements.
-
- \param p1 : First displacement.
- \param p2 : Second displacement.
- \param pres : Displacement resulting from the composition of p1 and p2.
- */
- void pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const ;
-
- /*!
- Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
-
- /*!
- Warp a point.
-
- \param i : i coordinate (along the rows) of the point to warp.
- \param j : j coordinate (along the columns) of the point to warp.
- \param i2 : i coordinate (along the rows) of the warped point.
- \param j2 : j coordinate (along the columns) of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM);
-
- /*!
- Inverse Warp a point.
-
- \param vX : Coordinates of the point to warp.
- \param vXres : Coordinates of the warped point.
- \param ParamM : Parameters of the warping function.
- */
- void warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM);
+public:
+ // constructor;
+ vpTemplateTrackerWarpTranslation();
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ void computeCoeff(const vpColVector & /*p*/) {}
+ void computeDenom(vpColVector & /*vX*/, const vpColVector & /*ParamM*/) {}
+#endif
+
+ /*!
+ Compute the derivative of the warping function according to its
+ parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warping function.
+ \param dW : Resulting derivative matrix.
+ */
+ void dWarp(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, vpMatrix &dW);
+
+ /*!
+ Compute the compositionnal derivative of the warping function according to
+ its parameters.
+
+ \param X1 : Point to consider in the derivative computation.
+ \param X2 : Point to consider in the derivative computation.
+ \param ParamM : Parameters of the warp.
+ \param dwdp0 : Derivative matrix of the warping function according to the
+ initial warping function parameters (p=0). \param dW : Resulting
+ compositionnal derivative matrix.
+ */
+ void dWarpCompo(const vpColVector &X1, const vpColVector &X2, const vpColVector &ParamM, const double *dwdp0,
+ vpMatrix &dW);
+
+ /*!
+ Compute the derivative of the image with relation to the warping function
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dy : Derivative on the y-axis (along the
+ rows) of the point (i,j). \param dx : Derivative on the x-axis (along the
+ columns) of the point (i,j). \param dIdW : Resulting derivative matrix
+ (Image according to the warping function).
+ */
+ void getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW);
+
+ /*!
+ Compute the derivative of the warping function according to the initial
+ parameters.
+
+ \param i : i coordinate (along the rows) of the point to consider in the
+ image. \param j : j coordinate (along the columns) of the point to
+ consider in the image. \param dIdW : Resulting derivative matrix (Image
+ according to the warping function).
+ */
+ void getdWdp0(const int &i, const int &j, double *dIdW);
+
+ /*!
+ Get the inverse of the warping function parameters.
+
+ \param ParamM : Parameters of the warping function.
+ \param ParamMinv : Inverse parameters.
+ */
+ void getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const;
+
+ /*!
+ Get the parameters of the warping function one level down.
+
+ \param p : Current parameters of the warping function.
+ \param pdown : Resulting parameters on level down.
+ */
+ void getParamPyramidDown(const vpColVector &p, vpColVector &pdown);
+
+ /*!
+ Get the parameters of the warping function one level up.
+
+ \param p : Current parameters of the warping function.
+ \param pup : Resulting parameters one level up.
+ */
+ void getParamPyramidUp(const vpColVector &p, vpColVector &pup);
+
+ /*!
+ Tells if the warping function is ESM compatible.
+
+ \return True if it is ESM compatible, False otherwise.
+ */
+ bool isESMcompatible() const { return true; }
+
+ /*!
+ Get the displacement resulting from the composition of two other
+ displacements.
+
+ \param p1 : First displacement.
+ \param p2 : Second displacement.
+ \param pres : Displacement resulting from the composition of p1 and p2.
+ */
+ void pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const;
+
+ /*!
+ Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
+
+ /*!
+ Warp a point.
+
+ \param i : i coordinate (along the rows) of the point to warp.
+ \param j : j coordinate (along the columns) of the point to warp.
+ \param i2 : i coordinate (along the rows) of the warped point.
+ \param j2 : j coordinate (along the columns) of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM);
+
+ /*!
+ Inverse Warp a point.
+
+ \param vX : Coordinates of the point to warp.
+ \param vXres : Coordinates of the warped point.
+ \param ParamM : Parameters of the warping function.
+ */
+ void warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM);
};
#endif
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h
index 6e43d3f..deb05f8 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCC.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,14 +47,13 @@
#include <math.h>
-#include <visp3/tt/vpTemplateTracker.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/core/vpMath.h>
+#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/vision/vpHomography.h>
#define APPROX_NCC
@@ -62,22 +62,25 @@
\class vpTemplateTrackerZNCC
\ingroup group_tt_tracker
*/
-class VISP_EXPORT vpTemplateTrackerZNCC: public vpTemplateTracker
+class VISP_EXPORT vpTemplateTrackerZNCC : public vpTemplateTracker
{
- protected:
- vpRowVector DI;
- vpRowVector temp;
+protected:
+ vpRowVector DI;
+ vpRowVector temp;
- protected:
- double getCost(const vpImage<unsigned char> &I, const vpColVector &tp) ;
- double getCost(const vpImage<unsigned char> &I) {vpColVector tp; return getCost(I,tp);}
- virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
- virtual void trackNoPyr(const vpImage<unsigned char> &I)=0;
+protected:
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getCost(const vpImage<unsigned char> &I)
+ {
+ vpColVector tp;
+ return getCost(I, tp);
+ }
+ virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
- public:
- vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp);
+public:
+ explicit vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp);
- void setGain(double _gain){gain=_gain;}
+ void setGain(double _gain) { gain = _gain; }
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h
index 18b9dc4..41068d6 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,23 +45,22 @@
#ifndef vpTemplateTrackerZNCCForwardAdditional_hh
#define vpTemplateTrackerZNCCForwardAdditional_hh
-
#include <visp3/tt/vpTemplateTrackerZNCC.h>
/*!
\class vpTemplateTrackerZNCCForwardAdditional
\ingroup group_tt_tracker
- The algorithm implemented in this class is described in \cite Irani98a and \cite Marchand16a.
+ The algorithm implemented in this class is described in \cite Irani98a and
+ \cite Marchand16a.
*/
-class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional: public vpTemplateTrackerZNCC
+class VISP_EXPORT vpTemplateTrackerZNCCForwardAdditional : public vpTemplateTrackerZNCC
{
- protected:
- void initHessienDesired(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
+protected:
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
- public:
- vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp);
+public:
+ 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 35ee487..7e16ed8 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -51,31 +52,31 @@
/*!
\class vpTemplateTrackerZNCCInverseCompositional
\ingroup group_tt_tracker
- The algorithm implemented in this class is described in \cite Irani98a and \cite Marchand16a.
+ 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;
- //pour eval evolRMS
- double evolRMS;
- std::vector<double> x_pos;
- std::vector<double> y_pos;
- double threshold_RMS;
- vpColVector moydIrefdp;
+protected:
+ bool compoInitialised;
+ // pour eval evolRMS
+ double evolRMS;
+ std::vector<double> x_pos;
+ std::vector<double> y_pos;
+ double threshold_RMS;
+ vpColVector moydIrefdp;
- protected:
- void initCompInverse(const vpImage<unsigned char> &I);
- void initHessienDesired(const vpImage<unsigned char> &I);
- void trackNoPyr(const vpImage<unsigned char> &I);
- void deletePosEvalRMS();
- void computeEvalRMS(const vpColVector &p);
- void initPosEvalRMS(const vpColVector &p);
+protected:
+ void initCompInverse(const vpImage<unsigned char> &I);
+ void initHessienDesired(const vpImage<unsigned char> &I);
+ void trackNoPyr(const vpImage<unsigned char> &I);
+ void deletePosEvalRMS();
+ void computeEvalRMS(const vpColVector &p);
+ void initPosEvalRMS(const vpColVector &p);
- public:
- vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp);
+public:
+ explicit vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp);
- void setThresholdRMS(double threshold){threshold_RMS=threshold;}
+ void setThresholdRMS(double threshold) { threshold_RMS = threshold; }
};
#endif
-
diff --git a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h
index 11a25f2..ff8cd9d 100644
--- a/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h
+++ b/modules/tracker/tt/include/visp3/tt/vpTemplateTrackerZone.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,75 +45,79 @@
#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpImage.h>
-#include <visp3/tt/vpTemplateTrackerTriangle.h>
-#include <visp3/tt/vpTemplateTrackerHeader.h>
#include <visp3/core/vpRect.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
+#include <visp3/tt/vpTemplateTrackerTriangle.h>
/*!
\class vpTemplateTrackerZone
\ingroup group_tt_tools
- A zone is defined by a set of triangles defined as vpTemplateTrackerTriangle.
+ A zone is defined by a set of triangles defined as
+ vpTemplateTrackerTriangle.
- A zone can be initialized either by user interaction using mouse click in a display device
- throw initClick(), or by a list of points throw initFromPoints().
+ A zone can be initialized either by user interaction using mouse click in a
+ display device throw initClick(), or by a list of points throw
+ initFromPoints().
*/
class VISP_EXPORT vpTemplateTrackerZone
{
- protected:
- std::vector<vpTemplateTrackerTriangle> Zone; //!< Vector of triangles that defines the zone.
- int min_x; //!< Bounding box parameter
- int min_y; //!< Bounding box parameter
- int max_x; //!< Bounding box parameter
- int max_y; //!< Bounding box parameter
+protected:
+ std::vector<vpTemplateTrackerTriangle> Zone; //!< Vector of triangles that defines the zone.
+ int min_x; //!< Bounding box parameter
+ int min_y; //!< Bounding box parameter
+ int max_x; //!< Bounding box parameter
+ int max_y; //!< Bounding box parameter
- public:
- vpTemplateTrackerZone();
- vpTemplateTrackerZone(const vpTemplateTrackerZone &z);
- ~vpTemplateTrackerZone();
+public:
+ vpTemplateTrackerZone();
+ vpTemplateTrackerZone(const vpTemplateTrackerZone &z);
+ ~vpTemplateTrackerZone();
- //add a triangle to the zone
- void add(const vpTemplateTrackerTriangle &t);
- void clear();
- void copy(const vpTemplateTrackerZone& z);
+ // add a triangle to the zone
+ void add(const vpTemplateTrackerTriangle &t);
+ void clear();
+ void copy(const vpTemplateTrackerZone &z);
- //display the area on an image
- void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
- void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness=3);
+ // display the area on an image
+ void display(const vpImage<unsigned char> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
+ void display(const vpImage<vpRGBa> &I, const vpColor &col = vpColor::green, const unsigned int thickness = 3);
- //colorie le tieme triangle
- void fillTriangle(vpImage<unsigned char>& I, unsigned int id, unsigned char gray_level);
+ // colorie le tieme triangle
+ void fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level);
- double getArea() const;
- vpImagePoint getCenter() const;
- vpImagePoint getCenter(int borne_x, int borne_y) const;
- //get bounds of the area
- int getMaxx() const;
- int getMaxy() const;
- int getMinx() const;
- int getMiny() const;
- vpRect getBoundingBox() const;
+ double getArea() const;
+ vpImagePoint getCenter() const;
+ vpImagePoint getCenter(int borne_x, int borne_y) const;
+ // get bounds of the area
+ int getMaxx() const;
+ int getMaxy() const;
+ int getMinx() const;
+ int getMiny() const;
+ vpRect getBoundingBox() const;
- /*! Return the number of triangles that define the zone. \sa getTriangle() */
- unsigned int getNbTriangle() const { return (unsigned int)Zone.size(); }
- vpTemplateTrackerZone getPyramidDown() const;
- //renvoie le ieme triangle de la zone
- void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const;
- vpTemplateTrackerTriangle getTriangle(unsigned int i) const;
+ /*! Return the number of triangles that define the zone. \sa getTriangle()
+ */
+ unsigned int getNbTriangle() const { return (unsigned int)Zone.size(); }
+ vpTemplateTrackerZone getPyramidDown() const;
+ // renvoie le ieme triangle de la zone
+ void getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const;
+ vpTemplateTrackerTriangle getTriangle(unsigned int i) const;
- //create an area by clicking on an image
- void initClick(const vpImage<unsigned char>& I, bool delaunay = false);
- //create an area with a pointer of integer that describes a series of triangles:
- // *pt= t0.S1.x,t0.S1.y,t0.S2.x,t0.S2.y,t0.S3.x,t0.S3.y, t1.S1.x ...
- void initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &ip, bool delaunay = false);
+ // create an area by clicking on an image
+ void initClick(const vpImage<unsigned char> &I, bool delaunay = false);
+ // create an area with a pointer of integer that describes a series of
+ // triangles:
+ // *pt= t0.S1.x,t0.S1.y,t0.S2.x,t0.S2.y,t0.S3.x,t0.S3.y, t1.S1.x ...
+ void initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &ip, bool delaunay = false);
- //check if a point is in the area
- bool inZone(const int &i,const int &j) const;
- bool inZone(const double &i,const double &j) const;
- //check if a point is in the area and return the corresponding triangle id_triangle where the point is.
- bool inZone(const int &i,const int &j, unsigned int &id_triangle) const;
- bool inZone(const double &i,const double &j, unsigned int &id_triangle) const;
+ // check if a point is in the area
+ bool inZone(const int &i, const int &j) const;
+ bool inZone(const double &i, const double &j) const;
+ // check if a point is in the area and return the corresponding triangle
+ // id_triangle where the point is.
+ bool inZone(const int &i, const int &j, unsigned int &id_triangle) const;
+ bool inZone(const double &i, const double &j, unsigned int &id_triangle) const;
- vpTemplateTrackerZone & operator=(const vpTemplateTrackerZone &z);
+ vpTemplateTrackerZone &operator=(const vpTemplateTrackerZone &z);
};
#endif
-
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp
index 12ec417..e05d348 100644
--- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSD.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,13 +40,12 @@
#include <visp3/tt/vpTemplateTrackerSSD.h>
-vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
- : vpTemplateTracker(warp), DI(), temp()
+vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp) : vpTemplateTracker(warp), DI(), temp()
{
- dW.resize(2,nbParam);
+ dW.resize(2, nbParam);
G.resize(nbParam);
- H.resize(nbParam,nbParam);
- HLM.resize(nbParam,nbParam);
+ H.resize(nbParam, nbParam);
+ HLM.resize(nbParam, nbParam);
temp.resize(nbParam);
@@ -56,72 +56,70 @@ vpTemplateTrackerSSD::vpTemplateTrackerSSD(vpTemplateTrackerWarp *warp)
double vpTemplateTrackerSSD::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- double erreur=0;
+ double erreur = 0;
double IW;
- int Nbpoint=0;
+ int Nbpoint = 0;
Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
- double j2=X2[0];
- double i2=X2[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);
+ double j2 = X2[0];
+ double i2 = X2[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);
else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- erreur+=((double)Tij-IW)*((double)Tij-IW);
+ IW = BI.getValue(i2, j2);
+ // IW=getSubPixBspline4(I,i2,j2);
+ erreur += ((double)Tij - IW) * ((double)Tij - IW);
Nbpoint++;
}
}
- ratioPixelIn=(double)Nbpoint/(double)templateSize;
+ ratioPixelIn = (double)Nbpoint / (double)templateSize;
- if(Nbpoint==0)return 10e10;
- return erreur/Nbpoint;
+ if (Nbpoint == 0)
+ return 10e10;
+ return erreur / Nbpoint;
}
-
double vpTemplateTrackerSSD::getSSD(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- double erreur=0;
+ double erreur = 0;
double IW;
- unsigned int Nbpoint=0;
+ unsigned int Nbpoint = 0;
- if(pyrInitialised)
- {
- templateSize=templateSizePyr[0];
- ptTemplate=ptTemplatePyr[0];
+ if (pyrInitialised) {
+ templateSize = templateSizePyr[0];
+ ptTemplate = ptTemplatePyr[0];
}
Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
- double j2=X2[0];
- double i2=X2[1];
- 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);
- erreur+=((double)Tij-IW)*((double)Tij-IW);
+ double j2 = X2[0];
+ double i2 = X2[1];
+ 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);
+ erreur += ((double)Tij - IW) * ((double)Tij - IW);
Nbpoint++;
}
}
- if(Nbpoint==0)return 10e10;
- return erreur/Nbpoint;
+ if (Nbpoint == 0)
+ return 10e10;
+ return erreur / Nbpoint;
}
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp
index 2848327..bd5ee7d 100644
--- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDESM.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,182 +37,171 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt/vpTemplateTrackerSSDESM.h>
#include <visp3/core/vpImageFilter.h>
+#include <visp3/tt/vpTemplateTrackerSSDESM.h>
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;
-
- if(!Warp->isESMcompatible())
- std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
-
- HInv.resize(nbParam,nbParam);
- HDir.resize(nbParam,nbParam);
- HLMInv.resize(nbParam,nbParam);
- HLMDir.resize(nbParam,nbParam);
+ useCompositionnal = false;
+ useInverse = false;
+
+ if (!Warp->isESMcompatible())
+ std::cerr << "The selected warp function is not appropriate for the ESM "
+ "algorithm..."
+ << std::endl;
+
+ HInv.resize(nbParam, nbParam);
+ HDir.resize(nbParam, nbParam);
+ HLMInv.resize(nbParam, nbParam);
+ HLMDir.resize(nbParam, nbParam);
GInv.resize(nbParam);
GDir.resize(nbParam);
}
-void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompInverse(I);
-}
+void vpTemplateTrackerSSDESM::initHessienDesired(const vpImage<unsigned char> &I) { initCompInverse(I); }
-void vpTemplateTrackerSSDESM::initCompInverse(const vpImage<unsigned char> &/*I*/)
+void vpTemplateTrackerSSDESM::initCompInverse(const vpImage<unsigned char> & /*I*/)
{
- //std::cout<<"Initialise precomputed value of ESM with templateSize: "<< templateSize<<std::endl;
- ptTemplateCompo=new vpTemplateTrackerPointCompo[templateSize];
- int i,j;
- //direct
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- ptTemplateCompo[point].dW=new double[2*nbParam];
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
-
+ // std::cout<<"Initialise precomputed value of ESM with templateSize: "<<
+ // templateSize<<std::endl;
+ ptTemplateCompo = new vpTemplateTrackerPointCompo[templateSize];
+ int i, j;
+ // direct
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ ptTemplateCompo[point].dW = new double[2 * nbParam];
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ Warp->getdWdp0(i, j, ptTemplateCompo[point].dW);
}
- //inverse
- HInv=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
-
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
- Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- HInv[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
+ // inverse
+ HInv = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ ptTemplate[point].dW = new double[nbParam];
+ Warp->getdW0(i, j, ptTemplate[point].dy, ptTemplate[point].dx, ptTemplate[point].dW);
+
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ HInv[it][jt] += ptTemplate[point].dW[it] * ptTemplate[point].dW[jt];
}
- vpMatrix::computeHLM(HInv,lambdaDep,HLMInv);
+ vpMatrix::computeHLM(HInv, lambdaDep, HLMInv);
-compoInitialised=true;
+ compoInitialised = true;
}
void vpTemplateTrackerSSDESM::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- double IW,dIWx,dIWy;
+ double IW, dIWx, dIWy;
double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- unsigned int Nbpoint=0;
- double erreur=0;
- dp=0;
- HDir=0;
- GDir=0;
- GInv=0;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
+ double alpha = 2.;
+ do {
+ unsigned int Nbpoint = 0;
+ double erreur = 0;
+ dp = 0;
+ HDir = 0;
+ GDir = 0;
+ GInv = 0;
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- //INVERSE
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ // INVERSE
+ Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
Nbpoint++;
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- GInv[it]+=er*ptTemplate[point].dW[it];
+ double er = (Tij - IW);
+ for (unsigned int it = 0; it < nbParam; it++)
+ GInv[it] += er * ptTemplate[point].dW[it];
- erreur+=er*er;
+ erreur += er * er;
- //DIRECT
- //dIWx=dIx.getValue(i2,j2);
- //dIWy=dIy.getValue(i2,j2);
+ // DIRECT
+ // dIWx=dIx.getValue(i2,j2);
+ // dIWy=dIy.getValue(i2,j2);
- dIWx=dIx.getValue(i2,j2)+ptTemplate[point].dx;
- dIWy=dIy.getValue(i2,j2)+ptTemplate[point].dy;
+ dIWx = dIx.getValue(i2, j2) + ptTemplate[point].dx;
+ dIWy = dIy.getValue(i2, j2) + ptTemplate[point].dy;
- //Calcul du Hessien
- //Warp->dWarp(X1,X2,p,dW);
- Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+ // Calcul du Hessien
+ // Warp->dWarp(X1,X2,p,dW);
+ Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- HDir[it][jt]+=tempt[it]*tempt[jt];
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ HDir[it][jt] += tempt[it] * tempt[jt];
- for(unsigned int it=0;it<nbParam;it++)
- GDir[it]+=er*tempt[it];
+ for (unsigned int it = 0; it < nbParam; it++)
+ GDir[it] += er * tempt[it];
delete[] tempt;
}
-
-
}
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
- vpMatrix::computeHLM(HDir,lambdaDep,HLMDir);
+ vpMatrix::computeHLM(HDir, lambdaDep, HLMDir);
- try
- {
- //dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
- //dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
- //dp=HLMInv.inverseByLU()*GInv;
- dp=(HLMDir).inverseByLU()*(GDir);
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
+ try {
+ // dp=(HLMInv+HLMDir).inverseByLU()*(GInv+GDir);
+ // dp=HLMInv.inverseByLU()*GInv+HLMDir.inverseByLU()*GDir;
+ // dp=HLMInv.inverseByLU()*GInv;
+ dp = (HLMDir).inverseByLU() * (GDir);
+ } catch (vpException &e) {
+ // std::cout<<"probleme inversion"<<std::endl;
throw(e);
}
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
+ dp = gain * dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
+ dp = alpha * dp;
}
- //Warp->pRondp(p,dp,p);
- p+=dp;
+ // Warp->pRondp(p,dp,p);
+ p += dp;
iteration++;
- }
- while( (iteration < iterationMax));
+ } while ((iteration < iterationMax));
- nbIteration=iteration;
+ nbIteration = iteration;
}
-
/*void vpTemplateTrackerSSDESM::InitCompInverse(vpImage<unsigned char> &I)
{
ptTempateCompo=new vpTemplateTrackerPointCompo[taille_template];
@@ -323,4 +313,3 @@ void vpTemplateTrackerSSDESM::track(vpImage<unsigned char> &I)
NbIteration=iteration;
}*/
-
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
index 1ca08e7..6f73520 100644
--- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardAdditional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,170 +38,151 @@
*
*****************************************************************************/
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
-#include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
#include <visp3/core/vpImageTools.h>
+#include <visp3/tt/vpTemplateTrackerSSDForwardAdditional.h>
vpTemplateTrackerSSDForwardAdditional::vpTemplateTrackerSSDForwardAdditional(vpTemplateTrackerWarp *warp)
: vpTemplateTrackerSSD(warp), minimizationMethod(USE_NEWTON), p_prec(), G_prec(), KQuasiNewton()
{
- useCompositionnal=false;
+ useCompositionnal = false;
}
void vpTemplateTrackerSSDForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- dW=0;
+ dW = 0;
- double lambda=lambdaDep;
- double IW,dIWx,dIWy;
+ double lambda = lambdaDep;
+ double IW, dIWx, dIWy;
double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- unsigned int Nbpoint=0;
- double erreur=0;
- G=0;
- H=0 ;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
+ double alpha = 2.;
+ do {
+ unsigned int Nbpoint = 0;
+ double erreur = 0;
+ G = 0;
+ H = 0;
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
+ dIWx = dIx.getValue(i2, j2);
+ dIWy = dIy.getValue(i2, j2);
Nbpoint++;
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+ // Calcul du Hessien
+ Warp->dWarp(X1, X2, p, dW);
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=tempt[it]*tempt[jt];
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ H[it][jt] += tempt[it] * tempt[jt];
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=er*tempt[it];
+ double er = (Tij - IW);
+ for (unsigned int it = 0; it < nbParam; it++)
+ G[it] += er * tempt[it];
- erreur+=(er*er);
+ erreur += (er * er);
delete[] tempt;
}
-
-
}
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
- vpMatrix::computeHLM(H,lambda,HLM);
- try
- {
- dp=1.*HLM.inverseByLU()*G;
- }
- catch(vpException &e)
- {
+ vpMatrix::computeHLM(H, lambda, HLM);
+ try {
+ dp = 1. * HLM.inverseByLU() * G;
+ } catch (vpException &e) {
throw(e);
}
- switch(minimizationMethod)
- {
- case vpTemplateTrackerSSDForwardAdditional::USE_LMA:
- {
+ switch (minimizationMethod) {
+ case vpTemplateTrackerSSDForwardAdditional::USE_LMA: {
vpColVector p_test_LMA(nbParam);
- p_test_LMA=p+1.*dp;
- erreur=-getCost(I,p);
- double erreur_LMA=-getCost(I,p_test_LMA);
- if(erreur_LMA<erreur)
- {
- p=p_test_LMA;
- lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
- }
- else
- {
- lambda=(lambda*10.<1e6)?1e6:lambda*10.;
+ p_test_LMA = p + 1. * dp;
+ erreur = -getCost(I, p);
+ double erreur_LMA = -getCost(I, p_test_LMA);
+ if (erreur_LMA < erreur) {
+ p = p_test_LMA;
+ lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
+ } else {
+ lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
}
- }
- break;
- case vpTemplateTrackerSSDForwardAdditional::USE_GRADIENT:
- {
- dp=gain*0.000001*G;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur,dp,alpha);
- dp=alpha*dp;
+ } break;
+ case vpTemplateTrackerSSDForwardAdditional::USE_GRADIENT: {
+ dp = gain * 0.000001 * G;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur, dp, alpha);
+ dp = alpha * dp;
}
p += 1. * dp;
break;
}
- case vpTemplateTrackerSSDForwardAdditional::USE_QUASINEWTON:
- {
- if(iterationGlobale!=0)
- {
- vpColVector s_quasi=p-p_prec;
- vpColVector y_quasi=G-G_prec;
- double s_scal_y=s_quasi.t()*y_quasi;
- //if(s_scal_y!=0)//BFGS
+ case vpTemplateTrackerSSDForwardAdditional::USE_QUASINEWTON: {
+ if (iterationGlobale != 0) {
+ vpColVector s_quasi = p - p_prec;
+ vpColVector y_quasi = G - G_prec;
+ double s_scal_y = s_quasi.t() * y_quasi;
+ // if(s_scal_y!=0)//BFGS
// KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
- //if(s_scal_y!=0.0)//DFP
- if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) //DFP
- KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
+ // if(s_scal_y!=0.0)//DFP
+ if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) // DFP
+ KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.t() / s_scal_y -
+ KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
+ (y_quasi.t() * KQuasiNewton * y_quasi));
}
- dp=-KQuasiNewton*G;
- p_prec=p;
- G_prec=G;
- p-=1.01*dp;
- }
- break;
+ dp = -KQuasiNewton * G;
+ p_prec = p;
+ G_prec = G;
+ p -= 1.01 * dp;
+ } break;
case vpTemplateTrackerSSDForwardAdditional::USE_NEWTON:
- default:
- {
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur,dp,alpha);
- dp=alpha*dp;
+ default: {
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur, dp, alpha);
+ dp = alpha * dp;
}
- p+=1.*dp;
+ p += 1. * dp;
break;
}
}
iteration++;
iterationGlobale++;
- }
- while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
+ } while (/*( erreur_prec-erreur<50) && */ (iteration < iterationMax));
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
+ // std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration = iteration;
}
-
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
index 97d7622..34a38df 100644
--- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDForwardCompositional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,137 +37,127 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
#include <visp3/core/vpImageFilter.h>
+#include <visp3/tt/vpTemplateTrackerSSDForwardCompositional.h>
vpTemplateTrackerSSDForwardCompositional::vpTemplateTrackerSSDForwardCompositional(vpTemplateTrackerWarp *warp)
: vpTemplateTrackerSSD(warp), compoInitialised(false)
{
}
-void vpTemplateTrackerSSDForwardCompositional::initCompo(const vpImage<unsigned char> &/*I*/)
+void vpTemplateTrackerSSDForwardCompositional::initCompo(const vpImage<unsigned char> & /*I*/)
{
- // std::cout<<"Initialise precomputed value of Compositionnal Direct"<<std::endl;
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- ptTemplate[point].dW=new double[2*nbParam];
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->getdWdp0(i,j,ptTemplate[point].dW);
-
+ // std::cout<<"Initialise precomputed value of Compositionnal
+ // Direct"<<std::endl;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ ptTemplate[point].dW = new double[2 * nbParam];
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ Warp->getdWdp0(i, j, ptTemplate[point].dW);
}
- compoInitialised=true;
+ compoInitialised = true;
}
-void vpTemplateTrackerSSDForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I)
-{
- initCompo(I);
-}
+void vpTemplateTrackerSSDForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I) { initCompo(I); }
void vpTemplateTrackerSSDForwardCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(!compoInitialised)
- std::cout<<"Compositionnal tracking no initialised\nUse InitCompo(vpImage<unsigned char> &I) function"<<std::endl;
+ if (!compoInitialised)
+ std::cout << "Compositionnal tracking no initialised\nUse "
+ "InitCompo(vpImage<unsigned char> &I) function"
+ << std::endl;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- dW=0;
+ dW = 0;
- double lambda=lambdaDep;
- double IW,dIWx,dIWy;
+ double lambda = lambdaDep;
+ double IW, dIWx, dIWy;
double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- unsigned int Nbpoint=0;
- double erreur=0;
- G=0;
- H=0 ;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
+ double alpha = 2.;
+ do {
+ unsigned int Nbpoint = 0;
+ double erreur = 0;
+ G = 0;
+ H = 0;
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
+ dIWx = dIx.getValue(i2, j2);
+ dIWy = dIy.getValue(i2, j2);
Nbpoint++;
- //Calcul du Hessien
+ // Calcul du Hessien
/*Warp->dWarp(X1,X2,p,dW);
double *tempt=new double[nbParam];
for(int it=0;it<nbParam;it++)
tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;*/
- Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+ Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it] =dW[0][it]*dIWx+dW[1][it]*dIWy;
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=tempt[it]*tempt[jt];
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ H[it][jt] += tempt[it] * tempt[jt];
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=er*tempt[it];
+ double er = (Tij - IW);
+ for (unsigned int it = 0; it < nbParam; it++)
+ G[it] += er * tempt[it];
- erreur+=(er*er);
+ erreur += (er * er);
delete[] tempt;
}
-
-
}
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
- vpMatrix::computeHLM(H,lambda,HLM);
+ vpMatrix::computeHLM(H, lambda, HLM);
- try
- {
- dp=1.*HLM.inverseByLU()*G;
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
+ try {
+ dp = 1. * HLM.inverseByLU() * G;
+ } catch (vpException &e) {
+ // std::cout<<"probleme inversion"<<std::endl;
throw(e);
}
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
+ dp = gain * dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
+ dp = alpha * dp;
}
- Warp->pRondp(p,dp,p);
- //p+=Gain*dp;
+ Warp->pRondp(p, dp, p);
+ // p+=Gain*dp;
iteration++;
- }
- while( /*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax));
+ } while (/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax));
- nbIteration=iteration;
+ nbIteration = iteration;
}
-
diff --git a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
index a450661..380b47b 100644
--- a/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
+++ b/modules/tracker/tt/src/ssd/vpTemplateTrackerSSDInverseCompositional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,69 +37,65 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
#include <visp3/core/vpImageTools.h>
+#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
vpTemplateTrackerSSDInverseCompositional::vpTemplateTrackerSSDInverseCompositional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerSSD(warp), compoInitialised(false), HInv(), HCompInverse(), useTemplateSelect(false),
- evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8)
+ : 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);
+ useInverse = true;
+ HInv.resize(nbParam, nbParam);
+ HCompInverse.resize(nbParam, nbParam);
}
-void vpTemplateTrackerSSDInverseCompositional::initCompInverse(const vpImage<unsigned char> &/*I*/)
+void vpTemplateTrackerSSDInverseCompositional::initCompInverse(const vpImage<unsigned char> & /*I*/)
{
- H=0;
- int i,j;
+ H = 0;
+ int i, j;
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
+ for (unsigned int point = 0; point < templateSize; point++) {
+ if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ ptTemplate[point].dW = new double[nbParam];
- Warp->getdW0(i,j,ptTemplate[point].dy,ptTemplate[point].dx,ptTemplate[point].dW);
+ Warp->getdW0(i, j, ptTemplate[point].dy, ptTemplate[point].dx, ptTemplate[point].dW);
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- H[it][jt]+=ptTemplate[point].dW[it]*ptTemplate[point].dW[jt];
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ H[it][jt] += ptTemplate[point].dW[it] * ptTemplate[point].dW[jt];
}
-
}
- HInv=H;
- vpMatrix HLMtemp(nbParam,nbParam);
- vpMatrix::computeHLM(H,lambdaDep,HLMtemp);
+ HInv = H;
+ vpMatrix HLMtemp(nbParam, nbParam);
+ vpMatrix::computeHLM(H, lambdaDep, HLMtemp);
- HCompInverse.resize(nbParam,nbParam);
- HCompInverse=HLMtemp.inverseByLU();
- //std::cout<<Hinverse<<std::endl;
+ HCompInverse.resize(nbParam, nbParam);
+ HCompInverse = HLMtemp.inverseByLU();
+ // std::cout<<Hinverse<<std::endl;
vpColVector dWtemp(nbParam);
vpColVector HiGtemp(nbParam);
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- //i=ptTemplate[point].y;
- //j=ptTemplate[point].x;
- for(unsigned int it=0;it<nbParam;it++)
- dWtemp[it]=ptTemplate[point].dW[it];
-
- HiGtemp = -1.*HCompInverse*dWtemp;
- ptTemplate[point].HiG=new double[nbParam];
-
- for(unsigned int it=0;it<nbParam;it++)
- ptTemplate[point].HiG[it]=HiGtemp[it];
+ for (unsigned int point = 0; point < templateSize; point++) {
+ if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
+ // i=ptTemplate[point].y;
+ // j=ptTemplate[point].x;
+ for (unsigned int it = 0; it < nbParam; it++)
+ dWtemp[it] = ptTemplate[point].dW[it];
+
+ HiGtemp = -1. * HCompInverse * dWtemp;
+ ptTemplate[point].HiG = new double[nbParam];
+
+ for (unsigned int it = 0; it < nbParam; it++)
+ ptTemplate[point].HiG[it] = HiGtemp[it];
}
}
- compoInitialised=true;
+ compoInitialised = true;
}
void vpTemplateTrackerSSDInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
@@ -108,80 +105,77 @@ void vpTemplateTrackerSSDInverseCompositional::initHessienDesired(const vpImage<
void vpTemplateTrackerSSDInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
vpColVector dpinv(nbParam);
double IW;
double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- //vpTemplateTrackerPointtest *pt;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
+ double alpha = 2.;
+ // vpTemplateTrackerPointtest *pt;
initPosEvalRMS(p);
vpTemplateTrackerPoint *pt;
- do
- {
- unsigned int Nbpoint=0;
- double erreur=0;
- dp=0;
+ do {
+ unsigned int Nbpoint = 0;
+ double erreur = 0;
+ dp = 0;
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- if((!useTemplateSelect)||(ptTemplateSelect[point]))
- {
- //pt=&ptTemplatetest[point];
- pt=&ptTemplate[point];
- i=pt->y;
- j=pt->x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=pt->val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ if ((!useTemplateSelect) || (ptTemplateSelect[point])) {
+ // pt=&ptTemplatetest[point];
+ pt = &ptTemplate[point];
+ i = pt->y;
+ j = pt->x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+ j2 = X2[0];
+ i2 = X2[1];
+
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = pt->val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
Nbpoint++;
- double er=(Tij-IW);
- for(unsigned int it=0;it<nbParam;it++)
- dp[it]+=er*pt->HiG[it];
+ double er = (Tij - IW);
+ for (unsigned int it = 0; it < nbParam; it++)
+ dp[it] += er * pt->HiG[it];
- erreur+=er*er;
+ erreur += er * er;
}
}
}
- //std::cout << "npoint: " << Nbpoint << std::endl;
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ // std::cout << "npoint: " << Nbpoint << std::endl;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
- dp=gain*dp;
- //std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
+ dp = gain * dp;
+ // std::cout<<erreur/Nbpoint<<","<<GetCost(I,p)<<std::endl;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
+ dp = alpha * dp;
}
- Warp->getParamInverse(dp,dpinv);
- Warp->pRondp(p,dpinv,p);
+ Warp->getParamInverse(dp, dpinv);
+ Warp->pRondp(p, dpinv, p);
iteration++;
computeEvalRMS(p);
- //std::cout << "iteration: " << iteration << " max: " << iterationMax << std::endl;
- //std::cout << "evolRMS: " << evolRMS << " threshold: " << threshold_RMS << std::endl;
- }
- while(/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax)&&(evolRMS>threshold_RMS));
+ // std::cout << "iteration: " << iteration << " max: " << iterationMax <<
+ // std::endl; std::cout << "evolRMS: " << evolRMS << " threshold: " <<
+ // threshold_RMS << std::endl;
+ } while (/*( erreur_prec-erreur<50) &&*/ (iteration < iterationMax) && (evolRMS > threshold_RMS));
- nbIteration=iteration;
+ nbIteration = iteration;
deletePosEvalRMS();
}
@@ -194,16 +188,15 @@ void vpTemplateTrackerSSDInverseCompositional::initPosEvalRMS(const vpColVector
Warp->computeCoeff(p_);
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, p_);
+ Warp->warpX(X1, X2, p_);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
}
@@ -213,25 +206,23 @@ void vpTemplateTrackerSSDInverseCompositional::computeEvalRMS(const vpColVector
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
Warp->computeCoeff(p_);
- evolRMS=0;
+ evolRMS = 0;
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, p_);
+ Warp->warpX(X1, X2, p_);
+ evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
+ (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
- evolRMS=evolRMS/nb_corners;
+ evolRMS = evolRMS / nb_corners;
}
-void vpTemplateTrackerSSDInverseCompositional::deletePosEvalRMS()
-{
-}
+void vpTemplateTrackerSSDInverseCompositional::deletePosEvalRMS() {}
diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp
index 66979f4..92158d4 100644
--- a/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerBSpline.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,21 +43,19 @@
double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage<double> &I, double r, double t)
{
- double res=0;
- int cr=(int)(r);
- int ct=(int)(t);
- double er=(double)r-cr;
- double et=(double)t-ct;
- int height=(int)I.getHeight();//r
- int width=(int)I.getWidth();//t
- for(int ir=-1;ir<=2;ir++)
- {
- int tr=ir+cr;
- 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];
+ double res = 0;
+ int cr = (int)(r);
+ int ct = (int)(t);
+ double er = (double)r - cr;
+ double et = (double)t - ct;
+ int height = (int)I.getHeight(); // r
+ int width = (int)I.getWidth(); // t
+ for (int ir = -1; ir <= 2; ir++) {
+ int tr = ir + cr;
+ 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];
}
}
return res;
@@ -64,13 +63,14 @@ double vpTemplateTrackerBSpline::getSubPixBspline4(const vpImage<double> &I, dou
double vpTemplateTrackerBSpline::Bspline4(double diff)
{
- //double result;
- double aDiff=vpMath::abs(diff);
- if(aDiff<1.)
- return (aDiff*aDiff*aDiff/2.-aDiff*aDiff+4./6.);
- //return (0.5*(1.-aDiff)*(1.-aDiff)*(1.-aDiff)+0.5*(1.-aDiff)*(1.-aDiff)-0.5*(1.-aDiff)+1./6.);
- else if(aDiff<2.)
- return ((2.-aDiff)*(2.-aDiff)*(2.-aDiff)/6.);
+ // double result;
+ double aDiff = vpMath::abs(diff);
+ if (aDiff < 1.)
+ return (aDiff * aDiff * aDiff / 2. - aDiff * aDiff + 4. / 6.);
+ // return
+ // (0.5*(1.-aDiff)*(1.-aDiff)*(1.-aDiff)+0.5*(1.-aDiff)*(1.-aDiff)-0.5*(1.-aDiff)+1./6.);
+ else if (aDiff < 2.)
+ return ((2. - aDiff) * (2. - aDiff) * (2. - aDiff) / 6.);
else
return 0;
}
diff --git a/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp
index bad6e76..8b9d737 100644
--- a/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerTriangle.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,17 +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;
}
@@ -60,31 +61,31 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpTemplateTrackerTria
/*!
Copy operator.
*/
-vpTemplateTrackerTriangle & vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle& T)
+vpTemplateTrackerTriangle &vpTemplateTrackerTriangle::operator=(const vpTemplateTrackerTriangle &T)
{
- minx_temp=T.minx_temp;
- miny_temp=T.miny_temp;
-
- l_t=T.l_t;
- h_t=T.h_t;
- C1.x=T.C1.x;
- C1.y=T.C1.y;
- C2.x=T.C2.x;
- C2.y=T.C2.y;
- C3.x=T.C3.x;
- C3.y=T.C3.y;
- //uvinv.resize(2,2);
- //uvinv=T.uvinv;
- //p_ds_uv.resize(2);
- //p_ds_uv=T.p_ds_uv;
- //ptempo.resize(2);
- //ptempo=T.ptempo;
- not_good=T.not_good;
-
- uvinv00=T.uvinv00;
- uvinv01=T.uvinv01;
- uvinv10=T.uvinv10;
- uvinv11=T.uvinv11;
+ minx_temp = T.minx_temp;
+ miny_temp = T.miny_temp;
+
+ l_t = T.l_t;
+ h_t = T.h_t;
+ C1.x = T.C1.x;
+ C1.y = T.C1.y;
+ C2.x = T.C2.x;
+ C2.y = T.C2.y;
+ C3.x = T.C3.x;
+ C3.y = T.C3.y;
+ // uvinv.resize(2,2);
+ // uvinv=T.uvinv;
+ // p_ds_uv.resize(2);
+ // p_ds_uv=T.p_ds_uv;
+ // ptempo.resize(2);
+ // ptempo=T.ptempo;
+ not_good = T.not_good;
+
+ uvinv00 = T.uvinv00;
+ uvinv01 = T.uvinv01;
+ uvinv10 = T.uvinv10;
+ uvinv11 = T.uvinv11;
marge_triangle = T.marge_triangle;
area = T.area;
@@ -98,15 +99,15 @@ vpTemplateTrackerTriangle & vpTemplateTrackerTriangle::operator=(const vpTemplat
\param c2 : Second corner.
\param c3 : Third corner.
- The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
+ 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]);
+ init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
}
/*!
Return a triangle with coordinates that are down scaled by a factor 2.
@@ -114,7 +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;
}
@@ -123,11 +124,11 @@ 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);
+ init(x1, y1, x2, y2, x3, y3);
}
/*!
@@ -136,9 +137,10 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(int x1,int y1, int x2,int y
\param c2 : Second corner.
\param c3 : Third corner.
*/
-vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2, const vpImagePoint &c3)
- : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false),
- uvinv00(0.), uvinv01(0.), uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
+vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, const vpImagePoint &c2,
+ const vpImagePoint &c3)
+ : minx_temp(0), miny_temp(0), C1(), C2(), C3(), l_t(0), h_t(0), not_good(false), uvinv00(0.), uvinv01(0.),
+ uvinv10(0.), uvinv11(0.), marge_triangle(0.00001), area(0)
{
init(c1.get_u(), c1.get_v(), c2.get_u(), c2.get_v(), c3.get_u(), c3.get_v());
}
@@ -148,11 +150,11 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(const vpImagePoint &c1, con
- 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);
+ init(x1, y1, x2, y2, x3, y3);
}
/*!
Initializes a triangle from 3 corners.
@@ -160,11 +162,12 @@ vpTemplateTrackerTriangle::vpTemplateTrackerTriangle(double x1,double y1, double
\param c2 : Second corner.
\param c3 : Third corner.
- The coordinates of the points are defined as a 2 dimension vector with coordinates (x,y).
+ 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]);
+ init(c1[0], c1[1], c2[0], c2[1], c3[0], c3[1]);
}
/*!
Initializes a triangle from 3 corners defined as image points.
@@ -178,78 +181,81 @@ void vpTemplateTrackerTriangle::init(const vpImagePoint &c1, const vpImagePoint
}
/*!
- Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2),
+ (x3,y3).
- x coordinate is along the columns
- y coordinate is along the rows.
*/
-void vpTemplateTrackerTriangle::init(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);
}
/*!
- Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2), (x3,y3).
+ Initializes a triangle from 3 corners with coordinates (x1,y1), (x2,y2),
+ (x3,y3).
- x coordinate is along the columns
- y coordinate is along the rows.
*/
-void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2, double x3, double y3)
+void vpTemplateTrackerTriangle::init(double x1, double y1, double x2, double y2, double x3, double y3)
{
- C1.x=x1;C1.y=y1;
- C2.x=x2;C2.y=y2;
- C3.x=x3;C3.y=y3;
-
- double minx,miny,maxx,maxy;
- //calcul du rectangle minimal contenant le triangle seletionne
- minx=(x1<x2)?x1:x2;
- miny=(y1<y2)?y1:y2;
- minx=(minx<x3)?minx:x3;
- miny=(miny<y3)?miny:y3;
- maxx=(x1>x2)?x1:x2;
- maxy=(y1>y2)?y1:y2;
- maxx=(maxx>x3)?maxx:x3;
- maxy=(maxy>y3)?maxy:y3;
+ C1.x = x1;
+ C1.y = y1;
+ C2.x = x2;
+ C2.y = y2;
+ C3.x = x3;
+ C3.y = y3;
+
+ double minx, miny, maxx, maxy;
+ // calcul du rectangle minimal contenant le triangle seletionne
+ minx = (x1 < x2) ? x1 : x2;
+ miny = (y1 < y2) ? y1 : y2;
+ minx = (minx < x3) ? minx : x3;
+ miny = (miny < y3) ? miny : y3;
+ maxx = (x1 > x2) ? x1 : x2;
+ maxy = (y1 > y2) ? y1 : y2;
+ maxx = (maxx > x3) ? maxx : x3;
+ maxy = (maxy > y3) ? maxy : y3;
vpColVector u;
vpColVector v;
u.resize(2);
v.resize(2);
- vpMatrix uv(2,2);
- vpMatrix uvinv(2,2);
-
- u[0]=C2.x-C1.x;
- u[1]=C2.y-C1.y;
-
- v[0]=C3.x-C1.x;
- v[1]=C3.y-C1.y;
-
- uv[0][0]=u[0];uv[1][0]=v[0];
- uv[0][1]=u[1];uv[1][1]=v[1];
- try
- {
- uvinv=uv.inverseByLU();
- not_good=false;
+ vpMatrix uv(2, 2);
+ vpMatrix uvinv(2, 2);
+
+ u[0] = C2.x - C1.x;
+ u[1] = C2.y - C1.y;
+
+ v[0] = C3.x - C1.x;
+ v[1] = C3.y - C1.y;
+
+ uv[0][0] = u[0];
+ uv[1][0] = v[0];
+ uv[0][1] = u[1];
+ uv[1][1] = v[1];
+ try {
+ uvinv = uv.inverseByLU();
+ not_good = false;
+ } catch (...) {
+ not_good = true;
+ std::cout << "Triangle vide" << std::endl;
}
- catch(...)
- {
- not_good=true;
- std::cout<<"Triangle vide"<<std::endl;
+ uvinv00 = uvinv[0][0];
+ uvinv01 = uvinv[0][1];
+ uvinv10 = uvinv[1][0];
+ uvinv11 = uvinv[1][1];
- }
- uvinv00=uvinv[0][0];
- uvinv01=uvinv[0][1];
- uvinv10=uvinv[1][0];
- uvinv11=uvinv[1][1];
-
- l_t=maxx-minx;
- h_t=maxy-miny;
- minx_temp=minx;
- miny_temp=miny;
+ l_t = maxx - minx;
+ h_t = maxy - miny;
+ minx_temp = minx;
+ miny_temp = miny;
marge_triangle = 0.00001;
area = 0.5 * fabs(uv.det());
}
-//marge ajoutee a zone pour que sommet soit pris en compte
+// marge ajoutee a zone pour que sommet soit pris en compte
/*!
Indicates if a point with coordinates (i,j) is in the triangle.
@@ -258,7 +264,7 @@ void vpTemplateTrackerTriangle::init(double x1, double y1, double x2,double y2,
*/
bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
{
- if(not_good)
+ if (not_good)
return false;
/*ptempo[0]=j-C1.x;
@@ -267,11 +273,11 @@ bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
p_ds_uv=ptempo*uvinv;
return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
- double ptempo0=j-C1.x;
- double ptempo1=i-C1.y;
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
- return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
+ double ptempo0 = j - C1.x;
+ double ptempo1 = i - C1.y;
+ double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
+ double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
+ return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
}
/*!
@@ -281,28 +287,25 @@ bool vpTemplateTrackerTriangle::inTriangle(const int &i, const int &j) const
*/
bool vpTemplateTrackerTriangle::inTriangle(const double &i, const double &j) const
{
- if(not_good)
+ if (not_good)
return false;
/*ptempo[0]=j-C1.x;
ptempo[1]=i-C1.y;
p_ds_uv=ptempo*uvinv;
return (p_ds_uv[0]+p_ds_uv[1]<1. && p_ds_uv[0]>0 && p_ds_uv[1]>0);*/
- double ptempo0=j-C1.x;
- double ptempo1=i-C1.y;
- double p_ds_uv0=ptempo0*uvinv00+ptempo1*uvinv10;
- double p_ds_uv1=ptempo0*uvinv01+ptempo1*uvinv11;
- return (p_ds_uv0+p_ds_uv1<1.+marge_triangle && p_ds_uv0>-marge_triangle && p_ds_uv1>-marge_triangle);
+ double ptempo0 = j - C1.x;
+ double ptempo1 = i - C1.y;
+ double p_ds_uv0 = ptempo0 * uvinv00 + ptempo1 * uvinv10;
+ double p_ds_uv1 = ptempo0 * uvinv01 + ptempo1 * uvinv11;
+ return (p_ds_uv0 + p_ds_uv1 < 1. + marge_triangle && p_ds_uv0 > -marge_triangle && p_ds_uv1 > -marge_triangle);
}
/*!
Indicates if an image point is in the triangle.
\param ip : Image point to consider.
*/
-bool vpTemplateTrackerTriangle::inTriangle(const vpImagePoint &ip) const
-{
- return inTriangle(ip.get_i(), ip.get_j());
-}
+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.
@@ -317,8 +320,9 @@ void vpTemplateTrackerTriangle::getCorners(vpImagePoint &c1, vpImagePoint &c2, v
}
/*!
- Returns the coordinates of the triangle corners as a 3 dimension vector of image points.
- \param c : 3 dimension vector of image points that correspond to the triangle corners.
+ Returns the coordinates of the triangle corners as a 3 dimension vector of
+ image points. \param c : 3 dimension vector of image points that correspond
+ to the triangle corners.
*/
void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
{
@@ -329,51 +333,53 @@ void vpTemplateTrackerTriangle::getCorners(std::vector<vpImagePoint> &c) const
}
/*!
- Returns the coordinates of the triangle corners as a 2 dimension vector (x,y).
- \param c1 : First corner.
- \param c2 : Second corner.
- \param c3 : Third corner.
+ Returns the coordinates of the triangle corners as a 2 dimension vector
+ (x,y). \param c1 : First corner. \param c2 : Second corner. \param c3 :
+ Third corner.
*/
-void vpTemplateTrackerTriangle::getCorners(vpColVector &c1,vpColVector &c2,vpColVector &c3) const
+void vpTemplateTrackerTriangle::getCorners(vpColVector &c1, vpColVector &c2, vpColVector &c3) const
{
- c1=getCorner1();
- c2=getCorner2();
- c3=getCorner3();
+ c1 = getCorner1();
+ c2 = getCorner2();
+ c3 = getCorner3();
}
/*!
Returns the coordinates of the triangle first corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of
+ the corner.
*/
vpColVector vpTemplateTrackerTriangle::getCorner1() const
{
vpColVector c(2);
- c[0]=C1.x;
- c[1]=C1.y;
+ c[0] = C1.x;
+ c[1] = C1.y;
return c;
}
/*!
Returns the coordinates of the triangle second corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of
+ the corner.
*/
vpColVector vpTemplateTrackerTriangle::getCorner2() const
{
vpColVector c(2);
- c[0]=C2.x;
- c[1]=C2.y;
+ c[0] = C2.x;
+ c[1] = C2.y;
return c;
}
/*!
Returns the coordinates of the triangle third corner.
- \return A vector with dimension 2, that contains the coordinates (x,y) of the corner.
+ \return A vector with dimension 2, that contains the coordinates (x,y) of
+ the corner.
*/
vpColVector vpTemplateTrackerTriangle::getCorner3() const
{
vpColVector c(2);
- c[0]=C3.x;
- c[1]=C3.y;
+ c[0] = C3.x;
+ c[1] = C3.y;
return c;
}
@@ -382,54 +388,39 @@ vpColVector vpTemplateTrackerTriangle::getCorner3() const
\param w : Bounding box width.
\param h : Bounding box height.
*/
-void vpTemplateTrackerTriangle::getSize(double &w,double &h) const
+void vpTemplateTrackerTriangle::getSize(double &w, double &h) const
{
- w=l_t;
- h=h_t;
+ w = l_t;
+ h = h_t;
}
/*!
Get the size of the triangle bounding box.
\param w : Bounding box width.
\param h : Bounding box height.
*/
-void vpTemplateTrackerTriangle::getSize(int &w,int &h) const
+void vpTemplateTrackerTriangle::getSize(int &w, int &h) const
{
- w=(int)l_t+1;
- h=(int)h_t+1;
+ w = (int)l_t + 1;
+ h = (int)h_t + 1;
}
/*!
- \return The minimal x coordinate (along the columns of the image) of the points that are in the triangle.
- \sa getMaxx()
+ \return The minimal x coordinate (along the columns of the image) of the
+ points that are in the triangle. \sa getMaxx()
*/
-double vpTemplateTrackerTriangle::getMinx() const
-{
- return minx_temp-1;
-}
+double vpTemplateTrackerTriangle::getMinx() const { return minx_temp - 1; }
/*!
- \return The minimal y coordinate (along the rows of the image) of the points that are in the triangle.
- \sa getMaxy()
+ \return The minimal y coordinate (along the rows of the image) of the points
+ that are in the triangle. \sa getMaxy()
*/
-double vpTemplateTrackerTriangle::getMiny() const
-{
- return miny_temp-1;
-}
+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()
+ \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()
+ \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 94733cc..d3abff8 100644
--- a/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp
+++ b/modules/tracker/tt/src/tools/vpTemplateTrackerZone.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,7 @@
*
*****************************************************************************/
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
#include <visp3/core/vpConfig.h>
@@ -47,14 +48,10 @@
#include <visp3/tt/vpTemplateTrackerZone.h>
-
/*!
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.
@@ -70,10 +67,10 @@ vpTemplateTrackerZone::vpTemplateTrackerZone(const vpTemplateTrackerZone &z)
*/
void vpTemplateTrackerZone::clear()
{
- min_x=-1;
- min_y=-1;
- max_x=-1;
- max_y=-1;
+ min_x = -1;
+ min_y = -1;
+ max_x = -1;
+ max_y = -1;
Zone.clear();
}
@@ -81,7 +78,7 @@ void vpTemplateTrackerZone::clear()
/*!
Copy operator.
*/
-vpTemplateTrackerZone & vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z)
+vpTemplateTrackerZone &vpTemplateTrackerZone::operator=(const vpTemplateTrackerZone &z)
{
clear();
@@ -94,19 +91,21 @@ vpTemplateTrackerZone & vpTemplateTrackerZone::operator=(const vpTemplateTracker
\param I : Image used to select the zone.
\param delaunay : Flag used to enable Delaunay triangulation.
- - If true, from the image points selected by the user, a Delaunay triangulation is performed
- to initialize the zone.
+ - If true, from the image points selected by the user, a Delaunay
+ triangulation is performed to initialize the zone.
- A left click select a image point;
- - A right click select the last image point and ends the initialisation stage.
- In that case at least 3 points need to be selected by the user.
+ - A right click select the last image point and ends the initialisation
+ stage. In that case at least 3 points need to be selected by the user.
- If false, the user select directly points as successive triangle corners.
Three successive points define a triangle. It is not mandatory
- that triangles have one edge in common; they can define a discontinued area.
+ that triangles have one edge in common; they can define a discontinued
+ area.
- A left click select a triangle corner;
- - A right click select the last triangle corner and ends the initialisation stage.
- The number of points that are selected by the user should be a multiple of 3.
- For example, to select a zone as two triangles, the user has to left click
- five times and finish the selection on the sixth corner with a right click.
+ - A right click select the last triangle corner and ends the
+ initialisation stage. The number of points that are selected by the user
+ should be a multiple of 3. For example, to select a zone as two triangles,
+ the user has to left click five times and finish the selection on the sixth
+ corner with a right click.
*/
void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool delaunay)
@@ -120,7 +119,7 @@ void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool dela
do {
vpImagePoint p;
vpMouseButton::vpMouseButtonType button;
- if (vpDisplay::getClick(I, p, button, false) ) {
+ if (vpDisplay::getClick(I, p, button, false)) {
vip.push_back(p);
vpDisplay::displayCross(I, p, 7, vpColor::red);
@@ -128,19 +127,17 @@ void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool dela
if (vip.size() > 1) {
if (delaunay) {
// Draw a line between the 2 last points
- vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
- }
- else {
- if(vip.size() % 3 ==2)
+ vpDisplay::displayLine(I, p, vip[vip.size() - 2], vpColor::blue, 3);
+ } else {
+ if (vip.size() % 3 == 2)
// draw line between point 2-1
- vpDisplay::displayLine(I, p, vip[vip.size()-2], vpColor::blue, 3);
- else if(vip.size() % 3 ==0) {
+ 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);
}
-
}
}
@@ -150,7 +147,7 @@ void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool dela
vpTime::wait(20);
vpDisplay::flush(I);
- } while(!end);
+ } while (!end);
initFromPoints(I, vip, delaunay);
}
@@ -162,17 +159,19 @@ void vpTemplateTrackerZone::initClick(const vpImage<unsigned char> &I, bool dela
\param I : Image to process.
\param vip : Vector of image points used as initialization.
\param delaunay :
- - If true, a Delaunay triangulation is perfomed on the vector of image points. This functionality
- is only available if ViSP is build with OpenCV >2.3 third-party.
- - If false, the vector of image points describe triangles. Its size is then a multiple of 3.
+ - If true, a Delaunay triangulation is perfomed on the vector of image
+ points. This functionality is only available if ViSP is build with OpenCV
+ >2.3 third-party.
+ - If false, the vector of image points describe triangles. Its size is then
+ a multiple of 3.
*/
-void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, const std::vector< vpImagePoint > &vip, bool delaunay)
+void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &vip,
+ bool delaunay)
{
if (delaunay) {
- if(vip.size() == 3) {
+ if (vip.size() == 3) {
initFromPoints(I, vip, false);
- }
- else if(vip.size() == 4) {
+ } else if (vip.size() == 4) {
std::vector<vpImagePoint> vip_delaunay;
vip_delaunay.push_back(vip[0]);
vip_delaunay.push_back(vip[1]);
@@ -181,14 +180,13 @@ void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, cons
vip_delaunay.push_back(vip[3]);
vip_delaunay.push_back(vip[0]);
initFromPoints(I, vip_delaunay, false);
- }
- else {
+ } else {
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
// Init Delaunay
cv::Subdiv2D subdiv(cv::Rect(0, 0, (int)I.getWidth(), (int)I.getHeight()));
- for(size_t i=0; i< vip.size(); i++) {
+ 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;
+ // std::cout << "Click point: " << vip[i] << std::endl;
subdiv.insert(fp);
}
@@ -200,7 +198,7 @@ void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, cons
vpRect rect(0, 0, I.getWidth(), I.getHeight());
std::vector<vpImagePoint> vip_delaunay;
- for( size_t i = 0; i < triangleList.size(); i++ ) {
+ for (size_t i = 0; i < triangleList.size(); i++) {
cv::Vec6f t = triangleList[i];
std::vector<vpImagePoint> p(3);
@@ -217,30 +215,29 @@ void vpTemplateTrackerZone::initFromPoints(const vpImage<unsigned char>& I, cons
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 {
+ } else {
Zone.clear();
- for(unsigned int i=0; i<vip.size(); i+=3) {
- vpTemplateTrackerTriangle triangle(vip[i], vip[i+1], vip[i+2]);
+ for (unsigned int i = 0; i < vip.size(); i += 3) {
+ vpTemplateTrackerTriangle triangle(vip[i], vip[i + 1], vip[i + 2]);
add(triangle);
-// vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green, 1);
-// vpDisplay::displayLine(I, vip[i+1], vip[i+2], vpColor::green, 1);
-// vpDisplay::displayLine(I, vip[i+2], vip[i], vpColor::green,1);
-// vpDisplay::flush(I) ;
+ // vpDisplay::displayLine(I, vip[i], vip[i+1], vpColor::green,
+ // 1); vpDisplay::displayLine(I, vip[i+1], vip[i+2],
+ // vpColor::green, 1); vpDisplay::displayLine(I, vip[i+2], vip[i],
+ // vpColor::green,1); vpDisplay::flush(I) ;
// Update the bounding box
- if((triangle.getMinx()<min_x)||(min_x==-1))
- min_x=(int)triangle.getMinx();
- if((triangle.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)triangle.getMaxx();
- if((triangle.getMiny()<min_y)||(min_y==-1))
- min_y=(int)triangle.getMiny();
- if((triangle.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)triangle.getMaxy();
+ if ((triangle.getMinx() < min_x) || (min_x == -1))
+ min_x = (int)triangle.getMinx();
+ if ((triangle.getMaxx() > max_x) || (max_x == -1))
+ max_x = (int)triangle.getMaxx();
+ if ((triangle.getMiny() < min_y) || (min_y == -1))
+ min_y = (int)triangle.getMiny();
+ if ((triangle.getMaxy() > max_y) || (max_y == -1))
+ max_y = (int)triangle.getMaxy();
}
}
}
@@ -254,27 +251,27 @@ void vpTemplateTrackerZone::add(const vpTemplateTrackerTriangle &t)
Zone.push_back(t);
// Update the bounding box
- if((t.getMinx()<min_x)||(min_x==-1))
- min_x=(int)t.getMinx();
- if((t.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)t.getMaxx();
- if((t.getMiny()<min_y)||(min_y==-1))
- min_y=(int)t.getMiny();
- if((t.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)t.getMaxy();
+ if ((t.getMinx() < min_x) || (min_x == -1))
+ min_x = (int)t.getMinx();
+ if ((t.getMaxx() > max_x) || (max_x == -1))
+ max_x = (int)t.getMaxx();
+ if ((t.getMiny() < min_y) || (min_y == -1))
+ min_y = (int)t.getMiny();
+ if ((t.getMaxy() > max_y) || (max_y == -1))
+ max_y = (int)t.getMaxy();
}
/*!
Test if a pixel with coordinates (i,j) is in the zone..
\param i, j : Coordinates of the pixel to test.
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a
+ set of triangles, false otherwise.
*/
bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
{
std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();++Iterateurvecteur)
- {
- if(Iterateurvecteur->inTriangle(i,j))
+ for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
+ if (Iterateurvecteur->inTriangle(i, j))
return true;
}
return false;
@@ -283,35 +280,33 @@ bool vpTemplateTrackerZone::inZone(const int &i, const int &j) const
/*!
Test if a pixel with coordinates (i,j) is in the zone..
\param i, j : Coordinates of the pixel to test.
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ \return true if the pixel with coordinates (i,j) is in the zone defined by a
+ set of triangles, false otherwise.
*/
-bool vpTemplateTrackerZone::inZone(const double &i,const double &j) const
+bool vpTemplateTrackerZone::inZone(const double &i, const double &j) const
{
std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();++Iterateurvecteur)
- {
- if(Iterateurvecteur->inTriangle(i,j))
+ for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
+ if (Iterateurvecteur->inTriangle(i, j))
return true;
}
return false;
}
/*!
- Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
- the triangle that contains the pixel.
- \param i, j : Coordinates of the pixel to test.
- \param id_triangle : Index of the triangle that contains the pixel (i,j).
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ Test if a pixel with coordinates (i,j) is in the zone and returns also the
+ index of the triangle that contains the pixel. \param i, j : Coordinates of
+ the pixel to test. \param id_triangle : Index of the triangle that contains
+ the pixel (i,j). \return true if the pixel with coordinates (i,j) is in the
+ zone defined by a set of triangles, false otherwise.
*/
-bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_triangle) const
+bool vpTemplateTrackerZone::inZone(const int &i, const int &j, unsigned int &id_triangle) const
{
- unsigned int id=0;
+ unsigned int id = 0;
std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end();++Iterateurvecteur)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- {
- id_triangle=id;
+ for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
+ if (Iterateurvecteur->inTriangle(i, j)) {
+ id_triangle = id;
return true;
}
id++;
@@ -320,21 +315,19 @@ bool vpTemplateTrackerZone::inZone(const int &i,const int &j, unsigned int &id_t
}
/*!
- Test if a pixel with coordinates (i,j) is in the zone and returns also the index of
- the triangle that contains the pixel.
- \param i, j : Coordinates of the pixel to test.
- \param id_triangle : Index of the triangle that contains the pixel (i,j).
- \return true if the pixel with coordinates (i,j) is in the zone defined by a set of triangles, false otherwise.
+ Test if a pixel with coordinates (i,j) is in the zone and returns also the
+ index of the triangle that contains the pixel. \param i, j : Coordinates of
+ the pixel to test. \param id_triangle : Index of the triangle that contains
+ the pixel (i,j). \return true if the pixel with coordinates (i,j) is in the
+ zone defined by a set of triangles, false otherwise.
*/
-bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int &id_triangle) const
+bool vpTemplateTrackerZone::inZone(const double &i, const double &j, unsigned int &id_triangle) const
{
- unsigned int id=0;
+ unsigned int id = 0;
std::vector<vpTemplateTrackerTriangle>::const_iterator Iterateurvecteur;
- for(Iterateurvecteur=Zone.begin();Iterateurvecteur!=Zone.end(); ++Iterateurvecteur)
- {
- if(Iterateurvecteur->inTriangle(i,j))
- {
- id_triangle=id;
+ for (Iterateurvecteur = Zone.begin(); Iterateurvecteur != Zone.end(); ++Iterateurvecteur) {
+ if (Iterateurvecteur->inTriangle(i, j)) {
+ id_triangle = id;
return true;
}
id++;
@@ -343,10 +336,10 @@ bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int
}
/*!
- A zone is defined by a set of triangles. This function returns the ith triangle.
- \param i : Index of the triangle to return.
- \param T : The triangle corresponding to index i.
- \return true if the triangle with index i was found, false otherwise.
+ A zone is defined by a set of triangles. This function returns the ith
+ triangle. \param i : Index of the triangle to return. \param T : The
+ triangle corresponding to index i. \return true if the triangle with index i
+ was found, false otherwise.
The following sample code shows how to use this function:
\code
@@ -358,17 +351,17 @@ bool vpTemplateTrackerZone::inZone(const double &i,const double &j, unsigned int
}
\endcode
*/
-void vpTemplateTrackerZone::getTriangle(unsigned int i,vpTemplateTrackerTriangle &T) const
+void vpTemplateTrackerZone::getTriangle(unsigned int i, vpTemplateTrackerTriangle &T) const
{
- if (i > getNbTriangle()-1)
+ if (i > getNbTriangle() - 1)
throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
T = Zone[i];
}
/*!
- A zone is defined by a set of triangles. This function returns the ith triangle.
- \param i : Index of the triangle to return.
- \return The triangle corresponding to index i.
+ A zone is defined by a set of triangles. This function returns the ith
+ triangle. \param i : Index of the triangle to return. \return The triangle
+ corresponding to index i.
The following sample code shows how to use this function:
\code
@@ -381,7 +374,7 @@ void vpTemplateTrackerZone::getTriangle(unsigned int i,vpTemplateTrackerTriangle
*/
vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) const
{
- if (i > getNbTriangle()-1)
+ if (i > getNbTriangle() - 1)
throw(vpException(vpException::badValue, "Cannot get triangle with index %u", i));
return Zone[i];
@@ -392,60 +385,46 @@ vpTemplateTrackerTriangle vpTemplateTrackerZone::getTriangle(unsigned int i) con
*/
vpImagePoint vpTemplateTrackerZone::getCenter() const
{
- double xc=0;
- double yc=0;
- int cpt=0;
- for(int i=min_y;i<max_y;i++)
- for(int j=min_x;j<max_x;j++)
- if(inZone(i,j))
- {
- xc+=j;
- yc+=i;
- cpt ++;
+ double xc = 0;
+ double yc = 0;
+ int cpt = 0;
+ for (int i = min_y; i < max_y; i++)
+ for (int j = min_x; j < max_x; j++)
+ if (inZone(i, j)) {
+ xc += j;
+ yc += i;
+ cpt++;
}
- if(! cpt) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot compute the zone center: size = 0")) ;
+ if (!cpt) {
+ throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
}
- xc=xc/cpt;
- yc=yc/cpt;
+ xc = xc / cpt;
+ yc = yc / cpt;
vpImagePoint ip;
ip.set_uv(xc, yc);
return ip;
}
/*!
- \return The maximal x coordinate (along the columns of the image) of the points that are in the zone.
- \sa getMinx(), getBoundingBox()
+ \return The maximal x coordinate (along the columns of the image) of the
+ points that are in the zone. \sa getMinx(), getBoundingBox()
*/
-int vpTemplateTrackerZone::getMaxx() const
-{
- return max_x;
-}
+int vpTemplateTrackerZone::getMaxx() const { return max_x; }
/*!
- \return The maximal y coordinate (along the rows of the image) of the points that are in the zone.
- \sa getMiny(), getBoundingBox()
+ \return The maximal y coordinate (along the rows of the image) of the points
+ that are in the zone. \sa getMiny(), getBoundingBox()
*/
-int vpTemplateTrackerZone::getMaxy() const
-{
- return max_y;
-}
+int vpTemplateTrackerZone::getMaxy() const { return max_y; }
/*!
- \return The minimal x coordinate (along the columns of the image) of the points that are in the zone.
- \sa getMaxx(), getBoundingBox()
+ \return The minimal x coordinate (along the columns of the image) of the
+ points that are in the zone. \sa getMaxx(), getBoundingBox()
*/
-int vpTemplateTrackerZone::getMinx() const
-{
- return min_x;
-}
+int vpTemplateTrackerZone::getMinx() const { return min_x; }
/*!
- \return The minimal y coordinate (along the rows of the image) of the points that are in the zone.
- \sa getMaxy(), getBoundingBox()
+ \return The minimal y coordinate (along the rows of the image) of the points
+ that are in the zone. \sa getMaxy(), getBoundingBox()
*/
-int vpTemplateTrackerZone::getMiny() const
-{
- return min_y;
-}
+int vpTemplateTrackerZone::getMiny() const { return min_y; }
/*!
Return a rectangle that defines the bounding box of the zone.
@@ -460,15 +439,14 @@ vpRect vpTemplateTrackerZone::getBoundingBox() const
}
/*!
- If a display device is associated to image \c I, display in overlay the triangles that define the zone.
- \param I : Image.
- \param col : Color used to display the triangles.
- \param thickness : Thickness of the triangle lines.
+ If a display device is associated to image \c I, display in overlay the
+ triangles that define the zone. \param I : Image. \param col : Color used to
+ display the triangles. \param thickness : Thickness of the triangle lines.
*/
void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
{
std::vector<vpImagePoint> ip;
- for (unsigned int i=0; i < Zone.size(); i++) {
+ for (unsigned int i = 0; i < Zone.size(); i++) {
vpTemplateTrackerTriangle triangle;
Zone[i].getCorners(ip);
vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
@@ -478,15 +456,14 @@ void vpTemplateTrackerZone::display(const vpImage<unsigned char> &I, const vpCol
}
/*!
- If a display device is associated to image \c I, display in overlay the triangles that define the zone.
- \param I : Image.
- \param col : Color used to display the triangles.
- \param thickness : Thickness of the triangle lines.
+ If a display device is associated to image \c I, display in overlay the
+ triangles that define the zone. \param I : Image. \param col : Color used to
+ display the triangles. \param thickness : Thickness of the triangle lines.
*/
void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
{
std::vector<vpImagePoint> ip;
- for (unsigned int i=0; i < Zone.size(); i++) {
+ for (unsigned int i = 0; i < Zone.size(); i++) {
vpTemplateTrackerTriangle triangle;
Zone[i].getCorners(ip);
vpDisplay::displayLine(I, ip[0], ip[1], col, thickness);
@@ -498,10 +475,7 @@ void vpTemplateTrackerZone::display(const vpImage<vpRGBa> &I, const vpColor &col
/*!
Destructor.
*/
-vpTemplateTrackerZone::~vpTemplateTrackerZone()
-{
- clear();
-}
+vpTemplateTrackerZone::~vpTemplateTrackerZone() { clear(); }
/*!
Modify all the pixels inside a triangle with a given gray level.
@@ -510,18 +484,15 @@ vpTemplateTrackerZone::~vpTemplateTrackerZone()
of triangles used to define the zone and available using getNbTriangle().
\param gray_level: Color used to fill the triangle with.
*/
-void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char>& I, unsigned int id,unsigned char gray_level)
+void vpTemplateTrackerZone::fillTriangle(vpImage<unsigned char> &I, unsigned int id, unsigned char gray_level)
{
assert(id < getNbTriangle());
vpTemplateTrackerTriangle triangle;
getTriangle(id, triangle);
- for (int i=0 ; i < (int) I.getHeight() ; i++)
- {
- for (int j=0 ; j < (int) I.getWidth() ; j++)
- {
- if(triangle.inTriangle(i,j))
- {
- I[i][j]=gray_level;
+ for (int i = 0; i < (int)I.getHeight(); i++) {
+ for (int j = 0; j < (int)I.getWidth(); j++) {
+ if (triangle.inTriangle(i, j)) {
+ I[i][j] = gray_level;
}
}
}
@@ -535,10 +506,9 @@ vpTemplateTrackerZone vpTemplateTrackerZone::getPyramidDown() const
vpTemplateTrackerZone tempZone;
vpTemplateTrackerTriangle Ttemp;
vpTemplateTrackerTriangle TtempDown;
- for(unsigned int i=0;i<getNbTriangle();i++)
- {
- getTriangle(i,Ttemp);
- TtempDown=Ttemp.getPyramidDown();
+ for (unsigned int i = 0; i < getNbTriangle(); i++) {
+ getTriangle(i, Ttemp);
+ TtempDown = Ttemp.getPyramidDown();
tempZone.add(TtempDown);
}
return tempZone;
@@ -549,22 +519,21 @@ vpTemplateTrackerZone vpTemplateTrackerZone::getPyramidDown() const
update the zone bounding box.
\param z : Zone with a set of triangles provided as input.
*/
-void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone& z)
+void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone &z)
{
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<z.getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < z.getNbTriangle(); i++) {
z.getTriangle(i, triangle);
add(triangle);
// Update the bounding box
- if((triangle.getMinx()<min_x)||(min_x==-1))
- min_x=(int)triangle.getMinx();
- if((triangle.getMaxx()>max_x)||(max_x==-1))
- max_x=(int)triangle.getMaxx();
- if((triangle.getMiny()<min_y)||(min_y==-1))
- min_y=(int)triangle.getMiny();
- if((triangle.getMaxy()>max_y)||(max_y==-1))
- max_y=(int)triangle.getMaxy();
+ if ((triangle.getMinx() < min_x) || (min_x == -1))
+ min_x = (int)triangle.getMinx();
+ if ((triangle.getMaxx() > max_x) || (max_x == -1))
+ max_x = (int)triangle.getMaxx();
+ if ((triangle.getMiny() < min_y) || (min_y == -1))
+ min_y = (int)triangle.getMiny();
+ if ((triangle.getMaxy() > max_y) || (max_y == -1))
+ max_y = (int)triangle.getMaxy();
}
}
@@ -577,24 +546,22 @@ void vpTemplateTrackerZone::copy(const vpTemplateTrackerZone& z)
vpImagePoint vpTemplateTrackerZone::getCenter(int borne_x, int borne_y) const
{
- int cpt_pt=0;
- double x_center=0,y_center=0;
- for(int j=0;j<borne_x;j++)
- for(int i=0;i<borne_y;i++)
- if(inZone(i,j))
- {
- x_center+=j;
- y_center+=i;
+ int cpt_pt = 0;
+ double x_center = 0, y_center = 0;
+ for (int j = 0; j < borne_x; j++)
+ for (int i = 0; i < borne_y; i++)
+ if (inZone(i, j)) {
+ x_center += j;
+ y_center += i;
cpt_pt++;
}
- if(! cpt_pt) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot compute the zone center: size = 0")) ;
+ if (!cpt_pt) {
+ throw(vpException(vpException::divideByZeroError, "Cannot compute the zone center: size = 0"));
}
- x_center=x_center/cpt_pt;
- y_center=y_center/cpt_pt;
+ x_center = x_center / cpt_pt;
+ y_center = y_center / cpt_pt;
vpImagePoint center;
center.set_uv(x_center, y_center);
return center;
@@ -607,10 +574,9 @@ double vpTemplateTrackerZone::getArea() const
{
double area = 0;
vpTemplateTrackerTriangle triangle;
- for (unsigned int i=0; i < getNbTriangle(); i++) {
+ for (unsigned int i = 0; i < getNbTriangle(); i++) {
getTriangle(i, triangle);
area += triangle.getArea();
}
return area;
}
-
diff --git a/modules/tracker/tt/src/vpTemplateTracker.cpp b/modules/tracker/tt/src/vpTemplateTracker.cpp
index dff2cac..4dd82f7 100644
--- a/modules/tracker/tt/src/vpTemplateTracker.cpp
+++ b/modules/tracker/tt/src/vpTemplateTracker.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,123 +42,123 @@
#include <visp3/tt/vpTemplateTrackerBSpline.h>
vpTemplateTracker::vpTemplateTracker(vpTemplateTrackerWarp *_warp)
- : nbLvlPyr(1), l0Pyr(0), pyrInitialised(false), ptTemplate(NULL), ptTemplatePyr(NULL),
- ptTemplateInit(false), templateSize(0), templateSizePyr(NULL),
- ptTemplateSelect(NULL), ptTemplateSelectPyr(NULL), ptTemplateSelectInit(false),
- templateSelectSize(0), ptTemplateSupp(NULL), ptTemplateSuppPyr(NULL),
- ptTemplateCompo(NULL), ptTemplateCompoPyr(NULL), zoneTracked(NULL), zoneTrackedPyr(NULL),
- pyr_IDes(NULL), H(), Hdesire(), HdesirePyr(), HLM(), HLMdesire(), HLMdesirePyr(),
- HLMdesireInverse(), HLMdesireInversePyr(), G(), gain(1.), thresholdGradient(40),
- costFunctionVerification(false), blur(true), useBrent(false), nbIterBrent(3),
- taillef(7), fgG(NULL), fgdG(NULL), ratioPixelIn(0), mod_i(1), mod_j(1), nbParam(0),
- lambdaDep(0.001), iterationMax(30), iterationGlobale(0), diverge(false), nbIteration(0),
- useCompositionnal(true), useInverse(false), Warp(_warp), p(0), dp(), X1(), X2(),
- dW(), BI(), dIx(), dIy(), zoneRef_()
+ : 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() ;
+ nbParam = Warp->getNbParam();
p.resize(nbParam);
dp.resize(nbParam);
- fgG=new double[(taillef+1)/2] ;
- vpImageFilter::getGaussianKernel(fgG, taillef) ;
+ fgG = new double[(taillef + 1) / 2];
+ vpImageFilter::getGaussianKernel(fgG, taillef);
- fgdG=new double[(taillef+1)/2] ;
- vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
+ fgdG = new double[(taillef + 1) / 2];
+ vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef);
}
void vpTemplateTracker::setGaussianFilterSize(unsigned int new_taill)
{
- taillef=new_taill;
- if (fgG) delete[] fgG;
- fgG=new double[taillef] ;
- vpImageFilter::getGaussianKernel(fgG, taillef) ;
-
- if (fgdG) delete[] fgdG;
- fgdG=new double[taillef] ;
- vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef) ;
+ taillef = new_taill;
+ if (fgG)
+ delete[] fgG;
+ fgG = new double[taillef];
+ vpImageFilter::getGaussianKernel(fgG, taillef);
+
+ if (fgdG)
+ delete[] fgdG;
+ fgdG = new double[taillef];
+ vpImageFilter::getGaussianDerivativeKernel(fgdG, taillef);
}
-void vpTemplateTracker::initTracking(const vpImage<unsigned char>& I, vpTemplateTrackerZone &zone)
+void vpTemplateTracker::initTracking(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone)
{
// std::cout<<"\tInitialise reference..."<<std::endl;
- zoneTracked=&zone;
+ zoneTracked = &zone;
- int largeur_im=(int)I.getWidth();
- int hauteur_im=(int)I.getHeight();
+ int largeur_im = (int)I.getWidth();
+ int hauteur_im = (int)I.getHeight();
- unsigned int NbPointDsZone=0;
- //double xtotal=0,ytotal=0;
- int mod_fi,mod_fj;
- mod_fi=mod_i;mod_fj=mod_i;
+ unsigned int NbPointDsZone = 0;
+ // double xtotal=0,ytotal=0;
+ int mod_fi, mod_fj;
+ mod_fi = mod_i;
+ mod_fj = mod_i;
- for(int i=0;i<hauteur_im;i+=mod_fi) {
- for(int j=0;j<largeur_im;j+=mod_fj) {
- if(zone.inZone(i,j)) {
+ for (int i = 0; i < hauteur_im; i += mod_fi) {
+ for (int j = 0; j < largeur_im; j += mod_fj) {
+ if (zone.inZone(i, j)) {
NbPointDsZone++;
- //xtotal+=j;
- //ytotal+=i;
+ // xtotal+=j;
+ // ytotal+=i;
}
}
}
- //Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
+ // Warp->setCentre((double)xtotal/NbPointDsZone,(double)ytotal/NbPointDsZone);
- templateSize=NbPointDsZone;
- ptTemplate = new vpTemplateTrackerPoint[templateSize];ptTemplateInit=true;
- ptTemplateSelect = new bool[templateSize];ptTemplateSelectInit=true;
+ templateSize = NbPointDsZone;
+ ptTemplate = new vpTemplateTrackerPoint[templateSize];
+ ptTemplateInit = true;
+ ptTemplateSelect = new bool[templateSize];
+ ptTemplateSelectInit = true;
- Hdesire.resize(nbParam,nbParam);
- HLMdesire.resize(nbParam,nbParam);
+ Hdesire.resize(nbParam, nbParam);
+ HLMdesire.resize(nbParam, nbParam);
vpTemplateTrackerPoint pt;
- //vpTemplateTrackerZPoint ptZ;
- vpImage<double> GaussI ;
- vpImageFilter::filter(I, GaussI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
-
- unsigned int cpt_point=0;
- templateSelectSize=0;
- for(int i=0;i<hauteur_im;i+=mod_i)
- {
- //for(int j=minx_t;j<maxx_t;j++)
- for(int j=0;j<largeur_im;j+=mod_j)
- {
-// if(i%mod_i ==0 && j%mod_j ==0)
- if(zone.inZone(i,j))
- {
- pt.x=j;
- pt.y=i;
-
- pt.dx=dIx[i][j];
- pt.dy=dIy[i][j];
-
- if(pt.dx*pt.dx+pt.dy*pt.dy>thresholdGradient)
- {
- ptTemplateSelect[cpt_point]=true;
- templateSelectSize++;
- }
- else
- ptTemplateSelect[cpt_point]=false;
- //ptTemplate_select[cpt_point]=true;
-
- /*if(blur)
- pt.val=GaussI[i][j];
- else
- pt.val=I[i][j];*/
- pt.val=vpTemplateTrackerBSpline::getSubPixBspline4(GaussI,i,j);
- //ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
+ // vpTemplateTrackerZPoint ptZ;
+ vpImage<double> GaussI;
+ vpImageFilter::filter(I, GaussI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
+
+ unsigned int cpt_point = 0;
+ templateSelectSize = 0;
+ for (int i = 0; i < hauteur_im; i += mod_i) {
+ // for(int j=minx_t;j<maxx_t;j++)
+ for (int j = 0; j < largeur_im; j += mod_j) {
+ // if(i%mod_i ==0 && j%mod_j ==0)
+ if (zone.inZone(i, j)) {
+ pt.x = j;
+ pt.y = i;
+
+ pt.dx = dIx[i][j];
+ pt.dy = dIy[i][j];
+
+ if (pt.dx * pt.dx + pt.dy * pt.dy > thresholdGradient) {
+ ptTemplateSelect[cpt_point] = true;
+ templateSelectSize++;
+ } else
+ ptTemplateSelect[cpt_point] = false;
+ // ptTemplate_select[cpt_point]=true;
+
+ /*if(blur)
+ pt.val=GaussI[i][j];
+ else
+ pt.val=I[i][j];*/
+ pt.val = vpTemplateTrackerBSpline::getSubPixBspline4(GaussI, i, j);
+ // ptZone_pyr[NbLevelPyramid-cpt].push_back(pt);
- ptTemplate[cpt_point]=pt;
- cpt_point++;
- }
+ ptTemplate[cpt_point] = pt;
+ cpt_point++;
+ }
}
}
// std::cout<<"\tNb pt template apres scale:"<<cpt_point<<std::endl;
- // std::cout<<"utilisation de "<<taille_template_select<<"/"<<cpt_point<<" = "<<100.*taille_template_select/cpt_point<<"% pour calcul derivees"<<std::endl;
+ // std::cout<<"utilisation de
+ // "<<taille_template_select<<"/"<<cpt_point<<" =
+ // "<<100.*taille_template_select/cpt_point<<"% pour calcul
+ // derivees"<<std::endl;
- templateSize=cpt_point;
+ templateSize = cpt_point;
GaussI.destroy();
// std::cout<<"\tEnd of reference initialisation ..."<<std::endl;
}
@@ -169,10 +170,11 @@ vpTemplateTracker::~vpTemplateTracker()
delete[] fgdG;
resetTracker();
-}
+}
/*!
- Reset the tracker by freeing the memory allocated by the template tracker during the initialization.
+ Reset the tracker by freeing the memory allocated by the template tracker
+ during the initialization.
*/
void vpTemplateTracker::resetTracker()
{
@@ -180,29 +182,25 @@ void vpTemplateTracker::resetTracker()
p = 0;
// vpTRACE("resetTracking");
- if(pyrInitialised)
- {
- if(ptTemplatePyr){
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
- if(ptTemplatePyr[i]){
- for(unsigned int point=0;point<templateSizePyr[i];point++)
- {
- delete[] ptTemplatePyr[i][point].dW;
- delete[] ptTemplatePyr[i][point].HiG;
- }
- delete[] ptTemplatePyr[i];
+ if (pyrInitialised) {
+ if (ptTemplatePyr) {
+ for (unsigned int i = 0; i < nbLvlPyr; i++) {
+ if (ptTemplatePyr[i]) {
+ for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
+ delete[] ptTemplatePyr[i][point].dW;
+ delete[] ptTemplatePyr[i][point].HiG;
}
+ delete[] ptTemplatePyr[i];
}
- delete[] ptTemplatePyr;
- ptTemplatePyr = NULL;
+ }
+ delete[] ptTemplatePyr;
+ ptTemplatePyr = NULL;
}
if (ptTemplateCompoPyr) {
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
+ for (unsigned int i = 0; i < nbLvlPyr; i++) {
if (ptTemplateCompoPyr[i]) {
- for(unsigned int point=0;point<templateSizePyr[i];point++) {
+ for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
delete[] ptTemplateCompoPyr[i][point].dW;
}
delete[] ptTemplateCompoPyr[i];
@@ -213,10 +211,9 @@ void vpTemplateTracker::resetTracker()
}
if (ptTemplateSuppPyr) {
- for(unsigned int i=0;i<nbLvlPyr;i++)
- {
+ for (unsigned int i = 0; i < nbLvlPyr; i++) {
if (ptTemplateSuppPyr[i]) {
- for(unsigned int point=0;point<templateSizePyr[i];point++) {
+ for (unsigned int point = 0; point < templateSizePyr[i]; point++) {
delete[] ptTemplateSuppPyr[i][point].Bt;
delete[] ptTemplateSuppPyr[i][point].BtInit;
delete[] ptTemplateSuppPyr[i][point].dBt;
@@ -231,51 +228,47 @@ void vpTemplateTracker::resetTracker()
ptTemplateSuppPyr = NULL;
}
- if(ptTemplateSelectPyr){
- for(unsigned int i=0;i<nbLvlPyr;i++){
- if(ptTemplateSelectPyr[i])
- delete[] ptTemplateSelectPyr[i];
- }
- delete[] ptTemplateSelectPyr;
- ptTemplateSelectPyr = NULL;
+ if (ptTemplateSelectPyr) {
+ for (unsigned int i = 0; i < nbLvlPyr; i++) {
+ if (ptTemplateSelectPyr[i])
+ delete[] ptTemplateSelectPyr[i];
+ }
+ delete[] ptTemplateSelectPyr;
+ ptTemplateSelectPyr = NULL;
}
- if(templateSizePyr){
- delete[] templateSizePyr;
- templateSizePyr = NULL;
+ if (templateSizePyr) {
+ delete[] templateSizePyr;
+ templateSizePyr = NULL;
}
- if(HdesirePyr) {
- delete[] HdesirePyr;
- HdesirePyr = NULL;
+ if (HdesirePyr) {
+ delete[] HdesirePyr;
+ HdesirePyr = NULL;
}
- if(HLMdesirePyr){
- delete[] HLMdesirePyr;
- HLMdesirePyr = NULL;
+ if (HLMdesirePyr) {
+ delete[] HLMdesirePyr;
+ HLMdesirePyr = NULL;
}
- if(HLMdesireInversePyr){
- delete[] HLMdesireInversePyr;
- HLMdesireInversePyr = NULL;
+ if (HLMdesireInversePyr) {
+ delete[] HLMdesireInversePyr;
+ HLMdesireInversePyr = NULL;
}
- if(zoneTrackedPyr){
- delete[] zoneTrackedPyr;
- zoneTrackedPyr = NULL;
+ if (zoneTrackedPyr) {
+ delete[] zoneTrackedPyr;
+ zoneTrackedPyr = NULL;
}
- if(pyr_IDes){
- delete[] pyr_IDes;
- pyr_IDes = NULL;
+ if (pyr_IDes) {
+ delete[] pyr_IDes;
+ pyr_IDes = NULL;
}
- }
- else
- {
- if(ptTemplateInit)
- {
- for(unsigned int point=0;point<templateSize;point++)
- {
+ } else {
+ if (ptTemplateInit) {
+ for (unsigned int point = 0; point < templateSize; point++) {
delete[] ptTemplate[point].dW;
delete[] ptTemplate[point].HiG;
}
@@ -284,16 +277,14 @@ void vpTemplateTracker::resetTracker()
ptTemplateInit = false;
}
if (ptTemplateCompo) {
- for(unsigned int point=0;point<templateSize;point++)
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
delete[] ptTemplateCompo[point].dW;
}
delete[] ptTemplateCompo;
ptTemplateCompo = NULL;
}
if (ptTemplateSupp) {
- for(unsigned int point=0;point<templateSize;point++)
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
delete[] ptTemplateSupp[point].Bt;
delete[] ptTemplateSupp[point].BtInit;
delete[] ptTemplateSupp[point].dBt;
@@ -304,13 +295,14 @@ void vpTemplateTracker::resetTracker()
delete[] ptTemplateSupp;
ptTemplateSupp = NULL;
}
- if(ptTemplateSelectInit) {
+ if (ptTemplateSelectInit) {
if (ptTemplateSelect) {
- delete[] ptTemplateSelect; ptTemplateSelect = NULL;
+ delete[] ptTemplateSelect;
+ ptTemplateSelect = NULL;
}
}
}
-}
+}
/*!
Display the warped reference template in an image.
@@ -345,7 +337,7 @@ int main()
}
\endcode
*/
-void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor& col, const unsigned int thickness)
+void vpTemplateTracker::display(const vpImage<unsigned char> &I, const vpColor &col, const unsigned int thickness)
{
if (I.display) { // Only if a display is associated to the image
vpTemplateTrackerZone zoneWarped;
@@ -387,7 +379,7 @@ int main()
}
\endcode
*/
-void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor& col, const unsigned int thickness)
+void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor &col, const unsigned int thickness)
{
if (I.display) { // Only if a display is associated to the image
vpTemplateTrackerZone zoneWarped;
@@ -396,139 +388,134 @@ void vpTemplateTracker::display(const vpImage<vpRGBa> &I, const vpColor& col, co
}
}
-void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I,vpColVector &tp,double tMI,vpColVector &direction,double &alpha)
+void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I, vpColVector &tp, double tMI,
+ vpColVector &direction, double &alpha)
{
vpColVector **ptp;
- ptp=new vpColVector*[4];
+ ptp = new vpColVector *[4];
vpColVector p0(nbParam);
- p0=tp;
+ p0 = tp;
- //valeur necessaire si conditionnel
+ // valeur necessaire si conditionnel
vpColVector dpt(Warp->getNbParam());
vpColVector adpt(Warp->getNbParam());
vpColVector p1(nbParam);
- if(useCompositionnal)
- {
- if(useInverse)
- Warp->getParamInverse(direction,dpt);
+ if (useCompositionnal) {
+ if (useInverse)
+ Warp->getParamInverse(direction, dpt);
else
- dpt=direction;
- Warp->pRondp(tp,dpt,p1);
- }
- else
- {
- p1=tp+direction;
+ dpt = direction;
+ Warp->pRondp(tp, dpt, p1);
+ } else {
+ p1 = tp + direction;
}
vpColVector p2(nbParam);
- if(useCompositionnal)
- {
- adpt=alpha*direction;
- if(useInverse)
- Warp->getParamInverse(adpt,dpt);
+ if (useCompositionnal) {
+ adpt = alpha * direction;
+ if (useInverse)
+ Warp->getParamInverse(adpt, dpt);
else
- dpt=adpt;
- Warp->pRondp(tp,dpt,p2);
- }
- else
- {
- p2=tp+alpha*direction;
+ dpt = adpt;
+ Warp->pRondp(tp, dpt, p2);
+ } else {
+ p2 = tp + alpha * direction;
}
vpColVector p3(nbParam);
- ptp[0]=&p0;
- ptp[1]=&p1;
- ptp[2]=&p2;
- ptp[3]=&p3;
-
-
- double *Cost=new double[4];
- //Cost[0]=getCost(I,p0);
- Cost[0]=tMI;
- Cost[1]=getCost(I,p1);
- Cost[2]=getCost(I,p2);
-
-
- double *talpha=new double[4];
- talpha[0]=0;
- talpha[1]=1.;
- talpha[2]=alpha;
-
- //for(int i=0;i<3;i++)
- // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] = "<<Cost[i]<<std::endl;
-
- //Utilise trois estimï¿œes de paraboles succesive ...
- //A changer pour rendre adaptable
- for(unsigned int opt=0;opt<nbIterBrent;opt++)
- {
- //double a=talpha[0];
- //double b=talpha[1];
- //double c=talpha[2];
- //double Cost0=Cost[0];
- //double Cost1=Cost[1];
- //double Cost2=Cost[2];
-
- vpMatrix A(3,3);
- for(unsigned int i=0;i<3;i++){A[i][0]=talpha[i]*talpha[i];A[i][1]=talpha[i];A[i][2]=1.;}
- //std::cout<<"A="<<A<<std::endl;
- vpColVector B(3);for(unsigned int i=0;i<3;i++)B[i]=Cost[i];
- vpColVector parabol(3);parabol=(A.t()*A).inverseByLU()*A.t()*B;
- //vpColVector parabol(3);parabol=A.pseudoInverse()*B;
-
- //si convexe
- if(parabol[0]>0)
- {
- talpha[3]=-0.5*parabol[1]/parabol[0];
- //std::cout<<"parabol = "<<parabol<<std::endl;
- //std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
+ ptp[0] = &p0;
+ ptp[1] = &p1;
+ ptp[2] = &p2;
+ ptp[3] = &p3;
+
+ double *Cost = new double[4];
+ // Cost[0]=getCost(I,p0);
+ Cost[0] = tMI;
+ Cost[1] = getCost(I, p1);
+ Cost[2] = getCost(I, p2);
+
+ double *talpha = new double[4];
+ talpha[0] = 0;
+ talpha[1] = 1.;
+ talpha[2] = alpha;
+
+ // for(int i=0;i<3;i++)
+ // std::cout<<"alpha["<<i<<"] = "<<talpha[i]<<" Cost["<<i<<"] =
+ //"<<Cost[i]<<std::endl;
+
+ // Utilise trois estimï¿œes de paraboles succesive ...
+ // A changer pour rendre adaptable
+ for (unsigned int opt = 0; opt < nbIterBrent; opt++) {
+ // double a=talpha[0];
+ // double b=talpha[1];
+ // double c=talpha[2];
+ // double Cost0=Cost[0];
+ // double Cost1=Cost[1];
+ // double Cost2=Cost[2];
+
+ vpMatrix A(3, 3);
+ for (unsigned int i = 0; i < 3; i++) {
+ A[i][0] = talpha[i] * talpha[i];
+ A[i][1] = talpha[i];
+ A[i][2] = 1.;
}
- else//si concave
+ // std::cout<<"A="<<A<<std::endl;
+ vpColVector B(3);
+ for (unsigned int i = 0; i < 3; i++)
+ B[i] = Cost[i];
+ vpColVector parabol(3);
+ parabol = (A.t() * A).inverseByLU() * A.t() * B;
+ // vpColVector parabol(3);parabol=A.pseudoInverse()*B;
+
+ // si convexe
+ if (parabol[0] > 0) {
+ talpha[3] = -0.5 * parabol[1] / parabol[0];
+ // std::cout<<"parabol = "<<parabol<<std::endl;
+ // std::cout<<"convexe talpha = "<<talpha[3]<<std::endl;
+ } else // si concave
{
- int tindic_x_min=0;
- int tindic_x_max=0;
- for(int i=1;i<3;i++)
- {
- if(talpha[i]<talpha[tindic_x_min])
- tindic_x_min=i;
- if(talpha[i]>talpha[tindic_x_max])
- tindic_x_max=i;
+ int tindic_x_min = 0;
+ int tindic_x_max = 0;
+ for (int i = 1; i < 3; i++) {
+ if (talpha[i] < talpha[tindic_x_min])
+ tindic_x_min = i;
+ if (talpha[i] > talpha[tindic_x_max])
+ tindic_x_max = i;
}
- if(Cost[tindic_x_max]<Cost[tindic_x_min])
- {
- //talpha[3]=talpha[tindic_x_max]+1.;
- talpha[3]=talpha[tindic_x_max]+1.;
+ if (Cost[tindic_x_max] < Cost[tindic_x_min]) {
+ // talpha[3]=talpha[tindic_x_max]+1.;
+ talpha[3] = talpha[tindic_x_max] + 1.;
/*if(talpha[tindic_x_min]>talpha[tindic_x_max])
talpha[3]=talpha[tindic_x_min]+1.;
else
talpha[3]=talpha[tindic_x_min]-1.;*/
- }
- else
- {
- //talpha[3]=talpha[tindic_x_min]-1.;
- talpha[3]=talpha[tindic_x_min]-1.;
+ } else {
+ // talpha[3]=talpha[tindic_x_min]-1.;
+ talpha[3] = talpha[tindic_x_min] - 1.;
/*if(talpha[tindic_x_min]<talpha[tindic_x_max])
talpha[3]=talpha[tindic_x_max]+1.;
else
talpha[3]=talpha[tindic_x_max]-1.;*/
}
- //std::cout<<"concave talpha="<<talpha[3]<<std::endl;
+ // std::cout<<"concave talpha="<<talpha[3]<<std::endl;
}
- //std::cout<<"talpha="<<talpha[3]<<std::endl;
- int indic_x_min=0;
- int indic_x_max=0;
- for(int i=1;i<3;i++)
- {
- if(talpha[i]<talpha[indic_x_min])
- indic_x_min=i;
- if(talpha[i]>talpha[indic_x_max])
- indic_x_max=i;
+ // std::cout<<"talpha="<<talpha[3]<<std::endl;
+ int indic_x_min = 0;
+ int indic_x_max = 0;
+ for (int i = 1; i < 3; i++) {
+ if (talpha[i] < talpha[indic_x_min])
+ indic_x_min = i;
+ if (talpha[i] > talpha[indic_x_max])
+ indic_x_max = i;
}
- //std::cout<<"talpha = "<<talpha[3]<<std::endl;
- if(talpha[3]>talpha[indic_x_max])
- if((talpha[3]-talpha[indic_x_max])>alpha)talpha[3]=talpha[indic_x_max]+4.;
- if(talpha[3]<talpha[indic_x_min])
- if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-4.;
+ // std::cout<<"talpha = "<<talpha[3]<<std::endl;
+ if (talpha[3] > talpha[indic_x_max])
+ if ((talpha[3] - talpha[indic_x_max]) > alpha)
+ talpha[3] = talpha[indic_x_max] + 4.;
+ if (talpha[3] < talpha[indic_x_min])
+ if ((talpha[indic_x_min] - talpha[3]) > alpha)
+ talpha[3] = talpha[indic_x_min] - 4.;
/*if(((b-a)*(Cost1-Cost2)-(b-c)*(Cost1-Cost0))==0)
{Cost[3]=1000;break;}
@@ -550,109 +537,102 @@ void vpTemplateTracker::computeOptimalBrentGain(const vpImage<unsigned char> &I,
if(talpha[3]<talpha[indic_x_min])
if((talpha[indic_x_min]-talpha[3])>alpha)talpha[3]=talpha[indic_x_min]-alpha;*/
- //p3=tp-talpha[3]*direction;
- if(useCompositionnal)
- {
- adpt=talpha[3]*direction;
- if(useInverse)
- Warp->getParamInverse(adpt,dpt);
+ // p3=tp-talpha[3]*direction;
+ if (useCompositionnal) {
+ adpt = talpha[3] * direction;
+ if (useInverse)
+ Warp->getParamInverse(adpt, dpt);
else
- dpt=adpt;
- Warp->pRondp(tp,dpt,p3);
+ dpt = adpt;
+ Warp->pRondp(tp, dpt, p3);
+ } else {
+ p3 = tp + talpha[3] * direction;
}
- else
- {
- p3=tp+talpha[3]*direction;
- }
-
- Cost[3]=getCost(I,p3);
- //std::cout<<"new cost="<<Cost[3]<<std::endl;
- int indice_f_max=0;
- for(int i=1;i<4;i++)
- if(Cost[i]>Cost[indice_f_max])
- indice_f_max=i;
- if(indice_f_max!=3)
- {
- *ptp[indice_f_max]=*ptp[3];
- Cost[indice_f_max]=Cost[3];
- talpha[indice_f_max]=talpha[3];
- }
- else
+ Cost[3] = getCost(I, p3);
+ // std::cout<<"new cost="<<Cost[3]<<std::endl;
+
+ int indice_f_max = 0;
+ for (int i = 1; i < 4; i++)
+ if (Cost[i] > Cost[indice_f_max])
+ indice_f_max = i;
+ if (indice_f_max != 3) {
+ *ptp[indice_f_max] = *ptp[3];
+ Cost[indice_f_max] = Cost[3];
+ talpha[indice_f_max] = talpha[3];
+ } else
break;
}
- int indice_f_min=0;
- for(int i=0;i<4;i++)
- if(Cost[i]<Cost[indice_f_min])
- indice_f_min=i;
+ int indice_f_min = 0;
+ for (int i = 0; i < 4; i++)
+ if (Cost[i] < Cost[indice_f_min])
+ indice_f_min = i;
- alpha=talpha[indice_f_min];
+ alpha = talpha[indice_f_min];
- if(alpha<1)alpha=1.;
+ if (alpha < 1)
+ alpha = 1.;
delete[] ptp;
delete[] Cost;
delete[] talpha;
}
-
/*!
\param nbLvl : Number of levels in the pyramid.
- \param l0 : Pyramid level where the tracking is stopped. The level with the highest resolution is 0.
+ \param l0 : Pyramid level where the tracking is stopped. The level with the
+ highest resolution is 0.
*/
void vpTemplateTracker::initPyramidal(unsigned int nbLvl, unsigned int l0)
{
// vpTRACE("init_pyramidal");
- nbLvlPyr=nbLvl;
- l0Pyr=l0;
-
- zoneTrackedPyr=new vpTemplateTrackerZone[nbLvlPyr];
- pyr_IDes=new vpImage<unsigned char>[nbLvlPyr];
- ptTemplatePyr=new vpTemplateTrackerPoint*[nbLvlPyr];
- ptTemplateSelectPyr=new bool*[nbLvlPyr];
- ptTemplateSuppPyr=new vpTemplateTrackerPointSuppMIInv*[nbLvlPyr];
- ptTemplateCompoPyr=new vpTemplateTrackerPointCompo*[nbLvlPyr];
- for(unsigned int i=0; i< nbLvlPyr; i++) {
- ptTemplatePyr[i] = NULL;
- ptTemplateSuppPyr[i] = NULL;
+ nbLvlPyr = nbLvl;
+ l0Pyr = l0;
+
+ zoneTrackedPyr = new vpTemplateTrackerZone[nbLvlPyr];
+ pyr_IDes = new vpImage<unsigned char>[nbLvlPyr];
+ ptTemplatePyr = new vpTemplateTrackerPoint *[nbLvlPyr];
+ ptTemplateSelectPyr = new bool *[nbLvlPyr];
+ ptTemplateSuppPyr = new vpTemplateTrackerPointSuppMIInv *[nbLvlPyr];
+ ptTemplateCompoPyr = new vpTemplateTrackerPointCompo *[nbLvlPyr];
+ for (unsigned int i = 0; i < nbLvlPyr; i++) {
+ ptTemplatePyr[i] = NULL;
+ ptTemplateSuppPyr[i] = NULL;
ptTemplateSelectPyr[i] = NULL;
- ptTemplateCompoPyr[i] = NULL;
+ ptTemplateCompoPyr[i] = NULL;
}
- templateSizePyr=new unsigned int[nbLvlPyr];
- HdesirePyr=new vpMatrix[nbLvlPyr];
- HLMdesirePyr=new vpMatrix[nbLvlPyr];
- HLMdesireInversePyr=new vpMatrix[nbLvlPyr];
+ templateSizePyr = new unsigned int[nbLvlPyr];
+ HdesirePyr = new vpMatrix[nbLvlPyr];
+ HLMdesirePyr = new vpMatrix[nbLvlPyr];
+ HLMdesireInversePyr = new vpMatrix[nbLvlPyr];
- pyrInitialised=true;
+ pyrInitialised = true;
// vpTRACE("fin init_pyramidal");
-
}
-void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char>& I,vpTemplateTrackerZone &zone)
+void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char> &I, vpTemplateTrackerZone &zone)
{
// vpTRACE("initTrackingPyr");
zoneTrackedPyr[0].copy(zone);
- //vpTRACE("fin copy zone");
+ // vpTRACE("fin copy zone");
- pyr_IDes[0]=I;
- initTracking(pyr_IDes[0],zoneTrackedPyr[0]);
- ptTemplatePyr[0]=ptTemplate;
- ptTemplateSelectPyr[0]=ptTemplateSelect;
- templateSizePyr[0]=templateSize;
+ pyr_IDes[0] = I;
+ initTracking(pyr_IDes[0], zoneTrackedPyr[0]);
+ ptTemplatePyr[0] = ptTemplate;
+ ptTemplateSelectPyr[0] = ptTemplateSelect;
+ templateSizePyr[0] = templateSize;
- //creationpyramide de zones et images desiree
- if(nbLvlPyr>1)
- {
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- zoneTrackedPyr[i]=zoneTrackedPyr[i-1].getPyramidDown();
- vpImageFilter::getGaussPyramidal(pyr_IDes[i-1],pyr_IDes[i]);
-
- initTracking(pyr_IDes[i],zoneTrackedPyr[i]);
- ptTemplatePyr[i]=ptTemplate;
- ptTemplateSelectPyr[i]=ptTemplateSelect;
- templateSizePyr[i]=templateSize;
- //reste probleme avec le Hessien
+ // creationpyramide de zones et images desiree
+ if (nbLvlPyr > 1) {
+ for (unsigned int i = 1; i < nbLvlPyr; i++) {
+ zoneTrackedPyr[i] = zoneTrackedPyr[i - 1].getPyramidDown();
+ vpImageFilter::getGaussPyramidal(pyr_IDes[i - 1], pyr_IDes[i]);
+
+ initTracking(pyr_IDes[i], zoneTrackedPyr[i]);
+ ptTemplatePyr[i] = ptTemplate;
+ ptTemplateSelectPyr[i] = ptTemplateSelect;
+ templateSizePyr[i] = templateSize;
+ // reste probleme avec le Hessien
}
}
/*for(int i=0;i<nbLvlPyr;i++)
@@ -661,10 +641,9 @@ void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char>& I,vpTempla
zoneTracked=&zoneTrackedPyr[i];
init_pos_evalRMS(ptemp);
}*/
- zoneTracked=&zoneTrackedPyr[0];
+ zoneTracked = &zoneTrackedPyr[0];
// vpTRACE("fin initTrackingPyr");
-
}
/*!
@@ -672,17 +651,20 @@ void vpTemplateTracker::initTrackingPyr(const vpImage<unsigned char>& I,vpTempla
\param I: Image containing the reference template.
\param delaunay: Flag used to enable Delaunay triangulation.
- - If true, from the image points selected by the user, a Delaunay triangulation is performed
- to initialize the reference template.
+ - If true, from the image points selected by the user, a Delaunay
+ triangulation is performed to initialize the reference template.
- A left click select a image point;
- - A right click select the last image point and ends the initialisation stage.
+ - A right click select the last image point and ends the initialisation
+ stage.
- If false, the user select directly points as successive triangle corners.
The size of \e v_ip vector should be a multiple of 3. It is not mandatory
- that triangles have one edge in common; they can define a discontinued area.
+ that triangles have one edge in common; they can define a discontinued
+ area.
- A left click select a triangle corner;
- - A right click select the last triangle corner and ends the initialisation stage.
- For example, to select the reference template as two triangles, the user has to left click
- five times and finish the selection on the sixth corner with a right click.
+ - A right click select the last triangle corner and ends the
+ initialisation stage. For example, to select the reference template as two
+ triangles, the user has to left click five times and finish the selection on
+ the sixth corner with a right click.
*/
void vpTemplateTracker::initClick(const vpImage<unsigned char> &I, bool delaunay)
@@ -693,11 +675,10 @@ void vpTemplateTracker::initClick(const vpImage<unsigned char> &I, bool delaunay
initPyramidal(nbLvlPyr, l0Pyr);
initTrackingPyr(I, zoneRef_);
initHessienDesiredPyr(I);
- }
- else {
+ } else {
initTracking(I, zoneRef_);
initHessienDesired(I);
-// trackNoPyr(I);
+ // trackNoPyr(I);
}
}
@@ -707,11 +688,13 @@ void vpTemplateTracker::initClick(const vpImage<unsigned char> &I, bool delaunay
\param I: Image containing the reference template.
\param v_ip: Vector of image points defining the reference template.
\param delaunay:
- - If true, from the image points defining the reference template enable Delaunay triangulation.
- - If false, the vector of image points define the reference template as a list of triangles.
- The size of \e v_ip vector should be a multiple of 3.
+ - If true, from the image points defining the reference template enable
+ Delaunay triangulation.
+ - If false, the vector of image points define the reference template as a
+ list of triangles. The size of \e v_ip vector should be a multiple of 3.
*/
-void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector< vpImagePoint > &v_ip, bool delaunay)
+void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const std::vector<vpImagePoint> &v_ip,
+ bool delaunay)
{
zoneRef_.initFromPoints(I, v_ip, delaunay);
@@ -719,11 +702,10 @@ void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const st
initPyramidal(nbLvlPyr, l0Pyr);
initTrackingPyr(I, zoneRef_);
initHessienDesiredPyr(I);
- }
- else {
+ } else {
initTracking(I, zoneRef_);
initHessienDesired(I);
- //trackNoPyr(I);
+ // trackNoPyr(I);
}
}
@@ -733,7 +715,7 @@ void vpTemplateTracker::initFromPoints(const vpImage<unsigned char> &I, const st
\param I: Image containing the reference template.
\param zone: The zone that describes the reference template.
*/
-void vpTemplateTracker::initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone& zone)
+void vpTemplateTracker::initFromZone(const vpImage<unsigned char> &I, const vpTemplateTrackerZone &zone)
{
zoneRef_ = zone;
@@ -741,11 +723,10 @@ void vpTemplateTracker::initFromZone(const vpImage<unsigned char> &I, const vpTe
initPyramidal(nbLvlPyr, l0Pyr);
initTrackingPyr(I, zoneRef_);
initHessienDesiredPyr(I);
- }
- else {
+ } else {
initTracking(I, zoneRef_);
initHessienDesired(I);
-// trackNoPyr(I);
+ // trackNoPyr(I);
}
}
@@ -753,56 +734,53 @@ void vpTemplateTracker::initHessienDesiredPyr(const vpImage<unsigned char> &I)
{
// vpTRACE("initHessienDesiredPyr");
- templateSize=templateSizePyr[0];
- //ptTemplateSupp=ptTemplateSuppPyr[0];
- //ptTemplateCompo=ptTemplateCompoPyr[0];
- ptTemplate=ptTemplatePyr[0];
- ptTemplateSelect=ptTemplateSelectPyr[0];
-// ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
- try{
- initHessienDesired(I);
- ptTemplateSuppPyr[0]=ptTemplateSupp;
- ptTemplateCompoPyr[0]=ptTemplateCompo;
- HdesirePyr[0]=Hdesire;
- HLMdesirePyr[0]=HLMdesire;
- HLMdesireInversePyr[0]=HLMdesireInverse;
- }
- catch(vpException &e){
- ptTemplateSuppPyr[0]=ptTemplateSupp;
- ptTemplateCompoPyr[0]=ptTemplateCompo;
- HdesirePyr[0]=Hdesire;
- HLMdesirePyr[0]=HLMdesire;
- HLMdesireInversePyr[0]=HLMdesireInverse;
- throw(e);
+ templateSize = templateSizePyr[0];
+ // ptTemplateSupp=ptTemplateSuppPyr[0];
+ // ptTemplateCompo=ptTemplateCompoPyr[0];
+ ptTemplate = ptTemplatePyr[0];
+ ptTemplateSelect = ptTemplateSelectPyr[0];
+ // ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ try {
+ initHessienDesired(I);
+ ptTemplateSuppPyr[0] = ptTemplateSupp;
+ ptTemplateCompoPyr[0] = ptTemplateCompo;
+ HdesirePyr[0] = Hdesire;
+ HLMdesirePyr[0] = HLMdesire;
+ HLMdesireInversePyr[0] = HLMdesireInverse;
+ } catch (vpException &e) {
+ ptTemplateSuppPyr[0] = ptTemplateSupp;
+ ptTemplateCompoPyr[0] = ptTemplateCompo;
+ HdesirePyr[0] = Hdesire;
+ HLMdesirePyr[0] = HLMdesire;
+ HLMdesireInversePyr[0] = HLMdesireInverse;
+ throw(e);
}
- if(nbLvlPyr>1)
- {
- vpImage<unsigned char> Itemp;Itemp=I;
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- vpImageFilter::getGaussPyramidal(Itemp,Itemp);
-
- templateSize=templateSizePyr[i];
- ptTemplate=ptTemplatePyr[i];
- ptTemplateSelect=ptTemplateSelectPyr[i];
- //ptTemplateSupp=ptTemplateSuppPyr[i];
- //ptTemplateCompo=ptTemplateCompoPyr[i];
- try{
+ if (nbLvlPyr > 1) {
+ vpImage<unsigned char> Itemp;
+ Itemp = I;
+ for (unsigned int i = 1; i < nbLvlPyr; i++) {
+ vpImageFilter::getGaussPyramidal(Itemp, Itemp);
+
+ templateSize = templateSizePyr[i];
+ ptTemplate = ptTemplatePyr[i];
+ ptTemplateSelect = ptTemplateSelectPyr[i];
+ // ptTemplateSupp=ptTemplateSuppPyr[i];
+ // ptTemplateCompo=ptTemplateCompoPyr[i];
+ try {
initHessienDesired(Itemp);
- ptTemplateSuppPyr[i]=ptTemplateSupp;
- ptTemplateCompoPyr[i]=ptTemplateCompo;
- HdesirePyr[i]=Hdesire;
- HLMdesirePyr[i]=HLMdesire;
- HLMdesireInversePyr[i]=HLMdesireInverse;
- }
- catch(vpException &e){
- ptTemplateSuppPyr[i]=ptTemplateSupp;
- ptTemplateCompoPyr[i]=ptTemplateCompo;
- HdesirePyr[i]=Hdesire;
- HLMdesirePyr[i]=HLMdesire;
- HLMdesireInversePyr[i]=HLMdesireInverse;
- throw(e);
+ ptTemplateSuppPyr[i] = ptTemplateSupp;
+ ptTemplateCompoPyr[i] = ptTemplateCompo;
+ HdesirePyr[i] = Hdesire;
+ HLMdesirePyr[i] = HLMdesire;
+ HLMdesireInversePyr[i] = HLMdesireInverse;
+ } catch (vpException &e) {
+ ptTemplateSuppPyr[i] = ptTemplateSupp;
+ ptTemplateCompoPyr[i] = ptTemplateCompo;
+ HdesirePyr[i] = Hdesire;
+ HLMdesirePyr[i] = HLMdesire;
+ HLMdesireInversePyr[i] = HLMdesireInverse;
+ throw(e);
}
}
}
@@ -823,72 +801,67 @@ void vpTemplateTracker::track(const vpImage<unsigned char> &I)
void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
{
- //vpTRACE("trackPyr");
+ // vpTRACE("trackPyr");
vpImage<unsigned char> *pyr_I;
-// pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
- pyr_I=new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
- pyr_I[0]=I;
-
- try
- {
- vpColVector ptemp(nbParam);
- if(nbLvlPyr>1)
- {
- // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
- // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
-
- // p_sauv[0]=p;
- for(unsigned int i=1;i<nbLvlPyr;i++)
- {
- vpImageFilter::getGaussPyramidal(pyr_I[i-1],pyr_I[i]);
- //test getParamPyramidDown
- /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
- vpColVector vX_test2(2);
- Warp->computeCoeff(p);
- Warp->computeDenom(vX_test,p);
- Warp->warpX(vX_test,vX_test2,p);
- std::cout<<"p = "<<p.t()<<std::endl;*/
- //std::cout<<"get p down"<<std::endl;
- Warp->getParamPyramidDown(p,ptemp);
- p=ptemp;
- zoneTracked=&zoneTrackedPyr[i];
-
- // p_sauv[i]=p;
- /*std::cout<<"p_down = "<<p.t()<<std::endl;
-
- vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
- vpColVector vX_testd2(2);
- Warp->computeCoeff(p);
- Warp->computeDenom(vX_testd,p);
- Warp->warpX(vX_testd,vX_testd2,p);
- std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=> "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
+ // pyr_I=new vpImage<unsigned char>[nbLvlPyr+1]; // Why +1 ?
+ pyr_I = new vpImage<unsigned char>[nbLvlPyr]; // Why +1 ?
+ pyr_I[0] = I;
+
+ try {
+ vpColVector ptemp(nbParam);
+ if (nbLvlPyr > 1) {
+ // vpColVector *p_sauv=new vpColVector[nbLvlPyr];
+ // for(unsigned int i=0;i<nbLvlPyr;i++)p_sauv[i].resize(nbParam);
+
+ // p_sauv[0]=p;
+ for (unsigned int i = 1; i < nbLvlPyr; i++) {
+ vpImageFilter::getGaussPyramidal(pyr_I[i - 1], pyr_I[i]);
+ // test getParamPyramidDown
+ /*vpColVector vX_test(2);vX_test[0]=15.;vX_test[1]=30.;
+ vpColVector vX_test2(2);
+ Warp->computeCoeff(p);
+ Warp->computeDenom(vX_test,p);
+ Warp->warpX(vX_test,vX_test2,p);
+ std::cout<<"p = "<<p.t()<<std::endl;*/
+ // std::cout<<"get p down"<<std::endl;
+ Warp->getParamPyramidDown(p, ptemp);
+ p = ptemp;
+ zoneTracked = &zoneTrackedPyr[i];
+
+ // p_sauv[i]=p;
+ /*std::cout<<"p_down = "<<p.t()<<std::endl;
+
+ vpColVector vX_testd(2);vX_testd[0]=15./2.;vX_testd[1]=30./2.;
+ vpColVector vX_testd2(2);
+ Warp->computeCoeff(p);
+ Warp->computeDenom(vX_testd,p);
+ Warp->warpX(vX_testd,vX_testd2,p);
+ std::cout<<2.*vX_testd2[0]<<","<<2.*vX_testd2[1]<<" <=>
+ "<<vX_test2[0]<<","<<vX_test2[1]<<std::endl;*/
+ }
+ for (int i = (int)nbLvlPyr - 1; i >= 0; i--) {
+ if (i >= (int)l0Pyr) {
+ templateSize = templateSizePyr[i];
+ ptTemplate = ptTemplatePyr[i];
+ ptTemplateSelect = ptTemplateSelectPyr[i];
+ ptTemplateSupp = ptTemplateSuppPyr[i];
+ ptTemplateCompo = ptTemplateCompoPyr[i];
+ H = HdesirePyr[i];
+ HLM = HLMdesirePyr[i];
+ HLMdesireInverse = HLMdesireInversePyr[i];
+ // zoneTracked=&zoneTrackedPyr[i];
+ trackRobust(pyr_I[i]);
}
-
- for(int i=(int)nbLvlPyr-1;i>=0;i--)
- {
- if(i>=(int)l0Pyr)
- {
- templateSize=templateSizePyr[i];
- ptTemplate=ptTemplatePyr[i];
- ptTemplateSelect=ptTemplateSelectPyr[i];
- ptTemplateSupp=ptTemplateSuppPyr[i];
- ptTemplateCompo=ptTemplateCompoPyr[i];
- H=HdesirePyr[i];
- HLM=HLMdesirePyr[i];
- HLMdesireInverse=HLMdesireInversePyr[i];
- // zoneTracked=&zoneTrackedPyr[i];
- trackRobust(pyr_I[i]);
- }
- //std::cout<<"get p up"<<std::endl;
- // ptemp=p_sauv[i-1];
- if (i > 0) {
- Warp->getParamPyramidUp(p,ptemp);
- p=ptemp;
- zoneTracked=&zoneTrackedPyr[i-1];
- }
+ // std::cout<<"get p up"<<std::endl;
+ // ptemp=p_sauv[i-1];
+ if (i > 0) {
+ Warp->getParamPyramidUp(p, ptemp);
+ p = ptemp;
+ zoneTracked = &zoneTrackedPyr[i - 1];
}
- #if 0
+ }
+#if 0
if(l0Pyr==0)
{
templateSize=templateSizePyr[0];
@@ -910,38 +883,34 @@ void vpTemplateTracker::trackPyr(const vpImage<unsigned char> &I)
// }
zoneTracked=&zoneTrackedPyr[0];
}
- #endif
- // delete [] p_sauv;
- }
- else
- {
- //std::cout<<"reviens a tracker de base"<<std::endl;
- trackRobust(I);
- }
- delete[] pyr_I;
- }
- catch(vpException &e){
- delete[] pyr_I;
- throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
+#endif
+ // delete [] p_sauv;
+ } else {
+ // std::cout<<"reviens a tracker de base"<<std::endl;
+ trackRobust(I);
+ }
+ delete[] pyr_I;
+ } catch (vpException &e) {
+ delete[] pyr_I;
+ throw(vpTrackingException(vpTrackingException::badValue, e.getMessage()));
}
}
void vpTemplateTracker::trackRobust(const vpImage<unsigned char> &I)
{
- if(costFunctionVerification)
- {
- vpColVector p_pre_estimation;p_pre_estimation=p;
+ if (costFunctionVerification) {
+ vpColVector p_pre_estimation;
+ p_pre_estimation = p;
getGaussianBluredImage(I);
- double pre_fcost=getCost(I,p);
+ double pre_fcost = getCost(I, p);
trackNoPyr(I);
- //std::cout<<"fct avant : "<<pre_fcost<<std::endl;
- double post_fcost=getCost(I,p);
- //std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
- if(pre_fcost<post_fcost)
- p=p_pre_estimation;
- }
- else
+ // std::cout<<"fct avant : "<<pre_fcost<<std::endl;
+ double post_fcost = getCost(I, p);
+ // std::cout<<"fct apres : "<<post_fcost<<std::endl<<std::endl;
+ if (pre_fcost < post_fcost)
+ p = p_pre_estimation;
+ } else
trackNoPyr(I);
}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp
index 761727d..a83beaf 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarp.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,31 +39,33 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerWarp.h>
-void vpTemplateTrackerWarp::warpTriangle(const vpTemplateTrackerTriangle &in,const vpColVector &p, vpTemplateTrackerTriangle &out)
+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;
+ vpCTRACE << "Bad template tracker warp parameters dimension. Should "
+ "never occur. "
+ << std::endl;
throw(vpException(vpException::dimensionError, "Bad template tracker warp parameters dimension"));
}
- vpColVector S1(2),S2(2),S3(2);
- vpColVector rS1(2),rS2(2),rS3(2);
- in.getCorners(S1,S2,S3);
- computeDenom(S1,p);
- warpX(S1,rS1,p);
- computeDenom(S2,p);
- warpX(S2,rS2,p);
- computeDenom(S3,p);
- warpX(S3,rS3,p);
- out.init(rS1,rS2,rS3);
+ vpColVector S1(2), S2(2), S3(2);
+ vpColVector rS1(2), rS2(2), rS3(2);
+ in.getCorners(S1, S2, S3);
+ computeDenom(S1, p);
+ warpX(S1, rS1, p);
+ computeDenom(S2, p);
+ warpX(S2, rS2, p);
+ computeDenom(S3, p);
+ warpX(S3, rS3, p);
+ out.init(rS1, rS2, rS3);
}
-void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in,const vpColVector &p, vpTemplateTrackerZone &out)
+void vpTemplateTrackerWarp::warpZone(const vpTemplateTrackerZone &in, const vpColVector &p, vpTemplateTrackerZone &out)
{
- vpTemplateTrackerTriangle TR,TT;
+ vpTemplateTrackerTriangle TR, TT;
out.clear();
- for(unsigned int i=0;i<in.getNbTriangle();i++)
- {
- in.getTriangle(i,TR);
- warpTriangle(TR,p,TT);
+ for (unsigned int i = 0; i < in.getNbTriangle(); i++) {
+ in.getTriangle(i, TR);
+ warpTriangle(TR, p, TT);
out.add(TT);
}
}
@@ -71,94 +74,88 @@ double vpTemplateTrackerWarp::getDistanceBetweenZoneAndWarpedZone(const vpTempla
{
unsigned int nb_corners = Z.getNbTriangle() * 3;
computeCoeff(p);
- vpColVector X1(2),X2(2);
+ vpColVector X1(2), X2(2);
- double res=0;
+ double res = 0;
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<Z.getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < Z.getNbTriangle(); i++) {
Z.getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- computeDenom(X1,p);
- warpX(X1,X2,p);
- res+=sqrt((X2[0]-X1[0])*(X2[0]-X1[0])+(X2[1]-X1[1])*(X2[1]-X1[1]));
+ computeDenom(X1, p);
+ warpX(X1, X2, p);
+ res += sqrt((X2[0] - X1[0]) * (X2[0] - X1[0]) + (X2[1] - X1[1]) * (X2[1] - X1[1]));
}
}
- return res/nb_corners;
+ return res / nb_corners;
}
-void vpTemplateTrackerWarp::warp(const double *ut0,const double *vt0,int nb_pt,const vpColVector& p,double *u,double *v)
+void vpTemplateTrackerWarp::warp(const double *ut0, const double *vt0, int nb_pt, const vpColVector &p, double *u,
+ double *v)
{
computeCoeff(p);
- vpColVector X1(2),X2(2);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,X2,p);
- u[i]=X2[0];
- v[i]=X2[1];
- //std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
+ vpColVector X1(2), X2(2);
+ for (int i = 0; i < nb_pt; i++) {
+ X1[0] = ut0[i];
+ X1[1] = vt0[i];
+ computeDenom(X1, p);
+ warpX(X1, X2, p);
+ u[i] = X2[0];
+ v[i] = X2[1];
+ // std::cout<<"warp "<<X2[0]<<","<<X2[1]<<std::endl;
}
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void vpTemplateTrackerWarp::findWarp(const double *ut0,const double *vt0,const double *u,const double *v,int nb_pt,vpColVector& p)
+void vpTemplateTrackerWarp::findWarp(const double *ut0, const double *vt0, const double *u, const double *v, int nb_pt,
+ vpColVector &p)
{
- vpMatrix dW_(2,nbParam);
- vpMatrix dX(2,1);
- vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
- vpMatrix G(nbParam,1);
+ vpMatrix dW_(2, nbParam);
+ vpMatrix dX(2, 1);
+ vpMatrix H(nbParam, nbParam), HLM(nbParam, nbParam);
+ vpMatrix G(nbParam, 1);
- int cpt=0;
+ int cpt = 0;
vpColVector X1(2);
vpColVector fX1(2);
vpColVector X2(2);
- double erreur=0;
+ double erreur = 0;
double erreur_prec;
- double lambda=0.01;
- do
- {
- erreur_prec=erreur;
- H=0;
- G=0;
- erreur=0;
+ double lambda = 0.01;
+ do {
+ erreur_prec = erreur;
+ H = 0;
+ G = 0;
+ erreur = 0;
computeCoeff(p);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,fX1,p);
- dWarp(X1,fX1,p,dW_);
- H+=dW_.AtA();
+ for (int i = 0; i < nb_pt; i++) {
+ X1[0] = ut0[i];
+ X1[1] = vt0[i];
+ computeDenom(X1, p);
+ warpX(X1, fX1, p);
+ dWarp(X1, fX1, p, dW_);
+ H += dW_.AtA();
- X2[0]=u[i];
- X2[1]=v[i];
+ X2[0] = u[i];
+ X2[1] = v[i];
- dX=X2-fX1;
- G+=dW_.t()*dX;
-
- erreur+=((u[i]-fX1[0])*(u[i]-fX1[0])+(v[i]-fX1[1])*(v[i]-fX1[1]));
+ dX = X2 - fX1;
+ G += dW_.t() * dX;
+ erreur += ((u[i] - fX1[0]) * (u[i] - fX1[0]) + (v[i] - fX1[1]) * (v[i] - fX1[1]));
}
vpMatrix::computeHLM(H, lambda, HLM);
- try{
- p+=(vpColVector)(HLM.inverseByLU()*G, 0u);
- }
- catch(vpException &e) {
- //std::cout<<"Cannot inverse the matrix by LU " << std::endl;
+ try {
+ p += (vpColVector)(HLM.inverseByLU() * G, 0u);
+ } catch (vpException &e) {
+ // std::cout<<"Cannot inverse the matrix by LU " << std::endl;
throw(e);
}
cpt++;
- }
- while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
- //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
+ } while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
+ // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
}
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp
index 44b13a3..911f78e 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpAffine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,132 +39,151 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerWarpAffine.h>
-
vpTemplateTrackerWarpAffine::vpTemplateTrackerWarpAffine()
{
- nbParam = 6 ;
- dW.resize(2,nbParam);
+ nbParam = 6;
+ dW.resize(2, nbParam);
}
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpAffine::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpAffine::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- pdown=p;
- pdown[4]=p[4]/2.;
- pdown[5]=p[5]/2.;
+ pdown = p;
+ pdown[4] = p[4] / 2.;
+ pdown[5] = p[5] / 2.;
}
-void vpTemplateTrackerWarpAffine::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpAffine::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- pup=p;
- pup[4]=p[4]*2.;
- pup[5]=p[5]*2.;
+ pup = p;
+ pup[4] = p[4] * 2.;
+ pup[5] = p[5] * 2.;
}
/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpAffine::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+ */
+void vpTemplateTrackerWarpAffine::getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
{
- dIdW[0]=j*dx;
- dIdW[1]=j*dy;
- dIdW[2]=i*dx;
- dIdW[3]=i*dy;
- dIdW[4]=dx;
- dIdW[5]=dy;
+ dIdW[0] = j * dx;
+ dIdW[1] = j * dy;
+ dIdW[2] = i * dx;
+ dIdW[3] = i * dy;
+ dIdW[4] = dx;
+ dIdW[5] = dy;
}
/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpAffine::getdWdp0(const int &i,const int &j,double *dIdW)
+ */
+void vpTemplateTrackerWarpAffine::getdWdp0(const int &i, const int &j, double *dIdW)
{
- dIdW[0]=j;
- dIdW[1]=0;
- dIdW[2]=i;
- dIdW[3]=0;
- dIdW[4]=1.;
- dIdW[5]=0;
-
- dIdW[6]=0;
- dIdW[7]=j;
- dIdW[8]=0;
- dIdW[9]=i;
- dIdW[10]=0;
- dIdW[11]=1.;
+ dIdW[0] = j;
+ dIdW[1] = 0;
+ dIdW[2] = i;
+ dIdW[3] = 0;
+ dIdW[4] = 1.;
+ dIdW[5] = 0;
+
+ dIdW[6] = 0;
+ dIdW[7] = j;
+ dIdW[8] = 0;
+ dIdW[9] = i;
+ dIdW[10] = 0;
+ dIdW[11] = 1.;
}
-void vpTemplateTrackerWarpAffine::warpX(const int &i, const int &j,double &i2,double &j2, const vpColVector &ParamM)
+void vpTemplateTrackerWarpAffine::warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)
{
- j2=(1+ParamM[0])*j+ParamM[2]*i+ParamM[4];
- i2=ParamM[1]*j+(1+ParamM[3])*i+ParamM[5];
+ j2 = (1 + ParamM[0]) * j + ParamM[2] * i + ParamM[4];
+ i2 = ParamM[1] * j + (1 + ParamM[3]) * i + ParamM[5];
}
-
-void vpTemplateTrackerWarpAffine::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpAffine::warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=(1.0+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
- vXres[1]=ParamM[1]*vX[0]+(1.0+ParamM[3])*vX[1]+ParamM[5];
+ vXres[0] = (1.0 + ParamM[0]) * vX[0] + ParamM[2] * vX[1] + ParamM[4];
+ vXres[1] = ParamM[1] * vX[0] + (1.0 + ParamM[3]) * vX[1] + ParamM[5];
}
-void vpTemplateTrackerWarpAffine::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+void vpTemplateTrackerWarpAffine::dWarp(const vpColVector &X1, const vpColVector & /*X2*/,
+ const vpColVector & /*ParamM*/, vpMatrix &dW_)
{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=j;dW_[0][2]=i;dW_[0][4]=1;
- dW_[1][1]=j;dW_[1][3]=i;dW_[1][5]=1;
+ double j = X1[0];
+ double i = X1[1];
+ dW_ = 0;
+ dW_[0][0] = j;
+ dW_[0][2] = i;
+ dW_[0][4] = 1;
+ dW_[1][1] = j;
+ dW_[1][3] = i;
+ dW_[1][5] = 1;
}
/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpAffine::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
- const double *dwdp0, vpMatrix &dW_)
+ */
+void vpTemplateTrackerWarpAffine::dWarpCompo(const vpColVector & /*X1*/, const vpColVector & /*X2*/,
+ const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_)
{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=(1.+ParamM[0])*dwdp0[i]+ParamM[2]*dwdp0[i+nbParam];
- dW_[1][i]=ParamM[1]*dwdp0[i]+(1.+ParamM[3])*dwdp0[i+nbParam];
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] = (1. + ParamM[0]) * dwdp0[i] + ParamM[2] * dwdp0[i + nbParam];
+ dW_[1][i] = ParamM[1] * dwdp0[i] + (1. + ParamM[3]) * dwdp0[i + nbParam];
}
}
-void vpTemplateTrackerWarpAffine::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpAffine::warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=(1+ParamM[0])*vX[0]+ParamM[2]*vX[1]+ParamM[4];
- vXres[1]=ParamM[1]*vX[0]+(1+ParamM[3])*vX[1]+ParamM[5];
-}
-void vpTemplateTrackerWarpAffine::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+ vXres[0] = (1 + ParamM[0]) * vX[0] + ParamM[2] * vX[1] + ParamM[4];
+ vXres[1] = ParamM[1] * vX[0] + (1 + ParamM[3]) * vX[1] + ParamM[5];
+}
+void vpTemplateTrackerWarpAffine::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
vpColVector Trans(2);
- vpMatrix MWrap(2,2);
- Trans[0]=ParamM[4];Trans[1]=ParamM[5];
- MWrap[0][0]=1+ParamM[0];MWrap[0][1]=ParamM[2];
- MWrap[1][0]=ParamM[1];MWrap[1][1]=1+ParamM[3];
-
- vpMatrix MWrapInv(2,2);MWrapInv=MWrap.inverseByLU();
- vpColVector TransInv(2);TransInv=-1*MWrapInv*Trans;
-
- ParamMinv[0]=MWrapInv[0][0]-1;ParamMinv[2]=MWrapInv[0][1];
- ParamMinv[1]=MWrapInv[1][0];ParamMinv[3]=MWrapInv[1][1]-1;
- ParamMinv[4]=TransInv[0];ParamMinv[5]=TransInv[1];
-}
+ vpMatrix MWrap(2, 2);
+ Trans[0] = ParamM[4];
+ Trans[1] = ParamM[5];
+ MWrap[0][0] = 1 + ParamM[0];
+ MWrap[0][1] = ParamM[2];
+ MWrap[1][0] = ParamM[1];
+ MWrap[1][1] = 1 + ParamM[3];
+
+ vpMatrix MWrapInv(2, 2);
+ MWrapInv = MWrap.inverseByLU();
+ vpColVector TransInv(2);
+ TransInv = -1 * MWrapInv * Trans;
+
+ ParamMinv[0] = MWrapInv[0][0] - 1;
+ ParamMinv[2] = MWrapInv[0][1];
+ ParamMinv[1] = MWrapInv[1][0];
+ ParamMinv[3] = MWrapInv[1][1] - 1;
+ ParamMinv[4] = TransInv[0];
+ ParamMinv[5] = TransInv[1];
+}
-void vpTemplateTrackerWarpAffine::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+void vpTemplateTrackerWarpAffine::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
{
vpColVector Trans1(2);
- vpMatrix MWrap1(2,2);
- Trans1[0]=p1[4];Trans1[1]=p1[5];
- MWrap1[0][0]=1+p1[0];MWrap1[0][1]=p1[2];
- MWrap1[1][0]=p1[1];MWrap1[1][1]=1+p1[3];
+ vpMatrix MWrap1(2, 2);
+ Trans1[0] = p1[4];
+ Trans1[1] = p1[5];
+ MWrap1[0][0] = 1 + p1[0];
+ MWrap1[0][1] = p1[2];
+ MWrap1[1][0] = p1[1];
+ MWrap1[1][1] = 1 + p1[3];
vpColVector Trans2(2);
- vpMatrix MWrap2(2,2);
- Trans2[0]=p2[4];Trans2[1]=p2[5];
- MWrap2[0][0]=1+p2[0];MWrap2[0][1]=p2[2];
- MWrap2[1][0]=p2[1];MWrap2[1][1]=1+p2[3];
+ vpMatrix MWrap2(2, 2);
+ Trans2[0] = p2[4];
+ Trans2[1] = p2[5];
+ MWrap2[0][0] = 1 + p2[0];
+ MWrap2[0][1] = p2[2];
+ MWrap2[1][0] = p2[1];
+ MWrap2[1][1] = 1 + p2[3];
vpColVector TransRes(2);
- vpMatrix MWrapRes(2,2);
- TransRes=MWrap1*Trans2+Trans1;
- MWrapRes=MWrap1*MWrap2;
-
- pres[0]=MWrapRes[0][0]-1;pres[2]=MWrapRes[0][1];
- pres[1]=MWrapRes[1][0];pres[3]=MWrapRes[1][1]-1;
- pres[4]=TransRes[0];pres[5]=TransRes[1];
+ vpMatrix MWrapRes(2, 2);
+ TransRes = MWrap1 * Trans2 + Trans1;
+ MWrapRes = MWrap1 * MWrap2;
+
+ pres[0] = MWrapRes[0][0] - 1;
+ pres[2] = MWrapRes[0][1];
+ pres[1] = MWrapRes[1][0];
+ pres[3] = MWrapRes[1][1] - 1;
+ pres[4] = TransRes[0];
+ pres[5] = TransRes[1];
}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp
index 991955e..351cc4a 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomography.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,183 +37,179 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
#include <visp3/core/vpTrackingException.h>
+#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
vpTemplateTrackerWarpHomography::vpTemplateTrackerWarpHomography()
{
- nbParam = 8 ;
- dW.resize(2,nbParam);
+ nbParam = 8;
+ dW.resize(2, nbParam);
}
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpHomography::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpHomography::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- pdown=p;
- pdown[2]=p[2]*2.;
- pdown[5]=p[5]*2.;
- pdown[6]=p[6]/2.;
- pdown[7]=p[7]/2.;
+ pdown = p;
+ pdown[2] = p[2] * 2.;
+ pdown[5] = p[5] * 2.;
+ pdown[6] = p[6] / 2.;
+ pdown[7] = p[7] / 2.;
}
-void vpTemplateTrackerWarpHomography::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpHomography::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- pup=p;
- pup[2]=p[2]/2.;
- pup[5]=p[5]/2.;
- pup[6]=p[6]*2.;
- pup[7]=p[7]*2.;
+ pup = p;
+ pup[2] = p[2] / 2.;
+ pup[5] = p[5] / 2.;
+ pup[6] = p[6] * 2.;
+ pup[7] = p[7] * 2.;
}
/*calcul de di*dw(x,p0)/dp */
-void vpTemplateTrackerWarpHomography::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+void vpTemplateTrackerWarpHomography::getdW0(const int &i, const int &j, const double &dy, const double &dx,
+ double *dIdW)
{
- dIdW[0]=j*dx;
- dIdW[1]=j*dy;
- dIdW[2]=-j*j*dx-i*j*dy;
- dIdW[3]=i*dx;
- dIdW[4]=i*dy;
- dIdW[5]=-i*j*dx-i*i*dy;
- dIdW[6]=dx;
- dIdW[7]=dy;
+ dIdW[0] = j * dx;
+ dIdW[1] = j * dy;
+ dIdW[2] = -j * j * dx - i * j * dy;
+ dIdW[3] = i * dx;
+ dIdW[4] = i * dy;
+ dIdW[5] = -i * j * dx - i * i * dy;
+ dIdW[6] = dx;
+ dIdW[7] = dy;
}
/*calcul de dw(x,p0)/dp */
-void vpTemplateTrackerWarpHomography::getdWdp0(const int &i,const int &j,double *dIdW)
-{
- dIdW[0]=j;
- dIdW[1]=0;
- dIdW[2]=-j*j;
- dIdW[3]=i;
- dIdW[4]=0;
- dIdW[5]=-i*j;
- dIdW[6]=1.;
- dIdW[7]=0;
-
- dIdW[8]=0;
- dIdW[9]=j;
- dIdW[10]=-i*j;
- dIdW[11]=0;
- dIdW[12]=i;
- dIdW[13]=-i*i;
- dIdW[14]=0;
- dIdW[15]=1.;
+void vpTemplateTrackerWarpHomography::getdWdp0(const int &i, const int &j, double *dIdW)
+{
+ dIdW[0] = j;
+ dIdW[1] = 0;
+ dIdW[2] = -j * j;
+ dIdW[3] = i;
+ dIdW[4] = 0;
+ dIdW[5] = -i * j;
+ dIdW[6] = 1.;
+ dIdW[7] = 0;
+
+ dIdW[8] = 0;
+ dIdW[9] = j;
+ dIdW[10] = -i * j;
+ dIdW[11] = 0;
+ dIdW[12] = i;
+ dIdW[13] = -i * i;
+ dIdW[14] = 0;
+ dIdW[15] = 1.;
}
void vpTemplateTrackerWarpHomography::computeDenom(vpColVector &vX, const vpColVector &ParamM)
{
- denom=(1./(ParamM[2]*vX[0]+ParamM[5]*vX[1]+1.));
+ denom = (1. / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1.));
}
-void vpTemplateTrackerWarpHomography::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+void vpTemplateTrackerWarpHomography::warpX(const int &i, const int &j, double &i2, double &j2,
+ const vpColVector &ParamM)
{
- j2=((1.+ParamM[0])*j+ParamM[3]*i+ParamM[6])*denom;
- i2=(ParamM[1]*j+(1.+ParamM[4])*i+ParamM[7])*denom;
+ j2 = ((1. + ParamM[0]) * j + ParamM[3] * i + ParamM[6]) * denom;
+ i2 = (ParamM[1] * j + (1. + ParamM[4]) * i + ParamM[7]) * denom;
}
-
-void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpHomography::warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- //if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
- if((denom)>0)// FS optimisation
+ // if((ParamM[2]*vX[0]+ParamM[5]*vX[1]+1)>0)//si dans le plan image reel
+ if ((denom) > 0) // FS optimisation
{
- vXres[0]=((1+ParamM[0])*vX[0]+ParamM[3]*vX[1]+ParamM[6])*denom;
- vXres[1]=(ParamM[1]*vX[0]+(1+ParamM[4])*vX[1]+ParamM[7])*denom;
- }
- else
- throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
+ vXres[0] = ((1 + ParamM[0]) * vX[0] + ParamM[3] * vX[1] + ParamM[6]) * denom;
+ vXres[1] = (ParamM[1] * vX[0] + (1 + ParamM[4]) * vX[1] + ParamM[7]) * denom;
+ } else
+ throw(vpTrackingException(vpTrackingException::fatalError,
+ "Division by zero in vpTemplateTrackerWarpHomography::warpX()"));
}
-void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+void vpTemplateTrackerWarpHomography::dWarp(const vpColVector &X1, const vpColVector &X2,
+ const vpColVector & /*ParamM*/, vpMatrix &dW_)
{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=j*denom;
- dW_[0][2]=-j*X2[0]*denom;
- dW_[0][3]=i*denom;
- dW_[0][5]=-i*X2[0]*denom;
- dW_[0][6]=denom;
-
- dW_[1][1]=j*denom;
- dW_[1][2]=-j*X2[1]*denom;
- dW_[1][4]=i*denom;
- dW_[1][5]=-i*X2[1]*denom;
- dW_[1][7]=denom;
+ double j = X1[0];
+ double i = X1[1];
+ dW_ = 0;
+ dW_[0][0] = j * denom;
+ dW_[0][2] = -j * X2[0] * denom;
+ dW_[0][3] = i * denom;
+ dW_[0][5] = -i * X2[0] * denom;
+ dW_[0][6] = denom;
+
+ dW_[1][1] = j * denom;
+ dW_[1][2] = -j * X2[1] * denom;
+ dW_[1][4] = i * denom;
+ dW_[1][5] = -i * X2[1] * denom;
+ dW_[1][7] = denom;
}
/*compute dw=dw/dx*dw/dp */
-void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &ParamM,
- const double *dwdp0,vpMatrix &dW_)
+void vpTemplateTrackerWarpHomography::dWarpCompo(const vpColVector & /*X1*/, const vpColVector &X2,
+ const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_)
{
- double dwdx0,dwdx1;
- double dwdy0,dwdy1;
-
- dwdx0=((1.+ParamM[0])-X2[0]*ParamM[2])*denom;
- dwdx1=(ParamM[1]-X2[1]*ParamM[2])*denom;
- dwdy0=(ParamM[3]-X2[0]*ParamM[5])*denom;
- dwdy1=((1.+ParamM[4])-X2[1]*ParamM[5])*denom;
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=dwdx0*dwdp0[i]+dwdy0*dwdp0[i+nbParam];
- dW_[1][i]=dwdx1*dwdp0[i]+dwdy1*dwdp0[i+nbParam];
+ double dwdx0, dwdx1;
+ double dwdy0, dwdy1;
+
+ dwdx0 = ((1. + ParamM[0]) - X2[0] * ParamM[2]) * denom;
+ dwdx1 = (ParamM[1] - X2[1] * ParamM[2]) * denom;
+ dwdy0 = (ParamM[3] - X2[0] * ParamM[5]) * denom;
+ dwdy1 = ((1. + ParamM[4]) - X2[1] * ParamM[5]) * denom;
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] = dwdx0 * dwdp0[i] + dwdy0 * dwdp0[i + nbParam];
+ dW_[1][i] = dwdx1 * dwdp0[i] + dwdy1 * dwdp0[i + nbParam];
}
}
-void vpTemplateTrackerWarpHomography::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+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);
- }
- else
- throw(vpTrackingException(vpTrackingException::fatalError,"Division by zero in vpTemplateTrackerWarpHomography::warpXSpecialInv()")) ;
-}
-void vpTemplateTrackerWarpHomography::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+ vXres[0] = ((1 + ParamM[0]) * vX[0] + ParamM[3] * vX[1] + ParamM[6]) / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1);
+ vXres[1] = (ParamM[1] * vX[0] + (1 + ParamM[4]) * vX[1] + ParamM[7]) / (ParamM[2] * vX[0] + ParamM[5] * vX[1] + 1);
+ } else
+ throw(vpTrackingException(vpTrackingException::fatalError, "Division by zero in "
+ "vpTemplateTrackerWarpHomography::"
+ "warpXSpecialInv()"));
+}
+void vpTemplateTrackerWarpHomography::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
- vpHomography H = getHomography(ParamM);
+ vpHomography H = getHomography(ParamM);
vpHomography Hinv = H.inverse();
getParam(Hinv, ParamMinv);
-}
-
+}
vpHomography vpTemplateTrackerWarpHomography::getHomography(const vpColVector &ParamM) const
{
vpHomography H;
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- {
- if(i+3*j!=8)
- {
- H[i][j]=ParamM[i+3*j];
- if(i==j)H[i][j]++;
- }
- else
- H[i][j]=1.;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++) {
+ if (i + 3 * j != 8) {
+ H[i][j] = ParamM[i + 3 * j];
+ if (i == j)
+ H[i][j]++;
+ } else
+ H[i][j] = 1.;
}
return H;
}
-void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H,vpColVector &par) const
+void vpTemplateTrackerWarpHomography::getParam(const vpHomography &H, vpColVector &par) const
{
- par=0;
- for(unsigned int i=0;i<3;i++)
- for(unsigned int j=0;j<3;j++)
- {
- if(i+3*j!=8)
- {
- par[i+3*j]=H[i][j]/H[2][2];
- if(i==j)par[i+3*j]--;
+ par = 0;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++) {
+ if (i + 3 * j != 8) {
+ par[i + 3 * j] = H[i][j] / H[2][2];
+ if (i == j)
+ par[i + 3 * j]--;
}
}
}
-
-
-void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+void vpTemplateTrackerWarpHomography::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
{
vpHomography H1 = getHomography(p1);
vpHomography H2 = getHomography(p2);
- vpHomography H = H1*H2;
- getParam(H,pres);
+ vpHomography H = H1 * H2;
+ getParam(H, pres);
}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp
index 2bd1138..99ceea1 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpHomographySL3.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,177 +39,187 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerWarpHomographySL3.h>
-//findWarp special a SL3 car methode additionnelle ne marche pas (la derivee n est calculable qu en p=0)
+// 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"<<std::endl;
+ // std::cout<<"findWarp OVERLOADE"<<std::endl;
vpColVector dp(nbParam);
- vpMatrix dW_(2,nbParam);
- vpMatrix dX(2,1);
- vpMatrix H(nbParam,nbParam), HLM(nbParam,nbParam);
- vpMatrix G_(nbParam,1);
-
- //vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
- double **dW_ddp0=new double*[(unsigned int)nb_pt];
- for(int i=0;i<nb_pt;i++)
- {
- //dW_ddp0[i].resize(2,nbParam);
- dW_ddp0[i]=new double[2*nbParam];
- //getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
- //std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
- getdWdp0(v[i],u[i],dW_ddp0[i]);
+ vpMatrix dW_(2, nbParam);
+ vpMatrix dX(2, 1);
+ vpMatrix H(nbParam, nbParam), HLM(nbParam, nbParam);
+ vpMatrix G_(nbParam, 1);
+
+ // vpMatrix *dW_ddp0=new vpMatrix[nb_pt];
+ double **dW_ddp0 = new double *[(unsigned int)nb_pt];
+ for (int i = 0; i < nb_pt; i++) {
+ // dW_ddp0[i].resize(2,nbParam);
+ dW_ddp0[i] = new double[2 * nbParam];
+ // getdWdp0(vt0[i],ut0[i],dW_ddp0[i]);
+ // std::cout<<"findWarp"<<v[i]<<","<<u[i]<<std::endl;
+ getdWdp0(v[i], u[i], dW_ddp0[i]);
}
-
- int cpt=0;
+
+ int cpt = 0;
vpColVector X1(2);
vpColVector fX1(2);
vpColVector X2(2);
- double erreur=0;
+ double erreur = 0;
double erreur_prec;
- double lambda=0.00001;
- do
- {
- erreur_prec=erreur;
- H=0;
- G_=0;
- erreur=0;
+ double lambda = 0.00001;
+ do {
+ erreur_prec = erreur;
+ H = 0;
+ G_ = 0;
+ erreur = 0;
computeCoeff(p);
- for(int i=0;i<nb_pt;i++)
- {
- X1[0]=ut0[i];
- X1[1]=vt0[i];
- computeDenom(X1,p);
- warpX(X1,fX1,p);
- //dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
- //dWarp(X1,fX1,p,dW);
- for(unsigned int ip=0;ip<nbParam;ip++)
- {
- dW_[0][ip]=dW_ddp0[i][ip];
- dW_[1][ip]=dW_ddp0[i][ip+nbParam];
+ for (int i = 0; i < nb_pt; i++) {
+ X1[0] = ut0[i];
+ X1[1] = vt0[i];
+ computeDenom(X1, p);
+ warpX(X1, fX1, p);
+ // dWarpCompo(X1,fX1,p,dW_ddp0[i],dW);
+ // dWarp(X1,fX1,p,dW);
+ for (unsigned int ip = 0; ip < nbParam; ip++) {
+ dW_[0][ip] = dW_ddp0[i][ip];
+ dW_[1][ip] = dW_ddp0[i][ip + nbParam];
}
- H+=dW_.AtA();
-
- X2[0]=u[i];
- X2[1]=v[i];
+ H += dW_.AtA();
+
+ X2[0] = u[i];
+ X2[1] = v[i];
- dX=X2-fX1;
- G_+=dW_.t()*dX;
+ 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);
- try{
- dp=HLM.inverseByLU()*G_;
- }
- catch(vpException &e){
- //std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
+ try {
+ dp = HLM.inverseByLU() * G_;
+ } catch (vpException &e) {
+ // std::cout<<"Cannot inverse the matrix by LU "<<std::endl;
throw(e);
}
- pRondp(p,dp,p);
+ pRondp(p, dp, p);
cpt++;
// std::cout<<"erreur ="<<erreur<<std::endl;
}
- //while((cpt<1500));
- while((cpt<150)&&(sqrt((erreur_prec-erreur)*(erreur_prec-erreur))>1e-20));
-
- //std::cout<<"erreur apres transformation="<<erreur<<std::endl;
- for(int i=0;i<nb_pt;i++)
+ // while((cpt<1500));
+ while ((cpt < 150) && (sqrt((erreur_prec - erreur) * (erreur_prec - erreur)) > 1e-20));
+
+ // std::cout<<"erreur apres transformation="<<erreur<<std::endl;
+ for (int i = 0; i < nb_pt; i++)
delete[] dW_ddp0[i];
delete[] dW_ddp0;
-
}
-vpTemplateTrackerWarpHomographySL3::vpTemplateTrackerWarpHomographySL3()
- : G(), dGx(), A()
+vpTemplateTrackerWarpHomographySL3::vpTemplateTrackerWarpHomographySL3() : G(), dGx(), A()
{
- nbParam = 8 ;
- G.resize(3,3);
- dGx.resize(3,nbParam);
+ nbParam = 8;
+ G.resize(3, 3);
+ dGx.resize(3, nbParam);
A.resize(8);
- for(unsigned int i=0;i<8;i++)
- {
- A[i].resize(3,3);
- A[i]=0;
+ for (unsigned int i = 0; i < 8; i++) {
+ A[i].resize(3, 3);
+ A[i] = 0;
}
- A[0][0][2]=1;
- A[1][1][2]=1;
- A[2][0][1]=1;
- A[3][1][0]=1;
- A[4][0][0]=1;
- A[4][1][1]=-1;
- A[5][1][1]=-1;
- A[5][2][2]=1;
- A[6][2][0]=1;
- A[7][2][1]=1;
+ A[0][0][2] = 1;
+ A[1][1][2] = 1;
+ A[2][0][1] = 1;
+ A[3][1][0] = 1;
+ A[4][0][0] = 1;
+ A[4][1][1] = -1;
+ A[5][1][1] = -1;
+ A[5][2][2] = 1;
+ A[6][2][0] = 1;
+ A[7][2][1] = 1;
}
-vpTemplateTrackerWarpHomographySL3::~vpTemplateTrackerWarpHomographySL3()
-{
-}
+vpTemplateTrackerWarpHomographySL3::~vpTemplateTrackerWarpHomographySL3() {}
-//get the parameter corresponding to the lower level of a gaussian pyramid
-//a refaire de facon analytique
-void vpTemplateTrackerWarpHomographySL3::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+// a refaire de facon analytique
+void vpTemplateTrackerWarpHomographySL3::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- double *u,*v;u=new double[4];v=new double[4];
- //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
- u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
- double *u2,*v2;u2=new double[4];v2=new double[4];
- warp(u,v,4,p,u2,v2);
- //p=0;findWarp(u,v,u2,v2,4,p);
- for(int i=0;i<4;i++)
- {
- u[i]=u[i]/2.;
- v[i]=v[i]/2.;
- u2[i]=u2[i]/2.;
- v2[i]=v2[i]/2.;
- //std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
+ double *u, *v;
+ u = new double[4];
+ v = new double[4];
+ // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
+ u[0] = 0;
+ v[0] = 0;
+ u[1] = 160;
+ v[1] = 0;
+ u[2] = 160;
+ v[2] = 120;
+ u[3] = 0;
+ v[3] = 120;
+ double *u2, *v2;
+ u2 = new double[4];
+ v2 = new double[4];
+ warp(u, v, 4, p, u2, v2);
+ // p=0;findWarp(u,v,u2,v2,4,p);
+ for (int i = 0; i < 4; i++) {
+ u[i] = u[i] / 2.;
+ v[i] = v[i] / 2.;
+ u2[i] = u2[i] / 2.;
+ v2[i] = v2[i] / 2.;
+ // std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;
}
- pdown=p;
- findWarp(u,v,u2,v2,4,pdown);
+ pdown = p;
+ findWarp(u, v, u2, v2, 4, pdown);
delete[] u;
delete[] v;
delete[] u2;
delete[] v2;
}
-void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- double *u,*v;u=new double[4];v=new double[4];
- //u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
- u[0]=0;v[0]=0;u[1]=160;v[1]=0;u[2]=160;v[2]=120;u[3]=0;v[3]=120;
- //u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
- double *u2,*v2;u2=new double[4];v2=new double[4];
-
- //pup=p;
+ double *u, *v;
+ u = new double[4];
+ v = new double[4];
+ // u[0]=0;v[0]=0;u[1]=640;v[1]=0;u[2]=640;v[2]=480;u[3]=0;v[3]=480;
+ u[0] = 0;
+ v[0] = 0;
+ u[1] = 160;
+ v[1] = 0;
+ u[2] = 160;
+ v[2] = 120;
+ u[3] = 0;
+ v[3] = 120;
+ // u[0]=40;v[0]=30;u[1]=160;v[1]=30;u[2]=160;v[2]=120;u[3]=40;v[3]=120;
+ double *u2, *v2;
+ u2 = new double[4];
+ v2 = new double[4];
+
+ // pup=p;
/*vpColVector ptest=pup;
warp(u,v,4,ptest,u2,v2);
for(int i=0;i<4;i++)
std::cout<<"test "<<u2[i]<<","<<v2[i]<<std::endl;*/
- warp(u,v,4,p,u2,v2);
- //p=0;findWarp(u,v,u2,v2,4,p);
-
+ warp(u, v, 4, p, u2, v2);
+ // p=0;findWarp(u,v,u2,v2,4,p);
- for(int i=0;i<4;i++)
- {
- u[i]=u[i]*2.;
- v[i]=v[i]*2.;
- u2[i]=u2[i]*2.;
- v2[i]=v2[i]*2.;
+ for (int i = 0; i < 4; i++) {
+ u[i] = u[i] * 2.;
+ v[i] = v[i] * 2.;
+ u2[i] = u2[i] * 2.;
+ v2[i] = v2[i] * 2.;
/*std::cout<<"#########################################################################################"<<std::endl;
std::cout<<"#########################################################################################"<<std::endl;
std::cout<<"#########################################################################################"<<std::endl;
std::cout<<"recherche "<<u2[i]<<","<<v2[i]<<std::endl;*/
}
- findWarp(u,v,u2,v2,4,pup);
+ findWarp(u, v, u2, v2, 4, pup);
delete[] u;
delete[] v;
@@ -216,184 +227,185 @@ void vpTemplateTrackerWarpHomographySL3::getParamPyramidUp(const vpColVector &p,
delete[] v2;
}
-void vpTemplateTrackerWarpHomographySL3::computeDenom(vpColVector &vX, const vpColVector &/*ParamM*/)
+void vpTemplateTrackerWarpHomographySL3::computeDenom(vpColVector &vX, const vpColVector & /*ParamM*/)
{
- denom=vX[0]*G[2][0]+vX[1]*G[2][1]+G[2][2];
+ denom = vX[0] * G[2][0] + vX[1] * G[2][1] + G[2][2];
}
void vpTemplateTrackerWarpHomographySL3::computeCoeff(const vpColVector &p)
{
- vpMatrix pA(3,3);
- pA[0][0]=p[4];
- pA[0][1]=p[2];
- pA[0][2]=p[0];
+ vpMatrix pA(3, 3);
+ pA[0][0] = p[4];
+ pA[0][1] = p[2];
+ pA[0][2] = p[0];
- pA[1][0]=p[3];
- pA[1][1]=-p[4]-p[5];
- pA[1][2]=p[1];
+ pA[1][0] = p[3];
+ pA[1][1] = -p[4] - p[5];
+ pA[1][2] = p[1];
- pA[2][0]=p[6];
- pA[2][1]=p[7];
- pA[2][2]=p[5];
+ pA[2][0] = p[6];
+ pA[2][1] = p[7];
+ pA[2][2] = p[5];
- G=pA.expm();
+ G = pA.expm();
}
-
-void vpTemplateTrackerWarpHomographySL3::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &/*ParamM*/)
+void vpTemplateTrackerWarpHomographySL3::warpX(const vpColVector &vX, vpColVector &vXres,
+ const vpColVector & /*ParamM*/)
{
- double i=vX[1],j=vX[0];
- vXres[0]=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
- vXres[1]=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
+ double i = vX[1], j = vX[0];
+ vXres[0] = (j * G[0][0] + i * G[0][1] + G[0][2]) / denom;
+ vXres[1] = (j * G[1][0] + i * G[1][1] + G[1][2]) / denom;
}
-void vpTemplateTrackerWarpHomographySL3::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &/*ParamM*/)
+void vpTemplateTrackerWarpHomographySL3::warpX(const int &i, const int &j, double &i2, double &j2,
+ const vpColVector & /*ParamM*/)
{
- j2=(j*G[0][0]+i*G[0][1]+G[0][2])/denom;
- i2=(j*G[1][0]+i*G[1][1]+G[1][2])/denom;
+ j2 = (j * G[0][0] + i * G[0][1] + G[0][2]) / denom;
+ i2 = (j * G[1][0] + i * G[1][1] + G[1][2]) / denom;
}
vpHomography vpTemplateTrackerWarpHomographySL3::getHomography() const
{
vpHomography H;
- for (unsigned int i=0; i<3; i++)
- for (unsigned int j=0; j<3; j++)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
H[i][j] = G[i][j];
return H;
}
-void vpTemplateTrackerWarpHomographySL3::dWarp(const vpColVector &X1,const vpColVector &X2,const vpColVector &/*ParamM*/,vpMatrix &dW_)
+void vpTemplateTrackerWarpHomographySL3::dWarp(const vpColVector &X1, const vpColVector &X2,
+ const vpColVector & /*ParamM*/, vpMatrix &dW_)
{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1./denom;dhdx[1][1]=1./denom;dhdx[0][2]=-X2[0]/(denom);dhdx[1][2]=-X2[1]/(denom);
- dGx=0;
- for(unsigned int i=0;i<3;i++)
- {
- dGx[i][0]=G[i][0];
- dGx[i][1]=G[i][1];
- dGx[i][2]=G[i][0]*X1[1];
- dGx[i][3]=G[i][1]*X1[0];
- dGx[i][4]=G[i][0]*X1[0]-G[i][1]*X1[1];
- dGx[i][5]=G[i][2]-G[i][1]*X1[1];
- dGx[i][6]=G[i][2]*X1[0];
- dGx[i][7]=G[i][2]*X1[1];
+ vpMatrix dhdx(2, 3);
+ dhdx = 0;
+ dhdx[0][0] = 1. / denom;
+ dhdx[1][1] = 1. / denom;
+ dhdx[0][2] = -X2[0] / (denom);
+ dhdx[1][2] = -X2[1] / (denom);
+ dGx = 0;
+ for (unsigned int i = 0; i < 3; i++) {
+ dGx[i][0] = G[i][0];
+ dGx[i][1] = G[i][1];
+ dGx[i][2] = G[i][0] * X1[1];
+ dGx[i][3] = G[i][1] * X1[0];
+ dGx[i][4] = G[i][0] * X1[0] - G[i][1] * X1[1];
+ dGx[i][5] = G[i][2] - G[i][1] * X1[1];
+ dGx[i][6] = G[i][2] * X1[0];
+ dGx[i][7] = G[i][2] * X1[1];
}
- dW_=dhdx*dGx;
-
+ dW_ = dhdx * dGx;
}
/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+ */
+void vpTemplateTrackerWarpHomographySL3::getdW0(const int &i, const int &j, const double &dy, const double &dx,
+ double *dIdW)
{
- vpMatrix dhdx(1,3);
- dhdx=0;
- dhdx[0][0]=dx;dhdx[0][1]=dy;dhdx[0][2]=-j*dx-i*dy;
+ vpMatrix dhdx(1, 3);
+ dhdx = 0;
+ dhdx[0][0] = dx;
+ dhdx[0][1] = dy;
+ dhdx[0][2] = -j * dx - i * dy;
G.eye();
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
+ dGx = 0;
+ for (unsigned int par = 0; par < 3; par++) {
+ dGx[par][0] = G[par][0];
+ dGx[par][1] = G[par][1];
+ dGx[par][2] = G[par][0] * i;
+ dGx[par][3] = G[par][1] * j;
+ dGx[par][4] = G[par][0] * j - G[par][1] * i;
+ dGx[par][5] = G[par][2] - G[par][1] * i;
+ dGx[par][6] = G[par][2] * j;
+ dGx[par][7] = G[par][2] * i;
}
- for(unsigned int par=0;par<nbParam;par++)
- {
- double res=0;
- for(unsigned int par2=0;par2<3;par2++)
- res+=dhdx[0][par2]*dGx[par2][par];
- dIdW[par]=res;
+ for (unsigned int par = 0; par < nbParam; par++) {
+ double res = 0;
+ for (unsigned int par2 = 0; par2 < 3; par2++)
+ res += dhdx[0][par2] * dGx[par2][par];
+ dIdW[par] = res;
}
-
}
/*calcul de dw(x,p0)/dp
-*/
+ */
-void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i,const int &j,double *dIdW)
+void vpTemplateTrackerWarpHomographySL3::getdWdp0(const int &i, const int &j, double *dIdW)
{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
+ vpMatrix dhdx(2, 3);
+ dhdx = 0;
+ dhdx[0][0] = 1.;
+ dhdx[1][1] = 1.;
+ dhdx[0][2] = -j;
+ dhdx[1][2] = -i;
G.eye();
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
+ dGx = 0;
+ for (unsigned int par = 0; par < 3; par++) {
+ dGx[par][0] = G[par][0];
+ dGx[par][1] = G[par][1];
+ dGx[par][2] = G[par][0] * i;
+ dGx[par][3] = G[par][1] * j;
+ dGx[par][4] = G[par][0] * j - G[par][1] * i;
+ dGx[par][5] = G[par][2] - G[par][1] * i;
+ dGx[par][6] = G[par][2] * j;
+ dGx[par][7] = G[par][2] * i;
}
- vpMatrix dIdW_temp(2,nbParam);
- dIdW_temp=dhdx*dGx;
+ vpMatrix dIdW_temp(2, nbParam);
+ dIdW_temp = dhdx * dGx;
- for(unsigned int par=0;par<nbParam;par++)
- {
- dIdW[par]=dIdW_temp[0][par];
- dIdW[par+nbParam]=dIdW_temp[1][par];
+ for (unsigned int par = 0; par < nbParam; par++) {
+ dIdW[par] = dIdW_temp[0][par];
+ dIdW[par + nbParam] = dIdW_temp[1][par];
}
-
}
-void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i,const double &j,double *dIdW)
+void vpTemplateTrackerWarpHomographySL3::getdWdp0(const double &i, const double &j, double *dIdW)
{
- vpMatrix dhdx(2,3);
- dhdx=0;
- dhdx[0][0]=1.;dhdx[1][1]=1.;dhdx[0][2]=-j;dhdx[1][2]=-i;
+ vpMatrix dhdx(2, 3);
+ dhdx = 0;
+ dhdx[0][0] = 1.;
+ dhdx[1][1] = 1.;
+ dhdx[0][2] = -j;
+ dhdx[1][2] = -i;
G.eye();
- dGx=0;
- for(unsigned int par=0;par<3;par++)
- {
- dGx[par][0]=G[par][0];
- dGx[par][1]=G[par][1];
- dGx[par][2]=G[par][0]*i;
- dGx[par][3]=G[par][1]*j;
- dGx[par][4]=G[par][0]*j-G[par][1]*i;
- dGx[par][5]=G[par][2]-G[par][1]*i;
- dGx[par][6]=G[par][2]*j;
- dGx[par][7]=G[par][2]*i;
+ dGx = 0;
+ for (unsigned int par = 0; par < 3; par++) {
+ dGx[par][0] = G[par][0];
+ dGx[par][1] = G[par][1];
+ dGx[par][2] = G[par][0] * i;
+ dGx[par][3] = G[par][1] * j;
+ dGx[par][4] = G[par][0] * j - G[par][1] * i;
+ dGx[par][5] = G[par][2] - G[par][1] * i;
+ dGx[par][6] = G[par][2] * j;
+ dGx[par][7] = G[par][2] * i;
}
- vpMatrix dIdW_temp(2,nbParam);
- dIdW_temp=dhdx*dGx;
+ vpMatrix dIdW_temp(2, nbParam);
+ dIdW_temp = dhdx * dGx;
- for(unsigned int par=0;par<nbParam;par++)
- {
- dIdW[par]=dIdW_temp[0][par];
- dIdW[par+nbParam]=dIdW_temp[1][par];
+ for (unsigned int par = 0; par < nbParam; par++) {
+ dIdW[par] = dIdW_temp[0][par];
+ dIdW[par + nbParam] = dIdW_temp[1][par];
}
-
}
/*compute dw=dw/dx*dw/dp
-*/
+ */
-void vpTemplateTrackerWarpHomographySL3::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &X2,const vpColVector &/*ParamM*/,
- const double *dwdp0,vpMatrix &dW_)
+void vpTemplateTrackerWarpHomographySL3::dWarpCompo(const vpColVector & /*X1*/, const vpColVector &X2,
+ const vpColVector & /*ParamM*/, const double *dwdp0, vpMatrix &dW_)
{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=denom*((G[0][0]-X2[0]*G[2][0])*dwdp0[i]+(G[0][1]-X2[0]*G[2][1])*dwdp0[i+nbParam]);
- dW_[1][i]=denom*((G[1][0]-X2[1]*G[2][0])*dwdp0[i]+(G[1][1]-X2[1]*G[2][1])*dwdp0[i+nbParam]);
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] = denom * ((G[0][0] - X2[0] * G[2][0]) * dwdp0[i] + (G[0][1] - X2[0] * G[2][1]) * dwdp0[i + nbParam]);
+ dW_[1][i] = denom * ((G[1][0] - X2[1] * G[2][0]) * dwdp0[i] + (G[1][1] - X2[1] * G[2][1]) * dwdp0[i + nbParam]);
}
}
-void vpTemplateTrackerWarpHomographySL3::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
-{
- ParamMinv=-ParamM;
-}
-void vpTemplateTrackerWarpHomographySL3::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+void vpTemplateTrackerWarpHomographySL3::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
- //vrai que si commutatif ...
- pres=p1+p2;
+ ParamMinv = -ParamM;
+}
+void vpTemplateTrackerWarpHomographySL3::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
+{
+ // vrai que si commutatif ...
+ pres = p1 + p2;
}
-
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp
index 528be0b..151b71f 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpRT.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,140 +39,140 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerWarpRT.h>
-
vpTemplateTrackerWarpRT::vpTemplateTrackerWarpRT()
{
nbParam = 3;
- dW.resize(2,nbParam);
+ dW.resize(2, nbParam);
}
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpRT::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpRT::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- pdown=p;
- pdown[1]=p[1]/2.;
- pdown[2]=p[2]/2.;
+ pdown = p;
+ pdown[1] = p[1] / 2.;
+ pdown[2] = p[2] / 2.;
}
-void vpTemplateTrackerWarpRT::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpRT::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- pup=p;
- pup[1]=p[1]*2.;
- pup[2]=p[2]*2.;
+ pup = p;
+ pup[1] = p[1] * 2.;
+ pup[2] = p[2] * 2.;
}
/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpRT::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+ */
+void vpTemplateTrackerWarpRT::getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
{
// std::cout << "getdW0" << std::endl;
- dIdW[0]=-i*dx + j*dy;
- dIdW[1]=dx;
- dIdW[2]=dy;
+ dIdW[0] = -i * dx + j * dy;
+ dIdW[1] = dx;
+ dIdW[2] = dy;
}
/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpRT::getdWdp0(const int &i,const int &j,double *dIdW)
+ */
+void vpTemplateTrackerWarpRT::getdWdp0(const int &i, const int &j, double *dIdW)
{
- dIdW[0]=-i;
- dIdW[1]=1.;
- dIdW[2]=0;
+ dIdW[0] = -i;
+ dIdW[1] = 1.;
+ dIdW[2] = 0;
- dIdW[3]=j;
- dIdW[4]=0;
- dIdW[5]=1.;
+ dIdW[3] = j;
+ dIdW[4] = 0;
+ dIdW[5] = 1.;
}
-void vpTemplateTrackerWarpRT::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+void vpTemplateTrackerWarpRT::warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)
{
- j2=(cos(ParamM[0])*j) - (sin(ParamM[0])*i) + ParamM[1];
- i2=(sin(ParamM[0])*j) + (cos(ParamM[0])*i) + ParamM[2];
+ j2 = (cos(ParamM[0]) * j) - (sin(ParamM[0]) * i) + ParamM[1];
+ i2 = (sin(ParamM[0]) * j) + (cos(ParamM[0]) * i) + ParamM[2];
}
-
-void vpTemplateTrackerWarpRT::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpRT::warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=(cos(ParamM[0])*vX[0]) - (sin(ParamM[0])*vX[1]) + ParamM[1];
- vXres[1]=(sin(ParamM[0])*vX[0]) + (cos(ParamM[0])*vX[1]) + ParamM[2];
+ vXres[0] = (cos(ParamM[0]) * vX[0]) - (sin(ParamM[0]) * vX[1]) + ParamM[1];
+ vXres[1] = (sin(ParamM[0]) * vX[0]) + (cos(ParamM[0]) * vX[1]) + ParamM[2];
}
-void vpTemplateTrackerWarpRT::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &ParamM,vpMatrix &dW_)
+void vpTemplateTrackerWarpRT::dWarp(const vpColVector &X1, const vpColVector & /*X2*/, const vpColVector &ParamM,
+ vpMatrix &dW_)
{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=(-sin(ParamM[0])*j) - (cos(ParamM[0])*i);
- dW_[0][1]=1;
-
- dW_[1][0]=cos(ParamM[0])*j - sin(ParamM[0])*i;
- dW_[1][2]=1;
+ double j = X1[0];
+ double i = X1[1];
+ dW_ = 0;
+ dW_[0][0] = (-sin(ParamM[0]) * j) - (cos(ParamM[0]) * i);
+ dW_[0][1] = 1;
+
+ dW_[1][0] = cos(ParamM[0]) * j - sin(ParamM[0]) * i;
+ dW_[1][2] = 1;
}
/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpRT::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
- const double *dwdp0,vpMatrix &dW_)
+ */
+void vpTemplateTrackerWarpRT::dWarpCompo(const vpColVector & /*X1*/, const vpColVector & /*X2*/,
+ const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_)
{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=(cos(ParamM[0])*dwdp0[i]) - (sin(ParamM[0])*dwdp0[i+nbParam]);
- dW_[1][i]=(sin(ParamM[0])*dwdp0[i]) + (cos(ParamM[0])*dwdp0[i+nbParam]);
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] = (cos(ParamM[0]) * dwdp0[i]) - (sin(ParamM[0]) * dwdp0[i + nbParam]);
+ dW_[1][i] = (sin(ParamM[0]) * dwdp0[i]) + (cos(ParamM[0]) * dwdp0[i + nbParam]);
}
}
-void vpTemplateTrackerWarpRT::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpRT::warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
// std::cout << "warpXspe" << std::endl;
- vXres[0]=(cos(ParamM[0])*vX[0]) - (sin(ParamM[0])*vX[1]) + ParamM[1];
- vXres[1]=(sin(ParamM[0])*vX[0]) + (cos(ParamM[0])*vX[1]) + ParamM[2];
-}
+ vXres[0] = (cos(ParamM[0]) * vX[0]) - (sin(ParamM[0]) * vX[1]) + ParamM[1];
+ vXres[1] = (sin(ParamM[0]) * vX[0]) + (cos(ParamM[0]) * vX[1]) + ParamM[2];
+}
-void vpTemplateTrackerWarpRT::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+void vpTemplateTrackerWarpRT::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
vpColVector Trans(2);
- vpMatrix MWrap(2,2);
- Trans[0]=ParamM[1];
- Trans[1]=ParamM[2];
- MWrap[0][0]=cos(ParamM[0]);
- MWrap[0][1]=-sin(ParamM[0]);
- MWrap[1][0]=sin(ParamM[0]);
- MWrap[1][1]=cos(ParamM[0]);
-
- vpMatrix MWrapInv(2,2);
- MWrapInv=MWrap.transpose();
+ vpMatrix MWrap(2, 2);
+ Trans[0] = ParamM[1];
+ Trans[1] = ParamM[2];
+ MWrap[0][0] = cos(ParamM[0]);
+ MWrap[0][1] = -sin(ParamM[0]);
+ MWrap[1][0] = sin(ParamM[0]);
+ MWrap[1][1] = cos(ParamM[0]);
+
+ vpMatrix MWrapInv(2, 2);
+ MWrapInv = MWrap.transpose();
vpColVector TransInv(2);
- TransInv=(-1.0)*MWrapInv*Trans;
+ TransInv = (-1.0) * MWrapInv * Trans;
- ParamMinv[0]= atan2(MWrapInv[1][0],MWrapInv[1][1]);
- ParamMinv[1]=TransInv[0];
- ParamMinv[2]=TransInv[1];
-}
+ ParamMinv[0] = atan2(MWrapInv[1][0], MWrapInv[1][1]);
+ ParamMinv[1] = TransInv[0];
+ ParamMinv[2] = TransInv[1];
+}
-void vpTemplateTrackerWarpRT::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+void vpTemplateTrackerWarpRT::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
{
vpColVector Trans1(2);
- vpMatrix MWrap1(2,2);
- Trans1[0]=p1[1];Trans1[1]=p1[2];
-
- MWrap1[0][0]=cos(p1[0]);
- MWrap1[0][1]=-sin(p1[0]);
- MWrap1[1][0]=sin(p1[0]);
- MWrap1[1][1]=cos(p1[0]);
+ vpMatrix MWrap1(2, 2);
+ Trans1[0] = p1[1];
+ Trans1[1] = p1[2];
+
+ MWrap1[0][0] = cos(p1[0]);
+ MWrap1[0][1] = -sin(p1[0]);
+ MWrap1[1][0] = sin(p1[0]);
+ MWrap1[1][1] = cos(p1[0]);
vpColVector Trans2(2);
- vpMatrix MWrap2(2,2);
- Trans2[0]=p2[1];Trans2[1]=p2[1];
-
- MWrap2[0][0]=cos(p2[0]);
- MWrap2[0][1]=-sin(p2[0]);
- MWrap2[1][0]=sin(p2[0]);
- MWrap2[1][1]=cos(p2[0]);
-
+ vpMatrix MWrap2(2, 2);
+ Trans2[0] = p2[1];
+ Trans2[1] = p2[1];
+
+ MWrap2[0][0] = cos(p2[0]);
+ MWrap2[0][1] = -sin(p2[0]);
+ MWrap2[1][0] = sin(p2[0]);
+ MWrap2[1][1] = cos(p2[0]);
+
vpColVector TransRes(2);
- vpMatrix MWrapRes(2,2);
- TransRes=MWrap1*Trans2+Trans1;
- MWrapRes=MWrap1*MWrap2;
+ vpMatrix MWrapRes(2, 2);
+ TransRes = MWrap1 * Trans2 + Trans1;
+ MWrapRes = MWrap1 * MWrap2;
- pres[0]=atan2(MWrapRes[1][0],MWrapRes[1][1]);
- pres[1]=TransRes[0];
- pres[2]=TransRes[1];
+ pres[0] = atan2(MWrapRes[1][0], MWrapRes[1][1]);
+ pres[1] = TransRes[0];
+ pres[2] = TransRes[1];
}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp
index 142a501..d1529c9 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpSRT.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,148 +39,150 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerWarpSRT.h>
-
vpTemplateTrackerWarpSRT::vpTemplateTrackerWarpSRT()
{
- nbParam = 4 ;
- dW.resize(2,nbParam);
+ nbParam = 4;
+ dW.resize(2, nbParam);
}
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpSRT::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpSRT::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- pdown=p;
- pdown[2]=p[2]/2.;
- pdown[3]=p[3]/2.;
+ pdown = p;
+ pdown[2] = p[2] / 2.;
+ pdown[3] = p[3] / 2.;
}
-void vpTemplateTrackerWarpSRT::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpSRT::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- pup=p;
- pup[2]=p[2]*2.;
- pup[3]=p[3]*2.;
+ pup = p;
+ pup[2] = p[2] * 2.;
+ pup[3] = p[3] * 2.;
}
/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpSRT::getdW0(const int &i,const int &j,const double &dy,const double &dx,double *dIdW)
+ */
+void vpTemplateTrackerWarpSRT::getdW0(const int &i, const int &j, const double &dy, const double &dx, double *dIdW)
{
// std::cout << "getdW0" << std::endl;
- dIdW[0]=j*dx + i*dy;
- dIdW[1]=-i*dx + j*dy;
- dIdW[2]=dx;
- dIdW[3]=dy;
+ dIdW[0] = j * dx + i * dy;
+ dIdW[1] = -i * dx + j * dy;
+ dIdW[2] = dx;
+ dIdW[3] = dy;
}
/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpSRT::getdWdp0(const int &i,const int &j,double *dIdW)
+ */
+void vpTemplateTrackerWarpSRT::getdWdp0(const int &i, const int &j, double *dIdW)
{
- dIdW[0]=j;
- dIdW[1]=-i;
- dIdW[2]=1.;
- dIdW[3]=0;
-
- dIdW[4]=i;
- dIdW[5]=j;
- dIdW[6]=0;
- dIdW[7]=1.;
+ dIdW[0] = j;
+ dIdW[1] = -i;
+ dIdW[2] = 1.;
+ dIdW[3] = 0;
+
+ dIdW[4] = i;
+ dIdW[5] = j;
+ dIdW[6] = 0;
+ dIdW[7] = 1.;
}
-void vpTemplateTrackerWarpSRT::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+void vpTemplateTrackerWarpSRT::warpX(const int &i, const int &j, double &i2, double &j2, const vpColVector &ParamM)
{
- j2=((1.0+ParamM[0])*cos(ParamM[1])*j) - ((1.0+ParamM[0])*sin(ParamM[1])*i) + ParamM[2];
- i2=((1.0+ParamM[0])*sin(ParamM[1])*j) + ((1.0+ParamM[0])*cos(ParamM[1])*i) + ParamM[3];
+ j2 = ((1.0 + ParamM[0]) * cos(ParamM[1]) * j) - ((1.0 + ParamM[0]) * sin(ParamM[1]) * i) + ParamM[2];
+ i2 = ((1.0 + ParamM[0]) * sin(ParamM[1]) * j) + ((1.0 + ParamM[0]) * cos(ParamM[1]) * i) + ParamM[3];
}
-
-void vpTemplateTrackerWarpSRT::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpSRT::warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
- vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
+ vXres[0] = ((1.0 + ParamM[0]) * cos(ParamM[1]) * vX[0]) - ((1.0 + ParamM[0]) * sin(ParamM[1]) * vX[1]) + ParamM[2];
+ vXres[1] = ((1.0 + ParamM[0]) * sin(ParamM[1]) * vX[0]) + ((1.0 + ParamM[0]) * cos(ParamM[1]) * vX[1]) + ParamM[3];
}
-void vpTemplateTrackerWarpSRT::dWarp(const vpColVector &X1,const vpColVector &/*X2*/,const vpColVector &ParamM,vpMatrix &dW_)
+void vpTemplateTrackerWarpSRT::dWarp(const vpColVector &X1, const vpColVector & /*X2*/, const vpColVector &ParamM,
+ vpMatrix &dW_)
{
- double j=X1[0];
- double i=X1[1];
- dW_=0;
- dW_[0][0]=cos(ParamM[1])*j - sin(ParamM[1])*i;
- dW_[0][1]=(-(1.0+ParamM[0])*sin(ParamM[1])*j) - ((1.0+ParamM[0])*cos(ParamM[1])*i);
- dW_[0][2]=1;
-
- dW_[1][0]=sin(ParamM[1])*j + cos(ParamM[1])*i;
- dW_[1][1]=(1.0+ParamM[0])*cos(ParamM[1])*j - (1.0+ParamM[0])*sin(ParamM[1])*i;
- dW_[1][3]=1;
+ double j = X1[0];
+ double i = X1[1];
+ dW_ = 0;
+ dW_[0][0] = cos(ParamM[1]) * j - sin(ParamM[1]) * i;
+ dW_[0][1] = (-(1.0 + ParamM[0]) * sin(ParamM[1]) * j) - ((1.0 + ParamM[0]) * cos(ParamM[1]) * i);
+ dW_[0][2] = 1;
+
+ dW_[1][0] = sin(ParamM[1]) * j + cos(ParamM[1]) * i;
+ dW_[1][1] = (1.0 + ParamM[0]) * cos(ParamM[1]) * j - (1.0 + ParamM[0]) * sin(ParamM[1]) * i;
+ dW_[1][3] = 1;
}
/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpSRT::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &ParamM,
- const double *dwdp0,vpMatrix &dW_)
+ */
+void vpTemplateTrackerWarpSRT::dWarpCompo(const vpColVector & /*X1*/, const vpColVector & /*X2*/,
+ const vpColVector &ParamM, const double *dwdp0, vpMatrix &dW_)
{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=((1.+ParamM[0])*cos(ParamM[1])*dwdp0[i]) - ((1.0+ParamM[0])*sin(ParamM[1])*dwdp0[i+nbParam]);
- dW_[1][i]=((1.+ParamM[0])*sin(ParamM[1])*dwdp0[i]) + ((1.0+ParamM[0])*cos(ParamM[1])*dwdp0[i+nbParam]);
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] =
+ ((1. + ParamM[0]) * cos(ParamM[1]) * dwdp0[i]) - ((1.0 + ParamM[0]) * sin(ParamM[1]) * dwdp0[i + nbParam]);
+ dW_[1][i] =
+ ((1. + ParamM[0]) * sin(ParamM[1]) * dwdp0[i]) + ((1.0 + ParamM[0]) * cos(ParamM[1]) * dwdp0[i + nbParam]);
}
}
-void vpTemplateTrackerWarpSRT::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpSRT::warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
// std::cout << "warpXspe" << std::endl;
- vXres[0]=((1.0+ParamM[0])*cos(ParamM[1])*vX[0]) - ((1.0+ParamM[0])*sin(ParamM[1])*vX[1]) + ParamM[2];
- vXres[1]=((1.0+ParamM[0])*sin(ParamM[1])*vX[0]) + ((1.0+ParamM[0])*cos(ParamM[1])*vX[1]) + ParamM[3];
-}
+ vXres[0] = ((1.0 + ParamM[0]) * cos(ParamM[1]) * vX[0]) - ((1.0 + ParamM[0]) * sin(ParamM[1]) * vX[1]) + ParamM[2];
+ vXres[1] = ((1.0 + ParamM[0]) * sin(ParamM[1]) * vX[0]) + ((1.0 + ParamM[0]) * cos(ParamM[1]) * vX[1]) + ParamM[3];
+}
-void vpTemplateTrackerWarpSRT::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+void vpTemplateTrackerWarpSRT::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
vpColVector Trans(2);
- vpMatrix MWrap(2,2);
- Trans[0]=ParamM[2];
- Trans[1]=ParamM[3];
- MWrap[0][0]=cos(ParamM[1]);
- MWrap[0][1]=-sin(ParamM[1]);
- MWrap[1][0]=sin(ParamM[1]);
- MWrap[1][1]=cos(ParamM[1]);
-
- vpMatrix MWrapInv(2,2);
- MWrapInv=MWrap.transpose();
+ vpMatrix MWrap(2, 2);
+ Trans[0] = ParamM[2];
+ Trans[1] = ParamM[3];
+ MWrap[0][0] = cos(ParamM[1]);
+ MWrap[0][1] = -sin(ParamM[1]);
+ MWrap[1][0] = sin(ParamM[1]);
+ MWrap[1][1] = cos(ParamM[1]);
+
+ vpMatrix MWrapInv(2, 2);
+ MWrapInv = MWrap.transpose();
vpColVector TransInv(2);
- TransInv=(-1.0/(1.0+ParamM[0]))*MWrapInv*Trans;
-
- ParamMinv[0]=1.0/(1.0+ParamM[0]) - 1.0;
- ParamMinv[1]= atan2(MWrapInv[1][0],MWrapInv[1][1]);
- ParamMinv[2]=TransInv[0];
- ParamMinv[3]=TransInv[1];
-}
-
-void vpTemplateTrackerWarpSRT::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+ TransInv = (-1.0 / (1.0 + ParamM[0])) * MWrapInv * Trans;
+
+ ParamMinv[0] = 1.0 / (1.0 + ParamM[0]) - 1.0;
+ ParamMinv[1] = atan2(MWrapInv[1][0], MWrapInv[1][1]);
+ ParamMinv[2] = TransInv[0];
+ ParamMinv[3] = TransInv[1];
+}
+
+void vpTemplateTrackerWarpSRT::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
{
vpColVector Trans1(2);
- vpMatrix MWrap1(2,2);
- Trans1[0]=p1[2];Trans1[1]=p1[3];
-
- MWrap1[0][0]=cos(p1[1]);
- MWrap1[0][1]=-sin(p1[1]);
- MWrap1[1][0]=sin(p1[1]);
- MWrap1[1][1]=cos(p1[1]);
+ vpMatrix MWrap1(2, 2);
+ Trans1[0] = p1[2];
+ Trans1[1] = p1[3];
+
+ MWrap1[0][0] = cos(p1[1]);
+ MWrap1[0][1] = -sin(p1[1]);
+ MWrap1[1][0] = sin(p1[1]);
+ MWrap1[1][1] = cos(p1[1]);
vpColVector Trans2(2);
- vpMatrix MWrap2(2,2);
- Trans2[0]=p2[2];Trans2[1]=p2[3];
-
- MWrap2[0][0]=cos(p2[1]);
- MWrap2[0][1]=-sin(p2[1]);
- MWrap2[1][0]=sin(p2[1]);
- MWrap2[1][1]=cos(p2[1]);
-
+ vpMatrix MWrap2(2, 2);
+ Trans2[0] = p2[2];
+ Trans2[1] = p2[3];
+
+ MWrap2[0][0] = cos(p2[1]);
+ MWrap2[0][1] = -sin(p2[1]);
+ MWrap2[1][0] = sin(p2[1]);
+ MWrap2[1][1] = cos(p2[1]);
+
vpColVector TransRes(2);
- vpMatrix MWrapRes(2,2);
- TransRes=(1.0+p1[0])*MWrap1*Trans2+Trans1;
- MWrapRes=MWrap1*MWrap2;
-
- pres[0]=(1.0+p1[0])*(1.0+p2[0]) - 1.0;
- pres[1]=atan2(MWrapRes[1][0],MWrapRes[1][1]);
-
- pres[2]=TransRes[0];
- pres[3]=TransRes[1];
+ vpMatrix MWrapRes(2, 2);
+ TransRes = (1.0 + p1[0]) * MWrap1 * Trans2 + Trans1;
+ MWrapRes = MWrap1 * MWrap2;
+
+ pres[0] = (1.0 + p1[0]) * (1.0 + p2[0]) - 1.0;
+ pres[1] = atan2(MWrapRes[1][0], MWrapRes[1][1]);
+
+ pres[2] = TransRes[0];
+ pres[3] = TransRes[1];
}
diff --git a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp
index ba900c4..c196cb3 100644
--- a/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp
+++ b/modules/tracker/tt/src/warp/vpTemplateTrackerWarpTranslation.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,87 +41,88 @@
vpTemplateTrackerWarpTranslation::vpTemplateTrackerWarpTranslation()
{
- nbParam = 2 ;
- dW.resize(2,nbParam);
+ nbParam = 2;
+ dW.resize(2, nbParam);
}
-
-//get the parameter corresponding to the lower level of a gaussian pyramid
-void vpTemplateTrackerWarpTranslation::getParamPyramidDown(const vpColVector &p,vpColVector &pdown)
+// get the parameter corresponding to the lower level of a gaussian pyramid
+void vpTemplateTrackerWarpTranslation::getParamPyramidDown(const vpColVector &p, vpColVector &pdown)
{
- pdown[0]=p[0]/2.;
- pdown[1]=p[1]/2.;
+ pdown[0] = p[0] / 2.;
+ pdown[1] = p[1] / 2.;
}
-void vpTemplateTrackerWarpTranslation::getParamPyramidUp(const vpColVector &p,vpColVector &pup)
+void vpTemplateTrackerWarpTranslation::getParamPyramidUp(const vpColVector &p, vpColVector &pup)
{
- pup[0]=p[0]*2.;
- pup[1]=p[1]*2.;
+ pup[0] = p[0] * 2.;
+ pup[1] = p[1] * 2.;
}
/*calcul de di*dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpTranslation::getdW0(const int &/*i*/,const int &/*j*/,const double &dy,const double &dx,double *dIdW)
+ */
+void vpTemplateTrackerWarpTranslation::getdW0(const int & /*i*/, const int & /*j*/, const double &dy, const double &dx,
+ double *dIdW)
{
- dIdW[0]=dx;
- dIdW[1]=dy;
+ dIdW[0] = dx;
+ dIdW[1] = dy;
}
/*calcul de dw(x,p0)/dp
-*/
-void vpTemplateTrackerWarpTranslation::getdWdp0(const int &/*i*/,const int &/*j*/,double *dIdW)
+ */
+void vpTemplateTrackerWarpTranslation::getdWdp0(const int & /*i*/, const int & /*j*/, double *dIdW)
{
- dIdW[0]=1.;
- dIdW[1]=0;
+ dIdW[0] = 1.;
+ dIdW[1] = 0;
- dIdW[2]=0;
- dIdW[3]=1.;
+ dIdW[2] = 0;
+ dIdW[3] = 1.;
}
-void vpTemplateTrackerWarpTranslation::warpX(const int &i,const int &j,double &i2,double &j2,const vpColVector &ParamM)
+void vpTemplateTrackerWarpTranslation::warpX(const int &i, const int &j, double &i2, double &j2,
+ const vpColVector &ParamM)
{
- j2=j+ParamM[0];
- i2=i+ParamM[1];
+ j2 = j + ParamM[0];
+ i2 = i + ParamM[1];
}
-
-void vpTemplateTrackerWarpTranslation::warpX(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpTranslation::warpX(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=vX[0]+ParamM[0];
- vXres[1]=vX[1]+ParamM[1];
+ vXres[0] = vX[0] + ParamM[0];
+ vXres[1] = vX[1] + ParamM[1];
}
-void vpTemplateTrackerWarpTranslation::dWarp(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
- vpMatrix &dW_)
+void vpTemplateTrackerWarpTranslation::dWarp(const vpColVector & /*X1*/, const vpColVector & /*X2*/,
+ const vpColVector & /*ParamM*/, vpMatrix &dW_)
{
- dW_[0][0]=1;dW_[0][1]=0;
- dW_[1][0]=0;dW_[1][1]=1;
+ dW_[0][0] = 1;
+ dW_[0][1] = 0;
+ dW_[1][0] = 0;
+ dW_[1][1] = 1;
}
/*compute dw=dw/dx*dw/dp
-*/
-void vpTemplateTrackerWarpTranslation::dWarpCompo(const vpColVector &/*X1*/,const vpColVector &/*X2*/,const vpColVector &/*ParamM*/,
- const double *dwdp0,vpMatrix &dW_)
+ */
+void vpTemplateTrackerWarpTranslation::dWarpCompo(const vpColVector & /*X1*/, const vpColVector & /*X2*/,
+ const vpColVector & /*ParamM*/, const double *dwdp0, vpMatrix &dW_)
{
- for(unsigned int i=0;i<nbParam;i++)
- {
- dW_[0][i]=dwdp0[i];
- dW_[1][i]=dwdp0[i+nbParam];
+ for (unsigned int i = 0; i < nbParam; i++) {
+ dW_[0][i] = dwdp0[i];
+ dW_[1][i] = dwdp0[i + nbParam];
}
}
-void vpTemplateTrackerWarpTranslation::warpXInv(const vpColVector &vX,vpColVector &vXres,const vpColVector &ParamM)
+void vpTemplateTrackerWarpTranslation::warpXInv(const vpColVector &vX, vpColVector &vXres, const vpColVector &ParamM)
{
- vXres[0]=vX[0]+ParamM[0];
- vXres[1]=vX[1]+ParamM[1];
-}
-void vpTemplateTrackerWarpTranslation::getParamInverse(const vpColVector &ParamM,vpColVector &ParamMinv) const
+ vXres[0] = vX[0] + ParamM[0];
+ vXres[1] = vX[1] + ParamM[1];
+}
+void vpTemplateTrackerWarpTranslation::getParamInverse(const vpColVector &ParamM, vpColVector &ParamMinv) const
{
- ParamMinv[0]=-ParamM[0];
- ParamMinv[1]=-ParamM[1];
-}
+ ParamMinv[0] = -ParamM[0];
+ ParamMinv[1] = -ParamM[1];
+}
-void vpTemplateTrackerWarpTranslation::pRondp(const vpColVector &p1, const vpColVector &p2,vpColVector &pres) const
+void vpTemplateTrackerWarpTranslation::pRondp(const vpColVector &p1, const vpColVector &p2, vpColVector &pres) const
{
- pres[0]=p1[0]+p2[0];
- pres[1]=p1[1]+p2[1];
+ pres[0] = p1[0] + p2[0];
+ pres[1] = p1[1] + p2[1];
}
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp
index afc7917..5837f8f 100644
--- a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCC.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,13 +39,12 @@
*****************************************************************************/
#include <visp3/tt/vpTemplateTrackerZNCC.h>
-vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
- : vpTemplateTracker(warp), DI(), temp()
+vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp) : vpTemplateTracker(warp), DI(), temp()
{
- dW.resize(2,nbParam);
+ dW.resize(2, nbParam);
G.resize(nbParam);
- H.resize(nbParam,nbParam);
- HLM.resize(nbParam,nbParam);
+ H.resize(nbParam, nbParam);
+ HLM.resize(nbParam, nbParam);
temp.resize(nbParam);
@@ -53,79 +53,75 @@ vpTemplateTrackerZNCC::vpTemplateTrackerZNCC(vpTemplateTrackerWarp *warp)
DI.resize(2);
}
-
double vpTemplateTrackerZNCC::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- double IW,Tij;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
+ double IW, Tij;
+ int i, j;
+ double i2, j2;
+ int Nbpoint = 0;
Warp->computeCoeff(tp);
- double moyTij=0;
- double moyIW=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
+ double moyTij = 0;
+ double moyIW = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
+ Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- moyTij+=Tij;
- moyIW+=IW;
+ IW = BI.getValue(i2, j2);
+ // IW=getSubPixBspline4(I,i2,j2);
+ moyTij += Tij;
+ moyIW += IW;
Nbpoint++;
}
}
- ratioPixelIn=(double)Nbpoint/(double)templateSize;
- if(! Nbpoint) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot get cost: size = 0")) ;
+ ratioPixelIn = (double)Nbpoint / (double)templateSize;
+ if (!Nbpoint) {
+ throw(vpException(vpException::divideByZeroError, "Cannot get cost: size = 0"));
}
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
+ moyTij = moyTij / Nbpoint;
+ moyIW = moyIW / Nbpoint;
- double nom=0;//,denom=0;
- double var1=0,var2=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
+ double nom = 0; //,denom=0;
+ double var1 = 0, var2 = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
- j2=X2[0];i2=X2[1];
- if((j2<I.getWidth()-1)&&(i2<I.getHeight()-1)&&(i2>0)&&(j2>0))
- {
- Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((j2 < I.getWidth() - 1) && (i2 < I.getHeight() - 1) && (i2 > 0) && (j2 > 0)) {
+ Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
- //IW=getSubPixBspline4(I,i2,j2);
- nom+=(Tij-moyTij)*(IW-moyIW);
- //denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
- var1+=(IW-moyIW)*(IW-moyIW);
- var2+=(Tij-moyTij)*(Tij-moyTij);
+ IW = BI.getValue(i2, j2);
+ // IW=getSubPixBspline4(I,i2,j2);
+ nom += (Tij - moyTij) * (IW - moyIW);
+ // denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ var1 += (IW - moyIW) * (IW - moyIW);
+ var2 += (Tij - moyTij) * (Tij - moyTij);
Nbpoint++;
}
}
// if(Nbpoint==0)return 10e10; // cannot occur
- //return -nom/sqrt(denom);
- return -nom/sqrt(var1*var2);
+ // return -nom/sqrt(denom);
+ return -nom / sqrt(var1 * var2);
}
-
-
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
index 800efe5..d1a1f51 100644
--- a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCForwardAdditional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,130 +37,129 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
#include <visp3/core/vpImageFilter.h>
+#include <visp3/tt/vpTemplateTrackerZNCCForwardAdditional.h>
-vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp):vpTemplateTrackerZNCC(warp)
+vpTemplateTrackerZNCCForwardAdditional::vpTemplateTrackerZNCCForwardAdditional(vpTemplateTrackerWarp *warp)
+ : vpTemplateTrackerZNCC(warp)
{
- useCompositionnal=false;
+ useCompositionnal = false;
}
void vpTemplateTrackerZNCCForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- vpImage<double> dIxx,dIxy,dIyx,dIyy;
- vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
- vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
+ vpImage<double> dIxx, dIxy, dIyx, dIyy;
+ vpImageFilter::getGradX(dIx, dIxx, fgdG, taillef);
+ vpImageFilter::getGradY(dIx, dIxy, fgdG, taillef);
- vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
- vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
+ vpImageFilter::getGradX(dIy, dIyx, fgdG, taillef);
+ vpImageFilter::getGradY(dIy, dIyy, fgdG, taillef);
Warp->computeCoeff(p);
- double IW,dIWx,dIWy;
+ double IW, dIWx, dIWy;
double Tij;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- double moyTij=0;
- double moyIW=0;
- double denom=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
+ int i, j;
+ double i2, j2;
+ int Nbpoint = 0;
+
+ double moyTij = 0;
+ double moyIW = 0;
+ double denom = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ X2[0] = j;
+ X2[1] = i;
+
+ Warp->computeDenom(X1, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
Nbpoint++;
- moyTij+=Tij;
- moyIW+=IW;
+ moyTij += Tij;
+ moyIW += IW;
}
}
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
- Hdesire=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
+ moyTij = moyTij / Nbpoint;
+ moyIW = moyIW / Nbpoint;
+ Hdesire = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ X2[0] = j;
+ X2[1] = i;
+
+ Warp->computeDenom(X1, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
-
-
- double prod=(Tij-moyTij);
-
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- Hdesire[it][jt] +=prod*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
+ IW = BI.getValue(i2, j2);
+
+ dIWx = dIx.getValue(i2, j2);
+ dIWy = dIy.getValue(i2, j2);
+ // Calcul du Hessien
+ Warp->dWarp(X1, X2, p, dW);
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
+
+ double prod = (Tij - moyTij);
+
+ double d_Ixx = dIxx.getValue(i2, j2);
+ double d_Iyy = dIyy.getValue(i2, j2);
+ double d_Ixy = dIxy.getValue(i2, j2);
+
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ Hdesire[it][jt] += prod * (dW[0][it] * (dW[0][jt] * d_Ixx + dW[1][jt] * d_Ixy) +
+ dW[1][it] * (dW[0][jt] * d_Ixy + dW[1][jt] * d_Iyy));
/*Hdesire[0][0]+=prod*d_Ixx;
Hdesire[1][0]+=prod*d_Ixy;
Hdesire[0][1]+=prod*d_Ixy;
Hdesire[1][1]+=prod*d_Iyy;*/
- denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
delete[] tempt;
}
-
-
}
- Hdesire=Hdesire/sqrt(denom);
- vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
-
+ Hdesire = Hdesire / sqrt(denom);
+ vpMatrix::computeHLM(Hdesire, lambdaDep, HLMdesire);
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
}
void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
/*vpImage<double> dIxx,dIxy,dIyx,dIyy;
getGradX(dIx, dIxx, fgdG,taillef);
@@ -168,90 +168,87 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned c
getGradX(dIy, dIyx, fgdG,taillef);
getGradY(dIy, dIyy, fgdG,taillef);*/
- dW=0;
+ dW = 0;
- //double lambda=lambdaDep;
- double IW,dIWx,dIWy;
+ // double lambda=lambdaDep;
+ double IW, dIWx, dIWy;
double Tij;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
- double alpha=2.;
- do
- {
- int Nbpoint=0;
- double erreur=0;
- G=0;
- H=0 ;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
+ double alpha = 2.;
+ do {
+ int Nbpoint = 0;
+ double erreur = 0;
+ G = 0;
+ H = 0;
Warp->computeCoeff(p);
- double moyTij=0;
- double moyIW=0;
- double denom=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
+ double moyTij = 0;
+ double moyIW = 0;
+ double denom = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
Nbpoint++;
- moyTij+=Tij;
- moyIW+=IW;
+ moyTij += Tij;
+ moyIW += IW;
}
}
- if(! Nbpoint) {
- throw(vpException(vpException::divideByZeroError,
- "Cannot track the template: no point")) ;
+ if (!Nbpoint) {
+ throw(vpException(vpException::divideByZeroError, "Cannot track the template: no point"));
}
- moyTij=moyTij/Nbpoint;
- moyIW=moyIW/Nbpoint;
- //vpMatrix d2Wx(nbParam,nbParam);
- //vpMatrix d2Wy(nbParam,nbParam);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Tij=ptTemplate[point].val;
-
- if(!blur)
- IW=I.getValue(i2,j2);
+ moyTij = moyTij / Nbpoint;
+ moyIW = moyIW / Nbpoint;
+ // vpMatrix d2Wx(nbParam,nbParam);
+ // vpMatrix d2Wy(nbParam,nbParam);
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Tij = ptTemplate[point].val;
+
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
-
- dIWx=dIx.getValue(i2,j2);
- dIWy=dIy.getValue(i2,j2);
- //Calcul du Hessien
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIWx+dW[1][it]*dIWy;
+ IW = BI.getValue(i2, j2);
+ dIWx = dIx.getValue(i2, j2);
+ dIWy = dIy.getValue(i2, j2);
+ // Calcul du Hessien
+ Warp->dWarp(X1, X2, p, dW);
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIWx + dW[1][it] * dIWy;
- double prod=(Tij-moyTij);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=prod*tempt[it];
+ double prod = (Tij - moyTij);
+ for (unsigned int it = 0; it < nbParam; it++)
+ G[it] += prod * tempt[it];
/* Warp->d2Warp(X1,X2,p,d2Wx,d2Wy);
for(int it=0;it<nbParam;it++)
@@ -270,48 +267,41 @@ void vpTemplateTrackerZNCCForwardAdditional::trackNoPyr(const vpImage<unsigned c
H[0][1]+=prod*d_Ixy;
H[1][1]+=prod*d_Iyy;*/
- double er=(Tij-IW);
- erreur+=(er*er);
- denom+=(Tij-moyTij)*(Tij-moyTij)*(IW-moyIW)*(IW-moyIW);
+ double er = (Tij - IW);
+ erreur += (er * er);
+ denom += (Tij - moyTij) * (Tij - moyTij) * (IW - moyIW) * (IW - moyIW);
delete[] tempt;
}
-
-
}
/*std::cout<<"G="<<G<<std::endl;
std::cout<<"H="<<H<<std::endl;
std::cout<<" denom="<<denom<<std::endl;*/
- G=G/sqrt(denom);
- //std::cout<<G<<std::endl;
- H=H/sqrt(denom);
-
- //if(Nbpoint==0)std::cout<<"plus de point dans template suivi"<<std::endl; // cannot occur
-
- try
- {
- //vpMatrix::computeHLM(H,lambda,HLM);
- //dp=1.*HLM.inverseByLU()*G;
- dp=1.*HLMdesireInverse*G;
- }
- catch(vpException &e)
- {
- //std::cout<<"probleme inversion"<<std::endl;
+ G = G / sqrt(denom);
+ // std::cout<<G<<std::endl;
+ H = H / sqrt(denom);
+
+ // if(Nbpoint==0)std::cout<<"plus de point dans template
+ // suivi"<<std::endl; // cannot occur
+
+ try {
+ // vpMatrix::computeHLM(H,lambda,HLM);
+ // dp=1.*HLM.inverseByLU()*G;
+ dp = 1. * HLMdesireInverse * G;
+ } catch (vpException &e) {
+ // std::cout<<"probleme inversion"<<std::endl;
throw(e);
}
- dp=gain*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,erreur/Nbpoint,dp,alpha);
- dp=alpha*dp;
+ dp = gain * dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, erreur / Nbpoint, dp, alpha);
+ dp = alpha * dp;
}
- p-=dp;
+ p -= dp;
iteration++;
- }
- while( /*( erreur_prec-erreur<50) && */(iteration < iterationMax));
+ } while (/*( erreur_prec-erreur<50) && */ (iteration < iterationMax));
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
+ // std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration = iteration;
}
-
diff --git a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
index 81315cb..98568da 100644
--- a/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
+++ b/modules/tracker/tt/src/zncc/vpTemplateTrackerZNCCInverseCompositional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,344 +37,332 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
-#include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
#include <visp3/core/vpImageFilter.h>
+#include <visp3/tt/vpTemplateTrackerZNCCInverseCompositional.h>
vpTemplateTrackerZNCCInverseCompositional::vpTemplateTrackerZNCCInverseCompositional(vpTemplateTrackerWarp *warp)
- : vpTemplateTrackerZNCC(warp), compoInitialised(false),
- evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8), moydIrefdp()
+ : vpTemplateTrackerZNCC(warp), compoInitialised(false), evolRMS(0), x_pos(), y_pos(), threshold_RMS(1e-8),
+ moydIrefdp()
{
- useInverse=true;
+ useInverse = true;
}
void vpTemplateTrackerZNCCInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
{
- //std::cout<<"Initialise precomputed value of Compositionnal Inverse"<<std::endl;
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ // std::cout<<"Initialise precomputed value of Compositionnal
+ // Inverse"<<std::endl;
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ ptTemplate[point].dW = new double[nbParam];
- double dx=ptTemplate[point].dx;
- double dy=ptTemplate[point].dy;
- //std::cout<<ptTemplate[point].dx<<","<<ptTemplate[point].dy<<std::endl;
-
- Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
+ double dx = ptTemplate[point].dx;
+ double dy = ptTemplate[point].dy;
+ // std::cout<<ptTemplate[point].dx<<","<<ptTemplate[point].dy<<std::endl;
+ Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
}
- //vpTRACE("fin Comp Inverse");
- compoInitialised=true;
+ // vpTRACE("fin Comp Inverse");
+ compoInitialised = true;
}
void vpTemplateTrackerZNCCInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse(I);
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- vpImage<double> dIxx,dIxy,dIyx,dIyy;
- vpImageFilter::getGradX(dIx, dIxx, fgdG,taillef);
- vpImageFilter::getGradY(dIx, dIxy, fgdG,taillef);
+ vpImage<double> dIxx, dIxy, dIyx, dIyy;
+ vpImageFilter::getGradX(dIx, dIxx, fgdG, taillef);
+ vpImageFilter::getGradY(dIx, dIxy, fgdG, taillef);
- vpImageFilter::getGradX(dIy, dIyx, fgdG,taillef);
- vpImageFilter::getGradY(dIy, dIyy, fgdG,taillef);
+ vpImageFilter::getGradX(dIy, dIyx, fgdG, taillef);
+ vpImageFilter::getGradY(dIy, dIyy, fgdG, taillef);
Warp->computeCoeff(p);
- double Ic,dIcx=0.,dIcy=0.;
+ double Ic, dIcx = 0., dIcy = 0.;
double Iref;
- int i,j;
- double i2,j2;
- int Nbpoint=0;
-
- double moyIref=0;
- double moyIc=0;
- double denom=0;
- moydIrefdp.resize(nbParam); moydIrefdp=0;
- vpMatrix moyd2Iref(nbParam,nbParam);moyd2Iref=0;
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
+ int i, j;
+ double i2, j2;
+ int Nbpoint = 0;
+
+ double moyIref = 0;
+ double moyIc = 0;
+ double denom = 0;
+ moydIrefdp.resize(nbParam);
+ moydIrefdp = 0;
+ vpMatrix moyd2Iref(nbParam, nbParam);
+ moyd2Iref = 0;
+
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ X2[0] = j;
+ X2[1] = i;
+
+ Warp->computeDenom(X1, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Iref = ptTemplate[point].val;
+
+ if (!blur)
+ Ic = I.getValue(i2, j2);
else
- Ic=BI.getValue(i2,j2);
+ Ic = BI.getValue(i2, j2);
Nbpoint++;
- moyIref+=Iref;
- moyIc+=Ic;
-
- for(unsigned int it=0;it<nbParam;it++)
- moydIrefdp[it]+=ptTemplate[point].dW[it];
-
-
- Warp->dWarp(X1,X2,p,dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
-
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- {
- moyd2Iref[it][jt] +=(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy));
+ moyIref += Iref;
+ moyIc += Ic;
+
+ for (unsigned int it = 0; it < nbParam; it++)
+ moydIrefdp[it] += ptTemplate[point].dW[it];
+
+ Warp->dWarp(X1, X2, p, dW);
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIcx + dW[1][it] * dIcy;
+ double d_Ixx = dIxx.getValue(i2, j2);
+ double d_Iyy = dIyy.getValue(i2, j2);
+ double d_Ixy = dIxy.getValue(i2, j2);
+
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++) {
+ moyd2Iref[it][jt] += (dW[0][it] * (dW[0][jt] * d_Ixx + dW[1][jt] * d_Ixy) +
+ dW[1][it] * (dW[0][jt] * d_Ixy + dW[1][jt] * d_Iyy));
}
delete[] tempt;
-
-
}
}
- moyIref=moyIref/Nbpoint;
- moydIrefdp=moydIrefdp/Nbpoint;
- moyd2Iref=moyd2Iref/Nbpoint;
- moyIc=moyIc/Nbpoint;
- Hdesire=0;
- double covarIref=0,covarIc=0;
- double sIcIref=0;
- vpColVector sIcdIref(nbParam);sIcdIref=0;
- vpMatrix sIcd2Iref(nbParam,nbParam);sIcd2Iref=0;
- vpMatrix sdIrefdIref(nbParam,nbParam);sdIrefdIref=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
-
- Warp->computeDenom(X1,p);
-
- j2=X2[0];i2=X2[1];
-
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
+ moyIref = moyIref / Nbpoint;
+ moydIrefdp = moydIrefdp / Nbpoint;
+ moyd2Iref = moyd2Iref / Nbpoint;
+ moyIc = moyIc / Nbpoint;
+ Hdesire = 0;
+ double covarIref = 0, covarIc = 0;
+ double sIcIref = 0;
+ vpColVector sIcdIref(nbParam);
+ sIcdIref = 0;
+ vpMatrix sIcd2Iref(nbParam, nbParam);
+ sIcd2Iref = 0;
+ vpMatrix sdIrefdIref(nbParam, nbParam);
+ sdIrefdIref = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ X2[0] = j;
+ X2[1] = i;
+
+ Warp->computeDenom(X1, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Iref = ptTemplate[point].val;
+
+ if (!blur)
+ Ic = I.getValue(i2, j2);
else
- Ic=BI.getValue(i2,j2);
+ Ic = BI.getValue(i2, j2);
- dIcx=dIx.getValue(i2,j2);
- dIcy=dIy.getValue(i2,j2);
+ dIcx = dIx.getValue(i2, j2);
+ dIcy = dIy.getValue(i2, j2);
- Warp->dWarp(X1,X2,p,dW);
+ Warp->dWarp(X1, X2, p, dW);
- double *tempt=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tempt[it]=dW[0][it]*dIcx+dW[1][it]*dIcy;
+ double *tempt = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tempt[it] = dW[0][it] * dIcx + dW[1][it] * dIcy;
- double prodIc=(Ic-moyIc);
+ double prodIc = (Ic - moyIc);
- double d_Ixx=dIxx.getValue(i2,j2);
- double d_Iyy=dIyy.getValue(i2,j2);
- double d_Ixy=dIxy.getValue(i2,j2);
+ double d_Ixx = dIxx.getValue(i2, j2);
+ double d_Iyy = dIyy.getValue(i2, j2);
+ double d_Ixy = dIxy.getValue(i2, j2);
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- {
- sIcd2Iref[it][jt] +=prodIc*(dW[0][it]*(dW[0][jt]*d_Ixx+dW[1][jt]*d_Ixy)
- +dW[1][it]*(dW[0][jt]*d_Ixy+dW[1][jt]*d_Iyy)-moyd2Iref[it][jt]);
- sdIrefdIref[it][jt] +=(ptTemplate[point].dW[it]-moydIrefdp[it])*(ptTemplate[point].dW[jt]-moydIrefdp[jt]);
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++) {
+ sIcd2Iref[it][jt] += prodIc * (dW[0][it] * (dW[0][jt] * d_Ixx + dW[1][jt] * d_Ixy) +
+ dW[1][it] * (dW[0][jt] * d_Ixy + dW[1][jt] * d_Iyy) - moyd2Iref[it][jt]);
+ sdIrefdIref[it][jt] +=
+ (ptTemplate[point].dW[it] - moydIrefdp[it]) * (ptTemplate[point].dW[jt] - moydIrefdp[jt]);
}
-
delete[] tempt;
- for(unsigned int it=0;it<nbParam;it++)
- sIcdIref[it]+=prodIc*(ptTemplate[point].dW[it]-moydIrefdp[it]);
+ for (unsigned int it = 0; it < nbParam; it++)
+ sIcdIref[it] += prodIc * (ptTemplate[point].dW[it] - moydIrefdp[it]);
- covarIref+=(Iref-moyIref)*(Iref-moyIref);
- covarIc+=(Ic-moyIc)*(Ic-moyIc);
- sIcIref+=(Iref-moyIref)*(Ic-moyIc);
+ covarIref += (Iref - moyIref) * (Iref - moyIref);
+ covarIc += (Ic - moyIc) * (Ic - moyIc);
+ sIcIref += (Iref - moyIref) * (Ic - moyIc);
}
-
-
}
- covarIref=sqrt(covarIref);
- covarIc=sqrt(covarIc);
+ covarIref = sqrt(covarIref);
+ covarIc = sqrt(covarIc);
- denom=covarIref*covarIc;
+ denom = covarIref * covarIc;
- double NCC=sIcIref/denom;
- //std::cout<<"NCC = "<<NCC<<std::endl;
- vpColVector dcovarIref(nbParam);dcovarIref=-sIcdIref/covarIref;
+ double NCC = sIcIref / denom;
+ // std::cout<<"NCC = "<<NCC<<std::endl;
+ vpColVector dcovarIref(nbParam);
+ dcovarIref = -sIcdIref / covarIref;
- vpColVector dNCC(nbParam);dNCC=(sIcdIref/denom-NCC*dcovarIref/covarIref);
- vpMatrix d2covarIref(nbParam,nbParam);
- d2covarIref=-(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/covarIref;
+ vpColVector dNCC(nbParam);
+ dNCC = (sIcdIref / denom - NCC * dcovarIref / covarIref);
+ vpMatrix d2covarIref(nbParam, nbParam);
+ d2covarIref = -(sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.t()) / covarIref;
#ifdef APPROX_NCC
- Hdesire=sIcd2Iref/denom;
+ Hdesire = sIcd2Iref / denom;
#else
- Hdesire=(sIcd2Iref-sdIrefdIref+dcovarIref*dcovarIref.t())/denom;
+ Hdesire = (sIcd2Iref - sdIrefdIref + dcovarIref * dcovarIref.t()) / denom;
#endif
- vpMatrix::computeHLM(Hdesire,lambdaDep,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+ vpMatrix::computeHLM(Hdesire, lambdaDep, HLMdesire);
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
}
void vpTemplateTrackerZNCCInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
- //double erreur=0;
+ // double erreur=0;
vpColVector dpinv(nbParam);
double Ic;
double Iref;
- unsigned int iteration=0;
- int i,j;
- double i2,j2;
+ unsigned int iteration = 0;
+ int i, j;
+ double i2, j2;
initPosEvalRMS(p);
- do
- {
- unsigned int Nbpoint=0;
- //erreur=0;
- G=0;
+ do {
+ unsigned int Nbpoint = 0;
+ // erreur=0;
+ G = 0;
Warp->computeCoeff(p);
- double moyIref=0;
- double moyIc=0;
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
+ double moyIref = 0;
+ double moyIc = 0;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Iref = ptTemplate[point].val;
+
+ if (!blur)
+ Ic = I.getValue(i2, j2);
else
- Ic=BI.getValue(i2,j2);
+ Ic = BI.getValue(i2, j2);
Nbpoint++;
- moyIref+=Iref;
- moyIc+=Ic;
+ moyIref += Iref;
+ moyIc += Ic;
}
-
-
}
- if(Nbpoint > 0)
- {
- moyIref=moyIref/Nbpoint;
- moyIc=moyIc/Nbpoint;
- double sIcIref=0;
- double covarIref=0,covarIc=0;
- vpColVector sIcdIref(nbParam);sIcdIref=0;
- vpColVector sIrefdIref(nbParam);sIrefdIref=0;
-
-
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
-
- j2=X2[0];i2=X2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- Iref=ptTemplate[point].val;
-
- if(!blur)
- Ic=I.getValue(i2,j2);
+ if (Nbpoint > 0) {
+ moyIref = moyIref / Nbpoint;
+ moyIc = moyIc / Nbpoint;
+ double sIcIref = 0;
+ double covarIref = 0, covarIc = 0;
+ vpColVector sIcdIref(nbParam);
+ sIcdIref = 0;
+ vpColVector sIrefdIref(nbParam);
+ sIrefdIref = 0;
+
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
+
+ j2 = X2[0];
+ i2 = X2[1];
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ Iref = ptTemplate[point].val;
+
+ if (!blur)
+ Ic = I.getValue(i2, j2);
else
- Ic=BI.getValue(i2,j2);
-
-
- double prod=(Ic-moyIc);
- for(unsigned int it=0;it<nbParam;it++)
- sIcdIref[it]+=prod*(ptTemplate[point].dW[it]-moydIrefdp[it]);
- for(unsigned int it=0;it<nbParam;it++)
- sIrefdIref[it]+=(Iref-moyIref)*(ptTemplate[point].dW[it]-moydIrefdp[it]);
-
- //double er=(Iref-Ic);
- //erreur+=(er*er);
- //denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
- covarIref+=(Iref-moyIref)*(Iref-moyIref);
- covarIc+=(Ic-moyIc)*(Ic-moyIc);
- sIcIref+=(Iref-moyIref)*(Ic-moyIc);
+ Ic = BI.getValue(i2, j2);
+
+ double prod = (Ic - moyIc);
+ for (unsigned int it = 0; it < nbParam; it++)
+ sIcdIref[it] += prod * (ptTemplate[point].dW[it] - moydIrefdp[it]);
+ for (unsigned int it = 0; it < nbParam; it++)
+ sIrefdIref[it] += (Iref - moyIref) * (ptTemplate[point].dW[it] - moydIrefdp[it]);
+
+ // double er=(Iref-Ic);
+ // erreur+=(er*er);
+ // denom+=(Iref-moyIref)*(Iref-moyIref)*(Ic-moyIc)*(Ic-moyIc);
+ covarIref += (Iref - moyIref) * (Iref - moyIref);
+ covarIc += (Ic - moyIc) * (Ic - moyIc);
+ sIcIref += (Iref - moyIref) * (Ic - moyIc);
}
-
-
- }
- covarIref=sqrt(covarIref);
- covarIc=sqrt(covarIc);
- double denom=covarIref*covarIc;
-
- //if(denom==0.0)
- if (std::fabs(denom) <= std::numeric_limits<double>::epsilon())
- {
- diverge=true;
}
- else
- {
- double NCC=sIcIref/denom;
- vpColVector dcovarIref(nbParam);dcovarIref=sIrefdIref/covarIref;
- G=1.*(sIcdIref/denom-NCC*dcovarIref/covarIref);
-
-
- try
- {
- dp=-1.*HLMdesireInverse*G;
- }
- catch(...)
- {
- std::cout<<"probleme inversion"<<std::endl;
+ covarIref = sqrt(covarIref);
+ covarIc = sqrt(covarIc);
+ double denom = covarIref * covarIc;
+
+ // if(denom==0.0)
+ if (std::fabs(denom) <= std::numeric_limits<double>::epsilon()) {
+ diverge = true;
+ } else {
+ double NCC = sIcIref / denom;
+ vpColVector dcovarIref(nbParam);
+ dcovarIref = sIrefdIref / covarIref;
+ G = 1. * (sIcdIref / denom - NCC * dcovarIref / covarIref);
+
+ try {
+ dp = -1. * HLMdesireInverse * G;
+ } catch (...) {
+ std::cout << "probleme inversion" << std::endl;
break;
}
- Warp->getParamInverse(dp,dpinv);
- Warp->pRondp(p,dpinv,p);
+ Warp->getParamInverse(dp, dpinv);
+ Warp->pRondp(p, dpinv, p);
computeEvalRMS(p);
}
- }
- else
- diverge=true;
+ } else
+ diverge = true;
iteration++;
- }
- while( (!diverge &&(evolRMS>threshold_RMS) && (iteration < iterationMax)));
+ } while ((!diverge && (evolRMS > threshold_RMS) && (iteration < iterationMax)));
- //std::cout<<"erreur "<<erreur<<std::endl;
- nbIteration=iteration;
+ // std::cout<<"erreur "<<erreur<<std::endl;
+ nbIteration = iteration;
deletePosEvalRMS();
}
@@ -387,16 +376,15 @@ void vpTemplateTrackerZNCCInverseCompositional::initPosEvalRMS(const vpColVector
Warp->computeCoeff(p);
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, p_);
+ Warp->warpX(X1, X2, p_);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
}
@@ -406,26 +394,23 @@ void vpTemplateTrackerZNCCInverseCompositional::computeEvalRMS(const vpColVector
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
Warp->computeCoeff(p_);
- evolRMS=0;
+ evolRMS = 0;
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,p_);
- Warp->warpX(X1,X2,p_);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, p_);
+ Warp->warpX(X1, X2, p_);
+ evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
+ (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
- evolRMS=evolRMS/nb_corners;
-
+ evolRMS = evolRMS / nb_corners;
}
-void vpTemplateTrackerZNCCInverseCompositional::deletePosEvalRMS()
-{
-}
+void vpTemplateTrackerZNCCInverseCompositional::deletePosEvalRMS() {}
diff --git a/modules/tracker/tt_mi/CMakeLists.txt b/modules/tracker/tt_mi/CMakeLists.txt
index c9fe4b1..17deeee 100644
--- a/modules/tracker/tt_mi/CMakeLists.txt
+++ b/modules/tracker/tt_mi/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
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 af57e21..52de5ed 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMI.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,15 +42,15 @@
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/core/vpImageFilter.h>
/*!
\class vpTemplateTrackerMI
\ingroup group_tt_mi_tracker
*/
-class VISP_EXPORT vpTemplateTrackerMI: public vpTemplateTracker
+class VISP_EXPORT vpTemplateTrackerMI : public vpTemplateTracker
{
public:
/*! Hessian approximation. */
@@ -63,20 +64,13 @@ public:
} 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;
+ vpHessienType hessianComputation;
vpHessienApproximationType ApproxHessian;
double lambda;
@@ -94,13 +88,13 @@ protected:
int influBspline;
int bspline;
- //Nombre de couleur concidere dans l'histogramme
+ // Nombre de couleur concidere dans l'histogramme
int Nc;
int Ncb;
- vpImage<double> d2Ix ;
- vpImage<double> d2Iy ;
- vpImage<double> d2Ixy ;
+ vpImage<double> d2Ix;
+ vpImage<double> d2Iy;
+ vpImage<double> d2Ixy;
double MI_preEstimation;
double MI_postEstimation;
@@ -108,65 +102,67 @@ protected:
double NMI_preEstimation;
double NMI_postEstimation;
- vpMatrix covarianceMatrix;
- bool computeCovariance;
+ vpMatrix covarianceMatrix;
+ bool computeCovariance;
protected:
- void computeGradient();
- void computeHessien(vpMatrix &H);
- void computeHessienNormalized(vpMatrix &H);
- void computeMI(double &MI);
- void computeProba(int &nbpoint);
- double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
- double getCost(const vpImage<unsigned char> &I){return getCost(I,p);}
- double getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp);
- double getNormalizedCost(const vpImage<unsigned char> &I){return getNormalizedCost(I,p);}
- virtual void initHessienDesired(const vpImage<unsigned char> &I)=0;
- virtual void trackNoPyr(const vpImage<unsigned char> &I)=0;
- void zeroProbabilities();
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpTemplateTrackerMI(const vpTemplateTrackerMI &)
-// : vpTemplateTracker(), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_0), lambda(0),
-// temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
-// dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(0), Nc(0), Ncb(0),
-// d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
-// NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpTemplateTrackerMI &operator=(const vpTemplateTrackerMI &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ void computeGradient();
+ void computeHessien(vpMatrix &H);
+ void computeHessienNormalized(vpMatrix &H);
+ void computeMI(double &MI);
+ void computeProba(int &nbpoint);
+ double getCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getCost(const vpImage<unsigned char> &I) { return getCost(I, p); }
+ double getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp);
+ double getNormalizedCost(const vpImage<unsigned char> &I) { return getNormalizedCost(I, p); }
+ virtual void initHessienDesired(const vpImage<unsigned char> &I) = 0;
+ virtual void trackNoPyr(const vpImage<unsigned char> &I) = 0;
+ void zeroProbabilities();
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpTemplateTrackerMI(const vpTemplateTrackerMI &)
+ // : vpTemplateTracker(), hessianComputation(USE_HESSIEN_NORMAL),
+ // ApproxHessian(HESSIAN_0), lambda(0),
+ // temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL),
+ // PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
+ // influBspline(0), bspline(0), Nc(0), Ncb(0), d2Ix(), d2Iy(), d2Ixy(),
+ // MI_preEstimation(0), MI_postEstimation(0), NMI_preEstimation(0),
+ // NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpTemplateTrackerMI &operator=(const vpTemplateTrackerMI &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
- //constructeur
+ // 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),
+ : vpTemplateTracker(), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_0), lambda(0), temp(NULL),
+ Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
+ influBspline(0), bspline(0), Nc(0), Ncb(0), d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
- {}
- vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp);
+ {
+ }
+ explicit vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp);
~vpTemplateTrackerMI();
vpMatrix getCovarianceMatrix() const { return covarianceMatrix; }
- double getMI() const {return MI_postEstimation;}
- double getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline,vpColVector &tp);
+ double getMI() const { return MI_postEstimation; }
+ double getMI(const vpImage<unsigned char> &I, int &nc, const int &bspline, vpColVector &tp);
double getMI256(const vpImage<unsigned char> &I, const vpColVector &tp);
- double getNMI() const {return NMI_postEstimation;}
- //initialisation du Hessien en position desiree
- void setApprocHessian(vpHessienApproximationType approx){ApproxHessian=approx;}
- void setCovarianceComputation(const bool & flag){ computeCovariance = flag; }
- void setHessianComputation(vpHessienType type){hessianComputation=type;}
+ double getNMI() const { return NMI_postEstimation; }
+ // initialisation du Hessien en position desiree
+ void setApprocHessian(vpHessienApproximationType approx) { ApproxHessian = approx; }
+ void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
+ void setHessianComputation(vpHessienType type) { hessianComputation = type; }
void setBspline(const vpBsplineType &newbs);
- void setLambda(double _l) {lambda = _l ; }
+ void setLambda(double _l) { lambda = _l; }
void setNc(int newNc);
};
#endif
-
diff --git a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h
index 907267c..3c2d1ad 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIBSpline.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -48,8 +49,8 @@
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
-#include <visp3/tt/vpTemplateTrackerHeader.h>
#include <visp3/tt/vpTemplateTrackerBSpline.h>
+#include <visp3/tt/vpTemplateTrackerHeader.h>
#include <visp3/tt_mi/vpTemplateTrackerMI.h>
@@ -58,39 +59,55 @@
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, unsigned int &NbParam, int °ree);
- static void PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam, int °ree);
- static void PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam);
- static void PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam);
- static void PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et,int Nc, double *val, unsigned int &NbParam);
- static void PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, double et,int Ncb, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc, double *val,
+ unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et, int Ncb,
+ double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc, double *val,
+ unsigned int &NbParam);
+ static void PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,
+ int Ncb, double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4(double *Prt, int cr, double er, int ct, double et, int Nc, double *val,
+ unsigned int &NbParam);
+ static void PutTotPVBspline4(double *Prt, double *dPrt, double *d2Prt, int cr, double er, int ct, double et, int Ncb,
+ double *val, unsigned int &NbParam);
// AY Optimisation
static void PutTotPVBspline3(double *Prt, double &er, double *et, unsigned int NbParam);
static void PutTotPVBspline4(double *Prt, double &er, double *et, unsigned int NbParam);
//
- static void PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam, int °ree);
- static void PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam, int °ree);
- static void PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam);
- static void PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam);
- static void PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam);
- static void PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam);
+ static void PutTotPVBsplineNoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, double *val,
+ unsigned int &NbParam, int °ree);
+ static void PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, int &Ncb,
+ double *val, unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, double *val,
+ unsigned int &NbParam);
+ static void PutTotPVBspline3NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, int &Ncb,
+ double *val, unsigned int &NbParam);
+ static void PutTotPVBspline4NoSecond(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc, double *val,
+ unsigned int &NbParam);
+ static void PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et, int &Ncb,
+ double *val, unsigned int &NbParam);
- static void PutTotPVBsplinePrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam, int °ree);
- static void PutTotPVBspline3PrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam);
- static void PutTotPVBspline4PrtTout(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam);
+ static void PutTotPVBsplinePrtTout(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
+ unsigned int &NbParam, int °ree);
+ static void PutTotPVBspline3PrtTout(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
+ unsigned int &NbParam);
+ static void PutTotPVBspline4PrtTout(double *Prt, int &cr, double &er, int &ct, double &et, int &Nc,
+ unsigned int &NbParam);
- static void PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb, unsigned int &NbParam, int °ree);
- static void PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb);
- static void PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb);
+ static void PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb, unsigned int &NbParam,
+ int °ree);
+ static void PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb);
+ static void PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb);
static double Bspline3(double diff);
- static double Bspline4i(double diff,int &interv);
+ static double Bspline4i(double diff, int &interv);
static double dBspline3(double diff);
static double dBspline4(double diff);
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 7241b5c..cfcbbc5 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIESM.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,10 +40,9 @@
#ifndef vpTemplateTrackerMIESM_hh
#define vpTemplateTrackerMIESM_hh
-
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/tt_mi/vpTemplateTrackerMI.h>
#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
@@ -51,14 +51,14 @@
\class vpTemplateTrackerMIESM
\ingroup group_tt_mi_tracker
*/
-class VISP_EXPORT vpTemplateTrackerMIESM: public vpTemplateTrackerMI
+class VISP_EXPORT vpTemplateTrackerMIESM : public vpTemplateTrackerMI
{
/*! Minimization method. */
typedef enum {
USE_NEWTON, // not used
- USE_LMA, // not used
+ USE_LMA, // not used
USE_GRADIENT,
- USE_QUASINEWTON //not used => see default equivalence
+ USE_QUASINEWTON // not used => see default equivalence
} vpMinimizationTypeMIESM;
protected:
@@ -76,30 +76,33 @@ protected:
void initHessienDesired(const vpImage<unsigned char> &I);
void trackNoPyr(const vpImage<unsigned char> &I);
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpTemplateTrackerMIESM(const vpTemplateTrackerMIESM &)
-// : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), CompoInitialised(false),
-// HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpTemplateTrackerMIESM &operator=(const vpTemplateTrackerMIESM &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpTemplateTrackerMIESM(const vpTemplateTrackerMIESM &)
+ // : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON),
+ // CompoInitialised(false),
+ // HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(),
+ // GInverse()
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpTemplateTrackerMIESM &operator=(const vpTemplateTrackerMIESM &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
//! Default constructor.
vpTemplateTrackerMIESM()
- : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), CompoInitialised(false),
- HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
- {}
- vpTemplateTrackerMIESM(vpTemplateTrackerWarp *_warp);
+ : 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 3b7acc6..e43046b 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardAdditional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,40 +42,36 @@
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/core/vpImageFilter.h>
-#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
#include <visp3/tt_mi/vpTemplateTrackerMI.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
/*!
\class vpTemplateTrackerMIForwardAdditional
\ingroup group_tt_mi_tracker
- The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a.
+ 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;
- //pour eval evolRMS
- double evolRMS;
- double *x_pos;
- double *y_pos;
- double threshold_RMS;
- //valeur pour calculer Quasi_Newton
+ // pour eval evolRMS
+ double evolRMS;
+ double *x_pos;
+ double *y_pos;
+ double threshold_RMS;
+ // valeur pour calculer Quasi_Newton
vpColVector p_prec;
vpColVector G_prec;
- vpMatrix KQuasiNewton;
+ vpMatrix KQuasiNewton;
protected:
void initHessienDesired(const vpImage<unsigned char> &I);
@@ -83,30 +80,34 @@ protected:
void computeEvalRMS(const vpColVector &p);
void initPosEvalRMS(const vpColVector &p);
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpTemplateTrackerMIForwardAdditional(const vpTemplateTrackerMIForwardAdditional &)
-// : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
-// threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpTemplateTrackerMIForwardAdditional &operator=(const vpTemplateTrackerMIForwardAdditional &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpTemplateTrackerMIForwardAdditional(const
+ // vpTemplateTrackerMIForwardAdditional &)
+ // : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0),
+ // x_pos(NULL), y_pos(NULL),
+ // threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpTemplateTrackerMIForwardAdditional &operator=(const
+ // vpTemplateTrackerMIForwardAdditional &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
//! Default constructor.
vpTemplateTrackerMIForwardAdditional()
- : vpTemplateTrackerMI(), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL),
- threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()
- {}
- vpTemplateTrackerMIForwardAdditional(vpTemplateTrackerWarp *_warp);
- void setThresholdRMS(double threshold){threshold_RMS=threshold;}
- void setMinimizationMethod(vpMinimizationTypeMIForwardAdditional method){minimizationMethod=method;}
+ : 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; }
};
#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 28bba9d..6452529 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIForwardCompositional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,9 +42,9 @@
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/tt_mi/vpTemplateTrackerMI.h>
#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
@@ -51,9 +52,10 @@
/*!
\class vpTemplateTrackerMIForwardCompositional
\ingroup group_tt_mi_tracker
- The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a.
+ 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;
@@ -64,8 +66,7 @@ protected:
void trackNoPyr(const vpImage<unsigned char> &I);
public:
- 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 2de0c86..1ad3386 100644
--- a/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h
+++ b/modules/tracker/tt_mi/include/visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,9 +42,9 @@
#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpImageFilter.h>
#include <visp3/tt/vpTemplateTracker.h>
#include <visp3/tt/vpTemplateTrackerHeader.h>
-#include <visp3/core/vpImageFilter.h>
#include <visp3/tt_mi/vpTemplateTrackerMI.h>
#include <visp3/tt_mi/vpTemplateTrackerMIBSpline.h>
@@ -51,34 +52,31 @@
/*!
\class vpTemplateTrackerMIInverseCompositional
\ingroup group_tt_mi_tracker
- The algorithm implemented in this class is described in \cite Dame12a and \cite Marchand16a.
+ 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;
- bool CompoInitialised;
- bool useTemplateSelect;//use only the strong gradient pixels to compute the Jabocian
- //pour eval evolRMS
- double evolRMS;
- double *x_pos;
- double *y_pos;
- double threshold_RMS;
- //valeur pour calculer Quasi_Newton
+ bool CompoInitialised;
+ bool useTemplateSelect; // use only the strong gradient pixels to compute
+ // the Jabocian
+ // pour eval evolRMS
+ double evolRMS;
+ double *x_pos;
+ double *y_pos;
+ double threshold_RMS;
+ // valeur pour calculer Quasi_Newton
vpColVector p_prec;
vpColVector G_prec;
- vpMatrix KQuasiNewton;
+ vpMatrix KQuasiNewton;
- //bool useAYOptim;
+ // bool useAYOptim;
public: // AY Optimisation
void initTemplateRefBspline(unsigned int ptIndex, double &et);
@@ -91,32 +89,38 @@ protected:
void computeEvalRMS(const vpColVector &p);
void initPosEvalRMS(const vpColVector &p);
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpTemplateTrackerMIInverseCompositional(const vpTemplateTrackerMIInverseCompositional &)
-// : vpTemplateTrackerMI(), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
-// evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()//, useAYOptim(false)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpTemplateTrackerMIInverseCompositional &operator=(const vpTemplateTrackerMIInverseCompositional &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpTemplateTrackerMIInverseCompositional(const
+ // vpTemplateTrackerMIInverseCompositional &)
+ // : vpTemplateTrackerMI(), minimizationMethod(USE_LMA),
+ // CompoInitialised(false), useTemplateSelect(false),
+ // evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(),
+ // G_prec(), KQuasiNewton()//, useAYOptim(false)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpTemplateTrackerMIInverseCompositional &operator=(const
+ // vpTemplateTrackerMIInverseCompositional &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
//! Default constructor.
vpTemplateTrackerMIInverseCompositional()
- : vpTemplateTrackerMI(), minimizationMethod(USE_LMA), CompoInitialised(false), useTemplateSelect(false),
- evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0), p_prec(), G_prec(), KQuasiNewton()//, useAYOptim(false)
- {}
- vpTemplateTrackerMIInverseCompositional(vpTemplateTrackerWarp *_warp);
+ : 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);
- /*! 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;}
+ /*! Use only the strong gradient pixels to compute the Jabobian. By default
+ * this feature is disabled. */
+ void setUseTemplateSelect(bool b) { useTemplateSelect = b; }
+ void setThresholdRMS(double threshold) { threshold_RMS = threshold; }
+ void setMinimizationMethod(vpMinimizationTypeMIInverseCompositional method) { minimizationMethod = method; }
};
#endif
-
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp
index db57a04..b971640 100644
--- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMI.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,235 +43,244 @@
void vpTemplateTrackerMI::setBspline(const vpBsplineType &newbs)
{
- bspline=(int)newbs;
- influBspline=bspline*bspline;
- Ncb=Nc+bspline;
- if (Pt) delete[] Pt;
- if (Pr) delete[] Pr;
- if (Prt) delete[] Prt;
- if (dPrt) delete[] dPrt;
- if (d2Prt) delete[] d2Prt;
- if (PrtD) delete[] PrtD;
- if (dPrtD) delete[] dPrtD;
- if (PrtTout) delete[] PrtTout;
-
- Pt= new double[Ncb];
- Pr= new double[Ncb];
-
- Prt= new double[Ncb*Ncb];
- dPrt= new double[Ncb*Ncb*(int)(nbParam)];
- d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
+ bspline = (int)newbs;
+ influBspline = bspline * bspline;
+ Ncb = Nc + bspline;
+ if (Pt)
+ delete[] Pt;
+ if (Pr)
+ delete[] Pr;
+ if (Prt)
+ delete[] Prt;
+ if (dPrt)
+ delete[] dPrt;
+ if (d2Prt)
+ delete[] d2Prt;
+ if (PrtD)
+ delete[] PrtD;
+ if (dPrtD)
+ delete[] dPrtD;
+ if (PrtTout)
+ delete[] PrtTout;
+
+ Pt = new double[Ncb];
+ Pr = new double[Ncb];
+
+ Prt = new double[Ncb * Ncb];
+ dPrt = new double[Ncb * Ncb * (int)(nbParam)];
+ d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)];
/*std::cout<<Nc*Nc*influBspline<<std::endl;std::cout<<Nc*Nc*nbParam*influBspline<<std::endl;*/
- PrtD= new double[Nc*Nc*influBspline];
- dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
- PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+ PrtD = new double[Nc * Nc * influBspline];
+ dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
+ PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
- hessianComputation=USE_HESSIEN_DESIRE;
+ hessianComputation = USE_HESSIEN_DESIRE;
}
-
vpTemplateTrackerMI::vpTemplateTrackerMI(vpTemplateTrackerWarp *_warp)
- : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0),
- temp(NULL), Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL),
- dprtemp(NULL), PrtD(NULL), dPrtD(NULL), influBspline(0), bspline(3), Nc(8), Ncb(0),
- d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
+ : vpTemplateTracker(_warp), hessianComputation(USE_HESSIEN_NORMAL), ApproxHessian(HESSIAN_NEW), lambda(0), temp(NULL),
+ Prt(NULL), dPrt(NULL), Pt(NULL), Pr(NULL), d2Prt(NULL), PrtTout(NULL), dprtemp(NULL), PrtD(NULL), dPrtD(NULL),
+ influBspline(0), bspline(3), Nc(8), Ncb(0), d2Ix(), d2Iy(), d2Ixy(), MI_preEstimation(0), MI_postEstimation(0),
NMI_preEstimation(0), NMI_postEstimation(0), covarianceMatrix(), computeCovariance(false)
{
- Ncb=Nc+bspline;
- influBspline=bspline*bspline;
+ Ncb = Nc + bspline;
+ influBspline = bspline * bspline;
- dW.resize(2,nbParam);
- H.resize(nbParam,nbParam);
+ dW.resize(2, nbParam);
+ H.resize(nbParam, nbParam);
G.resize(nbParam);
- Hdesire.resize(nbParam,nbParam);
- HLM.resize(nbParam,nbParam);
- HLMdesire.resize(nbParam,nbParam);
- dprtemp= new double[nbParam];
- temp= new double[nbParam];
+ Hdesire.resize(nbParam, nbParam);
+ HLM.resize(nbParam, nbParam);
+ HLMdesire.resize(nbParam, nbParam);
+ dprtemp = new double[nbParam];
+ temp = new double[nbParam];
X1.resize(2);
X2.resize(2);
- PrtD= new double[Nc*Nc*influBspline];//(r,t)
- dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
+ PrtD = new double[Nc * Nc * influBspline]; //(r,t)
+ dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
- Prt= new double[Ncb*Ncb];//(r,t)
- Pt= new double[Ncb];
- Pr= new double[Ncb];
- dPrt= new double[Ncb*Ncb*(int)(nbParam)];
- d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];
+ Prt = new double[Ncb * Ncb]; //(r,t)
+ Pt = new double[Ncb];
+ Pr = new double[Ncb];
+ dPrt = new double[Ncb * Ncb * (int)(nbParam)];
+ d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)];
- PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+ PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
- lambda=lambdaDep;
+ lambda = lambdaDep;
}
void vpTemplateTrackerMI::setNc(int nc)
{
- Nc=nc;
- Ncb=Nc+bspline;
-
- if (Pt) delete[] Pt;
- if (Pr) delete[] Pr;
- if (Prt) delete[] Prt;
- if (dPrt) delete[] dPrt;
- if (d2Prt) delete[] d2Prt;
- if (PrtD) delete[] PrtD;
- if (dPrtD) delete[] dPrtD;
- if (PrtTout) delete[] PrtTout;
-
- PrtD= new double[Nc*Nc*influBspline];//(r,t)
- dPrtD= new double[Nc*Nc*(int)(nbParam)*influBspline];
- Prt= new double[Ncb*Ncb];//(r,t)
- dPrt= new double[Ncb*Ncb*(int)(nbParam)];
- Pt= new double[Ncb];
- Pr= new double[Ncb];
- d2Prt= new double[Ncb*Ncb*(int)(nbParam*nbParam)];//(r,t)
- PrtTout= new double[Nc*Nc*influBspline*(1+(int)(nbParam+nbParam*nbParam))];
+ Nc = nc;
+ Ncb = Nc + bspline;
+
+ if (Pt)
+ delete[] Pt;
+ if (Pr)
+ delete[] Pr;
+ if (Prt)
+ delete[] Prt;
+ if (dPrt)
+ delete[] dPrt;
+ if (d2Prt)
+ delete[] d2Prt;
+ if (PrtD)
+ delete[] PrtD;
+ if (dPrtD)
+ delete[] dPrtD;
+ if (PrtTout)
+ delete[] PrtTout;
+
+ PrtD = new double[Nc * Nc * influBspline]; //(r,t)
+ dPrtD = new double[Nc * Nc * (int)(nbParam)*influBspline];
+ Prt = new double[Ncb * Ncb]; //(r,t)
+ dPrt = new double[Ncb * Ncb * (int)(nbParam)];
+ Pt = new double[Ncb];
+ Pr = new double[Ncb];
+ d2Prt = new double[Ncb * Ncb * (int)(nbParam * nbParam)]; //(r,t)
+ PrtTout = new double[Nc * Nc * influBspline * (1 + (int)(nbParam + nbParam * nbParam))];
}
-
double vpTemplateTrackerMI::getCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- double MI=0;
- int Nbpoint=0;
+ double MI = 0;
+ int Nbpoint = 0;
double IW;
- unsigned int Ncb_ = (unsigned int) Ncb;
- unsigned int Nc_ = (unsigned int) Nc;
- unsigned int influBspline_ = (unsigned int) influBspline;
+ unsigned int Ncb_ = (unsigned int)Ncb;
+ unsigned int Nc_ = (unsigned int)Nc;
+ unsigned int influBspline_ = (unsigned int)influBspline;
- memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
- memset(PrtD, 0, Nc_*Nc_*influBspline_*sizeof(double));
+ memset(Prt, 0, Ncb_ * Ncb_ * sizeof(double));
+ memset(PrtD, 0, Nc_ * Nc_ * influBspline_ * sizeof(double));
- //Warp->ComputeMAtWarp(tp);
+ // Warp->ComputeMAtWarp(tp);
Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
- double j2=X2[0];
- 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))
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
+ double j2 = X2[0];
+ 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)) {
Nbpoint++;
- double Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ double Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
- int cr=(int)((IW*(Nc-1))/255.);
- int ct=(int)((Tij*(Nc-1))/255.);
- double er=(IW*(Nc-1))/255.-cr;
- double et=((double)Tij*(Nc-1))/255.-ct;
+ int cr = (int)((IW * (Nc - 1)) / 255.);
+ int ct = (int)((Tij * (Nc - 1)) / 255.);
+ 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);
}
}
- ratioPixelIn=(double)Nbpoint/(double)templateSize;
+ ratioPixelIn = (double)Nbpoint / (double)templateSize;
- double *pt=PrtD;
- for(int r=0;r<Nc;r++)
- for(int t=0;t<Nc;t++)
- {
- for(int i=0;i<influBspline;i++)
- {
- int r2,t2;
- r2=r+i/bspline;
- t2=t+i%bspline;
- Prt[r2*Ncb+t2]+=*pt;
+ double *pt = PrtD;
+ for (int r = 0; r < Nc; r++)
+ for (int t = 0; t < Nc; t++) {
+ for (int i = 0; i < influBspline; i++) {
+ int r2, t2;
+ r2 = r + i / bspline;
+ t2 = t + i % bspline;
+ Prt[r2 * Ncb + t2] += *pt;
pt++;
}
}
- if(Nbpoint==0)
+ if (Nbpoint == 0)
return 0;
- for(unsigned int r=0;r<Ncb_;r++)
- for(unsigned int t=0;t<Ncb_;t++)
- //printf("%f ",Prt[r*Ncb+t]);
- Prt[r*Ncb_+t]=Prt[r*Ncb_+t]/Nbpoint;
- //calcul Pr;
- memset(Pr, 0, Ncb_*sizeof(double));
- for(unsigned int r=0;r<Ncb_;r++)
- {
- for(unsigned int t=0;t<Ncb_;t++)
- Pr[r]+=Prt[r*Ncb_+t];
+ for (unsigned int r = 0; r < Ncb_; r++)
+ for (unsigned int t = 0; t < Ncb_; t++)
+ // printf("%f ",Prt[r*Ncb+t]);
+ Prt[r * Ncb_ + t] = Prt[r * Ncb_ + t] / Nbpoint;
+ // calcul Pr;
+ memset(Pr, 0, Ncb_ * sizeof(double));
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ for (unsigned int t = 0; t < Ncb_; t++)
+ Pr[r] += Prt[r * Ncb_ + t];
}
- //calcul Pt;
- memset(Pt, 0, Ncb_*sizeof(double));
- for(unsigned int t=0;t<Ncb_;t++)
- {
- for(unsigned int r=0;r<Ncb_;r++)
- Pt[t]+=Prt[r*Ncb_+t];
+ // calcul Pt;
+ memset(Pt, 0, Ncb_ * sizeof(double));
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ for (unsigned int r = 0; r < Ncb_; r++)
+ Pt[t] += Prt[r * Ncb_ + t];
}
- for(unsigned int r=0;r<Ncb_;r++)
- //if(Pr[r]!=0)
- if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
- MI-=Pr[r]*log(Pr[r]);
-
- for(unsigned int t=0;t<Ncb_;t++)
- //if(Pt[t]!=0)
- if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
- MI-=Pt[t]*log(Pt[t]);
-
- for(unsigned int r=0;r<Ncb_;r++)
- for(unsigned int t=0;t<Ncb_;t++)
- //if(Prt[r*Ncb+t]!=0)
- if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
- MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+ for (unsigned int r = 0; r < Ncb_; r++)
+ // if(Pr[r]!=0)
+ if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
+ MI -= Pr[r] * log(Pr[r]);
+
+ for (unsigned int t = 0; t < Ncb_; t++)
+ // if(Pt[t]!=0)
+ if (std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
+ MI -= Pt[t] * log(Pt[t]);
+
+ for (unsigned int r = 0; r < Ncb_; r++)
+ for (unsigned int t = 0; t < Ncb_; t++)
+ // if(Prt[r*Ncb+t]!=0)
+ if (std::fabs(Prt[r * Ncb_ + t]) > std::numeric_limits<double>::epsilon())
+ MI += Prt[r * Ncb_ + t] * log(Prt[r * Ncb_ + t]);
return -MI;
}
double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- // Attention, cette version calculée de la NMI ne pourra pas atteindre le maximum de 2
- // Ceci est du au fait que par defaut, l'image est floutée dans vpTemplateTracker::initTracking()
+ // Attention, cette version calculée de la NMI ne pourra pas atteindre le
+ // maximum de 2 Ceci est du au fait que par defaut, l'image est floutée dans
+ // vpTemplateTracker::initTracking()
- double MI=0;
- double Nbpoint=0;
+ double MI = 0;
+ double Nbpoint = 0;
double IW;
double Pr_[256];
double Pt_[256];
double Prt_[256][256];
- memset(Pr_, 0, 256*sizeof(double));
- memset(Pt_, 0, 256*sizeof(double));
- memset(Prt_, 0, 256*256*sizeof(double));
+ memset(Pr_, 0, 256 * sizeof(double));
+ memset(Pt_, 0, 256 * sizeof(double));
+ memset(Prt_, 0, 256 * 256 * sizeof(double));
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
- double j2=X2[0];
- double i2=X2[1];
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
+ 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)
- IW=I[(int)i2][(int)j2];
+ double Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I[(int)i2][(int)j2];
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
Pr_[(int)Tij]++;
Pt_[(int)IW]++;
@@ -278,72 +288,79 @@ double vpTemplateTrackerMI::getNormalizedCost(const vpImage<unsigned char> &I, c
}
}
- for(int i = 0 ; i < 256 ; i++)
- {
+ for (int i = 0; i < 256; i++) {
Pr_[i] /= Nbpoint;
Pt_[i] /= Nbpoint;
- for(int j = 0 ; j < 256 ; j++)
+ for (int j = 0; j < 256; j++)
Prt_[i][j] /= Nbpoint;
}
- for(int r=0;r<256;r++)
- //if(Pr_[r]!=0)
- if(std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon())
- MI-=Pr_[r]*log(Pr_[r]);
+ for (int r = 0; r < 256; r++)
+ // if(Pr_[r]!=0)
+ if (std::fabs(Pr_[r]) > std::numeric_limits<double>::epsilon())
+ MI -= Pr_[r] * log(Pr_[r]);
- for(int t=0;t<256;t++)
- //if(Pt_[t]!=0)
- if(std::fabs(Pt_[t]) > std::numeric_limits<double>::epsilon())
- MI-=Pt_[t]*log(Pt_[t]);
+ for (int t = 0; t < 256; t++)
+ // if(Pt_[t]!=0)
+ if (std::fabs(Pt_[t]) > std::numeric_limits<double>::epsilon())
+ MI -= Pt_[t] * log(Pt_[t]);
double denom = 0;
- for(int r=0;r<256;r++)
- for(int t=0;t<256;t++)
- //if(Prt_[r][t]!=0)
- if(std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon())
- denom-=(Prt_[r][t]*log(Prt_[r][t]));
-
- //if(denom != 0)
- if(std::fabs(denom) > std::numeric_limits<double>::epsilon())
- MI = MI/denom;
- else MI = 0;
+ for (int r = 0; r < 256; r++)
+ for (int t = 0; t < 256; t++)
+ // if(Prt_[r][t]!=0)
+ if (std::fabs(Prt_[r][t]) > std::numeric_limits<double>::epsilon())
+ denom -= (Prt_[r][t] * log(Prt_[r][t]));
+
+ // if(denom != 0)
+ if (std::fabs(denom) > std::numeric_limits<double>::epsilon())
+ MI = MI / denom;
+ else
+ MI = 0;
return -MI;
}
vpTemplateTrackerMI::~vpTemplateTrackerMI()
{
- if (Pt) delete[] Pt;
- if (Pr) delete[] Pr;
- if (Prt) delete[] Prt;
- if (dPrt) delete[] dPrt;
- if (d2Prt) delete[] d2Prt;
- if (PrtD) delete[] PrtD;
- if (dPrtD) delete[] dPrtD;
- if (PrtTout) delete[] PrtTout;
- if (temp) delete[] temp;
- if (dprtemp) delete[] dprtemp;
+ if (Pt)
+ delete[] Pt;
+ if (Pr)
+ delete[] Pr;
+ if (Prt)
+ delete[] Prt;
+ if (dPrt)
+ delete[] dPrt;
+ if (d2Prt)
+ delete[] d2Prt;
+ if (PrtD)
+ delete[] PrtD;
+ if (dPrtD)
+ delete[] dPrtD;
+ if (PrtTout)
+ delete[] PrtTout;
+ if (temp)
+ delete[] temp;
+ if (dprtemp)
+ delete[] dprtemp;
}
void vpTemplateTrackerMI::computeProba(int &nbpoint)
{
- double *pt=PrtTout;
+ double *pt = PrtTout;
unsigned int Nc_ = (unsigned int)Nc;
unsigned int Ncb_ = (unsigned int)Ncb;
unsigned int bspline_ = (unsigned int)bspline;
- for(unsigned int r=0;r<Nc_;r++) {
- for(unsigned int t=0;t<Nc_;t++)
- {
- for(unsigned int r2=0;r2<bspline_;r2++) {
- for(unsigned int t2=0;t2<bspline_;t2++)
- {
- Prt[((r2+r)*Ncb_+(t2+t))]+=*pt++;
- for(unsigned int ip=0;ip<nbParam;ip++)
- {
- dPrt[((r2+r)*Ncb_+(t2+t))*nbParam+ip]+=*pt++;
- for(unsigned int it=0;it<nbParam;it++){
- d2Prt[((r2+r)*Ncb_+(t2+t))*nbParam*nbParam+ip*nbParam+it]+=*pt++;
+ for (unsigned int r = 0; r < Nc_; r++) {
+ for (unsigned int t = 0; t < Nc_; t++) {
+ for (unsigned int r2 = 0; r2 < bspline_; r2++) {
+ for (unsigned int t2 = 0; t2 < bspline_; t2++) {
+ Prt[((r2 + r) * Ncb_ + (t2 + t))] += *pt++;
+ for (unsigned int ip = 0; ip < nbParam; ip++) {
+ dPrt[((r2 + r) * Ncb_ + (t2 + t)) * nbParam + ip] += *pt++;
+ for (unsigned int it = 0; it < nbParam; it++) {
+ d2Prt[((r2 + r) * Ncb_ + (t2 + t)) * nbParam * nbParam + ip * nbParam + it] += *pt++;
}
}
}
@@ -396,19 +413,19 @@ void vpTemplateTrackerMI::computeProba(int &nbpoint)
// }
// }
- if(nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ if (nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
unsigned int indd, indd2;
indd = indd2 = 0;
- for(int i=0;i<Ncb*Ncb;i++){
- Prt[i]=Prt[i]/nbpoint;
- for(unsigned int j=0;j<nbParam;j++){
- dPrt[indd]=dPrt[indd]/nbpoint;
+ for (volatile int i = 0; i < Ncb * Ncb; i++) {
+ Prt[i] = Prt[i] / nbpoint;
+ for (unsigned int j = 0; j < nbParam; j++) {
+ dPrt[indd] = dPrt[indd] / nbpoint;
indd++;
- for(unsigned int k=0;k<nbParam;k++){
- d2Prt[indd2]=d2Prt[indd2]/nbpoint;
+ for (unsigned int k = 0; k < nbParam; k++) {
+ d2Prt[indd2] = d2Prt[indd2] / nbpoint;
indd2++;
}
}
@@ -417,82 +434,73 @@ void vpTemplateTrackerMI::computeProba(int &nbpoint)
void vpTemplateTrackerMI::computeMI(double &MI)
{
- unsigned int Ncb_ = (unsigned int) Ncb;
-
- //calcul Pr;
- memset(Pr, 0, Ncb_*sizeof(double));
- for(unsigned int r=0;r<Ncb_;r++)
- {
- for(unsigned int t=0;t<Ncb_;t++)
- Pr[r]+=Prt[r*Ncb_+t];
+ unsigned int Ncb_ = (unsigned int)Ncb;
+
+ // calcul Pr;
+ memset(Pr, 0, Ncb_ * sizeof(double));
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ for (unsigned int t = 0; t < Ncb_; t++)
+ Pr[r] += Prt[r * Ncb_ + t];
}
- //calcul Pt;
- memset(Pt, 0, Ncb_*sizeof(double));
- for(unsigned int t=0;t<Ncb_;t++)
- {
- for(unsigned int r=0;r<Ncb_;r++)
- Pt[t]+=Prt[r*Ncb_+t];
+ // calcul Pt;
+ memset(Pt, 0, Ncb_ * sizeof(double));
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ for (unsigned int r = 0; r < Ncb_; r++)
+ Pt[t] += Prt[r * Ncb_ + t];
}
- //calcul Entropies;
- //double entropieI=0;
- for(unsigned int r=0;r<Ncb_;r++)
- {
- //if(Pr[r]!=0)
- if(std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon())
- {
- //entropieI-=Pr[r]*log(Pr[r]);
- MI-=Pr[r]*log(Pr[r]);
+ // calcul Entropies;
+ // double entropieI=0;
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ // if(Pr[r]!=0)
+ if (std::fabs(Pr[r]) > std::numeric_limits<double>::epsilon()) {
+ // entropieI-=Pr[r]*log(Pr[r]);
+ MI -= Pr[r] * log(Pr[r]);
}
}
- //double entropieT=0;
- for(unsigned int t=0;t<Ncb_;t++)
- {
- //if(Pt[t]!=0)
- if(std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon())
- {
- //entropieT-=Pt[t]*log(Pt[t]);
- MI-=Pt[t]*log(Pt[t]);
+ // double entropieT=0;
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ // if(Pt[t]!=0)
+ if (std::fabs(Pt[t]) > std::numeric_limits<double>::epsilon()) {
+ // entropieT-=Pt[t]*log(Pt[t]);
+ MI -= Pt[t] * log(Pt[t]);
}
}
- for(unsigned int r=0;r<Ncb_;r++)
- for(unsigned int t=0;t<Ncb_;t++)
- //if(Prt[r*Ncb+t]!=0)
- if(std::fabs(Prt[r*Ncb_+t]) > std::numeric_limits<double>::epsilon())
- MI+=Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+ for (unsigned int r = 0; r < Ncb_; r++)
+ for (unsigned int t = 0; t < Ncb_; t++)
+ // if(Prt[r*Ncb+t]!=0)
+ if (std::fabs(Prt[r * Ncb_ + t]) > std::numeric_limits<double>::epsilon())
+ MI += Prt[r * Ncb_ + t] * log(Prt[r * Ncb_ + t]);
}
void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
{
- double seuilevitinf=1e-200;
- Hessian=0;
+ double seuilevitinf = 1e-200;
+ Hessian = 0;
double dtemp;
unsigned int Ncb_ = (unsigned int)Ncb;
- for(unsigned int t=0;t<Ncb_;t++)
- {
- //if(Pt[t]!=0)
- if(Pt[t]>seuilevitinf)
- {
- for(unsigned int r=0;r<Ncb_;r++)
- {
- //if(Prt[r*Ncb+t]!=0)
- if(Prt[r*Ncb_+t]>seuilevitinf)
- {
- for(unsigned int it=0;it<nbParam;it++)
- dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
-
- dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
- for(unsigned int it=0;it<nbParam;it++)
- for(unsigned int jt=0;jt<nbParam;jt++)
- if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_NEW)
- Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t])+d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
- else if(ApproxHessian==HESSIAN_NEW)
- Hessian[it][jt]+=d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*dtemp;
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ // if(Pt[t]!=0)
+ if (Pt[t] > seuilevitinf) {
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ // if(Prt[r*Ncb+t]!=0)
+ if (Prt[r * Ncb_ + t] > seuilevitinf) {
+ for (unsigned int it = 0; it < nbParam; it++)
+ dprtemp[it] = dPrt[(r * Ncb_ + t) * nbParam + it];
+
+ dtemp = 1. + log(Prt[r * Ncb_ + t] / Pt[t]);
+ for (unsigned int it = 0; it < nbParam; it++)
+ for (unsigned int jt = 0; jt < nbParam; jt++)
+ if (ApproxHessian != HESSIAN_NONSECOND && ApproxHessian != HESSIAN_NEW)
+ Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * (1. / Prt[r * Ncb_ + t] - 1. / Pt[t]) +
+ d2Prt[(r * Ncb_ + t) * nbParam * nbParam + it * nbParam + jt] * dtemp;
+ else if (ApproxHessian == HESSIAN_NEW)
+ Hessian[it][jt] += d2Prt[(r * Ncb_ + t) * nbParam * nbParam + it * nbParam + jt] * dtemp;
else
- Hessian[it][jt]+=dprtemp[it]*dprtemp[jt]*(1./Prt[r*Ncb_+t]-1./Pt[t]);
+ Hessian[it][jt] += dprtemp[it] * dprtemp[jt] * (1. / Prt[r * Ncb_ + t] - 1. / Pt[t]);
/*std::cout<<"Prt[r*Ncb+t]="<<Prt[r*Ncb+t]<<std::endl;
std::cout<<"Pt[t]="<<Pt[t]<<std::endl;
@@ -506,12 +514,12 @@ void vpTemplateTrackerMI::computeHessien(vpMatrix &Hessian)
void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
{
- double seuilevitinf=1e-200;
- //double dtemp;
- double u=0,v=0,B=0;
+ double seuilevitinf = 1e-200;
+ // double dtemp;
+ double u = 0, v = 0, B = 0;
double *du = new double[nbParam];
double *dv = new double[nbParam];
- double *A = new double[nbParam];
+ double *A = new double[nbParam];
double *dB = new double[nbParam];
double **d2u = new double *[nbParam];
double **d2v = new double *[nbParam];
@@ -519,58 +527,54 @@ void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
for (unsigned int i = 0; i < nbParam; i++) {
d2u[i] = new double[nbParam];
d2v[i] = new double[nbParam];
- dA[i] = new double[nbParam];
+ dA[i] = new double[nbParam];
}
- memset(du, 0, nbParam*sizeof(double));
- memset(dv, 0, nbParam*sizeof(double));
- memset(A, 0, nbParam*sizeof(double));
- memset(dB, 0, nbParam*sizeof(double));
- memset(dprtemp, 0, nbParam*sizeof(double));
-
- for(unsigned int it=0;it<nbParam;it++){
- for(unsigned int jt=0;jt<nbParam;jt++){
- memset(d2u[it], 0, nbParam*sizeof(double));
- memset(d2v[it], 0, nbParam*sizeof(double));
- memset(dA[it], 0, nbParam*sizeof(double));
+ memset(du, 0, nbParam * sizeof(double));
+ memset(dv, 0, nbParam * sizeof(double));
+ memset(A, 0, nbParam * sizeof(double));
+ memset(dB, 0, nbParam * sizeof(double));
+ memset(dprtemp, 0, nbParam * sizeof(double));
+
+ for (unsigned int it = 0; it < nbParam; it++) {
+ for (unsigned int jt = 0; jt < nbParam; jt++) {
+ memset(d2u[it], 0, nbParam * sizeof(double));
+ memset(d2v[it], 0, nbParam * sizeof(double));
+ memset(dA[it], 0, nbParam * sizeof(double));
}
}
unsigned int Ncb_ = (unsigned int)Ncb;
- for(unsigned int t=0;t<Ncb_;t++)
- {
- //if(Pt[t]!=0)
- if(Pt[t]>seuilevitinf)
- {
- for(unsigned int r=0;r<Ncb_;r++)
- {
- //if(Prt[r*Ncb+t]!=0)
- if(Prt[r*Ncb_+t]>seuilevitinf)
- {
- for(unsigned int it=0;it<nbParam;it++){
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ // if(Pt[t]!=0)
+ if (Pt[t] > seuilevitinf) {
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ // if(Prt[r*Ncb+t]!=0)
+ if (Prt[r * Ncb_ + t] > seuilevitinf) {
+ for (unsigned int it = 0; it < nbParam; it++) {
// dPxy/dt
- dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
+ dprtemp[it] = dPrt[(r * Ncb_ + t) * nbParam + it];
}
- //dtemp=1.+log(Prt[r*Ncb+t]/Pt[t]);
+ // dtemp=1.+log(Prt[r*Ncb+t]/Pt[t]);
// u = som(Pxy.logPxPy)
- u += Prt[r*Ncb_+t]*log(Pt[t]*Pr[r]);
+ u += Prt[r * Ncb_ + t] * log(Pt[t] * Pr[r]);
// v = som(Pxy.logPxy)
- v += Prt[r*Ncb_+t]*log(Prt[r*Ncb_+t]);
+ v += Prt[r * Ncb_ + t] * log(Prt[r * Ncb_ + t]);
- for(unsigned int it=0;it<nbParam;it++){
+ for (unsigned int it = 0; it < nbParam; it++) {
// u' = som dPxylog(PxPy)
- du[it] += dprtemp[it]*log(Pt[t]*Pr[r]);
+ du[it] += dprtemp[it] * log(Pt[t] * Pr[r]);
// v' = som dPxy(1+log(Pxy))
- dv[it] += dprtemp[it]*(1+log(Prt[r*Ncb_+t]));
-
+ dv[it] += dprtemp[it] * (1 + log(Prt[r * Ncb_ + t]));
}
- for(unsigned int it=0;it<nbParam;it++){
- for(unsigned int jt=0;jt<nbParam;jt++){
- d2u[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*log(Pt[t]*Pr[r])
- + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
- d2v[it][jt] += d2Prt[(r*Ncb_+t)*nbParam*nbParam+it*nbParam+jt]*(1+log(Prt[r*Ncb_+t]))
- + (1.0/Prt[r*Ncb_+t])*(dprtemp[it]*dprtemp[it]);
+ for (unsigned int it = 0; it < nbParam; it++) {
+ for (unsigned int jt = 0; jt < nbParam; jt++) {
+ d2u[it][jt] += d2Prt[(r * Ncb_ + t) * nbParam * nbParam + it * nbParam + jt] * log(Pt[t] * Pr[r]) +
+ (1.0 / Prt[r * Ncb_ + t]) * (dprtemp[it] * dprtemp[it]);
+ d2v[it][jt] +=
+ d2Prt[(r * Ncb_ + t) * nbParam * nbParam + it * nbParam + jt] * (1 + log(Prt[r * Ncb_ + t])) +
+ (1.0 / Prt[r * Ncb_ + t]) * (dprtemp[it] * dprtemp[it]);
}
}
}
@@ -578,19 +582,19 @@ void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
}
}
// B = v2
- B = (v*v);
- for(unsigned int it=0;it<nbParam;it++){
+ B = (v * v);
+ for (unsigned int it = 0; it < nbParam; it++) {
// A = u'v-uv'
A[it] = du[it] * v - u * dv[it];
// B' = 2vv'
dB[it] = 2 * v * dv[it];
// G = (u'v-v'u)/v2
// G[it] = A[it]/B;
- for(unsigned int jt=0;jt<nbParam;jt++){
+ for (unsigned int jt = 0; jt < nbParam; jt++) {
// A' = u''v-v''u
- dA[it][jt] = d2u[it][jt]*v-d2v[it][jt]*u;
+ dA[it][jt] = d2u[it][jt] * v - d2v[it][jt] * u;
// Hessian = (A'B-AB')/B2
- Hessian[it][jt] = (dA[it][jt] * B -A[it] * dB[it])/(B*B);
+ Hessian[it][jt] = (dA[it][jt] * B - A[it] * dB[it]) / (B * B);
}
}
@@ -607,39 +611,36 @@ void vpTemplateTrackerMI::computeHessienNormalized(vpMatrix &Hessian)
delete[] d2v;
delete[] dA;
- // std::cout<<"Hdes - compute Hessien\n"<<u<<"\n"<<v<<"\n"<<du[0]<<" "<<du[1]<<" "<<du[2]<<"\n"<<dv[2]<<"\n"<<d2u[2][2]<<"\n"<<d2v[2][2]<<"\n"<<H<<std::endl;
+ // std::cout<<"Hdes - compute Hessien\n"<<u<<"\n"<<v<<"\n"<<du[0]<<"
+ // "<<du[1]<<"
+ // "<<du[2]<<"\n"<<dv[2]<<"\n"<<d2u[2][2]<<"\n"<<d2v[2][2]<<"\n"<<H<<std::endl;
}
-
void vpTemplateTrackerMI::computeGradient()
{
- double seuilevitinf=1e-200;
- G=0;
+ double seuilevitinf = 1e-200;
+ G = 0;
unsigned int Ncb_ = (unsigned int)Ncb;
double dtemp;
- for(unsigned int t=0;t<Ncb_;t++)
- {
- //if(Pt[t]!=0)
- if(Pt[t]>seuilevitinf)
- {
- for(unsigned int r=0;r<Ncb_;r++)
- {
- if(Prt[r*Ncb_+t]>seuilevitinf)
- //if(Prt[r*Ncb+t]!=0)
+ for (unsigned int t = 0; t < Ncb_; t++) {
+ // if(Pt[t]!=0)
+ if (Pt[t] > seuilevitinf) {
+ for (unsigned int r = 0; r < Ncb_; r++) {
+ if (Prt[r * Ncb_ + t] > seuilevitinf)
+ // if(Prt[r*Ncb+t]!=0)
{
- for(unsigned int it=0;it<nbParam;it++)
- dprtemp[it]=dPrt[(r*Ncb_+t)*nbParam+it];
+ for (unsigned int it = 0; it < nbParam; it++)
+ dprtemp[it] = dPrt[(r * Ncb_ + t) * nbParam + it];
- dtemp=1.+log(Prt[r*Ncb_+t]/Pt[t]);
+ dtemp = 1. + log(Prt[r * Ncb_ + t] / Pt[t]);
- for(unsigned int it=0;it<nbParam;it++)
- G[it]+=dtemp*dprtemp[it];
+ for (unsigned int it = 0; it < nbParam; it++)
+ G[it] += dtemp * dprtemp[it];
}
}
}
}
-
}
void vpTemplateTrackerMI::zeroProbabilities()
{
@@ -647,82 +648,81 @@ void vpTemplateTrackerMI::zeroProbabilities()
unsigned int Nc_ = (unsigned int)Nc;
unsigned int influBspline_ = (unsigned int)influBspline;
- memset(Prt, 0, Ncb_*Ncb_*sizeof(double));
- memset(dPrt, 0, Ncb_*Ncb_*nbParam*sizeof(double));
- memset(d2Prt, 0, Ncb_*Ncb_*nbParam*nbParam*sizeof(double));
- memset(PrtTout, 0, Nc_*Nc_*influBspline_*(1+nbParam+nbParam*nbParam)*sizeof(double));
+ memset(Prt, 0, Ncb_ * Ncb_ * sizeof(double));
+ memset(dPrt, 0, Ncb_ * Ncb_ * nbParam * sizeof(double));
+ memset(d2Prt, 0, Ncb_ * Ncb_ * nbParam * nbParam * sizeof(double));
+ memset(PrtTout, 0, Nc_ * Nc_ * influBspline_ * (1 + nbParam + nbParam * nbParam) * sizeof(double));
// std::cout << Ncb*Ncb << std::endl;
// std::cout << Ncb*Ncb*nbParam << std::endl;
// std::cout << Ncb*Ncb*nbParam*nbParam << std::endl;
- // std::cout << Ncb*Ncb*influBspline*(1+nbParam+nbParam*nbParam) << std::endl;
+ // std::cout << Ncb*Ncb*influBspline*(1+nbParam+nbParam*nbParam) <<
+ // std::endl;
}
-double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const int &bspline_,vpColVector &tp)
+double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I, int &nc, const int &bspline_, vpColVector &tp)
{
- unsigned int tNcb = (unsigned int)(nc+bspline_);
- unsigned int tinfluBspline = (unsigned int)(bspline_*bspline_);
+ unsigned int tNcb = (unsigned int)(nc + bspline_);
+ unsigned int tinfluBspline = (unsigned int)(bspline_ * bspline_);
unsigned int nc_ = (unsigned int)nc;
- double *tPrtD = new double[nc_*nc_*tinfluBspline];
- double *tPrt = new double[tNcb*tNcb];
+ double *tPrtD = new double[nc_ * nc_ * tinfluBspline];
+ double *tPrt = new double[tNcb * tNcb];
double *tPr = new double[tNcb];
double *tPt = new double[tNcb];
- double MI=0;
- int Nbpoint=0;
+ double MI = 0;
+ volatile int Nbpoint = 0;
double IW;
- vpImage<double> GaussI ;
- vpImageFilter::filter(I, GaussI,fgG,taillef);
+ vpImage<double> GaussI;
+ vpImageFilter::filter(I, GaussI, fgG, taillef);
- memset(tPrt, 0, tNcb*tNcb*sizeof(double));
- memset(tPrtD, 0, nc_*nc_*tinfluBspline*sizeof(double));
+ memset(tPrt, 0, tNcb * tNcb * sizeof(double));
+ memset(tPrtD, 0, nc_ * nc_ * tinfluBspline * sizeof(double));
- //Warp->ComputeMAtWarp(tp);
+ // Warp->ComputeMAtWarp(tp);
Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
- double j2=X2[0];
- 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)
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
+ double j2 = X2[0];
+ 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) {
Nbpoint++;
- double Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ double Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=GaussI.getValue(i2,j2);
+ IW = GaussI.getValue(i2, j2);
- int cr=(int)((IW*(nc-1))/255.);
- int ct=(int)((Tij*(nc-1))/255.);
- double er=(IW*(nc-1))/255.-cr;
- double et=((double)Tij*(nc-1))/255.-ct;
+ int cr = (int)((IW * (nc - 1)) / 255.);
+ int ct = (int)((Tij * (nc - 1)) / 255.);
+ double er = (IW * (nc - 1)) / 255. - cr;
+ double et = ((double)Tij * (nc - 1)) / 255. - ct;
- //Calcul de l'histogramme joint par interpolation bilineaire (Bspline_ ordre 1)
- vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1.,bspline_);
+ // Calcul de l'histogramme joint par interpolation bilineaire (Bspline_
+ // ordre 1)
+ vpTemplateTrackerMIBSpline::PutPVBsplineD(tPrtD, cr, er, ct, et, nc, 1., bspline_);
}
}
- double *pt=tPrtD;
+ double *pt = tPrtD;
int tNcb_ = (int)tNcb;
int tinfluBspline_ = (int)tinfluBspline;
- for(int r=0;r<nc;r++)
- for(int t=0;t<nc;t++)
- {
- for(int i=0;i<tinfluBspline_;i++)
- {
- int r2,t2;
- r2=r+i/bspline_;
- t2=t+i%bspline_;
- tPrt[r2*tNcb_+t2]+=*pt;
+ for (int r = 0; r < nc; r++)
+ for (int t = 0; t < nc; t++) {
+ for (volatile int i = 0; i < tinfluBspline_; i++) {
+ int r2, t2;
+ r2 = r + i / bspline_;
+ t2 = t + i % bspline_;
+ tPrt[r2 * tNcb_ + t2] += *pt;
pt++;
}
@@ -734,44 +734,40 @@ double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const
delete[] tPr;
delete[] tPt;
- return 0;
- }
- else
- {
- for(unsigned int r=0;r<tNcb;r++)
- for(unsigned int t=0;t<tNcb;t++)
- //printf("%f ",tPrt[r*tNcb+t]);
- tPrt[r*tNcb+t]=tPrt[r*tNcb+t]/Nbpoint;
- //calcul Pr;
- memset(tPr, 0, tNcb*sizeof(double));
- for(unsigned int r=0;r<tNcb;r++)
- {
- for(unsigned int t=0;t<tNcb;t++)
- tPr[r]+=tPrt[r*tNcb+t];
+ return 0;
+ } else {
+ for (unsigned int r = 0; r < tNcb; r++)
+ for (unsigned int t = 0; t < tNcb; t++)
+ // printf("%f ",tPrt[r*tNcb+t]);
+ tPrt[r * tNcb + t] = tPrt[r * tNcb + t] / Nbpoint;
+ // calcul Pr;
+ memset(tPr, 0, tNcb * sizeof(double));
+ for (unsigned int r = 0; r < tNcb; r++) {
+ for (unsigned int t = 0; t < tNcb; t++)
+ tPr[r] += tPrt[r * tNcb + t];
}
- //calcul Pt;
- memset(tPt, 0, (size_t)(tNcb*sizeof(double)));
- for(unsigned int t=0;t<tNcb;t++)
- {
- for(unsigned int r=0;r<tNcb;r++)
- tPt[t]+=tPrt[r*tNcb+t];
+ // calcul Pt;
+ memset(tPt, 0, (size_t)(tNcb * sizeof(double)));
+ for (unsigned int t = 0; t < tNcb; t++) {
+ for (unsigned int r = 0; r < tNcb; r++)
+ tPt[t] += tPrt[r * tNcb + t];
}
- for(unsigned int r=0;r<tNcb;r++)
- //if(tPr[r]!=0)
- if(std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
- MI-=tPr[r]*log(tPr[r]);
-
- for(unsigned int t=0;t<tNcb;t++)
- //if(tPt[t]!=0)
- if(std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
- MI-=tPt[t]*log(tPt[t]);
-
- for(unsigned int r=0;r<tNcb;r++)
- for(unsigned int t=0;t<tNcb;t++)
- //if(tPrt[r*tNcb+t]!=0)
- if(std::fabs(tPrt[r*tNcb+t]) > std::numeric_limits<double>::epsilon())
- MI+=tPrt[r*tNcb+t]*log(tPrt[r*tNcb+t]);
+ for (unsigned int r = 0; r < tNcb; r++)
+ // if(tPr[r]!=0)
+ if (std::fabs(tPr[r]) > std::numeric_limits<double>::epsilon())
+ MI -= tPr[r] * log(tPr[r]);
+
+ for (unsigned int t = 0; t < tNcb; t++)
+ // if(tPt[t]!=0)
+ if (std::fabs(tPt[t]) > std::numeric_limits<double>::epsilon())
+ MI -= tPt[t] * log(tPt[t]);
+
+ for (unsigned int r = 0; r < tNcb; r++)
+ for (unsigned int t = 0; t < tNcb; t++)
+ // if(tPrt[r*tNcb+t]!=0)
+ if (std::fabs(tPrt[r * tNcb + t]) > std::numeric_limits<double>::epsilon())
+ MI += tPrt[r * tNcb + t] * log(tPrt[r * tNcb + t]);
}
delete[] tPrtD;
delete[] tPrt;
@@ -783,40 +779,42 @@ double vpTemplateTrackerMI::getMI(const vpImage<unsigned char> &I,int &nc, const
double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpColVector &tp)
{
- vpMatrix Prt256(256,256);Prt256=0;
- vpColVector Pr256(256);Pr256=0;
- vpColVector Pt256(256);Pt256=0;
+ vpMatrix Prt256(256, 256);
+ Prt256 = 0;
+ vpColVector Pr256(256);
+ Pr256 = 0;
+ vpColVector Pt256(256);
+ Pt256 = 0;
- int Nbpoint=0;
- unsigned int Tij,IW;
+ volatile int Nbpoint = 0;
+ unsigned int Tij, IW;
- vpImage<double> GaussI ;
- if(blur)
- vpImageFilter::filter(I, GaussI,fgG,taillef);
+ vpImage<double> GaussI;
+ if (blur)
+ vpImageFilter::filter(I, GaussI, fgG, taillef);
- //Warp->ComputeMAtWarp(tp);
+ // Warp->ComputeMAtWarp(tp);
Warp->computeCoeff(tp);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,tp);
- Warp->warpX(X1,X2,tp);
- double j2=X2[0];
- 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)
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+
+ Warp->computeDenom(X1, tp);
+ Warp->warpX(X1, X2, tp);
+ double j2 = X2[0];
+ 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) {
Nbpoint++;
- Tij=(unsigned int)ptTemplate[point].val;
- if(!blur)
- IW=(unsigned int)I.getValue(i2,j2);
+ Tij = (unsigned int)ptTemplate[point].val;
+ if (!blur)
+ IW = (unsigned int)I.getValue(i2, j2);
else
- IW=(unsigned int)GaussI.getValue(i2,j2);
+ IW = (unsigned int)GaussI.getValue(i2, j2);
Prt256[Tij][IW]++;
Pr256[Tij]++;
@@ -824,31 +822,27 @@ double vpTemplateTrackerMI::getMI256(const vpImage<unsigned char> &I, const vpCo
}
}
- if (Nbpoint == 0){
- throw(vpException(vpException::divideByZeroError,
- "Cannot get MI; number of points = 0"));
+ if (Nbpoint == 0) {
+ throw(vpException(vpException::divideByZeroError, "Cannot get MI; number of points = 0"));
}
- Prt256=Prt256/Nbpoint;
- Pr256=Pr256/Nbpoint;
- Pt256=Pt256/Nbpoint;
-
- double MI=0;
-
- for(unsigned int t=0;t<256;t++)
- {
- for(unsigned int r=0;r<256;r++)
- {
- //if(Prt256[r][t]!=0)
- if(std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
- MI+=Prt256[r][t]*log(Prt256[r][t]);
- }
- //if(Pt256[t]!=0)
- if(std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
- MI+=-Pt256[t]*log(Pt256[t]);
- //if(Pr256[t]!=0)
- if(std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
- MI+=-Pr256[t]*log(Pr256[t]);
+ Prt256 = Prt256 / Nbpoint;
+ Pr256 = Pr256 / Nbpoint;
+ Pt256 = Pt256 / Nbpoint;
+ double MI = 0;
+
+ for (unsigned int t = 0; t < 256; t++) {
+ for (unsigned int r = 0; r < 256; r++) {
+ // if(Prt256[r][t]!=0)
+ if (std::fabs(Prt256[r][t]) > std::numeric_limits<double>::epsilon())
+ MI += Prt256[r][t] * log(Prt256[r][t]);
+ }
+ // if(Pt256[t]!=0)
+ if (std::fabs(Pt256[t]) > std::numeric_limits<double>::epsilon())
+ MI += -Pt256[t] * log(Pt256[t]);
+ // if(Pr256[t]!=0)
+ if (std::fabs(Pr256[t]) > std::numeric_limits<double>::epsilon())
+ MI += -Pr256[t] * log(Pr256[t]);
}
return MI;
}
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp
index 93f399a..0e0d59b 100644
--- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIESM.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,144 +45,158 @@
#endif
vpTemplateTrackerMIESM::vpTemplateTrackerMIESM(vpTemplateTrackerWarp *_warp)
- : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false),
- HDirect(), HInverse(), HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
+ : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), CompoInitialised(false), HDirect(), HInverse(),
+ HdesireDirect(), HdesireInverse(), GDirect(), GInverse()
{
- useCompositionnal=false;
- useInverse=false;
- if(!Warp->isESMcompatible())
- std::cerr<<"The selected warp function is not appropriate for the ESM algorithm..."<<std::endl;
+ useCompositionnal = false;
+ useInverse = false;
+ if (!Warp->isESMcompatible())
+ std::cerr << "The selected warp function is not appropriate for the ESM "
+ "algorithm..."
+ << std::endl;
}
void vpTemplateTrackerMIESM::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse();
- std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+ std::cout << "Initialise Hessian at Desired position..." << std::endl;
- dW=0;
+ dW = 0;
- //double erreur=0;
- int Nbpoint=0;
+ // double erreur=0;
+ int Nbpoint = 0;
- double i2,j2;
- //double Tij;
- double /*IW,*/dx,dy;
- //int cr,ct;
- //double er,et;
- int i,j;
+ double i2, j2;
+ // double Tij;
+ double /*IW,*/ dx, dy;
+ // int cr,ct;
+ // double er,et;
+ int i, j;
- Nbpoint=0;
- //erreur=0;
+ Nbpoint = 0;
+ // erreur=0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
zeroProbabilities();
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- j2=X2[0];i2=X2[1];
+ 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);
-// else
-// IW=I.getValue(i2,j2);
-
- //ct=ptTemplateSupp[point].ct;
- //et=ptTemplateSupp[point].et;
- //cr=(int)((IW*(Nc-1))/255.);
- //er=((double)IW*(Nc-1))/255.-cr;
-
- // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
- // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // if(blur)
+ // IW=BI.getValue(i2,j2);
+ // else
+ // IW=I.getValue(i2,j2);
+
+ // ct=ptTemplateSupp[point].ct;
+ // et=ptTemplateSupp[point].et;
+ // cr=(int)((IW*(Nc-1))/255.);
+ // er=((double)IW*(Nc-1))/255.-cr;
+
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND)
+ // // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr,
+ // er, ct, et, Nc, ptTemplate[point].dW, nbParam,
+ // bspline);
// else
- // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr,
+ // er, ct, et, Nc, ptTemplate[point].dW, nbParam,
+ // bspline);
- // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,false);
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr,
+ // er, ct, et, Nc, ptTemplate[point].dW,
+ // nbParam,bspline,ApproxHessian,false);
}
}
-
double MI;
computeProba(Nbpoint);
computeMI(MI);
computeHessien(HdesireInverse);
- //std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;
+ // std::cout<<"HdesireInverse"<<std::endl<<HdesireInverse<<std::endl;
/////////////////////////////////////////////////////////////////////////
// DIRECT COMPO
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
- if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
- {
- vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
- vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
- vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
+ if (ApproxHessian != HESSIAN_NONSECOND && ApproxHessian != HESSIAN_0 && ApproxHessian != HESSIAN_NEW &&
+ ApproxHessian != HESSIAN_YOUCEF) {
+ vpImageFilter::getGradX(dIx, d2Ix, fgdG, taillef);
+ vpImageFilter::getGradY(dIx, d2Ixy, fgdG, taillef);
+ vpImageFilter::getGradY(dIy, d2Iy, fgdG, taillef);
}
- Nbpoint=0;
- //erreur=0;
+ Nbpoint = 0;
+ // erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- j2=X2[0];i2=X2[1];
+ 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)
+ // Tij=ptTemplate[point].val;
+ // if(!blur)
// IW=I.getValue(i2,j2);
- //else
+ // else
// IW=BI.getValue(i2,j2);
- dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- //cr=ptTemplateSupp[point].ct;
- //er=ptTemplateSupp[point].et;
- //ct=(int)((IW*(Nc-1))/255.);
- //et=((double)IW*(Nc-1))/255.-ct;
+ // cr=ptTemplateSupp[point].ct;
+ // er=ptTemplateSupp[point].et;
+ // ct=(int)((IW*(Nc-1))/255.);
+ // et=((double)IW*(Nc-1))/255.-ct;
- Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+ Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
- double *tptemp=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+ double *tptemp = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr,
+ // er, ct, et, Nc, ptTemplate[point].dW,
+ // nbParam,bspline,ApproxHessian,
+ // hessianComputation==
+ // vpTemplateTrackerMI::USE_HESSIEN_DESIRE);
+ // calcul de l'erreur
+ // erreur+=(Tij-IW)*(Tij-IW);
- // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,bspline,ApproxHessian,
- // hessianComputation== vpTemplateTrackerMI::USE_HESSIEN_DESIRE);
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
-
- // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) // cas à tester AY
- // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND) //
+ // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,cr,
+ // er, ct, et, Nc, tptemp, nbParam, bspline);
// else
- // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam, bspline);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,cr,
+ // er, ct, et, Nc, tptemp, nbParam, bspline);
- // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,false);
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er,
+ // ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,false);
delete[] tptemp;
}
@@ -190,296 +205,291 @@ void vpTemplateTrackerMIESM::initHessienDesired(const vpImage<unsigned char> &I)
computeProba(Nbpoint);
computeMI(MI);
computeHessien(HdesireDirect);
- //std::cout<<"HdesireDirect"<<std::endl<<HdesireDirect<<std::endl;
-
- lambda=lambdaDep;
+ // std::cout<<"HdesireDirect"<<std::endl<<HdesireDirect<<std::endl;
- Hdesire=HdesireDirect+HdesireInverse;
- //Hdesire=HdesireDirect;
- //Hdesire=HdesireInverse;
+ lambda = lambdaDep;
- //std::cout<<"HdesireDirect "<<HdesireDirect<<std::endl;
- //std::cout<<"HdesireInverse "<<HdesireInverse<<std::endl;
- vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
+ Hdesire = HdesireDirect + HdesireInverse;
+ // Hdesire=HdesireDirect;
+ // Hdesire=HdesireInverse;
- //std::cout<<"\tEnd initialisation..."<<std::endl;
+ // std::cout<<"HdesireDirect "<<HdesireDirect<<std::endl;
+ // std::cout<<"HdesireInverse "<<HdesireInverse<<std::endl;
+ vpMatrix::computeHLM(Hdesire, lambda, HLMdesire);
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ // std::cout<<"\tEnd initialisation..."<<std::endl;
}
void vpTemplateTrackerMIESM::initCompInverse()
-{
- //std::cout<<"Initialise precomputed value of ESM Tracker"<<std::endl;
+{
+ // std::cout<<"Initialise precomputed value of ESM Tracker"<<std::endl;
- HDirect.resize(nbParam,nbParam);
- HInverse.resize(nbParam,nbParam);
- HdesireDirect.resize(nbParam,nbParam);
- HdesireInverse.resize(nbParam,nbParam);
+ HDirect.resize(nbParam, nbParam);
+ HInverse.resize(nbParam, nbParam);
+ HdesireDirect.resize(nbParam, nbParam);
+ HdesireInverse.resize(nbParam, nbParam);
GDirect.resize(nbParam);
GInverse.resize(nbParam);
- ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
- ptTemplateCompo=new vpTemplateTrackerPointCompo[templateSize];
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
-
- ptTemplateCompo[point].dW=new double[2*nbParam];
- Warp->getdWdp0(i,j,ptTemplateCompo[point].dW);
-
- ptTemplate[point].dW=new double[nbParam];
- double dx=ptTemplate[point].dx*(Nc-1)/255.;
- double dy=ptTemplate[point].dy*(Nc-1)/255.;
- Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
-
- double Tij=ptTemplate[point].val;
- int ct=(int)((Tij*(Nc-1))/255.);
- double et=(Tij*(Nc-1))/255.-ct;
- ptTemplateSupp[point].et=et;
- ptTemplateSupp[point].ct=ct;
- ptTemplateSupp[point].Bt=new double[4];
- ptTemplateSupp[point].dBt=new double[4];
- for(char it=-1;it<=2;it++)
- {
- ptTemplateSupp[point].Bt[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
- ptTemplateSupp[point].dBt[it+1] =vpTemplateTrackerMIBSpline::dBspline4(-it+et);
+ ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
+ ptTemplateCompo = new vpTemplateTrackerPointCompo[templateSize];
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+
+ ptTemplateCompo[point].dW = new double[2 * nbParam];
+ Warp->getdWdp0(i, j, ptTemplateCompo[point].dW);
+
+ ptTemplate[point].dW = new double[nbParam];
+ double dx = ptTemplate[point].dx * (Nc - 1) / 255.;
+ double dy = ptTemplate[point].dy * (Nc - 1) / 255.;
+ Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
+
+ double Tij = ptTemplate[point].val;
+ int ct = (int)((Tij * (Nc - 1)) / 255.);
+ double et = (Tij * (Nc - 1)) / 255. - ct;
+ ptTemplateSupp[point].et = et;
+ ptTemplateSupp[point].ct = ct;
+ ptTemplateSupp[point].Bt = new double[4];
+ ptTemplateSupp[point].dBt = new double[4];
+ for (char it = -1; it <= 2; it++) {
+ ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
}
}
- CompoInitialised=true;
+ CompoInitialised = true;
}
void vpTemplateTrackerMIESM::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(!CompoInitialised)
- std::cout<<"Compositionnal tracking no initialised\nUse initCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
- dW=0;
-
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
- /* if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
+ if (!CompoInitialised)
+ std::cout << "Compositionnal tracking no initialised\nUse "
+ "initCompInverse(vpImage<unsigned char> &I) function"
+ << std::endl;
+ dW = 0;
+
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
+ /* if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 &&
+ ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
{
getGradX(dIx, d2Ix,fgdG,taillef);
getGradY(dIx, d2Ixy,fgdG,taillef);
getGradY(dIy, d2Iy,fgdG,taillef);
}*/
- //double erreur=0;
+ // double erreur=0;
int point;
- MI_preEstimation=-getCost(I,p);
+ MI_preEstimation = -getCost(I, p);
- lambda=lambdaDep;
- //double MIprec=-1000;
+ lambda = lambdaDep;
+ // double MIprec=-1000;
- double i2,j2;
- //double Tij;
- //double IW;
- //int cr,ct;
- //double er,et;
+ double i2, j2;
+ // double Tij;
+ // double IW;
+ // int cr,ct;
+ // double er,et;
vpColVector dpinv(nbParam);
- double alpha=2.;
+ double alpha = 2.;
- int i,j;
- unsigned int iteration=0;
- do
- {
- int Nbpoint=0;
- //MIprec=MI;
- double MI=0;
- //erreur=0;
+ int i, j;
+ unsigned int iteration = 0;
+ do {
+ int Nbpoint = 0;
+ // MIprec=MI;
+ double MI = 0;
+ // erreur=0;
zeroProbabilities();
/////////////////////////////////////////////////////////////////////////
// Inverse
Warp->computeCoeff(p);
- for(point=0;point<(int)templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
+ for (point = 0; point < (int)templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- j2=X2[0];i2=X2[1];
+ 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)
+ // Tij=ptTemplate[point].val;
+ // if(!blur)
// IW=I.getValue(i2,j2);
- //else
+ // else
// IW=BI.getValue(i2,j2);
- //ct=ptTemplateSupp[point].ct;
- //et=ptTemplateSupp[point].et;
- //cr=(int)((IW*(Nc-1))/255.);
- //er=((double)IW*(Nc-1))/255.-cr;
+ // ct=ptTemplateSupp[point].ct;
+ // et=ptTemplateSupp[point].et;
+ // cr=(int)((IW*(Nc-1))/255.);
+ // er=((double)IW*(Nc-1))/255.-cr;
- // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) // cas à tester AY
- // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // if(ApproxHessian==vpTemplateTrackerMI::HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ // // cas à tester AY
+ // vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout,
+ // cr, er, ct, et, Nc, ptTemplate[point].dW,
+ // nbParam, bspline);
// else
- // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout,
+ // cr, er, ct, et, Nc, ptTemplate[point].dW,
+ // nbParam, bspline);
}
}
- if(Nbpoint==0)
- {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- diverge=true;
- MI=0;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge = true;
+ MI = 0;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
- else
- {
+ } else {
computeProba(Nbpoint);
computeMI(MI);
- if(hessianComputation!= vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ if (hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
computeHessien(HInverse);
computeGradient();
- GInverse=G;
+ GInverse = G;
/////////////////////////////////////////////////////////////////////////
// DIRECT
- Nbpoint=0;
- //MIprec=MI;
- MI=0;
- //erreur=0;
+ Nbpoint = 0;
+ // MIprec=MI;
+ MI = 0;
+ // erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
- int nthreads = omp_get_num_procs() ;
- //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
+ int nthreads = omp_get_num_procs();
+ // std::cout << "file: " __FILE__ << " line: " << __LINE__ << "
+ // function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
omp_set_num_threads(nthreads);
-#pragma omp parallel for private(point, i,j,i2,j2) default(shared)
+#pragma omp parallel for private(point, i, j, i2, j2) default(shared)
#endif
- for(point=0;point<(int)templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- Warp->warpX(i,j,i2,j2,p);
- X2[0]=j2;X2[1]=i2;
-
- //Warp->computeDenom(X1,p);
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
+ for (point = 0; point < (int)templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ Warp->warpX(i, j, i2, j2, p);
+ X2[0] = j2;
+ X2[1] = i2;
+
+ // Warp->computeDenom(X1,p);
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
Nbpoint++;
- //Tij=ptTemplate[point].val;
- //Tij=Iterateurvecteur->val;
- //if(!blur)
+ // Tij=ptTemplate[point].val;
+ // Tij=Iterateurvecteur->val;
+ // if(!blur)
// IW=I.getValue(i2,j2);
- //else
+ // else
// IW=BI.getValue(i2,j2);
- double dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- double dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ double dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ double dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- //ct=(int)((IW*(Nc-1))/255.);
- //et=((double)IW*(Nc-1))/255.-ct;
- //cr=ptTemplateSupp[point].ct;
- //er=ptTemplateSupp[point].et;
+ // ct=(int)((IW*(Nc-1))/255.);
+ // et=((double)IW*(Nc-1))/255.-ct;
+ // cr=ptTemplateSupp[point].ct;
+ // er=ptTemplateSupp[point].et;
- Warp->dWarpCompo(X1,X2,p,ptTemplateCompo[point].dW,dW);
+ Warp->dWarpCompo(X1, X2, p, ptTemplateCompo[point].dW, dW);
- double *tptemp=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+ double *tptemp = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
-
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
+ // calcul de l'erreur
+ // erreur+=(Tij-IW)*(Tij-IW);
// if(bspline==3)
- // vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(PrtTout,
+ // cr, er, ct, et, Nc, tptemp, nbParam);
// else
- // vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam);
- // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr, er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(PrtTout,
+ // cr, er, ct, et, Nc, tptemp, nbParam);
+ // vpTemplateTrackerMIBSpline::computeProbabilities(PrtTout,cr,
+ // er, ct, et, Nc, tptemp, nbParam,bspline,ApproxHessian,
// hessianComputation==vpHessienType::USE_HESSIEN_DESIRE);
delete[] tptemp;
-
}
}
computeProba(Nbpoint);
computeMI(MI);
- if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ if (hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
computeHessien(HDirect);
computeGradient();
- GDirect=G;
+ GDirect = G;
- if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
- {
- H=HDirect+HInverse;
- vpMatrix::computeHLM(H,lambda,HLM);
+ if (hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE) {
+ H = HDirect + HInverse;
+ vpMatrix::computeHLM(H, lambda, HLM);
}
- G=GDirect-GInverse;
- //G=GDirect;
- //G=-GInverse;
-
- try
- {
- if(minimizationMethod==vpTemplateTrackerMIESM::USE_GRADIENT)
- dp=-gain*0.3*G;
- else
- {
- switch(hessianComputation)
- {
+ G = GDirect - GInverse;
+ // G=GDirect;
+ // G=-GInverse;
+
+ try {
+ if (minimizationMethod == vpTemplateTrackerMIESM::USE_GRADIENT)
+ dp = -gain * 0.3 * G;
+ else {
+ switch (hessianComputation) {
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
- dp=gain*HLMdesireInverse*G;
+ dp = gain * HLMdesireInverse * G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
- if(HLM.cond()>HLMdesire.cond())
- dp=gain*HLMdesireInverse*G;
+ if (HLM.cond() > HLMdesire.cond())
+ dp = gain * HLMdesireInverse * G;
else
- dp=gain*0.3*HLM.inverseByLU()*G;
+ dp = gain * 0.3 * HLM.inverseByLU() * G;
break;
default:
- dp=gain*0.3*HLM.inverseByLU()*G;
+ dp = gain * 0.3 * HLM.inverseByLU() * G;
break;
}
}
- }
- catch(vpException &e)
- {
- //std::cerr<<"probleme inversion"<<std::endl;
+ } catch (vpException &e) {
+ // std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
- if(ApproxHessian==HESSIAN_NONSECOND)
- dp=-1.*dp;
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ dp = -1. * dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,-MI,dp,alpha);
- dp=alpha*dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, -MI, dp, alpha);
+ dp = alpha * dp;
}
- p+=dp;
+ p += dp;
iteration++;
}
- }
- while( /*(MI!=MIprec) &&*/(iteration< iterationMax));
+ } while (/*(MI!=MIprec) &&*/ (iteration < iterationMax));
- MI_postEstimation=-getCost(I,p);
- if(MI_preEstimation>MI_postEstimation)
- {
+ MI_postEstimation = -getCost(I, p);
+ if (MI_preEstimation > MI_postEstimation) {
MI_postEstimation = -1;
}
- nbIteration=iteration;
+ nbIteration = iteration;
}
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp
index 48a2a6d..440e6e7 100644
--- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardAdditional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -44,309 +45,283 @@
#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()
+ : vpTemplateTrackerMI(_warp), minimizationMethod(USE_NEWTON), evolRMS(0), x_pos(NULL), y_pos(NULL), threshold_RMS(0),
+ p_prec(), G_prec(), KQuasiNewton()
{
- evolRMS = 0;
- x_pos = y_pos = NULL;
- useCompositionnal=false;
- threshold_RMS=1e-20;
- minimizationMethod=USE_NEWTON;
+ useCompositionnal = false;
+ threshold_RMS = 1e-20;
}
void vpTemplateTrackerMIForwardAdditional::initHessienDesired(const vpImage<unsigned char> &I)
{
- //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+ // std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
- dW=0;
+ dW = 0;
- int Nbpoint=0;
+ int Nbpoint = 0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
double Tij;
- double IW,dx,dy;
- int cr,ct;
- double er,et;
+ double IW, dx, dy;
+ int cr, ct;
+ double er, et;
- Nbpoint=0;
+ Nbpoint = 0;
zeroProbabilities();
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- X2[0]=j;X2[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ X2[0] = j;
+ X2[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- double j2=X2[0];
- double i2=X2[1];
+ 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)
- IW=I.getValue(i2,j2);
+ Tij = ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
- dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- ct=(int)((IW*(Nc-1))/255.);
- cr=(int)((Tij*(Nc-1))/255.);
- et=(IW*(Nc-1))/255.-ct;
- er=((double)Tij*(Nc-1))/255.-cr;
- //std::cout<<"test"<<std::endl;
- Warp->dWarp(X1,X2,p,dW);
+ ct = (int)((IW * (Nc - 1)) / 255.);
+ cr = (int)((Tij * (Nc - 1)) / 255.);
+ et = (IW * (Nc - 1)) / 255. - ct;
+ er = ((double)Tij * (Nc - 1)) / 255. - cr;
+ // std::cout<<"test"<<std::endl;
+ Warp->dWarp(X1, X2, p, dW);
- double *tptemp=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+ double *tptemp = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
- if(ApproxHessian==HESSIAN_NONSECOND)
+ if (ApproxHessian == HESSIAN_NONSECOND)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
- else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
+ else if (ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
}
}
- if(Nbpoint>0)
- {
+ if (Nbpoint > 0) {
double MI;
computeProba(Nbpoint);
computeMI(MI);
computeHessien(Hdesire);
-
// double conditionnement=GetConditionnement(Hdesire);
// std::cout<<"conditionnement : "<<conditionnement<<std::endl;
- vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
- try
- {
- HLMdesireInverse=HLMdesire.inverseByLU();
- }
- catch(vpException &e)
- {
- //std::cerr<<"probleme inversion"<<std::endl;
+ vpMatrix::computeHLM(Hdesire, lambda, HLMdesire);
+ try {
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ } catch (vpException &e) {
+ // std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
- //std::cout<<"\tEnd initialisation..."<<std::endl;
+ // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+ // std::cout<<"\tEnd initialisation..."<<std::endl;
}
-
}
void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned char> &I)
{
- dW=0;
+ dW = 0;
- //double erreur=0;
- int Nbpoint=0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ // double erreur=0;
+ int Nbpoint = 0;
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- double MI=0,MIprec=-1000;
+ double MI = 0, MIprec = -1000;
- MI_preEstimation=-getCost(I,p);
+ MI_preEstimation = -getCost(I, p);
- double alpha=2.;
+ double alpha = 2.;
- unsigned int iteration=0;
+ unsigned int iteration = 0;
initPosEvalRMS(p);
- do
- {
- if(iteration%5==0)
+ do {
+ if (iteration % 5 == 0)
initHessienDesired(I);
- Nbpoint=0;
- MIprec=MI;
- MI=0;
- //erreur=0;
+ Nbpoint = 0;
+ MIprec = MI;
+ MI = 0;
+ // erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
#ifdef VISP_HAVE_OPENMP
- int nthreads = omp_get_num_procs() ;
- //std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function: " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
+ int nthreads = omp_get_num_procs();
+ // std::cout << "file: " __FILE__ << " line: " << __LINE__ << " function:
+ // " << __FUNCTION__ << " nthread: " << nthreads << std::endl;
omp_set_num_threads(nthreads);
#pragma omp parallel for default(shared)
#endif
- for(int point=0;point<(int)templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
+ for (int point = 0; point < (int)templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- double j2=X2[0];
- double i2=X2[1];
+ 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;
- if(!blur)
- IW=I.getValue(i2,j2);
+ double Tij = ptTemplate[point].val;
+ double IW;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
- double dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- double dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ double dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ double dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- int ct=(int)((IW*(Nc-1))/255.);
- int cr=(int)((Tij*(Nc-1))/255.);
- double et=(IW*(Nc-1))/255.-ct;
- double er=((double)Tij*(Nc-1))/255.-cr;
+ int ct = (int)((IW * (Nc - 1)) / 255.);
+ int cr = (int)((Tij * (Nc - 1)) / 255.);
+ double et = (IW * (Nc - 1)) / 255. - ct;
+ double er = ((double)Tij * (Nc - 1)) / 255. - cr;
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
+ // calcul de l'erreur
+ // erreur+=(Tij-IW)*(Tij-IW);
- //Calcul de l'histogramme joint par interpolation bilinÈaire (Bspline ordre 1)
- Warp->dWarp(X1,X2,p,dW);
+ // Calcul de l'histogramme joint par interpolation bilinÈaire
+ // (Bspline ordre 1)
+ Warp->dWarp(X1, X2, p, dW);
- //double *tptemp=temp;
- double *tptemp=new double[nbParam];;
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =(dW[0][it]*dx+dW[1][it]*dy);
+ // double *tptemp=temp;
+ double *tptemp = new double[nbParam];
+ ;
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = (dW[0][it] * dx + dW[1][it] * dy);
//*tptemp++ =dW[0][it]*dIWx+dW[1][it]*dIWy;
- //std::cout<<cr<<" "<<ct<<" ; ";
- if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ // std::cout<<cr<<" "<<ct<<" ; ";
+ if (ApproxHessian == HESSIAN_NONSECOND || hessianComputation == vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
- else if(ApproxHessian==HESSIAN_0 || ApproxHessian==HESSIAN_NEW)
+ else if (ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
}
}
- if(Nbpoint==0)
- {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- diverge=true;
- MI=0;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge = true;
+ MI = 0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
- else
- {
+ } else {
computeProba(Nbpoint);
computeMI(MI);
- //std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
+ // std::cout<<iteration<<"\tMI= "<<MI<<std::endl;
computeHessien(H);
computeGradient();
- vpMatrix::computeHLM(H,lambda,HLM);
- try
- {
- switch(hessianComputation)
- {
+ vpMatrix::computeHLM(H, lambda, HLM);
+ try {
+ switch (hessianComputation) {
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
- dp=gain*HLMdesireInverse*G;
+ dp = gain * HLMdesireInverse * G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
- if(HLM.cond()>HLMdesire.cond())
- dp=gain*HLMdesireInverse*G;
+ if (HLM.cond() > HLMdesire.cond())
+ dp = gain * HLMdesireInverse * G;
else
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
default:
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
}
- }
- catch(vpException &e)
- {
- //std::cerr<<"probleme inversion"<<std::endl;
+ } catch (vpException &e) {
+ // std::cerr<<"probleme inversion"<<std::endl;
deletePosEvalRMS();
throw(e);
}
}
- switch(minimizationMethod)
- {
- case vpTemplateTrackerMIForwardAdditional::USE_LMA:
- {
+ switch (minimizationMethod) {
+ case vpTemplateTrackerMIForwardAdditional::USE_LMA: {
vpColVector p_test_LMA(nbParam);
- if(ApproxHessian==HESSIAN_NONSECOND)
- p_test_LMA=p-100000.1*dp;
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ p_test_LMA = p - 100000.1 * dp;
else
- p_test_LMA=p+1.*dp;
- MI=-getCost(I,p);
- double MI_LMA=-getCost(I,p_test_LMA);
- if(MI_LMA>MI)
- {
- p=p_test_LMA;
- lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
+ p_test_LMA = p + 1. * dp;
+ MI = -getCost(I, p);
+ double MI_LMA = -getCost(I, p_test_LMA);
+ if (MI_LMA > MI) {
+ p = p_test_LMA;
+ lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
+ } else {
+ lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
}
- else
- {
- lambda=(lambda*10.<1e6)?1e6:lambda*10.;
+ } break;
+ case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT: {
+ dp = -gain * 6.0 * G;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, -MI, dp, alpha);
+ dp = alpha * dp;
}
- }
- break;
- case vpTemplateTrackerMIForwardAdditional::USE_GRADIENT:
- {
- dp=-gain*6.0*G;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,-MI,dp,alpha);
- dp=alpha*dp;
- }
- p+=1.*dp;
+ p += 1. * dp;
break;
}
- case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON:
- {
- if(iterationGlobale!=0)
- {
- vpColVector s_quasi=p-p_prec;
- vpColVector y_quasi=G-G_prec;
- double s_scal_y=s_quasi.t()*y_quasi;
- //if(s_scal_y!=0)//BFGS
+ case vpTemplateTrackerMIForwardAdditional::USE_QUASINEWTON: {
+ if (iterationGlobale != 0) {
+ vpColVector s_quasi = p - p_prec;
+ vpColVector y_quasi = G - G_prec;
+ double s_scal_y = s_quasi.t() * y_quasi;
+ // if(s_scal_y!=0)//BFGS
// KQuasiNewton=KQuasiNewton-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y;
- //if(s_scal_y!=0)//DFP
- if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
- KQuasiNewton=KQuasiNewton+0.001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
+ // if(s_scal_y!=0)//DFP
+ if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())
+ KQuasiNewton = KQuasiNewton + 0.001 * (s_quasi * s_quasi.t() / s_scal_y -
+ KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
+ (y_quasi.t() * KQuasiNewton * y_quasi));
}
- dp=-KQuasiNewton*G;
- p_prec=p;
- G_prec=G;
- p-=1.01*dp;
- }
- break;
-
- default:
- {
- if(ApproxHessian==HESSIAN_NONSECOND)
- dp=-0.1*dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,-MI,dp,alpha);
- //std::cout<<alpha<<std::endl;
- dp=alpha*dp;
+ dp = -KQuasiNewton * G;
+ p_prec = p;
+ G_prec = G;
+ p -= 1.01 * dp;
+ } break;
+
+ default: {
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ dp = -0.1 * dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, -MI, dp, alpha);
+ // std::cout<<alpha<<std::endl;
+ dp = alpha * dp;
}
- p+=1.*dp;
+ p += 1. * dp;
break;
}
}
@@ -355,19 +330,19 @@ void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned cha
iteration++;
iterationGlobale++;
- }
- while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
- //while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
- if(Nbpoint==0) {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
+ } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
+ (iteration < iterationMax) && (evolRMS > threshold_RMS));
+ // while( (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS)
+ // );
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
}
- nbIteration=iteration;
- MI_postEstimation=-getCost(I,p);
- if(MI_preEstimation>MI_postEstimation)
- {
+ nbIteration = iteration;
+ MI_postEstimation = -getCost(I, p);
+ if (MI_preEstimation > MI_postEstimation) {
MI_postEstimation = -1;
}
deletePosEvalRMS();
@@ -376,22 +351,21 @@ void vpTemplateTrackerMIForwardAdditional::trackNoPyr(const vpImage<unsigned cha
void vpTemplateTrackerMIForwardAdditional::initPosEvalRMS(const vpColVector &pw)
{
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
- x_pos=new double[nb_corners];
- y_pos=new double[nb_corners];
+ x_pos = new double[nb_corners];
+ y_pos = new double[nb_corners];
Warp->computeCoeff(pw);
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,pw);
- Warp->warpX(X1,X2,pw);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, pw);
+ Warp->warpX(X1, X2, pw);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
}
@@ -401,23 +375,23 @@ void vpTemplateTrackerMIForwardAdditional::computeEvalRMS(const vpColVector &pw)
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
Warp->computeCoeff(pw);
- evolRMS=0;
+ evolRMS = 0;
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,pw);
- Warp->warpX(X1,X2,pw);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, pw);
+ Warp->warpX(X1, X2, pw);
+ evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
+ (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
- evolRMS=evolRMS/nb_corners;
+ evolRMS = evolRMS / nb_corners;
}
void vpTemplateTrackerMIForwardAdditional::deletePosEvalRMS()
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp
index 8b98065..452e061 100644
--- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIForwardCompositional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,100 +46,96 @@ vpTemplateTrackerMIForwardCompositional::vpTemplateTrackerMIForwardCompositional
void vpTemplateTrackerMIForwardCompositional::initCompo()
{
- 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;
- int j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[2*nbParam];
- Warp->getdWdp0(i,j,ptTemplate[point].dW);
-
- double Tij=ptTemplate[point].val;
- int ct=(int)((Tij*(Nc-1))/255.);
- double et=(Tij*(Nc-1))/255.-ct;
- ptTemplateSupp[point].et=et;
- ptTemplateSupp[point].ct=ct;
- ptTemplateSupp[point].Bt=new double[4];
- ptTemplateSupp[point].dBt=new double[4];
- for(char it=-1;it<=2;it++)
- {
- ptTemplateSupp[point].Bt[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
- ptTemplateSupp[point].dBt[it+1] =vpTemplateTrackerMIBSpline::dBspline4(-it+et);
+ 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;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->computeDenom(X1, p);
+ ptTemplate[point].dW = new double[2 * nbParam];
+ Warp->getdWdp0(i, j, ptTemplate[point].dW);
+
+ double Tij = ptTemplate[point].val;
+ int ct = (int)((Tij * (Nc - 1)) / 255.);
+ double et = (Tij * (Nc - 1)) / 255. - ct;
+ ptTemplateSupp[point].et = et;
+ ptTemplateSupp[point].ct = ct;
+ ptTemplateSupp[point].Bt = new double[4];
+ ptTemplateSupp[point].dBt = new double[4];
+ for (char it = -1; it <= 2; it++) {
+ ptTemplateSupp[point].Bt[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ ptTemplateSupp[point].dBt[it + 1] = vpTemplateTrackerMIBSpline::dBspline4(-it + et);
}
-
}
- CompoInitialised=true;
+ CompoInitialised = true;
}
void vpTemplateTrackerMIForwardCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompo();
- //std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
+ // std::cout<<"Initialise Hessian at Desired position..."<<std::endl;
- dW=0;
+ dW = 0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- //double erreur=0;
- int Nbpoint=0;
+ // double erreur=0;
+ int Nbpoint = 0;
- //double Tij;
- double IW,dx,dy;
- int cr,ct;
- double er,et;
+ // double Tij;
+ double IW, dx, dy;
+ int cr, ct;
+ double er, et;
- Nbpoint=0;
- //erreur=0;
+ Nbpoint = 0;
+ // erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;
- X1[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- double j2=X2[0];
- double i2=X2[1];
+ 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)
- IW=I.getValue(i2,j2);
+ // Tij=ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
+ IW = BI.getValue(i2, j2);
- dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- cr=ptTemplateSupp[point].ct;
- er=ptTemplateSupp[point].et;
- ct=(int)((IW*(Nc-1))/255.);
- et=((double)IW*(Nc-1))/255.-ct;
+ cr = ptTemplateSupp[point].ct;
+ er = ptTemplateSupp[point].et;
+ ct = (int)((IW * (Nc - 1)) / 255.);
+ et = ((double)IW * (Nc - 1)) / 255. - ct;
- Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+ Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
- double *tptemp=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+ double *tptemp = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
+ // 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;
}
@@ -148,169 +145,156 @@ void vpTemplateTrackerMIForwardCompositional::initHessienDesired(const vpImage<u
computeMI(MI);
computeHessien(Hdesire);
- lambda=lambdaDep;
+ lambda = lambdaDep;
- vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- //std::cout<<"Hdesire = "<<Hdesire<<std::endl;
- //std::cout<<"\tEnd initialisation..."<<std::endl;
+ vpMatrix::computeHLM(Hdesire, lambda, HLMdesire);
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ // std::cout<<"Hdesire = "<<Hdesire<<std::endl;
+ // std::cout<<"\tEnd initialisation..."<<std::endl;
}
-
void vpTemplateTrackerMIForwardCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(!CompoInitialised)
- std::cout<<"Compositionnal tracking no initialised\nUse initCompo(vpImage<unsigned char> &I) function"<<std::endl;
- dW=0;
+ if (!CompoInitialised)
+ std::cout << "Compositionnal tracking no initialised\nUse "
+ "initCompo(vpImage<unsigned char> &I) function"
+ << std::endl;
+ dW = 0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- //double erreur=0;
+ // double erreur=0;
- lambda=lambdaDep;
- double MI=0,MIprec=-1000;
+ lambda = lambdaDep;
+ double MI = 0, MIprec = -1000;
- MI_preEstimation=-getCost(I,p);
+ MI_preEstimation = -getCost(I, p);
- double i2,j2;
- //double Tij;
+ double i2, j2;
+ // double Tij;
double IW;
- //unsigned
- int cr,ct;
- double er,et;
- double dx,dy;
+ // unsigned
+ int cr, ct;
+ double er, et;
+ double dx, dy;
vpColVector dpinv(nbParam);
- double alpha=2.;
+ double alpha = 2.;
- int i,j;
- unsigned int iteration=0;
- do
- {
- int Nbpoint=0;
- MIprec=MI;
- MI=0;
- //erreur=0;
+ int i, j;
+ unsigned int iteration = 0;
+ do {
+ int Nbpoint = 0;
+ MIprec = MI;
+ MI = 0;
+ // erreur=0;
zeroProbabilities();
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- i=ptTemplate[point].y;
- j=ptTemplate[point].x;
- X1[0]=j;X1[1]=i;
- Warp->warpX(i,j,i2,j2,p);
- X2[0]=j2;X2[1]=i2;
-
- Warp->computeDenom(X1,p);
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
+ for (unsigned int point = 0; point < templateSize; point++) {
+ i = ptTemplate[point].y;
+ j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
+ Warp->warpX(i, j, i2, j2, p);
+ X2[0] = j2;
+ X2[1] = i2;
+
+ Warp->computeDenom(X1, p);
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
Nbpoint++;
- //Tij=ptTemplate[point].val;
- if(!blur)
- IW=I.getValue(i2,j2);
+ // Tij=ptTemplate[point].val;
+ if (!blur)
+ IW = I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
-
- dx=1.*dIx.getValue(i2,j2)*(Nc-1)/255.;
- dy=1.*dIy.getValue(i2,j2)*(Nc-1)/255.;
+ IW = BI.getValue(i2, j2);
- ct=(int)((IW*(Nc-1))/255.);
- et=((double)IW*(Nc-1))/255.-ct;
- cr=ptTemplateSupp[point].ct;
- er=ptTemplateSupp[point].et;
+ dx = 1. * dIx.getValue(i2, j2) * (Nc - 1) / 255.;
+ dy = 1. * dIy.getValue(i2, j2) * (Nc - 1) / 255.;
- Warp->dWarpCompo(X1,X2,p,ptTemplate[point].dW,dW);
+ ct = (int)((IW * (Nc - 1)) / 255.);
+ et = ((double)IW * (Nc - 1)) / 255. - ct;
+ cr = ptTemplateSupp[point].ct;
+ er = ptTemplateSupp[point].et;
- double *tptemp=new double[nbParam];
- for(unsigned int it=0;it<nbParam;it++)
- tptemp[it] =dW[0][it]*dx+dW[1][it]*dy;
+ Warp->dWarpCompo(X1, X2, p, ptTemplate[point].dW, dW);
+ double *tptemp = new double[nbParam];
+ for (unsigned int it = 0; it < nbParam; it++)
+ tptemp[it] = dW[0][it] * dx + dW[1][it] * dy;
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
+ // calcul de l'erreur
+ // erreur+=(Tij-IW)*(Tij-IW);
- if(ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ 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)
+ else if (ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW)
vpTemplateTrackerMIBSpline::PutTotPVBspline(PrtTout, cr, er, ct, et, Nc, tptemp, nbParam, bspline);
delete[] tptemp;
-
}
}
- if(Nbpoint==0)
- {
- //std::cout<<"plus de point dans template suivi"<<std::endl;
- diverge=true;
- MI=0;
+ if (Nbpoint == 0) {
+ // std::cout<<"plus de point dans template suivi"<<std::endl;
+ diverge = true;
+ MI = 0;
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
- else
- {
+ } else {
computeProba(Nbpoint);
computeMI(MI);
- if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
+ if (hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE)
computeHessien(H);
computeGradient();
- vpMatrix::computeHLM(H,lambda,HLM);
+ vpMatrix::computeHLM(H, lambda, HLM);
- try
- {
- switch(hessianComputation)
- {
+ try {
+ switch (hessianComputation) {
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
- dp=gain*HLMdesireInverse*G;
+ dp = gain * HLMdesireInverse * G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
- if(HLM.cond()>HLMdesire.cond())
- dp=gain*HLMdesireInverse*G;
+ if (HLM.cond() > HLMdesire.cond())
+ dp = gain * HLMdesireInverse * G;
else
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
default:
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
}
- }
- catch(vpException &e)
- {
- //std::cerr<<"probleme inversion"<<std::endl;
+ } catch (vpException &e) {
+ // std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
}
- if(ApproxHessian==HESSIAN_NONSECOND)
- dp=-0.04*dp;
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ dp = -0.04 * dp;
else
- dp=1.*dp;
+ dp = 1. * dp;
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,-MI,dp,alpha);
- dp=alpha*dp;
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, -MI, dp, alpha);
+ dp = alpha * dp;
}
- Warp->pRondp(p,dp,p);
+ Warp->pRondp(p, dp, p);
iteration++;
+ } while ((std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
+ (iteration < iterationMax));
+ // while( (MI!=MIprec) && (iteration< iterationMax) );
+ nbIteration = iteration;
- }
- while( (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) && (iteration< iterationMax) );
- //while( (MI!=MIprec) && (iteration< iterationMax) );
- nbIteration=iteration;
-
- MI_postEstimation=-getCost(I,p);
- if(MI_preEstimation>MI_postEstimation)
- {
+ MI_postEstimation = -getCost(I, p);
+ if (MI_preEstimation > MI_postEstimation) {
MI_postEstimation = -1;
}
}
-
diff --git a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp
index ed105c3..a3ba565 100644
--- a/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp
+++ b/modules/tracker/tt_mi/src/mi/vpTemplateTrackerMIInverseCompositional.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,8 +37,8 @@
* Fabien Spindler
*
*****************************************************************************/
-#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
#include <visp3/core/vpTrackingException.h>
+#include <visp3/tt_mi/vpTemplateTrackerMIInverseCompositional.h>
#include <memory>
@@ -45,12 +46,12 @@ vpTemplateTrackerMIInverseCompositional::vpTemplateTrackerMIInverseCompositional
: 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;
+ 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;
@@ -58,29 +59,28 @@ void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned in
double (*ptBspFct)(double);
double (*ptdBspFct)(double);
double (*ptd2BspFct)(double);
- if(bspline == 3){
- if(et>0.5){et=et-1;}
+ if (bspline == 3) {
+ if (et > 0.5) {
+ et = et - 1;
+ }
ptBspFct = &vpTemplateTrackerMIBSpline::Bspline3;
ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline3;
ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline3;
- }
- else{
+ } else {
ptBspFct = &vpTemplateTrackerBSpline::Bspline4;
ptdBspFct = &vpTemplateTrackerMIBSpline::dBspline4;
ptd2BspFct = &vpTemplateTrackerMIBSpline::d2Bspline4;
endIndex = 2;
}
- for(int it=-1; it<=endIndex; it++)
- {
- ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it)+et);
+ for (int it = -1; it <= endIndex; it++) {
+ ptTemplateSupp[ptIndex].BtInit[index++] = (*ptBspFct)((double)(-it) + et);
- for(unsigned int ip=0;ip<nbParam;++ip)
- {
- ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
- for(unsigned int ip2=0;ip2<nbParam;++ip2)
- {
- ptTemplateSupp[ptIndex].BtInit[index++] = (*ptd2BspFct)((double)(-it)+et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
+ for (unsigned int ip = 0; ip < nbParam; ++ip) {
+ ptTemplateSupp[ptIndex].BtInit[index++] = (*ptdBspFct)((double)(-it) + et) * ptTemplate[ptIndex].dW[ip] * (-1.0);
+ for (unsigned int ip2 = 0; ip2 < nbParam; ++ip2) {
+ ptTemplateSupp[ptIndex].BtInit[index++] =
+ (*ptd2BspFct)((double)(-it) + et) * ptTemplate[ptIndex].dW[ip] * ptTemplate[ptIndex].dW[ip2];
}
}
}
@@ -88,136 +88,134 @@ void vpTemplateTrackerMIInverseCompositional::initTemplateRefBspline(unsigned in
void vpTemplateTrackerMIInverseCompositional::initCompInverse(const vpImage<unsigned char> &I)
{
- ptTemplateSupp=new vpTemplateTrackerPointSuppMIInv[templateSize];
+ ptTemplateSupp = new vpTemplateTrackerPointSuppMIInv[templateSize];
- vpImageFilter::getGradXGauss2D(I, dIx, fgG,fgdG,taillef);
- vpImageFilter::getGradYGauss2D(I, dIy, fgG,fgdG,taillef);
+ vpImageFilter::getGradXGauss2D(I, dIx, fgG, fgdG, taillef);
+ vpImageFilter::getGradYGauss2D(I, dIy, fgG, fgdG, taillef);
- if(ApproxHessian!=HESSIAN_NONSECOND && ApproxHessian!=HESSIAN_0 && ApproxHessian!=HESSIAN_NEW && ApproxHessian!=HESSIAN_YOUCEF)
- {
- vpImageFilter::getGradX(dIx, d2Ix,fgdG,taillef);
- vpImageFilter::getGradY(dIx, d2Ixy,fgdG,taillef);
- vpImageFilter::getGradY(dIy, d2Iy,fgdG,taillef);
+ if (ApproxHessian != HESSIAN_NONSECOND && ApproxHessian != HESSIAN_0 && ApproxHessian != HESSIAN_NEW &&
+ ApproxHessian != HESSIAN_YOUCEF) {
+ vpImageFilter::getGradX(dIx, d2Ix, fgdG, taillef);
+ vpImageFilter::getGradY(dIx, d2Ixy, fgdG, taillef);
+ vpImageFilter::getGradY(dIy, d2Iy, fgdG, taillef);
}
Warp->computeCoeff(p);
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+
+ X1[0] = j;
+ X1[1] = i;
- X1[0]=j;X1[1]=i;
-
- Warp->computeDenom(X1,p);
- ptTemplate[point].dW=new double[nbParam];
+ Warp->computeDenom(X1, p);
+ ptTemplate[point].dW = new double[nbParam];
- double dx=ptTemplate[point].dx*(Nc-1)/255.;
- double dy=ptTemplate[point].dy*(Nc-1)/255.;
+ double dx = ptTemplate[point].dx * (Nc - 1) / 255.;
+ double dy = ptTemplate[point].dy * (Nc - 1) / 255.;
- Warp->getdW0(i,j,dy,dx,ptTemplate[point].dW);
- double Tij=ptTemplate[point].val;
- int ct=(int)((Tij*(Nc-1))/255.);
- double et=(Tij*(Nc-1))/255.-ct;
+ Warp->getdW0(i, j, dy, dx, ptTemplate[point].dW);
+ double Tij = ptTemplate[point].val;
+ int ct = (int)((Tij * (Nc - 1)) / 255.);
+ double et = (Tij * (Nc - 1)) / 255. - ct;
- ptTemplateSupp[point].et=et;
- ptTemplateSupp[point].ct=ct;
+ ptTemplateSupp[point].et = et;
+ ptTemplateSupp[point].ct = ct;
// ###### AY Optim
// if(useAYOptim)
- // if(ApproxHessian != HESSIAN_NONSECOND /*&& hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE*/)
+ // if(ApproxHessian != HESSIAN_NONSECOND /*&&
+ // hessianComputation !=
+ // vpTemplateTrackerMI::USE_HESSIEN_DESIRE*/)
// initTemplateRefBspline(point, et);
// ###################
}
- CompoInitialised=true;
-
+ CompoInitialised = true;
}
void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<unsigned char> &I)
{
initCompInverse(I);
- //double erreur=0;
- int Nbpoint=0;
+ // double erreur=0;
+ int Nbpoint = 0;
- //double Tij;
+ // double Tij;
double IW;
- int cr,ct;
- double er,et;
+ int cr, ct;
+ double er, et;
- Nbpoint=0;
- //erreur=0;
+ Nbpoint = 0;
+ // erreur=0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
zeroProbabilities();
Warp->computeCoeff(p);
// AY : Optim
- // unsigned int totParam = (bspline * bspline)*(1+nbParam+nbParam*nbParam);
- // unsigned int size = (1 + nbParam + nbParam*nbParam)*bspline;
- // double *ptb;
+ // unsigned int totParam = (bspline *
+ // bspline)*(1+nbParam+nbParam*nbParam); unsigned int size = (1 + nbParam
+ // + nbParam*nbParam)*bspline; double *ptb;
- for(unsigned int point=0;point<templateSize;point++)
- {
- int i=ptTemplate[point].y;
- int j=ptTemplate[point].x;
- X1[0]=j;
- X1[1]=i;
+ for (unsigned int point = 0; point < templateSize; point++) {
+ int i = ptTemplate[point].y;
+ int j = ptTemplate[point].x;
+ X1[0] = j;
+ X1[1] = i;
- Warp->computeDenom(X1,p);
- Warp->warpX(X1,X2,p);
+ Warp->computeDenom(X1, p);
+ Warp->warpX(X1, X2, p);
- double j2=X2[0];
- double i2=X2[1];
+ 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;
+ // Tij=ptTemplate[point].val;
- if(blur)
- IW=BI.getValue(i2,j2);
+ if (blur)
+ IW = BI.getValue(i2, j2);
else
- IW=I.getValue(i2,j2);
-
- ct=ptTemplateSupp[point].ct;
- et=ptTemplateSupp[point].et;
- cr=(int)((IW*(Nc-1))/255.);
- er=((double)IW*(Nc-1))/255.-cr;
-
- //calcul de l'erreur
- //erreur+=(Tij-IW)*(Tij-IW);
-
- if( ApproxHessian==HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect) )
- {
- vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam, bspline);
- }
- else if ((ApproxHessian==HESSIAN_0||ApproxHessian==HESSIAN_NEW) && (ptTemplateSelect[point] || !useTemplateSelect))
- {
- if(bspline==3){
+ IW = I.getValue(i2, j2);
+
+ ct = ptTemplateSupp[point].ct;
+ et = ptTemplateSupp[point].et;
+ cr = (int)((IW * (Nc - 1)) / 255.);
+ er = ((double)IW * (Nc - 1)) / 255. - cr;
+
+ // calcul de l'erreur
+ // erreur+=(Tij-IW)*(Tij-IW);
+
+ if (ApproxHessian == HESSIAN_NONSECOND && (ptTemplateSelect[point] || !useTemplateSelect)) {
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam,
+ bspline);
+ } else if ((ApproxHessian == HESSIAN_0 || ApproxHessian == HESSIAN_NEW) &&
+ (ptTemplateSelect[point] || !useTemplateSelect)) {
+ if (bspline == 3) {
vpTemplateTrackerMIBSpline::PutTotPVBspline3(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
// {
// if(et>0.5){ct++;}
// if(er>0.5){cr++;}
// int index = (cr*Nc+ct)*totParam;
// double *ptb = &PrtTout[index];
- // vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb, er, ptTemplateSupp[point].BtInit, size);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline3(ptb,
+ // er, ptTemplateSupp[point].BtInit, size);
// }
- }
- else{
+ } else {
vpTemplateTrackerMIBSpline::PutTotPVBspline4(PrtTout, cr, er, ct, et, Nc, ptTemplate[point].dW, nbParam);
// {
// // ################### AY : Optim
// unsigned int index = (cr*Nc+ct)*totParam;
// ptb = &PrtTout[index];
- // vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb, er, ptTemplateSupp[point].BtInit, size);
+ // vpTemplateTrackerMIBSpline::PutTotPVBspline4(ptb,
+ // er, ptTemplateSupp[point].BtInit, size);
// // ###################
// }
}
- }
- else if (ptTemplateSelect[point] || !useTemplateSelect)
- vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc,nbParam, bspline);
+ } else if (ptTemplateSelect[point] || !useTemplateSelect)
+ vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(PrtTout, cr, er, ct, et, Nc, nbParam, bspline);
}
}
@@ -226,29 +224,32 @@ void vpTemplateTrackerMIInverseCompositional::initHessienDesired(const vpImage<u
computeMI(MI);
computeHessien(Hdesire);
- lambda=lambdaDep;
+ lambda = lambdaDep;
- vpMatrix::computeHLM(Hdesire,lambda,HLMdesire);
+ vpMatrix::computeHLM(Hdesire, lambda, HLMdesire);
- HLMdesireInverse=HLMdesire.inverseByLU();
- KQuasiNewton=HLMdesireInverse;
+ HLMdesireInverse = HLMdesire.inverseByLU();
+ KQuasiNewton = HLMdesireInverse;
}
void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned char> &I)
{
- if(!CompoInitialised)
- std::cout<<"Compositionnal tracking no initialised\nUse InitCompInverse(vpImage<unsigned char> &I) function"<<std::endl;
- dW=0;
+ if (!CompoInitialised)
+ std::cout << "Compositionnal tracking no initialised\nUse "
+ "InitCompInverse(vpImage<unsigned char> &I) function"
+ << std::endl;
+ dW = 0;
- if(blur)
- vpImageFilter::filter(I, BI,fgG,taillef);
+ if (blur)
+ vpImageFilter::filter(I, BI, fgG, taillef);
- lambda=lambdaDep;
- double MI=0,MIprec=-1000;
+ lambda = lambdaDep;
+ double MI = 0, MIprec = -1000;
- vpColVector p_avant_estimation;p_avant_estimation=p;
- MI_preEstimation=-getCost(I,p);
- NMI_preEstimation=-getNormalizedCost(I,p);
+ vpColVector p_avant_estimation;
+ p_avant_estimation = p;
+ MI_preEstimation = -getCost(I, p);
+ NMI_preEstimation = -getNormalizedCost(I, p);
// std::cout << "MI avant: " << MI_preEstimation << std::endl;
// std::cout << "NMI avant: " << NMI_preEstimation << std::endl;
@@ -256,101 +257,98 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned
initPosEvalRMS(p);
vpColVector dpinv(nbParam);
- double alpha=2.;
+ double alpha = 2.;
- unsigned int iteration=0;
+ unsigned int iteration = 0;
- //unsigned int bspline_ = (unsigned int) bspline;
- //unsigned int totParam = (bspline_ * bspline_)*(1+nbParam+nbParam*nbParam);
+ // unsigned int bspline_ = (unsigned int) bspline;
+ // unsigned int totParam = (bspline_ *
+ // bspline_)*(1+nbParam+nbParam*nbParam);
- vpMatrix Hnorm(nbParam,nbParam);
+ vpMatrix Hnorm(nbParam, nbParam);
- do
- {
- int Nbpoint=0;
- MIprec=MI;
- MI=0;
+ do {
+ int Nbpoint = 0;
+ MIprec = MI;
+ MI = 0;
zeroProbabilities();
Warp->computeCoeff(p);
{
- for(int point=0;point<(int)templateSize;point++)
- {
- vpColVector x1(2),x2(2);
- double i2,j2;
+ for (int point = 0; point < (int)templateSize; point++) {
+ vpColVector x1(2), x2(2);
+ double i2, j2;
- x1[0]=(double)ptTemplate[point].x;
- x1[1]=(double)ptTemplate[point].y;
+ x1[0] = (double)ptTemplate[point].x;
+ x1[1] = (double)ptTemplate[point].y;
- Warp->computeDenom(x1,p); // A modif pour parallelisation mais ne pose pas de pb avec warp utilises dans DECSA
- Warp->warpX(x1,x2,p);
+ Warp->computeDenom(x1, p); // A modif pour parallelisation mais ne
+ // pose pas de pb avec warp utilises dans
+ // DECSA
+ Warp->warpX(x1, x2, p);
- j2=x2[0];
- i2=x2[1];
+ j2 = x2[0];
+ i2 = x2[1];
- if((i2>=0)&&(j2>=0)&&(i2<I.getHeight()-1)&&(j2<I.getWidth()-1))
- {
- //if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() == I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() && (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
+ if ((i2 >= 0) && (j2 >= 0) && (i2 < I.getHeight() - 1) && (j2 < I.getWidth() - 1)) {
+ // if(m_ptCurrentMask == NULL ||(m_ptCurrentMask->getWidth() ==
+ // I.getWidth() && m_ptCurrentMask->getHeight() == I.getHeight() &&
+ // (*m_ptCurrentMask)[(unsigned int)i2][(unsigned int)j2] > 128))
{
Nbpoint++;
double IW;
- if(!blur)
- IW=(double)I.getValue(i2,j2);
+ if (!blur)
+ IW = (double)I.getValue(i2, j2);
else
- IW=BI.getValue(i2,j2);
-
- int ct=ptTemplateSupp[point].ct;
- double et=ptTemplateSupp[point].et;
- double tmp = IW*(((double)Nc)-1.f)/255.f;
- int cr=(int)tmp;
- double er=tmp-(double)cr;
-
- if( (ApproxHessian==HESSIAN_NONSECOND||hessianComputation==vpTemplateTrackerMI::USE_HESSIEN_DESIRE) && (ptTemplateSelect[point] || !useTemplateSelect) )
- {
- vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam, bspline);
- }
- else if (ptTemplateSelect[point] || !useTemplateSelect)
- {
- if(bspline==3){
- vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
- }
- else{
- vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, ptTemplate[point].dW, nbParam);
+ IW = BI.getValue(i2, j2);
+
+ int ct = ptTemplateSupp[point].ct;
+ double et = ptTemplateSupp[point].et;
+ double tmp = IW * (((double)Nc) - 1.f) / 255.f;
+ int cr = (int)tmp;
+ double er = tmp - (double)cr;
+
+ if ((ApproxHessian == HESSIAN_NONSECOND || hessianComputation == vpTemplateTrackerMI::USE_HESSIEN_DESIRE) &&
+ (ptTemplateSelect[point] || !useTemplateSelect)) {
+ vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(Prt, dPrt, cr, er, ct, et, Ncb, ptTemplate[point].dW,
+ nbParam, bspline);
+ } else if (ptTemplateSelect[point] || !useTemplateSelect) {
+ if (bspline == 3) {
+ vpTemplateTrackerMIBSpline::PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
+ ptTemplate[point].dW, nbParam);
+ } else {
+ vpTemplateTrackerMIBSpline::PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb,
+ ptTemplate[point].dW, nbParam);
}
- }
- else{
- vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb,nbParam, bspline);
+ } else {
+ vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(Prt, cr, er, ct, et, Ncb, nbParam, bspline);
}
}
-
}
}
}
- if(Nbpoint==0)
- {
- diverge=true;
- MI=0;
+ if (Nbpoint == 0) {
+ diverge = true;
+ MI = 0;
deletePosEvalRMS();
throw(vpTrackingException(vpTrackingException::notEnoughPointError, "No points in the template"));
- }
- else
- {
+ } else {
// computeProba(Nbpoint);
unsigned int indd, indd2;
indd = indd2 = 0;
unsigned int Ncb_ = (unsigned int)Ncb;
- for(unsigned int i=0;i<Ncb_*Ncb_;i++){
- Prt[i]=Prt[i]/Nbpoint;
- for(unsigned int j=0;j<nbParam;j++){
- dPrt[indd]=dPrt[indd]/Nbpoint;
+ for (unsigned int i = 0; i < Ncb_ * Ncb_; i++) {
+ Prt[i] = Prt[i] / Nbpoint;
+ for (unsigned int j = 0; j < nbParam; j++) {
+ dPrt[indd] = dPrt[indd] / Nbpoint;
indd++;
- for(unsigned int k=0;k<nbParam;k++){
- d2Prt[indd2]=d2Prt[indd2]/Nbpoint;
+ for (unsigned int k = 0; k < nbParam; k++) {
+ d2Prt[indd2] = d2Prt[indd2] / Nbpoint;
indd2++;
}
}
@@ -358,113 +356,100 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned
computeMI(MI);
- if(hessianComputation!=vpTemplateTrackerMI::USE_HESSIEN_DESIRE){
+ if (hessianComputation != vpTemplateTrackerMI::USE_HESSIEN_DESIRE) {
computeHessienNormalized(Hnorm);
computeHessien(H);
}
computeGradient();
- vpMatrix::computeHLM(H,lambda,HLM);
+ vpMatrix::computeHLM(H, lambda, HLM);
- try
- {
- switch(hessianComputation)
- {
+ try {
+ switch (hessianComputation) {
case vpTemplateTrackerMI::USE_HESSIEN_DESIRE:
- dp=gain*HLMdesireInverse*G;
+ dp = gain * HLMdesireInverse * G;
break;
case vpTemplateTrackerMI::USE_HESSIEN_BEST_COND:
- if(HLM.cond()>HLMdesire.cond())
- dp=gain*HLMdesireInverse*G;
+ if (HLM.cond() > HLMdesire.cond())
+ dp = gain * HLMdesireInverse * G;
else
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
default:
- dp=gain*0.2*HLM.inverseByLU()*G;
+ dp = gain * 0.2 * HLM.inverseByLU() * G;
break;
}
- }
- catch(vpException &e)
- {
- //std::cerr<<"probleme inversion"<<std::endl;
+ } catch (vpException &e) {
+ // std::cerr<<"probleme inversion"<<std::endl;
throw(e);
}
}
- switch(minimizationMethod)
- {
- case vpTemplateTrackerMIInverseCompositional::USE_LMA:
- {
+ switch (minimizationMethod) {
+ case vpTemplateTrackerMIInverseCompositional::USE_LMA: {
vpColVector dp_test_LMA(nbParam);
vpColVector dpinv_test_LMA(nbParam);
vpColVector p_test_LMA(nbParam);
- if(ApproxHessian==HESSIAN_NONSECOND)
- dp_test_LMA=-100000.1*dp;
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ dp_test_LMA = -100000.1 * dp;
else
- dp_test_LMA=1.*dp;
- Warp->getParamInverse(dp_test_LMA,dpinv_test_LMA);
- Warp->pRondp(p,dpinv_test_LMA,p_test_LMA);
-
- MI=-getCost(I,p);
- double MI_LMA=-getCost(I,p_test_LMA);
- if(MI_LMA>MI)
- {
- dp=dp_test_LMA;
- lambda=(lambda/10.<1e-6)?lambda/10.:1e-6;
+ dp_test_LMA = 1. * dp;
+ Warp->getParamInverse(dp_test_LMA, dpinv_test_LMA);
+ Warp->pRondp(p, dpinv_test_LMA, p_test_LMA);
+
+ MI = -getCost(I, p);
+ double MI_LMA = -getCost(I, p_test_LMA);
+ if (MI_LMA > MI) {
+ dp = dp_test_LMA;
+ lambda = (lambda / 10. < 1e-6) ? lambda / 10. : 1e-6;
+ } else {
+ dp = 0;
+ lambda = (lambda * 10. < 1e6) ? 1e6 : lambda * 10.;
}
- else
- {
- dp=0;
- lambda=(lambda*10.<1e6)?1e6:lambda*10.;
- }
- }
- break;
+ } break;
case vpTemplateTrackerMIInverseCompositional::USE_GRADIENT:
- dp=-gain*0.3*G*20;
+ dp = -gain * 0.3 * G * 20;
break;
- case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON:
- {
- if(iterationGlobale!=0)
- {
- vpColVector s_quasi=p-p_prec;
- vpColVector y_quasi=G-G_prec;
- double s_scal_y=s_quasi.t()*y_quasi;
- //std::cout<<"mise a jour K"<<std::endl;
+ case vpTemplateTrackerMIInverseCompositional::USE_QUASINEWTON: {
+ if (iterationGlobale != 0) {
+ vpColVector s_quasi = p - p_prec;
+ vpColVector y_quasi = G - G_prec;
+ double s_scal_y = s_quasi.t() * y_quasi;
+ // std::cout<<"mise a jour K"<<std::endl;
/*if(s_scal_y!=0)//BFGS
KQuasiNewton=KQuasiNewton+0.01*(-(s_quasi*y_quasi.t()*KQuasiNewton+KQuasiNewton*y_quasi*s_quasi.t())/s_scal_y+(1.+y_quasi.t()*(KQuasiNewton*y_quasi)/s_scal_y)*s_quasi*s_quasi.t()/s_scal_y);*/
- //if(s_scal_y!=0)//DFP
- if(std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon())//DFP
+ // if(s_scal_y!=0)//DFP
+ if (std::fabs(s_scal_y) > std::numeric_limits<double>::epsilon()) // DFP
{
- KQuasiNewton=KQuasiNewton+0.0001*(s_quasi*s_quasi.t()/s_scal_y-KQuasiNewton*y_quasi*y_quasi.t()*KQuasiNewton/(y_quasi.t()*KQuasiNewton*y_quasi));
- //std::cout<<"mise a jour K"<<std::endl;
+ KQuasiNewton = KQuasiNewton + 0.0001 * (s_quasi * s_quasi.t() / s_scal_y -
+ KQuasiNewton * y_quasi * y_quasi.t() * KQuasiNewton /
+ (y_quasi.t() * KQuasiNewton * y_quasi));
+ // std::cout<<"mise a jour K"<<std::endl;
}
}
- dp=gain*KQuasiNewton*G;
- //std::cout<<KQuasiNewton<<std::endl<<std::endl;
- p_prec=p;
- G_prec=G;
- //p-=1.01*dp;
- }
- break;
-
- default:
- {
- if(useBrent)
- {
- alpha=2.;
- computeOptimalBrentGain(I,p,-MI,dp,alpha);
- dp=alpha*dp;
+ dp = gain * KQuasiNewton * G;
+ // std::cout<<KQuasiNewton<<std::endl<<std::endl;
+ p_prec = p;
+ G_prec = G;
+ // p-=1.01*dp;
+ } break;
+
+ default: {
+ if (useBrent) {
+ alpha = 2.;
+ computeOptimalBrentGain(I, p, -MI, dp, alpha);
+ dp = alpha * dp;
}
- if(ApproxHessian==HESSIAN_NONSECOND)
- dp=-1.*dp;
+ if (ApproxHessian == HESSIAN_NONSECOND)
+ dp = -1. * dp;
break;
}
}
- Warp->getParamInverse(dp,dpinv);
- Warp->pRondp(p,dpinv,p);
+ Warp->getParamInverse(dp, dpinv);
+ Warp->pRondp(p, dpinv, p);
iteration++;
iterationGlobale++;
@@ -472,48 +457,45 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned
computeEvalRMS(p);
// std::cout << p.t() << std::endl;
- }
- while( (!diverge) && (std::fabs(MI-MIprec) > std::fabs(MI)*std::numeric_limits<double>::epsilon()) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
- //while( (!diverge) && (MI!=MIprec) &&(iteration< iterationMax)&&(evolRMS>threshold_RMS) );
+ } while ((!diverge) && (std::fabs(MI - MIprec) > std::fabs(MI) * std::numeric_limits<double>::epsilon()) &&
+ (iteration < iterationMax) && (evolRMS > threshold_RMS));
+ // while( (!diverge) && (MI!=MIprec) &&(iteration<
+ // iterationMax)&&(evolRMS>threshold_RMS) );
- nbIteration=iteration;
+ nbIteration = iteration;
- if(diverge)
- {
- if(computeCovariance){
- covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ if (diverge) {
+ if (computeCovariance) {
+ covarianceMatrix = vpMatrix(Warp->getNbParam(), Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
}
deletePosEvalRMS();
- // throw(vpTrackingException(vpTrackingException::badValue, "Tracking failed")) ;
- }
- else
- {
- MI_postEstimation=-getCost(I,p);
- NMI_postEstimation=-getNormalizedCost(I,p);
+ // throw(vpTrackingException(vpTrackingException::badValue,
+ // "Tracking failed")) ;
+ } else {
+ MI_postEstimation = -getCost(I, p);
+ NMI_postEstimation = -getNormalizedCost(I, p);
// std::cout << "MI apres: " << MI_postEstimation << std::endl;
// std::cout << "NMI apres: " << NMI_postEstimation << std::endl;
- if(MI_preEstimation>MI_postEstimation)
- {
- p=p_avant_estimation;
+ if (MI_preEstimation > MI_postEstimation) {
+ p = p_avant_estimation;
MI_postEstimation = MI_preEstimation;
NMI_postEstimation = NMI_preEstimation;
- covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ covarianceMatrix = vpMatrix(Warp->getNbParam(), Warp->getNbParam());
covarianceMatrix = -1;
}
deletePosEvalRMS();
- if(computeCovariance){
- try{
+ if (computeCovariance) {
+ try {
covarianceMatrix = (-H).inverseByLU();
// covarianceMatrix = (-Hnorm).inverseByLU();
- }
- catch(...){
- covarianceMatrix = vpMatrix(Warp->getNbParam(),Warp->getNbParam());
+ } catch (...) {
+ covarianceMatrix = vpMatrix(Warp->getNbParam(), Warp->getNbParam());
covarianceMatrix = -1;
MI_postEstimation = -1;
NMI_postEstimation = -1;
@@ -526,22 +508,21 @@ void vpTemplateTrackerMIInverseCompositional::trackNoPyr(const vpImage<unsigned
void vpTemplateTrackerMIInverseCompositional::initPosEvalRMS(const vpColVector &pw)
{
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
- x_pos=new double[nb_corners];
- y_pos=new double[nb_corners];
+ x_pos = new double[nb_corners];
+ y_pos = new double[nb_corners];
Warp->computeCoeff(pw);
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,pw);
- Warp->warpX(X1,X2,p);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, pw);
+ Warp->warpX(X1, X2, p);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
}
@@ -551,23 +532,23 @@ void vpTemplateTrackerMIInverseCompositional::computeEvalRMS(const vpColVector &
unsigned int nb_corners = zoneTracked->getNbTriangle() * 3;
Warp->computeCoeff(pw);
- evolRMS=0;
+ evolRMS = 0;
vpTemplateTrackerTriangle triangle;
- for(unsigned int i=0;i<zoneTracked->getNbTriangle();i++)
- {
+ for (unsigned int i = 0; i < zoneTracked->getNbTriangle(); i++) {
zoneTracked->getTriangle(i, triangle);
- for (unsigned int j=0; j<3; j++) {
+ for (unsigned int j = 0; j < 3; j++) {
triangle.getCorner(j, X1[0], X1[1]);
- Warp->computeDenom(X1,pw);
- Warp->warpX(X1,X2,pw);
- evolRMS+=(x_pos[i*3+j]-X2[0])*(x_pos[i*3+j]-X2[0])+(y_pos[i*3+j]-X2[1])*(y_pos[i*3+j]-X2[1]);
- x_pos[i*3+j]=X2[0];
- y_pos[i*3+j]=X2[1];
+ Warp->computeDenom(X1, pw);
+ Warp->warpX(X1, X2, pw);
+ evolRMS += (x_pos[i * 3 + j] - X2[0]) * (x_pos[i * 3 + j] - X2[0]) +
+ (y_pos[i * 3 + j] - X2[1]) * (y_pos[i * 3 + j] - X2[1]);
+ x_pos[i * 3 + j] = X2[0];
+ y_pos[i * 3 + j] = X2[1];
}
}
- evolRMS=evolRMS/nb_corners;
+ evolRMS = evolRMS / nb_corners;
}
void vpTemplateTrackerMIInverseCompositional::deletePosEvalRMS()
diff --git a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp
index 72bd28f..e6063d1 100644
--- a/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp
+++ b/modules/tracker/tt_mi/src/tools/vpTemplateTrackerMIBSpline.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,85 +41,88 @@
#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)
- {
+ switch (degre) {
case 4:
- PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);break;
+ PutPVBsplineD4(Prt, cr, er, ct, et, Nc, val);
+ break;
default:
- PutPVBsplineD3(Prt, cr, er, ct, et, Nc, val);//std::cout<<"DEFAUT"<<std::endl;
+ PutPVBsplineD3(Prt, cr, er, ct, et, Nc,
+ val); // std::cout<<"DEFAUT"<<std::endl;
}
}
-void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et,int Nc, double val)
+void vpTemplateTrackerMIBSpline::PutPVBsplineD3(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
{
- int sr=0;
- int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
- double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9];
- for(int ir=-1;ir<=1;ir++)
- for(int it=-1;it<=1;it++)
- {
- *pt++ +=Bspline3(ir-er)*Bspline3(it-et)*val;
- //pt++;
+ int sr = 0;
+ int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
+ double *pt = &Prt[((cr + sr) * Nc + (ct + st)) * 9];
+ for (int ir = -1; ir <= 1; ir++)
+ for (int it = -1; it <= 1; it++) {
+ *pt++ += Bspline3(ir - er) * Bspline3(it - et) * val;
+ // pt++;
}
}
-void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et,int Nc, double val)
+void vpTemplateTrackerMIBSpline::PutPVBsplineD4(double *Prt, int cr, double er, int ct, double et, int Nc, double val)
{
double Bti[4];
- double *ptBti=&Bti[0];
- for(int it=-1;it<=2;it++)
- {
- *ptBti++ =Bspline4i(it-et,it);
- //pt++;
- }
- double *pt=&Prt[(cr*Nc+ct)*16];
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=Bspline4i(ir-er,ir);
- ptBti=&Bti[0];
- for(int it=-1;it<=2;it++)
- {
- *pt++ +=Br* *ptBti++ *val;
+ double *ptBti = &Bti[0];
+ for (int it = -1; it <= 2; it++) {
+ *ptBti++ = Bspline4i(it - et, it);
+ // pt++;
+ }
+ double *pt = &Prt[(cr * Nc + ct) * 16];
+ for (int ir = -1; ir <= 2; ir++) {
+ double Br = Bspline4i(ir - er, ir);
+ ptBti = &Bti[0];
+ for (int it = -1; it <= 2; it++) {
+ *pt++ += Br * *ptBti++ * val;
}
}
}
double vpTemplateTrackerMIBSpline::Bspline3(double diff)
{
- //double result;
- double aDiff=std::fabs(diff);
+ // double result;
+ double aDiff = std::fabs(diff);
// if(aDiff<0.5)
//// return (-(aDiff-0.5)*(aDiff-0.5)+(-aDiff+0.5)+0.5);
// return (-(aDiff * aDiff) + 0.75);
// else if(aDiff<1.5)
// return (0.5*(1.5-aDiff)*(1.5-aDiff));
- if(aDiff<1.5){
- if(aDiff<0.5)
+ if (aDiff < 1.5) {
+ if (aDiff < 0.5)
return (-(aDiff * aDiff) + 0.75);
- return (0.5*(1.5-aDiff)*(1.5-aDiff));
+ return (0.5 * (1.5 - aDiff) * (1.5 - aDiff));
}
return 0;
}
-double vpTemplateTrackerMIBSpline::Bspline4i(double diff,int &interv)
+double vpTemplateTrackerMIBSpline::Bspline4i(double diff, int &interv)
{
- switch(interv)
- {
+ switch (interv) {
case -1:
- return ((2.+diff)*(2.+diff)*(2.+diff)/6.);
+ return ((2. + diff) * (2. + diff) * (2. + diff) / 6.);
case 0:
- return (-diff*diff*diff/2.-diff*diff+4./6.);
+ return (-diff * diff * diff / 2. - diff * diff + 4. / 6.);
case 1:
- return (diff*diff*diff/2.-diff*diff+4./6.);
+ return (diff * diff * diff / 2. - diff * diff + 4. / 6.);
case 2:
- return ((2.-diff)*(2.-diff)*(2.-diff)/6.);
+ return ((2. - diff) * (2. - diff) * (2. - diff) / 6.);
default:
return 0;
}
@@ -126,12 +130,12 @@ double vpTemplateTrackerMIBSpline::Bspline4i(double diff,int &interv)
double vpTemplateTrackerMIBSpline::dBspline3(double diff)
{
- if((diff>-1.5)&&(diff<=-0.5))
- return diff+1.5;
- else if((diff>-0.5)&&(diff<=0.5))
- return -2.*diff;
- else if((diff>0.5)&&(diff<=1.5))
- return diff-1.5;
+ if ((diff > -1.5) && (diff <= -0.5))
+ return diff + 1.5;
+ else if ((diff > -0.5) && (diff <= 0.5))
+ return -2. * diff;
+ else if ((diff > 0.5) && (diff <= 1.5))
+ return diff - 1.5;
// if(fabs(diff + 1.5) <= (-0.5 + 1.5))
// return diff+1.5;
@@ -145,25 +149,25 @@ double vpTemplateTrackerMIBSpline::dBspline3(double diff)
double vpTemplateTrackerMIBSpline::dBspline4(double diff)
{
- if((diff>-2.)&&(diff<=-1.))
- return (diff+2.)*(diff+2.)/2.;
- else if((diff>-1.)&&(diff<=0.))
- return -3.*diff*diff/2.-2.*diff;
- else if((diff>0.)&&(diff<=1.))
- return 3.*diff*diff/2.-2.*diff;
- else if((diff>1.)&&(diff<=2.))
- return -(diff-2.)*(diff-2.)/2.;
+ if ((diff > -2.) && (diff <= -1.))
+ return (diff + 2.) * (diff + 2.) / 2.;
+ else if ((diff > -1.) && (diff <= 0.))
+ return -3. * diff * diff / 2. - 2. * diff;
+ else if ((diff > 0.) && (diff <= 1.))
+ return 3. * diff * diff / 2. - 2. * diff;
+ else if ((diff > 1.) && (diff <= 2.))
+ return -(diff - 2.) * (diff - 2.) / 2.;
else
return 0;
}
double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
{
- if((diff>-1.5)&&(diff<=-0.5))
+ if ((diff > -1.5) && (diff <= -0.5))
return 1.;
- else if((diff>-0.5)&&(diff<=0.5))
+ else if ((diff > -0.5) && (diff <= 0.5))
return -2.;
- else if((diff>0.5)&&(diff<=1.5))
+ else if ((diff > 0.5) && (diff <= 1.5))
return 1.;
else
return 0;
@@ -171,125 +175,132 @@ double vpTemplateTrackerMIBSpline::d2Bspline3(double diff)
double vpTemplateTrackerMIBSpline::d2Bspline4(double diff)
{
- if((diff>-2.)&&(diff<=-1.))
- return (diff+2.);
- else if((diff>-1.)&&(diff<=0.))
- return -3.*diff-2.;
- else if((diff>0.)&&(diff<=1.))
- return 3.*diff-2.;
- else if((diff>1.)&&(diff<=2.))
- return -(diff-2.);
+ if ((diff > -2.) && (diff <= -1.))
+ return (diff + 2.);
+ else if ((diff > -1.) && (diff <= 0.))
+ return -3. * diff - 2.;
+ else if ((diff > 0.) && (diff <= 1.))
+ return 3. * diff - 2.;
+ else if ((diff > 1.) && (diff <= 2.))
+ return -(diff - 2.);
else
return 0;
-
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam, int °ree)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, int cr, double &er, int ct, double &et, int Nc,
+ double *val, unsigned int &NbParam, int °ree)
{
- switch(degree)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam); break;
+ PutTotPVBspline4(Prt, cr, er, ct, et, Nc, val, NbParam);
+ break;
default:
PutTotPVBspline3(Prt, cr, er, ct, et, Nc, val, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam, int °ree)
+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)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam); break;
+ PutTotPVBspline4(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
+ break;
default:
PutTotPVBspline3(Prt, dPrt, d2Prt, cr, er, ct, et, Ncb, val, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et,int Nc, double *val, unsigned int &NbParam)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, int cr, double &er, int ct, double &et, int Nc,
+ double *val, unsigned int &NbParam)
{
- short int sr=0;
- short int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ short int sr = 0;
+ short int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3];
double dBti[3];
double d2Bti[3];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- double *ptd2Bti=&d2Bti[0];
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ double *ptd2Bti = &d2Bti[0];
- for(short int it=1;it>=-1;it--)
- {
- *ptBti++ =Bspline3(it+et);
- *ptdBti++ =dBspline3(it+et);
- *ptd2Bti++ =d2Bspline3(it+et);
+ for (short int it = 1; it >= -1; it--) {
+ *ptBti++ = Bspline3(it + et);
+ *ptdBti++ = dBspline3(it + et);
+ *ptd2Bti++ = d2Bspline3(it + et);
}
- double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+(int)(NbParam+NbParam*NbParam))];
- for(short int ir=-1;ir<=1;++ir)
- {
- double Br=Bspline3(-ir+er);
+ 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);
- for(short unsigned int it=0;it<=2;++it)
- {
+ for (short unsigned int it = 0; it <= 2; ++it) {
*pt++ += Br * (Bti[it]);
double v1 = Br * (dBti[it]);
- for(short unsigned int ip=0;ip<NbParam;++ip)
- {
- *pt++ -= v1*val[ip];
+ for (short unsigned int ip = 0; ip < NbParam; ++ip) {
+ *pt++ -= v1 * val[ip];
double v2 = Br * (d2Bti[it]) * val[ip];
- for(short unsigned int ip2=0;ip2<NbParam;++ip2)
- *pt++ += v2*val[ip2];
+ for (short unsigned int ip2 = 0; ip2 < NbParam; ++ip2)
+ *pt++ += v2 * val[ip2];
}
}
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct, double &et,int Ncb, double *val, unsigned int &NbParam)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, double *d2Prt, int cr, double &er, int ct,
+ double &et, int Ncb, double *val, unsigned int &NbParam)
{
- short int sr=0;
- short int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ short int sr = 0;
+ short int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3];
double dBti[3];
double d2Bti[3];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- double *ptd2Bti=&d2Bti[0];
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ double *ptd2Bti = &d2Bti[0];
- for(short int it=1;it>=-1;it--)
- {
- *ptBti++ =Bspline3(it+et);
- *ptdBti++ =dBspline3(it+et);
- *ptd2Bti++ =d2Bspline3(it+et);
+ for (short int it = 1; it >= -1; it--) {
+ *ptBti++ = Bspline3(it + et);
+ *ptdBti++ = dBspline3(it + et);
+ *ptd2Bti++ = d2Bspline3(it + et);
}
int NbParam_ = (int)NbParam;
- for(short int ir=-1;ir<=1;++ir)
- {
- double Br=Bspline3(-ir+er);
+ for (short int ir = -1; ir <= 1; ++ir) {
+ double Br = Bspline3(-ir + er);
short int irInd = ir + 1;
- short int ind = (cr+sr+irInd)*Ncb;
- for(short int it=0;it<=2;++it)
- {
- Prt[ind+(ct+st+it)] += Br * (Bti[it]);
+ short int ind = (cr + sr + irInd) * Ncb;
+ for (short int it = 0; it <= 2; ++it) {
+ Prt[ind + (ct + st + it)] += Br * (Bti[it]);
double v1 = Br * (dBti[it]);
- int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
- for (int ip=0;ip<NbParam_;++ip)
- {
- dPrt[ind1+ip]-= v1*val[ip];
+ int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
+ for (int ip = 0; ip < NbParam_; ++ip) {
+ dPrt[ind1 + ip] -= v1 * val[ip];
double v2 = Br * (d2Bti[it]) * val[ip];
- int ind2 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_*NbParam_+ip*NbParam_;
- for(short int ip2=0;ip2<NbParam_;++ip2)
- d2Prt[ind2+ip2] += v2*val[ip2];
+ 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];
}
}
}
@@ -297,28 +308,29 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double *dPrt, dou
void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, double *bt, unsigned int size)
{
- // double Br;
- // for(short int ir=-1;ir<=1;++ir)
- // {
- // Br=Bspline3(-ir+er);
+// double Br;
+// for(short int ir=-1;ir<=1;++ir)
+// {
+// Br=Bspline3(-ir+er);
- // for(short unsigned int it=0;it < size;++it)
- // *Prt++ += Br * bt[it];
- // }
-#define LSIZE 12
+// for(short unsigned int it=0;it < size;++it)
+// *Prt++ += Br * bt[it];
+// }
+#define LSIZE 12
double *bt0 = &bt[0];
- if(er>0.5){er=er-1.0;}
+ if (er > 0.5) {
+ er = er - 1.0;
+ }
- for(int ir=-1;ir<=1;++ir)
- {
- double Br=Bspline3(-ir+er);
+ for (int ir = -1; ir <= 1; ++ir) {
+ double Br = Bspline3(-ir + er);
const double *btend = bt0 + size;
bt = bt0;
if (size >= LSIZE) {
btend -= LSIZE - 1;
- for (; bt < btend; bt += LSIZE){
+ for (; bt < btend; bt += LSIZE) {
*Prt++ += Br * bt[0];
*Prt++ += Br * bt[1];
*Prt++ += Br * bt[2];
@@ -334,45 +346,42 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3(double *Prt, double &er, doubl
}
btend += LSIZE - 1;
}
- for (; bt < btend; *Prt++ += Br * *bt++) {};
-
+ for (; bt < btend; *Prt++ += Br * *bt++) {
+ };
}
-#undef LSIZE
+#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];
double d2Bti[4];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- double *ptd2Bti=&d2Bti[0];
- for(char it=-1;it<=2;it++)
- {
- *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
- *ptdBti++ =dBspline4(-it+et);
- *ptd2Bti++ =d2Bspline4(-it+et);
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ double *ptd2Bti = &d2Bti[0];
+ for (char it = -1; it <= 2; it++) {
+ *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ *ptdBti++ = dBspline4(-it + et);
+ *ptd2Bti++ = d2Bspline4(-it + et);
}
int NbParam_ = (int)NbParam;
- double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
- for(char ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
- ptBti=&Bti[0];
- ptdBti=&dBti[0];
- ptd2Bti=&d2Bti[0];
- for(char it=-1;it<=2;it++)
- {
- *pt++ +=Br**ptBti;
- for(short int ip=0;ip<NbParam_;ip++)
- {
- *pt++ -=Br**ptdBti*val[ip];
- for(short int ip2=0;ip2<NbParam_;ip2++)
- *pt++ +=Br**ptd2Bti*val[ip]*val[ip2];
+ 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];
+ ptdBti = &dBti[0];
+ ptd2Bti = &d2Bti[0];
+ for (char it = -1; it <= 2; it++) {
+ *pt++ += Br * *ptBti;
+ for (short int ip = 0; ip < NbParam_; ip++) {
+ *pt++ -= Br * *ptdBti * val[ip];
+ for (short int ip2 = 0; ip2 < NbParam_; ip2++)
+ *pt++ += Br * *ptd2Bti * val[ip] * val[ip2];
}
ptBti++;
ptdBti++;
@@ -381,44 +390,41 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, int cr, double er
}
}
-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];
double d2Bti[4];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- double *ptd2Bti=&d2Bti[0];
- for(char it=-1;it<=2;it++)
- {
- *ptBti++ =vpTemplateTrackerBSpline::Bspline4(-it+et);
- *ptdBti++ =dBspline4(-it+et);
- *ptd2Bti++ =d2Bspline4(-it+et);
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ double *ptd2Bti = &d2Bti[0];
+ for (char it = -1; it <= 2; it++) {
+ *ptBti++ = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ *ptdBti++ = dBspline4(-it + et);
+ *ptd2Bti++ = d2Bspline4(-it + et);
}
int NbParam_ = (int)NbParam;
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for (int ir = -1; ir <= 2; ir++) {
+ double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
int irInd = ir + 1;
- int ind = (cr+irInd)*Ncb+ct;
-
- ptBti=&Bti[0];
- ptdBti=&dBti[0];
- ptd2Bti=&d2Bti[0];
-
- for(int it=-1;it<=2;it++)
- {
- Prt[ind+it] += Br * *ptBti;
- int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
- for(int ip=0;ip<NbParam_;ip++)
- {
- dPrt[ind1+ip]-= Br**ptdBti*val[ip];
- int ind2 = ((cr+irInd)*Ncb+(ct+it))*NbParam_*NbParam_+ip*NbParam_;
- for(int ip2=0;ip2<NbParam_;ip2++)
- d2Prt[ind2+ip2] += Br**ptd2Bti*val[ip]*val[ip2];
+ int ind = (cr + irInd) * Ncb + ct;
+
+ ptBti = &Bti[0];
+ ptdBti = &dBti[0];
+ ptd2Bti = &d2Bti[0];
+
+ for (int it = -1; it <= 2; it++) {
+ Prt[ind + it] += Br * *ptBti;
+ int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
+ for (int ip = 0; ip < NbParam_; ip++) {
+ dPrt[ind1 + ip] -= Br * *ptdBti * val[ip];
+ int ind2 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_ * NbParam_ + ip * NbParam_;
+ for (int ip2 = 0; ip2 < NbParam_; ip2++)
+ d2Prt[ind2 + ip2] += Br * *ptd2Bti * val[ip] * val[ip2];
}
ptBti++;
ptdBti++;
@@ -429,18 +435,17 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double *dPrt, dou
void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, double *bt, unsigned int size)
{
-#define LSIZE 12
+#define LSIZE 12
double *bt0 = &bt[0];
- for(int ir=-1;ir<=2;++ir)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for (int ir = -1; ir <= 2; ++ir) {
+ double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
const double *btend = bt0 + size;
bt = bt0;
if (size >= LSIZE) {
btend -= LSIZE - 1;
- for (; bt < btend; bt += LSIZE){
+ for (; bt < btend; bt += LSIZE) {
*Prt++ += Br * bt[0];
*Prt++ += Br * bt[1];
*Prt++ += Br * bt[2];
@@ -456,67 +461,73 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline4(double *Prt, double &er, doubl
}
btend += LSIZE - 1;
}
- for (; bt < btend; *Prt++ += Br * *bt++) {};
-
+ for (; bt < btend; *Prt++ += Br * *bt++) {
+ };
}
-#undef LSIZE
+#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)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam); break;
+ PutTotPVBspline4NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
+ break;
default:
PutTotPVBspline3NoSecond(Prt, cr, er, ct, et, Nc, val, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBsplineNoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam, int °ree)
+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)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam); break;
+ PutTotPVBspline4NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
+ break;
default:
PutTotPVBspline3NoSecond(Prt, dPrt, cr, er, ct, et, Ncb, val, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,int &Nc, double *val, unsigned int &NbParam)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr, double &er, int &ct, double &et,
+ int &Nc, double *val, unsigned int &NbParam)
{
- int sr=0;
- int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ int sr = 0;
+ int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3];
double dBti[3];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- for(char it=-1;it<=1;it++)
- {
- *ptBti++ =Bspline3(-it+et);
- *ptdBti++ =dBspline3(-it+et);
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ for (char it = -1; it <= 1; it++) {
+ *ptBti++ = Bspline3(-it + et);
+ *ptdBti++ = dBspline3(-it + et);
}
int NbParam_ = (int)NbParam;
- double *pt=&Prt[((cr+sr)*Nc+(ct+st))*9*(1+NbParam_+NbParam_*NbParam_)];
- for(char ir=-1;ir<=1;ir++)
- {
- double Br=Bspline3(-ir+er);
- ptBti=&Bti[0];
- ptdBti=&dBti[0];
- for(char it=-1;it<=1;it++)
- {
- *pt++ +=Br**ptBti;
- for(unsigned int ip=0;ip<NbParam;ip++)
- {
- *pt++ -=Br**ptdBti*val[ip];
- pt=pt+NbParam; // Modif AY
+ 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];
+ ptdBti = &dBti[0];
+ for (char it = -1; it <= 1; it++) {
+ *pt++ += Br * *ptBti;
+ for (unsigned int ip = 0; ip < NbParam; ip++) {
+ *pt++ -= Br * *ptdBti * val[ip];
+ pt = pt + NbParam; // Modif AY
}
// pt=pt+NbParam*NbParam; // Modif AY
ptBti++;
@@ -525,42 +536,45 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, int &cr,
}
}
-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;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ int sr = 0;
+ int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3];
double dBti[3];
- double *ptBti=&Bti[0];
- double *ptdBti=&dBti[0];
- for(char it=-1;it<=1;it++)
- {
- *ptBti++ =Bspline3(-it+et);
- *ptdBti++ =dBspline3(-it+et);
+ double *ptBti = &Bti[0];
+ double *ptdBti = &dBti[0];
+ for (char it = -1; it <= 1; it++) {
+ *ptBti++ = Bspline3(-it + et);
+ *ptdBti++ = dBspline3(-it + et);
}
int NbParam_ = (int)NbParam;
- for(char ir=-1;ir<=1;ir++)
- {
- double Br=Bspline3(-ir+er);
+ for (char ir = -1; ir <= 1; ir++) {
+ double Br = Bspline3(-ir + er);
int irInd = ir + 1;
- int ind = (cr+sr+irInd)*Ncb;
-
- ptBti=&Bti[0];
- ptdBti=&dBti[0];
- for(char it=-1;it<=1;it++)
- {
- Prt[ind+(ct+st+it)] += Br**ptBti;
- int ind1 = ((cr+sr+irInd)*Ncb+(ct+st+it))*NbParam_;
- for(short int ip=0;ip<NbParam_;ip++)
- {
- dPrt[ind1+ip]-= Br**ptdBti*val[ip];
+ int ind = (cr + sr + irInd) * Ncb;
+
+ ptBti = &Bti[0];
+ ptdBti = &dBti[0];
+ for (char it = -1; it <= 1; it++) {
+ Prt[ind + (ct + st + it)] += Br * *ptBti;
+ int ind1 = ((cr + sr + irInd) * Ncb + (ct + st + it)) * NbParam_;
+ for (short int ip = 0; ip < NbParam_; ip++) {
+ dPrt[ind1 + ip] -= Br * *ptdBti * val[ip];
}
ptBti++;
ptdBti++;
@@ -568,190 +582,186 @@ void vpTemplateTrackerMIBSpline::PutTotPVBspline3NoSecond(double *Prt, double *d
}
}
-
-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};
- for(char it=-1;it<=2;it++)
- {
- Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
- dBti[it+1] = dBspline4(-it+et);
+ for (char it = -1; it <= 2; it++) {
+ Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ dBti[it + 1] = dBspline4(-it + et);
}
int NbParam_ = (int)NbParam;
- double *pt=&Prt[(cr*Nc+ct)*16*(1+NbParam_+NbParam_*NbParam_)];
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
- for(int it=0;it<=3;it++)
- {
- (*pt++) +=Br*Bti[it];
-
- for(int ip=0;ip<NbParam_;ip++)
- {
- (*pt++) -=Br*dBti[it]*val[ip];
- pt=pt+NbParam_; // Modif AY
+ 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++) {
+ (*pt++) += Br * Bti[it];
+
+ for (int ip = 0; ip < NbParam_; ip++) {
+ (*pt++) -= Br * dBti[it] * val[ip];
+ pt = pt + NbParam_; // Modif AY
}
// pt=pt+NbParam*NbParam; // Modif AY
}
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline4NoSecond(double *Prt, double *dPrt, int &cr, double &er, int &ct, double &et,int &Ncb, double *val, unsigned int &NbParam)
+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};
- for(char it=-1;it<=2;it++)
- {
- Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
- dBti[it+1] = dBspline4(-it+et);
+ for (char it = -1; it <= 2; it++) {
+ Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
+ dBti[it + 1] = dBspline4(-it + et);
}
int NbParam_ = (int)NbParam;
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for (int ir = -1; ir <= 2; ir++) {
+ double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
int irInd = ir + 1;
- int ind = (cr+irInd)*Ncb+ct;
+ int ind = (cr + irInd) * Ncb + ct;
- for(int it=0;it<=3;it++)
- {
- Prt[ind+it] += Br*Bti[it];
- int ind1 = ((cr+irInd)*Ncb+(ct+it))*NbParam_;
+ for (int it = 0; it <= 3; it++) {
+ Prt[ind + it] += Br * Bti[it];
+ int ind1 = ((cr + irInd) * Ncb + (ct + it)) * NbParam_;
- for(int ip=0;ip<NbParam_;ip++)
- {
- dPrt[ind1+ip]-= Br*dBti[it]*val[ip];
+ for (int ip = 0; ip < NbParam_; ip++) {
+ dPrt[ind1 + ip] -= Br * dBti[it] * val[ip];
}
}
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam, int °ree)
+void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
+ int &Nc, unsigned int &NbParam, int °ree)
{
- switch(degree)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam); break;
+ PutTotPVBspline4PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
+ break;
default:
PutTotPVBspline3PrtTout(PrtTout, cr, er, ct, et, Nc, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,
+ int &Nc, unsigned int &NbParam)
{
- int sr=0;
- int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ int sr = 0;
+ int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3] = {0, 0, 0};
- for(char it=-1;it<=1;it++)
- {
- Bti[it+1] = Bspline3(-it+et);
+ for (char it = -1; it <= 1; it++) {
+ Bti[it + 1] = Bspline3(-it + et);
}
int NbParam_ = (int)NbParam;
- int NbParam_val = NbParam_+NbParam_*NbParam_;
-
- double *pt=&PrtTout[(unsigned int)(((cr+sr)*Nc+(ct+st))*9*(1+NbParam_val))];
- for(int ir=-1;ir<=1;ir++)
- {
- double Br= Bspline3(-ir+er);
- for(int it=0;it<=2;it++)
- {
- (*pt++) +=Br*Bti[it];
- pt=pt+NbParam_val;
+ int NbParam_val = NbParam_ + NbParam_ * NbParam_;
+
+ double *pt = &PrtTout[(unsigned int)(((cr + sr) * Nc + (ct + st)) * 9 * (1 + NbParam_val))];
+ for (int ir = -1; ir <= 1; ir++) {
+ double Br = Bspline3(-ir + er);
+ for (int it = 0; it <= 2; it++) {
+ (*pt++) += Br * Bti[it];
+ pt = pt + NbParam_val;
}
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline4PrtTout(double *PrtTout, int &cr, double &er, int &ct, double &et,int &Nc, unsigned int &NbParam)
+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};
- for(char it=-1;it<=2;it++)
- {
- Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ for (char it = -1; it <= 2; it++) {
+ Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
}
int NbParam_ = (int)NbParam;
- int NbParam_val = NbParam_+NbParam_*NbParam_;
- double *pt=&PrtTout[(unsigned int)((cr*Nc+ct)*16*(1+NbParam_val))];
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
- for(int it=0;it<=3;it++)
- {
- (*pt++) +=Br*Bti[it];
- pt=pt+NbParam_val;
+ int NbParam_val = NbParam_ + NbParam_ * NbParam_;
+ double *pt = &PrtTout[(unsigned int)((cr * Nc + ct) * 16 * (1 + NbParam_val))];
+ for (int ir = -1; ir <= 2; ir++) {
+ double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
+ for (int it = 0; it <= 3; it++) {
+ (*pt++) += Br * Bti[it];
+ pt = pt + NbParam_val;
}
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb, unsigned int &NbParam, int °ree)
+void vpTemplateTrackerMIBSpline::PutTotPVBsplinePrt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb,
+ unsigned int &NbParam, int °ree)
{
- switch(degree)
- {
+ switch (degree) {
case 4:
- PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam); break;
+ PutTotPVBspline4PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
+ break;
default:
PutTotPVBspline3PrtTout(Prt, cr, er, ct, et, Ncb, NbParam);
}
}
-void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline3Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
{
- int sr=0;
- int st=0;
- if(er>0.5){sr=1;er=er-1;}
- if(et>0.5){st=1;et=et-1;}
+ int sr = 0;
+ int st = 0;
+ if (er > 0.5) {
+ sr = 1;
+ er = er - 1;
+ }
+ if (et > 0.5) {
+ st = 1;
+ et = et - 1;
+ }
double Bti[3] = {0, 0, 0};
- for(char it=-1;it<=1;it++)
- {
- Bti[it+1] = Bspline3(-it+et);
+ for (char it = -1; it <= 1; it++) {
+ Bti[it + 1] = Bspline3(-it + et);
}
- for(int ir=-1;ir<=1;ir++)
- {
- double Br= Bspline3(-ir+er);
+ for (int ir = -1; ir <= 1; ir++) {
+ double Br = Bspline3(-ir + er);
int irInd = ir + 1;
- int ind = (cr+sr+irInd)*Ncb;
- for(int it=0;it<=2;it++)
- {
- Prt[ind+(ct+st+it)] += Br*Bti[it];
+ int ind = (cr + sr + irInd) * Ncb;
+ for (int it = 0; it <= 2; it++) {
+ Prt[ind + (ct + st + it)] += Br * Bti[it];
}
}
}
-
-void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et,int &Ncb)
+void vpTemplateTrackerMIBSpline::PutTotPVBspline4Prt(double *Prt, int &cr, double &er, int &ct, double &et, int &Ncb)
{
double Bti[4] = {0, 0, 0, 0};
- for(char it=-1;it<=2;it++)
- {
- Bti[it+1] =vpTemplateTrackerBSpline::Bspline4(-it+et);
+ for (char it = -1; it <= 2; it++) {
+ Bti[it + 1] = vpTemplateTrackerBSpline::Bspline4(-it + et);
}
- for(int ir=-1;ir<=2;ir++)
- {
- double Br=vpTemplateTrackerBSpline::Bspline4(-ir+er);
+ for (int ir = -1; ir <= 2; ir++) {
+ double Br = vpTemplateTrackerBSpline::Bspline4(-ir + er);
int irInd = ir + 1;
- int ind = (cr+irInd)*Ncb+ct;
+ int ind = (cr + irInd) * Ncb + ct;
- for(int it=0;it<=3;it++)
- {
- Prt[ind+it] += Br*Bti[it];
+ for (int it = 0; it <= 3; it++) {
+ Prt[ind + it] += Br * Bti[it];
}
}
}
diff --git a/modules/vision/CMakeLists.txt b/modules/vision/CMakeLists.txt
index f0abe44..ad9d0e4 100644
--- a/modules/vision/CMakeLists.txt
+++ b/modules/vision/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
@@ -50,7 +51,7 @@ else()
vp_add_tests(DEPENDS_ON visp_mbt visp_gui visp_io)
if(USE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- vp_set_source_file_compile_flag(test/key-point/testKeyPoint-2.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- vp_set_source_file_compile_flag(test/key-point/testKeyPoint-4.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ vp_set_source_file_compile_flag(test/key-point/testKeyPoint-2.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ vp_set_source_file_compile_flag(test/key-point/testKeyPoint-4.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
endif()
diff --git a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h
index 00d69bb..c4287fa 100644
--- a/modules/vision/include/visp3/vision/vpBasicKeyPoint.h
+++ b/modules/vision/include/visp3/vision/vpBasicKeyPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpBasicKeyPoint_H
#define vpBasicKeyPoint_H
@@ -51,7 +51,6 @@
#include <vector>
-
/*!
\class vpBasicKeyPoint
\ingroup group_vision_keypoints
@@ -62,186 +61,194 @@
*/
class VISP_EXPORT vpBasicKeyPoint
{
- public:
- vpBasicKeyPoint();
+public:
+ vpBasicKeyPoint();
- virtual ~vpBasicKeyPoint() {
+ virtual ~vpBasicKeyPoint()
+ {
matchedReferencePoints.resize(0);
currentImagePointsList.resize(0);
referenceImagePointsList.resize(0);
- };
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I) =0;
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) =0;
-
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) =0;
-
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle) =0;
-
- virtual void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3) =0;
-
- virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- const vpColor &color=vpColor::green) =0;
-
- /*!
- Indicate wether the reference has been built or not.
-
- \return True if the reference of the current instance has been built.
- */
- bool referenceBuilt() const {return _reference_computed;}
-
- /*!
- Get the pointer to the complete list of reference points. The pointer is const. Thus the points can not be modified
-
- \return The pointer to the complete list of reference points.
- */
- inline const vpImagePoint* getAllPointsInReferenceImage() {
- return &referenceImagePointsList[0];
- } ;
-
- /*!
- Get the nth reference point. This point is copied in the vpImagePoint instance given in argument.
-
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
- \param referencePoint : The coordinates of the desired reference point are copied there.
- */
- inline void getReferencePoint (const unsigned int index, vpImagePoint &referencePoint )
- {
- if (index >= referenceImagePointsList.size())
- {
- vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
- }
-
- referencePoint.set_ij(referenceImagePointsList[index].get_i(), referenceImagePointsList[index].get_j());
- }
-
- /*!
- Get the nth couple of reference point and current point which have been matched. These points are copied in the vpImagePoint instances given in argument.
-
- \param index : The index of the desired couple of reference point and current point . The index must be between 0 and the number of matched points - 1.
- \param referencePoint : The coordinates of the desired reference point are copied here.
- \param currentPoint : The coordinates of the desired current point are copied here.
- */
- inline void getMatchedPoints(const unsigned int index, vpImagePoint &referencePoint, vpImagePoint ¤tPoint) {
- if (index >= matchedReferencePoints.size())
- {
- vpTRACE("Index of the matched points out of range");
- throw(vpException(vpException::fatalError,"Index of the matched points out of range"));
- }
- referencePoint.set_ij(referenceImagePointsList[matchedReferencePoints[index]].get_i(),referenceImagePointsList[matchedReferencePoints[index]].get_j());
- currentPoint.set_ij(currentImagePointsList[index].get_i(), currentImagePointsList[index].get_j());
- };
-
- /*!
- Get the nth matched reference point index in the complete list of reference point.
-
- In the code below referencePoint1 and referencePoint2 correspond to the same matched reference point.
-
- \code
- vpKeyPointSurf surf;
-
- //Here the code to compute the reference points and the current points.
-
- vpImagePoint referencePoint1;
- vpImagePoint currentPoint;
- surf.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points
-
- vpImagePoint referencePoint2;
- const vpImagePoint* referencePointsList = surf.getAllPointsInReferenceImage();
- int index = surf.getIndexInAllReferencePointList(1); //Get the first matched reference point index in the complete reference point list
- referencePoint2 = referencePointsList[index]; //Get the first matched reference point
- \endcode
- */
- inline unsigned int getIndexInAllReferencePointList ( const unsigned int indexInMatchedPointList ) {
- if (indexInMatchedPointList >= matchedReferencePoints.size())
- {
+ };
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I) = 0;
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width) = 0;
+
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle) = 0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I) = 0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width) = 0;
+
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle) = 0;
+
+ virtual void display(const vpImage<unsigned char> &Iref, const vpImage<unsigned char> &Icurrent,
+ unsigned int size = 3) = 0;
+
+ virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size = 3,
+ const vpColor &color = vpColor::green) = 0;
+
+ /*!
+ Indicate wether the reference has been built or not.
+
+ \return True if the reference of the current instance has been built.
+ */
+ bool referenceBuilt() const { return _reference_computed; }
+
+ /*!
+ Get the pointer to the complete list of reference points. The pointer is
+ const. Thus the points can not be modified
+
+ \return The pointer to the complete list of reference points.
+ */
+ inline const vpImagePoint *getAllPointsInReferenceImage() { return &referenceImagePointsList[0]; };
+
+ /*!
+ Get the nth reference point. This point is copied in the vpImagePoint
+ instance given in argument.
+
+ \param index : The index of the desired reference point. The index must be
+ between 0 and the number of reference points - 1. \param referencePoint :
+ The coordinates of the desired reference point are copied there.
+ */
+ inline void getReferencePoint(const unsigned int index, vpImagePoint &referencePoint)
+ {
+ if (index >= referenceImagePointsList.size()) {
+ vpTRACE("Index of the reference point out of range");
+ throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
+ }
+
+ referencePoint.set_ij(referenceImagePointsList[index].get_i(), referenceImagePointsList[index].get_j());
+ }
+
+ /*!
+ Get the nth couple of reference point and current point which have been
+ matched. These points are copied in the vpImagePoint instances given in
+ argument.
+
+ \param index : The index of the desired couple of reference point and
+ current point . The index must be between 0 and the number of matched
+ points - 1. \param referencePoint : The coordinates of the desired
+ reference point are copied here. \param currentPoint : The coordinates of
+ the desired current point are copied here.
+ */
+ inline void getMatchedPoints(const unsigned int index, vpImagePoint &referencePoint, vpImagePoint ¤tPoint)
+ {
+ if (index >= matchedReferencePoints.size()) {
+ vpTRACE("Index of the matched points out of range");
+ throw(vpException(vpException::fatalError, "Index of the matched points out of range"));
+ }
+ referencePoint.set_ij(referenceImagePointsList[matchedReferencePoints[index]].get_i(),
+ referenceImagePointsList[matchedReferencePoints[index]].get_j());
+ currentPoint.set_ij(currentImagePointsList[index].get_i(), currentImagePointsList[index].get_j());
+ };
+
+ /*!
+ Get the nth matched reference point index in the complete list of
+ reference point.
+
+ In the code below referencePoint1 and referencePoint2 correspond to the
+ same matched reference point.
+
+ \code
+ vpKeyPointSurf surf;
+
+ //Here the code to compute the reference points and the current points.
+
+ vpImagePoint referencePoint1;
+ vpImagePoint currentPoint;
+ surf.getMatchedPoints(1, referencePoint1, currentPoint); //Get the first matched points
+
+ vpImagePoint referencePoint2;
+ const vpImagePoint* referencePointsList = surf.getAllPointsInReferenceImage();
+ // 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)
+ {
+ 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];
- }
+ }
- /*!
- Get the number of reference points.
+ /*!
+ Get the number of reference points.
- \return the number of reference points.
- */
- inline unsigned int getReferencePointNumber() const {return (unsigned int)referenceImagePointsList.size();};
+ \return the number of reference points.
+ */
+ inline unsigned int getReferencePointNumber() const { return (unsigned int)referenceImagePointsList.size(); };
- /*!
- Get the number of matched points.
+ /*!
+ Get the number of matched points.
- \return the number of matched points.
- */
- inline unsigned int getMatchedPointNumber() const {return (unsigned int)matchedReferencePoints.size();};
+ \return the number of matched points.
+ */
+ inline unsigned int getMatchedPointNumber() const { return (unsigned int)matchedReferencePoints.size(); };
- /*!
- Return the vector of reference image point.
+ /*!
+ Return the vector of reference image point.
- \warning Should not be modified.
+ \warning Should not be modified.
- \return Vector of reference image point.
- */
- const std::vector<vpImagePoint>& getReferenceImagePointsList() const {return referenceImagePointsList;}
+ \return Vector of reference image point.
+ */
+ const std::vector<vpImagePoint> &getReferenceImagePointsList() const { return referenceImagePointsList; }
- /*!
- Return the vector of current image point.
+ /*!
+ Return the vector of current image point.
- \warning Should not be modified.
+ \warning Should not be modified.
- \return Vector of the current image point.
- */
- const std::vector<vpImagePoint>& getCurrentImagePointsList() const {return currentImagePointsList;}
+ \return Vector of the current image point.
+ */
+ const std::vector<vpImagePoint> &getCurrentImagePointsList() const { return currentImagePointsList; }
- /*!
- Return the index of the matched associated to the current image point i.
- The ith element of the vector is the index of the reference image point
- matching with the current image point.
+ /*!
+ Return the index of the matched associated to the current image point i.
+ The ith element of the vector is the index of the reference image point
+ matching with the current image point.
- \warning Should not be modified.
+ \warning Should not be modified.
- \return The vector of matching index.
- */
- const std::vector<unsigned int>& getMatchedReferencePoints() const {return matchedReferencePoints;}
+ \return The vector of matching index.
+ */
+ const std::vector<unsigned int> &getMatchedReferencePoints() const { return matchedReferencePoints; }
- private:
- virtual void init()=0;
+private:
+ virtual void init() = 0;
- protected:
- /*!
- List of the points which define the reference.
- */
- std::vector<vpImagePoint> referenceImagePointsList;
+protected:
+ /*!
+ List of the points which define the reference.
+ */
+ std::vector<vpImagePoint> referenceImagePointsList;
- /*!
- List of the points which belong to the current image and have
- been matched with points belonging to the reference.
- */
- std::vector<vpImagePoint> currentImagePointsList;
+ /*!
+ List of the points which belong to the current image and have
+ been matched with points belonging to the reference.
+ */
+ std::vector<vpImagePoint> currentImagePointsList;
- /*!
- Array containing the index in the array "referenceImagePointsList" of the reference points which have been matched.
+ /*!
+ Array containing the index in the array "referenceImagePointsList" of the
+ reference points which have been matched.
- The first element of the "currentImagePointsList" array is matched with the nth element of the "referenceImagePointsList" array.
- The value of n is stored in the first element of the "matchedReferencePoints" array.
- */
- std::vector<unsigned int> matchedReferencePoints;
+ The first element of the "currentImagePointsList" array is matched with
+ the nth element of the "referenceImagePointsList" array. The value of n is
+ stored in the first element of the "matchedReferencePoints" array.
+ */
+ std::vector<unsigned int> matchedReferencePoints;
- //! flag to indicate if the reference has been built.
- bool _reference_computed;
+ //! flag to indicate if the reference has been built.
+ bool _reference_computed;
};
#endif
@@ -251,4 +258,3 @@ class VISP_EXPORT vpBasicKeyPoint
* c-basic-offset: 2
* End:
*/
-
diff --git a/modules/vision/include/visp3/vision/vpCalibration.h b/modules/vision/include/visp3/vision/vpCalibration.h
index 11c897d..4ec29a2 100644
--- a/modules/vision/include/visp3/vision/vpCalibration.h
+++ b/modules/vision/include/visp3/vision/vpCalibration.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,11 +38,10 @@
*
*****************************************************************************/
-
/*!
\file vpCalibration.h
\brief Tools for camera calibration.
-
+
\author Eric Marchand (INRIA) using code from Francois Chaumette (INRIA)
\sa the example in calibrate.cpp
@@ -49,17 +49,17 @@
#ifndef vpCalibration_h
#define vpCalibration_h
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
+#include <list>
+#include <vector>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpExponentialMap.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/vision/vpCalibrationException.h>
#include <visp3/core/vpImagePoint.h>
-#include <list>
-#include <vector>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/vision/vpCalibrationException.h>
/*!
\class vpCalibration
@@ -74,20 +74,28 @@ public:
/*!
Minimization algorithm use to estimate the camera parameters.
*/
- typedef enum{
- CALIB_LAGRANGE, /*!< Lagrange approach without estimation of the distortion. */
- CALIB_VIRTUAL_VS, /*!< Virtual visual servoing approach without estimation of the distortion (results are similar to Lowe approach). */
- CALIB_VIRTUAL_VS_DIST, /*!< Virtual visual servoing approach with estimation of the distortion. */
- CALIB_LAGRANGE_VIRTUAL_VS, /*!< Lagrange approach first, than virtual visual servoing approach, without estimation of the distortion. */
- CALIB_LAGRANGE_VIRTUAL_VS_DIST, /*!< Lagrange approach first, than virtual visual servoing approach, with estimation of the distortion. */
- } vpCalibrationMethodType ;
-
- vpHomogeneousMatrix cMo ; //!< the pose computed for the model without distortion
+ typedef enum {
+ CALIB_LAGRANGE, /*!< Lagrange approach without estimation of the
+ distortion. */
+ CALIB_VIRTUAL_VS, /*!< Virtual visual servoing approach without estimation
+ of the distortion (results are similar to Lowe
+ approach). */
+ CALIB_VIRTUAL_VS_DIST, /*!< Virtual visual servoing approach with
+ estimation of the distortion. */
+ CALIB_LAGRANGE_VIRTUAL_VS, /*!< Lagrange approach first, than virtual
+ visual servoing approach, without
+ estimation of the distortion. */
+ CALIB_LAGRANGE_VIRTUAL_VS_DIST, /*!< Lagrange approach first, than virtual
+ visual servoing approach, with
+ estimation of the distortion. */
+ } vpCalibrationMethodType;
+
+ vpHomogeneousMatrix cMo; //!< the pose computed for the model without distortion
//!< (as a 3x4 matrix [R T])
- vpHomogeneousMatrix cMo_dist ; //!< the pose computed for perspective projection
+ 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
+ vpCameraParameters cam; //!< camera intrinsic parameters for perspective
//!< projection model without distortion
vpCameraParameters cam_dist; //!< camera intrinsic parameters for perspective
//!< projection model with distortion
@@ -99,107 +107,90 @@ public:
public:
// Constructor
- vpCalibration() ;
- vpCalibration(const vpCalibration& c) ;
+ vpCalibration();
+ vpCalibration(const vpCalibration &c);
// Destructor
- virtual ~vpCalibration() ;
+ virtual ~vpCalibration();
// Add a new point in this array
- int addPoint(double X, double Y, double Z, vpImagePoint &ip) ;
+ int addPoint(double X, double Y, double Z, vpImagePoint &ip);
// = operator
- vpCalibration& operator=(const vpCalibration& twinCalibration);
+ vpCalibration &operator=(const vpCalibration &twinCalibration);
- static void calibrationTsai(std::vector<vpHomogeneousMatrix> &cMo,
- std::vector<vpHomogeneousMatrix> &rMe,
+ static void calibrationTsai(const std::vector<vpHomogeneousMatrix> &cMo, const std::vector<vpHomogeneousMatrix> &rMe,
vpHomogeneousMatrix &eMc);
//! Suppress all the point in the array of point
- int clearPoint() ;
+ int clearPoint();
void computeStdDeviation(double &deviation, double &deviation_dist);
- int computeCalibration(vpCalibrationMethodType method,
- vpHomogeneousMatrix &cMo_est,
- vpCameraParameters &cam_est,
- bool verbose = false) ;
- static int computeCalibrationMulti(vpCalibrationMethodType method,
- std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError,
- bool verbose = false) ;
-
- static int computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
- vpHomogeneousMatrix &eMc,
+ int computeCalibration(vpCalibrationMethodType method, vpHomogeneousMatrix &cMo_est, vpCameraParameters &cam_est,
+ bool verbose = false);
+ static int computeCalibrationMulti(vpCalibrationMethodType method, std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam, double &globalReprojectionError, bool verbose = false);
+
+ static int computeCalibrationTsai(const std::vector<vpCalibration> &table_cal, vpHomogeneousMatrix &eMc,
vpHomogeneousMatrix &eMc_dist);
- double computeStdDeviation(const vpHomogeneousMatrix &cMo_est,
- const vpCameraParameters &camera);
- double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo,
- const vpCameraParameters &cam);
- int displayData(vpImage<unsigned char> &I, vpColor color=vpColor::red,
- unsigned int thickness=1, int subsampling_factor=1) ;
- int displayGrid(vpImage<unsigned char> &I, vpColor color=vpColor::yellow,
- unsigned int thickness=1, int subsampling_factor=1) ;
+ double computeStdDeviation(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera);
+ double computeStdDeviation_dist(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+ int displayData(vpImage<unsigned char> &I, vpColor color = vpColor::red, unsigned int thickness = 1,
+ int subsampling_factor = 1);
+ int displayGrid(vpImage<unsigned char> &I, vpColor color = vpColor::yellow, unsigned int thickness = 1,
+ int subsampling_factor = 1);
//! Set the gain for the virtual visual servoing algorithm.
- static double getLambda(){return gain;}
+ static double getLambda() { return gain; }
- //!get the residual in pixels
- double getResidual(void) const {return residual;}
- //!get the residual for perspective projection with distortion (in pixels)
- double getResidual_dist(void) const {return residual_dist;}
- //!get the number of points
- unsigned int get_npt() const {return npt;}
+ //! get the residual in pixels
+ double getResidual(void) const { return residual; }
+ //! get the residual for perspective projection with distortion (in pixels)
+ double getResidual_dist(void) const { return residual_dist; }
+ //! get the number of points
+ unsigned int get_npt() const { return npt; }
- int init() ;
+ int init();
- int readData(const char *filename) ;
- static int readGrid(const char *filename,unsigned int &n,
- std::list<double> &oX, std::list<double> &oY, std::list<double> &oZ,
- bool verbose = false);
+ int readData(const char *filename);
+ static int readGrid(const char *filename, unsigned int &n, std::list<double> &oX, std::list<double> &oY,
+ std::list<double> &oZ, bool verbose = false);
- //!set the gain for the virtual visual servoing algorithm
- static void setLambda(const double &lambda){gain = lambda;}
- int writeData(const char *filename) ;
+ //! set the gain for the virtual visual servoing algorithm
+ static void setLambda(const double &lambda) { gain = lambda; }
+ int writeData(const char *filename);
private:
void computePose(const vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
- void calibLagrange( vpCameraParameters &cam , vpHomogeneousMatrix &cMo) ;
+ void calibLagrange(vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
//! Compute the calibration using virtual visual servoing approach
- void calibVVS( vpCameraParameters &cam , vpHomogeneousMatrix &cMo,
- bool verbose = false) ;
-
- static void calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[],
- vpCameraParameters &cam, bool verbose = false);
- static void calibVVSMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError, bool verbose = false) ;
- void calibVVSWithDistortion( vpCameraParameters &cam,
- vpHomogeneousMatrix &cMo,
- bool verbose = false) ;
- static void calibVVSWithDistortionMulti( unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam,
- bool verbose = false );
- static void calibVVSWithDistortionMulti( std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam,
- double &globalReprojectionError,
- bool verbose = false);
+ void calibVVS(vpCameraParameters &cam, vpHomogeneousMatrix &cMo, bool verbose = false);
+
+ static void calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam,
+ bool verbose = false);
+ static void calibVVSMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam,
+ double &globalReprojectionError, bool verbose = false);
+ void calibVVSWithDistortion(vpCameraParameters &cam, vpHomogeneousMatrix &cMo, bool verbose = false);
+ static void calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam,
+ bool verbose = false);
+ static void calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam,
+ double &globalReprojectionError, bool verbose = false);
private:
- unsigned int npt ; //!< number of points used in calibration computation
- std::list<double> LoX, LoY, LoZ ; //!< list of points coordinates (3D in meters)
- std::list<vpImagePoint> Lip ; //!< list of points coordinates (2D in pixels)
+ unsigned int npt; //!< number of points used in calibration computation
+ std::list<double> LoX, LoY,
+ LoZ; //!< list of points coordinates (3D in meters)
+ std::list<vpImagePoint> Lip; //!< list of points coordinates (2D in pixels)
- double residual ; //!< residual in pixel for camera model without distortion
- double residual_dist ; //!< residual in pixel for perspective projection with distortion model
+ double residual; //!< residual in pixel for camera model without distortion
+ double residual_dist; //!< residual in pixel for perspective projection with
+ //!< distortion model
static double threshold;
static unsigned int nbIterMax;
static double gain;
-
-} ;
+};
#endif
diff --git a/modules/vision/include/visp3/vision/vpCalibrationException.h b/modules/vision/include/visp3/vision/vpCalibrationException.h
index b5597e2..180696e 100644
--- a/modules/vision/include/visp3/vision/vpCalibrationException.h
+++ b/modules/vision/include/visp3/vision/vpCalibrationException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,23 +39,27 @@
#ifndef __vpCalibrationException_H
#define __vpCalibrationException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* Classes standards. */
//
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
@@ -63,39 +68,36 @@
*/
class VISP_EXPORT vpCalibrationException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpCalibration member
- */
- enum errorCodeEnum
- {
- //! error returns by a constructor
- constructionError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedError,
- //! index out of range
- outOfRangeError,
- //! iterative algorithm doesn't converge
- convergencyError,
- forbiddenOperatorError,
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpCalibration member
+ */
+ enum errorCodeEnum {
+ //! error returns by a constructor
+ constructionError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedError,
+ //! index out of range
+ outOfRangeError,
+ //! iterative algorithm doesn't converge
+ convergencyError,
+ forbiddenOperatorError,
+ };
- public:
- vpCalibrationException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpCalibrationException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpCalibrationException (const int id)
- : vpException(id){ ; }
+public:
+ vpCalibrationException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpCalibrationException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpCalibrationException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/vision/include/visp3/vision/vpFernClassifier.h b/modules/vision/include/visp3/vision/vpFernClassifier.h
index cb9f31a..7981860 100644
--- a/modules/vision/include/visp3/vision/vpFernClassifier.h
+++ b/modules/vision/include/visp3/vision/vpFernClassifier.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,14 +44,15 @@
#include <string>
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/legacy/legacy.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/legacy/legacy.hpp>
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
-# include <cv.h>
-# include <cvaux.hpp>
+#include <cv.h>
+#include <cvaux.hpp>
#endif
#include <visp3/vision/vpBasicKeyPoint.h>
@@ -59,28 +61,29 @@
\class vpFernClassifier
\ingroup group_vision_keypoints
- \brief Class that implements the Fern classifier and the YAPE detector thanks
- to the OpenCV library.
-
+ \brief Class that implements the Fern classifier and the YAPE detector
+thanks to the OpenCV library.
+
\deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
- This class provides a way to detect and match point using the YAPE and
+ This class provides a way to detect and match point using the YAPE and
a Fern Classifiers, thanks to the OpenCV library (version >= 2.0)
- This class provides a tool to match points between a model and the current
- image. The points of interests belonging to the model and the points detected
- in the current image are given in pixels thanks to the vpImagePoint class.
-
+ This class provides a tool to match points between a model and the current
+ image. The points of interests belonging to the model and the points
+detected in the current image are given in pixels thanks to the vpImagePoint
+class.
+
For more details about the Ferns Classifier and the point detector,
see \cite Ozuysal10 and \cite Lepetit04c.
-
- To use this class, you first have to detect points in the model and train the
- associated Fern classifier. Then, for each new grabbed image, You can detect
- points and try to match them with the model.
-
- As training can requires up to several minutes, it is possible to save (in a
+
+ To use this class, you first have to detect points in the model and train
+the associated Fern classifier. Then, for each new grabbed image, You can
+detect points and try to match them with the model.
+
+ As training can requires up to several minutes, it is possible to save (in a
file) and load the classifier.
-
+
The following small example shows how to use the class.
\code
@@ -88,8 +91,8 @@
#include <visp3/core/vpImage.h>
#include <visp3/vision/vpFernClassifier.h>
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
-int main()
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available
+since OpenCV-2.0.0 int main()
{
vpImage<unsigned char> Ireference;
vpImage<unsigned char> Icurrent;
@@ -121,12 +124,12 @@ int main() {}
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/vision/vpFernClassifier.h>
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available since OpenCV-2.0.0
-int main()
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Fern classifier only available
+since OpenCV-2.0.0 int main()
{
vpImage<unsigned char> Ireference;
vpImage<unsigned char> Icurrent;
@@ -136,8 +139,7 @@ int main()
//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++)
- {
+ for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Ireference, corners[i]);
}
@@ -148,21 +150,20 @@ int main()
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++)
- {
+ 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.
+ // 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);
@@ -172,22 +173,22 @@ int main() {}
#endif
\endcode
*/
-class VISP_EXPORT vpFernClassifier: public vpBasicKeyPoint
+class VISP_EXPORT vpFernClassifier : public vpBasicKeyPoint
{
protected:
//! The points of interest detector.
cv::LDetector ldetector;
-
+
//! The Fern classifier.
cv::FernClassifier fernClassifier;
//! The patch generator (OpenCV format).
cv::PatchGenerator gen;
-
+
//! Flag to indicate whether the classifier has been trained or not.
bool hasLearn;
-
- /* parameters for the key-points detector and the classifier */
+
+ /* parameters for the key-points detector and the classifier */
//! Threshold to accept or reject points (usually around 20)
int threshold;
//! Number of view to generate for the training
@@ -207,28 +208,30 @@ protected:
//! Maximal number of points
int nbPoints;
- /* parameters for the filter */
- //! Flag to specify whether the reference image have to be blurred or not in order to improve the recognition rate
+ /* parameters for the filter */
+ //! Flag to specify whether the reference image have to be blurred or not in
+ //! order to improve the recognition rate
bool blurImage;
- //! Radius of the kernel used to blur the image
+ //! Radius of the kernel used to blur the image
int radiusBlur;
//! Sigma of the kernel used to blur the image
int sigmaBlur;
-
- //! Number of minimum point below which the homography is not estimated (must be at least four)
+
+ //! Number of minimum point below which the homography is not estimated
+ //! (must be at least four)
unsigned int nbMinPoint;
-
- //! The current image in the OpenCV format.
+
+//! The current image in the OpenCV format.
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
cv::Mat curImg;
#else
- IplImage* curImg;
+ IplImage *curImg;
#endif
-
+
//! keypoints detected in the reference image.
std::vector<cv::KeyPoint> objKeypoints;
//! the ROI in the reference image.
- cv::Rect modelROI_Ref;
+ cv::Rect modelROI_Ref;
//! the ROI for the reference image.
cv::Rect modelROI;
//! the vector containing the points in the model.
@@ -237,95 +240,88 @@ protected:
std::vector<cv::KeyPoint> imgKeypoints;
//! vector in the OpenCV format to be used by the detector.
std::vector<cv::Point2f> refPt, curPt;
-
public:
vpFernClassifier();
- vpFernClassifier(const std::string& _dataFile, const std::string& _objectName);
+ vpFernClassifier(const std::string &_dataFile, const std::string &_objectName);
virtual ~vpFernClassifier();
/* build reference */
virtual unsigned int buildReference(const vpImage<unsigned char> &I);
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP,
const unsigned int height, const unsigned int width);
- virtual unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
+ virtual unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle);
/* matching */
virtual unsigned int matchPoint(const vpImage<unsigned char> &I);
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- virtual unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ virtual unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle);
/* display */
- virtual void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3);
- virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- const vpColor &color=vpColor::green);
-
+ virtual void display(const vpImage<unsigned char> &Iref, const vpImage<unsigned char> &Icurrent,
+ unsigned int size = 3);
+ virtual void display(const vpImage<unsigned char> &Icurrent, unsigned int size = 3,
+ const vpColor &color = vpColor::green);
+
/* io methods */
- void load(const std::string& _dataFile, const std::string& /*_objectName*/);
- void record(const std::string& _objectName, const std::string& _dataFile);
-
-
-
- /* accessors */
+ void load(const std::string &_dataFile, const std::string & /*_objectName*/);
+ void record(const std::string &_objectName, const std::string &_dataFile);
+
+ /* accessors */
/*!
- The image is blurred before being processed. This solution can lead to a
+ The image is blurred before being processed. This solution can lead to a
better recognition rate.
-
+
\param _blur : the new option for the blur
*/
- inline void setBlurSetting(const bool _blur){ blurImage = _blur;}
+ inline void setBlurSetting(const bool _blur) { blurImage = _blur; }
inline void setBlurSettings(const bool _blur, int _sigma, int _size);
-
+
/*!
- Return the blur option.
- The Blur option is used to activate a filter used to blur the input image.
+ Return the blur option.
+ The Blur option is used to activate a filter used to blur the input image.
The blurring can improve the robustness of the detection.
-
+
\return the value of the blur option.
*/
- inline bool getBlurSetting(){ return blurImage;}
-
+ inline bool getBlurSetting() { return blurImage; }
+
/*!
Return the blur sigma (for the filter) option.
-
+
\return The value of the sigma for the blur filter.
*/
- inline int getBlurSigma() { return this->sigmaBlur;}
-
+ inline int getBlurSigma() { return this->sigmaBlur; }
+
/*!
return the blur size (for the filter) option
-
+
\return the value of the radius for the blur filter
*/
- inline int getBlurSize() { return this->radiusBlur;}
+ inline int getBlurSize() { return this->radiusBlur; }
/*!
Return a reference on the vector of reference points in the OpenCV format
-
+
\return the list of reference points
*/
- const std::vector<cv::Point2f>& getRefPt() const {return refPt;}
-
+ const std::vector<cv::Point2f> &getRefPt() const { return refPt; }
+
/*!
Return a reference on the vector of current points in the OpenCV format
-
+
\return the list of current points
*/
- const std::vector<cv::Point2f>& getCurPt() const {return curPt;}
-
+ const std::vector<cv::Point2f> &getCurPt() const { return curPt; }
+
/*!
- Return the region of interest in the OpenCV format
+ Return the region of interest in the OpenCV format
*/
- cv::Rect getModelROI() const { return modelROI;}
+ cv::Rect getModelROI() const { return modelROI; }
protected:
- void setImage(const vpImage<unsigned char>& I);
+ void setImage(const vpImage<unsigned char> &I);
void train();
virtual void init();
};
@@ -333,4 +329,3 @@ protected:
#endif /* opencv ver > 2.0.0 */
#endif
-
diff --git a/modules/vision/include/visp3/vision/vpHomography.h b/modules/vision/include/visp3/vision/vpHomography.h
index 0b64ce4..53b05ad 100644
--- a/modules/vision/include/visp3/vision/vpHomography.h
+++ b/modules/vision/include/visp3/vision/vpHomography.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,12 +37,11 @@
*
*****************************************************************************/
-
/*!
\file vpHomography.h
-This file defines an homography transformation. This class aims to provide some
-tools for homography computation.
+This file defines an homography transformation. This class aims to provide
+some tools for homography computation.
*/
#ifndef vpHomography_hh
@@ -51,14 +51,13 @@ tools for homography computation.
#include <vector>
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpPlane.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPoint.h>
-/*!
+/*!
\class vpHomography
\ingroup group_vision_homography
@@ -74,9 +73,9 @@ tools for homography computation.
the corresponding point (image of the same 3D point) in the other image
points set. These 2 sets are the only data needed to compute the
homography. One method used is the one introduced by Ezio Malis during his
- PhD \cite TheseMalis. A normalization is carried out on this points in order to improve the
- conditioning of the problem, what leads to improve the stability of the
- result.
+ PhD \cite TheseMalis. A normalization is carried out on this points in order
+to improve the conditioning of the problem, what leads to improve the
+stability of the result.
Store and compute the homography such that
\f[
@@ -98,9 +97,9 @@ tools for homography computation.
\code
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/vision/vpHomography.h>
int main()
{
@@ -131,12 +130,10 @@ int main()
vpPoint Pa[4];
std::vector<double> xa(4), ya(4); // Coordinates in pixels of the points in frame a
for(int i=0 ; i < 4 ; i++) {
- Pa[i] = Po[i];
- Pa[i].project(aMo); // Project the points from object frame to
- // camera frame a
- vpMeterPixelConversion::convertPoint(cam,
- Pa[i].get_x(), Pa[i].get_y(),
- xa[i], ya[i]);
+ Pa[i] = Po[i]; Pa[i].project(aMo); // Project the points from object frame to camera frame a
+ vpMeterPixelConversion::convertPoint(cam,
+ Pa[i].get_x(), Pa[i].get_y(),
+ xa[i], ya[i]);
}
// Compute the coordinates in pixels of the 4 object points in the
@@ -144,9 +141,7 @@ int main()
vpPoint Pb[4];
std::vector<double> xb(4), yb(4); // Coordinates in pixels of the points in frame b
for(int i=0 ; i < 4 ; i++) {
- Pb[i] = Po[i];
- Pb[i].project(bMo); // Project the points from object frame to
- // camera frame a
+ 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
@@ -162,7 +157,7 @@ int main()
vpHomography bHa = aHb.inverse();
for(int i = 0; i < 4 ; i++){
double inv_z = 1. / (bHa[2][0] * xa[i] + bHa[2][1] * ya[i] + bHa[2][2]);
-
+
xb[i] = (bHa[0][0] * xa[i] + bHa[0][1] * ya[i] + bHa[0][2]) * inv_z;
yb[i] = (bHa[1][0] * xa[i] + bHa[1][1] * ya[i] + bHa[1][2]) * inv_z;
}
@@ -178,214 +173,149 @@ int main()
*/
class VISP_EXPORT vpHomography : public vpArray2D<double>
{
- private:
- static const double sing_threshold; // = 0.0001;
- static const double threshold_rotation;
- static const double threshold_displacement;
- vpHomogeneousMatrix aMb ;
- // bool isplanar;
- //! reference plane coordinates expressed in Rb
- vpPlane bP ;
-
- private:
- //! build the homography from aMb and Rb
- void build() ;
-
- //! insert a rotation matrix
- void insert(const vpHomogeneousMatrix &aRb) ;
- //! insert a rotation matrix
- void insert(const vpRotationMatrix &aRb) ;
- //! insert a theta u vector (transformation into a rotation matrix)
- void insert(const vpThetaUVector &tu) ;
- //! insert a translation vector
- void insert(const vpTranslationVector &atb) ;
- //! insert a translation vector
- void insert(const vpPlane &bP) ;
-
- static void initRansac(unsigned int n,
- double *xb, double *yb,
- double *xa, double *ya,
- vpColVector &x) ;
-
- public:
- vpHomography() ;
- vpHomography(const vpHomography &H) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpRotationMatrix &aRb,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- vpHomography(const vpPoseVector &arb,
- const vpPlane &bP) ;
- virtual ~vpHomography() {};
-
- //! Construction from Translation and rotation and a plane
- void buildFrom(const vpRotationMatrix &aRb,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- void buildFrom(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &bP) ;
- //! Construction from Translation and rotation and a plane
- void buildFrom(const vpPoseVector &arb,
- const vpPlane &bP) ;
- //! Construction from homogeneous matrix and a plane
- void buildFrom(const vpHomogeneousMatrix &aMb,
- const vpPlane &bP) ;
- vpMatrix convert() const;
-
- void computeDisplacement(vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- void computeDisplacement(const vpColVector& nd,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- void eye();
-
- //! invert the homography
- vpHomography inverse() const ;
- //! invert the homography
- void inverse(vpHomography &Hi) const;
-
- //! Load an homography from a file
- void load(std::ifstream &f) ;
-
- // Multiplication by an homography
- vpHomography operator*(const vpHomography &H) const;
- // Multiplication by a scalar
- vpHomography operator*(const double &v) const;
- vpColVector operator*(const vpColVector &b) const;
- // Multiplication by a point
- vpPoint operator*(const vpPoint &H) const;
-
- // Division by a scalar
- vpHomography operator/(const double &v) const;
- vpHomography & operator/=(double v);
- vpHomography & operator=(const vpHomography &H);
- vpHomography & operator=(const vpMatrix &H);
-
- vpImagePoint projection(const vpImagePoint &p);
-
- /*!
- This function is not applicable to an homography that is always a
- 3-by-3 matrix.
- \exception vpException::fatalError When this function is called.
- */
- void resize(const unsigned int nrows, const unsigned int ncols,
- const bool flagNullify = true)
- {
- (void)nrows;
- (void)ncols;
- (void)flagNullify;
- throw(vpException(vpException::fatalError, "Cannot resize an homography matrix"));
- };
-
- void save(std::ofstream &f) const ;
-
- static void DLT(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya ,
- vpHomography &aHb,
- bool normalization=true);
-
- static void HLM(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- bool isplanar,
- vpHomography &aHb) ;
-
- static bool ransac(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- unsigned int nbInliersConsensus,
- double threshold,
- bool normalization=true);
-
- static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa);
- static vpPoint project(const vpHomography &bHa, const vpPoint &Pa);
-
- static void robust(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inlier,
- double &residual,
- double weights_threshold=0.4,
- unsigned int niter=4,
- bool normalization=true);
+private:
+ static const double sing_threshold; // = 0.0001;
+ static const double threshold_rotation;
+ static const double threshold_displacement;
+ vpHomogeneousMatrix aMb;
+ // bool isplanar;
+ //! reference plane coordinates expressed in Rb
+ vpPlane bP;
+
+private:
+ //! build the homography from aMb and Rb
+ void build();
+
+ //! insert a rotation matrix
+ void insert(const vpHomogeneousMatrix &aRb);
+ //! insert a rotation matrix
+ void insert(const vpRotationMatrix &aRb);
+ //! insert a theta u vector (transformation into a rotation matrix)
+ void insert(const vpThetaUVector &tu);
+ //! insert a translation vector
+ void insert(const vpTranslationVector &atb);
+ //! insert a translation vector
+ void insert(const vpPlane &bP);
+
+ static void initRansac(unsigned int n, double *xb, double *yb, double *xa, double *ya, vpColVector &x);
+
+public:
+ vpHomography();
+ vpHomography(const vpHomography &H);
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpHomogeneousMatrix &aMb, const vpPlane &bP);
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP);
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP);
+ //! Construction from Translation and rotation and a plane
+ vpHomography(const vpPoseVector &arb, const vpPlane &bP);
+ virtual ~vpHomography(){};
+
+ //! Construction from Translation and rotation and a plane
+ void buildFrom(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &bP);
+ //! Construction from Translation and rotation and a plane
+ void buildFrom(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &bP);
+ //! Construction from Translation and rotation and a plane
+ void buildFrom(const vpPoseVector &arb, const vpPlane &bP);
+ //! Construction from homogeneous matrix and a plane
+ void buildFrom(const vpHomogeneousMatrix &aMb, const vpPlane &bP);
+ vpMatrix convert() const;
+
+ void computeDisplacement(vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n);
+
+ void computeDisplacement(const vpColVector &nd, vpRotationMatrix &aRb, vpTranslationVector &atb, vpColVector &n);
+
+ void eye();
+
+ //! invert the homography
+ vpHomography inverse() const;
+ //! invert the homography
+ void inverse(vpHomography &Hi) const;
+
+ //! Load an homography from a file
+ void load(std::ifstream &f);
+
+ // Multiplication by an homography
+ vpHomography operator*(const vpHomography &H) const;
+ // Multiplication by a scalar
+ vpHomography operator*(const double &v) const;
+ vpColVector operator*(const vpColVector &b) const;
+ // Multiplication by a point
+ vpPoint operator*(const vpPoint &H) const;
+
+ // Division by a scalar
+ vpHomography operator/(const double &v) const;
+ vpHomography &operator/=(double v);
+ vpHomography &operator=(const vpHomography &H);
+ vpHomography &operator=(const vpMatrix &H);
+
+ vpImagePoint projection(const vpImagePoint &p);
+
+ /*!
+ This function is not applicable to an homography that is always a
+ 3-by-3 matrix.
+ \exception vpException::fatalError When this function is called.
+ */
+ void resize(const unsigned int nrows, const unsigned int ncols, const bool flagNullify = true)
+ {
+ (void)nrows;
+ (void)ncols;
+ (void)flagNullify;
+ throw(vpException(vpException::fatalError, "Cannot resize an homography matrix"));
+ };
+
+ void save(std::ofstream &f) const;
+
+ static void DLT(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, bool normalization = true);
+
+ static void HLM(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, bool isplanar, vpHomography &aHb);
+
+ static bool ransac(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers, double &residual,
+ unsigned int nbInliersConsensus, double threshold, bool normalization = true);
+
+ static vpImagePoint project(const vpCameraParameters &cam, const vpHomography &bHa, const vpImagePoint &iPa);
+ static vpPoint project(const vpHomography &bHa, const vpPoint &Pa);
+
+ static void robust(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inlier, double &residual,
+ double weights_threshold = 0.4, unsigned int niter = 4, bool normalization = true);
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- //! build the homography from aMb and Rb
- static void build(vpHomography &aHb,
- const vpHomogeneousMatrix &aMb,
- const vpPlane &bP) ;
-
- static void computeDisplacement(const vpHomography &aHb,
- const vpColVector& nd,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- static void computeDisplacement (const vpHomography &aHb,
- vpRotationMatrix &aRb,
- vpTranslationVector &atb,
- vpColVector &n) ;
-
- static void computeDisplacement(const vpHomography &H,
- const double x,
- const double y,
- std::list<vpRotationMatrix> & vR,
- std::list<vpTranslationVector> & vT,
- std::list<vpColVector> & vN) ;
- static double computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane &oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- ) ;
- static double computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane *oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- ) ;
- static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d);
- // VVS
- static double computeRotation(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpHomogeneousMatrix &c2Mc1,
- int userobust) ;
- static void computeTransformation(vpColVector &x,unsigned int *ind, vpColVector &M) ;
- static bool degenerateConfiguration(vpColVector &x,unsigned int *ind) ;
- static bool degenerateConfiguration(vpColVector &x,unsigned int *ind, double threshold_area);
- static bool degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya);
- static void HartleyNormalization(unsigned int n,
- const double *x, const double *y,
- double *xn, double *yn,
- double &xg, double &yg,
- double &coef);
- static void HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
- std::vector<double> &xn, std::vector<double> &yn,
- double &xg, double &yg, double &coef);
- static void HartleyDenormalization(vpHomography &aHbn,
- vpHomography &aHb,
- double xg1, double yg1, double coef1,
- double xg2, double yg2, double coef2 ) ;
+ //! build the homography from aMb and Rb
+ static void build(vpHomography &aHb, const vpHomogeneousMatrix &aMb, const vpPlane &bP);
+
+ static void computeDisplacement(const vpHomography &aHb, const vpColVector &nd, vpRotationMatrix &aRb,
+ vpTranslationVector &atb, vpColVector &n);
+
+ static void computeDisplacement(const vpHomography &aHb, vpRotationMatrix &aRb, vpTranslationVector &atb,
+ vpColVector &n);
+
+ static void computeDisplacement(const vpHomography &H, const double x, const double y,
+ std::list<vpRotationMatrix> &vR, std::list<vpTranslationVector> &vT,
+ std::list<vpColVector> &vN);
+ static double computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane &oN,
+ vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust);
+ static double computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane *oN,
+ vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust);
+ static double computeResidual(vpColVector &x, vpColVector &M, vpColVector &d);
+ // VVS
+ static double computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1,
+ int userobust);
+ static void computeTransformation(vpColVector &x, unsigned int *ind, vpColVector &M);
+ static bool degenerateConfiguration(vpColVector &x, unsigned int *ind);
+ static bool degenerateConfiguration(vpColVector &x, unsigned int *ind, double threshold_area);
+ static bool degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya);
+ static void HartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn, double &xg,
+ double &yg, double &coef);
+ static void HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y, std::vector<double> &xn,
+ std::vector<double> &yn, double &xg, double &yg, double &coef);
+ static void HartleyDenormalization(vpHomography &aHbn, vpHomography &aHb, double xg1, double yg1, double coef1,
+ double xg2, double yg2, double coef2);
#endif // DOXYGEN_SHOULD_SKIP_THIS
@@ -394,10 +324,9 @@ class VISP_EXPORT vpHomography : public vpArray2D<double>
\deprecated You should rather use eye().
*/
//@{
- void setIdentity();
- //@}
+ void setIdentity();
+//@}
#endif
-
};
#endif
diff --git a/modules/vision/include/visp3/vision/vpKeyPoint.h b/modules/vision/include/visp3/vision/vpKeyPoint.h
index 87daf03..d269d22 100644
--- a/modules/vision/include/visp3/vision/vpKeyPoint.h
+++ b/modules/vision/include/visp3/vision/vpKeyPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,90 +38,96 @@
#ifndef __vpKeyPoint_h__
#define __vpKeyPoint_h__
-#include <algorithm> // std::transform
-#include <vector> // std::vector
-#include <stdlib.h> // srand, rand
-#include <time.h> // time
-#include <fstream> // std::ofstream
-#include <numeric> // std::accumulate
-#include <float.h> // DBL_MAX
-#include <map> // std::map
+#include <algorithm> // std::transform
+#include <float.h> // DBL_MAX
+#include <fstream> // std::ofstream
#include <limits>
+#include <map> // std::map
+#include <numeric> // std::accumulate
+#include <stdlib.h> // srand, rand
+#include <time.h> // time
+#include <vector> // std::vector
#include <visp3/core/vpConfig.h>
-#include <visp3/vision/vpBasicKeyPoint.h>
-#include <visp3/core/vpImageConvert.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpPlane.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpBasicKeyPoint.h>
#include <visp3/vision/vpPose.h>
#ifdef VISP_HAVE_MODULE_IO
-# include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpImageIo.h>
#endif
-#include <visp3/core/vpPolygon.h>
-#include <visp3/vision/vpXmlConfigParserKeyPoint.h>
#include <visp3/core/vpConvert.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/core/vpPolygon.h>
+#include <visp3/vision/vpXmlConfigParserKeyPoint.h>
// Require at least OpenCV >= 2.1.1
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
-#include <opencv2/features2d/features2d.hpp>
#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/features2d/features2d.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#if defined(VISP_HAVE_OPENCV_XFEATURES2D) // OpenCV >= 3.0.0
-# include <opencv2/xfeatures2d.hpp>
-#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
-# include <opencv2/nonfree/nonfree.hpp>
+#include <opencv2/xfeatures2d.hpp>
+#elif defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000)
+#include <opencv2/nonfree/nonfree.hpp>
#endif
#ifdef VISP_HAVE_XML2
-# include <libxml/xmlwriter.h>
+#include <libxml/xmlwriter.h>
#endif
/*!
\class vpKeyPoint
\ingroup group_vision_keypoints
- \brief Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV library.
- Thus to enable this class OpenCV should be installed. Installation
- instructions are provided here https://visp.inria.fr/3rd_opencv.
+ \brief Class that allows keypoints detection (and descriptors extraction)
+and matching thanks to OpenCV library. Thus to enable this class OpenCV should
+be installed. Installation instructions are provided here
+https://visp.inria.fr/3rd_opencv.
- This class permits to use different types of detectors, extractors and matchers easily.
- So, the classical SIFT and SURF keypoints could be used, as well as ORB, FAST, (etc.) keypoints,
- depending of the version of OpenCV you use.
+ This class permits to use different types of detectors, extractors and
+matchers easily. So, the classical SIFT and SURF keypoints could be used, as
+well as ORB, FAST, (etc.) keypoints, depending of the version of OpenCV you
+use.
- \note Due to some patents, SIFT and SURF are packaged in an external module called nonfree module
- in OpenCV version before 3.0.0 and in xfeatures2d from 3.0.0. You have to check you have the
- corresponding module to use SIFT and SURF.
+ \note Due to some patents, SIFT and SURF are packaged in an external module
+called nonfree module in OpenCV version before 3.0.0 and in xfeatures2d
+from 3.0.0. You have to check you have the corresponding module to use SIFT
+and SURF.
- The goal of this class is to provide a tool to match reference keypoints from a
- reference image (or train keypoints in OpenCV terminology) and detected keypoints from a current image (or query
- keypoints in OpenCV terminology).
+ The goal of this class is to provide a tool to match reference keypoints
+from a reference image (or train keypoints in OpenCV terminology) and detected
+keypoints from a current image (or query keypoints in OpenCV terminology).
- If you supply the corresponding 3D coordinates corresponding to the 2D coordinates of the reference keypoints,
- you can also estimate the pose of the object by matching a set of detected keypoints in the current image with
- the reference keypoints.
+ If you supply the corresponding 3D coordinates corresponding to the 2D
+coordinates of the reference keypoints, you can also estimate the pose of the
+object by matching a set of detected keypoints in the current image with the
+reference keypoints.
If you use this class, the first thing you have to do is to build
- the reference keypoints by detecting keypoints in a reference image which contains the
- object to detect. Then you match keypoints detected in a current image with those detected in a reference image
- by calling matchPoint() methods.
- You can access to the lists of matched points thanks to the
- methods getMatchedPointsInReferenceImage() and
- getMatchedPointsInCurrentImage(). These two methods return a list of
- matched points. The nth element of the first list is matched with
- the nth element of the second list.
- To provide easy compatibility with OpenCV terminology, getTrainKeyPoints() give you access to the list
- of keypoints detected in train images (or reference images) and getQueryKeyPoints() give you access to the list
- of keypoints detected in a query image (or current image).
- The method getMatches() give you access to a list of cv::DMatch with the correspondence between the index of the
- train keypoints and the index of the query keypoints.
-
- The following small example shows how to use the class to do the matching between current and reference keypoints.
+ the reference keypoints by detecting keypoints in a reference image which
+contains the object to detect. Then you match keypoints detected in a current
+image with those detected in a reference image by calling matchPoint()
+methods. You can access to the lists of matched points thanks to the methods
+getMatchedPointsInReferenceImage() and getMatchedPointsInCurrentImage(). These
+two methods return a list of matched points. The nth element of the first list
+is matched with the nth element of the second list. To provide easy
+compatibility with OpenCV terminology, getTrainKeyPoints() give you access to
+the list of keypoints detected in train images (or reference images) and
+getQueryKeyPoints() give you access to the list of keypoints detected in a
+query image (or current image). The method getMatches() give you access to a
+list of cv::DMatch with the correspondence between the index of the train
+keypoints and the index of the query keypoints.
+
+ The following small example shows how to use the class to do the matching
+between current and reference keypoints.
\code
#include <visp3/core/vpConfig.h>
@@ -155,13 +162,14 @@ int main()
}
\endcode
- It is also possible to build the reference keypoints in a region of interest (ROI) of an image
- and find keypoints to match in only a part of the current image. The small following example shows how to do this:
+ It is also possible to build the reference keypoints in a region of interest
+(ROI) of an image and find keypoints to match in only a part of the current
+image. The small following example shows how to do this:
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/vision/vpKeyPoint.h>
int main()
@@ -178,8 +186,7 @@ int main()
//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++)
- {
+ for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Ireference, corners[i]);
}
@@ -193,8 +200,7 @@ int main()
//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++)
- {
+ for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Icurrent, corners[i]);
}
@@ -214,32 +220,39 @@ int main()
This class is also described in \ref tutorial-matching.
*/
-class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint {
+class VISP_EXPORT vpKeyPoint : public vpBasicKeyPoint
+{
public:
-
/*! Predefined filtering method identifier. */
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. */
+ 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. */
};
/*! 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. */
+ 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. */
};
/*! Predefined constant for training image format. */
typedef enum {
- jpgImageFormat, /*!< Save training images in JPG format. */
- pngImageFormat, /*!< Save training images in PNG format. */
- ppmImageFormat, /*!< Save training images in PPM format. */
- pgmImageFormat /*!< Save training images in PGM format. */
+ jpgImageFormat, /*!< Save training images in JPG format. */
+ pngImageFormat, /*!< Save training images in PNG format. */
+ ppmImageFormat, /*!< Save training images in PPM format. */
+ pgmImageFormat /*!< Save training images in PGM format. */
} vpImageFormatType;
/*! Predefined constant for feature detection type. */
@@ -251,21 +264,21 @@ public:
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)
+#endif
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
DETECTOR_SIFT,
DETECTOR_SURF,
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
DETECTOR_KAZE,
DETECTOR_AKAZE,
DETECTOR_AGAST,
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
DETECTOR_MSD,
- #endif
+#endif
#endif
DETECTOR_TYPE_SIZE
};
@@ -275,132 +288,151 @@ public:
#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
- #if defined(VISP_HAVE_OPENCV_NONFREE) || defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
DESCRIPTOR_SIFT,
DESCRIPTOR_SURF,
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
DESCRIPTOR_KAZE,
DESCRIPTOR_AKAZE,
- #if defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
DESCRIPTOR_DAISY,
DESCRIPTOR_LATCH,
- #endif
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#endif
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
DESCRIPTOR_VGG,
DESCRIPTOR_BoostDesc,
- #endif
+#endif
#endif
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",
- const std::string &matcherName="BruteForce-Hamming", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
+ 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<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
- const std::string &matcherName="BruteForce", const vpFilterMatchingType &filterType=ratioDistanceThreshold);
+ const std::string &matcherName = "BruteForce",
+ const vpFilterMatchingType &filterType = ratioDistanceThreshold);
unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect& rectangle);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle);
void buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoint,
- std::vector<cv::Point3f> &points3f, const bool append=false, const int class_id=-1);
+ std::vector<cv::Point3f> &points3f, const bool append = false, const int class_id = -1);
void buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoint,
const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
- const bool append=false, const int class_id=-1);
+ const bool append = false, const int class_id = -1);
- static void compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point);
+ static void compute3D(const cv::KeyPoint &candidate, const std::vector<vpPoint> &roi, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, cv::Point3f &point);
- static void compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point);
+ static void compute3D(const vpImagePoint &candidate, const std::vector<vpPoint> &roi, const vpCameraParameters &cam,
+ const vpHomogeneousMatrix &cMo, vpPoint &point);
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<cv::KeyPoint> &candidates, const std::vector<vpPolygon> &polygons,
- const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors=NULL);
+ std::vector<cv::KeyPoint> &candidates,
+ const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt,
+ std::vector<cv::Point3f> &points, cv::Mat *descriptors = NULL);
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidates, const std::vector<vpPolygon> &polygons,
- const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors=NULL);
-
- static void compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<cv::KeyPoint> &candidates, const std::vector<vpCylinder> &cylinders,
- const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
- std::vector<cv::Point3f> &points, cv::Mat *descriptors=NULL);
-
- static void compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidates, const std::vector<vpCylinder> &cylinders,
- const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
- std::vector<vpPoint> &points, cv::Mat *descriptors=NULL);
+ std::vector<vpImagePoint> &candidates,
+ const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt,
+ std::vector<vpPoint> &points, cv::Mat *descriptors = NULL);
+
+ static void
+ compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<cv::KeyPoint> &candidates, const std::vector<vpCylinder> &cylinders,
+ const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
+ std::vector<cv::Point3f> &points, cv::Mat *descriptors = NULL);
+
+ static void
+ compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
+ std::vector<vpImagePoint> &candidates, const std::vector<vpCylinder> &cylinders,
+ const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
+ std::vector<vpPoint> &points, cv::Mat *descriptors = NULL);
bool computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
- double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL);
- bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+ bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector<vpPoint> &inliers,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL);
- bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
- double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL);
+ bool computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo, std::vector<vpPoint> &inliers,
+ std::vector<unsigned int> &inlierIndex, double &elapsedTime,
+ bool (*func)(vpHomogeneousMatrix *) = NULL);
- void createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
+ void createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
+ vpImage<unsigned char> &IMatching);
void createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching);
- void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, const vpRect& rectangle=vpRect());
- void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, const cv::Mat &mask=cv::Mat());
+ void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints,
+ const vpRect &rectangle = vpRect());
+ void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, const cv::Mat &mask = cv::Mat());
void detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const vpRect& rectangle=vpRect());
+ const vpRect &rectangle = vpRect());
void detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const cv::Mat &mask=cv::Mat());
+ const cv::Mat &mask = cv::Mat());
- void detectExtractAffine(const vpImage<unsigned char> &I, std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
- std::vector<cv::Mat>& listOfDescriptors,
- std::vector<vpImage<unsigned char> > *listOfAffineI=NULL);
+ void detectExtractAffine(const vpImage<unsigned char> &I, std::vector<std::vector<cv::KeyPoint> > &listOfKeypoints,
+ std::vector<cv::Mat> &listOfDescriptors,
+ std::vector<vpImage<unsigned char> > *listOfAffineI = NULL);
- void display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size=3);
- void display(const vpImage<unsigned char> &ICurrent, unsigned int size=3, const vpColor &color=vpColor::green);
+ void display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size = 3);
+ void display(const vpImage<unsigned char> &ICurrent, unsigned int size = 3, const vpColor &color = vpColor::green);
- void displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
- unsigned int crossSize, unsigned int lineThickness=1,
- const vpColor &color=vpColor::green);
+ void displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching, unsigned int crossSize,
+ unsigned int lineThickness = 1, const vpColor &color = vpColor::green);
void displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
- const std::vector<vpImagePoint> &ransacInliers=std::vector<vpImagePoint>(), unsigned int crossSize=3,
- unsigned int lineThickness=1);
+ const std::vector<vpImagePoint> &ransacInliers = std::vector<vpImagePoint>(),
+ unsigned int crossSize = 3, unsigned int lineThickness = 1);
- void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, std::vector<cv::Point3f> *trainPoints=NULL);
- void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, std::vector<cv::Point3f> *trainPoints=NULL);
void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime, std::vector<cv::Point3f> *trainPoints=NULL);
+ std::vector<cv::Point3f> *trainPoints = NULL);
void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime, std::vector<cv::Point3f> *trainPoints=NULL);
+ std::vector<cv::Point3f> *trainPoints = NULL);
+ void extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints = NULL);
+ void extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors, double &elapsedTime,
+ std::vector<cv::Point3f> *trainPoints = NULL);
/*!
- Get the covariance matrix when estimating the pose using the Virtual Visual Servoing approach.
+ Get the covariance matrix when estimating the pose using the Virtual
+ Visual Servoing approach.
- \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
+ \warning The compute covariance flag has to be true if you want to compute
+ the covariance matrix.
\sa setCovarianceComputation
*/
- inline vpMatrix getCovarianceMatrix() const {
- if(!m_computeCovariance) {
- std::cout << "Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it." << std::endl;
+ inline vpMatrix getCovarianceMatrix() const
+ {
+ if (!m_computeCovariance) {
+ std::cout << "Warning : The covariance matrix has not been computed. "
+ "See setCovarianceComputation() to do it."
+ << std::endl;
return vpMatrix();
}
- if(m_computeCovariance && !m_useRansacVVS) {
- std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
- << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual Visual Servoing approach."
- << std::endl;
+ if (m_computeCovariance && !m_useRansacVVS) {
+ std::cout << "Warning : The covariance matrix can only be computed "
+ "with a Virtual Visual Servoing approach."
+ << std::endl
+ << "Use setUseRansacVVS(true) to choose to use a pose "
+ "estimation method based on a Virtual Visual Servoing "
+ "approach."
+ << std::endl;
return vpMatrix();
}
@@ -412,24 +444,27 @@ public:
\return The elapsed time.
*/
- inline double getDetectionTime() const {
- return m_detectionTime;
- }
+ inline double getDetectionTime() const { return m_detectionTime; }
/*!
Get the detector pointer.
\param type : Type of the detector.
- \return The detector or NULL if the type passed in parameter does not exist.
+ \return The detector or NULL if the type passed in parameter does not
+ exist.
*/
- inline cv::Ptr<cv::FeatureDetector> getDetector(const vpFeatureDetectorType &type) const {
+ inline cv::Ptr<cv::FeatureDetector> getDetector(const vpFeatureDetectorType &type) const
+ {
std::map<vpFeatureDetectorType, std::string>::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::cerr << "Internal problem with the feature type and the "
+ "corresponding name!"
+ << std::endl;
}
- std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector = m_detectors.find(it_name->second);
- if(findDetector != m_detectors.end()) {
+ std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector =
+ m_detectors.find(it_name->second);
+ if (findDetector != m_detectors.end()) {
return findDetector->second;
}
@@ -441,11 +476,13 @@ public:
Get the detector pointer.
\param name : Name of the detector.
- \return The detector or NULL if the name passed in parameter does not exist.
+ \return The detector or NULL if the name passed in parameter does not
+ exist.
*/
- inline cv::Ptr<cv::FeatureDetector> getDetector(const std::string &name) const {
+ inline cv::Ptr<cv::FeatureDetector> getDetector(const std::string &name) const
+ {
std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator findDetector = m_detectors.find(name);
- if(findDetector != m_detectors.end()) {
+ if (findDetector != m_detectors.end()) {
return findDetector->second;
}
@@ -456,33 +493,34 @@ public:
/*!
Get the feature detector name associated to the type.
*/
- inline std::map<vpFeatureDetectorType, std::string> getDetectorNames() const {
- return m_mapOfDetectorNames;
- }
+ inline std::map<vpFeatureDetectorType, std::string> getDetectorNames() const { return m_mapOfDetectorNames; }
/*!
Get the elapsed time to compute the keypoint extraction.
\return The elapsed time.
*/
- inline double getExtractionTime() const {
- return m_extractionTime;
- }
+ inline double getExtractionTime() const { return m_extractionTime; }
/*!
Get the extractor pointer.
\param type : Type of the descriptor extractor.
- \return The descriptor extractor or NULL if the name passed in parameter does not exist.
+ \return The descriptor extractor or NULL if the name passed in parameter
+ does not exist.
*/
- inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const vpFeatureDescriptorType &type) const {
+ inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const vpFeatureDescriptorType &type) const
+ {
std::map<vpFeatureDescriptorType, std::string>::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::cerr << "Internal problem with the feature type and the "
+ "corresponding name!"
+ << std::endl;
}
- std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor = m_extractors.find(it_name->second);
- if(findExtractor != m_extractors.end()) {
+ std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor =
+ m_extractors.find(it_name->second);
+ if (findExtractor != m_extractors.end()) {
return findExtractor->second;
}
@@ -494,11 +532,13 @@ public:
Get the extractor pointer.
\param name : Name of the descriptor extractor.
- \return The descriptor extractor or NULL if the name passed in parameter does not exist.
+ \return The descriptor extractor or NULL if the name passed in parameter
+ does not exist.
*/
- inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const std::string &name) const {
+ inline cv::Ptr<cv::DescriptorExtractor> getExtractor(const std::string &name) const
+ {
std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator findExtractor = m_extractors.find(name);
- if(findExtractor != m_extractors.end()) {
+ if (findExtractor != m_extractors.end()) {
return findExtractor->second;
}
@@ -509,57 +549,51 @@ public:
/*!
Get the feature descriptor extractor name associated to the type.
*/
- inline std::map<vpFeatureDescriptorType, std::string> getExtractorNames() const {
- return m_mapOfDescriptorNames;
- }
+ inline std::map<vpFeatureDescriptorType, std::string> getExtractorNames() const { return m_mapOfDescriptorNames; }
/*!
Get the image format to use when saving training images.
\return The image format.
*/
- inline vpImageFormatType getImageFormat() const {
- return m_imageFormat;
- }
+ inline vpImageFormatType getImageFormat() const { return m_imageFormat; }
/*!
Get the elapsed time to compute the matching.
\return The elapsed time.
*/
- inline double getMatchingTime() const {
- return m_matchingTime;
- }
+ inline double getMatchingTime() const { return m_matchingTime; }
/*!
Get the matcher pointer.
\return The matcher pointer.
*/
- inline cv::Ptr<cv::DescriptorMatcher> getMatcher() const {
- return m_matcher;
- }
+ inline cv::Ptr<cv::DescriptorMatcher> getMatcher() const { return m_matcher; }
/*!
- Get the list of matches (correspondences between the indexes of the detected keypoints and the train keypoints).
+ Get the list of matches (correspondences between the indexes of the
+ detected keypoints and the train keypoints).
\return The list of matches.
*/
- inline std::vector<cv::DMatch> getMatches() const {
- return m_filteredMatches;
- }
+ inline std::vector<cv::DMatch> getMatches() const { return m_filteredMatches; }
/*!
- Get the list of pairs with the correspondence between the matched query and train keypoints.
+ Get the list of pairs with the correspondence between the matched query
+ and train keypoints.
- \return The list of pairs with the correspondence between the matched query and train keypoints.
+ \return The list of pairs with the correspondence between the matched
+ query and train keypoints.
*/
- inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const {
+ inline std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > getMatchQueryToTrainKeyPoints() const
+ {
std::vector<std::pair<cv::KeyPoint, cv::KeyPoint> > matchQueryToTrainKeyPoints(m_filteredMatches.size());
for (size_t i = 0; i < m_filteredMatches.size(); i++) {
- matchQueryToTrainKeyPoints.push_back(std::pair<cv::KeyPoint, cv::KeyPoint>(
- m_queryFilteredKeyPoints[(size_t) m_filteredMatches[i].queryIdx],
- m_trainKeyPoints[(size_t) m_filteredMatches[i].trainIdx]));
+ matchQueryToTrainKeyPoints.push_back(
+ std::pair<cv::KeyPoint, cv::KeyPoint>(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx],
+ m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx]));
}
return matchQueryToTrainKeyPoints;
}
@@ -569,9 +603,7 @@ public:
\return The number of train images.
*/
- inline unsigned int getNbImages() const {
- return static_cast<unsigned int>(m_mapOfImages.size());
- }
+ inline unsigned int getNbImages() const { return static_cast<unsigned int>(m_mapOfImages.size()); }
void getObjectPoints(std::vector<cv::Point3f> &objectPoints) const;
void getObjectPoints(std::vector<vpPoint> &objectPoints) const;
@@ -581,18 +613,15 @@ public:
\return The elapsed time.
*/
- inline double getPoseTime() const {
- return m_poseTime;
- }
+ inline double getPoseTime() const { return m_poseTime; }
/*!
Get the descriptors matrix for the query keypoints.
- \return Matrix with descriptors values at each row for each query keypoints.
+ \return Matrix with descriptors values at each row for each query
+ keypoints.
*/
- inline cv::Mat getQueryDescriptors() const {
- return m_queryDescriptors;
- }
+ inline cv::Mat getQueryDescriptors() const { return m_queryDescriptors; }
void getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
void getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
@@ -602,27 +631,22 @@ public:
\return The list of Ransac inliers.
*/
- inline std::vector<vpImagePoint> getRansacInliers() const {
- return m_ransacInliers;
- }
+ inline std::vector<vpImagePoint> getRansacInliers() const { return m_ransacInliers; }
/*!
Get the list of Ransac outliers.
\return The list of Ransac outliers.
*/
- inline std::vector<vpImagePoint> getRansacOutliers() const {
- return m_ransacOutliers;
- }
+ inline std::vector<vpImagePoint> getRansacOutliers() const { return m_ransacOutliers; }
/*!
Get the train descriptors matrix.
- \return : Matrix with descriptors values at each row for each train keypoints (or reference keypoints).
+ \return : Matrix with descriptors values at each row for each train
+ keypoints (or reference keypoints).
*/
- inline cv::Mat getTrainDescriptors() const {
- return m_trainDescriptors;
- }
+ inline cv::Mat getTrainDescriptors() const { return m_trainDescriptors; }
void getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const;
void getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const;
@@ -640,46 +664,53 @@ public:
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<cv::DMatch> &matches, double &elapsedTime);
+ void match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors, std::vector<cv::DMatch> &matches,
+ double &elapsedTime);
unsigned int matchPoint(const vpImage<unsigned char> &I);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle);
bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- bool (*func)(vpHomogeneousMatrix *)=NULL, const vpRect& rectangle=vpRect());
+ bool (*func)(vpHomogeneousMatrix *) = NULL, const vpRect &rectangle = vpRect());
bool matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)=NULL,
- const vpRect& rectangle=vpRect());
+ double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *) = NULL,
+ const vpRect &rectangle = vpRect());
bool matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- const bool isPlanarObject=true, std::vector<vpImagePoint> *imPts1=NULL,
- std::vector<vpImagePoint> *imPts2=NULL, double *meanDescriptorDistance=NULL,
- double *detectionScore=NULL, const vpRect& rectangle=vpRect());
+ const bool isPlanarObject = true, std::vector<vpImagePoint> *imPts1 = NULL,
+ std::vector<vpImagePoint> *imPts2 = NULL, double *meanDescriptorDistance = NULL,
+ double *detectionScore = NULL, const vpRect &rectangle = vpRect());
bool matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- bool (*func)(vpHomogeneousMatrix *)=NULL, const vpRect& rectangle=vpRect());
+ bool (*func)(vpHomogeneousMatrix *) = NULL, const vpRect &rectangle = vpRect());
void reset();
- void saveLearningData(const std::string &filename, const bool binaryMode=false, const bool saveTrainingImages=true);
+ void saveLearningData(const std::string &filename, const bool binaryMode = false,
+ const bool saveTrainingImages = true);
/*!
- Set if the covariance matrix has to be computed in the Virtual Visual Servoing approach.
+ Set if the covariance matrix has to be computed in the Virtual Visual
+ Servoing approach.
\param flag : True if the covariance has to be computed, false otherwise.
*/
- inline void setCovarianceComputation(const bool& flag) {
+ inline void setCovarianceComputation(const bool &flag)
+ {
m_computeCovariance = flag;
- if(!m_useRansacVVS) {
- std::cout << "Warning : The covariance matrix can only be computed with a Virtual Visual Servoing approach." << std::endl
- << "Use setUseRansacVVS(true) to choose to use a pose estimation method based on a Virtual "
- "Visual Servoing approach." << std::endl;
+ if (!m_useRansacVVS) {
+ std::cout << "Warning : The covariance matrix can only be computed "
+ "with a Virtual Visual Servoing approach."
+ << std::endl
+ << "Use setUseRansacVVS(true) to choose to use a pose "
+ "estimation method based on a Virtual "
+ "Visual Servoing approach."
+ << std::endl;
}
}
@@ -688,16 +719,15 @@ public:
\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.
\param detectorType : Type of the detector.
*/
- inline void setDetector(const vpFeatureDetectorType &detectorType) {
+ inline void setDetector(const vpFeatureDetectorType &detectorType)
+ {
m_detectorNames.clear();
m_detectorNames.push_back(m_mapOfDetectorNames[detectorType]);
m_detectors.clear();
@@ -709,7 +739,8 @@ public:
\param detectorName : Name of the detector.
*/
- inline void setDetector(const std::string &detectorName) {
+ inline void setDetector(const std::string &detectorName)
+ {
m_detectorNames.clear();
m_detectorNames.push_back(detectorName);
m_detectors.clear();
@@ -718,26 +749,30 @@ public:
#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.
+ Template function to set to a \p parameterName a value for a specific
+ detector named by his \p detectorName.
\param detectorName : Name of the detector
\param parameterName : Name of the parameter
\param value : Value to set
*/
- template<typename T1, typename T2, typename T3> inline void setDetectorParameter(const T1 detectorName,
- const T2 parameterName, const T3 value) {
- if(m_detectors.find(detectorName) != m_detectors.end()) {
+ template <typename T1, typename T2, typename T3>
+ inline void setDetectorParameter(const T1 detectorName, const T2 parameterName, const T3 value)
+ {
+ if (m_detectors.find(detectorName) != m_detectors.end()) {
m_detectors[detectorName]->set(parameterName, value);
}
}
#endif
/*!
- Set and initialize a list of detectors denominated by their names \p detectorNames.
+ Set and initialize a list of detectors denominated by their names \p
+ detectorNames.
\param detectorNames : List of detector names.
*/
- inline void setDetectors(const std::vector<std::string> &detectorNames) {
+ inline void setDetectors(const std::vector<std::string> &detectorNames)
+ {
m_detectorNames.clear();
m_detectors.clear();
m_detectorNames = detectorNames;
@@ -749,7 +784,8 @@ public:
\param extractorType : Type of the descriptor extractor.
*/
- inline void setExtractor(const vpFeatureDescriptorType &extractorType) {
+ inline void setExtractor(const vpFeatureDescriptorType &extractorType)
+ {
m_extractorNames.clear();
m_extractorNames.push_back(m_mapOfDescriptorNames[extractorType]);
m_extractors.clear();
@@ -757,11 +793,13 @@ public:
}
/*!
- Set and initialize a descriptor extractor denominated by his name \p extractorName.
+ Set and initialize a descriptor extractor denominated by his name \p
+ extractorName.
\param extractorName : Name of the extractor.
*/
- inline void setExtractor(const std::string &extractorName) {
+ inline void setExtractor(const std::string &extractorName)
+ {
m_extractorNames.clear();
m_extractorNames.push_back(extractorName);
m_extractors.clear();
@@ -770,26 +808,30 @@ public:
#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.
+ Template function to set to a \p parameterName a value for a specific
+ extractor named by his \p extractorName.
\param extractorName : Name of the extractor
\param parameterName : Name of the parameter
\param value : Value to set
*/
- template<typename T1, typename T2, typename T3> inline void setExtractorParameter(const T1 extractorName,
- const T2 parameterName, const T3 value) {
- if(m_extractors.find(extractorName) != m_extractors.end()) {
+ template <typename T1, typename T2, typename T3>
+ inline void setExtractorParameter(const T1 extractorName, const T2 parameterName, const T3 value)
+ {
+ if (m_extractors.find(extractorName) != m_extractors.end()) {
m_extractors[extractorName]->set(parameterName, value);
}
}
#endif
/*!
- Set and initialize a list of extractors denominated by their names \p extractorNames.
+ Set and initialize a list of extractors denominated by their names \p
+ extractorNames.
\param extractorNames : List of extractor names.
*/
- inline void setExtractors(const std::vector<std::string> &extractorNames) {
+ inline void setExtractors(const std::vector<std::string> &extractorNames)
+ {
m_extractorNames.clear();
m_extractorNames = extractorNames;
m_extractors.clear();
@@ -801,9 +843,7 @@ public:
\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.
@@ -814,12 +854,14 @@ public:
- BruteForce-Hamming(2)
- FlannBased
- L1 and L2 norms are preferable choices for SIFT and SURF descriptors, NORM_HAMMING should be used with ORB,
- BRISK and BRIEF, NORM_HAMMING2 should be used with ORB when WTA_K==3 or 4.
+ L1 and L2 norms are preferable choices for SIFT and SURF descriptors,
+ NORM_HAMMING should be used with ORB, BRISK and BRIEF, NORM_HAMMING2
+ should be used with ORB when WTA_K==3 or 4.
\param matcherName : Name of the matcher.
*/
- inline void setMatcher(const std::string &matcherName) {
+ inline void setMatcher(const std::string &matcherName)
+ {
m_matcherName = matcherName;
initMatcher(m_matcherName);
}
@@ -827,48 +869,56 @@ public:
/*!
Set the filtering method to eliminate false matching.
The different methods are:
- - constantFactorDistanceThreshold (keep matches whose the descriptor distance is below dist_min * factor)
- - stdDistanceThreshold (keep matches whose the descriptor distance is below dist_min + standard_deviation)
- - ratioDistanceThreshold (keep matches enough discriminated: the ratio distance between the 2 best matches is below the threshold)
- - stdAndRatioDistanceThreshold (keep matches that agree with at least one of the two conditions)
+ - constantFactorDistanceThreshold (keep matches whose the descriptor
+ distance is below dist_min * factor)
+ - stdDistanceThreshold (keep matches whose the descriptor distance is
+ below dist_min + standard_deviation)
+ - ratioDistanceThreshold (keep matches enough discriminated: the ratio
+ distance between the 2 best matches is below the threshold)
+ - stdAndRatioDistanceThreshold (keep matches that agree with at least
+ one of the two conditions)
- noFilterMatching
\param filterType : Type of the filtering method
*/
- inline void setFilterMatchingType(const vpFilterMatchingType &filterType) {
+ inline void setFilterMatchingType(const vpFilterMatchingType &filterType)
+ {
m_filterType = filterType;
- //Use k-nearest neighbors (knn) to retrieve the two best matches for a keypoint
- //So this is useful only for ratioDistanceThreshold method
- if(filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
+ // Use k-nearest neighbors (knn) to retrieve the two best matches for a
+ // keypoint So this is useful only for ratioDistanceThreshold method
+ if (filterType == ratioDistanceThreshold || filterType == stdAndRatioDistanceThreshold) {
m_useKnn = true;
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
- if(m_matcher != NULL && m_matcherName == "BruteForce") {
- //if a matcher is already initialized, disable the crossCheck
- //because it will not work with knnMatch
- m_matcher->set("crossCheck", false);
- }
- #endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if (m_matcher != NULL && m_matcherName == "BruteForce") {
+ // if a matcher is already initialized, disable the crossCheck
+ // because it will not work with knnMatch
+ m_matcher->set("crossCheck", false);
+ }
+#endif
} else {
m_useKnn = false;
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
- if(m_matcher != NULL && m_matcherName == "BruteForce") {
- //if a matcher is already initialized, set the crossCheck mode if necessary
- m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
- }
- #endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
+ if (m_matcher != NULL && m_matcherName == "BruteForce") {
+ // if a matcher is already initialized, set the crossCheck mode if
+ // necessary
+ m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
+ }
+#endif
}
}
/*!
- Set the factor value for the filtering method: constantFactorDistanceThreshold.
+ Set the factor value for the filtering method:
+ constantFactorDistanceThreshold.
\param factor : Factor value
*/
- inline void setMatchingFactorThreshold(const double factor) {
- if(factor > 0.0) {
+ inline void setMatchingFactorThreshold(const double factor)
+ {
+ if (factor > 0.0) {
m_matchingFactorThreshold = factor;
} else {
throw vpException(vpException::badValue, "The factor must be positive.");
@@ -880,8 +930,9 @@ public:
\param ratio : Ratio value (]0 ; 1])
*/
- inline void setMatchingRatioThreshold(const double ratio) {
- if(ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits<double>::epsilon())) {
+ inline void setMatchingRatioThreshold(const double ratio)
+ {
+ if (ratio > 0.0 && (ratio < 1.0 || std::fabs(ratio - 1.0) < std::numeric_limits<double>::epsilon())) {
m_matchingRatioThreshold = ratio;
} else {
throw vpException(vpException::badValue, "The ratio must be in the interval ]0 ; 1].");
@@ -889,12 +940,15 @@ public:
}
/*!
- Set the percentage value for defining the cardinality of the consensus group.
+ Set the percentage value for defining the cardinality of the consensus
+ group.
\param percentage : Percentage value (]0 ; 100])
*/
- inline void setRansacConsensusPercentage(const double percentage) {
- if(percentage > 0.0 && (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits<double>::epsilon())) {
+ inline void setRansacConsensusPercentage(const double percentage)
+ {
+ if (percentage > 0.0 &&
+ (percentage < 100.0 || std::fabs(percentage - 100.0) < std::numeric_limits<double>::epsilon())) {
m_ransacConsensusPercentage = percentage;
} else {
throw vpException(vpException::badValue, "The percentage must be in the interval ]0 ; 100].");
@@ -902,12 +956,14 @@ public:
}
/*!
- Set the maximum number of iterations for the Ransac pose estimation method.
+ Set the maximum number of iterations for the Ransac pose estimation
+ method.
\param nbIter : Maximum number of iterations for the Ransac
*/
- inline void setRansacIteration(const int nbIter) {
- if(nbIter > 0) {
+ inline void setRansacIteration(const int nbIter)
+ {
+ if (nbIter > 0) {
m_nbRansacIterations = nbIter;
} else {
throw vpException(vpException::badValue, "The number of iterations must be greater than zero.");
@@ -915,15 +971,20 @@ public:
}
/*!
- Set the maximum reprojection error (in pixel) to determine if a point is an inlier or not.
+ Set the maximum reprojection error (in pixel) to determine if a point is
+ an inlier or not.
- \param reprojectionError : Maximum reprojection error in pixel (used by OpenCV function)
+ \param reprojectionError : Maximum reprojection error in pixel (used by
+ OpenCV function)
*/
- inline void setRansacReprojectionError(const double reprojectionError) {
- if(reprojectionError > 0.0) {
+ inline void setRansacReprojectionError(const double reprojectionError)
+ {
+ if (reprojectionError > 0.0) {
m_ransacReprojectionError = reprojectionError;
} else {
- throw vpException(vpException::badValue, "The Ransac reprojection threshold must be positive as we deal with distance.");
+ throw vpException(vpException::badValue, "The Ransac reprojection "
+ "threshold must be positive "
+ "as we deal with distance.");
}
}
@@ -932,8 +993,9 @@ public:
\param minCount : Minimum number of inlier for the consensus
*/
- inline void setRansacMinInlierCount(const int minCount) {
- if(minCount > 0) {
+ inline void setRansacMinInlierCount(const int minCount)
+ {
+ if (minCount > 0) {
m_nbRansacMinInlierCount = minCount;
} else {
throw vpException(vpException::badValue, "The minimum number of inliers must be greater than zero.");
@@ -941,12 +1003,14 @@ public:
}
/*!
- Set the maximum error (in meter) to determine if a point is an inlier or not.
+ Set the maximum error (in meter) to determine if a point is an inlier or
+ not.
\param threshold : Maximum error in meter for ViSP function
*/
- inline void setRansacThreshold(const double threshold) {
- if(threshold > 0.0) {
+ inline void setRansacThreshold(const double threshold)
+ {
+ if (threshold > 0.0) {
m_ransacThreshold = threshold;
} else {
throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
@@ -954,26 +1018,30 @@ public:
}
/*!
- Set if multiple affine transformations must be used to detect and extract keypoints.
+ Set if multiple affine transformations must be used to detect and extract
+ keypoints.
- \param useAffine : True to use multiple affine transformations, false otherwise
+ \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)
/*!
- Set if cross check method must be used to eliminate some false matches with a brute-force matching method.
+ Set if cross check method must be used to eliminate some false matches
+ with a brute-force matching method.
\param useCrossCheck : True to use cross check, false otherwise
*/
- inline void setUseBruteForceCrossCheck(const bool useCrossCheck) {
- //Only available with BruteForce and with k=1 (i.e not used with a ratioDistanceThreshold method)
- if(m_matcher != NULL && !m_useKnn && m_matcherName == "BruteForce") {
+ inline void setUseBruteForceCrossCheck(const bool useCrossCheck)
+ {
+ // Only available with BruteForce and with k=1 (i.e not used with a
+ // ratioDistanceThreshold method)
+ if (m_matcher != NULL && !m_useKnn && m_matcherName == "BruteForce") {
m_matcher->set("crossCheck", useCrossCheck);
- } else if(m_matcher != NULL && m_useKnn && m_matcherName == "BruteForce") {
- std::cout << "Warning, you try to set the crossCheck parameter with a BruteForce matcher but knn is enabled";
+ } 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;
}
}
@@ -982,65 +1050,70 @@ public:
/*!
Set if we want to match the train keypoints to the query keypoints.
- \param useMatchTrainToQuery : True to match the train keypoints to the query keypoints
+ \param useMatchTrainToQuery : True to match the train keypoints to the
+ query keypoints
*/
- inline void setUseMatchTrainToQuery(const bool useMatchTrainToQuery) {
+ inline void setUseMatchTrainToQuery(const bool useMatchTrainToQuery)
+ {
m_useMatchTrainToQuery = useMatchTrainToQuery;
}
/*!
- Set the flag to choose between a percentage value of inliers for the cardinality of the consensus group
- or a minimum number.
+ Set the flag to choose between a percentage value of inliers for the
+ cardinality of the consensus group or a minimum number.
- \param usePercentage : True to a percentage ratio of inliers, otherwise use a specified number of inliers
+ \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 function.
+ Set the flag to choose between the OpenCV or ViSP Ransac pose estimation
+ function.
- \param ransacVVS : True to use ViSP function, otherwise use OpenCV function
+ \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 to the same train keypoints.
+ Set the flag to filter matches where multiple query keypoints are matched
+ to the same train keypoints.
\param singleMatchFilter : True to use the single match filter.
*/
- inline void setUseSingleMatchFilter(const bool singleMatchFilter) {
- m_useSingleMatchFilter = singleMatchFilter;
- }
+ inline void setUseSingleMatchFilter(const bool singleMatchFilter) { m_useSingleMatchFilter = singleMatchFilter; }
private:
- //! If true, compute covariance matrix if the user select the pose estimation method using ViSP
+ //! If true, compute covariance matrix if the user select the pose
+ //! estimation method using ViSP
bool m_computeCovariance;
//! Covariance matrix
vpMatrix m_covarianceMatrix;
//! Current id associated to the training image used for the learning.
int m_currentImageId;
- //! Method (based on descriptor distances) to decide if the object is present or not.
+ //! Method (based on descriptor distances) to decide if the object is
+ //! present or not.
vpDetectionMethodType m_detectionMethod;
//! Detection score to decide if the object is present or not.
double m_detectionScore;
- //! Detection threshold based on average of descriptor distances to decide if the object is present or not.
+ //! Detection threshold based on average of descriptor distances to decide
+ //! if the object is present or not.
double m_detectionThreshold;
//! Elapsed time to detect keypoints.
double m_detectionTime;
//! List of detector names.
std::vector<std::string> m_detectorNames;
- //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) detectors,
+ //! Map of smart reference-counting pointers (similar to shared_ptr in
+ //! Boost) detectors,
// with a key based upon the detector name.
std::map<std::string, cv::Ptr<cv::FeatureDetector> > m_detectors;
//! Elapsed time to extract descriptors for the detected keypoints.
double m_extractionTime;
//! List of extractor name.
std::vector<std::string> m_extractorNames;
- //! Map of smart reference-counting pointers (similar to shared_ptr in Boost) extractors,
+ //! Map of smart reference-counting pointers (similar to shared_ptr in
+ //! Boost) extractors,
// with a key based upon the extractor name.
std::map<std::string, cv::Ptr<cv::DescriptorExtractor> > m_extractors;
//! List of filtered matches between the detected and the trained keypoints.
@@ -1049,17 +1122,21 @@ private:
vpFilterMatchingType m_filterType;
//! Image format to use when saving the training images
vpImageFormatType m_imageFormat;
- //! List of k-nearest neighbors for each detected keypoints (if the method chosen is based upon on knn).
+ //! List of k-nearest neighbors for each detected keypoints (if the method
+ //! chosen is based upon on knn).
std::vector<std::vector<cv::DMatch> > m_knnMatches;
//! Map descriptor enum type to string.
std::map<vpFeatureDescriptorType, std::string> m_mapOfDescriptorNames;
//! Map detector enum type to string.
std::map<vpFeatureDetectorType, std::string> m_mapOfDetectorNames;
- //! Map of image id to know to which training image is related a training keypoints.
+ //! Map of image id to know to which training image is related a training
+ //! keypoints.
std::map<int, int> m_mapOfImageId;
- //! Map of images to have access to the image buffer according to his image id.
+ //! Map of images to have access to the image buffer according to his image
+ //! id.
std::map<int, vpImage<unsigned char> > m_mapOfImages;
- //! Smart reference-counting pointer (similar to shared_ptr in Boost) of descriptor matcher (e.g. BruteForce or FlannBased).
+ //! Smart reference-counting pointer (similar to shared_ptr in Boost) of
+ //! descriptor matcher (e.g. BruteForce or FlannBased).
cv::Ptr<cv::DescriptorMatcher> m_matcher;
//! Name of the matcher.
std::string m_matcherName;
@@ -1077,58 +1154,69 @@ private:
int m_nbRansacIterations;
//! Minimum number of inliers for the Ransac method.
int m_nbRansacMinInlierCount;
- //! List of 3D points (in the object frame) filtered after the matching to compute the pose.
+ //! List of 3D points (in the object frame) filtered after the matching to
+ //! compute the pose.
std::vector<cv::Point3f> m_objectFilteredPoints;
//! Elapsed time to compute the pose.
double m_poseTime;
- /*! Matrix of descriptors (each row contains the descriptors values for each keypoints
- detected in the current image). */
+ /*! Matrix of descriptors (each row contains the descriptors values for each
+ keypoints detected in the current image). */
cv::Mat m_queryDescriptors;
//! List of detected keypoints filtered after the matching.
std::vector<cv::KeyPoint> m_queryFilteredKeyPoints;
//! List of keypoints detected in the current image.
std::vector<cv::KeyPoint> m_queryKeyPoints;
- //! Percentage value to determine the number of inliers for the Ransac method.
+ //! Percentage value to determine the number of inliers for the Ransac
+ //! method.
double m_ransacConsensusPercentage;
//! List of inliers.
std::vector<vpImagePoint> m_ransacInliers;
//! List of outliers.
std::vector<vpImagePoint> m_ransacOutliers;
- //! Maximum reprojection error (in pixel for the OpenCV method) to decide if a point is an inlier or not.
+ //! Maximum reprojection error (in pixel for the OpenCV method) to decide if
+ //! a point is an inlier or not.
double m_ransacReprojectionError;
- //! Maximum error (in meter for the ViSP method) to decide if a point is an inlier or not.
+ //! Maximum error (in meter for the ViSP method) to decide if a point is an
+ //! inlier or not.
double m_ransacThreshold;
- //! Matrix of descriptors (each row contains the descriptors values for each keypoints
- //detected in the train images).
+ //! Matrix of descriptors (each row contains the descriptors values for each
+ //! keypoints
+ // detected in the train images).
cv::Mat m_trainDescriptors;
//! List of keypoints detected in the train images.
std::vector<cv::KeyPoint> m_trainKeyPoints;
- //! List of 3D points (in the object frame) corresponding to the train keypoints.
+ //! List of 3D points (in the object frame) corresponding to the train
+ //! keypoints.
std::vector<cv::Point3f> m_trainPoints;
- //! List of 3D points in vpPoint format (in the object frame) corresponding to the train keypoints.
+ //! List of 3D points in vpPoint format (in the object frame) corresponding
+ //! to the train keypoints.
std::vector<vpPoint> m_trainVpPoints;
- //! If true, use multiple affine transformations to cober the 6 affine parameters
+ //! If true, use multiple affine transformations to cober the 6 affine
+ //! parameters
bool m_useAffineDetection;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
- //! If true, some false matches will be eliminate by keeping only pairs (i,j) such that for i-th
- //! query descriptor the j-th descriptor in the matcher’s collection is the nearest and vice versa.
+ //! If true, some false matches will be eliminate by keeping only pairs
+ //! (i,j) such that for i-th query descriptor the j-th descriptor in the
+ //! matcher’s collection is the nearest and vice versa.
bool m_useBruteForceCrossCheck;
#endif
- //! Flag set if a percentage value is used to determine the number of inliers for the Ransac method.
+ //! Flag set if a percentage value is used to determine the number of
+ //! inliers for the Ransac method.
bool m_useConsensusPercentage;
//! Flag set if a knn matching method must be used.
bool m_useKnn;
- //! Flag set if we want to match the train keypoints to the query keypoints, useful when there is only one train image
- //! because it reduces the number of possible false matches (by default it is the inverse because normally there are multiple
- //! train images of different views of the object)
+ //! Flag set if we want to match the train keypoints to the query keypoints,
+ //! useful when there is only one train image because it reduces the number
+ //! of possible false matches (by default it is the inverse because normally
+ //! there are multiple train images of different views of the object)
bool m_useMatchTrainToQuery;
//! Flag set if a Ransac VVS pose estimation must be used.
bool m_useRansacVVS;
- //! If true, keep only pairs of keypoints where each train keypoint is matched to a single query keypoint
+ //! If true, keep only pairs of keypoints where each train keypoint is
+ //! matched to a single query keypoint
bool m_useSingleMatchFilter;
-
- void affineSkew(double tilt, double phi, cv::Mat& img, cv::Mat& mask, cv::Mat& Ai);
+ void affineSkew(double tilt, double phi, cv::Mat &img, cv::Mat &mask, cv::Mat &Ai);
double computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est);
@@ -1144,43 +1232,47 @@ private:
void initFeatureNames();
- inline size_t myKeypointHash(const cv::KeyPoint &kp) {
+ inline size_t myKeypointHash(const cv::KeyPoint &kp)
+ {
size_t _Val = 2166136261U, scale = 16777619U;
Cv32suf u;
- u.f = kp.pt.x; _Val = (scale * _Val) ^ u.u;
- u.f = kp.pt.y; _Val = (scale * _Val) ^ u.u;
- u.f = kp.size; _Val = (scale * _Val) ^ u.u;
- //As the keypoint angle can be computed for certain type of keypoint only when extracting
- //the corresponding descriptor, the angle field is not taking into account for the hash
-// u.f = kp.angle; _Val = (scale * _Val) ^ u.u;
- u.f = kp.response; _Val = (scale * _Val) ^ u.u;
- _Val = (scale * _Val) ^ ((size_t) kp.octave);
- _Val = (scale * _Val) ^ ((size_t) kp.class_id);
+ u.f = kp.pt.x;
+ _Val = (scale * _Val) ^ u.u;
+ u.f = kp.pt.y;
+ _Val = (scale * _Val) ^ u.u;
+ u.f = kp.size;
+ _Val = (scale * _Val) ^ u.u;
+ // As the keypoint angle can be computed for certain type of keypoint only
+ // when extracting the corresponding descriptor, the angle field is not
+ // taking into account for the hash
+ // u.f = kp.angle; _Val = (scale * _Val) ^ u.u;
+ u.f = kp.response;
+ _Val = (scale * _Val) ^ u.u;
+ _Val = (scale * _Val) ^ ((size_t)kp.octave);
+ _Val = (scale * _Val) ^ ((size_t)kp.class_id);
return _Val;
}
-
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
/*
* Adapts a detector to detect points over multiple levels of a Gaussian
* pyramid. Useful for detectors that are not inherently scaled.
* From OpenCV 2.4.11 source code.
*/
- class PyramidAdaptedFeatureDetector: public cv::FeatureDetector {
+ class PyramidAdaptedFeatureDetector : public cv::FeatureDetector
+ {
public:
// maxLevel - The 0-based index of the last pyramid layer
- PyramidAdaptedFeatureDetector(const cv::Ptr<cv::FeatureDetector>& detector, int maxLevel = 2);
+ PyramidAdaptedFeatureDetector(const cv::Ptr<cv::FeatureDetector> &detector, int maxLevel = 2);
// TODO implement read/write
virtual bool empty() const;
protected:
- virtual void detect(cv::InputArray image,
- CV_OUT std::vector<cv::KeyPoint>& keypoints, cv::InputArray mask =
- cv::noArray());
- virtual void detectImpl(const cv::Mat& image,
- std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask =
- cv::Mat()) const;
+ virtual void detect(cv::InputArray image, CV_OUT std::vector<cv::KeyPoint> &keypoints,
+ cv::InputArray mask = cv::noArray());
+ virtual void detectImpl(const cv::Mat &image, std::vector<cv::KeyPoint> &keypoints,
+ const cv::Mat &mask = cv::Mat()) const;
cv::Ptr<cv::FeatureDetector> detector;
int maxLevel;
@@ -1188,38 +1280,37 @@ private:
/*
* A class filters a vector of keypoints.
- * Because now it is difficult to provide a convenient interface for all usage scenarios of the keypoints filter class,
- * it has only several needed by now static methods.
+ * Because now it is difficult to provide a convenient interface for all
+ * usage scenarios of the keypoints filter class, it has only several needed
+ * by now static methods.
*/
- class KeyPointsFilter {
+ class KeyPointsFilter
+ {
public:
- KeyPointsFilter() {
- }
+ KeyPointsFilter() {}
/*
* Remove keypoints within borderPixels of an image edge.
*/
- static void runByImageBorder(std::vector<cv::KeyPoint>& keypoints,
- cv::Size imageSize, int borderSize);
+ static void runByImageBorder(std::vector<cv::KeyPoint> &keypoints, cv::Size imageSize, int borderSize);
/*
* Remove keypoints of sizes out of range.
*/
- static void runByKeypointSize(std::vector<cv::KeyPoint>& keypoints,
- float minSize, float maxSize = FLT_MAX);
+ static void runByKeypointSize(std::vector<cv::KeyPoint> &keypoints, float minSize, float maxSize = FLT_MAX);
/*
* Remove keypoints from some image by mask for pixels of this image.
*/
- static void runByPixelsMask(std::vector<cv::KeyPoint>& keypoints,
- const cv::Mat& mask);
+ static void runByPixelsMask(std::vector<cv::KeyPoint> &keypoints, const cv::Mat &mask);
/*
* Remove duplicated keypoints.
*/
- static void removeDuplicated(std::vector<cv::KeyPoint>& keypoints);
+ static void removeDuplicated(std::vector<cv::KeyPoint> &keypoints);
/*
- * Retain the specified number of the best keypoints (according to the response)
+ * Retain the specified number of the best keypoints (according to the
+ * response)
*/
- static void retainBest(std::vector<cv::KeyPoint>& keypoints, int npoints);
+ static void retainBest(std::vector<cv::KeyPoint> &keypoints, int npoints);
};
#endif
diff --git a/modules/vision/include/visp3/vision/vpKeyPointSurf.h b/modules/vision/include/visp3/vision/vpKeyPointSurf.h
index 9741e2a..f68445a 100644
--- a/modules/vision/include/visp3/vision/vpKeyPointSurf.h
+++ b/modules/vision/include/visp3/vision/vpKeyPointSurf.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpKeyPointSurf_H
#define vpKeyPointSurf_H
@@ -51,18 +51,18 @@
#include <list>
#include <vector>
-#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
+#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 1.1.0 < 3.0.0
-# include <opencv2/core/core.hpp>
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/legacy/compat.hpp>
-# include <opencv2/nonfree/nonfree.hpp>
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400) // Require opencv >= 1.1.0 < 3.0.0
+#include <opencv2/core/core.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/legacy/compat.hpp>
+#include <opencv2/nonfree/nonfree.hpp>
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/features2d/features2d.hpp>
+#include <opencv2/features2d/features2d.hpp>
#elif (VISP_HAVE_OPENCV_VERSION >= 0x010100) // Require opencv >= 1.1.0
-# include <cxcore.h>
-# include <cv.h>
+#include <cv.h>
+#include <cxcore.h>
#endif
/*!
@@ -105,8 +105,7 @@
int main()
{
#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
+ vpImage<unsigned char> Ireference; vpImage<unsigned char> Icurrent;
vpKeyPointSurf surf;
// First grab the reference image Ireference
@@ -133,23 +132,20 @@ int main()
\code
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/vision/vpKeyPointSurf.h>
int main()
{
#if defined (VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
- vpImage<unsigned char> Ireference;
- vpImage<unsigned char> Icurrent;
+ vpImage<unsigned char> Ireference; vpImage<unsigned char> Icurrent;
vpKeyPointSurf surf;
//First grab the reference image Ireference
//Select a part of the image by clincking on two points which define a rectangle
- vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
+ vpImagePoint corners[2]; for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Ireference, corners[i]);
}
@@ -163,8 +159,7 @@ int main()
//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++)
- {
+ for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Icurrent, corners[i]);
}
@@ -187,115 +182,108 @@ int main()
class VISP_EXPORT vpKeyPointSurf : public vpBasicKeyPoint
{
- public:
- /*!
- This enumerate enables to set the detail level of the
- descriptors.
- */
- typedef enum
- {
- basicDescriptor, /*<! basicDescriptor means that the descriptors are
- composed by 64 elements floating-point vector. */
- extendedDescriptor /*<! Means that the descriptors are composed by
- 128 elements floating-point vector. */
- } vpDescriptorType;
-
- public:
- vpKeyPointSurf();
-
- virtual ~vpKeyPointSurf();
-
- unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle);
- unsigned int matchPoint(const vpImage<unsigned char> &I);
- unsigned int matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect& rectangle);
- void display(const vpImage<unsigned char> &Iref,
- const vpImage<unsigned char> &Icurrent, unsigned int size=3);
- void display(const vpImage<unsigned char> &Icurrent, unsigned int size=3,
- const vpColor &color=vpColor::green);
- std::list<int*>* matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList);
- float* getDescriptorReferencePoint (const int index);
- int getLaplacianReferencePoint (const int index);
- void getDescriptorParamReferencePoint (const int index, int& size, float& dir);
- /*!
-
- Sets the value of the hessian threhold. Note that during the
- computation of the hessian for each potential points, only the
- points which have a hessian value higher than the threshold are
- keeped. Fore more details about the threshold see the article
- Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
- Robust Features", Proceedings of the 9th European Conference on
- Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
- 2006.
-
- \param hessian_threshold : Desired hessian threshold value.
- */
- void setHessianThreshold (double hessian_threshold) {
- this->hessianThreshold = hessian_threshold;
- params = cvSURFParams(this->hessianThreshold, this->descriptorType);
- } ;
-
- /*!
-
- Sets the type of descriptors to use.
-
- \param descriptor_type : Type of descriptor to use.
- */
- void setDescriptorType (vpDescriptorType descriptor_type) {
- this->descriptorType = descriptor_type;
- params = cvSURFParams(this->hessianThreshold, this->descriptorType);
- } ;
-
- /*!
- Gets the value of the hessian threhold.
-
- \return the hessian threshold value.
- */
- double getHessianThreshold () {return hessianThreshold;}
-
- /*!
- Gets the type of descriptor used.
-
- \return the type of descriptor used.
- */
- vpDescriptorType getDescriptorType () {return descriptorType;}
-
- private:
- void init();
-
- private:
- //OpenCV Parameters
- CvMemStorage* storage;
- CvSURFParams params;
- CvMemStorage* storage_cur;
-
- CvSeq* image_keypoints;
- CvSeq* image_descriptors;
-
- CvSeq* ref_keypoints;
- CvSeq* ref_descriptors;
-
- /*!
- only features with keypoint.hessian larger than that are extracted.
- Good default value is ~300-500 (can depend on the average
- local contrast and sharpness of the image).
- User can further filter out some features based on their hessian values
- and other characteristics.
- */
- double hessianThreshold;
- vpDescriptorType descriptorType;
+public:
+ /*!
+ This enumerate enables to set the detail level of the
+ descriptors.
+ */
+ typedef enum {
+ basicDescriptor, /*<! basicDescriptor means that the descriptors are
+ composed by 64 elements floating-point vector. */
+ extendedDescriptor /*<! Means that the descriptors are composed by
+ 128 elements floating-point vector. */
+ } vpDescriptorType;
+
+public:
+ vpKeyPointSurf();
+
+ virtual ~vpKeyPointSurf();
+
+ unsigned int buildReference(const vpImage<unsigned char> &I);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle);
+ unsigned int matchPoint(const vpImage<unsigned char> &I);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ unsigned int matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle);
+ void display(const vpImage<unsigned char> &Iref, const vpImage<unsigned char> &Icurrent, unsigned int size = 3);
+ void display(const vpImage<unsigned char> &Icurrent, unsigned int size = 3, const vpColor &color = vpColor::green);
+ std::list<int *> *matchPoint(std::list<float *> descriptorList, std::list<int> laplacianList);
+ float *getDescriptorReferencePoint(const int index);
+ int getLaplacianReferencePoint(const int index);
+ void getDescriptorParamReferencePoint(const int index, int &size, float &dir);
+ /*!
+
+ Sets the value of the hessian threhold. Note that during the
+ computation of the hessian for each potential points, only the
+ points which have a hessian value higher than the threshold are
+ keeped. Fore more details about the threshold see the article
+ Herbert Bay, Tinne Tuytelaars and Luc Van Gool "SURF: Speeded Up
+ Robust Features", Proceedings of the 9th European Conference on
+ Computer Vision, Springer LNCS volume 3951, part 1, pp 404--417,
+ 2006.
+
+ \param hessian_threshold : Desired hessian threshold value.
+ */
+ void setHessianThreshold(double hessian_threshold)
+ {
+ this->hessianThreshold = hessian_threshold;
+ params = cvSURFParams(this->hessianThreshold, this->descriptorType);
+ };
-};
+ /*!
+ Sets the type of descriptors to use.
-#endif
+ \param descriptor_type : Type of descriptor to use.
+ */
+ void setDescriptorType(vpDescriptorType descriptor_type)
+ {
+ this->descriptorType = descriptor_type;
+ params = cvSURFParams(this->hessianThreshold, this->descriptorType);
+ };
+
+ /*!
+ Gets the value of the hessian threhold.
+
+ \return the hessian threshold value.
+ */
+ double getHessianThreshold() { return hessianThreshold; }
+
+ /*!
+ Gets the type of descriptor used.
+
+ \return the type of descriptor used.
+ */
+ vpDescriptorType getDescriptorType() { return descriptorType; }
+
+private:
+ void init();
+
+private:
+ // OpenCV Parameters
+ CvMemStorage *storage;
+ CvSURFParams params;
+ CvMemStorage *storage_cur;
+
+ CvSeq *image_keypoints;
+ CvSeq *image_descriptors;
+
+ CvSeq *ref_keypoints;
+ CvSeq *ref_descriptors;
+
+ /*!
+ only features with keypoint.hessian larger than that are extracted.
+ Good default value is ~300-500 (can depend on the average
+ local contrast and sharpness of the image).
+ User can further filter out some features based on their hessian values
+ and other characteristics.
+ */
+ double hessianThreshold;
+ vpDescriptorType descriptorType;
+};
#endif
-
+#endif
diff --git a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h
index 2dff5ae..daecfd0 100644
--- a/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h
+++ b/modules/vision/include/visp3/vision/vpLevenbergMarquartd.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,58 +37,43 @@
*
*****************************************************************************/
-
#ifndef vpLevenbergMarquartd_h
#define vpLevenbergMarquartd_h
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpMath.h>
-#include <stdio.h>
#include <errno.h>
+#include <float.h>
#include <math.h>
+#include <stdio.h>
#include <stdlib.h>
-#include <float.h>
-
-int VISP_EXPORT
-qrsolv (int n, double *r, int ldr, int *ipvt, double *diag,
- double *qtb, double *x, double *sdiag, double *wa) ;
-double VISP_EXPORT
-enorm (const double *x, int n);
+int VISP_EXPORT qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag,
+ double *wa);
-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 enorm(const double *x, int n);
-double VISP_EXPORT
-pythag (double a, double b);
+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
-qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
- int lipvt, double *rdiag, double *acnorm, double *wa);
+double VISP_EXPORT pythag(double a, double b);
-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 qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt, int lipvt, double *rdiag, double *acnorm,
+ 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 qrsolv(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb, double *x, double *sdiag,
+ 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);
+int VISP_EXPORT lmder(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag),
+ int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double ftol, double xtol,
+ double gtol, unsigned int maxfev, double *diag, int mode, const double factor, int nprint,
+ int *info, unsigned int *nfev, int *njev, int *ipvt, double *qtf, double *wa1, double *wa2,
+ double *wa3, double *wa4);
+int VISP_EXPORT lmder1(void (*ptr_fcn)(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag),
+ int m, int n, double *x, double *fvec, double *fjac, int ldfjac, double tol, int *info,
+ int *ipvt, int lwa, double *wa);
#endif
@@ -96,4 +82,3 @@ lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* c-basic-offset: 2
* End:
*/
-
diff --git a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h
index 48709a9..ace91fe 100644
--- a/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h
+++ b/modules/vision/include/visp3/vision/vpPlanarObjectDetector.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,58 +41,58 @@
#include <visp3/core/vpConfig.h>
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) // Require opencv >= 2.1.1
-# include <opencv2/imgproc/imgproc.hpp>
-# include <opencv2/features2d/features2d.hpp>
-# include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
#elif (VISP_HAVE_OPENCV_VERSION >= 0x020000) // Require opencv >= 2.0.0
-# include <cv.h>
-# include <cvaux.hpp>
+#include <cv.h>
+#include <cvaux.hpp>
#endif
-#include <visp3/core/vpImagePoint.h>
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpRect.h>
#include <visp3/core/vpImagePoint.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/vision/vpHomography.h>
+#include <visp3/core/vpRect.h>
#include <visp3/vision/vpFernClassifier.h>
+#include <visp3/vision/vpHomography.h>
/*!
\class vpPlanarObjectDetector
\ingroup group_vision_keypoints
- \brief Class used to detect a planar surface.
+ \brief Class used to detect a planar surface.
\deprecated This class is deprecated with OpenCV 3.0.0 or more recent.
-
- This class allows to learn and recognise a surface in an image based on the
+
+ This class allows to learn and recognise a surface in an image based on the
Fern Classifier or any other point of interest matching class.
-
- It uses the class vpFernClassifier to extract points of interest in a
- reference image. These points are recorded and a classifier is trained to
- recognises them.
-
- In this class the points detected are assumed to belong to a planar surface.
- Therefore an homography can be computed between the reference image and the
- current image if the object is detected in the image.
-
- A robust method (RANSAC) is used to remove outliers in the matching process.
-
+
+ It uses the class vpFernClassifier to extract points of interest in a
+ reference image. These points are recorded and a classifier is trained to
+ recognises them.
+
+ In this class the points detected are assumed to belong to a planar surface.
+ Therefore an homography can be computed between the reference image and the
+ current image if the object is detected in the image.
+
+ A robust method (RANSAC) is used to remove outliers in the matching process.
+
The following example shows how to use the class.
-
- \code
+
+ \code
#include <visp3/core/vpConfig.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/vision/vpPlanarObjectDetector.h>
-#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Surf Fern classifier only available since 2.1.0
-int main()
+#if VISP_HAVE_OPENCV_VERSION >= 0x020000 // Surf Fern classifier only
+available since 2.1.0 int main()
{
vpImage<unsigned char> Ireference;
vpImage<unsigned char> Icurrent;
@@ -101,8 +102,7 @@ int main()
//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++)
- {
+ for (int i=0 ; i < 2 ; i++) {
vpDisplay::getClick(Ireference, corners[i]);
}
@@ -116,8 +116,7 @@ int main()
//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());
+ 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);
@@ -137,147 +136,145 @@ int main()
int main() {}
#endif
\endcode
-
-*/
-class VISP_EXPORT vpPlanarObjectDetector{
-protected:
- //! Fern Classifier used to match the points between a reference image and the current image.
+
+*/
+class VISP_EXPORT vpPlanarObjectDetector
+{
+protected:
+ //! Fern Classifier used to match the points between a reference image and
+ //! the current image.
vpFernClassifier fern;
-
+
//! Computed homography in the ViSP format.
vpHomography homography;
//! Computed homography in the OpenCV format.
cv::Mat H;
-
- //! The estimated new coordinates of the corners (reprojected using the homography).
- std::vector<cv::Point2f> dst_corners;
+
+ //! The estimated new coordinates of the corners (reprojected using the
+ //! homography).
+ std::vector<cv::Point2f> dst_corners;
//! Flag to indicate wether the last computed homography is correct or not.
bool isCorrect;
-
+
//! The corners in the reference image
std::vector<cv::Point2f> ref_corners;
-
- //! The ROI for the reference image.
+
+ //! The ROI for the reference image.
cv::Rect modelROI;
-
- //! Vector of the image point in the current image that match after the deletion of the outliers with the RANSAC.
+
+ //! Vector of the image point in the current image that match after the
+ //! deletion of the outliers with the RANSAC.
std::vector<vpImagePoint> currentImagePoints;
- //! Vector of the image point in the reference image that match after the deletion of the outliers with the RANSAC.
+ //! Vector of the image point in the reference image that match after the
+ //! deletion of the outliers with the RANSAC.
std::vector<vpImagePoint> refImagePoints;
-
- //! Minimal number of point to after the ransac needed to suppose that the homography has been correctly computed.
+
+ //! Minimal number of point to after the ransac needed to suppose that the
+ //! homography has been correctly computed.
unsigned int minNbMatching;
public:
-
- // constructors and destructors
+ // 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
+ // main functions
+ // creation of reference
unsigned int buildReference(const vpImage<unsigned char> &I);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- unsigned int height, unsigned int width);
- unsigned int buildReference(const vpImage<unsigned char> &I,
- const vpRect rectangle);
-
- // matching
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP, unsigned int height,
+ unsigned int width);
+ unsigned int buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle);
+
+ // matching
bool matchPoint(const vpImage<unsigned char> &I);
- bool matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width);
- bool matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle);
- // database management
- void recordDetector(const std::string& objectName, const std::string& dataFile);
- void load(const std::string& dataFilename, const std::string& objName);
-
-
- // display
+ bool matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width);
+ bool matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle);
+ // database management
+ void recordDetector(const std::string &objectName, const std::string &dataFile);
+ void load(const std::string &dataFilename, const std::string &objName);
+
+ // display
void display(vpImage<unsigned char> &I, bool displayKpts = false);
- void display(vpImage<unsigned char> &Iref,
- vpImage<unsigned char> &Icurrent, bool displayKpts = false);
-
+ void display(vpImage<unsigned char> &Iref, vpImage<unsigned char> &Icurrent, bool displayKpts = false);
+
/*!
Return the positions of the detected corners.
-
- \return a vector of vpImagePoint containing the position of the corners of
+
+ \return a vector of vpImagePoint containing the position of the corners of
the planar surface in the current image.
*/
std::vector<vpImagePoint> getDetectedCorners() const;
-
+
/*!
Return a reference to the classifier.
-
+
\return The fern classifier.
*/
- vpFernClassifier& getFernClassifier() {return this->fern;}
-
+ vpFernClassifier &getFernClassifier() { return this->fern; }
+
/*!
- Return the computed homography between the reference image and the current
+ Return the computed homography between the reference image and the current
image.
-
+
\param _H : The computed homography.
*/
- inline void getHomography(vpHomography& _H) const { _H = this->homography;}
-
+ inline void getHomography(vpHomography &_H) const { _H = this->homography; }
+
/*!
Return the number of reference points
-
+
\return Number of reference points.
*/
- inline unsigned int getNbRefPoints() {return (unsigned int)currentImagePoints.size() ;}
-
+ inline unsigned int getNbRefPoints() { return (unsigned int)currentImagePoints.size(); }
+
/*!
Get the i-th reference point.
-
+
\throw vpException if _i is out if bound.
-
+
\param _i : index of the point to get
- \param _imPoint : image point returned by the
+ \param _imPoint : image point returned by the
*/
- void getReferencePoint(const unsigned int _i, vpImagePoint& _imPoint);
+ void getReferencePoint(const unsigned int _i, vpImagePoint &_imPoint);
- /*!
- Get the nth couple of reference point and current point which have been matched. These points are copied in the vpImagePoint instances given in argument.
+ /*!
+ Get the nth couple of reference point and current point which have been
+ matched. These points are copied in the vpImagePoint instances given in
+ argument.
+
+ \param _index : The index of the desired couple of reference point and
+ current point . The index must be between 0 and the number of matched
+ points - 1. \param _referencePoint : The coordinates of the desired
+ reference point are copied here. \param _currentPoint : The coordinates of
+ the desired current point are copied here.
+ */
+ void getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint, vpImagePoint &_currentPoint);
- \param _index : The index of the desired couple of reference point and current point . The index must be between 0 and the number of matched points - 1.
- \param _referencePoint : The coordinates of the desired reference point are copied here.
- \param _currentPoint : The coordinates of the desired current point are copied here.
- */
- void getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint);
-
/*!
- Set the threshold for the minimal number of point to validate the homography.
- Default value is 10.
-
+ Set the threshold for the minimal number of point to validate the
+ homography. Default value is 10.
+
\param _min : the new threshold.
*/
- void setMinNbPointValidation(const unsigned int _min){ this->minNbMatching = _min;}
-
-
+ void setMinNbPointValidation(const unsigned int _min) { this->minNbMatching = _min; }
+
/*!
- Get the threshold for the minimal number of point to validate the homography.
- Default value is 10.
-
+ Get the threshold for the minimal number of point to validate the
+ homography. Default value is 10.
+
\return : the current threshold.
*/
- unsigned int getMinNbPointValidation() const { return this->minNbMatching;}
-
-protected:
+ unsigned int getMinNbPointValidation() const { return this->minNbMatching; }
+protected:
virtual void init();
- void computeRoi(vpImagePoint* ip, const unsigned int nbpt);
- void initialiseRefCorners(const cv::Rect& _modelROI);
+ void computeRoi(vpImagePoint *ip, const unsigned int nbpt);
+ void initialiseRefCorners(const cv::Rect &_modelROI);
};
#endif
#endif /* VPPLANAROBJECTDETECTOR_H_ */
-
-
-
-
-
diff --git a/modules/vision/include/visp3/vision/vpPose.h b/modules/vision/include/visp3/vision/vpPose.h
index 24e3769..7950c9f 100644
--- a/modules/vision/include/visp3/vision/vpPose.h
+++ b/modules/vision/include/visp3/vision/vpPose.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,122 +50,143 @@
#define vpPOSE_HH
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/vision/vpHomography.h>
#ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
-# include <visp3/core/vpList.h>
+#include <visp3/core/vpList.h>
#endif
#include <visp3/core/vpThread.h>
-#include <math.h>
#include <list>
+#include <math.h>
#include <vector>
/*!
\class vpPose
\ingroup group_vision_pose
\brief Class used for pose computation from N points (pose from point only).
- Some of the algorithms implemented in this class are described in \cite Marchand16a.
+ Some of the algorithms implemented in this class are described in
+ \cite Marchand16a.
- \note It is also possible to estimate a pose from other features using vpPoseFeatures class.
+ \note It is also possible to estimate a pose from other features using
+ vpPoseFeatures class.
- To see how to use this class you can follow the \ref tutorial-pose-estimation.
+ To see how to use this class you can follow the \ref
+ tutorial-pose-estimation.
*/
-
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 */
- DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach initialized by Dementhon approach */
- LAGRANGE_VIRTUAL_VS /*!< Non linear virtual visual servoing approach initialized by Lagrange approach */
- } vpPoseMethodType;
+ 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 */
+ DEMENTHON_VIRTUAL_VS, /*!< Non linear virtual visual servoing approach
+ initialized by Dementhon approach */
+ LAGRANGE_VIRTUAL_VS /*!< Non linear virtual visual servoing approach
+ initialized by Lagrange approach */
+ } vpPoseMethodType;
enum FILTERING_RANSAC_FLAGS {
- 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_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. */
};
- unsigned int npt ; //!< Number of point used in pose computation
- std::list<vpPoint> listP ; //!< Array of point (use here class vpPoint)
+ unsigned int npt; //!< Number of point used in pose computation
+ std::list<vpPoint> listP; //!< Array of point (use here class vpPoint)
- double residual ; //!< Residual in meter
+ double residual; //!< Residual in meter
-protected :
- double lambda ;//!< parameters use for the virtual visual servoing approach
+protected:
+ double lambda; //!< parameters use for the virtual visual servoing approach
private:
- int vvsIterMax ; //! define the maximum number of iteration in VVS
+ //! define the maximum number of iteration in VVS
+ int vvsIterMax;
//! variable used in the Dementhon approach
- std::vector<vpPoint> c3d ;
+ std::vector<vpPoint> c3d;
//! Flag used to specify if the covariance matrix has to be computed or not.
bool computeCovariance;
//! Covariance matrix
vpMatrix covarianceMatrix;
-
+ //! Found a solution when there are at least a minimum number of points in
+ //! the consensus set
unsigned int ransacNbInlierConsensus;
+ //! Maximum number of iterations for the RANSAC
int ransacMaxTrials;
+ //! List of inlier points
std::vector<vpPoint> ransacInliers;
+ //! List of inlier point indexes (from the input list)
std::vector<unsigned int> ransacInlierIndex;
+ //! RANSAC threshold to consider a sample inlier or not
double ransacThreshold;
+ //! Minimal distance point to plane to consider if the point belongs or not
+ //! to the plane
double distanceToPlaneForCoplanarityTest;
+ //! RANSAC flags to remove or not degenerate points
int ransacFlags;
+ //! List of points used for the RANSAC (std::vector is contiguous whereas
+ //! std::list is a linked list)
std::vector<vpPoint> listOfPoints;
+ //! If true, use a parallel RANSAC implementation
bool useParallelRansac;
+ //! Number of threads to spawn for the parallel RANSAC implementation
int nbParallelRansacThreads;
+ //! Stop the optimization loop when the residual change (|r-r_prec|) <=
+ //! epsilon
+ double vvsEpsilon;
-
- //For parallel RANSAC
- class RansacFunctor {
+ // For parallel RANSAC
+ class RansacFunctor
+ {
public:
- RansacFunctor(const vpHomogeneousMatrix &cMo_,
- const unsigned int ransacNbInlierConsensus_, const int ransacMaxTrials_,
- const double ransacThreshold_, const unsigned int initial_seed_,
+ 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<vpPoint> &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_ransacThreshold(ransacThreshold_) {
+ 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_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() {
+ 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()
+ {
}
- void operator()() {
- m_foundSolution = poseRansacImpl();
- }
+ void operator()() { m_foundSolution = poseRansacImpl(); }
// Access the return value.
- bool getResult() const {
- return m_foundSolution;
- }
+ bool getResult() const { return m_foundSolution; }
- std::vector<unsigned int> getBestConsensus() const {
- return m_best_consensus;
- }
+ std::vector<unsigned int> getBestConsensus() const { return m_best_consensus; }
- vpHomogeneousMatrix getEstimatedPose() const {
- return m_cMo;
- }
+ vpHomogeneousMatrix getEstimatedPose() const { return m_cMo; }
- unsigned int getNbInliers() const {
- return m_nbInliers;
- }
+ unsigned int getNbInliers() const { return m_nbInliers; }
private:
std::vector<unsigned int> m_best_consensus;
@@ -186,163 +208,154 @@ private:
static vpThread::Return poseRansacImplThread(vpThread::Args arg);
#endif
-
protected:
- double computeResidualDementhon(const vpHomogeneousMatrix &cMo) ;
+ 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() ;
- virtual ~vpPose() ;
- void addPoint(const vpPoint& P) ;
- void addPoints(const std::vector<vpPoint>& lP);
- void clearPoint() ;
-
- 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<unsigned char> &I,
- vpCameraParameters &cam,
- vpColor col=vpColor::none) ;
- void displayModel(vpImage<vpRGBa> &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 poseLagrangeNonPlan(vpHomogeneousMatrix &cMo) ;
- void poseLowe(vpHomogeneousMatrix & cMo) ;
- bool poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *)=NULL) ;
- void poseVirtualVSrobust(vpHomogeneousMatrix & cMo) ;
- void poseVirtualVS(vpHomogeneousMatrix & cMo) ;
- void printPoint() ;
- void setDistanceToPlaneForCoplanarityTest(double d) ;
- void setLambda(double a) { lambda = a ; }
- void setVvsIterMax(int nb) { vvsIterMax = nb ; }
-
- void setRansacNbInliersToReachConsensus(const unsigned int &nbC){ ransacNbInlierConsensus = nbC; }
- void setRansacThreshold(const double &t) {
- //Test whether or not t is > 0
- if(t > 0) {
+ vpPose();
+ virtual ~vpPose();
+ void addPoint(const vpPoint &P);
+ void addPoints(const std::vector<vpPoint> &lP);
+ void clearPoint();
+
+ 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<unsigned char> &I, vpCameraParameters &cam, vpColor col = vpColor::none);
+ void displayModel(vpImage<vpRGBa> &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 poseLagrangeNonPlan(vpHomogeneousMatrix &cMo);
+ void poseLowe(vpHomogeneousMatrix &cMo);
+ bool poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *) = NULL);
+ void poseVirtualVSrobust(vpHomogeneousMatrix &cMo);
+ void poseVirtualVS(vpHomogeneousMatrix &cMo);
+ void printPoint();
+ void setDistanceToPlaneForCoplanarityTest(double d);
+ void setLambda(double a) { lambda = a; }
+ void setVvsEpsilon(const double eps)
+ {
+ if (eps >= 0) {
+ vvsEpsilon = eps;
+ } else {
+ throw vpException(vpException::badValue, "Epsilon value must be >= 0.");
+ }
+ }
+ void setVvsIterMax(int nb) { vvsIterMax = nb; }
+
+ void setRansacNbInliersToReachConsensus(const unsigned int &nbC) { ransacNbInlierConsensus = nbC; }
+ void setRansacThreshold(const double &t)
+ {
+ // Test whether or not t is > 0
+ if (t > std::numeric_limits<double>::epsilon()) {
ransacThreshold = t;
} else {
throw vpException(vpException::badValue, "The Ransac threshold must be positive as we deal with distance.");
}
}
- void setRansacMaxTrials(const int &rM){ ransacMaxTrials = rM; }
- unsigned int getRansacNbInliers() const { return (unsigned int) ransacInliers.size(); }
- std::vector<unsigned int> getRansacInlierIndex() const{ return ransacInlierIndex; }
- std::vector<vpPoint> getRansacInliers() const{ return ransacInliers; }
-
+ void setRansacMaxTrials(const int &rM) { ransacMaxTrials = rM; }
+ unsigned int getRansacNbInliers() const { return (unsigned int)ransacInliers.size(); }
+ std::vector<unsigned int> getRansacInlierIndex() const { return ransacInlierIndex; }
+ std::vector<vpPoint> getRansacInliers() const { return ransacInliers; }
+
/*!
- Set if the covaraince matrix has to be computed in the Virtual Visual Servoing approach.
+ Set if the covariance matrix has to be computed in the Virtual Visual
+ Servoing approach.
\param flag : True if the covariance has to be computed, false otherwise.
*/
- void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
-
+ void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
+
/*!
- Get the covariance matrix computed in the Virtual Visual Servoing approach.
-
- \warning The compute covariance flag has to be true if you want to compute the covariance matrix.
-
+ Get the covariance matrix computed in the Virtual Visual Servoing
+ approach.
+
+ \warning The compute covariance flag has to be true if you want to compute
+ the covariance matrix.
+
\sa setCovarianceComputation
*/
- vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
+ vpMatrix getCovarianceMatrix() const
+ {
+ if (!computeCovariance)
+ vpTRACE("Warning : The covariance matrix has not been computed. See "
+ "setCovarianceComputation() to do it.");
+
+ return covarianceMatrix;
}
/*!
Set RANSAC filtering flags.
- \param flags : Flags to use, e.g. \e setRansacFilterFlags(PREFILTER_DUPLICATE_POINTS + CHECK_DEGENERATE_POINTS).
- \sa FILTERING_RANSAC_FLAGS
+ \param flags : Flags to use, e.g. \e
+ setRansacFilterFlags(PREFILTER_DUPLICATE_POINTS +
+ CHECK_DEGENERATE_POINTS). \sa FILTERING_RANSAC_FLAGS
*/
- inline void setRansacFilterFlags(const int flags) {
- ransacFlags = flags;
- }
+ inline void setRansacFilterFlags(const int flags) { ransacFlags = flags; }
/*!
Get the number of threads for the parallel RANSAC implementation.
\sa setNbParallelRansacThreads
*/
- inline int getNbParallelRansacThreads() const {
- return nbParallelRansacThreads;
- }
+ inline int getNbParallelRansacThreads() const { return nbParallelRansacThreads; }
/*!
Set the number of threads for the parallel RANSAC implementation.
\note You have to enable the parallel version with setUseParallelRansac().
- If the number of threads is 0, the number of threads to use is automatically determined with OpenMP.
- \sa setUseParallelRansac
+ 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.
\sa setUseParallelRansac
*/
- inline bool getUseParallelRansac() const {
- return useParallelRansac;
- }
+ inline bool getUseParallelRansac() const { return useParallelRansac; }
/*!
Set if parallel RANSAC version should be used or not.
\note Need Pthread.
*/
- inline void setUseParallelRansac(const bool use) {
- useParallelRansac = use;
- }
+ inline void setUseParallelRansac(const bool use) { useParallelRansac = use; }
/*!
Get the vector of points.
\return The vector of points.
*/
- std::vector<vpPoint> getPoints() const {
+ std::vector<vpPoint> getPoints() const
+ {
std::vector<vpPoint> vectorOfPoints(listP.begin(), listP.end());
return vectorOfPoints;
}
- static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam, double size,
- vpColor col=vpColor::none) ;
- static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam, double size,
- vpColor col=vpColor::none) ;
- static double poseFromRectangle(vpPoint &p1,vpPoint &p2,
- vpPoint &p3,vpPoint &p4,
- double lx, vpCameraParameters & cam,
- vpHomogeneousMatrix & cMo) ;
-
- static int computeRansacIterations(double probability, double epsilon,
- const int sampleSize=4, int maxIterations=2000);
-
- static void findMatch(std::vector<vpPoint> &p2D,
- std::vector<vpPoint> &p3D,
- const unsigned int &numberOfInlierToReachAConsensus,
- const double &threshold,
- unsigned int &ninliers,
- std::vector<vpPoint> &listInliers,
- vpHomogeneousMatrix &cMo,
- const int &maxNbTrials = 10000);
-} ;
+ static void display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
+ vpColor col = vpColor::none);
+ static void display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
+ vpColor col = vpColor::none);
+ static double poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx,
+ vpCameraParameters &cam, vpHomogeneousMatrix &cMo);
+ static int computeRansacIterations(double probability, double epsilon, const int sampleSize = 4,
+ int maxIterations = 2000);
-#endif
+ static void findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
+ const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
+ unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
+ const int &maxNbTrials = 10000);
+};
+#endif
/*
* Local variables:
diff --git a/modules/vision/include/visp3/vision/vpPoseException.h b/modules/vision/include/visp3/vision/vpPoseException.h
index 6bdeb58..3a600bc 100644
--- a/modules/vision/include/visp3/vision/vpPoseException.h
+++ b/modules/vision/include/visp3/vision/vpPoseException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,26 +36,29 @@
*
*****************************************************************************/
-
#ifndef __vpPoseException_H
#define __vpPoseException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* Classes standards. */
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpPoseException
@@ -63,37 +67,34 @@
*/
class VISP_EXPORT vpPoseException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpPose member
- */
- enum errorCodeEnum
- {
- poseError,
- //! something is not initialized
- notInitializedError,
- //! function not implemented
- notImplementedERR,
- //! index out of range
- outOfRangeError,
- notEnoughPointError
- } ;
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpPose member
+ */
+ enum errorCodeEnum {
+ poseError,
+ //! something is not initialized
+ notInitializedError,
+ //! function not implemented
+ notImplementedERR,
+ //! index out of range
+ outOfRangeError,
+ notEnoughPointError
+ };
- public:
- vpPoseException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpPoseException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpPoseException (const int id)
- : vpException(id){ ; }
- // vpPoseException() : vpException() { ;}
+public:
+ vpPoseException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpPoseException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpPoseException(const int id) : vpException(id) { ; }
+ // vpPoseException() : vpException() { ;}
};
#endif
diff --git a/modules/vision/include/visp3/vision/vpPoseFeatures.h b/modules/vision/include/visp3/vision/vpPoseFeatures.h
index a171cb4..318bc66 100644
--- a/modules/vision/include/visp3/vision/vpPoseFeatures.h
+++ b/modules/vision/include/visp3/vision/vpPoseFeatures.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -50,23 +51,23 @@
#ifdef VISP_HAVE_MODULE_VISUAL_FEATURES
+#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpCylinder.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpException.h>
#include <visp3/core/vpExponentialMap.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpBasicFeature.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/visual_features/vpFeatureEllipse.h>
-#include <visp3/core/vpRobust.h>
#include <visp3/core/vpForwardProjection.h>
+#include <visp3/core/vpLine.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpCircle.h>
+#include <visp3/core/vpRobust.h>
#include <visp3/core/vpSphere.h>
-#include <visp3/core/vpLine.h>
-#include <visp3/core/vpCylinder.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
-#include <vector>
#include <iostream>
+#include <vector>
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
#include <tuple>
@@ -75,51 +76,37 @@
//#################################################
//## Call a function with a tuple as parameters
//#################################################
-template < unsigned int N >
-struct vpDesiredFeatureBuilderWithTuple
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args &&... args )
+template <unsigned int N> struct vpDesiredFeatureBuilderWithTuple {
+ template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
+ static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple<ArgsT...> &t,
+ Args &&... args)
{
- vpDesiredFeatureBuilderWithTuple<N-1>::buildDesiredFeatureWithTuple( feature, f, t, std::get<N-1>( t ), args... );
+ vpDesiredFeatureBuilderWithTuple<N - 1>::buildDesiredFeatureWithTuple(feature, f, t, std::get<N - 1>(t), args...);
}
};
-template <>
-struct vpDesiredFeatureBuilderWithTuple<0>
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &/* feature */,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& /* t */,
- Args&&... args )
+template <> struct vpDesiredFeatureBuilderWithTuple<0> {
+ template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
+ static void buildDesiredFeatureWithTuple(featureType & /* feature */, RetType (*f)(ArgsF...),
+ const std::tuple<ArgsT...> & /* t */, Args &&... args)
{
- f( args... );
+ f(args...);
}
};
-template <>
-struct vpDesiredFeatureBuilderWithTuple<1>
-{
- template < typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args&&... args )
+template <> struct vpDesiredFeatureBuilderWithTuple<1> {
+ template <typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args>
+ static void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsF...), const std::tuple<ArgsT...> &t,
+ Args &&... args)
{
- vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple( feature, f, t, feature, args... );
+ vpDesiredFeatureBuilderWithTuple<0>::buildDesiredFeatureWithTuple(feature, f, t, feature, args...);
}
};
-template < typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
-void buildDesiredFeatureWithTuple( featureType &feature,
- RetType (*f)(ArgsFunc...),
- std::tuple<Args...> const& t )
+template <typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
+void buildDesiredFeatureWithTuple(featureType &feature, RetType (*f)(ArgsFunc...), std::tuple<Args...> const &t)
{
- vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple( feature, f, t );
+ vpDesiredFeatureBuilderWithTuple<sizeof...(Args)>::buildDesiredFeatureWithTuple(feature, f, t);
}
//#################################################
@@ -127,51 +114,43 @@ void buildDesiredFeatureWithTuple( featureType &feature,
//## Object Mode
//#################################################
-template < unsigned int N >
-struct vpDesiredFeatureBuilderObjectWithTuple
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args &&... args )
+template <unsigned int N> struct vpDesiredFeatureBuilderObjectWithTuple {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
+ typename... Args>
+ static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
+ const std::tuple<ArgsT...> &t, Args &&... args)
{
- vpDesiredFeatureBuilderObjectWithTuple<N-1>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, std::get<N-1>( t ), args... );
+ vpDesiredFeatureBuilderObjectWithTuple<N - 1>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t,
+ std::get<N - 1>(t), args...);
}
};
-template <>
-struct vpDesiredFeatureBuilderObjectWithTuple<0>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType & /*feature*/,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& /* t */,
- Args&&... args )
+template <> struct vpDesiredFeatureBuilderObjectWithTuple<0> {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
+ typename... Args>
+ static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
+ RetType (objType::*f)(ArgsF...), const std::tuple<ArgsT...> & /* t */,
+ Args &&... args)
{
- (obj->*f)( args... );
+ (obj->*f)(args...);
}
};
-template <>
-struct vpDesiredFeatureBuilderObjectWithTuple<1>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT, typename... Args >
- static void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)( ArgsF... ),
- const std::tuple<ArgsT...>& t,
- Args&&... args )
+template <> struct vpDesiredFeatureBuilderObjectWithTuple<1> {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsF, typename... ArgsT,
+ typename... Args>
+ static void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsF...),
+ const std::tuple<ArgsT...> &t, Args &&... args)
{
- vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t, feature, args... );
+ vpDesiredFeatureBuilderObjectWithTuple<0>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t, feature, args...);
}
};
-template < typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc >
-void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
- RetType (objType::*f)(ArgsFunc...),
- std::tuple<Args...> const& t )
+template <typename objType, typename featureType, typename RetType, typename... Args, typename... ArgsFunc>
+void buildDesiredFeatureObjectWithTuple(objType *obj, featureType &feature, RetType (objType::*f)(ArgsFunc...),
+ std::tuple<Args...> const &t)
{
- vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple( obj, feature, f, t );
+ vpDesiredFeatureBuilderObjectWithTuple<sizeof...(Args)>::buildDesiredFeatureObjectWithTuple(obj, feature, f, t);
}
//#####################################################
@@ -180,56 +159,44 @@ void buildDesiredFeatureObjectWithTuple( objType *obj, featureType &feature,
//## Except the first one (must be de "BasicFeature"
//#####################################################
-template < unsigned int N >
-struct vpCurrentFeatureBuilderWithTuple
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsF...), std::tuple<ArgsTuple...>& t,
- ArgsDecomposed &&... args )
+template <unsigned int N> struct vpCurrentFeatureBuilderWithTuple {
+ template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
+ std::tuple<ArgsTuple...> &t, ArgsDecomposed &&... args)
{
- auto proj = std::get<N-1>( t );
+ auto proj = std::get<N - 1>(t);
proj.track(cMo);
- vpCurrentFeatureBuilderWithTuple<N-1>::buildCurrentFeatureWithTuple( feature, cMo, f, t, proj, args... );
+ vpCurrentFeatureBuilderWithTuple<N - 1>::buildCurrentFeatureWithTuple(feature, cMo, f, t, proj, args...);
}
};
-template <>
-struct vpCurrentFeatureBuilderWithTuple<0>
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &/*feature*/,
- const vpHomogeneousMatrix & /*cMo*/,
- RetType (*f)(ArgsF...),
- std::tuple<ArgsTuple...>&,
- ArgsDecomposed &&... args )
+template <> struct vpCurrentFeatureBuilderWithTuple<0> {
+ template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureWithTuple(featureType & /*feature*/, const vpHomogeneousMatrix & /*cMo*/,
+ RetType (*f)(ArgsF...), std::tuple<ArgsTuple...> &,
+ ArgsDecomposed &&... args)
{
- f( args... );
+ f(args...);
}
};
-template <>
-struct vpCurrentFeatureBuilderWithTuple<1>
-{
- template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsF...),
- std::tuple<ArgsTuple...>&t,
- ArgsDecomposed &&... args )
+template <> struct vpCurrentFeatureBuilderWithTuple<1> {
+ template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsF...),
+ std::tuple<ArgsTuple...> &t, ArgsDecomposed &&... args)
{
- vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple( feature, cMo, f, t, feature, args... );
+ vpCurrentFeatureBuilderWithTuple<0>::buildCurrentFeatureWithTuple(feature, cMo, f, t, feature, args...);
}
};
-template < typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
-void buildCurrentFeatureWithTuple( featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (*f)(ArgsFunc...),
- std::tuple<ArgsTuple...> &t )
+template <typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
+void buildCurrentFeatureWithTuple(featureType &feature, const vpHomogeneousMatrix &cMo, RetType (*f)(ArgsFunc...),
+ std::tuple<ArgsTuple...> &t)
{
- vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple( feature, cMo, f, t );
+ vpCurrentFeatureBuilderWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureWithTuple(feature, cMo, f, t);
}
//#####################################################
@@ -239,57 +206,49 @@ void buildCurrentFeatureWithTuple( featureType &feature,
//## Object Mode
//#####################################################
-template < unsigned int N >
-struct vpCurrentFeatureBuilderObjectWithTuple
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>& t,
- ArgsDecomposed &&... args )
+template <unsigned int N> struct vpCurrentFeatureBuilderObjectWithTuple {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
+ ArgsDecomposed &&... args)
{
- auto proj = std::get<N-1>( t );
+ auto proj = std::get<N - 1>(t);
proj.track(cMo);
- vpCurrentFeatureBuilderObjectWithTuple<N-1>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, proj, args... );
+ vpCurrentFeatureBuilderObjectWithTuple<N - 1>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, proj,
+ args...);
}
};
-template <>
-struct vpCurrentFeatureBuilderObjectWithTuple<0>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &/*feature*/,
- const vpHomogeneousMatrix &/*cMo*/,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>&,
- ArgsDecomposed &&... args )
+template <> struct vpCurrentFeatureBuilderObjectWithTuple<0> {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType & /*feature*/,
+ const vpHomogeneousMatrix & /*cMo*/, RetType (objType::*f)(ArgsF...),
+ std::tuple<ArgsTuple...> &, ArgsDecomposed &&... args)
{
- (obj->*f)( args... );
+ (obj->*f)(args...);
}
};
-template <>
-struct vpCurrentFeatureBuilderObjectWithTuple<1>
-{
- template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed, typename... ArgsF >
- static void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsF...),
- std::tuple<ArgsTuple...>&t,
- ArgsDecomposed &&... args )
+template <> struct vpCurrentFeatureBuilderObjectWithTuple<1> {
+ template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsDecomposed,
+ typename... ArgsF>
+ static void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsF...), std::tuple<ArgsTuple...> &t,
+ ArgsDecomposed &&... args)
{
- vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t, feature, args... );
+ vpCurrentFeatureBuilderObjectWithTuple<0>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f, t, feature,
+ args...);
}
};
-template < typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc >
-void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
- const vpHomogeneousMatrix &cMo,
- RetType (objType::*f)(ArgsFunc...),
- std::tuple<ArgsTuple...> &t )
+template <typename objType, typename featureType, typename RetType, typename... ArgsTuple, typename... ArgsFunc>
+void buildCurrentFeatureObjectWithTuple(objType *obj, featureType &feature, const vpHomogeneousMatrix &cMo,
+ RetType (objType::*f)(ArgsFunc...), std::tuple<ArgsTuple...> &t)
{
- vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple( obj, feature, cMo, f, t );
+ vpCurrentFeatureBuilderObjectWithTuple<sizeof...(ArgsTuple)>::buildCurrentFeatureObjectWithTuple(obj, feature, cMo, f,
+ t);
}
//#################################################
@@ -299,14 +258,15 @@ void buildCurrentFeatureObjectWithTuple( objType *obj, featureType &feature,
/*!
\class vpPoseSpecificFeature
\ingroup group_vision_pose
- \brief Class used to define specific features that could be considered in pose estimation from visual features implemented in vpPoseFeatures.
+ \brief Class used to define specific features that could be considered in
+ pose estimation from visual features implemented in vpPoseFeatures.
*/
class VISP_EXPORT vpPoseSpecificFeature
{
-public:
- vpPoseSpecificFeature(){}
+public:
+ vpPoseSpecificFeature() {}
virtual ~vpPoseSpecificFeature(){};
-
+
virtual vpColVector error() = 0;
virtual vpMatrix currentInteraction() = 0;
virtual void createDesired() = 0;
@@ -320,9 +280,10 @@ public:
/*!
\class vpPoseSpecificFeatureTemplate
\ingroup group_vision_pose
- \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
+ \brief Template class that allows to estimate a pose from all kind of
+ specific features if the compiler support C++ 11.
*/
-template< typename featureType, typename RetType, typename ...Args >
+template <typename featureType, typename RetType, typename... Args>
class vpPoseSpecificFeatureTemplate : public vpPoseSpecificFeature
{
private:
@@ -330,33 +291,29 @@ private:
featureType currentFeature;
std::tuple<Args...> *tuple;
RetType (*func_ptr)(Args...);
-
-public:
- vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&...args)
+
+public:
+ vpPoseSpecificFeatureTemplate(RetType (*f_ptr)(Args...), Args &&... args)
{
- func_ptr = f_ptr; //std::move(f_ptr);
+ func_ptr = f_ptr; // std::move(f_ptr);
tuple = new std::tuple<Args...>(args...);
}
-
- virtual ~vpPoseSpecificFeatureTemplate()
+
+ virtual ~vpPoseSpecificFeatureTemplate() { delete tuple; };
+
+ virtual void createDesired() { buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple); }
+
+ virtual vpColVector error()
{
- delete tuple;
- };
-
- virtual void createDesired(){
- buildDesiredFeatureWithTuple(desiredFeature, func_ptr, *tuple);
- }
-
- virtual vpColVector error(){
- //std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() << std::endl;
+ // std::cout << "Getting S... : " << std::get<0>(*tuple).get_s() <<
+ // std::endl;
return currentFeature.error(desiredFeature);
}
-
- virtual vpMatrix currentInteraction(){
- return currentFeature.interaction();
- }
-
- virtual void createCurrent(const vpHomogeneousMatrix &cMo){
+
+ virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
+
+ virtual void createCurrent(const vpHomogeneousMatrix &cMo)
+ {
buildCurrentFeatureWithTuple(currentFeature, cMo, func_ptr, *tuple);
}
};
@@ -369,9 +326,10 @@ public:
/*!
\class vpPoseSpecificFeatureTemplateObject
\ingroup group_vision_pose
- \brief Template class that allows to estimate a pose from all kind of specific features if the compiler support C++ 11.
+ \brief Template class that allows to estimate a pose from all kind of
+ specific features if the compiler support C++ 11.
*/
-template< typename ObjectType, typename featureType, typename RetType, typename ...Args >
+template <typename ObjectType, typename featureType, typename RetType, typename... Args>
class vpPoseSpecificFeatureTemplateObject : public vpPoseSpecificFeature
{
private:
@@ -379,47 +337,41 @@ private:
featureType currentFeature;
std::tuple<Args...> *tuple;
RetType (ObjectType::*func_ptr)(Args...);
- ObjectType* obj;
-
-public:
- vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&...args)
+ ObjectType *obj;
+
+public:
+ vpPoseSpecificFeatureTemplateObject(ObjectType *o, RetType (ObjectType::*f_ptr)(Args...), Args &&... args)
{
- func_ptr = f_ptr; //std::move(f_ptr);
+ func_ptr = f_ptr; // std::move(f_ptr);
tuple = new std::tuple<Args...>(args...);
obj = o;
}
-
- virtual ~vpPoseSpecificFeatureTemplateObject()
+
+ virtual ~vpPoseSpecificFeatureTemplateObject() { delete tuple; };
+
+ virtual void createDesired() { buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple); }
+
+ virtual vpColVector error() { return currentFeature.error(desiredFeature); }
+
+ virtual vpMatrix currentInteraction() { return currentFeature.interaction(); }
+
+ virtual void createCurrent(const vpHomogeneousMatrix &cMo)
{
- delete tuple;
- };
-
- virtual void createDesired(){
- buildDesiredFeatureObjectWithTuple(obj, desiredFeature, func_ptr, *tuple);
- }
-
- virtual vpColVector error(){
- return currentFeature.error(desiredFeature);
- }
-
- virtual vpMatrix currentInteraction(){
- return currentFeature.interaction();
- }
-
- virtual void createCurrent(const vpHomogeneousMatrix &cMo){
buildCurrentFeatureObjectWithTuple(obj, currentFeature, cMo, func_ptr, *tuple);
}
};
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
-#endif //VISP_HAVE_CPP11_COMPATIBILITY
+#endif // VISP_HAVE_CPP11_COMPATIBILITY
/*!
\class vpPoseFeatures
\brief Tools for pose computation from any feature.
\ingroup group_vision_pose
- This class allows to estimate a pose by virtual visual servoing from visual features. The features that are considered are points, segments, lines, ellipses.
- If the compiler is compatible with C++ 11, it is possible to introduce specific features that are not directly implemented in ViSP.
+ This class allows to estimate a pose by virtual visual servoing from visual
+ features. The features that are considered are points, segments, lines,
+ ellipses. If the compiler is compatible with C++ 11, it is possible to
+ introduce specific features that are not directly implemented in ViSP.
*/
class VISP_EXPORT vpPoseFeatures
{
@@ -427,171 +379,173 @@ public:
/*!
Method that will be used to estimate the pose from visual features.
*/
- typedef enum
- {
- VIRTUAL_VS, /*!< Virtual visual servoing approach. */
- ROBUST_VIRTUAL_VS /*!< Robust virtual visual servoing approach. */
- } vpPoseFeaturesMethodType;
-
+ typedef enum {
+ VIRTUAL_VS, /*!< Virtual visual servoing approach. */
+ ROBUST_VIRTUAL_VS /*!< Robust virtual visual servoing approach. */
+ } vpPoseFeaturesMethodType;
+
private:
-
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- template<typename FeatureType, typename FirstParamType>
- struct vpDuo{
- FeatureType *desiredFeature;
+ template <typename FeatureType, typename FirstParamType> struct vpDuo {
+ FeatureType *desiredFeature;
FirstParamType firstParam;
vpDuo() : desiredFeature(NULL), firstParam() {}
};
-
- template<typename FeatureType, typename FirstParamType, typename SecondParamType>
- struct vpTrio{
- FeatureType *desiredFeature;
- FirstParamType firstParam;
+
+ template <typename FeatureType, typename FirstParamType, typename SecondParamType> struct vpTrio {
+ FeatureType *desiredFeature;
+ FirstParamType firstParam;
SecondParamType secondParam;
vpTrio() : desiredFeature(NULL), firstParam(), secondParam() {}
};
#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
- unsigned int maxSize;
- unsigned int totalSize;
- unsigned int vvsIterMax;
- double lambda;
-
- bool verbose;
-
- bool computeCovariance;
- vpMatrix covarianceMatrix;
-
- //vpFeaturePoint
- std::vector<vpDuo<vpFeaturePoint,vpPoint> > featurePoint_Point_list;
- //vpFeaturePoint3D
- std::vector<vpDuo<vpFeaturePoint3D,vpPoint> > featurePoint3D_Point_list;
- //vpFeatureVanishingPoint
- std::vector<vpDuo<vpFeatureVanishingPoint,vpPoint> > featureVanishingPoint_Point_list;
- std::vector<vpTrio<vpFeatureVanishingPoint,vpLine,vpLine> > featureVanishingPoint_DuoLine_list;
- //vpFeatureEllipse
- std::vector<vpDuo<vpFeatureEllipse,vpSphere> > featureEllipse_Sphere_list;
- std::vector<vpDuo<vpFeatureEllipse,vpCircle> > featureEllipse_Circle_list;
- //vpFeatureLine
- std::vector<vpDuo<vpFeatureLine,vpLine> > featureLine_Line_list;
- std::vector<vpTrio<vpFeatureLine,vpCylinder,int> > featureLine_DuoLineInt_List;
- //vpFeatureSegment
- std::vector<vpTrio<vpFeatureSegment,vpPoint,vpPoint> > featureSegment_DuoPoints_list;
-
+ unsigned int maxSize;
+ unsigned int totalSize;
+ unsigned int vvsIterMax;
+ double lambda;
+
+ bool verbose;
+
+ bool computeCovariance;
+ vpMatrix covarianceMatrix;
+
+ // vpFeaturePoint
+ std::vector<vpDuo<vpFeaturePoint, vpPoint> > featurePoint_Point_list;
+ // vpFeaturePoint3D
+ std::vector<vpDuo<vpFeaturePoint3D, vpPoint> > featurePoint3D_Point_list;
+ // vpFeatureVanishingPoint
+ std::vector<vpDuo<vpFeatureVanishingPoint, vpPoint> > featureVanishingPoint_Point_list;
+ std::vector<vpTrio<vpFeatureVanishingPoint, vpLine, vpLine> > featureVanishingPoint_DuoLine_list;
+ // vpFeatureEllipse
+ std::vector<vpDuo<vpFeatureEllipse, vpSphere> > featureEllipse_Sphere_list;
+ std::vector<vpDuo<vpFeatureEllipse, vpCircle> > featureEllipse_Circle_list;
+ // vpFeatureLine
+ std::vector<vpDuo<vpFeatureLine, vpLine> > featureLine_Line_list;
+ std::vector<vpTrio<vpFeatureLine, vpCylinder, int> > featureLine_DuoLineInt_List;
+ // vpFeatureSegment
+ std::vector<vpTrio<vpFeatureSegment, vpPoint, vpPoint> > featureSegment_DuoPoints_list;
+
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- //Specific features
- std::vector<vpPoseSpecificFeature*> featureSpecific_list;
+ // Specific features
+ std::vector<vpPoseSpecificFeature *> featureSpecific_list;
#endif
-
+
public:
-
- vpPoseFeatures();
- virtual ~vpPoseFeatures();
-
- // ! Features addition
- void addFeaturePoint(const vpPoint&);
-
- void addFeaturePoint3D(const vpPoint&);
-
- void addFeatureVanishingPoint(const vpPoint&);
- void addFeatureVanishingPoint(const vpLine&, const vpLine&);
-
- void addFeatureEllipse(const vpCircle&);
- void addFeatureEllipse(const vpSphere&);
-
- void addFeatureLine(const vpLine&);
- void addFeatureLine(const vpCylinder&, const int &line);
-
- void addFeatureSegment(vpPoint &, vpPoint&);
-
-
+ vpPoseFeatures();
+ virtual ~vpPoseFeatures();
+
+ // ! Features addition
+ void addFeaturePoint(const vpPoint &);
+
+ void addFeaturePoint3D(const vpPoint &);
+
+ void addFeatureVanishingPoint(const vpPoint &);
+ void addFeatureVanishingPoint(const vpLine &, const vpLine &);
+
+ void addFeatureEllipse(const vpCircle &);
+ void addFeatureEllipse(const vpSphere &);
+
+ void addFeatureLine(const vpLine &);
+ void addFeatureLine(const vpCylinder &, const int &line);
+
+ void addFeatureSegment(vpPoint &, vpPoint &);
+
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- template<typename RetType, typename ...ArgsFunc, typename ...Args>
- void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args);
-
- template<typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
- void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args);
+ template <typename RetType, typename... ArgsFunc, typename... Args>
+ void addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args);
+
+ template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
+ void addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args);
#endif
-
+
void clear();
-
- // ! Pose computation
- void computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
-
+
+ // ! Pose computation
+ void computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type = VIRTUAL_VS);
+
/*!
- Get the covariance matrix of the pose parameters computed by virtual visual servoing.
-
- \warning By default, the covariance matrix is not computed. To enable the computation, use setCovarianceComputation().
+ Get the covariance matrix of the pose parameters computed by virtual
+ visual servoing.
+
+ \warning By default, the covariance matrix is not computed. To enable the
+ computation, use setCovarianceComputation().
*/
- vpMatrix getCovarianceMatrix() const {
- if(!computeCovariance)
- vpTRACE("Warning : The covariance matrix has not been computed. See setCovarianceComputation() to do it.");
-
- return covarianceMatrix;
+ vpMatrix getCovarianceMatrix() const
+ {
+ if (!computeCovariance)
+ vpTRACE("Warning : The covariance matrix has not been computed. See "
+ "setCovarianceComputation() to do it.");
+
+ return covarianceMatrix;
}
-
+
/*!
- Get the gain that is used to compute the pose with the control law \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
+ Get the gain that is used to compute the pose with the control law \f${\bf
+ v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
\return Value of \f$\lambda\f$, the gain of the control law.
*/
- double getLambda(){ return lambda; }
-
+ double getLambda() { return lambda; }
+
/*!
- Get the maximum number of iterations of the virtual visual servoing (VVS) scheme implemented in computePose().
+ Get the maximum number of iterations of the virtual visual servoing (VVS)
+ scheme implemented in computePose().
\return Maximum number of iterations used during VVS minimization.
*/
- unsigned int getVVSIterMax(){ return vvsIterMax; }
-
+ unsigned int getVVSIterMax() { return vvsIterMax; }
+
/*!
Enable or disable covariance computation of the pose parameters.
\param flag : True if the covariance has to be computed, false otherwise.
*/
- void setCovarianceComputation(const bool& flag) { computeCovariance = flag; }
-
+ void setCovarianceComputation(const bool &flag) { computeCovariance = flag; }
+
/*!
- Set the gain used in the virtual visual servoing scheme : \f${\bf v} = -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
+ Set the gain used in the virtual visual servoing scheme : \f${\bf v} =
+ -\lambda {\bf L}^+ ({\bf s} - {\bf s}^*)\f$.
\param val : Value of the gain \f$\lambda\f$.
*/
- void setLambda(const double &val){ lambda = val; }
-
+ void setLambda(const double &val) { lambda = val; }
+
/*!
Set the maximum number of iterations used in computePose().
\param val : Maximum number of iteration used in the VVS scheme.
*/
- void setVVSIterMax(const unsigned int &val){ vvsIterMax = val; }
-
+ void setVVSIterMax(const unsigned int &val) { vvsIterMax = val; }
+
/*!
Turn the verbose mode ON / OFF.
-
+
\param mode : new verbose state. True to turn ON, false otherwise.
*/
- void setVerbose(const bool &mode){ verbose = mode; }
-
-
+ void setVerbose(const bool &mode) { verbose = mode; }
+
private:
- void error_and_interaction(vpHomogeneousMatrix & cMo, vpColVector &err, vpMatrix &L);
-
- void computePoseVVS(vpHomogeneousMatrix & cMo);
- void computePoseRobustVVS(vpHomogeneousMatrix & cMo);
+ void error_and_interaction(vpHomogeneousMatrix &cMo, vpColVector &err, vpMatrix &L);
+
+ void computePoseVVS(vpHomogeneousMatrix &cMo);
+ void computePoseRobustVVS(vpHomogeneousMatrix &cMo);
};
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
/*!
Add a specific feature for the pose computation.
-
+
\param fct_ptr : pointer on the function used to create the feature.
- \param args : List of function parameters;
- First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
- others are supposed to be derived from vpForwardProjection (redefine track() function)
-
- \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
-
+ \param args : List of function parameters;
+ First argument supposed to be derived from vpBasicFeature
+(redefine interaction() and error() functions), others are supposed to be
+derived from vpForwardProjection (redefine track() function)
+
+ \warning This function is only available with C++11. It has to be activated
+with USE_CPP11 option from CMake.
+
\code
#include <visp3/core/vpConfig.h>
#include <visp3/vision/vpPoseFeatures.h>
@@ -612,56 +566,57 @@ void vp_createLine(vpFeatureLine &fp,const vpLine &l){
int main()
{
vpPoseFeatures pose;
-
+
vpPoint pts[4];
vpLine line;
-
- //... Projection of the points and line
+
+ //... Projection of the points and line
vpFeaturePoint fp;
vpFeatureLine fl;
void (*ptr)(vpFeaturePoint&, const vpPoint&) = &vpFeatureBuilder::create;
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
pose.addSpecificFeature(ptr, fp, pts[0]);
pose.addSpecificFeature(&vp_createPoint, fp, pts[1]);
pose.addSpecificFeature(&vp_createTwoPoint, fp, pts[2], pts[3]);
pose.addSpecificFeature(&vp_createLine, fl, line);
#endif
-
+
//... Pose Computation
return 0;
}
\endcode
*/
-template< typename RetType, typename ...ArgsFunc, typename ...Args>
-void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &&...args)
+template <typename RetType, typename... ArgsFunc, typename... Args>
+void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc...), Args &&... args)
{
typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
- typedef typename std::remove_reference<featureTypeReference>::type featureType;
+ typedef typename std::remove_reference<featureTypeReference>::type featureType;
featureSpecific_list.push_back(
- new vpPoseSpecificFeatureTemplate< featureType, RetType, ArgsFunc... >(fct_ptr,std::forward<ArgsFunc>(args)...)
- );
-
+ new vpPoseSpecificFeatureTemplate<featureType, RetType, ArgsFunc...>(fct_ptr, std::forward<ArgsFunc>(args)...));
+
featureSpecific_list.back()->createDesired();
-
+
totalSize++;
- if(featureSpecific_list.size() > maxSize)
+ if (featureSpecific_list.size() > maxSize)
maxSize = (unsigned int)featureSpecific_list.size();
}
/*!
Add a specific feature for the pose computation.
-
+
\param obj : object used to call the function defined by fct_ptr.
\param fct_ptr : pointer on the function used to create the feature.
- \param args : List of function parameters;
- First argument supposed to be derived from vpBasicFeature (redefine interaction() and error() functions),
- others are supposed to be derived from vpForwardProjection (redefine track() function)
-
- \warning This function is only available with C++11. It has to be activated with USE_CPP11 option from CMake.
-
+ \param args : List of function parameters;
+ First argument supposed to be derived from vpBasicFeature
+(redefine interaction() and error() functions), others are supposed to be
+derived from vpForwardProjection (redefine track() function)
+
+ \warning This function is only available with C++11. It has to be activated
+with USE_CPP11 option from CMake.
+
\code
#include <visp3/core/vpConfig.h>
#include <visp3/vision/vpPoseFeatures.h>
@@ -669,18 +624,17 @@ void vpPoseFeatures::addSpecificFeature(RetType (*fct_ptr)(ArgsFunc ...), Args &
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);
}
};
@@ -688,45 +642,47 @@ public:
int main()
{
vpPoseFeatures pose;
-
+
vpPoint pts[3];
vpLine line;
-
- //... Projection of the points and line
+
+ //... Projection of the points and line
vpFeaturePoint fp;
vpFeatureLine fl;
-
+
vp_createClass cpClass;
- int (vp_createClass::*ptrClassPoint)(vpFeaturePoint&, const vpPoint&) = &vp_createClass::vp_createPoint;
- void (vp_createClass::*ptrClassTwoPoint)(vpFeaturePoint&, const vpPoint&, const vpPoint&) = &vp_createClass::vp_createTwoPoint;
- void (vp_createClass::*ptrClassLine)(vpFeatureLine &, const vpLine &) = &vp_createClass::vp_createLine;
-
-#ifdef VISP_HAVE_CPP11_COMPATIBILITY
+ int (vp_createClass::*ptrClassPoint)(vpFeaturePoint&, const vpPoint&)
+ = &vp_createClass::vp_createPoint;
+ void (vp_createClass::*ptrClassTwoPoint)(vpFeaturePoint&, const vpPoint&, const vpPoint&)
+ = &vp_createClass::vp_createTwoPoint;
+ void (vp_createClass::*ptrClassLine)(vpFeatureLine &, const vpLine &)
+ = &vp_createClass::vp_createLine;
+
+#ifdef VISP_HAVE_CPP11_COMPATIBILITY
pose.addSpecificFeature(&cpClass, ptrClassPoint, fp, pts[0]);
pose.addSpecificFeature(&cpClass, ptrClassTwoPoint, fp, pts[1], pts[2]);
pose.addSpecificFeature(&cpClass, ptrClassLine, fl, line);
#endif
-
+
//... Pose Computation
return 0;
}
\endcode
*/
-template< typename ObjType, typename RetType, typename ...ArgsFunc, typename ...Args>
-void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc ...), Args &&...args)
+template <typename ObjType, typename RetType, typename... ArgsFunc, typename... Args>
+void vpPoseFeatures::addSpecificFeature(ObjType *obj, RetType (ObjType::*fct_ptr)(ArgsFunc...), Args &&... args)
{
typedef typename std::tuple_element<0, std::tuple<Args...> >::type featureTypeReference;
- typedef typename std::remove_reference<featureTypeReference>::type featureType;
- featureSpecific_list.push_back(
- new vpPoseSpecificFeatureTemplateObject< ObjType, featureType, RetType, ArgsFunc... >(obj, fct_ptr,std::forward<ArgsFunc>(args)...)
- );
-
+ typedef typename std::remove_reference<featureTypeReference>::type featureType;
+ featureSpecific_list.push_back(new vpPoseSpecificFeatureTemplateObject<ObjType, featureType, RetType, ArgsFunc...>(
+ obj, fct_ptr, std::forward<ArgsFunc>(args)...));
+
featureSpecific_list.back()->createDesired();
-
+
totalSize++;
- if(featureSpecific_list.size() > maxSize)
+ if (featureSpecific_list.size() > maxSize)
maxSize = (unsigned int)featureSpecific_list.size();
}
#endif
diff --git a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h
index a1c4b67..bb3b768 100644
--- a/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h
+++ b/modules/vision/include/visp3/vision/vpXmlConfigParserKeyPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,7 +39,8 @@
/*!
\file vpXmlConfigParserKeyPoint.cpp
\brief Definition of the vpXmlConfigParserKeyPoint class member functions.
- Class vpXmlConfigParserKeyPoint allows to load configuration defined in a XML file for vpKeyPoint class.
+ Class vpXmlConfigParserKeyPoint allows to load configuration defined in a
+ XML file for vpKeyPoint class.
*/
@@ -52,7 +54,7 @@
#include <iostream>
#include <stdlib.h>
-#include <libxml/xmlmemory.h> // Functions of libxml.
+#include <libxml/xmlmemory.h> // Functions of libxml.
#include <visp3/core/vpXmlParser.h>
@@ -60,11 +62,12 @@
\class vpXmlConfigParserKeyPoint
\ingroup group_vision_keypoints
- \warning This class is only available if libxml2 is installed and detected by ViSP.
- Installation instructions are provided here https://visp.inria.fr/3rd_xml2.
+ \warning This class is only available if libxml2 is installed and detected
+ by ViSP. Installation instructions are provided here
+ https://visp.inria.fr/3rd_xml2.
*/
-class VISP_EXPORT vpXmlConfigParserKeyPoint: public vpXmlParser
+class VISP_EXPORT vpXmlConfigParserKeyPoint : public vpXmlParser
{
public:
@@ -76,33 +79,52 @@ public:
matcher, /*!< Identifier associated to the matcher tag. */
name, /*!< Identifier associated to the name tag. */
matching_method, /*!< Identifier associated to the matching_method tag. */
- constant_factor_distance_threshold, /*!< Identifier associated to the constant_factor_distance_threshold tag. */
- std_distance_threshold, /*!< Identifier associated to the std_distance_threshold tag. */
- ratio_distance_threshold, /*!< Identifier associated to the ratio_distance_threshold tag. */
- std_and_ratio_distance_threshold, /*!< Identifier associated to the std_and_ratio_distance_threshold tag. */
- no_filter_matching, /*!< Identifier associated to the no_filter_matching tag. */
- matching_factor_threshold, /*!< Identifier associated to the matching_factor_threshold tag. */
- matching_ratio_threshold, /*!< Identifier associated to the matching_ratio_threshold tag. */
+ constant_factor_distance_threshold, /*!< Identifier associated to the
+ constant_factor_distance_threshold
+ tag. */
+ std_distance_threshold, /*!< Identifier associated to the
+ std_distance_threshold tag. */
+ ratio_distance_threshold, /*!< Identifier associated to the
+ ratio_distance_threshold tag. */
+ std_and_ratio_distance_threshold, /*!< Identifier associated to the
+ std_and_ratio_distance_threshold tag.
+ */
+ no_filter_matching, /*!< Identifier associated to the no_filter_matching
+ tag. */
+ matching_factor_threshold, /*!< Identifier associated to the
+ matching_factor_threshold tag. */
+ matching_ratio_threshold, /*!< Identifier associated to the
+ matching_ratio_threshold tag. */
ransac, /*!< Identifier associated to the ransac tag. */
use_ransac_vvs, /*!< Identifier associated to the use_ransac_vvs tag. */
- use_ransac_consensus_percentage, /*!< Identifier associated to the use_ransac_consensus_percentage tag. */
- nb_ransac_iterations, /*!< Identifier associated to the nb_ransac_iterations tag. */
- ransac_reprojection_error, /*!< Identifier associated to the ransac_reprojection_error tag. */
- nb_ransac_min_inlier_count, /*!< Identifier associated to the nb_ransac_min_inlier_count tag. */
- ransac_threshold, /*!< Identifier associated to the ransac_threshold tag. */
- ransac_consensus_percentage /*!< Identifier associated to the ransac_consensus_percentage tag. */
+ 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. */
+ constantFactorDistanceThreshold, /*!< Keep all the points below a constant
+ factor threshold. */
+ stdDistanceThreshold, /*!< Keep all the points below a minimal distance +
+ the standard deviation. */
+ ratioDistanceThreshold, /*!< Keep all the points enough discriminated. */
+ stdAndRatioDistanceThreshold, /*!< Keep all the points which fall with the
+ two conditions. */
+ noFilterMatching /*!< No filtering. */
} vpMatchingMethodEnum;
-private :
+private:
//! Name of the keypoint detector.
std::string m_detectorName;
//! Name of the keypoint extractor.
@@ -121,128 +143,109 @@ private :
int m_nbRansacMinInlierCount;
//! Percentage value of inliers compared to outliers for the Ransac method.
double m_ransacConsensusPercentage;
- //! Maximum reprojection error (in pixel for OpenCV method) to consider a point as an inlier.
+ //! Maximum reprojection error (in pixel for OpenCV method) to consider a
+ //! point as an inlier.
double m_ransacReprojectionError;
- //! Maximum error (in meter for ViSP method) to consider a point as an inlier.
+ //! Maximum error (in meter for ViSP method) to consider a point as an
+ //! inlier.
double m_ransacThreshold;
- //! If true, the cardinality of the consensus is based upon a percentage, otherwise
- //it is based on a fixed number.
+ //! If true, the cardinality of the consensus is based upon a percentage,
+ //! otherwise
+ // it is based on a fixed number.
bool m_useRansacConsensusPercentage;
- //! If true, use ViSP Ransac VVS pose estimation method, otherwise use OpenCV method.
+ //! If true, use ViSP Ransac VVS pose estimation method, otherwise use
+ //! OpenCV method.
bool m_useRansacVVS;
-
public:
-
vpXmlConfigParserKeyPoint();
//! Default destructor.
- virtual ~vpXmlConfigParserKeyPoint() {};
+ virtual ~vpXmlConfigParserKeyPoint(){};
/*!
Get the detector name.
\return The detector name.
*/
- inline std::string getDetectorName() const {
- return m_detectorName;
- }
+ inline std::string getDetectorName() const { return m_detectorName; }
/*!
Get the extractor name.
\return The extractor name.
*/
- inline std::string getExtractorName() const {
- return m_extractorName;
- }
+ inline std::string getExtractorName() const { return m_extractorName; }
/*!
Get the matcher name.
\return The detector name.
*/
- inline std::string getMatcherName() const {
- return m_matcherName;
- }
+ inline std::string getMatcherName() const { return m_matcherName; }
/*!
Get the factor value.
- \return The factor value for the filtering method: constantFactorDistanceThreshold.
+ \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.
\return The maximum number of iterations for the Ransac method.
*/
- inline int getNbRansacIterations() const {
- return m_nbRansacIterations;
- }
+ inline int getNbRansacIterations() const { return m_nbRansacIterations; }
/*!
Get the minimum number of inliers for the Ransac method.
\return The minimum number of inliers for the Ransac method.
*/
- inline int getNbRansacMinInlierCount() const {
- return m_nbRansacMinInlierCount;
- }
+ 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 method.
+ Get the maximum reprojection error for a candidate inlier for the Ransac
+ method.
\return The maximum reprojection error for the Ransac method.
*/
- inline double getRansacReprojectionError() const {
- return m_ransacReprojectionError;
- }
+ inline double getRansacReprojectionError() const { return m_ransacReprojectionError; }
/*!
Get the maximum error for a candidate inlier for the Ransac method.
\return The maximum error for the Ransac method.
*/
- inline double getRansacThreshold() const {
- return m_ransacThreshold;
- }
+ inline double getRansacThreshold() const { return m_ransacThreshold; }
/*!
- Get the flag state to choose between a percentage of inliers or a fixed number.
+ Get the flag state to choose between a percentage of inliers or a fixed
+ number.
\return True to use a percentage value for inliers, false otherwise.
*/
- inline bool getUseRansacConsensusPercentage() const {
- return m_useRansacConsensusPercentage;
- }
+ inline bool getUseRansacConsensusPercentage() const { return m_useRansacConsensusPercentage; }
/*!
Get the flag state to choose between OpenCV Ransac pose estimation or ViSP
@@ -250,9 +253,7 @@ public:
\return True to use ViSP method, false otherwise.
*/
- inline bool getUseRansacVVSPoseEstimation() const {
- return m_useRansacVVS;
- }
+ inline bool getUseRansacVVSPoseEstimation() const { return m_useRansacVVS; }
/*!
Parse an XML file to get configuration value for vpKeyPoint class.
@@ -261,17 +262,14 @@ public:
*/
void parse(const std::string &filename);
-
private:
-
void init();
void read_detector(xmlDocPtr doc, xmlNodePtr node);
void read_extractor(xmlDocPtr doc, xmlNodePtr node);
void read_matcher(xmlDocPtr doc, xmlNodePtr node);
virtual void readMainClass(xmlDocPtr doc, xmlNodePtr node);
void read_ransac(xmlDocPtr doc, xmlNodePtr node);
- virtual void writeMainClass(xmlNodePtr ){};
-
+ virtual void writeMainClass(xmlNodePtr){};
};
-#endif //VISP_HAVE_XML2
+#endif // VISP_HAVE_XML2
#endif
diff --git a/modules/vision/src/calibration/vpCalibration.cpp b/modules/vision/src/calibration/vpCalibration.cpp
index 1d1380a..bf2c517 100644
--- a/modules/vision/src/calibration/vpCalibration.cpp
+++ b/modules/vision/src/calibration/vpCalibration.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,16 +38,15 @@
*
*****************************************************************************/
-
/*!
\file vpCalibration.cpp
\brief Tools for camera calibration.
*/
-#include <visp3/vision/vpCalibration.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/vision/vpPose.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpCalibration.h>
+#include <visp3/vision/vpPose.h>
double vpCalibration::threshold = 1e-10f;
unsigned int vpCalibration::nbIterMax = 4000;
@@ -56,66 +56,62 @@ double vpCalibration::gain = 0.25;
*/
int vpCalibration::init()
{
- npt = 0 ;
+ npt = 0;
residual = residual_dist = 1000.;
- LoX.clear() ;
- LoY.clear() ;
- LoZ.clear() ;
- Lip.clear() ;
+ LoX.clear();
+ LoY.clear();
+ LoZ.clear();
+ Lip.clear();
- return 0 ;
+ return 0;
}
/*!
Default constructor.
*/
vpCalibration::vpCalibration()
- : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
- npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), residual_dist(1000.)
+ : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
+ residual_dist(1000.)
{
- init() ;
+ init();
}
/*!
Copy constructor.
*/
vpCalibration::vpCalibration(const vpCalibration &c)
- : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(),
- npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.), residual_dist(1000.)
+ : cMo(), cMo_dist(), cam(), cam_dist(), rMe(), eMc(), eMc_dist(), npt(0), LoX(), LoY(), LoZ(), Lip(), residual(1000.),
+ residual_dist(1000.)
{
(*this) = c;
}
-
/*!
Destructor : delete the array of point (freed the memory)
*/
-vpCalibration::~vpCalibration()
-{
- clearPoint() ;
-}
+vpCalibration::~vpCalibration() { clearPoint(); }
/*!
= operator.
\param twinCalibration : object to be copied
*/
-vpCalibration& vpCalibration::operator=(const vpCalibration& twinCalibration )
+vpCalibration &vpCalibration::operator=(const vpCalibration &twinCalibration)
{
- npt = twinCalibration.npt ;
- LoX = twinCalibration.LoX ;
- LoY = twinCalibration.LoY ;
- LoZ = twinCalibration.LoZ ;
- Lip = twinCalibration.Lip ;
+ npt = twinCalibration.npt;
+ LoX = twinCalibration.LoX;
+ LoY = twinCalibration.LoY;
+ LoZ = twinCalibration.LoZ;
+ Lip = twinCalibration.Lip;
residual = twinCalibration.residual;
cMo = twinCalibration.cMo;
residual_dist = twinCalibration.residual_dist;
- cMo_dist = twinCalibration.cMo_dist ;
+ cMo_dist = twinCalibration.cMo_dist;
- cam = twinCalibration.cam ;
- cam_dist = twinCalibration.cam_dist ;
+ cam = twinCalibration.cam;
+ cam_dist = twinCalibration.cam_dist;
rMe = twinCalibration.rMe;
@@ -125,23 +121,22 @@ vpCalibration& vpCalibration::operator=(const vpCalibration& twinCalibration )
return (*this);
}
-
/*!
Delete the array of points.
*/
int vpCalibration::clearPoint()
{
- LoX.clear() ;
- LoY.clear() ;
- LoZ.clear() ;
- Lip.clear() ;
- npt = 0 ;
+ LoX.clear();
+ LoY.clear();
+ LoZ.clear();
+ Lip.clear();
+ npt = 0;
- return 0 ;
+ return 0;
}
/*!
-
+
Add a new point in the array of points.
\param X,Y,Z : 3D coordinates of a point in the object frame
\param ip : 2D Coordinates of the point in the camera frame.
@@ -154,9 +149,9 @@ int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip)
Lip.push_back(ip);
- npt++ ;
+ npt++;
- return 0 ;
+ return 0;
}
/*!
@@ -166,23 +161,23 @@ int vpCalibration::addPoint(double X, double Y, double Z, vpImagePoint &ip)
*/
void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousMatrix &cMo_est)
{
- // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y) )
- vpPose pose ;
+ // The vpPose class mainly contents a list of vpPoint (that is (X,Y,Z, x, y)
+ // )
+ vpPose pose;
// the list of point is cleared (if that's not done before)
- pose.clearPoint() ;
+ pose.clearPoint();
// we set the 3D points coordinates (in meter !) in the object/world frame
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
- for (unsigned int i =0 ; i < npt ; i++)
- {
+ for (unsigned int i = 0; i < npt; i++) {
vpPoint P(*it_LoX, *it_LoY, *it_LoZ);
- double x=0,y=0 ;
- vpPixelMeterConversion::convertPoint(camera, *it_Lip, x,y) ;
- P.set_x(x) ;
- P.set_y(y) ;
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(camera, *it_Lip, x, y);
+ P.set_x(x);
+ P.set_y(y);
pose.addPoint(P);
++it_LoX;
@@ -190,84 +185,82 @@ void vpCalibration::computePose(const vpCameraParameters &camera, vpHomogeneousM
++it_LoZ;
++it_Lip;
}
- vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
+ vpHomogeneousMatrix cMo_dementhon; // computed pose with dementhon
vpHomogeneousMatrix cMo_lagrange; // computed pose with dementhon
// compute the initial pose using Lagrange method followed by a non linear
// minimisation method
// Pose by Lagrange it provides an initialization of the pose
- pose.computePose(vpPose::LAGRANGE, cMo_lagrange) ;
+ pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
double residual_lagrange = pose.computeResidual(cMo_lagrange);
// compute the initial pose using Dementhon method followed by a non linear
// minimisation method
// Pose by Dementhon it provides an initialization of the pose
- pose.computePose(vpPose::DEMENTHON, cMo_dementhon) ;
+ pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
double residual_dementhon = pose.computeResidual(cMo_dementhon);
- //we keep the better initialization
+ // we keep the better initialization
if (residual_lagrange < residual_dementhon)
cMo_est = cMo_lagrange;
else
cMo_est = cMo_dementhon;
-
+
// the pose is now refined using the virtual visual servoing approach
// Warning: cMo needs to be initialized otherwise it may diverge
- pose.computePose(vpPose::VIRTUAL_VS, cMo_est) ;
+ pose.computePose(vpPose::VIRTUAL_VS, cMo_est);
}
/*!
Compute and return the standard deviation expressed in pixel
- for pose matrix and camera intrinsic parameters for model without distortion.
- \param cMo_est : the matrix that defines the pose to be tested.
+ for pose matrix and camera intrinsic parameters for model without
+ distortion. \param cMo_est : the matrix that defines the pose to be tested.
\param camera : camera intrinsic parameters to be tested.
\return the standard deviation by point of the error in pixel .
*/
-double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix& cMo_est,
- const vpCameraParameters& camera)
+double vpCalibration::computeStdDeviation(const vpHomogeneousMatrix &cMo_est, const vpCameraParameters &camera)
{
- double residual_ = 0 ;
+ double residual_ = 0;
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
- double u0 = camera.get_u0() ;
- double v0 = camera.get_v0() ;
- double px = camera.get_px() ;
- double py = camera.get_py() ;
+ double u0 = camera.get_u0();
+ double v0 = camera.get_v0();
+ double px = camera.get_px();
+ double py = camera.get_py();
vpImagePoint ip;
- for (unsigned int i =0 ; i < npt ; i++)
- {
+ for (unsigned int i = 0; i < npt; i++) {
double oX = *it_LoX;
double oY = *it_LoY;
double oZ = *it_LoZ;
- double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
- double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
- double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
+ double 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 ;
+ double x = cX / cZ;
+ double y = cY / cZ;
ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
+ double u = ip.get_u();
+ double v = ip.get_v();
- double xp = u0 + x*px;
- double yp = v0 + y*py;
+ double xp = u0 + x * px;
+ double yp = v0 + y * py;
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+ residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
++it_LoX;
++it_LoY;
++it_LoZ;
++it_Lip;
}
- this->residual = residual_ ;
- return sqrt(residual_/npt) ;
+ this->residual = residual_;
+ return sqrt(residual_ / npt);
}
/*!
Compute and return the standard deviation expressed in pixel
@@ -276,82 +269,79 @@ 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 ;
+ double residual_ = 0;
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
- double u0 = camera.get_u0() ;
- double v0 = camera.get_v0() ;
- double px = camera.get_px() ;
- double py = camera.get_py() ;
- double kud = camera.get_kud() ;
- double kdu = camera.get_kdu() ;
+ double u0 = camera.get_u0();
+ double v0 = camera.get_v0();
+ double px = camera.get_px();
+ double py = camera.get_py();
+ double kud = camera.get_kud();
+ double kdu = camera.get_kdu();
- double inv_px = 1/px;
- double inv_py = 1/px;
+ double inv_px = 1 / px;
+ double inv_py = 1 / px;
vpImagePoint ip;
- for (unsigned int i =0 ; i < npt ; i++)
- {
+ for (unsigned int i = 0; i < npt; i++) {
double oX = *it_LoX;
double oY = *it_LoY;
double oZ = *it_LoZ;
- double cX = oX*cMo_est[0][0]+oY*cMo_est[0][1]+oZ*cMo_est[0][2] + cMo_est[0][3];
- double cY = oX*cMo_est[1][0]+oY*cMo_est[1][1]+oZ*cMo_est[1][2] + cMo_est[1][3];
- double cZ = oX*cMo_est[2][0]+oY*cMo_est[2][1]+oZ*cMo_est[2][2] + cMo_est[2][3];
+ double 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 ;
+ double x = cX / cZ;
+ double y = cY / cZ;
ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
+ double u = ip.get_u();
+ double v = ip.get_v();
- double r2ud = 1+kud*(vpMath::sqr(x)+vpMath::sqr(y)) ;
+ double r2ud = 1 + kud * (vpMath::sqr(x) + vpMath::sqr(y));
- double xp = u0 + x*px*r2ud;
- double yp = v0 + y*py*r2ud;
+ double xp = u0 + x * px * r2ud;
+ double yp = v0 + y * py * r2ud;
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+ 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;
+ xp = u0 + x * px - kdu * (u - u0) * r2du;
+ yp = v0 + y * py - kdu * (v - v0) * r2du;
- residual_ += (vpMath::sqr(xp-u) + vpMath::sqr(yp-v)) ;
+ residual_ += (vpMath::sqr(xp - u) + vpMath::sqr(yp - v));
++it_LoX;
++it_LoY;
++it_LoZ;
++it_Lip;
}
- residual_ /=2;
+ residual_ /= 2;
this->residual_dist = residual_;
- return sqrt(residual_/npt) ;
+ return sqrt(residual_ / npt);
}
/*!
Compute and return the standard deviation expressed in pixel
for pose matrix and camera intrinsic parameters.
- \param deviation : the standard deviation computed for the model without distortion.
- \param deviation_dist : the standard deviation computed for the model with distortion.
+ \param deviation : the standard deviation computed for the model without
+ distortion. \param deviation_dist : the standard deviation computed for the
+ model with distortion.
*/
-void
- vpCalibration::computeStdDeviation(double &deviation,double &deviation_dist)
+void vpCalibration::computeStdDeviation(double &deviation, double &deviation_dist)
{
- deviation = computeStdDeviation(cMo,cam);
- deviation_dist = computeStdDeviation_dist(cMo_dist,cam_dist);
+ deviation = computeStdDeviation(cMo, cam);
+ deviation_dist = computeStdDeviation_dist(cMo_dist, cam_dist);
}
-
/*!
Compute the calibration according to the desired method using one pose.
@@ -363,21 +353,16 @@ void
\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);
- switch (method)
- {
- case CALIB_LAGRANGE :
- case CALIB_LAGRANGE_VIRTUAL_VS :
- {
- calibLagrange(cam_est, cMo_est);
- }
- break;
+ try {
+ computePose(cam_est, cMo_est);
+ switch (method) {
+ case CALIB_LAGRANGE:
+ case CALIB_LAGRANGE_VIRTUAL_VS: {
+ calibLagrange(cam_est, cMo_est);
+ } break;
case CALIB_VIRTUAL_VS:
case CALIB_VIRTUAL_VS_DIST:
case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
@@ -385,17 +370,16 @@ int vpCalibration::computeCalibration(vpCalibrationMethodType method,
break;
}
- switch (method)
- {
+ switch (method) {
case CALIB_VIRTUAL_VS:
case CALIB_VIRTUAL_VS_DIST:
case CALIB_LAGRANGE_VIRTUAL_VS:
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
- {
- if (verbose){std::cout << "start calibration without distortion"<< std::endl;}
- calibVVS(cam_est, cMo_est, verbose);
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST: {
+ if (verbose) {
+ std::cout << "start calibration without distortion" << std::endl;
}
- break ;
+ calibVVS(cam_est, cMo_est, verbose);
+ } break;
case CALIB_LAGRANGE:
default:
break;
@@ -403,192 +387,187 @@ int vpCalibration::computeCalibration(vpCalibrationMethodType method,
this->cMo = cMo_est;
this->cMo_dist = cMo_est;
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
+ // Print camera parameters
+ if (verbose) {
+ // std::cout << "Camera parameters without distortion :" <<
+ // std::endl;
cam_est.printParameters();
}
this->cam = cam_est;
- switch (method)
- {
+ switch (method) {
case CALIB_VIRTUAL_VS_DIST:
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
- {
- if (verbose){std::cout << "start calibration with distortion"<< std::endl;}
- calibVVSWithDistortion(cam_est, cMo_est, verbose);
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST: {
+ if (verbose) {
+ std::cout << "start calibration with distortion" << std::endl;
}
- break ;
+ calibVVSWithDistortion(cam_est, cMo_est, verbose);
+ } break;
case CALIB_LAGRANGE:
case CALIB_VIRTUAL_VS:
case CALIB_LAGRANGE_VIRTUAL_VS:
default:
break;
}
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
+ // Print camera parameters
+ if (verbose) {
+ // std::cout << "Camera parameters without distortion :" <<
+ // std::endl;
this->cam.printParameters();
- // std::cout << "Camera parameters with distortion :" << std::endl;
+ // std::cout << "Camera parameters with distortion :" <<
+ // std::endl;
cam_est.printParameters();
}
- this->cam_dist = cam_est ;
+ this->cam_dist = cam_est;
this->cMo_dist = cMo_est;
- return 0 ;
- }
- catch(...){
+ return 0;
+ } catch (...) {
throw;
}
}
/*!
- Compute the multi-images calibration according to the desired method using many poses.
+ Compute the multi-images calibration according to the desired method using
+ many poses.
\param method : Method used to estimate the camera parameters.
\param table_cal : Vector of vpCalibration.
\param cam_est : Estimated intrinsic camera parameters.
- \param globalReprojectionError : Global reprojection error or global residual.
- \param verbose : Set at true if information about the residual at each loop
- of the algorithm is hoped.
+ \param globalReprojectionError : Global reprojection error or global
+ residual. \param verbose : Set at true if information about the residual at
+ each loop of the algorithm is hoped.
\return 0 if the computation was managed succeed.
*/
-int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method,
- std::vector<vpCalibration> &table_cal,
- vpCameraParameters& cam_est,
- double &globalReprojectionError,
- bool verbose)
+int vpCalibration::computeCalibrationMulti(vpCalibrationMethodType method, std::vector<vpCalibration> &table_cal,
+ vpCameraParameters &cam_est, double &globalReprojectionError, bool verbose)
{
- try{
- unsigned int nbPose = (unsigned int) table_cal.size();
- for(unsigned int i=0;i<nbPose;i++){
- if(table_cal[i].get_npt()>3)
- table_cal[i].computePose(cam_est,table_cal[i].cMo);
+ try {
+ unsigned int nbPose = (unsigned int)table_cal.size();
+ for (unsigned int i = 0; i < nbPose; i++) {
+ if (table_cal[i].get_npt() > 3)
+ table_cal[i].computePose(cam_est, table_cal[i].cMo);
}
switch (method) {
- case CALIB_LAGRANGE : {
- if(nbPose > 1){
+ case CALIB_LAGRANGE: {
+ if (nbPose > 1) {
std::cout << "this calibration method is not available in" << std::endl
<< "vpCalibration::computeCalibrationMulti()" << std::endl;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
+ return -1;
+ } else {
+ table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
+ table_cal[0].cam = cam_est;
+ table_cal[0].cam_dist = cam_est;
+ table_cal[0].cMo_dist = table_cal[0].cMo;
}
break;
}
- case CALIB_LAGRANGE_VIRTUAL_VS :
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST : {
- if(nbPose > 1){
+ case CALIB_LAGRANGE_VIRTUAL_VS:
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST: {
+ if (nbPose > 1) {
std::cout << "this calibration method is not available in" << std::endl
<< "vpCalibration::computeCalibrationMulti()" << std::endl
<< "with several images." << std::endl;
- return -1 ;
- }
- else {
- table_cal[0].calibLagrange(cam_est,table_cal[0].cMo);
- table_cal[0].cam = cam_est ;
- table_cal[0].cam_dist = cam_est ;
- table_cal[0].cMo_dist = table_cal[0].cMo ;
+ return -1;
+ } else {
+ table_cal[0].calibLagrange(cam_est, table_cal[0].cMo);
+ table_cal[0].cam = cam_est;
+ table_cal[0].cam_dist = cam_est;
+ table_cal[0].cMo_dist = table_cal[0].cMo;
}
calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
- break ;
+ break;
}
case CALIB_VIRTUAL_VS:
case CALIB_VIRTUAL_VS_DIST: {
calibVVSMulti(table_cal, cam_est, globalReprojectionError, verbose);
- break ;
+ break;
}
}
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
+ // Print camera parameters
+ if (verbose) {
+ // std::cout << "Camera parameters without distortion :" <<
+ // std::endl;
cam_est.printParameters();
}
- switch (method)
- {
- case CALIB_LAGRANGE :
- case CALIB_LAGRANGE_VIRTUAL_VS :
+ switch (method) {
+ case CALIB_LAGRANGE:
+ case CALIB_LAGRANGE_VIRTUAL_VS:
case CALIB_VIRTUAL_VS:
- verbose = false ;
+ verbose = false;
break;
- case CALIB_LAGRANGE_VIRTUAL_VS_DIST :
- case CALIB_VIRTUAL_VS_DIST:
- {
- if(verbose)
- std::cout << "Compute camera parameters with distortion"<<std::endl;
+ case CALIB_LAGRANGE_VIRTUAL_VS_DIST:
+ case CALIB_VIRTUAL_VS_DIST: {
+ if (verbose)
+ std::cout << "Compute camera parameters with distortion" << std::endl;
- calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
- }
- break ;
+ calibVVSWithDistortionMulti(table_cal, cam_est, globalReprojectionError, verbose);
+ } break;
}
- //Print camera parameters
- if(verbose){
- // std::cout << "Camera parameters without distortion :" << std::endl;
+ // Print camera parameters
+ if (verbose) {
+ // std::cout << "Camera parameters without distortion :" <<
+ // std::endl;
table_cal[0].cam.printParameters();
// std::cout << "Camera parameters with distortion:" << std::endl;
cam_est.printParameters();
- std::cout<<std::endl;
+ std::cout << std::endl;
}
- return 0 ;
+ return 0;
+ } catch (...) {
+ throw;
}
- catch(...){ throw; }
}
/*!
- \brief Compute the multi-image calibration of effector-camera from R. Tsai and R. Lenz \cite Tsai89a.
+ \brief Compute the multi-image calibration of effector-camera from R. Tsai
+ and R. Lenz \cite Tsai89a.
Compute extrinsic camera parameters : the constant transformation from
- the end-effector to the camera frame \f${^e}{\bf M}_c\f$ considering the camera model with or without distortion.
-
- \param[in] table_cal : Vector of vpCalibration that contains for each index a couple of
- \f${^r}{\bf M}_e\f$ (world to end-effector) and \f${^c}{\bf M}_o\f$ (camera to object)
- transformations.
- \param[out] eMc : Estimated pose of the camera in relation to the end-effector
- considering the camera model without distortion.
- \param[out] eMc_dist : Estimated pose of the camera in relation to the
- end-effector considering the model with distortion.
- \return 0 if the computation managed, -1 if less than three poses are provides as input.
+ the end-effector to the camera frame \f${^e}{\bf M}_c\f$ considering the
+ camera model with or without distortion.
+
+ \param[in] table_cal : Vector of vpCalibration that contains for each index
+ a couple of \f${^r}{\bf M}_e\f$ (world to end-effector) and \f${^c}{\bf
+ M}_o\f$ (camera to object) transformations. \param[out] eMc : Estimated pose
+ of the camera in relation to the end-effector considering the camera model
+ without distortion. \param[out] eMc_dist : Estimated pose of the camera in
+ relation to the end-effector considering the model with distortion. \return
+ 0 if the computation managed, -1 if less than three poses are provides as
+ input.
*/
-int vpCalibration::computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
- vpHomogeneousMatrix& eMc,
- vpHomogeneousMatrix& eMc_dist)
+int vpCalibration::computeCalibrationTsai(const std::vector<vpCalibration> &table_cal, vpHomogeneousMatrix &eMc,
+ vpHomogeneousMatrix &eMc_dist)
{
- try{
+ try {
unsigned int nbPose = (unsigned int)table_cal.size();
- if (nbPose > 2){
+ if (nbPose > 2) {
std::vector<vpHomogeneousMatrix> table_cMo(nbPose);
std::vector<vpHomogeneousMatrix> table_cMo_dist(nbPose);
std::vector<vpHomogeneousMatrix> table_rMe(nbPose);
- for(unsigned int i=0;i<nbPose;i++){
+ for (unsigned int i = 0; i < nbPose; i++) {
table_cMo[i] = table_cal[i].cMo;
table_cMo_dist[i] = table_cal[i].cMo_dist;
table_rMe[i] = table_cal[i].rMe;
}
- calibrationTsai(table_cMo, table_rMe, eMc);
+ calibrationTsai(table_cMo, table_rMe, eMc);
calibrationTsai(table_cMo_dist, table_rMe, eMc_dist);
return 0;
- }
- else{
+ } else {
vpERROR_TRACE("Three images are needed to compute Tsai calibration !\n");
return -1;
}
- }
- catch(...){
+ } catch (...) {
throw;
}
}
-
/*!
Write data into a file.
@@ -598,7 +577,7 @@ int vpCalibration::computeCalibrationTsai(std::vector<vpCalibration> &table_cal,
*/
int vpCalibration::writeData(const char *filename)
{
- std::ofstream f(filename) ;
+ std::ofstream f(filename);
vpImagePoint ip;
std::list<double>::const_iterator it_LoX = LoX.begin();
@@ -607,31 +586,29 @@ int vpCalibration::writeData(const char *filename)
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
f.precision(10);
- f.setf(std::ios::fixed,std::ios::floatfield);
- f << LoX.size() << std::endl ;
+ f.setf(std::ios::fixed, std::ios::floatfield);
+ f << LoX.size() << std::endl;
- for (unsigned int i =0 ; i < LoX.size() ; i++)
- {
+ for (unsigned int i = 0; i < LoX.size(); i++) {
double oX = *it_LoX;
double oY = *it_LoY;
double oZ = *it_LoZ;
ip = *it_Lip;
- double u = ip.get_u() ;
- double v = ip.get_v() ;
+ double u = ip.get_u();
+ double v = ip.get_v();
- f << oX <<" " << oY << " " << oZ << " " << u << " " << v <<std::endl ;
+ f << oX << " " << oY << " " << oZ << " " << u << " " << v << std::endl;
++it_LoX;
++it_LoY;
++it_LoZ;
++it_Lip;
-
}
- f.close() ;
- return 0 ;
+ f.close();
+ return 0;
}
/*!
@@ -640,35 +617,33 @@ int vpCalibration::writeData(const char *filename)
\param filename : name of the file
*/
-int vpCalibration::readData(const char* filename)
+int vpCalibration::readData(const char *filename)
{
vpImagePoint ip;
std::ifstream f;
f.open(filename);
- if (! f.fail()){
- unsigned int n ;
- f >> n ;
- std::cout << "There are "<< n <<" point on the calibration grid " << std::endl ;
+ if (!f.fail()) {
+ unsigned int n;
+ f >> n;
+ std::cout << "There are " << n << " point on the calibration grid " << std::endl;
- clearPoint() ;
+ clearPoint();
if (n > 100000)
throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
- for (unsigned int i=0 ; i < n ; i++)
- {
- double x, y, z, u, v ;
- f >> x >> y >> z >> u >> v ;
- std::cout << x <<" "<<y <<" "<<z<<" "<<u<<" "<<v <<std::endl ;
- ip.set_u( u );
- ip.set_v( v );
- addPoint(x, y, z, ip) ;
+ for (unsigned int i = 0; i < n; i++) {
+ double x, y, z, u, v;
+ f >> x >> y >> z >> u >> v;
+ std::cout << x << " " << y << " " << z << " " << u << " " << v << std::endl;
+ ip.set_u(u);
+ ip.set_v(v);
+ addPoint(x, y, z, ip);
}
- f.close() ;
- return 0 ;
- }
- else{
+ f.close();
+ return 0;
+ } else {
return -1;
}
}
@@ -688,19 +663,19 @@ 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<double> &oX, std::list<double> &oY, std::list<double> &oZ, bool verbose)
+int vpCalibration::readGrid(const char *filename, unsigned int &n, std::list<double> &oX, std::list<double> &oY,
+ std::list<double> &oZ, bool verbose)
{
- try{
+ try {
std::ifstream f;
f.open(filename);
- if (! f.fail()){
+ if (!f.fail()) {
- f >> n ;
- if(verbose)
- std::cout << "There are "<< n <<" points on the calibration grid " << std::endl ;
+ f >> n;
+ if (verbose)
+ std::cout << "There are " << n << " points on the calibration grid " << std::endl;
int no_pt;
- double x,y,z;
+ double x, y, z;
// clear the list
oX.clear();
@@ -710,26 +685,25 @@ int vpCalibration::readGrid(const char* filename, unsigned int &n,
if (n > 100000)
throw(vpException(vpException::badValue, "Bad number of point in the calibration grid"));
- for (unsigned int i=0 ; i < n ; i++)
- {
- f >> no_pt >> x >> y >> z ;
- if(verbose){
- std::cout << no_pt <<std::endl ;
- std::cout << x <<" "<< y <<" "<< z <<std::endl ;
- }
- oX.push_back(x) ;
- oY.push_back(y) ;
- oZ.push_back(z) ;
+ for (unsigned int i = 0; i < n; i++) {
+ f >> no_pt >> x >> y >> z;
+ if (verbose) {
+ std::cout << no_pt << std::endl;
+ std::cout << x << " " << y << " " << z << std::endl;
+ }
+ oX.push_back(x);
+ oY.push_back(y);
+ oZ.push_back(z);
}
- f.close() ;
- }
- else{
+ f.close();
+ } else {
return -1;
}
+ } catch (...) {
+ return -1;
}
- catch(...){return -1;}
- return 0 ;
+ return 0;
}
/*!
@@ -742,19 +716,18 @@ int vpCalibration::readGrid(const char* filename, unsigned int &n,
coordinates of the data points resulting from image processing.
*/
-int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color,
- unsigned int thickness, int subsampling_factor)
+int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
{
- for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++ it) {
+ for (std::list<vpImagePoint>::const_iterator it = Lip.begin(); it != Lip.end(); ++it) {
vpImagePoint ip = *it;
if (subsampling_factor > 1.) {
- ip.set_u( ip.get_u() / subsampling_factor);
- ip.set_v( ip.get_v() / subsampling_factor);
+ ip.set_u(ip.get_u() / subsampling_factor);
+ ip.set_v(ip.get_v() / subsampling_factor);
}
- vpDisplay::displayCross(I, ip, 12, color, thickness) ;
+ vpDisplay::displayCross(I, ip, 12, color, thickness);
}
- return 0 ;
+ return 0;
}
/*!
@@ -767,14 +740,13 @@ int vpCalibration::displayData(vpImage<unsigned char> &I, vpColor color,
Admissible values are multiple of 2. Divide by this parameter the
values of the camera parameters.
*/
-int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color,
- unsigned int thickness, int subsampling_factor)
+int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color, unsigned int thickness, int subsampling_factor)
{
double u0_dist = cam_dist.get_u0() / subsampling_factor;
double v0_dist = cam_dist.get_v0() / subsampling_factor;
double px_dist = cam_dist.get_px() / subsampling_factor;
double py_dist = cam_dist.get_py() / subsampling_factor;
- double kud_dist = cam_dist.get_kud() ;
+ double kud_dist = cam_dist.get_kud();
// double kdu_dist = cam_dist.get_kdu() ;
// double u0 = cam.get_u0() ;
@@ -786,39 +758,39 @@ int vpCalibration::displayGrid(vpImage<unsigned char> &I, vpColor color,
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
- for (unsigned int i =0 ; i < npt ; i++)
- {
+ for (unsigned int i = 0; i < npt; i++) {
double oX = *it_LoX;
double oY = *it_LoY;
double oZ = *it_LoZ;
- //double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
- //double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
- //double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
+ // double cX = oX*cMo[0][0]+oY*cMo[0][1]+oZ*cMo[0][2] + cMo[0][3];
+ // double cY = oX*cMo[1][0]+oY*cMo[1][1]+oZ*cMo[1][2] + cMo[1][3];
+ // double cZ = oX*cMo[2][0]+oY*cMo[2][1]+oZ*cMo[2][2] + cMo[2][3];
- //double x = cX/cZ ;
- //double y = cY/cZ ;
+ // double x = cX/cZ ;
+ // double y = cY/cZ ;
// double xp = u0 + x*px ;
// double yp = v0 + y*py ;
- // vpDisplay::displayCross(I,(int)vpMath::round(yp), (int)vpMath::round(xp),
+ // vpDisplay::displayCross(I,(int)vpMath::round(yp),
+ // (int)vpMath::round(xp),
// 5,col) ;
- double cX = oX*cMo_dist[0][0]+oY*cMo_dist[0][1]+oZ*cMo_dist[0][2]+cMo_dist[0][3];
- double cY = oX*cMo_dist[1][0]+oY*cMo_dist[1][1]+oZ*cMo_dist[1][2]+cMo_dist[1][3];
- double cZ = oX*cMo_dist[2][0]+oY*cMo_dist[2][1]+oZ*cMo_dist[2][2]+cMo_dist[2][3];
+ 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 ;
+ double x = cX / cZ;
+ double y = cY / cZ;
- double r2 = 1+kud_dist*(vpMath::sqr(x)+vpMath::sqr(y)) ;
+ double r2 = 1 + kud_dist * (vpMath::sqr(x) + vpMath::sqr(y));
vpImagePoint ip;
- ip.set_u( u0_dist + x*px_dist*r2 );
- ip.set_v( v0_dist + y*py_dist*r2 );
+ ip.set_u(u0_dist + x * px_dist * r2);
+ ip.set_v(v0_dist + y * py_dist * r2);
- vpDisplay::displayCross(I, ip, 6, color, thickness) ;
+ vpDisplay::displayCross(I, ip, 6, color, thickness);
///////////////////////////////////////
// std::cout << oX << " " << oY << " " <<oZ << std::endl ;
diff --git a/modules/vision/src/calibration/vpCalibrationTools.cpp b/modules/vision/src/calibration/vpCalibrationTools.cpp
index eaed621..5450bed 100644
--- a/modules/vision/src/calibration/vpCalibrationTools.cpp
+++ b/modules/vision/src/calibration/vpCalibrationTools.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,23 +37,22 @@
*
*****************************************************************************/
-#include <visp3/vision/vpCalibration.h>
#include <visp3/core/vpMath.h>
-#include <visp3/vision/vpPose.h>
#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/vision/vpCalibration.h>
+#include <visp3/vision/vpPose.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#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) ;
- vpMatrix B(2*npt,9) ;
+ vpMatrix A(2 * npt, 3);
+ vpMatrix B(2 * npt, 9);
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
@@ -61,42 +61,41 @@ vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &c
vpImagePoint ip;
- for (unsigned int i = 0 ; i < npt ; i++)
- {
+ for (unsigned int i = 0; i < npt; i++) {
double x0 = *it_LoX;
double y0 = *it_LoY;
double z0 = *it_LoZ;
ip = *it_Lip;
-
- double xi = ip.get_u() ;
- double yi = ip.get_v() ;
-
- A[2*i][0] = x0*xi;
- A[2*i][1] = y0*xi;
- A[2*i][2] = z0*xi;
- B[2*i][0] = -x0;
- B[2*i][1] = -y0;
- B[2*i][2] = -z0;
- B[2*i][3] = 0.0;
- B[2*i][4] = 0.0;
- B[2*i][5] = 0.0;
- B[2*i][6] = -1.0;
- B[2*i][7] = 0.0;
- B[2*i][8] = xi;
- A[2*i+1][0] = x0*yi;
- A[2*i+1][1] = y0*yi;
- A[2*i+1][2] = z0*yi;
- B[2*i+1][0] = 0.0;
- B[2*i+1][1] = 0.0;
- B[2*i+1][2] = 0.0;
- B[2*i+1][3] = -x0;
- B[2*i+1][4] = -y0;
- B[2*i+1][5] = -z0;
- B[2*i+1][6] = 0.0;
- B[2*i+1][7] = -1.0;
- B[2*i+1][8] = yi;
+
+ double xi = ip.get_u();
+ double yi = ip.get_v();
+
+ A[2 * i][0] = x0 * xi;
+ A[2 * i][1] = y0 * xi;
+ A[2 * i][2] = z0 * xi;
+ B[2 * i][0] = -x0;
+ B[2 * i][1] = -y0;
+ B[2 * i][2] = -z0;
+ B[2 * i][3] = 0.0;
+ B[2 * i][4] = 0.0;
+ B[2 * i][5] = 0.0;
+ B[2 * i][6] = -1.0;
+ B[2 * i][7] = 0.0;
+ B[2 * i][8] = xi;
+ A[2 * i + 1][0] = x0 * yi;
+ A[2 * i + 1][1] = y0 * yi;
+ A[2 * i + 1][2] = z0 * yi;
+ B[2 * i + 1][0] = 0.0;
+ B[2 * i + 1][1] = 0.0;
+ B[2 * i + 1][2] = 0.0;
+ B[2 * i + 1][3] = -x0;
+ B[2 * i + 1][4] = -y0;
+ B[2 * i + 1][5] = -z0;
+ B[2 * i + 1][6] = 0.0;
+ B[2 * i + 1][7] = -1.0;
+ B[2 * i + 1][8] = yi;
++it_LoX;
++it_LoY;
@@ -104,128 +103,125 @@ vpCalibration::calibLagrange(vpCameraParameters &cam_est, vpHomogeneousMatrix &c
++it_Lip;
}
- vpMatrix BtB ; /* compute B^T B */
- BtB = B.t() * B ;
+ vpMatrix BtB; /* compute B^T B */
+ BtB = B.t() * B;
/* compute (B^T B)^(-1) */
/* input : btb (dimension 9 x 9) = (B^T B) */
/* output : btbinv (dimension 9 x 9) = (B^T B)^(-1) */
- vpMatrix BtBinv ;
- BtBinv = BtB.pseudoInverse(1e-16) ;
-
- vpMatrix BtA ;
- BtA = B.t()*A ; /* compute B^T A */
+ vpMatrix BtBinv;
+ BtBinv = BtB.pseudoInverse(1e-16);
+ vpMatrix BtA;
+ BtA = B.t() * A; /* compute B^T A */
- vpMatrix r ;
- r = BtBinv*BtA ; /* compute (B^T B)^(-1) B^T A */
+ vpMatrix r;
+ r = BtBinv * BtA; /* compute (B^T B)^(-1) B^T A */
- vpMatrix e ; /* compute - A^T B (B^T B)^(-1) B^T A*/
- e = -(A.t()*B)*r ;
+ vpMatrix e; /* compute - A^T B (B^T B)^(-1) B^T A*/
+ e = -(A.t() * B) * r;
- vpMatrix AtA ; /* compute A^T A */
- AtA = A.AtA() ;
+ vpMatrix AtA; /* compute A^T A */
+ AtA = A.AtA();
- e += AtA ; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
+ e += AtA; /* compute E = A^T A - A^T B (B^T B)^(-1) B^T A */
- vpColVector x1(3) ;
- vpColVector x2 ;
+ vpColVector x1(3);
+ vpColVector x2;
- e.svd(x1,AtA) ;// destructive on e
+ e.svd(x1, AtA); // destructive on e
// eigenvector computation of E corresponding to the min eigenvalue.
/* SVmax computation*/
- double svm = 0.0;
+ double svm = 0.0;
unsigned int imin = 1;
- for (unsigned int i=0;i<x1.getRows();i++)
- {
- if (x1[i] > svm)
- {
+ for (unsigned int i = 0; i < x1.getRows(); i++) {
+ if (x1[i] > svm) {
svm = x1[i];
imin = i;
}
}
- //svm *= 0.1; /* for the rank */
+ // svm *= 0.1; /* for the rank */
- for (unsigned int i=0;i<x1.getRows();i++)
- {
- if (x1[i] < x1[imin]) imin = i;
+ for (unsigned int i = 0; i < x1.getRows(); i++) {
+ if (x1[i] < x1[imin])
+ imin = i;
}
- for (unsigned int i=0;i<x1.getRows();i++)
+ for (unsigned int i = 0; i < x1.getRows(); i++)
x1[i] = AtA[i][imin];
- x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
-
+ x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
- vpColVector sol(12) ;
- vpColVector resul(7) ;
- for (unsigned int i=0;i<3;i++) sol[i] = x1[i]; /* X_1 */
- for (unsigned int i=0;i<9;i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
+ vpColVector sol(12);
+ vpColVector resul(7);
+ for (unsigned int i = 0; i < 3; i++)
+ sol[i] = x1[i]; /* X_1 */
+ for (unsigned int i = 0; i < 9; i++) /* X_2 = - (B^T B)^(-1) B^T A X_1 */
{
- sol[i+3] = x2[i];
+ sol[i + 3] = x2[i];
}
- if (sol[11] < 0.0) for (unsigned int i=0;i<12;i++) sol[i] = -sol[i]; /* since Z0 > 0 */
+ if (sol[11] < 0.0)
+ for (unsigned int i = 0; i < 12; i++)
+ sol[i] = -sol[i]; /* since Z0 > 0 */
- resul[0] = sol[3]*sol[0]+sol[4]*sol[1]+sol[5]*sol[2]; /* u0 */
+ resul[0] = sol[3] * sol[0] + sol[4] * sol[1] + sol[5] * sol[2]; /* u0 */
- resul[1] = sol[6]*sol[0]+sol[7]*sol[1]+sol[8]*sol[2]; /* v0 */
+ resul[1] = sol[6] * sol[0] + sol[7] * sol[1] + sol[8] * sol[2]; /* v0 */
- resul[2] = sqrt(sol[3]*sol[3]+sol[4]*sol[4]+sol[5]*sol[5] /* px */
- -resul[0]*resul[0]);
- resul[3] = sqrt(sol[6]*sol[6]+sol[7]*sol[7]+sol[8]*sol[8] /* py */
- -resul[1]*resul[1]);
+ resul[2] = sqrt(sol[3] * sol[3] + sol[4] * sol[4] + sol[5] * sol[5] /* px */
+ - resul[0] * resul[0]);
+ resul[3] = sqrt(sol[6] * sol[6] + sol[7] * sol[7] + sol[8] * sol[8] /* py */
+ - resul[1] * resul[1]);
- cam_est.initPersProjWithoutDistortion(resul[2],resul[3],resul[0],resul[1]);
+ cam_est.initPersProjWithoutDistortion(resul[2], resul[3], resul[0], resul[1]);
- resul[4] = (sol[9]-sol[11]*resul[0])/resul[2]; /* X0 */
- resul[5] = (sol[10]-sol[11]*resul[1])/resul[3]; /* Y0 */
- resul[6] = sol[11]; /* Z0 */
+ resul[4] = (sol[9] - sol[11] * resul[0]) / resul[2]; /* X0 */
+ resul[5] = (sol[10] - sol[11] * resul[1]) / resul[3]; /* Y0 */
+ resul[6] = sol[11]; /* Z0 */
- vpMatrix rd(3,3) ;
+ vpMatrix rd(3, 3);
/* fill rotation matrix */
- for (unsigned int i=0;i<3;i++) rd[0][i] = (sol[i+3]-sol[i]*resul[0])/resul[2];
- for (unsigned int i=0;i<3;i++) rd[1][i] = (sol[i+6]-sol[i]*resul[1])/resul[3];
- for (unsigned int i=0;i<3;i++) rd[2][i] = sol[i];
+ for (unsigned int i = 0; i < 3; i++)
+ rd[0][i] = (sol[i + 3] - sol[i] * resul[0]) / resul[2];
+ for (unsigned int i = 0; i < 3; i++)
+ rd[1][i] = (sol[i + 6] - sol[i] * resul[1]) / resul[3];
+ for (unsigned int i = 0; i < 3; i++)
+ rd[2][i] = sol[i];
// std::cout << "norme X1 " << x1.sumSquare() <<std::endl;
// std::cout << rd*rd.t() ;
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- for (unsigned int j=0 ; j < 3 ; j++)
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++)
cMo_est[i][j] = rd[i][j];
}
- for (unsigned int i=0 ; i < 3 ; i++) cMo_est[i][3] = resul[i+4] ;
+ for (unsigned int i = 0; i < 3; i++)
+ cMo_est[i][3] = resul[i + 4];
- this->cMo = cMo_est ;
+ this->cMo = cMo_est;
this->cMo_dist = cMo_est;
- double deviation,deviation_dist;
- this->computeStdDeviation(deviation,deviation_dist);
-
+ double deviation, deviation_dist;
+ 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::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int n_points = npt ;
+ unsigned int n_points = npt;
- vpColVector oX(n_points), cX(n_points) ;
- vpColVector oY(n_points), cY(n_points) ;
- vpColVector oZ(n_points), cZ(n_points) ;
- vpColVector u(n_points) ;
- vpColVector v(n_points) ;
+ vpColVector oX(n_points), cX(n_points);
+ vpColVector oY(n_points), cY(n_points);
+ vpColVector oZ(n_points), cZ(n_points);
+ vpColVector u(n_points);
+ vpColVector v(n_points);
- vpColVector P(2*n_points) ;
- vpColVector Pd(2*n_points) ;
+ vpColVector P(2 * n_points);
+ vpColVector Pd(2 * n_points);
vpImagePoint ip;
@@ -234,16 +230,15 @@ vpCalibration::calibVVS(vpCameraParameters &cam_est,
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
- for (unsigned int i =0 ; i < n_points ; i++)
- {
- oX[i] = *it_LoX;
- oY[i] = *it_LoY;
- oZ[i] = *it_LoZ;
+ for (unsigned int i = 0; i < n_points; i++) {
+ oX[i] = *it_LoX;
+ oY[i] = *it_LoY;
+ oZ[i] = *it_LoZ;
ip = *it_Lip;
- u[i] = ip.get_u() ;
- v[i] = ip.get_v() ;
+ u[i] = ip.get_u();
+ v[i] = ip.get_v();
++it_LoX;
++it_LoY;
@@ -252,876 +247,813 @@ vpCalibration::calibVVS(vpCameraParameters &cam_est,
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
+ iter++;
+ residu_1 = r;
double px = cam_est.get_px();
double py = cam_est.get_py();
double u0 = cam_est.get_u0();
double v0 = cam_est.get_v0();
- r = 0 ;
+ 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];
+ for (unsigned int i = 0; i < n_points; i++) {
+ cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
+ cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
+ cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
- Pd[2*i] = u[i] ;
- Pd[2*i+1] = v[i] ;
+ Pd[2 * i] = u[i];
+ Pd[2 * i + 1] = v[i];
- P[2*i] = cX[i]/cZ[i]*px + u0 ;
- P[2*i+1] = cY[i]/cZ[i]*py + v0 ;
+ 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 ;
- error = P-Pd ;
- //r = r/n_points ;
-
- vpMatrix L(n_points*2,10) ;
- for (unsigned int i=0 ; i < n_points; i++)
- {
- double x = cX[i] ;
- double y = cY[i] ;
- double z = cZ[i] ;
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
+ vpColVector error;
+ error = P - Pd;
+ // r = r/n_points ;
+
+ vpMatrix L(n_points * 2, 10);
+ for (unsigned int i = 0; i < n_points; i++) {
+ double x = cX[i];
+ double y = cY[i];
+ double z = cZ[i];
+ double inv_z = 1 / z;
+
+ double X = x * inv_z;
+ double Y = y * inv_z;
//---------------
{
- L[2*i][0] = px * (-inv_z) ;
- L[2*i][1] = 0 ;
- L[2*i][2] = px*X*inv_z ;
- L[2*i][3] = px*X*Y ;
- L[2*i][4] = -px*(1+X*X) ;
- L[2*i][5] = px*Y ;
+ L[2 * i][0] = px * (-inv_z);
+ L[2 * i][1] = 0;
+ L[2 * i][2] = px * X * inv_z;
+ L[2 * i][3] = px * X * Y;
+ L[2 * i][4] = -px * (1 + X * X);
+ L[2 * i][5] = px * Y;
}
{
- L[2*i][6]= 1 ;
- L[2*i][7]= 0 ;
- L[2*i][8]= X ;
- L[2*i][9]= 0;
+ L[2 * i][6] = 1;
+ L[2 * i][7] = 0;
+ L[2 * i][8] = X;
+ L[2 * i][9] = 0;
}
{
- L[2*i+1][0] = 0 ;
- L[2*i+1][1] = py*(-inv_z) ;
- L[2*i+1][2] = py*(Y*inv_z) ;
- L[2*i+1][3] = py* (1+Y*Y) ;
- L[2*i+1][4] = -py*X*Y ;
- L[2*i+1][5] = -py*X ;
+ L[2 * i + 1][0] = 0;
+ L[2 * i + 1][1] = py * (-inv_z);
+ L[2 * i + 1][2] = py * (Y * inv_z);
+ L[2 * i + 1][3] = py * (1 + Y * Y);
+ L[2 * i + 1][4] = -py * X * Y;
+ L[2 * i + 1][5] = -py * X;
}
{
- L[2*i+1][6]= 0 ;
- L[2*i+1][7]= 1 ;
- L[2*i+1][8]= 0;
- L[2*i+1][9]= Y ;
+ L[2 * i + 1][6] = 0;
+ L[2 * i + 1][7] = 1;
+ L[2 * i + 1][8] = 0;
+ L[2 * i + 1][9] = Y;
}
- } // end interaction
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
+ } // end interaction
+ vpMatrix Lp;
+ Lp = L.pseudoInverse(1e-10);
- vpColVector e ;
- e = Lp*error ;
+ vpColVector e;
+ e = Lp * error;
- vpColVector Tc, Tc_v(6) ;
- Tc = -e*gain ;
+ vpColVector Tc, Tc_v(6);
+ Tc = -e * gain;
// Tc_v =0 ;
- for (unsigned int i=0 ; i <6 ; i++)
- Tc_v[i] = Tc[i] ;
+ 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 ;
+ cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
-
+ 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")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
- this->cMo = cMo_est;
+ this->cMo = cMo_est;
this->cMo_dist = cMo_est;
this->residual = r;
this->residual_dist = r;
if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+ std::cout << " std dev " << sqrt(r / n_points) << std::endl;
// Restore ostream format
std::cout.flags(original_flags);
}
-void
-vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam_est,
- double &globalReprojectionError,
- bool verbose)
+void vpCalibration::calibVVSMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
+ double &globalReprojectionError, bool verbose)
{
- std::ios::fmtflags original_flags( std::cout.flags() );
+ std::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int nbPoint[256]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPoint[256]; // number of points by image
+ unsigned int nbPointTotal = 0; // total number of points
unsigned int nbPose = (unsigned int)table_cal.size();
- unsigned int nbPose6 = 6*nbPose;
+ unsigned int nbPose6 = 6 * nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
+ for (unsigned int i = 0; i < nbPose; i++) {
nbPoint[i] = table_cal[i].npt;
nbPointTotal += nbPoint[i];
}
if (nbPointTotal < 4) {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
+ // vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
}
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
+ vpColVector oX(nbPointTotal), cX(nbPointTotal);
+ vpColVector oY(nbPointTotal), cY(nbPointTotal);
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
+ vpColVector u(nbPointTotal);
+ vpColVector v(nbPointTotal);
- vpColVector P(2*nbPointTotal) ;
- vpColVector Pd(2*nbPointTotal) ;
+ vpColVector P(2 * nbPointTotal);
+ vpColVector Pd(2 * nbPointTotal);
vpImagePoint ip;
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ unsigned int curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
-
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
-
+
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
+
ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
-
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
-
+ u[curPoint] = ip.get_u();
+ v[curPoint] = ip.get_v();
+
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
+
curPoint++;
}
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
+
+ iter++;
+ residu_1 = r;
- iter++ ;
- residu_1 = r ;
-
double px = cam_est.get_px();
double py = cam_est.get_py();
double u0 = cam_est.get_u0();
double v0 = cam_est.get_v0();
-
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+
+ r = 0;
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
-
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
-
- Pd[curPoint2] = u[curPoint] ;
- Pd[curPoint2+1] = v[curPoint] ;
-
- P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
- P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
-
- r += (vpMath::sqr(P[curPoint2]-Pd[curPoint2])
- + vpMath::sqr(P[curPoint2+1]-Pd[curPoint2+1])) ;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ unsigned int curPoint2 = 2 * curPoint;
+
+ cX[curPoint] =
+ oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] =
+ oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] =
+ oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
+
+ Pd[curPoint2] = u[curPoint];
+ Pd[curPoint2 + 1] = v[curPoint];
+
+ P[curPoint2] = cX[curPoint] / cZ[curPoint] * px + u0;
+ P[curPoint2 + 1] = cY[curPoint] / cZ[curPoint] * py + v0;
+
+ r += (vpMath::sqr(P[curPoint2] - Pd[curPoint2]) + vpMath::sqr(P[curPoint2 + 1] - Pd[curPoint2 + 1]));
curPoint++;
}
}
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
+ vpColVector error;
+ error = P - Pd;
+ // r = r/nbPointTotal ;
- vpMatrix L(nbPointTotal*2,nbPose6+4) ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
+ vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
+ unsigned int q = 6 * p;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ unsigned int curPoint2 = 2 * curPoint;
unsigned int curPoint21 = curPoint2 + 1;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
+ double x = cX[curPoint];
+ double y = cY[curPoint];
+ double z = cZ[curPoint];
+
+ double inv_z = 1 / z;
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
+ double X = x * inv_z;
+ double Y = y * inv_z;
//---------------
{
{
- L[curPoint2][q] = px * (-inv_z) ;
- L[curPoint2][q+1] = 0 ;
- L[curPoint2][q+2] = px*(X*inv_z) ;
- L[curPoint2][q+3] = px*X*Y ;
- L[curPoint2][q+4] = -px*(1+X*X) ;
- L[curPoint2][q+5] = px*Y ;
+ L[curPoint2][q] = px * (-inv_z);
+ L[curPoint2][q + 1] = 0;
+ L[curPoint2][q + 2] = px * (X * inv_z);
+ L[curPoint2][q + 3] = px * X * Y;
+ L[curPoint2][q + 4] = -px * (1 + X * X);
+ L[curPoint2][q + 5] = px * Y;
}
{
- L[curPoint2][nbPose6]= 1 ;
- L[curPoint2][nbPose6+1]= 0 ;
- L[curPoint2][nbPose6+2]= X ;
- L[curPoint2][nbPose6+3]= 0;
+ L[curPoint2][nbPose6] = 1;
+ L[curPoint2][nbPose6 + 1] = 0;
+ L[curPoint2][nbPose6 + 2] = X;
+ L[curPoint2][nbPose6 + 3] = 0;
}
{
- L[curPoint21][q] = 0 ;
- L[curPoint21][q+1] = py*(-inv_z) ;
- L[curPoint21][q+2] = py*(Y*inv_z) ;
- L[curPoint21][q+3] = py* (1+Y*Y) ;
- L[curPoint21][q+4] = -py*X*Y ;
- L[curPoint21][q+5] = -py*X ;
+ L[curPoint21][q] = 0;
+ L[curPoint21][q + 1] = py * (-inv_z);
+ L[curPoint21][q + 2] = py * (Y * inv_z);
+ L[curPoint21][q + 3] = py * (1 + Y * Y);
+ L[curPoint21][q + 4] = -py * X * Y;
+ L[curPoint21][q + 5] = -py * X;
}
{
- L[curPoint21][nbPose6]= 0 ;
- L[curPoint21][nbPose6+1]= 1 ;
- L[curPoint21][nbPose6+2]= 0;
- L[curPoint21][nbPose6+3]= Y ;
+ L[curPoint21][nbPose6] = 0;
+ L[curPoint21][nbPose6 + 1] = 1;
+ L[curPoint21][nbPose6 + 2] = 0;
+ L[curPoint21][nbPose6 + 3] = Y;
}
-
}
curPoint++;
- } // end interaction
+ } // end interaction
}
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
+ vpMatrix Lp;
+ Lp = L.pseudoInverse(1e-10);
- vpColVector e ;
- e = Lp*error ;
+ vpColVector e;
+ e = Lp * error;
- vpColVector Tc, Tc_v(nbPose6) ;
- Tc = -e*gain ;
+ vpColVector Tc, Tc_v(nbPose6);
+ Tc = -e * gain;
// Tc_v =0 ;
- for (unsigned int i = 0 ; i < nbPose6 ; i++)
- Tc_v[i] = Tc[i] ;
+ 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) ;
+ 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];
+ for (unsigned int p = 0; p < nbPose; p++) {
+ for (unsigned int i = 0; i < 6; i++)
+ Tc_v_Tmp[i] = Tc_v[6 * p + i];
- table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
- * table_cal[p].cMo;
+ 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;
-
+ 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")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cMo_dist = table_cal[p].cMo ;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ table_cal[p].cMo_dist = table_cal[p].cMo;
table_cal[p].cam = cam_est;
table_cal[p].cam_dist = cam_est;
- double deviation,deviation_dist;
- table_cal[p].computeStdDeviation(deviation,deviation_dist);
+ double deviation, deviation_dist;
+ table_cal[p].computeStdDeviation(deviation, deviation_dist);
}
- globalReprojectionError = sqrt(r/nbPointTotal);
+ globalReprojectionError = sqrt(r / nbPointTotal);
// Restore ostream format
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::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int n_points =npt ;
+ unsigned int n_points = npt;
- vpColVector oX(n_points), cX(n_points) ;
- vpColVector oY(n_points), cY(n_points) ;
- vpColVector oZ(n_points), cZ(n_points) ;
- vpColVector u(n_points) ;
- vpColVector v(n_points) ;
+ vpColVector oX(n_points), cX(n_points);
+ vpColVector oY(n_points), cY(n_points);
+ vpColVector oZ(n_points), cZ(n_points);
+ vpColVector u(n_points);
+ vpColVector v(n_points);
- vpColVector P(4*n_points) ;
- vpColVector Pd(4*n_points) ;
+ vpColVector P(4 * n_points);
+ vpColVector Pd(4 * n_points);
std::list<double>::const_iterator it_LoX = LoX.begin();
std::list<double>::const_iterator it_LoY = LoY.begin();
std::list<double>::const_iterator it_LoZ = LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = Lip.begin();
-
+
vpImagePoint ip;
- for (unsigned int i =0 ; i < n_points ; i++)
- {
- oX[i] = *it_LoX;
- oY[i] = *it_LoY;
- oZ[i] = *it_LoZ;
+ for (unsigned int i = 0; i < n_points; i++) {
+ oX[i] = *it_LoX;
+ oY[i] = *it_LoY;
+ oZ[i] = *it_LoZ;
ip = *it_Lip;
u[i] = ip.get_u();
v[i] = ip.get_v();
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
-
- r = 0 ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
-
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
-
- double inv_px = 1/px ;
- double inv_py = 1/py ;
-
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
-
- double k2ud = 2*kud;
- double k2du = 2*kdu;
- vpMatrix L(n_points*4,12) ;
-
- for (unsigned int i=0 ; i < n_points; i++)
- {
- unsigned int i4 = 4*i;
- unsigned int i41 = 4*i+1;
- unsigned int i42 = 4*i+2;
- unsigned int i43 = 4*i+3;
-
- cX[i] = oX[i]*cMo_est[0][0]+oY[i]*cMo_est[0][1]+oZ[i]*cMo_est[0][2] + cMo_est[0][3];
- cY[i] = oX[i]*cMo_est[1][0]+oY[i]*cMo_est[1][1]+oZ[i]*cMo_est[1][2] + cMo_est[1][3];
- cZ[i] = oX[i]*cMo_est[2][0]+oY[i]*cMo_est[2][1]+oZ[i]*cMo_est[2][2] + cMo_est[2][3];
-
- double x = cX[i] ;
- double y = cY[i] ;
- double z = cZ[i] ;
- double inv_z = 1/z;
-
- double X = x*inv_z ;
- double Y = y*inv_z ;
-
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
-
- double up = u[i] ;
- double vp = v[i] ;
-
- Pd[i4] = up ;
- Pd[i41] = vp ;
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
+ iter++;
+ residu_1 = r;
+
+ r = 0;
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
+
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+
+ double inv_px = 1 / px;
+ double inv_py = 1 / py;
+
+ double kud = cam_est.get_kud();
+ double kdu = cam_est.get_kdu();
+
+ double k2ud = 2 * kud;
+ double k2du = 2 * kdu;
+ vpMatrix L(n_points * 4, 12);
+
+ for (unsigned int i = 0; i < n_points; i++) {
+ unsigned int i4 = 4 * i;
+ unsigned int i41 = 4 * i + 1;
+ unsigned int i42 = 4 * i + 2;
+ unsigned int i43 = 4 * i + 3;
+
+ cX[i] = oX[i] * cMo_est[0][0] + oY[i] * cMo_est[0][1] + oZ[i] * cMo_est[0][2] + cMo_est[0][3];
+ cY[i] = oX[i] * cMo_est[1][0] + oY[i] * cMo_est[1][1] + oZ[i] * cMo_est[1][2] + cMo_est[1][3];
+ cZ[i] = oX[i] * cMo_est[2][0] + oY[i] * cMo_est[2][1] + oZ[i] * cMo_est[2][2] + cMo_est[2][3];
+
+ double x = cX[i];
+ double y = cY[i];
+ double z = cZ[i];
+ double inv_z = 1 / z;
+
+ double X = x * inv_z;
+ double Y = y * inv_z;
+
+ double X2 = X * X;
+ double Y2 = Y * Y;
+ double XY = X * Y;
+
+ double up = u[i];
+ double vp = v[i];
+
+ Pd[i4] = up;
+ Pd[i41] = vp;
double up0 = up - u0;
double vp0 = vp - v0;
double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
-
- double yp0 = vp0 * inv_py;
+ double xp02 = xp0 * xp0;
+
+ double yp0 = vp0 * inv_py;
double yp02 = yp0 * yp0;
-
- double r2du = xp02 + yp02 ;
- double kr2du = kdu * r2du;
- P[i4] = u0 + px*X - kr2du *(up0) ;
- P[i41] = v0 + py*Y - kr2du *(vp0) ;
+ double r2du = xp02 + yp02;
+ double kr2du = kdu * r2du;
+
+ P[i4] = u0 + px * X - kr2du * (up0);
+ P[i41] = v0 + py * Y - kr2du * (vp0);
- double r2ud = X2 + Y2 ;
+ double r2ud = X2 + Y2;
double kr2ud = 1 + kud * r2ud;
-
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
- Pd[i42] = up ;
- Pd[i43] = vp ;
+ double Axx = px * (kr2ud + k2ud * X2);
+ double Axy = px * k2ud * XY;
+ double Ayy = py * (kr2ud + k2ud * Y2);
+ double Ayx = py * k2ud * XY;
- P[i42] = u0 + px*X*kr2ud ;
- P[i43] = v0 + py*Y*kr2ud ;
+ Pd[i42] = up;
+ Pd[i43] = vp;
- 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;
+ P[i42] = u0 + px * X * kr2ud;
+ P[i43] = v0 + py * Y * kr2ud;
+
+ r += (vpMath::sqr(P[i4] - Pd[i4]) + vpMath::sqr(P[i41] - Pd[i41]) + vpMath::sqr(P[i42] - Pd[i42]) +
+ vpMath::sqr(P[i43] - Pd[i43])) *
+ 0.5;
//--distorted to undistorted
{
{
- L[i4][0] = px * (-inv_z) ;
- L[i4][1] = 0 ;
- L[i4][2] = px*X*inv_z ;
- L[i4][3] = px*X*Y ;
- L[i4][4] = -px*(1+X2) ;
- L[i4][5] = px*Y ;
+ L[i4][0] = px * (-inv_z);
+ L[i4][1] = 0;
+ L[i4][2] = px * X * inv_z;
+ L[i4][3] = px * X * Y;
+ L[i4][4] = -px * (1 + X2);
+ L[i4][5] = px * Y;
}
{
- L[i4][6]= 1 + kr2du + k2du*xp02 ;
- L[i4][7]= k2du*up0*yp0*inv_py ;
- L[i4][8]= X + k2du*xp02*xp0 ;
- L[i4][9]= k2du*up0*yp02*inv_py ;
- L[i4][10] = -(up0)*(r2du) ;
- L[i4][11] = 0 ;
+ L[i4][6] = 1 + kr2du + k2du * xp02;
+ L[i4][7] = k2du * up0 * yp0 * inv_py;
+ L[i4][8] = X + k2du * xp02 * xp0;
+ L[i4][9] = k2du * up0 * yp02 * inv_py;
+ L[i4][10] = -(up0) * (r2du);
+ L[i4][11] = 0;
}
{
- L[i41][0] = 0 ;
- L[i41][1] = py*(-inv_z) ;
- L[i41][2] = py*Y*inv_z ;
- L[i41][3] = py* (1+Y2) ;
- L[i41][4] = -py*XY ;
- L[i41][5] = -py*X ;
+ L[i41][0] = 0;
+ L[i41][1] = py * (-inv_z);
+ L[i41][2] = py * Y * inv_z;
+ L[i41][3] = py * (1 + Y2);
+ L[i41][4] = -py * XY;
+ L[i41][5] = -py * X;
}
{
- L[i41][6]= k2du*xp0*vp0*inv_px ;
- L[i41][7]= 1 + kr2du + k2du*yp02;
- L[i41][8]= k2du*vp0*xp02*inv_px;
- L[i41][9]= Y + k2du*yp02*yp0;
- L[i41][10] = -vp0*r2du ;
- L[i41][11] = 0 ;
+ L[i41][6] = k2du * xp0 * vp0 * inv_px;
+ L[i41][7] = 1 + kr2du + k2du * yp02;
+ L[i41][8] = k2du * vp0 * xp02 * inv_px;
+ L[i41][9] = Y + k2du * yp02 * yp0;
+ L[i41][10] = -vp0 * r2du;
+ L[i41][11] = 0;
}
- //---undistorted to distorted
- {
- L[i42][0] = Axx*(-inv_z) ;
- L[i42][1] = Axy*(-inv_z) ;
- L[i42][2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[i42][3] = Axx*X*Y + Axy*(1+Y2);
- L[i42][4] = -Axx*(1+X2) - Axy*XY;
- L[i42][5] = Axx*Y -Axy*X;
- }
- {
- L[i42][6]= 1 ;
- L[i42][7]= 0 ;
- L[i42][8]= X*kr2ud ;
- L[i42][9]= 0;
- L[i42][10] = 0 ;
- L[i42][11] = px*X*r2ud ;
- }
- {
- L[i43][0] = Ayx*(-inv_z) ;
- L[i43][1] = Ayy*(-inv_z) ;
- L[i43][2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[i43][3] = Ayx*XY + Ayy*(1+Y2) ;
- L[i43][4] = -Ayx*(1+X2) -Ayy*XY ;
- L[i43][5] = Ayx*Y -Ayy*X;
- }
- {
- L[i43][6]= 0 ;
- L[i43][7]= 1;
- L[i43][8]= 0;
- L[i43][9]= Y*kr2ud ;
- L[i43][10] = 0 ;
- L[i43][11] = py*Y*r2ud ;
- }
- } // end interaction
- } // end interaction
-
- vpColVector error ;
- error = P-Pd ;
- //r = r/n_points ;
-
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
-
- vpColVector e ;
- e = Lp*error ;
-
- vpColVector Tc, Tc_v(6) ;
- Tc = -e*gain ;
-
- for (unsigned int i=0 ; i <6 ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9],
- u0 + Tc[6], v0 + Tc[7],
- kud + Tc[11],
- kdu + Tc[10]);
-
- cMo_est = vpExponentialMap::direct(Tc_v).inverse()*cMo_est ;
- if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+ //---undistorted to distorted
+ {
+ L[i42][0] = Axx * (-inv_z);
+ L[i42][1] = Axy * (-inv_z);
+ L[i42][2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
+ L[i42][3] = Axx * X * Y + Axy * (1 + Y2);
+ L[i42][4] = -Axx * (1 + X2) - Axy * XY;
+ L[i42][5] = Axx * Y - Axy * X;
+ }
+ {
+ L[i42][6] = 1;
+ L[i42][7] = 0;
+ L[i42][8] = X * kr2ud;
+ L[i42][9] = 0;
+ L[i42][10] = 0;
+ L[i42][11] = px * X * r2ud;
+ }
+ {
+ L[i43][0] = Ayx * (-inv_z);
+ L[i43][1] = Ayy * (-inv_z);
+ L[i43][2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
+ L[i43][3] = Ayx * XY + Ayy * (1 + Y2);
+ L[i43][4] = -Ayx * (1 + X2) - Ayy * XY;
+ L[i43][5] = Ayx * Y - Ayy * X;
+ }
+ {
+ L[i43][6] = 0;
+ L[i43][7] = 1;
+ L[i43][8] = 0;
+ L[i43][9] = Y * kr2ud;
+ L[i43][10] = 0;
+ L[i43][11] = py * Y * r2ud;
+ }
+ } // end interaction
+ } // end interaction
+
+ vpColVector error;
+ error = P - Pd;
+ // r = r/n_points ;
+
+ vpMatrix Lp;
+ Lp = L.pseudoInverse(1e-10);
+
+ vpColVector e;
+ e = Lp * error;
+
+ vpColVector Tc, Tc_v(6);
+ Tc = -e * gain;
+
+ for (unsigned int i = 0; i < 6; i++)
+ Tc_v[i] = Tc[i];
+
+ cam_est.initPersProjWithDistortion(px + Tc[8], py + Tc[9], u0 + Tc[6], v0 + Tc[7], kud + Tc[11], kdu + Tc[10]);
+ cMo_est = vpExponentialMap::direct(Tc_v).inverse() * cMo_est;
+ if (verbose)
+ std::cout << " std dev " << sqrt(r / n_points) << std::endl;
}
- if (iter == nbIterMax)
- {
- vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)",nbIterMax);
- throw(vpCalibrationException(vpCalibrationException::convergencyError,
- "Maximum number of iterations reached")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
this->residual_dist = r;
- this->cMo_dist = cMo_est ;
- this->cam_dist = cam_est ;
+ this->cMo_dist = cMo_est;
+ this->cam_dist = cam_est;
if (verbose)
- std::cout << " std dev " << sqrt(r/n_points) << std::endl;
+ std::cout << " std dev " << sqrt(r / n_points) << std::endl;
// Restore ostream format
std::cout.flags(original_flags);
}
-
-void
-vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal,
- vpCameraParameters &cam_est, double &globalReprojectionError,
- bool verbose)
+void vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal, vpCameraParameters &cam_est,
+ double &globalReprojectionError, bool verbose)
{
- std::ios::fmtflags original_flags( std::cout.flags() );
+ std::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int nbPoint[1024]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPoint[1024]; // number of points by image
+ unsigned int nbPointTotal = 0; // total number of points
unsigned int nbPose = (unsigned int)table_cal.size();
- unsigned int nbPose6 = 6*nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
+ unsigned int nbPose6 = 6 * nbPose;
+ for (unsigned int i = 0; i < nbPose; i++) {
nbPoint[i] = table_cal[i].npt;
nbPointTotal += nbPoint[i];
}
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
+ if (nbPointTotal < 4) {
+ // vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
}
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
+ vpColVector oX(nbPointTotal), cX(nbPointTotal);
+ vpColVector oY(nbPointTotal), cY(nbPointTotal);
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
+ vpColVector u(nbPointTotal);
+ vpColVector v(nbPointTotal);
- vpColVector P(4*nbPointTotal) ;
- vpColVector Pd(4*nbPointTotal) ;
+ vpColVector P(4 * nbPointTotal);
+ vpColVector Pd(4 * nbPointTotal);
vpImagePoint ip;
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ unsigned int curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
+ u[curPoint] = ip.get_u();
+ v[curPoint] = ip.get_v();
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
curPoint++;
}
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
+ iter++;
+ residu_1 = r;
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ r = 0;
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ cX[curPoint] =
+ oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] =
+ oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] =
+ oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
curPoint++;
}
}
+ vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
+ curPoint = 0; // current point indice
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
- vpMatrix L(nbPointTotal*4,nbPose6+6) ;
- curPoint = 0 ; //current point indice
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
+ double inv_px = 1 / px;
+ double inv_py = 1 / py;
- double inv_px = 1/px ;
- double inv_py = 1/py ;
+ double kud = cam_est.get_kud();
+ double kdu = cam_est.get_kdu();
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
+ double k2ud = 2 * kud;
+ double k2du = 2 * kdu;
- double k2ud = 2*kud;
- double k2du = 2*kdu;
-
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint4 = 4*curPoint;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ unsigned int q = 6 * p;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ unsigned int curPoint4 = 4 * curPoint;
+ double x = cX[curPoint];
+ double y = cY[curPoint];
+ double z = cZ[curPoint];
+
+ double inv_z = 1 / z;
+ double X = x * inv_z;
+ double Y = y * inv_z;
- double inv_z = 1/z;
- double X = x*inv_z ;
- double Y = y*inv_z ;
+ double X2 = X * X;
+ double Y2 = Y * Y;
+ double XY = X * Y;
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
-
- double up = u[curPoint] ;
- double vp = v[curPoint] ;
+ double up = u[curPoint];
+ double vp = v[curPoint];
- Pd[curPoint4] = up ;
- Pd[curPoint4+1] = vp ;
+ Pd[curPoint4] = up;
+ Pd[curPoint4 + 1] = vp;
double up0 = up - u0;
double vp0 = vp - v0;
double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
-
+ double xp02 = xp0 * xp0;
+
double yp0 = vp0 * inv_py;
double yp02 = yp0 * yp0;
-
- double r2du = xp02 + yp02 ;
+
+ double r2du = xp02 + yp02;
double kr2du = kdu * r2du;
- P[curPoint4] = u0 + px*X - kr2du *(up0) ;
- P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
+ P[curPoint4] = u0 + px * X - kr2du * (up0);
+ P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
- double r2ud = X2 + Y2 ;
+ double r2ud = X2 + Y2;
double kr2ud = 1 + kud * r2ud;
-
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
- Pd[curPoint4+2] = up ;
- Pd[curPoint4+3] = vp ;
+ double Axx = px * (kr2ud + k2ud * X2);
+ double Axy = px * k2ud * XY;
+ double Ayy = py * (kr2ud + k2ud * Y2);
+ double Ayx = py * k2ud * XY;
- P[curPoint4+2] = u0 + px*X*kr2ud ;
- P[curPoint4+3] = v0 + py*Y*kr2ud ;
+ Pd[curPoint4 + 2] = up;
+ Pd[curPoint4 + 3] = vp;
- 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 ;
+ P[curPoint4 + 2] = u0 + px * X * kr2ud;
+ P[curPoint4 + 3] = v0 + py * Y * kr2ud;
+
+ r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
+ vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
+ 0.5;
unsigned int curInd = curPoint4;
//---------------
{
{
- L[curInd][q] = px * (-inv_z) ;
- L[curInd][q+1] = 0 ;
- L[curInd][q+2] = px*X*inv_z ;
- L[curInd][q+3] = px*X*Y ;
- L[curInd][q+4] = -px*(1+X2) ;
- L[curInd][q+5] = px*Y ;
+ L[curInd][q] = px * (-inv_z);
+ L[curInd][q + 1] = 0;
+ L[curInd][q + 2] = px * X * inv_z;
+ L[curInd][q + 3] = px * X * Y;
+ L[curInd][q + 4] = -px * (1 + X2);
+ L[curInd][q + 5] = px * Y;
}
{
- L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
- L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
- L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
- L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
- L[curInd][nbPose6+4] = -(up0)*(r2du) ;
- L[curInd][nbPose6+5] = 0 ;
+ L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
+ L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
+ L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
+ L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
+ L[curInd][nbPose6 + 4] = -(up0) * (r2du);
+ L[curInd][nbPose6 + 5] = 0;
}
- curInd++;
+ curInd++;
{
- L[curInd][q] = 0 ;
- L[curInd][q+1] = py*(-inv_z) ;
- L[curInd][q+2] = py*Y*inv_z ;
- L[curInd][q+3] = py* (1+Y2) ;
- L[curInd][q+4] = -py*XY ;
- L[curInd][q+5] = -py*X ;
+ L[curInd][q] = 0;
+ L[curInd][q + 1] = py * (-inv_z);
+ L[curInd][q + 2] = py * Y * inv_z;
+ L[curInd][q + 3] = py * (1 + Y2);
+ L[curInd][q + 4] = -py * XY;
+ L[curInd][q + 5] = -py * X;
}
{
- L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
- L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
- L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
- L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
- L[curInd][nbPose6+4] = -vp0*r2du ;
- L[curInd][nbPose6+5] = 0 ;
+ L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
+ L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
+ L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
+ L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
+ L[curInd][nbPose6 + 4] = -vp0 * r2du;
+ L[curInd][nbPose6 + 5] = 0;
}
- curInd++;
- //---undistorted to distorted
+ curInd++;
+ //---undistorted to distorted
{
- L[curInd][q] = Axx*(-inv_z) ;
- L[curInd][q+1] = Axy*(-inv_z) ;
- L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
- L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
- L[curInd][q+5] = Axx*Y -Axy*X;
+ L[curInd][q] = Axx * (-inv_z);
+ L[curInd][q + 1] = Axy * (-inv_z);
+ L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
+ L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
+ L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
+ L[curInd][q + 5] = Axx * Y - Axy * X;
}
{
- L[curInd][nbPose6]= 1 ;
- L[curInd][nbPose6+1]= 0 ;
- L[curInd][nbPose6+2]= X*kr2ud ;
- L[curInd][nbPose6+3]= 0;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = px*X*r2ud ;
+ L[curInd][nbPose6] = 1;
+ L[curInd][nbPose6 + 1] = 0;
+ L[curInd][nbPose6 + 2] = X * kr2ud;
+ L[curInd][nbPose6 + 3] = 0;
+ L[curInd][nbPose6 + 4] = 0;
+ L[curInd][nbPose6 + 5] = px * X * r2ud;
}
- curInd++;
+ curInd++;
{
- L[curInd][q] = Ayx*(-inv_z) ;
- L[curInd][q+1] = Ayy*(-inv_z) ;
- L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
- L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
- L[curInd][q+5] = Ayx*Y -Ayy*X;
+ L[curInd][q] = Ayx * (-inv_z);
+ L[curInd][q + 1] = Ayy * (-inv_z);
+ L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
+ L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
+ L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
+ L[curInd][q + 5] = Ayx * Y - Ayy * X;
}
{
- L[curInd][nbPose6]= 0 ;
- L[curInd][nbPose6+1]= 1;
- L[curInd][nbPose6+2]= 0;
- L[curInd][nbPose6+3]= Y*kr2ud ;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = py*Y*r2ud ;
+ L[curInd][nbPose6] = 0;
+ L[curInd][nbPose6 + 1] = 1;
+ L[curInd][nbPose6 + 2] = 0;
+ L[curInd][nbPose6 + 3] = Y * kr2ud;
+ L[curInd][nbPose6 + 4] = 0;
+ L[curInd][nbPose6 + 5] = py * Y * r2ud;
}
- } // end interaction
+ } // end interaction
curPoint++;
- } // end interaction
+ } // end interaction
}
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
+ vpColVector error;
+ error = P - Pd;
+ // r = r/nbPointTotal ;
- vpMatrix Lp ;
+ vpMatrix Lp;
/*double rank =*/
- L.pseudoInverse(Lp,1e-10) ;
- vpColVector e ;
- e = Lp*error ;
- vpColVector Tc, Tc_v(6*nbPose) ;
- Tc = -e*gain ;
- for (unsigned int i = 0 ; i < 6*nbPose ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
- u0+Tc[nbPose6], v0+Tc[nbPose6+1],
- kud + Tc[nbPose6+5],
- kdu + Tc[nbPose6+4]);
-
- vpColVector Tc_v_Tmp(6) ;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
- * table_cal[p].cMo_dist;
+ L.pseudoInverse(Lp, 1e-10);
+ vpColVector e;
+ e = Lp * error;
+ vpColVector Tc, Tc_v(6 * nbPose);
+ Tc = -e * gain;
+ for (unsigned int i = 0; i < 6 * nbPose; i++)
+ Tc_v[i] = Tc[i];
+
+ cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
+ v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
+
+ vpColVector Tc_v_Tmp(6);
+ for (unsigned int p = 0; p < nbPose; p++) {
+ for (unsigned int i = 0; i < 6; i++)
+ Tc_v_Tmp[i] = Tc_v[6 * p + i];
+
+ table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
}
if (verbose)
- std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
- //std::cout << " residual: " << r << std::endl;
-
+ 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")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
- //double perViewError;
- //double totalError = 0;
- //int totalPoints = 0;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cam_dist = cam_est ;
- //perViewError = table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
- //totalError += perViewError*perViewError * table_cal[p].npt;
- //totalPoints += (int)table_cal[p].npt;
+ // double perViewError;
+ // double totalError = 0;
+ // int totalPoints = 0;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ table_cal[p].cam_dist = cam_est;
+ // perViewError =
+ // table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
+ // totalError += perViewError*perViewError * table_cal[p].npt;
+ // totalPoints += (int)table_cal[p].npt;
}
- globalReprojectionError = sqrt(r/(nbPointTotal));
+ globalReprojectionError = sqrt(r / (nbPointTotal));
// Restore ostream format
std::cout.flags(original_flags);
}
/*!
- \brief calibration method of effector-camera from R. Tsai and R. Lorenz \cite Tsai89a.
+ \brief calibration method of effector-camera from R. Tsai and R. Lorenz
+ \cite Tsai89a.
Compute extrinsic camera parameters : the constant transformation from
the effector to the camera coordinates (eMc).
@@ -1129,38 +1061,37 @@ vpCalibration::calibVVSWithDistortionMulti(std::vector<vpCalibration> &table_cal
\param cMo : vector of homogeneous matrices representing the transformation
between the camera and the scene (input)
\param rMe : vector of homogeneous matrices representing the transformation
- between the effector (where the camera is fixed) and the reference coordinates
- (base of the manipulator) (input). Must be the same size as cMo.
+ between the effector (where the camera is fixed) and the reference
+ coordinates (base of the manipulator) (input). Must be the same size as cMo.
\param eMc : homogeneous matrix representing the transformation
between the effector and the camera (output)
*/
-void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
- std::vector<vpHomogeneousMatrix>& rMe,
- vpHomogeneousMatrix &eMc){
+void vpCalibration::calibrationTsai(const std::vector<vpHomogeneousMatrix> &cMo,
+ const std::vector<vpHomogeneousMatrix> &rMe, vpHomogeneousMatrix &eMc)
+{
- vpColVector x ;
+ vpColVector x;
unsigned int nbPose = (unsigned int)cMo.size();
- if(cMo.size()!=rMe.size()) throw vpCalibrationException(vpCalibrationException::dimensionError,"cMo and rMe have different sizes");
+ if (cMo.size() != rMe.size())
+ throw vpCalibrationException(vpCalibrationException::dimensionError, "cMo and rMe have different sizes");
{
- vpMatrix A ;
- vpColVector B ;
- unsigned int k = 0 ;
+ vpMatrix A;
+ vpColVector B;
+ unsigned int k = 0;
// for all couples ij
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- //std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
+ for (unsigned int i = 0; i < nbPose; i++) {
+ vpRotationMatrix rRei, ciRo;
+ rMe[i].extract(rRei);
+ cMo[i].extract(ciRo);
+ // std::cout << "rMei: " << std::endl << rMe[i] << std::endl;
+
+ for (unsigned int j = 0; j < nbPose; j++) {
+ if (j > i) // we don't use two times same couples...
{
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
- //std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
+ vpRotationMatrix rRej, cjRo;
+ rMe[j].extract(rRej);
+ cMo[j].extract(cjRo);
+ // std::cout << "rMej: " << std::endl << rMe[j] << std::endl;
vpRotationMatrix rReij = rRej.t() * rRei;
@@ -1168,669 +1099,611 @@ void vpCalibration::calibrationTsai(std::vector<vpHomogeneousMatrix>& cMo,
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);
+ for (unsigned int m = 0; m < 3; m++)
+ rPeij[m] = rPeij[m] * vpMath::sinc(theta / 2);
- vpThetaUVector cijPo(cijRo) ;
- theta = sqrt(cijPo[0]*cijPo[0] + cijPo[1]*cijPo[1]
- + cijPo[2]*cijPo[2]);
- for (unsigned int m=0;m<3;m++) cijPo[m] = cijPo[m] * vpMath::sinc(theta/2);
+ vpThetaUVector cijPo(cijRo);
+ theta = sqrt(cijPo[0] * cijPo[0] + cijPo[1] * cijPo[1] + cijPo[2] * cijPo[2]);
+ for (unsigned int m = 0; m < 3; m++)
+ cijPo[m] = cijPo[m] * vpMath::sinc(theta / 2);
vpMatrix As;
- vpColVector b(3) ;
+ vpColVector b(3);
- As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo)) ;
+ As = vpColVector::skew(vpColVector(rPeij) + vpColVector(cijPo));
- b = (vpColVector)cijPo - (vpColVector)rPeij ; // A.40
+ b = (vpColVector)cijPo - (vpColVector)rPeij; // A.40
- if (k==0)
- {
- A = As ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stack(A,As) ;
- B = vpColVector::stack(B,b) ;
+ if (k == 0) {
+ A = As;
+ B = b;
+ } else {
+ A = vpMatrix::stack(A, As);
+ B = vpColVector::stack(B, b);
}
- k++ ;
+ k++;
}
}
}
-
+
// the linear system is defined
// x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
+ vpMatrix AtA = A.AtA();
- vpMatrix Ap ;
- AtA.pseudoInverse(Ap, 1e-6) ; // rank 3
- x = Ap*A.t()*B ;
+ vpMatrix Ap;
+ AtA.pseudoInverse(Ap, 1e-6); // rank 3
+ x = Ap * A.t() * B;
-// {
-// // Residual
-// vpColVector residual;
-// residual = A*x-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
+ // {
+ // // Residual
+ // vpColVector residual;
+ // residual = A*x-B;
+ // std::cout << "Residual: " << std::endl << residual << std::endl;
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("Mean residual = %lf\n",res);
-// }
+ // double res = 0;
+ // for (int i=0; i < residual.getRows(); i++)
+ // res += residual[i]*residual[i];
+ // res = sqrt(res/residual.getRows());
+ // printf("Mean residual = %lf\n",res);
+ // }
// extraction of theta and U
- double theta ;
- double d=x.sumSquare() ;
- for (unsigned int i=0 ; i < 3 ; i++) x[i] = 2*x[i]/sqrt(1+d) ;
- theta = sqrt(x.sumSquare())/2 ;
- theta = 2*asin(theta) ;
- //if (theta !=0)
- if (std::fabs(theta) > std::numeric_limits<double>::epsilon())
- {
- for (unsigned int i=0 ; i < 3 ; i++) x[i] *= theta/(2*sin(theta/2)) ;
- }
- else
- x = 0 ;
+ double theta;
+ double d = x.sumSquare();
+ for (unsigned int i = 0; i < 3; i++)
+ x[i] = 2 * x[i] / sqrt(1 + d);
+ theta = sqrt(x.sumSquare()) / 2;
+ theta = 2 * asin(theta);
+ // if (theta !=0)
+ if (std::fabs(theta) > std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < 3; i++)
+ x[i] *= theta / (2 * sin(theta / 2));
+ } else
+ x = 0;
}
// Building of the rotation matrix eRc
- vpThetaUVector xP(x[0],x[1],x[2]);
+ vpThetaUVector xP(x[0], x[1], x[2]);
vpRotationMatrix eRc(xP);
{
- vpMatrix A ;
- vpColVector B ;
+ vpMatrix A;
+ vpColVector B;
// Building of the system for the translation estimation
// for all couples ij
- vpRotationMatrix I3 ;
- I3.eye() ;
- int k = 0 ;
- for (unsigned int i=0 ; i < nbPose ; i++)
- {
- vpRotationMatrix rRei, ciRo ;
- vpTranslationVector rTei, ciTo ;
- rMe[i].extract(rRei) ;
- cMo[i].extract(ciRo) ;
- rMe[i].extract(rTei) ;
- cMo[i].extract(ciTo) ;
-
-
- for (unsigned int j=0 ; j < nbPose ; j++)
- {
- if (j>i) // we don't use two times same couples...
+ vpRotationMatrix I3;
+ I3.eye();
+ int k = 0;
+ for (unsigned int i = 0; i < nbPose; i++) {
+ vpRotationMatrix rRei, ciRo;
+ vpTranslationVector rTei, ciTo;
+ rMe[i].extract(rRei);
+ cMo[i].extract(ciRo);
+ rMe[i].extract(rTei);
+ cMo[i].extract(ciTo);
+
+ for (unsigned int j = 0; j < nbPose; j++) {
+ if (j > i) // we don't use two times same couples...
{
- vpRotationMatrix rRej, cjRo ;
- rMe[j].extract(rRej) ;
- cMo[j].extract(cjRo) ;
+ vpRotationMatrix rRej, cjRo;
+ rMe[j].extract(rRej);
+ cMo[j].extract(cjRo);
- vpTranslationVector rTej, cjTo ;
- rMe[j].extract(rTej) ;
- cMo[j].extract(cjTo) ;
+ vpTranslationVector rTej, cjTo;
+ rMe[j].extract(rTej);
+ cMo[j].extract(cjTo);
- vpRotationMatrix rReij = rRej.t() * rRei ;
+ vpRotationMatrix rReij = rRej.t() * rRei;
- vpTranslationVector rTeij = rTej+ (-rTei);
+ vpTranslationVector rTeij = rTej + (-rTei);
- rTeij = rRej.t()*rTeij ;
+ rTeij = rRej.t() * rTeij;
vpMatrix a = vpMatrix(rReij) - vpMatrix(I3);
- vpTranslationVector b ;
- b = eRc*cjTo - rReij*eRc*ciTo + rTeij ;
+ vpTranslationVector b;
+ b = eRc * cjTo - rReij * eRc * ciTo + rTeij;
- if (k==0)
- {
- A = a ;
- B = b ;
- }
- else
- {
- A = vpMatrix::stack(A,a) ;
- B = vpColVector::stack(B,b) ;
+ if (k == 0) {
+ A = a;
+ B = b;
+ } else {
+ A = vpMatrix::stack(A, a);
+ B = vpColVector::stack(B, b);
}
- k++ ;
+ k++;
}
}
}
// the linear system is solved
// x = AtA^-1AtB is solved
- vpMatrix AtA = A.AtA() ;
- vpMatrix Ap ;
- vpColVector AeTc ;
- AtA.pseudoInverse(Ap, 1e-6) ;
- AeTc = Ap*A.t()*B ;
-
-// {
-// // residual
-// vpColVector residual;
-// residual = A*AeTc-B;
-// std::cout << "Residual: " << std::endl << residual << std::endl;
-// double res = 0;
-// for (int i=0; i < residual.getRows(); i++)
-// res += residual[i]*residual[i];
-// res = sqrt(res/residual.getRows());
-// printf("mean residual = %lf\n",res);
-// }
-
- vpTranslationVector eTc(AeTc[0],AeTc[1],AeTc[2]);
-
- eMc.insert(eTc) ;
- eMc.insert(eRc) ;
+ vpMatrix AtA = A.AtA();
+ vpMatrix Ap;
+ vpColVector AeTc;
+ AtA.pseudoInverse(Ap, 1e-6);
+ AeTc = Ap * A.t() * B;
+
+ // {
+ // // residual
+ // vpColVector residual;
+ // residual = A*AeTc-B;
+ // std::cout << "Residual: " << std::endl << residual << std::endl;
+ // double res = 0;
+ // for (int i=0; i < residual.getRows(); i++)
+ // res += residual[i]*residual[i];
+ // res = sqrt(res/residual.getRows());
+ // printf("mean residual = %lf\n",res);
+ // }
+
+ vpTranslationVector eTc(AeTc[0], AeTc[1], AeTc[2]);
+
+ eMc.insert(eTc);
+ eMc.insert(eRc);
}
}
-
-void
-vpCalibration::calibVVSMulti(unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam_est,
- bool verbose)
+void vpCalibration::calibVVSMulti(unsigned int nbPose, vpCalibration table_cal[], vpCameraParameters &cam_est,
+ bool verbose)
{
- std::ios::fmtflags original_flags( std::cout.flags() );
+ std::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int nbPoint[256]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPoint[256]; // number of points by image
+ unsigned int nbPointTotal = 0; // total number of points
- unsigned int nbPose6 = 6*nbPose;
+ unsigned int nbPose6 = 6 * nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
+ for (unsigned int i = 0; i < nbPose; i++) {
nbPoint[i] = table_cal[i].npt;
nbPointTotal += nbPoint[i];
}
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
+ if (nbPointTotal < 4) {
+ // vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
}
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
+ vpColVector oX(nbPointTotal), cX(nbPointTotal);
+ vpColVector oY(nbPointTotal), cY(nbPointTotal);
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
+ vpColVector u(nbPointTotal);
+ vpColVector v(nbPointTotal);
- vpColVector P(2*nbPointTotal) ;
- vpColVector Pd(2*nbPointTotal) ;
+ vpColVector P(2 * nbPointTotal);
+ vpColVector Pd(2 * nbPointTotal);
vpImagePoint ip;
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ unsigned int curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
+ u[curPoint] = ip.get_u();
+ v[curPoint] = ip.get_v();
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
curPoint++;
}
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
- iter++ ;
- residu_1 = r ;
+ iter++;
+ residu_1 = r;
double px = cam_est.get_px();
double py = cam_est.get_py();
double u0 = cam_est.get_u0();
double v0 = cam_est.get_v0();
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ r = 0;
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
vpHomogeneousMatrix cMoTmp = table_cal[p].cMo;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
+ 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] ;
+ Pd[curPoint2] = u[curPoint];
+ Pd[curPoint2 + 1] = v[curPoint];
- P[curPoint2] = cX[curPoint]/cZ[curPoint]*px + u0 ;
- P[curPoint2+1] = cY[curPoint]/cZ[curPoint]*py + v0 ;
+ 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++;
}
}
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
+ vpColVector error;
+ error = P - Pd;
+ // r = r/nbPointTotal ;
- vpMatrix L(nbPointTotal*2,nbPose6+4) ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint2 = 2*curPoint;
+ vpMatrix L(nbPointTotal * 2, nbPose6 + 4);
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
+ unsigned int q = 6 * p;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ unsigned int curPoint2 = 2 * curPoint;
unsigned int curPoint21 = curPoint2 + 1;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
+ double x = cX[curPoint];
+ double y = cY[curPoint];
+ double z = cZ[curPoint];
- double inv_z = 1/z;
+ double inv_z = 1 / z;
- double X = x*inv_z ;
- double Y = y*inv_z ;
+ double X = x * inv_z;
+ double Y = y * inv_z;
//---------------
{
{
- L[curPoint2][q] = px * (-inv_z) ;
- L[curPoint2][q+1] = 0 ;
- L[curPoint2][q+2] = px*(X*inv_z) ;
- L[curPoint2][q+3] = px*X*Y ;
- L[curPoint2][q+4] = -px*(1+X*X) ;
- L[curPoint2][q+5] = px*Y ;
+ L[curPoint2][q] = px * (-inv_z);
+ L[curPoint2][q + 1] = 0;
+ L[curPoint2][q + 2] = px * (X * inv_z);
+ L[curPoint2][q + 3] = px * X * Y;
+ L[curPoint2][q + 4] = -px * (1 + X * X);
+ L[curPoint2][q + 5] = px * Y;
}
{
- L[curPoint2][nbPose6]= 1 ;
- L[curPoint2][nbPose6+1]= 0 ;
- L[curPoint2][nbPose6+2]= X ;
- L[curPoint2][nbPose6+3]= 0;
+ L[curPoint2][nbPose6] = 1;
+ L[curPoint2][nbPose6 + 1] = 0;
+ L[curPoint2][nbPose6 + 2] = X;
+ L[curPoint2][nbPose6 + 3] = 0;
}
{
- L[curPoint21][q] = 0 ;
- L[curPoint21][q+1] = py*(-inv_z) ;
- L[curPoint21][q+2] = py*(Y*inv_z) ;
- L[curPoint21][q+3] = py* (1+Y*Y) ;
- L[curPoint21][q+4] = -py*X*Y ;
- L[curPoint21][q+5] = -py*X ;
+ L[curPoint21][q] = 0;
+ L[curPoint21][q + 1] = py * (-inv_z);
+ L[curPoint21][q + 2] = py * (Y * inv_z);
+ L[curPoint21][q + 3] = py * (1 + Y * Y);
+ L[curPoint21][q + 4] = -py * X * Y;
+ L[curPoint21][q + 5] = -py * X;
}
{
- L[curPoint21][nbPose6]= 0 ;
- L[curPoint21][nbPose6+1]= 1 ;
- L[curPoint21][nbPose6+2]= 0;
- L[curPoint21][nbPose6+3]= Y ;
+ L[curPoint21][nbPose6] = 0;
+ L[curPoint21][nbPose6 + 1] = 1;
+ L[curPoint21][nbPose6 + 2] = 0;
+ L[curPoint21][nbPose6 + 3] = Y;
}
-
}
curPoint++;
- } // end interaction
+ } // end interaction
}
- vpMatrix Lp ;
- Lp = L.pseudoInverse(1e-10) ;
+ vpMatrix Lp;
+ Lp = L.pseudoInverse(1e-10);
- vpColVector e ;
- e = Lp*error ;
+ vpColVector e;
+ e = Lp * error;
- vpColVector Tc, Tc_v(nbPose6) ;
- Tc = -e*gain ;
+ vpColVector Tc, Tc_v(nbPose6);
+ Tc = -e * gain;
// Tc_v =0 ;
- for (unsigned int i = 0 ; i < nbPose6 ; i++)
- Tc_v[i] = Tc[i] ;
+ 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) ;
+ 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];
+ for (unsigned int p = 0; p < nbPose; p++) {
+ for (unsigned int i = 0; i < 6; i++)
+ Tc_v_Tmp[i] = Tc_v[6 * p + i];
- table_cal[p].cMo = vpExponentialMap::direct(Tc_v_Tmp,1).inverse()
- * table_cal[p].cMo;
+ 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;
-
+ 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")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cMo_dist = table_cal[p].cMo ;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ table_cal[p].cMo_dist = table_cal[p].cMo;
table_cal[p].cam = cam_est;
table_cal[p].cam_dist = cam_est;
- double deviation,deviation_dist;
- table_cal[p].computeStdDeviation(deviation,deviation_dist);
+ double deviation, deviation_dist;
+ table_cal[p].computeStdDeviation(deviation, deviation_dist);
}
if (verbose)
- std::cout << " Global std dev " << sqrt(r/nbPointTotal) << std::endl;
+ std::cout << " Global std dev " << sqrt(r / nbPointTotal) << std::endl;
// Restore ostream format
std::cout.flags(original_flags);
}
-
-void
-vpCalibration::calibVVSWithDistortionMulti(
- unsigned int nbPose,
- vpCalibration table_cal[],
- vpCameraParameters &cam_est,
- bool verbose)
+void vpCalibration::calibVVSWithDistortionMulti(unsigned int nbPose, vpCalibration table_cal[],
+ vpCameraParameters &cam_est, bool verbose)
{
- std::ios::fmtflags original_flags( std::cout.flags() );
+ std::ios::fmtflags original_flags(std::cout.flags());
std::cout.precision(10);
- unsigned int nbPoint[1024]; //number of points by image
- unsigned int nbPointTotal = 0; //total number of points
+ unsigned int nbPoint[1024]; // number of points by image
+ unsigned int nbPointTotal = 0; // total number of points
- unsigned int nbPose6 = 6*nbPose;
- for (unsigned int i=0; i<nbPose ; i++)
- {
+ unsigned int nbPose6 = 6 * nbPose;
+ for (unsigned int i = 0; i < nbPose; i++) {
nbPoint[i] = table_cal[i].npt;
nbPointTotal += nbPoint[i];
}
- if (nbPointTotal < 4)
- {
- //vpERROR_TRACE("Not enough point to calibrate");
- throw(vpCalibrationException(vpCalibrationException::notInitializedError,
- "Not enough point to calibrate")) ;
+ if (nbPointTotal < 4) {
+ // vpERROR_TRACE("Not enough point to calibrate");
+ throw(vpCalibrationException(vpCalibrationException::notInitializedError, "Not enough point to calibrate"));
}
- vpColVector oX(nbPointTotal), cX(nbPointTotal) ;
- vpColVector oY(nbPointTotal), cY(nbPointTotal) ;
- vpColVector oZ(nbPointTotal), cZ(nbPointTotal) ;
- vpColVector u(nbPointTotal) ;
- vpColVector v(nbPointTotal) ;
+ vpColVector oX(nbPointTotal), cX(nbPointTotal);
+ vpColVector oY(nbPointTotal), cY(nbPointTotal);
+ vpColVector oZ(nbPointTotal), cZ(nbPointTotal);
+ vpColVector u(nbPointTotal);
+ vpColVector v(nbPointTotal);
- vpColVector P(4*nbPointTotal) ;
- vpColVector Pd(4*nbPointTotal) ;
+ vpColVector P(4 * nbPointTotal);
+ vpColVector Pd(4 * nbPointTotal);
vpImagePoint ip;
- unsigned int curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ unsigned int curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
std::list<double>::const_iterator it_LoX = table_cal[p].LoX.begin();
std::list<double>::const_iterator it_LoY = table_cal[p].LoY.begin();
std::list<double>::const_iterator it_LoZ = table_cal[p].LoZ.begin();
std::list<vpImagePoint>::const_iterator it_Lip = table_cal[p].Lip.begin();
- for (unsigned int i =0 ; i < nbPoint[p] ; i++)
- {
- oX[curPoint] = *it_LoX;
- oY[curPoint] = *it_LoY;
- oZ[curPoint] = *it_LoZ;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ oX[curPoint] = *it_LoX;
+ oY[curPoint] = *it_LoY;
+ oZ[curPoint] = *it_LoZ;
ip = *it_Lip;
- u[curPoint] = ip.get_u() ;
- v[curPoint] = ip.get_v() ;
+ u[curPoint] = ip.get_u();
+ v[curPoint] = ip.get_v();
- ++ it_LoX;
- ++ it_LoY;
- ++ it_LoZ;
- ++ it_Lip;
+ ++it_LoX;
+ ++it_LoY;
+ ++it_LoZ;
+ ++it_Lip;
curPoint++;
}
}
// double lambda = 0.1 ;
- unsigned int iter = 0 ;
+ unsigned int iter = 0;
- double residu_1 = 1e12 ;
- double r =1e12-1;
- while (vpMath::equal(residu_1,r,threshold) == false && iter < nbIterMax)
- {
- iter++ ;
- residu_1 = r ;
+ double residu_1 = 1e12;
+ double r = 1e12 - 1;
+ while (vpMath::equal(residu_1, r, threshold) == false && iter < nbIterMax) {
+ iter++;
+ residu_1 = r;
- r = 0 ;
- curPoint = 0 ; //current point indice
- for (unsigned int p=0; p<nbPose ; p++)
- {
+ r = 0;
+ curPoint = 0; // current point indice
+ for (unsigned int p = 0; p < nbPose; p++) {
vpHomogeneousMatrix cMoTmp = table_cal[p].cMo_dist;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- cX[curPoint] = oX[curPoint]*cMoTmp[0][0]+oY[curPoint]*cMoTmp[0][1]
- +oZ[curPoint]*cMoTmp[0][2] + cMoTmp[0][3];
- cY[curPoint] = oX[curPoint]*cMoTmp[1][0]+oY[curPoint]*cMoTmp[1][1]
- +oZ[curPoint]*cMoTmp[1][2] + cMoTmp[1][3];
- cZ[curPoint] = oX[curPoint]*cMoTmp[2][0]+oY[curPoint]*cMoTmp[2][1]
- +oZ[curPoint]*cMoTmp[2][2] + cMoTmp[2][3];
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ cX[curPoint] =
+ oX[curPoint] * cMoTmp[0][0] + oY[curPoint] * cMoTmp[0][1] + oZ[curPoint] * cMoTmp[0][2] + cMoTmp[0][3];
+ cY[curPoint] =
+ oX[curPoint] * cMoTmp[1][0] + oY[curPoint] * cMoTmp[1][1] + oZ[curPoint] * cMoTmp[1][2] + cMoTmp[1][3];
+ cZ[curPoint] =
+ oX[curPoint] * cMoTmp[2][0] + oY[curPoint] * cMoTmp[2][1] + oZ[curPoint] * cMoTmp[2][2] + cMoTmp[2][3];
curPoint++;
}
}
+ vpMatrix L(nbPointTotal * 4, nbPose6 + 6);
+ curPoint = 0; // current point indice
+ double px = cam_est.get_px();
+ double py = cam_est.get_py();
+ double u0 = cam_est.get_u0();
+ double v0 = cam_est.get_v0();
- vpMatrix L(nbPointTotal*4,nbPose6+6) ;
- curPoint = 0 ; //current point indice
- double px = cam_est.get_px() ;
- double py = cam_est.get_py() ;
- double u0 = cam_est.get_u0() ;
- double v0 = cam_est.get_v0() ;
-
- double inv_px = 1/px ;
- double inv_py = 1/py ;
+ double inv_px = 1 / px;
+ double inv_py = 1 / py;
- double kud = cam_est.get_kud() ;
- double kdu = cam_est.get_kdu() ;
+ double kud = cam_est.get_kud();
+ double kdu = cam_est.get_kdu();
- double k2ud = 2*kud;
- double k2du = 2*kdu;
+ double k2ud = 2 * kud;
+ double k2du = 2 * kdu;
- for (unsigned int p=0; p<nbPose ; p++)
- {
- unsigned int q = 6*p;
- for (unsigned int i=0 ; i < nbPoint[p]; i++)
- {
- unsigned int curPoint4 = 4*curPoint;
- double x = cX[curPoint] ;
- double y = cY[curPoint] ;
- double z = cZ[curPoint] ;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ unsigned int q = 6 * p;
+ for (unsigned int i = 0; i < nbPoint[p]; i++) {
+ unsigned int curPoint4 = 4 * curPoint;
+ double x = cX[curPoint];
+ double y = cY[curPoint];
+ double z = cZ[curPoint];
- double inv_z = 1/z;
- double X = x*inv_z ;
- double Y = y*inv_z ;
+ double inv_z = 1 / z;
+ double X = x * inv_z;
+ double Y = y * inv_z;
- double X2 = X*X;
- double Y2 = Y*Y;
- double XY = X*Y;
+ double X2 = X * X;
+ double Y2 = Y * Y;
+ double XY = X * Y;
- double up = u[curPoint] ;
- double vp = v[curPoint] ;
+ double up = u[curPoint];
+ double vp = v[curPoint];
- Pd[curPoint4] = up ;
- Pd[curPoint4+1] = vp ;
+ Pd[curPoint4] = up;
+ Pd[curPoint4 + 1] = vp;
double up0 = up - u0;
double vp0 = vp - v0;
double xp0 = up0 * inv_px;
- double xp02 = xp0 *xp0 ;
+ double xp02 = xp0 * xp0;
double yp0 = vp0 * inv_py;
double yp02 = yp0 * yp0;
- double r2du = xp02 + yp02 ;
+ double r2du = xp02 + yp02;
double kr2du = kdu * r2du;
- P[curPoint4] = u0 + px*X - kr2du *(up0) ;
- P[curPoint4+1] = v0 + py*Y - kr2du *(vp0) ;
+ P[curPoint4] = u0 + px * X - kr2du * (up0);
+ P[curPoint4 + 1] = v0 + py * Y - kr2du * (vp0);
- double r2ud = X2 + Y2 ;
+ double r2ud = X2 + Y2;
double kr2ud = 1 + kud * r2ud;
- double Axx = px*(kr2ud+k2ud*X2);
- double Axy = px*k2ud*XY;
- double Ayy = py*(kr2ud+k2ud*Y2);
- double Ayx = py*k2ud*XY;
+ double Axx = px * (kr2ud + k2ud * X2);
+ double Axy = px * k2ud * XY;
+ double Ayy = py * (kr2ud + k2ud * Y2);
+ double Ayx = py * k2ud * XY;
- Pd[curPoint4+2] = up ;
- Pd[curPoint4+3] = vp ;
+ Pd[curPoint4 + 2] = up;
+ Pd[curPoint4 + 3] = vp;
- P[curPoint4+2] = u0 + px*X*kr2ud ;
- P[curPoint4+3] = v0 + py*Y*kr2ud ;
+ P[curPoint4 + 2] = u0 + px * X * kr2ud;
+ P[curPoint4 + 3] = v0 + py * Y * kr2ud;
- r += (vpMath::sqr(P[curPoint4]-Pd[curPoint4]) +
- vpMath::sqr(P[curPoint4+1]-Pd[curPoint4+1]) +
- vpMath::sqr(P[curPoint4+2]-Pd[curPoint4+2]) +
- vpMath::sqr(P[curPoint4+3]-Pd[curPoint4+3]))*0.5 ;
+ r += (vpMath::sqr(P[curPoint4] - Pd[curPoint4]) + vpMath::sqr(P[curPoint4 + 1] - Pd[curPoint4 + 1]) +
+ vpMath::sqr(P[curPoint4 + 2] - Pd[curPoint4 + 2]) + vpMath::sqr(P[curPoint4 + 3] - Pd[curPoint4 + 3])) *
+ 0.5;
unsigned int curInd = curPoint4;
//---------------
{
{
- L[curInd][q] = px * (-inv_z) ;
- L[curInd][q+1] = 0 ;
- L[curInd][q+2] = px*X*inv_z ;
- L[curInd][q+3] = px*X*Y ;
- L[curInd][q+4] = -px*(1+X2) ;
- L[curInd][q+5] = px*Y ;
+ L[curInd][q] = px * (-inv_z);
+ L[curInd][q + 1] = 0;
+ L[curInd][q + 2] = px * X * inv_z;
+ L[curInd][q + 3] = px * X * Y;
+ L[curInd][q + 4] = -px * (1 + X2);
+ L[curInd][q + 5] = px * Y;
}
{
- L[curInd][nbPose6]= 1 + kr2du + k2du*xp02 ;
- L[curInd][nbPose6+1]= k2du*up0*yp0*inv_py ;
- L[curInd][nbPose6+2]= X + k2du*xp02*xp0 ;
- L[curInd][nbPose6+3]= k2du*up0*yp02*inv_py ;
- L[curInd][nbPose6+4] = -(up0)*(r2du) ;
- L[curInd][nbPose6+5] = 0 ;
+ L[curInd][nbPose6] = 1 + kr2du + k2du * xp02;
+ L[curInd][nbPose6 + 1] = k2du * up0 * yp0 * inv_py;
+ L[curInd][nbPose6 + 2] = X + k2du * xp02 * xp0;
+ L[curInd][nbPose6 + 3] = k2du * up0 * yp02 * inv_py;
+ L[curInd][nbPose6 + 4] = -(up0) * (r2du);
+ L[curInd][nbPose6 + 5] = 0;
}
- curInd++;
+ curInd++;
{
- L[curInd][q] = 0 ;
- L[curInd][q+1] = py*(-inv_z) ;
- L[curInd][q+2] = py*Y*inv_z ;
- L[curInd][q+3] = py* (1+Y2) ;
- L[curInd][q+4] = -py*XY ;
- L[curInd][q+5] = -py*X ;
+ L[curInd][q] = 0;
+ L[curInd][q + 1] = py * (-inv_z);
+ L[curInd][q + 2] = py * Y * inv_z;
+ L[curInd][q + 3] = py * (1 + Y2);
+ L[curInd][q + 4] = -py * XY;
+ L[curInd][q + 5] = -py * X;
}
{
- L[curInd][nbPose6]= k2du*xp0*vp0*inv_px ;
- L[curInd][nbPose6+1]= 1 + kr2du + k2du*yp02;
- L[curInd][nbPose6+2]= k2du*vp0*xp02*inv_px;
- L[curInd][nbPose6+3]= Y + k2du*yp02*yp0;
- L[curInd][nbPose6+4] = -vp0*r2du ;
- L[curInd][nbPose6+5] = 0 ;
+ L[curInd][nbPose6] = k2du * xp0 * vp0 * inv_px;
+ L[curInd][nbPose6 + 1] = 1 + kr2du + k2du * yp02;
+ L[curInd][nbPose6 + 2] = k2du * vp0 * xp02 * inv_px;
+ L[curInd][nbPose6 + 3] = Y + k2du * yp02 * yp0;
+ L[curInd][nbPose6 + 4] = -vp0 * r2du;
+ L[curInd][nbPose6 + 5] = 0;
}
- curInd++;
- //---undistorted to distorted
+ curInd++;
+ //---undistorted to distorted
{
- L[curInd][q] = Axx*(-inv_z) ;
- L[curInd][q+1] = Axy*(-inv_z) ;
- L[curInd][q+2] = Axx*(X*inv_z) + Axy*(Y*inv_z) ;
- L[curInd][q+3] = Axx*X*Y + Axy*(1+Y2);
- L[curInd][q+4] = -Axx*(1+X2) - Axy*XY;
- L[curInd][q+5] = Axx*Y -Axy*X;
+ L[curInd][q] = Axx * (-inv_z);
+ L[curInd][q + 1] = Axy * (-inv_z);
+ L[curInd][q + 2] = Axx * (X * inv_z) + Axy * (Y * inv_z);
+ L[curInd][q + 3] = Axx * X * Y + Axy * (1 + Y2);
+ L[curInd][q + 4] = -Axx * (1 + X2) - Axy * XY;
+ L[curInd][q + 5] = Axx * Y - Axy * X;
}
{
- L[curInd][nbPose6]= 1 ;
- L[curInd][nbPose6+1]= 0 ;
- L[curInd][nbPose6+2]= X*kr2ud ;
- L[curInd][nbPose6+3]= 0;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = px*X*r2ud ;
+ L[curInd][nbPose6] = 1;
+ L[curInd][nbPose6 + 1] = 0;
+ L[curInd][nbPose6 + 2] = X * kr2ud;
+ L[curInd][nbPose6 + 3] = 0;
+ L[curInd][nbPose6 + 4] = 0;
+ L[curInd][nbPose6 + 5] = px * X * r2ud;
}
- curInd++;
+ curInd++;
{
- L[curInd][q] = Ayx*(-inv_z) ;
- L[curInd][q+1] = Ayy*(-inv_z) ;
- L[curInd][q+2] = Ayx*(X*inv_z) + Ayy*(Y*inv_z) ;
- L[curInd][q+3] = Ayx*XY + Ayy*(1+Y2) ;
- L[curInd][q+4] = -Ayx*(1+X2) -Ayy*XY ;
- L[curInd][q+5] = Ayx*Y -Ayy*X;
+ L[curInd][q] = Ayx * (-inv_z);
+ L[curInd][q + 1] = Ayy * (-inv_z);
+ L[curInd][q + 2] = Ayx * (X * inv_z) + Ayy * (Y * inv_z);
+ L[curInd][q + 3] = Ayx * XY + Ayy * (1 + Y2);
+ L[curInd][q + 4] = -Ayx * (1 + X2) - Ayy * XY;
+ L[curInd][q + 5] = Ayx * Y - Ayy * X;
}
{
- L[curInd][nbPose6]= 0 ;
- L[curInd][nbPose6+1]= 1;
- L[curInd][nbPose6+2]= 0;
- L[curInd][nbPose6+3]= Y*kr2ud ;
- L[curInd][nbPose6+4] = 0 ;
- L[curInd][nbPose6+5] = py*Y*r2ud ;
+ L[curInd][nbPose6] = 0;
+ L[curInd][nbPose6 + 1] = 1;
+ L[curInd][nbPose6 + 2] = 0;
+ L[curInd][nbPose6 + 3] = Y * kr2ud;
+ L[curInd][nbPose6 + 4] = 0;
+ L[curInd][nbPose6 + 5] = py * Y * r2ud;
}
- } // end interaction
+ } // end interaction
curPoint++;
- } // end interaction
+ } // end interaction
}
- vpColVector error ;
- error = P-Pd ;
- //r = r/nbPointTotal ;
+ vpColVector error;
+ error = P - Pd;
+ // r = r/nbPointTotal ;
- vpMatrix Lp ;
+ vpMatrix Lp;
/*double rank =*/
- L.pseudoInverse(Lp,1e-10) ;
- vpColVector e ;
- e = Lp*error ;
- vpColVector Tc, Tc_v(6*nbPose) ;
- Tc = -e*gain ;
- for (unsigned int i = 0 ; i < 6*nbPose ; i++)
- Tc_v[i] = Tc[i] ;
-
- cam_est.initPersProjWithDistortion( px+Tc[nbPose6+2], py+Tc[nbPose6+3],
- u0+Tc[nbPose6], v0+Tc[nbPose6+1],
- kud + Tc[nbPose6+5],
- kdu + Tc[nbPose6+4]);
-
- vpColVector Tc_v_Tmp(6) ;
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- for (unsigned int i = 0 ; i < 6 ; i++)
- Tc_v_Tmp[i] = Tc_v[6*p + i];
-
- table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse()
- * table_cal[p].cMo_dist;
+ L.pseudoInverse(Lp, 1e-10);
+ vpColVector e;
+ e = Lp * error;
+ vpColVector Tc, Tc_v(6 * nbPose);
+ Tc = -e * gain;
+ for (unsigned int i = 0; i < 6 * nbPose; i++)
+ Tc_v[i] = Tc[i];
+
+ cam_est.initPersProjWithDistortion(px + Tc[nbPose6 + 2], py + Tc[nbPose6 + 3], u0 + Tc[nbPose6],
+ v0 + Tc[nbPose6 + 1], kud + Tc[nbPose6 + 5], kdu + Tc[nbPose6 + 4]);
+
+ vpColVector Tc_v_Tmp(6);
+ for (unsigned int p = 0; p < nbPose; p++) {
+ for (unsigned int i = 0; i < 6; i++)
+ Tc_v_Tmp[i] = Tc_v[6 * p + i];
+
+ table_cal[p].cMo_dist = vpExponentialMap::direct(Tc_v_Tmp).inverse() * table_cal[p].cMo_dist;
}
if (verbose)
- std::cout << " std dev: " << sqrt(r/nbPointTotal) << std::endl;
- //std::cout << " residual: " << r << std::endl;
-
+ 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")) ;
+ if (iter == nbIterMax) {
+ vpERROR_TRACE("Iterations number exceed the maximum allowed (%d)", nbIterMax);
+ throw(vpCalibrationException(vpCalibrationException::convergencyError, "Maximum number of iterations reached"));
}
- for (unsigned int p = 0 ; p < nbPose ; p++)
- {
- table_cal[p].cam_dist = cam_est ;
+ for (unsigned int p = 0; p < nbPose; p++) {
+ table_cal[p].cam_dist = cam_est;
table_cal[p].computeStdDeviation_dist(table_cal[p].cMo_dist, cam_est);
}
if (verbose)
- std::cout <<" Global std dev " << sqrt(r/(nbPointTotal)) << std::endl;
+ std::cout << " Global std dev " << sqrt(r / (nbPointTotal)) << std::endl;
// Restore ostream format
std::cout.flags(original_flags);
diff --git a/modules/vision/src/homography-estimation/vpHomography.cpp b/modules/vision/src/homography-estimation/vpHomography.cpp
index ebb6ff5..fc5a399 100644
--- a/modules/vision/src/homography-estimation/vpHomography.cpp
+++ b/modules/vision/src/homography-estimation/vpHomography.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
/*!
\file vpHomography.cpp
\brief Definition de la classe vpHomography. Class that consider
@@ -46,9 +46,9 @@
#include <stdio.h>
#include <visp3/core/vpDebug.h>
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRobust.h>
+#include <visp3/vision/vpHomography.h>
// Exception
#include <visp3/core/vpException.h>
@@ -57,157 +57,112 @@
/*!
\brief initialize an homography as Identity
*/
-vpHomography::vpHomography()
- : vpArray2D<double>(3,3), aMb(), bP()
-{
- eye();
-}
-
+vpHomography::vpHomography() : vpArray2D<double>(3, 3), aMb(), bP() { eye(); }
/*!
\brief initialize an homography from another homography
*/
-vpHomography::vpHomography(const vpHomography &H)
- : vpArray2D<double>(3,3), aMb(), bP()
-{
- *this = H;
-}
+vpHomography::vpHomography(const vpHomography &H) : vpArray2D<double>(3, 3), aMb(), bP() { *this = H; }
/*!
\brief initialize an homography from another homography
*/
-vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p)
- : vpArray2D<double>(3,3), aMb(), bP()
+vpHomography::vpHomography(const vpHomogeneousMatrix &M, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
{
- buildFrom(M, p) ;
+ buildFrom(M, p);
}
-vpHomography::vpHomography(const vpThetaUVector &tu,
- const vpTranslationVector &atb,
- const vpPlane &p)
- : vpArray2D<double>(3,3), aMb(), bP()
+vpHomography::vpHomography(const vpThetaUVector &tu, const vpTranslationVector &atb, const vpPlane &p)
+ : vpArray2D<double>(3, 3), aMb(), bP()
{
- buildFrom(tu, atb, p) ;
+ buildFrom(tu, atb, p);
}
-vpHomography::vpHomography(const vpRotationMatrix &aRb,
- const vpTranslationVector &atb,
- const vpPlane &p)
- : vpArray2D<double>(3,3), aMb(), bP()
+vpHomography::vpHomography(const vpRotationMatrix &aRb, const vpTranslationVector &atb, const vpPlane &p)
+ : vpArray2D<double>(3, 3), aMb(), bP()
{
- buildFrom(aRb, atb, p) ;
+ buildFrom(aRb, atb, p);
}
-vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p)
- : vpArray2D<double>(3,3), aMb(), bP()
+vpHomography::vpHomography(const vpPoseVector &arb, const vpPlane &p) : vpArray2D<double>(3, 3), aMb(), bP()
{
- buildFrom(arb, p) ;
+ buildFrom(arb, p);
}
-void
-vpHomography::buildFrom(const vpHomogeneousMatrix &M,
- const vpPlane &p)
+void vpHomography::buildFrom(const vpHomogeneousMatrix &M, const vpPlane &p)
{
- insert(M) ;
- insert(p) ;
- build() ;
+ insert(M);
+ insert(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) ;
- insert(p) ;
- build() ;
+ insert(tu);
+ insert(atb);
+ insert(p);
+ 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) ;
- insert(p) ;
- build() ;
+ insert(aRb);
+ insert(atb);
+ insert(p);
+ build();
}
-void
-vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
+void vpHomography::buildFrom(const vpPoseVector &arb, const vpPlane &p)
{
- aMb.buildFrom(arb[0],arb[1],arb[2],arb[3],arb[4],arb[5]) ;
- insert(p) ;
- build() ;
+ aMb.buildFrom(arb[0], arb[1], arb[2], arb[3], arb[4], arb[5]);
+ insert(p);
+ build();
}
-
-
/*********************************************************************/
-
/*!
\brief insert the rotational component.
To recompute the homography call build().
*/
-void
-vpHomography::insert(const vpRotationMatrix &aRb)
-{
- aMb.insert(aRb) ;
-}
+void vpHomography::insert(const vpRotationMatrix &aRb) { aMb.insert(aRb); }
/*!
\brief insert the rotational component.
To recompute the homography call build().
*/
-void
-vpHomography::insert(const vpHomogeneousMatrix &M)
-{
- this->aMb = M ;
-}
+void vpHomography::insert(const vpHomogeneousMatrix &M) { this->aMb = M; }
/*! \brief insert the rotational component, insert a
theta u vector (transformation into a rotation matrix).
To recompute the homography call build().
*/
-void
-vpHomography::insert(const vpThetaUVector &tu)
+void vpHomography::insert(const vpThetaUVector &tu)
{
- vpRotationMatrix aRb(tu) ;
- aMb.insert(aRb) ;
+ vpRotationMatrix aRb(tu);
+ aMb.insert(aRb);
}
/*!
\brief insert the translational component in a homography.
To recompute the homography call build().
*/
-void
-vpHomography::insert(const vpTranslationVector &atb)
-{
- aMb.insert(atb) ;
-}
+void vpHomography::insert(const vpTranslationVector &atb) { aMb.insert(atb); }
/*!
\brief insert the reference plane.
To recompute the homography call build().
*/
-void
-vpHomography::insert(const vpPlane &p)
-{
- this->bP = p;
-}
+void vpHomography::insert(const vpPlane &p) { this->bP = p; }
/*!
\brief Invert the homography
\return \f$\bf H^{-1}\f$
*/
-vpHomography
-vpHomography::inverse() const
+vpHomography vpHomography::inverse() const
{
vpMatrix M = (*this).convert();
vpMatrix Minv;
@@ -215,8 +170,8 @@ vpHomography::inverse() const
vpHomography H;
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
H[i][j] = Minv[i][j];
return H;
@@ -227,51 +182,44 @@ vpHomography::inverse() const
\param bHa : \f$\bf H^{-1}\f$ with H = *this.
*/
-void
-vpHomography::inverse(vpHomography &bHa) const
-{
- bHa = inverse() ;
-}
+void vpHomography::inverse(vpHomography &bHa) const { bHa = inverse(); }
/*!
Save an homography in a file.
- The laod() function allows then to read and set the homography from this file.
+ The laod() function allows then to read and set the homography from this
+ file.
\sa load()
*/
-void
-vpHomography::save(std::ofstream &f) const
+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")) ;
+ if (!f.fail()) {
+ f << *this;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot write the homography to the output stream"));
}
}
/*!
-
+
Multiplication by an homography.
\param H : Homography to multiply with.
- \code
- vpHomography aHb, bHc;
+ \code
+ vpHomography aHb, bHc;
// Initialize aHb and bHc homographies
- vpHomography aHc = aHb * bHc;
+ vpHomography aHc = aHb * bHc;
\endcode
*/
vpHomography vpHomography::operator*(const vpHomography &H) const
{
vpHomography Hp;
- for(unsigned int i = 0; i < 3; i++) {
- for(unsigned int j = 0; j < 3; j++) {
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++) {
double s = 0.;
- for(unsigned int k = 0; k < 3; k ++) {
+ for (unsigned int k = 0; k < 3; k++) {
s += (*this)[i][k] * H[k][j];
}
Hp[i][j] = s;
@@ -285,16 +233,16 @@ vpHomography vpHomography::operator*(const vpHomography &H) const
\param b : 3 dimension vector.
*/
-vpColVector
-vpHomography::operator*(const vpColVector &b) 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++) {
+ for (unsigned int i = 0; i < 3; i++) {
a[i] = 0.;
- for(unsigned int j=0; j<3; j++)
+ for (unsigned int j = 0; j < 3; j++)
a[i] += (*this)[i][j] * b[j];
}
@@ -302,24 +250,24 @@ vpHomography::operator*(const vpColVector &b) const
}
/*!
-
+
Multiply an homography by a scalar.
\param v : Value of the scalar.
- \code
+ \code
double v = 1.1;
- vpHomography aHb;
+ vpHomography aHb;
// Initialize aHb
- vpHomography H = aHb * v;
+ vpHomography H = aHb * v;
\endcode
*/
vpHomography vpHomography::operator*(const double &v) const
{
vpHomography H;
-
- for (unsigned int i=0; i < 9; i ++) {
+
+ for (unsigned int i = 0; i < 9; i++) {
H.data[i] = this->data[i] * v;
}
@@ -327,43 +275,44 @@ vpHomography vpHomography::operator*(const double &v) const
}
/*!
- From the coordinates of the point in image plane b and the homography between image
- a and b computes the coordinates of the point in image plane a.
+ From the coordinates of the point in image plane b and the homography
+ between image a and b computes the coordinates of the point in image plane
+ a.
\param b_P : 2D coordinates of the point in the image plane b.
\return A point with 2D coordinates in the image plane a.
*/
-vpPoint vpHomography::operator*(const vpPoint& b_P) const
+vpPoint vpHomography::operator*(const vpPoint &b_P) const
{
- vpPoint a_P ;
- vpColVector v(3),v1(3) ;
+ vpPoint a_P;
+ vpColVector v(3), v1(3);
- v[0] = b_P.get_x() ;
- v[1] = b_P.get_y() ;
- v[2] = b_P.get_w() ;
+ v[0] = b_P.get_x();
+ v[1] = b_P.get_y();
+ v[2] = b_P.get_w();
- v1[0] = (*this)[0][0]*v[0] + (*this)[0][1]*v[1]+ (*this)[0][2]*v[2] ;
- v1[1] = (*this)[1][0]*v[0] + (*this)[1][1]*v[1]+ (*this)[1][2]*v[2] ;
- v1[2] = (*this)[2][0]*v[0] + (*this)[2][1]*v[1]+ (*this)[2][2]*v[2] ;
+ v1[0] = (*this)[0][0] * v[0] + (*this)[0][1] * v[1] + (*this)[0][2] * v[2];
+ v1[1] = (*this)[1][0] * v[0] + (*this)[1][1] * v[1] + (*this)[1][2] * v[2];
+ v1[2] = (*this)[2][0] * v[0] + (*this)[2][1] * v[1] + (*this)[2][2] * v[2];
// v1 = M*v ;
- a_P.set_x(v1[0]) ;
- a_P.set_y(v1[1]) ;
- a_P.set_w(v1[2]) ;
+ a_P.set_x(v1[0]);
+ a_P.set_y(v1[1]);
+ a_P.set_w(v1[2]);
- return a_P ;
+ return a_P;
}
/*!
-
+
Divide an homography by a scalar.
\param v : Value of the scalar.
- \code
- vpHomography aHb;
+ \code
+ vpHomography aHb;
// Initialize aHb
- vpHomography H = aHb / aHb[2][2];
+ vpHomography H = aHb / aHb[2][2];
\endcode
*/
@@ -373,9 +322,9 @@ vpHomography vpHomography::operator/(const double &v) const
if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
- double vinv = 1/v ;
+ double vinv = 1 / v;
- for (unsigned int i=0; i < 9; i ++) {
+ for (unsigned int i = 0; i < 9; i++) {
H.data[i] = this->data[i] * vinv;
}
@@ -383,15 +332,15 @@ vpHomography vpHomography::operator/(const double &v) const
}
//! Divide all the element of the homography matrix by v : Hij = Hij / v
-vpHomography & vpHomography::operator/=(double v)
+vpHomography &vpHomography::operator/=(double v)
{
- //if (x == 0)
+ // if (x == 0)
if (std::fabs(v) <= std::numeric_limits<double>::epsilon())
throw vpMatrixException(vpMatrixException::divideByZeroError, "Divide by zero in method /=(double v)");
- double vinv = 1/v ;
+ double vinv = 1 / v;
- for (unsigned int i=0;i<9;i++)
+ for (unsigned int i = 0; i < 9; i++)
data[i] *= vinv;
return *this;
@@ -403,13 +352,14 @@ vpHomography & vpHomography::operator/=(double v)
\param H : Homography matrix to be copied.
*/
-vpHomography &
-vpHomography::operator=(const vpHomography &H)
+vpHomography &vpHomography::operator=(const vpHomography &H)
{
- for (unsigned int i=0; i< 3; i++)
- for (unsigned int j=0; j< 3; j++)
- (*this)[i][j] = H[i][j];
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ (*this)[i][j] = H[i][j];
+ aMb = H.aMb;
+ bP = H.bP;
return *this;
}
/*!
@@ -418,15 +368,14 @@ vpHomography::operator=(const vpHomography &H)
\param H : Matrix to be copied.
*/
-vpHomography &
-vpHomography::operator=(const vpMatrix &H)
+vpHomography &vpHomography::operator=(const vpMatrix &H)
{
- if (H.getRows() !=3 || H.getCols() != 3)
+ if (H.getRows() != 3 || H.getCols() != 3)
throw(vpException(vpException::dimensionError, "The matrix is not an homography"));
- for (unsigned int i=0; i< 3; i++)
- for (unsigned int j=0; j< 3; j++)
- (*this)[i][j] = H[i][j];
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ (*this)[i][j] = H[i][j];
return *this;
}
@@ -439,20 +388,15 @@ vpHomography::operator=(const vpMatrix &H)
\sa save()
*/
-void
-vpHomography::load(std::ifstream &f)
+void vpHomography::load(std::ifstream &f)
{
- if (! f.fail())
- {
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- {
- f >> (*this)[i][j] ;
+ if (!f.fail()) {
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++) {
+ f >> (*this)[i][j];
}
- }
- else
- {
- throw(vpException(vpException::ioError, "Cannot read the homography from the input stream")) ;
+ } else {
+ throw(vpException(vpException::ioError, "Cannot read the homography from the input stream"));
}
}
@@ -463,28 +407,26 @@ vpHomography::load(std::ifstream &f)
{ ^b{\bf n}^T}
\f]
*/
-void
-vpHomography::build()
+void vpHomography::build()
{
- vpColVector n(3) ;
- vpColVector atb(3) ;
- vpMatrix aRb(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- {
+ vpColVector n(3);
+ vpColVector atb(3);
+ vpMatrix aRb(3, 3);
+ for (unsigned int i = 0; i < 3; i++) {
atb[i] = aMb[i][3];
- for (unsigned int j=0 ; j < 3 ; j++)
+ for (unsigned int j = 0; j < 3; j++)
aRb[i][j] = aMb[i][j];
}
- bP.getNormal(n) ;
+ 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
+ double d = bP.getD();
+ vpMatrix aHb = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
+ // plane equation. So if the plane is described by
+ // Ax+By+Cz+D=0, d=-D
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
(*this)[i][j] = aHb[i][j];
}
@@ -496,30 +438,26 @@ 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) ;
- vpMatrix aRb(3,3);
- for (unsigned int i=0 ; i < 3 ; i++)
- {
- atb[i] = aMb[i][3] ;
- for (unsigned int j=0 ; j < 3 ; j++)
+ vpColVector n(3);
+ vpColVector atb(3);
+ vpMatrix aRb(3, 3);
+ for (unsigned int i = 0; i < 3; i++) {
+ atb[i] = aMb[i][3];
+ for (unsigned int j = 0; j < 3; j++)
aRb[i][j] = aMb[i][j];
}
- bP.getNormal(n) ;
+ 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
+ double d = bP.getD();
+ vpMatrix aHb_ = aRb - atb * n.t() / d; // the d used in the equation is such as nX=d is the
+ // plane equation. So if the plane is described by
+ // Ax+By+Cz+D=0, d=-D
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
aHb[i][j] = aHb_[i][j];
}
@@ -527,13 +465,12 @@ vpHomography::build(vpHomography &aHb,
Set the homography as identity transformation by setting the diagonal to 1
and all other values to 0.
*/
-void
-vpHomography::eye()
+void vpHomography::eye()
{
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3; j++)
- if (i==j)
- (*this)[i][j] = 1.0 ;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ if (i == j)
+ (*this)[i][j] = 1.0;
else
(*this)[i][j] = 0.0;
}
@@ -545,22 +482,20 @@ vpHomography::eye()
Set the homography as identity transformation.
\sa eye()
*/
-void
-vpHomography::setIdentity()
-{
- eye();
-}
+void vpHomography::setIdentity() { eye(); }
#endif // #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
/*!
- Given \c iPa a point with coordinates \f$(u_a,v_a)\f$ expressed in pixel in image a, and the homography \c bHa that
- links image a and b, computes the coordinates of the point \f$(u_b,v_b)\f$ in the image b using the camera parameters
- matrix \f$\bf K\f$.
+ Given \c iPa a point with coordinates \f$(u_a,v_a)\f$ expressed in pixel in
+ image a, and the homography \c bHa that links image a and b, computes the
+ coordinates of the point \f$(u_b,v_b)\f$ in the image b using the camera
+ parameters matrix \f$\bf K\f$.
- Compute \f$^b{\bf p} = {\bf K} \; {^b}{\bf H}_a \; {\bf K}^{-1} {^a}{\bf p}\f$ with \f$^a{\bf p}=(u_a,v_a,1)\f$
- and \f$^b{\bf p}=(u_b,v_b,1)\f$
+ Compute \f$^b{\bf p} = {\bf K} \; {^b}{\bf H}_a \; {\bf K}^{-1} {^a}{\bf
+ p}\f$ with \f$^a{\bf p}=(u_a,v_a,1)\f$ and \f$^b{\bf p}=(u_b,v_b,1)\f$
- \return The coordinates in pixel of the point with coordinates \f$(u_b,v_b)\f$.
+ \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)
{
@@ -577,13 +512,16 @@ vpImagePoint vpHomography::project(const vpCameraParameters &cam, const vpHomogr
}
/*!
- Given \c Pa a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the image plane a, and the homography \c bHa that
- links image a and b, computes the normalized coordinates of the point \f$(x_b,y_b,1)\f$ in the image plane b.
+ Given \c Pa a point with normalized coordinates \f$(x_a,y_a,1)\f$ in the
+ image plane a, and the homography \c bHa that links image a and b, computes
+ the normalized coordinates of the point \f$(x_b,y_b,1)\f$ in the image plane
+ b.
- Compute \f$^b{\bf p} = {^b}{\bf H}_a \; {^a}{\bf p}\f$ with \f$^a{\bf p}=(x_a,y_a,1)\f$
- and \f$^b{\bf p}=(x_b,y_b,1)\f$
+ Compute \f$^b{\bf p} = {^b}{\bf H}_a \; {^a}{\bf p}\f$ with \f$^a{\bf
+ p}=(x_a,y_a,1)\f$ and \f$^b{\bf p}=(x_b,y_b,1)\f$
- \return The coordinates in meter of the point with coordinates \f$(x_b,y_b)\f$.
+ \return The coordinates in meter of the point with coordinates
+ \f$(x_b,y_b)\f$.
*/
vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa)
{
@@ -602,63 +540,57 @@ vpPoint vpHomography::project(const vpHomography &bHa, const vpPoint &Pa)
/*!
From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a
- and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, computes the
- homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}\f$
- using a robust estimation scheme.
+ and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates,
+ computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\;
+ ^b{\bf p}\f$ using a robust estimation scheme.
- This method is to compare to DLT() except that here a robust estimator is used to reject
- couples of points that are considered as outliers.
+ This method is to compare to DLT() except that here a robust estimator is
+ used to reject couples of points that are considered as outliers.
At least 4 couples of points are needed.
- \param xb, yb : Coordinates vector of matched points in image b. These coordinates are expressed in meters.
- \param xa, ya : Coordinates vector of matched points in image a. These coordinates are expressed in meters.
- \param aHb : Estimated homography that relies the transformation from image a to image b.
- \param inliers : Vector that indicates if a matched point is an inlier (true) or an outlier (false).
- \param residual : Global residual computed as
- \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the
- number of inliers.
- \param weights_threshold : Threshold applied on the weights updated during the robust estimation and
- used to consider if a point is an outlier or an inlier. Values should be in [0:1].
- A couple of matched points that have a weight lower than this threshold is considered as an outlier.
- A value equal to zero indicates that all the points are inliers.
- \param niter : Number of iterations of the estimation process.
- \param normalization : When set to true, the coordinates of the points are normalized. The normalization
+ \param xb, yb : Coordinates vector of matched points in image b. These
+ coordinates are expressed in meters. \param xa, ya : Coordinates vector of
+ matched points in image a. These coordinates are expressed in meters. \param
+ aHb : Estimated homography that relies the transformation from image a to
+ image b. \param inliers : Vector that indicates if a matched point is an
+ inlier (true) or an outlier (false). \param residual : Global residual
+ computed as \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf
+ H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the number of inliers. \param
+ weights_threshold : Threshold applied on the weights updated during the
+ robust estimation and used to consider if a point is an outlier or an
+ inlier. Values should be in [0:1]. A couple of matched points that have a
+ weight lower than this threshold is considered as an outlier. A value equal
+ to zero indicates that all the points are inliers. \param niter : Number of
+ iterations of the estimation process. \param normalization : When set to
+ true, the coordinates of the points are normalized. The normalization
carried out is the one preconized by Hartley.
\sa DLT(), ransac()
*/
-void
-vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- double weights_threshold,
- unsigned int niter,
- bool normalization)
+void vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
+ double &residual, double weights_threshold, unsigned int niter, bool normalization)
{
- unsigned int n = (unsigned int) xb.size();
+ 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)
+ if (n < 4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
- try{
+ try {
std::vector<double> xan, yan, xbn, ybn;
- double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
+ double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
vpHomography aHbn;
if (normalization) {
vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
- }
- else {
+ } else {
xbn = xb;
ybn = yb;
xan = xa;
@@ -666,111 +598,114 @@ vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &y
}
unsigned int nbLinesA = 2;
- vpMatrix A(nbLinesA*n,8);
+ vpMatrix A(nbLinesA * n, 8);
vpColVector X(8);
- vpColVector Y(nbLinesA*n);
- vpMatrix W(nbLinesA*n, nbLinesA*n) ; // Weight matrix
+ vpColVector Y(nbLinesA * n);
+ vpMatrix W(nbLinesA * n, nbLinesA * n); // Weight matrix
- vpColVector w(nbLinesA*n) ;
+ vpColVector w(nbLinesA * n);
- // All the weights are set to 1 at the beginning to use a classical least square scheme
+ // All the weights are set to 1 at the beginning to use a classical least
+ // square scheme
w = 1;
// Update the square matrix associated to the weights
- for (unsigned int i=0; i < nbLinesA*n; i ++) {
+ for (unsigned int i = 0; i < nbLinesA * n; i++) {
W[i][i] = w[i];
}
// build matrix A
- for(unsigned int i=0; i<n;i++) {
- A[nbLinesA*i][0]=xbn[i];
- A[nbLinesA*i][1]=ybn[i];
- A[nbLinesA*i][2]=1;
- A[nbLinesA*i][3]=0 ;
- A[nbLinesA*i][4]=0 ;
- A[nbLinesA*i][5]=0;
- A[nbLinesA*i][6]=-xbn[i]*xan[i] ;
- A[nbLinesA*i][7]=-ybn[i]*xan[i];
-
- A[nbLinesA*i+1][0]=0 ;
- A[nbLinesA*i+1][1]=0;
- A[nbLinesA*i+1][2]=0;
- A[nbLinesA*i+1][3]=xbn[i];
- A[nbLinesA*i+1][4]=ybn[i];
- A[nbLinesA*i+1][5]=1;
- A[nbLinesA*i+1][6]=-xbn[i]*yan[i];
- A[nbLinesA*i+1][7]=-ybn[i]*yan[i];
-
- Y[nbLinesA*i] = xan[i];
- Y[nbLinesA*i+1] = yan[i];
+ for (unsigned int i = 0; i < n; i++) {
+ A[nbLinesA * i][0] = xbn[i];
+ A[nbLinesA * i][1] = ybn[i];
+ A[nbLinesA * i][2] = 1;
+ A[nbLinesA * i][3] = 0;
+ A[nbLinesA * i][4] = 0;
+ A[nbLinesA * i][5] = 0;
+ A[nbLinesA * i][6] = -xbn[i] * xan[i];
+ A[nbLinesA * i][7] = -ybn[i] * xan[i];
+
+ A[nbLinesA * i + 1][0] = 0;
+ A[nbLinesA * i + 1][1] = 0;
+ A[nbLinesA * i + 1][2] = 0;
+ A[nbLinesA * i + 1][3] = xbn[i];
+ A[nbLinesA * i + 1][4] = ybn[i];
+ A[nbLinesA * i + 1][5] = 1;
+ A[nbLinesA * i + 1][6] = -xbn[i] * yan[i];
+ A[nbLinesA * i + 1][7] = -ybn[i] * yan[i];
+
+ Y[nbLinesA * i] = xan[i];
+ Y[nbLinesA * i + 1] = yan[i];
}
vpMatrix WA;
vpMatrix WAp;
unsigned int iter = 0;
- vpRobust r(nbLinesA*n) ; // M-Estimator
+ vpRobust r(nbLinesA * n); // M-Estimator
while (iter < niter) {
WA = W * A;
- X = WA.pseudoInverse(1e-26)*W*Y;
+ X = WA.pseudoInverse(1e-26) * W * Y;
vpColVector residu;
residu = Y - A * X;
// Compute the weights using the Tukey biweight M-Estimator
- r.setIteration(iter) ;
- r.MEstimator(vpRobust::TUKEY, residu, w) ;
+ r.setIteration(iter);
+ r.MEstimator(vpRobust::TUKEY, residu, w);
// Update the weights matrix
- for (unsigned int i=0; i < n*nbLinesA; i ++) {
+ for (unsigned int i = 0; i < n * nbLinesA; i++) {
W[i][i] = w[i];
}
// Build the homography
- for(unsigned int i=0;i<8;i++)
- aHbn.data[i]= X[i];
+ for (unsigned int i = 0; i < 8; i++)
+ aHbn.data[i] = X[i];
aHbn[2][2] = 1;
{
vpMatrix aHbnorm = aHbn.convert();
- aHbnorm /= aHbnorm[2][2] ;
+ aHbnorm /= aHbnorm[2][2];
}
- iter ++;
+ iter++;
}
inliers.resize(n);
unsigned int nbinliers = 0;
- for(unsigned int i=0; i< n; i++) {
- if (w[i*2] < weights_threshold && w[i*2+1] < weights_threshold)
+ for (unsigned int i = 0; i < n; i++) {
+ if (w[i * 2] < weights_threshold && w[i * 2 + 1] < weights_threshold)
inliers[i] = false;
else {
inliers[i] = true;
- nbinliers ++;
+ nbinliers++;
}
}
if (normalization) {
// H after denormalization
vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
- }
- else {
+ } else {
aHb = aHbn;
}
- residual = 0 ;
+ residual = 0;
vpColVector a(3), b(3), c(3);
- for (unsigned int i=0 ; i < n ; i++) {
+ for (unsigned int i = 0; i < n; i++) {
if (inliers[i]) {
- a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
- b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- residual += (a-c).sumSquare();
+ a[0] = xa[i];
+ a[1] = ya[i];
+ a[2] = 1;
+ b[0] = xb[i];
+ b[1] = yb[i];
+ b[2] = 1;
+
+ c = aHb * b;
+ c /= c[2];
+ residual += (a - c).sumSquare();
}
}
- residual = sqrt(residual/nbinliers);
- }
- catch(...)
- {
- throw(vpException(vpException::fatalError, "Cannot estimate an homography")) ;
+ residual = sqrt(residual / nbinliers);
+ } catch (...) {
+ throw(vpException(vpException::fatalError, "Cannot estimate an homography"));
}
}
@@ -781,8 +716,7 @@ vpHomography::robust(const std::vector<double> &xb, const std::vector<double> &y
\param ipb : Homography defining the relation between frame a and frame b.
\return The projected image point in the frame a.
*/
-vpImagePoint
-vpHomography::projection(const vpImagePoint &ipb)
+vpImagePoint vpHomography::projection(const vpImagePoint &ipb)
{
vpImagePoint ipa;
double u = ipb.get_u();
@@ -792,7 +726,7 @@ vpHomography::projection(const vpImagePoint &ipb)
double v_a = (*this)[1][0] * u + (*this)[1][1] * v + (*this)[1][2];
double w_a = (*this)[2][0] * u + (*this)[2][1] * v + (*this)[2][2];
- if(std::fabs(w_a) > std::numeric_limits<double>::epsilon()){
+ if (std::fabs(w_a) > std::numeric_limits<double>::epsilon()) {
ipa.set_u(u_a / w_a);
ipa.set_v(v_a / w_a);
}
@@ -807,8 +741,8 @@ vpHomography::projection(const vpImagePoint &ipb)
vpMatrix vpHomography::convert() const
{
vpMatrix M(3, 3);
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
M[i][j] = (*this)[i][j];
return M;
diff --git a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp
index f172033..82aa35b 100644
--- a/modules/vision/src/homography-estimation/vpHomographyDLT.cpp
+++ b/modules/vision/src/homography-estimation/vpHomographyDLT.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpHomographyDLT.cpp
@@ -43,111 +43,100 @@
estimation using the DLT algorithm
*/
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpMatrixException.h>
+#include <visp3/vision/vpHomography.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-void
-vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
- std::vector<double> &xn, std::vector<double> &yn,
- double &xg, double &yg, double &coef)
+void vpHomography::HartleyNormalization(const std::vector<double> &x, const std::vector<double> &y,
+ std::vector<double> &xn, std::vector<double> &yn, double &xg, double &yg,
+ double &coef)
{
if (x.size() != y.size())
- throw(vpException(vpException::dimensionError,
- "Hartley normalization require that x and y vector have the same dimension"));
+ throw(vpException(vpException::dimensionError, "Hartley normalization require that x and y vector "
+ "have the same dimension"));
- unsigned int n = (unsigned int) x.size();
+ unsigned int n = (unsigned int)x.size();
if (xn.size() != n)
xn.resize(n);
if (yn.size() != n)
yn.resize(n);
- xg = 0 ;
- yg = 0 ;
+ xg = 0;
+ yg = 0;
- for (unsigned int i =0 ; i < n ; i++)
- {
- xg += x[i] ;
- yg += y[i] ;
+ for (unsigned int i = 0; i < n; i++) {
+ xg += x[i];
+ yg += y[i];
}
- xg /= n ;
- yg /= n ;
+ xg /= n;
+ yg /= n;
// Changement d'origine : le centre de gravite doit correspondre
// a l'origine des coordonnees
- double distance=0;
- for(unsigned int i=0; i<n;i++)
- {
- double xni=x[i]-xg;
- double yni=y[i]-yg;
- xn[i] = xni ;
- yn[i] = yni ;
- distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
- }//for translation sur tous les points
-
- //Changement d'echelle
- distance/=n;
- //calcul du coef de changement d'echelle
- //if(distance ==0)
- if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
- coef=1;
+ double distance = 0;
+ for (unsigned int i = 0; i < n; i++) {
+ double xni = x[i] - xg;
+ double yni = y[i] - yg;
+ xn[i] = xni;
+ yn[i] = yni;
+ distance += sqrt(vpMath::sqr(xni) + vpMath::sqr(yni));
+ } // for translation sur tous les points
+
+ // Changement d'echelle
+ distance /= n;
+ // calcul du coef de changement d'echelle
+ // if(distance ==0)
+ if (std::fabs(distance) <= std::numeric_limits<double>::epsilon())
+ coef = 1;
else
- coef=sqrt(2.0)/distance;
+ coef = sqrt(2.0) / distance;
- for(unsigned int i=0; i<n;i++)
- {
+ for (unsigned int i = 0; i < n; i++) {
xn[i] *= coef;
yn[i] *= coef;
}
}
-void
-vpHomography::HartleyNormalization(unsigned int n,
- const double *x, const double *y,
- double *xn, double *yn,
- double &xg, double &yg,
- double &coef)
+void vpHomography::HartleyNormalization(unsigned int n, const double *x, const double *y, double *xn, double *yn,
+ double &xg, double &yg, double &coef)
{
unsigned int i;
- xg = 0 ;
- yg = 0 ;
+ xg = 0;
+ yg = 0;
- for (i =0 ; i < n ; i++)
- {
- xg += x[i] ;
- yg += y[i] ;
+ for (i = 0; i < n; i++) {
+ xg += x[i];
+ yg += y[i];
}
- xg /= n ;
- yg /= n ;
+ xg /= n;
+ yg /= n;
- //Changement d'origine : le centre de gravite doit correspondre
+ // Changement d'origine : le centre de gravite doit correspondre
// a l'origine des coordonnees
- double distance=0;
- for(i=0; i<n;i++)
- {
- double xni=x[i]-xg;
- double yni=y[i]-yg;
- xn[i] = xni ;
- yn[i] = yni ;
- distance+=sqrt(vpMath::sqr(xni)+vpMath::sqr(yni));
- }//for translation sur tous les points
-
- //Changement d'echelle
- distance/=n;
- //calcul du coef de changement d'echelle
- //if(distance ==0)
- if(std::fabs(distance) <= std::numeric_limits<double>::epsilon())
- coef=1;
+ double distance = 0;
+ for (i = 0; i < n; i++) {
+ double xni = x[i] - xg;
+ double yni = y[i] - yg;
+ xn[i] = xni;
+ yn[i] = yni;
+ distance += sqrt(vpMath::sqr(xni) + vpMath::sqr(yni));
+ } // for translation sur tous les points
+
+ // Changement d'echelle
+ distance /= n;
+ // calcul du coef de changement d'echelle
+ // if(distance ==0)
+ if (std::fabs(distance) <= std::numeric_limits<double>::epsilon())
+ coef = 1;
else
- coef=sqrt(2.0)/distance;
+ coef = sqrt(2.0) / distance;
- for(i=0; i<n;i++)
- {
+ for (i = 0; i < n; i++) {
xn[i] *= coef;
yn[i] *= coef;
}
@@ -155,53 +144,50 @@ vpHomography::HartleyNormalization(unsigned int n,
//---------------------------------------------------------------------------------------
-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
- //en fonction des deux normalisations effectuees au debut sur
- //les points: aHb = T2^ aHbn T1
- vpMatrix T1(3,3);
- vpMatrix T2(3,3);
- vpMatrix T2T(3,3);
+ // calcul des transformations a appliquer sur M_norm pour obtenir M
+ // en fonction des deux normalisations effectuees au debut sur
+ // les points: aHb = T2^ aHbn T1
+ vpMatrix T1(3, 3);
+ vpMatrix T2(3, 3);
+ vpMatrix T2T(3, 3);
T1.eye();
T2.eye();
T2T.eye();
- T1[0][0]=T1[1][1]=coef1;
- T1[0][2]=-coef1*xg1 ;
- T1[1][2]=-coef1*yg1 ;
+ T1[0][0] = T1[1][1] = coef1;
+ T1[0][2] = -coef1 * xg1;
+ T1[1][2] = -coef1 * yg1;
- T2[0][0]=T2[1][1]=coef2;
- T2[0][2]=-coef2*xg2 ;
- T2[1][2]=-coef2*yg2 ;
+ T2[0][0] = T2[1][1] = coef2;
+ T2[0][2] = -coef2 * xg2;
+ T2[1][2] = -coef2 * yg2;
- T2T=T2.pseudoInverse(1e-16) ;
+ T2T = T2.pseudoInverse(1e-16);
- vpMatrix aHbn_(3,3);
- for(unsigned int i=0; i<3; i++)
- for(unsigned int j=0; j<3; j++)
+ vpMatrix aHbn_(3, 3);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
aHbn_[i][j] = aHbn[i][j];
- vpMatrix maHb=T2T*aHbn_*T1;
+ vpMatrix maHb = T2T * aHbn_ * T1;
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- aHb[i][j] = maHb[i][j] ;
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ aHb[i][j] = maHb[i][j];
}
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a
- and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, computes the
- homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}\f$
- using the DLT (Direct Linear Transform) algorithm.
+ and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates,
+ computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\;
+ ^b{\bf p}\f$ using the DLT (Direct Linear Transform) algorithm.
At least 4 couples of points are needed.
@@ -218,15 +204,17 @@ vpHomography::HartleyDenormalization(vpHomography &aHbn,
\f[
^a{\bf p} \times {^a{\bf H}_b \; ^b{\bf p}} =0
\f]
- If we note \f$\mathbf{h}_j^T\f$ the \f$j^{\textrm{th}}\f$ line of \f$^a{\bf H}_b\f$, we can write:
- \f[
- ^a{\bf H}_b \; ^b{\bf p} = \left( \begin{array}{c}\mathbf{h}_1^T \;^b{\bf p} \\\mathbf{h}_2^T \; ^b{\bf p} \\\mathbf{h}_3^T \;^b{\bf p} \end{array}\right)
- \f]
-
- Setting \f$^a{\bf p}=(x_{a},y_{a},w_{a})\f$, the cross product can be rewritten by:
- \f[
- ^a{\bf p} \times ^a{\bf H}_b \; ^b{\bf p} =\left( \begin{array}{c}y_{a}\mathbf{h}_3^T \; ^b{\bf p}-w_{a}\mathbf{h}_2^T \; ^b{\bf p} \\w_{a}\mathbf{h}_1^T \; ^b{\bf p} -x_{a}\mathbf{h}_3^T \; ^b{\bf p} \\x_{a}\mathbf{h}_2^T \; ^b{\bf p}- y_{a}\mathbf{h}_1^T \; ^b{\bf p}\end{array}\right)
- \f]
+ If we note \f$\mathbf{h}_j^T\f$ the \f$j^{\textrm{th}}\f$ line of
+ \f$^a{\bf H}_b\f$, we can write: \f[ ^a{\bf H}_b \; ^b{\bf p} = \left(
+ \begin{array}{c}\mathbf{h}_1^T \;^b{\bf p} \\\mathbf{h}_2^T \; ^b{\bf p}
+ \\\mathbf{h}_3^T \;^b{\bf p} \end{array}\right) \f]
+
+ Setting \f$^a{\bf p}=(x_{a},y_{a},w_{a})\f$, the cross product can be
+ rewritten by: \f[ ^a{\bf p} \times ^a{\bf H}_b \; ^b{\bf p} =\left(
+ \begin{array}{c}y_{a}\mathbf{h}_3^T \; ^b{\bf p}-w_{a}\mathbf{h}_2^T \;
+ ^b{\bf p} \\w_{a}\mathbf{h}_1^T \; ^b{\bf p} -x_{a}\mathbf{h}_3^T \; ^b{\bf
+ p} \\x_{a}\mathbf{h}_2^T \; ^b{\bf p}- y_{a}\mathbf{h}_1^T \; ^b{\bf
+ p}\end{array}\right) \f]
\f[
\underbrace{\left( \begin{array}{ccc}\mathbf{0}^T & -w_{a} \; ^b{\bf p}^T
@@ -235,152 +223,146 @@ vpHomography::HartleyDenormalization(vpHomography &aHbn,
-y_{a} \; ^b{\bf p}^T & x_{a} \; ^b{\bf p}^T &
\mathbf{0}^T\end{array}\right)}_{\mathbf{A}_i (3\times 9)}
\underbrace{\left( \begin{array}{c}\mathbf{h}_{1}^{T} \\
- \mathbf{h}_{2}^{T}\\\mathbf{h}_{3}^{T}\end{array}\right)}_{\mathbf{h} (9\times 1)}=0
- \f]
+ \mathbf{h}_{2}^{T}\\\mathbf{h}_{3}^{T}\end{array}\right)}_{\mathbf{h}
+ (9\times 1)}=0 \f]
- leading to an homogeneous system to be solved: \f$\mathbf{A}\mathbf{h}=0\f$ with
- \f$\mathbf{A}=\left(\mathbf{A}_1^T, ..., \mathbf{A}_i^T, ..., \mathbf{A}_n^T \right)^T\f$.
+ leading to an homogeneous system to be solved:
+ \f$\mathbf{A}\mathbf{h}=0\f$ with \f$\mathbf{A}=\left(\mathbf{A}_1^T, ...,
+ \mathbf{A}_i^T, ..., \mathbf{A}_n^T \right)^T\f$.
It can be solved using an SVD decomposition:
\f[\bf A = UDV^T \f]
- <b>h</b> is the column of <b>V</b> associated with the smalest singular value of <b>A
+ <b>h</b> is the column of <b>V</b> associated with the smalest singular
+ value of <b>A
</b>
- \param xb, yb : Coordinates vector of matched points in image b. These coordinates are expressed in meters.
- \param xa, ya : Coordinates vector of matched points in image a. These coordinates are expressed in meters.
- \param aHb : Estimated homography that relies the transformation from image a to image b.
- \param normalization : When set to true, the coordinates of the points are normalized. The normalization
- carried out is the one preconized by Hartley.
+ \param xb, yb : Coordinates vector of matched points in image b. These
+ coordinates are expressed in meters. \param xa, ya : Coordinates vector of
+ matched points in image a. These coordinates are expressed in meters. \param
+ aHb : Estimated homography that relies the transformation from image a to
+ image b. \param normalization : When set to true, the coordinates of the
+ points are normalized. The normalization carried out is the one preconized
+ by Hartley.
\exception vpMatrixException::rankDeficient : When the rank of the matrix
that should be 8 is deficient.
*/
-void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya ,
- vpHomography &aHb,
- bool normalization)
+void vpHomography::DLT(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, bool normalization)
{
- unsigned int n = (unsigned int) xb.size();
+ 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)
+ if (n < 4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
- try{
+ try {
std::vector<double> xan, yan, xbn, ybn;
- double xg1=0., yg1=0., coef1=0., xg2=0., yg2=0., coef2=0.;
+ double xg1 = 0., yg1 = 0., coef1 = 0., xg2 = 0., yg2 = 0., coef2 = 0.;
vpHomography aHbn;
if (normalization) {
vpHomography::HartleyNormalization(xb, yb, xbn, ybn, xg1, yg1, coef1);
vpHomography::HartleyNormalization(xa, ya, xan, yan, xg2, yg2, coef2);
- }
- else {
+ } else {
xbn = xb;
ybn = yb;
xan = xa;
yan = ya;
}
- vpMatrix A(2*n,9);
+ vpMatrix A(2 * n, 9);
vpColVector h(9);
vpColVector D(9);
- vpMatrix V(9,9);
+ vpMatrix V(9, 9);
// We need here to compute the SVD on a (n*2)*9 matrix (where n is
// the number of points). if n == 4, the matrix has more columns
// than rows. This kind of matrix is not supported by GSL for
// SVD. The solution is to add an extra line with zeros
if (n == 4)
- A.resize(2*n+1,9);
+ A.resize(2 * n + 1, 9);
// build matrix A
- for(unsigned int i=0; i<n;i++)
- {
- A[2*i][0]=0;
- A[2*i][1]=0;
- A[2*i][2]=0;
- A[2*i][3]=-xbn[i] ;
- A[2*i][4]=-ybn[i] ;
- A[2*i][5]=-1;
- A[2*i][6]=xbn[i]*yan[i] ;
- A[2*i][7]=ybn[i]*yan[i];
- A[2*i][8]=yan[i];
-
-
- A[2*i+1][0]=xbn[i] ;
- A[2*i+1][1]=ybn[i] ;
- A[2*i+1][2]=1;
- A[2*i+1][3]=0;
- A[2*i+1][4]=0;
- A[2*i+1][5]=0;
- A[2*i+1][6]=-xbn[i]*xan[i];
- A[2*i+1][7]=-ybn[i]*xan[i];
- A[2*i+1][8]=-xan[i] ;
+ for (unsigned int i = 0; i < n; i++) {
+ A[2 * i][0] = 0;
+ A[2 * i][1] = 0;
+ A[2 * i][2] = 0;
+ A[2 * i][3] = -xbn[i];
+ A[2 * i][4] = -ybn[i];
+ A[2 * i][5] = -1;
+ A[2 * i][6] = xbn[i] * yan[i];
+ A[2 * i][7] = ybn[i] * yan[i];
+ A[2 * i][8] = yan[i];
+
+ A[2 * i + 1][0] = xbn[i];
+ A[2 * i + 1][1] = ybn[i];
+ A[2 * i + 1][2] = 1;
+ A[2 * i + 1][3] = 0;
+ A[2 * i + 1][4] = 0;
+ A[2 * i + 1][5] = 0;
+ A[2 * i + 1][6] = -xbn[i] * xan[i];
+ A[2 * i + 1][7] = -ybn[i] * xan[i];
+ A[2 * i + 1][8] = -xan[i];
}
// Add an extra line with zero.
if (n == 4) {
- for (unsigned int i=0; i < 9; i ++) {
- A[2*n][i] = 0;
+ for (unsigned int i = 0; i < 9; i++) {
+ A[2 * n][i] = 0;
}
}
// solve Ah = 0
// SVD Decomposition A = UDV^T (destructive wrt A)
- A.svd(D,V);
+ A.svd(D, V);
// on en profite pour effectuer un controle sur le rang de la matrice :
// pas plus de 2 valeurs singulieres quasi=0
- int rank=0;
- for(unsigned int i = 0; i<9;i++) if(D[i]>1e-7) rank++;
- if(rank <7)
- {
- vpTRACE(" Rank is : %d, should be 8", rank);
- throw(vpMatrixException(vpMatrixException::rankDeficient,
- "\n\t\t Matrix rank is deficient")) ;
+ int rank = 0;
+ for (unsigned int i = 0; i < 9; i++)
+ if (D[i] > 1e-7)
+ rank++;
+ if (rank < 7) {
+ 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
+ // h = is the column of V associated with the smallest singular value of A
// since we are not sure that the svd implemented sort the
// singular value... we seek for the smallest
- double smallestSv = 1e30 ;
- unsigned int indexSmallestSv = 0 ;
- for (unsigned int i=0 ; i < 9 ; i++)
- if ((D[i] < smallestSv) ){ smallestSv = D[i] ;indexSmallestSv = i ; }
+ double smallestSv = 1e30;
+ unsigned int indexSmallestSv = 0;
+ for (unsigned int i = 0; i < 9; i++)
+ if ((D[i] < smallestSv)) {
+ smallestSv = D[i];
+ indexSmallestSv = i;
+ }
- h=V.getCol(indexSmallestSv);
+ h = V.getCol(indexSmallestSv);
// build the homography
- for(unsigned int i =0;i<3;i++)
- {
- for(unsigned int j=0;j<3;j++)
- aHbn[i][j]=h[3*i+j];
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++)
+ aHbn[i][j] = h[3 * i + j];
}
if (normalization) {
// H after denormalization
vpHomography::HartleyDenormalization(aHbn, aHb, xg1, yg1, coef1, xg2, yg2, coef2);
- }
- else {
+ } else {
aHb = aHbn;
}
- }
- catch(vpMatrixException &me)
- {
- vpTRACE("Matrix Exception ") ;
- throw(me) ;
- }
- catch(vpException &me)
- {
- vpERROR_TRACE("caught another error") ;
- std::cout <<std::endl << me << std::endl ;
- throw(me) ;
+ } catch (vpMatrixException &me) {
+ vpTRACE("Matrix Exception ");
+ throw(me);
+ } catch (vpException &me) {
+ vpERROR_TRACE("caught another error");
+ std::cout << std::endl << me << std::endl;
+ throw(me);
}
}
diff --git a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp
index c196701..728e70d 100644
--- a/modules/vision/src/homography-estimation/vpHomographyExtract.cpp
+++ b/modules/vision/src/homography-estimation/vpHomographyExtract.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,9 +36,9 @@
*
*****************************************************************************/
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpMath.h>
#include <math.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpHomography.h>
//#define DEBUG_Homographie 0
@@ -57,17 +58,15 @@ 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);
+ nd[0] = 0;
+ nd[1] = 0;
+ nd[2] = 1;
- vpColVector nd(3) ;
- nd[0]=0;nd[1]=0;nd[2]=1;
-
- computeDisplacement(*this,aRb,atb,n);
-
+ computeDisplacement(*this, aRb, atb, n);
}
/*!
@@ -90,12 +89,10 @@ 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);
+ computeDisplacement(*this, nd, aRb, atb, n);
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
@@ -122,53 +119,50 @@ 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 ****/
- vpMatrix aRbint(3,3) ;
+ vpMatrix aRbint(3, 3);
vpColVector svTemp(3), sv(3);
- vpMatrix mX(3,2) ;
+ vpMatrix mX(3, 2);
vpColVector aTbp(3), normaleEstimee(3);
double distanceFictive;
- double sinusTheta, cosinusTheta, signeSinus= 1;
+ double sinusTheta, cosinusTheta, signeSinus = 1;
double s, determinantU, determinantV;
unsigned int vOrdre[3];
- //vpColVector normaleDesiree(3) ;
- //normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
+ // vpColVector normaleDesiree(3) ;
+ // normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
vpColVector normaleDesiree(nd);
- /**** Corps de la focntion ****/
+/**** Corps de la focntion ****/
#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
+ printf("debut : Homographie_EstimationDeplacementCamera\n");
#endif
/* Allocation des matrices */
- vpMatrix mTempU(3,3) ;
- vpMatrix mTempV(3,3) ;
- vpMatrix mU(3,3) ;
- vpMatrix mV(3,3) ;
- vpMatrix aRbp(3,3) ;
+ vpMatrix mTempU(3, 3);
+ vpMatrix mTempV(3, 3);
+ vpMatrix mU(3, 3);
+ vpMatrix mV(3, 3);
+ vpMatrix aRbp(3, 3);
- vpMatrix mH(3,3) ;
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
+ vpMatrix mH(3, 3);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ mH[i][j] = aHb[i][j];
/* Preparation au calcul de la SVD */
- mTempU = mH ;
+ mTempU = mH;
/*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
*****/
- mTempU.svd(svTemp,mTempV) ;
+ mTempU.svd(svTemp, mTempV);
/* On va mettre les valeurs singulieres en ordre decroissant : */
@@ -176,27 +170,40 @@ void
if (svTemp[0] >= svTemp[1]) {
if (svTemp[0] >= svTemp[2]) {
if (svTemp[1] > svTemp[2]) {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
+ vOrdre[0] = 0;
+ vOrdre[1] = 1;
+ vOrdre[2] = 2;
} else {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ vOrdre[0] = 0;
+ vOrdre[1] = 2;
+ vOrdre[2] = 1;
}
} else {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ vOrdre[0] = 2;
+ vOrdre[1] = 0;
+ vOrdre[2] = 1;
}
} else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2])
- { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
- else
- { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
+ if (svTemp[1] >= svTemp[2]) {
+ if (svTemp[0] > svTemp[2]) {
+ vOrdre[0] = 1;
+ vOrdre[1] = 0;
+ vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 1;
+ vOrdre[1] = 2;
+ vOrdre[2] = 0;
+ }
} else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ vOrdre[0] = 2;
+ vOrdre[1] = 1;
+ vOrdre[2] = 0;
}
}
/*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
*****/
for (unsigned int i = 0; i < 3; i++) {
@@ -208,9 +215,12 @@ void
}
#ifdef DEBUG_Homographie
- printf("U : \n") ; std::cout << mU << std::endl ;
- printf("V : \n") ; std::cout << mV << std::endl ;
- printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
+ printf("U : \n");
+ std::cout << mU << std::endl;
+ printf("V : \n");
+ std::cout << mV << std::endl;
+ printf("Valeurs singulieres : ");
+ std::cout << sv.t();
#endif
/* A verifier si necessaire!!! */
@@ -220,26 +230,26 @@ void
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 ;
+ if (s < 0)
+ mV *= -1;
/* d' = d2 */
distanceFictive = sv[1];
#ifdef DEBUG_Homographie
- printf ("d = %f\n",distanceFictive);
+ printf("d = %f\n", distanceFictive);
#endif
- n.resize(3) ;
+ 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
/*****
- Cas ou le deplacement est une rotation pure
- et la normale reste indefini
- sv[0] = sv[1]= sv[2]
+ Cas ou le deplacement est une rotation pure
+ et la normale reste indefini
+ sv[0] = sv[1]= sv[2]
*****/
aTbp[0] = 0;
aTbp[1] = 0;
@@ -248,29 +258,25 @@ void
n[0] = normaleDesiree[0];
n[1] = normaleDesiree[1];
n[2] = normaleDesiree[2];
- }
- else
- {
+ } else {
#ifdef DEBUG_Homographie
printf("\nCas general\n");
#endif
/* Cas general */
/*****
- test pour determiner quelle est la bonne solution on teste
- d'abord quelle solution est plus proche de la perpendiculaire
- au plan de la cible constuction de la normale n
+ test pour determiner quelle est la bonne solution on teste
+ d'abord quelle solution est plus proche de la perpendiculaire
+ au plan de la cible constuction de la normale n
*****/
/*****
- Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
- dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
+ 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];
@@ -278,7 +284,7 @@ void
/* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
double cosinusAncien = 0.0;
- for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
+ for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */
/* Calcul de la normale estimee : n = V.n' */
@@ -295,11 +301,10 @@ void
cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
/*****
- Si la solution est meilleur
- Remarque : On ne teste pas le cas oppose (cos<0)
+ Si la solution est meilleur
+ Remarque : On ne teste pas le cas oppose (cos<0)
*****/
- if (cosinusDesireeEstimee > cosinusAncien)
- {
+ if (cosinusDesireeEstimee > cosinusAncien) {
cosinusAncien = cosinusDesireeEstimee;
/* Affectation de la normale qui est retourner */
@@ -317,10 +322,9 @@ void
else
signeSinus = 1;
} /* fin if (cosinusDesireeEstimee > cosinusAncien) */
- } /* fin k */
- } /* fin w */
- } /* fin else */
-
+ } /* fin k */
+ } /* fin w */
+ } /* fin else */
/* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
for (unsigned int i = 0; i < 3; i++) {
@@ -331,30 +335,37 @@ void
atb[i] /= distanceFictive;
}
-
#ifdef DEBUG_Homographie
- printf("t' : ") ; std::cout << aTbp.t() ;
- printf("t/d : ") ; std::cout << atb.t() ;
- printf("n : ") ; std::cout << n.t() ;
+ printf("t' : ");
+ std::cout << aTbp.t();
+ printf("t/d : ");
+ std::cout << atb.t();
+ printf("n : ");
+ std::cout << n.t();
#endif
/* Calcul de la matrice de rotation R */
/*****
- Calcul du sinus(theta) et du cosinus(theta)
- Remarque : sinus(theta) pourra changer de signe en fonction
- de la solution retenue (cf. ci-dessous)
+ Calcul du sinus(theta) et du cosinus(theta)
+ Remarque : sinus(theta) pourra changer de signe en fonction
+ de la solution retenue (cf. ci-dessous)
*****/
- sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
- *(sv[1]*sv[1] -sv[2]*sv[2]))
- / ((sv[0] + sv[2]) * sv[1]);
+ 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]);
+ cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
/* construction de la matrice de rotation R' */
- aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
- aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
- aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
+ aRbp[0][0] = cosinusTheta;
+ aRbp[0][1] = 0;
+ aRbp[0][2] = -sinusTheta;
+ aRbp[1][0] = 0;
+ aRbp[1][1] = 1;
+ aRbp[1][2] = 0;
+ aRbp[2][0] = sinusTheta;
+ aRbp[2][1] = 0;
+ aRbp[2][2] = cosinusTheta;
/* multiplication Rint = U R' */
for (unsigned int i = 0; i < 3; i++) {
@@ -375,9 +386,10 @@ void
}
}
}
- /*transpose_carre(aRb,3); */
+/*transpose_carre(aRb,3); */
#ifdef DEBUG_Homographie
- printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
+ printf("R : %d\n", aRb.isARotationMatrix());
+ std::cout << aRb << std::endl;
#endif
}
@@ -399,52 +411,51 @@ void
\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 ****/
- vpMatrix aRbint(3,3) ;
+ vpMatrix aRbint(3, 3);
vpColVector svTemp(3), sv(3);
- vpMatrix mX(3,2) ;
+ vpMatrix mX(3, 2);
vpColVector aTbp(3), normaleEstimee(3);
double distanceFictive;
- double sinusTheta, cosinusTheta, signeSinus= 1;
+ double sinusTheta, cosinusTheta, signeSinus = 1;
double s, determinantU, determinantV;
unsigned int vOrdre[3];
- vpColVector normaleDesiree(3) ;
- normaleDesiree[0]=0;normaleDesiree[1]=0;normaleDesiree[2]=1;
+ vpColVector normaleDesiree(3);
+ normaleDesiree[0] = 0;
+ normaleDesiree[1] = 0;
+ normaleDesiree[2] = 1;
- /**** Corps de la focntion ****/
+/**** Corps de la focntion ****/
#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
+ printf("debut : Homographie_EstimationDeplacementCamera\n");
#endif
/* Allocation des matrices */
- vpMatrix mTempU(3,3) ;
- vpMatrix mTempV(3,3) ;
- vpMatrix mU(3,3) ;
- vpMatrix mV(3,3) ;
- vpMatrix aRbp(3,3) ;
-
+ vpMatrix mTempU(3, 3);
+ vpMatrix mTempV(3, 3);
+ vpMatrix mU(3, 3);
+ vpMatrix mV(3, 3);
+ vpMatrix aRbp(3, 3);
- vpMatrix mH(3,3) ;
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++) mH[i][j] = aHb[i][j];
+ vpMatrix mH(3, 3);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ mH[i][j] = aHb[i][j];
/* Preparation au calcul de la SVD */
- mTempU = mH ;
+ mTempU = mH;
/*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
*****/
- mTempU.svd(svTemp,mTempV) ;
+ mTempU.svd(svTemp, mTempV);
/* On va mettre les valeurs singulieres en ordre decroissant : */
@@ -452,27 +463,40 @@ void
if (svTemp[0] >= svTemp[1]) {
if (svTemp[0] >= svTemp[2]) {
if (svTemp[1] > svTemp[2]) {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
+ vOrdre[0] = 0;
+ vOrdre[1] = 1;
+ vOrdre[2] = 2;
} else {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ vOrdre[0] = 0;
+ vOrdre[1] = 2;
+ vOrdre[2] = 1;
}
} else {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ vOrdre[0] = 2;
+ vOrdre[1] = 0;
+ vOrdre[2] = 1;
}
} else {
- if (svTemp[1] >= svTemp[2]){
- if (svTemp[0] > svTemp[2])
- { vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2; }
- else
- { vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0; }
+ if (svTemp[1] >= svTemp[2]) {
+ if (svTemp[0] > svTemp[2]) {
+ vOrdre[0] = 1;
+ vOrdre[1] = 0;
+ vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 1;
+ vOrdre[1] = 2;
+ vOrdre[2] = 0;
+ }
} else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ vOrdre[0] = 2;
+ vOrdre[1] = 1;
+ vOrdre[2] = 0;
}
}
/*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
*****/
for (unsigned int i = 0; i < 3; i++) {
@@ -484,9 +508,12 @@ void
}
#ifdef DEBUG_Homographie
- printf("U : \n") ; std::cout << mU << std::endl ;
- printf("V : \n") ; std::cout << mV << std::endl ;
- printf("Valeurs singulieres : ") ; std::cout << sv.t() ;
+ printf("U : \n");
+ std::cout << mU << std::endl;
+ printf("V : \n");
+ std::cout << mV << std::endl;
+ printf("Valeurs singulieres : ");
+ std::cout << sv.t();
#endif
/* A verifier si necessaire!!! */
@@ -496,26 +523,26 @@ void
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 ;
+ if (s < 0)
+ mV *= -1;
/* d' = d2 */
distanceFictive = sv[1];
#ifdef DEBUG_Homographie
- printf ("d = %f\n",distanceFictive);
+ printf("d = %f\n", distanceFictive);
#endif
- n.resize(3) ;
+ 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
/*****
- Cas ou le deplacement est une rotation pure
- et la normale reste indefini
- sv[0] = sv[1]= sv[2]
+ Cas ou le deplacement est une rotation pure
+ et la normale reste indefini
+ sv[0] = sv[1]= sv[2]
*****/
aTbp[0] = 0;
aTbp[1] = 0;
@@ -524,29 +551,25 @@ void
n[0] = normaleDesiree[0];
n[1] = normaleDesiree[1];
n[2] = normaleDesiree[2];
- }
- else
- {
+ } else {
#ifdef DEBUG_Homographie
printf("\nCas general\n");
#endif
/* Cas general */
/*****
- test pour determiner quelle est la bonne solution on teste
- d'abord quelle solution est plus proche de la perpendiculaire
- au plan de la cible constuction de la normale n
+ test pour determiner quelle est la bonne solution on teste
+ d'abord quelle solution est plus proche de la perpendiculaire
+ au plan de la cible constuction de la normale n
*****/
/*****
- Calcul de la normale au plan : n' = [ esp1*x1 , x2=0 , esp3*x3 ]
- dans l'ordre : cas (esp1=+1, esp3=+1) et (esp1=-1, esp3=+1)
+ 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];
@@ -554,7 +577,7 @@ void
/* Il y a 4 solutions pour n : 2 par cas => n1, -n1, n2, -n2 */
double cosinusAncien = 0.0;
- for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
+ for (unsigned int w = 0; w < 2; w++) { /* Pour les 2 cas */
for (unsigned int k = 0; k < 2; k++) { /* Pour le signe */
/* Calcul de la normale estimee : n = V.n' */
@@ -565,18 +588,16 @@ void
}
}
-
/* Calcul du cosinus de l'angle entre la normale reelle et desire */
double cosinusDesireeEstimee = 0.0;
for (unsigned int i = 0; i < 3; i++)
cosinusDesireeEstimee += normaleEstimee[i] * normaleDesiree[i];
/*****
- Si la solution est meilleur
- Remarque : On ne teste pas le cas oppose (cos<0)
+ Si la solution est meilleur
+ Remarque : On ne teste pas le cas oppose (cos<0)
*****/
- if (cosinusDesireeEstimee > cosinusAncien)
- {
+ if (cosinusDesireeEstimee > cosinusAncien) {
cosinusAncien = cosinusDesireeEstimee;
/* Affectation de la normale qui est retourner */
@@ -594,10 +615,9 @@ void
else
signeSinus = 1;
} /* fin if (cosinusDesireeEstimee > cosinusAncien) */
- } /* fin k */
- } /* fin w */
- } /* fin else */
-
+ } /* fin k */
+ } /* fin w */
+ } /* fin else */
/* Calcul du vecteur de translation qui est retourner : t = (U * t') / d */
for (unsigned int i = 0; i < 3; i++) {
@@ -609,29 +629,36 @@ void
}
#ifdef DEBUG_Homographie
- printf("t' : ") ; std::cout << aTbp.t() ;
- printf("t/d : ") ; std::cout << atb.t() ;
- printf("n : ") ; std::cout << n.t() ;
+ printf("t' : ");
+ std::cout << aTbp.t();
+ printf("t/d : ");
+ std::cout << atb.t();
+ printf("n : ");
+ std::cout << n.t();
#endif
-
/* Calcul de la matrice de rotation R */
/*****
- Calcul du sinus(theta) et du cosinus(theta)
- Remarque : sinus(theta) pourra changer de signe en fonction
- de la solution retenue (cf. ci-dessous)
+ Calcul du sinus(theta) et du cosinus(theta)
+ Remarque : sinus(theta) pourra changer de signe en fonction
+ de la solution retenue (cf. ci-dessous)
*****/
- sinusTheta = signeSinus*sqrt((sv[0]*sv[0] -sv[1]*sv[1])
- *(sv[1]*sv[1] -sv[2]*sv[2]))
- / ((sv[0] + sv[2]) * sv[1]);
+ 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]);
+ cosinusTheta = (sv[1] * sv[1] + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
/* construction de la matrice de rotation R' */
- aRbp[0][0] = cosinusTheta; aRbp[0][1] = 0; aRbp[0][2] = -sinusTheta;
- aRbp[1][0] = 0; aRbp[1][1] = 1; aRbp[1][2] = 0;
- aRbp[2][0] = sinusTheta; aRbp[2][1] = 0; aRbp[2][2] = cosinusTheta;
+ aRbp[0][0] = cosinusTheta;
+ aRbp[0][1] = 0;
+ aRbp[0][2] = -sinusTheta;
+ aRbp[1][0] = 0;
+ aRbp[1][1] = 1;
+ aRbp[1][2] = 0;
+ aRbp[2][0] = sinusTheta;
+ aRbp[2][1] = 0;
+ aRbp[2][2] = cosinusTheta;
/* multiplication Rint = U R' */
for (unsigned int i = 0; i < 3; i++) {
@@ -652,23 +679,20 @@ void
}
}
}
- /*transpose_carre(aRb,3); */
+/*transpose_carre(aRb,3); */
#ifdef DEBUG_Homographie
- printf("R : %d\n",aRb.isARotationMatrix() ) ; std::cout << aRb << std::endl ;
+ printf("R : %d\n", aRb.isARotationMatrix());
+ std::cout << aRb << std::endl;
#endif
-
}
-void vpHomography::computeDisplacement(const vpHomography &H,
- const double x,
- const double y,
- std::list<vpRotationMatrix> & vR,
- std::list<vpTranslationVector> & vT,
- std::list<vpColVector> & vN)
+void vpHomography::computeDisplacement(const vpHomography &H, const double x, const double y,
+ std::list<vpRotationMatrix> &vR, std::list<vpTranslationVector> &vT,
+ std::list<vpColVector> &vN)
{
#ifdef DEBUG_Homographie
- printf ("debut : Homographie_EstimationDeplacementCamera\n");
+ printf("debut : Homographie_EstimationDeplacementCamera\n");
#endif
vR.clear();
@@ -676,17 +700,17 @@ void vpHomography::computeDisplacement(const vpHomography &H,
vN.clear();
/**** Declarations des variables ****/
- int cas1 =1, cas2=2, cas3=3, cas4=4;
- int cas =0;
- bool norm1ok=false, norm2ok = false,norm3ok=false,norm4ok =false;
+ int cas1 = 1, cas2 = 2, cas3 = 3, cas4 = 4;
+ int cas = 0;
+ bool norm1ok = false, norm2ok = false, norm3ok = false, norm4ok = false;
- double tmp,determinantU,determinantV,s,distanceFictive;
- vpMatrix mTempU(3,3),mTempV(3,3),U(3,3),V(3,3);
+ double tmp, determinantU, determinantV, s, distanceFictive;
+ vpMatrix mTempU(3, 3), mTempV(3, 3), U(3, 3), V(3, 3);
- vpRotationMatrix Rprim1,R1;
- vpRotationMatrix Rprim2,R2;
- vpRotationMatrix Rprim3,R3;
- vpRotationMatrix Rprim4,R4;
+ vpRotationMatrix Rprim1, R1;
+ vpRotationMatrix Rprim2, R2;
+ vpRotationMatrix Rprim3, R3;
+ vpRotationMatrix Rprim4, R4;
vpTranslationVector Tprim1, T1;
vpTranslationVector Tprim2, T2;
vpTranslationVector Tprim3, T3;
@@ -696,18 +720,17 @@ void vpHomography::computeDisplacement(const vpHomography &H,
vpColVector Nprim3(3), N3(3);
vpColVector Nprim4(3), N4(3);
- vpColVector svTemp(3),sv(3);
+ vpColVector svTemp(3), sv(3);
unsigned int vOrdre[3];
vpColVector vTp(3);
-
/* Preparation au calcul de la SVD */
mTempU = H.convert();
/*****
- Remarque : mTempU, svTemp et mTempV sont modifies par svd
- Il est necessaire apres de les trier dans l'ordre decroissant
- des valeurs singulieres
+ Remarque : mTempU, svTemp et mTempV sont modifies par svd
+ Il est necessaire apres de les trier dans l'ordre decroissant
+ des valeurs singulieres
*****/
// cette fonction ne renvoit pas d'erreur
@@ -716,36 +739,42 @@ void vpHomography::computeDisplacement(const vpHomography &H,
/* On va mettre les valeurs singulieres en ordre decroissant : */
/* Determination de l'ordre des valeurs */
if (svTemp[0] >= svTemp[1]) {
- if (svTemp[0] >= svTemp[2])
- {
- if (svTemp[1] > svTemp[2])
- {
- vOrdre[0] = 0; vOrdre[1] = 1; vOrdre[2] = 2;
- }
- else
- {
- vOrdre[0] = 0; vOrdre[1] = 2; vOrdre[2] = 1;
+ if (svTemp[0] >= svTemp[2]) {
+ if (svTemp[1] > svTemp[2]) {
+ vOrdre[0] = 0;
+ vOrdre[1] = 1;
+ vOrdre[2] = 2;
+ } else {
+ vOrdre[0] = 0;
+ vOrdre[1] = 2;
+ vOrdre[2] = 1;
}
- }
- else
- {
- vOrdre[0] = 2; vOrdre[1] = 0; vOrdre[2] = 1;
+ } else {
+ vOrdre[0] = 2;
+ vOrdre[1] = 0;
+ vOrdre[2] = 1;
}
} else {
- if (svTemp[1] >= svTemp[2]){
+ if (svTemp[1] >= svTemp[2]) {
if (svTemp[0] > svTemp[2]) {
- vOrdre[0] = 1; vOrdre[1] = 0; vOrdre[2] = 2;
+ vOrdre[0] = 1;
+ vOrdre[1] = 0;
+ vOrdre[2] = 2;
} else {
- vOrdre[0] = 1; vOrdre[1] = 2; vOrdre[2] = 0;
+ vOrdre[0] = 1;
+ vOrdre[1] = 2;
+ vOrdre[2] = 0;
}
} else {
- vOrdre[0] = 2; vOrdre[1] = 1; vOrdre[2] = 0;
+ vOrdre[0] = 2;
+ vOrdre[1] = 1;
+ vOrdre[2] = 0;
}
}
/*****
- Tri decroissant des matrices U, V, sv
- en fonction des valeurs singulieres car
- hypothese : sv[0]>=sv[1]>=sv[2]>=0
+ Tri decroissant des matrices U, V, sv
+ en fonction des valeurs singulieres car
+ hypothese : sv[0]>=sv[1]>=sv[2]>=0
*****/
for (unsigned int i = 0; i < 3; i++) {
sv[i] = svTemp[vOrdre[i]];
@@ -756,24 +785,25 @@ void vpHomography::computeDisplacement(const vpHomography &H,
}
#ifdef DEBUG_Homographie
- printf("U : \n") ; Affiche(U) ;
- printf("V : \n") ; affiche(V) ;
- printf("Valeurs singulieres : ") ; affiche(sv);
+ printf("U : \n");
+ Affiche(U);
+ printf("V : \n");
+ affiche(V);
+ printf("Valeurs singulieres : ");
+ affiche(sv);
#endif
// calcul du determinant de U et V
- determinantU = U[0][0] * (U[1][1]*U[2][2] - U[1][2]*U[2][1]) +
- U[0][1] * (U[1][2]*U[2][0] - U[1][0]*U[2][2]) +
- U[0][2] * (U[1][0]*U[2][1] - U[1][1]*U[2][0]);
+ determinantU = U[0][0] * (U[1][1] * U[2][2] - U[1][2] * U[2][1]) + U[0][1] * (U[1][2] * U[2][0] - U[1][0] * U[2][2]) +
+ U[0][2] * (U[1][0] * U[2][1] - U[1][1] * U[2][0]);
- determinantV = V[0][0] * (V[1][1]*V[2][2] - V[1][2]*V[2][1]) +
- V[0][1] * (V[1][2]*V[2][0] - V[1][0]*V[2][2]) +
- V[0][2] * (V[1][0]*V[2][1] - V[1][1]*V[2][0]);
+ 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 :
@@ -786,9 +816,9 @@ void vpHomography::computeDisplacement(const vpHomography &H,
// (h_31x_1 + h_32x_2 + h_33)/d > 0
// et d = sd'
- tmp = H[2][0]*x + H[2][1]*y + H[2][2] ;
+ tmp = H[2][0] * x + H[2][1] * y + H[2][2];
- if ((tmp/(sv[1] *s)) > 0)
+ if ((tmp / (sv[1] * s)) > 0)
distanceFictive = sv[1];
else
distanceFictive = -sv[1];
@@ -799,22 +829,22 @@ void vpHomography::computeDisplacement(const vpHomography &H,
// 3eme cas : d1 <>d2 =d3
// 4eme cas : d1 =d2=d3
- if ((sv[0] - sv[1]) < sing_threshold)
- {
+ if ((sv[0] - sv[1]) < sing_threshold) {
if ((sv[1] - sv[2]) < sing_threshold)
cas = cas4;
else
cas = cas2;
- }
- else
- {
+ } else {
if ((sv[1] - sv[2]) < sing_threshold)
cas = cas3;
else
cas = cas1;
}
- Nprim1 = 0.0; Nprim2 = 0.0; Nprim3 = 0.0; Nprim4 = 0.0;
+ Nprim1 = 0.0;
+ Nprim2 = 0.0;
+ Nprim3 = 0.0;
+ Nprim4 = 0.0;
// on filtre ensuite les diff'erentes normales possibles
// condition : nm/d > 0
@@ -830,29 +860,27 @@ void vpHomography::computeDisplacement(const vpHomography &H,
// on calcule les quatres cas de normale
- if (cas == cas1)
- {
+ 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];
- Nprim3[0] = - Nprim1[0]; Nprim3[2] = Nprim1[2];
- Nprim4[0] = - Nprim1[0]; Nprim4[2] = - Nprim1[2];
+ Nprim2[0] = Nprim1[0];
+ Nprim2[2] = -Nprim1[2];
+ Nprim3[0] = -Nprim1[0];
+ Nprim3[2] = Nprim1[2];
+ Nprim4[0] = -Nprim1[0];
+ Nprim4[2] = -Nprim1[2];
}
- if (cas == cas2)
- {
+ if (cas == cas2) {
// 2 normales sont possibles
// x3 = +-1
Nprim1[2] = 1;
Nprim2[2] = -1;
}
- if (cas == cas3)
- {
+ if (cas == cas3) {
// 2 normales sont possibles
// x1 = +-1
Nprim1[0] = 1;
@@ -865,234 +893,200 @@ void vpHomography::computeDisplacement(const vpHomography &H,
// n^tm / d > 0
// attention, il faut travailler avec la bonne normale,
// soit Ni et non pas Nprimi
- if (cas == cas1)
- {
- N1 = V *Nprim1;
+ if (cas == cas1) {
+ N1 = V * Nprim1;
tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive *s);
- norm1ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm1ok = (tmp > 0);
- N2 = V *Nprim2;
- tmp = N2[0] * x + N2[1] *y+ N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
+ N2 = V * Nprim2;
+ tmp = N2[0] * x + N2[1] * y + N2[2];
+ tmp /= (distanceFictive * s);
+ norm2ok = (tmp > 0);
- N3 = V *Nprim3;
- tmp = N3[0] * x + N3[1]*y+ N3[2];
- tmp/= (distanceFictive*s);
- norm3ok = (tmp>0);
+ N3 = V * Nprim3;
+ tmp = N3[0] * x + N3[1] * y + N3[2];
+ tmp /= (distanceFictive * s);
+ norm3ok = (tmp > 0);
- N4 = V *Nprim4;
+ N4 = V * Nprim4;
tmp = N4[0] * x + N4[1] * y + N4[2];
- tmp/= (distanceFictive*s);
- norm4ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm4ok = (tmp > 0);
}
- if (cas == cas2)
- {
- N1 = V *Nprim1;
+ if (cas == cas2) {
+ N1 = V * Nprim1;
tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm1ok = (tmp > 0);
- N2 = V *Nprim2;
+ N2 = V * Nprim2;
tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm2ok = (tmp > 0);
}
- if (cas == cas3)
- {
- N1 = V *Nprim1;
+ if (cas == cas3) {
+ N1 = V * Nprim1;
tmp = N1[0] * x + N1[1] * y + N1[2];
- tmp/= (distanceFictive*s);
- norm1ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm1ok = (tmp > 0);
- N2 = V *Nprim2;
+ N2 = V * Nprim2;
tmp = N2[0] * x + N2[1] * y + N2[2];
- tmp/= (distanceFictive*s);
- norm2ok = (tmp>0);
+ tmp /= (distanceFictive * s);
+ norm2ok = (tmp > 0);
}
// on a donc maintenant les differents cas possibles
// on peut ensuite remonter aux deplacements
// on separe encore les cas 1,2,3
// la, deux choix se posent suivant le signe de d
- if (distanceFictive>0)
- {
- if (cas == cas1)
- {
- if (norm1ok)
- {
-
- //cos theta
- Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
+ if (distanceFictive > 0) {
+ if (cas == cas1) {
+ if (norm1ok) {
+
+ // cos theta
+ Rprim1[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
Rprim1[2][2] = Rprim1[0][0];
// sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
+ Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] + sv[2]) * sv[1]);
Rprim1[0][2] = -Rprim1[2][0];
- Rprim1[1][1] =1.0;
+ Rprim1[1][1] = 1.0;
Tprim1[0] = Nprim1[0];
Tprim1[1] = 0.0;
Tprim1[2] = -Nprim1[2];
- Tprim1*=(sv[0] - sv[2]);
-
+ Tprim1 *= (sv[0] - sv[2]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
- //cos theta
- Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
+ // cos theta
+ Rprim2[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
Rprim2[2][2] = Rprim2[0][0];
// sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
+ Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] + sv[2]) * sv[1]);
Rprim2[0][2] = -Rprim2[2][0];
- Rprim2[1][1] =1.0;
+ Rprim2[1][1] = 1.0;
Tprim2[0] = Nprim2[0];
Tprim2[1] = 0.0;
Tprim2[2] = -Nprim2[2];
- Tprim2*=(sv[0] - sv[2]);
-
+ Tprim2 *= (sv[0] - sv[2]);
}
- if (norm3ok)
- {
+ if (norm3ok) {
- //cos theta
- Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) /
- ((sv[0] + sv[2]) * sv[1]);
+ // cos theta
+ Rprim3[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
Rprim3[2][2] = Rprim3[0][0];
// sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
+ Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] + sv[2]) * sv[1]);
Rprim3[0][2] = -Rprim3[2][0];
- Rprim3[1][1] =1.0;
+ Rprim3[1][1] = 1.0;
Tprim3[0] = Nprim3[0];
Tprim3[1] = 0.0;
Tprim3[2] = -Nprim3[2];
- Tprim3*=(sv[0] - sv[2]);
-
+ Tprim3 *= (sv[0] - sv[2]);
}
- if (norm4ok)
- {
+ if (norm4ok) {
- //cos theta
- Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2])/
- ((sv[0] + sv[2]) * sv[1]);
+ // cos theta
+ Rprim4[0][0] = (vpMath::sqr(sv[1]) + sv[0] * sv[2]) / ((sv[0] + sv[2]) * sv[1]);
Rprim4[2][2] = Rprim4[0][0];
// sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] + sv[2]) * sv[1]);
+ Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] + sv[2]) * sv[1]);
Rprim4[0][2] = -Rprim4[2][0];
- Rprim4[1][1] =1.0;
+ Rprim4[1][1] = 1.0;
Tprim4[0] = Nprim4[0];
Tprim4[1] = 0.0;
Tprim4[2] = -Nprim4[2];
- Tprim4*=(sv[0] - sv[2]);
-
+ Tprim4 *= (sv[0] - sv[2]);
}
}
- if (cas == cas2)
- {
+ if (cas == cas2) {
// 2 normales sont potentiellement candidates
- if (norm1ok)
- {
+ if (norm1ok) {
Rprim1.eye();
Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] - sv[0]);
+ Tprim1 *= (sv[2] - sv[0]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
Rprim2.eye();
Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] - sv[0]);
+ Tprim2 *= (sv[2] - sv[0]);
}
}
- if (cas == cas3)
- {
- if (norm1ok)
- {
+ if (cas == cas3) {
+ if (norm1ok) {
Rprim1.eye();
Tprim1 = Nprim1[0];
- Tprim1*= (sv[0] - sv[1]);
+ Tprim1 *= (sv[0] - sv[1]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
Rprim2.eye();
Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] - sv[1]);
+ Tprim2 *= (sv[0] - sv[1]);
}
}
- if (cas == cas4)
- {
+ if (cas == cas4) {
// on ne connait pas la normale dans ce cas la
Rprim1.eye();
Tprim1 = 0.0;
}
}
- if (distanceFictive <0)
- {
+ if (distanceFictive < 0) {
- if (cas == cas1)
- {
- if (norm1ok)
- {
+ if (cas == cas1) {
+ if (norm1ok) {
-
- //cos theta
- Rprim1[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
+ // cos theta
+ Rprim1[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
Rprim1[2][2] = -Rprim1[0][0];
// sin theta
- Rprim1[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
+ Rprim1[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];
@@ -1102,49 +1096,41 @@ void vpHomography::computeDisplacement(const vpHomography &H,
Tprim1[1] = 0.0;
Tprim1[2] = Nprim1[2];
- Tprim1*=(sv[0] + sv[2]);
-
+ Tprim1 *= (sv[0] + sv[2]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
- //cos theta
- Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
+ // cos theta
+ Rprim2[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
Rprim2[2][2] = -Rprim2[0][0];
// sin theta
- Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
+ Rprim2[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] - sv[2]) * sv[1]);
Rprim2[0][2] = Rprim2[2][0];
- Rprim2[1][1] = - 1.0;
+ Rprim2[1][1] = -1.0;
Tprim2[0] = Nprim2[0];
Tprim2[1] = 0.0;
Tprim2[2] = Nprim2[2];
- Tprim2*=(sv[0] + sv[2]);
-
+ Tprim2 *= (sv[0] + sv[2]);
}
- if (norm3ok)
- {
+ if (norm3ok) {
- //cos theta
- Rprim3[0][0] = ( sv[0] * sv[2] - vpMath::sqr(sv[1])) /
- ((sv[0] - sv[2]) * sv[1]);
+ // cos theta
+ Rprim3[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
Rprim3[2][2] = -Rprim3[0][0];
// sin theta
- Rprim3[2][0] = -(sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
+ Rprim3[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];
@@ -1154,77 +1140,68 @@ void vpHomography::computeDisplacement(const vpHomography &H,
Tprim3[1] = 0.0;
Tprim3[2] = Nprim3[2];
- Tprim3*=(sv[0] + sv[2]);
-
+ Tprim3 *= (sv[0] + sv[2]);
}
- if (norm4ok)
- {
- //cos theta
- Rprim4[0][0] = ( sv[0] * sv[2]-vpMath::sqr(sv[1]))/((sv[0] - sv[2]) * sv[1]);
+ if (norm4ok) {
+ // cos theta
+ Rprim4[0][0] = (sv[0] * sv[2] - vpMath::sqr(sv[1])) / ((sv[0] - sv[2]) * sv[1]);
Rprim4[2][2] = -Rprim4[0][0];
// sin theta
- Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) *
- (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2]))))
- / ((sv[0] - sv[2]) * sv[1]);
+ Rprim4[2][0] = (sqrt((vpMath::sqr(sv[0]) - vpMath::sqr(sv[1])) * (vpMath::sqr(sv[1]) - vpMath::sqr(sv[2])))) /
+ ((sv[0] - sv[2]) * sv[1]);
Rprim4[0][2] = Rprim4[2][0];
- Rprim4[1][1] = - 1.0;
+ Rprim4[1][1] = -1.0;
Tprim4[0] = Nprim4[0];
Tprim4[1] = 0.0;
Tprim4[2] = Nprim4[2];
- Tprim4*=(sv[0] + sv[2]);
+ Tprim4 *= (sv[0] + sv[2]);
}
}
- if (cas == cas2)
- {
+ if (cas == cas2) {
// 2 normales sont potentiellement candidates
- if (norm1ok)
- {
+ if (norm1ok) {
Rprim1.eye();
Rprim1[0][0] = -1;
Rprim1[1][1] = -1;
Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
+ Tprim1 *= (sv[2] + sv[0]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
Rprim2.eye();
Rprim2[0][0] = -1;
Rprim2[1][1] = -1;
Tprim2 = Nprim2[1];
- Tprim2*= (sv[2] + sv[0]);
+ Tprim2 *= (sv[2] + sv[0]);
}
}
- if (cas == cas3)
- {
- if (norm1ok)
- {
+ if (cas == cas3) {
+ if (norm1ok) {
Rprim1.eye();
Rprim1[2][2] = -1;
Rprim1[1][1] = -1;
Tprim1 = Nprim1[0];
- Tprim1*= (sv[2] + sv[0]);
+ Tprim1 *= (sv[2] + sv[0]);
}
- if (norm2ok)
- {
+ if (norm2ok) {
Rprim2.eye();
Rprim2[2][2] = -1;
Rprim2[1][1] = -1;
Tprim2 = Nprim2[1];
- Tprim2*= (sv[0] + sv[2]);
+ Tprim2 *= (sv[0] + sv[2]);
}
}
@@ -1234,78 +1211,73 @@ void vpHomography::computeDisplacement(const vpHomography &H,
// on peut maintenant recuperer la
// rotation, et la translation
// IL Y A JUSTE LE CAS D<0 ET CAS 4 QU'ON NE TRAITE PAS
- if ((distanceFictive>0) || (cas != cas4))
- {
+ if ((distanceFictive > 0) || (cas != cas4)) {
// on controle juste si les normales sont ok
- if (norm1ok)
- {
+ if (norm1ok) {
R1 = s * U * Rprim1 * V.t();
T1 = U * Tprim1;
- T1 /= (distanceFictive *s);
- N1 = V *Nprim1;
+ T1 /= (distanceFictive * s);
+ N1 = V * Nprim1;
// je rajoute le resultat
vR.push_back(R1);
vT.push_back(T1);
vN.push_back(N1);
}
- if (norm2ok)
- {
+ if (norm2ok) {
R2 = s * U * Rprim2 * V.t();
T2 = U * Tprim2;
- T2 /= (distanceFictive *s);
- N2 = V *Nprim2;
+ T2 /= (distanceFictive * s);
+ N2 = V * Nprim2;
// je rajoute le resultat
vR.push_back(R2);
vT.push_back(T2);
vN.push_back(N2);
}
- if (norm3ok)
- {
+ if (norm3ok) {
R3 = s * U * Rprim3 * V.t();
T3 = U * Tprim3;
- T3 /= (distanceFictive *s);
- N3 = V *Nprim3;
+ T3 /= (distanceFictive * s);
+ N3 = V * Nprim3;
// je rajoute le resultat
vR.push_back(R3);
vT.push_back(T3);
vN.push_back(N3);
}
- if (norm4ok)
- {
+ if (norm4ok) {
R4 = s * U * Rprim4 * V.t();
T4 = U * Tprim4;
- T4 /= (distanceFictive *s);
- N4 = V *Nprim4;
+ T4 /= (distanceFictive * s);
+ N4 = V * Nprim4;
// je rajoute le resultat
vR.push_back(R4);
vT.push_back(T4);
vN.push_back(N4);
}
- }
- else
- {
- std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas estimable!" << std::endl;
+ } else {
+ std::cout << "On tombe dans le cas particulier ou le mouvement n'est pas "
+ "estimable!"
+ << std::endl;
}
- // on peut ensuite afficher les resultats...
- /* std::cout << "Analyse des resultats : "<< std::endl; */
- /* if (cas==cas1) */
- /* std::cout << "On est dans le cas 1" << std::endl; */
- /* if (cas==cas2) */
- /* std::cout << "On est dans le cas 2" << std::endl; */
- /* if (cas==cas3) */
- /* std::cout << "On est dans le cas 3" << std::endl; */
- /* if (cas==cas4) */
- /* std::cout << "On est dans le cas 4" << std::endl; */
-
- /* if (distanceFictive < 0) */
- /* std::cout << "d'<0" << std::endl; */
- /* else */
- /* std::cout << "d'>0" << std::endl; */
+// on peut ensuite afficher les resultats...
+/* std::cout << "Analyse des resultats : "<< std::endl; */
+/* if (cas==cas1) */
+/* std::cout << "On est dans le cas 1" << std::endl; */
+/* if (cas==cas2) */
+/* std::cout << "On est dans le cas 2" << std::endl; */
+/* if (cas==cas3) */
+/* std::cout << "On est dans le cas 3" << std::endl; */
+/* if (cas==cas4) */
+/* std::cout << "On est dans le cas 4" << std::endl; */
+
+/* if (distanceFictive < 0) */
+/* std::cout << "d'<0" << std::endl; */
+/* else */
+/* std::cout << "d'>0" << std::endl; */
#ifdef DEBUG_Homographie
printf("fin : Homographie_EstimationDeplacementCamera\n");
diff --git a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp
index be66d5a..4ce4e76 100644
--- a/modules/vision/src/homography-estimation/vpHomographyMalis.cpp
+++ b/modules/vision/src/homography-estimation/vpHomographyMalis.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
/*!
\file vpHomographyMalis.cpp
@@ -48,15 +47,15 @@
*/
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMatrixException.h>
+#include <visp3/vision/vpHomography.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-const double eps = 1e-6 ;
+const double eps = 1e-6;
/**************************************************************************
* NOM :
@@ -87,88 +86,69 @@ const double eps = 1e-6 ;
****************************************************************************
*/
-void changeFrame(unsigned int *pts_ref,
- unsigned int nb_pts,
- vpMatrix &pd, vpMatrix &p,
- vpMatrix &pnd, vpMatrix &pn,
+void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn,
vpMatrix &M, vpMatrix &Mdp);
-void HLM2D(unsigned int nb_pts,
- vpMatrix &points_des,
- vpMatrix &points_cour,
- vpMatrix &H);
-void HLM3D(unsigned int nb_pts,
- vpMatrix &pd,
- vpMatrix &p,
- vpMatrix &H);
-void HLM(unsigned int q_cible,
- unsigned int nbpt,
- double *xm, double *ym,
- double *xmi, double *ymi,
- vpMatrix &H);
-
-void HLM(unsigned int q_cible,
- const std::vector<double> &xm, const std::vector<double> &ym,
- const std::vector<double> &xmi, const std::vector<double> &ymi,
- vpMatrix &H);
-
-void changeFrame(unsigned int *pts_ref,
- unsigned int nb_pts,
- vpMatrix &pd, vpMatrix &p,
- vpMatrix &pnd, vpMatrix &pn,
+void HLM2D(unsigned int nb_pts, vpMatrix &points_des, vpMatrix &points_cour, vpMatrix &H);
+void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H);
+void HLM(unsigned int q_cible, unsigned int nbpt, double *xm, double *ym, double *xmi, double *ymi, vpMatrix &H);
+
+void HLM(unsigned int q_cible, const std::vector<double> &xm, const std::vector<double> &ym,
+ const std::vector<double> &xmi, const std::vector<double> &ymi, vpMatrix &H);
+
+void changeFrame(unsigned int *pts_ref, unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &pnd, vpMatrix &pn,
vpMatrix &M, vpMatrix &Mdp)
{
- unsigned int i,j, k ;
- unsigned int cont_pts; /* */
+ unsigned int i, j, k;
+ unsigned int cont_pts; /* */
/* Construction des matrices de changement de repere */
- vpMatrix Md(3,3) ;
- vpMatrix Mp(3,3) ;
+ vpMatrix Md(3, 3);
+ vpMatrix Mp(3, 3);
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- M[j][i] = p[pts_ref[i]][j] ;
- Md[j][i] = pd[pts_ref[i]][j] ;
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ M[j][i] = p[pts_ref[i]][j];
+ Md[j][i] = pd[pts_ref[i]][j];
}
}
/*calcul de la pseudo inverse */
- Mp= M.pseudoInverse(1e-16) ;
- Mdp = Md.pseudoInverse(1e-16) ;
+ Mp = M.pseudoInverse(1e-16);
+ Mdp = Md.pseudoInverse(1e-16);
if (pts_ref[3] > 0) {
double lamb_des[3];
double lamb_cour[3];
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- lamb_cour[i] = Mp[i][j]*p[pts_ref[3]][j] ;
- lamb_des[i] = Mdp[i][j]*pd[pts_ref[3]][j] ;
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ lamb_cour[i] = Mp[i][j] * p[pts_ref[3]][j];
+ lamb_des[i] = Mdp[i][j] * pd[pts_ref[3]][j];
}
}
- for (i=0;i<3;i++) {
- for (j=0;j<3;j++) {
- M[i][j] = M[i][j]*lamb_cour[j] ;
- Md[i][j] = Md[i][j]*lamb_des[j] ;
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ M[i][j] = M[i][j] * lamb_cour[j];
+ Md[i][j] = Md[i][j] * lamb_des[j];
}
}
Mdp = Md.pseudoInverse(1e-16);
}
-
/* changement de repere pour tous les points autres que
les trois points de reference */
- cont_pts = 0 ;
- for (k=0;k<nb_pts;k++) {
+ cont_pts = 0;
+ for (k = 0; k < nb_pts; k++) {
if ((pts_ref[0] != k) && (pts_ref[1] != k) && (pts_ref[2] != k)) {
- for (i=0;i<3;i++) {
- pn[cont_pts][i] = 0.0 ;
- pnd[cont_pts][i] = 0.0 ;
- for (j=0;j<3;j++) {
- pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j]*p[k][j] ;
- pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j]*pd[k][j] ;
+ for (i = 0; i < 3; i++) {
+ pn[cont_pts][i] = 0.0;
+ pnd[cont_pts][i] = 0.0;
+ for (j = 0; j < 3; j++) {
+ pn[cont_pts][i] = pn[cont_pts][i] + Mp[i][j] * p[k][j];
+ pnd[cont_pts][i] = pnd[cont_pts][i] + Mdp[i][j] * pd[k][j];
}
}
cont_pts = cont_pts + 1;
@@ -176,15 +156,14 @@ void changeFrame(unsigned int *pts_ref,
}
}
-
/**************************************************************************
* NOM :
* Homographie_CrvMafEstHomoPointsCible2D
*
* DESCRIPTION :
- * Calcul de l'homographie entre une image courante et une image desiree dans le
- * cas particulier d'une cible planaire de points (cible pi).
- * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
+ * Calcul de l'homographie entre une image courante et une image desiree dans
+ *le cas particulier d'une cible planaire de points (cible pi). Cette
+ *procedure est appellee par "Homographie_CrvMafCalculHomographie".
*
****************************************************************************
* ENTREES :
@@ -204,110 +183,101 @@ void changeFrame(unsigned int *pts_ref,
* 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 ;
+ unsigned int i, j;
- double vals_inf ;
- unsigned int contZeros, vect;
+ double vals_inf;
+ unsigned int contZeros, vect;
/** allocation des matrices utilisees uniquement dans la procedure **/
- vpMatrix M(3*nb_pts,9) ;
- vpMatrix V(9,9) ;
- vpColVector sv(9) ;
+ vpMatrix M(3 * nb_pts, 9);
+ vpMatrix V(9, 9);
+ vpColVector sv(9);
/** construction de la matrice M des coefficients dans le cas general **/
- for (j=0; j<nb_pts ;j++) {
- M[3*j][0] = 0 ;
- M[3*j][1] = 0 ;
- M[3*j][2] = 0 ;
- M[3*j][3] = -points_des[j][0]*points_cour[j][2] ;
- M[3*j][4] = -points_des[j][1]*points_cour[j][2] ;
- M[3*j][5] = -points_des[j][2]*points_cour[j][2] ;
- M[3*j][6] = points_des[j][0]*points_cour[j][1] ;
- M[3*j][7] = points_des[j][1]*points_cour[j][1] ;
- M[3*j][8] = points_des[j][2]*points_cour[j][1] ;
-
- M[3*j+1][0] = points_des[j][0]*points_cour[j][2] ;
- M[3*j+1][1] = points_des[j][1]*points_cour[j][2] ;
- M[3*j+1][2] = points_des[j][2]*points_cour[j][2] ;
- M[3*j+1][3] = 0 ;
- M[3*j+1][4] = 0 ;
- M[3*j+1][5] = 0 ;
- M[3*j+1][6] = -points_des[j][0]*points_cour[j][0] ;
- M[3*j+1][7] = -points_des[j][1]*points_cour[j][0] ;
- M[3*j+1][8] = -points_des[j][2]*points_cour[j][0] ;
-
- M[3*j+2][0] = -points_des[j][0]*points_cour[j][1] ;
- M[3*j+2][1] = -points_des[j][1]*points_cour[j][1] ;
- M[3*j+2][2] = -points_des[j][2]*points_cour[j][1] ;
- M[3*j+2][3] = points_des[j][0]*points_cour[j][0] ;
- M[3*j+2][4] = points_des[j][1]*points_cour[j][0] ;
- M[3*j+2][5] = points_des[j][2]*points_cour[j][0] ;
- M[3*j+2][6] = 0 ;
- M[3*j+2][7] = 0 ;
- M[3*j+2][8] = 0 ;
+ for (j = 0; j < nb_pts; j++) {
+ M[3 * j][0] = 0;
+ M[3 * j][1] = 0;
+ M[3 * j][2] = 0;
+ M[3 * j][3] = -points_des[j][0] * points_cour[j][2];
+ M[3 * j][4] = -points_des[j][1] * points_cour[j][2];
+ M[3 * j][5] = -points_des[j][2] * points_cour[j][2];
+ M[3 * j][6] = points_des[j][0] * points_cour[j][1];
+ M[3 * j][7] = points_des[j][1] * points_cour[j][1];
+ M[3 * j][8] = points_des[j][2] * points_cour[j][1];
+
+ M[3 * j + 1][0] = points_des[j][0] * points_cour[j][2];
+ M[3 * j + 1][1] = points_des[j][1] * points_cour[j][2];
+ M[3 * j + 1][2] = points_des[j][2] * points_cour[j][2];
+ M[3 * j + 1][3] = 0;
+ M[3 * j + 1][4] = 0;
+ M[3 * j + 1][5] = 0;
+ M[3 * j + 1][6] = -points_des[j][0] * points_cour[j][0];
+ M[3 * j + 1][7] = -points_des[j][1] * points_cour[j][0];
+ M[3 * j + 1][8] = -points_des[j][2] * points_cour[j][0];
+
+ M[3 * j + 2][0] = -points_des[j][0] * points_cour[j][1];
+ M[3 * j + 2][1] = -points_des[j][1] * points_cour[j][1];
+ M[3 * j + 2][2] = -points_des[j][2] * points_cour[j][1];
+ M[3 * j + 2][3] = points_des[j][0] * points_cour[j][0];
+ M[3 * j + 2][4] = points_des[j][1] * points_cour[j][0];
+ M[3 * j + 2][5] = points_des[j][2] * points_cour[j][0];
+ M[3 * j + 2][6] = 0;
+ M[3 * j + 2][7] = 0;
+ M[3 * j + 2][8] = 0;
}
/** calcul de la pseudo-inverse V de M et des valeurs singulieres **/
- M.svd(sv,V);
+ M.svd(sv, V);
/*****
- La meilleure solution est le vecteur de V associe
- a la valeur singuliere la plus petite en valeur absolu.
- Pour cela on parcourt la matrice des valeurs singulieres
- et on repere la plus petite valeur singuliere, on en profite
- pour effectuer un controle sur le rang de la matrice : pas plus
- de 2 valeurs singulieres quasi=0
+ La meilleure solution est le vecteur de V associe
+ a la valeur singuliere la plus petite en valeur absolu.
+ Pour cela on parcourt la matrice des valeurs singulieres
+ et on repere la plus petite valeur singuliere, on en profite
+ pour effectuer un controle sur le rang de la matrice : pas plus
+ de 2 valeurs singulieres quasi=0
*****/
- vals_inf = fabs(sv[0]) ;
- vect = 0 ;
+ vals_inf = fabs(sv[0]);
+ vect = 0;
contZeros = 0;
if (fabs(sv[0]) < eps) {
- contZeros = contZeros + 1 ;
+ contZeros = contZeros + 1;
}
- for (j=1; j<9; j++) {
+ for (j = 1; j < 9; j++) {
if (fabs(sv[j]) < vals_inf) {
vals_inf = fabs(sv[j]);
- vect = j ;
+ vect = j;
}
if (fabs(sv[j]) < eps) {
- contZeros = contZeros + 1 ;
+ contZeros = contZeros + 1;
}
}
-
/** cas d'erreur : plus de 2 valeurs singulieres =0 **/
if (contZeros > 2) {
- //vpERROR_TRACE("matrix is rank deficient");
- throw (vpMatrixException(vpMatrixException::matrixError,
- "matrix is rank deficient"));
+ // vpERROR_TRACE("matrix is rank deficient");
+ throw(vpMatrixException(vpMatrixException::matrixError, "matrix is rank deficient"));
}
- H.resize(3,3) ;
+ H.resize(3, 3);
/** construction de la matrice H **/
- for (i=0; i<3; i++) {
- for (j=0; j<3; j++){
- H[i][j] = V[3*i+j][vect];
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < 3; j++) {
+ H[i][j] = V[3 * i + j][vect];
}
}
-
-
}
-
/**************************************************************************
* NOM :
* Homographie_CrvMafEstHomoPointsC3DEzio
*
* DESCRIPTION :
- * Calcul de l'homographie entre une image courante et une image desiree dans le
- * cas particulier d'une cible non planaire de points (cible pi).
- * Cette procedure est appellee par "Homographie_CrvMafCalculHomographie".
+ * Calcul de l'homographie entre une image courante et une image desiree dans
+ *le cas particulier d'une cible non planaire de points (cible pi). Cette
+ *procedure est appellee par "Homographie_CrvMafCalculHomographie".
*
*
****************************************************************************
@@ -323,82 +293,75 @@ HLM2D(unsigned int nb_pts,
*
****************************************************************************
**/
-void
-HLM3D(unsigned int nb_pts,
- vpMatrix &pd,
- vpMatrix &p,
- vpMatrix &H)
+void HLM3D(unsigned int nb_pts, vpMatrix &pd, vpMatrix &p, vpMatrix &H)
{
- unsigned int i,j,k,ii,jj ;
- unsigned int cont_pts; /* Pour compter le nombre de points dans l'image */
- //unsigned int nl; /*** Nombre de lignes ***/
- unsigned int nc ; /*** Nombre de colonnes ***/
- unsigned int pts_ref[4]; /*** definit lesquels des points de
- l'image sont les points de reference***/
+ unsigned int i, j, k, ii, jj;
+ unsigned int cont_pts; /* Pour compter le nombre de points dans l'image */
+ // unsigned int nl; /*** Nombre de lignes ***/
+ unsigned int nc; /*** Nombre de colonnes ***/
+ unsigned int pts_ref[4]; /*** definit lesquels des points de
+ l'image sont les points de reference***/
/*** ***/
- unsigned int perm; /*** Compte le nombre de permutations, quand le nombre
- de permutations =0 arret de l'ordonnancement **/
- int cont_zeros; /*** pour compter les valeurs quasi= a zero ***/
- unsigned int cont;
- unsigned int vect;
+ unsigned int perm; /*** Compte le nombre de permutations, quand le nombre
+ de permutations =0 arret de l'ordonnancement **/
+ int cont_zeros; /*** pour compter les valeurs quasi= a zero ***/
+ unsigned int cont;
+ unsigned int vect;
- //int prob;
+ // int prob;
/***** Corps de la fonction *****/
/* allocation des matrices utilisees uniquement dans la procedure */
- //prob=0;
-
- vpMatrix M(3,3) ;
- vpMatrix Mdp(3,3) ;
- vpMatrix c(8,2) ; // matrice des coeff C
+ // prob=0;
- vpColVector d(8) ;
- vpMatrix cp(2,8) ; //matrice pseudo-inverse de C
+ vpMatrix M(3, 3);
+ vpMatrix Mdp(3, 3);
+ vpMatrix c(8, 2); // matrice des coeff C
+ vpColVector d(8);
+ vpMatrix cp(2, 8); // matrice pseudo-inverse de C
- vpMatrix H_int(3,3) ;
- vpMatrix pn((nb_pts-3),3) ; //points courant nouveau repere
+ vpMatrix H_int(3, 3);
+ vpMatrix pn((nb_pts - 3), 3); // points courant nouveau repere
-
- vpMatrix pnd((nb_pts-3),3) ; //points derives nouveau repere
+ vpMatrix pnd((nb_pts - 3), 3); // points derives nouveau repere
/* preparation du changement de repere */
/****
comme plan de reference on choisit pour le moment
arbitrairement le plan contenant les points 1,2,3 du cinq
****/
- pts_ref[0] = 0 ;
- pts_ref[1] = 1 ;
- pts_ref[2] = 2 ;
- pts_ref[3] = 0 ;
-
- /* changement de repere pour tous les points autres que les trois points de reference */
+ pts_ref[0] = 0;
+ pts_ref[1] = 1;
+ pts_ref[2] = 2;
+ pts_ref[3] = 0;
- changeFrame(pts_ref,nb_pts,pd,p,pnd,pn,M,Mdp);
+ /* changement de repere pour tous les points autres que les trois points de
+ * reference */
+ changeFrame(pts_ref, nb_pts, pd, p, pnd, pn, M, Mdp);
- cont_pts = nb_pts - 3 ;
+ cont_pts = nb_pts - 3;
- if (cont_pts < 5)
- {
- //vpERROR_TRACE(" not enough point to compute the homography ... ");
- throw (vpMatrixException(vpMatrixException::matrixError,
- "Not enough point to compute the homography"));
+ if (cont_pts < 5) {
+ // vpERROR_TRACE(" not enough point to compute the homography ... ");
+ throw(vpMatrixException(vpMatrixException::matrixError, "Not enough point to compute the homography"));
}
- //nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ;
- nc = 7 ;
+ // nl = cont_pts*(cont_pts-1)*(cont_pts-2)/6 ;
+ nc = 7;
/* Allocation matrice CtC */
- vpMatrix CtC(nc,nc) ;
+ vpMatrix CtC(nc, nc);
/* Initialisation matrice CtC */
- for (i=0;i<nc;i++) for (j=0;j<nc;j++) CtC[i][j] = 0.0;
-
+ for (i = 0; i < nc; i++)
+ for (j = 0; j < nc; j++)
+ CtC[i][j] = 0.0;
/* Allocation matrice M */
- vpColVector C(nc) ; //Matrice des coefficients
+ vpColVector C(nc); // Matrice des coefficients
/* construction de la matrice M des coefficients dans le cas general */
/****
@@ -410,151 +373,151 @@ HLM3D(unsigned int nb_pts,
v3 = x2*x4 ; v4 = x2*x6 ;
v5 = x3*x5 ; v6 = x3*x4 ;
****/
- cont = 0 ;
- for (i=0 ; i<nb_pts-5; i++) {
- for (j = i+1 ; j<nb_pts-4; j++) {
- for (k = j+1 ; k<nb_pts-3; k ++) {
- /* coeff a^2*b */
- C[0] = pn[i][2]*pn[j][2]*pn[k][1]*pnd[k][0] //
- * (pnd[j][0]*pnd[i][1] - pnd[j][1]*pnd[i][0])//
- + pn[i][2]*pn[k][2]*pn[j][1]*pnd[j][0]//
- *(pnd[i][0]*pnd[k][1] - pnd[i][1]*pnd[k][0])//
- + pn[j][2]*pn[k][2]*pn[i][1]*pnd[i][0] //
- *(pnd[k][0]*pnd[j][1] - pnd[k][1]*pnd[j][0]) ; //
- /* coeff a*b^2 */
- C[1] = pn[i][2]*pn[j][2]*pn[k][0]*pnd[k][1]//
- *(pnd[i][0]*pnd[j][1] - pnd[i][1]*pnd[j][0])//
- + pn[i][2]*pn[k][2]*pn[j][0]*pnd[j][1]//
- *(pnd[k][0]*pnd[i][1] - pnd[k][1]*pnd[i][0])//
- + pn[j][2]*pn[k][2]*pn[i][0]*pnd[i][1]//
- *(pnd[j][0]*pnd[k][1] - pnd[j][1]*pnd[k][0]) ;//
- /* coeff a^2 */
- C[2] = + pn[i][1]*pn[k][1]*pn[j][2]*pnd[j][0]//
- *(pnd[k][2]*pnd[i][0] - pnd[k][0]*pnd[i][2])//
- +pn[i][1]*pn[j][1]*pn[k][2]*pnd[k][0] //
- *(pnd[i][2]*pnd[j][0] - pnd[i][0]*pnd[j][2])
- + pn[j][1]*pn[k][1]*pn[i][2]*pnd[i][0] //
- *(pnd[j][2]*pnd[k][0] - pnd[j][0]*pnd[k][2]) ; //
-
-
-
- /* coeff b^2 */
- C[3] = pn[i][0]*pn[j][0]*pn[k][2]*pnd[k][1] //
- *(pnd[i][2]*pnd[j][1] - pnd[i][1]*pnd[j][2]) //
- + pn[i][0]*pn[k][0]*pn[j][2]*pnd[j][1] //
- *(pnd[k][2]*pnd[i][1] - pnd[k][1]*pnd[i][2]) //
- + pn[j][0]*pn[k][0]*pn[i][2]*pnd[i][1] //
- *(pnd[j][2]*pnd[k][1] - pnd[j][1]*pnd[k][2]) ; //
-
- /* coeff a */
- C[5] = pn[i][1]*pn[j][1]*pn[k][0]*pnd[k][2]//
- *(pnd[i][0]*pnd[j][2] - pnd[i][2]*pnd[j][0])//
- + pn[i][1]*pn[k][1]*pn[j][0]*pnd[j][2] //
- *(pnd[k][0]*pnd[i][2] - pnd[k][2]*pnd[i][0])//
- + pn[j][1]*pn[k][1]*pn[i][0]*pnd[i][2]//
- *(pnd[j][0]*pnd[k][2] - pnd[j][2]*pnd[k][0]) ;//
- /* coeff b */
- C[6] = pn[i][0]*pn[j][0]*pn[k][1]*pnd[k][2] //
- *(pnd[i][1]*pnd[j][2] - pnd[i][2]*pnd[j][1])//
- + pn[i][0]*pn[k][0]*pn[j][1]*pnd[j][2]//
- *(pnd[k][1]*pnd[i][2] - pnd[k][2]*pnd[i][1])//
- + pn[j][0]*pn[k][0]*pn[i][1]*pnd[i][2]//
- *(pnd[j][1]*pnd[k][2] - pnd[j][2]*pnd[k][1]) ;//
- /* coeff a*b */
- C[4] = pn[i][0]*pn[k][1]*pn[j][2] //
- *(pnd[k][0]*pnd[j][1]*pnd[i][2] - pnd[j][0]*pnd[i][1]*pnd[k][2])//
- + pn[k][0]*pn[i][1]*pn[j][2]//
- *(pnd[j][0]*pnd[k][1]*pnd[i][2] - pnd[i][0]*pnd[j][1]*pnd[k][2])//
- + pn[i][0]*pn[j][1]*pn[k][2]//
- *(pnd[k][0]*pnd[i][1]*pnd[j][2] - pnd[j][0]*pnd[k][1]*pnd[i][2])//
- + pn[j][0]*pn[i][1]*pn[k][2]//
- *(pnd[i][0]*pnd[k][1]*pnd[j][2] - pnd[k][0]*pnd[j][1]*pnd[i][2])//
- + pn[k][0]*pn[j][1]*pn[i][2]//
- *(pnd[j][0]*pnd[i][1]*pnd[k][2] - pnd[i][0]*pnd[k][1]*pnd[j][2])//
- + pn[j][0]*pn[k][1]*pn[i][2]//
- *(pnd[i][0]*pnd[j][1]*pnd[k][2] - pnd[k][0]*pnd[i][1]*pnd[j][2]) ;//
-
- cont = cont+1 ;
- /* construction de la matrice CtC */
- for (ii=0;ii<nc;ii++) {
- for (jj=ii;jj<nc;jj++) {
- CtC[ii][jj] = CtC[ii][jj] + C[ii]*C[jj];
- }
- }
-
+ cont = 0;
+ for (i = 0; i < nb_pts - 5; i++) {
+ for (j = i + 1; j < nb_pts - 4; j++) {
+ for (k = j + 1; k < nb_pts - 3; k++) {
+ /* coeff a^2*b */
+ C[0] = pn[i][2] * pn[j][2] * pn[k][1] * pnd[k][0] //
+ * (pnd[j][0] * pnd[i][1] - pnd[j][1] * pnd[i][0]) //
+ + pn[i][2] * pn[k][2] * pn[j][1] * pnd[j][0] //
+ * (pnd[i][0] * pnd[k][1] - pnd[i][1] * pnd[k][0]) //
+ + pn[j][2] * pn[k][2] * pn[i][1] * pnd[i][0] //
+ * (pnd[k][0] * pnd[j][1] - pnd[k][1] * pnd[j][0]); //
+ /* coeff a*b^2 */
+ C[1] = pn[i][2] * pn[j][2] * pn[k][0] * pnd[k][1] //
+ * (pnd[i][0] * pnd[j][1] - pnd[i][1] * pnd[j][0]) //
+ + pn[i][2] * pn[k][2] * pn[j][0] * pnd[j][1] //
+ * (pnd[k][0] * pnd[i][1] - pnd[k][1] * pnd[i][0]) //
+ + pn[j][2] * pn[k][2] * pn[i][0] * pnd[i][1] //
+ * (pnd[j][0] * pnd[k][1] - pnd[j][1] * pnd[k][0]); //
+ /* coeff a^2 */
+ C[2] = +pn[i][1] * pn[k][1] * pn[j][2] * pnd[j][0] //
+ * (pnd[k][2] * pnd[i][0] - pnd[k][0] * pnd[i][2]) //
+ + pn[i][1] * pn[j][1] * pn[k][2] * pnd[k][0] //
+ * (pnd[i][2] * pnd[j][0] - pnd[i][0] * pnd[j][2]) +
+ pn[j][1] * pn[k][1] * pn[i][2] * pnd[i][0] //
+ * (pnd[j][2] * pnd[k][0] - pnd[j][0] * pnd[k][2]); //
+
+ /* coeff b^2 */
+ C[3] = pn[i][0] * pn[j][0] * pn[k][2] * pnd[k][1] //
+ * (pnd[i][2] * pnd[j][1] - pnd[i][1] * pnd[j][2]) //
+ + pn[i][0] * pn[k][0] * pn[j][2] * pnd[j][1] //
+ * (pnd[k][2] * pnd[i][1] - pnd[k][1] * pnd[i][2]) //
+ + pn[j][0] * pn[k][0] * pn[i][2] * pnd[i][1] //
+ * (pnd[j][2] * pnd[k][1] - pnd[j][1] * pnd[k][2]); //
+
+ /* coeff a */
+ C[5] = pn[i][1] * pn[j][1] * pn[k][0] * pnd[k][2] //
+ * (pnd[i][0] * pnd[j][2] - pnd[i][2] * pnd[j][0]) //
+ + pn[i][1] * pn[k][1] * pn[j][0] * pnd[j][2] //
+ * (pnd[k][0] * pnd[i][2] - pnd[k][2] * pnd[i][0]) //
+ + pn[j][1] * pn[k][1] * pn[i][0] * pnd[i][2] //
+ * (pnd[j][0] * pnd[k][2] - pnd[j][2] * pnd[k][0]); //
+ /* coeff b */
+ C[6] = pn[i][0] * pn[j][0] * pn[k][1] * pnd[k][2] //
+ * (pnd[i][1] * pnd[j][2] - pnd[i][2] * pnd[j][1]) //
+ + pn[i][0] * pn[k][0] * pn[j][1] * pnd[j][2] //
+ * (pnd[k][1] * pnd[i][2] - pnd[k][2] * pnd[i][1]) //
+ + pn[j][0] * pn[k][0] * pn[i][1] * pnd[i][2] //
+ * (pnd[j][1] * pnd[k][2] - pnd[j][2] * pnd[k][1]); //
+ /* coeff a*b */
+ C[4] = pn[i][0] * pn[k][1] * pn[j][2] //
+ * (pnd[k][0] * pnd[j][1] * pnd[i][2] - pnd[j][0] * pnd[i][1] * pnd[k][2]) //
+ + pn[k][0] * pn[i][1] * pn[j][2] //
+ * (pnd[j][0] * pnd[k][1] * pnd[i][2] - pnd[i][0] * pnd[j][1] * pnd[k][2]) //
+ + pn[i][0] * pn[j][1] * pn[k][2] //
+ * (pnd[k][0] * pnd[i][1] * pnd[j][2] - pnd[j][0] * pnd[k][1] * pnd[i][2]) //
+ + pn[j][0] * pn[i][1] * pn[k][2] //
+ * (pnd[i][0] * pnd[k][1] * pnd[j][2] - pnd[k][0] * pnd[j][1] * pnd[i][2]) //
+ + pn[k][0] * pn[j][1] * pn[i][2] //
+ * (pnd[j][0] * pnd[i][1] * pnd[k][2] - pnd[i][0] * pnd[k][1] * pnd[j][2]) //
+ + pn[j][0] * pn[k][1] * pn[i][2] //
+ * (pnd[i][0] * pnd[j][1] * pnd[k][2] - pnd[k][0] * pnd[i][1] * pnd[j][2]); //
+
+ cont = cont + 1;
+ /* construction de la matrice CtC */
+ for (ii = 0; ii < nc; ii++) {
+ for (jj = ii; jj < nc; jj++) {
+ CtC[ii][jj] = CtC[ii][jj] + C[ii] * C[jj];
+ }
+ }
}
}
}
-
-
/* calcul de CtC */
- for (i=0; i<nc ;i++) {
- for (j=i+1; j<nc ;j++) CtC[j][i] = CtC[i][j];
+ for (i = 0; i < nc; i++) {
+ for (j = i + 1; j < nc; j++)
+ CtC[j][i] = CtC[i][j];
}
- //nl = cont ; /* nombre de lignes */
- nc = 7 ; /* nombre de colonnes */
+ // nl = cont ; /* nombre de lignes */
+ nc = 7; /* nombre de colonnes */
/* Creation de matrice CtC termine */
/* Allocation matrice V */
- vpMatrix V(nc,nc) ;
+ vpMatrix V(nc, nc);
/*****
- Preparation au calcul de la svd (pseudo-inverse)
- pour obtenir une solution il faut au moins 5 equations independantes
- donc il faut au moins la mise en correspondence de 3+5 points
+ Preparation au calcul de la svd (pseudo-inverse)
+ pour obtenir une solution il faut au moins 5 equations independantes
+ donc il faut au moins la mise en correspondence de 3+5 points
*****/
- vpColVector sv(nc) ; //Vecteur contenant les valeurs singulieres
+ vpColVector sv(nc); // Vecteur contenant les valeurs singulieres
- CtC.svd(sv,V) ;
+ CtC.svd(sv, V);
/*****
- Il faut un controle sur le rang de la matrice !!
- La meilleure solution est le vecteur de V associe
- a la valeur singuliere la plus petite en valeur
- absolu
+ Il faut un controle sur le rang de la matrice !!
+ La meilleure solution est le vecteur de V associe
+ a la valeur singuliere la plus petite en valeur
+ absolu
*****/
- vpColVector svSorted(nc) ; // sorted singular value
+ vpColVector svSorted(nc); // sorted singular value
// sorting the singular value
- for (i=0; i < nc ;i++) svSorted[i] = sv[i] ;
- perm = 1 ;
+ for (i = 0; i < nc; i++)
+ svSorted[i] = sv[i];
+ perm = 1;
double v_temp;
while (perm != 0) {
perm = 0;
- for (i=1; i < nc ;i++) {
- if (svSorted[i-1] > svSorted[i]) {
- v_temp = svSorted[i-1] ;
- svSorted[i-1] = svSorted[i] ;
- svSorted[i] = v_temp ;
+ for (i = 1; i < nc; i++) {
+ if (svSorted[i - 1] > svSorted[i]) {
+ v_temp = svSorted[i - 1];
+ svSorted[i - 1] = svSorted[i];
+ svSorted[i] = v_temp;
perm = perm + 1;
}
}
}
/*****
- Parcours de la matrice ordonnee des valeurs singulieres
- On note "cont_zeros" le nbre de valeurs quasi= a 0.
- On note "vect" le rang de la plus petite valeur singliere
- en valeur absolu
+ Parcours de la matrice ordonnee des valeurs singulieres
+ On note "cont_zeros" le nbre de valeurs quasi= a 0.
+ On note "vect" le rang de la plus petite valeur singliere
+ en valeur absolu
*****/
- vect = 0 ; cont_zeros = 0 ; cont = 0 ;
- for (j=0; j < nc; j++) {
- //if (fabs(sv[j]) == svSorted[cont]) vect = j ;
- if (std::fabs(sv[j]-svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j],svSorted[cont]))) vect = j ;
- if (std::fabs(sv[j]/svSorted[nc-1]) < eps) cont_zeros = cont_zeros + 1 ;
+ vect = 0;
+ cont_zeros = 0;
+ cont = 0;
+ for (j = 0; j < nc; j++) {
+ // if (fabs(sv[j]) == svSorted[cont]) vect = j ;
+ if (std::fabs(sv[j] - svSorted[cont]) <= std::fabs(vpMath::maximum(sv[j], svSorted[cont])))
+ vect = j;
+ if (std::fabs(sv[j] / svSorted[nc - 1]) < eps)
+ cont_zeros = cont_zeros + 1;
}
if (cont_zeros > 5) {
// printf("erreur dans le rang de la matrice: %d \r\n ",7-cont_zeros);
- HLM2D(nb_pts,pd,p,H);
- }
- else
- {
+ HLM2D(nb_pts, pd, p, H);
+ } else {
- // estimation de a = 1,b,c ; je cherche le min de somme(i=1:n) (0.5*(ei)^2)
+ // estimation de a = 1,b,c ; je cherche le min de somme(i=1:n)
+ // (0.5*(ei)^2)
// e1 = V[1][.] * b - V[3][.] = 0 ;
// e2 = V[2][.] * c - V[3][.] = 0 ;
// e3 = V[2][.] * b - V[3][.] * c = 0 ;
@@ -563,66 +526,75 @@ HLM3D(unsigned int nb_pts,
// e6 = V[6][.] * b - V[5][.] * c = 0 ;
// e7 = V[7][.] * b - V[8][.] = 0 ;
// e8 = V[7][.] * c - V[9][.] = 0 ;
- d[0] = V[2][vect] ;
- d[1] = V[4][vect] ;
- d[2] = V[1][vect] ;
- d[3] = V[0][vect] ;
- d[4] = V[3][vect] ;
- d[5] = V[4][vect] ;
- d[6] = V[0][vect] ;
- d[7] = V[1][vect] ;
-
- c[0][0] = V[5][vect] ; c[0][1] = 0.0 ;
- c[1][0] = V[6][vect] ; c[1][1] = 0.0 ;
- c[2][0] = V[3][vect] ; c[2][1] = 0.0 ;
- c[3][0] = V[4][vect] ; c[3][1] = 0.0
- ;
- c[4][0] = 0.0 ; c[4][1] = V[6][vect] ;
- c[5][0] = 0.0 ; c[5][1] = V[5][vect] ;
- c[6][0] = 0.0 ; c[6][1] = V[2][vect] ;
- c[7][0] = 0.0 ; c[7][1] = V[4][vect] ;
-
-
+ d[0] = V[2][vect];
+ d[1] = V[4][vect];
+ d[2] = V[1][vect];
+ d[3] = V[0][vect];
+ d[4] = V[3][vect];
+ d[5] = V[4][vect];
+ d[6] = V[0][vect];
+ d[7] = V[1][vect];
+
+ c[0][0] = V[5][vect];
+ c[0][1] = 0.0;
+ c[1][0] = V[6][vect];
+ c[1][1] = 0.0;
+ c[2][0] = V[3][vect];
+ c[2][1] = 0.0;
+ c[3][0] = V[4][vect];
+ c[3][1] = 0.0;
+ c[4][0] = 0.0;
+ c[4][1] = V[6][vect];
+ c[5][0] = 0.0;
+ c[5][1] = V[5][vect];
+ c[6][0] = 0.0;
+ c[6][1] = V[2][vect];
+ c[7][0] = 0.0;
+ c[7][1] = V[4][vect];
/// Calcul de la pseudo-inverse de C
- cp = c.pseudoInverse(1e-6) ;
+ cp = c.pseudoInverse(1e-6);
-
- vpColVector H_nr(3), temp ; // Homographie diagonale
+ vpColVector H_nr(3), temp; // Homographie diagonale
// Multiplication de la matrice H_nr par le vecteur cp
- temp = cp * d;
-
- H_nr[0] = temp[0] ; H_nr[1] = temp[1] ;
- H_nr[2] = 1.0 ;
-
- vpMatrix T(9,3) ; T =0 ;
- T[0][0] = -V[1][vect] ; T[0][1] = V[0][vect] ;
- T[1][0] = V[4][vect] ; T[1][2] = -V[2][vect] ;
- T[2][0] = -V[6][vect] ; T[2][1] = V[2][vect] ;
- T[3][0] = V[6][vect] ; T[3][2] = -V[0][vect] ;
- T[4][0] = -V[3][vect] ; T[4][1] = V[6][vect] ;
- T[5][0] = V[3][vect] ; T[5][2] = -V[1][vect] ;
- T[6][0] = -V[5][vect] ; T[6][1] = V[4][vect] ;
- T[7][0] = V[5][vect] ; T[7][2] = -V[6][vect] ;
- T[8][1] = -V[5][vect] ; T[8][2] = V[2][vect] ;
-
-
- vpMatrix Hd(3,3) ; // diag(gu,gv,gw)
- for (i=0 ; i < 3 ; i++) Hd[i][i] = H_nr[i] ;
+ temp = cp * d;
+
+ H_nr[0] = temp[0];
+ H_nr[1] = temp[1];
+ H_nr[2] = 1.0;
+
+ vpMatrix T(9, 3);
+ T = 0;
+ T[0][0] = -V[1][vect];
+ T[0][1] = V[0][vect];
+ T[1][0] = V[4][vect];
+ T[1][2] = -V[2][vect];
+ T[2][0] = -V[6][vect];
+ T[2][1] = V[2][vect];
+ T[3][0] = V[6][vect];
+ T[3][2] = -V[0][vect];
+ T[4][0] = -V[3][vect];
+ T[4][1] = V[6][vect];
+ T[5][0] = V[3][vect];
+ T[5][2] = -V[1][vect];
+ T[6][0] = -V[5][vect];
+ T[6][1] = V[4][vect];
+ T[7][0] = V[5][vect];
+ T[7][2] = -V[6][vect];
+ T[8][1] = -V[5][vect];
+ T[8][2] = V[2][vect];
+
+ vpMatrix Hd(3, 3); // diag(gu,gv,gw)
+ for (i = 0; i < 3; i++)
+ Hd[i][i] = H_nr[i];
// H = M diag(gu,gv,gw) M*-1
- H = M*Hd*Mdp ;
-
-
-
+ H = M * Hd * Mdp;
}
}
-void
-HLM(unsigned int q_cible,
- const std::vector<double> &xm, const std::vector<double> &ym,
- const std::vector<double> &xmi, const std::vector<double> &ymi,
- vpMatrix &H)
+void HLM(unsigned int q_cible, const std::vector<double> &xm, const std::vector<double> &ym,
+ const std::vector<double> &xmi, const std::vector<double> &ymi, vpMatrix &H)
{
unsigned int nbpt = (unsigned int)xm.size();
@@ -630,20 +602,20 @@ HLM(unsigned int q_cible,
on regarde si il y a au moins un point mais pour l'homographie
il faut au moins quatre points
****/
- vpMatrix pd(nbpt,3) ;
- vpMatrix p(nbpt,3) ;
+ vpMatrix pd(nbpt, 3);
+ vpMatrix p(nbpt, 3);
- for (unsigned int i=0;i<nbpt;i++) {
+ for (unsigned int i = 0; i < nbpt; i++) {
/****
on assigne les points fournies par la structure robot
pour la commande globale
****/
pd[i][0] = xmi[i];
pd[i][1] = ymi[i];
- pd[i][2] = 1.0 ;
+ pd[i][2] = 1.0;
p[i][0] = xm[i];
p[i][1] = ym[i];
- p[i][2] = 1.0 ;
+ p[i][2] = 1.0;
}
switch (q_cible) {
@@ -651,54 +623,50 @@ HLM(unsigned int q_cible,
case (2):
/* La cible est planaire de type points */
- HLM2D(nbpt,pd,p,H);
+ HLM2D(nbpt, pd, p, H);
break;
- case (3) : /* cible non planaire : chateau */
+ case (3): /* cible non planaire : chateau */
/* cible non planaire de type points */
- HLM3D(nbpt,pd,p,H);
+ HLM3D(nbpt, pd, p, H);
break;
} /* fin switch */
-
-
} /* fin procedure calcul_homogaphie */
#endif // #ifndef DOXYGEN_SHOULD_SKIP_THIS
/*!
From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a
- and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, computes the
- homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}\f$
- using Ezio Malis linear method (HLM) \cite Malis00b.
+ and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates,
+ computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\;
+ ^b{\bf p}\f$ using Ezio Malis linear method (HLM) \cite Malis00b.
- This method can consider points that are planar or non planar. The algorithm for planar
- scene implemented in this file is described in Ezio
- Malis PhD thesis \cite TheseMalis.
+ This method can consider points that are planar or non planar. The algorithm
+ for planar scene implemented in this file is described in Ezio Malis PhD
+ thesis \cite TheseMalis.
- \param xb, yb : Coordinates vector of matched points in image b. These coordinates are expressed in meters.
- \param xa, ya : Coordinates vector of matched points in image a. These coordinates are expressed in meters.
- \param isplanar : If true the points are assumed to be in a plane,
- otherwise there are assumed to be non planar.
- \param aHb : Estimated homography that relies the transformation from image a to image b.
+ \param xb, yb : Coordinates vector of matched points in image b. These
+ coordinates are expressed in meters. \param xa, ya : Coordinates vector of
+ matched points in image a. These coordinates are expressed in meters. \param
+ isplanar : If true the points are assumed to be in a plane, otherwise there
+ are assumed to be non planar. \param aHb : Estimated homography that relies
+ the transformation from image a to image b.
If the boolean isplanar is true the points are assumed to be in a plane
otherwise there are assumed to be non planar.
\sa DLT() when the scene is planar.
*/
-void vpHomography::HLM(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- bool isplanar,
- vpHomography &aHb)
+void vpHomography::HLM(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, bool isplanar, vpHomography &aHb)
{
- unsigned int n = (unsigned int) xb.size();
+ 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)
+ if (n < 4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
// The reference plane is the plane build from the 3 first points.
@@ -706,12 +674,11 @@ void vpHomography::HLM(const std::vector<double> &xb, const std::vector<double>
vpMatrix H; // matrice d'homographie en metre
if (isplanar)
- q_cible =1;
+ q_cible = 1;
else
- q_cible =3;
+ q_cible = 3;
- ::HLM(q_cible, xa, ya, xb, yb, H) ;
+ ::HLM(q_cible, xa, ya, xb, yb, H);
aHb = H;
}
-
diff --git a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp
index 9a77c1d..73fee0e 100644
--- a/modules/vision/src/homography-estimation/vpHomographyRansac.cpp
+++ b/modules/vision/src/homography-estimation/vpHomographyRansac.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,12 +36,12 @@
*
*****************************************************************************/
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpColVector.h>
#include <visp3/core/vpRansac.h>
+#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpImage.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpMeterPixelConversion.h>
#define vpEps 1e-6
@@ -55,89 +56,76 @@
bool iscolinear(double *x1, double *x2, double *x3);
bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3);
-bool
-iscolinear(double *x1, double *x2, double *x3)
+bool iscolinear(double *x1, double *x2, double *x3)
{
vpColVector p1(3), p2(3), p3(3);
- p1 << x1 ;
- p2 << x2 ;
- p3 << x3 ;
- //vpColVector v;
- //vpColVector::cross(p2-p1, p3-p1, v);
- //return (v.sumSquare() < vpEps);
+ p1 << x1;
+ p2 << x2;
+ p3 << x3;
+ // vpColVector v;
+ // vpColVector::cross(p2-p1, p3-p1, v);
+ // return (v.sumSquare() < vpEps);
// Assume inhomogeneous coords, or homogeneous coords with equal
// scale.
- return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
+ return ((vpColVector::cross(p2 - p1, p3 - p1).sumSquare()) < vpEps);
}
-bool
-isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
+bool isColinear(vpColVector &p1, vpColVector &p2, vpColVector &p3)
{
- return ((vpColVector::cross(p2-p1, p3-p1).sumSquare()) < vpEps);
+ 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;
- for (i=1 ; i < 4 ; i++)
- for (j=0 ; j<i ; j++)
- if (ind[i]==ind[j]) return true ;
-
- unsigned int n = x.getRows()/4 ;
- double pa[4][3] ;
- double pb[4][3] ;
-
+ for (i = 1; i < 4; i++)
+ for (j = 0; j < i; j++)
+ if (ind[i] == ind[j])
+ return true;
+ unsigned int n = x.getRows() / 4;
+ double pa[4][3];
+ double pb[4][3];
- for(i = 0 ; i < 4 ; i++)
- {
- pb[i][0] = x[2*ind[i]] ;
- pb[i][1] = x[2*ind[i]+1] ;
+ for (i = 0; i < 4; i++) {
+ pb[i][0] = x[2 * ind[i]];
+ pb[i][1] = x[2 * ind[i] + 1];
pb[i][2] = 1;
- pa[i][0] = x[2*n+2*ind[i]] ;
- pa[i][1] = x[2*n+2*ind[i]+1] ;
+ pa[i][0] = x[2 * n + 2 * ind[i]];
+ pa[i][1] = x[2 * n + 2 * ind[i] + 1];
pa[i][2] = 1;
}
i = 0, j = 1, k = 2;
- double area012 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+ double area012 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
+ -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
- i = 0; j = 1, k = 3;
- double area013 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+ i = 0;
+ j = 1, k = 3;
+ double area013 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
+ -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
- i = 0; j = 2, k = 3;
- double area023 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+ i = 0;
+ j = 2, k = 3;
+ double area023 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
+ -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
- i = 1; j = 2, k = 3;
- double area123 = (-pa[j][0]*pa[i][1] + pa[k][0]*pa[i][1] +
- pa[i][0]*pa[j][1] - pa[k][0]*pa[j][1] +
- -pa[i][0]*pa[k][1] + pa[1][j]*pa[k][1]);
+ i = 1;
+ j = 2, k = 3;
+ double area123 = (-pa[j][0] * pa[i][1] + pa[k][0] * pa[i][1] + pa[i][0] * pa[j][1] - pa[k][0] * pa[j][1] +
+ -pa[i][0] * pa[k][1] + pa[1][j] * pa[k][1]);
double sum_area = area012 + area013 + area023 + area123;
return ((sum_area < threshold_area) ||
- (iscolinear(pa[0],pa[1],pa[2]) ||
- iscolinear(pa[0],pa[1],pa[3]) ||
- iscolinear(pa[0],pa[2],pa[3]) ||
- iscolinear(pa[1],pa[2],pa[3]) ||
- iscolinear(pb[0],pb[1],pb[2]) ||
- iscolinear(pb[0],pb[1],pb[3]) ||
- iscolinear(pb[0],pb[2],pb[3]) ||
- iscolinear(pb[1],pb[2],pb[3])));
+ (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
+ iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
+ iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3])));
}
- /*
+/*
\brief
Function to determine if a set of 4 pairs of matched points give rise
to a degeneracy in the calculation of a homography as needed by RANSAC.
@@ -149,47 +137,40 @@ x1b,y1b, x2b, y2b, ... xnb, ynb
x1a,y1a, x2a, y2a, ... xna, yna
leading to 2*2*n
*/
-bool
-vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
+bool vpHomography::degenerateConfiguration(vpColVector &x, unsigned int *ind)
{
- for (unsigned int i = 1; i < 4 ; i++)
- for (unsigned int j = 0 ;j < i ; j++)
- if (ind[i] == ind[j]) return true ;
+ for (unsigned int i = 1; i < 4; i++)
+ for (unsigned int j = 0; j < i; j++)
+ if (ind[i] == ind[j])
+ return true;
- unsigned int n = x.getRows()/4;
+ unsigned int n = x.getRows() / 4;
double pa[4][3];
double pb[4][3];
unsigned int n2 = 2 * n;
- for(unsigned int i = 0; i < 4 ;i++)
- {
- unsigned int ind2 = 2 * ind[i];
- pb[i][0] = x[ind2];
- pb[i][1] = x[ind2+1];
- pb[i][2] = 1;
-
- pa[i][0] = x[n2+ind2] ;
- pa[i][1] = x[n2+ind2+1] ;
- pa[i][2] = 1;
- }
- return ( iscolinear(pa[0],pa[1],pa[2]) ||
- iscolinear(pa[0],pa[1],pa[3]) ||
- iscolinear(pa[0],pa[2],pa[3]) ||
- iscolinear(pa[1],pa[2],pa[3]) ||
- iscolinear(pb[0],pb[1],pb[2]) ||
- iscolinear(pb[0],pb[1],pb[3]) ||
- iscolinear(pb[0],pb[2],pb[3]) ||
- iscolinear(pb[1],pb[2],pb[3]));
+ for (unsigned int i = 0; i < 4; i++) {
+ unsigned int ind2 = 2 * ind[i];
+ pb[i][0] = x[ind2];
+ pb[i][1] = x[ind2 + 1];
+ pb[i][2] = 1;
+
+ pa[i][0] = x[n2 + ind2];
+ pa[i][1] = x[n2 + ind2 + 1];
+ pa[i][2] = 1;
+ }
+ return (iscolinear(pa[0], pa[1], pa[2]) || iscolinear(pa[0], pa[1], pa[3]) || iscolinear(pa[0], pa[2], pa[3]) ||
+ iscolinear(pa[1], pa[2], pa[3]) || iscolinear(pb[0], pb[1], pb[2]) || iscolinear(pb[0], pb[1], pb[3]) ||
+ iscolinear(pb[0], pb[2], pb[3]) || iscolinear(pb[1], pb[2], pb[3]));
}
-bool
-vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya)
+bool vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::vector<double> &yb,
+ const std::vector<double> &xa, const std::vector<double> &ya)
{
unsigned int n = (unsigned int)xb.size();
if (n < 4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
std::vector<vpColVector> pa(n), pb(n);
- for (unsigned i=0; i<n;i++) {
+ for (unsigned i = 0; i < n; i++) {
pa[i].resize(3);
pa[i][0] = xa[i];
pa[i][1] = ya[i];
@@ -200,14 +181,13 @@ vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::
pb[i][2] = 1;
}
- for (unsigned int i = 0; i < n-2; i++) {
- for (unsigned int j = i+1; j < n-1; j++) {
- for (unsigned int k = j+1; k < n ; k++)
- {
+ for (unsigned int i = 0; i < n - 2; i++) {
+ for (unsigned int j = i + 1; j < n - 1; j++) {
+ for (unsigned int k = j + 1; k < n; k++) {
if (isColinear(pa[i], pa[j], pa[k])) {
return true;
}
- if (isColinear(pb[i], pb[j], pb[k])){
+ if (isColinear(pb[i], pb[j], pb[k])) {
return true;
}
}
@@ -216,157 +196,138 @@ vpHomography::degenerateConfiguration(const std::vector<double> &xb, const std::
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 ;
+ unsigned int n = x.getRows() / 4;
std::vector<double> xa(4), xb(4);
std::vector<double> ya(4), yb(4);
unsigned int n2 = n * 2;
- for(unsigned int i=0 ; i < 4 ; i++)
- {
- unsigned int ind2 = 2 * ind[i];
- xb[i] = x[ind2] ;
- yb[i] = x[ind2+1] ;
-
- xa[i] = x[n2+ind2] ;
- ya[i] = x[n2+ind2+1] ;
- }
+ for (unsigned int i = 0; i < 4; i++) {
+ unsigned int ind2 = 2 * ind[i];
+ xb[i] = x[ind2];
+ yb[i] = x[ind2 + 1];
+
+ xa[i] = x[n2 + ind2];
+ ya[i] = x[n2 + ind2 + 1];
+ }
- vpHomography aHb ;
+ vpHomography aHb;
try {
vpHomography::HLM(xb, yb, xa, ya, true, aHb);
+ } catch (...) {
+ aHb.eye();
}
- catch(...)
- {
- aHb.eye();
- }
M.resize(9);
- for (unsigned int i=0 ; i <9 ; i++)
- {
- M[i] = aHb.data[i] ;
- }
- aHb /= aHb[2][2] ;
+ for (unsigned int i = 0; i < 9; i++) {
+ M[i] = aHb.data[i];
+ }
+ aHb /= aHb[2][2];
}
-
// Evaluate distances between points and model.
-double
-vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
+double vpHomography::computeResidual(vpColVector &x, vpColVector &M, vpColVector &d)
{
- unsigned int n = x.getRows()/4 ;
- unsigned int n2 = n * 2;
+ unsigned int n = x.getRows() / 4;
+ unsigned int n2 = n * 2;
vpColVector *pa;
vpColVector *pb;
- pa = new vpColVector [n];
- pb = new vpColVector [n];
-
- for(unsigned int i=0 ; i < n ; i++)
- {
- unsigned int i2 = 2 * i;
- pb[i].resize(3) ;
- pb[i][0] = x[i2] ;
- pb[i][1] = x[i2+1] ;
- pb[i][2] = 1;
-
- pa[i].resize(3) ;
- pa[i][0] = x[n2+i2] ;
- pa[i][1] = x[n2+i2+1] ;
- pa[i][2] = 1;
- }
+ pa = new vpColVector[n];
+ pb = new vpColVector[n];
- vpMatrix aHb(3,3) ;
+ for (unsigned int i = 0; i < n; i++) {
+ unsigned int i2 = 2 * i;
+ pb[i].resize(3);
+ pb[i][0] = x[i2];
+ pb[i][1] = x[i2 + 1];
+ pb[i][2] = 1;
- for (unsigned int i=0 ; i <9 ; i++)
- {
- aHb.data[i] = M[i];
- }
+ pa[i].resize(3);
+ pa[i][0] = x[n2 + i2];
+ pa[i][1] = x[n2 + i2 + 1];
+ pa[i][2] = 1;
+ }
+
+ vpMatrix aHb(3, 3);
+
+ for (unsigned int i = 0; i < 9; i++) {
+ aHb.data[i] = M[i];
+ }
aHb /= aHb[2][2];
d.resize(n);
- vpColVector Hpb ;
- for (unsigned int i=0 ; i <n ; i++)
- {
- Hpb = aHb*pb[i] ;
- Hpb /= Hpb[2] ;
- d[i] = sqrt((pa[i] - Hpb ).sumSquare()) ;
- }
+ vpColVector Hpb;
+ for (unsigned int i = 0; i < n; i++) {
+ Hpb = aHb * pb[i];
+ Hpb /= Hpb[2];
+ d[i] = sqrt((pa[i] - Hpb).sumSquare());
+ }
- delete [] pa;
- delete [] pb;
+ delete[] pa;
+ delete[] pb;
- return 0 ;
+ return 0;
}
#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) ;
+ x.resize(4 * n);
unsigned int n2 = n * 2;
- for (unsigned int i=0 ; i < n ; i++)
- {
+ for (unsigned int i = 0; i < n; i++) {
unsigned int i2 = 2 * i;
- x[i2] = xb[i] ;
- x[i2+1] = yb[i] ;
- x[n2+i2] = xa[i] ;
- x[n2+i2+1] = ya[i] ;
+ x[i2] = xb[i];
+ x[i2 + 1] = yb[i];
+ x[n2 + i2] = xa[i];
+ x[n2 + i2 + 1] = ya[i];
}
}
/*!
From couples of matched points \f$^a{\bf p}=(x_a,y_a,1)\f$ in image a
- and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates, computes the
- homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\; ^b{\bf p}\f$
- using Ransac algorithm.
+ and \f$^b{\bf p}=(x_b,y_b,1)\f$ in image b with homogeneous coordinates,
+ computes the homography matrix by resolving \f$^a{\bf p} = ^a{\bf H}_b\;
+ ^b{\bf p}\f$ using Ransac algorithm.
- \param xb, yb : Coordinates vector of matched points in image b. These coordinates are expressed in meters.
- \param xa, ya : Coordinates vector of matched points in image a. These coordinates are expressed in meters.
- \param aHb : Estimated homography that relies the transformation from image a to image b.
- \param inliers : Vector that indicates if a matched point is an inlier (true) or an outlier (false).
- \param residual : Global residual computed as
- \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the
- number of inliers.
+ \param xb, yb : Coordinates vector of matched points in image b. These
+ coordinates are expressed in meters. \param xa, ya : Coordinates vector of
+ matched points in image a. These coordinates are expressed in meters. \param
+ aHb : Estimated homography that relies the transformation from image a to
+ image b. \param inliers : Vector that indicates if a matched point is an
+ inlier (true) or an outlier (false). \param residual : Global residual
+ computed as \f$r = \sqrt{1/n \sum_{inliers} {\| {^a{\bf p} - {\hat{^a{\bf
+ H}_b}} {^b{\bf p}}} \|}^{2}}\f$ with \f$n\f$ the number of inliers.
- \param nbInliersConsensus : Minimal number of points requested to fit the estimated homography.
+ \param nbInliersConsensus : Minimal number of points requested to fit the
+ estimated homography.
- \param threshold : Threshold for outlier removing. A point is considered as an outlier if the reprojection error
- \f$\| {^a{\bf p} - {\hat{^a{\bf H}_b}} {^b{\bf p}}} \|\f$ is greater than this threshold.
+ \param threshold : Threshold for outlier removing. A point is considered as
+ an outlier if the reprojection error \f$\| {^a{\bf p} - {\hat{^a{\bf H}_b}}
+ {^b{\bf p}}} \|\f$ is greater than this threshold.
- \param normalization : When set to true, the coordinates of the points are normalized. The normalization
- carried out is the one preconized by Hartley.
+ \param normalization : When set to true, the coordinates of the points are
+ normalized. The normalization carried out is the one preconized by Hartley.
\return true if the homography could be computed, false otherwise.
*/
-bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<double> &yb,
- const std::vector<double> &xa, const std::vector<double> &ya,
- vpHomography &aHb,
- std::vector<bool> &inliers,
- double &residual,
- unsigned int nbInliersConsensus,
- double threshold,
- bool normalization)
+bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<double> &yb, const std::vector<double> &xa,
+ const std::vector<double> &ya, vpHomography &aHb, std::vector<bool> &inliers,
+ double &residual, unsigned int nbInliersConsensus, double threshold, bool normalization)
{
unsigned int n = (unsigned int)xb.size();
if (yb.size() != n || xa.size() != n || ya.size() != n)
- throw(vpException(vpException::dimensionError,
- "Bad dimension for robust homography estimation"));
+ throw(vpException(vpException::dimensionError, "Bad dimension for robust homography estimation"));
// 4 point are required
- if(n<4)
+ if (n < 4)
throw(vpException(vpException::fatalError, "There must be at least 4 matched points"));
- vpUniRand random((const long)time(NULL)) ;
+ vpUniRand random((const long)time(NULL));
std::vector<unsigned int> best_consensus;
std::vector<unsigned int> cur_consensus;
@@ -375,7 +336,7 @@ bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<doubl
std::vector<unsigned int> rand_ind;
- unsigned int nbMinRandom = 4 ;
+ unsigned int nbMinRandom = 4;
unsigned int ransacMaxTrials = 1000;
unsigned int maxDegenerateIter = 1000;
@@ -393,22 +354,20 @@ bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<doubl
if (inliers.size() != n)
inliers.resize(n);
- while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus)
- {
+ while (nbTrials < ransacMaxTrials && nbInliers < nbInliersConsensus) {
cur_outliers.clear();
cur_randoms.clear();
bool degenerate = true;
- while(degenerate == true){
+ while (degenerate == true) {
std::vector<bool> usedPt(n, false);
rand_ind.clear();
- for(unsigned int i = 0; i < nbMinRandom; i++)
- {
+ for (unsigned int i = 0; i < nbMinRandom; i++) {
// Generate random indicies in the range 0..n
- unsigned int r = (unsigned int)ceil(random()*n) -1;
- while(usedPt[r]) {
- r = (unsigned int)ceil(random()*n) -1;
+ unsigned int r = (unsigned int)ceil(random() * n) - 1;
+ while (usedPt[r]) {
+ r = (unsigned int)ceil(random() * n) - 1;
}
usedPt[r] = true;
rand_ind.push_back(r);
@@ -419,64 +378,70 @@ bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<doubl
yb_rand[i] = yb[r];
}
- try{
- if (! vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
+ try {
+ if (!vpHomography::degenerateConfiguration(xb_rand, yb_rand, xa_rand, ya_rand)) {
vpHomography::DLT(xb_rand, yb_rand, xa_rand, ya_rand, aHb, normalization);
- degenerate = false;
+ degenerate = false;
}
- }
- catch(...){
+ } catch (...) {
degenerate = true;
}
- nbDegenerateIter ++;
+ nbDegenerateIter++;
- if (nbDegenerateIter > maxDegenerateIter){
+ if (nbDegenerateIter > maxDegenerateIter) {
vpERROR_TRACE("Unable to select a nondegenerate data set");
throw(vpException(vpException::fatalError, "Unable to select a nondegenerate data set"));
}
}
- aHb /= aHb[2][2] ;
+ aHb /= aHb[2][2];
// Computing Residual
double r = 0;
- vpColVector a(3), b(3), c(3) ;
- for (unsigned int i=0 ; i < nbMinRandom ; i++) {
- a[0] = xa_rand[i] ; a[1] = ya_rand[i] ; a[2] = 1 ;
- b[0] = xb_rand[i] ; b[1] = yb_rand[i] ; b[2] = 1 ;
-
- c = aHb*b; c /= c[2] ;
- r += (a-c).sumSquare() ;
- //cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
+ vpColVector a(3), b(3), c(3);
+ for (unsigned int i = 0; i < nbMinRandom; i++) {
+ a[0] = xa_rand[i];
+ a[1] = ya_rand[i];
+ a[2] = 1;
+ b[0] = xb_rand[i];
+ b[1] = yb_rand[i];
+ b[2] = 1;
+
+ c = aHb * b;
+ c /= c[2];
+ r += (a - c).sumSquare();
+ // cout << "point " <<i << " " << (a-c).sumSquare() <<endl ;;
}
// Finding inliers & ouliers
- r = sqrt(r/nbMinRandom);
- //std::cout << "Candidate residual: " << r << std::endl;
- if (r < threshold)
- {
+ r = sqrt(r / nbMinRandom);
+ // std::cout << "Candidate residual: " << r << std::endl;
+ if (r < threshold) {
unsigned int nbInliersCur = 0;
- for (unsigned int i = 0; i < n ; i++)
- {
- a[0] = xa[i] ; a[1] = ya[i] ; a[2] = 1 ;
- b[0] = xb[i] ; b[1] = yb[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- double error = sqrt((a-c).sumSquare()) ;
- if(error <= threshold){
+ for (unsigned int i = 0; i < n; i++) {
+ a[0] = xa[i];
+ a[1] = ya[i];
+ a[2] = 1;
+ b[0] = xb[i];
+ b[1] = yb[i];
+ b[2] = 1;
+
+ c = aHb * b;
+ c /= c[2];
+ double error = sqrt((a - c).sumSquare());
+ if (error <= threshold) {
nbInliersCur++;
cur_consensus.push_back(i);
inliers[i] = true;
- }
- else{
+ } else {
cur_outliers.push_back(i);
inliers[i] = false;
}
}
- //std::cout << "nb inliers that matches: " << nbInliersCur << std::endl;
- if(nbInliersCur > nbInliers)
- {
+ // std::cout << "nb inliers that matches: " << nbInliersCur <<
+ // std::endl;
+ if (nbInliersCur > nbInliers) {
foundSolution = true;
best_consensus = cur_consensus;
nbInliers = nbInliersCur;
@@ -486,49 +451,50 @@ bool vpHomography::ransac(const std::vector<double> &xb, const std::vector<doubl
}
nbTrials++;
- if(nbTrials >= ransacMaxTrials){
+ if (nbTrials >= ransacMaxTrials) {
vpERROR_TRACE("Ransac reached the maximum number of trials");
foundSolution = true;
}
}
- if(foundSolution){
- if(nbInliers >= nbInliersConsensus)
- {
+ if (foundSolution) {
+ if (nbInliers >= nbInliersConsensus) {
std::vector<double> xa_best(best_consensus.size());
std::vector<double> ya_best(best_consensus.size());
std::vector<double> xb_best(best_consensus.size());
std::vector<double> yb_best(best_consensus.size());
- for(unsigned i = 0 ; i < best_consensus.size(); i++)
- {
+ for (unsigned i = 0; i < best_consensus.size(); i++) {
xa_best[i] = xa[best_consensus[i]];
ya_best[i] = ya[best_consensus[i]];
xb_best[i] = xb[best_consensus[i]];
yb_best[i] = yb[best_consensus[i]];
}
- vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization) ;
+ vpHomography::DLT(xb_best, yb_best, xa_best, ya_best, aHb, normalization);
aHb /= aHb[2][2];
- residual = 0 ;
+ residual = 0;
vpColVector a(3), b(3), c(3);
- for (unsigned int i=0 ; i < best_consensus.size() ; i++) {
- a[0] = xa_best[i] ; a[1] = ya_best[i] ; a[2] = 1 ;
- b[0] = xb_best[i] ; b[1] = yb_best[i] ; b[2] = 1 ;
-
- c = aHb*b ; c /= c[2] ;
- residual += (a-c).sumSquare() ;
+ for (unsigned int i = 0; i < best_consensus.size(); i++) {
+ a[0] = xa_best[i];
+ a[1] = ya_best[i];
+ a[2] = 1;
+ b[0] = xb_best[i];
+ b[1] = yb_best[i];
+ b[2] = 1;
+
+ c = aHb * b;
+ c /= c[2];
+ residual += (a - c).sumSquare();
}
- residual = sqrt(residual/best_consensus.size());
+ residual = sqrt(residual / best_consensus.size());
return true;
- }
- else {
+ } else {
return false;
}
- }
- else {
+ } else {
return false;
}
}
diff --git a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp
index da91d5a..95c51dd 100644
--- a/modules/vision/src/homography-estimation/vpHomographyVVS.cpp
+++ b/modules/vision/src/homography-estimation/vpHomographyVVS.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,679 +38,666 @@
//#include <computeHomography.h>
//#include <utilsHomography.h>
-#include <visp3/core/vpRobust.h>
+#include <iostream>
+#include <visp3/core/vpExponentialMap.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vision/vpHomography.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpPlane.h>
-#include <iostream>
-#include <visp3/core/vpExponentialMap.h>
-
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpRobust.h>
+#include <visp3/vision/vpHomography.h>
const double vpHomography::threshold_rotation = 1e-7;
const double vpHomography::threshold_displacement = 1e-18;
#ifndef DOXYGEN_SHOULD_SKIP_THIS
-static void
-updatePoseRotation(vpColVector& dx,vpHomogeneousMatrix& mati)
+static void updatePoseRotation(vpColVector &dx, vpHomogeneousMatrix &mati)
{
- vpRotationMatrix rd ;
+ vpRotationMatrix rd;
- double s = sqrt(dx[0]*dx[0] + dx[1]*dx[1] + dx[2]*dx[2]);
- if (s > 1.e-25)
- {
+ double s = sqrt(dx[0] * dx[0] + dx[1] * dx[1] + dx[2] * dx[2]);
+ if (s > 1.e-25) {
double u[3];
- for (unsigned int i=0;i<3;i++) u[i] = dx[i]/s;
+ for (unsigned int i = 0; i < 3; i++)
+ u[i] = dx[i] / s;
double sinu = sin(s);
double cosi = cos(s);
- double mcosi = 1-cosi;
- rd[0][0] = cosi + mcosi*u[0]*u[0];
- rd[0][1] = -sinu*u[2] + mcosi*u[0]*u[1];
- rd[0][2] = sinu*u[1] + mcosi*u[0]*u[2];
- rd[1][0] = sinu*u[2] + mcosi*u[1]*u[0];
- rd[1][1] = cosi + mcosi*u[1]*u[1];
- rd[1][2] = -sinu*u[0] + mcosi*u[1]*u[2];
- rd[2][0] = -sinu*u[1] + mcosi*u[2]*u[0];
- rd[2][1] = sinu*u[0] + mcosi*u[2]*u[1];
- rd[2][2] = cosi + mcosi*u[2]*u[2];
- }
- else
- {
- for (unsigned int i=0;i<3;i++)
- {
- for (unsigned int j=0;j<3;j++) rd[i][j] = 0.0;
+ double mcosi = 1 - cosi;
+ rd[0][0] = cosi + mcosi * u[0] * u[0];
+ rd[0][1] = -sinu * u[2] + mcosi * u[0] * u[1];
+ rd[0][2] = sinu * u[1] + mcosi * u[0] * u[2];
+ rd[1][0] = sinu * u[2] + mcosi * u[1] * u[0];
+ rd[1][1] = cosi + mcosi * u[1] * u[1];
+ rd[1][2] = -sinu * u[0] + mcosi * u[1] * u[2];
+ rd[2][0] = -sinu * u[1] + mcosi * u[2] * u[0];
+ rd[2][1] = sinu * u[0] + mcosi * u[2] * u[1];
+ rd[2][2] = cosi + mcosi * u[2] * u[2];
+ } else {
+ for (unsigned int i = 0; i < 3; i++) {
+ for (unsigned int j = 0; j < 3; j++)
+ rd[i][j] = 0.0;
rd[i][i] = 1.0;
}
}
- vpHomogeneousMatrix Delta ;
- Delta.insert(rd) ;
+ vpHomogeneousMatrix Delta;
+ Delta.insert(rd);
- mati = Delta.inverse() * mati ;
+ mati = Delta.inverse() * mati;
}
-double
-vpHomography::computeRotation(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpHomogeneousMatrix &c2Mc1,
- int userobust
- )
+double vpHomography::computeRotation(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpHomogeneousMatrix &c2Mc1,
+ int userobust)
{
- vpColVector e(2) ;
- double r_1 = -1 ;
+ vpColVector e(2);
+ double r_1 = -1;
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
+ vpColVector p2(3);
+ vpColVector p1(3);
+ vpColVector Hp2(3);
+ vpColVector Hp1(3);
- vpMatrix H2(2,3) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,3) ;
- vpColVector e1(2) ;
+ vpMatrix H2(2, 3);
+ vpColVector e2(2);
+ vpMatrix H1(2, 3);
+ vpColVector e1(2);
bool only_1 = false;
bool only_2 = false;
- int iter = 0 ;
+ int iter = 0;
- unsigned int n=0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++) {
+ unsigned int n = 0;
+ for (unsigned int i = 0; i < nbpoint; i++) {
// if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
- if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
- &&
- (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
- {
- n++ ;
+ if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) &&
+ (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) {
+ n++;
}
}
- if ( (! only_1) && (! only_2) )
- n *=2 ;
+ if ((!only_1) && (!only_2))
+ n *= 2;
vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
- vpMatrix c2Rc1(3,3) ;
- double r =0 ;
- while (vpMath::equal(r_1,r,threshold_rotation) == false )
- {
-
- r_1 =r ;
+ vpColVector res(n);
+ vpColVector w(n);
+ w = 1;
+ robust.setThreshold(0.0000);
+ vpMatrix W(2 * n, 2 * n);
+ W = 0;
+ vpMatrix c2Rc1(3, 3);
+ double r = 0;
+ while (vpMath::equal(r_1, r, threshold_rotation) == false) {
+
+ r_1 = r;
// compute current position
- //Change frame (current)
- for (unsigned int i=0 ; i < 3 ; i++)
- for (unsigned int j=0 ; j < 3 ; j++)
- c2Rc1[i][j] = c2Mc1[i][j] ;
-
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++) {
- //if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
- if ( (std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.)))
- &&
- (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.))) )
- {
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
-
- Hp2 = c2Rc1.t()*p2 ; // p2 = Hp1
- Hp1 = c2Rc1*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
-
+ // Change frame (current)
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ c2Rc1[i][j] = c2Mc1[i][j];
+
+ vpMatrix L(2, 3), Lp;
+ int k = 0;
+ for (unsigned int i = 0; i < nbpoint; i++) {
+ // if ((c2P[i].get_x() !=-1) && (c1P[i].get_x() !=-1))
+ if ((std::fabs(c2P[i].get_x() + 1) > std::fabs(vpMath::maximum(c2P[i].get_x(), 1.))) &&
+ (std::fabs(c1P[i].get_x() + 1) > std::fabs(vpMath::maximum(c1P[i].get_x(), 1.)))) {
+ p2[0] = c2P[i].get_x();
+ p2[1] = c2P[i].get_y();
+ p2[2] = 1.0;
+ p1[0] = c1P[i].get_x();
+ p1[1] = c1P[i].get_y();
+ p1[2] = 1.0;
+
+ Hp2 = c2Rc1.t() * p2; // p2 = Hp1
+ Hp1 = c2Rc1 * p1; // p1 = Hp2
+
+ Hp2 /= Hp2[2]; // normalisation
+ Hp1 /= Hp1[2];
// set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
-
- H2[0][0] = x*y ; H2[0][1] = -(1+x*x) ; H2[0][2] = y ;
- H2[1][0] = 1+y*y ; H2[1][1] = -x*y ; H2[1][2] = -x ;
- H2 *=-1 ;
- H2 = H2*c2Rc1.t() ;
+ double x = Hp2[0];
+ double y = Hp2[1];
+
+ H2[0][0] = x * y;
+ H2[0][1] = -(1 + x * x);
+ H2[0][2] = y;
+ H2[1][0] = 1 + y * y;
+ H2[1][1] = -x * y;
+ H2[1][2] = -x;
+ H2 *= -1;
+ H2 = H2 * c2Rc1.t();
// Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
+ e2[0] = Hp2[0] - c1P[i].get_x();
+ e2[1] = Hp2[1] - c1P[i].get_y();
// set up the interaction matrix
- x = Hp1[0] ;
- y = Hp1[1] ;
+ x = Hp1[0];
+ y = Hp1[1];
- H1[0][0] = x*y ; H1[0][1] = -(1+x*x) ; H1[0][2] = y ;
- H1[1][0] = 1+y*y ; H1[1][1] = -x*y ; H1[1][2] = -x ;
+ H1[0][0] = x * y;
+ H1[0][1] = -(1 + x * x);
+ H1[0][2] = y;
+ H1[1][0] = 1 + y * y;
+ H1[1][1] = -x * y;
+ H1[1][2] = -x;
// Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
- if (only_2)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
+ e1[0] = Hp1[0] - c2P[i].get_x();
+ e1[1] = Hp1[1] - c2P[i].get_y();
+
+ if (only_2) {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
- }
- else
- if (only_1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
- }
+ } else if (only_1) {
+ if (k == 0) {
+ L = H1;
+ e = e1;
+ } else {
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
}
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
- }
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
+ } else {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
+ }
- k++ ;
+ k++;
}
}
- if (userobust)
- {
+ if (userobust) {
robust.setIteration(0);
- for (unsigned int l=0 ; l < n ; l++)
- {
- res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
+ for (unsigned int l = 0; l < n; l++) {
+ res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
}
robust.MEstimator(vpRobust::TUKEY, res, w);
-
// compute the pseudo inverse of the interaction matrix
- for (unsigned int l=0 ; l < n ; l++)
- {
- W[2*l][2*l] = w[l] ;
- W[2*l+1][2*l+1] = w[l] ;
+ for (unsigned int l = 0; l < n; l++) {
+ W[2 * l][2 * l] = w[l];
+ W[2 * l + 1][2 * l + 1] = w[l];
}
- }
- else
- {
- for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
+ } else {
+ for (unsigned int l = 0; l < 2 * n; l++)
+ W[l][l] = 1;
}
// CreateDiagonalMatrix(w, W) ;
- (L).pseudoInverse(Lp, 1e-6) ;
+ (L).pseudoInverse(Lp, 1e-6);
// Compute the camera velocity
- vpColVector c2rc1, v(6) ;
+ vpColVector c2rc1, v(6);
- c2rc1 = -2*Lp*W*e ;
- for (unsigned int i=0 ; i < 3 ; i++) v[i+3] = c2rc1[i] ;
+ c2rc1 = -2 * Lp * W * e;
+ for (unsigned int i = 0; i < 3; i++)
+ v[i + 3] = c2rc1[i];
// only for simulation
- updatePoseRotation(c2rc1, c2Mc1) ;
- r =e.sumSquare() ;
-
- if ((W*e).sumSquare() < 1e-10) break ;
- if (iter>25) break ;
- iter++ ; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e=" <<(W*e).sumSquare() <<std::endl ;
+ updatePoseRotation(c2rc1, c2Mc1);
+ r = e.sumSquare();
+ if ((W * e).sumSquare() < 1e-10)
+ break;
+ if (iter > 25)
+ break;
+ iter++; // std::cout << iter <<" e=" <<(e).sumSquare() <<" e="
+ // <<(W*e).sumSquare() <<std::endl ;
}
// std::cout << c2Mc1 <<std::endl ;
- return (W*e).sumSquare() ;
+ 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 D1 = oN.getD() - (cMo[0][3]*A1 + cMo[1][3]*B1 + cMo[2][3]*C1);
-
- cN.resize(3) ;
- cN[0] = A1 ;
- cN[1] = B1 ;
- cN[2] = C1 ;
- cd = -D1 ;
+ double A1 = cMo[0][0] * oN.getA() + cMo[0][1] * oN.getB() + cMo[0][2] * oN.getC();
+ double B1 = cMo[1][0] * oN.getA() + cMo[1][1] * oN.getB() + cMo[1][2] * oN.getC();
+ double C1 = cMo[2][0] * oN.getA() + cMo[2][1] * oN.getB() + cMo[2][2] * oN.getC();
+ double D1 = oN.getD() - (cMo[0][3] * A1 + cMo[1][3] * B1 + cMo[2][3] * C1);
+
+ cN.resize(3);
+ cN[0] = A1;
+ cN[1] = B1;
+ cN[2] = C1;
+ cd = -D1;
}
-double
-vpHomography::computeDisplacement(unsigned int nbpoint,
- vpPoint *c1P,
- vpPoint *c2P,
- vpPlane &oN,
- vpHomogeneousMatrix &c2Mc1,
- vpHomogeneousMatrix &c1Mo,
- int userobust
- )
+double vpHomography::computeDisplacement(unsigned int nbpoint, vpPoint *c1P, vpPoint *c2P, vpPlane &oN,
+ vpHomogeneousMatrix &c2Mc1, vpHomogeneousMatrix &c1Mo, int userobust)
{
- vpColVector e(2) ;
- double r_1 = -1 ;
+ vpColVector e(2);
+ double r_1 = -1;
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
+ vpColVector p2(3);
+ vpColVector p1(3);
+ vpColVector Hp2(3);
+ vpColVector Hp1(3);
- vpMatrix H2(2,6) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,6) ;
- vpColVector e1(2) ;
+ vpMatrix H2(2, 6);
+ vpColVector e2(2);
+ vpMatrix H1(2, 6);
+ vpColVector e1(2);
bool only_1 = true;
bool only_2 = false;
- int iter = 0 ;
- unsigned int n=0 ;
- n = nbpoint ;
+ int iter = 0;
+ unsigned int n = 0;
+ n = nbpoint;
// next 2 lines are useless (detected by Coverity Scan)
- //if ( (! only_1) && (! only_2) )
+ // if ( (! only_1) && (! only_2) )
// n *=2 ;
vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
-
- vpColVector N1(3), N2(3) ;
- double d1, d2 ;
-
- double r =1e10 ;
- iter =0 ;
- while (vpMath::equal(r_1,r,threshold_displacement) == false )
- {
- r_1 =r ;
+ vpColVector res(n);
+ vpColVector w(n);
+ w = 1;
+ robust.setThreshold(0.0000);
+ vpMatrix W(2 * n, 2 * n);
+ W = 0;
+
+ vpColVector N1(3), N2(3);
+ double d1, d2;
+
+ double r = 1e10;
+ iter = 0;
+ while (vpMath::equal(r_1, r, threshold_displacement) == false) {
+ r_1 = r;
// compute current position
- //Change frame (current)
- vpHomogeneousMatrix c1Mc2, c2Mo ;
- vpRotationMatrix c1Rc2, c2Rc1 ;
- vpTranslationVector c1Tc2, c2Tc1 ;
- c1Mc2 = c2Mc1.inverse() ;
- c2Mc1.extract(c2Rc1) ;
- c2Mc1.extract(c2Tc1) ;
- c2Mc1.extract(c1Rc2) ;
- c1Mc2.extract(c1Tc2) ;
+ // Change frame (current)
+ vpHomogeneousMatrix c1Mc2, c2Mo;
+ vpRotationMatrix c1Rc2, c2Rc1;
+ vpTranslationVector c1Tc2, c2Tc1;
+ c1Mc2 = c2Mc1.inverse();
+ c2Mc1.extract(c2Rc1);
+ c2Mc1.extract(c2Tc1);
+ c2Mc1.extract(c1Rc2);
+ c1Mc2.extract(c1Tc2);
- c2Mo = c2Mc1*c1Mo ;
+ c2Mo = c2Mc1 * c1Mo;
- getPlaneInfo(oN, c1Mo, N1, d1) ;
- getPlaneInfo(oN, c2Mo, N2, d2) ;
+ getPlaneInfo(oN, c1Mo, N1, d1);
+ getPlaneInfo(oN, c2Mo, N2, d2);
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (unsigned int i=0 ; i < nbpoint ; i++)
- {
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
+ vpMatrix L(2, 3), Lp;
+ int k = 0;
+ for (unsigned int i = 0; i < nbpoint; i++) {
+ p2[0] = c2P[i].get_x();
+ p2[1] = c2P[i].get_y();
+ p2[2] = 1.0;
+ p1[0] = c1P[i].get_x();
+ p1[1] = c1P[i].get_y();
+ p1[2] = 1.0;
- vpMatrix H(3,3) ;
+ vpMatrix H(3, 3);
- Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
- Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
+ Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
+ Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
+ Hp2 /= Hp2[2]; // normalisation
+ Hp1 /= Hp1[2];
// set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
- double Z1 ;
-
- Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
-
-
- H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
- H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
- H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
- H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
- H2 *=-1 ;
-
- vpMatrix c1CFc2(6,6) ;
+ double x = Hp2[0];
+ double y = Hp2[1];
+ double Z1;
+
+ Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
+
+ H2[0][0] = -Z1;
+ H2[0][1] = 0;
+ H2[0][2] = x * Z1;
+ H2[1][0] = 0;
+ H2[1][1] = -Z1;
+ H2[1][2] = y * Z1;
+ H2[0][3] = x * y;
+ H2[0][4] = -(1 + x * x);
+ H2[0][5] = y;
+ H2[1][3] = 1 + y * y;
+ H2[1][4] = -x * y;
+ H2[1][5] = -x;
+ H2 *= -1;
+
+ vpMatrix c1CFc2(6, 6);
{
- vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
- for (unsigned int k_=0 ; k_ < 3 ; k_++)
- for (unsigned int l=0 ; l<3 ; l++)
- {
- c1CFc2[k_][l] = c1Rc2[k_][l] ;
- c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
- c1CFc2[k_][l+3] = sTR[k_][l] ;
+ vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
+ for (unsigned int k_ = 0; k_ < 3; k_++)
+ for (unsigned int l = 0; l < 3; l++) {
+ c1CFc2[k_][l] = c1Rc2[k_][l];
+ c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
+ c1CFc2[k_][l + 3] = sTR[k_][l];
}
}
- H2 = H2*c1CFc2 ;
+ H2 = H2 * c1CFc2;
// Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
-
- x = Hp1[0] ;
- y = Hp1[1] ;
-
- Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
-
- H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
- H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
- H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
- H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
+ e2[0] = Hp2[0] - c1P[i].get_x();
+ e2[1] = Hp2[1] - c1P[i].get_y();
+
+ x = Hp1[0];
+ y = Hp1[1];
+
+ Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
+
+ H1[0][0] = -Z1;
+ H1[0][1] = 0;
+ H1[0][2] = x * Z1;
+ H1[1][0] = 0;
+ H1[1][1] = -Z1;
+ H1[1][2] = y * Z1;
+ H1[0][3] = x * y;
+ H1[0][4] = -(1 + x * x);
+ H1[0][5] = y;
+ H1[1][3] = 1 + y * y;
+ H1[1][4] = -x * y;
+ H1[1][5] = -x;
// Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
- if (only_2)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
+ e1[0] = Hp1[0] - c2P[i].get_x();
+ e1[1] = Hp1[1] - c2P[i].get_y();
+
+ if (only_2) {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
- }
- else
- if (only_1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
- }
+ } else if (only_1) {
+ if (k == 0) {
+ L = H1;
+ e = e1;
+ } else {
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
}
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
- }
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
+ } else {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
+ }
-
- k++ ;
+ k++;
}
- if (userobust)
- {
+ if (userobust) {
robust.setIteration(0);
- for (unsigned int l=0 ; l < n ; l++)
- {
- res[l] = vpMath::sqr(e[2*l]) + vpMath::sqr(e[2*l+1]) ;
+ for (unsigned int l = 0; l < n; l++) {
+ res[l] = vpMath::sqr(e[2 * l]) + vpMath::sqr(e[2 * l + 1]);
}
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
- for (unsigned int l=0 ; l < n ; l++)
- {
- W[2*l][2*l] = w[l] ;
- W[2*l+1][2*l+1] = w[l] ;
+ for (unsigned int l = 0; l < n; l++) {
+ W[2 * l][2 * l] = w[l];
+ W[2 * l + 1][2 * l + 1] = w[l];
}
+ } else {
+ for (unsigned int l = 0; l < 2 * n; l++)
+ W[l][l] = 1;
}
- else
- {
- for (unsigned int l=0 ; l < 2*n ; l++) W[l][l] = 1 ;
- }
- (W*L).pseudoInverse(Lp, 1e-16) ;
+ (W * L).pseudoInverse(Lp, 1e-16);
// Compute the camera velocity
- vpColVector c2Tcc1 ;
+ vpColVector c2Tcc1;
- c2Tcc1 = -1*Lp*W*e ;
+ c2Tcc1 = -1 * Lp * W * e;
// only for simulation
- c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
+ c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
+ ;
// UpdatePose2(c2Tcc1, c2Mc1) ;
- r =(W*e).sumSquare() ;
+ r = (W * e).sumSquare();
- if (r < 1e-15) {break ; }
- if (iter>1000){break ; }
- if (r>r_1) { break ; }
- iter++ ;
+ if (r < 1e-15) {
+ break;
+ }
+ if (iter > 1000) {
+ break;
+ }
+ if (r > r_1) {
+ break;
+ }
+ iter++;
}
- return (W*e).sumSquare() ;
-
+ 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);
+ double r_1 = -1;
- vpColVector e(2) ;
- double r_1 = -1 ;
+ vpColVector p2(3);
+ vpColVector p1(3);
+ vpColVector Hp2(3);
+ vpColVector Hp1(3);
- vpColVector p2(3) ;
- vpColVector p1(3) ;
- vpColVector Hp2(3) ;
- vpColVector Hp1(3) ;
-
- vpMatrix H2(2,6) ;
- vpColVector e2(2) ;
- vpMatrix H1(2,6) ;
- vpColVector e1(2) ;
+ vpMatrix H2(2, 6);
+ vpColVector e2(2);
+ vpMatrix H1(2, 6);
+ vpColVector e1(2);
bool only_1 = true;
bool only_2 = false;
- int iter = 0 ;
- unsigned int i ;
- unsigned int n=0 ;
- n = nbpoint ;
+ int iter = 0;
+ unsigned int i;
+ unsigned int n = 0;
+ n = nbpoint;
// next 2 lines are useless (detected by Coverity Scan)
- //if ( (! only_1) && (! only_2) )
+ // if ( (! only_1) && (! only_2) )
// n *=2 ;
vpRobust robust(n);
- vpColVector res(n) ;
- vpColVector w(n) ;
- w =1 ;
- robust.setThreshold(0.0000) ;
- vpMatrix W(2*n,2*n) ;
- W = 0 ;
-
- vpColVector N1(3), N2(3) ;
- double d1, d2 ;
-
- double r =1e10 ;
- iter =0 ;
- while (vpMath::equal(r_1,r,threshold_displacement) == false )
- {
- r_1 =r ;
+ vpColVector res(n);
+ vpColVector w(n);
+ w = 1;
+ robust.setThreshold(0.0000);
+ vpMatrix W(2 * n, 2 * n);
+ W = 0;
+
+ vpColVector N1(3), N2(3);
+ double d1, d2;
+
+ double r = 1e10;
+ iter = 0;
+ while (vpMath::equal(r_1, r, threshold_displacement) == false) {
+ r_1 = r;
// compute current position
- //Change frame (current)
- vpHomogeneousMatrix c1Mc2, c2Mo ;
- vpRotationMatrix c1Rc2, c2Rc1 ;
- vpTranslationVector c1Tc2, c2Tc1 ;
- c1Mc2 = c2Mc1.inverse() ;
- c2Mc1.extract(c2Rc1) ;
- c2Mc1.extract(c2Tc1) ;
- c2Mc1.extract(c1Rc2) ;
- c1Mc2.extract(c1Tc2) ;
-
- c2Mo = c2Mc1*c1Mo ;
-
- vpMatrix L(2,3), Lp ;
- int k =0 ;
- for (i=0 ; i < nbpoint ; i++)
- {
- getPlaneInfo(oN[i], c1Mo, N1, d1) ;
- getPlaneInfo(oN[i], c2Mo, N2, d2) ;
- p2[0] = c2P[i].get_x() ;
- p2[1] = c2P[i].get_y() ;
- p2[2] = 1.0 ;
- p1[0] = c1P[i].get_x() ;
- p1[1] = c1P[i].get_y() ;
- p1[2] = 1.0 ;
-
- vpMatrix H(3,3) ;
-
- Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2*N2.t())/d2)*p2 ; // p2 = Hp1
- Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1*N1.t())/d1)*p1 ; // p1 = Hp2
-
- Hp2 /= Hp2[2] ; // normalisation
- Hp1 /= Hp1[2] ;
+ // Change frame (current)
+ vpHomogeneousMatrix c1Mc2, c2Mo;
+ vpRotationMatrix c1Rc2, c2Rc1;
+ vpTranslationVector c1Tc2, c2Tc1;
+ c1Mc2 = c2Mc1.inverse();
+ c2Mc1.extract(c2Rc1);
+ c2Mc1.extract(c2Tc1);
+ c2Mc1.extract(c1Rc2);
+ c1Mc2.extract(c1Tc2);
+
+ c2Mo = c2Mc1 * c1Mo;
+
+ vpMatrix L(2, 3), Lp;
+ int k = 0;
+ for (i = 0; i < nbpoint; i++) {
+ getPlaneInfo(oN[i], c1Mo, N1, d1);
+ getPlaneInfo(oN[i], c2Mo, N2, d2);
+ p2[0] = c2P[i].get_x();
+ p2[1] = c2P[i].get_y();
+ p2[2] = 1.0;
+ p1[0] = c1P[i].get_x();
+ p1[1] = c1P[i].get_y();
+ p1[2] = 1.0;
+
+ vpMatrix H(3, 3);
+
+ Hp2 = ((vpMatrix)c1Rc2 + (c1Tc2 * N2.t()) / d2) * p2; // p2 = Hp1
+ Hp1 = ((vpMatrix)c2Rc1 + (c2Tc1 * N1.t()) / d1) * p1; // p1 = Hp2
+
+ Hp2 /= Hp2[2]; // normalisation
+ Hp1 /= Hp1[2];
// set up the interaction matrix
- double x = Hp2[0] ;
- double y = Hp2[1] ;
- double Z1 ;
-
- Z1 = (N1[0]*x+N1[1]*y+N1[2])/d1 ; // 1/z
-
- H2[0][0] = -Z1 ; H2[0][1] = 0 ; H2[0][2] = x*Z1 ;
- H2[1][0] = 0 ; H2[1][1] = -Z1 ; H2[1][2] = y*Z1 ;
- H2[0][3] = x*y ; H2[0][4] = -(1+x*x) ; H2[0][5] = y ;
- H2[1][3] = 1+y*y ; H2[1][4] = -x*y ; H2[1][5] = -x ;
- H2 *=-1 ;
-
- vpMatrix c1CFc2(6,6) ;
+ double x = Hp2[0];
+ double y = Hp2[1];
+ double Z1;
+
+ Z1 = (N1[0] * x + N1[1] * y + N1[2]) / d1; // 1/z
+
+ H2[0][0] = -Z1;
+ H2[0][1] = 0;
+ H2[0][2] = x * Z1;
+ H2[1][0] = 0;
+ H2[1][1] = -Z1;
+ H2[1][2] = y * Z1;
+ H2[0][3] = x * y;
+ H2[0][4] = -(1 + x * x);
+ H2[0][5] = y;
+ H2[1][3] = 1 + y * y;
+ H2[1][4] = -x * y;
+ H2[1][5] = -x;
+ H2 *= -1;
+
+ vpMatrix c1CFc2(6, 6);
{
- vpMatrix sTR = c1Tc2.skew()*(vpMatrix)c1Rc2 ;
- for (unsigned int k_=0 ; k_ < 3 ; k_++)
- for (unsigned int l=0 ; l<3 ; l++)
- {
- c1CFc2[k_][l] = c1Rc2[k_][l] ;
- c1CFc2[k_+3][l+3] = c1Rc2[k_][l] ;
- c1CFc2[k_][l+3] = sTR[k_][l] ;
+ vpMatrix sTR = c1Tc2.skew() * (vpMatrix)c1Rc2;
+ for (unsigned int k_ = 0; k_ < 3; k_++)
+ for (unsigned int l = 0; l < 3; l++) {
+ c1CFc2[k_][l] = c1Rc2[k_][l];
+ c1CFc2[k_ + 3][l + 3] = c1Rc2[k_][l];
+ c1CFc2[k_][l + 3] = sTR[k_][l];
}
}
- H2 = H2*c1CFc2 ;
+ H2 = H2 * c1CFc2;
// Set up the error vector
- e2[0] = Hp2[0] - c1P[i].get_x() ;
- e2[1] = Hp2[1] - c1P[i].get_y() ;
-
- x = Hp1[0] ;
- y = Hp1[1] ;
-
- Z1 = (N2[0]*x+N2[1]*y+N2[2])/d2 ; // 1/z
-
- H1[0][0] = -Z1 ; H1[0][1] = 0 ; H1[0][2] = x*Z1 ;
- H1[1][0] = 0 ; H1[1][1] = -Z1 ; H1[1][2] = y*Z1;
- H1[0][3] = x*y ; H1[0][4] = -(1+x*x) ; H1[0][5] = y ;
- H1[1][3] = 1+y*y ; H1[1][4] = -x*y ; H1[1][5] = -x ;
+ e2[0] = Hp2[0] - c1P[i].get_x();
+ e2[1] = Hp2[1] - c1P[i].get_y();
+
+ x = Hp1[0];
+ y = Hp1[1];
+
+ Z1 = (N2[0] * x + N2[1] * y + N2[2]) / d2; // 1/z
+
+ H1[0][0] = -Z1;
+ H1[0][1] = 0;
+ H1[0][2] = x * Z1;
+ H1[1][0] = 0;
+ H1[1][1] = -Z1;
+ H1[1][2] = y * Z1;
+ H1[0][3] = x * y;
+ H1[0][4] = -(1 + x * x);
+ H1[0][5] = y;
+ H1[1][3] = 1 + y * y;
+ H1[1][4] = -x * y;
+ H1[1][5] = -x;
// Set up the error vector
- e1[0] = Hp1[0] - c2P[i].get_x() ;
- e1[1] = Hp1[1] - c2P[i].get_y() ;
-
-
- if (only_2)
- {
- if (k == 0) { L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
+ e1[0] = Hp1[0] - c2P[i].get_x();
+ e1[1] = Hp1[1] - c2P[i].get_y();
+
+ if (only_2) {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
- }
- else
- if (only_1)
- {
- if (k == 0) { L = H1 ; e= e1 ; }
- else
- {
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
- }
+ } else if (only_1) {
+ if (k == 0) {
+ L = H1;
+ e = e1;
+ } else {
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
}
- else
- {
- if (k == 0) {L = H2 ; e = e2 ; }
- else
- {
- L = vpMatrix::stack(L,H2) ;
- e = vpColVector::stack(e,e2) ;
- }
- L = vpMatrix::stack(L,H1) ;
- e = vpColVector::stack(e,e1) ;
+ } else {
+ if (k == 0) {
+ L = H2;
+ e = e2;
+ } else {
+ L = vpMatrix::stack(L, H2);
+ e = vpColVector::stack(e, e2);
}
+ L = vpMatrix::stack(L, H1);
+ e = vpColVector::stack(e, e1);
+ }
-
- k++ ;
+ k++;
}
- if (userobust)
- {
+ if (userobust) {
robust.setIteration(0);
- for (unsigned int k_=0 ; k_ < n ; k_++)
- {
- res[k_] = vpMath::sqr(e[2*k_]) + vpMath::sqr(e[2*k_+1]) ;
+ for (unsigned int k_ = 0; k_ < n; k_++) {
+ res[k_] = vpMath::sqr(e[2 * k_]) + vpMath::sqr(e[2 * k_ + 1]);
}
robust.MEstimator(vpRobust::TUKEY, res, w);
-
// compute the pseudo inverse of the interaction matrix
- for (unsigned int k_=0 ; k_ < n ; k_++)
- {
- W[2*k_][2*k_] = w[k_] ;
- W[2*k_+1][2*k_+1] = w[k_] ;
+ for (unsigned int k_ = 0; k_ < n; k_++) {
+ W[2 * k_][2 * k_] = w[k_];
+ W[2 * k_ + 1][2 * k_ + 1] = w[k_];
}
+ } else {
+ for (unsigned int k_ = 0; k_ < 2 * n; k_++)
+ W[k_][k_] = 1;
}
- else
- {
- for (unsigned int k_=0 ; k_ < 2*n ; k_++) W[k_][k_] = 1 ;
- }
- (W*L).pseudoInverse(Lp, 1e-16) ;
+ (W * L).pseudoInverse(Lp, 1e-16);
// Compute the camera velocity
- vpColVector c2Tcc1 ;
+ vpColVector c2Tcc1;
- c2Tcc1 = -1*Lp*W*e ;
+ c2Tcc1 = -1 * Lp * W * e;
// only for simulation
- c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse()*c2Mc1 ; ;
+ c2Mc1 = vpExponentialMap::direct(c2Tcc1).inverse() * c2Mc1;
+ ;
// UpdatePose2(c2Tcc1, c2Mc1) ;
- r =(W*e).sumSquare() ;
-
+ r = (W * e).sumSquare();
- if (r < 1e-15) {break ; }
- if (iter>1000){break ; }
- if (r>r_1) { break ; }
- iter++ ;
+ if (r < 1e-15) {
+ break;
+ }
+ if (iter > 1000) {
+ break;
+ }
+ if (r > r_1) {
+ break;
+ }
+ iter++;
}
- return (W*e).sumSquare() ;
-
+ return (W * e).sumSquare();
}
#endif //#ifndef DOXYGEN_SHOULD_SKIP_THIS
diff --git a/modules/vision/src/key-point/vpBasicKeyPoint.cpp b/modules/vision/src/key-point/vpBasicKeyPoint.cpp
index 8188416..305ea1f 100644
--- a/modules/vision/src/key-point/vpBasicKeyPoint.cpp
+++ b/modules/vision/src/key-point/vpBasicKeyPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#include <visp3/vision/vpBasicKeyPoint.h>
/*!
@@ -45,5 +45,3 @@ vpBasicKeyPoint::vpBasicKeyPoint()
: referenceImagePointsList(), currentImagePointsList(), matchedReferencePoints(), _reference_computed(false)
{
}
-
-
diff --git a/modules/vision/src/key-point/vpFernClassifier.cpp b/modules/vision/src/key-point/vpFernClassifier.cpp
index ebdc48f..56be542 100644
--- a/modules/vision/src/key-point/vpFernClassifier.cpp
+++ b/modules/vision/src/key-point/vpFernClassifier.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,89 +39,87 @@
#include <visp3/core/vpConfig.h>
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-#include <visp3/vision/vpFernClassifier.h>
-#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpColor.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpImageTools.h>
+#include <visp3/vision/vpFernClassifier.h>
/*!
- Basic constructor
-
+ Basic constructor
+
*/
vpFernClassifier::vpFernClassifier()
- : vpBasicKeyPoint(),
- ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
- hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
- nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
- sigmaBlur(1), nbMinPoint(10),
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+ : vpBasicKeyPoint(), ldetector(), fernClassifier(),
+ gen(0, 256, 5, true, 0.6, 1.5, -CV_PI / 2, CV_PI / 2, -CV_PI / 2, CV_PI / 2), hasLearn(false), threshold(20),
+ nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11), nbOctave(2), patchSize(32), radius(7), nbPoints(200),
+ blurImage(true), radiusBlur(7), sigmaBlur(1), nbMinPoint(10),
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
curImg(),
- #else
+#else
curImg(NULL),
- #endif
- objKeypoints(), modelROI_Ref(), modelROI(),
- modelPoints(), imgKeypoints(), refPt(), curPt()
+#endif
+ objKeypoints(), modelROI_Ref(), modelROI(), modelPoints(), imgKeypoints(), refPt(), curPt()
{
}
/*!
- Constructor using the provided data file to load the object given by its name.
-
- If the name of the object does not correspond in the file, a
+ Constructor using the provided data file to load the object given by its
+ name.
+
+ If the name of the object does not correspond in the file, a
vpException::ioError is thrown.
-
- \param _dataFile : the name of the file saved after a training (with the
+
+ \param _dataFile : the name of the file saved after a training (with the
record methods).
\param _objectName : the name of the object to load
*/
-vpFernClassifier::vpFernClassifier(const std::string& _dataFile, const std::string& _objectName)
- : vpBasicKeyPoint(),
- ldetector(), fernClassifier(), gen(0, 256, 5, true, 0.6, 1.5, -CV_PI/2, CV_PI/2, -CV_PI/2, CV_PI/2),
- hasLearn(false), threshold(20), nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11),
- nbOctave(2), patchSize(32), radius(7), nbPoints(200), blurImage(true), radiusBlur(7),
- sigmaBlur(1), nbMinPoint(10),
- #if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
+vpFernClassifier::vpFernClassifier(const std::string &_dataFile, const std::string &_objectName)
+ : vpBasicKeyPoint(), ldetector(), fernClassifier(),
+ gen(0, 256, 5, true, 0.6, 1.5, -CV_PI / 2, CV_PI / 2, -CV_PI / 2, CV_PI / 2), hasLearn(false), threshold(20),
+ nbView(2000), dist(2), nbClassfier(100), ClassifierSize(11), nbOctave(2), patchSize(32), radius(7), nbPoints(200),
+ blurImage(true), radiusBlur(7), sigmaBlur(1), nbMinPoint(10),
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
curImg(),
- #else
+#else
curImg(NULL),
- #endif
- objKeypoints(), modelROI_Ref(), modelROI(),
- modelPoints(), imgKeypoints(), refPt(), curPt()
+#endif
+ objKeypoints(), modelROI_Ref(), modelROI(), modelPoints(), imgKeypoints(), refPt(), curPt()
{
- this->load(_dataFile, _objectName);
+ this->load(_dataFile, _objectName);
}
/*!
Basic destructor
-
+
*/
vpFernClassifier::~vpFernClassifier()
{
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- if(curImg != NULL){
- if(curImg->width % 8 == 0){
+ if (curImg != NULL) {
+ if (curImg->width % 8 == 0) {
curImg->imageData = NULL;
cvReleaseImageHeader(&curImg);
- }else{
+ } else {
cvReleaseImage(&curImg);
}
curImg = NULL;
- }
+ }
#endif
}
/*!
initialise any OpenCV parameters
-
- The classifier need to be trained again or to be loaded from an external file.
+
+ The classifier need to be trained again or to be loaded from an external
+ file.
*/
-void
-vpFernClassifier::init()
+void vpFernClassifier::init()
{
- hasLearn = false;
+ hasLearn = false;
nbClassfier = 100;
ClassifierSize = 11;
nbPoints = 200;
@@ -135,77 +134,69 @@ vpFernClassifier::init()
threshold = 20;
nbOctave = 2;
nbView = 2000;
- dist = 2;
+ dist = 2;
nbMinPoint = 10;
}
-
-
-
-/*!
+/*!
Train the classifier.
*/
-void
-vpFernClassifier::train()
+void vpFernClassifier::train()
{
- // initialise detector
+ // initialise detector
cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
-
- //blur
+
+ // blur
cv::Mat obj = (cv::Mat)curImg;
-
- if(this->getBlurSetting()){
+
+ if (this->getBlurSetting()) {
cv::GaussianBlur(obj, obj, cv::Size(getBlurSize(), getBlurSize()), getBlurSigma(), getBlurSigma());
}
-
- // build pyramid
+
+ // build pyramid
std::vector<cv::Mat> objpyr;
- cv::buildPyramid(obj, objpyr, d.nOctaves-1);
-
- // getPoints
+ cv::buildPyramid(obj, objpyr, d.nOctaves - 1);
+
+ // getPoints
d.getMostStable2D(obj, objKeypoints, 100, gen);
-
+
ldetector = d;
-
- // train classifier
+
+ // train classifier
modelROI = cv::Rect(0, 0, objpyr[0].cols, objpyr[0].rows);
ldetector.getMostStable2D(objpyr[0], modelPoints, 100, gen);
-
- fernClassifier.trainFromSingleView(objpyr[0], modelPoints,
- patchSize, (int)modelPoints.size(), 100, 11, 10000,
+
+ 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);
+ for (unsigned int i = 0; i < modelPoints.size(); i += 1) {
+ vpImagePoint ip(modelPoints[i].pt.y + modelROI_Ref.y, modelPoints[i].pt.x + modelROI_Ref.x);
referenceImagePointsList.push_back(ip);
}
-
- // set flag
+
+ // set flag
hasLearn = true;
}
/*!
Build the list of reference points. The computation of the points is
- made all over the image I. It also includes the training of the fern
+ made all over the image I. It also includes the training of the fern
classifier.
-
- \warning thie method can take up to several minutes depending on the
+
+ \warning thie method can take up to several minutes depending on the
parameters of the classifier and on the size of the image.
\param _I : The gray scaled image where the reference points are computed.
\return the number of reference points.
*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
+unsigned int vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
{
this->setImage(_I);
-
+
train();
_reference_computed = true;
@@ -217,10 +208,10 @@ vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
Build the list of reference points. The computation of the points is
made only on a part of the image. This part is a rectangle defined
by its top left corner, its height and its width. The parameters of
- this rectangle must be given in pixel. It also includes the training of the
+ this rectangle must be given in pixel. It also includes the training of the
fern classifier.
-
- \warning the method can take up to several minutes depending on the
+
+ \warning the method can take up to several minutes depending on the
parameters of the classifier and on the size of the image.
\param _I : The gray scaled image where the reference points are computed.
@@ -230,45 +221,38 @@ vpFernClassifier::buildReference(const vpImage<unsigned char> &_I)
\return the number of reference points.
*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- const unsigned int _height, const unsigned int _width)
+unsigned int vpFernClassifier::buildReference(const vpImage<unsigned char> &_I, const vpImagePoint &_iP,
+ const unsigned int _height, const unsigned int _width)
{
- if((_iP.get_i()+_height) >= _I.getHeight()
- || (_iP.get_j()+_width) >= _I.getWidth()) {
+ 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<unsigned char> 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
- reference image */
+
+ /* initialise a structure containing the region of interest used in the
+ reference image */
modelROI_Ref.x = (int)_iP.get_u();
modelROI_Ref.y = (int)_iP.get_v();
modelROI_Ref.width = (int)_width;
- modelROI_Ref.height = (int)_height;
-
+ modelROI_Ref.height = (int)_height;
+
train();
- return (unsigned int)objKeypoints.size();
+ return (unsigned int)objKeypoints.size();
}
/*!
Build the list of reference points. The computation of the points is
made only on a part of the image. This part is a rectangle. The
- parameters of this rectangle must be given in pixel. It also includes the
+ parameters of this rectangle must be given in pixel. It also includes the
training of the fern classifier.
-
- \warning the method can take up to several minutes depending on the
+
+ \warning the method can take up to several minutes depending on the
parameters of the classifier and on the size of the image.
\param _I : The gray scaled image where the reference points are computed.
@@ -277,50 +261,47 @@ vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
\return The number of reference points.
*/
-unsigned int
-vpFernClassifier::buildReference(const vpImage<unsigned char> &_I,
- const vpRect& _rectangle)
+unsigned int vpFernClassifier::buildReference(const vpImage<unsigned char> &_I, const vpRect &_rectangle)
{
vpImagePoint iP;
iP.set_i(_rectangle.getTop());
iP.set_j(_rectangle.getLeft());
- return (this->buildReference(_I, iP,
- (unsigned int)_rectangle.getHeight(),
- (unsigned int)_rectangle.getWidth()));
+ return (this->buildReference(_I, iP, (unsigned int)_rectangle.getHeight(), (unsigned int)_rectangle.getWidth()));
}
/*!
- Compute the points of interest in the current image and try to match them
- using the trained classifier. The matched pairs can be found with the
- getMatchedPointByRef function.
-
- \exception vpException::notInitialised if the classifier has not been trained.
-
+ Compute the points of interest in the current image and try to match them
+ using the trained classifier. The matched pairs can be found with the
+ getMatchedPointByRef function.
+
+ \exception vpException::notInitialised if the classifier has not been
+ trained.
+
\param _I : The gray scaled image where the points are computed.
\return The number of point which have been matched.
*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
+unsigned int vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
{
- if(!hasLearn){
+ 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);
- // resize image
-// cv::resize(_image, image, Size(), 1./imgscale, 1./imgscale, INTER_CUBIC);
+ // resize image
+ // cv::resize(_image, image, Size(), 1./imgscale, 1./imgscale,
+ // INTER_CUBIC);
cv::Mat img = this->curImg;
-
- if(this->getBlurSetting()){
- cv::GaussianBlur(img, img, cv::Size(this->getBlurSize(), this->getBlurSize()), this->getBlurSigma(), this->getBlurSigma());
+
+ if (this->getBlurSetting()) {
+ cv::GaussianBlur(img, img, cv::Size(this->getBlurSize(), this->getBlurSize()), this->getBlurSigma(),
+ this->getBlurSigma());
}
-
+
std::vector<cv::Mat> imgPyr;
- cv::buildPyramid(img, imgPyr, ldetector.nOctaves-1);
-
-
+ cv::buildPyramid(img, imgPyr, ldetector.nOctaves - 1);
+
ldetector(imgPyr, imgKeypoints, 500);
unsigned int m = (unsigned int)modelPoints.size();
@@ -330,33 +311,31 @@ vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
std::vector<float> signature;
unsigned int totalMatch = 0;
-
/* part of code from OpenCV planarObjectDetector */
currentImagePointsList.resize(0);
matchedReferencePoints.resize(0);
- for(unsigned int i = 0; i < n; i++ ){
+ for (unsigned int i = 0; i < n; i++) {
cv::KeyPoint kpt = imgKeypoints[i];
kpt.pt.x /= (float)(1 << kpt.octave);
kpt.pt.y /= (float)(1 << kpt.octave);
int k = fernClassifier(imgPyr[(unsigned int)kpt.octave], kpt.pt, signature);
- if( k >= 0 && (bestMatches[(unsigned int)k] < 0 || signature[(unsigned int)k] > maxLogProb[(unsigned int)k]) ){
+ if (k >= 0 && (bestMatches[(unsigned int)k] < 0 || signature[(unsigned int)k] > maxLogProb[(unsigned int)k])) {
maxLogProb[(unsigned int)k] = signature[(unsigned int)k];
bestMatches[(unsigned int)k] = (int)i;
totalMatch++;
-
- vpImagePoint ip_cur( imgKeypoints[i].pt.y, imgKeypoints[i].pt.x);
-
+
+ vpImagePoint ip_cur(imgKeypoints[i].pt.y, imgKeypoints[i].pt.x);
+
currentImagePointsList.push_back(ip_cur);
matchedReferencePoints.push_back((unsigned int)k);
-
}
}
refPt.resize(0);
curPt.resize(0);
- for(unsigned int i = 0; i < m; i++ ){
- if( bestMatches[i] >= 0 ){
+ for (unsigned int i = 0; i < m; i++) {
+ if (bestMatches[i] >= 0) {
refPt.push_back(modelPoints[i].pt);
curPt.push_back(imgKeypoints[(unsigned int)bestMatches[i]].pt);
}
@@ -365,14 +344,12 @@ vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
return totalMatch;
}
-
-
/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs
+ can be found with the getMatchedPointByRef function. The homography between
+ the two planar surfaces is also computed.
+
\param _I : The gray scaled image where the points are computed.
\param _iP : the top left corner of the rectangle defining the ROI
\param _height : the height of the ROI
@@ -380,52 +357,40 @@ vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I)
\return the number of point which have been matched.
*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- const unsigned int _height, const unsigned int _width)
+unsigned int vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I, const vpImagePoint &_iP,
+ const unsigned int _height, const unsigned int _width)
{
- if((_iP.get_i()+_height) >= _I.getHeight()
- || (_iP.get_j()+_width) >= _I.getWidth()) {
+ 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<unsigned char> 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);
}
/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs
+ can be found with the getMatchedPointByRef function. The homography between
+ the two planar surfaces is also computed.
+
\param _I : The gray scaled image where the points are computed.
\param _rectangle : the rectangle defining the ROI
\return the number of point which have been matched.
*/
-unsigned int
-vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
- const vpRect& _rectangle)
+unsigned int vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I, const vpRect &_rectangle)
{
vpImagePoint iP;
iP.set_i(_rectangle.getTop());
iP.set_j(_rectangle.getLeft());
- return (this->matchPoint(_I, iP,
- (unsigned int)_rectangle.getHeight(),
- (unsigned int)_rectangle.getWidth()));
+ return (this->matchPoint(_I, iP, (unsigned int)_rectangle.getHeight(), (unsigned int)_rectangle.getWidth()));
}
-
/*!
This function displays the matched reference points and the matched
points computed in the current image. The reference points are
@@ -440,36 +405,33 @@ vpFernClassifier::matchPoint(const vpImage<unsigned char> &_I,
\param _Icurrent : The image where the matched points computed in the
current image are displayed.
- \param size : Size in pixels of the cross that is used to display matched points.
+ \param size : Size in pixels of the cross that is used to display matched
+ points.
*/
-void
-vpFernClassifier::display(const vpImage<unsigned char> &_Iref,
- const vpImage<unsigned char> &_Icurrent, unsigned int size)
+void vpFernClassifier::display(const vpImage<unsigned char> &_Iref, const vpImage<unsigned char> &_Icurrent,
+ unsigned int size)
{
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
- vpDisplay::displayCross (_Iref, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
- vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, vpColor::green);
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
+ vpDisplay::displayCross(_Iref, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
+ vpDisplay::displayCross(_Icurrent, currentImagePointsList[i], size, vpColor::green);
}
-
}
-
/*!
This function displays only the matched points computed in the
current image. They are displayed in the image Icurrent.
-
+
\param _Icurrent : the gray scaled image for the display
- \param size : Size in pixels of the cross that is used to display matched points.
+ \param size : Size in pixels of the cross that is used to display matched
+ points.
\param color : Color used to display the matched points.
*/
-void
-vpFernClassifier::display(const vpImage<unsigned char> &_Icurrent, unsigned int size,
- const vpColor &color)
-{
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++){
- vpDisplay::displayCross (_Icurrent, currentImagePointsList[i], size, color);
+void vpFernClassifier::display(const vpImage<unsigned char> &_Icurrent, unsigned int size, const vpColor &color)
+{
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
+ vpDisplay::displayCross(_Icurrent, currentImagePointsList[i], size, color);
}
}
@@ -477,50 +439,49 @@ vpFernClassifier::display(const vpImage<unsigned char> &_Icurrent, unsigned int
/*!
\brief load the Fern classifier
-
- Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
+
+ Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
can take up to sevral seconds.
-
- \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
+
+ \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;
-
+
/* part of code from OpenCV planarObjectDetector */
cv::FileStorage fs(_dataFile, cv::FileStorage::READ);
cv::FileNode node = fs.getFirstTopLevelNode();
-
+
cv::FileNodeIterator it = node["model-roi"].begin(), it_end;
it >> modelROI.x >> modelROI.y >> modelROI.width >> modelROI.height;
-
+
ldetector.read(node["detector"]);
fernClassifier.read(node["fern-classifier"]);
-
+
const cv::FileNode node_ = node["model-points"];
cv::read(node_, modelPoints);
-
+
cv::LDetector d(radius, threshold, nbOctave, nbView, patchSize, dist);
ldetector = d;
hasLearn = true;
}
-
/*!
\brief record the Ferns classifier in the text file
\param _objectName : The name of the object to store in the data file.
- \param _dataFile : The name of the data filename (very large text file). It can have any file extension.
+ \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);
-
+
cv::WriteStructContext ws(fs, _objectName, CV_NODE_MAP);
-
+
{
cv::WriteStructContext wsroi(fs, "model-roi", CV_NODE_SEQ + CV_NODE_FLOW);
cv::write(fs, modelROI_Ref.x);
@@ -528,56 +489,52 @@ vpFernClassifier::record(const std::string& _objectName, const std::string& _dat
cv::write(fs, modelROI_Ref.width);
cv::write(fs, modelROI_Ref.height);
}
-
+
ldetector.write(fs, "detector");
cv::write(fs, "model-points", modelPoints);
fernClassifier.write(fs, "fern-classifier");
}
-
-
/*!
Set the current image. This method allows to convert a image from the ViSP
- format to the OpenCV one.
-
+ format to the OpenCV one.
+
\param I : the input image.
*/
-void
-vpFernClassifier::setImage(const vpImage<unsigned char>& I)
-{
+void vpFernClassifier::setImage(const vpImage<unsigned char> &I)
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020408)
vpImageConvert::convert(I, curImg);
#else
- if(curImg != NULL){
+ if (curImg != NULL) {
cvResetImageROI(curImg);
- if((curImg->width % 8) == 0){
+ if ((curImg->width % 8) == 0) {
curImg->imageData = NULL;
cvReleaseImageHeader(&curImg);
- }else{
+ } else {
cvReleaseImage(&curImg);
}
curImg = NULL;
}
- if((I.getWidth()%8) == 0){
+ if ((I.getWidth() % 8) == 0) {
curImg = cvCreateImageHeader(cvSize((int)I.getWidth(), (int)I.getHeight()), IPL_DEPTH_8U, 1);
- if(curImg != NULL){
- curImg->imageData = (char*)I.bitmap;
- }else{
+ if (curImg != NULL) {
+ curImg->imageData = (char *)I.bitmap;
+ } else {
throw vpException(vpException::memoryAllocationError, "Could not create the image in the OpenCV format.");
}
- }else{
+ } else {
vpImageConvert::convert(I, curImg);
}
- if(curImg == NULL){
+ if (curImg == NULL) {
std::cout << "!> conversion failed" << std::endl;
- throw vpException(vpException::notInitialized , "conversion failed");
+ throw vpException(vpException::notInitialized, "conversion failed");
}
#endif
-
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_vision.a(vpFernClassifier.cpp.o) has no symbols
-void dummy_vpFernClassifier() {};
+// Work arround to avoid warning: libvisp_vision.a(vpFernClassifier.cpp.o) has
+// no symbols
+void dummy_vpFernClassifier(){};
#endif
-
diff --git a/modules/vision/src/key-point/vpKeyPoint.cpp b/modules/vision/src/key-point/vpKeyPoint.cpp
index 17b6373..15a75b9 100644
--- a/modules/vision/src/key-point/vpKeyPoint.cpp
+++ b/modules/vision/src/key-point/vpKeyPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,296 +36,276 @@
*
*****************************************************************************/
-#include <limits>
#include <iomanip>
+#include <limits>
#include <stdint.h> //uint32_t ; works also with >= VS2010 / _MSC_VER >= 1600
-#include <visp3/vision/vpKeyPoint.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/vision/vpKeyPoint.h>
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
-# include <opencv2/calib3d/calib3d.hpp>
+#include <opencv2/calib3d/calib3d.hpp>
#endif
-//Detect endianness of the host machine
-//Reference: http://www.boost.org/doc/libs/1_36_0/boost/detail/endian.hpp
-#if defined (__GLIBC__)
-# include <endian.h>
-# if (__BYTE_ORDER == __LITTLE_ENDIAN)
-# define VISP_LITTLE_ENDIAN
-# elif (__BYTE_ORDER == __BIG_ENDIAN)
-# define VISP_BIG_ENDIAN
-# elif (__BYTE_ORDER == __PDP_ENDIAN)
- //Currently not supported when reading / writing binary file
-# define VISP_PDP_ENDIAN
-# else
-# error Unknown machine endianness detected.
-# endif
-#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__)
-# define VISP_LITTLE_ENDIAN
-#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)
-
-# define VISP_LITTLE_ENDIAN
+// Detect endianness of the host machine
+// Reference: http://www.boost.org/doc/libs/1_36_0/boost/detail/endian.hpp
+#if defined(__GLIBC__)
+#include <endian.h>
+#if (__BYTE_ORDER == __LITTLE_ENDIAN)
+#define VISP_LITTLE_ENDIAN
+#elif (__BYTE_ORDER == __BIG_ENDIAN)
+#define VISP_BIG_ENDIAN
+#elif (__BYTE_ORDER == __PDP_ENDIAN)
+// Currently not supported when reading / writing binary file
+#define VISP_PDP_ENDIAN
#else
-# error Cannot detect host machine endianness.
+#error Unknown machine endianness detected.
#endif
+#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__)
+#define VISP_LITTLE_ENDIAN
+#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)
-namespace {
- //Specific Type transformation functions
- ///*!
- // Convert a list of cv::DMatch to a cv::DMatch (extract the first cv::DMatch, the nearest neighbor).
- //
- // \param knnMatches : List of cv::DMatch.
- // \return The nearest neighbor.
- // */
- inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches) {
- if(knnMatches.size() > 0) {
- return knnMatches[0];
- }
+#define VISP_LITTLE_ENDIAN
+#else
+#error Cannot detect host machine endianness.
+#endif
- return cv::DMatch();
+namespace
+{
+// Specific Type transformation functions
+///*!
+// Convert a list of cv::DMatch to a cv::DMatch (extract the first
+// cv::DMatch, the nearest neighbor).
+//
+// \param knnMatches : List of cv::DMatch.
+// \return The nearest neighbor.
+// */
+inline cv::DMatch knnToDMatch(const std::vector<cv::DMatch> &knnMatches)
+{
+ if (knnMatches.size() > 0) {
+ return knnMatches[0];
}
- ///*!
- // Convert a cv::DMatch to an index (extract the train index).
- //
- // \param match : Point to convert in ViSP type.
- // \return The train index.
- // */
- inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair) {
- return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
- }
+ return cv::DMatch();
+}
+
+///*!
+// Convert a cv::DMatch to an index (extract the train index).
+//
+// \param match : Point to convert in ViSP type.
+// \return The train index.
+// */
+inline vpImagePoint matchRansacToVpImage(const std::pair<cv::KeyPoint, cv::Point3f> &pair)
+{
+ return vpImagePoint(pair.first.pt.y, pair.first.pt.x);
+}
- //Keep this function to know how to detect big endian with code
- //bool isBigEndian() {
- // union {
- // uint32_t i;
- // char c[4];
- // } bint = { 0x01020304 };
- //
- // return bint.c[0] == 1;
- //}
+// Keep this function to know how to detect big endian with code
+// bool isBigEndian() {
+// union {
+// uint32_t i;
+// char c[4];
+// } bint = { 0x01020304 };
+//
+// return bint.c[0] == 1;
+//}
#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) );
- }
+// 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)); }
- //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) );
- }
+// 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));
+}
- //Swap a float, the union is necessary because of the representation of a float in memory in IEEE 754.
- float swapFloat(const float f) {
- union {
- float f;
- unsigned char b[4];
- } dat1, dat2;
-
- dat1.f = f;
- dat2.b[0] = dat1.b[3];
- dat2.b[1] = dat1.b[2];
- dat2.b[2] = dat1.b[1];
- dat2.b[3] = dat1.b[0];
- return dat2.f;
- }
+// Swap a float, the union is necessary because of the representation of a
+// float in memory in IEEE 754.
+float swapFloat(const float f)
+{
+ union {
+ float f;
+ unsigned char b[4];
+ } dat1, dat2;
+
+ dat1.f = f;
+ dat2.b[0] = dat1.b[3];
+ dat2.b[1] = dat1.b[2];
+ dat2.b[2] = dat1.b[1];
+ dat2.b[3] = dat1.b[0];
+ return dat2.f;
+}
- //Swap a double, the union is necessary because of the representation of a double in memory in IEEE 754.
- double swapDouble(const double d) {
- union {
- double d;
- unsigned char b[8];
- } dat1, dat2;
-
- dat1.d = d;
- dat2.b[0] = dat1.b[7];
- dat2.b[1] = dat1.b[6];
- dat2.b[2] = dat1.b[5];
- dat2.b[3] = dat1.b[4];
- dat2.b[4] = dat1.b[3];
- dat2.b[5] = dat1.b[2];
- dat2.b[6] = dat1.b[1];
- dat2.b[7] = dat1.b[0];
- return dat2.d;
- }
+// Swap a double, the union is necessary because of the representation of a
+// double in memory in IEEE 754.
+double swapDouble(const double d)
+{
+ union {
+ double d;
+ unsigned char b[8];
+ } dat1, dat2;
+
+ dat1.d = d;
+ dat2.b[0] = dat1.b[7];
+ dat2.b[1] = dat1.b[6];
+ dat2.b[2] = dat1.b[5];
+ dat2.b[3] = dat1.b[4];
+ dat2.b[4] = dat1.b[3];
+ dat2.b[5] = dat1.b[2];
+ dat2.b[6] = dat1.b[1];
+ dat2.b[7] = dat1.b[0];
+ return dat2.d;
+}
#endif
- //Read an unsigned short int stored in little endian
- void readBinaryUShortLE(std::ifstream &file, unsigned short &ushort_value) {
- //Read
- file.read((char *)(&ushort_value), sizeof(ushort_value));
+// Read an unsigned short int stored in little endian
+void readBinaryUShortLE(std::ifstream &file, unsigned short &ushort_value)
+{
+ // Read
+ file.read((char *)(&ushort_value), sizeof(ushort_value));
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order from little endian to big endian
- ushort_value = swap16bits(ushort_value);
- #endif
- }
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order from little endian to big endian
+ ushort_value = swap16bits(ushort_value);
+#endif
+}
- //Read a short int stored in little endian
- void readBinaryShortLE(std::ifstream &file, short &short_value) {
- //Read
- file.read((char *)(&short_value), sizeof(short_value));
+// Read a short int stored in little endian
+void readBinaryShortLE(std::ifstream &file, short &short_value)
+{
+ // Read
+ file.read((char *)(&short_value), sizeof(short_value));
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order from little endian to big endian
- short_value = (short) swap16bits((uint16_t) short_value);
- #endif
- }
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order from little endian to big endian
+ short_value = (short)swap16bits((uint16_t)short_value);
+#endif
+}
-// //Read an unsigned int stored in little endian
-// void readBinaryUIntLE(std::ifstream &file, unsigned int &uint_value) {
-// //Read
-// file.read((char *)(&uint_value), sizeof(uint_value));
-
-// #ifdef VISP_BIG_ENDIAN
-// //Swap bytes order from little endian to big endian
-// if(sizeof(uint_value) == 4) {
-// uint_value = swap32bits(uint_value);
-// } else {
-// uint_value = swap16bits(uint_value);
-// }
-// #endif
-// }
-
- //Read an int stored in little endian
- void readBinaryIntLE(std::ifstream &file, int &int_value) {
- //Read
- file.read((char *)(&int_value), sizeof(int_value));
-
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order from little endian to big endian
- if(sizeof(int_value) == 4) {
- int_value = (int) swap32bits((uint32_t) int_value);
- } else {
- int_value = swap16bits((uint16_t) int_value);
- }
- #endif
+// Read an int stored in little endian
+void readBinaryIntLE(std::ifstream &file, int &int_value)
+{
+ // Read
+ file.read((char *)(&int_value), sizeof(int_value));
+
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order from little endian to big endian
+ if (sizeof(int_value) == 4) {
+ int_value = (int)swap32bits((uint32_t)int_value);
+ } else {
+ int_value = swap16bits((uint16_t)int_value);
}
+#endif
+}
- //Read a float stored in little endian
- void readBinaryFloatLE(std::ifstream &file, float &float_value) {
- //Read
- file.read((char *)(&float_value), sizeof(float_value));
+// Read a float stored in little endian
+void readBinaryFloatLE(std::ifstream &file, float &float_value)
+{
+ // Read
+ file.read((char *)(&float_value), sizeof(float_value));
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order from little endian to big endian
- float_value = swapFloat(float_value);
- #endif
- }
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order from little endian to big endian
+ float_value = swapFloat(float_value);
+#endif
+}
- //Read a double stored in little endian
- void readBinaryDoubleLE(std::ifstream &file, double &double_value) {
- //Read
- file.read((char *)(&double_value), sizeof(double_value));
+// Read a double stored in little endian
+void readBinaryDoubleLE(std::ifstream &file, double &double_value)
+{
+ // Read
+ file.read((char *)(&double_value), sizeof(double_value));
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order from little endian to big endian
- double_value = swapDouble(double_value);
- #endif
- }
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order from little endian to big endian
+ double_value = swapDouble(double_value);
+#endif
+}
- //Write an unsigned short in little endian
- void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value) {
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order to little endian
- uint16_t swap_ushort = swap16bits(ushort_value);
- file.write((char *)(&swap_ushort), sizeof(swap_ushort));
- #else
- file.write((char *)(&ushort_value), sizeof(ushort_value));
- #endif
- }
+// Write an unsigned short in little endian
+void writeBinaryUShortLE(std::ofstream &file, const unsigned short ushort_value)
+{
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order to little endian
+ uint16_t swap_ushort = swap16bits(ushort_value);
+ file.write((char *)(&swap_ushort), sizeof(swap_ushort));
+#else
+ file.write((char *)(&ushort_value), sizeof(ushort_value));
+#endif
+}
- //Write a short in little endian
- void writeBinaryShortLE(std::ofstream &file, const short short_value) {
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order to little endian
- uint16_t swap_short = swap16bits((uint16_t) short_value);
- file.write((char *)(&swap_short), sizeof(swap_short));
- #else
- file.write((char *)(&short_value), sizeof(short_value));
- #endif
- }
+// Write a short in little endian
+void writeBinaryShortLE(std::ofstream &file, const short short_value)
+{
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order to little endian
+ uint16_t swap_short = swap16bits((uint16_t)short_value);
+ file.write((char *)(&swap_short), sizeof(swap_short));
+#else
+ file.write((char *)(&short_value), sizeof(short_value));
+#endif
+}
- //Write an unsigned int in little endian
-// void writeBinaryUIntLE(std::ofstream &file, const unsigned int uint_value) {
-// #ifdef VISP_BIG_ENDIAN
-// //Swap bytes order to little endian
-// //More info on data type: http://en.cppreference.com/w/cpp/language/types
-// if(sizeof(uint_value) == 4) {
-// uint32_t swap_uint = swap32bits(uint_value);
-// file.write((char *)(&swap_uint), sizeof(swap_uint));
-// } else {
-// uint16_t swap_uint = swap16bits(uint_value);
-// file.write((char *)(&swap_uint), sizeof(swap_uint));
-// }
-// #else
-// file.write((char *)(&uint_value), sizeof(uint_value));
-// #endif
-// }
-
- //Write an int in little endian
- void writeBinaryIntLE(std::ofstream &file, const int int_value) {
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order to little endian
- //More info on data type: http://en.cppreference.com/w/cpp/language/types
- if(sizeof(int_value) == 4) {
- uint32_t swap_int = swap32bits((uint32_t) int_value);
- file.write((char *)(&swap_int), sizeof(swap_int));
- } else {
- uint16_t swap_int = swap16bits((uint16_t) int_value);
- file.write((char *)(&swap_int), sizeof(swap_int));
- }
- #else
- file.write((char *)(&int_value), sizeof(int_value));
- #endif
+// Write an int in little endian
+void writeBinaryIntLE(std::ofstream &file, const int int_value)
+{
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order to little endian
+ // More info on data type: http://en.cppreference.com/w/cpp/language/types
+ if (sizeof(int_value) == 4) {
+ uint32_t swap_int = swap32bits((uint32_t)int_value);
+ file.write((char *)(&swap_int), sizeof(swap_int));
+ } else {
+ uint16_t swap_int = swap16bits((uint16_t)int_value);
+ file.write((char *)(&swap_int), sizeof(swap_int));
}
+#else
+ file.write((char *)(&int_value), sizeof(int_value));
+#endif
+}
- //Write a float in little endian
- void writeBinaryFloatLE(std::ofstream &file, const float float_value) {
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order to little endian
- float swap_float = swapFloat(float_value);
- file.write((char *)(&swap_float), sizeof(swap_float));
- #else
- file.write((char *)(&float_value), sizeof(float_value));
- #endif
- }
+// Write a float in little endian
+void writeBinaryFloatLE(std::ofstream &file, const float float_value)
+{
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order to little endian
+ float swap_float = swapFloat(float_value);
+ file.write((char *)(&swap_float), sizeof(swap_float));
+#else
+ file.write((char *)(&float_value), sizeof(float_value));
+#endif
+}
- //Write a double in little endian
- void writeBinaryDoubleLE(std::ofstream &file, const double double_value) {
- #ifdef VISP_BIG_ENDIAN
- //Swap bytes order to little endian
- double swap_double = swapDouble(double_value);
- file.write((char *)(&swap_double), sizeof(swap_double));
- #else
- file.write((char *)(&double_value), sizeof(double_value));
- #endif
- }
+// Write a double in little endian
+void writeBinaryDoubleLE(std::ofstream &file, const double double_value)
+{
+#ifdef VISP_BIG_ENDIAN
+ // Swap bytes order to little endian
+ double swap_double = swapDouble(double_value);
+ file.write((char *)(&swap_double), sizeof(swap_double));
+#else
+ file.write((char *)(&double_value), sizeof(double_value));
+#endif
+}
}
/*!
- Constructor to initialize the specified detector, descriptor, matcher and filtering method.
+ Constructor to initialize the specified detector, descriptor, matcher and
+ filtering method.
\param detectorType : Type of feature detector.
\param descriptorType : Type of the descriptor extractor.
@@ -334,21 +315,20 @@ namespace {
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_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),
+ m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(), m_trainVpPoints(),
+ m_useAffineDetection(false),
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
m_useBruteForceCrossCheck(true),
#endif
- m_useConsensusPercentage(false),
- m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
+ m_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true),
+ m_useSingleMatchFilter(true)
{
initFeatureNames();
@@ -359,7 +339,8 @@ vpKeyPoint::vpKeyPoint(const vpFeatureDetectorType &detectorType, const vpFeatur
}
/*!
- Constructor to initialize the specified detector, descriptor, matcher and filtering method.
+ Constructor to initialize the specified detector, descriptor, matcher and
+ filtering method.
\param detectorName : Name of the detector.
\param extractorName : Name of the extractor.
@@ -369,21 +350,20 @@ vpKeyPoint::vpKeyPoint(const vpFeatureDetectorType &detectorType, const vpFeatur
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_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),
+ m_ransacThreshold(0.01), m_trainDescriptors(), m_trainKeyPoints(), m_trainPoints(), m_trainVpPoints(),
+ m_useAffineDetection(false),
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
m_useBruteForceCrossCheck(true),
#endif
- m_useConsensusPercentage(false),
- m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
+ m_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true),
+ m_useSingleMatchFilter(true)
{
initFeatureNames();
@@ -394,12 +374,13 @@ vpKeyPoint::vpKeyPoint(const std::string &detectorName, const std::string &extra
}
/*!
- Constructor to initialize specified detector, extractor, matcher and filtering method.
+ Constructor to initialize specified detector, extractor, matcher and
+ filtering method.
- \param detectorNames : List of name detector for allowing multiple detectors.
- \param extractorNames : List of name extractor for allowing multiple extractors.
- \param matcherName : Name of the matcher.
- \param filterType : Filtering matching method chosen.
+ \param detectorNames : List of name detector for allowing multiple
+ detectors. \param extractorNames : List of name extractor for allowing
+ multiple extractors. \param matcherName : Name of the matcher. \param
+ filterType : Filtering matching method chosen.
*/
vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std::vector<std::string> &extractorNames,
const std::string &matcherName, const vpFilterMatchingType &filterType)
@@ -407,18 +388,17 @@ vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std:
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),
+ m_matcher(), m_matcherName(matcherName), m_matches(), m_matchingFactorThreshold(2.0),
+ m_matchingRatioThreshold(0.85), m_matchingTime(0.), m_matchRansacKeyPointsToPoints(), m_nbRansacIterations(200),
+ m_nbRansacMinInlierCount(100), m_objectFilteredPoints(), m_poseTime(0.), m_queryDescriptors(),
+ m_queryFilteredKeyPoints(), m_queryKeyPoints(), m_ransacConsensusPercentage(20.0), m_ransacInliers(),
+ m_ransacOutliers(), m_ransacReprojectionError(6.0), m_ransacThreshold(0.01), m_trainDescriptors(),
+ m_trainKeyPoints(), m_trainPoints(), m_trainVpPoints(), m_useAffineDetection(false),
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
m_useBruteForceCrossCheck(true),
#endif
- m_useConsensusPercentage(false),
- m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true), m_useSingleMatchFilter(true)
+ m_useConsensusPercentage(false), m_useKnn(false), m_useMatchTrainToQuery(false), m_useRansacVVS(true),
+ m_useSingleMatchFilter(true)
{
initFeatureNames();
init();
@@ -429,11 +409,11 @@ vpKeyPoint::vpKeyPoint(const std::vector<std::string> &detectorNames, const std:
\param tilt : Tilt value in the direction of x
\param phi : Rotation value
\param img : Modified image after the transformation
- \param mask : Mask containing the location of the image pixels after the transformation
- \param Ai : Inverse affine matrix
+ \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;
@@ -441,7 +421,7 @@ void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
cv::Mat A = cv::Mat::eye(2, 3, CV_32F);
- //if (phi != 0.0) {
+ // if (phi != 0.0) {
if (std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
phi *= M_PI / 180.;
double s = sin(phi);
@@ -462,18 +442,18 @@ void vpKeyPoint::affineSkew(double tilt, double phi, cv::Mat& img,
cv::Rect rect = cv::boundingRect(tcorners);
A = (cv::Mat_<float>(2, 3) << c, -s, -rect.x, s, c, -rect.y);
- cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height),
- cv::INTER_LINEAR, cv::BORDER_REPLICATE);
+ cv::warpAffine(img, img, A, cv::Size(rect.width, rect.height), cv::INTER_LINEAR, cv::BORDER_REPLICATE);
}
- //if (tilt != 1.0) {
+ // if (tilt != 1.0) {
if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon()) {
double s = 0.8 * sqrt(tilt * tilt - 1);
cv::GaussianBlur(img, img, cv::Size(0, 0), s, 0.01);
cv::resize(img, img, cv::Size(0, 0), 1.0 / tilt, 1.0, cv::INTER_NEAREST);
A.row(0) = A.row(0) / tilt;
}
- //if (tilt != 1.0 || phi != 0.0) {
- if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() || std::fabs(phi) > std::numeric_limits<double>::epsilon() ) {
+ // if (tilt != 1.0 || phi != 0.0) {
+ if (std::fabs(tilt - 1.0) > std::numeric_limits<double>::epsilon() ||
+ std::fabs(phi) > std::numeric_limits<double>::epsilon()) {
h = img.rows;
w = img.cols;
cv::warpAffine(mask, mask, A, cv::Size(w, h), cv::INTER_NEAREST);
@@ -487,9 +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<unsigned char> &I) {
- return buildReference(I, vpRect());
-}
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I) { return buildReference(I, vpRect()); }
/*!
Build the reference keypoints list in a region of interest in the image.
@@ -500,9 +478,9 @@ unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &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<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) {
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
+{
return buildReference(I, vpRect(iP, width, height));
}
@@ -514,32 +492,33 @@ unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &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<unsigned char> &I,
- const vpRect &rectangle) {
- //Reset variables used when dealing with 3D models
- //So as no 3D point list is passed, we dont need this variables
+unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle)
+{
+ // Reset variables used when dealing with 3D models
+ // So as no 3D point list is passed, we dont need this variables
m_trainPoints.clear();
m_mapOfImageId.clear();
m_mapOfImages.clear();
m_currentImageId = 1;
- if(m_useAffineDetection) {
+ if (m_useAffineDetection) {
std::vector<std::vector<cv::KeyPoint> > listOfTrainKeyPoints;
std::vector<cv::Mat> listOfTrainDescriptors;
- //Detect keypoints and extract descriptors on multiple images
+ // Detect keypoints and extract descriptors on multiple images
detectExtractAffine(I, listOfTrainKeyPoints, listOfTrainDescriptors);
- //Flatten the different train lists
+ // Flatten the different train lists
m_trainKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
- it != listOfTrainKeyPoints.end(); ++it) {
+ for (std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfTrainKeyPoints.begin();
+ it != listOfTrainKeyPoints.end(); ++it) {
m_trainKeyPoints.insert(m_trainKeyPoints.end(), it->begin(), it->end());
}
bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end(); ++it) {
- if(first) {
+ for (std::vector<cv::Mat>::const_iterator it = listOfTrainDescriptors.begin(); it != listOfTrainDescriptors.end();
+ ++it) {
+ if (first) {
first = false;
it->copyTo(m_trainDescriptors);
} else {
@@ -551,21 +530,21 @@ unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
extract(I, m_trainKeyPoints, m_trainDescriptors, m_extractionTime);
}
- //Save the correspondence keypoint class_id with the training image_id in a map
- //Used to display the matching with all the training images
- for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
+ // Save the correspondence keypoint class_id with the training image_id in a
+ // map Used to display the matching with all the training images
+ for (std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
m_mapOfImageId[it->class_id] = m_currentImageId;
}
- //Save the image in a map at a specific image_id
+ // Save the image in a map at a specific image_id
m_mapOfImages[m_currentImageId] = I;
- //Convert OpenCV type to ViSP type for compatibility
+ // Convert OpenCV type to ViSP type for compatibility
vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
_reference_computed = true;
- //Add train descriptors in matcher object
+ // Add train descriptors in matcher object
m_matcher->clear();
m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
@@ -573,39 +552,44 @@ unsigned int vpKeyPoint::buildReference(const vpImage<unsigned char> &I,
}
/*!
- Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
+ Build the reference keypoints list and compute the 3D position
+ corresponding of the keypoints locations.
\param I : Input image
\param trainKeyPoints : List of the train keypoints.
- \param points3f : Output list of the 3D position corresponding of the keypoints locations.
- \param append : If true, append the supply train keypoints with those already present.
- \param class_id : The class id to be set to the input cv::KeyPoint if != -1.
+ \param points3f : Output list of the 3D position corresponding of the
+ keypoints locations. \param append : If true, append the supply train
+ keypoints with those already present. \param class_id : The class id to be
+ set to the input cv::KeyPoint if != -1.
*/
void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &trainKeyPoints,
- std::vector<cv::Point3f> &points3f, const bool append, const int class_id) {
+ std::vector<cv::Point3f> &points3f, const bool append, const int class_id)
+{
cv::Mat trainDescriptors;
- //Copy the input list of keypoints
+ // Copy the input list of keypoints
std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
extract(I, trainKeyPoints, trainDescriptors, m_extractionTime, &points3f);
- if(trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
- //Keypoints have been removed
- //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ if (trainKeyPoints.size() != trainKeyPoints_tmp.size()) {
+ // Keypoints have been removed
+ // Store the hash of a keypoint as the key and the index of the keypoint
+ // as the value
std::map<size_t, size_t> mapOfKeypointHashes;
size_t cpt = 0;
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it, cpt++) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end();
+ ++it, cpt++) {
mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
}
std::vector<cv::Point3f> trainPoints_tmp;
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
- if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
+ if (mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
trainPoints_tmp.push_back(points3f[mapOfKeypointHashes[myKeypointHash(*it)]]);
}
}
- //Copy trainPoints_tmp to points3f
+ // Copy trainPoints_tmp to points3f
points3f = trainPoints_tmp;
}
@@ -613,19 +597,22 @@ void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, std::vector<cv:
}
/*!
- Build the reference keypoints list and compute the 3D position corresponding of the keypoints locations.
+ Build the reference keypoints list and compute the 3D position
+ corresponding of the keypoints locations.
\param I : Input image
\param trainKeyPoints : List of the train keypoints.
- \param points3f : List of the 3D position corresponding of the keypoints locations.
- \param trainDescriptors : List of the train descriptors.
- \param append : If true, append the supply train keypoints with those already present.
- \param class_id : The class id to be set to the input cv::KeyPoint if != -1.
+ \param points3f : List of the 3D position corresponding of the keypoints
+ locations. \param trainDescriptors : List of the train descriptors. \param
+ append : If true, append the supply train keypoints with those already
+ present. \param class_id : The class id to be set to the input cv::KeyPoint
+ if != -1.
*/
void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vector<cv::KeyPoint> &trainKeyPoints,
const cv::Mat &trainDescriptors, const std::vector<cv::Point3f> &points3f,
- const bool append, const int class_id) {
- if(!append) {
+ const bool append, const int class_id)
+{
+ if (!append) {
m_currentImageId = 0;
m_mapOfImageId.clear();
m_mapOfImages.clear();
@@ -636,37 +623,37 @@ void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vect
m_currentImageId++;
std::vector<cv::KeyPoint> trainKeyPoints_tmp = trainKeyPoints;
- //Set class_id if != -1
- if(class_id != -1) {
- for(std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
+ // Set class_id if != -1
+ if (class_id != -1) {
+ for (std::vector<cv::KeyPoint>::iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
it->class_id = class_id;
}
}
- //Save the correspondence keypoint class_id with the training image_id in a map
- //Used to display the matching with all the training images
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end(); ++it) {
+ // Save the correspondence keypoint class_id with the training image_id in a
+ // map Used to display the matching with all the training images
+ for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints_tmp.begin(); it != trainKeyPoints_tmp.end();
+ ++it) {
m_mapOfImageId[it->class_id] = m_currentImageId;
}
- //Save the image in a map at a specific image_id
+ // Save the image in a map at a specific image_id
m_mapOfImages[m_currentImageId] = I;
- //Append reference lists
+ // Append reference lists
this->m_trainKeyPoints.insert(this->m_trainKeyPoints.end(), trainKeyPoints_tmp.begin(), trainKeyPoints_tmp.end());
- if(!append) {
+ if (!append) {
trainDescriptors.copyTo(this->m_trainDescriptors);
} else {
this->m_trainDescriptors.push_back(trainDescriptors);
}
this->m_trainPoints.insert(this->m_trainPoints.end(), points3f.begin(), points3f.end());
-
- //Convert OpenCV type to ViSP type for compatibility
+ // Convert OpenCV type to ViSP type for compatibility
vpConvert::convertFromOpenCV(this->m_trainKeyPoints, referenceImagePointsList);
vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
- //Add train descriptors in matcher object
+ // Add train descriptors in matcher object
m_matcher->clear();
m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
@@ -674,19 +661,23 @@ void vpKeyPoint::buildReference(const vpImage<unsigned char> &I, const std::vect
}
/*!
- Compute the 3D coordinate in the world/object frame given the 2D image coordinate and under the assumption that the point is located on a plane
+ Compute the 3D coordinate in the world/object frame given the 2D image
+ coordinate and under the assumption that the point is located on a plane
whose the plane equation is known in the camera frame.
- The Z-coordinate is retrieved according to the proportional relationship between the plane equation expressed in the
- normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
+ The Z-coordinate is retrieved according to the proportional relationship
+ between the plane equation expressed in the normalized camera frame
+ (derived from the image coordinate) and the same plane equation expressed
+ in the camera frame.
\param candidate : Keypoint we want to compute the 3D coordinate.
- \param roi : List of 3D points in the camera frame representing a planar face.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param point : 3D coordinate in the world/object frame computed.
+ \param roi : List of 3D points in the camera frame representing a planar
+ face. \param cam : Camera parameters. \param cMo : Homogeneous matrix
+ 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<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point) {
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, cv::Point3f &point)
+{
/* compute plane equation */
std::vector<vpPoint>::const_iterator it_roi = roi.begin();
vpPoint pts[3];
@@ -708,23 +699,27 @@ void vpKeyPoint::compute3D(const cv::KeyPoint &candidate, const std::vector<vpPo
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]);
}
/*!
- Compute the 3D coordinate in the world/object frame given the 2D image coordinate and under the assumption that the point is located on a plane
+ Compute the 3D coordinate in the world/object frame given the 2D image
+ coordinate and under the assumption that the point is located on a plane
whose the plane equation is known in the camera frame.
- The Z-coordinate is retrieved according to the proportional relationship between the plane equation expressed in the
- normalized camera frame (derived from the image coordinate) and the same plane equation expressed in the camera frame.
+ The Z-coordinate is retrieved according to the proportional relationship
+ between the plane equation expressed in the normalized camera frame
+ (derived from the image coordinate) and the same plane equation expressed
+ in the camera frame.
\param candidate : vpImagePoint we want to compute the 3D coordinate.
- \param roi : List of 3D points in the camera frame representing a planar face.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the world and the camera frames.
- \param point : 3D coordinate in the world/object frame computed.
+ \param roi : List of 3D points in the camera frame representing a planar
+ face. \param cam : Camera parameters. \param cMo : Homogeneous matrix
+ 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<vpPoint> &roi,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point) {
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo, vpPoint &point)
+{
/* compute plane equation */
std::vector<vpPoint>::const_iterator it_roi = roi.begin();
vpPoint pts[3];
@@ -750,21 +745,26 @@ void vpKeyPoint::compute3D(const vpImagePoint &candidate, const std::vector<vpPo
}
/*!
- Keep only keypoints located on faces and compute for those keypoints the 3D coordinate in the world/object frame given the 2D image coordinate
- and under the assumption that the point is located on a plane.
+ Keep only keypoints located on faces and compute for those keypoints the 3D
+ coordinate in the world/object frame given the 2D image coordinate and
+ under the assumption that the point is located on a plane.
\param cMo : Homogeneous matrix between the world and the camera frames.
\param cam : Camera parameters.
- \param candidates : In input, list of keypoints detected in the whole image, in output, list of keypoints only located
- on planes.
- \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
- \param roisPt : List of faces, with the 3D coordinates known in the camera frame.
- \param points : Output list of computed 3D coordinates (in the world/object frame) of keypoints located only on faces.
- \param descriptors : Optional parameter, pointer to the descriptors to filter.
+ \param candidates : In input, list of keypoints detected in the whole
+ image, in output, list of keypoints only located on planes. \param polygons
+ : List of 2D polygons representing the projection of the faces in the image
+ plane. \param roisPt : List of faces, with the 3D coordinates known in the
+ camera frame. \param points : Output list of computed 3D coordinates (in
+ 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<cv::KeyPoint> &candidates, const std::vector<vpPolygon> &polygons,
- const std::vector<std::vector<vpPoint> > &roisPt, std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
+ std::vector<cv::KeyPoint> &candidates,
+ const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt,
+ std::vector<cv::Point3f> &points, cv::Mat *descriptors)
+{
std::vector<cv::KeyPoint> candidatesToCheck = candidates;
candidates.clear();
@@ -774,7 +774,7 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
cv::Mat desc;
std::vector<std::pair<cv::KeyPoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
- for(size_t i = 0; i < candidatesToCheck.size(); i++) {
+ for (size_t i = 0; i < candidatesToCheck.size(); i++) {
pairOfCandidatesToCheck[i] = std::pair<cv::KeyPoint, size_t>(candidatesToCheck[i], i);
}
@@ -783,18 +783,18 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
std::vector<std::pair<cv::KeyPoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
- while(it2 != pairOfCandidatesToCheck.end()) {
+ while (it2 != pairOfCandidatesToCheck.end()) {
imPt.set_ij(it2->first.pt.y, it2->first.pt.x);
if (it1->isInside(imPt)) {
candidates.push_back(it2->first);
vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
points.push_back(pt);
- if(descriptors != NULL) {
- desc.push_back(descriptors->row((int) it2->second));
+ if (descriptors != NULL) {
+ desc.push_back(descriptors->row((int)it2->second));
}
- //Remove candidate keypoint which is located on the current polygon
+ // Remove candidate keypoint which is located on the current polygon
it2 = pairOfCandidatesToCheck.erase(it2);
} else {
++it2;
@@ -802,27 +802,33 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
}
}
- if(descriptors != NULL) {
+ if (descriptors != NULL) {
desc.copyTo(*descriptors);
}
}
/*!
- Keep only keypoints located on faces and compute for those keypoints the 3D coordinate in the world/object frame given the 2D image coordinate
- and under the assumption that the point is located on a plane.
+ Keep only keypoints located on faces and compute for those keypoints the 3D
+ coordinate in the world/object frame given the 2D image coordinate and
+ under the assumption that the point is located on a plane.
\param cMo : Homogeneous matrix between the world and the camera frames.
\param cam : Camera parameters.
- \param candidates : In input, list of vpImagePoint located in the whole image, in output, list of vpImagePoint only located
- on planes.
- \param polygons : List of 2D polygons representing the projection of the faces in the image plane.
- \param roisPt : List of faces, with the 3D coordinates known in the camera frame.
- \param points : Output list of computed 3D coordinates (in the world/object frame) of vpImagePoint located only on faces.
- \param descriptors : Optional parameter, pointer to the descriptors to filter
+ \param candidates : In input, list of vpImagePoint located in the whole
+ image, in output, list of vpImagePoint only located on planes. \param
+ polygons : List of 2D polygons representing the projection of the faces in
+ the image plane. \param roisPt : List of faces, with the 3D coordinates
+ known in the camera frame. \param points : Output list of computed 3D
+ coordinates (in the world/object frame) of vpImagePoint located only on
+ faces. \param descriptors : Optional parameter, pointer to the descriptors
+ to filter
*/
void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidates, const std::vector<vpPolygon> &polygons,
- const std::vector<std::vector<vpPoint> > &roisPt, std::vector<vpPoint> &points, cv::Mat *descriptors) {
+ std::vector<vpImagePoint> &candidates,
+ const std::vector<vpPolygon> &polygons,
+ const std::vector<std::vector<vpPoint> > &roisPt,
+ std::vector<vpPoint> &points, cv::Mat *descriptors)
+{
std::vector<vpImagePoint> candidatesToCheck = candidates;
candidates.clear();
@@ -831,7 +837,7 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
cv::Mat desc;
std::vector<std::pair<vpImagePoint, size_t> > pairOfCandidatesToCheck(candidatesToCheck.size());
- for(size_t i = 0; i < candidatesToCheck.size(); i++) {
+ for (size_t i = 0; i < candidatesToCheck.size(); i++) {
pairOfCandidatesToCheck[i] = std::pair<vpImagePoint, size_t>(candidatesToCheck[i], i);
}
@@ -840,17 +846,17 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
for (std::vector<vpPolygon>::iterator it1 = polygons_tmp.begin(); it1 != polygons_tmp.end(); ++it1, cpt1++) {
std::vector<std::pair<vpImagePoint, size_t> >::iterator it2 = pairOfCandidatesToCheck.begin();
- while(it2 != pairOfCandidatesToCheck.end()) {
+ while (it2 != pairOfCandidatesToCheck.end()) {
if (it1->isInside(it2->first)) {
candidates.push_back(it2->first);
vpKeyPoint::compute3D(it2->first, roisPt[cpt1], cam, cMo, pt);
points.push_back(pt);
- if(descriptors != NULL) {
- desc.push_back(descriptors->row((int) it2->second));
+ if (descriptors != NULL) {
+ desc.push_back(descriptors->row((int)it2->second));
}
- //Remove candidate keypoint which is located on the current polygon
+ // Remove candidate keypoint which is located on the current polygon
it2 = pairOfCandidatesToCheck.erase(it2);
} else {
++it2;
@@ -860,46 +866,53 @@ void vpKeyPoint::compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, co
}
/*!
- Keep only keypoints located on cylinders and compute the 3D coordinates in the world/object frame given the 2D image coordinates.
+ Keep only keypoints located on cylinders and compute the 3D coordinates in
+ the world/object frame given the 2D image coordinates.
\param cMo : Homogeneous matrix between the world and the camera frames.
\param cam : Camera parameters.
- \param candidates : In input, list of keypoints detected in the whole image, in output, list of keypoints only located
- on cylinders.
- \param cylinders : List of vpCylinder corresponding of the cylinder objects in the scene, projected in the camera frame.
- \param vectorOfCylinderRois : For each cylinder, the corresponding list of bounding box.
- \param points : Output list of computed 3D coordinates in the world/object frame for each keypoint located on a cylinder.
- \param descriptors : Optional parameter, pointer to the descriptors to filter.
+ \param candidates : In input, list of keypoints detected in the whole
+ image, in output, list of keypoints only located on cylinders. \param
+ cylinders : List of vpCylinder corresponding of the cylinder objects in the
+ scene, projected in the camera frame. \param vectorOfCylinderRois : For
+ each cylinder, the corresponding list of bounding box. \param points :
+ Output list of computed 3D coordinates in the world/object frame for each
+ keypoint located on a cylinder. \param descriptors : Optional parameter,
+ pointer to the descriptors to filter.
*/
-void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<cv::KeyPoint> &candidates, const std::vector<vpCylinder> &cylinders,
- const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
- std::vector<cv::Point3f> &points, cv::Mat *descriptors) {
+void vpKeyPoint::compute3DForPointsOnCylinders(
+ const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector<cv::KeyPoint> &candidates,
+ const std::vector<vpCylinder> &cylinders,
+ const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois, std::vector<cv::Point3f> &points,
+ cv::Mat *descriptors)
+{
std::vector<cv::KeyPoint> candidatesToCheck = candidates;
candidates.clear();
points.clear();
cv::Mat desc;
- //Keep only keypoints on cylinders
+ // Keep only keypoints on cylinders
size_t cpt_keypoint = 0;
- for(std::vector<cv::KeyPoint>::const_iterator it1 = candidatesToCheck.begin();
- it1 != candidatesToCheck.end(); ++it1, cpt_keypoint++) {
+ for (std::vector<cv::KeyPoint>::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<std::vector<std::vector<vpImagePoint> > >::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<std::vector<vpImagePoint> >::const_iterator it3 = it2->begin(); it3 != it2->end(); ++it3) {
+ // Iterate through the list of vpCylinders
+ for (std::vector<std::vector<std::vector<vpImagePoint> > >::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<std::vector<vpImagePoint> >::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
+ // 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);
double Z = cylinders[cpt_cylinder].computeZ(xm, ym);
- if(!vpMath::isNaN(Z) && Z > std::numeric_limits<double>::epsilon()) {
+ if (!vpMath::isNaN(Z) && Z > std::numeric_limits<double>::epsilon()) {
vpColVector point_cam(4);
point_cam[0] = xm * Z;
point_cam[1] = ym * Z;
@@ -909,10 +922,10 @@ void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, c
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));
+ if (descriptors != NULL) {
+ desc.push_back(descriptors->row((int)cpt_keypoint));
}
break;
@@ -922,52 +935,59 @@ void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, c
}
}
- if(descriptors != NULL) {
+ if (descriptors != NULL) {
desc.copyTo(*descriptors);
}
}
/*!
- Keep only vpImagePoint located on cylinders and compute the 3D coordinates in the world/object frame given the 2D image coordinates.
+ Keep only vpImagePoint located on cylinders and compute the 3D coordinates
+ in the world/object frame given the 2D image coordinates.
\param cMo : Homogeneous matrix between the world and the camera frames.
\param cam : Camera parameters.
- \param candidates : In input, list of vpImagePoint located in the image, in output, list of vpImagePoint only located
- on cylinders.
- \param cylinders : List of vpCylinder corresponding of the cylinder objects in the scene, projected in the camera frame.
- \param vectorOfCylinderRois : For each cylinder, the corresponding list of bounding box.
- \param points : Output list of computed 3D coordinates in the world/object frame for each vpImagePoint located on a cylinder.
- \param descriptors : Optional parameter, pointer to the descriptors to filter.
+ \param candidates : In input, list of vpImagePoint located in the image, in
+ output, list of vpImagePoint only located on cylinders. \param cylinders :
+ List of vpCylinder corresponding of the cylinder objects in the scene,
+ projected in the camera frame. \param vectorOfCylinderRois : For each
+ cylinder, the corresponding list of bounding box. \param points : Output
+ list of computed 3D coordinates in the world/object frame for each
+ vpImagePoint located on a cylinder. \param descriptors : Optional
+ parameter, pointer to the descriptors to filter.
*/
-void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam,
- std::vector<vpImagePoint> &candidates, const std::vector<vpCylinder> &cylinders,
- const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois,
- std::vector<vpPoint> &points, cv::Mat *descriptors) {
+void vpKeyPoint::compute3DForPointsOnCylinders(
+ const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector<vpImagePoint> &candidates,
+ const std::vector<vpCylinder> &cylinders,
+ const std::vector<std::vector<std::vector<vpImagePoint> > > &vectorOfCylinderRois, std::vector<vpPoint> &points,
+ cv::Mat *descriptors)
+{
std::vector<vpImagePoint> candidatesToCheck = candidates;
candidates.clear();
points.clear();
cv::Mat desc;
- //Keep only keypoints on cylinders
+ // Keep only keypoints on cylinders
size_t cpt_keypoint = 0;
- for(std::vector<vpImagePoint>::const_iterator it1 = candidatesToCheck.begin();
- it1 != candidatesToCheck.end(); ++it1, cpt_keypoint++) {
+ for (std::vector<vpImagePoint>::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<std::vector<std::vector<vpImagePoint> > >::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<std::vector<vpImagePoint> >::const_iterator it3 = it2->begin(); it3 != it2->end(); ++it3) {
+ // Iterate through the list of vpCylinders
+ for (std::vector<std::vector<std::vector<vpImagePoint> > >::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<std::vector<vpImagePoint> >::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
+ // 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);
double Z = cylinders[cpt_cylinder].computeZ(xm, ym);
- if(!vpMath::isNaN(Z) && Z > std::numeric_limits<double>::epsilon()) {
+ if (!vpMath::isNaN(Z) && Z > std::numeric_limits<double>::epsilon()) {
vpColVector point_cam(4);
point_cam[0] = xm * Z;
point_cam[1] = ym * Z;
@@ -979,8 +999,8 @@ void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, c
pt.setWorldCoordinates(point_obj);
points.push_back(pt);
- if(descriptors != NULL) {
- desc.push_back(descriptors->row((int) cpt_keypoint));
+ if (descriptors != NULL) {
+ desc.push_back(descriptors->row((int)cpt_keypoint));
}
break;
@@ -990,31 +1010,35 @@ void vpKeyPoint::compute3DForPointsOnCylinders(const vpHomogeneousMatrix &cMo, c
}
}
- if(descriptors != NULL) {
+ if (descriptors != NULL) {
desc.copyTo(*descriptors);
}
}
/*!
- Compute the pose using the correspondence between 2D points and 3D points using OpenCV function with RANSAC method.
-
- \param imagePoints : List of 2D points corresponding to the location of the detected keypoints.
- \param objectPoints : List of the 3D points in the object frame matched.
- \param cam : Camera parameters.
- \param cMo : Homogeneous matrix between the object frame and the camera frame.
- \param inlierIndex : List of indexes of inliers.
- \param elapsedTime : Elapsed time.
- \param func : Function pointer to filter the final pose returned by OpenCV pose estimation method.
- \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
+ Compute the pose using the correspondence between 2D points and 3D points
+ using OpenCV function with RANSAC method.
+
+ \param imagePoints : List of 2D points corresponding to the location of the
+ detected keypoints. \param objectPoints : List of the 3D points in the
+ object frame matched. \param cam : Camera parameters. \param cMo :
+ Homogeneous matrix between the object frame and the camera frame. \param
+ inlierIndex : List of indexes of inliers. \param elapsedTime : Elapsed
+ time. \param func : Function pointer to filter the final pose returned by
+ OpenCV pose estimation method. \return True if the pose has been computed,
+ false otherwise (not enough points, or size list mismatch).
*/
bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const std::vector<cv::Point3f> &objectPoints,
- const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
- double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ const vpCameraParameters &cam, vpHomogeneousMatrix &cMo, std::vector<int> &inlierIndex,
+ double &elapsedTime, bool (*func)(vpHomogeneousMatrix *))
+{
double t = vpTime::measureTimeMs();
- if(imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
+ if (imagePoints.size() < 4 || objectPoints.size() < 4 || imagePoints.size() != objectPoints.size()) {
elapsedTime = (vpTime::measureTimeMs() - t);
- std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
+ std::cerr << "Not enough points to compute the pose (at least 4 points "
+ "are needed)."
+ << std::endl;
return false;
}
@@ -1023,61 +1047,62 @@ bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const
(cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
cv::Mat rvec, tvec;
-
- //Bug with OpenCV < 2.4.0 when zero distorsion is provided by an empty array.
- //http://code.opencv.org/issues/1700 ; http://code.opencv.org/issues/1718
- //what(): Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector in function cvProjectPoints2
- //Fixed in OpenCV 2.4.0 (r7558)
-// cv::Mat distCoeffs;
+ // Bug with OpenCV < 2.4.0 when zero distorsion is provided by an empty
+ // array. http://code.opencv.org/issues/1700 ;
+ // http://code.opencv.org/issues/1718 what(): Distortion coefficients must
+ // be 1x4, 4x1, 1x5, 5x1, 1x8 or 8x1 floating-point vector in function
+ // cvProjectPoints2 Fixed in OpenCV 2.4.0 (r7558)
+ // cv::Mat distCoeffs;
cv::Mat distCoeffs = cv::Mat::zeros(1, 5, CV_64F);
try {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- //OpenCV 3.0.0 (2014/12/12)
- cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
- rvec, tvec, false, m_nbRansacIterations, (float) m_ransacReprojectionError,
- 0.99,//confidence=0.99 (default) – The probability that the algorithm produces a useful result.
- inlierIndex,
- cv::SOLVEPNP_ITERATIVE);
- //SOLVEPNP_ITERATIVE (default): Iterative method is based on Levenberg-Marquardt optimization.
- //In this case the function finds such a pose that minimizes reprojection error, that is the sum of squared distances
- //between the observed projections imagePoints and the projected (using projectPoints() ) objectPoints .
- //SOLVEPNP_P3P: Method is based on the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification
- //for the Perspective-Three-Point Problem”. In this case the function requires exactly four object and image points.
- //SOLVEPNP_EPNP: Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient
- //Perspective-n-Point Camera Pose Estimation”.
- //SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I. Roumeliotis. “A Direct Least-Squares (DLS)
- //Method for PnP”.
- //SOLVEPNP_UPNP Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto, F.Moreno-Noguer.
- //“Exhaustive Linearization for Robust Camera Pose and Focal Length Estimation”. In this case the function also
- //estimates the parameters f_x and f_y assuming that both have the same value. Then the cameraMatrix is updated with the
- //estimated focal length.
+ // OpenCV 3.0.0 (2014/12/12)
+ cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, m_nbRansacIterations,
+ (float)m_ransacReprojectionError,
+ 0.99, // confidence=0.99 (default) – The probability
+ // that the algorithm produces a useful result.
+ inlierIndex, cv::SOLVEPNP_ITERATIVE);
+// SOLVEPNP_ITERATIVE (default): Iterative method is based on
+// Levenberg-Marquardt optimization. In this case the function finds such a
+// pose that minimizes reprojection error, that is the sum of squared
+// distances between the observed projections imagePoints and the projected
+// (using projectPoints() ) objectPoints . SOLVEPNP_P3P: Method is based on
+// the paper of X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution
+// Classification for the Perspective-Three-Point Problem”. In this case the
+// function requires exactly four object and image points. SOLVEPNP_EPNP:
+// Method has been introduced by F.Moreno-Noguer, V.Lepetit and P.Fua in the
+// paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”.
+// SOLVEPNP_DLS: Method is based on the paper of Joel A. Hesch and Stergios I.
+// Roumeliotis. “A Direct Least-Squares (DLS) Method for PnP”. SOLVEPNP_UPNP
+// Method is based on the paper of A.Penate-Sanchez, J.Andrade-Cetto,
+// F.Moreno-Noguer. “Exhaustive Linearization for Robust Camera Pose and Focal
+// Length Estimation”. In this case the function also estimates the
+// parameters
+// f_x and f_y assuming that both have the same value. Then the cameraMatrix
+// is updated with the estimated focal length.
#else
int nbInlierToReachConsensus = m_nbRansacMinInlierCount;
- if(m_useConsensusPercentage) {
- nbInlierToReachConsensus = (int) (m_ransacConsensusPercentage / 100.0 * (double) m_queryFilteredKeyPoints.size());
+ if (m_useConsensusPercentage) {
+ nbInlierToReachConsensus = (int)(m_ransacConsensusPercentage / 100.0 * (double)m_queryFilteredKeyPoints.size());
}
- cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs,
- rvec, tvec, false, m_nbRansacIterations,
- (float) m_ransacReprojectionError, nbInlierToReachConsensus,
- inlierIndex);
+ cv::solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, false, m_nbRansacIterations,
+ (float)m_ransacReprojectionError, nbInlierToReachConsensus, inlierIndex);
#endif
} catch (cv::Exception &e) {
std::cerr << e.what() << std::endl;
elapsedTime = (vpTime::measureTimeMs() - t);
return false;
}
- vpTranslationVector translationVec(tvec.at<double>(0),
- tvec.at<double>(1), tvec.at<double>(2));
- vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1),
- rvec.at<double>(2));
+ vpTranslationVector translationVec(tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2));
+ vpThetaUVector thetaUVector(rvec.at<double>(0), rvec.at<double>(1), rvec.at<double>(2));
cMo = vpHomogeneousMatrix(translationVec, thetaUVector);
- if(func != NULL) {
- //Check the final pose returned by solvePnPRansac to discard
- //solutions which do not respect the pose criterion.
- if(!func(&cMo)) {
+ if (func != NULL) {
+ // Check the final pose returned by solvePnPRansac to discard
+ // solutions which do not respect the pose criterion.
+ if (!func(&cMo)) {
elapsedTime = (vpTime::measureTimeMs() - t);
return false;
}
@@ -1088,56 +1113,62 @@ bool vpKeyPoint::computePose(const std::vector<cv::Point2f> &imagePoints, const
}
/*!
- Compute the pose using the correspondence between 2D points and 3D points using ViSP function with RANSAC method.
-
- \param objectVpPoints : List of vpPoint with coordinates expressed in the object and in the camera frame.
- \param cMo : Homogeneous matrix between the object frame and the camera frame.
- \param inliers : List of inliers.
- \param elapsedTime : Elapsed time.
- \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
+ Compute the pose using the correspondence between 2D points and 3D points
+ using ViSP function with RANSAC method.
+
+ \param objectVpPoints : List of vpPoint with coordinates expressed in the
+ object and in the camera frame. \param cMo : Homogeneous matrix between the
+ object frame and the camera frame. \param inliers : List of inliers. \param
+ elapsedTime : Elapsed time. \return True if the pose has been computed,
+ false otherwise (not enough points, or size list mismatch). \param func :
+ Function pointer to filter the pose in Ransac pose estimation, if we want
+ to eliminate the poses which do not respect some criterion
*/
bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ std::vector<vpPoint> &inliers, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *))
+{
std::vector<unsigned int> inlierIndex;
return computePose(objectVpPoints, cMo, inliers, inlierIndex, elapsedTime, func);
}
/*!
- Compute the pose using the correspondence between 2D points and 3D points using ViSP function with RANSAC method.
+ Compute the pose using the correspondence between 2D points and 3D points
+ using ViSP function with RANSAC method.
- \param objectVpPoints : List of vpPoint with coordinates expressed in the object and in the camera frame.
- \param cMo : Homogeneous matrix between the object frame and the camera frame.
- \param inliers : List of inlier points.
+ \param objectVpPoints : List of vpPoint with coordinates expressed in the
+ object and in the camera frame. \param cMo : Homogeneous matrix between the
+ object frame and the camera frame. \param inliers : List of inlier points.
\param inlierIndex : List of inlier index.
\param elapsedTime : Elapsed time.
- \return True if the pose has been computed, false otherwise (not enough points, or size list mismatch).
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
+ \return True if the pose has been computed, false otherwise (not enough
+ points, or size list mismatch). \param func : Function pointer to filter
+ the pose in Ransac pose estimation, if we want to eliminate the poses which
+ do not respect some criterion
*/
bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomogeneousMatrix &cMo,
- std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex,
- double &elapsedTime, bool (*func)(vpHomogeneousMatrix *)) {
+ std::vector<vpPoint> &inliers, std::vector<unsigned int> &inlierIndex, double &elapsedTime,
+ bool (*func)(vpHomogeneousMatrix *))
+{
double t = vpTime::measureTimeMs();
- if(objectVpPoints.size() < 4) {
+ if (objectVpPoints.size() < 4) {
elapsedTime = (vpTime::measureTimeMs() - t);
-// std::cerr << "Not enough points to compute the pose (at least 4 points are needed)." << std::endl;
+ // std::cerr << "Not enough points to compute the pose (at least 4
+ // points are needed)." << std::endl;
return false;
}
vpPose pose;
- for(std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
+ for (std::vector<vpPoint>::const_iterator it = objectVpPoints.begin(); it != objectVpPoints.end(); ++it) {
pose.addPoint(*it);
}
- unsigned int nbInlierToReachConsensus = (unsigned int) m_nbRansacMinInlierCount;
- if(m_useConsensusPercentage) {
- nbInlierToReachConsensus = (unsigned int) (m_ransacConsensusPercentage / 100.0 *
- (double) m_queryFilteredKeyPoints.size());
+ unsigned int nbInlierToReachConsensus = (unsigned int)m_nbRansacMinInlierCount;
+ if (m_useConsensusPercentage) {
+ nbInlierToReachConsensus =
+ (unsigned int)(m_ransacConsensusPercentage / 100.0 * (double)m_queryFilteredKeyPoints.size());
}
pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
@@ -1151,123 +1182,135 @@ bool vpKeyPoint::computePose(const std::vector<vpPoint> &objectVpPoints, vpHomog
inliers = pose.getRansacInliers();
inlierIndex = pose.getRansacInlierIndex();
- if(m_computeCovariance) {
+ if (m_computeCovariance) {
m_covarianceMatrix = pose.getCovarianceMatrix();
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "e=" << e.what() << std::endl;
elapsedTime = (vpTime::measureTimeMs() - t);
return false;
}
-// if(func != NULL && isRansacPoseEstimationOk) {
-// //Check the final pose returned by the Ransac VVS pose estimation as in rare some cases
-// //we can converge toward a final cMo that does not respect the pose criterion even
-// //if the 4 minimal points picked to respect the pose criterion.
-// if(!func(&cMo)) {
-// elapsedTime = (vpTime::measureTimeMs() - t);
-// return false;
-// }
-// }
+ // if(func != NULL && isRansacPoseEstimationOk) {
+ // //Check the final pose returned by the Ransac VVS pose estimation as
+ // in rare some cases
+ // //we can converge toward a final cMo that does not respect the pose
+ // criterion even
+ // //if the 4 minimal points picked to respect the pose criterion.
+ // if(!func(&cMo)) {
+ // elapsedTime = (vpTime::measureTimeMs() - t);
+ // return false;
+ // }
+ // }
elapsedTime = (vpTime::measureTimeMs() - t);
return isRansacPoseEstimationOk;
}
/*!
- Compute the pose estimation error, the mean square error (in pixel) between the location of the detected keypoints
- and the location of the projection of the 3D model with the estimated pose.
+ Compute the pose estimation error, the mean square error (in pixel) between
+ the location of the detected keypoints and the location of the projection
+ of the 3D model with the estimated pose.
- \param matchKeyPoints : List of pairs between the detected keypoints and the corresponding 3D points.
- \param cam : Camera parameters.
- \param cMo_est : Estimated pose of the object.
+ \param matchKeyPoints : List of pairs between the detected keypoints and
+ the corresponding 3D points. \param cam : Camera parameters. \param cMo_est
+ : Estimated pose of the object.
- \return The mean square error (in pixel) between the location of the detected keypoints
- and the location of the projection of the 3D model with the estimated pose.
+ \return The mean square error (in pixel) between the location of the
+ detected keypoints and the location of the projection of the 3D model with
+ the estimated pose.
*/
double vpKeyPoint::computePoseEstimationError(const std::vector<std::pair<cv::KeyPoint, cv::Point3f> > &matchKeyPoints,
- const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est) {
- if(matchKeyPoints.size() == 0) {
- //return std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ const vpCameraParameters &cam, const vpHomogeneousMatrix &cMo_est)
+{
+ if (matchKeyPoints.size() == 0) {
+ // return std::numeric_limits<double>::max(); // create an error under
+ // Windows. To fix it we have to add #undef max
return DBL_MAX;
}
std::vector<double> errors(matchKeyPoints.size());
size_t cpt = 0;
vpPoint pt;
- for(std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
- it != matchKeyPoints.end(); ++it, cpt++) {
+ for (std::vector<std::pair<cv::KeyPoint, cv::Point3f> >::const_iterator it = matchKeyPoints.begin();
+ it != matchKeyPoints.end(); ++it, cpt++) {
pt.set_oX(it->second.x);
pt.set_oY(it->second.y);
pt.set_oZ(it->second.z);
pt.project(cMo_est);
double u = 0.0, v = 0.0;
vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), u, v);
- errors[cpt] = std::sqrt((u-it->first.pt.x)*(u-it->first.pt.x) + (v-it->first.pt.y)*(v-it->first.pt.y));
+ errors[cpt] = std::sqrt((u - it->first.pt.x) * (u - it->first.pt.x) + (v - it->first.pt.y) * (v - it->first.pt.y));
}
return std::accumulate(errors.begin(), errors.end(), 0.0) / errors.size();
}
/*!
- Initialize the size of the matching image (case with a matching side by side between IRef and ICurrent).
+ Initialize the size of the matching image (case with a matching side by
+ side between IRef and ICurrent).
\param IRef : Reference image.
\param ICurrent : Current image.
\param IMatching : Image matching.
*/
void vpKeyPoint::createImageMatching(vpImage<unsigned char> &IRef, vpImage<unsigned char> &ICurrent,
- vpImage<unsigned char> &IMatching) {
- //Image matching side by side
+ vpImage<unsigned char> &IMatching)
+{
+ // Image matching side by side
unsigned int width = IRef.getWidth() + ICurrent.getWidth();
- unsigned int height = (std::max)(IRef.getHeight(), ICurrent.getHeight());
+ unsigned int height = ((std::max))(IRef.getHeight(), ICurrent.getHeight());
IMatching = vpImage<unsigned char>(height, width);
}
/*!
- Initialize the size of the matching image with appropriate size according to the number of training images.
- Used to display the matching of keypoints detected in the current image with those detected in multiple
- training images.
+ Initialize the size of the matching image with appropriate size according
+ to the number of training images. Used to display the matching of keypoints
+ detected in the current image with those detected in multiple training
+ images.
\param ICurrent : Current image.
\param IMatching : Image initialized with appropriate size.
*/
-void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
- //Nb images in the training database + the current image we want to detect the object
- unsigned int nbImg = (unsigned int) (m_mapOfImages.size() + 1);
+void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching)
+{
+ // Nb images in the training database + the current image we want to detect
+ // the object
+ unsigned int nbImg = (unsigned int)(m_mapOfImages.size() + 1);
- if(m_mapOfImages.empty()) {
+ if (m_mapOfImages.empty()) {
std::cerr << "There is no training image loaded !" << std::endl;
return;
}
- if(nbImg == 2) {
- //Only one training image, so we display them side by side
+ if (nbImg == 2) {
+ // Only one training image, so we display them side by side
createImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
} else {
- //Multiple training images, display them as a mosaic image
+ // 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
+ // Number of columns in the mosaic grid
unsigned int nbWidth = nbImgSqrt;
- //Number of rows in the mosaic grid
+ // Number of rows in the mosaic grid
unsigned int nbHeight = nbImgSqrt;
- //Deals with non square mosaic grid and the total number of images
- if(nbImgSqrt * nbImgSqrt < nbImg) {
+ // Deals with non square mosaic grid and the total number of images
+ if (nbImgSqrt * nbImgSqrt < nbImg) {
nbWidth++;
}
unsigned int maxW = ICurrent.getWidth();
unsigned int maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(maxW < it->second.getWidth()) {
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it) {
+ if (maxW < it->second.getWidth()) {
maxW = it->second.getWidth();
}
- if(maxH < it->second.getHeight()) {
+ if (maxH < it->second.getHeight()) {
maxH = it->second.getHeight();
}
}
@@ -1283,7 +1326,8 @@ void vpKeyPoint::createImageMatching(vpImage<unsigned char> &ICurrent, vpImage<u
\param keyPoints : Output list of the detected keypoints.
\param rectangle : Optional rectangle of the region of interest.
*/
-void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, const vpRect &rectangle) {
+void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, const vpRect &rectangle)
+{
double elapsedTime;
detect(I, keyPoints, elapsedTime, rectangle);
}
@@ -1293,9 +1337,11 @@ void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoin
\param matImg : Input image.
\param keyPoints : Output list of the detected keypoints.
- \param mask : Optional 8-bit integer mask to detect only where mask[i][j] != 0.
+ \param mask : Optional 8-bit integer mask to detect only where mask[i][j]
+ != 0.
*/
-void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, const cv::Mat &mask) {
+void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, const cv::Mat &mask)
+{
double elapsedTime;
detect(matImg, keyPoints, elapsedTime, mask);
}
@@ -1309,14 +1355,15 @@ void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoi
\param rectangle : Optional rectangle of the region of interest.
*/
void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const vpRect &rectangle) {
+ const vpRect &rectangle)
+{
cv::Mat matImg;
vpImageConvert::convert(I, matImg, false);
cv::Mat mask = cv::Mat::zeros(matImg.rows, matImg.cols, CV_8U);
- if(rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
- cv::Point leftTop((int) rectangle.getLeft(), (int) rectangle.getTop()), rightBottom((int) rectangle.getRight(),
- (int) rectangle.getBottom());
+ if (rectangle.getWidth() > 0 && rectangle.getHeight() > 0) {
+ cv::Point leftTop((int)rectangle.getLeft(), (int)rectangle.getTop()),
+ rightBottom((int)rectangle.getRight(), (int)rectangle.getBottom());
cv::rectangle(mask, leftTop, rightBottom, cv::Scalar(255), CV_FILLED);
} else {
mask = cv::Mat::ones(matImg.rows, matImg.cols, CV_8U) * 255;
@@ -1331,14 +1378,17 @@ void vpKeyPoint::detect(const vpImage<unsigned char> &I, std::vector<cv::KeyPoin
\param matImg : Input image.
\param keyPoints : Output list of the detected keypoints.
\param elapsedTime : Elapsed time.
- \param mask : Optional 8-bit integer mask to detect only where mask[i][j] != 0.
+ \param mask : Optional 8-bit integer mask to detect only where mask[i][j]
+ != 0.
*/
void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, double &elapsedTime,
- const cv::Mat &mask) {
+ const cv::Mat &mask)
+{
double t = vpTime::measureTimeMs();
keyPoints.clear();
- for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin(); it != m_detectors.end(); ++it) {
+ for (std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
+ it != m_detectors.end(); ++it) {
std::vector<cv::KeyPoint> kp;
it->second->detect(matImg, kp, mask);
keyPoints.insert(keyPoints.end(), kp.begin(), kp.end());
@@ -1354,14 +1404,14 @@ void vpKeyPoint::detect(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoi
\param ICurrent : Input current image.
\param size : Size of the displayed cross.
*/
-void vpKeyPoint::display(const vpImage<unsigned char> &IRef,
- const vpImage<unsigned char> &ICurrent, unsigned int size) {
+void vpKeyPoint::display(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent, unsigned int size)
+{
std::vector<vpImagePoint> vpQueryImageKeyPoints;
getQueryKeyPoints(vpQueryImageKeyPoints);
std::vector<vpImagePoint> vpTrainImageKeyPoints;
getTrainKeyPoints(vpTrainImageKeyPoints);
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ for (std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
vpDisplay::displayCross(IRef, vpTrainImageKeyPoints[(size_t)(it->trainIdx)], size, vpColor::red);
vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, vpColor::green);
}
@@ -1374,28 +1424,32 @@ void vpKeyPoint::display(const vpImage<unsigned char> &IRef,
\param size : Size of the displayed crosses.
\param color : Color of the crosses.
*/
-void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color) {
+void vpKeyPoint::display(const vpImage<unsigned char> &ICurrent, unsigned int size, const vpColor &color)
+{
std::vector<vpImagePoint> vpQueryImageKeyPoints;
getQueryKeyPoints(vpQueryImageKeyPoints);
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- vpDisplay::displayCross (ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
+ for (std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ vpDisplay::displayCross(ICurrent, vpQueryImageKeyPoints[(size_t)(it->queryIdx)], size, color);
}
}
/*!
- Display the matching lines between the detected keypoints with those detected in one training image.
+ Display the matching lines between the detected keypoints with those
+ detected in one training image.
\param IRef : Reference image, used to have the x-offset.
\param IMatching : Resulting image matching.
\param crossSize : Size of the displayed crosses.
\param lineThickness : Thickness of the displayed lines.
- \param color : Color to use, if none, we pick randomly a color for each pair of matching.
+ \param color : Color to use, if none, we pick randomly a color for each pair
+ of matching.
*/
void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<unsigned char> &IMatching,
- unsigned int crossSize, unsigned int lineThickness, const vpColor &color) {
+ unsigned int crossSize, unsigned int lineThickness, const vpColor &color)
+{
bool randomColor = (color == vpColor::none);
- srand((unsigned int) time(NULL));
+ srand((unsigned int)time(NULL));
vpColor currentColor = color;
std::vector<vpImagePoint> queryImageKeyPoints;
@@ -1404,14 +1458,14 @@ void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<uns
getTrainKeyPoints(trainImageKeyPoints);
vpImagePoint leftPt, rightPt;
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- if(randomColor) {
+ for (std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ if (randomColor) {
currentColor = vpColor((rand() % 256), (rand() % 256), (rand() % 256));
}
leftPt = trainImageKeyPoints[(size_t)(it->trainIdx)];
rightPt = vpImagePoint(queryImageKeyPoints[(size_t)(it->queryIdx)].get_i(),
- queryImageKeyPoints[(size_t)it->queryIdx].get_j() + IRef.getWidth());
+ 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);
@@ -1419,114 +1473,121 @@ void vpKeyPoint::displayMatching(const vpImage<unsigned char> &IRef, vpImage<uns
}
/*!
- Display matching between keypoints detected in the current image and with those detected in the multiple training
- images. Display also RANSAC inliers if the list is supplied.
+ Display matching between keypoints detected in the current image and with
+ those detected in the multiple training images. Display also RANSAC inliers
+ if the list is supplied.
\param ICurrent : Current image.
\param IMatching : Resulting matching image.
- \param ransacInliers : List of Ransac inliers or empty list if not available.
- \param crossSize : Size of the displayed crosses.
- \param lineThickness : Thickness of the displayed line.
+ \param ransacInliers : List of Ransac inliers or empty list if not
+ available. \param crossSize : Size of the displayed crosses. \param
+ lineThickness : Thickness of the displayed line.
*/
void vpKeyPoint::displayMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching,
- const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize, unsigned int lineThickness) {
- if(m_mapOfImages.empty() || m_mapOfImageId.empty()) {
- //No training images so return
+ const std::vector<vpImagePoint> &ransacInliers, unsigned int crossSize,
+ unsigned int lineThickness)
+{
+ if (m_mapOfImages.empty() || m_mapOfImageId.empty()) {
+ // No training images so return
std::cerr << "There is no training image loaded !" << std::endl;
return;
}
- //Nb images in the training database + the current image we want to detect the object
- int nbImg = (int) (m_mapOfImages.size() + 1);
+ // Nb images in the training database + the current image we want to detect
+ // the object
+ int nbImg = (int)(m_mapOfImages.size() + 1);
- if(nbImg == 2) {
- //Only one training image, so we display the matching result side-by-side
+ if (nbImg == 2) {
+ // Only one training image, so we display the matching result side-by-side
displayMatching(m_mapOfImages.begin()->second, IMatching, crossSize);
} else {
- //Multiple training images, display them as a mosaic image
- int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
+ // Multiple training images, display them as a mosaic image
+ int nbImgSqrt = vpMath::round(std::sqrt((double)nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
int nbWidth = nbImgSqrt;
int nbHeight = nbImgSqrt;
- if(nbImgSqrt * nbImgSqrt < nbImg) {
+ if (nbImgSqrt * nbImgSqrt < nbImg) {
nbWidth++;
}
std::map<int, int> mapOfImageIdIndex;
int cpt = 0;
unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it, cpt++) {
mapOfImageIdIndex[it->first] = cpt;
- if(maxW < it->second.getWidth()) {
+ if (maxW < it->second.getWidth()) {
maxW = it->second.getWidth();
}
- if(maxH < it->second.getHeight()) {
+ if (maxH < it->second.getHeight()) {
maxH = it->second.getHeight();
}
}
- //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
+ // Indexes of the current image in the grid computed to put preferably the
+ // image in the center of the mosaic grid
int medianI = nbHeight / 2;
int medianJ = nbWidth / 2;
int medianIndex = medianI * nbWidth + medianJ;
- for(std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = m_trainKeyPoints.begin(); it != m_trainKeyPoints.end(); ++it) {
vpImagePoint topLeftCorner;
int current_class_id_index = 0;
- if(mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
+ if (mapOfImageIdIndex[m_mapOfImageId[it->class_id]] < medianIndex) {
current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]];
} else {
- //Shift of one unity the index of the training images which are after the current image
+ // Shift of one unity the index of the training images which are after
+ // the current image
current_class_id_index = mapOfImageIdIndex[m_mapOfImageId[it->class_id]] + 1;
}
int indexI = current_class_id_index / nbWidth;
int indexJ = current_class_id_index - (indexI * nbWidth);
- topLeftCorner.set_ij((int)maxH*indexI, (int)maxW*indexJ);
+ 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);
+ // Display cross for keypoints in the learning database
+ vpDisplay::displayCross(IMatching, (int)(it->pt.y + topLeftCorner.get_i()),
+ (int)(it->pt.x + topLeftCorner.get_j()), crossSize, vpColor::red);
}
- vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
- for(std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
- //Display cross for keypoints detected in the current image
- vpDisplay::displayCross(IMatching, (int) (it->pt.y + topLeftCorner.get_i()), (int) (it->pt.x + topLeftCorner.get_j()),
- crossSize, vpColor::red);
+ vpImagePoint topLeftCorner((int)maxH * medianI, (int)maxW * medianJ);
+ for (std::vector<cv::KeyPoint>::const_iterator it = m_queryKeyPoints.begin(); it != m_queryKeyPoints.end(); ++it) {
+ // Display cross for keypoints detected in the current image
+ vpDisplay::displayCross(IMatching, (int)(it->pt.y + topLeftCorner.get_i()),
+ (int)(it->pt.x + topLeftCorner.get_j()), crossSize, vpColor::red);
}
- for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin();
- it != ransacInliers.end(); ++it) {
- //Display green circle for RANSAC inliers
- vpDisplay::displayCircle(IMatching, (int) (it->get_v() + topLeftCorner.get_i()), (int) (it->get_u() +
- topLeftCorner.get_j()), 4, vpColor::green);
+ for (std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
+ // Display green circle for RANSAC inliers
+ vpDisplay::displayCircle(IMatching, (int)(it->get_v() + topLeftCorner.get_i()),
+ (int)(it->get_u() + topLeftCorner.get_j()), 4, vpColor::green);
}
- for(std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
- //Display red circle for RANSAC outliers
- vpDisplay::displayCircle(IMatching, (int) (it->get_i() + topLeftCorner.get_i()), (int) (it->get_j() +
- topLeftCorner.get_j()), 4, vpColor::red);
+ for (std::vector<vpImagePoint>::const_iterator it = m_ransacOutliers.begin(); it != m_ransacOutliers.end(); ++it) {
+ // Display red circle for RANSAC outliers
+ vpDisplay::displayCircle(IMatching, (int)(it->get_i() + topLeftCorner.get_i()),
+ (int)(it->get_j() + topLeftCorner.get_j()), 4, vpColor::red);
}
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ for (std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
int current_class_id = 0;
- if(mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]] < medianIndex) {
- current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t) it->trainIdx].class_id]];
+ 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;
+ // Shift of one unity the index of the training images which are after
+ // the current image
+ current_class_id = mapOfImageIdIndex[m_mapOfImageId[m_trainKeyPoints[(size_t)it->trainIdx].class_id]] + 1;
}
int indexI = current_class_id / nbWidth;
int indexJ = current_class_id - (indexI * nbWidth);
- vpImagePoint end((int)maxH*indexI + m_trainKeyPoints[(size_t) it->trainIdx].pt.y,
- (int)maxW*indexJ + m_trainKeyPoints[(size_t) it->trainIdx].pt.x);
- vpImagePoint start((int)maxH*medianI + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.y,
- (int)maxW*medianJ + m_queryFilteredKeyPoints[(size_t) it->queryIdx].pt.x);
+ 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
+ // 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);
}
}
@@ -1537,12 +1598,14 @@ void vpKeyPoint::displayMatching(const vpImage<unsigned char> &ICurrent, vpImage
\param I : Input image.
\param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
- the corresponding 3D point.
+ \param descriptors : Descriptors matrix with at each row the descriptors
+ values for each keypoint. \param trainPoints : Pointer to the list of 3D
+ train points, when a keypoint cannot be extracted, we need to remove the
+ corresponding 3D point.
*/
void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- std::vector<cv::Point3f> *trainPoints) {
+ std::vector<cv::Point3f> *trainPoints)
+{
double elapsedTime;
extract(I, keyPoints, descriptors, elapsedTime, trainPoints);
}
@@ -1552,12 +1615,14 @@ void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoi
\param matImg : Input image.
\param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
- the corresponding 3D point.
+ \param descriptors : Descriptors matrix with at each row the descriptors
+ values for each keypoint. \param trainPoints : Pointer to the list of 3D
+ train points, when a keypoint cannot be extracted, we need to remove the
+ corresponding 3D point.
*/
void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- std::vector<cv::Point3f> *trainPoints) {
+ std::vector<cv::Point3f> *trainPoints)
+{
double elapsedTime;
extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
}
@@ -1567,13 +1632,14 @@ void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPo
\param I : Input image.
\param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param elapsedTime : Elapsed time.
- \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
- the corresponding 3D point.
+ \param descriptors : Descriptors matrix with at each row the descriptors
+ values for each keypoint. \param elapsedTime : Elapsed time. \param
+ trainPoints : Pointer to the list of 3D train points, when a keypoint
+ cannot be extracted, we need to remove the corresponding 3D point.
*/
void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints)
+{
cv::Mat matImg;
vpImageConvert::convert(I, matImg, false);
extract(matImg, keyPoints, descriptors, elapsedTime, trainPoints);
@@ -1584,92 +1650,99 @@ void vpKeyPoint::extract(const vpImage<unsigned char> &I, std::vector<cv::KeyPoi
\param matImg : Input image.
\param keyPoints : List of keypoints we want to extract their descriptors.
- \param descriptors : Descriptors matrix with at each row the descriptors values for each keypoint.
- \param elapsedTime : Elapsed time.
- \param trainPoints : Pointer to the list of 3D train points, when a keypoint cannot be extracted, we need to remove
- the corresponding 3D point.
+ \param descriptors : Descriptors matrix with at each row the descriptors
+ values for each keypoint. \param elapsedTime : Elapsed time. \param
+ trainPoints : Pointer to the list of 3D train points, when a keypoint
+ cannot be extracted, we need to remove the corresponding 3D point.
*/
void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPoints, cv::Mat &descriptors,
- double &elapsedTime, std::vector<cv::Point3f> *trainPoints) {
+ double &elapsedTime, std::vector<cv::Point3f> *trainPoints)
+{
double t = vpTime::measureTimeMs();
bool first = true;
- for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
- itd != m_extractors.end(); ++itd) {
- if(first) {
+ for (std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator itd = m_extractors.begin();
+ itd != m_extractors.end(); ++itd) {
+ if (first) {
first = false;
- //Check if we have 3D object points information
- if(trainPoints != NULL && !trainPoints->empty()) {
- //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
+ // Check if we have 3D object points information
+ if (trainPoints != NULL && !trainPoints->empty()) {
+ // Copy the input list of keypoints, keypoints that cannot be computed
+ // are removed in the function compute
std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
- //Extract descriptors for the given list of keypoints
+ // Extract descriptors for the given list of keypoints
itd->second->compute(matImg, keyPoints, descriptors);
- if(keyPoints.size() != keyPoints_tmp.size()) {
- //Keypoints have been removed
- //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ if (keyPoints.size() != keyPoints_tmp.size()) {
+ // Keypoints have been removed
+ // Store the hash of a keypoint as the key and the index of the
+ // keypoint as the value
std::map<size_t, size_t> mapOfKeypointHashes;
size_t cpt = 0;
- for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end();
+ ++it, cpt++) {
mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
}
std::vector<cv::Point3f> trainPoints_tmp;
- for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
- if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
+ if (mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
}
}
- //Copy trainPoints_tmp to m_trainPoints
+ // Copy trainPoints_tmp to m_trainPoints
*trainPoints = trainPoints_tmp;
}
} else {
- //Extract descriptors for the given list of keypoints
+ // Extract descriptors for the given list of keypoints
itd->second->compute(matImg, keyPoints, descriptors);
}
} else {
- //Copy the input list of keypoints, keypoints that cannot be computed are removed in the function compute
+ // Copy the input list of keypoints, keypoints that cannot be computed
+ // are removed in the function compute
std::vector<cv::KeyPoint> keyPoints_tmp = keyPoints;
cv::Mat desc;
- //Extract descriptors for the given list of keypoints
+ // Extract descriptors for the given list of keypoints
itd->second->compute(matImg, keyPoints, desc);
- if(keyPoints.size() != keyPoints_tmp.size()) {
- //Keypoints have been removed
- //Store the hash of a keypoint as the key and the index of the keypoint as the value
+ if (keyPoints.size() != keyPoints_tmp.size()) {
+ // Keypoints have been removed
+ // Store the hash of a keypoint as the key and the index of the
+ // keypoint as the value
std::map<size_t, size_t> mapOfKeypointHashes;
size_t cpt = 0;
- for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end(); ++it, cpt++) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = keyPoints_tmp.begin(); it != keyPoints_tmp.end();
+ ++it, cpt++) {
mapOfKeypointHashes[myKeypointHash(*it)] = cpt;
}
std::vector<cv::Point3f> trainPoints_tmp;
cv::Mat descriptors_tmp;
- for(std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
- if(mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
- if(trainPoints != NULL && !trainPoints->empty()) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = keyPoints.begin(); it != keyPoints.end(); ++it) {
+ if (mapOfKeypointHashes.find(myKeypointHash(*it)) != mapOfKeypointHashes.end()) {
+ if (trainPoints != NULL && !trainPoints->empty()) {
trainPoints_tmp.push_back((*trainPoints)[mapOfKeypointHashes[myKeypointHash(*it)]]);
}
- if(!descriptors.empty()) {
- descriptors_tmp.push_back(descriptors.row((int) mapOfKeypointHashes[myKeypointHash(*it)]));
+ if (!descriptors.empty()) {
+ descriptors_tmp.push_back(descriptors.row((int)mapOfKeypointHashes[myKeypointHash(*it)]));
}
}
}
- if(trainPoints != NULL) {
- //Copy trainPoints_tmp to m_trainPoints
+ if (trainPoints != NULL) {
+ // Copy trainPoints_tmp to m_trainPoints
*trainPoints = trainPoints_tmp;
}
- //Copy descriptors_tmp to descriptors
+ // Copy descriptors_tmp to descriptors
descriptors_tmp.copyTo(descriptors);
}
- //Merge descriptors horizontally
- if(descriptors.empty()) {
+ // Merge descriptors horizontally
+ if (descriptors.empty()) {
desc.copyTo(descriptors);
} else {
cv::hconcat(descriptors, desc, descriptors);
@@ -1677,7 +1750,7 @@ void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPo
}
}
- if(keyPoints.size() != (size_t) descriptors.rows) {
+ if (keyPoints.size() != (size_t)descriptors.rows) {
std::cerr << "keyPoints.size() != (size_t) descriptors.rows" << std::endl;
}
elapsedTime = vpTime::measureTimeMs() - t;
@@ -1686,20 +1759,22 @@ void vpKeyPoint::extract(const cv::Mat &matImg, std::vector<cv::KeyPoint> &keyPo
/*!
Filter the matches using the desired filtering method.
*/
-void vpKeyPoint::filterMatches() {
+void vpKeyPoint::filterMatches()
+{
std::vector<cv::KeyPoint> queryKpts;
std::vector<cv::Point3f> trainPts;
std::vector<cv::DMatch> m;
- if(m_useKnn) {
- double max_dist = 0;
- //double min_dist = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ if (m_useKnn) {
+ // double max_dist = 0;
+ // double min_dist = std::numeric_limits<double>::max(); // create an
+ // error under Windows. To fix it we have to add #undef max
double min_dist = DBL_MAX;
double mean = 0.0;
std::vector<double> distance_vec(m_knnMatches.size());
- if(m_filterType == stdAndRatioDistanceThreshold) {
- for(size_t i = 0; i < m_knnMatches.size(); i++) {
+ if (m_filterType == stdAndRatioDistanceThreshold) {
+ for (size_t i = 0; i < m_knnMatches.size(); i++) {
double dist = m_knnMatches[i][0].distance;
mean += dist;
distance_vec[i] = dist;
@@ -1707,9 +1782,9 @@ void vpKeyPoint::filterMatches() {
if (dist < min_dist) {
min_dist = dist;
}
- if (dist > max_dist) {
- max_dist = dist;
- }
+ // if (dist > max_dist) {
+ // max_dist = dist;
+ //}
}
mean /= m_queryDescriptors.rows;
}
@@ -1718,18 +1793,21 @@ void vpKeyPoint::filterMatches() {
double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
double threshold = min_dist + stdev;
- for(size_t i = 0; i < m_knnMatches.size(); i++) {
- if(m_knnMatches[i].size() >= 2) {
- //Calculate ratio of the descriptor distance between the two nearest neighbors of the keypoint
+ for (size_t i = 0; i < m_knnMatches.size(); i++) {
+ if (m_knnMatches[i].size() >= 2) {
+ // Calculate ratio of the descriptor distance between the two nearest
+ // neighbors of the keypoint
float ratio = m_knnMatches[i][0].distance / m_knnMatches[i][1].distance;
-// float ratio = std::sqrt((vecMatches[i][0].distance * vecMatches[i][0].distance)
-// / (vecMatches[i][1].distance * vecMatches[i][1].distance));
+ // 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()) {
+ if (!m_trainPoints.empty()) {
trainPts.push_back(m_trainPoints[(size_t)m_knnMatches[i][0].trainIdx]);
}
queryKpts.push_back(m_queryKeyPoints[(size_t)m_knnMatches[i][0].queryIdx]);
@@ -1737,13 +1815,13 @@ void vpKeyPoint::filterMatches() {
}
}
} else {
- double max_dist = 0;
+ // double max_dist = 0;
// create an error under Windows. To fix it we have to add #undef max
- //double min_dist = std::numeric_limits<double>::max();
+ // double min_dist = std::numeric_limits<double>::max();
double min_dist = DBL_MAX;
double mean = 0.0;
std::vector<double> distance_vec(m_matches.size());
- for(size_t i = 0; i < m_matches.size(); i++) {
+ for (size_t i = 0; i < m_matches.size(); i++) {
double dist = m_matches[i].distance;
mean += dist;
distance_vec[i] = dist;
@@ -1751,25 +1829,28 @@ void vpKeyPoint::filterMatches() {
if (dist < min_dist) {
min_dist = dist;
}
- if (dist > max_dist) {
- max_dist = dist;
- }
+ // if (dist > max_dist) {
+ // max_dist = dist;
+ // }
}
mean /= m_queryDescriptors.rows;
double sq_sum = std::inner_product(distance_vec.begin(), distance_vec.end(), distance_vec.begin(), 0.0);
double stdev = std::sqrt(sq_sum / distance_vec.size() - mean * mean);
- //Define a threshold where we keep all keypoints whose the descriptor distance falls below a factor of the
- //minimum descriptor distance (for all the query keypoints)
- //or below the minimum descriptor distance + the standard deviation (calculated on all the query descriptor distances)
- double threshold = m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;
+ // Define a threshold where we keep all keypoints whose the descriptor
+ // distance falls below a factor of the minimum descriptor distance (for
+ // all the query keypoints) or below the minimum descriptor distance +
+ // the standard deviation (calculated on all the query descriptor
+ // distances)
+ double threshold =
+ m_filterType == constantFactorDistanceThreshold ? m_matchingFactorThreshold * min_dist : min_dist + stdev;
for (size_t i = 0; i < m_matches.size(); i++) {
- if(m_matches[i].distance <= threshold) {
- m.push_back(cv::DMatch((int) queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
+ if (m_matches[i].distance <= threshold) {
+ m.push_back(cv::DMatch((int)queryKpts.size(), m_matches[i].trainIdx, m_matches[i].distance));
- if(!m_trainPoints.empty()) {
+ if (!m_trainPoints.empty()) {
trainPts.push_back(m_trainPoints[(size_t)m_matches[i].trainIdx]);
}
queryKpts.push_back(m_queryKeyPoints[(size_t)m_matches[i].queryIdx]);
@@ -1777,27 +1858,28 @@ void vpKeyPoint::filterMatches() {
}
}
- if(m_useSingleMatchFilter) {
- //Eliminate matches where multiple query keypoints are matched to the same train keypoint
+ if (m_useSingleMatchFilter) {
+ // Eliminate matches where multiple query keypoints are matched to the
+ // same train keypoint
std::vector<cv::DMatch> mTmp;
std::vector<cv::Point3f> trainPtsTmp;
std::vector<cv::KeyPoint> queryKptsTmp;
std::map<int, int> mapOfTrainIdx;
- //Count the number of query points matched to the same train point
- for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
+ // Count the number of query points matched to the same train point
+ for (std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
mapOfTrainIdx[it->trainIdx]++;
}
- //Keep matches with only one correspondence
- for(std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
- if(mapOfTrainIdx[it->trainIdx] == 1) {
- mTmp.push_back(cv::DMatch((int) queryKptsTmp.size(), it->trainIdx, it->distance));
+ // Keep matches with only one correspondence
+ for (std::vector<cv::DMatch>::const_iterator it = m.begin(); it != m.end(); ++it) {
+ if (mapOfTrainIdx[it->trainIdx] == 1) {
+ mTmp.push_back(cv::DMatch((int)queryKptsTmp.size(), it->trainIdx, it->distance));
- if(!m_trainPoints.empty()) {
- trainPtsTmp.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ if (!m_trainPoints.empty()) {
+ trainPtsTmp.push_back(m_trainPoints[(size_t)it->trainIdx]);
}
- queryKptsTmp.push_back(queryKpts[(size_t) it->queryIdx]);
+ queryKptsTmp.push_back(queryKpts[(size_t)it->queryIdx]);
}
}
@@ -1812,95 +1894,92 @@ void vpKeyPoint::filterMatches() {
}
/*!
- Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
- of the keypoints detected in the current image after the matching).
+ Get the 3D coordinates of the object points matched (the corresponding 3D
+ coordinates in the object frame of the keypoints detected in the current
+ image after the matching).
\param objectPoints : List of 3D coordinates in the object frame.
*/
-void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const {
+void vpKeyPoint::getObjectPoints(std::vector<cv::Point3f> &objectPoints) const
+{
objectPoints = m_objectFilteredPoints;
}
/*!
- Get the 3D coordinates of the object points matched (the corresponding 3D coordinates in the object frame
- of the keypoints detected in the current image after the matching).
+ Get the 3D coordinates of the object points matched (the corresponding 3D
+ coordinates in the object frame of the keypoints detected in the current
+ image after the matching).
\param objectPoints : List of 3D coordinates in the object frame.
*/
-void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const {
+void vpKeyPoint::getObjectPoints(std::vector<vpPoint> &objectPoints) const
+{
vpConvert::convertFromOpenCV(m_objectFilteredPoints, objectPoints);
}
/*!
Get the query keypoints list in OpenCV type.
- \param keyPoints : List of query keypoints (or keypoints detected in the current image).
+ \param keyPoints : List of query keypoints (or keypoints detected in the
+ current image).
*/
-void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
- keyPoints = m_queryFilteredKeyPoints;
-}
+void vpKeyPoint::getQueryKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const { keyPoints = m_queryFilteredKeyPoints; }
/*!
Get the query keypoints list in ViSP type.
- \param keyPoints : List of query keypoints (or keypoints detected in the current image).
+ \param keyPoints : List of query keypoints (or keypoints detected in the
+ current image).
*/
-void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
- keyPoints = currentImagePointsList;
-}
+void vpKeyPoint::getQueryKeyPoints(std::vector<vpImagePoint> &keyPoints) const { keyPoints = currentImagePointsList; }
/*!
Get the train keypoints list in OpenCV type.
\param keyPoints : List of train keypoints (or reference keypoints).
*/
-void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const {
- keyPoints = m_trainKeyPoints;
-}
+void vpKeyPoint::getTrainKeyPoints(std::vector<cv::KeyPoint> &keyPoints) const { keyPoints = m_trainKeyPoints; }
/*!
Get the train keypoints list in ViSP type.
\param keyPoints : List of train keypoints (or reference keypoints).
*/
-void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const {
- keyPoints = referenceImagePointsList;
-}
+void vpKeyPoint::getTrainKeyPoints(std::vector<vpImagePoint> &keyPoints) const { keyPoints = referenceImagePointsList; }
/*!
- Get the train points (the 3D coordinates in the object frame) list in OpenCV type.
+ Get the train points (the 3D coordinates in the object frame) list in
+ OpenCV type.
\param points : List of train points (or reference points).
*/
-void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const {
- points = m_trainPoints;
-}
+void vpKeyPoint::getTrainPoints(std::vector<cv::Point3f> &points) const { points = m_trainPoints; }
/*!
- Get the train points (the 3D coordinates in the object frame) list in ViSP type.
+ Get the train points (the 3D coordinates in the object frame) list in ViSP
+ type.
\param points : List of train points (or reference points).
*/
-void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const {
- points = m_trainVpPoints;
-}
+void vpKeyPoint::getTrainPoints(std::vector<vpPoint> &points) const { points = m_trainVpPoints; }
/*!
- Initialize method for RANSAC parameters and for detectors, extractors and matcher, and for others parameters.
+ Initialize method for RANSAC parameters and for detectors, extractors and
+ matcher, and for others parameters.
*/
-void vpKeyPoint::init() {
- // Require 2.4.0 <= opencv < 3.0.0
+void vpKeyPoint::init()
+{
+// Require 2.4.0 <= opencv < 3.0.0
#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x020400) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
- //The following line must be called in order to use SIFT or SURF
+ // 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) {
+ // 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) {
m_useKnn = true;
}
@@ -1914,14 +1993,15 @@ void vpKeyPoint::init() {
\param detectorName : Name of the detector (e.g FAST, SIFT, SURF, etc.).
*/
-void vpKeyPoint::initDetector(const std::string &detectorName) {
+void vpKeyPoint::initDetector(const std::string &detectorName)
+{
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
m_detectors[detectorName] = cv::FeatureDetector::create(detectorName);
- if(m_detectors[detectorName] == NULL) {
+ 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 << ".";
+ ss_msg << "Fail to initialize the detector: " << detectorName
+ << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
throw vpException(vpException::fatalError, ss_msg.str());
}
#else
@@ -1934,7 +2014,7 @@ void vpKeyPoint::initDetector(const std::string &detectorName) {
usePyramid = true;
}
- if(detectorNameTmp == "SIFT") {
+ if (detectorNameTmp == "SIFT") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
cv::Ptr<cv::FeatureDetector> siftDetector = cv::xfeatures2d::SIFT::create();
if (!usePyramid) {
@@ -1945,8 +2025,8 @@ void vpKeyPoint::initDetector(const std::string &detectorName) {
}
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: SIFT. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ 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") {
@@ -1960,8 +2040,8 @@ void vpKeyPoint::initDetector(const std::string &detectorName) {
}
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: SURF. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ 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") {
@@ -2034,54 +2114,53 @@ void vpKeyPoint::initDetector(const std::string &detectorName) {
}
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the detector: STAR. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ ss_msg << "Fail to initialize the detector: STAR. OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION
+ << " was not build with xFeatures2d module.";
throw vpException(vpException::fatalError, ss_msg.str());
#endif
} else if (detectorNameTmp == "AGAST") {
cv::Ptr<cv::FeatureDetector> agastDetector = cv::AgastFeatureDetector::create();
- if(!usePyramid) {
+ if (!usePyramid) {
m_detectors[detectorNameTmp] = agastDetector;
} else {
m_detectors[detectorName] = cv::makePtr<PyramidAdaptedFeatureDetector>(agastDetector);
}
} else if (detectorNameTmp == "MSD") {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030100)
- #if defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
cv::Ptr<cv::FeatureDetector> msdDetector = cv::xfeatures2d::MSDDetector::create();
- if(!usePyramid) {
+ 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<PyramidAdaptedFeatureDetector>(msdDetector);
}
- #else
- std::stringstream ss_msg;
- 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 << VISP_HAVE_OPENCV_VERSION << " (require >= OpenCV 3.1).";
+ 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 {
+#else
+ std::stringstream ss_msg;
+ 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;
}
bool detectorInitialized = false;
- if(!usePyramid) {
+ if (!usePyramid) {
detectorInitialized = (m_detectors[detectorNameTmp] != NULL);
} else {
detectorInitialized = (m_detectors[detectorName] != NULL);
}
- if(!detectorInitialized) {
+ 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 << ".";
+ ss_msg << "Fail to initialize the detector: " << detectorNameTmp
+ << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
throw vpException(vpException::fatalError, ss_msg.str());
}
@@ -2089,12 +2168,14 @@ void vpKeyPoint::initDetector(const std::string &detectorName) {
}
/*!
- Initialize a list of keypoints detectors if we want to concatenate multiple detectors.
+ Initialize a list of keypoints detectors if we want to concatenate multiple
+ detectors.
\param detectorNames : List of detector names.
*/
-void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
- for(std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
+void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames)
+{
+ for (std::vector<std::string>::const_iterator it = detectorNames.begin(); it != detectorNames.end(); ++it) {
initDetector(*it);
}
}
@@ -2104,211 +2185,214 @@ void vpKeyPoint::initDetectors(const std::vector<std::string> &detectorNames) {
\param extractorName : Name of the extractor (e.g SIFT, SURF, ORB, etc.).
*/
-void vpKeyPoint::initExtractor(const std::string &extractorName) {
+void vpKeyPoint::initExtractor(const std::string &extractorName)
+{
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
m_extractors[extractorName] = cv::DescriptorExtractor::create(extractorName);
#else
- if(extractorName == "SIFT") {
+ if (extractorName == "SIFT") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
m_extractors[extractorName] = cv::xfeatures2d::SIFT::create();
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: SIFT. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ 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") {
+ } else if (extractorName == "SURF") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
- //Use extended set of SURF descriptors (128 instead of 64)
+ // Use extended set of SURF descriptors (128 instead of 64)
m_extractors[extractorName] = cv::xfeatures2d::SURF::create(100, 4, 3, true);
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: SURF. OpenCV version "
- << std::hex << VISP_HAVE_OPENCV_VERSION << " was not build with xFeatures2d module.";
+ ss_msg << "Fail to initialize the extractor: SURF. OpenCV version " << std::hex << VISP_HAVE_OPENCV_VERSION
+ << " was not build with xFeatures2d module.";
throw vpException(vpException::fatalError, ss_msg.str());
#endif
- } else if(extractorName == "ORB") {
+ } else if (extractorName == "ORB") {
m_extractors[extractorName] = cv::ORB::create();
- } else if(extractorName == "BRISK") {
+ } else if (extractorName == "BRISK") {
m_extractors[extractorName] = cv::BRISK::create();
- } else if(extractorName == "FREAK") {
+ } else if (extractorName == "FREAK") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
m_extractors[extractorName] = cv::xfeatures2d::FREAK::create();
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: " << 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") {
+ } else if (extractorName == "BRIEF") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
m_extractors[extractorName] = cv::xfeatures2d::BriefDescriptorExtractor::create();
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: " << 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") {
+ } else if (extractorName == "KAZE") {
m_extractors[extractorName] = cv::KAZE::create();
- } else if(extractorName == "AKAZE") {
+ } else if (extractorName == "AKAZE") {
m_extractors[extractorName] = cv::AKAZE::create();
- } else if(extractorName == "DAISY") {
+ } else if (extractorName == "DAISY") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
m_extractors[extractorName] = cv::xfeatures2d::DAISY::create();
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: " << 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") {
+ } else if (extractorName == "LATCH") {
#ifdef VISP_HAVE_OPENCV_XFEATURES2D
m_extractors[extractorName] = cv::xfeatures2d::LATCH::create();
#else
std::stringstream ss_msg;
- ss_msg << "Fail to initialize the extractor: " << 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") {
+ } 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
+ // 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.");
#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") {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030200)
- #if defined (VISP_HAVE_OPENCV_XFEATURES2D)
- 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.";
- throw vpException(vpException::fatalError, ss_msg.str());
- #endif
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ 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 << " but requires at least OpenCV 3.2.";
+ 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.";
throw vpException(vpException::fatalError, ss_msg.str());
#endif
} else if (extractorName == "BoostDesc") {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030200)
- #if defined (VISP_HAVE_OPENCV_XFEATURES2D)
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
m_extractors[extractorName] = cv::xfeatures2d::BoostDesc::create();
- #else
+#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
+#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 {
+ } else {
std::cerr << "The extractor:" << extractorName << " is not available." << std::endl;
}
#endif
- if(m_extractors[extractorName] == NULL) {
+ 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 << ".";
+ ss_msg << "Fail to initialize the extractor: " << extractorName
+ << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
throw vpException(vpException::fatalError, ss_msg.str());
}
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
- if(extractorName == "SURF") {
- //Use extended set of SURF descriptors (128 instead of 64)
+ if (extractorName == "SURF") {
+ // Use extended set of SURF descriptors (128 instead of 64)
m_extractors[extractorName]->set("extended", 1);
}
#endif
}
/*!
- Initialize a list of descriptor extractors if we want to concatenate multiple extractors.
+ Initialize a list of descriptor extractors if we want to concatenate
+ multiple extractors.
\param extractorNames : List of extractor names.
*/
-void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames) {
- for(std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
+void vpKeyPoint::initExtractors(const std::vector<std::string> &extractorNames)
+{
+ for (std::vector<std::string>::const_iterator it = extractorNames.begin(); it != extractorNames.end(); ++it) {
initExtractor(*it);
}
int descriptorType = CV_32F;
bool firstIteration = true;
- for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
- it != m_extractors.end(); ++it) {
- if(firstIteration) {
+ for (std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
+ it != m_extractors.end(); ++it) {
+ if (firstIteration) {
firstIteration = false;
descriptorType = it->second->descriptorType();
} else {
- if(descriptorType != it->second->descriptorType()) {
+ if (descriptorType != it->second->descriptorType()) {
throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
}
}
}
}
-void vpKeyPoint::initFeatureNames() {
- //Create map enum to string
+void vpKeyPoint::initFeatureNames()
+{
+// Create map enum to string
#if (VISP_HAVE_OPENCV_VERSION >= 0x020403)
- m_mapOfDetectorNames[DETECTOR_FAST] = "FAST";
- m_mapOfDetectorNames[DETECTOR_MSER] = "MSER";
- m_mapOfDetectorNames[DETECTOR_ORB] = "ORB";
- 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))
- m_mapOfDetectorNames[DETECTOR_STAR] = "STAR";
- #endif
- #if defined(VISP_HAVE_OPENCV_NONFREE) || defined (VISP_HAVE_OPENCV_XFEATURES2D)
- m_mapOfDetectorNames[DETECTOR_SIFT] = "SIFT";
- m_mapOfDetectorNames[DETECTOR_SURF] = "SURF";
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- m_mapOfDetectorNames[DETECTOR_KAZE] = "KAZE";
- m_mapOfDetectorNames[DETECTOR_AKAZE] = "AKAZE";
- m_mapOfDetectorNames[DETECTOR_AGAST] = "AGAST";
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined (VISP_HAVE_OPENCV_XFEATURES2D)
- m_mapOfDetectorNames[DETECTOR_MSD] = "MSD";
- #endif
+ m_mapOfDetectorNames[DETECTOR_FAST] = "FAST";
+ m_mapOfDetectorNames[DETECTOR_MSER] = "MSER";
+ m_mapOfDetectorNames[DETECTOR_ORB] = "ORB";
+ 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))
+ m_mapOfDetectorNames[DETECTOR_STAR] = "STAR";
+#endif
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ m_mapOfDetectorNames[DETECTOR_SIFT] = "SIFT";
+ m_mapOfDetectorNames[DETECTOR_SURF] = "SURF";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ m_mapOfDetectorNames[DETECTOR_KAZE] = "KAZE";
+ m_mapOfDetectorNames[DETECTOR_AKAZE] = "AKAZE";
+ m_mapOfDetectorNames[DETECTOR_AGAST] = "AGAST";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030100) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ m_mapOfDetectorNames[DETECTOR_MSD] = "MSD";
+#endif
#endif
#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))
- m_mapOfDescriptorNames[DESCRIPTOR_FREAK] = "FREAK";
- m_mapOfDescriptorNames[DESCRIPTOR_BRIEF] = "BRIEF";
+ m_mapOfDescriptorNames[DESCRIPTOR_ORB] = "ORB";
+ m_mapOfDescriptorNames[DESCRIPTOR_BRISK] = "BRISK";
+#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || (defined(VISP_HAVE_OPENCV_XFEATURES2D))
+ m_mapOfDescriptorNames[DESCRIPTOR_FREAK] = "FREAK";
+ m_mapOfDescriptorNames[DESCRIPTOR_BRIEF] = "BRIEF";
+#endif
+#if defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ m_mapOfDescriptorNames[DESCRIPTOR_SIFT] = "SIFT";
+ m_mapOfDescriptorNames[DESCRIPTOR_SURF] = "SURF";
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ m_mapOfDescriptorNames[DESCRIPTOR_KAZE] = "KAZE";
+ m_mapOfDescriptorNames[DESCRIPTOR_AKAZE] = "AKAZE";
+#if defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ m_mapOfDescriptorNames[DESCRIPTOR_DAISY] = "DAISY";
+ m_mapOfDescriptorNames[DESCRIPTOR_LATCH] = "LATCH";
+#endif
+#endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
+ m_mapOfDescriptorNames[DESCRIPTOR_VGG] = "VGG";
+ m_mapOfDescriptorNames[DESCRIPTOR_BoostDesc] = "BoostDesc";
#endif
- #if defined(VISP_HAVE_OPENCV_NONFREE) || defined (VISP_HAVE_OPENCV_XFEATURES2D)
- m_mapOfDescriptorNames[DESCRIPTOR_SIFT] = "SIFT";
- m_mapOfDescriptorNames[DESCRIPTOR_SURF] = "SURF";
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- m_mapOfDescriptorNames[DESCRIPTOR_KAZE] = "KAZE";
- m_mapOfDescriptorNames[DESCRIPTOR_AKAZE] = "AKAZE";
- #if defined (VISP_HAVE_OPENCV_XFEATURES2D)
- m_mapOfDescriptorNames[DESCRIPTOR_DAISY] = "DAISY";
- m_mapOfDescriptorNames[DESCRIPTOR_LATCH] = "LATCH";
- #endif
- #endif
- #if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined (VISP_HAVE_OPENCV_XFEATURES2D)
- m_mapOfDescriptorNames[DESCRIPTOR_VGG] = "VGG";
- m_mapOfDescriptorNames[DESCRIPTOR_BoostDesc] = "BoostDesc";
- #endif
#endif
}
@@ -2317,28 +2401,31 @@ void vpKeyPoint::initFeatureNames() {
\param matcherName : Name of the matcher (e.g BruteForce, FlannBased).
*/
-void vpKeyPoint::initMatcher(const std::string &matcherName) {
+void vpKeyPoint::initMatcher(const std::string &matcherName)
+{
int descriptorType = CV_32F;
bool firstIteration = true;
- for(std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
- it != m_extractors.end(); ++it) {
- if(firstIteration) {
+ for (std::map<std::string, cv::Ptr<cv::DescriptorExtractor> >::const_iterator it = m_extractors.begin();
+ it != m_extractors.end(); ++it) {
+ if (firstIteration) {
firstIteration = false;
descriptorType = it->second->descriptorType();
} else {
- if(descriptorType != it->second->descriptorType()) {
+ if (descriptorType != it->second->descriptorType()) {
throw vpException(vpException::fatalError, "All the descriptors must have the same type !");
}
}
}
- if(matcherName == "FlannBased") {
- if(m_extractors.empty()) {
- std::cout << "Warning: No extractor initialized, by default use floating values (CV_32F) "
- "for descriptor type !" << std::endl;
+ if (matcherName == "FlannBased") {
+ if (m_extractors.empty()) {
+ std::cout << "Warning: No extractor initialized, by default use "
+ "floating values (CV_32F) "
+ "for descriptor type !"
+ << std::endl;
}
- if(descriptorType == CV_8U) {
+ if (descriptorType == CV_8U) {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
m_matcher = cv::makePtr<cv::FlannBasedMatcher>(cv::makePtr<cv::flann::LshIndexParams>(12, 20, 2));
#else
@@ -2356,15 +2443,15 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) {
}
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
- if(m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
+ if (m_matcher != NULL && !m_useKnn && matcherName == "BruteForce") {
m_matcher->set("crossCheck", m_useBruteForceCrossCheck);
}
#endif
- if(m_matcher == NULL) {
+ 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 << ".";
+ ss_msg << "Fail to initialize the matcher: " << matcherName
+ << " or it is not available in OpenCV version: " << std::hex << VISP_HAVE_OPENCV_VERSION << ".";
throw vpException(vpException::fatalError, ss_msg.str());
}
}
@@ -2374,11 +2461,12 @@ void vpKeyPoint::initMatcher(const std::string &matcherName) {
\param IRef : Reference image.
\param ICurrent : Current image.
- \param IMatching : Matching image for displaying all the matching between the query keypoints and those
- detected in the training images.
+ \param IMatching : Matching image for displaying all the matching between
+ the query keypoints and those detected in the training images.
*/
void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &IRef, const vpImage<unsigned char> &ICurrent,
- vpImage<unsigned char> &IMatching) {
+ vpImage<unsigned char> &IMatching)
+{
vpImagePoint topLeftCorner(0, 0);
IMatching.insert(IRef, topLeftCorner);
topLeftCorner = vpImagePoint(0, IRef.getWidth());
@@ -2389,62 +2477,68 @@ void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &IRef, const v
Insert the different training images in the matching image.
\param ICurrent : Current image.
- \param IMatching : Matching image for displaying all the matching between the query keypoints and those
- detected in the training images
+ \param IMatching : Matching image for displaying all the matching between
+ the query keypoints and those detected in the training images
*/
-void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching) {
- //Nb images in the training database + the current image we want to detect the object
- int nbImg = (int) (m_mapOfImages.size() + 1);
+void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpImage<unsigned char> &IMatching)
+{
+ // Nb images in the training database + the current image we want to detect
+ // the object
+ int nbImg = (int)(m_mapOfImages.size() + 1);
- if(m_mapOfImages.empty()) {
+ if (m_mapOfImages.empty()) {
std::cerr << "There is no training image loaded !" << std::endl;
return;
}
- if(nbImg == 2) {
- //Only one training image, so we display them side by side
+ if (nbImg == 2) {
+ // Only one training image, so we display them side by side
insertImageMatching(m_mapOfImages.begin()->second, ICurrent, IMatching);
} else {
- //Multiple training images, display them as a mosaic image
- int nbImgSqrt = vpMath::round(std::sqrt((double) nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
+ // Multiple training images, display them as a mosaic image
+ int nbImgSqrt = vpMath::round(std::sqrt((double)nbImg)); //(int) std::floor(std::sqrt((double) nbImg) + 0.5);
int nbWidth = nbImgSqrt;
int nbHeight = nbImgSqrt;
- if(nbImgSqrt * nbImgSqrt < nbImg) {
+ if (nbImgSqrt * nbImgSqrt < nbImg) {
nbWidth++;
}
unsigned int maxW = ICurrent.getWidth(), maxH = ICurrent.getHeight();
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(maxW < it->second.getWidth()) {
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it) {
+ if (maxW < it->second.getWidth()) {
maxW = it->second.getWidth();
}
- if(maxH < it->second.getHeight()) {
+ if (maxH < it->second.getHeight()) {
maxH = it->second.getHeight();
}
}
- //Indexes of the current image in the grid made in order to the image is in the center square in the mosaic grid
+ // Indexes of the current image in the grid made in order to the image is
+ // in the center square in the mosaic grid
int medianI = nbHeight / 2;
int medianJ = nbWidth / 2;
int medianIndex = medianI * nbWidth + medianJ;
int cpt = 0;
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it, cpt++) {
int local_cpt = cpt;
- if(cpt >= medianIndex) {
- //Shift of one unity the index of the training images which are after the current image
+ if (cpt >= medianIndex) {
+ // Shift of one unity the index of the training images which are after
+ // the current image
local_cpt++;
}
int indexI = local_cpt / nbWidth;
int indexJ = local_cpt - (indexI * nbWidth);
- vpImagePoint topLeftCorner((int)maxH*indexI, (int)maxW*indexJ);
+ vpImagePoint topLeftCorner((int)maxH * indexI, (int)maxW * indexJ);
IMatching.insert(it->second, topLeftCorner);
}
- vpImagePoint topLeftCorner((int)maxH*medianI, (int)maxW*medianJ);
+ vpImagePoint topLeftCorner((int)maxH * medianI, (int)maxW * medianJ);
IMatching.insert(ICurrent, topLeftCorner);
}
}
@@ -2455,24 +2549,27 @@ void vpKeyPoint::insertImageMatching(const vpImage<unsigned char> &ICurrent, vpI
\param configFile : Path to the XML config file.
*/
-void vpKeyPoint::loadConfigFile(const std::string &configFile) {
+void vpKeyPoint::loadConfigFile(const std::string &configFile)
+{
vpXmlConfigParserKeyPoint xmlp;
try {
- //Reset detector and extractor
+ // Reset detector and extractor
m_detectorNames.clear();
m_extractorNames.clear();
m_detectors.clear();
m_extractors.clear();
- std::cout << " *********** Parsing XML for configuration for vpKeyPoint ************ " << std::endl;
+ std::cout << " *********** Parsing XML for configuration for vpKeyPoint "
+ "************ "
+ << std::endl;
xmlp.parse(configFile);
m_detectorNames.push_back(xmlp.getDetectorName());
m_extractorNames.push_back(xmlp.getExtractorName());
m_matcherName = xmlp.getMatcherName();
- switch(xmlp.getMatchingMethod()) {
+ switch (xmlp.getMatchingMethod()) {
case vpXmlConfigParserKeyPoint::constantFactorDistanceThreshold:
m_filterType = constantFactorDistanceThreshold;
break;
@@ -2508,15 +2605,14 @@ 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;
}
init();
- }
- catch(...) {
+ } catch (...) {
throw vpException(vpException::ioError, "Can't open XML file \"%s\"\n ", configFile.c_str());
}
}
@@ -2526,70 +2622,72 @@ void vpKeyPoint::loadConfigFile(const std::string &configFile) {
Load learning data saved on disk.
\param filename : Path of the learning file.
- \param binaryMode : If true, the learning file is in a binary mode, otherwise it is in XML mode.
- \param append : If true, concatenate the learning data, otherwise reset the variables.
+ \param binaryMode : If true, the learning file is in a binary mode,
+ otherwise it is in XML mode. \param append : If true, concatenate the
+ learning data, otherwise reset the variables.
*/
-void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append) {
+void vpKeyPoint::loadLearningData(const std::string &filename, const bool binaryMode, const bool append)
+{
int startClassId = 0;
int startImageId = 0;
- if(!append) {
+ if (!append) {
m_trainKeyPoints.clear();
m_trainPoints.clear();
m_mapOfImageId.clear();
m_mapOfImages.clear();
} else {
- //In append case, find the max index of keypoint class Id
- for(std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
- if(startClassId < it->first) {
+ // In append case, find the max index of keypoint class Id
+ for (std::map<int, int>::const_iterator it = m_mapOfImageId.begin(); it != m_mapOfImageId.end(); ++it) {
+ if (startClassId < it->first) {
startClassId = it->first;
}
}
- //In append case, find the max index of images Id
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it) {
- if(startImageId < it->first) {
+ // In append case, find the max index of images Id
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it) {
+ if (startImageId < it->first) {
startImageId = it->first;
}
}
}
- //Get parent directory
+ // Get parent directory
std::string parent = vpIoTools::getParent(filename);
- if(!parent.empty()) {
+ if (!parent.empty()) {
parent += "/";
}
- if(binaryMode) {
+ if (binaryMode) {
std::ifstream file(filename.c_str(), std::ifstream::binary);
- if(!file.is_open()){
+ if (!file.is_open()) {
throw vpException(vpException::ioError, "Cannot open the file.");
}
- //Read info about training images
+ // Read info about training images
int nbImgs = 0;
-// file.read((char *)(&nbImgs), sizeof(nbImgs));
readBinaryIntLE(file, nbImgs);
#if !defined(VISP_HAVE_MODULE_IO)
- if(nbImgs > 0) {
- std::cout << "Warning: The learning file contains image data that will not be loaded as visp_io module "
- "is not available !" << std::endl;
+ if (nbImgs > 0) {
+ std::cout << "Warning: The learning file contains image data that will "
+ "not be loaded as visp_io module "
+ "is not available !"
+ << std::endl;
}
#endif
- for(int i = 0; i < nbImgs; i++) {
- //Read image_id
+ for (int i = 0; i < nbImgs; i++) {
+ // Read image_id
int id = 0;
-// file.read((char *)(&id), sizeof(id));
readBinaryIntLE(file, id);
int length = 0;
-// file.read((char *)(&length), sizeof(length));
readBinaryIntLE(file, length);
- //Will contain the path to the training images
- char* path = new char[length + 1];//char path[length + 1];
+ // Will contain the path to the training images
+ char *path = new char[length + 1]; // char path[length + 1];
- for(int cpt = 0; cpt < length; cpt++) {
+ for (int cpt = 0; cpt < length; cpt++) {
char c;
file.read((char *)(&c), sizeof(c));
path[cpt] = c;
@@ -2598,161 +2696,124 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
vpImage<unsigned char> I;
#ifdef VISP_HAVE_MODULE_IO
- if(vpIoTools::isAbsolutePathname(std::string(path))) {
+ if (vpIoTools::isAbsolutePathname(std::string(path))) {
vpImageIo::read(I, path);
} else {
vpImageIo::read(I, parent + path);
}
- //Add the image previously loaded only if VISP_HAVE_MODULE_IO
+ // Add the image previously loaded only if VISP_HAVE_MODULE_IO
m_mapOfImages[id + startImageId] = I;
#endif
- //Delete path
+ // Delete path
delete[] path;
}
- //Read if 3D point information are saved or not
+ // Read if 3D point information are saved or not
int have3DInfoInt = 0;
-// file.read((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
readBinaryIntLE(file, have3DInfoInt);
bool have3DInfo = have3DInfoInt != 0;
- //Read the number of descriptors
+ // Read the number of descriptors
int nRows = 0;
-// file.read((char *)(&nRows), sizeof(nRows));
readBinaryIntLE(file, nRows);
- //Read the size of the descriptor
+ // Read the size of the descriptor
int nCols = 0;
-// file.read((char *)(&nCols), sizeof(nCols));
readBinaryIntLE(file, nCols);
- //Read the type of the descriptor
- int descriptorType = 5; //CV_32F
-// file.read((char *)(&descriptorType), sizeof(descriptorType));
+ // Read the type of the descriptor
+ int descriptorType = 5; // CV_32F
readBinaryIntLE(file, descriptorType);
cv::Mat trainDescriptorsTmp = cv::Mat(nRows, nCols, descriptorType);
- for(int i = 0; i < nRows; i++) {
- //Read information about keyPoint
+ for (int i = 0; i < nRows; i++) {
+ // Read information about keyPoint
float u, v, size, angle, response;
int octave, class_id, image_id;
-// file.read((char *)(&u), sizeof(u));
readBinaryFloatLE(file, u);
-// file.read((char *)(&v), sizeof(v));
readBinaryFloatLE(file, v);
-// file.read((char *)(&size), sizeof(size));
readBinaryFloatLE(file, size);
-// file.read((char *)(&angle), sizeof(angle));
readBinaryFloatLE(file, angle);
-// file.read((char *)(&response), sizeof(response));
readBinaryFloatLE(file, response);
-// file.read((char *)(&octave), sizeof(octave));
readBinaryIntLE(file, octave);
-// file.read((char *)(&class_id), sizeof(class_id));
readBinaryIntLE(file, class_id);
-// file.read((char *)(&image_id), sizeof(image_id));
readBinaryIntLE(file, image_id);
cv::KeyPoint keyPoint(cv::Point2f(u, v), size, angle, response, octave, (class_id + startClassId));
m_trainKeyPoints.push_back(keyPoint);
- if(image_id != -1) {
+ if (image_id != -1) {
#ifdef VISP_HAVE_MODULE_IO
- //No training images if image_id == -1
- m_mapOfImageId[class_id] = image_id + startImageId;
+ // No training images if image_id == -1
+ m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
#endif
}
- if(have3DInfo) {
- //Read oX, oY, oZ
+ if (have3DInfo) {
+ // Read oX, oY, oZ
float oX, oY, oZ;
-// file.read((char *)(&oX), sizeof(oX));
readBinaryFloatLE(file, oX);
-// file.read((char *)(&oY), sizeof(oY));
readBinaryFloatLE(file, oY);
-// file.read((char *)(&oZ), sizeof(oZ));
readBinaryFloatLE(file, oZ);
m_trainPoints.push_back(cv::Point3f(oX, oY, oZ));
}
- for(int j = 0; j < nCols; j++) {
- //Read the value of the descriptor
- switch(descriptorType) {
- case CV_8U:
- {
- unsigned char value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<unsigned char>(i, j) = value;
- }
- break;
-
- case CV_8S:
- {
- char value;
- file.read((char *)(&value), sizeof(value));
- trainDescriptorsTmp.at<char>(i, j) = value;
- }
- break;
-
- case CV_16U:
- {
- unsigned short int value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryUShortLE(file, value);
- trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
- }
- break;
-
- case CV_16S:
- {
- short int value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryShortLE(file, value);
- trainDescriptorsTmp.at<short int>(i, j) = value;
- }
- break;
-
- case CV_32S:
- {
- int value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryIntLE(file, value);
- trainDescriptorsTmp.at<int>(i, j) = value;
- }
- break;
-
- case CV_32F:
- {
- float value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryFloatLE(file, value);
- trainDescriptorsTmp.at<float>(i, j) = value;
- }
- break;
-
- case CV_64F:
- {
- double value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryDoubleLE(file, value);
- trainDescriptorsTmp.at<double>(i, j) = value;
- }
- break;
-
- default:
- {
- float value;
-// file.read((char *)(&value), sizeof(value));
- readBinaryFloatLE(file, value);
- trainDescriptorsTmp.at<float>(i, j) = value;
- }
- break;
+ for (int j = 0; j < nCols; j++) {
+ // Read the value of the descriptor
+ switch (descriptorType) {
+ case CV_8U: {
+ unsigned char value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<unsigned char>(i, j) = value;
+ } break;
+
+ case CV_8S: {
+ char value;
+ file.read((char *)(&value), sizeof(value));
+ trainDescriptorsTmp.at<char>(i, j) = value;
+ } break;
+
+ case CV_16U: {
+ unsigned short int value;
+ readBinaryUShortLE(file, value);
+ trainDescriptorsTmp.at<unsigned short int>(i, j) = value;
+ } break;
+
+ case CV_16S: {
+ short int value;
+ readBinaryShortLE(file, value);
+ trainDescriptorsTmp.at<short int>(i, j) = value;
+ } break;
+
+ case CV_32S: {
+ int value;
+ readBinaryIntLE(file, value);
+ trainDescriptorsTmp.at<int>(i, j) = value;
+ } break;
+
+ case CV_32F: {
+ float value;
+ readBinaryFloatLE(file, value);
+ trainDescriptorsTmp.at<float>(i, j) = value;
+ } break;
+
+ case CV_64F: {
+ double value;
+ readBinaryDoubleLE(file, value);
+ trainDescriptorsTmp.at<double>(i, j) = value;
+ } break;
+
+ default: {
+ float value;
+ readBinaryFloatLE(file, value);
+ trainDescriptorsTmp.at<float>(i, j) = value;
+ } break;
}
}
}
- if(!append || m_trainDescriptors.empty()) {
+ if (!append || m_trainDescriptors.empty()) {
trainDescriptorsTmp.copyTo(m_trainDescriptors);
} else {
cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
@@ -2783,60 +2844,58 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
xmlNodePtr first_level_node = NULL;
char *pEnd = NULL;
- int descriptorType = CV_32F; //float
+ int descriptorType = CV_32F; // float
int nRows = 0, nCols = 0;
int i = 0, j = 0;
cv::Mat trainDescriptorsTmp;
- for (first_level_node = root_element->children; first_level_node;
- first_level_node = first_level_node->next) {
+ 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);
+ std::string name((char *)first_level_node->name);
if (first_level_node->type == XML_ELEMENT_NODE && name == "TrainingImageInfo") {
xmlNodePtr image_info_node = NULL;
- for (image_info_node = first_level_node->children; image_info_node; image_info_node =
- image_info_node->next) {
- name = std::string ((char *) image_info_node->name);
+ 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
+ if (name == "trainImg") {
+ // Read image_id
xmlChar *image_id_property = xmlGetProp(image_info_node, BAD_CAST "image_id");
int id = 0;
- if(image_id_property) {
- id = std::atoi((char *) image_id_property);
+ if (image_id_property) {
+ id = std::atoi((char *)image_id_property);
}
xmlFree(image_id_property);
vpImage<unsigned char> I;
#ifdef VISP_HAVE_MODULE_IO
- std::string path((char *) image_info_node->children->content);
- //Read path to the training images
- if(vpIoTools::isAbsolutePathname(std::string(path))) {
+ std::string path((char *)image_info_node->children->content);
+ // Read path to the training images
+ if (vpIoTools::isAbsolutePathname(std::string(path))) {
vpImageIo::read(I, path);
} else {
vpImageIo::read(I, parent + path);
}
- //Add the image previously loaded only if VISP_HAVE_MODULE_IO
+ // Add the image previously loaded only if VISP_HAVE_MODULE_IO
m_mapOfImages[id + startImageId] = I;
#endif
}
}
- } else if(first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
+ } else if (first_level_node->type == XML_ELEMENT_NODE && name == "DescriptorsInfo") {
xmlNodePtr descriptors_info_node = NULL;
- for (descriptors_info_node = first_level_node->children; descriptors_info_node; descriptors_info_node =
- descriptors_info_node->next) {
+ for (descriptors_info_node = first_level_node->children; descriptors_info_node;
+ descriptors_info_node = descriptors_info_node->next) {
if (descriptors_info_node->type == XML_ELEMENT_NODE) {
- name = std::string ((char *) descriptors_info_node->name);
-
- if(name == "nrows") {
- nRows = std::atoi((char *) descriptors_info_node->children->content);
- } else if(name == "ncols") {
- nCols = std::atoi((char *) descriptors_info_node->children->content);
- } else if(name == "type") {
- descriptorType = std::atoi((char *) descriptors_info_node->children->content);
+ name = std::string((char *)descriptors_info_node->name);
+
+ if (name == "nrows") {
+ nRows = std::atoi((char *)descriptors_info_node->children->content);
+ } else if (name == "ncols") {
+ nCols = std::atoi((char *)descriptors_info_node->children->content);
+ } else if (name == "type") {
+ descriptorType = std::atoi((char *)descriptors_info_node->children->content);
}
}
}
@@ -2850,98 +2909,94 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
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);
-
- //Read information about keypoints
- if(name == "u") {
- u = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "v") {
- v = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "size") {
- size = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "angle") {
- angle = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "response") {
- response = std::strtod((char *) point_node->children->content, &pEnd);
- } else if(name == "octave") {
- octave = std::atoi((char *) point_node->children->content);
- } else if(name == "class_id") {
- class_id = std::atoi((char *) point_node->children->content);
- cv::KeyPoint keyPoint(cv::Point2f((float) u, (float) v), (float) size,
- (float) angle, (float) response, octave, (class_id + startClassId));
+ name = std::string((char *)point_node->name);
+
+ // Read information about keypoints
+ if (name == "u") {
+ u = std::strtod((char *)point_node->children->content, &pEnd);
+ } else if (name == "v") {
+ v = std::strtod((char *)point_node->children->content, &pEnd);
+ } else if (name == "size") {
+ size = std::strtod((char *)point_node->children->content, &pEnd);
+ } else if (name == "angle") {
+ angle = std::strtod((char *)point_node->children->content, &pEnd);
+ } else if (name == "response") {
+ response = std::strtod((char *)point_node->children->content, &pEnd);
+ } else if (name == "octave") {
+ octave = std::atoi((char *)point_node->children->content);
+ } else if (name == "class_id") {
+ class_id = std::atoi((char *)point_node->children->content);
+ cv::KeyPoint keyPoint(cv::Point2f((float)u, (float)v), (float)size, (float)angle, (float)response, octave,
+ (class_id + startClassId));
m_trainKeyPoints.push_back(keyPoint);
- } else if(name == "image_id") {
- image_id = std::atoi((char *) point_node->children->content);
- if(image_id != -1) {
+ } 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
+ // No training images if image_id == -1
m_mapOfImageId[m_trainKeyPoints.back().class_id] = image_id + startImageId;
#endif
}
} else if (name == "oX") {
- oX = std::atof((char *) point_node->children->content);
+ oX = std::atof((char *)point_node->children->content);
} else if (name == "oY") {
- oY = std::atof((char *) point_node->children->content);
+ 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));
+ oZ = std::atof((char *)point_node->children->content);
+ m_trainPoints.push_back(cv::Point3f((float)oX, (float)oY, (float)oZ));
} else if (name == "desc") {
xmlNodePtr descriptor_value_node = NULL;
j = 0;
- for (descriptor_value_node = point_node->children;
- descriptor_value_node; descriptor_value_node =
- descriptor_value_node->next) {
+ 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);
+ // Read descriptors values
+ std::string parseStr((char *)descriptor_value_node->children->content);
ss.clear();
ss.str(parseStr);
- if(!ss.fail()) {
- switch(descriptorType) {
- case CV_8U:
- {
- //Parse the numeric value [0 ; 255] to an int
- int parseValue;
- ss >> parseValue;
- trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char) parseValue;
- }
+ if (!ss.fail()) {
+ switch (descriptorType) {
+ case CV_8U: {
+ // Parse the numeric value [0 ; 255] to an int
+ int parseValue;
+ ss >> parseValue;
+ trainDescriptorsTmp.at<unsigned char>(i, j) = (unsigned char)parseValue;
+ } break;
+
+ case CV_8S:
+ // Parse the numeric value [-128 ; 127] to an int
+ int parseValue;
+ ss >> parseValue;
+ trainDescriptorsTmp.at<char>(i, j) = (char)parseValue;
break;
- case CV_8S:
- //Parse the numeric value [-128 ; 127] to an int
- int parseValue;
- ss >> parseValue;
- trainDescriptorsTmp.at<char>(i, j) = (char) parseValue;
+ case CV_16U:
+ ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
break;
- case CV_16U:
- ss >> trainDescriptorsTmp.at<unsigned short int>(i, j);
+ case CV_16S:
+ ss >> trainDescriptorsTmp.at<short int>(i, j);
break;
- case CV_16S:
- ss >> trainDescriptorsTmp.at<short int>(i, j);
+ case CV_32S:
+ ss >> trainDescriptorsTmp.at<int>(i, j);
break;
- case CV_32S:
- ss >> trainDescriptorsTmp.at<int>(i, j);
+ case CV_32F:
+ ss >> trainDescriptorsTmp.at<float>(i, j);
break;
- case CV_32F:
- ss >> trainDescriptorsTmp.at<float>(i, j);
+ case CV_64F:
+ ss >> trainDescriptorsTmp.at<double>(i, j);
break;
- case CV_64F:
- ss >> trainDescriptorsTmp.at<double>(i, j);
- break;
-
- default:
- ss >> trainDescriptorsTmp.at<float>(i, j);
+ default:
+ ss >> trainDescriptorsTmp.at<float>(i, j);
break;
}
} else {
@@ -2958,7 +3013,7 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
}
}
- if(!append || m_trainDescriptors.empty()) {
+ if (!append || m_trainDescriptors.empty()) {
trainDescriptorsTmp.copyTo(m_trainDescriptors);
} else {
cv::vconcat(m_trainDescriptors, trainDescriptorsTmp, m_trainDescriptors);
@@ -2977,16 +3032,19 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
#endif
}
- //Convert OpenCV type to ViSP type for compatibility
+ // Convert OpenCV type to ViSP type for compatibility
vpConvert::convertFromOpenCV(m_trainKeyPoints, referenceImagePointsList);
vpConvert::convertFromOpenCV(this->m_trainPoints, m_trainVpPoints);
- //Add train descriptors in matcher object
+ // Add train descriptors in matcher object
m_matcher->clear();
m_matcher->add(std::vector<cv::Mat>(1, m_trainDescriptors));
- //Set _reference_computed to true as we load learning file
+ // Set _reference_computed to true as we load a learning file
_reference_computed = true;
+
+ // Set m_currentImageId
+ m_currentImageId = (int)m_mapOfImages.size();
}
/*!
@@ -2998,22 +3056,24 @@ void vpKeyPoint::loadLearningData(const std::string &filename, const bool binary
\param elapsedTime : Elapsed time.
*/
void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDescriptors,
- std::vector<cv::DMatch> &matches, double &elapsedTime) {
+ std::vector<cv::DMatch> &matches, double &elapsedTime)
+{
double t = vpTime::measureTimeMs();
- if(m_useKnn) {
+ if (m_useKnn) {
m_knnMatches.clear();
- if(m_useMatchTrainToQuery) {
+ if (m_useMatchTrainToQuery) {
std::vector<std::vector<cv::DMatch> > knnMatchesTmp;
- //Match train descriptors to query descriptors
+ // Match train descriptors to query descriptors
cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
matcherTmp->knnMatch(trainDescriptors, queryDescriptors, knnMatchesTmp, 2);
- for(std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin(); it1 != knnMatchesTmp.end(); ++it1) {
+ for (std::vector<std::vector<cv::DMatch> >::const_iterator it1 = knnMatchesTmp.begin();
+ it1 != knnMatchesTmp.end(); ++it1) {
std::vector<cv::DMatch> tmp;
- for(std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
+ for (std::vector<cv::DMatch>::const_iterator it2 = it1->begin(); it2 != it1->end(); ++it2) {
tmp.push_back(cv::DMatch(it2->trainIdx, it2->queryIdx, it2->distance));
}
m_knnMatches.push_back(tmp);
@@ -3022,7 +3082,7 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDesc
matches.resize(m_knnMatches.size());
std::transform(m_knnMatches.begin(), m_knnMatches.end(), matches.begin(), knnToDMatch);
} else {
- //Match query descriptors to train descriptors
+ // 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);
@@ -3030,17 +3090,17 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDesc
} else {
matches.clear();
- if(m_useMatchTrainToQuery) {
+ if (m_useMatchTrainToQuery) {
std::vector<cv::DMatch> matchesTmp;
- //Match train descriptors to query descriptors
+ // Match train descriptors to query descriptors
cv::Ptr<cv::DescriptorMatcher> matcherTmp = m_matcher->clone(true);
matcherTmp->match(trainDescriptors, queryDescriptors, matchesTmp);
- for(std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
+ for (std::vector<cv::DMatch>::const_iterator it = matchesTmp.begin(); it != matchesTmp.end(); ++it) {
matches.push_back(cv::DMatch(it->trainIdx, it->queryIdx, it->distance));
}
} else {
- //Match query descriptors to train descriptors
+ // Match query descriptors to train descriptors
m_matcher->match(queryDescriptors, matches);
}
}
@@ -3048,14 +3108,13 @@ void vpKeyPoint::match(const cv::Mat &trainDescriptors, const cv::Mat &queryDesc
}
/*!
- Match keypoints detected in the image with those built in the reference list.
+ Match keypoints detected in the image with those built in the reference
+ list.
\param I : Input current image.
\return The number of matched keypoints.
*/
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) {
- return matchPoint(I, vpRect());
-}
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) { return matchPoint(I, vpRect()); }
/*!
Match keypoints detected in a region of interest of the image with those
@@ -3067,9 +3126,9 @@ unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I) {
\param width : Width of the region of interest
\return The number of matched keypoints
*/
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width) {
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP, const unsigned int height,
+ const unsigned int width)
+{
return matchPoint(I, vpRect(iP, width, height));
}
@@ -3081,11 +3140,11 @@ unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
\param rectangle : Rectangle of the region of interest
\return The number of matched keypoints
*/
-unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle) {
- if(m_trainDescriptors.empty()) {
+unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
+{
+ if (m_trainDescriptors.empty()) {
std::cerr << "Reference is empty." << std::endl;
- if(!_reference_computed) {
+ if (!_reference_computed) {
std::cerr << "Reference is not computed." << std::endl;
}
std::cerr << "Matching is not possible." << std::endl;
@@ -3093,23 +3152,24 @@ unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
return 0;
}
- if(m_useAffineDetection) {
+ if (m_useAffineDetection) {
std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
std::vector<cv::Mat> listOfQueryDescriptors;
- //Detect keypoints and extract descriptors on multiple images
+ // Detect keypoints and extract descriptors on multiple images
detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
- //Flatten the different train lists
+ // Flatten the different train lists
m_queryKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
- it != listOfQueryKeyPoints.end(); ++it) {
+ for (std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
+ it != listOfQueryKeyPoints.end(); ++it) {
m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
}
bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
- if(first) {
+ for (std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end();
+ ++it) {
+ if (first) {
first = false;
it->copyTo(m_queryDescriptors);
} else {
@@ -3123,38 +3183,39 @@ unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
match(m_trainDescriptors, m_queryDescriptors, m_matches, m_matchingTime);
- if(m_filterType != noFilterMatching) {
+ if (m_filterType != noFilterMatching) {
m_queryFilteredKeyPoints.clear();
m_objectFilteredPoints.clear();
m_filteredMatches.clear();
filterMatches();
} else {
- if(m_useMatchTrainToQuery) {
- //Add only query keypoints matched with a train keypoints
+ if (m_useMatchTrainToQuery) {
+ // Add only query keypoints matched with a train keypoints
m_queryFilteredKeyPoints.clear();
m_filteredMatches.clear();
- for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
- m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
- m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
+ for (std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ m_filteredMatches.push_back(cv::DMatch((int)m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
+ m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t)it->queryIdx]);
}
} else {
m_queryFilteredKeyPoints = m_queryKeyPoints;
m_filteredMatches = m_matches;
}
- if(!m_trainPoints.empty()) {
+ if (!m_trainPoints.empty()) {
m_objectFilteredPoints.clear();
- //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
+ // Add 3D object points such as the same index in
+ // m_queryFilteredKeyPoints and in m_objectFilteredPoints
// matches to the same train object
- for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
- //m_matches is normally ordered following the queryDescriptor index
- m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ for (std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ // m_matches is normally ordered following the queryDescriptor index
+ m_objectFilteredPoints.push_back(m_trainPoints[(size_t)it->trainIdx]);
}
}
}
- //Convert OpenCV type to ViSP type for compatibility
+ // Convert OpenCV type to ViSP type for compatibility
vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
@@ -3162,42 +3223,49 @@ unsigned int vpKeyPoint::matchPoint(const vpImage<unsigned char> &I,
}
/*!
- Match keypoints detected in the image with those built in the reference list and compute the pose.
+ Match keypoints detected in the image with those built in the reference
+ list and compute the pose.
\param I : Input image
\param cam : Camera parameters
- \param cMo : Homogeneous matrix between the object frame and the camera frame
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
- \return True if the matching and the pose estimation are OK, false otherwise
+ \param cMo : Homogeneous matrix between the object frame and the camera
+ frame \param func : Function pointer to filter the pose in Ransac pose
+ estimation, if we want to eliminate the poses which do not respect some
+ criterion \param rectangle : Rectangle corresponding to the ROI (Region of
+ Interest) to consider \return True if the matching and the pose estimation
+ are OK, false otherwise
*/
bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
+ bool (*func)(vpHomogeneousMatrix *), const vpRect &rectangle)
+{
double error, elapsedTime;
return matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
}
/*!
- Match keypoints detected in the image with those built in the reference list and compute the pose.
+ Match keypoints detected in the image with those built in the reference
+ list and compute the pose.
\param I : Input image
\param cam : Camera parameters
- \param cMo : Homogeneous matrix between the object frame and the camera frame
- \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
- the estimated pose
+ \param cMo : Homogeneous matrix between the object frame and the camera
+ frame \param error : Reprojection mean square error (in pixel) between the
+ 2D points and the projection of the 3D points with the estimated pose
\param elapsedTime : Time to detect, extract, match and compute the pose
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
- \return True if the matching and the pose estimation are OK, false otherwise
+ \param func : Function pointer to filter the pose in Ransac pose
+ estimation, if we want to eliminate the poses which do not respect some
+ criterion \param rectangle : Rectangle corresponding to the ROI (Region of
+ Interest) to consider \return True if the matching and the pose estimation
+ are OK, false otherwise
*/
bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
- //Check if we have training descriptors
- if(m_trainDescriptors.empty()) {
+ double &error, double &elapsedTime, bool (*func)(vpHomogeneousMatrix *),
+ const vpRect &rectangle)
+{
+ // Check if we have training descriptors
+ if (m_trainDescriptors.empty()) {
std::cerr << "Reference is empty." << std::endl;
- if(!_reference_computed) {
+ if (!_reference_computed) {
std::cerr << "Reference is not computed." << std::endl;
}
std::cerr << "Matching is not possible." << std::endl;
@@ -3205,23 +3273,24 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
return false;
}
- if(m_useAffineDetection) {
+ if (m_useAffineDetection) {
std::vector<std::vector<cv::KeyPoint> > listOfQueryKeyPoints;
std::vector<cv::Mat> listOfQueryDescriptors;
- //Detect keypoints and extract descriptors on multiple images
+ // Detect keypoints and extract descriptors on multiple images
detectExtractAffine(I, listOfQueryKeyPoints, listOfQueryDescriptors);
- //Flatten the different train lists
+ // Flatten the different train lists
m_queryKeyPoints.clear();
- for(std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
- it != listOfQueryKeyPoints.end(); ++it) {
+ for (std::vector<std::vector<cv::KeyPoint> >::const_iterator it = listOfQueryKeyPoints.begin();
+ it != listOfQueryKeyPoints.end(); ++it) {
m_queryKeyPoints.insert(m_queryKeyPoints.end(), it->begin(), it->end());
}
bool first = true;
- for(std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end(); ++it) {
- if(first) {
+ for (std::vector<cv::Mat>::const_iterator it = listOfQueryDescriptors.begin(); it != listOfQueryDescriptors.end();
+ ++it) {
+ if (first) {
first = false;
it->copyTo(m_queryDescriptors);
} else {
@@ -3237,52 +3306,55 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
elapsedTime = m_detectionTime + m_extractionTime + m_matchingTime;
- if(m_filterType != noFilterMatching) {
+ if (m_filterType != noFilterMatching) {
m_queryFilteredKeyPoints.clear();
m_objectFilteredPoints.clear();
m_filteredMatches.clear();
filterMatches();
} else {
- if(m_useMatchTrainToQuery) {
- //Add only query keypoints matched with a train keypoints
+ if (m_useMatchTrainToQuery) {
+ // Add only query keypoints matched with a train keypoints
m_queryFilteredKeyPoints.clear();
m_filteredMatches.clear();
- for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
- m_filteredMatches.push_back(cv::DMatch((int) m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
- m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t) it->queryIdx]);
+ for (std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ m_filteredMatches.push_back(cv::DMatch((int)m_queryFilteredKeyPoints.size(), it->trainIdx, it->distance));
+ m_queryFilteredKeyPoints.push_back(m_queryKeyPoints[(size_t)it->queryIdx]);
}
} else {
m_queryFilteredKeyPoints = m_queryKeyPoints;
m_filteredMatches = m_matches;
}
- if(!m_trainPoints.empty()) {
+ if (!m_trainPoints.empty()) {
m_objectFilteredPoints.clear();
- //Add 3D object points such as the same index in m_queryFilteredKeyPoints and in m_objectFilteredPoints
+ // Add 3D object points such as the same index in
+ // m_queryFilteredKeyPoints and in m_objectFilteredPoints
// matches to the same train object
- for(std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
- //m_matches is normally ordered following the queryDescriptor index
- m_objectFilteredPoints.push_back(m_trainPoints[(size_t) it->trainIdx]);
+ for (std::vector<cv::DMatch>::const_iterator it = m_matches.begin(); it != m_matches.end(); ++it) {
+ // m_matches is normally ordered following the queryDescriptor index
+ m_objectFilteredPoints.push_back(m_trainPoints[(size_t)it->trainIdx]);
}
}
}
- //Convert OpenCV type to ViSP type for compatibility
+ // Convert OpenCV type to ViSP type for compatibility
vpConvert::convertFromOpenCV(m_queryFilteredKeyPoints, currentImagePointsList);
vpConvert::convertFromOpenCV(m_filteredMatches, matchedReferencePoints);
- //error = std::numeric_limits<double>::max(); // create an error under Windows. To fix it we have to add #undef max
+ // error = std::numeric_limits<double>::max(); // create an error under
+ // Windows. To fix it we have to add #undef max
error = DBL_MAX;
m_ransacInliers.clear();
m_ransacOutliers.clear();
- if(m_useRansacVVS) {
+ if (m_useRansacVVS) {
std::vector<vpPoint> objectVpPoints(m_objectFilteredPoints.size());
size_t cpt = 0;
- //Create a list of vpPoint with 2D coordinates (current keypoint location) + 3D coordinates (world/object coordinates)
- for(std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin(); it != m_objectFilteredPoints.end();
- ++it, cpt++) {
+ // Create a list of vpPoint with 2D coordinates (current keypoint
+ // location) + 3D coordinates (world/object coordinates)
+ for (std::vector<cv::Point3f>::const_iterator it = m_objectFilteredPoints.begin();
+ it != m_objectFilteredPoints.end(); ++it, cpt++) {
vpPoint pt;
pt.setWorldCoordinates(it->x, it->y, it->z);
@@ -3305,13 +3377,13 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
m_matchRansacKeyPointsToPoints.clear();
for (std::vector<unsigned int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
- m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
- m_objectFilteredPoints[(size_t)(*it)]));
+ m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(
+ m_queryFilteredKeyPoints[(size_t)(*it)], m_objectFilteredPoints[(size_t)(*it)]));
mapOfInlierIndex[*it] = true;
}
- for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
- if(mapOfInlierIndex.find((unsigned int) i) == mapOfInlierIndex.end()) {
+ 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));
}
}
@@ -3319,8 +3391,8 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
- std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
- matchRansacToVpImage);
+ std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(),
+ m_ransacInliers.begin(), matchRansacToVpImage);
elapsedTime += m_poseTime;
@@ -3335,13 +3407,13 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
m_matchRansacKeyPointsToPoints.clear();
for (std::vector<int>::const_iterator it = inlierIndex.begin(); it != inlierIndex.end(); ++it) {
- m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(m_queryFilteredKeyPoints[(size_t)(*it)],
- m_objectFilteredPoints[(size_t)(*it)]));
+ m_matchRansacKeyPointsToPoints.push_back(std::pair<cv::KeyPoint, cv::Point3f>(
+ m_queryFilteredKeyPoints[(size_t)(*it)], m_objectFilteredPoints[(size_t)(*it)]));
mapOfInlierIndex[*it] = true;
}
- for(size_t i = 0; i < m_queryFilteredKeyPoints.size(); i++) {
- if(mapOfInlierIndex.find((int) i) == mapOfInlierIndex.end()) {
+ 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));
}
}
@@ -3349,8 +3421,8 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
error = computePoseEstimationError(m_matchRansacKeyPointsToPoints, cam, cMo);
m_ransacInliers.resize(m_matchRansacKeyPointsToPoints.size());
- std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(), m_ransacInliers.begin(),
- matchRansacToVpImage);
+ std::transform(m_matchRansacKeyPointsToPoints.begin(), m_matchRansacKeyPointsToPoints.end(),
+ m_ransacInliers.begin(), matchRansacToVpImage);
elapsedTime += m_poseTime;
@@ -3359,26 +3431,29 @@ bool vpKeyPoint::matchPoint(const vpImage<unsigned char> &I, const vpCameraParam
}
/*!
- Match keypoints detected in the image with those built in the reference list and return the bounding box and the center
- of gravity.
+ Match keypoints detected in the image with those built in the reference
+ list and return the bounding box and the center of gravity.
\param I : Input image
\param boundingBox : Bounding box that contains the good matches
- \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
- the bounding box)
- \param isPlanarObject : If the object is planar, the homography matrix is estimated to eliminate outliers, otherwise
- it is the fundamental matrix which is estimated
- \param imPts1 : Pointer to the list of reference keypoints if not null
- \param imPts2 : Pointer to the list of current keypoints if not null
- \param meanDescriptorDistance : Pointer to the value of the average distance of the descriptors if not null
- \param detection_score : Pointer to the value of the detection score if not null
- \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
- \return True if the object is present, false otherwise
+ \param centerOfGravity : Center of gravity computed from the location of
+ the good matches (could differ of the center of the bounding box) \param
+ isPlanarObject : If the object is planar, the homography matrix is
+ estimated to eliminate outliers, otherwise it is the fundamental matrix
+ which is estimated \param imPts1 : Pointer to the list of reference
+ keypoints if not null \param imPts2 : Pointer to the list of current
+ keypoints if not null \param meanDescriptorDistance : Pointer to the value
+ of the average distance of the descriptors if not null \param
+ detection_score : Pointer to the value of the detection score if not null
+ \param rectangle : Rectangle corresponding to the ROI (Region of Interest)
+ to consider \return True if the object is present, false otherwise
*/
-bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- const bool isPlanarObject, std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
- double *meanDescriptorDistance, double *detection_score, const vpRect& rectangle) {
- if(imPts1 != NULL && imPts2 != NULL) {
+bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &boundingBox,
+ vpImagePoint ¢erOfGravity, const bool isPlanarObject,
+ std::vector<vpImagePoint> *imPts1, std::vector<vpImagePoint> *imPts2,
+ double *meanDescriptorDistance, double *detection_score, const vpRect &rectangle)
+{
+ if (imPts1 != NULL && imPts2 != NULL) {
imPts1->clear();
imPts2->clear();
}
@@ -3386,105 +3461,106 @@ bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &bo
matchPoint(I, rectangle);
double meanDescriptorDistanceTmp = 0.0;
- for(std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
- meanDescriptorDistanceTmp += (double) it->distance;
+ for (std::vector<cv::DMatch>::const_iterator it = m_filteredMatches.begin(); it != m_filteredMatches.end(); ++it) {
+ meanDescriptorDistanceTmp += (double)it->distance;
}
- meanDescriptorDistanceTmp /= (double) m_filteredMatches.size();
- double score = (double) m_filteredMatches.size() / meanDescriptorDistanceTmp;
+ meanDescriptorDistanceTmp /= (double)m_filteredMatches.size();
+ double score = (double)m_filteredMatches.size() / meanDescriptorDistanceTmp;
- if(meanDescriptorDistance != NULL) {
+ if (meanDescriptorDistance != NULL) {
*meanDescriptorDistance = meanDescriptorDistanceTmp;
}
- if(detection_score != NULL) {
+ if (detection_score != NULL) {
*detection_score = score;
}
- if(m_filteredMatches.size() >= 4) {
- //Training / Reference 2D points
+ if (m_filteredMatches.size() >= 4) {
+ // Training / Reference 2D points
std::vector<cv::Point2f> points1(m_filteredMatches.size());
- //Query / Current 2D points
+ // Query / Current 2D points
std::vector<cv::Point2f> points2(m_filteredMatches.size());
- for(size_t i = 0; i < m_filteredMatches.size(); i++) {
+ for (size_t i = 0; i < m_filteredMatches.size(); i++) {
points1[i] = cv::Point2f(m_trainKeyPoints[(size_t)m_filteredMatches[i].trainIdx].pt);
points2[i] = cv::Point2f(m_queryFilteredKeyPoints[(size_t)m_filteredMatches[i].queryIdx].pt);
}
std::vector<vpImagePoint> inliers;
- if(isPlanarObject) {
+ if (isPlanarObject) {
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
cv::Mat homographyMatrix = cv::findHomography(points1, points2, CV_RANSAC);
#else
cv::Mat homographyMatrix = cv::findHomography(points1, points2, cv::RANSAC);
#endif
- for(size_t i = 0; i < m_filteredMatches.size(); i++ ) {
- //Compute reprojection error
+ for (size_t i = 0; i < m_filteredMatches.size(); i++) {
+ // Compute reprojection error
cv::Mat realPoint = cv::Mat(3, 1, CV_64F);
- realPoint.at<double>(0,0) = points1[i].x;
- realPoint.at<double>(1,0) = points1[i].y;
- realPoint.at<double>(2,0) = 1.f;
+ realPoint.at<double>(0, 0) = points1[i].x;
+ realPoint.at<double>(1, 0) = points1[i].y;
+ realPoint.at<double>(2, 0) = 1.f;
cv::Mat reprojectedPoint = homographyMatrix * realPoint;
- double err_x = (reprojectedPoint.at<double>(0,0) / reprojectedPoint.at<double>(2,0)) - points2[i].x;
- double err_y = (reprojectedPoint.at<double>(1,0) / reprojectedPoint.at<double>(2,0)) - points2[i].y;
- double reprojectionError = std::sqrt(err_x*err_x + err_y*err_y);
-
- if(reprojectionError < 6.0) {
- inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
- if(imPts1 != NULL) {
- imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
+ double err_x = (reprojectedPoint.at<double>(0, 0) / reprojectedPoint.at<double>(2, 0)) - points2[i].x;
+ double err_y = (reprojectedPoint.at<double>(1, 0) / reprojectedPoint.at<double>(2, 0)) - points2[i].y;
+ double reprojectionError = std::sqrt(err_x * err_x + err_y * err_y);
+
+ if (reprojectionError < 6.0) {
+ inliers.push_back(vpImagePoint((double)points2[i].y, (double)points2[i].x));
+ if (imPts1 != NULL) {
+ imPts1->push_back(vpImagePoint((double)points1[i].y, (double)points1[i].x));
}
- if(imPts2 != NULL) {
- imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ if (imPts2 != NULL) {
+ imPts2->push_back(vpImagePoint((double)points2[i].y, (double)points2[i].x));
}
}
}
- } else if(m_filteredMatches.size() >= 8) {
+ } else if (m_filteredMatches.size() >= 8) {
cv::Mat fundamentalInliers;
cv::Mat fundamentalMatrix = cv::findFundamentalMat(points1, points2, cv::FM_RANSAC, 3, 0.99, fundamentalInliers);
- for(size_t i = 0; i < (size_t) fundamentalInliers.rows; i++) {
- if(fundamentalInliers.at<uchar>((int) i, 0)) {
- inliers.push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ for (size_t i = 0; i < (size_t)fundamentalInliers.rows; i++) {
+ if (fundamentalInliers.at<uchar>((int)i, 0)) {
+ inliers.push_back(vpImagePoint((double)points2[i].y, (double)points2[i].x));
- if(imPts1 != NULL) {
- imPts1->push_back(vpImagePoint((double) points1[i].y, (double) points1[i].x));
+ if (imPts1 != NULL) {
+ imPts1->push_back(vpImagePoint((double)points1[i].y, (double)points1[i].x));
}
- if(imPts2 != NULL) {
- imPts2->push_back(vpImagePoint((double) points2[i].y, (double) points2[i].x));
+ if (imPts2 != NULL) {
+ imPts2->push_back(vpImagePoint((double)points2[i].y, (double)points2[i].x));
}
}
}
}
- if(!inliers.empty()) {
- //Build a polygon with the list of inlier keypoints detected in the current image to get the bounding box
+ if (!inliers.empty()) {
+ // Build a polygon with the list of inlier keypoints detected in the
+ // current image to get the bounding box
vpPolygon polygon(inliers);
boundingBox = polygon.getBoundingBox();
- //Compute the center of gravity
+ // Compute the center of gravity
double meanU = 0.0, meanV = 0.0;
- for(std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = inliers.begin(); it != inliers.end(); ++it) {
meanU += it->get_u();
meanV += it->get_v();
}
- meanU /= (double) inliers.size();
- meanV /= (double) inliers.size();
+ meanU /= (double)inliers.size();
+ meanV /= (double)inliers.size();
centerOfGravity.set_u(meanU);
centerOfGravity.set_v(meanV);
}
} else {
- //Too few matches
+ // Too few matches
return false;
}
- if(m_detectionMethod == detectionThreshold) {
+ if (m_detectionMethod == detectionThreshold) {
return meanDescriptorDistanceTmp < m_detectionThreshold;
} else {
return score > m_detectionScore;
@@ -3492,54 +3568,58 @@ bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, vpRect &bo
}
/*!
- Match keypoints detected in the image with those built in the reference list, compute the pose and return also
- the bounding box and the center of gravity.
+ Match keypoints detected in the image with those built in the reference
+ list, compute the pose and return also the bounding box and the center of
+ gravity.
\param I : Input image
\param cam : Camera parameters
- \param cMo : Homogeneous matrix between the object frame and the camera frame
- \param error : Reprojection mean square error (in pixel) between the 2D points and the projection of the 3D points with
- the estimated pose
+ \param cMo : Homogeneous matrix between the object frame and the camera
+ frame \param error : Reprojection mean square error (in pixel) between the
+ 2D points and the projection of the 3D points with the estimated pose
\param elapsedTime : Time to detect, extract, match and compute the pose
\param boundingBox : Bounding box that contains the good matches
- \param centerOfGravity : Center of gravity computed from the location of the good matches (could differ of the center of
- the bounding box)
- \param func : Function pointer to filter the pose in Ransac pose estimation, if we want to eliminate
- the poses which do not respect some criterion
- \param rectangle : Rectangle corresponding to the ROI (Region of Interest) to consider
- \return True if the matching and the pose estimation are OK, false otherwise.
+ \param centerOfGravity : Center of gravity computed from the location of
+ the good matches (could differ of the center of the bounding box) \param
+ func : Function pointer to filter the pose in Ransac pose estimation, if we
+ want to eliminate the poses which do not respect some criterion \param
+ rectangle : Rectangle corresponding to the ROI (Region of Interest) to
+ consider \return True if the matching and the pose estimation are OK, false
+ otherwise.
*/
-bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam, vpHomogeneousMatrix &cMo,
- double &error, double &elapsedTime, vpRect &boundingBox, vpImagePoint ¢erOfGravity,
- bool (*func)(vpHomogeneousMatrix *), const vpRect& rectangle) {
+bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCameraParameters &cam,
+ vpHomogeneousMatrix &cMo, double &error, double &elapsedTime, vpRect &boundingBox,
+ vpImagePoint ¢erOfGravity, bool (*func)(vpHomogeneousMatrix *),
+ const vpRect &rectangle)
+{
bool isMatchOk = matchPoint(I, cam, cMo, error, elapsedTime, func, rectangle);
- if(isMatchOk) {
- //Use the pose estimated to project the model points in the image
+ if (isMatchOk) {
+ // Use the pose estimated to project the model points in the image
vpPoint pt;
vpImagePoint imPt;
std::vector<vpImagePoint> modelImagePoints(m_trainVpPoints.size());
size_t cpt = 0;
- for(std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
+ for (std::vector<vpPoint>::const_iterator it = m_trainVpPoints.begin(); it != m_trainVpPoints.end(); ++it, cpt++) {
pt = *it;
pt.project(cMo);
vpMeterPixelConversion::convertPoint(cam, pt.get_x(), pt.get_y(), imPt);
modelImagePoints[cpt] = imPt;
}
- //Build a polygon with the list of model image points to get the bounding box
+ // Build a polygon with the list of model image points to get the bounding
+ // box
vpPolygon polygon(modelImagePoints);
boundingBox = polygon.getBoundingBox();
- //Compute the center of gravity of the current inlier keypoints
+ // Compute the center of gravity of the current inlier keypoints
double meanU = 0.0, meanV = 0.0;
- for(std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end();
- ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = m_ransacInliers.begin(); it != m_ransacInliers.end(); ++it) {
meanU += it->get_u();
meanV += it->get_v();
}
- meanU /= (double) m_ransacInliers.size();
- meanV /= (double) m_ransacInliers.size();
+ meanU /= (double)m_ransacInliers.size();
+ meanV /= (double)m_ransacInliers.size();
centerOfGravity.set_u(meanU);
centerOfGravity.set_v(meanV);
@@ -3552,15 +3632,19 @@ bool vpKeyPoint::matchPointAndDetect(const vpImage<unsigned char> &I, const vpCa
Apply a set of affine transormations to the image, detect keypoints and
reproject them into initial image coordinates.
See http://www.ipol.im/pub/algo/my_affine_sift/ for the details.
- See https://github.com/Itseez/opencv/blob/master/samples/python2/asift.py for the Python implementation by Itseez
- and Matt Sheckells for the current implementation in C++.
- \param I : Input image
- \param listOfKeypoints : List of detected keypoints in the multiple images after affine transformations
+ See https://github.com/Itseez/opencv/blob/master/samples/python2/asift.py
+ for the Python implementation by Itseez and Matt Sheckells for the current
+ implementation in C++. \param I : Input image \param listOfKeypoints : List
+ of detected keypoints in the multiple images after affine transformations
\param listOfDescriptors : Corresponding list of descriptors
- \param listOfAffineI : Optional parameter, list of images after affine transformations
+ \param listOfAffineI : Optional parameter, list of images after affine
+ transformations
*/
-void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector<std::vector<cv::KeyPoint> >& listOfKeypoints,
- std::vector<cv::Mat>& listOfDescriptors, std::vector<vpImage<unsigned char> > *listOfAffineI) {
+void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,
+ std::vector<std::vector<cv::KeyPoint> > &listOfKeypoints,
+ std::vector<cv::Mat> &listOfDescriptors,
+ std::vector<vpImage<unsigned char> > *listOfAffineI)
+{
#if 0
cv::Mat img;
vpImageConvert::convert(I, img);
@@ -3620,7 +3704,7 @@ void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector
cv::Mat img;
vpImageConvert::convert(I, img);
- //Create a vector for storing the affine skew parameters
+ // Create a vector for storing the affine skew parameters
std::vector<std::pair<double, int> > listOfAffineParams;
for (int tl = 1; tl < 6; tl++) {
double t = pow(2, 0.5 * tl);
@@ -3632,29 +3716,28 @@ void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector
listOfKeypoints.resize(listOfAffineParams.size());
listOfDescriptors.resize(listOfAffineParams.size());
- if(listOfAffineI != NULL) {
+ if (listOfAffineI != NULL) {
listOfAffineI->resize(listOfAffineParams.size());
}
#ifdef VISP_HAVE_OPENMP
- #pragma omp parallel for
+#pragma omp parallel for
#endif
- for(int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
+ for (int cpt = 0; cpt < static_cast<int>(listOfAffineParams.size()); cpt++) {
std::vector<cv::KeyPoint> keypoints;
cv::Mat descriptors;
cv::Mat timg, mask, Ai;
img.copyTo(timg);
- affineSkew(listOfAffineParams[(size_t) cpt].first, listOfAffineParams[(size_t) cpt].second, timg, mask, Ai);
+ affineSkew(listOfAffineParams[(size_t)cpt].first, listOfAffineParams[(size_t)cpt].second, timg, mask, Ai);
-
- if(listOfAffineI != NULL) {
+ if (listOfAffineI != NULL) {
cv::Mat img_disp;
bitwise_and(mask, timg, img_disp);
vpImage<unsigned char> tI;
vpImageConvert::convert(img_disp, tI);
- (*listOfAffineI)[(size_t) cpt] = tI;
+ (*listOfAffineI)[(size_t)cpt] = tI;
}
#if 0
@@ -3665,8 +3748,8 @@ void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector
cv::waitKey(0);
#endif
- for(std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
- it != m_detectors.end(); ++it) {
+ for (std::map<std::string, cv::Ptr<cv::FeatureDetector> >::const_iterator it = m_detectors.begin();
+ it != m_detectors.end(); ++it) {
std::vector<cv::KeyPoint> kp;
it->second->detect(timg, kp, mask);
keypoints.insert(keypoints.end(), kp.begin(), kp.end());
@@ -3675,15 +3758,15 @@ void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector
double elapsedTime;
extract(timg, keypoints, descriptors, elapsedTime);
- for(size_t i = 0; i < keypoints.size(); i++) {
+ for (size_t i = 0; i < keypoints.size(); i++) {
cv::Point3f kpt(keypoints[i].pt.x, keypoints[i].pt.y, 1.f);
cv::Mat kpt_t = Ai * cv::Mat(kpt);
keypoints[i].pt.x = kpt_t.at<float>(0, 0);
keypoints[i].pt.y = kpt_t.at<float>(1, 0);
}
- listOfKeypoints[(size_t) cpt] = keypoints;
- listOfDescriptors[(size_t) cpt] = descriptors;
+ listOfKeypoints[(size_t)cpt] = keypoints;
+ listOfDescriptors[(size_t)cpt] = descriptors;
}
#endif
}
@@ -3691,30 +3774,64 @@ void vpKeyPoint::detectExtractAffine(const vpImage<unsigned char> &I,std::vector
/*!
Reset the instance as if we would declare another vpKeyPoint variable.
*/
-void vpKeyPoint::reset() {
- //vpBasicKeyPoint class
- referenceImagePointsList.clear(); currentImagePointsList.clear(); matchedReferencePoints.clear(); _reference_computed = false;
-
-
- m_computeCovariance = false; m_covarianceMatrix = vpMatrix(); m_currentImageId = 0; m_detectionMethod = detectionScore;
- m_detectionScore = 0.15; m_detectionThreshold = 100.0; m_detectionTime = 0.0; m_detectorNames.clear();
- m_detectors.clear(); m_extractionTime = 0.0; m_extractorNames.clear(); m_extractors.clear(); m_filteredMatches.clear();
+void vpKeyPoint::reset()
+{
+ // vpBasicKeyPoint class
+ referenceImagePointsList.clear();
+ currentImagePointsList.clear();
+ matchedReferencePoints.clear();
+ _reference_computed = false;
+
+ m_computeCovariance = false;
+ m_covarianceMatrix = vpMatrix();
+ m_currentImageId = 0;
+ m_detectionMethod = detectionScore;
+ m_detectionScore = 0.15;
+ m_detectionThreshold = 100.0;
+ m_detectionTime = 0.0;
+ m_detectorNames.clear();
+ m_detectors.clear();
+ m_extractionTime = 0.0;
+ m_extractorNames.clear();
+ m_extractors.clear();
+ m_filteredMatches.clear();
m_filterType = ratioDistanceThreshold;
- m_imageFormat = jpgImageFormat; m_knnMatches.clear(); m_mapOfImageId.clear(); m_mapOfImages.clear();
- m_matcher = cv::Ptr<cv::DescriptorMatcher>(); m_matcherName = "BruteForce-Hamming";
- m_matches.clear(); m_matchingFactorThreshold = 2.0; m_matchingRatioThreshold = 0.85; m_matchingTime = 0.0;
- m_matchRansacKeyPointsToPoints.clear(); m_nbRansacIterations = 200; m_nbRansacMinInlierCount = 100;
+ m_imageFormat = jpgImageFormat;
+ m_knnMatches.clear();
+ m_mapOfImageId.clear();
+ m_mapOfImages.clear();
+ m_matcher = cv::Ptr<cv::DescriptorMatcher>();
+ m_matcherName = "BruteForce-Hamming";
+ m_matches.clear();
+ m_matchingFactorThreshold = 2.0;
+ m_matchingRatioThreshold = 0.85;
+ m_matchingTime = 0.0;
+ m_matchRansacKeyPointsToPoints.clear();
+ m_nbRansacIterations = 200;
+ m_nbRansacMinInlierCount = 100;
m_objectFilteredPoints.clear();
- m_poseTime = 0.0; m_queryDescriptors = cv::Mat(); m_queryFilteredKeyPoints.clear(); m_queryKeyPoints.clear();
- m_ransacConsensusPercentage = 20.0; m_ransacInliers.clear(); m_ransacOutliers.clear(); m_ransacReprojectionError = 6.0;
- m_ransacThreshold = 0.01; m_trainDescriptors = cv::Mat(); m_trainKeyPoints.clear(); m_trainPoints.clear();
- m_trainVpPoints.clear(); m_useAffineDetection = false;
+ m_poseTime = 0.0;
+ m_queryDescriptors = cv::Mat();
+ m_queryFilteredKeyPoints.clear();
+ m_queryKeyPoints.clear();
+ m_ransacConsensusPercentage = 20.0;
+ m_ransacInliers.clear();
+ m_ransacOutliers.clear();
+ m_ransacReprojectionError = 6.0;
+ m_ransacThreshold = 0.01;
+ m_trainDescriptors = cv::Mat();
+ m_trainKeyPoints.clear();
+ m_trainPoints.clear();
+ m_trainVpPoints.clear();
+ m_useAffineDetection = false;
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400 && VISP_HAVE_OPENCV_VERSION < 0x030000)
m_useBruteForceCrossCheck = true;
#endif
m_useConsensusPercentage = false;
- m_useKnn = true; //as m_filterType == ratioDistanceThreshold
- m_useMatchTrainToQuery = false; m_useRansacVVS = true; m_useSingleMatchFilter = true;
+ m_useKnn = true; // as m_filterType == ratioDistanceThreshold
+ m_useMatchTrainToQuery = false;
+ m_useRansacVVS = true;
+ m_useSingleMatchFilter = true;
m_detectorNames.push_back("ORB");
m_extractorNames.push_back("ORB");
@@ -3726,32 +3843,33 @@ void vpKeyPoint::reset() {
Save the learning data in a file in XML or binary mode.
\param filename : Path of the save file
- \param binaryMode : If true, the data are saved in binary mode, otherwise in XML mode
- \param saveTrainingImages : If true, save also the training images on disk
+ \param binaryMode : If true, the data are saved in binary mode, otherwise
+ in XML mode \param saveTrainingImages : If true, save also the training
+ images on disk
*/
-void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages) {
+void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode, const bool saveTrainingImages)
+{
std::string parent = vpIoTools::getParent(filename);
- if(!parent.empty()) {
+ if (!parent.empty()) {
vpIoTools::makeDirectory(parent);
}
std::map<int, std::string> mapOfImgPath;
- if(saveTrainingImages) {
+ if (saveTrainingImages) {
#ifdef VISP_HAVE_MODULE_IO
- //Save the training image files in the same directory
- int cpt = 0;
+ // Save the training image files in the same directory
+ unsigned int cpt = 0;
- for(std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end(); ++it, cpt++) {
- if(cpt > 999) {
+ for (std::map<int, vpImage<unsigned char> >::const_iterator it = m_mapOfImages.begin(); it != m_mapOfImages.end();
+ ++it, cpt++) {
+ if (cpt > 999) {
throw vpException(vpException::fatalError, "The number of training images to save is too big !");
}
- char buffer[4];
- sprintf(buffer, "%03d", cpt);
std::stringstream ss;
- ss << "train_image_" << buffer;
+ ss << "train_image_" << std::setfill('0') << std::setw(3) << cpt;
- switch(m_imageFormat) {
+ switch (m_imageFormat) {
case jpgImageFormat:
ss << ".jpg";
break;
@@ -3778,138 +3896,139 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
vpImageIo::write(it->second, parent + (!parent.empty() ? "/" : "") + imgFilename);
}
#else
- std::cout << "Warning: in vpKeyPoint::saveLearningData() training images are not saved because "
- "visp_io module is not available !" << std::endl;
+ std::cout << "Warning: in vpKeyPoint::saveLearningData() training images "
+ "are not saved because "
+ "visp_io module is not available !"
+ << std::endl;
#endif
}
bool have3DInfo = m_trainPoints.size() > 0;
- if(have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
+ if (have3DInfo && m_trainPoints.size() != m_trainKeyPoints.size()) {
throw vpException(vpException::fatalError, "List of keypoints and list of 3D points have different size !");
}
- if(binaryMode) {
- //Save the learning data into little endian binary file.
+ if (binaryMode) {
+ // Save the learning data into little endian binary file.
std::ofstream file(filename.c_str(), std::ofstream::binary);
- if(!file.is_open()) {
+ if (!file.is_open()) {
throw vpException(vpException::ioError, "Cannot create the file.");
}
- //Write info about training images
- int nbImgs = (int) mapOfImgPath.size();
-// file.write((char *)(&nbImgs), sizeof(nbImgs));
+ // Write info about training images
+ int nbImgs = (int)mapOfImgPath.size();
+ // file.write((char *)(&nbImgs), sizeof(nbImgs));
writeBinaryIntLE(file, nbImgs);
#ifdef VISP_HAVE_MODULE_IO
- for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
- //Write image_id
+ for (std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
+ // Write image_id
int id = it->first;
-// file.write((char *)(&id), sizeof(id));
+ // file.write((char *)(&id), sizeof(id));
writeBinaryIntLE(file, id);
- //Write image path
+ // Write image path
std::string path = it->second;
- int length = (int) path.length();
-// file.write((char *)(&length), sizeof(length));
+ int length = (int)path.length();
+ // file.write((char *)(&length), sizeof(length));
writeBinaryIntLE(file, length);
- for(int cpt = 0; cpt < length; cpt++) {
- file.write((char *) (&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
+ for (int cpt = 0; cpt < length; cpt++) {
+ file.write((char *)(&path[(size_t)cpt]), sizeof(path[(size_t)cpt]));
}
}
#endif
- //Write if we have 3D point information
+ // Write if we have 3D point information
int have3DInfoInt = have3DInfo ? 1 : 0;
-// file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
+ // file.write((char *)(&have3DInfoInt), sizeof(have3DInfoInt));
writeBinaryIntLE(file, have3DInfoInt);
-
- int nRows = m_trainDescriptors.rows,
- nCols = m_trainDescriptors.cols;
+ int nRows = m_trainDescriptors.rows, nCols = m_trainDescriptors.cols;
int descriptorType = m_trainDescriptors.type();
- //Write the number of descriptors
-// file.write((char *)(&nRows), sizeof(nRows));
+ // Write the number of descriptors
+ // file.write((char *)(&nRows), sizeof(nRows));
writeBinaryIntLE(file, nRows);
- //Write the size of the descriptor
-// file.write((char *)(&nCols), sizeof(nCols));
+ // Write the size of the descriptor
+ // file.write((char *)(&nCols), sizeof(nCols));
writeBinaryIntLE(file, nCols);
- //Write the type of the descriptor
-// file.write((char *)(&descriptorType), sizeof(descriptorType));
+ // Write the type of the descriptor
+ // file.write((char *)(&descriptorType), sizeof(descriptorType));
writeBinaryIntLE(file, descriptorType);
for (int i = 0; i < nRows; i++) {
- unsigned int i_ = (unsigned int) i;
- //Write u
+ unsigned int i_ = (unsigned int)i;
+ // Write u
float u = m_trainKeyPoints[i_].pt.x;
-// file.write((char *)(&u), sizeof(u));
+ // file.write((char *)(&u), sizeof(u));
writeBinaryFloatLE(file, u);
- //Write v
+ // Write v
float v = m_trainKeyPoints[i_].pt.y;
-// file.write((char *)(&v), sizeof(v));
+ // file.write((char *)(&v), sizeof(v));
writeBinaryFloatLE(file, v);
- //Write size
+ // Write size
float size = m_trainKeyPoints[i_].size;
-// file.write((char *)(&size), sizeof(size));
+ // file.write((char *)(&size), sizeof(size));
writeBinaryFloatLE(file, size);
- //Write angle
+ // Write angle
float angle = m_trainKeyPoints[i_].angle;
-// file.write((char *)(&angle), sizeof(angle));
+ // file.write((char *)(&angle), sizeof(angle));
writeBinaryFloatLE(file, angle);
- //Write response
+ // Write response
float response = m_trainKeyPoints[i_].response;
-// file.write((char *)(&response), sizeof(response));
+ // file.write((char *)(&response), sizeof(response));
writeBinaryFloatLE(file, response);
- //Write octave
+ // Write octave
int octave = m_trainKeyPoints[i_].octave;
-// file.write((char *)(&octave), sizeof(octave));
+ // file.write((char *)(&octave), sizeof(octave));
writeBinaryIntLE(file, octave);
- //Write class_id
+ // Write class_id
int class_id = m_trainKeyPoints[i_].class_id;
-// file.write((char *)(&class_id), sizeof(class_id));
+ // file.write((char *)(&class_id), sizeof(class_id));
writeBinaryIntLE(file, class_id);
- //Write image_id
+// Write image_id
#ifdef VISP_HAVE_MODULE_IO
std::map<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
int image_id = (saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1;
-// file.write((char *)(&image_id), sizeof(image_id));
+ // file.write((char *)(&image_id), sizeof(image_id));
writeBinaryIntLE(file, image_id);
#else
int image_id = -1;
-// file.write((char *)(&image_id), sizeof(image_id));
+ // file.write((char *)(&image_id), sizeof(image_id));
writeBinaryIntLE(file, image_id);
#endif
- if(have3DInfo) {
+ if (have3DInfo) {
float oX = m_trainPoints[i_].x, oY = m_trainPoints[i_].y, oZ = m_trainPoints[i_].z;
- //Write oX
-// file.write((char *)(&oX), sizeof(oX));
+ // Write oX
+ // file.write((char *)(&oX), sizeof(oX));
writeBinaryFloatLE(file, oX);
- //Write oY
-// file.write((char *)(&oY), sizeof(oY));
+ // Write oY
+ // file.write((char *)(&oY), sizeof(oY));
writeBinaryFloatLE(file, oY);
- //Write oZ
-// file.write((char *)(&oZ), sizeof(oZ));
+ // Write oZ
+ // file.write((char *)(&oZ), sizeof(oZ));
writeBinaryFloatLE(file, oZ);
}
for (int j = 0; j < nCols; j++) {
- //Write the descriptor value
- switch(descriptorType) {
+ // Write the descriptor value
+ switch (descriptorType) {
case CV_8U:
- file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)), sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
+ file.write((char *)(&m_trainDescriptors.at<unsigned char>(i, j)),
+ sizeof(m_trainDescriptors.at<unsigned char>(i, j)));
break;
case CV_8S:
@@ -3917,27 +4036,33 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
break;
case CV_16U:
-// file.write((char *)(&m_trainDescriptors.at<unsigned short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned short int>(i, j)));
+ // file.write((char *)(&m_trainDescriptors.at<unsigned
+ // short int>(i, j)), sizeof(m_trainDescriptors.at<unsigned
+ // short int>(i, j)));
writeBinaryUShortLE(file, m_trainDescriptors.at<unsigned short int>(i, j));
break;
case CV_16S:
-// file.write((char *)(&m_trainDescriptors.at<short int>(i, j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
+ // file.write((char *)(&m_trainDescriptors.at<short int>(i,
+ // j)), sizeof(m_trainDescriptors.at<short int>(i, j)));
writeBinaryShortLE(file, m_trainDescriptors.at<short int>(i, j));
break;
case CV_32S:
-// file.write((char *)(&m_trainDescriptors.at<int>(i, j)), sizeof(m_trainDescriptors.at<int>(i, j)));
+ // file.write((char *)(&m_trainDescriptors.at<int>(i, j)),
+ // sizeof(m_trainDescriptors.at<int>(i, j)));
writeBinaryIntLE(file, m_trainDescriptors.at<int>(i, j));
break;
case CV_32F:
-// file.write((char *)(&m_trainDescriptors.at<float>(i, j)), sizeof(m_trainDescriptors.at<float>(i, j)));
+ // file.write((char *)(&m_trainDescriptors.at<float>(i,
+ // j)), sizeof(m_trainDescriptors.at<float>(i, j)));
writeBinaryFloatLE(file, m_trainDescriptors.at<float>(i, j));
break;
case CV_64F:
-// file.write((char *)(&m_trainDescriptors.at<double>(i, j)), sizeof(m_trainDescriptors.at<double>(i, j)));
+ // file.write((char *)(&m_trainDescriptors.at<double>(i,
+ // j)), sizeof(m_trainDescriptors.at<double>(i, j)));
writeBinaryDoubleLE(file, m_trainDescriptors.at<double>(i, j));
break;
@@ -3948,13 +4073,12 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
}
}
-
file.close();
} else {
#ifdef VISP_HAVE_XML2
xmlDocPtr doc = NULL;
xmlNodePtr root_node = NULL, image_node = NULL, image_info_node = NULL, descriptors_info_node = NULL,
- descriptor_node = NULL, desc_node = NULL;
+ descriptor_node = NULL, desc_node = NULL;
/*
* this initialize the library and check potential ABI mismatches
@@ -3963,7 +4087,7 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
*/
LIBXML_TEST_VERSION
- doc = xmlNewDoc(BAD_CAST "1.0");
+ doc = xmlNewDoc(BAD_CAST "1.0");
if (doc == NULL) {
throw vpException(vpException::ioError, "Error with file " + filename);
}
@@ -3973,116 +4097,101 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
std::stringstream ss;
- //Write the training images info
+ // Write the training images info
image_node = xmlNewChild(root_node, NULL, BAD_CAST "TrainingImageInfo", NULL);
#ifdef VISP_HAVE_MODULE_IO
- for(std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
- image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg",
- BAD_CAST it->second.c_str());
+ for (std::map<int, std::string>::const_iterator it = mapOfImgPath.begin(); it != mapOfImgPath.end(); ++it) {
+ image_info_node = xmlNewChild(image_node, NULL, BAD_CAST "trainImg", BAD_CAST it->second.c_str());
ss.str("");
ss << it->first;
xmlNewProp(image_info_node, BAD_CAST "image_id", BAD_CAST ss.str().c_str());
}
#endif
- //Write information about descriptors
+ // Write information about descriptors
descriptors_info_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorsInfo", NULL);
- int nRows = m_trainDescriptors.rows,
- nCols = m_trainDescriptors.cols;
+ int nRows = m_trainDescriptors.rows, nCols = m_trainDescriptors.cols;
int descriptorType = m_trainDescriptors.type();
- //Write the number of rows
+ // Write the number of rows
ss.str("");
ss << nRows;
xmlNewChild(descriptors_info_node, NULL, BAD_CAST "nrows", BAD_CAST ss.str().c_str());
- //Write the number of cols
+ // Write the number of cols
ss.str("");
ss << nCols;
xmlNewChild(descriptors_info_node, NULL, BAD_CAST "ncols", BAD_CAST ss.str().c_str());
- //Write the descriptors type
+ // Write the descriptors type
ss.str("");
ss << descriptorType;
xmlNewChild(descriptors_info_node, NULL, BAD_CAST "type", BAD_CAST ss.str().c_str());
for (int i = 0; i < nRows; i++) {
- unsigned int i_ = (unsigned int) i;
- descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo",
- NULL);
+ unsigned int i_ = (unsigned int)i;
+ descriptor_node = xmlNewChild(root_node, NULL, BAD_CAST "DescriptorInfo", NULL);
ss.str("");
- //max_digits10 == 9 for float
+ // 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
+ // 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
+ // 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
+ // 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
+ // 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());
+ 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<int, int>::const_iterator it_findImgId = m_mapOfImageId.find(m_trainKeyPoints[i_].class_id);
ss << ((saveTrainingImages && it_findImgId != m_mapOfImageId.end()) ? it_findImgId->second : -1);
- xmlNewChild(descriptor_node, NULL, BAD_CAST "image_id",
- BAD_CAST ss.str().c_str());
+ 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
+ // 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
+ // 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
+ // 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);
@@ -4090,57 +4199,52 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
for (int j = 0; j < nCols; j++) {
ss.str("");
- switch(descriptorType) {
- case CV_8U:
- {
- //Promote an unsigned char to an int
- //val_tmp holds the numeric value that will be written
- //We save the value in numeric form otherwise libxml2 will not be able to parse
- //A better solution could be possible
- int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
- ss << val_tmp;
- }
- break;
+ switch (descriptorType) {
+ case CV_8U: {
+ // Promote an unsigned char to an int
+ // val_tmp holds the numeric value that will be written
+ // We save the value in numeric form otherwise libxml2 will not be
+ // able to parse A better solution could be possible
+ int val_tmp = m_trainDescriptors.at<unsigned char>(i, j);
+ ss << val_tmp;
+ } break;
+
+ case CV_8S: {
+ // Promote a char to an int
+ // val_tmp holds the numeric value that will be written
+ // We save the value in numeric form otherwise libxml2 will not be
+ // able to parse A better solution could be possible
+ int val_tmp = m_trainDescriptors.at<char>(i, j);
+ ss << val_tmp;
+ } break;
- case CV_8S:
- {
- //Promote a char to an int
- //val_tmp holds the numeric value that will be written
- //We save the value in numeric form otherwise libxml2 will not be able to parse
- //A better solution could be possible
- int val_tmp = m_trainDescriptors.at<char>(i, j);
- ss << val_tmp;
- }
+ case CV_16U:
+ ss << m_trainDescriptors.at<unsigned short int>(i, j);
break;
- case CV_16U:
- ss << m_trainDescriptors.at<unsigned short int>(i, j);
- break;
-
- case CV_16S:
- ss << m_trainDescriptors.at<short int>(i, j);
- break;
+ case CV_16S:
+ ss << m_trainDescriptors.at<short int>(i, j);
+ break;
- case CV_32S:
- ss << m_trainDescriptors.at<int>(i, j);
- break;
+ case CV_32S:
+ ss << m_trainDescriptors.at<int>(i, j);
+ break;
- case CV_32F:
- //max_digits10 == 9 for float
- ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<float>(i, j);
- break;
+ case CV_32F:
+ // max_digits10 == 9 for float
+ ss << std::fixed << std::setprecision(9) << m_trainDescriptors.at<float>(i, j);
+ break;
- case CV_64F:
- //max_digits10 == 17 for double
- ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<double>(i, j);
- break;
+ case CV_64F:
+ // max_digits10 == 17 for double
+ ss << std::fixed << std::setprecision(17) << m_trainDescriptors.at<double>(i, j);
+ break;
- default:
- throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
- break;
+ default:
+ throw vpException(vpException::fatalError, "Problem with the data type of descriptors !");
+ break;
}
- xmlNewChild(desc_node, NULL, BAD_CAST "val",
- BAD_CAST ss.str().c_str());
+ xmlNewChild(desc_node, NULL, BAD_CAST "val", BAD_CAST ss.str().c_str());
}
}
@@ -4161,80 +4265,70 @@ void vpKeyPoint::saveLearningData(const std::string &filename, bool binaryMode,
}
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
-//From OpenCV 2.4.11 source code.
+// 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;
- }
+ KeypointResponseGreaterThanThreshold(float _value) : value(_value) {}
+ inline bool operator()(const cv::KeyPoint &kpt) const { return kpt.response >= value; }
float value;
};
struct KeypointResponseGreater {
- inline bool operator()(const cv::KeyPoint& kp1,
- const cv::KeyPoint& kp2) const {
- return kp1.response > kp2.response;
- }
+ inline bool operator()(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2) const { return kp1.response > kp2.response; }
};
// takes keypoints and culls them by the response
-void vpKeyPoint::KeyPointsFilter::retainBest(
- std::vector<cv::KeyPoint>& keypoints, int n_points) {
- //this is only necessary if the keypoints size is greater than the number of desired points.
- if (n_points >= 0 && keypoints.size() > (size_t) n_points) {
+void vpKeyPoint::KeyPointsFilter::retainBest(std::vector<cv::KeyPoint> &keypoints, int n_points)
+{
+ // this is only necessary if the keypoints size is greater than the number
+ // of desired points.
+ if (n_points >= 0 && keypoints.size() > (size_t)n_points) {
if (n_points == 0) {
keypoints.clear();
return;
}
- //first use nth element to partition the keypoints into the best and worst.
- std::nth_element(keypoints.begin(), keypoints.begin() + n_points,
- keypoints.end(), KeypointResponseGreater());
- //this is the boundary response, and in the case of FAST may be ambiguous
- float ambiguous_response = keypoints[(size_t) (n_points - 1)].response;
- //use std::partition to grab all of the keypoints with the boundary response.
+ // first use nth element to partition the keypoints into the best and
+ // worst.
+ std::nth_element(keypoints.begin(), keypoints.begin() + n_points, keypoints.end(), KeypointResponseGreater());
+ // this is the boundary response, and in the case of FAST may be ambiguous
+ float ambiguous_response = keypoints[(size_t)(n_points - 1)].response;
+ // use std::partition to grab all of the keypoints with the boundary
+ // response.
std::vector<cv::KeyPoint>::const_iterator new_end = std::partition(
- keypoints.begin() + n_points, keypoints.end(),
- KeypointResponseGreaterThanThreshold(ambiguous_response));
- //resize the keypoints, given this new end point. nth_element and partition reordered the points inplace
- keypoints.resize((size_t) (new_end - keypoints.begin()));
+ keypoints.begin() + n_points, keypoints.end(), KeypointResponseGreaterThanThreshold(ambiguous_response));
+ // resize the keypoints, given this new end point. nth_element and
+ // partition reordered the points inplace
+ keypoints.resize((size_t)(new_end - keypoints.begin()));
}
}
struct RoiPredicate {
- RoiPredicate(const cv::Rect& _r) :
- r(_r) {
- }
+ 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<cv::KeyPoint>& keypoints, cv::Size imageSize, int borderSize) {
+void vpKeyPoint::KeyPointsFilter::runByImageBorder(std::vector<cv::KeyPoint> &keypoints, cv::Size imageSize,
+ int borderSize)
+{
if (borderSize > 0) {
if (imageSize.height <= borderSize * 2 || imageSize.width <= borderSize * 2)
keypoints.clear();
else
- keypoints.erase(
- std::remove_if(keypoints.begin(), keypoints.end(),
- RoiPredicate(
- cv::Rect(cv::Point(borderSize, borderSize),
- cv::Point(imageSize.width - borderSize,
- imageSize.height - borderSize)))), keypoints.end());
+ 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 {
+ bool operator()(const cv::KeyPoint &keyPt) const
+ {
float size = keyPt.size;
return (size < minSize) || (size > maxSize);
}
@@ -4242,66 +4336,70 @@ struct SizePredicate {
float minSize, maxSize;
};
-void vpKeyPoint::KeyPointsFilter::runByKeypointSize(
- std::vector<cv::KeyPoint>& keypoints, float minSize, float maxSize) {
+void vpKeyPoint::KeyPointsFilter::runByKeypointSize(std::vector<cv::KeyPoint> &keypoints, float minSize, float maxSize)
+{
CV_Assert(minSize >= 0);
CV_Assert(maxSize >= 0);
CV_Assert(minSize <= maxSize);
- keypoints.erase(
- std::remove_if(keypoints.begin(), keypoints.end(),
- SizePredicate(minSize, maxSize)), keypoints.end());
+ keypoints.erase(std::remove_if(keypoints.begin(), keypoints.end(), SizePredicate(minSize, maxSize)), keypoints.end());
}
-class MaskPredicate {
+class MaskPredicate
+{
public:
- MaskPredicate(const cv::Mat& _mask) :
- mask(_mask) {
- }
- bool operator()(const cv::KeyPoint& key_pt) const {
- return mask.at<uchar>((int) (key_pt.pt.y + 0.5f),
- (int) (key_pt.pt.x + 0.5f)) == 0;
+ MaskPredicate(const cv::Mat &_mask) : mask(_mask) {}
+ bool operator()(const cv::KeyPoint &key_pt) const
+ {
+ return mask.at<uchar>((int)(key_pt.pt.y + 0.5f), (int)(key_pt.pt.x + 0.5f)) == 0;
}
private:
const cv::Mat mask;
- MaskPredicate& operator=(const MaskPredicate&);
+ MaskPredicate &operator=(const MaskPredicate &);
};
-void vpKeyPoint::KeyPointsFilter::runByPixelsMask(
- std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) {
+void vpKeyPoint::KeyPointsFilter::runByPixelsMask(std::vector<cv::KeyPoint> &keypoints, const cv::Mat &mask)
+{
if (mask.empty())
return;
- keypoints.erase(
- std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)),
- keypoints.end());
+ keypoints.erase(std::remove_if(keypoints.begin(), keypoints.end(), MaskPredicate(mask)), keypoints.end());
}
struct KeyPoint_LessThan {
- KeyPoint_LessThan(const std::vector<cv::KeyPoint>& _kp) :
- kp(&_kp) {
- }
- bool operator()(/*int i, int j*/ size_t i, size_t j) const {
- const cv::KeyPoint& kp1 = (*kp)[/*(size_t)*/ i];
- const cv::KeyPoint& kp2 = (*kp)[/*(size_t)*/ j];
- if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.x != kp2.pt.x) {
+ KeyPoint_LessThan(const std::vector<cv::KeyPoint> &_kp) : kp(&_kp) {}
+ bool operator()(/*int i, int j*/ size_t i, size_t j) const
+ {
+ const cv::KeyPoint &kp1 = (*kp)[/*(size_t)*/ i];
+ const cv::KeyPoint &kp2 = (*kp)[/*(size_t)*/ j];
+ if (!vpMath::equal(kp1.pt.x, kp2.pt.x,
+ std::numeric_limits<float>::epsilon())) { // if (kp1.pt.x !=
+ // kp2.pt.x) {
return kp1.pt.x < kp2.pt.x;
}
- if (!vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon())) { //if (kp1.pt.y != kp2.pt.y) {
+ if (!vpMath::equal(kp1.pt.y, kp2.pt.y,
+ std::numeric_limits<float>::epsilon())) { // if (kp1.pt.y !=
+ // kp2.pt.y) {
return kp1.pt.y < kp2.pt.y;
}
- if (!vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon())) { //if (kp1.size != kp2.size) {
+ if (!vpMath::equal(kp1.size, kp2.size,
+ std::numeric_limits<float>::epsilon())) { // if (kp1.size !=
+ // kp2.size) {
return kp1.size > kp2.size;
}
- if (!vpMath::equal(kp1.angle, kp2.angle, std::numeric_limits<float>::epsilon())) { //if (kp1.angle != kp2.angle) {
+ if (!vpMath::equal(kp1.angle, kp2.angle,
+ std::numeric_limits<float>::epsilon())) { // if (kp1.angle !=
+ // kp2.angle) {
return kp1.angle < kp2.angle;
}
- if (!vpMath::equal(kp1.response, kp2.response, std::numeric_limits<float>::epsilon())) { //if (kp1.response != kp2.response) {
+ if (!vpMath::equal(kp1.response, kp2.response,
+ std::numeric_limits<float>::epsilon())) { // if (kp1.response !=
+ // kp2.response) {
return kp1.response > kp2.response;
}
@@ -4315,23 +4413,24 @@ struct KeyPoint_LessThan {
return i < j;
}
- const std::vector<cv::KeyPoint>* kp;
+ const std::vector<cv::KeyPoint> *kp;
};
-void vpKeyPoint::KeyPointsFilter::removeDuplicated(
- std::vector<cv::KeyPoint>& keypoints) {
+void vpKeyPoint::KeyPointsFilter::removeDuplicated(std::vector<cv::KeyPoint> &keypoints)
+{
size_t i, j, n = keypoints.size();
std::vector<size_t> kpidx(n);
- std::vector<uchar> mask(n, (uchar) 1);
+ std::vector<uchar> mask(n, (uchar)1);
for (i = 0; i < n; i++) {
kpidx[i] = i;
}
std::sort(kpidx.begin(), kpidx.end(), KeyPoint_LessThan(keypoints));
for (i = 1, j = 0; i < n; i++) {
- cv::KeyPoint& kp1 = keypoints[kpidx[i]];
- cv::KeyPoint& kp2 = keypoints[kpidx[j]];
-// if (kp1.pt.x != kp2.pt.x || kp1.pt.y != kp2.pt.y || kp1.size != kp2.size || kp1.angle != kp2.angle) {
+ cv::KeyPoint &kp1 = keypoints[kpidx[i]];
+ cv::KeyPoint &kp2 = keypoints[kpidx[j]];
+ // if (kp1.pt.x != kp2.pt.x || kp1.pt.y != kp2.pt.y || kp1.size !=
+ // kp2.size || kp1.angle != kp2.angle) {
if (!vpMath::equal(kp1.pt.x, kp2.pt.x, std::numeric_limits<float>::epsilon()) ||
!vpMath::equal(kp1.pt.y, kp2.pt.y, std::numeric_limits<float>::epsilon()) ||
!vpMath::equal(kp1.size, kp2.size, std::numeric_limits<float>::epsilon()) ||
@@ -4356,22 +4455,26 @@ void vpKeyPoint::KeyPointsFilter::removeDuplicated(
/*
* PyramidAdaptedFeatureDetector
*/
-vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(
- const cv::Ptr<cv::FeatureDetector>& _detector, int _maxLevel) :
- detector(_detector), maxLevel(_maxLevel) {
+vpKeyPoint::PyramidAdaptedFeatureDetector::PyramidAdaptedFeatureDetector(const cv::Ptr<cv::FeatureDetector> &_detector,
+ int _maxLevel)
+ : detector(_detector), maxLevel(_maxLevel)
+{
}
-bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const {
- return detector.empty() || (cv::FeatureDetector*) detector->empty();
+bool vpKeyPoint::PyramidAdaptedFeatureDetector::empty() const
+{
+ return detector.empty() || (cv::FeatureDetector *)detector->empty();
}
-void vpKeyPoint::PyramidAdaptedFeatureDetector::detect( cv::InputArray image, CV_OUT std::vector<cv::KeyPoint>& keypoints,
- cv::InputArray mask ) {
+void vpKeyPoint::PyramidAdaptedFeatureDetector::detect(cv::InputArray image,
+ CV_OUT std::vector<cv::KeyPoint> &keypoints, cv::InputArray mask)
+{
detectImpl(image.getMat(), keypoints, mask.getMat());
}
-void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
- std::vector<cv::KeyPoint>& keypoints, const cv::Mat& mask) const {
+void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat &image, std::vector<cv::KeyPoint> &keypoints,
+ const cv::Mat &mask) const
+{
cv::Mat src = image;
cv::Mat src_mask = mask;
@@ -4387,8 +4490,7 @@ void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
// Detect on current level of the pyramid
std::vector<cv::KeyPoint> new_pts;
detector->detect(src, new_pts, src_mask);
- std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end =
- new_pts.end();
+ std::vector<cv::KeyPoint>::iterator it = new_pts.begin(), end = new_pts.end();
for (; it != end; ++it) {
it->pt.x *= multiplier;
it->pt.y *= multiplier;
@@ -4414,6 +4516,7 @@ void vpKeyPoint::PyramidAdaptedFeatureDetector::detectImpl(const cv::Mat& image,
#endif
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work around to avoid warning: libvisp_vision.a(vpKeyPoint.cpp.o) has no symbols
-void dummy_vpKeyPoint() {};
+// Work around to avoid warning: libvisp_vision.a(vpKeyPoint.cpp.o) has no
+// symbols
+void dummy_vpKeyPoint(){};
#endif
diff --git a/modules/vision/src/key-point/vpKeyPointSurf.cpp b/modules/vision/src/key-point/vpKeyPointSurf.cpp
index 18fc768..e06107e 100644
--- a/modules/vision/src/key-point/vpKeyPointSurf.cpp
+++ b/modules/vision/src/key-point/vpKeyPointSurf.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,158 +36,128 @@
*
*****************************************************************************/
-
#include <visp3/vision/vpKeyPointSurf.h>
-#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x010100) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 1.1.0 < 3.0.0
+#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION >= 0x010100) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 1.1.0 < 3.0.0
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImageTools.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/core/vpDebug.h>
#include <ctype.h>
+#include <list>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
-#include <list>
-double compareSURFDescriptors( const float* d1, const float* d2, double best, int length );
-int naiveNearestNeighbor( const float *vec, int laplacian,
- const CvSeq *model_keypoints,
- const CvSeq *model_descriptors );
-int naiveNearestNeighbor( const float *vec,
- const CvSeq *ref_keypoints,
- const CvSeq *ref_descriptors );
-void findPairs( const CvSeq* objectKeypoints,
- const CvSeq* objectDescriptors,
- const CvSeq* imageKeypoints,
- const CvSeq* imageDescriptors,
- std::vector<int>& ptpairs );
+double compareSURFDescriptors(const float *d1, const float *d2, double best, int length);
+int naiveNearestNeighbor(const float *vec, int laplacian, const CvSeq *model_keypoints, const CvSeq *model_descriptors);
+int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, const CvSeq *ref_descriptors);
+void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints,
+ const CvSeq *imageDescriptors, std::vector<int> &ptpairs);
// Compare two surf descriptors.
-double compareSURFDescriptors( const float* d1, const float* d2, double best, int length )
+double compareSURFDescriptors(const float *d1, const float *d2, double best, int length)
{
double total_cost = 0;
int i;
- assert( length % 4 == 0 );
- for( i = 0; i < length; i += 4 )
- {
+ assert(length % 4 == 0);
+ for (i = 0; i < length; i += 4) {
double t0 = d1[i] - d2[i];
- double t1 = d1[i+1] - d2[i+1];
- double t2 = d1[i+2] - d2[i+2];
- double t3 = d1[i+3] - d2[i+3];
- total_cost += t0*t0 + t1*t1 + t2*t2 + t3*t3;
- if( total_cost > best )
+ double t1 = d1[i + 1] - d2[i + 1];
+ double t2 = d1[i + 2] - d2[i + 2];
+ double t3 = d1[i + 3] - d2[i + 3];
+ total_cost += t0 * t0 + t1 * t1 + t2 * t2 + t3 * t3;
+ if (total_cost > best)
break;
}
return total_cost;
}
-
-//Find for a point candidate the most similar point in the reference point list.
-int naiveNearestNeighbor( const float *vec, int laplacian,
- const CvSeq *model_keypoints,
- const CvSeq *model_descriptors )
+// Find for a point candidate the most similar point in the reference point
+// list.
+int naiveNearestNeighbor(const float *vec, int laplacian, const CvSeq *model_keypoints, const CvSeq *model_descriptors)
{
- int length = (int)(model_descriptors->elem_size/(int)sizeof(float));
+ int length = (int)(model_descriptors->elem_size / (int)sizeof(float));
int i, neighbor = -1;
double d, dist1 = 1e6, dist2 = 1e6;
CvSeqReader reader, kreader;
- cvStartReadSeq( model_keypoints, &kreader, 0 );
- cvStartReadSeq( model_descriptors, &reader, 0 );
-
- for( i = 0; i < model_descriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* mvec = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- if( laplacian != kp->laplacian )
+ cvStartReadSeq(model_keypoints, &kreader, 0);
+ cvStartReadSeq(model_descriptors, &reader, 0);
+
+ for (i = 0; i < model_descriptors->total; i++) {
+ const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
+ const float *mvec = (const float *)reader.ptr;
+ CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ if (laplacian != kp->laplacian)
continue;
- d = compareSURFDescriptors( vec, mvec, dist2, length );
- if( d < dist1 )
- {
+ d = compareSURFDescriptors(vec, mvec, dist2, length);
+ if (d < dist1) {
dist2 = dist1;
dist1 = d;
neighbor = i;
- }
- else if ( d < dist2 )
+ } else if (d < dist2)
dist2 = d;
}
- if ( dist1 < 0.6*dist2 )
+ if (dist1 < 0.6 * dist2)
return neighbor;
return -1;
}
-
-//Find for a point candidate the most similar point in the reference point list.
-int naiveNearestNeighbor( const float *vec,
- const CvSeq *ref_keypoints,
- const CvSeq *ref_descriptors )
+// Find for a point candidate the most similar point in the reference point
+// list.
+int naiveNearestNeighbor(const float *vec, const CvSeq *ref_keypoints, const CvSeq *ref_descriptors)
{
- int length = (int)(ref_descriptors->elem_size/(int)sizeof(float));
+ int length = (int)(ref_descriptors->elem_size / (int)sizeof(float));
int i, neighbor = -1;
- double d, dist1 = 1e6, dist2 = 1e6;
+ double dist1 = 1e6, dist2 = 1e6;
CvSeqReader reader, kreader;
- cvStartReadSeq( ref_keypoints, &kreader, 0 );
- cvStartReadSeq( ref_descriptors, &reader, 0 );
-
- for( i = 0; i < ref_descriptors->total; i++ )
- {
- const float* mvec = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- d = compareSURFDescriptors( vec, mvec, dist2, length );
- if( d < dist1 )
- {
+ cvStartReadSeq(ref_keypoints, &kreader, 0);
+ cvStartReadSeq(ref_descriptors, &reader, 0);
+
+ for (i = 0; i < ref_descriptors->total; i++) {
+ const float *mvec = (const float *)reader.ptr;
+ CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ double d = compareSURFDescriptors(vec, mvec, dist2, length);
+ if (d < dist1) {
dist2 = dist1;
dist1 = d;
neighbor = i;
- }
- else if ( d < dist2 )
+ } else if (d < dist2)
dist2 = d;
}
- if ( dist1 < 0.6*dist2 )
+ if (dist1 < 0.6 * dist2)
return neighbor;
return -1;
}
-
-
-//Find all the matched points
-void findPairs( const CvSeq* objectKeypoints,
- const CvSeq* objectDescriptors,
- const CvSeq* imageKeypoints,
- const CvSeq* imageDescriptors,
- std::vector<int>& ptpairs )
+// Find all the matched points
+void findPairs(const CvSeq *objectKeypoints, const CvSeq *objectDescriptors, const CvSeq *imageKeypoints,
+ const CvSeq *imageDescriptors, std::vector<int> &ptpairs)
{
int i;
CvSeqReader reader, kreader;
- cvStartReadSeq( objectKeypoints, &kreader );
- cvStartReadSeq( objectDescriptors, &reader );
+ cvStartReadSeq(objectKeypoints, &kreader);
+ cvStartReadSeq(objectDescriptors, &reader);
ptpairs.clear();
- for( i = 0; i < objectDescriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* descriptor = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- int nearest_neighbor = naiveNearestNeighbor( descriptor,
- kp->laplacian,
- imageKeypoints,
- imageDescriptors );
- if( nearest_neighbor >= 0 )
- {
+ for (i = 0; i < objectDescriptors->total; i++) {
+ const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
+ const float *descriptor = (const float *)reader.ptr;
+ CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, imageKeypoints, imageDescriptors);
+ if (nearest_neighbor >= 0) {
ptpairs.push_back(i);
ptpairs.push_back(nearest_neighbor);
}
}
}
-
-
/*!
Basic constructor. It sets by default the hessian threshold to 500
@@ -195,8 +166,7 @@ void findPairs( const CvSeq* objectKeypoints,
*/
vpKeyPointSurf::vpKeyPointSurf()
- : vpBasicKeyPoint(),
- storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
+ : vpBasicKeyPoint(), storage(NULL), params(), storage_cur(NULL), image_keypoints(NULL), image_descriptors(NULL),
ref_keypoints(NULL), ref_descriptors(NULL), hessianThreshold(500), descriptorType(extendedDescriptor)
{
init();
@@ -222,8 +192,8 @@ void vpKeyPointSurf::init()
vpKeyPointSurf::~vpKeyPointSurf()
{
cvReleaseMemStorage(&storage);
- if(storage_cur!=NULL){
- cvReleaseMemStorage(&storage_cur);
+ if (storage_cur != NULL) {
+ cvReleaseMemStorage(&storage_cur);
}
}
@@ -238,36 +208,35 @@ vpKeyPointSurf::~vpKeyPointSurf()
*/
unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
{
- IplImage* model = NULL;
+ IplImage *model = NULL;
- if((I.getWidth() % 8) == 0){
+ if ((I.getWidth() % 8) == 0) {
int height = (int)I.getHeight();
- int width = (int)I.getWidth();
+ int width = (int)I.getWidth();
CvSize size = cvSize(width, height);
model = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
- model->imageData = (char*)I.bitmap;
- }else{
- vpImageConvert::convert(I,model);
+ model->imageData = (char *)I.bitmap;
+ } else {
+ vpImageConvert::convert(I, model);
}
- cvExtractSURF( model, 0, &ref_keypoints, &ref_descriptors, storage, params );
-
+ cvExtractSURF(model, 0, &ref_keypoints, &ref_descriptors, storage, params);
+
const unsigned int nbPoints = (unsigned int)ref_keypoints->total;
referenceImagePointsList.resize(nbPoints);
- for(unsigned int i = 0; i < nbPoints; i++ )
- {
- CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(ref_keypoints, (int)i);
+ for (unsigned int i = 0; i < nbPoints; i++) {
+ CvSURFPoint *r1 = (CvSURFPoint *)cvGetSeqElem(ref_keypoints, (int)i);
referenceImagePointsList[i].set_i(r1->pt.y);
referenceImagePointsList[i].set_j(r1->pt.x);
}
- if((I.getWidth() % 8) == 0){
+ if ((I.getWidth() % 8) == 0) {
model->imageData = NULL;
cvReleaseImageHeader(&model);
- }else{
+ } else {
cvReleaseImage(&model);
}
@@ -275,7 +244,6 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
return nbPoints;
}
-
/*!
Build the list of reference points. The computation of the points is
@@ -293,38 +261,27 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I)
\return the number of reference points.
*/
-unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width)
+unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth())
- {
+ 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<unsigned char> 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());
+ for (unsigned int k = 0; k < nbRefPoint; k++) {
+ (referenceImagePointsList[k]).set_i((referenceImagePointsList[k]).get_i() + iP.get_i());
+ (referenceImagePointsList[k]).set_j((referenceImagePointsList[k]).get_j() + iP.get_j());
}
- return(nbRefPoint);
+ return (nbRefPoint);
}
-
/*!
Build the list of reference points. The computation of the points is
@@ -338,18 +295,14 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
\return the number of reference points.
*/
-unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
- const vpRect& rectangle)
+unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I, const vpRect &rectangle)
{
vpImagePoint iP;
iP.set_i(rectangle.getTop());
iP.set_j(rectangle.getLeft());
- return (this->buildReference(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
+ return (this->buildReference(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
}
-
/*!
Computes the SURF points in the current image I and try to matched
@@ -362,52 +315,44 @@ unsigned int vpKeyPointSurf::buildReference(const vpImage<unsigned char> &I,
*/
unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
{
- IplImage* currentImage = NULL;
+ IplImage *currentImage = NULL;
- if((I.getWidth() % 8) == 0){
+ if ((I.getWidth() % 8) == 0) {
int height = (int)I.getHeight();
- int width = (int)I.getWidth();
+ int width = (int)I.getWidth();
CvSize size = cvSize(width, height);
currentImage = cvCreateImageHeader(size, IPL_DEPTH_8U, 1);
- currentImage->imageData = (char*)I.bitmap;
- }else{
- vpImageConvert::convert(I,currentImage);
+ currentImage->imageData = (char *)I.bitmap;
+ } else {
+ vpImageConvert::convert(I, currentImage);
}
-
- /* we release the memory storage for the current points (it has to be kept
+
+ /* we release the memory storage for the current points (it has to be kept
allocated for the get descriptor points, ...) */
- if(storage_cur != NULL){
+ if (storage_cur != NULL) {
cvReleaseMemStorage(&storage_cur);
storage_cur = NULL;
}
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 );
- cvStartReadSeq( ref_descriptors, &reader );
-
+ cvStartReadSeq(ref_keypoints, &kreader);
+ cvStartReadSeq(ref_descriptors, &reader);
std::list<int> indexImagePair;
std::list<int> indexReferencePair;
-
unsigned int nbrPair = 0;
- for(int i = 0; i < ref_descriptors->total; i++ )
- {
- const CvSURFPoint* kp = (const CvSURFPoint*)kreader.ptr;
- const float* descriptor = (const float*)reader.ptr;
- CV_NEXT_SEQ_ELEM( kreader.seq->elem_size, kreader );
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- int nearest_neighbor = naiveNearestNeighbor( descriptor,
- kp->laplacian,
- image_keypoints,
- image_descriptors );
- if( nearest_neighbor >= 0 )
- {
+ for (int i = 0; i < ref_descriptors->total; i++) {
+ const CvSURFPoint *kp = (const CvSURFPoint *)kreader.ptr;
+ const float *descriptor = (const float *)reader.ptr;
+ CV_NEXT_SEQ_ELEM(kreader.seq->elem_size, kreader);
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ int nearest_neighbor = naiveNearestNeighbor(descriptor, kp->laplacian, image_keypoints, image_descriptors);
+ if (nearest_neighbor >= 0) {
indexReferencePair.push_back(i);
indexImagePair.push_back(nearest_neighbor);
nbrPair++;
@@ -425,33 +370,30 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
currentImagePointsList.resize(nbrPair);
matchedReferencePoints.resize(nbrPair);
- for (unsigned int i = 0; i < nbrPair; i++)
- {
- int index = *indexImagePairIter;
+ for (unsigned int i = 0; i < nbrPair; i++) {
+ int index = *indexImagePairIter;
- CvSURFPoint* r1 = (CvSURFPoint*)cvGetSeqElem(image_keypoints, index);
+ CvSURFPoint *r1 = (CvSURFPoint *)cvGetSeqElem(image_keypoints, index);
- currentImagePointsList[i].set_i(r1->pt.y);
- currentImagePointsList[i].set_j(r1->pt.x);
+ currentImagePointsList[i].set_i(r1->pt.y);
+ currentImagePointsList[i].set_j(r1->pt.x);
- matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
+ matchedReferencePoints[i] = (unsigned int)*indexReferencePairIter;
-
- ++indexImagePairIter;
- ++indexReferencePairIter;
+ ++indexImagePairIter;
+ ++indexReferencePairIter;
}
- if((I.getWidth() % 8) == 0){
+ if ((I.getWidth() % 8) == 0) {
currentImage->imageData = NULL;
cvReleaseImageHeader(¤tImage);
- }else{
+ } else {
cvReleaseImage(¤tImage);
}
return nbrPair;
}
-
/*!
Computes the SURF points in only a part of the current image I and
@@ -470,39 +412,28 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I)
\return the number of point which have been matched.
*/
-unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP,
- const unsigned int height, const unsigned int width)
+unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth())
- {
+ 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<unsigned char> 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());
+ for (unsigned int k = 0; k < nbMatchedPoint; k++) {
+ (currentImagePointsList[k]).set_i((currentImagePointsList[k]).get_i() + iP.get_i());
+ (currentImagePointsList[k]).set_j((currentImagePointsList[k]).get_j() + iP.get_j());
}
- return(nbMatchedPoint);
+ return (nbMatchedPoint);
}
-
/*!
Computes the SURF points in only a part of the current image I and
@@ -517,18 +448,14 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
\return the number of point which have been matched.
*/
-unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
- const vpRect& rectangle)
+unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
{
vpImagePoint iP;
iP.set_i(rectangle.getTop());
iP.set_j(rectangle.getLeft());
- return (this->matchPoint(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
+ return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
}
-
/*!
This function displays the matched reference points and the matched
@@ -544,32 +471,32 @@ unsigned int vpKeyPointSurf::matchPoint(const vpImage<unsigned char> &I,
\param Icurrent : The image where the matched points computed in the
current image are displayed.
- \param size : Size in pixels of the cross that is used to display matched points.
+ \param size : Size in pixels of the cross that is used to display matched
+ points.
*/
-void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference,
- const vpImage<unsigned char> &Icurrent, unsigned int size)
+void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference, const vpImage<unsigned char> &Icurrent,
+ unsigned int size)
{
-// matchedPointsCurrentImageList.front();
-// matchedPointsReferenceImageList.front();
-
-// if (matchedPointsCurrentImageList.nbElements()
-// != matchedPointsReferenceImageList.nbElements())
-// {
-// vpTRACE("Numbers of points mismatch");
-// throw(vpException(vpException::fatalError,"Numbers of points mismatch"));
-// }
-
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
- {
- vpDisplay::displayCross (Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
- vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, vpColor::green);
-// matchedPointsReferenceImageList.next();
-// matchedPointsCurrentImageList.next();
+ // matchedPointsCurrentImageList.front();
+ // matchedPointsReferenceImageList.front();
+
+ // if (matchedPointsCurrentImageList.nbElements()
+ // != matchedPointsReferenceImageList.nbElements())
+ // {
+ // vpTRACE("Numbers of points mismatch");
+ // throw(vpException(vpException::fatalError,"Numbers of points
+ // mismatch"));
+ // }
+
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
+ vpDisplay::displayCross(Ireference, referenceImagePointsList[matchedReferencePoints[i]], size, vpColor::red);
+ vpDisplay::displayCross(Icurrent, currentImagePointsList[i], size, vpColor::green);
+ // matchedPointsReferenceImageList.next();
+ // matchedPointsCurrentImageList.next();
}
}
-
/*!
This function displays only the matched points computed in the
@@ -578,45 +505,45 @@ void vpKeyPointSurf::display(const vpImage<unsigned char> &Ireference,
\param Icurrent : The image where the matched points computed in the
current image are displayed.
- \param size : Size in pixels of the cross that is used to display matched points.
+ \param size : Size in pixels of the cross that is used to display matched
+ points.
\param color : Color used to display the matched points.
*/
void vpKeyPointSurf::display(const vpImage<unsigned char> &Icurrent, unsigned int size, const vpColor &color)
{
-// matchedPointsCurrentImageList.front();
-//
-// vpImagePoint ipCur;
-//
- for (unsigned int i = 0; i < matchedReferencePoints.size(); i++)
- {
- vpDisplay::displayCross (Icurrent, currentImagePointsList[i], size, color);
+ // matchedPointsCurrentImageList.front();
+ //
+ // vpImagePoint ipCur;
+ //
+ for (unsigned int i = 0; i < matchedReferencePoints.size(); i++) {
+ vpDisplay::displayCross(Icurrent, currentImagePointsList[i], size, color);
}
}
-std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, std::list<int> laplacianList)
+std::list<int *> *vpKeyPointSurf::matchPoint(std::list<float *> descriptorList, std::list<int> laplacianList)
{
- std::list<int*>* pairPoints = new std::list<int*>;
+ std::list<int *> *pairPoints = new std::list<int *>;
- if(descriptorList.size() != laplacianList.size()){
+ if (descriptorList.size() != laplacianList.size()) {
vpTRACE("Error, the two lists have different number of element");
return pairPoints;
}
CvSeqReader reader;
- cvStartReadSeq( ref_descriptors, &reader );
+ cvStartReadSeq(ref_descriptors, &reader);
- std::list<float*>::const_iterator descriptorListIter = descriptorList.begin();
+ std::list<float *>::const_iterator descriptorListIter = descriptorList.begin();
std::list<int>::const_iterator laplacianListIter = laplacianList.begin();
descriptorList.front();
int indexList = 0;
- while(descriptorListIter != descriptorList.end()){
- float* descriptor = *descriptorListIter;
+ 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;
+ if (nearest_neighbor >= 0) {
+ int *tab;
tab = new int[2];
tab[0] = nearest_neighbor;
tab[1] = indexList;
@@ -633,90 +560,93 @@ std::list<int*>* vpKeyPointSurf::matchPoint(std::list<float*> descriptorList, st
/*!
Get the descriptor of the nth reference point.
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
+ \param index : The index of the desired reference point. The index must be
+ between 0 and the number of reference points - 1.
*/
-float* vpKeyPointSurf::getDescriptorReferencePoint (const int index)
+float *vpKeyPointSurf::getDescriptorReferencePoint(const int index)
{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
}
- float* descriptor = NULL;
+ float *descriptor = NULL;
- CvSeqReader reader;
- cvStartReadSeq( ref_descriptors, &reader );
+ CvSeqReader reader;
+ cvStartReadSeq(ref_descriptors, &reader);
- for(int j = 0; j < ref_descriptors->total; j++ ){
- if(j== index){
- descriptor = (float*)reader.ptr;
- break;
- }
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- }
+ for (int j = 0; j < ref_descriptors->total; j++) {
+ if (j == index) {
+ descriptor = (float *)reader.ptr;
+ break;
+ }
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ }
- return descriptor;
+ return descriptor;
}
/*!
Get the laplacian of the nth reference point.
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
+ \param index : The index of the desired reference point. The index must be
+ between 0 and the number of reference points - 1.
*/
-int vpKeyPointSurf::getLaplacianReferencePoint (const int index)
+int vpKeyPointSurf::getLaplacianReferencePoint(const int index)
{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
}
- CvSeqReader reader;
- cvStartReadSeq( ref_keypoints, &reader );
+ CvSeqReader reader;
+ cvStartReadSeq(ref_keypoints, &reader);
- int laplacian = 0;/* normally only -1, 0, +1 are possible */
+ int laplacian = 0; /* normally only -1, 0, +1 are possible */
- for(int j = 0; j < ref_keypoints->total; j++ ){
- if(j== index){
- const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
- laplacian = kp->laplacian;
- break;
- }
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
- }
+ for (int j = 0; j < ref_keypoints->total; j++) {
+ if (j == index) {
+ const CvSURFPoint *kp = (const CvSURFPoint *)reader.ptr;
+ laplacian = kp->laplacian;
+ break;
+ }
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
+ }
- return laplacian;
+ return laplacian;
}
/*!
Get the parameters of the descriptor of the nth reference point.
- \param index : The index of the desired reference point. The index must be between 0 and the number of reference points - 1.
- \param size : The size of the point used to compute the descriptor.
- \param dir : The orientation of the descriptor (in degree).
+ \param index : The index of the desired reference point. The index must be
+ between 0 and the number of reference points - 1. \param size : The size of
+ the point used to compute the descriptor. \param dir : The orientation of the
+ descriptor (in degree).
*/
-void vpKeyPointSurf::getDescriptorParamReferencePoint (const int index, int& size, float& dir)
+void vpKeyPointSurf::getDescriptorParamReferencePoint(const int index, int &size, float &dir)
{
- if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0){
+ if (index >= static_cast<int>(referenceImagePointsList.size()) || index < 0) {
vpTRACE("Index of the reference point out of range");
- throw(vpException(vpException::fatalError,"Index of the reference point out of range"));
+ throw(vpException(vpException::fatalError, "Index of the reference point out of range"));
}
CvSeqReader reader;
- cvStartReadSeq( ref_keypoints, &reader );
+ cvStartReadSeq(ref_keypoints, &reader);
- for(int j = 0; j < ref_keypoints->total; j++ ){
- if(j== index){
- const CvSURFPoint* kp = (const CvSURFPoint*)reader.ptr;
+ for (int j = 0; j < ref_keypoints->total; j++) {
+ if (j == index) {
+ const CvSURFPoint *kp = (const CvSURFPoint *)reader.ptr;
size = kp->size;
dir = kp->dir;
break;
}
- CV_NEXT_SEQ_ELEM( reader.seq->elem_size, reader );
+ CV_NEXT_SEQ_ELEM(reader.seq->elem_size, reader);
}
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_vision.a(vpKeyPointSurf.cpp.o) has no symbols
-void dummy_vpKeyPointSurf() {};
+// Work arround to avoid warning: libvisp_vision.a(vpKeyPointSurf.cpp.o) has
+// no symbols
+void dummy_vpKeyPointSurf(){};
#endif
-
diff --git a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp
index 6af3000..36ec70e 100644
--- a/modules/vision/src/key-point/vpPlanarObjectDetector.cpp
+++ b/modules/vision/src/key-point/vpPlanarObjectDetector.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,17 +38,18 @@
#include <visp3/vision/vpPlanarObjectDetector.h>
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020000) && \
+ (VISP_HAVE_OPENCV_VERSION < 0x030000) // Require opencv >= 2.0.0 and < 3.0.0
-#include <visp3/core/vpImageConvert.h>
+#include <visp3/core/vpColor.h>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpException.h>
+#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpDisplay.h>
-#include <visp3/core/vpColor.h>
#include <visp3/core/vpImageTools.h>
-#include <vector>
#include <iostream>
+#include <vector>
/*!
@@ -63,12 +65,12 @@ vpPlanarObjectDetector::vpPlanarObjectDetector()
/*!
Basic constructor, load data from a file.
-
+
\param _dataFile : the name of the file.
\param _objectName : the name of the object to load.
*/
-vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, const std::string& _objectName)
+vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string &_dataFile, const std::string &_objectName)
: fern(), homography(), H(), dst_corners(), isCorrect(false), ref_corners(), modelROI(), currentImagePoints(),
refImagePoints(), minNbMatching(10)
{
@@ -78,221 +80,197 @@ vpPlanarObjectDetector::vpPlanarObjectDetector(const std::string& _dataFile, con
/*!
Initialise stuff. For the moment does nothing.
*/
-void
-vpPlanarObjectDetector::init()
-{
-}
+void vpPlanarObjectDetector::init() {}
/*!
Basic destructor
*/
-vpPlanarObjectDetector::~vpPlanarObjectDetector()
-{
-
-}
+vpPlanarObjectDetector::~vpPlanarObjectDetector() {}
/*!
- Compute the rectangular ROI from at least 4 points and set the region of
- interest on the current image.
-
+ Compute the rectangular ROI from at least 4 points and set the region of
+ interest on the current image.
+
\param ip : the list of image point.
\param nbpt : the number of point.
*/
-void
-vpPlanarObjectDetector::computeRoi(vpImagePoint* ip, const unsigned int nbpt)
+void vpPlanarObjectDetector::computeRoi(vpImagePoint *ip, const unsigned int nbpt)
{
- if(nbpt < 3){
+ if (nbpt < 3) {
throw vpException(vpException::badValue, "Not enough point to compute the region of interest.");
}
-
- std::vector < vpImagePoint > ptsx(nbpt);
- std::vector < vpImagePoint > ptsy(nbpt);
- for(unsigned int i=0; i<nbpt; i++){
+
+ std::vector<vpImagePoint> ptsx(nbpt);
+ std::vector<vpImagePoint> ptsy(nbpt);
+ for (unsigned int i = 0; i < nbpt; i++) {
ptsx[i] = ptsy[i] = ip[i];
}
-
- for(unsigned int i=0; i<nbpt; i++){
- for(unsigned int j=0; j<nbpt-1; j++){
- if(ptsx[j].get_j() > ptsx[j+1].get_j()){
- double tmp = ptsx[j+1].get_j();
- ptsx[j+1].set_j(ptsx[j].get_j());
+
+ for (unsigned int i = 0; i < nbpt; i++) {
+ for (unsigned int j = 0; j < nbpt - 1; j++) {
+ if (ptsx[j].get_j() > ptsx[j + 1].get_j()) {
+ double tmp = ptsx[j + 1].get_j();
+ ptsx[j + 1].set_j(ptsx[j].get_j());
ptsx[j].set_j(tmp);
}
}
}
- for(unsigned int i=0; i<nbpt; i++){
- for(unsigned int j=0; j<nbpt-1; j++){
- if(ptsy[j].get_i() > ptsy[j+1].get_i()){
- double tmp = ptsy[j+1].get_i();
- ptsy[j+1].set_i(ptsy[j].get_i());
+ for (unsigned int i = 0; i < nbpt; i++) {
+ for (unsigned int j = 0; j < nbpt - 1; j++) {
+ if (ptsy[j].get_i() > ptsy[j + 1].get_i()) {
+ double tmp = ptsy[j + 1].get_i();
+ ptsy[j + 1].set_i(ptsy[j].get_i());
ptsy[j].set_i(tmp);
}
}
}
-
}
-
/*!
- Train the classifier from the entire image (it is therefore assumed that the
+ Train the classifier from the entire image (it is therefore assumed that the
image is planar).
-
+
Depending on the parameters, the training can take up to several minutes.
-
+
\param _I : The image use to train the classifier.
-
+
\return The number of reference points.
*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
+unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I)
{
modelROI.x = 0;
modelROI.y = 0;
modelROI.width = (int)_I.getWidth();
- modelROI.height = (int)_I.getHeight();
+ modelROI.height = (int)_I.getHeight();
+
+ initialiseRefCorners(modelROI);
- initialiseRefCorners(modelROI);
-
return fern.buildReference(_I);
}
-
/*!
- Train the classifier on a region of the entire image. The region is a
- rectangle defined by its top left corner, its height and its width. The
- parameters of this rectangle must be given in pixel. It also includes the
+ Train the classifier on a region of the entire image. The region is a
+ rectangle defined by its top left corner, its height and its width. The
+ parameters of this rectangle must be given in pixel. It also includes the
training of the fern classifier.
-
+
\param _I : The image use to train the classifier.
- \param _iP : The top left corner of the rectangle defining the region of interest (ROI).
- \param _height : The height of the ROI.
- \param _width : The width of the ROI.
-
+ \param _iP : The top left corner of the rectangle defining the region of
+ interest (ROI). \param _height : The height of the ROI. \param _width : The
+ width of the ROI.
+
\return the number of reference points
*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
- const vpImagePoint &_iP,
- unsigned int _height, unsigned int _width)
+unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpImagePoint &_iP,
+ unsigned int _height, unsigned int _width)
{
unsigned int res = fern.buildReference(_I, _iP, _height, _width);
modelROI.x = (int)_iP.get_u();
modelROI.y = (int)_iP.get_v();
modelROI.width = (int)_width;
- modelROI.height = (int)_height;
+ modelROI.height = (int)_height;
+
+ initialiseRefCorners(modelROI);
- initialiseRefCorners(modelROI);
-
return res;
}
-
-
/*!
- Train the classifier on a region of the entire image. The region is a
- rectangle. The parameters of this rectangle must be given in pixel. It also
+ Train the classifier on a region of the entire image. The region is a
+ rectangle. The parameters of this rectangle must be given in pixel. It also
includes the training of the fern classifier.
-
+
\param _I : The image use to train the classifier.
\param _rectangle : The rectangle defining the region of interest (ROI).
-
+
\return The number of reference points.
*/
-unsigned int
-vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I,
- const vpRect _rectangle)
+unsigned int vpPlanarObjectDetector::buildReference(const vpImage<unsigned char> &_I, const vpRect &_rectangle)
{
unsigned int res = fern.buildReference(_I, _rectangle);
-
+
vpImagePoint iP = _rectangle.getTopLeft();
-
+
modelROI.x = (int)iP.get_u();
modelROI.y = (int)iP.get_v();
modelROI.width = (int)_rectangle.getWidth();
- modelROI.height = (int)_rectangle.getHeight();
+ modelROI.height = (int)_rectangle.getHeight();
+
+ initialiseRefCorners(modelROI);
- initialiseRefCorners(modelROI);
-
return res;
}
-
/*!
- Compute the points of interest in the current image and try to recognise them
- using the trained classifier. The matched pairs can be found with the
- getMatchedPointByRef function. The homography between the two planar
+ Compute the points of interest in the current image and try to recognise
+ them using the trained classifier. The matched pairs can be found with the
+ getMatchedPointByRef function. The homography between the two planar
surfaces is also computed.
-
+
\param I : The gray scaled image where the points are computed.
-
+
\return True if the surface has been found.
*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
+bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
{
fern.matchPoint(I);
-
+
/* compute homography */
std::vector<cv::Point2f> refPts = fern.getRefPt();
std::vector<cv::Point2f> curPts = fern.getCurPt();
-
- for(unsigned int i=0; i<refPts.size(); ++i){
+
+ for (unsigned int i = 0; i < refPts.size(); ++i) {
refPts[i].x += modelROI.x;
refPts[i].y += modelROI.y;
}
- for(unsigned int i=0; i<curPts.size(); ++i){
+ for (unsigned int i = 0; i < curPts.size(); ++i) {
curPts[i].x += modelROI.x;
curPts[i].y += modelROI.y;
}
-
- if(curPts.size() < 4){
- for (unsigned int i = 0; i < 3; i += 1){
- for (unsigned int j = 0; j < 3; j += 1){
- if(i == j){
+
+ if (curPts.size() < 4) {
+ for (unsigned int i = 0; i < 3; i += 1) {
+ for (unsigned int j = 0; j < 3; j += 1) {
+ if (i == j) {
homography[i][j] = 1;
- }
- else{
+ } else {
homography[i][j] = 0;
}
}
}
return false;
- }
+ }
/* part of code from OpenCV planarObjectDetector */
std::vector<unsigned char> mask;
H = cv::findHomography(cv::Mat(refPts), cv::Mat(curPts), mask, cv::RANSAC, 10);
-
- if( H.data )
- {
- const cv::Mat_<double>& H_tmp = H;
+
+ if (H.data) {
+ const cv::Mat_<double> &H_tmp = H;
dst_corners.resize(4);
- for(unsigned int i = 0; i < 4; i++ )
- {
- cv::Point2f pt = ref_corners[i];
+ for (unsigned int i = 0; i < 4; i++) {
+ cv::Point2f pt = ref_corners[i];
- double w = 1./(H_tmp(2,0)*pt.x + H_tmp(2,1)*pt.y + H_tmp(2,2));
- dst_corners[i] = cv::Point2f((float)((H_tmp(0,0)*pt.x + H_tmp(0,1)*pt.y + H_tmp(0,2))*w),
- (float)((H_tmp(1,0)*pt.x + H_tmp(1,1)*pt.y + H_tmp(1,2))*w));
+ double w = 1. / (H_tmp(2, 0) * pt.x + H_tmp(2, 1) * pt.y + H_tmp(2, 2));
+ dst_corners[i] = cv::Point2f((float)((H_tmp(0, 0) * pt.x + H_tmp(0, 1) * pt.y + H_tmp(0, 2)) * w),
+ (float)((H_tmp(1, 0) * pt.x + H_tmp(1, 1) * pt.y + H_tmp(1, 2)) * w));
}
-
- double* ptr = (double*)H_tmp.data;
- for(unsigned int i=0; i<9; i++){
- this->homography[(unsigned int)(i/3)][i%3] = *(ptr++);
+
+ double *ptr = (double *)H_tmp.data;
+ for (unsigned int i = 0; i < 9; i++) {
+ this->homography[(unsigned int)(i / 3)][i % 3] = *(ptr++);
}
isCorrect = true;
- }
- else{
+ } else {
isCorrect = false;
}
-
-
+
currentImagePoints.resize(0);
refImagePoints.resize(0);
- for (unsigned int i = 0; i < mask.size(); i += 1){
- if(mask[i] != 0){
+ for (unsigned int i = 0; i < mask.size(); i += 1) {
+ if (mask[i] != 0) {
vpImagePoint ip;
ip.set_i(curPts[i].y);
ip.set_j(curPts[i].x);
@@ -302,106 +280,88 @@ vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I)
refImagePoints.push_back(ip);
}
}
-
- if(currentImagePoints.size() < minNbMatching){
+
+ if (currentImagePoints.size() < minNbMatching) {
isCorrect = false;
}
return isCorrect;
}
-
-
/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs
+ can be found with the getMatchedPointByRef function. The homography between
+ the two planar surfaces is also computed.
+
\param I : The gray scaled image where the points are computed.
\param iP : the top left corner of the rectangle defining the ROI
\param height : the height of the ROI
\param width : the width of the ROI
-
+
\return true if the surface has been found.
*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I,
- const vpImagePoint &iP, const unsigned int height, const unsigned int width)
+bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpImagePoint &iP,
+ const unsigned int height, const unsigned int width)
{
- if((iP.get_i()+height) >= I.getHeight()
- || (iP.get_j()+width) >= I.getWidth()) {
+ 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<unsigned char> 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);
}
/*!
- Compute the points of interest in the specified region of the current image
- and try to recognise them using the trained classifier. The matched pairs can
- be found with the getMatchedPointByRef function. The homography between the
- two planar surfaces is also computed.
-
+ Compute the points of interest in the specified region of the current image
+ and try to recognise them using the trained classifier. The matched pairs
+ can be found with the getMatchedPointByRef function. The homography between
+ the two planar surfaces is also computed.
+
\param I : The gray scaled image where the points are computed.
\param rectangle : The rectangle defining the ROI.
-
+
\return True if the surface has been found.
*/
-bool
-vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect rectangle)
+bool vpPlanarObjectDetector::matchPoint(const vpImage<unsigned char> &I, const vpRect &rectangle)
{
vpImagePoint iP;
iP.set_i(rectangle.getTop());
iP.set_j(rectangle.getLeft());
- return (this->matchPoint(I, iP,
- (unsigned int)rectangle.getHeight(),
- (unsigned int)rectangle.getWidth()));
+ return (this->matchPoint(I, iP, (unsigned int)rectangle.getHeight(), (unsigned int)rectangle.getWidth()));
}
-
/*!
- Display the result of the matching. The plane is displayed in red and the
- points of interests detected in the image are shown by a red dot surrounded by
- a green circle (the radius of the circle depends on the octave at which it has
- been detected).
-
+ Display the result of the matching. The plane is displayed in red and the
+ points of interests detected in the image are shown by a red dot surrounded
+ by a green circle (the radius of the circle depends on the octave at which
+ it has been detected).
+
\param I : The gray scaled image for the display.
- \param displayKpts : The flag to display keypoints in addition to the surface.
+ \param displayKpts : The flag to display keypoints in addition to the
+ surface.
*/
-void
-vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
+void vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
{
- for(unsigned int i=0; i<dst_corners.size(); i++){
- vpImagePoint ip1(
- dst_corners[i].y - modelROI.y,
- dst_corners[i].x - modelROI.x);
- vpImagePoint ip2(
- dst_corners[(i+1)%dst_corners.size()].y - modelROI.y,
- dst_corners[(i+1)%dst_corners.size()].x - modelROI.x);
- vpDisplay::displayLine(I, ip1, ip2, vpColor::red) ;
+ for (unsigned int i = 0; i < dst_corners.size(); i++) {
+ vpImagePoint ip1(dst_corners[i].y - modelROI.y, dst_corners[i].x - modelROI.x);
+ vpImagePoint ip2(dst_corners[(i + 1) % dst_corners.size()].y - modelROI.y,
+ dst_corners[(i + 1) % dst_corners.size()].x - modelROI.x);
+ vpDisplay::displayLine(I, ip1, ip2, vpColor::red);
}
-
- if(displayKpts){
- for(unsigned int i=0; i<currentImagePoints.size(); ++i){
- vpImagePoint ip(
- currentImagePoints[i].get_i() - modelROI.y,
- currentImagePoints[i].get_j() - modelROI.x);
+
+ if (displayKpts) {
+ for (unsigned int i = 0; i < currentImagePoints.size(); ++i) {
+ vpImagePoint ip(currentImagePoints[i].get_i() - modelROI.y, currentImagePoints[i].get_j() - modelROI.x);
vpDisplay::displayCross(I, ip, 5, vpColor::red);
}
}
}
-
/*!
This function displays the matched reference points and the matched
points computed in the current image. The reference points are
@@ -416,68 +376,61 @@ vpPlanarObjectDetector::display(vpImage<unsigned char> &I, bool displayKpts)
\param Icurrent : The image where the matched points computed in the
current image are displayed.
- \param displayKpts : The flag to display keypoints in addition to the surface.
+ \param displayKpts : The flag to display keypoints in addition to the
+ surface.
*/
-void
-vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref,
- vpImage<unsigned char> &Icurrent, bool displayKpts)
+void vpPlanarObjectDetector::display(vpImage<unsigned char> &Iref, vpImage<unsigned char> &Icurrent, bool displayKpts)
{
display(Icurrent, displayKpts);
- if(displayKpts){
- for(unsigned int i=0; i<refImagePoints.size(); ++i){
+ if (displayKpts) {
+ for (unsigned int i = 0; i < refImagePoints.size(); ++i) {
vpDisplay::displayCross(Iref, refImagePoints[i], 5, vpColor::green);
}
}
}
-
/*!
\brief Load the Fern classifier.
-
- Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
+
+ Load and initialize the Fern classifier and load the 3D and 2D keypoints. It
can take up to sevral seconds.
-
- \param dataFilename : The name of the data filename (very large text file).
+
+ \param dataFilename : The name of the data filename (very large text file).
It can have any file extension.
\param objName : The name of the object.
*/
-void
-vpPlanarObjectDetector::load(const std::string& dataFilename, const std::string& objName)
+void vpPlanarObjectDetector::load(const std::string &dataFilename, const std::string &objName)
{
fern.load(dataFilename, objName);
modelROI = fern.getModelROI();
initialiseRefCorners(modelROI);
}
-
/*!
\brief Record the Ferns classifier in the text file.
\param objectName : The name of the object to store in the data file.
- \param dataFile : The name of the data filename (very large text file).
+ \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);
}
-
-
/*!
Return the last positions of the detected corners.
-
+
\return The vectors of corners' postions.
*/
-std::vector<vpImagePoint>
-vpPlanarObjectDetector::getDetectedCorners() const{
+std::vector<vpImagePoint> vpPlanarObjectDetector::getDetectedCorners() const
+{
vpImagePoint ip;
- std::vector <vpImagePoint> corners;
+ std::vector<vpImagePoint> corners;
corners.clear();
- for(unsigned int i=0; i<dst_corners.size(); i++){
- ip.set_uv( dst_corners[i].x, dst_corners[i].y);
+ for (unsigned int i = 0; i < dst_corners.size(); i++) {
+ ip.set_uv(dst_corners[i].x, dst_corners[i].y);
corners.push_back(ip);
}
@@ -486,46 +439,43 @@ vpPlanarObjectDetector::getDetectedCorners() const{
/*!
Initialise the internal reference corners from the rectangle.
-
+
\param _modelROI : The rectangle defining the region of interest.
*/
-void
-vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect& _modelROI)
+void vpPlanarObjectDetector::initialiseRefCorners(const cv::Rect &_modelROI)
{
cv::Point2f ip;
-
- ip.y = (float)_modelROI.y;
+
+ ip.y = (float)_modelROI.y;
ip.x = (float)_modelROI.x;
ref_corners.push_back(ip);
-
- ip.y = (float)(_modelROI.y+_modelROI.height);
- ip.x = (float)_modelROI.x;
+
+ ip.y = (float)(_modelROI.y + _modelROI.height);
+ ip.x = (float)_modelROI.x;
ref_corners.push_back(ip);
-
- ip.y = (float)(_modelROI.y+_modelROI.height);
- ip.x = (float)(_modelROI.x+_modelROI.width);
- ref_corners.push_back(ip);
-
- ip.y = (float)_modelROI.y;
- ip.x = (float)(_modelROI.x+_modelROI.width);
+
+ ip.y = (float)(_modelROI.y + _modelROI.height);
+ ip.x = (float)(_modelROI.x + _modelROI.width);
ref_corners.push_back(ip);
-}
+ ip.y = (float)_modelROI.y;
+ ip.x = (float)(_modelROI.x + _modelROI.width);
+ ref_corners.push_back(ip);
+}
-void
-vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint& _imPoint)
+void vpPlanarObjectDetector::getReferencePoint(unsigned int _i, vpImagePoint &_imPoint)
{
- if(_i >= refImagePoints.size()){
+ if (_i >= refImagePoints.size()) {
throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
}
_imPoint = refImagePoints[_i];
}
-void
-vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint& _referencePoint, vpImagePoint& _currentPoint)
+void vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint &_referencePoint,
+ vpImagePoint &_currentPoint)
{
-// fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
- if(_index >= currentImagePoints.size()){
+ // fern.getMatchedPoints(_index, _referencePoint, _currentPoint);
+ if (_index >= currentImagePoints.size()) {
throw vpException(vpException::fatalError, "index out of bound in getMatchedPoints.");
}
@@ -534,7 +484,7 @@ vpPlanarObjectDetector::getMatchedPoints(const unsigned int _index, vpImagePoint
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
-void dummy_vpPlanarObjectDetector() {};
+// Work arround to avoid warning:
+// libvisp_vision.a(vpPlanarObjectDetector.cpp.o) has no symbols
+void dummy_vpPlanarObjectDetector(){};
#endif
-
diff --git a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp
index 44f0590..b2bfb0c 100644
--- a/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp
+++ b/modules/vision/src/key-point/vpXmlConfigParserKeyPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,7 +39,8 @@
/*!
\file vpXmlConfigParserKeyPoint.cpp
\brief Definition of the vpXmlConfigParserKeyPoint class member functions.
- Class vpXmlConfigParserKeyPoint permits to load configuration defined in a XML file for vpKeyPoint class.
+ Class vpXmlConfigParserKeyPoint permits to load configuration defined in a
+ XML file for vpKeyPoint class.
*/
@@ -48,11 +50,11 @@
#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)
+vpXmlConfigParserKeyPoint::vpXmlConfigParserKeyPoint()
+ : m_detectorName("ORB"), m_extractorName("ORB"), m_matcherName("BruteForce-Hamming"), m_matchingFactorThreshold(2.0),
+ m_matchingMethod(ratioDistanceThreshold), m_matchingRatioThreshold(0.85), m_nbRansacIterations(200),
+ m_nbRansacMinInlierCount(100), m_ransacConsensusPercentage(20.0), m_ransacReprojectionError(6.0),
+ m_ransacThreshold(0.01), m_useRansacConsensusPercentage(false), m_useRansacVVS(true)
{
init();
}
@@ -60,8 +62,7 @@ vpXmlConfigParserKeyPoint::vpXmlConfigParserKeyPoint() : m_detectorName("ORB"),
/*!
Initialize the nodeMap for the node parsing.
*/
-void
-vpXmlConfigParserKeyPoint::init()
+void vpXmlConfigParserKeyPoint::init()
{
setMainTag("conf");
@@ -92,11 +93,7 @@ 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
@@ -105,18 +102,17 @@ vpXmlConfigParserKeyPoint::parse(const std::string &filename)
\param doc : Document to parse.
\param node : Root node.
*/
-void
-vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
+void vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
{
bool detector_node = false;
bool extractor_node = false;
bool matcher_node = false;
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE){
- std::map<std::string, int>::iterator iter_data= this->nodeMap.find((char*)dataNode->name);
- if(iter_data != nodeMap.end()){
- switch (iter_data->second){
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
case detector:
this->read_detector(doc, dataNode);
detector_node = true;
@@ -143,16 +139,16 @@ vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
}
}
- if(!detector_node) {
- std::cout << "detector: name: "<< m_detectorName << " (default)" << std::endl;
+ if (!detector_node) {
+ std::cout << "detector: name: " << m_detectorName << " (default)" << std::endl;
}
- if(!extractor_node) {
- std::cout << "extractor: name: "<< m_extractorName << " (default)" << std::endl;
+ if (!extractor_node) {
+ std::cout << "extractor: name: " << m_extractorName << " (default)" << std::endl;
}
- if(!matcher_node) {
- std::cout << "matcher: name: "<< m_matcherName << " (default)" << std::endl;
+ if (!matcher_node) {
+ std::cout << "matcher: name: " << m_matcherName << " (default)" << std::endl;
}
}
@@ -162,16 +158,15 @@ vpXmlConfigParserKeyPoint::readMainClass(xmlDocPtr doc, xmlNodePtr node)
\param doc : Document to parse.
\param node : Detector node.
*/
-void
-vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
+void vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
{
bool detector_name_node = false;
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
if (iter_data != nodeMap.end()) {
- switch(iter_data->second) {
+ switch (iter_data->second) {
case name:
m_detectorName = xmlReadStringChild(doc, dataNode);
detector_name_node = true;
@@ -184,10 +179,10 @@ vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
}
}
- if(!detector_name_node)
- std::cout << "detector : Name : "<< m_detectorName << " (default)" << std::endl;
+ if (!detector_name_node)
+ std::cout << "detector : Name : " << m_detectorName << " (default)" << std::endl;
else
- std::cout << "detector : Name : "<< m_detectorName << std::endl;
+ std::cout << "detector : Name : " << m_detectorName << std::endl;
}
/*!
@@ -196,16 +191,15 @@ vpXmlConfigParserKeyPoint::read_detector(xmlDocPtr doc, xmlNodePtr node)
\param doc : Document to parse.
\param node : Extractor node.
*/
-void
-vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
+void vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
{
bool extractor_name_node = false;
- for(xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
if (iter_data != nodeMap.end()) {
- switch(iter_data->second) {
+ switch (iter_data->second) {
case name:
m_extractorName = xmlReadStringChild(doc, dataNode);
extractor_name_node = true;
@@ -218,10 +212,10 @@ vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
}
}
- if(!extractor_name_node)
- std::cout << "extractor : Name : "<< m_extractorName << " (default)" << std::endl;
+ if (!extractor_name_node)
+ std::cout << "extractor : Name : " << m_extractorName << " (default)" << std::endl;
else
- std::cout << "extractor : Name : "<< m_extractorName << std::endl;
+ std::cout << "extractor : Name : " << m_extractorName << std::endl;
}
/*!
@@ -230,8 +224,7 @@ vpXmlConfigParserKeyPoint::read_extractor(xmlDocPtr doc, xmlNodePtr node)
\param doc : Document to parse.
\param node : Matcher node.
*/
-void
-vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
+void vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
{
bool matcher_name_node = false;
bool matching_method_node = false;
@@ -239,24 +232,23 @@ 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) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if(iter_data != nodeMap.end()) {
- switch(iter_data->second) {
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
case name:
m_matcherName = xmlReadStringChild(doc, dataNode);
matcher_name_node = true;
break;
- case matching_method:
- {
+ case matching_method: {
matchingMethodName = xmlReadStringChild(doc, dataNode);
std::map<std::string, int>::iterator iter_data2 = nodeMap.find(matchingMethodName);
- if(iter_data2 != nodeMap.end()) {
+ if (iter_data2 != nodeMap.end()) {
matching_method_node = true;
- switch(iter_data2->second) {
+ switch (iter_data2->second) {
case constant_factor_distance_threshold:
m_matchingMethod = constantFactorDistanceThreshold;
break;
@@ -302,25 +294,25 @@ vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
}
}
- if(!matcher_name_node)
- std::cout << "matcher : Name : "<< m_matcherName << " (default)" << std::endl;
+ if (!matcher_name_node)
+ std::cout << "matcher : Name : " << m_matcherName << " (default)" << std::endl;
else
- std::cout << "matcher : Name : "<< m_matcherName <<std::endl;
+ std::cout << "matcher : Name : " << m_matcherName << std::endl;
- if(!matching_method_node)
- std::cout << "matcher : Filter method : "<< matchingMethodName << " (default)" << std::endl;
+ if (!matching_method_node)
+ 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;
+ if (!matching_factor_threshold_node)
+ std::cout << "matcher : matching factor threshold : " << m_matchingFactorThreshold << " (default)" << std::endl;
else
- std::cout << "matcher : matching factor threshold : "<< m_matchingFactorThreshold << std::endl;
+ 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;
+ if (!matching_ratio_threshold_node)
+ std::cout << "matcher : matching ratio threshold : " << m_matchingRatioThreshold << " (default)" << std::endl;
else
- std::cout << "matcher : matching ratio threshold : "<< m_matchingRatioThreshold << std::endl;
+ std::cout << "matcher : matching ratio threshold : " << m_matchingRatioThreshold << std::endl;
}
/*!
@@ -329,8 +321,7 @@ vpXmlConfigParserKeyPoint::read_matcher(xmlDocPtr doc, xmlNodePtr node)
\param doc : Document to parse.
\param node : Ransac node.
*/
-void
-vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node)
+void vpXmlConfigParserKeyPoint::read_ransac(xmlDocPtr doc, xmlNodePtr node)
{
bool use_ransac_vvs_node = false;
bool use_ransac_consensus_percentage_node = false;
@@ -340,11 +331,11 @@ 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) {
- if(dataNode->type == XML_ELEMENT_NODE) {
- std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char*) dataNode->name);
- if(iter_data != nodeMap.end()) {
- switch(iter_data->second) {
+ for (xmlNodePtr dataNode = node->xmlChildrenNode; dataNode != NULL; dataNode = dataNode->next) {
+ if (dataNode->type == XML_ELEMENT_NODE) {
+ std::map<std::string, int>::iterator iter_data = this->nodeMap.find((char *)dataNode->name);
+ if (iter_data != nodeMap.end()) {
+ switch (iter_data->second) {
case use_ransac_vvs:
m_useRansacVVS = xmlReadIntChild(doc, dataNode) != 0;
use_ransac_vvs_node = true;
@@ -387,43 +378,49 @@ 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;
+ if (!use_ransac_vvs_node)
+ std::cout << "ransac: use ransac vvs pose estimation: " << m_useRansacVVS << " (default)" << std::endl;
else
- std::cout << "ransac: use ransac vvs pose estimation: "<< m_useRansacVVS <<std::endl;
+ 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;
+ if (!use_ransac_consensus_percentage_node)
+ std::cout << "ransac: use consensus percentage: " << m_useRansacConsensusPercentage << " (default)" << std::endl;
else
- std::cout << "ransac: use consensus percentage: "<< m_useRansacConsensusPercentage <<std::endl;
+ 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;
+ if (!nb_ransac_iterations_node)
+ std::cout << "ransac: nb ransac iterations: " << m_nbRansacIterations << " (default)" << std::endl;
else
- std::cout << "ransac: nb ransac iterations: "<< m_nbRansacIterations <<std::endl;
+ std::cout << "ransac: nb ransac iterations: " << m_nbRansacIterations << std::endl;
- if(!ransac_reprojection_error_node)
- std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError << " (default)" << std::endl;
+ if (!ransac_reprojection_error_node)
+ std::cout << "ransac: ransac reprojection error in pixel (for OpenCV "
+ "function): "
+ << m_ransacReprojectionError << " (default)" << std::endl;
else
- std::cout << "ransac: ransac reprojection error in pixel (for OpenCV function): "<< m_ransacReprojectionError <<std::endl;
+ std::cout << "ransac: ransac reprojection error in pixel (for OpenCV "
+ "function): "
+ << m_ransacReprojectionError << std::endl;
- if(!nb_ransac_min_inlier_count_node)
- std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount << " (default)" << std::endl;
+ if (!nb_ransac_min_inlier_count_node)
+ std::cout << "ransac: nb ransac min inlier count: " << m_nbRansacMinInlierCount << " (default)" << std::endl;
else
- std::cout << "ransac: nb ransac min inlier count: "<< m_nbRansacMinInlierCount <<std::endl;
+ 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;
+ if (!ransac_threshold_node)
+ std::cout << "ransac: ransac threshold in meter (for ViSP function): " << m_ransacThreshold << " (default)"
+ << std::endl;
else
- std::cout << "ransac: ransac threshold in meter (for ViSP function): "<< m_ransacThreshold <<std::endl;
+ 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;
+ if (!ransac_consensus_percentage_node)
+ std::cout << "ransac: consensus percentage: " << m_ransacConsensusPercentage << " (default)" << std::endl;
else
- std::cout << "ransac: consensus percentage: "<< m_ransacConsensusPercentage <<std::endl;
+ std::cout << "ransac: consensus percentage: " << m_ransacConsensusPercentage << std::endl;
}
#elif !defined(VISP_BUILD_SHARED_LIBS)
-// Work arround to avoid warning: libvisp_vision.a(vpXmlConfigParserKeyPoint.cpp.o) has no symbols
-void dummy_vpXmlConfigParserKeyPoint() {};
-#endif //VISP_HAVE_XML2
+// Work arround to avoid warning:
+// libvisp_vision.a(vpXmlConfigParserKeyPoint.cpp.o) has no symbols
+void dummy_vpXmlConfigParserKeyPoint(){};
+#endif // VISP_HAVE_XML2
diff --git a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp
index 6f81ac6..2a28d72 100644
--- a/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp
+++ b/modules/vision/src/pose-estimation/vpLevenbergMarquartd.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,19 +37,24 @@
*
*****************************************************************************/
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <algorithm> // (std::min)
+#include <cmath> // std::fabs
#include <iostream>
-#include <algorithm> // std::min
+#include <limits> // numeric_limits
-#include <visp3/vision/vpLevenbergMarquartd.h>
#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpLevenbergMarquartd.h>
-#define SIGN(x) ((x) < 0 ? -1 : 1)
-#define SWAP(a,b,c) {(c) = (a); (a) = (b); (b) = (c);}
-#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
-#define TRUE 1
-#define FALSE 0
+#define SIGN(x) ((x) < 0 ? -1 : 1)
+#define SWAP(a, b, c) \
+ { \
+ (c) = (a); \
+ (a) = (b); \
+ (b) = (c); \
+ }
+#define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
+#define TRUE 1
+#define FALSE 0
/*
* PROCEDURE : enorm
@@ -77,62 +83,52 @@
* initialisee pour indiquee le type de l'erreur.
*
*/
-double enorm (const double *x, int n)
+double enorm(const double *x, int n)
{
- const double rdwarf = 3.834e-20;
- const double rgiant = 1.304e19;
+ const double rdwarf = 3.834e-20;
+ const double rgiant = 1.304e19;
- int i;
- double agiant, floatn;
- double norm_eucl = 0.0;
- double s1 = 0.0, s2 = 0.0, s3 = 0.0;
- double x1max = 0.0 , x3max = 0.0;
+ int i;
+ double agiant, floatn;
+ double norm_eucl = 0.0;
+ double s1 = 0.0, s2 = 0.0, s3 = 0.0;
+ double x1max = 0.0, x3max = 0.0;
- floatn = (double) n;
+ floatn = (double)n;
agiant = rgiant / floatn;
- for (i = 0; i < n; i++)
- {
+ for (i = 0; i < n; i++) {
double xabs = std::fabs(x[i]);
- if ((xabs > rdwarf) && (xabs < agiant))
- {
+ if ((xabs > rdwarf) && (xabs < agiant)) {
/*
* somme pour elements intemediaires.
*/
s2 += xabs * xabs;
}
- else if (xabs <= rdwarf)
- {
+ else if (xabs <= rdwarf) {
/*
* somme pour elements petits.
*/
- if (xabs <= x3max)
- {
- //if (xabs != 0.0)
- if (xabs > std::numeric_limits<double>::epsilon())
- s3 += (xabs / x3max) * (xabs / x3max);
- }
- else
- {
- s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
- x3max = xabs;
+ if (xabs <= x3max) {
+ // if (xabs != 0.0)
+ if (xabs > std::numeric_limits<double>::epsilon())
+ s3 += (xabs / x3max) * (xabs / x3max);
+ } else {
+ s3 = 1.0 + s3 * (x3max / xabs) * (x3max / xabs);
+ x3max = xabs;
}
}
- else
- {
+ else {
/*
* somme pour elements grand.
*/
- if (xabs <= x1max)
- {
- s1 += (xabs / x1max) * (xabs / x1max);
- }
- else
- {
- s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
- x1max = xabs;
+ if (xabs <= x1max) {
+ s1 += (xabs / x1max) * (xabs / x1max);
+ } else {
+ s1 = 1.0 + s1 * (x1max / xabs) * (x1max / xabs);
+ x1max = xabs;
}
}
}
@@ -140,19 +136,17 @@ double enorm (const double *x, int n)
/*
* calcul de la norme.
*/
- //if (s1 == 0.0)
- if (std::fabs(s1) <= std::numeric_limits<double>::epsilon())
- {
- //if (s2 == 0.0)
+ // if (s1 == 0.0)
+ if (std::fabs(s1) <= std::numeric_limits<double>::epsilon()) {
+ // if (s2 == 0.0)
if (std::fabs(s2) <= std::numeric_limits<double>::epsilon())
norm_eucl = x3max * sqrt(s3);
else if (s2 >= x3max)
- norm_eucl = sqrt (s2 * (1.0 + ( x3max / s2) * (x3max * s3)));
- else if (s2 < x3max)
- norm_eucl = sqrt (x3max * ((s2 / x3max) + (x3max * s3)));
- }
- else
- norm_eucl = x1max * sqrt (s1 + (s2 / x1max) / x1max);
+ norm_eucl = sqrt(s2 * (1.0 + (x3max / s2) * (x3max * s3)));
+ else /*if (s2 < x3max)*/
+ norm_eucl = sqrt(x3max * ((s2 / x3max) + (x3max * s3)));
+ } else
+ norm_eucl = x1max * sqrt(s1 + (s2 / x1max) / x1max);
return (norm_eucl);
}
@@ -162,15 +156,14 @@ double enorm (const double *x, int n)
* ENTREE :
* n Ordre de la matrice "r".
* r Matrice de taille "n" x "n". En entree, la toute la partie
- * triangulaire superieure doit contenir toute la partie triangulaire
- * superieure de "r".
+ * triangulaire superieure doit contenir toute la partie
+ *triangulaire superieure de "r".
*
* ldr Taille maximum de la matrice "r". "ldr" >= "n".
*
- * ipvt Vecteur de taille "n" qui definit la matrice de permutation "p"
- * tel que :
- * a * p = q * r.
- * La jeme colonne de p la colonne ipvt[j] de la matrice d'identite.
+ * ipvt Vecteur de taille "n" qui definit la matrice de permutation
+ *"p" tel que : a * p = q * r. La jeme colonne de p la colonne ipvt[j] de la
+ *matrice d'identite.
*
* diag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice "d".
@@ -184,10 +177,11 @@ double enorm (const double *x, int n)
* wa1, wa2 Vecteurs de taille "n" de travail.
*
* DESCRIPTION :
- * La procedure determine le parametre de Levenberg-Marquardt. Soit une matrice
- * "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de taille
- * "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta, le probleme
- * est le calcul du parametre "par" de telle facon que si "x" resoud le systeme
+ * La procedure determine le parametre de Levenberg-Marquardt. Soit une
+ *matrice "a" de taille "m" x "n", une matrice diagonale "d" non singuliere de
+ *taille "n" x "n", un vecteur "b" de taille "m" et un nombre positf delta,
+ *le probleme est le calcul du parametre "par" de telle facon que si "x"
+ *resoud le systeme
*
* a * x = b , sqrt(par) * d * x = 0 ,
*
@@ -198,10 +192,11 @@ double enorm (const double *x, int n)
* nessaires de la factorisation qr, avec pivotage de colonnes de a.
* Donc, si a * p = q * r, ou "p" est une matrice de permutation, les colonnes
* de "q" sont orthogonales, et "r" est une matrice triangulaire superieure
- * avec les elements diagonaux classes par ordre decroissant de leur valeur, lmpar
- * attend une matrice triangulaire superieure complete, la matrice de permutation
- * "p" et les "n" premiers elements de (q transpose) * b. En sortie, la procedure
- * lmpar fournit aussi une matrice triangulaire superieure "s" telle que
+ * avec les elements diagonaux classes par ordre decroissant de leur valeur,
+ *lmpar attend une matrice triangulaire superieure complete, la matrice de
+ *permutation "p" et les "n" premiers elements de (q transpose) * b. En
+ *sortie, la procedure lmpar fournit aussi une matrice triangulaire
+ *superieure "s" telle que
*
* t t t
* p * (a * a + par * d * d )* p = s * s .
@@ -218,8 +213,8 @@ double enorm (const double *x, int n)
* triangulaire superieure (transpose) de la matrice triangulaire
* superieure de "s".
* par Estimee finale du parametre de Levenberg-Marquardt.
- * x Vecteur de taille "n" contenant la solution au sens des moindres
- * carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le
+ * x Vecteur de taille "n" contenant la solution au sens des
+ *moindres carres du systeme a * x = b, sqrt(par) * d * x = 0, pour le
* parametre en sortie "par"
* sdiag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice triangulaire "s".
@@ -228,17 +223,17 @@ 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 */
+ const double tol1 = 0.1; /* tolerance a 0.1 */
- int l;
- unsigned int iter; /* compteur d'iteration */
- int nsing; /* nombre de singularite de la matrice */
- double dxnorm, fp;
- double temp;
- double dwarf = DBL_MIN; /* plus petite amplitude positive */
+ int l;
+ unsigned int iter; /* compteur d'iteration */
+ int nsing; /* nombre de singularite de la matrice */
+ double dxnorm, fp;
+ double temp;
+ double dwarf = DBL_MIN; /* plus petite amplitude positive */
/*
* calcul et stockage dans "x" de la direction de Gauss-Newton. Si
@@ -247,11 +242,10 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
*/
nsing = n;
- for (int i = 0; i < n; i++)
- {
+ for (int i = 0; i < n; i++) {
wa1[i] = qtb[i];
double *pt = MIJ(r, i, i, ldr);
- //if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
+ // if (*MIJ(r, i, i, ldr) == 0.0 && nsing == n)
if (std::fabs(*pt) <= std::numeric_limits<double>::epsilon() && nsing == n)
nsing = i - 1;
@@ -259,25 +253,21 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
wa1[i] = 0.0;
}
- if (nsing >= 0)
- {
- for (int k = 0; k < nsing; k++)
- {
+ if (nsing >= 0) {
+ for (int k = 0; k < nsing; k++) {
int i = nsing - 1 - k;
wa1[i] /= *MIJ(r, i, i, ldr);
temp = wa1[i];
int jm1 = i - 1;
- if (jm1 >= 0)
- {
+ if (jm1 >= 0) {
for (unsigned int j = 0; j <= (unsigned int)jm1; j++)
wa1[j] -= *MIJ(r, i, j, ldr) * temp;
}
}
}
- for (int j = 0; j < n; j++)
- {
+ for (int j = 0; j < n; j++) {
l = ipvt[j];
x[l] = wa1[j];
}
@@ -296,8 +286,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
fp = dxnorm - *delta;
- if (fp > tol1 * (*delta))
- {
+ if (fp > tol1 * (*delta)) {
/*
* Si le jacobien n'a pas de rangee deficiente,l'etape de
* Newton fournit une limite inferieure, parl pour le
@@ -305,22 +294,18 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
*/
double parl = 0.0;
- if (nsing >= n)
- {
- for (int i = 0; i < n; i++)
- {
+ if (nsing >= n) {
+ for (int i = 0; i < n; i++) {
l = ipvt[i];
wa1[i] = diag[l] * (wa2[l] / dxnorm);
}
- for (int i = 0; i < n; i++)
- {
- long im1;
+ for (int i = 0; i < n; i++) {
+ long im1;
double sum = 0.0;
im1 = (i - 1L);
- if (im1 >= 0)
- {
+ if (im1 >= 0) {
for (unsigned int j = 0; j <= (unsigned int)im1; j++)
sum += (*MIJ(r, i, j, ldr) * wa1[j]);
}
@@ -335,8 +320,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* calcul d'une limite superieure, paru, pour le zero de la
* fonction.
*/
- for (int i = 0; i < n; i++)
- {
+ for (int i = 0; i < n; i++) {
double sum = 0.0;
for (int j = 0; j <= i; j++)
@@ -349,7 +333,7 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
double gnorm = enorm(wa1, n);
double paru = gnorm / *delta;
- //if (paru == 0.0)
+ // if (paru == 0.0)
if (std::fabs(paru) <= std::numeric_limits<double>::epsilon())
paru = dwarf / vpMath::minimum(*delta, tol1);
@@ -361,14 +345,14 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
*par = vpMath::maximum(*par, parl);
*par = vpMath::maximum(*par, paru);
- //if (*par == 0.0)
- if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() )
+ // if (*par == 0.0)
+ if (std::fabs(*par) <= std::numeric_limits<double>::epsilon())
*par = gnorm / dxnorm;
/*
* debut d'une iteration.
*/
- for(;;) //iter >= 0)
+ for (;;) // iter >= 0)
{
iter++;
@@ -376,9 +360,9 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* evaluation de la fonction a la valeur courant
* de "par".
*/
- //if (*par == 0.0)
- if (std::fabs(*par) <= std::numeric_limits<double>::epsilon() ) {
- const double tol001 = 0.001; /* tolerance a 0.001 */
+ // if (*par == 0.0)
+ if (std::fabs(*par) <= std::numeric_limits<double>::epsilon()) {
+ const double tol001 = 0.001; /* tolerance a 0.001 */
*par = vpMath::maximum(dwarf, (tol001 * paru));
}
@@ -402,16 +386,17 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* ou parl est nul et ou le nombre d'iteration a
* atteint 10.
*/
- //if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <= temp)
+ // if ((std::fabs(fp) <= tol1 * (*delta)) || ((parl == 0.0) && (fp <=
+ // temp)
// && (temp < 0.0)) || (iter == 10))
- if ((std::fabs(fp) <= tol1 * (*delta)) || ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp)
- && (temp < 0.0)) || (iter == 10))
- {
+ if ((std::fabs(fp) <= tol1 * (*delta)) ||
+ ((std::fabs(parl) <= std::numeric_limits<double>::epsilon()) && (fp <= temp) && (temp < 0.0)) ||
+ (iter == 10)) {
// terminaison.
// Remove the two next lines since this is a dead code
/* if (iter == 0)
- *par = 0.0; */
+ *par = 0.0; */
return (0);
}
@@ -420,20 +405,17 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* calcul de la correction de Newton.
*/
- for (int i = 0; i < n; i++)
- {
+ for (int i = 0; i < n; i++) {
l = ipvt[i];
wa1[i] = diag[l] * (wa2[l] / dxnorm);
}
- for (unsigned int i = 0; i < (unsigned int)n; i++)
- {
+ for (unsigned int i = 0; i < (unsigned int)n; i++) {
wa1[i] = wa1[i] / sdiag[i];
temp = wa1[i];
unsigned int jp1 = i + 1;
- if ( (unsigned int) n >= jp1)
- {
- for (unsigned int j = jp1; j < (unsigned int)n; j++)
+ if ((unsigned int)n >= jp1) {
+ for (unsigned int j = jp1; j < (unsigned int)n; j++)
wa1[j] -= (*MIJ(r, i, j, ldr) * temp);
}
}
@@ -455,8 +437,8 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* calcul d'une estimee ameliree de "par".
*/
*par = vpMath::maximum(parl, (*par + parc));
- }/* fin boucle sur iter */
- }/* fin fp > tol1 * delta */
+ } /* fin boucle sur iter */
+ } /* fin fp > tol1 * delta */
/*
* terminaison.
@@ -481,25 +463,23 @@ int lmpar(int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* La procedure retourne la racine carre de a^2 + b^2.
*
*/
-double pythag (double a, double b)
+double pythag(double a, double b)
{
- double pyth, p, r, t;
+ double pyth, p, r, t;
p = vpMath::maximum(std::fabs(a), std::fabs(b));
- //if (p == 0.0)
- if (std::fabs(p) <= std::numeric_limits<double>::epsilon() )
- {
+ // if (p == 0.0)
+ if (std::fabs(p) <= std::numeric_limits<double>::epsilon()) {
pyth = p;
return (pyth);
}
- r = (std::min(std::fabs(a), std::fabs(b)) / p) * (std::min(std::fabs(a), std::fabs(b)) / p);
+ r = ((std::min)(std::fabs(a), std::fabs(b)) / p) * ((std::min)(std::fabs(a), std::fabs(b)) / p);
t = 4.0 + r;
- //while (t != 4.0)
- while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t,4.0)) * std::numeric_limits<double>::epsilon() )
- {
+ // while (t != 4.0)
+ while (std::fabs(t - 4.0) < std::fabs(vpMath::maximum(t, 4.0)) * std::numeric_limits<double>::epsilon()) {
double s = r / t;
double u = 1.0 + 2.0 * s;
p *= u;
@@ -517,11 +497,11 @@ double pythag (double a, double b)
* ENTREE :
* m Nombre de lignes de la matrice "a".
* n Nombre de colonne de la matrice "a".
- * a Matrice de taille "m" x "n". elle contient, en entree la matrice
- * dont on veut sa factorisation qr.
+ * a Matrice de taille "m" x "n". elle contient, en entree la
+ *matrice dont on veut sa factorisation qr.
* lda Taille maximale de "a". lda >= m.
- * pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est realise
- * Si pivot = FALSE, pas de pivotage.
+ * pivot Booleen. Si pivot est TRUE, le pivotage de colonnes est
+ *realise Si pivot = FALSE, pas de pivotage.
* lipvt Taille du vecteur "ipvt". Si pivot est FALSE, lipvt est de
* l'ordre de 1. Sinon lipvt est de l'ordre de "n".
* wa Vecteur de travail de taille "n". Si pivot = FALSE "wa"
@@ -529,13 +509,13 @@ double pythag (double a, double b)
*
* DESCRIPTION :
* La procedure effectue une decomposition de la matrice "a"par la methode qr.
- * Elle utilise les transformations de householders avec pivotage sur les colonnes
- * (option) pour calculer la factorisation qr de la matrice "a" de taille "m" x "n".
- * La procedure determine une matrice orthogonale "q", une matrice de permutation
- * "p" et une matrice trapesoidale superieure "r" dont les elements diagonaux
- * sont ordonnes dans l'ordre decroissant de leurs valeurs,tel que a * p = q * r.
- * La transformation de householder pour la colonne k, k = 1,2,...,min(m,n),
- * est de la forme
+ * Elle utilise les transformations de householders avec pivotage sur les
+ *colonnes (option) pour calculer la factorisation qr de la matrice "a" de
+ *taille "m" x "n". La procedure determine une matrice orthogonale "q", une
+ *matrice de permutation "p" et une matrice trapesoidale superieure "r" dont
+ *les elements diagonaux sont ordonnes dans l'ordre decroissant de leurs
+ *valeurs,tel que a * p = q * r. La transformation de householder pour la
+ *colonne k, k = 1,2,...,min(m,n), est de la forme
* t
* i - (1 / u(k)) * u * u
*
@@ -547,25 +527,23 @@ double pythag (double a, double b)
* trapezoidale inferieure de "a" contient une forme factorisee
* de "q" (les elements non triviaux du vecteurs "u" sont decrits
* ci-dessus).
- * ipvt Vecteur de taille "n". Il definit la matrice de permutation "p"
- * tel que a * p = q * r. La jeme colonne de p est la colonne
- * ipvt[j] de la matrice d'identite. Si pivot = FALSE, ipvt n'est
- * pas referencee.
- * rdiag Vecteur de taille "n" contenant les elements diagonaux de la
- * matrice "r".
- * acnorm Vecteur de taille "n" contenant les normes des lignes
+ * ipvt Vecteur de taille "n". Il definit la matrice de permutation
+ *"p" tel que a * p = q * r. La jeme colonne de p est la colonne ipvt[j] de la
+ *matrice d'identite. Si pivot = FALSE, ipvt n'est pas referencee. rdiag
+ *Vecteur de taille "n" contenant les elements diagonaux de la matrice
+ *"r". acnorm Vecteur de taille "n" contenant les normes des lignes
* correspondantes de la matrice "a". Si cette information n'est
* pas requise, acnorm coincide avec rdiag.
*
*/
-int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
- int /* lipvt */, double *rdiag, double *acnorm, double *wa)
+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;
+ const double tolerance = 0.05;
- int i, j, ip1, k, kmax, minmn;
- double epsmch;
- double sum, temp, tmp;
+ int i, j, ip1, k, kmax, minmn;
+ double epsmch;
+ double sum, temp, tmp;
/*
* epsmch est la precision machine.
@@ -576,8 +554,7 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* calcul des normes initiales des lignes et initialisation
* de plusieurs tableaux.
*/
- for (i = 0; i < m; i++)
- {
+ for (i = 0; i < m; i++) {
acnorm[i] = enorm(MIJ(a, i, 0, lda), n);
rdiag[i] = acnorm[i];
wa[i] = rdiag[i];
@@ -589,31 +566,26 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* reduction de "a" en "r" avec les tranformations de Householder.
*/
minmn = vpMath::minimum(m, n);
- for (i = 0; i < minmn; i++)
- {
- if (pivot)
- {
+ for (i = 0; i < minmn; i++) {
+ if (pivot) {
/*
* met la ligne de plus grande norme en position
* de pivot.
*/
kmax = i;
- for (k = i; k < m; k++)
- {
+ for (k = i; k < m; k++) {
if (rdiag[k] > rdiag[kmax])
kmax = k;
}
- if (kmax != i)
- {
+ if (kmax != i) {
for (j = 0; j < n; j++)
- SWAP(*MIJ(a, i, j, lda),
- *MIJ(a, kmax, j, lda), tmp);
+ SWAP(*MIJ(a, i, j, lda), *MIJ(a, kmax, j, lda), tmp);
rdiag[kmax] = rdiag[i];
wa[kmax] = wa[i];
- SWAP( ipvt[i], ipvt[kmax], k);
+ SWAP(ipvt[i], ipvt[kmax], k);
}
}
@@ -623,9 +595,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
*/
double ajnorm = enorm(MIJ(a, i, i, lda), n - i);
- //if (ajnorm != 0.0)
- if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon() )
- {
+ // if (ajnorm != 0.0)
+ if (std::fabs(ajnorm) > std::numeric_limits<double>::epsilon()) {
if (*MIJ(a, i, i, lda) < 0.0)
ajnorm = -ajnorm;
@@ -639,10 +610,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
*/
ip1 = i + 1;
- if (m >= ip1)
- {
- for (k = ip1; k < m; k++)
- {
+ if (m >= ip1) {
+ for (k = ip1; k < m; k++) {
sum = 0.0;
for (j = i; j < n; j++)
sum += *MIJ(a, i, j, lda) * *MIJ(a, k, j, lda);
@@ -652,20 +621,17 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
for (j = i; j < n; j++)
*MIJ(a, k, j, lda) -= temp * *MIJ(a, i, j, lda);
- //if (pivot && rdiag[k] != 0.0)
- if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon()) )
- {
- temp = *MIJ (a, k, i, lda) / rdiag[k];
+ // if (pivot && rdiag[k] != 0.0)
+ if (pivot && (std::fabs(rdiag[k]) > std::numeric_limits<double>::epsilon())) {
+ temp = *MIJ(a, k, i, lda) / rdiag[k];
rdiag[k] *= sqrt(vpMath::maximum(0.0, (1.0 - temp * temp)));
- if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch)
- {
- rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n -1 - (int) i));
+ if (tolerance * (rdiag[k] / wa[k]) * (rdiag[k] / wa[k]) <= epsmch) {
+ rdiag[k] = enorm(MIJ(a, k, ip1, lda), (n - 1 - (int)i));
wa[k] = rdiag[k];
}
}
- }/* fin boucle for k */
-
+ } /* fin boucle for k */
}
} /* fin if (ajnorm) */
@@ -680,12 +646,11 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* ENTREE :
* n Ordre de la matrice "r".
* r Matrice de taille "n" x "n". En entree, la partie triangulaire
- * complete de "r" doit contenir la partie triangulaire superieure
- * complete de "r".
+ * complete de "r" doit contenir la partie triangulaire
+ *superieure complete de "r".
* ldr Taille maximale de la matrice "r". "ldr" >= n.
- * ipvt Vecteur de taille "n" definissant la matrice de permutation "p"
- * La jeme colonne de de "p" est la colonne ipvt[j] de la matrice
- * identite.
+ * ipvt Vecteur de taille "n" definissant la matrice de permutation
+ *"p" La jeme colonne de de "p" est la colonne ipvt[j] de la matrice identite.
* diag Vecteur de taille "n" contenant les elements diagonaux de la
* matrice "d".
* qtb Vecteur de taille "n" contenant les "n" premiers elements du
@@ -693,22 +658,22 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* wa Vecteur de travail de taille "n".
*
* DESCRIPTION :
- * La procedure complete la solution du probleme, si on fournit les information
- * necessaires de la factorisation qr, avec pivotage des colonnes.
- * Soit une matrice "a" de taille "m" x "n" donnee, une matrice diagonale "d" de
- * taille "n" x "n" et un vecteur "b" de taille "m", le probleme est la determination
- * un vecteur "x" qui est solution du systeme
+ * La procedure complete la solution du probleme, si on fournit les
+ *information necessaires de la factorisation qr, avec pivotage des colonnes.
+ * Soit une matrice "a" de taille "m" x "n" donnee, une matrice diagonale "d"
+ *de taille "n" x "n" et un vecteur "b" de taille "m", le probleme est la
+ *determination un vecteur "x" qui est solution du systeme
*
* a*x = b , d*x = 0 ,
*
* Au sens des moindres carres.
*
- * Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de "q"
- * sont orthogonales et "r" est une matrice traingulaire superieure dont les
- * elements diagonaux sont classes de l'ordre decroissant de leur valeur. Cette
- * procedure attend donc la matrice triangulaire superieure remplie "r", la
- * matrice de permutaion "p" et les "n" premiers elements de (q transpose) * b.
- * Le systeme
+ * Soit a * p = q * r, ou p est une matrice de permutation, les colonnes de
+ *"q" sont orthogonales et "r" est une matrice traingulaire superieure dont
+ *les elements diagonaux sont classes de l'ordre decroissant de leur valeur.
+ *Cette procedure attend donc la matrice triangulaire superieure remplie "r",
+ *la matrice de permutaion "p" et les "n" premiers elements de (q transpose)
+ ** b. Le systeme
*
* a * x = b, d * x = 0, est alors equivalent a
*
@@ -716,8 +681,8 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* r * z = q * b , p * d * p * z = 0 ,
*
* Ou x = p * z. Si ce systeme ne possede pas de rangee pleine, alors une
- * solution au moindre carre est obtenue. En sortie, la procedure fournit aussi
- * une matrice triangulaire superieure "s" tel que
+ * solution au moindre carre est obtenue. En sortie, la procedure fournit
+ *aussi une matrice triangulaire superieure "s" tel que
*
* t t t
* p * (a * a + d * d) * p = s * s .
@@ -725,29 +690,26 @@ int qrfac(int m, int n, double *a, int lda, int *pivot, int *ipvt,
* "s" est calculee a l'interieure de qrsolv et peut etre hors interet.
*
* SORTIE :
- * r En sortie, le triangle superieur n'est pas altere, et la partie
- * triangulaire inferieure contient la partie triangulaire superieure
+ * r En sortie, le triangle superieur n'est pas altere, et la
+ *partie triangulaire inferieure contient la partie triangulaire superieure
* (transpose) de la matrice triangulaire "s".
- * x Vecteur de taille "n" contenant les solutions au moindres carres du
- * systeme a * x = b, d * x = 0.
- * sdiag Vecteur de taille "n" contenant les elements diagonaux de la
- * matrice triangulaire superieure "s".
+ * x Vecteur de taille "n" contenant les solutions au moindres
+ *carres du systeme a * x = b, d * x = 0. sdiag Vecteur de taille "n"
+ *contenant les elements diagonaux de la matrice triangulaire superieure "s".
*
*/
-int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
- double *x, double *sdiag, double *wa)
+int 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;
- double cosi, cotg, qtbpj, sinu, tg, temp;
+ int i, j, k, kp1, l; /* compteur de boucle */
+ int nsing;
+ double cosi, cotg, qtbpj, sinu, tg, temp;
/*
* copie de r et (q transpose) * b afin de preserver l'entree
* et initialisation de "s". En particulier, sauvegarde des elements
* diagonaux de "r" dans "x".
*/
- for (i = 0; i < n; i++)
- {
+ for (i = 0; i < n; i++) {
for (j = i; j < n; j++)
*MIJ(r, i, j, ldr) = *MIJ(r, j, i, ldr);
@@ -760,8 +722,7 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* connue.
*/
- for (i = 0; i < n; i++)
- {
+ for (i = 0; i < n; i++) {
/*
* preparation de la colonne de d a eliminer, reperage de
* l'element diagonal par utilisation de p de la
@@ -769,9 +730,8 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
*/
l = ipvt[i];
- //if (diag[l] != 0.0)
- if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon())
- {
+ // if (diag[l] != 0.0)
+ if (std::fabs(diag[l]) > std::numeric_limits<double>::epsilon()) {
for (k = i; k < n; k++)
sdiag[k] = 0.0;
@@ -786,61 +746,52 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
qtbpj = 0.0;
- for (k = i; k < n; k++)
- {
+ for (k = i; k < n; k++) {
/*
- * determination d'une rotation qui elimine
- * les elements appropriees dans la colonne
- * courante de d.
- */
-
- //if (sdiag[k] != 0.0)
- if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon())
- {
- if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k]))
- {
+ * determination d'une rotation qui elimine
+ * les elements appropriees dans la colonne
+ * courante de d.
+ */
+
+ // if (sdiag[k] != 0.0)
+ if (std::fabs(sdiag[k]) > std::numeric_limits<double>::epsilon()) {
+ if (std::fabs(*MIJ(r, k, k, ldr)) >= std::fabs(sdiag[k])) {
tg = sdiag[k] / *MIJ(r, k, k, ldr);
cosi = 0.5 / sqrt(0.25 + 0.25 * (tg * tg));
sinu = cosi * tg;
- }
- else
- {
+ } else {
cotg = *MIJ(r, k, k, ldr) / sdiag[k];
sinu = 0.5 / sqrt(0.25 + 0.25 * (cotg * cotg));
cosi = sinu * cotg;
}
/*
- * calcul des elements de la diagonale modifiee
- * de r et des elements modifies de
- * ((q transpose)*b,0).
- */
+ * calcul des elements de la diagonale modifiee
+ * de r et des elements modifies de
+ * ((q transpose)*b,0).
+ */
*MIJ(r, k, k, ldr) = cosi * *MIJ(r, k, k, ldr) + sinu * sdiag[k];
temp = cosi * wa[k] + sinu * qtbpj;
qtbpj = -sinu * wa[k] + cosi * qtbpj;
wa[k] = temp;
/*
- * accumulation des tranformations dans
- * les lignes de s.
- */
+ * accumulation des tranformations dans
+ * les lignes de s.
+ */
kp1 = k + 1;
- if ( n >= kp1)
- {
- for (j = kp1; j < n; j++)
- {
- temp = cosi * *MIJ(r, k, j, ldr) +
- sinu * sdiag[j];
- sdiag[j] = - sinu * *MIJ(r, k, j, ldr) +
- cosi * sdiag[j];
+ if (n >= kp1) {
+ for (j = kp1; j < n; j++) {
+ temp = cosi * *MIJ(r, k, j, ldr) + sinu * sdiag[j];
+ sdiag[j] = -sinu * *MIJ(r, k, j, ldr) + cosi * sdiag[j];
*MIJ(r, k, j, ldr) = temp;
}
}
- }/* fin if diag[] !=0 */
- } /* fin boucle for k -> n */
- }/* fin if diag =0 */
+ } /* fin if diag[] !=0 */
+ } /* fin boucle for k -> n */
+ } /* fin if diag =0 */
/*
* stokage de l'element diagonal de s et restauration de
@@ -854,28 +805,24 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* resolution du systeme triangulaire pour z. Si le systeme est
* singulier, on obtient une solution au moindres carres.
*/
- nsing = n;
+ nsing = n;
- for (i = 0; i < n; i++)
- {
- //if (sdiag[i] == 0.0 && nsing == n)
- if ( (std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
+ for (i = 0; i < n; i++) {
+ // if (sdiag[i] == 0.0 && nsing == n)
+ if ((std::fabs(sdiag[i]) <= std::numeric_limits<double>::epsilon()) && nsing == n)
nsing = i - 1;
if (nsing < n)
wa[i] = 0.0;
}
- if (nsing >= 0)
- {
- for (k = 0; k < nsing; k++)
- {
+ if (nsing >= 0) {
+ for (k = 0; k < nsing; k++) {
i = nsing - 1 - k;
double sum = 0.0;
int jp1 = i + 1;
- if (nsing >= jp1)
- {
+ if (nsing >= jp1) {
for (j = jp1; j < nsing; j++)
sum += *MIJ(r, i, j, ldr) * wa[j];
}
@@ -886,11 +833,9 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* permutation arriere des composants de z et des componants de x.
*/
- for (j = 0; j < n; j++)
- {
+ for (j = 0; j < n; j++) {
l = ipvt[j];
x[l] = wa[j];
-
}
return (0);
}
@@ -900,43 +845,41 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
*
*
* ENTREE :
- * fcn Fonction qui calcule la fonction et le jacobien de la fonction.
- * m Nombre de fonctions.
+ * fcn Fonction qui calcule la fonction et le jacobien de la
+ *fonction. m Nombre de fonctions.
* n Nombre de variables. n <= m
* x Vecteur de taille "n" contenant en entree une estimation
* initiale de la solution.
* ldfjac Taille dominante de la matrice "fjac". ldfjac >= "m".
- * ftol Erreur relative desiree dans la somme des carre. La terminaison
- * survient quand les preductions estimee et vraie de la somme des
+ * ftol Erreur relative desiree dans la somme des carre. La
+ *terminaison survient quand les preductions estimee et vraie de la somme des
* carres sont toutes deux au moins egal a ftol.
* xtol Erreur relative desiree dans la solution approximee. La
* terminaison survient quand l'erreur relative entre deux
* iterations consecutives est au moins egal a xtol.
- * gtol Mesure de l'orthogonalite entre le vecteur des fonctions et les
- * colonnes du jacobien. La terminaison survient quand le cosinus
- * de l'angle entre fvec et n'importe quelle colonne du jacobien
- * est au moins egal a gtol, en valeur absolue.
- * maxfev Nombre d'appel maximum. La terminaison se produit lorsque le
- * nombre d'appel a fcn avec iflag = 1 a atteint "maxfev".
+ * gtol Mesure de l'orthogonalite entre le vecteur des fonctions et
+ *les colonnes du jacobien. La terminaison survient quand le cosinus de
+ *l'angle entre fvec et n'importe quelle colonne du jacobien est au moins
+ *egal a gtol, en valeur absolue. maxfev Nombre d'appel maximum. La
+ *terminaison se produit lorsque le nombre d'appel a fcn avec iflag = 1 a
+ *atteint "maxfev".
* diag Vecteur de taille "n". Si mode = 1 (voir ci-apres), diag est
* initialisee en interne. Si mode = 2, diag doit contenir les
- * entree positives qui servent de facteurs d'echelle aux variables.
+ * entree positives qui servent de facteurs d'echelle aux
+ *variables.
* mode Si mode = 1, les variables seront mis a l'echelle en interne.
- * Si mode = 2, la mise a l'echelle est specifie par l'entree diag.
- * Les autres valeurs de mode sont equivalents a mode = 1.
- * factor Definit la limite de l'etape initial. Cette limite est initialise
- * au produit de "factor" et de la norme euclidienne de "diag" * "x"
- * sinon nul. ou a "factor" lui meme. Dans la plupart des cas,
- * "factor" doit se trouve dans l'intervalle (1, 100); ou 100 est
- * la valeur recommandee.
- * nprint Controle de l'impression des iterees (si valeur positive).
- * Dans ce cas, fcn est appelle avec iflag = 0 au debut de la
- * premiere iteration et apres chaque nprint iteration, x, fvec,
- * et fjac sont disponible pour impression, cela avant de quitter
- * la procedure. Si "nprint" est negatif, aucun appel special de
- * fcn est faite.
- * wa1, wa2, wa3 Vecteur de travail de taille "n".
- * wa4 Vecteur de travail de taille "m".
+ * Si mode = 2, la mise a l'echelle est specifie par l'entree
+ *diag. Les autres valeurs de mode sont equivalents a mode = 1. factor
+ *Definit la limite de l'etape initial. Cette limite est initialise au
+ *produit de "factor" et de la norme euclidienne de "diag" * "x" sinon nul.
+ *ou a "factor" lui meme. Dans la plupart des cas, "factor" doit se trouve
+ *dans l'intervalle (1, 100); ou 100 est la valeur recommandee. nprint
+ *Controle de l'impression des iterees (si valeur positive). Dans ce
+ *cas, fcn est appelle avec iflag = 0 au debut de la premiere iteration et
+ *apres chaque nprint iteration, x, fvec, et fjac sont disponible pour
+ *impression, cela avant de quitter la procedure. Si "nprint" est negatif,
+ *aucun appel special de fcn est faite. wa1, wa2, wa3 Vecteur de travail de
+ *taille "n". wa4 Vecteur de travail de taille "m".
*
*
* SORTIE :
@@ -956,8 +899,8 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* La colonne j de p est la colonne ipvt (j) (voir ci apres) de
* la matrice identite. La partie trapesoidale inferieure de fjac
* contient les information genere durant le calcul de r.
- * info Information de l'execution de la procedure. Lorsque la procedure
- * a termine son execution, "info" est inialisee a la valeur
+ * info Information de l'execution de la procedure. Lorsque la
+ *procedure a termine son execution, "info" est inialisee a la valeur
* (negative) de iflag. sinon elle prend les valeurs suivantes :
* info = 0 : parametres en entree non valides.
* info = 1 : les reductions relatives reelle et estimee de la
@@ -965,16 +908,12 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* info = 2 : erreur relative entre deux iteres consecutives sont
* egaux a xtol.
* info = 3 : conditions info = 1 et info = 2 tous deux requis.
- * info = 4 : le cosinus de l'angle entre fvec et n'importe quelle
- * colonne du jacobien est au moins egal a gtol, en
- * valeur absolue.
- * info = 5 : nombre d'appels a fcn avec iflag = 1 a atteint
- * maxfev.
- * info = 6 : ftol est trop petit. Plus moyen de reduire de la
- * somme des carres.
- * info = 7 : xtol est trop petit. Plus d'amelioration possible
- * pour approximer la solution x.
- * info = 8 : gtol est trop petit. "fvec" est orthogonal aux
+ * info = 4 : le cosinus de l'angle entre fvec et n'importe
+ *quelle colonne du jacobien est au moins egal a gtol, en valeur absolue. info
+ *= 5 : nombre d'appels a fcn avec iflag = 1 a atteint maxfev. info = 6 :
+ *ftol est trop petit. Plus moyen de reduire de la somme des carres. info =
+ *7 : xtol est trop petit. Plus d'amelioration possible pour approximer la
+ *solution x. info = 8 : gtol est trop petit. "fvec" est orthogonal aux
* colonnes du jacobien a la precision machine pres.
* nfev Nombre d'appel a "fcn" avec iflag = 1.
* njev Nombre d'appel a "fcn" avec iflag = 2.
@@ -982,9 +921,9 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* tel que jac * p = q * p, ou jac est le jacbien final calcule,
* q est orthogonal (non socke) et r est triangulaire superieur,
* avec les elements diagonaux classes en ordre decroissant de
- * leur valeur. La colonne j de p est ipvt[j] de la matrice identite.
- * qtf Vecteur de taille n contenant les n premiers elements du
- * vecteur qT * fvec.
+ * leur valeur. La colonne j de p est ipvt[j] de la matrice
+ *identite. qtf Vecteur de taille n contenant les n premiers elements
+ *du vecteur qT * fvec.
*
* DESCRIPTION :
* La procedure minimize la somme de carre de m equation non lineaire a n
@@ -995,7 +934,8 @@ int qrsolv (int n, double *r, int ldr, int *ipvt, double *diag, double *qtb,
* le jacobien.
* "fcn" doit etre declare dans une instruction externe a la procedure et doit
* etre appele comme suit :
- * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
+ * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
+ **iflag)
*
* si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
* fjac n'est pas modifie.
@@ -1006,23 +946,20 @@ 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;
- int oncol = TRUE;
- int iflag, iter;
+ const double tol1 = 0.1, tol5 = 0.5, tol25 = 0.25, tol75 = 0.75, tol0001 = 0.0001;
+ int oncol = TRUE;
+ int iflag, iter;
int count = 0;
- int i, j, l;
- double actred, delta, dirder, epsmch, fnorm, fnorm1;
- double ratio = std::numeric_limits<double>::epsilon();
- double par, pnorm, prered;
- double sum, temp, temp1, temp2, xnorm = 0.0;
+ int i, j, l;
+ double actred, delta, dirder, epsmch, fnorm, fnorm1;
+ double ratio = std::numeric_limits<double>::epsilon();
+ double par, pnorm, prered;
+ double sum, temp, temp1, temp2, xnorm = 0.0;
/* epsmch est la precision machine. */
epsmch = std::numeric_limits<double>::epsilon();
@@ -1040,19 +977,18 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
return 0;
if (ldfjac < m)
return 0;
- if (ftol < 0.0)
+ if (ftol < 0.0)
return 0;
if (xtol < 0.0)
return 0;
if (gtol < 0.0)
return 0;
- if (maxfev == 0)
+ if (maxfev == 0)
return 0;
if (factor <= 0.0)
return 0;
- if ((n <= 0) || (m < n) || (ldfjac < m) || (ftol < 0.0) || (xtol < 0.0)
- || (gtol < 0.0) || (maxfev == 0) || (factor <= 0.0))
- {
+ 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.
*/
@@ -1067,15 +1003,12 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
return (count);
}
- if (mode == 2)
- {
- for (j = 0; j < n; j++)
- {
- if (diag[j] <= 0.0)
- {
+ if (mode == 2) {
+ for (j = 0; j < n; j++) {
+ if (diag[j] <= 0.0) {
/*
- * termination, normal ou imposee par l'utilisateur.
- */
+ * termination, normal ou imposee par l'utilisateur.
+ */
if (iflag < 0)
*info = iflag;
@@ -1097,11 +1030,9 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
-
*nfev = 1;
- if (iflag < 0)
- {
+ if (iflag < 0) {
/*
* termination, normal ou imposee par l'utilisateur.
*/
@@ -1129,8 +1060,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
/*
* debut de la boucle la plus externe.
*/
- while (count < (int)maxfev)
- {
+ while (count < (int)maxfev) {
count++;
/*
* calcul de la matrice jacobienne.
@@ -1140,10 +1070,9 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
- (*njev) ++;
+ (*njev)++;
- if (iflag < 0)
- {
+ if (iflag < 0) {
/*
* termination, normal ou imposee par l'utilisateur.
*/
@@ -1161,17 +1090,15 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
/*
* si demandee, appel de fcn pour impression des iterees.
*/
- if (nprint > 0)
- {
+ if (nprint > 0) {
iflag = 0;
- if ((iter-1) % nprint == 0)
+ if ((iter - 1) % nprint == 0)
(*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
- if (iflag < 0)
- {
+ if (iflag < 0) {
/*
- * termination, normal ou imposee par l'utilisateur.
- */
+ * termination, normal ou imposee par l'utilisateur.
+ */
if (iflag < 0)
*info = iflag;
@@ -1194,14 +1121,11 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* en accord avec les normes des colonnes du jacobien initial.
*/
- if (iter == 1)
- {
- if (mode != 2)
- {
- for (j = 0; j < n; j++)
- {
+ if (iter == 1) {
+ if (mode != 2) {
+ for (j = 0; j < n; j++) {
diag[j] = wa2[j];
- //if (wa2[j] == 0.0)
+ // if (wa2[j] == 0.0)
if (std::fabs(wa2[j]) <= std::numeric_limits<double>::epsilon())
diag[j] = 1.0;
}
@@ -1216,10 +1140,10 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
for (j = 0; j < n; j++)
wa3[j] = diag[j] * x[j];
- xnorm = enorm (wa3, n);
+ xnorm = enorm(wa3, n);
delta = factor * xnorm;
- //if (delta == 0.0)
+ // if (delta == 0.0)
if (std::fabs(delta) <= std::numeric_limits<double>::epsilon())
delta = factor;
}
@@ -1231,22 +1155,19 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
for (i = 0; i < m; i++)
wa4[i] = fvec[i];
- for (i = 0; i < n; i++)
- {
+ for (i = 0; i < n; i++) {
double *pt = MIJ(fjac, i, i, ldfjac);
- //if (*MIJ(fjac, i, i, ldfjac) != 0.0)
- if (std::fabs(*pt) > std::numeric_limits<double>::epsilon() )
- {
+ // if (*MIJ(fjac, i, i, ldfjac) != 0.0)
+ if (std::fabs(*pt) > std::numeric_limits<double>::epsilon()) {
sum = 0.0;
for (j = i; j < m; j++)
sum += *MIJ(fjac, i, j, ldfjac) * wa4[j];
- temp = - sum / *MIJ(fjac, i, i, ldfjac);
+ temp = -sum / *MIJ(fjac, i, i, ldfjac);
for (j = i; j < m; j++)
wa4[j] += *MIJ(fjac, i, j, ldfjac) * temp;
-
}
*MIJ(fjac, i, i, ldfjac) = wa1[i];
@@ -1259,15 +1180,12 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
double gnorm = 0.0;
- //if (fnorm != 0.0)
- if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon())
- {
- for (i = 0; i < n; i++)
- {
+ // if (fnorm != 0.0)
+ if (std::fabs(fnorm) > std::numeric_limits<double>::epsilon()) {
+ for (i = 0; i < n; i++) {
l = ipvt[i];
- //if (wa2[l] != 0.0)
- if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon())
- {
+ // if (wa2[l] != 0.0)
+ if (std::fabs(wa2[l]) > std::numeric_limits<double>::epsilon()) {
sum = 0.0;
for (j = 0; j <= i; j++)
sum += *MIJ(fjac, i, j, ldfjac) * (qtf[j] / fnorm);
@@ -1284,8 +1202,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
if (gnorm <= gtol)
*info = 4;
- if (*info != 0)
- {
+ if (*info != 0) {
/*
* termination, normal ou imposee par l'utilisateur.
*/
@@ -1304,8 +1221,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* remise a l'echelle si necessaire.
*/
- if (mode != 2)
- {
+ if (mode != 2) {
for (j = 0; j < n; j++)
diag[j] = vpMath::maximum(diag[j], wa2[j]);
}
@@ -1314,22 +1230,19 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* debut de la boucle la plus interne.
*/
ratio = 0.0;
- while (ratio < tol0001)
- {
+ while (ratio < tol0001) {
/*
* 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.
*/
- for (j = 0; j < n; j++)
- {
- wa1[j] = - wa1[j];
+ for (j = 0; j < n; j++) {
+ wa1[j] = -wa1[j];
wa2[j] = x[j] + wa1[j];
wa3[j] = diag[j] * wa1[j];
}
@@ -1353,8 +1266,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
(*nfev)++;
- if (iflag < 0)
- {
+ if (iflag < 0) {
// termination, normal ou imposee par l'utilisateur.
if (iflag < 0)
*info = iflag;
@@ -1373,7 +1285,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* calcul de la reduction reelle mise a l'echelle.
*/
- actred = - 1.0;
+ actred = -1.0;
if ((tol1 * fnorm1) < fnorm)
actred = 1.0 - ((fnorm1 / fnorm) * (fnorm1 / fnorm));
@@ -1383,8 +1295,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* de la derivee directionnelle mise a l'echelle.
*/
- for (i = 0; i < n; i++)
- {
+ for (i = 0; i < n; i++) {
wa3[i] = 0.0;
l = ipvt[i];
temp = wa1[l];
@@ -1395,7 +1306,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
temp1 = enorm(wa3, n) / fnorm;
temp2 = (sqrt(par) * pnorm) / fnorm;
prered = (temp1 * temp1) + (temp2 * temp2) / tol5;
- dirder = - ((temp1 * temp1) + (temp2 * temp2));
+ dirder = -((temp1 * temp1) + (temp2 * temp2));
/*
* calcul du rapport entre la reduction reel et predit.
@@ -1403,7 +1314,7 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
ratio = 0.0;
- //if (prered != 0.0)
+ // if (prered != 0.0)
if (std::fabs(prered) > std::numeric_limits<double>::epsilon())
ratio = actred / prered;
@@ -1411,17 +1322,13 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* mise a jour de la limite de l'etape.
*/
- if (ratio > tol25)
- {
- //if ((par == 0.0) || (ratio <= tol75))
- if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75))
- {
+ if (ratio > tol25) {
+ // if ((par == 0.0) || (ratio <= tol75))
+ if ((std::fabs(par) <= std::numeric_limits<double>::epsilon()) || (ratio <= tol75)) {
delta = pnorm / tol5;
par *= tol5;
}
- }
- else
- {
+ } else {
if (actred >= 0.0)
temp = tol5;
@@ -1438,15 +1345,13 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
/*
* test pour une iteration reussie.
*/
- if (ratio >= tol0001)
- {
+ if (ratio >= tol0001) {
/*
- * iteration reussie. mise a jour de x, de fvec, et de
- * leurs normes.
- */
+ * iteration reussie. mise a jour de x, de fvec, et de
+ * leurs normes.
+ */
- for (j = 0; j < n; j++)
- {
+ for (j = 0; j < n; j++) {
x[j] = wa2[j];
wa2[j] = diag[j] * x[j];
}
@@ -1469,22 +1374,20 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
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)
- {
+ if (*info != 0) {
/*
- * termination, normal ou imposee par l'utilisateur.
- */
+ * termination, normal ou imposee par l'utilisateur.
+ */
if (iflag < 0)
*info = iflag;
iflag = 0;
if (nprint > 0)
- (*ptr_fcn)(m,n,x,fvec,fjac,ldfjac, iflag);
+ (*ptr_fcn)(m, n, x, fvec, fjac, ldfjac, iflag);
return (count);
}
@@ -1496,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)
@@ -1506,11 +1408,10 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
if (gnorm <= epsmch)
*info = 8;
- if (*info != 0)
- {
+ if (*info != 0) {
/*
- * termination, normal ou imposee par l'utilisateur.
- */
+ * termination, normal ou imposee par l'utilisateur.
+ */
if (iflag < 0)
*info = iflag;
@@ -1521,23 +1422,20 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
return (count);
}
- }/* fin while ratio >=tol0001 */
- }/*fin while 1*/
+ } /* fin while ratio >=tol0001 */
+ } /*fin while 1*/
- return 0 ;
+ return 0;
}
-
-
/*
* PROCEDURE : lmder1
*
* ENTREE :
- * fcn Fonction qui calcule la fonction et le jacobien de la fonction.
- * m Nombre de fonctions.
- * n Nombre de variables (parametres). n <= m
- * x Vecteur de taille "n" contenant en entree une estimation
- * initiale de la solution.
+ * fcn Fonction qui calcule la fonction et le jacobien de la
+ *fonction. m Nombre de fonctions. n Nombre de variables
+ *(parametres). n <= m x Vecteur de taille "n" contenant en
+ *entree une estimation initiale de la solution.
* ldfjac Taille maximale de la matrice "fjac". ldfjac >= "m".
* tol Tolerance. La terminaison de la procedure survient quand
* l'algorithme estime que l'erreur relative dans la somme des
@@ -1564,8 +1462,8 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* La colonne j de p est la colonne ipvt (j) (voir ci apres) de
* la matrice identite. La partie trapesoidale inferieure de fjac
* contient les information genere durant le calcul de r.
- * info Information de l'executionde la procedure. Lorsque la procedure
- * a termine son execution, "info" est inialisee a la valeur
+ * info Information de l'executionde la procedure. Lorsque la
+ *procedure a termine son execution, "info" est inialisee a la valeur
* (negative) de iflag. sinon elle prend les valeurs suivantes :
* info = 0 : parametres en entre non valides.
* info = 1 : estimation par l'algorithme que l'erreur relative
@@ -1584,7 +1482,8 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* tel que jac * p = q * p, ou jac est le jacbien final calcule,
* q est orthogonal (non socke) et r est triangulaire superieur,
* avec les elements diagonaux classes en ordre decroissant de
- * leur valeur. La colonne j de p est ipvt[j] de la matrice identite.
+ * leur valeur. La colonne j de p est ipvt[j] de la matrice
+ *identite.
*
* DESCRIPTION :
* La procedure minimize la somme de carre de m equation non lineaire a n
@@ -1596,7 +1495,8 @@ int lmder (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
* le jacobien.
* "fcn" doit etre declare dans une instruction externe a la procedure et doit
* etre appele comme suit :
- * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int *iflag)
+ * fcn (int m, int n, int ldfjac, double *x, double *fvec, double *fjac, int
+ **iflag)
*
* si iflag = 1 calcul de la fonction en x et retour de ce vecteur dans fvec.
* fjac n'est pas modifie.
@@ -1608,25 +1508,20 @@ 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;
- int mode, njev, nprint;
- double ftol, gtol, xtol;
+ const double factor = 100.0;
+ unsigned int maxfev, nfev;
+ int mode, njev, nprint;
+ double ftol, gtol, xtol;
*info = 0;
/* verification des parametres en entree qui causent des erreurs */
-
- if (/*(n <= 0) ||*/ (m < n) || (ldfjac < m) || (tol < 0.0) ||
- (lwa < (5 * n + m)) )
- {
- printf("%d %d %d %d \n", (m < n), (ldfjac < m), (tol < 0.0) , (lwa < (5 * n + m))) ;
+ 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);
}
@@ -1639,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;
@@ -1651,10 +1545,3 @@ int lmder1 (void (*ptr_fcn)(int m, int n, double *xc, double *fvecc,
#undef TRUE
#undef FALSE
-
-
-/*
- * Local variables:
- * c-basic-offset: 2
- * End:
- */
diff --git a/modules/vision/src/pose-estimation/vpPose.cpp b/modules/vision/src/pose-estimation/vpPose.cpp
index 00ae427..c1d3346 100644
--- a/modules/vision/src/pose-estimation/vpPose.cpp
+++ b/modules/vision/src/pose-estimation/vpPose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,76 +37,68 @@
*
*****************************************************************************/
-
/*!
\file vpPose.cpp
\brief Fichier contenant la classe vpPose (tout ce qui est necessaire
pour faire du calcul de pose par difference methode
*/
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpException.h>
-#include <visp3/vision/vpPoseException.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpDisplay.h>
+#include <visp3/core/vpException.h>
#include <visp3/core/vpMath.h>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/vision/vpPose.h>
+#include <visp3/vision/vpPoseException.h>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <cmath> // std::fabs
+#include <limits> // numeric_limits
#define DEBUG_LEVEL1 0
/*!
Basic initialisation that is called by the constructors.
*/
-void
-vpPose::init()
+void vpPose::init()
{
#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::Init() " << std::endl ;
+ std::cout << "begin vpPose::Init() " << std::endl;
#endif
- npt = 0 ;
+
+ npt = 0;
listP.clear();
+ residual = 0;
+ lambda = 0.25;
+ vvsIterMax = 200;
c3d.clear();
-
- lambda = 0.25 ;
-
- vvsIterMax = 200 ;
-
- distanceToPlaneForCoplanarityTest = 0.001 ;
-
computeCovariance = false;
-
+ covarianceMatrix.clear();
+ ransacNbInlierConsensus = 4;
ransacMaxTrials = 1000;
+ ransacInliers.clear();
+ ransacInlierIndex.clear();
ransacThreshold = 0.0001;
- ransacNbInlierConsensus = 4;
+ distanceToPlaneForCoplanarityTest = 0.001;
+ ransacFlags = PREFILTER_DUPLICATE_POINTS;
+ listOfPoints.clear();
+ useParallelRansac = false;
+ nbParallelRansacThreads = 0;
+ vvsEpsilon = 1e-8;
- residual = 0;
#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::Init() " << std::endl ;
+ std::cout << "end vpPose::Init() " << std::endl;
#endif
-
}
/*! Default constructor. */
vpPose::vpPose()
- : npt(0), listP(), residual(0), lambda(0.25), vvsIterMax(200), c3d(),
- computeCovariance(false), covarianceMatrix(),
+ : 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
+ 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
+ vvsEpsilon(1e-8)
{
-#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::vpPose() " << std::endl ;
-#endif
-
- init() ;
-
-#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::vpPose() " << std::endl ;
-#endif
-
}
/*!
@@ -114,23 +107,23 @@ vpPose::vpPose()
vpPose::~vpPose()
{
#if (DEBUG_LEVEL1)
- std::cout << "begin vpPose::~vpPose() " << std::endl ;
+ std::cout << "begin vpPose::~vpPose() " << std::endl;
#endif
listP.clear();
#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::~vpPose() " << std::endl ;
+ std::cout << "end vpPose::~vpPose() " << std::endl;
#endif
}
/*!
Delete the array of point
*/
-void
-vpPose::clearPoint()
+void vpPose::clearPoint()
{
listP.clear();
- npt = 0 ;
+ listOfPoints.clear();
+ npt = 0;
}
/*!
@@ -141,12 +134,11 @@ vpPose::clearPoint()
its 2D coordinates in the image plane. These 5 fields must be initialized
to be used within this function.
*/
-void
-vpPose::addPoint(const vpPoint& newP)
+void vpPose::addPoint(const vpPoint &newP)
{
listP.push_back(newP);
listOfPoints.push_back(newP);
- npt++ ;
+ npt++;
}
/*!
@@ -157,18 +149,14 @@ vpPose::addPoint(const vpPoint& newP)
its 2D coordinates in the image plane. These 5 fields must be initialized
to be used within this function.
*/
-void
-vpPose::addPoints(const std::vector<vpPoint> &lP) {
+void vpPose::addPoints(const std::vector<vpPoint> &lP)
+{
listP.insert(listP.end(), lP.begin(), lP.end());
listOfPoints.insert(listOfPoints.end(), lP.begin(), lP.end());
- npt = (unsigned int) listP.size();
+ 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
@@ -181,24 +169,22 @@ vpPose::setDistanceToPlaneForCoplanarityTest(double d)
0: any other plane
\return true if points are coplanar false otherwise.
*/
-bool
-vpPose::coplanar(int &coplanar_plane_type)
+bool vpPose::coplanar(int &coplanar_plane_type)
{
coplanar_plane_type = 0;
- if (npt <2)
- {
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
+ if (npt < 2) {
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
}
- if (npt ==3) return true ;
+ 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<vpPoint>::const_iterator it = listP.begin();
- vpPoint P1, P2, P3 ;
+ vpPoint P1, P2, P3;
// Get three 3D points that are not collinear and that is not at origin
bool degenerate = true;
@@ -206,65 +192,68 @@ vpPose::coplanar(int &coplanar_plane_type)
std::list<vpPoint>::const_iterator it_tmp;
std::list<vpPoint>::const_iterator it_i, it_j, it_k;
- for (it_i=listP.begin(); it_i != listP.end(); ++it_i) {
+ for (it_i = listP.begin(); it_i != listP.end(); ++it_i) {
if (degenerate == false) {
- //std::cout << "Found a non degenerate configuration" << std::endl;
+ // std::cout << "Found a non degenerate configuration" << std::endl;
break;
}
P1 = *it_i;
// Test if point is on origin
- if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
- not_on_origin = false;
- }
- else {
+ if ((std::fabs(P1.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P1.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P1.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ not_on_origin = false;
+ } else {
not_on_origin = true;
}
if (not_on_origin) {
- it_tmp = it_i; ++it_tmp; // j = i+1
- for (it_j=it_tmp; it_j != listP.end(); ++it_j) {
+ it_tmp = it_i;
+ ++it_tmp; // j = i+1
+ for (it_j = it_tmp; it_j != listP.end(); ++it_j) {
if (degenerate == false) {
- //std::cout << "Found a non degenerate configuration" << std::endl;
+ // std::cout << "Found a non degenerate configuration" << std::endl;
break;
}
P2 = *it_j;
- if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ if ((std::fabs(P2.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P2.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P2.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
not_on_origin = false;
- }
- else {
+ } else {
not_on_origin = true;
}
if (not_on_origin) {
- it_tmp = it_j; ++it_tmp; // k = j+1
- for (it_k=it_tmp; it_k != listP.end(); ++it_k) {
+ it_tmp = it_j;
+ ++it_tmp; // k = j+1
+ for (it_k = it_tmp; it_k != listP.end(); ++it_k) {
P3 = *it_k;
- if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon())
- && (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
+ if ((std::fabs(P3.get_oX()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P3.get_oY()) <= std::numeric_limits<double>::epsilon()) &&
+ (std::fabs(P3.get_oZ()) <= std::numeric_limits<double>::epsilon())) {
not_on_origin = false;
- }
- else {
+ } else {
not_on_origin = true;
}
if (not_on_origin) {
- x1 = P1.get_oX() ;
- x2 = P2.get_oX() ;
- x3 = P3.get_oX() ;
+ x1 = P1.get_oX();
+ x2 = P2.get_oX();
+ x3 = P3.get_oX();
- y1 = P1.get_oY() ;
- y2 = P2.get_oY() ;
- y3 = P3.get_oY() ;
+ y1 = P1.get_oY();
+ y2 = P2.get_oY();
+ y3 = P3.get_oY();
- z1 = P1.get_oZ() ;
- z2 = P2.get_oZ() ;
- z3 = P3.get_oZ() ;
+ z1 = P1.get_oZ();
+ z2 = P2.get_oZ();
+ z3 = P3.get_oZ();
vpColVector a_b(3), b_c(3), cross_prod;
- a_b[0] = x1-x2; a_b[1] = y1-y2; a_b[2] = z1-z2;
- b_c[0] = x2-x3; b_c[1] = y2-y3; b_c[2] = z2-z3;
+ a_b[0] = x1 - x2;
+ a_b[1] = y1 - y2;
+ a_b[2] = z1 - z2;
+ b_c[0] = x2 - x3;
+ b_c[1] = y2 - y3;
+ b_c[2] = z2 - z3;
cross_prod = vpColVector::crossProd(a_b, b_c);
if (cross_prod.sumSquare() <= std::numeric_limits<double>::epsilon())
@@ -285,43 +274,42 @@ vpPose::coplanar(int &coplanar_plane_type)
return true;
}
- double a = y1*z2 - y1*z3 - y2*z1 + y2*z3 + y3*z1 - y3*z2 ;
- double b = -x1*z2 + x1*z3 + x2*z1 - x2*z3 - x3*z1 + x3*z2 ;
- double c = x1*y2 - x1*y3 - x2*y1 + x2*y3 + x3*y1 - x3*y2 ;
- double d = -x1*y2*z3 + x1*y3*z2 +x2*y1*z3 - x2*y3*z1 - x3*y1*z2 + x3*y2*z1 ;
+ double a = y1 * z2 - y1 * z3 - y2 * z1 + y2 * z3 + y3 * z1 - y3 * z2;
+ double b = -x1 * z2 + x1 * z3 + x2 * z1 - x2 * z3 - x3 * z1 + x3 * z2;
+ double c = x1 * y2 - x1 * y3 - x2 * y1 + x2 * y3 + x3 * y1 - x3 * y2;
+ double d = -x1 * y2 * z3 + x1 * y3 * z2 + x2 * y1 * z3 - x2 * y3 * z1 - x3 * y1 * z2 + x3 * y2 * z1;
- //std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d << std::endl;
- if (std::fabs(b) <= std::numeric_limits<double>::epsilon()
- && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
+ // std::cout << "a=" << a << " b=" << b << " c=" << c << " d=" << d <<
+ // std::endl;
+ if (std::fabs(b) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
coplanar_plane_type = 1; // ax=d
- } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
- && std::fabs(c) <= std::numeric_limits<double>::epsilon() ) {
+ } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(c) <= std::numeric_limits<double>::epsilon()) {
coplanar_plane_type = 2; // by=d
- } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon()
- && std::fabs(b) <= std::numeric_limits<double>::epsilon() ) {
+ } else if (std::fabs(a) <= std::numeric_limits<double>::epsilon() &&
+ std::fabs(b) <= std::numeric_limits<double>::epsilon()) {
coplanar_plane_type = 3; // cz=d
}
- double D = sqrt(vpMath::sqr(a)+vpMath::sqr(b)+vpMath::sqr(c)) ;
+ double D = sqrt(vpMath::sqr(a) + vpMath::sqr(b) + vpMath::sqr(c));
- 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 ;
- //std::cout << "dist= " << dist << std::endl;
+ 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;
+ // std::cout << "dist= " << dist << std::endl;
- if (fabs(dist) > distanceToPlaneForCoplanarityTest)
- {
- vpDEBUG_TRACE(10," points are not coplanar ") ;
+ if (fabs(dist) > distanceToPlaneForCoplanarityTest) {
+ vpDEBUG_TRACE(10, " points are not coplanar ");
// TRACE(" points are not coplanar ") ;
- return false ;
+ return false;
}
}
- vpDEBUG_TRACE(10," points are coplanar ") ;
+ vpDEBUG_TRACE(10, " points are coplanar ");
// vpTRACE(" points are coplanar ") ;
- return true ;
+ return true;
}
/*!
@@ -333,287 +321,230 @@ the pose matrix 'cMo'.
\return The value of he residual in meter.
*/
-double
-vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const
+double vpPose::computeResidual(const vpHomogeneousMatrix &cMo) const
{
- double residual_ = 0 ;
- vpPoint P ;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
+ double residual_ = 0;
+ vpPoint P;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- double x = P.get_x() ;
- double y = P.get_y() ;
+ double x = P.get_x();
+ double y = P.get_y();
- P.track(cMo) ;
+ P.track(cMo);
- residual_ += vpMath::sqr(x-P.get_x()) + vpMath::sqr(y-P.get_y()) ;
+ residual_ += vpMath::sqr(x - P.get_x()) + vpMath::sqr(y - P.get_y());
}
- return residual_ ;
+ return residual_;
}
-
-
/*!
Compute the pose according to the desired method which are:
- - vpPose::LAGRANGE: Linear Lagrange approach (test is done to switch between planar and
- non planar algorithm)
- - vpPose::DEMENTHON: Linear Dementhon approach (test is done to switch between planar and
- non planar algorithm)
- - vpPose::LOWE: Lowe aproach based on a Levenberg Marquartd non linear minimization scheme that needs an initialization from Lagrange or Dementhon aproach
- - vpPose::LAGRANGE_LOWE: Non linear Lowe aproach initialized by Lagrange approach
- - vpPose::DEMENTHON_LOWE: Non linear Lowe aproach initialized by Dementhon approach
- - vpPose::VIRTUAL_VS: Non linear virtual visual servoing approach that needs an initialization from Lagrange or Dementhon aproach
- - vpPose::DEMENTHON_VIRTUAL_VS: Non linear virtual visual servoing approach initialized by Dementhon approach
- - vpPose::LAGRANGE_VIRTUAL_VS: Non linear virtual visual servoing approach initialized by Lagrange approach
+ - vpPose::LAGRANGE: Linear Lagrange approach (test is done to switch between
+ planar and non planar algorithm)
+ - vpPose::DEMENTHON: Linear Dementhon approach (test is done to switch
+ between planar and non planar algorithm)
+ - vpPose::LOWE: Lowe aproach based on a Levenberg Marquartd non linear
+ minimization scheme that needs an initialization from Lagrange or Dementhon
+ aproach
+ - vpPose::LAGRANGE_LOWE: Non linear Lowe aproach initialized by Lagrange
+ approach
+ - vpPose::DEMENTHON_LOWE: Non linear Lowe aproach initialized by Dementhon
+ approach
+ - vpPose::VIRTUAL_VS: Non linear virtual visual servoing approach that needs
+ an initialization from Lagrange or Dementhon aproach
+ - vpPose::DEMENTHON_VIRTUAL_VS: Non linear virtual visual servoing approach
+ initialized by Dementhon approach
+ - vpPose::LAGRANGE_VIRTUAL_VS: Non linear virtual visual servoing approach
+ initialized by Lagrange approach
- 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 ")) ;
+ if (npt < 4) {
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "No enough point "));
}
- switch (method)
- {
- case DEMENTHON :
- case DEMENTHON_VIRTUAL_VS :
- case DEMENTHON_LOWE :
- {
- if (npt <4)
- {
- vpERROR_TRACE("Dementhon method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
+ switch (method) {
+ case DEMENTHON:
+ case DEMENTHON_VIRTUAL_VS:
+ case DEMENTHON_LOWE: {
+ if (npt < 4) {
+ vpERROR_TRACE("Dementhon method cannot be used in that case ");
+ vpERROR_TRACE("(at least 4 points are required)");
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
+ }
- // test si les point 3D sont coplanaires
- int coplanar_plane_type = 0;
- bool plan = coplanar(coplanar_plane_type) ;
- if (plan == true)
- {
- //std::cout << "Plan" << std::endl;
- try{
- poseDementhonPlan(cMo);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
- //std::cout << "Fin Plan" << std::endl;
+ // test si les point 3D sont coplanaires
+ int coplanar_plane_type = 0;
+ bool plan = coplanar(coplanar_plane_type);
+ if (plan == true) {
+ // std::cout << "Plan" << std::endl;
+ try {
+ poseDementhonPlan(cMo);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
- else
- {
- //std::cout << "No Plan" << std::endl;
- try{
- poseDementhonNonPlan(cMo) ;
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
- //std::cout << "Fin No Plan" << std::endl;
+ // std::cout << "Fin Plan" << std::endl;
+ } else {
+ // std::cout << "No Plan" << std::endl;
+ try {
+ poseDementhonNonPlan(cMo);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
+ // std::cout << "Fin No Plan" << std::endl;
}
- break ;
- case LAGRANGE :
- case LAGRANGE_VIRTUAL_VS :
- case LAGRANGE_LOWE :
+ } break;
+ case LAGRANGE:
+ case LAGRANGE_VIRTUAL_VS:
+ case LAGRANGE_LOWE: {
+ // test si les point 3D sont coplanaires
+ int coplanar_plane_type;
+ bool plan = coplanar(coplanar_plane_type);
+
+ if (plan == true && coplanar_plane_type > 0) // only plane oX=d, oY=d or oZ=d
{
- // test si les point 3D sont coplanaires
- int coplanar_plane_type;
- bool plan = coplanar(coplanar_plane_type) ;
-
- if (plan == true && coplanar_plane_type > 0) // only plane oX=d, oY=d or oZ=d
- {
-
- if (coplanar_plane_type == 4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(points are collinear)") ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Points are collinear ")) ;
- }
- if (npt <4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- try {
- poseLagrangePlan(cMo, coplanar_plane_type);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
+
+ 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 "));
}
- else
- {
- if (npt <4)
- {
- vpERROR_TRACE("Lagrange method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
- }
- try {
- poseLagrangeNonPlan(cMo);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
+ if (npt < 4) {
+ vpERROR_TRACE("Lagrange method cannot be used in that case ");
+ vpERROR_TRACE("(at least 4 points are required)");
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
+ }
+ try {
+ poseLagrangePlan(cMo, coplanar_plane_type);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
+ }
+ } else {
+ if (npt < 4) {
+ vpERROR_TRACE("Lagrange method cannot be used in that case ");
+ vpERROR_TRACE("(at least 4 points are required)");
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
+ }
+ try {
+ poseLagrangeNonPlan(cMo);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
}
- break;
+ } break;
case RANSAC:
- if (npt <4)
- {
- vpERROR_TRACE("Ransac method cannot be used in that case ") ;
- vpERROR_TRACE("(at least 4 points are required)") ;
- vpERROR_TRACE("Not enough point (%d) to compute the pose ",npt) ;
- throw(vpPoseException(vpPoseException::notEnoughPointError,
- "Not enough points ")) ;
+ if (npt < 4) {
+ vpERROR_TRACE("Ransac method cannot be used in that case ");
+ vpERROR_TRACE("(at least 4 points are required)");
+ vpERROR_TRACE("Not enough point (%d) to compute the pose ", npt);
+ throw(vpPoseException(vpPoseException::notEnoughPointError, "Not enough points "));
}
try {
return poseRansac(cMo, func);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
break;
- case LOWE :
+ case LOWE:
case VIRTUAL_VS:
- break ;
+ break;
}
- switch (method)
- {
- case LAGRANGE :
- case DEMENTHON :
- case RANSAC :
- break ;
+ switch (method) {
+ case LAGRANGE:
+ case DEMENTHON:
+ case RANSAC:
+ break;
case VIRTUAL_VS:
case LAGRANGE_VIRTUAL_VS:
- case DEMENTHON_VIRTUAL_VS:
- {
- try
- {
- poseVirtualVS(cMo);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
+ case DEMENTHON_VIRTUAL_VS: {
+ try {
+ poseVirtualVS(cMo);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
- break ;
+ } break;
case LOWE:
case LAGRANGE_LOWE:
- case DEMENTHON_LOWE:
- {
- try
- {
- poseLowe(cMo);
- }
- catch(...)
- {
-// vpERROR_TRACE(" ") ;
- throw ;
- }
+ case DEMENTHON_LOWE: {
+ try {
+ poseLowe(cMo);
+ } catch (...) {
+ // vpERROR_TRACE(" ") ;
+ throw;
}
- break ;
+ } break;
}
- //If here, there was no exception thrown so return true
+ // If here, there was no exception thrown so return true
return true;
}
-
-
-void
-vpPose::printPoint()
+void vpPose::printPoint()
{
vpPoint P;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
+ P = *it;
- std::cout << "3D oP " << P.oP.t() ;
- std::cout << "3D cP " << P.cP.t() ;
- std::cout << "2D " << P.p.t() ;
+ std::cout << "3D oP " << P.oP.t();
+ std::cout << "3D cP " << P.cP.t();
+ std::cout << "2D " << P.p.t();
}
}
/*!
- Display in the image \e I the pose represented by its homogenous transformation \e cMo as a 3 axis frame.
- \param I: Image where the pose is displayed in overlay.
- \param cMo: Considered pose to display.
- \param cam: Camera parameters associated to image \e I.
- \param size: length in meter of the axis that will be displayed
- \param col: Color used to display the 3 axis. If vpColor::none, red, green and blue will represent
- x-axiw, y-axis and z-axis respectively.
+ Display in the image \e I the pose represented by its homogenous
+ transformation \e cMo as a 3 axis frame. \param I: Image where the pose is
+ displayed in overlay. \param cMo: Considered pose to display. \param cam:
+ Camera parameters associated to image \e I. \param size: length in meter of
+ the axis that will be displayed \param col: Color used to display the 3
+ axis. If vpColor::none, red, green and blue will represent x-axiw, y-axis
+ and z-axis respectively.
*/
-void
-vpPose::display(vpImage<unsigned char> &I,
- vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam,
- double size,
- vpColor col)
+void vpPose::display(vpImage<unsigned char> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size,
+ vpColor col)
{
vpDisplay::displayFrame(I, cMo, cam, size, col);
}
/*!
- Display in the image \e I the pose represented by its homogenous transformation \e cMo as a 3 axis frame.
- \param I: Image where the pose is displayed in overlay.
- \param cMo: Considered pose to display.
- \param cam: Camera parameters associated to image \e I.
- \param size: length in meter of the axis that will be displayed
- \param col: Color used to display the 3 axis. If vpColor::none, red, green and blue will represent
- x-axiw, y-axis and z-axis respectively.
+ Display in the image \e I the pose represented by its homogenous
+ transformation \e cMo as a 3 axis frame. \param I: Image where the pose is
+ displayed in overlay. \param cMo: Considered pose to display. \param cam:
+ Camera parameters associated to image \e I. \param size: length in meter of
+ the axis that will be displayed \param col: Color used to display the 3
+ axis. If vpColor::none, red, green and blue will represent x-axiw, y-axis
+ and z-axis respectively.
*/
-void
-vpPose::display(vpImage<vpRGBa> &I,
- vpHomogeneousMatrix &cMo,
- vpCameraParameters &cam,
- double size,
- vpColor col)
+void vpPose::display(vpImage<vpRGBa> &I, vpHomogeneousMatrix &cMo, vpCameraParameters &cam, double size, vpColor col)
{
- vpDisplay::displayFrame(I,cMo,cam, size,col);
+ vpDisplay::displayFrame(I, cMo, cam, size, col);
}
/*!
- Display the coordinates of the points in the image plane that are used to compute the pose in image I.
+ Display the coordinates of the points in the image plane that are used to
+ compute the pose in image I.
*/
-void
-vpPose::displayModel(vpImage<unsigned char> &I,
- vpCameraParameters &cam,
- vpColor col)
-{
- vpPoint P ;
+void vpPose::displayModel(vpImage<unsigned char> &I, vpCameraParameters &cam, vpColor col)
+{
+ vpPoint P;
vpImagePoint ip;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
- vpDisplay::displayCross(I, ip, 5, col) ;
+ vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
+ vpDisplay::displayCross(I, ip, 5, col);
// std::cout << "3D oP " << P.oP.t() ;
// std::cout << "3D cP " << P.cP.t() ;
// std::cout << "2D " << P.p.t() ;
@@ -621,27 +552,23 @@ vpPose::displayModel(vpImage<unsigned char> &I,
}
/*!
- Display the coordinates of the points in the image plane that are used to compute the pose in image I.
+ Display the coordinates of the points in the image plane that are used to
+ compute the pose in image I.
*/
-void
-vpPose::displayModel(vpImage<vpRGBa> &I,
- vpCameraParameters &cam,
- vpColor col)
-{
- vpPoint P ;
+void vpPose::displayModel(vpImage<vpRGBa> &I, vpCameraParameters &cam, vpColor col)
+{
+ vpPoint P;
vpImagePoint ip;
- for(std::list<vpPoint>::const_iterator it=listP.begin(); it != listP.end(); ++it)
- {
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip) ;
- vpDisplay::displayCross(I, ip, 5, col) ;
+ vpMeterPixelConversion::convertPoint(cam, P.p[0], P.p[1], ip);
+ vpDisplay::displayCross(I, ip, 5, col);
// std::cout << "3D oP " << P.oP.t() ;
// std::cout << "3D cP " << P.cP.t() ;
// std::cout << "2D " << P.p.t() ;
}
}
-
/*!
\brief Carries out the camera pose the image of a rectangle and
the intrinsec parameters, the length on x axis is known but the
@@ -661,76 +588,73 @@ 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,
- vpHomogeneousMatrix & cMo)
+double vpPose::poseFromRectangle(vpPoint &p1, vpPoint &p2, vpPoint &p3, vpPoint &p4, double lx, vpCameraParameters &cam,
+ vpHomogeneousMatrix &cMo)
{
- std::vector<double> rectx(4) ;
- std::vector<double> recty(4) ;
- rectx[0]= 0 ;
- recty[0]=0 ;
- rectx[1]=1 ;
- recty[1]=0 ;
- rectx[2]=1 ;
- recty[2]=1 ;
- rectx[3]=0 ;
- recty[3]=1 ;
- std::vector<double> irectx(4) ;
- std::vector<double> irecty(4) ;
- irectx[0]=(p1.get_x()) ;
- irecty[0]=(p1.get_y()) ;
- irectx[1]=(p2.get_x()) ;
- irecty[1]=(p2.get_y()) ;
- irectx[2]=(p3.get_x()) ;
- irecty[2]=(p3.get_y()) ;
- irectx[3]=(p4.get_x()) ;
- irecty[3]=(p4.get_y()) ;
-
- //calcul de l'homographie
- vpMatrix H(3,3);
+ std::vector<double> rectx(4);
+ std::vector<double> recty(4);
+ rectx[0] = 0;
+ recty[0] = 0;
+ rectx[1] = 1;
+ recty[1] = 0;
+ rectx[2] = 1;
+ recty[2] = 1;
+ rectx[3] = 0;
+ recty[3] = 1;
+ std::vector<double> irectx(4);
+ std::vector<double> irecty(4);
+ irectx[0] = (p1.get_x());
+ irecty[0] = (p1.get_y());
+ irectx[1] = (p2.get_x());
+ irecty[1] = (p2.get_y());
+ irectx[2] = (p3.get_x());
+ irecty[2] = (p3.get_y());
+ irectx[3] = (p4.get_x());
+ irecty[3] = (p4.get_y());
+
+ // calcul de l'homographie
+ vpMatrix H(3, 3);
vpHomography hom;
// vpHomography::HartleyDLT(rectx,recty,irectx,irecty,hom);
- vpHomography::HLM(rectx,recty,irectx,irecty,1,hom);
- for (unsigned int i=0 ; i < 3 ; i++)
- for(unsigned int j=0 ; j < 3 ; j++)
- H[i][j] = hom[i][j] ;
- //calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y axis)
+ vpHomography::HLM(rectx, recty, irectx, irecty, 1, hom);
+ for (unsigned int i = 0; i < 3; i++)
+ for (unsigned int j = 0; j < 3; j++)
+ H[i][j] = hom[i][j];
+ // calcul de s = ||Kh1||/ ||Kh2|| =ratio (length on x axis/ length on y
+ // axis)
vpColVector kh1(3);
vpColVector kh2(3);
- vpMatrix K(3,3);
+ vpMatrix K(3, 3);
K = cam.get_K();
K.eye();
- vpMatrix Kinv =K.pseudoInverse();
+ vpMatrix Kinv = K.pseudoInverse();
- vpMatrix KinvH =Kinv*H;
- kh1=KinvH.getCol(0);
- kh2=KinvH.getCol(1);
+ vpMatrix KinvH = Kinv * H;
+ kh1 = KinvH.getCol(0);
+ kh2 = KinvH.getCol(1);
- double s= sqrt(kh1.sumSquare())/sqrt(kh2.sumSquare());
+ double s = sqrt(kh1.sumSquare()) / sqrt(kh2.sumSquare());
- vpMatrix D(3,3);
+ vpMatrix D(3, 3);
D.eye();
- D[1][1]=1/s;
- vpMatrix cHo=H*D;
+ D[1][1] = 1 / s;
+ vpMatrix cHo = H * D;
- //Calcul de la rotation et de la translation
+ // Calcul de la rotation et de la translation
// PoseFromRectangle(p1,p2,p3,p4,1/s,lx,cam,cMo );
- p1.setWorldCoordinates(0,0,0) ;
- p2.setWorldCoordinates(lx,0,0) ;
- p3.setWorldCoordinates(lx,lx/s,0) ;
- p4.setWorldCoordinates(0,lx/s,0) ;
-
-
- vpPose P ;
- P.addPoint(p1) ;
- P.addPoint(p2) ;
- P.addPoint(p3) ;
- P.addPoint(p4) ;
-
- P.computePose(vpPose::DEMENTHON_LOWE,cMo) ;
- return lx/s ;
+ p1.setWorldCoordinates(0, 0, 0);
+ p2.setWorldCoordinates(lx, 0, 0);
+ p3.setWorldCoordinates(lx, lx / s, 0);
+ p4.setWorldCoordinates(0, lx / s, 0);
+
+ vpPose P;
+ P.addPoint(p1);
+ P.addPoint(p2);
+ P.addPoint(p3);
+ P.addPoint(p4);
+
+ P.computePose(vpPose::DEMENTHON_LOWE, cMo);
+ return lx / s;
}
diff --git a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp
index d5c5ec2..af7839a 100644
--- a/modules/vision/src/pose-estimation/vpPoseDementhon.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseDementhon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,10 +37,8 @@
*
*****************************************************************************/
-
-
-#include <visp3/vision/vpPose.h>
#include <visp3/core/vpMath.h>
+#include <visp3/vision/vpPose.h>
#define DEBUG_LEVEL1 0
#define DEBUG_LEVEL2 0
@@ -58,175 +57,154 @@ Dementhon and Davis in their 1995 paper \cite Dementhon95.
*/
-void
-vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
+void vpPose::poseDementhonNonPlan(vpHomogeneousMatrix &cMo)
{
double normI = 0., normJ = 0.;
double Z0 = 0.;
- //double seuil=1.0;
- double f=1.;
+ // double seuil=1.0;
+ double f = 1.;
- vpPoint p0 = listP.front() ;
+ vpPoint p0 = listP.front();
c3d.clear();
vpPoint P;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = (*it);
- P.set_oX(P.get_oX()-p0.get_oX()) ;
- P.set_oY(P.get_oY()-p0.get_oY()) ;
- P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
- c3d.push_back(P) ;
+ P.set_oX(P.get_oX() - p0.get_oX());
+ P.set_oY(P.get_oY() - p0.get_oY());
+ P.set_oZ(P.get_oZ() - p0.get_oZ());
+ c3d.push_back(P);
}
- vpMatrix a(npt,3) ;
+ vpMatrix a(npt, 3);
- for (unsigned int i=0 ; i < npt ; i++)
- {
- a[i][0]=c3d[i].get_oX();
- a[i][1]=c3d[i].get_oY();
- a[i][2]=c3d[i].get_oZ();
+ for (unsigned int i = 0; i < npt; i++) {
+ a[i][0] = c3d[i].get_oX();
+ a[i][1] = c3d[i].get_oY();
+ a[i][2] = c3d[i].get_oZ();
}
- //std::cout << a << std::endl ;
+ // std::cout << a << std::endl ;
// calcul a^T a
- vpMatrix ata ;
- ata = a.t()*a ;
+ vpMatrix ata;
+ ata = a.t() * a;
// calcul (a^T a)^-1 par decomposition LU
- vpMatrix ata1 ;
- ata1 = ata.pseudoInverse(1e-6) ; //InverseByLU() ;
+ vpMatrix ata1;
+ ata1 = ata.pseudoInverse(1e-6); // InverseByLU() ;
- vpMatrix b ;
- b = (a*ata1).t() ;
+ vpMatrix b;
+ b = (a * ata1).t();
#if (DEBUG_LEVEL2)
{
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata<<std::endl ;
- std::cout << "ata1" << std::endl <<ata1<<std::endl ;
- std::cout<< " ata*ata1" << std::endl << ata*ata1 ;
- std::cout<< " b" << std::endl << (a*ata1).t() ;
-
+ std::cout << "a" << std::endl << a << std::endl;
+ std::cout << "ata" << std::endl << ata << std::endl;
+ std::cout << "ata1" << std::endl << ata1 << std::endl;
+ std::cout << " ata*ata1" << std::endl << ata * ata1;
+ std::cout << " b" << std::endl << (a * ata1).t();
}
#endif
// calcul de la premiere solution
- vpColVector eps(npt) ;
- eps =0 ;
+ vpColVector eps(npt);
+ eps = 0;
- int cpt = 0 ;
- vpColVector I, J, k ;
- I.resize(3) ;
- J.resize(3) ;
- k.resize(3) ;
+ int cpt = 0;
+ vpColVector I, J, k;
+ I.resize(3);
+ J.resize(3);
+ k.resize(3);
- while(cpt < 20)
- {
- I = 0 ;
- J = 0 ;
-
- vpColVector xprim(npt) ;
- vpColVector yprim(npt) ;
- for (unsigned int i=0;i<npt;i++)
- {
- xprim[i]=(1+ eps[i])*c3d[i].get_x() - c3d[0].get_x();
- yprim[i]=(1+ eps[i])*c3d[i].get_y() - c3d[0].get_y();
+ while (cpt < 20) {
+ I = 0;
+ J = 0;
+
+ vpColVector xprim(npt);
+ vpColVector yprim(npt);
+ for (unsigned int i = 0; i < npt; i++) {
+ xprim[i] = (1 + eps[i]) * c3d[i].get_x() - c3d[0].get_x();
+ yprim[i] = (1 + eps[i]) * c3d[i].get_y() - c3d[0].get_y();
}
- I = b*xprim ;
- J = b*yprim ;
- normI = sqrt(I.sumSquare()) ;
- normJ = sqrt(J.sumSquare()) ;
- I = I/normI ;
- J = J/normJ ;
-
- if (normI+normJ < 1e-10)
- {
- //vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
+ I = b * xprim;
+ J = b * yprim;
+ normI = sqrt(I.sumSquare());
+ normJ = sqrt(J.sumSquare());
+ I = I / normI;
+ J = J / normJ;
+
+ if (normI + normJ < 1e-10) {
+ // vpERROR_TRACE(" normI+normJ = 0, division par zero " ) ;
throw(vpException(vpException::divideByZeroError,
- "Division by zero in Dementhon pose computation: normI+normJ = 0")) ;
+ "Division by zero in Dementhon pose computation: normI+normJ = 0"));
}
- k = vpColVector::cross(I,J) ;
- Z0=2*f/(normI+normJ);
- cpt=cpt+1; //seuil=0.0;
- for (unsigned int i=0; i<npt; i++)
- {
- //double epsi_1 = eps[i] ;
- eps[i]=(c3d[i].get_oX()*k[0]+c3d[i].get_oY()*k[1]+c3d[i].get_oZ()*k[2])/Z0;
- //seuil+=fabs(eps[i]-epsi_1);
+ k = vpColVector::cross(I, J);
+ Z0 = 2 * f / (normI + normJ);
+ cpt = cpt + 1; // seuil=0.0;
+ for (unsigned int i = 0; i < npt; i++) {
+ // double epsi_1 = eps[i] ;
+ eps[i] = (c3d[i].get_oX() * k[0] + c3d[i].get_oY() * k[1] + c3d[i].get_oZ() * k[2]) / Z0;
+ // seuil+=fabs(eps[i]-epsi_1);
}
- if (npt==0)
- {
- //vpERROR_TRACE( " npt = 0, division par zero ");
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Dementhon pose computation: no points")) ;
+ if (npt == 0) {
+ // vpERROR_TRACE( " npt = 0, division par zero ");
+ throw(vpException(vpException::divideByZeroError, "Division by zero in Dementhon pose computation: no points"));
}
- //seuil/=npt;
+ // seuil/=npt;
}
k.normalize();
- J = vpColVector::cross(k,I) ;
+ J = vpColVector::cross(k, I);
/*matrice de passage*/
- cMo[0][0]=I[0];
- cMo[0][1]=I[1];
- cMo[0][2]=I[2];
- cMo[0][3]=c3d[0].get_x()*2/(normI+normJ);
+ cMo[0][0] = I[0];
+ cMo[0][1] = I[1];
+ cMo[0][2] = I[2];
+ cMo[0][3] = c3d[0].get_x() * 2 / (normI + normJ);
- cMo[1][0]=J[0];
- cMo[1][1]=J[1];
- cMo[1][2]=J[2];
- cMo[1][3]=c3d[0].get_y()*2/(normI+normJ);
+ cMo[1][0] = J[0];
+ cMo[1][1] = J[1];
+ cMo[1][2] = J[2];
+ cMo[1][3] = c3d[0].get_y() * 2 / (normI + normJ);
- cMo[2][0]=k[0];
- cMo[2][1]=k[1];
- cMo[2][2]=k[2];
- cMo[2][3]=Z0;
+ cMo[2][0] = k[0];
+ cMo[2][1] = k[1];
+ cMo[2][2] = k[2];
+ cMo[2][3] = Z0;
- cMo[0][3] -= (p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2]);
- cMo[1][3] -= (p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2]);
- cMo[2][3] -= (p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2]);
+ cMo[0][3] -= (p0.get_oX() * cMo[0][0] + p0.get_oY() * cMo[0][1] + p0.get_oZ() * cMo[0][2]);
+ cMo[1][3] -= (p0.get_oX() * cMo[1][0] + p0.get_oY() * cMo[1][1] + p0.get_oZ() * cMo[1][2]);
+ cMo[2][3] -= (p0.get_oX() * cMo[2][0] + p0.get_oY() * cMo[2][1] + p0.get_oZ() * cMo[2][2]);
}
+#define DMIN 0.01 /* distance min entre la cible et la camera */
+#define EPS 0.0000001
+#define EPS_DEM 0.001
-#define DMIN 0.01 /* distance min entre la cible et la camera */
-#define EPS 0.0000001
-#define EPS_DEM 0.001
-
-static void
-calculRTheta(double s, double c, double &r, double &theta)
+static void calculRTheta(double s, double c, double &r, double &theta)
{
- if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM))
- {
- r = sqrt(sqrt(s*s+c*c));
- theta = atan2(s,c)/2.0;
- }
- else
- {
- if (fabs(c) > fabs(s))
- {
+ if ((fabs(c) > EPS_DEM) || (fabs(s) > EPS_DEM)) {
+ r = sqrt(sqrt(s * s + c * c));
+ theta = atan2(s, c) / 2.0;
+ } else {
+ if (fabs(c) > fabs(s)) {
r = fabs(c);
if (c >= 0.0)
- theta = M_PI/2;
+ theta = M_PI / 2;
else
- theta = -M_PI/2;
- }
- else
- {
+ theta = -M_PI / 2;
+ } else {
r = fabs(s);
if (s >= 0.0)
- theta = M_PI/4.0;
+ theta = M_PI / 4.0;
else
- theta = -M_PI/4.0;
+ theta = -M_PI / 4.0;
}
}
}
-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)
@@ -234,51 +212,46 @@ void calculSolutionDementhon(double xi0, double yi0,
#endif
double normI, normJ, normk, Z0;
- vpColVector k(3);
+ vpColVector k(3);
// normalisation de I et J
- normI = sqrt(I.sumSquare()) ;
- normJ = sqrt(J.sumSquare()) ;
+ normI = sqrt(I.sumSquare());
+ normJ = sqrt(J.sumSquare());
- I/=normI;
- J/=normJ;
+ I /= normI;
+ J /= normJ;
+ k = vpColVector::cross(I, J); // k = I^I
- k = vpColVector::cross(I,J) ; // k = I^I
+ Z0 = 2.0 / (normI + normJ);
- Z0=2.0/(normI+normJ);
+ normk = sqrt(k.sumSquare());
+ k /= normk;
- normk = sqrt(k.sumSquare()) ;
- k /= normk ;
+ J = vpColVector::cross(k, I);
- J = vpColVector::cross(k,I) ;
+ // calcul de la matrice de passage
+ cMo[0][0] = I[0];
+ cMo[0][1] = I[1];
+ cMo[0][2] = I[2];
+ cMo[0][3] = xi0 * Z0;
- //calcul de la matrice de passage
- cMo[0][0]=I[0];
- cMo[0][1]=I[1];
- cMo[0][2]=I[2];
- cMo[0][3]=xi0*Z0;
-
- cMo[1][0]=J[0];
- cMo[1][1]=J[1];
- cMo[1][2]=J[2];
- cMo[1][3]=yi0*Z0;
-
- cMo[2][0]=k[0];
- cMo[2][1]=k[1];
- cMo[2][2]=k[2];
- cMo[2][3]=Z0;
+ cMo[1][0] = J[0];
+ cMo[1][1] = J[1];
+ cMo[1][2] = J[2];
+ cMo[1][3] = yi0 * Z0;
+ cMo[2][0] = k[0];
+ cMo[2][1] = k[1];
+ cMo[2][2] = k[2];
+ cMo[2][3] = Z0;
#if (DEBUG_LEVEL1)
std::cout << "end (Dementhon.cc)CalculSolutionDementhon() " << std::endl;
#endif
-
}
-int
-vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
- vpHomogeneousMatrix &cMo)
+int vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U, vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
@@ -287,174 +260,160 @@ vpPose::calculArbreDementhon(vpMatrix &b, vpColVector &U,
int erreur = 0;
double smin;
- vpHomogeneousMatrix cMo1,cMo2,cMo_old;
+ vpHomogeneousMatrix cMo1, cMo2, cMo_old;
unsigned int iter_max = 20;
- vpMatrix eps(iter_max+1,npt) ;
-
+ vpMatrix eps(iter_max + 1, npt);
// 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];
- if (z <= 0.0) erreur = -1;
+ 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];
+ if (z <= 0.0)
+ erreur = -1;
}
- smin = sqrt(computeResidualDementhon(cMo)/npt) ;
+ smin = sqrt(computeResidualDementhon(cMo) / npt);
- vpColVector xi(npt) ;
- vpColVector yi(npt) ;
+ vpColVector xi(npt);
+ vpColVector yi(npt);
- if (erreur==0)
- {
- unsigned int k=0;
- for(unsigned int i = 0; i < npt; i++)
- {
+ if (erreur == 0) {
+ unsigned int k = 0;
+ for (unsigned int i = 0; i < npt; i++) {
xi[k] = c3d[i].get_x();
yi[k] = c3d[i].get_y();
- if (k != 0)
- { // On ne prend pas le 1er point
- eps[0][k] = (cMo[2][0]*c3d[i].get_oX() +
- cMo[2][1]*c3d[i].get_oY() +
- cMo[2][2]*c3d[i].get_oZ())/cMo[2][3];
+ if (k != 0) { // On ne prend pas le 1er point
+ eps[0][k] =
+ (cMo[2][0] * c3d[i].get_oX() + cMo[2][1] * c3d[i].get_oY() + cMo[2][2] * c3d[i].get_oZ()) / cMo[2][3];
}
k++;
}
+ vpColVector I0(3);
+ vpColVector J0(3);
+ vpColVector I(3);
+ vpColVector J(3);
- vpColVector I0(3) ;
- vpColVector J0(3) ;
- vpColVector I(3) ;
- vpColVector J(3) ;
-
- double smin_old = 2*smin ;
+ double smin_old = 2 * smin;
unsigned int cpt = 0;
- while ((cpt<20) && (smin_old > 0.01) && (smin <= smin_old))
- {
+ while ((cpt < 20) && (smin_old > 0.01) && (smin <= smin_old)) {
double r, theta, s1, s2;
#if (DEBUG_LEVEL2)
{
- std::cout << "cpt " << cpt << std::endl ;
- std::cout << "smin_old " << smin_old << std::endl ;
- std::cout << "smin " << smin << std::endl ;
+ std::cout << "cpt " << cpt << std::endl;
+ std::cout << "smin_old " << smin_old << std::endl;
+ std::cout << "smin " << smin << std::endl;
}
#endif
smin_old = smin;
cMo_old = cMo;
- I0 = 0 ;
- J0 = 0 ;
-
- for (unsigned int i=1;i<npt;i++)
- {
- double s = (1.0+eps[cpt][i])*xi[i] - xi[0];
- I0[0] += b[0][i-1] * s;
- I0[1] += b[1][i-1] * s;
- I0[2] += b[2][i-1] * s;
- s = (1.0+eps[cpt][i])*yi[i] - yi[0];
- J0[0] += b[0][i-1] * s;
- J0[1] += b[1][i-1] * s;
- J0[2] += b[2][i-1] * s;
+ I0 = 0;
+ J0 = 0;
+
+ for (unsigned int i = 1; i < npt; i++) {
+ double s = (1.0 + eps[cpt][i]) * xi[i] - xi[0];
+ I0[0] += b[0][i - 1] * s;
+ I0[1] += b[1][i - 1] * s;
+ I0[2] += b[2][i - 1] * s;
+ s = (1.0 + eps[cpt][i]) * yi[i] - yi[0];
+ J0[0] += b[0][i - 1] * s;
+ J0[1] += b[1][i - 1] * s;
+ J0[2] += b[2][i - 1] * s;
}
- double s = -2.0*(vpColVector::dotProd(I0,J0));
- double c = J0.sumSquare() - I0.sumSquare() ;
+ double s = -2.0 * (vpColVector::dotProd(I0, J0));
+ double c = J0.sumSquare() - I0.sumSquare();
- calculRTheta(s,c,r,theta);
+ calculRTheta(s, c, r, theta);
double co = cos(theta);
double si = sin(theta);
/* 1ere branche */
- I = I0 + U*r*co ;
- J = J0 + U*r*si ;
+ I = I0 + U * r * co;
+ J = J0 + U * r * si;
#if (DEBUG_LEVEL3)
{
- std::cout << "I " << I.t() ;
- std::cout << "J " << J.t() ;
+ std::cout << "I " << I.t();
+ std::cout << "J " << J.t();
}
#endif
- calculSolutionDementhon(xi[0],yi[0],I,J,cMo1);
- s1 = sqrt(computeResidualDementhon(cMo1)/npt) ;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo1);
+ s1 = sqrt(computeResidualDementhon(cMo1) / npt);
#if (DEBUG_LEVEL3)
- std::cout << "cMo1 "<< std::endl << cMo1 << std::endl ;
+ std::cout << "cMo1 " << std::endl << cMo1 << std::endl;
#endif
/* 2eme branche */
- I = I0 - U*r*co ;
- J = J0 - U*r*si ;
+ I = I0 - U * r * co;
+ J = J0 - U * r * si;
#if (DEBUG_LEVEL3)
{
- std::cout << "I " << I.t() ;
- std::cout << "J " << J.t() ;
+ std::cout << "I " << I.t();
+ std::cout << "J " << J.t();
}
#endif
- calculSolutionDementhon(xi[0],yi[0],I,J,cMo2);
- s2 = sqrt(computeResidualDementhon(cMo2)/npt) ;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo2);
+ s2 = sqrt(computeResidualDementhon(cMo2) / npt);
#if (DEBUG_LEVEL3)
- std::cout << "cMo2 "<< std::endl << cMo2 << std::endl ;
+ std::cout << "cMo2 " << std::endl << cMo2 << std::endl;
#endif
- cpt ++;
- if (s1 <= s2)
- {
+ cpt++;
+ if (s1 <= s2) {
smin = s1;
k = 0;
- for(unsigned int i = 0; i < npt; i++)
- {
+ 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++;
}
- cMo = cMo1 ;
- }
- else
- {
+ cMo = cMo1;
+ } else {
smin = s2;
k = 0;
- for(unsigned int i = 0; i < npt; i++)
- {
+ 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++;
}
- cMo = cMo2 ;
+ cMo = cMo2;
}
- if (smin > smin_old)
- {
-#if (DEBUG_LEVEL2)
- std::cout << "Divergence " << std::endl ;
+ if (smin > smin_old) {
+#if (DEBUG_LEVEL2)
+ std::cout << "Divergence " << std::endl;
#endif
- cMo = cMo_old ;
+ cMo = cMo_old;
}
#if (DEBUG_LEVEL2)
{
- std::cout << "s1 = " << s1 << std::endl ;
- std::cout << "s2 = " << s2 << std::endl ;
- std::cout << "smin = " << smin << std::endl ;
- std::cout << "smin_old = " << smin_old << std::endl ;
+ std::cout << "s1 = " << s1 << std::endl;
+ std::cout << "s2 = " << s2 << std::endl;
+ std::cout << "smin = " << smin << std::endl;
+ std::cout << "smin_old = " << smin_old << std::endl;
}
#endif
}
}
#if (DEBUG_LEVEL1)
- std::cout << "end vpPose::CalculArbreDementhon() return "<< erreur << std::endl;
+ std::cout << "end vpPose::CalculArbreDementhon() return " << erreur << std::endl;
#endif
- return erreur ;
+ return erreur;
}
/*!
@@ -465,50 +424,43 @@ Dementhon in his PhD.
\author Francois Chaumette (simplified by Eric Marchand)
*/
-void
-vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
-{
+void vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
+{
#if (DEBUG_LEVEL1)
- std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl ;
+ std::cout << "begin CCalculPose::PoseDementhonPlan()" << std::endl;
#endif
- unsigned int i,j,k ;
+ unsigned int i, j, k;
- vpPoint p0 = listP.front() ;
+ vpPoint p0 = listP.front();
- vpPoint P ;
+ vpPoint P;
c3d.clear();
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- P.set_oX(P.get_oX()-p0.get_oX()) ;
- P.set_oY(P.get_oY()-p0.get_oY()) ;
- P.set_oZ(P.get_oZ()-p0.get_oZ()) ;
+ P.set_oX(P.get_oX() - p0.get_oX());
+ P.set_oY(P.get_oY() - p0.get_oY());
+ P.set_oZ(P.get_oZ() - p0.get_oZ());
c3d.push_back(P);
}
- vpMatrix a ;
- try
- {
- a.resize(npt-1,3) ;
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
+ vpMatrix a;
+ try {
+ a.resize(npt - 1, 3);
+ } catch (...) {
+ vpERROR_TRACE(" ");
+ throw;
}
-
- for (i=1 ; i < npt ; i++)
- {
- a[i-1][0]=c3d[i].get_oX();
- a[i-1][1]=c3d[i].get_oY();
- a[i-1][2]=c3d[i].get_oZ();
+ for (i = 1; i < npt; i++) {
+ a[i - 1][0] = c3d[i].get_oX();
+ a[i - 1][1] = c3d[i].get_oY();
+ a[i - 1][2] = c3d[i].get_oZ();
}
// calcul a^T a
- vpMatrix ata ;
- ata = a.t()*a ;
+ vpMatrix ata;
+ ata = a.t() * a;
/* essai FC pour debug SVD */
/*
@@ -524,149 +476,155 @@ vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
#if (DEBUG_LEVEL2)
{
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata<<std::endl ;
+ std::cout << "a" << std::endl << a << std::endl;
+ std::cout << "ata" << std::endl << ata << std::endl;
}
#endif
// calcul (a^T a)^-1
- vpMatrix ata1(ata.getRows(),ata.getCols()) ;
- vpMatrix v(ata.getRows(),ata.getCols());
+ vpMatrix ata1(ata.getRows(), ata.getCols());
+ vpMatrix v(ata.getRows(), ata.getCols());
vpColVector sv(ata.getRows());
// ata1 = ata.i() ;
unsigned int imin = 0;
double s = 0.0;
- //calcul de ata^-1
- ata.svd(sv,v) ;
+ // calcul de ata^-1
+ ata.svd(sv, v);
- unsigned int nc = sv.getRows() ;
- for (i=0; i < nc ; i++)
- if (sv[i] > s) s = sv[i];
+ unsigned int nc = sv.getRows();
+ for (i = 0; i < nc; i++)
+ if (sv[i] > s)
+ s = sv[i];
s *= 0.0002;
- int irank = 0;
- for (i=0;i<nc;i++)
- if (sv[i] > s ) irank++;
+ int irank = 0;
+ for (i = 0; i < nc; i++)
+ if (sv[i] > s)
+ irank++;
double svm = 100.0;
- for (i = 0; i < nc; i++)
- if (sv[i] < svm) { imin = i; svm = sv[i]; }
+ for (i = 0; i < nc; i++) {
+ if (sv[i] < svm) {
+ imin = i;
+ svm = sv[i];
+ }
+ }
#if (DEBUG_LEVEL2)
- {
- std::cout << "rang: " << irank << std::endl ;;
- std::cout <<"imin = " << imin << std::endl ;
- std::cout << "sv " << sv.t() << std::endl ;
- }
+ {
+ std::cout << "rang: " << irank << std::endl;
+ ;
+ std::cout << "imin = " << imin << std::endl;
+ std::cout << "sv " << sv.t() << std::endl;
+ }
#endif
- for (i=0 ; i < ata.getRows() ; i++)
- for (j=0 ; j < ata.getCols() ; j++)
- {
- ata1[i][j] = 0.0;
- for (k=0 ; k < nc ; k++)
- if (sv[k] > s)
- ata1[i][j] += ((v[i][k]*ata[j][k])/sv[k]);
- }
-
-
+ for (i = 0; i < ata.getRows(); i++) {
+ for (j = 0; j < ata.getCols(); j++) {
+ ata1[i][j] = 0.0;
+ for (k = 0; k < nc; k++)
+ if (sv[k] > s)
+ ata1[i][j] += ((v[i][k] * ata[j][k]) / sv[k]);
+ }
+ }
- vpMatrix b ; // b=(at a)^-1*at
- b = ata1*a.t() ;
+ vpMatrix b; // b=(at a)^-1*at
+ b = ata1 * a.t();
- //calcul de U
- vpColVector U(3) ;
- U = ata.getCol(imin) ;
+ // calcul de U
+ vpColVector U(3);
+ U = ata.getCol(imin);
#if (DEBUG_LEVEL2)
- {
- std::cout << "a" << std::endl <<a<<std::endl ;
- std::cout << "ata" << std::endl <<ata_sav<<std::endl ;
- std::cout << "ata1" << std::endl <<ata1<<std::endl ;
- std::cout << "ata1*ata" << std::endl << ata1*ata_sav ;
- std::cout << "b" << std::endl << b ;
- std::cout << "U " << U.t() << std::endl ;
- }
+ {
+ std::cout << "a" << std::endl << a << std::endl;
+ std::cout << "ata" << std::endl << ata_sav << std::endl;
+ std::cout << "ata1" << std::endl << ata1 << std::endl;
+ std::cout << "ata1*ata" << std::endl << ata1 * ata_sav;
+ std::cout << "b" << std::endl << b;
+ std::cout << "U " << U.t() << std::endl;
+ }
#endif
- vpColVector xi(npt) ;
- vpColVector yi(npt) ;
- //calcul de la premiere solution
- for (i = 0; i < npt; i++)
- {
- xi[i] = c3d[i].get_x() ;
- yi[i] = c3d[i].get_y() ;
-
- }
-
- vpColVector I0(3) ; I0 = 0 ;
- vpColVector J0(3) ; J0 = 0 ;
- vpColVector I(3) ;
- vpColVector J(3) ;
-
- for (i=1;i<npt;i++)
- {
- I0[0] += b[0][i-1] * (xi[i]-xi[0]);
- I0[1] += b[1][i-1] * (xi[i]-xi[0]);
- I0[2] += b[2][i-1] * (xi[i]-xi[0]);
-
- J0[0] += b[0][i-1] * (yi[i]-yi[0]);
- J0[1] += b[1][i-1] * (yi[i]-yi[0]);
- J0[2] += b[2][i-1] * (yi[i]-yi[0]);
- }
+ vpColVector xi(npt);
+ vpColVector yi(npt);
+ // calcul de la premiere solution
+ for (i = 0; i < npt; i++) {
+ xi[i] = c3d[i].get_x();
+ yi[i] = c3d[i].get_y();
+ }
+ vpColVector I0(3);
+ I0 = 0;
+ vpColVector J0(3);
+ J0 = 0;
+ vpColVector I(3);
+ vpColVector J(3);
+
+ for (i = 1; i < npt; i++) {
+ I0[0] += b[0][i - 1] * (xi[i] - xi[0]);
+ I0[1] += b[1][i - 1] * (xi[i] - xi[0]);
+ I0[2] += b[2][i - 1] * (xi[i] - xi[0]);
+
+ J0[0] += b[0][i - 1] * (yi[i] - yi[0]);
+ J0[1] += b[1][i - 1] * (yi[i] - yi[0]);
+ J0[2] += b[2][i - 1] * (yi[i] - yi[0]);
+ }
#if (DEBUG_LEVEL2)
- {
- std::cout << "I0 "<<I0.t() ;
- std::cout << "J0 "<<J0.t() ;
- }
+ {
+ std::cout << "I0 " << I0.t();
+ std::cout << "J0 " << J0.t();
+ }
#endif
- s = -2.0*vpColVector::dotProd(I0,J0);
- double c = J0.sumSquare() - I0.sumSquare() ;
+ s = -2.0 * vpColVector::dotProd(I0, J0);
+ double c = J0.sumSquare() - I0.sumSquare();
- double r,theta,si,co ;
- calculRTheta(s, c, r, theta);
- co = cos(theta);
- si = sin(theta);
+ double r, theta, si, co;
+ calculRTheta(s, c, r, theta);
+ co = cos(theta);
+ si = sin(theta);
- // calcul de la premiere solution
- I = I0 + U*r*co ;
- J = J0 + U*r*si ;
-
- vpHomogeneousMatrix cMo1f ;
- calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
+ // calcul de la premiere solution
+ I = I0 + U * r * co;
+ J = J0 + U * r * si;
+ vpHomogeneousMatrix cMo1f;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo1f);
- int erreur1 = calculArbreDementhon(b, U, cMo1f);
+ int erreur1 = calculArbreDementhon(b, U, cMo1f);
- // calcul de la deuxieme solution
- I = I0 - U*r*co ;
- J = J0 - U*r*si ;
+ // calcul de la deuxieme solution
+ I = I0 - U * r * co;
+ J = J0 - U * r * si;
- vpHomogeneousMatrix cMo2f;
- calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
+ vpHomogeneousMatrix cMo2f;
+ calculSolutionDementhon(xi[0], yi[0], I, J, cMo2f);
- int erreur2 = calculArbreDementhon(b, U, cMo2f);
+ int erreur2 = calculArbreDementhon(b, U, cMo2f);
- if ((erreur1 == 0) && (erreur2 == -1)) cMo = cMo1f ;
- if ((erreur1 == -1) && (erreur2 == 0)) cMo = cMo2f ;
- if ((erreur1 == 0) && (erreur2 == 0))
- {
- double s1 = sqrt(computeResidualDementhon(cMo1f)/npt) ;
- double s2 = sqrt(computeResidualDementhon(cMo2f)/npt) ;
+ if ((erreur1 == 0) && (erreur2 == -1))
+ cMo = cMo1f;
+ if ((erreur1 == -1) && (erreur2 == 0))
+ cMo = cMo2f;
+ if ((erreur1 == 0) && (erreur2 == 0)) {
+ double s1 = sqrt(computeResidualDementhon(cMo1f) / npt);
+ double s2 = sqrt(computeResidualDementhon(cMo2f) / npt);
- if (s1<=s2) cMo = cMo1f ; else cMo = cMo2f ;
- }
+ if (s1 <= s2)
+ cMo = cMo1f;
+ else
+ cMo = cMo2f;
+ }
- cMo[0][3] -= p0.get_oX()*cMo[0][0]+p0.get_oY()*cMo[0][1]+p0.get_oZ()*cMo[0][2];
- cMo[1][3] -= p0.get_oX()*cMo[1][0]+p0.get_oY()*cMo[1][1]+p0.get_oZ()*cMo[1][2];
- cMo[2][3] -= p0.get_oX()*cMo[2][0]+p0.get_oY()*cMo[2][1]+p0.get_oZ()*cMo[2][2];
+ 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 ;
+ std::cout << "end CCalculPose::PoseDementhonPlan()" << std::endl;
#endif
}
@@ -674,7 +632,6 @@ vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
#undef EPS
#undef EPS_DEM
-
/*!
\brief Compute and return the residual expressed in meter for the pose matrix
'pose'.
@@ -685,26 +642,23 @@ vpPose::poseDementhonPlan(vpHomogeneousMatrix &cMo)
*/
double vpPose::computeResidualDementhon(const vpHomogeneousMatrix &cMo)
{
- double residual_ = 0 ;
+ double residual_ = 0;
- residual_ =0 ;
- for (unsigned int i =0 ; i < npt ; i++)
- {
+ 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 ;
+ 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_ ;
+ return residual_;
}
-
#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2
#undef DEBUG_LEVEL3
-
diff --git a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp
index 19735a9..35eb36a 100644
--- a/modules/vision/src/pose-estimation/vpPoseFeatures.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseFeatures.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -52,219 +53,225 @@ vpPoseFeatures::vpPoseFeatures()
/*!
Destructor that deletes the array of features and projections.
*/
-vpPoseFeatures::~vpPoseFeatures()
-{
- clear();
-}
+vpPoseFeatures::~vpPoseFeatures() { clear(); }
/*!
Clear all the features
*/
void vpPoseFeatures::clear()
-{
- for(int i = (int)featurePoint_Point_list.size()-1 ; i >= 0 ; i--)
+{
+ for (int i = (int)featurePoint_Point_list.size() - 1; i >= 0; i--)
delete featurePoint_Point_list[(unsigned int)i].desiredFeature;
featurePoint_Point_list.clear();
-
- for(int i = (int)featurePoint3D_Point_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featurePoint3D_Point_list.size() - 1; i >= 0; i--)
delete featurePoint3D_Point_list[(unsigned int)i].desiredFeature;
featurePoint3D_Point_list.clear();
-
- for(int i = (int)featureVanishingPoint_Point_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureVanishingPoint_Point_list.size() - 1; i >= 0; i--)
delete featureVanishingPoint_Point_list[(unsigned int)i].desiredFeature;
featureVanishingPoint_Point_list.clear();
-
- for(int i = (int)featureVanishingPoint_DuoLine_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureVanishingPoint_DuoLine_list.size() - 1; i >= 0; i--)
delete featureVanishingPoint_DuoLine_list[(unsigned int)i].desiredFeature;
featureVanishingPoint_DuoLine_list.clear();
-
- for(int i = (int)featureEllipse_Sphere_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureEllipse_Sphere_list.size() - 1; i >= 0; i--)
delete featureEllipse_Sphere_list[(unsigned int)i].desiredFeature;
featureEllipse_Sphere_list.clear();
-
- for(int i = (int)featureEllipse_Circle_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureEllipse_Circle_list.size() - 1; i >= 0; i--)
delete featureEllipse_Circle_list[(unsigned int)i].desiredFeature;
featureEllipse_Circle_list.clear();
-
- for(int i = (int)featureLine_Line_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureLine_Line_list.size() - 1; i >= 0; i--)
delete featureLine_Line_list[(unsigned int)i].desiredFeature;
featureLine_Line_list.clear();
-
- for(int i = (int)featureLine_DuoLineInt_List.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureLine_DuoLineInt_List.size() - 1; i >= 0; i--)
delete featureLine_DuoLineInt_List[(unsigned int)i].desiredFeature;
featureLine_DuoLineInt_List.clear();
-
- for(int i = (int)featureSegment_DuoPoints_list.size()-1 ; i >= 0 ; i--)
+
+ for (int i = (int)featureSegment_DuoPoints_list.size() - 1; i >= 0; i--)
delete featureSegment_DuoPoints_list[(unsigned int)i].desiredFeature;
featureSegment_DuoPoints_list.clear();
-
+
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
- for(int i = (int)featureSpecific_list.size()-1 ; i >= 0 ; i--)
+ for (int i = (int)featureSpecific_list.size() - 1; i >= 0; i--)
delete featureSpecific_list[(unsigned int)i];
featureSpecific_list.clear();
#endif
-
- maxSize = 0;
- totalSize = 0;
+
+ maxSize = 0;
+ totalSize = 0;
}
/*!
- Add a point feature to the list of features to be considered in the pose computation.
+ Add a point feature to the list of features to be considered in the pose
+ computation.
\param p : Point projection expressed as a vpPoint.
*/
void vpPoseFeatures::addFeaturePoint(const vpPoint &p)
{
- featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint,vpPoint>());
+ featurePoint_Point_list.push_back(vpDuo<vpFeaturePoint, vpPoint>());
featurePoint_Point_list.back().firstParam = p;
featurePoint_Point_list.back().desiredFeature = new vpFeaturePoint();
- vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature,p);
-
+ vpFeatureBuilder::create(*featurePoint_Point_list.back().desiredFeature, p);
+
totalSize++;
- if(featurePoint_Point_list.size() > maxSize)
+ if (featurePoint_Point_list.size() > maxSize)
maxSize = (unsigned int)featurePoint_Point_list.size();
}
/*!
- Add a point 3D feature to the list of features to be considered in the pose computation.
+ Add a point 3D feature to the list of features to be considered in the pose
+ computation.
\param p : Projection expressed as a vpPoint.
*/
void vpPoseFeatures::addFeaturePoint3D(const vpPoint &p)
-{
- featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D,vpPoint>());
+{
+ featurePoint3D_Point_list.push_back(vpDuo<vpFeaturePoint3D, vpPoint>());
featurePoint3D_Point_list.back().firstParam = p;
featurePoint3D_Point_list.back().desiredFeature = new vpFeaturePoint3D();
- vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature,p);
-
+ vpFeatureBuilder::create(*featurePoint3D_Point_list.back().desiredFeature, p);
+
totalSize++;
- if(featurePoint3D_Point_list.size() > maxSize)
+ if (featurePoint3D_Point_list.size() > maxSize)
maxSize = (unsigned int)featurePoint3D_Point_list.size();
}
/*!
- Add a vanishing point feature to the list of features to be considered in the pose computation.
+ Add a vanishing point feature to the list of features to be considered in
+ the pose computation.
\param p : Projection expressed as a vpPoint.
*/
void vpPoseFeatures::addFeatureVanishingPoint(const vpPoint &p)
-{
- featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint,vpPoint>());
+{
+ featureVanishingPoint_Point_list.push_back(vpDuo<vpFeatureVanishingPoint, vpPoint>());
featureVanishingPoint_Point_list.back().firstParam = p;
featureVanishingPoint_Point_list.back().desiredFeature = new vpFeatureVanishingPoint();
- vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature,p);
-
+ vpFeatureBuilder::create(*featureVanishingPoint_Point_list.back().desiredFeature, p);
+
totalSize++;
- if(featureVanishingPoint_Point_list.size() > maxSize)
+ if (featureVanishingPoint_Point_list.size() > maxSize)
maxSize = (unsigned int)featureVanishingPoint_Point_list.size();
}
/*!
- Add a vanishing point feature to the list of features to be considered in the pose computation.
+ Add a vanishing point feature to the list of features to be considered in
+ the pose computation.
\param l1 : First line used to create the feature.
\param l2 : Second line used to create the feature.
*/
void vpPoseFeatures::addFeatureVanishingPoint(const vpLine &l1, const vpLine &l2)
-{
- featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint,vpLine,vpLine>());
+{
+ featureVanishingPoint_DuoLine_list.push_back(vpTrio<vpFeatureVanishingPoint, vpLine, vpLine>());
featureVanishingPoint_DuoLine_list.back().firstParam = l1;
featureVanishingPoint_DuoLine_list.back().secondParam = l2;
featureVanishingPoint_DuoLine_list.back().desiredFeature = new vpFeatureVanishingPoint();
- vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature,l1,l2);
-
+ vpFeatureBuilder::create(*featureVanishingPoint_DuoLine_list.back().desiredFeature, l1, l2);
+
totalSize++;
- if(featureVanishingPoint_DuoLine_list.size() > maxSize)
+ if (featureVanishingPoint_DuoLine_list.size() > maxSize)
maxSize = (unsigned int)featureVanishingPoint_DuoLine_list.size();
}
/*!
- Add an ellipse feature to the list of features to be considered in the pose computation.
+ Add an ellipse feature to the list of features to be considered in the pose
+ computation.
\param s : Ellipse projection expressed as a vpSphere.
*/
void vpPoseFeatures::addFeatureEllipse(const vpSphere &s)
-{
- featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse,vpSphere>());
+{
+ featureEllipse_Sphere_list.push_back(vpDuo<vpFeatureEllipse, vpSphere>());
featureEllipse_Sphere_list.back().firstParam = s;
featureEllipse_Sphere_list.back().desiredFeature = new vpFeatureEllipse();
- vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature,s);
-
+ vpFeatureBuilder::create(*featureEllipse_Sphere_list.back().desiredFeature, s);
+
totalSize++;
- if(featureEllipse_Sphere_list.size() > maxSize)
+ if (featureEllipse_Sphere_list.size() > maxSize)
maxSize = (unsigned int)featureEllipse_Sphere_list.size();
}
/*!
- Add an ellipse feature to the list of features to be considered in the pose computation.
+ Add an ellipse feature to the list of features to be considered in the pose
+ computation.
\param c : Ellipse projection expressed as a vpCircle.
*/
void vpPoseFeatures::addFeatureEllipse(const vpCircle &c)
-{
- featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse,vpCircle>());
+{
+ featureEllipse_Circle_list.push_back(vpDuo<vpFeatureEllipse, vpCircle>());
featureEllipse_Circle_list.back().firstParam = c;
featureEllipse_Circle_list.back().desiredFeature = new vpFeatureEllipse();
- vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature,c);
-
+ vpFeatureBuilder::create(*featureEllipse_Circle_list.back().desiredFeature, c);
+
totalSize++;
- if(featureEllipse_Circle_list.size() > maxSize)
+ if (featureEllipse_Circle_list.size() > maxSize)
maxSize = (unsigned int)featureEllipse_Circle_list.size();
}
-
+
/*!
- Add a line feature to the list of features to be considered in the pose computation.
+ Add a line feature to the list of features to be considered in the pose
+ computation.
\param l : Line projection expressed as a vpLine.
*/
void vpPoseFeatures::addFeatureLine(const vpLine &l)
-{
- featureLine_Line_list.push_back(vpDuo<vpFeatureLine,vpLine>());
+{
+ featureLine_Line_list.push_back(vpDuo<vpFeatureLine, vpLine>());
featureLine_Line_list.back().firstParam = l;
featureLine_Line_list.back().desiredFeature = new vpFeatureLine();
- vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature,l);
-
+ vpFeatureBuilder::create(*featureLine_Line_list.back().desiredFeature, l);
+
totalSize++;
- if(featureLine_Line_list.size() > maxSize)
+ if (featureLine_Line_list.size() > maxSize)
maxSize = (unsigned int)featureLine_Line_list.size();
}
/*!
- Add a line feature to the list of features to be considered in the pose computation.
+ Add a line feature to the list of features to be considered in the pose
+ computation.
\param c : Line projection expressed as a vpCylinder.
- \param line : Integer id that indicates which limb of the cylinder is to consider.
- It can be vpCylinder::line1 or vpCylinder::line2.
+ \param line : Integer id that indicates which limb of the cylinder is to
+ consider. It can be vpCylinder::line1 or vpCylinder::line2.
*/
void vpPoseFeatures::addFeatureLine(const vpCylinder &c, const int &line)
-{
- featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine,vpCylinder,int>());
+{
+ featureLine_DuoLineInt_List.push_back(vpTrio<vpFeatureLine, vpCylinder, int>());
featureLine_DuoLineInt_List.back().firstParam = c;
featureLine_DuoLineInt_List.back().secondParam = line;
featureLine_DuoLineInt_List.back().desiredFeature = new vpFeatureLine();
- vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature,c,line);
-
+ vpFeatureBuilder::create(*featureLine_DuoLineInt_List.back().desiredFeature, c, line);
+
totalSize++;
- if(featureLine_DuoLineInt_List.size() > maxSize)
+ if (featureLine_DuoLineInt_List.size() > maxSize)
maxSize = (unsigned int)featureLine_DuoLineInt_List.size();
}
/*!
- Add a segment feature to the list of features to be considered in the pose computation.
+ Add a segment feature to the list of features to be considered in the pose
+ computation.
\param P1 : First extremity projection.
\param P2 : Second extremity projection.
*/
void vpPoseFeatures::addFeatureSegment(vpPoint &P1, vpPoint &P2)
-{
- featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment,vpPoint,vpPoint>());
+{
+ featureSegment_DuoPoints_list.push_back(vpTrio<vpFeatureSegment, vpPoint, vpPoint>());
featureSegment_DuoPoints_list.back().firstParam = P1;
featureSegment_DuoPoints_list.back().secondParam = P2;
featureSegment_DuoPoints_list.back().desiredFeature = new vpFeatureSegment();
- vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature,P1,P2);
-
+ vpFeatureBuilder::create(*featureSegment_DuoPoints_list.back().desiredFeature, P1, P2);
+
totalSize++;
- if(featureSegment_DuoPoints_list.size() > maxSize)
+ if (featureSegment_DuoPoints_list.size() > maxSize)
maxSize = (unsigned int)featureSegment_DuoPoints_list.size();
}
@@ -275,16 +282,15 @@ 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();
-
- for(unsigned int i = 0 ; i < maxSize ; i++)
- {
+
+ for (unsigned int i = 0; i < maxSize; i++) {
//--------------vpFeaturePoint--------------
- //From vpPoint
- if( i < featurePoint_Point_list.size() ){
+ // From vpPoint
+ if (i < featurePoint_Point_list.size()) {
vpFeaturePoint fp;
vpPoint p(featurePoint_Point_list[i].firstParam);
p.track(cMo);
@@ -292,10 +298,10 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fp.error(*(featurePoint_Point_list[i].desiredFeature)));
L.stack(fp.interaction());
}
-
+
//--------------vpFeaturePoint3D--------------
- //From vpPoint
- if( i < featurePoint3D_Point_list.size() ){
+ // From vpPoint
+ if (i < featurePoint3D_Point_list.size()) {
vpFeaturePoint3D fp3D;
vpPoint p(featurePoint3D_Point_list[i].firstParam);
p.track(cMo);
@@ -303,10 +309,10 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fp3D.error(*(featurePoint3D_Point_list[i].desiredFeature)));
L.stack(fp3D.interaction());
}
-
+
//--------------vpFeatureVanishingPoint--------------
- //From vpPoint
- if( i < featureVanishingPoint_Point_list.size() ){
+ // From vpPoint
+ if (i < featureVanishingPoint_Point_list.size()) {
vpFeatureVanishingPoint fvp;
vpPoint p(featureVanishingPoint_Point_list[i].firstParam);
p.track(cMo);
@@ -314,8 +320,8 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fvp.error(*(featureVanishingPoint_Point_list[i].desiredFeature)));
L.stack(fvp.interaction());
}
- //From Duo of vpLines
- if( i < featureVanishingPoint_DuoLine_list.size() ){
+ // From Duo of vpLines
+ if (i < featureVanishingPoint_DuoLine_list.size()) {
vpFeatureVanishingPoint fvp;
vpLine l1(featureVanishingPoint_DuoLine_list[i].firstParam);
vpLine l2(featureVanishingPoint_DuoLine_list[i].secondParam);
@@ -325,10 +331,10 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fvp.error(*(featureVanishingPoint_DuoLine_list[i].desiredFeature)));
L.stack(fvp.interaction());
}
-
+
//--------------vpFeatureEllipse--------------
- //From vpSphere
- if( i < featureEllipse_Sphere_list.size() ){
+ // From vpSphere
+ if (i < featureEllipse_Sphere_list.size()) {
vpFeatureEllipse fe;
vpSphere s(featureEllipse_Sphere_list[i].firstParam);
s.track(cMo);
@@ -336,8 +342,8 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fe.error(*(featureEllipse_Sphere_list[i].desiredFeature)));
L.stack(fe.interaction());
}
- //From vpCircle
- if( i < featureEllipse_Circle_list.size() ){
+ // From vpCircle
+ if (i < featureEllipse_Circle_list.size()) {
vpFeatureEllipse fe;
vpCircle c(featureEllipse_Circle_list[i].firstParam);
c.track(cMo);
@@ -345,10 +351,10 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fe.error(*(featureEllipse_Circle_list[i].desiredFeature)));
L.stack(fe.interaction());
}
-
+
//--------------vpFeatureLine--------------
- //From vpLine
- if( i < featureLine_Line_list.size() ){
+ // From vpLine
+ if (i < featureLine_Line_list.size()) {
vpFeatureLine fl;
vpLine l(featureLine_Line_list[i].firstParam);
l.track(cMo);
@@ -356,8 +362,8 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fl.error(*(featureLine_Line_list[i].desiredFeature)));
L.stack(fl.interaction());
}
- //From Duo of vpCylinder / Integer
- if( i < featureLine_DuoLineInt_List.size() ){
+ // From Duo of vpCylinder / Integer
+ if (i < featureLine_DuoLineInt_List.size()) {
vpFeatureLine fl;
vpCylinder c(featureLine_DuoLineInt_List[i].firstParam);
c.track(cMo);
@@ -365,10 +371,10 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
err.stack(fl.error(*(featureLine_DuoLineInt_List[i].desiredFeature)));
L.stack(fl.interaction());
}
-
+
//--------------vpFeatureSegment--------------
- //From Duo of vpPoints
- if( i < featureSegment_DuoPoints_list.size() ){
+ // From Duo of vpPoints
+ if (i < featureSegment_DuoPoints_list.size()) {
vpFeatureSegment fs;
vpPoint p1(featureSegment_DuoPoints_list[i].firstParam);
vpPoint p2(featureSegment_DuoPoints_list[i].secondParam);
@@ -381,7 +387,7 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
//--------------Specific Feature--------------
- if( i < featureSpecific_list.size() ){
+ if (i < featureSpecific_list.size()) {
featureSpecific_list[i]->createCurrent(cMo);
err.stack(featureSpecific_list[i]->error());
L.stack(featureSpecific_list[i]->currentInteraction());
@@ -390,9 +396,9 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
}
}
-
/*!
- Compute the pose according to the desired method (virtual visual servoing, or robust virtual visual servoing approach).
+ Compute the pose according to the desired method (virtual visual servoing,
+ or robust virtual visual servoing approach).
\param cMo : Computed pose.
@@ -400,21 +406,21 @@ void vpPoseFeatures::error_and_interaction(vpHomogeneousMatrix & cMo, vpColVecto
- The virtual visual servoing approach is described in \cite Marchand02c.
- - The robust virtual visual servoing approach is described in \cite Comport06b.
+ - The robust virtual visual servoing approach is described in
+ \cite Comport06b.
*/
-void vpPoseFeatures::computePose(vpHomogeneousMatrix & cMo, const vpPoseFeaturesMethodType &type)
+void vpPoseFeatures::computePose(vpHomogeneousMatrix &cMo, const vpPoseFeaturesMethodType &type)
{
- switch(type)
- {
- case VIRTUAL_VS:
- computePoseVVS(cMo);
- break;
- case ROBUST_VIRTUAL_VS:
- computePoseRobustVVS(cMo);
- break;
- default:
- break;
+ switch (type) {
+ case VIRTUAL_VS:
+ computePoseVVS(cMo);
+ break;
+ case ROBUST_VIRTUAL_VS:
+ computePoseRobustVVS(cMo);
+ break;
+ default:
+ break;
}
}
@@ -423,154 +429,149 @@ void vpPoseFeatures::computePose(vpHomogeneousMatrix & cMo, const vpPoseFeatures
This approach is described in:
- E. Marchand, F. Chaumette. Virtual Visual Servoing: a framework for real-time
- augmented reality. In EUROGRAPHICS 2002 Conference Proceeding, G. Drettakis,
- H.-P. Seidel (eds.), Computer Graphics Forum, Volume 21(3), Pages 289-298,
- Sarrebruck, Allemagne, 2002.
-
+ E. Marchand, F. Chaumette. Virtual Visual Servoing: a framework for
+ real-time augmented reality. In EUROGRAPHICS 2002 Conference Proceeding, G.
+ Drettakis, H.-P. Seidel (eds.), Computer Graphics Forum, Volume 21(3), Pages
+ 289-298, Sarrebruck, Allemagne, 2002.
+
\param cMo : Computed pose.
*/
-void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix & cMo)
+void vpPoseFeatures::computePoseVVS(vpHomogeneousMatrix &cMo)
{
- try
- {
- double residu_1 = 1e8 ;
- double r =1e8-1;
+ try {
+ double residu_1 = 1e8;
+ double r = 1e8 - 1;
// we stop the minimization when the error is bellow 1e-8
-
+
vpMatrix L;
vpColVector err;
- vpColVector v ;
-
+ vpColVector v;
+
unsigned int iter = 0;
-
- //while((int)((residu_1 - r)*1e12) != 0 )
- while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon() )
- {
- residu_1 = r ;
+
+ // while((int)((residu_1 - r)*1e12) != 0 )
+ while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
+ residu_1 = r;
// Compute the interaction matrix and the error
- error_and_interaction(cMo,err,L);
+ error_and_interaction(cMo, err, L);
// compute the residual
- r = err.sumSquare() ;
+ r = err.sumSquare();
// compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- unsigned int rank = L.pseudoInverse(Lp,1e-16) ;
-
- if(rank < 6){
- if(verbose)
+ vpMatrix Lp;
+ unsigned int rank = L.pseudoInverse(Lp, 1e-16);
+
+ if (rank < 6) {
+ if (verbose)
vpTRACE("Rank must be at least 6 ! cMo not computed.");
-
+
break;
}
-
+
// compute the VVS control law
- v = -lambda*Lp*err ;
+ v = -lambda * Lp * err;
- cMo = vpExponentialMap::direct(v).inverse()*cMo ;
- if (iter++>vvsIterMax){
- vpTRACE("Max iteration reached") ;
- break ;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ if (iter++ > vvsIterMax) {
+ vpTRACE("Max iteration reached");
+ break;
}
}
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*err);
-
- }
- catch(...)
- {
- vpERROR_TRACE("vpPoseFeatures::computePoseVVS") ;
- throw ;
+
+ if (computeCovariance)
+ covarianceMatrix = vpMatrix::computeCovarianceMatrix(L, v, -lambda * err);
+
+ } catch (...) {
+ vpERROR_TRACE("vpPoseFeatures::computePoseVVS");
+ throw;
}
}
-
/*!
- Compute the pose thanks to the robust virtual visual servoing approach described in \cite Comport06b.
+ Compute the pose thanks to the robust virtual visual servoing approach
+ described in \cite Comport06b.
\param cMo : Computed pose.
*/
-void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix & cMo)
+void vpPoseFeatures::computePoseRobustVVS(vpHomogeneousMatrix &cMo)
{
- try{
- double residu_1 = 1e8 ;
- double r =1e8-1;
+ try {
+ double residu_1 = 1e8;
+ double r = 1e8 - 1;
// we stop the minimization when the error is bellow 1e-8
vpMatrix L, W;
vpColVector w, res;
- vpColVector v ;
- vpColVector error ; // error vector
-
- vpRobust robust(2*totalSize) ;
- robust.setThreshold(0.0000) ;
-
- unsigned int iter = 0 ;
-
- //while((int)((residu_1 - r)*1e12) !=0)
- while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon())
- {
- residu_1 = r ;
-
+ vpColVector v;
+ vpColVector error; // error vector
+
+ vpRobust robust(2 * totalSize);
+ robust.setThreshold(0.0000);
+
+ unsigned int iter = 0;
+
+ // while((int)((residu_1 - r)*1e12) !=0)
+ while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
+ residu_1 = r;
+
// Compute the interaction matrix and the error
- error_and_interaction(cMo,error,L);
+ error_and_interaction(cMo, error, L);
// compute the residual
- r = error.sumSquare() ;
-
- if(iter == 0){
- res.resize(error.getRows()/2);
- w.resize(error.getRows()/2);
- W.resize(error.getRows(),error.getRows());
+ r = error.sumSquare();
+
+ if (iter == 0) {
+ res.resize(error.getRows() / 2);
+ w.resize(error.getRows() / 2);
+ W.resize(error.getRows(), error.getRows());
w = 1;
}
- for(unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
+ for (unsigned int k = 0; k < error.getRows() / 2; k++) {
+ res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
}
robust.setIteration(0);
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
- for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- W[2*k][2*k] = w[k] ;
- W[2*k+1][2*k+1] = w[k] ;
+ for (unsigned int k = 0; k < error.getRows() / 2; k++) {
+ W[2 * k][2 * k] = w[k];
+ W[2 * k + 1][2 * k + 1] = w[k];
}
// compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
+ vpMatrix Lp;
vpMatrix LRank;
- (W*L).pseudoInverse(Lp,1e-6) ;
- unsigned int rank = L.pseudoInverse(LRank,1e-6) ;
+ (W * L).pseudoInverse(Lp, 1e-6);
+ unsigned int rank = L.pseudoInverse(LRank, 1e-6);
- if(rank < 6){
- if(verbose)
+ if (rank < 6) {
+ if (verbose)
vpTRACE("Rank must be at least 6 ! cMo not computed.");
-
+
break;
}
// compute the VVS control law
- v = -lambda*Lp*W*error ;
+ v = -lambda * Lp * W * error;
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax){
- vpTRACE("Max iteration reached") ;
- break ;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ ;
+ if (iter++ > vvsIterMax) {
+ vpTRACE("Max iteration reached");
+ break;
}
}
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
- }
- catch(...)
- {
- vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS") ;
- throw ;
+
+ if (computeCovariance)
+ covarianceMatrix =
+ vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
+ // matrix is diagonale, but using W*W
+ // is more efficient.
+ } catch (...) {
+ vpERROR_TRACE("vpPoseFeatures::computePoseRobustVVS");
+ throw;
}
}
diff --git a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp
index eff0354..b52ec0f 100644
--- a/modules/vision/src/pose-estimation/vpPoseLagrange.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseLagrange.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#include <visp3/vision/vpPose.h>
#define DEBUG_LEVEL1 0
@@ -49,86 +49,76 @@
/* rotation */
/**********************************************************************/
-static
-void
-calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
- unsigned int nc3, vpColVector &x1, vpColVector &x2)
+static void calculTranslation(vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1, unsigned int nc3,
+ vpColVector &x1, vpColVector &x2)
{
- try
- {
- unsigned int i,j;
+ try {
+ unsigned int i, j;
- vpMatrix ct(3,nl) ;
- for (i=0 ; i < 3 ; i++)
- {
- for (j=0 ; j < nl ; j++)
- ct[i][j] = b[j][i+nc3] ;
+ vpMatrix ct(3, nl);
+ for (i = 0; i < 3; i++) {
+ for (j = 0; j < nl; j++)
+ ct[i][j] = b[j][i + nc3];
}
- vpMatrix c ;
- c = ct.t() ;
+ vpMatrix c;
+ c = ct.t();
- vpMatrix ctc ;
- ctc = ct*c ;
+ vpMatrix ctc;
+ ctc = ct * c;
- vpMatrix ctc1 ; // (C^T C)^(-1)
- ctc1 = ctc.inverseByLU() ;
+ vpMatrix ctc1; // (C^T C)^(-1)
+ ctc1 = ctc.inverseByLU();
- vpMatrix cta ;
- vpMatrix ctb ;
- cta = ct*a ; /* C^T A */
- ctb = ct*b ; /* C^T B */
+ vpMatrix cta;
+ vpMatrix ctb;
+ cta = ct * a; /* C^T A */
+ ctb = ct * b; /* C^T B */
#if (DEBUG_LEVEL2)
{
- std::cout <<"ctc " << std::endl << ctc ;
- std::cout <<"cta " << std::endl << cta ;
- std::cout <<"ctb " << std::endl << ctb ;
+ std::cout << "ctc " << std::endl << ctc;
+ std::cout << "cta " << std::endl << cta;
+ std::cout << "ctb " << std::endl << ctb;
}
#endif
- vpColVector X2(nc3) ;
- vpMatrix CTB(nc1,nc3) ;
- for (i=0 ; i < nc1 ; i++)
- {
- for (j=0 ; j < nc3 ; j++)
- CTB[i][j] = ctb[i][j] ;
+ vpColVector X2(nc3);
+ vpMatrix CTB(nc1, nc3);
+ for (i = 0; i < nc1; i++) {
+ for (j = 0; j < nc3; j++)
+ CTB[i][j] = ctb[i][j];
}
- for (j=0 ; j < nc3 ; j++)
- X2[j] = x2[j] ;
+ for (j = 0; j < nc3; j++)
+ X2[j] = x2[j];
- vpColVector sv ; // C^T A X1 + C^T B X2)
- sv = cta*x1 + CTB*X2 ;// C^T A X1 + C^T B X2)
+ vpColVector sv; // C^T A X1 + C^T B X2)
+ sv = cta * x1 + CTB * X2; // C^T A X1 + C^T B X2)
#if (DEBUG_LEVEL2)
- std::cout << "sv " << sv.t() ;
+ std::cout << "sv " << sv.t();
#endif
- vpColVector X3 ; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
- X3 = -ctc1*sv ;
+ vpColVector X3; /* X3 = - (C^T C )^{-1} C^T (A X1 + B X2) */
+ X3 = -ctc1 * sv;
#if (DEBUG_LEVEL2)
- std::cout << "x3 " << X3.t() ;
+ std::cout << "x3 " << X3.t();
#endif
- for (i=0 ; i < nc1 ; i++)
- x2[i+nc3] = X3[i] ;
- }
- catch(...)
- {
+ for (i = 0; i < nc1; i++)
+ x2[i + nc3] = X3[i];
+ } catch (...) {
// en fait il y a des dizaines de raisons qui font que cette fonction
// rende une erreur (matrice pas inversible, pb de memoire etc...)
- vpERROR_TRACE(" ") ;
- throw ;
+ vpERROR_TRACE(" ");
+ throw;
}
-
-
}
-
//*********************************************************************
// FONCTION LAGRANGE :
// -------------------
@@ -139,44 +129,71 @@ calculTranslation (vpMatrix &a, vpMatrix &b, unsigned int nl, unsigned int nc1,
//#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;
#endif
- try{
- unsigned int i,imin;
-
- vpMatrix ata ; // A^T A
- ata = a.t()*a ;
- vpMatrix btb ; // B^T B
- btb = b.t()*b ;
-
- vpMatrix bta ; // B^T A
- bta = b.t()*a ;
-
- vpMatrix btb1 ; // (B^T B)^(-1)
-
- if (b.getRows() >= b.getCols()) btb1 = btb.inverseByLU() ;
- else btb1 = btb.pseudoInverse();
+ try {
+ unsigned int i, imin;
+
+ vpMatrix ata; // A^T A
+ ata = a.t() * a;
+ vpMatrix btb; // B^T B
+ btb = b.t() * b;
+
+ vpMatrix bta; // B^T A
+ bta = b.t() * a;
+
+ vpMatrix btb1; // (B^T B)^(-1)
+
+/* Warning:
+ when using btb.inverseByLU() that call cv::inv(cv::DECOMP_LU) with
+ OpenCV 3.1.0 and 3.2.0 we notice that OpenCV is not able to compute the
+ inverse of the following matrix:
+
+ btb[9,9]=
+ 0.015925 0.0 0.0030866 0.00035 0.0 0.000041 0.105
+ 0.0 0.0346242 0.0 0.015925 -0.0050979 0.0 0.00035
+ -0.000063 0.0 0.105 -0.0637464 0.0030866 -0.0050979 0.0032301
+ 0.000041 -0.000063 0.000016 0.0346242 -0.0637464 0.0311185 0.00035
+ 0.0 0.000041 0.0001 0.0 0.000012 0.01 0.0
+ 0.0011594 0.0 0.00035 -0.000063 0.0 0.0001 -0.000018
+ 0.0 0.01 -0.0018040 0.000041 -0.000063 0.000016 0.000012
+ -0.000018 0.000005 0.0011594 -0.0018040 0.0004599 0.105 0.0
+ 0.0346242 0.01 0.0 0.0011594 5.0 0.0 0.13287
+ 0.0 0.105 -0.0637464 0.0 0.01 -0.0018040 0.0
+ 5.0 -0.731499 0.0346242 -0.0637464 0.0311185 0.0011594 -0.0018040
+ 0.0004599 0.13287 -0.731499 0.454006
+
+ That's why instead of using inverseByLU() we are now using pseudoInverse()
+ */
+#if 0
+ if (b.getRows() >= b.getCols()) {
+ btb1 = btb.inverseByLU();
+ }
+ else {
+ btb1 = btb.pseudoInverse();
+ }
+#else
+ btb1 = btb.pseudoInverse();
+#endif
#if (DEBUG_LEVEL1)
{
- std::cout << " BTB1 * BTB : " << std::endl << btb1*btb << std::endl;
- std::cout << " BTB * BTB1 : " << std::endl << btb*btb1 << std::endl;
+ std::cout << " BTB1 * BTB : " << std::endl << btb1 * btb << std::endl;
+ std::cout << " BTB * BTB1 : " << std::endl << btb * btb1 << std::endl;
}
#endif
- vpMatrix r ; // (B^T B)^(-1) B^T A
- r = btb1*bta ;
+ vpMatrix r; // (B^T B)^(-1) B^T A
+ r = btb1 * bta;
- vpMatrix e ; // - A^T B (B^T B)^(-1) B^T A
- e = - (a.t()*b) *r ;
+ vpMatrix e; // - A^T B (B^T B)^(-1) B^T A
+ e = -(a.t() * b) * r;
- e += ata ; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
+ e += ata; // calcul E = A^T A - A^T B (B^T B)^(-1) B^T A
#if (DEBUG_LEVEL1)
{
@@ -186,9 +203,8 @@ lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
// vpColVector sv ;
// vpMatrix v ;
- e.svd(x1,ata) ;// destructif sur e
+ e.svd(x1, ata); // destructif sur e
// calcul du vecteur propre de E correspondant a la valeur propre min.
-
/* calcul de SVmax */
imin = 0;
// FC : Pourquoi calculer SVmax ??????
@@ -199,31 +215,30 @@ lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
// }
// svm *= EPS; /* pour le rang */
- for (i=0;i<x1.getRows();i++)
- if (x1[i] < x1[imin]) imin = i;
+ for (i = 0; i < x1.getRows(); i++)
+ if (x1[i] < x1[imin])
+ imin = i;
#if (DEBUG_LEVEL1)
{
- printf("SV(E) : %.15lf %.15lf %.15lf\n",x1[0],x1[1],x1[2]);
+ printf("SV(E) : %.15lf %.15lf %.15lf\n", x1[0], x1[1], x1[2]);
std::cout << " i_min " << imin << std::endl;
}
#endif
- for (i=0;i<x1.getRows();i++)
+ for (i = 0; i < x1.getRows(); i++)
x1[i] = ata[i][imin];
- x2 = - (r*x1) ; // X_2 = - (B^T B)^(-1) B^T A X_1
+ x2 = -(r * x1); // X_2 = - (B^T B)^(-1) B^T A X_1
#if (DEBUG_LEVEL1)
{
- std::cout << " X1 : " << x1.t() << std::endl;
+ std::cout << " X1 : " << x1.t() << std::endl;
std::cout << " V : " << std::endl << ata << std::endl;
}
#endif
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE(" ");
+ throw;
}
#if (DEBUG_LEVEL1)
std::cout << "end (CLagrange.cc)Lagrange(...) " << std::endl;
@@ -242,166 +257,167 @@ lagrange (vpMatrix &a, vpMatrix &b, vpColVector &x1, vpColVector &x2)
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)
- std::cout << "begin vpPose::PoseLagrange(...) " << std::endl ;
+ std::cout << "begin vpPose::PoseLagrange(...) " << std::endl;
#endif
- try
- {
+ try {
double s;
unsigned int i;
- unsigned int k=0;
- unsigned int nl=npt*2;
+ unsigned int k = 0;
+ unsigned int nl = npt * 2;
-
- vpMatrix a(nl,3) ;
- vpMatrix b(nl,6);
- vpPoint P ;
- i=0 ;
+ vpMatrix a(nl, 3);
+ vpMatrix b(nl, 6);
+ vpPoint P;
+ i = 0;
if (coplanar_plane_type == 1) { // plane ax=d
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oY();
- a[k][1] = 0.0;
- a[k][2] = P.get_oY()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oY();
- a[k+1][2] = P.get_oY()*P.get_y();
-
- b[k][0] = -P.get_oZ();
- b[k][1] = 0.0;
- b[k][2] = P.get_oZ()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oZ();
- b[k+1][2] = P.get_oZ()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
+ P = *it;
+ a[k][0] = -P.get_oY();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oY() * P.get_x();
+
+ a[k + 1][0] = 0.0;
+ a[k + 1][1] = -P.get_oY();
+ a[k + 1][2] = P.get_oY() * P.get_y();
+
+ b[k][0] = -P.get_oZ();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oZ() * P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k + 1][0] = 0.0;
+ b[k + 1][1] = -P.get_oZ();
+ b[k + 1][2] = P.get_oZ() * P.get_y();
+ b[k + 1][3] = 0.0;
+ b[k + 1][4] = -1.0;
+ b[k + 1][5] = P.get_y();
k += 2;
}
- }
- else if (coplanar_plane_type == 2) { // plane by=d
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
-
- b[k][0] = -P.get_oZ();
- b[k][1] = 0.0;
- b[k][2] = P.get_oZ()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oZ();
- b[k+1][2] = P.get_oZ()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
+ } else if (coplanar_plane_type == 2) { // plane by=d
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
+ P = *it;
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX() * P.get_x();
+
+ a[k + 1][0] = 0.0;
+ a[k + 1][1] = -P.get_oX();
+ a[k + 1][2] = P.get_oX() * P.get_y();
+
+ b[k][0] = -P.get_oZ();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oZ() * P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k + 1][0] = 0.0;
+ b[k + 1][1] = -P.get_oZ();
+ b[k + 1][2] = P.get_oZ() * P.get_y();
+ b[k + 1][3] = 0.0;
+ b[k + 1][4] = -1.0;
+ b[k + 1][5] = P.get_y();
k += 2;
}
- }
- else { // plane cz=d or any other
-
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
- P = *it ;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
-
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
-
- b[k][0] = -P.get_oY();
- b[k][1] = 0.0;
- b[k][2] = P.get_oY()*P.get_x();
- b[k][3] = -1.0;
- b[k][4] = 0.0;
- b[k][5] = P.get_x();
-
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oY();
- b[k+1][2] = P.get_oY()*P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -1.0;
- b[k+1][5] = P.get_y();
+ } else { // plane cz=d or any other
+
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
+ P = *it;
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX() * P.get_x();
+
+ a[k + 1][0] = 0.0;
+ a[k + 1][1] = -P.get_oX();
+ a[k + 1][2] = P.get_oX() * P.get_y();
+
+ b[k][0] = -P.get_oY();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oY() * P.get_x();
+ b[k][3] = -1.0;
+ b[k][4] = 0.0;
+ b[k][5] = P.get_x();
+
+ b[k + 1][0] = 0.0;
+ b[k + 1][1] = -P.get_oY();
+ b[k + 1][2] = P.get_oY() * P.get_y();
+ b[k + 1][3] = 0.0;
+ b[k + 1][4] = -1.0;
+ b[k + 1][5] = P.get_y();
k += 2;
}
}
- vpColVector X1(3) ;
- vpColVector X2(6) ;
+ vpColVector X1(3);
+ vpColVector X2(6);
#if (DEBUG_LEVEL2)
{
- std::cout <<"a " << a << std::endl ;
- std::cout <<"b " << b << std::endl ;
+ std::cout << "a " << a << std::endl;
+ std::cout << "b " << b << std::endl;
}
#endif
- lagrange(a,b,X1,X2);
+ lagrange(a, b, X1, X2);
#if (DEBUG_LEVEL2)
{
- std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
- std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
+ std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
+ std::cout << "norme X1 " << X1.sumSquare() << std::endl;
+ ;
}
#endif
- if (X2[5] < 0.0)
- { /* car Zo > 0 */
- for (i=0;i<3;i++) X1[i] = -X1[i];
- for (i=0;i<6;i++) X2[i] = -X2[i];
+ if (X2[5] < 0.0) { /* car Zo > 0 */
+ for (i = 0; i < 3; i++)
+ X1[i] = -X1[i];
+ for (i = 0; i < 6; i++)
+ X2[i] = -X2[i];
}
s = 0.0;
- for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
- for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
-
- //s = 0.0;
- //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
- s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
-
- if (s<1e-10)
- {
-// std::cout << "Points that produce an error: " << std::endl;
-// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
-// {
-// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
-// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
-// }
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Lagrange pose computation (planar plane case)")) ;
+ for (i = 0; i < 3; i++) {
+ s += (X1[i] * X2[i]);
+ }
+ for (i = 0; i < 3; i++) {
+ X2[i] -= (s * X1[i]);
+ } /* X1^T X2 = 0 */
+
+ // s = 0.0;
+ // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
+ s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
+
+ if (s < 1e-10) {
+ // std::cout << "Points that produce an error: " << std::endl;
+ // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
+ // != listP.end(); ++it)
+ // {
+ // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
+ // " "
+ // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
+ // (*it).get_oZ() << std::endl;
+ // }
+ throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation "
+ "(planar plane case)"));
}
- s = 1.0/sqrt(s);
- for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
-
+ s = 1.0 / sqrt(s);
+ for (i = 0; i < 3; i++) {
+ X2[i] *= s;
+ } /* X2^T X2 = 1 */
- calculTranslation (a, b, nl, 3, 3, X1, X2) ;
+ calculTranslation(a, b, nl, 3, 3, X1, X2);
// if (err != OK)
{
@@ -411,124 +427,113 @@ vpPose::poseLagrangePlan(vpHomogeneousMatrix &cMo, const int coplanar_plane_type
}
if (coplanar_plane_type == 1) { // plane ax=d
- cMo[0][0] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][0] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][0] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+ cMo[0][0] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
+ cMo[1][0] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
+ cMo[2][0] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
+ for (i = 0; i < 3; i++) { /* calcul de la matrice de passage */
cMo[i][1] = X1[i];
cMo[i][2] = X2[i];
- cMo[i][3] = X2[i+3];
+ cMo[i][3] = X2[i + 3];
}
- }
- else if (coplanar_plane_type == 2) { // plane by=d
- cMo[0][1] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][1] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][1] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+ } else if (coplanar_plane_type == 2) { // plane by=d
+ cMo[0][1] = (X1[2] * X2[1]) - (X1[1] * X2[2]);
+ cMo[1][1] = (X1[0] * X2[2]) - (X1[2] * X2[0]);
+ cMo[2][1] = (X1[1] * X2[0]) - (X1[0] * X2[1]);
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
+ for (i = 0; i < 3; i++) { /* calcul de la matrice de passage */
cMo[i][0] = X1[i];
cMo[i][2] = X2[i];
- cMo[i][3] = X2[i+3];
+ cMo[i][3] = X2[i + 3];
}
- }
- else { // plane cz=d or any other
+ } else { // plane cz=d or any other
+ cMo[0][2] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
+ cMo[1][2] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
+ cMo[2][2] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
- cMo[0][2] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- cMo[1][2] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- cMo[2][2] = (X1[0]*X2[1])-(X1[1]*X2[0]);
-
- for (i=0;i<3;i++)
- { /* calcul de la matrice de passage */
+ for (i = 0; i < 3; i++) { /* calcul de la matrice de passage */
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
- cMo[i][3] = X2[i+3];
+ cMo[i][3] = X2[i + 3];
}
}
- }
- catch(...)
- {
+ } catch (...) {
throw; // throw the original exception
}
#if (DEBUG_LEVEL1)
- std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
+ std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl;
#endif
// return(OK);
}
-
-void
-vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
+void vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
- std::cout << "begin CPose::PoseLagrange(...) " << std::endl ;
+ std::cout << "begin CPose::PoseLagrange(...) " << std::endl;
#endif
- try{
+ try {
double s;
unsigned int i;
- unsigned int k=0;
- unsigned int nl=npt*2;
+ unsigned int k = 0;
+ unsigned int nl = npt * 2;
- vpMatrix a(nl,3) ;
- vpMatrix b(nl,9);
- b =0 ;
+ vpMatrix a(nl, 3);
+ vpMatrix b(nl, 9);
+ b = 0;
- vpPoint P ;
- i=0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ vpPoint P;
+ i = 0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- a[k][0] = -P.get_oX();
- a[k][1] = 0.0;
- a[k][2] = P.get_oX()*P.get_x();
+ a[k][0] = -P.get_oX();
+ a[k][1] = 0.0;
+ a[k][2] = P.get_oX() * P.get_x();
- a[k+1][0] = 0.0;
- a[k+1][1] = -P.get_oX();
- a[k+1][2] = P.get_oX()*P.get_y();
+ a[k + 1][0] = 0.0;
+ a[k + 1][1] = -P.get_oX();
+ a[k + 1][2] = P.get_oX() * P.get_y();
- b[k][0] = -P.get_oY();
- b[k][1] = 0.0;
- b[k][2] = P.get_oY()*P.get_x();
+ b[k][0] = -P.get_oY();
+ b[k][1] = 0.0;
+ b[k][2] = P.get_oY() * P.get_x();
- b[k][3] = -P.get_oZ();
- b[k][4] = 0.0;
- b[k][5] = P.get_oZ()*P.get_x();
+ b[k][3] = -P.get_oZ();
+ b[k][4] = 0.0;
+ b[k][5] = P.get_oZ() * P.get_x();
- b[k][6] = -1.0;
- b[k][7] = 0.0;
- b[k][8] = P.get_x();
+ b[k][6] = -1.0;
+ b[k][7] = 0.0;
+ b[k][8] = P.get_x();
- b[k+1][0] = 0.0;
- b[k+1][1] = -P.get_oY();
- b[k+1][2] = P.get_oY()*P.get_y();
+ b[k + 1][0] = 0.0;
+ b[k + 1][1] = -P.get_oY();
+ b[k + 1][2] = P.get_oY() * P.get_y();
- b[k+1][3] = 0.0;
- b[k+1][4] = -P.get_oZ();
- b[k+1][5] = P.get_oZ()*P.get_y();
+ b[k + 1][3] = 0.0;
+ b[k + 1][4] = -P.get_oZ();
+ b[k + 1][5] = P.get_oZ() * P.get_y();
- b[k+1][6] = 0.0;
- b[k+1][7] = -1.0;
- b[k+1][8] = P.get_y();
+ b[k + 1][6] = 0.0;
+ b[k + 1][7] = -1.0;
+ b[k + 1][8] = P.get_y();
k += 2;
}
- vpColVector X1(3) ;
- vpColVector X2(9) ;
+ vpColVector X1(3);
+ vpColVector X2(9);
#if (DEBUG_LEVEL2)
{
- std::cout <<"a " << a << std::endl ;
- std::cout <<"b " << b << std::endl ;
+ std::cout << "a " << a << std::endl;
+ std::cout << "b " << b << std::endl;
}
#endif
- lagrange(a,b,X1,X2);
+ lagrange(a, b, X1, X2);
// if (err != OK)
{
// std::cout << "in (CLagrange.cc)Lagrange returns " ;
@@ -536,68 +541,71 @@ vpPose::poseLagrangeNonPlan(vpHomogeneousMatrix &cMo)
// return err ;
}
-
#if (DEBUG_LEVEL2)
{
- std::cout << "ax1+bx2 (devrait etre 0) " << (a*X1 + b*X2).t() << std::endl ;
- std::cout << "norme X1 " << X1.sumSquare() << std::endl ;;
+ std::cout << "ax1+bx2 (devrait etre 0) " << (a * X1 + b * X2).t() << std::endl;
+ std::cout << "norme X1 " << X1.sumSquare() << std::endl;
+ ;
}
#endif
- if (X2[8] < 0.0)
- { /* car Zo > 0 */
- X1 *= -1 ;
- X2 *= -1 ;
+ if (X2[8] < 0.0) { /* car Zo > 0 */
+ X1 *= -1;
+ X2 *= -1;
}
s = 0.0;
- for (i=0;i<3;i++) {s += (X1[i]*X2[i]);}
- for (i=0;i<3;i++) {X2[i] -= (s*X1[i]);} /* X1^T X2 = 0 */
-
- //s = 0.0;
- //for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
- s = X2[0]*X2[0] + X2[1]*X2[1] + X2[2]*X2[2]; // To avoid a Coverity copy/past error
-
- if (s<1e-10)
- {
-// std::cout << "Points that produce an error: " << std::endl;
-// for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
-// {
-// std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() << " "
-// << (*it).get_oX() << " " << (*it).get_oY() << " " << (*it).get_oZ() << std::endl;
-// }
- //vpERROR_TRACE(" division par zero " ) ;
- throw(vpException(vpException::divideByZeroError,
- "Division by zero in Lagrange pose computation (non planar plane case)")) ;
+ for (i = 0; i < 3; i++) {
+ s += (X1[i] * X2[i]);
+ }
+ for (i = 0; i < 3; i++) {
+ X2[i] -= (s * X1[i]);
+ } /* X1^T X2 = 0 */
+
+ // s = 0.0;
+ // for (i=0;i<3;i++) {s += (X2[i]*X2[i]);}
+ s = X2[0] * X2[0] + X2[1] * X2[1] + X2[2] * X2[2]; // To avoid a Coverity copy/past error
+
+ if (s < 1e-10) {
+ // std::cout << "Points that produce an error: " << std::endl;
+ // for (std::list<vpPoint>::const_iterator it = listP.begin(); it
+ // != listP.end(); ++it)
+ // {
+ // std::cout << "P: " << (*it).get_x() << " " << (*it).get_y() <<
+ // " "
+ // << (*it).get_oX() << " " << (*it).get_oY() << " " <<
+ // (*it).get_oZ() << std::endl;
+ // }
+ // vpERROR_TRACE(" division par zero " ) ;
+ throw(vpException(vpException::divideByZeroError, "Division by zero in Lagrange pose computation (non "
+ "planar plane case)"));
}
- s = 1.0/sqrt(s);
- for (i=0;i<3;i++) {X2[i] *= s;} /* X2^T X2 = 1 */
+ s = 1.0 / sqrt(s);
+ for (i = 0; i < 3; i++) {
+ X2[i] *= s;
+ } /* X2^T X2 = 1 */
- X2[3] = (X1[1]*X2[2])-(X1[2]*X2[1]);
- X2[4] = (X1[2]*X2[0])-(X1[0]*X2[2]);
- X2[5] = (X1[0]*X2[1])-(X1[1]*X2[0]);
+ X2[3] = (X1[1] * X2[2]) - (X1[2] * X2[1]);
+ X2[4] = (X1[2] * X2[0]) - (X1[0] * X2[2]);
+ X2[5] = (X1[0] * X2[1]) - (X1[1] * X2[0]);
- calculTranslation (a, b, nl, 3, 6, X1, X2) ;
+ calculTranslation(a, b, nl, 3, 6, X1, X2);
- for (i=0 ; i<3 ; i++)
- {
+ for (i = 0; i < 3; i++) {
cMo[i][0] = X1[i];
cMo[i][1] = X2[i];
- cMo[i][2] = X2[i+3];
- cMo[i][3] = X2[i+6];
+ cMo[i][2] = X2[i + 3];
+ cMo[i][3] = X2[i + 6];
}
- }
- catch(...)
- {
+ } catch (...) {
throw; // throw the original exception
}
#if (DEBUG_LEVEL1)
- std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl ;
+ std::cout << "end vpCalculPose::PoseLagrange(...) " << std::endl;
#endif
}
-
#undef DEBUG_LEVEL1
#undef DEBUG_LEVEL2
diff --git a/modules/vision/src/pose-estimation/vpPoseLowe.cpp b/modules/vision/src/pose-estimation/vpPoseLowe.cpp
index ba5ed09..0fc2285 100644
--- a/modules/vision/src/pose-estimation/vpPoseLowe.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseLowe.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,18 +37,18 @@
*
*****************************************************************************/
-#include <math.h>
#include <float.h>
+#include <limits> // numeric_limits
+#include <math.h>
#include <string.h>
-#include <limits> // numeric_limits
// besoin de la librairie mathematique, en particulier des
// fonctions de minimisation de Levenberg Marquartd
#include <visp3/vision/vpLevenbergMarquartd.h>
#include <visp3/vision/vpPose.h>
-#define NBR_PAR 6
-#define X3_SIZE 3
+#define NBR_PAR 6
+#define X3_SIZE 3
#define MINIMUM 0.000001
#define DEBUG_LEVEL1 0
@@ -59,124 +60,120 @@
// ------------------------------------------------------------------------
/*
-* MACRO : MIJ
-*
-* ENTREE :
-* m Matrice.
-* i Indice ligne de l'element.
-* j Indice colonne de l'element.
-* s Taille en nombre d'elements d'une ligne de la matrice "m".
-*
-* DESCRIPTION :
-* La macro-instruction calcule l'adresse de l'element de la "i"eme ligne et
-* de la "j"eme colonne de la matrice "m", soit &m[i][j].
-*
-* RETOUR :
-* L'adresse de m[i][j] est retournee.
-*
-* HISTORIQUE :
-* 1.00 - 11/02/93 - Original.
-*/
-#define MIJ(m,i,j,s) ((m) + ((long) (i) * (long) (s)) + (long) (j))
+ * MACRO : MIJ
+ *
+ * ENTREE :
+ * m Matrice.
+ * i Indice ligne de l'element.
+ * j Indice colonne de l'element.
+ * s Taille en nombre d'elements d'une ligne de la matrice "m".
+ *
+ * DESCRIPTION :
+ * La macro-instruction calcule l'adresse de l'element de la "i"eme ligne et
+ * de la "j"eme colonne de la matrice "m", soit &m[i][j].
+ *
+ * RETOUR :
+ * L'adresse de m[i][j] est retournee.
+ *
+ * HISTORIQUE :
+ * 1.00 - 11/02/93 - Original.
+ */
+#define MIJ(m, i, j, s) ((m) + ((long)(i) * (long)(s)) + (long)(j))
#define NBPTMAX 50
// Je hurle d'horreur devant ces variable globale...
-static double XI[NBPTMAX],YI[NBPTMAX];
-static double XO[NBPTMAX],YO[NBPTMAX],ZO[NBPTMAX];
-
+static double XI[NBPTMAX], YI[NBPTMAX];
+static double XO[NBPTMAX], YO[NBPTMAX], ZO[NBPTMAX];
#define MINI 0.001
#define MINIMUM 0.000001
-void eval_function(int npt,double *xc,double *f);
-void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag);
+void eval_function(int npt, double *xc, double *f);
+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)
+void eval_function(int npt, double *xc, double *f)
{
int i;
double u[3];
- u[0] = xc[3]; /* Rx */
- u[1] = xc[4]; /* Ry */
- u[2] = xc[5]; /* Rz */
+ u[0] = xc[3]; /* Rx */
+ u[1] = xc[4]; /* Ry */
+ u[2] = xc[5]; /* Rz */
- vpRotationMatrix rd(u[0],u[1],u[2]) ;
+ vpRotationMatrix rd(u[0], u[1], u[2]);
// rot_mat(u,rd); /* matrice de rotation correspondante */
- for (i=0;i<npt;i++)
- {
- double x = rd[0][0]*XO[i] + rd[0][1]*YO[i] + rd[0][2]*ZO[i] + xc[0];
- double y = rd[1][0]*XO[i] + rd[1][1]*YO[i] + rd[1][2]*ZO[i] + xc[1];
- double z = rd[2][0]*XO[i] + rd[2][1]*YO[i] + rd[2][2]*ZO[i] + xc[2];
- f[i] = x/z - XI[i];
- f[npt+i] = y/z - YI[i];
+ for (i = 0; i < npt; i++) {
+ double x = rd[0][0] * XO[i] + rd[0][1] * YO[i] + rd[0][2] * ZO[i] + xc[0];
+ double y = rd[1][0] * XO[i] + rd[1][1] * YO[i] + rd[1][2] * ZO[i] + xc[1];
+ double z = rd[2][0] * XO[i] + rd[2][1] * YO[i] + rd[2][2] * ZO[i] + xc[2];
+ f[i] = x / z - XI[i];
+ f[npt + i] = y / z - YI[i];
// std::cout << f[i] << " " << f[i+1] << std::endl ;
}
}
-
/*
-* PROCEDURE : fcn
-*
-* ENTREES :
-* m Nombre d'equations.
-* n Nombre de variables.
-* xc Valeur courante des parametres.
-* fvecc Resultat de l'evaluation de la fonction.
-* ldfjac Plus grande dimension de la matrice jac.
-* iflag Choix du calcul de la fonction ou du jacobien.
-*
-* SORTIE :
-* jac Jacobien de la fonction.
-*
-* DESCRIPTION :
-* La procedure calcule la fonction et le jacobien.
-* Si iflag == 1, la procedure calcule la fonction en "xc" et le resultat est
-* stocke dans "fvecc" et "fjac" reste inchange.
-* Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
-* stocke dans "fjac" et "fvecc" reste inchange.
-*
-* HISTORIQUE :
-* 1.00 - xx/xx/xx - Original.
-* 1.01 - 06/07/95 - Modifications.
-* 2.00 - 24/10/95 - Tableau jac monodimensionnel.
-*/
-void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
+ * PROCEDURE : fcn
+ *
+ * ENTREES :
+ * m Nombre d'equations.
+ * n Nombre de variables.
+ * xc Valeur courante des parametres.
+ * fvecc Resultat de l'evaluation de la fonction.
+ * ldfjac Plus grande dimension de la matrice jac.
+ * iflag Choix du calcul de la fonction ou du jacobien.
+ *
+ * SORTIE :
+ * jac Jacobien de la fonction.
+ *
+ * DESCRIPTION :
+ * La procedure calcule la fonction et le jacobien.
+ * Si iflag == 1, la procedure calcule la fonction en "xc" et le resultat est
+ * stocke dans "fvecc" et "fjac" reste inchange.
+ * Si iflag == 2, la procedure calcule le jacobien en "xc" et le resultat est
+ * stocke dans "fjac" et "fvecc" reste inchange.
+ *
+ * HISTORIQUE :
+ * 1.00 - xx/xx/xx - Original.
+ * 1.01 - 06/07/95 - Modifications.
+ * 2.00 - 24/10/95 - Tableau jac monodimensionnel.
+ */
+void fcn(int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int iflag)
{
- double u[X3_SIZE];// rd[X3_SIZE][X3_SIZE],
- vpRotationMatrix rd ;
- int npt;
+ double u[X3_SIZE]; // rd[X3_SIZE][X3_SIZE],
+ vpRotationMatrix rd;
+ int npt;
- if (m < n) printf("pas assez de points\n");
+ if (m < n)
+ printf("pas assez de points\n");
npt = m / 2;
- if (iflag == 1) eval_function (npt, xc, fvecc);
- else if (iflag == 2)
- {
- double u1, u2, u3;
- u[0] =xc[3];
- u[1]= xc[4];
- u[2]= xc[5];
+ if (iflag == 1)
+ eval_function(npt, xc, fvecc);
+ else if (iflag == 2) {
+ double u1, u2, u3;
+ u[0] = xc[3];
+ u[1] = xc[4];
+ u[2] = xc[5];
- rd.buildFrom(u[0],u[1],u[2]) ;
+ rd.buildFrom(u[0], u[1], u[2]);
/* a partir de l'axe de rotation, calcul de la matrice de rotation. */
// rot_mat(u, rd);
- double tt = sqrt (u[0] * u[0] + u[1] * u[1] + u[2] * u[2]); /* angle de rot */
- if (tt >= MINIMUM)
- {
+ 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 */
+ u2 = u[1] / tt; /* axe de rotation unitaire */
u3 = u[2] / tt;
- }
- else u1 = u2 = u3 = 0.0;
+ } else
+ u1 = u2 = u3 = 0.0;
double co = cos(tt);
double mco = 1.0 - co;
double si = sin(tt);
- for (int i = 0; i < npt; i++)
- {
+ for (int i = 0; i < npt; i++) {
double x = XO[i];
- double y = YO[i]; /* coordonnees du point i */
+ double y = YO[i]; /* coordonnees du point i */
double z = ZO[i];
/* coordonnees du point i dans le repere camera */
@@ -185,79 +182,67 @@ void fcn (int m, int n, double *xc, double *fvecc, double *jac, int ldfjac, int
double rz = rd[2][0] * x + rd[2][1] * y + rd[2][2] * z + xc[2];
/* derive des fonctions rx, ry et rz par rapport
- * a tt, u1, u2, u3.
- */
- double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y
- + (si * u1 * u1 - si) * x;
+ * a tt, u1, u2, u3.
+ */
+ double drxt = (si * u1 * u3 + co * u2) * z + (si * u1 * u2 - co * u3) * y + (si * u1 * u1 - si) * x;
double drxu1 = mco * u3 * z + mco * u2 * y + 2 * mco * u1 * x;
double 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;
/* derive de la fonction representant le modele de la
- * camera (sans distortion) par rapport a tt, u1, u2 et u3.
- */
- double dxit = drxt / rz - rx * drzt / (rz * rz);
+ * camera (sans distortion) par rapport a tt, u1, u2 et u3.
+ */
+ double dxit = drxt / rz - rx * drzt / (rz * rz);
- double dyit = dryt / rz - ry * drzt / (rz * rz);
+ double dyit = dryt / rz - ry * drzt / (rz * rz);
- double dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
- double dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
+ double dxiu1 = drxu1 / rz - drzu1 * rx / (rz * rz);
+ double dyiu1 = dryu1 / rz - drzu1 * ry / (rz * rz);
- double dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
- double dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
+ double dxiu2 = drxu2 / rz - drzu2 * rx / (rz * rz);
+ double dyiu2 = dryu2 / rz - drzu2 * ry / (rz * rz);
- double dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
- double dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
+ double dxiu3 = drxu3 / rz - drzu3 * rx / (rz * rz);
+ double dyiu3 = dryu3 / rz - drzu3 * ry / (rz * rz);
/* calcul du jacobien : le jacobien represente la
- * derivee de la fonction representant le modele de la
- * camera par rapport aux parametres.
- */
+ * derivee de la fonction representant le modele de la
+ * camera par rapport aux parametres.
+ */
*MIJ(jac, 0, i, ldfjac) = 1 / rz;
*MIJ(jac, 1, i, ldfjac) = 0.0;
- *MIJ(jac, 2, i, ldfjac) = - rx / (rz * rz);
- if (tt >= MINIMUM)
- {
- *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt
- - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
- *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt
- + (1 - u2 * u2) * dxiu2 / tt- u2 * u3 * dxiu3 / tt;
-
- *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt
- + (1 - u3 * u3) * dxiu3 / tt;
- }
- else
- {
+ *MIJ(jac, 2, i, ldfjac) = -rx / (rz * rz);
+ if (tt >= MINIMUM) {
+ *MIJ(jac, 3, i, ldfjac) = u1 * dxit + (1 - u1 * u1) * dxiu1 / tt - u1 * u2 * dxiu2 / tt - u1 * u3 * dxiu3 / tt;
+ *MIJ(jac, 4, i, ldfjac) = u2 * dxit - u1 * u2 * dxiu1 / tt + (1 - u2 * u2) * dxiu2 / tt - u2 * u3 * dxiu3 / tt;
+
+ *MIJ(jac, 5, i, ldfjac) = u3 * dxit - u1 * u3 * dxiu1 / tt - u2 * u3 * dxiu2 / tt + (1 - u3 * u3) * dxiu3 / tt;
+ } else {
*MIJ(jac, 3, i, ldfjac) = 0.0;
*MIJ(jac, 4, i, ldfjac) = 0.0;
*MIJ(jac, 5, i, ldfjac) = 0.0;
}
*MIJ(jac, 0, npt + i, ldfjac) = 0.0;
*MIJ(jac, 1, npt + i, ldfjac) = 1 / rz;
- *MIJ(jac, 2, npt + i, ldfjac) = - ry / (rz * rz);
- if (tt >= MINIMUM)
- {
- *MIJ(jac, 3, npt + i, ldfjac) = u1 * dyit + (1 - u1 * u1) * dyiu1 / tt
- - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
- *MIJ(jac, 4, npt + i, ldfjac) = u2 * dyit - u1 * u2 * dyiu1 / tt
- + (1 - u2 * u2) * dyiu2 / tt- u2 * u3 * dyiu3 / tt;
- *MIJ(jac, 5, npt + i, ldfjac) = u3 * dyit - u1 * u3 * dyiu1 / tt
- - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
- }
- else
- {
+ *MIJ(jac, 2, npt + i, ldfjac) = -ry / (rz * rz);
+ if (tt >= MINIMUM) {
+ *MIJ(jac, 3, npt + i, ldfjac) =
+ u1 * dyit + (1 - u1 * u1) * dyiu1 / tt - u1 * u2 * dyiu2 / tt - u1 * u3 * dyiu3 / tt;
+ *MIJ(jac, 4, npt + i, ldfjac) =
+ u2 * dyit - u1 * u2 * dyiu1 / tt + (1 - u2 * u2) * dyiu2 / tt - u2 * u3 * dyiu3 / tt;
+ *MIJ(jac, 5, npt + i, ldfjac) =
+ u3 * dyit - u1 * u3 * dyiu1 / tt - u2 * u3 * dyiu2 / tt + (1 - u3 * u3) * dyiu3 / tt;
+ } else {
*MIJ(jac, 3, npt + i, ldfjac) = 0.0;
*MIJ(jac, 4, npt + i, ldfjac) = 0.0;
*MIJ(jac, 5, npt + i, ldfjac) = 0.0;
@@ -274,57 +259,53 @@ the levenberg marquartd approach.
The approach has been proposed by D.G Lowe in 1992 paper \cite Lowe92a.
*/
-void
-vpPose::poseLowe(vpHomogeneousMatrix & cMo)
+void vpPose::poseLowe(vpHomogeneousMatrix &cMo)
{
#if (DEBUG_LEVEL1)
std::cout << "begin CCalcuvpPose::PoseLowe(...) " << std::endl;
#endif
- int n, m; /* nombre d'elements dans la matrice jac */
- int lwa; /* taille du vecteur wa */
- int ldfjac; /* taille maximum d'une ligne de jac */
- int info, ipvt[NBR_PAR];
- int tst_lmder;
+ int n, m; /* nombre d'elements dans la matrice jac */
+ int lwa; /* taille du vecteur wa */
+ int ldfjac; /* taille maximum d'une ligne de jac */
+ int info, ipvt[NBR_PAR];
+ int tst_lmder;
double f[2 * NBPTMAX], sol[NBR_PAR];
- double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
+ double tol, jac[NBR_PAR][2 * NBPTMAX], wa[2 * NBPTMAX + 50];
// double u[3]; /* vecteur de rotation */
// double rd[3][3]; /* matrice de rotation */
- n = NBR_PAR; /* nombres d'inconnues */
- m = (int)(2 * npt); /* nombres d'equations */
- lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
- ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
- tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
+ n = NBR_PAR; /* nombres d'inconnues */
+ m = (int)(2 * npt); /* nombres d'equations */
+ lwa = 2 * NBPTMAX + 50; /* taille du vecteur de travail */
+ ldfjac = 2 * NBPTMAX; /* nombre d'elements max sur une ligne */
+ tol = std::numeric_limits<double>::epsilon(); /* critere d'arret */
// c = cam ;
// for (i=0;i<3;i++)
// for (j=0;j<3;j++) rd[i][j] = cMo[i][j];
// mat_rot(rd,u);
- vpRotationMatrix cRo ;
- cMo.extract(cRo) ;
- vpThetaUVector u(cRo) ;
- for (unsigned int i=0;i<3;i++)
- {
+ vpRotationMatrix cRo;
+ cMo.extract(cRo);
+ vpThetaUVector u(cRo);
+ for (unsigned int i = 0; i < 3; i++) {
sol[i] = cMo[i][3];
- sol[i+3] = u[i];
+ sol[i + 3] = u[i];
}
- vpPoint P ;
- unsigned int i_=0;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ vpPoint P;
+ unsigned int i_ = 0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- XI[i_] = P.get_x();//*cam.px + cam.xc ;
- YI[i_] = P.get_y() ;//;*cam.py + cam.yc ;
+ XI[i_] = P.get_x(); //*cam.px + cam.xc ;
+ YI[i_] = P.get_y(); //;*cam.py + cam.yc ;
XO[i_] = P.get_oX();
YO[i_] = P.get_oY();
ZO[i_] = P.get_oZ();
++i_;
}
- tst_lmder = lmder1 (&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
- if (tst_lmder == -1)
- {
- std::cout << " in CCalculPose::PoseLowe(...) : " ;
+ tst_lmder = lmder1(&fcn, m, n, sol, f, &jac[0][0], ldfjac, tol, &info, ipvt, lwa, wa);
+ if (tst_lmder == -1) {
+ std::cout << " in CCalculPose::PoseLowe(...) : ";
std::cout << "pb de minimisation, returns FATAL_ERROR";
// return FATAL_ERROR ;
}
@@ -332,16 +313,15 @@ vpPose::poseLowe(vpHomogeneousMatrix & cMo)
for (unsigned int i = 0; i < 3; i++)
u[i] = sol[i + 3];
- for (unsigned int i=0;i<3;i++)
- {
+ for (unsigned int i = 0; i < 3; i++) {
cMo[i][3] = sol[i];
- u[i] = sol[i+3];
+ u[i] = sol[i + 3];
}
- vpRotationMatrix rd(u) ;
- cMo.insert(rd) ;
- // rot_mat(u,rd);
- // for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
+ vpRotationMatrix rd(u);
+ cMo.insert(rd);
+// rot_mat(u,rd);
+// for (i=0;i<3;i++) for (j=0;j<3;j++) cMo[i][j] = rd[i][j];
#if (DEBUG_LEVEL1)
std::cout << "end CCalculPose::PoseLowe(...) " << std::endl;
@@ -349,16 +329,13 @@ vpPose::poseLowe(vpHomogeneousMatrix & cMo)
// return OK ;
}
-
#undef MINI
#undef MINIMUM
-
#undef DEBUG_LEVEL1
-
/*
-* Local variables:
-* c-basic-offset: 2
-* End:
-*/
+ * Local variables:
+ * c-basic-offset: 2
+ * End:
+ */
diff --git a/modules/vision/src/pose-estimation/vpPoseRansac.cpp b/modules/vision/src/pose-estimation/vpPoseRansac.cpp
index 25fd382..ea0f0be 100644
--- a/modules/vision/src/pose-estimation/vpPoseRansac.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseRansac.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,219 +38,257 @@
*
*****************************************************************************/
-
/*!
\file vpPoseRansac.cpp
\brief function used to estimate a pose using the Ransac algorithm
*/
+#include <algorithm> // std::count
+#include <cmath> // std::fabs
+#include <float.h> // DBL_MAX
#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
-#include <stdlib.h>
-#include <algorithm> // std::count
-#include <float.h> // DBL_MAX
+#include <limits> // numeric_limits
#include <map>
+#include <stdlib.h>
-#include <visp3/vision/vpPose.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpRansac.h>
+#include <visp3/vision/vpPose.h>
#include <visp3/vision/vpPoseException.h>
-#include <visp3/core/vpMath.h>
-#if defined (VISP_HAVE_CPP11_COMPATIBILITY)
-# include <unordered_map>
+#if defined(VISP_HAVE_CPP11_COMPATIBILITY)
+#include <unordered_map>
#endif
#if defined(VISP_HAVE_OPENMP)
-# include <omp.h>
+#include <omp.h>
#endif
#define eps 1e-6
-
-namespace {
-//For std::map<vpPoint>
+namespace
+{
+// For std::map<vpPoint>
struct ComparePointDuplicate {
- bool operator()( const vpPoint &point1, const vpPoint &point2 ) const {
- if (point1.oP[0] < point2.oP[0]) return true;
- if (point1.oP[0] > point2.oP[0]) return false;
+ bool operator()(const vpPoint &point1, const vpPoint &point2) const
+ {
+ if (point1.oP[0] < point2.oP[0])
+ return true;
+ if (point1.oP[0] > point2.oP[0])
+ return false;
- if (point1.oP[1] < point2.oP[1]) return true;
- if (point1.oP[1] > point2.oP[1]) return false;
+ if (point1.oP[1] < point2.oP[1])
+ return true;
+ if (point1.oP[1] > point2.oP[1])
+ return false;
- if (point1.oP[2] < point2.oP[2]) return true;
- if (point1.oP[2] > point2.oP[2]) return false;
+ if (point1.oP[2] < point2.oP[2])
+ return true;
+ if (point1.oP[2] > point2.oP[2])
+ return false;
- if (point1.p[0] < point2.p[0]) return true;
- if (point1.p[0] > point2.p[0]) return false;
+ if (point1.p[0] < point2.p[0])
+ return true;
+ if (point1.p[0] > point2.p[0])
+ return false;
- if (point1.p[1] < point2.p[1]) return true;
- if (point1.p[1] > point2.p[1]) return false;
+ if (point1.p[1] < point2.p[1])
+ return true;
+ if (point1.p[1] > point2.p[1])
+ return false;
return false;
}
};
-//For std::map<vpPoint>
+// For std::map<vpPoint>
struct ComparePointAlmostDuplicate {
- bool operator()( const vpPoint &point1, const vpPoint &point2 ) const {
- if (point1.oP[0] - point2.oP[0] < -eps) return true;
- if (point1.oP[0] - point2.oP[0] > eps) return false;
+ bool operator()(const vpPoint &point1, const vpPoint &point2) const
+ {
+ if (point1.oP[0] - point2.oP[0] < -eps)
+ return true;
+ if (point1.oP[0] - point2.oP[0] > eps)
+ return false;
- if (point1.oP[1] - point2.oP[1] < -eps) return true;
- if (point1.oP[1] - point2.oP[1] > eps) return false;
+ if (point1.oP[1] - point2.oP[1] < -eps)
+ return true;
+ if (point1.oP[1] - point2.oP[1] > eps)
+ return false;
- if (point1.oP[2] - point2.oP[2] < -eps) return true;
- if (point1.oP[2] - point2.oP[2] > eps) return false;
+ if (point1.oP[2] - point2.oP[2] < -eps)
+ return true;
+ if (point1.oP[2] - point2.oP[2] > eps)
+ return false;
- if (point1.p[0] - point2.p[0] < -eps) return true;
- if (point1.p[0] - point2.p[0] > eps) return false;
+ if (point1.p[0] - point2.p[0] < -eps)
+ return true;
+ if (point1.p[0] - point2.p[0] > eps)
+ return false;
- if (point1.p[1] - point2.p[1] < -eps) return true;
- if (point1.p[1] - point2.p[1] > eps) return false;
+ if (point1.p[1] - point2.p[1] < -eps)
+ return true;
+ if (point1.p[1] - point2.p[1] > eps)
+ return false;
return false;
}
};
-//For std::map<vpPoint>
+// For std::map<vpPoint>
struct CompareObjectPointDegenerate {
- bool operator()( const vpPoint &point1, const vpPoint &point2 ) const {
- if (point1.oP[0] - point2.oP[0] < -eps) return true;
- if (point1.oP[0] - point2.oP[0] > eps) return false;
+ bool operator()(const vpPoint &point1, const vpPoint &point2) const
+ {
+ if (point1.oP[0] - point2.oP[0] < -eps)
+ return true;
+ if (point1.oP[0] - point2.oP[0] > eps)
+ return false;
- if (point1.oP[1] - point2.oP[1] < -eps) return true;
- if (point1.oP[1] - point2.oP[1] > eps) return false;
+ if (point1.oP[1] - point2.oP[1] < -eps)
+ return true;
+ if (point1.oP[1] - point2.oP[1] > eps)
+ return false;
- if (point1.oP[2] - point2.oP[2] < -eps) return true;
- if (point1.oP[2] - point2.oP[2] > eps) return false;
+ if (point1.oP[2] - point2.oP[2] < -eps)
+ return true;
+ if (point1.oP[2] - point2.oP[2] > eps)
+ return false;
return false;
}
};
-//For std::map<vpPoint>
+// For std::map<vpPoint>
struct CompareImagePointDegenerate {
- bool operator()( const vpPoint &point1, const vpPoint &point2 ) const {
- if (point1.p[0] - point2.p[0] < -eps) return true;
- if (point1.p[0] - point2.p[0] > eps) return false;
+ bool operator()(const vpPoint &point1, const vpPoint &point2) const
+ {
+ if (point1.p[0] - point2.p[0] < -eps)
+ return true;
+ if (point1.p[0] - point2.p[0] > eps)
+ return false;
- if (point1.p[1] - point2.p[1] < -eps) return true;
- if (point1.p[1] - point2.p[1] > eps) return false;
+ if (point1.p[1] - point2.p[1] < -eps)
+ return true;
+ if (point1.p[1] - point2.p[1] > eps)
+ return false;
return false;
}
};
-//std::find_if
+// std::find_if
struct FindDegeneratePoint {
- FindDegeneratePoint( const vpPoint &pt ) : m_pt(pt) { }
-
- 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 &&
- 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) );
+ explicit FindDegeneratePoint(const vpPoint &pt) : m_pt(pt) {}
+
+ 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 &&
+ 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));
}
vpPoint m_pt;
};
-#if defined (VISP_HAVE_CPP11_COMPATIBILITY)
-//For unordered_map<vpPoint>
+#if defined(VISP_HAVE_CPP11_COMPATIBILITY)
+// For unordered_map<vpPoint>
struct HashDuplicate {
- std::size_t operator()(const vpPoint &point) const {
+ std::size_t operator()(const vpPoint &point) const
+ {
using std::size_t;
using std::hash;
size_t res = 17;
- res = res * 31 + hash<double>()( point.oP[0] );
- res = res * 31 + hash<double>()( point.oP[1] );
- res = res * 31 + hash<double>()( point.oP[2] );
- res = res * 31 + hash<double>()( point.p[0] );
- res = res * 31 + hash<double>()( point.p[1] );
+ res = res * 31 + hash<double>()(point.oP[0]);
+ res = res * 31 + hash<double>()(point.oP[1]);
+ res = res * 31 + hash<double>()(point.oP[2]);
+ res = res * 31 + hash<double>()(point.p[0]);
+ res = res * 31 + hash<double>()(point.p[1]);
return res;
}
};
-//For unordered_map<vpPoint>
+// For unordered_map<vpPoint>
struct ComparePointDuplicateUnorderedMap {
- bool operator()( const vpPoint &point1, const vpPoint &point2 ) const {
- return (
- std::fabs(point1.oP[0] - point2.oP[0]) < std::numeric_limits<double>::epsilon() &&
- std::fabs(point1.oP[1] - point2.oP[1]) < std::numeric_limits<double>::epsilon() &&
- std::fabs(point1.oP[2] - point2.oP[2]) < std::numeric_limits<double>::epsilon() &&
- std::fabs(point1.p[0] - point2.p[0]) < std::numeric_limits<double>::epsilon() &&
- std::fabs(point1.p[1] - point2.p[1]) < std::numeric_limits<double>::epsilon()
- );
+ bool operator()(const vpPoint &point1, const vpPoint &point2) const
+ {
+ return (std::fabs(point1.oP[0] - point2.oP[0]) < std::numeric_limits<double>::epsilon() &&
+ std::fabs(point1.oP[1] - point2.oP[1]) < std::numeric_limits<double>::epsilon() &&
+ std::fabs(point1.oP[2] - point2.oP[2]) < std::numeric_limits<double>::epsilon() &&
+ std::fabs(point1.p[0] - point2.p[0]) < std::numeric_limits<double>::epsilon() &&
+ std::fabs(point1.p[1] - point2.p[1]) < std::numeric_limits<double>::epsilon());
}
};
#endif
}
-bool vpPose::RansacFunctor::poseRansacImpl() {
- unsigned int size = (unsigned int) m_listOfUniquePoints.size();
+bool vpPose::RansacFunctor::poseRansacImpl()
+{
+ unsigned int size = (unsigned int)m_listOfUniquePoints.size();
int nbTrials = 0;
unsigned int nbMinRandom = 4;
-#if defined(_WIN32) && defined(_MSC_VER)
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
srand(m_initial_seed);
#endif
+ vpPoint p; // Point used to project using the estimated pose
+
bool foundSolution = false;
- while (nbTrials < m_ransacMaxTrials && m_nbInliers < (unsigned int) m_ransacNbInlierConsensus)
- {
- //Hold the list of the index of the inliers (points in the consensus set)
+ 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<unsigned int> cur_consensus;
- //Hold the list of the index of the outliers
+ // Hold the list of the index of the outliers
std::vector<unsigned int> cur_outliers;
- //Hold the list of the index of the points randomly picked
+ // Hold the list of the index of the points randomly picked
std::vector<unsigned int> cur_randoms;
- //Hold the list of the current inliers points to avoid to add a degenerate point if the flag is set
+ // Hold the list of the current inliers points to avoid to add a
+ // degenerate point if the flag is set
std::vector<vpPoint> cur_inliers;
vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
- //Use a temporary variable because if not, the cMo passed in parameters will be modified when
- // we compute the pose for the minimal sample sets but if the pose is not correct when we pass
- // a function pointer we do not want to modify the cMo passed in parameters
+ // Use a temporary variable because if not, the cMo passed in parameters
+ // will be modified when
+ // we compute the pose for the minimal sample sets but if the pose is not
+ // correct when we pass a function pointer we do not want to modify the
+ // cMo passed in parameters
vpHomogeneousMatrix cMo_tmp;
- //Vector of used points, initialized at false for all points
+ // Vector of used points, initialized at false for all points
std::vector<bool> usedPt(size, false);
vpPose poseMin;
- for(unsigned int i = 0; i < nbMinRandom;)
- {
- if((size_t) std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
- //All points was picked once, break otherwise we stay in an infinite loop
+ for (unsigned int i = 0; i < nbMinRandom;) {
+ if ((size_t)std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
+ // All points was picked once, break otherwise we stay in an infinite
+ // loop
break;
}
- //Pick a point randomly
-#if defined(_WIN32) && defined(_MSC_VER)
- unsigned int r_ = (unsigned int) rand() % size;
+// Pick a point randomly
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
+ unsigned int r_ = (unsigned int)rand() % size;
#else
- unsigned int r_ = (unsigned int) rand_r(&m_initial_seed) % size;
+ unsigned int r_ = (unsigned int)rand_r(&m_initial_seed) % size;
#endif
- while(usedPt[r_]) {
- //If already picked, pick another point randomly
-#if defined(_WIN32) && defined(_MSC_VER)
- r_ = (unsigned int) rand() % size;
+ while (usedPt[r_]) {
+// If already picked, pick another point randomly
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
+ r_ = (unsigned int)rand() % size;
#else
- r_ = (unsigned int) rand_r(&m_initial_seed) % size;
+ r_ = (unsigned int)rand_r(&m_initial_seed) % size;
#endif
}
- //Mark this point as already picked
+ // Mark this point as already picked
usedPt[r_] = true;
vpPoint pt = m_listOfUniquePoints[r_];
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;
}
}
@@ -257,21 +296,21 @@ bool vpPose::RansacFunctor::poseRansacImpl() {
if (!degenerate) {
poseMin.addPoint(pt);
cur_randoms.push_back(r_);
- //Increment the number of points picked
+ // Increment the number of points picked
i++;
}
}
- if(poseMin.npt < nbMinRandom) {
+ if (poseMin.npt < nbMinRandom) {
nbTrials++;
continue;
}
- //Flags set if pose computation is OK
+ // Flags set if pose computation is OK
bool is_valid_lagrange = false;
bool is_valid_dementhon = false;
- //Set maximum value for residuals
+ // Set maximum value for residuals
double r_lagrange = DBL_MAX;
double r_dementhon = DBL_MAX;
@@ -279,87 +318,86 @@ bool vpPose::RansacFunctor::poseRansacImpl() {
poseMin.computePose(vpPose::LAGRANGE, cMo_lagrange);
r_lagrange = poseMin.computeResidual(cMo_lagrange);
is_valid_lagrange = true;
- } catch(...) { }
+ } catch (...) {
+ }
try {
poseMin.computePose(vpPose::DEMENTHON, cMo_dementhon);
r_dementhon = poseMin.computeResidual(cMo_dementhon);
is_valid_dementhon = true;
- } catch(...) { }
+ } catch (...) {
+ }
- //If residual returned is not a number (NAN), set valid to false
- if(vpMath::isNaN(r_lagrange)) {
+ // If residual returned is not a number (NAN), set valid to false
+ if (vpMath::isNaN(r_lagrange)) {
is_valid_lagrange = false;
r_lagrange = DBL_MAX;
}
- if(vpMath::isNaN(r_dementhon)) {
+ if (vpMath::isNaN(r_dementhon)) {
is_valid_dementhon = false;
r_dementhon = DBL_MAX;
}
- //If at least one pose computation is OK,
- //we can continue, otherwise pick another random set
- if(is_valid_lagrange || is_valid_dementhon) {
+ // If at least one pose computation is OK,
+ // we can continue, otherwise pick another random set
+ if (is_valid_lagrange || is_valid_dementhon) {
double r;
if (r_lagrange < r_dementhon) {
r = r_lagrange;
cMo_tmp = cMo_lagrange;
- }
- else {
+ } else {
r = r_dementhon;
cMo_tmp = cMo_dementhon;
}
- r = sqrt(r) / (double) nbMinRandom;
+ r = sqrt(r) / (double)nbMinRandom;
- //Filter the pose using some criterion (orientation angles, translations, etc.)
+ // Filter the pose using some criterion (orientation angles,
+ // translations, etc.)
bool isPoseValid = true;
- if(m_func != NULL) {
+ if (m_func != NULL) {
isPoseValid = m_func(&cMo_tmp);
- if(isPoseValid) {
+ if (isPoseValid) {
m_cMo = cMo_tmp;
}
} else {
- //No post filtering on pose, so copy cMo_temp to cMo
+ // No post filtering on pose, so copy cMo_temp to cMo
m_cMo = cMo_tmp;
}
- if (isPoseValid && r < m_ransacThreshold)
- {
+ if (isPoseValid && r < m_ransacThreshold) {
unsigned int nbInliersCur = 0;
unsigned int iter = 0;
- for (std::vector<vpPoint>::const_iterator it = m_listOfUniquePoints.begin(); it != m_listOfUniquePoints.end(); ++it, iter++)
- {
- vpPoint pt = *it;
- vpPoint p(pt) ;
- p.track(m_cMo) ;
+ for (std::vector<vpPoint>::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() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.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) {
+ if (error < m_ransacThreshold) {
bool degenerate = false;
if (m_checkDegeneratePoints) {
- if ( std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(pt)) != cur_inliers.end() ) {
+ if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) != cur_inliers.end()) {
degenerate = true;
}
}
if (!degenerate) {
- // the point is considered as inlier if the error is below the threshold
+ // the point is considered as inlier if the error is below the
+ // threshold
nbInliersCur++;
cur_consensus.push_back(iter);
cur_inliers.push_back(*it);
} else {
cur_outliers.push_back(iter);
}
- }
- else {
+ } else {
cur_outliers.push_back(iter);
}
}
- if(nbInliersCur > m_nbInliers)
- {
+ if (nbInliersCur > m_nbInliers) {
foundSolution = true;
m_best_consensus = cur_consensus;
m_nbInliers = nbInliersCur;
@@ -367,11 +405,10 @@ bool vpPose::RansacFunctor::poseRansacImpl() {
nbTrials++;
- if(nbTrials >= m_ransacMaxTrials) {
+ if (nbTrials >= m_ransacMaxTrials) {
foundSolution = true;
}
- }
- else {
+ } else {
nbTrials++;
}
} else {
@@ -383,8 +420,9 @@ bool vpPose::RansacFunctor::poseRansacImpl() {
}
#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
-vpThread::Return vpPose::poseRansacImplThread(vpThread::Args arg) {
- vpPose::RansacFunctor* f = reinterpret_cast<vpPose::RansacFunctor*>(arg);
+vpThread::Return vpPose::poseRansacImplThread(vpThread::Args arg)
+{
+ vpPose::RansacFunctor *f = reinterpret_cast<vpPose::RansacFunctor *>(arg);
(*f)();
return 0;
}
@@ -394,14 +432,15 @@ vpThread::Return vpPose::poseRansacImplThread(vpThread::Args arg) {
Compute the pose using the Ransac approach.
\param cMo : Computed pose
- \param func : Pointer to a function that takes in parameter a vpHomogeneousMatrix
- and returns true if the pose check is OK or false otherwise
- \return True if we found at least 4 points with a reprojection error below ransacThreshold.
+ \param func : Pointer to a function that takes in parameter a
+ vpHomogeneousMatrix and returns true if the pose check is OK or false
+ otherwise \return True if we found at least 4 points with a reprojection
+ error below ransacThreshold.
*/
-bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMatrix *))
+bool vpPose::poseRansac(vpHomogeneousMatrix &cMo, bool (*func)(vpHomogeneousMatrix *))
{
- //Check only for adding / removing problem
- //Do not take into account problem with element modification here
+ // Check only for adding / removing problem
+ // Do not take into account problem with element modification here
if (listP.size() != listOfPoints.size()) {
std::cerr << "You should not modify vpPose::listP!" << std::endl;
listOfPoints = std::vector<vpPoint>(listP.begin(), listP.end());
@@ -416,123 +455,131 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
if (listOfPoints.size() < 4) {
- //vpERROR_TRACE("Not enough point to compute the pose");
- throw(vpPoseException(vpPoseException::notInitializedError,
- "Not enough point to compute the pose")) ;
+ // vpERROR_TRACE("Not enough point to compute the pose");
+ throw(vpPoseException(vpPoseException::notInitializedError, "Not enough point to compute the pose"));
}
std::vector<vpPoint> listOfUniquePoints;
std::map<size_t, size_t> mapOfUniquePointIndex;
- //Get RANSAC flags
+ // 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 checkDegeneratePoints = (ransacFlags & CHECK_DEGENERATE_POINTS) != 0;
if (prefilterDuplicatePoints || prefilterAlmostDuplicatePoints || prefilterDegeneratePoints) {
- //Prefiltering
+ // Prefiltering
if (prefilterDuplicatePoints) {
-#if defined (VISP_HAVE_CPP11_COMPATIBILITY)
+#if defined(VISP_HAVE_CPP11_COMPATIBILITY)
std::unordered_map<vpPoint, size_t, HashDuplicate, ComparePointDuplicateUnorderedMap> filterMap;
size_t index_pt = 0;
- for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); ++it_pt, index_pt++) {
+ for (std::vector<vpPoint>::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;
listOfUniquePoints.push_back(*it_pt);
- mapOfUniquePointIndex[listOfUniquePoints.size()-1] = index_pt;
+ mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = index_pt;
}
}
#else
std::map<vpPoint, size_t, ComparePointDuplicate> filterMap;
size_t index_pt = 0;
- for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); ++it_pt, index_pt++) {
+ for (std::vector<vpPoint>::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;
listOfUniquePoints.push_back(*it_pt);
- mapOfUniquePointIndex[listOfUniquePoints.size()-1] = index_pt;
+ mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = index_pt;
}
}
#endif
} else if (prefilterAlmostDuplicatePoints) {
std::map<vpPoint, size_t, ComparePointAlmostDuplicate> filterMap;
size_t index_pt = 0;
- for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); ++it_pt, index_pt++) {
+ for (std::vector<vpPoint>::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;
listOfUniquePoints.push_back(*it_pt);
- mapOfUniquePointIndex[listOfUniquePoints.size()-1] = index_pt;
+ mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = index_pt;
}
}
} else {
- //Remove other degenerate object points
+ // Remove other degenerate object points
std::map<vpPoint, size_t, CompareObjectPointDegenerate> filterObjectPointMap;
size_t index_pt = 0;
- for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); ++it_pt, index_pt++) {
+ for (std::vector<vpPoint>::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<vpPoint, size_t, CompareImagePointDegenerate> filterImagePointMap;
- for (std::map<vpPoint, size_t>::const_iterator it = filterObjectPointMap.begin(); it != filterObjectPointMap.end(); ++it) {
+ for (std::map<vpPoint, size_t>::const_iterator it = filterObjectPointMap.begin();
+ it != filterObjectPointMap.end(); ++it) {
if (filterImagePointMap.find(it->first) == filterImagePointMap.end()) {
filterImagePointMap[it->first] = it->second;
listOfUniquePoints.push_back(it->first);
- mapOfUniquePointIndex[listOfUniquePoints.size()-1] = it->second;
+ mapOfUniquePointIndex[listOfUniquePoints.size() - 1] = it->second;
}
}
}
} else {
- //No prefiltering
+ // No prefiltering
listOfUniquePoints = listOfPoints;
size_t index_pt = 0;
- for (std::vector<vpPoint>::const_iterator it_pt = listOfPoints.begin(); it_pt != listOfPoints.end(); ++it_pt, index_pt++) {
+ for (std::vector<vpPoint>::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();
+ 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;
-#if defined (VISP_HAVE_PTHREAD) || (defined (_WIN32) && !defined(WINRT_8_0))
-# define VP_THREAD_OK
+#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0))
+#define VP_THREAD_OK
int nbThreads = 1;
#endif
if (executeParallelVersion) {
-#if !defined (VP_THREAD_OK) && !defined (VISP_HAVE_OPENMP)
+#if !defined(VP_THREAD_OK) && !defined(VISP_HAVE_OPENMP)
executeParallelVersion = false;
- std::cerr << "Pthread or WIN32 API or OpenMP is needed to use the parallel RANSAC version." << std::endl;
-#elif !defined (VP_THREAD_OK)
- //Use OpenMP
+ std::cerr << "Pthread or WIN32 API or OpenMP is needed to use the "
+ "parallel RANSAC version."
+ << std::endl;
+#elif !defined(VP_THREAD_OK)
+// Use OpenMP
#define PARALLEL_RANSAC_OPEN_MP
-#elif !defined (VISP_HAVE_OPENMP)
- if(nbParallelRansacThreads <= 0) {
- //Cannot get the number of CPU threads so use the sequential mode
+#elif !defined(VISP_HAVE_OPENMP)
+ if (nbParallelRansacThreads <= 0) {
+ // Cannot get the number of CPU threads so use the sequential mode
executeParallelVersion = false;
- std::cerr << "OpenMP is needed to get the number of CPU threads so use the sequential mode instead." << std::endl;
+ std::cerr << "OpenMP is needed to get the number of CPU threads so use "
+ "the sequential mode instead."
+ << std::endl;
} else {
nbThreads = nbParallelRansacThreads;
if (nbThreads == 1) {
executeParallelVersion = false;
}
}
-#elif defined (VP_THREAD_OK) && defined (VISP_HAVE_OPENMP)
+#elif defined(VP_THREAD_OK) && defined(VISP_HAVE_OPENMP)
if (nbParallelRansacThreads <= 0) {
- //Use OpenMP to get the number of CPU threads
+ // Use OpenMP to get the number of CPU threads
nbThreads = omp_get_max_threads();
- if(nbThreads <= 1) {
+ if (nbThreads <= 1) {
nbThreads = 1;
executeParallelVersion = false;
}
@@ -542,88 +589,98 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
bool foundSolution = false;
- if(executeParallelVersion) {
-#if defined (PARALLEL_RANSAC_OPEN_MP)
- //List of points picked randomly (minimal sample set, MSS)
- //std::vector<unsigned int> best_randoms; // never used
-
- //Section of code run in parallel
- //All variables declared before the parallel keyword are shared between the team of threads (if private keyword is not used)
- //Code in parallel section are duplicated between the team of threads
+ if (executeParallelVersion) {
+#if defined(PARALLEL_RANSAC_OPEN_MP)
+// List of points picked randomly (minimal sample set, MSS)
+// std::vector<unsigned int> best_randoms; // never used
+
+// Section of code run in parallel
+// All variables declared before the parallel keyword are shared between the
+// team of threads (if private keyword is not used) Code in parallel section
+// are duplicated between the team of threads
#pragma omp parallel
{
#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
+ // 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) (int(time(NULL)) ^ omp_get_thread_num());
- if(omp_get_num_threads() == 1) {
- initial_seed = 0; //Same seed at each run
+ if (omp_get_num_threads() == 1) {
+ initial_seed = 0; // Same seed at each run
}
#else
unsigned int initial_seed = 0;
#endif
-#if defined(_WIN32) && defined(_MSC_VER)
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
srand(initial_seed);
#endif
unsigned int nbMinRandom = 4;
- //Numbers of points in the best consensus set
+ // Numbers of points in the best consensus set
unsigned int nb_best_inliers = 0;
- //True if we found a consensus set with a size > ransacNbInlierConsensus
+ // True if we found a consensus set with a size >
+ // ransacNbInlierConsensus
bool foundSolutionWithConsensus = false;
+ vpPoint p; // Point used to project using the estimated pose
+
#pragma omp for
- for(int nbTrials = 0; nbTrials < ransacMaxTrials; nbTrials++) {
- //Flag to check if a solution has been founded, used to "cancel" the threads
- if(!foundSolutionWithConsensus) {
- //Hold the list of the index of the inliers (points in the consensus set)
+ for (int nbTrials = 0; nbTrials < ransacMaxTrials; nbTrials++) {
+ // Flag to check if a solution has been founded, used to "cancel" the
+ // threads
+ if (!foundSolutionWithConsensus) {
+ // Hold the list of the index of the inliers (points in the
+ // consensus set)
std::vector<unsigned int> cur_consensus;
- //Hold the list of the index of the outliers
+ // Hold the list of the index of the outliers
std::vector<unsigned int> cur_outliers;
- //Hold the list of the index of the points randomly picked
+ // Hold the list of the index of the points randomly picked
std::vector<unsigned int> cur_randoms;
- //Hold the list of the current inliers points to avoid to add a degenerate point if the flag is set
+ // Hold the list of the current inliers points to avoid to add a
+ // degenerate point if the flag is set
std::vector<vpPoint> cur_inliers;
vpHomogeneousMatrix cMo_lagrange, cMo_dementhon;
vpHomogeneousMatrix cMo_tmp;
- //Vector of used points, initialized at false for all points
+ // Vector of used points, initialized at false for all points
std::vector<bool> usedPt(size, false);
vpPose poseMin;
- for(unsigned int i = 0; i < nbMinRandom;) {
- if((size_t) std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
- //All points was picked once, break otherwise we stay in an infinite loop
+ for (unsigned int i = 0; i < nbMinRandom;) {
+ if ((size_t)std::count(usedPt.begin(), usedPt.end(), true) == usedPt.size()) {
+ // All points was picked once, break otherwise we stay in an
+ // infinite loop
break;
}
- //Pick a point randomly
-#if defined(_WIN32) && defined(_MSC_VER)
- unsigned int r_ = (unsigned int) rand() % size;
+// Pick a point randomly
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
+ unsigned int r_ = (unsigned int)rand() % size;
#else
- unsigned int r_ = (unsigned int) rand_r(&initial_seed) % size;
+ unsigned int r_ = (unsigned int)rand_r(&initial_seed) % size;
#endif
- while(usedPt[r_]) {
- //If already picked, pick another point randomly
-#if defined(_WIN32) && defined(_MSC_VER)
- r_ = (unsigned int) rand() % size;
+ while (usedPt[r_]) {
+// If already picked, pick another point randomly
+#if defined(_WIN32) && (defined(_MSC_VER) || defined(__MINGW32__))
+ r_ = (unsigned int)rand() % size;
#else
- r_ = (unsigned int) rand_r(&initial_seed) % size;
+ r_ = (unsigned int)rand_r(&initial_seed) % size;
#endif
}
- //Mark this point as already picked
+ // Mark this point as already picked
usedPt[r_] = true;
vpPoint pt = listOfUniquePoints[r_];
bool degenerate = false;
if (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;
}
}
@@ -631,17 +688,17 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
if (!degenerate) {
poseMin.addPoint(pt);
cur_randoms.push_back(r_);
- //Increment the number of points picked
+ // Increment the number of points picked
i++;
}
}
- if(poseMin.npt >= nbMinRandom) {
- //Flags set if pose computation is OK
+ if (poseMin.npt >= nbMinRandom) {
+ // Flags set if pose computation is OK
bool is_valid_lagrange = false;
bool is_valid_dementhon = false;
- //Set maximum value for residuals
+ // Set maximum value for residuals
double r = DBL_MAX;
double r_lagrange = DBL_MAX;
double r_dementhon = DBL_MAX;
@@ -650,64 +707,68 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
poseMin.computePose(vpPose::LAGRANGE, cMo_lagrange);
r_lagrange = poseMin.computeResidual(cMo_lagrange);
is_valid_lagrange = true;
- } catch(...) { }
+ } catch (...) {
+ }
try {
poseMin.computePose(vpPose::DEMENTHON, cMo_dementhon);
r_dementhon = poseMin.computeResidual(cMo_dementhon);
is_valid_dementhon = true;
- } catch(...) { }
+ } catch (...) {
+ }
- //If residual returned is not a number (NAN), set valid to false
- if(vpMath::isNaN(r_lagrange)) {
+ // If residual returned is not a number (NAN), set valid to false
+ if (vpMath::isNaN(r_lagrange)) {
is_valid_lagrange = false;
r_lagrange = DBL_MAX;
}
- if(vpMath::isNaN(r_dementhon)) {
+ if (vpMath::isNaN(r_dementhon)) {
is_valid_dementhon = false;
r_dementhon = DBL_MAX;
}
- //If at least one pose computation is OK,
- //we can continue, otherwise pick another random set
- if(is_valid_lagrange || is_valid_dementhon) {
+ // If at least one pose computation is OK,
+ // we can continue, otherwise pick another random set
+ if (is_valid_lagrange || is_valid_dementhon) {
if (r_lagrange < r_dementhon) {
r = r_lagrange;
cMo_tmp = cMo_lagrange;
- }
- else {
+ } else {
r = r_dementhon;
cMo_tmp = cMo_dementhon;
}
- r = sqrt(r) / (double) nbMinRandom;
+ r = sqrt(r) / (double)nbMinRandom;
- //Filter the pose using some criterion (orientation angles, translations, etc.)
+ // Filter the pose using some criterion (orientation angles,
+ // translations, etc.)
bool isPoseValid = true;
- if(func != NULL) {
+ if (func != NULL) {
isPoseValid = func(&cMo_tmp);
}
if (isPoseValid && r < ransacThreshold) {
unsigned int nbInliersCur = 0;
unsigned int iter = 0;
- for (std::vector<vpPoint>::const_iterator it = listOfUniquePoints.begin(); it != listOfUniquePoints.end(); ++it, iter++) {
- vpPoint pt = *it;
- vpPoint p(pt) ;
- p.track(cMo_tmp) ;
-
- double d = vpMath::sqr(p.get_x() - pt.get_x()) + vpMath::sqr(p.get_y() - pt.get_y()) ;
- double error = sqrt(d) ;
- if(error < ransacThreshold) {
+ for (std::vector<vpPoint>::const_iterator it = listOfUniquePoints.begin();
+ it != listOfUniquePoints.end(); ++it, iter++) {
+ 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 error = sqrt(d);
+ if (error < ransacThreshold) {
bool degenerate = false;
if (checkDegeneratePoints) {
- if ( std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(pt)) != cur_inliers.end() ) {
+ if (std::find_if(cur_inliers.begin(), cur_inliers.end(), FindDegeneratePoint(*it)) !=
+ cur_inliers.end()) {
degenerate = true;
}
}
if (!degenerate) {
- // the point is considered as inlier if the error is below the threshold
+ // the point is considered as inlier if the error is
+ // below the threshold
nbInliersCur++;
cur_consensus.push_back(iter);
cur_inliers.push_back(*it);
@@ -719,15 +780,15 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
}
}
-#pragma omp critical (update_best_consensus_set)
+#pragma omp critical(update_best_consensus_set)
{
- if(nbInliersCur > nb_best_inliers) {
+ if (nbInliersCur > nb_best_inliers) {
foundSolution = true;
best_consensus = cur_consensus;
- //best_randoms = cur_randoms; // never used
+ // best_randoms = cur_randoms; // never used
nb_best_inliers = nbInliersCur;
- if(nbInliersCur >= ransacNbInlierConsensus) {
+ if (nbInliersCur >= ransacNbInlierConsensus) {
foundSolutionWithConsensus = true;
}
}
@@ -741,42 +802,42 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
nbInliers = best_consensus.size();
}
#elif defined(VP_THREAD_OK)
- std::vector<vpThread *> threads((size_t) nbThreads);
- std::vector<RansacFunctor> ransac_func((size_t) nbThreads);
+ std::vector<vpThread *> threads((size_t)nbThreads);
+ std::vector<RansacFunctor> ransac_func((size_t)nbThreads);
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);
- if(i < (size_t) nbThreads-1) {
- ransac_func[i] = RansacFunctor(cMo, ransacNbInlierConsensus, splitTrials, ransacThreshold,
- initial_seed, checkDegeneratePoints, listOfUniquePoints, func);
+ for (size_t i = 0; i < (size_t)nbThreads; 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);
} 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
+ // Get the best pose between the threads
vpPose final_pose;
- for(std::vector<vpPoint>::const_iterator it = listOfPoints.begin(); it != listOfPoints.end(); ++it) {
+ for (std::vector<vpPoint>::const_iterator it = listOfPoints.begin(); it != listOfPoints.end(); ++it) {
final_pose.addPoint(*it);
}
- for(size_t i = 0; i < (size_t) nbThreads; i++) {
+ for (size_t i = 0; i < (size_t)nbThreads; i++) {
threads[i]->join();
delete threads[i];
}
bool successRansac = false;
size_t best_consensus_size = 0;
- for(size_t i = 0; i < (size_t) nbThreads; i++) {
- if(ransac_func[i].getResult()) {
+ for (size_t i = 0; i < (size_t)nbThreads; i++) {
+ if (ransac_func[i].getResult()) {
successRansac = true;
- if(ransac_func[i].getBestConsensus().size() > best_consensus_size) {
+ if (ransac_func[i].getBestConsensus().size() > best_consensus_size) {
nbInliers = ransac_func[i].getNbInliers();
best_consensus = ransac_func[i].getBestConsensus();
best_consensus_size = ransac_func[i].getBestConsensus().size();
@@ -787,9 +848,9 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
foundSolution = successRansac;
#endif
} else {
- //Sequential RANSAC
- RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold,
- 0, checkDegeneratePoints, listOfUniquePoints, func);
+ // Sequential RANSAC
+ RansacFunctor sequentialRansac(cMo, ransacNbInlierConsensus, ransacMaxTrials, ransacThreshold, 0,
+ checkDegeneratePoints, listOfUniquePoints, func);
sequentialRansac();
foundSolution = sequentialRansac.getResult();
@@ -799,11 +860,11 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
}
}
- if(foundSolution) {
+ if (foundSolution) {
unsigned int nbMinRandom = 4;
// std::cout << "Nombre d'inliers " << nbInliers << std::endl ;
- //Display the random picked points
+ // Display the random picked points
/*
std::cout << "Randoms : ";
for(unsigned int i = 0 ; i < cur_randoms.size() ; i++)
@@ -811,7 +872,7 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
std::cout << std::endl;
*/
- //Display the outliers
+ // Display the outliers
/*
std::cout << "Outliers : ";
for(unsigned int i = 0 ; i < cur_outliers.size() ; i++)
@@ -819,32 +880,34 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
std::cout << std::endl;
*/
- //Even if the cardinality of the best consensus set is inferior to ransacNbInlierConsensus,
- //we want to refine the solution with data in best_consensus and return this pose.
- //This is an approach used for example in p118 in Multiple View Geometry in Computer Vision, Hartley, R.~I. and Zisserman, A.
- if(nbInliers >= nbMinRandom) //if(nbInliers >= (unsigned)ransacNbInlierConsensus)
+ // Even if the cardinality of the best consensus set is inferior to
+ // ransacNbInlierConsensus, we want to refine the solution with data in
+ // best_consensus and return this pose. This is an approach used for
+ // example in p118 in Multiple View Geometry in Computer Vision, Hartley,
+ // R.~I. and Zisserman, A.
+ if (nbInliers >= nbMinRandom) // if(nbInliers >= (unsigned)ransacNbInlierConsensus)
{
- //Refine the solution using all the points in the consensus set and with VVS pose estimation
+ // Refine the solution using all the points in the consensus set and
+ // with VVS pose estimation
vpPose pose;
- for(size_t i = 0 ; i < best_consensus.size(); i++)
- {
+ for (size_t i = 0; i < best_consensus.size(); i++) {
vpPoint pt = listOfUniquePoints[best_consensus[i]];
- pose.addPoint(pt) ;
+ pose.addPoint(pt);
ransacInliers.push_back(pt);
}
- //Update the list of inlier index
- for(std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
- it_index != best_consensus.end(); ++it_index) {
- ransacInlierIndex.push_back((unsigned int) mapOfUniquePointIndex[*it_index]);
+ // Update the list of inlier index
+ for (std::vector<unsigned int>::const_iterator it_index = best_consensus.begin();
+ it_index != best_consensus.end(); ++it_index) {
+ ransacInlierIndex.push_back((unsigned int)mapOfUniquePointIndex[*it_index]);
}
- //Flags set if pose computation is OK
+ // Flags set if pose computation is OK
bool is_valid_lagrange = false;
bool is_valid_dementhon = false;
- //Set maximum value for residuals
+ // Set maximum value for residuals
double r_lagrange = DBL_MAX;
double r_dementhon = DBL_MAX;
@@ -852,43 +915,45 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
r_lagrange = pose.computeResidual(cMo_lagrange);
is_valid_lagrange = true;
- } catch(...) { }
+ } catch (...) {
+ }
try {
pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
r_dementhon = pose.computeResidual(cMo_dementhon);
is_valid_dementhon = true;
- } catch(...) { }
+ } catch (...) {
+ }
- //If residual returned is not a number (NAN), set valid to false
- if(vpMath::isNaN(r_lagrange)) {
+ // If residual returned is not a number (NAN), set valid to false
+ if (vpMath::isNaN(r_lagrange)) {
is_valid_lagrange = false;
r_lagrange = DBL_MAX;
}
- if(vpMath::isNaN(r_dementhon)) {
+ if (vpMath::isNaN(r_dementhon)) {
is_valid_dementhon = false;
r_dementhon = DBL_MAX;
}
- if(is_valid_lagrange || is_valid_dementhon) {
+ if (is_valid_lagrange || is_valid_dementhon) {
if (r_lagrange < r_dementhon) {
cMo = cMo_lagrange;
- }
- else {
+ } else {
cMo = cMo_dementhon;
}
pose.setCovarianceComputation(computeCovariance);
pose.computePose(vpPose::VIRTUAL_VS, cMo);
- //In some rare cases, the final pose could not respect the pose criterion even
- //if the 4 minimal points picked respect the pose criterion.
- if(func != NULL && !func(&cMo)) {
+ // In some rare cases, the final pose could not respect the pose
+ // criterion even if the 4 minimal points picked respect the pose
+ // criterion.
+ if (func != NULL && !func(&cMo)) {
return false;
}
- if(computeCovariance) {
+ if (computeCovariance) {
covarianceMatrix = pose.covarianceMatrix;
}
}
@@ -902,25 +967,29 @@ bool vpPose::poseRansac(vpHomogeneousMatrix & cMo, bool (*func)(vpHomogeneousMat
/*!
Compute the number of RANSAC iterations to ensure with a probability \e p
- that at least one of the random samples of \e s points is free from outliers.
- \note See: Hartley and Zisserman, Multiple View Geometry in Computer Vision, p119 (2. How many samples?).
-
- \param probability : Probability that at least one of the random samples is free from outliers (typically p=0.99).
- \param epsilon : Probability that a selected point is an outlier (between 0 and 1).
- \param sampleSize : Minimum number of points to estimate the model (4 for a pose estimation).
- \param maxIterations : Upper bound on the number of iterations or -1 for INT_MAX.
+ that at least one of the random samples of \e s points is free from
+ outliers. \note See: Hartley and Zisserman, Multiple View Geometry in
+ Computer Vision, p119 (2. How many samples?).
+
+ \param probability : Probability that at least one of the random samples is
+ free from outliers (typically p=0.99). \param epsilon : Probability that a
+ selected point is an outlier (between 0 and 1). \param sampleSize : Minimum
+ number of points to estimate the model (4 for a pose estimation). \param
+ maxIterations : Upper bound on the number of iterations or -1 for INT_MAX.
\return The number of RANSAC iterations to ensure with a probability \e p
- that at least one of the random samples of \e s points is free from outliers or \p maxIterations if it exceeds
- the desired upper bound or \e INT_MAX if maxIterations=-1.
+ that at least one of the random samples of \e s points is free from outliers
+ 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) {
- probability = std::max(probability, 0.0);
- probability = std::min(probability, 1.0);
- epsilon = std::max(epsilon, 0.0);
- epsilon = std::min(epsilon, 1.0);
+int vpPose::computeRansacIterations(double probability, double epsilon, const int sampleSize, int maxIterations)
+{
+ probability = (std::max)(probability, 0.0);
+ probability = (std::min)(probability, 1.0);
+ epsilon = (std::max)(epsilon, 0.0);
+ epsilon = (std::min)(epsilon, 1.0);
if (vpMath::nul(epsilon)) {
- //no outliers
+ // no outliers
return 1;
}
@@ -930,16 +999,21 @@ int vpPose::computeRansacIterations(double probability, double epsilon, const in
double logarg, logval, N;
logarg = -std::pow(1.0 - epsilon, sampleSize);
+#ifdef VISP_HAVE_FUNC_LOG1P
+ logval = log1p(logarg);
+#else
logval = log(1.0 + logarg);
+#endif
if (vpMath::nul(logval, std::numeric_limits<double>::epsilon())) {
- std::cerr << "vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, sampleSize)), std::numeric_limits<double>::epsilon())"
+ std::cerr << "vpMath::nul(log(1.0 - std::pow(1.0 - epsilon, "
+ "sampleSize)), std::numeric_limits<double>::epsilon())"
<< std::endl;
return 0;
}
- N = log(std::max(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
+ N = log((std::max)(1.0 - probability, std::numeric_limits<double>::epsilon())) / logval;
if (logval < 0.0 && N < maxIterations) {
- return (int) ceil(N);
+ return (int)ceil(N);
}
return maxIterations;
@@ -958,33 +1032,26 @@ int vpPose::computeRansacIterations(double probability, double epsilon, const in
\param p2D : Vector of 2d points (x and y attributes are used).
\param p3D : Vector of 3d points (oX, oY and oZ attributes are used).
- \param numberOfInlierToReachAConsensus : The minimum number of inlier to have
- to consider a trial as correct.
- \param threshold : The maximum error allowed between the 2d points and the
- reprojection of its associated 3d points by the current pose (in meter).
- \param ninliers : Number of inliers found for the best solution.
- \param listInliers : Vector of points (2d and 3d) that are inliers for the best solution.
- \param cMo : The computed pose (best solution).
- \param maxNbTrials : Maximum number of trials before considering a solution
- fitting the required \e numberOfInlierToReachAConsensus and \e threshold
- cannot be found.
+ \param numberOfInlierToReachAConsensus : The minimum number of inlier to
+ have to consider a trial as correct. \param threshold : The maximum error
+ allowed between the 2d points and the reprojection of its associated 3d
+ points by the current pose (in meter). \param ninliers : Number of inliers
+ found for the best solution. \param listInliers : Vector of points (2d and
+ 3d) that are inliers for the best solution. \param cMo : The computed pose
+ (best solution). \param maxNbTrials : Maximum number of trials before
+ considering a solution fitting the required \e
+ numberOfInlierToReachAConsensus and \e threshold cannot be found.
*/
-void vpPose::findMatch(std::vector<vpPoint> &p2D,
- std::vector<vpPoint> &p3D,
- const unsigned int &numberOfInlierToReachAConsensus,
- const double &threshold,
- unsigned int &ninliers,
- std::vector<vpPoint> &listInliers,
- vpHomogeneousMatrix &cMo,
- const int &maxNbTrials )
+void vpPose::findMatch(std::vector<vpPoint> &p2D, std::vector<vpPoint> &p3D,
+ const unsigned int &numberOfInlierToReachAConsensus, const double &threshold,
+ unsigned int &ninliers, std::vector<vpPoint> &listInliers, vpHomogeneousMatrix &cMo,
+ const int &maxNbTrials)
{
vpPose pose;
int nbPts = 0;
- for(unsigned int i = 0 ; i < p2D.size() ; i++)
- {
- for(unsigned int j = 0 ; j < p3D.size() ; j++)
- {
+ for (unsigned int i = 0; i < p2D.size(); i++) {
+ for (unsigned int j = 0; j < p3D.size(); j++) {
vpPoint pt(p3D[j].getWorldCoordinates());
pt.set_x(p2D[i].get_x());
pt.set_y(p2D[i].get_y());
@@ -993,16 +1060,13 @@ void vpPose::findMatch(std::vector<vpPoint> &p2D,
}
}
- 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())) ;
- }
- else
- {
+ 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()));
+ } else {
pose.setRansacMaxTrials(maxNbTrials);
pose.setRansacNbInliersToReachConsensus(numberOfInlierToReachAConsensus);
pose.setRansacThreshold(threshold);
diff --git a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp
index 49c88eb..48d1d58 100644
--- a/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp
+++ b/modules/vision/src/pose-estimation/vpPoseVirtualVisualServoing.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -40,10 +41,10 @@
\brief Compute the pose using virtual visual servoing approach
*/
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpExponentialMap.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRobust.h>
+#include <visp3/vision/vpPose.h>
/*!
\brief Compute the pose using virtual visual servoing approach
@@ -52,102 +53,100 @@
*/
-void
-vpPose::poseVirtualVS(vpHomogeneousMatrix & cMo)
+void vpPose::poseVirtualVS(vpHomogeneousMatrix &cMo)
{
- try
- {
+ try {
- double residu_1 = 1e8 ;
- double r =1e8-1;
+ double residu_1 = 1e8;
+ double r = 1e8 - 1;
// we stop the minimization when the error is bellow 1e-8
- int iter = 0 ;
+ int iter = 0;
+
+ unsigned int nb = (unsigned int)listP.size();
+ vpMatrix L(2 * nb, 6);
+ vpColVector err(2 * nb);
+ vpColVector sd(2 * nb), s(2 * nb);
+ vpColVector v;
- unsigned int nb = (unsigned int) listP.size() ;
- vpMatrix L(2*nb,6) ;
- vpColVector err(2*nb) ;
- vpColVector sd(2*nb),s(2*nb) ;
- vpColVector v ;
-
vpPoint P;
- std::list<vpPoint> lP ;
+ std::list<vpPoint> lP;
// create sd
- unsigned int k =0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ unsigned int k = 0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- sd[2*k] = P.get_x() ;
- sd[2*k+1] = P.get_y() ;
+ sd[2 * k] = P.get_x();
+ sd[2 * k + 1] = P.get_y();
lP.push_back(P);
- k ++;
+ k++;
}
vpHomogeneousMatrix cMoPrev = cMo;
- //while((int)((residu_1 - r)*1e12) !=0)
- while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon())
- {
- residu_1 = r ;
+ // while((int)((residu_1 - r)*1e12) !=0)
+ // while(std::fabs((residu_1 - r)*1e12) >
+ // std::numeric_limits<double>::epsilon())
+ while (std::fabs(residu_1 - r) > vvsEpsilon) {
+ residu_1 = r;
// Compute the interaction matrix and the error
- k =0 ;
- for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
- {
+ k = 0;
+ for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
P = *it;
// forward projection of the 3D model for a given pose
// change frame coordinates
// perspective projection
- P.track(cMo) ;
-
- double x = s[2*k] = P.get_x(); /* point projected from cMo */
- double y = s[2*k+1] = P.get_y();
- double Z = P.get_Z() ;
- L[2*k][0] = -1/Z ;
- L[2*k][1] = 0 ;
- L[2*k][2] = x/Z ;
- L[2*k][3] = x*y ;
- L[2*k][4] = -(1+x*x) ;
- L[2*k][5] = y ;
-
- L[2*k+1][0] = 0 ;
- L[2*k+1][1] = -1/Z ;
- L[2*k+1][2] = y/Z ;
- L[2*k+1][3] = 1+y*y ;
- L[2*k+1][4] = -x*y ;
- L[2*k+1][5] = -x ;
-
- k+=1 ;
+ P.track(cMo);
+
+ double x = s[2 * k] = P.get_x(); /* point projected from cMo */
+ double y = s[2 * k + 1] = P.get_y();
+ double Z = P.get_Z();
+ L[2 * k][0] = -1 / Z;
+ L[2 * k][1] = 0;
+ L[2 * k][2] = x / Z;
+ L[2 * k][3] = x * y;
+ L[2 * k][4] = -(1 + x * x);
+ L[2 * k][5] = y;
+
+ L[2 * k + 1][0] = 0;
+ L[2 * k + 1][1] = -1 / Z;
+ L[2 * k + 1][2] = y / Z;
+ L[2 * k + 1][3] = 1 + y * y;
+ L[2 * k + 1][4] = -x * y;
+ L[2 * k + 1][5] = -x;
+
+ k += 1;
}
- err = s - sd ;
+ err = s - sd;
// compute the residual
- r = err.sumSquare() ;
+ r = err.sumSquare();
// compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- L.pseudoInverse(Lp,1e-16) ;
+ vpMatrix Lp;
+ L.pseudoInverse(Lp, 1e-16);
// compute the VVS control law
- v = -lambda*Lp*err ;
+ v = -lambda * Lp * err;
- //std::cout << "r=" << r <<std::endl ;
+ // std::cout << "r=" << r <<std::endl ;
// update the pose
cMoPrev = cMo;
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax) break ;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ ;
+ if (iter++ > vvsIterMax)
+ break;
}
-
- if(computeCovariance)
+
+ if (computeCovariance)
covarianceMatrix = vpMatrix::computeCovarianceMatrixVVS(cMoPrev, err, L);
}
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
+ catch (...) {
+ vpERROR_TRACE(" ");
+ throw;
}
}
@@ -158,119 +157,112 @@ vpPose::poseVirtualVS(vpHomogeneousMatrix & cMo)
This approach is described in \cite Comport06b.
*/
-void
-vpPose::poseVirtualVSrobust(vpHomogeneousMatrix & cMo)
+void vpPose::poseVirtualVSrobust(vpHomogeneousMatrix &cMo)
{
- try{
+ try {
- double residu_1 = 1e8 ;
- double r =1e8-1;
+ double residu_1 = 1e8;
+ double r = 1e8 - 1;
// we stop the minimization when the error is bellow 1e-8
- vpMatrix W ;
- vpRobust robust((unsigned int)(2*listP.size())) ;
- robust.setThreshold(0.0000) ;
- vpColVector w,res ;
-
- unsigned int nb = (unsigned int) listP.size() ;
- vpMatrix L(2*nb,6) ;
- vpColVector error(2*nb) ;
- vpColVector sd(2*nb),s(2*nb) ;
- vpColVector v ;
-
- listP.front() ;
+ vpMatrix W;
+ vpRobust robust((unsigned int)(2 * listP.size()));
+ robust.setThreshold(0.0000);
+ vpColVector w, res;
+
+ unsigned int nb = (unsigned int)listP.size();
+ vpMatrix L(2 * nb, 6);
+ vpColVector error(2 * nb);
+ vpColVector sd(2 * nb), s(2 * nb);
+ vpColVector v;
+
+ listP.front();
vpPoint P;
- std::list<vpPoint> lP ;
+ std::list<vpPoint> lP;
// create sd
- unsigned int k_ =0 ;
- for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it)
- {
+ unsigned int k_ = 0;
+ for (std::list<vpPoint>::const_iterator it = listP.begin(); it != listP.end(); ++it) {
P = *it;
- sd[2*k_] = P.get_x() ;
- sd[2*k_+1] = P.get_y() ;
- lP.push_back(P) ;
- k_ ++;
+ sd[2 * k_] = P.get_x();
+ sd[2 * k_ + 1] = P.get_y();
+ lP.push_back(P);
+ k_++;
}
- int iter = 0 ;
- res.resize(s.getRows()/2) ;
- w.resize(s.getRows()/2) ;
- W.resize(s.getRows(), s.getRows()) ;
- w =1 ;
+ int iter = 0;
+ res.resize(s.getRows() / 2);
+ w.resize(s.getRows() / 2);
+ W.resize(s.getRows(), s.getRows());
+ w = 1;
- //while((int)((residu_1 - r)*1e12) !=0)
- while(std::fabs((residu_1 - r)*1e12) > std::numeric_limits<double>::epsilon())
- {
- residu_1 = r ;
+ // while((int)((residu_1 - r)*1e12) !=0)
+ while (std::fabs((residu_1 - r) * 1e12) > std::numeric_limits<double>::epsilon()) {
+ residu_1 = r;
// Compute the interaction matrix and the error
- k_ =0 ;
- for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
- {
+ k_ = 0;
+ for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it) {
P = *it;
// forward projection of the 3D model for a given pose
// change frame coordinates
// perspective projection
- P.track(cMo) ;
-
- double x = s[2*k_] = P.get_x(); // point projected from cMo
- double y = s[2*k_+1] = P.get_y();
- double Z = P.get_Z() ;
- L[2*k_][0] = -1/Z ;
- L[2*k_][1] = 0 ;
- L[2*k_][2] = x/Z ;
- L[2*k_][3] = x*y ;
- L[2*k_][4] = -(1+x*x) ;
- L[2*k_][5] = y ;
-
- L[2*k_+1][0] = 0 ;
- L[2*k_+1][1] = -1/Z ;
- L[2*k_+1][2] = y/Z ;
- L[2*k_+1][3] = 1+y*y ;
- L[2*k_+1][4] = -x*y ;
- L[2*k_+1][5] = -x ;
-
- k_ ++;
-
+ P.track(cMo);
+
+ double x = s[2 * k_] = P.get_x(); // point projected from cMo
+ double y = s[2 * k_ + 1] = P.get_y();
+ double Z = P.get_Z();
+ L[2 * k_][0] = -1 / Z;
+ L[2 * k_][1] = 0;
+ L[2 * k_][2] = x / Z;
+ L[2 * k_][3] = x * y;
+ L[2 * k_][4] = -(1 + x * x);
+ L[2 * k_][5] = y;
+
+ L[2 * k_ + 1][0] = 0;
+ L[2 * k_ + 1][1] = -1 / Z;
+ L[2 * k_ + 1][2] = y / Z;
+ L[2 * k_ + 1][3] = 1 + y * y;
+ L[2 * k_ + 1][4] = -x * y;
+ L[2 * k_ + 1][5] = -x;
+
+ k_++;
}
- error = s - sd ;
+ error = s - sd;
// compute the residual
- r = error.sumSquare() ;
+ r = error.sumSquare();
- for(unsigned int k=0 ; k <error.getRows()/2 ; k++)
- {
- res[k] = vpMath::sqr(error[2*k]) + vpMath::sqr(error[2*k+1]) ;
+ for (unsigned int k = 0; k < error.getRows() / 2; k++) {
+ res[k] = vpMath::sqr(error[2 * k]) + vpMath::sqr(error[2 * k + 1]);
}
robust.setIteration(0);
robust.MEstimator(vpRobust::TUKEY, res, w);
// compute the pseudo inverse of the interaction matrix
- for (unsigned int k=0 ; k < error.getRows()/2 ; k++)
- {
- W[2*k][2*k] = w[k] ;
- W[2*k+1][2*k+1] = w[k] ;
+ for (unsigned int k = 0; k < error.getRows() / 2; k++) {
+ W[2 * k][2 * k] = w[k];
+ W[2 * k + 1][2 * k + 1] = w[k];
}
// compute the pseudo inverse of the interaction matrix
- vpMatrix Lp ;
- (W*L).pseudoInverse(Lp,1e-6) ;
+ vpMatrix Lp;
+ (W * L).pseudoInverse(Lp, 1e-6);
// compute the VVS control law
- v = -lambda*Lp*W*error ;
+ v = -lambda * Lp * W * error;
- cMo = vpExponentialMap::direct(v).inverse()*cMo ; ;
- if (iter++>vvsIterMax) break ;
+ cMo = vpExponentialMap::direct(v).inverse() * cMo;
+ ;
+ if (iter++ > vvsIterMax)
+ break;
}
-
- if(computeCovariance)
- covarianceMatrix = vpMatrix::computeCovarianceMatrix(L,v,-lambda*error, W*W); // Remark: W*W = W*W.t() since the matrix is diagonale, but using W*W is more efficient.
- }
- catch(...)
- {
- vpERROR_TRACE(" ") ;
- throw ;
- }
+ if (computeCovariance)
+ covarianceMatrix =
+ vpMatrix::computeCovarianceMatrix(L, v, -lambda * error, W * W); // Remark: W*W = W*W.t() since the
+ // matrix is diagonale, but using W*W
+ // is more efficient.
+ } catch (...) {
+ vpERROR_TRACE(" ");
+ throw;
+ }
}
-
-
diff --git a/modules/vision/test/homography/testDisplacement.cpp b/modules/vision/test/homography/testDisplacement.cpp
index 3797ed6..1d2b885 100644
--- a/modules/vision/test/homography/testDisplacement.cpp
+++ b/modules/vision/test/homography/testDisplacement.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file testDisplacement.cpp
\brief Tests transformation within various representations of rotation
@@ -45,25 +45,25 @@
\example testDisplacement.cpp
*/
+#include <stdio.h>
+#include <stdlib.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/vision/vpHomography.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/core/vpThetaUVector.h>
-#include <stdlib.h>
-#include <stdio.h>
+#include <visp3/vision/vpHomography.h>
bool test(const std::string &s, const vpHomography &H, const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s << "(" << H.getRows() << "," << H.getCols() << ") = \n[" << H << "]" << std::endl;
- if(bench.size() != H.size()) {
+ if (bench.size() != H.size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
- for (unsigned int i=0; i<H.size(); i++) {
- if (std::fabs(H.data[i]-bench[i]) > std::fabs(H.data[i])*std::numeric_limits<double>::epsilon()) {
+ for (unsigned int i = 0; i < H.size(); i++) {
+ if (std::fabs(H.data[i] - bench[i]) > std::fabs(H.data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
@@ -83,111 +83,110 @@ int main()
int err = 1;
if (test("H", H, bench) == false)
return err;
- if (test("H", H/H[2][2], bench) == false)
+ if (test("H", H / H[2][2], bench) == false)
return err;
}
{
- vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
+ vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45));
- std::cout << "Initialization " <<std::endl ;
+ std::cout << "Initialization " << std::endl;
// std::cout << tu << std::endl ;
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- vpRotationMatrix R(tu) ;
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
+ vpRotationMatrix R(tu);
// pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
+ vpHomogeneousMatrix M;
+ M.insert(R);
- std::cout << "M" <<std::endl <<M << std::endl ;
- vpPlane p(0,0,1,1) ;
+ std::cout << "M" << std::endl << M << std::endl;
+ vpPlane p(0, 0, 1, 1);
- vpHomography H(M,p) ;
+ vpHomography H(M, p);
- std::cout << "H" <<std::endl <<H << std::endl ;
+ std::cout << "H" << std::endl << H << std::endl;
- vpColVector n ;
- vpTranslationVector T ;
+ vpColVector n;
+ vpTranslationVector T;
- H.computeDisplacement(R,T,n) ;
+ H.computeDisplacement(R, T, n);
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
+ std::cout << "R" << std::endl << R;
+ std::cout << "T" << std::endl << T.t() << std::endl;
+ std::cout << "n" << std::endl << n.t() << std::endl;
}
- std::cout <<"------------------------------------------------------" << std::endl ;
+ std::cout << "------------------------------------------------------" << std::endl;
{
- vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45)) ;
+ vpThetaUVector tu(vpMath::rad(90), vpMath::rad(120), vpMath::rad(45));
- std::cout << "Initialization " << std::endl ;
+ std::cout << "Initialization " << std::endl;
// std::cout << tu << std::endl ;
- std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl ;
- vpRotationMatrix R(tu) ;
+ std::cout << "From vpThetaUVector to vpRotationMatrix " << std::endl;
+ vpRotationMatrix R(tu);
// pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
+ vpHomogeneousMatrix M;
+ M.insert(R);
- M[0][3] = 0.21 ;
- M[1][3] = 0.31 ;
- M[2][3] = 0.5 ;
+ M[0][3] = 0.21;
+ M[1][3] = 0.31;
+ M[2][3] = 0.5;
- std::cout << "M" << std::endl << M << std::endl ;
- vpPlane p(0,0,1,1) ;
+ std::cout << "M" << std::endl << M << std::endl;
+ vpPlane p(0, 0, 1, 1);
- vpHomography H(M,p) ;
+ vpHomography H(M, p);
- std::cout << "H" << std::endl << H << std::endl ;
+ std::cout << "H" << std::endl << H << std::endl;
- vpColVector n ;
- vpTranslationVector T ;
+ vpColVector n;
+ vpTranslationVector T;
- H.computeDisplacement(R,T,n) ;
+ H.computeDisplacement(R, T, n);
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
+ std::cout << "R" << std::endl << R;
+ std::cout << "T" << std::endl << T.t() << std::endl;
+ std::cout << "n" << std::endl << n.t() << std::endl;
}
- std::cout <<"------------------------------------------------------" << std::endl ;
+ std::cout << "------------------------------------------------------" << std::endl;
{
- vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45)) ;
+ vpThetaUVector tu(vpMath::rad(-190), vpMath::rad(12), vpMath::rad(-45));
- vpRotationMatrix R(tu) ;
+ vpRotationMatrix R(tu);
// pure rotation
- vpHomogeneousMatrix M ;
- M.insert(R) ;
+ vpHomogeneousMatrix M;
+ M.insert(R);
- M[0][3] = 0.21 ;
- M[1][3] = -0.31 ;
- M[2][3] = 0.5 ;
+ M[0][3] = 0.21;
+ M[1][3] = -0.31;
+ M[2][3] = 0.5;
- std::cout << "M" << std::endl << M << std::endl ;
- vpPlane p(0.4,-0.5,0.5,1) ;
+ std::cout << "M" << std::endl << M << std::endl;
+ vpPlane p(0.4, -0.5, 0.5, 1);
- vpHomography H(M,p) ;
+ vpHomography H(M, p);
- std::cout << "H" << std::endl << H << std::endl ;
+ std::cout << "H" << std::endl << H << std::endl;
- vpColVector n ;
- vpTranslationVector T ;
- H.computeDisplacement(R,T,n) ;
+ vpColVector n;
+ vpTranslationVector T;
+ H.computeDisplacement(R, T, n);
- std::cout << "R" <<std::endl << R ;
- std::cout << "T" <<std::endl << T.t() << std::endl;
- std::cout << "n" <<std::endl << n.t() << std::endl;
+ std::cout << "R" << std::endl << R;
+ std::cout << "T" << std::endl << T.t() << std::endl;
+ std::cout << "n" << std::endl << n.t() << std::endl;
- vpPlane p1(n[0],n[1],n[2],1.0) ;
- H.buildFrom(R,T,p1) ;
- std::cout << "H" << std::endl << H << std::endl ;
+ vpPlane p1(n[0], n[1], n[2], 1.0);
+ H.buildFrom(R, T, p1);
+ std::cout << "H" << std::endl << H << std::endl;
}
std::cout << "All tests succeed" << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vision/test/key-point/testKeyPoint-2.cpp b/modules/vision/test/key-point/testKeyPoint-2.cpp
index 2076af2..170a08f 100644
--- a/modules/vision/test/key-point/testKeyPoint-2.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,20 +42,20 @@
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
-#include <visp3/vision/vpKeyPoint.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpVideoReader.h>
-#include <visp3/core/vpIoTools.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
-#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpKeyPoint.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -106,17 +107,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -136,7 +145,8 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
\brief Test keypoint matching and pose estimation.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -144,23 +154,26 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return -1;
}
vpImage<unsigned char> I;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube");
- //Build the name of the image files
+ // Build the name of the image files
std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
vpImageIo::read(I, filenameRef);
std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
@@ -182,139 +195,132 @@ int main(int argc, const char ** argv) {
vpCameraParameters cam;
vpMbEdgeTracker tracker;
- //Load config for tracker
- std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.xml");
+ // Load config for tracker
+ std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- bool usexml = false;
#ifdef VISP_HAVE_XML2
tracker.loadConfigFile(tracker_config_file);
tracker.getCameraParameters(cam);
-
- usexml = true;
+#else
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ me.setNbTotalSample(250);
+ tracker.setMovingEdge(me);
+ cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
+ tracker.setCameraParameters(cam);
+ tracker.setNearClippingDistance(0.01);
+ tracker.setFarClippingDistance(100.0);
+ tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
#endif
- if (! usexml) {
- vpMe me;
- me.setMaskSize(5);
- me.setMaskNumber(180);
- me.setRange(8);
- me.setThreshold(10000);
- me.setMu1(0.5);
- me.setMu2(0.5);
- me.setSampleStep(4);
- me.setNbTotalSample(250);
- tracker.setMovingEdge(me);
- cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
- tracker.setCameraParameters(cam);
- tracker.setNearClippingDistance(0.01);
- tracker.setFarClippingDistance(100.0);
- tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
- }
tracker.setAngleAppear(vpMath::rad(89));
tracker.setAngleDisappear(vpMath::rad(89));
- //Load CAO model
- std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.cao");
+ // Load CAO model
+ 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, "ViSP-images/mbt/cube.init");
+ // Initialize the pose
+ std::string init_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.init");
if (opt_display && opt_click_allowed) {
tracker.initClick(I, init_file);
- }
- else
- {
+ } else {
vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
tracker.initFromPose(I, cMoi);
}
- //Get the init pose
+ // Get the init pose
vpHomogeneousMatrix cMo;
tracker.getPose(cMo);
- //Init keypoints
+ // Init keypoints
vpKeyPoint keypoints("ORB", "ORB", "BruteForce-Hamming");
#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
- //Bug when using LSH index with FLANN and OpenCV 2.3.1.
- //see http://code.opencv.org/issues/1741 (Bug #1741)
+ // Bug when using LSH index with FLANN and OpenCV 2.3.1.
+ // see http://code.opencv.org/issues/1741 (Bug #1741)
keypoints.setMatcher("FlannBased");
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
- keypoints.setDetectorParameter("ORB", "nLevels", 1);
+ keypoints.setDetectorParameter("ORB", "nLevels", 1);
#else
- cv::Ptr<cv::ORB> orb_detector = keypoints.getDetector("ORB").dynamicCast<cv::ORB>();
- if(orb_detector != NULL) {
- orb_detector->setNLevels(1);
- }
+ cv::Ptr<cv::ORB> orb_detector = keypoints.getDetector("ORB").dynamicCast<cv::ORB>();
+ if (orb_detector != NULL) {
+ orb_detector->setNLevels(1);
+ }
#endif
#endif
- //Detect keypoints on the current image
+ // Detect keypoints on the current image
std::vector<cv::KeyPoint> trainKeyPoints;
double elapsedTime;
keypoints.detect(I, trainKeyPoints, elapsedTime);
- //Keep only keypoints on the cube
+ // Keep only keypoints on the cube
std::vector<vpPolygon> polygons;
std::vector<std::vector<vpPoint> > roisPt;
- std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(true); //To detect an issue with CI
+ std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair =
+ tracker.getPolygonFaces(true); // To detect an issue with CI
polygons = pair.first;
roisPt = pair.second;
- //Compute the 3D coordinates
+ // Compute the 3D coordinates
std::vector<cv::Point3f> points3f;
vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
- //Build the reference keypoints
+ // Build the reference keypoints
keypoints.buildReference(I, trainKeyPoints, points3f, false, 1);
-
- //Read image 150
+ // Read image 150
filenameRef = vpIoTools::createFilePath(dirname, "image0150.pgm");
vpImageIo::read(I, filenameRef);
- //Init pose at image 150
+ // Init pose at image 150
cMo.buildFrom(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828);
tracker.initFromPose(I, cMo);
- //Detect keypoints on the image 150
+ // Detect keypoints on the image 150
keypoints.detect(I, trainKeyPoints, elapsedTime);
- //Keep only keypoints on the cube
- pair = tracker.getPolygonFaces(true, true, true); //To detect an issue with CI
+ // Keep only keypoints on the cube
+ pair = tracker.getPolygonFaces(true, true,
+ true); // To detect an issue with CI
polygons = pair.first;
roisPt = pair.second;
- //Compute the 3D coordinates
+ // Compute the 3D coordinates
vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
- //Build the reference keypoints
+ // Build the reference keypoints
keypoints.buildReference(I, trainKeyPoints, points3f, true, 2);
-
- //Read image 200
+ // Read image 200
filenameRef = vpIoTools::createFilePath(dirname, "image0200.pgm");
vpImageIo::read(I, filenameRef);
- //Init pose at image 200
+ // Init pose at image 200
cMo.buildFrom(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025);
tracker.initFromPose(I, cMo);
- //Detect keypoints on the image 200
+ // Detect keypoints on the image 200
keypoints.detect(I, trainKeyPoints, elapsedTime);
- //Keep only keypoints on the cube
- pair = tracker.getPolygonFaces(false); //To detect an issue with CI
+ // Keep only keypoints on the cube
+ pair = tracker.getPolygonFaces(false); // To detect an issue with CI
polygons = pair.first;
roisPt = pair.second;
- //Compute the 3D coordinates
+ // Compute the 3D coordinates
vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
- //Build the reference keypoints
+ // Build the reference keypoints
keypoints.buildReference(I, trainKeyPoints, points3f, true, 3);
-
- //Init reader for getting the input image sequence
+ // Init reader for getting the input image sequence
vpVideoReader g;
g.setFileName(filenameCur);
g.open(I);
@@ -336,37 +342,37 @@ 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) {
+ if (opt_display) {
vpDisplay::display(I);
- //Display image matching
+ // Display image matching
keypoints.insertImageMatching(I, IMatching);
vpDisplay::display(IMatching);
}
- //Match keypoints and estimate the pose
- if(keypoints.matchPoint(I, cam, cMo, error, elapsedTime)) {
+ // Match keypoints and estimate the pose
+ if (keypoints.matchPoint(I, cam, cMo, error, elapsedTime)) {
tracker.setCameraParameters(cam);
tracker.setPose(I, cMo);
- if(opt_display) {
+ if (opt_display) {
tracker.display(I, cMo, cam, vpColor::red, 2);
vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
std::vector<vpImagePoint> ransacInliers = keypoints.getRansacInliers();
std::vector<vpImagePoint> ransacOutliers = keypoints.getRansacOutliers();
- for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
vpDisplay::displayCircle(I, *it, 4, vpColor::green);
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
@@ -374,7 +380,8 @@ int main(int argc, const char ** argv) {
vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::green);
}
- for(std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end();
+ ++it) {
vpDisplay::displayCircle(I, *it, 4, vpColor::red);
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
@@ -384,10 +391,10 @@ int main(int argc, const char ** argv) {
keypoints.displayMatching(I, IMatching);
- //Display model in the correct sub-image in IMatching
+ // Display model in the correct sub-image in IMatching
vpCameraParameters cam2;
- cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(),
- cam.get_u0() + I.getWidth(), cam.get_v0() + I.getHeight());
+ 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);
@@ -395,25 +402,24 @@ int main(int argc, const char ** argv) {
}
}
- if(opt_display) {
+ if (opt_display) {
vpDisplay::flush(I);
vpDisplay::flush(IMatching);
}
if (opt_click_allowed && opt_display) {
- //Click requested to process next image
- if(opt_click) {
+ // Click requested to process next image
+ if (opt_click) {
vpDisplay::getClick(I, button, true);
- if(button == vpMouseButton::button3) {
+ if (button == vpMouseButton::button3) {
opt_click = false;
}
} else {
- //Use right click to enable/disable step by step tracking
- if(vpDisplay::getClick(I, button, false)) {
+ // Use right click to enable/disable step by step tracking
+ if (vpDisplay::getClick(I, button, false)) {
if (button == vpMouseButton::button3) {
opt_click = true;
- }
- else if(button == vpMouseButton::button1) {
+ } else if (button == vpMouseButton::button1) {
break;
}
}
@@ -421,7 +427,7 @@ int main(int argc, const char ** argv) {
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return -1;
}
@@ -430,7 +436,8 @@ int main(int argc, const char ** argv) {
return 0;
}
#else
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return 0;
diff --git a/modules/vision/test/key-point/testKeyPoint-3.cpp b/modules/vision/test/key-point/testKeyPoint-3.cpp
index ac1c87c..05401ff 100644
--- a/modules/vision/test/key-point/testKeyPoint-3.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-3.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,17 +46,17 @@
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/io/vpVideoReader.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -106,17 +107,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -137,7 +146,8 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
\brief Test keypoint matching with mostly OpenCV functions calls
to detect potential memory leaks in testKeyPoint.cpp.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -145,28 +155,31 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return -1;
}
vpImage<unsigned char> Iref, Icur, Imatch;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube");
- //Build the name of the image files
+ // Build the name of the image files
std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
vpImageIo::read(Iref, filenameRef);
std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
- //Init keypoints
+ // Init keypoints
cv::Ptr<cv::FeatureDetector> detector;
cv::Ptr<cv::DescriptorExtractor> extractor;
cv::Ptr<cv::DescriptorMatcher> matcher;
@@ -191,8 +204,8 @@ int main(int argc, const char ** argv) {
g.open(Icur);
g.acquire(Icur);
- Imatch.resize(Icur.getHeight(), 2*Icur.getWidth());
- Imatch.insert(Iref, vpImagePoint(0,0));
+ Imatch.resize(Icur.getHeight(), 2 * Icur.getWidth());
+ Imatch.insert(Iref, vpImagePoint(0, 0));
#if defined VISP_HAVE_X11
vpDisplayX display;
@@ -211,11 +224,11 @@ int main(int argc, const char ** argv) {
bool opt_click = false;
vpMouseButton::vpMouseButtonType button;
- while(!g.end()) {
+ while (!g.end()) {
g.acquire(Icur);
Imatch.insert(Icur, vpImagePoint(0, Icur.getWidth()));
- if(opt_display) {
+ if (opt_display) {
vpDisplay::display(Imatch);
}
@@ -229,40 +242,40 @@ int main(int argc, const char ** argv) {
std::vector<std::vector<cv::DMatch> > knn_matches;
std::vector<cv::DMatch> matches;
matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
- for(std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end(); ++it) {
- if(it->size() > 1) {
+ for (std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end();
+ ++it) {
+ if (it->size() > 1) {
double ratio = (*it)[0].distance / (*it)[1].distance;
- if(ratio < 0.85) {
+ if (ratio < 0.85) {
matches.push_back((*it)[0]);
}
}
}
- if(opt_display) {
- for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
- vpImagePoint leftPt(trainKeyPoints[(size_t) it->trainIdx].pt.y, trainKeyPoints[(size_t) it->trainIdx].pt.x);
- vpImagePoint rightPt(queryKeyPoints[(size_t) it->queryIdx].pt.y, queryKeyPoints[(size_t) it->queryIdx].pt.x
- + Iref.getWidth());
+ if (opt_display) {
+ for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpImagePoint leftPt(trainKeyPoints[(size_t)it->trainIdx].pt.y, trainKeyPoints[(size_t)it->trainIdx].pt.x);
+ vpImagePoint rightPt(queryKeyPoints[(size_t)it->queryIdx].pt.y,
+ queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth());
vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
}
vpDisplay::flush(Imatch);
}
- //Click requested to process next image
+ // Click requested to process next image
if (opt_click_allowed && opt_display) {
- if(opt_click) {
+ if (opt_click) {
vpDisplay::getClick(Imatch, button, true);
- if(button == vpMouseButton::button3) {
+ if (button == vpMouseButton::button3) {
opt_click = false;
}
} else {
- //Use right click to enable/disable step by step tracking
- if(vpDisplay::getClick(Imatch, button, false)) {
+ // Use right click to enable/disable step by step tracking
+ if (vpDisplay::getClick(Imatch, button, false)) {
if (button == vpMouseButton::button3) {
opt_click = true;
- }
- else if(button == vpMouseButton::button1) {
+ } else if (button == vpMouseButton::button1) {
break;
}
}
@@ -270,7 +283,7 @@ int main(int argc, const char ** argv) {
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return -1;
}
@@ -279,7 +292,8 @@ int main(int argc, const char ** argv) {
return 0;
}
#else
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return 0;
diff --git a/modules/vision/test/key-point/testKeyPoint-4.cpp b/modules/vision/test/key-point/testKeyPoint-4.cpp
index b2b06a9..ab9135e 100644
--- a/modules/vision/test/key-point/testKeyPoint-4.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-4.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,8 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Test keypoint matching and pose estimation with mostly OpenCV functions calls
- * to detect potential memory leaks in testKeyPoint-2.cpp.
+ * Test keypoint matching and pose estimation with mostly OpenCV functions
+ *calls to detect potential memory leaks in testKeyPoint-2.cpp.
*
* Authors:
* Souriya Trinh
@@ -44,21 +45,21 @@
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/io/vpVideoReader.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbEdgeTracker.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/vision/vpKeyPoint.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -110,17 +111,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -138,10 +147,11 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
/*!
\example testKeyPoint-4.cpp
- \brief Test keypoint matching and pose estimation with mostly OpenCV functions calls
- to detect potential memory leaks in testKeyPoint-2.cpp.
+ \brief Test keypoint matching and pose estimation with mostly OpenCV
+ functions calls to detect potential memory leaks in testKeyPoint-2.cpp.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -149,23 +159,26 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return -1;
}
vpImage<unsigned char> I, Imatch, Iref;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube");
- //Build the name of the image files
+ // Build the name of the image files
std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
vpImageIo::read(I, filenameRef);
Iref = I;
@@ -184,65 +197,59 @@ int main(int argc, const char ** argv) {
if (opt_display) {
display.setDownScalingFactor(vpDisplay::SCALE_AUTO);
display.init(I, 0, 0, "ORB keypoints matching");
- Imatch.resize(I.getHeight(), 2*I.getWidth());
+ 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, "ViSP-images/mbt/cube.xml");
+ // Load config for tracker
+ std::string tracker_config_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.xml");
- bool usexml = false;
#ifdef VISP_HAVE_XML2
tracker.loadConfigFile(tracker_config_file);
tracker.getCameraParameters(cam);
-
- usexml = true;
+#else
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ me.setNbTotalSample(250);
+ tracker.setMovingEdge(me);
+ cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
+ tracker.setCameraParameters(cam);
+ tracker.setNearClippingDistance(0.01);
+ tracker.setFarClippingDistance(100.0);
+ tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
#endif
- if (! usexml) {
- vpMe me;
- me.setMaskSize(5);
- me.setMaskNumber(180);
- me.setRange(8);
- me.setThreshold(10000);
- me.setMu1(0.5);
- me.setMu2(0.5);
- me.setSampleStep(4);
- me.setNbTotalSample(250);
- tracker.setMovingEdge(me);
- cam.initPersProjWithoutDistortion(547.7367575, 542.0744058, 338.7036994, 234.5083345);
- tracker.setCameraParameters(cam);
- tracker.setNearClippingDistance(0.01);
- tracker.setFarClippingDistance(100.0);
- tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
- }
tracker.setAngleAppear(vpMath::rad(89));
tracker.setAngleDisappear(vpMath::rad(89));
- //Load CAO model
- std::string cao_model_file = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube.cao");
+ // Load CAO model
+ 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, "ViSP-images/mbt/cube.init");
+ // Initialize the pose
+ std::string init_file = vpIoTools::createFilePath(env_ipath, "mbt/cube.init");
if (opt_display && opt_click_allowed) {
tracker.initClick(I, init_file);
- }
- else
- {
+ } else {
vpHomogeneousMatrix cMoi(0.02044769891, 0.1101505452, 0.5078963719, 2.063603907, 1.110231561, -0.4392789872);
tracker.initFromPose(I, cMoi);
}
- //Get the init pose
+ // Get the init pose
vpHomogeneousMatrix cMo;
tracker.getPose(cMo);
- //Init keypoints
+ // Init keypoints
cv::Ptr<cv::FeatureDetector> detector;
cv::Ptr<cv::DescriptorExtractor> extractor;
cv::Ptr<cv::DescriptorMatcher> matcher;
@@ -260,36 +267,33 @@ int main(int argc, const char ** argv) {
detector->set("nLevels", 1);
#endif
- //Detect keypoints on the current image
+ // Detect keypoints on the current image
std::vector<cv::KeyPoint> trainKeyPoints;
cv::Mat matImg;
vpImageConvert::convert(I, matImg);
detector->detect(matImg, trainKeyPoints);
-
- //Keep only keypoints on the cube
+ // Keep only keypoints on the cube
std::vector<vpPolygon> polygons;
std::vector<std::vector<vpPoint> > roisPt;
std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces(false);
polygons = pair.first;
roisPt = pair.second;
- //Compute the 3D coordinates
+ // Compute the 3D coordinates
std::vector<cv::Point3f> points3f;
vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
-
- //Extract descriptors
+ // 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;
}
-
- //Init reader for getting the input image sequence
+ // Init reader for getting the input image sequence
vpVideoReader g;
g.setFileName(filenameCur);
g.open(I);
@@ -297,7 +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);
@@ -310,23 +314,24 @@ int main(int argc, const char ** argv) {
std::vector<std::vector<cv::DMatch> > knn_matches;
std::vector<cv::DMatch> matches;
matcher->knnMatch(queryDescriptors, trainDescriptors, knn_matches, 2);
- for(std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end(); ++it) {
- if(it->size() > 1) {
+ for (std::vector<std::vector<cv::DMatch> >::const_iterator it = knn_matches.begin(); it != knn_matches.end();
+ ++it) {
+ if (it->size() > 1) {
double ratio = (*it)[0].distance / (*it)[1].distance;
- if(ratio < 0.85) {
+ if (ratio < 0.85) {
matches.push_back((*it)[0]);
}
}
}
vpPose estimated_pose;
- for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
- vpPoint pt(points3f[(size_t)(it->trainIdx)].x,
- points3f[(size_t)(it->trainIdx)].y,
- points3f[(size_t)(it->trainIdx)].z);
+ for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpPoint pt(points3f[(size_t)(it->trainIdx)].x, points3f[(size_t)(it->trainIdx)].y,
+ points3f[(size_t)(it->trainIdx)].z);
double x = 0.0, y = 0.0;
- vpPixelMeterConversion::convertPoint(cam, queryKeyPoints[(size_t)(it->queryIdx)].pt.x, queryKeyPoints[(size_t)(it->queryIdx)].pt.y, x, y);
+ 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);
@@ -334,32 +339,32 @@ int main(int argc, const char ** argv) {
}
bool is_pose_estimated = false;
- if(estimated_pose.npt >= 4) {
+ if (estimated_pose.npt >= 4) {
try {
- unsigned int nb_inliers = (unsigned int) (0.6 * estimated_pose.npt);
+ unsigned int nb_inliers = (unsigned int)(0.6 * estimated_pose.npt);
estimated_pose.setRansacNbInliersToReachConsensus(nb_inliers);
estimated_pose.setRansacThreshold(0.01);
estimated_pose.setRansacMaxTrials(500);
estimated_pose.computePose(vpPose::RANSAC, cMo);
is_pose_estimated = true;
- } catch(...) {
+ } catch (...) {
is_pose_estimated = false;
}
}
- if(opt_display) {
+ if (opt_display) {
vpDisplay::display(I);
Imatch.insert(I, vpImagePoint(0, Iref.getWidth()));
vpDisplay::display(Imatch);
- for(std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
- vpImagePoint leftPt(trainKeyPoints[(size_t) it->trainIdx].pt.y, trainKeyPoints[(size_t) it->trainIdx].pt.x);
- vpImagePoint rightPt(queryKeyPoints[(size_t) it->queryIdx].pt.y, queryKeyPoints[(size_t) it->queryIdx].pt.x
- + Iref.getWidth());
+ for (std::vector<cv::DMatch>::const_iterator it = matches.begin(); it != matches.end(); ++it) {
+ vpImagePoint leftPt(trainKeyPoints[(size_t)it->trainIdx].pt.y, trainKeyPoints[(size_t)it->trainIdx].pt.x);
+ vpImagePoint rightPt(queryKeyPoints[(size_t)it->queryIdx].pt.y,
+ queryKeyPoints[(size_t)it->queryIdx].pt.x + Iref.getWidth());
vpDisplay::displayLine(Imatch, leftPt, rightPt, vpColor::green);
}
- if(is_pose_estimated) {
+ if (is_pose_estimated) {
tracker.setPose(I, cMo);
tracker.display(I, cMo, cam, vpColor::red);
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
@@ -369,20 +374,19 @@ int main(int argc, const char ** argv) {
vpDisplay::flush(I);
}
- //Click requested to process next image
+ // Click requested to process next image
if (opt_click_allowed && opt_display) {
- if(opt_click) {
+ if (opt_click) {
vpDisplay::getClick(I, button, true);
- if(button == vpMouseButton::button3) {
+ if (button == vpMouseButton::button3) {
opt_click = false;
}
} else {
- //Use right click to enable/disable step by step tracking
- if(vpDisplay::getClick(I, button, false)) {
+ // Use right click to enable/disable step by step tracking
+ if (vpDisplay::getClick(I, button, false)) {
if (button == vpMouseButton::button3) {
opt_click = true;
- }
- else if(button == vpMouseButton::button1) {
+ } else if (button == vpMouseButton::button1) {
break;
}
}
@@ -390,7 +394,7 @@ int main(int argc, const char ** argv) {
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return -1;
}
@@ -399,7 +403,8 @@ int main(int argc, const char ** argv) {
return 0;
}
#else
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return 0;
diff --git a/modules/vision/test/key-point/testKeyPoint-5.cpp b/modules/vision/test/key-point/testKeyPoint-5.cpp
index d903f75..3c2b955 100644
--- a/modules/vision/test/key-point/testKeyPoint-5.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-5.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,17 +44,17 @@
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/vision/vpKeyPoint.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -105,17 +106,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -133,10 +142,11 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
/*!
\example testKeyPoint-5.cpp
- \brief Test keypoints detection with OpenCV, specially the Pyramid implementation
- feature missing in OpenCV 3.0.
+ \brief Test keypoints detection with OpenCV, specially the Pyramid
+ implementation feature missing in OpenCV 3.0.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -144,23 +154,26 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return EXIT_FAILURE;
}
vpImage<unsigned char> I;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "Klimt");
- //Build the name of the image files
+ // Build the name of the image files
std::string filename = vpIoTools::createFilePath(dirname, "/Klimt.png");
vpImageIo::read(I, filename);
@@ -174,19 +187,22 @@ int main(int argc, const char ** argv) {
vpDisplayOpenCV display;
#endif
- if(opt_display) {
+ if (opt_display) {
display.init(I, 0, 0, "KeyPoints detection.");
}
- //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.
+ // 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 a problem
- //between OpenCV versions, modules or constructors
+ // Will test the different types of keypoints detection to see if there is
+ // a problem between OpenCV versions, modules or constructors
std::vector<std::string> detectorNames;
detectorNames.push_back("PyramidFAST");
detectorNames.push_back("FAST");
@@ -196,7 +212,7 @@ int main(int argc, const char ** argv) {
detectorNames.push_back("GFTT");
detectorNames.push_back("PyramidSimpleBlob");
detectorNames.push_back("SimpleBlob");
- //In contrib modules
+// In contrib modules
#if (VISP_HAVE_OPENCV_VERSION < 0x030000) || defined(VISP_HAVE_OPENCV_XFEATURES2D)
detectorNames.push_back("PyramidSTAR");
detectorNames.push_back("STAR");
@@ -225,14 +241,14 @@ int main(int argc, const char ** argv) {
detectorNames.push_back("SURF");
#endif
- for(std::vector<std::string>::const_iterator itd = detectorNames.begin(); itd != detectorNames.end(); ++itd) {
+ for (std::vector<std::string>::const_iterator itd = detectorNames.begin(); itd != detectorNames.end(); ++itd) {
keyPoints.setDetector(*itd);
std::vector<cv::KeyPoint> kpts;
keyPoints.detect(I, kpts);
std::cout << "Nb keypoints detected: " << kpts.size() << " for " << *itd << " method." << std::endl;
- if(kpts.empty()) {
+ if (kpts.empty()) {
std::cerr << "No keypoints detected with " << *itd << " and image: " << filename << "." << std::endl;
return EXIT_FAILURE;
}
@@ -240,7 +256,7 @@ int main(int argc, const char ** argv) {
if (opt_display) {
vpDisplay::display(I);
- for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
vpImagePoint imPt;
imPt.set_uv(it->pt.x, it->pt.y);
@@ -259,21 +275,23 @@ int main(int argc, const char ** argv) {
std::map<vpKeyPoint::vpFeatureDetectorType, std::string> mapOfDetectorNames = keyPoints.getDetectorNames();
for (int i = 0; i < vpKeyPoint::DETECTOR_TYPE_SIZE; i++) {
- keyPoints.setDetector( (vpKeyPoint::vpFeatureDetectorType) i );
+ keyPoints.setDetector((vpKeyPoint::vpFeatureDetectorType)i);
std::vector<cv::KeyPoint> kpts;
keyPoints.detect(I, kpts);
- std::cout << "Nb keypoints detected: " << kpts.size() << " for " << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType) i] << " method." << std::endl;
+ std::cout << "Nb keypoints detected: " << kpts.size() << " for "
+ << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i] << " method." << std::endl;
if (kpts.empty()) {
- std::cerr << "No keypoints detected with " << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType) i] << " method and image: " << filename << "." << std::endl;
+ std::cerr << "No keypoints detected with " << mapOfDetectorNames[(vpKeyPoint::vpFeatureDetectorType)i]
+ << " method and image: " << filename << "." << std::endl;
return EXIT_FAILURE;
}
if (opt_display) {
vpDisplay::display(I);
- for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
vpImagePoint imPt;
imPt.set_uv(it->pt.x, it->pt.y);
@@ -288,7 +306,7 @@ int main(int argc, const char ** argv) {
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
@@ -299,7 +317,8 @@ int main(int argc, const char ** argv) {
#else
#include <cstdlib>
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return EXIT_SUCCESS;
diff --git a/modules/vision/test/key-point/testKeyPoint-6.cpp b/modules/vision/test/key-point/testKeyPoint-6.cpp
index b75757b..fac8dfa 100644
--- a/modules/vision/test/key-point/testKeyPoint-6.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-6.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,12 +43,12 @@
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/vision/vpKeyPoint.h>
@@ -102,17 +103,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,10 +144,11 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
\return The string OpenCV type.
*/
-std::string getOpenCVType(const int type) {
+std::string getOpenCVType(const int type)
+{
std::string type_string = "";
- switch(type) {
+ switch (type) {
case CV_8U:
type_string = "CV_8U";
break;
@@ -180,7 +190,8 @@ std::string getOpenCVType(const int type) {
\brief Test descriptor extraction.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -188,23 +199,26 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (EXIT_FAILURE);
+ exit(EXIT_FAILURE);
}
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return EXIT_FAILURE;
}
vpImage<unsigned char> I;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "Klimt");
- //Build the name of the image files
+ // Build the name of the image files
std::string filename = vpIoTools::createFilePath(dirname, "/Klimt.png");
vpImageIo::read(I, filename);
@@ -263,7 +277,7 @@ int main(int argc, const char ** argv) {
return EXIT_FAILURE;
}
- for(std::vector<std::string>::const_iterator itd = descriptorNames.begin(); itd != descriptorNames.end(); ++itd) {
+ for (std::vector<std::string>::const_iterator itd = descriptorNames.begin(); itd != descriptorNames.end(); ++itd) {
keyPoints.setExtractor(*itd);
if (*itd == "KAZE") {
@@ -287,7 +301,7 @@ int main(int argc, const char ** argv) {
} else if (*itd == "BoostDesc") {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
cv::Ptr<cv::Feature2D> boostDesc = keyPoints.getExtractor("BoostDesc");
- //Init BIN BOOST descriptor for FAST keypoints
+ // Init BIN BOOST descriptor for FAST keypoints
boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f);
#endif
}
@@ -297,9 +311,10 @@ 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 " << *itd << " method in " << t << " ms." << std::endl;
- if(descriptor.empty()) {
+ std::cout << "Descriptor: " << descriptor.rows << "x" << descriptor.cols
+ << " (rows x cols) ; type=" << getOpenCVType(descriptor.type()) << " for " << *itd << " method in " << t
+ << " ms." << std::endl;
+ if (descriptor.empty()) {
std::cerr << "No descriptor extracted with " << *itd << " and image:" << filename << "." << std::endl;
return EXIT_FAILURE;
}
@@ -307,7 +322,7 @@ int main(int argc, const char ** argv) {
if (opt_display) {
vpDisplay::display(I);
- for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
vpImagePoint imPt;
imPt.set_uv(it->pt.x, it->pt.y);
@@ -316,7 +331,7 @@ int main(int argc, const char ** argv) {
vpDisplay::flush(I);
- if(opt_click_allowed) {
+ if (opt_click_allowed) {
vpDisplay::getClick(I);
}
}
@@ -327,9 +342,9 @@ int main(int argc, const char ** argv) {
std::map<vpKeyPoint::vpFeatureDescriptorType, std::string> mapOfDescriptorNames = keyPoints.getExtractorNames();
for (int i = 0; i < vpKeyPoint::DESCRIPTOR_TYPE_SIZE; i++) {
- keyPoints.setExtractor( (vpKeyPoint::vpFeatureDescriptorType) 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);
@@ -338,7 +353,7 @@ int main(int argc, const char ** argv) {
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);
@@ -347,7 +362,7 @@ int main(int argc, const char ** argv) {
std::cerr << "No keypoints detected with " << detectorName << " and image:" << filename << "." << std::endl;
return EXIT_FAILURE;
}
- } else if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType) i] == "BoostDesc") {
+ } else if (mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType)i] == "BoostDesc") {
#if (VISP_HAVE_OPENCV_VERSION >= 0x030200) && defined(VISP_HAVE_OPENCV_XFEATURES2D)
detectorName = "FAST";
keyPoints.setDetector(detectorName);
@@ -359,7 +374,7 @@ int main(int argc, const char ** argv) {
}
cv::Ptr<cv::Feature2D> boostDesc = keyPoints.getExtractor("BoostDesc");
- //Init BIN BOOST descriptor for FAST keypoints
+ // Init BIN BOOST descriptor for FAST keypoints
boostDesc = cv::xfeatures2d::BoostDesc::create(cv::xfeatures2d::BoostDesc::BINBOOST_256, true, 5.0f);
#endif
}
@@ -369,18 +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;
- if(descriptor.empty()) {
- std::cerr << "No descriptor extracted with " << mapOfDescriptorNames[(vpKeyPoint::vpFeatureDescriptorType) i] << " and image:" << filename << "." << 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;
return EXIT_FAILURE;
}
if (opt_display) {
vpDisplay::display(I);
- for(std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
+ for (std::vector<cv::KeyPoint>::const_iterator it = kpts.begin(); it != kpts.end(); ++it) {
vpImagePoint imPt;
imPt.set_uv(it->pt.x, it->pt.y);
@@ -389,13 +406,13 @@ int main(int argc, const char ** argv) {
vpDisplay::flush(I);
- if(opt_click_allowed) {
+ if (opt_click_allowed) {
vpDisplay::getClick(I);
}
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return EXIT_FAILURE;
}
@@ -406,7 +423,8 @@ int main(int argc, const char ** argv) {
#else
#include <cstdlib>
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return EXIT_SUCCESS;
diff --git a/modules/vision/test/key-point/testKeyPoint-7.cpp b/modules/vision/test/key-point/testKeyPoint-7.cpp
index 42e0b8f..ff01e01 100644
--- a/modules/vision/test/key-point/testKeyPoint-7.cpp
+++ b/modules/vision/test/key-point/testKeyPoint-7.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,25 +36,22 @@
*
*****************************************************************************/
-#include <iostream>
#include <iomanip>
+#include <iostream>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
+#include <visp3/core/vpException.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/vision/vpKeyPoint.h>
-#include <visp3/core/vpException.h>
// List of allowed command line options
-#define GETOPTARGS "cdo:h"
-
-void usage(const char *name, const char *badparam, std::string opath, std::string user);
-bool getOptions(int argc, const char **argv, std::string &opath, std::string user);
+#define GETOPTARGS "cdo:h"
/*!
@@ -63,7 +61,7 @@ bool getOptions(int argc, const char **argv, std::string &opath, std::string use
\param badparam : Bad parameter name.
*/
-void usage(const char *name, const char *badparam, std::string opath, 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\
@@ -81,8 +79,7 @@ OPTIONS: \n\
learning files will be written.\n\
\n\
-h\n\
- Print the help.\n",
- opath.c_str(), user.c_str());
+ Print the help.\n", opath.c_str(), user.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
@@ -99,21 +96,31 @@ OPTIONS: \n\
\return false if the program has to be stopped, true otherwise.
*/
-bool getOptions(int argc, const char **argv, std::string &opath, std::string user)
+bool getOptions(int argc, const char **argv, std::string &opath, const std::string &user)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': break; //not used, to avoid error with default arguments ctest
- case 'd': break; //not used, to avoid error with default arguments ctest
- case 'o': opath = optarg_; break;
- case 'h': usage(argv[0], NULL, opath, user); return false; break;
+ case 'c':
+ break; // not used, to avoid error with default arguments ctest
+ case 'd':
+ break; // not used, to avoid error with default arguments ctest
+ case 'o':
+ opath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, opath, user);
+ return false;
+ break;
default:
- usage(argv[0], optarg_, opath, user); return false; break;
- return false; break;
+ usage(argv[0], optarg_, opath, user);
+ return false;
+ break;
+ return false;
+ break;
}
}
@@ -136,51 +143,52 @@ bool getOptions(int argc, const char **argv, std::string &opath, std::string use
\return True if the two vectors are identical, false otherwise.
*/
-bool compareKeyPoints(const std::vector<cv::KeyPoint> &keypoints1, const std::vector<cv::KeyPoint> &keypoints2) {
- if(keypoints1.size() != keypoints2.size()) {
+bool compareKeyPoints(const std::vector<cv::KeyPoint> &keypoints1, const std::vector<cv::KeyPoint> &keypoints2)
+{
+ if (keypoints1.size() != keypoints2.size()) {
return false;
}
- for(size_t cpt = 0; cpt < keypoints1.size(); cpt++) {
- if(!vpMath::equal(keypoints1[cpt].angle, keypoints2[cpt].angle, std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].angle=" << keypoints1[cpt].angle <<
- " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle << std::endl;
+ for (size_t cpt = 0; cpt < keypoints1.size(); cpt++) {
+ if (!vpMath::equal(keypoints1[cpt].angle, keypoints2[cpt].angle, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].angle=" << keypoints1[cpt].angle
+ << " ; keypoints2[cpt].angle=" << keypoints2[cpt].angle << std::endl;
return false;
}
- if(keypoints1[cpt].class_id != keypoints2[cpt].class_id) {
- std::cerr << "keypoints1[cpt].class_id=" << keypoints1[cpt].class_id << " ; keypoints2[cpt].class_id=" <<
- keypoints2[cpt].class_id << std::endl;
+ if (keypoints1[cpt].class_id != keypoints2[cpt].class_id) {
+ std::cerr << "keypoints1[cpt].class_id=" << keypoints1[cpt].class_id
+ << " ; keypoints2[cpt].class_id=" << keypoints2[cpt].class_id << std::endl;
return false;
}
- if(keypoints1[cpt].octave != keypoints2[cpt].octave) {
- std::cerr << "keypoints1[cpt].octave=" << keypoints1[cpt].octave << " ; keypoints2[cpt].octave=" <<
- keypoints2[cpt].octave << std::endl;
+ if (keypoints1[cpt].octave != keypoints2[cpt].octave) {
+ std::cerr << "keypoints1[cpt].octave=" << keypoints1[cpt].octave
+ << " ; keypoints2[cpt].octave=" << keypoints2[cpt].octave << std::endl;
return false;
}
- if(!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.x=" << keypoints1[cpt].pt.x <<
- " ; keypoints2[cpt].pt.x=" << keypoints2[cpt].pt.x << std::endl;
+ if (!vpMath::equal(keypoints1[cpt].pt.x, keypoints2[cpt].pt.x, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.x=" << keypoints1[cpt].pt.x
+ << " ; keypoints2[cpt].pt.x=" << keypoints2[cpt].pt.x << std::endl;
return false;
}
- if(!vpMath::equal(keypoints1[cpt].pt.y, keypoints2[cpt].pt.y, std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.y=" << keypoints1[cpt].pt.y <<
- " ; keypoints2[cpt].pt.y=" << keypoints2[cpt].pt.y << std::endl;
+ if (!vpMath::equal(keypoints1[cpt].pt.y, keypoints2[cpt].pt.y, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].pt.y=" << keypoints1[cpt].pt.y
+ << " ; keypoints2[cpt].pt.y=" << keypoints2[cpt].pt.y << std::endl;
return false;
}
- if(!vpMath::equal(keypoints1[cpt].response, keypoints2[cpt].response, std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].response=" << keypoints1[cpt].response <<
- " ; keypoints2[cpt].response=" << keypoints2[cpt].response << std::endl;
+ if (!vpMath::equal(keypoints1[cpt].response, keypoints2[cpt].response, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].response=" << keypoints1[cpt].response
+ << " ; keypoints2[cpt].response=" << keypoints2[cpt].response << std::endl;
return false;
}
- if(!vpMath::equal(keypoints1[cpt].size, keypoints2[cpt].size, std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].size=" << keypoints1[cpt].size <<
- " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl;
+ if (!vpMath::equal(keypoints1[cpt].size, keypoints2[cpt].size, std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9) << "keypoints1[cpt].size=" << keypoints1[cpt].size
+ << " ; keypoints2[cpt].size=" << keypoints2[cpt].size << std::endl;
return false;
}
}
@@ -196,67 +204,72 @@ bool compareKeyPoints(const std::vector<cv::KeyPoint> &keypoints1, const std::ve
\return True if the two vectors are identical, false otherwise.
*/
-bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2) {
- if(descriptors1.rows != descriptors2.rows || descriptors1.cols != descriptors2.cols ||
+bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2)
+{
+ if (descriptors1.rows != descriptors2.rows || descriptors1.cols != descriptors2.cols ||
descriptors1.type() != descriptors2.type()) {
return false;
}
- for(int i = 0; i < descriptors1.rows; i++) {
- for(int j = 0; j < descriptors1.cols; j++) {
- switch(descriptors1.type()) {
+ for (int i = 0; i < descriptors1.rows; i++) {
+ for (int j = 0; j < descriptors1.cols; j++) {
+ switch (descriptors1.type()) {
case CV_8U:
- if(descriptors1.at<unsigned char>(i,j) != descriptors2.at<unsigned char>(i,j)) {
- std::cerr << "descriptors1.at<unsigned char>(i,j)=" << descriptors1.at<unsigned char>(i,j) <<
- " ; descriptors2.at<unsigned char>(i,j)=" << descriptors2.at<unsigned char>(i,j) << std::endl;
+ if (descriptors1.at<unsigned char>(i, j) != descriptors2.at<unsigned char>(i, j)) {
+ std::cerr << "descriptors1.at<unsigned char>(i,j)=" << descriptors1.at<unsigned char>(i, j)
+ << " ; descriptors2.at<unsigned char>(i,j)=" << descriptors2.at<unsigned char>(i, j) << std::endl;
return false;
}
break;
case CV_8S:
- if(descriptors1.at<char>(i,j) != descriptors2.at<char>(i,j)) {
- std::cerr << "descriptors1.at<char>(i,j)=" << descriptors1.at<char>(i,j) <<
- " ; descriptors2.at<char>(i,j)=" << descriptors2.at<char>(i,j) << std::endl;
+ if (descriptors1.at<char>(i, j) != descriptors2.at<char>(i, j)) {
+ std::cerr << "descriptors1.at<char>(i,j)=" << descriptors1.at<char>(i, j)
+ << " ; descriptors2.at<char>(i,j)=" << descriptors2.at<char>(i, j) << std::endl;
return false;
}
break;
case CV_16U:
- if(descriptors1.at<unsigned short>(i,j) != descriptors2.at<unsigned short>(i,j)) {
- std::cerr << "descriptors1.at<unsigned short>(i,j)=" << descriptors1.at<unsigned short>(i,j) <<
- " ; descriptors2.at<unsigned short>(i,j)=" << descriptors2.at<unsigned short>(i,j) << std::endl;
+ if (descriptors1.at<unsigned short>(i, j) != descriptors2.at<unsigned short>(i, j)) {
+ std::cerr << "descriptors1.at<unsigned short>(i,j)=" << descriptors1.at<unsigned short>(i, j)
+ << " ; descriptors2.at<unsigned short>(i,j)=" << descriptors2.at<unsigned short>(i, j) << std::endl;
return false;
}
break;
case CV_16S:
- if(descriptors1.at<short>(i,j) != descriptors2.at<short>(i,j)) {
- std::cerr << "descriptors1.at<short>(i,j)=" << descriptors1.at<short>(i,j) <<
- " ; descriptors2.at<short>(i,j)=" << descriptors2.at<short>(i,j) << std::endl;
+ if (descriptors1.at<short>(i, j) != descriptors2.at<short>(i, j)) {
+ std::cerr << "descriptors1.at<short>(i,j)=" << descriptors1.at<short>(i, j)
+ << " ; descriptors2.at<short>(i,j)=" << descriptors2.at<short>(i, j) << std::endl;
return false;
}
break;
case CV_32S:
- if(descriptors1.at<int>(i,j) != descriptors2.at<int>(i,j)) {
- std::cerr << "descriptors1.at<int>(i,j)=" << descriptors1.at<int>(i,j) <<
- " ; descriptors2.at<int>(i,j)=" << descriptors2.at<int>(i,j) << std::endl;
+ if (descriptors1.at<int>(i, j) != descriptors2.at<int>(i, j)) {
+ std::cerr << "descriptors1.at<int>(i,j)=" << descriptors1.at<int>(i, j)
+ << " ; descriptors2.at<int>(i,j)=" << descriptors2.at<int>(i, j) << std::endl;
return false;
}
break;
case CV_32F:
- if(!vpMath::equal(descriptors1.at<float>(i,j), descriptors2.at<float>(i,j), std::numeric_limits<float>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(9) << "descriptors1.at<float>(i,j)=" << descriptors1.at<float>(i,j)
- << " ; descriptors2.at<float>(i,j)=" << descriptors2.at<float>(i,j) << std::endl;
+ if (!vpMath::equal(descriptors1.at<float>(i, j), descriptors2.at<float>(i, j),
+ std::numeric_limits<float>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(9)
+ << "descriptors1.at<float>(i,j)=" << descriptors1.at<float>(i, j)
+ << " ; descriptors2.at<float>(i,j)=" << descriptors2.at<float>(i, j) << std::endl;
return false;
}
break;
case CV_64F:
- if(!vpMath::equal(descriptors1.at<double>(i,j), descriptors2.at<double>(i,j), std::numeric_limits<double>::epsilon())) {
- std::cerr << std::fixed << std::setprecision(17) << "descriptors1.at<double>(i,j)=" << descriptors1.at<double>(i,j)
- << " ; descriptors2.at<double>(i,j)=" << descriptors2.at<double>(i,j) << std::endl;
+ if (!vpMath::equal(descriptors1.at<double>(i, j), descriptors2.at<double>(i, j),
+ std::numeric_limits<double>::epsilon())) {
+ std::cerr << std::fixed << std::setprecision(17)
+ << "descriptors1.at<double>(i,j)=" << descriptors1.at<double>(i, j)
+ << " ; descriptors2.at<double>(i,j)=" << descriptors2.at<double>(i, j) << std::endl;
return false;
}
break;
@@ -276,7 +289,8 @@ bool compareDescriptors(const cv::Mat &descriptors1, const cv::Mat &descriptors2
\brief Test saving / loading learning file.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
std::string opt_opath;
@@ -284,14 +298,15 @@ int main(int argc, const char ** argv) {
std::string opath;
std::string filename;
- //Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
+ if (env_ipath.empty()) {
throw vpException(vpException::ioError, "Please set the VISP_INPUT_IMAGE_PATH environment variable value.");
}
- // Set the default output path
+// Set the default output path
#if defined(_WIN32)
opt_opath = "C:/temp";
#else
@@ -314,34 +329,18 @@ int main(int argc, const char ** argv) {
// Append to the output path string, the login name of the user
opath = vpIoTools::createFilePath(opath, username);
- // Test if the output path exist. If no try to create it
- if (vpIoTools::checkDirectory(opath) == false) {
- try {
- // Create the dirname
- vpIoTools::makeDirectory(opath);
- }
- catch (...) {
- usage(argv[0], NULL, opt_opath, username);
- std::stringstream ss;
- ss << std::endl << "ERROR:" << std::endl;
- ss << " Cannot create " << opath << std::endl;
- ss << " Check your -o " << opt_opath << " option " << std::endl;
- throw vpException(vpException::ioError, ss.str().c_str());
- }
- }
-
vpImage<unsigned char> I;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/Klimt");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "Klimt");
- //Build the name of the image files
+ // Build the name of the image files
std::string img_filename = vpIoTools::createFilePath(dirname, "/Klimt.ppm");
vpImageIo::read(I, img_filename);
vpKeyPoint keyPoints;
- //Test with binary descriptor
+ // Test with binary descriptor
{
std::string keypointName = "ORB";
keyPoints.setDetector(keypointName);
@@ -352,141 +351,144 @@ int main(int argc, const char ** argv) {
std::vector<cv::KeyPoint> trainKeyPoints;
keyPoints.getTrainKeyPoints(trainKeyPoints);
cv::Mat trainDescriptors = keyPoints.getTrainDescriptors();
- if(trainKeyPoints.empty() || trainDescriptors.empty() || (int) trainKeyPoints.size() != trainDescriptors.rows) {
- throw vpException(vpException::fatalError, "Problem when detecting keypoints or when computing descriptors !");
+ if (trainKeyPoints.empty() || trainDescriptors.empty() || (int)trainKeyPoints.size() != trainDescriptors.rows) {
+ throw vpException(vpException::fatalError, "Problem when detecting "
+ "keypoints or when "
+ "computing descriptors !");
}
- //Save in binary with training images
+ // Save in binary with training images
filename = vpIoTools::createFilePath(opath, "bin_with_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin");
keyPoints.saveLearningData(filename, true, true);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint1;
read_keypoint1.loadLearningData(filename, true);
std::vector<cv::KeyPoint> trainKeyPoints_read;
read_keypoint1.getTrainKeyPoints(trainKeyPoints_read);
cv::Mat trainDescriptors_read = read_keypoint1.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved "
- "in binary with train images 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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "binary with train images saved !");
}
-
- //Save in binary with no training images
+ // Save in binary with no training images
filename = vpIoTools::createFilePath(opath, "bin_without_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin");
keyPoints.saveLearningData(filename, true, false);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint2;
read_keypoint2.loadLearningData(filename, true);
trainKeyPoints_read.clear();
read_keypoint2.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint2.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "binary without train images !");
+ "binary without train images !");
}
- if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
- throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
- "binary without train images !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "binary without train images !");
}
-
#if defined(VISP_HAVE_XML2)
- //Save in xml with training images
+ // Save in xml with training images
filename = vpIoTools::createFilePath(opath, "xml_with_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml");
keyPoints.saveLearningData(filename, false, true);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint3;
read_keypoint3.loadLearningData(filename, false);
trainKeyPoints_read.clear();
read_keypoint3.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint3.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "xml with train images saved !");
+ "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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "xml with train images saved !");
}
-
- //Save in xml without training images
+ // Save in xml without training images
filename = vpIoTools::createFilePath(opath, "xml_without_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml");
keyPoints.saveLearningData(filename, false, false);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint4;
read_keypoint4.loadLearningData(filename, false);
trainKeyPoints_read.clear();
read_keypoint4.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint4.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "xml without train images saved !");
+ "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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "xml without train images saved !");
}
#endif
std::cout << "Saving / loading learning files with binary descriptor are ok !" << std::endl;
}
-
- //Test with floating point descriptor
-#if defined(VISP_HAVE_OPENCV_NONFREE) || ( (VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(VISP_HAVE_OPENCV_XFEATURES2D) )
+// Test with floating point descriptor
+#if defined(VISP_HAVE_OPENCV_NONFREE) || \
+ ((VISP_HAVE_OPENCV_VERSION >= 0x030000) && defined(VISP_HAVE_OPENCV_XFEATURES2D))
{
std::string keypointName = "SIFT";
keyPoints.setDetector(keypointName);
@@ -497,139 +499,142 @@ int main(int argc, const char ** argv) {
std::vector<cv::KeyPoint> trainKeyPoints;
keyPoints.getTrainKeyPoints(trainKeyPoints);
cv::Mat trainDescriptors = keyPoints.getTrainDescriptors();
- if(trainKeyPoints.empty() || trainDescriptors.empty() || (int) trainKeyPoints.size() != trainDescriptors.rows) {
- throw vpException(vpException::fatalError, "Problem when detecting keypoints or when computing descriptors (SIFT) !");
+ 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
+ // Save in binary with training images
filename = vpIoTools::createFilePath(opath, "bin_with_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_bin_with_img.bin");
keyPoints.saveLearningData(filename, true, true);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint1;
read_keypoint1.loadLearningData(filename, true);
std::vector<cv::KeyPoint> trainKeyPoints_read;
read_keypoint1.getTrainKeyPoints(trainKeyPoints_read);
cv::Mat trainDescriptors_read = read_keypoint1.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "binary with train images saved !");
+ "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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "binary with train images saved !");
}
-
- //Save in binary with no training images
+ // Save in binary with no training images
filename = vpIoTools::createFilePath(opath, "bin_without_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_bin_without_img.bin");
keyPoints.saveLearningData(filename, true, false);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint2;
read_keypoint2.loadLearningData(filename, true);
trainKeyPoints_read.clear();
read_keypoint2.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint2.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "binary without train images saved !");
+ "binary without train images saved !");
}
- if(!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
- throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading learning file saved in "
- "binary without train images saved !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "binary without train images saved !");
}
-
#if defined(VISP_HAVE_XML2)
- //Save in xml with training images
+ // Save in xml with training images
filename = vpIoTools::createFilePath(opath, "xml_with_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_xml_with_img.xml");
keyPoints.saveLearningData(filename, false, true);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint3;
read_keypoint3.loadLearningData(filename, false);
trainKeyPoints_read.clear();
read_keypoint3.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint3.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "xml with train images saved !");
+ "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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "xml with train images saved !");
}
-
- //Save in xml without training images
+ // Save in xml without training images
filename = vpIoTools::createFilePath(opath, "xml_without_img");
vpIoTools::makeDirectory(filename);
filename = vpIoTools::createFilePath(filename, "test_save_in_xml_without_img.xml");
keyPoints.saveLearningData(filename, false, false);
- //Test if save is ok
- if(!vpIoTools::checkFilename(filename)) {
+ // Test if save is ok
+ if (!vpIoTools::checkFilename(filename)) {
std::stringstream ss;
ss << "Problem when saving file=" << filename;
throw vpException(vpException::ioError, ss.str().c_str());
}
- //Test if read is ok
+ // Test if read is ok
vpKeyPoint read_keypoint4;
read_keypoint4.loadLearningData(filename, false);
trainKeyPoints_read.clear();
read_keypoint4.getTrainKeyPoints(trainKeyPoints_read);
trainDescriptors_read = read_keypoint4.getTrainDescriptors();
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_read)) {
throw vpException(vpException::fatalError, "Problem with trainKeyPoints when reading learning file saved in "
- "xml without train images saved !");
+ "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 !");
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_read)) {
+ throw vpException(vpException::fatalError, "Problem with trainDescriptors when reading "
+ "learning file saved in "
+ "xml without train images saved !");
}
#endif
- std::cout << "Saving / loading learning files with floating point descriptor are ok !" << std::endl;
+ std::cout << "Saving / loading learning files with floating point "
+ "descriptor are ok !"
+ << std::endl;
-
- //Test vpKeyPoint::reset()
+ // Test vpKeyPoint::reset()
vpKeyPoint keypoint_reset;
keypointName = "ORB";
@@ -638,7 +643,7 @@ int main(int argc, const char ** argv) {
keypoint_reset.buildReference(I);
- //reset
+ // reset
keypoint_reset.reset();
keypointName = "SIFT";
@@ -651,21 +656,23 @@ int main(int argc, const char ** argv) {
keypoint_reset.getTrainKeyPoints(trainKeyPoints_reset);
cv::Mat trainDescriptors_reset = keypoint_reset.getTrainDescriptors();
- //If reset is ok, we should get the same keypoints and the same descriptors
- if(!compareKeyPoints(trainKeyPoints, trainKeyPoints_reset)) {
+ // If reset is ok, we should get the same keypoints and the same
+ // descriptors
+ if (!compareKeyPoints(trainKeyPoints, trainKeyPoints_reset)) {
throw vpException(vpException::fatalError, "Problem with vpKeyPoint::reset() and trainKeyPoints !");
}
- if(!compareDescriptors(trainDescriptors, trainDescriptors_reset)) {
+ if (!compareDescriptors(trainDescriptors, trainDescriptors_reset)) {
throw vpException(vpException::fatalError, "Problem with vpKeyPoint::reset() and trainDescriptors !");
}
- std::cout << "vpKeyPoint::reset() is ok with trainKeyPoints and trainDescriptors !" << std::endl;
+ std::cout << "vpKeyPoint::reset() is ok with trainKeyPoints and "
+ "trainDescriptors !"
+ << std::endl;
}
#endif
-
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return -1;
}
@@ -675,7 +682,8 @@ int main(int argc, const char ** argv) {
return 0;
}
#else
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return 0;
diff --git a/modules/vision/test/key-point/testKeyPoint.cpp b/modules/vision/test/key-point/testKeyPoint.cpp
index d180414..1d27745 100644
--- a/modules/vision/test/key-point/testKeyPoint.cpp
+++ b/modules/vision/test/key-point/testKeyPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -41,19 +42,19 @@
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020301)
-#include <visp3/vision/vpKeyPoint.h>
#include <visp3/core/vpImage.h>
-#include <visp3/io/vpImageIo.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/io/vpVideoReader.h>
-#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/io/vpVideoReader.h>
+#include <visp3/vision/vpKeyPoint.h>
// List of allowed command line options
-#define GETOPTARGS "cdh"
+#define GETOPTARGS "cdh"
void usage(const char *name, const char *badparam);
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display);
@@ -105,17 +106,25 @@ OPTIONS: \n\
bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
{
const char *optarg_;
- int c;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'h': usage(argv[0], NULL); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'h':
+ usage(argv[0], NULL);
+ return false;
+ break;
default:
usage(argv[0], optarg_);
- return false; break;
+ return false;
+ break;
}
}
@@ -135,7 +144,8 @@ bool getOptions(int argc, const char **argv, bool &click_allowed, bool &display)
\brief Test keypoint matching.
*/
-int main(int argc, const char ** argv) {
+int main(int argc, const char **argv)
+{
try {
std::string env_ipath;
bool opt_click_allowed = true;
@@ -143,28 +153,31 @@ int main(int argc, const char ** argv) {
// Read the command line options
if (getOptions(argc, argv, opt_click_allowed, opt_display) == false) {
- exit (-1);
+ exit(-1);
}
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
- if(env_ipath.empty()) {
- std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment variable value." << std::endl;
+ if (env_ipath.empty()) {
+ std::cerr << "Please set the VISP_INPUT_IMAGE_PATH environment "
+ "variable value."
+ << std::endl;
return -1;
}
vpImage<unsigned char> Iref, Icur, Imatch;
- //Set the path location of the image sequence
- std::string dirname = vpIoTools::createFilePath(env_ipath, "ViSP-images/mbt/cube");
+ // Set the path location of the image sequence
+ std::string dirname = vpIoTools::createFilePath(env_ipath, "mbt/cube");
- //Build the name of the image files
+ // Build the name of the image files
std::string filenameRef = vpIoTools::createFilePath(dirname, "image0000.pgm");
vpImageIo::read(Iref, filenameRef);
std::string filenameCur = vpIoTools::createFilePath(dirname, "image%04d.pgm");
- //Init keypoints
+ // Init keypoints
vpKeyPoint keypoints("ORB", "ORB", "BruteForce-Hamming");
std::cout << "Build " << keypoints.buildReference(Iref) << " reference points." << std::endl;
@@ -173,8 +186,8 @@ int main(int argc, const char ** argv) {
g.open(Icur);
g.acquire(Icur);
- Imatch.resize(Icur.getHeight(), 2*Icur.getWidth());
- Imatch.insert(Iref, vpImagePoint(0,0));
+ Imatch.resize(Icur.getHeight(), 2 * Icur.getWidth());
+ Imatch.insert(Iref, vpImagePoint(0, 0));
#if defined VISP_HAVE_X11
vpDisplayX display;
@@ -193,37 +206,36 @@ int main(int argc, const char ** argv) {
bool opt_click = false;
vpMouseButton::vpMouseButtonType button;
- while(!g.end()) {
+ while (!g.end()) {
g.acquire(Icur);
Imatch.insert(Icur, vpImagePoint(0, Icur.getWidth()));
- if(opt_display) {
+ if (opt_display) {
vpDisplay::display(Imatch);
}
- //Match keypoints
+ // Match keypoints
keypoints.matchPoint(Icur);
- //Display image with keypoints matched
+ // Display image with keypoints matched
keypoints.displayMatching(Iref, Imatch);
- if(opt_display) {
+ if (opt_display) {
vpDisplay::flush(Imatch);
}
- //Click requested to process next image
+ // Click requested to process next image
if (opt_click_allowed && opt_display) {
- if(opt_click) {
+ if (opt_click) {
vpDisplay::getClick(Imatch, button, true);
- if(button == vpMouseButton::button3) {
+ if (button == vpMouseButton::button3) {
opt_click = false;
}
} else {
- //Use right click to enable/disable step by step tracking
- if(vpDisplay::getClick(Imatch, button, false)) {
+ // Use right click to enable/disable step by step tracking
+ if (vpDisplay::getClick(Imatch, button, false)) {
if (button == vpMouseButton::button3) {
opt_click = true;
- }
- else if(button == vpMouseButton::button1) {
+ } else if (button == vpMouseButton::button1) {
break;
}
}
@@ -231,7 +243,7 @@ int main(int argc, const char ** argv) {
}
}
- } catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << e.what() << std::endl;
return -1;
}
@@ -240,7 +252,8 @@ int main(int argc, const char ** argv) {
return 0;
}
#else
-int main() {
+int main()
+{
std::cerr << "You need OpenCV library." << std::endl;
return 0;
diff --git a/modules/vision/test/key-point/testSurfKeyPoint.cpp b/modules/vision/test/key-point/testSurfKeyPoint.cpp
index 4d19703..10bbb6d 100644
--- a/modules/vision/test/key-point/testSurfKeyPoint.cpp
+++ b/modules/vision/test/key-point/testSurfKeyPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,26 +36,26 @@
*
*****************************************************************************/
-#include <stdlib.h>
-#include <stdio.h>
-#include <sstream>
#include <iomanip>
#include <iostream>
+#include <sstream>
+#include <stdio.h>
+#include <stdlib.h>
#include <visp3/core/vpConfig.h>
-#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 <visp3/core/vpCameraParameters.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGTK.h>
+#include <visp3/core/vpImage.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/vision/vpKeyPointSurf.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
+#include <visp3/vision/vpKeyPointSurf.h>
/*!
\example testSurfKeyPoint.cpp
@@ -63,11 +64,10 @@
*/
// List of allowed command line options
-#define GETOPTARGS "cdi:h"
+#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);
/*!
@@ -91,7 +91,7 @@ OPTIONS: Default\n\
-i <input image path> %s\n\
Set image input path.\n\
From this path read image \n\
- \"ViSP-images/ellipse/ellipse.pgm\"\n\
+ \"ellipse/ellipse.pgm\"\n\
Setting the VISP_INPUT_IMAGE_PATH environment\n\
variable produces the same behaviour than using\n\
this option.\n\
@@ -104,12 +104,10 @@ OPTIONS: Default\n\
Turn off the display.\n\
\n\
-h\n\
- Print the help.\n",
- ipath.c_str());
+ Print the help.\n", ipath.c_str());
if (badparam)
fprintf(stdout, "\nERROR: Bad parameter [%s]\n", badparam);
-
}
/*!
@@ -123,22 +121,31 @@ 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;
+ int c;
while ((c = vpParseArgv::parse(argc, argv, GETOPTARGS, &optarg_)) > 1) {
switch (c) {
- case 'c': click_allowed = false; break;
- case 'd': display = false; break;
- case 'i': ipath = optarg_; break;
- case 'h': usage(argv[0], NULL, ipath); return false; break;
+ case 'c':
+ click_allowed = false;
+ break;
+ case 'd':
+ display = false;
+ break;
+ case 'i':
+ ipath = optarg_;
+ break;
+ case 'h':
+ usage(argv[0], NULL, ipath);
+ return false;
+ break;
default:
usage(argv[0], optarg_, ipath);
- return false; break;
+ return false;
+ break;
}
}
@@ -153,9 +160,7 @@ bool getOptions(int argc, const char **argv, std::string &ipath,
return true;
}
-
-int
-main(int argc, const char ** argv)
+int main(int argc, const char **argv)
{
try {
std::string env_ipath;
@@ -167,17 +172,17 @@ main(int argc, const char ** argv)
bool opt_click_allowed = true;
bool opt_display = true;
- // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH environment variable value
+ // Get the visp-images-data package path or VISP_INPUT_IMAGE_PATH
+ // environment variable value
env_ipath = vpIoTools::getViSPImagesDataPath();
// Set the default input path
- if (! env_ipath.empty())
+ if (!env_ipath.empty())
ipath = env_ipath;
// Read the command line options
- if (getOptions(argc, argv, opt_ipath,
- opt_click_allowed, opt_display) == false) {
- exit (-1);
+ if (getOptions(argc, argv, opt_ipath, opt_click_allowed, opt_display) == false) {
+ exit(-1);
}
// Get the option values
@@ -188,8 +193,7 @@ main(int argc, const char ** argv)
// the input path comming from the command line option
if (!opt_ipath.empty() && !env_ipath.empty()) {
if (ipath != env_ipath) {
- std::cout << std::endl
- << "WARNING: " << std::endl;
+ std::cout << std::endl << "WARNING: " << std::endl;
std::cout << " Since -i <visp image path=" << ipath << "> "
<< " is different from VISP_IMAGE_PATH=" << env_ipath << std::endl
<< " we skip the environment variable." << std::endl;
@@ -197,26 +201,24 @@ main(int argc, const char ** argv)
}
// Test if an input path is set
- if (opt_ipath.empty() && env_ipath.empty()){
+ if (opt_ipath.empty() && env_ipath.empty()) {
usage(argv[0], NULL, ipath);
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH "
- << std::endl
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Use -i <visp image path> option or set VISP_INPUT_IMAGE_PATH " << std::endl
<< " environment variable to specify the location of the " << std::endl
- << " image path where test images are located." << std::endl << std::endl;
+ << " image path where test images are located." << std::endl
+ << std::endl;
exit(-1);
}
-
// Declare an image, this is a gray level image (unsigned char)
// it size is not defined yet, it will be defined when the image will
// read on the disk
- vpImage<unsigned char> Iref ;
- vpImage<unsigned char> Icur ;
+ vpImage<unsigned char> Iref;
+ vpImage<unsigned char> Icur;
// Set the path location of the image sequence
- dirname = vpIoTools::createFilePath(ipath, "ViSP-images/cube");
+ dirname = vpIoTools::createFilePath(ipath, "cube");
// Build the name of the image file
filenameRef = vpIoTools::createFilePath(dirname, "image.0000.pgm");
@@ -228,31 +230,27 @@ main(int argc, const char ** argv)
//
// exception readPGM may throw various exception if, for example,
// the file does not exist, or if the memory cannot be allocated
- try{
+ try {
std::cout << "Load: " << filenameRef << std::endl;
- vpImageIo::read(Iref, filenameRef) ;
+ vpImageIo::read(Iref, filenameRef);
std::cout << "Load: " << filenameCur << std::endl;
- vpImageIo::read(Icur, filenameCur) ;
- }
- catch(...)
- {
- // an exception is throwned if an exception from readPGM has been catched
- // here this will result in the end of the program
- // Note that another error message has been printed from readPGM
- // to give more information about the error
- std::cerr << std::endl
- << "ERROR:" << std::endl;
- std::cerr << " Cannot read " << filenameRef << "or" << filenameCur <<std::endl;
+ vpImageIo::read(Icur, filenameCur);
+ } catch (...) {
+ // an exception is throwned if an exception from readPGM has been
+ // catched here this will result in the end of the program Note that
+ // another error message has been printed from readPGM to give more
+ // information about the error
+ std::cerr << std::endl << "ERROR:" << std::endl;
+ std::cerr << " Cannot read " << filenameRef << "or" << filenameCur << std::endl;
std::cerr << " Check your -i " << ipath << " option " << std::endl
- << " or VISP_INPUT_IMAGE_PATH environment variable."
- << std::endl;
+ << " or VISP_INPUT_IMAGE_PATH environment variable." << std::endl;
exit(-1);
}
- // We open a window using either X11, GTK or GDI.
+// We open a window using either X11, GTK or GDI.
#if defined VISP_HAVE_X11
vpDisplayX display[2];
#elif defined VISP_HAVE_GTK
@@ -265,27 +263,29 @@ main(int argc, const char ** argv)
if (opt_display) {
// Display size is automatically defined by the image (I) size
- display[0].init(Iref, 100, 100, "Reference image") ;
+ display[0].init(Iref, 100, 100, "Reference image");
// Display the image
// The image class has a member that specify a pointer toward
// the display that has been initialized in the display declaration
// therefore is is no longuer necessary to make a reference to the
// display variable.
- vpDisplay::display(Iref) ;
- //Flush the display
- vpDisplay::flush(Iref) ;
+ vpDisplay::display(Iref);
+ // Flush the display
+ vpDisplay::flush(Iref);
}
vpKeyPointSurf surf;
unsigned int nbrRef;
- if (opt_click_allowed && opt_display)
- {
- std::cout << "Select a part of the image where the reference points will be computed. This part is a rectangle." << std::endl;
- std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
+ if (opt_click_allowed && opt_display) {
+ std::cout << "Select a part of the image where the reference points "
+ "will be computed. This part is a rectangle."
+ << std::endl;
+ std::cout << "Click first on the top left corner and then on the "
+ "bottom right corner."
+ << std::endl;
vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
+ for (int i = 0; i < 2; i++) {
vpDisplay::getClick(Iref, corners[i]);
}
@@ -295,37 +295,37 @@ main(int argc, const char ** argv)
height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- //Computes the reference points
+ // Computes the reference points
nbrRef = surf.buildReference(Iref, corners[0], height, width);
}
- else
- {
+ else {
nbrRef = surf.buildReference(Iref);
}
- if(nbrRef < 1)
- {
+ if (nbrRef < 1) {
std::cerr << "No reference point" << std::endl;
exit(-1);
}
unsigned int nbrPair;
if (opt_display) {
- display[1].init(Icur, (int)(100+Iref.getWidth()), 100, "Current image") ;
+ display[1].init(Icur, (int)(100 + Iref.getWidth()), 100, "Current image");
// display variable.
- vpDisplay::display(Icur) ;
- //Flush the display
- vpDisplay::flush(Icur) ;
+ vpDisplay::display(Icur);
+ // Flush the display
+ vpDisplay::flush(Icur);
}
- if (opt_click_allowed && opt_display)
- {
- std::cout << "Select a part of the current image where the reference will be search. This part is a rectangle." << std::endl;
- std::cout << "Click first on the top left corner and then on the bottom right corner." << std::endl;
+ if (opt_click_allowed && opt_display) {
+ std::cout << "Select a part of the current image where the reference "
+ "will be search. This part is a rectangle."
+ << std::endl;
+ std::cout << "Click first on the top left corner and then on the "
+ "bottom right corner."
+ << std::endl;
vpImagePoint corners[2];
- for (int i=0 ; i < 2 ; i++)
- {
+ for (int i = 0; i < 2; i++) {
vpDisplay::getClick(Icur, corners[i]);
}
vpDisplay::displayRectangle(Icur, corners[0], corners[1], vpColor::green);
@@ -334,43 +334,39 @@ main(int argc, const char ** argv)
height = (unsigned int)(corners[1].get_i() - corners[0].get_i());
width = (unsigned int)(corners[1].get_j() - corners[0].get_j());
- //Computes the reference points
+ // Computes the reference points
nbrPair = surf.matchPoint(Icur, corners[0], height, width);
}
- else
- {
+ else {
nbrPair = surf.matchPoint(Icur);
}
- if(nbrPair < 1)
- {
+ if (nbrPair < 1) {
std::cout << "No point matched" << std::endl;
}
- if (opt_display)
- {
+ if (opt_display) {
surf.display(Iref, Icur, 7);
- vpDisplay::flush(Iref) ;
- vpDisplay::flush(Icur) ;
- if (opt_click_allowed)
- {
+ vpDisplay::flush(Iref);
+ vpDisplay::flush(Icur);
+ if (opt_click_allowed) {
std::cout << "A click on the reference image to exit..." << std::endl;
vpDisplay::getClick(Iref);
}
}
return (0);
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return (1);
}
}
#else
-int
-main()
+int main()
{
- std::cerr << "You do not have 1.1.0 <= OpenCV < 2.3.0 that contains opencv_nonfree component..." << std::endl;
+ std::cerr << "You do not have 1.1.0 <= OpenCV < 2.3.0 that contains "
+ "opencv_nonfree component..."
+ << std::endl;
}
#endif
diff --git a/modules/vision/test/pose/testFindMatch.cpp b/modules/vision/test/pose/testFindMatch.cpp
index 5fd6d91..9310002 100644
--- a/modules/vision/test/pose/testFindMatch.cpp
+++ b/modules/vision/test/pose/testFindMatch.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,17 +38,16 @@
*
*****************************************************************************/
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#define L 0.1
-
/*!
\example testFindMatch.cpp
@@ -55,43 +55,40 @@
*/
-int
-main()
+int main()
{
try {
std::cout << "Find Matches using Ransac" << std::endl;
std::vector<vpPoint> P;
- P.push_back( vpPoint(-L,-L, 0 ) );
- P.push_back( vpPoint(L,-L, 0 ) );
- P.push_back( vpPoint(L,L, 0 ) );
- P.push_back( vpPoint(-L,L, 0 ) );
- P.push_back( vpPoint(-0,L/2., L ) );
+ P.push_back(vpPoint(-L, -L, 0));
+ P.push_back(vpPoint(L, -L, 0));
+ P.push_back(vpPoint(L, L, 0));
+ P.push_back(vpPoint(-L, L, 0));
+ P.push_back(vpPoint(-0, L / 2., L));
- vpHomogeneousMatrix cMo_ref(0, 0.2, 1, vpMath::rad(3), vpMath::rad(-2), vpMath::rad(10)) ;
+ vpHomogeneousMatrix cMo_ref(0, 0.2, 1, vpMath::rad(3), vpMath::rad(-2), vpMath::rad(10));
- std::vector<vpPoint> p( P.size() );
- for(unsigned int i=0 ; i < P.size() ; i++)
- {
+ std::vector<vpPoint> p(P.size());
+ for (unsigned int i = 0; i < P.size(); i++) {
vpPoint pt = P[i];
pt.project(cMo_ref);
p[i].set_x(pt.get_x());
p[i].set_y(pt.get_y());
}
- unsigned int ninliers ;
+ unsigned int ninliers;
std::vector<vpPoint> inliers;
double threshold = 1e-6;
unsigned int nbInlierToReachConsensus = (unsigned int)(P.size());
- vpHomogeneousMatrix cMo ;
+ 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++)
- {
- inliers[i].print() ;
+ for (unsigned int i = 0; i < inliers.size(); i++) {
+ inliers[i].print();
std::cout << std::endl;
}
@@ -105,16 +102,15 @@ main()
std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
int test_fail = 0;
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ for (unsigned int i = 0; i < 6; i++) {
+ if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
test_fail = 1;
}
std::cout << "Matching is " << (test_fail ? "badly" : "well") << " performed" << std::endl;
return test_fail;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vision/test/pose/testPose.cpp b/modules/vision/test/pose/testPose.cpp
index fc9dd03..b63520f 100644
--- a/modules/vision/test/pose/testPose.cpp
+++ b/modules/vision/test/pose/testPose.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,17 +38,17 @@
*
*****************************************************************************/
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/core/vpRotationMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpRxyzVector.h>
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/vision/vpPose.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#define L 0.035
@@ -68,7 +69,8 @@ void print_pose(const vpHomogeneousMatrix &cMo, const std::string &legend)
{
vpPoseVector cpo = vpPoseVector(cMo);
- std::cout << std::endl << legend << "\n "
+ std::cout << std::endl
+ << legend << "\n "
<< "tx = " << cpo[0] << "\n "
<< "ty = " << cpo[1] << "\n "
<< "tz = " << cpo[2] << "\n "
@@ -88,8 +90,8 @@ int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const v
int fail = 0;
// Test done on the 3D pose
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ for (unsigned int i = 0; i < 6; i++) {
+ if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
fail = 1;
}
@@ -102,72 +104,74 @@ int compare_pose(const vpPose &pose, const vpHomogeneousMatrix &cMo_ref, const v
std::cout << "Not enough point" << std::endl;
return fail;
}
- r = sqrt(r)/pose.listP.size();
- //std::cout << "Residual on each point (meter): " << r << std::endl;
+ 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;
}
int main()
{
try {
- vpPoint P[5] ; // Point to be tracked
- vpPose pose ;
- pose.clearPoint() ;
+ vpPoint P[5]; // Point to be tracked
+ vpPose pose;
+ pose.clearPoint();
- P[0].setWorldCoordinates(-L,-L, 0 ) ;
- P[1].setWorldCoordinates(L,-L, 0 ) ;
- P[2].setWorldCoordinates(L,L, 0 ) ;
- P[3].setWorldCoordinates(-2*L, 3*L, 0 ) ;
- P[4].setWorldCoordinates(-L,L, 0.01 ) ;
+ P[0].setWorldCoordinates(-L, -L, 0);
+ P[1].setWorldCoordinates(L, -L, 0);
+ P[2].setWorldCoordinates(L, L, 0);
+ P[3].setWorldCoordinates(-2 * L, 3 * L, 0);
+ P[4].setWorldCoordinates(-L, L, 0.01);
int test_fail = 0, fail = 0;
- vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0,vpMath::rad(10));
- vpHomogeneousMatrix cMo_ref(cpo_ref) ;
- vpHomogeneousMatrix cMo ; // will contain the estimated pose
-
- for(int i=0 ; i < 5 ; i++) {
- P[i].project(cMo_ref) ;
- //P[i].print();
- pose.addPoint(P[i]) ; // and added to the pose computation class
+ vpPoseVector cpo_ref = vpPoseVector(0.01, 0.02, 0.25, vpMath::rad(5), 0, vpMath::rad(10));
+ vpHomogeneousMatrix cMo_ref(cpo_ref);
+ vpHomogeneousMatrix cMo; // will contain the estimated pose
+
+ for (int i = 0; i < 5; i++) {
+ P[i].project(cMo_ref);
+ // P[i].print();
+ pose.addPoint(P[i]); // and added to the pose computation class
}
// Let's go ...
- print_pose(cMo_ref, std::string("Reference pose")); // print the reference pose
+ print_pose(cMo_ref,
+ std::string("Reference pose")); // print the reference pose
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE, cMo) ;
+ std::cout << "-------------------------------------------------" << std::endl;
+ pose.computePose(vpPose::LAGRANGE, cMo);
print_pose(cMo, std::string("Pose estimated by Lagrange"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange");
test_fail |= fail;
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON, cMo) ;
+ 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) ;
+ pose.computePose(vpPose::RANSAC, cMo);
print_pose(cMo, std::string("Pose estimated by Ransac"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by Ransac");
test_fail |= fail;
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE_LOWE, cMo) ;
+ std::cout << "--------------------------------------------------" << std::endl;
+ pose.computePose(vpPose::LAGRANGE_LOWE, cMo);
print_pose(cMo, std::string("Pose estimated by Lagrange than Lowe"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by Lagrange than Lowe");
test_fail |= fail;
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON_LOWE, cMo) ;
+ std::cout << "--------------------------------------------------" << std::endl;
+ pose.computePose(vpPose::DEMENTHON_LOWE, cMo);
print_pose(cMo, std::string("Pose estimated by Dementhon than Lowe"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than Lowe");
@@ -175,22 +179,22 @@ int main()
// Now Virtual Visual servoing
- std::cout <<"--------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::VIRTUAL_VS, cMo) ;
+ std::cout << "--------------------------------------------------" << std::endl;
+ pose.computePose(vpPose::VIRTUAL_VS, cMo);
print_pose(cMo, std::string("Pose estimated by VVS"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by VVS");
test_fail |= fail;
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo) ;
+ std::cout << "-------------------------------------------------" << std::endl;
+ pose.computePose(vpPose::DEMENTHON_VIRTUAL_VS, cMo);
print_pose(cMo, std::string("Pose estimated by Dementhon than by VVS"));
fail = compare_pose(pose, cMo_ref, cMo, "pose by Dementhon than by VVS");
test_fail |= fail;
- std::cout <<"-------------------------------------------------"<<std::endl ;
- pose.computePose(vpPose::LAGRANGE_VIRTUAL_VS, cMo) ;
+ 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");
@@ -199,8 +203,7 @@ int main()
std::cout << "\nGlobal pose estimation test " << (test_fail ? "fail" : "is ok") << std::endl;
return test_fail;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vision/test/pose/testPoseFeatures.cpp b/modules/vision/test/pose/testPoseFeatures.cpp
index 987350c..6417410 100644
--- a/modules/vision/test/pose/testPoseFeatures.cpp
+++ b/modules/vision/test/pose/testPoseFeatures.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,16 +37,16 @@
*****************************************************************************/
#include <iostream>
-#include <vector>
#include <limits>
+#include <vector>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/core/vpPoint.h>
#include <visp3/core/vpImage.h>
-#include <visp3/core/vpCameraParameters.h>
-#include <visp3/vision/vpPoseFeatures.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/vision/vpPose.h>
+#include <visp3/vision/vpPoseFeatures.h>
/*!
\example testPoseFeatures.cpp
@@ -57,30 +58,27 @@
#ifndef DOXYGEN_SHOULD_SKIP_THIS
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
-class vp_createPointClass{
+class vp_createPointClass
+{
public:
int value;
-
- vp_createPointClass() : value(0){}
-
- int vp_createPoint(vpFeaturePoint &fp,const vpPoint &v){
+
+ vp_createPointClass() : value(0) {}
+
+ int vp_createPoint(vpFeaturePoint &fp, const vpPoint &v)
+ {
value += 1;
- vpFeatureBuilder::create(fp,v);
+ vpFeatureBuilder::create(fp, v);
return value;
}
};
-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)
@@ -88,7 +86,7 @@ int test_pose(bool use_robust)
else
std::cout << "** Test pose estimation from features\n" << std::endl;
- vpImage<unsigned char> I(600,600);
+ vpImage<unsigned char> I(600, 600);
vpHomogeneousMatrix cMo_ref(0., 0., 1., vpMath::rad(0), vpMath::rad(0), vpMath::rad(60));
vpPoseVector pose_ref = vpPoseVector(cMo_ref);
@@ -103,35 +101,32 @@ int test_pose(bool use_robust)
double val = 0.25;
double val2 = 0.0;
- //2D Point Feature
- pts.push_back( vpPoint(0.0,-val,val2) );
- pts.push_back( vpPoint(0.0,val,val2) );
- pts.push_back( vpPoint(-val,val,val2) );
+ // 2D Point Feature
+ pts.push_back(vpPoint(0.0, -val, val2));
+ pts.push_back(vpPoint(0.0, val, val2));
+ pts.push_back(vpPoint(-val, val, val2));
- //Segment Feature
- pts.push_back( vpPoint(-val,-val/2.0,val2) );
- pts.push_back( vpPoint(val,val/2.0,val2) );
+ // Segment Feature
+ pts.push_back(vpPoint(-val, -val / 2.0, val2));
+ pts.push_back(vpPoint(val, val / 2.0, val2));
- //3D point Feature
- pts.push_back( vpPoint(0.0,0.0,-1.5) );
+ // 3D point Feature
+ pts.push_back(vpPoint(0.0, 0.0, -1.5));
- //Line Feature
+ // Line Feature
vpLine line;
- line.setWorldCoordinates(0.0,1.0,0.0,.0,
- 0.0,0.0,1.0,0.0);
+ line.setWorldCoordinates(0.0, 1.0, 0.0, .0, 0.0, 0.0, 1.0, 0.0);
- //Vanishing Point Feature
+ // Vanishing Point Feature
vpLine l1;
- l1.setWorldCoordinates(0.0,1.0,0.2,0.0,
- 1.0,0.0,0.0,-0.25);
+ l1.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, 1.0, 0.0, 0.0, -0.25);
vpLine l2;
- l2.setWorldCoordinates(0.0,1.0,0.2,0.0,
- -1.0,0.0,0.0,-0.25);
+ l2.setWorldCoordinates(0.0, 1.0, 0.2, 0.0, -1.0, 0.0, 0.0, -0.25);
- //Ellipse Feature
+ // Ellipse Feature
vpCircle circle;
- circle.setWorldCoordinates(0.0, 0.0, 1.0 , 0.0, 0.0, 0.0, 0.25);
+ circle.setWorldCoordinates(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.25);
pts[0].project(cMo_ref);
pts[1].project(cMo_ref);
@@ -155,7 +150,7 @@ int test_pose(bool use_robust)
pose.addFeaturePoint3D(pts[5]);
- pose.addFeatureVanishingPoint(l1,l2);
+ pose.addFeatureVanishingPoint(l1, l2);
// pose.addFeatureSegment(pts[3],pts[4]);
//
@@ -167,9 +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]);
@@ -185,12 +180,12 @@ int test_pose(bool use_robust)
std::cout << "\nPose used as initialisation of the pose computation : " << std::endl;
std::cout << pose_est.t() << std::endl;
- if (! use_robust)
+ if (!use_robust)
pose.computePose(cMo_est);
else
pose.computePose(cMo_est, vpPoseFeatures::ROBUST_VIRTUAL_VS);
- if (! 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;
@@ -200,13 +195,12 @@ int test_pose(bool use_robust)
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++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ for (unsigned int i = 0; i < 6; i++) {
+ if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
test_fail = 1;
}
@@ -225,8 +219,7 @@ int main()
return -1;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
return -1;
}
diff --git a/modules/vision/test/pose/testPoseRansac.cpp b/modules/vision/test/pose/testPoseRansac.cpp
index 0319754..7d9e542 100644
--- a/modules/vision/test/pose/testPoseRansac.cpp
+++ b/modules/vision/test/pose/testPoseRansac.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,17 +38,16 @@
*
*****************************************************************************/
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
#define L 0.1
-
/*!
\example testPoseRansac.cpp
@@ -55,57 +55,54 @@
*/
-int
-main()
+int main()
{
try {
std::cout << "Pose computation with matched points" << std::endl;
- std::vector<vpPoint> P; // Point to be tracked
-
- P.push_back( vpPoint(-L,-L, 0 ) );
- P.push_back( vpPoint(L,-L, 0 ) );
- P.push_back( vpPoint(L,L, 0 ) );
- P.push_back( vpPoint(-L,L, 0 ) );
-
- double L2 = L*3.0;
- P.push_back( vpPoint(0,-L2, 0 ) );
- P.push_back( vpPoint(L2,0, 0 ) );
- P.push_back( vpPoint(0,L2, 0 ) );
- P.push_back( vpPoint(-L2,0, 0 ) );
-
- vpHomogeneousMatrix cMo_ref(0, 0.2, 1, 0, 0, 0) ;
- for(size_t i=0 ; i < P.size(); i++)
- {
- P[i].project(cMo_ref) ;
- P[i].print() ;
+ std::vector<vpPoint> P; // Point to be tracked
+
+ P.push_back(vpPoint(-L, -L, 0));
+ P.push_back(vpPoint(L, -L, 0));
+ P.push_back(vpPoint(L, L, 0));
+ P.push_back(vpPoint(-L, L, 0));
+
+ double L2 = L * 3.0;
+ P.push_back(vpPoint(0, -L2, 0));
+ P.push_back(vpPoint(L2, 0, 0));
+ P.push_back(vpPoint(0, L2, 0));
+ P.push_back(vpPoint(-L2, 0, 0));
+
+ vpHomogeneousMatrix cMo_ref(0, 0.2, 1, 0, 0, 0);
+ for (size_t i = 0; i < P.size(); i++) {
+ P[i].project(cMo_ref);
+ P[i].print();
std::cout << std::endl;
}
- //Introduce an error
+ // Introduce an error
double error = 0.01;
- P[3].set_y(P[3].get_y() + 2*error);
+ P[3].set_y(P[3].get_y() + 2 * error);
P[6].set_x(P[6].get_x() + error);
-
+
vpPose pose;
- for(size_t i=0 ; i < P.size() ; i++)
+ for (size_t i = 0; i < P.size(); i++)
pose.addPoint(P[i]);
-
+
unsigned int nbInlierToReachConsensus = (unsigned int)(75.0 * (double)(P.size()) / 100.0);
double threshold = 0.001;
-
+
pose.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
pose.setRansacThreshold(threshold);
-
- vpHomogeneousMatrix cMo ;
- //vpPose::ransac(lp,lP, 5, 1e-6, ninliers, lPi, cMo) ;
+
+ vpHomogeneousMatrix cMo;
+ // vpPose::ransac(lp,lP, 5, 1e-6, ninliers, lPi, cMo) ;
pose.computePose(vpPose::RANSAC, cMo);
-
+
std::vector<vpPoint> inliers = pose.getRansacInliers();
-
+
std::cout << "Inliers: " << std::endl;
- for (unsigned int i = 0; i < inliers.size() ; i++)
- {
- inliers[i].print() ;
+ for (unsigned int i = 0; i < inliers.size(); i++) {
+ inliers[i].print();
std::cout << std::endl;
}
@@ -117,15 +114,14 @@ main()
std::cout << "estimated cMo :\n" << pose_est.t() << std::endl << std::endl;
int test_fail = 0;
- for(unsigned int i=0; i<6; i++) {
- if (std::fabs(pose_ref[i]-pose_est[i]) > 0.001)
+ for (unsigned int i = 0; i < 6; i++) {
+ if (std::fabs(pose_ref[i] - pose_est[i]) > 0.001)
test_fail = 1;
}
std::cout << "Pose is " << (test_fail ? "badly" : "well") << " estimated" << std::endl;
return test_fail;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vision/test/pose/testPoseRansac2.cpp b/modules/vision/test/pose/testPoseRansac2.cpp
index ad0481e..2aca266 100644
--- a/modules/vision/test/pose/testPoseRansac2.cpp
+++ b/modules/vision/test/pose/testPoseRansac2.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,1657 +36,1659 @@
*
*****************************************************************************/
-#include <iomanip>
-#include <sstream>
#include <algorithm>
+#include <iomanip>
#include <map>
-#include <visp3/vision/vpPose.h>
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMath.h>
+#include <sstream>
#include <visp3/core/vpGaussRand.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/core/vpMath.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/vision/vpPose.h>
-//Stanford Bunny Model points
-const std::string file_content =
- "0.038043 0.109755 0.016169 "
- "-0.080459 0.135827 0.050032 "
- "-0.080437 0.137229 0.049095 "
- "-0.039537 0.165252 0.003107 "
- "-0.029517 0.175738 -0.005254 "
- "-0.076590 0.172837 -0.038783 "
- "0.037067 0.109719 0.023178 "
- "0.002482 0.038559 0.046163 "
- "-0.071500 0.163630 -0.013838 "
- "-0.063129 0.154030 -0.010769 "
- "0.038061 0.108343 0.022172 "
- "-0.038632 0.167867 0.002788 "
- "0.001485 0.039988 0.046431 "
- "0.002487 0.039963 0.046169 "
- "-0.076694 0.171427 -0.035893 "
- "-0.037523 0.124042 0.025944 "
- "-0.057469 0.041348 0.046193 "
- "-0.044485 0.041114 0.043653 "
- "-0.043493 0.041078 0.043361 "
- "-0.062047 0.176838 -0.060831 "
- "-0.022629 0.157855 -0.004227 "
- "-0.058497 0.042632 0.044737 "
- "-0.043500 0.042437 0.043101 "
- "-0.038476 0.042327 0.041891 "
- "-0.021510 0.043531 0.053479 "
- "-0.001517 0.042851 0.046801 "
- "-0.035500 0.174151 -0.001806 "
- "0.043233 0.089594 -0.006456 "
- "-0.058494 0.043959 0.043965 "
- "-0.052515 0.043997 0.044898 "
- "-0.021503 0.044884 0.053016 "
- "-0.019514 0.044832 0.052531 "
- "-0.026748 0.109750 0.038619 "
- "-0.080524 0.148418 0.038173 "
- "-0.041486 0.169687 0.001623 "
- "-0.063449 0.097112 -0.017938 "
- "-0.054487 0.045217 0.043831 "
- "-0.053511 0.045215 0.043764 "
- "-0.037512 0.045091 0.040752 "
- "-0.088156 0.151596 0.022886 "
- "-0.025891 0.048891 0.049042 "
- "-0.071506 0.148353 0.040851 "
- "0.027212 0.088765 -0.022232 "
- "-0.054494 0.046553 0.042265 "
- "-0.058656 0.152453 0.032611 "
- "-0.070511 0.148356 0.040726 "
- "-0.023471 0.183379 -0.010712 "
- "-0.072524 0.137240 0.048963 "
- "0.039501 0.046973 0.031597 "
- "0.045346 0.082603 0.023076 "
- "-0.061964 0.147832 -0.004318 "
- "-0.034505 0.049198 0.039001 "
- "-0.027492 0.049767 0.045472 "
- "-0.078513 0.145668 0.042746 "
- "-0.046493 0.165223 0.004593 "
- "0.032495 0.048497 0.032163 "
- "0.041518 0.048439 0.032146 "
- "-0.064113 0.155183 -0.040605 "
- "-0.044898 0.168579 0.001542 "
- "-0.051381 0.050043 0.034731 "
- "-0.032509 0.050660 0.038421 "
- "0.006619 0.047417 0.050356 "
- "-0.015062 0.160937 -0.011420 "
- "-0.080526 0.149910 0.035970 "
- "-0.040710 0.050423 0.039629 "
- "-0.031463 0.051903 0.037377 "
- "-0.006309 0.051457 0.052597 "
- "-0.056444 0.115317 0.036137 "
- "-0.063698 0.164648 -0.028601 "
- "0.034501 0.051352 0.032162 "
- "-0.083451 0.149763 0.033502 "
- "-0.064494 0.149793 0.037510 "
- "-0.063485 0.149796 0.037152 "
- "0.031455 0.113206 0.029881 "
- "-0.045515 0.053319 0.038154 "
- "0.010509 0.054658 0.052359 "
- "-0.057480 0.115365 0.036701 "
- "0.037597 0.088359 -0.014764 "
- "0.035531 0.052720 0.031990 "
- "-0.064662 0.151974 0.036264 "
- "-0.018605 0.049922 0.047107 "
- "-0.000094 0.052014 0.054278 "
- "-0.075359 0.113961 -0.005684 "
- "0.036518 0.054117 0.031704 "
- "-0.013502 0.057389 0.051750 "
- "0.009410 0.060149 0.052895 "
- "-0.018488 0.122208 0.030928 "
- "0.038169 0.109765 0.014166 "
- "-0.042948 0.170625 -0.000616 "
- "-0.053583 0.055075 0.011440 "
- "-0.017474 0.058729 0.050990 "
- "-0.013551 0.058830 0.052544 "
- "0.000322 0.058135 0.054484 "
- "0.008500 0.058924 0.053176 "
- "-0.069486 0.116115 0.051835 "
- "-0.052433 0.115217 0.034103 "
- "-0.085941 0.125175 0.049068 "
- "-0.072600 0.145757 -0.016854 "
- "-0.021451 0.184864 -0.013038 "
- "-0.027746 0.064856 0.037498 "
- "-0.017457 0.060172 0.051453 "
- "-0.014477 0.060305 0.052805 "
- "-0.013472 0.060385 0.053624 "
- "0.010494 0.118230 0.036879 "
- "-0.025456 0.120601 0.028847 "
- "-0.024489 0.120706 0.029404 "
- "-0.051475 0.115278 0.033811 "
- "-0.023488 0.122057 0.027634 "
- "0.038511 0.058329 0.031423 "
- "-0.076502 0.121776 0.052772 "
- "-0.014480 0.061748 0.053510 "
- "0.003858 0.062044 0.056777 "
- "0.011472 0.118200 0.036572 "
- "-0.077835 0.145715 0.043808 "
- "0.039532 0.059677 0.030697 "
- "-0.054472 0.060781 0.026950 "
- "-0.048101 0.135891 0.016805 "
- "-0.014501 0.063146 0.053679 "
- "-0.005881 0.062411 0.056478 "
- "-0.062841 0.144409 -0.007635 "
- "-0.020619 0.186125 -0.015175 "
- "-0.057485 0.062333 0.028342 "
- "-0.047781 0.063839 0.037342 "
- "-0.069510 0.148387 0.040505 "
- "0.011484 0.123198 0.034270 "
- "-0.009507 0.125161 0.031090 "
- "-0.062211 0.147546 -0.005052 "
- "-0.053516 0.064085 0.033078 "
- "-0.026557 0.064650 0.039328 "
- "0.046038 0.067997 0.025283 "
- "0.009517 0.066006 0.054677 "
- "0.011502 0.119616 0.035894 "
- "-0.047437 0.120726 0.028345 "
- "-0.011482 0.125053 0.030655 "
- "0.051514 0.063765 0.028901 "
- "-0.044486 0.066153 0.040137 "
- "-0.012589 0.069082 0.054415 "
- "0.010512 0.067367 0.054298 "
- "-0.048480 0.117974 0.031095 "
- "-0.049455 0.118007 0.031644 "
- "-0.091160 0.146144 0.025161 "
- "0.042563 0.065071 0.027562 "
- "-0.005859 0.087537 -0.037176 "
- "-0.044518 0.067563 0.040463 "
- "0.000030 0.068579 0.056492 "
- "0.010524 0.068777 0.054575 "
- "-0.047469 0.117985 0.030630 "
- "-0.081528 0.148400 0.037410 "
- "-0.029791 0.154227 -0.004672 "
- "-0.016743 0.123139 0.030848 "
- "0.053516 0.066419 0.026864 "
- "-0.071191 0.155859 0.001279 "
- "-0.036068 0.156277 -0.010490 "
- "0.042534 0.067858 0.027368 "
- "0.044646 0.069351 0.024242 "
- "0.047498 0.067780 0.026220 "
- "-0.047725 0.135673 0.014006 "
- "-0.065141 0.159482 -0.056501 "
- "-0.046491 0.070446 0.041037 "
- "-0.045986 0.073503 0.042256 "
- "-0.038661 0.063179 0.041795 "
- "-0.003043 0.073778 0.059461 "
- "0.009520 0.071626 0.055408 "
- "0.036826 0.109713 0.024182 "
- "0.043569 0.069164 0.025997 "
- "0.049500 0.069124 0.025332 "
- "0.051511 0.069144 0.025235 "
- "-0.069372 0.180628 -0.057909 "
- "-0.072540 0.162229 -0.011353 "
- "-0.020454 0.184888 -0.013987 "
- "-0.048492 0.071808 0.040533 "
- "-0.009124 0.125328 0.030976 "
- "-0.017522 0.187118 -0.019415 "
- "-0.018462 0.072978 0.054580 "
- "-0.080508 0.121673 0.049920 "
- "0.043584 0.070558 0.025859 "
- "-0.011119 0.163582 -0.018751 "
- "-0.019434 0.184905 -0.015328 "
- "-0.027457 0.073402 0.042686 "
- "-0.031497 0.117989 0.030856 "
- "-0.019943 0.187176 -0.015950 "
- "-0.091048 0.113254 0.017639 "
- "-0.089443 0.151569 0.016470 "
- "0.006504 0.123782 0.033960 "
- "-0.072537 0.138625 0.048318 "
- "-0.000692 0.100255 0.047827 "
- "-0.076123 0.163992 -0.018367 "
- "-0.008085 0.125292 0.031359 "
- "-0.020456 0.077205 0.054931 "
- "-0.011270 0.120275 0.036868 "
- "-0.068487 0.120374 0.052788 "
- "-0.036504 0.168234 0.000586 "
- "-0.046713 0.131987 0.005569 "
- "-0.064501 0.077478 0.041325 "
- "-0.029667 0.077446 0.041883 "
- "0.009089 0.080660 0.054966 "
- "-0.079510 0.149797 0.036758 "
- "-0.017682 0.082305 0.057622 "
- "-0.071505 0.147007 0.042162 "
- "-0.082501 0.107588 0.027351 "
- "-0.078078 0.154685 0.029078 "
- "-0.031275 0.118165 0.030639 "
- "-0.050393 0.085761 0.046102 "
- "-0.053471 0.115278 0.034303 "
- "0.007481 0.115492 0.039253 "
- "-0.075260 0.127230 0.053502 "
- "-0.042039 0.084825 0.042519 "
- "-0.007645 0.079109 0.058188 "
- "0.018475 0.082463 0.051596 "
- "-0.074281 0.117286 0.053631 "
- "-0.065353 0.085654 0.044968 "
- "-0.059945 0.074585 0.041788 "
- "-0.037174 0.081950 0.043756 "
- "-0.068377 0.176522 -0.058018 "
- "0.017494 0.083857 0.051403 "
- "0.018494 0.083833 0.051046 "
- "0.019476 0.083796 0.050749 "
- "-0.047453 0.119349 0.029360 "
- "-0.003971 0.124981 0.033251 "
- "-0.025836 0.094514 -0.026615 "
- "0.017485 0.085207 0.050786 "
- "0.020474 0.085138 0.050012 "
- "-0.074496 0.130260 0.052409 "
- "-0.026922 0.181455 -0.007884 "
- "-0.032593 0.177828 -0.005142 "
- "-0.030548 0.087232 0.043284 "
- "-0.020478 0.086949 0.055949 "
- "-0.012103 0.092198 0.056667 "
- "0.008495 0.086943 0.055839 "
- "-0.092808 0.120209 0.041284 "
- "0.021487 0.086500 0.049224 "
- "-0.023441 0.181825 -0.010570 "
- "-0.036983 0.092657 0.044193 "
- "0.005296 0.086140 0.057126 "
- "0.009509 0.088290 0.055046 "
- "0.018490 0.087902 0.049248 "
- "0.022460 0.087857 0.048776 "
- "0.037946 0.109770 0.017170 "
- "-0.058473 0.115363 0.036971 "
- "0.040458 0.097140 0.028174 "
- "0.039909 0.107011 0.006164 "
- "-0.071514 0.088744 0.041619 "
- "0.011490 0.089612 0.054036 "
- "-0.010050 0.166619 -0.017705 "
- "0.019647 0.086817 0.049574 "
- "0.022481 0.089252 0.048474 "
- "-0.018485 0.091067 0.055002 "
- "-0.038692 0.115422 0.033305 "
- "-0.069491 0.124597 0.052657 "
- "-0.004492 0.123763 0.034322 "
- "-0.070533 0.144185 0.044275 "
- "-0.070146 0.089763 0.042620 "
- "0.012498 0.092313 0.052383 "
- "-0.018064 0.122407 0.030816 "
- "-0.018232 0.187056 -0.018294 "
- "-0.072494 0.092871 0.041339 "
- "0.017475 0.093454 0.048154 "
- "-0.018226 0.117876 0.036486 "
- "-0.059685 0.147446 0.036516 "
- "-0.071526 0.094419 0.041653 "
- "-0.050053 0.096180 0.044423 "
- "0.013460 0.094917 0.050136 "
- "-0.072540 0.095957 0.041270 "
- "-0.056884 0.092762 0.045348 "
- "-0.040294 0.098952 0.041786 "
- "-0.024503 0.096017 0.044275 "
- "-0.014473 0.096429 0.052209 "
- "-0.013468 0.096423 0.052316 "
- "0.022488 0.096164 0.046634 "
- "-0.044495 0.120650 0.027643 "
- "-0.073439 0.148627 -0.029805 "
- "0.006439 0.132232 0.014596 "
- "-0.080457 0.138622 0.048117 "
- "-0.027509 0.181505 -0.008742 "
- "-0.071519 0.097212 0.040997 "
- "-0.053895 0.097200 0.043345 "
- "-0.024482 0.097451 0.044460 "
- "-0.023481 0.097449 0.044821 "
- "-0.009483 0.097835 0.051868 "
- "-0.002497 0.097829 0.052428 "
- "-0.000511 0.097839 0.052433 "
- "0.001458 0.097737 0.051907 "
- "-0.020496 0.098841 0.044481 "
- "-0.013463 0.098956 0.047529 "
- "0.023359 0.095899 0.046777 "
- "0.020477 0.098952 0.046347 "
- "-0.011316 0.129221 0.002910 "
- "-0.079478 0.142827 0.045105 "
- "-0.046949 0.093514 -0.021920 "
- "-0.070498 0.099990 0.040323 "
- "-0.059835 0.102266 0.042689 "
- "-0.029833 0.101135 0.043080 "
- "-0.019498 0.100240 0.044034 "
- "-0.016507 0.100263 0.043963 "
- "-0.015502 0.100285 0.044225 "
- "-0.012572 0.100502 0.044597 "
- "0.001493 0.100277 0.046986 "
- "0.011888 0.102476 0.047452 "
- "0.019476 0.100345 0.046212 "
- "-0.046466 0.167133 0.003131 "
- "-0.070478 0.114632 0.051042 "
- "-0.065862 0.153866 -0.001908 "
- "-0.016492 0.101629 0.043954 "
- "-0.033748 0.119587 0.029726 "
- "-0.001496 0.101631 0.044188 "
- "-0.074502 0.116130 0.052068 "
- "-0.024607 0.157782 -0.002504 "
- "-0.074558 0.102629 0.036985 "
- "-0.044387 0.106839 0.040714 "
- "-0.018030 0.106099 0.042558 "
- "-0.014500 0.103011 0.043250 "
- "-0.012495 0.103038 0.043735 "
- "-0.006500 0.103045 0.043680 "
- "-0.001500 0.103045 0.044056 "
- "0.035089 0.111006 -0.001837 "
- "-0.068495 0.121776 0.052768 "
- "-0.046574 0.153815 0.009349 "
- "-0.074536 0.104026 0.036830 "
- "-0.047007 0.099425 0.042698 "
- "-0.001513 0.104469 0.044133 "
- "-0.042032 0.171559 -0.001711 "
- "-0.008282 0.101652 0.043830 "
- "-0.006486 0.108283 0.043970 "
- "-0.001528 0.105863 0.043996 "
- "0.024524 0.105615 0.039234 "
- "0.021424 0.121505 0.031182 "
- "0.017255 0.109608 0.039746 "
- "-0.035520 0.169730 -0.000115 "
- "0.024505 0.108393 0.038558 "
- "-0.029469 0.178820 -0.005543 "
- "-0.000500 0.100727 0.046395 "
- "-0.069489 0.109539 0.038095 "
- "-0.068505 0.109678 0.037959 "
- "-0.066496 0.109745 0.037770 "
- "0.005485 0.109984 0.041677 "
- "0.023135 0.125159 0.021213 "
- "-0.040910 0.173193 -0.005611 "
- "-0.032536 0.172727 -0.002702 "
- "0.006478 0.111357 0.041027 "
- "0.022491 0.111137 0.037371 "
- "-0.050136 0.110464 0.037566 "
- "-0.019238 0.036205 0.052830 "
- "-0.005178 0.036734 0.048450 "
- "-0.004153 0.036789 0.048168 "
- "-0.031089 0.159271 -0.013062 "
- "0.017745 0.037928 0.042910 "
- "-0.073907 0.171564 -0.047586 "
- "-0.018941 0.037618 0.053049 "
- "0.007356 0.038818 0.045452 "
- "0.018182 0.036268 0.042512 "
- "-0.053475 0.036001 0.046489 "
- "-0.050165 0.036131 0.046172 "
- "-0.043194 0.036383 0.044045 "
- "0.022179 0.040783 0.040543 "
- "0.018183 0.102054 0.045770 "
- "0.026779 0.040277 0.031351 "
- "-0.054330 0.097111 -0.022408 "
- "-0.000216 0.041170 0.046762 "
- "0.000822 0.041223 0.046540 "
- "0.025055 0.042091 0.037664 "
- "0.026269 0.039553 0.030992 "
- "-0.029066 0.093469 -0.024545 "
- "-0.039288 0.044575 0.041301 "
- "-0.006058 0.045881 0.047354 "
- "-0.066457 0.110304 0.037295 "
- "0.011942 0.123166 0.033243 "
- "-0.022142 0.043052 0.053661 "
- "-0.021404 0.045562 0.053215 "
- "-0.086889 0.102318 0.023370 "
- "-0.015871 0.161111 -0.009045 "
- "0.017650 0.043039 0.044185 "
- "0.041315 0.045195 0.029602 "
- "-0.040635 0.159922 0.002471 "
- "-0.020287 0.044518 0.052893 "
- "-0.012904 0.181603 -0.024044 "
- "0.011322 0.045022 0.044806 "
- "0.040105 0.046687 0.031387 "
- "0.043756 0.046763 0.029636 "
- "0.049174 0.044307 0.021629 "
- "0.050879 0.046558 0.022316 "
- "0.051380 0.046479 0.021262 "
- "-0.054311 0.149562 0.026707 "
- "-0.047190 0.156504 0.008879 "
- "-0.018702 0.045934 0.051674 "
- "-0.055378 0.151032 0.028750 "
- "-0.013113 0.184484 -0.026054 "
- "0.028021 0.046282 0.036458 "
- "0.017700 0.103348 0.045066 "
- "-0.067150 0.145376 0.041641 "
- "-0.032860 0.153633 -0.000344 "
- "0.009909 0.048971 0.049252 "
- "0.010676 0.048959 0.048584 "
- "0.033645 0.049093 0.031488 "
- "0.043084 0.049674 0.032233 "
- "0.054174 0.049532 0.021869 "
- "0.036990 0.111103 0.004167 "
- "-0.011129 0.182158 -0.027227 "
- "-0.023928 0.181600 -0.010098 "
- "0.055409 0.050963 0.021892 "
- "-0.054071 0.123768 0.038080 "
- "-0.016353 0.160230 -0.010916 "
- "0.015237 0.121915 0.032797 "
- "-0.071652 0.139715 0.047438 "
- "0.020741 0.055550 0.048100 "
- "0.023003 0.049323 0.039928 "
- "-0.023553 0.156826 -0.005180 "
- "-0.049505 0.050280 0.036331 "
- "-0.029135 0.177171 -0.005342 "
- "0.035495 0.103128 -0.011159 "
- "0.059989 0.062227 0.021516 "
- "0.012111 0.116449 0.037015 "
- "-0.054345 0.151005 0.026686 "
- "0.013667 0.055693 0.050689 "
- "0.043020 0.049120 0.032996 "
- "0.053744 0.054858 0.027754 "
- "-0.062742 0.148221 0.037477 "
- "-0.049178 0.130401 0.030288 "
- "0.039228 0.108354 0.009166 "
- "0.010869 0.057643 0.052478 "
- "0.058756 0.057992 0.021536 "
- "-0.055127 0.047834 0.040050 "
- "-0.072139 0.128420 0.051978 "
- "-0.067933 0.145349 0.042265 "
- "0.051151 0.060858 0.029927 "
- "0.039303 0.101950 0.027071 "
- "-0.028849 0.162315 -0.003816 "
- "-0.071123 0.141136 0.046452 "
- "-0.036101 0.158265 0.004249 "
- "-0.069556 0.114278 0.050291 "
- "-0.089109 0.099057 0.008227 "
- "0.026136 0.060229 0.044245 "
- "0.040315 0.061632 0.030023 "
- "-0.070208 0.141157 0.046016 "
- "-0.015086 0.162585 -0.009690 "
- "-0.026913 0.180109 -0.007400 "
- "-0.001352 0.101336 0.044402 "
- "0.006923 0.063037 0.055588 "
- "0.007933 0.063074 0.055296 "
- "0.034320 0.060597 0.039225 "
- "0.035670 0.108162 -0.004821 "
- "0.019494 0.107309 0.040648 "
- "0.038763 0.104620 0.026268 "
- "-0.068821 0.145351 0.042741 "
- "-0.053805 0.048637 0.038267 "
- "-0.014344 0.162546 -0.010432 "
- "0.039538 0.100622 0.027388 "
- "-0.013982 0.178957 -0.021269 "
- "0.008946 0.064444 0.055029 "
- "0.012129 0.065567 0.053747 "
- "0.020511 0.065463 0.048451 "
- "0.052356 0.064799 0.028141 "
- "0.053298 0.064822 0.027758 "
- "-0.052653 0.152155 0.015019 "
- "0.036409 0.085446 -0.016706 "
- "0.028693 0.111728 0.034550 "
- "0.002438 0.114145 0.041742 "
- "-0.030297 0.121635 -0.007023 "
- "0.008263 0.110977 0.040210 "
- "0.037500 0.107296 0.026215 "
- "0.039845 0.059964 0.030392 "
- "-0.063695 0.167813 -0.041594 "
- "0.051687 0.064527 0.029555 "
- "0.027852 0.115393 0.031660 "
- "0.009143 0.112364 0.039751 "
- "0.017991 0.120614 0.033226 "
- "-0.069120 0.118603 0.052746 "
- "0.039512 0.070945 0.033551 "
- "0.056979 0.068693 0.022604 "
- "0.057617 0.068678 0.021770 "
- "0.058779 0.068629 0.019916 "
- "0.016910 0.120624 0.033430 "
- "-0.049644 0.132456 0.028190 "
- "-0.069785 0.145343 0.043102 "
- "-0.010211 0.168384 -0.018420 "
- "-0.010519 0.165484 -0.017015 "
- "-0.013977 0.164028 -0.011589 "
- "0.007651 0.075327 0.056504 "
- "-0.007476 0.101243 0.044380 "
- "-0.062397 0.162983 -0.056654 "
- "-0.010634 0.174261 -0.022600 "
- "0.029870 0.114081 0.031047 "
- "0.026908 0.070176 0.043296 "
- "0.032967 0.077598 0.041811 "
- "0.040252 0.102372 0.026220 "
- "-0.060796 0.125745 0.042799 "
- "-0.002700 0.109146 -0.021617 "
- "-0.053082 0.139518 0.026662 "
- "-0.072831 0.136891 0.049250 "
- "-0.022318 0.181221 -0.012616 "
- "-0.062587 0.137282 0.036031 "
- "0.036468 0.111067 0.002168 "
- "0.022493 0.120697 0.030928 "
- "-0.068515 0.132239 0.047795 "
- "0.015592 0.078141 0.054165 "
- "-0.044768 0.146394 0.003586 "
- "0.004576 0.108164 0.041888 "
- "0.042099 0.093008 0.027170 "
- "-0.063799 0.143909 0.038920 "
- "-0.065799 0.120921 0.051999 "
- "-0.061867 0.112377 0.036730 "
- "-0.033558 0.173613 -0.001108 "
- "0.010985 0.123169 0.033628 "
- "-0.069391 0.117164 0.052337 "
- "-0.049240 0.118066 0.031545 "
- "0.045144 0.080268 0.023028 "
- "0.045515 0.080198 0.021690 "
- "-0.009929 0.166943 -0.018055 "
- "0.033894 0.111352 0.028046 "
- "-0.068718 0.114088 0.050433 "
- "-0.062044 0.114243 0.037354 "
- "0.027865 0.103405 0.039124 "
- "-0.055094 0.114335 0.035233 "
- "0.026730 0.079214 0.047114 "
- "-0.046699 0.122484 0.027073 "
- "-0.009970 0.167686 -0.017702 "
- "0.043315 0.083190 0.027513 "
- "0.022758 0.094213 0.047144 "
- "-0.045635 0.123874 0.025084 "
- "0.020065 0.084847 0.050222 "
- "0.036554 0.108655 0.026577 "
- "-0.033646 0.165935 -0.004786 "
- "0.020807 0.086180 0.049534 "
- "0.004625 0.109546 0.041960 "
- "-0.069930 0.141590 0.046209 "
- "0.021629 0.087551 0.049039 "
- "0.039804 0.089822 0.033495 "
- "-0.048053 0.150190 -0.004201 "
- "-0.024682 0.159952 -0.001088 "
- "-0.050727 0.115538 0.033580 "
- "0.009088 0.097645 0.049418 "
- "-0.024774 0.180145 -0.009568 "
- "-0.061113 0.149657 0.036321 "
- "-0.017561 0.186044 -0.018449 "
- "-0.045252 0.122806 0.025016 "
- "-0.058955 0.151077 0.034187 "
- "-0.010889 0.108738 -0.021743 "
- "-0.073382 0.135448 0.050183 "
- "-0.048532 0.143887 0.006514 "
- "0.014126 0.090787 0.052728 "
- "-0.054919 0.148123 0.027541 "
- "-0.073651 0.180693 -0.054018 "
- "0.035433 0.111001 -0.000827 "
- "-0.074141 0.105214 0.036938 "
- "0.034274 0.091666 0.040805 "
- "-0.026146 0.180134 -0.008033 "
- "0.041527 0.097216 -0.003903 "
- "0.039787 0.104095 -0.000790 "
- "-0.010438 0.171321 -0.020552 "
- "0.040891 0.094256 -0.006770 "
- "-0.084037 0.145378 0.004810 "
- "0.019655 0.035085 -0.003456 "
- "0.017978 0.035483 -0.012673 "
- "0.017351 0.035184 -0.014686 "
- "0.016671 0.035381 -0.015659 "
- "0.022996 0.037585 0.033289 "
- "-0.089659 0.099833 0.017847 "
- "0.039376 0.039372 0.002323 "
- "0.016325 0.035201 -0.018314 "
- "0.024913 0.039060 0.032333 "
- "0.045899 0.041406 0.011234 "
- "0.045451 0.041238 0.010235 "
- "0.036964 0.040555 -0.001588 "
- "0.014050 0.037936 -0.021724 "
- "0.023136 0.040456 0.039246 "
- "-0.062395 0.163038 -0.054588 "
- "0.048458 0.043018 0.014220 "
- "0.041031 0.042290 -0.000688 "
- "0.020591 0.043659 -0.020978 "
- "-0.058686 0.060316 0.023126 "
- "-0.009223 0.174168 -0.028567 "
- "0.024695 0.042022 0.038270 "
- "-0.062676 0.177191 -0.057627 "
- "0.050252 0.044634 0.018204 "
- "0.025338 0.041873 -0.005885 "
- "0.022728 0.039972 -0.005642 "
- "0.017317 0.041198 -0.021676 "
- "0.040137 0.095364 -0.006888 "
- "-0.086614 0.111799 0.023338 "
- "-0.062457 0.177245 -0.058615 "
- "0.047024 0.045876 -0.000609 "
- "-0.062908 0.150560 -0.025587 "
- "0.052608 0.047598 0.021222 "
- "0.056546 0.049738 0.018075 "
- "0.047681 0.047268 -0.001643 "
- "0.022565 0.045669 -0.018540 "
- "0.018420 0.044214 -0.022632 "
- "-0.009629 0.168316 -0.023669 "
- "-0.014554 0.162843 -0.010082 "
- "0.047073 0.048551 -0.003646 "
- "0.031126 0.046970 -0.006535 "
- "0.028058 0.049173 -0.018234 "
- "0.019626 0.045759 -0.021723 "
- "0.018445 0.045652 -0.022673 "
- "-0.013671 0.162609 -0.015656 "
- "-0.064753 0.156678 -0.047594 "
- "0.048231 0.050030 -0.003699 "
- "-0.061903 0.152590 -0.029073 "
- "-0.064629 0.156694 -0.046598 "
- "-0.060478 0.144516 -0.003635 "
- "0.057159 0.050804 0.007605 "
- "0.053557 0.058091 -0.003559 "
- "0.050638 0.051672 -0.002735 "
- "-0.051929 0.136980 0.026387 "
- "-0.064488 0.156696 -0.045603 "
- "-0.036286 0.151198 0.000402 "
- "0.032998 0.051431 -0.007635 "
- "-0.063640 0.163113 -0.024591 "
- "-0.048075 0.134034 0.005399 "
- "-0.053174 0.150869 0.021398 "
- "-0.053869 0.152421 0.021400 "
- "0.061282 0.060979 0.008052 "
- "-0.049294 0.133961 0.001427 "
- "0.038369 0.088421 -0.013773 "
- "0.054270 0.057579 -0.001787 "
- "0.038928 0.064147 -0.010753 "
- "0.030796 0.048804 -0.007779 "
- "0.026392 0.054968 -0.020802 "
- "-0.050474 0.137043 0.022400 "
- "0.038624 0.089851 -0.012769 "
- "0.054545 0.058989 -0.001818 "
- "0.038123 0.057470 -0.005915 "
- "-0.025394 0.106122 -0.022387 "
- "0.055301 0.060464 -0.000843 "
- "-0.088206 0.135559 0.003366 "
- "0.039239 0.058959 -0.005124 "
- "-0.015832 0.184840 -0.020727 "
- "-0.063934 0.169417 -0.044580 "
- "-0.061679 0.175513 -0.061381 "
- "-0.061651 0.157813 -0.019788 "
- "0.056298 0.063302 0.001233 "
- "0.030795 0.058601 -0.017734 "
- "0.021937 0.055669 -0.027055 "
- "-0.094871 0.122902 0.022189 "
- "-0.091313 0.113305 0.016325 "
- "-0.010049 0.165099 -0.020759 "
- "-0.055288 0.147721 0.028349 "
- "-0.078988 0.138940 -0.004741 "
- "-0.062540 0.149101 -0.006575 "
- "-0.062522 0.149113 -0.005581 "
- "-0.052484 0.150872 0.017395 "
- "0.059075 0.067744 0.020163 "
- "0.042193 0.066261 -0.002834 "
- "0.042068 0.063212 -0.002158 "
- "-0.062769 0.175495 -0.061570 "
- "-0.068864 0.156533 -0.053205 "
- "-0.068854 0.147497 -0.029579 "
- "0.058829 0.069107 0.019159 "
- "0.058229 0.069067 0.006136 "
- "-0.048864 0.162635 -0.005797 "
- "-0.062219 0.177210 -0.060600 "
- "-0.016222 0.186444 -0.020735 "
- "0.059153 0.070495 0.013155 "
- "-0.090970 0.113313 0.018324 "
- "0.041049 0.106185 0.006150 "
- "-0.062655 0.150596 -0.022579 "
- "-0.051989 0.142827 0.018310 "
- "-0.010049 0.177211 -0.025754 "
- "0.038336 0.102640 -0.005826 "
- "-0.010940 0.165104 -0.015661 "
- "-0.039084 0.149487 -0.000629 "
- "-0.057085 0.136926 -0.005008 "
- "-0.079775 0.106127 0.031384 "
- "-0.086972 0.121397 -0.001898 "
- "-0.024832 0.093010 -0.031618 "
- "-0.025615 0.156162 -0.003544 "
- "-0.064398 0.155173 -0.041604 "
- "-0.053935 0.139875 0.028353 "
- "-0.010793 0.180280 -0.026745 "
- "-0.010315 0.180354 -0.028878 "
- "0.046126 0.073615 0.004216 "
- "0.031952 0.069103 -0.018709 "
- "-0.018142 0.159077 -0.009325 "
- "-0.063220 0.150553 -0.027592 "
- "-0.063051 0.152177 -0.008587 "
- "-0.053928 0.152376 0.022392 "
- "-0.092423 0.113974 0.016676 "
- "-0.061997 0.147610 -0.004622 "
- "-0.063084 0.164691 -0.032592 "
- "-0.010363 0.180289 -0.027734 "
- "0.039220 0.077501 -0.010108 "
- "0.029524 0.075033 -0.021815 "
- "-0.023646 0.184410 -0.015053 "
- "-0.052765 0.138519 0.026397 "
- "-0.026155 0.155801 -0.005687 "
- "-0.072526 0.149972 -0.042469 "
- "-0.072234 0.147352 -0.024232 "
- "-0.077900 0.105799 -0.007577 "
- "0.045717 0.079213 0.021174 "
- "0.041064 0.092872 -0.007795 "
- "0.046713 0.072341 0.005563 "
- "-0.063865 0.166212 -0.033602 "
- "-0.062874 0.175671 -0.055595 "
- "0.045825 0.080617 0.020170 "
- "-0.076643 0.149485 -0.008679 "
- "-0.063297 0.144361 -0.009605 "
- "0.045163 0.095673 0.008717 "
- "0.033399 0.080994 -0.018737 "
- "-0.081982 0.112797 -0.000848 "
- "-0.091672 0.117524 0.028069 "
- "-0.048022 0.144366 0.004427 "
- "0.044133 0.080232 -0.002522 "
- "0.037727 0.085584 -0.016367 "
- "-0.030192 0.153974 -0.003571 "
- "-0.062777 0.161085 -0.047210 "
- "-0.062519 0.144437 -0.006582 "
- "0.043849 0.084675 0.026172 "
- "-0.076848 0.148423 0.040238 "
- "-0.067453 0.180926 -0.058380 "
- "-0.045884 0.129671 0.002936 "
- "-0.093435 0.121095 0.042248 "
- "-0.067905 0.173751 -0.043985 "
- "-0.050555 0.136728 0.023252 "
- "-0.071802 0.181053 -0.055655 "
- "-0.010362 0.179472 -0.029571 "
- "-0.005897 0.098097 0.051999 "
- "-0.062828 0.151059 0.036055 "
- "-0.050078 0.136736 0.021957 "
- "-0.045979 0.167363 0.002591 "
- "-0.011364 0.182132 -0.028821 "
- "-0.052576 0.136549 0.028410 "
- "-0.063846 0.180771 -0.058948 "
- "0.003007 0.099142 0.049182 "
- "-0.064141 0.171114 -0.047178 "
- "-0.075916 0.179384 -0.051606 "
- "0.000053 0.097862 0.053297 "
- "-0.074748 0.158885 -0.007836 "
- "-0.011087 0.180248 -0.025982 "
- "-0.076524 0.178854 -0.050295 "
- "-0.086930 0.151358 0.010227 "
- "-0.019131 0.122285 0.030395 "
- "-0.058101 0.158247 0.004154 "
- "-0.082061 0.151973 0.004324 "
- "0.026954 0.116596 0.031065 "
- "-0.013723 0.183161 -0.024107 "
- "-0.082061 0.154804 0.013629 "
- "-0.085429 0.151325 0.028294 "
- "-0.076439 0.098866 -0.011539 "
- "-0.086549 0.109000 0.015694 "
- "-0.091717 0.141326 0.017134 "
- "-0.062003 0.154899 0.004205 "
- "-0.092111 0.128137 0.031104 "
- "-0.068605 0.155434 -0.051807 "
- "-0.005161 0.100957 0.044271 "
- "-0.044251 0.120873 0.027396 "
- "-0.089317 0.151601 0.019497 "
- "-0.063020 0.168283 -0.045257 "
- "-0.077704 0.175863 -0.045075 "
- "-0.091075 0.135224 0.016144 "
- "0.049370 0.069379 0.025027 "
- "-0.007104 0.125174 0.031741 "
- "0.033637 0.112962 -0.002307 "
- "-0.074694 0.111142 -0.007602 "
- "-0.017221 0.187225 -0.021155 "
- "-0.030168 0.118019 0.030802 "
- "-0.028460 0.125795 0.008832 "
- "0.057735 0.070506 0.007469 "
- "0.003964 0.097777 0.051060 "
- "0.032895 0.114630 -0.001059 "
- "0.027501 0.118700 -0.003048 "
- "-0.079635 0.167489 -0.036775 "
- "-0.017050 0.122405 0.031234 "
- "-0.007453 0.100521 0.046910 "
- "-0.053725 0.151161 0.024627 "
- "-0.045982 0.130505 0.004443 "
- "0.012743 0.127966 0.027041 "
- "0.025743 0.120365 -0.002249 "
- "-0.081065 0.111196 -0.000918 "
- "-0.051027 0.136724 0.024552 "
- "-0.067871 0.156792 0.022379 "
- "-0.063800 0.166551 -0.035494 "
- "0.060392 0.069417 0.011831 "
- "-0.047170 0.165970 0.003632 "
- "0.030267 0.120025 0.005493 "
- "-0.091498 0.150215 0.020796 "
- "-0.071332 0.151217 -0.043920 "
- "-0.087751 0.128902 0.000900 "
- "-0.063805 0.152122 -0.002245 "
- "-0.031285 0.119331 0.029156 "
- "0.022998 0.123119 -0.000380 "
- "0.022167 0.123584 -0.000681 "
- "-0.054554 0.160714 0.005405 "
- "0.057670 0.071756 0.018414 "
- "0.022042 0.124260 0.000701 "
- "-0.087413 0.093190 0.004840 "
- "-0.009347 0.098183 0.051937 "
- "-0.061079 0.151413 0.035139 "
- "0.002048 0.102202 0.043813 "
- "-0.066613 0.148623 0.039262 "
- "-0.055002 0.153297 0.028117 "
- "0.046790 0.073773 0.016792 "
- "-0.031448 0.177007 -0.013927 "
- "-0.001162 0.098034 0.052196 "
- "-0.051351 0.152104 0.013223 "
- "-0.042883 0.128827 0.014388 "
- "-0.022585 0.185119 -0.013184 "
- "0.032239 0.117348 0.023130 "
- "0.011504 0.129313 0.001094 "
- "-0.023460 0.184384 -0.011932 "
- "-0.075946 0.097459 -0.012513 "
- "-0.005179 0.129883 0.025223 "
- "0.005861 0.130491 0.001853 "
- "-0.029850 0.180049 -0.011606 "
- "-0.067096 0.164861 -0.018887 "
- "-0.073324 0.170102 -0.027685 "
- "0.052646 0.072994 0.020857 "
- "-0.003124 0.123427 0.034663 "
- "-0.014724 0.129248 0.015884 "
- "-0.075527 0.109807 -0.007602 "
- "-0.027112 0.124435 0.018388 "
- "-0.044261 0.122001 0.025892 "
- "-0.077187 0.153815 0.000184 "
- "-0.084777 0.095970 -0.003931 "
- "-0.047172 0.133935 0.009815 "
- "0.052984 0.069258 0.024906 "
- "0.052194 0.069772 0.024557 "
- "0.007149 0.123902 0.033879 "
- "-0.046057 0.131197 0.005824 "
- "0.007587 0.130944 0.001903 "
- "-0.072750 0.180616 -0.054977 "
- "-0.018876 0.105884 -0.022628 "
- "-0.017875 0.105871 -0.022589 "
- "-0.066685 0.036123 0.035141 "
- "-0.065936 0.036420 0.038649 "
- "-0.060884 0.036716 0.044984 "
- "-0.059243 0.036753 0.046137 "
- "-0.047036 0.035908 0.045106 "
- "-0.044737 0.035714 0.044172 "
- "-0.069135 0.035455 0.012250 "
- "-0.068464 0.035536 0.013028 "
- "-0.062423 0.035753 0.019657 "
- "-0.061746 0.042600 0.042925 "
- "-0.055906 0.037983 0.047068 "
- "-0.037607 0.036543 0.044419 "
- "-0.036930 0.036554 0.045141 "
- "-0.036270 0.036560 0.045881 "
- "-0.033821 0.036733 0.049100 "
- "-0.033215 0.036763 0.049913 "
- "-0.031638 0.036774 0.051182 "
- "-0.030039 0.036799 0.052422 "
- "-0.028341 0.036810 0.053558 "
- "-0.055235 0.043304 0.046580 "
- "-0.026570 0.038204 0.055248 "
- "-0.064199 0.042219 0.027537 "
- "-0.067406 0.035436 0.036194 "
- "-0.064134 0.044628 0.034748 "
- "-0.034115 0.042437 0.048702 "
- "-0.088565 0.096304 0.022720 "
- "-0.071415 0.038661 0.000579 "
- "-0.071384 0.035533 0.010286 "
- "-0.059415 0.039567 0.019974 "
- "-0.002780 0.100393 -0.023542 "
- "-0.030725 0.079300 0.041026 "
- "-0.089150 0.139122 0.037510 "
- "-0.064435 0.042410 0.030248 "
- "-0.064596 0.043007 0.035823 "
- "-0.038969 0.042042 0.041595 "
- "-0.024661 0.042233 0.053955 "
- "-0.078686 0.174223 -0.045863 "
- "-0.055578 0.159581 0.006379 "
- "-0.067561 0.044863 0.007377 "
- "-0.071811 0.095541 0.041327 "
- "-0.063288 0.044436 0.037293 "
- "-0.088305 0.149858 0.026519 "
- "-0.028815 0.043632 0.051009 "
- "-0.021138 0.098548 0.044502 "
- "-0.083902 0.105911 0.026288 "
- "-0.056782 0.043535 0.016717 "
- "-0.090816 0.116185 0.042702 "
- "-0.067315 0.044668 0.006020 "
- "-0.060400 0.047013 0.037152 "
- "-0.074680 0.157704 -0.005566 "
- "-0.045598 0.169625 -0.002419 "
- "-0.057758 0.047918 0.035950 "
- "-0.058863 0.156048 0.009540 "
- "-0.055124 0.048280 0.038911 "
- "-0.020816 0.079725 0.056038 "
- "-0.077790 0.164478 -0.025199 "
- "-0.078272 0.165861 -0.029036 "
- "-0.057476 0.047878 0.009161 "
- "-0.076982 0.179572 -0.051371 "
- "-0.079848 0.116269 0.048758 "
- "-0.023736 0.086541 0.054457 "
- "-0.078852 0.107465 0.032436 "
- "-0.032961 0.051054 0.038223 "
- "-0.075260 0.157705 -0.006422 "
- "-0.055289 0.160933 0.003284 "
- "-0.058227 0.048399 0.006759 "
- "-0.050039 0.047811 0.016868 "
- "-0.058040 0.060877 0.024555 "
- "-0.027311 0.091770 0.045195 "
- "-0.083080 0.081450 0.029210 "
- "-0.077793 0.167262 -0.029530 "
- "-0.075338 0.161792 -0.013478 "
- "-0.051770 0.051602 0.031230 "
- "-0.051573 0.052359 0.030554 "
- "-0.020587 0.094703 0.051310 "
- "-0.087066 0.141474 0.040183 "
- "-0.073808 0.102191 0.037495 "
- "-0.079748 0.078938 0.031423 "
- "-0.082082 0.150001 0.034405 "
- "-0.051491 0.052957 0.030880 "
- "-0.048255 0.053230 0.036171 "
- "-0.047395 0.035957 0.045283 "
- "-0.014451 0.103914 0.043265 "
- "-0.076840 0.178514 -0.050928 "
- "-0.077412 0.160346 -0.016940 "
- "-0.027792 0.053068 0.036540 "
- "-0.076742 0.164619 -0.022017 "
- "-0.051707 0.163863 0.002419 "
- "-0.079102 0.170034 -0.038269 "
- "-0.079102 0.171279 -0.041032 "
- "-0.050567 0.054554 0.017337 "
- "-0.052122 0.055334 0.025863 "
- "-0.077519 0.147451 0.041178 "
- "-0.071550 0.076334 0.037454 "
- "-0.085225 0.114025 0.048015 "
- "-0.088460 0.097654 0.022602 "
- "-0.079604 0.174107 -0.046390 "
- "-0.052890 0.055616 0.012484 "
- "-0.052288 0.055669 0.013306 "
- "-0.051297 0.055683 0.015038 "
- "-0.022939 0.098598 0.044810 "
- "-0.016571 0.056922 0.051097 "
- "-0.020326 0.082530 0.056871 "
- "-0.078583 0.087018 0.037216 "
- "-0.078901 0.172846 -0.044095 "
- "-0.057294 0.059745 0.022361 "
- "-0.053553 0.060429 0.026462 "
- "-0.043647 0.060159 0.039893 "
- "-0.026540 0.059202 0.038381 "
- "-0.068767 0.090095 0.042705 "
- "-0.077518 0.172972 -0.039913 "
- "-0.023874 0.105835 -0.022678 "
- "-0.053287 0.061060 0.028214 "
- "-0.043822 0.061568 0.040004 "
- "-0.085799 0.151046 0.029438 "
- "-0.016547 0.061194 0.052368 "
- "-0.090279 0.116199 0.043584 "
- "-0.077750 0.161717 -0.020949 "
- "-0.079195 0.168601 -0.036643 "
- "-0.085221 0.132036 0.049369 "
- "-0.053062 0.062559 0.030763 "
- "-0.051701 0.062677 0.032235 "
- "-0.051142 0.062742 0.033098 "
- "-0.016685 0.062588 0.052478 "
- "-0.074605 0.114104 -0.006416 "
- "-0.081131 0.129963 0.052448 "
- "-0.077190 0.117737 0.051681 "
- "-0.025047 0.094997 0.044896 "
- "-0.077356 0.163115 -0.021355 "
- "-0.026296 0.063447 0.039421 "
- "-0.016414 0.099612 0.044078 "
- "-0.074764 0.107316 0.036203 "
- "-0.066853 0.091456 0.043506 "
- "-0.082460 0.108623 0.027619 "
- "-0.078951 0.128836 0.054193 "
- "-0.045045 0.065713 0.039695 "
- "-0.025749 0.036557 0.054009 "
- "-0.016813 0.062115 0.052682 "
- "-0.016674 0.066891 0.053711 "
- "-0.011794 0.066719 0.054462 "
- "-0.076681 0.154876 -0.006314 "
- "-0.090499 0.148452 0.024515 "
- "-0.028731 0.074896 0.041482 "
- "0.010138 0.104418 -0.020246 "
- "-0.015186 0.168246 -0.021561 "
- "-0.042881 0.108469 -0.019386 "
- "-0.058997 0.084194 -0.020854 "
- "-0.091683 0.116094 0.040744 "
- "-0.085234 0.104506 0.024826 "
- "-0.071459 0.151188 -0.043322 "
- "-0.017603 0.071155 0.054519 "
- "-0.078807 0.106130 0.032485 "
- "-0.025047 0.080547 0.053580 "
- "-0.071901 0.152408 -0.043423 "
- "-0.023799 0.184341 -0.014597 "
- "-0.019317 0.075402 0.054688 "
- "-0.010806 0.034295 -0.025597 "
- "-0.008683 0.097657 -0.030445 "
- "-0.070396 0.038222 -0.000484 "
- "-0.064320 0.035936 0.024151 "
- "-0.070750 0.162623 -0.045378 "
- "-0.047784 0.038449 -0.012233 "
- "-0.067401 0.040625 -0.003341 "
- "0.012498 0.036363 -0.021917 "
- "-0.090556 0.129700 0.040228 "
- "-0.071563 0.151347 -0.043624 "
- "-0.044911 0.045780 -0.010946 "
- "-0.047318 0.039794 -0.012062 "
- "-0.068755 0.116060 -0.008746 "
- "-0.089126 0.137079 0.025122 "
- "-0.069476 0.042643 0.000668 "
- "-0.072853 0.035550 0.002529 "
- "-0.075578 0.154868 -0.000834 "
- "-0.054371 0.158326 -0.001789 "
- "-0.060851 0.155133 0.003690 "
- "-0.056894 0.158351 0.001188 "
- "-0.057497 0.158400 0.002092 "
- "-0.065238 0.041953 0.033705 "
- "-0.077268 0.155563 -0.011897 "
- "-0.043908 0.034308 -0.026883 "
- "-0.044744 0.039642 -0.021577 "
- "0.022162 0.095920 -0.021972 "
- "-0.068386 0.043493 0.004694 "
- "-0.068639 0.043534 0.005698 "
- "-0.038348 0.041711 -0.027346 "
- "-0.061186 0.044121 -0.004327 "
- "-0.059362 0.047606 0.000101 "
- "-0.039022 0.045911 -0.022759 "
- "-0.063007 0.045830 0.008721 "
- "-0.047064 0.168385 -0.001002 "
- "-0.038149 0.044868 -0.024210 "
- "-0.054255 0.049028 0.029769 "
- "-0.038766 0.045933 -0.022284 "
- "-0.041323 0.046240 -0.016312 "
- "-0.043267 0.046587 -0.011389 "
- "-0.075668 0.155622 -0.002603 "
- "-0.091032 0.144770 0.026167 "
- "-0.086328 0.151358 0.009233 "
- "-0.057566 0.048138 0.034664 "
- "-0.057553 0.048178 0.038938 "
- "-0.036151 0.047160 -0.021326 "
- "-0.082537 0.151557 0.003650 "
- "-0.041416 0.047637 -0.012230 "
- "-0.052279 0.049031 0.035659 "
- "-0.073572 0.148626 -0.026849 "
- "-0.036200 0.048331 -0.016295 "
- "-0.036606 0.048448 -0.014284 "
- "-0.053168 0.050303 -0.006388 "
- "-0.057148 0.052574 0.000631 "
- "-0.030353 0.049890 -0.019369 "
- "-0.052512 0.051984 -0.007188 "
- "0.011853 0.105542 -0.019754 "
- "-0.056771 0.057251 0.007253 "
- "-0.071828 0.141507 -0.008016 "
- "-0.051032 0.054419 0.019619 "
- "-0.088924 0.151541 0.021101 "
- "-0.052832 0.055837 -0.007420 "
- "-0.049863 0.054275 0.015634 "
- "-0.094007 0.129802 0.018180 "
- "-0.024936 0.054649 -0.028408 "
- "-0.075829 0.093523 -0.013687 "
- "0.018167 0.095997 -0.023265 "
- "-0.065589 0.167628 -0.059737 "
- "-0.074146 0.151129 -0.029188 "
- "0.016944 0.094696 -0.024307 "
- "-0.057089 0.060232 -0.002574 "
- "-0.037067 0.156264 -0.010370 "
- "-0.091018 0.149734 0.013441 "
- "-0.052736 0.163457 -0.000904 "
- "-0.078459 0.159178 -0.022073 "
- "-0.056704 0.111275 -0.017686 "
- "-0.079616 0.070320 0.016623 "
- "-0.040892 0.109911 -0.018800 "
- "-0.081009 0.136808 -0.002869 "
- "-0.062766 0.170921 -0.061607 "
- "-0.052123 0.049165 0.035634 "
- "0.040891 0.091776 -0.009100 "
- "0.015157 0.107807 -0.018185 "
- "-0.077880 0.103388 -0.008666 "
- "-0.040161 0.110268 -0.018707 "
- "-0.074397 0.160314 -0.030905 "
- "-0.008919 0.033847 -0.025125 "
- "0.029469 0.095176 -0.018643 "
- "-0.075086 0.067468 0.003551 "
- "-0.078424 0.146692 -0.003613 "
- "-0.091437 0.114681 0.021315 "
- "-0.090341 0.130900 0.040068 "
- "0.004647 0.093885 -0.032195 "
- "-0.077076 0.068957 0.008539 "
- "-0.065286 0.176792 -0.060802 "
- "-0.013252 0.180136 -0.028613 "
- "-0.052901 0.114056 -0.016235 "
- "-0.017913 0.184413 -0.023990 "
- "-0.079885 0.070630 0.012550 "
- "-0.079889 0.070630 0.013550 "
- "-0.025189 0.178943 -0.018780 "
- "-0.077799 0.073099 -0.005325 "
- "-0.080225 0.072000 0.005544 "
- "-0.080010 0.072030 0.018553 "
- "-0.079546 0.071986 0.019558 "
- "-0.026014 0.068834 -0.034504 "
- "-0.030094 0.067301 -0.027970 "
- "-0.030573 0.059945 -0.018933 "
- "-0.075706 0.070108 -0.000047 "
- "-0.039761 0.120231 -0.012793 "
- "-0.072928 0.148723 -0.032782 "
- "-0.038619 0.047455 -0.016386 "
- "0.027694 0.085014 -0.022063 "
- "-0.072448 0.148649 -0.034807 "
- "-0.082956 0.076257 0.016526 "
- "-0.066412 0.162305 -0.057942 "
- "-0.072012 0.148777 -0.035760 "
- "-0.077608 0.077089 -0.007600 "
- "-0.084539 0.077802 0.014517 "
- "-0.071014 0.148895 -0.037822 "
- "-0.075920 0.078323 -0.009591 "
- "-0.086853 0.081079 0.010849 "
- "-0.071911 0.144135 -0.014659 "
- "-0.074627 0.149901 -0.029871 "
- "-0.077891 0.079796 -0.008639 "
- "-0.082390 0.074996 0.003304 "
- "-0.070947 0.140954 -0.008050 "
- "-0.077146 0.156966 -0.011900 "
- "-0.073019 0.080841 -0.014580 "
- "-0.077886 0.081156 -0.009633 "
- "-0.036361 0.078336 -0.019535 "
- "-0.074019 0.082262 -0.014630 "
- "-0.075495 0.155610 -0.001907 "
- "-0.081644 0.129364 -0.004806 "
- "-0.032331 0.078279 -0.031704 "
- "-0.079744 0.081910 -0.007905 "
- "-0.089763 0.133813 0.039204 "
- "0.038710 0.105846 0.000022 "
- "-0.061587 0.117934 -0.010507 "
- "-0.075033 0.085103 -0.014560 "
- "-0.040541 0.166202 -0.012095 "
- "-0.087722 0.151511 0.024071 "
- "-0.033651 0.085644 -0.024757 "
- "-0.075237 0.086484 -0.014514 "
- "-0.087724 0.085428 0.014220 "
- "-0.089609 0.141602 0.031992 "
- "-0.075188 0.152833 -0.015966 "
- "-0.068568 0.156629 -0.052997 "
- "-0.009676 0.104885 -0.023104 "
- "-0.085360 0.109807 0.006222 "
- "-0.039019 0.174111 -0.009029 "
- "-0.027379 0.085971 -0.035654 "
- "-0.055531 0.040944 -0.009922 "
- "-0.086394 0.085570 -0.000840 "
- "-0.076358 0.141673 -0.005738 "
- "-0.032093 0.087730 -0.025498 "
- "-0.017229 0.104564 -0.022745 "
- "-0.049660 0.118919 -0.014131 "
- "-0.089823 0.092558 0.021822 "
- "-0.076654 0.113990 -0.004814 "
- "-0.092635 0.116935 0.008842 "
- "-0.041134 0.037889 -0.027518 "
- "-0.028876 0.046967 -0.026215 "
- "0.000689 0.105475 -0.021690 "
- "0.017762 0.100614 -0.022454 "
- "0.038365 0.101327 -0.007241 "
- "0.016593 0.100464 -0.022378 "
- "-0.025269 0.093126 -0.030867 "
- "-0.052243 0.126999 -0.005034 "
- "0.053623 0.058755 -0.002690 "
- "-0.048269 0.037339 -0.012384 "
- "-0.021680 0.184690 -0.018817 "
- "0.029353 0.117442 -0.002543 "
- "0.021961 0.100321 -0.021160 "
- "-0.012106 0.086958 -0.038462 "
- "-0.013342 0.112081 -0.018621 "
- "0.024541 0.081954 -0.024971 "
- "-0.007420 0.035877 -0.024927 "
- "-0.009175 0.087571 -0.037094 "
- "0.054837 0.073109 0.009023 "
- "0.055426 0.071416 0.006408 "
- "0.056334 0.071137 0.006716 "
- "-0.020732 0.114935 -0.016742 "
- "0.017679 0.085219 -0.028353 "
- "-0.018768 0.034185 -0.027695 "
- "-0.035284 0.173981 -0.012629 "
- "-0.026663 0.178341 -0.017682 "
- "-0.023595 0.180085 -0.019676 "
- "-0.014488 0.185446 -0.025770 "
- "-0.032519 0.174119 -0.014863 "
- "-0.065357 0.155100 0.005646 "
- "-0.064416 0.154996 0.005865 "
- "-0.044287 0.166804 -0.008538 "
- "-0.023240 0.093652 -0.032678 "
- "-0.055818 0.158024 -0.000145 "
- "-0.052392 0.160438 -0.003076 "
- "-0.041472 0.167040 -0.010815 "
- "-0.014584 0.185601 -0.026982 "
- "-0.026852 0.098187 -0.024073 "
- "-0.027912 0.034067 -0.029247 "
- "-0.025710 0.098352 -0.024235 "
- "-0.038543 0.038993 -0.028624 "
- "-0.052489 0.159007 -0.003405 "
- "-0.056184 0.037462 -0.010574 "
- "-0.010553 0.180959 -0.028854 "
- "-0.020256 0.036506 -0.028012 "
- "-0.019833 0.107697 -0.022052 "
- "-0.011421 0.107659 -0.022044 "
- "-0.059774 0.149902 -0.001229 "
- "-0.019309 0.036282 -0.027779 "
- "0.025109 0.111719 -0.012490 "
- "-0.026057 0.037591 -0.029189 "
- "0.011270 0.104834 -0.020502 "
- "0.023069 0.121925 -0.004197 "
- "0.046473 0.074730 0.007000 "
- "-0.039662 0.039902 -0.027457 "
- "-0.037022 0.041095 -0.028655 "
- "-0.009941 0.089177 -0.036764 "
- "-0.005608 0.125002 -0.009489 "
- "-0.033775 0.105400 -0.020968 "
- "-0.016581 0.035588 -0.026711 "
- "-0.058428 0.038335 -0.009417 "
- "-0.063375 0.036885 -0.008048 "
- "-0.011756 0.034513 -0.025836 "
- "0.006642 0.099379 -0.022123 "
- "0.039487 0.105310 0.000603 "
- "-0.061814 0.045340 -0.001725 "
- "-0.063482 0.035282 -0.008247 "
- "-0.055554 0.042026 -0.008894 "
- "-0.005249 0.101176 -0.023207 "
- "-0.028861 0.044835 -0.028193 "
- "-0.028589 0.093525 -0.025224 "
- "-0.058144 0.114319 -0.014834 "
- "-0.015397 0.125153 -0.004109 "
- "-0.010059 0.109235 -0.021708 "
- "-0.059319 0.114075 -0.014560 "
- "-0.010537 0.085053 -0.038446 "
- "-0.046412 0.035458 -0.018725 "
- "-0.065683 0.180548 -0.059031 "
- "0.014582 0.100670 -0.022593 "
- "-0.069184 0.181162 -0.058506 "
- "-0.002499 0.112386 -0.019640 "
- "-0.049884 0.034085 -0.013307 "
- "-0.006156 0.110566 -0.022214 "
- "-0.066229 0.042518 -0.003252 "
- "-0.009516 0.084860 -0.038222 "
- "-0.028518 0.094880 -0.024629 "
- "-0.040442 0.035113 -0.028939 "
- "-0.018198 0.089616 -0.037280 "
- "-0.063353 0.045449 0.000290 "
- "-0.035084 0.107815 -0.020111 "
- "-0.086923 0.112018 0.023246 "
- "0.032361 0.078690 -0.019246 "
- "0.027427 0.078192 -0.023091 "
- "0.005420 0.091290 -0.032762 "
- "-0.002657 0.089995 -0.035491 "
- "-0.009946 0.033902 -0.025192 "
- "0.004610 0.092806 -0.032334 "
- "0.001474 0.103921 -0.022070 "
- "-0.055345 0.138992 -0.003070 "
- "-0.049948 0.135176 0.001169 "
- "-0.048008 0.135924 0.010304 "
- "-0.020760 0.034720 -0.027738 "
- "0.030631 0.076191 -0.020965 "
- "-0.063071 0.173863 -0.061680 "
- "-0.038215 0.047167 -0.018189 "
- "-0.051474 0.135214 -0.000925 "
- "0.011863 0.089406 -0.030726 "
- "0.009956 0.089786 -0.031175 "
- "0.004929 0.114402 -0.020103 "
- "-0.061876 0.138649 -0.006770 "
- "0.027932 0.081332 -0.022263 "
- "0.051888 0.065658 -0.001557 "
- "0.018711 0.100467 -0.022355 "
- "-0.018396 0.036084 -0.027420 "
- "-0.028317 0.096340 -0.024150 "
- "0.001762 0.130465 0.001572 "
- "-0.032558 0.114916 -0.016084 "
- "-0.069082 0.181440 -0.056457 "
- "-0.057620 0.135609 -0.005468 "
- "0.021367 0.085150 -0.026249 "
- "-0.015764 0.035327 -0.026378 "
- "0.028626 0.063844 -0.020406 "
- "-0.036903 0.033739 -0.030894 "
- "-0.057875 0.134121 -0.005853 "
- "-0.043875 0.128361 0.000409 "
- "0.030532 0.081181 -0.020086 "
- "-0.020082 0.097870 -0.024530 "
- "0.015406 0.088042 -0.029297 "
- "-0.021725 0.126122 0.002697 "
- "0.012366 0.128443 -0.000398 "
- "0.016865 0.095885 -0.023523 "
- "-0.023274 0.106158 -0.022428 "
- "-0.064314 0.044528 -0.000908 "
- "-0.058084 0.132712 -0.006348 "
- "-0.040510 0.127658 -0.000962 "
- "-0.039493 0.127549 -0.000886 "
- "0.032698 0.111022 -0.007977 "
- "-0.057988 0.041427 -0.008327 "
- "-0.059581 0.038095 -0.009172 "
- "-0.046740 0.129245 0.000886 "
- "-0.040696 0.125707 -0.008047 "
- "-0.027903 0.124426 0.000511 "
- "0.003607 0.126142 -0.007161 "
- "0.018408 0.127764 0.003915 "
- "-0.065444 0.178190 -0.060631 "
- "-0.064392 0.178295 -0.060775 "
- "-0.042616 0.035917 -0.027130 "
- "0.011575 0.104003 -0.020168 "
- "0.035855 0.084336 -0.017290 "
- "0.057575 0.067384 0.002722 "
- "0.009753 0.120711 -0.014067 "
- "-0.037293 0.039467 -0.029119 "
- "0.039034 0.102562 -0.004541 "
- "-0.028854 0.124656 0.002099 "
- "-0.009828 0.034195 -0.024366 "
- "-0.014578 0.034825 -0.025972 "
- "-0.024541 0.034994 -0.028704 "
- "0.010385 0.036060 -0.022439 "
- "-0.007582 0.036205 -0.025079 "
- "-0.008587 0.036240 -0.025275 "
- "-0.068515 0.033856 -0.004706 "
- "0.008998 0.036490 -0.023427 "
- "-0.015608 0.037809 -0.026925 "
- "-0.070832 0.034582 -0.001037 "
- "-0.071532 0.034754 -0.000134 "
- "-0.029580 0.036889 0.052851 "
- "-0.063931 0.115260 -0.011323 "
- "-0.033262 0.042424 -0.030211 "
- "-0.057517 0.037335 -0.010190 "
- "0.029434 0.038378 -0.001734 "
- "-0.084040 0.132376 -0.001828 "
- "-0.008576 0.043164 -0.026091 "
- "-0.014324 0.103680 -0.023389 "
- "0.037023 0.040523 -0.002041 "
- "0.024475 0.040042 -0.005110 "
- "0.005315 0.042896 -0.024171 "
- "0.019159 0.100268 -0.022296 "
- "0.015149 0.100260 -0.022640 "
- "0.012143 0.100268 -0.022675 "
- "-0.011195 0.178453 -0.029630 "
- "-0.013516 0.162506 -0.015305 "
- "-0.011766 0.182389 -0.028750 "
- "-0.021881 0.107267 -0.022087 "
- "-0.070124 0.109501 0.038232 "
- "-0.056543 0.041557 -0.008718 "
- "-0.071200 0.150790 -0.043462 "
- "-0.063514 0.041263 -0.006393 "
- "-0.064494 0.041204 -0.005990 "
- "0.013531 0.047821 -0.026028 "
- "-0.023856 0.100178 -0.023968 "
- "-0.024856 0.100174 -0.023964 "
- "-0.046567 0.043315 -0.011194 "
- "0.041065 0.050916 -0.007309 "
- "-0.010696 0.176084 -0.029251 "
- "-0.008375 0.045859 -0.029537 "
- "0.028544 0.045965 -0.007084 "
- "0.001825 0.048337 -0.030067 "
- "-0.018756 0.044979 -0.027907 "
- "-0.016257 0.053183 -0.032944 "
- "-0.065583 0.062695 -0.002740 "
- "-0.053491 0.045812 -0.007550 "
- "0.022227 0.051381 -0.024145 "
- "-0.027082 0.048959 -0.025227 "
- "-0.027650 0.050413 -0.024084 "
- "-0.025115 0.163764 -0.015904 "
- "0.047380 0.047547 -0.002962 "
- "0.026329 0.051777 -0.020970 "
- "-0.032809 0.050877 -0.011141 "
- "-0.035189 0.171161 -0.014036 "
- "0.026360 0.053174 -0.020887 "
- "0.008578 0.054156 -0.030513 "
- "-0.052565 0.051568 -0.006607 "
- "-0.007171 0.054903 -0.033099 "
- "-0.010840 0.175678 -0.029032 "
- "-0.025680 0.061671 -0.031107 "
- "-0.067790 0.180834 -0.058549 "
- "0.011146 0.101633 -0.021979 "
- "-0.021147 0.037771 0.053826 "
- "0.036614 0.054238 -0.006713 "
- "-0.040331 0.107690 -0.019920 "
- "-0.042983 0.054604 -0.011188 "
- "-0.034077 0.172586 -0.014513 "
- "0.003429 0.056934 -0.031203 "
- "0.000547 0.063627 -0.034407 "
- "-0.038058 0.154792 -0.009202 "
- "-0.057976 0.145744 -0.001916 "
- "0.048347 0.058938 -0.004908 "
- "0.043359 0.058916 -0.004699 "
- "0.023338 0.060640 -0.025362 "
- "0.021085 0.061931 -0.026580 "
- "-0.044209 0.165146 -0.009112 "
- "0.011063 0.036040 -0.022358 "
- "-0.026875 0.105825 -0.022448 "
- "-0.025640 0.106222 -0.022782 "
- "-0.014088 0.062639 -0.036940 "
- "-0.054028 0.131061 -0.005002 "
- "-0.061313 0.103293 -0.018311 "
- "-0.064291 0.169609 -0.060793 "
- "-0.033408 0.056284 -0.010273 "
- "-0.073436 0.073412 0.034880 "
- "-0.043887 0.108471 -0.019159 "
- "-0.054118 0.157553 -0.002300 "
- "-0.057247 0.060528 -0.003067 "
- "0.026280 0.066086 -0.022657 "
- "0.021876 0.069295 -0.027500 "
- "-0.061063 0.138403 -0.006548 "
- "-0.078029 0.141290 -0.004917 "
- "-0.060035 0.121358 -0.008732 "
- "0.010879 0.066111 -0.031139 "
- "-0.002871 0.068252 -0.034506 "
- "-0.015884 0.037769 0.051576 "
- "0.012128 0.110130 -0.018785 "
- "-0.038213 0.169665 -0.012413 "
- "-0.021532 0.173488 -0.021241 "
- "-0.008517 0.074198 -0.037653 "
- "-0.065608 0.066936 -0.007497 "
- "-0.050280 0.143741 0.000824 "
- "-0.011878 0.107304 -0.022095 "
- "-0.040535 0.159216 -0.011492 "
- "0.028230 0.071783 -0.022893 "
- "-0.021022 0.065418 -0.036655 "
- "0.010141 0.103018 -0.020770 "
- "-0.051433 0.067108 -0.012491 "
- "-0.049160 0.160603 -0.005985 "
- "0.028264 0.073195 -0.022943 "
- "-0.015261 0.076268 -0.038966 "
- "-0.034533 0.064421 -0.014132 "
- "-0.043908 0.067784 -0.016159 "
- "-0.069183 0.153837 -0.049560 "
- "0.024869 0.084767 -0.024560 "
- "-0.000856 0.103081 -0.022665 "
- "-0.001863 0.103074 -0.022753 "
- "-0.059077 0.069973 -0.015104 "
- "-0.012182 0.178673 -0.029235 "
- "-0.012881 0.107288 -0.021846 "
- "-0.047615 0.037865 -0.012539 "
- "-0.067038 0.155239 -0.051934 "
- "-0.041303 0.084378 -0.021267 "
- "0.000115 0.110154 -0.020162 "
- "-0.021633 0.035044 -0.028053 "
- "-0.045200 0.165138 -0.008331 "
- "-0.069516 0.075346 -0.014957 "
- "-0.076657 0.075864 -0.007819 "
- "-0.067246 0.173921 -0.058281 "
- "-0.046064 0.151688 -0.005548 "
- "-0.016173 0.168253 -0.021040 "
- "0.008721 0.073259 -0.033724 "
- "0.005912 0.084442 -0.033198 "
- "-0.026776 0.036519 0.053822 "
- "-0.027674 0.086866 -0.036187 "
- "0.034221 0.081361 -0.018238 "
- "0.030238 0.081488 -0.020170 "
- "0.025200 0.081800 -0.024570 "
- "0.014356 0.076788 -0.030104 "
- "-0.064199 0.163840 -0.059407 "
- "-0.080908 0.111715 -0.000772 "
- "0.014113 0.087083 -0.030146 "
- "-0.074816 0.084473 -0.015099 "
- "-0.074011 0.141343 -0.006888 "
- "-0.076411 0.094841 -0.013406 "
- "-0.077868 0.101964 -0.009289 "
- "-0.068244 0.179461 -0.058921 "
- "-0.002226 0.083034 -0.037089 "
- "-0.022749 0.077591 -0.039002 "
- "-0.055926 0.079634 -0.020972 "
- "-0.065841 0.083605 -0.018912 "
- "-0.064292 0.168172 -0.060588 "
- "-0.075004 0.141334 -0.006491 "
- "-0.015214 0.091053 -0.037504 "
- "-0.054630 0.152755 -0.002679 "
- "0.033230 0.087092 -0.018683 "
- "0.011114 0.110132 -0.019128 "
- "-0.075927 0.125275 -0.008277 "
- "-0.015831 0.088312 -0.038067 "
- "-0.018345 0.033731 -0.026079 "
- "0.004456 0.033966 -0.021007 "
- "-0.003268 0.034370 -0.017376 "
- "0.011950 0.034304 -0.011204 "
- "-0.045950 0.035101 -0.022412 "
- "-0.031721 0.033828 -0.021186 "
- "0.007540 0.038802 -0.009431 "
- "-0.080480 0.114080 0.047569 "
- "-0.004172 0.039154 -0.014111 "
- "-0.032684 0.038589 -0.016529 "
- "-0.051977 0.034100 -0.013438 "
- "0.032543 0.038050 0.000016 "
- "0.033741 0.038472 -0.000065 "
- "-0.039195 0.034033 -0.015253 "
- "-0.073604 0.148628 -0.025599 "
- "0.007509 0.034411 0.004670 "
- "0.025586 0.034909 0.002751 "
- "-0.062356 0.033753 -0.009695 "
- "-0.042372 0.033809 -0.003477 "
- "0.017120 0.034271 0.001954 "
- "-0.059944 0.033627 -0.010153 "
- "-0.038120 0.038250 -0.006977 "
- "-0.004094 0.038391 0.008568 "
- "0.025074 0.034411 0.011174 "
- "0.044118 0.040892 0.004384 "
- "-0.048695 0.033571 -0.013389 "
- "-0.022206 0.038690 -0.017188 "
- "0.002026 0.034165 0.004107 "
- "-0.079652 0.070444 0.015237 "
- "0.048793 0.042901 0.011011 "
- "-0.089979 0.116378 0.044138 "
- "-0.069850 0.033539 0.004727 "
- "-0.029039 0.038221 0.002720 "
- "-0.069196 0.034579 -0.006020 "
- "0.005046 0.034177 0.015760 "
- "0.034111 0.035601 0.011664 "
- "-0.086781 0.115997 0.046920 "
- "0.002619 0.034323 0.019675 "
- "-0.051528 0.033550 0.007814 "
- "-0.034901 0.033747 0.010379 "
- "-0.030015 0.034351 0.018172 "
- "-0.027929 0.037711 0.020035 "
- "-0.072159 0.034912 0.007119 "
- "0.019617 0.034742 0.021397 "
- "-0.071684 0.035136 0.008802 "
- "-0.071483 0.036208 0.009431 "
- "-0.000694 0.038513 0.021743 "
- "0.048576 0.046442 0.024984 "
- "0.022319 0.035282 0.024092 "
- "0.034684 0.038735 0.024611 "
- "-0.070531 0.151779 -0.045373 "
- "-0.066422 0.035267 0.015055 "
- "-0.062077 0.034516 0.020294 "
- "-0.063971 0.034365 0.016456 "
- "-0.081219 0.078592 0.029019 "
- "-0.051600 0.066088 0.036174 "
- "0.031905 0.043740 0.029538 "
- "-0.063341 0.035770 0.018750 "
- "-0.004483 0.039218 0.035614 "
- "-0.082655 0.076999 0.021106 "
- "-0.055121 0.034056 0.024036 "
- "0.009041 0.035141 0.035926 "
- "0.012972 0.034672 0.029418 "
- "-0.070622 0.146831 -0.025571 "
- "-0.090089 0.112403 0.037453 "
- "-0.066997 0.062934 -0.001956 "
- "-0.035388 0.034596 0.034245 "
- "0.007859 0.039240 0.030372 "
- "-0.071388 0.147097 -0.026896 "
- "-0.078147 0.116431 0.050321 "
- "-0.070936 0.078069 0.038683 "
- "-0.075074 0.077317 0.034901 "
- "-0.016213 0.038057 0.016945 "
- "-0.006919 0.034711 0.042546 "
- "-0.050487 0.064658 0.035198 "
- "-0.064600 0.034724 0.026530 "
- "-0.031802 0.037839 0.032183 "
- "0.004891 0.034575 0.043617 "
- "-0.084324 0.077527 0.016962 "
- "-0.091292 0.116401 0.042037 "
- "-0.069946 0.064187 0.001256 "
- "-0.073267 0.148476 -0.029258 "
- "-0.084864 0.078984 0.019130 "
- "-0.069386 0.061033 0.009766 "
- "-0.036723 0.034210 0.026670 "
- "-0.024903 0.039047 0.038063 "
- "0.014685 0.037467 0.044223 "
- "-0.079247 0.080308 0.033239 "
- "-0.082419 0.078886 0.026767 "
- "-0.091014 0.114490 0.034742 "
- "-0.066996 0.034958 0.031010 "
- "-0.067902 0.146313 -0.025723 "
- "-0.053202 0.073352 0.041234 "
- "-0.037419 0.034781 0.041799 "
- "-0.012835 0.035252 0.050222 "
- "0.009197 0.039194 0.045161 "
- "-0.082296 0.077332 0.022849 "
- "-0.061775 0.065542 0.032955 "
- "-0.074682 0.066385 0.006255 "
- "-0.051757 0.056601 0.019874 "
- "-0.046877 0.035027 0.045407 "
- "-0.070681 0.109318 0.037859 "
- "-0.075092 0.066454 0.007232 "
- "-0.074437 0.065821 0.007911 "
- "0.004325 0.035606 0.045888 "
- "-0.089199 0.115860 0.044703 "
- "-0.066365 0.036009 0.037225 "
- "-0.038999 0.036125 0.042901 "
- "-0.072850 0.107163 0.037442 "
- "-0.076428 0.117143 0.051891 "
- "-0.079737 0.071741 0.005181 "
- "-0.084462 0.111705 0.028857 "
- "-0.060273 0.034788 0.045444 "
- "-0.030763 0.035297 0.052008 "
- "-0.082857 0.076402 0.018249 "
- "-0.074827 0.110670 0.046334 "
- "-0.079080 0.070406 0.008268 "
- "-0.076864 0.067957 0.011082 "
- "-0.069649 0.061738 0.018026 "
- "-0.058329 0.059742 0.021921 "
- "-0.075934 0.078899 0.035147 "
- "-0.065150 0.036692 0.040216 "
- "-0.069152 0.078835 0.040145 "
- "-0.035996 0.151076 -0.002026 "
- "-0.063069 0.150547 -0.026594 "
- "-0.069062 0.149033 -0.037134 "
- "-0.062215 0.166391 -0.060509 "
- "-0.062798 0.167240 -0.060784 "
- "-0.039381 0.154717 -0.008957 "
- "-0.049932 0.141822 0.003858 "
- "-0.064944 0.157314 -0.054480 "
- "-0.069406 0.153071 -0.048274 "
- "-0.044493 0.149423 -0.004750 "
- "-0.056918 0.136838 -0.005280 "
- "-0.064125 0.156393 -0.043120 "
- "-0.071932 0.149607 -0.039423 "
- "-0.009322 0.175020 -0.028678 "
- "-0.060545 0.169731 -0.062221 "
- "-0.068381 0.149044 -0.036363 "
- "-0.050757 0.137975 0.001565 "
- "-0.027652 0.155350 -0.003855 "
- "-0.027938 0.155215 -0.003967 "
- "-0.026306 0.157004 -0.009894 "
- "-0.028940 0.154865 -0.002858 "
- "-0.009716 0.176357 -0.030374 "
- "-0.008588 0.169530 -0.025410 "
- "-0.067979 0.154919 -0.051359 "
- "-0.018495 0.165075 -0.017689 "
- "-0.013589 0.169514 -0.023467 "
- "-0.009085 0.173025 -0.027717 "
- "-0.066223 0.155560 -0.052081 "
- "-0.029333 0.154386 -0.003606 "
- "-0.063933 0.153712 -0.036871 "
- "-0.028864 0.154571 -0.003558 "
- "-0.064135 0.160881 -0.057617 "
- "-0.072553 0.150608 -0.041367 "
- "-0.030385 0.154168 -0.002474 "
- "-0.009715 0.177216 -0.029461 "
- "-0.014691 0.160456 -0.011333 "
- "-0.063650 0.159498 -0.051954 "
- "-0.072177 0.146332 -0.024671 "
- "-0.020377 0.158233 -0.007259 "
- "-0.062349 0.141045 -0.006499 "
- "-0.064196 0.142314 -0.007840 "
- "-0.068829 0.148429 -0.034137 "
- "-0.067194 0.151677 -0.045577 "
- "-0.071411 0.144356 -0.015185 "
- "-0.009825 0.166960 -0.022046 "
- "-0.052896 0.143026 -0.000456 "
- "-0.032692 0.152804 -0.002950 "
- "-0.072038 0.149669 -0.039594 "
- "-0.072282 0.143895 -0.012909 ";
-
-
-bool samePoints(const vpPoint &pt1, const vpPoint &pt2) {
+// Stanford Bunny Model points
+const std::string file_content = "0.038043 0.109755 0.016169 "
+ "-0.080459 0.135827 0.050032 "
+ "-0.080437 0.137229 0.049095 "
+ "-0.039537 0.165252 0.003107 "
+ "-0.029517 0.175738 -0.005254 "
+ "-0.076590 0.172837 -0.038783 "
+ "0.037067 0.109719 0.023178 "
+ "0.002482 0.038559 0.046163 "
+ "-0.071500 0.163630 -0.013838 "
+ "-0.063129 0.154030 -0.010769 "
+ "0.038061 0.108343 0.022172 "
+ "-0.038632 0.167867 0.002788 "
+ "0.001485 0.039988 0.046431 "
+ "0.002487 0.039963 0.046169 "
+ "-0.076694 0.171427 -0.035893 "
+ "-0.037523 0.124042 0.025944 "
+ "-0.057469 0.041348 0.046193 "
+ "-0.044485 0.041114 0.043653 "
+ "-0.043493 0.041078 0.043361 "
+ "-0.062047 0.176838 -0.060831 "
+ "-0.022629 0.157855 -0.004227 "
+ "-0.058497 0.042632 0.044737 "
+ "-0.043500 0.042437 0.043101 "
+ "-0.038476 0.042327 0.041891 "
+ "-0.021510 0.043531 0.053479 "
+ "-0.001517 0.042851 0.046801 "
+ "-0.035500 0.174151 -0.001806 "
+ "0.043233 0.089594 -0.006456 "
+ "-0.058494 0.043959 0.043965 "
+ "-0.052515 0.043997 0.044898 "
+ "-0.021503 0.044884 0.053016 "
+ "-0.019514 0.044832 0.052531 "
+ "-0.026748 0.109750 0.038619 "
+ "-0.080524 0.148418 0.038173 "
+ "-0.041486 0.169687 0.001623 "
+ "-0.063449 0.097112 -0.017938 "
+ "-0.054487 0.045217 0.043831 "
+ "-0.053511 0.045215 0.043764 "
+ "-0.037512 0.045091 0.040752 "
+ "-0.088156 0.151596 0.022886 "
+ "-0.025891 0.048891 0.049042 "
+ "-0.071506 0.148353 0.040851 "
+ "0.027212 0.088765 -0.022232 "
+ "-0.054494 0.046553 0.042265 "
+ "-0.058656 0.152453 0.032611 "
+ "-0.070511 0.148356 0.040726 "
+ "-0.023471 0.183379 -0.010712 "
+ "-0.072524 0.137240 0.048963 "
+ "0.039501 0.046973 0.031597 "
+ "0.045346 0.082603 0.023076 "
+ "-0.061964 0.147832 -0.004318 "
+ "-0.034505 0.049198 0.039001 "
+ "-0.027492 0.049767 0.045472 "
+ "-0.078513 0.145668 0.042746 "
+ "-0.046493 0.165223 0.004593 "
+ "0.032495 0.048497 0.032163 "
+ "0.041518 0.048439 0.032146 "
+ "-0.064113 0.155183 -0.040605 "
+ "-0.044898 0.168579 0.001542 "
+ "-0.051381 0.050043 0.034731 "
+ "-0.032509 0.050660 0.038421 "
+ "0.006619 0.047417 0.050356 "
+ "-0.015062 0.160937 -0.011420 "
+ "-0.080526 0.149910 0.035970 "
+ "-0.040710 0.050423 0.039629 "
+ "-0.031463 0.051903 0.037377 "
+ "-0.006309 0.051457 0.052597 "
+ "-0.056444 0.115317 0.036137 "
+ "-0.063698 0.164648 -0.028601 "
+ "0.034501 0.051352 0.032162 "
+ "-0.083451 0.149763 0.033502 "
+ "-0.064494 0.149793 0.037510 "
+ "-0.063485 0.149796 0.037152 "
+ "0.031455 0.113206 0.029881 "
+ "-0.045515 0.053319 0.038154 "
+ "0.010509 0.054658 0.052359 "
+ "-0.057480 0.115365 0.036701 "
+ "0.037597 0.088359 -0.014764 "
+ "0.035531 0.052720 0.031990 "
+ "-0.064662 0.151974 0.036264 "
+ "-0.018605 0.049922 0.047107 "
+ "-0.000094 0.052014 0.054278 "
+ "-0.075359 0.113961 -0.005684 "
+ "0.036518 0.054117 0.031704 "
+ "-0.013502 0.057389 0.051750 "
+ "0.009410 0.060149 0.052895 "
+ "-0.018488 0.122208 0.030928 "
+ "0.038169 0.109765 0.014166 "
+ "-0.042948 0.170625 -0.000616 "
+ "-0.053583 0.055075 0.011440 "
+ "-0.017474 0.058729 0.050990 "
+ "-0.013551 0.058830 0.052544 "
+ "0.000322 0.058135 0.054484 "
+ "0.008500 0.058924 0.053176 "
+ "-0.069486 0.116115 0.051835 "
+ "-0.052433 0.115217 0.034103 "
+ "-0.085941 0.125175 0.049068 "
+ "-0.072600 0.145757 -0.016854 "
+ "-0.021451 0.184864 -0.013038 "
+ "-0.027746 0.064856 0.037498 "
+ "-0.017457 0.060172 0.051453 "
+ "-0.014477 0.060305 0.052805 "
+ "-0.013472 0.060385 0.053624 "
+ "0.010494 0.118230 0.036879 "
+ "-0.025456 0.120601 0.028847 "
+ "-0.024489 0.120706 0.029404 "
+ "-0.051475 0.115278 0.033811 "
+ "-0.023488 0.122057 0.027634 "
+ "0.038511 0.058329 0.031423 "
+ "-0.076502 0.121776 0.052772 "
+ "-0.014480 0.061748 0.053510 "
+ "0.003858 0.062044 0.056777 "
+ "0.011472 0.118200 0.036572 "
+ "-0.077835 0.145715 0.043808 "
+ "0.039532 0.059677 0.030697 "
+ "-0.054472 0.060781 0.026950 "
+ "-0.048101 0.135891 0.016805 "
+ "-0.014501 0.063146 0.053679 "
+ "-0.005881 0.062411 0.056478 "
+ "-0.062841 0.144409 -0.007635 "
+ "-0.020619 0.186125 -0.015175 "
+ "-0.057485 0.062333 0.028342 "
+ "-0.047781 0.063839 0.037342 "
+ "-0.069510 0.148387 0.040505 "
+ "0.011484 0.123198 0.034270 "
+ "-0.009507 0.125161 0.031090 "
+ "-0.062211 0.147546 -0.005052 "
+ "-0.053516 0.064085 0.033078 "
+ "-0.026557 0.064650 0.039328 "
+ "0.046038 0.067997 0.025283 "
+ "0.009517 0.066006 0.054677 "
+ "0.011502 0.119616 0.035894 "
+ "-0.047437 0.120726 0.028345 "
+ "-0.011482 0.125053 0.030655 "
+ "0.051514 0.063765 0.028901 "
+ "-0.044486 0.066153 0.040137 "
+ "-0.012589 0.069082 0.054415 "
+ "0.010512 0.067367 0.054298 "
+ "-0.048480 0.117974 0.031095 "
+ "-0.049455 0.118007 0.031644 "
+ "-0.091160 0.146144 0.025161 "
+ "0.042563 0.065071 0.027562 "
+ "-0.005859 0.087537 -0.037176 "
+ "-0.044518 0.067563 0.040463 "
+ "0.000030 0.068579 0.056492 "
+ "0.010524 0.068777 0.054575 "
+ "-0.047469 0.117985 0.030630 "
+ "-0.081528 0.148400 0.037410 "
+ "-0.029791 0.154227 -0.004672 "
+ "-0.016743 0.123139 0.030848 "
+ "0.053516 0.066419 0.026864 "
+ "-0.071191 0.155859 0.001279 "
+ "-0.036068 0.156277 -0.010490 "
+ "0.042534 0.067858 0.027368 "
+ "0.044646 0.069351 0.024242 "
+ "0.047498 0.067780 0.026220 "
+ "-0.047725 0.135673 0.014006 "
+ "-0.065141 0.159482 -0.056501 "
+ "-0.046491 0.070446 0.041037 "
+ "-0.045986 0.073503 0.042256 "
+ "-0.038661 0.063179 0.041795 "
+ "-0.003043 0.073778 0.059461 "
+ "0.009520 0.071626 0.055408 "
+ "0.036826 0.109713 0.024182 "
+ "0.043569 0.069164 0.025997 "
+ "0.049500 0.069124 0.025332 "
+ "0.051511 0.069144 0.025235 "
+ "-0.069372 0.180628 -0.057909 "
+ "-0.072540 0.162229 -0.011353 "
+ "-0.020454 0.184888 -0.013987 "
+ "-0.048492 0.071808 0.040533 "
+ "-0.009124 0.125328 0.030976 "
+ "-0.017522 0.187118 -0.019415 "
+ "-0.018462 0.072978 0.054580 "
+ "-0.080508 0.121673 0.049920 "
+ "0.043584 0.070558 0.025859 "
+ "-0.011119 0.163582 -0.018751 "
+ "-0.019434 0.184905 -0.015328 "
+ "-0.027457 0.073402 0.042686 "
+ "-0.031497 0.117989 0.030856 "
+ "-0.019943 0.187176 -0.015950 "
+ "-0.091048 0.113254 0.017639 "
+ "-0.089443 0.151569 0.016470 "
+ "0.006504 0.123782 0.033960 "
+ "-0.072537 0.138625 0.048318 "
+ "-0.000692 0.100255 0.047827 "
+ "-0.076123 0.163992 -0.018367 "
+ "-0.008085 0.125292 0.031359 "
+ "-0.020456 0.077205 0.054931 "
+ "-0.011270 0.120275 0.036868 "
+ "-0.068487 0.120374 0.052788 "
+ "-0.036504 0.168234 0.000586 "
+ "-0.046713 0.131987 0.005569 "
+ "-0.064501 0.077478 0.041325 "
+ "-0.029667 0.077446 0.041883 "
+ "0.009089 0.080660 0.054966 "
+ "-0.079510 0.149797 0.036758 "
+ "-0.017682 0.082305 0.057622 "
+ "-0.071505 0.147007 0.042162 "
+ "-0.082501 0.107588 0.027351 "
+ "-0.078078 0.154685 0.029078 "
+ "-0.031275 0.118165 0.030639 "
+ "-0.050393 0.085761 0.046102 "
+ "-0.053471 0.115278 0.034303 "
+ "0.007481 0.115492 0.039253 "
+ "-0.075260 0.127230 0.053502 "
+ "-0.042039 0.084825 0.042519 "
+ "-0.007645 0.079109 0.058188 "
+ "0.018475 0.082463 0.051596 "
+ "-0.074281 0.117286 0.053631 "
+ "-0.065353 0.085654 0.044968 "
+ "-0.059945 0.074585 0.041788 "
+ "-0.037174 0.081950 0.043756 "
+ "-0.068377 0.176522 -0.058018 "
+ "0.017494 0.083857 0.051403 "
+ "0.018494 0.083833 0.051046 "
+ "0.019476 0.083796 0.050749 "
+ "-0.047453 0.119349 0.029360 "
+ "-0.003971 0.124981 0.033251 "
+ "-0.025836 0.094514 -0.026615 "
+ "0.017485 0.085207 0.050786 "
+ "0.020474 0.085138 0.050012 "
+ "-0.074496 0.130260 0.052409 "
+ "-0.026922 0.181455 -0.007884 "
+ "-0.032593 0.177828 -0.005142 "
+ "-0.030548 0.087232 0.043284 "
+ "-0.020478 0.086949 0.055949 "
+ "-0.012103 0.092198 0.056667 "
+ "0.008495 0.086943 0.055839 "
+ "-0.092808 0.120209 0.041284 "
+ "0.021487 0.086500 0.049224 "
+ "-0.023441 0.181825 -0.010570 "
+ "-0.036983 0.092657 0.044193 "
+ "0.005296 0.086140 0.057126 "
+ "0.009509 0.088290 0.055046 "
+ "0.018490 0.087902 0.049248 "
+ "0.022460 0.087857 0.048776 "
+ "0.037946 0.109770 0.017170 "
+ "-0.058473 0.115363 0.036971 "
+ "0.040458 0.097140 0.028174 "
+ "0.039909 0.107011 0.006164 "
+ "-0.071514 0.088744 0.041619 "
+ "0.011490 0.089612 0.054036 "
+ "-0.010050 0.166619 -0.017705 "
+ "0.019647 0.086817 0.049574 "
+ "0.022481 0.089252 0.048474 "
+ "-0.018485 0.091067 0.055002 "
+ "-0.038692 0.115422 0.033305 "
+ "-0.069491 0.124597 0.052657 "
+ "-0.004492 0.123763 0.034322 "
+ "-0.070533 0.144185 0.044275 "
+ "-0.070146 0.089763 0.042620 "
+ "0.012498 0.092313 0.052383 "
+ "-0.018064 0.122407 0.030816 "
+ "-0.018232 0.187056 -0.018294 "
+ "-0.072494 0.092871 0.041339 "
+ "0.017475 0.093454 0.048154 "
+ "-0.018226 0.117876 0.036486 "
+ "-0.059685 0.147446 0.036516 "
+ "-0.071526 0.094419 0.041653 "
+ "-0.050053 0.096180 0.044423 "
+ "0.013460 0.094917 0.050136 "
+ "-0.072540 0.095957 0.041270 "
+ "-0.056884 0.092762 0.045348 "
+ "-0.040294 0.098952 0.041786 "
+ "-0.024503 0.096017 0.044275 "
+ "-0.014473 0.096429 0.052209 "
+ "-0.013468 0.096423 0.052316 "
+ "0.022488 0.096164 0.046634 "
+ "-0.044495 0.120650 0.027643 "
+ "-0.073439 0.148627 -0.029805 "
+ "0.006439 0.132232 0.014596 "
+ "-0.080457 0.138622 0.048117 "
+ "-0.027509 0.181505 -0.008742 "
+ "-0.071519 0.097212 0.040997 "
+ "-0.053895 0.097200 0.043345 "
+ "-0.024482 0.097451 0.044460 "
+ "-0.023481 0.097449 0.044821 "
+ "-0.009483 0.097835 0.051868 "
+ "-0.002497 0.097829 0.052428 "
+ "-0.000511 0.097839 0.052433 "
+ "0.001458 0.097737 0.051907 "
+ "-0.020496 0.098841 0.044481 "
+ "-0.013463 0.098956 0.047529 "
+ "0.023359 0.095899 0.046777 "
+ "0.020477 0.098952 0.046347 "
+ "-0.011316 0.129221 0.002910 "
+ "-0.079478 0.142827 0.045105 "
+ "-0.046949 0.093514 -0.021920 "
+ "-0.070498 0.099990 0.040323 "
+ "-0.059835 0.102266 0.042689 "
+ "-0.029833 0.101135 0.043080 "
+ "-0.019498 0.100240 0.044034 "
+ "-0.016507 0.100263 0.043963 "
+ "-0.015502 0.100285 0.044225 "
+ "-0.012572 0.100502 0.044597 "
+ "0.001493 0.100277 0.046986 "
+ "0.011888 0.102476 0.047452 "
+ "0.019476 0.100345 0.046212 "
+ "-0.046466 0.167133 0.003131 "
+ "-0.070478 0.114632 0.051042 "
+ "-0.065862 0.153866 -0.001908 "
+ "-0.016492 0.101629 0.043954 "
+ "-0.033748 0.119587 0.029726 "
+ "-0.001496 0.101631 0.044188 "
+ "-0.074502 0.116130 0.052068 "
+ "-0.024607 0.157782 -0.002504 "
+ "-0.074558 0.102629 0.036985 "
+ "-0.044387 0.106839 0.040714 "
+ "-0.018030 0.106099 0.042558 "
+ "-0.014500 0.103011 0.043250 "
+ "-0.012495 0.103038 0.043735 "
+ "-0.006500 0.103045 0.043680 "
+ "-0.001500 0.103045 0.044056 "
+ "0.035089 0.111006 -0.001837 "
+ "-0.068495 0.121776 0.052768 "
+ "-0.046574 0.153815 0.009349 "
+ "-0.074536 0.104026 0.036830 "
+ "-0.047007 0.099425 0.042698 "
+ "-0.001513 0.104469 0.044133 "
+ "-0.042032 0.171559 -0.001711 "
+ "-0.008282 0.101652 0.043830 "
+ "-0.006486 0.108283 0.043970 "
+ "-0.001528 0.105863 0.043996 "
+ "0.024524 0.105615 0.039234 "
+ "0.021424 0.121505 0.031182 "
+ "0.017255 0.109608 0.039746 "
+ "-0.035520 0.169730 -0.000115 "
+ "0.024505 0.108393 0.038558 "
+ "-0.029469 0.178820 -0.005543 "
+ "-0.000500 0.100727 0.046395 "
+ "-0.069489 0.109539 0.038095 "
+ "-0.068505 0.109678 0.037959 "
+ "-0.066496 0.109745 0.037770 "
+ "0.005485 0.109984 0.041677 "
+ "0.023135 0.125159 0.021213 "
+ "-0.040910 0.173193 -0.005611 "
+ "-0.032536 0.172727 -0.002702 "
+ "0.006478 0.111357 0.041027 "
+ "0.022491 0.111137 0.037371 "
+ "-0.050136 0.110464 0.037566 "
+ "-0.019238 0.036205 0.052830 "
+ "-0.005178 0.036734 0.048450 "
+ "-0.004153 0.036789 0.048168 "
+ "-0.031089 0.159271 -0.013062 "
+ "0.017745 0.037928 0.042910 "
+ "-0.073907 0.171564 -0.047586 "
+ "-0.018941 0.037618 0.053049 "
+ "0.007356 0.038818 0.045452 "
+ "0.018182 0.036268 0.042512 "
+ "-0.053475 0.036001 0.046489 "
+ "-0.050165 0.036131 0.046172 "
+ "-0.043194 0.036383 0.044045 "
+ "0.022179 0.040783 0.040543 "
+ "0.018183 0.102054 0.045770 "
+ "0.026779 0.040277 0.031351 "
+ "-0.054330 0.097111 -0.022408 "
+ "-0.000216 0.041170 0.046762 "
+ "0.000822 0.041223 0.046540 "
+ "0.025055 0.042091 0.037664 "
+ "0.026269 0.039553 0.030992 "
+ "-0.029066 0.093469 -0.024545 "
+ "-0.039288 0.044575 0.041301 "
+ "-0.006058 0.045881 0.047354 "
+ "-0.066457 0.110304 0.037295 "
+ "0.011942 0.123166 0.033243 "
+ "-0.022142 0.043052 0.053661 "
+ "-0.021404 0.045562 0.053215 "
+ "-0.086889 0.102318 0.023370 "
+ "-0.015871 0.161111 -0.009045 "
+ "0.017650 0.043039 0.044185 "
+ "0.041315 0.045195 0.029602 "
+ "-0.040635 0.159922 0.002471 "
+ "-0.020287 0.044518 0.052893 "
+ "-0.012904 0.181603 -0.024044 "
+ "0.011322 0.045022 0.044806 "
+ "0.040105 0.046687 0.031387 "
+ "0.043756 0.046763 0.029636 "
+ "0.049174 0.044307 0.021629 "
+ "0.050879 0.046558 0.022316 "
+ "0.051380 0.046479 0.021262 "
+ "-0.054311 0.149562 0.026707 "
+ "-0.047190 0.156504 0.008879 "
+ "-0.018702 0.045934 0.051674 "
+ "-0.055378 0.151032 0.028750 "
+ "-0.013113 0.184484 -0.026054 "
+ "0.028021 0.046282 0.036458 "
+ "0.017700 0.103348 0.045066 "
+ "-0.067150 0.145376 0.041641 "
+ "-0.032860 0.153633 -0.000344 "
+ "0.009909 0.048971 0.049252 "
+ "0.010676 0.048959 0.048584 "
+ "0.033645 0.049093 0.031488 "
+ "0.043084 0.049674 0.032233 "
+ "0.054174 0.049532 0.021869 "
+ "0.036990 0.111103 0.004167 "
+ "-0.011129 0.182158 -0.027227 "
+ "-0.023928 0.181600 -0.010098 "
+ "0.055409 0.050963 0.021892 "
+ "-0.054071 0.123768 0.038080 "
+ "-0.016353 0.160230 -0.010916 "
+ "0.015237 0.121915 0.032797 "
+ "-0.071652 0.139715 0.047438 "
+ "0.020741 0.055550 0.048100 "
+ "0.023003 0.049323 0.039928 "
+ "-0.023553 0.156826 -0.005180 "
+ "-0.049505 0.050280 0.036331 "
+ "-0.029135 0.177171 -0.005342 "
+ "0.035495 0.103128 -0.011159 "
+ "0.059989 0.062227 0.021516 "
+ "0.012111 0.116449 0.037015 "
+ "-0.054345 0.151005 0.026686 "
+ "0.013667 0.055693 0.050689 "
+ "0.043020 0.049120 0.032996 "
+ "0.053744 0.054858 0.027754 "
+ "-0.062742 0.148221 0.037477 "
+ "-0.049178 0.130401 0.030288 "
+ "0.039228 0.108354 0.009166 "
+ "0.010869 0.057643 0.052478 "
+ "0.058756 0.057992 0.021536 "
+ "-0.055127 0.047834 0.040050 "
+ "-0.072139 0.128420 0.051978 "
+ "-0.067933 0.145349 0.042265 "
+ "0.051151 0.060858 0.029927 "
+ "0.039303 0.101950 0.027071 "
+ "-0.028849 0.162315 -0.003816 "
+ "-0.071123 0.141136 0.046452 "
+ "-0.036101 0.158265 0.004249 "
+ "-0.069556 0.114278 0.050291 "
+ "-0.089109 0.099057 0.008227 "
+ "0.026136 0.060229 0.044245 "
+ "0.040315 0.061632 0.030023 "
+ "-0.070208 0.141157 0.046016 "
+ "-0.015086 0.162585 -0.009690 "
+ "-0.026913 0.180109 -0.007400 "
+ "-0.001352 0.101336 0.044402 "
+ "0.006923 0.063037 0.055588 "
+ "0.007933 0.063074 0.055296 "
+ "0.034320 0.060597 0.039225 "
+ "0.035670 0.108162 -0.004821 "
+ "0.019494 0.107309 0.040648 "
+ "0.038763 0.104620 0.026268 "
+ "-0.068821 0.145351 0.042741 "
+ "-0.053805 0.048637 0.038267 "
+ "-0.014344 0.162546 -0.010432 "
+ "0.039538 0.100622 0.027388 "
+ "-0.013982 0.178957 -0.021269 "
+ "0.008946 0.064444 0.055029 "
+ "0.012129 0.065567 0.053747 "
+ "0.020511 0.065463 0.048451 "
+ "0.052356 0.064799 0.028141 "
+ "0.053298 0.064822 0.027758 "
+ "-0.052653 0.152155 0.015019 "
+ "0.036409 0.085446 -0.016706 "
+ "0.028693 0.111728 0.034550 "
+ "0.002438 0.114145 0.041742 "
+ "-0.030297 0.121635 -0.007023 "
+ "0.008263 0.110977 0.040210 "
+ "0.037500 0.107296 0.026215 "
+ "0.039845 0.059964 0.030392 "
+ "-0.063695 0.167813 -0.041594 "
+ "0.051687 0.064527 0.029555 "
+ "0.027852 0.115393 0.031660 "
+ "0.009143 0.112364 0.039751 "
+ "0.017991 0.120614 0.033226 "
+ "-0.069120 0.118603 0.052746 "
+ "0.039512 0.070945 0.033551 "
+ "0.056979 0.068693 0.022604 "
+ "0.057617 0.068678 0.021770 "
+ "0.058779 0.068629 0.019916 "
+ "0.016910 0.120624 0.033430 "
+ "-0.049644 0.132456 0.028190 "
+ "-0.069785 0.145343 0.043102 "
+ "-0.010211 0.168384 -0.018420 "
+ "-0.010519 0.165484 -0.017015 "
+ "-0.013977 0.164028 -0.011589 "
+ "0.007651 0.075327 0.056504 "
+ "-0.007476 0.101243 0.044380 "
+ "-0.062397 0.162983 -0.056654 "
+ "-0.010634 0.174261 -0.022600 "
+ "0.029870 0.114081 0.031047 "
+ "0.026908 0.070176 0.043296 "
+ "0.032967 0.077598 0.041811 "
+ "0.040252 0.102372 0.026220 "
+ "-0.060796 0.125745 0.042799 "
+ "-0.002700 0.109146 -0.021617 "
+ "-0.053082 0.139518 0.026662 "
+ "-0.072831 0.136891 0.049250 "
+ "-0.022318 0.181221 -0.012616 "
+ "-0.062587 0.137282 0.036031 "
+ "0.036468 0.111067 0.002168 "
+ "0.022493 0.120697 0.030928 "
+ "-0.068515 0.132239 0.047795 "
+ "0.015592 0.078141 0.054165 "
+ "-0.044768 0.146394 0.003586 "
+ "0.004576 0.108164 0.041888 "
+ "0.042099 0.093008 0.027170 "
+ "-0.063799 0.143909 0.038920 "
+ "-0.065799 0.120921 0.051999 "
+ "-0.061867 0.112377 0.036730 "
+ "-0.033558 0.173613 -0.001108 "
+ "0.010985 0.123169 0.033628 "
+ "-0.069391 0.117164 0.052337 "
+ "-0.049240 0.118066 0.031545 "
+ "0.045144 0.080268 0.023028 "
+ "0.045515 0.080198 0.021690 "
+ "-0.009929 0.166943 -0.018055 "
+ "0.033894 0.111352 0.028046 "
+ "-0.068718 0.114088 0.050433 "
+ "-0.062044 0.114243 0.037354 "
+ "0.027865 0.103405 0.039124 "
+ "-0.055094 0.114335 0.035233 "
+ "0.026730 0.079214 0.047114 "
+ "-0.046699 0.122484 0.027073 "
+ "-0.009970 0.167686 -0.017702 "
+ "0.043315 0.083190 0.027513 "
+ "0.022758 0.094213 0.047144 "
+ "-0.045635 0.123874 0.025084 "
+ "0.020065 0.084847 0.050222 "
+ "0.036554 0.108655 0.026577 "
+ "-0.033646 0.165935 -0.004786 "
+ "0.020807 0.086180 0.049534 "
+ "0.004625 0.109546 0.041960 "
+ "-0.069930 0.141590 0.046209 "
+ "0.021629 0.087551 0.049039 "
+ "0.039804 0.089822 0.033495 "
+ "-0.048053 0.150190 -0.004201 "
+ "-0.024682 0.159952 -0.001088 "
+ "-0.050727 0.115538 0.033580 "
+ "0.009088 0.097645 0.049418 "
+ "-0.024774 0.180145 -0.009568 "
+ "-0.061113 0.149657 0.036321 "
+ "-0.017561 0.186044 -0.018449 "
+ "-0.045252 0.122806 0.025016 "
+ "-0.058955 0.151077 0.034187 "
+ "-0.010889 0.108738 -0.021743 "
+ "-0.073382 0.135448 0.050183 "
+ "-0.048532 0.143887 0.006514 "
+ "0.014126 0.090787 0.052728 "
+ "-0.054919 0.148123 0.027541 "
+ "-0.073651 0.180693 -0.054018 "
+ "0.035433 0.111001 -0.000827 "
+ "-0.074141 0.105214 0.036938 "
+ "0.034274 0.091666 0.040805 "
+ "-0.026146 0.180134 -0.008033 "
+ "0.041527 0.097216 -0.003903 "
+ "0.039787 0.104095 -0.000790 "
+ "-0.010438 0.171321 -0.020552 "
+ "0.040891 0.094256 -0.006770 "
+ "-0.084037 0.145378 0.004810 "
+ "0.019655 0.035085 -0.003456 "
+ "0.017978 0.035483 -0.012673 "
+ "0.017351 0.035184 -0.014686 "
+ "0.016671 0.035381 -0.015659 "
+ "0.022996 0.037585 0.033289 "
+ "-0.089659 0.099833 0.017847 "
+ "0.039376 0.039372 0.002323 "
+ "0.016325 0.035201 -0.018314 "
+ "0.024913 0.039060 0.032333 "
+ "0.045899 0.041406 0.011234 "
+ "0.045451 0.041238 0.010235 "
+ "0.036964 0.040555 -0.001588 "
+ "0.014050 0.037936 -0.021724 "
+ "0.023136 0.040456 0.039246 "
+ "-0.062395 0.163038 -0.054588 "
+ "0.048458 0.043018 0.014220 "
+ "0.041031 0.042290 -0.000688 "
+ "0.020591 0.043659 -0.020978 "
+ "-0.058686 0.060316 0.023126 "
+ "-0.009223 0.174168 -0.028567 "
+ "0.024695 0.042022 0.038270 "
+ "-0.062676 0.177191 -0.057627 "
+ "0.050252 0.044634 0.018204 "
+ "0.025338 0.041873 -0.005885 "
+ "0.022728 0.039972 -0.005642 "
+ "0.017317 0.041198 -0.021676 "
+ "0.040137 0.095364 -0.006888 "
+ "-0.086614 0.111799 0.023338 "
+ "-0.062457 0.177245 -0.058615 "
+ "0.047024 0.045876 -0.000609 "
+ "-0.062908 0.150560 -0.025587 "
+ "0.052608 0.047598 0.021222 "
+ "0.056546 0.049738 0.018075 "
+ "0.047681 0.047268 -0.001643 "
+ "0.022565 0.045669 -0.018540 "
+ "0.018420 0.044214 -0.022632 "
+ "-0.009629 0.168316 -0.023669 "
+ "-0.014554 0.162843 -0.010082 "
+ "0.047073 0.048551 -0.003646 "
+ "0.031126 0.046970 -0.006535 "
+ "0.028058 0.049173 -0.018234 "
+ "0.019626 0.045759 -0.021723 "
+ "0.018445 0.045652 -0.022673 "
+ "-0.013671 0.162609 -0.015656 "
+ "-0.064753 0.156678 -0.047594 "
+ "0.048231 0.050030 -0.003699 "
+ "-0.061903 0.152590 -0.029073 "
+ "-0.064629 0.156694 -0.046598 "
+ "-0.060478 0.144516 -0.003635 "
+ "0.057159 0.050804 0.007605 "
+ "0.053557 0.058091 -0.003559 "
+ "0.050638 0.051672 -0.002735 "
+ "-0.051929 0.136980 0.026387 "
+ "-0.064488 0.156696 -0.045603 "
+ "-0.036286 0.151198 0.000402 "
+ "0.032998 0.051431 -0.007635 "
+ "-0.063640 0.163113 -0.024591 "
+ "-0.048075 0.134034 0.005399 "
+ "-0.053174 0.150869 0.021398 "
+ "-0.053869 0.152421 0.021400 "
+ "0.061282 0.060979 0.008052 "
+ "-0.049294 0.133961 0.001427 "
+ "0.038369 0.088421 -0.013773 "
+ "0.054270 0.057579 -0.001787 "
+ "0.038928 0.064147 -0.010753 "
+ "0.030796 0.048804 -0.007779 "
+ "0.026392 0.054968 -0.020802 "
+ "-0.050474 0.137043 0.022400 "
+ "0.038624 0.089851 -0.012769 "
+ "0.054545 0.058989 -0.001818 "
+ "0.038123 0.057470 -0.005915 "
+ "-0.025394 0.106122 -0.022387 "
+ "0.055301 0.060464 -0.000843 "
+ "-0.088206 0.135559 0.003366 "
+ "0.039239 0.058959 -0.005124 "
+ "-0.015832 0.184840 -0.020727 "
+ "-0.063934 0.169417 -0.044580 "
+ "-0.061679 0.175513 -0.061381 "
+ "-0.061651 0.157813 -0.019788 "
+ "0.056298 0.063302 0.001233 "
+ "0.030795 0.058601 -0.017734 "
+ "0.021937 0.055669 -0.027055 "
+ "-0.094871 0.122902 0.022189 "
+ "-0.091313 0.113305 0.016325 "
+ "-0.010049 0.165099 -0.020759 "
+ "-0.055288 0.147721 0.028349 "
+ "-0.078988 0.138940 -0.004741 "
+ "-0.062540 0.149101 -0.006575 "
+ "-0.062522 0.149113 -0.005581 "
+ "-0.052484 0.150872 0.017395 "
+ "0.059075 0.067744 0.020163 "
+ "0.042193 0.066261 -0.002834 "
+ "0.042068 0.063212 -0.002158 "
+ "-0.062769 0.175495 -0.061570 "
+ "-0.068864 0.156533 -0.053205 "
+ "-0.068854 0.147497 -0.029579 "
+ "0.058829 0.069107 0.019159 "
+ "0.058229 0.069067 0.006136 "
+ "-0.048864 0.162635 -0.005797 "
+ "-0.062219 0.177210 -0.060600 "
+ "-0.016222 0.186444 -0.020735 "
+ "0.059153 0.070495 0.013155 "
+ "-0.090970 0.113313 0.018324 "
+ "0.041049 0.106185 0.006150 "
+ "-0.062655 0.150596 -0.022579 "
+ "-0.051989 0.142827 0.018310 "
+ "-0.010049 0.177211 -0.025754 "
+ "0.038336 0.102640 -0.005826 "
+ "-0.010940 0.165104 -0.015661 "
+ "-0.039084 0.149487 -0.000629 "
+ "-0.057085 0.136926 -0.005008 "
+ "-0.079775 0.106127 0.031384 "
+ "-0.086972 0.121397 -0.001898 "
+ "-0.024832 0.093010 -0.031618 "
+ "-0.025615 0.156162 -0.003544 "
+ "-0.064398 0.155173 -0.041604 "
+ "-0.053935 0.139875 0.028353 "
+ "-0.010793 0.180280 -0.026745 "
+ "-0.010315 0.180354 -0.028878 "
+ "0.046126 0.073615 0.004216 "
+ "0.031952 0.069103 -0.018709 "
+ "-0.018142 0.159077 -0.009325 "
+ "-0.063220 0.150553 -0.027592 "
+ "-0.063051 0.152177 -0.008587 "
+ "-0.053928 0.152376 0.022392 "
+ "-0.092423 0.113974 0.016676 "
+ "-0.061997 0.147610 -0.004622 "
+ "-0.063084 0.164691 -0.032592 "
+ "-0.010363 0.180289 -0.027734 "
+ "0.039220 0.077501 -0.010108 "
+ "0.029524 0.075033 -0.021815 "
+ "-0.023646 0.184410 -0.015053 "
+ "-0.052765 0.138519 0.026397 "
+ "-0.026155 0.155801 -0.005687 "
+ "-0.072526 0.149972 -0.042469 "
+ "-0.072234 0.147352 -0.024232 "
+ "-0.077900 0.105799 -0.007577 "
+ "0.045717 0.079213 0.021174 "
+ "0.041064 0.092872 -0.007795 "
+ "0.046713 0.072341 0.005563 "
+ "-0.063865 0.166212 -0.033602 "
+ "-0.062874 0.175671 -0.055595 "
+ "0.045825 0.080617 0.020170 "
+ "-0.076643 0.149485 -0.008679 "
+ "-0.063297 0.144361 -0.009605 "
+ "0.045163 0.095673 0.008717 "
+ "0.033399 0.080994 -0.018737 "
+ "-0.081982 0.112797 -0.000848 "
+ "-0.091672 0.117524 0.028069 "
+ "-0.048022 0.144366 0.004427 "
+ "0.044133 0.080232 -0.002522 "
+ "0.037727 0.085584 -0.016367 "
+ "-0.030192 0.153974 -0.003571 "
+ "-0.062777 0.161085 -0.047210 "
+ "-0.062519 0.144437 -0.006582 "
+ "0.043849 0.084675 0.026172 "
+ "-0.076848 0.148423 0.040238 "
+ "-0.067453 0.180926 -0.058380 "
+ "-0.045884 0.129671 0.002936 "
+ "-0.093435 0.121095 0.042248 "
+ "-0.067905 0.173751 -0.043985 "
+ "-0.050555 0.136728 0.023252 "
+ "-0.071802 0.181053 -0.055655 "
+ "-0.010362 0.179472 -0.029571 "
+ "-0.005897 0.098097 0.051999 "
+ "-0.062828 0.151059 0.036055 "
+ "-0.050078 0.136736 0.021957 "
+ "-0.045979 0.167363 0.002591 "
+ "-0.011364 0.182132 -0.028821 "
+ "-0.052576 0.136549 0.028410 "
+ "-0.063846 0.180771 -0.058948 "
+ "0.003007 0.099142 0.049182 "
+ "-0.064141 0.171114 -0.047178 "
+ "-0.075916 0.179384 -0.051606 "
+ "0.000053 0.097862 0.053297 "
+ "-0.074748 0.158885 -0.007836 "
+ "-0.011087 0.180248 -0.025982 "
+ "-0.076524 0.178854 -0.050295 "
+ "-0.086930 0.151358 0.010227 "
+ "-0.019131 0.122285 0.030395 "
+ "-0.058101 0.158247 0.004154 "
+ "-0.082061 0.151973 0.004324 "
+ "0.026954 0.116596 0.031065 "
+ "-0.013723 0.183161 -0.024107 "
+ "-0.082061 0.154804 0.013629 "
+ "-0.085429 0.151325 0.028294 "
+ "-0.076439 0.098866 -0.011539 "
+ "-0.086549 0.109000 0.015694 "
+ "-0.091717 0.141326 0.017134 "
+ "-0.062003 0.154899 0.004205 "
+ "-0.092111 0.128137 0.031104 "
+ "-0.068605 0.155434 -0.051807 "
+ "-0.005161 0.100957 0.044271 "
+ "-0.044251 0.120873 0.027396 "
+ "-0.089317 0.151601 0.019497 "
+ "-0.063020 0.168283 -0.045257 "
+ "-0.077704 0.175863 -0.045075 "
+ "-0.091075 0.135224 0.016144 "
+ "0.049370 0.069379 0.025027 "
+ "-0.007104 0.125174 0.031741 "
+ "0.033637 0.112962 -0.002307 "
+ "-0.074694 0.111142 -0.007602 "
+ "-0.017221 0.187225 -0.021155 "
+ "-0.030168 0.118019 0.030802 "
+ "-0.028460 0.125795 0.008832 "
+ "0.057735 0.070506 0.007469 "
+ "0.003964 0.097777 0.051060 "
+ "0.032895 0.114630 -0.001059 "
+ "0.027501 0.118700 -0.003048 "
+ "-0.079635 0.167489 -0.036775 "
+ "-0.017050 0.122405 0.031234 "
+ "-0.007453 0.100521 0.046910 "
+ "-0.053725 0.151161 0.024627 "
+ "-0.045982 0.130505 0.004443 "
+ "0.012743 0.127966 0.027041 "
+ "0.025743 0.120365 -0.002249 "
+ "-0.081065 0.111196 -0.000918 "
+ "-0.051027 0.136724 0.024552 "
+ "-0.067871 0.156792 0.022379 "
+ "-0.063800 0.166551 -0.035494 "
+ "0.060392 0.069417 0.011831 "
+ "-0.047170 0.165970 0.003632 "
+ "0.030267 0.120025 0.005493 "
+ "-0.091498 0.150215 0.020796 "
+ "-0.071332 0.151217 -0.043920 "
+ "-0.087751 0.128902 0.000900 "
+ "-0.063805 0.152122 -0.002245 "
+ "-0.031285 0.119331 0.029156 "
+ "0.022998 0.123119 -0.000380 "
+ "0.022167 0.123584 -0.000681 "
+ "-0.054554 0.160714 0.005405 "
+ "0.057670 0.071756 0.018414 "
+ "0.022042 0.124260 0.000701 "
+ "-0.087413 0.093190 0.004840 "
+ "-0.009347 0.098183 0.051937 "
+ "-0.061079 0.151413 0.035139 "
+ "0.002048 0.102202 0.043813 "
+ "-0.066613 0.148623 0.039262 "
+ "-0.055002 0.153297 0.028117 "
+ "0.046790 0.073773 0.016792 "
+ "-0.031448 0.177007 -0.013927 "
+ "-0.001162 0.098034 0.052196 "
+ "-0.051351 0.152104 0.013223 "
+ "-0.042883 0.128827 0.014388 "
+ "-0.022585 0.185119 -0.013184 "
+ "0.032239 0.117348 0.023130 "
+ "0.011504 0.129313 0.001094 "
+ "-0.023460 0.184384 -0.011932 "
+ "-0.075946 0.097459 -0.012513 "
+ "-0.005179 0.129883 0.025223 "
+ "0.005861 0.130491 0.001853 "
+ "-0.029850 0.180049 -0.011606 "
+ "-0.067096 0.164861 -0.018887 "
+ "-0.073324 0.170102 -0.027685 "
+ "0.052646 0.072994 0.020857 "
+ "-0.003124 0.123427 0.034663 "
+ "-0.014724 0.129248 0.015884 "
+ "-0.075527 0.109807 -0.007602 "
+ "-0.027112 0.124435 0.018388 "
+ "-0.044261 0.122001 0.025892 "
+ "-0.077187 0.153815 0.000184 "
+ "-0.084777 0.095970 -0.003931 "
+ "-0.047172 0.133935 0.009815 "
+ "0.052984 0.069258 0.024906 "
+ "0.052194 0.069772 0.024557 "
+ "0.007149 0.123902 0.033879 "
+ "-0.046057 0.131197 0.005824 "
+ "0.007587 0.130944 0.001903 "
+ "-0.072750 0.180616 -0.054977 "
+ "-0.018876 0.105884 -0.022628 "
+ "-0.017875 0.105871 -0.022589 "
+ "-0.066685 0.036123 0.035141 "
+ "-0.065936 0.036420 0.038649 "
+ "-0.060884 0.036716 0.044984 "
+ "-0.059243 0.036753 0.046137 "
+ "-0.047036 0.035908 0.045106 "
+ "-0.044737 0.035714 0.044172 "
+ "-0.069135 0.035455 0.012250 "
+ "-0.068464 0.035536 0.013028 "
+ "-0.062423 0.035753 0.019657 "
+ "-0.061746 0.042600 0.042925 "
+ "-0.055906 0.037983 0.047068 "
+ "-0.037607 0.036543 0.044419 "
+ "-0.036930 0.036554 0.045141 "
+ "-0.036270 0.036560 0.045881 "
+ "-0.033821 0.036733 0.049100 "
+ "-0.033215 0.036763 0.049913 "
+ "-0.031638 0.036774 0.051182 "
+ "-0.030039 0.036799 0.052422 "
+ "-0.028341 0.036810 0.053558 "
+ "-0.055235 0.043304 0.046580 "
+ "-0.026570 0.038204 0.055248 "
+ "-0.064199 0.042219 0.027537 "
+ "-0.067406 0.035436 0.036194 "
+ "-0.064134 0.044628 0.034748 "
+ "-0.034115 0.042437 0.048702 "
+ "-0.088565 0.096304 0.022720 "
+ "-0.071415 0.038661 0.000579 "
+ "-0.071384 0.035533 0.010286 "
+ "-0.059415 0.039567 0.019974 "
+ "-0.002780 0.100393 -0.023542 "
+ "-0.030725 0.079300 0.041026 "
+ "-0.089150 0.139122 0.037510 "
+ "-0.064435 0.042410 0.030248 "
+ "-0.064596 0.043007 0.035823 "
+ "-0.038969 0.042042 0.041595 "
+ "-0.024661 0.042233 0.053955 "
+ "-0.078686 0.174223 -0.045863 "
+ "-0.055578 0.159581 0.006379 "
+ "-0.067561 0.044863 0.007377 "
+ "-0.071811 0.095541 0.041327 "
+ "-0.063288 0.044436 0.037293 "
+ "-0.088305 0.149858 0.026519 "
+ "-0.028815 0.043632 0.051009 "
+ "-0.021138 0.098548 0.044502 "
+ "-0.083902 0.105911 0.026288 "
+ "-0.056782 0.043535 0.016717 "
+ "-0.090816 0.116185 0.042702 "
+ "-0.067315 0.044668 0.006020 "
+ "-0.060400 0.047013 0.037152 "
+ "-0.074680 0.157704 -0.005566 "
+ "-0.045598 0.169625 -0.002419 "
+ "-0.057758 0.047918 0.035950 "
+ "-0.058863 0.156048 0.009540 "
+ "-0.055124 0.048280 0.038911 "
+ "-0.020816 0.079725 0.056038 "
+ "-0.077790 0.164478 -0.025199 "
+ "-0.078272 0.165861 -0.029036 "
+ "-0.057476 0.047878 0.009161 "
+ "-0.076982 0.179572 -0.051371 "
+ "-0.079848 0.116269 0.048758 "
+ "-0.023736 0.086541 0.054457 "
+ "-0.078852 0.107465 0.032436 "
+ "-0.032961 0.051054 0.038223 "
+ "-0.075260 0.157705 -0.006422 "
+ "-0.055289 0.160933 0.003284 "
+ "-0.058227 0.048399 0.006759 "
+ "-0.050039 0.047811 0.016868 "
+ "-0.058040 0.060877 0.024555 "
+ "-0.027311 0.091770 0.045195 "
+ "-0.083080 0.081450 0.029210 "
+ "-0.077793 0.167262 -0.029530 "
+ "-0.075338 0.161792 -0.013478 "
+ "-0.051770 0.051602 0.031230 "
+ "-0.051573 0.052359 0.030554 "
+ "-0.020587 0.094703 0.051310 "
+ "-0.087066 0.141474 0.040183 "
+ "-0.073808 0.102191 0.037495 "
+ "-0.079748 0.078938 0.031423 "
+ "-0.082082 0.150001 0.034405 "
+ "-0.051491 0.052957 0.030880 "
+ "-0.048255 0.053230 0.036171 "
+ "-0.047395 0.035957 0.045283 "
+ "-0.014451 0.103914 0.043265 "
+ "-0.076840 0.178514 -0.050928 "
+ "-0.077412 0.160346 -0.016940 "
+ "-0.027792 0.053068 0.036540 "
+ "-0.076742 0.164619 -0.022017 "
+ "-0.051707 0.163863 0.002419 "
+ "-0.079102 0.170034 -0.038269 "
+ "-0.079102 0.171279 -0.041032 "
+ "-0.050567 0.054554 0.017337 "
+ "-0.052122 0.055334 0.025863 "
+ "-0.077519 0.147451 0.041178 "
+ "-0.071550 0.076334 0.037454 "
+ "-0.085225 0.114025 0.048015 "
+ "-0.088460 0.097654 0.022602 "
+ "-0.079604 0.174107 -0.046390 "
+ "-0.052890 0.055616 0.012484 "
+ "-0.052288 0.055669 0.013306 "
+ "-0.051297 0.055683 0.015038 "
+ "-0.022939 0.098598 0.044810 "
+ "-0.016571 0.056922 0.051097 "
+ "-0.020326 0.082530 0.056871 "
+ "-0.078583 0.087018 0.037216 "
+ "-0.078901 0.172846 -0.044095 "
+ "-0.057294 0.059745 0.022361 "
+ "-0.053553 0.060429 0.026462 "
+ "-0.043647 0.060159 0.039893 "
+ "-0.026540 0.059202 0.038381 "
+ "-0.068767 0.090095 0.042705 "
+ "-0.077518 0.172972 -0.039913 "
+ "-0.023874 0.105835 -0.022678 "
+ "-0.053287 0.061060 0.028214 "
+ "-0.043822 0.061568 0.040004 "
+ "-0.085799 0.151046 0.029438 "
+ "-0.016547 0.061194 0.052368 "
+ "-0.090279 0.116199 0.043584 "
+ "-0.077750 0.161717 -0.020949 "
+ "-0.079195 0.168601 -0.036643 "
+ "-0.085221 0.132036 0.049369 "
+ "-0.053062 0.062559 0.030763 "
+ "-0.051701 0.062677 0.032235 "
+ "-0.051142 0.062742 0.033098 "
+ "-0.016685 0.062588 0.052478 "
+ "-0.074605 0.114104 -0.006416 "
+ "-0.081131 0.129963 0.052448 "
+ "-0.077190 0.117737 0.051681 "
+ "-0.025047 0.094997 0.044896 "
+ "-0.077356 0.163115 -0.021355 "
+ "-0.026296 0.063447 0.039421 "
+ "-0.016414 0.099612 0.044078 "
+ "-0.074764 0.107316 0.036203 "
+ "-0.066853 0.091456 0.043506 "
+ "-0.082460 0.108623 0.027619 "
+ "-0.078951 0.128836 0.054193 "
+ "-0.045045 0.065713 0.039695 "
+ "-0.025749 0.036557 0.054009 "
+ "-0.016813 0.062115 0.052682 "
+ "-0.016674 0.066891 0.053711 "
+ "-0.011794 0.066719 0.054462 "
+ "-0.076681 0.154876 -0.006314 "
+ "-0.090499 0.148452 0.024515 "
+ "-0.028731 0.074896 0.041482 "
+ "0.010138 0.104418 -0.020246 "
+ "-0.015186 0.168246 -0.021561 "
+ "-0.042881 0.108469 -0.019386 "
+ "-0.058997 0.084194 -0.020854 "
+ "-0.091683 0.116094 0.040744 "
+ "-0.085234 0.104506 0.024826 "
+ "-0.071459 0.151188 -0.043322 "
+ "-0.017603 0.071155 0.054519 "
+ "-0.078807 0.106130 0.032485 "
+ "-0.025047 0.080547 0.053580 "
+ "-0.071901 0.152408 -0.043423 "
+ "-0.023799 0.184341 -0.014597 "
+ "-0.019317 0.075402 0.054688 "
+ "-0.010806 0.034295 -0.025597 "
+ "-0.008683 0.097657 -0.030445 "
+ "-0.070396 0.038222 -0.000484 "
+ "-0.064320 0.035936 0.024151 "
+ "-0.070750 0.162623 -0.045378 "
+ "-0.047784 0.038449 -0.012233 "
+ "-0.067401 0.040625 -0.003341 "
+ "0.012498 0.036363 -0.021917 "
+ "-0.090556 0.129700 0.040228 "
+ "-0.071563 0.151347 -0.043624 "
+ "-0.044911 0.045780 -0.010946 "
+ "-0.047318 0.039794 -0.012062 "
+ "-0.068755 0.116060 -0.008746 "
+ "-0.089126 0.137079 0.025122 "
+ "-0.069476 0.042643 0.000668 "
+ "-0.072853 0.035550 0.002529 "
+ "-0.075578 0.154868 -0.000834 "
+ "-0.054371 0.158326 -0.001789 "
+ "-0.060851 0.155133 0.003690 "
+ "-0.056894 0.158351 0.001188 "
+ "-0.057497 0.158400 0.002092 "
+ "-0.065238 0.041953 0.033705 "
+ "-0.077268 0.155563 -0.011897 "
+ "-0.043908 0.034308 -0.026883 "
+ "-0.044744 0.039642 -0.021577 "
+ "0.022162 0.095920 -0.021972 "
+ "-0.068386 0.043493 0.004694 "
+ "-0.068639 0.043534 0.005698 "
+ "-0.038348 0.041711 -0.027346 "
+ "-0.061186 0.044121 -0.004327 "
+ "-0.059362 0.047606 0.000101 "
+ "-0.039022 0.045911 -0.022759 "
+ "-0.063007 0.045830 0.008721 "
+ "-0.047064 0.168385 -0.001002 "
+ "-0.038149 0.044868 -0.024210 "
+ "-0.054255 0.049028 0.029769 "
+ "-0.038766 0.045933 -0.022284 "
+ "-0.041323 0.046240 -0.016312 "
+ "-0.043267 0.046587 -0.011389 "
+ "-0.075668 0.155622 -0.002603 "
+ "-0.091032 0.144770 0.026167 "
+ "-0.086328 0.151358 0.009233 "
+ "-0.057566 0.048138 0.034664 "
+ "-0.057553 0.048178 0.038938 "
+ "-0.036151 0.047160 -0.021326 "
+ "-0.082537 0.151557 0.003650 "
+ "-0.041416 0.047637 -0.012230 "
+ "-0.052279 0.049031 0.035659 "
+ "-0.073572 0.148626 -0.026849 "
+ "-0.036200 0.048331 -0.016295 "
+ "-0.036606 0.048448 -0.014284 "
+ "-0.053168 0.050303 -0.006388 "
+ "-0.057148 0.052574 0.000631 "
+ "-0.030353 0.049890 -0.019369 "
+ "-0.052512 0.051984 -0.007188 "
+ "0.011853 0.105542 -0.019754 "
+ "-0.056771 0.057251 0.007253 "
+ "-0.071828 0.141507 -0.008016 "
+ "-0.051032 0.054419 0.019619 "
+ "-0.088924 0.151541 0.021101 "
+ "-0.052832 0.055837 -0.007420 "
+ "-0.049863 0.054275 0.015634 "
+ "-0.094007 0.129802 0.018180 "
+ "-0.024936 0.054649 -0.028408 "
+ "-0.075829 0.093523 -0.013687 "
+ "0.018167 0.095997 -0.023265 "
+ "-0.065589 0.167628 -0.059737 "
+ "-0.074146 0.151129 -0.029188 "
+ "0.016944 0.094696 -0.024307 "
+ "-0.057089 0.060232 -0.002574 "
+ "-0.037067 0.156264 -0.010370 "
+ "-0.091018 0.149734 0.013441 "
+ "-0.052736 0.163457 -0.000904 "
+ "-0.078459 0.159178 -0.022073 "
+ "-0.056704 0.111275 -0.017686 "
+ "-0.079616 0.070320 0.016623 "
+ "-0.040892 0.109911 -0.018800 "
+ "-0.081009 0.136808 -0.002869 "
+ "-0.062766 0.170921 -0.061607 "
+ "-0.052123 0.049165 0.035634 "
+ "0.040891 0.091776 -0.009100 "
+ "0.015157 0.107807 -0.018185 "
+ "-0.077880 0.103388 -0.008666 "
+ "-0.040161 0.110268 -0.018707 "
+ "-0.074397 0.160314 -0.030905 "
+ "-0.008919 0.033847 -0.025125 "
+ "0.029469 0.095176 -0.018643 "
+ "-0.075086 0.067468 0.003551 "
+ "-0.078424 0.146692 -0.003613 "
+ "-0.091437 0.114681 0.021315 "
+ "-0.090341 0.130900 0.040068 "
+ "0.004647 0.093885 -0.032195 "
+ "-0.077076 0.068957 0.008539 "
+ "-0.065286 0.176792 -0.060802 "
+ "-0.013252 0.180136 -0.028613 "
+ "-0.052901 0.114056 -0.016235 "
+ "-0.017913 0.184413 -0.023990 "
+ "-0.079885 0.070630 0.012550 "
+ "-0.079889 0.070630 0.013550 "
+ "-0.025189 0.178943 -0.018780 "
+ "-0.077799 0.073099 -0.005325 "
+ "-0.080225 0.072000 0.005544 "
+ "-0.080010 0.072030 0.018553 "
+ "-0.079546 0.071986 0.019558 "
+ "-0.026014 0.068834 -0.034504 "
+ "-0.030094 0.067301 -0.027970 "
+ "-0.030573 0.059945 -0.018933 "
+ "-0.075706 0.070108 -0.000047 "
+ "-0.039761 0.120231 -0.012793 "
+ "-0.072928 0.148723 -0.032782 "
+ "-0.038619 0.047455 -0.016386 "
+ "0.027694 0.085014 -0.022063 "
+ "-0.072448 0.148649 -0.034807 "
+ "-0.082956 0.076257 0.016526 "
+ "-0.066412 0.162305 -0.057942 "
+ "-0.072012 0.148777 -0.035760 "
+ "-0.077608 0.077089 -0.007600 "
+ "-0.084539 0.077802 0.014517 "
+ "-0.071014 0.148895 -0.037822 "
+ "-0.075920 0.078323 -0.009591 "
+ "-0.086853 0.081079 0.010849 "
+ "-0.071911 0.144135 -0.014659 "
+ "-0.074627 0.149901 -0.029871 "
+ "-0.077891 0.079796 -0.008639 "
+ "-0.082390 0.074996 0.003304 "
+ "-0.070947 0.140954 -0.008050 "
+ "-0.077146 0.156966 -0.011900 "
+ "-0.073019 0.080841 -0.014580 "
+ "-0.077886 0.081156 -0.009633 "
+ "-0.036361 0.078336 -0.019535 "
+ "-0.074019 0.082262 -0.014630 "
+ "-0.075495 0.155610 -0.001907 "
+ "-0.081644 0.129364 -0.004806 "
+ "-0.032331 0.078279 -0.031704 "
+ "-0.079744 0.081910 -0.007905 "
+ "-0.089763 0.133813 0.039204 "
+ "0.038710 0.105846 0.000022 "
+ "-0.061587 0.117934 -0.010507 "
+ "-0.075033 0.085103 -0.014560 "
+ "-0.040541 0.166202 -0.012095 "
+ "-0.087722 0.151511 0.024071 "
+ "-0.033651 0.085644 -0.024757 "
+ "-0.075237 0.086484 -0.014514 "
+ "-0.087724 0.085428 0.014220 "
+ "-0.089609 0.141602 0.031992 "
+ "-0.075188 0.152833 -0.015966 "
+ "-0.068568 0.156629 -0.052997 "
+ "-0.009676 0.104885 -0.023104 "
+ "-0.085360 0.109807 0.006222 "
+ "-0.039019 0.174111 -0.009029 "
+ "-0.027379 0.085971 -0.035654 "
+ "-0.055531 0.040944 -0.009922 "
+ "-0.086394 0.085570 -0.000840 "
+ "-0.076358 0.141673 -0.005738 "
+ "-0.032093 0.087730 -0.025498 "
+ "-0.017229 0.104564 -0.022745 "
+ "-0.049660 0.118919 -0.014131 "
+ "-0.089823 0.092558 0.021822 "
+ "-0.076654 0.113990 -0.004814 "
+ "-0.092635 0.116935 0.008842 "
+ "-0.041134 0.037889 -0.027518 "
+ "-0.028876 0.046967 -0.026215 "
+ "0.000689 0.105475 -0.021690 "
+ "0.017762 0.100614 -0.022454 "
+ "0.038365 0.101327 -0.007241 "
+ "0.016593 0.100464 -0.022378 "
+ "-0.025269 0.093126 -0.030867 "
+ "-0.052243 0.126999 -0.005034 "
+ "0.053623 0.058755 -0.002690 "
+ "-0.048269 0.037339 -0.012384 "
+ "-0.021680 0.184690 -0.018817 "
+ "0.029353 0.117442 -0.002543 "
+ "0.021961 0.100321 -0.021160 "
+ "-0.012106 0.086958 -0.038462 "
+ "-0.013342 0.112081 -0.018621 "
+ "0.024541 0.081954 -0.024971 "
+ "-0.007420 0.035877 -0.024927 "
+ "-0.009175 0.087571 -0.037094 "
+ "0.054837 0.073109 0.009023 "
+ "0.055426 0.071416 0.006408 "
+ "0.056334 0.071137 0.006716 "
+ "-0.020732 0.114935 -0.016742 "
+ "0.017679 0.085219 -0.028353 "
+ "-0.018768 0.034185 -0.027695 "
+ "-0.035284 0.173981 -0.012629 "
+ "-0.026663 0.178341 -0.017682 "
+ "-0.023595 0.180085 -0.019676 "
+ "-0.014488 0.185446 -0.025770 "
+ "-0.032519 0.174119 -0.014863 "
+ "-0.065357 0.155100 0.005646 "
+ "-0.064416 0.154996 0.005865 "
+ "-0.044287 0.166804 -0.008538 "
+ "-0.023240 0.093652 -0.032678 "
+ "-0.055818 0.158024 -0.000145 "
+ "-0.052392 0.160438 -0.003076 "
+ "-0.041472 0.167040 -0.010815 "
+ "-0.014584 0.185601 -0.026982 "
+ "-0.026852 0.098187 -0.024073 "
+ "-0.027912 0.034067 -0.029247 "
+ "-0.025710 0.098352 -0.024235 "
+ "-0.038543 0.038993 -0.028624 "
+ "-0.052489 0.159007 -0.003405 "
+ "-0.056184 0.037462 -0.010574 "
+ "-0.010553 0.180959 -0.028854 "
+ "-0.020256 0.036506 -0.028012 "
+ "-0.019833 0.107697 -0.022052 "
+ "-0.011421 0.107659 -0.022044 "
+ "-0.059774 0.149902 -0.001229 "
+ "-0.019309 0.036282 -0.027779 "
+ "0.025109 0.111719 -0.012490 "
+ "-0.026057 0.037591 -0.029189 "
+ "0.011270 0.104834 -0.020502 "
+ "0.023069 0.121925 -0.004197 "
+ "0.046473 0.074730 0.007000 "
+ "-0.039662 0.039902 -0.027457 "
+ "-0.037022 0.041095 -0.028655 "
+ "-0.009941 0.089177 -0.036764 "
+ "-0.005608 0.125002 -0.009489 "
+ "-0.033775 0.105400 -0.020968 "
+ "-0.016581 0.035588 -0.026711 "
+ "-0.058428 0.038335 -0.009417 "
+ "-0.063375 0.036885 -0.008048 "
+ "-0.011756 0.034513 -0.025836 "
+ "0.006642 0.099379 -0.022123 "
+ "0.039487 0.105310 0.000603 "
+ "-0.061814 0.045340 -0.001725 "
+ "-0.063482 0.035282 -0.008247 "
+ "-0.055554 0.042026 -0.008894 "
+ "-0.005249 0.101176 -0.023207 "
+ "-0.028861 0.044835 -0.028193 "
+ "-0.028589 0.093525 -0.025224 "
+ "-0.058144 0.114319 -0.014834 "
+ "-0.015397 0.125153 -0.004109 "
+ "-0.010059 0.109235 -0.021708 "
+ "-0.059319 0.114075 -0.014560 "
+ "-0.010537 0.085053 -0.038446 "
+ "-0.046412 0.035458 -0.018725 "
+ "-0.065683 0.180548 -0.059031 "
+ "0.014582 0.100670 -0.022593 "
+ "-0.069184 0.181162 -0.058506 "
+ "-0.002499 0.112386 -0.019640 "
+ "-0.049884 0.034085 -0.013307 "
+ "-0.006156 0.110566 -0.022214 "
+ "-0.066229 0.042518 -0.003252 "
+ "-0.009516 0.084860 -0.038222 "
+ "-0.028518 0.094880 -0.024629 "
+ "-0.040442 0.035113 -0.028939 "
+ "-0.018198 0.089616 -0.037280 "
+ "-0.063353 0.045449 0.000290 "
+ "-0.035084 0.107815 -0.020111 "
+ "-0.086923 0.112018 0.023246 "
+ "0.032361 0.078690 -0.019246 "
+ "0.027427 0.078192 -0.023091 "
+ "0.005420 0.091290 -0.032762 "
+ "-0.002657 0.089995 -0.035491 "
+ "-0.009946 0.033902 -0.025192 "
+ "0.004610 0.092806 -0.032334 "
+ "0.001474 0.103921 -0.022070 "
+ "-0.055345 0.138992 -0.003070 "
+ "-0.049948 0.135176 0.001169 "
+ "-0.048008 0.135924 0.010304 "
+ "-0.020760 0.034720 -0.027738 "
+ "0.030631 0.076191 -0.020965 "
+ "-0.063071 0.173863 -0.061680 "
+ "-0.038215 0.047167 -0.018189 "
+ "-0.051474 0.135214 -0.000925 "
+ "0.011863 0.089406 -0.030726 "
+ "0.009956 0.089786 -0.031175 "
+ "0.004929 0.114402 -0.020103 "
+ "-0.061876 0.138649 -0.006770 "
+ "0.027932 0.081332 -0.022263 "
+ "0.051888 0.065658 -0.001557 "
+ "0.018711 0.100467 -0.022355 "
+ "-0.018396 0.036084 -0.027420 "
+ "-0.028317 0.096340 -0.024150 "
+ "0.001762 0.130465 0.001572 "
+ "-0.032558 0.114916 -0.016084 "
+ "-0.069082 0.181440 -0.056457 "
+ "-0.057620 0.135609 -0.005468 "
+ "0.021367 0.085150 -0.026249 "
+ "-0.015764 0.035327 -0.026378 "
+ "0.028626 0.063844 -0.020406 "
+ "-0.036903 0.033739 -0.030894 "
+ "-0.057875 0.134121 -0.005853 "
+ "-0.043875 0.128361 0.000409 "
+ "0.030532 0.081181 -0.020086 "
+ "-0.020082 0.097870 -0.024530 "
+ "0.015406 0.088042 -0.029297 "
+ "-0.021725 0.126122 0.002697 "
+ "0.012366 0.128443 -0.000398 "
+ "0.016865 0.095885 -0.023523 "
+ "-0.023274 0.106158 -0.022428 "
+ "-0.064314 0.044528 -0.000908 "
+ "-0.058084 0.132712 -0.006348 "
+ "-0.040510 0.127658 -0.000962 "
+ "-0.039493 0.127549 -0.000886 "
+ "0.032698 0.111022 -0.007977 "
+ "-0.057988 0.041427 -0.008327 "
+ "-0.059581 0.038095 -0.009172 "
+ "-0.046740 0.129245 0.000886 "
+ "-0.040696 0.125707 -0.008047 "
+ "-0.027903 0.124426 0.000511 "
+ "0.003607 0.126142 -0.007161 "
+ "0.018408 0.127764 0.003915 "
+ "-0.065444 0.178190 -0.060631 "
+ "-0.064392 0.178295 -0.060775 "
+ "-0.042616 0.035917 -0.027130 "
+ "0.011575 0.104003 -0.020168 "
+ "0.035855 0.084336 -0.017290 "
+ "0.057575 0.067384 0.002722 "
+ "0.009753 0.120711 -0.014067 "
+ "-0.037293 0.039467 -0.029119 "
+ "0.039034 0.102562 -0.004541 "
+ "-0.028854 0.124656 0.002099 "
+ "-0.009828 0.034195 -0.024366 "
+ "-0.014578 0.034825 -0.025972 "
+ "-0.024541 0.034994 -0.028704 "
+ "0.010385 0.036060 -0.022439 "
+ "-0.007582 0.036205 -0.025079 "
+ "-0.008587 0.036240 -0.025275 "
+ "-0.068515 0.033856 -0.004706 "
+ "0.008998 0.036490 -0.023427 "
+ "-0.015608 0.037809 -0.026925 "
+ "-0.070832 0.034582 -0.001037 "
+ "-0.071532 0.034754 -0.000134 "
+ "-0.029580 0.036889 0.052851 "
+ "-0.063931 0.115260 -0.011323 "
+ "-0.033262 0.042424 -0.030211 "
+ "-0.057517 0.037335 -0.010190 "
+ "0.029434 0.038378 -0.001734 "
+ "-0.084040 0.132376 -0.001828 "
+ "-0.008576 0.043164 -0.026091 "
+ "-0.014324 0.103680 -0.023389 "
+ "0.037023 0.040523 -0.002041 "
+ "0.024475 0.040042 -0.005110 "
+ "0.005315 0.042896 -0.024171 "
+ "0.019159 0.100268 -0.022296 "
+ "0.015149 0.100260 -0.022640 "
+ "0.012143 0.100268 -0.022675 "
+ "-0.011195 0.178453 -0.029630 "
+ "-0.013516 0.162506 -0.015305 "
+ "-0.011766 0.182389 -0.028750 "
+ "-0.021881 0.107267 -0.022087 "
+ "-0.070124 0.109501 0.038232 "
+ "-0.056543 0.041557 -0.008718 "
+ "-0.071200 0.150790 -0.043462 "
+ "-0.063514 0.041263 -0.006393 "
+ "-0.064494 0.041204 -0.005990 "
+ "0.013531 0.047821 -0.026028 "
+ "-0.023856 0.100178 -0.023968 "
+ "-0.024856 0.100174 -0.023964 "
+ "-0.046567 0.043315 -0.011194 "
+ "0.041065 0.050916 -0.007309 "
+ "-0.010696 0.176084 -0.029251 "
+ "-0.008375 0.045859 -0.029537 "
+ "0.028544 0.045965 -0.007084 "
+ "0.001825 0.048337 -0.030067 "
+ "-0.018756 0.044979 -0.027907 "
+ "-0.016257 0.053183 -0.032944 "
+ "-0.065583 0.062695 -0.002740 "
+ "-0.053491 0.045812 -0.007550 "
+ "0.022227 0.051381 -0.024145 "
+ "-0.027082 0.048959 -0.025227 "
+ "-0.027650 0.050413 -0.024084 "
+ "-0.025115 0.163764 -0.015904 "
+ "0.047380 0.047547 -0.002962 "
+ "0.026329 0.051777 -0.020970 "
+ "-0.032809 0.050877 -0.011141 "
+ "-0.035189 0.171161 -0.014036 "
+ "0.026360 0.053174 -0.020887 "
+ "0.008578 0.054156 -0.030513 "
+ "-0.052565 0.051568 -0.006607 "
+ "-0.007171 0.054903 -0.033099 "
+ "-0.010840 0.175678 -0.029032 "
+ "-0.025680 0.061671 -0.031107 "
+ "-0.067790 0.180834 -0.058549 "
+ "0.011146 0.101633 -0.021979 "
+ "-0.021147 0.037771 0.053826 "
+ "0.036614 0.054238 -0.006713 "
+ "-0.040331 0.107690 -0.019920 "
+ "-0.042983 0.054604 -0.011188 "
+ "-0.034077 0.172586 -0.014513 "
+ "0.003429 0.056934 -0.031203 "
+ "0.000547 0.063627 -0.034407 "
+ "-0.038058 0.154792 -0.009202 "
+ "-0.057976 0.145744 -0.001916 "
+ "0.048347 0.058938 -0.004908 "
+ "0.043359 0.058916 -0.004699 "
+ "0.023338 0.060640 -0.025362 "
+ "0.021085 0.061931 -0.026580 "
+ "-0.044209 0.165146 -0.009112 "
+ "0.011063 0.036040 -0.022358 "
+ "-0.026875 0.105825 -0.022448 "
+ "-0.025640 0.106222 -0.022782 "
+ "-0.014088 0.062639 -0.036940 "
+ "-0.054028 0.131061 -0.005002 "
+ "-0.061313 0.103293 -0.018311 "
+ "-0.064291 0.169609 -0.060793 "
+ "-0.033408 0.056284 -0.010273 "
+ "-0.073436 0.073412 0.034880 "
+ "-0.043887 0.108471 -0.019159 "
+ "-0.054118 0.157553 -0.002300 "
+ "-0.057247 0.060528 -0.003067 "
+ "0.026280 0.066086 -0.022657 "
+ "0.021876 0.069295 -0.027500 "
+ "-0.061063 0.138403 -0.006548 "
+ "-0.078029 0.141290 -0.004917 "
+ "-0.060035 0.121358 -0.008732 "
+ "0.010879 0.066111 -0.031139 "
+ "-0.002871 0.068252 -0.034506 "
+ "-0.015884 0.037769 0.051576 "
+ "0.012128 0.110130 -0.018785 "
+ "-0.038213 0.169665 -0.012413 "
+ "-0.021532 0.173488 -0.021241 "
+ "-0.008517 0.074198 -0.037653 "
+ "-0.065608 0.066936 -0.007497 "
+ "-0.050280 0.143741 0.000824 "
+ "-0.011878 0.107304 -0.022095 "
+ "-0.040535 0.159216 -0.011492 "
+ "0.028230 0.071783 -0.022893 "
+ "-0.021022 0.065418 -0.036655 "
+ "0.010141 0.103018 -0.020770 "
+ "-0.051433 0.067108 -0.012491 "
+ "-0.049160 0.160603 -0.005985 "
+ "0.028264 0.073195 -0.022943 "
+ "-0.015261 0.076268 -0.038966 "
+ "-0.034533 0.064421 -0.014132 "
+ "-0.043908 0.067784 -0.016159 "
+ "-0.069183 0.153837 -0.049560 "
+ "0.024869 0.084767 -0.024560 "
+ "-0.000856 0.103081 -0.022665 "
+ "-0.001863 0.103074 -0.022753 "
+ "-0.059077 0.069973 -0.015104 "
+ "-0.012182 0.178673 -0.029235 "
+ "-0.012881 0.107288 -0.021846 "
+ "-0.047615 0.037865 -0.012539 "
+ "-0.067038 0.155239 -0.051934 "
+ "-0.041303 0.084378 -0.021267 "
+ "0.000115 0.110154 -0.020162 "
+ "-0.021633 0.035044 -0.028053 "
+ "-0.045200 0.165138 -0.008331 "
+ "-0.069516 0.075346 -0.014957 "
+ "-0.076657 0.075864 -0.007819 "
+ "-0.067246 0.173921 -0.058281 "
+ "-0.046064 0.151688 -0.005548 "
+ "-0.016173 0.168253 -0.021040 "
+ "0.008721 0.073259 -0.033724 "
+ "0.005912 0.084442 -0.033198 "
+ "-0.026776 0.036519 0.053822 "
+ "-0.027674 0.086866 -0.036187 "
+ "0.034221 0.081361 -0.018238 "
+ "0.030238 0.081488 -0.020170 "
+ "0.025200 0.081800 -0.024570 "
+ "0.014356 0.076788 -0.030104 "
+ "-0.064199 0.163840 -0.059407 "
+ "-0.080908 0.111715 -0.000772 "
+ "0.014113 0.087083 -0.030146 "
+ "-0.074816 0.084473 -0.015099 "
+ "-0.074011 0.141343 -0.006888 "
+ "-0.076411 0.094841 -0.013406 "
+ "-0.077868 0.101964 -0.009289 "
+ "-0.068244 0.179461 -0.058921 "
+ "-0.002226 0.083034 -0.037089 "
+ "-0.022749 0.077591 -0.039002 "
+ "-0.055926 0.079634 -0.020972 "
+ "-0.065841 0.083605 -0.018912 "
+ "-0.064292 0.168172 -0.060588 "
+ "-0.075004 0.141334 -0.006491 "
+ "-0.015214 0.091053 -0.037504 "
+ "-0.054630 0.152755 -0.002679 "
+ "0.033230 0.087092 -0.018683 "
+ "0.011114 0.110132 -0.019128 "
+ "-0.075927 0.125275 -0.008277 "
+ "-0.015831 0.088312 -0.038067 "
+ "-0.018345 0.033731 -0.026079 "
+ "0.004456 0.033966 -0.021007 "
+ "-0.003268 0.034370 -0.017376 "
+ "0.011950 0.034304 -0.011204 "
+ "-0.045950 0.035101 -0.022412 "
+ "-0.031721 0.033828 -0.021186 "
+ "0.007540 0.038802 -0.009431 "
+ "-0.080480 0.114080 0.047569 "
+ "-0.004172 0.039154 -0.014111 "
+ "-0.032684 0.038589 -0.016529 "
+ "-0.051977 0.034100 -0.013438 "
+ "0.032543 0.038050 0.000016 "
+ "0.033741 0.038472 -0.000065 "
+ "-0.039195 0.034033 -0.015253 "
+ "-0.073604 0.148628 -0.025599 "
+ "0.007509 0.034411 0.004670 "
+ "0.025586 0.034909 0.002751 "
+ "-0.062356 0.033753 -0.009695 "
+ "-0.042372 0.033809 -0.003477 "
+ "0.017120 0.034271 0.001954 "
+ "-0.059944 0.033627 -0.010153 "
+ "-0.038120 0.038250 -0.006977 "
+ "-0.004094 0.038391 0.008568 "
+ "0.025074 0.034411 0.011174 "
+ "0.044118 0.040892 0.004384 "
+ "-0.048695 0.033571 -0.013389 "
+ "-0.022206 0.038690 -0.017188 "
+ "0.002026 0.034165 0.004107 "
+ "-0.079652 0.070444 0.015237 "
+ "0.048793 0.042901 0.011011 "
+ "-0.089979 0.116378 0.044138 "
+ "-0.069850 0.033539 0.004727 "
+ "-0.029039 0.038221 0.002720 "
+ "-0.069196 0.034579 -0.006020 "
+ "0.005046 0.034177 0.015760 "
+ "0.034111 0.035601 0.011664 "
+ "-0.086781 0.115997 0.046920 "
+ "0.002619 0.034323 0.019675 "
+ "-0.051528 0.033550 0.007814 "
+ "-0.034901 0.033747 0.010379 "
+ "-0.030015 0.034351 0.018172 "
+ "-0.027929 0.037711 0.020035 "
+ "-0.072159 0.034912 0.007119 "
+ "0.019617 0.034742 0.021397 "
+ "-0.071684 0.035136 0.008802 "
+ "-0.071483 0.036208 0.009431 "
+ "-0.000694 0.038513 0.021743 "
+ "0.048576 0.046442 0.024984 "
+ "0.022319 0.035282 0.024092 "
+ "0.034684 0.038735 0.024611 "
+ "-0.070531 0.151779 -0.045373 "
+ "-0.066422 0.035267 0.015055 "
+ "-0.062077 0.034516 0.020294 "
+ "-0.063971 0.034365 0.016456 "
+ "-0.081219 0.078592 0.029019 "
+ "-0.051600 0.066088 0.036174 "
+ "0.031905 0.043740 0.029538 "
+ "-0.063341 0.035770 0.018750 "
+ "-0.004483 0.039218 0.035614 "
+ "-0.082655 0.076999 0.021106 "
+ "-0.055121 0.034056 0.024036 "
+ "0.009041 0.035141 0.035926 "
+ "0.012972 0.034672 0.029418 "
+ "-0.070622 0.146831 -0.025571 "
+ "-0.090089 0.112403 0.037453 "
+ "-0.066997 0.062934 -0.001956 "
+ "-0.035388 0.034596 0.034245 "
+ "0.007859 0.039240 0.030372 "
+ "-0.071388 0.147097 -0.026896 "
+ "-0.078147 0.116431 0.050321 "
+ "-0.070936 0.078069 0.038683 "
+ "-0.075074 0.077317 0.034901 "
+ "-0.016213 0.038057 0.016945 "
+ "-0.006919 0.034711 0.042546 "
+ "-0.050487 0.064658 0.035198 "
+ "-0.064600 0.034724 0.026530 "
+ "-0.031802 0.037839 0.032183 "
+ "0.004891 0.034575 0.043617 "
+ "-0.084324 0.077527 0.016962 "
+ "-0.091292 0.116401 0.042037 "
+ "-0.069946 0.064187 0.001256 "
+ "-0.073267 0.148476 -0.029258 "
+ "-0.084864 0.078984 0.019130 "
+ "-0.069386 0.061033 0.009766 "
+ "-0.036723 0.034210 0.026670 "
+ "-0.024903 0.039047 0.038063 "
+ "0.014685 0.037467 0.044223 "
+ "-0.079247 0.080308 0.033239 "
+ "-0.082419 0.078886 0.026767 "
+ "-0.091014 0.114490 0.034742 "
+ "-0.066996 0.034958 0.031010 "
+ "-0.067902 0.146313 -0.025723 "
+ "-0.053202 0.073352 0.041234 "
+ "-0.037419 0.034781 0.041799 "
+ "-0.012835 0.035252 0.050222 "
+ "0.009197 0.039194 0.045161 "
+ "-0.082296 0.077332 0.022849 "
+ "-0.061775 0.065542 0.032955 "
+ "-0.074682 0.066385 0.006255 "
+ "-0.051757 0.056601 0.019874 "
+ "-0.046877 0.035027 0.045407 "
+ "-0.070681 0.109318 0.037859 "
+ "-0.075092 0.066454 0.007232 "
+ "-0.074437 0.065821 0.007911 "
+ "0.004325 0.035606 0.045888 "
+ "-0.089199 0.115860 0.044703 "
+ "-0.066365 0.036009 0.037225 "
+ "-0.038999 0.036125 0.042901 "
+ "-0.072850 0.107163 0.037442 "
+ "-0.076428 0.117143 0.051891 "
+ "-0.079737 0.071741 0.005181 "
+ "-0.084462 0.111705 0.028857 "
+ "-0.060273 0.034788 0.045444 "
+ "-0.030763 0.035297 0.052008 "
+ "-0.082857 0.076402 0.018249 "
+ "-0.074827 0.110670 0.046334 "
+ "-0.079080 0.070406 0.008268 "
+ "-0.076864 0.067957 0.011082 "
+ "-0.069649 0.061738 0.018026 "
+ "-0.058329 0.059742 0.021921 "
+ "-0.075934 0.078899 0.035147 "
+ "-0.065150 0.036692 0.040216 "
+ "-0.069152 0.078835 0.040145 "
+ "-0.035996 0.151076 -0.002026 "
+ "-0.063069 0.150547 -0.026594 "
+ "-0.069062 0.149033 -0.037134 "
+ "-0.062215 0.166391 -0.060509 "
+ "-0.062798 0.167240 -0.060784 "
+ "-0.039381 0.154717 -0.008957 "
+ "-0.049932 0.141822 0.003858 "
+ "-0.064944 0.157314 -0.054480 "
+ "-0.069406 0.153071 -0.048274 "
+ "-0.044493 0.149423 -0.004750 "
+ "-0.056918 0.136838 -0.005280 "
+ "-0.064125 0.156393 -0.043120 "
+ "-0.071932 0.149607 -0.039423 "
+ "-0.009322 0.175020 -0.028678 "
+ "-0.060545 0.169731 -0.062221 "
+ "-0.068381 0.149044 -0.036363 "
+ "-0.050757 0.137975 0.001565 "
+ "-0.027652 0.155350 -0.003855 "
+ "-0.027938 0.155215 -0.003967 "
+ "-0.026306 0.157004 -0.009894 "
+ "-0.028940 0.154865 -0.002858 "
+ "-0.009716 0.176357 -0.030374 "
+ "-0.008588 0.169530 -0.025410 "
+ "-0.067979 0.154919 -0.051359 "
+ "-0.018495 0.165075 -0.017689 "
+ "-0.013589 0.169514 -0.023467 "
+ "-0.009085 0.173025 -0.027717 "
+ "-0.066223 0.155560 -0.052081 "
+ "-0.029333 0.154386 -0.003606 "
+ "-0.063933 0.153712 -0.036871 "
+ "-0.028864 0.154571 -0.003558 "
+ "-0.064135 0.160881 -0.057617 "
+ "-0.072553 0.150608 -0.041367 "
+ "-0.030385 0.154168 -0.002474 "
+ "-0.009715 0.177216 -0.029461 "
+ "-0.014691 0.160456 -0.011333 "
+ "-0.063650 0.159498 -0.051954 "
+ "-0.072177 0.146332 -0.024671 "
+ "-0.020377 0.158233 -0.007259 "
+ "-0.062349 0.141045 -0.006499 "
+ "-0.064196 0.142314 -0.007840 "
+ "-0.068829 0.148429 -0.034137 "
+ "-0.067194 0.151677 -0.045577 "
+ "-0.071411 0.144356 -0.015185 "
+ "-0.009825 0.166960 -0.022046 "
+ "-0.052896 0.143026 -0.000456 "
+ "-0.032692 0.152804 -0.002950 "
+ "-0.072038 0.149669 -0.039594 "
+ "-0.072282 0.143895 -0.012909 ";
+
+bool samePoints(const vpPoint &pt1, const vpPoint &pt2)
+{
return vpMath::equal(pt1.get_oX(), pt2.get_oX(), std::numeric_limits<double>::epsilon()) &&
- vpMath::equal(pt1.get_oY(), pt2.get_oY(), std::numeric_limits<double>::epsilon()) &&
- vpMath::equal(pt1.get_oZ(), pt2.get_oZ(), std::numeric_limits<double>::epsilon()) &&
- vpMath::equal(pt1.get_x(), pt2.get_x(), std::numeric_limits<double>::epsilon()) &&
- vpMath::equal(pt1.get_y(), pt2.get_y(), std::numeric_limits<double>::epsilon());
+ vpMath::equal(pt1.get_oY(), pt2.get_oY(), std::numeric_limits<double>::epsilon()) &&
+ vpMath::equal(pt1.get_oZ(), pt2.get_oZ(), std::numeric_limits<double>::epsilon()) &&
+ vpMath::equal(pt1.get_x(), pt2.get_x(), std::numeric_limits<double>::epsilon()) &&
+ vpMath::equal(pt1.get_y(), pt2.get_y(), std::numeric_limits<double>::epsilon());
}
-int checkInlierIndex(const std::vector<unsigned int> &vectorOfFoundInlierIndex, const std::vector<bool> &vectorOfOutlierFlags) {
+int checkInlierIndex(const std::vector<unsigned int> &vectorOfFoundInlierIndex,
+ const std::vector<bool> &vectorOfOutlierFlags)
+{
int nbInlierIndexOk = 0;
- for(std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
- it != vectorOfFoundInlierIndex.end(); ++it) {
- if(!vectorOfOutlierFlags[*it]) {
+ for (std::vector<unsigned int>::const_iterator it = vectorOfFoundInlierIndex.begin();
+ it != vectorOfFoundInlierIndex.end(); ++it) {
+ if (!vectorOfOutlierFlags[*it]) {
nbInlierIndexOk++;
}
}
@@ -1693,11 +1696,15 @@ int checkInlierIndex(const std::vector<unsigned int> &vectorOfFoundInlierIndex,
return nbInlierIndexOk;
}
-bool checkInlierPoints(const std::vector<vpPoint> &vectorOfFoundInlierPoints, const std::vector<unsigned int> &vectorOfFoundInlierIndex,
- const std::vector<vpPoint> &bunnyModelPoints_noisy) {
- for(size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) {
- if(!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) {
- std::cerr << "Problem with the inlier index and the corresponding inlier point !" << std::endl;
+bool checkInlierPoints(const std::vector<vpPoint> &vectorOfFoundInlierPoints,
+ const std::vector<unsigned int> &vectorOfFoundInlierIndex,
+ const std::vector<vpPoint> &bunnyModelPoints_noisy)
+{
+ for (size_t i = 0; i < vectorOfFoundInlierPoints.size(); i++) {
+ if (!samePoints(vectorOfFoundInlierPoints[i], bunnyModelPoints_noisy[vectorOfFoundInlierIndex[i]])) {
+ std::cerr << "Problem with the inlier index and the corresponding "
+ "inlier point !"
+ << std::endl;
return false;
}
}
@@ -1705,19 +1712,21 @@ bool checkInlierPoints(const std::vector<vpPoint> &vectorOfFoundInlierPoints, co
return true;
}
-void readBunnyModelPoints(std::vector<vpPoint> &bunnyModelPoints, std::vector<vpPoint> &bunnyModelPoints_noisy) {
- //Read the string
+void readBunnyModelPoints(std::vector<vpPoint> &bunnyModelPoints, std::vector<vpPoint> &bunnyModelPoints_noisy)
+{
+ // Read the string
std::istringstream iss_file_content(file_content);
-// vpTranslationVector translation(-0.1, 0.15, 1.0);
+ // vpTranslationVector translation(-0.1, 0.15, 1.0);
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(10.0f), vpMath::rad(-15.0f),
+ // vpMath::rad(38.0f));
vpRzyxVector zyxVector(vpMath::rad(12.4146f), vpMath::rad(-75.5478f), vpMath::rad(138.5607f));
vpThetaUVector thetau(zyxVector);
vpHomogeneousMatrix cMo_groundTruth(translation, thetau);
- vpGaussRand gaussian_noise(0.0002, 0.0/*, time(NULL)*/);
+ vpGaussRand gaussian_noise(0.0002, 0.0 /*, time(NULL)*/);
while (iss_file_content) {
double oX, oY, oZ;
iss_file_content >> oX >> oY >> oZ;
@@ -1726,22 +1735,24 @@ void readBunnyModelPoints(std::vector<vpPoint> &bunnyModelPoints, std::vector<vp
pt.project(cMo_groundTruth);
bunnyModelPoints.push_back(pt);
- //Add a small gaussian noise to data to be a little more realistic
+ // Add a small gaussian noise to data to be a little more realistic
pt.set_x(pt.get_x() + gaussian_noise());
pt.set_y(pt.get_y() + gaussian_noise());
bunnyModelPoints_noisy.push_back(pt);
}
- //Print the number of model points
+ // 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;
}
-bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std::vector<vpPoint> &bunnyModelPoints_noisy_original,
- const size_t nb_model_points, const bool test_duplicate, const bool test_degenerate) {
+bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original,
+ const std::vector<vpPoint> &bunnyModelPoints_noisy_original, const size_t nb_model_points,
+ const bool test_duplicate, const bool test_degenerate)
+{
std::vector<vpPoint> bunnyModelPoints = bunnyModelPoints_original;
std::vector<vpPoint> bunnyModelPoints_noisy = bunnyModelPoints_noisy_original;
- //Resize
+ // Resize
if (nb_model_points > 0) {
bunnyModelPoints.resize(nb_model_points);
bunnyModelPoints_noisy.resize(nb_model_points);
@@ -1758,7 +1769,7 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
double r_lagrange = real_pose.computeResidual(cMo_lagrange);
vpHomogeneousMatrix cMo_estimated;
- if(r_lagrange < r_dementhon) {
+ if (r_lagrange < r_dementhon) {
cMo_estimated = cMo_lagrange;
} else {
cMo_estimated = cMo_dementhon;
@@ -1769,23 +1780,22 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
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
+ 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<bool> vectorOfOutlierFlags(bunnyModelPoints_noisy.size(), false);
- //Generate outliers points
- for(size_t i = 0; i < nbOutliers; i++) {
+ // 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());
vectorOfOutlierFlags[i] = true;
}
if (test_duplicate) {
- //Add some duplicate points
+ // Add some duplicate points
size_t nbDuplicatePoints = 100;
for (size_t i = 0; i < nbDuplicatePoints; i++) {
- size_t index = (size_t) rand() % bunnyModelPoints_noisy.size();
+ size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
vpPoint duplicatePoint = bunnyModelPoints_noisy[index];
bunnyModelPoints_noisy.push_back(duplicatePoint);
vectorOfOutlierFlags.push_back(true);
@@ -1793,31 +1803,32 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
}
if (test_degenerate) {
- //Add some degenerate points
+ // Add some degenerate points
size_t nbDegeneratePoints = 100;
- double degenerate_tolerence = 9.999e-7; //1e-6 is used in the code to detect if a point is degenerate or not
+ double degenerate_tolerence = 9.999e-7; // 1e-6 is used in the code to
+ // detect if a point is degenerate
+ // or not
std::vector<vpPoint> listOfDegeneratePoints;
- for(size_t i = 0; i < nbDegeneratePoints; i++) {
- size_t index = (size_t) rand() % bunnyModelPoints_noisy.size();
+ for (size_t i = 0; i < nbDegeneratePoints; i++) {
+ size_t index = (size_t)rand() % bunnyModelPoints_noisy.size();
vpPoint degeneratePoint = bunnyModelPoints_noisy[index];
- //Object point is degenerate
+ // Object point is degenerate
degeneratePoint.set_oX(degeneratePoint.get_oX() + degenerate_tolerence);
degeneratePoint.set_oY(degeneratePoint.get_oY() + degenerate_tolerence);
degeneratePoint.set_oZ(degeneratePoint.get_oZ() - degenerate_tolerence);
- //Add duplicate 3D points
+ // Add duplicate 3D points
listOfDegeneratePoints.push_back(degeneratePoint);
-
- //Image point is degenerate
- index = (size_t) rand() % bunnyModelPoints_noisy.size();
+ // Image point is degenerate
+ index = (size_t)rand() % bunnyModelPoints_noisy.size();
degeneratePoint = bunnyModelPoints_noisy[index];
degeneratePoint.set_x(degeneratePoint.get_x() + degenerate_tolerence);
degeneratePoint.set_y(degeneratePoint.get_y() - degenerate_tolerence);
- //Add duplicate 2D points
+ // Add duplicate 2D points
listOfDegeneratePoints.push_back(degeneratePoint);
}
@@ -1828,9 +1839,9 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
}
}
- //Shuffle the data vector
+ // Shuffle the data vector
std::vector<size_t> vectorOfIndex(bunnyModelPoints_noisy.size());
- for(size_t i = 0; i < vectorOfIndex.size(); i++) {
+ for (size_t i = 0; i < vectorOfIndex.size(); i++) {
vectorOfIndex[i] = i;
}
std::random_shuffle(vectorOfIndex.begin(), vectorOfIndex.end());
@@ -1839,68 +1850,81 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
bunnyModelPoints_noisy.clear();
std::vector<bool> vectorOfOutlierFlags_tmp = vectorOfOutlierFlags;
vectorOfOutlierFlags.clear();
- for(std::vector<size_t>::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) {
+ for (std::vector<size_t>::const_iterator it = vectorOfIndex.begin(); it != vectorOfIndex.end(); ++it) {
bunnyModelPoints_noisy.push_back(bunnyModelPoints_noisy_tmp[*it]);
vectorOfOutlierFlags.push_back(vectorOfOutlierFlags_tmp[*it]);
}
- //Add data to vpPose
+ // Add data to vpPose
vpPose pose;
vpPose pose_ransac, pose_ransac2;
-#if defined (VISP_HAVE_PTHREAD) || (defined (_WIN32) && !defined(WINRT_8_0)) || defined (VISP_HAVE_OPENMP)
-# define TEST_PARALLEL_RANSAC
+#if defined(VISP_HAVE_PTHREAD) || (defined(_WIN32) && !defined(WINRT_8_0)) || defined(VISP_HAVE_OPENMP)
+#define TEST_PARALLEL_RANSAC
#endif
+ std::string appveyor_threading = "";
+ try {
+ appveyor_threading = vpIoTools::getenv("APPVEYOR_THREADING");
+ } catch (...) {
+ }
+
#ifdef TEST_PARALLEL_RANSAC
vpPose pose_ransac_parallel, pose_ransac_parallel2;
- pose_ransac_parallel.setUseParallelRansac(true);
- pose_ransac_parallel2.setUseParallelRansac(true);
+ if (appveyor_threading == "true") {
+ 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);
- pose_ransac_parallel2.setNbParallelRansacThreads(2);
+#if !defined(VISP_HAVE_OPENMP)
+ pose_ransac_parallel.setNbParallelRansacThreads(2);
+ pose_ransac_parallel2.setNbParallelRansacThreads(2);
#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<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin();
- it != bunnyModelPoints_noisy.end(); ++it) {
+ for (std::vector<vpPoint>::const_iterator it = bunnyModelPoints_noisy.begin(); it != bunnyModelPoints_noisy.end();
+ ++it) {
pose.addPoint(*it);
}
- //Test addPoints
+ // Test addPoints
pose_ransac.addPoints(bunnyModelPoints_noisy);
pose_ransac2.addPoints(bunnyModelPoints_noisy);
#ifdef TEST_PARALLEL_RANSAC
- pose_ransac_parallel.addPoints(bunnyModelPoints_noisy);
- pose_ransac_parallel2.addPoints(bunnyModelPoints_noisy);
+ if (appveyor_threading == "true") {
+ pose_ransac_parallel.addPoints(bunnyModelPoints_noisy);
+ pose_ransac_parallel2.addPoints(bunnyModelPoints_noisy);
+ }
#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::endl;
+ // 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::endl;
unsigned int nbInlierToReachConsensus = (unsigned int)(60.0 * (double)(bunnyModelPoints_noisy.size()) / 100.0);
double threshold = 0.001;
- //RANSAC with 1000 iterations
+ // RANSAC with 1000 iterations
pose_ransac.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
pose_ransac.setRansacThreshold(threshold);
pose_ransac.setRansacMaxTrials(1000);
#ifdef TEST_PARALLEL_RANSAC
- pose_ransac_parallel.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
- pose_ransac_parallel.setRansacThreshold(threshold);
- pose_ransac_parallel.setRansacMaxTrials(1000);
-
- pose_ransac_parallel2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
- pose_ransac_parallel2.setRansacThreshold(threshold);
- pose_ransac_parallel2.setRansacMaxTrials(vpPose::computeRansacIterations(0.99, 0.4, 4, -1));
+ if (appveyor_threading == "true") {
+ pose_ransac_parallel.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
+ pose_ransac_parallel.setRansacThreshold(threshold);
+ pose_ransac_parallel.setRansacMaxTrials(1000);
+
+ pose_ransac_parallel2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
+ pose_ransac_parallel2.setRansacThreshold(threshold);
+ pose_ransac_parallel2.setRansacMaxTrials(vpPose::computeRansacIterations(0.99, 0.4, 4, -1));
+ }
#endif
- //RANSAC with p=0.99, epsilon=0.4
+ // RANSAC with p=0.99, epsilon=0.4
pose_ransac2.setRansacNbInliersToReachConsensus(nbInlierToReachConsensus);
pose_ransac2.setRansacThreshold(threshold);
int ransac_iterations = vpPose::computeRansacIterations(0.99, 0.4, 4, -1);
@@ -1930,13 +1954,12 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
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);
r_dementhon = pose.computeResidual(cMo_dementhon);
r_lagrange = pose.computeResidual(cMo_lagrange);
- if(r_lagrange < r_dementhon) {
+ if (r_lagrange < r_dementhon) {
cMo_estimated = cMo_lagrange;
} else {
cMo_estimated = cMo_dementhon;
@@ -1949,52 +1972,59 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
std::cout << "Corresponding residual: " << r_estimated << std::endl;
#ifdef TEST_PARALLEL_RANSAC
- vpHomogeneousMatrix cMo_estimated_RANSAC_parallel;
- double t_RANSAC_parallel = vpTime::measureTimeMs();
- 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;
-
- double 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);
- t_RANSAC_parallel2 = vpTime::measureTimeMs() - t_RANSAC_parallel2;
-
- 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;
-
- 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;
+ double r_RANSAC_estimated_parallel = std::numeric_limits<double>::max();
+ 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);
+ 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;
+
+ 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);
+ t_RANSAC_parallel2 = vpTime::measureTimeMs() - t_RANSAC_parallel2;
+
+ 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;
+
+ 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
+ // Check inlier index
std::vector<unsigned int> vectorOfFoundInlierIndex = pose_ransac.getRansacInlierIndex();
int nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex, vectorOfOutlierFlags);
- int nbTrueInlierIndex = (int) std::count(vectorOfOutlierFlags.begin(), vectorOfOutlierFlags.end(), false);
+ 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
+ // Check inlier points returned
std::vector<vpPoint> vectorOfFoundInlierPoints = pose_ransac.getRansacInliers();
- if(vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
- std::cerr << "The number of inlier index is different with the number of inlier points !" << std::endl;
+ if (vectorOfFoundInlierPoints.size() != vectorOfFoundInlierIndex.size()) {
+ std::cerr << "The number of inlier index is different with the number of "
+ "inlier points !"
+ << std::endl;
return false;
}
if (!checkInlierPoints(vectorOfFoundInlierPoints, vectorOfFoundInlierIndex, bunnyModelPoints_noisy)) {
return false;
}
-
- //Check for RANSAC with p=0.99, epsilon=0.4
- //Check inlier index
+ // 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<unsigned int> vectorOfFoundInlierIndex_2 = pose_ransac2.getRansacInlierIndex();
nbInlierIndexOk = checkInlierIndex(vectorOfFoundInlierIndex_2, vectorOfOutlierFlags);
@@ -2002,72 +2032,84 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
std::cout << "There are " << nbInlierIndexOk << " true inliers found, " << vectorOfFoundInlierIndex_2.size()
<< " inliers returned and " << nbTrueInlierIndex << " true inliers." << std::endl;
- //Check inlier points returned
+ // Check inlier points returned
std::vector<vpPoint> 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;
+ 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)) {
return false;
}
-
#ifdef TEST_PARALLEL_RANSAC
- //Check for parallel RANSAC
- //Check inlier index
- std::cout << "\nCheck for parallel RANSAC (1000 iterations)" << std::endl;
- std::vector<unsigned int> 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;
-
- //Check inlier points returned
- std::vector<vpPoint> 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, 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<unsigned int> 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;
+ if (appveyor_threading == "true") {
+ // Check for parallel RANSAC
+ // Check inlier index
+ std::cout << "\nCheck for parallel RANSAC (1000 iterations)" << std::endl;
+ std::vector<unsigned int> 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;
+
+ // Check inlier points returned
+ std::vector<vpPoint> 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,
+ bunnyModelPoints_noisy)) {
+ return false;
+ }
- //Check inlier points returned
- std::vector<vpPoint> 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, 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<unsigned int> 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<vpPoint> 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,
+ bunnyModelPoints_noisy)) {
+ return false;
+ }
}
#endif
-
- if(r_RANSAC_estimated > threshold /*|| r_RANSAC_estimated_2 > threshold*/) {
+ 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;
} else {
#ifdef TEST_PARALLEL_RANSAC
- if (r_RANSAC_estimated_parallel > threshold) {
- 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 << "threshold=" << threshold << std::endl;
- return false;
+ if (appveyor_threading == "true") {
+ if (r_RANSAC_estimated_parallel > threshold) {
+ 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 << "threshold=" << threshold << std::endl;
+ return false;
+ }
}
- #endif
+#endif
std::cout << "The pose estimated with the RANSAC method is well estimated!" << std::endl;
}
@@ -2079,12 +2121,13 @@ bool testRansac(const std::vector<vpPoint> &bunnyModelPoints_original, const std
Compute the pose of a 3D object using the Ransac method.
*/
-int main() {
- //vpPose::computeRansacIterations
+int main()
+{
+ // vpPose::computeRansacIterations
int sample_sizes[] = {2, 3, 4, 5, 6, 7, 8};
double epsilon[] = {0.05, 0.1, 0.2, 0.25, 0.3, 0.4, 0.5};
- //Format output
+ // Format output
std::cout << " ";
for (int cpt2 = 0; cpt2 < 7; cpt2++) {
std::cout << std::setfill(' ') << std::setw(5) << epsilon[cpt2] << " ";
@@ -2118,7 +2161,9 @@ int main() {
readBunnyModelPoints(bunnyModelPoints, bunnyModelPoints_noisy_original);
for (int cpt = 0; cpt < nb; cpt++) {
- std::cout << "\n\n====================================================================" << std::endl;
+ std::cout << "\n\n====================================================="
+ "==============="
+ << std::endl;
if (model_sizes[cpt] == 0) {
std::cout << "Test on " << bunnyModelPoints_noisy_original.size() << " model points." << std::endl;
} else {
@@ -2126,15 +2171,15 @@ int main() {
}
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;
}
}
std::cout << "\n\ntestPoseRansac2 is ok!" << std::endl;
return EXIT_SUCCESS;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
diff --git a/modules/visual_features/CMakeLists.txt b/modules/visual_features/CMakeLists.txt
index 80092a0..211f9b2 100644
--- a/modules/visual_features/CMakeLists.txt
+++ b/modules/visual_features/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h
index 160c5f8..150e36c 100644
--- a/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h
+++ b/modules/visual_features/include/visp3/visual_features/vpBasicFeature.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,8 +37,6 @@
*
*****************************************************************************/
-
-
#ifndef vpBasicFeature_H
#define vpBasicFeature_H
@@ -46,15 +45,15 @@
\brief class that defines what is a visual feature
*/
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
// Display Issue
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
-//Color / image / display
+// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
@@ -70,7 +69,6 @@
// #define FEATURE_LINE7 0x40
// #define FEATURE_LINE8 0x80
-
/*!
\class vpBasicFeature
\ingroup group_core_features
@@ -79,86 +77,76 @@
class VISP_EXPORT vpBasicFeature
{
public: // Public constantes
- static const unsigned int FEATURE_LINE [32];
+ static const unsigned int FEATURE_LINE[32];
- enum {
- FEATURE_ALL = 0xffff
- };
+ enum { FEATURE_ALL = 0xffff };
/*!
\enum vpBasicFeatureDeallocatorType
Indicates who should deallocate the feature.
*/
- typedef enum
- {
- user,
- vpServo
- } vpBasicFeatureDeallocatorType;
+ typedef enum { user, vpServo } vpBasicFeatureDeallocatorType;
protected:
//! State of the visual feature.
- vpColVector s ;
+ vpColVector s;
//! Dimension of the visual feature.
- unsigned int dim_s ;
- //int featureLine[8] ;
- //! Ensure that all the parameters needed to compute the iteraction matrix are set.
+ unsigned int dim_s;
+ // int featureLine[8] ;
+ //! Ensure that all the parameters needed to compute the iteraction matrix
+ //! are set.
bool *flags;
//! Number of parameters needed to compute the interaction matrix.
unsigned int nbParameters;
public:
- vpBasicFeature() ;
- vpBasicFeature(const vpBasicFeature &f) ;
+ vpBasicFeature();
+ vpBasicFeature(const vpBasicFeature &f);
virtual ~vpBasicFeature();
/** @name Inherited functionalities from vpBasicFeature */
//@{
/*! Return the dimension of the feature vector \f$\bf s\f$. */
- unsigned int dimension_s() { return dim_s ; }
+ unsigned int dimension_s() { return dim_s; }
- virtual void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const = 0;
- virtual void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const = 0;
+ virtual void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color = vpColor::green, unsigned int thickness = 1) const = 0;
+ virtual void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const = 0;
- virtual void init() = 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;
- vpBasicFeatureDeallocatorType getDeallocate() { return deallocate ; }
+ vpColVector get_s(unsigned int select = FEATURE_ALL) const;
+ vpBasicFeatureDeallocatorType getDeallocate() { return deallocate; }
// Get the feature vector dimension.
- unsigned int getDimension(const unsigned int select=FEATURE_ALL) const;
+ unsigned int getDimension(const unsigned int select = FEATURE_ALL) const;
//! Compute the interaction matrix from a subset of the possible features.
virtual vpMatrix interaction(const unsigned int select = FEATURE_ALL) = 0;
//! Return element \e i in the state vector (usage : x = s[i] )
- virtual inline double operator[](const unsigned int i) const { return s[i]; }
- vpBasicFeature &operator=(const vpBasicFeature &f) ;
+ 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 ;
+ virtual void print(const unsigned int select = FEATURE_ALL) const = 0;
- virtual vpBasicFeature *duplicate() const = 0 ;
+ virtual vpBasicFeature *duplicate() const = 0;
- void setDeallocate(vpBasicFeatureDeallocatorType d) { deallocate = d ; }
+ void setDeallocate(vpBasicFeatureDeallocatorType d) { deallocate = d; }
void setFlags();
//@}
//! Select all the features.
- static unsigned int selectAll() { return FEATURE_ALL ; }
+ static unsigned int selectAll() { return FEATURE_ALL; }
protected:
void resetFlags();
protected:
- vpBasicFeatureDeallocatorType deallocate ;
-} ;
+ vpBasicFeatureDeallocatorType deallocate;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h
index c30ef6d..b09bc5b 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureBuilder.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -47,41 +48,40 @@
// tracker
#ifdef VISP_HAVE_MODULE_BLOB
-# include <visp3/blob/vpDot.h>
-# include <visp3/blob/vpDot2.h>
+#include <visp3/blob/vpDot.h>
+#include <visp3/blob/vpDot2.h>
#endif
#ifdef VISP_HAVE_MODULE_ME
-# include <visp3/me/vpMeLine.h>
-# include <visp3/me/vpMeEllipse.h>
+#include <visp3/me/vpMeEllipse.h>
+#include <visp3/me/vpMeLine.h>
#endif
// forward projection tracker
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpLine.h>
-#include <visp3/core/vpSphere.h>
#include <visp3/core/vpCircle.h>
#include <visp3/core/vpCylinder.h>
+#include <visp3/core/vpLine.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/core/vpSphere.h>
// visual feature
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/visual_features/vpFeaturePointPolar.h>
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
+#include <visp3/visual_features/vpFeatureLine.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/visual_features/vpFeaturePoint3D.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/visual_features/vpFeatureVanishingPoint.h>
-#include <visp3/visual_features/vpFeatureSegment.h>
// others
#include <visp3/core/vpImagePoint.h>
-//pixel / meter conversion
+// pixel / meter conversion
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpMeterPixelConversion.h>
-
+#include <visp3/core/vpPixelMeterConversion.h>
/*!
\class vpFeatureBuilder
@@ -92,94 +92,72 @@
class VISP_EXPORT vpFeatureBuilder
{
public:
- // create vpFeaturePoint feature
+// 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 vpPoint &p) ;
- static void create(vpFeaturePoint &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p) ;
+ static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t);
+ static void create(vpFeaturePoint &s, const vpPoint &p);
+ static void create(vpFeaturePoint &s, const vpCameraParameters &goodCam, const vpCameraParameters &wrongCam,
+ const vpPoint &p);
#ifdef VISP_HAVE_MODULE_BLOB
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2 ) ;
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2) ;
+ static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2);
+ static void create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2);
#endif
- static void create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpImagePoint &ip1, const vpImagePoint &ip2);
- static void create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2);
+ 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
+// 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 vpPoint &p) ;
- static void create(vpFeaturePointPolar &s,
- const vpCameraParameters &goodCam,
- const vpCameraParameters &wrongCam,
- const vpPoint &p) ;
+ static void create(vpFeaturePointPolar &s, const vpCameraParameters &cam, const vpImagePoint &iP);
+ static void create(vpFeaturePointPolar &s, const vpPoint &p);
+ static void create(vpFeaturePointPolar &s, const vpCameraParameters &goodCam, const vpCameraParameters &wrongCam,
+ const vpPoint &p);
// create vpFeaturePoint3D feature
- static void create(vpFeaturePoint3D &s, const vpPoint &p ) ;
+ static void create(vpFeaturePoint3D &s, const vpPoint &p);
// create vpFeatureLine feature
- static void create(vpFeatureLine &s, const vpLine &l ) ;
- static void create(vpFeatureLine &s, const vpCylinder &c, const int line) ;
+ static void create(vpFeatureLine &s, const vpLine &l);
+ static void create(vpFeatureLine &s, const vpCylinder &c, const int line);
#ifdef VISP_HAVE_MODULE_ME
- static void create(vpFeatureLine &s,
- const vpCameraParameters &cam,
- const vpMeLine &mel) ;
+ 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) ;
+ 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
/*!
- create vpFeatureVanishingPoint feature from the 2D coordinates of a point
- in the image plane
+ create vpFeatureVanishingPoint feature from the 2D coordinates of a
+ point in the image plane
*/
static void create(vpFeatureVanishingPoint &s, const vpPoint &p);
/*!
create vpFeatureVanishingPoint feature from 2 FeatureLine, ie lines in
the image plane (error if the 2 lines are parallel)
*/
- static void create(vpFeatureVanishingPoint &s, const vpFeatureLine &l1, const vpFeatureLine &l2 );
+ 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 fe579c8..ae9e49d 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureDepth.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureDepth_H
#define vpFeatureDepth_H
@@ -50,7 +50,6 @@
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeatureDepth
\ingroup group_visual_features
@@ -103,7 +102,7 @@ int main()
//Set the point feature thanks to the current parameters.
s.buildfrom(x, y, Z, log(Z/Zs));
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the desired visual features sd
@@ -115,10 +114,10 @@ int main()
// Control loop
for ( ; ; ) {
// The new parameters x, y and Z must be computed here.
-
+
// Update the current point visual feature
s.buildfrom(x, y, Z, log(Z/Zs));
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -127,14 +126,14 @@ int main()
\endcode
If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
build the interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeatureDepth.h>
-#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeatureDepth.h>
int main()
{
@@ -151,27 +150,29 @@ int main()
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.
+ vpColVector s_star(1); // The dimension is 1.
+ s_star(1) = 0; // The value of s* is 0.
s.error(s_star);
}
\endcode
*/
-
class VISP_EXPORT vpFeatureDepth : public vpBasicFeature
{
private:
- //! The \f$ x \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ //! The \f$ x \f$ 2D coordinate of the point in the camera frame (required
+ //! to compute the interaction matrix)
double x;
- //! The \f$ y \f$ 2D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ //! The \f$ y \f$ 2D coordinate of the point in the camera frame (required
+ //! to compute the interaction matrix)
double y;
- //! The \f$ Z \f$ 3D coordinate of the point in the camera frame (required to compute the interaction matrix)
+ //! The \f$ Z \f$ 3D coordinate of the point in the camera frame (required
+ //! to compute the interaction matrix)
double Z;
public:
- vpFeatureDepth() ;
+ vpFeatureDepth();
//! Destructor.
virtual ~vpFeatureDepth() {}
@@ -179,43 +180,37 @@ public:
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<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- vpFeatureDepth *duplicate() const ;
- vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL) ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ vpFeatureDepth *duplicate() const;
+ vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL);
- double get_x() const ;
+ double get_x() const;
- double get_y() const ;
+ double get_y() const;
- double get_Z() const ;
+ double get_Z() const;
- double get_LogZoverZstar() const ;
+ double get_LogZoverZstar() const;
- void init() ;
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void init();
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void print(const unsigned int select = FEATURE_ALL ) const ;
- void set_x(const double x) ;
+ void print(const unsigned int select = FEATURE_ALL) const;
+ void set_x(const double x);
- void set_y(const double y) ;
+ void set_y(const double y);
- void set_Z(const double Z) ;
+ void set_Z(const double Z);
void set_LogZoverZstar(const double LogZoverZstar);
- void set_xyZLogZoverZstar(const double x, const double y, const double Z, const double logZZs) ;
-} ;
-
-
+ 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 25f070c..44e702b 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureEllipse.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef vpFeatureEllipse_H
#define vpFeatureEllipse_H
@@ -51,7 +50,6 @@
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeatureEllipse
\ingroup group_visual_features
@@ -66,67 +64,58 @@ class VISP_EXPORT vpFeatureEllipse : public vpBasicFeature
private:
//! FeatureEllipse depth (required to compute the interaction matrix)
//! default Z = 1m
- double A,B,C ;
-
+ double A, B, C;
public:
//! Default constructor.
- vpFeatureEllipse() ;
+ vpFeatureEllipse();
//! Destructor.
- virtual ~vpFeatureEllipse() { }
+ virtual ~vpFeatureEllipse() {}
/*!
\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<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ 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<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
//! Feature duplication
- vpFeatureEllipse *duplicate() const ;
+ 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) ;
+ vpColVector error(const unsigned int select = FEATURE_ALL);
- double get_x() const {return s[0];}
- double get_y() const {return s[1];}
- double getMu20() const {return s[2];}
- double getMu11() const {return s[3];}
- double getMu02() const {return s[4];}
+ double get_x() const { return s[0]; }
+ double get_y() const { return s[1]; }
+ double getMu20() const { return s[2]; }
+ double getMu11() const { return s[3]; }
+ double getMu02() const { return s[4]; }
//! Default initialization.
- void init() ;
+ void init();
//! compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
//! print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
void set_x(const double x);
void set_y(const double y);
- void set_xy(const double x,const double y);
- void setABC(const double A, const double B, const double C) ;
- void setMu(const double mu20, const double mu11, const double mu02) ;
-
+ void set_xy(const double x, const double y);
+ void setABC(const double A, const double B, const double C);
+ void setMu(const double mu20, const double mu11, const double mu02);
+
public:
/*!
vpBasicFeature method instantiation
@@ -138,6 +127,6 @@ public:
static unsigned int selectMu20();
static unsigned int selectMu11();
static unsigned int selectMu02();
-} ;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h
index e2bc45e..23e329c 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureException.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,65 +36,65 @@
*
*****************************************************************************/
-
#ifndef __vpFeatureException_H
#define __vpFeatureException_H
-
-/* ------------------------------------------------------------------------- */
-/* --- INCLUDE ------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
-
+/* -------------------------------------------------------------------------
+ */
+/* --- INCLUDE -------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/* \file vpFeatureException.h
\brief error that can be emited by the vpFeature class and its derivates
*/
/* Classes standards. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpFeatureException
\ingroup group_visual_features
- \brief Error that can be emited by the vpBasicFeature class and its derivates.
+ \brief Error that can be emited by the vpBasicFeature class and its
+ derivates.
*/
class VISP_EXPORT vpFeatureException : public vpException
{
- public:
- /*!
- \brief Lists the possible error than can be emmited while calling
- vpFeature member
- */
- enum errorFeatureCodeEnum
- {
- //! feature list or desired feature list is empty
- badErrorVectorError,
- sizeMismatchError,
- notInitializedError,
- badInitializationError
- } ;
-
- public:
- vpFeatureException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpFeatureException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpFeatureException (const int id)
- : vpException(id){ ; }
+public:
+ /*!
+ \brief Lists the possible error than can be emmited while calling
+ vpFeature member
+ */
+ enum errorFeatureCodeEnum {
+ //! feature list or desired feature list is empty
+ badErrorVectorError,
+ sizeMismatchError,
+ notInitializedError,
+ badInitializationError
+ };
+public:
+ vpFeatureException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpFeatureException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpFeatureException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h
index 6a3bc4a..83ed430 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLine.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureLine_H
#define vpFeatureLine_H
@@ -47,11 +47,9 @@
#include <visp3/core/vpMatrix.h>
#include <visp3/visual_features/vpBasicFeature.h>
-
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeatureLine
\ingroup group_visual_features
@@ -128,7 +126,7 @@ int main()
//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.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the desired visual features sd
@@ -140,10 +138,10 @@ int main()
// Control loop
for ( ; ; ) {
// The new parameters rho and theta must be computed here.
-
+
// Update the current line visual feature
s.buildfrom(rho, theta);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -151,11 +149,14 @@ int main()
}
\endcode
- If you want to build your own control law, this other example shows how to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D line visual feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
+ If you want to build your own control law, this other example shows how to
+create a current (\f$s\f$) and desired (\f$s^*\f$) 2D line visual feature,
+compute the corresponding error vector \f$(s-s^*)\f$ and finally build the
+interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeatureLine.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeatureLine.h>
int main()
{
@@ -164,10 +165,7 @@ int main()
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;
+ 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);
@@ -203,60 +201,48 @@ class VISP_EXPORT vpFeatureLine : public vpBasicFeature
private:
//! FeatureLine depth (required to compute the interaction matrix)
//! equation of a plane
- double A,B,C,D ;
+ double A, B, C, D;
public:
- vpFeatureLine() ;
+ vpFeatureLine();
//! Destructor.
virtual ~vpFeatureLine() {}
-
// 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 display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- vpFeatureLine *duplicate() const ;
-
- vpColVector error(const vpBasicFeature &s_star,
- const unsigned int select = FEATURE_ALL) ;
- //vpColVector error(const int select = FEATURE_ALL) ;
+ 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 display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ vpFeatureLine *duplicate() const;
+
+ vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL);
+ // vpColVector error(const int select = FEATURE_ALL) ;
/*!
Return the \f$ \rho \f$ subset value of the visual feature \f$ s \f$.
*/
- double getRho() const { return s[0] ; }
+ double getRho() const { return s[0]; }
/*!
Return the \f$ \theta \f$ subset value of the visual feature \f$ s \f$.
*/
- double getTheta() const { return s[1] ; }
-
- void init() ;
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ double getTheta() const { return s[1]; }
- void print(const unsigned int select= FEATURE_ALL) const ;
+ void init();
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void setRhoTheta(const double rho, const double theta) ;
- void setABCD(const double A, const double B,
- const double C, const double D) ;
+ 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);
public:
-
static unsigned int selectRho();
static unsigned int selectTheta();
-} ;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h
index 122ca3c..cb64bb4 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureLuminance.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,10 +39,9 @@
#ifndef vpFeatureLuminance_h
#define vpFeatureLuminance_h
+#include <visp3/core/vpImage.h>
#include <visp3/core/vpMatrix.h>
#include <visp3/visual_features/vpBasicFeature.h>
-#include <visp3/core/vpImage.h>
-
/*!
\file vpFeatureLuminance.h
@@ -60,12 +60,11 @@
*/
class VISP_EXPORT vpLuminance
{
- public:
+public:
double x, y; // point coordinates (in meter)
- double I ; // pixel intensity
- double Ix,Iy ; // pixel gradient
- double Z; // pixel depth
-
+ double I; // pixel intensity
+ double Ix, Iy; // pixel gradient
+ double Z; // pixel depth
};
#endif
@@ -79,64 +78,58 @@ class VISP_EXPORT vpLuminance
class VISP_EXPORT vpFeatureLuminance : public vpBasicFeature
{
- protected:
+protected:
//! FeaturePoint depth (required to compute the interaction matrix)
//! default Z = 1m
- double Z ;
+ double Z;
//! Number of rows.
- unsigned int nbr ;
+ unsigned int nbr;
//! Number of column.
- unsigned int nbc ;
+ unsigned int nbc;
//! Border size.
- unsigned int bord ;
-
- //! Store the image (as a vector with intensity and gradient I, Ix, Iy)
- vpLuminance *pixInfo ;
- int firstTimeIn ;
-
- public:
- vpFeatureLuminance() ;
- vpFeatureLuminance(const vpFeatureLuminance& f) ;
- //! Destructor.
- virtual ~vpFeatureLuminance() ;
+ unsigned int bord;
- void buildFrom(vpImage<unsigned char> &I) ;
+ //! Store the image (as a vector with intensity and gradient I, Ix, Iy)
+ vpLuminance *pixInfo;
+ int firstTimeIn;
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
+public:
+ vpFeatureLuminance();
+ vpFeatureLuminance(const vpFeatureLuminance &f);
+ //! Destructor.
+ virtual ~vpFeatureLuminance();
- vpFeatureLuminance *duplicate() const ;
+ void buildFrom(vpImage<unsigned char> &I);
- 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) ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ vpFeatureLuminance *duplicate() const;
- double get_Z() const ;
+ 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);
- void init() ;
- void init(unsigned int _nbr, unsigned int _nbc, double _Z) ;
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void interaction(vpMatrix &L);
+ double get_Z() const;
- vpFeatureLuminance &operator=(const vpFeatureLuminance& f) ;
+ void init();
+ void init(unsigned int _nbr, unsigned int _nbc, double _Z);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void interaction(vpMatrix &L);
- void print(const unsigned int select = FEATURE_ALL ) const ;
+ vpFeatureLuminance &operator=(const vpFeatureLuminance &f);
- void setCameraParameters(vpCameraParameters &_cam) ;
- void set_Z(const double Z) ;
+ void print(const unsigned int select = FEATURE_ALL) const;
+ void setCameraParameters(vpCameraParameters &_cam);
+ void set_Z(const double Z);
- public:
- vpCameraParameters cam ;
-} ;
+public:
+ vpCameraParameters cam;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h
index 246ed17..6136e38 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMoment.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -38,15 +39,16 @@
\file vpFeatureMoment.h
\brief Base class for moment features.
-Handles common system operations like selection, duplication. Functionality is computed in derived classes.
+Handles common system operations like selection, duplication. Functionality is
+computed in derived classes.
*/
#ifndef __FEATUREMOMENT_H__
#define __FEATUREMOMENT_H__
+#include <vector>
#include <visp3/core/vpConfig.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpException.h>
-#include <vector>
+#include <visp3/visual_features/vpBasicFeature.h>
class vpMomentObject;
class vpMomentDatabase;
@@ -58,46 +60,64 @@ class vpMoment;
\ingroup group_visual_features
-\brief This class defines shared system methods/attributes for 2D moment features but no functional code.
-It is used to compute interaction matrices for moment features.
+\brief This class defines shared system methods/attributes for 2D moment
+features but no functional code. It is used to compute interaction matrices
+for moment features.
-While vpMoment-type classes do only compute moment values and can by used for almost anything, vpFeatureMoment-type classes
-are specifically designed for visual servoing. More importantly, a vpFeatureMoment is used to compute the interaction matrix associated to it's moment primitive.
+While vpMoment-type classes do only compute moment values and can by used for
+almost anything, vpFeatureMoment-type classes are specifically designed for
+visual servoing. More importantly, a vpFeatureMoment is used to compute the
+interaction matrix associated to it's moment primitive.
-This class is virtual and cannot be used directly. It defines the following characteristics common to all moment features:
+This class is virtual and cannot be used directly. It defines the following
+characteristics common to all moment features:
- Plane orientation parameters (A,B,C):
-Each camera frame corresponds to a physical planar object contained in a plane. This plane's equation has the following form:
-\f$ A \times x+B \times y + C = \frac{1}{Z} \f$.
-These parameters can be updated anytime.
-- Get corresponding moment primitive: for example a vpFeatureMomentCInvariant will provide access to a vpMomentCInvariant instance.
+Each camera frame corresponds to a physical planar object contained in a
+plane. This plane's equation has the following form: \f$ A \times x+B \times y
++ C = \frac{1}{Z} \f$. These parameters can be updated anytime.
+- Get corresponding moment primitive: for example a vpFeatureMomentCInvariant
+will provide access to a vpMomentCInvariant instance.
- Provide access to a feature database (vpFeatureMomentDatabase).
-- All interaction matrices (different from vpBasicFeature::interaction which selects the required interaction matrix).
-
-Like vpMoment, vpFeatureMoment provides a vpFeatureMoment::update() method. But unlike vpMoment::update() which only acknowledges the new object,
-the vpFeatureMoment::update() acknowledges the new plane parameters AND computes the interaction matrices associated with the feature.
-
-A vpFeatureMoment will be often part of a vpFeatureMomentDatabase in the same way a vpMoment is part of a vpMomentDatabase. This database is specified inside the
-vpFeatureMoment::vpFeatureMoment() constructor.
-As a result, a vpFeatureMoment will be able to access other vpFeatureMoments through this database.
-
-A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
-is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
-
-Note that you can use vpFeatureMoment to do visual servoing but it is not it's only purpose. You may compute your interaction matrices with vpFeatureMoment::update()
-and use them for any purpose.
-
-\attention - A vpFeatureMoment is not responsible for updating the moment primitives it depends on. Make sure your vpMoments are all up to date before computing an interaction matrix using vpFeatureMoment.
-
-\attention - Be careful with orders. Often, computing a feature of order n requires vpMoment primitives of order n+1.
-Make sure to check the documentation of the specialised vpFeatureMoment classes when deciding to which order you want to initialize the object.
-An object of order 6 should be sufficient for all classic implementations of vpFeatureMoment.
-
-Here is an example of how to use a vpFeatureMoment (in this case vpFeatureMomentBasic).
+- All interaction matrices (different from vpBasicFeature::interaction which
+selects the required interaction matrix).
+
+Like vpMoment, vpFeatureMoment provides a vpFeatureMoment::update() method.
+But unlike vpMoment::update() which only acknowledges the new object, the
+vpFeatureMoment::update() acknowledges the new plane parameters AND computes
+the interaction matrices associated with the feature.
+
+A vpFeatureMoment will be often part of a vpFeatureMomentDatabase in the same
+way a vpMoment is part of a vpMomentDatabase. This database is specified
+inside the vpFeatureMoment::vpFeatureMoment() constructor. As a result, a
+vpFeatureMoment will be able to access other vpFeatureMoments through this
+database.
+
+A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that
+case, all data in the vpBasicFeature is copied but the feature's name is lost.
+For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will
+be operational but could not be used in a vpFeatureMomentDatabase.
+
+Note that you can use vpFeatureMoment to do visual servoing but it is not it's
+only purpose. You may compute your interaction matrices with
+vpFeatureMoment::update() and use them for any purpose.
+
+\attention - A vpFeatureMoment is not responsible for updating the moment
+primitives it depends on. Make sure your vpMoments are all up to date before
+computing an interaction matrix using vpFeatureMoment.
+
+\attention - Be careful with orders. Often, computing a feature of order n
+requires vpMoment primitives of order n+1. Make sure to check the
+documentation of the specialised vpFeatureMoment classes when deciding to
+which order you want to initialize the object. An object of order 6 should be
+sufficient for all classic implementations of vpFeatureMoment.
+
+Here is an example of how to use a vpFeatureMoment (in this case
+vpFeatureMomentBasic).
\code
-#include <visp3/core/vpPoint.h>
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentBasic.h>
#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPoint.h>
#include <visp3/visual_features/vpFeatureMoment.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
@@ -112,9 +132,9 @@ int main()
vec_p.push_back(p);
//////////////////////////////REFERENCE VALUES////////////////////////////////
- vpMomentObject obj(3); // Init object of order 3 because we need
- // vpFeatureMomentBasic of order 2 which
+ // 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);
@@ -139,10 +159,10 @@ int main()
class VISP_EXPORT vpFeatureMoment : public vpBasicFeature
{
protected:
- const vpMoment* moment;
- const vpMoment& getMoment() const {return *moment;}
- vpMomentDatabase& moments;
- vpFeatureMomentDatabase* featureMomentsDataBase;
+ const vpMoment *moment;
+ const vpMoment &getMoment() const { return *moment; }
+ vpMomentDatabase &moments;
+ vpFeatureMomentDatabase *featureMomentsDataBase;
std::vector<vpMatrix> interaction_matrices;
double A;
@@ -150,71 +170,76 @@ protected:
double C;
char _name[255];
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpFeatureMoment(const vpFeatureMoment &fm)
-// : vpBasicFeature(), moment(NULL), moments(fm.moments), featureMomentsDataBase(NULL),
-// interaction_matrices(), A(0), B(0), C(0)
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpFeatureMoment &operator=(const vpFeatureMoment &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpFeatureMoment(const vpFeatureMoment &fm)
+ // : vpBasicFeature(), moment(NULL), moments(fm.moments),
+ // featureMomentsDataBase(NULL),
+ // interaction_matrices(), A(0), B(0), C(0)
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpFeatureMoment &operator=(const vpFeatureMoment &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
/*!
- Initializes the feature with information about the database of moment primitives, the object plane, feature database and matrix size.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database
- \param nbmatrices : If you want to create a new vpFeatureMoment implementation, your feature will often have a matrix size of n lines. You can specify the number of lines by this parameter.
+ Initializes the feature with information about the database of moment
+ primitives, the object plane, feature database and matrix size. \param
+ data_base : Moment database. The database of moment primitives (first
+ parameter) is mandatory. It is used to access different moment values later
+ used to compute the final matrix. \param A_ : Plane coefficient in a \f$ A
+ \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param B_ : Plane
+ coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
+ \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param featureMoments : Feature database \param
+ nbmatrices : If you want to create a new vpFeatureMoment implementation,
+ your feature will often have a matrix size of n lines. You can specify the
+ number of lines by this parameter.
*/
- vpFeatureMoment(vpMomentDatabase& data_base,double A_=0.0, double B_=0.0, double C_=0.0,
- vpFeatureMomentDatabase* featureMoments=NULL,unsigned int nbmatrices=1)
- : vpBasicFeature(), moment(NULL),
- moments(data_base),
- featureMomentsDataBase(featureMoments),
- interaction_matrices(nbmatrices),
- A(A_),B(B_),C(C_)
- {}
+ 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()
+ {
+ }
virtual ~vpFeatureMoment();
/** @name Inherited functionalities from vpFeatureMoment */
//@{
- virtual void compute_interaction (void);
- vpBasicFeature* duplicate () const;
- void display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
- void display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
- const vpColor &color=vpColor::green, unsigned int thickness=1) const ;
-
- int getDimension (unsigned int select=FEATURE_ALL) const;
- void init (void);
- vpMatrix interaction (const unsigned int select=FEATURE_ALL) ;
- void linkTo(vpFeatureMomentDatabase& featureMoments);
+ virtual void compute_interaction(void);
+ vpBasicFeature *duplicate() const;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+
+ int getDimension(unsigned int select = FEATURE_ALL) const;
+ void init(void);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void linkTo(vpFeatureMomentDatabase &featureMoments);
/*!
- Name of the moment corresponding to the feature. This allows to locate the moment
- associated with the feature in the provided database.
+ Name of the moment corresponding to the feature. This allows to locate
+ the moment associated with the feature in the provided database.
*/
- virtual const char* momentName() const = 0;
+ virtual const char *momentName() const = 0;
/*!
Name of the feature used to locate it in the database of features.
*/
- virtual const char* name() const = 0;
- void print (const unsigned int select=FEATURE_ALL) const ;
- virtual void printDependencies(std::ostream& os) const;
+ virtual const char *name() const = 0;
+ void print(const unsigned int select = FEATURE_ALL) const;
+ virtual void printDependencies(std::ostream &os) const;
- void update (double A, double B, double C);
+ 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);
};
/*!
@@ -222,28 +247,36 @@ public:
\ingroup group_visual_features
-\brief This class defines a generic feature used for moment feature duplication.
+\brief This class defines a generic feature used for moment feature
+duplication.
-A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that case, all data in the vpBasicFeature is copied but the feature's name
-is lost. For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will be operational but could not be used in a vpFeatureMomentDatabase.
-The reason for this is that a vpMomentGenericFeature can refer to anything therefore it has no specific name.
+A vpBasicFeature can be duplicated into a vpMomentGenericFeature. In that
+case, all data in the vpBasicFeature is copied but the feature's name is lost.
+For example if a vpFeatureMomentCInvariant is duplicated, the duplicata will
+be operational but could not be used in a vpFeatureMomentDatabase. The reason
+for this is that a vpMomentGenericFeature can refer to anything therefore it
+has no specific name.
Duplication is mostly used internally in ViSP.
*/
-class VISP_EXPORT vpMomentGenericFeature : public vpFeatureMoment{
+class VISP_EXPORT vpMomentGenericFeature : public vpFeatureMoment
+{
public:
- vpMomentGenericFeature(vpMomentDatabase& data_base,double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments, const vpMoment* p_moment)
- : vpFeatureMoment(data_base,A_,B_,C_,featureMoments){this->moment = p_moment;}
+ vpMomentGenericFeature(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments, const vpMoment *p_moment)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments)
+ {
+ this->moment = p_moment;
+ }
/*!
No specific moment name.
*/
- const char* momentName() const { return NULL;}
+ const char *momentName() const { return NULL; }
/*!
No specific feature name.
*/
- virtual const char* name() const { return NULL;}
+ virtual const char *name() const { return NULL; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h
index 4e205a0..3a56c75 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAlpha.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,31 +37,36 @@
*****************************************************************************/
/*!
\file vpFeatureMomentAlpha.h
- \brief Implementation of the interaction matrix computation for vpMomentAlpha.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentAlpha.
*/
#ifndef __FEATUREMOMENTALPHA_H__
#define __FEATUREMOMENTALPHA_H__
-#include <visp3/visual_features/vpFeatureMoment.h>
#include <visp3/core/vpColVector.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
#ifdef VISP_MOMENTS_COMBINE_MATRICES
-
class vpMomentDatabase;
/*!
\class vpFeatureMomentAlpha
\ingroup group_visual_features
- \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$. Computes the interaction matrix associated with vpMomentAlpha.
+ \brief Functionality computation for in-plane rotation moment feature \f$
+ \alpha \f$. Computes the interaction matrix associated with vpMomentAlpha.
The interaction matrix for the feature can be deduced from \cite Tahri05z.
- This class allows to compute the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
+ This class allows to compute the interaction matrix associated to \f$ \alpha
+ = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment
+ primitive.
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
+ The interaction matrix computed is single-dimension (no selection possible)
+ and can be obtained by calling vpFeatureMomentAlpha::interaction().
- This feature is often used in moment-based visual servoing to control the planar rotation parameter.
+ This feature is often used in moment-based visual servoing to control the
+ planar rotation parameter.
Minimum vpMomentObject order needed to compute this feature: 4.
@@ -68,30 +74,36 @@ class vpMomentDatabase;
- vpMomentCentered
- vpFeatureMomentCentered.
*/
-class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
+class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param moments : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentAlpha(vpMomentDatabase &moments, double A, double B, double C,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(moments, A, B, C, featureMoments, 1)
+ {
+ }
+
+ void compute_interaction();
+ /*!
+ associated moment name
*/
- vpFeatureMomentAlpha(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(moments,A,B,C,featureMoments,1)
- {}
-
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAlpha";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAlpha";}
+ const char *momentName() const { return "vpMomentAlpha"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentAlpha"; }
};
#else
class vpMomentDatabase;
@@ -100,9 +112,11 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for in-plane rotation moment feature \f$ \alpha \f$: computes the interaction matrix associated with vpMomentAlpha.
+ \brief Functionality computation for in-plane rotation moment feature \f$
+\alpha \f$: computes the interaction matrix associated with vpMomentAlpha.
- This class computes the interaction matrix associated to \f$ \alpha = \frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
+ This class computes the interaction matrix associated to \f$ \alpha =
+\frac{1}{2} arctan(\frac{2\mu_{11}}{\mu_{20}-\mu_{02}}) \f$ moment primitive.
The interaction matrix for the feature has the following form:
\f[{
@@ -123,7 +137,8 @@ _{{1,1}}}^{2} \right) +\gamma\,y_{{g}}\mu_{{1,1}} \left( \mu_{{2,0}}+
2} \right) }{d}}\\ \noalign{\medskip}-1\end {array} \right]
}^t
\f]
-with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} = DA^2+4{\mu_{1,1}}^2\f$.
+with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} =
+DA^2+4{\mu_{1,1}}^2\f$.
- In the discrete case:
\f$beta = 4\f$,\f$gamma = 2\f$.
@@ -131,9 +146,11 @@ with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} = DA^2+4{\mu_{1,1}}
\f$beta = 5\f$,\f$gamma = 1\f$.
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentAlpha::interaction().
+ The interaction matrix computed is single-dimension (no selection possible)
+and can be obtained by calling vpFeatureMomentAlpha::interaction().
- This feature is often used in moment-based visual servoing to control the planar rotation parameter.
+ This feature is often used in moment-based visual servoing to control the
+planar rotation parameter.
Minimum vpMomentObject order needed to compute this feature: 4.
@@ -141,32 +158,38 @@ with \f${\it DA} = \mu_{{2,0}}-\mu_{{0,2}}\f$ and \f${\it d} = DA^2+4{\mu_{1,1}}
- vpMomentCentered
- vpMomentGravityCenter
*/
-class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
+class VISP_EXPORT vpFeatureMomentAlpha : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentAlpha(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1)
+ {
+ }
+
+ void compute_interaction();
+ /*!
+ Associated moment name.
*/
- vpFeatureMomentAlpha(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1)
- {}
-
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentAlpha";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentAlpha";}
-
- vpColVector error (const vpBasicFeature &s_star, const unsigned int select=FEATURE_ALL);
+ const char *momentName() const { return "vpMomentAlpha"; }
+ /*!
+ Feature name.
+ */
+ const char *name() const { return "vpFeatureMomentAlpha"; }
+
+ vpColVector error(const vpBasicFeature &s_star, const unsigned int select = FEATURE_ALL);
};
#endif
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h
index 5b29d2e..caf9127 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentArea.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentArea.h
- \brief Implementation of the interaction matrix computation for vpMomentArea.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentArea.
*/
#ifndef __FEATUREMOMENTAREA_H__
#define __FEATUREMOMENTAREA_H__
@@ -49,33 +51,40 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Surface moment feature. Computes the interaction matrix associated with vpMomentArea.
+ \brief Surface moment feature. Computes the interaction matrix associated
+ with vpMomentArea.
*/
-class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
+class VISP_EXPORT vpFeatureMomentArea : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
- */
- vpFeatureMomentArea(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
-
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentArea";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentArea";}
+ */
+ vpFeatureMomentArea(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentArea"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentArea"; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h
index 3776ded..2271f44 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentAreaNormalized.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentAreaNormalized.h
- \brief Implementation of the interaction matrix computation for vpMomentAreaNormalized.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentAreaNormalized.
*/
#ifndef __FEATUREMOMENTAREANORMALIZED_H__
#define __FEATUREMOMENTAREANORMALIZED_H__
@@ -49,18 +51,25 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
+ \brief Functionality computation for normalized surface moment feature.
+ Computes the interaction matrix associated with vpMomentAreaNormalized.
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z.
+ The interaction matrix for the moment feature can be deduced from \cite
+ Tahri05z.
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ a_n \f$.
+ To do so, one must derive it and obtain a combination of interaction
+ matrices by using (1). It allows to compute the interaction matrix for \f$
+ a_n \f$.
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
+ The interaction matrix computed is single-dimension (no selection possible)
+ and can be obtained by calling
+ vpFeatureMomentGravityCenterNormalized::interaction.
- This feature is often used in moment-based visual servoing to control the depth parameter.
+ This feature is often used in moment-based visual servoing to control the
+ depth parameter.
- Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
+ Minimum vpMomentObject order needed to compute this feature: 1 in dense mode
+ and 3 in discrete mode.
This feature depends on:
- vpMomentCentered
@@ -69,29 +78,35 @@ class vpMomentDatabase;
- vpFeatureMomentBasic
*/
-class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAreaNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,1){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAreaNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAreaNormalized";}
-
+class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param database : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentAreaNormalized(vpMomentDatabase &database, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(database, A_, B_, C_, featureMoments, 1)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentAreaNormalized"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentAreaNormalized"; }
};
#else
@@ -102,7 +117,8 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for normalized surface moment feature. Computes the interaction matrix associated with vpMomentAreaNormalized.
+ \brief Functionality computation for normalized surface moment feature.
+Computes the interaction matrix associated with vpMomentAreaNormalized.
The interaction matrix for the moment has the following form:
- In the discrete case:
@@ -137,14 +153,19 @@ with:
- \f$e_{i,j}=\frac{\mu_{i,j}}{NA}\f$
- \f$NA=\mu_{2,0}+\mu_{0,2}\f$
- \f$\eta\f$ is the centered and normalized moment.
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ a_n \f$.
+ To do so, one must derive it and obtain a combination of interaction
+matrices by using (1). It allows to compute the interaction matrix for \f$ a_n
+\f$.
- The interaction matrix computed is single-dimension (no selection possible) and can be obtained by calling vpFeatureMomentGravityCenterNormalized::interaction.
+ The interaction matrix computed is single-dimension (no selection possible)
+and can be obtained by calling
+vpFeatureMomentGravityCenterNormalized::interaction.
- This feature is often used in moment-based visual servoing to control the depth parameter.
+ This feature is often used in moment-based visual servoing to control the
+depth parameter.
- Minimum vpMomentObject order needed to compute this feature: 1 in dense mode and 3 in discrete mode.
+ Minimum vpMomentObject order needed to compute this feature: 1 in dense mode
+and 3 in discrete mode.
This feature depends on:
- vpMomentCentered
@@ -152,29 +173,35 @@ with:
- vpMomentGravityCenter
*/
-class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentAreaNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,1){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentAreaNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentAreaNormalized";}
-
+class VISP_EXPORT vpFeatureMomentAreaNormalized : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentAreaNormalized(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 1)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentAreaNormalized"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentAreaNormalized"; }
};
#endif
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h
index e76652a..059e41e 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentBasic.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,59 +37,70 @@
*****************************************************************************/
/*!
\file vpFeatureMomentBasic.h
- \brief Implementation of the interaction matrix computation for vpMomentBasic.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentBasic.
*/
#ifndef __FEATUREMOMENTBASIC_H__
#define __FEATUREMOMENTBASIC_H__
-#include <visp3/visual_features/vpFeatureMoment.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
class vpMomentDatabase;
/*!
\class vpFeatureMomentBasic
\ingroup group_visual_features
- \brief Functionality computation for basic moment feature. Computes the interaction matrix associated with vpMomentBasic.
+ \brief Functionality computation for basic moment feature. Computes the
+ interaction matrix associated with vpMomentBasic.
- The interaction matrix for the basic moment feature is defined in \cite Tahri05z, equation (13).
- This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
- The interaction matrix \f$ L_{m_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ m_{ij} \f$ obtained by vpMomentBasic::get (i,j).
- vpFeatureMomentBasic computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
- \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
- For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
- \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
+ The interaction matrix for the basic moment feature is defined in
+ \cite Tahri05z, equation (13). This vpFeatureMoment, as well as it's
+ corresponding moment primitive is double-indexed. The interaction matrix \f$
+ L_{m_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction
+ (i,j) and is associated to \f$ m_{ij} \f$ obtained by vpMomentBasic::get
+ (i,j). vpFeatureMomentBasic computes interaction matrices all interaction
+ matrices up to vpMomentObject::getOrder()-1. \attention The maximum order
+ reached by vpFeatureMomentBasic is NOT the maximum order of the
+ vpMomentObject, it is one unit smaller. For example if you define your
+ vpMomentObject up to order n then vpFeatureMomentBasic will be able to
+ compute interaction matrices up to order n-1 that is \f$ L_{m_{ij}} \f$ with
+ \f$ i+j<=n-1 \f$.
- You can see an example of vpFeatureMomentBasic by looking at the documentation of the vpFeatureMoment class.
+ You can see an example of vpFeatureMomentBasic by looking at the
+ documentation of the vpFeatureMoment class.
This feature depends on:
- vpMomentBasic
*/
-class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment{
+class VISP_EXPORT vpFeatureMomentBasic : public vpFeatureMoment
+{
protected:
- unsigned int order;
- public:
- vpFeatureMomentBasic(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
- void compute_interaction();
+ unsigned int order;
+
+public:
+ vpFeatureMomentBasic(vpMomentDatabase &moments, double A, double B, double C,
+ vpFeatureMomentDatabase *featureMoments = NULL);
+ void compute_interaction();
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /* Add function due to pure virtual definition in vpBasicFeature.h */
- vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
+ /* Add function due to pure virtual definition in vpBasicFeature.h */
+ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */)
+ {
+ throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+ }
#endif
- vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentBasic";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentBasic";}
-
+ vpMatrix interaction(unsigned int select_one, unsigned int select_two) const;
+ /*!
+ Associated moment name.
+ */
+ const char *momentName() const { return "vpMomentBasic"; }
+ /*!
+ Feature name.
+ */
+ const char *name() const { return "vpFeatureMomentBasic"; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h
index 59e69f4..9f1f343 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCInvariant.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentCInvariant.h
- \brief Implementation of the interaction matrix computation for vpMomentCInvariant.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentCInvariant.
*/
#ifndef __FEATUREMOMENTCINVARIANT_H__
#define __FEATUREMOMENTCINVARIANT_H__
@@ -47,24 +49,36 @@
\ingroup group_visual_features
- \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
+ \brief Functionality computation for 2D rotation/translation/scale
+ non-symmetric invariant moment feature. Computes the interaction matrix
+ associated with vpMomentCInvariant.
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
- To do so, one must derive them and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
+ The interaction matrix for the moment feature can be deduced from \cite
+ Tahri05z, equations (9). To do so, one must derive them and obtain a
+ combination of interaction matrices by using (1). It allows to compute the
+ interaction matrix for \f$ c_i, i \in [1..10] \f$.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
- The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
- For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
- Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
- Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
+ These interaction matrices may be selected afterwards by calling
+ vpFeatureMomentCInvariant::interaction(). The selection by the
+ vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$. For example,
+ to select \f$ L_{c_1} \f$ you should input
+ vpFeatureMomentCInvariant::selectC1() into ViSP's selector. Special matrices
+ for features \f$ S_x \f$ and \f$ S_y \f$ are selected by
+ vpFeatureMomentCInvariant::selectSx() and
+ vpFeatureMomentCInvariant::selectSy() respectively. Special matrices for
+ features \f$ P_x \f$ and \f$ P_y \f$ are selected by
+ vpFeatureMomentCInvariant::selectPx() and
+ vpFeatureMomentCInvariant::selectPy() respectively.
- These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
+ These features are often used in moment-based visual servoing to control the
+ two out-of-plane rotations.
- Be careful about the nature of your object when selecting the right features.
- Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
+ Be careful about the nature of your object when selecting the right
+ features. Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing
+ with a symmetric object all other features otherwise.
- Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
+ Minimum vpMomentObject order needed to compute this feature: 6. This is the
+ highest ordrer required by classic features.
This feature depends on:
- vpMomentCentered
@@ -72,89 +86,96 @@
- vpMomentCInvariant
- vpFeatureMomentBasic
- An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
+ An example of how to use vpFeatureMomentCInvariant in a complete visual
+ servoing example is given in vpFeatureMomentCommon.
*/
-class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param moments : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
+class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param moments : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+ */
+ vpFeatureMomentCInvariant(vpMomentDatabase &moments, double A, double B, double C,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(moments, A, B, C, featureMoments, 16)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
*/
- vpFeatureMomentCInvariant(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(moments,A,B,C,featureMoments,16){}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCInvariant";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCInvariant";}
-
- /*!
- Shortcut selector for \f$C_1\f$.
- */
- static unsigned int selectC1(){ return 1 << 0; }
- /*!
- Shortcut selector for \f$C_2\f$.
- */
- static unsigned int selectC2(){ return 1 << 1; }
- /*!
- Shortcut selector for \f$C_3\f$.
- */
- static unsigned int selectC3(){ return 1 << 2; }
- /*!
- Shortcut selector for \f$C_4\f$.
- */
- static unsigned int selectC4(){ return 1 << 3; }
- /*!
- Shortcut selector for \f$C_5\f$.
- */
- static unsigned int selectC5(){ return 1 << 4; }
- /*!
- Shortcut selector for \f$C_6\f$.
- */
- static unsigned int selectC6(){ return 1 << 5; }
- /*!
- Shortcut selector for \f$C_7\f$.
- */
- static unsigned int selectC7(){ return 1 << 6; }
- /*!
- Shortcut selector for \f$C_8\f$.
- */
- static unsigned int selectC8(){ return 1 << 7; }
- /*!
- Shortcut selector for \f$C_9\f$.
- */
- static unsigned int selectC9(){ return 1 << 8; }
- /*!
- Shortcut selector for \f$C_{10}\f$.
- */
- static unsigned int selectC10(){ return 1 << 9; }
- /*!
- Shortcut selector for \f$S_x\f$.
- */
- static unsigned int selectSx(){ return 1 << 10; }
- /*!
- Shortcut selector for \f$S_y\f$.
- */
- static unsigned int selectSy(){ return 1 << 11; }
- /*!
- Shortcut selector for \f$P_x\f$.
- */
- static unsigned int selectPx(){ return 1 << 12; }
- /*!
- Shortcut selector for \f$P_y\f$.
- */
- static unsigned int selectPy(){ return 1 << 13; }
+ const char *momentName() const { return "vpMomentCInvariant"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentCInvariant"; }
+ /*!
+ Shortcut selector for \f$C_1\f$.
+ */
+ static unsigned int selectC1() { return 1 << 0; }
+ /*!
+ Shortcut selector for \f$C_2\f$.
+ */
+ static unsigned int selectC2() { return 1 << 1; }
+ /*!
+ Shortcut selector for \f$C_3\f$.
+ */
+ static unsigned int selectC3() { return 1 << 2; }
+ /*!
+ Shortcut selector for \f$C_4\f$.
+ */
+ static unsigned int selectC4() { return 1 << 3; }
+ /*!
+ Shortcut selector for \f$C_5\f$.
+ */
+ static unsigned int selectC5() { return 1 << 4; }
+ /*!
+ Shortcut selector for \f$C_6\f$.
+ */
+ static unsigned int selectC6() { return 1 << 5; }
+ /*!
+ Shortcut selector for \f$C_7\f$.
+ */
+ static unsigned int selectC7() { return 1 << 6; }
+ /*!
+ Shortcut selector for \f$C_8\f$.
+ */
+ static unsigned int selectC8() { return 1 << 7; }
+ /*!
+ Shortcut selector for \f$C_9\f$.
+ */
+ static unsigned int selectC9() { return 1 << 8; }
+ /*!
+ Shortcut selector for \f$C_{10}\f$.
+ */
+ static unsigned int selectC10() { return 1 << 9; }
+ /*!
+ Shortcut selector for \f$S_x\f$.
+ */
+ static unsigned int selectSx() { return 1 << 10; }
+ /*!
+ Shortcut selector for \f$S_y\f$.
+ */
+ static unsigned int selectSy() { return 1 << 11; }
+ /*!
+ Shortcut selector for \f$P_x\f$.
+ */
+ static unsigned int selectPx() { return 1 << 12; }
+ /*!
+ Shortcut selector for \f$P_y\f$.
+ */
+ static unsigned int selectPy() { return 1 << 13; }
};
#else
@@ -165,24 +186,36 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for 2D rotation/translation/scale non-symmetric invariant moment feature. Computes the interaction matrix associated with vpMomentCInvariant.
+ \brief Functionality computation for 2D rotation/translation/scale
+ non-symmetric invariant moment feature. Computes the interaction matrix
+ associated with vpMomentCInvariant.
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equations (9).
- To do so, one must derive them and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrix for \f$ c_i, i \in [1..10] \f$.
+ The interaction matrix for the moment feature can be deduced from
+ \cite Tahri05z, equations (9). To do so, one must derive them and obtain a
+ combination of interaction matrices by using (1). It allows to compute the
+ interaction matrix for \f$ c_i, i \in [1..10] \f$.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentCInvariant::interaction().
- The selection by the vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$.
- For example, to select \f$ L_{c_1} \f$ you should input vpFeatureMomentCInvariant::selectC1() into ViSP's selector.
- Special matrices for features \f$ S_x \f$ and \f$ S_y \f$ are selected by vpFeatureMomentCInvariant::selectSx() and vpFeatureMomentCInvariant::selectSy() respectively.
- Special matrices for features \f$ P_x \f$ and \f$ P_y \f$ are selected by vpFeatureMomentCInvariant::selectPx() and vpFeatureMomentCInvariant::selectPy() respectively.
+ These interaction matrices may be selected afterwards by calling
+ vpFeatureMomentCInvariant::interaction(). The selection by the
+ vpFeatureMomentCInvariant::selectCi method for \f$ L_{c_i} \f$. For example,
+ to select \f$ L_{c_1} \f$ you should input
+ vpFeatureMomentCInvariant::selectC1() into ViSP's selector. Special matrices
+ for features \f$ S_x \f$ and \f$ S_y \f$ are selected by
+ vpFeatureMomentCInvariant::selectSx() and
+ vpFeatureMomentCInvariant::selectSy() respectively. Special matrices for
+ features \f$ P_x \f$ and \f$ P_y \f$ are selected by
+ vpFeatureMomentCInvariant::selectPx() and
+ vpFeatureMomentCInvariant::selectPy() respectively.
- These features are often used in moment-based visual servoing to control the two out-of-plane rotations.
+ These features are often used in moment-based visual servoing to control the
+ two out-of-plane rotations.
- Be careful about the nature of your object when selecting the right features.
- Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing with a symmetric object all other features otherwise.
+ Be careful about the nature of your object when selecting the right
+ features. Use \f$ L_{S_{x}} \f$ and \f$ L_{S_{y}} \f$ when you're dealing
+ with a symmetric object all other features otherwise.
- Minimum vpMomentObject order needed to compute this feature: 6. This is the highest ordrer required by classic features.
+ Minimum vpMomentObject order needed to compute this feature: 6. This is the
+ highest ordrer required by classic features.
This feature depends on:
- vpMomentCentered
@@ -190,100 +223,107 @@ class vpMomentDatabase;
- vpMomentCInvariant
- vpFeatureMomentBasic
- An example of how to use vpFeatureMomentCInvariant in a complete visual servoing example is given in vpFeatureMomentCommon.
+ An example of how to use vpFeatureMomentCInvariant in a complete visual
+ servoing example is given in vpFeatureMomentCommon.
*/
-class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment{
- private:
- std::vector<vpMatrix> LI;
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
+class VISP_EXPORT vpFeatureMomentCInvariant : public vpFeatureMoment
+{
+private:
+ std::vector<vpMatrix> LI;
- */
- vpFeatureMomentCInvariant(vpMomentDatabase& data_base,double A_, double B_, double C_, vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(data_base,A_,B_,C_,featureMoments,16), LI(16)
- {}
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCInvariant";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCInvariant";}
+ */
+ vpFeatureMomentCInvariant(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 16), LI(16)
+ {
+ }
- /*!
- Shortcut selector for \f$C_1\f$.
- */
- static unsigned int selectC1(){ return 1 << 0; }
- /*!
- Shortcut selector for \f$C_2\f$.
- */
- static unsigned int selectC2(){ return 1 << 1; }
- /*!
- Shortcut selector for \f$C_3\f$.
- */
- static unsigned int selectC3(){ return 1 << 2; }
- /*!
- Shortcut selector for \f$C_4\f$.
- */
- static unsigned int selectC4(){ return 1 << 3; }
- /*!
- Shortcut selector for \f$C_5\f$.
- */
- static unsigned int selectC5(){ return 1 << 4; }
- /*!
- Shortcut selector for \f$C_6\f$.
- */
- static unsigned int selectC6(){ return 1 << 5; }
- /*!
- Shortcut selector for \f$C_7\f$.
- */
- static unsigned int selectC7(){ return 1 << 6; }
- /*!
- Shortcut selector for \f$C_8\f$.
- */
- static unsigned int selectC8(){ return 1 << 7; }
- /*!
- Shortcut selector for \f$C_9\f$.
- */
- static unsigned int selectC9(){ return 1 << 8; }
- /*!
- Shortcut selector for \f$C_{10}\f$.
- */
- static unsigned int selectC10(){ return 1 << 9; }
- /*!
- Shortcut selector for \f$S_x\f$.
- */
- static unsigned int selectSx(){ return 1 << 10; }
- /*!
- Shortcut selector for \f$S_y\f$.
- */
- static unsigned int selectSy(){ return 1 << 11; }
- /*!
- Shortcut selector for \f$P_x\f$.
- */
- static unsigned int selectPx(){ return 1 << 12; }
- /*!
- Shortcut selector for \f$P_y\f$.
- */
- static unsigned int selectPy(){ return 1 << 13; }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentCInvariant"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentCInvariant"; }
- /*!
- Print all the interaction matrices of the moment invariants
- */
- void printLsofInvariants(std::ostream& os) const;
+ /*!
+ Shortcut selector for \f$C_1\f$.
+ */
+ static unsigned int selectC1() { return 1 << 0; }
+ /*!
+ Shortcut selector for \f$C_2\f$.
+ */
+ static unsigned int selectC2() { return 1 << 1; }
+ /*!
+ Shortcut selector for \f$C_3\f$.
+ */
+ static unsigned int selectC3() { return 1 << 2; }
+ /*!
+ Shortcut selector for \f$C_4\f$.
+ */
+ static unsigned int selectC4() { return 1 << 3; }
+ /*!
+ Shortcut selector for \f$C_5\f$.
+ */
+ static unsigned int selectC5() { return 1 << 4; }
+ /*!
+ Shortcut selector for \f$C_6\f$.
+ */
+ static unsigned int selectC6() { return 1 << 5; }
+ /*!
+ Shortcut selector for \f$C_7\f$.
+ */
+ static unsigned int selectC7() { return 1 << 6; }
+ /*!
+ Shortcut selector for \f$C_8\f$.
+ */
+ static unsigned int selectC8() { return 1 << 7; }
+ /*!
+ Shortcut selector for \f$C_9\f$.
+ */
+ static unsigned int selectC9() { return 1 << 8; }
+ /*!
+ Shortcut selector for \f$C_{10}\f$.
+ */
+ static unsigned int selectC10() { return 1 << 9; }
+ /*!
+ Shortcut selector for \f$S_x\f$.
+ */
+ static unsigned int selectSx() { return 1 << 10; }
+ /*!
+ Shortcut selector for \f$S_y\f$.
+ */
+ static unsigned int selectSy() { return 1 << 11; }
+ /*!
+ Shortcut selector for \f$P_x\f$.
+ */
+ static unsigned int selectPx() { return 1 << 12; }
+ /*!
+ Shortcut selector for \f$P_y\f$.
+ */
+ static unsigned int selectPy() { return 1 << 13; }
- friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCInvariant& featcinv);
+ /*!
+ Print all the interaction matrices of the moment invariants
+ */
+ void printLsofInvariants(std::ostream &os) const;
+ 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 4f74ee6..2a2f1cd 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCentered.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentCentered.h
- \brief Implementation of the interaction matrix computation for vpMomentCentered.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentCentered.
*/
#ifndef __FEATUREMOMENTCENTERED_H__
#define __FEATUREMOMENTCENTERED_H__
@@ -48,63 +50,71 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for centered moment feature. Computes the interaction matrix associated with vpMomentCentered.
+ \brief Functionality computation for centered moment feature. Computes the
+ interaction matrix associated with vpMomentCentered.
- The interaction matrix for the feature is defined in \cite Tahri05z, equation (17).
- This vpFeatureMoment, as well as it's corresponding moment primitive is double-indexed.
- The interaction matrix \f$ L_{\mu_{ij}} \f$ is obtained by calling vpFeatureMomentBasic::interaction (i,j) and is associated to \f$ \mu_{ij} \f$ obtained by vpFeatureMomentCentered::get (i,j).
+ The interaction matrix for the feature is defined in \cite Tahri05z,
+ equation (17). This vpFeatureMoment, as well as it's corresponding moment
+ primitive is double-indexed. The interaction matrix \f$ L_{\mu_{ij}} \f$ is
+ obtained by calling vpFeatureMomentBasic::interaction (i,j) and is
+ associated to \f$ \mu_{ij} \f$ obtained by vpFeatureMomentCentered::get
+ (i,j).
- vpFeatureMomentCentered computes interaction matrices all interaction matrices up to vpMomentObject::getOrder()-1.
- \attention The maximum order reached by vpFeatureMomentBasic is NOT the maximum order of the vpMomentObject, it is one unit smaller.
- For example if you define your vpMomentObject up to order n then vpFeatureMomentBasic will be able to compute interaction matrices up to order n-1 that is
- \f$ L_{m_{ij}} \f$ with \f$ i+j<=n-1 \f$.
+ vpFeatureMomentCentered computes interaction matrices all interaction
+ matrices up to vpMomentObject::getOrder()-1. \attention The maximum order
+ reached by vpFeatureMomentBasic is NOT the maximum order of the
+ vpMomentObject, it is one unit smaller. For example if you define your
+ vpMomentObject up to order n then vpFeatureMomentBasic will be able to
+ compute interaction matrices up to order n-1 that is \f$ L_{m_{ij}} \f$ with
+ \f$ i+j<=n-1 \f$.
This feature depends on:
- vpFeatureMomentBasic
- vpFeatureMomentGravityCenter
- vpMomentGravityCenter
*/
-class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment{
+class VISP_EXPORT vpFeatureMomentCentered : public vpFeatureMoment
+{
protected:
-
- unsigned int order;
- /*!
- Core computation of interaction matrix for moment m_pq
- */
- vpMatrix
- compute_Lmu_pq(const unsigned int& p, const unsigned int& q, const double& xg, const double& yg,
- const vpMatrix& L_xg, const vpMatrix& L_yg,
- const vpMomentBasic& m, const vpFeatureMomentBasic& feature_moment_m) const;
+ unsigned int order;
+ /*!
+ Core computation of interaction matrix for moment m_pq
+ */
+ vpMatrix compute_Lmu_pq(const unsigned int &p, const unsigned int &q, const double &xg, const double &yg,
+ const vpMatrix &L_xg, const vpMatrix &L_yg, const vpMomentBasic &m,
+ const vpFeatureMomentBasic &feature_moment_m) const;
- public:
- vpFeatureMomentCentered(vpMomentDatabase& moments,double A, double B, double C,vpFeatureMomentDatabase* featureMoments=NULL);
- void compute_interaction();
+public:
+ vpFeatureMomentCentered(vpMomentDatabase &moments, double A, double B, double C,
+ vpFeatureMomentDatabase *featureMoments = NULL);
+ void compute_interaction();
#ifndef DOXYGEN_SHOULD_SKIP_THIS
- /* Add function due to pure virtual definition in vpBasicFeature.h */
- vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */){
- throw vpException(vpException::functionNotImplementedError,"Not implemented!");
- }
+ /* Add function due to pure virtual definition in vpBasicFeature.h */
+ vpMatrix interaction(const unsigned int /* select = FEATURE_ALL */)
+ {
+ throw vpException(vpException::functionNotImplementedError, "Not implemented!");
+ }
#endif
- /*!
- Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
- \param select_one : first index (i)
- \param select_two : second index (j)
- \return Interaction matrix corresponding to the moment
- */
- vpMatrix interaction (unsigned int select_one,unsigned int select_two) const;
+ /*!
+ Interaction matrix corresponding to \f$ \mu_{ij} \f$ moment
+ \param select_one : first index (i)
+ \param select_two : second index (j)
+ \return Interaction matrix corresponding to the moment
+ */
+ vpMatrix interaction(unsigned int select_one, unsigned int select_two) const;
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentCentered";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentCentered";}
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentCentered"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentCentered"; }
-friend VISP_EXPORT std::ostream & operator<<(std::ostream & os, const vpFeatureMomentCentered& v);
+ 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 b137020..366bdbd 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentCommon.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,7 +29,8 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Pre-filled pseudo-database used to handle dependencies between common moment features.
+ * Pre-filled pseudo-database used to handle dependencies between common
+ *moment features.
*
* Authors:
* Filip Novotny
@@ -37,21 +39,22 @@
/*!
\file vpFeatureMomentCommon.h
- \brief Pre-filled pseudo-database used to handle dependencies between common moment features.
+ \brief Pre-filled pseudo-database used to handle dependencies between common
+ moment features.
*/
#ifndef __FEATUREMOMENTCOMMON_H__
#define __FEATUREMOMENTCOMMON_H__
#include <visp3/visual_features/vpFeatureMomentAlpha.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentArea.h>
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
-#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
-#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <visp3/visual_features/vpFeatureMomentArea.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
class vpMomentDatabase;
class vpServo;
@@ -60,7 +63,8 @@ class vpServo;
\ingroup group_visual_features
- \brief This class allows to access common vpFeatureMoments in a pre-filled database.
+ \brief This class allows to access common vpFeatureMoments in a pre-filled
+database.
It is a vpMomentDatabase filled with the following moments:
- vpFeatureMomentGravityCenter
@@ -72,23 +76,28 @@ class vpServo;
- vpFeatureMomentBasic
- There is no need to do the linkTo operations manually nor is it necessary to care about the order of feature computation.
+ There is no need to do the linkTo operations manually nor is it necessary to
+care about the order of feature computation.
- This class has an vpMomentCommon::updateAll method capable of updating the plane parameters AND computing interaction matrices inside the features.
+ This class has an vpMomentCommon::updateAll method capable of updating the
+plane parameters AND computing interaction matrices inside the features.
The moment features computed by this class are classical moments
features used in moment-based visual servoing. For more
information see \cite Tahri05z.
- To initialize this feature set, the user needs to supply a vpMomentDatabase containing at least the contents of vpMomentCommon.
+ To initialize this feature set, the user needs to supply a vpMomentDatabase
+containing at least the contents of vpMomentCommon.
- The features can be retrieved like from a normal vpFeatureMomentDatabase. However, some shortcuts to retrieve the features are provided.
+ The features can be retrieved like from a normal vpFeatureMomentDatabase.
+However, some shortcuts to retrieve the features are provided.
- \attention Make sure your object is at least of order 6 when using this pre-filled database.
-
- The following code demonstrates the construction of a 6x6 interaction matrix as described in [1].
- \code
+ \attention Make sure your object is at least of order 6 when using this
+pre-filled database.
+ The following code demonstrates the construction of a 6x6 interaction matrix
+as described in [1].
+\code
#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentCommon.h>
#include <visp3/core/vpPoint.h>
@@ -214,54 +223,55 @@ Gain : Zero= 1 Inf= 1 Deriv= 0
\endcode
*/
-class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase{
+class VISP_EXPORT vpFeatureMomentCommon : public vpFeatureMomentDatabase
+{
private:
- vpFeatureMomentGravityCenter featureGravity;
- vpFeatureMomentGravityCenterNormalized featureGravityNormalized;
- vpFeatureMomentAreaNormalized featureAn;
- vpFeatureMomentCInvariant featureCInvariant;
- vpFeatureMomentAlpha featureAlpha;
- vpFeatureMomentCentered featureCentered;
- vpFeatureMomentBasic featureMomentBasic;
- vpFeatureMomentArea feature_moment_area;
+ vpFeatureMomentGravityCenter featureGravity;
+ vpFeatureMomentGravityCenterNormalized featureGravityNormalized;
+ vpFeatureMomentAreaNormalized featureAn;
+ vpFeatureMomentCInvariant featureCInvariant;
+ vpFeatureMomentAlpha featureAlpha;
+ vpFeatureMomentCentered featureCentered;
+ vpFeatureMomentBasic featureMomentBasic;
+ vpFeatureMomentArea feature_moment_area;
public:
- vpFeatureMomentCommon(vpMomentDatabase& moments,double A=0.0,double B=0.0,double C=1.0);
- void updateAll(double A,double B,double C);
- /*!
- Returns alpha.
- */
- vpFeatureMomentAlpha& getFeatureAlpha(){ return featureAlpha;}
-
- /*!
- Returns normalized surface.
- */
- vpFeatureMomentAreaNormalized& getFeatureAn(){ return featureAn;}
- /*!
- Returns basic moment.
- */
- vpFeatureMomentBasic& getFeatureMomentBasic(){ return featureMomentBasic;}
- /*!
- Returns centered moments.
- */
- vpFeatureMomentCentered& getFeatureCentered(){ return featureCentered;}
-
- /*!
- Returns non-symmetric invariants.
- */
- vpFeatureMomentCInvariant& getFeatureCInvariant(){ return featureCInvariant;}
- /*!
- Returns normalized gravity center.
- */
- vpFeatureMomentGravityCenterNormalized& getFeatureGravityNormalized(){ return featureGravityNormalized;}
- /*!
- Returns the area
- */
- vpFeatureMomentArea& getFeatureArea(){ return feature_moment_area;}
- /*!
- Returns gravity center
- */
- vpFeatureMomentGravityCenter& getFeatureGravityCenter(){ return featureGravity;}
+ vpFeatureMomentCommon(vpMomentDatabase &moments, double A = 0.0, double B = 0.0, double C = 1.0);
+ void updateAll(double A, double B, double C);
+ /*!
+ Returns alpha.
+ */
+ vpFeatureMomentAlpha &getFeatureAlpha() { return featureAlpha; }
+
+ /*!
+ Returns normalized surface.
+ */
+ vpFeatureMomentAreaNormalized &getFeatureAn() { return featureAn; }
+ /*!
+ Returns basic moment.
+ */
+ vpFeatureMomentBasic &getFeatureMomentBasic() { return featureMomentBasic; }
+ /*!
+ Returns centered moments.
+ */
+ vpFeatureMomentCentered &getFeatureCentered() { return featureCentered; }
+
+ /*!
+ Returns non-symmetric invariants.
+ */
+ vpFeatureMomentCInvariant &getFeatureCInvariant() { return featureCInvariant; }
+ /*!
+ Returns normalized gravity center.
+ */
+ vpFeatureMomentGravityCenterNormalized &getFeatureGravityNormalized() { return featureGravityNormalized; }
+ /*!
+ Returns the area
+ */
+ vpFeatureMomentArea &getFeatureArea() { return feature_moment_area; }
+ /*!
+ Returns gravity center
+ */
+ vpFeatureMomentGravityCenter &getFeatureGravityCenter() { return featureGravity; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h
index 5089d55..cc6d8c5 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentDatabase.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,10 +44,10 @@
#ifndef __FEATUREMOMENTDATABASE_H__
#define __FEATUREMOMENTDATABASE_H__
-#include <visp3/core/vpConfig.h>
-#include <map>
-#include <iostream>
#include <cstring>
+#include <iostream>
+#include <map>
+#include <visp3/core/vpConfig.h>
class vpFeatureMoment;
class vpMomentObject;
@@ -55,17 +56,26 @@ class vpMomentObject;
\ingroup group_visual_features
- \brief This class allows to register all feature moments (implemented in vpFeatureMoment... classes) so they can access each other according to their dependencies.
-
- Like moments (implemented in vpMoment... classes), a vpFeatureMoment needs to have access to other vpFeatureMoment's values to be computed.
- In most cases, a vpFeatureMoment needs both: vpMoments and vpFeatureMoments which explains the two databases (see vpFeatureMoment::vpFeatureMoment).
- For example vpFeatureMomentAlpha needs additionnal information about centered moments vpMomentCentered AND their interaction matrices obtained
- by vpFeatureMomentCentered in order to compute the moment's value from a vpMomentObject.
- Like the vpMomentCentered is stored in a vpMomentDatabase, the vpFeatureMomentCentered should be stored in a vpFeatureMomentDatabase.
-
- All moment features in a database can access each other freely at any time. They can also verify if a moment feature is present in the database or not.
- This code illustrates the use of both databases to handle dependencies between moment primitives and moment features:
- \code
+ \brief This class allows to register all feature moments (implemented in
+vpFeatureMoment... classes) so they can access each other according to their
+dependencies.
+
+ Like moments (implemented in vpMoment... classes), a vpFeatureMoment needs
+to have access to other vpFeatureMoment's values to be computed. In most
+cases, a vpFeatureMoment needs both: vpMoments and vpFeatureMoments which
+explains the two databases (see vpFeatureMoment::vpFeatureMoment). For example
+vpFeatureMomentAlpha needs additionnal information about centered moments
+vpMomentCentered AND their interaction matrices obtained by
+vpFeatureMomentCentered in order to compute the moment's value from a
+vpMomentObject. Like the vpMomentCentered is stored in a vpMomentDatabase, the
+vpFeatureMomentCentered should be stored in a vpFeatureMomentDatabase.
+
+ All moment features in a database can access each other freely at any time.
+They can also verify if a moment feature is present in the database or not.
+ This code illustrates the use of both databases to handle dependencies
+between moment primitives and moment features:
+
+\code
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpMomentObject.h>
@@ -146,17 +156,17 @@ int main()
}
\endcode
*/
-class VISP_EXPORT vpFeatureMomentDatabase{
- private:
+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<const char*,vpFeatureMoment*,cmp_str> featureMomentsDataBase;
- void add(vpFeatureMoment& featureMoment,char* name);
- public:
+ std::map<const char *, vpFeatureMoment *, cmp_str> featureMomentsDataBase;
+ void add(vpFeatureMoment &featureMoment, char *name);
+
+public:
/*!
Default constructor.
*/
@@ -165,11 +175,12 @@ class VISP_EXPORT vpFeatureMomentDatabase{
Virtual destructor that does nothing.
*/
virtual ~vpFeatureMomentDatabase() {}
- virtual void updateAll(double A=0.0, double B=0.0, double C=1.0);
+ virtual void updateAll(double A = 0.0, double B = 0.0, double C = 1.0);
- vpFeatureMoment& get(const char* type, bool& found);
+ vpFeatureMoment &get(const char *type, bool &found);
- //friend VISP_EXPORT std::ostream & operator<<(std::ostream& os, const vpFeatureMomentDatabase& m);
+ // friend VISP_EXPORT std::ostream & operator<<(std::ostream& os, const
+ // vpFeatureMomentDatabase& m);
friend class vpFeatureMoment;
};
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h
index 5fe2fbb..3734c23 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenter.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentGravityCenter.h
- \brief Implementation of the interaction matrix computation for vpMomentGravityCenter.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentGravityCenter.
*/
#ifndef __FEATUREMOMENTGRAVITYCENTER_H__
#define __FEATUREMOMENTGRAVITYCENTER_H__
@@ -48,14 +50,17 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
+ \brief Functionality computation for gravity center moment feature. Computes
+the interaction matrix associated with vpMomentGravityCenter.
The interaction matrix for the is defined in \cite Tahri05z, equation (16).
It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
- The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
- The following code demonstrates a selection of \f$ L_{y_{g}} \f$:
+ These interaction matrices may be selected afterwards by calling
+vpFeatureMomentGravityCenter::interaction(). The selection is done by the
+following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}}
+\f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$. The
+following code demonstrates a selection of \f$ L_{y_{g}} \f$:
\code
#include <visp3/core/vpMomentObject.h>
@@ -121,7 +126,6 @@ int main()
return 0;
}
-
\endcode
This code produces the following output:
@@ -129,49 +133,57 @@ int main()
0 -1 1.5 3.5 -2.5 -1.5
\endcode
- You can also use the shortcut selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
+ You can also use the shortcut selectors
+vpFeatureMomentGravityCenter::selectXg or
+vpFeatureMomentGravityCenter::selectYg as follows:
\code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
+ task.addFeature(db_src.getFeatureGravityNormalized(), db_dst.getFeatureGravityNormalized(),
+ vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
\endcode
This feature depends on:
- vpFeatureMomentBasic
Minimum vpMomentObject order needed to compute this feature: 2.
*/
-class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
- */
- vpFeatureMomentGravityCenter(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentGravityCenter";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentGravityCenter";}
-
- /*!
- Shortcut selector for \f$x_g\f$.
- */
- static unsigned int selectXg(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_g\f$.
- */
- static unsigned int selectYg(){ return 1 << 1; }
-
+class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param database : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+ */
+ vpFeatureMomentGravityCenter(vpMomentDatabase &database, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(database, A_, B_, C_, featureMoments, 2)
+ {
+ }
+ void compute_interaction();
+ /*!
+ Associated moment name.
+ */
+ const char *momentName() const { return "vpMomentGravityCenter"; }
+ /*!
+ Feature name.
+ */
+ const char *name() const { return "vpFeatureMomentGravityCenter"; }
+
+ /*!
+ Shortcut selector for \f$x_g\f$.
+ */
+ static unsigned int selectXg() { return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_g\f$.
+ */
+ static unsigned int selectYg() { return 1 << 1; }
};
#else
@@ -181,18 +193,23 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for gravity center moment feature. Computes the interaction matrix associated with vpMomentGravityCenter.
+ \brief Functionality computation for gravity center moment feature. Computes
+ the interaction matrix associated with vpMomentGravityCenter.
The interaction matrix for the is defined in \cite Tahri05z, equation (16).
It allows to compute the interaction matrices for \f$ (x_g,y_g) \f$.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenter::interaction().
- The selection is done by the following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}} \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
+ These interaction matrices may be selected afterwards by calling
+ vpFeatureMomentGravityCenter::interaction(). The selection is done by the
+ following methods: vpFeatureMomentGravityCenter::selectXg for \f$ L_{x_{g}}
+ \f$ and vpFeatureMomentGravityCenter::selectYg for \f$ L_{y_{g}} \f$.
- You can use the selectors vpFeatureMomentGravityCenter::selectXg or vpFeatureMomentGravityCenter::selectYg as follows:
+ You can use the selectors vpFeatureMomentGravityCenter::selectXg or
+ vpFeatureMomentGravityCenter::selectYg as follows:
\code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
+ task.addFeature(db_src.getFeatureGravityNormalized(), db_dst.getFeatureGravityNormalized(),
+ vpFeatureMomentGravityCenter::selectXg() | vpFeatureMomentGravityCenter::selectYg());
\endcode
This feature depends on:
- vpMomentCentered
@@ -200,39 +217,44 @@ class vpMomentDatabase;
Minimum vpMomentObject order needed to compute this feature: 2.
*/
-class VISP_EXPORT vpFeatureMomentGravityCenter: public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
- */
- vpFeatureMomentGravityCenter(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- Associated moment name.
- */
- const char* momentName() const { return "vpMomentGravityCenter";}
- /*!
- Feature name.
- */
- const char* name() const { return "vpFeatureMomentGravityCenter";}
-
- /*!
- Shortcut selector for \f$x_g\f$.
- */
- static unsigned int selectXg(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_g\f$.
- */
- static unsigned int selectYg(){ return 1 << 1; }
-
+class VISP_EXPORT vpFeatureMomentGravityCenter : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+ */
+ vpFeatureMomentGravityCenter(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 2)
+ {
+ }
+ void compute_interaction();
+ /*!
+ Associated moment name.
+ */
+ const char *momentName() const { return "vpMomentGravityCenter"; }
+ /*!
+ Feature name.
+ */
+ const char *name() const { return "vpFeatureMomentGravityCenter"; }
+
+ /*!
+ Shortcut selector for \f$x_g\f$.
+ */
+ static unsigned int selectXg() { return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_g\f$.
+ */
+ static unsigned int selectYg() { return 1 << 1; }
};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h
index d90ffdc..2048203 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,8 @@
*****************************************************************************/
/*!
\file vpFeatureMomentGravityCenterNormalized.h
- \brief Implementation of the interaction matrix computation for vpMomentGravityCenterNormalized.
+ \brief Implementation of the interaction matrix computation for
+ vpMomentGravityCenterNormalized.
*/
#ifndef __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
#define __FEATUREMOMENTGRAVITYCENTERNORMALIZED_H__
@@ -48,25 +50,35 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
+ \brief Functionality computation for centered and normalized moment feature.
+ Computes the interaction matrix associated with
+ vpMomentGravityCenterNormalized.
- The interaction matrix for the moment feature can be deduced from \cite Tahri05z, equation (19).
- To do so, one must derive it and obtain a combination of interaction matrices by using (1).
- It allows to compute the interaction matrices for \f$ (x_n,y_n) \f$.
+ The interaction matrix for the moment feature can be deduced from \cite
+ Tahri05z, equation (19). To do so, one must derive it and obtain a
+ combination of interaction matrices by using (1). It allows to compute the
+ interaction matrices for \f$ (x_n,y_n) \f$.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
- The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
- You can use these shortcut selectors as follows:
+ These interaction matrices may be selected afterwards by calling
+ vpFeatureMomentGravityCenterNormalized::interaction. The selection is done
+ by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn
+ for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn
+ for \f$ L_{y_{n}} \f$. You can use these shortcut selectors as follows:
\code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
+ task.addFeature(db_src.getFeatureGravityNormalized(), db_dst.getFeatureGravityNormalized(),
+ vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
\endcode
- The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
+ The behaviour of this feature is very similar to
+ vpFeatureMomentGravityCenter which also contains a sample code demonstrating
+ a selection.
- This feature is often used in moment-based visual servoing to control the planar translation parameters.
+ This feature is often used in moment-based visual servoing to control the
+ planar translation parameters.
- Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
+ Minimum vpMomentObject order needed to compute this feature: 2 in dense mode
+ and 3 in discrete mode.
This feature depends on:
- vpFeatureMomentGravityCenter
@@ -75,40 +87,45 @@ class vpMomentDatabase;
- vpFeatureMomentAreaNormalized
*/
-class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param database : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& database,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(database,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentGravityCenterNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
-
- /*!
- Shortcut selector for \f$x_n\f$.
- */
- static unsigned int selectXn(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_n\f$.
- */
- static unsigned int selectYn(){ return 1 << 1; }
-
+class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param database : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentGravityCenterNormalized(vpMomentDatabase &database, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(database, A_, B_, C_, featureMoments, 2)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentGravityCenterNormalized"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentGravityCenterNormalized"; }
+
+ /*!
+ Shortcut selector for \f$x_n\f$.
+ */
+ static unsigned int selectXn() { return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_n\f$.
+ */
+ static unsigned int selectYn() { return 1 << 1; }
};
#else
@@ -118,7 +135,9 @@ class vpMomentDatabase;
\ingroup group_visual_features
- \brief Functionality computation for centered and normalized moment feature. Computes the interaction matrix associated with vpMomentGravityCenterNormalized.
+ \brief Functionality computation for centered and normalized moment feature.
+Computes the interaction matrix associated with
+vpMomentGravityCenterNormalized.
It computes the interaction matrices for \f$ (x_n,y_n) \f$.
The interaction matrix for the moment feature has the following expression:
@@ -190,19 +209,27 @@ with:
- \f$\theta=\frac{\eta_{0,2}}{NA}\f$
- \f$\eta\f$ is the centered and normalized moment.
- These interaction matrices may be selected afterwards by calling vpFeatureMomentGravityCenterNormalized::interaction.
- The selection is done by the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for \f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$ L_{y_{n}} \f$.
- You can use these shortcut selectors as follows:
+ These interaction matrices may be selected afterwards by calling
+vpFeatureMomentGravityCenterNormalized::interaction. The selection is done by
+the following methods: vpFeatureMomentGravityCenterNormalized::selectXn for
+\f$ L_{x_{n}} \f$ and vpFeatureMomentGravityCenterNormalized::selectYn for \f$
+L_{y_{n}} \f$. You can use these shortcut selectors as follows:
\code
- task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),vpFeatureMomentGravityCenterNormalized::selectXn() | vpFeatureMomentGravityCenterNormalized::selectYn());
+ task.addFeature(db_src.getFeatureGravityNormalized(),db_dst.getFeatureGravityNormalized(),
+ vpFeatureMomentGravityCenterNormalized::selectXn() |
+ vpFeatureMomentGravityCenterNormalized::selectYn());
\endcode
- The behaviour of this feature is very similar to vpFeatureMomentGravityCenter which also contains a sample code demonstrating a selection.
+ The behaviour of this feature is very similar to
+vpFeatureMomentGravityCenter which also contains a sample code demonstrating a
+selection.
- This feature is often used in moment-based visual servoing to control the planar translation parameters.
+ This feature is often used in moment-based visual servoing to control the
+planar translation parameters.
- Minimum vpMomentObject order needed to compute this feature: 2 in dense mode and 3 in discrete mode.
+ Minimum vpMomentObject order needed to compute this feature: 2 in dense mode
+and 3 in discrete mode.
This feature depends on:
- vpFeatureMomentGravityCenter
@@ -211,41 +238,45 @@ with:
- vpFeatureMomentAreaNormalized
*/
-class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment{
- public:
- /*!
- Initializes the feature with information about the database of moment primitives, the object plane and feature database.
- \param data_base : Moment database. The database of moment primitives (first parameter) is mandatory. It is used to access different moment values later used to compute the final matrix.
- \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param B_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param C_ : Plane coefficient in a \f$ A \times x+B \times y + C = \frac{1}{Z} \f$ plane.
- \param featureMoments : Feature database.
-
- */
- vpFeatureMomentGravityCenterNormalized(vpMomentDatabase& data_base,double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments=NULL)
- : vpFeatureMoment(data_base,A_,B_,C_,featureMoments,2)
- {}
- void compute_interaction();
- /*!
- associated moment name
- */
- const char* momentName() const { return "vpMomentGravityCenterNormalized";}
- /*!
- feature name
- */
- const char* name() const { return "vpFeatureMomentGravityCenterNormalized";}
-
- /*!
- Shortcut selector for \f$x_n\f$.
- */
- static unsigned int selectXn(){ return 1 << 0; }
-
- /*!
- Shortcut selector for \f$y_n\f$.
- */
- static unsigned int selectYn(){ return 1 << 1; }
-
+class VISP_EXPORT vpFeatureMomentGravityCenterNormalized : public vpFeatureMoment
+{
+public:
+ /*!
+ Initializes the feature with information about the database of moment
+ primitives, the object plane and feature database. \param data_base : Moment
+ database. The database of moment primitives (first parameter) is mandatory.
+ It is used to access different moment values later used to compute the final
+ matrix. \param A_ : Plane coefficient in a \f$ A \times x+B \times y + C =
+ \frac{1}{Z} \f$ plane. \param B_ : Plane coefficient in a \f$ A \times x+B
+ \times y + C = \frac{1}{Z} \f$ plane. \param C_ : Plane coefficient in a \f$
+ A \times x+B \times y + C = \frac{1}{Z} \f$ plane. \param featureMoments :
+ Feature database.
+
+ */
+ vpFeatureMomentGravityCenterNormalized(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments = NULL)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments, 2)
+ {
+ }
+ void compute_interaction();
+ /*!
+ associated moment name
+ */
+ const char *momentName() const { return "vpMomentGravityCenterNormalized"; }
+ /*!
+ feature name
+ */
+ const char *name() const { return "vpFeatureMomentGravityCenterNormalized"; }
+
+ /*!
+ Shortcut selector for \f$x_n\f$.
+ */
+ static unsigned int selectXn() { return 1 << 0; }
+
+ /*!
+ Shortcut selector for \f$y_n\f$.
+ */
+ static unsigned int selectYn() { return 1 << 1; }
};
#endif
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h
index a75d94c..4f957ba 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpFeaturePoint_H
#define vpFeaturePoint_H
@@ -45,13 +45,12 @@
*/
#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeaturePoint
\ingroup group_visual_features
@@ -114,7 +113,7 @@ int main()
//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.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the desired visual features sd
@@ -126,10 +125,10 @@ int main()
// Control loop
for ( ; ; ) {
// The new parameters x and y must be computed here.
-
+
// Update the current point visual feature
s.buildFrom(x, y, Z);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -137,14 +136,14 @@ int main()
}
\endcode
- If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
+ If you want to build your own control law, this other example shows how
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature, compute the corresponding error vector \f$(s-s^*)\f$ and finally
build the interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
int main()
{
@@ -179,56 +178,49 @@ int main()
*/
-
class VISP_EXPORT vpFeaturePoint : public vpBasicFeature
{
private:
//! FeaturePoint depth (required to compute the interaction matrix)
//! default Z = 1m
- double Z ;
+ double Z;
public:
- vpFeaturePoint() ;
+ vpFeaturePoint();
//! Destructor.
virtual ~vpFeaturePoint() {}
- void buildFrom(const double x, const double y, const double Z) ;
+ void buildFrom(const double x, const double y, const double Z);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
- vpFeaturePoint *duplicate() 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) ;
+ vpColVector error(const unsigned int select = FEATURE_ALL);
- double get_x() const ;
+ double get_x() const;
- double get_y() const ;
+ double get_y() const;
- double get_Z() const ;
+ double get_Z() const;
- void init() ;
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void init();
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void print(const unsigned int select = FEATURE_ALL ) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
- void set_x(const double x) ;
+ void set_x(const double x);
- void set_y(const double y) ;
+ void set_y(const double y);
- void set_Z(const double Z) ;
-
- void set_xyZ(const double x, const double y, const double Z) ;
+ void set_Z(const double Z);
+ void set_xyZ(const double x, const double y, const double Z);
// feature selection
static unsigned int selectX();
@@ -237,12 +229,10 @@ public:
/*!
@name Deprecated functions
*/
- typedef enum
- {
- X = 1, // x coordinates
- Y = 2 // y coordinates
+ typedef enum {
+ X = 1, // x coordinates
+ Y = 2 // y coordinates
} vpFeaturePointType;
-
-} ;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h
index d38aaf5..a4b6b0f 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePoint3D.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpFeaturePoint3d_H
#define vpFeaturePoint3d_H
@@ -46,8 +46,8 @@
*/
#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
@@ -61,10 +61,8 @@
{\bf X} = (X,Y,Z)\f$ coordinates in the camera frame.
This class is intended to manipulate the 3D point visual feature
- \f$ s = (X,Y,Z) \f$. The interaction matrix related to \f$ s \f$ is given by:
- \f[
- L = \left[
- \begin{array}{rrrrrr}
+ \f$ s = (X,Y,Z) \f$. The interaction matrix related to \f$ s \f$ is given
+by: \f[ L = \left[ \begin{array}{rrrrrr}
-1 & 0 & 0 & 0 & -Z & Y \\
0 & -1 & 0 & Z & 0 & -X \\
0 & 0 & -1 & -Y & X & 0 \\
@@ -100,8 +98,8 @@
\code
#include <iostream>
-#include <visp3/visual_features/vpFeaturePoint3D.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
#include <visp3/vs/vpServo.h>
int main()
@@ -112,7 +110,7 @@ int main()
vpPoint point(0.1, -0.1, 0);
vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
- cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
+ cMo.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
@@ -122,40 +120,40 @@ int main()
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*.
+ // Creation of the desired feature s*.
vpFeaturePoint3D s_star;
s_star.buildFrom(0, 0, 1); // Z*=1 meter
s_star.print();
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the desired visual features s*
- task.setInteractionMatrixType(vpServo::DESIRED);
+ task.setInteractionMatrixType(vpServo::DESIRED);
// Set the constant gain
double lambda = 0.8;
- task.setLambda(lambda);
+ task.setLambda(lambda);
// Add the 3D point feature to the task
- task.addFeature(s, s_star);
+ task.addFeature(s, s_star);
// Control loop
for ( ; ; ) {
// ... cMo need here to be estimated from for example a pose estimation.
point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
-
+
// Update the current 3D point visual feature
s.buildFrom(point);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
}
\endcode
- If you want to deal only with the \f$(X,Y)\f$ subset feature from the 3D
- point feature, you have just to modify the addFeature() call in
- the previous example by the following line. In that case, the dimension
+ If you want to deal only with the \f$(X,Y)\f$ subset feature from the 3D
+ point feature, you have just to modify the addFeature() call in
+ the previous example by the following line. In that case, the dimension
of \f$s\f$ is two.
\code
@@ -170,9 +168,9 @@ int main()
\code
#include <iostream>
-#include <visp3/visual_features/vpFeaturePoint3D.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeaturePoint3D.h>
int main()
{
@@ -180,17 +178,17 @@ int main()
vpPoint point(0.1, -0.1, 0);
vpHomogeneousMatrix cMo; // Pose between the camera and the object frame
- cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
+ cMo.buildFrom(0, 0, 1.2, 0, 0, 0);
// ... cMo need here to be computed from a pose estimation
point.changeFrame(cMo); // Compute the 3D point coordinates in the camera frame cP = cMo * oP
// Creation of the current feature s
vpFeaturePoint3D s;
- s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame
+ s.buildFrom(point); // Initialize the feature from the 3D point coordinates in the camera frame
s.print();
- // Creation of the desired feature s*.
+ // Creation of the desired feature s*.
vpFeaturePoint3D s_star;
s_star.buildFrom(0, 0, 1); // Z*=1 meter
s_star.print();
@@ -213,7 +211,7 @@ class VISP_EXPORT vpFeaturePoint3D : public vpBasicFeature
public:
// basic constructor
- vpFeaturePoint3D() ;
+ vpFeaturePoint3D();
//! Destructor. Does nothing.
virtual ~vpFeaturePoint3D() {}
@@ -222,56 +220,49 @@ public:
*/
// build feature from a point (vpPoint)
- void buildFrom(const vpPoint &p) ;
+ void buildFrom(const vpPoint &p);
// set the point XY and Z-coordinates
- void buildFrom(const double X, const double Y, const double Z) ;
+ void buildFrom(const double X, const double Y, const double Z);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
// feature duplication
- vpFeaturePoint3D *duplicate() const ;
+ vpFeaturePoint3D *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 point X-coordinates
- double get_X() const ;
+ double get_X() const;
// get the point Y-coordinates
- double get_Y() const ;
+ double get_Y() const;
// get the point depth (camera frame)
- double get_Z() const ;
+ double get_Z() const;
// basic construction
- void init() ;
+ void init();
// compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
// print the name of the feature
- void print(const unsigned int select=FEATURE_ALL ) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
// set the point X-coordinates
- void set_X(const double X) ;
+ void set_X(const double X);
// set the point Y-coordinates
- void set_Y(const double Y) ;
+ void set_Y(const double Y);
// set the point depth (camera frame)
- void set_Z(const double Z) ;
+ void set_Z(const double Z);
// set the point XY and Z-coordinates
- void set_XYZ(const double X, const double Y, const double Z) ;
+ void set_XYZ(const double X, const double Y, const double Z);
static unsigned int selectX();
static unsigned int selectY();
static unsigned int selectZ();
-} ;
-
-
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h
index b32933d..be8c0b8 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeaturePointPolar.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
#ifndef vpFeaturePointPolar_H
#define vpFeaturePointPolar_H
@@ -45,13 +45,12 @@
*/
#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeaturePointPolar
\ingroup group_visual_features
@@ -87,12 +86,11 @@
=
\left[
\begin{array}{cccccc}
- \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} & (1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\
+ \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} &
+(1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\
\;\\ \
- \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta /\rho & \sin\theta/\rho & -1 \\
- \end{array}
- \right]
- \f]
+ \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta
+/\rho & \sin\theta/\rho & -1 \\ \end{array} \right] \f]
where \f$Z\f$ is the 3D depth of the considered point in the camera frame.
@@ -150,13 +148,13 @@ int main()
point[3].setWorldCoordinates(-0.1, 0.1, 0);
// Initialize the desired pose between the camera and the object frame
- vpHomogeneousMatrix cMod;
+ vpHomogeneousMatrix cMod;
cMod.buildFrom(0, 0, 1, 0, 0, 0);
- // Compute the desired position of the point
+ // Compute the desired position of the point
for (int i = 0 ; i < 4 ; i++) {
// Compute the 3D point coordinates in the camera frame cP = cMod * oP
- point[i].changeFrame(cMod);
+ point[i].changeFrame(cMod);
// Compute the perspective projection to set (x,y)
point[i].projection();
}
@@ -164,33 +162,33 @@ int main()
// Create 4 desired visual features as 2D points with polar coordinates
vpFeaturePointPolar pd[4];
// Initialize the desired visual feature from the desired point positions
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(pd[i], point[i]);
-
+
// Initialize the current pose between the camera and the object frame
- vpHomogeneousMatrix cMo;
- cMo.buildFrom(0, 0, 1.2, 0, 0, M_PI);
+ vpHomogeneousMatrix cMo;
+ cMo.buildFrom(0, 0, 1.2, 0, 0, M_PI);
// ... cMo need here to be computed from a pose estimation
for (int i = 0 ; i < 4 ; i++) {
// Compute the 3D point coordinates in the camera frame cP = cMo * oP
- point[i].changeFrame(cMo);
+ point[i].changeFrame(cMo);
// Compute the perspective projection to set (x,y)
point[i].projection();
}
// Create 4 current visual features as 2D points with polar coordinates
vpFeaturePointPolar p[4];
// Initialize the current visual feature from the current point positions
- for (int i = 0 ; i < 4 ; i++)
+ for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i], point[i]);
// Visual servo task initialization
vpServo task;
// - Camera is monted on the robot end-effector and velocities are
// computed in the camera frame
- task.setServo(vpServo::EYEINHAND_CAMERA);
+ task.setServo(vpServo::EYEINHAND_CAMERA);
// - Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// - Set the contant gain to 1
task.setLambda(1);
// - Add current and desired features
@@ -204,11 +202,11 @@ int main()
// coordinates in the image plane
for (int i = 0 ; i < 4 ; i++)
point[i].track(cMo) ;
-
+
// Update the current 2D point visual feature with polar coordinates
for (int i = 0 ; i < 4 ; i++)
vpFeatureBuilder::create(p[i], point[i]);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -217,9 +215,9 @@ int main()
}
\endcode
- If you want to deal only with the \f$\rho\f$ subset feature from the 2D
- point feature set, you have just to modify the addFeature() call in the
- previous example by the following line. In that case, the dimension of
+ If you want to deal only with the \f$\rho\f$ subset feature from the 2D
+ point feature set, you have just to modify the addFeature() call in the
+ previous example by the following line. In that case, the dimension of
\f$s\f$ is four.
\code
@@ -227,14 +225,14 @@ int main()
task.addFeature(p[i], pd[i], vpFeaturePointPolar::selectRho());
\endcode
- If you want to build your own control law, this other example shows how
- to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
- feature with polar coordinates, compute the corresponding error vector
+ If you want to build your own control law, this other example shows how
+ to create a current (\f$s\f$) and desired (\f$s^*\f$) 2D point visual
+ feature with polar coordinates, compute the corresponding error vector
\f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeaturePointPolar.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeaturePointPolar.h>
int main()
{
@@ -264,57 +262,52 @@ class VISP_EXPORT vpFeaturePointPolar : public vpBasicFeature
private:
//! FeaturePoint depth (required to compute the interaction matrix)
//! default Z = 1m
- double Z ;
+ double Z;
public:
// basic constructor
- vpFeaturePointPolar() ;
+ vpFeaturePointPolar();
//! Destructor. Does nothing.
- virtual ~vpFeaturePointPolar() { }
+ virtual ~vpFeaturePointPolar() {}
- void buildFrom(const double rho, const double theta, const double Z) ;
+ void buildFrom(const double rho, const double theta, const double Z);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
// feature duplication
- vpFeaturePointPolar *duplicate() const ;
+ vpFeaturePointPolar *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);
// basic construction
- void init() ;
+ void init();
// get the point rho-coordinates
- double get_rho() const ;
+ double get_rho() const;
// get the point theta-coordinates
- double get_theta() const ;
+ double get_theta() const;
// get the point depth (camera frame)
- double get_Z() const ;
+ double get_Z() const;
// compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
// print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
// set the point rho-coordinates
- void set_rho(const double rho) ;
+ void set_rho(const double rho);
// set the point theta-coordinates
- void set_theta(const double theta) ;
+ void set_theta(const double theta);
// set the point depth (camera frame)
- void set_Z(const double Z) ;
+ void set_Z(const double Z);
// set the point rho, theta polar coordinates and Z coordinate
- void set_rhoThetaZ(const double rho, const double theta, const double Z) ;
+ void set_rhoThetaZ(const double rho, const double theta, const double Z);
/*
vpBasicFeature method instantiation
@@ -326,10 +319,8 @@ public:
@name Deprecated functions
*/
//! compute the error between a visual features and zero
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-} ;
-
-
+ vpColVector error(const unsigned int select = FEATURE_ALL);
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h
index 96227a2..7359077 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureSegment.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureSegment_H
#define vpFeatureSegment_H
@@ -47,8 +47,8 @@
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureException.h>
/*!
@@ -57,96 +57,97 @@
\brief Class that defines a 2D segment visual features.
This class allow to consider two sets of visual features:
- - the non normalised features \f${\bf s} = (x_c, y_c, l, \alpha)\f$ where \f$(x_c,y_c)\f$
- are the coordinates of the segment center, \f$ l \f$ the segment length
- and \f$ \alpha \f$ the orientation of the segment with respect to the \f$ x \f$ axis.
- - or the normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ with \f$x_n = x_c/l\f$,
- \f$y_n = y_c/l\f$ and \f$l_n = 1/l\f$.
+ - the non normalised features \f${\bf s} = (x_c, y_c, l, \alpha)\f$ where
+ \f$(x_c,y_c)\f$ are the coordinates of the segment center, \f$ l \f$ the
+ segment length and \f$ \alpha \f$ the orientation of the segment with
+ respect to the \f$ x \f$ axis.
+ - or the normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ with
+ \f$x_n = x_c/l\f$, \f$y_n = y_c/l\f$ and \f$l_n = 1/l\f$.
- The selection of the feature set is done either during construction using vpFeatureSegment(bool),
- or by setNormalized(bool).
+ The selection of the feature set is done either during construction using
+ vpFeatureSegment(bool), or by setNormalized(bool).
*/
class VISP_EXPORT vpFeatureSegment : public vpBasicFeature
{
public:
- //empty constructor
- vpFeatureSegment(bool normalized=false);
+ // empty constructor
+ explicit vpFeatureSegment(bool normalized = false);
//! Destructor. Does nothing.
~vpFeatureSegment() {}
// change values of the segment
void buildFrom(const double x1, const double y1, const double Z1, const double x2, const double y2, const double Z2);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
//! Feature duplication.
- vpFeatureSegment *duplicate() const ;
+ 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.
- \return If normalized features are used, return \f$ x_n = x_c / l \f$. Otherwise return \f$ x_c \f$.
+ \return If normalized features are used, return \f$ x_n = x_c / l \f$.
+ Otherwise return \f$ x_c \f$.
*/
- inline double getXc() const { return s[0] ; }
+ inline double getXc() const { return s[0]; }
/*!
Get the y coordinate of the segment center in the image plane.
- \return If normalized features are used, return \f$ y_n = y_c / l \f$. Otherwise return \f$ y_c \f$.
+ \return If normalized features are used, return \f$ y_n = y_c / l \f$.
+ Otherwise return \f$ y_c \f$.
*/
- inline double getYc() const { return s[1] ; }
+ inline double getYc() const { return s[1]; }
/*!
Get the length of the segment.
- \return If normalized features are used, return \f$ l_n = 1 / l \f$. Otherwise return \f$ l \f$.
+ \return If normalized features are used, return \f$ l_n = 1 / l \f$.
+ Otherwise return \f$ l \f$.
*/
- inline double getL() const { return s[2] ; }
+ inline double getL() const { return s[2]; }
/*!
- Get the value of \f$ \alpha \f$ which represents the orientation of the segment.
+ Get the value of \f$ \alpha \f$ which represents the orientation of
+ the segment.
\return The value of \f$ \alpha \f$.
*/
- inline double getAlpha() const { return s[3] ;}
+ inline double getAlpha() const { return s[3]; }
/*!
- Get the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment first point.
+ Get the value of \f$ Z_1 \f$ which represents the Z coordinate in the
+ camera frame of the 3D point that corresponds to the segment first point.
\return The value of the depth \f$ Z_1 \f$.
*/
- inline double getZ1() const { return Z1_ ;}
+ inline double getZ1() const { return Z1_; }
/*!
- Get the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment second point.
+ Get the value of \f$ Z_2 \f$ which represents the Z coordinate in the
+ camera frame of the 3D point that corresponds to the segment second
+ point.
\return The value of the depth \f$ Z_2 \f$.
*/
- inline double getZ2() const { return Z2_ ;}
+ inline double getZ2() const { return Z2_; }
// Basic construction.
- void init() ;
+ void init();
// compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void print(const unsigned int select= FEATURE_ALL) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
/*!
Indicates if the normalized features are considered.
@@ -157,11 +158,12 @@ public:
static unsigned int selectYc();
static unsigned int selectL();
static unsigned int selectAlpha();
-
+
/*!
Set the king of feature to consider.
- \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
- If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
+ \param normalized : If true, use normalized features \f${\bf s} = (x_n,
+ y_n, l_n, \alpha)\f$. If false, use non normalized features \f${\bf s} =
+ (x_c, y_c, l_c, \alpha)\f$.
*/
void setNormalized(bool normalized) { normalized_ = normalized; };
/*!
@@ -169,10 +171,12 @@ public:
Set the value of the x coordinate of the segment center
in the image plane. It is one parameter of the visual feature \f$ s \f$.
- \param val : Value to set, that is either equal to \f$ x_n = x_c/l \f$ when normalized features
- are considered, or equal to \f$ x_c \f$ otherwise.
+ \param val : Value to set, that is either equal to \f$ x_n = x_c/l \f$
+ when normalized features are considered, or equal to \f$ x_c \f$
+ otherwise.
*/
- inline void setXc(const double val){
+ inline void setXc(const double val)
+ {
s[0] = xc_ = val;
flags[0] = true;
}
@@ -181,32 +185,38 @@ public:
Set the value of the y coordinate of the segment center
in the image plane. It is one parameter of the visual feature \f$ s \f$.
- \param val : Value to set, that is either equal to \f$ y_n = y_c/l \f$ when normalized features
- are considered, or equal to \f$ y_c \f$ otherwise.
+ \param val : Value to set, that is either equal to \f$ y_n = y_c/l \f$
+ when normalized features are considered, or equal to \f$ y_c \f$
+ otherwise.
*/
- inline void setYc(const double val){
+ inline void setYc(const double val)
+ {
s[1] = yc_ = val;
flags[1] = true;
}
/*!
- Set the value of the segment length in the image plane. It is one parameter of the visual feature \f$ s \f$.
+ Set the value of the segment length in the image plane. It is one
+ parameter of the visual feature \f$ s \f$.
- \param val : Value to set, that is either equal to \f$l_n= 1/l \f$ when normalized features
- are considered, or equal to \f$ l \f$ otherwise.
+ \param val : Value to set, that is either equal to \f$l_n= 1/l \f$ when
+ normalized features are considered, or equal to \f$ l \f$ otherwise.
*/
- inline void setL(const double val){
+ inline void setL(const double val)
+ {
s[2] = l_ = val;
flags[2] = true;
}
/*!
- Set the value of \f$ \alpha \f$ which represents the orientation of the segment
- in the image plane. It is one parameter of the visual feature \f$ s \f$.
+ Set the value of \f$ \alpha \f$ which represents the orientation of the
+ segment in the image plane. It is one parameter of the visual feature \f$
+ s \f$.
\param val : \f$ \alpha \f$ value to set.
*/
- inline void setAlpha(const double val){
+ inline void setAlpha(const double val)
+ {
s[3] = alpha_ = val;
cos_a_ = cos(val);
sin_a_ = sin(val);
@@ -215,35 +225,32 @@ public:
/*!
- Set the value of \f$ Z_1 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment first point.
+ Set the value of \f$ Z_1 \f$ which represents the Z coordinate in the
+ camera frame of the 3D point that corresponds to the segment first point.
This value is requested to compute the interaction matrix.
\param val : \f$ Z_1 \f$ value to set.
- \exception vpFeatureException::badInitializationError : If Z1 is behind the camera or equal to zero.
+ \exception vpFeatureException::badInitializationError : If Z1 is behind
+ the camera or equal to zero.
*/
inline void setZ1(const double val)
{
Z1_ = val;
- if (Z1_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z1 = " << Z1_ << std::endl ;
+ if (Z1_ < 0) {
+ vpERROR_TRACE("Point is behind the camera ");
+ std::cout << "Z1 = " << Z1_ << std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z1 is behind the camera ")) ;
+ 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 ;
+ 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;
@@ -251,41 +258,37 @@ public:
/*!
- Set the value of \f$ Z_2 \f$ which represents the Z coordinate in the camera frame
- of the 3D point that corresponds to the segment second point.
+ Set the value of \f$ Z_2 \f$ which represents the Z coordinate in the
+ camera frame of the 3D point that corresponds to the segment second point.
This value is requested to compute the interaction matrix.
\param val : \f$ Z_2 \f$ value to set.
- \exception vpFeatureException::badInitializationError : If Z2 is behind the camera or equal to zero.
+ \exception vpFeatureException::badInitializationError : If Z2 is behind
+ the camera or equal to zero.
*/
inline void setZ2(const double val)
{
Z2_ = val;
- if (Z2_ < 0)
- {
- vpERROR_TRACE("Point Z2 is behind the camera ") ;
- std::cout <<"Z2 = " << Z2_ << std::endl ;
+ if (Z2_ < 0) {
+ vpERROR_TRACE("Point Z2 is behind the camera ");
+ std::cout << "Z2 = " << Z2_ << std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z2 is behind the camera ")) ;
+ 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 ;
+ 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;
}
-
private:
double xc_;
double yc_;
@@ -296,7 +299,6 @@ private:
double cos_a_;
double sin_a_;
bool normalized_;
-} ;
+};
#endif
-
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h
index 149f6e7..e0df60c 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureThetaU.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureThetaU_H
#define vpFeatureThetaU_H
@@ -52,7 +52,6 @@
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
/*!
\class vpFeatureThetaU
\ingroup group_visual_features
@@ -65,7 +64,7 @@
It is convenient to consider two coordinate frames: the current
camera frame \f$ {\cal{F}}_c \f$ and the desired camera frame \f$
- {\cal{F}}_{c^*} \f$.
+ {\cal{F}}_{c^*} \f$.
Let \f$^{c^*}R_c \f$ be the rotation matrix that gives the
orientation of the current camera frame relative to the desired camera
@@ -139,8 +138,8 @@
while() loop while \f$s^*\f$ is considered as zero.
\code
-#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/vs/vpServo.h>
int main()
@@ -156,11 +155,11 @@ int main()
vpFeatureThetaU s(vpFeatureThetaU::cRcd);
s.buildFrom(cMcd); // Initialization of the feature
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Add the 3D ThetaU feature to the task
task.addFeature(s); // s* is here considered as zero
@@ -168,22 +167,22 @@ int main()
// Control loop
for ( ; ; ) {
// ... cMcd need here to be initialized from for example a pose estimation.
-
+
// Update the current ThetaU visual feature
s.buildFrom(cMcd);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
}
\endcode
- If you want to deal only with the \f$(\theta u_x,\theta u_y)\f$ subset
- feature from the 3D \f$\theta u\f$ , you have just to modify the
- addFeature() call in the previous example by the following line. In
+ If you want to deal only with the \f$(\theta u_x,\theta u_y)\f$ subset
+ feature from the 3D \f$\theta u\f$ , you have just to modify the
+ addFeature() call in the previous example by the following line. In
that case, the dimension of \f$s\f$ is two.
- \code
+ \code
// Add the (ThetaU_x, ThetaU_y) subset features from the 3D ThetaU
// rotation to the task
task.addFeature(s, vpFeatureThetaU::selectTUx() | vpFeatureThetaU::selectTUy());
@@ -195,9 +194,9 @@ int main()
vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeatureThetaU.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
int main()
{
@@ -208,9 +207,9 @@ int main()
vpFeatureThetaU s(vpFeatureThetaU::cdRc);
s.buildFrom(cdMc); // Initialization of the feature
- // Creation of the desired feature s*. By default this feature is
+ // Creation of the desired feature s*. By default this feature is
// initialized to zero
- vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
+ vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
// Compute the interaction matrix L_s for the current ThetaU feature
vpMatrix L = s.interaction();
@@ -219,32 +218,30 @@ int main()
s.error(s_star);
}
\endcode
-
+
*/
class VISP_EXPORT vpFeatureThetaU : public vpBasicFeature
{
public:
- typedef enum
- {
- TUx = 1, /*!< Select the subset \f$ \theta u_x \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- TUy = 2, /*!< Select the subset \f$ \theta u_y \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- TUz = 4 /*!< Select the subset \f$ \theta u_z \f$ visual feature
- from the \f$ \theta u\f$ angle/axis representation. */
- } vpFeatureThetaUType;
- typedef enum
- {
- cdRc, /*!< Selector used to manipulate the visual feature \f$ s
- = \theta u_{^{c^*}R_c} \f$. This visual feature
- represent the orientation of the current camera frame
- relative to the desired camera frame. */
- cRcd /*!< Selector used to manipulate the visual feature \f$ s = \theta
- u_{^{c}R_{c^*}} \f$. This visual feature
- represent the orientation of the desired camera frame
- relative to the current camera frame. */
- } vpFeatureThetaURotationRepresentationType;
+ typedef enum {
+ TUx = 1, /*!< Select the subset \f$ \theta u_x \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ TUy = 2, /*!< Select the subset \f$ \theta u_y \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ TUz = 4 /*!< Select the subset \f$ \theta u_z \f$ visual feature
+ from the \f$ \theta u\f$ angle/axis representation. */
+ } vpFeatureThetaUType;
+ typedef enum {
+ cdRc, /*!< Selector used to manipulate the visual feature \f$ s
+ = \theta u_{^{c^*}R_c} \f$. This visual feature
+ represent the orientation of the current camera frame
+ relative to the desired camera frame. */
+ cRcd /*!< Selector used to manipulate the visual feature \f$ s = \theta
+ u_{^{c}R_{c^*}} \f$. This visual feature
+ represent the orientation of the desired camera frame
+ relative to the current camera frame. */
+ } vpFeatureThetaURotationRepresentationType;
/*
attributes and members directly related to the vpBasicFeature needs
other functionalities are useful but not mandatory
@@ -252,39 +249,31 @@ public:
public:
// Basic constructor.
- vpFeatureThetaU() ;
- vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpThetaUVector &tu,
- vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpRotationMatrix &R,
- vpFeatureThetaURotationRepresentationType r) ;
- vpFeatureThetaU(vpHomogeneousMatrix &M,
- vpFeatureThetaURotationRepresentationType r) ;
+ vpFeatureThetaU();
+ explicit vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r);
+ vpFeatureThetaU(vpThetaUVector &tu, vpFeatureThetaURotationRepresentationType r);
+ vpFeatureThetaU(vpRotationMatrix &R, vpFeatureThetaURotationRepresentationType r);
+ vpFeatureThetaU(vpHomogeneousMatrix &M, vpFeatureThetaURotationRepresentationType r);
//! Destructor. Does nothing.
virtual ~vpFeatureThetaU() {}
- void buildFrom(vpThetaUVector &tu) ;
+ void buildFrom(vpThetaUVector &tu);
// build from a rotation matrix
- void buildFrom(const vpRotationMatrix &R) ;
+ void buildFrom(const vpRotationMatrix &R);
// build from an homogeneous matrix
- void buildFrom(const vpHomogeneousMatrix &M) ;
+ void buildFrom(const vpHomogeneousMatrix &M);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
//! Feature duplication.
- vpFeatureThetaU *duplicate() const ;
+ vpFeatureThetaU *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);
vpFeatureThetaURotationRepresentationType getFeatureThetaURotationType() const;
@@ -293,15 +282,15 @@ public:
double get_TUz() const;
// Basic construction.
- void init() ;
+ void init();
// compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
- void print(const unsigned int select= FEATURE_ALL) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
- void set_TUx(const double tu_x) ;
- void set_TUy(const double tu_y) ;
- void set_TUz(const double tu_z) ;
+ void set_TUx(const double tu_x);
+ void set_TUy(const double tu_y);
+ void set_TUz(const double tu_z);
void setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r);
@@ -315,7 +304,6 @@ public:
private:
vpFeatureThetaURotationRepresentationType rotation;
-} ;
-
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h
index 343ef93..4894fb3 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureTranslation.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#ifndef vpFeatureTranslation_H
#define vpFeatureTranslation_H
@@ -45,12 +45,11 @@
\brief class that defines the translation visual feature.
*/
-#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
-#include <visp3/core/vpTranslationVector.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpRGBa.h>
-
+#include <visp3/core/vpTranslationVector.h>
+#include <visp3/visual_features/vpBasicFeature.h>
/*!
\class vpFeatureTranslation
@@ -59,11 +58,13 @@
\brief Class that defines the translation visual feature
\f$s=(t_x,t_y,t_z)\f$.
- It is convenient to consider two coordinate frames noted here \f$ {\cal{F}}_1 \f$ and \f$
- {\cal{F}}_{2} \f$.
+ It is convenient to consider two coordinate frames noted here \f$
+{\cal{F}}_1 \f$ and \f$
+ {\cal{F}}_{2} \f$.
- Let \f$^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ be the homogeneous matrix that gives the
- orientation and the translation of the frame \f$ {\cal{F}}_1 \f$ with respect to the frame \f$ {\cal{F}}_2 \f$.
+ Let \f$^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ be the homogeneous matrix that
+gives the orientation and the translation of the frame \f$ {\cal{F}}_1 \f$
+with respect to the frame \f$ {\cal{F}}_2 \f$.
\f[
^{{\cal{F}}_2}M_{{\cal{F}}_1} = \left(\begin{array}{cc}
@@ -73,30 +74,31 @@
\right)
\f]
- with \f$^{{\cal{F}}_2}R_{{\cal{F}}_1} \f$ the rotation matrix that gives the orientation
- of the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$ and
- \f$^{{\cal{F}}_2}t_{{\cal{F}}_1} \f$ the translation vector that gives the position of
- the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$ {\cal{F}}_2 \f$. To
- know more about homogeneous matrices see vpHomogeneousMatrix
- documentation.
+ with \f$^{{\cal{F}}_2}R_{{\cal{F}}_1} \f$ the rotation matrix that gives the
+orientation of the frame \f$ {\cal{F}}_1 \f$ relative to the frame \f$
+{\cal{F}}_2 \f$ and \f$^{{\cal{F}}_2}t_{{\cal{F}}_1} \f$ the translation
+vector that gives the position of the frame \f$ {\cal{F}}_1 \f$ relative to
+the frame \f$ {\cal{F}}_2 \f$. To know more about homogeneous matrices see
+vpHomogeneousMatrix documentation.
This class can be used to manipulate three kind of visual features:
- This class can be used to manipulate the translation visual feature
\f$s= ^{c^*}t_c\f$ which gives the position of
- the current camera frame relative to the desired camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
- visual feature \f$ s^* \f$ is equal to zero. The corresponding error
- is than equal to \f$ e=(s-s^*) = ^{c^*}t_c \f$. In this case, the
- interaction matrix related to \f$ s \f$ is given by \f[ L = [
+ the current camera frame relative to the desired camera frame. It is
+composed by the three components \f$(t_x,t_y,t_z)\f$. The desired visual
+feature \f$ s^* \f$ is equal to zero. The corresponding error is than equal to
+\f$ e=(s-s^*) = ^{c^*}t_c \f$. In this case, the interaction matrix related to
+\f$ s \f$ is given by \f[ L = [
^{c^*}R_c \;\; 0_3] \f]
- This class can also be used to manipulate the translation visual feature
\f$s= ^{c}t_{c^*}\f$ which gives the position of
- the desired camera frame relative to the current camera frame. It is composed by the three components \f$(t_x,t_y,t_z)\f$. The desired
- visual feature \f$ s^* \f$ is equal to zero. The corresponding error
- is than equal to \f$ e=(s-s^*) = ^{c}t_{c^*} \f$. In this case, the
- interaction matrix related to \f$ s \f$ is given by \f[ L = [
- -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
+ the desired camera frame relative to the current camera frame. It is
+composed by the three components \f$(t_x,t_y,t_z)\f$. The desired visual
+feature \f$ s^* \f$ is equal to zero. The corresponding error is than equal to
+\f$ e=(s-s^*) = ^{c}t_{c^*} \f$. In this case, the interaction matrix related
+to \f$ s \f$ is given by \f[ L = [ -I_3 \;\; [^{c}t_{c^*}]_\times] \f]
- Actually, this class can also be used to manipulate the
translation visual feature \f$s= ^{c}t_o\f$ which gives the position
@@ -131,8 +133,8 @@
while \f$s^*\f$ is set to zero.
\code
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/vs/vpServo.h>
int main()
@@ -146,14 +148,14 @@ int main()
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.buildFrom(cdMc); // Initialization of the current feature s=(tx,ty,tz)
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Set the constant gain
double lambda = 0.8;
- task.setLambda(lambda);
+ task.setLambda(lambda);
// Add the 3D translation feature to the task
task.addFeature(s); // s* is here considered as zero
@@ -161,35 +163,35 @@ int main()
// Control loop
for ( ; ; ) {
// ... cdMc need here to be initialized from for example a pose estimation.
-
+
// Update the current 3D translation visual feature
s.buildFrom(cdMc);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
}
\endcode
- If you want to deal only with the \f$(t_x,t_y)\f$ subset feature from the 3D
- translation, you have just to modify the addFeature() call in
- the previous example by the following line. In that case, the dimension
+ If you want to deal only with the \f$(t_x,t_y)\f$ subset feature from the 3D
+ translation, you have just to modify the addFeature() call in
+ the previous example by the following line. In that case, the dimension
of \f$s\f$ is two.
\code
// Add the (tx,ty) subset features from 3D translation to the task
task.addFeature(s, vpFeatureTranslation::selectTx() | vpFeatureTranslation::selectTy());
\endcode
-
+
If you want to build your own control law, this other example shows
how to create a current (\f$s\f$) and desired (\f$s^*\f$) 3D
translation visual feature, compute the corresponding error
vector \f$(s-s^*)\f$ and finally build the interaction matrix \f$L_s\f$.
\code
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
int main()
{
@@ -200,9 +202,9 @@ int main()
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.buildFrom(cdMc); // Initialization of the feature
- // Creation of the desired feature s*. By default this feature is
+ // Creation of the desired feature s*. By default this feature is
// initialized to zero
- vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
+ vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
// Compute the interaction matrix for the translation feature
vpMatrix L = s.interaction();
@@ -215,16 +217,16 @@ int main()
The code below shows how to create an eye-in hand visual servoing
task using a 3D translation feature \f$(t_x,t_y,t_z)\f$ that
correspond to the 3D translation between the current camera frame
- and the object frame. Like with the previous examples, to
- control six degrees of freedom, at least three other features must be
- considered like vpFeatureThetaU visual features. The way to initialize
- the visual features is quite the same as before. The difference is that
- the cMo method must be precised and the desired feature is note
+ and the object frame. Like with the previous examples, to
+ control six degrees of freedom, at least three other features must be
+ considered like vpFeatureThetaU visual features. The way to initialize
+ the visual features is quite the same as before. The difference is that
+ the cMo method must be precised and the desired feature is note
necessary equal to zero.
\code
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/vs/vpServo.h>
int main()
@@ -245,14 +247,14 @@ int main()
vpFeatureTranslation s(vpFeatureTranslation::cMo);
s.buildFrom(cMo); // Initialization of the current feature s=(tx,ty,tz)
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current visual features s
- task.setInteractionMatrixType(vpServo::CURRENT);
+ task.setInteractionMatrixType(vpServo::CURRENT);
// Set the constant gain
double lambda = 0.8;
- task.setLambda(lambda);
+ task.setLambda(lambda);
// Add the 3D translation feature to the task
task.addFeature(s, s_star); // s* is here considered as zero
@@ -260,10 +262,10 @@ int main()
// Control loop
for ( ; ; ) {
// ... cMo need here to be computed from for example a pose estimation.
-
+
// Update the current 3D translation visual feature
s.buildFrom(cMo);
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -274,78 +276,71 @@ int main()
class VISP_EXPORT vpFeatureTranslation : public vpBasicFeature
{
public:
-
- /*!
- \enum vpFeatureTranslationRepresentationType
+ /*!
+ \enum vpFeatureTranslationRepresentationType
Kind of implemented 3D translation feature.
*/
typedef enum {
/*! Selector used to manipulate the visual feature \f$s=
^{c^*}t_c\f$ which gives the position of the current camera frame
relative to the desired camera frame.*/
- cdMc,
+ cdMc,
/*! Selector used to manipulate the visual feature \f$s=
^{c}t_{c^*}\f$ which gives the position of the desired camera frame
relative to the current camera frame.*/
- cMcd,
+ cMcd,
/*! Selector used to manipulate the visual feature \f$s=
^{c}t_o\f$ which gives the position of the object frame relative to
the current camera frame. */
- cMo
+ cMo
} vpFeatureTranslationRepresentationType;
// basic contructor
- vpFeatureTranslation() ;
+ vpFeatureTranslation();
// basic constructor specifying the type of translation feature
- vpFeatureTranslation(vpFeatureTranslationRepresentationType r) ;
+ 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() {}
// build from an homogeneous matrix
// cdMc is the displacement that the camera has to realize
- void buildFrom(const vpHomogeneousMatrix &f2Mf1) ;
+ void buildFrom(const vpHomogeneousMatrix &f2Mf1);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
//! Feature duplication
- vpFeatureTranslation *duplicate() const ;
+ vpFeatureTranslation *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);
vpFeatureTranslationRepresentationType getFeatureTranslationType() const;
- double get_Tx() const ;
- double get_Ty() const ;
- double get_Tz() const ;
+ double get_Tx() const;
+ double get_Ty() const;
+ double get_Tz() const;
// basic construction
- void init() ;
+ void init();
// compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
// print the name of the feature
- void print(const unsigned int select= FEATURE_ALL) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
- void set_Tx(const double t_x) ;
- void set_Ty(const double t_y) ;
- void set_Tz(const double t_z) ;
+ void set_Tx(const double t_x);
+ void set_Ty(const double t_y);
+ void set_Tz(const double t_z);
void setFeatureTranslationType(const vpFeatureTranslationRepresentationType r);
-
// feature selection
static unsigned int selectTx();
static unsigned int selectTy();
@@ -355,7 +350,6 @@ private:
//! displacement that the camera has to realize
vpHomogeneousMatrix f2Mf1;
vpFeatureTranslationRepresentationType translation;
-} ;
-
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h
index e57f5a8..2bef898 100644
--- a/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h
+++ b/modules/visual_features/include/visp3/visual_features/vpFeatureVanishingPoint.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,26 +36,21 @@
*
*****************************************************************************/
-
#ifndef vpFeatureVanishingPoint_H
#define vpFeatureVanishingPoint_H
-
-
/*!
\file vpFeatureVanishingPoint.h \brief Class that defines 2D vanishing
point visual feature (Z coordinate in 3D space is infinity)
*/
#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpRGBa.h>
-
-
/*!
\class vpFeatureVanishingPoint
\ingroup group_visual_features
@@ -65,67 +61,59 @@
class VISP_EXPORT vpFeatureVanishingPoint : public vpBasicFeature
{
public:
- typedef enum
- {
- X = 1, // x coordinates
- Y = 2 // y coordinates
- } vpFeatureVanishingPointType;
+ typedef enum {
+ X = 1, // x coordinates
+ Y = 2 // y coordinates
+ } vpFeatureVanishingPointType;
/*
attributes and members directly related to the vpBasicFeature needs
other functionalities ar useful but not mandatory
*/
- //no Z required
+ // no Z required
public:
//! Default constructor.
- vpFeatureVanishingPoint() ;
+ vpFeatureVanishingPoint();
//! Destructor.
virtual ~vpFeatureVanishingPoint() {}
// void buildFrom(const vpPoint &p) ;
- void buildFrom(const double _x, const double _y) ;
+ void buildFrom(const double _x, const double _y);
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
//! feature duplication
- vpFeatureVanishingPoint *duplicate() const ;
+ vpFeatureVanishingPoint *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) ;
+ vpColVector error(const unsigned int select = FEATURE_ALL);
//! get the point x-coordinates
- double get_x() const ;
+ double get_x() const;
//! get the point y-coordinates
- double get_y() const ;
+ double get_y() const;
//! Default initialization.
- void init() ;
+ void init();
//! compute the interaction matrix from a subset a the possible features
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
//! print the name of the feature
- void print(const unsigned int select = FEATURE_ALL ) const ;
+ void print(const unsigned int select = FEATURE_ALL) const;
//! Set the point x-coordinates
- void set_x(const double _x) ;
+ void set_x(const double _x);
//! Set the point y-coordinates
- void set_y(const double _y) ;
+ void set_y(const double _y);
//! Set the point xy coordinates
- void set_xy(const double _x, const double _y) ;
-
-
+ void set_xy(const double _x, const double _y);
public:
/*
@@ -134,6 +122,6 @@ public:
// feature selection
static unsigned int selectX();
static unsigned int selectY();
-} ;
+};
#endif
diff --git a/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h
index e5f8b4d..c8fcc96 100644
--- a/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h
+++ b/modules/visual_features/include/visp3/visual_features/vpGenericFeature.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,8 +36,6 @@
*
*****************************************************************************/
-
-
#ifndef vpGenericFeature_hh
#define vpGenericFeature_hh
@@ -47,8 +46,8 @@
*/
#include <visp3/core/vpMatrix.h>
-#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/core/vpRGBa.h>
+#include <visp3/visual_features/vpBasicFeature.h>
#include <math.h>
@@ -56,9 +55,17 @@
\class vpGenericFeature
\ingroup group_core_features
- \brief Class that enables to define a feature or a set of features which are not implemented in ViSP as a specific class. It is indeed possible to create its own features, to use the corresponding interaction matrix, and to compute an error between the current and the desired feature. Moreover the created features can be mixed with features already implemented.
-
- The following example shows how to use the vpGenericFeature class to create and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The interaction matrix corresponding to this feature is \f[ L = \left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f].
+ \brief Class that enables to define a feature or a set of features which are
+not implemented in ViSP as a specific class. It is indeed possible to create
+its own features, to use the corresponding interaction matrix, and to compute
+an error between the current and the desired feature. Moreover the created
+features can be mixed with features already implemented.
+
+ The following example shows how to use the vpGenericFeature class to create
+and use the feature \f$ log(Z) \f$ where Z corresponds to the depth of a point
+whose 2D coordinates in the camera frame are \f$ x \f$ and \f$ y \f$. The
+interaction matrix corresponding to this feature is \f[ L =
+\left[\begin{array}{cccccc} 0 & 0 & -1/Z & -y & x & 0 \end{array}\right]\f].
\code
#include <visp3/core/vpGenericFeature.h>
#include <visp3/vs/vpServo.h>
@@ -81,7 +88,7 @@ int main()
vpGenericFeature logZ(1); //The dimension of the feature is 1.
logZ.set_s( log(Z) );
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current visual features sd
@@ -93,7 +100,7 @@ int main()
// Control loop
for ( ; ; ) {
// The new parameters x, y and Z must be computed here.
-
+
// Update the current point visual feature
logZ.set_s( log(Z) ) ;
@@ -105,7 +112,7 @@ int main()
LlogZ[0][4] = x;
logZ.setInteractionMatrix(LlogZ) ;
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -113,7 +120,9 @@ int main()
}
\endcode
-The second example shows how to create and use a feature whose specificity is to have a desired feature fixed to zero. It is the case for the feature \f$ log( \frac{Z}{Z^*}) \f$.
+The second example shows how to create and use a feature whose specificity is
+to have a desired feature fixed to zero. It is the case for the feature \f$
+log( \frac{Z}{Z^*}) \f$.
\code
#include <visp3/core/vpGenericFeature.h>
@@ -135,7 +144,7 @@ int main()
vpGenericFeature logZ(1); //The dimension of the feature is 1.
logZ.set_s( log(Z/Zd) );
- // Set eye-in-hand control law.
+ // Set eye-in-hand control law.
// The computed velocities will be expressed in the camera frame
task.setServo(vpServo::EYEINHAND_CAMERA);
// Interaction matrix is computed with the current visual features sd
@@ -147,7 +156,7 @@ int main()
// Control loop
for ( ; ; ) {
// The new parameters x, y and Z must be computed here.
-
+
// Update the current point visual feature
logZ.set_s( log(Z/Zd) ) ;
@@ -159,7 +168,7 @@ int main()
LlogZ[0][4] = x;
logZ.setInteractionMatrix(LlogZ) ;
-
+
// compute the control law
vpColVector v = task.computeControlLaw(); // camera velocity
}
@@ -167,64 +176,53 @@ int main()
}
\endcode
-If the feature needs to be use with other features, the example servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it.
+If the feature needs to be use with other features, the example
+servoSimuPoint2DhalfCamVelocity2.cpp shows how to do it.
*/
class VISP_EXPORT vpGenericFeature : public vpBasicFeature
{
private:
- vpGenericFeature() ;
+ vpGenericFeature();
public:
- vpGenericFeature(unsigned int dim) ;
- virtual ~vpGenericFeature() ;
+ explicit vpGenericFeature(unsigned int dim);
+ virtual ~vpGenericFeature();
- void display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
- void display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color=vpColor::green,
- unsigned int thickness=1) const ;
+ void display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
+ void display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color = vpColor::green,
+ unsigned int thickness = 1) const;
- vpGenericFeature *duplicate() 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);
- vpColVector error(const unsigned int select = FEATURE_ALL) ;
-
- vpMatrix getInteractionMatrix() const { return L ; }
+ vpMatrix getInteractionMatrix() const { return L; }
void get_s(vpColVector &s) const;
void get_s(double &s0) const;
void get_s(double &s0, double &s1) const;
void get_s(double &s0, double &s1, double &s2) const;
- void init() ;
-
- vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void init();
- void print(const unsigned int select = FEATURE_ALL ) const ;
- void setInteractionMatrix(const vpMatrix &L) ;
- void setError(const vpColVector &error_vector) ;
- void set_s(const vpColVector &s) ;
- void set_s(const double s0) ;
- void set_s(const double s0, const double s1) ;
- void set_s(const double s0, const double s1, const double s2) ;
+ vpMatrix interaction(const unsigned int select = FEATURE_ALL);
+ void print(const unsigned int select = FEATURE_ALL) const;
+ void setInteractionMatrix(const vpMatrix &L);
+ void setError(const vpColVector &error_vector);
+ void set_s(const vpColVector &s);
+ void set_s(const double s0);
+ void set_s(const double s0, const double s1);
+ void set_s(const double s0, const double s1, const double s2);
private:
- typedef enum
- {
- errorNotInitalized,
- errorInitialized,
- errorHasToBeUpdated
- } vpGenericFeatureErrorType;
-
- vpMatrix L ;
- vpColVector err ;
- vpGenericFeatureErrorType errorStatus ;
-} ;
+ typedef enum { errorNotInitalized, errorInitialized, errorHasToBeUpdated } vpGenericFeatureErrorType;
+
+ vpMatrix L;
+ vpColVector err;
+ vpGenericFeatureErrorType errorStatus;
+};
#endif
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp
index 147e87a..5e36f3c 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderEllipse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,68 +36,60 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderEllipse.cpp
\brief conversion between tracker
and visual feature Ellipse
*/
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpMath.h>
-
-
+#include <visp3/visual_features/vpFeatureBuilder.h>
/*!
Initialize an ellipse feature thanks to a vpCircle.
- The vpFeatureEllipse is initialized thanks to the parameters of the circle in the camera frame and in the image plan.
- All the parameters are given in meter.
+ The vpFeatureEllipse is initialized thanks to the parameters of the circle
+ in the camera frame and in the image plan. All the parameters are given in
+ meter.
\warning To be sure that the vpFeatureEllipse is well initialized,
- you have to be sure that at least the circle coordinates in the image
+ you have to be sure that at least the circle coordinates in the image
plan and in the camera frame are computed and stored in the vpCircle.
\param s : Visual feature to initialize.
\param t : The vpCircle used to create the vpFeatureEllipse.
*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCircle &t )
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCircle &t)
{
- try
- {
+ try {
// 3D data
- double alpha = t.cP[0] ;
- double beta = t.cP[1] ;
- double gamma = t.cP[2] ;
+ double alpha = t.cP[0];
+ double beta = t.cP[1];
+ double gamma = t.cP[2];
- double X0 = t.cP[3] ;
- double Y0 = t.cP[4] ;
- double Z0 = t.cP[5] ;
+ double X0 = t.cP[3];
+ double Y0 = t.cP[4];
+ double Z0 = t.cP[5];
// equation p 318 prior eq (39)
- double d = alpha*X0 + beta*Y0 + gamma*Z0 ;
-
- double A = alpha / d ;
- double B = beta / d ;
- double C = gamma / d ;
+ double d = alpha * X0 + beta * Y0 + gamma * Z0;
- s.setABC(A,B,C) ;
+ double A = alpha / d;
+ double B = beta / d;
+ double C = gamma / d;
+ s.setABC(A, B, C);
- //2D data
- s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
+ // 2D data
+ s.buildFrom(t.p[0], t.p[1], t.p[2], t.p[3], t.p[4]);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
-
}
-
/*!
Initialize an ellipse feature thanks to a vpSphere.
The vpFeatureEllipse is initialized thanks to the parameters
@@ -104,47 +97,40 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCircle &t )
All the parameters are given in meter.
\warning To be sure that the vpFeatureEllipse is well initialized,
- you have to be sure that at least the sphere coordinates in the image
+ you have to be sure that at least the sphere coordinates in the image
plan and in the camera frame are computed and stored in the vpSphere.
\param s : Visual feature to initialize.
\param t : The vpSphere used to create the vpFeatureEllipse.
*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t)
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t)
{
- try
- {
+ try {
// 3D data
- double X0 = t.cP[0] ;
- double Y0 = t.cP[1] ;
- double Z0 = t.cP[2] ;
- double R = t.cP[3] ;
-
- double d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) -
- vpMath::sqr(R) ;
-
+ double X0 = t.cP[0];
+ double Y0 = t.cP[1];
+ double Z0 = t.cP[2];
+ double R = t.cP[3];
- double A = X0 / d ;
- double B = Y0 / d ;
- double C = Z0 / d ;
+ double d = vpMath::sqr(X0) + vpMath::sqr(Y0) + vpMath::sqr(Z0) - vpMath::sqr(R);
- s.setABC(A,B,C) ;
+ double A = X0 / d;
+ double B = Y0 / d;
+ double C = Z0 / d;
- //2D data
- s.buildFrom( t.p[0], t.p[1], t.p[2], t.p[3], t.p[4] ) ;
+ s.setABC(A, B, C);
+ // 2D data
+ s.buildFrom(t.p[0], t.p[1], t.p[2], t.p[3], t.p[4]);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
#ifdef VISP_HAVE_MODULE_BLOB
/*!
Initialize an ellipse feature thanks to a vpDot and camera parameters.
@@ -152,56 +138,54 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpSphere &t)
of the dot given in pixel. The camera parameters are used to convert the
pixel parameters to parameters given in meter.
- \warning With a vpDot there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
+ \warning With a vpDot there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them
+ and initialized them outside the method.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the vpDot.
\param t : The vpDot used to create the vpFeatureEllipse.
*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot &t )
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpDot &t)
{
- try
- {
+ try {
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
+ unsigned int order = 3;
+ vpMatrix mp(order, order);
+ mp = 0;
+ vpMatrix m(order, order);
+ m = 0;
- mp[0][0] = t.m00 ;
+ mp[0][0] = t.m00;
mp[1][0] = t.m10;
- mp[0][1] = t.m01 ;
- mp[2][0] = t.m20 ;
- mp[1][1] = t.m11 ;
- mp[0][2] = t.m02 ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ mp[0][1] = t.m01;
+ mp[2][0] = t.m20;
+ mp[1][1] = t.m11;
+ mp[0][2] = t.m02;
+
+ vpPixelMeterConversion::convertMoment(cam, order, mp, m);
+
+ double m00 = m[0][0];
+ double m01 = m[0][1];
+ double m10 = m[1][0];
+ double m02 = m[0][2];
+ double m11 = m[1][1];
+ double m20 = m[2][0];
+
+ double xc = m10 / m00; // sum j /S
+ double yc = m01 / m00; // sum i /S
+
+ double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
+ double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
+ double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -209,58 +193,56 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s,
Initialize an ellipse feature thanks to a vpDot2 and camera parameters.
The vpFeatureEllipse is initialized thanks to the parameters
of the dot given in pixel. The camera parameters are used to convert the
- pixel parameters to parameters given in meter.
+ pixel parameters to parameters given in meter.
- \warning With a vpDot2 there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
+ \warning With a vpDot2 there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them
+ and initialized them outside the method.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the vpDot2.
\param t : The vpDot2 used to create the vpFeatureEllipse.
*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpDot2 &t )
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpDot2 &t)
{
- try
- {
+ try {
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
+ unsigned int order = 3;
+ vpMatrix mp(order, order);
+ mp = 0;
+ vpMatrix m(order, order);
+ m = 0;
- mp[0][0] = t.m00 ;
+ mp[0][0] = t.m00;
mp[1][0] = t.m10;
- mp[0][1] = t.m01 ;
- mp[2][0] = t.m20 ;
- mp[1][1] = t.m11 ;
- mp[0][2] = t.m02 ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ mp[0][1] = t.m01;
+ mp[2][0] = t.m20;
+ mp[1][1] = t.m11;
+ mp[0][2] = t.m02;
+
+ vpPixelMeterConversion::convertMoment(cam, order, mp, m);
+
+ double m00 = m[0][0];
+ double m01 = m[0][1];
+ double m10 = m[1][0];
+ double m02 = m[0][2];
+ double m11 = m[1][1];
+ double m20 = m[2][0];
+
+ double xc = m10 / m00; // sum j /S
+ double yc = m01 / m00; // sum i /S
+
+ double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
+ double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
+ double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
#endif //#ifdef VISP_HAVE_MODULE_BLOB
@@ -270,61 +252,59 @@ void vpFeatureBuilder::create(vpFeatureEllipse &s,
Initialize an ellipse feature thanks to a vpMeEllipse and camera parameters.
The vpFeatureEllipse is initialized thanks to the parameters
of the ellipse given in pixel. The camera parameters are used to convert the
- pixel parameters to parameters given in meter.
+ pixel parameters to parameters given in meter.
- \warning With a vpMeEllipse there is no information about 3D parameters.
- Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them and
- initialized them outside the method.
+ \warning With a vpMeEllipse there is no information about 3D parameters.
+ Thus the parameters \f$(A,B,C)\f$ can not be set. You have to compute them
+ and initialized them outside the method.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the vpDot2.
\param t : The vpMeEllipse used to create the vpFeatureEllipse.
*/
-void vpFeatureBuilder::create(vpFeatureEllipse &s,
- const vpCameraParameters &cam,
- const vpMeEllipse &t )
+void vpFeatureBuilder::create(vpFeatureEllipse &s, const vpCameraParameters &cam, const vpMeEllipse &t)
{
- try
- {
-
- unsigned int order = 3 ;
- vpMatrix mp(order,order) ; mp =0 ;
- vpMatrix m(order,order) ; m = 0 ;
-
- //The opposite of vpDot and vpDot2 because moments in vpMeEllipse
- //are computed in the ij coordinate system whereas the moments in vpDot and vpDot2
- //are computed in the uv coordinate system
- mp[0][0] = t.get_m00() ;
+ try {
+
+ unsigned int order = 3;
+ vpMatrix mp(order, order);
+ mp = 0;
+ vpMatrix m(order, order);
+ m = 0;
+
+ // The opposite of vpDot and vpDot2 because moments in vpMeEllipse
+ // are computed in the ij coordinate system whereas the moments in vpDot
+ // and vpDot2 are computed in the uv coordinate system
+ mp[0][0] = t.get_m00();
mp[1][0] = t.get_m01();
- mp[0][1] = t.get_m10() ;
- mp[2][0] = t.get_m02() ;
- mp[1][1] = t.get_m11() ;
- mp[0][2] = t.get_m20() ;
-
- vpPixelMeterConversion::convertMoment(cam,order,mp,m) ;
-
- double m00 = m[0][0] ;
- double m01 = m[0][1] ;
- double m10 = m[1][0] ;
- double m02 = m[0][2] ;
- double m11 = m[1][1] ;
- double m20 = m[2][0] ;
-
- double xc = m10/m00 ; // sum j /S
- double yc = m01/m00 ; // sum i /S
-
- double mu20 = 4*(m20 - m00*vpMath::sqr(xc))/(m00) ;
- double mu02 = 4*(m02 - m00*vpMath::sqr(yc))/(m00) ;
- double mu11 = 4*(m11 - m00*xc*yc)/(m00) ;
-
- s.buildFrom(xc, yc, mu20, mu11, mu02 ) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ mp[0][1] = t.get_m10();
+ mp[2][0] = t.get_m02();
+ mp[1][1] = t.get_m11();
+ mp[0][2] = t.get_m20();
+
+ vpPixelMeterConversion::convertMoment(cam, order, mp, m);
+
+ double m00 = m[0][0];
+ double m01 = m[0][1];
+ double m10 = m[1][0];
+ double m02 = m[0][2];
+ double m11 = m[1][1];
+ double m20 = m[2][0];
+
+ double xc = m10 / m00; // sum j /S
+ double yc = m01 / m00; // sum i /S
+
+ double mu20 = 4 * (m20 - m00 * vpMath::sqr(xc)) / (m00);
+ double mu02 = 4 * (m02 - m00 * vpMath::sqr(yc)) / (m00);
+ double mu11 = 4 * (m11 - m00 * xc * yc) / (m00);
+
+ s.buildFrom(xc, yc, mu20, mu11, mu02);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
#endif //#ifdef VISP_HAVE_MODULE_ME
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp
index 5370bc9..29f4000 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,68 +36,61 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderLine.cpp
\brief conversion between tracker
and visual feature Line
*/
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpMath.h>
-
-
+#include <visp3/visual_features/vpFeatureBuilder.h>
/*!
Initialize a line feature thanks to a vpLine.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. In vpLine there are the parameters of two plans,
- but the one which have the biggest D parameter is copied in the vpFeatureLine parameters.
+ A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are
+ expressed in meter. It also contains the parameters of a plan equation
+ \f$(A,B,C,D)\f$. In vpLine there are the parameters of two plans, but the
+ one which have the biggest D parameter is copied in the vpFeatureLine
+ parameters.
\param s : Visual feature to initialize.
\param t : The vpLine used to create the vpFeatureLine.
*/
-void vpFeatureBuilder::create(vpFeatureLine &s, const vpLine &t )
+void vpFeatureBuilder::create(vpFeatureLine &s, const vpLine &t)
{
- try
- {
- double A,B,C,D ;
- s.setRhoTheta(t.getRho(),t.getTheta()) ;
+ try {
+ double A, B, C, D;
+ s.setRhoTheta(t.getRho(), t.getTheta());
if (fabs(t.cP[3]) > fabs(t.cP[7])) // |D1| > |D2|
{
- A = t.cP[0] ;
- B = t.cP[1] ;
- C = t.cP[2] ;
- D = t.cP[3] ;
- }
- else
- {
- A = t.cP[4] ;
- B = t.cP[5] ;
- C = t.cP[6] ;
- D = t.cP[7] ;
+ A = t.cP[0];
+ B = t.cP[1];
+ C = t.cP[2];
+ D = t.cP[3];
+ } else {
+ A = t.cP[4];
+ B = t.cP[5];
+ C = t.cP[6];
+ D = t.cP[7];
}
+ s.setABCD(A, B, C, D);
- s.setABCD(A,B,C,D) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
-
}
/*!
Initialize a line feature thanks to a vpCylinder.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- It also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are computed
- thanks to the parameters that are contained in vpCylinder. It is possible to choose which edge of the cylinder to use to
- initialize the vpFeatureLine.
+ A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are
+ expressed in meter. It also contains the parameters of a plan equation
+ \f$(A,B,C,D)\f$. These parameters are computed thanks to the parameters that
+ are contained in vpCylinder. It is possible to choose which edge of the
+ cylinder to use to initialize the vpFeatureLine.
\param s : Visual feature to initialize.
@@ -105,77 +99,68 @@ 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
- {
+ try {
- double a = t.getA() ;
- double b = t.getB() ;
- double c = t.getC() ;
+ double a = t.getA();
+ double b = t.getB();
+ double c = t.getC();
- double x0 = t.getX() ;
- double y0 = t.getY() ;
- double z0 = t.getZ() ;
+ double x0 = t.getX();
+ double y0 = t.getY();
+ double z0 = t.getZ();
- double R = t.getR() ;
+ 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);
+ vpMath::sqr(x0) + vpMath::sqr(y0) + vpMath::sqr(z0) - vpMath::sqr(R) - vpMath::sqr(a * x0 + b * y0 + c * z0);
- double alpha1 = (1 - a*a)*x0 - a*b*y0 - a*c*z0;
- double beta1 = -a*b*x0 + (1 - b*b)*y0 - b*c*z0;
- double gamma1 = -a*c*x0 - b*c*y0 + (1 - c*c)*z0;
+ double alpha1 = (1 - a * a) * x0 - a * b * y0 - a * c * z0;
+ double beta1 = -a * b * x0 + (1 - b * b) * y0 - b * c * z0;
+ double gamma1 = -a * c * x0 - b * c * y0 + (1 - c * c) * z0;
- D*=-1 ;
+ D *= -1;
- if (D<0)
- {
- alpha1*=-1 ;
- beta1*=-1 ;
- gamma1*=-1 ;
- D*=-1 ;
+ if (D < 0) {
+ alpha1 *= -1;
+ beta1 *= -1;
+ gamma1 *= -1;
+ D *= -1;
}
- s.setABCD(alpha1,beta1,gamma1,D) ;
+ s.setABCD(alpha1, beta1, gamma1, D);
- if (line==vpCylinder::line1)
- {
+ if (line == vpCylinder::line1) {
- s.setRhoTheta(t.getRho1(),t.getTheta1()) ;
+ s.setRhoTheta(t.getRho1(), t.getTheta1());
- }
- else
- {
+ } else {
- s.setRhoTheta(t.getRho2(),t.getTheta2()) ;
+ s.setRhoTheta(t.getRho2(), t.getTheta2());
}
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
#ifdef VISP_HAVE_MODULE_ME
/*!
- Initialize a line feature thanks to a vpMeLine and the parameters of the camera.
- A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which are expressed in meter.
- In vpMeLine these parameters are given in pixel. The conversion is done thanks to the camera parameters.
+ Initialize a line feature thanks to a vpMeLine and the parameters of the
+ camera. A vpFeatureLine contains the parameters \f$(\rho,\theta)\f$ which
+ are expressed in meter. In vpMeLine these parameters are given in pixel. The
+ conversion is done thanks to the camera parameters.
- \warning vpFeatureLine also contains the parameters of a plan equation \f$(A,B,C,D)\f$. These parameters are needed to
- compute the interaction matrix but can not be computed thanks to a vpMeLine. You have to compute and set these parameters
- outside the function.
+ \warning vpFeatureLine also contains the parameters of a plan equation
+ \f$(A,B,C,D)\f$. These parameters are needed to compute the interaction
+ matrix but can not be computed thanks to a vpMeLine. You have to compute and
+ set these parameters outside the function.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the line.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the line.
\param t : The vpLine used to create the vpFeatureLine.
@@ -196,7 +181,7 @@ void vpFeatureBuilder::create(vpFeatureLine &s,
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, line);
-
+
// A pose estimation is requested to initialize A, B, C and D the
//parameters of the equation plan.
double A = 1;
@@ -207,41 +192,39 @@ void vpFeatureBuilder::create(vpFeatureLine &s,
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() ;
+ try {
+ double rhop = t.getRho();
double thetap = t.getTheta();
- double rho ;
- double theta ;
+ double rho;
+ double theta;
- //Gives the rho and theta coordinates in the (u,v) coordinate system.
- if (thetap >= 0 && thetap < M_PI/2)
- {
- thetap = M_PI/2 - thetap;
+ // Gives the rho and theta coordinates in the (u,v) coordinate system.
+ if (thetap >= 0 && thetap < M_PI / 2) {
+ thetap = M_PI / 2 - thetap;
}
- else if (thetap >= M_PI/2 && thetap < 3*M_PI/2)
- {
- thetap = 3*M_PI/2 + M_PI - thetap;
+ else if (thetap >= M_PI / 2 && thetap < 3 * M_PI / 2) {
+ thetap = 3 * M_PI / 2 + M_PI - thetap;
}
- else if (thetap >= 3*M_PI/2 && thetap <= 2*M_PI)
- {
- thetap = M_PI/2 + 2*M_PI - thetap;
+ else if (thetap >= 3 * M_PI / 2 && thetap <= 2 * M_PI) {
+ thetap = M_PI / 2 + 2 * M_PI - thetap;
}
- //while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
- //while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
+ // while (thetap > M_PI/2) { thetap -= M_PI ; rhop *= -1 ; }
+ // while (thetap < -M_PI/2) { thetap += M_PI ; rhop *= -1 ; }
// vpTRACE("pixel %f %f",rhop, thetap) ;
- vpPixelMeterConversion::convertLine(cam,rhop,thetap, rho,theta) ;
+ vpPixelMeterConversion::convertLine(cam, rhop, thetap, rho, theta);
- while (theta > M_PI) { theta -= 2*M_PI ; }
- while (theta < -M_PI) { theta += 2*M_PI ; }
+ while (theta > M_PI) {
+ theta -= 2 * M_PI;
+ }
+ while (theta < -M_PI) {
+ theta += 2 * M_PI;
+ }
// vpTRACE("meter %f %f",rho, theta) ;
/*
@@ -263,14 +246,12 @@ vpFeatureBuilder::create(vpFeatureLine &s,
rho *= -1 ;
}
*/
- s.buildFrom(rho,theta) ;
+ s.buildFrom(rho, theta);
// vpTRACE("meter %f %f",rho, theta) ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
- }
}
#endif //#ifdef VISP_HAVE_MODULE_ME
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp
index a197773..81131e4 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,31 +36,34 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderPoint.cpp
\brief conversion between tracker
and visual feature Point
*/
+#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpException.h>
#ifdef VISP_HAVE_MODULE_BLOB
/*!
Create a vpFeaturePoint thanks to a vpDot and the parameters of the camera.
The vpDot contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
+ Thus this method uses the camera parameters to compute the meter coordinates
+ \f$ x \f$ and \f$ y \f$ in the image plan. Those coordinates are stored in
+ the vpFeaturePoint.
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in
+ the camera frame thanks to a vpDot. This coordinate is needed in
+ vpFeaturePoint to compute the interaction matrix. So this value must be
+ computed outside this function.
\param s : Visual feature \f$(x, y)\f$ to initialize. Be
aware, the 3D depth \f$Z\f$ requested to compute the interaction
matrix is not initialized by this function.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot.
- \param d : The vpDot used to create the vpFeaturePoint.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the vpDot. \param d : The vpDot used to create the
+ vpFeaturePoint.
The code below shows how to initialize a vpFeaturePoint visual
feature. First, we initialize the \f$x,y\f$, and lastly we
@@ -78,7 +82,7 @@
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -86,42 +90,40 @@
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;
+ try {
+ double x = 0, y = 0;
vpImagePoint cog;
cog = d.getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ s.set_x(x);
+ s.set_y(y);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
Create a vpFeaturePoint thanks to a vpDot2 and the parameters of the camera.
The vpDot2 contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
+ Thus this method uses the camera parameters to compute the meter coordinates
+ \f$ x \f$ and \f$ y \f$ in the image plan. Those coordinates are stored in
+ the vpFeaturePoint.
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpDot2.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in
+ the camera frame thanks to a vpDot2. This coordinate is needed in
+ vpFeaturePoint to compute the interaction matrix. So this value must be
+ computed outside this function.
\param s : The feature point.
- \param cam : The parameters of the camera used to acquire the image containing the vpDot2.
- \param d : The vpDot2 used to create the vpFeaturePoint.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the vpDot2. \param d : The vpDot2 used to create the
+ vpFeaturePoint.
The code below shows how to initialize a vpFeaturePoint visual
feature. First, we initialize the \f$x,y\f$, and lastly we
@@ -140,7 +142,7 @@ void vpFeatureBuilder::create(vpFeaturePoint &s,
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -148,43 +150,41 @@ 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;
+ try {
+ double x = 0, y = 0;
vpImagePoint cog;
cog = d.getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ s.set_x(x);
+ s.set_y(y);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
#endif //#ifdef VISP_HAVE_MODULE_BLOB
-
/*!
- Create a vpFeaturePoint thanks to a vpImagePoint and the parameters of the camera.
- The vpImagePoint contains only the pixel coordinates of the point in an image.
- Thus this method uses the camera parameters to compute the meter coordinates \f$ x \f$ and \f$ y \f$ in the image plan.
- Those coordinates are stored in the vpFeaturePoint.
+ Create a vpFeaturePoint thanks to a vpImagePoint and the parameters of the
+ camera. The vpImagePoint contains only the pixel coordinates of the point in
+ an image. Thus this method uses the camera parameters to compute the meter
+ coordinates \f$ x \f$ and \f$ y \f$ in the image plan. Those coordinates are
+ stored in the vpFeaturePoint.
- \warning It is not possible to compute the depth of the point \f$ Z \f$ in the camera frame thanks to a vpImagePoint.
- This coordinate is needed in vpFeaturePoint to compute the interaction matrix. So this value must be computed outside this function.
+ \warning It is not possible to compute the depth of the point \f$ Z \f$ in
+ the camera frame thanks to a vpImagePoint. This coordinate is needed in
+ vpFeaturePoint to compute the interaction matrix. So this value must be
+ computed outside this function.
\param s : The feature point.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param ip : The vpImagePoint used to create the vpFeaturePoint.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the point. \param ip : The vpImagePoint used to create the
+ vpFeaturePoint.
The code below shows how to initialize a vpFeaturePoint visual
feature. First, we initialize the \f$x,y\f$, and lastly we
@@ -198,13 +198,12 @@ void vpFeatureBuilder::create(vpFeaturePoint &s,
vpFeaturePoint s; // Point feature
...
- // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
- iP.set_i(0);
- iP.set_j(0);
+ // Set the point coordinates in the image (here the coordinates are given in
+ the (i,j) frame iP.set_i(0); iP.set_j(0);
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, iP);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -212,86 +211,81 @@ 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;
+ try {
+ double x = 0, y = 0;
- vpPixelMeterConversion::convertPoint(cam, ip, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, ip, x, y);
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ s.set_x(x);
+ s.set_y(y);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
Create a vpFeaturePoint thanks to a vpPoint.
- This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
- The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
+ This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image
+ plan to set the visual feature parameters. The value of the depth \f$ Z \f$
+ in the camera frame is also computed thanks to the coordinates in the camera
+ frame which are stored in vpPoint.
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to
+ be sure that at least the point coordinates in the image plan and in the
+ camera frame are computed and stored in the vpPoint.
\param s : The feature point.
\param p : The vpPoint used to create the vpFeaturePoint.
*/
-void
-vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p)
+void vpFeatureBuilder::create(vpFeaturePoint &s, const vpPoint &p)
{
- try
- {
- s.set_x( p.get_x()) ;
- s.set_y( p.get_y()) ;
+ try {
+ s.set_x(p.get_x());
+ s.set_y(p.get_y());
- s.set_Z( p.cP[2]/p.cP[3]) ;
+ s.set_Z(p.cP[2] / p.cP[3]);
- if (s.get_Z() < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
+ if (s.get_Z() < 0) {
+ vpERROR_TRACE("Point is behind the camera ");
+ std::cout << "Z = " << s.get_Z() << std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
+ 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 ;
+ 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(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
- Create a vpFeaturePoint thanks to a vpPoint. In this method noise is introduced during the initialization of the vpFeaturePoint.
- This method uses the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the visual feature parameters.
- The value of the depth \f$ Z \f$ in the camera frame is also computed thanks to the coordinates in the camera frame which are stored in vpPoint.
-
- This function intends to introduce noise after the initialization of the parameters. Cartesian \f$(x,y)\f$ coordinates
- are first converted in pixel coordinates in the image using \e
- goodCam camera parameters. Then, the pixels coordinates of the point
- are converted back to cartesian coordinates \f$(x^{'},y^{'})\f$ using
- the noisy camera parameters \e wrongCam. These last parameters are stored in the vpFeaturePoint.
-
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point coordinates in the image plan and in the camera frame are computed and stored in the vpPoint.
+ Create a vpFeaturePoint thanks to a vpPoint. In this method noise is
+ introduced during the initialization of the vpFeaturePoint. This method uses
+ the point coordinates \f$ x \f$ and \f$ y \f$ in the image plan to set the
+ visual feature parameters. The value of the depth \f$ Z \f$ in the camera
+ frame is also computed thanks to the coordinates in the camera frame which
+ are stored in vpPoint.
+
+ This function intends to introduce noise after the initialization of the
+ parameters. Cartesian \f$(x,y)\f$ coordinates are first converted in pixel
+ coordinates in the image using \e goodCam camera parameters. Then, the
+ pixels coordinates of the point are converted back to cartesian coordinates
+ \f$(x^{'},y^{'})\f$ using the noisy camera parameters \e wrongCam. These
+ last parameters are stored in the vpFeaturePoint.
+
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to
+ be sure that at least the point coordinates in the image plan and in the
+ camera frame are computed and stored in the vpPoint.
\param s : The feature point.
@@ -305,30 +299,23 @@ 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,
- const vpPoint &p)
+void vpFeatureBuilder::create(vpFeaturePoint &s, const vpCameraParameters &goodCam, const vpCameraParameters &wrongCam,
+ const vpPoint &p)
{
- try
- {
- double x = p.p[0] ;
- double y = p.p[1] ;
+ try {
+ double x = p.p[0];
+ double y = p.p[1];
- s.set_Z( p.cP[2]/p.cP[3]) ;
+ s.set_Z(p.cP[2] / p.cP[3]);
- double u=0, v=0;
- vpMeterPixelConversion::convertPoint(goodCam,x,y,u,v) ;
- vpPixelMeterConversion::convertPoint(wrongCam,u,v,x,y) ;
+ double u = 0, v = 0;
+ vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
+ vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
-
- s.set_x(x) ;
- s.set_y(y) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ s.set_x(x);
+ s.set_y(y);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp
index 9e0ca20..aabb622 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPoint3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,16 +36,14 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderPoint3D.cpp
\brief conversion between tracker
and visual feature 3D Point
*/
+#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpException.h>
-
/*!
@@ -52,36 +51,28 @@
\f$(X,Y,Z)\f$ in the camera frame. The values of \f$(X,Y,Z)\f$ are
expressed in meters.
- \warning To be sure that the vpFeaturePoint is well initialized, you have to be sure that at least the point
- coordinates in the camera frame are computed and stored in the vpPoint.
+ \warning To be sure that the vpFeaturePoint is well initialized, you have to
+ be sure that at least the point coordinates in the camera frame are computed
+ and stored in the vpPoint.
\param s : Visual feature to initialize.
\param t : The vpPoint used to create the vpFeaturePoint.
*/
-void
-vpFeatureBuilder::create(vpFeaturePoint3D &s, const vpPoint &t )
+void vpFeatureBuilder::create(vpFeaturePoint3D &s, const vpPoint &t)
{
- try
- {
-
+ try {
+ s.set_X(t.cP[0] / t.cP[3]);
+ s.set_Y(t.cP[1] / t.cP[3]);
+ s.set_Z(t.cP[2] / t.cP[3]);
- s.set_X( t.cP[0]/t.cP[3]) ;
- s.set_Y( t.cP[1]/t.cP[3]) ;
- s.set_Z( t.cP[2]/t.cP[3]) ;
-
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp
index e1fbd85..966cad8 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderPointPolar.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,16 +37,15 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderPointPolar.cpp
\brief Conversion between tracker and visual feature point with
polar coordinates.
*/
+#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpException.h>
#ifdef VISP_HAVE_MODULE_BLOB
/*!
@@ -63,11 +63,11 @@
\warning This function does not initialize \f$Z\f$ which is
requested to compute the interaction matrix by
vpfeaturePointPolar::interaction().
-
+
\param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
aware, the 3D depth \f$Z\f$ requested to compute the interaction
matrix is not initialized by this function.
-
+
\param cam : Camera parameters.
\param dot : Tracked dot. The center of gravity corresponds to the
@@ -90,7 +90,7 @@
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -99,31 +99,27 @@
\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;
+ double x = 0, y = 0;
vpImagePoint cog;
cog = dot.getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- double rho = sqrt(x*x + y*y);
+ double rho = sqrt(x * x + y * y);
double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+
+ s.set_rho(rho);
+ s.set_theta(theta);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
Initialize a point feature with polar coordinates
@@ -139,11 +135,11 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s,
\warning This function does not initialize \f$Z\f$ which is
requested to compute the interaction matrix by
vpfeaturePointPolar::interaction().
-
+
\param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
aware, the 3D depth \f$Z\f$ requested to compute the interaction
matrix is not initialized by this function.
-
+
\param cam : Camera parameters.
\param dot : Tracked dot. The center of gravity corresponds to the
@@ -166,7 +162,7 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s,
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, dot);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -175,51 +171,47 @@ 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;
+ double x = 0, y = 0;
vpImagePoint cog;
cog = dot.getCog();
- vpPixelMeterConversion::convertPoint(cam, cog, x, y) ;
+ vpPixelMeterConversion::convertPoint(cam, cog, x, y);
- double rho = sqrt(x*x + y*y);
+ double rho = sqrt(x * x + y * y);
double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+
+ s.set_rho(rho);
+ s.set_theta(theta);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
#endif //#ifdef VISP_HAVE_MODULE_BLOB
-
/*!
Initialize a point feature with polar coordinates
\f$(\rho,\theta)\f$ using the coordinates of the point in pixels
- obtained by image processing. The points coordinates are stored in a vpImagePoint. Using the camera
- parameters, the pixels coordinates of the point are first
- converted in cartesian \f$(x,y)\f$ coordinates in meter in the
- camera frame and than in polar coordinates by:
+ obtained by image processing. The points coordinates are stored in a
+ vpImagePoint. Using the camera parameters, the pixels coordinates of the
+ point are first converted in cartesian \f$(x,y)\f$ coordinates in meter in
+ the camera frame and than in polar coordinates by:
\f[\rho = \sqrt{x^2+y^2} \hbox{,}\; \; \theta = \arctan \frac{y}{x}\f]
\warning This function does not initialize \f$Z\f$ which is
requested to compute the interaction matrix by
vpfeaturePointPolar::interaction().
-
+
\param s : Visual feature \f$(\rho,\theta)\f$ to initialize. Be
aware, the 3D depth \f$Z\f$ requested to compute the interaction
matrix is not initialized by this function.
-
+
\param cam : Camera parameters.
\param iP : The vpImagePoint used to create the vpFeaturePoint.
@@ -236,13 +228,12 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s,
vpFeaturePointPolar s; // Point feature with polar coordinates
...
- // Set the point coordinates in the image (here the coordinates are given in the (i,j) frame
- iP.set_i(0);
- iP.set_j(0);
+ // Set the point coordinates in the image (here the coordinates are given in
+ the (i,j) frame iP.set_i(0); iP.set_j(0);
// Initialize rho,theta visual feature
vpFeatureBuilder::create(s, cam, iP);
-
+
// A pose estimation is requested to initialize Z, the depth of the
// point in the camera frame.
double Z = 1; // Depth of the point in meters
@@ -251,28 +242,24 @@ 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;
+ double x = 0, y = 0;
- vpPixelMeterConversion::convertPoint(cam,iP,x,y) ;
+ vpPixelMeterConversion::convertPoint(cam, iP, x, y);
- double rho = sqrt(x*x + y*y);
+ double rho = sqrt(x * x + y * y);
double theta = atan2(y, x);
-
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+
+ s.set_rho(rho);
+ s.set_theta(theta);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
Initialize a point feature with polar coordinates
@@ -291,43 +278,38 @@ void vpFeatureBuilder::create(vpFeaturePointPolar &s,
image plane corresponding to the camera perspective projection, and
with 3D depth \f$Z\f$.
*/
-void
-vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p)
+void vpFeatureBuilder::create(vpFeaturePointPolar &s, const vpPoint &p)
{
try {
double x = p.get_x();
double y = p.get_y();
- double rho = sqrt(x*x + y*y);
+ double rho = sqrt(x * x + y * y);
double theta = atan2(y, x);
- s.set_rho(rho) ;
- s.set_theta(theta) ;
-
- s.set_Z( p.get_Z() ) ;
+ s.set_rho(rho);
+ s.set_theta(theta);
+ s.set_Z(p.get_Z());
if (s.get_Z() < 0) {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << s.get_Z() << std::endl ;
+ 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 ;
+ 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(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -364,35 +346,30 @@ 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();
double y = p.get_y();
- s.set_Z( p.get_Z() );
+ s.set_Z(p.get_Z());
- double u=0, v=0;
+ double u = 0, v = 0;
vpMeterPixelConversion::convertPoint(goodCam, x, y, u, v);
vpPixelMeterConversion::convertPoint(wrongCam, u, v, x, y);
- double rho = sqrt(x*x + y*y);
+ double rho = sqrt(x * x + y * y);
double theta = atan2(y, x);
- s.set_rho(rho) ;
- s.set_theta(theta) ;
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ s.set_rho(rho);
+ s.set_theta(theta);
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp
index 05ceb7d..b03a532 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderSegment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,14 +37,13 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderSegment.cpp
\brief Segment creation out of dots.
*/
-#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/core/vpMath.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#ifdef VISP_HAVE_MODULE_BLOB
@@ -51,74 +51,68 @@
Initialize a segment feature out of vpDots and camera parameters.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param d1 : The dot corresponding to the first point of the segment.
- \param d2 : The dot corresponding to the second point of the segment.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the point. \param d1 : The dot corresponding to the first point
+ of the segment. \param d2 : The dot corresponding to the second point of the
+ segment.
*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpDot &d1, const vpDot &d2 )
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot &d1, const vpDot &d2)
{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
+ double x1 = 0, y1 = 0, x2 = 0, y2 = 0;
+
+ vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1);
+ vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2);
+
+ double xc = (x1 + x2) / 2.;
+ double yc = (y1 + y2) / 2.;
+ double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+
+ if (s.isNormalized()) {
+ s.setXc(xc / l);
+ s.setYc(yc / l);
+ s.setL(1 / l);
+ } else {
+ s.setXc(xc);
+ s.setYc(yc);
+ s.setL(l);
}
- s.setAlpha(atan2(y1-y2,x1-x2));
+ s.setAlpha(atan2(y1 - y2, x1 - x2));
}
/*!
Initialize a segment feature out of vpDots and camera parameters.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param d1 : The dot corresponding to the first point of the segment.
- \param d2 : The dot corresponding to the second point of the segment.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the point. \param d1 : The dot corresponding to the first point
+ of the segment. \param d2 : The dot corresponding to the second point of the
+ segment.
*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpDot2 &d1, const vpDot2 &d2)
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpDot2 &d1, const vpDot2 &d2)
{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
+ double x1 = 0, y1 = 0, x2 = 0, y2 = 0;
+
+ vpPixelMeterConversion::convertPoint(cam, d1.getCog(), x1, y1);
+ vpPixelMeterConversion::convertPoint(cam, d2.getCog(), x2, y2);
+
+ double xc = (x1 + x2) / 2.;
+ double yc = (y1 + y2) / 2.;
+ double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+
+ if (s.isNormalized()) {
+ s.setXc(xc / l);
+ s.setYc(yc / l);
+ s.setL(1 / l);
+ } else {
+ s.setXc(xc);
+ s.setYc(yc);
+ s.setL(l);
}
- s.setAlpha(atan2(y1-y2,x1-x2));
+ s.setAlpha(atan2(y1 - y2, x1 - x2));
}
#endif //#ifdef VISP_HAVE_MODULE_BLOB
@@ -126,37 +120,35 @@ void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam
Initialize a segment feature out of image points and camera parameters.
\param s : Visual feature to initialize.
- \param cam : The parameters of the camera used to acquire the image containing the point.
- \param ip1 : The image point corresponding to the first point of the segment.
- \param ip2 : The image point corresponding to the second point of the segment.
+ \param cam : The parameters of the camera used to acquire the image
+ containing the point. \param ip1 : The image point corresponding to the
+ first point of the segment. \param ip2 : The image point corresponding to
+ the second point of the segment.
*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam,
- const vpImagePoint &ip1, const vpImagePoint &ip2)
+void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam, const vpImagePoint &ip1,
+ const vpImagePoint &ip2)
{
- double x1=0, y1=0, x2=0, y2=0;
-
- vpPixelMeterConversion::convertPoint(cam, ip1, x1, y1) ;
- vpPixelMeterConversion::convertPoint(cam, ip2, x2, y2) ;
-
- double xc = (x1+x2)/2.;
- double yc = (y1+y2)/2.;
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
-
- if (s.isNormalized())
- {
- s.setXc( xc/l );
- s.setYc( yc/l );
- s.setL ( 1/l );
- }
- else
- {
- s.setXc( xc );
- s.setYc( yc );
- s.setL ( l );
+ double x1 = 0, y1 = 0, x2 = 0, y2 = 0;
+
+ vpPixelMeterConversion::convertPoint(cam, ip1, x1, y1);
+ vpPixelMeterConversion::convertPoint(cam, ip2, x2, y2);
+
+ double xc = (x1 + x2) / 2.;
+ double yc = (y1 + y2) / 2.;
+ double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+
+ if (s.isNormalized()) {
+ s.setXc(xc / l);
+ s.setYc(yc / l);
+ s.setL(1 / l);
+ } else {
+ s.setXc(xc);
+ s.setYc(yc);
+ s.setL(l);
}
- s.setAlpha(atan2(y1-y2,x1-x2));
+ s.setAlpha(atan2(y1 - y2, x1 - x2));
}
/*!
@@ -164,19 +156,20 @@ void vpFeatureBuilder::create(vpFeatureSegment &s, const vpCameraParameters &cam
Build a segment visual feature from two points.
\param s : Visual feature to initialize.
- \param P1, P2 : Two points defining the segment. These points must contain the 3D coordinates
- in the camera frame (cP) and the projected coordinates in the image plane (p).
+ \param P1, P2 : Two points defining the segment. These points must contain
+ the 3D coordinates in the camera frame (cP) and the projected coordinates in
+ the image plane (p).
*/
-void vpFeatureBuilder::create(vpFeatureSegment &s, vpPoint& P1, vpPoint& P2)
+void vpFeatureBuilder::create(vpFeatureSegment &s, vpPoint &P1, vpPoint &P2)
{
double x1 = P1.get_x();
double y1 = P1.get_y();
double x2 = P2.get_x();
double y2 = P2.get_y();
- double Z1 = P1.cP[2]/P1.cP[3];
- double Z2 = P2.cP[2]/P2.cP[3];
+ double Z1 = P1.cP[2] / P1.cP[3];
+ double Z2 = P2.cP[2] / P2.cP[3];
s.buildFrom(x1, y1, Z1, x2, y2, Z2);
}
diff --git a/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp
index 2e1167f..5a9bff3 100644
--- a/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp
+++ b/modules/visual_features/src/feature-builder/vpFeatureBuilderVanishingPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,48 +36,41 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureBuilderVanishingPoint.cpp
\brief conversion between vpPoint
and visual feature vanishing point.
*/
+#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpException.h>
-
/*!
Initialize a vpFeatureVanishingPoint thanks to a vpPoint.
- The vpFeatureVanishingPoint is initialized thanks to the parameters of the point in the image plan.
- All the parameters are given in meter.
+ The vpFeatureVanishingPoint is initialized thanks to the parameters of the
+ point in the image plan. All the parameters are given in meter.
\param s : Visual feature to initialize.
\param t : The vpPoint used to create the vpFeatureVanishingPoint.
*/
-void
-vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &t)
+void vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpPoint &t)
{
- try
- {
- s.set_x( t.get_x()) ;
- s.set_y( t.get_y()) ;
- }
- catch(...)
- {
- vpERROR_TRACE("Cannot create vanishing point feature") ;
- throw ;
+ try {
+ s.set_x(t.get_x());
+ s.set_y(t.get_y());
+ } catch (...) {
+ vpERROR_TRACE("Cannot create vanishing point feature");
+ throw;
}
}
-
/*!
Initialize a vpFeatureVanishingPoint thanks to two vpFeatureLine.
- The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
- All the parameters are given in meter.
+ The vpFeatureVanishingPoint is initialized thanks to the coordinate of the
+ intersection point in the image plan. All the parameters are given in meter.
- \warning An exception is thrown if the two lines are parallels
+ \warning An exception is thrown if the two lines are parallels
\param s : Visual feature to initialize.
@@ -84,8 +78,7 @@ 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;
@@ -96,8 +89,8 @@ vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, co
double c_r;
double s_r;
- rho_l = L1.getRho();
- rho_r = L2.getRho();
+ rho_l = L1.getRho();
+ rho_r = L2.getRho();
theta_l = L1.getTheta();
theta_r = L2.getTheta();
c_l = cos(theta_l);
@@ -105,33 +98,31 @@ vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, co
s_l = sin(theta_l);
s_r = sin(theta_r);
-
- double x,y;
+ double x, y;
double min = 0.0001;
- if(fabs(theta_r-theta_l)<min || fabs(fabs(theta_r-theta_l)-M_PI)<min \
- || fabs(fabs(theta_r-theta_l)-2*M_PI)<min)
- {
- vpCERROR<<"There is no vanishing point : the lines are parallel in the image plane"<<std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "There is no vanishing point : the lines are parallel in the image plane")) ;
+ if (fabs(theta_r - theta_l) < min || fabs(fabs(theta_r - theta_l) - M_PI) < min ||
+ fabs(fabs(theta_r - theta_l) - 2 * M_PI) < min) {
+ vpCERROR << "There is no vanishing point : the lines are parallel in the "
+ "image plane"
+ << std::endl;
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "There is no vanishing point : the lines are "
+ "parallel in the image plane"));
}
- y = (rho_r *c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l );
- x = (rho_r *s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l );
+ y = (rho_r * c_l - rho_l * c_r) / (-s_l * c_r + s_r * c_l);
+ x = (rho_r * s_l - rho_l * s_r) / (-c_l * s_r + c_r * s_l);
- s.set_x ( x );
- s.set_y ( y );
+ s.set_x(x);
+ s.set_y(y);
}
-
-
/*!
Initialize a vpFeatureVanishingPoint thanks to two vpLine.
- The vpFeatureVanishingPoint is initialized thanks to the coordinate of the intersection point in the image plan.
- All the parameters are given in meter.
+ The vpFeatureVanishingPoint is initialized thanks to the coordinate of the
+ intersection point in the image plan. All the parameters are given in meter.
- \warning An exception is thrown if the two lines are parallels
+ \warning An exception is thrown if the two lines are parallels
\param s : Visual feature to initialize.
@@ -139,13 +130,11 @@ vpFeatureBuilder::create(vpFeatureVanishingPoint &s, const vpFeatureLine &L1, co
\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) ;
- vpFeatureBuilder::create (l2,L2) ;
-
- vpFeatureBuilder::create (s, l1, l2) ;
+ vpFeatureLine l1, l2;
+ vpFeatureBuilder::create(l1, L1);
+ vpFeatureBuilder::create(l2, L2);
+ vpFeatureBuilder::create(s, l1, l2);
}
diff --git a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp
index fa9e8b0..bd1a73c 100644
--- a/modules/visual_features/src/visual-feature/vpBasicFeature.cpp
+++ b/modules/visual_features/src/visual-feature/vpBasicFeature.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,29 +37,17 @@
*
*****************************************************************************/
-
-
#include <visp3/visual_features/vpBasicFeature.h>
-const unsigned int vpBasicFeature::FEATURE_LINE [32] =
- {
- (unsigned int)(1 << 0), (unsigned int)(1 << 1),
- (unsigned int)(1 << 2), (unsigned int)(1 << 3),
- (unsigned int)(1 << 4), (unsigned int)(1 << 5),
- (unsigned int)(1 << 6), (unsigned int)(1 << 7),
- (unsigned int)(1 << 8), (unsigned int)(1 << 9),
- (unsigned int)(1 << 10), (unsigned int)(1 << 11),
- (unsigned int)(1 << 12), (unsigned int)(1 << 13),
- (unsigned int)(1 << 14), (unsigned int)(1 << 15),
- (unsigned int)(1 << 16), (unsigned int)(1 << 17),
- (unsigned int)(1 << 18), (unsigned int)(1 << 19),
- (unsigned int)(1 << 20), (unsigned int)(1 << 21),
- (unsigned int)(1 << 22), (unsigned int)(1 << 23),
- (unsigned int)(1 << 24), (unsigned int)(1 << 25),
- (unsigned int)(1 << 26), (unsigned int)(1 << 27),
- (unsigned int)(1 << 28), (unsigned int)(1 << 29),
- (unsigned int)(1 << 30), (unsigned int)(1 << 31)
- };
+const unsigned int vpBasicFeature::FEATURE_LINE[32] = {
+ (unsigned int)(1 << 0), (unsigned int)(1 << 1), (unsigned int)(1 << 2), (unsigned int)(1 << 3),
+ (unsigned int)(1 << 4), (unsigned int)(1 << 5), (unsigned int)(1 << 6), (unsigned int)(1 << 7),
+ (unsigned int)(1 << 8), (unsigned int)(1 << 9), (unsigned int)(1 << 10), (unsigned int)(1 << 11),
+ (unsigned int)(1 << 12), (unsigned int)(1 << 13), (unsigned int)(1 << 14), (unsigned int)(1 << 15),
+ (unsigned int)(1 << 16), (unsigned int)(1 << 17), (unsigned int)(1 << 18), (unsigned int)(1 << 19),
+ (unsigned int)(1 << 20), (unsigned int)(1 << 21), (unsigned int)(1 << 22), (unsigned int)(1 << 23),
+ (unsigned int)(1 << 24), (unsigned int)(1 << 25), (unsigned int)(1 << 26), (unsigned int)(1 << 27),
+ (unsigned int)(1 << 28), (unsigned int)(1 << 29), (unsigned int)(1 << 30), (unsigned int)(1 << 31)};
/*!
\file vpBasicFeature.cpp
@@ -67,10 +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.
@@ -78,7 +64,7 @@ vpBasicFeature::vpBasicFeature()
vpBasicFeature::~vpBasicFeature()
{
if (flags != NULL) {
- delete [] flags;
+ delete[] flags;
flags = NULL;
}
}
@@ -102,8 +88,8 @@ vpBasicFeature &vpBasicFeature::operator=(const vpBasicFeature &f)
nbParameters = f.nbParameters;
deallocate = f.deallocate;
if (flags)
- delete [] flags;
- flags = new bool [nbParameters];
+ delete[] flags;
+ flags = new bool[nbParameters];
for (unsigned int i = 0; i < nbParameters; i++)
flags[i] = f.flags[i];
@@ -111,83 +97,75 @@ vpBasicFeature &vpBasicFeature::operator=(const vpBasicFeature &f)
}
//! Get the feature vector dimension.
-unsigned int
-vpBasicFeature::getDimension(unsigned int select) const
+unsigned int vpBasicFeature::getDimension(unsigned int select) const
{
- unsigned int dim = 0 ;
- if(dim_s>31)
- return dim_s;
- for (unsigned int i=0 ; i < s.getRows() ; i++)
- {
- // printf("%x %x %d \n",select, featureLine[i], featureLine[i] & select);
- if (FEATURE_LINE[i] & select) dim +=1 ;
- }
- return dim ;
+ unsigned int dim = 0;
+ if (dim_s > 31)
+ return dim_s;
+ for (unsigned int i = 0; i < s.getRows(); i++) {
+ // printf("%x %x %d \n",select, featureLine[i], featureLine[i] & select);
+ if (FEATURE_LINE[i] & select)
+ dim += 1;
+ }
+ return dim;
}
//! Get the feature vector \f$\bf s\f$.
-vpColVector
-vpBasicFeature::get_s(const unsigned int select) const
+vpColVector vpBasicFeature::get_s(const unsigned int select) const
{
vpColVector state(0), stateLine(1);
- // if s is higher than the possible selections (photometry), send back the whole vector
- if(dim_s > 31)
+ // if s is higher than the possible selections (photometry), send back the
+ // whole vector
+ if (dim_s > 31)
return s;
- for(unsigned int i=0;i<dim_s;++i)
- {
- if(FEATURE_LINE[i] & select)
- {
+ for (unsigned int i = 0; i < dim_s; ++i) {
+ if (FEATURE_LINE[i] & select) {
stateLine[0] = s[i];
state.stack(stateLine);
}
}
- return state ;
+ return state;
}
void vpBasicFeature::resetFlags()
{
- if (flags != NULL)
- {
+ if (flags != NULL) {
for (unsigned int i = 0; i < nbParameters; i++)
flags[i] = false;
}
}
-//! Set feature flags to true to prevent warning when re-computing the interaction matrix without having updated the feature.
+//! Set feature flags to true to prevent warning when re-computing the
+//! interaction matrix without having updated the feature.
void vpBasicFeature::setFlags()
{
- if (flags != NULL)
- {
+ if (flags != NULL) {
for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = true;
- }
+ flags[i] = true;
+ }
}
-//! Compute the error between two visual features from a subset of the possible features.
-vpColVector vpBasicFeature::error(const vpBasicFeature &s_star,
- const unsigned int select)
+//! Compute the error between two visual features from a subset of the
+//! possible features.
+vpColVector vpBasicFeature::error(const vpBasicFeature &s_star, const unsigned int select)
{
- vpColVector e(0),eLine(1);
- if (dim_s <= 31)
- {
- for(unsigned int i=0;i<dim_s;++i){
- if(FEATURE_LINE[i] & select)
- {
- eLine[0] = s[i] - s_star[i];
+ vpColVector e(0), eLine(1);
+ if (dim_s <= 31) {
+ for (unsigned int i = 0; i < dim_s; ++i) {
+ if (FEATURE_LINE[i] & select) {
+ eLine[0] = s[i] - s_star[i];
e.stack(eLine);
- //std::cout << "dim_s <= 31"<<std::endl;
- }
- }
- }
- else
- {
- e.resize(dim_s);
- vpColVector sd = s_star.get_s();
- e = s - sd;
- }
+ // std::cout << "dim_s <= 31"<<std::endl;
+ }
+ }
+ } else {
+ e.resize(dim_s);
+ vpColVector sd = s_star.get_s();
+ e = s - sd;
+ }
- return e ;
+ return e;
}
/*
diff --git a/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp
index c894661..75de349 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureDepth.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureDepth.cpp
\brief Class that defines 2D point visual feature
*/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureDepth.h>
@@ -57,8 +56,6 @@
#include <visp3/core/vpFeatureDisplay.h>
-
-
/*
@@ -75,194 +72,171 @@ other functionalities ar useful but not mandatory
/*!
Initialize the memory space requested for 3D depth visual feature.
*/
-void
-vpFeatureDepth::init()
+void vpFeatureDepth::init()
{
- //feature dimension
- dim_s = 1 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
- x = y = 0.;
- Z = 1.;
+ // feature dimension
+ dim_s = 1;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+ x = y = 0.;
+ Z = 1.;
}
-
-/*!
+/*!
Default constructor that build a visual feature.
*/
-vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.)
-{
- init() ;
-}
-
+vpFeatureDepth::vpFeatureDepth() : x(0), y(0), Z(1.) { init(); }
/*!
- Set the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+ Set the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm
+ of the current depth relative to the desired depth.
\param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
*/
-void
-vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar)
-{
- s[0] = LogZoverZstar ;
-}
-
+void vpFeatureDepth::set_LogZoverZstar(const double LogZoverZstar) { s[0] = LogZoverZstar; }
/*!
- Get the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+ Get the value of \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm
+ of the current depth relative to the desired depth.
\return The value of \f$ log(\frac{Z}{Z^*}) \f$.
*/
-double
-vpFeatureDepth::get_LogZoverZstar() const
-{
- return s[0] ;
-}
-
+double vpFeatureDepth::get_LogZoverZstar() const { return s[0]; }
/*!
- Set the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
+ Set the value of \f$ x \f$ which represents the x coordinate of the point in
+ the camera frame.
\param x_ : \f$ x \f$ value to set.
*/
-void
-vpFeatureDepth::set_x(const double x_)
+void vpFeatureDepth::set_x(const double x_)
{
- this->x = x_ ;
- flags[0] = true;
+ this->x = x_;
+ flags[0] = true;
}
-
/*!
- Get the value of \f$ x \f$ which represents the x coordinate of the point in the camera frame.
+ Get the value of \f$ x \f$ which represents the x coordinate of the point in
+ the camera frame.
\return The value of \f$ x \f$.
*/
-double
-vpFeatureDepth::get_x() const
-{
- return x ;
-}
-
+double vpFeatureDepth::get_x() const { return x; }
/*!
- Set the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
+ Set the value of \f$ y \f$ which represents the y coordinate of the point in
+ the camera frame.
\param y_ : \f$ y \f$ value to set.
*/
-void
-vpFeatureDepth::set_y(const double y_)
+void vpFeatureDepth::set_y(const double y_)
{
- this->y = y_ ;
- flags[1] = true;
+ this->y = y_;
+ flags[1] = true;
}
-
/*!
- Get the value of \f$ y \f$ which represents the y coordinate of the point in the camera frame.
+ Get the value of \f$ y \f$ which represents the y coordinate of the point in
+ the camera frame.
\return The value of \f$ y \f$.
*/
-double
-vpFeatureDepth::get_y() const
-{
- return y ;
-}
+double vpFeatureDepth::get_y() const { return y; }
/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\param Z_ : \f$ Z \f$ value to set.
*/
-void
-vpFeatureDepth::set_Z(const double Z_)
+void vpFeatureDepth::set_Z(const double Z_)
{
- this->Z = Z_ ;
- flags[2] = true;
+ this->Z = Z_;
+ flags[2] = true;
}
-
/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\return The value of \f$ Z \f$.
*/
-double
-vpFeatureDepth::get_Z() const
-{
- return Z ;
-}
-
+double vpFeatureDepth::get_Z() const { return Z; }
/*!
- Set the value of \f$ x \f$, \f$ y \f$, \f$ Z \f$ and \f$ log(\frac{Z}{Z^*}) \f$. \f$ x \f$ and \f$ y \f$ represent the coordinates of the point in the camera frame. \f$ Z \f$ is the 3D coordinate representing the depth. \f$ log(\frac{Z}{Z^*}) \f$ represents the logarithm of the current depth relative to the desired depth.
+ Set the value of \f$ x \f$, \f$ y \f$, \f$ Z \f$ and \f$ log(\frac{Z}{Z^*})
+ \f$. \f$ x \f$ and \f$ y \f$ represent the coordinates of the point in the
+ camera frame. \f$ Z \f$ is the 3D coordinate representing the depth. \f$
+ log(\frac{Z}{Z^*}) \f$ represents the logarithm of the current depth
+ relative to the desired depth.
\param x_ : \f$ x \f$ value to set.
\param y_ : \f$ y \f$ value to set.
\param Z_ : \f$ Z \f$ value to set.
\param LogZoverZstar : \f$ log(\frac{Z}{Z^*}) \f$ value to set.
*/
-void
-vpFeatureDepth::set_xyZLogZoverZstar(const double x_,
- const double y_,
- const double Z_,
- const double LogZoverZstar)
+void vpFeatureDepth::set_xyZLogZoverZstar(const double x_, const double y_, const double Z_, const double LogZoverZstar)
{
- set_x(x_) ;
- set_y(y_) ;
- set_Z(Z_) ;
- set_LogZoverZstar(LogZoverZstar) ;
- for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ set_x(x_);
+ set_y(y_);
+ set_Z(Z_);
+ set_LogZoverZstar(LogZoverZstar);
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
- Compute and return the interaction matrix \f$ L \f$. The computation is made thanks to the values of the point coordinates \f$ x \f$ and \f$ y \f$ and the depth \f$ Z \f$.
+ Compute and return the interaction matrix \f$ L \f$. The computation is made
+ thanks to the values of the point coordinates \f$ x \f$ and \f$ y \f$ and
+ the depth \f$ Z \f$.
\f[ L =
\left[\begin{array}{cccccc}
0 & 0 & -1/Z & -y & x & 0
\end{array}\right]\f]
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+ \param select : unuseful in the case of vpFeatureDepth. Always set to
+ FEATURE_ALL.
\return The interaction matrix computed from the point feature.
- The code below shows how to compute the interaction matrix associated to the visual feature \f$ s = log(\frac{Z}{Z^*}) \f$.
+ The code below shows how to compute the interaction matrix associated to the
+ visual feature \f$ s = log(\frac{Z}{Z^*}) \f$.
\code
// Creation of the current feature s
vpFeatureDepth s;
- s.buildFrom(0, 0, 5, log(5/1)); //The current depth is 5 metters and the desired is 1 metter.
+ s.buildFrom(0, 0, 5, log(5/1)); //The current depth is 5 metters and the
+ desired is 1 metter.
vpMatrix L_x = s.interaction();
\endcode
*/
-vpMatrix
-vpFeatureDepth::interaction(const unsigned int select)
+vpMatrix vpFeatureDepth::interaction(const unsigned int select)
{
- vpMatrix L ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ vpMatrix L;
+
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but x was "
+ "not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but y was "
+ "not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but z was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but z was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -271,55 +245,50 @@ vpFeatureDepth::interaction(const unsigned int select)
resetFlags();
}
- L.resize(1,6) ;
+ L.resize(1, 6);
double x_ = get_x();
double y_ = get_y();
double Z_ = get_Z();
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
+ 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 ;
+ 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)
- {
+ if (FEATURE_ALL & select) {
L = 0;
L[0][0] = 0;
L[0][1] = 0;
- L[0][2] = -1/Z_;
+ L[0][2] = -1 / Z_;
L[0][3] = -y_;
L[0][4] = x_;
L[0][5] = 0;
}
- return L ;
+ return L;
}
-
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
- Since this visual feature \f$ s \f$ represent the current depth relative to the desired depth, the desired visual
- feature \f$ s^* \f$ should be zero. Thus, the error is here equal to
- the current visual feature \f$ s \f$.
+ Since this visual feature \f$ s \f$ represent the current depth relative to
+ the desired depth, the desired visual feature \f$ s^* \f$ should be zero.
+ Thus, the error is here equal to the current visual feature \f$ s \f$.
\param s_star : Desired visual visual feature that should be equal to zero.
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+ \param select : unuseful in the case of vpFeatureDepth. Always set to
+ FEATURE_ALL.
\return The error \f$ (s-s^*)\f$ between the current and the desired
visual feature.
@@ -333,7 +302,7 @@ vpFeatureDepth::interaction(const unsigned int select)
// Creation of the current feature s
vpFeatureDepth s;
- // Creation of the desired feature s^*. By default this feature is
+ // Creation of the desired feature s^*. By default this feature is
// initialized to zero
vpFeatureDepth s_star;
s_star.setLogZoverZstar(0)
@@ -345,32 +314,27 @@ 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 !")) ;
- }
+ if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
+ vpERROR_TRACE("s* should be zero ! ");
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
+ }
- vpColVector e(1) ;
- if(FEATURE_ALL & select)
- {
+ vpColVector e(1);
+ if (FEATURE_ALL & select) {
e[0] = s[0];
}
- return e ;
+ return e;
}
-
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
- \param select : unuseful in the case of vpFeatureDepth. Always set to FEATURE_ALL.
+ \param select : unuseful in the case of vpFeatureDepth. Always set to
+ FEATURE_ALL.
\code
vpFeatureDepth s; // Current visual feature s
@@ -381,64 +345,57 @@ vpFeatureDepth::error(const vpBasicFeature &s_star,
s.print(); // print all the 2 components of the feature
\endcode
*/
-void
-vpFeatureDepth::print(const unsigned int select ) const
+void vpFeatureDepth::print(const unsigned int select) const
{
- if (FEATURE_ALL & select)
- {
- std::cout <<"Point: x=" << get_x() ;
- std::cout <<" Point: y=" << get_y() ;
- std::cout <<" Point: Z=" << get_Z() ;
+ if (FEATURE_ALL & select) {
+ std::cout << "Point: x=" << get_x();
+ std::cout << " Point: y=" << get_y();
+ std::cout << " Point: Z=" << get_Z();
- std::cout << " log(Z/Z*)=" << get_LogZoverZstar() ;
+ std::cout << " log(Z/Z*)=" << get_LogZoverZstar();
- std::cout <<std::endl ;
+ std::cout << std::endl;
}
}
-
/*!
- Build a 3D depth visual feature from the point coordinates \f$ x \f$ and \f$ y \f$ given in the camera frame, \f$ Z \f$ which describes the depth and \f$ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth relative to the desired depth.
+ Build a 3D depth visual feature from the point coordinates \f$ x \f$ and \f$
+ y \f$ given in the camera frame, \f$ Z \f$ which describes the depth and \f$
+ log(\frac{Z}{Z^*}) \f$ which represents the logarithm of the current depth
+ relative to the desired depth.
\param x_ : The \f$ x \f$ parameter.
\param y_ : The \f$ y \f$ parameter.
\param Z_ : The \f$ Z \f$ parameter.
\param LogZoverZstar : The \f$ log(\frac{Z}{Z^*}) \f$ parameter.
*/
-void
-vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
+void vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, const double LogZoverZstar)
{
s[0] = LogZoverZstar;
- this->x = x_ ;
- this->y = y_ ;
- this->Z = Z_ ;
+ this->x = x_;
+ this->y = y_;
+ this->Z = Z_;
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
+ 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 ;
+ 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++) flags[i] = true;
-
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
-
/*!
Create an object with the same type.
@@ -452,7 +409,7 @@ vpFeatureDepth::buildFrom(const double x_, const double y_, const double Z_, con
vpFeatureDepth *vpFeatureDepth::duplicate() const
{
vpFeatureDepth *feature = new vpFeatureDepth;
- return feature ;
+ return feature;
}
/*!
@@ -460,39 +417,31 @@ vpFeatureDepth *vpFeatureDepth::duplicate() const
Not implemented.
*/
-void
-vpFeatureDepth::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
- }
+ }
}
/*!
Not implemented.
*/
-void
-vpFeatureDepth::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureDepth::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp
index 75f440d..3fc2e99 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureEllipse.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureEllipse.cpp
\brief Class that defines 2D ellipse visual feature
*/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureEllipse.h>
@@ -55,11 +54,8 @@
// math
#include <visp3/core/vpMath.h>
-
-
#include <visp3/core/vpFeatureDisplay.h>
-
/*
@@ -73,71 +69,69 @@ other functionalities ar useful but not mandatory
*/
-void
-vpFeatureEllipse::init()
-{
- //feature dimension
- dim_s = 5 ;
- nbParameters = 8;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default depth values
- A = B = 0;
- C =1 ;
-
-}
-
-vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0)
+void vpFeatureEllipse::init()
{
- init() ;
+ // feature dimension
+ dim_s = 5;
+ nbParameters = 8;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ // default depth values
+ A = B = 0;
+ C = 1;
}
-
+vpFeatureEllipse::vpFeatureEllipse() : A(0), B(0), C(0) { init(); }
//! compute the interaction matrix from a subset a the possible features
-vpMatrix
-vpFeatureEllipse::interaction(const unsigned int select)
+vpMatrix vpFeatureEllipse::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but x was "
+ "not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but y was "
+ "not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but mu20 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but mu20 "
+ "was not set yet");
+ break;
case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but mu11 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but mu11 "
+ "was not set yet");
+ break;
case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but mu02 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but mu02 "
+ "was not set yet");
+ break;
case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but A was "
+ "not set yet");
+ break;
case 6:
- vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but B was "
+ "not set yet");
+ break;
case 7:
- vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but C was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -146,250 +140,222 @@ vpFeatureEllipse::interaction(const unsigned int select)
resetFlags();
}
- double xc = s[0] ;
- double yc = s[1] ;
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
+ double xc = s[0];
+ double yc = s[1];
+ double mu20 = s[2];
+ double mu11 = s[3];
+ double mu02 = s[4];
- //eq 39
- double Z = 1/(A*xc + B*yc + C) ;
+ // eq 39
+ double Z = 1 / (A * xc + B * yc + C);
+ if (vpFeatureEllipse::selectX() & select) {
+ vpMatrix H(1, 6);
+ H = 0;
-
- if (vpFeatureEllipse::selectX() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
-
- H[0][0] = -1/Z;
- H[0][1] = 0 ;
- H[0][2] = xc/Z + A*mu20 + B*mu11;
- H[0][3] = xc*yc + mu11;
- H[0][4] = -1-vpMath::sqr(xc)-mu20;
+ H[0][0] = -1 / Z;
+ H[0][1] = 0;
+ H[0][2] = xc / Z + A * mu20 + B * mu11;
+ H[0][3] = xc * yc + mu11;
+ H[0][4] = -1 - vpMath::sqr(xc) - mu20;
H[0][5] = yc;
-
- L = vpMatrix::stack(L,H) ;
+ L = vpMatrix::stack(L, H);
}
- if (vpFeatureEllipse::selectY() & select )
- {
- vpMatrix H(1,6) ; H = 0;
+ if (vpFeatureEllipse::selectY() & select) {
+ vpMatrix H(1, 6);
+ H = 0;
-
- H[0][0] = 0 ;
- H[0][1] = -1/Z;
- H[0][2] = yc/Z + A*mu11 + B*mu02;
- H[0][3] = 1+vpMath::sqr(yc)+mu02;
- H[0][4] = -xc*yc - mu11;
+ H[0][0] = 0;
+ H[0][1] = -1 / Z;
+ H[0][2] = yc / Z + A * mu11 + B * mu02;
+ H[0][3] = 1 + vpMath::sqr(yc) + mu02;
+ H[0][4] = -xc * yc - mu11;
H[0][5] = -xc;
- L = vpMatrix::stack(L,H) ;
+ L = vpMatrix::stack(L, H);
}
- if (vpFeatureEllipse::selectMu20() & select )
- {
- vpMatrix H(1,6) ; H = 0;
+ if (vpFeatureEllipse::selectMu20() & select) {
+ vpMatrix H(1, 6);
+ H = 0;
- H[0][0] = -2*(A*mu20+B*mu11);
- H[0][1] = 0 ;
- H[0][2] = 2*((1/Z+A*xc)*mu20+B*xc*mu11) ;
- H[0][3] = 2*(yc*mu20+xc*mu11);
- H[0][4] = -4*mu20*xc;
- H[0][5] = 2*mu11;
+ H[0][0] = -2 * (A * mu20 + B * mu11);
+ H[0][1] = 0;
+ H[0][2] = 2 * ((1 / Z + A * xc) * mu20 + B * xc * mu11);
+ H[0][3] = 2 * (yc * mu20 + xc * mu11);
+ H[0][4] = -4 * mu20 * xc;
+ H[0][5] = 2 * mu11;
- L = vpMatrix::stack(L,H) ;
+ L = vpMatrix::stack(L, H);
}
- if (vpFeatureEllipse::selectMu11() & select )
- {
- vpMatrix H(1,6) ; H = 0;
+ if (vpFeatureEllipse::selectMu11() & select) {
+ vpMatrix H(1, 6);
+ H = 0;
- H[0][0] = -A*mu11-B*mu02;
- H[0][1] = -A*mu20-B*mu11;
- H[0][2] = A*yc*mu20+(3/Z-C)*mu11+B*xc*mu02;
- H[0][3] = 3*yc*mu11+xc*mu02;
- H[0][4] = -yc*mu20-3*xc*mu11;
- H[0][5] = mu02-mu20;
+ H[0][0] = -A * mu11 - B * mu02;
+ H[0][1] = -A * mu20 - B * mu11;
+ H[0][2] = A * yc * mu20 + (3 / Z - C) * mu11 + B * xc * mu02;
+ H[0][3] = 3 * yc * mu11 + xc * mu02;
+ H[0][4] = -yc * mu20 - 3 * xc * mu11;
+ H[0][5] = mu02 - mu20;
- L = vpMatrix::stack(L,H) ;
+ L = vpMatrix::stack(L, H);
}
- if (vpFeatureEllipse::selectMu02() & select )
- {
- vpMatrix H(1,6) ; H = 0;
-
- H[0][0] = 0 ;
- H[0][1] = -2*(A*mu11+B*mu02);
- H[0][2] = 2*((1/Z+B*yc)*mu02+A*yc*mu11);
- H[0][3] = 4*yc*mu02;
- H[0][4] = -2*(yc*mu11 +xc*mu02) ;
- H[0][5] = -2*mu11 ;
- L = vpMatrix::stack(L,H) ;
+ if (vpFeatureEllipse::selectMu02() & select) {
+ vpMatrix H(1, 6);
+ H = 0;
+
+ H[0][0] = 0;
+ H[0][1] = -2 * (A * mu11 + B * mu02);
+ H[0][2] = 2 * ((1 / Z + B * yc) * mu02 + A * yc * mu11);
+ H[0][3] = 4 * yc * mu02;
+ H[0][4] = -2 * (yc * mu11 + xc * mu02);
+ H[0][5] = -2 * mu11;
+ L = vpMatrix::stack(L, H);
}
-
- return L ;
+ return L;
}
//! compute the error between two visual features from a subset
//! a the possible features
-vpColVector
-vpFeatureEllipse::error(const vpBasicFeature &s_star,
- const unsigned int select)
+vpColVector vpFeatureEllipse::error(const vpBasicFeature &s_star, const unsigned int select)
{
- vpColVector e(0) ;
+ vpColVector e(0);
- try{
- if (vpFeatureEllipse::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
+ try {
+ if (vpFeatureEllipse::selectX() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0] - s_star[0];
- e = vpColVector::stack(e,ex) ;
+ e = vpColVector::stack(e, ex);
}
- if (vpFeatureEllipse::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeatureEllipse::selectY() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1] - s_star[1];
+ e = vpColVector::stack(e, ey);
}
- if (vpFeatureEllipse::selectMu20() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[2] - s_star[2] ;
+ if (vpFeatureEllipse::selectMu20() & select) {
+ vpColVector ex(1);
+ ex[0] = s[2] - s_star[2];
- e = vpColVector::stack(e,ex) ;
+ e = vpColVector::stack(e, ex);
}
- if (vpFeatureEllipse::selectMu11() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[3] - s_star[3] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeatureEllipse::selectMu11() & select) {
+ vpColVector ey(1);
+ ey[0] = s[3] - s_star[3];
+ e = vpColVector::stack(e, ey);
}
- if (vpFeatureEllipse::selectMu02() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[4] - s_star[4] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeatureEllipse::selectMu02() & select) {
+ vpColVector ey(1);
+ ey[0] = s[4] - s_star[4];
+ e = vpColVector::stack(e, ey);
}
- }
- catch(...) {
+ } catch (...) {
throw;
}
- return e ;
-
+ return e;
}
-
-void
-vpFeatureEllipse::print(const unsigned int select ) const
+void vpFeatureEllipse::print(const unsigned int select) const
{
- std::cout <<"Ellipse: " << std::endl ;
- if (vpFeatureEllipse::selectX() & select )
- std::cout << " x=" << s[0] <<std::endl ;;
- if (vpFeatureEllipse::selectY() & select )
- std::cout << " y=" << s[1] <<std::endl ;
- if (vpFeatureEllipse::selectMu20() & select )
- std::cout << " mu20=" << s[2] <<std::endl ;
- if (vpFeatureEllipse::selectMu11() & select )
- std::cout << " mu11=" << s[3] <<std::endl ;
- if (vpFeatureEllipse::selectMu02() & select )
- std::cout << " mu02=" << s[4] <<std::endl ;
- std::cout << "A = "<<A <<" B = "<<B <<" C = "<<C << std::endl ;
+ std::cout << "Ellipse: " << std::endl;
+ if (vpFeatureEllipse::selectX() & select)
+ std::cout << " x=" << s[0] << std::endl;
+ ;
+ if (vpFeatureEllipse::selectY() & select)
+ std::cout << " y=" << s[1] << std::endl;
+ if (vpFeatureEllipse::selectMu20() & select)
+ std::cout << " mu20=" << s[2] << std::endl;
+ if (vpFeatureEllipse::selectMu11() & select)
+ std::cout << " mu11=" << s[3] << std::endl;
+ if (vpFeatureEllipse::selectMu02() & select)
+ std::cout << " mu02=" << s[4] << std::endl;
+ std::cout << "A = " << A << " B = " << B << " C = " << C << std::endl;
}
-
-void
-vpFeatureEllipse::buildFrom(const double x, const double y,
- const double mu20, const double mu11,
- const double mu02)
+void vpFeatureEllipse::buildFrom(const double x, const double y, const double mu20, const double mu11,
+ const double mu02)
{
- s[0] = x ;
- s[1] = y ;
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
-
- for( int i = 0; i < 5; i++) flags[i] = true;
+ s[0] = x;
+ s[1] = y;
+ s[2] = mu20;
+ s[3] = mu11;
+ s[4] = mu02;
+ for (int i = 0; i < 5; i++)
+ flags[i] = true;
}
-void
-vpFeatureEllipse::buildFrom(const double x, const double y,
- const double mu20, const double mu11,
- const double mu02,
- const double a, const double b, const double c)
+void vpFeatureEllipse::buildFrom(const double x, const double y, const double mu20, const double mu11,
+ const double mu02, const double a, const double b, const double c)
{
- s[0] = x ;
- s[1] = y ;
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
+ s[0] = x;
+ s[1] = y;
+ s[2] = mu20;
+ s[3] = mu11;
+ s[4] = mu02;
- this->A = a ;
- this->B = b ;
- this->C = c ;
+ this->A = a;
+ this->B = b;
+ this->C = c;
- for( unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-void
-vpFeatureEllipse::set_x(const double x)
+void vpFeatureEllipse::set_x(const double x)
{
- s[0] = x ;
+ s[0] = x;
flags[0] = true;
}
-void
-vpFeatureEllipse::set_y(const double y)
+void vpFeatureEllipse::set_y(const double y)
{
- s[1] = y ;
+ s[1] = y;
flags[1] = true;
}
-void
-vpFeatureEllipse::set_xy(const double x,const double y)
+void vpFeatureEllipse::set_xy(const double x, const double y)
{
- s[0] = x ;
- s[1] = y ;
- for( int i = 0; i < 2; i++) flags[i] = true;
+ s[0] = x;
+ s[1] = y;
+ for (int i = 0; i < 2; i++)
+ flags[i] = true;
}
-void
-vpFeatureEllipse::setABC(const double a, const double b, const double c)
+void vpFeatureEllipse::setABC(const double a, const double b, const double c)
{
- this->A = a ;
- this->B = b ;
- this->C = c ;
- for( unsigned int i = 5; i < nbParameters; i++) flags[i] = true;
+ this->A = a;
+ this->B = b;
+ this->C = c;
+ for (unsigned int i = 5; i < nbParameters; i++)
+ flags[i] = true;
}
-
-void
-vpFeatureEllipse::setMu(const double mu20, const double mu11,
- const double mu02)
+void vpFeatureEllipse::setMu(const double mu20, const double mu11, const double mu02)
{
- s[2] = mu20 ;
- s[3] = mu11 ;
- s[4] = mu02 ;
- for( int i = 2; i < 5; i++) flags[i] = true;
-
+ s[2] = mu20;
+ s[3] = mu11;
+ s[4] = mu02;
+ for (int i = 2; i < 5; i++)
+ flags[i] = true;
}
-
-
/*!
Display ellipse feature.
@@ -399,28 +365,22 @@ 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<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness ) const
+void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double x = s[0] ;
- double y = s[1] ;
+ try {
+ double x = s[0];
+ double y = s[1];
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
+ double mu20 = s[2];
+ 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") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -433,41 +393,34 @@ 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<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness ) const
+void vpFeatureEllipse::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double x = s[0] ;
- double y = s[1] ;
+ try {
+ double x = s[0];
+ double y = s[1];
- double mu20 = s[2] ;
- double mu11 = s[3] ;
- double mu02 = s[4] ;
+ double mu20 = s[2];
+ 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") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
//! for memory issue (used by the vpServo class only)
vpFeatureEllipse *vpFeatureEllipse::duplicate() const
{
- vpFeatureEllipse *feature = new vpFeatureEllipse ;
- return feature ;
+ vpFeatureEllipse *feature = new vpFeatureEllipse;
+ return feature;
}
-unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0] ; }
-unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1] ; }
-unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2] ; }
-unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3] ; }
-unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4] ; }
+unsigned int vpFeatureEllipse::selectX() { return FEATURE_LINE[0]; }
+unsigned int vpFeatureEllipse::selectY() { return FEATURE_LINE[1]; }
+unsigned int vpFeatureEllipse::selectMu20() { return FEATURE_LINE[2]; }
+unsigned int vpFeatureEllipse::selectMu11() { return FEATURE_LINE[3]; }
+unsigned int vpFeatureEllipse::selectMu02() { return FEATURE_LINE[4]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureLine.cpp b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp
index c60312b..f3feae7 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureLine.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureLine.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpFeatureLine.cpp
\brief Class that defines 2D line visual feature
*/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureLine.h>
@@ -60,15 +59,12 @@
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
-//Color / image / display
+// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
-
-
#include <visp3/core/vpFeatureDisplay.h>
-
/*
@@ -85,89 +81,89 @@ other functionalities ar useful but not mandatory
/*!
Initialize the memory space requested for 2D line visual feature.
*/
-void
-vpFeatureLine::init()
+void vpFeatureLine::init()
{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 6;
-
- // memory allocation
- // x cos(theta) + y sin(theta) - rho = 0
- // s[0] = rho
- // s[1] = theta
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- A = B = C = D = 0.0 ;
+ // feature dimension
+ dim_s = 2;
+ nbParameters = 6;
+
+ // memory allocation
+ // x cos(theta) + y sin(theta) - rho = 0
+ // s[0] = rho
+ // s[1] = theta
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ A = B = C = D = 0.0;
}
-
-/*!
+/*!
Default constructor that build a visual feature.
*/
-vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0)
-{
- init() ;
-}
-
+vpFeatureLine::vpFeatureLine() : A(0), B(0), C(0), D(0) { init(); }
/*!
- Sets the values of \f$ \rho \f$ and \f$ \theta \f$ which represent the parameters of the 2D line feature.
+ Sets the values of \f$ \rho \f$ and \f$ \theta \f$ which represent the
+ parameters of the 2D line feature.
\param rho : \f$ \rho \f$ value to set.
\param theta : \f$ \theta \f$ value to set.
*/
-void
-vpFeatureLine::setRhoTheta(const double rho, const double theta)
+void vpFeatureLine::setRhoTheta(const double rho, const double theta)
{
- s[0] = rho ;
- s[1] = theta ;
- for( int i = 0; i < 2; i++) flags[i] = true;
+ s[0] = rho;
+ s[1] = theta;
+ for (int i = 0; i < 2; i++)
+ flags[i] = true;
}
-
/*!
- Sets the values of A, B, C and D which represent the parameters used to describe the equation of a plan in the camera frame.
- \f[ AX + BY + CZ + D = 0 \f]
- Those parameters are needed to compute the interaction matrix associated to a visual feature. Normally, two plans are needed to describe a line (the intersection of those two plans). But to compute the interaction matrix only one plan equation is required. The only one restrictions is that the value of D must not be equal to zero !
+ Sets the values of A, B, C and D which represent the parameters used to
+ describe the equation of a plan in the camera frame. \f[ AX + BY + CZ + D =
+ 0 \f] Those parameters are needed to compute the interaction matrix
+ associated to a visual feature. Normally, two plans are needed to describe a
+ line (the intersection of those two plans). But to compute the interaction
+ matrix only one plan equation is required. The only one restrictions is that
+ the value of D must not be equal to zero !
\param A_ : A value to set.
\param B_ : B value to set.
\param C_ : C value to set.
\param D_ : D value to set.
*/
-void
-vpFeatureLine::setABCD(const double A_, const double B_,
- const double C_, const double D_)
+void vpFeatureLine::setABCD(const double A_, const double B_, const double C_, const double D_)
{
- this->A = A_ ;
- this->B = B_ ;
- this->C = C_ ;
- this->D = D_ ;
- for(unsigned int i = 2; i < nbParameters; i++) flags[i] = true;
+ this->A = A_;
+ this->B = B_;
+ this->C = C_;
+ this->D = D_;
+ for (unsigned int i = 2; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Compute and return the interaction matrix \f$ L \f$. The computation
is made thanks to the values of the line feature \f$ \rho \f$ and
\f$ \theta \f$ and the equation of a plan to which the line belongs.
- \f[ L = \left[\begin{array}{c}L_{\rho} \\ L_{\theta}\end{array}\right] =
+ \f[ L = \left[\begin{array}{c}L_{\rho} \\ L_{\theta}\end{array}\right] =
\left[\begin{array}{cccccc}
- \lambda_{\rho}cos(\theta) & \lambda_{\rho}sin(\theta) & -\lambda_{\rho}\rho & (1+\rho^2)sin(\theta) & -(1+\rho^2)cos(\theta) & 0 \\
- \lambda_{\theta}cos(\theta) & \lambda_{\theta}sin(\theta) & -\lambda_{\theta}\rho & -\rho cos(\theta) & -\rho sin(\theta) & -1
+ \lambda_{\rho}cos(\theta) & \lambda_{\rho}sin(\theta) & -\lambda_{\rho}\rho
+ & (1+\rho^2)sin(\theta) & -(1+\rho^2)cos(\theta) & 0 \\
+ \lambda_{\theta}cos(\theta) & \lambda_{\theta}sin(\theta) &
+ -\lambda_{\theta}\rho & -\rho cos(\theta) & -\rho sin(\theta) & -1
\end{array}\right]\f]
Where :
- \f[ \lambda_{\rho} = (A \; \rho \; cos(\theta) + B \; \rho \; sin(\theta) + C) / D \f]
- \f[ \lambda_{\theta} = (A \; sin(\theta) - B \; cos(\theta)) / D \f]
+ \f[ \lambda_{\rho} = (A \; \rho \; cos(\theta) + B \; \rho \; sin(\theta) +
+ C) / D \f] \f[ \lambda_{\theta} = (A \; sin(\theta) - B \; cos(\theta)) / D
+ \f]
- \param select : Selection of a subset of the possible line features.
+ \param select : Selection of a subset of the possible line features.
- To compute the interaction matrix for all the two line features
use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
interaction matrix is \f$ [2 \times 6] \f$
@@ -199,38 +195,38 @@ vpFeatureLine::setABCD(const double A_, const double B_,
vpMatrix L_theta = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
*/
-vpMatrix
-vpFeatureLine::interaction(const unsigned int select)
+vpMatrix vpFeatureLine::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
-
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but rho "
+ "was not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but theta "
+ "was not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but A was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but A was "
+ "not set yet");
+ break;
case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but B was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but B was "
+ "not set yet");
+ break;
case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but C was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but C was "
+ "not set yet");
+ break;
case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but D was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but D was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -238,56 +234,49 @@ vpFeatureLine::interaction(const unsigned int select)
}
resetFlags();
}
- double rho = s[0] ;
- double theta = s[1] ;
-
+ double rho = s[0];
+ double theta = s[1];
double co = cos(theta);
double si = sin(theta);
- if (fabs(D) < 1e-6)
- {
- vpERROR_TRACE("Incorrect plane coordinates D is null, D = %f",D) ;
+ 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;
- double lambda_rho = (C + rho*A*co + rho*B*si)/D;
+ double lambda_theta = (A * si - B * co) / D;
+ double lambda_rho = (C + rho * A * co + rho * B * si) / D;
- if (vpFeatureLine::selectRho() & select )
- {
- vpMatrix Lrho(1,6) ;
+ if (vpFeatureLine::selectRho() & select) {
+ vpMatrix Lrho(1, 6);
+ Lrho[0][0] = co * lambda_rho;
+ Lrho[0][1] = si * lambda_rho;
+ Lrho[0][2] = -rho * lambda_rho;
+ Lrho[0][3] = si * (1.0 + rho * rho);
+ Lrho[0][4] = -co * (1.0 + rho * rho);
+ Lrho[0][5] = 0.0;
- Lrho[0][0]= co*lambda_rho;
- Lrho[0][1]= si*lambda_rho;
- Lrho[0][2]= -rho*lambda_rho;
- Lrho[0][3]= si*(1.0 + rho*rho);
- Lrho[0][4]= -co*(1.0 + rho*rho);
- Lrho[0][5]= 0.0;
-
- L = vpMatrix::stack(L,Lrho) ;
+ L.stack(Lrho);
}
- if (vpFeatureLine::selectTheta() & select )
- {
- vpMatrix Ltheta(1,6) ;
+ if (vpFeatureLine::selectTheta() & select) {
+ vpMatrix Ltheta(1, 6);
- Ltheta[0][0] = co*lambda_theta;
- Ltheta[0][1] = si*lambda_theta;
- Ltheta[0][2] = -rho*lambda_theta;
- Ltheta[0][3] = -rho*co;
- Ltheta[0][4] = -rho*si;
+ Ltheta[0][0] = co * lambda_theta;
+ Ltheta[0][1] = si * lambda_theta;
+ Ltheta[0][2] = -rho * lambda_theta;
+ Ltheta[0][3] = -rho * co;
+ Ltheta[0][4] = -rho * si;
Ltheta[0][5] = -1.0;
- L = vpMatrix::stack(L,Ltheta) ;
+ L.stack(Ltheta);
}
- return L ;
+ return L;
}
-
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
@@ -326,41 +315,37 @@ 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) ;
+ vpColVector e(0);
- try{
- if (vpFeatureLine::selectRho() & select )
- {
- vpColVector erho(1) ;
- erho[0] = s[0] - s_star[0] ;
+ try {
+ if (vpFeatureLine::selectRho() & select) {
+ vpColVector erho(1);
+ erho[0] = s[0] - s_star[0];
- e = vpColVector::stack(e,erho) ;
+ e = vpColVector::stack(e, erho);
}
- if (vpFeatureLine::selectTheta() & select )
- {
+ if (vpFeatureLine::selectTheta() & select) {
- double err = s[1] - s_star[1] ;
- while (err < -M_PI) err += 2*M_PI ;
- while (err > M_PI) err -= 2*M_PI ;
+ double err = s[1] - s_star[1];
+ while (err < -M_PI)
+ err += 2 * M_PI;
+ while (err > M_PI)
+ err -= 2 * M_PI;
- vpColVector etheta(1) ;
- etheta[0] = err ;
- e = vpColVector::stack(e,etheta) ;
+ vpColVector etheta(1);
+ etheta[0] = err;
+ e = vpColVector::stack(e, etheta);
}
- }
- catch(...) {
- throw ;
+ } catch (...) {
+ throw;
}
- return e ;
+ return e;
}
-
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
@@ -381,19 +366,18 @@ vpFeatureLine::error(const vpBasicFeature &s_star,
\endcode
*/
-void
-vpFeatureLine::print(const unsigned int select ) const
+void vpFeatureLine::print(const unsigned int select) const
{
- std::cout <<"Line:\t " << A <<"X+" << B <<"Y+" << C <<"Z +" << D <<"=0" <<std::endl ;;
- if (vpFeatureLine::selectRho() & select )
- std::cout << " \trho=" << s[0] ;
- if (vpFeatureLine::selectTheta() & select )
- std::cout << " \ttheta=" << s[1] ;
- std::cout <<std::endl ;
+ std::cout << "Line:\t " << A << "X+" << B << "Y+" << C << "Z +" << D << "=0" << std::endl;
+ ;
+ if (vpFeatureLine::selectRho() & select)
+ std::cout << " \trho=" << s[0];
+ if (vpFeatureLine::selectTheta() & select)
+ std::cout << " \ttheta=" << s[1];
+ std::cout << std::endl;
}
-
/*!
Build a 2D line visual feature from the line equation parameters \f$
@@ -408,15 +392,14 @@ vpFeatureLine::print(const unsigned int select ) const
\param theta : The \f$ \theta \f$ parameter.
*/
-void
-vpFeatureLine::buildFrom(const double rho, const double theta)
+void vpFeatureLine::buildFrom(const double rho, const double theta)
{
- s[0] = rho ;
- s[1] = theta ;
- for( int i = 0; i < 2; i++) flags[i] = true;
+ s[0] = rho;
+ s[1] = theta;
+ for (int i = 0; i < 2; i++)
+ flags[i] = true;
}
-
/*!
Build a 2D line visual feature from the line equation parameters \f$
@@ -445,20 +428,19 @@ 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 ;
- this->A = A_ ;
- this->B = B_ ;
- this->C = C_ ;
- this->D = D_ ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ s[0] = rho;
+ s[1] = theta;
+ this->A = A_;
+ this->B = B_;
+ this->C = C_;
+ this->D = D_;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Create an object with the same type.
@@ -471,12 +453,10 @@ void vpFeatureLine::buildFrom(const double rho, const double theta,
*/
vpFeatureLine *vpFeatureLine::duplicate() const
{
- vpFeatureLine *feature = new vpFeatureLine ;
- return feature ;
+ vpFeatureLine *feature = new vpFeatureLine;
+ return feature;
}
-
-
/*!
Display line feature.
@@ -487,24 +467,19 @@ vpFeatureLine *vpFeatureLine::duplicate() const
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeatureLine::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double rho,theta ;
- rho = getRho() ;
- theta = getTheta() ;
+ try {
+ double rho, theta;
+ rho = getRho();
+ theta = getTheta();
- vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -518,24 +493,19 @@ vpFeatureLine::display(const vpCameraParameters &cam,
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeatureLine::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeatureLine::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double rho,theta ;
- rho = getRho() ;
- theta = getTheta() ;
+ try {
+ double rho, theta;
+ rho = getRho();
+ theta = getTheta();
- vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayLine(rho, theta, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -543,9 +513,11 @@ vpFeatureLine::display(const vpCameraParameters &cam,
Function used to select the \f$ \rho \f$ subset of the line visual feature.
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \rho \f$.
+ This function is to use in conjunction with interaction() in order to
+ compute the interaction matrix associated to \f$ \rho \f$.
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+ This function is also useful in the vpServo class to indicate that a subset
+ of the visual feature is to use in the control law:
\code
vpFeatureLine s;
@@ -555,15 +527,18 @@ vpFeatureLine::display(const vpCameraParameters &cam,
task.addFeature(s, vpFeatureLine::selectRho());
\endcode
*/
-unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0]; }
/*!
- Function used to select the \f$ \theta \f$ subset of the line visual feature.
+ Function used to select the \f$ \theta \f$ subset of the line visual
+ feature.
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ \theta \f$.
+ This function is to use in conjunction with interaction() in order to
+ compute the interaction matrix associated to \f$ \theta \f$.
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+ This function is also useful in the vpServo class to indicate that a subset
+ of the visual feature is to use in the control law:
\code
vpFeatureLine s;
@@ -573,4 +548,4 @@ unsigned int vpFeatureLine::selectRho() { return FEATURE_LINE[0] ; }
task.addFeature(s, vpFeatureLine::selectTheta());
\endcode
*/
-unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureLine::selectTheta() { return FEATURE_LINE[1]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp
index f1abf5d..de8e686 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureLuminance.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,18 +36,16 @@
*
*****************************************************************************/
-
-#include <visp3/core/vpMatrix.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/core/vpException.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpImageFilter.h>
-#include <visp3/core/vpException.h>
+#include <visp3/core/vpMatrix.h>
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/visual_features/vpFeatureLuminance.h>
-
/*!
\file vpFeatureLuminance.cpp
\brief Class that defines the image luminance visual feature
@@ -54,69 +53,64 @@
For more details see \cite Collewet08c.
*/
-
-
/*!
Initialize the memory space requested for vpFeatureLuminance visual feature.
*/
-void
-vpFeatureLuminance::init()
+void vpFeatureLuminance::init()
{
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
- //default value Z (1 meters)
- Z = 1;
+ // default value Z (1 meters)
+ Z = 1;
- firstTimeIn =0 ;
+ firstTimeIn = 0;
- nbr = nbc = 0;
+ nbr = nbc = 0;
}
-
-void
-vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
+void vpFeatureLuminance::init(unsigned int _nbr, unsigned int _nbc, double _Z)
{
- init() ;
+ init();
- nbr = _nbr ;
- nbc = _nbc ;
+ nbr = _nbr;
+ nbc = _nbc;
- if((nbr < 2*bord) || (nbc < 2*bord)){
+ if ((nbr < 2 * bord) || (nbc < 2 * bord)) {
throw vpException(vpException::dimensionError, "border is too important compared to number of row or column.");
}
// number of feature = nb column x nb lines in the images
- dim_s = (nbr-2*bord)*(nbc-2*bord) ;
+ dim_s = (nbr - 2 * bord) * (nbc - 2 * bord);
+
+ s.resize(dim_s);
- s.resize(dim_s) ;
-
if (pixInfo != NULL)
- delete [] pixInfo;
+ delete[] pixInfo;
- pixInfo = new vpLuminance[dim_s] ;
-
- Z = _Z ;
+ pixInfo = new vpLuminance[dim_s];
+
+ Z = _Z;
}
-/*!
+/*!
Default constructor that build a visual feature.
*/
-vpFeatureLuminance::vpFeatureLuminance()
- : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
+vpFeatureLuminance::vpFeatureLuminance() : Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
{
- nbParameters = 1;
- dim_s = 0 ;
- flags = NULL;
+ nbParameters = 1;
+ dim_s = 0;
+ flags = NULL;
- init() ;
+ init();
}
/*!
Copy constructor.
*/
-vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance& f)
+vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance &f)
: vpBasicFeature(f), Z(1), nbr(0), nbc(0), bord(10), pixInfo(NULL), firstTimeIn(0), cam()
{
*this = f;
@@ -125,7 +119,7 @@ vpFeatureLuminance::vpFeatureLuminance(const vpFeatureLuminance& f)
/*!
Copy operator.
*/
-vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance& f)
+vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance &f)
{
Z = f.Z;
nbr = f.nbr;
@@ -134,240 +128,199 @@ vpFeatureLuminance &vpFeatureLuminance::operator=(const vpFeatureLuminance& f)
firstTimeIn = f.firstTimeIn;
cam = f.cam;
if (pixInfo)
- delete [] pixInfo;
- pixInfo = new vpLuminance[dim_s] ;
- for(unsigned int i=0; i< dim_s; i++)
+ delete[] pixInfo;
+ pixInfo = new vpLuminance[dim_s];
+ for (unsigned int i = 0; i < dim_s; i++)
pixInfo[i] = f.pixInfo[i];
return (*this);
}
-/*!
+/*!
Destructor that free allocated memory.
*/
-vpFeatureLuminance::~vpFeatureLuminance()
+vpFeatureLuminance::~vpFeatureLuminance()
{
- if (pixInfo != NULL) delete [] pixInfo ;
+ if (pixInfo != NULL)
+ delete[] pixInfo;
}
/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\param Z_ : \f$ Z \f$ value to set.
*/
-void
-vpFeatureLuminance::set_Z(const double Z_)
+void vpFeatureLuminance::set_Z(const double Z_)
{
- this->Z = Z_ ;
- flags[0] = true;
+ this->Z = Z_;
+ flags[0] = true;
}
-
/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\return The value of \f$ Z \f$.
*/
-double
-vpFeatureLuminance::get_Z() const
-{
- return Z ;
-}
-
-
-void
-vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam)
-{
- cam = _cam ;
-}
+double vpFeatureLuminance::get_Z() const { return Z; }
+void vpFeatureLuminance::setCameraParameters(vpCameraParameters &_cam) { cam = _cam; }
/*!
Build a luminance feature directly from the image
*/
-void
-vpFeatureLuminance::buildFrom(vpImage<unsigned char> &I)
+void vpFeatureLuminance::buildFrom(vpImage<unsigned char> &I)
{
unsigned int l = 0;
- double Ix,Iy ;
-
- double px = cam.get_px() ;
- double py = cam.get_py() ;
-
-
- if (firstTimeIn==0)
- {
- firstTimeIn=1 ;
- l =0 ;
- for (unsigned int i=bord; i < nbr-bord ; i++)
- {
- // cout << i << endl ;
- for (unsigned int j = bord ; j < nbc-bord; j++)
- { double x=0,y=0;
- vpPixelMeterConversion::convertPoint(cam,
- j,i,
- x,y) ;
-
- pixInfo[l].x = x;
- pixInfo[l].y = y;
-
- pixInfo[l].Z = Z ;
-
- l++;
- }
- }
- }
+ double Ix, Iy;
- l= 0 ;
- for (unsigned int i=bord; i < nbr-bord ; i++)
- {
+ double px = cam.get_px();
+ double py = cam.get_py();
+
+ if (firstTimeIn == 0) {
+ firstTimeIn = 1;
+ l = 0;
+ for (unsigned int i = bord; i < nbr - bord; i++) {
// cout << i << endl ;
- for (unsigned int j = bord ; j < nbc-bord; j++)
- {
- // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
- Ix = px * vpImageFilter::derivativeFilterX(I,i,j) ;
- Iy = py * vpImageFilter::derivativeFilterY(I,i,j) ;
-
- // Calcul de Z
-
- pixInfo[l].I = I[i][j] ;
- s[l] = I[i][j] ;
- pixInfo[l].Ix = Ix;
- pixInfo[l].Iy = Iy;
-
- l++;
- }
+ for (unsigned int j = bord; j < nbc - bord; j++) {
+ double x = 0, y = 0;
+ vpPixelMeterConversion::convertPoint(cam, j, i, x, y);
+
+ pixInfo[l].x = x;
+ pixInfo[l].y = y;
+
+ pixInfo[l].Z = Z;
+
+ l++;
+ }
}
+ }
-}
+ l = 0;
+ for (unsigned int i = bord; i < nbr - bord; i++) {
+ // cout << i << endl ;
+ for (unsigned int j = bord; j < nbc - bord; j++) {
+ // cout << dim_s <<" " <<l <<" " <<i << " " << j <<endl ;
+ Ix = px * vpImageFilter::derivativeFilterX(I, i, j);
+ Iy = py * vpImageFilter::derivativeFilterY(I, i, j);
+ // Calcul de Z
+ pixInfo[l].I = I[i][j];
+ s[l] = I[i][j];
+ pixInfo[l].Ix = Ix;
+ pixInfo[l].Iy = Iy;
+ l++;
+ }
+ }
+}
/*!
- Compute and return the interaction matrix \f$ L_I \f$. The computation is made
- thanks to the values of the luminance features \f$ I \f$
+ Compute and return the interaction matrix \f$ L_I \f$. The computation is
+ made thanks to the values of the luminance features \f$ I \f$
*/
-void
-vpFeatureLuminance::interaction(vpMatrix &L)
-{
- L.resize(dim_s,6) ;
+void vpFeatureLuminance::interaction(vpMatrix &L)
+{
+ L.resize(dim_s, 6);
- for(unsigned int m = 0; m< L.getRows(); m++)
- {
+ for (unsigned int m = 0; m < L.getRows(); m++) {
double Ix = pixInfo[m].Ix;
double Iy = pixInfo[m].Iy;
- double x = pixInfo[m].x ;
- double y = pixInfo[m].y ;
- double Zinv = 1 / pixInfo[m].Z;
+ double x = pixInfo[m].x;
+ double y = pixInfo[m].y;
+ double Zinv = 1 / pixInfo[m].Z;
{
L[m][0] = Ix * Zinv;
L[m][1] = Iy * Zinv;
- L[m][2] = -(x*Ix+y*Iy)*Zinv;
- L[m][3] = -Ix*x*y-(1+y*y)*Iy;
- L[m][4] = (1+x*x)*Ix + Iy*x*y;
- L[m][5] = Iy*x-Ix*y;
+ L[m][2] = -(x * Ix + y * Iy) * Zinv;
+ L[m][3] = -Ix * x * y - (1 + y * y) * Iy;
+ L[m][4] = (1 + x * x) * Ix + Iy * x * y;
+ L[m][5] = Iy * x - Ix * y;
}
}
}
/*!
- Compute and return the interaction matrix \f$ L_I \f$. The computation is made
- thanks to the values of the luminance features \f$ I \f$
+ Compute and return the interaction matrix \f$ L_I \f$. The computation is
+ made thanks to the values of the luminance features \f$ I \f$
*/
-vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
+vpMatrix vpFeatureLuminance::interaction(const unsigned int /* select */)
{
- /* static */ vpMatrix L ; // warning C4640: 'L' : construction of local static object is not thread-safe
- interaction(L) ;
- return L ;
+ /* static */ vpMatrix L; // warning C4640: 'L' : construction of local
+ // static object is not thread-safe
+ interaction(L);
+ return L;
}
-
/*!
Compute the error \f$ (I-I^*)\f$ between the current and the desired
-
+
\param s_star : Desired visual feature.
\param e : Error between the current and the desired features.
*/
-void
-vpFeatureLuminance::error(const vpBasicFeature &s_star,
- vpColVector &e)
+void vpFeatureLuminance::error(const vpBasicFeature &s_star, vpColVector &e)
{
- e.resize(dim_s) ;
+ e.resize(dim_s);
- for (unsigned int i =0 ; i < dim_s ; i++)
- {
- e[i] = s[i] - s_star[i] ;
- }
+ for (unsigned int i = 0; i < dim_s; i++) {
+ e[i] = s[i] - s_star[i];
+ }
}
-
-
/*!
Compute the error \f$ (I-I^*)\f$ between the current and the desired
-
+
\param s_star : Desired visual feature.
\param select : Not used.
*/
-vpColVector
-vpFeatureLuminance::error(const vpBasicFeature &s_star,
- const unsigned int /* select */)
+vpColVector vpFeatureLuminance::error(const vpBasicFeature &s_star, const unsigned int /* select */)
{
- /* static */ vpColVector e ; // warning C4640: 'e' : construction of local static object is not thread-safe
-
- error(s_star, e) ;
-
- return e ;
-
-}
-
+ /* static */ vpColVector e; // warning C4640: 'e' : construction of local
+ // static object is not thread-safe
+ error(s_star, e);
+ return e;
+}
/*!
Not implemented.
*/
-void
-vpFeatureLuminance::print(const unsigned int /* select */) const
+void vpFeatureLuminance::print(const unsigned int /* select */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
- }
-
-
+}
/*!
Not implemented.
*/
-void
-vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
- const vpImage<unsigned char> & /* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
@@ -378,24 +331,19 @@ vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
Not implemented.
*/
-void
-vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
- const vpImage<vpRGBa> & /* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureLuminance::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
}
-
/*!
Create an object with the same type.
@@ -408,11 +356,10 @@ vpFeatureLuminance::display(const vpCameraParameters & /* cam */,
*/
vpFeatureLuminance *vpFeatureLuminance::duplicate() const
{
- vpFeatureLuminance *feature = new vpFeatureLuminance ;
- return feature ;
+ vpFeatureLuminance *feature = new vpFeatureLuminance;
+ return feature;
}
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp
index 91b47bc..a5bed93 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMoment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,146 +36,156 @@
*
*****************************************************************************/
-#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpMoment.h>
-#include <visp3/visual_features/vpFeatureMomentDatabase.h>
#include <visp3/core/vpMomentDatabase.h>
-#include <visp3/core/vpMath.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpDebug.h>
#include <vector>
+#include <visp3/core/vpDebug.h>
class vpBasicFeature;
/*!
Initialize common parameters for moment features.
*/
-void vpFeatureMoment::init (){
- //feature dimension
- /*
- * The dimension of the visual feature is set according to the size of the vpMoment associated to it.
- * This partly explains why vpFeatureMomentBasic cannot be used directly as a visual feature.
- */
- if(this->moment!=NULL)
- dim_s = (unsigned int)this->moment->get().size();
- else
- dim_s = 0;
-
- nbParameters = 1;
-
- // memory allocation
- s.resize(dim_s) ;
- for(unsigned int i=0;i<dim_s;i++)
- s[i] = 0;
-
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = false;
+void vpFeatureMoment::init()
+{
+ // feature dimension
+ /*
+ * The dimension of the visual feature is set according to the size of the
+ * vpMoment associated to it. This partly explains why vpFeatureMomentBasic
+ * cannot be used directly as a visual feature.
+ */
+ if (this->moment != NULL)
+ dim_s = (unsigned int)this->moment->get().size();
+ else
+ dim_s = 0;
+
+ nbParameters = 1;
+
+ // memory allocation
+ s.resize(dim_s);
+ for (unsigned int i = 0; i < dim_s; i++)
+ s[i] = 0;
+
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
}
-
/*!
Feature's dimension according to selection.
*/
-int vpFeatureMoment::getDimension (unsigned int select) const{
- int dim=0;
+int vpFeatureMoment::getDimension(unsigned int select) const
+{
+ int dim = 0;
- for(unsigned int i=0;i<dim_s;++i)
- if(vpBasicFeature::FEATURE_LINE[i] & select)
- dim++;
+ for (unsigned int i = 0; i < dim_s; ++i)
+ if (vpBasicFeature::FEATURE_LINE[i] & select)
+ dim++;
- return dim;
+ return dim;
}
-
/*!
Outputs the content of the feature: it's corresponding selected moments.
*/
-void vpFeatureMoment::print (unsigned int select) const{
- for(unsigned int i=0;i<dim_s;++i){
- if(vpBasicFeature::FEATURE_LINE[i] & select){
- std::cout << s[i] << ",";
- }
+void vpFeatureMoment::print(unsigned int select) const
+{
+ for (unsigned int i = 0; i < dim_s; ++i) {
+ if (vpBasicFeature::FEATURE_LINE[i] & select) {
+ std::cout << s[i] << ",";
}
+ }
- std::cout << std::endl;
+ std::cout << std::endl;
}
/*!
- Not implemented since visual representation of a moment doesn't often make sense.
+ Not implemented since visual representation of a moment doesn't often make
+ sense.
*/
-void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< unsigned char > &I,
- const vpColor &color, unsigned int thickness) const
+void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
- //visual representation of a moment doesn't often make sense
- (void)cam;
- (void)I;
- (void)color;
- (void)thickness;
+ // visual representation of a moment doesn't often make sense
+ (void)cam;
+ (void)I;
+ (void)color;
+ (void)thickness;
}
/*!
- Not implemented since visual representation of a moment doesn't often make sense.
-*/
-void vpFeatureMoment::display (const vpCameraParameters &cam, const vpImage< vpRGBa > &I,
- const vpColor &color, unsigned int thickness) const
+ Not implemented since visual representation of a moment doesn't often make
+ sense.
+*/
+void vpFeatureMoment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
- (void)cam;
- (void)I;
- (void)color;
- (void)thickness;
+ (void)cam;
+ (void)I;
+ (void)color;
+ (void)thickness;
}
/*!
- Updates the interaction matrices with the image plane the camera is facing. The plane must be in the format: \f$ \frac{1}{Z}=Ax+By+C \f$ .
- The moment primitives MUST be updated before calling this function.
+ Updates the interaction matrices with the image plane the camera is facing.
+ The plane must be in the format: \f$ \frac{1}{Z}=Ax+By+C \f$ . The moment
+ primitives MUST be updated before calling this function.
- This method also computes the interaction matrix. Therefore, you must call vpFeatureMoment::update before calling vpFeatureMoment::interaction.
+ This method also computes the interaction matrix. Therefore, you must call
+ vpFeatureMoment::update before calling vpFeatureMoment::interaction.
- \attention The behaviour of this method is not the same as vpMoment::update which only acknowledges the new object. This method also computes the interaction matrices.
+ \attention The behaviour of this method is not the same as vpMoment::update
+ which only acknowledges the new object. This method also computes the
+ interaction matrices.
\param A_ : A coefficient of the plane.
\param B_ : B coefficient of the plane.
\param C_ : C coefficient of the plane.
*/
-void vpFeatureMoment::update (double A_, double B_, double C_){
- this->A = A_;
- this->B = B_;
- this->C = C_;
-
- if(moment==NULL){
- bool found;
- this->moment = &(moments.get(momentName(),found));
- if(!found) throw vpException(vpException::notInitialized,"Moment not found for feature");
- }
- nbParameters = 1;
- if(this->moment!=NULL){
- dim_s = (unsigned int)this->moment->get().size();
+void vpFeatureMoment::update(double A_, double B_, double C_)
+{
+ this->A = A_;
+ this->B = B_;
+ this->C = C_;
+
+ if (moment == NULL) {
+ bool found;
+ this->moment = &(moments.get(momentName(), found));
+ if (!found)
+ throw vpException(vpException::notInitialized, "Moment not found for feature");
+ }
+ nbParameters = 1;
+ if (this->moment != NULL) {
+ dim_s = (unsigned int)this->moment->get().size();
- s.resize(dim_s);
+ s.resize(dim_s);
- for(unsigned int i=0;i<dim_s;i++)
- s[i] = this->moment->get()[i];
+ for (unsigned int i = 0; i < dim_s; i++)
+ s[i] = this->moment->get()[i];
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++)
- flags[i] = false;
- }else
- dim_s = 0;
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+ } else
+ dim_s = 0;
- compute_interaction();
+ compute_interaction();
}
/*!
Retrieves the interaction matrix. No computation is done.
-
- \param select : Feature selector.
-
+
+ \param select : Feature selector.
+
\return The corresponding interaction matrix.
There is no rule about the format of the feature selector. It may be
@@ -185,16 +196,17 @@ void vpFeatureMoment::update (double A_, double B_, double C_){
select couples \f$ (i,j,k,l...) \f$ in the following format: 1 << i
+ 1 << j + 1 << k + 1 << l.
*/
-vpMatrix vpFeatureMoment::interaction (unsigned int select){
- vpMatrix L(0,0);
+vpMatrix vpFeatureMoment::interaction(unsigned int select)
+{
+ vpMatrix L(0, 0);
- for(unsigned int i=0;i<dim_s;++i){
- if(vpBasicFeature::FEATURE_LINE[i] & select){
- L.stack(interaction_matrices[i]);
- }
+ for (unsigned int i = 0; i < dim_s; ++i) {
+ if (vpBasicFeature::FEATURE_LINE[i] & select) {
+ L.stack(interaction_matrices[i]);
}
+ }
- return L;
+ return L;
}
/*! Duplicates the feature into a vpGenericFeature harbouring the
@@ -202,70 +214,74 @@ vpMatrix vpFeatureMoment::interaction (unsigned int select){
type. While it still can compute interaction matrices and has acces
to it's moment primitive, it has lost all precise information about
its precise type and therefore cannot be used in a feature database.
-
+
\return The corresponding feature.
*/
-vpBasicFeature* vpFeatureMoment::duplicate () const
+vpBasicFeature *vpFeatureMoment::duplicate() const
{
- vpFeatureMoment* feat = new vpMomentGenericFeature(moments,A,B,C,featureMomentsDataBase,moment);
- feat->interaction_matrices = interaction_matrices;
- feat->dim_s = dim_s;
- feat->nbParameters = nbParameters;
- // memory allocation
- feat->s.resize(dim_s) ;
- for(unsigned int i=0;i<dim_s;i++)
- feat->s[i] = this->s[i];
-
- feat->flags = new bool[(unsigned int)nbParameters];
- for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
- feat->flags[i] = flags[i];
-
- return feat;
+ vpFeatureMoment *feat = new vpMomentGenericFeature(moments, A, B, C, featureMomentsDataBase, moment);
+ feat->interaction_matrices = interaction_matrices;
+ feat->dim_s = dim_s;
+ feat->nbParameters = nbParameters;
+ // memory allocation
+ feat->s.resize(dim_s);
+ for (unsigned int i = 0; i < dim_s; i++)
+ feat->s[i] = this->s[i];
+
+ feat->flags = new bool[(unsigned int)nbParameters];
+ for (unsigned int i = 0; i < (unsigned int)nbParameters; i++)
+ feat->flags[i] = flags[i];
+
+ return feat;
}
/*!
- Links the feature to the feature's database. NB: The feature's database is different from the moment's database.
- \param featureMoments : database in which the moment features are stored.
-
+ Links the feature to the feature's database. NB: The feature's database is
+ different from the moment's database. \param featureMoments : database in
+ which the moment features are stored.
+
*/
-void vpFeatureMoment::linkTo(vpFeatureMomentDatabase& featureMoments){
- if (strlen( name() ) >= 255) {
- throw(vpException(vpException::memoryAllocationError,
- "Not enough memory to intialize the moment name"));
+void vpFeatureMoment::linkTo(vpFeatureMomentDatabase &featureMoments)
+{
+ if (strlen(name()) >= 255) {
+ throw(vpException(vpException::memoryAllocationError, "Not enough memory to intialize the moment name"));
}
- std::strcpy(_name,name());
- this->featureMomentsDataBase=&featureMoments;
+ std::strcpy(_name, name());
+ this->featureMomentsDataBase = &featureMoments;
- featureMoments.add(*this,_name);
+ featureMoments.add(*this, _name);
}
+void vpFeatureMoment::compute_interaction() {}
-void vpFeatureMoment::compute_interaction (){
-
-}
+vpFeatureMoment::~vpFeatureMoment() {}
-vpFeatureMoment::~vpFeatureMoment (){
-}
-
-VISP_EXPORT std::ostream& operator<<(std::ostream & os, const vpFeatureMoment& featM) {
- /*
- A const_cast is forced here since interaction() defined in vpBasicFeature() is not const
- But introducing const in vpBasicFeature() can break a lot of client code
- */
- vpMatrix Lcomplete((unsigned int)featM.getDimension(), 6); // 6 corresponds to 6velocities in standard interaction matrix
- Lcomplete = const_cast<vpFeatureMoment&>(featM).interaction(vpBasicFeature::FEATURE_ALL);
- Lcomplete.matlabPrint(os);
- return os;
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpFeatureMoment &featM)
+{
+ /*
+ A const_cast is forced here since interaction() defined in vpBasicFeature()
+ is not const But introducing const in vpBasicFeature() can break a lot of
+ client code
+ */
+ vpMatrix Lcomplete((unsigned int)featM.getDimension(),
+ 6); // 6 corresponds to 6velocities in standard interaction matrix
+ Lcomplete = const_cast<vpFeatureMoment &>(featM).interaction(vpBasicFeature::FEATURE_ALL);
+ Lcomplete.matlabPrint(os);
+ return os;
}
/*!
Interface function to display the moments and other interaction matrices
on which a particular vpFeatureMoment is dependent upon
Not made pure to maintain compatibility
-Recommended : Types inheriting from vpFeatureMoment should implement this function
+Recommended : Types inheriting from vpFeatureMoment should implement this
+function
*/
-void
-vpFeatureMoment::printDependencies(std::ostream& os) const{
- os << " WARNING : Falling back to base class version of printDependencies() in vpFeatureMoment. To prevent that, this has to be implemented in the derived classes!" << std::endl;
+void vpFeatureMoment::printDependencies(std::ostream &os) const
+{
+ os << " WARNING : Falling back to base class version of "
+ "printDependencies() in vpFeatureMoment. To prevent that, this has "
+ "to be implemented in the derived classes!"
+ << std::endl;
}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp
index 69f2eed..c57bb2a 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAlpha.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,15 +36,15 @@
*
*****************************************************************************/
-#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentAlpha.h>
-#include <visp3/core/vpMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <vector>
#include <limits>
+#include <vector>
#ifdef VISP_MOMENTS_COMBINE_MATRICES
@@ -54,20 +55,31 @@
- vpMomentCentered
- vpFeatureMomentCentered
*/
-void vpFeatureMomentAlpha::compute_interaction(){
- bool found_moment_centered;
- bool found_FeatureMoment_centered;
-
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- double multiplier = -1./(momentCentered.get(2,0)*momentCentered.get(2,0)-2*momentCentered.get(0,2)*momentCentered.get(2,0)+4*momentCentered.get(1,1)*momentCentered.get(1,1)+momentCentered.get(0,2)*momentCentered.get(0,2));
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[0] = multiplier*(momentCentered.get(1,1)*featureMomentCentered.interaction(2,0)+(momentCentered.get(0,2)-momentCentered.get(2,0))*featureMomentCentered.interaction(1,1)-momentCentered.get(1,1)*featureMomentCentered.interaction(0,2));
+void vpFeatureMomentAlpha::compute_interaction()
+{
+ bool found_moment_centered;
+ bool found_FeatureMoment_centered;
+
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
+ vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
+ featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
+
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_FeatureMoment_centered)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
+
+ double multiplier =
+ -1. /
+ (momentCentered.get(2, 0) * momentCentered.get(2, 0) - 2 * momentCentered.get(0, 2) * momentCentered.get(2, 0) +
+ 4 * momentCentered.get(1, 1) * momentCentered.get(1, 1) + momentCentered.get(0, 2) * momentCentered.get(0, 2));
+
+ interaction_matrices[0].resize(1, 6);
+ interaction_matrices[0] =
+ multiplier * (momentCentered.get(1, 1) * featureMomentCentered.interaction(2, 0) +
+ (momentCentered.get(0, 2) - momentCentered.get(2, 0)) * featureMomentCentered.interaction(1, 1) -
+ momentCentered.get(1, 1) * featureMomentCentered.interaction(0, 2));
}
#else
@@ -79,81 +91,92 @@ void vpFeatureMomentAlpha::compute_interaction(){
- vpMomentCentered
- vpMomentGravityCenter
*/
-void vpFeatureMomentAlpha::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentObject& momentObject = moment->getObject();
-
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- double mu11 = momentCentered.get(1,1);
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu12 = momentCentered.get(1,2);
- double mu21 = momentCentered.get(2,1);
- double mu03 = momentCentered.get(0,3);
- double mu30 = momentCentered.get(3,0);
-
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double Avx,Avy,Avz,Awx,Awy;
- double beta,gamma;
-
- if (momentObject.getType()==vpMomentObject::DISCRETE){
- beta = 4;
- gamma = 2;
- } else {
- beta = 5;
- gamma = 1;
- }
-
- double d = (mu20-mu02)*(mu20-mu02) + 4 * mu11*mu11;
- double DA = mu20+mu02;
- double DA_2 = DA*DA;
- double mu11_2 = mu11*mu11;
-
- Avx = mu11*DA*A/d+(DA*mu02+(0.5)*d-(0.5)*DA_2)*B/d;
- Avy = (DA*mu02-(0.5)*d-(.5)*DA_2)*A/d-B*mu11*DA/d;
-
- Awx = (beta*(mu12*(mu20-mu02)+mu11*(mu03-mu21))+gamma*Xg*(mu02*(mu20-mu02)-2*mu11_2)+gamma*Yg*mu11*(mu20+mu02))/d;
- Awy = (beta*(mu21*(mu02-mu20)+mu11*(mu30-mu12))+gamma*Xg*mu11*(mu20+mu02)+gamma*Yg*(mu20*(mu02-mu20)-2*mu11_2))/d;
-
- Avz = B*Awx-A*Awy;
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Avx;
- interaction_matrices[0][0][VY] = Avy;
- interaction_matrices[0][0][VZ] = Avz;
-
- interaction_matrices[0][0][WX] = Awx;
- interaction_matrices[0][0][WY] = Awy;
- interaction_matrices[0][0][WZ] = -1.;
+void vpFeatureMomentAlpha::compute_interaction()
+{
+ bool found_moment_centered;
+ bool found_moment_gravity;
+
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+ const vpMomentObject &momentObject = moment->getObject();
+
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+
+ double mu11 = momentCentered.get(1, 1);
+ double mu20 = momentCentered.get(2, 0);
+ double mu02 = momentCentered.get(0, 2);
+ double mu12 = momentCentered.get(1, 2);
+ double mu21 = momentCentered.get(2, 1);
+ double mu03 = momentCentered.get(0, 3);
+ double mu30 = momentCentered.get(3, 0);
+
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double Avx, Avy, Avz, Awx, Awy;
+ double beta, gamma;
+
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ beta = 4;
+ gamma = 2;
+ } else {
+ beta = 5;
+ gamma = 1;
+ }
+
+ double d = (mu20 - mu02) * (mu20 - mu02) + 4 * mu11 * mu11;
+ double DA = mu20 + mu02;
+ double DA_2 = DA * DA;
+ double mu11_2 = mu11 * mu11;
+
+ Avx = mu11 * DA * A / d + (DA * mu02 + (0.5) * d - (0.5) * DA_2) * B / d;
+ Avy = (DA * mu02 - (0.5) * d - (.5) * DA_2) * A / d - B * mu11 * DA / d;
+
+ Awx = (beta * (mu12 * (mu20 - mu02) + mu11 * (mu03 - mu21)) + gamma * Xg * (mu02 * (mu20 - mu02) - 2 * mu11_2) +
+ gamma * Yg * mu11 * (mu20 + mu02)) /
+ d;
+ Awy = (beta * (mu21 * (mu02 - mu20) + mu11 * (mu30 - mu12)) + gamma * Xg * mu11 * (mu20 + mu02) +
+ gamma * Yg * (mu20 * (mu02 - mu20) - 2 * mu11_2)) /
+ d;
+
+ Avz = B * Awx - A * Awy;
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1, 6);
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Avx;
+ interaction_matrices[0][0][VY] = Avy;
+ interaction_matrices[0][0][VZ] = Avz;
+
+ interaction_matrices[0][0][WX] = Awx;
+ interaction_matrices[0][0][WY] = Awy;
+ interaction_matrices[0][0][WZ] = -1.;
}
-vpColVector vpFeatureMomentAlpha::error (const vpBasicFeature &s_star, const unsigned int /* select */){
- vpColVector e(0) ;
- double err = s[0] - s_star[0] ;
+vpColVector vpFeatureMomentAlpha::error(const vpBasicFeature &s_star, const unsigned int /* select */)
+{
+ vpColVector e(0);
+ double err = s[0] - s_star[0];
- if (err < -M_PI) err += 2*M_PI ;
- if (err > M_PI) err -= 2*M_PI ;
+ if (err < -M_PI)
+ err += 2 * M_PI;
+ if (err > M_PI)
+ err -= 2 * M_PI;
- vpColVector ecv(1) ;
- ecv[0] = err ;
- e = vpColVector::stack(e,ecv) ;
+ vpColVector ecv(1);
+ ecv[0] = err;
+ e = vpColVector::stack(e, ecv);
return e;
}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp
index aad20e6..f4138e6 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentArea.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,67 +35,70 @@
* Manikandan Bakthavatchalam
*
*****************************************************************************/
-#include <visp3/core/vpConfig.h>
-#include <vector>
-#include <limits>
#include <cassert>
-#include <limits> // numeric_limits
+#include <limits>
+#include <limits> // numeric_limits
+#include <vector>
+#include <visp3/core/vpConfig.h>
-#include <visp3/visual_features/vpFeatureMomentArea.h>
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentGravityCenter.h>
#include <visp3/core/vpMomentArea.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/visual_features/vpFeatureMomentArea.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for the normalized surface moment. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentGravityCenter
- vpMomentArea
*/
-void vpFeatureMomentArea::compute_interaction(){
+void vpFeatureMomentArea::compute_interaction()
+{
+
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1, 6);
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
+ // Retreive the moment object associated with this feature
+ vpMomentObject mobj = moment->getObject();
+ if (mobj.getType() == vpMomentObject::DISCRETE) {
+ /*
+ * The interaction matrix for the discrete case is zero
+ * since the feature m00 is constant.
+ * Refer thesis of Omar Tahri 2005 [Section 3.4.22]
+ */
+ interaction_matrices[0][0][0] = 0.;
+ interaction_matrices[0][0][1] = 0.;
+ interaction_matrices[0][0][2] = 0.;
+ interaction_matrices[0][0][3] = 0.;
+ interaction_matrices[0][0][4] = 0.;
+ interaction_matrices[0][0][5] = 0.;
+ } else {
+ // Get Xg and Yg
+ bool found_xgyg;
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_xgyg));
+ if (!found_xgyg)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
- // Retreive the moment object associated with this feature
- vpMomentObject mobj = moment->getObject();
- if (mobj.getType()==vpMomentObject::DISCRETE) {
- /*
- * The interaction matrix for the discrete case is zero
- * since the feature m00 is constant.
- * Refer thesis of Omar Tahri 2005 [Section 3.4.22]
- */
- interaction_matrices[0][0][0] = 0.;
- interaction_matrices[0][0][1] = 0.;
- interaction_matrices[0][0][2] = 0.;
- interaction_matrices[0][0][3] = 0.;
- interaction_matrices[0][0][4] = 0.;
- interaction_matrices[0][0][5] = 0.;
- }
- else {
- // Get Xg and Yg
- bool found_xgyg;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_xgyg));
- if (!found_xgyg) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ bool found_m00;
+ const vpMomentArea &areamoment = static_cast<const vpMomentArea &>(moments.get("vpMomentArea", found_m00));
+ if (!found_m00)
+ throw vpException(vpException::notInitialized, "vpMomentArea not found");
- bool found_m00;
- const vpMomentArea& areamoment = static_cast<const vpMomentArea&>(moments.get("vpMomentArea", found_m00));
- if (!found_m00) throw vpException(vpException::notInitialized,"vpMomentArea not found");
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
+ double a = areamoment.get()[0]; // Area scalar
- double a = areamoment.get()[0]; // Area scalar
-
- assert(std::fabs(a-mobj.get(0,0)) < a*std::numeric_limits<double>::epsilon());
+ assert(std::fabs(a - mobj.get(0, 0)) < a * std::numeric_limits<double>::epsilon());
- interaction_matrices[0][0][0] = -a*A;
- interaction_matrices[0][0][1] = -a*B;
- interaction_matrices[0][0][2] = (3*a)*(A*Xg+B*Yg)+(2*C*a);
- interaction_matrices[0][0][3] = 3*a*Yg;
- interaction_matrices[0][0][4] = -3*a*Xg;
- interaction_matrices[0][0][5] = 0.;
- }
+ interaction_matrices[0][0][0] = -a * A;
+ interaction_matrices[0][0][1] = -a * B;
+ interaction_matrices[0][0][2] = (3 * a) * (A * Xg + B * Yg) + (2 * C * a);
+ interaction_matrices[0][0][3] = 3 * a * Yg;
+ interaction_matrices[0][0][4] = -3 * a * Xg;
+ interaction_matrices[0][0][5] = 0.;
+ }
}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
index bcfdb7b..1418890 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentAreaNormalized.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,156 +38,169 @@
#include <visp3/core/vpConfig.h>
#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <vector>
#include <limits>
+#include <vector>
+#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
-#include <visp3/core/vpMomentCentered.h>
-#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-
/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for the normalized surface moment. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentCentered
- vpFeatureMomentCentered
- vpMomentAreaNormalized
- vpFeatureMomentBasic
*/
-void vpFeatureMomentAreaNormalized::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_surface_normalized;
- bool found_FeatureMoment_centered;
-
- bool found_featuremoment_basic;
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
- double normalized_multiplier;
- double a;
- vpMatrix La;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2);
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
-
- normalized_multiplier = (-momentSurfaceNormalized.getDesiredDepth()/(2*a))*sqrt(momentSurfaceNormalized.getDesiredSurface()/a);
- interaction_matrices[0] = normalized_multiplier*La;
-
+void vpFeatureMomentAreaNormalized::compute_interaction()
+{
+ bool found_moment_centered;
+ bool found_moment_surface_normalized;
+ bool found_FeatureMoment_centered;
+
+ bool found_featuremoment_basic;
+ vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
+ featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
+
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
+ const vpMomentObject &momentObject = moment->getObject();
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
+ vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
+ featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
+
+ if (!found_FeatureMoment_centered)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_featuremoment_basic)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1, 6);
+ double normalized_multiplier;
+ double a;
+ vpMatrix La;
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ La = featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2);
+ } else {
+ a = momentObject.get(0, 0);
+ La = featureMomentBasic.interaction(0, 0);
+ }
+
+ normalized_multiplier =
+ (-momentSurfaceNormalized.getDesiredDepth() / (2 * a)) * sqrt(momentSurfaceNormalized.getDesiredSurface() / a);
+ interaction_matrices[0] = normalized_multiplier * La;
}
#else
-#include <vector>
#include <limits>
+#include <vector>
-#include <visp3/core/vpMomentObject.h>
#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentCentered.h>
-#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
/*!
- Computes interaction matrix for the normalized surface moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for the normalized surface moment. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentCentered
- vpMomentAreaNormalized
- vpMomentGravityCenter
*/
-void vpFeatureMomentAreaNormalized::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_surface_normalized;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
-
- if (!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if (!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if (!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- interaction_matrices.resize(1);
- interaction_matrices[0].resize(1,6);
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double An=momentSurfaceNormalized.get()[0];
-
- double Xn = An*Xg;
- double Yn = An*Yg;
-
- double Anvx,Anvy,Anvz,Anwx,Anwy;
-
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- double a = momentCentered.get(2,0)+momentCentered.get(0,2);
-
- double e01 = momentCentered.get(0,1)/a;
- double e10 = momentCentered.get(1,0)/a;
- double e11 = momentCentered.get(1,1)/a;
- double e02 = momentCentered.get(0,2)/a;
- double e20 = momentCentered.get(2,0)/a;
- double e12 = momentCentered.get(1,2)/a;
- double e21 = momentCentered.get(2,1)/a;
- double e03 = momentCentered.get(0,3)/a;
- double e30 = momentCentered.get(3,0)/a;
-
- Anvx = An*A*e20+An*B*e11;
- Anvy = An*A*e11+An*B*e02;
-
- Anwx = (n02*e01+n11*e10-e03-e21)*An-Xn*e11+(-1-e02)*Yn;
- Anwy = (e12+e30-n11*e01-n20*e10)*An+(2-e02)*Xn+Yn*e11;
-
- Anvz = -An*C+B*Anwx-A*Anwy;
-
- } else {
- Anvx = A*An/2.;
- Anvy = B*An/2.;
- Anvz = -An*C-(3./2.)*A*Xn-(3./2.)*B*Yn;
-
- Anwx = -(3./2.)*Yn;
- Anwy = (3./2.)*Xn;
- }
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Anvx;
- interaction_matrices[0][0][VY] = Anvy;
- interaction_matrices[0][0][VZ] = Anvz;
-
- interaction_matrices[0][0][WX] = Anwx;
- interaction_matrices[0][0][WY] = Anwy;
- interaction_matrices[0][0][WZ] = 0.;
+void vpFeatureMomentAreaNormalized::compute_interaction()
+{
+ bool found_moment_centered;
+ bool found_moment_surface_normalized;
+ bool found_moment_gravity;
+
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+ const vpMomentObject &momentObject = moment->getObject();
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
+
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+ interaction_matrices.resize(1);
+ interaction_matrices[0].resize(1, 6);
+ double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
+ double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
+ double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double An = momentSurfaceNormalized.get()[0];
+
+ double Xn = An * Xg;
+ double Yn = An * Yg;
+
+ double Anvx, Anvy, Anvz, Anwx, Anwy;
+
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ double a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+
+ double e01 = momentCentered.get(0, 1) / a;
+ double e10 = momentCentered.get(1, 0) / a;
+ double e11 = momentCentered.get(1, 1) / a;
+ double e02 = momentCentered.get(0, 2) / a;
+ double e20 = momentCentered.get(2, 0) / a;
+ double e12 = momentCentered.get(1, 2) / a;
+ double e21 = momentCentered.get(2, 1) / a;
+ double e03 = momentCentered.get(0, 3) / a;
+ double e30 = momentCentered.get(3, 0) / a;
+
+ Anvx = An * A * e20 + An * B * e11;
+ Anvy = An * A * e11 + An * B * e02;
+
+ Anwx = (n02 * e01 + n11 * e10 - e03 - e21) * An - Xn * e11 + (-1 - e02) * Yn;
+ Anwy = (e12 + e30 - n11 * e01 - n20 * e10) * An + (2 - e02) * Xn + Yn * e11;
+
+ Anvz = -An * C + B * Anwx - A * Anwy;
+
+ } else {
+ Anvx = A * An / 2.;
+ Anvy = B * An / 2.;
+ Anvz = -An * C - (3. / 2.) * A * Xn - (3. / 2.) * B * Yn;
+
+ Anwx = -(3. / 2.) * Yn;
+ Anwy = (3. / 2.) * Xn;
+ }
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Anvx;
+ interaction_matrices[0][0][VY] = Anvy;
+ interaction_matrices[0][0][VZ] = Anvz;
+
+ interaction_matrices[0][0][WX] = Anwx;
+ interaction_matrices[0][0][WY] = Anwy;
+ interaction_matrices[0][0][WZ] = 0.;
}
-
#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
index 37df54a..e2285d6 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentBasic.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,23 +36,23 @@
*
*****************************************************************************/
-
-
+#include <limits>
+#include <vector>
#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <vector>
-#include <limits>
/*!
Default constructor.
\param data_base : Database of moment primitives.
- \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param featureMoments : Database of features.
+ \param A_ : First plane coefficient for a plane equation of the following
+ type Ax+By+C=1/Z. \param B_ : Second plane coefficient for a plane equation
+ of the following type Ax+By+C=1/Z. \param C_ : Third plane coefficient for a
+ plane equation of the following type Ax+By+C=1/Z. \param featureMoments :
+ Database of features.
*/
-vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase& data_base,double A_, double B_, double C_,vpFeatureMomentDatabase* featureMoments) :
- vpFeatureMoment(data_base,A_,B_,C_,featureMoments), order(0)
+vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase &data_base, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments)
+ : vpFeatureMoment(data_base, A_, B_, C_, featureMoments), order(0)
{
}
@@ -59,84 +60,94 @@ vpFeatureMomentBasic::vpFeatureMomentBasic(vpMomentDatabase& data_base,double A_
Computes interaction matrix for basic moment. Called internally.
The moment primitives must be computed before calling this.
*/
-void vpFeatureMomentBasic::compute_interaction(){
- int delta;
- const vpMomentObject& m = moment->getObject();
- order = m.getOrder()+1;
- interaction_matrices.resize(order*order);
- for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end();++i)
- i->resize(1,6);
- if (m.getType()==vpMomentObject::DISCRETE){
- delta=0;
- } else {
- delta=1;
- }
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- //i=0;j=0
- interaction_matrices[0][0][VX] = -delta*A*m.get(0, 0);
- interaction_matrices[0][0][VY] = -delta*B*m.get(0, 0);
- interaction_matrices[0][0][VZ] = 3*delta*(A*m.get(1, 0)+B*m.get(0, 1)+C*m.get(0, 0))-delta*C*m.get(0, 0);
-
- interaction_matrices[0][0][WX] = 3*delta*m.get(0, 1);
- interaction_matrices[0][0][WY] = -3*delta*m.get(1, 0);
- interaction_matrices[0][0][WZ] = 0;
-
- // int i=0;
- for(int j=1;j<(int)order-1;j++){
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
-
- interaction_matrices[j_*order][0][VX] = -delta*A*m.get(0, j_);
- interaction_matrices[j_*order][0][VY] = -j*(A*m.get(1,jm1_)+B*m.get(0,j_)+C*m.get(0,jm1_))-delta*B*m.get(0,j_);
- interaction_matrices[j_*order][0][VZ] = (j+3*delta)*(A*m.get(1,j_)+B*m.get(0,jp1_)+C*m.get(0,j_))-delta*C*m.get(0,j_);
-
- interaction_matrices[j_*order][0][WX] = (j+3*delta)*m.get(0,jp1_)+j*m.get(0,jm1_);
- interaction_matrices[j_*order][0][WY] = -(j+3*delta)*m.get(1,j_);
- interaction_matrices[j_*order][0][WZ] = -j*m.get(1,jm1_);
- }
-
- //int j=0;
- for(int i=1;i<(int)order-1;i++){
- unsigned int i_ = (unsigned int) i;
+void vpFeatureMomentBasic::compute_interaction()
+{
+ int delta;
+ const vpMomentObject &m = moment->getObject();
+ order = m.getOrder() + 1;
+ interaction_matrices.resize(order * order);
+ for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
+ i->resize(1, 6);
+ if (m.getType() == vpMomentObject::DISCRETE) {
+ delta = 0;
+ } else {
+ delta = 1;
+ }
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ // i=0;j=0
+ interaction_matrices[0][0][VX] = -delta * A * m.get(0, 0);
+ interaction_matrices[0][0][VY] = -delta * B * m.get(0, 0);
+ interaction_matrices[0][0][VZ] =
+ 3 * delta * (A * m.get(1, 0) + B * m.get(0, 1) + C * m.get(0, 0)) - delta * C * m.get(0, 0);
+
+ interaction_matrices[0][0][WX] = 3 * delta * m.get(0, 1);
+ interaction_matrices[0][0][WY] = -3 * delta * m.get(1, 0);
+ interaction_matrices[0][0][WZ] = 0;
+
+ // int i=0;
+ for (int j = 1; j < (int)order - 1; j++) {
+ unsigned int j_ = (unsigned int)j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+
+ interaction_matrices[j_ * order][0][VX] = -delta * A * m.get(0, j_);
+ interaction_matrices[j_ * order][0][VY] =
+ -j * (A * m.get(1, jm1_) + B * m.get(0, j_) + C * m.get(0, jm1_)) - delta * B * m.get(0, j_);
+ interaction_matrices[j_ * order][0][VZ] =
+ (j + 3 * delta) * (A * m.get(1, j_) + B * m.get(0, jp1_) + C * m.get(0, j_)) - delta * C * m.get(0, j_);
+
+ interaction_matrices[j_ * order][0][WX] = (j + 3 * delta) * m.get(0, jp1_) + j * m.get(0, jm1_);
+ interaction_matrices[j_ * order][0][WY] = -(j + 3 * delta) * m.get(1, j_);
+ interaction_matrices[j_ * order][0][WZ] = -j * m.get(1, jm1_);
+ }
+
+ // int j=0;
+ for (int i = 1; i < (int)order - 1; i++) {
+ unsigned int i_ = (unsigned int)i;
+ unsigned int im1_ = i_ - 1;
+ unsigned int ip1_ = i_ + 1;
+
+ interaction_matrices[i_][0][VX] =
+ -i * (A * m.get(i_, 0) + B * m.get(im1_, 1) + C * m.get(im1_, 0)) - delta * A * m.get(i_, 0);
+ interaction_matrices[i_][0][VY] = -delta * B * m.get(i_, 0);
+ interaction_matrices[i_][0][VZ] =
+ (i + 3 * delta) * (A * m.get(ip1_, 0) + B * m.get(i_, 1) + C * m.get(i_, 0)) - delta * C * m.get(i_, 0);
+
+ interaction_matrices[i_][0][WX] = (i + 3 * delta) * m.get(i_, 1);
+ interaction_matrices[i_][0][WY] = -(i + 3 * delta) * m.get(ip1_, 0) - i * m.get(im1_, 0);
+ interaction_matrices[i_][0][WZ] = i * m.get(im1_, 1);
+ }
+
+ for (int j = 1; j < (int)order - 1; j++) {
+ unsigned int j_ = (unsigned int)j;
+ unsigned int jm1_ = j_ - 1;
+ unsigned int jp1_ = j_ + 1;
+
+ for (int i = 1; i < (int)order - j - 1; i++) {
+ unsigned int i_ = (unsigned int)i;
unsigned int im1_ = i_ - 1;
unsigned int ip1_ = i_ + 1;
- interaction_matrices[i_][0][VX] = -i*(A*m.get(i_, 0)+B*m.get(im1_, 1)+C*m.get(im1_, 0))-delta*A*m.get(i_, 0);
- interaction_matrices[i_][0][VY] = -delta*B*m.get(i_, 0);
- interaction_matrices[i_][0][VZ] = (i+3*delta)*(A*m.get(ip1_, 0)+B*m.get(i_, 1)+C*m.get(i_, 0))-delta*C*m.get(i_, 0);
-
- interaction_matrices[i_][0][WX] = (i+3*delta)*m.get(i_, 1);
- interaction_matrices[i_][0][WY] = -(i+3*delta)*m.get(ip1_, 0)-i*m.get(im1_, 0);
- interaction_matrices[i_][0][WZ] = i*m.get(im1_, 1);
- }
-
- for(int j=1; j<(int)order-1; j++){
- unsigned int j_ = (unsigned int) j;
- unsigned int jm1_ = j_ - 1;
- unsigned int jp1_ = j_ + 1;
-
- for(int i=1; i<(int)order-j-1; i++){
- unsigned int i_ = (unsigned int) i;
- unsigned int im1_ = i_ - 1;
- unsigned int ip1_ = i_ + 1;
-
- interaction_matrices[j_*order+i_][0][VX] = -i*(A*m.get(i_, j_)+B*m.get(im1_, jp1_)+C*m.get(im1_,j_))-delta*A*m.get(i_, j_);
- interaction_matrices[j_*order+i_][0][VY] = -j*(A*m.get(ip1_, jm1_)+B*m.get(i_, j_)+C*m.get(i_,jm1_))-delta*B*m.get(i_, j_);
- interaction_matrices[j_*order+i_][0][VZ] = (i+j+3*delta)*(A*m.get(ip1_, j_)+B*m.get(i_,jp1_)+C*m.get(i_, j_))-delta*C*m.get(i_,j_);
-
- interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*m.get(i_, jp1_)+j*m.get(i_, jm1_);
- interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*m.get(ip1_, j_)-i*m.get(im1_, j_);
- interaction_matrices[j_*order+i_][0][WZ] = i*m.get(im1_,jp1_)-j*m.get(ip1_, jm1_);
- }
+ interaction_matrices[j_ * order + i_][0][VX] =
+ -i * (A * m.get(i_, j_) + B * m.get(im1_, jp1_) + C * m.get(im1_, j_)) - delta * A * m.get(i_, j_);
+ interaction_matrices[j_ * order + i_][0][VY] =
+ -j * (A * m.get(ip1_, jm1_) + B * m.get(i_, j_) + C * m.get(i_, jm1_)) - delta * B * m.get(i_, j_);
+ interaction_matrices[j_ * order + i_][0][VZ] =
+ (i + j + 3 * delta) * (A * m.get(ip1_, j_) + B * m.get(i_, jp1_) + C * m.get(i_, j_)) -
+ delta * C * m.get(i_, j_);
+
+ interaction_matrices[j_ * order + i_][0][WX] = (i + j + 3 * delta) * m.get(i_, jp1_) + j * m.get(i_, jm1_);
+ interaction_matrices[j_ * order + i_][0][WY] = -(i + j + 3 * delta) * m.get(ip1_, j_) - i * m.get(im1_, j_);
+ interaction_matrices[j_ * order + i_][0][WZ] = i * m.get(im1_, jp1_) - j * m.get(ip1_, jm1_);
}
+ }
}
/*!
@@ -145,8 +156,11 @@ 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 {
- if(select_one+select_two>moment->getObject().getOrder())
- throw vpException(vpException::badValue,"The requested value has not been computed, you should specify a higher order.");
- return interaction_matrices[select_two*order+select_one];
+vpMatrix vpFeatureMomentBasic::interaction(unsigned int select_one, unsigned int select_two) const
+{
+ if (select_one + select_two > moment->getObject().getOrder())
+ throw vpException(vpException::badValue, "The requested value has not "
+ "been computed, you should "
+ "specify a higher order.");
+ return interaction_matrices[select_two * order + select_one];
}
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp
index 35c5471..fe56b1c 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCInvariant.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,345 +37,648 @@
*****************************************************************************/
#include <visp3/core/vpMomentObject.h>
#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
-#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentCInvariant.h>
-#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <vector>
#include <limits>
-
+#include <vector>
/*!
- Computes interaction matrix for space-scale-rotation invariants. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for space-scale-rotation invariants. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentCentered
- vpFeatureMomentCentered
- vpMomentCInvariant
- vpFeatureMomentBasic
*/
-void vpFeatureMomentCInvariant::compute_interaction(){
- std::vector<vpMatrix> LI(16);
- bool found_moment_centered;
- bool found_moment_cinvariant;
- bool found_FeatureMoment_centered;
- bool found_featuremoment_basic;
-
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- vpMatrix zeros(1,6);
- for(int i=0;i<6;i++) zeros[0][i]=0;
-
-
- LI[1]=-featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) - momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) + 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
-
-
- LI[2]=2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) * (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) + 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
-
-
- LI[3]=2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) + 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) * (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
-
-
- LI[4]=2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) * (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) + 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) * (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
-
- LI[5]=-2*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)-4*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(3,0)*featureMomentCentered.interaction(2,1)-12*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interac [...]
-
-
- LI[6]=6*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(1,2)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)-6*momentCentered.get(0,3)*momentCentered.get(1,2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+2*pow(momentCentered.get(1,2),3)*featureMomentCentered.interaction(3,0)-12*moment [...]
-
-
- LI[7] = -3*momentCentered.get(0,3)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+6*momentCentered.get(1,2)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)-2*pow(momentCentered.get(2,1),3)*featureMomentCentered.interaction(3,0)-3*momentCentered.get(0,3)*pow(momentCentered.get(2,1),2)*featureMomentCentered.interaction(3,0)+6*pow(momentCentered.get(1,2),2)*momentCentered.get(2,1)*featureMomentCentered.interaction(3,0)+3*mome [...]
-
- LI[8]=-2 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * featureMomentCentered.interaction(3, 0) + 6 * momentCentered.get(3, 0) * momentCentered.get(2, 1) * momentCentered.get(1, 2) * featureMomentCentered.interaction(2, 1) - 6 * featureMomentCentered.interaction(3, 0) * momentCentered.get(2, 1) * momentCentered.get(0, 3) * momentCentered.get(1, 2) - 6 * momentCentered.get(3, 0) * featureMomentCentered.interaction(2, 1) * momentCentered.get(0, 3) * [...]
-
- LI[9] = 4*pow(momentCentered.get(3,0),3)*featureMomentCentered.interaction(3,0)+18*momentCentered.get(1,2)*pow(momentCentered.get(3,0),2)*featureMomentCentered.interaction(3,0)+12*momentCentered.get(0,3)*momentCentered.get(2,1)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*pow(momentCentered.get(1,2),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+4*pow(momentCentered.get(0,3),2)*momentCentered.get(3,0)*featureMomentCentered.interaction(3,0)+18*m [...]
-
-
- LI[10]=featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) - 4 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) - 4 * momentCentered.get(3, 1) * featureMomentCentered.interaction(1, 3) + 6 * momentCentered.get(2, 2) * featureMomentCentered.interaction(2, 2);
-
- LI[11]=-3 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) - 3 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) - 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) - 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 6 * momentCentered.get(3, 1) * featureMomentCentered.interaction(3, 1) + 2 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 2 * momentCentered.get(3, 1) * featureMome [...]
-
-
- LI[12]=6 * momentCentered.get(4, 0) * featureMomentCentered.interaction(4, 0) + 12 * featureMomentCentered.interaction(4, 0) * momentCentered.get(2, 2) + 12 * momentCentered.get(4, 0) * featureMomentCentered.interaction(2, 2) + 2 * featureMomentCentered.interaction(4, 0) * momentCentered.get(0, 4) + 2 * momentCentered.get(4, 0) * featureMomentCentered.interaction(0, 4) + 16 * featureMomentCentered.interaction(3, 1) * momentCentered.get(1, 3) + 16 * momentCentered.get(3, 1) * featureM [...]
-
-
- LI[13]=2 * (momentCentered.get(5, 0) + 2 * momentCentered.get(3, 2) + momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) + 2 * featureMomentCentered.interaction(3, 2) + featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) + 2 * momentCentered.get(2, 3) + momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) + 2 * featureMomentCentered.interaction(2, 3) + featureMomentCentered.interaction(4, 1));
-
- LI[14]=2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) - 3 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) - 3 * featureMomentCentered.interaction(4, 1));
-
- LI[15]=2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) * (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) + 5 * featureMomentCentered.interaction(1, 4)) + 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) * (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) + 5 * featureMomentCentered.interaction(4, 1));
-
- double s3 = momentCInvariant.getS(3);
- double s2 = momentCInvariant.getS(2);
- double c3 = momentCInvariant.getC(3);
- double c2 = momentCInvariant.getC(2);
- double I1 = momentCInvariant.getII(1);
- double I2 = momentCInvariant.getII(2);
- double I3 = momentCInvariant.getII(3);
-
- vpMatrix Lc2 = featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1);
- vpMatrix Ls2 = featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2);
- vpMatrix Lc3 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) - 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
- vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1,1) * (momentCentered.get(2,0) - momentCentered.get(0,2)) + 4 * momentCentered.get(1,1) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2));
- vpMatrix LI1 = 2 * (momentCentered.get(2,0) - momentCentered.get(0,2)) * (featureMomentCentered.interaction(2,0) - featureMomentCentered.interaction(0,2)) + 8 * momentCentered.get(1,1) * featureMomentCentered.interaction(1,1);
- vpMatrix LI2 = 2 * (momentCentered.get(0,3) - 3 * momentCentered.get(2,1)) * (featureMomentCentered.interaction(0,3) - 3 * featureMomentCentered.interaction(2,1)) + 2 * (momentCentered.get(3,0) - 3 * momentCentered.get(1,2)) * (featureMomentCentered.interaction(3,0) - 3 * featureMomentCentered.interaction(1,2));
- vpMatrix LI3 = featureMomentCentered.interaction(2,0) + featureMomentCentered.interaction(0,2);
-
-
- vpMatrix La(1,6);
- double a;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
- interaction_matrices.resize(14);
-
-
- interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
- interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
-
- interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
-
- interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
-
- interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
-
- interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
-
-
- interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
-
- interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
-
- interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
-
- interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
-
-
- interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
-
- interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
- interaction_matrices[13] = (I2/(I3*I3*I3))*La+(a/(I3*I3*I3))*LI2-(3*a*I2/(I3*I3*I3*I3))*LI3;
+void vpFeatureMomentCInvariant::compute_interaction()
+{
+ std::vector<vpMatrix> LI(16);
+ bool found_moment_centered;
+ bool found_moment_cinvariant;
+ bool found_FeatureMoment_centered;
+ bool found_featuremoment_basic;
+
+ const vpMomentObject &momentObject = moment->getObject();
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
+ const vpMomentCInvariant &momentCInvariant =
+ (static_cast<const vpMomentCInvariant &>(moments.get("vpMomentCInvariant", found_moment_cinvariant)));
+ vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
+ featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
+
+ vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
+ featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
+
+ if (!found_featuremoment_basic)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
+
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_cinvariant)
+ throw vpException(vpException::notInitialized, "vpMomentCInvariant not found");
+ if (!found_FeatureMoment_centered)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
+
+ vpMatrix zeros(1, 6);
+ for (int i = 0; i < 6; i++)
+ zeros[0][i] = 0;
+
+ LI[1] = -featureMomentCentered.interaction(2, 0) * momentCentered.get(0, 2) -
+ momentCentered.get(2, 0) * featureMomentCentered.interaction(0, 2) +
+ 2 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+
+ LI[2] = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
+ (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) +
+ 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+
+ LI[3] = 2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) *
+ (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2)) +
+ 2 * (3 * momentCentered.get(2, 1) - momentCentered.get(0, 3)) *
+ (3 * featureMomentCentered.interaction(2, 1) - featureMomentCentered.interaction(0, 3));
+
+ LI[4] = 2 * (momentCentered.get(3, 0) + momentCentered.get(1, 2)) *
+ (featureMomentCentered.interaction(3, 0) + featureMomentCentered.interaction(1, 2)) +
+ 2 * (momentCentered.get(2, 1) + momentCentered.get(0, 3)) *
+ (featureMomentCentered.interaction(2, 1) + featureMomentCentered.interaction(0, 3));
+
+ LI[5] = -2 * pow(momentCentered.get(0, 3), 2) * momentCentered.get(3, 0) * featureMomentCentered.interaction(3, 0) +
+ 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(2, 1) *
+ featureMomentCentered.interaction(3, 0) -
+ 4 * pow(momentCentered.get(1, 2), 3) * featureMomentCentered.interaction(3, 0) +
+ 6 * momentCentered.get(0, 3) * momentCentered.get(1, 2) * momentCentered.get(3, 0) *
+ featureMomentCentered.interaction(2, 1) -
+ 12 * momentCentered.get(0, 3) * pow(momentCentered.get(2, 1), 2) * featureMomentCentered.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) +
+ featureMomentCentered.interaction(4, 1));
+
+ LI[14] = 2 * (momentCentered.get(5, 0) - 2 * momentCentered.get(3, 2) - 3 * momentCentered.get(1, 4)) *
+ (featureMomentCentered.interaction(5, 0) - 2 * featureMomentCentered.interaction(3, 2) -
+ 3 * featureMomentCentered.interaction(1, 4)) +
+ 2 * (momentCentered.get(0, 5) - 2 * momentCentered.get(2, 3) - 3 * momentCentered.get(4, 1)) *
+ (featureMomentCentered.interaction(0, 5) - 2 * featureMomentCentered.interaction(2, 3) -
+ 3 * featureMomentCentered.interaction(4, 1));
+
+ LI[15] = 2 * (momentCentered.get(5, 0) - 10 * momentCentered.get(3, 2) + 5 * momentCentered.get(1, 4)) *
+ (featureMomentCentered.interaction(5, 0) - 10 * featureMomentCentered.interaction(3, 2) +
+ 5 * featureMomentCentered.interaction(1, 4)) +
+ 2 * (momentCentered.get(0, 5) - 10 * momentCentered.get(2, 3) + 5 * momentCentered.get(4, 1)) *
+ (featureMomentCentered.interaction(0, 5) - 10 * featureMomentCentered.interaction(2, 3) +
+ 5 * featureMomentCentered.interaction(4, 1));
+
+ double s3 = momentCInvariant.getS(3);
+ double s2 = momentCInvariant.getS(2);
+ double c3 = momentCInvariant.getC(3);
+ double c2 = momentCInvariant.getC(2);
+ double I1 = momentCInvariant.getII(1);
+ double I2 = momentCInvariant.getII(2);
+ double I3 = momentCInvariant.getII(3);
+
+ vpMatrix Lc2 = featureMomentCentered.interaction(0, 3) - 3 * featureMomentCentered.interaction(2, 1);
+ vpMatrix Ls2 = featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2);
+ vpMatrix Lc3 = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
+ (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) -
+ 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+ vpMatrix Ls3 = 4 * featureMomentCentered.interaction(1, 1) * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) +
+ 4 * momentCentered.get(1, 1) *
+ (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2));
+ vpMatrix LI1 = 2 * (momentCentered.get(2, 0) - momentCentered.get(0, 2)) *
+ (featureMomentCentered.interaction(2, 0) - featureMomentCentered.interaction(0, 2)) +
+ 8 * momentCentered.get(1, 1) * featureMomentCentered.interaction(1, 1);
+ vpMatrix LI2 = 2 * (momentCentered.get(0, 3) - 3 * momentCentered.get(2, 1)) *
+ (featureMomentCentered.interaction(0, 3) - 3 * featureMomentCentered.interaction(2, 1)) +
+ 2 * (momentCentered.get(3, 0) - 3 * momentCentered.get(1, 2)) *
+ (featureMomentCentered.interaction(3, 0) - 3 * featureMomentCentered.interaction(1, 2));
+ vpMatrix LI3 = featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2);
+
+ vpMatrix La(1, 6);
+ double a;
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ La = (featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2));
+ } else {
+ a = momentObject.get(0, 0);
+ La = featureMomentBasic.interaction(0, 0);
+ }
+ interaction_matrices.resize(14);
+
+ interaction_matrices[0] = (1. / (momentCInvariant.getI(2) * momentCInvariant.getI(2))) *
+ (momentCInvariant.getI(2) * LI[1] - momentCInvariant.getI(1) * LI[2]);
+ interaction_matrices[1] = (1. / (momentCInvariant.getI(4) * momentCInvariant.getI(4))) *
+ (momentCInvariant.getI(4) * LI[3] - momentCInvariant.getI(3) * LI[4]);
+
+ interaction_matrices[2] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[5] - momentCInvariant.getI(5) * LI[6]);
+
+ interaction_matrices[3] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[7] - momentCInvariant.getI(7) * LI[6]);
+
+ interaction_matrices[4] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[8] - momentCInvariant.getI(8) * LI[6]);
+
+ interaction_matrices[5] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[9] - momentCInvariant.getI(9) * LI[6]);
+
+ interaction_matrices[6] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
+ (momentCInvariant.getI(10) * LI[11] - momentCInvariant.getI(11) * LI[10]);
+
+ interaction_matrices[7] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
+ (momentCInvariant.getI(10) * LI[12] - momentCInvariant.getI(12) * LI[10]);
+
+ interaction_matrices[8] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
+ (momentCInvariant.getI(15) * LI[13] - momentCInvariant.getI(13) * LI[15]);
+
+ interaction_matrices[9] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
+ (momentCInvariant.getI(15) * LI[14] - momentCInvariant.getI(14) * LI[15]);
+
+ interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
+ (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
+ (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
+ 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
+ (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
+ (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
+ 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[12] = (1 / (I3 * I3)) * LI1 - (2 * I1 / (I3 * I3 * I3)) * LI3;
+ interaction_matrices[13] =
+ (I2 / (I3 * I3 * I3)) * La + (a / (I3 * I3 * I3)) * LI2 - (3 * a * I2 / (I3 * I3 * I3 * I3)) * LI3;
}
#else
-#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
-#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentCInvariant.h>
-#include <visp3/visual_features/vpFeatureMomentCentered.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <vector>
+#include <cmath>
#include <iostream>
#include <limits>
-#include <cmath>
+#include <vector>
/*!
- Computes interaction matrix for space-scale-rotation invariants. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for space-scale-rotation invariants. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentCentered
- vpFeatureMomentCentered
- vpMomentCInvariant
- vpFeatureMomentBasic
*/
-void vpFeatureMomentCInvariant::compute_interaction(){
-
- //std::vector<vpMatrix> LI(16);
- LI.resize(16); // LI made class member
-
- bool found_moment_centered;
- bool found_moment_cinvariant;
- bool found_FeatureMoment_centered;
- bool found_featuremoment_basic;
-
- const vpMomentObject& momentObject = moment->getObject();
- const vpMomentCentered& momentCentered = (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentCInvariant& momentCInvariant = (static_cast<const vpMomentCInvariant&>(moments.get("vpMomentCInvariant",found_moment_cinvariant)));
-
- vpFeatureMomentCentered& featureMomentCentered = (static_cast<vpFeatureMomentCentered&>(featureMomentsDataBase->get("vpFeatureMomentCentered",found_FeatureMoment_centered)));
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_cinvariant) throw vpException(vpException::notInitialized,"vpMomentCInvariant not found");
- if(!found_FeatureMoment_centered) throw vpException(vpException::notInitialized,"vpFeatureMomentCentered not found");
-
- vpMatrix zeros(1,6);
- for(int i=0;i<6;i++) zeros[0][i]=0;
-
- double mu30 = momentCentered.get(3,0);
- double mu30_2 = mu30*mu30;
- double mu30_3 = mu30_2*mu30;
-
- double mu03 = momentCentered.get(0,3);
- double mu03_2 = mu03*mu03;
- double mu03_3 = mu03*mu03_2;
-
- double mu20 = momentCentered.get(2,0);
- double mu02 = momentCentered.get(0,2);
- double mu50 = momentCentered.get(5,0);
- double mu32 = momentCentered.get(3,2);
- double mu14 = momentCentered.get(1,4);
- double mu05 = momentCentered.get(0,5);
- double mu23 = momentCentered.get(2,3);
- double mu41 = momentCentered.get(4,1);
- double mu40 = momentCentered.get(4,0);
- double mu04 = momentCentered.get(0,4);
- double mu31 = momentCentered.get(3,1);
- double mu13 = momentCentered.get(1,3);
- double mu22 = momentCentered.get(2,2);
- double mu21 = momentCentered.get(2,1);
- double mu12 = momentCentered.get(1,2);
- double mu11 = momentCentered.get(1,1);
-
- double mu12_2 = mu12*mu12;
- double mu21_2 = mu21*mu21;
- double mu21_3 = mu21*mu21_2;
- double mu12_3 = mu12_2*mu12;
-
- vpMatrix Lmu20 = featureMomentCentered.interaction(2,0);
- vpMatrix Lmu02 = featureMomentCentered.interaction(0,2);
- vpMatrix Lmu11 = featureMomentCentered.interaction(1,1);
- vpMatrix Lmu12 = featureMomentCentered.interaction(1,2);
- vpMatrix Lmu03 = featureMomentCentered.interaction(0,3);
- vpMatrix Lmu21 = featureMomentCentered.interaction(2,1);
- vpMatrix Lmu30 = featureMomentCentered.interaction(3,0);
- vpMatrix Lmu40 = featureMomentCentered.interaction(4,0);
- vpMatrix Lmu04 = featureMomentCentered.interaction(0,4);
- vpMatrix Lmu31 = featureMomentCentered.interaction(3,1);
- vpMatrix Lmu13 = featureMomentCentered.interaction(1,3);
- vpMatrix Lmu22 = featureMomentCentered.interaction(2,2);
- vpMatrix Lmu05 = featureMomentCentered.interaction(0,5);
- vpMatrix Lmu14 = featureMomentCentered.interaction(1,4);
- vpMatrix Lmu23 = featureMomentCentered.interaction(2,3);
- vpMatrix Lmu32 = featureMomentCentered.interaction(3,2);
- vpMatrix Lmu41 = featureMomentCentered.interaction(4,1);
- vpMatrix Lmu50 = featureMomentCentered.interaction(5,0);
-
- LI[1]= -Lmu20*mu02-mu20*Lmu02+2*mu11*Lmu11;
-
- LI[2]= (-2*mu20+2*mu02)*Lmu02+8*mu11*Lmu11+(2*mu20-2*mu02)*Lmu20;
-
- LI[3]= (-6*mu21+2*mu03)*Lmu03+(-6*mu30+18*mu12)*Lmu12+(18*mu21-6*mu03)*Lmu21+(2*mu30-6*mu12)*Lmu30;
-
- LI[4]= (2*mu21+2*mu03)*Lmu03+(2*mu30+2*mu12)*Lmu12+(2*mu21+2*mu03)*Lmu21+(2*mu30+2*mu12)*Lmu30;
-
- LI[5]= (-2*mu30_2*mu03+6*mu30*mu21*mu12-4*mu21_3)*Lmu03+(6*mu30*mu21*mu03-12*mu30*mu12_2+6*mu21_2*mu12)*Lmu12+(6*mu30*mu12*mu03-12*mu21_2*mu03+6*mu21*mu12_2)*Lmu21+(-2*mu30*mu03_2-4*mu12_3+6*mu21*mu12*mu03)*Lmu30;
-
- LI[6]= (-6*mu30*mu21*mu12-6*mu21*mu12_2+6*mu21_2*mu03+2*mu21_3+4*mu30_2*mu03)*Lmu03+(-6*mu30*mu21_2-6*mu30*mu21*mu03+12*mu12_3+6*mu30_2*mu12-12*mu21*mu12*mu03+6*mu30*mu12_2)*Lmu12+(6*mu21*mu03_2+6*mu21_2*mu03-6*mu30*mu12*mu03+12*mu21_3-12*mu30*mu21*mu12-6*mu12_2*mu03)*Lmu21+(6*mu30*mu12_2+2*mu12_3+4*mu30*mu03_2-6*mu21_2*mu12-6*mu21*mu12*mu03)*Lmu30;
-
- LI[7] = (-6*mu21_2*mu12+3*mu30*mu03_2-mu30_3-3*mu30*mu21_2-6*mu21*mu12*mu03+3*mu30*mu12_2+2*mu12_3)*Lmu03+(-3*mu21*mu03_2+12*mu30*mu21*mu12+6*mu30*mu12*mu03+3*mu30_2*mu21+9*mu21*mu12_2-6*mu21_2*mu03-3*mu21_3+6*mu12_2*mu03)*Lmu12+(3*mu30_2*mu12-9*mu21_2*mu12-12*mu21*mu12*mu03-6*mu30*mu21*mu03-6*mu30*mu21_2+6*mu30*mu12_2+3*mu12_3-3*mu12*mu03_2)*Lmu21+(6*mu21*mu12_2+6*mu30*mu21*mu12-3*mu30_2*mu03+3*mu12_2*mu03-3*mu21_2*mu03-2*mu21_3+mu03_3)*Lmu30;
-
- LI[8]= (6*mu21_3-2*mu30*mu12*mu03+2*mu12_2*mu03+3*mu21*mu12_2-6*mu30*mu21*mu12-mu30_2*mu21-4*mu21_2*mu03-3*mu21*mu03_2)*Lmu03+(2*mu12*mu03_2-4*mu30_2*mu12+9*mu30*mu12_2-mu30*mu03_2-6*mu30*mu21*mu03+3*mu30*mu21_2+6*mu21*mu12*mu03-mu30_3)*Lmu12+(18*mu21_2*mu03+6*mu30*mu21*mu12-4*mu21*mu03_2-mu03_3-mu30_2*mu03-6*mu30*mu12*mu03+3*mu12_2*mu03+2*mu30_2*mu21)*Lmu21+(-6*mu21*mu12*mu03-4*mu30*mu12_2-2*mu30*mu21*mu03+2*mu30*mu21_2+3*mu12_3+3*mu21_2*mu12-3*mu30_2*mu12-mu12*mu03_2)*Lmu30;
-
- LI[9] = (2*(2*mu03+3*mu21))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu03+6*mu30*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu12+6*mu03*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu21+(2*(2*mu30+3*mu12))*(3*mu03*mu21+3*mu30*mu12+mu30_2+mu03_2)*Lmu30;
-
- LI[10]= Lmu40*mu04+mu40*Lmu04-4*Lmu31*mu13-4*mu31*Lmu13+6*mu22*Lmu22;
-
- LI[11]= (-2*mu40-3*mu22)*Lmu04+(2*mu31+6*mu13)*Lmu13+(-3*mu04-3*mu40)*Lmu22+(2*mu13+6*mu31)*Lmu31+(-3*mu22-2*mu04)*Lmu40;
-
- LI[12]= (2*mu40+12*mu22+6*mu04)*Lmu04+16*mu31*Lmu13+(12*mu40+12*mu04)*Lmu22+16*Lmu31*mu13+(6*mu40+12*mu22+2*mu04)*Lmu40;
-
- LI[13]=(2*mu05+4*mu23+2*mu41)*Lmu05+(2*mu50+4*mu32+2*mu14)*Lmu14+(4*mu05+8*mu23+4*mu41)*Lmu23+(4*mu50+8*mu32+4*mu14)*Lmu32+(2*mu05+4*mu23+2*mu41)*Lmu41+(2*mu50+4*mu32+2*mu14)*Lmu50 ;
-
- LI[14]= (2*mu05-4*mu23-6*mu41)*Lmu05+(-6*mu50+12*mu32+18*mu14)*Lmu14+(-4*mu05+8*mu23+12*mu41)*Lmu23+(-4*mu50+8*mu32+12*mu14)*Lmu32+(-6*mu05+12*mu23+18*mu41)*Lmu41+(2*mu50-4*mu32-6*mu14)*Lmu50;
-
- LI[15]= (2*mu05-20*mu23+10*mu41)*Lmu05+(10*mu50-100*mu32+50*mu14)*Lmu14+(-20*mu05+200*mu23-100*mu41)*Lmu23+(-20*mu50+200*mu32-100*mu14)*Lmu32+(10*mu05-100*mu23+50*mu41)*Lmu41+(2*mu50-20*mu32+10*mu14)*Lmu50;
-
- double s3 = momentCInvariant.getS(3);
- double s2 = momentCInvariant.getS(2);
- double c3 = momentCInvariant.getC(3);
- double c2 = momentCInvariant.getC(2);
-
- double I1 = momentCInvariant.getII(1);
- double I2 = momentCInvariant.getII(2);
- double I3 = momentCInvariant.getII(3);
-
- vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
- double mu20__mu02 = mu20 - mu02;
- vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
- vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
- vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02) - 8. * mu11 * Lmu11;
- vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
- vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
- vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
- vpMatrix LI3 = Lmu20 + Lmu02;
-
- vpMatrix La(1,6);
- double a;
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- a = momentCentered.get(2,0)+momentCentered.get(0,2);
- La = (featureMomentCentered.interaction(2,0)+featureMomentCentered.interaction(0,2));
- }else{
- a = momentObject.get(0,0);
- La = featureMomentBasic.interaction(0,0);
- }
-
- interaction_matrices.resize(14);
-
- /*
- momentCInvariant.printInvariants(std::cout);
- printLsofInvariants(std::cout);
- */
-
- interaction_matrices[0] = (1./(momentCInvariant.getI(2)*momentCInvariant.getI(2)))*(momentCInvariant.getI(2)*LI[1]-momentCInvariant.getI(1)*LI[2]);
-
- interaction_matrices[1] = (1./(momentCInvariant.getI(4)*momentCInvariant.getI(4)))*(momentCInvariant.getI(4)*LI[3]-momentCInvariant.getI(3)*LI[4]);
-
- interaction_matrices[2] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[5]-momentCInvariant.getI(5)*LI[6]);
-
- interaction_matrices[3] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[7]-momentCInvariant.getI(7)*LI[6]);
-
- interaction_matrices[4] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[8]-momentCInvariant.getI(8)*LI[6]);
-
- interaction_matrices[5] = (1./(momentCInvariant.getI(6)*momentCInvariant.getI(6)))*(momentCInvariant.getI(6)*LI[9]-momentCInvariant.getI(9)*LI[6]);
-
- interaction_matrices[6] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[11]-momentCInvariant.getI(11)*LI[10]);
-
- interaction_matrices[7] = (1./(momentCInvariant.getI(10)*momentCInvariant.getI(10)))*(momentCInvariant.getI(10)*LI[12]-momentCInvariant.getI(12)*LI[10]);
-
- interaction_matrices[8] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[13]-momentCInvariant.getI(13)*LI[15]);
-
- interaction_matrices[9] = (1./(momentCInvariant.getI(15)*momentCInvariant.getI(15)))*(momentCInvariant.getI(15)*LI[14]-momentCInvariant.getI(14)*LI[15]);
-
- interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) + (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 - (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 - 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+void vpFeatureMomentCInvariant::compute_interaction()
+{
- 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;
+ // std::vector<vpMatrix> LI(16);
+ LI.resize(16); // LI made class member
+
+ bool found_moment_centered;
+ bool found_moment_cinvariant;
+ bool found_FeatureMoment_centered;
+ bool found_featuremoment_basic;
+
+ const vpMomentObject &momentObject = moment->getObject();
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
+ const vpMomentCInvariant &momentCInvariant =
+ (static_cast<const vpMomentCInvariant &>(moments.get("vpMomentCInvariant", found_moment_cinvariant)));
+
+ vpFeatureMomentCentered &featureMomentCentered = (static_cast<vpFeatureMomentCentered &>(
+ featureMomentsDataBase->get("vpFeatureMomentCentered", found_FeatureMoment_centered)));
+
+ vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
+ featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
+
+ if (!found_featuremoment_basic)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_cinvariant)
+ throw vpException(vpException::notInitialized, "vpMomentCInvariant not found");
+ if (!found_FeatureMoment_centered)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentCentered not found");
+
+ vpMatrix zeros(1, 6);
+ for (int i = 0; i < 6; i++)
+ zeros[0][i] = 0;
+
+ double mu30 = momentCentered.get(3, 0);
+ double mu30_2 = mu30 * mu30;
+ double mu30_3 = mu30_2 * mu30;
+
+ double mu03 = momentCentered.get(0, 3);
+ double mu03_2 = mu03 * mu03;
+ double mu03_3 = mu03 * mu03_2;
+
+ double mu20 = momentCentered.get(2, 0);
+ double mu02 = momentCentered.get(0, 2);
+ double mu50 = momentCentered.get(5, 0);
+ double mu32 = momentCentered.get(3, 2);
+ double mu14 = momentCentered.get(1, 4);
+ double mu05 = momentCentered.get(0, 5);
+ double mu23 = momentCentered.get(2, 3);
+ double mu41 = momentCentered.get(4, 1);
+ double mu40 = momentCentered.get(4, 0);
+ double mu04 = momentCentered.get(0, 4);
+ double mu31 = momentCentered.get(3, 1);
+ double mu13 = momentCentered.get(1, 3);
+ double mu22 = momentCentered.get(2, 2);
+ double mu21 = momentCentered.get(2, 1);
+ double mu12 = momentCentered.get(1, 2);
+ double mu11 = momentCentered.get(1, 1);
+
+ double mu12_2 = mu12 * mu12;
+ double mu21_2 = mu21 * mu21;
+ double mu21_3 = mu21 * mu21_2;
+ double mu12_3 = mu12_2 * mu12;
+
+ vpMatrix Lmu20 = featureMomentCentered.interaction(2, 0);
+ vpMatrix Lmu02 = featureMomentCentered.interaction(0, 2);
+ vpMatrix Lmu11 = featureMomentCentered.interaction(1, 1);
+ vpMatrix Lmu12 = featureMomentCentered.interaction(1, 2);
+ vpMatrix Lmu03 = featureMomentCentered.interaction(0, 3);
+ vpMatrix Lmu21 = featureMomentCentered.interaction(2, 1);
+ vpMatrix Lmu30 = featureMomentCentered.interaction(3, 0);
+ vpMatrix Lmu40 = featureMomentCentered.interaction(4, 0);
+ vpMatrix Lmu04 = featureMomentCentered.interaction(0, 4);
+ vpMatrix Lmu31 = featureMomentCentered.interaction(3, 1);
+ vpMatrix Lmu13 = featureMomentCentered.interaction(1, 3);
+ vpMatrix Lmu22 = featureMomentCentered.interaction(2, 2);
+ vpMatrix Lmu05 = featureMomentCentered.interaction(0, 5);
+ vpMatrix Lmu14 = featureMomentCentered.interaction(1, 4);
+ vpMatrix Lmu23 = featureMomentCentered.interaction(2, 3);
+ vpMatrix Lmu32 = featureMomentCentered.interaction(3, 2);
+ vpMatrix Lmu41 = featureMomentCentered.interaction(4, 1);
+ vpMatrix Lmu50 = featureMomentCentered.interaction(5, 0);
+
+ LI[1] = -Lmu20 * mu02 - mu20 * Lmu02 + 2 * mu11 * Lmu11;
+
+ LI[2] = (-2 * mu20 + 2 * mu02) * Lmu02 + 8 * mu11 * Lmu11 + (2 * mu20 - 2 * mu02) * Lmu20;
+
+ LI[3] = (-6 * mu21 + 2 * mu03) * Lmu03 + (-6 * mu30 + 18 * mu12) * Lmu12 + (18 * mu21 - 6 * mu03) * Lmu21 +
+ (2 * mu30 - 6 * mu12) * Lmu30;
+
+ LI[4] = (2 * mu21 + 2 * mu03) * Lmu03 + (2 * mu30 + 2 * mu12) * Lmu12 + (2 * mu21 + 2 * mu03) * Lmu21 +
+ (2 * mu30 + 2 * mu12) * Lmu30;
+
+ LI[5] = (-2 * mu30_2 * mu03 + 6 * mu30 * mu21 * mu12 - 4 * mu21_3) * Lmu03 +
+ (6 * mu30 * mu21 * mu03 - 12 * mu30 * mu12_2 + 6 * mu21_2 * mu12) * Lmu12 +
+ (6 * mu30 * mu12 * mu03 - 12 * mu21_2 * mu03 + 6 * mu21 * mu12_2) * Lmu21 +
+ (-2 * mu30 * mu03_2 - 4 * mu12_3 + 6 * mu21 * mu12 * mu03) * Lmu30;
+
+ LI[6] = (-6 * mu30 * mu21 * mu12 - 6 * mu21 * mu12_2 + 6 * mu21_2 * mu03 + 2 * mu21_3 + 4 * mu30_2 * mu03) * Lmu03 +
+ (-6 * mu30 * mu21_2 - 6 * mu30 * mu21 * mu03 + 12 * mu12_3 + 6 * mu30_2 * mu12 - 12 * mu21 * mu12 * mu03 +
+ 6 * mu30 * mu12_2) *
+ Lmu12 +
+ (6 * mu21 * mu03_2 + 6 * mu21_2 * mu03 - 6 * mu30 * mu12 * mu03 + 12 * mu21_3 - 12 * mu30 * mu21 * mu12 -
+ 6 * mu12_2 * mu03) *
+ Lmu21 +
+ (6 * mu30 * mu12_2 + 2 * mu12_3 + 4 * mu30 * mu03_2 - 6 * mu21_2 * mu12 - 6 * mu21 * mu12 * mu03) * Lmu30;
+
+ LI[7] = (-6 * mu21_2 * mu12 + 3 * mu30 * mu03_2 - mu30_3 - 3 * mu30 * mu21_2 - 6 * mu21 * mu12 * mu03 +
+ 3 * mu30 * mu12_2 + 2 * mu12_3) *
+ Lmu03 +
+ (-3 * mu21 * mu03_2 + 12 * mu30 * mu21 * mu12 + 6 * mu30 * mu12 * mu03 + 3 * mu30_2 * mu21 +
+ 9 * mu21 * mu12_2 - 6 * mu21_2 * mu03 - 3 * mu21_3 + 6 * mu12_2 * mu03) *
+ Lmu12 +
+ (3 * mu30_2 * mu12 - 9 * mu21_2 * mu12 - 12 * mu21 * mu12 * mu03 - 6 * mu30 * mu21 * mu03 -
+ 6 * mu30 * mu21_2 + 6 * mu30 * mu12_2 + 3 * mu12_3 - 3 * mu12 * mu03_2) *
+ Lmu21 +
+ (6 * mu21 * mu12_2 + 6 * mu30 * mu21 * mu12 - 3 * mu30_2 * mu03 + 3 * mu12_2 * mu03 - 3 * mu21_2 * mu03 -
+ 2 * mu21_3 + mu03_3) *
+ Lmu30;
+
+ LI[8] = (6 * mu21_3 - 2 * mu30 * mu12 * mu03 + 2 * mu12_2 * mu03 + 3 * mu21 * mu12_2 - 6 * mu30 * mu21 * mu12 -
+ mu30_2 * mu21 - 4 * mu21_2 * mu03 - 3 * mu21 * mu03_2) *
+ Lmu03 +
+ (2 * mu12 * mu03_2 - 4 * mu30_2 * mu12 + 9 * mu30 * mu12_2 - mu30 * mu03_2 - 6 * mu30 * mu21 * mu03 +
+ 3 * mu30 * mu21_2 + 6 * mu21 * mu12 * mu03 - mu30_3) *
+ Lmu12 +
+ (18 * mu21_2 * mu03 + 6 * mu30 * mu21 * mu12 - 4 * mu21 * mu03_2 - mu03_3 - mu30_2 * mu03 -
+ 6 * mu30 * mu12 * mu03 + 3 * mu12_2 * mu03 + 2 * mu30_2 * mu21) *
+ Lmu21 +
+ (-6 * mu21 * mu12 * mu03 - 4 * mu30 * mu12_2 - 2 * mu30 * mu21 * mu03 + 2 * mu30 * mu21_2 + 3 * mu12_3 +
+ 3 * mu21_2 * mu12 - 3 * mu30_2 * mu12 - mu12 * mu03_2) *
+ Lmu30;
+
+ LI[9] = (2 * (2 * mu03 + 3 * mu21)) * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu03 +
+ 6 * mu30 * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu12 +
+ 6 * mu03 * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu21 +
+ (2 * (2 * mu30 + 3 * mu12)) * (3 * mu03 * mu21 + 3 * mu30 * mu12 + mu30_2 + mu03_2) * Lmu30;
+
+ LI[10] = Lmu40 * mu04 + mu40 * Lmu04 - 4 * Lmu31 * mu13 - 4 * mu31 * Lmu13 + 6 * mu22 * Lmu22;
+
+ LI[11] = (-2 * mu40 - 3 * mu22) * Lmu04 + (2 * mu31 + 6 * mu13) * Lmu13 + (-3 * mu04 - 3 * mu40) * Lmu22 +
+ (2 * mu13 + 6 * mu31) * Lmu31 + (-3 * mu22 - 2 * mu04) * Lmu40;
+
+ LI[12] = (2 * mu40 + 12 * mu22 + 6 * mu04) * Lmu04 + 16 * mu31 * Lmu13 + (12 * mu40 + 12 * mu04) * Lmu22 +
+ 16 * Lmu31 * mu13 + (6 * mu40 + 12 * mu22 + 2 * mu04) * Lmu40;
+
+ LI[13] = (2 * mu05 + 4 * mu23 + 2 * mu41) * Lmu05 + (2 * mu50 + 4 * mu32 + 2 * mu14) * Lmu14 +
+ (4 * mu05 + 8 * mu23 + 4 * mu41) * Lmu23 + (4 * mu50 + 8 * mu32 + 4 * mu14) * Lmu32 +
+ (2 * mu05 + 4 * mu23 + 2 * mu41) * Lmu41 + (2 * mu50 + 4 * mu32 + 2 * mu14) * Lmu50;
+
+ LI[14] = (2 * mu05 - 4 * mu23 - 6 * mu41) * Lmu05 + (-6 * mu50 + 12 * mu32 + 18 * mu14) * Lmu14 +
+ (-4 * mu05 + 8 * mu23 + 12 * mu41) * Lmu23 + (-4 * mu50 + 8 * mu32 + 12 * mu14) * Lmu32 +
+ (-6 * mu05 + 12 * mu23 + 18 * mu41) * Lmu41 + (2 * mu50 - 4 * mu32 - 6 * mu14) * Lmu50;
+
+ LI[15] = (2 * mu05 - 20 * mu23 + 10 * mu41) * Lmu05 + (10 * mu50 - 100 * mu32 + 50 * mu14) * Lmu14 +
+ (-20 * mu05 + 200 * mu23 - 100 * mu41) * Lmu23 + (-20 * mu50 + 200 * mu32 - 100 * mu14) * Lmu32 +
+ (10 * mu05 - 100 * mu23 + 50 * mu41) * Lmu41 + (2 * mu50 - 20 * mu32 + 10 * mu14) * Lmu50;
+
+ double s3 = momentCInvariant.getS(3);
+ double s2 = momentCInvariant.getS(2);
+ double c3 = momentCInvariant.getC(3);
+ double c2 = momentCInvariant.getC(2);
+
+ double I1 = momentCInvariant.getII(1);
+ double I2 = momentCInvariant.getII(2);
+ double I3 = momentCInvariant.getII(3);
+
+ vpMatrix Lmu20__Lmu02 = Lmu20 - Lmu02;
+ double mu20__mu02 = mu20 - mu02;
+ vpMatrix Lc2 = Lmu03 - 3 * Lmu21;
+ vpMatrix Ls2 = Lmu30 - 3 * Lmu12;
+ vpMatrix Lc3 = 2 * (mu20__mu02) * (Lmu20__Lmu02)-8. * mu11 * Lmu11;
+ vpMatrix Ls3 = 4 * Lmu11 * (mu20__mu02) + 4 * mu11 * (Lmu20__Lmu02);
+ vpMatrix LI1 = 2 * (mu20__mu02) * (Lmu20__Lmu02) + 8 * mu11 * Lmu11;
+ vpMatrix LI2 = 2 * (mu03 - 3 * mu21) * (Lc2) + 2 * (mu30 - 3 * mu12) * (Ls2);
+ vpMatrix LI3 = Lmu20 + Lmu02;
+
+ vpMatrix La(1, 6);
+ double a;
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+ La = (featureMomentCentered.interaction(2, 0) + featureMomentCentered.interaction(0, 2));
+ } else {
+ a = momentObject.get(0, 0);
+ La = featureMomentBasic.interaction(0, 0);
+ }
+
+ interaction_matrices.resize(14);
+
+ /*
+ momentCInvariant.printInvariants(std::cout);
+ printLsofInvariants(std::cout);
+ */
+
+ interaction_matrices[0] = (1. / (momentCInvariant.getI(2) * momentCInvariant.getI(2))) *
+ (momentCInvariant.getI(2) * LI[1] - momentCInvariant.getI(1) * LI[2]);
+
+ interaction_matrices[1] = (1. / (momentCInvariant.getI(4) * momentCInvariant.getI(4))) *
+ (momentCInvariant.getI(4) * LI[3] - momentCInvariant.getI(3) * LI[4]);
+
+ interaction_matrices[2] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[5] - momentCInvariant.getI(5) * LI[6]);
+
+ interaction_matrices[3] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[7] - momentCInvariant.getI(7) * LI[6]);
+
+ interaction_matrices[4] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[8] - momentCInvariant.getI(8) * LI[6]);
+
+ interaction_matrices[5] = (1. / (momentCInvariant.getI(6) * momentCInvariant.getI(6))) *
+ (momentCInvariant.getI(6) * LI[9] - momentCInvariant.getI(9) * LI[6]);
+
+ interaction_matrices[6] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
+ (momentCInvariant.getI(10) * LI[11] - momentCInvariant.getI(11) * LI[10]);
+
+ interaction_matrices[7] = (1. / (momentCInvariant.getI(10) * momentCInvariant.getI(10))) *
+ (momentCInvariant.getI(10) * LI[12] - momentCInvariant.getI(12) * LI[10]);
+
+ interaction_matrices[8] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
+ (momentCInvariant.getI(15) * LI[13] - momentCInvariant.getI(13) * LI[15]);
+
+ interaction_matrices[9] = (1. / (momentCInvariant.getI(15) * momentCInvariant.getI(15))) *
+ (momentCInvariant.getI(15) * LI[14] - momentCInvariant.getI(14) * LI[15]);
+
+ interaction_matrices[10] = (Lc2 * c3 + c2 * Lc3 + Ls2 * s3 + s2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
+ (c2 * c3 + s2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
+ (c2 * c3 + s2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
+ 0.3e1 / 0.2e1 * (c2 * c3 + s2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
+
+ interaction_matrices[11] = (Ls2 * c3 + s2 * Lc3 - Lc2 * s3 - c2 * Ls3) * sqrt(a) / I1 * pow(I3, -0.3e1 / 0.2e1) +
+ (s2 * c3 - c2 * s3) * pow(a, -0.1e1 / 0.2e1) / I1 * pow(I3, -0.3e1 / 0.2e1) * La / 0.2e1 -
+ (s2 * c3 - c2 * s3) * sqrt(a) * pow(I1, -0.2e1) * pow(I3, -0.3e1 / 0.2e1) * LI1 -
+ 0.3e1 / 0.2e1 * (s2 * c3 - c2 * s3) * sqrt(a) / I1 * pow(I3, -0.5e1 / 0.2e1) * LI3;
- interaction_matrices[12] = (1/(I3*I3))*LI1-(2*I1/(I3*I3*I3))*LI3;
+ interaction_matrices[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[13] =
+ (I2 / (I3 * I3 * I3)) * La + (a / (I3 * I3 * I3)) * LI2 - (3 * a * I2 / (I3 * I3 * I3 * I3)) * LI3;
- /*
- std::cout << (*this);
- vpTRACE("Done.");
- std::exit(-1);
- */
+ /*
+ std::cout << (*this);
+ vpTRACE("Done.");
+ std::exit(-1);
+ */
}
/*!
@@ -382,43 +686,43 @@ void vpFeatureMomentCInvariant::compute_interaction(){
There are 15 of them, as in [Point-based and region based.ITRO05]
\cite Tahri05z
*/
-void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream& os) const{
- for (unsigned int i = 1; i < 15; ++i){
- os << "LI[" << i << "] = ";
- LI[i].matlabPrint(os);
- os << std::endl;
- }
+void vpFeatureMomentCInvariant::printLsofInvariants(std::ostream &os) const
+{
+ for (unsigned int i = 1; i < 15; ++i) {
+ os << "LI[" << i << "] = ";
+ LI[i].matlabPrint(os);
+ os << std::endl;
+ }
}
/*!
\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){
- os << "L_c[" << i << "] = ";
- featcinv.interaction_matrices[i].matlabPrint(os);
- os << std::endl;
- }
-
- // sx, sy
- os << "L_sx = ";
- featcinv.interaction_matrices[10].matlabPrint(os);
- os << std::endl;
- os << "L_sy = ";
- featcinv.interaction_matrices[11].matlabPrint(os);
- os << std::endl;
- // Px,Py
- os << "L_Px = ";
- featcinv.interaction_matrices[12].matlabPrint(os);
+ // Print L for c1 .. c10
+ for (unsigned int i = 0; i < 10; ++i) {
+ os << "L_c[" << i << "] = ";
+ featcinv.interaction_matrices[i].matlabPrint(os);
os << std::endl;
- os << "L_Py = ";
- featcinv.interaction_matrices[13].matlabPrint(os);
- os << std::endl;
-
- return os;
-
+ }
+
+ // sx, sy
+ os << "L_sx = ";
+ featcinv.interaction_matrices[10].matlabPrint(os);
+ os << std::endl;
+ os << "L_sy = ";
+ featcinv.interaction_matrices[11].matlabPrint(os);
+ os << std::endl;
+ // Px,Py
+ os << "L_Px = ";
+ featcinv.interaction_matrices[12].matlabPrint(os);
+ os << std::endl;
+ os << "L_Py = ";
+ featcinv.interaction_matrices[13].matlabPrint(os);
+ os << std::endl;
+
+ return os;
}
#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp
index a8ddc82..5cb899e 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCentered.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,8 +38,8 @@
#include <visp3/core/vpConfig.h>
-#include <vector>
#include <limits>
+#include <vector>
#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentGravityCenter.h>
@@ -48,18 +49,17 @@
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
-
/*!
Default constructor
\param moments_ : Database of moment primitives.
- \param A_ : First plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param B_ : Second plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param C_ : Third plane coefficient for a plane equation of the following type Ax+By+C=1/Z.
- \param featureMoments : Database of features.
+ \param A_ : First plane coefficient for a plane equation of the following
+ type Ax+By+C=1/Z. \param B_ : Second plane coefficient for a plane equation
+ of the following type Ax+By+C=1/Z. \param C_ : Third plane coefficient for a
+ plane equation of the following type Ax+By+C=1/Z. \param featureMoments :
+ Database of features.
*/
-vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase& moments_,
- double A_, double B_, double C_,
- vpFeatureMomentDatabase* featureMoments)
+vpFeatureMomentCentered::vpFeatureMomentCentered(vpMomentDatabase &moments_, double A_, double B_, double C_,
+ vpFeatureMomentDatabase *featureMoments)
: vpFeatureMoment(moments_, A_, B_, C_, featureMoments), order(0)
{
}
@@ -70,66 +70,69 @@ 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 {
- if(select_one+select_two>moment->getObject().getOrder())
- throw vpException(vpException::badValue,
- "The requested value has not been computed, you should specify a higher order.");
- return interaction_matrices[select_two*order+select_one];
+vpMatrix vpFeatureMomentCentered::interaction(unsigned int select_one, unsigned int select_two) const
+{
+ if (select_one + select_two > moment->getObject().getOrder())
+ throw vpException(vpException::badValue, "The requested value has not "
+ "been computed, you should "
+ "specify a higher order.");
+ return interaction_matrices[select_two * order + select_one];
}
/*!
* Core function for the interaction matrix computation for moment m_pq
- * Given its dependent moment and interaction matrices, computes the interaction matrix of centred moments
+ * 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
- double term1 = 0.0;
- double term2 = 0.0;
- vpMatrix Lterm3(1,6);
+ // term1, term2 and Lterm3 (matrix) will be repeatedly computed inside the
+ // innermost loop
+ double term1 = 0.0;
+ double term2 = 0.0;
+ vpMatrix Lterm3(1, 6);
- double qcombl = 0.0;
+ double qcombl = 0.0;
double pcombkqcombl = 0.0;
double mkl = 0.0;
vpMatrix L_mkl;
- int qml = 0; // q-l
+ int qml = 0; // q-l
double minus1pow = 0.; // (-1)^(p+q-k-l)
double pintom = 0.;
- for (unsigned int k = 0; k <=p; ++k)
- {
- int pmk = (int)p-(int)k;
- double pcombk = static_cast<double>(vpMath::comb(p,k));
- for (unsigned int l = 0; l <= q; ++l)
- {
- qml = (int)q - (int)l;
- qcombl = static_cast<double>(vpMath::comb(q,l));
- minus1pow = pow((double)-1, (double)(pmk + qml));
- pcombkqcombl = pcombk * qcombl;
- mkl = m.get(k, l);
- pintom = pcombkqcombl * mkl;
- L_mkl = feature_moment_m.interaction(k, l);
- if(pmk>0)
- term1 += pintom * pmk * pow(xg, pmk-1) * pow(yg, qml) * minus1pow;
- if(qml>0)
- term2 += pintom * qml * pow(xg, pmk) * pow(yg, qml-1) * minus1pow;
- Lterm3 += pcombkqcombl * pow(xg, pmk) * pow(yg, qml) * L_mkl * minus1pow;
- }
+ for (unsigned int k = 0; k <= p; ++k) {
+ int pmk = (int)p - (int)k;
+ double pcombk = static_cast<double>(vpMath::comb(p, k));
+ for (unsigned int l = 0; l <= q; ++l) {
+ qml = (int)q - (int)l;
+ qcombl = static_cast<double>(vpMath::comb(q, l));
+ minus1pow = pow((double)-1, (double)(pmk + qml));
+ pcombkqcombl = pcombk * qcombl;
+ mkl = m.get(k, l);
+ pintom = pcombkqcombl * mkl;
+ L_mkl = feature_moment_m.interaction(k, l);
+ if (pmk > 0)
+ term1 += pintom * pmk * pow(xg, pmk - 1) * pow(yg, qml) * minus1pow;
+ if (qml > 0)
+ term2 += pintom * qml * pow(xg, pmk) * pow(yg, qml - 1) * minus1pow;
+ Lterm3 += pcombkqcombl * pow(xg, pmk) * pow(yg, qml) * L_mkl * minus1pow;
+ }
}
- // L_xg and L_yg stay constant with respect to the above loops. Lterm3 is summed over
- vpMatrix L_mupq = L_xg*term1 + L_yg*term2 + Lterm3;
+ // L_xg and L_yg stay constant with respect to the above loops. Lterm3 is
+ // summed over
+ vpMatrix L_mupq = L_xg * term1 + L_yg * term2 + Lterm3;
return L_mupq;
}
/*!
- Interface to the interaction matrix computation for centered moments. Called internally.
- Calls compute_Lmu_pq() for main computation moments (upto order-1)
+ Interface to the interaction matrix computation for centered moments. Called
+internally. Calls compute_Lmu_pq() for main computation moments (upto order-1)
Dependencies:
Moment classes
- vpMomentBasic
@@ -138,70 +141,84 @@ Dependencies:
- vpFeatureMomentBasic
- vpFeatureMomentGravityCenter
*/
-void vpFeatureMomentCentered::compute_interaction() {
+void vpFeatureMomentCentered::compute_interaction()
+{
#ifdef VISP_MOMENTS_COMBINE_MATRICES
- const vpMomentObject& momentObject = moment->getObject();
- order = momentObject.getOrder()+1;
- interaction_matrices.resize(order*order);
- for(std::vector< vpMatrix >::iterator i=interaction_matrices.begin();i!=interaction_matrices.end(); ++i)
- i->resize(1,6);
-
- bool found_moment_gravity;
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- double xg = momentGravity.get()[0];
- double yg = momentGravity.get()[1];
-
- bool found_feature_gravity_center;
- vpFeatureMomentGravityCenter& featureMomentGravityCenter= (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_feature_gravity_center)));
- if(!found_feature_gravity_center) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
- vpMatrix Lxg = featureMomentGravityCenter.interaction(1<<0);
- vpMatrix Lyg = featureMomentGravityCenter.interaction(1<<1);
-
- bool found_moment_basic;
- const vpMomentBasic& momentbasic = static_cast<const vpMomentBasic&>(moments.get("vpMomentBasic",found_moment_basic));
- if(!found_moment_basic) throw vpException(vpException::notInitialized,"vpMomentBasic not found");
-
- bool found_featuremoment_basic;
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
- // Calls the main compute_Lmu_pq function for moments upto order-1
- for(int i=0;i<(int)order-1;i++){
- for(int j=0;j<(int)order-1-i;j++){
- interaction_matrices[(unsigned int)j*order+(unsigned int)i] = compute_Lmu_pq(i, j, xg, yg, Lxg, Lyg, momentbasic, featureMomentBasic);
- }
+ const vpMomentObject &momentObject = moment->getObject();
+ order = momentObject.getOrder() + 1;
+ interaction_matrices.resize(order * order);
+ for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
+ i->resize(1, 6);
+
+ bool found_moment_gravity;
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+ double xg = momentGravity.get()[0];
+ double yg = momentGravity.get()[1];
+
+ bool found_feature_gravity_center;
+ vpFeatureMomentGravityCenter &featureMomentGravityCenter = (static_cast<vpFeatureMomentGravityCenter &>(
+ featureMomentsDataBase->get("vpFeatureMomentGravityCenter", found_feature_gravity_center)));
+ if (!found_feature_gravity_center)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentGravityCenter not found");
+ vpMatrix Lxg = featureMomentGravityCenter.interaction(1 << 0);
+ vpMatrix Lyg = featureMomentGravityCenter.interaction(1 << 1);
+
+ bool found_moment_basic;
+ const vpMomentBasic &momentbasic =
+ static_cast<const vpMomentBasic &>(moments.get("vpMomentBasic", found_moment_basic));
+ if (!found_moment_basic)
+ throw vpException(vpException::notInitialized, "vpMomentBasic not found");
+
+ bool found_featuremoment_basic;
+ vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
+ featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
+ if (!found_featuremoment_basic)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
+
+ // Calls the main compute_Lmu_pq function for moments upto order-1
+ for (int i = 0; i < (int)order - 1; i++) {
+ for (int j = 0; j < (int)order - 1 - i; j++) {
+ interaction_matrices[(unsigned int)j * order + (unsigned int)i] =
+ compute_Lmu_pq(i, j, xg, yg, Lxg, Lyg, momentbasic, featureMomentBasic);
}
-#else // #ifdef VISP_MOMENTS_COMBINE_MATRICES
+ }
+#else // #ifdef VISP_MOMENTS_COMBINE_MATRICES
bool found_moment_centered;
bool found_moment_gravity;
- const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
int delta;
int epsilon;
- const vpMomentObject& momentObject = moment->getObject();
- order = momentObject.getOrder()+1;
- interaction_matrices.resize(order*order);
- for (std::vector< vpMatrix >::iterator i=interaction_matrices.begin(); i!=interaction_matrices.end(); ++i)
- i->resize(1,6);
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- delta=0;
- epsilon=1;
+ const vpMomentObject &momentObject = moment->getObject();
+ order = momentObject.getOrder() + 1;
+ interaction_matrices.resize(order * order);
+ for (std::vector<vpMatrix>::iterator i = interaction_matrices.begin(); i != interaction_matrices.end(); ++i)
+ i->resize(1, 6);
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ delta = 0;
+ epsilon = 1;
} else {
- delta=1;
- epsilon=4;
+ delta = 1;
+ epsilon = 4;
}
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
+ double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
+ double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
+ double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
double Xg = momentGravity.getXg();
double Yg = momentGravity.getYg();
- double mu00 = momentCentered.get(0,0);
+ double mu00 = momentCentered.get(0, 0);
unsigned int VX = 0;
unsigned int VY = 1;
@@ -210,103 +227,113 @@ void vpFeatureMomentCentered::compute_interaction() {
unsigned int WY = 4;
unsigned int WZ = 5;
- interaction_matrices[0][0][VX] = -(delta)*A*mu00;
- interaction_matrices[0][0][VY] = -(delta)*B*mu00;
+ interaction_matrices[0][0][VX] = -(delta)*A * mu00;
+ interaction_matrices[0][0][VY] = -(delta)*B * mu00;
// Since mu10=0 and mu01=0
// interaction_matrices[0][0][WX] = (3*delta)*MU(0,1)+(3*delta)*Yg*mu00;
// interaction_matrices[0][0][WY] = -(3*delta)*MU(1,0)-(3*delta)*Xg*mu00;
// we get the simplification:
- interaction_matrices[0][0][WX] = (3*delta)*Yg*mu00;
- interaction_matrices[0][0][WY] = -(3*delta)*Xg*mu00;
- interaction_matrices[0][0][VZ] = -A*interaction_matrices[0][0][WY]+B*interaction_matrices[0][0][WX]+(2*delta)*C*mu00;
+ interaction_matrices[0][0][WX] = (3 * delta) * Yg * mu00;
+ interaction_matrices[0][0][WY] = -(3 * delta) * Xg * mu00;
+ interaction_matrices[0][0][VZ] =
+ -A * interaction_matrices[0][0][WY] + B * interaction_matrices[0][0][WX] + (2 * delta) * C * mu00;
interaction_matrices[0][0][WZ] = 0.;
- for (int i=1; i<(int)order-1; i++){
- unsigned int i_ = (unsigned int) i;
+ for (int i = 1; i < (int)order - 1; i++) {
+ unsigned int i_ = (unsigned int)i;
unsigned int im1_ = i_ - 1;
unsigned int ip1_ = i_ + 1;
- double mu_im10 = momentCentered.get(im1_,0);
- double mu_ip10 = momentCentered.get(ip1_,0);
- double mu_im11 = momentCentered.get(im1_,1);
- double mu_i0 = momentCentered.get(i_,0);
- double mu_i1 = momentCentered.get(i_,1);
-
- interaction_matrices[i_][0][VX] = -(i+delta)*A*mu_i0-(i*B*mu_im11);
- interaction_matrices[i_][0][VY] = -(delta)*B*mu_i0;
-
- interaction_matrices[i_][0][WX] = (i+3*delta)*mu_i1+(i+3*delta)*Yg*mu_i0+i*Xg*mu_im11-i*epsilon*n11*mu_im10;
- interaction_matrices[i_][0][WY] = -(i+3*delta)*mu_ip10-(2*i+3*delta)*Xg*mu_i0+i*epsilon*n20*mu_im10;
- interaction_matrices[i_][0][VZ] = -A*interaction_matrices[i_][0][WY]+B*interaction_matrices[i_][0][WX]+(i+2*delta)*C*mu_i0;
- interaction_matrices[i_][0][WZ] = i*mu_im11;
+ double mu_im10 = momentCentered.get(im1_, 0);
+ double mu_ip10 = momentCentered.get(ip1_, 0);
+ double mu_im11 = momentCentered.get(im1_, 1);
+ double mu_i0 = momentCentered.get(i_, 0);
+ double mu_i1 = momentCentered.get(i_, 1);
+
+ interaction_matrices[i_][0][VX] = -(i + delta) * A * mu_i0 - (i * B * mu_im11);
+ interaction_matrices[i_][0][VY] = -(delta)*B * mu_i0;
+
+ interaction_matrices[i_][0][WX] =
+ (i + 3 * delta) * mu_i1 + (i + 3 * delta) * Yg * mu_i0 + i * Xg * mu_im11 - i * epsilon * n11 * mu_im10;
+ interaction_matrices[i_][0][WY] =
+ -(i + 3 * delta) * mu_ip10 - (2 * i + 3 * delta) * Xg * mu_i0 + i * epsilon * n20 * mu_im10;
+ interaction_matrices[i_][0][VZ] =
+ -A * interaction_matrices[i_][0][WY] + B * interaction_matrices[i_][0][WX] + (i + 2 * delta) * C * mu_i0;
+ interaction_matrices[i_][0][WZ] = i * mu_im11;
}
- for(int j=1;j<(int)order-1;j++){
- unsigned int j_ = (unsigned int) j;
+ for (int j = 1; j < (int)order - 1; j++) {
+ unsigned int j_ = (unsigned int)j;
unsigned int jm1_ = j_ - 1;
unsigned int jp1_ = j_ + 1;
- double mu_0jm1 = momentCentered.get(0,jm1_);
- double mu_0jp1 = momentCentered.get(0,jp1_);
- double mu_1jm1 = momentCentered.get(1,jm1_);
- double mu_0j = momentCentered.get(0,j_);
- double mu_1j = momentCentered.get(1,j_);
-
- interaction_matrices[j_*order][0][VX] = -(delta)*A*mu_0j;
- interaction_matrices[j_*order][0][VY] = -j*A*mu_1jm1-(j+delta)*B*mu_0j;
-
- interaction_matrices[j_*order][0][WX] = (j+3*delta)*mu_0jp1+(2*j+3*delta)*Yg*mu_0j-j*epsilon*n02*mu_0jm1;
- interaction_matrices[j_*order][0][WY] = -(j+3*delta)*mu_1j-(j+3*delta)*Xg*mu_0j-j*Yg*mu_1jm1+j*epsilon*n11*mu_0jm1;
- interaction_matrices[j_*order][0][VZ] = -A*interaction_matrices[j_*order][0][WY]+B*interaction_matrices[j_*order][0][WX]+(j+2*delta)*C*mu_0j;
- interaction_matrices[j_*order][0][WZ] = -j*mu_1jm1;
+ double mu_0jm1 = momentCentered.get(0, jm1_);
+ double mu_0jp1 = momentCentered.get(0, jp1_);
+ double mu_1jm1 = momentCentered.get(1, jm1_);
+ double mu_0j = momentCentered.get(0, j_);
+ double mu_1j = momentCentered.get(1, j_);
+
+ interaction_matrices[j_ * order][0][VX] = -(delta)*A * mu_0j;
+ interaction_matrices[j_ * order][0][VY] = -j * A * mu_1jm1 - (j + delta) * B * mu_0j;
+
+ interaction_matrices[j_ * order][0][WX] =
+ (j + 3 * delta) * mu_0jp1 + (2 * j + 3 * delta) * Yg * mu_0j - j * epsilon * n02 * mu_0jm1;
+ interaction_matrices[j_ * order][0][WY] =
+ -(j + 3 * delta) * mu_1j - (j + 3 * delta) * Xg * mu_0j - j * Yg * mu_1jm1 + j * epsilon * n11 * mu_0jm1;
+ interaction_matrices[j_ * order][0][VZ] = -A * interaction_matrices[j_ * order][0][WY] +
+ B * interaction_matrices[j_ * order][0][WX] + (j + 2 * delta) * C * mu_0j;
+ interaction_matrices[j_ * order][0][WZ] = -j * mu_1jm1;
}
- for(int j=1; j<(int)order-1; j++) {
- unsigned int j_ = (unsigned int) j;
+ for (int j = 1; j < (int)order - 1; j++) {
+ unsigned int j_ = (unsigned int)j;
unsigned int jm1_ = j_ - 1;
unsigned int jp1_ = j_ + 1;
- for(int i=1; i<(int)order-j-1; i++) {
- unsigned int i_ = (unsigned int) i;
+ for (int i = 1; i < (int)order - j - 1; i++) {
+ unsigned int i_ = (unsigned int)i;
unsigned int im1_ = i_ - 1;
unsigned int ip1_ = i_ + 1;
- double mu_ijm1 = momentCentered.get(i_,jm1_);
- double mu_ij = momentCentered.get(i_,j_);
- double mu_ijp1 = momentCentered.get(i_,jp1_);
- double mu_im1j = momentCentered.get(im1_,j_);
- double mu_im1jp1 = momentCentered.get(im1_,jp1_);
- double mu_ip1jm1 = momentCentered.get(ip1_,jm1_);
- double mu_ip1j = momentCentered.get(ip1_,j_);
-
- interaction_matrices[j_*order+i_][0][VX] = -(i+delta)*A*mu_ij-i*B*mu_im1jp1;
- interaction_matrices[j_*order+i_][0][VY] = -j*A*mu_ip1jm1-(j+delta)*B*mu_ij;
-
- interaction_matrices[j_*order+i_][0][WX] = (i+j+3*delta)*mu_ijp1+(i+2*j+3*delta)*Yg*mu_ij
- +i*Xg*mu_im1jp1-i*epsilon*n11*mu_im1j-j*epsilon*n02*mu_ijm1;
- interaction_matrices[j_*order+i_][0][WY] = -(i+j+3*delta)*mu_ip1j-(2*i+j+3*delta)*Xg*mu_ij
- -j*Yg*mu_ip1jm1+i*epsilon*n20*mu_im1j+j*epsilon*n11*mu_ijm1;
- interaction_matrices[j_*order+i_][0][VZ] = -A*interaction_matrices[j_*order+i_][0][WY]+B*interaction_matrices[j_*order+i_][0][WX]+(i+j+2*delta)*C*mu_ij;
- interaction_matrices[j_*order+i_][0][WZ] = i*mu_im1jp1-j*mu_ip1jm1;
+ double mu_ijm1 = momentCentered.get(i_, jm1_);
+ double mu_ij = momentCentered.get(i_, j_);
+ double mu_ijp1 = momentCentered.get(i_, jp1_);
+ double mu_im1j = momentCentered.get(im1_, j_);
+ double mu_im1jp1 = momentCentered.get(im1_, jp1_);
+ double mu_ip1jm1 = momentCentered.get(ip1_, jm1_);
+ double mu_ip1j = momentCentered.get(ip1_, j_);
+
+ interaction_matrices[j_ * order + i_][0][VX] = -(i + delta) * A * mu_ij - i * B * mu_im1jp1;
+ interaction_matrices[j_ * order + i_][0][VY] = -j * A * mu_ip1jm1 - (j + delta) * B * mu_ij;
+
+ interaction_matrices[j_ * order + i_][0][WX] = (i + j + 3 * delta) * mu_ijp1 +
+ (i + 2 * j + 3 * delta) * Yg * mu_ij + i * Xg * mu_im1jp1 -
+ i * epsilon * n11 * mu_im1j - j * epsilon * n02 * mu_ijm1;
+ interaction_matrices[j_ * order + i_][0][WY] = -(i + j + 3 * delta) * mu_ip1j -
+ (2 * i + j + 3 * delta) * Xg * mu_ij - j * Yg * mu_ip1jm1 +
+ i * epsilon * n20 * mu_im1j + j * epsilon * n11 * mu_ijm1;
+ interaction_matrices[j_ * order + i_][0][VZ] = -A * interaction_matrices[j_ * order + i_][0][WY] +
+ B * interaction_matrices[j_ * order + i_][0][WX] +
+ (i + j + 2 * delta) * C * mu_ij;
+ interaction_matrices[j_ * order + i_][0][WZ] = i * mu_im1jp1 - j * mu_ip1jm1;
}
}
#endif // #ifdef VISP_MOMENTS_COMBINE_MATRICES
}
-
/*!
\relates vpFeatureMomentCentered
Print all the interaction matrices of visual features
*/
-std::ostream& operator<<(std::ostream & os, const vpFeatureMomentCentered& mu){
+std::ostream &operator<<(std::ostream &os, const vpFeatureMomentCentered &mu)
+{
vpTRACE(" << Ls - CENTRED MOMENTS >>");
unsigned int order_m_1 = (unsigned int)(mu.order - 1);
- for(unsigned int i=0; i<order_m_1; i++){
- for(unsigned int j=0; j<order_m_1-i; j++){
- os << "L_mu[" << i << "," << j << "] = ";
- mu.interaction(i,j).matlabPrint(os);
- }
+ for (unsigned int i = 0; i < order_m_1; i++) {
+ for (unsigned int j = 0; j < order_m_1 - i; j++) {
+ os << "L_mu[" << i << "," << j << "] = ";
+ mu.interaction(i, j).matlabPrint(os);
+ }
}
return os;
}
-
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp
index 51fac7b..3885fe9 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentCommon.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -28,58 +29,56 @@
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Description:
- * Pre-filled pseudo-database used to handle dependencies between common moment features.
+ * Pre-filled pseudo-database used to handle dependencies between common
+ *moment features.
*
* Authors:
* Filip Novotny
*
*****************************************************************************/
-#include <visp3/visual_features/vpFeatureMomentCommon.h>
#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
/*!
Constructor which initializes and links all common features in the database
\param moments : database for moment primitives
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ \param A : first plane coefficient for a plane equation of the following
+ type Ax+By+C=1/Z \param B : second plane coefficient for a plane equation of
+ the following type Ax+By+C=1/Z \param C : third plane coefficient for a
+ plane equation of the following type Ax+By+C=1/Z
*/
-vpFeatureMomentCommon::vpFeatureMomentCommon(vpMomentDatabase& moments,double A,double B,double C) :
- featureGravity(moments,A,B,C),
- featureGravityNormalized(moments,A,B,C),
- featureAn(moments,A,B,C),
- featureCInvariant(moments,A,B,C),
- featureAlpha(moments,A,B,C),
- featureCentered(moments,A,B,C),
- featureMomentBasic(moments,A,B,C),
- feature_moment_area(moments,A,B,C)
+vpFeatureMomentCommon::vpFeatureMomentCommon(vpMomentDatabase &moments, double A, double B, double C)
+ : featureGravity(moments, A, B, C), featureGravityNormalized(moments, A, B, C), featureAn(moments, A, B, C),
+ featureCInvariant(moments, A, B, C), featureAlpha(moments, A, B, C), featureCentered(moments, A, B, C),
+ featureMomentBasic(moments, A, B, C), feature_moment_area(moments, A, B, C)
{
- featureGravity.linkTo(*this);
- featureGravityNormalized.linkTo(*this);
- featureAn.linkTo(*this);
- featureCInvariant.linkTo(*this);
- featureAlpha.linkTo(*this);
- featureMomentBasic.linkTo(*this);
- featureCentered.linkTo(*this);
- feature_moment_area.linkTo(*this);
+ featureGravity.linkTo(*this);
+ featureGravityNormalized.linkTo(*this);
+ featureAn.linkTo(*this);
+ featureCInvariant.linkTo(*this);
+ featureAlpha.linkTo(*this);
+ featureMomentBasic.linkTo(*this);
+ featureCentered.linkTo(*this);
+ feature_moment_area.linkTo(*this);
}
/*!
- Update all moment features in the database with plane coefficients
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ Update all moment features in the database with plane coefficients
+ \param A : first plane coefficient for a plane equation of the following
+ type Ax+By+C=1/Z \param B : second plane coefficient for a plane equation of
+ the following type Ax+By+C=1/Z \param C : third plane coefficient for a
+ plane equation of the following type Ax+By+C=1/Z
*/
-void vpFeatureMomentCommon::updateAll(double A,double B,double C){
- featureMomentBasic.update(A,B,C);
- featureGravity.update(A,B,C);
- featureCentered.update(A,B,C);
- featureAn.update(A,B,C);
- featureGravityNormalized.update(A,B,C);
- featureCInvariant.update(A,B,C);
- featureAlpha.update(A,B,C);
- feature_moment_area.update(A,B,C);
+void vpFeatureMomentCommon::updateAll(double A, double B, double C)
+{
+ featureMomentBasic.update(A, B, C);
+ featureGravity.update(A, B, C);
+ featureCentered.update(A, B, C);
+ featureAn.update(A, B, C);
+ featureGravityNormalized.update(A, B, C);
+ featureCInvariant.update(A, B, C);
+ featureAlpha.update(A, B, C);
+ feature_moment_area.update(A, B, C);
}
-
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp
index c0dfade..d55f971 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentDatabase.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,66 +36,73 @@
*
*****************************************************************************/
-#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-#include <visp3/visual_features/vpFeatureMoment.h>
-#include <typeinfo>
#include <iostream>
+#include <typeinfo>
#include <visp3/core/vpConfig.h>
+#include <visp3/visual_features/vpFeatureMoment.h>
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
/*!
Add a moment and it's corresponding name to the database
\param featureMoment : database for moment features
- \param name : the feature's name, usually the string naming it's class. Each name must be unique
+ \param name : the feature's name, usually the string naming it's class. Each
+ name must be unique
*/
-void vpFeatureMomentDatabase::add(vpFeatureMoment& featureMoment,char* name){
- featureMomentsDataBase.insert(std::pair<const char*,vpFeatureMoment*>((const char*)name,&featureMoment));
+void vpFeatureMomentDatabase::add(vpFeatureMoment &featureMoment, char *name)
+{
+ featureMomentsDataBase.insert(std::pair<const char *, vpFeatureMoment *>((const char *)name, &featureMoment));
}
/*!
Retrieves a moment feature from the database
\param type : the name of the feature, the one specified when using add
- \param found : true if the type string is found inside the database, false otherwise
+ \param found : true if the type string is found inside the database, false
+ otherwise
\return the moment feature corresponding to the type string
*/
-vpFeatureMoment& vpFeatureMomentDatabase::get(const char* type, bool& found){
- std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator it = featureMomentsDataBase.find(type);
+vpFeatureMoment &vpFeatureMomentDatabase::get(const char *type, bool &found)
+{
+ std::map<const char *, vpFeatureMoment *, vpFeatureMomentDatabase::cmp_str>::const_iterator it =
+ featureMomentsDataBase.find(type);
- found = (it!=featureMomentsDataBase.end());
- return *(it->second);
+ found = (it != featureMomentsDataBase.end());
+ return *(it->second);
}
/*!
- Update all moment features in the database with plane coefficients
- \param A : first plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param B : second plane coefficient for a plane equation of the following type Ax+By+C=1/Z
- \param C : third plane coefficient for a plane equation of the following type Ax+By+C=1/Z
+ Update all moment features in the database with plane coefficients
+ \param A : first plane coefficient for a plane equation of the following
+ type Ax+By+C=1/Z \param B : second plane coefficient for a plane equation of
+ the following type Ax+By+C=1/Z \param C : third plane coefficient for a
+ plane equation of the following type Ax+By+C=1/Z
*/
void vpFeatureMomentDatabase::updateAll(double A, double B, double C)
{
- std::map<const char*,vpFeatureMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
+ std::map<const char *, vpFeatureMoment *, vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
#ifdef VISP_HAVE_OPENMP
- std::vector<vpFeatureMoment*> values;
+ std::vector<vpFeatureMoment *> 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)
- for(int i=0;i<(int)values.size();i++){
+#pragma omp parallel for shared(A, B, C)
+ for (int i = 0; i < (int)values.size(); i++) {
unsigned int i_ = static_cast<unsigned int>(i);
- values[i_]->update(A,B,C);
+ values[i_]->update(A, B, C);
}
#else
- for(itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); ++itr){
- (*itr).second->update(A,B,C);
- }
+ for (itr = featureMomentsDataBase.begin(); itr != featureMomentsDataBase.end(); ++itr) {
+ (*itr).second->update(A, B, C);
+ }
#endif
}
/*
-std::ostream & operator<<(std::ostream & os, const vpFeatureMomentDatabase& m){
- std::map<const char*,vpMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr;
- os << "{";
+std::ostream & operator<<(std::ostream & os, const vpFeatureMomentDatabase&
+m){ std::map<const
+char*,vpMoment*,vpFeatureMomentDatabase::cmp_str>::const_iterator itr; os <<
+"{";
for(itr = m.featureMoments.begin(); itr != m.featureMoments.end(); itr++){
os << (*itr).first << ": [" << *((*itr).second) << "],";
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp
index 2c05e5f..44f204b 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenter.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,14 +40,13 @@
#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <vector>
#include <limits>
+#include <vector>
#include <visp3/core/vpMomentObject.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
#include <visp3/visual_features/vpFeatureMomentBasic.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
-
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
/*!
Computes interaction matrix for gravity center moment. Called internally.
@@ -56,36 +56,38 @@
Minimum vpMomentObject order needed to compute this feature: 2.
*/
-void vpFeatureMomentGravityCenter::compute_interaction(){
- bool found_featuremoment_basic;
-
- vpFeatureMomentBasic& featureMomentBasic= (static_cast<vpFeatureMomentBasic&>(featureMomentsDataBase->get("vpFeatureMomentBasic",found_featuremoment_basic)));
- const vpMomentObject& momentObject = moment->getObject();
-
- if(!found_featuremoment_basic) throw vpException(vpException::notInitialized,"vpFeatureMomentBasic not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
- interaction_matrices[0] = featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) - momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
- interaction_matrices[1] = featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) - momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
-
-
+void vpFeatureMomentGravityCenter::compute_interaction()
+{
+ bool found_featuremoment_basic;
+
+ vpFeatureMomentBasic &featureMomentBasic = (static_cast<vpFeatureMomentBasic &>(
+ featureMomentsDataBase->get("vpFeatureMomentBasic", found_featuremoment_basic)));
+ const vpMomentObject &momentObject = moment->getObject();
+
+ if (!found_featuremoment_basic)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentBasic not found");
+
+ interaction_matrices[0].resize(1, 6);
+ interaction_matrices[1].resize(1, 6);
+
+ interaction_matrices[0] =
+ featureMomentBasic.interaction(1, 0) / momentObject.get(0, 0) -
+ momentObject.get(1, 0) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
+ interaction_matrices[1] =
+ featureMomentBasic.interaction(0, 1) / momentObject.get(0, 0) -
+ momentObject.get(0, 1) * pow(momentObject.get(0, 0), -0.2e1) * featureMomentBasic.interaction(0, 0);
}
#else
-#include <vector>
#include <limits>
+#include <vector>
-#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentDatabase.h>
#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/core/vpMomentCentered.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
#include <visp3/core/vpMomentObject.h>
-
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
/*!
Computes interaction matrix for gravity center moment. Called internally.
@@ -96,64 +98,66 @@ void vpFeatureMomentGravityCenter::compute_interaction(){
Minimum vpMomentObject order needed to compute this feature: 2.
*/
-void vpFeatureMomentGravityCenter::compute_interaction(){
- bool found_moment_centered;
- bool found_moment_gravity;
-
- const vpMomentCentered& momentCentered= (static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered)));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
-
-
- const vpMomentObject& momentObject = moment->getObject();
-
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
- int epsilon;
- if (momentObject.getType()==vpMomentObject::DISCRETE) {
- epsilon=1;
- } else {
- epsilon=4;
- }
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
- double Zg_inv = A*Xg+B*Yg+C;
- double Xgvz = Xg*Zg_inv+A*epsilon*n20+B*epsilon*n11;
- double Ygvz = Yg*Zg_inv+A*epsilon*n11+B*epsilon*n02;
- double Xgwx = Xg*Yg+epsilon*n11;
- double Ygwy = -Xgwx;
- double Xgwy = -(1+Xg*Xg+epsilon*n20);
- double Ygwx = 1+Yg*Yg+epsilon*n02;
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = -Zg_inv;
- interaction_matrices[0][0][VY] = 0;
- interaction_matrices[0][0][VZ] = Xgvz;
-
- interaction_matrices[0][0][WX] = Xgwx;
- interaction_matrices[0][0][WY] = Xgwy;
- interaction_matrices[0][0][WZ] = Yg;
-
- interaction_matrices[1][0][VX] = 0;
- interaction_matrices[1][0][VY] = -Zg_inv;
- interaction_matrices[1][0][VZ] = Ygvz;
-
- interaction_matrices[1][0][WX] = Ygwx;
- interaction_matrices[1][0][WY] = Ygwy;
- interaction_matrices[1][0][WZ] = -Xg;
-
+void vpFeatureMomentGravityCenter::compute_interaction()
+{
+ bool found_moment_centered;
+ bool found_moment_gravity;
+
+ const vpMomentCentered &momentCentered =
+ (static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered)));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+
+ const vpMomentObject &momentObject = moment->getObject();
+
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+
+ interaction_matrices[0].resize(1, 6);
+ interaction_matrices[1].resize(1, 6);
+ int epsilon;
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ epsilon = 1;
+ } else {
+ epsilon = 4;
+ }
+ double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
+ double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
+ double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+ double Zg_inv = A * Xg + B * Yg + C;
+ double Xgvz = Xg * Zg_inv + A * epsilon * n20 + B * epsilon * n11;
+ double Ygvz = Yg * Zg_inv + A * epsilon * n11 + B * epsilon * n02;
+ double Xgwx = Xg * Yg + epsilon * n11;
+ double Ygwy = -Xgwx;
+ double Xgwy = -(1 + Xg * Xg + epsilon * n20);
+ double Ygwx = 1 + Yg * Yg + epsilon * n02;
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = -Zg_inv;
+ interaction_matrices[0][0][VY] = 0;
+ interaction_matrices[0][0][VZ] = Xgvz;
+
+ interaction_matrices[0][0][WX] = Xgwx;
+ interaction_matrices[0][0][WY] = Xgwy;
+ interaction_matrices[0][0][WZ] = Yg;
+
+ interaction_matrices[1][0][VX] = 0;
+ interaction_matrices[1][0][VY] = -Zg_inv;
+ interaction_matrices[1][0][VZ] = Ygvz;
+
+ interaction_matrices[1][0][WX] = Ygwx;
+ interaction_matrices[1][0][WY] = Ygwy;
+ interaction_matrices[1][0][WZ] = -Xg;
}
#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
index 699eb71..57a3f5d 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureMomentGravityCenterNormalized.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -39,179 +40,190 @@
#ifdef VISP_MOMENTS_COMBINE_MATRICES
-#include <vector>
#include <limits>
+#include <vector>
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentAreaNormalized.h>
+#include <visp3/core/vpMomentCentered.h>
#include <visp3/core/vpMomentGravityCenter.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
-#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
/*!
- Computes interaction matrix for centered and normalized moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for centered and normalized moment. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpFeatureMomentGravityCenter
- vpMomentGravityCenter
- vpMomentAreaNormalized
- vpFeatureMomentAreaNormalized
*/
-void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
- bool found_moment_gravity;
- bool found_moment_surface_normalized;
-
- bool found_featuremoment_gravity;
- bool found_featuremoment_surfacenormalized;
-
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- vpFeatureMomentGravityCenter& featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter&>(featureMomentsDataBase->get("vpFeatureMomentGravityCenter",found_featuremoment_gravity)));
- vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized&>(featureMomentsDataBase->get("vpFeatureMomentAreaNormalized",found_featuremoment_surfacenormalized)));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
-
- if(!found_featuremoment_gravity) throw vpException(vpException::notInitialized,"vpFeatureMomentGravityCenter not found");
- if(!found_featuremoment_surfacenormalized) throw vpException(vpException::notInitialized,"vpFeatureMomentAreaNormalized not found");
-
-
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
- interaction_matrices[0] = momentGravity.get()[0]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(1);
- interaction_matrices[1] = momentGravity.get()[1]*featureMomentAreaNormalized.interaction(1) + momentSurfaceNormalized.get()[0]*featureMomentGravity.interaction(2);
+void vpFeatureMomentGravityCenterNormalized::compute_interaction()
+{
+ bool found_moment_gravity;
+ bool found_moment_surface_normalized;
+
+ bool found_featuremoment_gravity;
+ bool found_featuremoment_surfacenormalized;
+
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+ vpFeatureMomentGravityCenter &featureMomentGravity = (static_cast<vpFeatureMomentGravityCenter &>(
+ featureMomentsDataBase->get("vpFeatureMomentGravityCenter", found_featuremoment_gravity)));
+ vpFeatureMomentAreaNormalized featureMomentAreaNormalized = (static_cast<vpFeatureMomentAreaNormalized &>(
+ featureMomentsDataBase->get("vpFeatureMomentAreaNormalized", found_featuremoment_surfacenormalized)));
+
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+
+ if (!found_featuremoment_gravity)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentGravityCenter not found");
+ if (!found_featuremoment_surfacenormalized)
+ throw vpException(vpException::notInitialized, "vpFeatureMomentAreaNormalized not found");
+
+ interaction_matrices[0].resize(1, 6);
+ interaction_matrices[1].resize(1, 6);
+
+ interaction_matrices[0] = momentGravity.get()[0] * featureMomentAreaNormalized.interaction(1) +
+ momentSurfaceNormalized.get()[0] * featureMomentGravity.interaction(1);
+ interaction_matrices[1] = momentGravity.get()[1] * featureMomentAreaNormalized.interaction(1) +
+ momentSurfaceNormalized.get()[0] * featureMomentGravity.interaction(2);
}
#else
-#include <vector>
#include <limits>
+#include <vector>
-#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpMomentAreaNormalized.h>
#include <visp3/core/vpMomentCentered.h>
+#include <visp3/core/vpMomentGravityCenter.h>
+#include <visp3/core/vpMomentObject.h>
#include <visp3/visual_features/vpFeatureMomentDatabase.h>
#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
-#include <visp3/core/vpMomentAreaNormalized.h>
-#include <visp3/core/vpMomentGravityCenter.h>
-
/*!
- Computes interaction matrix for centered and normalized moment. Called internally.
- The moment primitives must be computed before calling this.
- This feature depends on:
+ Computes interaction matrix for centered and normalized moment. Called
+ internally. The moment primitives must be computed before calling this. This
+ feature depends on:
- vpMomentCentered
- vpMomentAreaNormalized
- vpMomentGravityCenter
*/
-void vpFeatureMomentGravityCenterNormalized::compute_interaction(){
-
- bool found_moment_surface_normalized;
- bool found_moment_gravity;
- bool found_moment_centered;
-
- const vpMomentCentered& momentCentered = static_cast<const vpMomentCentered&>(moments.get("vpMomentCentered",found_moment_centered));
- const vpMomentGravityCenter& momentGravity = static_cast<const vpMomentGravityCenter&>(moments.get("vpMomentGravityCenter",found_moment_gravity));
- const vpMomentAreaNormalized& momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized&>(moments.get("vpMomentAreaNormalized",found_moment_surface_normalized));
-
- if(!found_moment_surface_normalized) throw vpException(vpException::notInitialized,"vpMomentAreaNormalized not found");
- if(!found_moment_gravity) throw vpException(vpException::notInitialized,"vpMomentGravityCenter not found");
- if(!found_moment_centered) throw vpException(vpException::notInitialized,"vpMomentCentered not found");
-
- const vpMomentObject& momentObject = moment->getObject();
- interaction_matrices[0].resize(1,6);
- interaction_matrices[1].resize(1,6);
-
-
- double n11 = momentCentered.get(1,1)/momentObject.get(0,0);
- double n20 = momentCentered.get(2,0)/momentObject.get(0,0);
- double n02 = momentCentered.get(0,2)/momentObject.get(0,0);
- double n10 = momentCentered.get(1,0)/momentObject.get(0,0);
- double n01 = momentCentered.get(0,1)/momentObject.get(0,0);
- double n03 = momentCentered.get(0,3)/momentObject.get(0,0);
-
-
- double Xg = momentGravity.getXg();
- double Yg = momentGravity.getYg();
-
- double An=momentSurfaceNormalized.get()[0];
-
- double Xn = An*Xg;
- double Yn = An*Yg;
-
-
- double Xnvx,Xnvy,Xnvz,Xnwx,Xnwy;
- double Ynvx,Ynvy,Ynvz,Ynwx,Ynwy;
-
- if(momentObject.getType()==vpMomentObject::DISCRETE){
- double a = momentCentered.get(2,0)+momentCentered.get(0,2);
-
- double e11 = momentCentered.get(1,1)/a;
- double e12 = momentCentered.get(1,2)/a;
- double e21 = momentCentered.get(2,1)/a;
- double e30 = momentCentered.get(3,0)/a;
-
- double NA = n20+n02;
-
-
- Xnvx = B*Xn*e11-Yn*B-An*C-n02*A*Xn/NA;
- Xnvy = A*Xn*e11+n02*B*Xn/NA;
-
- Xnwx = An*e11*NA+Yn*n10-Xn*Xg*e11+Xn*n01+Xn*n10*e11-Xn*e21+(-Xn*n03+(Xn*n01-Yn*Xg)*n02)/NA;
- Xnwy = -An*NA+Xn*e12+Xn*Xg-An+e11*Xg*Yn-Xn*n01*e11-2*Xn*n10+Xn*e30+n02*An+(-Xn*Xg+Xn*n10)*n02/NA;
-
-
-
- Ynvx = (Yn-n02*Yn/NA)*A+Yn*e11*B;
- Ynvy = (-Xn+e11*Yn)*A+(-Yn+n02*Yn/NA)*B-An*C;
-
- Ynwx = n02*An+Yn*n10*e11-e11*Xg*Yn+An-Yn*e21+Yn*n01+(-Yn*n03+(Yn*n01-Yn*Yg)*n02)/NA;
- Ynwy = -An*e11*NA+Yn*e11*Yg-Yn*n01*e11+Yn*Xg+Yn*e12+Yn*e30-Xn*n01-2*Yn*n10+(Yn*n10-Yn*Xg)*n02/NA;
-
-
- }else{
- Xnvx = -An*C-A*Xn-Yn*B;
- Xnvy = (0.5)*B*Xn;
-
- Xnwx = (0.5*(8.*n10-Xg))*Yn+4.*An*n11+4.*n01*Xn;
- Xnwy = (0.5*(-2.-8.*n20))*An+(0.5)*Xn*(-8.*n10+Xg);
-
- Ynvx = (0.5)*A*Yn;
- Ynvy = -(0.5)*B*Yn-C*An-A*Xn;
-
- Ynwx = (0.5)*Yn*(8.*n01-Yg)+(.5*(2.+8.*n02))*An;
- Ynwy = (0.5*(-8.*n10+Xg))*Yn-4.*An*n11-4.*n01*Xn;
-
- }
-
- Ynvz = -A*Ynwy+(-An+Ynwx)*B;
- Xnvz = -A*An-A*Xnwy+B*Xnwx;
-
- int VX = 0;
- int VY = 1;
- int VZ = 2;
- int WX = 3;
- int WY = 4;
- int WZ = 5;
-
- interaction_matrices[0][0][VX] = Xnvx;
- interaction_matrices[0][0][VY] = Xnvy;
- interaction_matrices[0][0][VZ] = Xnvz;
-
- interaction_matrices[0][0][WX] = Xnwx;
- interaction_matrices[0][0][WY] = Xnwy;
- interaction_matrices[0][0][WZ] = Yn;
-
- interaction_matrices[1][0][VX] = Ynvx;
- interaction_matrices[1][0][VY] = Ynvy;
- interaction_matrices[1][0][VZ] = Ynvz;
+void vpFeatureMomentGravityCenterNormalized::compute_interaction()
+{
- interaction_matrices[1][0][WX] = Ynwx;
- interaction_matrices[1][0][WY] = Ynwy;
- interaction_matrices[1][0][WZ] = -Xn;
+ bool found_moment_surface_normalized;
+ bool found_moment_gravity;
+ bool found_moment_centered;
+ const vpMomentCentered &momentCentered =
+ static_cast<const vpMomentCentered &>(moments.get("vpMomentCentered", found_moment_centered));
+ const vpMomentGravityCenter &momentGravity =
+ static_cast<const vpMomentGravityCenter &>(moments.get("vpMomentGravityCenter", found_moment_gravity));
+ const vpMomentAreaNormalized &momentSurfaceNormalized = static_cast<const vpMomentAreaNormalized &>(
+ moments.get("vpMomentAreaNormalized", found_moment_surface_normalized));
+
+ if (!found_moment_surface_normalized)
+ throw vpException(vpException::notInitialized, "vpMomentAreaNormalized not found");
+ if (!found_moment_gravity)
+ throw vpException(vpException::notInitialized, "vpMomentGravityCenter not found");
+ if (!found_moment_centered)
+ throw vpException(vpException::notInitialized, "vpMomentCentered not found");
+
+ const vpMomentObject &momentObject = moment->getObject();
+ interaction_matrices[0].resize(1, 6);
+ interaction_matrices[1].resize(1, 6);
+
+ double n11 = momentCentered.get(1, 1) / momentObject.get(0, 0);
+ double n20 = momentCentered.get(2, 0) / momentObject.get(0, 0);
+ double n02 = momentCentered.get(0, 2) / momentObject.get(0, 0);
+ double n10 = momentCentered.get(1, 0) / momentObject.get(0, 0);
+ double n01 = momentCentered.get(0, 1) / momentObject.get(0, 0);
+ double n03 = momentCentered.get(0, 3) / momentObject.get(0, 0);
+
+ double Xg = momentGravity.getXg();
+ double Yg = momentGravity.getYg();
+
+ double An = momentSurfaceNormalized.get()[0];
+
+ double Xn = An * Xg;
+ double Yn = An * Yg;
+
+ double Xnvx, Xnvy, Xnvz, Xnwx, Xnwy;
+ double Ynvx, Ynvy, Ynvz, Ynwx, Ynwy;
+
+ if (momentObject.getType() == vpMomentObject::DISCRETE) {
+ double a = momentCentered.get(2, 0) + momentCentered.get(0, 2);
+
+ double e11 = momentCentered.get(1, 1) / a;
+ double e12 = momentCentered.get(1, 2) / a;
+ double e21 = momentCentered.get(2, 1) / a;
+ double e30 = momentCentered.get(3, 0) / a;
+
+ double NA = n20 + n02;
+
+ Xnvx = B * Xn * e11 - Yn * B - An * C - n02 * A * Xn / NA;
+ Xnvy = A * Xn * e11 + n02 * B * Xn / NA;
+
+ Xnwx = An * e11 * NA + Yn * n10 - Xn * Xg * e11 + Xn * n01 + Xn * n10 * e11 - Xn * e21 +
+ (-Xn * n03 + (Xn * n01 - Yn * Xg) * n02) / NA;
+ Xnwy = -An * NA + Xn * e12 + Xn * Xg - An + e11 * Xg * Yn - Xn * n01 * e11 - 2 * Xn * n10 + Xn * e30 + n02 * An +
+ (-Xn * Xg + Xn * n10) * n02 / NA;
+
+ Ynvx = (Yn - n02 * Yn / NA) * A + Yn * e11 * B;
+ Ynvy = (-Xn + e11 * Yn) * A + (-Yn + n02 * Yn / NA) * B - An * C;
+
+ Ynwx = n02 * An + Yn * n10 * e11 - e11 * Xg * Yn + An - Yn * e21 + Yn * n01 +
+ (-Yn * n03 + (Yn * n01 - Yn * Yg) * n02) / NA;
+ Ynwy = -An * e11 * NA + Yn * e11 * Yg - Yn * n01 * e11 + Yn * Xg + Yn * e12 + Yn * e30 - Xn * n01 - 2 * Yn * n10 +
+ (Yn * n10 - Yn * Xg) * n02 / NA;
+
+ } else {
+ Xnvx = -An * C - A * Xn - Yn * B;
+ Xnvy = (0.5) * B * Xn;
+
+ Xnwx = (0.5 * (8. * n10 - Xg)) * Yn + 4. * An * n11 + 4. * n01 * Xn;
+ Xnwy = (0.5 * (-2. - 8. * n20)) * An + (0.5) * Xn * (-8. * n10 + Xg);
+
+ Ynvx = (0.5) * A * Yn;
+ Ynvy = -(0.5) * B * Yn - C * An - A * Xn;
+
+ Ynwx = (0.5) * Yn * (8. * n01 - Yg) + (.5 * (2. + 8. * n02)) * An;
+ Ynwy = (0.5 * (-8. * n10 + Xg)) * Yn - 4. * An * n11 - 4. * n01 * Xn;
+ }
+
+ Ynvz = -A * Ynwy + (-An + Ynwx) * B;
+ Xnvz = -A * An - A * Xnwy + B * Xnwx;
+
+ int VX = 0;
+ int VY = 1;
+ int VZ = 2;
+ int WX = 3;
+ int WY = 4;
+ int WZ = 5;
+
+ interaction_matrices[0][0][VX] = Xnvx;
+ interaction_matrices[0][0][VY] = Xnvy;
+ interaction_matrices[0][0][VZ] = Xnvz;
+
+ interaction_matrices[0][0][WX] = Xnwx;
+ interaction_matrices[0][0][WY] = Xnwy;
+ interaction_matrices[0][0][WZ] = Yn;
+
+ interaction_matrices[1][0][VX] = Ynvx;
+ interaction_matrices[1][0][VY] = Ynvy;
+ interaction_matrices[1][0][VZ] = Ynvz;
+
+ interaction_matrices[1][0][WX] = Ynwx;
+ interaction_matrices[1][0][WY] = Ynwy;
+ interaction_matrices[1][0][WZ] = -Xn;
}
#endif
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp
index d43e899..78eb5e9 100644
--- a/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeaturePoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpFeaturePoint.cpp
\brief Class that defines 2D point visual feature
*/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeaturePoint.h>
@@ -57,8 +56,6 @@
#include <visp3/core/vpFeatureDisplay.h>
-
-
/*
attributes and members directly related to the vpBasicFeature needs
@@ -69,152 +66,129 @@ other functionalities ar useful but not mandatory
/*!
Initialize the memory space requested for 2D point visual feature.
*/
-void
-vpFeaturePoint::init()
+void vpFeaturePoint::init()
{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value Z (1 meters)
- Z = 1;
+ // feature dimension
+ dim_s = 2;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ // default value Z (1 meters)
+ Z = 1;
}
-/*!
+/*!
Default constructor that build a visual feature.
*/
-vpFeaturePoint::vpFeaturePoint() : Z(1.)
-{
- init() ;
-}
-
+vpFeaturePoint::vpFeaturePoint() : Z(1.) { init(); }
/*!
- Set the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Set the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\param Z_ : \f$ Z \f$ value to set.
*/
-void
-vpFeaturePoint::set_Z(const double Z_)
+void vpFeaturePoint::set_Z(const double Z_)
{
- this->Z = Z_ ;
- flags[2] = true;
+ this->Z = Z_;
+ flags[2] = true;
}
-
/*!
- Get the value of \f$ Z \f$ which represents the depth in the 3D camera frame.
+ Get the value of \f$ Z \f$ which represents the depth in the 3D camera
+ frame.
\return The value of \f$ Z \f$.
*/
-double
-vpFeaturePoint::get_Z() const
-{
- return Z ;
-}
-
+double vpFeaturePoint::get_Z() const { return Z; }
-/*!
+/*!
Set the value of \f$ x \f$ which represents the x coordinate of the point
in image plan. It is one parameter of the visual feature \f$ s \f$.
\param x : \f$ x \f$ value to set.
*/
-void
-vpFeaturePoint::set_x(const double x)
+void vpFeaturePoint::set_x(const double x)
{
- s[0] = x ;
- flags[0] = true;
+ s[0] = x;
+ flags[0] = true;
}
-
/*!
- Get the value of \f$ x \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+ Get the value of \f$ x \f$ which represents the x coordinate of the point in
+ the image plan. It is one parameter of the visual feature \f$ s \f$.
\return The value of \f$ x \f$.
*/
-double
-vpFeaturePoint::get_x() const
-{
- return s[0] ;
-}
-
+double vpFeaturePoint::get_x() const { return s[0]; }
/*!
- Set the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+ Set the value of \f$ y \f$ which represents the x coordinate of the point in
+ the image plan. It is one parameter of the visual feature \f$ s \f$.
\param y : \f$ y \f$ value to set.
*/
-void
-vpFeaturePoint::set_y(const double y)
+void vpFeaturePoint::set_y(const double y)
{
- s[1] = y ;
- flags[1] = true;
+ s[1] = y;
+ flags[1] = true;
}
-
/*!
- Get the value of \f$ y \f$ which represents the x coordinate of the point in the image plan. It is one parameter of the visual feature \f$ s \f$.
+ Get the value of \f$ y \f$ which represents the x coordinate of the point in
+ the image plan. It is one parameter of the visual feature \f$ s \f$.
\return The value of \f$ y \f$.
*/
-double
-vpFeaturePoint::get_y() const
-{
- return s[1] ;
-}
-
+double vpFeaturePoint::get_y() const { return s[1]; }
/*!
Set the value of \f$ x \f$, \f$ y \f$ and \f$ Z \f$. \f$ x \f$ and \f$ y \f$
represent the coordinates of the point in the image plan and are the
- parameters of the visual feature \f$ s \f$. \f$ Z \f$ is the 3D coordinate in the camera frame
- representing the depth.
+ parameters of the visual feature \f$ s \f$. \f$ Z \f$ is the 3D coordinate
+ in the camera frame representing the depth.
\param x_ : \f$ x \f$ value to set.
\param y_ : \f$ y \f$ value to set.
\param Z_ : \f$ Z \f$ value to set.
*/
-void
-vpFeaturePoint::set_xyZ(const double x_,
- const double y_,
- const double Z_)
+void vpFeaturePoint::set_xyZ(const double x_, const double y_, const double Z_)
{
- set_x(x_) ;
- set_y(y_) ;
- set_Z(Z_) ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ set_x(x_);
+ set_y(y_);
+ set_Z(Z_);
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Compute and return the interaction matrix \f$ L \f$. The computation is made
thanks to the values of the point features \f$ x \f$ and \f$ y \f$ and the
depth \f$ Z \f$.
- \f[ L = \left[\begin{array}{c}L_{x} \\ L_{y}\end{array}\right] =
+ \f[ L = \left[\begin{array}{c}L_{x} \\ L_{y}\end{array}\right] =
\left[\begin{array}{cccccc}
-1/Z & 0 & x/Z & xy & -(1+x^2) & y \\
0 & -1/Z & y/Z & 1+y^2 & -xy & -x
\end{array}\right]\f]
- \param select : Selection of a subset of the possible point features.
+ \param select : Selection of a subset of the possible point features.
- To compute the interaction matrix for all the two point features use
vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
matrix is \f$ [2 \times 6] \f$
- To compute the interaction matrix for only one of the point component
feature (\f$ x, y \f$) use one of the corresponding function selectX() or
- selectY(). In that case the returned interaction matrix is \f$ [1 \times 6]
- \f$ dimension.
+ selectY(). In that case the returned interaction matrix is \f$ [1 \times
+ 6] \f$ dimension.
\return The interaction matrix computed from the point features.
@@ -238,29 +212,28 @@ vpFeaturePoint::set_xyZ(const double x_,
vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_ALL );
\endcode
*/
-vpMatrix
-vpFeaturePoint::interaction(const unsigned int select)
+vpMatrix vpFeaturePoint::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but x was "
+ "not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but y was "
+ "not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -269,59 +242,54 @@ vpFeaturePoint::interaction(const unsigned int select)
resetFlags();
}
- double x_ = get_x() ;
- double y_ = get_y() ;
- double Z_ = get_Z() ;
+ double x_ = get_x();
+ double y_ = get_y();
+ double Z_ = get_Z();
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
+ 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 ;
+ 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 )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
+ if (vpFeaturePoint::selectX() & select) {
+ vpMatrix Lx(1, 6);
+ Lx = 0;
- Lx[0][0] = -1/Z_ ;
- Lx[0][1] = 0 ;
- Lx[0][2] = x_/Z_ ;
- Lx[0][3] = x_*y_ ;
- Lx[0][4] = -(1+x_*x_) ;
- Lx[0][5] = y_ ;
+ Lx[0][0] = -1 / Z_;
+ Lx[0][1] = 0;
+ Lx[0][2] = x_ / Z_;
+ Lx[0][3] = x_ * y_;
+ Lx[0][4] = -(1 + x_ * x_);
+ Lx[0][5] = y_;
- L = vpMatrix::stack(L,Lx) ;
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeaturePoint::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
+ if (vpFeaturePoint::selectY() & select) {
+ vpMatrix Ly(1, 6);
+ Ly = 0;
- Ly[0][0] = 0 ;
- Ly[0][1] = -1/Z_ ;
- Ly[0][2] = y_/Z_ ;
- Ly[0][3] = 1+y_*y_ ;
- Ly[0][4] = -x_*y_ ;
- Ly[0][5] = -x_ ;
+ Ly[0][0] = 0;
+ Ly[0][1] = -1 / Z_;
+ Ly[0][2] = y_ / Z_;
+ Ly[0][3] = 1 + y_ * y_;
+ Ly[0][4] = -x_ * y_;
+ Ly[0][5] = -x_;
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- return L ;
+ return L;
}
-
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
@@ -330,12 +298,18 @@ vpFeaturePoint::interaction(const unsigned int select)
\param select : The error can be computed for a selection of a
subset of the possible point features.
- - To compute the error for all the two point features use vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2 dimension column vector.
- - To compute the error for only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY(). In that case the error vector is a 1 dimension column vector.
-
- \return The error \f$ (s-s^*)\f$ between the current and the desired visual feature.
-
- The code below shows how to use this method to manipulate the \f$ x \f$ subset:
+ - To compute the error for all the two point features use
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 2 dimension
+ column vector.
+ - To compute the error for only one of the point component feature (\f$ x, y
+ \f$) use one of the corresponding function selectX() or selectY(). In that
+ case the error vector is a 1 dimension column vector.
+
+ \return The error \f$ (s-s^*)\f$ between the current and the desired visual
+ feature.
+
+ The code below shows how to use this method to manipulate the \f$ x \f$
+ subset:
\code
// Creation of the current feature s
vpFeaturePoint s;
@@ -352,44 +326,37 @@ 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) ;
+ vpColVector e(0);
- try{
- if (vpFeaturePoint::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
+ try {
+ if (vpFeaturePoint::selectX() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0] - s_star[0];
- e = vpColVector::stack(e,ex) ;
+ e = vpColVector::stack(e, ex);
}
- if (vpFeaturePoint::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeaturePoint::selectY() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1] - s_star[1];
+ e = vpColVector::stack(e, ey);
}
- }
- catch(...) {
- throw ;
+ } catch (...) {
+ throw;
}
-
- return e ;
-
+ return e;
}
-
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
\param select : Selection of a subset of the possible point features.
- To print all the two point features use vpBasicFeature::FEATURE_ALL.
- - To print only one of the point component feature (\f$ x, y \f$) use one of the corresponding function selectX() or selectY().
+ - To print only one of the point component feature (\f$ x, y \f$) use one of
+ the corresponding function selectX() or selectY().
\code
vpFeaturePoint s; // Current visual feature s
@@ -402,60 +369,55 @@ vpFeaturePoint::error(const vpBasicFeature &s_star,
s.print(vpFeaturePoint::selectX()); // print only the x component
\endcode
*/
-void
-vpFeaturePoint::print(const unsigned int select ) const
+void vpFeaturePoint::print(const unsigned int select) const
{
- std::cout <<"Point: Z=" << get_Z() ;
- if (vpFeaturePoint::selectX() & select )
- std::cout << " x=" << get_x() ;
- if (vpFeaturePoint::selectY() & select )
- std::cout << " y=" << get_y() ;
- std::cout <<std::endl ;
+ std::cout << "Point: Z=" << get_Z();
+ if (vpFeaturePoint::selectX() & select)
+ std::cout << " x=" << get_x();
+ if (vpFeaturePoint::selectY() & select)
+ std::cout << " y=" << get_y();
+ std::cout << std::endl;
}
-
/*!
- Build a 2D point visual feature from the point coordinates in the image plan \f$ x \f$ and \f$ y \f$. The parameter Z which describes the depth, is set in the same time.
+ Build a 2D point visual feature from the point coordinates in the image plan
+ \f$ x \f$ and \f$ y \f$. The parameter Z which describes the depth, is set
+ in the same time.
- See the vpFeaturePoint class description for more details about \f$ x \f$ and \f$ y \f$.
+ See the vpFeaturePoint class description for more details about \f$ x \f$
+ and \f$ y \f$.
\param x_ : The \f$ x \f$ parameter.
\param y_ : The \f$ y \f$ parameter.
\param Z_ : The \f$ Z \f$ parameter.
*/
-void
-vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_)
+void vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_)
{
- s[0] = x_ ;
- s[1] = y_ ;
+ s[0] = x_;
+ s[1] = y_;
- this->Z = Z_ ;
+ this->Z = Z_;
- if (Z_ < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z_ << std::endl ;
+ 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 ;
+ 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++) flags[i] = true;
-
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Display point feature.
@@ -466,24 +428,19 @@ vpFeaturePoint::buildFrom(const double x_, const double y_, const double Z_)
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeaturePoint::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeaturePoint::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
+ try {
+ double x, y;
+ x = get_x();
+ y = get_y();
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -497,28 +454,22 @@ vpFeaturePoint::display(const vpCameraParameters &cam,
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeaturePoint::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeaturePoint::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
+ try {
+ double x, y;
+ x = get_x();
+ y = get_y();
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*!
Create an object with the same type.
@@ -531,17 +482,19 @@ vpFeaturePoint::display(const vpCameraParameters &cam,
*/
vpFeaturePoint *vpFeaturePoint::duplicate() const
{
- vpFeaturePoint *feature = new vpFeaturePoint ;
- return feature ;
+ vpFeaturePoint *feature = new vpFeaturePoint;
+ return feature;
}
/*!
Function used to select the \f$ x \f$ subset of the point visual feature.
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ x \f$.
+ This function is to use in conjunction with interaction() in order to
+ compute the interaction matrix associated to \f$ x \f$.
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+ This function is also useful in the vpServo class to indicate that a subset
+ of the visual feature is to use in the control law:
\code
vpFeaturePoint s;
@@ -551,15 +504,17 @@ vpFeaturePoint *vpFeaturePoint::duplicate() const
task.addFeature(s, vpFeaturePoint::selectX());
\endcode
*/
-unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0] ; }
+unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0]; }
/*!
Function used to select the \f$ y \f$ subset of the point visual feature.
- This function is to use in conjunction with interaction() in order to compute the interaction matrix associated to \f$ y \f$.
+ This function is to use in conjunction with interaction() in order to
+ compute the interaction matrix associated to \f$ y \f$.
- This function is also useful in the vpServo class to indicate that a subset of the visual feature is to use in the control law:
+ This function is also useful in the vpServo class to indicate that a subset
+ of the visual feature is to use in the control law:
\code
vpFeaturePoint s;
@@ -569,4 +524,4 @@ unsigned int vpFeaturePoint::selectX() { return FEATURE_LINE[0] ; }
task.addFeature(s, vpFeaturePoint::selectY());
\endcode
*/
-unsigned int vpFeaturePoint::selectY() { return FEATURE_LINE[1] ; }
+unsigned int vpFeaturePoint::selectY() { return FEATURE_LINE[1]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp
index 5b8e9e5..29366db 100644
--- a/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeaturePoint3D.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeaturePoint3D.h>
@@ -47,10 +47,6 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-
-
-
-
/*
attributes and members directly related to the vpBasicFeature needs
@@ -58,44 +54,39 @@
*/
-/*!
+/*!
Initialise the memory space requested for a 3D point visual
feature.
-
+
By default this feature is initialized to \f${\bf X} = (0, 0, 1)\f$.
*/
-void
-vpFeaturePoint3D::init()
+void vpFeaturePoint3D::init()
{
- //feature dimension
- dim_s = 3 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value XYZ
- s[0] = 0;
- s[1] = 0;
- s[2] = 1;
-
+ // feature dimension
+ dim_s = 3;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ // default value XYZ
+ s[0] = 0;
+ s[1] = 0;
+ s[2] = 1;
}
-/*!
+/*!
Default constructor that build a 3D point visual feature and
initialize it to \f${\bf X} = (0, 0, 1)\f$.
*/
-vpFeaturePoint3D::vpFeaturePoint3D()
-{
- init() ;
-}
-
+vpFeaturePoint3D::vpFeaturePoint3D() { init(); }
/*!
@@ -106,11 +97,10 @@ vpFeaturePoint3D::vpFeaturePoint3D()
\sa get_X()
*/
-void
-vpFeaturePoint3D::set_X(const double X)
+void vpFeaturePoint3D::set_X(const double X)
{
- s[0] = X ;
- flags[0] = true;
+ s[0] = X;
+ flags[0] = true;
}
/*!
@@ -122,11 +112,10 @@ vpFeaturePoint3D::set_X(const double X)
\sa get_Y()
*/
-void
-vpFeaturePoint3D::set_Y(const double Y)
+void vpFeaturePoint3D::set_Y(const double Y)
{
- s[1] = Y ;
- flags[1] = true;
+ s[1] = Y;
+ flags[1] = true;
}
/*!
@@ -138,53 +127,38 @@ vpFeaturePoint3D::set_Y(const double Y)
\sa get_Z()
*/
-void
-vpFeaturePoint3D::set_Z(const double Z)
+void vpFeaturePoint3D::set_Z(const double Z)
{
- s[2] = Z ;
- flags[2] = true;
+ s[2] = Z;
+ flags[2] = true;
}
-/*!
+/*!
Initialize the 3D point coordinates.
\param X,Y,Z : \f$(X,Y,Z)\f$ coordinates in the camera frame of the
3D point visual feature.
-
+
\sa set_X(), set_Y(), set_Z()
*/
-void
-vpFeaturePoint3D::set_XYZ(const double X,
- const double Y,
- const double Z)
+void vpFeaturePoint3D::set_XYZ(const double X, const double Y, const double Z)
{
- set_X(X) ;
- set_Y(Y) ;
- set_Z(Z) ;
+ set_X(X);
+ set_Y(Y);
+ set_Z(Z);
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
//! Return the \f$X\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_X() const
-{
- return s[0] ;
-}
+double vpFeaturePoint3D::get_X() const { return s[0]; }
//! Return the \f$Y\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_Y() const
-{
- return s[1] ;
-}
+double vpFeaturePoint3D::get_Y() const { return s[1]; }
//! Return the \f$Z\f$ coordinate in the camera frame of the 3D point.
-double
-vpFeaturePoint3D::get_Z() const
-{
- return s[2] ;
-}
+double vpFeaturePoint3D::get_Z() const { return s[2]; }
/*!
Compute and return the interaction matrix \f$ L \f$ associated to a subset
@@ -203,12 +177,12 @@ vpFeaturePoint3D::get_Z() const
\param select : Selection of a subset of the possible 3D point coordinate
- features.
- - To compute the interaction matrix for all the three
+ features.
+ - To compute the interaction matrix for all the three
subset features \f$(X,Y,Z)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [3 \times
6] \f$
- - To compute the interaction matrix for only one of the
+ - To compute the interaction matrix for only one of the
subset (\f$X, Y,Z\f$) use
one of the corresponding function selectX(), selectY() or
selectZ(). In that case the returned interaction matrix is \f$ [1
@@ -218,11 +192,11 @@ vpFeaturePoint3D::get_Z() const
features.
The code below shows how to compute the interaction matrix
- associated to the visual feature \f$s = X \f$.
+ associated to the visual feature \f$s = X \f$.
\code
vpPoint point;
- ...
+ ...
// Creation of the current feature s
vpFeaturePoint3D s;
s.buildFrom(point);
@@ -253,29 +227,28 @@ vpFeaturePoint3D::get_Z() const
line corresponds to the \f$ Z \f$ visual feature.
*/
-vpMatrix
-vpFeaturePoint3D::interaction(const unsigned int select)
+vpMatrix vpFeaturePoint3D::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but X was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but X was "
+ "not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Y was "
+ "not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -284,51 +257,51 @@ vpFeaturePoint3D::interaction(const unsigned int select)
resetFlags();
}
- double X = get_X() ;
- double Y = get_Y() ;
- double Z = get_Z() ;
+ double X = get_X();
+ double Y = get_Y();
+ double Z = get_Z();
- if (vpFeaturePoint3D::selectX() & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
+ if (vpFeaturePoint3D::selectX() & select) {
+ vpMatrix Lx(1, 6);
+ Lx = 0;
- Lx[0][0] = -1 ;
- Lx[0][1] = 0 ;
- Lx[0][2] = 0 ;
- Lx[0][3] = 0 ;
- Lx[0][4] = -Z ;
- Lx[0][5] = Y ;
+ Lx[0][0] = -1;
+ Lx[0][1] = 0;
+ Lx[0][2] = 0;
+ Lx[0][3] = 0;
+ Lx[0][4] = -Z;
+ Lx[0][5] = Y;
- L = vpMatrix::stack(L,Lx) ;
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeaturePoint3D::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
+ if (vpFeaturePoint3D::selectY() & select) {
+ vpMatrix Ly(1, 6);
+ Ly = 0;
- Ly[0][0] = 0 ;
- Ly[0][1] = -1 ;
- Ly[0][2] = 0 ;
- Ly[0][3] = Z ;
- Ly[0][4] = 0 ;
- Ly[0][5] = -X ;
+ Ly[0][0] = 0;
+ Ly[0][1] = -1;
+ Ly[0][2] = 0;
+ Ly[0][3] = Z;
+ Ly[0][4] = 0;
+ Ly[0][5] = -X;
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- if (vpFeaturePoint3D::selectZ() & select )
- {
- vpMatrix Lz(1,6) ; Lz = 0;
-
- Lz[0][0] = 0 ;
- Lz[0][1] = 0 ;
- Lz[0][2] = -1 ;
- Lz[0][3] = -Y ;
- Lz[0][4] = X ;
- Lz[0][5] = 0 ;
-
- L = vpMatrix::stack(L,Lz) ;
+ if (vpFeaturePoint3D::selectZ() & select) {
+ vpMatrix Lz(1, 6);
+ Lz = 0;
+
+ Lz[0][0] = 0;
+ Lz[0][1] = 0;
+ Lz[0][2] = -1;
+ Lz[0][3] = -Y;
+ Lz[0][4] = X;
+ Lz[0][5] = 0;
+
+ L = vpMatrix::stack(L, Lz);
}
- return L ;
+ return L;
}
/*!
@@ -340,7 +313,7 @@ vpFeaturePoint3D::interaction(const unsigned int select)
\param select : The error can be computed for a selection of a
subset of the possible 3D point coordinate features.
- To compute the error for all the three coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
dimension column vector.
- To compute the error for only one of the coordinate
feature \f$(X,Y, or Z)\f$ use one of the
@@ -358,9 +331,9 @@ vpFeaturePoint3D::interaction(const unsigned int select)
vpFeaturePoint3D s;
s.set_Z(0.8); // Initialization of the current Z feature
- // Creation of the desired feature s*.
- vpFeatureTranslation s_star;
- s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter
+ // Creation of the desired feature s*.
+ vpFeatureTranslation s_star;
+ s_star.set_Z(1); // Initialization of the current Z* feature to Z*=1 meter
// Compute the interaction matrix for the Z coordinate feature
vpMatrix L_Z = s.interaction( vpFeaturePoint3D::selectZ() );
@@ -373,52 +346,47 @@ vpFeaturePoint3D::interaction(const unsigned int select)
the code becomes:
\code
// Compute the interaction matrix for the Y, Z feature coordinates
- vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ() );
+ vpMatrix L_YZ = s.interaction( vpFeaturePoint3D::selectY() |
+ vpFeaturePoint3D::selectZ() );
// Compute the error vector e = (s-s*) for the Y, Z feature coordinates
- vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() | vpFeaturePoint3D::selectZ());
+ vpColVector e = s.error(s_star, vpFeaturePoint3D::selectY() |
+ 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) ;
+ vpColVector e(0);
- try{
- if (vpFeaturePoint3D::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
+ try {
+ if (vpFeaturePoint3D::selectX() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0] - s_star[0];
- e = vpColVector::stack(e,ex) ;
+ e = vpColVector::stack(e, ex);
}
- if (vpFeaturePoint3D::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeaturePoint3D::selectY() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1] - s_star[1];
+ e = vpColVector::stack(e, ey);
}
- if (vpFeaturePoint3D::selectZ() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2] - s_star[2] ;
- e = vpColVector::stack(e,ez) ;
+ if (vpFeaturePoint3D::selectZ() & select) {
+ vpColVector ez(1);
+ ez[0] = s[2] - s_star[2];
+ e = vpColVector::stack(e, ez);
}
+ } catch (...) {
+ throw;
}
- catch(...) {
- throw ;
- }
-
- return e ;
+ return e;
}
-/*!
-
+/*!
+
Build a 3D point visual feature from the camera frame coordinates
\f$(X,Y,Z)\f$ of a point.
@@ -433,41 +401,36 @@ vpFeaturePoint3D::error(const vpBasicFeature &s_star,
(\f$Z\f$ coordinate) is null. That means that the 3D point is
on the camera which is not possible.
*/
-void
-vpFeaturePoint3D::buildFrom(const vpPoint &p)
+void vpFeaturePoint3D::buildFrom(const vpPoint &p)
{
// cP is expressed in homogeneous coordinates
// we devide by the fourth coordinate
- s[0] = p.cP[0]/p.cP[3] ;
- s[1] = p.cP[1]/p.cP[3] ;
- s[2] = p.cP[2]/p.cP[3] ;
-
- double Z = s[2] ;
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point is behind the camera ")) ;
- }
+ s[0] = p.cP[0] / p.cP[3];
+ s[1] = p.cP[1] / p.cP[3];
+ s[2] = p.cP[2] / p.cP[3];
- if (fabs(Z) < 1e-6)
- {
- vpERROR_TRACE("Point Z coordinates is null ") ;
- std::cout <<"Z = " << Z << std::endl ;
+ double Z = s[2];
+ if (Z < 0) {
+ vpERROR_TRACE("Point is behind the camera ");
+ std::cout << "Z = " << Z << std::endl;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "Point Z coordinates is null")) ;
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "Point is behind the camera "));
}
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ if (fabs(Z) < 1e-6) {
+ vpERROR_TRACE("Point Z coordinates is null ");
+ std::cout << "Z = " << Z << std::endl;
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "Point Z coordinates is null"));
+ }
+
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-/*!
-
+/*!
+
Build a 3D point visual feature from the camera frame coordinates
\f$(X,Y,Z)\f$ of a point.
@@ -482,51 +445,45 @@ 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 ;
- s[1] = Y ;
- s[2] = Z ;
+ s[0] = X;
+ s[1] = Y;
+ s[2] = Z;
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
+ 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 ;
+ 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++) flags[i] = true;
-
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
\param select : Selection of a subset of the possible 3D point
feature coordinates.
- To print all the three coordinates used as features use
- vpBasicFeature::FEATURE_ALL.
+ vpBasicFeature::FEATURE_ALL.
- To print only one of the coordinate
feature \f$(X,Y,Z)\f$ use one of the
corresponding function selectX(), selectX() or selectZ().
\code
vpPoint point;
-
+
// Creation of the current feature s
vpFeaturePoint3D s;
s.buildFrom(point);
@@ -536,23 +493,21 @@ vpFeaturePoint3D::buildFrom(const double X, const double Y, const double Z)
s.print(vpFeaturePoint3D::selectZ()); // print only the Z component
\endcode
*/
-void
-vpFeaturePoint3D::print(const unsigned int select ) const
+void vpFeaturePoint3D::print(const unsigned int select) const
{
- std::cout <<"Point3D: " ;
- if (vpFeaturePoint3D::selectX() & select )
- std::cout << " X=" << get_X() ;
- if (vpFeaturePoint3D::selectY() & select )
- std::cout << " Y=" << get_Y() ;
- if (vpFeaturePoint3D::selectZ() & select )
- std::cout << " Z=" << get_Z() ;
- std::cout <<std::endl ;
+ std::cout << "Point3D: ";
+ if (vpFeaturePoint3D::selectX() & select)
+ std::cout << " X=" << get_X();
+ if (vpFeaturePoint3D::selectY() & select)
+ std::cout << " Y=" << get_Y();
+ if (vpFeaturePoint3D::selectZ() & select)
+ std::cout << " Z=" << get_Z();
+ std::cout << std::endl;
}
-
/*!
-
+
Create an object with the same type.
\code
@@ -564,26 +519,22 @@ vpFeaturePoint3D::print(const unsigned int select ) const
*/
vpFeaturePoint3D *vpFeaturePoint3D::duplicate() const
{
- vpFeaturePoint3D *feature = new vpFeaturePoint3D ;
- return feature ;
+ vpFeaturePoint3D *feature = new vpFeaturePoint3D;
+ return feature;
}
/*!
Not implemented.
*/
-void
-vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
@@ -593,18 +544,14 @@ vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
Not implemented.
*/
-void
-vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeaturePoint3D::display(const vpCameraParameters & /*cam*/, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
@@ -627,13 +574,14 @@ vpFeaturePoint3D::display(const vpCameraParameters &/*cam*/,
vpServo task;
...
// Add the (X,Y) subset coordinates features from a 3D point to the task
- task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
+ task.addFeature(p, vpFeaturePoint3D::selectX() |
+ vpFeaturePoint3D::selectY());
\endcode
\sa selectY(), selectZ()
*/
-unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0] ; }
+unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0]; }
/*!
@@ -653,13 +601,14 @@ unsigned int vpFeaturePoint3D::selectX() { return FEATURE_LINE[0] ; }
vpServo task;
...
// Add the (X,Y) subset coordinates features from a 3D point to the task
- task.addFeature(p, vpFeaturePoint3D::selectX() | vpFeaturePoint3D::selectY());
+ task.addFeature(p, vpFeaturePoint3D::selectX() |
+ vpFeaturePoint3D::selectY());
\endcode
\sa selectX(), selectZ()
*/
-unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1] ; }
+unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1]; }
/*!
@@ -685,4 +634,4 @@ unsigned int vpFeaturePoint3D::selectY() { return FEATURE_LINE[1] ; }
\sa selectX(), selectY()
*/
-unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2] ; }
+unsigned int vpFeaturePoint3D::selectZ() { return FEATURE_LINE[2]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp
index f2d7d87..0407984 100644
--- a/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeaturePointPolar.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,13 +36,11 @@
*
*****************************************************************************/
-
/*!
\file vpFeaturePointPolar.cpp
\brief Class that defines a 2D point visual feature with polar coordinates..
*/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeaturePointPolar.h>
@@ -57,8 +56,6 @@
#include <visp3/core/vpFeatureDisplay.h>
-
-
/*
attributes and members directly related to the vpBasicFeature needs
@@ -66,34 +63,33 @@
*/
-/*!
+/*!
Initialise the memory space requested for a 2D point visual
feature with polar coordinates.
-
+
By default this feature is initialized to \f$(\rho, \theta) = (0,
0)\f$. The 3D depth of the point requested in the interaction
matrix (see interaction()) is initialized to \f$Z=1\f$.
*/
-void
-vpFeaturePointPolar::init()
+void vpFeaturePointPolar::init()
{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 3;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //default value Z (1 meters)
- Z = 1;
-
+ // feature dimension
+ dim_s = 2;
+ nbParameters = 3;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ // default value Z (1 meters)
+ Z = 1;
}
-/*!
+/*!
Default constructor that build a 2D point visual feature with
polar coordinates and initialize it to \f$(\rho, \theta) = (0,
@@ -103,100 +99,77 @@ vpFeaturePointPolar::init()
matrix (see interaction()) is initialized to \f$Z=1\f$.
*/
-vpFeaturePointPolar::vpFeaturePointPolar() : Z(1.)
-{
- init() ;
-}
+vpFeaturePointPolar::vpFeaturePointPolar() : Z(1.) { init(); }
-
-/*!
+/*!
Set the image point \f$\rho\f$ polar coordinate.
\sa set_theta()
*/
-void
-vpFeaturePointPolar::set_rho(const double rho)
+void vpFeaturePointPolar::set_rho(const double rho)
{
- s[0] = rho ;
- flags[0] = true;
+ s[0] = rho;
+ flags[0] = true;
}
-/*!
+/*!
Set the image point \f$\theta\f$ polar coordinate.
\sa set_rho()
*/
-void
-vpFeaturePointPolar::set_theta(const double theta)
+void vpFeaturePointPolar::set_theta(const double theta)
{
- s[1] = theta ;
- flags[1] = true;
+ s[1] = theta;
+ flags[1] = true;
}
-/*!
+/*!
Set the 3D point depth in the camera frame.
*/
-void
-vpFeaturePointPolar::set_Z(const double Z_)
+void vpFeaturePointPolar::set_Z(const double Z_)
{
- this->Z = Z_ ;
- flags[2] = true;
+ this->Z = Z_;
+ flags[2] = true;
}
-/*!
+/*!
Initialize the image point visual feature with polar coordinates.
\param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
the image point.
\param Z_ : 3D depth of the point in the camera frame.
-
+
\sa set_rho(), set_theta(), set_Z()
*/
-void
-vpFeaturePointPolar::set_rhoThetaZ(const double rho,
- const double theta,
- const double Z_)
+void vpFeaturePointPolar::set_rhoThetaZ(const double rho, const double theta, const double Z_)
{
- set_rho(rho) ;
- set_theta(theta) ;
- set_Z(Z_) ;
+ set_rho(rho);
+ set_theta(theta);
+ set_Z(Z_);
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-/*!
+/*!
Get the image point \f$\rho\f$ polar coordinate.
\sa get_theta()
*/
-double
-vpFeaturePointPolar::get_rho() const
-{
- return s[0] ;
-}
+double vpFeaturePointPolar::get_rho() const { return s[0]; }
-/*!
+/*!
Get the image point \f$\theta\f$ polar coordinate.
\sa get_rho()
*/
-double
-vpFeaturePointPolar::get_theta() const
-{
- return s[1] ;
-}
-/*!
+double vpFeaturePointPolar::get_theta() const { return s[1]; }
+/*!
Get the 3D point depth in the camera frame.
*/
-double
-vpFeaturePointPolar::get_Z() const
-{
- return this->Z ;
-}
-
-
+double vpFeaturePointPolar::get_Z() const { return this->Z; }
/*!
Compute and return the interaction matrix \f$ L \f$ associated to a
@@ -214,18 +187,17 @@ vpFeaturePointPolar::get_Z() const
=
\left[
\begin{array}{cccccc}
- \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} & (1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\
+ \frac{-\cos \theta}{Z} & \frac{-\sin \theta}{Z} & \frac{\rho}{Z} &
+ (1+\rho^2)\sin\theta & -(1+\rho^2)\cos\theta & 0 \\
\; \\
- \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta /\rho & \sin\theta/\rho & -1 \\
- \end{array}
- \right]
- \f]
+ \frac{\sin\theta}{\rho Z} & \frac{-\cos\theta}{\rho Z} & 0 & \cos\theta
+ /\rho & \sin\theta/\rho & -1 \\ \end{array} \right] \f]
where \f$Z\f$ is the 3D depth of the considered point.
\param select : Selection of a subset of the possible polar
point coordinate features.
- - To compute the interaction matrix for all the two
+ - To compute the interaction matrix for all the two
subset features \f$(\rho,\theta)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [2 \times
6] \f$
@@ -241,7 +213,7 @@ vpFeaturePointPolar::get_Z() const
is behind the camera \f$(Z < 0)\f$, or if the 3D depth is null \f$(Z
= 0)\f$, or if the \f$\rho\f$ polar coordinate of the point is null.
- The code below shows how to compute the interaction matrix associated to
+ The code below shows how to compute the interaction matrix associated to
the visual feature \f$s = (\rho,\theta)\f$.
\code
vpFeaturePointPolar s;
@@ -273,29 +245,28 @@ vpFeaturePointPolar::get_Z() const
In that case, L_theta is a 1 by 6 matrix.
*/
-vpMatrix
-vpFeaturePointPolar::interaction(const unsigned int select)
+vpMatrix vpFeaturePointPolar::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but rho was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but rho "
+ "was not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but theta was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but theta "
+ "was not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Z was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Z was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -304,74 +275,69 @@ vpFeaturePointPolar::interaction(const unsigned int select)
resetFlags();
}
- double rho = get_rho() ;
- double theta = get_theta() ;
- double Z_ = get_Z() ;
+ double rho = get_rho();
+ double theta = get_theta();
+ double Z_ = get_Z();
double c_ = cos(theta);
double s_ = sin(theta);
- double rho2 = rho*rho;
+ double rho2 = rho * rho;
if (fabs(rho) < 1e-6) {
- vpERROR_TRACE("rho polar coordinate of the point is null") ;
- std::cout <<"rho = " << rho << std::endl ;
+ 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 ;
+ 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 ;
+ 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 )
- {
- vpMatrix Lrho(1,6) ; Lrho = 0;
+ if (vpFeaturePointPolar::selectRho() & select) {
+ vpMatrix Lrho(1, 6);
+ Lrho = 0;
- Lrho[0][0] = -c_/Z_ ;
- Lrho[0][1] = -s_/Z_ ;
- Lrho[0][2] = rho/Z_ ;
- Lrho[0][3] = (1+rho2)*s_ ;
- Lrho[0][4] = -(1+rho2)*c_ ;
- Lrho[0][5] = 0 ;
+ Lrho[0][0] = -c_ / Z_;
+ Lrho[0][1] = -s_ / Z_;
+ Lrho[0][2] = rho / Z_;
+ Lrho[0][3] = (1 + rho2) * s_;
+ Lrho[0][4] = -(1 + rho2) * c_;
+ Lrho[0][5] = 0;
-// printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
-// std::cout << "Lrho: " << Lrho << std::endl;
+ // printf("Lrho: rho %f theta %f Z %f\n", rho, theta, Z);
+ // std::cout << "Lrho: " << Lrho << std::endl;
- L = vpMatrix::stack(L,Lrho) ;
+ L = vpMatrix::stack(L, Lrho);
}
- if (vpFeaturePointPolar::selectTheta() & select )
- {
- vpMatrix Ltheta(1,6) ; Ltheta = 0;
+ if (vpFeaturePointPolar::selectTheta() & select) {
+ vpMatrix Ltheta(1, 6);
+ Ltheta = 0;
- Ltheta[0][0] = s_/(rho*Z_) ;
- Ltheta[0][1] = -c_/(rho*Z_) ;
- Ltheta[0][2] = 0 ;
- Ltheta[0][3] = c_/rho ;
- Ltheta[0][4] = s_/rho ;
- Ltheta[0][5] = -1 ;
+ Ltheta[0][0] = s_ / (rho * Z_);
+ Ltheta[0][1] = -c_ / (rho * Z_);
+ Ltheta[0][2] = 0;
+ Ltheta[0][3] = c_ / rho;
+ Ltheta[0][4] = s_ / rho;
+ Ltheta[0][5] = -1;
-// printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
-// std::cout << "Ltheta: " << Ltheta << std::endl;
- L = vpMatrix::stack(L,Ltheta) ;
+ // printf("Ltheta: rho %f theta %f Z %f\n", rho, theta, Z);
+ // std::cout << "Ltheta: " << Ltheta << std::endl;
+ L = vpMatrix::stack(L, Ltheta);
}
- return L ;
+ return L;
}
/*!
@@ -388,7 +354,7 @@ vpFeaturePointPolar::interaction(const unsigned int select)
\param select : The error can be computed for a selection of a
subset of the possible 2D point polar coordinate features.
- To compute the error for all the three coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
dimension column vector.
- To compute the error for only one of the polar coordinate
feature \f$(\rho,\theta)\f$ use one of the
@@ -416,54 +382,49 @@ 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) ;
-
- try{
- if (vpFeaturePointPolar::selectRho() & select )
- {
- vpColVector erho(1) ;
- erho[0] = s[0] - s_star[0] ;
-
- e = vpColVector::stack(e,erho) ;
+ vpColVector e(0);
+
+ try {
+ if (vpFeaturePointPolar::selectRho() & select) {
+ vpColVector erho(1);
+ erho[0] = s[0] - s_star[0];
+
+ e = vpColVector::stack(e, erho);
}
- if (vpFeaturePointPolar::selectTheta() & select )
- {
+ if (vpFeaturePointPolar::selectTheta() & select) {
- // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] - s_star[1]);
- double err = s[1] - s_star[1] ;
+ // printf("err: %f - %f = %f\n", s[1], s_star[1], s[1] -
+ // s_star[1]);
+ double err = s[1] - s_star[1];
// printf("Error: %f ", err );
- while (err < -M_PI) err += 2*M_PI ;
- while (err > M_PI) err -= 2*M_PI ;
+ while (err < -M_PI)
+ err += 2 * M_PI;
+ while (err > M_PI)
+ err -= 2 * M_PI;
// printf(" modif %f \n", err );
-
- vpColVector etheta(1) ;
- etheta[0] = err ;
- e = vpColVector::stack(e,etheta) ;
+
+ vpColVector etheta(1);
+ etheta[0] = err;
+ e = vpColVector::stack(e, etheta);
}
- }
- catch(...) {
- throw ;
+ } catch (...) {
+ throw;
}
-
- return e ;
-
+ return e;
}
-
/*!
Print to stdout the values of the current visual feature.
\param select : Selection of a subset of the possible 2D image point
feature coordinates.
- - To print all the two polar coordinates \f$(\rho,\theta)\f$ used as
- features use vpBasicFeature::FEATURE_ALL.
+ - To print all the two polar coordinates \f$(\rho,\theta)\f$ used as
+ features use vpBasicFeature::FEATURE_ALL.
- To print only one of the polar coordinate
feature \f$(\rho,\theta)\f$ use one of the
corresponding function selectRho() or selectTheta().
@@ -478,21 +439,19 @@ vpFeaturePointPolar::error(const vpBasicFeature &s_star,
s.print(vpFeaturePointPolar::selectRho()); // print only the rho component
\endcode
*/
-void
-vpFeaturePointPolar::print(const unsigned int select ) const
+void vpFeaturePointPolar::print(const unsigned int select) const
{
- std::cout <<"Point: Z=" << get_Z() ;
- if (vpFeaturePointPolar::selectRho() & select )
- std::cout << " rho=" << get_rho() ;
- if (vpFeaturePointPolar::selectTheta() & select )
- std::cout << " theta=" << get_theta() ;
- std::cout <<std::endl ;
+ std::cout << "Point: Z=" << get_Z();
+ if (vpFeaturePointPolar::selectRho() & select)
+ std::cout << " rho=" << get_rho();
+ if (vpFeaturePointPolar::selectTheta() & select)
+ std::cout << " theta=" << get_theta();
+ std::cout << std::endl;
}
+/*!
-/*!
-
Build a 2D image point visual feature with polar coordinates.
\param rho, theta : Polar coordinates \f$(\rho,\theta)\f$ of
@@ -508,38 +467,32 @@ 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 ;
- s[1] = theta ;
+ s[0] = rho;
+ s[1] = theta;
- this->Z = Z_ ;
+ this->Z = Z_;
- if (Z < 0)
- {
- vpERROR_TRACE("Point is behind the camera ") ;
- std::cout <<"Z = " << Z << std::endl ;
+ 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 ;
+ 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++) flags[i] = true;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Display image point feature.
@@ -550,26 +503,22 @@ vpFeaturePointPolar::buildFrom(const double rho, const double theta,
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeaturePointPolar::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
try {
- double rho,theta;
- rho = get_rho();
+ double rho, theta;
+ rho = get_rho();
theta = get_theta();
- double x,y;
- x = rho*cos(theta);
- y = rho*sin(theta);
+ double x, y;
+ x = rho * cos(theta);
+ y = rho * sin(theta);
vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
@@ -583,32 +532,28 @@ vpFeaturePointPolar::display(const vpCameraParameters &cam,
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeaturePointPolar::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeaturePointPolar::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
try {
- double rho,theta;
+ double rho, theta;
rho = get_rho();
theta = get_theta();
- double x,y;
- x = rho*cos(theta);
- y = rho*sin(theta);
+ double x, y;
+ x = rho * cos(theta);
+ y = rho * sin(theta);
vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...) {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
-
+
Create an object with the same type.
\code
@@ -621,7 +566,7 @@ vpFeaturePointPolar::display(const vpCameraParameters &cam,
vpFeaturePointPolar *vpFeaturePointPolar::duplicate() const
{
vpFeaturePointPolar *feature = new vpFeaturePointPolar;
- return feature ;
+ return feature;
}
/*!
@@ -641,14 +586,13 @@ vpFeaturePointPolar *vpFeaturePointPolar::duplicate() const
vpFeaturePointPolar p;
vpServo task;
...
- // Add only the rho subset coordinate feature from an image point to the task
- task.addFeature(p, vpFeaturePointPolar::selectRho());
+ // Add only the rho subset coordinate feature from an image point to the
+ task task.addFeature(p, vpFeaturePointPolar::selectRho());
\endcode
\sa selectTheta()
*/
-unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0] ; }
-
+unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0]; }
/*!
@@ -667,10 +611,9 @@ unsigned int vpFeaturePointPolar::selectRho() { return FEATURE_LINE[0] ; }
vpFeaturePointPolar p;
vpServo task;
...
- // Add only the theta subset coordinate feature from an image point to the task
- task.addFeature(p, vpFeaturePointPolar::selectTheta());
- \endcode
+ // Add only the theta subset coordinate feature from an image point to the
+ task task.addFeature(p, vpFeaturePointPolar::selectTheta()); \endcode
\sa selectRho()
*/
-unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1] ; }
+unsigned int vpFeaturePointPolar::selectTheta() { return FEATURE_LINE[1]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp
index 32d556e..9e9782f 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureSegment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,14 +37,13 @@
*
*****************************************************************************/
-
-#include <visp3/visual_features/vpBasicFeature.h>
-#include <visp3/visual_features/vpFeatureSegment.h>
+#include <cmath>
+#include <visp3/core/vpDisplay.h>
#include <visp3/core/vpImagePoint.h>
-#include <visp3/core/vpMeterPixelConversion.h>
#include <visp3/core/vpMath.h>
-#include <visp3/core/vpDisplay.h>
-#include <cmath>
+#include <visp3/core/vpMeterPixelConversion.h>
+#include <visp3/visual_features/vpBasicFeature.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
// Exception
#include <visp3/core/vpException.h>
@@ -51,37 +51,37 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-
/*!
\file vpFeatureSegment.cpp
\brief class that defines the vpFeatureSegment visual feature
*/
-/*!
+/*!
Initialise the memory space requested for segment visual
feature.
*/
-void
-vpFeatureSegment::init()
+void vpFeatureSegment::init()
{
- //feature dimension
- dim_s = 4 ;
+ // feature dimension
+ dim_s = 4;
nbParameters = 6;
// memory allocation
- s.resize(dim_s) ;
+ s.resize(dim_s);
if (flags == NULL)
flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
}
-/*!
+/*!
Default constructor that builds an empty segment visual feature.
- \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
- If false, use non normalized features \f${\bf s} = (x_c, y_c, l_c, \alpha)\f$.
+ \param normalized : If true, use normalized features \f${\bf s} = (x_n, y_n,
+ l_n, \alpha)\f$. If false, use non normalized features \f${\bf s} = (x_c,
+ y_c, l_c, \alpha)\f$.
*/
vpFeatureSegment::vpFeatureSegment(bool normalized)
: xc_(0), yc_(0), l_(0), alpha_(0), Z1_(0), Z2_(0), cos_a_(0), sin_a_(0), normalized_(normalized)
@@ -94,8 +94,8 @@ vpFeatureSegment::vpFeatureSegment(bool normalized)
subset of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
- The interaction matrix of the non normalized feature set is of the following form:
- \f[
+ The interaction matrix of the non normalized feature set is of the following
+form: \f[
{\bf L} = \left[
\begin{array}{c}
L_{x_c} \\
@@ -128,27 +128,28 @@ vpFeatureSegment::vpFeatureSegment(bool normalized)
\right]
\f]
- with \f$ \lambda_1 = \frac{Z_1 - Z_2}{Z_1 Z_2}\f$ and \f$ \lambda_2 = \frac{Z_1 + Z_2}{2 Z_1 Z_2}\f$
- where \f$Z_i\f$ are the depths of the points.
+ with \f$ \lambda_1 = \frac{Z_1 - Z_2}{Z_1 Z_2}\f$ and \f$ \lambda_2 =
+\frac{Z_1 + Z_2}{2 Z_1 Z_2}\f$ where \f$Z_i\f$ are the depths of the points.
\param select : Selection of a subset of the possible segment features.
- - To compute the interaction matrix for all the four
+ - To compute the interaction matrix for all the four
subset features \f$(x_c \f$, \f$ y_c \f$, \f$ l \f$, \f$ \alpha)\f$ or
- \f$(x_n \f$, \f$ y_n \f$, \f$ l_n \f$, \f$ \alpha)\f$ use vpBasicFeature::FEATURE_ALL. In
- that case the dimension of the interaction matrix is \f$ [4 \times 6] \f$.
+ \f$(x_n \f$, \f$ y_n \f$, \f$ l_n \f$, \f$ \alpha)\f$ use
+vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
+matrix is \f$ [4 \times 6] \f$.
- To compute the interaction matrix for only one of the subset
use one of the following functions:
- selectXc(), selectYc(), selectL(), selectAlpha(). In that case, the returned
- interaction matrix is of dimension \f$ [1 \times 6] \f$ .
+ selectXc(), selectYc(), selectL(), selectAlpha(). In that case, the
+returned interaction matrix is of dimension \f$ [1 \times 6] \f$ .
\return The interaction matrix computed from the segment features.
- The code below shows how to compute the interaction matrix associated to
+ The code below shows how to compute the interaction matrix associated to
the visual feature \f${\bf s} = (x_c, y_c, l, \alpha)\f$.
\code
-#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
int main()
{
@@ -161,8 +162,7 @@ int main()
p1.changeFrame(cMo);
p2.changeFrame(cMo);
// Compute the coordinates of the points in the image plane by perspective projection
- p1.project();
- p2.project();
+ p1.project(); p2.project();
// Build the segment visual feature
vpFeatureSegment s;
@@ -176,8 +176,8 @@ int main()
In this case, L is a 4 by 6 matrix.
It is also possible to build the interaction matrix associated to
- one of the possible features. The code below shows how to modify the previous code to consider as
- visual feature \f$s = (l, \alpha)\f$.
+ one of the possible features. The code below shows how to modify the
+previous code to consider as visual feature \f$s = (l, \alpha)\f$.
\code
vpMatrix L = s.interaction( vpFeatureSegment::selectL() | vpFeatureSegment::selectAlpha() );
@@ -185,38 +185,40 @@ int main()
In that case, L is a 2 by 6 matrix.
*/
-vpMatrix
-vpFeatureSegment::interaction( const unsigned int select )
+vpMatrix vpFeatureSegment::interaction(const unsigned int select)
{
- vpMatrix L ;
- L.resize(0,6) ;
+ vpMatrix L;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but xc was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but xc "
+ "was not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Yc was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Yc "
+ "was not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but l was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but l was "
+ "not set yet");
+ break;
case 3:
- vpTRACE("Warning !!! The interaction matrix is computed but alpha was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but alpha "
+ "was not set yet");
+ break;
case 4:
- vpTRACE("Warning !!! The interaction matrix is computed but Z1 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Z1 "
+ "was not set yet");
+ break;
case 5:
- vpTRACE("Warning !!! The interaction matrix is computed but Z2 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Z2 "
+ "was not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -224,12 +226,11 @@ vpFeatureSegment::interaction( const unsigned int select )
}
}
- //This version is a simplification
- double lambda1 = (Z1_-Z2_)/(Z1_*Z2_); // -l * lambda
- double lambda2 = (Z1_+Z2_)/(2*Z1_*Z2_); // 1/Zm
-
- if (normalized_)
- {
+ // This version is a simplification
+ double lambda1 = (Z1_ - Z2_) / (Z1_ * Z2_); // -l * lambda
+ double lambda2 = (Z1_ + Z2_) / (2 * Z1_ * Z2_); // 1/Zm
+
+ if (normalized_) {
// here var xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
double xn = xc_;
double yn = yc_;
@@ -238,107 +239,106 @@ vpFeatureSegment::interaction( const unsigned int select )
double Zn_inv = lambda2 * ln;
double lc = cos_a_ / ln;
double ls = sin_a_ / ln;
- double xnalpha = xn*cos_a_+yn*sin_a_;
- double lnc = cos_a_ * ln;
- double lns = sin_a_ * ln;
-
- if (vpFeatureSegment::selectXc() & select ){
- vpMatrix Lxn(1,6);
- Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
- Lxn[0][1] = lambda * xn * sin_a_ ;
- Lxn[0][2] = lambda1 * (xn*xnalpha - cos_a_ /4.);
- Lxn[0][3] = sin_a_*cos_a_/4/ln - xn*xnalpha*sin_a_/ln;
- Lxn[0][4] = -ln*(1.+lc*lc/4.) + xn*xnalpha*cos_a_/ln ;
- Lxn[0][5] = yn ;
- L = vpMatrix::stack(L, Lxn) ;
+ double xnalpha = xn * cos_a_ + yn * sin_a_;
+ double lnc = cos_a_ * ln;
+ double lns = sin_a_ * ln;
+
+ if (vpFeatureSegment::selectXc() & select) {
+ vpMatrix Lxn(1, 6);
+ Lxn[0][0] = -Zn_inv + lambda * xn * cos_a_;
+ Lxn[0][1] = lambda * xn * sin_a_;
+ Lxn[0][2] = lambda1 * (xn * xnalpha - cos_a_ / 4.);
+ Lxn[0][3] = sin_a_ * cos_a_ / 4 / ln - xn * xnalpha * sin_a_ / ln;
+ Lxn[0][4] = -ln * (1. + lc * lc / 4.) + xn * xnalpha * cos_a_ / ln;
+ Lxn[0][5] = yn;
+ L = vpMatrix::stack(L, Lxn);
}
- if (vpFeatureSegment::selectYc() & select ){
- vpMatrix Lyn(1,6);
- Lyn[0][0] = lambda*yn*cos_a_ ;
- Lyn[0][1] = -Zn_inv + lambda*yn*sin_a_ ;
- Lyn[0][2] = lambda1 * (yn*xnalpha - sin_a_/4.);
- Lyn[0][3] = ln*(1+ls*ls/4.)-yn*xnalpha*sin_a_/ln ;
- Lyn[0][4] = -sin_a_*cos_a_/4/ln + yn*xnalpha*cos_a_/ln;
- Lyn[0][5] = -xn ;
- L = vpMatrix::stack(L, Lyn) ;
+ if (vpFeatureSegment::selectYc() & select) {
+ vpMatrix Lyn(1, 6);
+ Lyn[0][0] = lambda * yn * cos_a_;
+ Lyn[0][1] = -Zn_inv + lambda * yn * sin_a_;
+ Lyn[0][2] = lambda1 * (yn * xnalpha - sin_a_ / 4.);
+ Lyn[0][3] = ln * (1 + ls * ls / 4.) - yn * xnalpha * sin_a_ / ln;
+ Lyn[0][4] = -sin_a_ * cos_a_ / 4 / ln + yn * xnalpha * cos_a_ / ln;
+ Lyn[0][5] = -xn;
+ L = vpMatrix::stack(L, Lyn);
}
- if (vpFeatureSegment::selectL() & select ){
- vpMatrix Lln(1,6);
- Lln[0][0] = lambda * lnc ;
- Lln[0][1] = lambda * lns ;
- Lln[0][2] = -(Zn_inv + lambda*xnalpha);
- Lln[0][3] = -yn-xnalpha*sin_a_ ;
- Lln[0][4] = xn + xnalpha*cos_a_ ;
- Lln[0][5] = 0 ;
- L = vpMatrix::stack(L, Lln) ;
+ if (vpFeatureSegment::selectL() & select) {
+ vpMatrix Lln(1, 6);
+ Lln[0][0] = lambda * lnc;
+ Lln[0][1] = lambda * lns;
+ Lln[0][2] = -(Zn_inv + lambda * xnalpha);
+ Lln[0][3] = -yn - xnalpha * sin_a_;
+ Lln[0][4] = xn + xnalpha * cos_a_;
+ Lln[0][5] = 0;
+ L = vpMatrix::stack(L, Lln);
}
- if (vpFeatureSegment::selectAlpha() & select ){
- // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains 1/l
- vpMatrix Lalpha(1,6);
- Lalpha[0][0] = -lambda1*sin_a_*l_ ;
- Lalpha[0][1] = lambda1*cos_a_*l_ ;
- Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_);
- Lalpha[0][3] = (-xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_)/l_;
- Lalpha[0][4] = (xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_)/l_ ;
- Lalpha[0][5] = -1 ;
- L = vpMatrix::stack(L,Lalpha) ;
+ if (vpFeatureSegment::selectAlpha() & select) {
+ // We recall that xc_ contains xc/l, yc_ contains yc/l and l_ contains
+ // 1/l
+ vpMatrix Lalpha(1, 6);
+ Lalpha[0][0] = -lambda1 * sin_a_ * l_;
+ Lalpha[0][1] = lambda1 * cos_a_ * l_;
+ Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_);
+ Lalpha[0][3] = (-xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_) / l_;
+ Lalpha[0][4] = (xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_) / l_;
+ Lalpha[0][5] = -1;
+ L = vpMatrix::stack(L, Lalpha);
}
- }
- else
- {
- if (vpFeatureSegment::selectXc() & select ){
- vpMatrix Lxc(1,6);
- Lxc[0][0] = -lambda2 ;
- Lxc[0][1] = 0. ;
- Lxc[0][2] = lambda2*xc_ - lambda1*l_*cos_a_/4.;
- Lxc[0][3] = xc_*yc_ + l_*l_*cos_a_*sin_a_/4. ;
- Lxc[0][4] = -(1+xc_*xc_+l_*l_*cos_a_*cos_a_/4.) ;
- Lxc[0][5] = yc_ ;
- L = vpMatrix::stack(L,Lxc) ;
+ } else {
+ if (vpFeatureSegment::selectXc() & select) {
+ vpMatrix Lxc(1, 6);
+ Lxc[0][0] = -lambda2;
+ Lxc[0][1] = 0.;
+ Lxc[0][2] = lambda2 * xc_ - lambda1 * l_ * cos_a_ / 4.;
+ Lxc[0][3] = xc_ * yc_ + l_ * l_ * cos_a_ * sin_a_ / 4.;
+ Lxc[0][4] = -(1 + xc_ * xc_ + l_ * l_ * cos_a_ * cos_a_ / 4.);
+ Lxc[0][5] = yc_;
+ L = vpMatrix::stack(L, Lxc);
}
- if (vpFeatureSegment::selectYc() & select ){
- vpMatrix Lyc(1,6);
- Lyc[0][0] = 0. ;
- Lyc[0][1] = -lambda2 ;
- Lyc[0][2] = lambda2*yc_ - lambda1*l_*sin_a_/4.;
- Lyc[0][3] = 1+yc_*yc_+l_*l_*sin_a_*sin_a_/4. ;
- Lyc[0][4] = -xc_*yc_-l_*l_*cos_a_*sin_a_/4. ;
- Lyc[0][5] = -xc_ ;
- L = vpMatrix::stack(L,Lyc) ;
+ if (vpFeatureSegment::selectYc() & select) {
+ vpMatrix Lyc(1, 6);
+ Lyc[0][0] = 0.;
+ Lyc[0][1] = -lambda2;
+ Lyc[0][2] = lambda2 * yc_ - lambda1 * l_ * sin_a_ / 4.;
+ Lyc[0][3] = 1 + yc_ * yc_ + l_ * l_ * sin_a_ * sin_a_ / 4.;
+ Lyc[0][4] = -xc_ * yc_ - l_ * l_ * cos_a_ * sin_a_ / 4.;
+ Lyc[0][5] = -xc_;
+ L = vpMatrix::stack(L, Lyc);
}
- if (vpFeatureSegment::selectL() & select ){
- vpMatrix Ll(1,6);
- Ll[0][0] = lambda1*cos_a_ ;
- Ll[0][1] = lambda1*sin_a_ ;
- Ll[0][2] = lambda2*l_-lambda1*(xc_*cos_a_+yc_*sin_a_);
- Ll[0][3] = l_*(xc_*cos_a_*sin_a_ + yc_*(1+sin_a_*sin_a_)) ;
- Ll[0][4] = -l_*(xc_*(1+cos_a_*cos_a_)+yc_*cos_a_*sin_a_) ;
- Ll[0][5] = 0 ;
- L = vpMatrix::stack(L,Ll) ;
+ if (vpFeatureSegment::selectL() & select) {
+ vpMatrix Ll(1, 6);
+ Ll[0][0] = lambda1 * cos_a_;
+ Ll[0][1] = lambda1 * sin_a_;
+ Ll[0][2] = lambda2 * l_ - lambda1 * (xc_ * cos_a_ + yc_ * sin_a_);
+ Ll[0][3] = l_ * (xc_ * cos_a_ * sin_a_ + yc_ * (1 + sin_a_ * sin_a_));
+ Ll[0][4] = -l_ * (xc_ * (1 + cos_a_ * cos_a_) + yc_ * cos_a_ * sin_a_);
+ Ll[0][5] = 0;
+ L = vpMatrix::stack(L, Ll);
}
- if (vpFeatureSegment::selectAlpha() & select ){
- vpMatrix Lalpha(1,6);
- Lalpha[0][0] = -lambda1*sin_a_/l_ ;
- Lalpha[0][1] = lambda1*cos_a_/l_ ;
- Lalpha[0][2] = lambda1*(xc_*sin_a_-yc_*cos_a_)/l_;
- Lalpha[0][3] = -xc_*sin_a_*sin_a_+yc_*cos_a_*sin_a_;
- Lalpha[0][4] = xc_*cos_a_*sin_a_ - yc_*cos_a_*cos_a_ ;
- Lalpha[0][5] = -1 ;
- L = vpMatrix::stack(L,Lalpha) ;
+ if (vpFeatureSegment::selectAlpha() & select) {
+ vpMatrix Lalpha(1, 6);
+ Lalpha[0][0] = -lambda1 * sin_a_ / l_;
+ Lalpha[0][1] = lambda1 * cos_a_ / l_;
+ Lalpha[0][2] = lambda1 * (xc_ * sin_a_ - yc_ * cos_a_) / l_;
+ Lalpha[0][3] = -xc_ * sin_a_ * sin_a_ + yc_ * cos_a_ * sin_a_;
+ Lalpha[0][4] = xc_ * cos_a_ * sin_a_ - yc_ * cos_a_ * cos_a_;
+ Lalpha[0][5] = -1;
+ L = vpMatrix::stack(L, Lalpha);
}
}
- return L ;
+ return L;
}
/*!
- Computes the error between the current and the desired visual features from a subset
- of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
- or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
+ Computes the error between the current and the desired visual features from
+ a subset of the possible features \f${\bf s} = (x_c, y_c, l, \alpha)\f$ or
+ \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$.
For the angular component \f$\alpha\f$, we define the error as
\f$\alpha \ominus \alpha^*\f$, where \f$\ominus\f$ is modulo \f$2\pi\f$
@@ -352,52 +352,55 @@ vpFeatureSegment::interaction( const unsigned int select )
vpBasicFeature::FEATURE_ALL. In that case the error vector is a 4
dimension column vector.
- To compute the error for only one subfeature of
- \f${\bf s} = (x_c, y_c, l, \alpha)\f$ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ feature set
- use one of the following functions: selectXc(), selectYc(), selectL(), selectAlpha().
+ \f${\bf s} = (x_c, y_c, l, \alpha)\f$ or \f${\bf s} = (x_n, y_n, l_n,
+ \alpha)\f$ feature set use one of the following functions: selectXc(),
+ selectYc(), selectL(), selectAlpha().
\return The error between the current and the desired
visual feature.
*/
-vpColVector
-vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select )
-{
- vpColVector e(0) ;
-
- if (vpFeatureSegment::selectXc() & select ){
- vpColVector exc(1) ;
- exc[0] = xc_-s_star[0];
- e = vpColVector::stack(e,exc) ;
+vpColVector vpFeatureSegment::error(const vpBasicFeature &s_star, const unsigned int select)
+{
+ vpColVector e(0);
+
+ if (vpFeatureSegment::selectXc() & select) {
+ vpColVector exc(1);
+ exc[0] = xc_ - s_star[0];
+ e = vpColVector::stack(e, exc);
}
- if (vpFeatureSegment::selectYc() & select ){
- vpColVector eyc(1) ;
+ if (vpFeatureSegment::selectYc() & select) {
+ vpColVector eyc(1);
eyc[0] = yc_ - s_star[1];
- e = vpColVector::stack(e,eyc) ;
+ e = vpColVector::stack(e, eyc);
}
- if (vpFeatureSegment::selectL() & select ){
- vpColVector eL(1) ;
+ if (vpFeatureSegment::selectL() & select) {
+ vpColVector eL(1);
eL[0] = l_ - s_star[2];
- e = vpColVector::stack(e,eL) ;
+ e = vpColVector::stack(e, eL);
}
- if (vpFeatureSegment::selectAlpha() & select ){
- vpColVector eAlpha(1) ;
+ if (vpFeatureSegment::selectAlpha() & select) {
+ vpColVector eAlpha(1);
eAlpha[0] = alpha_ - s_star[3];
- while (eAlpha[0] < -M_PI) eAlpha[0] += 2*M_PI ;
- while (eAlpha[0] > M_PI) eAlpha[0] -= 2*M_PI ;
- e = vpColVector::stack(e,eAlpha) ;
+ while (eAlpha[0] < -M_PI)
+ eAlpha[0] += 2 * M_PI;
+ while (eAlpha[0] > M_PI)
+ eAlpha[0] -= 2 * M_PI;
+ e = vpColVector::stack(e, eAlpha);
}
- return e ;
+ return e;
}
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
- \param select : Selection of a subset of the possible segement features (\f$ x_c \f$,\f$ y_c \f$,\f$ l \f$,\f$ \alpha \f$).
+ \param select : Selection of a subset of the possible segement features (\f$
+ x_c \f$,\f$ y_c \f$,\f$ l \f$,\f$ \alpha \f$).
- \code
+ \code
s.print();
\endcode
@@ -418,38 +421,37 @@ vpFeatureSegment::error( const vpBasicFeature &s_star, const unsigned int select
vpFeatureSegment: (l = 0.105005; alpha = 92.1305 deg)
\endcode
*/
-void
-vpFeatureSegment::print( const unsigned int select ) const
+void vpFeatureSegment::print(const unsigned int select) const
{
- std::cout <<"vpFeatureSegment: (";
- if (vpFeatureSegment::selectXc() & select ) {
+ std::cout << "vpFeatureSegment: (";
+ if (vpFeatureSegment::selectXc() & select) {
if (normalized_)
std::cout << "xn = ";
else
std::cout << "xc = ";
std::cout << s[0] << "; ";
}
- if (vpFeatureSegment::selectYc() & select ) {
+ if (vpFeatureSegment::selectYc() & select) {
if (normalized_)
std::cout << "yn = ";
else
std::cout << "yc = ";
std::cout << s[1] << "; ";
}
- if (vpFeatureSegment::selectL() & select ) {
+ if (vpFeatureSegment::selectL() & select) {
if (normalized_)
std::cout << "ln = ";
else
std::cout << "l = ";
std::cout << s[2] << "; ";
}
- if (vpFeatureSegment::selectAlpha() & select ) {
+ if (vpFeatureSegment::selectAlpha() & select) {
std::cout << "alpha = " << vpMath::deg(s[3]) << " deg";
}
std::cout << ")" << std::endl;
}
-/*!
+/*!
Create an object with the same type.
\code
@@ -462,9 +464,9 @@ vpFeatureSegment::print( const unsigned int select ) const
vpFeatureSegment *vpFeatureSegment::duplicate() const
{
vpFeatureSegment *feature;
-
- feature = new vpFeatureSegment(*this) ;
- return feature ;
+
+ feature = new vpFeatureSegment(*this);
+ return feature;
}
/*!
@@ -478,30 +480,26 @@ vpFeatureSegment *vpFeatureSegment::duplicate() const
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeatureSegment::display(const vpCameraParameters &cam ,
- const vpImage<unsigned char> & I,
- const vpColor &color,
- unsigned int thickness ) const
+void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I, const vpColor &color,
+ unsigned int thickness) const
{
double l, x, y;
if (normalized_) {
- l = 1./l_;
+ l = 1. / l_;
x = xc_ * l;
y = yc_ * l;
- }
- else {
+ } else {
l = l_;
x = xc_;
y = yc_;
}
- double x1 = x - (l/2.)*cos_a_;
- double x2 = x + (l/2.)*cos_a_;
+ double x1 = x - (l / 2.) * cos_a_;
+ double x2 = x + (l / 2.) * cos_a_;
- double y1 = y - (l/2.)*sin_a_;
- double y2 = y + (l/2.)*sin_a_;
- vpImagePoint ip1,ip2;
+ double y1 = y - (l / 2.) * sin_a_;
+ double y2 = y + (l / 2.) * sin_a_;
+ vpImagePoint ip1, ip2;
vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
@@ -520,37 +518,33 @@ 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<vpRGBa> & I,
- const vpColor &color,
- unsigned int thickness ) const
+void vpFeatureSegment::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
double l, x, y;
if (normalized_) {
- l = 1./l_;
+ l = 1. / l_;
x = xc_ * l;
y = yc_ * l;
- }
- else {
+ } else {
l = l_;
x = xc_;
y = yc_;
}
- double x1 = x - (l/2.)*cos_a_;
- double x2 = x + (l/2.)*cos_a_;
+ double x1 = x - (l / 2.) * cos_a_;
+ double x2 = x + (l / 2.) * cos_a_;
- double y1 = y - (l/2.)*sin_a_;
- double y2 = y + (l/2.)*sin_a_;
- vpImagePoint ip1,ip2;
+ double y1 = y - (l / 2.) * sin_a_;
+ double y2 = y + (l / 2.) * sin_a_;
+ vpImagePoint ip1, ip2;
vpMeterPixelConversion::convertPoint(cam, x1, y1, ip1);
vpMeterPixelConversion::convertPoint(cam, x2, y2, ip2);
vpDisplay::displayLine(I, ip1, ip2, color, thickness);
- vpDisplay::displayCircle(I, ip1, 5,vpColor::cyan, true);
- vpDisplay::displayCircle(I, ip2, 5,vpColor::yellow, true);
-}
+ vpDisplay::displayCircle(I, ip1, 5, vpColor::cyan, true);
+ vpDisplay::displayCircle(I, ip2, 5, vpColor::yellow, true);
+}
/*!
@@ -562,36 +556,34 @@ vpFeatureSegment::display(const vpCameraParameters & cam ,
\param x2, y2 : coordinates of the second point in the image plane.
\param Z2 : depth of the second point in the camera frame.
- Depending on the feature set that is considered, the features \f${\bf s} = (x_c, y_c, l, \alpha)\f$
- or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ are computed from the two points using the
- following formulae:
- \f[ x_c = \frac{x_1 + x_2}{2} \f]
- \f[ y_c = \frac{y_1 + y_2}{2} \f]
- \f[ l = \sqrt{{x_1 - x_2}^2 + {y_1 - y_2}^2} \f]
- \f[ \alpha = arctan(\frac{y_1 - y_2}{x_1 - x_2}) \f]
+ Depending on the feature set that is considered, the features \f${\bf s} =
+ (x_c, y_c, l, \alpha)\f$ or \f${\bf s} = (x_n, y_n, l_n, \alpha)\f$ are
+ computed from the two points using the following formulae: \f[ x_c =
+ \frac{x_1 + x_2}{2} \f] \f[ y_c = \frac{y_1 + y_2}{2} \f] \f[ l = \sqrt{{x_1
+ - x_2}^2 + {y_1 - y_2}^2} \f] \f[ \alpha = arctan(\frac{y_1 - y_2}{x_1 -
+ x_2}) \f]
*/
-void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1,
- const double x2, const double y2, const double Z2)
+void vpFeatureSegment::buildFrom(const double x1, const double y1, const double Z1, const double x2, const double y2,
+ const double Z2)
{
- double l = sqrt( (x1-x2)*(x1-x2) + (y1-y2)*(y1-y2));
- double x_c = (x1+x2)/2.;
- double y_c = (y1+y2)/2.;
- double alpha = atan2(y1-y2,x1-x2);
+ double l = sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
+ double x_c = (x1 + x2) / 2.;
+ double y_c = (y1 + y2) / 2.;
+ double alpha = atan2(y1 - y2, x1 - x2);
if (normalized_) {
- setXc( x_c / l );
- setYc( y_c / l );
- setL( 1/l );
- setAlpha( alpha );
+ setXc(x_c / l);
+ setYc(y_c / l);
+ setL(1 / l);
+ setAlpha(alpha);
setZ1(Z1);
setZ2(Z2);
- }
- else {
- setXc( x_c );
- setYc( y_c );
- setL( l );
- setAlpha( alpha );
+ } else {
+ setXc(x_c);
+ setYc(y_c);
+ setL(l);
+ setAlpha(alpha);
setZ1(Z1);
setZ2(Z2);
@@ -603,7 +595,8 @@ void vpFeatureSegment::buildFrom(const double x1, const double y1, const double
Function used to select the \f$x_c\f$ or \f$x_n\f$ subfeature.
This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$x_c\f$ or \f$x_n\f$ feature.
+ to compute the interaction matrix associated to \f$x_c\f$ or \f$x_n\f$
+feature.
See the interaction() method for an usage example.
@@ -620,14 +613,15 @@ task.addFeature(s, s_star, vpFeatureSegment::selectXc());
\sa selectYc(), selectL(), selectAlpha()
*/
-unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureSegment::selectXc() { return FEATURE_LINE[0]; }
/*!
Function used to select the \f$y_c\f$ or \f$y_n\f$ subfeature.
This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$y_c\f$ or \f$y_n\f$ feature.
+ to compute the interaction matrix associated to \f$y_c\f$ or \f$y_n\f$
+feature.
See the interaction() method for an usage example.
@@ -644,14 +638,15 @@ task.addFeature(s, s_star, vpFeatureSegment::selectYc());
\sa selectXc(), selectL(), selectAlpha()
*/
-unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureSegment::selectYc() { return FEATURE_LINE[1]; }
/*!
Function used to select the \f$l\f$ or \f$l_n\f$ subfeature.
This function is to use in conjunction with interaction() in order
- to compute the interaction matrix associated to \f$l\f$ or \f$l_n\f$ feature.
+ to compute the interaction matrix associated to \f$l\f$ or \f$l_n\f$
+feature.
See the interaction() method for an usage example.
@@ -668,7 +663,7 @@ task.addFeature(s, s_star, vpFeatureSegment::selectL());
\sa selectXc(), selectYc(), selectAlpha()
*/
-unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2] ; }
+unsigned int vpFeatureSegment::selectL() { return FEATURE_LINE[2]; }
/*!
@@ -693,4 +688,4 @@ task.addFeature(s, s_star, vpFeatureSegment::selectAlpha());
\sa selectXc(), selectYc(), selectL()
*/
-unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3] ; }
+unsigned int vpFeatureSegment::selectAlpha() { return FEATURE_LINE[3]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp
index 5aeb26c..2f1c5ae 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureThetaU.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,10 +37,9 @@
*
*****************************************************************************/
-
+#include <visp3/core/vpMath.h>
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/core/vpMath.h>
// Exception
#include <visp3/core/vpException.h>
@@ -48,7 +48,6 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-
/*!
\file vpFeatureThetaU.cpp
\brief class that defines the ThetaU visual feature
@@ -60,23 +59,23 @@ other functionalities are useful but not mandatory
*/
-/*!
+/*!
Initialise the memory space requested for 3D \f$ \theta u \f$ visual
feature.
*/
-void
-vpFeatureThetaU::init()
+void vpFeatureThetaU::init()
{
- //feature dimension
- dim_s = 3 ;
+ // feature dimension
+ dim_s = 3;
nbParameters = 3;
// memory allocation
- s.resize(dim_s) ;
+ s.resize(dim_s);
if (flags == NULL)
flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
}
/*!
@@ -86,25 +85,23 @@ vpFeatureThetaU::init()
setFeatureThetaURotationType() to set the desired type of feature.
*/
-vpFeatureThetaU::vpFeatureThetaU()
- : rotation(vpFeatureThetaU::cdRc)
+vpFeatureThetaU::vpFeatureThetaU() : rotation(vpFeatureThetaU::cdRc)
{
- //vpTRACE("0x%x", this);
- init() ;
+ // vpTRACE("0x%x", this);
+ init();
}
-/*!
+/*!
Constructor that builds a visual feature and initialize it to zero.
\param r [in] : The rotation representation of the \f$ \theta u\f$
visual feature.
*/
-vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
+vpFeatureThetaU::vpFeatureThetaU(vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
- //vpTRACE("0x%x", this);
- init() ;
+ // vpTRACE("0x%x", this);
+ init();
// kind of rotation representation
rotation = r;
@@ -129,13 +126,11 @@ 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() ;
-
- buildFrom(tu) ;
+ init();
+
+ buildFrom(tu);
}
/*!
@@ -155,14 +150,12 @@ vpFeatureThetaU::vpFeatureThetaU(vpThetaUVector &tu,
*/
-vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
- vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
+vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R, vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
- init() ;
-
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
+ init();
+
+ vpThetaUVector tu(R);
+ buildFrom(tu);
}
/*!
@@ -184,15 +177,13 @@ vpFeatureThetaU::vpFeatureThetaU(vpRotationMatrix &R,
*/
-vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
- vpFeatureThetaURotationRepresentationType r)
- : rotation(r)
+vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M, vpFeatureThetaURotationRepresentationType r) : rotation(r)
{
- init() ;
- vpRotationMatrix R ;
- M.extract(R) ;
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
+ init();
+ vpRotationMatrix R;
+ M.extract(R);
+ vpThetaUVector tu(R);
+ buildFrom(tu);
}
/*!
@@ -211,13 +202,13 @@ vpFeatureThetaU::vpFeatureThetaU(vpHomogeneousMatrix &M,
to the desired one (\f$ ^{c}R_{c^*}\f$).
*/
-void
-vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
+void vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
{
- s[0] = tu[0] ;
- s[1] = tu[1] ;
- s[2] = tu[2] ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ s[0] = tu[0];
+ s[1] = tu[1];
+ s[2] = tu[2];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
/*!
@@ -235,11 +226,10 @@ vpFeatureThetaU::buildFrom(vpThetaUVector &tu)
to the desired one (\f$ ^{c}R_{c^*}\f$).
*/
-void
-vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
+void vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
{
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
+ vpThetaUVector tu(R);
+ buildFrom(tu);
}
/*!
@@ -260,27 +250,23 @@ vpFeatureThetaU::buildFrom(const vpRotationMatrix &R)
*/
-void
-vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
+void vpFeatureThetaU::buildFrom(const vpHomogeneousMatrix &M)
{
- vpRotationMatrix R ;
- M.extract(R) ;
- vpThetaUVector tu(R) ;
- buildFrom(tu) ;
+ vpRotationMatrix R;
+ M.extract(R);
+ vpThetaUVector tu(R);
+ buildFrom(tu);
}
/*!
Set the type of rotation feature.
- \param r : type of feature. It can be vpFeatureThetaU::cdRc or vpFeatureThetaU::cRcd.
- \sa getFeatureThetaURotationType()
+ \param r : type of feature. It can be vpFeatureThetaU::cdRc or
+ vpFeatureThetaU::cRcd. \sa getFeatureThetaURotationType()
*/
-void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r)
-{
- rotation = r;
-}
+void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotationRepresentationType r) { rotation = r; }
/*!
@@ -292,8 +278,8 @@ void vpFeatureThetaU::setFeatureThetaURotationType(const vpFeatureThetaURotation
*/
void vpFeatureThetaU::set_TUx(const double tu_x)
{
- s[0] = tu_x ;
- flags[0] = true;
+ s[0] = tu_x;
+ flags[0] = true;
}
/*!
@@ -305,8 +291,8 @@ void vpFeatureThetaU::set_TUx(const double tu_x)
*/
void vpFeatureThetaU::set_TUy(const double tu_y)
{
- s[1] = tu_y ;
- flags[1] = true;
+ s[1] = tu_y;
+ flags[1] = true;
}
/*!
@@ -316,19 +302,18 @@ void vpFeatureThetaU::set_TUy(const double tu_y)
\param tu_z : \f$\theta u_z \f$ subset value to initialize.
\sa get_TUz()
*/
-void
-vpFeatureThetaU::set_TUz(const double tu_z)
+void vpFeatureThetaU::set_TUz(const double tu_z)
{
- s[2] = tu_z ;
- flags[2] = true;
+ s[2] = tu_z;
+ flags[2] = true;
}
/*!
Get the type of rotation feature.
- \return Type of rotation feature. It can be vpFeatureThetaU::cdRc or vpFeatureThetaU::cRcd.
- \sa setFeatureThetaURotationType()
+ \return Type of rotation feature. It can be vpFeatureThetaU::cdRc or
+ vpFeatureThetaU::cRcd. \sa setFeatureThetaURotationType()
*/
vpFeatureThetaU::vpFeatureThetaURotationRepresentationType vpFeatureThetaU::getFeatureThetaURotationType() const
@@ -336,39 +321,27 @@ vpFeatureThetaU::vpFeatureThetaURotationRepresentationType vpFeatureThetaU::getF
return rotation;
}
-/*!
+/*!
- Return the \f$\theta u_x \f$ subset value of the visual feature
+ Return the \f$\theta u_x \f$ subset value of the visual feature
\f$s\f$.
*/
-double vpFeatureThetaU::get_TUx() const
-{
- return s[0] ;
-}
+double vpFeatureThetaU::get_TUx() const { return s[0]; }
/*!
- Return the \f$\theta u_y \f$ subset value of the visual feature
+ Return the \f$\theta u_y \f$ subset value of the visual feature
\f$s\f$.
*/
-double vpFeatureThetaU::get_TUy() const
-{
- return s[1] ;
-}
-
+double vpFeatureThetaU::get_TUy() const { return s[1]; }
/*!
- Return the \f$\theta u_z \f$ subset value of the visual feature
+ Return the \f$\theta u_z \f$ subset value of the visual feature
\f$s\f$.
*/
-double
-vpFeatureThetaU::get_TUz() const
-{
- return s[2] ;
-}
-
+double vpFeatureThetaU::get_TUz() const { return s[2]; }
/*!
@@ -377,28 +350,27 @@ vpFeatureThetaU::get_TUz() const
\f$ \theta u \f$ features that represent the 3D rotation
\f$^{c^*}R_c\f$ or \f$^{c}R_{c^*}\f$, with
- \f[ L = [ 0_3 \; L_{\theta u}] \f]
+ \f[ L = [ 0_3 \; L_{\theta u}] \f]
See the vpFeatureThetaU class description for the equations of
\f$L_{\theta u}\f$.
\param select : Selection of a subset of the possible \f$ \theta u \f$
- features.
- - To compute the interaction matrix for all the three \f$ \theta u \f$
- features use vpBasicFeature::FEATURE_ALL. In that case the dimension of the
- interaction matrix is \f$ [3 \times 6] \f$
- - To compute the interaction matrix for only one of the \f$ \theta u \f$
- component feature (\f$\theta u_x, \theta u_y, \theta u_z\f$) use one of the
- corresponding function selectTUx(), selectTUy() or selectTUz(). In
- that case the returned interaction matrix is \f$ [1 \times 6] \f$
- dimension.
+ features.
+ - To compute the interaction matrix for all the three \f$ \theta u \f$
+ features use vpBasicFeature::FEATURE_ALL. In that case the dimension of
+ the interaction matrix is \f$ [3 \times 6] \f$
+ - To compute the interaction matrix for only one of the \f$ \theta u \f$
+ component feature (\f$\theta u_x, \theta u_y, \theta u_z\f$) use one of
+ the corresponding function selectTUx(), selectTUy() or selectTUz(). In that
+ case the returned interaction matrix is \f$ [1 \times 6] \f$ dimension.
\return The interaction matrix computed from the \f$ \theta u \f$
features that represent either the rotation \f$^{c^*}R_c\f$ or the
rotation \f$^{c}R_{c^*}\f$.
The code below shows how to compute the interaction matrix
- associated to the visual feature \f$s = \theta u_x \f$.
+ associated to the visual feature \f$s = \theta u_x \f$.
\code
vpRotationMatrix cdMc;
@@ -433,29 +405,28 @@ vpFeatureThetaU::get_TUz() const
line corresponds to the \f$ \theta u_z \f$ visual feature.
*/
-vpMatrix
-vpFeatureThetaU::interaction(const unsigned int select)
+vpMatrix vpFeatureThetaU::interaction(const unsigned int select)
{
- vpMatrix L ;
- L.resize(0,6) ;
+ vpMatrix L;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_x was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_x "
+ "was not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_y "
+ "was not set yet");
+ break;
case 2:
- vpTRACE("Warning !!! The interaction matrix is computed but Tu_z was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but Tu_z "
+ "was not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -465,71 +436,75 @@ vpFeatureThetaU::interaction(const unsigned int select)
}
// Lw computed using Lw = [theta/2 u]_x +/- (I + alpha [u]_x [u]_x)
- vpColVector u(3) ;
- for (unsigned int i=0 ; i < 3 ; i++) {
- u[i] = s[i]/2.0 ;
+ vpColVector u(3);
+ for (unsigned int i = 0; i < 3; i++) {
+ u[i] = s[i] / 2.0;
}
-
- vpMatrix Lw(3,3) ;
- Lw = vpColVector::skew(u) ; /* [theta/2 u]_x */
-
- vpMatrix U2(3,3) ;
- U2.eye() ;
-
- double theta = sqrt(s.sumSquare()) ;
+
+ vpMatrix Lw(3, 3);
+ Lw = vpColVector::skew(u); /* [theta/2 u]_x */
+
+ vpMatrix U2(3, 3);
+ U2.eye();
+
+ double theta = sqrt(s.sumSquare());
if (theta >= 1e-6) {
- for (unsigned int i=0 ; i < 3 ; i++)
- u[i] = s[i]/theta ;
+ for (unsigned int i = 0; i < 3; i++)
+ u[i] = s[i] / theta;
- vpMatrix skew_u ;
- skew_u = vpColVector::skew(u) ;
- U2 += (1-vpMath::sinc(theta)/vpMath::sqr(vpMath::sinc(theta/2.0)))*skew_u*skew_u ;
+ vpMatrix skew_u;
+ skew_u = vpColVector::skew(u);
+ U2 += (1 - vpMath::sinc(theta) / vpMath::sqr(vpMath::sinc(theta / 2.0))) * skew_u * skew_u;
}
-
+
if (rotation == cdRc) {
- Lw += U2;
- }
- else {
- Lw -= U2;
+ Lw += U2;
+ } else {
+ Lw -= U2;
}
- //This version is a simplification
- if (vpFeatureThetaU::selectTUx() & select )
- {
- vpMatrix Lx(1,6) ;
-
- Lx[0][0] = 0 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Lx[0][i+3] = Lw[0][i] ;
+ // This version is a simplification
+ if (vpFeatureThetaU::selectTUx() & select) {
+ vpMatrix Lx(1, 6);
+ Lx[0][0] = 0;
+ Lx[0][1] = 0;
+ Lx[0][2] = 0;
+ for (int i = 0; i < 3; i++)
+ Lx[0][i + 3] = Lw[0][i];
- L = vpMatrix::stack(L,Lx) ;
- }
+ L = vpMatrix::stack(L, Lx);
+ }
- if (vpFeatureThetaU::selectTUy() & select )
- {
- vpMatrix Ly(1,6) ;
+ if (vpFeatureThetaU::selectTUy() & select) {
+ vpMatrix Ly(1, 6);
- Ly[0][0] = 0 ; Ly[0][1] = 0 ; Ly[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Ly[0][i+3] = Lw[1][i] ;
+ Ly[0][0] = 0;
+ Ly[0][1] = 0;
+ Ly[0][2] = 0;
+ for (int i = 0; i < 3; i++)
+ Ly[0][i + 3] = Lw[1][i];
- L = vpMatrix::stack(L,Ly) ;
- }
+ L = vpMatrix::stack(L, Ly);
+ }
- if (vpFeatureThetaU::selectTUz() & select )
- {
- vpMatrix Lz(1,6) ;
+ if (vpFeatureThetaU::selectTUz() & select) {
+ vpMatrix Lz(1, 6);
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = 0 ;
- for (int i=0 ; i < 3 ; i++) Lz[0][i+3] = Lw[2][i] ;
+ Lz[0][0] = 0;
+ Lz[0][1] = 0;
+ Lz[0][2] = 0;
+ for (int i = 0; i < 3; i++)
+ Lz[0][i + 3] = Lw[2][i];
- L = vpMatrix::stack(L,Lz) ;
- }
+ L = vpMatrix::stack(L, Lz);
+ }
- return L ;
+ return L;
}
/*!
-
+
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
@@ -545,9 +520,9 @@ vpFeatureThetaU::interaction(const unsigned int select)
\param select : The error can be computed for a selection of a
subset of the possible \f$ \theta u \f$ features.
- To compute the error for all the three \f$ \theta u \f$ features use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
dimension column vector.
- - To compute the error for only one of the \f$ \theta u \f$ component
+ - To compute the error for only one of the \f$ \theta u \f$ component
feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
corresponding function selectTUx(), selectTUy() or selectTUz(). In
that case the error vector is a 1 dimension column vector.
@@ -566,9 +541,9 @@ vpFeatureThetaU::interaction(const unsigned int select)
vpFeatureThetaU s(vpFeatureThetaU::cdRc);
s.set_TUz(0.3);
- // Creation of the desired feature s^*. By default this feature is
+ // Creation of the desired feature s^*. By default this feature is
// initialized to zero
- vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
+ vpFeatureThetaU s_star(vpFeatureThetaU::cdRc);
// Compute the interaction matrix for the ThetaU_z feature
vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
@@ -581,49 +556,42 @@ vpFeatureThetaU::interaction(const unsigned int select)
the code becomes:
\code
// Compute the interaction matrix for the ThetaU_y, ThetaU_z features
- vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz() );
+ vpMatrix L_yz = s.interaction( vpFeatureThetaU::selectTUy() |
+ vpFeatureThetaU::selectTUz() );
// Compute the error vector e = (s-s*) for the ThetaU_y, ThetaU_z feature
vpColVector e = s.error(s_star, vpFeatureThetaU::selectTUy() | vpFeatureThetaU::selectTUz());
\endcode
*/
-vpColVector
-vpFeatureThetaU::error(const vpBasicFeature &s_star,
- const unsigned int select)
+vpColVector vpFeatureThetaU::error(const vpBasicFeature &s_star, const unsigned int select)
{
- if (fabs(s_star.get_s().sumSquare()) > 1e-6)
- {
- vpERROR_TRACE("s* should be zero ! ") ;
- throw(vpFeatureException(vpFeatureException::badInitializationError,
- "s* should be zero !")) ;
- }
-
- vpColVector e(0) ;
+ if (fabs(s_star.get_s().sumSquare()) > 1e-6) {
+ vpERROR_TRACE("s* should be zero ! ");
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
+ }
+ vpColVector e(0);
- if (vpFeatureThetaU::selectTUx() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] ;
- e = vpColVector::stack(e,ex) ;
- }
+ if (vpFeatureThetaU::selectTUx() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0];
+ e = vpColVector::stack(e, ex);
+ }
- if (vpFeatureThetaU::selectTUy() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] ;
- e = vpColVector::stack(e,ey) ;
- }
+ if (vpFeatureThetaU::selectTUy() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1];
+ e = vpColVector::stack(e, ey);
+ }
- if (vpFeatureThetaU::selectTUz() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2] ;
- e = vpColVector::stack(e,ez) ;
- }
- return e ;
+ if (vpFeatureThetaU::selectTUz() & select) {
+ vpColVector ez(1);
+ ez[0] = s[2];
+ e = vpColVector::stack(e, ez);
+ }
+ return e;
}
/*!
@@ -632,8 +600,8 @@ vpFeatureThetaU::error(const vpBasicFeature &s_star,
\param select : Selection of a subset of the possible \f$ \theta u
\f$ features.
- To print all the three \f$ \theta u \f$ features use
- vpBasicFeature::FEATURE_ALL.
- - To print only one of the \f$ \theta u \f$ component
+ vpBasicFeature::FEATURE_ALL.
+ - To print only one of the \f$ \theta u \f$ component
feature (\f$ \theta u_x, \theta u_y, \theta u_z\f$) use one of the
corresponding function selectTUx(), selectTUy() or selectTUz().
@@ -642,7 +610,7 @@ vpFeatureThetaU::error(const vpBasicFeature &s_star,
tu[0] = 0.1;
tu[1] = 0.2;
tu[2] = 0.3;
-
+
// Creation of the current feature s
vpFeatureThetaU s(vpFeatureThetaU::cdRc);
s.buildFrom(tu);
@@ -652,24 +620,23 @@ vpFeatureThetaU::error(const vpBasicFeature &s_star,
s.print(vpFeatureThetaU::selectTUz()); // print only the ThetaU_z component
\endcode
*/
-void
-vpFeatureThetaU::print(const unsigned int select) const
+void vpFeatureThetaU::print(const unsigned int select) const
{
- std::cout <<"ThetaU: ";
- if (vpFeatureThetaU::selectTUx() & select ) {
+ std::cout << "ThetaU: ";
+ if (vpFeatureThetaU::selectTUx() & select) {
std::cout << s[0] << " ";
}
- if (vpFeatureThetaU::selectTUy() & select ) {
+ if (vpFeatureThetaU::selectTUy() & select) {
std::cout << s[1] << " ";
}
- if (vpFeatureThetaU::selectTUz() & select ) {
+ if (vpFeatureThetaU::selectTUz() & select) {
std::cout << s[2] << " ";
}
std::cout << std::endl;
}
/*!
-
+
Create an object with the same type.
\code
@@ -682,12 +649,12 @@ vpFeatureThetaU::print(const unsigned int select) const
vpFeatureThetaU *vpFeatureThetaU::duplicate() const
{
vpFeatureThetaU *feature;
- if (rotation == cdRc)
- feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc) ;
- else //if (rotation == cRcd
- feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd) ;
-
- return feature ;
+ if (rotation == cdRc)
+ feature = new vpFeatureThetaU(vpFeatureThetaU::cdRc);
+ else // if (rotation == cRcd
+ feature = new vpFeatureThetaU(vpFeatureThetaU::cRcd);
+
+ return feature;
}
/*!
@@ -695,39 +662,31 @@ vpFeatureThetaU *vpFeatureThetaU::duplicate() const
Not implemented.
*/
-void
-vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
- }
+ }
}
/*!
Not implemented.
*/
-void
-vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureThetaU::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
@@ -757,7 +716,7 @@ vpFeatureThetaU::display(const vpCameraParameters &/* cam */,
\sa selectTUy(), selectTUz()
*/
-unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0]; }
/*!
Function used to select the \f$ \theta u_y\f$ subset of the \f$
@@ -782,7 +741,7 @@ unsigned int vpFeatureThetaU::selectTUx() { return FEATURE_LINE[0] ; }
\sa selectTUx(), selectTUz()
*/
-unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1]; }
/*!
Function used to select the \f$ \theta u_z\f$ subset of the \f$
@@ -807,4 +766,4 @@ unsigned int vpFeatureThetaU::selectTUy() { return FEATURE_LINE[1] ; }
\sa selectTUx(), selectTUy()
*/
-unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2] ; }
+unsigned int vpFeatureThetaU::selectTUz() { return FEATURE_LINE[2]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp
index 6c6041b..07a2c2d 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureTranslation.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -36,7 +37,6 @@
*
*****************************************************************************/
-
#include <visp3/visual_features/vpBasicFeature.h>
#include <visp3/visual_features/vpFeatureTranslation.h>
@@ -49,7 +49,6 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-
/*!
\file vpFeatureTranslation.cpp
\brief class that defines 3D translation visual feature
@@ -61,24 +60,23 @@ other functionalities are useful but not mandatory
*/
-/*!
+/*!
Initialise the memory space requested for 3D translation visual
feature.
*/
-void
-vpFeatureTranslation::init()
+void vpFeatureTranslation::init()
{
- //feature dimension
- dim_s = 3 ;
+ // feature dimension
+ dim_s = 3;
nbParameters = 1;
// memory allocation
- s.resize(dim_s) ;
+ s.resize(dim_s);
if (flags == NULL)
flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
}
/*!
@@ -87,58 +85,55 @@ 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 specifying the type.
+/*!
+ Default constructor that builds a visual feature and initialize it to zero
+ specifying the type.
- \param r : Type of considered 3D translation feature.
+ \param r : Type of considered 3D translation feature.
*/
-vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r)
- : f2Mf1(), translation(r)
+vpFeatureTranslation::vpFeatureTranslation(vpFeatureTranslationRepresentationType r) : f2Mf1(), translation(r)
{
- init() ;
+ init();
}
-
/*!
Constructor that builds a 3D visual feature from an homogeneous
- matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
+ matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D
+ transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
\param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
- move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
- \param r : type of feature. It can be vpFeature::cdMc or vpFeature::cMo.
+ move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$
+ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$). \param r : type of feature. It can be
+ vpFeature::cdMc or vpFeature::cMo.
*/
vpFeatureTranslation::vpFeatureTranslation(vpHomogeneousMatrix &f2Mf1_, vpFeatureTranslationRepresentationType r)
: f2Mf1(), translation(r)
{
- init() ;
+ init();
- buildFrom(f2Mf1_) ;
+ buildFrom(f2Mf1_);
}
/*!
Build a 3D translation visual feature from an homogeneous
- matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
+ matrix \f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$ that represent the 3D
+ transformation between two frames \f${\cal{F}}_1\f$ and \f${\cal{F}}_2\f$.
\param f2Mf1_ [in] : 3D displacement that the camera has to achieve to
- move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
+ move from the frame \f${\cal{F}}_2\f$ to the frame \f${\cal{F}}_1\f$ (\f$
+ ^{{\cal{F}}_2}M_{{\cal{F}}_1} \f$).
*/
-void
-vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_)
+void vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_)
{
- this->f2Mf1 = f2Mf1_ ;
- s[0] = f2Mf1[0][3] ;
- s[1] = f2Mf1[1][3] ;
- s[2] = f2Mf1[2][3] ;
+ this->f2Mf1 = f2Mf1_;
+ s[0] = f2Mf1[0][3];
+ s[1] = f2Mf1[1][3];
+ s[2] = f2Mf1[2][3];
flags[0] = true;
}
@@ -147,14 +142,14 @@ vpFeatureTranslation::buildFrom(const vpHomogeneousMatrix &f2Mf1_)
Set the type of translation feature.
- \param r : type of translation feature. It can be vpFeatureTranslation::cdMc, vpFeatureTranslation::cMcd
- or vpFeatureTranslation::cMo.
- \sa getFeatureTranslationType()
+ \param r : type of translation feature. It can be
+ vpFeatureTranslation::cdMc, vpFeatureTranslation::cMcd or
+ vpFeatureTranslation::cMo. \sa getFeatureTranslationType()
*/
void vpFeatureTranslation::setFeatureTranslationType(const vpFeatureTranslationRepresentationType r)
{
- translation = r;
+ translation = r;
}
/*!
@@ -166,10 +161,7 @@ void vpFeatureTranslation::setFeatureTranslationType(const vpFeatureTranslationR
\sa get_Tx()
*/
-void vpFeatureTranslation::set_Tx(const double t_x)
-{
- s[0] = t_x ;
-}
+void vpFeatureTranslation::set_Tx(const double t_x) { s[0] = t_x; }
/*!
Initialise the \f$t_y \f$ subset value of the 3D
@@ -179,10 +171,7 @@ void vpFeatureTranslation::set_Tx(const double t_x)
\sa get_Ty()
*/
-void vpFeatureTranslation::set_Ty(const double t_y)
-{
- s[1] = t_y ;
-}
+void vpFeatureTranslation::set_Ty(const double t_y) { s[1] = t_y; }
/*!
Initialise the \f$t_z \f$ subset value of the 3D
@@ -192,60 +181,42 @@ void vpFeatureTranslation::set_Ty(const double t_y)
\sa get_Tz()
*/
-void
-vpFeatureTranslation::set_Tz(const double t_z)
-{
- s[2] = t_z ;
-}
-
+void vpFeatureTranslation::set_Tz(const double t_z) { s[2] = t_z; }
/*!
Get the type of translation feature.
- \return Type of translation feature. It can be vpFeatureTranslation::cdMc, vpFeatureTranslation::cMcd
- or vpFeatureTranslation::cMo.
- \sa setFeatureTranslationType()
+ \return Type of translation feature. It can be vpFeatureTranslation::cdMc,
+ vpFeatureTranslation::cMcd or vpFeatureTranslation::cMo. \sa
+ setFeatureTranslationType()
*/
-vpFeatureTranslation::vpFeatureTranslationRepresentationType vpFeatureTranslation::getFeatureTranslationType() const
+vpFeatureTranslation::vpFeatureTranslationRepresentationType vpFeatureTranslation::getFeatureTranslationType() const
{
- return translation ;
+ return translation;
}
/*!
- Return the \f$t_x \f$ subset value of the visual feature
+ Return the \f$t_x \f$ subset value of the visual feature
\f$s\f$.
*/
-double vpFeatureTranslation::get_Tx() const
-{
- return s[0] ;
-}
-
+double vpFeatureTranslation::get_Tx() const { return s[0]; }
/*!
- Return the \f$t_y \f$ subset value of the visual feature
+ Return the \f$t_y \f$ subset value of the visual feature
\f$s\f$.
*/
-double vpFeatureTranslation::get_Ty() const
-{
- return s[1] ;
-}
-
+double vpFeatureTranslation::get_Ty() const { return s[1]; }
/*!
- Return the \f$t_z \f$ subset value of the visual feature
+ Return the \f$t_z \f$ subset value of the visual feature
\f$s\f$.
*/
-double
-vpFeatureTranslation::get_Tz() const
-{
- return s[2] ;
-}
-
+double vpFeatureTranslation::get_Tz() const { return s[2]; }
/*!
@@ -258,7 +229,7 @@ vpFeatureTranslation::get_Tz() const
- With the feature type cdMc:
- \f[ L = [ ^{c^*}R_c \;\; 0_3] \f]
+ \f[ L = [ ^{c^*}R_c \;\; 0_3] \f]
where \f$^{c^*}R_c\f$ is the rotation the camera has to achieve to
move from the desired camera frame to the current camera frame.
@@ -278,7 +249,7 @@ vpFeatureTranslation::get_Tz() const
the object frame relative to the current camera frame.
\param select : Selection of a subset of the possible translation
- features.
+ features.
- To compute the interaction matrix for all the three translation
subset features \f$(t_x,t_y,t_y)\f$ use vpBasicFeature::FEATURE_ALL. In
that case the dimension of the interaction matrix is \f$ [3 \times
@@ -297,7 +268,7 @@ vpFeatureTranslation::get_Tz() const
\code
vpHomogeneousMatrix cdMc;
- ...
+ ...
// Creation of the current feature s
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.buildFrom(cdMc);
@@ -328,20 +299,20 @@ vpFeatureTranslation::get_Tz() const
line corresponds to the \f$ t_z \f$ visual feature.
*/
-vpMatrix
-vpFeatureTranslation::interaction(const unsigned int select)
+vpMatrix vpFeatureTranslation::interaction(const unsigned int select)
{
- vpMatrix L ;
- L.resize(0,6) ;
+ vpMatrix L;
+ L.resize(0, 6);
if (deallocate == vpBasicFeature::user) {
for (unsigned int i = 0; i < nbParameters; i++) {
if (flags[i] == false) {
- switch(i){
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but f2Mf1 "
+ "was not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -351,92 +322,122 @@ vpFeatureTranslation::interaction(const unsigned int select)
}
if (translation == cdMc) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
+ // This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select) {
+ vpMatrix Lx(1, 6);
- for (int i=0 ; i < 3 ; i++)
- Lx[0][i] = f2Mf1[0][i] ;
- Lx[0][3] = 0 ; Lx[0][4] = 0 ; Lx[0][5] = 0 ;
+ for (int i = 0; i < 3; i++)
+ Lx[0][i] = f2Mf1[0][i];
+ Lx[0][3] = 0;
+ Lx[0][4] = 0;
+ Lx[0][5] = 0;
- L = vpMatrix::stack(L,Lx) ;
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
+ if (vpFeatureTranslation::selectTy() & select) {
+ vpMatrix Ly(1, 6);
- for (int i=0 ; i < 3 ; i++)
- Ly[0][i] = f2Mf1[1][i] ;
- Ly[0][3] = 0 ; Ly[0][4] = 0 ; Ly[0][5] = 0 ;
+ for (int i = 0; i < 3; i++)
+ Ly[0][i] = f2Mf1[1][i];
+ Ly[0][3] = 0;
+ Ly[0][4] = 0;
+ Ly[0][5] = 0;
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
+ if (vpFeatureTranslation::selectTz() & select) {
+ vpMatrix Lz(1, 6);
- for (int i=0 ; i < 3 ; i++)
- Lz[0][i] = f2Mf1[2][i] ;
- Lz[0][3] = 0 ; Lz[0][4] = 0 ; Lz[0][5] = 0 ;
+ for (int i = 0; i < 3; i++)
+ Lz[0][i] = f2Mf1[2][i];
+ Lz[0][3] = 0;
+ Lz[0][4] = 0;
+ Lz[0][5] = 0;
- L = vpMatrix::stack(L,Lz) ;
+ L = vpMatrix::stack(L, Lz);
}
}
if (translation == cMcd) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
- Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
-
- L = vpMatrix::stack(L,Lx) ;
+ // This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select) {
+ vpMatrix Lx(1, 6);
+ Lx[0][0] = -1;
+ Lx[0][1] = 0;
+ Lx[0][2] = 0;
+ Lx[0][3] = 0;
+ Lx[0][4] = -s[2];
+ Lx[0][5] = s[1];
+
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
- Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
- Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
+ if (vpFeatureTranslation::selectTy() & select) {
+ vpMatrix Ly(1, 6);
+ Ly[0][0] = 0;
+ Ly[0][1] = -1;
+ Ly[0][2] = 0;
+ Ly[0][3] = s[2];
+ Ly[0][4] = 0;
+ Ly[0][5] = -s[0];
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
- Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
+ if (vpFeatureTranslation::selectTz() & select) {
+ vpMatrix Lz(1, 6);
+ Lz[0][0] = 0;
+ Lz[0][1] = 0;
+ Lz[0][2] = -1;
+ Lz[0][3] = -s[1];
+ Lz[0][4] = s[0];
+ Lz[0][5] = 0;
- L = vpMatrix::stack(L,Lz) ;
+ L = vpMatrix::stack(L, Lz);
}
}
if (translation == cMo) {
- //This version is a simplification
- if (vpFeatureTranslation::selectTx() & select ) {
- vpMatrix Lx(1,6) ;
- Lx[0][0] = -1 ; Lx[0][1] = 0 ; Lx[0][2] = 0 ;
- Lx[0][3] = 0 ; Lx[0][4] = -s[2] ; Lx[0][5] = s[1] ;
-
- L = vpMatrix::stack(L,Lx) ;
+ // This version is a simplification
+ if (vpFeatureTranslation::selectTx() & select) {
+ vpMatrix Lx(1, 6);
+ Lx[0][0] = -1;
+ Lx[0][1] = 0;
+ Lx[0][2] = 0;
+ Lx[0][3] = 0;
+ Lx[0][4] = -s[2];
+ Lx[0][5] = s[1];
+
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeatureTranslation::selectTy() & select ) {
- vpMatrix Ly(1,6) ;
- Ly[0][0] = 0 ; Ly[0][1] = -1 ; Ly[0][2] = 0 ;
- Ly[0][3] = s[2] ; Ly[0][4] = 0 ; Ly[0][5] = -s[0] ;
+ if (vpFeatureTranslation::selectTy() & select) {
+ vpMatrix Ly(1, 6);
+ Ly[0][0] = 0;
+ Ly[0][1] = -1;
+ Ly[0][2] = 0;
+ Ly[0][3] = s[2];
+ Ly[0][4] = 0;
+ Ly[0][5] = -s[0];
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- if (vpFeatureTranslation::selectTz() & select ) {
- vpMatrix Lz(1,6) ;
- Lz[0][0] = 0 ; Lz[0][1] = 0 ; Lz[0][2] = -1 ;
- Lz[0][3] = -s[1] ; Lz[0][4] = s[0] ; Lz[0][5] = 0 ;
+ if (vpFeatureTranslation::selectTz() & select) {
+ vpMatrix Lz(1, 6);
+ Lz[0][0] = 0;
+ Lz[0][1] = 0;
+ Lz[0][2] = -1;
+ Lz[0][3] = -s[1];
+ Lz[0][4] = s[0];
+ Lz[0][5] = 0;
- L = vpMatrix::stack(L,Lz) ;
+ L = vpMatrix::stack(L, Lz);
}
}
- return L ;
+ return L;
}
/*!
@@ -444,20 +445,21 @@ vpFeatureTranslation::interaction(const unsigned int select)
visual features from a subset of the possible features.
- With the feature type cdMc:
- Since this visual feature \f$ s \f$ represent the 3D translation from the desired
- camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
+ Since this visual feature \f$ s \f$ represent the 3D translation from the
+ desired camera frame to the current one \f$^{c^*}t_{c} \f$, the desired
visual feature \f$ s^* \f$ should be zero. Thus, the error is here
equal to the current visual feature \f$ s \f$.
- With the feature type cMo:
- In this case the desired feature is not necessary equal to zero. Thus, the error is here equal to \f$ s-s^* \f$.
+ In this case the desired feature is not necessary equal to zero. Thus, the
+ error is here equal to \f$ s-s^* \f$.
\param s_star : Desired visual feature.
\param select : The error can be computed for a selection of a
subset of the possible translation features.
- To compute the error for all the three translation vector coordinates use
- vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
+ vpBasicFeature::FEATURE_ALL. In that case the error vector is a 3
dimension column vector.
- To compute the error for only one of the translation vector coordinate
feature \f$(t_x, t_y, t_z)\f$ use one of the
@@ -468,19 +470,23 @@ vpFeatureTranslation::interaction(const unsigned int select)
visual feature.
\exception vpFeatureException::badInitializationError : If the
- desired visual feature \f$ s^* \f$ is not equal to zero in the case of the feature type is cdMc or cMcd.
+ desired visual feature \f$ s^* \f$ is not equal to zero in the case of the
+ feature type is cdMc or cMcd.
The code below shows how to use this method to manipulate the \f$
- t_z \f$ subset in the case of the cdMc feature type. It can be used also with the cMo feature type. In that case just change vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the declaration of the two vpFeatureTranslation features.
+ t_z \f$ subset in the case of the cdMc feature type. It can be used also
+ with the cMo feature type. In that case just change
+ vpFeatureTranslation::cdMc by vpFeatureTranslation::cMo during the
+ declaration of the two vpFeatureTranslation features.
\code
// Creation of the current feature s
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.set_TUz(0.3); // Initialization of the feature
- // Creation of the desired feature s*. By default this feature is
+ // Creation of the desired feature s*. By default this feature is
// initialized to zero
- vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
+ vpFeatureTranslation s_star(vpFeatureTranslation::cdMc);
// Compute the interaction matrix for the t_z translation feature
vpMatrix L_z = s.interaction( vpFeatureTranslation::selectTz() );
@@ -500,45 +506,36 @@ vpFeatureTranslation::interaction(const unsigned int select)
\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 !")) ;
+ vpColVector e(0);
+
+ if (translation == cdMc || translation == cMcd) {
+ if (s_star.get_s().sumSquare() > 1e-6) {
+ vpERROR_TRACE("s* should be zero ! ");
+ throw(vpFeatureException(vpFeatureException::badInitializationError, "s* should be zero !"));
}
}
+ if (vpFeatureTranslation::selectTx() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0] - s_star[0];
+ e = vpColVector::stack(e, ex);
+ }
- if (vpFeatureTranslation::selectTx() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0]-s_star[0] ;
- e = vpColVector::stack(e,ex) ;
- }
-
- if (vpFeatureTranslation::selectTy() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1]-s_star[1] ;
- e = vpColVector::stack(e,ey) ;
- }
+ if (vpFeatureTranslation::selectTy() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1] - s_star[1];
+ e = vpColVector::stack(e, ey);
+ }
- if (vpFeatureTranslation::selectTz() & select )
- {
- vpColVector ez(1) ;
- ez[0] = s[2]-s_star[2] ;
- e = vpColVector::stack(e,ez) ;
- }
+ if (vpFeatureTranslation::selectTz() & select) {
+ vpColVector ez(1);
+ ez[0] = s[2] - s_star[2];
+ e = vpColVector::stack(e, ez);
+ }
- return e ;
+ return e;
}
/*!
@@ -546,14 +543,15 @@ vpFeatureTranslation::error(const vpBasicFeature &s_star,
\param select : Selection of a subset of the possible translation features.
- To print all the three translation vector coordinates used as features use
- vpBasicFeature::FEATURE_ALL.
+ vpBasicFeature::FEATURE_ALL.
- To print only one of the translation coordinate
feature \f$(t_x, t_y, t_z)\f$ use one of the
corresponding function selectTx(), selectTy() or selectTz().
\code
- vpHomogeneousMatrix cdMc; // Homogeneous transformation between the desired camera frame and the current camera frame.
-
+ vpHomogeneousMatrix cdMc; // Homogeneous transformation between the desired
+ camera frame and the current camera frame.
+
// Creation of the current feature s
vpFeatureTranslation s(vpFeatureTranslation::cdMc);
s.buildFrom(cdMc);
@@ -563,31 +561,31 @@ vpFeatureTranslation::error(const vpBasicFeature &s_star,
s.print(vpFeatureTranslation::selectTz()); // print only the t_z component
\endcode
*/
-void
-vpFeatureTranslation::print(const unsigned int select) const
+void vpFeatureTranslation::print(const unsigned int select) const
{
- std::cout <<"Translation 3D: ";
- if (vpFeatureTranslation::selectTx() & select ) {
+ std::cout << "Translation 3D: ";
+ if (vpFeatureTranslation::selectTx() & select) {
std::cout << s[0] << " ";
}
- if (vpFeatureTranslation::selectTy() & select ) {
+ if (vpFeatureTranslation::selectTy() & select) {
std::cout << s[1] << " ";
}
- if (vpFeatureTranslation::selectTz() & select ) {
+ if (vpFeatureTranslation::selectTz() & select) {
std::cout << s[2] << " ";
}
std::cout << std::endl;
}
-
/*!
-
+
Create an object with the same type.
\code
vpBasicFeature *s_star;
- vpFeatureTranslation s(vpFeatureTranslation::cdMc); //or vpFeatureTranslation s(vpFeatureTranslation::cMo);
- s_star = s.duplicate(); // s_star is now a vpFeatureTranslation
+ vpFeatureTranslation s(vpFeatureTranslation::cdMc); //or
+ vpFeatureTranslation s(vpFeatureTranslation::cMo);
+ s_star = s.duplicate();
+ // s_star is now a vpFeatureTranslation
\endcode
*/
@@ -595,58 +593,49 @@ vpFeatureTranslation *vpFeatureTranslation::duplicate() const
{
vpFeatureTranslation *feature = NULL;
if (translation == cdMc)
- feature = new vpFeatureTranslation(cdMc) ;
+ feature = new vpFeatureTranslation(cdMc);
if (translation == cMo)
- feature = new vpFeatureTranslation(cMo) ;
+ feature = new vpFeatureTranslation(cMo);
if (translation == cMcd)
- feature = new vpFeatureTranslation(cMcd) ;
- return feature ;
+ feature = new vpFeatureTranslation(cMcd);
+ return feature;
}
-
/*!
Not implemented.
*/
-void
-vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
+ static int firsttime = 0;
+
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
}
/*!
Not implemented.
*/
-void
-vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpFeatureTranslation::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
-
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
- // Do not throw and error since it is not subject
- // to produce a failure
- }
+ static int firsttime = 0;
+
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
+ // Do not throw and error since it is not subject
+ // to produce a failure
+ }
}
-
+
/*!
Function used to select the \f$ t_x\f$ subset of the translation
@@ -691,7 +680,7 @@ vpFeatureTranslation::display(const vpCameraParameters &/* cam */,
\sa selectTy(), selectTz()
*/
-unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0] ; }
+unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0]; }
/*!
@@ -736,7 +725,7 @@ unsigned int vpFeatureTranslation::selectTx() { return FEATURE_LINE[0] ; }
\sa selectTx(), selectTz()
*/
-unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1]; }
/*!
@@ -781,4 +770,4 @@ unsigned int vpFeatureTranslation::selectTy() { return FEATURE_LINE[1] ; }
\sa selectTx(), selectTy()
*/
-unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2] ; }
+unsigned int vpFeatureTranslation::selectTz() { return FEATURE_LINE[2]; }
diff --git a/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp
index 00a6bd2..b7623d0 100644
--- a/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp
+++ b/modules/visual_features/src/visual-feature/vpFeatureVanishingPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*! \file vpFeatureVanishingPoint.cpp
\brief Class that defines 2D vanishing
point visual feature (Z coordinate in 3D space is infinity)
@@ -55,89 +55,68 @@
#include <visp3/core/vpFeatureDisplay.h>
-void
-vpFeatureVanishingPoint::init()
-{
- //feature dimension
- dim_s = 2 ;
- nbParameters = 2;
-
- // memory allocation
- s.resize(dim_s) ;
- if (flags == NULL)
- flags = new bool[nbParameters];
- for (unsigned int i = 0; i < nbParameters; i++) flags[i] = false;
-
- //Z not required (infinity)
- //set_Z(1) ;
-
-}
-vpFeatureVanishingPoint::vpFeatureVanishingPoint()
+void vpFeatureVanishingPoint::init()
{
- init() ;
+ // feature dimension
+ dim_s = 2;
+ nbParameters = 2;
+
+ // memory allocation
+ s.resize(dim_s);
+ if (flags == NULL)
+ flags = new bool[nbParameters];
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = false;
+
+ // Z not required (infinity)
+ // set_Z(1) ;
}
-
+vpFeatureVanishingPoint::vpFeatureVanishingPoint() { init(); }
//! set the point x-coordinates
-void
-vpFeatureVanishingPoint::set_x(const double _x)
+void vpFeatureVanishingPoint::set_x(const double _x)
{
- s[0] = _x ;
- flags[0] = true;
+ s[0] = _x;
+ flags[0] = true;
}
//! get the point x-coordinates
-double
-vpFeatureVanishingPoint::get_x() const
-{
- return s[0] ;
-}
+double vpFeatureVanishingPoint::get_x() const { return s[0]; }
//! set the point y-coordinates
-void
-vpFeatureVanishingPoint::set_y(const double _y)
+void vpFeatureVanishingPoint::set_y(const double _y)
{
- s[1] = _y ;
- flags[1] = true;
+ s[1] = _y;
+ flags[1] = true;
}
//! get the point y-coordinates
-double
-vpFeatureVanishingPoint::get_y() const
-{
- return s[1] ;
-}
-
+double vpFeatureVanishingPoint::get_y() const { return s[1]; }
//! set the point xy coordinates
-void
-vpFeatureVanishingPoint::set_xy(const double _x,
- const double _y)
+void vpFeatureVanishingPoint::set_xy(const double _x, const double _y)
{
- set_x(_x) ;
- set_y(_y) ;
+ set_x(_x);
+ set_y(_y);
}
-
//! compute the interaction matrix from a subset of the possible features
-vpMatrix
-vpFeatureVanishingPoint::interaction(const unsigned int select)
+vpMatrix vpFeatureVanishingPoint::interaction(const unsigned int select)
{
- vpMatrix L ;
+ vpMatrix L;
- L.resize(0,6) ;
+ L.resize(0, 6);
- if (deallocate == vpBasicFeature::user)
- {
- for (unsigned int i = 0; i < nbParameters; i++)
- {
- if (flags[i] == false)
- {
- switch(i){
+ if (deallocate == vpBasicFeature::user) {
+ for (unsigned int i = 0; i < nbParameters; i++) {
+ if (flags[i] == false) {
+ switch (i) {
case 0:
- vpTRACE("Warning !!! The interaction matrix is computed but x was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but x was "
+ "not set yet");
+ break;
case 1:
- vpTRACE("Warning !!! The interaction matrix is computed but y was not set yet");
- break;
+ vpTRACE("Warning !!! The interaction matrix is computed but y was "
+ "not set yet");
+ break;
default:
vpTRACE("Problem during the reading of the variable flags");
}
@@ -146,95 +125,84 @@ vpFeatureVanishingPoint::interaction(const unsigned int select)
resetFlags();
}
- double x = get_x() ;
- double y = get_y() ;
+ double x = get_x();
+ double y = get_y();
- if (vpFeatureVanishingPoint::selectX() & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
+ if (vpFeatureVanishingPoint::selectX() & select) {
+ vpMatrix Lx(1, 6);
+ Lx = 0;
- Lx[0][0] = 0. ;
- Lx[0][1] = 0. ;
- Lx[0][2] = 0. ;
- Lx[0][3] = x*y ;
- Lx[0][4] = -(1+x*x) ;
- Lx[0][5] = y ;
+ Lx[0][0] = 0.;
+ Lx[0][1] = 0.;
+ Lx[0][2] = 0.;
+ Lx[0][3] = x * y;
+ Lx[0][4] = -(1 + x * x);
+ Lx[0][5] = y;
- L = vpMatrix::stack(L,Lx) ;
+ L = vpMatrix::stack(L, Lx);
}
- if (vpFeatureVanishingPoint::selectY() & select )
- {
- vpMatrix Ly(1,6) ; Ly = 0;
+ if (vpFeatureVanishingPoint::selectY() & select) {
+ vpMatrix Ly(1, 6);
+ Ly = 0;
- Ly[0][0] = 0 ;
- Ly[0][1] = 0. ;
- Ly[0][2] = 0. ;
- Ly[0][3] = 1+y*y ;
- Ly[0][4] = -x*y ;
- Ly[0][5] = -x ;
+ Ly[0][0] = 0;
+ Ly[0][1] = 0.;
+ Ly[0][2] = 0.;
+ Ly[0][3] = 1 + y * y;
+ Ly[0][4] = -x * y;
+ Ly[0][5] = -x;
- L = vpMatrix::stack(L,Ly) ;
+ L = vpMatrix::stack(L, Ly);
}
- return L ;
+ return L;
}
-
-/*! compute the error between two visual features from a subset of the possible
- features
+/*! 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) ;
+ vpColVector e(0);
- try{
- if (vpFeatureVanishingPoint::selectX() & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[0] - s_star[0] ;
+ try {
+ if (vpFeatureVanishingPoint::selectX() & select) {
+ vpColVector ex(1);
+ ex[0] = s[0] - s_star[0];
- e = vpColVector::stack(e,ex) ;
+ e = vpColVector::stack(e, ex);
}
- if (vpFeatureVanishingPoint::selectY() & select )
- {
- vpColVector ey(1) ;
- ey[0] = s[1] - s_star[1] ;
- e = vpColVector::stack(e,ey) ;
+ if (vpFeatureVanishingPoint::selectY() & select) {
+ vpColVector ey(1);
+ ey[0] = s[1] - s_star[1];
+ e = vpColVector::stack(e, ey);
}
+ } catch (...) {
+ throw;
}
- catch(...) {
- throw ;
- }
- return e ;
+ return e;
}
-
-
-void
-vpFeatureVanishingPoint::print(const unsigned int select ) const
+void vpFeatureVanishingPoint::print(const unsigned int select) const
{
- std::cout <<"Point: " <<std::endl;
- if (vpFeatureVanishingPoint::selectX() & select )
- std::cout << " x=" << get_x() ;
- if (vpFeatureVanishingPoint::selectY() & select )
- std::cout << " y=" << get_y() ;
- std::cout <<std::endl ;
+ std::cout << "Point: " << std::endl;
+ if (vpFeatureVanishingPoint::selectX() & select)
+ std::cout << " x=" << get_x();
+ if (vpFeatureVanishingPoint::selectY() & select)
+ std::cout << " y=" << get_y();
+ std::cout << std::endl;
}
-
-void
-vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
+void vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
{
- s[0] = _x ;
- s[1] = _y ;
- for(unsigned int i = 0; i < nbParameters; i++) flags[i] = true;
+ s[0] = _x;
+ s[1] = _y;
+ for (unsigned int i = 0; i < nbParameters; i++)
+ flags[i] = true;
}
-
/*!
Display vanishing point feature.
@@ -245,24 +213,19 @@ vpFeatureVanishingPoint::buildFrom(const double _x, const double _y)
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ const vpColor &color, unsigned int thickness) const
{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
+ try {
+ double x, y;
+ x = get_x();
+ y = get_y();
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
/*!
@@ -275,35 +238,29 @@ vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
\param thickness : Thickness of the feature representation.
*/
-void
-vpFeatureVanishingPoint::display(const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- const vpColor &color,
- unsigned int thickness) const
+void vpFeatureVanishingPoint::display(const vpCameraParameters &cam, const vpImage<vpRGBa> &I, const vpColor &color,
+ unsigned int thickness) const
{
- try{
- double x,y ;
- x = get_x() ;
- y = get_y() ;
+ try {
+ double x, y;
+ x = get_x();
+ y = get_y();
- vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness) ;
+ vpFeatureDisplay::displayPoint(x, y, cam, I, color, thickness);
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-
/*! for memory issue (used by the vpServo class only)
*/
vpFeatureVanishingPoint *vpFeatureVanishingPoint::duplicate() const
{
- vpFeatureVanishingPoint *feature = new vpFeatureVanishingPoint ;
- return feature ;
+ vpFeatureVanishingPoint *feature = new vpFeatureVanishingPoint;
+ return feature;
}
-unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0] ; }
-unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1] ; }
+unsigned int vpFeatureVanishingPoint::selectX() { return FEATURE_LINE[0]; }
+unsigned int vpFeatureVanishingPoint::selectY() { return FEATURE_LINE[1]; }
diff --git a/modules/visual_features/src/visual-feature/vpGenericFeature.cpp b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp
index 5f6742c..ab8800b 100644
--- a/modules/visual_features/src/visual-feature/vpGenericFeature.cpp
+++ b/modules/visual_features/src/visual-feature/vpGenericFeature.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,10 +36,8 @@
*
*****************************************************************************/
-
#include <visp3/visual_features/vpGenericFeature.h>
-
// Exception
#include <visp3/core/vpException.h>
#include <visp3/visual_features/vpFeatureException.h>
@@ -46,26 +45,17 @@
// Debug trace
#include <visp3/core/vpDebug.h>
-
-
/*!
\file vpGenericFeature.cpp
- Class that defines what is a generic feature. This class could be used to create new
- features not implemented in ViSP.
+ Class that defines what is a generic feature. This class could be used to
+ create new features not implemented in ViSP.
*/
-vpGenericFeature::~vpGenericFeature()
-{
+vpGenericFeature::~vpGenericFeature() {}
-}
+void vpGenericFeature::init() { s = 0; }
-void vpGenericFeature::init()
-{
- s = 0 ;
-}
-
-
-/*!
+/*!
Default constructor. You are not allowed to use this
constructor. Please use the vpGenericFeature::vpGenericFeature(int
@@ -75,30 +65,29 @@ void vpGenericFeature::init()
this constructor.
*/
-vpGenericFeature::vpGenericFeature()
- : L(), err(), errorStatus(errorNotInitalized)
+vpGenericFeature::vpGenericFeature() : L(), err(), errorStatus(errorNotInitalized)
{
/*
vpERROR_TRACE("You are not allow to use this constructor ") ;
vpERROR_TRACE("Please, use vpGenericFeature::vpGenericFeature(int _dim) "
- "constructor") ;
+ "constructor") ;
vpERROR_TRACE("And provide the dimension of the visual feature ") ;
throw(vpException(vpException::cannotUseConstructorError,
- "You are not allow to use this constructor ")) ;
+ "You are not allow to use this constructor ")) ;
*/
}
-
/*!
- Constructor of the class you have to use. The feature table is initilialized with the good dimension.
+ Constructor of the class you have to use. The feature table is initilialized
+ with the good dimension.
- \param dimension_gen_s : Dimension of the generic feature. It corresponds to the number of features you want to create.
+ \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) ;
+ this->dim_s = dimension_gen_s;
+ s.resize(dimension_gen_s);
}
/*!
@@ -110,23 +99,18 @@ vpGenericFeature::vpGenericFeature(unsigned int dimension_gen_s)
\exception vpFeatureException::sizeMismatchError : If the size of
the error vector is bad.
*/
-void
-vpGenericFeature::setError(const vpColVector &error_vector)
+void vpGenericFeature::setError(const vpColVector &error_vector)
{
- if (error_vector.getRows() != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between error dimension"
+ "and feature dimension"));
}
- errorStatus = errorInitialized ;
- err = error_vector ;
+ errorStatus = errorInitialized;
+ err = error_vector;
}
-
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
visual features from a subset of the possible features.
@@ -138,8 +122,8 @@ vpGenericFeature::setError(const vpColVector &error_vector)
vpGenericFeature::serError is not used in the loop then an exception is
thrown
- obviously if vpGenericFeature::setError is not used then s_star is considered
- and this warning is meaningless.
+ obviously if vpGenericFeature::setError is not used then s_star is
+ considered and this warning is meaningless.
\param s_star : Desired visual feature.
@@ -182,70 +166,51 @@ vpGenericFeature::setError(const vpColVector &error_vector)
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)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s* dimension "
+ "and feature dimension"));
}
- vpColVector e(0) ;
+ vpColVector e(0);
- try
- {
- if (errorStatus == errorHasToBeUpdated)
- {
+ try {
+ if (errorStatus == errorHasToBeUpdated) {
vpERROR_TRACE("Error has no been updated since last iteration"
- "you should have used vpGenericFeature::setError"
- "in you visual servoing loop") ;
+ "you should have used vpGenericFeature::setError"
+ "in you visual servoing loop");
throw(vpFeatureException(vpFeatureException::badErrorVectorError,
- "Error has no been updated since last iteration"));
+ "Error has no been updated since last iteration"));
+ } else if (errorStatus == errorInitialized) {
+ vpDEBUG_TRACE(25, "Error init: e=e.");
+ errorStatus = errorHasToBeUpdated;
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ vpColVector ex(1);
+ ex[i] = err[i];
+
+ e = vpColVector::stack(e, ex);
+ }
+ } else {
+ vpDEBUG_TRACE(25, "Error not init: e=s-s*.");
+
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ vpColVector ex(1);
+ ex[0] = s[i] - s_star[i];
+
+ e = vpColVector::stack(e, ex);
+ }
}
- else
- if (errorStatus == errorInitialized)
- {
- vpDEBUG_TRACE(25,"Error init: e=e.");
- errorStatus = errorHasToBeUpdated ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = err[i] ;
-
- e = vpColVector::stack(e,ex) ;
- }
- }
- else
- {
- vpDEBUG_TRACE(25,"Error not init: e=s-s*.");
-
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[0] = s[i] - s_star[i] ;
-
- e = vpColVector::stack(e,ex) ;
- }
-
- }
- }
- catch(...) {
+ } catch (...) {
throw;
}
- return e ;
-
+ return e;
}
-
-
/*!
Compute the error \f$ (s-s^*)\f$ between the current and the desired
@@ -286,63 +251,49 @@ vpGenericFeature::error(const vpBasicFeature &s_star,
s.error(vpBasicFeature::FEATURE_LINE[0] | vpBasicFeature::FEATURE_LINE[1]);
\endcode
*/
-vpColVector
-vpGenericFeature::error( const unsigned int select)
+vpColVector vpGenericFeature::error(const unsigned int select)
{
- vpColVector e(0) ;
+ vpColVector e(0);
- try
- {
- if (errorStatus == errorHasToBeUpdated)
- {
+ try {
+ if (errorStatus == errorHasToBeUpdated) {
vpERROR_TRACE("Error has no been updated since last iteration"
- "you should have used vpGenericFeature::setError"
- "in you visual servoing loop") ;
+ "you should have used vpGenericFeature::setError"
+ "in you visual servoing loop");
throw(vpFeatureException(vpFeatureException::badErrorVectorError,
- "Error has no been updated since last iteration"));
+ "Error has no been updated since last iteration"));
+ } else if (errorStatus == errorInitialized) {
+ errorStatus = errorHasToBeUpdated;
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ vpColVector ex(1);
+ ex[i] = err[i];
+
+ e = vpColVector::stack(e, ex);
+ }
+ } else {
+
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ vpColVector ex(1);
+ ex[i] = s[i];
+
+ e = vpColVector::stack(e, ex);
+ }
}
- else
- if (errorStatus == errorInitialized)
- {
- errorStatus = errorHasToBeUpdated ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = err[i] ;
-
- e = vpColVector::stack(e,ex) ;
- }
- }
- else
- {
-
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpColVector ex(1) ;
- ex[i] = s[i] ;
-
- e = vpColVector::stack(e,ex) ;
- }
-
- }
- }
- catch(...) {
+ } catch (...) {
throw;
}
- return e ;
-
+ return e;
}
-
/*!
Compute and return the interaction matrix \f$ L \f$ for the whole
features or a part of them.
- \param select : Selection of a subset of the possible features.
+ \param select : Selection of a subset of the possible features.
- To compute the interaction matrix for all the features use
vpBasicFeature::FEATURE_ALL. In that case the dimension of the interaction
matrix is \f$ [number of features \times 6] \f$
@@ -368,8 +319,8 @@ vpGenericFeature::error( const unsigned int select)
vpGenericFeature s(3);
s.set_s(0, 0, 0);
- // Here you have to compute the interaction matrix L for all the three features
- s.setInteractionMatrix(L);
+ // Here you have to compute the interaction matrix L for all the three
+ features s.setInteractionMatrix(L);
vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0] );
\endcode
@@ -387,42 +338,37 @@ vpGenericFeature::error( const unsigned int select)
vpMatrix L_x = s.interaction( vpBasicFeature::FEATURE_LINE[0]|vpBasicFeature::FEATURE_LINE[1] );
\endcode
*/
-vpMatrix
-vpGenericFeature::interaction(const unsigned int select)
+vpMatrix vpGenericFeature::interaction(const unsigned int select)
{
- if (L.getRows() == 0)
- {
- std::cout << "interaction matrix " << L << std::endl ;
+ if (L.getRows() == 0) {
+ std::cout << "interaction matrix " << L << std::endl;
vpERROR_TRACE("Interaction has not been initialized");
- std::cout << "A possible reason (may be) is that you have set" << std::endl ;
- std::cout << "the interaction matrix for s and compute a control " << std::endl ;
- std::cout << "with Ls=s* (default) or vice versa" << std::endl ;
-
- throw(vpFeatureException(vpFeatureException::notInitializedError,
- "size mismatch between s* dimension "
- "and feature dimension"));
+ std::cout << "A possible reason (may be) is that you have set" << std::endl;
+ std::cout << "the interaction matrix for s and compute a control " << std::endl;
+ std::cout << "with Ls=s* (default) or vice versa" << std::endl;
+ throw(vpFeatureException(vpFeatureException::notInitializedError, "size mismatch between s* dimension "
+ "and feature dimension"));
}
- vpMatrix Ls ;
+ vpMatrix Ls;
- Ls.resize(0,6) ;
+ Ls.resize(0, 6);
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- vpMatrix Lx(1,6) ; Lx = 0;
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ vpMatrix Lx(1, 6);
+ Lx = 0;
- for (int j=0 ; j < 6 ; j++)
- Lx[0][j] = L[i][j] ;
+ for (int j = 0; j < 6; j++)
+ Lx[0][j] = L[i][j];
- Ls = vpMatrix::stack(Ls,Lx) ;
+ Ls = vpMatrix::stack(Ls, Lx);
}
- return Ls ;
+ return Ls;
}
-
/*!
\brief set the value of the interaction matrix.
@@ -432,73 +378,66 @@ vpGenericFeature::interaction(const unsigned int select)
matrix is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
+void vpGenericFeature::setInteractionMatrix(const vpMatrix &L_)
{
- if (L_.getRows() != dim_s)
- {
- std::cout << L_.getRows() <<" " << dim_s << std::endl ;;
+ if (L_.getRows() != dim_s) {
+ std::cout << L_.getRows() << " " << dim_s << std::endl;
+ ;
vpERROR_TRACE("size mismatch between interaction matrix size "
- "and feature dimension");
- throw(vpFeatureException(vpFeatureException::sizeMismatchError,
- "size mismatch between interaction matrix size "
- "and feature dimension"));
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between interaction matrix size "
+ "and feature dimension"));
}
- this->L = L_ ;
+ this->L = L_;
}
/*!
\brief set the value of all the features.
- \param s_vector : It is a vector containing the value of the visual features.
+ \param s_vector : It is a vector containing the value of the visual
+ features.
\exception an exception is thrown if the number of row of the vector s
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::set_s(const vpColVector &s_vector)
+void vpGenericFeature::set_s(const vpColVector &s_vector)
{
- if (s_vector.getRows() != dim_s)
- {
+ 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"));
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
+ "and feature dimension"));
}
- this->s = s_vector ;
+ this->s = s_vector;
}
-
/*!
\brief get the value of all the features.
- \param s_vector : It is a vector which will contain the value of the visual features.
+ \param s_vector : It is a vector which will contain the value of the visual
+ features.
\exception an exception is thrown if the number of row of the vector s
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::get_s(vpColVector &s_vector) const
+void vpGenericFeature::get_s(vpColVector &s_vector) const
{
- if (s_vector.getRows() != dim_s)
- {
+ 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"));
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between s dimension"
+ "and feature dimension"));
}
- s_vector = this->s ;
+ s_vector = this->s;
}
-
/*!
- \brief set the value of three features if the number of feature is equal to 3.
+ \brief set the value of three features if the number of feature is equal
+ to 3.
\param s0 : value of the first visual feature
@@ -510,25 +449,23 @@ 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)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
- s[0] = s0 ; s[1] = s1 ; s[2] = s2 ;
+ s[0] = s0;
+ s[1] = s1;
+ s[2] = s2;
}
-
/*!
- \brief get the value of three features if the number of feature is equal to 3.
+ \brief get the value of three features if the number of feature is equal
+ to 3.
\param s0 : value of the first visual feature
@@ -540,23 +477,20 @@ vpGenericFeature::set_s(const double s0, const double s1, const double s2)
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
+void vpGenericFeature::get_s(double &s0, double &s1, double &s2) const
{
- if (3 != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
- s0 = s[0] ; s1 = s[1] ; s2 = s[2] ;
+ s0 = s[0];
+ s1 = s[1];
+ s2 = s[2];
}
-
/*!
\brief set the value of two features if the number of feature is equal to 2.
@@ -568,23 +502,19 @@ vpGenericFeature::get_s(double &s0, double &s1, double &s2) 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)
+void vpGenericFeature::set_s(const double s0, const double s1)
{
- if (2 != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
- s[0] = s0 ; s[1] = s1 ;
+ s[0] = s0;
+ s[1] = s1;
}
-
/*!
\brief get the value of two features if the number of feature is equal to 2.
@@ -596,23 +526,19 @@ vpGenericFeature::set_s(const double s0, const double s1)
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::get_s(double &s0, double &s1) const
+void vpGenericFeature::get_s(double &s0, double &s1) const
{
- if (2 != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
- s0 = s[0] ; s1 = s[1] ;
+ s0 = s[0];
+ s1 = s[1];
}
-
/*!
\brief set the value of one feature if the number of feature is equal to 1.
@@ -622,23 +548,18 @@ vpGenericFeature::get_s(double &s0, double &s1) const
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::set_s(const double s0)
+void vpGenericFeature::set_s(const double s0)
{
- if (1 != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
- s[0] = s0 ;
+ s[0] = s0;
}
-
/*!
\brief get the value of one feature if the number of feature is equal to 1.
@@ -648,23 +569,18 @@ vpGenericFeature::set_s(const double s0)
is different from the dimension of the visual feature as specified
in the constructor
*/
-void
-vpGenericFeature::get_s(double &s0) const
+void vpGenericFeature::get_s(double &s0) const
{
- if (1 != dim_s)
- {
+ 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"));
-
+ "and feature dimension");
+ throw(vpFeatureException(vpFeatureException::sizeMismatchError, "size mismatch between number of parameters"
+ "and feature dimension"));
}
s0 = s[0];
}
-
/*!
Print to stdout the values of the current visual feature \f$ s \f$.
@@ -686,43 +602,37 @@ vpGenericFeature::get_s(double &s0) const
s.print(vpBasicFeature::FEATURE_LINE[0]); // print only the first component
\endcode
*/
-void
-vpGenericFeature::print(const unsigned int select) const
+void vpGenericFeature::print(const unsigned int select) const
{
- std::cout <<"Generic Feature: " ;
- for (unsigned int i=0 ; i < dim_s ; i++)
- if (FEATURE_LINE[i] & select )
- {
- std::cout << " s["<<i << "]=" << s[i] ;
+ std::cout << "Generic Feature: ";
+ for (unsigned int i = 0; i < dim_s; i++)
+ if (FEATURE_LINE[i] & select) {
+ std::cout << " s[" << i << "]=" << s[i];
}
- std::cout <<std::endl ;
+ std::cout << std::endl;
}
vpGenericFeature *vpGenericFeature::duplicate() const
{
- vpGenericFeature *feature= new vpGenericFeature(dim_s) ;
+ vpGenericFeature *feature = new vpGenericFeature(dim_s);
- vpTRACE("dims = %d",dim_s) ;
- return feature ;
+ vpTRACE("dims = %d", dim_s);
+ return feature;
}
/*!
Not implemented.
*/
-void
-vpGenericFeature::display(const vpCameraParameters &/* cam */,
- const vpImage<unsigned char> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<unsigned char> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
@@ -730,18 +640,14 @@ vpGenericFeature::display(const vpCameraParameters &/* cam */,
/*!
Not implemented.
*/
-void
-vpGenericFeature::display(const vpCameraParameters &/* cam */,
- const vpImage<vpRGBa> &/* I */,
- const vpColor &/* color */,
- unsigned int /* thickness */) const
+void vpGenericFeature::display(const vpCameraParameters & /* cam */, const vpImage<vpRGBa> & /* I */,
+ const vpColor & /* color */, unsigned int /* thickness */) const
{
- static int firsttime =0 ;
+ static int firsttime = 0;
- if (firsttime==0)
- {
- firsttime=1 ;
- vpERROR_TRACE("not implemented") ;
+ if (firsttime == 0) {
+ firsttime = 1;
+ vpERROR_TRACE("not implemented");
// Do not throw and error since it is not subject
// to produce a failure
}
diff --git a/modules/visual_features/test/feature/testPoint.cpp b/modules/visual_features/test/feature/testPoint.cpp
index 8db1eec..a14a990 100644
--- a/modules/visual_features/test/feature/testPoint.cpp
+++ b/modules/visual_features/test/feature/testPoint.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,133 +36,132 @@
*
*****************************************************************************/
-
-
/*!
\file testPoint.cpp
\brief Performs various tests on the the point class.
*/
-#include <visp3/core/vpMath.h>
+#include <visp3/core/vpDebug.h>
#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpMath.h>
#include <visp3/core/vpPoint.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/visual_features/vpFeatureException.h>
-#include <visp3/core/vpDebug.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureException.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
-#include <stdlib.h>
#include <stdio.h>
+#include <stdlib.h>
int main()
{
try {
- vpHomogeneousMatrix cMo ;
- cMo[0][3] = 0.1 ;
- cMo[1][3] = 0.2 ;
- cMo[2][3] = 2 ;
-
- vpPoint point ;
- vpTRACE("set point coordinates in the world frame ") ;
- point.setWorldCoordinates(0,0,0) ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the projection ") ;
- point.track(cMo) ;
-
- vpTRACE("coordinates in the world frame ") ;
- std::cout << point.oP.t() << std::endl ;
- vpTRACE("coordinates in the camera frame ") ;
- std::cout << point.cP.t() << std::endl ;
-
- vpTRACE("2D coordinates ") ;
- std::cout<< point.get_x() << " " << point.get_y() << std::endl ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the interaction matrix ") ;
-
- vpFeaturePoint p ;
- vpFeatureBuilder::create(p,point) ;
-
- vpMatrix L ;
- L = p.interaction() ;
- std::cout << L << std::endl ;
-
- vpTRACE("test the interaction matrix select") ;
- vpTRACE("\t only X") ;
- L = p.interaction(vpFeaturePoint::selectX()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t only Y") ;
- L = p.interaction(vpFeaturePoint::selectY()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t X & Y") ;
- L = p.interaction(vpFeaturePoint::selectX() |
- vpFeaturePoint::selectY()) ;
- std::cout << L << std::endl ;
-
- vpTRACE("\t selectAll") ;
- L = p.interaction(vpFeaturePoint::selectAll() ) ;
- std::cout << L << std::endl ;
-
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the error ") ;
-
- try{
- vpFeaturePoint pd ;
- pd.set_x(0) ;
- pd.set_y(0) ;
-
- pd.print() ; std::cout << std::endl ;
- vpColVector e ;
- e = p.error(pd) ;
- std::cout << e << std::endl ;
-
- vpTRACE("test the interaction matrix select") ;
- vpTRACE("\t only X") ;
- e = p.error(pd,vpFeaturePoint::selectX()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t only Y") ;
- e = p.error(pd,vpFeaturePoint::selectY()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t X & Y") ;
- e = p.error(pd,vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
- std::cout << e << std::endl ;
-
- vpTRACE("\t selectAll") ;
- e = p.error(pd,vpFeaturePoint::selectAll() ) ;
- std::cout << e << std::endl ;
+ vpHomogeneousMatrix cMo;
+ cMo[0][3] = 0.1;
+ cMo[1][3] = 0.2;
+ cMo[2][3] = 2;
+
+ vpPoint point;
+ vpTRACE("set point coordinates in the world frame ");
+ point.setWorldCoordinates(0, 0, 0);
+
+ std::cout << "------------------------------------------------------" << std::endl;
+ vpTRACE("test the projection ");
+ point.track(cMo);
+
+ vpTRACE("coordinates in the world frame ");
+ std::cout << point.oP.t() << std::endl;
+ vpTRACE("coordinates in the camera frame ");
+ std::cout << point.cP.t() << std::endl;
+
+ vpTRACE("2D coordinates ");
+ std::cout << point.get_x() << " " << point.get_y() << std::endl;
+
+ std::cout << "------------------------------------------------------" << std::endl;
+ vpTRACE("test the interaction matrix ");
+
+ vpFeaturePoint p;
+ vpFeatureBuilder::create(p, point);
+
+ vpMatrix L;
+ L = p.interaction();
+ std::cout << L << std::endl;
+
+ vpTRACE("test the interaction matrix select");
+ vpTRACE("\t only X");
+ L = p.interaction(vpFeaturePoint::selectX());
+ std::cout << L << std::endl;
+
+ vpTRACE("\t only Y");
+ L = p.interaction(vpFeaturePoint::selectY());
+ std::cout << L << std::endl;
+
+ vpTRACE("\t X & Y");
+ L = p.interaction(vpFeaturePoint::selectX() | vpFeaturePoint::selectY());
+ std::cout << L << std::endl;
+
+ vpTRACE("\t selectAll");
+ L = p.interaction(vpFeaturePoint::selectAll());
+ std::cout << L << std::endl;
+
+ std::cout << "------------------------------------------------------" << std::endl;
+ vpTRACE("test the error ");
+
+ try {
+ vpFeaturePoint pd;
+ pd.set_x(0);
+ pd.set_y(0);
+
+ pd.print();
+ std::cout << std::endl;
+ vpColVector e;
+ e = p.error(pd);
+ std::cout << e << std::endl;
+
+ vpTRACE("test the interaction matrix select");
+ vpTRACE("\t only X");
+ e = p.error(pd, vpFeaturePoint::selectX());
+ std::cout << e << std::endl;
+
+ vpTRACE("\t only Y");
+ e = p.error(pd, vpFeaturePoint::selectY());
+ std::cout << e << std::endl;
+
+ vpTRACE("\t X & Y");
+ e = p.error(pd, vpFeaturePoint::selectX() | vpFeaturePoint::selectY());
+ std::cout << e << std::endl;
+
+ vpTRACE("\t selectAll");
+ e = p.error(pd, vpFeaturePoint::selectAll());
+ std::cout << e << std::endl;
+ } catch (vpFeatureException &me) {
+ std::cout << me << std::endl;
+ } catch (vpException &me) {
+ std::cout << me << std::endl;
}
- catch(vpFeatureException &me){ std::cout << me << std::endl ; }
- catch(vpException &me){ std::cout << me << std::endl ; }
- std::cout <<"------------------------------------------------------"<<std::endl ;
- vpTRACE("test the dimension") ;
- unsigned int dim ;
- dim = p.getDimension() ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("test the dimension with select") ;
- vpTRACE("\t only X") ;
- dim = p.getDimension(vpFeaturePoint::selectX()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t only Y") ;
- dim = p.getDimension(vpFeaturePoint::selectY()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t X & Y") ;
- dim = p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY()) ;
- std::cout << "Dimension = " << dim << std::endl ;
-
- vpTRACE("\t selectAll") ;
- dim = p.getDimension(vpFeaturePoint::selectAll() ) ;
- std::cout << "Dimension = " << dim << std::endl ;
+ std::cout << "------------------------------------------------------" << std::endl;
+ vpTRACE("test the dimension");
+ unsigned int dim;
+ dim = p.getDimension();
+ std::cout << "Dimension = " << dim << std::endl;
+
+ vpTRACE("test the dimension with select");
+ vpTRACE("\t only X");
+ dim = p.getDimension(vpFeaturePoint::selectX());
+ std::cout << "Dimension = " << dim << std::endl;
+
+ vpTRACE("\t only Y");
+ dim = p.getDimension(vpFeaturePoint::selectY());
+ std::cout << "Dimension = " << dim << std::endl;
+
+ vpTRACE("\t X & Y");
+ dim = p.getDimension(vpFeaturePoint::selectX() | vpFeaturePoint::selectY());
+ std::cout << "Dimension = " << dim << std::endl;
+
+ vpTRACE("\t selectAll");
+ dim = p.getDimension(vpFeaturePoint::selectAll());
+ std::cout << "Dimension = " << dim << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vs/CMakeLists.txt b/modules/vs/CMakeLists.txt
index 2d16e43..7a11e68 100644
--- a/modules/vs/CMakeLists.txt
+++ b/modules/vs/CMakeLists.txt
@@ -3,9 +3,10 @@
# This file is part of the ViSP software.
# Copyright (C) 2005 - 2017 by Inria. All rights reserved.
#
-# This software is free software; you can redistribute it and/or
-# modify it under the terms of the GNU General Public License
-# ("GPL") version 2 as published by the Free Software Foundation.
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
# See the file LICENSE.txt at the root directory of this source
# distribution for additional information about the GNU GPL.
#
diff --git a/modules/vs/include/visp3/vs/vpAdaptiveGain.h b/modules/vs/include/visp3/vs/vpAdaptiveGain.h
index f62174c..0e2a79c 100644
--- a/modules/vs/include/visp3/vs/vpAdaptiveGain.h
+++ b/modules/vs/include/visp3/vs/vpAdaptiveGain.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -42,130 +43,168 @@
#ifndef __VP_ADAPTIVE_GAIN_H
#define __VP_ADAPTIVE_GAIN_H
-#include <visp3/core/vpConfig.h>
#include <iostream>
+#include <visp3/core/vpConfig.h>
class vpColVector;
/*!
\class vpAdaptiveGain
-
- \ingroup group_task
-
- \brief Adaptive gain computation.
-
- The formula used to compute the gain is the following :
-
- \f[ \lambda (x) = a * exp (-b*x) + c \f]
-
- where \f$ a \f$, \f$ b \f$ and \f$ c \f$ are constant parameters and \f$ x \f$ is the entry to consider.
-
- The parameters \f$a,b,c\f$ are not set directly. They are computed from three other parameters
- \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ that are more intuitive to tune:
- \f[ a = \lambda(0) - \lambda(\infty) \f]
- \f[ b = {\dot \lambda}(0) / a \f]
- \f[ c = \lambda(\infty) \f]
-
- where \f$ \lambda(0)\f$ represents the gain when \f$x=0\f$, \f$ \lambda(\infty)\f$ represents the gain when \f$x=\infty\f$
- and \f$ {\dot \lambda}(0)\f$ represents the slope of \f$\lambda(x)\f$ when \f$x=0\f$.
-
-*/
-
-class VISP_EXPORT vpAdaptiveGain
-{
-
-public: /* constantes */
- static const double DEFAULT_LAMBDA_ZERO;
- static const double DEFAULT_LAMBDA_INFINITY;
- static const double DEFAULT_LAMBDA_SLOPE;
+ \ingroup group_task
+ \brief Adaptive gain computation.
-private: /* Attributs*/
- /* Coefficient de la fonction de calcul de lambda.
- * lambda (x) = a * exp (-b*x) + c. */
- double coeff_a;
- double coeff_b;
- double coeff_c;
+ As described in \cite Kermorgant14a, a varying gain \f$ \lambda \f$ could be
+used in the visual servoing control law \f[{\bf v}_c = -\lambda {\bf
+L}^{+}_{e} {\bf e}\f] with
- /* Derniere valeur calculee. */
- mutable double lambda;
+ \f[ \lambda (|| {\bf e}||) = (\lambda_0 - \lambda_\infty) e^{ -\frac{
+\lambda'_0}{\lambda_0 - \lambda_\infty}||{\bf e}||} + \lambda_\infty \f]
+ where:
+ - \f$\lambda_0 = \lambda(0)\f$ is the gain in 0, that is for very small
+values of \f$||{\bf e}||\f$
+ - \f$\lambda_\infty = \lambda_{||{\bf e}|| \rightarrow \infty}\lambda(||{\bf
+e}||)\f$ is the gain to infinity, that is for very high values of \f$||{\bf
+e}||\f$
+ - \f$\lambda'_0\f$ is the slope of \f$\lambda\f$ at \f$||{\bf e}|| = 0\f$
-public: /* Methodes*/
+ As described in \ref tutorial-boost-vs, the interest of \ref adaptive_gain
+is to reduce the time to convergence in order to speed up the servo.
- /* --- CONSTRUCTOR -------------------------------------------------------- */
+ 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$.
- vpAdaptiveGain ();
- vpAdaptiveGain (double c);
- vpAdaptiveGain (double gain_at_zero,
- double gain_at_infinity,
- double slope_at_zero);
+\code
+#include <visp3/vs/vpAdaptiveGain.h>
+#include <visp3/vs/vpServo.h>
+int main()
+{
+ vpAdaptiveGain lambda(4, 0.4, 30); // lambda(0)=4, lambda(oo)=0.4 and lambda'(0)=30
- /* --- INIT --------------------------------------------------------------- */
- void initFromConstant (double c);
- void initFromVoid (void);
- void initStandard (double gain_at_zero,
- double gain_at_infinity,
- double slope_at_zero);
+ vpServo servo;
+ servo.setLambda(lambda);
+ while(1) {
- /* --- MODIFIORS ---------------------------------------------------------- */
- double setConstant (void);
+ vpColVector v = servo.computeControlLaw();
+ }
+}
+ \endcode
+ 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.
- /* --- COMPUTE ------------------------------------------------------------ */
- /* \brief Calcule la valeur de lambda au point courrant.
- *
- * Determine la valeur du lambda adaptatif en fonction de la valeur
- * de la norme de la fonction de tache e par extrapolation exponentielle.
- * La fonction est : (en_infini - en_zero) * exp (-pente * ||e|| ) + en_infini.
- * On a bien :
- * - lambda(10^5) = en_infini ;
- * - lambda(0) = en_zero ;
- * - lambda(x ~ 0) ~ - pente * x + en_zero.
- * \param val_e: valeur de la norme de l'erreur.
- * \return: valeur de gain au point courrant.
- */
- double value_const (double x) const;
+\code
+#include <visp3/vs/vpAdaptiveGain.h>
+#include <visp3/vs/vpServo.h>
- /* \brief Calcule la valeur de lambda au point courrant et stockage du
- * resultat.
- *
- * La fonction calcule la valeur de lambda d'apres la valeur de la norme
- * de l'erreur, comme le fait la fonction valeur_const.
- * La fonction non constante stocke de plus le resultat dans this ->lambda.
- * \param val_e: valeur de la norme de l'erreur.
- * \return: valeur de gain au point courrant.
- */
- double value (double x) const;
+int main()
+{
+ vpAdaptiveGain lambda(0.5);
- double limitValue_const (void) const;
+ vpServo servo;
+ servo.setLambda(lambda);
- double limitValue (void) const;
+ while(1) {
- /* --- ACCESSORS ---------------------------------------------------------- */
+ vpColVector v = servo.computeControlLaw();
+ }
+}
+ \endcode
+*/
- /*!
+class VISP_EXPORT vpAdaptiveGain
+{
+public:
+ static const double DEFAULT_LAMBDA_ZERO;
+ static const double DEFAULT_LAMBDA_INFINITY;
+ static const double DEFAULT_LAMBDA_SLOPE;
+
+private:
+ // Coefficient such as lambda (x) = a * exp (-b*x) + c
+ double coeff_a; // \f$ a = \lambda(0) - \lambda(\infty) \f$
+ double coeff_b; // \f$ b = {\dot \lambda}(0) / a \f$
+ double coeff_c; // \f$ c = \lambda(\infty) \f$
+
+ // Last computed value
+ mutable double lambda;
+
+public:
+ /* --- CONSTRUCTOR --------------------------------------------------------
+ */
+
+ vpAdaptiveGain();
+ explicit vpAdaptiveGain(double c);
+ vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero);
+
+ /* --- INIT ---------------------------------------------------------------
+ */
+ void initFromConstant(double c);
+ void initFromVoid(void);
+ void initStandard(double gain_at_zero, double gain_at_infinity, double slope_at_zero);
+
+ /* --- MODIFIORS ----------------------------------------------------------
+ */
+ double setConstant(void);
+
+ /* --- COMPUTE ------------------------------------------------------------
+ */
+ /* \brief Calcule la valeur de lambda au point courrant.
+ *
+ * Determine la valeur du lambda adaptatif en fonction de la valeur
+ * de la norme de la fonction de tache e par extrapolation exponentielle.
+ * La fonction est : (en_infini - en_zero) * exp (-pente * ||e|| ) +
+ * en_infini. On a bien :
+ * - lambda(10^5) = en_infini ;
+ * - lambda(0) = en_zero ;
+ * - lambda(x ~ 0) ~ - pente * x + en_zero.
+ * \param val_e: valeur de la norme de l'erreur.
+ * \return: valeur de gain au point courrant.
+ */
+ double value_const(double x) const;
+
+ /* \brief Calcule la valeur de lambda au point courrant et stockage du
+ * resultat.
+ *
+ * La fonction calcule la valeur de lambda d'apres la valeur de la norme
+ * de l'erreur, comme le fait la fonction valeur_const.
+ * La fonction non constante stocke de plus le resultat dans this ->lambda.
+ * \param val_e: valeur de la norme de l'erreur.
+ * \return: valeur de gain au point courrant.
+ */
+ double value(double x) const;
+
+ double limitValue_const(void) const;
+
+ double limitValue(void) const;
+
+ /* --- ACCESSORS ----------------------------------------------------------
+ */
+
+ /*!
Gets the last adaptive gain value which was stored in the class.
-
- \return It returns the last adaptive gain value which was stored in the class.
- */
- inline double getLastValue (void) const {return this ->lambda;}
-
- double operator() (double x) const;
- /* \brief Lance la fonction valeur avec la norme INFINIE du vecteur. */
- double operator() (const vpColVector & x) const;
+ \return It returns the last adaptive gain value which was stored in the
+ class.
+ */
+ inline double getLastValue(void) const { return this->lambda; }
- /* \brief Idem function limitValue. */
- double operator() (void) const;
+ double operator()(double x) const;
+ /* \brief Lance la fonction valeur avec la norme INFINIE du vecteur. */
+ double operator()(const vpColVector &x) const;
- /* --- IOSTREAM ----------------------------------------------------------- */
+ /* \brief Idem function limitValue. */
+ double operator()(void) const;
- friend VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda);
+ /* --- IOSTREAM -----------------------------------------------------------
+ */
+ friend VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda);
};
-#endif /* __VP_ADAPTIVE_GAIN_H */
+#endif
diff --git a/modules/vs/include/visp3/vs/vpServo.h b/modules/vs/include/visp3/vs/vpServo.h
index 0718fc7..84be04b 100644
--- a/modules/vs/include/visp3/vs/vpServo.h
+++ b/modules/vs/include/visp3/vs/vpServo.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
#ifndef vpServo_H
#define vpServo_H
@@ -51,9 +51,8 @@
#include <visp3/core/vpMatrix.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <visp3/visual_features/vpBasicFeature.h>
-#include <visp3/vs/vpServoException.h>
#include <visp3/vs/vpAdaptiveGain.h>
-
+#include <visp3/vs/vpServoException.h>
/*!
\class vpServo
@@ -67,9 +66,9 @@
destructor ~vpServo() launch an exception
vpServoException::notKilledProperly.
- To learn how to use this class, we suggest first to follow the \ref tutorial-ibvs.
- The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs are also useful
- for advanced usage of this class.
+ To learn how to use this class, we suggest first to follow the \ref
+tutorial-ibvs. The \ref tutorial-simu-robot-pioneer and \ref tutorial-boost-vs
+are also useful for advanced usage of this class.
The example below shows how to build a position-based visual servo
from 3D visual features \f$s=({^{c^*}}t_c,\theta u)\f$. In that
@@ -81,10 +80,10 @@
\code
#include <visp3/core/vpColVector.h>
-#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMatrix.h>
+#include <visp3/visual_features/vpFeatureThetaU.h>
+#include <visp3/visual_features/vpFeatureTranslation.h>
#include <visp3/vs/vpServo.h>
int main()
@@ -105,7 +104,7 @@ int main()
// 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
+ vpFeatureThetaU s_star_tu(vpFeatureThetaU::cdRc); // Default initialization to zero
vpColVector v; // Camera velocity
double error; // Task error
@@ -154,149 +153,155 @@ class VISP_EXPORT vpServo
Choice of the visual servoing control law
*/
public:
- typedef enum
- {
- NONE,
- /*!< No control law is specified. */
- EYEINHAND_CAMERA,
- /*!< Eye in hand visual servoing with the following control law
- \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f]
- where camera velocities are computed. */
- EYEINHAND_L_cVe_eJe,
- /*!< Eye in hand visual servoing with the following control law
- \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVe_eJe,
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVf_fVe_eJe,
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- EYETOHAND_L_cVf_fJe
- /*!< Eye to hand visual servoing with the following control law
- \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f {^f}{\bf J}_e} \right)^{+} {\bf e}\f]
- where joint velocities are computed. */
- } vpServoType;
-
- typedef enum
- {
- CURRENT,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_s \f$computed using the current features \f$\bf s\f$. */
- DESIRED,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired features \f${\bf s}^*\f$. */
- MEAN,
- /*!< In the control law (see vpServo::vpServoType), uses the interaction matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2 \f$. */
- USER_DEFINED
- /*!< In the control law (see vpServo::vpServoType), uses an interaction matrix set by the user. */
- } vpServoIteractionMatrixType;
-
- typedef enum
- {
- TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the transpose instead of the pseudo inverse. */
- PSEUDO_INVERSE /*!< In the control law (see vpServo::vpServoType), uses the pseudo inverse. */
- } vpServoInversionType;
-
- typedef enum
- {
- ALL, /*!< Print all the task information. */
- CONTROLLER, /*!< Print the type of controller law. */
- ERROR_VECTOR, /*!< Print the error vector \f$\bf e = (s-s^*)\f$. */
- FEATURE_CURRENT, /*!< Print the current features \f$\bf s\f$. */
- FEATURE_DESIRED, /*!< Print the desired features \f${\bf s}^*\f$. */
- GAIN, /*!< Print the gain \f$\lambda\f$. */
- INTERACTION_MATRIX, /*!< Print the interaction matrix. */
- MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */
- } vpServoPrintType;
-
-//private:
-//#ifndef DOXYGEN_SHOULD_SKIP_THIS
-// vpServo(const vpServo &)
-// : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
-// rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
-// interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
-// cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
-// errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
-// forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
-// {
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// }
-// vpServo &operator=(const vpServo &){
-// throw vpException(vpException::functionNotImplementedError, "Not implemented!");
-// return *this;
-// }
-//#endif
+ typedef enum {
+ NONE,
+ /*!< No control law is specified. */
+ EYEINHAND_CAMERA,
+ /*!< Eye in hand visual servoing with the following control law
+ \f[{\bf v}_c = -\lambda {\widehat {\bf L}}^{+}_{e} {\bf e}\f]
+ where camera velocities are computed. */
+ EYEINHAND_L_cVe_eJe,
+ /*!< Eye in hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = -\lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf
+ V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are
+ computed. */
+ EYETOHAND_L_cVe_eJe,
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_e
+ {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are
+ computed. */
+ EYETOHAND_L_cVf_fVe_eJe,
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f
+ {^f}{\bf V}_e {^e}{\bf J}_e} \right)^{+} {\bf e}\f] where joint
+ velocities are computed. */
+ EYETOHAND_L_cVf_fJe
+ /*!< Eye to hand visual servoing with the following control law
+ \f[{\dot {\bf q}} = \lambda \left( {{\widehat {\bf L}}_{e} {^c}{\bf V}_f
+ {^f}{\bf J}_e} \right)^{+} {\bf e}\f] where joint velocities are
+ computed. */
+ } vpServoType;
+
+ typedef enum {
+ CURRENT,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction
+ matrix \f${\widehat {\bf L}}_s \f$computed using the current features
+ \f$\bf s\f$. */
+ DESIRED,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction
+ matrix \f${\widehat {\bf L}}_{s^*} \f$computed using the desired
+ features \f${\bf s}^*\f$. */
+ MEAN,
+ /*!< In the control law (see vpServo::vpServoType), uses the interaction
+ matrix \f${\widehat {\bf L}} = \left({\widehat {\bf L}}_s + {\widehat
+ {\bf L}}_{s^*}\right)/2 \f$. */
+ USER_DEFINED
+ /*!< In the control law (see vpServo::vpServoType), uses an interaction
+ matrix set by the user. */
+ } vpServoIteractionMatrixType;
+
+ typedef enum {
+ TRANSPOSE, /*!< In the control law (see vpServo::vpServoType), uses the
+ transpose instead of the pseudo inverse. */
+ PSEUDO_INVERSE /*!< In the control law (see vpServo::vpServoType), uses
+ the pseudo inverse. */
+ } vpServoInversionType;
+
+ typedef enum {
+ ALL, /*!< Print all the task information. */
+ CONTROLLER, /*!< Print the type of controller law. */
+ ERROR_VECTOR, /*!< Print the error vector \f$\bf e = (s-s^*)\f$. */
+ FEATURE_CURRENT, /*!< Print the current features \f$\bf s\f$. */
+ FEATURE_DESIRED, /*!< Print the desired features \f${\bf s}^*\f$. */
+ GAIN, /*!< Print the gain \f$\lambda\f$. */
+ INTERACTION_MATRIX, /*!< Print the interaction matrix. */
+ MINIMUM /*!< Same as vpServo::vpServoPrintType::ERROR_VECTOR. */
+ } vpServoPrintType;
+
+ // private:
+ //#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ // vpServo(const vpServo &)
+ // : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(),
+ // servoType(vpServo::NONE),
+ // rankJ1(0), featureList(), desiredFeatureList(),
+ // featureSelectionList(), lambda(), signInteractionMatrix(1),
+ // interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE),
+ // cVe(), init_cVe(false), cVf(), init_cVf(false), fVe(),
+ // init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
+ // errorComputed(false), interactionMatrixComputed(false), dim_task(0),
+ // taskWasKilled(false), forceInteractionMatrixComputation(false),
+ // WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial()
+ // {
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!");
+ // }
+ // vpServo &operator=(const vpServo &){
+ // throw vpException(vpException::functionNotImplementedError, "Not
+ // implemented!"); return *this;
+ // }
+ //#endif
public:
// default constructor
vpServo();
// constructor with Choice of the visual servoing control law
- vpServo(vpServoType servoType) ;
+ explicit vpServo(vpServoType servoType);
// destructor
- virtual ~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() ;
+ vpColVector computeControlLaw();
// compute the desired control law
- vpColVector computeControlLaw(double t) ;
+ vpColVector computeControlLaw(double t);
vpColVector computeControlLaw(double t, const vpColVector &e_dot_init);
// compute the error between the current set of visual features and
// the desired set of visual features
- vpColVector computeError() ;
+ vpColVector computeError();
// compute the interaction matrix related to the set of visual features
- vpMatrix computeInteractionMatrix() ;
+ vpMatrix computeInteractionMatrix();
// Return the task dimension.
- unsigned int getDimension() const ;
+ unsigned int getDimension() const;
/*!
- Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual features
- \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
- The error vector is updated after a call of computeError() or computeControlLaw().
+ Return the error \f$\bf e = (s - s^*)\f$ between the current set of visual
+features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$. The
+error vector is updated after a call of computeError() or computeControlLaw().
\code
vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpColVector e = task.getError(); // Get the error vector
+ vpColVector e = task.getError(); // Get the error vector
\endcode
*/
- inline vpColVector getError() const
- {
- return error ;
- }
+ inline vpColVector getError() const { return error; }
+
/*
- Return the interaction matrix \f$L\f$ used to compute the task jacobian \f$J_1\f$.
- The interaction matrix is updated after a call to computeInteractionMatrix() or computeControlLaw().
+ Return the interaction matrix \f$L\f$ used to compute the task jacobian
+ \f$J_1\f$. The interaction matrix is updated after a call to
+ computeInteractionMatrix() or computeControlLaw().
\code
vpServo task;
...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix L = task.getInteractionMatrix(); // Get the interaction matrix used to compute v
+ vpColVector v = task.computeControlLaw(); // Compute the velocity
+ corresponding to the visual servoing vpMatrix L =
+ task.getInteractionMatrix(); // Get the interaction matrix used to compute v
\endcode
\sa getTaskJacobian()
*/
- inline vpMatrix getInteractionMatrix() const
- {
- return L;
- }
+ inline vpMatrix getInteractionMatrix() const { return L; }
vpMatrix getI_WpW() const;
/*!
Return the visual servo type.
*/
- inline vpServoType getServoType() const
- {
- return servoType;
- }
+ inline vpServoType getServoType() const { return servoType; }
vpMatrix getLargeP() const;
@@ -309,23 +314,25 @@ public:
\return Singular values that relies on the task jacobian pseudo inverse.
*/
- inline vpColVector getTaskSingularValues() const
- {
- return sv;
- }
+ inline vpColVector getTaskSingularValues() const { return sv; }
vpMatrix getWpW() const;
/*!
- Return the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ Return the velocity twist matrix used to transform a velocity skew vector
+ from end-effector frame into the camera frame.
*/
vpVelocityTwistMatrix get_cVe() const { return cVe; }
/*!
- Return the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ Return the velocity twist matrix used to transform a velocity skew vector
+ from robot fixed frame (also called world or base frame) into the camera
+ frame.
*/
vpVelocityTwistMatrix get_cVf() const { return cVf; }
/*!
- Return the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ Return the velocity twist matrix used to transform a velocity skew vector
+ from robot end-effector frame into the fixed frame (also called world or
+ base frame).
*/
vpVelocityTwistMatrix get_fVe() const { return fVe; }
/*!
@@ -333,36 +340,42 @@ public:
*/
vpMatrix get_eJe() const { return eJe; }
/*!
- Return the robot jacobian expressed in the robot fixed frame (also called world or base frame).
+ Return the robot jacobian expressed in the robot fixed frame (also called
+ world or base frame).
*/
vpMatrix get_fJe() const { return fJe; }
// destruction (memory deallocation if required)
- void kill() ;
+ void 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) ;
+ 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);
+ void setCameraDoF(const vpColVector &dof);
/*!
- Set a variable which enables to compute the interaction matrix at each iteration.
+ Set a variable which enables to compute the interaction matrix at each
+ iteration.
- When the interaction matrix is computed from the desired features \f${\bf s}^*\f$ which are in general constant,
- the interaction matrix \f${\widehat {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo loop.
- Sometimes, when the desired features are time dependent \f${{\bf s}(t)}^*\f$ or varying, the interaction matrix
- need to be computed at each iteration of the servo loop. This method allows to force the computation of
- \f${\widehat {\bf L}}\f$ in this particular case.
+ When the interaction matrix is computed from the desired features \f${\bf
+ s}^*\f$ which are in general constant, the interaction matrix \f${\widehat
+ {\bf L}}_{s^*}\f$ is computed just at the first iteration of the servo
+ loop. Sometimes, when the desired features are time dependent \f${{\bf
+ s}(t)}^*\f$ or varying, the interaction matrix need to be computed at each
+ iteration of the servo loop. This method allows to force the computation
+ of \f${\widehat {\bf L}}\f$ in this particular case.
- \param force_computation : If true it forces the interaction matrix computation even if it is already done.
+ \param force_computation : If true it forces the interaction matrix
+ computation even if it is already done.
*/
void setForceInteractionMatrixComputation(bool force_computation)
@@ -371,228 +384,277 @@ public:
}
/*!
- Set the interaction matrix type (current, desired, mean or user defined) and how its inverse is computed.
- \param interactionMatrixType : The interaction matrix type. See vpServo::vpServoIteractionMatrixType for
- more details.
- \param interactionMatrixInversion : How is the inverse computed. See vpServo::vpServoInversionType for
- more details.
+ Set the interaction matrix type (current, desired, mean or user defined)
+ and how its inverse is computed. \param interactionMatrixType : The
+ interaction matrix type. See vpServo::vpServoIteractionMatrixType for more
+ details. \param interactionMatrixInversion : How is the inverse computed.
+ See vpServo::vpServoInversionType for more details.
*/
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType,
- const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE) ;
+ const vpServoInversionType &interactionMatrixInversion = PSEUDO_INVERSE);
/*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as constant.
+ Set the gain \f$\lambda\f$ used in the control law (see
+ vpServo::vpServoType) as constant.
- The usage of an adaptive gain allows to reduce the convergence time, see setLambda(const vpAdaptiveGain&).
+ The usage of an adaptive gain allows to reduce the convergence time, see
+ setLambda(const vpAdaptiveGain&).
- \param c : Constant gain. Values are in general between 0.1 and 1. Higher is the gain, higher are the velocities
- that may be applied to the robot.
+ \param c : Constant gain. Values are in general between 0.1 and 1. Higher
+ is the gain, higher are the velocities that may be applied to the robot.
*/
- void setLambda(double c)
- {
- lambda .initFromConstant (c) ;
- }
+ void setLambda(double c) { lambda.initFromConstant(c); }
/*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
- Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
+ Set the gain \f$\lambda\f$ used in the control law (see
+ vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in
+ computeControlLaw() depend on the infinity norm of the task Jacobian.
- The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
+ The usage of an adaptive gain rather than a constant gain allows to reduce
+ the convergence time.
\param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+ \param gain_at_infinity : the expected gain when \f$x=\infty\f$:
+ \f$\lambda(\infty)\f$. \param slope_at_zero : the expected slope of
+ \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
For more details on these parameters see vpAdaptiveGain class.
*/
- void setLambda(const double gain_at_zero,
- const double gain_at_infinity,
- const double slope_at_zero)
+ 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) ;
+ lambda.initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
}
/*!
- Set the gain \f$\lambda\f$ used in the control law (see vpServo::vpServoType) as adaptive.
- Value of \f$\lambda\f$ that is used in computeControlLaw() depend on the infinity norm of the task Jacobian.
+ Set the gain \f$\lambda\f$ used in the control law (see
+ vpServo::vpServoType) as adaptive. Value of \f$\lambda\f$ that is used in
+ computeControlLaw() depend on the infinity norm of the task Jacobian.
- The usage of an adaptive gain rather than a constant gain allows to reduce the convergence time.
- \sa vpAdaptiveGain
+ The usage of an adaptive gain rather than a constant gain allows to reduce
+ the convergence time. \sa vpAdaptiveGain
*/
- void setLambda(const vpAdaptiveGain& l){lambda=l;}
+ void setLambda(const vpAdaptiveGain &l) { lambda = l; }
/*!
- Set the value of the parameter \f$\mu\f$ used to ensure the continuity of the velocities
- computed using computeControlLaw(double).
+ Set the value of the parameter \f$\mu\f$ used to ensure the continuity of
+ the velocities computed using computeControlLaw(double).
A recommended value is 4.
*/
- void setMu(double mu_){this->mu=mu_;}
+ void setMu(double mu_) { this->mu = mu_; }
// Choice of the visual servoing control law
- void setServo(const vpServoType &servo_type) ;
+ void setServo(const vpServoType &servo_type);
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from end-effector frame into the camera frame.
*/
- void set_cVe(const vpVelocityTwistMatrix &cVe_) { this->cVe = cVe_ ; init_cVe = true ; }
+ void set_cVe(const vpVelocityTwistMatrix &cVe_)
+ {
+ this->cVe = cVe_;
+ init_cVe = true;
+ }
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from end-effector frame into the camera frame.
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from end-effector frame into the camera frame.
*/
- void set_cVe(const vpHomogeneousMatrix &cMe) { cVe.buildFrom(cMe); init_cVe=true ;}
+ void set_cVe(const vpHomogeneousMatrix &cMe)
+ {
+ cVe.buildFrom(cMe);
+ init_cVe = true;
+ }
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from robot fixed frame (also called world or base frame) into the camera
+ frame.
*/
- void set_cVf(const vpVelocityTwistMatrix &cVf_) { this->cVf = cVf_ ; init_cVf = true ; }
+ void set_cVf(const vpVelocityTwistMatrix &cVf_)
+ {
+ this->cVf = cVf_;
+ init_cVf = true;
+ }
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot fixed frame (also called world or base frame) into the camera frame.
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from robot fixed frame (also called world or base frame) into the camera
+ frame.
*/
- void set_cVf(const vpHomogeneousMatrix &cMf) { cVf.buildFrom(cMf); init_cVf=true ;}
+ void set_cVf(const vpHomogeneousMatrix &cMf)
+ {
+ cVf.buildFrom(cMf);
+ init_cVf = true;
+ }
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from robot end-effector frame into the fixed frame (also called world or
+ base frame).
*/
- void set_fVe(const vpVelocityTwistMatrix &fVe_) { this->fVe = fVe_ ; init_fVe = true ; }
+ void set_fVe(const vpVelocityTwistMatrix &fVe_)
+ {
+ this->fVe = fVe_;
+ init_fVe = true;
+ }
/*!
- Set the velocity twist matrix used to transform a velocity skew vector from robot end-effector frame into the fixed frame (also called world or base frame).
+ Set the velocity twist matrix used to transform a velocity skew vector
+ from robot end-effector frame into the fixed frame (also called world or
+ base frame).
*/
- void set_fVe(const vpHomogeneousMatrix &fMe) { fVe.buildFrom(fMe); init_fVe=true ;}
+ void set_fVe(const vpHomogeneousMatrix &fMe)
+ {
+ fVe.buildFrom(fMe);
+ init_fVe = true;
+ }
/*!
Set the robot jacobian expressed in the end-effector frame.
*/
- void set_eJe(const vpMatrix &eJe_) { this->eJe = eJe_ ; init_eJe = true ; }
+ void set_eJe(const vpMatrix &eJe_)
+ {
+ this->eJe = eJe_;
+ init_eJe = true;
+ }
/*!
- Set the robot jacobian expressed in the robot fixed frame (also called world or base frame).
+ Set the robot jacobian expressed in the robot fixed frame (also called
+ world or base frame).
*/
- void set_fJe(const vpMatrix &fJe_) { this->fJe = fJe_ ; init_fJe = true ; }
+ void set_fJe(const vpMatrix &fJe_)
+ {
+ this->fJe = fJe_;
+ init_fJe = true;
+ }
/*!
- Test if all the initialization are correct. If true, the control law can be computed.
+ Test if all the initialization are correct. If true, the control law can
+ be computed.
*/
- bool testInitialization() ;
+ bool testInitialization();
/*!
Test if all the update are correct. If true control law can be computed.
*/
- bool testUpdated() ;
+ bool testUpdated();
- protected:
+protected:
//! Basic initialization.
- void init() ;
+ void init();
/*!
Compute the classic projetion operator and the large projection operator.
*/
void computeProjectionOperators();
- public:
+public:
//! Interaction matrix
- vpMatrix L ;
+ vpMatrix L;
//! Error \f$(s - s^*)\f$ between the current set of visual features
//! \f$s\f$ and the desired set of visual features \f$s^*\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector error ;
+ //! This vector is updated after a call of computeError() or
+ //! computeControlLaw().
+ vpColVector error;
//! Task Jacobian \f$J_1 = L {^c}V_a {^a}J_e\f$.
- vpMatrix J1 ;
+ vpMatrix J1;
//! Pseudo inverse \f${J_1}^{+}\f$ of the task Jacobian.
- vpMatrix J1p ;
+ vpMatrix J1p;
//! Current state of visual features \f$s\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector s ;
+ //! This vector is updated after a call of computeError() or
+ //! computeControlLaw().
+ vpColVector s;
//! Desired state of visual features \f$s^*\f$.
- //! This vector is updated after a call of computeError() or computeControlLaw().
- vpColVector sStar ;
+ //! This vector is updated after a call of computeError() or
+ //! computeControlLaw().
+ vpColVector sStar;
//! Primary task \f$e_1 = {J_1}^{+}(s-s*)\f$
- vpColVector e1 ;
+ vpColVector e1;
//! Task \f$e = e_1 + (I-{J_1}^{+} J_1) e_2\f$
- vpColVector e ;
+ vpColVector e;
//! Articular velocity
- vpColVector q_dot ;
+ vpColVector q_dot;
//! Camera velocity
- vpColVector v ;
+ vpColVector v;
//! Chosen visual servoing control law
vpServoType servoType;
//! Rank of the task Jacobian
- unsigned int rankJ1 ;
+ unsigned int rankJ1;
//! List of current visual features \f$\bf s\f$.
- std::list<vpBasicFeature *> featureList ;
+ std::list<vpBasicFeature *> featureList;
//! List of desired visual features \f$\bf s^*\f$.
- std::list<vpBasicFeature *> desiredFeatureList ;
+ std::list<vpBasicFeature *> desiredFeatureList;
//! List of selection among visual features
//! used for selection of a subset of each visual feature if required.
- std::list<unsigned int> featureSelectionList ;
+ std::list<unsigned int> featureSelectionList;
//! Gain used in the control law.
- vpAdaptiveGain lambda ;
+ vpAdaptiveGain lambda;
//! Sign of the interaction +/- 1 (1 for eye-in-hand, -1 for
//! eye-to-hand configuration)
- int signInteractionMatrix ;
+ int signInteractionMatrix;
//! Type of the interaction matrox (current, mean, desired, user)
- vpServoIteractionMatrixType interactionMatrixType ;
+ vpServoIteractionMatrixType interactionMatrixType;
//! Indicates if the transpose or the pseudo inverse of the
//! interaction matrix should be used to compute the task.
- vpServoInversionType inversionType ;
+ vpServoInversionType inversionType;
- protected:
+protected:
/*
Twist transformation matrix
*/
//! Twist transformation matrix between Re and Rc.
- vpVelocityTwistMatrix cVe ;
- bool init_cVe ;
+ vpVelocityTwistMatrix cVe;
+ bool init_cVe;
//! Twist transformation matrix between Rf and Rc.
- vpVelocityTwistMatrix cVf ;
- bool init_cVf ;
+ vpVelocityTwistMatrix cVf;
+ bool init_cVf;
//! Twist transformation matrix between Re and Rf.
- vpVelocityTwistMatrix fVe ;
- bool init_fVe ;
+ vpVelocityTwistMatrix fVe;
+ bool init_fVe;
/*
Jacobians
*/
//! Jacobian expressed in the end-effector frame.
- vpMatrix eJe ;
- bool init_eJe ;
+ vpMatrix eJe;
+ bool init_eJe;
//! Jacobian expressed in the robot reference frame.
- vpMatrix fJe ;
- bool init_fJe ;
+ vpMatrix fJe;
+ bool init_fJe;
/*
Task building
*/
//! true if the error has been computed.
- bool errorComputed ;
+ bool errorComputed;
//! true if the interaction matrix has been computed.
- bool interactionMatrixComputed ;
+ bool interactionMatrixComputed;
//! Dimension of the task updated during computeControlLaw().
- unsigned int dim_task ;
+ unsigned int dim_task;
//! Flag to indicate if the task was killed
bool taskWasKilled;
//! Force the interaction matrix computation even if it is already done.
bool forceInteractionMatrixComputation;
//! Projection operators \f$\bf WpW\f$.
- vpMatrix WpW ;
+ vpMatrix WpW;
//! Projection operators \f$\bf I-WpW\f$.
- vpMatrix I_WpW ;
+ vpMatrix I_WpW;
/*!
- New Large projection operator (see equation(24) in the paper \cite Marey:2010). This projection operator allows
- performing secondary task even when the main task is full rank.
- \f[
- {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left
- ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
- \f]
+ New Large projection operator (see equation(24) in the paper
+ \cite Marey:2010). This projection operator allows performing secondary task
+ even when the main task is full rank. \f[
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{
+ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \|
+ {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f]
with
\f[
- {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf
+ J_{{\bf e }} } {\bf J_{{\bf e }}^\top }
{\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
\f]
@@ -600,7 +662,7 @@ public:
vpMatrix P;
//! Singular values from the pseudo inverse.
- vpColVector sv ;
+ vpColVector sv;
double mu;
@@ -609,8 +671,9 @@ public:
//! Boolean to know if cJc is identity (for fast computation)
bool iscJcIdentity;
- //! A diag matrix used to determine which are the degrees of freedom that are controlled in the camera frame
+ //! A diag matrix used to determine which are the degrees of freedom that
+ //! are controlled in the camera frame
vpMatrix cJc;
-} ;
+};
#endif
diff --git a/modules/vs/include/visp3/vs/vpServoData.h b/modules/vs/include/visp3/vs/vpServoData.h
index 863470e..db4d956 100644
--- a/modules/vs/include/visp3/vs/vpServoData.h
+++ b/modules/vs/include/visp3/vs/vpServoData.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,8 +44,6 @@
\brief save data during the task execution
*/
-
-
// Servo
#include <visp3/vs/vpServo.h>
@@ -59,31 +58,29 @@ class VISP_EXPORT vpServoData
{
private:
- char baseDirectory[FILENAME_MAX] ;
+ char baseDirectory[FILENAME_MAX];
- std::ofstream velocityFile ;
- std::ofstream errorFile ;
- std::ofstream errorNormFile ;
- std::ofstream sFile ;
- std::ofstream sStarFile ;
- std::ofstream vNormFile ;
+ std::ofstream velocityFile;
+ std::ofstream errorFile;
+ std::ofstream errorNormFile;
+ std::ofstream sFile;
+ std::ofstream sStarFile;
+ std::ofstream vNormFile;
//! flag to known if velocity should be output in cm and degrees (true)
//! or in m/rad
- bool cmDeg ;
-
-
+ bool cmDeg;
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!");
+ }
+ vpServoData &operator=(const vpServoData &)
+ {
+ throw vpException(vpException::functionNotImplementedError, "Not implemented!");
}
#endif
@@ -91,21 +88,18 @@ public:
virtual ~vpServoData() { ; }
//! velocity output in cm and deg
- void setCmDeg() ;
+ void setCmDeg();
//! velocity output in meter and deg (default)
- void setMeterRad() ;
-
- void save(const vpServo &task) ;
- void open(const char *baseDirectory) ;
- void close() ;
-
- void empty() ;
- void push() ;
- void display(vpImage<unsigned char> &I) ;
-
-} ;
+ void setMeterRad();
+ void save(const vpServo &task);
+ void open(const char *baseDirectory);
+ void close();
+ void empty();
+ void push();
+ void display(vpImage<unsigned char> &I);
+};
#endif
diff --git a/modules/vs/include/visp3/vs/vpServoDisplay.h b/modules/vs/include/visp3/vs/vpServoDisplay.h
index e88bab9..23974a3 100644
--- a/modules/vs/include/visp3/vs/vpServoDisplay.h
+++ b/modules/vs/include/visp3/vs/vpServoDisplay.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -49,7 +50,7 @@
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
-//Color / image / display
+// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpRGBa.h>
@@ -61,19 +62,12 @@
class VISP_EXPORT vpServoDisplay
{
public:
- static void display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- vpColor currentColor = vpColor::green,
- vpColor desiredColor = vpColor::red,
- unsigned int thickness=1) ;
- static void display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- vpColor currentColor = vpColor::green,
- vpColor desiredColor = vpColor::red,
- unsigned int thickness=1) ;
-} ;
+ static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red,
+ unsigned int thickness = 1);
+ static void display(const vpServo &s, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ vpColor currentColor = vpColor::green, vpColor desiredColor = vpColor::red,
+ unsigned int thickness = 1);
+};
#endif
-
diff --git a/modules/vs/include/visp3/vs/vpServoException.h b/modules/vs/include/visp3/vs/vpServoException.h
index 955748c..bfff285 100644
--- a/modules/vs/include/visp3/vs/vpServoException.h
+++ b/modules/vs/include/visp3/vs/vpServoException.h
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -45,12 +46,15 @@
#include <visp3/core/vpException.h>
-#include <iostream> /* Classe std::ostream. */
-#include <string> /* Classe string. */
+#include <iostream> /* Classe std::ostream. */
+#include <string> /* Classe string. */
-/* ------------------------------------------------------------------------- */
-/* --- CLASS --------------------------------------------------------------- */
-/* ------------------------------------------------------------------------- */
+/* -------------------------------------------------------------------------
+ */
+/* --- CLASS ---------------------------------------------------------------
+ */
+/* -------------------------------------------------------------------------
+ */
/*!
\class vpServoException
@@ -59,38 +63,34 @@
*/
class VISP_EXPORT vpServoException : public vpException
{
- public:
- /*!
+public:
+ /*!
- \brief Lists the possible error than can be emmited while calling
- vpServo member
- */
- enum errorServoCodeEnum
- {
- //! Current or desired feature list is empty
- noFeatureError,
- //! No degree of freedom is available to achieve the secondary task.
- noDofFree,
- //! Task was not killed properly
- notKilledProperly,
- //! Other exception
- servoError
- } ;
-
- public:
- vpServoException (const int id, const char* format, ...)
- {
- this->code = id;
- va_list args;
- va_start(args, format);
- setMessage(format, args);
- va_end (args);
- }
- vpServoException (const int id, const std::string & msg)
- : vpException(id, msg){ ; }
- vpServoException (const int id)
- : vpException(id){ ; }
+ \brief Lists the possible error than can be emmited while calling
+ vpServo member
+ */
+ enum errorServoCodeEnum {
+ //! Current or desired feature list is empty
+ noFeatureError,
+ //! No degree of freedom is available to achieve the secondary task.
+ noDofFree,
+ //! Task was not killed properly
+ notKilledProperly,
+ //! Other exception
+ servoError
+ };
+public:
+ vpServoException(const int id, const char *format, ...)
+ {
+ this->code = id;
+ va_list args;
+ va_start(args, format);
+ setMessage(format, args);
+ va_end(args);
+ }
+ vpServoException(const int id, const std::string &msg) : vpException(id, msg) { ; }
+ explicit vpServoException(const int id) : vpException(id) { ; }
};
#endif
diff --git a/modules/vs/src/vpAdaptiveGain.cpp b/modules/vs/src/vpAdaptiveGain.cpp
index 3ff7df0..aced41c 100644
--- a/modules/vs/src/vpAdaptiveGain.cpp
+++ b/modules/vs/src/vpAdaptiveGain.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -43,320 +44,282 @@
#include <visp3/core/vpDebug.h>
#include <visp3/vs/vpAdaptiveGain.h>
+#include <cmath> // std::fabs
#include <iostream>
-#include <cmath> // std::fabs
-#include <limits> // numeric_limits
+#include <limits> // numeric_limits
const double vpAdaptiveGain::DEFAULT_LAMBDA_ZERO = 1.666;
const double vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY = 0.1666;
-const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
+const double vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE = 1.666;
-/* -------------------------------------------------------------------------- */
-/* --- CONSTRUCTION --------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- CONSTRUCTION ---------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
- Basic constructor which initializes all the parameters with their default value:
- - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
+ Basic constructor which initializes all the parameters with their default
+ value:
+ - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
+ - \f$ \lambda_\infty = 0.1666 \f$ using
+ vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
+ - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
*/
-vpAdaptiveGain::vpAdaptiveGain ()
- :
- coeff_a (),
- coeff_b (),
- coeff_c (),
- lambda(1.)
+vpAdaptiveGain::vpAdaptiveGain() : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
{
- vpDEBUG_TRACE (10, "# Entree constructeur par default.");
- this ->initFromVoid ();
+ this->initFromVoid();
- vpDEBUG_TRACE (10, "# Sortie constructeur par default.");
return;
}
/*!
- Constructor that initializes the gain as constant. In that case \f$\lambda(x) = c\f$.
+ Constructor that initializes the gain as constant. In that case
+ \f$\lambda(||{\bf e}||) = c\f$.
- \param c : Value of the constant gain.
+ \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.
- \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+ \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$:
+ \f$\lambda_0\f$. \param gain_at_infinity : the expected gain when \f$||{\bf
+ e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. \param slope_at_zero : the
+ expected slope of \f$\lambda(||{\bf e}||)\f$ when \f$||{\bf e}||=0\f$:
+ \f$\lambda'_0\f$.
*/
-vpAdaptiveGain::vpAdaptiveGain (double gain_at_zero, double gain_at_infinity, double slope_at_zero)
- :
- coeff_a (),
- coeff_b (),
- coeff_c (),
- lambda(1.)
+vpAdaptiveGain::vpAdaptiveGain(double gain_at_zero, double gain_at_infinity, double slope_at_zero)
+ : coeff_a(), coeff_b(), coeff_c(), lambda(1.)
{
initStandard(gain_at_zero, gain_at_infinity, slope_at_zero);
}
-/* -------------------------------------------------------------------------- */
-/* --- INIT ----------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- INIT -----------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
- Initializes the parameters to have a constant gain. In that case \f$\lambda(x) = c\f$.
+ Initializes the parameters to have a constant gain. In that case
+ \f$\lambda(||{\bf e}||) = c\f$.
- \param c : Value of the constant gain.
+ \param c : Value of the constant gain. A typical value is 0.5.
*/
-void vpAdaptiveGain::initFromConstant (const double c)
+void vpAdaptiveGain::initFromConstant(const double c)
{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->coeff_a = 0;
- this ->coeff_b = 1;
- this ->coeff_c = c;
-
- vpDEBUG_TRACE (10, "# Sortie.");
- return;
+ this->coeff_a = 0;
+ this->coeff_b = 1;
+ this->coeff_c = c;
+ return;
}
-
/*!
Initializes the parameters with the default value :
- - \f$ \lambda(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
- - \f$ \lambda(\infty) = 0.1666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
- - \f$ {\dot \lambda}(0) = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
+ - \f$ \lambda_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_ZERO
+ - \f$ \lambda_\infty = 0.1666 \f$ using
+ vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY
+ - \f$ \lambda'_0 = 1.666 \f$ using vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE
*/
-void vpAdaptiveGain::initFromVoid (void)
+void vpAdaptiveGain::initFromVoid(void)
{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->initStandard (vpAdaptiveGain::DEFAULT_LAMBDA_ZERO,
- vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
- vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);
-
- vpDEBUG_TRACE (10, "# Sortie.");
+ this->initStandard(vpAdaptiveGain::DEFAULT_LAMBDA_ZERO, vpAdaptiveGain::DEFAULT_LAMBDA_INFINITY,
+ vpAdaptiveGain::DEFAULT_LAMBDA_SLOPE);
return;
}
-
/*!
- Set the parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$ used to compute \f$\lambda(x)\f$.
-
- \param gain_at_zero : the expected gain when \f$x=0\f$: \f$\lambda(0)\f$.
- \param gain_at_infinity : the expected gain when \f$x=\infty\f$: \f$\lambda(\infty)\f$.
- \param slope_at_zero : the expected slope of \f$\lambda(x)\f$ when \f$x=0\f$: \f${\dot \lambda}(0)\f$.
+ Set the parameters \f$\lambda_0, \lambda_\infty, \lambda'_0\f$ used to
+ compute \f$\lambda(||{\bf e}||)\f$.
+
+ \param gain_at_zero : the expected gain when \f$||{\bf e}||=0\f$:
+ \f$\lambda_0\f$. \param gain_at_infinity : the expected gain when \f$||{\bf
+ e}||\rightarrow\infty\f$: \f$\lambda_\infty\f$. \param slope_at_zero : the
+ 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)
{
- vpDEBUG_TRACE (10, "# Entree.");
-
- this ->coeff_a = gain_at_zero - gain_at_infinity;
- //if (0 == this ->coeff_a)
- if (std::fabs(this ->coeff_a) <= std::numeric_limits<double>::epsilon())
- {
- this ->coeff_b = 0;
- }
- else
- {
- this ->coeff_b = slope_at_zero / ( this ->coeff_a);
- }
- this ->coeff_c = gain_at_infinity;
-
- vpDEBUG_TRACE (10, "# Sortie :a,b,c= %.3f,%.3f,%.3f.",
- this ->coeff_a, this ->coeff_b, this ->coeff_c);
+ this->coeff_a = gain_at_zero - gain_at_infinity;
+ // if (0 == this ->coeff_a)
+ if (std::fabs(this->coeff_a) <= std::numeric_limits<double>::epsilon()) {
+ this->coeff_b = 0;
+ } else {
+ this->coeff_b = slope_at_zero / (this->coeff_a);
+ }
+ this->coeff_c = gain_at_infinity;
+
return;
}
-
-
-/* -------------------------------------------------------------------------- */
-/* --- MODIFICATOR ---------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- MODIFICATOR ----------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
- Sets the internal parameters \f$a,b,c\f$ in order to obtain a constant gain equal to
- the gain in 0 set through the parameter \f$\lambda(0)\f$.
-
- \return It returns the value of the constant gain \f$\lambda(0)\f$.
+ Sets the internal parameters in order to obtain a constant gain equal to
+ the gain in 0 set through the parameter \f$\lambda_0\f$.
+
+ \return It returns the value of the constant gain \f$\lambda_0\f$.
*/
-double vpAdaptiveGain::setConstant (void)
+double vpAdaptiveGain::setConstant(void)
{
- vpDEBUG_TRACE (10, "# Entree.");
-
- double res = this ->coeff_a + this ->coeff_c;
+ double res = this->coeff_a + this->coeff_c;
- this ->coeff_a = 0;
- this ->coeff_b = 1;
- this ->coeff_c = res;
+ this->coeff_a = 0;
+ this->coeff_b = 1;
+ this->coeff_c = res;
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
-/* -------------------------------------------------------------------------- */
-/* --- VALEUR --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+/* --------------------------------------------------------------------------
+ */
+/* --- VALEUR ---------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
- \f[\lambda(x) = a * exp(-b*x) + c\f]
+ \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{
+ \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f]
+
+ \param x : Input value to consider. During a visual servo this value can be
+ the euclidian norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf
+ e}||_{\infty}\f$ of the task function.
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
-
\return It returns the value of the computed gain.
*/
-double vpAdaptiveGain::value_const (const double x) const
+double vpAdaptiveGain::value_const(const double x) const
{
- vpDEBUG_TRACE (10, "# Entree.");
+ double res = this->coeff_a * exp(-this->coeff_b * x) + this->coeff_c;
- double res = this ->coeff_a * exp (- this ->coeff_b * x) + this ->coeff_c;
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
/*!
- Gets the value of the gain at infinity (ie the value of \f$ \lambda(\infty) = c \f$).
- Similar to limitValue() except that here the value is not stored as a parameter of the class.
+ Gets the value of the gain at infinity (ie the value of \f$ \lambda_\infty =
+ c \f$). This function is similar to limitValue() except that here the value
+ is not stored as a parameter of the class.
\return It returns the value of the gain at infinity.
*/
-double vpAdaptiveGain::limitValue_const (void) const
+double vpAdaptiveGain::limitValue_const(void) const
{
- vpDEBUG_TRACE (10, "# Entree.");
-
- double res = this ->coeff_c;
+ double res = this->coeff_c;
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", res);
return res;
}
/*!
Computes the value of the adaptive gain \f$\lambda(x)\f$ using:
- \f[\lambda(x) = a * exp(-b*x) + c\f]
+ \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{
+ \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f]
This value is stored as a parameter of the class.
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
+ \param x : Input value to consider. During a visual servo this value can be
+ the euclidian norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf
+ e}||_{\infty}\f$ of the task function.
\return It returns the value of the computed gain.
*/
-double vpAdaptiveGain::value (const double x) const
+double vpAdaptiveGain::value(const double x) const
{
- vpDEBUG_TRACE (10, "# Entree.");
+ this->lambda = this->value_const(x);
- this ->lambda = this ->value_const (x);
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
return lambda;
}
-
/*!
- Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$) and stores it
- as a parameter of the class.
+ Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty =
+ c \f$) and stores it as a parameter of the class.
\return It returns the value of the gain at infinity.
*/
-double vpAdaptiveGain:: limitValue (void) const
+double vpAdaptiveGain::limitValue(void) const
{
- vpDEBUG_TRACE (10, "# Entree.");
+ this->lambda = this->limitValue_const();
- this ->lambda = this ->limitValue_const ();
-
- vpDEBUG_TRACE (10, "# Sortie: %.3f.", this ->lambda);
return lambda;
}
-/* -------------------------------------------------------------------------- */
-/* --- ACCESSORS ------------------------------------------------------------ */
-/* -------------------------------------------------------------------------- */
-
+/* --------------------------------------------------------------------------
+ */
+/* --- ACCESSORS ------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
+/*!
+ Operator that computes \f$\lambda(x)\f$ where
-// double vpAdaptiveGain::
-// getLastValue (void) const
-// {
-// return this ->lambda;
-// }
+ \f[ \lambda (x) = (\lambda_0 - \lambda_\infty) e^{ -\frac{
+ \lambda'_0}{\lambda_0 - \lambda_\infty}x} + \lambda_\infty \f]
-/*!
- Operator that computes \f$\lambda(x)\f$.
-
- \param x : Input value to consider. During a visual servo this value can be the euclidian
- norm \f$||s - s^*||\f$ or the infinity norm \f$||s - s^*||_{\infty}\f$ of the task function.
+ \param x : Input value to consider. During a visual servo this value can be
+ the euclidian norm \f$||{\bf e}||\f$ or the infinity norm \f$||{\bf
+ e}||_{\infty}\f$ of the task function.
\return It returns the value of the computed gain.
\sa value()
*/
-double vpAdaptiveGain::operator() (const double x) const
-{
- return this ->value (x);
-}
+double vpAdaptiveGain::operator()(const double x) const { return this->value(x); }
/*!
- Gets the value of the gain at infinity (ie the value of \f$\lambda(\infty) = c \f$).
+ Gets the value of the gain at infinity (ie the value of \f$\lambda_\infty =
+ c \f$).
\return It returns the value of the gain at infinity.
\sa limitValue()
*/
-double vpAdaptiveGain::operator() (void) const
-{
- return this ->limitValue ();
-}
+double vpAdaptiveGain::operator()(void) const { return this->limitValue(); }
/*!
- Operator which computes \f$\lambda({||x||}_{\infty})\f$.
-
- \param x : Input vector to consider.
-
- \return It returns the value of the computed gain.
-*/
-double vpAdaptiveGain::operator() (const vpColVector & x) const
-{
- return this ->value (x .infinityNorm());
-}
-
+ Operator which computes \f$\lambda({||x||}_{\infty})\f$ where
-// double operator() (double val_e) const;
-// double operator() (const CColVector & e) const;
+ \f[ \lambda ({||x||}_{\infty}) = (\lambda_0 - \lambda_\infty) e^{ -\frac{
+ \lambda'_0}{\lambda_0 - \lambda_\infty}{||x||}_{\infty}} + \lambda_\infty
+ \f]
-/* -------------------------------------------------------------------------- */
-/* --- OUTPUT --------------------------------------------------------------- */
-/* -------------------------------------------------------------------------- */
+ \param x : Input vector \f$ \bf x\f$ to consider.
+ \return It returns the value of the computed gain.
+*/
+double vpAdaptiveGain::operator()(const vpColVector &x) const { return this->value(x.infinityNorm()); }
+/* --------------------------------------------------------------------------
+ */
+/* --- OUTPUT ---------------------------------------------------------------
+ */
+/* --------------------------------------------------------------------------
+ */
/*!
- Prints the adaptive gain parameters \f$\lambda(0), \lambda(\infty), {\dot \lambda}(0)\f$.
-
+ Prints the adaptive gain parameters \f$\lambda_0, \lambda_\infty,
+ \lambda'_0\f$.
+
\param os : The stream where to print the adaptive gain parameters.
\param lambda : The adaptive gain containing the parameters to print.
*/
-VISP_EXPORT std::ostream& operator<< (std::ostream &os, const vpAdaptiveGain& lambda)
+VISP_EXPORT std::ostream &operator<<(std::ostream &os, const vpAdaptiveGain &lambda)
{
- os << "Zero= " << lambda .coeff_a + lambda .coeff_c
- << "\tInf= " << lambda .coeff_c
- << "\tDeriv= " << lambda .coeff_a * lambda .coeff_b;
+ 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 78bfeec..b743267 100644
--- a/modules/vs/src/vpServo.cpp
+++ b/modules/vs/src/vpServo.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,7 +38,6 @@
*
*****************************************************************************/
-
#include <visp3/vs/vpServo.h>
#include <sstream>
@@ -53,26 +53,26 @@
\brief Class required to compute the visual servoing control law
*/
-
/*!
Default constructor that initializes the following settings:
- - No control law is specified. The user has to call setServo() to specify the control law.
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
+ - No control law is specified. The user has to call setServo() to specify
+ the control law.
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is
+ computed with the desired features \f${\bf s}^*\f$. Using
+ setInteractionMatrixType() you can also compute the interaction matrix with
+ the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s
+ + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method
+ setInteractionMatrixType() allows to use the transpose instead.
*/
-vpServo::vpServo()
- : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(vpServo::NONE),
- rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
- interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
- cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
- errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
- forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4.), e1_initial(),
- iscJcIdentity(true), cJc(6,6)
+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)
{
cJc.eye();
}
@@ -82,22 +82,22 @@ vpServo::vpServo()
\param servo_type : Visual servoing control law.
The other settings are the following:
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is
+ computed with the desired features \f${\bf s}^*\f$. Using
+ setInteractionMatrixType() you can also compute the interaction matrix with
+ the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s
+ + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method
+ setInteractionMatrixType() allows to use the transpose instead.
*/
vpServo::vpServo(vpServoType servo_type)
- : L(), error(), J1(), J1p(), s(), sStar(), e1(), e(), q_dot(), v(), servoType(servo_type),
- rankJ1(0), featureList(), desiredFeatureList(), featureSelectionList(), lambda(), signInteractionMatrix(1),
- interactionMatrixType(DESIRED), inversionType(PSEUDO_INVERSE), cVe(), init_cVe(false),
- cVf(), init_cVf(false), fVe(), init_fVe(false), eJe(), init_eJe(false), fJe(), init_fJe(false),
- errorComputed(false), interactionMatrixComputed(false), dim_task(0), taskWasKilled(false),
- forceInteractionMatrixComputation(false), WpW(), I_WpW(), P(), sv(), mu(4), e1_initial(),
- 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();
}
@@ -124,46 +124,47 @@ vpServo::~vpServo()
}
}
-
/*!
Initialize the servo with the following settings:
- - No control law is specified. The user has to call setServo() to specify the control law.
- - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is computed with the desired
- features \f${\bf s}^*\f$. Using setInteractionMatrixType() you can also compute the
- interaction matrix with the current visual features, or from the mean
- \f$\left({\widehat {\bf L}}_s + {\widehat {\bf L}}_{s^*}\right)/2\f$.
- - In the control law the pseudo inverse will be used. The method setInteractionMatrixType()
- allows to use the transpose instead.
+ - No control law is specified. The user has to call setServo() to specify
+ the control law.
+ - In the control law, the interaction matrix \f${\widehat {\bf L}}_e \f$ is
+ computed with the desired features \f${\bf s}^*\f$. Using
+ setInteractionMatrixType() you can also compute the interaction matrix with
+ the current visual features, or from the mean \f$\left({\widehat {\bf L}}_s
+ + {\widehat {\bf L}}_{s^*}\right)/2\f$.
+ - In the control law the pseudo inverse will be used. The method
+ setInteractionMatrixType() allows to use the transpose instead.
*/
void vpServo::init()
{
// type of visual servoing
- servoType = vpServo::NONE ;
+ servoType = vpServo::NONE;
// Twist transformation matrix
- init_cVe = false ;
- init_cVf = false ;
- init_fVe = false ;
+ init_cVe = false;
+ init_cVf = false;
+ init_fVe = false;
// Jacobians
- init_eJe = false ;
- init_fJe = false ;
+ init_eJe = false;
+ init_fJe = false;
- dim_task = 0 ;
+ dim_task = 0;
- featureList.clear() ;
- desiredFeatureList.clear() ;
+ featureList.clear();
+ desiredFeatureList.clear();
featureSelectionList.clear();
- signInteractionMatrix = 1 ;
+ signInteractionMatrix = 1;
- interactionMatrixType = DESIRED ;
- inversionType=PSEUDO_INVERSE ;
+ interactionMatrixType = DESIRED;
+ inversionType = PSEUDO_INVERSE;
- interactionMatrixComputed = false ;
- errorComputed = false ;
+ interactionMatrixComputed = false;
+ errorComputed = false;
taskWasKilled = false;
@@ -194,22 +195,22 @@ void vpServo::kill()
// kill the current and desired feature lists
// current list
- for(std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
+ for (std::list<vpBasicFeature *>::iterator it = featureList.begin(); it != featureList.end(); ++it) {
if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
- delete (*it) ;
- (*it) = NULL ;
+ delete (*it);
+ (*it) = NULL;
}
}
- //desired list
- for(std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
+ // desired list
+ for (std::list<vpBasicFeature *>::iterator it = desiredFeatureList.begin(); it != desiredFeatureList.end(); ++it) {
if ((*it)->getDeallocate() == vpBasicFeature::vpServo) {
- delete (*it) ;
- (*it) = NULL ;
+ delete (*it);
+ (*it) = NULL;
}
}
- featureList.clear() ;
- desiredFeatureList.clear() ;
+ featureList.clear();
+ desiredFeatureList.clear();
taskWasKilled = true;
}
}
@@ -221,31 +222,32 @@ void vpServo::kill()
*/
void vpServo::setServo(const vpServoType &servo_type)
{
- this->servoType = servo_type ;
+ this->servoType = servo_type;
- if ((servoType==EYEINHAND_CAMERA) ||(servoType==EYEINHAND_L_cVe_eJe))
- signInteractionMatrix = 1 ;
+ if ((servoType == EYEINHAND_CAMERA) || (servoType == EYEINHAND_L_cVe_eJe))
+ signInteractionMatrix = 1;
else
- signInteractionMatrix = -1 ;
+ signInteractionMatrix = -1;
// when the control is directly compute in the camera frame
// we relieve the end-user to initialize cVa and aJe
- if (servoType==EYEINHAND_CAMERA)
- {
- vpVelocityTwistMatrix _cVe ; set_cVe(_cVe) ;
+ if (servoType == EYEINHAND_CAMERA) {
+ vpVelocityTwistMatrix _cVe;
+ set_cVe(_cVe);
- vpMatrix _eJe ;
- _eJe.eye(6) ;
- set_eJe(_eJe) ;
+ vpMatrix _eJe;
+ _eJe.eye(6);
+ set_eJe(_eJe);
};
}
/*!
- Set a 6-dim column vector representing the degrees of freedom that are controlled
- in the camera frame. When set to 1, all the 6 dof are controlled.
+ Set a 6-dim column vector representing the degrees of freedom that are
+controlled in the camera frame. When set to 1, all the 6 dof are controlled.
\param dof : Degrees of freedom to control in the camera frame.
- Below we give the correspondance between the index of the vector and the considered dof:
+ Below we give the correspondance between the index of the vector and the
+considered dof:
- dof[0] = 1 if translation along X is controled, 0 otherwise;
- dof[1] = 1 if translation along Y is controled, 0 otherwise;
- dof[2] = 1 if translation along Z is controled, 0 otherwise;
@@ -253,8 +255,9 @@ void vpServo::setServo(const vpServoType &servo_type)
- dof[4] = 1 if rotation along Y is controled, 0 otherwise;
- 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
+ The following example shows how to use this function to control only wx, wy
+like a pan/tilt:
+\code
#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/vs/vpServo.h>
@@ -283,17 +286,14 @@ int main()
}
\endcode
*/
-void
-vpServo::setCameraDoF(const vpColVector& dof)
+void vpServo::setCameraDoF(const vpColVector &dof)
{
- if(dof.size() == 6)
- {
+ if (dof.size() == 6) {
iscJcIdentity = true;
- for(unsigned int i = 0 ; i < 6 ; i++) {
- if(std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()){
+ for (unsigned int i = 0; i < 6; i++) {
+ if (std::fabs(dof[i]) > std::numeric_limits<double>::epsilon()) {
cJc[i][i] = 1.0;
- }
- else{
+ } else {
cJc[i][i] = 0.0;
iscJcIdentity = false;
}
@@ -301,168 +301,151 @@ vpServo::setCameraDoF(const vpColVector& dof)
}
}
-
/*!
Prints on \e os stream information about the task:
- \param displayLevel : Indicates which are the task information to print. See vpServo::vpServoPrintType for more details.
+ \param displayLevel : Indicates which are the task information to print. See
+ vpServo::vpServoPrintType for more details.
\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:
- {
- os << "Visual servoing task: " <<std::endl ;
-
- os << "Type of control law " <<std::endl ;
- switch( servoType )
- {
- case NONE :
- os << "Type of task have not been chosen yet ! " << std::endl ;
- break ;
- case EYEINHAND_CAMERA :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the camera frame " << std::endl ;
- break ;
- case EYEINHAND_L_cVe_eJe :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the articular frame " << std::endl ;
- break ;
- case EYETOHAND_L_cVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
- break ;
+ switch (displayLevel) {
+ case vpServo::ALL: {
+ os << "Visual servoing task: " << std::endl;
+
+ os << "Type of control law " << std::endl;
+ switch (servoType) {
+ case NONE:
+ os << "Type of task have not been chosen yet ! " << std::endl;
+ break;
+ case EYEINHAND_CAMERA:
+ os << "Eye-in-hand configuration " << std::endl;
+ os << "Control in the camera frame " << std::endl;
+ break;
+ case EYEINHAND_L_cVe_eJe:
+ os << "Eye-in-hand configuration " << std::endl;
+ os << "Control in the articular frame " << std::endl;
+ break;
+ case EYETOHAND_L_cVe_eJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
+ break;
+ case EYETOHAND_L_cVf_fJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
+ break;
}
- os << "List of visual features : s" <<std::endl ;
+ os << "List of visual features : s" << std::endl;
std::list<vpBasicFeature *>::const_iterator it_s;
std::list<vpBasicFeature *>::const_iterator it_s_star;
std::list<unsigned int>::const_iterator it_select;
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- os << "" ;
- (*it_s)->print( (*it_select) ) ;
+ for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
+ ++it_s, ++it_select) {
+ os << "";
+ (*it_s)->print((*it_select));
}
- os << "List of desired visual features : s*" <<std::endl ;
- for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
- {
- os << "" ;
- (*it_s_star)->print( (*it_select) ) ;
+ os << "List of desired visual features : s*" << std::endl;
+ for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
+ it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
+ os << "";
+ (*it_s_star)->print((*it_select));
}
- os <<"Interaction Matrix Ls "<<std::endl ;
- if (interactionMatrixComputed)
- {
+ os << "Interaction Matrix Ls " << std::endl;
+ if (interactionMatrixComputed) {
os << L << std::endl;
- }
- else
- {
- os << "not yet computed "<<std::endl ;
+ } else {
+ os << "not yet computed " << std::endl;
}
- os <<"Error vector (s-s*) "<<std::endl ;
- if (errorComputed)
- {
+ os << "Error vector (s-s*) " << std::endl;
+ if (errorComputed) {
os << error.t() << std::endl;
- }
- else
- {
- os << "not yet computed "<<std::endl ;
+ } else {
+ os << "not yet computed " << std::endl;
}
- os << "Gain : " << lambda <<std::endl ;
+ os << "Gain : " << lambda << std::endl;
break;
}
- case vpServo::CONTROLLER:
- {
- os << "Type of control law " <<std::endl ;
- switch( servoType )
- {
- case NONE :
- os << "Type of task have not been chosen yet ! " << std::endl ;
- break ;
- case EYEINHAND_CAMERA :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the camera frame " << std::endl ;
- break ;
- case EYEINHAND_L_cVe_eJe :
- os << "Eye-in-hand configuration " << std::endl ;
- os << "Control in the articular frame " << std::endl ;
- break ;
- case EYETOHAND_L_cVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- os << "Eye-to-hand configuration " << std::endl ;
- os << "s_dot = _L_cVf_fJe q_dot " << std::endl ;
- break ;
+ case vpServo::CONTROLLER: {
+ os << "Type of control law " << std::endl;
+ switch (servoType) {
+ case NONE:
+ os << "Type of task have not been chosen yet ! " << std::endl;
+ break;
+ case EYEINHAND_CAMERA:
+ os << "Eye-in-hand configuration " << std::endl;
+ os << "Control in the camera frame " << std::endl;
+ break;
+ case EYEINHAND_L_cVe_eJe:
+ os << "Eye-in-hand configuration " << std::endl;
+ os << "Control in the articular frame " << std::endl;
+ break;
+ case EYETOHAND_L_cVe_eJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVe_eJe q_dot " << std::endl;
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVe_fVe_eJe q_dot " << std::endl;
+ break;
+ case EYETOHAND_L_cVf_fJe:
+ os << "Eye-to-hand configuration " << std::endl;
+ os << "s_dot = _L_cVf_fJe q_dot " << std::endl;
+ break;
}
break;
}
- case vpServo::FEATURE_CURRENT:
- {
- os << "List of visual features : s" <<std::endl ;
+ case vpServo::FEATURE_CURRENT: {
+ os << "List of visual features : s" << std::endl;
std::list<vpBasicFeature *>::const_iterator it_s;
std::list<unsigned int>::const_iterator it_select;
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- os << "" ;
- (*it_s)->print( (*it_select) ) ;
+ for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end();
+ ++it_s, ++it_select) {
+ os << "";
+ (*it_s)->print((*it_select));
}
break;
}
- case vpServo::FEATURE_DESIRED:
- {
- os << "List of desired visual features : s*" <<std::endl ;
+ case vpServo::FEATURE_DESIRED: {
+ os << "List of desired visual features : s*" << std::endl;
std::list<vpBasicFeature *>::const_iterator it_s_star;
std::list<unsigned int>::const_iterator it_select;
- for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin(); it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select)
- {
- os << "" ;
- (*it_s_star)->print( (*it_select) ) ;
+ for (it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
+ it_s_star != desiredFeatureList.end(); ++it_s_star, ++it_select) {
+ os << "";
+ (*it_s_star)->print((*it_select));
}
break;
}
- case vpServo::GAIN:
- {
- os << "Gain : " << lambda <<std::endl ;
+ case vpServo::GAIN: {
+ os << "Gain : " << lambda << std::endl;
break;
}
- case vpServo::INTERACTION_MATRIX:
- {
- os <<"Interaction Matrix Ls "<<std::endl ;
+ case vpServo::INTERACTION_MATRIX: {
+ os << "Interaction Matrix Ls " << std::endl;
if (interactionMatrixComputed) {
os << L << std::endl;
- }
- else {
- os << "not yet computed "<<std::endl ;
+ } else {
+ os << "not yet computed " << std::endl;
}
break;
}
@@ -471,12 +454,11 @@ vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
case vpServo::MINIMUM:
{
- os <<"Error vector (s-s*) "<<std::endl ;
+ os << "Error vector (s-s*) " << std::endl;
if (errorComputed) {
os << error.t() << std::endl;
- }
- else {
- os << "not yet computed "<<std::endl ;
+ } else {
+ os << "not yet computed " << std::endl;
}
break;
@@ -489,18 +471,21 @@ vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
\param s_cur : Current visual feature denoted \f$\bf s\f$.
\param s_star : Desired visual feature denoted \f${\bf s}^*\f$.
- \param select : Feature selector. By default all the features in \e s and \e s_star are used,
- but is is possible to specify which one is used in case of multiple features.
+ \param select : Feature selector. By default all the features in \e s and \e
+ s_star are used, but is is possible to specify which one is used in case of
+ multiple features.
- The following sample code explain how to use this method to add a visual feature point \f$(x,y)\f$:
- \code
+ The following sample code explain how to use this method to add a visual
+ feature point \f$(x,y)\f$:
+ \code
vpFeaturePoint s, s_star;
...
vpServo task;
task.addFeature(s, s_star);
\endcode
- For example to use only the \f$x\f$ visual feature, the previous code becomes:
+ For example to use only the \f$x\f$ visual feature, the previous code
+ becomes:
\code
vpFeaturePoint s, s_star;
...
@@ -509,21 +494,24 @@ vpServo::print(const vpServo::vpServoPrintType displayLevel, std::ostream &os)
\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 );
- featureSelectionList.push_back( select );
+ featureList.push_back(&s_cur);
+ desiredFeatureList.push_back(&s_star);
+ featureSelectionList.push_back(select);
}
/*!
- Add a new features \f$\bf s\f$ in the task. The desired visual feature denoted \f${\bf s}^*\f$ is equal to zero.
+ Add a new features \f$\bf s\f$ in the task. The desired visual feature
+ denoted \f${\bf s}^*\f$ is equal to zero.
\param s_cur : Current visual feature denoted \f$\bf s\f$.
- \param select : Feature selector. By default all the features in \e s are used,
- but is is possible to specify which one is used in case of multiple features.
+ \param select : Feature selector. By default all the features in \e s are
+ used, but is is possible to specify which one is used in case of multiple
+ features.
- The following sample code explain how to use this method to add a \f$\theta {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature:
+ The following sample code explain how to use this method to add a \f$\theta
+ {\bf u} =(\theta u_x, \theta u_y, \theta u_z)\f$ feature:
\code
vpFeatureThetaU s(vpFeatureThetaU::cRcd);
...
@@ -531,7 +519,8 @@ void vpServo::addFeature(vpBasicFeature& s_cur, vpBasicFeature &s_star, unsigned
task.addFeature(s);
\endcode
- For example to use only the \f$\theta u_x\f$ feature, the previous code becomes:
+ For example to use only the \f$\theta u_x\f$ feature, the previous code
+ becomes:
\code
vpFeatureThetaU s(vpFeatureThetaU::cRcd);
...
@@ -539,9 +528,9 @@ void vpServo::addFeature(vpBasicFeature& s_cur, vpBasicFeature &s_star, unsigned
task.addFeature(s, vpFeatureThetaU::selectTUx);
\endcode
*/
-void vpServo::addFeature(vpBasicFeature& s_cur, unsigned int select)
+void vpServo::addFeature(vpBasicFeature &s_cur, unsigned int select)
{
- featureList.push_back( &s_cur );
+ featureList.push_back(&s_cur);
// in fact we have a problem with s_star that is not defined
// by the end user.
@@ -557,26 +546,26 @@ void vpServo::addFeature(vpBasicFeature& s_cur, unsigned int select)
// vpServo must deallocate the memory (see ~vpServo and kill() )
- vpBasicFeature *s_star ;
- s_star = s_cur.duplicate() ;
+ vpBasicFeature *s_star;
+ s_star = s_cur.duplicate();
- s_star->init() ;
- s_star->setDeallocate(vpBasicFeature::vpServo) ;
+ s_star->init();
+ s_star->setDeallocate(vpBasicFeature::vpServo);
- desiredFeatureList.push_back( s_star );
- featureSelectionList.push_back( select );
+ desiredFeatureList.push_back(s_star);
+ featureSelectionList.push_back(select);
}
//! Return the task dimension.
unsigned int vpServo::getDimension() const
{
- unsigned int dim = 0 ;
+ unsigned int dim = 0;
std::list<vpBasicFeature *>::const_iterator it_s;
std::list<unsigned int>::const_iterator it_select;
- for (it_s = featureList.begin(), it_select = featureSelectionList.begin(); it_s != featureList.end(); ++it_s, ++it_select)
- {
- dim += (*it_s)->getDimension(*it_select) ;
+ 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;
@@ -585,20 +574,16 @@ unsigned int vpServo::getDimension() const
void vpServo::setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrix_type,
const vpServoInversionType &interactionMatrixInversion)
{
- this->interactionMatrixType = interactionMatrix_type ;
- this->inversionType = interactionMatrixInversion ;
+ this->interactionMatrixType = interactionMatrix_type;
+ this->inversionType = interactionMatrixInversion;
}
-
-static void computeInteractionMatrixFromList (const std::list<vpBasicFeature *> & featureList,
- const std::list<unsigned int> & featureSelectionList,
- vpMatrix & L)
+static void computeInteractionMatrixFromList(const std::list<vpBasicFeature *> &featureList,
+ const std::list<unsigned int> &featureSelectionList, vpMatrix &L)
{
- if (featureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
+ if (featureList.empty()) {
+ vpERROR_TRACE("feature list empty, cannot compute Ls");
+ throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
}
/* The matrix dimension is not known before the affectation loop.
@@ -617,7 +602,7 @@ static void computeInteractionMatrixFromList (const std::list<vpBasicFeature *>
const unsigned int colL = 6;
if (0 == rowL) {
rowL = 1;
- L .resize(rowL, colL);
+ L.resize(rowL, colL);
}
/* vectTmp is used to store the return values of functions get_s() and
@@ -632,118 +617,95 @@ static void computeInteractionMatrixFromList (const std::list<vpBasicFeature *>
std::list<vpBasicFeature *>::const_iterator it;
std::list<unsigned int>::const_iterator it_select;
- for (it = featureList.begin(), it_select = featureSelectionList.begin(); it != featureList.end(); ++it, ++it_select)
- {
+ 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();
- unsigned int colMatrixTmp = matrixTmp .getCols();
+ matrixTmp = (*it)->interaction(*it_select);
+ unsigned int rowMatrixTmp = matrixTmp.getRows();
+ unsigned int colMatrixTmp = matrixTmp.getCols();
/* Check the matrix L size, and realloc if needed. */
while (rowMatrixTmp + cursorL > rowL) {
rowL *= 2;
- L.resize (rowL,colL,false);
- vpDEBUG_TRACE(15,"Realloc!");
+ L.resize(rowL, colL, false);
+ vpDEBUG_TRACE(15, "Realloc!");
}
/* Copy the temporarily matrix into L. */
for (unsigned int k = 0; k < rowMatrixTmp; ++k, ++cursorL) {
- for (unsigned int j = 0; j < colMatrixTmp; ++j) {
+ for (unsigned int j = 0; j < colMatrixTmp; ++j) {
L[cursorL][j] = matrixTmp[k][j];
}
}
}
- L.resize (cursorL,colL,false);
+ L.resize(cursorL, colL, false);
- return ;
+ return;
}
-
/*!
- Compute and return the interaction matrix related to the set of visual features.
+ Compute and return the interaction matrix related to the set of visual
+ features.
- \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the control law specified using setServo().
+ \return The interaction matrix \f${\widehat {\bf L}}_e\f$ used in the
+ control law specified using setServo().
*/
vpMatrix vpServo::computeInteractionMatrix()
{
try {
- switch (interactionMatrixType)
- {
- case CURRENT:
- {
- try
- {
- computeInteractionMatrixFromList(this ->featureList,
- this ->featureSelectionList,
- L);
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
+ switch (interactionMatrixType) {
+ case CURRENT: {
+ try {
+ computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
+ dim_task = L.getRows();
+ interactionMatrixComputed = true;
}
- catch(...)
- {
- throw ;
+ catch (...) {
+ throw;
}
- }
- break ;
- case DESIRED:
- {
- try
- {
- if (interactionMatrixComputed == false || forceInteractionMatrixComputation == true)
- {
- computeInteractionMatrixFromList(this ->desiredFeatureList,
- this ->featureSelectionList, L);
-
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
+ } break;
+ case DESIRED: {
+ try {
+ if (interactionMatrixComputed == false || forceInteractionMatrixComputation == true) {
+ computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, L);
+
+ dim_task = L.getRows();
+ interactionMatrixComputed = true;
}
+ } catch (...) {
+ throw;
}
- catch(...)
- {
- throw ;
- }
- }
- break ;
- case MEAN:
- {
- vpMatrix Lstar (L.getRows(), L.getCols());
- try
- {
- computeInteractionMatrixFromList(this ->featureList,
- this ->featureSelectionList, L);
- computeInteractionMatrixFromList(this ->desiredFeatureList,
- this ->featureSelectionList, Lstar);
- }
- catch(...)
- {
- throw ;
+ } break;
+ case MEAN: {
+ vpMatrix Lstar(L.getRows(), L.getCols());
+ try {
+ computeInteractionMatrixFromList(this->featureList, this->featureSelectionList, L);
+ computeInteractionMatrixFromList(this->desiredFeatureList, this->featureSelectionList, Lstar);
+ } catch (...) {
+ throw;
}
- L = (L+Lstar)/2;
+ L = (L + Lstar) / 2;
- dim_task = L.getRows() ;
- interactionMatrixComputed = true ;
- }
- break ;
+ dim_task = L.getRows();
+ interactionMatrixComputed = true;
+ } break;
case USER_DEFINED:
// dim_task = L.getRows() ;
- interactionMatrixComputed = false ;
+ interactionMatrixComputed = false;
break;
}
+ } catch (...) {
+ throw;
}
- catch(...)
- {
- throw ;
- }
- return L ;
+ return L;
}
-/*!
+/*!
Compute the error \f$\bf e =(s - s^*)\f$ between the current set of visual
features \f$\bf s\f$ and the desired set of visual features \f$\bf s^*\f$.
@@ -753,43 +715,48 @@ vpMatrix vpServo::computeInteractionMatrix()
*/
vpColVector vpServo::computeError()
{
- if (featureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
+ if (featureList.empty()) {
+ vpERROR_TRACE("feature list empty, cannot compute Ls");
+ throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
}
- if (desiredFeatureList.empty())
- {
- vpERROR_TRACE("feature list empty, cannot compute Ls") ;
- throw(vpServoException(vpServoException::noFeatureError,
- "feature list empty, cannot compute Ls")) ;
+ if (desiredFeatureList.empty()) {
+ vpERROR_TRACE("feature list empty, cannot compute Ls");
+ throw(vpServoException(vpServoException::noFeatureError, "feature list empty, cannot compute Ls"));
}
try {
- vpBasicFeature *current_s ;
- vpBasicFeature *desired_s ;
+ vpBasicFeature *current_s;
+ vpBasicFeature *desired_s;
/* The vector dimensions are not known before the affectation loop.
* They thus should be allocated on the flight, in the loop.
* The first assumption is that the size has not changed. A double
* reallocation (realloc(dim*2)) is done if necessary. In particular,
* [log_2(dim)+1] reallocations are done for the first error computation.
- * If the allocated size is too large, a correction is done after the loop.
- * The algorithmic cost is linear in affectation, logarithmic in allocation
- * numbers and linear in allocation size.
- * No assumptions are made concerning size of each vector: they are
- * not said equal, and could be different.
+ * If the allocated size is too large, a correction is done after the
+ * loop. The algorithmic cost is linear in affectation, logarithmic in
+ * allocation numbers and linear in allocation size. No assumptions are
+ * made concerning size of each vector: they are not said equal, and could
+ * be different.
*/
/* First assumption: vector dimensions have not changed. If 0, they are
* initialized to dim 1.*/
- unsigned int dimError = error .getRows();
- unsigned int dimS = s .getRows();
- unsigned int dimSStar = sStar .getRows();
- if (0 == dimError) { dimError = 1; error .resize(dimError);}
- if (0 == dimS) { dimS = 1; s .resize(dimS);}
- if (0 == dimSStar) { dimSStar = 1; sStar .resize(dimSStar);}
+ unsigned int dimError = error.getRows();
+ unsigned int dimS = s.getRows();
+ unsigned int dimSStar = sStar.getRows();
+ if (0 == dimError) {
+ dimError = 1;
+ error.resize(dimError);
+ }
+ if (0 == dimS) {
+ dimS = 1;
+ s.resize(dimS);
+ }
+ if (0 == dimSStar) {
+ dimSStar = 1;
+ sStar.resize(dimSStar);
+ }
/* vectTmp is used to store the return values of functions get_s() and
* error(). */
@@ -808,132 +775,143 @@ vpColVector vpServo::computeError()
std::list<unsigned int>::const_iterator it_select;
for (it_s = featureList.begin(), it_s_star = desiredFeatureList.begin(), it_select = featureSelectionList.begin();
- it_s != featureList.end();
- ++it_s, ++it_s_star, ++it_select)
- {
- current_s = (*it_s);
- desired_s = (*it_s_star);
+ it_s != featureList.end(); ++it_s, ++it_s_star, ++it_select) {
+ current_s = (*it_s);
+ desired_s = (*it_s_star);
unsigned int select = (*it_select);
/* Get s, and store it in the s vector. */
vectTmp = current_s->get_s(select);
- unsigned int dimVectTmp = vectTmp .getRows();
- while (dimVectTmp + cursorS > dimS)
- { dimS *= 2; s .resize (dimS,false); vpDEBUG_TRACE(15,"Realloc!"); }
- for (unsigned int k = 0; k < dimVectTmp; ++k) { s[cursorS++] = vectTmp[k]; }
+ unsigned int dimVectTmp = vectTmp.getRows();
+ while (dimVectTmp + cursorS > dimS) {
+ dimS *= 2;
+ s.resize(dimS, false);
+ vpDEBUG_TRACE(15, "Realloc!");
+ }
+ for (unsigned int k = 0; k < dimVectTmp; ++k) {
+ s[cursorS++] = vectTmp[k];
+ }
/* Get s_star, and store it in the s vector. */
vectTmp = desired_s->get_s(select);
- dimVectTmp = vectTmp .getRows();
+ dimVectTmp = vectTmp.getRows();
while (dimVectTmp + cursorSStar > dimSStar) {
dimSStar *= 2;
- sStar.resize (dimSStar,false);
+ sStar.resize(dimSStar, false);
}
- for (unsigned int k = 0; k < dimVectTmp; ++k) {
+ for (unsigned int k = 0; k < dimVectTmp; ++k) {
sStar[cursorSStar++] = vectTmp[k];
}
/* Get error, and store it in the s vector. */
- vectTmp = current_s->error(*desired_s, select) ;
- dimVectTmp = vectTmp .getRows();
+ vectTmp = current_s->error(*desired_s, select);
+ dimVectTmp = vectTmp.getRows();
while (dimVectTmp + cursorError > dimError) {
dimError *= 2;
- error.resize (dimError,false);
+ error.resize(dimError, false);
}
- for (unsigned int k = 0; k < dimVectTmp; ++k) {
+ for (unsigned int k = 0; k < dimVectTmp; ++k) {
error[cursorError++] = vectTmp[k];
}
}
/* If too much memory has been allocated, realloc. */
- s .resize(cursorS,false);
- sStar .resize(cursorSStar,false);
- error .resize(cursorError,false);
+ s.resize(cursorS, false);
+ sStar.resize(cursorSStar, false);
+ error.resize(cursorError, false);
/* Final modifications. */
- dim_task = error.getRows() ;
- errorComputed = true ;
- }
- catch(...)
- {
- throw ;
+ dim_task = error.getRows();
+ errorComputed = true;
+ } catch (...) {
+ throw;
}
- return error ;
+ return error;
}
bool vpServo::testInitialization()
{
- switch (servoType)
- {
+ switch (servoType) {
case NONE:
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
+ vpERROR_TRACE("No control law have been yet defined");
+ throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
+ break;
case EYEINHAND_CAMERA:
- return true ;
- break ;
+ return true;
+ break;
case EYEINHAND_L_cVe_eJe:
- case EYETOHAND_L_cVe_eJe:
- if (!init_cVe) vpERROR_TRACE("cVe not initialized") ;
- if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
- return (init_cVe && init_eJe) ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
- if (!init_fVe) vpERROR_TRACE("fVe not initialized") ;
- if (!init_eJe) vpERROR_TRACE("eJe not initialized") ;
- return (init_cVf && init_fVe && init_eJe) ;
- break ;
-
- case EYETOHAND_L_cVf_fJe :
- if (!init_cVf) vpERROR_TRACE("cVf not initialized") ;
- if (!init_fJe) vpERROR_TRACE("fJe not initialized") ;
- return (init_cVf && init_fJe) ;
- break ;
+ case EYETOHAND_L_cVe_eJe:
+ if (!init_cVe)
+ vpERROR_TRACE("cVe not initialized");
+ if (!init_eJe)
+ vpERROR_TRACE("eJe not initialized");
+ return (init_cVe && init_eJe);
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ if (!init_cVf)
+ vpERROR_TRACE("cVf not initialized");
+ if (!init_fVe)
+ vpERROR_TRACE("fVe not initialized");
+ if (!init_eJe)
+ vpERROR_TRACE("eJe not initialized");
+ return (init_cVf && init_fVe && init_eJe);
+ break;
+
+ case EYETOHAND_L_cVf_fJe:
+ if (!init_cVf)
+ vpERROR_TRACE("cVf not initialized");
+ if (!init_fJe)
+ vpERROR_TRACE("fJe not initialized");
+ return (init_cVf && init_fJe);
+ break;
}
- return false ;
+ return false;
}
bool vpServo::testUpdated()
{
- switch (servoType)
- {
+ switch (servoType) {
case NONE:
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
+ vpERROR_TRACE("No control law have been yet defined");
+ throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
+ break;
case EYEINHAND_CAMERA:
- return true ;
+ return true;
case EYEINHAND_L_cVe_eJe:
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_eJe) ;
- break ;
- case EYETOHAND_L_cVe_eJe:
- if (!init_cVe) vpERROR_TRACE("cVe not updated") ;
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_cVe && init_eJe) ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- if (!init_fVe) vpERROR_TRACE("fVe not updated") ;
- if (!init_eJe) vpERROR_TRACE("eJe not updated") ;
- return (init_fVe && init_eJe) ;
- break ;
-
- case EYETOHAND_L_cVf_fJe :
- if (!init_fJe) vpERROR_TRACE("fJe not updated") ;
- return (init_fJe) ;
- break ;
+ if (!init_eJe)
+ vpERROR_TRACE("eJe not updated");
+ return (init_eJe);
+ break;
+ case EYETOHAND_L_cVe_eJe:
+ if (!init_cVe)
+ vpERROR_TRACE("cVe not updated");
+ if (!init_eJe)
+ vpERROR_TRACE("eJe not updated");
+ return (init_cVe && init_eJe);
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ if (!init_fVe)
+ vpERROR_TRACE("fVe not updated");
+ if (!init_eJe)
+ vpERROR_TRACE("eJe not updated");
+ return (init_fVe && init_eJe);
+ break;
+
+ case EYETOHAND_L_cVf_fJe:
+ if (!init_fJe)
+ vpERROR_TRACE("fJe not updated");
+ return (init_fJe);
+ break;
}
- return false ;
+ return false;
}
/*!
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available. The \ref tutorial-ibvs and \ref tutorial-boost-vs
- are also useful to illustrate the usage of this function.
+ Compute the control law specified using setServo(). See vpServo::vpServoType
+ for more details concerning the control laws that are available. The \ref
+ tutorial-ibvs and \ref tutorial-boost-vs are also useful to illustrate the
+ usage of this function.
The general form of the control law is the following:
@@ -942,494 +920,479 @@ bool vpServo::testUpdated()
\f]
where :
- - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
+ - \f${\bf \dot q}\f$ is the resulting velocity command to apply to the
+ robot.
+ - the sign of the control law depends on the eye in hand or eye to hand
+ configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction
+ matrix and of the robot Jacobian.
- \f$\bf e = (s-s^*)\f$ is the error to regulate.
- To ensure continuous sequencing the computeControlLaw(double) function can be used. It will ensure that
- the velocities that are computed are continuous.
+ To ensure continuous sequencing the computeControlLaw(double) function can
+ be used. It will ensure that the velocities that are computed are
+ continuous.
*/
vpColVector vpServo::computeControlLaw()
{
- static int iteration =0;
+ static int iteration = 0;
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
+ try {
+ vpVelocityTwistMatrix cVa; // Twist transformation matrix
+ vpMatrix aJe; // Jacobian
- if (iteration==0)
- {
+ 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")) ;
+ vpERROR_TRACE("All the matrices are not correctly initialized");
+ throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized"));
}
}
if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
+ vpERROR_TRACE("All the matrices are not correctly updated");
}
// test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
+ switch (servoType) {
+ case NONE:
+ vpERROR_TRACE("No control law have been yet defined");
+ throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
+ break;
case EYEINHAND_CAMERA:
case EYEINHAND_L_cVe_eJe:
case EYETOHAND_L_cVe_eJe:
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
+ cVa = cVe;
+ aJe = eJe;
+
+ init_cVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf * fVe;
+ aJe = eJe;
+ init_fVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fJe:
+ cVa = cVf;
+ aJe = fJe;
+ init_fJe = false;
+ break;
}
- computeInteractionMatrix() ;
- computeError() ;
+ computeInteractionMatrix();
+ computeError();
// compute task Jacobian
- if(iscJcIdentity)
- J1 = L*cVa*aJe ;
+ if (iscJcIdentity)
+ J1 = L * cVa * aJe;
else
- J1 = L*cJc*cVa*aJe ;
+ J1 = L * cJc * cVa * aJe;
// handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
+ J1 *= signInteractionMatrix;
// pseudo inverse of the task Jacobian
// and rank of the task Jacobian
// the image of J1 is also computed to allows the computation
// of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
+ vpMatrix imJ1t, imJ1;
+ bool imageComputed = false;
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+ if (inversionType == PSEUDO_INVERSE) {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
+ imageComputed = true;
+ } else
+ J1p = J1.t();
- if (rankJ1 == J1.getCols())
- {
+ if (rankJ1 == J1.getCols()) {
/* if no degrees of freedom remains (rank J1 = ndof)
WpW = I, multiply by WpW is useless
*/
- e1 = J1p*error ;// primary task
+ e1 = J1p * error; // primary task
- WpW.eye(J1.getCols(), J1.getCols()) ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
+ WpW.eye(J1.getCols(), J1.getCols());
+ } else {
+ if (imageComputed != true) {
+ vpMatrix Jtmp;
// image of J1 is computed to allows the computation
// of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
}
- WpW = imJ1t*imJ1t.t() ;
+ WpW = imJ1t * imJ1t.t();
#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
+ std::cout << "rank J1: " << rankJ1 << std::endl;
+ imJ1t.print(std::cout, 10, "imJ1t");
+ imJ1.print(std::cout, 10, "imJ1");
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
+ WpW.print(std::cout, 10, "WpW");
+ J1.print(std::cout, 10, "J1");
+ J1p.print(std::cout, 10, "J1p");
#endif
- e1 = WpW*J1p*error ;
+ e1 = WpW * J1p * error;
}
- e = - lambda(e1) * e1 ;
+ e = -lambda(e1) * e1;
- vpMatrix I ;
+ vpMatrix I;
- I.eye(J1.getCols(),J1.getCols()) ;
+ I.eye(J1.getCols(), J1.getCols());
computeProjectionOperators();
- }
- catch(...) {
+ } catch (...) {
throw;
}
- iteration++ ;
- return e ;
+ iteration++;
+ return e;
}
/*!
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available. The \ref tutorial-boost-vs is also useful to
- illustrate the usage of this function.
+ Compute the control law specified using setServo(). See vpServo::vpServoType
+ for more details concerning the control laws that are available. The \ref
+ tutorial-boost-vs is also useful to illustrate the usage of this function.
- To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
- the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
- continuous velocities by avoiding abrupt changes in the command.
+ To the general form of the control law given in computeControlLaw(), we add
+ here an additional term that comes from the task sequencing approach
+ described in \cite Mansard07e equation (17). This additional term allows to
+ compute continuous velocities by avoiding abrupt changes in the command.
The form of the control law considered here is the following:
\f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t)
- \f]
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} \mp \lambda {{\bf
+ \widehat J}_{e(0)}}^+ {{\bf e}(0)} \exp(-\mu t) \f]
where :
- - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
+ - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply
+ to the robot.
+ - the sign of the control law depends on the eye in hand or eye to hand
+ configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction
+ matrix and of the robot Jacobian.
- \f$\bf e = (s-s^*)\f$ is the error to regulate.
- \f$t\f$ is the time given as parameter of this method.
- - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
- - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
- This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
-
- \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
+ - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified
+ using setMu().
+ - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf
+ \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored
+ either at the first call of this method, or when \e t parameter is set to 0.
+
+ \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+
+ {{\bf e}(0)}\f$ is refreshed internally.
*/
vpColVector vpServo::computeControlLaw(double t)
{
- static int iteration =0;
- //static vpColVector e1_initial;
+ static int iteration = 0;
+ // static vpColVector e1_initial;
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
+ try {
+ vpVelocityTwistMatrix cVa; // Twist transformation matrix
+ vpMatrix aJe; // Jacobian
- if (iteration==0)
- {
+ 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")) ;
+ vpERROR_TRACE("All the matrices are not correctly initialized");
+ throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized"));
}
}
if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
+ vpERROR_TRACE("All the matrices are not correctly updated");
}
// test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
+ switch (servoType) {
+ case NONE:
+ vpERROR_TRACE("No control law have been yet defined");
+ throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
+ break;
case EYEINHAND_CAMERA:
case EYEINHAND_L_cVe_eJe:
case EYETOHAND_L_cVe_eJe:
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
+ cVa = cVe;
+ aJe = eJe;
+
+ init_cVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf * fVe;
+ aJe = eJe;
+ init_fVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fJe:
+ cVa = cVf;
+ aJe = fJe;
+ init_fJe = false;
+ break;
}
- computeInteractionMatrix() ;
- computeError() ;
+ computeInteractionMatrix();
+ computeError();
// compute task Jacobian
- J1 = L*cVa*aJe ;
+ J1 = L * cVa * aJe;
// handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
+ J1 *= signInteractionMatrix;
// pseudo inverse of the task Jacobian
// and rank of the task Jacobian
// the image of J1 is also computed to allows the computation
// of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
+ vpMatrix imJ1t, imJ1;
+ bool imageComputed = false;
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+ if (inversionType == PSEUDO_INVERSE) {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
+ imageComputed = true;
+ } else
+ J1p = J1.t();
- if (rankJ1 == J1.getCols())
- {
+ if (rankJ1 == J1.getCols()) {
/* if no degrees of freedom remains (rank J1 = ndof)
WpW = I, multiply by WpW is useless
*/
- e1 = J1p*error ;// primary task
+ e1 = J1p * error; // primary task
- WpW.eye(J1.getCols(), J1.getCols()) ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
+ WpW.eye(J1.getCols(), J1.getCols());
+ } else {
+ if (imageComputed != true) {
+ vpMatrix Jtmp;
// image of J1 is computed to allows the computation
// of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
}
- WpW = imJ1t*imJ1t.t() ;
+ WpW = imJ1t * imJ1t.t();
#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
+ std::cout << "rank J1 " << rankJ1 << std::endl;
+ std::cout << "imJ1t" << std::endl << imJ1t;
+ std::cout << "imJ1" << std::endl << imJ1;
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
+ std::cout << "WpW" << std::endl << WpW;
+ std::cout << "J1" << std::endl << J1;
+ std::cout << "J1p" << std::endl << J1p;
#endif
- e1 = WpW*J1p*error ;
+ e1 = WpW * J1p * error;
}
- // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
- if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
+ // memorize the initial e1 value if the function is called the first time
+ // or if the time given as parameter is equal to 0.
+ if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
e1_initial = e1;
}
- // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
+ // Security check. If size of e1_initial and e1 differ, that means that
+ // e1_initial was not set
if (e1_initial.getRows() != e1.getRows())
e1_initial = e1;
- e = - lambda(e1) * e1 + lambda(e1) * e1_initial*exp(-mu*t);
+ e = -lambda(e1) * e1 + lambda(e1) * e1_initial * exp(-mu * t);
- vpMatrix I ;
+ vpMatrix I;
- I.eye(J1.getCols(), J1.getCols()) ;
+ I.eye(J1.getCols(), J1.getCols());
- computeProjectionOperators() ;
- }
- catch(...) {
+ computeProjectionOperators();
+ } catch (...) {
throw;
}
- iteration++ ;
- return e ;
+ iteration++;
+ return e;
}
/*!
- Compute the control law specified using setServo(). See vpServo::vpServoType for more
- details concerning the control laws that are available.
+ Compute the control law specified using setServo(). See vpServo::vpServoType
+ for more details concerning the control laws that are available.
- To the general form of the control law given in computeControlLaw(), we add here an additional term that comes from
- the task sequencing approach described in \cite Mansard07e equation (17). This additional term allows to compute
- continuous velocities by avoiding abrupt changes in the command.
+ To the general form of the control law given in computeControlLaw(), we add
+ here an additional term that comes from the task sequencing approach
+ described in \cite Mansard07e equation (17). This additional term allows to
+ compute continuous velocities by avoiding abrupt changes in the command.
The form of the control law considered here is the following:
\f[
- {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu t)
- \f]
+ {\bf \dot q} = \pm \lambda {{\bf \widehat J}_e}^+ {\bf e} + \left({\bf \dot
+ e}(0) \mp \lambda {{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\right) \exp(-\mu
+ t) \f]
where :
- - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply to the robot.
- - the sign of the control law depends on the eye in hand or eye to hand configuration.
- - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction matrix and of the robot
- Jacobian.
+ - \f${\bf \dot q}\f$ is the resulting continuous velocity command to apply
+ to the robot.
+ - the sign of the control law depends on the eye in hand or eye to hand
+ configuration.
+ - \f$\bf J\f$ is the Jacobian of the task. It is function of the interaction
+ matrix and of the robot Jacobian.
- \f$\bf e = (s-s^*)\f$ is the error to regulate.
- \f$t\f$ is the time given as parameter of this method.
- - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified using setMu().
- - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$.
- This value is internally stored either at the first call of this method, or when \e t parameter is set to 0.
-
- \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+ {{\bf e}(0)}\f$ is refreshed internally.
- \param e_dot_init : Initial value of \f${\bf \dot e}(0)\f$.
+ - \f$\mu\f$ is a gain that is set by default to 4 and that could be modified
+ using setMu().
+ - \f${\bf \widehat J}_{e(0)}^+ {\bf e}(0)\f$ is the value of \f${\bf
+ \widehat J}_e^+ {\bf e}\f$ when \f$t=0\f$. This value is internally stored
+ either at the first call of this method, or when \e t parameter is set to 0.
+
+ \param t : Time in second. When set to zero, \f${{\bf \widehat J}_{e(0)}}^+
+ {{\bf e}(0)}\f$ is refreshed internally. \param e_dot_init : Initial value
+ of \f${\bf \dot e}(0)\f$.
*/
vpColVector vpServo::computeControlLaw(double t, const vpColVector &e_dot_init)
{
- static int iteration =0;
+ static int iteration = 0;
- try
- {
- vpVelocityTwistMatrix cVa ; // Twist transformation matrix
- vpMatrix aJe ; // Jacobian
+ try {
+ vpVelocityTwistMatrix cVa; // Twist transformation matrix
+ vpMatrix aJe; // Jacobian
- if (iteration==0)
- {
+ 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")) ;
+ vpERROR_TRACE("All the matrices are not correctly initialized");
+ throw(vpServoException(vpServoException::servoError, "Cannot compute control law "
+ "All the matrices are not correctly"
+ "initialized"));
}
}
if (testUpdated() == false) {
- vpERROR_TRACE("All the matrices are not correctly updated") ;
+ vpERROR_TRACE("All the matrices are not correctly updated");
}
// test if all the required initialization have been done
- switch (servoType)
- {
- case NONE :
- vpERROR_TRACE("No control law have been yet defined") ;
- throw(vpServoException(vpServoException::servoError,
- "No control law have been yet defined")) ;
- break ;
+ switch (servoType) {
+ case NONE:
+ vpERROR_TRACE("No control law have been yet defined");
+ throw(vpServoException(vpServoException::servoError, "No control law have been yet defined"));
+ break;
case EYEINHAND_CAMERA:
case EYEINHAND_L_cVe_eJe:
case EYETOHAND_L_cVe_eJe:
- cVa = cVe ;
- aJe = eJe ;
-
- init_cVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fVe_eJe:
- cVa = cVf*fVe ;
- aJe = eJe ;
- init_fVe = false ;
- init_eJe = false ;
- break ;
- case EYETOHAND_L_cVf_fJe :
- cVa = cVf ;
- aJe = fJe ;
- init_fJe = false ;
- break ;
+ cVa = cVe;
+ aJe = eJe;
+
+ init_cVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fVe_eJe:
+ cVa = cVf * fVe;
+ aJe = eJe;
+ init_fVe = false;
+ init_eJe = false;
+ break;
+ case EYETOHAND_L_cVf_fJe:
+ cVa = cVf;
+ aJe = fJe;
+ init_fJe = false;
+ break;
}
- computeInteractionMatrix() ;
- computeError() ;
+ computeInteractionMatrix();
+ computeError();
// compute task Jacobian
- J1 = L*cVa*aJe ;
+ J1 = L * cVa * aJe;
// handle the eye-in-hand eye-to-hand case
- J1 *= signInteractionMatrix ;
+ J1 *= signInteractionMatrix;
// pseudo inverse of the task Jacobian
// and rank of the task Jacobian
// the image of J1 is also computed to allows the computation
// of the projection operator
- vpMatrix imJ1t, imJ1 ;
- bool imageComputed = false ;
+ vpMatrix imJ1t, imJ1;
+ bool imageComputed = false;
- if (inversionType==PSEUDO_INVERSE)
- {
- rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t) ;
+ if (inversionType == PSEUDO_INVERSE) {
+ rankJ1 = J1.pseudoInverse(J1p, sv, 1e-6, imJ1, imJ1t);
- imageComputed = true ;
- }
- else
- J1p = J1.t() ;
+ imageComputed = true;
+ } else
+ J1p = J1.t();
- if (rankJ1 == J1.getCols())
- {
+ if (rankJ1 == J1.getCols()) {
/* if no degrees of freedom remains (rank J1 = ndof)
WpW = I, multiply by WpW is useless
*/
- e1 = J1p*error ;// primary task
+ e1 = J1p * error; // primary task
- WpW.eye(J1.getCols(), J1.getCols()) ;
- }
- else
- {
- if (imageComputed!=true)
- {
- vpMatrix Jtmp ;
+ WpW.eye(J1.getCols(), J1.getCols());
+ } else {
+ if (imageComputed != true) {
+ vpMatrix Jtmp;
// image of J1 is computed to allows the computation
// of the projection operator
- rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t) ;
+ rankJ1 = J1.pseudoInverse(Jtmp, sv, 1e-6, imJ1, imJ1t);
}
- WpW = imJ1t*imJ1t.t() ;
+ WpW = imJ1t * imJ1t.t();
#ifdef DEBUG
- std::cout << "rank J1 " << rankJ1 <<std::endl ;
- std::cout << "imJ1t"<<std::endl << imJ1t ;
- std::cout << "imJ1"<<std::endl << imJ1 ;
+ std::cout << "rank J1 " << rankJ1 << std::endl;
+ std::cout << "imJ1t" << std::endl << imJ1t;
+ std::cout << "imJ1" << std::endl << imJ1;
- std::cout << "WpW" <<std::endl <<WpW ;
- std::cout << "J1" <<std::endl <<J1 ;
- std::cout << "J1p" <<std::endl <<J1p ;
+ std::cout << "WpW" << std::endl << WpW;
+ std::cout << "J1" << std::endl << J1;
+ std::cout << "J1p" << std::endl << J1p;
#endif
- e1 = WpW*J1p*error ;
+ e1 = WpW * J1p * error;
}
- // memorize the initial e1 value if the function is called the first time or if the time given as parameter is equal to 0.
- if (iteration==0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
+ // memorize the initial e1 value if the function is called the first time
+ // or if the time given as parameter is equal to 0.
+ if (iteration == 0 || std::fabs(t) < std::numeric_limits<double>::epsilon()) {
e1_initial = e1;
}
- // Security check. If size of e1_initial and e1 differ, that means that e1_initial was not set
+ // Security check. If size of e1_initial and e1 differ, that means that
+ // e1_initial was not set
if (e1_initial.getRows() != e1.getRows())
e1_initial = e1;
- e = - lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial)*exp(-mu*t);
+ e = -lambda(e1) * e1 + (e_dot_init + lambda(e1) * e1_initial) * exp(-mu * t);
- vpMatrix I ;
+ vpMatrix I;
- I.eye(J1.getCols(), J1.getCols()) ;
+ I.eye(J1.getCols(), J1.getCols());
computeProjectionOperators();
- }
- catch(...) {
+ } catch (...) {
throw;
}
- iteration++ ;
- return e ;
+ iteration++;
+ return e;
}
void vpServo::computeProjectionOperators()
{
// Initialization
unsigned int n = J1.getCols();
- P.resize(n,n);
+ P.resize(n, n);
vpMatrix I;
I.eye(n);
- //Compute classical projection operator
- I_WpW = (I - WpW) ;
+ // Compute classical projection operator
+ I_WpW = (I - WpW);
- // Compute gain depending by the task error to ensure a smooth change between the operators.
+ // Compute gain depending by the task error to ensure a smooth change
+ // between the operators.
double e0_ = 0.1;
double e1_ = 0.7;
double sig = 0.0;
- double norm_e = error.euclideanNorm() ;
+ double norm_e = error.euclideanNorm();
if (norm_e > e1_)
sig = 1.0;
- else if (e0_ <= norm_e && norm_e <= e1_ )
- sig = 1.0 / (1.0 + exp(-12.0 * ( (norm_e-e0_)/((e1_-e0_))) + 6.0 ) );
+ else if (e0_ <= norm_e && norm_e <= e1_)
+ sig = 1.0 / (1.0 + exp(-12.0 * ((norm_e - e0_) / ((e1_ - e0_))) + 6.0));
else
sig = 0.0;
@@ -1437,11 +1400,11 @@ void vpServo::computeProjectionOperators()
double pp = (error.t() * (J1 * J1t) * error);
- vpMatrix ee_t(n,n);
- ee_t = error * error.t();
+ vpMatrix ee_t(n, n);
+ ee_t = error * error.t();
- vpMatrix P_norm_e(n,n);
- P_norm_e = I - (1.0 / pp ) * J1t * ee_t * J1;
+ vpMatrix P_norm_e(n, n);
+ P_norm_e = I - (1.0 / pp) * J1t * ee_t * J1;
P = sig * P_norm_e + (1 - sig) * I_WpW;
@@ -1449,26 +1412,32 @@ void vpServo::computeProjectionOperators()
}
/*!
- Compute and return the secondary task vector according to the classic projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper \cite Marchand05b)
- or the new large projection operator (see equation(24) in the paper \cite Marey:2010).
+ Compute and return the secondary task vector according to the classic
+ projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper
+ \cite Marchand05b) or the new large projection operator (see equation(24)
+ in the paper \cite Marey:2010).
- \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
- \param useLargeProjectionOperator : if true will be use the large projection operator, if false the classic one (default).
+ \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the
+ derivative of the secondary task \f${\bf e}_2\f$. \param
+ useLargeProjectionOperator : if true will be use the large projection
+ operator, if false the classic one (default).
\return The secondary task vector.
- If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
+ If the classic projection operator is used ( useLargeProjectionOperator =
+ false (default value)) this function return:
\f[
({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
\f]
- Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
- \f[
+ Note that the secondary task vector need than to be added to the primary
+ task which can be in the general case written as: \f[
-\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
\f]
- Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
+ Otherwise if the new large projection operator is used (
+ useLargeProjectionOperator = true ) this function return:
\f[
{\bf P}\frac{\partial {\bf e_2}}{\partial t}
@@ -1477,32 +1446,35 @@ void vpServo::computeProjectionOperators()
where
\f[
- {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
- \f]
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{
+ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \|
+ {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f]
with
\f[
- {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
- \f]
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf
+ J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf
+ e }{\bf e }^\top{\bf J_{{\bf e }} } \f]
- \warning computeControlLaw() must be call prior to this function since it updates the projection operators.
+ \warning computeControlLaw() must be call prior to this function since it
+ updates the projection operators.
- The following sample code shows how to use this method to compute a secondary task using the classic projection operator:
+ The following sample code shows how to use this method to compute a
+ secondary task using the classic projection operator:
\code
- vpColVector v; // Velocity applied to the robot
- vpColVector de2dt;
- vpServo task;
+ vpColVector v;
+ // Velocity applied to the robot vpColVector de2dt; vpServo task;
...
v = task.computeControlLaw(); // Compute the primary task
v += task.secondaryTask(de2dt) // Compute and add the secondary task using the classical projection operator
\endcode
- The following sample code shows how to use this method to compute a secondary task using the large projection operator:
+ The following sample code shows how to use this method to compute a
+ secondary task using the large projection operator:
\code
- vpColVector v; // Velocity applied to the robot
- vpColVector de2dt;
- vpServo task;
+ 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
@@ -1512,18 +1484,13 @@ void vpServo::computeProjectionOperators()
*/
vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLargeProjectionOperator)
{
- vpColVector sec ;
+ vpColVector sec;
- if (!useLargeProjectionOperator)
- {
- if (rankJ1 == J1.getCols())
- {
- vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
- throw(vpServoException(vpServoException::noDofFree,
- "no degree of freedom is free, cannot use secondary task")) ;
- }
- else
- {
+ if (!useLargeProjectionOperator) {
+ if (rankJ1 == J1.getCols()) {
+ vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
+ throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
+ } else {
#if 0
// computed in computeControlLaw()
vpMatrix I ;
@@ -1533,39 +1500,45 @@ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLarg
I_WpW = (I - WpW) ;
#endif
// std::cout << "I-WpW" << std::endl << I_WpW <<std::endl ;
- sec = I_WpW*de2dt ;
+ sec = I_WpW * de2dt;
}
}
else
- sec = P*de2dt;
+ sec = P * de2dt;
- return sec ;
+ return sec;
}
/*!
- Compute and return the secondary task vector according to the classic projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper \cite Marchand05b)
- or the new large projection operator (see equation(24) in the paper \cite Marey:2010).
+ Compute and return the secondary task vector according to the classic
+ projection operator \f${\bf I-W^+W}\f$ (see equation(7) in the paper
+ \cite Marchand05b) or the new large projection operator (see equation(24)
+ in the paper \cite Marey:2010).
\param e2 : Value of the secondary task \f${\bf e}_2\f$.
- \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the derivative of the secondary task \f${\bf e}_2\f$.
- \param useLargeProjectionOperator: if true will be use the large projection operator, if false the classic one (default).
+ \param de2dt : Value of \f$\frac{\partial {\bf e_2}}{\partial t}\f$ the
+ derivative of the secondary task \f${\bf e}_2\f$. \param
+ useLargeProjectionOperator: if true will be use the large projection
+ operator, if false the classic one (default).
\return The secondary task vector.
- If the classic projection operator is used ( useLargeProjectionOperator = false (default value)) this function return:
+ If the classic projection operator is used ( useLargeProjectionOperator =
+ false (default value)) this function return:
\f[
- -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf e_2}}{\partial t}
- \f]
+ -\lambda ({\bf I-W^+W}) {\bf e_2} + ({\bf I-W^+W})\frac{\partial {\bf
+ e_2}}{\partial t} \f]
- Note that the secondary task vector need than to be added to the primary task which can be in the general case written as:
- \f[
+ Note that the secondary task vector need than to be added to the primary
+ task which can be in the general case written as: \f[
-\lambda {\bf W^+W {\widehat {\bf J}}_e^+({\bf s-s^*})}
\f]
- Otherwise if the new large projection operator is used ( useLargeProjectionOperator = true ) this function return:
+ Otherwise if the new large projection operator is used (
+ useLargeProjectionOperator = true ) this function return:
\f[
-\lambda {\bf P} {\bf e_2} + {\bf P}\frac{\partial {\bf e_2}}{\partial t}
@@ -1574,34 +1547,37 @@ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLarg
where
\f[
- {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \| {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right )
- \f]
+ {\bf P} =\bar{\lambda }\left ( \left \| {\bf e} \right \| \right ){\bf P}_{
+ \left \| {\bf e } \right \| } \left ( 1 - \bar{\lambda }\left ( \left \|
+ {\bf e } \right \| \right ) \right ) \left ( {\bf I-W^+W}\right ) \f]
with
\f[
- {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf e }{\bf e }^\top{\bf J_{{\bf e }} }
- \f]
+ {\bf P}_{\left \| {\bf e } \right \| } = I_{n} - \frac{1}{{\bf e }^\top {\bf
+ J_{{\bf e }} } {\bf J_{{\bf e }}^\top }{\bf e }}{\bf J_{{\bf e }}^\top }{\bf
+ e }{\bf e }^\top{\bf J_{{\bf e }} } \f]
- \warning computeControlLaw() must be call prior to this function since it updates the projection operators.
+ \warning computeControlLaw() must be call prior to this function since it
+ updates the projection operators.
- The following sample code shows how to use this method to compute a secondary task using the classical projection operator:
+ The following sample code shows how to use this method to compute a
+ secondary task using the classical projection operator:
\code
- vpColVector v; // Velocity applied to the robot
- vpColVector e2;
- vpColVector de2dt;
- vpServo task;
+ vpColVector v;
+ // Velocity applied to the robot vpColVector e2; vpColVector de2dt; vpServo
+ task;
...
v = task.computeControlLaw(); // Compute the primary task
v += task.secondaryTask(e2, de2dt) // Compute and add the secondary task using the classical projection operator
\endcode
- The following sample code shows how to use this method to compute a secondary task using the large projection operator:
+ The following sample code shows how to use this method to compute a
+ secondary task using the large projection operator:
\code
- vpColVector v; // Velocity applied to the robot
- vpColVector e2;
- vpColVector de2dt;
- vpServo task;
+ 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
@@ -1610,20 +1586,16 @@ vpColVector vpServo::secondaryTask(const vpColVector &de2dt, const bool &useLarg
\sa computeControlLaw()
*/
-vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2dt, const bool &useLargeProjectionOperator)
+vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2dt,
+ const bool &useLargeProjectionOperator)
{
- vpColVector sec ;
+ vpColVector sec;
- if (!useLargeProjectionOperator)
- {
- if (rankJ1 == J1.getCols())
- {
- vpERROR_TRACE("no degree of freedom is free, cannot use secondary task") ;
- throw(vpServoException(vpServoException::noDofFree,
- "no degree of freedom is free, cannot use secondary task")) ;
- }
- else
- {
+ if (!useLargeProjectionOperator) {
+ if (rankJ1 == J1.getCols()) {
+ vpERROR_TRACE("no degree of freedom is free, cannot use secondary task");
+ throw(vpServoException(vpServoException::noDofFree, "no degree of freedom is free, cannot use secondary task"));
+ } else {
#if 0
// computed in computeControlLaw()
@@ -1635,36 +1607,45 @@ vpColVector vpServo::secondaryTask(const vpColVector &e2, const vpColVector &de2
I_WpW = (I - WpW) ;
#endif
- // To be coherent with the primary task the gain must be the same between
- // primary and secondary task.
- sec = -lambda(e1) *I_WpW*e2 + I_WpW *de2dt ;
-
-
+ // To be coherent with the primary task the gain must be the same
+ // between primary and secondary task.
+ sec = -lambda(e1) * I_WpW * e2 + I_WpW * de2dt;
}
- }
- else
- sec = -lambda(e1) * P *e2 + P *de2dt ;
-
+ } else
+ sec = -lambda(e1) * P * e2 + P * de2dt;
- return sec ;
+ return sec;
}
/*!
- Compute and return the secondary task vector for joint limit avoidance \cite Marey:2010b using the
- new large projection operator (see equation(24) in the paper \cite Marey:2010). The robot avoids the
- joint limits very smoothly even when the main task constrains all the robot degrees of freedom.
+ Compute and return the secondary task vector for joint limit avoidance
+ \cite Marey:2010b using the new large projection operator (see equation(24)
+in the paper \cite Marey:2010). The robot avoids the joint limits very
+smoothly even when the main task constrains all the robot degrees of freedom.
\param q : Actual joint positions vector
+
\param dq : Actual joint velocities vector
- \param qmin : Vector containing the low limit value of each joint in the chain.
- \param qmax : Vector containing the high limit value of each joint in the chain.
- \param rho : tuning paramenter \f${\left [ 0,\frac{1}{2} \right ]}\f$ used to define the safe configuration for the joint. When
- the joint angle value cross the max or min boundaries (\f${ q_{l_{0}}^{max} }\f$ and \f${q_{l_{0}}^{min}}\f$) the secondary
- task is actived gradually.
- \param rho1 : tuning paramenter \f${\left ] 0,1 \right ]}\f$ to compute the external boundaries (\f${q_{l_{1}}^{max}}\f$ and
- \f${q_{l_{1}}^{min}}\f$) for the joint limits. Here the secondary task it completely activated with the highest gain.
- \param lambda_tune : value \f${\left [ 0,1 \right ]}\f$ used to tune the difference in magnitude between the absolute value of the
- elements of the primary task and the elements of the secondary task. (See equation (17) \cite Marey:2010b )
+
+ \param qmin : Vector containing the low limit value of each joint in the
+chain.
+ \param qmax : Vector containing the high limit value of each joint in
+the chain.
+
+ \param rho : tuning paramenter \f${\left [ 0,\frac{1}{2} \right
+]}\f$ used to define the safe configuration for the joint. When the joint
+angle value cross the max or min boundaries (\f${ q_{l_{0}}^{max} }\f$ and
+\f${q_{l_{0}}^{min}}\f$) the secondary task is actived gradually.
+
+ \param rho1
+: tuning paramenter \f${\left ] 0,1 \right ]}\f$ to compute the external
+boundaries (\f${q_{l_{1}}^{max}}\f$ and \f${q_{l_{1}}^{min}}\f$) for the joint
+limits. Here the secondary task it completely activated with the highest gain.
+
+ \param lambda_tune : value \f${\left [ 0,1 \right ]}\f$ used to tune the
+difference in magnitude between the absolute value of the elements of the
+primary task and the elements of the secondary task. (See equation (17)
+\cite Marey:2010b )
\code
vpServo task;
@@ -1675,35 +1656,39 @@ vpColVector dq;
// Fill vector qmin and qmax with min and max limits of the joints (same joint order than vector q).
// Update vector of joint position q and velocities dq;
...
-vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
-v += task.secondaryTaskJointLimitAvoidance(q, dq, qmin, qmax) // Compute and add the secondary task for the joint
-limit avoidance using the large projection operator
+// 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
+ const double &rho, const double &rho1,
+ const double &lambda_tune) const
{
unsigned int const n = J1.getCols();
- if (qmin.size() != n || qmax.size() != n )
- {
+ if (qmin.size() != n || qmax.size() != n) {
std::stringstream msg;
- msg << "Dimension vector qmin (" << qmin.size() << ") or qmax () does not correspond to the number of jacobian columns";
+ msg << "Dimension vector qmin (" << qmin.size()
+ << ") or qmax () does not correspond to the number of jacobian "
+ "columns";
msg << "qmin size: " << qmin.size() << std::endl;
- throw(vpServoException(vpServoException::dimensionError,msg.str())) ;
+ throw(vpServoException(vpServoException::dimensionError, msg.str()));
}
- if (q.size() != n || dq.size() != n )
- {
- vpERROR_TRACE("Dimension vector q or dq does not correspont to the number of jacobian columns") ;
- throw(vpServoException(vpServoException::dimensionError,
- "Dimension vector q or dq does not correspont to the number of jacobian columns")) ;
+ if (q.size() != n || dq.size() != n) {
+ vpERROR_TRACE("Dimension vector q or dq does not correspont to the "
+ "number of jacobian columns");
+ throw(vpServoException(vpServoException::dimensionError, "Dimension vector q or dq does not correspont to "
+ "the number of jacobian columns"));
}
double lambda_l = 0.0;
- vpColVector q2 (n);
+ vpColVector q2(n);
vpColVector q_l0_min(n);
vpColVector q_l0_max(n);
@@ -1711,50 +1696,47 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, cons
vpColVector q_l1_max(n);
// Computation of gi ([nx1] vector) and lambda_l ([nx1] vector)
- vpMatrix g(n,n);
+ vpMatrix g(n, n);
vpColVector q2_i(n);
- for(unsigned int i = 0; i < n; i++)
- {
- q_l0_min[i] = qmin[i] + rho *(qmax[i] - qmin[i]);
- q_l0_max[i] = qmax[i] - rho *(qmax[i] - qmin[i]);
+ for (unsigned int i = 0; i < n; i++) {
+ q_l0_min[i] = qmin[i] + rho * (qmax[i] - qmin[i]);
+ q_l0_max[i] = qmax[i] - rho * (qmax[i] - qmin[i]);
- q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
- q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
+ q_l1_min[i] = q_l0_min[i] - rho * rho1 * (qmax[i] - qmin[i]);
+ q_l1_max[i] = q_l0_max[i] + rho * rho1 * (qmax[i] - qmin[i]);
- if (q[i] < q_l0_min[i] )
+ if (q[i] < q_l0_min[i])
g[i][i] = -1;
- else if (q[i] > q_l0_max[i] )
+ else if (q[i] > q_l0_max[i])
g[i][i] = 1;
else
- g[i][i]= 0;
+ g[i][i] = 0;
}
- for(unsigned int i = 0; i < n; i++)
- {
+ for (unsigned int i = 0; i < n; i++) {
if (q[i] > q_l0_min[i] && q[i] < q_l0_max[i])
q2_i = 0 * q2_i;
- else
- {
+ else {
vpColVector Pg_i(n);
Pg_i = (P * g.getCol(i));
- double b = ( vpMath::abs(dq[i]) )/( vpMath::abs( Pg_i[i] ) );
+ double b = (vpMath::abs(dq[i])) / (vpMath::abs(Pg_i[i]));
- if (b < 1.) // If the ratio b is big we don't activate the joint avoidance limit for the joint.
+ if (b < 1.) // If the ratio b is big we don't activate the joint
+ // avoidance limit for the joint.
{
- if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i] )
- q2_i = - (1 + lambda_tune) * b * Pg_i;
+ if (q[i] < q_l1_min[i] || q[i] > q_l1_max[i])
+ q2_i = -(1 + lambda_tune) * b * Pg_i;
- else
- {
- if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i] )
- lambda_l = 1 / (1 + exp(-12 *( (q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i]) ) + 6 ) );
+ else {
+ if (q[i] >= q_l0_max[i] && q[i] <= q_l1_max[i])
+ lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_max[i]) / (q_l1_max[i] - q_l0_max[i])) + 6));
else if (q[i] >= q_l1_min[i] && q[i] <= q_l0_min[i])
- lambda_l = 1 / (1 + exp(-12 *( (q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i]) ) + 6 ) );
+ lambda_l = 1 / (1 + exp(-12 * ((q[i] - q_l0_min[i]) / (q_l1_min[i] - q_l0_min[i])) + 6));
- q2_i = - lambda_l * (1 + lambda_tune)* b * Pg_i;
+ q2_i = -lambda_l * (1 + lambda_tune) * b * Pg_i;
}
}
}
@@ -1764,109 +1746,93 @@ vpColVector vpServo::secondaryTaskJointLimitAvoidance(const vpColVector &q, cons
}
/*!
- Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This operator is updated
- after a call of computeControlLaw().
+ Return the projection operator \f${\bf I}-{\bf W}^+{\bf W}\f$. This
+operator is updated after a call of computeControlLaw().
\code
vpServo task;
...
vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator
+ vpMatrix I_WpW = task.getI_WpW(); // Get the projection operator
\endcode
- \sa getWpW()
+ \sa getWpW()
*/
-vpMatrix vpServo::getI_WpW() const
-{
- return I_WpW;
-}
-
+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()
+ \endcode
+ \sa getP()
*/
-vpMatrix vpServo::getLargeP() const
-{
- return P;
-}
-
+vpMatrix vpServo::getLargeP() const { return P; }
/*!
- Return the task jacobian \f$J\f$. The task jacobian is updated after a call of computeControlLaw().
+ Return the task jacobian \f$J\f$. The task jacobian is updated after a call
+of computeControlLaw().
- In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
-\code
+ In the general case, the task jacobian is given by \f${\bf J} = {\widehat
+{\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
+ \code
vpServo task;
...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix J = task.getTaskJacobian(); // Get the task jacobian used to compute v
-\endcode
- \sa getTaskJacobianPseudoInverse(), getInteractionMatrix()
+ 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;
-}
+vpMatrix vpServo::getTaskJacobian() const { return J1; }
/*!
Return the pseudo inverse of the task jacobian \f$J\f$.
- In the general case, the task jacobian is given by \f${\bf J} = {\widehat {\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
+ In the general case, the task jacobian is given by \f${\bf J} = {\widehat
+{\bf L}} {^c}{\bf V}_a {^a}{\bf J}_e\f$.
- The task jacobian and its pseudo inverse are updated after a call of computeControlLaw().
+ The task jacobian and its pseudo inverse are updated after a call of
+computeControlLaw().
\return Pseudo inverse \f${J}^{+}\f$ of the task jacobian.
\code
vpServo task;
...
- vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing
- vpMatrix Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v
-\endcode
+ vpColVector v = task.computeControlLaw(); // Compute the velocity corresponding to the visual servoing vpMatrix
+ Jp = task.getTaskJacobianPseudoInverse(); // Get the pseudo inverse of task jacobian used to compute v \endcode
\sa getTaskJacobian()
*/
-vpMatrix vpServo::getTaskJacobianPseudoInverse() const
-{
- return J1p;
-}
+vpMatrix vpServo::getTaskJacobianPseudoInverse() const { return J1p; }
/*!
- Return the rank of the task jacobian. The rank is updated after a call of computeControlLaw().
+ 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
+ unsigned int rank = task.getTaskRank(); // Get the rank of the task jacobian
+ \endcode
*/
-unsigned int vpServo::getTaskRank() const
-{
- return rankJ1;
-}
+unsigned int vpServo::getTaskRank() const { return rankJ1; }
/*!
- Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is updated
- after a call of computeControlLaw().
+ Return the projection operator \f${\bf W}^+{\bf W}\f$. This operator is
+updated after a call of computeControlLaw().
- When the dimension of the task is equal to the number of degrees of freedom available
- \f${\bf W^+W = I}\f$.
+ 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()
+ vpMatrix WpW = task.getWpW(); // Get the projection operator
+ \endcode
+ \sa getI_WpW()
*/
-vpMatrix vpServo::getWpW() const
-{
- return WpW;
-}
+vpMatrix vpServo::getWpW() const { return WpW; }
diff --git a/modules/vs/src/vpServoData.cpp b/modules/vs/src/vpServoData.cpp
index 42bd0da..248ad8a 100644
--- a/modules/vs/src/vpServoData.cpp
+++ b/modules/vs/src/vpServoData.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,7 +36,6 @@
*
*****************************************************************************/
-
/*!
\file vpServoData.cpp
\brief save data during the task execution
@@ -44,79 +44,65 @@
// Servo
#include <visp3/vs/vpServo.h>
-#include <visp3/vs/vpServoData.h>
#include <visp3/core/vpIoException.h>
#include <visp3/core/vpIoTools.h>
+#include <visp3/vs/vpServoData.h>
-void
-vpServoData::open(const char *directory)
+void vpServoData::open(const char *directory)
{
- try
- {
+ try {
if (vpIoTools::checkDirectory(directory) == false)
vpIoTools::makeDirectory(directory);
- char s[FILENAME_MAX] ;
-
- sprintf(s,"%s/vel.dat",directory) ;
- velocityFile.open(s) ;
- sprintf(s,"%s/error.dat",directory) ;
- errorFile.open(s) ;
- sprintf(s,"%s/errornorm.dat",directory) ;
- errorNormFile.open(s) ;
- sprintf(s,"%s/s.dat",directory) ;
- sFile.open(s) ;
- sprintf(s,"%s/sStar.dat",directory) ;
- sStarFile.open(s) ;
-
- }
- catch(...)
- {
- vpERROR_TRACE("Error caught") ;
- throw ;
+ char s[FILENAME_MAX];
+
+ sprintf(s, "%s/vel.dat", directory);
+ velocityFile.open(s);
+ sprintf(s, "%s/error.dat", directory);
+ errorFile.open(s);
+ sprintf(s, "%s/errornorm.dat", directory);
+ errorNormFile.open(s);
+ sprintf(s, "%s/s.dat", directory);
+ sFile.open(s);
+ sprintf(s, "%s/sStar.dat", directory);
+ sStarFile.open(s);
+
+ } catch (...) {
+ vpERROR_TRACE("Error caught");
+ throw;
}
}
-void vpServoData::setCmDeg()
-{
- cmDeg = true ;
-}
-void vpServoData::setMeterRad()
-{
- cmDeg = false ;
-}
+void vpServoData::setCmDeg() { cmDeg = true; }
+void vpServoData::setMeterRad() { cmDeg = false; }
void vpServoData::save(const vpServo &task)
{
- if (cmDeg==false) velocityFile << task.q_dot.t() ;
- else
- {
- for (unsigned int i=0 ; i < 3 ; i++)
- velocityFile << task.q_dot[i]*100 <<" " ;
- for (unsigned int i=4 ; i < 6 ; i++)
- velocityFile << vpMath::deg(task.q_dot[i]) <<" " ;
- velocityFile << std::endl ;
+ if (cmDeg == false)
+ velocityFile << task.q_dot.t();
+ else {
+ for (unsigned int i = 0; i < 3; i++)
+ velocityFile << task.q_dot[i] * 100 << " ";
+ for (unsigned int i = 4; i < 6; i++)
+ velocityFile << vpMath::deg(task.q_dot[i]) << " ";
+ velocityFile << std::endl;
}
- errorFile << ( task.getError() ).t() ;
- errorNormFile << ( task.getError() ).sumSquare() << std::endl ;
- vNormFile << task.q_dot.sumSquare() << std::endl ;
+ errorFile << (task.getError()).t();
+ errorNormFile << (task.getError()).sumSquare() << std::endl;
+ vNormFile << task.q_dot.sumSquare() << std::endl;
- sFile <<task.s.t() ;
+ sFile << task.s.t();
sStarFile << task.sStar.t();
}
-
-
void vpServoData::close()
{
- velocityFile.close() ;
- errorFile.close() ;
- errorNormFile.close() ;
- sFile.close() ;
- sStarFile.close() ;
+ velocityFile.close();
+ errorFile.close();
+ errorNormFile.close();
+ sFile.close();
+ sStarFile.close();
}
-
-
/*
* Local variables:
* c-basic-offset: 2
diff --git a/modules/vs/src/vpServoDisplay.cpp b/modules/vs/src/vpServoDisplay.cpp
index df55a0b..8218a86 100644
--- a/modules/vs/src/vpServoDisplay.cpp
+++ b/modules/vs/src/vpServoDisplay.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -46,7 +47,7 @@
// Meter/pixel conversion
#include <visp3/core/vpCameraParameters.h>
-//Color / image / display
+// Color / image / display
#include <visp3/core/vpColor.h>
#include <visp3/core/vpImage.h>
@@ -76,27 +77,21 @@
\param thickness : Thickness of the feature representation.
*/
-void vpServoDisplay::display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<unsigned char> &I,
- vpColor currentColor,
- vpColor desiredColor,
- unsigned int thickness)
+void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage<unsigned char> &I,
+ vpColor currentColor, vpColor desiredColor, unsigned int thickness)
{
std::list<vpBasicFeature *>::const_iterator it_s;
std::list<vpBasicFeature *>::const_iterator it_s_star;
- for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
- it_s != s.featureList.end();
- ++it_s, ++it_s_star)
- {
+ 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 ) ;
+ (*it_s_star)->display(cam, I, desiredColor, thickness);
}
if (currentColor != vpColor::none) {
// current list
- (*it_s)->display(cam, I, currentColor, thickness ) ;
+ (*it_s)->display(cam, I, currentColor, thickness);
}
}
// vpDisplay::flush(I) ;
@@ -122,28 +117,21 @@ void vpServoDisplay::display(const vpServo &s,
\param thickness : Thickness of the feature representation.
*/
-void
-vpServoDisplay::display(const vpServo &s,
- const vpCameraParameters &cam,
- const vpImage<vpRGBa> &I,
- vpColor currentColor,
- vpColor desiredColor,
- unsigned int thickness)
+void vpServoDisplay::display(const vpServo &s, const vpCameraParameters &cam, const vpImage<vpRGBa> &I,
+ vpColor currentColor, vpColor desiredColor, unsigned int thickness)
{
std::list<vpBasicFeature *>::const_iterator it_s;
std::list<vpBasicFeature *>::const_iterator it_s_star;
- for (it_s = s.featureList.begin(), it_s_star = s.desiredFeatureList.begin();
- it_s != s.featureList.end();
- ++it_s, ++it_s_star)
- {
+ 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 ) ;
+ (*it_s_star)->display(cam, I, desiredColor, thickness);
}
if (currentColor != vpColor::none) {
// current list
- (*it_s)->display(cam, I, currentColor, thickness ) ;
+ (*it_s)->display(cam, I, currentColor, thickness);
}
}
// vpDisplay::flush(I) ;
diff --git a/modules/vs/test/visual-feature/testFeature.cpp b/modules/vs/test/visual-feature/testFeature.cpp
index cde1ba1..e9efdef 100644
--- a/modules/vs/test/visual-feature/testFeature.cpp
+++ b/modules/vs/test/visual-feature/testFeature.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -35,18 +36,17 @@
*
*****************************************************************************/
-
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpColVector.h>
-#include <visp3/core/vpTranslationVector.h>
-#include <visp3/core/vpRotationMatrix.h>
#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/core/vpRotationMatrix.h>
+#include <visp3/core/vpTranslationVector.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
#include <visp3/visual_features/vpFeatureThetaU.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/core/vpCameraParameters.h>
+#include <visp3/visual_features/vpGenericFeature.h>
+#include <visp3/vs/vpServo.h>
#include <iostream>
@@ -60,15 +60,15 @@
int main()
{
try {
- for (int i=0; i < 3; i++) {
- vpServo task ;
+ for (int i = 0; i < 3; i++) {
+ vpServo task;
// Creation od a Theta U vector that represent the rotation
// between the desired camera frame and the current one.
vpThetaUVector tu_cdRc; // Current visual feature s
- tu_cdRc[0] =0.1;
- tu_cdRc[1] =0.2;
- tu_cdRc[2] =0.3;
+ tu_cdRc[0] = 0.1;
+ tu_cdRc[1] = 0.2;
+ tu_cdRc[2] = 0.3;
// Creation of the current feature s
vpFeatureThetaU s(vpFeatureThetaU::cdRc);
@@ -80,10 +80,10 @@ int main()
vpFeatureThetaU s_star(vpFeatureThetaU::cdRc); // init to zero
// Compute the interaction matrix for the ThetaU_z feature
- vpMatrix L_z = s.interaction( vpFeatureThetaU::selectTUz() );
+ vpMatrix L_z = s.interaction(vpFeatureThetaU::selectTUz());
// Compute the error vector (s-s^*) for the ThetaU_z feature
s.error(s_star, vpFeatureThetaU::selectTUz());
-
+
// A call to kill() is requested here to destroy properly the current
// and desired feature lists.
task.kill();
@@ -91,8 +91,7 @@ int main()
std::cout << "End, call vpServo destructors..." << std::endl;
}
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/modules/vs/test/visual-feature/testFeatureMoment.cpp b/modules/vs/test/visual-feature/testFeatureMoment.cpp
index e6524eb..68e732a 100644
--- a/modules/vs/test/visual-feature/testFeatureMoment.cpp
+++ b/modules/vs/test/visual-feature/testFeatureMoment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -34,175 +35,210 @@
* Filip Novotny
*
*****************************************************************************/
-#include <visp3/core/vpMomentObject.h>
-#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpDebug.h>
+#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMomentCommon.h>
+#include <visp3/core/vpMomentDatabase.h>
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/core/vpPlane.h>
#include <visp3/visual_features/vpFeatureMomentCommon.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/core/vpDebug.h>
-#include <visp3/core/vpPlane.h>
-#include <limits>
#include <iostream>
+#include <limits>
-//initialize scene in the interface
-void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst);
+// initialize scene in the interface
+void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
+ vpMomentObject &dst);
-vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst); //launch the test
-void planeToABC(const vpPlane& pl, double& A,double& B, double& C);
-int test(double x,double y,double z,double alpha);
+vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
+ vpMomentObject &dst); // launch the test
+void planeToABC(const vpPlane &pl, double &A, double &B, double &C);
+int test(double x, double y, double z, double alpha);
-//Compute a set of parallel positions and check if the matrix is in the right form;
+// Compute a set of parallel positions and check if the matrix is in the right
+// form;
int main()
{
try {
- int sum=0;
- for(double i=-0.2;i<0.2;i+=0.1){
- for(double j=-0.2;j<0.2;j+=0.1){
- for(double k=-vpMath::rad(30);k<vpMath::rad(30);k+=vpMath::rad(10)){
- for(double l=0.5;l<1.5;l+=0.1){
- sum+=test(i,j,l,k);
+ int sum = 0;
+ for (double i = -0.2; i < 0.2; i += 0.1) {
+ for (double j = -0.2; j < 0.2; j += 0.1) {
+ for (double k = -vpMath::rad(30); k < vpMath::rad(30); k += vpMath::rad(10)) {
+ for (double l = 0.5; l < 1.5; l += 0.1) {
+ sum += test(i, j, l, k);
}
}
-
}
}
- if(sum<0) return -1;
- else return 0;
- }
- catch(vpException &e) {
+ if (sum < 0)
+ return -1;
+ else
+ return 0;
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
}
-int test(double x,double y,double z,double alpha){
- //intial pose
- vpHomogeneousMatrix cMo(x,y,z,-vpMath::rad(0),vpMath::rad(0),alpha);
- //Desired pose
- vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0,0.0,1.0,vpMath::rad(0),vpMath::rad(0),-vpMath::rad(0)));
+int test(double x, double y, double z, double alpha)
+{
+ // intial pose
+ vpHomogeneousMatrix cMo(x, y, z, -vpMath::rad(0), vpMath::rad(0), alpha);
+ // Desired pose
+ vpHomogeneousMatrix cdMo(vpHomogeneousMatrix(0.0, 0.0, 1.0, vpMath::rad(0), vpMath::rad(0), -vpMath::rad(0)));
- //source and destination objects for moment manipulation
+ // source and destination objects for moment manipulation
vpMomentObject src(6);
vpMomentObject dst(6);
- //init and run the simulation
- initScene(cMo, cdMo, src, dst); //initialize graphical scene (for interface)
+ // init and run the simulation
+ initScene(cMo, cdMo, src, dst); // initialize graphical scene (for
+ // interface)
vpMatrix mat = execute(cMo, cdMo, src, dst);
- if(fabs(mat[0][0]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[0][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[0][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[1][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[1][1]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[1][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[2][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][2]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[2][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[3][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[3][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[4][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[4][5]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
-
- if(fabs(mat[5][0]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][1]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][2]-(0)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
- if(fabs(mat[5][5]-(-1)) > std::numeric_limits<double>::epsilon()*1e10) return -1;
+ if (fabs(mat[0][0] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[0][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[0][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+
+ if (fabs(mat[1][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[1][1] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[1][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+
+ if (fabs(mat[2][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[2][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[2][2] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[2][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+
+ if (fabs(mat[3][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[3][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[3][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[3][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+
+ if (fabs(mat[4][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[4][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[4][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[4][5] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+
+ if (fabs(mat[5][0] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[5][1] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[5][2] - (0)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
+ if (fabs(mat[5][5] - (-1)) > std::numeric_limits<double>::epsilon() * 1e10)
+ return -1;
return 0;
}
-void initScene(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst)
+void initScene(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
+ vpMomentObject &dst)
{
std::vector<vpPoint> src_pts;
std::vector<vpPoint> dst_pts;
- double x[5] = { 0.2, 0.2,-0.2,-0.2, 0.2 };
- double y[5] = {-0.1, 0.1, 0.1,-0.1,-0.1 };
+ double x[5] = {0.2, 0.2, -0.2, -0.2, 0.2};
+ double y[5] = {-0.1, 0.1, 0.1, -0.1, -0.1};
int nbpoints = 4;
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cMo);
src_pts.push_back(p);
}
src.setType(vpMomentObject::DENSE_POLYGON);
src.fromVector(src_pts);
- for (int i = 0 ; i < nbpoints ; i++){
- vpPoint p(x[i],y[i],0.0);
- p.track(cdMo) ;
+ for (int i = 0; i < nbpoints; i++) {
+ vpPoint p(x[i], y[i], 0.0);
+ p.track(cdMo);
dst_pts.push_back(p);
}
dst.setType(vpMomentObject::DENSE_POLYGON);
dst.fromVector(dst_pts);
}
-
-vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo,
- vpMomentObject &src, vpMomentObject &dst)
+vpMatrix execute(const vpHomogeneousMatrix &cMo, const vpHomogeneousMatrix &cdMo, vpMomentObject &src,
+ vpMomentObject &dst)
{
- vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT; ; //current or desired
+ vpServo::vpServoIteractionMatrixType interaction_type = vpServo::CURRENT;
+ ; // current or desired
vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
- //A,B,C parameters of source and destination plane
- double A; double B; double C;
- double Ad; double Bd; double Cd;
- //init main object: using moments up to order 6
-
- //Initializing values from regular plane (with ax+by+cz=d convention)
+ // A,B,C parameters of source and destination plane
+ double A;
+ double B;
+ double C;
+ double Ad;
+ double Bd;
+ double Cd;
+ // init main object: using moments up to order 6
+
+ // Initializing values from regular plane (with ax+by+cz=d convention)
vpPlane pl;
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cMo);
- planeToABC(pl,A,B,C);
+ planeToABC(pl, A, B, C);
- pl.setABCD(0,0,1.0,0);
+ pl.setABCD(0, 0, 1.0, 0);
pl.changeFrame(cdMo);
- planeToABC(pl,Ad,Bd,Cd);
+ planeToABC(pl, Ad, Bd, Cd);
- //extracting initial position (actually we only care about Zdst)
+ // extracting initial position (actually we only care about Zdst)
vpTranslationVector vec;
cdMo.extract(vec);
- ///////////////////////////// initializing moments and features /////////////////////////////////
- //don't need to be specific, vpMomentCommon automatically loads Xg,Yg,An,Ci,Cj,Alpha moments
- vpMomentCommon moments (vpMomentCommon ::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst), vec[2]);
- vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst),vpMomentCommon::getMu3(dst),vpMomentCommon::getAlpha(dst),vec[2]);
- //same thing with common features
+ ///////////////////////////// initializing moments and features
+ ////////////////////////////////////
+ // don't need to be specific, vpMomentCommon automatically loads
+ // Xg,Yg,An,Ci,Cj,Alpha moments
+ vpMomentCommon moments(vpMomentCommon ::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst),
+ vec[2]);
+ vpMomentCommon momentsDes(vpMomentCommon::getSurface(dst), vpMomentCommon::getMu3(dst), vpMomentCommon::getAlpha(dst),
+ vec[2]);
+ // same thing with common features
vpFeatureMomentCommon featureMoments(moments);
vpFeatureMomentCommon featureMomentsDes(momentsDes);
moments.updateAll(src);
momentsDes.updateAll(dst);
- featureMoments.updateAll(A,B,C);
- featureMomentsDes.updateAll(Ad,Bd,Cd);
+ featureMoments.updateAll(A, B, C);
+ featureMomentsDes.updateAll(Ad, Bd, Cd);
- //setup the interaction type
- task.setInteractionMatrixType(interaction_type) ;
- //////////////////////////////////add useful features to task//////////////////////////////
- task.addFeature(featureMoments.getFeatureGravityNormalized(),featureMomentsDes.getFeatureGravityNormalized());
- task.addFeature(featureMoments.getFeatureAn(),featureMomentsDes.getFeatureAn());
- //the moments are different in case of a symmetric object
- task.addFeature(featureMoments.getFeatureCInvariant(),featureMomentsDes.getFeatureCInvariant(),(1 << 10) | (1 << 11));
- task.addFeature(featureMoments.getFeatureAlpha(),featureMomentsDes.getFeatureAlpha());
+ // setup the interaction type
+ task.setInteractionMatrixType(interaction_type);
+ //////////////////////////////////add useful features to
+ /// task//////////////////////////////
+ task.addFeature(featureMoments.getFeatureGravityNormalized(), featureMomentsDes.getFeatureGravityNormalized());
+ task.addFeature(featureMoments.getFeatureAn(), featureMomentsDes.getFeatureAn());
+ // the moments are different in case of a symmetric object
+ task.addFeature(featureMoments.getFeatureCInvariant(), featureMomentsDes.getFeatureCInvariant(),
+ (1 << 10) | (1 << 11));
+ task.addFeature(featureMoments.getFeatureAlpha(), featureMomentsDes.getFeatureAlpha());
- task.setLambda(0.4) ;
+ task.setLambda(0.4);
task.computeControlLaw();
vpMatrix mat = task.computeInteractionMatrix();
@@ -210,11 +246,9 @@ vpMatrix execute(const vpHomogeneousMatrix& cMo, const vpHomogeneousMatrix& cdMo
return mat;
}
-
-void planeToABC(const vpPlane& pl, double& A,double& B, double& C)
+void planeToABC(const vpPlane &pl, double &A, double &B, double &C)
{
- A=-pl.getA()/pl.getD();
- B=-pl.getB()/pl.getD();
- C=-pl.getC()/pl.getD();
+ A = -pl.getA() / pl.getD();
+ B = -pl.getB() / pl.getD();
+ C = -pl.getC() / pl.getD();
}
-
diff --git a/modules/vs/test/visual-feature/testFeatureSegment.cpp b/modules/vs/test/visual-feature/testFeatureSegment.cpp
index 8dbda69..2d5315e 100644
--- a/modules/vs/test/visual-feature/testFeatureSegment.cpp
+++ b/modules/vs/test/visual-feature/testFeatureSegment.cpp
@@ -3,9 +3,10 @@
* This file is part of the ViSP software.
* Copyright (C) 2005 - 2017 by Inria. All rights reserved.
*
- * This software is free software; you can redistribute it and/or
- * modify it under the terms of the GNU General Public License
- * ("GPL") version 2 as published by the Free Software Foundation.
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
@@ -37,8 +38,8 @@
#include <fstream>
#include <iostream>
-#include <vector>
#include <numeric>
+#include <vector>
#include <visp3/core/vpConfig.h>
@@ -46,17 +47,17 @@
#include <visp3/core/vpCameraParameters.h>
#include <visp3/core/vpDisplay.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpImage.h>
#include <visp3/core/vpMath.h>
-#include <visp3/io/vpParseArgv.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/core/vpPoint.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/io/vpParseArgv.h>
#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/visual_features/vpFeatureSegment.h>
#include <visp3/vs/vpServo.h> //visual servoing task
/*!
@@ -67,45 +68,41 @@
*/
int main(int argc, const char **argv)
-{
+{
try {
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
int opt_no_display = 0;
int opt_curves = 1;
#endif
int opt_normalized = 1;
// Parse the command line to set the variables
- vpParseArgv::vpArgvInfo argTable[] =
- {
- #if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- {"-d", vpParseArgv::ARGV_CONSTANT, 0, (char *) &opt_no_display,
- "Disable display and graphics viewer."},
- #endif
- {"-normalized", vpParseArgv::ARGV_INT, (char*) NULL, (char *) &opt_normalized,
+ 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."},
+#endif
+ {"-normalized", vpParseArgv::ARGV_INT, (char *)NULL, (char *)&opt_normalized,
"1 to use normalized features, 0 for non normalized."},
- {"-h", vpParseArgv::ARGV_HELP, (char*) NULL, (char *) NULL,
- "Print the help."},
- {(char*) NULL, vpParseArgv::ARGV_END, (char*) NULL, (char*) NULL, (char*) NULL}
- } ;
+ {"-h", vpParseArgv::ARGV_HELP, (char *)NULL, (char *)NULL, "Print the help."},
+ {(char *)NULL, vpParseArgv::ARGV_END, (char *)NULL, (char *)NULL, (char *)NULL}
+ };
// Read the command line options
- if(vpParseArgv::parse(&argc, argv, argTable,
- vpParseArgv::ARGV_NO_LEFTOVERS |
- vpParseArgv::ARGV_NO_ABBREV |
- vpParseArgv::ARGV_NO_DEFAULTS)) {
+ if (vpParseArgv::parse(&argc, argv, argTable,
+ vpParseArgv::ARGV_NO_LEFTOVERS | vpParseArgv::ARGV_NO_ABBREV |
+ vpParseArgv::ARGV_NO_DEFAULTS)) {
return (false);
}
std::cout << "Used options: " << std::endl;
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
opt_curves = (opt_no_display == 0) ? 1 : 0;
std::cout << " - no display: " << opt_no_display << std::endl;
std::cout << " - curves : " << opt_curves << std::endl;
#endif
std::cout << " - normalized: " << opt_normalized << std::endl;
- vpCameraParameters cam(640.,480.,320.,240.);
+ vpCameraParameters cam(640., 480., 320., 240.);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
vpDisplay *display = NULL;
@@ -117,65 +114,63 @@ int main(int argc, const char **argv)
#endif
}
#endif
- vpImage<unsigned char> I(480,640,0);
+ vpImage<unsigned char> I(480, 640, 0);
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display)
display->init(I);
#endif
vpHomogeneousMatrix wMo; // Set to indentity. Robot world frame is equal to object frame
- vpHomogeneousMatrix cMo (-0.5, 0.5, 2., vpMath::rad(10), vpMath::rad(20), vpMath::rad(30));
+ 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
- P[0].setWorldCoordinates( .1, .1, 0.);
- P[1].setWorldCoordinates(-.1, .1, 0.);
+ P[0].setWorldCoordinates(.1, .1, 0.);
+ P[1].setWorldCoordinates(-.1, .1, 0.);
P[2].setWorldCoordinates(-.1, -.1, 0.);
- P[3].setWorldCoordinates( .1, -.1, 0.);
+ P[3].setWorldCoordinates(.1, -.1, 0.);
vpPoint Pd[4]; // 4 points in the desired camera frame
- for (int i=0; i<4; i++) {
+ for (int i = 0; i < 4; i++) {
Pd[i] = P[i];
Pd[i].project(cdMo);
}
vpPoint Pc[4]; // 4 points in the current camera frame
- for (int i=0; i<4; i++) {
+ for (int i = 0; i < 4; i++) {
Pc[i] = P[i];
Pc[i].project(cMo);
}
vpFeatureSegment seg_cur[2], seg_des[2]; // Current and desired features
- for (int i=0; i <2; i++)
- {
+ for (int i = 0; i < 2; i++) {
if (opt_normalized) {
seg_cur[i].setNormalized(true);
seg_des[i].setNormalized(true);
- }
- else {
+ } else {
seg_cur[i].setNormalized(false);
seg_des[i].setNormalized(false);
}
- vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
- vpFeatureBuilder::create(seg_des[i], Pd[i*2], Pd[i*2+1]);
+ vpFeatureBuilder::create(seg_cur[i], Pc[i * 2], Pc[i * 2 + 1]);
+ vpFeatureBuilder::create(seg_des[i], Pd[i * 2], Pd[i * 2 + 1]);
seg_cur[i].print();
seg_des[i].print();
}
- //define visual servoing task
+ // define visual servoing task
vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
- task.setLambda(2.) ;
+ task.setLambda(2.);
- for (int i=0; i <2; i++)
+ for (int i = 0; i < 2; i++)
task.addFeature(seg_cur[i], seg_des[i]);
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display) {
vpDisplay::display(I);
- for (int i=0; i <2; i++) {
+ for (int i = 0; i < 2; i++) {
seg_cur[i].display(cam, I, vpColor::red);
seg_des[i].display(cam, I, vpColor::green);
vpDisplay::flush(I);
@@ -183,22 +178,22 @@ int main(int argc, const char **argv)
}
#endif
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
vpPlot *graph = NULL;
- if (opt_curves)
- {
- //Create a window (700 by 700) at position (100, 200) with two graphics
+ if (opt_curves) {
+ // Create a window (700 by 700) at position (100, 200) with two graphics
graph = new vpPlot(2, 500, 500, 700, 10, "Curves...");
- //The first graphic contains 3 curve and the second graphic contains 3 curves
- graph->initGraph(0,6);
- graph->initGraph(1,8);
+ // The first graphic contains 3 curve and the second graphic contains 3
+ // curves
+ graph->initGraph(0, 6);
+ graph->initGraph(1, 8);
// graph->setTitle(0, "Velocities");
// graph->setTitle(1, "Error s-s*");
}
#endif
- //param robot
+ // param robot
vpSimulatorCamera robot;
float sampling_time = 0.02f; // Sampling period in seconds
robot.setSamplingTime(sampling_time);
@@ -206,22 +201,22 @@ int main(int argc, const char **argv)
robot.setMaxRotationVelocity(vpMath::rad(90.));
wMc = wMo * cMo.inverse();
robot.setPosition(wMc);
- int iter=0;
+ int iter = 0;
do {
double t = vpTime::measureTimeMs();
wMc = robot.getPosition();
cMo = wMc.inverse() * wMo;
- for (int i=0; i <4; i++)
+ for (int i = 0; i < 4; i++)
Pc[i].project(cMo);
- for (int i=0; i <2; i++)
- vpFeatureBuilder::create(seg_cur[i], Pc[i*2], Pc[i*2+1]);
+ for (int i = 0; i < 2; i++)
+ vpFeatureBuilder::create(seg_cur[i], Pc[i * 2], Pc[i * 2 + 1]);
-#if (defined (VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display) {
vpDisplay::display(I);
- for (int i=0; i <2; i++) {
+ for (int i = 0; i < 2; i++) {
seg_cur[i].display(cam, I, vpColor::red);
seg_des[i].display(cam, I, vpColor::green);
vpDisplay::flush(I);
@@ -230,38 +225,36 @@ int main(int argc, const char **argv)
#endif
vpColVector v = task.computeControlLaw();
- robot.setVelocity(vpRobot::CAMERA_FRAME, v) ;
+ robot.setVelocity(vpRobot::CAMERA_FRAME, v);
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
- if (opt_curves)
- {
- graph->plot(0, iter, v); // plot velocities applied to the robot
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
+ if (opt_curves) {
+ graph->plot(0, iter, v); // plot velocities applied to the robot
graph->plot(1, iter, task.getError()); // plot error vector
}
#endif
vpTime::wait(t, sampling_time * 1000); // Wait 10 ms
- iter ++;
+ iter++;
- } while(( task.getError() ).sumSquare() > 0.0005);
+ } while ((task.getError()).sumSquare() > 0.0005);
// A call to kill() is requested here to destroy properly the current
// and desired feature lists.
task.kill();
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (graph != NULL)
delete graph;
#endif
-#if (defined (VISP_HAVE_X11) || defined (VISP_HAVE_GDI))
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
if (!opt_no_display && display != NULL)
delete display;
#endif
- std::cout << "final error=" << ( task.getError() ).sumSquare() << std::endl;
+ std::cout << "final error=" << (task.getError()).sumSquare() << std::endl;
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/platforms/ios/build_framework.py b/platforms/ios/build_framework.py
index 01723aa..c65af4c 100644
--- a/platforms/ios/build_framework.py
+++ b/platforms/ios/build_framework.py
@@ -76,8 +76,10 @@ class Builder:
if self.contrib:
cmake_flags.append("-DVISP_CONTRIB_MODULES_PATH=%s" % self.contrib)
if xcode_ver >= 7 and t[1] == 'iPhoneOS':
- cmake_flags.append("-DCMAKE_C_FLAGS=-fembed-bitcode")
+ cmake_flags.append("-DCMAKE_C_FLAGS=-fembed-bitcode -Wno-implicit-function-declaration -Wno-logical-op-parentheses -Wno-unused-variable -Wno-parentheses -Wno-sometimes-uninitialized -Wno-unused-parameter -Wno-shorten-64-to-32")
cmake_flags.append("-DCMAKE_CXX_FLAGS=-fembed-bitcode")
+ else:
+ cmake_flags.append("-DCMAKE_C_FLAGS=-Wno-implicit-function-declaration -Wno-logical-op-parentheses -Wno-unused-variable -Wno-parentheses -Wno-sometimes-uninitialized -Wno-unused-parameter -Wno-shorten-64-to-32")
self.buildOne(t[0], t[1], mainBD, cmake_flags)
self.mergeLibs(mainBD)
self.makeFramework(outdir, dirs)
@@ -100,7 +102,6 @@ class Builder:
args = [
"cmake",
"-GXcode",
- "-DCMAKE_C_FLAGS=\"-Wno-implicit-function-declaration\" ",
"-DAPPLE_FRAMEWORK=ON",
"-DCMAKE_INSTALL_PREFIX=install",
"-DCMAKE_BUILD_TYPE=Release",
@@ -108,10 +109,7 @@ class Builder:
"-DBUILD_EXAMPLES=OFF",
"-DBUILD_TESTS=OFF",
"-DBUILD_TUTORIALS=OFF",
- "-DUSE_XML2=OFF",
- "-DUSE_PTHREAD=OFF",
- "-DUSE_ZLIB=OFF",
- ]
+ ]
return args
def getBuildCommand(self, arch, target):
diff --git a/platforms/ios/cmake/Modules/Platform/iOS.cmake b/platforms/ios/cmake/Modules/Platform/iOS.cmake
index 67a4093..78ae779 100644
--- a/platforms/ios/cmake/Modules/Platform/iOS.cmake
+++ b/platforms/ios/cmake/Modules/Platform/iOS.cmake
@@ -56,7 +56,7 @@ set (CMAKE_SHARED_LIBRARY_CREATE_C_FLAGS "-dynamiclib -headerpad_max_install_nam
set (CMAKE_SHARED_MODULE_CREATE_C_FLAGS "-bundle -headerpad_max_install_names")
set (CMAKE_SHARED_MODULE_LOADER_C_FLAG "-Wl,-bundle_loader,")
set (CMAKE_SHARED_MODULE_LOADER_CXX_FLAG "-Wl,-bundle_loader,")
-set (CMAKE_FIND_LIBRARY_SUFFIXES ".dylib" ".so" ".a")
+set (CMAKE_FIND_LIBRARY_SUFFIXES ".dylib" ".so" ".a" ".tbd")
# hack: if a new cmake (which uses CMAKE_INSTALL_NAME_TOOL) runs on an old build tree
# (where install_name_tool was hardcoded) and where CMAKE_INSTALL_NAME_TOOL isn't in the cache
@@ -67,61 +67,55 @@ if (NOT DEFINED CMAKE_INSTALL_NAME_TOOL)
endif (NOT DEFINED CMAKE_INSTALL_NAME_TOOL)
# Setup iOS developer location
-if (IPHONEOS)
+if(IPHONEOS)
+ execute_process(COMMAND xcrun --sdk iphoneos --show-sdk-path
+ OUTPUT_VARIABLE _CMAKE_IOS_SDK_ROOT OUTPUT_STRIP_TRAILING_WHITESPACE)
execute_process(COMMAND xcrun --sdk iphoneos --show-sdk-platform-path
- OUTPUT_VARIABLE _IPHONEOS_SDK_PATH OUTPUT_STRIP_TRAILING_WHITESPACE)
- set (_CMAKE_IOS_DEVELOPER_ROOT ${_IPHONEOS_SDK_PATH}/Developer)
-else ()
- if (IPHONESIMULATOR)
- execute_process(COMMAND xcrun --sdk iphonesimulator --show-sdk-platform-path
- OUTPUT_VARIABLE _IPHONESIMULATOR_SDK_PATH OUTPUT_STRIP_TRAILING_WHITESPACE)
- set (_CMAKE_IOS_DEVELOPER_ROOT "${_IPHONESIMULATOR_SDK_PATH}/Developer")
- endif ()
-endif ()
-# Find installed iOS SDKs
-file (GLOB _CMAKE_IOS_SDKS "${_CMAKE_IOS_DEVELOPER_ROOT}/SDKs/*")
-
-# Find and use the most recent iOS sdk
-if (_CMAKE_IOS_SDKS)
- list (SORT _CMAKE_IOS_SDKS)
- list (REVERSE _CMAKE_IOS_SDKS)
- list (GET _CMAKE_IOS_SDKS 0 _CMAKE_IOS_SDK_ROOT)
-
- # Set the sysroot default to the most recent SDK
- set (CMAKE_OSX_SYSROOT ${_CMAKE_IOS_SDK_ROOT} CACHE PATH "Sysroot used for iOS support")
-
- # set the architecture for iOS - this env var sets armv6,armv7 and appears to be XCode's standard. The other found is ARCHS_UNIVERSAL_IPHONE_OS but that is armv7 only
- set (CMAKE_OSX_ARCHITECTURES "$(ARCHS_STANDARD_32_BIT)" CACHE string "Build architecture for iOS")
-
- # Set the default based on this file and not the environment variable
- set (CMAKE_FIND_ROOT_PATH ${_CMAKE_IOS_DEVELOPER_ROOT} ${_CMAKE_IOS_SDK_ROOT} CACHE string "iOS library search path root")
-
- # default to searching for frameworks first
- set (CMAKE_FIND_FRAMEWORK FIRST)
-
- # set up the default search directories for frameworks
- set (CMAKE_SYSTEM_FRAMEWORK_PATH
- ${_CMAKE_IOS_SDK_ROOT}/System/Library/Frameworks
- ${_CMAKE_IOS_SDK_ROOT}/System/Library/PrivateFrameworks
- ${_CMAKE_IOS_DEVELOPER_ROOT}/Library/Frameworks
- )
-endif (_CMAKE_IOS_SDKS)
+ OUTPUT_VARIABLE _IPHONEOS_SDK_PLATFORM_PATH OUTPUT_STRIP_TRAILING_WHITESPACE)
+ set (_CMAKE_IOS_DEVELOPER_ROOT "${_IPHONEOS_SDK_PLATFORM_PATH}/Developer")
+elseif(IPHONESIMULATOR)
+ execute_process(COMMAND xcrun --sdk iphonesimulator --show-sdk-path
+ OUTPUT_VARIABLE _CMAKE_IOS_SDK_ROOT OUTPUT_STRIP_TRAILING_WHITESPACE)
+ execute_process(COMMAND xcrun --sdk iphonesimulator --show-sdk-platform-path
+ OUTPUT_VARIABLE _IPHONESIMULATOR_SDK_PLATFORM_PATH OUTPUT_STRIP_TRAILING_WHITESPACE)
+ set (_CMAKE_IOS_DEVELOPER_ROOT "${_IPHONESIMULATOR_SDK_PLATFORM_PATH}/Developer")
+endif()
+
+# Set the sysroot default to the most recent SDK
+set (CMAKE_OSX_SYSROOT ${_CMAKE_IOS_SDK_ROOT} CACHE PATH "Sysroot used for iOS support")
+
+# set the architecture for iOS - this env var sets armv6,armv7 and appears to be XCode's standard.
+# The other found is ARCHS_UNIVERSAL_IPHONE_OS but that is armv7 only
+set (CMAKE_OSX_ARCHITECTURES "$(ARCHS_STANDARD_32_BIT)" CACHE string "Build architecture for iOS")
+
+# Set the default based on this file and not the environment variable
+set (CMAKE_FIND_ROOT_PATH ${_CMAKE_IOS_DEVELOPER_ROOT} ${_CMAKE_IOS_SDK_ROOT} CACHE string "iOS library search path root")
+
+# default to searching for frameworks first
+set (CMAKE_FIND_FRAMEWORK FIRST)
+
+# set up the default search directories for frameworks
+set (CMAKE_SYSTEM_FRAMEWORK_PATH
+ ${_CMAKE_IOS_SDK_ROOT}/System/Library/Frameworks
+ ${_CMAKE_IOS_SDK_ROOT}/System/Library/PrivateFrameworks
+ ${_CMAKE_IOS_DEVELOPER_ROOT}/Library/Frameworks
+)
if ("${CMAKE_BACKWARDS_COMPATIBILITY}" MATCHES "^1\\.[0-6]$")
- set (CMAKE_SHARED_MODULE_CREATE_C_FLAGS "${CMAKE_SHARED_MODULE_CREATE_C_FLAGS} -flat_namespace -undefined suppress")
+ set (CMAKE_SHARED_MODULE_CREATE_C_FLAGS "${CMAKE_SHARED_MODULE_CREATE_C_FLAGS} -flat_namespace -undefined suppress")
endif ("${CMAKE_BACKWARDS_COMPATIBILITY}" MATCHES "^1\\.[0-6]$")
if (NOT XCODE)
- # Enable shared library versioning. This flag is not actually referenced
- # but the fact that the setting exists will cause the generators to support
- # soname computation.
- set (CMAKE_SHARED_LIBRARY_SONAME_C_FLAG "-install_name")
+ # Enable shared library versioning. This flag is not actually referenced
+ # but the fact that the setting exists will cause the generators to support
+ # soname computation.
+ set (CMAKE_SHARED_LIBRARY_SONAME_C_FLAG "-install_name")
endif (NOT XCODE)
# Xcode does not support -isystem yet.
if (XCODE)
- set (CMAKE_INCLUDE_SYSTEM_FLAG_C)
- set (CMAKE_INCLUDE_SYSTEM_FLAG_CXX)
+ set (CMAKE_INCLUDE_SYSTEM_FLAG_C)
+ set (CMAKE_INCLUDE_SYSTEM_FLAG_CXX)
endif (XCODE)
# Need to list dependent shared libraries on link line. When building
diff --git a/platforms/ios/cmake/Toolchains/Toolchain-iPhoneOS_Xcode.cmake b/platforms/ios/cmake/Toolchains/Toolchain-iPhoneOS_Xcode.cmake
index 14555cd..5c8cf29 100644
--- a/platforms/ios/cmake/Toolchains/Toolchain-iPhoneOS_Xcode.cmake
+++ b/platforms/ios/cmake/Toolchains/Toolchain-iPhoneOS_Xcode.cmake
@@ -1,31 +1,32 @@
message (STATUS "Setting up iPhoneOS toolchain")
-set (IPHONEOS TRUE)
+set(IPHONEOS TRUE)
# Standard settings
-set (CMAKE_SYSTEM_NAME iOS)
+set(CMAKE_SYSTEM_NAME iOS)
# Include extra modules for the iOS platform files
-set (CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/platforms/ios/cmake/Modules")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/platforms/ios/cmake/Modules")
# Force the compilers to clang for iOS
include (CMakeForceCompiler)
#CMAKE_FORCE_C_COMPILER (clang GNU)
#CMAKE_FORCE_CXX_COMPILER (clang++ GNU)
-set (CMAKE_C_SIZEOF_DATA_PTR 4)
-set (CMAKE_C_HAS_ISYSROOT 1)
-set (CMAKE_C_COMPILER_ABI ELF)
-set (CMAKE_CXX_SIZEOF_DATA_PTR 4)
-set (CMAKE_CXX_HAS_ISYSROOT 1)
-set (CMAKE_CXX_COMPILER_ABI ELF)
+set(CMAKE_C_SIZEOF_DATA_PTR 4)
+set(CMAKE_C_HAS_ISYSROOT 1)
+set(CMAKE_C_COMPILER_ABI ELF)
+set(CMAKE_CXX_SIZEOF_DATA_PTR 4)
+set(CMAKE_CXX_HAS_ISYSROOT 1)
+set(CMAKE_CXX_COMPILER_ABI ELF)
# Skip the platform compiler checks for cross compiling
-set (CMAKE_CXX_COMPILER_WORKS TRUE)
-set (CMAKE_C_COMPILER_WORKS TRUE)
+set(CMAKE_CXX_COMPILER_WORKS TRUE)
+set(CMAKE_C_COMPILER_WORKS TRUE)
# Search for programs in the build host directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM ONLY)
# for libraries and headers in the target directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
-SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
message (STATUS "iPhoneOS toolchain loaded")
diff --git a/platforms/ios/cmake/Toolchains/Toolchain-iPhoneSimulator_Xcode.cmake b/platforms/ios/cmake/Toolchains/Toolchain-iPhoneSimulator_Xcode.cmake
index 937079d..fc826d9 100644
--- a/platforms/ios/cmake/Toolchains/Toolchain-iPhoneSimulator_Xcode.cmake
+++ b/platforms/ios/cmake/Toolchains/Toolchain-iPhoneSimulator_Xcode.cmake
@@ -1,31 +1,32 @@
message (STATUS "Setting up iPhoneSimulator toolchain")
-set (IPHONESIMULATOR TRUE)
+set(IPHONESIMULATOR TRUE)
# Standard settings
-set (CMAKE_SYSTEM_NAME iOS)
+set(CMAKE_SYSTEM_NAME iOS)
# Include extra modules for the iOS platform files
-set (CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/platforms/ios/cmake/Modules")
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/platforms/ios/cmake/Modules")
# Force the compilers to clang for iOS
include (CMakeForceCompiler)
#CMAKE_FORCE_C_COMPILER (clang GNU)
#CMAKE_FORCE_CXX_COMPILER (clang++ GNU)
-set (CMAKE_C_SIZEOF_DATA_PTR 4)
-set (CMAKE_C_HAS_ISYSROOT 1)
-set (CMAKE_C_COMPILER_ABI ELF)
-set (CMAKE_CXX_SIZEOF_DATA_PTR 4)
-set (CMAKE_CXX_HAS_ISYSROOT 1)
-set (CMAKE_CXX_COMPILER_ABI ELF)
+set(CMAKE_C_SIZEOF_DATA_PTR 4)
+set(CMAKE_C_HAS_ISYSROOT 1)
+set(CMAKE_C_COMPILER_ABI ELF)
+set(CMAKE_CXX_SIZEOF_DATA_PTR 4)
+set(CMAKE_CXX_HAS_ISYSROOT 1)
+set(CMAKE_CXX_COMPILER_ABI ELF)
# Skip the platform compiler checks for cross compiling
-set (CMAKE_CXX_COMPILER_WORKS TRUE)
-set (CMAKE_C_COMPILER_WORKS TRUE)
+set(CMAKE_CXX_COMPILER_WORKS TRUE)
+set(CMAKE_C_COMPILER_WORKS TRUE)
# Search for programs in the build host directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM ONLY)
# for libraries and headers in the target directories
-SET(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
-SET(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
message (STATUS "iPhoneSimulator toolchain loaded")
diff --git a/platforms/naoqi/cmake/extra.cmake b/platforms/naoqi/cmake/extra.cmake
index b76c430..4218e22 100644
--- a/platforms/naoqi/cmake/extra.cmake
+++ b/platforms/naoqi/cmake/extra.cmake
@@ -1,11 +1,5 @@
get_filename_component(TOOLCHAIN_ROOT_DIR ${CMAKE_TOOLCHAIN_FILE} PATH)
-# BZip2 detection for ffmpeg
-list(APPEND CMAKE_INCLUDE_PATH "${TOOLCHAIN_ROOT_DIR}/bzip2/include")
-list(APPEND CMAKE_LIBRARY_PATH "${TOOLCHAIN_ROOT_DIR}/bzip2")
-
-# ffmpeg
-set(ENV{FFMPEG_DIR} "${TOOLCHAIN_ROOT_DIR}/ffmpeg")
# jpeg
set(ENV{LIBJPEG_DIR} "${TOOLCHAIN_ROOT_DIR}/jpeg")
# png
diff --git a/script/create_module.py b/script/create_module.py
new file mode 100644
index 0000000..a132a85
--- /dev/null
+++ b/script/create_module.py
@@ -0,0 +1,220 @@
+from __future__ import print_function
+
+#import shutil
+import getopt
+import sys
+import errno
+import os
+
+root_dir="."
+parent_name="visp_contrib"
+module_name="contrib"
+class_name="vpContrib"
+visp_modules="visp_io visp_gui visp_blob visp_vs visp_mbt visp_tt_mi visp_detection visp_robot visp_sensor"
+
+platform_type = sys.platform
+def convertPathToWin32(path_):
+ return path_.replace("/", "\\")
+
+def usage():
+ print(" ")
+ print("Create a ViSP module source tree for contrib:")
+ print(" ")
+ print("<root directory>")
+ print("|-- <parent name>")
+ print(" |-- modules")
+ print(" |-- <module name>")
+ print(" |-- CMakeLists.txt")
+ print(" |-- include")
+ print(" | |-- visp3")
+ print(" | |-- <module name>")
+ print(" | |-- <class name>.h")
+ print(" |-- src")
+ print(" |-- <class name>.cpp")
+ print(" ")
+ print("Usage:")
+ print(" python", sys.argv[0], "[--root-dir=<root directory>] [--parent-name=<parent name>] [--module-name=<module name>] [--class-name=<class name>] [--visp-modules=<visp modules deps>] [--help]")
+ print(" ")
+ print("Options:")
+ print(" --root-dir=<root directory>, -r <root directory>")
+ print(" Default: \"", root_dir, "\"", sep='')
+ print(" Root location of the module tree.")
+ print(" ")
+ print(" --parent-name=<parent name>, -p <parent name>")
+ print(" Default: \"", parent_name, "\"", sep='')
+ print(" Optional parent folder that allows to give a name to a collection of contrib modules.")
+ print(" ")
+ print(" --module-name=<module name>, -m <module name>")
+ print(" Default: \"", module_name, "\"", sep='')
+ print(" Name of the module to create as a new ViSP module.")
+ print(" ")
+ print(" --class-name=<module name>, -c <module name>")
+ print(" Default: \"", class_name, "\"", sep='')
+ print(" From this class name corresponding header and source files are created.")
+ print(" ")
+ print(" --visp-modules=<visp modules deps>, -v <visp modules deps>")
+ print(" Default: \"", visp_modules, "\"", sep='')
+ print(" ViSP modules dependencies: visp_core, ...")
+ print(" ")
+ print(" --help, -h")
+ print(" Print this help.")
+ return
+
+def mkdir_p(path_):
+ try:
+ os.makedirs(path_)
+ except OSError as exc: # Python >2.5
+ if exc.errno == errno.EEXIST and os.path.isdir(path_):
+ pass
+ else:
+ raise
+
+def create_tree(root_dir_, parent_name_, module_name_):
+ path_ = root_dir_ + "/" + parent_name_ + "/modules/" + module_name_
+ path_src_ = path_ + "/src"
+ path_inc_ = path_ + "/include/visp3/" + module_name_
+ path_test_ = path_ + "/test"
+ print("Create folder:", path_src_)
+ print("Create folder:", path_inc_)
+ print("Create folder:", path_test_)
+ mkdir_p(path_src_)
+ mkdir_p(path_inc_)
+ mkdir_p(path_test_)
+ return
+
+def create_cmakelists(root_dir_, parent_name_, module_name_, visp_modules_):
+ path_ = root_dir_ + "/" + parent_name_ + "/modules/" + module_name_
+ filename_ = path_ + "/CMakeLists.txt"
+ print("Create file :", filename_)
+ file_ = open(filename_, "w")
+ file_.write("vp_add_module(" + module_name_ + " " + visp_modules_ + ")\n")
+ file_.write("vp_glob_module_sources()\n")
+ file_.write("vp_module_include_directories()\n")
+ file_.write("vp_create_module()\n\n")
+ file_.write("vp_add_tests()\n")
+ file_.close()
+ return
+
+def create_header(root_dir_, parent_name_, module_name_, class_name_):
+ path_ = root_dir_ + "/" + parent_name_ + "/modules/" + module_name_ + "/include/visp3/" + module_name_
+ filename_ = path_ + "/" + class_name_ + ".h"
+ print("Create file :", filename_)
+ file_ = open(filename_, "w")
+ file_.write("#ifndef __" + class_name_ + "_h__\n")
+ file_.write("#define __" + class_name_ + "_h__\n\n")
+ file_.write("#include <visp3/core/vpConfig.h>\n\n")
+ file_.write("class VISP_EXPORT " + class_name_ + "\n")
+ file_.write("{\n")
+ file_.write("public:\n")
+ file_.write(" " + class_name_ + "();\n")
+ file_.write(" virtual ~" + class_name_ + "(){};\n")
+ file_.write("};\n\n")
+ file_.write("#endif\n")
+ file_.close()
+ return
+
+def create_source(root_dir_, parent_name_, module_name_, class_name_):
+ path_ = root_dir_ + "/" + parent_name_ + "/modules/" + module_name_ + "/src/"
+ filename_ = path_ + class_name_ + ".cpp"
+ print("Create file :", filename_)
+ file_ = open(filename_, "w")
+ file_.write("#include <iostream>\n\n")
+ file_.write("#include <visp3/" + module_name_+ "/" + class_name_ + ".h>\n\n")
+ file_.write(class_name_ + "::" + class_name + "()\n")
+ file_.write("{\n")
+ file_.write(" std::cout << \"I'm in my first contrib module\" << std::endl;\n")
+ file_.write("}\n")
+ file_.close()
+ return
+
+def create_test(root_dir_, parent_name_, module_name_, class_name_):
+ path_ = root_dir_ + "/" + parent_name_ + "/modules/" + module_name_ + "/test/"
+ filename_ = path_ + "test-" + class_name_ + ".cpp"
+ print("Create file :", filename_)
+ file_ = open(filename_, "w")
+ file_.write("#include <visp3/" + module_name_+ "/" + class_name_ + ".h>\n\n")
+ file_.write("int main()\n")
+ file_.write("{\n")
+ file_.write(" " + class_name_ + " contrib;\n")
+ file_.write("}\n")
+ file_.close()
+ return
+
+def print_howto(root_dir_, parent_name_, module_name_, class_name_):
+ path_ = root_dir_ + "/" + parent_name_
+ if platform_type == 'win32':
+ convertPathToWin32(path_)
+ print("\nThe new module \"" + module_name_ + "\" is available in : " + path_, sep='')
+ print("\nTo build this new module, launch the following instructions:")
+ print("- get visp source from github")
+ if platform_type == 'win32':
+ root_dir_ = convertPathToWin32(root_dir_)
+ print(" $ cd ", root_dir_, sep='')
+ print(" $ git clone https://github.com/lagadic/visp.git")
+ print("- create a build folder")
+ path_build_ = root_dir_ + "/" + parent_name_ + "-build"
+ if platform_type == 'win32':
+ path_build_ = convertPathToWin32(path_build_)
+
+ print(" $ mkdir ", path_build_, sep='')
+ print(" $ cd ", path_build_, sep='')
+ print("- configure the build")
+ if platform_type == 'win32':
+ print(" $ cmake-gui ../visp")
+ print(" -> Click \"Configure\" button")
+ print(" -> Secify the generator for this projet and click \"Finish\" button")
+ print(" -> Set VISP_CONTRIB_MODULES_PATH to ", path_ )
+ print(" -> Click \"Configure\" button")
+ print(" -> Click \"Generate\" button")
+ else:
+ print(" $ cmake ../visp -DVISP_CONTRIB_MODULES_PATH=", root_dir_ + "/" + parent_name_, sep='')
+
+ print("- build", module_name_, "module and the corresponding test")
+ if platform_type == 'win32':
+ print(" -> Open ", path_build_, "\\visp.sln solution file and build the project", sep='')
+ else:
+ print(" $ make -j4 visp_", module_name_, sep='')
+ print(" $ make test-", class_name_, sep='')
+ print("- execute the corresponding test")
+ print(" $ ./modules/" + module_name_ + "/test-", class_name_, sep='')
+ print("- build all the project")
+ print(" $ make -j4 ")
+ return
+
+
+def create_module(root_dir_, parent_name_, module_name_, class_name_, visp_modules_):
+ create_tree(root_dir_, parent_name_, module_name_)
+ create_cmakelists(root_dir_, parent_name_, module_name_, visp_modules_)
+ create_header(root_dir_, parent_name_, module_name_, class_name_)
+ create_source(root_dir_, parent_name_, module_name_, class_name_)
+ create_test(root_dir_, parent_name_, module_name_, class_name_)
+ print_howto(root_dir_, parent_name_, module_name_, class_name_)
+
+try:
+ opts, args = getopt.getopt(sys.argv[1:],"c:hm:p:r:v:", ["root-dir=", "parent-name=", "module-name=", "class-name=", "help"])
+
+except getopt.GetoptError:
+ # print help information and exit:
+ usage()
+ print("Error: ")
+ print(" Bad usage. ")
+ sys.exit(2)
+
+for o, v in opts:
+ if o in ("--root-dir", "-r"):
+ root_dir = v
+ if o in ("--parent-name", "-p"):
+ parent_name = v
+ if o in ("--module-name", "-m"):
+ module_name = v
+ if o in ("--class-name", "-c"):
+ class_name = v
+ if o in ("--visp-modules", "-v"):
+ visp_modules = v
+ if o in ("--help", "-h"):
+ usage()
+ sys.exit(2)
+
+print("New \"", module_name, "\" module creation in process...", sep='')
+
+create_module(root_dir, parent_name, module_name, class_name, visp_modules)
diff --git a/script/format-coding-style.sh b/script/format-coding-style.sh
new file mode 100644
index 0000000..e7d889b
--- /dev/null
+++ b/script/format-coding-style.sh
@@ -0,0 +1,33 @@
+#!/bin/sh
+
+script_path=`dirname $0`
+visp_root=`(cd $script_path/..; pwd)`
+
+# Change this if your clang-format executable is somewhere else
+clang_format="clang-format"
+
+command_exists () {
+ type "$1" &> /dev/null ;
+}
+
+if ! command_exists $clang_format ; then
+ echo "You have to install clang-format before using this script"
+ echo " Ubuntu: sudo apt-get install clang-format"
+ echo " OSX : brew update; brew install clang-format"
+ exit 0
+fi
+
+echo "Apply coding-style to visp: " $visp_root
+
+read -r -p "Are you sure? [Y/n] " answer
+answer="$(echo ${answer} | tr 'A-Z' 'a-z')"
+
+if echo "$answer" | grep -iq "^y" ;then
+ echo "We are applying coding-style rules..."
+else
+ echo "Exit the script"
+ exit 0
+fi
+
+find "$visp_root" \( -name '*.h' -or -name '*.hpp' -or -name '*.c' -or -name '*.cpp' \) -not -path "${visp_root}/3rdparty/*" -prune -print0 | xargs -0 "$clang_format" -i
+
diff --git a/tutorial/CMakeLists.txt b/tutorial/CMakeLists.txt
index f4a16c9..a0fbb29 100644
--- a/tutorial/CMakeLists.txt
+++ b/tutorial/CMakeLists.txt
@@ -25,24 +25,33 @@ if(MSVC)
endif()
endif()
-visp_add_subdirectory(bridge/opencv REQUIRED_DEPS visp_core visp_io)
-visp_add_subdirectory(computer-vision REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui)
-visp_add_subdirectory(grabber REQUIRED_DEPS visp_core visp_sensor visp_io visp_gui)
-visp_add_subdirectory(detection/barcode REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor)
-visp_add_subdirectory(detection/face REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor)
-visp_add_subdirectory(detection/matching REQUIRED_DEPS visp_core visp_vision visp_io visp_gui)
-visp_add_subdirectory(detection/object REQUIRED_DEPS visp_core visp_vision visp_mbt visp_io visp_gui)
-visp_add_subdirectory(image REQUIRED_DEPS visp_core visp_io visp_gui)
-visp_add_subdirectory(robot/pioneer REQUIRED_DEPS visp_core visp_robot visp_vs visp_gui)
-visp_add_subdirectory(simulator/image REQUIRED_DEPS visp_core visp_robot visp_io visp_gui)
-visp_add_subdirectory(trace REQUIRED_DEPS visp_core)
-visp_add_subdirectory(tracking/blob REQUIRED_DEPS visp_core visp_blob visp_io visp_gui visp_sensor)
-visp_add_subdirectory(tracking/keypoint REQUIRED_DEPS visp_core visp_vision visp_klt visp_io visp_gui visp_sensor)
-visp_add_subdirectory(tracking/model-based/generic REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
-visp_add_subdirectory(tracking/model-based/stereo REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
-visp_add_subdirectory(tracking/model-based/edges REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
-visp_add_subdirectory(tracking/model-based/hybrid REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
-visp_add_subdirectory(tracking/model-based/keypoint REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
-visp_add_subdirectory(tracking/template-tracker REQUIRED_DEPS visp_core visp_tt visp_io visp_gui)
-visp_add_subdirectory(tracking/moving-edges REQUIRED_DEPS visp_core visp_me visp_io visp_gui visp_sensor)
-visp_add_subdirectory(visual-servo/ibvs REQUIRED_DEPS visp_core visp_robot visp_vs visp_blob visp_vision visp_io visp_gui visp_ar)
+visp_add_subdirectory(bridge/opencv REQUIRED_DEPS visp_core visp_io)
+visp_add_subdirectory(computer-vision REQUIRED_DEPS visp_core visp_blob visp_vision visp_io visp_gui)
+visp_add_subdirectory(grabber REQUIRED_DEPS visp_core visp_sensor visp_io visp_gui)
+visp_add_subdirectory(detection/barcode REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor)
+visp_add_subdirectory(detection/face REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor)
+visp_add_subdirectory(detection/matching REQUIRED_DEPS visp_core visp_vision visp_io visp_gui)
+visp_add_subdirectory(detection/object REQUIRED_DEPS visp_core visp_vision visp_mbt visp_io visp_gui)
+visp_add_subdirectory(detection/tag REQUIRED_DEPS visp_core visp_detection visp_io visp_gui visp_sensor)
+visp_add_subdirectory(image REQUIRED_DEPS visp_core visp_io visp_gui)
+visp_add_subdirectory(imgproc/autothreshold REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/brightness REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/connected-components REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/contour REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/contrast-sharpening REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/count-coins REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(imgproc/flood-fill REQUIRED_DEPS visp_core visp_io visp_gui visp_imgproc)
+visp_add_subdirectory(robot/pioneer REQUIRED_DEPS visp_core visp_robot visp_vs visp_gui)
+visp_add_subdirectory(simulator/image REQUIRED_DEPS visp_core visp_robot visp_io visp_gui)
+visp_add_subdirectory(trace REQUIRED_DEPS visp_core)
+visp_add_subdirectory(tracking/blob REQUIRED_DEPS visp_core visp_blob visp_io visp_gui visp_sensor)
+visp_add_subdirectory(tracking/keypoint REQUIRED_DEPS visp_core visp_vision visp_klt visp_io visp_gui visp_sensor)
+visp_add_subdirectory(tracking/model-based/generic REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/stereo REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/stereo-generic REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/edges REQUIRED_DEPS visp_core visp_mbt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/hybrid REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
+visp_add_subdirectory(tracking/model-based/keypoint REQUIRED_DEPS visp_core visp_mbt visp_klt visp_io visp_gui)
+visp_add_subdirectory(tracking/template-tracker REQUIRED_DEPS visp_core visp_tt visp_io visp_gui)
+visp_add_subdirectory(tracking/moving-edges REQUIRED_DEPS visp_core visp_me visp_io visp_gui visp_sensor)
+visp_add_subdirectory(visual-servo/ibvs REQUIRED_DEPS visp_core visp_robot visp_vs visp_blob visp_vision visp_io visp_gui visp_ar)
diff --git a/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp
index be16f98..dca52f1 100644
--- a/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp
+++ b/tutorial/bridge/opencv/tutorial-bridge-opencv.cpp
@@ -1,7 +1,7 @@
//! \example tutorial-bridge-opencv.cpp
#include <visp3/core/vpCameraParameters.h>
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
#include <opencv2/core/core.hpp>
@@ -22,8 +22,8 @@ int main()
//! [Set ViSP camera parameters]
//! [Set OpenCV camera parameters]
- cv::Mat K = (cv::Mat_<double>(3,3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
- cv::Mat D = (cv::Mat_<double>(4,1) << cam.get_kud(), 0, 0, 0);
+ cv::Mat K = (cv::Mat_<double>(3, 3) << cam.get_px(), 0, cam.get_u0(), 0, cam.get_py(), cam.get_v0(), 0, 0, 1);
+ cv::Mat D = (cv::Mat_<double>(4, 1) << cam.get_kud(), 0, 0, 0);
//! [Set OpenCV camera parameters]
//! [Load ViSP image]
@@ -48,6 +48,6 @@ int main()
//! [Save image]
vpImageIo::write(IUndistorted, "chessboard-undistorted.pgm");
- //! [Save image]
+//! [Save image]
#endif
}
diff --git a/tutorial/computer-vision/CMakeLists.txt b/tutorial/computer-vision/CMakeLists.txt
index 01df152..080be9a 100644
--- a/tutorial/computer-vision/CMakeLists.txt
+++ b/tutorial/computer-vision/CMakeLists.txt
@@ -24,3 +24,8 @@ endforeach()
foreach(data ${tutorial_data})
visp_copy_data(tutorial-pose-from-points-image.cpp ${data})
endforeach()
+
+if(VISP_HAVE_ZBAR)
+ # Add specific build flag to turn off warnings coming from zbar 3rd party
+ visp_set_source_file_compile_flag(tutorial-pose-from-qrcode-image.cpp -Wno-unused-parameter)
+endif()
diff --git a/tutorial/computer-vision/tutorial-homography-from-points.cpp b/tutorial/computer-vision/tutorial-homography-from-points.cpp
index 6da57bf..407a7a0 100644
--- a/tutorial/computer-vision/tutorial-homography-from-points.cpp
+++ b/tutorial/computer-vision/tutorial-homography-from-points.cpp
@@ -10,23 +10,22 @@ int main()
//! [Set 3D points]
double L = 0.1;
std::vector<vpPoint> oP;
- oP.push_back( vpPoint( -L,-L, 0) );
- oP.push_back( vpPoint(2*L,-L, 0) );
- oP.push_back( vpPoint( L, 3*L, 0) );
- oP.push_back( vpPoint( -L, 4*L, 0) );
+ oP.push_back(vpPoint(-L, -L, 0));
+ oP.push_back(vpPoint(2 * L, -L, 0));
+ oP.push_back(vpPoint(L, 3 * L, 0));
+ oP.push_back(vpPoint(-L, 4 * L, 0));
//! [Set 3D points]
//! [Simulation]
- vpHomogeneousMatrix bMo(0.1, 0, 1, 0, vpMath::rad(15), 0);
+ 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 aMo = aMb*bMo;
+ vpHomogeneousMatrix aMo = aMb * bMo;
//! [Simulation]
//! [Image plane coordinates]
std::vector<vpPoint> aP(4), bP(4);
std::vector<double> xa(4), ya(4), xb(4), yb(4);
- for(unsigned int i=0 ; i < 4; i++)
- {
+ for (unsigned int i = 0; i < 4; i++) {
oP[i].project(aMo);
xa[i] = oP[i].get_x();
ya[i] = oP[i].get_y();
@@ -37,12 +36,12 @@ int main()
//! [Image plane coordinates]
//! [Compute homography]
- vpHomography aHb ;
+ 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]
@@ -53,12 +52,12 @@ int main()
//! [Get transformation]
//! [Print results]
- std::cout << "\nEstimated displacement:" << std::endl;
+ std::cout << "\nEstimated displacement:" << std::endl;
std::cout << " atb: " << atb.t() << std::endl;
vpThetaUVector atub;
atub.buildFrom(aRb);
std::cout << " athetaub: ";
- for(unsigned int i=0; i<3; i++)
+ for (unsigned int i = 0; i < 3; i++)
std::cout << vpMath::deg(atub[i]) << " ";
std::cout << std::endl;
std::cout << " n: " << n.t() << std::endl;
@@ -76,7 +75,7 @@ int main()
//! [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 78940e9..95de3d9 100644
--- a/tutorial/computer-vision/tutorial-pose-from-points-image.cpp
+++ b/tutorial/computer-vision/tutorial-pose-from-points-image.cpp
@@ -1,17 +1,18 @@
/*! \example tutorial-pose-from-points-image.cpp */
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/blob/vpDot2.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/vision/vpPose.h>
-void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
- const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
+void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot, const vpCameraParameters &cam, bool init,
+ vpHomogeneousMatrix &cMo)
{
- vpPose pose; double x=0, y=0;
- for (unsigned int i=0; i < point.size(); i ++) {
+ vpPose pose;
+ double x = 0, y = 0;
+ for (unsigned int i = 0; i < point.size(); i++) {
vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
point[i].set_x(x);
point[i].set_y(y);
@@ -47,24 +48,24 @@ int main()
vpDisplayOpenCV d(I);
#endif
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
std::vector<vpDot2> dot(4);
dot[0].initTracking(I, vpImagePoint(193, 157));
dot[1].initTracking(I, vpImagePoint(203, 366));
dot[2].initTracking(I, vpImagePoint(313, 402));
dot[3].initTracking(I, vpImagePoint(304, 133));
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.06, -0.06, 0) );
- point.push_back( vpPoint( 0.06, -0.06, 0) );
- point.push_back( vpPoint( 0.06, 0.06, 0) );
- point.push_back( vpPoint(-0.06, 0.06, 0) );
+ point.push_back(vpPoint(-0.06, -0.06, 0));
+ point.push_back(vpPoint(0.06, -0.06, 0));
+ point.push_back(vpPoint(0.06, 0.06, 0));
+ point.push_back(vpPoint(-0.06, 0.06, 0));
vpHomogeneousMatrix cMo;
bool init = true;
- while(1) {
+ while (1) {
vpImageIo::read(I, "square.pgm");
vpDisplay::display(I);
- for (unsigned int i=0; i < dot.size(); i ++) {
+ for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].setGraphics(true);
dot[i].track(I);
}
@@ -72,14 +73,15 @@ int main()
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
vpDisplay::flush(I);
- if (init) init = false; // turn off pose initialisation
+ if (init)
+ init = false; // turn off pose initialisation
- if (vpDisplay::getClick(I, false)) break;
+ if (vpDisplay::getClick(I, false))
+ break;
vpTime::wait(40);
}
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
}
diff --git a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
index f1a4b5d..4cf5a6e 100644
--- a/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
+++ b/tutorial/computer-vision/tutorial-pose-from-points-tracking.cpp
@@ -4,24 +4,25 @@
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#endif
+#include <visp3/blob/vpDot2.h>
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/blob/vpDot2.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/vision/vpPose.h>
-void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
- const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo);
+void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot, const vpCameraParameters &cam, bool init,
+ vpHomogeneousMatrix &cMo);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init);
#endif
-void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
- const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
+void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot, const vpCameraParameters &cam, bool init,
+ vpHomogeneousMatrix &cMo)
{
- vpPose pose; double x=0, y=0;
- for (unsigned int i=0; i < point.size(); i ++) {
+ vpPose pose;
+ double x = 0, y = 0;
+ for (unsigned int i = 0; i < point.size(); i++) {
vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
point[i].set_x(x);
point[i].set_y(y);
@@ -29,15 +30,15 @@ void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot,
}
if (init == true) {
- vpHomogeneousMatrix cMo_dem;
- vpHomogeneousMatrix cMo_lag;
- pose.computePose(vpPose::DEMENTHON, cMo_dem);
- pose.computePose(vpPose::LAGRANGE, cMo_lag);
- double residual_dem = pose.computeResidual(cMo_dem);
- double residual_lag = pose.computeResidual(cMo_lag);
- if (residual_dem < residual_lag)
+ vpHomogeneousMatrix cMo_dem;
+ vpHomogeneousMatrix cMo_lag;
+ pose.computePose(vpPose::DEMENTHON, cMo_dem);
+ pose.computePose(vpPose::LAGRANGE, cMo_lag);
+ double residual_dem = pose.computeResidual(cMo_dem);
+ double residual_lag = pose.computeResidual(cMo_lag);
+ if (residual_dem < residual_lag)
cMo = cMo_dem;
- else
+ else
cMo = cMo_lag;
}
pose.computePose(vpPose::VIRTUAL_VS, cMo);
@@ -48,15 +49,14 @@ void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
{
if (init) {
vpDisplay::flush(I);
- for(unsigned int i=0; i<dot.size(); i++) {
+ for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].setGraphics(true);
dot[i].setGraphicsThickness(2);
dot[i].initTracking(I);
vpDisplay::flush(I);
}
- }
- else {
- for(unsigned int i=0; i<dot.size(); i++) {
+ } else {
+ for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].track(I);
}
}
@@ -65,8 +65,10 @@ void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
int main()
{
-#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
- try { vpImage<unsigned char> I;
+#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
+ (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+ try {
+ vpImage<unsigned char> I;
#if defined(VISP_HAVE_DC1394)
vp1394TwoGrabber g;
@@ -76,17 +78,17 @@ int main()
g.open(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(0); // open the default camera
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
cv::Mat frame;
g >> frame; // get a new frame from camera
- vpImageConvert::convert(frame, I);
+ vpImageConvert::convert(frame, I);
#endif
// Parameters of our camera
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
// The pose container
vpHomogeneousMatrix cMo;
@@ -94,10 +96,10 @@ int main()
std::vector<vpDot2> dot(4);
std::vector<vpPoint> point;
double L = 0.06;
- point.push_back( vpPoint(-L, -L, 0) );
- point.push_back( vpPoint( L, -L, 0) );
- point.push_back( vpPoint( L, L, 0) );
- point.push_back( vpPoint(-L, L, 0) );
+ point.push_back(vpPoint(-L, -L, 0));
+ point.push_back(vpPoint(L, -L, 0));
+ point.push_back(vpPoint(L, L, 0));
+ point.push_back(vpPoint(-L, L, 0));
bool init = true;
#if defined(VISP_HAVE_X11)
@@ -108,12 +110,12 @@ int main()
vpDisplayOpenCV d(I);
#endif
- while(1){
- // Image Acquisition
+ while (1) {
+// Image Acquisition
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
- g >> frame;
+ g >> frame;
vpImageConvert::convert(frame, I);
#endif
@@ -122,13 +124,13 @@ int main()
computePose(point, dot, cam, init, cMo);
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
vpDisplay::flush(I);
- if (init) init = false; // turn off the initialisation specific stuff
+ if (init)
+ init = false; // turn off the initialisation specific stuff
if (vpDisplay::getClick(I, false))
break;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp b/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp
index 7a5a89a..25ac2b2 100644
--- a/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp
+++ b/tutorial/computer-vision/tutorial-pose-from-qrcode-image.cpp
@@ -1,17 +1,18 @@
/*! \example tutorial-pose-from-qrcode-image.cpp */
+#include <visp3/core/vpPixelMeterConversion.h>
+#include <visp3/detection/vpDetectorQRCode.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/vision/vpPose.h>
-#include <visp3/detection/vpDetectorQRCode.h>
-void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip,
- const vpCameraParameters &cam, bool init, vpHomogeneousMatrix &cMo)
+void computePose(std::vector<vpPoint> &point, const std::vector<vpImagePoint> &ip, const vpCameraParameters &cam,
+ bool init, vpHomogeneousMatrix &cMo)
{
- vpPose pose; double x=0, y=0;
- for (unsigned int i=0; i < point.size(); i ++) {
+ vpPose pose;
+ double x = 0, y = 0;
+ for (unsigned int i = 0; i < point.size(); i++) {
vpPixelMeterConversion::convertPoint(cam, ip[i], x, y);
point[i].set_x(x);
point[i].set_y(y);
@@ -49,21 +50,24 @@ int main()
#endif
// Camera parameters should be adapted to your camera
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
// 3D model of the QRcode: here we consider a 12cm by 12cm QRcode
std::vector<vpPoint> point;
- 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
- point.push_back( vpPoint(-0.06, 0.06, 0) ); // QCcode point 3 3D coordinates in plane Z=0
+ 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
+ point.push_back(vpPoint(-0.06, 0.06,
+ 0)); // QCcode point 3 3D coordinates in plane Z=0
vpHomogeneousMatrix cMo;
bool init = true;
vpDetectorQRCode detector;
- while(1) {
+ while (1) {
vpImageIo::read(I, "bar-code.pgm");
vpDisplay::display(I);
@@ -71,35 +75,36 @@ 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++) {
+ for (size_t i = 0; i < detector.getNbObjects(); i++) {
std::vector<vpImagePoint> p = detector.getPolygon(i); // get the four corners location in the image
- for(size_t j=0; j < p.size(); j++) {
+ 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
+ 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;
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)) break;
+ if (vpDisplay::getClick(I, false))
+ break;
vpTime::wait(40);
}
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
#else
diff --git a/tutorial/detection/barcode/CMakeLists.txt b/tutorial/detection/barcode/CMakeLists.txt
index 41d7477..2b08b69 100644
--- a/tutorial/detection/barcode/CMakeLists.txt
+++ b/tutorial/detection/barcode/CMakeLists.txt
@@ -22,3 +22,8 @@ foreach(data ${tutorial_data})
visp_copy_data(tutorial-barcode-detector.cpp ${data})
endforeach()
+if(VISP_HAVE_ZBAR)
+ # Add specific build flag to turn off warnings coming from zbar 3rd party
+ visp_set_source_file_compile_flag(tutorial-barcode-detector.cpp -Wno-unused-parameter)
+ visp_set_source_file_compile_flag(tutorial-barcode-detector-live.cpp -Wno-unused-parameter)
+endif()
diff --git a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
index a8d71dd..eecc6a5 100644
--- a/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
+++ b/tutorial/detection/barcode/tutorial-barcode-detector-live.cpp
@@ -1,29 +1,30 @@
//! \example tutorial-barcode-detector-live.cpp
#include <visp3/core/vpConfig.h>
-#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImageConvert.h>
#include <visp3/detection/vpDetectorDataMatrixCode.h>
#include <visp3/detection/vpDetectorQRCode.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vpV4l2Grabber.h>
#endif
-int main(int argc, const char** argv)
+int main(int argc, const char **argv)
{
#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
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--device")
- opt_device = atoi(argv[i+1]);
+ opt_device = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--code-type")
- opt_barcode = atoi(argv[i+1]);
+ opt_barcode = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0]
- << " [--device <camera number>] [--code-type <0 for QRcode | 1 for DataMatrix>] [--help]"
+ << " [--device <camera number>] [--code-type <0 for QRcode | "
+ "1 for DataMatrix>] [--help]"
<< std::endl;
return 0;
}
@@ -33,7 +34,7 @@ int main(int argc, const char** argv)
try {
vpImage<unsigned char> I; // for gray images
- //! [Construct grabber]
+//! [Construct grabber]
#if defined(VISP_HAVE_V4L2)
vpV4l2Grabber g;
std::ostringstream device;
@@ -43,7 +44,7 @@ int main(int argc, const char** argv)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture cap(opt_device); // open the default camera
- if(!cap.isOpened()) { // check if we succeeded
+ if (!cap.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -51,7 +52,7 @@ int main(int argc, const char** argv)
cap >> frame; // get a new frame from camera
vpImageConvert::convert(frame, I);
#endif
- //! [Construct grabber]
+//! [Construct grabber]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
@@ -76,8 +77,8 @@ int main(int argc, const char** argv)
(void)opt_barcode;
#endif
- for(;;) {
- //! [Acquisition]
+ for (;;) {
+//! [Acquisition]
#if defined(VISP_HAVE_V4L2)
g.acquire(I);
#else
@@ -93,28 +94,28 @@ int main(int argc, const char** argv)
vpDisplay::displayText(I, 10, 10, legend.str(), vpColor::red);
if (status) {
- for(size_t i=0; i < detector->getNbObjects(); i++) {
+ for (size_t i = 0; i < detector->getNbObjects(); i++) {
std::vector<vpImagePoint> 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);
- for(size_t j=0; j < p.size(); j++) {
+ 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;
}
delete detector;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/detection/barcode/tutorial-barcode-detector.cpp b/tutorial/detection/barcode/tutorial-barcode-detector.cpp
index 286947c..1d042e9 100644
--- a/tutorial/detection/barcode/tutorial-barcode-detector.cpp
+++ b/tutorial/detection/barcode/tutorial-barcode-detector.cpp
@@ -8,10 +8,11 @@
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-int main(int argc, const char** argv)
+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))
+//! [Macro defined]
+#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<unsigned char> I;
@@ -27,18 +28,16 @@ int main(int argc, const char** argv)
//! [Create base detector]
vpDetectorBase *detector = NULL;
- //! [Create base detector]
+//! [Create base detector]
#if (defined(VISP_HAVE_ZBAR) && defined(VISP_HAVE_DMTX))
int opt_barcode = 0; // 0=QRCode, 1=DataMatrix
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--code-type")
- opt_barcode = atoi(argv[i+1]);
+ 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;
}
}
@@ -47,7 +46,7 @@ int main(int argc, const char** argv)
detector = new vpDetectorQRCode;
else
detector = new vpDetectorDataMatrixCode;
- //! [Create detector]
+//! [Create detector]
#elif defined(VISP_HAVE_ZBAR)
detector = new vpDetectorQRCode;
(void)argc;
@@ -65,11 +64,11 @@ 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) {
- for(size_t i=0; i < detector->getNbObjects(); i++) {
+ for (size_t i = 0; i < detector->getNbObjects(); i++) {
//! [Parse detected codes]
//! [Get location]
std::vector<vpImagePoint> p = detector->getPolygon(i);
@@ -77,25 +76,23 @@ 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++) {
+ 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);
}
delete detector;
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
#else
diff --git a/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp b/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp
index c78b5de..0338e17 100644
--- a/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp
+++ b/tutorial/detection/face/tutorial-face-detector-live-threaded.cpp
@@ -5,9 +5,9 @@
#include <visp3/core/vpMutex.h>
#include <visp3/core/vpThread.h>
#include <visp3/core/vpTime.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/detection/vpDetectorFace.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/sensor/vpV4l2Grabber.h>
#if (VISP_HAVE_OPENCV_VERSION >= 0x020200) && (defined(VISP_HAVE_PTHREAD) || defined(_WIN32))
@@ -15,11 +15,7 @@
#include <opencv2/highgui/highgui.hpp>
// 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)
@@ -34,12 +30,12 @@ vpRect s_face_bbox;
vpThread::Return captureFunction(vpThread::Args args)
{
#if defined(VISP_HAVE_V4L2)
- vpV4l2Grabber cap = *((vpV4l2Grabber *) args);
+ vpV4l2Grabber cap = *(static_cast<vpV4l2Grabber *>(args));
#elif defined(VISP_HAVE_OPENCV)
- cv::VideoCapture cap = *((cv::VideoCapture *) args);
+ cv::VideoCapture cap = *((cv::VideoCapture *)args);
#endif
- // If the image is larger than 640 by 480, we subsample
+// If the image is larger than 640 by 480, we subsample
#if defined(VISP_HAVE_V4L2)
vpImage<unsigned char> frame_;
#elif defined(VISP_HAVE_OPENCV)
@@ -93,7 +89,8 @@ vpThread::Return displayFunction(vpThread::Args args)
// Check if a frame is available
if (capture_state_ == capture_started) {
- // Get the frame and convert it to a ViSP image used by the display class
+ // Get the frame and convert it to a ViSP image used by the display
+ // class
{
vpMutex::vpScopedLock lock(s_mutex_capture);
#if defined(VISP_HAVE_V4L2)
@@ -104,8 +101,8 @@ vpThread::Return displayFunction(vpThread::Args args)
}
// Check if we need to initialize the display with the first frame
- if (! display_initialized_) {
- // Initialize the display
+ if (!display_initialized_) {
+// Initialize the display
#if defined(VISP_HAVE_X11)
d_ = new vpDisplayX(I_);
display_initialized_ = true;
@@ -139,11 +136,10 @@ vpThread::Return displayFunction(vpThread::Args args)
// Update the display
vpDisplay::flush(I_);
- }
- else {
+ } else {
vpTime::wait(2); // Sleep 2ms
}
- } while(capture_state_ != capture_stopped);
+ } while (capture_state_ != capture_stopped);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
delete d_;
@@ -156,7 +152,7 @@ vpThread::Return displayFunction(vpThread::Args args)
//! [face-detection-threaded detectionFunction]
vpThread::Return detectionFunction(vpThread::Args args)
{
- std::string opt_face_cascade_name = *((std::string *) args);
+ std::string opt_face_cascade_name = *((std::string *)args);
vpDetectorFace face_detector_;
face_detector_.setCascadeClassifierFile(opt_face_cascade_name);
@@ -187,11 +183,10 @@ vpThread::Return detectionFunction(vpThread::Args args)
s_face_available = true;
s_face_bbox = face_detector_.getBBox(0); // Get largest face bounding box
}
- }
- else {
+ } else {
vpTime::wait(2); // Sleep 2ms
}
- } while(capture_state_ != capture_stopped);
+ } while (capture_state_ != capture_stopped);
std::cout << "End of face detection thread" << std::endl;
return 0;
@@ -199,26 +194,30 @@ vpThread::Return detectionFunction(vpThread::Args args)
//! [face-detection-threaded detectionFunction]
//! [face-detection-threaded mainFunction]
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
std::string opt_face_cascade_name = "./haarcascade_frontalface_alt.xml";
unsigned int opt_device = 0;
- unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn it to 1 to avoid subsampling
+ unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn
+ // it to 1 to avoid subsampling
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--haar")
- opt_face_cascade_name = std::string(argv[i+1]);
+ opt_face_cascade_name = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--device")
- opt_device = (unsigned int)atoi(argv[i+1]);
+ opt_device = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--scale")
- opt_scale = (unsigned int)atoi(argv[i+1]);
+ opt_scale = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "Usage: " << argv[0] << " [--haar <haarcascade xml filename>] [--device <camera device>] [--scale <subsampling factor>] [--help]" << std::endl;
+ std::cout << "Usage: " << argv[0]
+ << " [--haar <haarcascade xml filename>] [--device <camera "
+ "device>] [--scale <subsampling factor>] [--help]"
+ << std::endl;
return 0;
}
}
- // Instanciate the capture
+// Instanciate the capture
#if defined(VISP_HAVE_V4L2)
vpV4l2Grabber cap;
std::ostringstream device;
@@ -228,17 +227,17 @@ int main(int argc, const char* argv[])
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture cap;
cap.open(opt_device);
-# if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- int width = (int)cap.get(cv::CAP_PROP_FRAME_WIDTH);
- int height = (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT);
- cap.set(cv::CAP_PROP_FRAME_WIDTH, width/opt_scale);
- cap.set(cv::CAP_PROP_FRAME_HEIGHT, height/opt_scale);
-# else
- int width = cap.get(CV_CAP_PROP_FRAME_WIDTH);
- int height = cap.get(CV_CAP_PROP_FRAME_HEIGHT);
- cap.set(CV_CAP_PROP_FRAME_WIDTH, width/opt_scale);
- cap.set(CV_CAP_PROP_FRAME_HEIGHT, height/opt_scale);
-# endif
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ int width = (int)cap.get(cv::CAP_PROP_FRAME_WIDTH);
+ int height = (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT);
+ cap.set(cv::CAP_PROP_FRAME_WIDTH, width / opt_scale);
+ cap.set(cv::CAP_PROP_FRAME_HEIGHT, height / opt_scale);
+#else
+ int width = cap.get(CV_CAP_PROP_FRAME_WIDTH);
+ int height = cap.get(CV_CAP_PROP_FRAME_HEIGHT);
+ cap.set(CV_CAP_PROP_FRAME_WIDTH, width / opt_scale);
+ cap.set(CV_CAP_PROP_FRAME_HEIGHT, height / opt_scale);
+#endif
#endif
// Start the threads
@@ -258,13 +257,13 @@ int main(int argc, const char* argv[])
#else
int main()
{
-# ifndef VISP_HAVE_OPENCV
+#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
+#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
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
}
#endif
diff --git a/tutorial/detection/face/tutorial-face-detector-live.cpp b/tutorial/detection/face/tutorial-face-detector-live.cpp
index d5adc7c..f4c7f84 100644
--- a/tutorial/detection/face/tutorial-face-detector-live.cpp
+++ b/tutorial/detection/face/tutorial-face-detector-live.cpp
@@ -1,58 +1,63 @@
//! \example tutorial-face-detector-live.cpp
#include <visp3/core/vpConfig.h>
+#include <visp3/detection/vpDetectorFace.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/detection/vpDetectorFace.h>
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vpV4l2Grabber.h>
#endif
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020200)
try {
std::string opt_face_cascade_name = "./haarcascade_frontalface_alt.xml";
unsigned int opt_device = 0;
- unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn it to 1 to avoid subsampling
+ unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn
+ // it to 1 to avoid subsampling
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--haar")
- opt_face_cascade_name = std::string(argv[i+1]);
+ opt_face_cascade_name = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--device")
- opt_device = (unsigned int)atoi(argv[i+1]);
+ opt_device = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--scale")
- opt_scale = (unsigned int)atoi(argv[i+1]);
+ opt_scale = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "Usage: " << argv[0] << " [--haar <haarcascade xml filename>] [--device <camera device>] [--scale <subsampling factor>] [--help]" << std::endl;
+ std::cout << "Usage: " << argv[0]
+ << " [--haar <haarcascade xml filename>] [--device <camera "
+ "device>] [--scale <subsampling factor>] [--help]"
+ << std::endl;
return 0;
}
}
vpImage<unsigned char> I; // for gray images
- //! [Construct grabber]
+//! [Construct grabber]
#if defined(VISP_HAVE_V4L2)
vpV4l2Grabber g;
std::ostringstream device;
device << "/dev/video" << opt_device;
g.setDevice(device.str());
- g.setScale(opt_scale); // Default value is 2 in the constructor. Turn it to 1 to avoid subsampling
+ g.setScale(opt_scale); // Default value is 2 in the constructor. Turn it
+ // to 1 to avoid subsampling
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture cap(opt_device); // open the default camera
-# if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
- int width = (int)cap.get(cv::CAP_PROP_FRAME_WIDTH);
+#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
+ int width = (int)cap.get(cv::CAP_PROP_FRAME_WIDTH);
int height = (int)cap.get(cv::CAP_PROP_FRAME_HEIGHT);
- cap.set(cv::CAP_PROP_FRAME_WIDTH, width/opt_scale);
- cap.set(cv::CAP_PROP_FRAME_HEIGHT, height/opt_scale);
-# else
- int width = cap.get(CV_CAP_PROP_FRAME_WIDTH);
+ cap.set(cv::CAP_PROP_FRAME_WIDTH, width / opt_scale);
+ cap.set(cv::CAP_PROP_FRAME_HEIGHT, height / opt_scale);
+#else
+ int width = cap.get(CV_CAP_PROP_FRAME_WIDTH);
int height = cap.get(CV_CAP_PROP_FRAME_HEIGHT);
- cap.set(CV_CAP_PROP_FRAME_WIDTH, width/opt_scale);
- cap.set(CV_CAP_PROP_FRAME_HEIGHT, height/opt_scale);
-# endif
- if(!cap.isOpened()) { // check if we succeeded
+ cap.set(CV_CAP_PROP_FRAME_WIDTH, width / opt_scale);
+ cap.set(CV_CAP_PROP_FRAME_HEIGHT, height / opt_scale);
+#endif
+ if (!cap.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -60,7 +65,7 @@ int main(int argc, const char* argv[])
cap >> frame; // get a new frame from camera
vpImageConvert::convert(frame, I);
#endif
- //! [Construct grabber]
+//! [Construct grabber]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
@@ -74,9 +79,9 @@ int main(int argc, const char* argv[])
vpDetectorFace face_detector;
face_detector.setCascadeClassifierFile(opt_face_cascade_name);
- while(1) {
+ while (1) {
double t = vpTime::measureTimeMs();
- //! [Acquisition]
+//! [Acquisition]
#if defined(VISP_HAVE_V4L2)
g.acquire(I);
bool face_found = face_detector.detect(I);
@@ -93,22 +98,21 @@ int main(int argc, const char* argv[])
std::ostringstream text;
text << "Found " << face_detector.getNbObjects() << " face(s)";
vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
- for(size_t i=0; i < face_detector.getNbObjects(); i++) {
- std::vector<vpImagePoint> p = face_detector.getPolygon(i);
+ 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;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
}
#else
diff --git a/tutorial/detection/face/tutorial-face-detector.cpp b/tutorial/detection/face/tutorial-face-detector.cpp
index aa694cd..556afb3 100644
--- a/tutorial/detection/face/tutorial-face-detector.cpp
+++ b/tutorial/detection/face/tutorial-face-detector.cpp
@@ -7,7 +7,7 @@
//! [Include]
#include <visp3/io/vpVideoReader.h>
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
//! [Macro defined]
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020200)
@@ -18,13 +18,16 @@ int main(int argc, const char* argv[])
std::string opt_video = "video.mpeg";
//! [Default settings]
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--haar")
- opt_face_cascade_name = std::string(argv[i+1]);
+ opt_face_cascade_name = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--video")
- opt_video = std::string(argv[i+1]);
+ opt_video = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "Usage: " << argv[0] << " [--haar <haarcascade xml filename>] [--video <input video file>] [--help]" << std::endl;
+ std::cout << "Usage: " << argv[0]
+ << " [--haar <haarcascade xml filename>] [--video <input "
+ "video file>] [--help]"
+ << std::endl;
return 0;
}
}
@@ -52,7 +55,7 @@ int main(int argc, const char* argv[])
//! [Face detector setting]
bool exit_requested = false;
- while( ! g.end() && ! exit_requested) {
+ while (!g.end() && !exit_requested) {
g.acquire(I);
vpDisplay::display(I);
@@ -67,24 +70,22 @@ int main(int argc, const char* argv[])
//! [Get number faces]
vpDisplay::displayText(I, 10, 10, text.str(), vpColor::red);
//! [Get face characteristics]
- for(size_t i=0; i < face_detector.getNbObjects(); i++) {
- std::vector<vpImagePoint> p = face_detector.getPolygon(i);
+ 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(),
+ 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;
}
- if (! exit_requested)
+ if (!exit_requested)
vpDisplay::getClick(I);
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
}
#else
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
index aaa89ae..49ac5fe 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint-SIFT.cpp
@@ -1,14 +1,16 @@
//! \example tutorial-matching-keypoint-SIFT.cpp
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
#include <visp3/vision/vpKeyPoint.h>
//! [Include]
-int main() {
- //! [Define]
-#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
+int main()
+{
+//! [Define]
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020101) && \
+ (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
//! [Define]
vpImage<unsigned char> I;
@@ -19,7 +21,8 @@ int main() {
//! [Construction]
const std::string detectorName = "SIFT";
const std::string extractorName = "SIFT";
- //Use L2 distance with a matching done using FLANN (Fast Library for Approximate Nearest Neighbors)
+ // 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 keypoint(detectorName, extractorName, matcherName, filterType);
@@ -31,18 +34,17 @@ int main() {
//! [Create image]
vpImage<unsigned char> Idisp;
- Idisp.resize(I.getHeight(), 2*I.getWidth());
+ Idisp.resize(I.getHeight(), 2 * I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
//! [Create image]
//! [Init display]
- vpDisplayOpenCV d(Idisp, 0, 0, "Matching keypoints with SIFT keypoints") ;
+ vpDisplayOpenCV d(Idisp, 0, 0, "Matching keypoints with SIFT keypoints");
vpDisplay::display(Idisp);
vpDisplay::flush(Idisp);
//! [Init display]
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
//! [Acquisition]
reader.acquire(I);
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
@@ -50,7 +52,8 @@ int main() {
//! [Display]
vpDisplay::display(Idisp);
- vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2);
+ vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()),
+ vpColor::white, 2);
//! [Display]
//! [Matching]
@@ -61,8 +64,7 @@ int main() {
//! [Get matches]
vpImagePoint iPref, iPcur;
- for (unsigned int i = 0; i < nbMatch; i++)
- {
+ for (unsigned int i = 0; i < nbMatch; i++) {
keypoint.getMatchedPoints(i, iPref, iPcur);
//! [Get matches]
//! [Display matches]
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
index 42d0974..dfa4e5d 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint-homography.cpp
@@ -1,12 +1,12 @@
//! \example tutorial-matching-keypoint-homography.cpp
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/vision/vpHomography.h>
#include <visp3/vision/vpKeyPoint.h>
-#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/io/vpVideoReader.h>
int main(int argc, const char **argv)
-{
+{
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
//! [Select method]
int method = 0;
@@ -28,54 +28,55 @@ int main(int argc, const char **argv)
const std::string detectorName = "ORB";
const std::string extractorName = "ORB";
- //Hamming distance must be used with ORB
+ // Hamming distance must be used with ORB
const std::string matcherName = "BruteForce-Hamming";
vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType);
keypoint.buildReference(I);
vpImage<unsigned char> Idisp;
- Idisp.resize(I.getHeight(), 2*I.getWidth());
+ Idisp.resize(I.getHeight(), 2 * I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
- vpDisplayOpenCV d(Idisp, 0, 0, "Homography from matched keypoints") ;
+ vpDisplayOpenCV d(Idisp, 0, 0, "Homography from matched keypoints");
vpDisplay::display(Idisp);
vpDisplay::flush(Idisp);
//! [Set coordinates]
vpImagePoint corner_ref[4];
- corner_ref[0].set_ij(115, 64);
- corner_ref[1].set_ij( 83, 253);
+ corner_ref[0].set_ij(115, 64);
+ corner_ref[1].set_ij(83, 253);
corner_ref[2].set_ij(282, 307);
- corner_ref[3].set_ij(330, 72);
+ corner_ref[3].set_ij(330, 72);
//! [Set coordinates]
//! [Display]
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
vpDisplay::displayCross(Idisp, corner_ref[i], 12, vpColor::red);
}
vpDisplay::flush(Idisp);
//! [Display]
//! [Camera]
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
//! [Camera]
vpHomography curHref;
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
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()), vpColor::white, 2);
+ vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()),
+ vpColor::white, 2);
//! [Matching]
unsigned int nbMatch = keypoint.matchPoint(I);
std::cout << "Nb matches: " << nbMatch << std::endl;
//! [Matching]
- std::vector<vpImagePoint> iPref(nbMatch), iPcur(nbMatch); // Coordinates in pixels (for display only)
+ std::vector<vpImagePoint> iPref(nbMatch),
+ iPcur(nbMatch); // Coordinates in pixels (for display only)
//! [Allocation]
std::vector<double> mPref_x(nbMatch), mPref_y(nbMatch);
std::vector<double> mPcur_x(nbMatch), mPcur_y(nbMatch);
@@ -91,16 +92,14 @@ int main(int argc, const char **argv)
}
//! [Homography estimation]
- try{
+ 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);
+ (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);
- } catch(...)
- {
+ 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;
}
@@ -108,24 +107,21 @@ int main(int argc, const char **argv)
//! [Projection]
vpImagePoint corner_cur[4];
- for (int i=0; i< 4; i++) {
+ for (int i = 0; i < 4; i++) {
corner_cur[i] = vpHomography::project(cam, curHref, corner_ref[i]);
}
//! [Projection]
//! [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);
+ for (int i = 0; i < 4; i++) {
+ 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)
+ if (inliers[i] == true)
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green);
else
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red);
@@ -140,7 +136,8 @@ int main(int argc, const char **argv)
vpDisplay::getClick(Idisp);
#else
- (void)argc; (void)argv;
+ (void)argc;
+ (void)argv;
#endif
return 0;
}
diff --git a/tutorial/detection/matching/tutorial-matching-keypoint.cpp b/tutorial/detection/matching/tutorial-matching-keypoint.cpp
index 5e84a81..34d565b 100644
--- a/tutorial/detection/matching/tutorial-matching-keypoint.cpp
+++ b/tutorial/detection/matching/tutorial-matching-keypoint.cpp
@@ -1,13 +1,14 @@
//! \example tutorial-matching-keypoint.cpp
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/io/vpVideoReader.h>
#include <visp3/io/vpImageIo.h>
+#include <visp3/io/vpVideoReader.h>
//! [Include]
#include <visp3/vision/vpKeyPoint.h>
//! [Include]
-int main() {
- //! [Define]
+int main()
+{
+//! [Define]
#if (VISP_HAVE_OPENCV_VERSION >= 0x020101)
//! [Define]
vpImage<unsigned char> I;
@@ -19,7 +20,7 @@ int main() {
//! [Construction]
const std::string detectorName = "ORB";
const std::string extractorName = "ORB";
- //Hamming distance must be used with ORB
+ // Hamming distance must be used with ORB
const std::string matcherName = "BruteForce-Hamming";
vpKeyPoint::vpFilterMatchingType filterType = vpKeyPoint::ratioDistanceThreshold;
vpKeyPoint keypoint(detectorName, extractorName, matcherName, filterType);
@@ -28,18 +29,17 @@ int main() {
//! [Create image]
vpImage<unsigned char> Idisp;
- Idisp.resize(I.getHeight(), 2*I.getWidth());
+ Idisp.resize(I.getHeight(), 2 * I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
//! [Create image]
//! [Init display]
- vpDisplayOpenCV d(Idisp, 0, 0, "Matching keypoints with ORB keypoints") ;
+ vpDisplayOpenCV d(Idisp, 0, 0, "Matching keypoints with ORB keypoints");
vpDisplay::display(Idisp);
vpDisplay::flush(Idisp);
//! [Init display]
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
//! [Acquisition]
reader.acquire(I);
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
@@ -47,7 +47,8 @@ int main() {
//! [Display]
vpDisplay::display(Idisp);
- vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2);
+ vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()),
+ vpColor::white, 2);
//! [Display]
//! [Matching]
@@ -58,8 +59,7 @@ int main() {
//! [Get matches]
vpImagePoint iPref, iPcur;
- for (unsigned int i = 0; i < nbMatch; i++)
- {
+ for (unsigned int i = 0; i < nbMatch; i++) {
keypoint.getMatchedPoints(i, iPref, iPcur);
//! [Get matches]
//! [Display matches]
diff --git a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
index b6d6064..0a4c038 100644
--- a/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
+++ b/tutorial/detection/matching/tutorial-matching-surf-deprecated.cpp
@@ -6,8 +6,8 @@
//! [Include]
int main()
-{
- //! [Define]
+{
+//! [Define]
#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
//! [Define]
vpImage<unsigned char> I;
@@ -23,18 +23,17 @@ int main()
//! [Create image]
vpImage<unsigned char> Idisp;
- Idisp.resize(I.getHeight(), 2*I.getWidth());
+ Idisp.resize(I.getHeight(), 2 * I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
//! [Create image]
//! [Init display]
- vpDisplayOpenCV d(Idisp, 0, 0, "Matching surf keypoints") ;
+ vpDisplayOpenCV d(Idisp, 0, 0, "Matching surf keypoints");
vpDisplay::display(Idisp);
vpDisplay::flush(Idisp);
//! [Init display]
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
//! [Acquisition]
reader.acquire(I);
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
@@ -42,7 +41,8 @@ int main()
//! [Display]
vpDisplay::display(Idisp);
- vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()), vpColor::white, 2);
+ vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()),
+ vpColor::white, 2);
//! [Display]
//! [Matching]
@@ -51,8 +51,7 @@ int main()
//! [Get matches]
vpImagePoint iPref, iPcur;
- for (unsigned int i = 0; i < nbMatch; i++)
- {
+ for (unsigned int i = 0; i < nbMatch; i++) {
surf.getMatchedPoints(i, iPref, iPcur);
//! [Get matches]
//! [Display matches]
diff --git a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
index 0e31e8e..6e00c4e 100644
--- a/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
+++ b/tutorial/detection/matching/tutorial-matching-surf-homography-deprecated.cpp
@@ -1,12 +1,12 @@
//! \example tutorial-matching-surf-homography-deprecated.cpp
+#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/io/vpVideoReader.h>
#include <visp3/vision/vpHomography.h>
#include <visp3/vision/vpKeyPointSurf.h>
-#include <visp3/core/vpPixelMeterConversion.h>
-#include <visp3/io/vpVideoReader.h>
int main(int argc, const char **argv)
-{
+{
#if defined(VISP_HAVE_OPENCV_NONFREE) && (VISP_HAVE_OPENCV_VERSION < 0x030000)
//! [Select method]
int method = 0;
@@ -30,7 +30,7 @@ int main(int argc, const char **argv)
surf.buildReference(I);
vpImage<unsigned char> Idisp;
- Idisp.resize(I.getHeight(), 2*I.getWidth());
+ Idisp.resize(I.getHeight(), 2 * I.getWidth());
Idisp.insert(I, vpImagePoint(0, 0));
Idisp.insert(I, vpImagePoint(0, I.getWidth()));
@@ -40,35 +40,36 @@ int main(int argc, const char **argv)
//! [Set coordinates]
vpImagePoint corner_ref[4];
- corner_ref[0].set_ij(115, 64);
- corner_ref[1].set_ij( 83, 253);
+ corner_ref[0].set_ij(115, 64);
+ corner_ref[1].set_ij(83, 253);
corner_ref[2].set_ij(282, 307);
- corner_ref[3].set_ij(330, 72);
+ corner_ref[3].set_ij(330, 72);
//! [Set coordinates]
//! [Display]
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
vpDisplay::displayCross(Idisp, corner_ref[i], 12, vpColor::red);
}
vpDisplay::flush(Idisp);
//! [Display]
//! [Camera]
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
//! [Camera]
vpHomography curHref;
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
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()), vpColor::white, 2);
+ vpDisplay::displayLine(Idisp, vpImagePoint(0, I.getWidth()), vpImagePoint(I.getHeight(), I.getWidth()),
+ vpColor::white, 2);
//! [Matching]
unsigned int nbMatch = surf.matchPoint(I);
//! [Matching]
- std::vector<vpImagePoint> iPref(nbMatch), iPcur(nbMatch); // Coordinates in pixels (for display only)
+ std::vector<vpImagePoint> iPref(nbMatch),
+ iPcur(nbMatch); // Coordinates in pixels (for display only)
//! [Allocation]
std::vector<double> mPref_x(nbMatch), mPref_y(nbMatch);
std::vector<double> mPcur_x(nbMatch), mPcur_y(nbMatch);
@@ -92,32 +93,28 @@ int main(int argc, const char **argv)
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);
+ (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]
vpImagePoint corner_cur[4];
- for (int i=0; i< 4; i++) {
+ for (int i = 0; i < 4; i++) {
corner_cur[i] = vpHomography::project(cam, curHref, corner_ref[i]);
}
//! [Projection]
//! [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);
+ for (int i = 0; i < 4; i++) {
+ 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)
+ if (inliers[i] == true)
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::green);
else
vpDisplay::displayLine(Idisp, iPref[i], iPcur[i] + offset, vpColor::red);
@@ -132,7 +129,8 @@ int main(int argc, const char **argv)
vpDisplay::getClick(Idisp);
#else
- (void)argc; (void)argv;
+ (void)argc;
+ (void)argv;
#endif
return 0;
}
diff --git a/tutorial/detection/object/CMakeLists.txt b/tutorial/detection/object/CMakeLists.txt
index 2425be0..167d06a 100644
--- a/tutorial/detection/object/CMakeLists.txt
+++ b/tutorial/detection/object/CMakeLists.txt
@@ -16,6 +16,7 @@ list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.xml")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.cao")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.wrl")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.init")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.ppm")
list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.mpg")
list(APPEND tutorial_data2 "${CMAKE_CURRENT_SOURCE_DIR}/cube.xml")
@@ -31,7 +32,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/detection/object/teabox.ppm b/tutorial/detection/object/teabox.ppm
new file mode 100644
index 0000000..b20630a
--- /dev/null
+++ b/tutorial/detection/object/teabox.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+GGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGNNNGGGNNNNNNGGGNNNNNNNNNNNNNNNNNNNNNNNNNNNVVVNNNNNNVVVVVVVVVVVVVVVVVVVVVVVV^^^VVV^^^^^^^^^VVV^^^^^^^^^^^^^^^NNNNNNNNNNNNNNNNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVNNNVVVVVVVVVVVVVVV^^^VVV^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ [...]
\ No newline at end of file
diff --git a/tutorial/detection/object/tutorial-detection-object-mbt.cpp b/tutorial/detection/object/tutorial-detection-object-mbt.cpp
index 92c9fdc..0de5eb0 100644
--- a/tutorial/detection/object/tutorial-detection-object-mbt.cpp
+++ b/tutorial/detection/object/tutorial-detection-object-mbt.cpp
@@ -1,23 +1,23 @@
//! \example tutorial-detection-object-mbt.cpp
#include <visp3/core/vpConfig.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/vision/vpKeyPoint.h>
-#include <visp3/core/vpIoTools.h>
-
-int main(int argc, char ** argv) {
-#if defined(VISP_HAVE_OPENCV) && ((VISP_HAVE_OPENCV_VERSION >= 0x020400) || defined(VISP_HAVE_FFMPEG))
+int main(int argc, char **argv)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
//! [MBT code]
try {
std::string videoname = "teabox.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -26,12 +26,11 @@ int main(int argc, char ** argv) {
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -57,18 +56,18 @@ int main(int argc, char ** argv) {
return 0;
#endif
- display.init(I, 100, 100,"Model-based edge tracker");
+ display.init(I, 100, 100, "Model-based edge tracker");
vpMbEdgeTracker tracker;
bool usexml = false;
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
tracker.getCameraParameters(cam);
usexml = true;
}
#endif
- if (! usexml) {
+ if (!usexml) {
vpMe me;
me.setMaskSize(5);
me.setMaskNumber(180);
@@ -81,42 +80,42 @@ int main(int argc, char ** argv) {
tracker.setMovingEdge(me);
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
- tracker.setAngleAppear( vpMath::rad(70) );
- tracker.setAngleDisappear( vpMath::rad(80) );
+ tracker.setAngleAppear(vpMath::rad(70));
+ tracker.setAngleDisappear(vpMath::rad(80));
tracker.setNearClippingDistance(0.1);
tracker.setFarClippingDistance(100.0);
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
}
tracker.setOgreVisibilityTest(false);
- if(vpIoTools::checkFilename(objectname + ".cao"))
+ if (vpIoTools::checkFilename(objectname + ".cao"))
tracker.loadModel(objectname + ".cao");
- else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ else if (vpIoTools::checkFilename(objectname + ".wrl"))
tracker.loadModel(objectname + ".wrl");
tracker.setDisplayFeatures(true);
tracker.initClick(I, objectname + ".init", true);
tracker.track(I);
- //! [MBT code]
+//! [MBT code]
- //! [Keypoint selection]
+//! [Keypoint selection]
+#if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
+ std::string detectorName = "SIFT";
+ std::string extractorName = "SIFT";
+ std::string matcherName = "BruteForce";
+ std::string configurationFile = "detection-config-SIFT.xml";
+#else
std::string detectorName = "FAST";
std::string extractorName = "ORB";
std::string matcherName = "BruteForce-Hamming";
std::string configurationFile = "detection-config.xml";
-
-#if (defined(VISP_HAVE_OPENCV_NONFREE) || defined(VISP_HAVE_OPENCV_XFEATURES2D))
- detectorName = "SIFT";
- extractorName = "SIFT";
- matcherName = "BruteForce";
- configurationFile = "detection-config-SIFT.xml";
#endif
//! [Keypoint selection]
//! [Keypoint declaration]
vpKeyPoint keypoint_learning;
//! [Keypoint declaration]
- if(usexml) {
- //! [Keypoint xml config]
+ if (usexml) {
+//! [Keypoint xml config]
#ifdef VISP_HAVE_XML2
keypoint_learning.loadConfigFile(configurationFile);
#endif
@@ -157,8 +156,8 @@ int main(int argc, char ** argv) {
//! [Display reference keypoints]
vpDisplay::display(I);
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
- vpDisplay::displayCross(I, (int) it->pt.y, (int) it->pt.x, 4, vpColor::red);
+ for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
+ vpDisplay::displayCross(I, (int)it->pt.y, (int)it->pt.x, 4, vpColor::red);
}
vpDisplay::displayText(I, 10, 10, "Learning step: keypoints are detected on visible teabox faces", vpColor::red);
vpDisplay::displayText(I, 30, 10, "Click to continue with detection...", vpColor::red);
@@ -168,7 +167,7 @@ int main(int argc, char ** argv) {
//! [Init keypoint detection]
vpKeyPoint keypoint_detection;
- if(usexml) {
+ if (usexml) {
#ifdef VISP_HAVE_XML2
keypoint_detection.loadConfigFile(configurationFile);
#endif
@@ -193,14 +192,14 @@ int main(int argc, char ** argv) {
double error;
bool click_done = false;
- while(! g.end()) {
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
vpDisplay::displayText(I, 10, 10, "Detection and localization in process...", vpColor::red);
//! [Matching and pose estimation]
- if(keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
+ if (keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
//! [Matching and pose estimation]
//! [Tracker set pose]
@@ -219,7 +218,7 @@ int main(int argc, char ** argv) {
break;
}
}
- if (! click_done)
+ if (!click_done)
vpDisplay::getClick(I);
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
@@ -227,14 +226,13 @@ int main(int argc, char ** argv) {
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
(void)argc;
(void)argv;
- std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
#endif
return 0;
diff --git a/tutorial/detection/object/tutorial-detection-object-mbt2.cpp b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp
index c83eca3..1cd29d6 100644
--- a/tutorial/detection/object/tutorial-detection-object-mbt2.cpp
+++ b/tutorial/detection/object/tutorial-detection-object-mbt2.cpp
@@ -1,16 +1,16 @@
//! \example tutorial-detection-object-mbt2.cpp
#include <visp3/core/vpConfig.h>
-#include <visp3/gui/vpDisplayX.h>
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
-#include <visp3/mbt/vpMbEdgeTracker.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/vision/vpKeyPoint.h>
-#include <visp3/core/vpIoTools.h>
-
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020400)
-void learnCube(const vpImage<unsigned char> &I, vpMbEdgeTracker &tracker, vpKeyPoint &keypoint_learning, int id) {
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
+void learnCube(const vpImage<unsigned char> &I, vpMbEdgeTracker &tracker, vpKeyPoint &keypoint_learning, int id)
+{
//! [Keypoints reference detection]
std::vector<cv::KeyPoint> trainKeyPoints;
double elapsedTime;
@@ -37,22 +37,23 @@ void learnCube(const vpImage<unsigned char> &I, vpMbEdgeTracker &tracker, vpKeyP
//! [Keypoints build reference]
//! [Display reference keypoints]
- for(std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
- vpDisplay::displayCross(I, (int) it->pt.y, (int) it->pt.x, 4, vpColor::red);
+ for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
+ vpDisplay::displayCross(I, (int)it->pt.y, (int)it->pt.x, 4, vpColor::red);
}
//! [Display reference keypoints]
}
#endif
-int main(int argc, char ** argv) {
-#if defined(VISP_HAVE_OPENCV) && ((VISP_HAVE_OPENCV_VERSION >= 0x020400) || defined(VISP_HAVE_FFMPEG))
+int main(int argc, char **argv)
+{
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020400)
//! [MBT code]
try {
std::string videoname = "cube.mpeg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -61,12 +62,11 @@ int main(int argc, char ** argv) {
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -80,13 +80,13 @@ int main(int argc, char ** argv) {
vpMbEdgeTracker tracker;
bool usexml = false;
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
tracker.getCameraParameters(cam);
usexml = true;
}
#endif
- if (! usexml) {
+ if (!usexml) {
vpMe me;
me.setMaskSize(5);
me.setMaskNumber(180);
@@ -99,17 +99,17 @@ int main(int argc, char ** argv) {
tracker.setMovingEdge(me);
cam.initPersProjWithoutDistortion(547, 542, 339, 235);
tracker.setCameraParameters(cam);
- tracker.setAngleAppear( vpMath::rad(89) );
- tracker.setAngleDisappear( vpMath::rad(89) );
+ tracker.setAngleAppear(vpMath::rad(89));
+ tracker.setAngleDisappear(vpMath::rad(89));
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(10.0);
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
}
tracker.setOgreVisibilityTest(false);
- if(vpIoTools::checkFilename(objectname + ".cao"))
+ if (vpIoTools::checkFilename(objectname + ".cao"))
tracker.loadModel(objectname + ".cao");
- else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ else if (vpIoTools::checkFilename(objectname + ".wrl"))
tracker.loadModel(objectname + ".wrl");
tracker.setDisplayFeatures(true);
//! [MBT code]
@@ -117,14 +117,14 @@ int main(int argc, char ** argv) {
//! [Keypoint declaration]
vpKeyPoint keypoint_learning("ORB", "ORB", "BruteForce-Hamming");
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
- keypoint_learning.setDetectorParameter("ORB", "nLevels", 1);
+ keypoint_learning.setDetectorParameter("ORB", "nLevels", 1);
#else
cv::Ptr<cv::ORB> orb_learning = keypoint_learning.getDetector("ORB").dynamicCast<cv::ORB>();
- if(orb_learning != NULL) {
+ if (orb_learning != NULL) {
orb_learning->setNLevels(1);
}
#endif
- //! [Keypoint declaration]
+//! [Keypoint declaration]
#if defined(VISP_HAVE_X11)
vpDisplayX display;
@@ -145,9 +145,9 @@ int main(int argc, char ** argv) {
vpHomogeneousMatrix(0.02143385294, 0.1098083886, 0.5127439561, 2.087159614, 1.141775176, -0.4701291124),
vpHomogeneousMatrix(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828),
vpHomogeneousMatrix(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025)};
- for(int i = 0; i < 3; i++) {
+ for (int i = 0; i < 3; i++) {
vpImageIo::read(I, imageName[i]);
- if (i==0) {
+ if (i == 0) {
display.init(I, 10, 10);
}
std::stringstream title;
@@ -174,7 +174,7 @@ int main(int argc, char ** argv) {
//! [Learn cube call]
vpDisplay::displayText(I, 10, 10, "Learning step: keypoints are detected on visible cube faces", vpColor::red);
- if(i < 2) {
+ if (i < 2) {
vpDisplay::displayText(I, 30, 10, "Click to continue the learning...", vpColor::red);
} else {
vpDisplay::displayText(I, 30, 10, "Click to continue with the detection...", vpColor::red);
@@ -198,7 +198,7 @@ int main(int argc, char ** argv) {
#else
cv::Ptr<cv::ORB> orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
- if(orb_detector != NULL) {
+ if (orb_detector != NULL) {
orb_detector->setNLevels(1);
}
#endif
@@ -232,7 +232,7 @@ int main(int argc, char ** argv) {
double error;
bool click_done = false;
- while(! g.end()) {
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
@@ -245,7 +245,7 @@ int main(int argc, char ** argv) {
double elapsedTime;
//! [Matching and pose estimation]
- if(keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
+ if (keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
//! [Matching and pose estimation]
//! [Tracker set pose]
@@ -265,7 +265,7 @@ int main(int argc, char ** argv) {
//! [Get RANSAC inliers outliers]
//! [Display RANSAC inliers]
- for(std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
vpDisplay::displayCircle(I, *it, 4, vpColor::green);
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
@@ -275,7 +275,7 @@ int main(int argc, char ** argv) {
//! [Display RANSAC inliers]
//! [Display RANSAC outliers]
- for(std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
+ for (std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
vpDisplay::displayCircle(I, *it, 4, vpColor::red);
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
@@ -290,8 +290,8 @@ int main(int argc, char ** argv) {
//! [Display model image matching]
vpCameraParameters cam2;
- cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(),
- cam.get_u0() + I.getWidth(), cam.get_v0() + I.getHeight());
+ 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);
@@ -312,7 +312,7 @@ int main(int argc, char ** argv) {
}
}
- if (! click_done)
+ if (!click_done)
vpDisplay::getClick(IMatching);
#ifdef VISP_HAVE_XML2
@@ -321,14 +321,13 @@ int main(int argc, char ** argv) {
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
(void)argc;
(void)argv;
- std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
#endif
return 0;
diff --git a/tutorial/detection/tag/AprilTag.pgm b/tutorial/detection/tag/AprilTag.pgm
new file mode 100644
index 0000000..b06e71f
--- /dev/null
+++ b/tutorial/detection/tag/AprilTag.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: GIMP PNM Filter Version 1.1
+640 480
+255
+��)()-*(')&'(&&'%#%''&#&'%'&$&$##"""$$"#%&'$#$$''' %(&&('&%%'&%(&%('('(')+)(*,,,+,-.1-+.0/,,,//...,030.0-.1/-00140/..0202/1211/011/133365433423575344553477566864420-,-031//25/++.000..,,,,*&$%'&%$#%#$%&&&$'&""#&*((+.-+)(+*,./.--/0.*!#-445332457632666865533251%
!18888:8,
$/878;9;<;:99:1#
06768758;9997.
(AA>=;9;:=A?8!!*-,,./0$
!,220.12/)$
!(++.0/.11233----/.-$
&1BKIMURSXW9/$!&*++)(*.//-0-$
*(()'$&)*'"(-(
"'./10,&# "##"" !"$&%%$"""$&'+-,*($" ""!
[...]
\ No newline at end of file
diff --git a/tutorial/detection/tag/CMakeLists.txt b/tutorial/detection/tag/CMakeLists.txt
new file mode 100644
index 0000000..b7fc2b1
--- /dev/null
+++ b/tutorial/detection/tag/CMakeLists.txt
@@ -0,0 +1,24 @@
+project(tutorial-apriltag)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_detection visp_io visp_gui visp_sensor)
+
+set(tutorial_cpp
+ tutorial-apriltag-detector.cpp
+ tutorial-apriltag-detector-live.cpp
+)
+
+set(tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/AprilTag.pgm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-apriltag-detector.cpp ${data})
+endforeach()
diff --git a/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp b/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp
new file mode 100644
index 0000000..0c6fe77
--- /dev/null
+++ b/tutorial/detection/tag/tutorial-apriltag-detector-live.cpp
@@ -0,0 +1,175 @@
+//! \example tutorial-apriltag-detector-live.cpp
+//! [Include]
+#include <visp3/detection/vpDetectorAprilTag.h>
+//! [Include]
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#ifdef VISP_HAVE_XML2
+#include <visp3/core/vpXmlParserCamera.h>
+#endif
+#ifdef VISP_HAVE_V4L2
+#include <visp3/sensor/vpV4l2Grabber.h>
+#endif
+#include <visp3/io/vpImageIo.h>
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
+ (defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+
+ int opt_device = 0;
+ 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;
+ std::string intrinsic_file = "";
+ std::string camera_name = "";
+ bool display_tag = false;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
+ poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
+ tagSize = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ opt_device = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
+ quad_decimate = (float)atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
+ nThreads = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
+ intrinsic_file = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
+ camera_name = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--display_tag") {
+ display_tag = true;
+ } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
+ tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <camera input>] [--tag_size <tag_size in m>]"
+ " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
+ " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
+ " [--pose_method <method> (0: HOMOGRAPHY_VIRTUAL_VS, 1: "
+ "DEMENTHON_VIRTUAL_VS,"
+ " 2: LAGRANGE_VIRTUAL_VS, 3: BEST_RESIDUAL_VIRTUAL_VS)]"
+ " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: "
+ "TAG_36ARTOOLKIT,"
+ " 3: TAG_25h9, 4: TAG_25h7, 5: TAG_16h5)]"
+ " [--display_tag] [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
+#ifdef VISP_HAVE_XML2
+ vpXmlParserCamera parser;
+ if (!intrinsic_file.empty() && !camera_name.empty())
+ parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
+#endif
+ std::cout << "cam:\n" << cam << std::endl;
+ std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
+ std::cout << "tagFamily: " << tagFamily << std::endl;
+
+ try {
+ vpImage<unsigned char> I;
+
+//! [Construct grabber]
+#if defined(VISP_HAVE_V4L2)
+ vpV4l2Grabber g;
+ std::ostringstream device;
+ device << "/dev/video" << opt_device;
+ g.setDevice(device.str());
+ g.setScale(1);
+ g.acquire(I);
+#elif defined(VISP_HAVE_OPENCV)
+ cv::VideoCapture cap(opt_device); // open the default camera
+ if (!cap.isOpened()) { // check if we succeeded
+ std::cout << "Failed to open the camera" << std::endl;
+ return EXIT_FAILURE;
+ }
+ cv::Mat frame;
+ cap >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
+#endif
+//! [Construct grabber]
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d(I);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I);
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d(I);
+#endif
+
+ //! [Create base detector]
+ vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
+ //! [Create base detector]
+
+ //! [AprilTag detector settings]
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagNbThreads(nThreads);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setDisplayTag(display_tag);
+ //! [AprilTag detector settings]
+
+ std::vector<double> time_vec;
+ for (;;) {
+//! [Acquisition]
+#if defined(VISP_HAVE_V4L2)
+ g.acquire(I);
+#elif defined(VISP_HAVE_OPENCV)
+ cap >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
+#endif
+ //! [Acquisition]
+
+ vpDisplay::display(I);
+
+ double t = vpTime::measureTimeMs();
+ //! [Detect and compute pose]
+ std::vector<vpHomogeneousMatrix> cMo_vec;
+ dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
+ //! [Detect and compute pose]
+ t = vpTime::measureTimeMs() - t;
+ time_vec.push_back(t);
+
+ std::stringstream ss;
+ ss << "Detection time: " << t << " ms for " << detector->getNbObjects() << " tags";
+ vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
+
+ //! [Display camera pose for each tag]
+ for (size_t i = 0; i < cMo_vec.size(); i++) {
+ vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
+ }
+ //! [Display camera pose for each tag]
+
+ vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::flush(I);
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+
+ std::cout << "Benchmark computation time" << std::endl;
+ std::cout << "Mean / Median / Std: " << vpMath::getMean(time_vec) << " ms"
+ << " ; " << vpMath::getMedian(time_vec) << " ms"
+ << " ; " << vpMath::getStdev(time_vec) << " ms" << std::endl;
+
+ delete detector;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/detection/tag/tutorial-apriltag-detector.cpp b/tutorial/detection/tag/tutorial-apriltag-detector.cpp
new file mode 100644
index 0000000..0dd3aaa
--- /dev/null
+++ b/tutorial/detection/tag/tutorial-apriltag-detector.cpp
@@ -0,0 +1,159 @@
+//! \example tutorial-apriltag-detector.cpp
+//! [Include]
+#include <visp3/detection/vpDetectorAprilTag.h>
+//! [Include]
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+#ifdef VISP_HAVE_XML2
+#include <visp3/core/vpXmlParserCamera.h>
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_APRILTAG) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+
+ std::string input_filename = "AprilTag.pgm";
+ 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;
+ std::string intrinsic_file = "";
+ std::string camera_name = "";
+ bool display_tag = false;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--pose_method" && i + 1 < argc) {
+ poseEstimationMethod = (vpDetectorAprilTag::vpPoseEstimationMethod)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
+ tagSize = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
+ quad_decimate = (float)atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--nthreads" && i + 1 < argc) {
+ nThreads = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
+ intrinsic_file = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
+ camera_name = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--display_tag") {
+ display_tag = true;
+ } else if (std::string(argv[i]) == "--tag_family" && i + 1 < argc) {
+ tagFamily = (vpDetectorAprilTag::vpAprilTagFamily)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input file>] [--tag_size <tag_size in m>]"
+ " [--quad_decimate <quad_decimate>] [--nthreads <nb>]"
+ " [--intrinsic <intrinsic file>] [--camera_name <camera name>]"
+ " [--pose_method <method> (0: HOMOGRAPHY_VIRTUAL_VS, 1: "
+ "DEMENTHON_VIRTUAL_VS,"
+ " 2: LAGRANGE_VIRTUAL_VS, 3: BEST_RESIDUAL_VIRTUAL_VS)]"
+ " [--tag_family <family> (0: TAG_36h11, 1: TAG_36h10, 2: "
+ "TAG_36ARTOOLKIT,"
+ " 3: TAG_25h9, 4: TAG_25h7)]"
+ " [--display_tag] [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
+#ifdef VISP_HAVE_XML2
+ vpXmlParserCamera parser;
+ if (!intrinsic_file.empty() && !camera_name.empty())
+ parser.parse(cam, intrinsic_file, camera_name, vpCameraParameters::perspectiveProjWithoutDistortion);
+#endif
+ std::cout << "cam:\n" << cam << std::endl;
+ std::cout << "poseEstimationMethod: " << poseEstimationMethod << std::endl;
+ std::cout << "tagFamily: " << tagFamily << std::endl;
+
+ try {
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, input_filename);
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d(I);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I);
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d(I);
+#endif
+
+ //! [Create base detector]
+ vpDetectorBase *detector = new vpDetectorAprilTag(tagFamily);
+ //! [Create base detector]
+
+ //! [AprilTag detector settings]
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagQuadDecimate(quad_decimate);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagPoseEstimationMethod(poseEstimationMethod);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setAprilTagNbThreads(nThreads);
+ dynamic_cast<vpDetectorAprilTag *>(detector)->setDisplayTag(display_tag);
+ //! [AprilTag detector settings]
+
+ vpDisplay::display(I);
+
+ double t = vpTime::measureTimeMs();
+ //! [Detect and compute pose]
+ std::vector<vpHomogeneousMatrix> cMo_vec;
+ dynamic_cast<vpDetectorAprilTag *>(detector)->detect(I, tagSize, cam, cMo_vec);
+ //! [Detect and compute pose]
+ t = vpTime::measureTimeMs() - t;
+
+ std::stringstream ss;
+ ss << "Detection time: " << t << " ms for " << detector->getNbObjects() << " tags";
+ vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
+
+ //! [Parse detected codes]
+ for (size_t i = 0; i < detector->getNbObjects(); i++) {
+ //! [Parse detected codes]
+ //! [Get location]
+ std::vector<vpImagePoint> p = detector->getPolygon(i);
+ vpRect bbox = detector->getBBox(i);
+ //! [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);
+ //! [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, 20, 20, "Click to display tag poses", vpColor::red);
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I);
+
+ vpDisplay::display(I);
+
+ //! [Display camera pose for each tag]
+ for (size_t i = 0; i < cMo_vec.size(); i++) {
+ vpDisplay::displayFrame(I, cMo_vec[i], cam, tagSize / 2, vpColor::none, 3);
+ }
+ //! [Display camera pose for each tag]
+
+ vpDisplay::displayText(I, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::flush(I);
+ vpDisplay::getClick(I);
+
+ delete detector;
+ } catch (const vpException &e) {
+ std::cerr << "Catch an exception: " << e.getMessage() << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/grabber/CMakeLists.txt b/tutorial/grabber/CMakeLists.txt
index 99183c4..c584996 100644
--- a/tutorial/grabber/CMakeLists.txt
+++ b/tutorial/grabber/CMakeLists.txt
@@ -8,10 +8,12 @@ find_package(VISP REQUIRED visp_core visp_sensor visp_io visp_gui)
set(tutorial_cpp
tutorial-grabber-1394.cpp
tutorial-grabber-1394-writer.cpp
+ tutorial-grabber-basler-pylon.cpp
tutorial-grabber-CMU1394.cpp
tutorial-grabber-flycapture.cpp
tutorial-grabber-opencv.cpp
tutorial-grabber-opencv-threaded.cpp
+ tutorial-grabber-realsense.cpp
tutorial-grabber-v4l2.cpp
tutorial-grabber-v4l2-threaded.cpp
tutorial-video-reader.cpp
@@ -24,6 +26,13 @@ if(VISP_HAVE_FLYCAPTURE)
# Add specific build flag to turn off warnings coming from PointGrey flycapture 3rd party
visp_set_source_file_compile_flag(tutorial-grabber-flycapture.cpp -Wno-unknown-pragmas -Wno-ignored-qualifiers)
endif()
+if(VISP_HAVE_PYLON)
+ visp_set_source_file_compile_flag(tutorial-grabber-basler-pylon.cpp -Wno-unknown-pragmas -Wno-overloaded-virtual -Wno-unused-parameter)
+endif()
+if(VISP_HAVE_REALSENSE)
+ # Add specific build flag to turn off warnings coming from PointGrey flycapture 3rd party
+ visp_set_source_file_compile_flag(tutorial-grabber-realsense.cpp -Wno-pessimizing-move -Wno-unused-parameter)
+endif()
foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
@@ -36,3 +45,4 @@ endforeach()
foreach(data ${tutorial_data})
visp_copy_data(tutorial-video-reader.cpp ${data})
endforeach()
+
diff --git a/tutorial/grabber/tutorial-grabber-1394-writer.cpp b/tutorial/grabber/tutorial-grabber-1394-writer.cpp
index 53be1c1..bee058f 100644
--- a/tutorial/grabber/tutorial-grabber-1394-writer.cpp
+++ b/tutorial/grabber/tutorial-grabber-1394-writer.cpp
@@ -1,20 +1,20 @@
/*! \example tutorial-grabber-1394-writer.cpp */
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoWriter.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main(int argc, char **)
{
#ifdef VISP_HAVE_DC1394
try {
bool save = false;
- if(argc == 2) {
+ if (argc == 2) {
save = true;
}
- vpImage<unsigned char> I; // Create a gray level image container
- bool reset = true; // Enable bus reset during construction (default)
+ vpImage<unsigned char> I; // Create a gray level image container
+ bool reset = true; // Enable bus reset during construction (default)
vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
g.setVideoMode(vp1394TwoGrabber::vpVIDEO_MODE_640x480_MONO8);
@@ -34,7 +34,7 @@ int main(int argc, char **)
if (save)
writer.open(I);
- while(1) {
+ while (1) {
g.acquire(I);
if (save)
@@ -50,8 +50,7 @@ int main(int argc, char **)
if (save)
writer.close();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/grabber/tutorial-grabber-1394.cpp b/tutorial/grabber/tutorial-grabber-1394.cpp
index 81c039a..48d15d5 100644
--- a/tutorial/grabber/tutorial-grabber-1394.cpp
+++ b/tutorial/grabber/tutorial-grabber-1394.cpp
@@ -1,14 +1,14 @@
//! \example tutorial-grabber-1394.cpp
-#include <visp3/sensor/vp1394TwoGrabber.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vp1394TwoGrabber.h>
int main()
{
#ifdef VISP_HAVE_DC1394
try {
vpImage<unsigned char> I; // Create a gray level image container
- bool reset = true; // Enable bus reset during construction (default)
+ bool reset = true; // Enable bus reset during construction (default)
//! [vp1394TwoGrabber construction]
vp1394TwoGrabber g(reset); // Create a grabber based on libdc1394-2.x third party lib
//! [vp1394TwoGrabber construction]
@@ -29,7 +29,7 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
- while(1) {
+ while (1) {
//! [vp1394TwoGrabber acquire]
g.acquire(I);
//! [vp1394TwoGrabber acquire]
@@ -40,8 +40,7 @@ int main()
break;
//! [vp1394TwoGrabber click to exit]
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/grabber/tutorial-grabber-CMU1394.cpp b/tutorial/grabber/tutorial-grabber-CMU1394.cpp
index bc7fc93..22edca1 100644
--- a/tutorial/grabber/tutorial-grabber-CMU1394.cpp
+++ b/tutorial/grabber/tutorial-grabber-CMU1394.cpp
@@ -1,7 +1,7 @@
/*! \example tutorial-grabber-CMU1394.cpp */
-#include <visp3/sensor/vp1394CMUGrabber.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/sensor/vp1394CMUGrabber.h>
int main()
{
@@ -24,15 +24,14 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
- while(1) {
+ while (1) {
g.acquire(I);
vpDisplay::display(I);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false)) // A click to exit
break;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/grabber/tutorial-grabber-basler-pylon.cpp b/tutorial/grabber/tutorial-grabber-basler-pylon.cpp
new file mode 100644
index 0000000..3fb0a0f
--- /dev/null
+++ b/tutorial/grabber/tutorial-grabber-basler-pylon.cpp
@@ -0,0 +1,78 @@
+/*! \example tutorial-grabber-basler-pylon.cpp */
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpPylonFactory.h>
+
+/*!
+ Usage :
+ To get the help : ./tutorial-grabber-basler-pylon --help
+ To set the device : ./tutorial-grabber-basler-pylon --device GigE
+ --camera 1
+ */
+int main(int argc, const char *argv[])
+{
+#ifdef VISP_HAVE_PYLON
+ try {
+ unsigned int opt_camera = 0;
+ std::string opt_device("GigE");
+
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--camera")
+ opt_camera = (unsigned int)atoi(argv[i + 1]);
+ if (std::string(argv[i]) == "--device")
+ opt_device = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--help") {
+ std::cout << "Usage: " << argv[0] << " [--camera <0...9>] [--device <\"GigE\"|\"USB\">] [--help]" << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ vpImage<unsigned char> I;
+
+ vpPylonFactory &factory = vpPylonFactory::instance();
+
+ vpPylonGrabber *g;
+ if (opt_device == "GigE" || opt_device == "gige") {
+ g = factory.createPylonGrabber(vpPylonFactory::BASLER_GIGE);
+ std::cout << "Opening Basler GigE camera: " << opt_camera << std::endl;
+ } else if (opt_device == "USB" || opt_device == "usb") {
+ g = factory.createPylonGrabber(vpPylonFactory::BASLER_USB);
+ std::cout << "Opening Basler USB camera: " << opt_camera << std::endl;
+ } else {
+ std::cout << "Error: only Basler GigE or USB cameras are supported." << std::endl;
+ return EXIT_SUCCESS;
+ }
+ g->setCameraIndex(opt_camera);
+
+ g->open(I);
+
+ std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl;
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d(I);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I);
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d(I);
+#else
+ std::cout << "No image viewer is available..." << std::endl;
+#endif
+
+ while (1) {
+ g->acquire(I);
+ vpDisplay::display(I);
+ vpDisplay::displayText(I, 10, 10, "A click to quit", vpColor::red);
+ vpDisplay::flush(I);
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+ } catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+#else
+ (void)argc;
+ (void)argv;
+#endif
+}
diff --git a/tutorial/grabber/tutorial-grabber-flycapture.cpp b/tutorial/grabber/tutorial-grabber-flycapture.cpp
index 091a2bb..9e38edd 100644
--- a/tutorial/grabber/tutorial-grabber-flycapture.cpp
+++ b/tutorial/grabber/tutorial-grabber-flycapture.cpp
@@ -1,8 +1,8 @@
//! \example tutorial-grabber-flycapture.cpp
-#include <visp3/sensor/vpFlyCaptureGrabber.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpFlyCaptureGrabber.h>
int main()
{
@@ -18,8 +18,8 @@ int main()
g.setShutter(true); // Turn auto shutter on
g.setGain(true); // Turn auto gain on
g.setVideoModeAndFrameRate(FlyCapture2::VIDEOMODE_1280x960Y8, FlyCapture2::FRAMERATE_60);
- }
- catch(...) { // If settings are not available just catch execption to continue with default settings
+ } catch (...) { // If settings are not available just catch execption to
+ // continue with default settings
}
//! [vpFlyCaptureGrabber settings]
//! [vpFlyCaptureGrabber open]
@@ -36,7 +36,7 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
- while(1) {
+ while (1) {
//! [vpFlyCaptureGrabber acquire]
g.acquire(I);
//! [vpFlyCaptureGrabber acquire]
@@ -50,8 +50,7 @@ int main()
//! [vpFlyCaptureGrabber click to exit]
vpDisplay::flush(I);
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e.getStringMessage() << std::endl;
}
#endif
diff --git a/tutorial/grabber/tutorial-grabber-opencv-threaded.cpp b/tutorial/grabber/tutorial-grabber-opencv-threaded.cpp
index 59330a7..6949c2c 100644
--- a/tutorial/grabber/tutorial-grabber-opencv-threaded.cpp
+++ b/tutorial/grabber/tutorial-grabber-opencv-threaded.cpp
@@ -6,19 +6,15 @@
#include <visp3/core/vpMutex.h>
#include <visp3/core/vpThread.h>
#include <visp3/core/vpTime.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
#if (VISP_HAVE_OPENCV_VERSION >= 0x020100) && (defined(VISP_HAVE_PTHREAD) || defined(_WIN32))
#include <opencv2/highgui/highgui.hpp>
// 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;
cv::Mat s_frame;
vpMutex s_mutex_capture;
@@ -27,16 +23,17 @@ vpMutex s_mutex_capture;
//! [capture-multi-threaded captureFunction]
vpThread::Return captureFunction(vpThread::Args args)
{
- cv::VideoCapture cap = *((cv::VideoCapture *) args);
+ cv::VideoCapture cap = *((cv::VideoCapture *)args);
- if(!cap.isOpened()) { // check if we succeeded
+ if (!cap.isOpened()) { // check if we succeeded
std::cout << "Unable to start capture" << std::endl;
return 0;
}
cv::Mat frame_;
- int i=0;
- while ((i++ < 100) && !cap.read(frame_)) {}; // warm up camera by skiping unread frames
+ int i = 0;
+ while ((i++ < 100) && !cap.read(frame_)) {
+ }; // warm up camera by skiping unread frames
bool stop_capture_ = false;
@@ -87,15 +84,16 @@ vpThread::Return displayFunction(vpThread::Args args)
// Check if a frame is available
if (capture_state_ == capture_started) {
- // Get the frame and convert it to a ViSP image used by the display class
+ // Get the frame and convert it to a ViSP image used by the display
+ // class
{
vpMutex::vpScopedLock lock(s_mutex_capture);
vpImageConvert::convert(s_frame, I_);
}
// Check if we need to initialize the display with the first frame
- if (! display_initialized_) {
- // Initialize the display
+ if (!display_initialized_) {
+// Initialize the display
#if defined(VISP_HAVE_X11)
d_ = new vpDisplayX(I_);
display_initialized_ = true;
@@ -117,11 +115,10 @@ vpThread::Return displayFunction(vpThread::Args args)
// Update the display
vpDisplay::flush(I_);
- }
- else {
+ } else {
vpTime::wait(2); // Sleep 2ms
}
- } while(capture_state_ != capture_stopped);
+ } while (capture_state_ != capture_stopped);
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)
delete d_;
@@ -133,14 +130,14 @@ vpThread::Return displayFunction(vpThread::Args args)
//! [capture-multi-threaded displayFunction]
//! [capture-multi-threaded mainFunction]
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
int opt_device = 0;
// Command line options
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--device")
- opt_device = atoi(argv[i+1]);
+ opt_device = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << " [--device <camera device>] [--help]" << std::endl;
return 0;
@@ -166,13 +163,13 @@ int main(int argc, const char* argv[])
#else
int main()
{
-# ifndef VISP_HAVE_OPENCV
+#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
+#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
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
}
#endif
diff --git a/tutorial/grabber/tutorial-grabber-opencv.cpp b/tutorial/grabber/tutorial-grabber-opencv.cpp
index fc00350..b96c57c 100644
--- a/tutorial/grabber/tutorial-grabber-opencv.cpp
+++ b/tutorial/grabber/tutorial-grabber-opencv.cpp
@@ -1,12 +1,12 @@
/*! \example tutorial-grabber-opencv.cpp */
#include <stdlib.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
// usage: binary <device name>
// device name: 0 is the default to dial with the first camera,
// 1 to dial with a second camera attached to the computer
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
int device = 0;
if (argc > 1)
@@ -16,23 +16,24 @@ int main(int argc, char** argv)
#if (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
cv::VideoCapture cap(device); // open the default camera
- if(!cap.isOpened()) { // check if we succeeded
+ if (!cap.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
cv::Mat frame;
- int i=0;
- while ((i++ < 100) && !cap.read(frame)) {}; // warm up camera by skiping unread frames
+ int i = 0;
+ while ((i++ < 100) && !cap.read(frame)) {
+ }; // warm up camera by skiping unread frames
std::cout << "Image size: " << frame.rows << " " << frame.cols << std::endl;
- //vpImage<vpRGBa> I; // for color images
+ // vpImage<vpRGBa> I; // for color images
vpImage<unsigned char> I; // for gray images
vpImageConvert::convert(frame, I);
vpDisplayOpenCV d(I);
- for(;;) {
+ for (;;) {
cap >> frame; // get a new frame from camera
// Convert the image in ViSP format and display it
vpImageConvert::convert(frame, I);
@@ -41,10 +42,8 @@ int main(int argc, char** argv)
if (vpDisplay::getClick(I, false)) // a click to exit
break;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
-
diff --git a/tutorial/grabber/tutorial-grabber-realsense.cpp b/tutorial/grabber/tutorial-grabber-realsense.cpp
new file mode 100644
index 0000000..395b0c6
--- /dev/null
+++ b/tutorial/grabber/tutorial-grabber-realsense.cpp
@@ -0,0 +1,49 @@
+/*! \example tutorial-grabber-realsense.cpp */
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpRealSense.h>
+
+/*!
+ Grab images from an Intel realsense camera
+ */
+int main()
+{
+#ifdef VISP_HAVE_REALSENSE
+ try {
+ vpImage<unsigned char> I;
+
+ vpRealSense g;
+ unsigned int width = 640, height = 480;
+ g.setStreamSettings(rs::stream::color, vpRealSense::vpRsStreamParams(width, height, rs::format::rgba8, 60));
+ g.open();
+ g.acquire(I);
+
+ std::cout << "Image size: " << I.getWidth() << " " << I.getHeight() << std::endl;
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d(I);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I);
+#else
+ std::cout << "No image viewer is available..." << std::endl;
+#endif
+
+ while (1) {
+ double t = vpTime::measureTimeMs();
+ g.acquire(I);
+ vpDisplay::display(I);
+ vpDisplay::displayText(I, 10, 10, "A click to quit", vpColor::red);
+ std::stringstream ss;
+ ss << "Acquisition time: " << vpTime::measureTimeMs() - t << " ms";
+ vpDisplay::displayText(I, 40, 20, ss.str(), vpColor::red);
+
+ vpDisplay::flush(I);
+ if (vpDisplay::getClick(I, false))
+ break;
+ }
+ } catch (vpException &e) {
+ std::cout << "Catch an exception: " << e << std::endl;
+ }
+#endif
+}
diff --git a/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp b/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp
index 4b0f257..eca9269 100644
--- a/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp
+++ b/tutorial/grabber/tutorial-grabber-v4l2-threaded.cpp
@@ -12,11 +12,7 @@
#if defined(VISP_HAVE_V4L2) && defined(VISP_HAVE_PTHREAD)
// 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;
vpImage<unsigned char> s_frame;
vpMutex s_mutex_capture;
@@ -25,7 +21,7 @@ vpMutex s_mutex_capture;
//! [capture-multi-threaded captureFunction]
vpThread::Return captureFunction(vpThread::Args args)
{
- vpV4l2Grabber cap = *((vpV4l2Grabber *) args);
+ vpV4l2Grabber cap = *(static_cast<vpV4l2Grabber *>(args));
vpImage<unsigned char> frame_;
bool stop_capture_ = false;
@@ -82,8 +78,8 @@ vpThread::Return displayFunction(vpThread::Args args)
}
// Check if we need to initialize the display with the first frame
- if (! display_initialized_) {
- // Initialize the display
+ if (!display_initialized_) {
+// Initialize the display
#if defined(VISP_HAVE_X11)
d_ = new vpDisplayX(I_);
display_initialized_ = true;
@@ -102,11 +98,10 @@ vpThread::Return displayFunction(vpThread::Args args)
// Update the display
vpDisplay::flush(I_);
- }
- else {
+ } else {
vpTime::wait(2); // Sleep 2ms
}
- } while(capture_state_ != capture_stopped);
+ } while (capture_state_ != capture_stopped);
#if defined(VISP_HAVE_X11)
delete d_;
@@ -118,19 +113,23 @@ vpThread::Return displayFunction(vpThread::Args args)
//! [capture-multi-threaded displayFunction]
//! [capture-multi-threaded mainFunction]
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
unsigned int opt_device = 0; // Default is opening /dev/video0
- unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn it to 1 to avoid subsampling
+ unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn
+ // it to 1 to avoid subsampling
// Command line options
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--device")
- opt_device = (unsigned int)atoi(argv[i+1]);
+ opt_device = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--scale")
- opt_scale = (unsigned int)atoi(argv[i+1]);
+ opt_scale = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "Usage: " << argv[0] << " [--device <camera device>] [--scale <subsampling factor>] [--help]" << std::endl;
+ std::cout << "Usage: " << argv[0]
+ << " [--device <camera device>] [--scale <subsampling "
+ "factor>] [--help]"
+ << std::endl;
return 0;
}
}
@@ -157,13 +156,13 @@ int main(int argc, const char* argv[])
#else
int main()
{
-# ifndef VISP_HAVE_V4L2
+#ifndef VISP_HAVE_V4L2
std::cout << "You should enable V4L2 to make this example working..." << std::endl;
-# elif !defined(_WIN32) && (defined(__unix__) || defined(__unix) || (defined(__APPLE__) && defined(__MACH__))) // UNIX
+#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
+#else
std::cout << "Multi-threading seems not supported on this platform" << std::endl;
-# endif
+#endif
}
#endif
diff --git a/tutorial/grabber/tutorial-grabber-v4l2.cpp b/tutorial/grabber/tutorial-grabber-v4l2.cpp
index ed397ec..723710d 100644
--- a/tutorial/grabber/tutorial-grabber-v4l2.cpp
+++ b/tutorial/grabber/tutorial-grabber-v4l2.cpp
@@ -1,28 +1,32 @@
/*! \example tutorial-grabber-v4l2.cpp */
-#include <visp3/sensor/vpV4l2Grabber.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/sensor/vpV4l2Grabber.h>
/*!
Usage :
To get the help : ./tutorial-grabber-v4l2 --help
- To set the device : ./tutorial-grabber-v4l2 --device 1 (to use /dev/video1)
- To subsample images: ./tutorial-grabber-v4l2 --scale 2
+ To set the device : ./tutorial-grabber-v4l2 --device 1 (to use
+ /dev/video1) To subsample images: ./tutorial-grabber-v4l2 --scale 2
*/
-int main(int argc, const char* argv[])
+int main(int argc, const char *argv[])
{
#ifdef VISP_HAVE_V4L2
try {
unsigned int opt_device = 0;
- unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn it to 1 to avoid subsampling
+ unsigned int opt_scale = 2; // Default value is 2 in the constructor. Turn
+ // it to 1 to avoid subsampling
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--device")
- opt_device = (unsigned int)atoi(argv[i+1]);
+ opt_device = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--scale")
- opt_scale = (unsigned int)atoi(argv[i+1]);
+ opt_scale = (unsigned int)atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "Usage: " << argv[0] << " [--device <camera device>] [--scale <subsampling factor>] [--help]" << std::endl;
+ std::cout << "Usage: " << argv[0]
+ << " [--device <camera device>] [--scale <subsampling "
+ "factor>] [--help]"
+ << std::endl;
return 0;
}
}
@@ -44,15 +48,15 @@ int main(int argc, const char* argv[])
std::cout << "No image viewer is available..." << std::endl;
#endif
- while(1) {
+ while (1) {
g.acquire(I);
vpDisplay::display(I);
vpDisplay::displayText(I, 10, 10, "A click to quit", vpColor::red);
vpDisplay::flush(I);
- if (vpDisplay::getClick(I, false)) break;
+ if (vpDisplay::getClick(I, false))
+ break;
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/grabber/tutorial-video-reader.cpp b/tutorial/grabber/tutorial-video-reader.cpp
index 4a0fa22..346ccd2 100644
--- a/tutorial/grabber/tutorial-video-reader.cpp
+++ b/tutorial/grabber/tutorial-video-reader.cpp
@@ -9,19 +9,19 @@
/*!
This example allows to read and display a video from a file.
- It only requires that ViSP is build with OpenCV or ffmpeg.
+ It only requires that ViSP is build with OpenCV.
Example: ./tutorial-video-reader --name video.mpg
*/
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG)
+#if (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string videoname = "video.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -53,7 +53,7 @@ int main(int argc, char** argv)
#endif
vpDisplay::setTitle(I, "Video reader");
//! [vpVideoReader while loop]
- while (! g.end() ) {
+ while (!g.end()) {
//! [vpVideoReader while loop]
//! [vpVideoReader loop start time]
double t = vpTime::measureTimeMs();
@@ -63,18 +63,18 @@ int main(int argc, char** argv)
//! [vpVideoReader acquire]
vpDisplay::display(I);
vpDisplay::flush(I);
- if (vpDisplay::getClick(I, false)) break;
+ if (vpDisplay::getClick(I, false))
+ break;
//! [vpVideoReader loop rate]
vpTime::wait(t, 1000. / g.getFramerate());
//! [vpVideoReader loop rate]
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
}
#else
(void)argc;
(void)argv;
- std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
#endif
}
diff --git a/tutorial/grabber/tutorial-video-recorder.cpp b/tutorial/grabber/tutorial-video-recorder.cpp
index 9c41ed1..cb7a35c 100644
--- a/tutorial/grabber/tutorial-video-recorder.cpp
+++ b/tutorial/grabber/tutorial-video-recorder.cpp
@@ -1,9 +1,9 @@
/*! \example tutorial-video-recorder.cpp */
+#include <visp3/core/vpTime.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/core/vpTime.h>
#include <visp3/io/vpVideoWriter.h>
#include <visp3/sensor/vpV4l2Grabber.h>
@@ -11,21 +11,34 @@
This example allows to record a video from a camera.
It only requires that ViSP is build with OpenCV.
- Example: ./tutorial-video-recorder --device 0 --name myvideo.mpeg
+ Example to save an mpeg video:
+
+ ./tutorial-video-recorder --device 0 --name myvideo.mpeg
+
+ Example to save a sequence of png images:
+
+ ./tutorial-video-recorder --device 0 --name image%04d.png
+
+ Example to save one imags:
+
+ ./tutorial-video-recorder --device 0 --name image.jpeg
+
*/
int main(int argc, const char *argv[])
{
-#if ((defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)))
+#if ((defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)))
std::string opt_videoname = "video-recorded.mpg";
int opt_device = 0;
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--device")
- opt_device = atoi(argv[i+1]);
+ opt_device = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--name")
- opt_videoname = std::string(argv[i+1]);
+ opt_videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "\nUsage: " << argv[0] << " [--device <device number>] [--name <video name>] [--help]\n" << std::endl;
+ std::cout << "\nUsage: " << argv[0] << " [--device <device number>] [--name <video name>] [--help]\n"
+ << std::endl;
return 0;
}
}
@@ -34,7 +47,7 @@ int main(int argc, const char *argv[])
std::cout << "Record video in: " << opt_videoname << std::endl;
try {
- //vpImage<vpRGBa> I; // for color images
+ // vpImage<vpRGBa> I; // for color images
vpImage<unsigned char> I; // for gray images
#if defined(VISP_HAVE_V4L2)
@@ -42,11 +55,12 @@ int main(int argc, const char *argv[])
std::ostringstream device;
device << "/dev/video" << opt_device;
g.setDevice(device.str());
+ g.setScale(1); // Acquire full resolution images
g.open(I);
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(opt_device);
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -69,25 +83,16 @@ int main(int argc, const char *argv[])
d.init(I, 0, 0, "Camera view");
vpVideoWriter writer;
-#ifdef VISP_HAVE_FFMPEG
- // Set up the bit rate
- writer.setBitRate(1000000);
- // Set up the codec to use
-# if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(54,51,110) // libavcodec 54.51.100
- writer.setCodec(CODEC_ID_MPEG2VIDEO);
-# else
- writer.setCodec(AV_CODEC_ID_MPEG2VIDEO);
-# endif
-#elif VISP_HAVE_OPENCV_VERSION >= 0x030000
- writer.setCodec( cv::VideoWriter::fourcc('P','I','M','1') ); // MPEG-1 codec
+#if VISP_HAVE_OPENCV_VERSION >= 0x030000
+ writer.setCodec(cv::VideoWriter::fourcc('P', 'I', 'M', '1')); // MPEG-1 codec
#elif VISP_HAVE_OPENCV_VERSION >= 0x020100
- writer.setCodec( CV_FOURCC('P','I','M','1') );
+ writer.setCodec(CV_FOURCC('P', 'I', 'M', '1'));
#endif
writer.setFileName(opt_videoname);
writer.open(I);
bool recording = false;
- for(;;) {
+ for (;;) {
#if defined(VISP_HAVE_V4L2)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -99,8 +104,7 @@ int main(int argc, const char *argv[])
vpDisplay::displayText(I, 10, 10, "A click to start recording", vpColor::green);
if (vpDisplay::getClick(I, false))
recording = true;
- }
- else {
+ } else {
writer.saveFrame(I);
vpDisplay::displayText(I, 10, 10, "Recording: A click to stop and exit", vpColor::red);
if (vpDisplay::getClick(I, false))
@@ -110,8 +114,7 @@ int main(int argc, const char *argv[])
vpDisplay::flush(I);
}
std::cout << "The video was recorded in \"" << opt_videoname << "\"" << std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/image/tutorial-image-converter.cpp b/tutorial/image/tutorial-image-converter.cpp
index e0ae4d5..7d61795 100644
--- a/tutorial/image/tutorial-image-converter.cpp
+++ b/tutorial/image/tutorial-image-converter.cpp
@@ -1,6 +1,6 @@
/*! \example tutorial-image-converter.cpp */
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageConvert.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -16,11 +16,10 @@ int main()
vpImage<unsigned char> I;
vpImageConvert::convert(A, I);
-# ifdef VISP_HAVE_PNG
+#ifdef VISP_HAVE_PNG
vpImageIo::write(I, "monkey.png"); // Gray
-# endif
- }
- catch(vpException &e) {
+#endif
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/image/tutorial-image-display-scaled-auto.cpp b/tutorial/image/tutorial-image-display-scaled-auto.cpp
index e769374..25ac18a 100644
--- a/tutorial/image/tutorial-image-display-scaled-auto.cpp
+++ b/tutorial/image/tutorial-image-display-scaled-auto.cpp
@@ -7,7 +7,7 @@ int main()
vpImage<unsigned char> I(2160, 3840, 128);
try {
- //! [vpDisplay scale auto]
+//! [vpDisplay scale auto]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, vpDisplay::SCALE_AUTO);
#elif defined(VISP_HAVE_GDI)
@@ -17,12 +17,11 @@ int main()
vpDisplay::setTitle(I, "My image");
vpDisplay::display(I);
- vpDisplay::displayCircle(I, I.getHeight()/2, I.getWidth()/2, 200, vpColor::red, true);
+ vpDisplay::displayCircle(I, I.getHeight() / 2, I.getWidth() / 2, 200, vpColor::red, true);
vpDisplay::flush(I);
std::cout << "A click to quit..." << std::endl;
vpDisplay::getClick(I);
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
}
diff --git a/tutorial/image/tutorial-image-display-scaled-manu.cpp b/tutorial/image/tutorial-image-display-scaled-manu.cpp
index f93b8b2..cb02b1f 100644
--- a/tutorial/image/tutorial-image-display-scaled-manu.cpp
+++ b/tutorial/image/tutorial-image-display-scaled-manu.cpp
@@ -7,7 +7,7 @@ int main()
vpImage<unsigned char> I(2160, 3840, 128);
try {
- //! [vpDisplay scale manu]
+//! [vpDisplay scale manu]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, vpDisplay::SCALE_5);
#elif defined(VISP_HAVE_GDI)
@@ -16,12 +16,11 @@ int main()
//! [vpDisplay scale manu]
vpDisplay::setTitle(I, "My image");
vpDisplay::display(I);
- vpDisplay::displayCircle(I, I.getHeight()/2, I.getWidth()/2, 200, vpColor::red, true);
+ vpDisplay::displayCircle(I, I.getHeight() / 2, I.getWidth() / 2, 200, vpColor::red, true);
vpDisplay::flush(I);
std::cout << "A click to quit..." << std::endl;
vpDisplay::getClick(I);
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
}
diff --git a/tutorial/image/tutorial-image-display.cpp b/tutorial/image/tutorial-image-display.cpp
index 9e948c5..55b5c8f 100644
--- a/tutorial/image/tutorial-image-display.cpp
+++ b/tutorial/image/tutorial-image-display.cpp
@@ -15,12 +15,11 @@ int main()
vpDisplay::setTitle(I, "My image");
vpDisplay::display(I);
- vpDisplay::displayCircle(I, I.getHeight()/2, I.getWidth()/2, 200, vpColor::red, true);
+ vpDisplay::displayCircle(I, I.getHeight() / 2, I.getWidth() / 2, 200, vpColor::red, true);
vpDisplay::flush(I);
std::cout << "A click to quit..." << std::endl;
vpDisplay::getClick(I);
- }
- catch(const vpException &e) {
+ } catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
}
diff --git a/tutorial/image/tutorial-image-filter.cpp b/tutorial/image/tutorial-image-filter.cpp
index 8e01aeb..41cb88a 100644
--- a/tutorial/image/tutorial-image-filter.cpp
+++ b/tutorial/image/tutorial-image-filter.cpp
@@ -1,12 +1,12 @@
//! \example tutorial-image-filter.cpp
+#include <visp3/core/vpImageFilter.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpImageFilter.h>
void display(vpImage<unsigned char> &I, const std::string &title);
void display(vpImage<double> &D, const std::string &title);
@@ -29,7 +29,7 @@ void display(vpImage<unsigned char> &I, const std::string &title)
vpDisplay::setTitle(I, title.c_str());
vpDisplay::display(I);
- vpDisplay::displayText(I, 15,15, "Click to continue...", vpColor::red);
+ vpDisplay::displayText(I, 15, 15, "Click to continue...", vpColor::red);
vpDisplay::flush(I);
vpDisplay::getClick(I);
}
@@ -41,11 +41,11 @@ void display(vpImage<double> &D, const std::string &title)
display(I, title);
}
-int main(int argc, char** argv )
+int main(int argc, char **argv)
{
try {
- if(argc != 2) {
- printf( "Usage: %s <image name.[pgm,ppm,jpeg,png,bmp]>\n", argv[0] );
+ if (argc != 2) {
+ printf("Usage: %s <image name.[pgm,ppm,jpeg,png,bmp]>\n", argv[0]);
return -1;
}
//! [vpImage construction]
@@ -54,8 +54,7 @@ int main(int argc, char** argv )
try {
vpImageIo::read(I, argv[1]);
- }
- catch(...) {
+ } catch (...) {
std::cout << "Cannot read image \"" << argv[1] << "\"" << std::endl;
return -1;
}
@@ -83,7 +82,7 @@ int main(int argc, char** argv )
//! [Gradients y]
display(dIy, "Gradient dIy");
- //! [Canny]
+//! [Canny]
#if (VISP_HAVE_OPENCV_VERSION >= 0x020100)
vpImage<unsigned char> C;
vpImageFilter::canny(I, C, 5, 15, 3);
@@ -92,10 +91,16 @@ int main(int argc, char** argv )
//! [Canny]
//! [Convolution kernel]
- vpMatrix K(3,3); // Sobel kernel along x
- K[0][0] = 1; K[0][1] = 0; K[0][2] = -1;
- K[1][0] = 2; K[1][1] = 0; K[1][2] = -2;
- K[2][0] = 1; K[2][1] = 0; K[2][2] = -1;
+ vpMatrix K(3, 3); // Sobel kernel along x
+ K[0][0] = 1;
+ K[0][1] = 0;
+ K[0][2] = -1;
+ K[1][0] = 2;
+ K[1][1] = 0;
+ K[1][2] = -2;
+ K[2][0] = 1;
+ K[2][1] = 0;
+ K[2][2] = -1;
//! [Convolution kernel]
//! [Convolution]
vpImage<double> Gx;
@@ -105,16 +110,15 @@ int main(int argc, char** argv )
//! [Gaussian pyramid]
size_t nlevel = 3;
- std::vector< vpImage<unsigned char> > pyr(nlevel);
+ std::vector<vpImage<unsigned char> > pyr(nlevel);
pyr[0] = I;
- for (size_t i=1; i < nlevel; i++) {
- vpImageFilter::getGaussPyramidal(pyr[i-1], pyr[i]);
+ for (size_t i = 1; i < nlevel; i++) {
+ vpImageFilter::getGaussPyramidal(pyr[i - 1], pyr[i]);
display(pyr[i], "Pyramid");
}
//! [Gaussian pyramid]
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
return 1;
}
diff --git a/tutorial/image/tutorial-image-manipulation.cpp b/tutorial/image/tutorial-image-manipulation.cpp
index fdd64f7..981569b 100644
--- a/tutorial/image/tutorial-image-manipulation.cpp
+++ b/tutorial/image/tutorial-image-manipulation.cpp
@@ -13,18 +13,14 @@ int main()
unsigned int igray_max = gray_image.getHeight() - 1;
unsigned int jgray_max = gray_image.getWidth() - 1;
- std::cout << "Gray image, last pixel intensity: "
- << (int)gray_image[igray_max][jgray_max] << std::endl;
+ std::cout << "Gray image, last pixel intensity: " << (int)gray_image[igray_max][jgray_max] << std::endl;
unsigned int icolor_max = color_image.getHeight() - 1;
unsigned int jcolor_max = color_image.getWidth() - 1;
- std::cout << "Color image, last pixel RGB components: "
- << (int)color_image[icolor_max][jcolor_max].R << " "
- << (int)color_image[icolor_max][jcolor_max].G << " "
- << (int)color_image[icolor_max][jcolor_max].B
+ std::cout << "Color image, last pixel RGB components: " << (int)color_image[icolor_max][jcolor_max].R << " "
+ << (int)color_image[icolor_max][jcolor_max].G << " " << (int)color_image[icolor_max][jcolor_max].B
<< std::endl;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/image/tutorial-image-reader.cpp b/tutorial/image/tutorial-image-reader.cpp
index 44c2aba..7b5a0db 100644
--- a/tutorial/image/tutorial-image-reader.cpp
+++ b/tutorial/image/tutorial-image-reader.cpp
@@ -7,12 +7,9 @@ int main()
vpImage<vpRGBa> I;
vpImageIo::read(I, "monkey.jpeg");
vpImageIo::write(I, "monkey.png");
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << e.getMessage() << std::endl;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Unsupported image format" << std::endl;
}
}
-
diff --git a/tutorial/image/tutorial-image-viewer.cpp b/tutorial/image/tutorial-image-viewer.cpp
index 7ecbe04..fa559ac 100644
--- a/tutorial/image/tutorial-image-viewer.cpp
+++ b/tutorial/image/tutorial-image-viewer.cpp
@@ -1,9 +1,9 @@
/*! \example tutorial-image-viewer.cpp */
+#include <visp3/core/vpImagePoint.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpImagePoint.h>
int main()
{
@@ -23,7 +23,7 @@ int main()
vpDisplay::setTitle(I, "Monkey");
vpDisplay::display(I);
- vpDisplay::displayRectangle(I, vpImagePoint(90,90), 70, 90, vpColor::red, false, 2);
+ vpDisplay::displayRectangle(I, vpImagePoint(90, 90), 70, 90, vpColor::red, false, 2);
vpDisplay::flush(I);
vpImage<vpRGBa> O;
@@ -32,14 +32,12 @@ int main()
try {
vpImageIo::write(I, "monkey-out.jpg");
vpImageIo::write(O, "monkey-out-with-overlay.jpg");
- }
- catch(...) {
+ } catch (...) {
std::cout << "Cannot write the image: unsupported format..." << std::endl;
}
vpDisplay::getClick(I);
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/image/tutorial-undistort.cpp b/tutorial/image/tutorial-undistort.cpp
index c25a37a..e378d53 100644
--- a/tutorial/image/tutorial-undistort.cpp
+++ b/tutorial/image/tutorial-undistort.cpp
@@ -1,7 +1,7 @@
//! \example tutorial-undistort.cpp
-#include <visp3/io/vpImageIo.h>
#include <visp3/core/vpImageTools.h>
#include <visp3/core/vpXmlParserCamera.h>
+#include <visp3/io/vpImageIo.h>
int main()
{
@@ -17,11 +17,12 @@ int main()
vpXmlParserCamera p;
vpCameraParameters::vpCameraParametersProjType projModel;
projModel = vpCameraParameters::perspectiveProjWithDistortion;
- if (p.parse(cam, "camera.xml", "Camera", projModel, I.getWidth(), I.getHeight()) != vpXmlParserCamera::SEQUENCE_OK) {
+ if (p.parse(cam, "camera.xml", "Camera", projModel, I.getWidth(), I.getHeight()) !=
+ vpXmlParserCamera::SEQUENCE_OK) {
std::cout << "Cannot found parameters for camera named \"Camera\"" << std::endl;
}
- //! [Load camera parameters from xml]
- //! [Set camera parameters]
+//! [Load camera parameters from xml]
+//! [Set camera parameters]
#else
cam.initPersProjWithDistortion(582.7, 580.6, 326.6, 215.0, -0.3372, 0.4021);
#endif
@@ -34,8 +35,7 @@ int main()
vpImageTools::undistort(I, cam, Iud);
vpImageIo::write(Iud, "chessboard-undistort.pgm");
//! [Create image without distorsion]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
diff --git a/tutorial/image/tutorial-viewer.cpp b/tutorial/image/tutorial-viewer.cpp
index cf81c3a..e039982 100644
--- a/tutorial/image/tutorial-viewer.cpp
+++ b/tutorial/image/tutorial-viewer.cpp
@@ -3,17 +3,17 @@
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
//! [Include display]
//! [Include io]
#include <visp3/io/vpImageIo.h>
//! [Include io]
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
- if(argc != 2) {
- printf( "Usage: %s <image name.[pgm,ppm,jpeg,png,tiff,bmp,ras,jp2]>\n", argv[0] );
+ if (argc != 2) {
+ printf("Usage: %s <image name.[pgm,ppm,jpeg,png,tiff,bmp,ras,jp2]>\n", argv[0]);
return -1;
}
@@ -24,15 +24,14 @@ int main(int argc, char** argv)
//! [vpImage reading]
try {
vpImageIo::read(I, argv[1]);
- }
- catch(...) {
+ } catch (...) {
std::cout << "Cannot read image \"" << argv[1] << "\"" << std::endl;
return -1;
}
//! [vpImage reading]
try {
- //! [vpDisplay construction]
+//! [vpDisplay construction]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, vpDisplay::SCALE_AUTO);
#elif defined(VISP_HAVE_GDI)
@@ -59,8 +58,7 @@ int main(int argc, char** argv)
//! [vpDisplay get click]
vpDisplay::getClick(I);
//! [vpDisplay get click]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/imgproc/autothreshold/CMakeLists.txt b/tutorial/imgproc/autothreshold/CMakeLists.txt
new file mode 100644
index 0000000..d6bf147
--- /dev/null
+++ b/tutorial/imgproc/autothreshold/CMakeLists.txt
@@ -0,0 +1,23 @@
+project(tutorial-autothreshold)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-autothreshold.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/grid36-03.pgm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-autothreshold.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/autothreshold/grid36-03.pgm b/tutorial/imgproc/autothreshold/grid36-03.pgm
new file mode 100644
index 0000000..65b45f8
--- /dev/null
+++ b/tutorial/imgproc/autothreshold/grid36-03.pgm
@@ -0,0 +1,184 @@
+P5
+640 480
+255
+������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
#&%"''$!" Jv������������������������������������������������oO6
,=@`v���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
3^����������������������������������������������^9
+@au�����������������������������������������������������~{yjkrghs����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+ )Uj������������������������������������������������������tfYSV`hlr{��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}G [...]
+/$-1*%)%#/,
,0 %.3 4Q�����������������������������������������d*#-*##&# )$'*%
;`�����������������������������������������������}Z3
%Ih����������������������������������������������������������~|~|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������B &*#!&& [...]
*Y���������������������������������������g"$(((((((((%%%(& a{��������������������������������������������iT'
$Pk�����������������������������������������������������uc[TNEBCNYbq������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+&( #+&&&&####!$$$$$$$%%''$
D���������������������������������P.$))((&&%%&&&&&&&&$&)))$"
+H}�����������������������������������z$'"$((((((((((((++(#
!p�����������������������������������������L )1/'%%%"####&
-h�����������������������������������������������qW3&
"-To�����������������������������������������������������������~zz}�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z5
$2)&&&'$#### %%%%%%%!$$'&&&&!o�������������������������������:6#))('''&&&&&&&&&&$&&)))))
L����������������������������������G++#$.&&&&&&&&'$$$&)+)'-:h���������������������������������������\
%)(&##**++++,,71$%P��������������������������������������������a?&
+
!
5p�����������������������������������������������������}rc\XUQPPS]m}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������O %!!&) $$$$#### %%%%%%%!!$$&&)++-h�����������������������������n2'#
))''''##$$$&&&&&)&&$$$$$7
#e�������������������������������� [...]
+,...,*))-,)***((
"h�����������������������������������������������~eH."
%Qg������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������+0$ &()&$$$$$$$$ $$$%%%%$$!!$&&(,3����������������������������~&&(!((%&&'$$$$$$$$&&&))&$""$$ 0*f����������������������������� [...]
+.P���������������������������������������������rX/
+
_t���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������y"$&&#!$)$$$$$""" $$$%%%%$$$!$$#&*/
&T���������������������������E.
+/(%%%&&'$$$"$$$$&&&&&&$''&)6,
*(������������������������������2 1)
"''"
&*''''''''!$(((('(.*2!!>���������������������������������j *
6&2+&&%%!((**))))-+(&&&'+
>{������������������������������������N/
(0//'$$$ #$&%%% #''#. >9y����������������������������������������_2$''-3)23)((-/++*01,"Qm��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
: -01(!!'0&&&&&&&&
"&&'$$&$&,(#b�������������������������������n1)$%,/'! $)02))&&''$$
#'$
%*
S�����������������������������������i&$-)##+(%#%%$$#')'$(+(#7&=����������������������������������������[7 ''$%#
"
$
&)%3�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+S����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������u# &'''$$$$'''$$$$$$!!!!!##&##$$$$$$,&:-�������������������������F
#%%$!$$$$####$$$$$$$&'$$$$&&&(*#(.G��������������������������R (&,((((''%%))&&&&$$&&&&&&&&!$(()&%
=�����������������������������d!''')'&&$$$$&&&&$$$$&&# [...]
,n�������������������������������o$((%%%%&#$$$$########$$$$$$$'&
P�����������������������������������s7$$$$&&''%%####%%&&&&&&&&%
"
y���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������l#%''$$$$$$'$$$$$$$$!!!!#####$$$$$$,&0j�����������������������f!#%''%'$$$$$####$$$$$$$&$ [...]
+))&$$$$$%%####$$$$$$$$$&$$$$$&&& )(%(&X������������������������c,$ 1##$$$$##$$$$$$$$$$&&&&$$$$$$$&))$$i�������������������������i$
'&&&%%((&&$$$$&&&&$$$$&&$$''%%%%/"#*Y���������������������������~X& 3!%%%%####$$$$$$$$######$$)$!!$''$'`������������������������������L-'..&&####$$$$$$$$$$$$$$$$$$(!!$'"Z�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V*&'$$$!$$$$$!!!!!!$$$##############(( .#Y������������� [...]
+Q������������������������~H
!!##$'((&&$$$$&&&&$$$$&&$$$'%%%#5
,
:v��������������������������U
#
0&&&&$$$$##############$$$$####!!16�����������������������������z
3" +!!$$$$$$##$$$$##########"%((! f������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������[(($$$"!!$$$$!!!!!!$$$#### [...]
+b���������������������������Z&%
$3-!!"$$$$$$$####$$$$$$$$$$/(
$*
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������n! &### # !!!!!$####$$$$$$+0 (04����������������������'''&&&$$#%%##$$##$$$$$$$&$$$$$&&&3
&2%",&I����������������������V
+0''(%%&&$$##$$&&&&))$$$$$$$$%'&&$$"",(s�����������������������*3"###&&&'$&&$$$$&&&&$$$$&&!$$$$$$$+"' +) &������������������������z?8
%)&''$$$$!!########$$$$$$$$!!$$$$&&
.J�������������������������w3C%0+'##$$""""$$####$$#########
),'$&g���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
D������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������L$
&&$&&&&&%%##
$$$$$$$$"$$$$$$$$/###.4����������������������G)'##%%'$%%$$$$$$###########%%%%%$&%%$$*.4����������������������E
++)$&)'''&#$$######$$$$$$$$######$$#&%(!+n����������������������w,/"%%((((&&&&$$$$!!$$$$$$$$####$$#&$!&!(#{�����������������������T#*$'"+#$$$$$$$$$$$$$$$$####$$$$$$$$####
#(
c������������������������`$*('$%%'$''####$$$$$$$$$$$$!!$$&&$$$$####/v�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+-,&%))''$#$$$$####$$$$$$$$######$$#'%'$) c����������������������f)/'%%((&&%%$$$$$$$$$$$$$$$$####$$#&#!&#(#
r�����������������������>+()") $$$$$$$$$$$$$$$$####$$$$$$$$#### %.S������������������������I1,$!!$$$$$$!!! #$$$$$$$$$$$"!$$$$$%%##$$$d�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+//+&&&''$#$$$$####$$$$$$$$######$$#'%%&&_����������������������],)#%%((&&%%$$$$$$$$$$$$$$$$####$$#&!!(#( l����������������������{1
++)")$$$$$$$$$$$$$$$$$####$$$$$$$$####.#%"Y������������������������=1,$!!!$$$$!!!! """"""""$$$"!!"$$$%%##$$* V�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
6����������������������i&(&&$$&&&&&&&$##$$$$$$$$$$$&&&&$''##$&+-:����������������������K
,/.('%''$###$$####$$$$$$$$####&$$$##&&%%a����������������������[0$ &&&&&&$$$$$$$$$$$$$$$$$$####$$$&$")$+# l����������������������n0.)$!)$$$$$$$$$$$$$$$$$####$$$$$$$$####- !)[�����������������������m4'&##$$$$$!!!##
""""""""$$!!!!!!$$####$$&& O��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+7����������������������%!%&'$$&&&&&&&&##$$$$$$$$$$$&&&&&'%#%&)++E����������������������S
&)*(''''$###$$####$$$$$$$$####&&$$##&&%(
g����������������������V,))&&&&&&$$$$$$$$$$$$$$$$$$####$$$&$")$+#
r����������������������m2/)$!+ $$$$$$$$$$$$$$$$####$$$$$$$$#### - R�����������������������S0$$##&&$#$!!!##
$$$$$$$$""!!!!!$$$####$$!&
N������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������q'""$&$$##!!!!$$##$$$$$$$$ [...]
+e����������������������b4&'$#!$$$"$$ !!
$$$$$$$$
!!$$$$####$$)$
P��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������4'$$$1$$%%%#!!####$$########$$- 44%(;m������������������������B15%#2''%%""""%%%%%%%%#######&((1-"8*0r�����������������������N%""0)##$$$$"
$$$$$$$$$$$$&&&&& [...]
+=t����������������������S4+(&%###$$$$$$!!!!!!!!$$$$!!!!####$$$$$$%%-!,e���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������I!$($9%%%&$$$$'####$$#### !$$$&8**%)s�������������������������w;$)7,!
&&&&&$$$%%%%%%%%######%%
+)/:(._�������������������������,5$
'$$''$$""$$$$$$$$$$$$&&& [...]
N�����������������������N4((%%#$$##$$##!!!!!!!!######!!!!!!$"!!!$$&!7p����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������.*
)%%$!!$$'####$$### $$$&$ %%5���������������������������n"#,#
(&&&&$$$%%%%%%%%######%%+2, '-.��������������������������00-.,$%''&$""$$$$$$$$$$$$&&&& [...]
b�����������������������Q4'%%%#$$##$$##!!!!!!!!######$!!!!!$"! !$&)
P}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������`/1'"$$ #!!!####$$######$$#&%%"(11
g����������������������������K.*&''%%'''%&&&&&&&&&$$$$&&&'#+!1)
O��������������������������`
2+(!%%&&&&""&&&&&&&&$$$$&&&&&&%%%%%%
T�������������������������X+/))''''&#%%%%%%&&$$$$$$##&$$$$&%%.%$X������������������������z7,)$' ################$$$$$$$$$$$$$$$& +������������������������S 5%%%&#####$$$$!!!!!!""$$$$$$%%% $$""!#((
i������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t
$"$ ##$$####$$## [...]
(R�������������������������������������k6
+.((
")))'%%'+#()&&((#"<����������������������������������D&&('%,$!#(('''*## &+&"")% S������������������������������y5
-(&-%#%%''%%#((((%$$ $+.+$"#% -
3}����������������������������`
2/!#$(((&" ##$$%%% '%
$&))(1#E���������������������������\,"'/!.%##""!!!!########''$!!!$'#)&,*
n��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+!''+'&&%%0%
`��������������������������������������<
(.$'%"""%&&"%%%!!&*0#!I����������������������������������C'
,*+-(((#$&&&##$&&&&&/,*/3.'
h�������������������������������J+& 0'''$##$$%%&&$$####$&&((('*&(-
w�����������������������������6
+.&$
,!&$$##$$""########$$##'''' )8@f��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������fNSA;A83=E=JUa{���������������������������������������������aN&&)+++++'&%% '/:4&+ at g���������������������������������������sO(*/4.
*'%%(*++""'.-''*:.((Uy�Ȫ�������������������� [...]
#-+&&%%!
#!$*,"
6e��������������������������������������E'$
A#+())(((*((''&&&&!(/2$
m����������������������������������w20 **(()))+&&''&&##&&&&&&$$ .3 ]�������������������������������u9#
(-#)&($$$$$$%%$$$$$$$$$$$$$$""@0G��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������u\TNB [...]
+2(#%&&''&$(('###&&%+2/"5M������������������������������������O-0!%%%%&&$$&&''&&##&&&&&&%%*-"&S���������������������������������k
(
#$&''$$$$%%$$$$$$$$$$$$$$$$- !Nv������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+&&)%$))"
$(&$$
Y����������������������������������������$
8&(*&'''*))&$$&''$ %*#
+^������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������gN=<98..132 [...]
+
+5[|���������������������������������������������N( '(-/**-*% !$($ #[}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+
!
+,Vy������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+ :j�����������������������������������b������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+,n�������������������������������������������wW?
+
+/Uw������������������������������������������������������������������������������������������������������kbt����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
++&&((((''''''''*(# 8s���������������������������������������X##%'&(''#G|������������������������������������������������x^H86004B[l}�����������������������������������������Yt�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+##%'&(''&"!My���������������������������������������������uH:"
!7Vx���������������������������������������YW�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
Tv����������������������������������������`,
^�������������������������������������d������������xgZI;IIOY_i}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������c(((&####&&&&&&&&&&&&&&&)#
#01#i����������������������������}
+,,+2*##&''''&&&&$$&&(**(%%)!
���������������������������������j (3)#&%'%%%%$$*%$)+-,,+���������������������������������������Z)
%*.//$'+,8{���������������������������������������������x]?-% @Xz����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$#&
%%&&&'&!!#&&*/P������������������������������������������aA
)#
+ Pu���������������������������������������������������{e`]ZVV[_v���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Q &&(($$&&''&&&&&&&&&&&&&&&)((&#()'!(��������������������������; -1/)$%(*(&''''&&&&&)))%%%((',#0�����������������������������r " [...]
+!$''.)&&
+
z������������������������������������������������mTA*#
!$Bc{������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������.0+$&&&''(*&&&&&&&&&&&&&&&)%)($#(*'o������������������������� *,+(('(($
''''&&&&%'''%%%(/++$_���������������������������\
$,</!!!$! [...]
+
,u������������������������������������������xP1"
# #Bt���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������%0+"&))$ %(&&&&&&&&&&&&&&&)!)*&&..*"@�����������������������q "!' '())'%''''&&&&%%''%%(*$*
B+## 3���������������������������#E")$&4.
.''''&&''''#
#*( ($,/
R������������������������������S +!!+)&&%'**& $%%$!$%%08E���������������������������t_{qx���G :!,,)))'!&&"
$$(&%Pq����������������������������������������mF
'%),(##*
+7e��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������+2!&&&&&&&&&&$$$$&&$&&&&&&&(())((''2&[�õ�������������������z-
(&*)'''''''&&&&&&&&&&&&&&&&&(**))&&C���������������������f
3&&&&$$##$'&&&&%%%%&&&&''))(++ v����������������������������H''(((%%% [...]
+.��������������������������������c@#('$''''&&&&&&&&%%%%'$$&%
m�������������������������������������\.
'+*(((%%%%$$$$)'$&$$g�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~'.$&&&&&&&&&&$$$$&&$&&&&&&&(())((''+$
E����������������������e
&(&)'''''''&&&&&&&&&&&&&&&&&&(())))
+*������������������������z7 (#&&&&$$##$'&&&&%%%%&&&&'')'%(+#K~��������������������������W
''((%%%%####''''''####''+$#&$
+
+f�������������������������������-((''''''&&&&&&&&%%%%'$$&'
Cm�����������������������������������i
"'((&%%$$$$&&'''#$&$#
�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v$+)&&((''&&&&$$$$&&$&&&&&&&'(((((''+&0����������������������N)).('%%%%%&&&&&& [...]
+
h�����������������������D''0&&%%$$##$'&&&&%%%%&&&&''*''(('
Y��������������������������
(&(()&&&''####''''''####''&*( $((g����������������������������i7
).&$'''''&&&&&&&&%%%%%##$(( '���������������������������������w-&&$&))'$$$$$$$&&''&#$&((#
W���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������s(++&&((''&&&&$$$$&&$&&&&&&&''&&((''+1 r����� [...]
+####&#!!####$$$$$$$$####%%$$$$$$+ 2����������������������������xA)%3'%#$$''&$$$$$''''%"##$$%%+ 8��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������3+%&&''&&&&&&$$$$&&$&&&&&&&''%%%%&&'.$&A���������������������<
21',(%%%''&&&&&&&&&&&&&&&&&&((%%&&&&,' S������ [...]
+$'3&&''$$$$$$$$&&&&&&$$$$&&!($
"*(!
m�����������������������v)$""#
#''####$$$$$$$$####!$''''''$'b��������������������������X".$+ (###&))$!&&''$$$$##&&&&)) #)
n�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������I4$##))(($$%%%%&&&&&&&&''''%%%%%((*!7'";~� [...]
+H~������������������������T!,,+)$&%(####$$$$####$$$$$$''''##' -0&
)f������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������X// #))((&&%%%%&&&&&&&&''''%%%%%((*%6'";��������������������B,)(0**,,&&$$&&&&&&&&&&&&&&&&%%%%&((*')3���������������������@###%#&&%%##&&%%%%### [...]
+3u������������������������;
,*))$&%(####$$$$####$$$$$$''''##+!(+&
G������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t. 0%%&&&&))%%%%&&&&&&&&''''&&&&%((*,/&&D���������������������V&*')(())&&%%&&&&&&&&&&&&&&&&&&$$&((*#,3���������������������H'
%"'#&&$$$$%%&&&&$$$$ [...]
"''&&&&))%%%%&&&&&&&&''''&&&&%(((,&!&P���������������������j#*)
(())&&%%&&&&&&&&&&&&&&&&&&$$&(((#,7���������������������S'
*#*'&&$$$$%%&&&&$$$$$$$$&&&&&&&&&&%' &\��������������������z)
(, )&&&%###$$########!!######$$%%&&$$*#
����������������������=55#%$$$&%%%################# "$$$$##1( R����������������������~P(!**''$$%%##########$$$$$$$$''''##'$($-'������������������������������������������������������������������������������������������������������������������������������������ [...]
+1*!$$#&&&%$$$$$$$$$$$$$$$$$""&&''''#()
E����������������������g(%(((''&$$$$$$$$$####$$$$$$$$''''####,!++w�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������=&&((&&%#%%%%&&&&&&&&''''''%%%'')*(&x����������������������Y!2&(())((%%&&&&&&&&&&&&&&&&''$$$$''&#T���� [...]
*&!+$$&&&&$$$$$$$$$$$$$$$$$$%%&)))''*!&&D����������������������S+.&&''&$$$$$$$$$####$$$$$$$$''''###',!&&m�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������l.$&%'**%%%%%%&&&&&&&&''''''%%%'(*../�����������������������|/.*&&))((%%&&&&&&&&&&&&&&&&''(%%%&))"!e����� [...]
+U���������������������p1
#/$''))####$$$$!!!!$$$$$$$$!!""$$$$((*
X���������������������v2
0
%%&&''&&##%%&&$"####$$$$''''''''!!&#*& r���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V"//',"))(($$%%''''''&&2,,.%%-0&_��������������������������Q(0*+&#,%%%%%%%%&&&&&&&&$(.*%%''%������������������������U!&1,*((((&$ ##$'''''''''''''*%'%#(+& |�������� [...]
+#a���������������������u6'/&''))####$$$$!!!!$$$$$$$$!$$$$$$$$&$.f���������������������n) 0$%%&&''&&#&%%&$$$####$$$$###''''''!&#&# y����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z ,4 0$$&&&&''''((''(( $(181(0
,����������������������������0)(+%(*'((%%%% [...]
-++*('%$$$$&&&&&&&&&&&&&&'*'##*+&�����������������������r2!$,
))))('&&##''''######''##$%%%%"$$
Az���������������������{1,,$%%''##$$$$$$$$!!$$$$$$$$%%$$&&%% $Mv���������������������l, 0%&&''''%%%%&#$$$&####$$$$##$$$&''($'"'
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+33$&%%&&$$$$$$$$$$$$$$$$$$$$''''''%%.. y����������������������s7.&&&''''%%(%##$$&&####$$$$$$$$$$$$+
$*/$ *�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Z# $&()++)))'(''))!&'..!'?��������������������������������=('&)+%((((((%%&&))))))'')-$
(�������� [...]
+5z����������������������������������`$
+5)
((((((((''''''''##**
Jr�����������������������������o"**''((''%%%((((%%%%%%%%%&1''-D���������������������������R$",&&((('''$$''''$$$&%%&&&&(%#'*( 8k������������������������c'.&""%%&&$$%%&&&&$$$$$$$$$$$$$$''''&&%
+
D�����������������������|1.)&&''''%%#&%%&$$$####$$$$$$%''''')$+ D}�����������������������������������������������������������������������������������������������������������������������������������������������������������������������u�����������������������������������������������������������������������������������������������������������������������X')+,**&&****+-11+"5:2_������������������������������������F
)&3((((((((''''''''.*0*(s�������������������������������9
+**'&&%%%%%((( [...]
2$&&((('''$$''''$$$&%%&&&&(##(.&b�������������������������y;5)))%%&&$$%%&&&&$$$$&&&&&&&&$$$$''&&1
,f������������������������< .%&&''''%%##%%&&$"####$$$$&&''''''$$0
]�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������r;"
>p���������������������������������������}
+2-%%*0..(/'!*,,0
R����������������������������������]<&'*
0&&$$$)+((+/,)')-
+A
������������������������������T""&/(3"+"&&&)('''
#''(&)+& $(
8���������������������������t0&+0*$!"%"""$$$$$&&&&$$$$#!1#1 ,)r�������������������������:
/-#%& "&$$################
!$&&$&&1!/,����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+3U���������������������������{@#-3
"$''%%$$$$$$$$&&&&$$$$9 &)A(��������������������������R/110*&"""################$$$$$$$$!1'#����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������xqqt{��������������������������������������������������\
+
+
X���������������������������������������n-##''##%*,'&(-2/+7����������������������������������4
#+$1.+''&&&&&(&$##(+.1-* #������������������������������\ 5/.*+*$###'''''''''''$$$$)&1..'s��������������������������s93**))####''############'')'&$$&''*4D|�������������������������������������������������������������������������������������������������������������������������������������������������������������������������~���������������������������������������� [...]
+",)
+ Ro��������������������������������������>
8'00%"##'')))&&&')**(z���������������������������������0(&$'(!!'######''''''''''.
5[�����������������������������}0
4.),(
$&&&&&$$$$$$&&&&&&$!$'())' R���������������������������������������������������������������������������������������������������������������������������������������������������������������������������}���������������������������������������������������������������������������������������������������������������� [...]
+4z��������������������������������������/
+')*,&&&&&&&&&&&&))))".x���������������������������������P( #*"#-&&&&&&&&&&&&&&&&'$$&((#C�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������{������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
4Og���������������������������������������������]0
'-/)!!',$&))$
)m�������������������������������������rF
++), %##-33*
***'''&&
#/I������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
"%''().3,(**-+#
Iw����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
(8]k�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ 8g��žð����������������������������������������zj[LKQQ[k�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
!$>`�������������������������������������������������~zx{���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������zv�������������������������������������� [...]
+0X�����������������������������������������������zpjc`bkt{���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
Y�����������������������������������������U
+ !\v����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������i (/-&&(+))+-(((****'+
)-%(,0#N��������������� [...]
+
d��������������������������������������{C
+ew��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������
1/,)('''&&&&''''(((()&'''(''$8$);��������������������������
',*&*.((***''%%%%%''''*&"(($
5u����������������������������_/&/*(%%'''''''''''''(++ -�����������������������������������m
'.,,''&&''((+
@m�������������������������������������������eP5*
*;Ew���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
-/,&('''&&&&''''((((&&'''(''&2'+
�������������������������h(,*(**(((*''%%%%%%'''''&$(+&
D����������������������������< +/*(')'''''''''''''((+%-H|��������������������������������]$))((''&&''((-&! -}�����������������������������������������hD$
?b����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ <m�������������������������������������������������~pkcafq}������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Z&(,+$('''&&&&%%%%((((&&'''(''*#/,(
h������������������;((((++)'''(('%%%%%%%''&&#(**))-/
1}������������������������F [...]
3^������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������0( (-*(('''&&&&&&&&%%%(%%''&())&.213 #����������������������$))&&&)))))&&''&&&&''&&&&'('%$$&)'8�����������������������f'.#%''('''&&&&&&&&&&&$$&&&0("%%#��������������������������/ [...]
2)%%&%(/ $$$ . 3
)>w�������������������������������������t=
+
'%('''%!
I�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������5%)$+%'('''&&&&&&&&%%%%&&&&%())$//!-+o��������������������p(**((&&)*((&&#&''''''&&%%&&&&%#%%2s���������������������l
)<#%%%%&&&&'''''''''''$###',(
6 o����������������� [...]
+7���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������> ,$+ '('''&&&&&&&&%%%%&&&&%()) +((- R��������������������`)*-.*(&)*((&###''''''&&%%&&&&%'%%6
Y���������������������Z#,A'%$$$$##'''''''''''$####("%.%+
K�����������������������-
* &&(' [...]
+++(((%'$)&&&&&&&''''''''&&&(**(%'+. V��������������������.
-&('(((((%%%''''''&&&&&&&&%%)'$$'(** ���������������������<(!,.))''''&&%%''&&''''&&&&''$$$$)0#/!V����������������������80(4+
&(+''%%%%&&%%&&''''''''&&%%&(,,!�������������������������P:=$
'3''''''''&&&&&&&&''$$%')) 2G��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+/*+$(())(())''''''''&&&&&&&&$'&(''((�������������������b"0'*/&&(''$$&&''''&&%%%%((((&&'' #( )(��������������������~6&,)#($(3''%%%%&&&&&&''''''''&&&&&#.&
l����������������������m((8#)$/.((''####&&&&&&&&''')'''%+
)-�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+��������������������k +''+'&)&&%%%%''$&&&&&&&''''''''+,%$$"��������������������k%**((%%&$''''''$$$$####&&&&''''&&&&##+
b���������������������d
9'**(&%%&&$$''&&$$$$$$$$$$''''''''&&*. d������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o
/%1-*, [...]
��������������������������?#2&2&&&&&&&&((((((((%%''++((0) D�����������������������l
8#''))((%%)&&&&&&&&&&&&&&&)'((($%i���������������������T#
(+#/&(&&%%''''$&&&&&&&''''''''&".
#`��������������������[(*((%%&$''''''$$$$$$$$''''&&&&%%%%$%%(w���������������������20')))&&&&&&&$$$$&&$$$$$$$$$$$$$$$$ * #S������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+a����������������������z'*+!1&(&&%%''''$&&&&&&&''''''''&&)$>��������������������g (,((%%&$''''''$$$$$$$$''''&&&&%%%%%'%(���������������������5+')))&&&&)&&$$$$&&&&&&&&&&$$$$$$$$#' #Y����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t%
$/1)))+)))&(())****4* t�����������������������������W'*+((())))))))))(((%&)+++(
��������������������������j"(((((+)$$('''''''''''''''(#$&($������������������������2 /0 0$)''$$&&%%$&&&&&&&'''''''')+", i���������������������u/**((%%&$''''''$$$$$$$$&&&&''''''''')%+���������������������|8)+))&&&&))$$&&&&$$&&&&&&&&&&&&&&&&&##( f����������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+���������������������{8
&/))&&&&))$$&&&&$$))))))))&&&&&&&&'%/ p�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������_#
),%2%()..#7����������������������������������_
+
+","0+0**((((&%-&(.*
+j�����������������������������
)1.*(!
(!#((#%1%())&&)/))
+ E�������������������������{(,%/
'/+'')+& %%((((''&$$$&)&&3+ :�����������������������T/'&"('')+((##%%&&%%&&&&&&0'#*.&#$'"/T���������������������75"))))(%##&&&&'''$$&&&&&&&&&&&&&&$
)/)���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+D����������������������������������"
.**(%).)%%,3.' $'.7 %n����������������������������-22)#)11(
!+))&'$''$+2-++-? ��������������������������*2*')&#$(''$$%%''&&&&&&&&('!$+-)#4z�����������������������;
&+'%%%%'&)'''''''$$&&&&&&&$$&&&&&&& /',����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
&++&$&)'4*&1:2
+a���������������������������������
'46) ""$+##$$$'''-+&$(/47d����������������������������L
",,! '''$$$$''''''''''"+-"2<*`������������������������P8+++&$$$&)&&''''(#$'''''''##''((((/0`������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+)+-1*)+
&$
&.)1����������������������������������Q
'
+2+
'&&))((('(+--(# ������������������������������)*4,'#''$$$$''''''''''+$")2+&"`�������������������������u&8'-+)&&&&&&&''''(#$'''''''##''''''%
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+7����������������������������{7%%%%&&&&''&&''&#$'''''''''''''''R}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
P������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+
<e��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
#)Dd����������������������������������������������������������������������������������������������������������������������������������������������������������|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+##%**(''((%"
X��ĵ���������������������������������������rhZF>GV`n{��������������������������������������������������������������������������������������������������ap���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
K����������������������������������������gV-$
)CYu������������������������������������������������������������������������������������������������kv������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+C�����������������������������������Z
'*$
%b�������������������������������������������������������������������������������������������rQn���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ =d���������������������������������õ^]������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������/%%
#4''''''%%%%&&(())#*# /2,,������������� [...]
+
+T����������������������������������lp�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������R24%"-/''''''%%%%&&(())&(#%(
.%
{����������������������,&:(/*&&(*+(+'&&(+**#+'
$15/+ D��������������������������! (($5(1 %/+(%%%&'## $0+),7��������������������������������\-$&1.,(*.%"'.,z���������������������������~��������������jSJ6,+/25Vg|���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+
++Rm������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������&
&&(+)$''%%$$%%&&%%''&&%%&&&(((1(/���������������������`
+:
+%**((((''&&&&%%%%##$$''&@�����������������������_A--''&&&&%%%%((((%%%(((((2' <����������������������������##))+)(())(())((''((&++Q~����������������������������������U+ $$%%%%&&
+Fl��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{
"(()))'''%%$$%%''''&&&&&&''&)))*'
+n�������������������u%
!8#$*+))))&&&''''''''&$$&%%''&2(]����������������������0%(*''''%%%'''''((('''''''' 87 r��������������������������M 11$'))((&&''**((''((&!')E���������������������������������S
+
'++***,,,)&
}���������������������������������������������xpmbX\kyzz������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z
#(()&''''%%$$%%''''&&&&&&''&)))'#K�������������������a &*(&*))))&&&&''''''''&&&&%%%')+!-=� [...]
+
+
%2dx��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}*$&&''%&&''$$$$%%%%&&&&''&&&&&)))*%% 6������������������T3."2+'((((&&&&''''&&&&&&''%%'(&3
-���������������������W%.((((%%&&&&&&&&&&$$$$$&&&#* &*8���������������� [...]
+^����������������������������U.+&'
))('''##&&'#$$''Ey������������������������������������h<
+
+
+
|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������G !$''#$&''$$$$%%%%&&&&''&&&&&)))%%((*o������������������E52&2+
(((%&&&$''''&&&&&&''%%''&3
-e�������������������q#0 ((((%%%&$&&&&&$$$$$$$&&&.(&$������������������� [...]
+Mz������������������������������~D $.4%$*%/'&&&)''(.
R������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������(*" %&&$$$$%%&&''&&&&&&''&((('0
*o������������������U$+,)0((''''##&&&&&&&&(%''&&&&)
0$
Y�������������������J$1& [...]
)4%..&&&&''''''''&&&&%%%%%%&'--
,
$>|������������������G0&))&&''''&&&&&&&&&&''''''''')%%((>��������������������x+++((%%##''''''&&&&&&$$$$&&&&&&&($N����������������������Y( 2&&&''''''$$&&%%$$$$%%%%%%-$, o���������������������������>'
'.(&&&&&&&&&&&&&&&))'*+#
<����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
>�������������������?1).
&&''''&&&&&&&&&&''''''''''%%((
({�������������������X#
+((((%%&&''''''&&&&&&$$$$&&&&&&&(%!4����������������������6/.. &&''''''$$&&%%$$$$$%%%%% 3,
;}������������������������fB,0+#(&&&&&&&&&&&&&&&)'%'++$
+ X���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{
+70
1''%%%%%%&&&&&&&&&&&&&)))!0"Q��������������������1"" -)#&&&&''''''''&&&&''%%&&&($1$0 A�������������������F.)0
''&&%%%%&&&&&&&&''''''''&&%%')!
m������������������|+!&((((&&''''&&%%%%&&&&$$$$&&&&&&&(') ���������������������o/0)"''''''''$$&&&&$$$$$%%%%%&"+&I������������������������@ 33#:&(&&&&&&&&&&&&&&&+)$%+-+) +h���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v1 [...]
+���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������N*&%%&$$$%%&&&&&&&&&&&)))))'2 ����������������������2,*-'&&&&''''''''&&&&''''''%'*#*7~�������������������x'8$
''''''%%&&''''''''&&&&&&%(%%))
0w������������������m
-'''''''' [...]
+��������������������T3'0+*!%%&&&&&&##''''##%%$&%%%%-'$&&
p��������������������&1% +/('''''''''''''''""'*(($$$9!#�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o4+&(())))&&&&&&&&&&((((**i������������������������l$ [...]
��������������������Q0'0(/%%&&&&&&##''''##%%$&%%%%+$+(1( p��������������������[,
+.+*($%'
('''''''''''''''"''$## #%$!a�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������yD,(/-&2&"*+'(..,#*# +(-7
��������������������������G-5#/' +("-''''&&%%+& &,**/)-(o����������������������-
(1-*(#*,%''''''''$''(((((-"/# [...]
|�����������������������N,(%##%**,4''''''''''''(((()"3
*$ (���������������������h%
&(0%#&*%%&$%%$&%%%%&&&&((((((**
)"��������������������H3)00
)'''''$##''''''''"$%%%%''&#'$'* �������������������l(.)&&&&%%$$&&&&''''''''''%%&&((''&&"4' B����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+#1##-*--..//,,-' (.'#R���������������������������/&!$((''(''''''(((''''((5(!$Z�����������������������1:)+$'4*'('&&#'''''''''''))&&&&&& ~��������������������S7*- 2''''''##''''''''#%%%&&&&) 0$++ ��������������������P
6-(((%%%&&&&''''''''&&&&&&&&((&&&&&
%
G�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ v�������������������������#
+00'((((('''((((((((#&&&&&&&$,
4 ����������������������h$)+**''((&&''&&&&&&&&&&%%%%&&''''''%% y������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~bWOKF?=EAO^���� [...]
+ L����������������������������������g
432*# $',/$&(((("&$05i����������������������������D
.'*)'$%-4%%%%''%%0-(" %),'7E~�����������������������[&9.$ )+$
&&''&&''''&&''&&!)/,$$+3)8-������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+6�����������������������������u02*)),,)#
('&$$'%'%&
"+* &)�������������������������*"0+&+*&%'&&''''''''''''&&'%#(+)
##1z����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$ %T|���������������������������������V15,&()&&+)'&$$'%'("%*+(1=y���������������������������r
==*!%-,&&&&&&''''&&&&&&%%))%%-<(�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ ))
!+,,,,++**"
*.
Z}�����������������������������t $),&%(#&&&&&&''''&&&&&&' '24
i��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+//''''%%%%''((''(())#*/i��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+().*#!$)"'+'(&
+n����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
&
&97Ux��������������������������������pN������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
,�������������������������{��Yd��������gL>5*%$)9FYb���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+)$)4%))))(('*%%&&''$!e�������������������������_J���y5
+$$
"" j��������������������������������������������z��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+#57-&,.)('-+'00 Y������������������������������������eA!
+ )Ei�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ $Ns���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
.4'$+''''''''$(++$ b��������������������������������L0'
Hf������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������;5"''*('''''''''''#''''''(((&+.( [...]
+p�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������?&,''(('''$$'''''##'''''''(1$& 3%0������������ [...]
+2.+21+**0--2/ S|��������������������������������������������zz{}������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������G/#%%'''$$ &&$##$$$&$$$$&&&&/
%R������������������W%
$-'''$###'''''''''%$##$'''+!-+ [...]
+w��������������������������o
20)),,/2+!
' '&$%**%)
Q�����������������������������������^-
+ W����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w6&##+&
*2$&&&&&&&&&&&&&&&&)''%'F�����������������]).''%%&&####''&&&&&&''''&&+( +-+�������������������N""&&&&&''&&''''''##$&&&&&&&/$**&D��������� [...]
+
)/-*&&&&&&&&&&&((((( )4$x���������������������������������4$
*(('#
!Ep���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������U! 6# ! */$&&&&&&&&&&&&&&&&)#-#%&*H������������������e,)''%%&&####$'&&&&&&''''&&'+!((+
������������������1"&"&&&&''&&'''''$##$&&&&&&&+ '%+!/� [...]
("&-#&&&&&&&&&&&&&&&&&&' +#'4
M������������������p )$''&&''##$$$&&&''&&''''&&!-'(&+
t�����������������x')#''&&''&&'''''$##$&&&&&&&'"%#/+
��������������������%6((()&''&&&&&&''''%%&&&&'(-!$$3( ����������������������m"*6(!''&&&&&&&&&&&&&(((% .6&����������������������������f% +/2.)*-)),,++**$/6/
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
$, #*#''''''''''''''''-'&$6+��������������������V$+''''%%$$$$$%&&&&&&''''&&'+"+//�������������������; 0 &&%%%%&&'''''$##$&&&&&&&&+."(% y������������������@
,&)&'$$$$$$$$$$$##%%&&''&)&)-!+- E��������������������1 (%)) #,%''''''''$$$'''''!&((-53(
�����������������������\#-<$$%%$&**'%**)))'%$ &+))*00"2}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
"v������������������`))0$(%',&&&&&&&&$$$$&&&&1)!
'(&����������������������V-*-&%%(()))$
(())((%%$'(( !))
@�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������M%(&&''''''''''''''''08!<����������������������w
((&&''##$$$'''&&&&''''&&(('3)
N�������������������|!&)%%&&&&&&'''''$##$&&&&&&&*!""1% �������������������;'
! [...]
911""&(('$ .#"%"%.+3"8 ������������������������5
2
,6
#3"''&&''&&/$!&'
#&$3 #:'x��������������������5 3
&*##+) %$$$$&''&$$$#%%%)-$9
�������������������M$))&&&&&&&&&&''''''$$&&''((((+1$;|������������������E
+5$'')&&&%'%%%%%%%%%%%%%%%%$&%%''''o��������������������Y
'+'$!%%&&&&&&&&&&&&&&&&&&&&&&*&���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������O1/,(# !$$(.1,*(%#)(d������������������������}A
2,"1))))(((($' [...]
X�������������������m,&" ')$$$%%&&&&&&&&&&&&&&&&&&&&&&,\���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������P
.+&&+++(
'##//$@.d��������������������������{+!0+&+'$$$$$$$$
%(###$'#1:C ����������������������u&
+/ "'*'
$''##$&&&&&##$&%%3&"
(g�������������������|
0((''''''''''''''$$$$&&&&$+$ *' W�������������������D '4'')'&&&&''''''''''''''''&&%%((((
Z�������������������?3,&'('$(&&''&&''&&&&&&&&&&&&&&&&1) E���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+e���������������������,'((('''''$$$$$$$$$$$$&&&&(+)- u�������������������H+''')'&&&&''''''''''''''''&&%'((*(o�������������������>8/''('%(&&''&&''&&&&&&&&&&&&&&&&&)+ -w���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
++!)))))&''$$$$$$$$$$$$&&&&&($$+"��������������������A7&(()'''&&''''''''''''''''''''(((( z�������������������M2+'**%$'''''&&&&&&&&&&&&&&&&&&&&
/ o��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������hF
Ty�����������������������������������W
+
''(((())&#(//'Q����������������������������N&'''%$''&&&&((&&&&''((.+ W����������������������t
+))))))''''''&&&&$%%%&&((3(
+3��������������������A70(()'''&&''''''''''''''''''''(((( ��������������������M+(&**%$'''''&&&&&&&&&&&&&&&&&&&&,$$
+&r���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~k]VSWVSWZn��������������������������������������L$+++**++++1)*4-
.%������������������������������3&!/*
*&&&&'(((''&&%(''
*
+ \������������������������U&(((((())''''&&&&&&'((((()&)4+)\��������������������^/,((*(''%%&&&&&&&&&&&&&&&&(*((''((��������������������;*&#'('$&''''&&%%&&&&&&&&&&&&&&&&(7.n������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+Q���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������_M748612/6:32Lq�����������������������������������P
+0&0% '&##&$&+))=U���������������������������m< 1#)
((%%$$%%"(#$.(#,--(0 �����������������������L-&-4
'3$"'%%%&&''&&'$$$$&**>#8!t��������������������H
+&10%'$!(&&'$$$#'''''&&&())+)'')*)4 U��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~vst{�����������������������������������������{6 (##)+&+-(( [...]
9. (($(&&'$$$#'''''&&&(')))#
��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V=2#'!
$))
!8F��������������������������������V!.'1'%!$(*'$ 4-())8:y�������������������������=:-&$(!$((&&''''$$'(((('0($ 13
����������������������P+%#*-# +''&$##%%''''&&((#$((((((74������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
.������������������������������+%,)<"*'(((''''(+'##&)))
/:- y������������������������K/&((#13 ))&&&&%%(&&&'(((++'$$$''59����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+-&#'((((''((+'#')--+3,d��������������������������
+8%#.#))&&&&%%((&&((((+('(+-00��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
u����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ *K�¯������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
|������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ ?_������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
Rr����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+**:///(!
#%%.)(%&+034h�������������������������������w<
>`����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
D�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$
#--++))++*#+1+w������������������������������������zbZB=528DRZx��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
&"$+''''''''$&(($ q����������������������������������a8&#
+
Pp���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������\ *,#' '''''''''''''''''%%&&%%'' [...]
+#+#-+++****
e�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������M
1, #''&&&&$$$$$$$$&&&&' [...]
-CVr�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������N)''')$&&'$$$$$$$$$$'&&&&'&&&%(#/"������������������*
''## #$$##$$##$$##$$$$$$
+($( k� [...]
+
-2+$%%%'****(5&K���������������������������������������vXMHOSOPV���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������E-"%#################&$$$$&%%&6 ������������������,**%%%#$$##$$##$$##$'&$$ [...]
1x��������������������������������sD=n����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������b
"%)################$$$$$&%%+)$������������������&
+####%#$$##$$##$$##''&&$$#&!$20$!5�����������������p28(&&'$$$&$########$$## ##
'
$+.x������������������K .(1$#$#(* (('#####) $&$'*
g�����������������������# 1&$)%
++(($&'#0/
,%
�������������������������������h
@���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� -!;
!$$#&&&&&&&''&&)&! #$''&, $������������������ ,(&$$++&$$$$$######!!##(
&%(+$8����������� [...]
,��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������F<-,)''&&$ $$##$$$$ '(((++74
F������������������P 0$('
3&$$$$$######!!##$$++%%)$
D�����������������h
+$ ################ #*$ S���������� [...]
J��������������������I
$&&&&&%%%%''''''&&&&$$$&%((()U��������������������������B
+")++(&((''%%%%))++n��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������[ 7+$# #'('$####$$$$
#(($!$)%{������������������z
+,
'7 '$%%$$$$$$$$$$ &+'"$(%]�����������������s
+)%"$$$$$$!!! ######### !!#.
K�����������������}*!!% &$$$$$$$####$$$$####''''''-
/��������������������&*+&&&%%%%'%''''&&&$&&&&''''%)����������������������§P$&)++)''(((&&&&&&$'/-&+��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
" %&$$$$$$$####$$$$####''''*%1(
�������������������`
*&((&&%%$$%%''''$$$$&&&&''''8y�����������������������'-++++))''''&&&&&&&$$)221/
g��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������0
-+*-& '$ [...]
#%%+'$######!!!!####""##''''$((������������������l+
($ $"''&#$$$$&&&$$$$$$$$&')))'2=���������������������X7/$,++))''''''''&&&&''&$$&))14������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~������������������������������������������������������������������������������������������������}&! %$
'% [...]
+$$$-)
'$##$$$$&$$$$$$$$$$&')))-%
m��������������������% + .&&&&''''''''&$$$
%*($$$!))������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}��������������������������������������������������������������������������������������������������< "&$"*%%&&''&&%%(*
#�������������������������l (6&#$$## [...]
+
E��������������������y:#%"######$$$$$$##!!!!!!##& ' ]�������������������!*
#&$$$$$$$####$$$$""$$''&&)"-+
������������������O2%'5''##$$$$##########%%%%%%%% %))^�������������������d
!:/5&&&&&&&&&&&&$$$$%))')(&.
?�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������y����������������������������� [...]
+#��������������������L44"
!$&&&$$$$# !!'%##''% .
(+ B������������������Q&$, ''$$####""''####!!$$$$$$$$'-
Q������������������L
*)''+'&&&&&&&&$$$$$$$$##&&''%%'%$(\����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������qu����������������������������������������������������������������������������������� [...]
81($$(')((
%)) %40a��������������������X'&,$))&$$ !
$$$$# !!'%##%%" .:
T������������������R"4,*$!!$$####""''####!!$$$$$$$$" -O������������������G,*'%('&&&&&&&&$$$$$$$$##$&''%%"'&&
+<����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ss��������������������������������������������������������������������������������������������������������}rQ<)'$#2Fi}������������������������������������K!# "&%$
[����������������������������" ,
!
(42&
.��������������������� [...]
+**+)(&&&
%.( >������������������������3 *##$###$&$$$$$$##"$&&##'(+$x�������������������g
$$0"-''####$$########$$$$$&&&$+$ <z������������������?..% &(&&&&&&&&$$$$$$$$$$$'%%&#("%#&����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������zt������������������������������������������������������������� [...]
B]����������������������������������R &"&%'$$!
/$
'3.7��������������������������r
+((++)$!#$$$$$$$$!!$&&''&)6
6���������������������M,.*&&&&$$$$########$$$&&&&&'-
_�������������������@..% %'''''''''$$$$$$$$##$'&&'%*%*
0����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������yt��������������������������������������������������������������������������������������������������������������������������������������������������������������pYIE?9?IUd}������������������������� [...]
+6u���������������������y
)) ((&&&&$$$########$$&&&&&&'#��������������������A.+%"%%''''''''$$$$$$$$##$'&&''''5
+K����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{w���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z? %c�������������������������� [...]
@������������������������)))-$''&&&$##!$####''&&&&&&&&+
+6��������������������G
+*)%"%%&&&&&&&&$$$$$$$$""#&&&((($4x����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w>91*%% [...]
+)&%%%$&&&&&&&&$$$$$$$$""#&&&((+)
<������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~}���������������������������������� [...]
+'(((*% &'
"e���������������������������|;"&
+(
!! %$!&.'*##%*(#����������������������b,(''!/'(('##%'%"$$$$##&('" &))!(�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t�������������������������������������������������������������������������������������������������������������������� [...]
+ Bl���������������������������������v
"" $
)1%" %$" 3+/�������������������������~5*5.##&&))%%&&$$$$$$/'(31&%- e�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~{��������������������������������������������������������������������������������������������������������������������� [...]
+@����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o|���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+:�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������i|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ +����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������hc������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
1Nm��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������xh���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
\ No newline at end of file
diff --git a/tutorial/imgproc/autothreshold/tutorial-autothreshold.cpp b/tutorial/imgproc/autothreshold/tutorial-autothreshold.cpp
new file mode 100644
index 0000000..e2fdb34
--- /dev/null
+++ b/tutorial/imgproc/autothreshold/tutorial-autothreshold.cpp
@@ -0,0 +1,105 @@
+//! \example tutorial-autothreshold.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC)
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+ //!
+ std::string input_filename = "grid36-03.pgm";
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0] << " [--input <input image>] [--help]" << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, input_filename);
+
+ vpImage<unsigned char> I_res(3 * I.getHeight(), 3 * I.getWidth());
+ I_res.insert(I, vpImagePoint(I.getHeight(), I.getWidth()));
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d;
+#endif
+ d.setDownScalingFactor(vpDisplay::SCALE_2);
+ d.init(I_res);
+
+ //! [Huang]
+ vpImage<unsigned char> I_huang = I;
+ vp::autoThreshold(I_huang, vp::AUTO_THRESHOLD_HUANG);
+ //! [Huang]
+ I_res.insert(I_huang, vpImagePoint());
+
+ //! [Intermodes]
+ vpImage<unsigned char> I_intermodes = I;
+ vp::autoThreshold(I_intermodes, vp::AUTO_THRESHOLD_INTERMODES);
+ //! [Intermodes]
+ I_res.insert(I_intermodes, vpImagePoint(0, I.getWidth()));
+
+ //! [IsoData]
+ vpImage<unsigned char> I_isodata = I;
+ vp::autoThreshold(I_isodata, vp::AUTO_THRESHOLD_ISODATA);
+ //! [IsoData]
+ I_res.insert(I_isodata, vpImagePoint(0, 2 * I.getWidth()));
+
+ //! [Mean]
+ vpImage<unsigned char> I_mean = I;
+ vp::autoThreshold(I_mean, vp::AUTO_THRESHOLD_MEAN);
+ //! [Mean]
+ I_res.insert(I_mean, vpImagePoint(I.getHeight(), 0));
+
+ //! [Otsu]
+ vpImage<unsigned char> I_otsu = I;
+ vp::autoThreshold(I_otsu, vp::AUTO_THRESHOLD_OTSU);
+ //! [Otsu]
+ I_res.insert(I_otsu, vpImagePoint(I.getHeight(), 2 * I.getWidth()));
+
+ //! [Triangle]
+ vpImage<unsigned char> I_triangle = I;
+ vp::autoThreshold(I_triangle, vp::AUTO_THRESHOLD_TRIANGLE);
+ //! [Triangle]
+ I_res.insert(I_triangle, vpImagePoint(2 * I.getHeight(), 0));
+
+ vpDisplay::display(I_res);
+
+ vpDisplay::displayText(I_res, 30, 20, "Huang", vpColor::red);
+ vpDisplay::displayText(I_res, 30, 20 + I.getWidth(), "Intermodes", vpColor::red);
+ vpDisplay::displayText(I_res, 30, 20 + 2 * I.getWidth(), "IsoData", vpColor::red);
+ vpDisplay::displayText(I_res, 30 + I.getHeight(), 20, "Mean", vpColor::red);
+ vpDisplay::displayText(I_res, 30 + I.getHeight(), 20 + I.getWidth(), "Original", vpColor::red);
+ vpDisplay::displayText(I_res, 30 + I.getHeight(), 20 + 2 * I.getWidth(), "Otsu", vpColor::red);
+ vpDisplay::displayText(I_res, 30 + 2 * I.getHeight(), 20, "Triangle", vpColor::red);
+
+ vpDisplay::flush(I_res);
+ vpDisplay::getClick(I_res);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/brightness/CMakeLists.txt b/tutorial/imgproc/brightness/CMakeLists.txt
new file mode 100644
index 0000000..264f986
--- /dev/null
+++ b/tutorial/imgproc/brightness/CMakeLists.txt
@@ -0,0 +1,23 @@
+project(tutorial-brightness-adjustment)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-brightness-adjustment.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/Sample_low_brightness.png")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-brightness-adjustment.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/brightness/Sample_low_brightness.png b/tutorial/imgproc/brightness/Sample_low_brightness.png
new file mode 100644
index 0000000..acda26a
Binary files /dev/null and b/tutorial/imgproc/brightness/Sample_low_brightness.png differ
diff --git a/tutorial/imgproc/brightness/tutorial-brightness-adjustment.cpp b/tutorial/imgproc/brightness/tutorial-brightness-adjustment.cpp
new file mode 100644
index 0000000..5694901
--- /dev/null
+++ b/tutorial/imgproc/brightness/tutorial-brightness-adjustment.cpp
@@ -0,0 +1,143 @@
+//! \example tutorial-brightness-adjustment.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC)
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
+ (defined(VISP_HAVE_PNG) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+ //!
+ std::string input_filename = "Sample_low_brightness.png";
+ double alpha = 10.0, beta = 50.0;
+ double gamma = 3.5;
+ int scale = 240, scaleDiv = 3, level = 0, kernelSize = -1;
+ double dynamic = 3.0;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--alpha" && i + 1 < argc) {
+ alpha = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--beta" && i + 1 < argc) {
+ beta = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--gamma" && i + 1 < argc) {
+ gamma = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--scale" && i + 1 < argc) {
+ scale = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--scaleDiv" && i + 1 < argc) {
+ scaleDiv = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--level" && i + 1 < argc) {
+ level = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--kernelSize" && i + 1 < argc) {
+ kernelSize = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--dynamic" && i + 1 < argc) {
+ dynamic = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input image>]"
+ " [--alpha <alpha for vp::adjust()>] [--beta <beta for "
+ "vp::adjust()>]"
+ " [--gamma <gamma for vp::gammaCorrection()>]"
+ " [--scale <scale for vp::retinex()> [--scaleDiv for "
+ "vp::retinex()]"
+ " [--level <level for vp::retinex()> [--kernelSize "
+ "<kernelSize for vp::retinex()>]"
+ " [--dynamic <dynamic for vp::retinex()>] [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ vpImage<vpRGBa> I_color;
+ vpImageIo::read(I_color, input_filename);
+
+ vpImage<vpRGBa> I_color_res(I_color.getHeight(), 2 * I_color.getWidth());
+ I_color_res.insert(I_color, vpImagePoint());
+#ifdef VISP_HAVE_X11
+ vpDisplayX d(I_color_res);
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d(I_color_res);
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d(I_color_res);
+#endif
+
+ //! [Brightness contrast adjustment]
+ vpImage<vpRGBa> I_color_adjust;
+ vp::adjust(I_color, I_color_adjust, alpha, beta);
+ //! [Brightness contrast adjustment]
+ I_color_res.insert(I_color_adjust, vpImagePoint(0, I_color.getWidth()));
+ std::stringstream ss;
+ ss << "Sample_low_brightness_alpha=" << alpha << "_beta=" << beta << ".png";
+ vpImageIo::write(I_color_res, ss.str());
+
+ vpDisplay::display(I_color_res);
+ vpDisplay::displayText(I_color_res, 20, 20, "Brightness and contrast adjustment. Click to continue.", vpColor::red);
+ vpDisplay::flush(I_color_res);
+ vpDisplay::getClick(I_color_res);
+
+ //! [Gamma correction]
+ vpImage<vpRGBa> I_color_gamma_correction;
+ vp::gammaCorrection(I_color, I_color_gamma_correction, gamma);
+ //! [Gamma correction]
+ I_color_res.insert(I_color_gamma_correction, vpImagePoint(0, I_color.getWidth()));
+ ss.str("");
+ ss << "Sample_low_brightness_gamma=" << gamma << ".png";
+ vpImageIo::write(I_color_res, ss.str());
+
+ vpDisplay::display(I_color_res);
+ vpDisplay::displayText(I_color_res, 20, 20, "Gamma correction. Click to continue.", vpColor::red);
+ vpDisplay::flush(I_color_res);
+ vpDisplay::getClick(I_color_res);
+
+ //! [Histogram equalization]
+ vpImage<vpRGBa> I_color_equalize_histogram;
+ vp::equalizeHistogram(I_color, I_color_equalize_histogram);
+ //! [Histogram equalization]
+ I_color_res.insert(I_color_equalize_histogram, vpImagePoint(0, I_color.getWidth()));
+ ss.str("");
+ ss << "Sample_low_brightness_eqHist.png";
+ vpImageIo::write(I_color_res, ss.str());
+
+ vpDisplay::display(I_color_res);
+ vpDisplay::displayText(I_color_res, 20, 20, "Histogram equalization. Click to continue.", vpColor::red);
+ vpDisplay::flush(I_color_res);
+ vpDisplay::getClick(I_color_res);
+
+ //! [Retinex]
+ vpImage<vpRGBa> I_color_retinex;
+ vp::retinex(I_color, I_color_retinex, scale, scaleDiv, level, dynamic, kernelSize);
+ //! [Retinex]
+ I_color_res.insert(I_color_retinex, vpImagePoint(0, I_color.getWidth()));
+
+ ss.str("");
+ ss << "Sample_low_brightness_scale=" << scale << "_scaleDiv=" << scaleDiv << "_level=" << level
+ << "_dynamic=" << dynamic << "_kernelSize=" << kernelSize << ".png";
+ vpImageIo::write(I_color_res, ss.str());
+
+ vpDisplay::display(I_color_res);
+ vpDisplay::displayText(I_color_res, 20, 20, "Retinex. Click to quit.", vpColor::red);
+ vpDisplay::flush(I_color_res);
+ vpDisplay::getClick(I_color_res);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/connected-components/CMakeLists.txt b/tutorial/imgproc/connected-components/CMakeLists.txt
new file mode 100644
index 0000000..4e93dff
--- /dev/null
+++ b/tutorial/imgproc/connected-components/CMakeLists.txt
@@ -0,0 +1,23 @@
+project(tutorial-connected-components)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-connected-components.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/img.pgm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-connected-components.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/connected-components/img.pgm b/tutorial/imgproc/connected-components/img.pgm
new file mode 100644
index 0000000..f7136db
Binary files /dev/null and b/tutorial/imgproc/connected-components/img.pgm differ
diff --git a/tutorial/imgproc/connected-components/tutorial-connected-components.cpp b/tutorial/imgproc/connected-components/tutorial-connected-components.cpp
new file mode 100644
index 0000000..02f674b
--- /dev/null
+++ b/tutorial/imgproc/connected-components/tutorial-connected-components.cpp
@@ -0,0 +1,88 @@
+//! \example tutorial-connected-components.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC)
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+
+ std::string input_filename = "img.pgm";
+ vpImageMorphology::vpConnexityType connexity = vpImageMorphology::CONNEXITY_4;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--connexity" && i + 1 < argc) {
+ connexity = (vpImageMorphology::vpConnexityType)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input image>] [--connexity <0: 4-connexity, "
+ "1: 8-connexity>] [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ //! [Read]
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, input_filename);
+//! [Read]
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d, d2;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d, d2;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d, d2;
+#endif
+ d.init(I, 0, 0, "Input image");
+
+ //! [Connected components]
+ vpImage<int> labels;
+ int nbComponents = 0;
+ vp::connectedComponents(I, labels, nbComponents, connexity);
+ std::cout << "nbComponents=" << nbComponents << std::endl;
+ //! [Connected components]
+
+ //! [Draw connected components]
+ vpImage<vpRGBa> I_conn(I.getHeight(), I.getWidth());
+ for (unsigned int i = 0; i < I_conn.getHeight(); i++) {
+ for (unsigned int j = 0; j < I_conn.getWidth(); j++) {
+ if (labels[i][j] != 0) {
+ I_conn[i][j] =
+ vpRGBa(vpColor::getColor((unsigned int)labels[i][j]).R, vpColor::getColor((unsigned int)labels[i][j]).G,
+ vpColor::getColor((unsigned int)labels[i][j]).B);
+ }
+ }
+ }
+ //! [Draw connected components]
+ d2.init(I_conn, I.getWidth(), 10, "Connected components");
+
+ vpDisplay::display(I);
+ vpDisplay::display(I_conn);
+ vpDisplay::displayText(I_conn, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::flush(I);
+ vpDisplay::flush(I_conn);
+ vpDisplay::getClick(I_conn);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/contour/CMakeLists.txt b/tutorial/imgproc/contour/CMakeLists.txt
new file mode 100644
index 0000000..cb41e76
--- /dev/null
+++ b/tutorial/imgproc/contour/CMakeLists.txt
@@ -0,0 +1,24 @@
+project(tutorial-contour)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-contour.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/grid36-03.pgm")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/Contours_tree.pgm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-contour.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/contour/Contours_tree.pgm b/tutorial/imgproc/contour/Contours_tree.pgm
new file mode 100644
index 0000000..f7136db
Binary files /dev/null and b/tutorial/imgproc/contour/Contours_tree.pgm differ
diff --git a/tutorial/imgproc/contour/grid36-03.pgm b/tutorial/imgproc/contour/grid36-03.pgm
new file mode 100644
index 0000000..65b45f8
--- /dev/null
+++ b/tutorial/imgproc/contour/grid36-03.pgm
@@ -0,0 +1,184 @@
+P5
+640 480
+255
+������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
#&%"''$!" Jv������������������������������������������������oO6
,=@`v���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
3^����������������������������������������������^9
+@au�����������������������������������������������������~{yjkrghs����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+ )Uj������������������������������������������������������tfYSV`hlr{��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}G [...]
+/$-1*%)%#/,
,0 %.3 4Q�����������������������������������������d*#-*##&# )$'*%
;`�����������������������������������������������}Z3
%Ih����������������������������������������������������������~|~|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������B &*#!&& [...]
*Y���������������������������������������g"$(((((((((%%%(& a{��������������������������������������������iT'
$Pk�����������������������������������������������������uc[TNEBCNYbq������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+&( #+&&&&####!$$$$$$$%%''$
D���������������������������������P.$))((&&%%&&&&&&&&$&)))$"
+H}�����������������������������������z$'"$((((((((((((++(#
!p�����������������������������������������L )1/'%%%"####&
-h�����������������������������������������������qW3&
"-To�����������������������������������������������������������~zz}�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z5
$2)&&&'$#### %%%%%%%!$$'&&&&!o�������������������������������:6#))('''&&&&&&&&&&$&&)))))
L����������������������������������G++#$.&&&&&&&&'$$$&)+)'-:h���������������������������������������\
%)(&##**++++,,71$%P��������������������������������������������a?&
+
!
5p�����������������������������������������������������}rc\XUQPPS]m}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������O %!!&) $$$$#### %%%%%%%!!$$&&)++-h�����������������������������n2'#
))''''##$$$&&&&&)&&$$$$$7
#e�������������������������������� [...]
+,...,*))-,)***((
"h�����������������������������������������������~eH."
%Qg������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������+0$ &()&$$$$$$$$ $$$%%%%$$!!$&&(,3����������������������������~&&(!((%&&'$$$$$$$$&&&))&$""$$ 0*f����������������������������� [...]
+.P���������������������������������������������rX/
+
_t���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������y"$&&#!$)$$$$$""" $$$%%%%$$$!$$#&*/
&T���������������������������E.
+/(%%%&&'$$$"$$$$&&&&&&$''&)6,
*(������������������������������2 1)
"''"
&*''''''''!$(((('(.*2!!>���������������������������������j *
6&2+&&%%!((**))))-+(&&&'+
>{������������������������������������N/
(0//'$$$ #$&%%% #''#. >9y����������������������������������������_2$''-3)23)((-/++*01,"Qm��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
: -01(!!'0&&&&&&&&
"&&'$$&$&,(#b�������������������������������n1)$%,/'! $)02))&&''$$
#'$
%*
S�����������������������������������i&$-)##+(%#%%$$#')'$(+(#7&=����������������������������������������[7 ''$%#
"
$
&)%3�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+S����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������u# &'''$$$$'''$$$$$$!!!!!##&##$$$$$$,&:-�������������������������F
#%%$!$$$$####$$$$$$$&'$$$$&&&(*#(.G��������������������������R (&,((((''%%))&&&&$$&&&&&&&&!$(()&%
=�����������������������������d!''')'&&$$$$&&&&$$$$&&# [...]
,n�������������������������������o$((%%%%&#$$$$########$$$$$$$'&
P�����������������������������������s7$$$$&&''%%####%%&&&&&&&&%
"
y���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������l#%''$$$$$$'$$$$$$$$!!!!#####$$$$$$,&0j�����������������������f!#%''%'$$$$$####$$$$$$$&$ [...]
+))&$$$$$%%####$$$$$$$$$&$$$$$&&& )(%(&X������������������������c,$ 1##$$$$##$$$$$$$$$$&&&&$$$$$$$&))$$i�������������������������i$
'&&&%%((&&$$$$&&&&$$$$&&$$''%%%%/"#*Y���������������������������~X& 3!%%%%####$$$$$$$$######$$)$!!$''$'`������������������������������L-'..&&####$$$$$$$$$$$$$$$$$$(!!$'"Z�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V*&'$$$!$$$$$!!!!!!$$$##############(( .#Y������������� [...]
+Q������������������������~H
!!##$'((&&$$$$&&&&$$$$&&$$$'%%%#5
,
:v��������������������������U
#
0&&&&$$$$##############$$$$####!!16�����������������������������z
3" +!!$$$$$$##$$$$##########"%((! f������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������[(($$$"!!$$$$!!!!!!$$$#### [...]
+b���������������������������Z&%
$3-!!"$$$$$$$####$$$$$$$$$$/(
$*
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������n! &### # !!!!!$####$$$$$$+0 (04����������������������'''&&&$$#%%##$$##$$$$$$$&$$$$$&&&3
&2%",&I����������������������V
+0''(%%&&$$##$$&&&&))$$$$$$$$%'&&$$"",(s�����������������������*3"###&&&'$&&$$$$&&&&$$$$&&!$$$$$$$+"' +) &������������������������z?8
%)&''$$$$!!########$$$$$$$$!!$$$$&&
.J�������������������������w3C%0+'##$$""""$$####$$#########
),'$&g���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
D������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������L$
&&$&&&&&%%##
$$$$$$$$"$$$$$$$$/###.4����������������������G)'##%%'$%%$$$$$$###########%%%%%$&%%$$*.4����������������������E
++)$&)'''&#$$######$$$$$$$$######$$#&%(!+n����������������������w,/"%%((((&&&&$$$$!!$$$$$$$$####$$#&$!&!(#{�����������������������T#*$'"+#$$$$$$$$$$$$$$$$####$$$$$$$$####
#(
c������������������������`$*('$%%'$''####$$$$$$$$$$$$!!$$&&$$$$####/v�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+-,&%))''$#$$$$####$$$$$$$$######$$#'%'$) c����������������������f)/'%%((&&%%$$$$$$$$$$$$$$$$####$$#&#!&#(#
r�����������������������>+()") $$$$$$$$$$$$$$$$####$$$$$$$$#### %.S������������������������I1,$!!$$$$$$!!! #$$$$$$$$$$$"!$$$$$%%##$$$d�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+//+&&&''$#$$$$####$$$$$$$$######$$#'%%&&_����������������������],)#%%((&&%%$$$$$$$$$$$$$$$$####$$#&!!(#( l����������������������{1
++)")$$$$$$$$$$$$$$$$$####$$$$$$$$####.#%"Y������������������������=1,$!!!$$$$!!!! """"""""$$$"!!"$$$%%##$$* V�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
6����������������������i&(&&$$&&&&&&&$##$$$$$$$$$$$&&&&$''##$&+-:����������������������K
,/.('%''$###$$####$$$$$$$$####&$$$##&&%%a����������������������[0$ &&&&&&$$$$$$$$$$$$$$$$$$####$$$&$")$+# l����������������������n0.)$!)$$$$$$$$$$$$$$$$$####$$$$$$$$####- !)[�����������������������m4'&##$$$$$!!!##
""""""""$$!!!!!!$$####$$&& O��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+7����������������������%!%&'$$&&&&&&&&##$$$$$$$$$$$&&&&&'%#%&)++E����������������������S
&)*(''''$###$$####$$$$$$$$####&&$$##&&%(
g����������������������V,))&&&&&&$$$$$$$$$$$$$$$$$$####$$$&$")$+#
r����������������������m2/)$!+ $$$$$$$$$$$$$$$$####$$$$$$$$#### - R�����������������������S0$$##&&$#$!!!##
$$$$$$$$""!!!!!$$$####$$!&
N������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������q'""$&$$##!!!!$$##$$$$$$$$ [...]
+e����������������������b4&'$#!$$$"$$ !!
$$$$$$$$
!!$$$$####$$)$
P��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������4'$$$1$$%%%#!!####$$########$$- 44%(;m������������������������B15%#2''%%""""%%%%%%%%#######&((1-"8*0r�����������������������N%""0)##$$$$"
$$$$$$$$$$$$&&&&& [...]
+=t����������������������S4+(&%###$$$$$$!!!!!!!!$$$$!!!!####$$$$$$%%-!,e���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������I!$($9%%%&$$$$'####$$#### !$$$&8**%)s�������������������������w;$)7,!
&&&&&$$$%%%%%%%%######%%
+)/:(._�������������������������,5$
'$$''$$""$$$$$$$$$$$$&&& [...]
N�����������������������N4((%%#$$##$$##!!!!!!!!######!!!!!!$"!!!$$&!7p����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������.*
)%%$!!$$'####$$### $$$&$ %%5���������������������������n"#,#
(&&&&$$$%%%%%%%%######%%+2, '-.��������������������������00-.,$%''&$""$$$$$$$$$$$$&&&& [...]
b�����������������������Q4'%%%#$$##$$##!!!!!!!!######$!!!!!$"! !$&)
P}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������`/1'"$$ #!!!####$$######$$#&%%"(11
g����������������������������K.*&''%%'''%&&&&&&&&&$$$$&&&'#+!1)
O��������������������������`
2+(!%%&&&&""&&&&&&&&$$$$&&&&&&%%%%%%
T�������������������������X+/))''''&#%%%%%%&&$$$$$$##&$$$$&%%.%$X������������������������z7,)$' ################$$$$$$$$$$$$$$$& +������������������������S 5%%%&#####$$$$!!!!!!""$$$$$$%%% $$""!#((
i������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t
$"$ ##$$####$$## [...]
(R�������������������������������������k6
+.((
")))'%%'+#()&&((#"<����������������������������������D&&('%,$!#(('''*## &+&"")% S������������������������������y5
-(&-%#%%''%%#((((%$$ $+.+$"#% -
3}����������������������������`
2/!#$(((&" ##$$%%% '%
$&))(1#E���������������������������\,"'/!.%##""!!!!########''$!!!$'#)&,*
n��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+!''+'&&%%0%
`��������������������������������������<
(.$'%"""%&&"%%%!!&*0#!I����������������������������������C'
,*+-(((#$&&&##$&&&&&/,*/3.'
h�������������������������������J+& 0'''$##$$%%&&$$####$&&((('*&(-
w�����������������������������6
+.&$
,!&$$##$$""########$$##'''' )8@f��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������fNSA;A83=E=JUa{���������������������������������������������aN&&)+++++'&%% '/:4&+ at g���������������������������������������sO(*/4.
*'%%(*++""'.-''*:.((Uy�Ȫ�������������������� [...]
#-+&&%%!
#!$*,"
6e��������������������������������������E'$
A#+())(((*((''&&&&!(/2$
m����������������������������������w20 **(()))+&&''&&##&&&&&&$$ .3 ]�������������������������������u9#
(-#)&($$$$$$%%$$$$$$$$$$$$$$""@0G��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������u\TNB [...]
+2(#%&&''&$(('###&&%+2/"5M������������������������������������O-0!%%%%&&$$&&''&&##&&&&&&%%*-"&S���������������������������������k
(
#$&''$$$$%%$$$$$$$$$$$$$$$$- !Nv������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+&&)%$))"
$(&$$
Y����������������������������������������$
8&(*&'''*))&$$&''$ %*#
+^������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������gN=<98..132 [...]
+
+5[|���������������������������������������������N( '(-/**-*% !$($ #[}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+
!
+,Vy������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+ :j�����������������������������������b������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+,n�������������������������������������������wW?
+
+/Uw������������������������������������������������������������������������������������������������������kbt����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
++&&((((''''''''*(# 8s���������������������������������������X##%'&(''#G|������������������������������������������������x^H86004B[l}�����������������������������������������Yt�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+##%'&(''&"!My���������������������������������������������uH:"
!7Vx���������������������������������������YW�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
Tv����������������������������������������`,
^�������������������������������������d������������xgZI;IIOY_i}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������c(((&####&&&&&&&&&&&&&&&)#
#01#i����������������������������}
+,,+2*##&''''&&&&$$&&(**(%%)!
���������������������������������j (3)#&%'%%%%$$*%$)+-,,+���������������������������������������Z)
%*.//$'+,8{���������������������������������������������x]?-% @Xz����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$#&
%%&&&'&!!#&&*/P������������������������������������������aA
)#
+ Pu���������������������������������������������������{e`]ZVV[_v���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Q &&(($$&&''&&&&&&&&&&&&&&&)((&#()'!(��������������������������; -1/)$%(*(&''''&&&&&)))%%%((',#0�����������������������������r " [...]
+!$''.)&&
+
z������������������������������������������������mTA*#
!$Bc{������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������.0+$&&&''(*&&&&&&&&&&&&&&&)%)($#(*'o������������������������� *,+(('(($
''''&&&&%'''%%%(/++$_���������������������������\
$,</!!!$! [...]
+
,u������������������������������������������xP1"
# #Bt���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������%0+"&))$ %(&&&&&&&&&&&&&&&)!)*&&..*"@�����������������������q "!' '())'%''''&&&&%%''%%(*$*
B+## 3���������������������������#E")$&4.
.''''&&''''#
#*( ($,/
R������������������������������S +!!+)&&%'**& $%%$!$%%08E���������������������������t_{qx���G :!,,)))'!&&"
$$(&%Pq����������������������������������������mF
'%),(##*
+7e��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������+2!&&&&&&&&&&$$$$&&$&&&&&&&(())((''2&[�õ�������������������z-
(&*)'''''''&&&&&&&&&&&&&&&&&(**))&&C���������������������f
3&&&&$$##$'&&&&%%%%&&&&''))(++ v����������������������������H''(((%%% [...]
+.��������������������������������c@#('$''''&&&&&&&&%%%%'$$&%
m�������������������������������������\.
'+*(((%%%%$$$$)'$&$$g�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~'.$&&&&&&&&&&$$$$&&$&&&&&&&(())((''+$
E����������������������e
&(&)'''''''&&&&&&&&&&&&&&&&&&(())))
+*������������������������z7 (#&&&&$$##$'&&&&%%%%&&&&'')'%(+#K~��������������������������W
''((%%%%####''''''####''+$#&$
+
+f�������������������������������-((''''''&&&&&&&&%%%%'$$&'
Cm�����������������������������������i
"'((&%%$$$$&&'''#$&$#
�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v$+)&&((''&&&&$$$$&&$&&&&&&&'(((((''+&0����������������������N)).('%%%%%&&&&&& [...]
+
h�����������������������D''0&&%%$$##$'&&&&%%%%&&&&''*''(('
Y��������������������������
(&(()&&&''####''''''####''&*( $((g����������������������������i7
).&$'''''&&&&&&&&%%%%%##$(( '���������������������������������w-&&$&))'$$$$$$$&&''&#$&((#
W���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������s(++&&((''&&&&$$$$&&$&&&&&&&''&&((''+1 r����� [...]
+####&#!!####$$$$$$$$####%%$$$$$$+ 2����������������������������xA)%3'%#$$''&$$$$$''''%"##$$%%+ 8��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������3+%&&''&&&&&&$$$$&&$&&&&&&&''%%%%&&'.$&A���������������������<
21',(%%%''&&&&&&&&&&&&&&&&&&((%%&&&&,' S������ [...]
+$'3&&''$$$$$$$$&&&&&&$$$$&&!($
"*(!
m�����������������������v)$""#
#''####$$$$$$$$####!$''''''$'b��������������������������X".$+ (###&))$!&&''$$$$##&&&&)) #)
n�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������I4$##))(($$%%%%&&&&&&&&''''%%%%%((*!7'";~� [...]
+H~������������������������T!,,+)$&%(####$$$$####$$$$$$''''##' -0&
)f������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������X// #))((&&%%%%&&&&&&&&''''%%%%%((*%6'";��������������������B,)(0**,,&&$$&&&&&&&&&&&&&&&&%%%%&((*')3���������������������@###%#&&%%##&&%%%%### [...]
+3u������������������������;
,*))$&%(####$$$$####$$$$$$''''##+!(+&
G������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t. 0%%&&&&))%%%%&&&&&&&&''''&&&&%((*,/&&D���������������������V&*')(())&&%%&&&&&&&&&&&&&&&&&&$$&((*#,3���������������������H'
%"'#&&$$$$%%&&&&$$$$ [...]
"''&&&&))%%%%&&&&&&&&''''&&&&%(((,&!&P���������������������j#*)
(())&&%%&&&&&&&&&&&&&&&&&&$$&(((#,7���������������������S'
*#*'&&$$$$%%&&&&$$$$$$$$&&&&&&&&&&%' &\��������������������z)
(, )&&&%###$$########!!######$$%%&&$$*#
����������������������=55#%$$$&%%%################# "$$$$##1( R����������������������~P(!**''$$%%##########$$$$$$$$''''##'$($-'������������������������������������������������������������������������������������������������������������������������������������ [...]
+1*!$$#&&&%$$$$$$$$$$$$$$$$$""&&''''#()
E����������������������g(%(((''&$$$$$$$$$####$$$$$$$$''''####,!++w�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������=&&((&&%#%%%%&&&&&&&&''''''%%%'')*(&x����������������������Y!2&(())((%%&&&&&&&&&&&&&&&&''$$$$''&#T���� [...]
*&!+$$&&&&$$$$$$$$$$$$$$$$$$%%&)))''*!&&D����������������������S+.&&''&$$$$$$$$$####$$$$$$$$''''###',!&&m�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������l.$&%'**%%%%%%&&&&&&&&''''''%%%'(*../�����������������������|/.*&&))((%%&&&&&&&&&&&&&&&&''(%%%&))"!e����� [...]
+U���������������������p1
#/$''))####$$$$!!!!$$$$$$$$!!""$$$$((*
X���������������������v2
0
%%&&''&&##%%&&$"####$$$$''''''''!!&#*& r���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V"//',"))(($$%%''''''&&2,,.%%-0&_��������������������������Q(0*+&#,%%%%%%%%&&&&&&&&$(.*%%''%������������������������U!&1,*((((&$ ##$'''''''''''''*%'%#(+& |�������� [...]
+#a���������������������u6'/&''))####$$$$!!!!$$$$$$$$!$$$$$$$$&$.f���������������������n) 0$%%&&''&&#&%%&$$$####$$$$###''''''!&#&# y����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z ,4 0$$&&&&''''((''(( $(181(0
,����������������������������0)(+%(*'((%%%% [...]
-++*('%$$$$&&&&&&&&&&&&&&'*'##*+&�����������������������r2!$,
))))('&&##''''######''##$%%%%"$$
Az���������������������{1,,$%%''##$$$$$$$$!!$$$$$$$$%%$$&&%% $Mv���������������������l, 0%&&''''%%%%&#$$$&####$$$$##$$$&''($'"'
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+33$&%%&&$$$$$$$$$$$$$$$$$$$$''''''%%.. y����������������������s7.&&&''''%%(%##$$&&####$$$$$$$$$$$$+
$*/$ *�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Z# $&()++)))'(''))!&'..!'?��������������������������������=('&)+%((((((%%&&))))))'')-$
(�������� [...]
+5z����������������������������������`$
+5)
((((((((''''''''##**
Jr�����������������������������o"**''((''%%%((((%%%%%%%%%&1''-D���������������������������R$",&&((('''$$''''$$$&%%&&&&(%#'*( 8k������������������������c'.&""%%&&$$%%&&&&$$$$$$$$$$$$$$''''&&%
+
D�����������������������|1.)&&''''%%#&%%&$$$####$$$$$$%''''')$+ D}�����������������������������������������������������������������������������������������������������������������������������������������������������������������������u�����������������������������������������������������������������������������������������������������������������������X')+,**&&****+-11+"5:2_������������������������������������F
)&3((((((((''''''''.*0*(s�������������������������������9
+**'&&%%%%%((( [...]
2$&&((('''$$''''$$$&%%&&&&(##(.&b�������������������������y;5)))%%&&$$%%&&&&$$$$&&&&&&&&$$$$''&&1
,f������������������������< .%&&''''%%##%%&&$"####$$$$&&''''''$$0
]�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������r;"
>p���������������������������������������}
+2-%%*0..(/'!*,,0
R����������������������������������]<&'*
0&&$$$)+((+/,)')-
+A
������������������������������T""&/(3"+"&&&)('''
#''(&)+& $(
8���������������������������t0&+0*$!"%"""$$$$$&&&&$$$$#!1#1 ,)r�������������������������:
/-#%& "&$$################
!$&&$&&1!/,����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+3U���������������������������{@#-3
"$''%%$$$$$$$$&&&&$$$$9 &)A(��������������������������R/110*&"""################$$$$$$$$!1'#����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������xqqt{��������������������������������������������������\
+
+
X���������������������������������������n-##''##%*,'&(-2/+7����������������������������������4
#+$1.+''&&&&&(&$##(+.1-* #������������������������������\ 5/.*+*$###'''''''''''$$$$)&1..'s��������������������������s93**))####''############'')'&$$&''*4D|�������������������������������������������������������������������������������������������������������������������������������������������������������������������������~���������������������������������������� [...]
+",)
+ Ro��������������������������������������>
8'00%"##'')))&&&')**(z���������������������������������0(&$'(!!'######''''''''''.
5[�����������������������������}0
4.),(
$&&&&&$$$$$$&&&&&&$!$'())' R���������������������������������������������������������������������������������������������������������������������������������������������������������������������������}���������������������������������������������������������������������������������������������������������������� [...]
+4z��������������������������������������/
+')*,&&&&&&&&&&&&))))".x���������������������������������P( #*"#-&&&&&&&&&&&&&&&&'$$&((#C�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������{������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
4Og���������������������������������������������]0
'-/)!!',$&))$
)m�������������������������������������rF
++), %##-33*
***'''&&
#/I������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
"%''().3,(**-+#
Iw����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
(8]k�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ 8g��žð����������������������������������������zj[LKQQ[k�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
!$>`�������������������������������������������������~zx{���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������zv�������������������������������������� [...]
+0X�����������������������������������������������zpjc`bkt{���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
Y�����������������������������������������U
+ !\v����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������i (/-&&(+))+-(((****'+
)-%(,0#N��������������� [...]
+
d��������������������������������������{C
+ew��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������
1/,)('''&&&&''''(((()&'''(''$8$);��������������������������
',*&*.((***''%%%%%''''*&"(($
5u����������������������������_/&/*(%%'''''''''''''(++ -�����������������������������������m
'.,,''&&''((+
@m�������������������������������������������eP5*
*;Ew���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
-/,&('''&&&&''''((((&&'''(''&2'+
�������������������������h(,*(**(((*''%%%%%%'''''&$(+&
D����������������������������< +/*(')'''''''''''''((+%-H|��������������������������������]$))((''&&''((-&! -}�����������������������������������������hD$
?b����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ <m�������������������������������������������������~pkcafq}������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������Z&(,+$('''&&&&%%%%((((&&'''(''*#/,(
h������������������;((((++)'''(('%%%%%%%''&&#(**))-/
1}������������������������F [...]
3^������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������0( (-*(('''&&&&&&&&%%%(%%''&())&.213 #����������������������$))&&&)))))&&''&&&&''&&&&'('%$$&)'8�����������������������f'.#%''('''&&&&&&&&&&&$$&&&0("%%#��������������������������/ [...]
2)%%&%(/ $$$ . 3
)>w�������������������������������������t=
+
'%('''%!
I�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������5%)$+%'('''&&&&&&&&%%%%&&&&%())$//!-+o��������������������p(**((&&)*((&&#&''''''&&%%&&&&%#%%2s���������������������l
)<#%%%%&&&&'''''''''''$###',(
6 o����������������� [...]
+7���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������> ,$+ '('''&&&&&&&&%%%%&&&&%()) +((- R��������������������`)*-.*(&)*((&###''''''&&%%&&&&%'%%6
Y���������������������Z#,A'%$$$$##'''''''''''$####("%.%+
K�����������������������-
* &&(' [...]
+++(((%'$)&&&&&&&''''''''&&&(**(%'+. V��������������������.
-&('(((((%%%''''''&&&&&&&&%%)'$$'(** ���������������������<(!,.))''''&&%%''&&''''&&&&''$$$$)0#/!V����������������������80(4+
&(+''%%%%&&%%&&''''''''&&%%&(,,!�������������������������P:=$
'3''''''''&&&&&&&&''$$%')) 2G��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+/*+$(())(())''''''''&&&&&&&&$'&(''((�������������������b"0'*/&&(''$$&&''''&&%%%%((((&&'' #( )(��������������������~6&,)#($(3''%%%%&&&&&&''''''''&&&&&#.&
l����������������������m((8#)$/.((''####&&&&&&&&''')'''%+
)-�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+��������������������k +''+'&)&&%%%%''$&&&&&&&''''''''+,%$$"��������������������k%**((%%&$''''''$$$$####&&&&''''&&&&##+
b���������������������d
9'**(&%%&&$$''&&$$$$$$$$$$''''''''&&*. d������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o
/%1-*, [...]
��������������������������?#2&2&&&&&&&&((((((((%%''++((0) D�����������������������l
8#''))((%%)&&&&&&&&&&&&&&&)'((($%i���������������������T#
(+#/&(&&%%''''$&&&&&&&''''''''&".
#`��������������������[(*((%%&$''''''$$$$$$$$''''&&&&%%%%$%%(w���������������������20')))&&&&&&&$$$$&&$$$$$$$$$$$$$$$$ * #S������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+a����������������������z'*+!1&(&&%%''''$&&&&&&&''''''''&&)$>��������������������g (,((%%&$''''''$$$$$$$$''''&&&&%%%%%'%(���������������������5+')))&&&&)&&$$$$&&&&&&&&&&$$$$$$$$#' #Y����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t%
$/1)))+)))&(())****4* t�����������������������������W'*+((())))))))))(((%&)+++(
��������������������������j"(((((+)$$('''''''''''''''(#$&($������������������������2 /0 0$)''$$&&%%$&&&&&&&'''''''')+", i���������������������u/**((%%&$''''''$$$$$$$$&&&&''''''''')%+���������������������|8)+))&&&&))$$&&&&$$&&&&&&&&&&&&&&&&&##( f����������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+���������������������{8
&/))&&&&))$$&&&&$$))))))))&&&&&&&&'%/ p�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������_#
),%2%()..#7����������������������������������_
+
+","0+0**((((&%-&(.*
+j�����������������������������
)1.*(!
(!#((#%1%())&&)/))
+ E�������������������������{(,%/
'/+'')+& %%((((''&$$$&)&&3+ :�����������������������T/'&"('')+((##%%&&%%&&&&&&0'#*.&#$'"/T���������������������75"))))(%##&&&&'''$$&&&&&&&&&&&&&&$
)/)���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+D����������������������������������"
.**(%).)%%,3.' $'.7 %n����������������������������-22)#)11(
!+))&'$''$+2-++-? ��������������������������*2*')&#$(''$$%%''&&&&&&&&('!$+-)#4z�����������������������;
&+'%%%%'&)'''''''$$&&&&&&&$$&&&&&&& /',����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
&++&$&)'4*&1:2
+a���������������������������������
'46) ""$+##$$$'''-+&$(/47d����������������������������L
",,! '''$$$$''''''''''"+-"2<*`������������������������P8+++&$$$&)&&''''(#$'''''''##''((((/0`������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+)+-1*)+
&$
&.)1����������������������������������Q
'
+2+
'&&))((('(+--(# ������������������������������)*4,'#''$$$$''''''''''+$")2+&"`�������������������������u&8'-+)&&&&&&&''''(#$'''''''##''''''%
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+7����������������������������{7%%%%&&&&''&&''&#$'''''''''''''''R}���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
P������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
+
<e��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
#)Dd����������������������������������������������������������������������������������������������������������������������������������������������������������|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+##%**(''((%"
X��ĵ���������������������������������������rhZF>GV`n{��������������������������������������������������������������������������������������������������ap���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
K����������������������������������������gV-$
)CYu������������������������������������������������������������������������������������������������kv������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+C�����������������������������������Z
'*$
%b�������������������������������������������������������������������������������������������rQn���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ =d���������������������������������õ^]������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������/%%
#4''''''%%%%&&(())#*# /2,,������������� [...]
+
+T����������������������������������lp�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������R24%"-/''''''%%%%&&(())&(#%(
.%
{����������������������,&:(/*&&(*+(+'&&(+**#+'
$15/+ D��������������������������! (($5(1 %/+(%%%&'## $0+),7��������������������������������\-$&1.,(*.%"'.,z���������������������������~��������������jSJ6,+/25Vg|���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+
++Rm������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������&
&&(+)$''%%$$%%&&%%''&&%%&&&(((1(/���������������������`
+:
+%**((((''&&&&%%%%##$$''&@�����������������������_A--''&&&&%%%%((((%%%(((((2' <����������������������������##))+)(())(())((''((&++Q~����������������������������������U+ $$%%%%&&
+Fl��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{
"(()))'''%%$$%%''''&&&&&&''&)))*'
+n�������������������u%
!8#$*+))))&&&''''''''&$$&%%''&2(]����������������������0%(*''''%%%'''''((('''''''' 87 r��������������������������M 11$'))((&&''**((''((&!')E���������������������������������S
+
'++***,,,)&
}���������������������������������������������xpmbX\kyzz������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z
#(()&''''%%$$%%''''&&&&&&''&)))'#K�������������������a &*(&*))))&&&&''''''''&&&&%%%')+!-=� [...]
+
+
%2dx��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}*$&&''%&&''$$$$%%%%&&&&''&&&&&)))*%% 6������������������T3."2+'((((&&&&''''&&&&&&''%%'(&3
-���������������������W%.((((%%&&&&&&&&&&$$$$$&&&#* &*8���������������� [...]
+^����������������������������U.+&'
))('''##&&'#$$''Ey������������������������������������h<
+
+
+
|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������G !$''#$&''$$$$%%%%&&&&''&&&&&)))%%((*o������������������E52&2+
(((%&&&$''''&&&&&&''%%''&3
-e�������������������q#0 ((((%%%&$&&&&&$$$$$$$&&&.(&$������������������� [...]
+Mz������������������������������~D $.4%$*%/'&&&)''(.
R������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������(*" %&&$$$$%%&&''&&&&&&''&((('0
*o������������������U$+,)0((''''##&&&&&&&&(%''&&&&)
0$
Y�������������������J$1& [...]
)4%..&&&&''''''''&&&&%%%%%%&'--
,
$>|������������������G0&))&&''''&&&&&&&&&&''''''''')%%((>��������������������x+++((%%##''''''&&&&&&$$$$&&&&&&&($N����������������������Y( 2&&&''''''$$&&%%$$$$%%%%%%-$, o���������������������������>'
'.(&&&&&&&&&&&&&&&))'*+#
<����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
>�������������������?1).
&&''''&&&&&&&&&&''''''''''%%((
({�������������������X#
+((((%%&&''''''&&&&&&$$$$&&&&&&&(%!4����������������������6/.. &&''''''$$&&%%$$$$$%%%%% 3,
;}������������������������fB,0+#(&&&&&&&&&&&&&&&)'%'++$
+ X���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{
+70
1''%%%%%%&&&&&&&&&&&&&)))!0"Q��������������������1"" -)#&&&&''''''''&&&&''%%&&&($1$0 A�������������������F.)0
''&&%%%%&&&&&&&&''''''''&&%%')!
m������������������|+!&((((&&''''&&%%%%&&&&$$$$&&&&&&&(') ���������������������o/0)"''''''''$$&&&&$$$$$%%%%%&"+&I������������������������@ 33#:&(&&&&&&&&&&&&&&&+)$%+-+) +h���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v1 [...]
+���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������N*&%%&$$$%%&&&&&&&&&&&)))))'2 ����������������������2,*-'&&&&''''''''&&&&''''''%'*#*7~�������������������x'8$
''''''%%&&''''''''&&&&&&%(%%))
0w������������������m
-'''''''' [...]
+��������������������T3'0+*!%%&&&&&&##''''##%%$&%%%%-'$&&
p��������������������&1% +/('''''''''''''''""'*(($$$9!#�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o4+&(())))&&&&&&&&&&((((**i������������������������l$ [...]
��������������������Q0'0(/%%&&&&&&##''''##%%$&%%%%+$+(1( p��������������������[,
+.+*($%'
('''''''''''''''"''$## #%$!a�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������yD,(/-&2&"*+'(..,#*# +(-7
��������������������������G-5#/' +("-''''&&%%+& &,**/)-(o����������������������-
(1-*(#*,%''''''''$''(((((-"/# [...]
|�����������������������N,(%##%**,4''''''''''''(((()"3
*$ (���������������������h%
&(0%#&*%%&$%%$&%%%%&&&&((((((**
)"��������������������H3)00
)'''''$##''''''''"$%%%%''&#'$'* �������������������l(.)&&&&%%$$&&&&''''''''''%%&&((''&&"4' B����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+#1##-*--..//,,-' (.'#R���������������������������/&!$((''(''''''(((''''((5(!$Z�����������������������1:)+$'4*'('&&#'''''''''''))&&&&&& ~��������������������S7*- 2''''''##''''''''#%%%&&&&) 0$++ ��������������������P
6-(((%%%&&&&''''''''&&&&&&&&((&&&&&
%
G�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ v�������������������������#
+00'((((('''((((((((#&&&&&&&$,
4 ����������������������h$)+**''((&&''&&&&&&&&&&%%%%&&''''''%% y������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~bWOKF?=EAO^���� [...]
+ L����������������������������������g
432*# $',/$&(((("&$05i����������������������������D
.'*)'$%-4%%%%''%%0-(" %),'7E~�����������������������[&9.$ )+$
&&''&&''''&&''&&!)/,$$+3)8-������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+6�����������������������������u02*)),,)#
('&$$'%'%&
"+* &)�������������������������*"0+&+*&%'&&''''''''''''&&'%#(+)
##1z����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$ %T|���������������������������������V15,&()&&+)'&$$'%'("%*+(1=y���������������������������r
==*!%-,&&&&&&''''&&&&&&%%))%%-<(�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ ))
!+,,,,++**"
*.
Z}�����������������������������t $),&%(#&&&&&&''''&&&&&&' '24
i��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+//''''%%%%''((''(())#*/i��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+().*#!$)"'+'(&
+n����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
&
&97Ux��������������������������������pN������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
,�������������������������{��Yd��������gL>5*%$)9FYb���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+)$)4%))))(('*%%&&''$!e�������������������������_J���y5
+$$
"" j��������������������������������������������z��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+#57-&,.)('-+'00 Y������������������������������������eA!
+ )Ei�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ $Ns���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
.4'$+''''''''$(++$ b��������������������������������L0'
Hf������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������;5"''*('''''''''''#''''''(((&+.( [...]
+p�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������?&,''(('''$$'''''##'''''''(1$& 3%0������������ [...]
+2.+21+**0--2/ S|��������������������������������������������zz{}������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������G/#%%'''$$ &&$##$$$&$$$$&&&&/
%R������������������W%
$-'''$###'''''''''%$##$'''+!-+ [...]
+w��������������������������o
20)),,/2+!
' '&$%**%)
Q�����������������������������������^-
+ W����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w6&##+&
*2$&&&&&&&&&&&&&&&&)''%'F�����������������]).''%%&&####''&&&&&&''''&&+( +-+�������������������N""&&&&&''&&''''''##$&&&&&&&/$**&D��������� [...]
+
)/-*&&&&&&&&&&&((((( )4$x���������������������������������4$
*(('#
!Ep���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������U! 6# ! */$&&&&&&&&&&&&&&&&)#-#%&*H������������������e,)''%%&&####$'&&&&&&''''&&'+!((+
������������������1"&"&&&&''&&'''''$##$&&&&&&&+ '%+!/� [...]
("&-#&&&&&&&&&&&&&&&&&&' +#'4
M������������������p )$''&&''##$$$&&&''&&''''&&!-'(&+
t�����������������x')#''&&''&&'''''$##$&&&&&&&'"%#/+
��������������������%6((()&''&&&&&&''''%%&&&&'(-!$$3( ����������������������m"*6(!''&&&&&&&&&&&&&(((% .6&����������������������������f% +/2.)*-)),,++**$/6/
������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
$, #*#''''''''''''''''-'&$6+��������������������V$+''''%%$$$$$%&&&&&&''''&&'+"+//�������������������; 0 &&%%%%&&'''''$##$&&&&&&&&+."(% y������������������@
,&)&'$$$$$$$$$$$##%%&&''&)&)-!+- E��������������������1 (%)) #,%''''''''$$$'''''!&((-53(
�����������������������\#-<$$%%$&**'%**)))'%$ &+))*00"2}����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
"v������������������`))0$(%',&&&&&&&&$$$$&&&&1)!
'(&����������������������V-*-&%%(()))$
(())((%%$'(( !))
@�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������M%(&&''''''''''''''''08!<����������������������w
((&&''##$$$'''&&&&''''&&(('3)
N�������������������|!&)%%&&&&&&'''''$##$&&&&&&&*!""1% �������������������;'
! [...]
911""&(('$ .#"%"%.+3"8 ������������������������5
2
,6
#3"''&&''&&/$!&'
#&$3 #:'x��������������������5 3
&*##+) %$$$$&''&$$$#%%%)-$9
�������������������M$))&&&&&&&&&&''''''$$&&''((((+1$;|������������������E
+5$'')&&&%'%%%%%%%%%%%%%%%%$&%%''''o��������������������Y
'+'$!%%&&&&&&&&&&&&&&&&&&&&&&*&���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������O1/,(# !$$(.1,*(%#)(d������������������������}A
2,"1))))(((($' [...]
X�������������������m,&" ')$$$%%&&&&&&&&&&&&&&&&&&&&&&,\���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������P
.+&&+++(
'##//$@.d��������������������������{+!0+&+'$$$$$$$$
%(###$'#1:C ����������������������u&
+/ "'*'
$''##$&&&&&##$&%%3&"
(g�������������������|
0((''''''''''''''$$$$&&&&$+$ *' W�������������������D '4'')'&&&&''''''''''''''''&&%%((((
Z�������������������?3,&'('$(&&''&&''&&&&&&&&&&&&&&&&1) E���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
+e���������������������,'((('''''$$$$$$$$$$$$&&&&(+)- u�������������������H+''')'&&&&''''''''''''''''&&%'((*(o�������������������>8/''('%(&&''&&''&&&&&&&&&&&&&&&&&)+ -w���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
++!)))))&''$$$$$$$$$$$$&&&&&($$+"��������������������A7&(()'''&&''''''''''''''''''''(((( z�������������������M2+'**%$'''''&&&&&&&&&&&&&&&&&&&&
/ o��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������hF
Ty�����������������������������������W
+
''(((())&#(//'Q����������������������������N&'''%$''&&&&((&&&&''((.+ W����������������������t
+))))))''''''&&&&$%%%&&((3(
+3��������������������A70(()'''&&''''''''''''''''''''(((( ��������������������M+(&**%$'''''&&&&&&&&&&&&&&&&&&&&,$$
+&r���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~k]VSWVSWZn��������������������������������������L$+++**++++1)*4-
.%������������������������������3&!/*
*&&&&'(((''&&%(''
*
+ \������������������������U&(((((())''''&&&&&&'((((()&)4+)\��������������������^/,((*(''%%&&&&&&&&&&&&&&&&(*((''((��������������������;*&#'('$&''''&&%%&&&&&&&&&&&&&&&&(7.n������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+Q���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������_M748612/6:32Lq�����������������������������������P
+0&0% '&##&$&+))=U���������������������������m< 1#)
((%%$$%%"(#$.(#,--(0 �����������������������L-&-4
'3$"'%%%&&''&&'$$$$&**>#8!t��������������������H
+&10%'$!(&&'$$$#'''''&&&())+)'')*)4 U��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~vst{�����������������������������������������{6 (##)+&+-(( [...]
9. (($(&&'$$$#'''''&&&(')))#
��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������V=2#'!
$))
!8F��������������������������������V!.'1'%!$(*'$ 4-())8:y�������������������������=:-&$(!$((&&''''$$'(((('0($ 13
����������������������P+%#*-# +''&$##%%''''&&((#$((((((74������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
.������������������������������+%,)<"*'(((''''(+'##&)))
/:- y������������������������K/&((#13 ))&&&&%%(&&&'(((++'$$$''59����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+-&#'((((''((+'#')--+3,d��������������������������
+8%#.#))&&&&%%((&&((((+('(+-00��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
u����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ *K�¯������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
|������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ ?_������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
+
Rr����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+**:///(!
#%%.)(%&+034h�������������������������������w<
>`����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
D�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
$
#--++))++*#+1+w������������������������������������zbZB=528DRZx��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+
&"$+''''''''$&(($ q����������������������������������a8&#
+
Pp���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������\ *,#' '''''''''''''''''%%&&%%'' [...]
+#+#-+++****
e�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������M
1, #''&&&&$$$$$$$$&&&&' [...]
-CVr�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������N)''')$&&'$$$$$$$$$$'&&&&'&&&%(#/"������������������*
''## #$$##$$##$$##$$$$$$
+($( k� [...]
+
-2+$%%%'****(5&K���������������������������������������vXMHOSOPV���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������E-"%#################&$$$$&%%&6 ������������������,**%%%#$$##$$##$$##$'&$$ [...]
1x��������������������������������sD=n����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������b
"%)################$$$$$&%%+)$������������������&
+####%#$$##$$##$$##''&&$$#&!$20$!5�����������������p28(&&'$$$&$########$$## ##
'
$+.x������������������K .(1$#$#(* (('#####) $&$'*
g�����������������������# 1&$)%
++(($&'#0/
,%
�������������������������������h
@���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� -!;
!$$#&&&&&&&''&&)&! #$''&, $������������������ ,(&$$++&$$$$$######!!##(
&%(+$8����������� [...]
,��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������F<-,)''&&$ $$##$$$$ '(((++74
F������������������P 0$('
3&$$$$$######!!##$$++%%)$
D�����������������h
+$ ################ #*$ S���������� [...]
J��������������������I
$&&&&&%%%%''''''&&&&$$$&%((()U��������������������������B
+")++(&((''%%%%))++n��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������[ 7+$# #'('$####$$$$
#(($!$)%{������������������z
+,
'7 '$%%$$$$$$$$$$ &+'"$(%]�����������������s
+)%"$$$$$$!!! ######### !!#.
K�����������������}*!!% &$$$$$$$####$$$$####''''''-
/��������������������&*+&&&%%%%'%''''&&&$&&&&''''%)����������������������§P$&)++)''(((&&&&&&$'/-&+��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
" %&$$$$$$$####$$$$####''''*%1(
�������������������`
*&((&&%%$$%%''''$$$$&&&&''''8y�����������������������'-++++))''''&&&&&&&$$)221/
g��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������0
-+*-& '$ [...]
#%%+'$######!!!!####""##''''$((������������������l+
($ $"''&#$$$$&&&$$$$$$$$&')))'2=���������������������X7/$,++))''''''''&&&&''&$$&))14������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~������������������������������������������������������������������������������������������������}&! %$
'% [...]
+$$$-)
'$##$$$$&$$$$$$$$$$&')))-%
m��������������������% + .&&&&''''''''&$$$
%*($$$!))������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������}��������������������������������������������������������������������������������������������������< "&$"*%%&&''&&%%(*
#�������������������������l (6&#$$## [...]
+
E��������������������y:#%"######$$$$$$##!!!!!!##& ' ]�������������������!*
#&$$$$$$$####$$$$""$$''&&)"-+
������������������O2%'5''##$$$$##########%%%%%%%% %))^�������������������d
!:/5&&&&&&&&&&&&$$$$%))')(&.
?�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������y����������������������������� [...]
+#��������������������L44"
!$&&&$$$$# !!'%##''% .
(+ B������������������Q&$, ''$$####""''####!!$$$$$$$$'-
Q������������������L
*)''+'&&&&&&&&$$$$$$$$##&&''%%'%$(\����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������qu����������������������������������������������������������������������������������� [...]
81($$(')((
%)) %40a��������������������X'&,$))&$$ !
$$$$# !!'%##%%" .:
T������������������R"4,*$!!$$####""''####!!$$$$$$$$" -O������������������G,*'%('&&&&&&&&$$$$$$$$##$&''%%"'&&
+<����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ss��������������������������������������������������������������������������������������������������������}rQ<)'$#2Fi}������������������������������������K!# "&%$
[����������������������������" ,
!
(42&
.��������������������� [...]
+**+)(&&&
%.( >������������������������3 *##$###$&$$$$$$##"$&&##'(+$x�������������������g
$$0"-''####$$########$$$$$&&&$+$ <z������������������?..% &(&&&&&&&&$$$$$$$$$$$'%%&#("%#&����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������zt������������������������������������������������������������� [...]
B]����������������������������������R &"&%'$$!
/$
'3.7��������������������������r
+((++)$!#$$$$$$$$!!$&&''&)6
6���������������������M,.*&&&&$$$$########$$$&&&&&'-
_�������������������@..% %'''''''''$$$$$$$$##$'&&'%*%*
0����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������yt��������������������������������������������������������������������������������������������������������������������������������������������������������������pYIE?9?IUd}������������������������� [...]
+6u���������������������y
)) ((&&&&$$$########$$&&&&&&'#��������������������A.+%"%%''''''''$$$$$$$$##$'&&''''5
+K����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������{w���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������z? %c�������������������������� [...]
@������������������������)))-$''&&&$##!$####''&&&&&&&&+
+6��������������������G
+*)%"%%&&&&&&&&$$$$$$$$""#&&&((($4x����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������w>91*%% [...]
+)&%%%$&&&&&&&&$$$$$$$$""#&&&((+)
<������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������v����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~}���������������������������������� [...]
+'(((*% &'
"e���������������������������|;"&
+(
!! %$!&.'*##%*(#����������������������b,(''!/'(('##%'%"$$$$##&('" &))!(�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������t�������������������������������������������������������������������������������������������������������������������� [...]
+ Bl���������������������������������v
"" $
)1%" %$" 3+/�������������������������~5*5.##&&))%%&&$$$$$$/'(31&%- e�������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������~{��������������������������������������������������������������������������������������������������������������������� [...]
+@����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������o|���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+:�����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������i|��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
+ +����������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������hc������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
1Nm��������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������xh���������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������� [...]
\ No newline at end of file
diff --git a/tutorial/imgproc/contour/tutorial-contour.cpp b/tutorial/imgproc/contour/tutorial-contour.cpp
new file mode 100644
index 0000000..f984bff
--- /dev/null
+++ b/tutorial/imgproc/contour/tutorial-contour.cpp
@@ -0,0 +1,142 @@
+//! \example tutorial-contour.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+
+namespace
+{
+//! [Print contours hierarchy func]
+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::endl;
+ std::cout << "\tcontour size: " << contour.m_points.size() << std::endl;
+ std::cout << "\tnb children: " << contour.m_children.size() << std::endl;
+
+ for (std::vector<vp::vpContour *>::const_iterator it = contour.m_children.begin(); it != contour.m_children.end();
+ ++it) {
+ displayContourInfo(**it, level + 1);
+ }
+}
+//! [Print contours hierarchy func]
+
+//! [Draw contours hierarchical func]
+void drawContoursTree(vpImage<vpRGBa> &I, const vp::vpContour &contour)
+{
+ std::vector<std::vector<vpImagePoint> > contours;
+ contours.push_back(contour.m_points);
+ vp::drawContours(I, contours, contour.m_contourType == vp::CONTOUR_OUTER ? vpColor::red : vpColor::green);
+
+ for (std::vector<vp::vpContour *>::const_iterator it = contour.m_children.begin(); it != contour.m_children.end();
+ ++it) {
+ drawContoursTree(I, **it);
+ }
+}
+//! [Draw contours hierarchical func]
+} // namespace
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+ //!
+ std::string input_filename = "grid36-03.pgm";
+ bool white_foreground = false;
+ vp::vpContourRetrievalType extraction_method = vp::CONTOUR_RETR_TREE;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--white_foreground") {
+ white_foreground = true;
+ } else if (std::string(argv[i]) == "--method" && i + 1 < argc) {
+ extraction_method = (vp::vpContourRetrievalType)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input image>] [--method <0: "
+ "CONTOUR_RETR_TREE, 1: CONTOUR_RETR_LIST, 2: "
+ "CONTOUR_RETR_EXTERNAL>]"
+ " [--white_foreground] [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ //! [Read]
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, input_filename);
+ //! [Read]
+ vpImage<unsigned char> I_bin(I.getHeight(), I.getWidth());
+
+ vpImage<vpRGBa> I_draw_contours(I.getHeight(), I.getWidth());
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d, d2;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d, d2;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d, d2;
+#endif
+ d.init(I_bin, 0, 0, "After binarisation");
+ d2.init(I_draw_contours, I_bin.getWidth(), 10, "Contours");
+
+ //! [Otsu]
+ vp::autoThreshold(I, vp::AUTO_THRESHOLD_OTSU, white_foreground ? 0 : 1, white_foreground ? 1 : 0);
+ //! [Otsu]
+ for (unsigned int i = 0; i < I_bin.getSize(); i++) {
+ I_bin.bitmap[i] = 255 * I.bitmap[i];
+ }
+
+ //! [Find contours]
+ vp::vpContour vp_contours;
+ std::vector<std::vector<vpImagePoint> > contours;
+ vp::findContours(I, vp_contours, contours, extraction_method);
+ //! [Find contours]
+
+ //! [Draw contours]
+ vp::drawContours(I_draw_contours, contours, vpColor::red);
+ //! [Draw contours]
+
+ vpDisplay::display(I_bin);
+ vpDisplay::display(I_draw_contours);
+ vpDisplay::displayText(I_draw_contours, 20, 20, "Click to draw outer / hole contours.", vpColor::red);
+ vpDisplay::flush(I_bin);
+ vpDisplay::flush(I_draw_contours);
+ vpDisplay::getClick(I_draw_contours);
+
+ I_draw_contours = 0;
+ //! [Draw contours hierarchical]
+ drawContoursTree(I_draw_contours, vp_contours);
+ //! [Draw contours hierarchical]
+ displayContourInfo(vp_contours, 0);
+
+ vpDisplay::display(I_bin);
+ vpDisplay::display(I_draw_contours);
+ vpDisplay::displayText(I_draw_contours, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::displayText(I_draw_contours, 20, I_draw_contours.getWidth() - 200, "Outer contour", vpColor::red);
+ vpDisplay::displayText(I_draw_contours, 20, I_draw_contours.getWidth() - 100, "Hole contour", vpColor::green);
+ vpDisplay::flush(I_bin);
+ vpDisplay::flush(I_draw_contours);
+ vpDisplay::getClick(I_draw_contours);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/contrast-sharpening/CMakeLists.txt b/tutorial/imgproc/contrast-sharpening/CMakeLists.txt
new file mode 100644
index 0000000..bece337
--- /dev/null
+++ b/tutorial/imgproc/contrast-sharpening/CMakeLists.txt
@@ -0,0 +1,23 @@
+project(tutorial-contrast-sharpening)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-contrast-sharpening.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/Crayfish-low-contrast.ppm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-contrast-sharpening.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/contrast-sharpening/Crayfish-low-contrast.ppm b/tutorial/imgproc/contrast-sharpening/Crayfish-low-contrast.ppm
new file mode 100644
index 0000000..31a7fb0
--- /dev/null
+++ b/tutorial/imgproc/contrast-sharpening/Crayfish-low-contrast.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+640 427
+255
+kilfdgcaddbedbea_b`^acad_]`cadfdgecfb`ca_bdbegeh``b``b_^c]\aZY^ZY^^]ba`eYX^YX^YX^YX^YX`ZYaZYaZYa[\aZ[`YZ_XY^Z[`]^ca`fcbhfekgflkholiphela^ea^edah`[b^Y`YX^ZY_Z]b[^cY^bW\`W_bYad\ae[^cX[`Z[`]\b`_e^^`]]___addfhhjhhjcad^\_a_becfjeijeihcggbfidhlgklgkkfjidhfaec^bc^be`dgbfe`de`dd_cc^bd_cgbfjeikfjgejljopnsnlqgejb`edbggejjhmiglgejdbgcafechiglmkntpqsonqmlplkokjokjplkqmlplkuqpokjkgfsonxtsuqpuqpsrppomlkiihfihfkjhlkilkiihfjigkjhlkinmkonlpomqpnlhimijnjknjkmijmijnjkplmnjkmijlhikghjfgkghlhimijhhjhhjhhjhhj [...]
\ No newline at end of file
diff --git a/tutorial/imgproc/contrast-sharpening/tutorial-contrast-sharpening.cpp b/tutorial/imgproc/contrast-sharpening/tutorial-contrast-sharpening.cpp
new file mode 100644
index 0000000..992a0ba
--- /dev/null
+++ b/tutorial/imgproc/contrast-sharpening/tutorial-contrast-sharpening.cpp
@@ -0,0 +1,121 @@
+//! \example tutorial-contrast-sharpening.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC)
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+#endif
+
+int main(int argc, const char **argv)
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+ //!
+ std::string input_filename = "Crayfish-low-contrast.ppm";
+ int blockRadius = 150;
+ int bins = 256;
+ float slope = 3.0f;
+ unsigned int size = 11;
+ double weight = 0.5;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--blockRadius" && i + 1 < argc) {
+ blockRadius = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--bins" && i + 1 < argc) {
+ bins = atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--slope" && i + 1 < argc) {
+ slope = (float)atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--size" && i + 1 < argc) {
+ size = (unsigned int)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--weight" && i + 1 < argc) {
+ weight = atof(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input image>]"
+ " [--blockRadius <block radius for CLAHE>] [--bins <nb "
+ "histogram bins for CLAHE>] [--slope <slope for CLAHE>]"
+ " [--size <Gaussian kernel size>] [--weight <unsharp mask "
+ "weighting>]"
+ " [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ //! [Read]
+ vpImage<vpRGBa> I_color;
+ vpImageIo::read(I_color, input_filename);
+//! [Read]
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d, d2, d3, d4, d5, d6;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d, d2, d3, d4, d5, d6;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d, d2, d3, d4, d5, d6;
+#endif
+ d.init(I_color, 0, 0, "Input color image");
+
+ //! [Stretch contrast]
+ vpImage<vpRGBa> I_stretch;
+ vp::stretchContrast(I_color, I_stretch);
+ //! [Stretch contrast]
+ d2.init(I_stretch, I_color.getWidth(), 10, "Stretch contrast");
+
+ //! [Stretch contrast HSV]
+ vpImage<vpRGBa> I_stretch_hsv;
+ vp::stretchContrastHSV(I_color, I_stretch_hsv);
+ //! [Stretch contrast HSV]
+ d3.init(I_stretch_hsv, 0, I_color.getHeight() + 80, "Stretch contrast HSV");
+
+ //! [Histogram equalization]
+ vpImage<vpRGBa> I_hist_eq;
+ vp::equalizeHistogram(I_color, I_hist_eq);
+ //! [Histogram equalization]
+ d4.init(I_hist_eq, I_color.getWidth(), I_color.getHeight() + 80, "Histogram equalization");
+
+ //! [CLAHE]
+ vpImage<vpRGBa> I_clahe;
+ vp::clahe(I_color, I_clahe, blockRadius, bins, slope);
+ //! [CLAHE]
+ d5.init(I_clahe, 0, 2 * I_color.getHeight() + 80, "Histogram equalization");
+
+ //! [Unsharp mask]
+ vpImage<vpRGBa> I_unsharp;
+ vp::unsharpMask(I_clahe, I_unsharp, size, weight);
+ //! [Unsharp mask]
+ d6.init(I_unsharp, I_color.getWidth(), 2 * I_color.getHeight() + 80, "Unsharp mask");
+
+ vpDisplay::display(I_color);
+ vpDisplay::display(I_stretch);
+ vpDisplay::display(I_stretch_hsv);
+ vpDisplay::display(I_hist_eq);
+ vpDisplay::display(I_clahe);
+ vpDisplay::display(I_unsharp);
+ vpDisplay::displayText(I_unsharp, 20, 20, "Click to quit.", vpColor::red);
+ vpDisplay::flush(I_color);
+ vpDisplay::flush(I_stretch);
+ vpDisplay::flush(I_stretch_hsv);
+ vpDisplay::flush(I_hist_eq);
+ vpDisplay::flush(I_clahe);
+ vpDisplay::flush(I_unsharp);
+ vpDisplay::getClick(I_unsharp);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/count-coins/CMakeLists.txt b/tutorial/imgproc/count-coins/CMakeLists.txt
new file mode 100644
index 0000000..f973d88
--- /dev/null
+++ b/tutorial/imgproc/count-coins/CMakeLists.txt
@@ -0,0 +1,24 @@
+project(tutorial-)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-count-coins.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/coins1.pgm")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/coins2.pgm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-count-coins.cpp ${data})
+endforeach()
diff --git a/tutorial/imgproc/count-coins/coins1.pgm b/tutorial/imgproc/count-coins/coins1.pgm
new file mode 100644
index 0000000..39dd744
--- /dev/null
+++ b/tutorial/imgproc/count-coins/coins1.pgm
@@ -0,0 +1,5 @@
+P5
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+������������������������������������������������¾������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������þ���ÿ������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������������ [...]
\ No newline at end of file
diff --git a/tutorial/imgproc/count-coins/coins2.pgm b/tutorial/imgproc/count-coins/coins2.pgm
new file mode 100644
index 0000000..c8f1d2e
Binary files /dev/null and b/tutorial/imgproc/count-coins/coins2.pgm differ
diff --git a/tutorial/imgproc/count-coins/tutorial-count-coins.cpp b/tutorial/imgproc/count-coins/tutorial-count-coins.cpp
new file mode 100644
index 0000000..c78c51b
--- /dev/null
+++ b/tutorial/imgproc/count-coins/tutorial-count-coins.cpp
@@ -0,0 +1,154 @@
+//! \example tutorial-count-coins.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC)
+//! [Include]
+#include <visp3/core/vpMomentObject.h>
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+#endif
+
+int main(int argc, char *argv[])
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+
+ std::string input_filename = "coins1.pgm";
+ vp::vpAutoThresholdMethod method = vp::AUTO_THRESHOLD_OTSU;
+ bool white_foreground = false;
+
+ for (int i = 1; i < argc; i++) {
+ if (std::string(argv[i]) == "--input" && i + 1 < argc) {
+ input_filename = std::string(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--method" && i + 1 < argc) {
+ method = (vp::vpAutoThresholdMethod)atoi(argv[i + 1]);
+ } else if (std::string(argv[i]) == "--white_foreground") {
+ white_foreground = true;
+ } else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
+ std::cout << "Usage: " << argv[0]
+ << " [--input <input image>]"
+ " [--method <0: Huang, 1: Intermodes, 2: IsoData, 3: "
+ "Mean, 4: Otsu, 5: Triangle>]"
+ " [--white_foreground]"
+ " [--help]"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ //! [Read]
+ vpImage<unsigned char> I;
+ vpImageIo::read(I, input_filename);
+//! [Read]
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d, d2, d3, d4, d5;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d, d2, d3, d4, d5;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d, d2, d3, d4, d5;
+#endif
+ d.init(I, 0, 0, "Coins");
+
+ vpImage<unsigned char> I_bin, I_fill;
+ //! [Binarisation]
+ I_bin = I;
+ vp::autoThreshold(I_bin, method, white_foreground ? 0 : 255, white_foreground ? 255 : 0);
+ //! [Binarisation]
+ d2.init(I_bin, I.getWidth(), 0, "Binarisation");
+
+ //! [Fill holes]
+ I_fill = I_bin;
+ vp::fillHoles(I_fill);
+ //! [Fill holes]
+ d3.init(I_fill, 0, I.getHeight() + 80, "Fill holes");
+
+ //! [Opening]
+ vpImage<unsigned char> I_open = I_fill;
+ vpImageMorphology::erosion(I_open, vpImageMorphology::CONNEXITY_4);
+ vpImageMorphology::dilatation(I_open, vpImageMorphology::CONNEXITY_4);
+ //! [Opening]
+
+ //! [Closing]
+ vpImage<unsigned char> I_close = I_open;
+ vpImageMorphology::dilatation(I_close, vpImageMorphology::CONNEXITY_4);
+ vpImageMorphology::erosion(I_close, vpImageMorphology::CONNEXITY_4);
+ //! [Closing]
+ d4.init(I_close, I.getWidth(), I.getHeight() + 80, "Closing");
+
+ //! [Find contours]
+ vpImage<unsigned char> I_contours(I_close.getHeight(), I_close.getWidth());
+ for (unsigned int cpt = 0; cpt < I_close.getSize(); cpt++)
+ I_contours.bitmap[cpt] = I_close.bitmap[cpt] ? 1 : 0;
+
+ vp::vpContour vp_contours;
+ std::vector<std::vector<vpImagePoint> > contours;
+ vp::findContours(I_contours, vp_contours, contours, vp::CONTOUR_RETR_EXTERNAL);
+ //! [Find contours]
+
+ //! [Draw contours]
+ vpImage<vpRGBa> I_draw_contours(I_contours.getHeight(), I_contours.getWidth(), vpRGBa());
+ vp::drawContours(I_draw_contours, contours, vpColor::red);
+ //! [Draw contours]
+ d5.init(I_draw_contours, 0, 2 * I.getHeight() + 80, "Contours");
+
+ vpDisplay::display(I);
+ vpDisplay::display(I_bin);
+ vpDisplay::display(I_fill);
+ vpDisplay::display(I_close);
+ vpDisplay::display(I_draw_contours);
+
+ //! [Count coins]
+ int nb_coins = 0;
+ for (size_t i = 0; i < contours.size(); i++) {
+ std::vector<vpPoint> vec_p;
+
+ for (size_t j = 0; j < contours[i].size(); j++) {
+ vpPoint pt;
+ pt.set_x(contours[i][j].get_u());
+ pt.set_y(contours[i][j].get_v());
+ vec_p.push_back(pt);
+ }
+
+ vpMomentObject obj(1);
+ obj.setType(vpMomentObject::DENSE_POLYGON);
+ obj.fromVector(vec_p);
+
+ // sign(m00) depends of the contour orientation (clockwise or
+ // counter-clockwise) that's why we use fabs
+ if (std::fabs(obj.get(0, 0)) >= I.getSize() / 200) {
+ nb_coins++;
+ std::stringstream ss;
+ ss << "Coin " << nb_coins;
+
+ int centroid_x = (int)std::fabs(obj.get(1, 0) / obj.get(0, 0));
+ int centroid_y = (int)std::fabs(obj.get(0, 1) / obj.get(0, 0));
+ vpDisplay::displayText(I_draw_contours, centroid_y, centroid_x - 20, ss.str(), vpColor::red);
+ }
+ }
+ //! [Count coins]
+
+ vpDisplay::displayText(I_draw_contours, 20, 20, "Click to quit.", vpColor::red);
+
+ vpDisplay::flush(I);
+ vpDisplay::flush(I_bin);
+ vpDisplay::flush(I_fill);
+ vpDisplay::flush(I_close);
+ vpDisplay::flush(I_draw_contours);
+ vpDisplay::getClick(I_draw_contours);
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ return 0;
+#endif
+}
diff --git a/tutorial/imgproc/flood-fill/CMakeLists.txt b/tutorial/imgproc/flood-fill/CMakeLists.txt
new file mode 100644
index 0000000..31eb8da
--- /dev/null
+++ b/tutorial/imgproc/flood-fill/CMakeLists.txt
@@ -0,0 +1,16 @@
+project(tutorial-flood-fill)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_io visp_gui visp_imgproc)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-flood-fill.cpp)
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
diff --git a/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp b/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp
new file mode 100644
index 0000000..47f1929
--- /dev/null
+++ b/tutorial/imgproc/flood-fill/tutorial-flood-fill.cpp
@@ -0,0 +1,270 @@
+//! \example tutorial-flood-fill.cpp
+
+#include <cstdlib>
+#include <iostream>
+#include <visp3/core/vpImage.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+//! [Include]
+#include <visp3/imgproc/vpImgproc.h>
+//! [Include]
+
+namespace
+{
+//! [Bresenham's line algorithm]
+vpImagePoint switchToOctantZeroFrom(const int octant, const vpImagePoint &imPt)
+{
+ vpImagePoint imPt_switched = imPt;
+
+ switch (octant) {
+ case 0: // return (x, y)
+ imPt_switched.set_uv(imPt.get_u(), imPt.get_v());
+ break;
+
+ case 1: // return (y, x)
+ imPt_switched.set_uv(imPt.get_v(), imPt.get_u());
+ break;
+
+ case 2: // return (y, -x)
+ imPt_switched.set_uv(imPt.get_v(), -imPt.get_u());
+ break;
+
+ case 3: // return (-x, y)
+ imPt_switched.set_uv(-imPt.get_u(), imPt.get_v());
+ break;
+
+ case 4: // return (-x, -y)
+ imPt_switched.set_uv(-imPt.get_u(), -imPt.get_v());
+ break;
+
+ case 5: // return (-y, -x)
+ imPt_switched.set_uv(-imPt.get_v(), -imPt.get_u());
+ break;
+
+ case 6: // return (-y, x)
+ imPt_switched.set_uv(-imPt.get_v(), imPt.get_u());
+ break;
+
+ case 7: // return (x, -y)
+ imPt_switched.set_uv(imPt.get_u(), -imPt.get_v());
+ break;
+
+ default:
+ break;
+ }
+
+ return imPt_switched;
+}
+
+vpImagePoint switchFromOctantZeroTo(const int octant, const vpImagePoint &imPt)
+{
+ vpImagePoint imPt_switched = imPt;
+
+ switch (octant) {
+ case 0: // return (x, y)
+ imPt_switched.set_uv(imPt.get_u(), imPt.get_v());
+ break;
+
+ case 1: // return (y, x)
+ imPt_switched.set_uv(imPt.get_v(), imPt.get_u());
+ break;
+
+ case 2: // return (-y, x)
+ imPt_switched.set_uv(-imPt.get_v(), imPt.get_u());
+ break;
+
+ case 3: // return (-x, y)
+ imPt_switched.set_uv(-imPt.get_u(), imPt.get_v());
+ break;
+
+ case 4: // return (-x, -y)
+ imPt_switched.set_uv(-imPt.get_u(), -imPt.get_v());
+ break;
+
+ case 5: // return (-y, -x)
+ imPt_switched.set_uv(-imPt.get_v(), -imPt.get_u());
+ break;
+
+ case 6: // return (y, -x)
+ imPt_switched.set_uv(imPt.get_v(), -imPt.get_u());
+ break;
+
+ case 7: // return (x, -y)
+ imPt_switched.set_uv(imPt.get_u(), -imPt.get_v());
+ break;
+
+ default:
+ break;
+ }
+
+ return imPt_switched;
+}
+
+int getOctant(const vpImagePoint &imPt1, const vpImagePoint &imPt2)
+{
+ double dx = imPt2.get_u() - imPt1.get_u();
+ double dy = imPt2.get_v() - imPt1.get_v();
+
+ if (dx >= 0 && dy >= 0) {
+ if (dy >= dx) {
+ return 1;
+ } else {
+ return 0;
+ }
+ } else if (dx < 0 && dy >= 0) {
+ if (-dx >= dy) {
+ return 3;
+ } else {
+ return 2;
+ }
+ } else if (dx < 0 && dy < 0) {
+ if (dy <= dx) {
+ return 5;
+ } else {
+ return 4;
+ }
+ } else {
+ if (dx >= -dy) {
+ return 7;
+ } else {
+ return 6;
+ }
+ }
+}
+
+void drawLine(vpImage<unsigned char> &I, const unsigned char value, const vpImagePoint &imPt1_,
+ const vpImagePoint &imPt2_)
+{
+ vpImagePoint imPt1((int)imPt1_.get_v(), (int)imPt1_.get_u());
+ vpImagePoint imPt2((int)imPt2_.get_v(), (int)imPt2_.get_u());
+
+ int octant = getOctant(imPt1, imPt2);
+ imPt1 = switchToOctantZeroFrom(octant, imPt1);
+ imPt2 = switchToOctantZeroFrom(octant, imPt2);
+
+ double dx = imPt2.get_u() - imPt1.get_u();
+ double dy = imPt2.get_v() - imPt1.get_v();
+ double D = 2 * dy - dx;
+ double y = imPt1.get_v();
+
+ for (int x = (int)imPt1.get_u(); x <= (int)imPt2.get_u(); x++) {
+ vpImagePoint currentPt(y, x);
+ currentPt = switchFromOctantZeroTo(octant, currentPt);
+
+ unsigned int i = std::min(I.getHeight() - 1, (unsigned int)std::max(0.0, currentPt.get_i()));
+ unsigned int j = std::min(I.getWidth() - 1, (unsigned int)std::max(0.0, currentPt.get_j()));
+ I[i][j] = value;
+
+ if (D >= 0) {
+ y++;
+ D -= dx;
+ }
+
+ D += dy;
+ }
+}
+//! [Bresenham's line algorithm]
+} // namespace
+
+#endif
+
+int main()
+{
+//! [Macro defined]
+#if defined(VISP_HAVE_MODULE_IMGPROC) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+ //! [Macro defined]
+
+ //! [Create bitmap]
+ vpImage<vpRGBa> I(480, 640, vpRGBa());
+//! [Create bitmap]
+
+#ifdef VISP_HAVE_X11
+ vpDisplayX d;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI d;
+#elif defined(VISP_HAVE_OPENCV)
+ vpDisplayOpenCV d;
+#endif
+ d.init(I, 0, 0, "Paint");
+
+ //! [Draw polygons]
+ std::vector<vpPolygon> polygons;
+ for (int i = 0; i < 3; i++) {
+ vpDisplay::display(I);
+ std::stringstream ss;
+ ss << "Left click to draw polygon " << i + 1 << "/3"
+ << ", right click to close the shape.";
+ vpDisplay::displayText(I, 20, 20, ss.str(), vpColor::red);
+ vpDisplay::flush(I);
+
+ vpPolygon polygon;
+ polygon.initClick(I);
+ polygons.push_back(polygon);
+
+ vpDisplay::display(I);
+ vpDisplay::displayLine(I, polygon.getCorners(), true, vpColor::red);
+ vpDisplay::flush(I);
+
+ // Update the lines draw internally in the current image
+ vpDisplay::getImage(I, I);
+ }
+ //! [Draw polygons]
+
+ //! [Draw polygon lines]
+ vpImage<unsigned char> mask(I.getHeight(), I.getWidth(), 0);
+ for (size_t i = 0; i < polygons.size(); i++) {
+ if (polygons[i].getCorners().size() <= 1)
+ continue;
+
+ for (size_t j = 0; j < polygons[i].getCorners().size() - 1; j++)
+ drawLine(mask, 255, polygons[i].getCorners()[j], polygons[i].getCorners()[j + 1]);
+
+ drawLine(mask, 255, polygons[i].getCorners().front(), polygons[i].getCorners().back());
+ }
+ //! [Draw polygon lines]
+
+ bool quit = false;
+ while (!quit) {
+ vpDisplay::display(I);
+ vpDisplay::displayText(I, 20, 20,
+ "Left click on a pixel location to fill the "
+ "shape, right click to quit.",
+ vpColor::red);
+ vpDisplay::flush(I);
+
+ //! [Seed point click]
+ vpImagePoint ip;
+ vpMouseButton::vpMouseButtonType button;
+ if (vpDisplay::getClick(I, ip, button, false))
+ //! [Seed point click]
+ {
+ switch (button) {
+ case vpMouseButton::button1:
+ //! [Flood fill]
+ vp::floodFill(mask, ip, 0, 255, vpImageMorphology::CONNEXITY_4);
+ //! [Flood fill]
+
+ //! [Bucket fill]
+ for (unsigned int cpt = 0; cpt < mask.getSize(); cpt++) {
+ if (mask.bitmap[cpt])
+ I.bitmap[cpt] = vpColor::red;
+ }
+ //! [Bucket fill]
+ break;
+
+ case vpMouseButton::button3:
+ quit = true;
+ break;
+
+ default:
+ break;
+ }
+ }
+ }
+#endif
+
+ return EXIT_SUCCESS;
+}
diff --git a/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.pbxproj b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.pbxproj
new file mode 100644
index 0000000..6bcf630
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.pbxproj
@@ -0,0 +1,318 @@
+// !$*UTF8*$!
+{
+ archiveVersion = 1;
+ classes = {
+ };
+ objectVersion = 46;
+ objects = {
+
+/* Begin PBXBuildFile section */
+ 4B3FF7171F87DB8D006F9094 /* main.m in Sources */ = {isa = PBXBuildFile; fileRef = 4B3FF7161F87DB8D006F9094 /* main.m */; };
+ 4B3FF71A1F87DB8D006F9094 /* AppDelegate.m in Sources */ = {isa = PBXBuildFile; fileRef = 4B3FF7191F87DB8D006F9094 /* AppDelegate.m */; };
+ 4B3FF71D1F87DB8D006F9094 /* ViewController.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4B3FF71C1F87DB8D006F9094 /* ViewController.mm */; };
+ 4B3FF7201F87DB8D006F9094 /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4B3FF71E1F87DB8D006F9094 /* Main.storyboard */; };
+ 4B3FF7221F87DB8D006F9094 /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4B3FF7211F87DB8D006F9094 /* Assets.xcassets */; };
+ 4B3FF7251F87DB8D006F9094 /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4B3FF7231F87DB8D006F9094 /* LaunchScreen.storyboard */; };
+/* End PBXBuildFile section */
+
+/* Begin PBXFileReference section */
+ 4B3FF7121F87DB8D006F9094 /* GettingStarted.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = GettingStarted.app; sourceTree = BUILT_PRODUCTS_DIR; };
+ 4B3FF7161F87DB8D006F9094 /* main.m */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.objc; path = main.m; sourceTree = "<group>"; };
+ 4B3FF7181F87DB8D006F9094 /* AppDelegate.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = AppDelegate.h; sourceTree = "<group>"; };
+ 4B3FF7191F87DB8D006F9094 /* AppDelegate.m */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.objc; path = AppDelegate.m; sourceTree = "<group>"; };
+ 4B3FF71B1F87DB8D006F9094 /* ViewController.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = ViewController.h; sourceTree = "<group>"; };
+ 4B3FF71C1F87DB8D006F9094 /* ViewController.mm */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.objcpp; path = ViewController.mm; sourceTree = "<group>"; };
+ 4B3FF71F1F87DB8D006F9094 /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = Base.lproj/Main.storyboard; sourceTree = "<group>"; };
+ 4B3FF7211F87DB8D006F9094 /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; path = Assets.xcassets; sourceTree = "<group>"; };
+ 4B3FF7241F87DB8D006F9094 /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = Base.lproj/LaunchScreen.storyboard; sourceTree = "<group>"; };
+ 4B3FF7261F87DB8D006F9094 /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = "<group>"; };
+/* End PBXFileReference section */
+
+/* Begin PBXFrameworksBuildPhase section */
+ 4B3FF70F1F87DB8D006F9094 /* Frameworks */ = {
+ isa = PBXFrameworksBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXFrameworksBuildPhase section */
+
+/* Begin PBXGroup section */
+ 4B3FF7091F87DB8D006F9094 = {
+ isa = PBXGroup;
+ children = (
+ 4B3FF7141F87DB8D006F9094 /* GettingStarted */,
+ 4B3FF7131F87DB8D006F9094 /* Products */,
+ );
+ sourceTree = "<group>";
+ };
+ 4B3FF7131F87DB8D006F9094 /* Products */ = {
+ isa = PBXGroup;
+ children = (
+ 4B3FF7121F87DB8D006F9094 /* GettingStarted.app */,
+ );
+ name = Products;
+ sourceTree = "<group>";
+ };
+ 4B3FF7141F87DB8D006F9094 /* GettingStarted */ = {
+ isa = PBXGroup;
+ children = (
+ 4B3FF7181F87DB8D006F9094 /* AppDelegate.h */,
+ 4B3FF7191F87DB8D006F9094 /* AppDelegate.m */,
+ 4B3FF71B1F87DB8D006F9094 /* ViewController.h */,
+ 4B3FF71C1F87DB8D006F9094 /* ViewController.mm */,
+ 4B3FF71E1F87DB8D006F9094 /* Main.storyboard */,
+ 4B3FF7211F87DB8D006F9094 /* Assets.xcassets */,
+ 4B3FF7231F87DB8D006F9094 /* LaunchScreen.storyboard */,
+ 4B3FF7261F87DB8D006F9094 /* Info.plist */,
+ 4B3FF7151F87DB8D006F9094 /* Supporting Files */,
+ );
+ path = GettingStarted;
+ sourceTree = "<group>";
+ };
+ 4B3FF7151F87DB8D006F9094 /* Supporting Files */ = {
+ isa = PBXGroup;
+ children = (
+ 4B3FF7161F87DB8D006F9094 /* main.m */,
+ );
+ name = "Supporting Files";
+ sourceTree = "<group>";
+ };
+/* End PBXGroup section */
+
+/* Begin PBXNativeTarget section */
+ 4B3FF7111F87DB8D006F9094 /* GettingStarted */ = {
+ isa = PBXNativeTarget;
+ buildConfigurationList = 4B3FF7291F87DB8D006F9094 /* Build configuration list for PBXNativeTarget "GettingStarted" */;
+ buildPhases = (
+ 4B3FF70E1F87DB8D006F9094 /* Sources */,
+ 4B3FF70F1F87DB8D006F9094 /* Frameworks */,
+ 4B3FF7101F87DB8D006F9094 /* Resources */,
+ );
+ buildRules = (
+ );
+ dependencies = (
+ );
+ name = GettingStarted;
+ productName = GettingStarted;
+ productReference = 4B3FF7121F87DB8D006F9094 /* GettingStarted.app */;
+ productType = "com.apple.product-type.application";
+ };
+/* End PBXNativeTarget section */
+
+/* Begin PBXProject section */
+ 4B3FF70A1F87DB8D006F9094 /* Project object */ = {
+ isa = PBXProject;
+ attributes = {
+ LastUpgradeCheck = 0830;
+ ORGANIZATIONNAME = MyOrganization;
+ TargetAttributes = {
+ 4B3FF7111F87DB8D006F9094 = {
+ CreatedOnToolsVersion = 8.3.3;
+ ProvisioningStyle = Automatic;
+ };
+ };
+ };
+ buildConfigurationList = 4B3FF70D1F87DB8D006F9094 /* Build configuration list for PBXProject "GettingStarted" */;
+ compatibilityVersion = "Xcode 3.2";
+ developmentRegion = English;
+ hasScannedForEncodings = 0;
+ knownRegions = (
+ en,
+ Base,
+ );
+ mainGroup = 4B3FF7091F87DB8D006F9094;
+ productRefGroup = 4B3FF7131F87DB8D006F9094 /* Products */;
+ projectDirPath = "";
+ projectRoot = "";
+ targets = (
+ 4B3FF7111F87DB8D006F9094 /* GettingStarted */,
+ );
+ };
+/* End PBXProject section */
+
+/* Begin PBXResourcesBuildPhase section */
+ 4B3FF7101F87DB8D006F9094 /* Resources */ = {
+ isa = PBXResourcesBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ 4B3FF7251F87DB8D006F9094 /* LaunchScreen.storyboard in Resources */,
+ 4B3FF7221F87DB8D006F9094 /* Assets.xcassets in Resources */,
+ 4B3FF7201F87DB8D006F9094 /* Main.storyboard in Resources */,
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXResourcesBuildPhase section */
+
+/* Begin PBXSourcesBuildPhase section */
+ 4B3FF70E1F87DB8D006F9094 /* Sources */ = {
+ isa = PBXSourcesBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ 4B3FF71D1F87DB8D006F9094 /* ViewController.mm in Sources */,
+ 4B3FF71A1F87DB8D006F9094 /* AppDelegate.m in Sources */,
+ 4B3FF7171F87DB8D006F9094 /* main.m in Sources */,
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXSourcesBuildPhase section */
+
+/* Begin PBXVariantGroup section */
+ 4B3FF71E1F87DB8D006F9094 /* Main.storyboard */ = {
+ isa = PBXVariantGroup;
+ children = (
+ 4B3FF71F1F87DB8D006F9094 /* Base */,
+ );
+ name = Main.storyboard;
+ sourceTree = "<group>";
+ };
+ 4B3FF7231F87DB8D006F9094 /* LaunchScreen.storyboard */ = {
+ isa = PBXVariantGroup;
+ children = (
+ 4B3FF7241F87DB8D006F9094 /* Base */,
+ );
+ name = LaunchScreen.storyboard;
+ sourceTree = "<group>";
+ };
+/* End PBXVariantGroup section */
+
+/* Begin XCBuildConfiguration section */
+ 4B3FF7271F87DB8D006F9094 /* Debug */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ALWAYS_SEARCH_USER_PATHS = NO;
+ CLANG_ANALYZER_NONNULL = YES;
+ CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
+ CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
+ CLANG_CXX_LIBRARY = "libc++";
+ CLANG_ENABLE_MODULES = YES;
+ CLANG_ENABLE_OBJC_ARC = YES;
+ CLANG_WARN_BOOL_CONVERSION = YES;
+ CLANG_WARN_CONSTANT_CONVERSION = YES;
+ CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
+ CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
+ CLANG_WARN_EMPTY_BODY = YES;
+ CLANG_WARN_ENUM_CONVERSION = YES;
+ CLANG_WARN_INFINITE_RECURSION = YES;
+ CLANG_WARN_INT_CONVERSION = YES;
+ CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
+ CLANG_WARN_SUSPICIOUS_MOVE = YES;
+ CLANG_WARN_UNREACHABLE_CODE = YES;
+ CLANG_WARN__DUPLICATE_METHOD_MATCH = YES;
+ "CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer";
+ COPY_PHASE_STRIP = NO;
+ DEBUG_INFORMATION_FORMAT = dwarf;
+ ENABLE_STRICT_OBJC_MSGSEND = YES;
+ ENABLE_TESTABILITY = YES;
+ GCC_C_LANGUAGE_STANDARD = gnu99;
+ GCC_DYNAMIC_NO_PIC = NO;
+ GCC_NO_COMMON_BLOCKS = YES;
+ GCC_OPTIMIZATION_LEVEL = 0;
+ GCC_PREPROCESSOR_DEFINITIONS = (
+ "DEBUG=1",
+ "$(inherited)",
+ );
+ GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
+ GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;
+ GCC_WARN_UNDECLARED_SELECTOR = YES;
+ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;
+ GCC_WARN_UNUSED_FUNCTION = YES;
+ GCC_WARN_UNUSED_VARIABLE = YES;
+ IPHONEOS_DEPLOYMENT_TARGET = 10.3;
+ MTL_ENABLE_DEBUG_INFO = YES;
+ ONLY_ACTIVE_ARCH = YES;
+ SDKROOT = iphoneos;
+ TARGETED_DEVICE_FAMILY = "1,2";
+ };
+ name = Debug;
+ };
+ 4B3FF7281F87DB8D006F9094 /* Release */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ALWAYS_SEARCH_USER_PATHS = NO;
+ CLANG_ANALYZER_NONNULL = YES;
+ CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
+ CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
+ CLANG_CXX_LIBRARY = "libc++";
+ CLANG_ENABLE_MODULES = YES;
+ CLANG_ENABLE_OBJC_ARC = YES;
+ CLANG_WARN_BOOL_CONVERSION = YES;
+ CLANG_WARN_CONSTANT_CONVERSION = YES;
+ CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
+ CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
+ CLANG_WARN_EMPTY_BODY = YES;
+ CLANG_WARN_ENUM_CONVERSION = YES;
+ CLANG_WARN_INFINITE_RECURSION = YES;
+ CLANG_WARN_INT_CONVERSION = YES;
+ CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
+ CLANG_WARN_SUSPICIOUS_MOVE = YES;
+ CLANG_WARN_UNREACHABLE_CODE = YES;
+ CLANG_WARN__DUPLICATE_METHOD_MATCH = YES;
+ "CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer";
+ COPY_PHASE_STRIP = NO;
+ DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
+ ENABLE_NS_ASSERTIONS = NO;
+ ENABLE_STRICT_OBJC_MSGSEND = YES;
+ GCC_C_LANGUAGE_STANDARD = gnu99;
+ GCC_NO_COMMON_BLOCKS = YES;
+ GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
+ GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;
+ GCC_WARN_UNDECLARED_SELECTOR = YES;
+ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;
+ GCC_WARN_UNUSED_FUNCTION = YES;
+ GCC_WARN_UNUSED_VARIABLE = YES;
+ IPHONEOS_DEPLOYMENT_TARGET = 10.3;
+ MTL_ENABLE_DEBUG_INFO = NO;
+ SDKROOT = iphoneos;
+ TARGETED_DEVICE_FAMILY = "1,2";
+ VALIDATE_PRODUCT = YES;
+ };
+ name = Release;
+ };
+ 4B3FF72A1F87DB8D006F9094 /* Debug */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
+ INFOPLIST_FILE = GettingStarted/Info.plist;
+ LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
+ PRODUCT_BUNDLE_IDENTIFIER = MyOrg.GettingStarted;
+ PRODUCT_NAME = "$(TARGET_NAME)";
+ };
+ name = Debug;
+ };
+ 4B3FF72B1F87DB8D006F9094 /* Release */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
+ INFOPLIST_FILE = GettingStarted/Info.plist;
+ LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
+ PRODUCT_BUNDLE_IDENTIFIER = MyOrg.GettingStarted;
+ PRODUCT_NAME = "$(TARGET_NAME)";
+ };
+ name = Release;
+ };
+/* End XCBuildConfiguration section */
+
+/* Begin XCConfigurationList section */
+ 4B3FF70D1F87DB8D006F9094 /* Build configuration list for PBXProject "GettingStarted" */ = {
+ isa = XCConfigurationList;
+ buildConfigurations = (
+ 4B3FF7271F87DB8D006F9094 /* Debug */,
+ 4B3FF7281F87DB8D006F9094 /* Release */,
+ );
+ defaultConfigurationIsVisible = 0;
+ defaultConfigurationName = Release;
+ };
+ 4B3FF7291F87DB8D006F9094 /* Build configuration list for PBXNativeTarget "GettingStarted" */ = {
+ isa = XCConfigurationList;
+ buildConfigurations = (
+ 4B3FF72A1F87DB8D006F9094 /* Debug */,
+ 4B3FF72B1F87DB8D006F9094 /* Release */,
+ );
+ defaultConfigurationIsVisible = 0;
+ };
+/* End XCConfigurationList section */
+ };
+ rootObject = 4B3FF70A1F87DB8D006F9094 /* Project object */;
+}
diff --git a/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/contents.xcworkspacedata b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/contents.xcworkspacedata
new file mode 100644
index 0000000..abf5a28
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/contents.xcworkspacedata
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+ version = "1.0">
+ <FileRef
+ location = "self:GettingStarted.xcodeproj">
+ </FileRef>
+</Workspace>
diff --git a/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate
new file mode 100644
index 0000000..52c49ef
Binary files /dev/null and b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate differ
diff --git a/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/GettingStarted.xcscheme b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/GettingStarted.xcscheme
new file mode 100644
index 0000000..7f3d22b
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/GettingStarted.xcscheme
@@ -0,0 +1,91 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Scheme
+ LastUpgradeVersion = "0830"
+ version = "1.3">
+ <BuildAction
+ parallelizeBuildables = "YES"
+ buildImplicitDependencies = "YES">
+ <BuildActionEntries>
+ <BuildActionEntry
+ buildForTesting = "YES"
+ buildForRunning = "YES"
+ buildForProfiling = "YES"
+ buildForArchiving = "YES"
+ buildForAnalyzing = "YES">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4B3FF7111F87DB8D006F9094"
+ BuildableName = "GettingStarted.app"
+ BlueprintName = "GettingStarted"
+ ReferencedContainer = "container:GettingStarted.xcodeproj">
+ </BuildableReference>
+ </BuildActionEntry>
+ </BuildActionEntries>
+ </BuildAction>
+ <TestAction
+ buildConfiguration = "Debug"
+ selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+ selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+ shouldUseLaunchSchemeArgsEnv = "YES">
+ <Testables>
+ </Testables>
+ <MacroExpansion>
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4B3FF7111F87DB8D006F9094"
+ BuildableName = "GettingStarted.app"
+ BlueprintName = "GettingStarted"
+ ReferencedContainer = "container:GettingStarted.xcodeproj">
+ </BuildableReference>
+ </MacroExpansion>
+ <AdditionalOptions>
+ </AdditionalOptions>
+ </TestAction>
+ <LaunchAction
+ buildConfiguration = "Debug"
+ selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+ selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+ launchStyle = "0"
+ useCustomWorkingDirectory = "NO"
+ ignoresPersistentStateOnLaunch = "NO"
+ debugDocumentVersioning = "YES"
+ debugServiceExtension = "internal"
+ allowLocationSimulation = "YES">
+ <BuildableProductRunnable
+ runnableDebuggingMode = "0">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4B3FF7111F87DB8D006F9094"
+ BuildableName = "GettingStarted.app"
+ BlueprintName = "GettingStarted"
+ ReferencedContainer = "container:GettingStarted.xcodeproj">
+ </BuildableReference>
+ </BuildableProductRunnable>
+ <AdditionalOptions>
+ </AdditionalOptions>
+ </LaunchAction>
+ <ProfileAction
+ buildConfiguration = "Release"
+ shouldUseLaunchSchemeArgsEnv = "YES"
+ savedToolIdentifier = ""
+ useCustomWorkingDirectory = "NO"
+ debugDocumentVersioning = "YES">
+ <BuildableProductRunnable
+ runnableDebuggingMode = "0">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4B3FF7111F87DB8D006F9094"
+ BuildableName = "GettingStarted.app"
+ BlueprintName = "GettingStarted"
+ ReferencedContainer = "container:GettingStarted.xcodeproj">
+ </BuildableReference>
+ </BuildableProductRunnable>
+ </ProfileAction>
+ <AnalyzeAction
+ buildConfiguration = "Debug">
+ </AnalyzeAction>
+ <ArchiveAction
+ buildConfiguration = "Release"
+ revealArchiveInOrganizer = "YES">
+ </ArchiveAction>
+</Scheme>
diff --git a/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist
new file mode 100644
index 0000000..57ad8fd
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>SchemeUserState</key>
+ <dict>
+ <key>GettingStarted.xcscheme</key>
+ <dict>
+ <key>orderHint</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <key>SuppressBuildableAutocreation</key>
+ <dict>
+ <key>4B3FF7111F87DB8D006F9094</key>
+ <dict>
+ <key>primary</key>
+ <true/>
+ </dict>
+ </dict>
+</dict>
+</plist>
diff --git a/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.h b/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.h
new file mode 100644
index 0000000..2ccb23f
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.h
@@ -0,0 +1,11 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import <UIKit/UIKit.h>
+
+ at interface AppDelegate : UIResponder <UIApplicationDelegate>
+
+ at property(strong, nonatomic) UIWindow *window;
+
+ at end
+
+#endif
diff --git a/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.m b/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.m
new file mode 100644
index 0000000..0ef723b
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/AppDelegate.m
@@ -0,0 +1,46 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import "AppDelegate.h"
+
+ at interface AppDelegate ()
+
+ at end
+
+ at implementation AppDelegate
+
+
+- (BOOL)application:(UIApplication *)application didFinishLaunchingWithOptions:(NSDictionary *)launchOptions {
+ // Override point for customization after application launch.
+ return YES;
+}
+
+
+- (void)applicationWillResignActive:(UIApplication *)application {
+ // Sent when the application is about to move from active to inactive state. This can occur for certain types of temporary interruptions (such as an incoming phone call or SMS message) or when the user quits the application and it begins the transition to the background state.
+ // Use this method to pause ongoing tasks, disable timers, and invalidate graphics rendering callbacks. Games should use this method to pause the game.
+}
+
+
+- (void)applicationDidEnterBackground:(UIApplication *)application {
+ // Use this method to release shared resources, save user data, invalidate timers, and store enough application state information to restore your application to its current state in case it is terminated later.
+ // If your application supports background execution, this method is called instead of applicationWillTerminate: when the user quits.
+}
+
+
+- (void)applicationWillEnterForeground:(UIApplication *)application {
+ // Called as part of the transition from the background to the active state; here you can undo many of the changes made on entering the background.
+}
+
+
+- (void)applicationDidBecomeActive:(UIApplication *)application {
+ // Restart any tasks that were paused (or not yet started) while the application was inactive. If the application was previously in the background, optionally refresh the user interface.
+}
+
+
+- (void)applicationWillTerminate:(UIApplication *)application {
+ // Called when the application is about to terminate. Save data if appropriate. See also applicationDidEnterBackground:.
+}
+
+ at end
+
+#endif
diff --git a/tutorial/ios/GettingStarted/GettingStarted/Assets.xcassets/AppIcon.appiconset/Contents.json b/tutorial/ios/GettingStarted/GettingStarted/Assets.xcassets/AppIcon.appiconset/Contents.json
new file mode 100644
index 0000000..36d2c80
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/Assets.xcassets/AppIcon.appiconset/Contents.json
@@ -0,0 +1,68 @@
+{
+ "images" : [
+ {
+ "idiom" : "iphone",
+ "size" : "29x29",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "29x29",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "40x40",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "40x40",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "60x60",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "60x60",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "29x29",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "29x29",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "40x40",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "40x40",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "76x76",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "76x76",
+ "scale" : "2x"
+ }
+ ],
+ "info" : {
+ "version" : 1,
+ "author" : "xcode"
+ }
+}
\ No newline at end of file
diff --git a/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/LaunchScreen.storyboard b/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/LaunchScreen.storyboard
new file mode 100644
index 0000000..fdf3f97
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/LaunchScreen.storyboard
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="11134" systemVersion="15F34" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+ <dependencies>
+ <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="11106"/>
+ <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+ </dependencies>
+ <scenes>
+ <!--View Controller-->
+ <scene sceneID="EHf-IW-A2E">
+ <objects>
+ <viewController id="01J-lp-oVM" sceneMemberID="viewController">
+ <layoutGuides>
+ <viewControllerLayoutGuide type="top" id="Llm-lL-Icb"/>
+ <viewControllerLayoutGuide type="bottom" id="xb3-aO-Qok"/>
+ </layoutGuides>
+ <view key="view" contentMode="scaleToFill" id="Ze5-6b-2t3">
+ <rect key="frame" x="0.0" y="0.0" width="375" height="667"/>
+ <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+ <color key="backgroundColor" red="1" green="1" blue="1" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+ </view>
+ </viewController>
+ <placeholder placeholderIdentifier="IBFirstResponder" id="iYj-Kq-Ea1" userLabel="First Responder" sceneMemberID="firstResponder"/>
+ </objects>
+ <point key="canvasLocation" x="53" y="375"/>
+ </scene>
+ </scenes>
+</document>
diff --git a/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/Main.storyboard b/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/Main.storyboard
new file mode 100644
index 0000000..4529698
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/Base.lproj/Main.storyboard
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="11134" systemVersion="15F34" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" colorMatched="YES" initialViewController="BYZ-38-t0r">
+ <dependencies>
+ <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="11106"/>
+ <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+ </dependencies>
+ <scenes>
+ <!--View Controller-->
+ <scene sceneID="tne-QT-ifu">
+ <objects>
+ <viewController id="BYZ-38-t0r" customClass="ViewController" customModuleProvider="" sceneMemberID="viewController">
+ <layoutGuides>
+ <viewControllerLayoutGuide type="top" id="y3c-jy-aDJ"/>
+ <viewControllerLayoutGuide type="bottom" id="wfy-db-euE"/>
+ </layoutGuides>
+ <view key="view" contentMode="scaleToFill" id="8bC-Xf-vdC">
+ <rect key="frame" x="0.0" y="0.0" width="375" height="667"/>
+ <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+ <color key="backgroundColor" red="1" green="1" blue="1" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+ </view>
+ </viewController>
+ <placeholder placeholderIdentifier="IBFirstResponder" id="dkx-z0-nzr" sceneMemberID="firstResponder"/>
+ </objects>
+ </scene>
+ </scenes>
+</document>
diff --git a/tutorial/ios/GettingStarted/GettingStarted/Info.plist b/tutorial/ios/GettingStarted/GettingStarted/Info.plist
new file mode 100644
index 0000000..d052473
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/Info.plist
@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>CFBundleDevelopmentRegion</key>
+ <string>en</string>
+ <key>CFBundleExecutable</key>
+ <string>$(EXECUTABLE_NAME)</string>
+ <key>CFBundleIdentifier</key>
+ <string>$(PRODUCT_BUNDLE_IDENTIFIER)</string>
+ <key>CFBundleInfoDictionaryVersion</key>
+ <string>6.0</string>
+ <key>CFBundleName</key>
+ <string>$(PRODUCT_NAME)</string>
+ <key>CFBundlePackageType</key>
+ <string>APPL</string>
+ <key>CFBundleShortVersionString</key>
+ <string>1.0</string>
+ <key>CFBundleVersion</key>
+ <string>1</string>
+ <key>LSRequiresIPhoneOS</key>
+ <true/>
+ <key>UILaunchStoryboardName</key>
+ <string>LaunchScreen</string>
+ <key>UIMainStoryboardFile</key>
+ <string>Main</string>
+ <key>UIRequiredDeviceCapabilities</key>
+ <array>
+ <string>armv7</string>
+ </array>
+ <key>UISupportedInterfaceOrientations</key>
+ <array>
+ <string>UIInterfaceOrientationPortrait</string>
+ <string>UIInterfaceOrientationLandscapeLeft</string>
+ <string>UIInterfaceOrientationLandscapeRight</string>
+ </array>
+ <key>UISupportedInterfaceOrientations~ipad</key>
+ <array>
+ <string>UIInterfaceOrientationPortrait</string>
+ <string>UIInterfaceOrientationPortraitUpsideDown</string>
+ <string>UIInterfaceOrientationLandscapeLeft</string>
+ <string>UIInterfaceOrientationLandscapeRight</string>
+ </array>
+</dict>
+</plist>
diff --git a/tutorial/ios/GettingStarted/GettingStarted/ViewController.h b/tutorial/ios/GettingStarted/GettingStarted/ViewController.h
new file mode 100644
index 0000000..37a615d
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/ViewController.h
@@ -0,0 +1,9 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import <UIKit/UIKit.h>
+
+ at interface ViewController : UIViewController
+
+ at end
+
+#endif
diff --git a/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm b/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm
new file mode 100644
index 0000000..b30e52b
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/ViewController.mm
@@ -0,0 +1,116 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import "ViewController.h"
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+ at interface ViewController ()
+ at end
+
+ at implementation ViewController
+#pragma mark - Example of a function that uses ViSP
+- (void)processViSPHomography{
+
+ std::vector<vpPoint> oP(4), aP(4), bP(4);
+ double L = 0.1;
+
+ oP[0].setWorldCoordinates( -L,-L, 0);
+ oP[1].setWorldCoordinates(2*L,-L, 0);
+ oP[2].setWorldCoordinates( L, 3*L, 0);
+ oP[3].setWorldCoordinates( -L, 4*L, 0);
+
+ vpHomogeneousMatrix bMo(0,0, 1, 0, 0, 0) ;
+ vpHomogeneousMatrix aMb(0.2, 0, 0.1, 0,vpMath::rad(20), 0);
+ vpHomogeneousMatrix aMo = aMb*bMo ;
+
+ // Normalized coordinates of points in the image frame
+ std::vector<double> xa(4), ya(4), xb(4), yb(4);
+
+ for(int i=0 ; i < 4; i++){
+ oP[i].project(aMo);
+ xa[i] = oP[i].get_x();
+ ya[i] = oP[i].get_y();
+ oP[i].project(bMo);
+ xb[i] = oP[i].get_x();
+ yb[i] = oP[i].get_y();
+ }
+
+ vpHomography aHb ;
+
+ // Compute the homography
+ vpHomography::DLT(xb, yb, xa, ya, aHb, true);
+
+ std::cout << "Homography:\n" << aHb << std::endl;
+
+ vpRotationMatrix aRb;
+ vpTranslationVector atb;
+ vpColVector n;
+
+ // Compute the 3D transformation
+ aHb.computeDisplacement(aRb, atb, n);
+
+ std::cout << "atb: " << atb.t() << std::endl;
+
+ // Compute coordinates in pixels of point 3
+ vpImagePoint iPa, iPb;
+ vpCameraParameters cam;
+ vpMeterPixelConversion::convertPoint(cam, xb[3], yb[3], iPb);
+ vpMeterPixelConversion::convertPoint(cam, xa[3], ya[3], iPa);
+
+ std::cout << "Ground truth:" << std::endl;
+ std::cout << " Point 3 in pixels in frame b: " << iPb << std::endl;
+ std::cout << " Point 3 in pixels in frame a: " << iPa << std::endl;
+
+ // Estimate the position in pixel of point 3 from the homography
+ vpMatrix H = cam.get_K() * aHb * cam.get_K_inverse();
+
+ // Project the position in pixel of point 3 from the homography
+ std::cout << "Estimation from homography:" << std::endl;
+ std::cout << " Point 3 in pixels in frame a: " << vpHomography::project(cam, aHb, iPb) << std::endl;
+}
+- (void)viewDidLoad {
+ [super viewDidLoad];
+ // Do any additional setup after loading the view, typically from a nib.
+ [self processViSPHomography];
+}
+- (void)didReceiveMemoryWarning {
+ [super didReceiveMemoryWarning];
+ // Dispose of any resources that can be recreated.
+}
+ at end
+
+#endif
+
diff --git a/tutorial/ios/GettingStarted/GettingStarted/main.m b/tutorial/ios/GettingStarted/GettingStarted/main.m
new file mode 100644
index 0000000..240c82f
--- /dev/null
+++ b/tutorial/ios/GettingStarted/GettingStarted/main.m
@@ -0,0 +1,16 @@
+//
+// main.m
+// GettingStarted
+//
+// Created by Fabien Spindler on 06/10/2017.
+// Copyright © 2017 MyOrganization. All rights reserved.
+//
+
+#import <UIKit/UIKit.h>
+#import "AppDelegate.h"
+
+int main(int argc, char * argv[]) {
+ @autoreleasepool {
+ return UIApplicationMain(argc, argv, nil, NSStringFromClass([AppDelegate class]));
+ }
+}
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.pbxproj b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.pbxproj
new file mode 100644
index 0000000..c46dc4a
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.pbxproj
@@ -0,0 +1,335 @@
+// !$*UTF8*$!
+{
+ archiveVersion = 1;
+ classes = {
+ };
+ objectVersion = 46;
+ objects = {
+
+/* Begin PBXBuildFile section */
+ 4BC3FE281F876BE9006A29FB /* main.m in Sources */ = {isa = PBXBuildFile; fileRef = 4BC3FE271F876BE9006A29FB /* main.m */; };
+ 4BC3FE2B1F876BE9006A29FB /* AppDelegate.m in Sources */ = {isa = PBXBuildFile; fileRef = 4BC3FE2A1F876BE9006A29FB /* AppDelegate.m */; };
+ 4BC3FE2E1F876BE9006A29FB /* ViewController.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4BC3FE2D1F876BE9006A29FB /* ViewController.mm */; };
+ 4BC3FE311F876BE9006A29FB /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4BC3FE2F1F876BE9006A29FB /* Main.storyboard */; };
+ 4BC3FE331F876BE9006A29FB /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4BC3FE321F876BE9006A29FB /* Assets.xcassets */; };
+ 4BC3FE361F876BE9006A29FB /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4BC3FE341F876BE9006A29FB /* LaunchScreen.storyboard */; };
+ 4BC3FE411F876C27006A29FB /* ImageConversion.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4BC3FE3E1F876C27006A29FB /* ImageConversion.mm */; };
+ 4BC3FE421F876C27006A29FB /* ImageDisplay.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4BC3FE401F876C27006A29FB /* ImageDisplay.mm */; };
+ 4BFC47151F876DEA00C8764A /* AprilTag.png in Resources */ = {isa = PBXBuildFile; fileRef = 4BFC47141F876DEA00C8764A /* AprilTag.png */; };
+/* End PBXBuildFile section */
+
+/* Begin PBXFileReference section */
+ 4BC3FE231F876BE9006A29FB /* StartedAprilTag.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = StartedAprilTag.app; sourceTree = BUILT_PRODUCTS_DIR; };
+ 4BC3FE271F876BE9006A29FB /* main.m */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.objc; path = main.m; sourceTree = "<group>"; };
+ 4BC3FE291F876BE9006A29FB /* AppDelegate.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = AppDelegate.h; sourceTree = "<group>"; };
+ 4BC3FE2A1F876BE9006A29FB /* AppDelegate.m */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.objc; path = AppDelegate.m; sourceTree = "<group>"; };
+ 4BC3FE2C1F876BE9006A29FB /* ViewController.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = ViewController.h; sourceTree = "<group>"; };
+ 4BC3FE2D1F876BE9006A29FB /* ViewController.mm */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.cpp.objcpp; path = ViewController.mm; sourceTree = "<group>"; };
+ 4BC3FE301F876BE9006A29FB /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = Base.lproj/Main.storyboard; sourceTree = "<group>"; };
+ 4BC3FE321F876BE9006A29FB /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; path = Assets.xcassets; sourceTree = "<group>"; };
+ 4BC3FE351F876BE9006A29FB /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = Base.lproj/LaunchScreen.storyboard; sourceTree = "<group>"; };
+ 4BC3FE371F876BE9006A29FB /* Info.plist */ = {isa = PBXFileReference; lastKnownFileType = text.plist.xml; path = Info.plist; sourceTree = "<group>"; };
+ 4BC3FE3D1F876C27006A29FB /* ImageConversion.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ImageConversion.h; sourceTree = "<group>"; };
+ 4BC3FE3E1F876C27006A29FB /* ImageConversion.mm */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.objcpp; path = ImageConversion.mm; sourceTree = "<group>"; };
+ 4BC3FE3F1F876C27006A29FB /* ImageDisplay.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ImageDisplay.h; sourceTree = "<group>"; };
+ 4BC3FE401F876C27006A29FB /* ImageDisplay.mm */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.objcpp; path = ImageDisplay.mm; sourceTree = "<group>"; };
+ 4BFC47141F876DEA00C8764A /* AprilTag.png */ = {isa = PBXFileReference; lastKnownFileType = image.png; path = AprilTag.png; sourceTree = "<group>"; };
+/* End PBXFileReference section */
+
+/* Begin PBXFrameworksBuildPhase section */
+ 4BC3FE201F876BE9006A29FB /* Frameworks */ = {
+ isa = PBXFrameworksBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXFrameworksBuildPhase section */
+
+/* Begin PBXGroup section */
+ 4BC3FE1A1F876BE9006A29FB = {
+ isa = PBXGroup;
+ children = (
+ 4BC3FE251F876BE9006A29FB /* StartedAprilTag */,
+ 4BC3FE241F876BE9006A29FB /* Products */,
+ );
+ sourceTree = "<group>";
+ };
+ 4BC3FE241F876BE9006A29FB /* Products */ = {
+ isa = PBXGroup;
+ children = (
+ 4BC3FE231F876BE9006A29FB /* StartedAprilTag.app */,
+ );
+ name = Products;
+ sourceTree = "<group>";
+ };
+ 4BC3FE251F876BE9006A29FB /* StartedAprilTag */ = {
+ isa = PBXGroup;
+ children = (
+ 4BFC47141F876DEA00C8764A /* AprilTag.png */,
+ 4BC3FE3D1F876C27006A29FB /* ImageConversion.h */,
+ 4BC3FE3E1F876C27006A29FB /* ImageConversion.mm */,
+ 4BC3FE3F1F876C27006A29FB /* ImageDisplay.h */,
+ 4BC3FE401F876C27006A29FB /* ImageDisplay.mm */,
+ 4BC3FE291F876BE9006A29FB /* AppDelegate.h */,
+ 4BC3FE2A1F876BE9006A29FB /* AppDelegate.m */,
+ 4BC3FE2C1F876BE9006A29FB /* ViewController.h */,
+ 4BC3FE2D1F876BE9006A29FB /* ViewController.mm */,
+ 4BC3FE2F1F876BE9006A29FB /* Main.storyboard */,
+ 4BC3FE321F876BE9006A29FB /* Assets.xcassets */,
+ 4BC3FE341F876BE9006A29FB /* LaunchScreen.storyboard */,
+ 4BC3FE371F876BE9006A29FB /* Info.plist */,
+ 4BC3FE261F876BE9006A29FB /* Supporting Files */,
+ );
+ path = StartedAprilTag;
+ sourceTree = "<group>";
+ };
+ 4BC3FE261F876BE9006A29FB /* Supporting Files */ = {
+ isa = PBXGroup;
+ children = (
+ 4BC3FE271F876BE9006A29FB /* main.m */,
+ );
+ name = "Supporting Files";
+ sourceTree = "<group>";
+ };
+/* End PBXGroup section */
+
+/* Begin PBXNativeTarget section */
+ 4BC3FE221F876BE9006A29FB /* StartedAprilTag */ = {
+ isa = PBXNativeTarget;
+ buildConfigurationList = 4BC3FE3A1F876BE9006A29FB /* Build configuration list for PBXNativeTarget "StartedAprilTag" */;
+ buildPhases = (
+ 4BC3FE1F1F876BE9006A29FB /* Sources */,
+ 4BC3FE201F876BE9006A29FB /* Frameworks */,
+ 4BC3FE211F876BE9006A29FB /* Resources */,
+ );
+ buildRules = (
+ );
+ dependencies = (
+ );
+ name = StartedAprilTag;
+ productName = StartedAprilTag;
+ productReference = 4BC3FE231F876BE9006A29FB /* StartedAprilTag.app */;
+ productType = "com.apple.product-type.application";
+ };
+/* End PBXNativeTarget section */
+
+/* Begin PBXProject section */
+ 4BC3FE1B1F876BE9006A29FB /* Project object */ = {
+ isa = PBXProject;
+ attributes = {
+ LastUpgradeCheck = 0830;
+ ORGANIZATIONNAME = MyOrganization;
+ TargetAttributes = {
+ 4BC3FE221F876BE9006A29FB = {
+ CreatedOnToolsVersion = 8.3.3;
+ ProvisioningStyle = Automatic;
+ };
+ };
+ };
+ buildConfigurationList = 4BC3FE1E1F876BE9006A29FB /* Build configuration list for PBXProject "StartedAprilTag" */;
+ compatibilityVersion = "Xcode 3.2";
+ developmentRegion = English;
+ hasScannedForEncodings = 0;
+ knownRegions = (
+ en,
+ Base,
+ );
+ mainGroup = 4BC3FE1A1F876BE9006A29FB;
+ productRefGroup = 4BC3FE241F876BE9006A29FB /* Products */;
+ projectDirPath = "";
+ projectRoot = "";
+ targets = (
+ 4BC3FE221F876BE9006A29FB /* StartedAprilTag */,
+ );
+ };
+/* End PBXProject section */
+
+/* Begin PBXResourcesBuildPhase section */
+ 4BC3FE211F876BE9006A29FB /* Resources */ = {
+ isa = PBXResourcesBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ 4BC3FE361F876BE9006A29FB /* LaunchScreen.storyboard in Resources */,
+ 4BC3FE331F876BE9006A29FB /* Assets.xcassets in Resources */,
+ 4BC3FE311F876BE9006A29FB /* Main.storyboard in Resources */,
+ 4BFC47151F876DEA00C8764A /* AprilTag.png in Resources */,
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXResourcesBuildPhase section */
+
+/* Begin PBXSourcesBuildPhase section */
+ 4BC3FE1F1F876BE9006A29FB /* Sources */ = {
+ isa = PBXSourcesBuildPhase;
+ buildActionMask = 2147483647;
+ files = (
+ 4BC3FE2E1F876BE9006A29FB /* ViewController.mm in Sources */,
+ 4BC3FE2B1F876BE9006A29FB /* AppDelegate.m in Sources */,
+ 4BC3FE281F876BE9006A29FB /* main.m in Sources */,
+ 4BC3FE411F876C27006A29FB /* ImageConversion.mm in Sources */,
+ 4BC3FE421F876C27006A29FB /* ImageDisplay.mm in Sources */,
+ );
+ runOnlyForDeploymentPostprocessing = 0;
+ };
+/* End PBXSourcesBuildPhase section */
+
+/* Begin PBXVariantGroup section */
+ 4BC3FE2F1F876BE9006A29FB /* Main.storyboard */ = {
+ isa = PBXVariantGroup;
+ children = (
+ 4BC3FE301F876BE9006A29FB /* Base */,
+ );
+ name = Main.storyboard;
+ sourceTree = "<group>";
+ };
+ 4BC3FE341F876BE9006A29FB /* LaunchScreen.storyboard */ = {
+ isa = PBXVariantGroup;
+ children = (
+ 4BC3FE351F876BE9006A29FB /* Base */,
+ );
+ name = LaunchScreen.storyboard;
+ sourceTree = "<group>";
+ };
+/* End PBXVariantGroup section */
+
+/* Begin XCBuildConfiguration section */
+ 4BC3FE381F876BE9006A29FB /* Debug */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ALWAYS_SEARCH_USER_PATHS = NO;
+ CLANG_ANALYZER_NONNULL = YES;
+ CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
+ CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
+ CLANG_CXX_LIBRARY = "libc++";
+ CLANG_ENABLE_MODULES = YES;
+ CLANG_ENABLE_OBJC_ARC = YES;
+ CLANG_WARN_BOOL_CONVERSION = YES;
+ CLANG_WARN_CONSTANT_CONVERSION = YES;
+ CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
+ CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
+ CLANG_WARN_EMPTY_BODY = YES;
+ CLANG_WARN_ENUM_CONVERSION = YES;
+ CLANG_WARN_INFINITE_RECURSION = YES;
+ CLANG_WARN_INT_CONVERSION = YES;
+ CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
+ CLANG_WARN_SUSPICIOUS_MOVE = YES;
+ CLANG_WARN_UNREACHABLE_CODE = YES;
+ CLANG_WARN__DUPLICATE_METHOD_MATCH = YES;
+ "CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer";
+ COPY_PHASE_STRIP = NO;
+ DEBUG_INFORMATION_FORMAT = dwarf;
+ ENABLE_STRICT_OBJC_MSGSEND = YES;
+ ENABLE_TESTABILITY = YES;
+ GCC_C_LANGUAGE_STANDARD = gnu99;
+ GCC_DYNAMIC_NO_PIC = NO;
+ GCC_NO_COMMON_BLOCKS = YES;
+ GCC_OPTIMIZATION_LEVEL = 0;
+ GCC_PREPROCESSOR_DEFINITIONS = (
+ "DEBUG=1",
+ "$(inherited)",
+ );
+ GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
+ GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;
+ GCC_WARN_UNDECLARED_SELECTOR = YES;
+ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;
+ GCC_WARN_UNUSED_FUNCTION = YES;
+ GCC_WARN_UNUSED_VARIABLE = YES;
+ IPHONEOS_DEPLOYMENT_TARGET = 10.3;
+ MTL_ENABLE_DEBUG_INFO = YES;
+ ONLY_ACTIVE_ARCH = YES;
+ SDKROOT = iphoneos;
+ TARGETED_DEVICE_FAMILY = "1,2";
+ };
+ name = Debug;
+ };
+ 4BC3FE391F876BE9006A29FB /* Release */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ALWAYS_SEARCH_USER_PATHS = NO;
+ CLANG_ANALYZER_NONNULL = YES;
+ CLANG_ANALYZER_NUMBER_OBJECT_CONVERSION = YES_AGGRESSIVE;
+ CLANG_CXX_LANGUAGE_STANDARD = "gnu++0x";
+ CLANG_CXX_LIBRARY = "libc++";
+ CLANG_ENABLE_MODULES = YES;
+ CLANG_ENABLE_OBJC_ARC = YES;
+ CLANG_WARN_BOOL_CONVERSION = YES;
+ CLANG_WARN_CONSTANT_CONVERSION = YES;
+ CLANG_WARN_DIRECT_OBJC_ISA_USAGE = YES_ERROR;
+ CLANG_WARN_DOCUMENTATION_COMMENTS = YES;
+ CLANG_WARN_EMPTY_BODY = YES;
+ CLANG_WARN_ENUM_CONVERSION = YES;
+ CLANG_WARN_INFINITE_RECURSION = YES;
+ CLANG_WARN_INT_CONVERSION = YES;
+ CLANG_WARN_OBJC_ROOT_CLASS = YES_ERROR;
+ CLANG_WARN_SUSPICIOUS_MOVE = YES;
+ CLANG_WARN_UNREACHABLE_CODE = YES;
+ CLANG_WARN__DUPLICATE_METHOD_MATCH = YES;
+ "CODE_SIGN_IDENTITY[sdk=iphoneos*]" = "iPhone Developer";
+ COPY_PHASE_STRIP = NO;
+ DEBUG_INFORMATION_FORMAT = "dwarf-with-dsym";
+ ENABLE_NS_ASSERTIONS = NO;
+ ENABLE_STRICT_OBJC_MSGSEND = YES;
+ GCC_C_LANGUAGE_STANDARD = gnu99;
+ GCC_NO_COMMON_BLOCKS = YES;
+ GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
+ GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;
+ GCC_WARN_UNDECLARED_SELECTOR = YES;
+ GCC_WARN_UNINITIALIZED_AUTOS = YES_AGGRESSIVE;
+ GCC_WARN_UNUSED_FUNCTION = YES;
+ GCC_WARN_UNUSED_VARIABLE = YES;
+ IPHONEOS_DEPLOYMENT_TARGET = 10.3;
+ MTL_ENABLE_DEBUG_INFO = NO;
+ SDKROOT = iphoneos;
+ TARGETED_DEVICE_FAMILY = "1,2";
+ VALIDATE_PRODUCT = YES;
+ };
+ name = Release;
+ };
+ 4BC3FE3B1F876BE9006A29FB /* Debug */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
+ INFOPLIST_FILE = StartedAprilTag/Info.plist;
+ LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
+ PRODUCT_BUNDLE_IDENTIFIER = MyOrg.StartedAprilTag;
+ PRODUCT_NAME = "$(TARGET_NAME)";
+ };
+ name = Debug;
+ };
+ 4BC3FE3C1F876BE9006A29FB /* Release */ = {
+ isa = XCBuildConfiguration;
+ buildSettings = {
+ ASSETCATALOG_COMPILER_APPICON_NAME = AppIcon;
+ INFOPLIST_FILE = StartedAprilTag/Info.plist;
+ LD_RUNPATH_SEARCH_PATHS = "$(inherited) @executable_path/Frameworks";
+ PRODUCT_BUNDLE_IDENTIFIER = MyOrg.StartedAprilTag;
+ PRODUCT_NAME = "$(TARGET_NAME)";
+ };
+ name = Release;
+ };
+/* End XCBuildConfiguration section */
+
+/* Begin XCConfigurationList section */
+ 4BC3FE1E1F876BE9006A29FB /* Build configuration list for PBXProject "StartedAprilTag" */ = {
+ isa = XCConfigurationList;
+ buildConfigurations = (
+ 4BC3FE381F876BE9006A29FB /* Debug */,
+ 4BC3FE391F876BE9006A29FB /* Release */,
+ );
+ defaultConfigurationIsVisible = 0;
+ defaultConfigurationName = Release;
+ };
+ 4BC3FE3A1F876BE9006A29FB /* Build configuration list for PBXNativeTarget "StartedAprilTag" */ = {
+ isa = XCConfigurationList;
+ buildConfigurations = (
+ 4BC3FE3B1F876BE9006A29FB /* Debug */,
+ 4BC3FE3C1F876BE9006A29FB /* Release */,
+ );
+ defaultConfigurationIsVisible = 0;
+ defaultConfigurationName = Release;
+ };
+/* End XCConfigurationList section */
+ };
+ rootObject = 4BC3FE1B1F876BE9006A29FB /* Project object */;
+}
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/contents.xcworkspacedata b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/contents.xcworkspacedata
new file mode 100644
index 0000000..d35959a
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/contents.xcworkspacedata
@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+ version = "1.0">
+ <FileRef
+ location = "self:StartedAprilTag.xcodeproj">
+ </FileRef>
+</Workspace>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate
new file mode 100644
index 0000000..5b5af16
Binary files /dev/null and b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate differ
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/StartedAprilTag.xcscheme b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/StartedAprilTag.xcscheme
new file mode 100644
index 0000000..cb899fb
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/StartedAprilTag.xcscheme
@@ -0,0 +1,91 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Scheme
+ LastUpgradeVersion = "0830"
+ version = "1.3">
+ <BuildAction
+ parallelizeBuildables = "YES"
+ buildImplicitDependencies = "YES">
+ <BuildActionEntries>
+ <BuildActionEntry
+ buildForTesting = "YES"
+ buildForRunning = "YES"
+ buildForProfiling = "YES"
+ buildForArchiving = "YES"
+ buildForAnalyzing = "YES">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4BC3FE221F876BE9006A29FB"
+ BuildableName = "StartedAprilTag.app"
+ BlueprintName = "StartedAprilTag"
+ ReferencedContainer = "container:StartedAprilTag.xcodeproj">
+ </BuildableReference>
+ </BuildActionEntry>
+ </BuildActionEntries>
+ </BuildAction>
+ <TestAction
+ buildConfiguration = "Debug"
+ selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+ selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+ shouldUseLaunchSchemeArgsEnv = "YES">
+ <Testables>
+ </Testables>
+ <MacroExpansion>
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4BC3FE221F876BE9006A29FB"
+ BuildableName = "StartedAprilTag.app"
+ BlueprintName = "StartedAprilTag"
+ ReferencedContainer = "container:StartedAprilTag.xcodeproj">
+ </BuildableReference>
+ </MacroExpansion>
+ <AdditionalOptions>
+ </AdditionalOptions>
+ </TestAction>
+ <LaunchAction
+ buildConfiguration = "Debug"
+ selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+ selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+ launchStyle = "0"
+ useCustomWorkingDirectory = "NO"
+ ignoresPersistentStateOnLaunch = "NO"
+ debugDocumentVersioning = "YES"
+ debugServiceExtension = "internal"
+ allowLocationSimulation = "YES">
+ <BuildableProductRunnable
+ runnableDebuggingMode = "0">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4BC3FE221F876BE9006A29FB"
+ BuildableName = "StartedAprilTag.app"
+ BlueprintName = "StartedAprilTag"
+ ReferencedContainer = "container:StartedAprilTag.xcodeproj">
+ </BuildableReference>
+ </BuildableProductRunnable>
+ <AdditionalOptions>
+ </AdditionalOptions>
+ </LaunchAction>
+ <ProfileAction
+ buildConfiguration = "Release"
+ shouldUseLaunchSchemeArgsEnv = "YES"
+ savedToolIdentifier = ""
+ useCustomWorkingDirectory = "NO"
+ debugDocumentVersioning = "YES">
+ <BuildableProductRunnable
+ runnableDebuggingMode = "0">
+ <BuildableReference
+ BuildableIdentifier = "primary"
+ BlueprintIdentifier = "4BC3FE221F876BE9006A29FB"
+ BuildableName = "StartedAprilTag.app"
+ BlueprintName = "StartedAprilTag"
+ ReferencedContainer = "container:StartedAprilTag.xcodeproj">
+ </BuildableReference>
+ </BuildableProductRunnable>
+ </ProfileAction>
+ <AnalyzeAction
+ buildConfiguration = "Debug">
+ </AnalyzeAction>
+ <ArchiveAction
+ buildConfiguration = "Release"
+ revealArchiveInOrganizer = "YES">
+ </ArchiveAction>
+</Scheme>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist
new file mode 100644
index 0000000..e2033ab
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag.xcodeproj/xcuserdata/fspindle.xcuserdatad/xcschemes/xcschememanagement.plist
@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>SchemeUserState</key>
+ <dict>
+ <key>StartedAprilTag.xcscheme</key>
+ <dict>
+ <key>orderHint</key>
+ <integer>0</integer>
+ </dict>
+ </dict>
+ <key>SuppressBuildableAutocreation</key>
+ <dict>
+ <key>4BC3FE221F876BE9006A29FB</key>
+ <dict>
+ <key>primary</key>
+ <true/>
+ </dict>
+ </dict>
+</dict>
+</plist>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.h
new file mode 100644
index 0000000..2ccb23f
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.h
@@ -0,0 +1,11 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import <UIKit/UIKit.h>
+
+ at interface AppDelegate : UIResponder <UIApplicationDelegate>
+
+ at property(strong, nonatomic) UIWindow *window;
+
+ at end
+
+#endif
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.m b/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.m
new file mode 100644
index 0000000..009fae0
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/AppDelegate.m
@@ -0,0 +1,47 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import "AppDelegate.h"
+
+ at interface AppDelegate ()
+
+ at end
+
+ at implementation AppDelegate
+
+
+- (BOOL)application:(UIApplication *)application didFinishLaunchingWithOptions:(NSDictionary *)launchOptions {
+ // Override point for customization after application launch.
+ return YES;
+}
+
+
+- (void)applicationWillResignActive:(UIApplication *)application {
+ // Sent when the application is about to move from active to inactive state. This can occur for certain types of temporary interruptions (such as an incoming phone call or SMS message) or when the user quits the application and it begins the transition to the background state.
+ // Use this method to pause ongoing tasks, disable timers, and invalidate graphics rendering callbacks. Games should use this method to pause the game.
+}
+
+
+- (void)applicationDidEnterBackground:(UIApplication *)application {
+ // Use this method to release shared resources, save user data, invalidate timers, and store enough application state information to restore your application to its current state in case it is terminated later.
+ // If your application supports background execution, this method is called instead of applicationWillTerminate: when the user quits.
+}
+
+
+- (void)applicationWillEnterForeground:(UIApplication *)application {
+ // Called as part of the transition from the background to the active state; here you can undo many of the changes made on entering the background.
+}
+
+
+- (void)applicationDidBecomeActive:(UIApplication *)application {
+ // Restart any tasks that were paused (or not yet started) while the application was inactive. If the application was previously in the background, optionally refresh the user interface.
+}
+
+
+- (void)applicationWillTerminate:(UIApplication *)application {
+ // Called when the application is about to terminate. Save data if appropriate. See also applicationDidEnterBackground:.
+}
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/AprilTag.png b/tutorial/ios/StartedAprilTag/StartedAprilTag/AprilTag.png
new file mode 100644
index 0000000..9e48871
Binary files /dev/null and b/tutorial/ios/StartedAprilTag/StartedAprilTag/AprilTag.png differ
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/Assets.xcassets/AppIcon.appiconset/Contents.json b/tutorial/ios/StartedAprilTag/StartedAprilTag/Assets.xcassets/AppIcon.appiconset/Contents.json
new file mode 100644
index 0000000..36d2c80
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/Assets.xcassets/AppIcon.appiconset/Contents.json
@@ -0,0 +1,68 @@
+{
+ "images" : [
+ {
+ "idiom" : "iphone",
+ "size" : "29x29",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "29x29",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "40x40",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "40x40",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "60x60",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "iphone",
+ "size" : "60x60",
+ "scale" : "3x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "29x29",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "29x29",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "40x40",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "40x40",
+ "scale" : "2x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "76x76",
+ "scale" : "1x"
+ },
+ {
+ "idiom" : "ipad",
+ "size" : "76x76",
+ "scale" : "2x"
+ }
+ ],
+ "info" : {
+ "version" : 1,
+ "author" : "xcode"
+ }
+}
\ No newline at end of file
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/LaunchScreen.storyboard b/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/LaunchScreen.storyboard
new file mode 100644
index 0000000..fdf3f97
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/LaunchScreen.storyboard
@@ -0,0 +1,27 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="11134" systemVersion="15F34" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+ <dependencies>
+ <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="11106"/>
+ <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+ </dependencies>
+ <scenes>
+ <!--View Controller-->
+ <scene sceneID="EHf-IW-A2E">
+ <objects>
+ <viewController id="01J-lp-oVM" sceneMemberID="viewController">
+ <layoutGuides>
+ <viewControllerLayoutGuide type="top" id="Llm-lL-Icb"/>
+ <viewControllerLayoutGuide type="bottom" id="xb3-aO-Qok"/>
+ </layoutGuides>
+ <view key="view" contentMode="scaleToFill" id="Ze5-6b-2t3">
+ <rect key="frame" x="0.0" y="0.0" width="375" height="667"/>
+ <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+ <color key="backgroundColor" red="1" green="1" blue="1" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+ </view>
+ </viewController>
+ <placeholder placeholderIdentifier="IBFirstResponder" id="iYj-Kq-Ea1" userLabel="First Responder" sceneMemberID="firstResponder"/>
+ </objects>
+ <point key="canvasLocation" x="53" y="375"/>
+ </scene>
+ </scenes>
+</document>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/Main.storyboard b/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/Main.storyboard
new file mode 100644
index 0000000..4529698
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/Base.lproj/Main.storyboard
@@ -0,0 +1,26 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="11134" systemVersion="15F34" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" colorMatched="YES" initialViewController="BYZ-38-t0r">
+ <dependencies>
+ <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="11106"/>
+ <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+ </dependencies>
+ <scenes>
+ <!--View Controller-->
+ <scene sceneID="tne-QT-ifu">
+ <objects>
+ <viewController id="BYZ-38-t0r" customClass="ViewController" customModuleProvider="" sceneMemberID="viewController">
+ <layoutGuides>
+ <viewControllerLayoutGuide type="top" id="y3c-jy-aDJ"/>
+ <viewControllerLayoutGuide type="bottom" id="wfy-db-euE"/>
+ </layoutGuides>
+ <view key="view" contentMode="scaleToFill" id="8bC-Xf-vdC">
+ <rect key="frame" x="0.0" y="0.0" width="375" height="667"/>
+ <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+ <color key="backgroundColor" red="1" green="1" blue="1" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+ </view>
+ </viewController>
+ <placeholder placeholderIdentifier="IBFirstResponder" id="dkx-z0-nzr" sceneMemberID="firstResponder"/>
+ </objects>
+ </scene>
+ </scenes>
+</document>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h
new file mode 100644
index 0000000..5e7330d
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#import <Foundation/Foundation.h>
+#import <UIKit/UIKit.h>
+
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ at interface ImageConversion : NSObject
+
++ (vpImage<vpRGBa>)vpImageColorFromUIImage:(UIImage *)image;
++ (vpImage<unsigned char>)vpImageGrayFromUIImage:(UIImage *)image;
++ (UIImage *)UIImageFromVpImageColor:(const vpImage<vpRGBa> &)I;
++ (UIImage *)UIImageFromVpImageGray:(const vpImage<unsigned char> &)I;
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm
new file mode 100644
index 0000000..447d154
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageConversion.mm
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import "ImageConversion.h"
+
+ at implementation ImageConversion
+
+
+//! [vpImageColorFromUIImage]
+// Converts an UIImage that could be in gray or color into a ViSP color image
++ (vpImage<vpRGBa>)vpImageColorFromUIImage:(UIImage *)image
+{
+ CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
+
+ if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
+ NSLog(@"Input UIImage is grayscale");
+ vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
+
+ CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNone |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ vpImage<vpRGBa> color;
+ vpImageConvert::convert(gray, color);
+
+ return color;
+ }
+ else {
+ NSLog(@"Input UIImage is color");
+ vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ 4 * image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNoneSkipLast |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ return color;
+ }
+}
+//! [vpImageColorFromUIImage]
+
+//! [vpImageGrayFromUIImage]
+// Converts an UIImage that could be in gray or color into a ViSP gray image
++ (vpImage<unsigned char>)vpImageGrayFromUIImage:(UIImage *)image
+{
+ CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
+
+ if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
+ NSLog(@"Input UIImage is grayscale");
+ vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
+
+ CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNone |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ return gray;
+ } else {
+ NSLog(@"Input UIImage is color");
+ vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels (color channels + alpha)
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ 4 * image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNoneSkipLast |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ vpImage<unsigned char> gray;
+ vpImageConvert::convert(color, gray);
+
+ return gray;
+ }
+}
+//! [vpImageGrayFromUIImage]
+
+//! [UIImageFromVpImageColor]
+// Converts a color ViSP image into a color UIImage
++ (UIImage *)UIImageFromVpImageColor:(const vpImage<vpRGBa> &)I
+{
+ NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()*4];
+ CGColorSpaceRef colorSpace;
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
+
+ // Creating CGImage from vpImage
+ CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
+ I.getHeight(), // height
+ 8, // bits per component
+ 8 * 4, // bits per pixel
+ 4 * I.getWidth(), // bytesPerRow
+ colorSpace, // colorspace
+ kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
+ provider, // CGDataProviderRef
+ NULL, // decode
+ false, // should interpolate
+ kCGRenderingIntentDefault // intent
+ );
+
+
+ // Getting UIImage from CGImage
+ UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
+ CGImageRelease(imageRef);
+ CGDataProviderRelease(provider);
+ CGColorSpaceRelease(colorSpace);
+
+ return finalImage;
+}
+//! [UIImageFromVpImageColor]
+
+//! [UIImageFromVpImageGray]
+// Converts a gray level ViSP image into a gray level UIImage
++ (UIImage *)UIImageFromVpImageGray:(const vpImage<unsigned char> &)I
+{
+ NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()];
+ CGColorSpaceRef colorSpace;
+
+ colorSpace = CGColorSpaceCreateDeviceGray();
+
+ CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
+
+ // Creating CGImage from vpImage
+ CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
+ I.getHeight(), // height
+ 8, // bits per component
+ 8, // bits per pixel
+ I.getWidth(), // bytesPerRow
+ colorSpace, // colorspace
+ kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
+ provider, // CGDataProviderRef
+ NULL, // decode
+ false, // should interpolate
+ kCGRenderingIntentDefault // intent
+ );
+
+
+ // Getting UIImage from CGImage
+ UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
+ CGImageRelease(imageRef);
+ CGDataProviderRelease(provider);
+ CGColorSpaceRelease(colorSpace);
+
+ return finalImage;
+}
+//! [UIImageFromVpImageGray]
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h
new file mode 100644
index 0000000..9b8ca04
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.h
@@ -0,0 +1,49 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#import <UIKit/UIKit.h>
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ at interface ImageDisplay : NSObject
+
++ (UIImage *)displayLine:(UIImage *)image :(vpImagePoint &)ip1 :(vpImagePoint &)ip2 :(UIColor*)color :(int)tickness;
++ (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(const vpCameraParameters &)cam
+ :(double) size :(int)tickness;
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm
new file mode 100644
index 0000000..b9853cb
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ImageDisplay.mm
@@ -0,0 +1,140 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import <Foundation/Foundation.h>
+
+#import "ImageDisplay.h"
+
+ at implementation ImageDisplay
+
+//! [display line]
+// UIImage *image = <the image you want to add a line to>
+// vpImagePoint &ip1 = Line first point
+// vpImagePoint &ip2 = Line second point
+// UIColor *color = <the color of the line>
+// int tickness = <the tickness of the lines on the AprilTag contour>
++ (UIImage *)displayLine:(UIImage *)image :(vpImagePoint &)ip1 :(vpImagePoint &)ip2 :(UIColor*)color :(int)tickness
+{
+ UIGraphicsBeginImageContext(image.size);
+
+ // Draw the original image as the background
+ [image drawAtPoint:CGPointMake(0,0)];
+
+ // Draw the line on top of original image
+ CGContextRef context = UIGraphicsGetCurrentContext();
+ CGContextSetLineWidth(context, tickness);
+ CGContextSetStrokeColorWithColor(context, [color CGColor]);
+
+ CGContextMoveToPoint(context, ip1.get_u(), ip1.get_v());
+ CGContextAddLineToPoint(context, ip2.get_u(), ip2.get_v());
+
+ CGContextStrokePath(context);
+
+ // Create new image
+ UIImage *retImage = UIGraphicsGetImageFromCurrentImageContext();
+
+ // Tidy up
+ UIGraphicsEndImageContext();
+ return retImage;
+}
+//! [display line]
+
+//! [display frame]
+// UIImage *image = <the image you want to add a line to>
+// vpHomogeneousMatrix cMo = <Homegeneous transformation>
+// vpCameraParameters cam = <Camera parameters>
+// double size = <Size of the frame in meter>
+// int tickness = <the tickness of the lines describing the frame>
++ (UIImage *)displayFrame:(UIImage *)image :(const vpHomogeneousMatrix &)cMo :(const vpCameraParameters &)cam
+ :(double) size :(int)tickness
+{
+ UIGraphicsBeginImageContext(image.size);
+
+ // Draw the original image as the background
+ [image drawAtPoint:CGPointMake(0,0)];
+
+ vpPoint o( 0.0, 0.0, 0.0);
+ vpPoint x(size, 0.0, 0.0);
+ vpPoint y( 0.0, size, 0.0);
+ vpPoint z( 0.0, 0.0, size);
+
+ o.track(cMo);
+ x.track(cMo);
+ y.track(cMo);
+ z.track(cMo);
+
+ vpImagePoint ipo, ip1;
+
+ vpMeterPixelConversion::convertPoint (cam, o.p[0], o.p[1], ipo);
+
+ // Draw red line on top of original image
+ vpMeterPixelConversion::convertPoint (cam, x.p[0], x.p[1], ip1);
+ CGContextRef context = UIGraphicsGetCurrentContext();
+ CGContextSetLineWidth(context, tickness);
+ CGContextSetStrokeColorWithColor(context, [[UIColor redColor] CGColor]);
+ CGContextMoveToPoint(context, ipo.get_u(), ipo.get_v());
+ CGContextAddLineToPoint(context, ip1.get_u(), ip1.get_v());
+ CGContextStrokePath(context);
+
+ // Draw green line on top of original image
+ vpMeterPixelConversion::convertPoint ( cam, y.p[0], y.p[1], ip1) ;
+ context = UIGraphicsGetCurrentContext();
+ CGContextSetLineWidth(context, tickness);
+ CGContextSetStrokeColorWithColor(context, [[UIColor greenColor] CGColor]);
+ CGContextMoveToPoint(context, ipo.get_u(), ipo.get_v());
+ CGContextAddLineToPoint(context, ip1.get_u(), ip1.get_v());
+ CGContextStrokePath(context);
+
+ // Draw blue line on top of original image
+ vpMeterPixelConversion::convertPoint ( cam, z.p[0], z.p[1], ip1) ;
+ context = UIGraphicsGetCurrentContext();
+ CGContextSetLineWidth(context, tickness);
+ CGContextSetStrokeColorWithColor(context, [[UIColor blueColor] CGColor]);
+ CGContextMoveToPoint(context, ipo.get_u(), ipo.get_v());
+ CGContextAddLineToPoint(context, ip1.get_u(), ip1.get_v());
+ CGContextStrokePath(context);
+
+ // Create new image
+ UIImage *retImage = UIGraphicsGetImageFromCurrentImageContext();
+
+ // Tidy up
+ UIGraphicsEndImageContext();
+ return retImage;
+}
+//! [display frame]
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/Info.plist b/tutorial/ios/StartedAprilTag/StartedAprilTag/Info.plist
new file mode 100644
index 0000000..d052473
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/Info.plist
@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+ <key>CFBundleDevelopmentRegion</key>
+ <string>en</string>
+ <key>CFBundleExecutable</key>
+ <string>$(EXECUTABLE_NAME)</string>
+ <key>CFBundleIdentifier</key>
+ <string>$(PRODUCT_BUNDLE_IDENTIFIER)</string>
+ <key>CFBundleInfoDictionaryVersion</key>
+ <string>6.0</string>
+ <key>CFBundleName</key>
+ <string>$(PRODUCT_NAME)</string>
+ <key>CFBundlePackageType</key>
+ <string>APPL</string>
+ <key>CFBundleShortVersionString</key>
+ <string>1.0</string>
+ <key>CFBundleVersion</key>
+ <string>1</string>
+ <key>LSRequiresIPhoneOS</key>
+ <true/>
+ <key>UILaunchStoryboardName</key>
+ <string>LaunchScreen</string>
+ <key>UIMainStoryboardFile</key>
+ <string>Main</string>
+ <key>UIRequiredDeviceCapabilities</key>
+ <array>
+ <string>armv7</string>
+ </array>
+ <key>UISupportedInterfaceOrientations</key>
+ <array>
+ <string>UIInterfaceOrientationPortrait</string>
+ <string>UIInterfaceOrientationLandscapeLeft</string>
+ <string>UIInterfaceOrientationLandscapeRight</string>
+ </array>
+ <key>UISupportedInterfaceOrientations~ipad</key>
+ <array>
+ <string>UIInterfaceOrientationPortrait</string>
+ <string>UIInterfaceOrientationPortraitUpsideDown</string>
+ <string>UIInterfaceOrientationLandscapeLeft</string>
+ <string>UIInterfaceOrientationLandscapeRight</string>
+ </array>
+</dict>
+</plist>
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.h b/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.h
new file mode 100644
index 0000000..fda0a08
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.h
@@ -0,0 +1,11 @@
+#import <UIKit/UIKit.h>
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ at interface ViewController : UIViewController
+
+ at property(nonatomic, strong) UIImageView *myImageView;
+
+ at end
+
+#endif
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.mm b/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.mm
new file mode 100644
index 0000000..7e2663c
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/ViewController.mm
@@ -0,0 +1,89 @@
+#import "ViewController.h"
+#import "ImageConversion.h"
+#import "ImageDisplay.h"
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+ at interface ViewController ()
+ at end
+
+ at implementation ViewController
+
+ at synthesize myImageView;
+#endif
+
+
+- (void)viewDidLoad {
+
+ [super viewDidLoad];
+
+ // Load an image
+ UIImage *img = [UIImage imageNamed:@"AprilTag.png"];
+
+ // Image view instance to display the image
+ self.myImageView = [[UIImageView alloc] initWithImage:img];
+
+ // Set the frame for the image view
+ CGRect myFrame = CGRectMake(0.0f, 0.0f, self.myImageView.frame.size.width, self.myImageView.frame.size.height);
+ [self.myImageView setFrame:myFrame];
+
+ // Add the image view to the current view
+ [self.view addSubview:self.myImageView];
+
+ // View image
+ [myImageView setImage:img];
+
+ // Convert image to visp
+ vpImage<unsigned char> I = [ImageConversion vpImageGrayFromUIImage:img];
+
+ // Detect AprilTag
+ vpDetectorAprilTag::vpAprilTagFamily tagFamily = vpDetectorAprilTag::TAG_36h11;
+ vpDetectorAprilTag::vpPoseEstimationMethod poseEstimationMethod = vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS;
+ double tagSize = 0.053;
+ float quad_decimate = 3.0;
+ int nThreads = 1;
+ std::vector<vpHomogeneousMatrix> cMo_vec;
+
+ // Set camera parameters
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(615.1674805, 615.1675415, 312.1889954, 243.4373779);
+
+ // Initialize apriltag detector
+ vpDetectorAprilTag detector(tagFamily);
+ detector.setAprilTagQuadDecimate(quad_decimate);
+ detector.setAprilTagPoseEstimationMethod(poseEstimationMethod);
+ detector.setAprilTagNbThreads(nThreads);
+
+ // Detect all the tags in the image
+ detector.detect(I, tagSize, cam, cMo_vec);
+
+ // Parse detected tags for printings
+ std::cout << "Number of tags in the image: " << detector.getNbObjects() << std::endl;
+ for(size_t i=0; i < detector.getNbObjects(); i++) {
+ std::cout << "- Detected tag: " << detector.getMessage(i) << std::endl;
+ std::cout << " pose: " << cMo_vec[i] << std::endl;
+ }
+
+ // Draw red lines arround each tag
+ for (size_t i = 0; i < detector.getNbObjects() ; i++) {
+ std::vector<vpImagePoint> polygon = detector.getPolygon(i);
+ for (size_t j = 0; j < polygon.size(); j++) {
+ img = [ImageDisplay displayLine:img :polygon[j] :polygon[(j+1)%polygon.size()] :[UIColor redColor] :2];
+ }
+ }
+
+ // Draw a frame corresponding to the tag 3D location
+ for(size_t i=0; i < detector.getNbObjects(); i++) {
+ img = [ImageDisplay displayFrame:img :cMo_vec[i] :cam :0.025 :2];
+ }
+ [myImageView setImage:img];
+}
+
+- (void)didReceiveMemoryWarning {
+ [super didReceiveMemoryWarning];
+ // Dispose of any resources that can be recreated.
+}
+
+ at end
diff --git a/tutorial/ios/StartedAprilTag/StartedAprilTag/main.m b/tutorial/ios/StartedAprilTag/StartedAprilTag/main.m
new file mode 100644
index 0000000..d7a9403
--- /dev/null
+++ b/tutorial/ios/StartedAprilTag/StartedAprilTag/main.m
@@ -0,0 +1,16 @@
+//
+// main.m
+// StartedAprilTag
+//
+// Created by Fabien Spindler on 06/10/2017.
+// Copyright © 2017 MyOrganization. All rights reserved.
+//
+
+#import <UIKit/UIKit.h>
+#import "AppDelegate.h"
+
+int main(int argc, char * argv[]) {
+ @autoreleasepool {
+ return UIApplicationMain(argc, argv, nil, NSStringFromClass([AppDelegate class]));
+ }
+}
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.pbxproj b/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.pbxproj
index 9c25039..f6e5eda 100644
--- a/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.pbxproj
+++ b/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.pbxproj
@@ -7,6 +7,7 @@
objects = {
/* Begin PBXBuildFile section */
+ 4B4DDFA71F876EBA00369048 /* ImageConversion.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4B4DDFA61F876EBA00369048 /* ImageConversion.mm */; };
4BB56F241D9B99EF00C8F8FE /* main.m in Sources */ = {isa = PBXBuildFile; fileRef = 4BB56F231D9B99EF00C8F8FE /* main.m */; };
4BB56F271D9B99EF00C8F8FE /* AppDelegate.m in Sources */ = {isa = PBXBuildFile; fileRef = 4BB56F261D9B99EF00C8F8FE /* AppDelegate.m */; };
4BB56F2A1D9B99EF00C8F8FE /* ViewController.mm in Sources */ = {isa = PBXBuildFile; fileRef = 4BB56F291D9B99EF00C8F8FE /* ViewController.mm */; };
@@ -17,6 +18,8 @@
/* End PBXBuildFile section */
/* Begin PBXFileReference section */
+ 4B4DDFA51F876EBA00369048 /* ImageConversion.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ImageConversion.h; sourceTree = "<group>"; };
+ 4B4DDFA61F876EBA00369048 /* ImageConversion.mm */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.cpp.objcpp; path = ImageConversion.mm; sourceTree = "<group>"; };
4BB56F1F1D9B99EF00C8F8FE /* StartedImageProc.app */ = {isa = PBXFileReference; explicitFileType = wrapper.application; includeInIndex = 0; path = StartedImageProc.app; sourceTree = BUILT_PRODUCTS_DIR; };
4BB56F231D9B99EF00C8F8FE /* main.m */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.objc; path = main.m; sourceTree = "<group>"; };
4BB56F251D9B99EF00C8F8FE /* AppDelegate.h */ = {isa = PBXFileReference; lastKnownFileType = sourcecode.c.h; path = AppDelegate.h; sourceTree = "<group>"; };
@@ -63,6 +66,8 @@
4BF38A311D9BCAC500609A7C /* monkey.png */,
4BB56F251D9B99EF00C8F8FE /* AppDelegate.h */,
4BB56F261D9B99EF00C8F8FE /* AppDelegate.m */,
+ 4B4DDFA51F876EBA00369048 /* ImageConversion.h */,
+ 4B4DDFA61F876EBA00369048 /* ImageConversion.mm */,
4BB56F281D9B99EF00C8F8FE /* ViewController.h */,
4BB56F291D9B99EF00C8F8FE /* ViewController.mm */,
4BB56F2B1D9B99EF00C8F8FE /* Main.storyboard */,
@@ -157,6 +162,7 @@
4BB56F2A1D9B99EF00C8F8FE /* ViewController.mm in Sources */,
4BB56F271D9B99EF00C8F8FE /* AppDelegate.m in Sources */,
4BB56F241D9B99EF00C8F8FE /* main.m in Sources */,
+ 4B4DDFA71F876EBA00369048 /* ImageConversion.mm in Sources */,
);
runOnlyForDeploymentPostprocessing = 0;
};
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate b/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate
index 0ffb954..5a05742 100644
Binary files a/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate and b/tutorial/ios/StartedImageProc/StartedImageProc.xcodeproj/project.xcworkspace/xcuserdata/fspindle.xcuserdatad/UserInterfaceState.xcuserstate differ
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.h b/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.h
index 340aee5..2ccb23f 100644
--- a/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.h
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.h
@@ -1,17 +1,11 @@
-//
-// AppDelegate.h
-// StartedImageProc
-//
-// Created by Fabien Spindler on 28/09/2016.
-// Copyright © 2016 MyOrganization. All rights reserved.
-//
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
#import <UIKit/UIKit.h>
@interface AppDelegate : UIResponder <UIApplicationDelegate>
- at property (strong, nonatomic) UIWindow *window;
-
+ at property(strong, nonatomic) UIWindow *window;
@end
+#endif
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.m b/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.m
index 4826358..009fae0 100644
--- a/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.m
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/AppDelegate.m
@@ -1,10 +1,4 @@
-//
-// AppDelegate.m
-// StartedImageProc
-//
-// Created by Fabien Spindler on 28/09/2016.
-// Copyright © 2016 MyOrganization. All rights reserved.
-//
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
#import "AppDelegate.h"
@@ -47,5 +41,7 @@
// Called when the application is about to terminate. Save data if appropriate. See also applicationDidEnterBackground:.
}
-
@end
+
+#endif
+
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h
new file mode 100644
index 0000000..5e7330d
--- /dev/null
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#import <Foundation/Foundation.h>
+#import <UIKit/UIKit.h>
+
+#ifdef __cplusplus
+#import <visp3/visp.h>
+#endif
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+ at interface ImageConversion : NSObject
+
++ (vpImage<vpRGBa>)vpImageColorFromUIImage:(UIImage *)image;
++ (vpImage<unsigned char>)vpImageGrayFromUIImage:(UIImage *)image;
++ (UIImage *)UIImageFromVpImageColor:(const vpImage<vpRGBa> &)I;
++ (UIImage *)UIImageFromVpImageGray:(const vpImage<unsigned char> &)I;
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm
new file mode 100644
index 0000000..447d154
--- /dev/null
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/ImageConversion.mm
@@ -0,0 +1,214 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
+#import "ImageConversion.h"
+
+ at implementation ImageConversion
+
+
+//! [vpImageColorFromUIImage]
+// Converts an UIImage that could be in gray or color into a ViSP color image
++ (vpImage<vpRGBa>)vpImageColorFromUIImage:(UIImage *)image
+{
+ CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
+
+ if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
+ NSLog(@"Input UIImage is grayscale");
+ vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
+
+ CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNone |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ vpImage<vpRGBa> color;
+ vpImageConvert::convert(gray, color);
+
+ return color;
+ }
+ else {
+ NSLog(@"Input UIImage is color");
+ vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ 4 * image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNoneSkipLast |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ return color;
+ }
+}
+//! [vpImageColorFromUIImage]
+
+//! [vpImageGrayFromUIImage]
+// Converts an UIImage that could be in gray or color into a ViSP gray image
++ (vpImage<unsigned char>)vpImageGrayFromUIImage:(UIImage *)image
+{
+ CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
+
+ if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
+ NSLog(@"Input UIImage is grayscale");
+ vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
+
+ CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNone |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ return gray;
+ } else {
+ NSLog(@"Input UIImage is color");
+ vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels (color channels + alpha)
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
+ image.size.width, // width of bitmap
+ image.size.height, // height of bitmap
+ 8, // bits per component
+ 4 * image.size.width, // bytes per row
+ colorSpace, // colorspace
+ kCGImageAlphaNoneSkipLast |
+ kCGBitmapByteOrderDefault); // bitmap info flags
+
+ CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
+ CGContextRelease(contextRef);
+
+ vpImage<unsigned char> gray;
+ vpImageConvert::convert(color, gray);
+
+ return gray;
+ }
+}
+//! [vpImageGrayFromUIImage]
+
+//! [UIImageFromVpImageColor]
+// Converts a color ViSP image into a color UIImage
++ (UIImage *)UIImageFromVpImageColor:(const vpImage<vpRGBa> &)I
+{
+ NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()*4];
+ CGColorSpaceRef colorSpace;
+
+ colorSpace = CGColorSpaceCreateDeviceRGB();
+
+ CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
+
+ // Creating CGImage from vpImage
+ CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
+ I.getHeight(), // height
+ 8, // bits per component
+ 8 * 4, // bits per pixel
+ 4 * I.getWidth(), // bytesPerRow
+ colorSpace, // colorspace
+ kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
+ provider, // CGDataProviderRef
+ NULL, // decode
+ false, // should interpolate
+ kCGRenderingIntentDefault // intent
+ );
+
+
+ // Getting UIImage from CGImage
+ UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
+ CGImageRelease(imageRef);
+ CGDataProviderRelease(provider);
+ CGColorSpaceRelease(colorSpace);
+
+ return finalImage;
+}
+//! [UIImageFromVpImageColor]
+
+//! [UIImageFromVpImageGray]
+// Converts a gray level ViSP image into a gray level UIImage
++ (UIImage *)UIImageFromVpImageGray:(const vpImage<unsigned char> &)I
+{
+ NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()];
+ CGColorSpaceRef colorSpace;
+
+ colorSpace = CGColorSpaceCreateDeviceGray();
+
+ CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
+
+ // Creating CGImage from vpImage
+ CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
+ I.getHeight(), // height
+ 8, // bits per component
+ 8, // bits per pixel
+ I.getWidth(), // bytesPerRow
+ colorSpace, // colorspace
+ kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
+ provider, // CGDataProviderRef
+ NULL, // decode
+ false, // should interpolate
+ kCGRenderingIntentDefault // intent
+ );
+
+
+ // Getting UIImage from CGImage
+ UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
+ CGImageRelease(imageRef);
+ CGDataProviderRelease(provider);
+ CGColorSpaceRelease(colorSpace);
+
+ return finalImage;
+}
+//! [UIImageFromVpImageGray]
+
+ at end
+
+#endif
+
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.h b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.h
index 07bdb9c..bcbae78 100644
--- a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.h
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.h
@@ -1,8 +1,11 @@
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
#import <UIKit/UIKit.h>
@interface ViewController : UIViewController
- at property (nonatomic, strong) UIImageView *myImageView;
+ at property(nonatomic, strong) UIImageView *myImageView;
@end
+#endif
diff --git a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm
index 52f91e6..1a13f15 100644
--- a/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm
+++ b/tutorial/ios/StartedImageProc/StartedImageProc/ViewController.mm
@@ -1,4 +1,39 @@
+/****************************************************************************
+ *
+ * This file is part of the ViSP software.
+ * Copyright (C) 2005 - 2017 by Inria. All rights reserved.
+ *
+ * This software is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ * See the file LICENSE.txt at the root directory of this source
+ * distribution for additional information about the GNU GPL.
+ *
+ * For using ViSP with software that can not be combined with the GNU
+ * GPL, please contact Inria about acquiring a ViSP Professional
+ * Edition License.
+ *
+ * See http://visp.inria.fr for more information.
+ *
+ * This software was developed at:
+ * Inria Rennes - Bretagne Atlantique
+ * Campus Universitaire de Beaulieu
+ * 35042 Rennes Cedex
+ * France
+ *
+ * If you have questions regarding the use of this file, please contact
+ * Inria at visp at inria.fr
+ *
+ * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ *****************************************************************************/
+
+#ifndef DOXYGEN_SHOULD_SKIP_THIS
+
#import "ViewController.h"
+#import "ImageConversion.h"
#ifdef __cplusplus
#import <visp3/visp.h>
#endif
@@ -20,258 +55,90 @@ NSArray *process = [[NSArray alloc]initWithObjects:@"load image", @"convert to g
#endif
- (void)viewDidLoad {
-
- [super viewDidLoad];
-
- // create an image
- UIImage *myScreenShot = [UIImage imageNamed:@"monkey.png"];
-
- // image view instance to display the image
- self.myImageView = [[UIImageView alloc] initWithImage:myScreenShot];
-
- // set the frame for the image view
- CGRect myFrame = CGRectMake(0.0f, 0.0f, self.myImageView.frame.size.width*2, self.myImageView.frame.size.height*2);
- [self.myImageView setFrame:myFrame];
-
- // add the image view to the current view
- [self.view addSubview:self.myImageView];
-
- // create buttons
- CGFloat posx=140, posy=350;
- CGFloat padding = 50;
- CGSize button_size = CGSizeMake( 150, 25 );
- for (int i=0; i<[process count]; i++) {
- UIButton *button = [UIButton buttonWithType:UIButtonTypeRoundedRect];
- [button addTarget:self action:@selector(checkButtonClick:) forControlEvents:UIControlEventTouchUpInside];
- [button setTitle:[process objectAtIndex: i] forState:UIControlStateNormal];
-
- button.frame = CGRectMake(posx, posy+i*padding, button_size.width, button_size.height);
- [button setBackgroundColor:[UIColor blueColor]];
- [button setTitleColor:[UIColor whiteColor] forState:UIControlStateNormal];
- button.layer.cornerRadius = 10;
- [self.view addSubview:button];
- }
+
+ [super viewDidLoad];
+
+ // create an image
+ UIImage *myScreenShot = [UIImage imageNamed:@"monkey.png"];
+
+ // image view instance to display the image
+ self.myImageView = [[UIImageView alloc] initWithImage:myScreenShot];
+
+ // set the frame for the image view
+ CGRect myFrame = CGRectMake(0.0f, 0.0f, self.myImageView.frame.size.width*2, self.myImageView.frame.size.height*2);
+ [self.myImageView setFrame:myFrame];
+
+ // add the image view to the current view
+ [self.view addSubview:self.myImageView];
+
+ // create buttons
+ CGFloat posx=140, posy=350;
+ CGFloat padding = 50;
+ CGSize button_size = CGSizeMake( 150, 25 );
+ for (int i=0; i<[process count]; i++) {
+ UIButton *button = [UIButton buttonWithType:UIButtonTypeRoundedRect];
+ [button addTarget:self action:@selector(checkButtonClick:) forControlEvents:UIControlEventTouchUpInside];
+ [button setTitle:[process objectAtIndex: i] forState:UIControlStateNormal];
+
+ button.frame = CGRectMake(posx, posy+i*padding, button_size.width, button_size.height);
+ [button setBackgroundColor:[UIColor blueColor]];
+ [button setTitleColor:[UIColor whiteColor] forState:UIControlStateNormal];
+ button.layer.cornerRadius = 10;
+ [self.view addSubview:button];
+ }
}
- (void) checkButtonClick:(UIButton *)paramSender{
-
- UIButton *myButton = paramSender;
-
- //check which button was tapped
- if([myButton.currentTitle isEqualToString:[process objectAtIndex: 0]]){
- // load image
- NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 0]);
-
- [myImageView setImage:[UIImage imageNamed:@"monkey.png"]];
- }
- else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 1]]){
- // convert to gray
- NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 1]);
-
- UIImage *img = [UIImage imageNamed:@"monkey.png"];
- vpImage<unsigned char> gray = [self vpImageGrayFromUIImage:img];
- [myImageView setImage:[self UIImageFromVpImageGray:gray]];
- }
- else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 2]]){
- // compute gradient
- NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 2]);
-
- UIImage *img = [UIImage imageNamed:@"monkey.png"];
- vpImage<unsigned char> gray = [self vpImageGrayFromUIImage:img];
- vpImage<double> dIx;
- vpImageFilter::getGradX(gray, dIx);
- vpImageConvert::convert(dIx, gray);
-
- [myImageView setImage:[self UIImageFromVpImageGray:gray]];
- }
+
+ UIButton *myButton = paramSender;
+
+ //check which button was tapped
+ if([myButton.currentTitle isEqualToString:[process objectAtIndex: 0]]){
+ // load image
+ NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 0]);
+
+ [myImageView setImage:[UIImage imageNamed:@"monkey.png"]];
+ }
+ else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 1]]){
+ // convert to gray
+ NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 1]);
+
+ UIImage *img = [UIImage imageNamed:@"monkey.png"];
+ vpImage<unsigned char> gray = [ImageConversion vpImageGrayFromUIImage:img];
+ [myImageView setImage:[ImageConversion UIImageFromVpImageGray:gray]];
+ }
+ else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 2]]){
+ // compute gradient
+ NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 2]);
+
+ UIImage *img = [UIImage imageNamed:@"monkey.png"];
+ vpImage<unsigned char> gray = [ImageConversion vpImageGrayFromUIImage:img];
+ vpImage<double> dIx;
+ vpImageFilter::getGradX(gray, dIx);
+ vpImageConvert::convert(dIx, gray);
+
+ [myImageView setImage:[ImageConversion UIImageFromVpImageGray:gray]];
+ }
#if (VISP_HAVE_OPENCV_VERSION >= 0x020100)
- else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 3]]){
- // canny detector
- NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 3]);
-
- UIImage *img = [UIImage imageNamed:@"monkey.png"];
- vpImage<unsigned char> gray = [self vpImageGrayFromUIImage:img];
- vpImage<unsigned char> canny;
- vpImageFilter::canny(gray, canny, 5, 15, 3);
- [myImageView setImage:[self UIImageFromVpImageGray:canny]];
- }
+ else if([myButton.currentTitle isEqualToString:[process objectAtIndex: 3]]){
+ // canny detector
+ NSLog(@"Clicked on \"%@\" button ", [process objectAtIndex: 3]);
+
+ UIImage *img = [UIImage imageNamed:@"monkey.png"];
+ vpImage<unsigned char> gray = [ImageConversion vpImageGrayFromUIImage:img];
+ vpImage<unsigned char> canny;
+ vpImageFilter::canny(gray, canny, 5, 15, 3);
+ [myImageView setImage:[ImageConversion UIImageFromVpImageGray:canny]];
+ }
#endif
}
-//! [vpImageColorFromUIImage]
-// Converts an UIImage that could be in gray or color into a ViSP color image
-- (vpImage<vpRGBa>)vpImageColorFromUIImage:(UIImage *)image
-{
- CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
-
- if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
- NSLog(@"Input UIImage is grayscale");
- vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
-
- CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
- image.size.width, // width of bitmap
- image.size.height, // height of bitmap
- 8, // bits per component
- image.size.width, // bytes per row
- colorSpace, // colorspace
- kCGImageAlphaNone |
- kCGBitmapByteOrderDefault); // bitmap info flags
-
- CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
- CGContextRelease(contextRef);
-
- vpImage<vpRGBa> color;
- vpImageConvert::convert(gray, color);
-
- return color;
- }
- else {
- NSLog(@"Input UIImage is color");
- vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels
-
- colorSpace = CGColorSpaceCreateDeviceRGB();
-
- CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
- image.size.width, // width of bitmap
- image.size.height, // height of bitmap
- 8, // bits per component
- 4 * image.size.width, // bytes per row
- colorSpace, // colorspace
- kCGImageAlphaNoneSkipLast |
- kCGBitmapByteOrderDefault); // bitmap info flags
-
- CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
- CGContextRelease(contextRef);
-
- return color;
- }
-}
-//! [vpImageColorFromUIImage]
-
-//! [vpImageGrayFromUIImage]
-// Converts an UIImage that could be in gray or color into a ViSP gray image
-- (vpImage<unsigned char>)vpImageGrayFromUIImage:(UIImage *)image
-{
- CGColorSpaceRef colorSpace = CGImageGetColorSpace(image.CGImage);
-
- if (CGColorSpaceGetModel(colorSpace) == kCGColorSpaceModelMonochrome) {
- NSLog(@"Input UIImage is grayscale");
- vpImage<unsigned char> gray(image.size.height, image.size.width); // 8 bits per component, 1 channel
-
- CGContextRef contextRef = CGBitmapContextCreate(gray.bitmap, // pointer to data
- image.size.width, // width of bitmap
- image.size.height, // height of bitmap
- 8, // bits per component
- image.size.width, // bytes per row
- colorSpace, // colorspace
- kCGImageAlphaNone |
- kCGBitmapByteOrderDefault); // bitmap info flags
-
- CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
- CGContextRelease(contextRef);
-
- return gray;
- } else {
- NSLog(@"Input UIImage is color");
- vpImage<vpRGBa> color(image.size.height, image.size.width); // 8 bits per component, 4 channels (color channels + alpha)
-
- colorSpace = CGColorSpaceCreateDeviceRGB();
-
- CGContextRef contextRef = CGBitmapContextCreate(color.bitmap, // pointer to data
- image.size.width, // width of bitmap
- image.size.height, // height of bitmap
- 8, // bits per component
- 4 * image.size.width, // bytes per row
- colorSpace, // colorspace
- kCGImageAlphaNoneSkipLast |
- kCGBitmapByteOrderDefault); // bitmap info flags
-
- CGContextDrawImage(contextRef, CGRectMake(0, 0, image.size.width, image.size.height), image.CGImage);
- CGContextRelease(contextRef);
-
- vpImage<unsigned char> gray;
- vpImageConvert::convert(color, gray);
-
- return gray;
- }
-}
-//! [vpImageGrayFromUIImage]
-
-//! [UIImageFromVpImageColor]
-// Converts a color ViSP image into a color UIImage
--(UIImage *)UIImageFromVpImageColor:(vpImage<vpRGBa>)I
-{
- NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()*4];
- CGColorSpaceRef colorSpace;
-
- colorSpace = CGColorSpaceCreateDeviceRGB();
-
- CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
-
- // Creating CGImage from vpImage
- CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
- I.getHeight(), // height
- 8, // bits per component
- 8 * 4, // bits per pixel
- 4 * I.getWidth(), // bytesPerRow
- colorSpace, // colorspace
- kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
- provider, // CGDataProviderRef
- NULL, // decode
- false, // should interpolate
- kCGRenderingIntentDefault // intent
- );
-
-
- // Getting UIImage from CGImage
- UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
- CGImageRelease(imageRef);
- CGDataProviderRelease(provider);
- CGColorSpaceRelease(colorSpace);
-
- return finalImage;
-}
-//! [UIImageFromVpImageColor]
-
-//! [UIImageFromVpImageGray]
-// Converts a gray level ViSP image into a gray level UIImage
--(UIImage *)UIImageFromVpImageGray:(vpImage<unsigned char>)I
-{
- NSData *data = [NSData dataWithBytes:I.bitmap length:I.getSize()];
- CGColorSpaceRef colorSpace;
-
- colorSpace = CGColorSpaceCreateDeviceGray();
-
- CGDataProviderRef provider = CGDataProviderCreateWithCFData((__bridge CFDataRef)data);
-
- // Creating CGImage from vpImage
- CGImageRef imageRef = CGImageCreate(I.getWidth(), // width
- I.getHeight(), // height
- 8, // bits per component
- 8, // bits per pixel
- I.getWidth(), // bytesPerRow
- colorSpace, // colorspace
- kCGImageAlphaNone|kCGBitmapByteOrderDefault,// bitmap info
- provider, // CGDataProviderRef
- NULL, // decode
- false, // should interpolate
- kCGRenderingIntentDefault // intent
- );
-
-
- // Getting UIImage from CGImage
- UIImage *finalImage = [UIImage imageWithCGImage:imageRef];
- CGImageRelease(imageRef);
- CGDataProviderRelease(provider);
- CGColorSpaceRelease(colorSpace);
-
- return finalImage;
-}
-//! [UIImageFromVpImageGray]
-
- (void)didReceiveMemoryWarning {
- [super didReceiveMemoryWarning];
- // Dispose of any resources that can be recreated.
+ [super didReceiveMemoryWarning];
+ // Dispose of any resources that can be recreated.
}
@end
+
+#endif
+
diff --git a/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp b/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
index 0de85fb..01b3c56 100644
--- a/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
+++ b/tutorial/robot/pioneer/tutorial-pioneer-robot.cpp
@@ -36,8 +36,7 @@ int main()
robot.stopRunning();
robot.waitForRunExit();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
index 5ae93b4..944fdfa 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-adaptive.cpp
@@ -1,30 +1,31 @@
/*!
\example tutorial-simu-pioneer-continuous-gain-adaptive.cpp
- Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ Example that shows how to simulate a visual servoing on a Pioneer mobile
+ robot equipped with a camera. The current visual features that are used are
+ s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- x the abscisse of the point measured at each iteration
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
*/
#include <iostream>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorPioneer.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -37,16 +38,16 @@ int main()
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
- vpRotationMatrix cRo(0, atan2( cMo[0][3], cMo[1][3]), 0);
+ vpRotationMatrix cRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cRo);
- vpSimulatorPioneer robot ;
+ vpSimulatorPioneer robot;
robot.setSamplingTime(0.04);
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point(0,0,0);
+ vpPoint point(0, 0, 0);
point.track(cMo);
vpServo task;
@@ -69,7 +70,7 @@ int main()
vpFeatureDepth s_Z, s_Zd;
double Z = point.get_Z();
double Zd = cdMo[2][3];
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
s_Zd.buildFrom(0, 0, Zd, 0);
task.addFeature(s_Z, s_Zd);
@@ -78,9 +79,9 @@ int main()
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
- graph.initGraph(0,2);
- graph.initGraph(1,2);
- graph.initGraph(2,1);
+ graph.initGraph(0, 2);
+ graph.initGraph(1, 2);
+ graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
@@ -92,33 +93,32 @@ int main()
#endif
int iter = 0;
- for (; ;)
- {
- robot.getPosition(wMc) ;
+ for (;;) {
+ robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
vpFeatureBuilder::create(s_x, point);
- Z = point.get_Z() ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
+ Z = point.get_Z();
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
task.set_cVe(cVe);
robot.get_eJe(eJe);
task.set_eJe(eJe);
- vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());
+ vpColVector v = task.computeControlLaw(iter * robot.getSamplingTime());
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
- 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
- graph.plot(2, 0, iter, Z); // plot the depth
+ graph.plot(2, 0, iter, Z); // plot the depth
#endif
- iter ++;
+ iter++;
if (task.getError().sumSquare() < 0.0001) {
std::cout << "Reached a small error. We stop the loop... " << std::endl;
@@ -130,18 +130,16 @@ int main()
graph.saveData(1, "./error2.dat");
const char *legend = "Click to quit...";
- vpDisplay::displayText(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);
+ vpDisplay::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
vpDisplay::flush(graph.I);
vpDisplay::getClick(graph.I);
#endif
-
// Kill the servo task
- task.print() ;
+ task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
index d11ebc3..10d7bc9 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-continuous-gain-constant.cpp
@@ -1,30 +1,31 @@
/*!
\example tutorial-simu-pioneer-continuous-gain-constant.cpp
- Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ Example that shows how to simulate a visual servoing on a Pioneer mobile
+ robot equipped with a camera. The current visual features that are used are
+ s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- x the abscisse of the point measured at each iteration
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
*/
#include <iostream>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorPioneer.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -37,16 +38,16 @@ int main()
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
- vpRotationMatrix cRo(0, atan2( cMo[0][3], cMo[1][3]), 0);
+ vpRotationMatrix cRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cRo);
- vpSimulatorPioneer robot ;
+ vpSimulatorPioneer robot;
robot.setSamplingTime(0.04);
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point(0,0,0);
+ vpPoint point(0, 0, 0);
point.track(cMo);
vpServo task;
@@ -69,7 +70,7 @@ int main()
vpFeatureDepth s_Z, s_Zd;
double Z = point.get_Z();
double Zd = cdMo[2][3];
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
s_Zd.buildFrom(0, 0, Zd, 0);
task.addFeature(s_Z, s_Zd);
@@ -78,9 +79,9 @@ int main()
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
- graph.initGraph(0,2);
- graph.initGraph(1,2);
- graph.initGraph(2,1);
+ graph.initGraph(0, 2);
+ graph.initGraph(1, 2);
+ graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
@@ -92,33 +93,32 @@ int main()
#endif
int iter = 0;
- for (; ;)
- {
- robot.getPosition(wMc) ;
+ for (;;) {
+ robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
vpFeatureBuilder::create(s_x, point);
- Z = point.get_Z() ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
+ Z = point.get_Z();
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
task.set_cVe(cVe);
robot.get_eJe(eJe);
task.set_eJe(eJe);
- vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());
+ vpColVector v = task.computeControlLaw(iter * robot.getSamplingTime());
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
- 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
- graph.plot(2, 0, iter, Z); // plot the depth
+ graph.plot(2, 0, iter, Z); // plot the depth
#endif
- iter ++;
+ iter++;
if (task.getError().sumSquare() < 0.0001) {
std::cout << "Reached a small error. We stop the loop... " << std::endl;
@@ -130,18 +130,16 @@ int main()
graph.saveData(1, "./error2.dat");
const char *legend = "Click to quit...";
- vpDisplay::displayText(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);
+ vpDisplay::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
vpDisplay::flush(graph.I);
vpDisplay::getClick(graph.I);
#endif
-
// Kill the servo task
- task.print() ;
+ task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
index bc5e8d5..82268f1 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer-pan.cpp
@@ -1,37 +1,38 @@
/*!
\example tutorial-simu-pioneer-pan.cpp
- Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera
- able to move along the pan axis.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ Example that shows how to simulate a visual servoing on a Pioneer mobile
+ robot equipped with a camera able to move along the pan axis. The current
+ visual features that are used are s = (x, log(Z/Z*)). The desired one are s*
+ = (x*, 0), with:
- x the abscisse of the point measured at each iteration
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
*/
#include <iostream>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorPioneerPan.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorPioneerPan.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
try {
// Set the position the camera has to reach
- vpHomogeneousMatrix cdMo ;
+ vpHomogeneousMatrix cdMo;
cdMo[1][3] = 1.2; // t_y should be different from zero to be non singular
cdMo[2][3] = 0.5;
@@ -43,7 +44,7 @@ int main()
vpRotationMatrix cdRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cdRo);
- vpSimulatorPioneerPan robot ;
+ vpSimulatorPioneerPan robot;
robot.setSamplingTime(0.04);
vpHomogeneousMatrix wMc, wMo;
@@ -54,7 +55,7 @@ int main()
wMo = wMc * cMo;
// Define the target
- vpPoint point(0,0,0); // Coordinates in the object frame
+ vpPoint point(0, 0, 0); // Coordinates in the object frame
point.track(cMo);
vpServo task;
@@ -70,7 +71,8 @@ int main()
robot.get_eJe(eJe);
task.set_eJe(eJe);
- // Current and desired visual feature associated later to the x coordinate of the point
+ // Current and desired visual feature associated later to the x coordinate
+ // of the point
vpFeaturePoint s_x, s_xd;
// Create the current x visual feature
@@ -88,8 +90,9 @@ int main()
double Z = point.get_Z();
// Desired depth Z* of the target.
double Zd = cdMo[2][3];
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
- s_Zd.buildFrom(0, 0, Zd, 0); // log(Z/Z*) = 0 that's why the last parameter is 0
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
+ s_Zd.buildFrom(0, 0, Zd,
+ 0); // log(Z/Z*) = 0 that's why the last parameter is 0
// Add the feature
task.addFeature(s_Z, s_Zd);
@@ -99,9 +102,9 @@ int main()
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
- graph.initGraph(0,3);
- graph.initGraph(1,2);
- graph.initGraph(2,1);
+ graph.initGraph(0, 3);
+ graph.initGraph(1, 2);
+ graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
@@ -114,9 +117,8 @@ int main()
#endif
int iter = 0;
- for (; ;)
- {
- robot.getPosition(wMc) ;
+ for (;;) {
+ robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
@@ -124,27 +126,29 @@ int main()
// Update the current x feature
vpFeatureBuilder::create(s_x, point);
- // Update log(Z/Z*) feature. Since the depth Z change, we need to update the intection matrix
- Z = point.get_Z() ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
+ // Update log(Z/Z*) feature. Since the depth Z change, we need to update
+ // the intection matrix
+ Z = point.get_Z();
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
task.set_cVe(cVe);
robot.get_eJe(eJe);
task.set_eJe(eJe);
- // Compute the control law. Velocities are computed in the mobile robot reference frame
+ // Compute the control law. Velocities are computed in the mobile robot
+ // reference frame
vpColVector v = task.computeControlLaw();
// Send the velocity to the robot
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
- 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
- graph.plot(2, 0, iter, Z); // plot the depth
+ graph.plot(2, 0, iter, Z); // plot the depth
#endif
- iter ++;
+ iter++;
if (task.getError().sumSquare() < 0.0001) {
std::cout << "Reached a small error. We stop the loop... " << std::endl;
@@ -153,7 +157,7 @@ int main()
}
#ifdef VISP_HAVE_DISPLAY
const char *legend = "Click to quit...";
- vpDisplay::displayText(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);
+ vpDisplay::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
vpDisplay::flush(graph.I);
vpDisplay::getClick(graph.I);
#endif
@@ -161,8 +165,7 @@ int main()
// Kill the servo task
task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp b/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
index 496253f..478707e 100644
--- a/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
+++ b/tutorial/robot/pioneer/tutorial-simu-pioneer.cpp
@@ -1,30 +1,31 @@
/*!
\example tutorial-simu-pioneer.cpp
- Example that shows how to simulate a visual servoing on a Pioneer mobile robot equipped with a camera.
- The current visual features that are used are s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
+ Example that shows how to simulate a visual servoing on a Pioneer mobile
+ robot equipped with a camera. The current visual features that are used are
+ s = (x, log(Z/Z*)). The desired one are s* = (x*, 0), with:
- x the abscisse of the point measured at each iteration
- x* the desired abscisse position of the point (x* = 0)
- Z the depth of the point measured at each iteration
- Z* the desired depth of the point equal to the initial one.
- The degrees of freedom that are controlled are (vx, wz), where wz is the rotational velocity
- and vx the translational velocity of the mobile platform at point M located at the middle
- between the two wheels.
+ The degrees of freedom that are controlled are (vx, wz), where wz is the
+ rotational velocity and vx the translational velocity of the mobile platform
+ at point M located at the middle between the two wheels.
The feature x allows to control wy, while log(Z/Z*) allows to control vz.
*/
#include <iostream>
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpVelocityTwistMatrix.h>
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorPioneer.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/visual_features/vpFeatureDepth.h>
#include <visp3/visual_features/vpFeaturePoint.h>
-#include <visp3/core/vpHomogeneousMatrix.h>
-#include <visp3/gui/vpPlot.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorPioneer.h>
-#include <visp3/core/vpVelocityTwistMatrix.h>
int main()
{
@@ -37,16 +38,16 @@ int main()
cMo[0][3] = 0.3;
cMo[1][3] = cdMo[1][3];
cMo[2][3] = 1.;
- vpRotationMatrix cRo(0, atan2( cMo[0][3], cMo[1][3]), 0);
+ vpRotationMatrix cRo(0, atan2(cMo[0][3], cMo[1][3]), 0);
cMo.insert(cRo);
- vpSimulatorPioneer robot ;
+ vpSimulatorPioneer robot;
robot.setSamplingTime(0.04);
vpHomogeneousMatrix wMc, wMo;
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpPoint point(0,0,0);
+ vpPoint point(0, 0, 0);
point.track(cMo);
vpServo task;
@@ -69,7 +70,7 @@ int main()
vpFeatureDepth s_Z, s_Zd;
double Z = point.get_Z();
double Zd = cdMo[2][3];
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd));
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
s_Zd.buildFrom(0, 0, Zd, 0);
task.addFeature(s_Z, s_Zd);
@@ -78,9 +79,9 @@ int main()
vpPlot graph(3, 800, 500, 400, 10, "Curves...");
// Init the curve plotter
- graph.initGraph(0,2);
- graph.initGraph(1,2);
- graph.initGraph(2,1);
+ graph.initGraph(0, 2);
+ graph.initGraph(1, 2);
+ graph.initGraph(2, 1);
graph.setTitle(0, "Velocities");
graph.setTitle(1, "Error s-s*");
graph.setTitle(2, "Depth");
@@ -92,17 +93,16 @@ int main()
#endif
int iter = 0;
- for (; ;)
- {
- robot.getPosition(wMc) ;
+ for (;;) {
+ robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
point.track(cMo);
vpFeatureBuilder::create(s_x, point);
- Z = point.get_Z() ;
- s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z/Zd)) ;
+ Z = point.get_Z();
+ s_Z.buildFrom(s_x.get_x(), s_x.get_y(), Z, log(Z / Zd));
robot.get_cVe(cVe);
task.set_cVe(cVe);
@@ -113,12 +113,12 @@ int main()
robot.setVelocity(vpRobot::ARTICULAR_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
- 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
- graph.plot(2, 0, iter, Z); // plot the depth
+ graph.plot(2, 0, iter, Z); // plot the depth
#endif
- iter ++;
+ iter++;
if (task.getError().sumSquare() < 0.0001) {
std::cout << "Reached a small error. We stop the loop... " << std::endl;
@@ -130,18 +130,16 @@ int main()
graph.saveData(1, "./error2.dat");
const char *legend = "Click to quit...";
- vpDisplay::displayText(graph.I, (int)graph.I.getHeight()-60, (int)graph.I.getWidth()-150, legend, vpColor::red);
+ vpDisplay::displayText(graph.I, (int)graph.I.getHeight() - 60, (int)graph.I.getWidth() - 150, legend, vpColor::red);
vpDisplay::flush(graph.I);
vpDisplay::getClick(graph.I);
#endif
-
// Kill the servo task
- task.print() ;
+ task.print();
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/simulator/image/tutorial-image-simulator.cpp b/tutorial/simulator/image/tutorial-image-simulator.cpp
index 8498ba1..0808172 100644
--- a/tutorial/simulator/image/tutorial-image-simulator.cpp
+++ b/tutorial/simulator/image/tutorial-image-simulator.cpp
@@ -1,7 +1,7 @@
//! \example tutorial-image-simulator.cpp
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
//! [Include]
#include <visp3/robot/vpImageSimulator.h>
@@ -17,18 +17,28 @@ int main()
//! [Set model]
vpColVector X[4];
- for (int i = 0; i < 4; i++) X[i].resize(3);
+ for (int i = 0; i < 4; i++)
+ X[i].resize(3);
// Top left Top right Bottom right Bottom left
- X[0][0] = -0.1; X[1][0] = 0.1; X[2][0] = 0.1; X[3][0] = -0.1;
- X[0][1] = -0.1; X[1][1] = -0.1; X[2][1] = 0.1; X[3][1] = 0.1;
- X[0][2] = 0; X[1][2] = 0; X[2][2] = 0; X[3][2] = 0;
+ X[0][0] = -0.1;
+ X[1][0] = 0.1;
+ X[2][0] = 0.1;
+ X[3][0] = -0.1;
+ X[0][1] = -0.1;
+ X[1][1] = -0.1;
+ X[2][1] = 0.1;
+ X[3][1] = 0.1;
+ X[0][2] = 0;
+ X[1][2] = 0;
+ X[2][2] = 0;
+ X[3][2] = 0;
//! [Set model]
//! [Image construction]
vpImage<unsigned char> I(480, 640);
//! [Image construction]
//! [Camera parameters]
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
//! [Camera parameters]
//! [Set cMo]
vpHomogeneousMatrix cMo(0, 0, 0.35, 0, vpMath::rad(30), vpMath::rad(15));
@@ -50,11 +60,10 @@ int main()
//! [Write image]
try {
vpImageIo::write(I, "./rendered_image.jpg");
- }
- catch(...) {
+ } catch (...) {
std::cout << "Unsupported image format" << std::endl;
}
- //! [Write image]
+//! [Write image]
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
@@ -71,8 +80,7 @@ int main()
vpDisplay::flush(I);
std::cout << "A click to quit..." << std::endl;
vpDisplay::getClick(I);
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/trace/tutorial-trace.cpp b/tutorial/trace/tutorial-trace.cpp
index dfbe5fd..6644b8e 100644
--- a/tutorial/trace/tutorial-trace.cpp
+++ b/tutorial/trace/tutorial-trace.cpp
@@ -15,18 +15,22 @@ int main()
std::cout << "Debug level 3 active: " << vpDEBUG_ENABLE(3) << std::endl;
// C-like trace printings if VP_TRACE defined
- vpTRACE("C-like trace"); // std::cout
- vpTRACE(1, "C-like trace level 1"); // std::cout
+ vpTRACE("C-like trace"); // std::cout
+ vpTRACE(1, "C-like trace level 1"); // std::cout
vpERROR_TRACE("C-like error trace"); // std::cerr
- vpERROR_TRACE(1, "C-like error trace level 1"); // std::cerr if VP_DEBUG_MODE value is >= 1
+ vpERROR_TRACE(1, "C-like error trace level 1"); // std::cerr if
+ // VP_DEBUG_MODE value is >=
+ // 1
// C-like debug printings if VP_DEBUG defined
- vpDEBUG_TRACE ("C-like debug trace"); // stdout
+ vpDEBUG_TRACE("C-like debug trace"); // stdout
vpDERROR_TRACE("C-like error trace"); // stderr
- vpDEBUG_TRACE (2, "C-like debug trace level 2"); // std::cout if VP_DEBUG_MODE value >= 2
- vpDERROR_TRACE(2, "C-like error trace level 2"); // std::cerr if VP_DEBUG_MODE value >= 2
+ vpDEBUG_TRACE(2,
+ "C-like debug trace level 2"); // std::cout if VP_DEBUG_MODE value >= 2
+ vpDERROR_TRACE(2,
+ "C-like error trace level 2"); // std::cerr if VP_DEBUG_MODE value >= 2
// C++-like trace printings if VP_TRACE defined
vpCTRACE << "C++-like trace" << std::endl; // std::cout
diff --git a/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp b/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
index cc49988..ee79033 100644
--- a/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-auto-tracker.cpp
@@ -1,8 +1,8 @@
//! \example tutorial-blob-auto-tracker.cpp
+#include <visp3/blob/vpDot2.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/blob/vpDot2.h>
#include <visp3/io/vpImageIo.h>
int main()
@@ -38,7 +38,7 @@ int main()
std::cout << "Blob characteristics: " << std::endl;
std::cout << " width : " << blob.getWidth() << std::endl;
std::cout << " height: " << blob.getHeight() << std::endl;
-#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
+#if VISP_VERSION_INT > VP_VERSION_INT(2, 7, 0)
std::cout << " area: " << blob.getArea() << std::endl;
#endif
std::cout << " gray level min: " << blob.getGrayLevelMin() << std::endl;
@@ -53,7 +53,7 @@ int main()
// Set blob characteristics for the auto detection
blob.setWidth(50);
blob.setHeight(50);
-#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
+#if VISP_VERSION_INT > VP_VERSION_INT(2, 7, 0)
blob.setArea(1700);
#endif
blob.setGrayLevelMin(0);
@@ -71,19 +71,19 @@ int main()
//! [Add learned dot]
if (learn) {
- // The blob that is tracked by initTracking() is not in the list of auto detected blobs
- // We add it:
+ // The blob that is tracked by initTracking() is not in the list of auto
+ // detected blobs We add it:
blob_list.push_back(blob);
}
//! [Add learned dot]
std::cout << "Number of auto detected blob: " << blob_list.size() << std::endl;
std::cout << "A click to exit..." << std::endl;
- while(1) {
+ while (1) {
vpDisplay::display(I);
//! [Display]
- for(std::list<vpDot2>::iterator it=blob_list.begin(); it != blob_list.end(); ++it) {
+ for (std::list<vpDot2>::iterator it = blob_list.begin(); it != blob_list.end(); ++it) {
(*it).setGraphics(true);
(*it).setGraphicsThickness(3);
(*it).track(I);
@@ -97,8 +97,7 @@ int main()
vpTime::wait(40);
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp b/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
index 4fb2b75..84f7975 100644
--- a/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-tracker-live-firewire.cpp
@@ -4,14 +4,15 @@
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#endif
+#include <visp3/blob/vpDot2.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/blob/vpDot2.h>
int main()
{
-#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
vpImage<unsigned char> I; // Create a gray level image container
#if defined(VISP_HAVE_DC1394)
@@ -22,7 +23,7 @@ int main()
g.open(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(0); // open the default camera
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -50,7 +51,7 @@ int main()
vpImagePoint germ;
bool init_done = false;
- while(1) {
+ while (1) {
try {
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
@@ -60,24 +61,22 @@ int main()
#endif
vpDisplay::display(I);
- if (! init_done) {
- vpDisplay::displayText(I, vpImagePoint(10,10), "Click in the blob to initialize the tracker", vpColor::red);
+ if (!init_done) {
+ vpDisplay::displayText(I, vpImagePoint(10, 10), "Click in the blob to initialize the tracker", vpColor::red);
if (vpDisplay::getClick(I, germ, false)) {
//! [Init]
blob.initTracking(I, germ);
//! [Init]
init_done = true;
}
- }
- else {
+ } else {
//! [Track]
blob.track(I);
//! [Track]
}
vpDisplay::flush(I);
- }
- catch(...) {
+ } catch (...) {
init_done = false;
}
}
diff --git a/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp b/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
index cae17dc..86efa27 100644
--- a/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
+++ b/tutorial/tracking/blob/tutorial-blob-tracker-live-v4l2.cpp
@@ -3,15 +3,16 @@
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vpV4l2Grabber.h>
#endif
+#include <visp3/blob/vpDot2.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayGTK.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
-#include <visp3/blob/vpDot2.h>
int main()
{
-#if ((defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)))
+#if ((defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100)) && \
+ (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV) || defined(VISP_HAVE_GTK)))
vpImage<unsigned char> I; // Create a gray level image container
#if defined(VISP_HAVE_V4L2)
@@ -19,7 +20,7 @@ int main()
g.open(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(0); // open the default camera
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -45,7 +46,7 @@ int main()
vpImagePoint germ;
bool init_done = false;
std::cout << "Click!!!" << std::endl;
- while(1) {
+ while (1) {
try {
#if defined(VISP_HAVE_V4L2)
g.acquire(I);
@@ -55,19 +56,17 @@ int main()
#endif
vpDisplay::display(I);
- if (! init_done) {
- vpDisplay::displayText(I, vpImagePoint(10,10), "Click in the blob to initialize the tracker", vpColor::red);
+ if (!init_done) {
+ vpDisplay::displayText(I, vpImagePoint(10, 10), "Click in the blob to initialize the tracker", vpColor::red);
if (vpDisplay::getClick(I, germ, false)) {
blob.initTracking(I, germ);
init_done = true;
}
- }
- else {
+ } else {
blob.track(I);
}
vpDisplay::flush(I);
- }
- catch(...) {
+ } catch (...) {
init_done = false;
}
}
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
index 1dde401..41437c8 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker-live-v4l2.cpp
@@ -4,9 +4,9 @@
#include <visp3/sensor/vpV4l2Grabber.h>
#endif
#include <visp3/core/vpImageConvert.h>
-#include <visp3/klt/vpKltOpencv.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/klt/vpKltOpencv.h>
int main(int argc, const char *argv[])
{
@@ -15,11 +15,11 @@ int main(int argc, const char *argv[])
bool opt_init_by_click = false;
int opt_device = 0;
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--init-by-click")
opt_init_by_click = true;
else if (std::string(argv[i]) == "--device")
- opt_device = atoi(argv[i+1]);
+ opt_device = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "Usage: " << argv[0] << " [--init-by-click] [--device <camera device>] [--help]" << std::endl;
return 0;
@@ -37,7 +37,7 @@ int main(int argc, const char *argv[])
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(opt_device);
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -47,7 +47,7 @@ int main(int argc, const char *argv[])
#endif
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage * cvI = NULL;
+ IplImage *cvI = NULL;
#else
cv::Mat cvI;
#endif
@@ -86,15 +86,14 @@ int main(int argc, const char *argv[])
}
vpDisplay::flush(I);
vpTime::wait(20);
- } while(button != vpMouseButton::button3);
+ } while (button != vpMouseButton::button3);
tracker.initTracking(cvI, guess);
#endif
- }
- else {
+ } else {
tracker.initTracking(cvI);
}
- while ( 1 ) {
+ while (1) {
#if defined(VISP_HAVE_V4L2)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -118,8 +117,7 @@ int main(int argc, const char *argv[])
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
index 1eb1c94..d5ecad2 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker-with-reinit.cpp
@@ -1,8 +1,8 @@
//! \example tutorial-klt-tracker-with-reinit.cpp
#include <visp3/core/vpImageConvert.h>
-#include <visp3/klt/vpKltOpencv.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/klt/vpKltOpencv.h>
int main()
{
@@ -15,7 +15,7 @@ int main()
reader.acquire(I);
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage * cvI = NULL;
+ IplImage *cvI = NULL;
#else
cv::Mat cvI;
#endif
@@ -40,10 +40,9 @@ int main()
// Initialise the tracking
tracker.initTracking(cvI);
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
reader.acquire(I);
- std::cout << "acquire image " << reader.getFrameIndex() << std::endl;
+ std::cout << "Process image " << reader.getFrameIndex() << std::endl;
vpDisplay::display(I);
vpImageConvert::convert(I, cvI);
@@ -62,37 +61,39 @@ int main()
// Add previous features if they are not to close to detected one
double distance, minDistance_ = tracker.getMinDistance();
- bool is_redundant;
- for (size_t i=0; i < prev_features.size(); i++) {
- // Test if a previous feature is not redundant with one of the newly detected
- is_redundant = false;
- for (size_t j=0; j < new_features.size(); j++){
- distance = sqrt(vpMath::sqr(new_features[j].x-prev_features[i].x) + vpMath::sqr(new_features[j].y-prev_features[i].y));
- if(distance < minDistance_){
+ for (size_t i = 0; i < prev_features.size(); i++) {
+ // Test if a previous feature is not redundant with one of the newly
+ // detected
+ bool is_redundant = false;
+ for (size_t j = 0; j < new_features.size(); j++) {
+ distance = sqrt(vpMath::sqr(new_features[j].x - prev_features[i].x) +
+ vpMath::sqr(new_features[j].y - prev_features[i].y));
+ if (distance < minDistance_) {
is_redundant = true;
break;
}
}
- if(is_redundant){
+ if (is_redundant) {
continue;
}
- //std::cout << "Add previous feature with index " << i << std::endl;
+ // std::cout << "Add previous feature with index " << i <<
+ // std::endl;
tracker.addFeature(prev_features[i]);
}
#else
// Save of previous features
int prev_nfeatures = tracker.getNbFeatures();
- float x,y;
+ float x, y;
long id;
- int j=0;
+ int j = 0;
- CvPoint2D32f *prev_features = (CvPoint2D32f*)cvAlloc(prev_nfeatures*sizeof(CvPoint2D32f));
+ CvPoint2D32f *prev_features = (CvPoint2D32f *)cvAlloc(prev_nfeatures * sizeof(CvPoint2D32f));
- for (int i=0; i <prev_nfeatures ; i ++) {
+ for (int i = 0; i < prev_nfeatures; i++) {
tracker.getFeature(i, id, x, y);
- prev_features[i].x=x;
- prev_features[i].y=y;
- //printf("prev feature %d: id %d coord: %g %g\n", i, id, x, y);
+ prev_features[i].x = x;
+ prev_features[i].y = y;
+ // printf("prev feature %d: id %d coord: %g %g\n", i, id, x, y);
}
// Start a new feature detection
@@ -101,24 +102,24 @@ int main()
// Add previous features if they are not to close to detected one
double distance, minDistance_ = tracker.getMinDistance();
- for(int i = tracker.getNbFeatures() ;
- j<prev_nfeatures && i<tracker.getMaxFeatures() ;
- j++){
- // Test if a previous feature is not redundant with new the one that are newly detected
+ for (int i = tracker.getNbFeatures(); j < prev_nfeatures && i < tracker.getMaxFeatures(); j++) {
+ // Test if a previous feature is not redundant with new the one that
+ // are newly detected
bool is_redundant = false;
- for(int k=0; k<tracker.getNbFeatures(); k++){
- tracker.getFeature(k,id,x,y);
- //printf("curr feature %d: id %d coord: %g %g\n", k, id, x, y);
- distance = sqrt(vpMath::sqr(x-prev_features[j].x) + vpMath::sqr(y-prev_features[j].y));
- if(distance < minDistance_){
+ for (int k = 0; k < tracker.getNbFeatures(); k++) {
+ tracker.getFeature(k, id, x, y);
+ // printf("curr feature %d: id %d coord: %g %g\n", k, id, x, y);
+ distance = sqrt(vpMath::sqr(x - prev_features[j].x) + vpMath::sqr(y - prev_features[j].y));
+ if (distance < minDistance_) {
is_redundant = true;
break;
}
}
- if(is_redundant){
+ if (is_redundant) {
continue;
}
- //std::cout << "Add previous feature with index " << i << std::endl;
+ // std::cout << "Add previous feature with index " << i <<
+ // std::endl;
tracker.addFeature(i, prev_features[j].x, prev_features[j].y);
i++;
}
@@ -142,8 +143,7 @@ int main()
#endif
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp b/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
index a5570e8..bebb405 100644
--- a/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
+++ b/tutorial/tracking/keypoint/tutorial-klt-tracker.cpp
@@ -1,19 +1,19 @@
//! \example tutorial-klt-tracker.cpp
//! [Include]
#include <visp3/core/vpImageConvert.h>
-#include <visp3/klt/vpKltOpencv.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/klt/vpKltOpencv.h>
//! [Include]
int main(int argc, const char *argv[])
{
- //! [Check 3rd party]
+//! [Check 3rd party]
#ifdef VISP_HAVE_OPENCV
//! [Check 3rd party]
try {
bool opt_init_by_click = false;
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--init-by-click")
opt_init_by_click = true;
else if (std::string(argv[i]) == "--help") {
@@ -30,11 +30,11 @@ int main(int argc, const char *argv[])
//! [Acquire]
vpImage<unsigned char> I;
reader.acquire(I);
- //! [Acquire]
+//! [Acquire]
- //! [Convert to OpenCV image]
+//! [Convert to OpenCV image]
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
- IplImage * cvI = NULL;
+ IplImage *cvI = NULL;
#else
cv::Mat cvI;
#endif
@@ -71,9 +71,7 @@ int main(int argc, const char *argv[])
#endif
vpImagePoint ip;
do {
- vpDisplay::displayText(I, 10, 10,
- "Left click to select a point, right to start tracking",
- vpColor::red);
+ vpDisplay::displayText(I, 10, 10, "Left click to select a point, right to start tracking", vpColor::red);
if (vpDisplay::getClick(I, ip, button, false)) {
if (button == vpMouseButton::button1) {
feature.push_back(cv::Point2f((float)ip.get_u(), (float)ip.get_v()));
@@ -82,14 +80,13 @@ int main(int argc, const char *argv[])
}
vpDisplay::flush(I);
vpTime::wait(20);
- } while(button != vpMouseButton::button3);
+ } while (button != vpMouseButton::button3);
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
tracker.initTracking(cvI, &feature[0], feature.size());
#else
tracker.initTracking(cvI, feature);
#endif
- }
- else {
+ } else {
//! [Init tracker]
tracker.initTracking(cvI);
//! [Init tracker]
@@ -100,8 +97,7 @@ int main(int argc, const char *argv[])
//! [How many features]
//! [While loop]
- while ( ! reader.end() )
- {
+ while (!reader.end()) {
reader.acquire(I);
vpDisplay::display(I);
@@ -116,9 +112,7 @@ int main(int argc, const char *argv[])
#endif
vpImagePoint ip;
do {
- vpDisplay::displayText(I, 10, 10,
- "Left click to select a point, right to start tracking",
- vpColor::red);
+ vpDisplay::displayText(I, 10, 10, "Left click to select a point, right to start tracking", vpColor::red);
if (vpDisplay::getClick(I, ip, button, false)) {
if (button == vpMouseButton::button1) {
feature.push_back(cv::Point2f((float)ip.get_u(), (float)ip.get_v()));
@@ -127,7 +121,7 @@ int main(int argc, const char *argv[])
}
vpDisplay::flush(I);
vpTime::wait(20);
- } while(button != vpMouseButton::button3);
+ } while (button != vpMouseButton::button3);
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
tracker.initTracking(cvI, &feature[0], feature.size());
#else
@@ -144,17 +138,16 @@ int main(int argc, const char *argv[])
//! [Wait click]
vpDisplay::getClick(I);
- //! [Wait click]
+//! [Wait click]
- //! [Release IplImage]
+//! [Release IplImage]
#if (VISP_HAVE_OPENCV_VERSION < 0x020408)
cvReleaseImage(&cvI);
#endif
//! [Release IplImage]
return 0;
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
diff --git a/tutorial/tracking/model-based/edges/CMakeLists.txt b/tutorial/tracking/model-based/edges/CMakeLists.txt
index 322accc..f124075 100644
--- a/tutorial/tracking/model-based/edges/CMakeLists.txt
+++ b/tutorial/tracking/model-based/edges/CMakeLists.txt
@@ -19,7 +19,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp b/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
index 77cab0c..5c60b13 100644
--- a/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
+++ b/tutorial/tracking/model-based/edges/tutorial-mb-edge-tracker.cpp
@@ -1,23 +1,23 @@
//! \example tutorial-mb-edge-tracker.cpp
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
//! [Include]
#include <visp3/mbt/vpMbEdgeTracker.h>
//! [Include]
#include <visp3/io/vpVideoReader.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG)
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string videoname = "teabox.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -26,12 +26,11 @@ int main(int argc, char** argv)
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -61,21 +60,21 @@ int main(int argc, char** argv)
return 0;
#endif
- display.init(I, 100, 100,"Model-based edge tracker");
+ display.init(I, 100, 100, "Model-based edge tracker");
//! [Constructor]
vpMbEdgeTracker tracker;
//! [Constructor]
bool usexml = false;
- //! [Load xml]
+//! [Load xml]
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
usexml = true;
}
#endif
//! [Load xml]
- if (! usexml) {
+ if (!usexml) {
//! [Set parameters]
vpMe me;
me.setMaskSize(5);
@@ -89,8 +88,8 @@ int main(int argc, char** argv)
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
//! [Set angles]
- tracker.setAngleAppear( vpMath::rad(70) );
- tracker.setAngleDisappear( vpMath::rad(80) );
+ tracker.setAngleAppear(vpMath::rad(70));
+ tracker.setAngleDisappear(vpMath::rad(80));
//! [Set angles]
//! [Set clipping distance]
tracker.setNearClippingDistance(0.1);
@@ -106,11 +105,11 @@ int main(int argc, char** argv)
tracker.setOgreShowConfigDialog(false);
//! [Set ogre]
//! [Load cao]
- if(vpIoTools::checkFilename(objectname + ".cao"))
+ if (vpIoTools::checkFilename(objectname + ".cao"))
tracker.loadModel(objectname + ".cao");
//! [Load cao]
//! [Load wrl]
- else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ else if (vpIoTools::checkFilename(objectname + ".wrl"))
tracker.loadModel(objectname + ".wrl");
//! [Load wrl]
//! [Set display]
@@ -120,7 +119,7 @@ int main(int argc, char** argv)
tracker.initClick(I, objectname + ".init", true);
//! [Init]
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
//! [Track]
@@ -140,7 +139,7 @@ int main(int argc, char** argv)
break;
}
vpDisplay::getClick(I);
- //! [Cleanup]
+//! [Cleanup]
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
@@ -148,18 +147,17 @@ int main(int argc, char** argv)
SoDB::finish();
#endif
//! [Cleanup]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
}
#ifdef VISP_HAVE_OGRE
- catch(Ogre::Exception &e) {
+ catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
}
#endif
#else
(void)argc;
(void)argv;
- std::cout << "Install OpenCV or ffmpeg and rebuild ViSP to use this example." << std::endl;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
#endif
}
diff --git a/tutorial/tracking/model-based/generic/CMakeLists.txt b/tutorial/tracking/model-based/generic/CMakeLists.txt
index 5a01cf7..e529e8d 100644
--- a/tutorial/tracking/model-based/generic/CMakeLists.txt
+++ b/tutorial/tracking/model-based/generic/CMakeLists.txt
@@ -20,7 +20,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp
index e57569c..ca9bfd8 100644
--- a/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp
+++ b/tutorial/tracking/model-based/generic/tutorial-mb-tracker-full.cpp
@@ -1,42 +1,47 @@
//! \example tutorial-mb-tracker-full.cpp
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
//! [Include]
-#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/mbt/vpMbEdgeKltTracker.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
//! [Include]
#include <visp3/io/vpVideoReader.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string opt_videoname = "teabox.mpg";
+ std::string opt_modelname = "teabox.cao";
int opt_tracker = 0;
- for (int i=0; i<argc; i++) {
- if (std::string(argv[i]) == "--name")
- opt_videoname = std::string(argv[i+1]);
- if (std::string(argv[i]) == "--tracker")
- opt_tracker = atoi(argv[i+1]);
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--video")
+ opt_videoname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--model")
+ opt_modelname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--tracker")
+ opt_tracker = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n" << std::endl;
+ std::cout << "\nUsage: " << argv[0]
+ << " [--video <video name>] [--model <model name>] "
+ "[--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n"
+ << std::endl;
return 0;
}
}
- std::string parentname = vpIoTools::getParent(opt_videoname);
- std::string objectname = vpIoTools::getNameWE(opt_videoname);
+ std::string parentname = vpIoTools::getParent(opt_modelname);
+ std::string objectname = vpIoTools::getNameWE(opt_modelname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << opt_videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -68,7 +73,7 @@ int main(int argc, char** argv)
//! [Constructor]
vpMbTracker *tracker;
if (opt_tracker == 0)
- tracker = new vpMbEdgeTracker;
+ tracker = new vpMbEdgeTracker;
#ifdef VISP_HAVE_MODULE_KLT
else if (opt_tracker == 1)
tracker = new vpMbKltTracker;
@@ -76,23 +81,25 @@ int main(int argc, char** argv)
tracker = new vpMbEdgeKltTracker;
#else
else {
- std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ std::cout << "klt and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
return 0;
}
#endif
//! [Constructor]
bool usexml = false;
- //! [Load xml]
+//! [Load xml]
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker->loadConfigFile(objectname + ".xml");
usexml = true;
}
#endif
//! [Load xml]
- if (! usexml) {
+ if (!usexml) {
//! [Set parameters]
if (opt_tracker == 0 || opt_tracker == 2) {
//! [Set moving-edges parameters]
@@ -104,7 +111,7 @@ int main(int argc, char** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- dynamic_cast<vpMbEdgeTracker*>(tracker)->setMovingEdge(me);
+ dynamic_cast<vpMbEdgeTracker *>(tracker)->setMovingEdge(me);
//! [Set moving-edges parameters]
}
@@ -119,8 +126,8 @@ int main(int argc, char** argv)
klt_settings.setHarrisFreeParameter(0.01);
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
- dynamic_cast<vpMbKltTracker*>(tracker)->setKltOpencv(klt_settings);
- dynamic_cast<vpMbKltTracker*>(tracker)->setMaskBorder(5);
+ dynamic_cast<vpMbKltTracker *>(tracker)->setKltOpencv(klt_settings);
+ dynamic_cast<vpMbKltTracker *>(tracker)->setKltMaskBorder(5);
//! [Set klt parameters]
}
#endif
@@ -131,8 +138,8 @@ int main(int argc, char** argv)
//! [Set camera parameters]
//! [Set angles]
- tracker->setAngleAppear( vpMath::rad(70) );
- tracker->setAngleDisappear( vpMath::rad(80) );
+ tracker->setAngleAppear(vpMath::rad(70));
+ tracker->setAngleDisappear(vpMath::rad(80));
//! [Set angles]
//! [Set clipping distance]
tracker->setNearClippingDistance(0.1);
@@ -141,21 +148,24 @@ int main(int argc, char** argv)
//! [Set clipping fov]
tracker->setClipping(tracker->getClipping() | vpMbtPolygon::FOV_CLIPPING);
//! [Set clipping fov]
-
- //! [Set ogre]
- tracker->setOgreVisibilityTest(false);
- tracker->setOgreShowConfigDialog(false);
- //! [Set ogre]
-
//! [Set parameters]
}
+ //! [Set visibility parameters]
+ //! [Set ogre visibility]
+ tracker->setOgreVisibilityTest(false);
+ tracker->setOgreShowConfigDialog(false);
+ //! [Set ogre visibility]
+ //! [Set scanline visibility]
+ tracker->setScanLineVisibilityTest(true);
+ //! [Set scanline visibility]
+ //! [Set visibility parameters]
//! [Load cao]
- if(vpIoTools::checkFilename(objectname + ".cao"))
+ if (vpIoTools::checkFilename(objectname + ".cao"))
tracker->loadModel(objectname + ".cao");
//! [Load cao]
//! [Load wrl]
- else if(vpIoTools::checkFilename(objectname + ".wrl"))
+ else if (vpIoTools::checkFilename(objectname + ".wrl"))
tracker->loadModel(objectname + ".wrl");
//! [Load wrl]
//! [Set display]
@@ -165,7 +175,7 @@ int main(int argc, char** argv)
tracker->initClick(I, objectname + ".init", true);
//! [Init]
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
//! [Track]
@@ -176,7 +186,7 @@ int main(int argc, char** argv)
//! [Get pose]
//! [Display]
tracker->getCameraParameters(cam);
- tracker->display(I, cMo, cam, vpColor::red, 2, true);
+ tracker->display(I, cMo, cam, vpColor::red, 2);
//! [Display]
vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red);
@@ -186,7 +196,7 @@ int main(int argc, char** argv)
break;
}
vpDisplay::getClick(I);
- //! [Cleanup]
+//! [Cleanup]
#ifdef VISP_HAVE_XML2
vpXmlParser::cleanup();
#endif
@@ -196,12 +206,11 @@ int main(int argc, char** argv)
delete display;
delete tracker;
//! [Cleanup]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
}
#ifdef VISP_HAVE_OGRE
- catch(Ogre::Exception &e) {
+ catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
}
#endif
diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp
index 66fe160..053cf1c 100644
--- a/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp
+++ b/tutorial/tracking/model-based/generic/tutorial-mb-tracker.cpp
@@ -1,42 +1,47 @@
//! \example tutorial-mb-tracker.cpp
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
//! [Include]
-#include <visp3/mbt/vpMbEdgeTracker.h>
#include <visp3/mbt/vpMbEdgeKltTracker.h>
+#include <visp3/mbt/vpMbEdgeTracker.h>
//! [Include]
#include <visp3/io/vpVideoReader.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string opt_videoname = "teabox.mpg";
+ std::string opt_modelname = "teabox";
int opt_tracker = 0;
- for (int i=0; i<argc; i++) {
- if (std::string(argv[i]) == "--name")
- opt_videoname = std::string(argv[i+1]);
- if (std::string(argv[i]) == "--tracker")
- opt_tracker = atoi(argv[i+1]);
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--video")
+ opt_videoname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--model")
+ opt_modelname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--tracker")
+ opt_tracker = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n" << std::endl;
+ std::cout << "\nUsage: " << argv[0]
+ << " [--video <video name>] [--model <model name>] "
+ "[--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n"
+ << std::endl;
return 0;
}
}
- std::string parentname = vpIoTools::getParent(opt_videoname);
- std::string objectname = vpIoTools::getNameWE(opt_videoname);
+ std::string parentname = vpIoTools::getParent(opt_modelname);
+ std::string objectname = vpIoTools::getNameWE(opt_modelname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << opt_videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init, cao]" << std::endl;
+ std::cout << "Tracker requested config files: " << objectname << ".[init, cao]" << std::endl;
std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
//! [Image]
@@ -64,7 +69,7 @@ int main(int argc, char** argv)
//! [Constructor]
vpMbTracker *tracker;
if (opt_tracker == 0)
- tracker = new vpMbEdgeTracker;
+ tracker = new vpMbEdgeTracker;
#ifdef VISP_HAVE_MODULE_KLT
else if (opt_tracker == 1)
tracker = new vpMbKltTracker;
@@ -72,7 +77,9 @@ int main(int argc, char** argv)
tracker = new vpMbEdgeKltTracker;
#else
else {
- std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ std::cout << "klt and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
return 0;
}
#endif
@@ -88,7 +95,7 @@ int main(int argc, char** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- dynamic_cast<vpMbEdgeTracker*>(tracker)->setMovingEdge(me);
+ dynamic_cast<vpMbEdgeTracker *>(tracker)->setMovingEdge(me);
}
#ifdef VISP_HAVE_MODULE_KLT
@@ -101,8 +108,8 @@ int main(int argc, char** argv)
klt_settings.setHarrisFreeParameter(0.01);
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
- dynamic_cast<vpMbKltTracker*>(tracker)->setKltOpencv(klt_settings);
- dynamic_cast<vpMbKltTracker*>(tracker)->setMaskBorder(5);
+ dynamic_cast<vpMbKltTracker *>(tracker)->setKltOpencv(klt_settings);
+ dynamic_cast<vpMbKltTracker *>(tracker)->setKltMaskBorder(5);
}
#endif
@@ -122,7 +129,7 @@ int main(int argc, char** argv)
tracker->initClick(I, objectname + ".init", true);
//! [Init]
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
//! [Track]
@@ -147,8 +154,7 @@ int main(int argc, char** argv)
delete display;
delete tracker;
//! [Cleanup]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
}
#else
diff --git a/tutorial/tracking/model-based/hybrid/CMakeLists.txt b/tutorial/tracking/model-based/hybrid/CMakeLists.txt
index 125340f..3356653 100644
--- a/tutorial/tracking/model-based/hybrid/CMakeLists.txt
+++ b/tutorial/tracking/model-based/hybrid/CMakeLists.txt
@@ -18,7 +18,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp b/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
index cf1d92d..01bb989 100644
--- a/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
+++ b/tutorial/tracking/model-based/hybrid/tutorial-mb-hybrid-tracker.cpp
@@ -1,22 +1,22 @@
/*! \example tutorial-mb-hybrid-tracker.cpp */
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/mbt/vpMbEdgeKltTracker.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbEdgeKltTracker.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string videoname = "teabox.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -25,12 +25,11 @@ int main(int argc, char** argv)
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -46,11 +45,14 @@ int main(int argc, char** argv)
g.open(I);
#if defined(VISP_HAVE_X11)
- vpDisplayX display(I,100,100,"Model-based hybrid tracker");;
+ vpDisplayX display(I, 100, 100, "Model-based hybrid tracker");
+ ;
#elif defined(VISP_HAVE_GDI)
- vpDisplayGDI display(I,100,100,"Model-based hybrid tracker");;
+ vpDisplayGDI display(I, 100, 100, "Model-based hybrid tracker");
+ ;
#elif defined(VISP_HAVE_OPENCV)
- vpDisplayOpenCV display(I,100,100,"Model-based hybrid tracker");;
+ vpDisplayOpenCV display(I, 100, 100, "Model-based hybrid tracker");
+ ;
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
@@ -58,12 +60,12 @@ int main(int argc, char** argv)
vpMbEdgeKltTracker tracker;
bool usexml = false;
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
usexml = true;
}
#endif
- if (! usexml) {
+ if (!usexml) {
vpMe me;
me.setMaskSize(5);
me.setMaskNumber(180);
@@ -85,8 +87,8 @@ int main(int argc, char** argv)
tracker.setMaskBorder(5);
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
- tracker.setAngleAppear( vpMath::rad(70) );
- tracker.setAngleDisappear( vpMath::rad(80) );
+ tracker.setAngleAppear(vpMath::rad(70));
+ tracker.setAngleDisappear(vpMath::rad(80));
tracker.setNearClippingDistance(0.1);
tracker.setFarClippingDistance(100.0);
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
@@ -97,7 +99,7 @@ int main(int argc, char** argv)
tracker.setDisplayFeatures(true);
tracker.initClick(I, objectname + ".init", true);
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
tracker.track(I);
@@ -119,12 +121,11 @@ int main(int argc, char** argv)
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
}
#ifdef VISP_HAVE_OGRE
- catch(Ogre::Exception &e) {
+ catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
}
#endif
diff --git a/tutorial/tracking/model-based/keypoint/CMakeLists.txt b/tutorial/tracking/model-based/keypoint/CMakeLists.txt
index 8b95d31..5772026 100644
--- a/tutorial/tracking/model-based/keypoint/CMakeLists.txt
+++ b/tutorial/tracking/model-based/keypoint/CMakeLists.txt
@@ -19,7 +19,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp b/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
index ea6f161..02fec89 100644
--- a/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
+++ b/tutorial/tracking/model-based/keypoint/tutorial-mb-klt-tracker.cpp
@@ -1,22 +1,22 @@
/*! \example tutorial-mb-klt-tracker.cpp */
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
-#include <visp3/mbt/vpMbKltTracker.h>
#include <visp3/io/vpVideoReader.h>
+#include <visp3/mbt/vpMbKltTracker.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string videoname = "teabox.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -25,12 +25,11 @@ int main(int argc, char** argv)
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
- if(! parentname.empty())
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty())
+ objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init,"
+ std::cout << "Tracker requested config files: " << objectname << ".[init,"
#ifdef VISP_HAVE_XML2
<< "xml,"
#endif
@@ -55,21 +54,20 @@ int main(int argc, char** argv)
return 0;
#endif
- display.init(I, 100, 100,"Model-based keypoint tracker");
+ display.init(I, 100, 100, "Model-based keypoint tracker");
vpMbKltTracker tracker;
bool usexml = false;
- //! [Load xml]
+//! [Load xml]
#ifdef VISP_HAVE_XML2
- if(vpIoTools::checkFilename(objectname + ".xml")) {
+ if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
usexml = true;
}
#endif
//! [Load xml]
- if (! usexml) {
+ if (!usexml) {
//! [Set parameters]
- tracker.setMaskBorder(5);
vpKltOpencv klt_settings;
klt_settings.setMaxFeatures(300);
klt_settings.setWindowSize(5);
@@ -79,10 +77,11 @@ int main(int argc, char** argv)
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
tracker.setKltOpencv(klt_settings);
+ tracker.setKltMaskBorder(5);
cam.initPersProjWithoutDistortion(839, 839, 325, 243);
tracker.setCameraParameters(cam);
- tracker.setAngleAppear( vpMath::rad(70) );
- tracker.setAngleDisappear( vpMath::rad(80) );
+ tracker.setAngleAppear(vpMath::rad(70));
+ tracker.setAngleDisappear(vpMath::rad(80));
tracker.setNearClippingDistance(0.1);
tracker.setFarClippingDistance(100.0);
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
@@ -94,7 +93,7 @@ int main(int argc, char** argv)
tracker.setDisplayFeatures(true);
tracker.initClick(I, objectname + ".init", true);
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
tracker.track(I);
@@ -116,12 +115,11 @@ int main(int argc, char** argv)
#if defined(VISP_HAVE_COIN3D) && (COIN_MAJOR_VERSION == 3)
SoDB::finish();
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
}
#ifdef VISP_HAVE_OGRE
- catch(Ogre::Exception &e) {
+ catch (Ogre::Exception &e) {
std::cout << "Catch an Ogre exception: " << e.getDescription() << std::endl;
}
#endif
diff --git a/tutorial/tracking/model-based/stereo-generic/CMakeLists.txt b/tutorial/tracking/model-based/stereo-generic/CMakeLists.txt
new file mode 100644
index 0000000..f9f408a
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/CMakeLists.txt
@@ -0,0 +1,44 @@
+project(tutorial-tracking-mb-generic-stereo)
+
+cmake_minimum_required(VERSION 2.6)
+
+find_package(VISP REQUIRED visp_core visp_mbt visp_io visp_gui)
+
+# set the list of source files
+set(tutorial_cpp
+ tutorial-mb-generic-tracker-stereo.cpp
+ tutorial-mb-generic-tracker-stereo-mono.cpp)
+
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.mpg")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.xml")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.cao")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.init")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox.ppm")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/cRightMcLeft.txt")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_left.mpg")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_left.xml")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_left.cao")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_left.init")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_left.ppm")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_right.mpg")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_right.xml")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_right.cao")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_right.init")
+list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/teabox_right.ppm")
+
+foreach(cpp ${tutorial_cpp})
+ visp_add_target(${cpp})
+
+ if(VISP_HAVE_OGRE)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ endif()
+
+ if(COMMAND visp_add_dependency)
+ visp_add_dependency(${cpp} "tutorials")
+ endif()
+endforeach()
+
+# Copy the data files to the same location than the target
+foreach(data ${tutorial_data})
+ visp_copy_data(tutorial-mb-generic-tracker-stereo-mono.cpp ${data})
+endforeach()
diff --git a/tutorial/tracking/model-based/stereo-generic/cRightMcLeft.txt b/tutorial/tracking/model-based/stereo-generic/cRightMcLeft.txt
new file mode 100644
index 0000000..13dbade
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/cRightMcLeft.txt
@@ -0,0 +1,4 @@
+0.9881086234 -0.0937918748 -0.1218377305 -0.06178719805
+0.08936883689 0.9951425498 -0.04128579076 -0.0009545730595
+0.1251181815 0.02990634962 0.9916910058 -0.002820574395
+0 0 0 1
\ No newline at end of file
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox.cao b/tutorial/tracking/model-based/stereo-generic/teabox.cao
new file mode 100644
index 0000000..83c1fdf
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox.cao
@@ -0,0 +1,27 @@
+V1
+# 3D Points
+8 # Number of points
+0 0 0 # Point 0: X Y Z
+0 0 -0.08
+0.165 0 -0.08
+0.165 0 0
+0.165 0.068 0
+0.165 0.068 -0.08
+0 0.068 -0.08
+0 0.068 0 # Point 7
+# 3D Lines
+0 # Number of lines
+# Faces from 3D lines
+0 # Number of faces
+# Faces from 3D points
+6 # Number of faces
+4 0 1 2 3 # Face 0: [number of points] [index of the 3D points]...
+4 1 6 5 2
+4 4 5 6 7
+4 0 3 4 7
+4 5 4 3 2
+4 0 7 6 1 # Face 5
+# 3D cylinders
+0 # Number of cylinders
+# 3D circles
+0 # Number of circles
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox.init b/tutorial/tracking/model-based/stereo-generic/teabox.init
new file mode 100644
index 0000000..f109800
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox.init
@@ -0,0 +1,5 @@
+4 # Number of points
+0 0 0 # Point 0
+0.165 0 0 # Point 3
+0.165 0 -0.08 # Point 2
+0.165 0.068 -0.08 # Point 5
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox.mpg b/tutorial/tracking/model-based/stereo-generic/teabox.mpg
new file mode 100644
index 0000000..50fb8ad
Binary files /dev/null and b/tutorial/tracking/model-based/stereo-generic/teabox.mpg differ
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox.ppm b/tutorial/tracking/model-based/stereo-generic/teabox.ppm
new file mode 100644
index 0000000..b20630a
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+GGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGNNNGGGNNNNNNGGGNNNNNNNNNNNNNNNNNNNNNNNNNNNVVVNNNNNNVVVVVVVVVVVVVVVVVVVVVVVV^^^VVV^^^^^^^^^VVV^^^^^^^^^^^^^^^NNNNNNNNNNNNNNNNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVNNNVVVVVVVVVVVVVVV^^^VVV^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ [...]
\ No newline at end of file
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox.xml b/tutorial/tracking/model-based/stereo-generic/teabox.xml
new file mode 100644
index 0000000..9a5353e
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>8</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ <camera>
+ <u0>325.66776</u0>
+ <v0>243.69727</v0>
+ <px>839.21470</px>
+ <py>839.44555</py>
+ </camera>
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ <near_clipping>0.1</near_clipping>
+ <far_clipping>100</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+</conf>
+
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_left.cao b/tutorial/tracking/model-based/stereo-generic/teabox_left.cao
new file mode 100644
index 0000000..83c1fdf
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_left.cao
@@ -0,0 +1,27 @@
+V1
+# 3D Points
+8 # Number of points
+0 0 0 # Point 0: X Y Z
+0 0 -0.08
+0.165 0 -0.08
+0.165 0 0
+0.165 0.068 0
+0.165 0.068 -0.08
+0 0.068 -0.08
+0 0.068 0 # Point 7
+# 3D Lines
+0 # Number of lines
+# Faces from 3D lines
+0 # Number of faces
+# Faces from 3D points
+6 # Number of faces
+4 0 1 2 3 # Face 0: [number of points] [index of the 3D points]...
+4 1 6 5 2
+4 4 5 6 7
+4 0 3 4 7
+4 5 4 3 2
+4 0 7 6 1 # Face 5
+# 3D cylinders
+0 # Number of cylinders
+# 3D circles
+0 # Number of circles
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_left.init b/tutorial/tracking/model-based/stereo-generic/teabox_left.init
new file mode 100644
index 0000000..25c482f
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_left.init
@@ -0,0 +1,6 @@
+4 # Number of points
+0 0 0 # Point 0
+0.165 0 0 # Point 3
+0.165 0 -0.08 # Point 2
+#0.165 0.068 -0.08 # Point 5
+0.165 0.068 0 #
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_left.mpg b/tutorial/tracking/model-based/stereo-generic/teabox_left.mpg
new file mode 100644
index 0000000..9075c3c
Binary files /dev/null and b/tutorial/tracking/model-based/stereo-generic/teabox_left.mpg differ
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_left.ppm b/tutorial/tracking/model-based/stereo-generic/teabox_left.ppm
new file mode 100644
index 0000000..511eb0e
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_left.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+GGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGNNNGGGNNNNNNGGGNNNNNNNNNNNNNNNNNNNNNNNNNNNVVVNNNNNNVVVVVVVVVVVVVVVVVVVVVVVV^^^VVV^^^^^^^^^VVV^^^^^^^^^^^^^^^NNNNNNNNNNNNNNNNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVNNNVVVVVVVVVVVVVVV^^^VVV^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ [...]
\ No newline at end of file
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_left.xml b/tutorial/tracking/model-based/stereo-generic/teabox_left.xml
new file mode 100644
index 0000000..410b7cf
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_left.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>10</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ <camera>
+ <u0>338.1239478994</u0>
+ <v0>247.1568627435</v0>
+ <px>680.0610429442</px>
+ <py>680.8199518458</py>
+ </camera>
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>10</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+</conf>
+
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_right.cao b/tutorial/tracking/model-based/stereo-generic/teabox_right.cao
new file mode 100644
index 0000000..83c1fdf
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_right.cao
@@ -0,0 +1,27 @@
+V1
+# 3D Points
+8 # Number of points
+0 0 0 # Point 0: X Y Z
+0 0 -0.08
+0.165 0 -0.08
+0.165 0 0
+0.165 0.068 0
+0.165 0.068 -0.08
+0 0.068 -0.08
+0 0.068 0 # Point 7
+# 3D Lines
+0 # Number of lines
+# Faces from 3D lines
+0 # Number of faces
+# Faces from 3D points
+6 # Number of faces
+4 0 1 2 3 # Face 0: [number of points] [index of the 3D points]...
+4 1 6 5 2
+4 4 5 6 7
+4 0 3 4 7
+4 5 4 3 2
+4 0 7 6 1 # Face 5
+# 3D cylinders
+0 # Number of cylinders
+# 3D circles
+0 # Number of circles
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_right.init b/tutorial/tracking/model-based/stereo-generic/teabox_right.init
new file mode 100644
index 0000000..25c482f
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_right.init
@@ -0,0 +1,6 @@
+4 # Number of points
+0 0 0 # Point 0
+0.165 0 0 # Point 3
+0.165 0 -0.08 # Point 2
+#0.165 0.068 -0.08 # Point 5
+0.165 0.068 0 #
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_right.mpg b/tutorial/tracking/model-based/stereo-generic/teabox_right.mpg
new file mode 100644
index 0000000..0f5b54d
Binary files /dev/null and b/tutorial/tracking/model-based/stereo-generic/teabox_right.mpg differ
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_right.ppm b/tutorial/tracking/model-based/stereo-generic/teabox_right.ppm
new file mode 100644
index 0000000..511eb0e
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_right.ppm
@@ -0,0 +1,5 @@
+P6
+# CREATOR: GIMP PNM Filter Version 1.1
+320 240
+255
+GGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGGNNNGGGNNNNNNGGGNNNNNNNNNNNNNNNNNNNNNNNNNNNVVVNNNNNNVVVVVVVVVVVVVVVVVVVVVVVV^^^VVV^^^^^^^^^VVV^^^^^^^^^^^^^^^NNNNNNNNNNNNNNNNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVNNNVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVVNNNVVVVVVVVVVVVVVV^^^VVV^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^VVV^^^^^^^^^^^^VVV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ [...]
\ No newline at end of file
diff --git a/tutorial/tracking/model-based/stereo-generic/teabox_right.xml b/tutorial/tracking/model-based/stereo-generic/teabox_right.xml
new file mode 100644
index 0000000..05f61d0
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/teabox_right.xml
@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+<conf>
+ <ecm>
+ <mask>
+ <size>5</size>
+ <nb_mask>180</nb_mask>
+ </mask>
+ <range>
+ <tracking>10</tracking>
+ </range>
+ <contrast>
+ <edge_threshold>10000</edge_threshold>
+ <mu1>0.5</mu1>
+ <mu2>0.5</mu2>
+ </contrast>
+ <sample>
+ <step>4</step>
+ </sample>
+ </ecm>
+ <klt>
+ <mask_border>5</mask_border>
+ <max_features>300</max_features>
+ <window_size>5</window_size>
+ <quality>0.015</quality>
+ <min_distance>8</min_distance>
+ <harris>0.01</harris>
+ <size_block>3</size_block>
+ <pyramid_lvl>3</pyramid_lvl>
+ </klt>
+ <camera>
+ <u0>320.0806549595</u0>
+ <v0>225.3683556175</v0>
+ <px>682.2252216804</px>
+ <py>682.1643147488</py>
+ </camera>
+ <face>
+ <angle_appear>70</angle_appear>
+ <angle_disappear>80</angle_disappear>
+ <near_clipping>0.01</near_clipping>
+ <far_clipping>10</far_clipping>
+ <fov_clipping>1</fov_clipping>
+ </face>
+</conf>
+
diff --git a/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo-mono.cpp b/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo-mono.cpp
new file mode 100644
index 0000000..630a978
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo-mono.cpp
@@ -0,0 +1,171 @@
+//! \example tutorial-mb-generic-tracker-stereo-mono.cpp
+#include <cstdlib>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+//! [Include]
+#include <visp3/mbt/vpMbGenericTracker.h>
+//! [Include]
+#include <visp3/io/vpVideoReader.h>
+
+int main(int argc, char **argv)
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020300)
+ try {
+ std::string opt_videoname = "teabox.mpg";
+ int opt_tracker = vpMbGenericTracker::EDGE_TRACKER;
+
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--name" && i + 1 < argc)
+ opt_videoname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--tracker" && i + 1 < argc)
+ opt_tracker = atoi(argv[i + 1]);
+ else if (std::string(argv[i]) == "--help") {
+ std::cout << "\nUsage: " << argv[0]
+ << " [--name <video name>] [--tracker "
+ "<1=egde|2=keypoint|3=hybrid>] [--help]\n"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ if (opt_tracker < 1 || opt_tracker > 3) {
+ std::cerr << "Wrong tracker type. Correct values are: "
+ "1=egde|2=keypoint|3=hybrid."
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+
+ std::string parentname = vpIoTools::getParent(opt_videoname);
+ std::string objectname = vpIoTools::getNameWE(opt_videoname);
+
+ if (!parentname.empty()) {
+ objectname = parentname + "/" + objectname;
+ }
+
+ std::cout << "Video name: " << opt_videoname << std::endl;
+ std::cout << "Tracker requested config files: " << objectname << ".[init, cao]" << std::endl;
+ std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
+
+ //! [Image]
+ vpImage<unsigned char> I;
+ //! [Image]
+
+ vpVideoReader g;
+ g.setFileName(opt_videoname);
+ g.open(I);
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX display;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI display;
+#else
+ vpDisplayOpenCV display;
+#endif
+ display.init(I, 100, 100, "Model-based tracker");
+
+ //! [Constructor]
+ vpMbGenericTracker tracker(1, opt_tracker);
+//! [Constructor]
+
+#if !defined(VISP_HAVE_MODULE_KLT)
+ if (opt_tracker >= 2) {
+ std::cout << "KLT and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+#endif
+
+//! [Set parameters]
+#ifdef VISP_HAVE_XML2
+ //! [Load config file]
+ tracker.loadConfigFile(objectname + ".xml");
+//! [Load config file]
+#else
+ if (opt_tracker == 1 || opt_tracker == 3) {
+ vpMe me;
+ me.setMaskSize(5);
+ me.setMaskNumber(180);
+ me.setRange(8);
+ me.setThreshold(10000);
+ me.setMu1(0.5);
+ me.setMu2(0.5);
+ me.setSampleStep(4);
+ tracker.setMovingEdge(me);
+ }
+
+#ifdef VISP_HAVE_MODULE_KLT
+ if (opt_tracker == 2 || opt_tracker == 3) {
+ vpKltOpencv klt_settings;
+ tracker.setKltMaskBorder(5);
+ klt_settings.setMaxFeatures(300);
+ klt_settings.setWindowSize(5);
+ klt_settings.setQuality(0.015);
+ klt_settings.setMinDistance(8);
+ klt_settings.setHarrisFreeParameter(0.01);
+ klt_settings.setBlockSize(3);
+ klt_settings.setPyramidLevels(3);
+ tracker.setKltOpencv(klt_settings);
+ }
+#endif
+
+ {
+ //! [Set camera parameters]
+ vpCameraParameters cam;
+ cam.initPersProjWithoutDistortion(839.21470, 839.44555, 325.66776, 243.69727);
+ tracker.setCameraParameters(cam);
+ //! [Set camera parameters]
+ }
+#endif
+ //! [Set parameters]
+
+ //! [Load cao]
+ tracker.loadModel(objectname + ".cao");
+ //! [Load cao]
+ //! [Set display features]
+ tracker.setDisplayFeatures(true);
+ //! [Set display features]
+ //! [Init]
+ tracker.initClick(I, objectname + ".init", true);
+ //! [Init]
+
+ while (!g.end()) {
+ g.acquire(I);
+ vpDisplay::display(I);
+ //! [Track]
+ tracker.track(I);
+ //! [Track]
+ //! [Get pose]
+ vpHomogeneousMatrix cMo;
+ tracker.getPose(cMo);
+ //! [Get pose]
+ //! [Display]
+ vpCameraParameters cam;
+ tracker.getCameraParameters(cam);
+ tracker.display(I, cMo, cam, vpColor::red, 2, true);
+ //! [Display]
+ vpDisplay::displayFrame(I, cMo, cam, 0.025, vpColor::none, 3);
+ vpDisplay::displayText(I, 10, 10, "A click to exit...", vpColor::red);
+ vpDisplay::flush(I);
+
+ if (vpDisplay::getClick(I, false)) {
+ break;
+ }
+ }
+ vpDisplay::getClick(I);
+ } catch (const vpException &e) {
+ std::cerr << "Catch a ViSP exception: " << e.what() << std::endl;
+ }
+
+ return EXIT_SUCCESS;
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
+ return EXIT_SUCCESS;
+#endif
+}
diff --git a/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo.cpp b/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo.cpp
new file mode 100644
index 0000000..0e5b73a
--- /dev/null
+++ b/tutorial/tracking/model-based/stereo-generic/tutorial-mb-generic-tracker-stereo.cpp
@@ -0,0 +1,180 @@
+//! \example tutorial-mb-generic-tracker-stereo.cpp
+#include <cstdlib>
+#include <visp3/core/vpConfig.h>
+#include <visp3/core/vpIoTools.h>
+#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/io/vpImageIo.h>
+//! [Include]
+#include <visp3/mbt/vpMbGenericTracker.h>
+//! [Include]
+#include <visp3/io/vpVideoReader.h>
+
+int main(int argc, char **argv)
+{
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020300)
+ try {
+ std::string opt_videoname_left = "teabox_left.mpg";
+ std::string opt_videoname_right = "teabox_right.mpg";
+ int opt_tracker1 = vpMbGenericTracker::EDGE_TRACKER;
+ int opt_tracker2 = vpMbGenericTracker::EDGE_TRACKER;
+
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--name" && i + 2 < argc) {
+ opt_videoname_left = std::string(argv[i + 1]);
+ opt_videoname_right = std::string(argv[i + 2]);
+ } else if (std::string(argv[i]) == "--tracker" && i + 2 < argc) {
+ opt_tracker1 = atoi(argv[i + 1]);
+ opt_tracker2 = atoi(argv[i + 2]);
+ } else if (std::string(argv[i]) == "--help") {
+ std::cout << "\nUsage: " << argv[0]
+ << " [--name <video name left> <video name right>]"
+ " [--tracker <1=egde|2=klt|3=hybrid> <1=egde|2=klt|3=hybrid>]"
+ " [--help]\n"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+ }
+
+ if ((opt_tracker1 < 1 || opt_tracker1 > 3) && (opt_tracker2 < 1 || opt_tracker2 > 3)) {
+ std::cerr << "Wrong tracker type. Correct values are: "
+ "1=egde|2=keypoint|3=hybrid."
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+
+ std::string parentname = vpIoTools::getParent(opt_videoname_left);
+ std::string objectname_left = vpIoTools::getNameWE(opt_videoname_left);
+ std::string objectname_right = vpIoTools::getNameWE(opt_videoname_right);
+
+ if (!parentname.empty()) {
+ objectname_left = parentname + "/" + objectname_left;
+ }
+
+ std::cout << "Video name: " << opt_videoname_left << " ; " << opt_videoname_right << std::endl;
+ std::cout << "Tracker requested config files: " << objectname_left << ".[init, cao]"
+ << " and " << objectname_right << ".[init, cao]" << std::endl;
+ std::cout << "Tracker optional config files: " << opt_videoname_left << ".ppm"
+ << " and " << opt_videoname_right << ".ppm" << std::endl;
+
+ //! [Images]
+ vpImage<unsigned char> I_left, I_right;
+ //! [Images]
+
+ vpVideoReader g_left, g_right;
+ g_left.setFileName(opt_videoname_left);
+ g_left.open(I_left);
+ g_right.setFileName(opt_videoname_right);
+ g_right.open(I_right);
+
+#if defined(VISP_HAVE_X11)
+ vpDisplayX display_left;
+ vpDisplayX display_right;
+#elif defined(VISP_HAVE_GDI)
+ vpDisplayGDI display_left;
+ vpDisplayGDI display_right;
+#else
+ vpDisplayOpenCV display_left;
+ vpDisplayOpenCV display_right;
+#endif
+ display_left.setDownScalingFactor(vpDisplay::SCALE_AUTO);
+ display_right.setDownScalingFactor(vpDisplay::SCALE_AUTO);
+ display_left.init(I_left, 100, 100, "Model-based tracker (Left)");
+ display_right.init(I_right, 110 + (int)I_left.getWidth(), 100, "Model-based tracker (Right)");
+
+ //! [Constructor]
+ std::vector<int> trackerTypes(2);
+ trackerTypes[0] = opt_tracker1;
+ trackerTypes[1] = opt_tracker2;
+ vpMbGenericTracker tracker(trackerTypes);
+
+#if !defined(VISP_HAVE_MODULE_KLT)
+ if (opt_tracker >= 2) {
+ std::cout << "klt and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
+ return EXIT_SUCCESS;
+ }
+#endif
+ //! [Constructor]
+
+ //! [Load config file]
+ tracker.loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
+ //! [Load config file]
+
+ //! [Load cao]
+ tracker.loadModel(objectname_left + ".cao", objectname_right + ".cao");
+ //! [Load cao]
+ //! [Set display features]
+ tracker.setDisplayFeatures(true);
+ //! [Set display features]
+
+ //! [Set camera transformation matrix]
+ vpHomogeneousMatrix cRightMcLeft;
+ std::ifstream file_cRightMcLeft("cRightMcLeft.txt");
+ cRightMcLeft.load(file_cRightMcLeft);
+
+ std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformationMatrix;
+ mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
+ mapOfCameraTransformationMatrix["Camera2"] = cRightMcLeft;
+
+ tracker.setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
+//! [Set camera transformation matrix]
+
+#ifndef VISP_HAVE_XML2
+ std::cout << "\n**********************************************************\n"
+ << "Warning: we are not able to load the tracker settings from\n"
+ << "the xml config files since ViSP is not build with libxml2\n"
+ << "3rd party. As a consequence, the tracking may fail!"
+ << "\n**********************************************************\n"
+ << std::endl;
+#endif
+
+ //! [Init]
+ tracker.initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
+ //! [Init]
+
+ while (!g_left.end() && !g_right.end()) {
+ g_left.acquire(I_left);
+ g_right.acquire(I_right);
+
+ vpDisplay::display(I_left);
+ vpDisplay::display(I_right);
+
+ //! [Track]
+ tracker.track(I_left, I_right);
+ //! [Track]
+
+ //! [Get pose]
+ vpHomogeneousMatrix cLeftMo, cRightMo;
+ tracker.getPose(cLeftMo, cRightMo);
+ //! [Get pose]
+
+ //! [Display]
+ vpCameraParameters cam_left, cam_right;
+ tracker.getCameraParameters(cam_left, cam_right);
+ tracker.display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
+ //! [Display]
+
+ vpDisplay::displayFrame(I_left, cLeftMo, cam_left, 0.025, vpColor::none, 3);
+ vpDisplay::displayFrame(I_right, cRightMo, cam_right, 0.025, vpColor::none, 3);
+ vpDisplay::displayText(I_left, 10, 10, "A click to exit...", vpColor::red);
+
+ vpDisplay::flush(I_left);
+ vpDisplay::flush(I_right);
+
+ if (vpDisplay::getClick(I_left, false)) {
+ break;
+ }
+ }
+ vpDisplay::getClick(I_left);
+ } catch (const vpException &e) {
+ std::cerr << "Catch a ViSP exception: " << e.what() << std::endl;
+ }
+#else
+ (void)argc;
+ (void)argv;
+ std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
+#endif
+}
diff --git a/tutorial/tracking/model-based/stereo/CMakeLists.txt b/tutorial/tracking/model-based/stereo/CMakeLists.txt
index 4fef92f..ee153a5 100644
--- a/tutorial/tracking/model-based/stereo/CMakeLists.txt
+++ b/tutorial/tracking/model-based/stereo/CMakeLists.txt
@@ -29,7 +29,7 @@ foreach(cpp ${tutorial_cpp})
visp_add_target(${cpp})
if(VISP_HAVE_OGRE)
- visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(${cpp} -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
if(COMMAND visp_add_dependency)
diff --git a/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo-mono.cpp b/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo-mono.cpp
index 485d2b3..02ec80b 100644
--- a/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo-mono.cpp
+++ b/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo-mono.cpp
@@ -1,42 +1,44 @@
//! \example tutorial-mb-tracker-stereo-mono.cpp
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
//! [Include]
-#include <visp3/mbt/vpMbEdgeMultiTracker.h>
#include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
+#include <visp3/mbt/vpMbEdgeMultiTracker.h>
//! [Include]
#include <visp3/io/vpVideoReader.h>
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
std::string opt_videoname = "teabox.mpg";
int opt_tracker = 0;
- for (int i=0; i<argc; i++) {
- if (std::string(argv[i]) == "--name" && i+1 < argc)
- opt_videoname = std::string(argv[i+1]);
- else if (std::string(argv[i]) == "--tracker" && i+1 < argc)
- opt_tracker = atoi(argv[i+1]);
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--name" && i + 1 < argc)
+ opt_videoname = std::string(argv[i + 1]);
+ else if (std::string(argv[i]) == "--tracker" && i + 1 < argc)
+ opt_tracker = atoi(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
- std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--tracker <0=egde|1=keypoint|2=hybrid>] [--help]\n" << std::endl;
+ std::cout << "\nUsage: " << argv[0]
+ << " [--name <video name>] [--tracker "
+ "<0=egde|1=keypoint|2=hybrid>] [--help]\n"
+ << std::endl;
return 0;
}
}
std::string parentname = vpIoTools::getParent(opt_videoname);
std::string objectname = vpIoTools::getNameWE(opt_videoname);
- if(! parentname.empty()) {
- objectname = parentname + "/" + objectname;
+ if (!parentname.empty()) {
+ objectname = parentname + "/" + objectname;
}
std::cout << "Video name: " << opt_videoname << std::endl;
- std::cout << "Tracker requested config files: " << objectname
- << ".[init, cao]" << std::endl;
+ std::cout << "Tracker requested config files: " << objectname << ".[init, cao]" << std::endl;
std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
//! [Image]
@@ -64,7 +66,7 @@ int main(int argc, char** argv)
//! [Constructor]
vpMbTracker *tracker;
if (opt_tracker == 0)
- tracker = new vpMbEdgeMultiTracker;
+ tracker = new vpMbEdgeMultiTracker;
#ifdef VISP_HAVE_MODULE_KLT
else if (opt_tracker == 1)
tracker = new vpMbKltMultiTracker;
@@ -72,7 +74,9 @@ int main(int argc, char** argv)
tracker = new vpMbEdgeKltMultiTracker;
#else
else {
- std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ std::cout << "klt and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
return 0;
}
#endif
@@ -88,7 +92,7 @@ int main(int argc, char** argv)
me.setMu1(0.5);
me.setMu2(0.5);
me.setSampleStep(4);
- dynamic_cast<vpMbEdgeMultiTracker*>(tracker)->setMovingEdge(me);
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->setMovingEdge(me);
}
#ifdef VISP_HAVE_MODULE_KLT
@@ -101,8 +105,8 @@ int main(int argc, char** argv)
klt_settings.setHarrisFreeParameter(0.01);
klt_settings.setBlockSize(3);
klt_settings.setPyramidLevels(3);
- dynamic_cast<vpMbKltMultiTracker*>(tracker)->setKltOpencv(klt_settings);
- dynamic_cast<vpMbKltMultiTracker*>(tracker)->setMaskBorder(5);
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->setKltOpencv(klt_settings);
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->setKltMaskBorder(5);
}
#endif
@@ -122,7 +126,7 @@ int main(int argc, char** argv)
tracker->initClick(I, objectname + ".init", true);
//! [Init]
- while(! g.end()){
+ while (!g.end()) {
g.acquire(I);
vpDisplay::display(I);
//! [Track]
@@ -148,8 +152,7 @@ int main(int argc, char** argv)
delete display;
delete tracker;
//! [Cleanup]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch a ViSP exception: " << e << std::endl;
}
#else
diff --git a/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo.cpp b/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo.cpp
index cedeaa0..384d601 100644
--- a/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo.cpp
+++ b/tutorial/tracking/model-based/stereo/tutorial-mb-tracker-stereo.cpp
@@ -1,16 +1,16 @@
//! \example tutorial-mb-tracker-stereo.cpp
+#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
-#include <visp3/core/vpIoTools.h>
//! [Include]
#include <visp3/mbt/vpMbEdgeKltMultiTracker.h>
+#include <visp3/mbt/vpMbEdgeMultiTracker.h>
//! [Include]
#include <visp3/io/vpVideoReader.h>
-
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
try {
@@ -18,15 +18,17 @@ int main(int argc, char** argv)
std::string opt_videoname_right = "teabox_right.mpg";
int opt_tracker = 0;
- for (int i=0; i<argc; i++) {
- if (std::string(argv[i]) == "--name" && i+2 < argc) {
- opt_videoname_left = std::string(argv[i+1]);
- opt_videoname_right = std::string(argv[i+2]);
- } else if (std::string(argv[i]) == "--tracker" && i+1 < argc) {
- opt_tracker = atoi(argv[i+1]);
+ for (int i = 0; i < argc; i++) {
+ if (std::string(argv[i]) == "--name" && i + 2 < argc) {
+ opt_videoname_left = std::string(argv[i + 1]);
+ opt_videoname_right = std::string(argv[i + 2]);
+ } else if (std::string(argv[i]) == "--tracker" && i + 1 < argc) {
+ opt_tracker = atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--help") {
- std::cout << "\nUsage: " << argv[0] << " [--name <video name left> <video name right>] "
- "[--tracker <0=egde|1=klt|2=hybrid>] [--help]\n" << std::endl;
+ std::cout << "\nUsage: " << argv[0]
+ << " [--name <video name left> <video name right>] "
+ "[--tracker <0=egde|1=klt|2=hybrid>] [--help]\n"
+ << std::endl;
return 0;
}
}
@@ -34,14 +36,13 @@ int main(int argc, char** argv)
std::string objectname_left = vpIoTools::getNameWE(opt_videoname_left);
std::string objectname_right = vpIoTools::getNameWE(opt_videoname_right);
- if(! parentname.empty()) {
+ if (!parentname.empty()) {
objectname_left = parentname + "/" + objectname_left;
}
std::cout << "Video name: " << opt_videoname_left << " ; " << opt_videoname_right << std::endl;
- std::cout << "Tracker requested config files: " << objectname_left
- << ".[init, cao]" << " and " << objectname_right
- << ".[init, cao]" << std::endl;
+ std::cout << "Tracker requested config files: " << objectname_left << ".[init, cao]"
+ << " and " << objectname_right << ".[init, cao]" << std::endl;
std::cout << "Tracker optional config files: " << opt_videoname_left << ".ppm"
<< " and " << opt_videoname_right << ".ppm" << std::endl;
@@ -69,7 +70,7 @@ int main(int argc, char** argv)
display_left->setDownScalingFactor(vpDisplay::SCALE_AUTO);
display_right->setDownScalingFactor(vpDisplay::SCALE_AUTO);
display_left->init(I_left, 100, 100, "Model-based tracker (Left)");
- display_right->init(I_right, 110 + (int) I_left.getWidth(), 100, "Model-based tracker (Right)");
+ display_right->init(I_right, 110 + (int)I_left.getWidth(), 100, "Model-based tracker (Right)");
//! [Constructor]
vpMbTracker *tracker;
@@ -82,29 +83,37 @@ int main(int argc, char** argv)
tracker = new vpMbEdgeKltMultiTracker(2);
#else
else {
- std::cout << "klt and hybrid model-based tracker are not available since visp_klt module is missing" << std::endl;
+ std::cout << "klt and hybrid model-based tracker are not available "
+ "since visp_klt module is missing"
+ << std::endl;
return 0;
}
#endif
//! [Constructor]
//! [Load config file]
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->loadConfigFile(objectname_left + ".xml",
+ objectname_right + ".xml");
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->loadConfigFile(objectname_left + ".xml", objectname_right + ".xml");
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->loadConfigFile(objectname_left + ".xml",
+ objectname_right + ".xml");
+#endif
//! [Load config file]
//! [Get camera parameters]
vpCameraParameters cam_left, cam_right;
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->getCameraParameters(cam_left, cam_right);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->getCameraParameters(cam_left, cam_right);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->getCameraParameters(cam_left, cam_right);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->getCameraParameters(cam_left, cam_right);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->getCameraParameters(cam_left, cam_right);
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->getCameraParameters(cam_left, cam_right);
+#endif
//! [Get camera parameters]
//! [Load cao]
@@ -123,13 +132,15 @@ int main(int argc, char** argv)
mapOfCameraTransformationMatrix["Camera1"] = vpHomogeneousMatrix();
mapOfCameraTransformationMatrix["Camera2"] = cRightMcLeft;
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
- //! [Set camera transformation matrix]
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->setCameraTransformationMatrix(mapOfCameraTransformationMatrix);
+#endif
+//! [Set camera transformation matrix]
#ifndef VISP_HAVE_XML2
std::cout << "\n**********************************************************\n"
@@ -141,18 +152,23 @@ int main(int argc, char** argv)
#endif
//! [Init]
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->initClick(I_left, I_right, objectname_left + ".init",
+ objectname_right + ".init", true);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->initClick(I_left, I_right, objectname_left + ".init",
+ objectname_right + ".init", true);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->initClick(I_left, I_right, objectname_left + ".init", objectname_right + ".init", true);
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->initClick(I_left, I_right, objectname_left + ".init",
+ objectname_right + ".init", true);
+#endif
//! [Init]
//! [cMo]
vpHomogeneousMatrix cLeftMo, cRightMo;
//! [cMo]
- while(!g_left.end() && !g_right.end()) {
+ while (!g_left.end() && !g_right.end()) {
g_left.acquire(I_left);
g_right.acquire(I_right);
@@ -160,30 +176,39 @@ int main(int argc, char** argv)
vpDisplay::display(I_right);
//! [Track]
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->track(I_left, I_right);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->track(I_left, I_right);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->track(I_left, I_right);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->track(I_left, I_right);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->track(I_left, I_right);
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->track(I_left, I_right);
+#endif
//! [Track]
//! [Get pose]
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->getPose(cLeftMo, cRightMo);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->getPose(cLeftMo, cRightMo);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->getPose(cLeftMo, cRightMo);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->getPose(cLeftMo, cRightMo);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->getPose(cLeftMo, cRightMo);
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->getPose(cLeftMo, cRightMo);
+#endif
//! [Get pose]
//! [Display]
- if(opt_tracker == 0)
- dynamic_cast<vpMbEdgeMultiTracker*> (tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
- else if(opt_tracker == 1)
- dynamic_cast<vpMbKltMultiTracker*> (tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
+ if (opt_tracker == 0)
+ dynamic_cast<vpMbEdgeMultiTracker *>(tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right,
+ vpColor::red, 2);
+#if defined(VISP_HAVE_MODULE_KLT)
+ else if (opt_tracker == 1)
+ dynamic_cast<vpMbKltMultiTracker *>(tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right,
+ vpColor::red, 2);
else
- dynamic_cast<vpMbEdgeKltMultiTracker*>(tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left, cam_right, vpColor::red, 2);
+ dynamic_cast<vpMbEdgeKltMultiTracker *>(tracker)->display(I_left, I_right, cLeftMo, cRightMo, cam_left,
+ cam_right, vpColor::red, 2);
+#endif
//! [Display]
vpDisplay::displayFrame(I_left, cLeftMo, cam_left, 0.025, vpColor::none, 3);
@@ -204,8 +229,7 @@ int main(int argc, char** argv)
delete display_right;
delete tracker;
//! [Cleanup]
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cerr << "Catch a ViSP exception: " << e.getMessage() << std::endl;
}
#else
diff --git a/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp b/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
index 5b0094c..6395e2d 100644
--- a/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
+++ b/tutorial/tracking/moving-edges/tutorial-me-ellipse-tracker.cpp
@@ -12,7 +12,8 @@
int main()
{
-#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
+#if (defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || defined(VISP_HAVE_V4L2) || \
+ (VISP_HAVE_OPENCV_VERSION >= 0x020100))
try {
vpImage<unsigned char> I;
@@ -24,7 +25,7 @@ int main()
vpV4l2Grabber g;
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(0); // open the default camera
- if(!g.isOpened()) { // check if we succeeded
+ if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
@@ -61,7 +62,7 @@ int main()
ellipse.setDisplay(vpMeSite::RANGE_RESULT);
ellipse.initTracking(I);
- while(1) {
+ while (1) {
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -73,8 +74,7 @@ int main()
ellipse.display(I, vpColor::red);
vpDisplay::flush(I);
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp b/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
index 38fcf2a..b0796bd 100644
--- a/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
+++ b/tutorial/tracking/moving-edges/tutorial-me-line-tracker.cpp
@@ -23,14 +23,14 @@ int main()
#elif defined(VISP_HAVE_V4L2)
vpV4l2Grabber g;
#elif defined(VISP_HAVE_OPENCV)
- cv::VideoCapture g(0); // open the default camera
- if(!g.isOpened()) { // check if we succeeded
- std::cout << "Failed to open the camera" << std::endl;
- return -1;
- }
- cv::Mat frame;
- g >> frame; // get a new frame from camera
- vpImageConvert::convert(frame, I);
+ cv::VideoCapture g(0); // open the default camera
+ if (!g.isOpened()) { // check if we succeeded
+ std::cout << "Failed to open the camera" << std::endl;
+ return -1;
+ }
+ cv::Mat frame;
+ g >> frame; // get a new frame from camera
+ vpImageConvert::convert(frame, I);
#endif
g.open(I);
g.acquire(I);
@@ -58,7 +58,7 @@ int main()
line.setDisplay(vpMeSite::RANGE_RESULT);
line.initTracking(I);
- while(1) {
+ while (1) {
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_V4L2) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
@@ -70,8 +70,7 @@ int main()
line.display(I, vpColor::red);
vpDisplay::flush(I);
}
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp b/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
index 63d64ed..be829f7 100644
--- a/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
+++ b/tutorial/tracking/template-tracker/tutorial-template-tracker.cpp
@@ -1,21 +1,21 @@
/*! \example tutorial-template-tracker.cpp */
#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoReader.h>
//! [Include]
#include <visp3/tt/vpTemplateTrackerSSDInverseCompositional.h>
#include <visp3/tt/vpTemplateTrackerWarpHomography.h>
//! [Include]
-int main(int argc, char** argv)
+int main(int argc, char **argv)
{
-#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100) || defined(VISP_HAVE_FFMPEG)
+#if defined(VISP_HAVE_OPENCV) && (VISP_HAVE_OPENCV_VERSION >= 0x020100)
std::string videoname = "bruegel.mpg";
- for (int i=0; i<argc; i++) {
+ for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--videoname")
- videoname = std::string(argv[i+1]);
+ videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help]\n" << std::endl;
return 0;
@@ -58,7 +58,7 @@ int main(int argc, char** argv)
tracker.initClick(I);
//! [Init]
- while(1){
+ while (1) {
g.acquire(I);
vpDisplay::display(I);
diff --git a/tutorial/visual-servo/ibvs/CMakeLists.txt b/tutorial/visual-servo/ibvs/CMakeLists.txt
index 5f4eace..472a6dd 100644
--- a/tutorial/visual-servo/ibvs/CMakeLists.txt
+++ b/tutorial/visual-servo/ibvs/CMakeLists.txt
@@ -55,8 +55,8 @@ list(APPEND tutorial_data "${CMAKE_CURRENT_SOURCE_DIR}/target_square.pgm" )
if(VISP_HAVE_OGRE)
# Add specific build flag to turn off warnings coming from libogre and libois 3rd party
- visp_set_source_file_compile_flag(tutorial-ibvs-4pts-ogre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
- visp_set_source_file_compile_flag(tutorial-ibvs-4pts-ogre-tracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter)
+ visp_set_source_file_compile_flag(tutorial-ibvs-4pts-ogre.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
+ visp_set_source_file_compile_flag(tutorial-ibvs-4pts-ogre-tracking.cpp -Wno-unused-parameter -Wno-unused-but-set-parameter -Wno-overloaded-virtual)
endif()
foreach(cpp ${tutorial_cpp})
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
index 14001ff..0443379 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-display.cpp
@@ -1,30 +1,30 @@
/*! \example tutorial-ibvs-4pts-display.cpp */
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam);
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam)
{
static std::vector<vpImagePoint> traj[4];
vpImagePoint cog;
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Project the point at the given camera position
point[i].project(cMo);
vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
traj[i].push_back(cog);
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);
}
}
}
@@ -33,22 +33,21 @@ int main()
{
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
- vpHomogeneousMatrix cMo(0.15, -0.1, 1.,
- vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
+ vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1, 0.1, 0) );
- point.push_back( vpPoint(-0.1, 0.1, 0) );
+ point.push_back(vpPoint(-0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, 0.1, 0));
+ point.push_back(vpPoint(-0.1, 0.1, 0));
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -62,8 +61,8 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpImage<unsigned char> Iint(480, 640, 255) ;
- vpImage<unsigned char> Iext(480, 640, 255) ;
+ vpImage<unsigned char> Iint(480, 640, 255);
+ vpImage<unsigned char> Iext(480, 640, 255);
#if defined(VISP_HAVE_X11)
vpDisplayX displayInt(Iint, 0, 0, "Internal view");
vpDisplayX displayExt(Iext, 670, 0, "External view");
@@ -77,27 +76,26 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
-
#if defined(VISP_HAVE_DISPLAY)
vpProjectionDisplay externalview;
- for (unsigned int i = 0 ; i < 4 ; i++)
- externalview.insert(point[i]) ;
+ for (unsigned int i = 0; i < 4; i++)
+ externalview.insert(point[i]);
#endif
- vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
- vpHomogeneousMatrix cextMo(0,0,3, 0,0,0);
+ vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
+ vpHomogeneousMatrix cextMo(0, 0, 3, 0, 0, 0);
- while(1) {
+ while (1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
display_trajectory(Iint, point, cMo, cam);
vpServoDisplay::display(task, cam, Iint, vpColor::green, vpColor::red);
@@ -111,11 +109,10 @@ int main()
if (vpDisplay::getClick(Iint, false) || vpDisplay::getClick(Iext, false))
break;
- vpTime::wait( robot.getSamplingTime() * 1000);
+ vpTime::wait(robot.getSamplingTime() * 1000);
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
index e275771..4e9b25c 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-image-tracking.cpp
@@ -1,13 +1,13 @@
/*! \example tutorial-ibvs-4pts-image-tracking.cpp */
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/robot/vpImageSimulator.h>
+#include <visp3/robot/vpSimulatorCamera.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot);
@@ -24,16 +24,25 @@ public:
\param filename : File name corresponding to an image of a target.
\param cam : Intrinsic camera parameters.
*/
- vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam)
- : sim_(), target_(), cam_()
+ vpVirtualGrabber(const std::string &filename, const vpCameraParameters &cam) : sim_(), target_(), cam_()
{
// The target is a square 20cm by 2cm square
// Initialise the 3D coordinates of the target corners
- for (int i = 0; i < 4; i++) X_[i].resize(3);
+ for (int i = 0; i < 4; i++)
+ X_[i].resize(3);
// Top left Top right Bottom right Bottom left
- X_[0][0] = -0.1; X_[1][0] = 0.1; X_[2][0] = 0.1; X_[3][0] = -0.1;
- X_[0][1] = -0.1; X_[1][1] = -0.1; X_[2][1] = 0.1; X_[3][1] = 0.1;
- X_[0][2] = 0; X_[1][2] = 0; X_[2][2] = 0; X_[3][2] = 0;
+ X_[0][0] = -0.1;
+ X_[1][0] = 0.1;
+ X_[2][0] = 0.1;
+ X_[3][0] = -0.1;
+ X_[0][1] = -0.1;
+ X_[1][1] = -0.1;
+ X_[2][1] = 0.1;
+ X_[3][1] = 0.1;
+ X_[0][2] = 0;
+ X_[1][2] = 0;
+ X_[2][2] = 0;
+ X_[3][2] = 0;
vpImageIo::read(target_, filename);
@@ -65,21 +74,19 @@ private:
vpCameraParameters cam_;
};
-
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot)
{
static std::vector<vpImagePoint> traj[4];
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
traj[i].push_back(dot[i].getCog());
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);
}
}
}
-
int main()
{
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
@@ -88,15 +95,15 @@ int main()
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
vpImage<unsigned char> I(480, 640, 255);
- vpCameraParameters cam(840, 840, I.getWidth()/2, I.getHeight()/2);
+ vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1, 0.1, 0) );
- point.push_back( vpPoint(-0.1, 0.1, 0) );
+ point.push_back(vpPoint(-0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, 0.1, 0));
+ point.push_back(vpPoint(-0.1, 0.1, 0));
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
@@ -115,15 +122,14 @@ int main()
#endif
vpDisplay::display(I);
- vpDisplay::displayText(I, 10, 10,
- "Click in the 4 dots to initialise the tracking and start the servo",
+ vpDisplay::displayText(I, 10, 10, "Click in the 4 dots to initialise the tracking and start the servo",
vpColor::red);
vpDisplay::flush(I);
vpFeaturePoint p[4], pd[4];
std::vector<vpDot2> dot(4);
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
@@ -141,7 +147,7 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- for (; ; ) {
+ for (;;) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
@@ -149,32 +155,30 @@ int main()
vpDisplay::display(I);
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
dot[i].track(I);
vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
vpColVector v = task.computeControlLaw();
display_trajectory(I, dot);
- vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red) ;
+ vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red);
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false))
break;
- vpTime::wait( robot.getSamplingTime() * 1000);
+ vpTime::wait(robot.getSamplingTime() * 1000);
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
index 4ec7d72..e7e2267 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre-tracking.cpp
@@ -3,46 +3,48 @@
#ifdef VISP_HAVE_MODULE_AR
#include <visp3/ar/vpAROgre.h>
#endif
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/blob/vpDot2.h>
#include <visp3/gui/vpDisplayGDI.h>
-#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/vision/vpPose.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
#include <visp3/vs/vpServoDisplay.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/blob/vpDot2.h>
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot, unsigned int thickness);
#if defined(VISP_HAVE_OGRE)
-void ogre_get_render_image(vpAROgre &ogre, const vpImage<unsigned char> &background,
- const vpHomogeneousMatrix &cMo, vpImage<unsigned char> &I);
+void ogre_get_render_image(vpAROgre &ogre, const vpImage<unsigned char> &background, const vpHomogeneousMatrix &cMo,
+ vpImage<unsigned char> &I);
#endif
void display_trajectory(const vpImage<unsigned char> &I, const std::vector<vpDot2> &dot, unsigned int thickness)
{
static std::vector<vpImagePoint> traj[4];
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
traj[i].push_back(dot[i].getCog());
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green, thickness);
}
}
}
#if defined(VISP_HAVE_OGRE)
-void ogre_get_render_image(vpAROgre &ogre, const vpImage<unsigned char> &background,
- const vpHomogeneousMatrix &cMo, vpImage<unsigned char> &I)
+void ogre_get_render_image(vpAROgre &ogre, const vpImage<unsigned char> &background, const vpHomogeneousMatrix &cMo,
+ vpImage<unsigned char> &I)
{
static vpImage<vpRGBa> Irender; // Image from ogre scene rendering
ogre.display(background, cMo);
ogre.getRenderingOutput(Irender, cMo);
vpImageConvert::convert(Irender, I);
- // Due to the light that was added to the scene, we need to threshold the image
- vpImageTools::binarise(I, (unsigned char)254, (unsigned char)255, (unsigned char)0, (unsigned char)255, (unsigned char)255);
+ // Due to the light that was added to the scene, we need to threshold the
+ // image
+ vpImageTools::binarise(I, (unsigned char)254, (unsigned char)255, (unsigned char)0, (unsigned char)255,
+ (unsigned char)255);
}
#endif
@@ -59,14 +61,14 @@ int main()
vpImage<unsigned char> background(480, 640, 255);
// Parameters of our camera
- vpCameraParameters cam(840, 840, background.getWidth()/2, background.getHeight()/2);
+ vpCameraParameters cam(840, 840, background.getWidth() / 2, background.getHeight() / 2);
// Define the target as 4 points
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1, 0.1, 0) );
- point.push_back( vpPoint(-0.1, 0.1, 0) );
+ point.push_back(vpPoint(-0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, 0.1, 0));
+ point.push_back(vpPoint(-0.1, 0.1, 0));
// Our object
// A simulator with the camera parameters defined above,
@@ -76,28 +78,31 @@ int main()
ogre.setShowConfigDialog(false);
ogre.addResource("./"); // Add the path to the Sphere.mesh resource
ogre.init(background, false, true);
- //ogre.setWindowPosition(680, 400);
+ // ogre.setWindowPosition(680, 400);
// Create the scene that contains 4 spheres
// Sphere.mesh contains a sphere with 1 meter radius
std::vector<std::string> name(4);
- for (unsigned int i=0; i<4; i++) {
- std::ostringstream s; s << "Sphere" << i; name[i] = s.str();
+ for (unsigned int i = 0; i < 4; i++) {
+ std::ostringstream s;
+ s << "Sphere" << i;
+ name[i] = s.str();
ogre.load(name[i], "Sphere.mesh");
- ogre.setScale(name[i], 0.02f, 0.02f, 0.02f); // Rescale the sphere to 2 cm radius
+ ogre.setScale(name[i], 0.02f, 0.02f,
+ 0.02f); // Rescale the sphere to 2 cm radius
// Set the position of each sphere in the object frame
ogre.setPosition(name[i], vpTranslationVector(point[i].get_oX(), point[i].get_oY(), point[i].get_oZ()));
- ogre.setRotation(name[i], vpRotationMatrix(M_PI/2, 0, 0));
+ ogre.setRotation(name[i], vpRotationMatrix(M_PI / 2, 0, 0));
}
// Add an optional point light source
- Ogre::Light * light = ogre.getSceneManager()->createLight();
- light->setDiffuseColour(1, 1, 1); // scaled RGB values
+ Ogre::Light *light = ogre.getSceneManager()->createLight();
+ light->setDiffuseColour(1, 1, 1); // scaled RGB values
light->setSpecularColour(1, 1, 1); // scaled RGB values
light->setPosition((Ogre::Real)cdMo[0][3], (Ogre::Real)cdMo[1][3], (Ogre::Real)(-cdMo[2][3]));
light->setType(Ogre::Light::LT_POINT);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
@@ -108,7 +113,7 @@ int main()
// Render the scene at the desired position
ogre_get_render_image(ogre, background, cdMo, I);
- // Display the image in which we will do the tracking
+// Display the image in which we will do the tracking
#if defined(VISP_HAVE_X11)
vpDisplayX d(I, 0, 0, "Camera view at desired position");
#elif defined(VISP_HAVE_GDI)
@@ -126,7 +131,7 @@ int main()
std::vector<vpDot2> dot(4);
vpFeaturePoint p[4], pd[4];
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Compute the desired feature at the desired position
dot[i].setGraphics(true);
dot[i].setGraphicsThickness(thickness);
@@ -140,15 +145,16 @@ int main()
vpDisplay::display(I);
vpDisplay::setTitle(I, "Current camera view");
- vpDisplay::displayText(I, 10, 10, "Click in the 4 dots to initialise the tracking and start the servo", vpColor::red);
+ vpDisplay::displayText(I, 10, 10, "Click in the 4 dots to initialise the tracking and start the servo",
+ vpColor::red);
vpDisplay::flush(I);
- for (unsigned int i = 0 ; i < 4 ; i++) {
- // We notice that if we project the scene at a given pose, the pose estimated from
- // the rendered image differs a little. That's why we cannot simply compute the desired
- // feature from the desired pose using the next two lines. We will rather compute the
- // desired position of the features from a learning stage.
- // point[i].project(cdMo);
+ for (unsigned int i = 0; i < 4; i++) {
+ // We notice that if we project the scene at a given pose, the pose
+ // estimated from the rendered image differs a little. That's why we
+ // cannot simply compute the desired feature from the desired pose using
+ // the next two lines. We will rather compute the desired position of
+ // the features from a learning stage. point[i].project(cdMo);
// vpFeatureBuilder::create(pd[i], point[i]);
// Compute the current feature at the initial position
@@ -158,10 +164,10 @@ int main()
vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
}
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
task.addFeature(p[i], pd[i]);
@@ -173,8 +179,9 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- for (; ; ) {
- // From the camera position in the world frame we retrieve the object position
+ for (;;) {
+ // From the camera position in the world frame we retrieve the object
+ // position
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
@@ -183,39 +190,36 @@ int main()
vpDisplay::display(I);
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
dot[i].track(I);
vpFeatureBuilder::create(p[i], cam, dot[i].getCog());
}
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Set the feature Z coordinate from the pose
vpColVector cP;
- point[i].changeFrame(cMo, cP) ;
+ point[i].changeFrame(cMo, cP);
p[i].set_Z(cP[2]);
}
vpColVector v = task.computeControlLaw();
display_trajectory(I, dot, thickness);
- vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red, thickness+2) ;
+ vpServoDisplay::display(task, cam, I, vpColor::green, vpColor::red, thickness + 2);
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::flush(I);
if (vpDisplay::getClick(I, false))
break;
- vpTime::wait( robot.getSamplingTime() * 1000);
+ vpTime::wait(robot.getSamplingTime() * 1000);
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch a ViSP exception: " << e << std::endl;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
return 1;
}
#endif
}
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
index e1976c7..efe89a2 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-ogre.cpp
@@ -3,9 +3,9 @@
#ifdef VISP_HAVE_MODULE_AR
#include <visp3/ar/vpAROgre.h>
#endif
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
int main()
{
@@ -14,10 +14,10 @@ int main()
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
// Define the target as 4 points
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
#if defined(VISP_HAVE_OGRE)
@@ -25,7 +25,7 @@ int main()
vpImage<unsigned char> background(480, 640, 255);
// Parameters of our camera
- vpCameraParameters cam(840, 840, background.getWidth()/2, background.getHeight()/2);
+ vpCameraParameters cam(840, 840, background.getWidth() / 2, background.getHeight() / 2);
// Our object
// A simulator with the camera parameters defined above,
@@ -39,29 +39,32 @@ int main()
// Create the scene that contains 4 spheres
// Sphere.mesh contains a sphere with 1 meter radius
std::vector<std::string> name(4);
- for (unsigned int i=0; i<4; i++) {
- std::ostringstream s; s << "Sphere" << i; name[i] = s.str();
+ for (unsigned int i = 0; i < 4; i++) {
+ std::ostringstream s;
+ s << "Sphere" << i;
+ name[i] = s.str();
ogre.load(name[i], "Sphere.mesh");
- ogre.setScale(name[i], 0.02f, 0.02f, 0.02f); // Rescale the sphere to 2 cm radius
+ ogre.setScale(name[i], 0.02f, 0.02f,
+ 0.02f); // Rescale the sphere to 2 cm radius
// Set the position of each sphere in the object frame
ogre.setPosition(name[i], vpTranslationVector(point[i].get_oX(), point[i].get_oY(), point[i].get_oZ()));
}
// Add an optional point light source
- Ogre::Light * light = ogre.getSceneManager()->createLight();
- light->setDiffuseColour(1, 1, 1); // scaled RGB values
+ Ogre::Light *light = ogre.getSceneManager()->createLight();
+ light->setDiffuseColour(1, 1, 1); // scaled RGB values
light->setSpecularColour(1, 1, 1); // scaled RGB values
light->setPosition((Ogre::Real)cdMo[0][3], (Ogre::Real)cdMo[1][3], (Ogre::Real)(-cdMo[2][3]));
light->setType(Ogre::Light::LT_POINT);
#endif
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -75,10 +78,10 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- for (unsigned int iter=0; iter < 150; iter ++) {
+ for (unsigned int iter = 0; iter < 150; iter++) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (int i = 0 ; i < 4 ; i++) {
+ for (int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -88,15 +91,12 @@ int main()
#endif
vpColVector v = task.computeControlLaw();
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
- vpTime::wait( robot.getSamplingTime() * 1000);
+ vpTime::wait(robot.getSamplingTime() * 1000);
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
- }
- catch(...) {
+ } catch (...) {
std::cout << "Catch an exception " << std::endl;
}
}
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
index f0c3e14..70f1e44 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter-continuous-gain-adaptive.cpp */
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/gui/vpPlot.h>
int main()
{
@@ -10,21 +10,21 @@ int main()
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
vpAdaptiveGain lambda(4, 0.4, 30);
task.setLambda(lambda);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -39,7 +39,7 @@ int main()
wMo = wMc * cMo;
#ifdef VISP_HAVE_DISPLAY
- vpPlot plotter(2, 250*2, 500, 100, 200, "Real time curves plotter");
+ vpPlot plotter(2, 250 * 2, 500, 100, 200, "Real time curves plotter");
plotter.setTitle(0, "Visual features error");
plotter.setTitle(1, "Camera velocities");
@@ -64,21 +64,21 @@ int main()
#endif
unsigned int iter = 0;
- while(1) {
+ while (1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
- vpColVector v = task.computeControlLaw(iter*robot.getSamplingTime());
+ vpColVector v = task.computeControlLaw(iter * robot.getSamplingTime());
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
#ifdef VISP_HAVE_DISPLAY
plotter.plot(0, iter, task.getError());
plotter.plot(1, iter, v);
#endif
- if (( task.getError() ).sumSquare() < 0.0001)
+ if ((task.getError()).sumSquare() < 0.0001)
break;
iter++;
@@ -93,10 +93,7 @@ int main()
vpDisplay::getClick(plotter.I);
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
-
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
index 0c16b99..01198b2 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter-gain-adaptive.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter-gain-adaptive.cpp */
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/gui/vpPlot.h>
int main()
{
@@ -10,21 +10,21 @@ int main()
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
vpAdaptiveGain lambda(4, 0.4, 30);
task.setLambda(lambda);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -39,7 +39,7 @@ int main()
wMo = wMc * cMo;
#ifdef VISP_HAVE_DISPLAY
- vpPlot plotter(2, 250*2, 500, 100, 200, "Real time curves plotter");
+ vpPlot plotter(2, 250 * 2, 500, 100, 200, "Real time curves plotter");
plotter.setTitle(0, "Visual features error");
plotter.setTitle(1, "Camera velocities");
@@ -64,10 +64,10 @@ int main()
#endif
unsigned int iter = 0;
- while(1) {
+ while (1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -78,7 +78,7 @@ int main()
plotter.plot(0, iter, task.getError());
plotter.plot(1, iter, v);
#endif
- if (( task.getError() ).sumSquare() < 0.0001)
+ if ((task.getError()).sumSquare() < 0.0001)
break;
iter++;
@@ -93,10 +93,7 @@ int main()
vpDisplay::getClick(plotter.I);
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
-
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
index 841cdf3..c1d0ec3 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-plotter.cpp
@@ -1,8 +1,8 @@
/*! \example tutorial-ibvs-4pts-plotter.cpp */
+#include <visp3/gui/vpPlot.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/gui/vpPlot.h>
int main()
{
@@ -10,19 +10,19 @@ int main()
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -37,7 +37,7 @@ int main()
wMo = wMc * cMo;
#ifdef VISP_HAVE_DISPLAY
- vpPlot plotter(2, 250*2, 500, 100, 200, "Real time curves plotter");
+ vpPlot plotter(2, 250 * 2, 500, 100, 200, "Real time curves plotter");
plotter.setTitle(0, "Visual features error");
plotter.setTitle(1, "Camera velocities");
@@ -62,10 +62,10 @@ int main()
#endif
unsigned int iter = 0;
- while(1) {
+ while (1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -76,7 +76,7 @@ int main()
plotter.plot(0, iter, task.getError());
plotter.plot(1, iter, v);
#endif
- if (( task.getError() ).sumSquare() < 0.0001)
+ if ((task.getError()).sumSquare() < 0.0001)
break;
iter++;
@@ -91,10 +91,7 @@ int main()
vpDisplay::getClick(plotter.I);
#endif
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
-
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
index 4b4560a..e09f99d 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-camera.cpp
@@ -1,31 +1,31 @@
/*! \example tutorial-ibvs-4pts-wireframe-camera.cpp */
-#include <visp3/visual_features/vpFeatureBuilder.h>
-#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
-#include <visp3/gui/vpDisplayX.h>
-#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayGDI.h>
+#include <visp3/gui/vpDisplayOpenCV.h>
+#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpProjectionDisplay.h>
-#include <visp3/vs/vpServoDisplay.h>
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/robot/vpWireFrameSimulator.h>
+#include <visp3/visual_features/vpFeatureBuilder.h>
+#include <visp3/vs/vpServo.h>
+#include <visp3/vs/vpServoDisplay.h>
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam);
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam)
{
static std::vector<vpImagePoint> traj[4];
vpImagePoint cog;
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Project the point at the given camera position
point[i].project(cMo);
vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
traj[i].push_back(cog);
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);
}
}
}
@@ -36,19 +36,19 @@ int main()
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- std::vector<vpPoint> point(4) ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ std::vector<vpPoint> point(4);
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -62,23 +62,23 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- vpImage<unsigned char> Iint(480, 640, 0) ;
- vpImage<unsigned char> Iext(480, 640, 0) ;
+ vpImage<unsigned char> Iint(480, 640, 0);
+ vpImage<unsigned char> Iext(480, 640, 0);
#if defined VISP_HAVE_X11
vpDisplayX displayInt(Iint, 0, 0, "Internal view");
vpDisplayX displayExt(Iext, 670, 0, "External view");
-#elif defined VISP_HAVE_GDI
+#elif defined VISP_HAVE_GDI
vpDisplayGDI displayInt(Iint, 0, 0, "Internal view");
vpDisplayGDI displayExt(Iext, 670, 0, "External view");
-#elif defined VISP_HAVE_OPENCV
+#elif defined VISP_HAVE_OPENCV
vpDisplayOpenCV displayInt(Iint, 0, 0, "Internal view");
vpDisplayOpenCV displayExt(Iext, 670, 0, "External view");
#else
std::cout << "No image viewer is available..." << std::endl;
#endif
- vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
- vpHomogeneousMatrix cextMo(0,0,3, 0,0,0);
+ vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
+ vpHomogeneousMatrix cextMo(0, 0, 3, 0, 0, 0);
vpWireFrameSimulator sim;
sim.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
@@ -88,10 +88,10 @@ int main()
sim.setInternalCameraParameters(cam);
sim.setExternalCameraParameters(cam);
- while(1) {
+ while (1) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -100,8 +100,8 @@ int main()
sim.setCameraPositionRelObj(cMo);
- vpDisplay::display(Iint) ;
- vpDisplay::display(Iext) ;
+ vpDisplay::display(Iint);
+ vpDisplay::display(Iext);
sim.getInternalImage(Iint);
sim.getExternalImage(Iext);
@@ -113,12 +113,10 @@ int main()
// A click in the internal view to exit
if (vpDisplay::getClick(Iint, false))
break;
- vpTime::wait(1000*robot.getSamplingTime());
+ vpTime::wait(1000 * robot.getSamplingTime());
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
-
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
index ab1a21c..8c3af85 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-afma6.cpp
@@ -2,28 +2,28 @@
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpSimulatorAfma6.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorAfma6.h>
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam);
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam)
{
unsigned int thickness = 3;
static std::vector<vpImagePoint> traj[4];
vpImagePoint cog;
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Project the point at the given camera position
point[i].project(cMo);
vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
traj[i].push_back(cog);
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green, thickness);
}
}
}
@@ -61,18 +61,18 @@ int main()
vpHomogeneousMatrix wMo(0, 0, 1., 0, 0, 0);
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1, 0.1, 0) );
- point.push_back( vpPoint(-0.1, 0.1, 0) );
+ point.push_back(vpPoint(-0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, 0.1, 0));
+ point.push_back(vpPoint(-0.1, 0.1, 0));
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -88,20 +88,17 @@ int main()
vpColVector qmax = robot.getJointMax();
std::cout << "Robot joint limits: " << std::endl;
- for (unsigned int i=0; i< 3; i ++)
+ for (unsigned int i = 0; i < 3; i++)
std::cout << "Joint " << i << ": min " << qmin[i] << " max " << qmax[i] << " (m)" << std::endl;
- for (unsigned int i=3; i< qmin.size(); i ++)
- std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
+ for (unsigned int i = 3; i < qmin.size(); i++)
+ std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)"
+ << std::endl;
robot.init(vpAfma6::TOOL_CCMOP, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
robot.set_fMo(wMo);
- bool ret = true;
-#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
- ret =
- #endif
- robot.initialiseCameraRelativeToObject(cMo);
+ bool ret = robot.initialiseCameraRelativeToObject(cMo);
if (ret == false)
return 0; // Not able to set the position
robot.setDesiredCameraPosition(cdMo);
@@ -117,15 +114,14 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
- vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
+ vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
robot.setCameraParameters(cam);
bool start = true;
- for ( ; ; )
- {
+ for (;;) {
cMo = robot.get_cMo();
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -154,11 +150,10 @@ int main()
vpDisplay::getClick(Iint);
}
- vpTime::wait(1000*robot.getSamplingTime());
+ vpTime::wait(1000 * robot.getSamplingTime());
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
index e68a4a1..96b22f3 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts-wireframe-robot-viper.cpp
@@ -2,28 +2,28 @@
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
+#include <visp3/robot/vpSimulatorViper850.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorViper850.h>
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam);
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam);
-void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point,
- const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam)
+void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
+ const vpCameraParameters &cam)
{
unsigned int thickness = 3;
static std::vector<vpImagePoint> traj[4];
vpImagePoint cog;
- for (unsigned int i=0; i<4; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
// Project the point at the given camera position
point[i].project(cMo);
vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
traj[i].push_back(cog);
}
- for (unsigned int i=0; i<4; i++) {
- for (unsigned int j=1; j<traj[i].size(); j++) {
- vpDisplay::displayLine(I, traj[i][j-1], traj[i][j], vpColor::green, thickness);
+ for (unsigned int i = 0; i < 4; i++) {
+ for (unsigned int j = 1; j < traj[i].size(); j++) {
+ vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green, thickness);
}
}
}
@@ -57,22 +57,21 @@ int main()
c_x <--
*/
- vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15),
- vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI/2.)));
+ vpHomogeneousMatrix wMo(vpTranslationVector(0.40, 0, -0.15), vpRotationMatrix(vpRxyzVector(-M_PI, 0, M_PI / 2.)));
std::vector<vpPoint> point;
- point.push_back( vpPoint(-0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1,-0.1, 0) );
- point.push_back( vpPoint( 0.1, 0.1, 0) );
- point.push_back( vpPoint(-0.1, 0.1, 0) );
+ point.push_back(vpPoint(-0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, -0.1, 0));
+ point.push_back(vpPoint(0.1, 0.1, 0));
+ point.push_back(vpPoint(-0.1, 0.1, 0));
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -87,33 +86,30 @@ int main()
vpColVector qmin = robot.getJointMin();
vpColVector qmax = robot.getJointMax();
qmin[0] = -vpMath::rad(180);
- qmax[0] = vpMath::rad(180);
- qmax[1] = vpMath::rad(0);
- qmax[2] = vpMath::rad(270);
+ qmax[0] = vpMath::rad(180);
+ qmax[1] = vpMath::rad(0);
+ qmax[2] = vpMath::rad(270);
qmin[4] = -vpMath::rad(180);
- qmax[4] = vpMath::rad(180);
+ qmax[4] = vpMath::rad(180);
robot.setJointLimit(qmin, qmax);
std::cout << "Robot joint limits: " << std::endl;
- for (unsigned int i=0; i< qmin.size(); i ++)
- std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)" << std::endl;
+ for (unsigned int i = 0; i < qmin.size(); i++)
+ std::cout << "Joint " << i << ": min " << vpMath::deg(qmin[i]) << " max " << vpMath::deg(qmax[i]) << " (deg)"
+ << std::endl;
robot.init(vpViper850::TOOL_PTGREY_FLEA2_CAMERA, vpCameraParameters::perspectiveProjWithoutDistortion);
robot.setRobotState(vpRobot::STATE_VELOCITY_CONTROL);
robot.initScene(vpWireFrameSimulator::PLATE, vpWireFrameSimulator::D_STANDARD);
robot.set_fMo(wMo);
- bool ret = true;
-#if VISP_VERSION_INT > VP_VERSION_INT(2,7,0)
- ret =
- #endif
- robot.initialiseCameraRelativeToObject(cMo);
+ bool ret = robot.initialiseCameraRelativeToObject(cMo);
if (ret == false)
return 0; // Not able to set the position
robot.setDesiredCameraPosition(cdMo);
// We modify the default external camera position
- robot.setExternalCameraPosition(vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2),
- vpRotationMatrix(vpRxyzVector(M_PI/2,0,0))));
+ robot.setExternalCameraPosition(
+ vpHomogeneousMatrix(vpTranslationVector(-0.4, 0.4, 2), vpRotationMatrix(vpRxyzVector(M_PI / 2, 0, 0))));
vpImage<unsigned char> Iint(480, 640, 255);
#if defined(VISP_HAVE_X11)
@@ -126,17 +122,17 @@ int main()
std::cout << "No image viewer is available..." << std::endl;
#endif
- vpCameraParameters cam(840, 840, Iint.getWidth()/2, Iint.getHeight()/2);
- // Modify the camera parameters to match those used in the other simulations
+ vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
+ // Modify the camera parameters to match those used in the other
+ // simulations
robot.setCameraParameters(cam);
bool start = true;
- //for ( ; ; )
- for (int iter =0; iter < 275; iter ++)
- {
+ // for ( ; ; )
+ for (int iter = 0; iter < 275; iter++) {
cMo = robot.get_cMo();
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -162,14 +158,13 @@ int main()
robot.setVelocity(vpRobot::CAMERA_FRAME, v);
vpDisplay::displayText(Iint, 40, 120, "Click to start the servo...", vpColor::blue);
vpDisplay::flush(Iint);
- //vpDisplay::getClick(Iint);
+ // vpDisplay::getClick(Iint);
}
- vpTime::wait(1000*robot.getSamplingTime());
+ vpTime::wait(1000 * robot.getSamplingTime());
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
diff --git a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
index 6fe7162..6b8e525 100644
--- a/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
+++ b/tutorial/visual-servo/ibvs/tutorial-ibvs-4pts.cpp
@@ -1,28 +1,27 @@
/*! \example tutorial-ibvs-4pts.cpp */
+#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>
-#include <visp3/robot/vpSimulatorCamera.h>
int main()
{
try {
vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
- vpHomogeneousMatrix cMo(0.15, -0.1, 1.,
- vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
+ vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
- vpPoint point[4] ;
- point[0].setWorldCoordinates(-0.1,-0.1, 0);
- point[1].setWorldCoordinates( 0.1,-0.1, 0);
- point[2].setWorldCoordinates( 0.1, 0.1, 0);
+ vpPoint point[4];
+ point[0].setWorldCoordinates(-0.1, -0.1, 0);
+ point[1].setWorldCoordinates(0.1, -0.1, 0);
+ point[2].setWorldCoordinates(0.1, 0.1, 0);
point[3].setWorldCoordinates(-0.1, 0.1, 0);
- vpServo task ;
+ vpServo task;
task.setServo(vpServo::EYEINHAND_CAMERA);
task.setInteractionMatrixType(vpServo::CURRENT);
task.setLambda(0.5);
- vpFeaturePoint p[4], pd[4] ;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ vpFeaturePoint p[4], pd[4];
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cdMo);
vpFeatureBuilder::create(pd[i], point[i]);
point[i].track(cMo);
@@ -36,10 +35,10 @@ int main()
robot.getPosition(wMc);
wMo = wMc * cMo;
- for (unsigned int iter=0; iter < 150; iter ++) {
+ for (unsigned int iter = 0; iter < 150; iter++) {
robot.getPosition(wMc);
cMo = wMc.inverse() * wMo;
- for (unsigned int i = 0 ; i < 4 ; i++) {
+ for (unsigned int i = 0; i < 4; i++) {
point[i].track(cMo);
vpFeatureBuilder::create(p[i], point[i]);
}
@@ -48,9 +47,7 @@ int main()
}
task.kill();
- }
- catch(vpException &e) {
+ } catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
}
-
--
Alioth's /usr/local/bin/git-commit-notice on /srv/git.debian.org/git/debian-science/packages/visp.git
More information about the debian-science-commits
mailing list